Exemplo n.º 1
0
        private void ProcessFrameFindFaces()
        {
            var stereoCalibration = Options.StereoCalibrationOptions;

            if (stereoCalibration == null)
            {
                return;
            }

            var leftImageR  = new Image <Gray, byte>(_cameras[0].Image.Width, _cameras[0].Image.Height);
            var rightImageR = new Image <Gray, byte>(_cameras[1].Image.Width, _cameras[1].Image.Height);

            try
            {
                CvInvoke.cvRemap(_cameras[0].Image.Ptr, leftImageR.Ptr,
                                 stereoCalibration.MapXLeft, stereoCalibration.MapYLeft, 0, new MCvScalar(0));
            }
            catch (Exception ex)
            {
            }

            CvInvoke.cvRemap(_cameras[1].Image.Ptr, rightImageR.Ptr,
                             stereoCalibration.MapXRight, stereoCalibration.MapYRight, 0, new MCvScalar(0));

            // find first face points
            var leftFaceRegions  = Helper2D.GetFaceRegion2Ds(leftImageR, FaceWidth, FaceHeight, true, true);
            var rightFaceRegions = Helper2D.GetFaceRegion2Ds(rightImageR, FaceWidth, FaceHeight, true, true);

            FaceRegion2D leftFace;
            FaceRegion2D rightFace;

            if (leftFaceRegions != null &&
                rightFaceRegions != null &&
                (leftFace = leftFaceRegions.FirstOrDefault()) != null &&
                (rightFace = rightFaceRegions.FirstOrDefault()) != null)
            {
                if (leftFace.EyeAngle != 0)
                {
                    _leftRoll = leftFace.EyeAngle;
                }
                if (rightFace.EyeAngle != 0)
                {
                    _rightRoll = rightFace.EyeAngle;
                }

                var leftPoints  = new Point[4]; // face location, left eye, right eye, mouth
                var rightPoints = new Point[4];

                #region Points

                // face
                leftPoints[0]  = new Point(leftFace.Face.Location.X + leftFace.Face.Width / 2, leftFace.Face.Location.Y + leftFace.Face.Height / 2);
                rightPoints[0] = new Point(rightFace.Face.Location.X + rightFace.Face.Width / 2, rightFace.Face.Location.Y + rightFace.Face.Height / 2);

                // left eye
                if (leftFace.LeftEye != null && rightFace.LeftEye != null)
                {
                    leftPoints[1] = new Point(leftFace.Face.Location.X + leftFace.LeftEye.Location.X + leftFace.LeftEye.Width / 2,
                                              leftFace.Face.Location.Y + leftFace.LeftEye.Location.Y + leftFace.LeftEye.Height / 2);

                    rightPoints[1] = new Point(rightFace.Face.Location.X + rightFace.LeftEye.Location.X + rightFace.LeftEye.Width / 2,
                                               rightFace.Face.Location.Y + rightFace.LeftEye.Location.Y + rightFace.LeftEye.Height / 2);
                }

                // right eye
                if (leftFace.RightEye != null && rightFace.RightEye != null)
                {
                    leftPoints[2] = new Point(leftFace.Face.Location.X + leftFace.RightEye.Location.X + leftFace.RightEye.Width / 2,
                                              leftFace.Face.Location.Y + leftFace.RightEye.Location.Y + leftFace.RightEye.Height / 2);

                    rightPoints[2] = new Point(rightFace.Face.Location.X + rightFace.RightEye.Location.X + rightFace.RightEye.Width / 2,
                                               rightFace.Face.Location.Y + rightFace.RightEye.Location.Y + rightFace.RightEye.Height / 2);
                }

                // mouth
                if (leftFace.Mouth != null && rightFace.Mouth != null)
                {
                    leftPoints[3] = new Point(leftFace.Face.Location.X + leftFace.Mouth.Location.X + leftFace.Mouth.Width / 2,
                                              leftFace.Face.Location.Y + leftFace.Mouth.Location.Y + leftFace.Mouth.Height / 2);

                    rightPoints[3] = new Point(rightFace.Face.Location.X + rightFace.Mouth.Location.X + rightFace.Mouth.Width / 2,
                                               rightFace.Face.Location.Y + rightFace.Mouth.Location.Y + rightFace.Mouth.Height / 2);
                }

                #endregion

                #region Manual Point Cloud Calculation

                {
                    var pointCloud = new MCvPoint3D64f[leftPoints.Length];

                    #region Calculate Point Cloud

                    for (int i = 0; i < leftPoints.Length; i++)
                    {
                        if (leftPoints[i].X == 0 && leftPoints[i].Y == 0)
                        {
                            continue;
                        }

                        var d = rightPoints[i].X - leftPoints[i].X;

                        var X = leftPoints[i].X * stereoCalibration.Q[0, 0] + stereoCalibration.Q[0, 3];
                        var Y = leftPoints[i].Y * stereoCalibration.Q[1, 1] + stereoCalibration.Q[1, 3];
                        var Z = stereoCalibration.Q[2, 3];
                        var W = d * stereoCalibration.Q[3, 2] + stereoCalibration.Q[3, 3];

                        X = X / W;
                        Y = Y / W;
                        Z = Z / W;

                        leftImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, leftPoints[i], new Gray(255));
                        rightImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, rightPoints[i], new Gray(255));

                        pointCloud[i] = new MCvPoint3D64f(X, Y, Z);
                    }

                    #endregion


                    _foundFace3d = new Face3D()
                    {
                        Location = pointCloud[0].x == 0 && pointCloud[0].y == 0 && pointCloud[0].z == 0 ? (MCvPoint3D64f?)null : pointCloud[0],
                        LeftEye  = pointCloud[1].x == 0 && pointCloud[1].y == 0 && pointCloud[1].z == 0 ? (MCvPoint3D64f?)null : pointCloud[1],
                        RightEye = pointCloud[2].x == 0 && pointCloud[2].y == 0 && pointCloud[2].z == 0 ? (MCvPoint3D64f?)null : pointCloud[2],
                        Mouth    = pointCloud[3].x == 0 && pointCloud[3].y == 0 && pointCloud[3].z == 0 ? (MCvPoint3D64f?)null : pointCloud[3],
                    };

                    if (_foundFace3d.LeftEye != null &&
                        _foundFace3d.RightEye != null &&
                        _foundFace3d.Mouth != null)
                    {
                        var srcMatrix = new Matrix <float>(3, 4);

                        srcMatrix[0, 0] = (float)_foundFace3d.LeftEye.Value.x;
                        srcMatrix[1, 0] = (float)_foundFace3d.LeftEye.Value.y;
                        srcMatrix[2, 0] = (float)_foundFace3d.LeftEye.Value.z;

                        srcMatrix[0, 1] = (float)_foundFace3d.RightEye.Value.x;
                        srcMatrix[1, 1] = (float)_foundFace3d.RightEye.Value.y;
                        srcMatrix[2, 1] = (float)_foundFace3d.RightEye.Value.z;

                        srcMatrix[0, 2] = (float)_foundFace3d.Mouth.Value.x;
                        srcMatrix[1, 2] = (float)_foundFace3d.Mouth.Value.y;
                        srcMatrix[2, 2] = (float)_foundFace3d.Mouth.Value.z;

                        srcMatrix[0, 3] = (float)_foundFace3d.Location.Value.x;
                        srcMatrix[1, 3] = (float)_foundFace3d.Location.Value.y;
                        srcMatrix[2, 3] = (float)_foundFace3d.Location.Value.z;


                        var dstMatrix = new Matrix <float>(3, 4);

                        dstMatrix[0, 0] = (float)_foundFace3d.LeftEye.Value.x;
                        dstMatrix[1, 0] = (float)_foundFace3d.LeftEye.Value.y;
                        dstMatrix[2, 0] = (float)30;

                        dstMatrix[0, 1] = (float)_foundFace3d.RightEye.Value.x;
                        dstMatrix[1, 1] = (float)_foundFace3d.RightEye.Value.y;
                        dstMatrix[2, 1] = (float)30;

                        dstMatrix[0, 2] = (float)_foundFace3d.Mouth.Value.x;
                        dstMatrix[1, 2] = (float)_foundFace3d.Mouth.Value.y;
                        dstMatrix[2, 2] = (float)30;

                        dstMatrix[0, 3] = (float)_foundFace3d.Location.Value.x;
                        dstMatrix[1, 3] = (float)_foundFace3d.Location.Value.y;
                        dstMatrix[2, 3] = (float)30;

                        HomographyMatrix homographyMatrix = CameraCalibration.FindHomography(srcMatrix, dstMatrix, HOMOGRAPHY_METHOD.DEFAULT, 1);

                        if (homographyMatrix != null)
                        {
                            try
                            {
                                leftImageR = leftImageR.WarpPerspective(homographyMatrix, INTER.CV_INTER_LINEAR, WARP.CV_WARP_DEFAULT, new Gray(0));
                            }
                            catch (Exception ex)
                            {
                            }
                        }
                    }
                }

                #endregion

                #region Automatic Point Cloud

                {
                    _imagePointsDisparity = new Image <Gray, byte>(_cameras[0].Image.Width, _cameras[0].Image.Height);
                    _imagePointsLeft      = new Image <Gray, byte>(_cameras[0].Image.Width, _cameras[0].Image.Height, new Gray(255));
                    _imagePointsRight     = new Image <Gray, byte>(_cameras[0].Image.Width, _cameras[0].Image.Height, new Gray(255));

                    for (int i = 0; i < leftPoints.Length; i++)
                    {
                        if (leftPoints[i].X == 0 && leftPoints[i].Y == 0)
                        {
                            continue;
                        }

                        _imagePointsLeft.Draw(new Rectangle(new Point(leftPoints[i].X, leftPoints[i].Y), new Size(10, 10)), new Gray(0), 10);
                        _imagePointsRight.Draw(new Rectangle(new Point(rightPoints[i].X, rightPoints[i].Y), new Size(10, 10)), new Gray(0), 10);
                    }

                    var imagePointsDisparityGpu = new GpuImage <Gray, byte>(_imagePointsDisparity);

                    _stereoSolver.FindStereoCorrespondence(new GpuImage <Gray, byte>(_imagePointsLeft), new GpuImage <Gray, byte>(_imagePointsRight),
                                                           imagePointsDisparityGpu, null);

                    _imagePointsDisparity = imagePointsDisparityGpu.ToImage();



                    //MCvPoint3D32f[] pointCloud = PointCollection.ReprojectImageTo3D(_imagePointsDisparity, stereoCalibration.Q);

                    //var filteredPointCloud = pointCloud.
                    //    Where(item => item.z != 10000).
                    //    GroupBy(item => item.z).
                    //    Select(item => new
                    //    {
                    //        z = item.Key,
                    //        x = item.Average(point => point.x),
                    //        y = item.Average(point => point.y)
                    //    }).ToArray();

                    //for (int i = 0; i < filteredPointCloud.Length; i++)
                    //{
                    //    _imagePointsDisparity.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", filteredPointCloud[i].x, filteredPointCloud[i].y, filteredPointCloud[i].z),
                    //        ref _font, new Point((int)filteredPointCloud[i].x, (int)filteredPointCloud[i].y), new Gray(255));
                    //}
                }

                #endregion
            }

            var oldLeft  = _cameras[0].Image;
            var oldRight = _cameras[1].Image;

            _cameras[0].Image = leftImageR;
            _cameras[1].Image = rightImageR;

            oldLeft.Dispose();
            oldRight.Dispose();
        }
Exemplo n.º 2
0
        private void ProcessFrame()
        {
            while (_captureContinue)
            {
                while (_processQueueLength != 2)
                {
                    Thread.Sleep(SleepInterval);
                }

                if ((Helper2D.FilterInstances.Mode == Mode.Cpu && useGpuMenuItem.Checked) ||
                    (Helper2D.FilterInstances.Mode == Mode.Gpu && useGpuMenuItem.Checked == false))
                {
                    Helper2D.FilterInstances = new FilterInstances(useGpuMenuItem.Checked ? Mode.Gpu : Mode.Cpu);
                }

                double distance;

                if (_cameras[0].FaceRegions == null ||
                    _cameras[1].FaceRegions == null)
                {
                    _resetCorrelation = true;
                }

                if (_resetCorrelation == false)
                {
                    #region Find correlation with previous face regions

                    // iterate through all cameras and track faces
                    foreach (Camera camera in _cameras)
                    {
                        var newRawFaceRegions = new List <FaceRegion2D>();

                        // iterate through every face found previously, rotate image and find faces
                        foreach (FaceRegion2D faceRegion in camera.FaceRegions)
                        {
                            Image <Bgr, byte> image = camera.Image.Rotate(faceRegion.EyeAngle,
                                                                          new PointF(faceRegion.Face.Location.X + faceRegion.Face.Width / 2, faceRegion.Face.Location.Y + faceRegion.Face.Height / 2),
                                                                          INTER.CV_INTER_CUBIC, _rotateBackground, true);

                            if (_debugRotation)
                            {
                                camera.Image = image;
                            }

                            // find faces in rotated image
                            newRawFaceRegions.AddRange(Helper2D.GetFaceRegion2Ds(image, FaceWidth, FaceHeight, true, false));
                        }

                        // find best corespondence between old faces and new faces
                        IEnumerable <Tuple <int, int> > corespondences = Helper.FindCorespondence
                                                                             (camera.FaceRegions.Select(item => item.Face.Location).ToArray(),
                                                                             newRawFaceRegions.Select(item => item.Face.Location).ToArray(),
                                                                             out distance);

                        if (corespondences == null ||
                            corespondences.Any() == false)
                        {
                            // face regions lost .. RESET both cameras
                            _resetCorrelation = true;
                            break;
                        }

                        var newFaceRegions = new FaceRegion2D[corespondences.Count()];

                        for (int i = 0; i < corespondences.Count(); i++)
                        {
                            FaceRegion2D faceRegion = newRawFaceRegions.ElementAt(corespondences.ElementAt(i).Item2);

                            faceRegion.SetHistory(camera.FaceRegions.ElementAt(corespondences.ElementAt(i).Item1));

                            newFaceRegions[i] = faceRegion;
                        }

                        camera.FaceRegions = newFaceRegions;
                    }

                    #endregion
                }

                if (_resetCorrelation)
                {
                    #region Reset Found Faces

                    foreach (Camera camera in _cameras)
                    {
                        camera.FaceRegions = Helper2D.GetFaceRegion2Ds(camera.Image, FaceWidth, FaceHeight, true, false);
                    }

                    #endregion
                }

                if (_cameras[0].FaceRegions.Length > 0 &&
                    _cameras[1].FaceRegions.Length > 0)
                {
                    #region Find correlation in stereo images and add history

                    IEnumerable <Point>[] points = _cameras.Select(camera => camera.FaceRegions.
                                                                   Select(region => region.Face.Location)).ToArray();

                    List <Tuple <int, int> > correlations = Helper.FindCorespondence(points.ElementAt(0), points.ElementAt(1), out distance).ToList();


                    // images have incorect correlations and history
                    if (_resetCorrelation == false &&
                        correlations.Any(item => _cameras[0].FaceRegions.ElementAt(item.Item1).Id != _cameras[1].FaceRegions.ElementAt(item.Item2).Id))
                    {
                        _resetCorrelation = true;
                    }

                    if (_resetCorrelation)
                    {
                        // assign faces color and Id
                        foreach (var correlation in correlations)
                        {
                            var color = new Bgr(_random.NextDouble() * 255, _random.NextDouble() * 255, _random.NextDouble() * 255);

                            FaceRegion2D leftFaceRegion  = _cameras[0].FaceRegions.ElementAt(correlation.Item1);
                            FaceRegion2D rightFaceRegion = _cameras[1].FaceRegions.ElementAt(correlation.Item2);

                            rightFaceRegion.Id = leftFaceRegion.Id;

                            leftFaceRegion.BoundingBoxColor  = color;
                            rightFaceRegion.BoundingBoxColor = color;
                        }
                    }

                    #endregion

                    #region Recognize Faces

                    _cameras.ForEach(camera =>
                    {
                        if (camera.FaceRegions != null)
                        {
                            camera.FaceRegions.ToList().ForEach(faceRegion =>
                            {
                                Helper.DrawFaceRegionCircle(camera.Image, faceRegion, faceRegion.BoundingBoxColor);

                                string label = HelperFaces.Recognize(faceRegion.FaceImage);

                                camera.Image.Draw(string.Format("{0}", label),
                                                  ref _font, faceRegion.Face.Location, new Bgr(0, 0, 255));
                            });
                        }
                    });

                    #endregion

                    EventHandler <FaceRegionsEventArgs> facesAvailableHandler = FacesAvailable;
                    if (facesAvailableHandler != null)
                    {
                        facesAvailableHandler(this, new FaceRegionsEventArgs(_cameras[0].FaceRegions, _cameras[1].FaceRegions, null));
                    }

                    _faces = _cameras.SelectMany(camera => camera.FaceRegions).Select(item => item.FaceImage).ToArray();
                }

                _resetCorrelation = false;

                PostProcess();

                lock (this)
                {
                    _processQueueLength = 0;
                }
            }
        }
Exemplo n.º 3
0
        private void CaptureFrames()
        {
            while (_captureContinue)
            {
                while (_processQueueLength == 2)
                {
                    Thread.Sleep(SleepInterval);
                }

                Invoke(new Action(delegate
                {
                    _cameras[0].Image = _capture1.QueryGrayFrame();
                    _cameras[1].Image = _capture2.QueryGrayFrame();

                    _processQueueLength = 2;
                }));

                while (_processQueueLength != 0)
                {
                    Thread.Sleep(SleepInterval);
                }

                if (IsDisposed == false)
                {
                    Invoke(new Action(delegate
                    {
                        Image <Bgr, byte> image = _cameras[0].Image.ConcateHorizontal(_cameras[1].Image).Convert <Bgr, byte>();

                        if (_debugCorespondence)
                        {
                            (from left in _cameras[0].FaceRegions
                             join right in _cameras[1].FaceRegions on left.Id equals right.Id
                             select new { Left = left, Right = right }).ToList().
                            ForEach(pair => image.Draw(new LineSegment2D(new Point(pair.Left.Face.Location.X + pair.Left.Face.Width / 2, pair.Left.Face.Location.Y + pair.Left.Face.Height / 2),
                                                                         new Point(pair.Right.Face.Location.X + _cameras[0].Image.Width + pair.Right.Face.Width / 2, pair.Right.Face.Location.Y + pair.Right.Face.Height / 2)),
                                                       pair.Left.BoundingBoxColor, 2));
                        }

                        imageBoxCamera.Image = image;

                        if (_foundFace3d != null)
                        {
                            decimal roll       = (decimal)Helper2D.CalculateFaceRoll(_foundFace3d);
                            decimal yaw        = (decimal)Helper2D.CalculateFaceYaw(_foundFace3d);
                            decimal pitch      = (decimal)Helper2D.CalculateFacePitch(_foundFace3d);
                            nudFaceRoll.Value  = roll == 0 ? nudFaceRoll.Value : roll;
                            nudFaceYaw.Value   = yaw == 0 ? nudFaceYaw.Value : yaw;
                            nudFacePitch.Value = pitch == 0 ? nudFacePitch.Value : pitch;
                        }

                        nudLeftRoll.Value  = (decimal)_leftRoll;
                        nudRightRoll.Value = (decimal)_rightRoll;

                        if (_imagePointsDisparity != null)
                        {
                            imageBoxPoints.Image = _imagePointsDisparity;
                        }

                        //imageBoxTransformed.Image = _transformed;

                        //if (_faces != null)
                        //{
                        //    Image<Gray, byte> firstFace = _faces.First();

                        //    _faces.Skip(1).ToList().ForEach(face => firstFace = firstFace.ConcateHorizontal(face));

                        //    imageBoxFaces.Image = firstFace;
                        //}
                        //else
                        //{
                        //    imageBoxFaces.Image = null;
                        //}

                        Application.DoEvents();
                    }));
                }
            }
        }
Exemplo n.º 4
0
        private void ProcessFrameFindFaces()
        {
            if (Options.StereoCalibrationOptions == null)
            {
                return;
            }

            var leftImageR  = new Image <Gray, byte>(new Size(_cameras[0].Image.Width, _cameras[0].Image.Height));
            var rightImageR = new Image <Gray, byte>(new Size(_cameras[1].Image.Width, _cameras[1].Image.Height));

            CvInvoke.cvRemap(_cameras[0].Image.Ptr, leftImageR.Ptr,
                             Options.StereoCalibrationOptions.MapXLeft, Options.StereoCalibrationOptions.MapYLeft, 0, new MCvScalar(0));

            CvInvoke.cvRemap(_cameras[1].Image.Ptr, rightImageR.Ptr,
                             Options.StereoCalibrationOptions.MapXRight, Options.StereoCalibrationOptions.MapYRight, 0, new MCvScalar(0));

            //PointCollection.ReprojectImageTo3D()

            // find first face points
            var leftFaceRegions  = Helper2D.GetFaceRegion2Ds(leftImageR, FaceWidth, FaceHeight, true, true);
            var rightFaceRegions = Helper2D.GetFaceRegion2Ds(rightImageR, FaceWidth, FaceHeight, true, true);

            FaceRegion2D leftFace;
            FaceRegion2D rightFace;

            if (leftFaceRegions != null &&
                rightFaceRegions != null &&
                (leftFace = leftFaceRegions.FirstOrDefault()) != null &&
                (rightFace = rightFaceRegions.FirstOrDefault()) != null)
            {
                var leftPoints  = new List <Point>();
                var rightPoints = new List <Point>();

                #region Points

                // face
                leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.Face.Width / 2, leftFace.Face.Location.Y + leftFace.Face.Height / 2));
                rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.Face.Width / 2, rightFace.Face.Location.Y + rightFace.Face.Height / 2));

                // left eye
                if (leftFace.LeftEye != null && rightFace.LeftEye != null)
                {
                    leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.LeftEye.Location.X + leftFace.LeftEye.Width / 2,
                                             leftFace.Face.Location.Y + leftFace.LeftEye.Location.Y + leftFace.LeftEye.Height / 2));

                    rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.LeftEye.Location.X + rightFace.LeftEye.Width / 2,
                                              rightFace.Face.Location.Y + rightFace.LeftEye.Location.Y + rightFace.LeftEye.Height / 2));
                }

                // right eye
                if (leftFace.RightEye != null && rightFace.RightEye != null)
                {
                    leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.RightEye.Location.X + leftFace.RightEye.Width / 2,
                                             leftFace.Face.Location.Y + leftFace.RightEye.Location.Y + leftFace.RightEye.Height / 2));

                    rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.RightEye.Location.X + rightFace.RightEye.Width / 2,
                                              rightFace.Face.Location.Y + rightFace.RightEye.Location.Y + rightFace.RightEye.Height / 2));
                }

                // mouth
                if (leftFace.Mouth != null && rightFace.Mouth != null)
                {
                    leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.Mouth.Location.X + leftFace.Mouth.Width / 2,
                                             leftFace.Face.Location.Y + leftFace.Mouth.Location.Y + leftFace.Mouth.Height / 2));

                    rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.Mouth.Location.X + rightFace.Mouth.Width / 2,
                                              rightFace.Face.Location.Y + rightFace.Mouth.Location.Y + rightFace.Mouth.Height / 2));
                }

                #endregion

                var pointCloud = new MCvPoint3D64f[leftPoints.Count];

                #region Calculate Point Cloud

                for (int i = 0; i < leftPoints.Count; i++)
                {
                    var d = rightPoints[i].X - leftPoints[i].X;

                    var X = leftPoints[i].X * Options.StereoCalibrationOptions.Q[0, 0] + Options.StereoCalibrationOptions.Q[0, 3];
                    var Y = leftPoints[i].Y * Options.StereoCalibrationOptions.Q[1, 1] + Options.StereoCalibrationOptions.Q[1, 3];
                    var Z = Options.StereoCalibrationOptions.Q[2, 3];
                    var W = d * Options.StereoCalibrationOptions.Q[3, 2] + Options.StereoCalibrationOptions.Q[3, 3];

                    X = X / W;
                    Y = Y / W;
                    Z = Z / W;

                    leftImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, leftPoints[i], new Gray(255));
                    rightImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, rightPoints[i], new Gray(255));

                    pointCloud[i] = new MCvPoint3D64f(X, Y, Z);
                }

                #endregion

                if (pointCloud.Length >= 4)
                {
                    var srcPoints = new Matrix <float>(pointCloud.Length, 3);
                    var dstPoints = new Matrix <float>(pointCloud.Length, 3);

                    for (int i = 0; i < pointCloud.Length; i++)
                    {
                        srcPoints[i, 0] = (float)pointCloud[i].x;
                        srcPoints[i, 1] = (float)pointCloud[i].y;
                        srcPoints[i, 2] = (float)pointCloud[i].z;

                        dstPoints[i, 0] = (float)pointCloud[i].x;
                        dstPoints[i, 1] = (float)pointCloud[i].y;
                        dstPoints[i, 2] = 0;
                    }

                    var mapMatrix = new Matrix <double>(3, 3);
                    CvInvoke.cvGetPerspectiveTransform(srcPoints.Ptr, dstPoints.Ptr, mapMatrix.Ptr);

                    try
                    {
                        if (_transformed == null)
                        {
                            _transformed = new Image <Gray, byte>(leftImageR.Width, leftImageR.Height);
                        }

                        CvInvoke.cvPerspectiveTransform(leftImageR.Ptr, _transformed.Ptr, mapMatrix.Ptr);

                        //_transformed = leftImageR.WarpAffine(mapMatrix, INTER.CV_INTER_CUBIC, WARP.CV_WARP_DEFAULT, new Gray(0));
                    }
                    catch (Exception ex)
                    {
                    }

                    //HomographyMatrix homographyMatrix = CameraCalibration.FindHomography(srcPoints, dstPoints, HOMOGRAPHY_METHOD.RANSAC, 2);

                    //_transformed = leftImageR.WarpPerspective(homographyMatrix, INTER.CV_INTER_CUBIC, WARP.CV_WARP_DEFAULT, new Gray(0));
                }
            }



            var oldLeft  = _cameras[0].Image;
            var oldRight = _cameras[1].Image;

            _cameras[0].Image = leftImageR;
            _cameras[1].Image = rightImageR;

            oldLeft.Dispose();
            oldRight.Dispose();
        }