/// <summary>
/// Given a camera result/frame, adds it to the calibration pool
/// </summary>
private void OnCapturedPhotoToMemory(PhotoCapture.PhotoCaptureResult result, PhotoCaptureFrame frame)
{
if (result.success)
{
Debug.Log("SNAP!!!");
WebcamSystem.CaptureFrameInstance currFrame = new WebcamSystem.CaptureFrameInstance(frame);
if (DEBUG)
{
Debug.LogWarning("dumping img...");
int res = NativeFiducialFunctions.image_u8_write_pnm(currFrame.unmanagedFrame, "m:\\debugImg\\garbooggle.pnm");
}
int prevCount = calibImgs.Count;
int newCount = NativeFiducialFunctions.supply_calibration_image(currFrame.unmanagedFrame);
if (newCount > prevCount)
{
Debug.Log(string.Format("Good picture! Currently have {0} pictures stored out of the needed {1}.", newCount, MINIMUM_CALIBRATION_IMGS));
calibImgs.Add(currFrame);
}
else
{
Debug.Log("Couldn't detect the checkerboard. Please try again.");
}
}
else
{
Debug.LogError("Unable to take photo!");
}
calibButton.UpdateState(calibImgs.Count >= MINIMUM_CALIBRATION_IMGS);
}
/// <summary>
/// Background thread that repeatedly queries WebcamSystem for images and pipes them
/// to apriltag until an anchor is successfully created
/// </summary>
private IEnumerator PinSpace()
{
bool fin = false;
// Keep taking pictures until UpdateSpacePinning pins the robot
while (true)
{
// Wait for the FiducialSystem to finish initializing before trying to use it...
if (!_active)
{
yield return(new WaitForSeconds(0.1f));
continue;
}
// Only request another photo after the first request's callback is processed
lock (pinLock)
{
if (!cameraInUse)
{
cameraInUse = true;
// Pass anonymous function as callback which sets the currFrame on success
WebcamSystem.instance.CapturePhoto((PhotoCapture.PhotoCaptureResult result, PhotoCaptureFrame frame) =>
{
if (result.success && frame != null)
{
WebcamSystem.CaptureFrameInstance currFrame = null;
Debug.Log("cr snap!");
currFrame = new WebcamSystem.CaptureFrameInstance(frame);
// If the callback succeeded in this iteration, we can try to locate fiducials
// If the pin is successful, we can stop the coroutine
if (currFrame != null && UpdateSpacePinning(currFrame))
{
fin = true;
}
}
else
{
Debug.LogError("space pinning webcam call failed!");
}
cameraInUse = false;
});
}
}
if (fin)
{
break;
}
yield return(new WaitForSeconds(0.5f));
}
Debug.Log("Pin successful");
}
/// <summary>
/// Attempts to locate the ROS world origin, placing a world anchor at that location if found.
/// </summary>
/// <returns>
/// true if a world anchor is created successfully, otherwise false.
/// A world anchor could fail to be created for a number of reasons: webcam image did not have
/// an identifiable apriltag, tf2 messages are not being found (no robot discovered, incorrect frame name)
/// etc.
/// </returns>
private bool UpdateSpacePinning(WebcamSystem.CaptureFrameInstance captureFrame)
{
if (_pinning != null)
{
return(false);
}
if (DEBUG_DUMP_IMAGE)
{
Debug.LogWarning("dumping processed img...");
int res = NativeFiducialFunctions.image_u8_write_pnm(captureFrame.unmanagedFrame, DEBUG_DUMP_IMAGE_NAME + "_" + System.DateTime.Now.Ticks + "_processed.pnm");
}
IntPtr nativeDetectionsHandle = NativeFiducialFunctions.apriltag_detector_detect(_detector, captureFrame.unmanagedFrame);
_detections = Marshal.PtrToStructure <ZArray>(nativeDetectionsHandle);
Dictionary <int, RelTransform> fiducialCentersRelWebcam = new Dictionary <int, RelTransform>();
// Iterate over all detected apriltags in image
for (int i = 0; i < _detections.size; i++)
{
IntPtr detPtr = IntPtr.Add(_detections.data, i * (int)_detections.el_sz);
detPtr = Marshal.ReadIntPtr(detPtr); // the zarray stores an array of detection ptrs
ApriltagDetection det = Marshal.PtrToStructure <ApriltagDetection>(detPtr);
Debug.Log("Detected an apriltag of id: " + det.id);
AprilTagDetectionInfo info = new AprilTagDetectionInfo();
info.det = detPtr;
info.tagsize = tagSize;
info.fx = _intrensics.fx;
info.fy = _intrensics.fy;
info.cx = _intrensics.cx;
info.cy = _intrensics.cy;
AprilTagPose pose = new AprilTagPose();
double err = NativeFiducialFunctions.estimate_tag_pose(in info, out pose);
Matd t = Marshal.PtrToStructure <Matd>(pose.t);
Matd R = Marshal.PtrToStructure <Matd>(pose.R);
Vector3? fiducialCenterRelWebcamTranslation = TransformHelper.VectorAprilToUnity(t);
Quaternion?fiducialCenterRelWebcamRotation = TransformHelper.QuatAprilToUnity(R);
if (fiducialCenterRelWebcamTranslation.HasValue && fiducialCenterRelWebcamRotation.HasValue)
{
fiducialCentersRelWebcam.Add(det.id, new RelTransform(fiducialCenterRelWebcamTranslation.Value,
fiducialCenterRelWebcamRotation.Value));
}
else
{
Debug.LogError("A TransformHelper apriltag conversion failed!");
}
}
// Deallocate the Zarray of detections on native side
NativeFiducialFunctions.apriltag_detections_destroy(nativeDetectionsHandle);
nativeDetectionsHandle = IntPtr.Zero;
// Use the fiducial tag labeled '1' to pin the anchor
if (fiducialCentersRelWebcam.ContainsKey(1))
{
// Raw TF values follow a different xyz coordinate system, must be converted with the TransformHelper class
TfVector3? worldZeroRelFiducialCenterTranslationTF = _listener.LookupTranslation("fiducial_link", "odom");
TfQuaternion?worldZeroRelFiducialCenterRotationTF = _listener.LookupRotation("fiducial_link", "odom");
if (worldZeroRelFiducialCenterTranslationTF.HasValue && worldZeroRelFiducialCenterRotationTF.HasValue)
{
Vector3 worldZeroRelFiducialCenterTranslation = TransformHelper.VectorTfToUnity(worldZeroRelFiducialCenterTranslationTF.Value);
Quaternion worldZeroRelFiducialCenterRotation = TransformHelper.QuatTfToUnity(worldZeroRelFiducialCenterRotationTF.Value);
// translate from camera pos -> fiducial tag pos
Vector3 fiducialPos = Camera.main.transform.position + fiducialCentersRelWebcam[1].translation;
// rotate from camera space -> fiducial space
Quaternion fiducialRot = Camera.main.transform.rotation * fiducialCentersRelWebcam[1].rotation;
GameObject anchor = new GameObject("WorldZero");
_pinning = anchor.AddComponent <WorldAnchor>();
// first put anchor at fiducial location...
anchor.transform.position = fiducialPos;
anchor.transform.rotation = fiducialRot;
// use fiducial location to utilize fiducial-spaced translation
/*anchor.transform.position = anchor.transform.position + anchor.transform.right * worldZeroRelFiducialCenterTranslation.x
+ anchor.transform.up * worldZeroRelFiducialCenterTranslation.y + anchor.transform.forward * worldZeroRelFiducialCenterTranslation.z;*/
// end anchor positioning!
Collocator.StartCollocation(_pinning);
Debug.Log("Anchor laid at Unity-space coords" + anchor.transform.position);
return(true);
}
else
{
Debug.LogError("TF2 failed to query between fiducial tag and ROS world zero... is the ROS graph active?");
}
}
return(false);
}
