i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing OpenCV<3 build

Browse Source
This commit is contained in:
matlabbe
2020-05-19 15:44:41 -04:00
parent 609d45ac9b
commit cb60443a33
4 changed files with 205 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

198
corelib/src/opencv/five-point.cpp
View File

@@ -36,10 +36,10 @@ using namespace cv;
namespace cv3
{
class EMEstimatorCallback CV_FINAL : public PointSetRegistrator::Callback
class EMEstimatorCallback : public PointSetRegistrator::Callback
{
public:
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
{
Mat q1 = _m1.getMat(), q2 = _m2.getMat();
Mat Q1 = q1.reshape(1, (int)q1.total());
@@ -372,7 +372,7 @@ protected:
}
void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
{
Mat X1 = _m1.getMat(), X2 = _m2.getMat(), model = _model.getMat();
const Point2d* x1ptr = X1.ptr<Point2d>();
@@ -447,4 +447,196 @@ cv::Mat findEssentialMat( InputArray _points1, InputArray _points2, InputArray _
return E;
}
void decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t )
{
Mat E = _E.getMat().reshape(1, 3);
CV_Assert(E.cols == 3 && E.rows == 3);
Mat D, U, Vt;
SVD::compute(E, D, U, Vt);
if (determinant(U) < 0) U *= -1.;
if (determinant(Vt) < 0) Vt *= -1.;
Mat W = (Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
W.convertTo(W, E.type());
Mat R1, R2, t;
R1 = U * W * Vt;
R2 = U * W.t() * Vt;
t = U.col(2) * 1.0;
R1.copyTo(_R1);
R2.copyTo(_R2);
t.copyTo(_t);
}
int recoverPose( InputArray E, InputArray _points1, InputArray _points2,
InputArray _cameraMatrix, OutputArray _R, OutputArray _t, double distanceThresh,
InputOutputArray _mask, OutputArray triangulatedPoints)
{
Mat points1, points2, cameraMatrix;
_points1.getMat().convertTo(points1, CV_64F);
_points2.getMat().convertTo(points2, CV_64F);
_cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);
int npoints = points1.checkVector(2);
CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
points1.type() == points2.type());
CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
if (points1.channels() > 1)
{
points1 = points1.reshape(1, npoints);
points2 = points2.reshape(1, npoints);
}
double fx = cameraMatrix.at<double>(0,0);
double fy = cameraMatrix.at<double>(1,1);
double cx = cameraMatrix.at<double>(0,2);
double cy = cameraMatrix.at<double>(1,2);
points1.col(0) = (points1.col(0) - cx) / fx;
points2.col(0) = (points2.col(0) - cx) / fx;
points1.col(1) = (points1.col(1) - cy) / fy;
points2.col(1) = (points2.col(1) - cy) / fy;
points1 = points1.t();
points2 = points2.t();
Mat R1, R2, t;
cv3::decomposeEssentialMat(E, R1, R2, t);
Mat P0 = Mat::eye(3, 4, R1.type());
Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type());
P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0;
P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0;
P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0;
P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0;
// 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 negative.
std::vector<Mat> allTriangulations(4);
Mat Q;
triangulatePoints(P0, P1, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[0]);
Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask1 = (Q.row(2) < distanceThresh) & mask1;
Q = P1 * Q;
mask1 = (Q.row(2) > 0) & mask1;
mask1 = (Q.row(2) < distanceThresh) & mask1;
triangulatePoints(P0, P2, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[1]);
Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask2 = (Q.row(2) < distanceThresh) & mask2;
Q = P2 * Q;
mask2 = (Q.row(2) > 0) & mask2;
mask2 = (Q.row(2) < distanceThresh) & mask2;
triangulatePoints(P0, P3, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[2]);
Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask3 = (Q.row(2) < distanceThresh) & mask3;
Q = P3 * Q;
mask3 = (Q.row(2) > 0) & mask3;
mask3 = (Q.row(2) < distanceThresh) & mask3;
triangulatePoints(P0, P4, points1, points2, Q);
if(triangulatedPoints.needed())
Q.copyTo(allTriangulations[3]);
Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask4 = (Q.row(2) < distanceThresh) & mask4;
Q = P4 * Q;
mask4 = (Q.row(2) > 0) & mask4;
mask4 = (Q.row(2) < distanceThresh) & mask4;
mask1 = mask1.t();
mask2 = mask2.t();
mask3 = mask3.t();
mask4 = mask4.t();
// If _mask is given, then use it to filter outliers.
if (!_mask.empty())
{
Mat mask = _mask.getMat();
CV_Assert(npoints == mask.checkVector(1));
mask = mask.reshape(1, npoints);
bitwise_and(mask, mask1, mask1);
bitwise_and(mask, mask2, mask2);
bitwise_and(mask, mask3, mask3);
bitwise_and(mask, mask4, mask4);
}
if (_mask.empty() && _mask.needed())
{
_mask.create(mask1.size(), CV_8U);
}
CV_Assert(_R.needed() && _t.needed());
_R.create(3, 3, R1.type());
_t.create(3, 1, t.type());
int good1 = countNonZero(mask1);
int good2 = countNonZero(mask2);
int good3 = countNonZero(mask3);
int good4 = countNonZero(mask4);
if (good1 >= good2 && good1 >= good3 && good1 >= good4)
{
if(triangulatedPoints.needed()) allTriangulations[0].copyTo(triangulatedPoints);
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask1.copyTo(_mask);
return good1;
}
else if (good2 >= good1 && good2 >= good3 && good2 >= good4)
{
if(triangulatedPoints.needed()) allTriangulations[1].copyTo(triangulatedPoints);
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask2.copyTo(_mask);
return good2;
}
else if (good3 >= good1 && good3 >= good2 && good3 >= good4)
{
if(triangulatedPoints.needed()) allTriangulations[2].copyTo(triangulatedPoints);
t = -t;
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask3.copyTo(_mask);
return good3;
}
else
{
if(triangulatedPoints.needed()) allTriangulations[3].copyTo(triangulatedPoints);
t = -t;
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask4.copyTo(_mask);
return good4;
}
}
}

6
corelib/src/opencv/five-point.h
View File

@@ -12,7 +12,11 @@ namespace cv3
{
cv::Mat findEssentialMat( cv::InputArray _points1, cv::InputArray _points2, cv::InputArray _cameraMatrix,
int method, double prob, double threshold, cv::OutputArray _mask = cv::noArray());
int method, double prob, double threshold, cv::OutputArray _mask = cv::noArray());
int recoverPose( cv::InputArray E, cv::InputArray _points1, cv::InputArray _points2,
cv::InputArray _cameraMatrix, cv::OutputArray _R, cv::OutputArray _t, double distanceThresh,
cv::InputOutputArray _mask, cv::OutputArray triangulatedPoints);
}

7
corelib/src/opencv/readme.txt
View File

@@ -1,6 +1,7 @@
These files are built only if RTAB-Map is built against OpenCV 2.
Used with OpenCV 2:
* Orb.cpp is a modified version of OpenCV2 Orb with FAST object from rtabmap (FAST with Grid adaptor).
Used for all OpenCV versions:
* solvepnp.cpp is a copy of the OpenCV3 version of solvePnPRansac.
* five-point.cpp is a copy of the same file in OpenCV (d2872afce0fcc84a52b5753960730595550e1b62) just for findEssentialMat() with RANSAC estimator using 6 points instead of 5 points to avoid "DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences. (expected: 'count >= 6')" error on recent OpenCV versions using DLT by default.
* five-point.cpp is a copy of the same file in OpenCV (d2872afce0fcc84a52b5753960730595550e1b62) but with RANSAC estimator using 6 points instead of 5 points to avoid "DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences. (expected: 'count >= 6')" error on recent OpenCV versions using DLT by default.