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