/// <summary>
/// Initialize the non serialized properties from the serialized properties.
/// </summary>
public void UpdateNonSerializedProperties()
{
RotationVector = new Cv.Vec3d(RotationVectorValues[0], RotationVectorValues[1], RotationVectorValues[2]);
TranslationVector = new Cv.Vec3d(TranslationVectorValues[0], TranslationVectorValues[1], TranslationVectorValues[2]);
RotationMatrices = CameraParameters.CreateProperty(RotationMatricesType, RotationMatricesValues);
NewCameraMatrices = CameraParameters.CreateProperty(NewCameraMatricesType, NewCameraMatricesValues);
}
/// <summary>
/// Update the gameObject's transform of an ArUco object.
/// </summary>
/// <param name="arucoObject">The ArUco object to place.</param>
/// <param name="rvec">The estimated rotation vector of the ArUco object.</param>
/// <param name="tvec">The estimated translation vector of the ArUco object.</param>
/// <param name="cameraId">The id of the camera to use. The gameObject is placed and oriented relative to this camera.</param>
/// <param name="positionFactor">Factor on the position vector.</param>
protected void PlaceArucoObject(ArucoObject arucoObject, Cv.Vec3d rvec, Cv.Vec3d tvec, int cameraId, float positionFactor = 1f)
{
GameObject arucoGameObject = arucoObject.gameObject;
arucoGameObject.transform.SetParent(arucoTracker.ArucoCamera.ImageCameras[cameraId].transform);
arucoGameObject.transform.localPosition = tvec.ToPosition() * positionFactor;
arucoGameObject.transform.localRotation = rvec.ToRotation();
arucoGameObject.SetActive(true);
}
/// <summary>
/// Update the gameObject's transform of an ArUco object.
/// </summary>
/// <param name="arucoObject">The ArUco object to place.</param>
/// <param name="rvec">The estimated rotation vector of the ArUco object.</param>
/// <param name="tvec">The estimated translation vector of the ArUco object.</param>
/// <param name="cameraId">The id of the camera to use. The gameObject is placed and oriented relative to this camera.</param>
/// <param name="positionFactor">Factor on the position vector.</param>
protected void PlaceArucoObject(ArucoObject arucoObject, Cv.Vec3d rvec, Cv.Vec3d tvec, int cameraId, float positionFactor = 1f)
{
GameObject arucoGameObject = arucoObject.gameObject;
arucoGameObject.transform.SetParent(arucoTracker.ArucoCamera.ImageCameras[cameraId].transform);
arucoGameObject.transform.localPosition = tvec.ToPosition() * positionFactor
+ arucoGameObject.transform.up * arucoGameObject.transform.localScale.y / 2; // Move up the object to coincide with the marker;
arucoGameObject.transform.localRotation = rvec.ToRotation();
arucoGameObject.SetActive(true);
}
public override void EstimateTransforms(int cameraId, Aruco.Dictionary dictionary)
{
foreach (var arucoGridBoard in arucoTracker.GetArucoObjects <ArucoGridBoard>(dictionary))
{
Cv.Vec3d rvec = null, tvec = null;
int markersUsedForEstimation = 0;
if (arucoTracker.MarkerTracker.DetectedMarkers[cameraId][dictionary] > 0 && cameraParameters != null)
{
markersUsedForEstimation = Aruco.EstimatePoseBoard(arucoTracker.MarkerTracker.MarkerCorners[cameraId][dictionary],
arucoTracker.MarkerTracker.MarkerIds[cameraId][dictionary], arucoGridBoard.Board, cameraParameters.CameraMatrices[cameraId],
cameraParameters.DistCoeffs[cameraId], out rvec, out tvec);
}
arucoGridBoard.Rvec = rvec;
arucoGridBoard.Tvec = tvec;
arucoGridBoard.MarkersUsedForEstimation = markersUsedForEstimation;
}
}
public override void EstimateTransforms(int cameraId, Aruco.Dictionary dictionary)
{
foreach (var arucoCharucoBoard in arucoTracker.GetArucoObjects <ArucoCharucoBoard>(dictionary))
{
Cv.Vec3d rvec = null, tvec = null;
bool validTransform = false;
if (arucoTracker.MarkerTracker.DetectedMarkers[cameraId][dictionary] > 0 && cameraParameters != null)
{
validTransform = Aruco.EstimatePoseCharucoBoard(arucoCharucoBoard.DetectedCorners, arucoCharucoBoard.DetectedIds,
(Aruco.CharucoBoard)arucoCharucoBoard.Board, cameraParameters.CameraMatrices[cameraId], cameraParameters.DistCoeffs[cameraId], out rvec,
out tvec);
}
arucoCharucoBoard.Rvec = rvec;
arucoCharucoBoard.Tvec = tvec;
arucoCharucoBoard.ValidTransform = validTransform;
}
}
/// <summary>
/// Initializes the non serialized properties from the serialized properties.
/// </summary>
public void UpdateNonSerializedProperties()
{
RotationVector = new Cv.Vec3d(RotationVectorValues[0], RotationVectorValues[1], RotationVectorValues[2]);
TranslationVector = new Cv.Vec3d(TranslationVectorValues[0], TranslationVectorValues[1],
TranslationVectorValues[2]);
}
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);
}
