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