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