Exemple #1
0
            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);
            }