/// <summary>
/// Given the left and right image, computer the disparity map and the 3D point cloud.
/// </summary>
/// <param name="left">The left image</param>
/// <param name="right">The right image</param>
/// <param name="leftDisparityMap">The left disparity map</param>
/// <param name="points">The 3D point cloud within a [-0.5, 0.5] cube</param>
private static void Computer3DPointsFromImages(Image<Gray, Byte> left, Image<Gray, Byte> right, out Image<Gray, Int16> leftDisparityMap, out MCvPoint3D32f[] points)
{
Size size = left.Size;
using (Image<Gray, Int16> leftDisparity = new Image<Gray, Int16>(size))
using (Image<Gray, Int16> rightDisparity = new Image<Gray, Int16>(size))
using (StereoGC gc = new StereoGC(16, 2))
{
gc.FindStereoCorrespondence(left, right, leftDisparity, rightDisparity);
leftDisparityMap = leftDisparity * (-16);
float scale = Math.Max(size.Width, size.Height);
//Construct a simple Q matrix, if you have a matrix from cvStereoRectify, you should use that instead
using (Matrix<double> q = new Matrix<double>(
new double[,] {
{1.0, 0.0, 0.0, -size.Width/2}, //shift the x origin to image center
{0.0, -1.0, 0.0, size.Height/2}, //shift the y origin to image center and flip it upside down
{0.0, 0.0, 16.0, 0.0}, //Multiply the z value by 16,
{0.0, 0.0, 0.0, scale}})) //scale the object's corrdinate to within a [-0.5, 0.5] cube
points = PointCollection.ReprojectImageTo3D(leftDisparity, q);
}
}