/// <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);
}
// Start is called before the first frame update
private void Start()
{
if (instance == null)
{
instance = this;
DontDestroyOnLoad(this);
RclCppDotnet.Init();
_listener = new TransformListener();
_detector = NativeFiducialFunctions.apriltag_detector_create();
_family = NativeFiducialFunctions.tagStandard41h12_create();
// Start detecting Apriltags from the 41h12 family:
// https://github.com/AprilRobotics/apriltag-imgs/tree/master/tagStandard41h12
NativeFiducialFunctions.apriltag_detector_add_family_bits(this._detector, this._family);
_active = true;
}
else
{
Debug.LogWarning("Duplicate FiducialSystem tried to initialize in scene on gameobject " + this.gameObject + "; Destroying self!");
Destroy(this);
return;
}
if (tagSize == 0)
{
Debug.LogError("Fiducial system is missing information about how large the physical AprilTag squares are!");
}
}
private void Shutdown()
{
if (calibImgs.Count > 0)
{
calibImgs.Clear();
NativeFiducialFunctions.clear_calibration_images();
}
}
private void Shutdown()
{
if (_active)
{
RclCppDotnet.Shutdown();
NativeFiducialFunctions.apriltag_detector_destroy(_detector);
NativeFiducialFunctions.tagStandard41h12_destroy(_family);
_active = false;
}
}
public async void DoCalibration()
{
if (calibImgs.Count < MINIMUM_CALIBRATION_IMGS)
{
Debug.LogError("Attempted calibration before acquiring the minimum required images!");
}
else
{
// Calibration will take a noticeable amount of time, so put it in its own thread
Task <Intrensics> task = Task.Run(() =>
{
Debug.Log("Dispatching background thread to perform calibration!");
Intrensics intr = new Intrensics();
int success = NativeFiducialFunctions.calibrate(squareSize, out intr);
return(intr);
});
// Start UI feedback indicating work is being done...
loadingComet.SetActive(true);
Intrensics foundIntr = await task;
if (!CameraIntrensicsHelper.WriteIntrensics(foundIntr))
{
Debug.LogError("Failed to write intrensics to disk!!");
return;
}
// End UI feedback/transition scene
loadingComet.SetActive(false); // Not strictly needed, since we change scenes
this.Shutdown(); // Clear the unmanaged memory!
SceneManager.LoadScene("RobotScene");
}
}
/// <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);
}
