private void SaveCalibration()
{
Vector3 rawScale, rawPos;
Quaternion rawRot;
Vector3 posSum = Vector3.Zero;
Quaternion rotSum = Quaternion.Identity;
int count = relativeTransforms.Count;
for (int i = 0; i < count; i++)
{
relativeTransforms[i].Decompose(out rawScale, out rawRot, out rawPos);
posSum += rawPos;
rotSum += rawRot;
}
Vector3 avgPos = posSum / count;
rotSum.Normalize();
rawScale = Vector3Helper.RadiansToDegrees(Vector3Helper.QuaternionToEulerAngleVector3(rotSum));
Console.WriteLine("Relative camera position & orientation:");
Console.WriteLine("Pos: " + avgPos.ToString() + ", Length: " + avgPos.Length() + ", Yaw: "
+ rawScale.X + ", Pitch: " + rawScale.Y + ", Roll: " + rawScale.Z);
Matrix avgTransform = Matrix.CreateFromQuaternion(rotSum);
avgTransform.Translation = avgPos;
MatrixHelper.SaveMatrixToXML(calibrationFilename, avgTransform);
((StereoCamera)scene.CameraNode.Camera).RightView = Matrix.Invert(avgTransform);
CreateTestObjects();
}
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;
}