// 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); }