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(); }