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