// ArucoCameraUndistortion methods
protected override void InitializeRectification()
{
int cameraId1 = StereoArucoCamera.CameraId1;
int cameraId2 = StereoArucoCamera.CameraId2;
Cv.Mat rotationMatrix, rectificationMatrix1, rectificationMatrix2, newCameraMatrix1, newCameraMatrix2, disparityMatrix;
Cv.StereoRectifyFlags stereoRectifyFlags = RectificationZeroDisparity ? Cv.StereoRectifyFlags.ZeroDisparity : 0;
Cv.Rodrigues(stereoCameraParameters.RotationVector, out rotationMatrix);
Cv.StereoRectify(CameraParameters.CameraMatrices[cameraId1], CameraParameters.DistCoeffs[cameraId1],
CameraParameters.CameraMatrices[cameraId2], CameraParameters.DistCoeffs[cameraId2], ArucoCamera.Images[cameraId1].Size, rotationMatrix,
stereoCameraParameters.TranslationVector, out rectificationMatrix1, out rectificationMatrix2, out newCameraMatrix1, out newCameraMatrix2,
out disparityMatrix, stereoRectifyFlags, RectificationScalingFactor);
RectifiedCameraMatrices[cameraId1] = newCameraMatrix1;
RectifiedCameraMatrices[cameraId2] = newCameraMatrix2;
RectificationMatrices[cameraId1] = rectificationMatrix1;
RectificationMatrices[cameraId2] = rectificationMatrix2;
}
public StereoCameraParameters Calibrate(ArucoCamera arucoCamera, CameraParameters cameraParameters, Std.VectorVectorPoint3f[] objectPoints,
Std.VectorVectorPoint2f[] imagePoints)
{
// Prepare the camera parameters
StereoCameraParameters stereoCameraParameters = new StereoCameraParameters()
{
CameraIds = new int[] { CameraId1, CameraId2 },
CalibrationFlagsValue = CalibrationFlagsController.CalibrationFlagsValue
};
// Estimates transformation between the two cameras
Cv.Mat cameraMatrix1 = cameraParameters.CameraMatrices[CameraId1];
Cv.Mat distCoeffs1 = cameraParameters.DistCoeffs[CameraId1];
Cv.Mat xi1 = cameraParameters.OmnidirXis[CameraId1];
Cv.Mat cameraMatrix2 = cameraParameters.CameraMatrices[CameraId2];
Cv.Mat distCoeffs2 = cameraParameters.DistCoeffs[CameraId2];
Cv.Mat xi2 = cameraParameters.OmnidirXis[CameraId2];
Cv.Size imageSize = arucoCamera.Images[CameraId1].Size;
Cv.Vec3d rvec, tvec;
Cv.Mat essentialMatrix, fundamentalMatrix;
if (calibrationFlagsPinholeController)
{
stereoCameraParameters.ReprojectionError = Cv.StereoCalibrate(objectPoints[CameraId1], imagePoints[CameraId1], imagePoints[CameraId2],
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, out rvec, out tvec, out essentialMatrix, out fundamentalMatrix,
calibrationFlagsPinholeController.CalibrationFlags);
}
else if (calibrationFlagsOmnidirController)
{
Cv.Mat rvecsL, tvecsL;
stereoCameraParameters.ReprojectionError = Cv.Omnidir.StereoCalibrate(objectPoints[CameraId1], imagePoints[CameraId1],
imagePoints[CameraId2], imageSize, imageSize, cameraMatrix1, xi1, distCoeffs1, cameraMatrix2, xi2, distCoeffs2, out rvec, out tvec,
out rvecsL, out tvecsL, calibrationFlagsOmnidirController.CalibrationFlags);
}
else
{
rvec = new Cv.Vec3d();
tvec = new Cv.Vec3d();
}
// Computes rectification transforms
Cv.Mat rotationMatrix1, rotationMatrix2, newCameraMatrix1, newCameraMatrix2, Q;
if (calibrationFlagsPinholeController)
{
Cv.StereoRectifyFlags stereoRectifyFlags = (calibrationFlagsPinholeController.ZeroDisparity) ? Cv.StereoRectifyFlags.ZeroDisparity : 0;
Cv.StereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, rvec, tvec, out rotationMatrix1,
out rotationMatrix2, out newCameraMatrix1, out newCameraMatrix2, out Q, stereoRectifyFlags,
calibrationFlagsPinholeController.Skew, NewImageSize);
}
else if (calibrationFlagsOmnidirController)
{
Cv.Omnidir.StereoRectify(rvec, tvec, out rotationMatrix1, out rotationMatrix2);
newCameraMatrix1 = new Cv.Mat();
newCameraMatrix2 = new Cv.Mat();
}
else
{
rotationMatrix1 = new Cv.Mat();
rotationMatrix2 = new Cv.Mat();
newCameraMatrix1 = new Cv.Mat();
newCameraMatrix2 = new Cv.Mat();
}
// Save the camera parameters
stereoCameraParameters.RotationVector = rvec;
stereoCameraParameters.TranslationVector = tvec;
stereoCameraParameters.RotationMatrices = new Cv.Mat[] { rotationMatrix1, rotationMatrix2 };
stereoCameraParameters.NewCameraMatrices = new Cv.Mat[] { newCameraMatrix1, newCameraMatrix2 };
return(stereoCameraParameters);
}
