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