/// <summary>
/// Estimates transformation between the 2 cameras making a stereo pair. If we have a stereo camera, where the relative position and orientatation of the 2 cameras is fixed, and if we computed poses of an object relative to the fist camera and to the second camera, (R1, T1) and (R2, T2), respectively (that can be done with cvFindExtrinsicCameraParams2), obviously, those poses will relate to each other, i.e. given (R1, T1) it should be possible to compute (R2, T2) - we only need to know the position and orientation of the 2nd camera relative to the 1st camera. That's what the described function does. It computes (R, T) such that:
/// R2=R*R1,
/// T2=R*T1 + T
/// </summary>
/// <param name="objectPoints">The 3D location of the object points. The first index is the index of image, second index is the index of the point</param>
/// <param name="imagePoints1">The 2D image location of the points for camera 1. The first index is the index of the image, second index is the index of the point</param>
/// <param name="imagePoints2">The 2D image location of the points for camera 2. The first index is the index of the image, second index is the index of the point</param>
/// <param name="intrinsicParam1">The intrisinc parameters for camera 1, might contains some initial values. The values will be modified by this function.</param>
/// <param name="intrinsicParam2">The intrisinc parameters for camera 2, might contains some initial values. The values will be modified by this function.</param>
/// <param name="imageSize">Size of the image, used only to initialize intrinsic camera matrix</param>
/// <param name="flags">Different flags</param>
/// <param name="extrinsicParams">The extrinsic parameters which contains:
/// R - The rotation matrix between the 1st and the 2nd cameras' coordinate systems;
/// T - The translation vector between the cameras' coordinate systems. </param>
/// <param name="essentialMatrix">The essential matrix</param>
/// <param name="termCrit">Termination criteria for the iterative optimiziation algorithm </param>
/// <param name="foundamentalMatrix">The fundamental matrix</param>
public static void StereoCalibrate(
MCvPoint3D32f[][] objectPoints,
PointF[][] imagePoints1,
PointF[][] imagePoints2,
IntrinsicCameraParameters intrinsicParam1,
IntrinsicCameraParameters intrinsicParam2,
Size imageSize,
CvEnum.CALIB_TYPE flags,
MCvTermCriteria termCrit,
out ExtrinsicCameraParameters extrinsicParams,
out Matrix <double> foundamentalMatrix,
out Matrix <double> essentialMatrix)
{
Debug.Assert(objectPoints.Length == imagePoints1.Length && objectPoints.Length == imagePoints2.Length, "The number of images for objects points should be equal to the number of images for image points");
#region get the matrix that represent the point counts
int[,] pointCounts = new int[objectPoints.Length, 1];
for (int i = 0; i < objectPoints.Length; i++)
{
Debug.Assert(objectPoints[i].Length == imagePoints1[i].Length && objectPoints[i].Length == imagePoints2[i].Length, String.Format("Number of 3D points and image points should be equal in the {0}th image", i));
pointCounts[i, 0] = objectPoints[i].Length;
}
#endregion
using (Matrix <float> objectPointMatrix = ToMatrix(objectPoints))
using (Matrix <float> imagePointMatrix1 = ToMatrix(imagePoints1))
using (Matrix <float> imagePointMatrix2 = ToMatrix(imagePoints2))
using (Matrix <int> pointCountsMatrix = new Matrix <int>(pointCounts))
{
extrinsicParams = new ExtrinsicCameraParameters();
essentialMatrix = new Matrix <double>(3, 3);
foundamentalMatrix = new Matrix <double>(3, 3);
CvInvoke.cvStereoCalibrate(
objectPointMatrix.Ptr,
imagePointMatrix1.Ptr,
imagePointMatrix2.Ptr,
pointCountsMatrix.Ptr,
intrinsicParam1.IntrinsicMatrix,
intrinsicParam1.DistortionCoeffs,
intrinsicParam2.IntrinsicMatrix,
intrinsicParam2.DistortionCoeffs,
imageSize,
extrinsicParams.RotationVector,
extrinsicParams.TranslationVector,
essentialMatrix.Ptr,
foundamentalMatrix.Ptr,
termCrit,
flags);
}
}
