public static double CalculateFaceYaw(Face3D face)
        {
            if (face.LeftEye == null || face.RightEye == null)
            {
                return 0;
            }

            return CalculateAngle3D(face.LeftEye.Value, face.RightEye.Value,
                new MCvPoint3D64f(face.RightEye.Value.x, face.RightEye.Value.y, face.LeftEye.Value.z));
        }
        public static double CalculateFacePitch(Face3D face)
        {
            if (face.Location == null || face.Mouth == null)
            {
                return 0;
            }

            return CalculateAngle3D(face.Location.Value, face.Mouth.Value,
                new MCvPoint3D64f(face.Location.Value.x, face.Mouth.Value.y, face.Location.Value.z));
        }
        public static double CalculateFaceRoll(Face3D face)
        {
            if (face.LeftEye == null || face.RightEye == null)
            {
                return 0;
            }

            return CalculateAngle3D(face.LeftEye.Value, face.RightEye.Value,
                new MCvPoint3D64f(face.RightEye.Value.x, face.LeftEye.Value.y, face.RightEye.Value.z) /*the point directly under the right eye*/);
        }
        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();
        }