// OutputArray _R, OutputArray _t, double distanceThresh, InputOutputArray _mask, OutputArray triangulatedPoints public static int RecoverPose(Mat E, PointF[] _points1, PointF[] _points2, Mat _cameraMatrix, double _distanceThresh, out Mat _R, out Mat _t, out Mat _PM) { Mat pointsMat1 = new Mat(); Mat pointsMat2 = new Mat(); Mat cameraMat = new Mat(); cameraMat = _cameraMatrix; _R = new Mat(); _t = new Mat(); _PM = new Mat(); double[,] p1; double[,] p2; if (_points1.Length <= _points2.Length) { p1 = new double[_points1.Length, 2]; p2 = new double[_points1.Length, 2]; } else { p1 = new double[_points2.Length, 2]; p2 = new double[_points2.Length, 2]; } for (int i = 0; i < _points1.Length && i < _points2.Length; i++) { p1[i, 0] = (double)_points1[i].X; p1[i, 1] = (double)_points1[i].Y; p2[i, 0] = (double)_points2[i].X; p2[i, 1] = (double)_points2[i].Y; } Matrix <double> points1 = new Matrix <double>(p1); Matrix <double> points2 = new Matrix <double>(p2); Matrix <double> cameraMatrix = new Matrix <double>(cameraMat.Rows, cameraMat.Cols, cameraMat.NumberOfChannels); cameraMat.CopyTo(cameraMatrix); //Need points1.checkVector Debug.Assert(cameraMatrix.Rows == 3 && cameraMatrix.Cols == 3 && cameraMatrix.NumberOfChannels == 1); double fx = cameraMatrix[0, 0]; double fy = cameraMatrix[1, 1]; double cx = cameraMatrix[0, 2]; double cy = cameraMatrix[1, 2]; for (int i = 0; i < points1.Rows; i++) { points1[i, 0] = (points1[i, 0] - cx) / fx; points2[i, 0] = (points2[i, 0] - cx) / fx; points1[i, 1] = (points1[i, 1] - cy) / fy; points2[i, 1] = (points2[i, 1] - cy) / fy; } Matrix <double> temp1 = new Matrix <double>(points1.Cols, points1.Rows); Matrix <double> temp2 = new Matrix <double>(points2.Cols, points2.Rows); CvInvoke.Transpose(points1, temp1); CvInvoke.Transpose(points2, temp2); points1 = temp1; points2 = temp2; Matrix <double> R1; Matrix <double> R2; Matrix <double> t; DecomposeEssentialMat(E, out R1, out R2, out t); // we are doing decomposition of E double[,] I = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 } }; Matrix <double> P0 = new Matrix <double>(I); // And we have 4 results Matrix <double> P1 = R1.ConcateHorizontal(t); Matrix <double> P2 = R2.ConcateHorizontal(t); Matrix <double> P3 = R1.ConcateHorizontal(t * (-1)); Matrix <double> P4 = R2.ConcateHorizontal(t * (-1)); // Do the cheirality check. // Notice here a threshold dist is used to filter // out far away points (i.e. infinite points) since // their depth may vary between positive and negtive. Mat[] allTriangulations = new Mat[4]; int good1 = Mask(P0, P1, points1, points2, _distanceThresh); int good2 = Mask(P0, P2, points1, points2, _distanceThresh); int good3 = Mask(P0, P3, points1, points2, _distanceThresh); int good4 = Mask(P0, P4, points1, points2, _distanceThresh); if (good1 >= good2 && good1 >= good3 && good1 >= good4) { _R = R1.Mat; _t = t.Mat; _PM = P1.Mat; return(good1); } else if (good2 >= good1 && good2 >= good3 && good2 >= good4) { _R = R2.Mat; _t = t.Mat; _PM = P2.Mat; return(good2); } else if (good3 >= good1 && good3 >= good2 && good3 >= good4) { t = t * (-1); _R = R1.Mat; _t = t.Mat; _PM = P3.Mat; return(good3); } else { t = t * (-1); _R = R2.Mat; _t = t.Mat; _PM = P4.Mat; return(good4); } }