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