/// <summary>
/// Convert Apriltag-spaced Matd to Unity-spaced Quaternion
/// </summary>
/// <param name="R">A matd containing a rotation matrix (NOT translation)</param>
/// <returns>A Quaternion?, which is null if the provided R was bad, otherwise the Unity-spaced quaternion</returns>
public static Quaternion?QuatAprilToUnity(Matd R)
{
if (R.nrows * R.ncols != 9) // rotation matrix
{
Debug.LogError("Recieved a Matd of unexpected size! Expected 9 but got " + R.nrows * R.ncols);
return(null);
}
unsafe
{
// First convert the rotation to Euler angles (easier to reason about and debug)
// Math is from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
float sy = Mathf.Sqrt((float)R.data[0] * (float)R.data[0] + (float)R.data[3] * (float)R.data[3]);
float x, y, z;
if (sy < 1e-6)
{
x = Mathf.Atan2((float)R.data[7], (float)R.data[8]);
y = Mathf.Atan2(-(float)R.data[6], sy);
z = Mathf.Atan2((float)R.data[3], (float)R.data[0]);
}
else
{
x = Mathf.Atan2(-(float)R.data[5], (float)R.data[4]);
y = Mathf.Atan2(-(float)R.data[6], sy);
z = 0f;
}
Debug.Log(String.Format("Found an april rotation of <x:{0}, y:{1}, z:{2}>", x * Mathf.Rad2Deg, y * Mathf.Rad2Deg, z * Mathf.Rad2Deg));
// Finally, let the Unity Quaternion class handle conversion from Eulers to Quaternion
return(Quaternion.Euler(x * Mathf.Rad2Deg, y * Mathf.Rad2Deg, z * Mathf.Rad2Deg));
}
}
/// <summary>
/// Convert Apriltag-spaced Matd to Unity-spaced Vector3
/// </summary>
/// <param name="t">A matd containing a translation vector (NOT rotation)</param>
/// <returns>A vector3?, which is null if the provided t was bad, otherwise the Unity-spaced vector3</returns>
public static Vector3?VectorAprilToUnity(Matd t)
{
if (t.nrows * t.ncols != 3) // xyz
{
Debug.LogError("Recieved a Matd of unexpected size! Expected 3 but got " + t.nrows * t.ncols);
return(null);
}
unsafe
{
return(new Vector3((float)t.data[0], (float)t.data[1] * -1, (float)t.data[2]));
}
}
/// <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);
}
