static void Main(string[] args)
{
MCvPoint3D32f[] _points;
Mat _left = CvInvoke.Imread("imL.png", ImreadModes.Color);
Mat _right = CvInvoke.Imread("imR.png", ImreadModes.Color);
Mat disparityMap = new Mat();
Stopwatch watch = Stopwatch.StartNew();
UMat leftGray = new UMat();
UMat rightGray = new UMat();
CvInvoke.CvtColor(_left, leftGray, ColorConversion.Bgr2Gray);
CvInvoke.CvtColor(_right, rightGray, ColorConversion.Bgr2Gray);
Mat points = new Mat();
Computer3DPointsFromStereoPair(leftGray, rightGray, disparityMap, points);
watch.Stop();
long disparityComputationTime = watch.ElapsedMilliseconds;
Mat pointsArray = points.Reshape(points.NumberOfChannels, points.Rows * points.Cols);
Mat colorArray = _left.Reshape(_left.NumberOfChannels, _left.Rows * _left.Cols);
Mat colorArrayFloat = new Mat();
colorArray.ConvertTo(colorArrayFloat, DepthType.Cv32F);
WCloud cloud = new WCloud(pointsArray, colorArray);
Emgu.CV.Viz3d v = new Emgu.CV.Viz3d("Simple stereo reconstruction");
WText wtext = new WText("3d point cloud", new System.Drawing.Point(20, 20), 20, new MCvScalar(255, 255, 255));
WCoordinateSystem wCoordinate = new WCoordinateSystem(1.0);
v.ShowWidget("text", wtext);
//v.ShowWidget("coordinate", wCoordinate);
v.ShowWidget("cloud", cloud);
v.Spin();
}
public static Viz3d GetViz3d(Mat left, Mat right)
{
Mat disparityMap = new Mat();
Stopwatch watch = Stopwatch.StartNew();
UMat leftGray = new UMat();
UMat rightGray = new UMat();
CvInvoke.CvtColor(left, leftGray, ColorConversion.Bgr2Gray);
CvInvoke.CvtColor(right, rightGray, ColorConversion.Bgr2Gray);
Mat points = new Mat();
Computer3DPointsFromStereoPair(leftGray, rightGray, disparityMap, points);
watch.Stop();
long disparityComputationTime = watch.ElapsedMilliseconds;
Mat pointsArray = points.Reshape(points.NumberOfChannels, points.Rows * points.Cols);
Mat colorArray = left.Reshape(left.NumberOfChannels, left.Rows * left.Cols);
Mat colorArrayFloat = new Mat();
colorArray.ConvertTo(colorArrayFloat, DepthType.Cv32F);
WCloud cloud = new WCloud(pointsArray, colorArray);
Emgu.CV.Viz3d v = new Emgu.CV.Viz3d("Simple stereo reconstruction");
WText wtext = new WText("3d point cloud", new System.Drawing.Point(20, 20), 20, new MCvScalar(255, 255, 255));
WCoordinateSystem wCoordinate = new WCoordinateSystem(1.0);
v.ShowWidget("text", wtext);
//v.ShowWidget("coordinate", wCoordinate);
v.ShowWidget("cloud", cloud);
return(v);
}
private void Display3D(Mat left, Mat right)
{
//TODO: try to use StereoSGBM
using (StereoBM stereoSolver = new StereoBM())
{
Mat output = new Mat();
Mat left8bit = ConvertInto8bitMat(left);
Mat right8bit = ConvertInto8bitMat(right);
stereoSolver.Compute(left8bit, right8bit, output);
Mat points = new Mat();
float scale = Math.Max(left.Size.Width, left.Size.Height);
if (!_isCalibrate)
{
Q = new Matrix <double>(
new double[, ]
{
{ 1.0, 0.0, 0.0, -left.Width / 2 }, //shift the x origin to image center
{ 0.0, -1.0, 0.0, left.Height / 2 }, //shift the y origin to image center and flip it upside down
{ 0.0, 0.0, -1.0, 0.0 }, //Multiply the z value by -1.0,
{ 0.0, 0.0, 0.0, scale }
});
_isCalibrate = true;
}
//Construct a simple Q matrix, if you have a matrix from cvStereoRectify, you should use that instead
//scale the object's coordinate to within a [-0.5, 0.5] cube
if (_isCameraMatrixCount)
{
Mat map11 = new Mat();
Mat map12 = new Mat();
Mat map21 = new Mat();
Mat map22 = new Mat();
CvInvoke.InitUndistortRectifyMap(cameraMatrix1, distCoeff1, R1, P1, left8bit.Size,
DepthType.Cv16S, map11, map12);
CvInvoke.InitUndistortRectifyMap(cameraMatrix2, distCoeff2, R2, P2, left8bit.Size,
DepthType.Cv16S, map21, map22);
Mat img1r = new Mat();
Mat img2r = new Mat();
CvInvoke.Remap(left8bit, img1r, map11, map12, Inter.Linear);
CvInvoke.Remap(right8bit, img2r, map21, map22, Inter.Linear);
left8bit = img1r;
right8bit = img2r;
}
//stereoSolver.FindStereoCorrespondence(left, right, disparityMap);
CvInvoke.ReprojectImageTo3D(output, points, Q, false, DepthType.Cv32F);
//points = PointCollection.ReprojectImageTo3D(output, Q);
Mat pointsArray = points.Reshape(points.NumberOfChannels, points.Rows * points.Cols);
Mat colorArray = left.Reshape(left.NumberOfChannels, left.Rows * left.Cols);
Mat colorArrayFloat = new Mat();
colorArray.ConvertTo(colorArrayFloat, DepthType.Cv32F);
WCloud cloud = new WCloud(pointsArray, colorArray);
Display3DImage(cloud);
//points = PointCollection.ReprojectImageTo3D(outputDisparityMap, q);
}
}
private void Display3DImage(WCloud cloud)
{
if (_is3dStart)
{
viz3d.RemoveWidget("cloud");
}
viz3d.ShowWidget("cloud", cloud);
viz3d.SpinOnce();
_is3dStart = true;
}
public static Viz3d GetViz3d(Mat pointMat, Mat colorMat)
{
WCloud cloud = new WCloud(pointMat, colorMat);
Emgu.CV.Viz3d v = new Emgu.CV.Viz3d("Simple stereo reconstruction");
WText wtext = new WText("3d point cloud", new System.Drawing.Point(20, 20), 20,
new MCvScalar(255, 255, 255));
WCoordinateSystem wCoordinate = new WCoordinateSystem(1.0);
v.ShowWidget("text", wtext);
//v.ShowWidget("coordinate", wCoordinate);
v.ShowWidget("cloud", cloud);
return(v);
}
static void Main(string[] args)
{
MCvPoint3D32f[] _points;
Mat _left = CvInvoke.Imread("imL.png", ImreadModes.Color);
Mat _right = CvInvoke.Imread("imR.png", ImreadModes.Color);
Mat disparityMap = new Mat();
Stopwatch watch = Stopwatch.StartNew();
UMat leftGray = new UMat();
UMat rightGray = new UMat();
CvInvoke.CvtColor(_left, leftGray, ColorConversion.Bgr2Gray);
CvInvoke.CvtColor(_right, rightGray, ColorConversion.Bgr2Gray);
Mat points = new Mat();
Computer3DPointsFromStereoPair(leftGray, rightGray, disparityMap, points);
watch.Stop();
long disparityComputationTime = watch.ElapsedMilliseconds;
Mat pointsArray = points.Reshape(points.NumberOfChannels, points.Rows * points.Cols);
Mat colorArray = _left.Reshape(_left.NumberOfChannels, _left.Rows * _left.Cols);
Mat colorArrayFloat = new Mat();
colorArray.ConvertTo(colorArrayFloat, DepthType.Cv32F);
WCloud cloud = new WCloud(pointsArray, colorArray);
// My attempt to grab a pixel.
Image <Bgr, Byte> image = disparityMap.ToImage <Bgr, Byte>();
int threshhold = 190;
bool objectFound = false;
for (int i = 0; i < image.Rows; i++)
{
for (int j = 0; j < disparityMap.Cols; j++)
{
// If it's below the threshhold black it out.
if (image.Data[i, j, 0] < threshhold)
{
image.Data[i, j, 2] = 0;
image.Data[i, j, 1] = 0;
image.Data[i, j, 0] = 0;
}
else
{
objectFound = true;
Console.Write("(" + image.Data[i, j, 0].ToString().PadLeft(3, '0') + ","); // Blue;
Console.Write(image.Data[i, j, 1].ToString().PadLeft(3, '0') + ","); // Green;
Console.Write(image.Data[i, j, 2].ToString().PadLeft(3, '0') + ")"); // Red;
}
}
Console.WriteLine(); // new row end line;
}
// 2D Disparity Display
Mat show = new Mat();
disparityMap.ConvertTo(show, DepthType.Cv8U);
CvInvoke.Imshow("Disparity", show);
disparityMap.ConvertTo(show, DepthType.Cv8U);
CvInvoke.Imshow("Disparity Restricted", image);
//try
//{
// if (objectFound)
// {
// SerialPort serialPort1 = new SerialPort("COM6", 9600);
// serialPort1.Open();
// serialPort1.WriteLine("");
// System.Threading.Thread.Sleep(1000);
// serialPort1.WriteLine("");
// serialPort1.Close();
// }
//}
//catch (Exception ex)
//{
//}
// 3D IMAGE DISPLAY
Emgu.CV.Viz3d v = new Emgu.CV.Viz3d("Simple stereo reconstruction");
WText wtext = new WText("3d point cloud", new System.Drawing.Point(20, 20), 20, new MCvScalar(255, 255, 255));
WCoordinateSystem wCoordinate = new WCoordinateSystem(1.0);
v.ShowWidget("text", wtext);
//v.ShowWidget("coordinate", wCoordinate);
v.ShowWidget("cloud", cloud);
v.Spin();
CvInvoke.WaitKey(0);
}
