private void CalibrateStereo()
{
calibrating = true;
// get the left and right camera iamges
leftCaptureDevice.GetImageTexture(null, ref leftImagePtr);
rightCaptureDevice.GetImageTexture(null, ref rightImagePtr);
markerTracker.DetectorID = 0;
markerTracker.CameraID = 0;
markerTracker.ProcessImage(leftCaptureDevice, leftImagePtr);
bool markerFoundOnLeftVideo = markerTracker.FindMarker(markerID);
if (markerFoundOnLeftVideo)
{
Matrix leftEyeTransform = markerTracker.GetMarkerTransform();
markerTracker.DetectorID = 1;
markerTracker.CameraID = 1;
markerTracker.ProcessImage(rightCaptureDevice, rightImagePtr);
bool markerFoundOnRightVideo = markerTracker.FindMarker(markerID);
if (markerFoundOnRightVideo)
{
Matrix rightEyeTransform = markerTracker.GetMarkerTransform();
leftEyeTransform = Matrix.Invert(leftEyeTransform);
rightEyeTransform = Matrix.Invert(rightEyeTransform);
Matrix relativeTransform = rightEyeTransform * Matrix.Invert(leftEyeTransform);
Vector3 rawScale, rawPos;
Quaternion rawRot;
relativeTransform.Decompose(out rawScale, out rawRot, out rawPos);
float xGap = Math.Abs(rawPos.X);
float yGap = Math.Abs(rawPos.Y);
float zGap = Math.Abs(rawPos.Z);
float xyRatio = yGap / xGap;
float xzRatio = zGap / xGap;
if (xyRatio < 0.2 && xzRatio < 0.2 && rawPos.Length() > EXPECTED_GAP_MIN && rawPos.Length() < EXPECTED_GAP_MAX)
{
relativeTransforms.Add(relativeTransform);
Console.WriteLine("Completed calculation " + (captureCount + 1));
Notifier.AddMessage("Completed calculation: " + (captureCount + 1) + "/" + CALIB_COUNT_MAX);
rawScale = Vector3Helper.QuaternionToEulerAngleVector3(rawRot);
rawScale = Vector3Helper.RadiansToDegrees(rawScale);
Console.WriteLine("Pos: " + rawPos.ToString() + ", Length: " + rawPos.Length() + ", Yaw: "
+ rawScale.X + ", Pitch: " + rawScale.Y + ": Roll, " + rawScale.Z);
Console.WriteLine();
captureCount++;
}
else
{
Console.WriteLine("Failed: Pos: " + rawPos.ToString() + ", Length: " + rawPos.Length());
Console.WriteLine();
Notifier.AddMessage("Failed. Try again");
}
}
}
if (captureCount >= CALIB_COUNT_MAX)
{
SaveCalibration();
Console.WriteLine("Finished calibration. Saved " + calibrationFilename);
Notifier.AddMessage("Finished calibration!!");
finalized = true;
}
calibrating = false;
}
