// Methods /// <summary> /// Resets the properties. /// </summary> public virtual void ResetCalibration() { AllMarkerCorners = new Std.VectorVectorVectorPoint2f[ArucoCamera.CameraNumber]; AllMarkerIds = new Std.VectorVectorInt[ArucoCamera.CameraNumber]; Images = new Std.VectorMat[ArucoCamera.CameraNumber]; for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++) { AllMarkerCorners[cameraId] = new Std.VectorVectorVectorPoint2f(); AllMarkerIds[cameraId] = new Std.VectorVectorInt(); Images[cameraId] = new Std.VectorMat(); } Rvecs = new Std.VectorVec3d[ArucoCamera.CameraNumber]; Tvecs = new Std.VectorVec3d[ArucoCamera.CameraNumber]; MarkerCorners = new Std.VectorVectorPoint2f[ArucoCamera.CameraNumber]; MarkerIds = new Std.VectorInt[ArucoCamera.CameraNumber]; IsCalibrated = false; }
// Methods /// <summary> /// Reset the properties. /// </summary> public void ResetCalibration() { MarkerCorners = new Std.VectorVectorVectorPoint2f[ArucoCamera.CameraNumber]; MarkerIds = new Std.VectorVectorInt[ArucoCamera.CameraNumber]; CameraImages = new Std.VectorMat[ArucoCamera.CameraNumber]; for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++) { MarkerCorners[cameraId] = new Std.VectorVectorVectorPoint2f(); MarkerIds[cameraId] = new Std.VectorVectorInt(); CameraImages[cameraId] = new Std.VectorMat(); } Rvecs = new Std.VectorVec3d[ArucoCamera.CameraNumber]; Tvecs = new Std.VectorVec3d[ArucoCamera.CameraNumber]; MarkerCornersCurrentImage = new Std.VectorVectorPoint2f[ArucoCamera.CameraNumber]; MarkerIdsCurrentImage = new Std.VectorInt[ArucoCamera.CameraNumber]; CameraParameters = null; IsCalibrated = false; }
/// <summary> /// Calibrates each camera of the <see cref="ArucoObjectDetector.ArucoCamera"/> system using the detected markers added with /// <see cref="AddCurrentFrameForCalibration()"/>, the <see cref="CameraParameters"/>, the <see cref="ArucoCameraUndistortion"/> and save /// the results on a calibration file. Stereo calibrations will be additionally executed on these results for every camera pair in /// <see cref="StereoCalibrationCameraPairs"/>. /// </summary> public virtual void Calibrate() { if (!IsConfigured) { throw new Exception("Configure the calibration controller before starting the calibration."); } // Update state calibratingMutex.WaitOne(); { IsCalibrated = false; CalibrationRunning = true; } calibratingMutex.ReleaseMutex(); // Check if there is enough captured frames for calibration Aruco.CharucoBoard charucoBoard = CalibrationBoard.Board as Aruco.CharucoBoard; for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++) { if (charucoBoard == null && MarkerIds[cameraId].Size() < 3) { throw new Exception("Need at least three frames captured for the camera " + (cameraId + 1) + "/" + ArucoCamera.CameraNumber + " to calibrate."); } else if (charucoBoard != null && MarkerIds[cameraId].Size() < 4) { throw new Exception("Need at least four frames captured for the camera " + (cameraId + 1) + "/" + ArucoCamera.CameraNumber + " to calibrate with a ChAruco board."); } } InitializeCameraParameters(); // Initialize and configure the camera parameters // Get objet and image calibration points from detected ids and corners Std.VectorVectorPoint2f[] imagePoints = new Std.VectorVectorPoint2f[ArucoCamera.CameraNumber]; Std.VectorVectorPoint3f[] objectPoints = new Std.VectorVectorPoint3f[ArucoCamera.CameraNumber]; for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++) { imagePoints[cameraId] = new Std.VectorVectorPoint2f(); objectPoints[cameraId] = new Std.VectorVectorPoint3f(); uint frameCount = MarkerCorners[cameraId].Size(); for (uint frame = 0; frame < frameCount; frame++) { Std.VectorPoint2f frameImagePoints; Std.VectorPoint3f frameObjectPoints; if (charucoBoard == null) { // Using a grid board Aruco.GetBoardObjectAndImagePoints(CalibrationBoard.Board, MarkerCorners[cameraId].At(frame), MarkerIds[cameraId].At(frame), out frameObjectPoints, out frameImagePoints); } else { // Using a charuco board Std.VectorInt charucoIds; Aruco.InterpolateCornersCharuco(MarkerCorners[cameraId].At(frame), MarkerIds[cameraId].At(frame), CalibrationImages[cameraId].At(frame), charucoBoard, out frameImagePoints, out charucoIds); // Join the object points corresponding to the detected markers frameObjectPoints = new Std.VectorPoint3f(); uint markerCount = charucoIds.Size(); for (uint marker = 0; marker < markerCount; marker++) { uint pointId = (uint)charucoIds.At(marker); Cv.Point3f objectPoint = charucoBoard.ChessboardCorners.At(pointId); frameObjectPoints.PushBack(objectPoint); } } imagePoints[cameraId].PushBack(frameImagePoints); objectPoints[cameraId].PushBack(frameObjectPoints); } } // Calibrate the Aruco camera Calibrate(imagePoints, objectPoints); // Save the camera parameters CameraParametersController.CameraParametersFilename = ArucoCamera.Name + " - " + CameraParametersController.CameraParameters.CalibrationDateTime.ToString("yyyy-MM-dd_HH-mm-ss") + ".xml"; CameraParametersController.Save(); // Update state calibratingMutex.WaitOne(); { IsCalibrated = true; } calibratingMutex.ReleaseMutex(); }
public void Calibrate() { Aruco.CharucoBoard charucoBoard = CalibrationBoard.Board as Aruco.CharucoBoard; // Check if there is enough captured frames for calibration for (int cameraId = 0; cameraId < ArucoCamera.CamerasNumber; cameraId++) { // Calibration with a grid board if (charucoBoard == null) { if (AllIds[cameraId].Size() < 1) { throw new System.Exception("Need at least one frame captured for the camera " + (cameraId + 1) + "/" + ArucoCamera.CamerasNumber + " to calibrate."); } } // Calibration with a charuco board else { if (AllCharucoIds[cameraId].Size() < 4) { IsCalibrated = false; throw new System.Exception("Need at least four frames captured for the camera " + (cameraId + 1) + "/" + ArucoCamera.CamerasNumber + " to calibrate with a ChAruco board."); } } } // Prepare camera parameters Cv.Core.Mat[] camerasMatrix = new Cv.Core.Mat[ArucoCamera.CamerasNumber], distCoeffs = new Cv.Core.Mat[ArucoCamera.CamerasNumber]; double[] reprojectionErrors = new double[ArucoCamera.CamerasNumber]; // Calibrate each camera for (int cameraId = 0; cameraId < ArucoCamera.CamerasNumber; cameraId++) { // Prepare camera parameters if (CalibrationFlagsController.FixAspectRatio) { camerasMatrix[cameraId] = new Cv.Core.Mat(3, 3, Cv.Core.TYPE.CV_64F, new double[9] { CalibrationFlagsController.FixAspectRatioValue, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }); } else { camerasMatrix[cameraId] = new Cv.Core.Mat(); } distCoeffs[cameraId] = new Cv.Core.Mat(); // Prepare data for calibration Std.VectorVectorPoint2f allCornersContenated = new Std.VectorVectorPoint2f(); Std.VectorInt allIdsContenated = new Std.VectorInt(); Std.VectorInt markerCounterPerFrame = new Std.VectorInt(); uint allCornersSize = AllCorners[cameraId].Size(); markerCounterPerFrame.Reserve(allCornersSize); for (uint i = 0; i < allCornersSize; i++) { Std.VectorVectorPoint2f allCornersI = AllCorners[cameraId].At(i); uint allCornersISize = allCornersI.Size(); markerCounterPerFrame.PushBack((int)allCornersISize); for (uint j = 0; j < allCornersISize; j++) { allCornersContenated.PushBack(allCornersI.At(j)); allIdsContenated.PushBack(AllIds[cameraId].At(i).At(j)); } } // Calibrate camera with aruco Cv.Core.Size imageSize = ArucoCamera.Images[cameraId].size; Std.VectorMat rvecs, tvecs; reprojectionErrors[cameraId] = Aruco.CalibrateCameraAruco(allCornersContenated, allIdsContenated, markerCounterPerFrame, CalibrationBoard.Board, imageSize, camerasMatrix[cameraId], distCoeffs[cameraId], out rvecs, out tvecs, CalibrationFlagsController.CalibrationFlags, CalibrationTermCriteria); // If the used board is a charuco board, refine the calibration if (charucoBoard != null) { AllCharucoCorners[cameraId] = new Std.VectorVectorPoint2f(); AllCharucoIds[cameraId] = new Std.VectorVectorInt(); // Interpolate charuco corners using camera parameters for (uint i = 0; i < AllIds[cameraId].Size(); i++) { Std.VectorPoint2f charucoCorners; Std.VectorInt charucoIds; Aruco.InterpolateCornersCharuco(AllCorners[cameraId].At(i), AllIds[cameraId].At(i), AllImages[cameraId].At(i), charucoBoard, out charucoCorners, out charucoIds); AllCharucoCorners[cameraId].PushBack(charucoCorners); AllCharucoIds[cameraId].PushBack(charucoIds); } // Calibrate camera using charuco reprojectionErrors[cameraId] = Aruco.CalibrateCameraCharuco(AllCharucoCorners[cameraId], AllCharucoIds[cameraId], charucoBoard, imageSize, camerasMatrix[cameraId], distCoeffs[cameraId], out rvecs, out tvecs, CalibrationFlagsController.CalibrationFlags, CalibrationTermCriteria); } // Save calibration parameters Rvecs[cameraId] = rvecs; Tvecs[cameraId] = tvecs; } IsCalibrated = true; // Create camera parameters CameraParameters = new CameraParameters(ArucoCamera.CamerasNumber) { CalibrationFlags = (int)CalibrationFlagsController.CalibrationFlags, FixAspectRatioValue = CalibrationFlagsController.FixAspectRatioValue, ReprojectionError = reprojectionErrors, CamerasMatrix = camerasMatrix, DistCoeffs = distCoeffs }; for (int cameraId = 0; cameraId < ArucoCamera.CamerasNumber; cameraId++) { CameraParameters.ImagesHeight[cameraId] = ArucoCamera.ImageTextures[cameraId].height; CameraParameters.ImagesWidth[cameraId] = ArucoCamera.ImageTextures[cameraId].width; } // Save camera parameters string outputFolderPath = Path.Combine((Application.isEditor) ? Application.dataPath : Application.persistentDataPath, OutputFolder); if (!Directory.Exists(outputFolderPath)) { Directory.CreateDirectory(outputFolderPath); } string calibrationFilePath = outputFolderPath; calibrationFilePath += (CalibrationFilename == null || CalibrationFilename.Length == 0) ? ArucoCamera.Name + " - " + DateTime.Now.ToString("yyyy-MM-dd_HH-mm-ss") : CalibrationFilename; calibrationFilePath += ".xml"; CameraParameters.SaveToXmlFile(calibrationFilePath); }
/// <summary> /// Calibrate each camera of the <see cref="ArucoObjectDetector.ArucoCamera"/> system using the detected markers added with /// <see cref="AddCurrentFrameForCalibration()"/>, the <see cref="CameraParameters"/>, the <see cref="CalibrationFlagsController"/> and save /// the results on a calibration file. Stereo calibrations will be additionally executed on these results for every camera pair in /// <see cref="StereoCalibrationCameraPairs"/>. /// </summary> public void Calibrate() { // Prepare data Aruco.CharucoBoard charucoBoard = CalibrationBoard.Board as Aruco.CharucoBoard; // Check if there is enough captured frames for calibration for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++) { if (charucoBoard == null && MarkerIds[cameraId].Size() < 1) { throw new Exception("Need at least one frame captured for the camera " + (cameraId + 1) + "/" + ArucoCamera.CameraNumber + " to calibrate."); } else if (charucoBoard != null && MarkerIds[cameraId].Size() < 4) { throw new Exception("Need at least four frames captured for the camera " + (cameraId + 1) + "/" + ArucoCamera.CameraNumber + " to calibrate with a ChAruco board."); } } // Load the camera parameters if they exist string cameraParametersFilePath; string cameraParametersFolderPath = Path.Combine((Application.isEditor) ? Application.dataPath : Application.persistentDataPath, CalibrationFolder); if (!Directory.Exists(cameraParametersFolderPath)) { Directory.CreateDirectory(cameraParametersFolderPath); } if (CalibrationFilename != null && CalibrationFilename.Length > 0) { cameraParametersFilePath = cameraParametersFolderPath + CalibrationFilename; CameraParameters = CameraParameters.LoadFromXmlFile(cameraParametersFilePath); if (CameraParameters.CameraNumber != ArucoCamera.CameraNumber) { throw new ArgumentException("The loaded camera parameters from the file '" + cameraParametersFilePath + "' is for a system with " + CameraParameters.CameraNumber + " camera. But the current calibrating camera has " + ArucoCamera.CameraNumber + ". These numbers" + " must be equal.", "CalibrationFilename"); } } // Or initialize the camera parameters else { CameraParameters = new CameraParameters(ArucoCamera.CameraNumber) { CalibrationFlagsValue = CalibrationFlagsController.CalibrationFlagsValue, FixAspectRatioValue = (calibrationFlagsPinholeController) ? calibrationFlagsPinholeController.FixAspectRatioValue : 0 }; for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++) { CameraParameters.ImageHeights[cameraId] = ArucoCamera.ImageTextures[cameraId].height; CameraParameters.ImageWidths[cameraId] = ArucoCamera.ImageTextures[cameraId].width; double cameraMatrixAspectRatio = (calibrationFlagsPinholeController && calibrationFlagsPinholeController.FixAspectRatio) ? calibrationFlagsPinholeController.FixAspectRatioValue : 1.0; CameraParameters.CameraMatrices[cameraId] = new Cv.Mat(3, 3, Cv.Type.CV_64F, new double[9] { cameraMatrixAspectRatio, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }); CameraParameters.DistCoeffs[cameraId] = new Cv.Mat(); CameraParameters.OmnidirXis[cameraId] = new Cv.Mat(); } } // Calibrate each camera Std.VectorVectorPoint3f[] objectPoints = new Std.VectorVectorPoint3f[ArucoCamera.CameraNumber]; Std.VectorVectorPoint2f[] imagePoints = new Std.VectorVectorPoint2f[ArucoCamera.CameraNumber]; for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++) { // Get objet and image calibration points from detected ids and corners Std.VectorVectorPoint3f boardObjectPoints = new Std.VectorVectorPoint3f(); Std.VectorVectorPoint2f boardImagePoints = new Std.VectorVectorPoint2f(); uint frameCount = MarkerCorners[cameraId].Size(); for (uint frame = 0; frame < frameCount; frame++) { Std.VectorPoint3f frameObjectPoints; Std.VectorPoint2f frameImagePoints; Aruco.GetBoardObjectAndImagePoints(CalibrationBoard.Board, MarkerCorners[cameraId].At(frame), MarkerIds[cameraId].At(frame), out frameObjectPoints, out frameImagePoints); boardObjectPoints.PushBack(frameObjectPoints); boardImagePoints.PushBack(frameImagePoints); } objectPoints[cameraId] = boardObjectPoints; imagePoints[cameraId] = boardImagePoints; // Calibrate the camera Cv.Size imageSize = ArucoCamera.Images[cameraId].Size; Std.VectorVec3d rvecs, tvecs; if (calibrationFlagsPinholeController) { CameraParameters.ReprojectionErrors[cameraId] = Cv.CalibrateCamera(boardObjectPoints, boardImagePoints, imageSize, CameraParameters.CameraMatrices[cameraId], CameraParameters.DistCoeffs[cameraId], out rvecs, out tvecs, calibrationFlagsPinholeController.CalibrationFlags); } else if (calibrationFlagsOmnidirController) { CameraParameters.ReprojectionErrors[cameraId] = Cv.Omnidir.Calibrate(boardObjectPoints, boardImagePoints, imageSize, CameraParameters.CameraMatrices[cameraId], CameraParameters.OmnidirXis[cameraId], CameraParameters.DistCoeffs[cameraId], out rvecs, out tvecs, calibrationFlagsOmnidirController.CalibrationFlags); } else { rvecs = new Std.VectorVec3d(); tvecs = new Std.VectorVec3d(); } // If the used board is a charuco board, refine the calibration if (charucoBoard != null) { // Prepare data to refine the calibration Std.VectorVectorPoint3f charucoObjectPoints = new Std.VectorVectorPoint3f(); Std.VectorVectorPoint2f charucoImagePoints = new Std.VectorVectorPoint2f(); for (uint frame = 0; frame < frameCount; frame++) { // Interpolate charuco corners using camera parameters Std.VectorPoint2f charucoCorners; Std.VectorInt charucoIds; Aruco.InterpolateCornersCharuco(MarkerCorners[cameraId].At(frame), MarkerIds[cameraId].At(frame), CameraImages[cameraId].At(frame), charucoBoard, out charucoCorners, out charucoIds); charucoImagePoints.PushBack(charucoCorners); // Join the object points corresponding to the detected markers charucoObjectPoints.PushBack(new Std.VectorPoint3f()); uint markerCount = charucoIds.Size(); for (uint marker = 0; marker < markerCount; marker++) { uint pointId = (uint)charucoIds.At(marker); Cv.Point3f objectPoint = charucoBoard.ChessboardCorners.At(pointId); charucoObjectPoints.At(frame).PushBack(objectPoint); } } objectPoints[cameraId] = boardObjectPoints; // Refine the calibration if (calibrationFlagsPinholeController) { CameraParameters.ReprojectionErrors[cameraId] = Cv.CalibrateCamera(charucoObjectPoints, charucoImagePoints, imageSize, CameraParameters.CameraMatrices[cameraId], CameraParameters.DistCoeffs[cameraId], out rvecs, out tvecs, calibrationFlagsPinholeController.CalibrationFlags); } else if (calibrationFlagsOmnidirController) { CameraParameters.ReprojectionErrors[cameraId] = Cv.Omnidir.Calibrate(boardObjectPoints, boardImagePoints, imageSize, CameraParameters.CameraMatrices[cameraId], CameraParameters.OmnidirXis[cameraId], CameraParameters.DistCoeffs[cameraId], out rvecs, out tvecs, calibrationFlagsOmnidirController.CalibrationFlags); } } // Save calibration extrinsic parameters Rvecs[cameraId] = rvecs; Tvecs[cameraId] = tvecs; } // If required, apply a stereo calibration and save the resuts in the camera parameters CameraParameters.StereoCameraParametersList = new StereoCameraParameters[StereoCalibrationCameraPairs.Length]; for (int i = 0; i < StereoCalibrationCameraPairs.Length; i++) { CameraParameters.StereoCameraParametersList[i] = StereoCalibrationCameraPairs[i].Calibrate(ArucoCamera, CameraParameters, objectPoints, imagePoints); } IsCalibrated = true; // Save the camera parameters cameraParametersFilePath = cameraParametersFolderPath; if (CalibrationFilename != null && CalibrationFilename.Length > 0) { cameraParametersFilePath += CalibrationFilename; } else { cameraParametersFilePath += ArucoCamera.Name + " - " + DateTime.Now.ToString("yyyy-MM-dd_HH-mm-ss") + ".xml"; } CameraParameters.SaveToXmlFile(cameraParametersFilePath); }