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