Matrix <double> FindFundamentalMatrix(CameraModel cam1, CameraModel cam2) { IntPtr points1Ptr = CvHelper.CreatePointListPointer(CvHelper.NormalizePoints(cam1.Calibration.ObjectPointsProjected, cam1.Calibration.IntrinsicParameters)); IntPtr points2Ptr = CvHelper.CreatePointListPointer(CvHelper.NormalizePoints(cam2.Calibration.ObjectPointsProjected, cam1.Calibration.IntrinsicParameters)); Matrix <double> fundamentalMatrix = new Matrix <double>(3, 3); IntPtr fundamentalMatrixPtr = CvInvoke.cvCreateMat(3, 3, MAT_DEPTH.CV_32F); CvInvoke.cvFindFundamentalMat(points1Ptr, points2Ptr, fundamentalMatrix.Ptr, CV_FM.CV_FM_RANSAC, 3, 0.99, IntPtr.Zero); return(fundamentalMatrix); }
private void TrackerService_OnImageReady(object sender, EventArgs e) { OnImageReadyEventArgs args = (OnImageReadyEventArgs)e; IEnumerable <CameraModel> orderedCameras = _cameras.Cameras.OrderBy(camera => camera.Calibration.Index); foreach (CameraModel cameraModel1 in orderedCameras) { foreach (CameraModel cameraModel2 in orderedCameras) { if (cameraModel1.Calibration.ObjectPointsProjected == null || cameraModel2.Calibration.ObjectPointsProjected == null) { continue; } if (cameraModel1.Calibration.Index == cameraModel2.Calibration.Index) { continue; } if (cameraModel1.Calibration.Index > 1) { continue; } IntPtr points1Ptr = CvHelper.CreatePointListPointer(cameraModel1.Calibration.ObjectPointsProjected); IntPtr points2Ptr = CvHelper.CreatePointListPointer(cameraModel2.Calibration.ObjectPointsProjected); Matrix <double> fundamentalMatrix = new Matrix <double>(3, 3); IntPtr fundamentalMatrixPtr = CvInvoke.cvCreateMat(3, 3, MAT_DEPTH.CV_32F); CvInvoke.cvFindFundamentalMat(points1Ptr, points2Ptr, fundamentalMatrix, CV_FM.CV_FM_RANSAC, 3, 0.99, IntPtr.Zero); Matrix <double> lines1 = new Matrix <double>(8, 3); CvInvoke.cvComputeCorrespondEpilines(points2Ptr, 2, fundamentalMatrix, lines1); Matrix <double> lines2 = new Matrix <double>(8, 3); CvInvoke.cvComputeCorrespondEpilines(points1Ptr, 1, fundamentalMatrix, lines2); for (int i = 0; i < cameraModel1.Calibration.ObjectPointsProjected.Length; i++) { { Point[] points = new Point[2] { new Point(0, (int)-(lines2[i, 2] / lines2[i, 1])), new Point(args.Image.Cols, (int)(-(lines2[i, 2] + lines2[i, 0] * args.Image.Cols) / lines2[i, 1])) }; args.Image.DrawPolyline(points, false, new Bgr(255, 255, 0), 1); } //{ // System.Drawing.Point[] points = new System.Drawing.Point[2] // { // new System.Drawing.Point(0, (int) -(lines1[i, 2]/lines1[i, 1])), // new System.Drawing.Point(img.Cols, (int) (-(lines1[i, 2] + lines1[i, 0] * img.Cols) / lines1[i, 1])) // }; // img.DrawPolyline(points, false, new Bgr(255, 0, 255), 1); //} } } } }