// MonoBehaviour methods
protected override void Awake()
{
base.Awake();
ImageSize = new Cv.Size();
UpdateBoard();
UpdateArucoHashCode();
}
IEnumerator GetOptimalCameraMatrix(ArucoCameraParameters camCalibration)
{
//get dimensions of camera image
Cv.Size imageSize = new Cv.Size(camCalibration.ImageWidths[0], camCalibration.ImageHeights[0]);
//wait a frame before reading FOV incase another source is resetting it (i.e. VR SDK)
yield return(new WaitForEndOfFrame());
//convert Unity FOV to openCV friendly version
float cameraFocalLength = imageSize.Height / (2f * Mathf.Tan(0.5f * viewingCamera.fieldOfView * Mathf.Deg2Rad));
//this is what normally gets passed to opencv function initUndistortRectifyMap()
float3x3 rectifiedCamMatrices = new float3x3(cameraFocalLength, 0, (float)imageSize.Width / 2,
0, cameraFocalLength, (float)imageSize.Height / 2,
0, 0, 1);
//we get the inverse here rather than doing it in shader since we only have to do it once
float3x3 inputMatrix = math.inverse(rectifiedCamMatrices);
SetCameraMatrixValues(camCalibration, inputMatrix);
}
// ArucoBoard methods
public override Cv.Mat Draw()
{
#if UNITY_EDITOR
if (!UnityEditor.EditorApplication.isPlayingOrWillChangePlaymode && (SquaresNumberX <= 1 || SquaresNumberY <= 1 || SquareSideLength <= 0 ||
MarkerSideLength <= 0 || SquareSideLength <= MarkerSideLength || MarkerBorderBits <= 0 || Dictionary == null))
{
return(null);
}
#endif
int squareSideLength = GetInPixels(SquareSideLength);
int markerSideLength = GetInPixels(MarkerSideLength);
Aruco.CharucoBoard board = Aruco.CharucoBoard.Create(SquaresNumberX, SquaresNumberY, squareSideLength, markerSideLength, Dictionary);
Cv.Size imageSize = new Cv.Size();
imageSize.Width = GetInPixels(SquaresNumberX * squareSideLength + 2 * MarginsLength);
imageSize.Height = GetInPixels(SquaresNumberY * squareSideLength + 2 * MarginsLength);
Cv.Mat image;
board.Draw(imageSize, out image, MarginsLength, (int)MarkerBorderBits);
return(image);
}
// ArucoBoard methods
public override Cv.Mat Draw()
{
#if UNITY_EDITOR
if (!UnityEditor.EditorApplication.isPlayingOrWillChangePlaymode && (MarkersNumberX <= 0 || MarkersNumberY <= 0 || MarkerSideLength <= 0 ||
MarkerSeparation <= 0 || MarkerBorderBits <= 0))
{
return(null);
}
#endif
int markerSideLength = GetInPixels(MarkerSideLength);
int markerSeparation = GetInPixels(MarkerSeparation);
Aruco.GridBoard board = Aruco.GridBoard.Create(MarkersNumberX, MarkersNumberY, markerSideLength, markerSeparation, Dictionary);
Cv.Size imageSize = new Cv.Size();
imageSize.Width = GetInPixels(MarkersNumberX * (markerSideLength + markerSeparation) - markerSeparation + 2 * MarginsLength);
imageSize.Height = GetInPixels(MarkersNumberY * (markerSideLength + markerSeparation) - markerSeparation + 2 * MarginsLength);
Cv.Mat image;
board.Draw(imageSize, out image, MarginsLength, (int)MarkerBorderBits);
return(image);
}
/// <summary>
/// Adds the current images of the cameras and the detected corners for the calibration.
/// </summary>
public virtual void AddCurrentFrameForCalibration()
{
if (!IsConfigured)
{
throw new Exception("Configure the calibration controller before adding the current frame for calibration.");
}
// Check for validity
uint markerIdsNumber = (MarkerIdsCurrentImage[0] != null) ? MarkerIdsCurrentImage[0].Size() : 0;
for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++)
{
if (MarkerIdsCurrentImage[cameraId] == null || MarkerIdsCurrentImage[cameraId].Size() < 1)
{
throw new Exception("No markers detected for the camera " + (cameraId + 1) + "/" + ArucoCamera.CameraNumber + " to add the"
+ " current images for the calibration. At least one marker detected is required for calibrating the camera.");
}
if (markerIdsNumber != MarkerIdsCurrentImage[cameraId].Size())
{
throw new Exception("The cameras must have detected the same number of markers to add the current images for the calibration.");
}
}
// Save the images and the detected corners
Cv.Mat[] cameraImages = ArucoCamera.Images;
for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++)
{
MarkerCorners[cameraId].PushBack(MarkerCornersCurrentImage[cameraId]);
MarkerIds[cameraId].PushBack(MarkerIdsCurrentImage[cameraId]);
CalibrationImages[cameraId].PushBack(ArucoCamera.Images[cameraId].Clone());
if (calibrationImageSizes[cameraId] == null)
{
calibrationImageSizes[cameraId] = new Cv.Size(ArucoCamera.Images[cameraId].Size.Width, ArucoCamera.Images[cameraId].Size.Height);
}
}
}
/// <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);
}
public StereoCameraParameters Calibrate(ArucoCamera arucoCamera, CameraParameters cameraParameters, Std.VectorVectorPoint3f[] objectPoints,
Std.VectorVectorPoint2f[] imagePoints)
{
// Prepare the camera parameters
StereoCameraParameters stereoCameraParameters = new StereoCameraParameters()
{
CameraIds = new int[] { CameraId1, CameraId2 },
CalibrationFlagsValue = CalibrationFlagsController.CalibrationFlagsValue
};
// Estimates transformation between the two cameras
Cv.Mat cameraMatrix1 = cameraParameters.CameraMatrices[CameraId1];
Cv.Mat distCoeffs1 = cameraParameters.DistCoeffs[CameraId1];
Cv.Mat xi1 = cameraParameters.OmnidirXis[CameraId1];
Cv.Mat cameraMatrix2 = cameraParameters.CameraMatrices[CameraId2];
Cv.Mat distCoeffs2 = cameraParameters.DistCoeffs[CameraId2];
Cv.Mat xi2 = cameraParameters.OmnidirXis[CameraId2];
Cv.Size imageSize = arucoCamera.Images[CameraId1].Size;
Cv.Vec3d rvec, tvec;
Cv.Mat essentialMatrix, fundamentalMatrix;
if (calibrationFlagsPinholeController)
{
stereoCameraParameters.ReprojectionError = Cv.StereoCalibrate(objectPoints[CameraId1], imagePoints[CameraId1], imagePoints[CameraId2],
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, out rvec, out tvec, out essentialMatrix, out fundamentalMatrix,
calibrationFlagsPinholeController.CalibrationFlags);
}
else if (calibrationFlagsOmnidirController)
{
Cv.Mat rvecsL, tvecsL;
stereoCameraParameters.ReprojectionError = Cv.Omnidir.StereoCalibrate(objectPoints[CameraId1], imagePoints[CameraId1],
imagePoints[CameraId2], imageSize, imageSize, cameraMatrix1, xi1, distCoeffs1, cameraMatrix2, xi2, distCoeffs2, out rvec, out tvec,
out rvecsL, out tvecsL, calibrationFlagsOmnidirController.CalibrationFlags);
}
else
{
rvec = new Cv.Vec3d();
tvec = new Cv.Vec3d();
}
// Computes rectification transforms
Cv.Mat rotationMatrix1, rotationMatrix2, newCameraMatrix1, newCameraMatrix2, Q;
if (calibrationFlagsPinholeController)
{
Cv.StereoRectifyFlags stereoRectifyFlags = (calibrationFlagsPinholeController.ZeroDisparity) ? Cv.StereoRectifyFlags.ZeroDisparity : 0;
Cv.StereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, rvec, tvec, out rotationMatrix1,
out rotationMatrix2, out newCameraMatrix1, out newCameraMatrix2, out Q, stereoRectifyFlags,
calibrationFlagsPinholeController.Skew, NewImageSize);
}
else if (calibrationFlagsOmnidirController)
{
Cv.Omnidir.StereoRectify(rvec, tvec, out rotationMatrix1, out rotationMatrix2);
newCameraMatrix1 = new Cv.Mat();
newCameraMatrix2 = new Cv.Mat();
}
else
{
rotationMatrix1 = new Cv.Mat();
rotationMatrix2 = new Cv.Mat();
newCameraMatrix1 = new Cv.Mat();
newCameraMatrix2 = new Cv.Mat();
}
// Save the camera parameters
stereoCameraParameters.RotationVector = rvec;
stereoCameraParameters.TranslationVector = tvec;
stereoCameraParameters.RotationMatrices = new Cv.Mat[] { rotationMatrix1, rotationMatrix2 };
stereoCameraParameters.NewCameraMatrices = new Cv.Mat[] { newCameraMatrix1, newCameraMatrix2 };
return(stereoCameraParameters);
}
