From ca19ae8b5ae53afe3ca0084c55178b8d0796b28b Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Mon, 16 Mar 2015 16:56:26 +0300 Subject: [PATCH] in solvePnPRansac call the solvePnP in the end with all the inliers to get more precise estimate --- modules/calib3d/perf/perf_pnp.cpp | 8 ++- modules/calib3d/src/epnp.cpp | 21 ++++--- modules/calib3d/src/epnp.h | 5 ++ modules/calib3d/src/fundam.cpp | 18 +----- modules/calib3d/src/precomp.hpp | 13 +++++ modules/calib3d/src/solvepnp.cpp | 93 ++++++++++++++++++++----------- 6 files changed, 97 insertions(+), 61 deletions(-) diff --git a/modules/calib3d/perf/perf_pnp.cpp b/modules/calib3d/perf/perf_pnp.cpp index 9048f2be60..e5a92bf1c4 100644 --- a/modules/calib3d/perf/perf_pnp.cpp +++ b/modules/calib3d/perf/perf_pnp.cpp @@ -20,7 +20,7 @@ typedef perf::TestBaseWithParam PointsNum; PERF_TEST_P(PointsNum_Algo, solvePnP, testing::Combine( testing::Values(5, 3*9, 7*13), //TODO: find why results on 4 points are too unstable - testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP) + testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS) ) ) { @@ -64,13 +64,15 @@ PERF_TEST_P(PointsNum_Algo, solvePnP, PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, testing::Combine( - testing::Values(4), - testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP) + testing::Values(5), + testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP) ) ) { int pointsNum = get<0>(GetParam()); pnpAlgo algo = get<1>(GetParam()); + if( algo == SOLVEPNP_P3P ) + pointsNum = 4; vector points2d(pointsNum); vector points3d(pointsNum); diff --git a/modules/calib3d/src/epnp.cpp b/modules/calib3d/src/epnp.cpp index edbcaffd39..ec7dfe0ad7 100644 --- a/modules/calib3d/src/epnp.cpp +++ b/modules/calib3d/src/epnp.cpp @@ -2,7 +2,10 @@ #include "precomp.hpp" #include "epnp.h" -epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints) +namespace cv +{ + +epnp::epnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints) { if (cameraMatrix.depth() == CV_32F) init_camera_parameters(cameraMatrix); @@ -17,14 +20,14 @@ epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& i if (opoints.depth() == ipoints.depth()) { if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); + init_points(opoints, ipoints); else - init_points(opoints, ipoints); + init_points(opoints, ipoints); } else if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); + init_points(opoints, ipoints); else - init_points(opoints, ipoints); + init_points(opoints, ipoints); alphas.resize(4 * number_of_correspondences); pcs.resize(3 * number_of_correspondences); @@ -144,7 +147,7 @@ void epnp::compute_pcs(void) } } -void epnp::compute_pose(cv::Mat& R, cv::Mat& t) +void epnp::compute_pose(Mat& R, Mat& t) { choose_control_points(); compute_barycentric_coordinates(); @@ -189,8 +192,8 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t) if (rep_errors[2] < rep_errors[1]) N = 2; if (rep_errors[3] < rep_errors[N]) N = 3; - cv::Mat(3, 1, CV_64F, ts[N]).copyTo(t); - cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R); + Mat(3, 1, CV_64F, ts[N]).copyTo(t); + Mat(3, 3, CV_64F, Rs[N]).copyTo(R); } void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3], @@ -621,3 +624,5 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) pX[i] = (pb[i] - sum) / A2[i]; } } + +} diff --git a/modules/calib3d/src/epnp.h b/modules/calib3d/src/epnp.h index 2619f75957..350e9d4822 100644 --- a/modules/calib3d/src/epnp.h +++ b/modules/calib3d/src/epnp.h @@ -4,6 +4,9 @@ #include "precomp.hpp" #include "opencv2/core/core_c.h" +namespace cv +{ + class epnp { public: epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints); @@ -78,4 +81,6 @@ class epnp { double * A1, * A2; }; +} + #endif diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index c700ece70d..bc5c77b425 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -69,20 +69,6 @@ static bool haveCollinearPoints( const Mat& m, int count ) } -template int compressPoints( T* ptr, const uchar* mask, int mstep, int count ) -{ - int i, j; - for( i = j = 0; i < count; i++ ) - if( mask[i*mstep] ) - { - if( i > j ) - ptr[j] = ptr[i]; - j++; - } - return j; -} - - class HomographyEstimatorCallback : public PointSetRegistrator::Callback { public: @@ -322,8 +308,8 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, if( result && npoints > 4 ) { - compressPoints( src.ptr(), tempMask.ptr(), 1, npoints ); - npoints = compressPoints( dst.ptr(), tempMask.ptr(), 1, npoints ); + compressElems( src.ptr(), tempMask.ptr(), 1, npoints ); + npoints = compressElems( dst.ptr(), tempMask.ptr(), 1, npoints ); if( npoints > 0 ) { Mat src1 = src.rowRange(0, npoints); diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp index e8a81120f5..83a513dcaa 100644 --- a/modules/calib3d/src/precomp.hpp +++ b/modules/calib3d/src/precomp.hpp @@ -102,6 +102,19 @@ CV_EXPORTS Ptr createRANSACPointSetRegistrator(const Ptr createLMeDSPointSetRegistrator(const Ptr& cb, int modelPoints, double confidence=0.99, int maxIters=1000 ); +template inline int compressElems( T* ptr, const uchar* mask, int mstep, int count ) +{ + int i, j; + for( i = j = 0; i < count; i++ ) + if( mask[i*mstep] ) + { + if( i > j ) + ptr[j] = ptr[i]; + j++; + } + return j; +} + } #endif diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index eeec156dfa..dd5c5eb8cb 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -48,11 +48,13 @@ #include "opencv2/calib3d/calib3d_c.h" #include -using namespace cv; -bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, - InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) +namespace cv +{ + +bool solvePnP( InputArray _opoints, InputArray _ipoints, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) { Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); @@ -63,26 +65,26 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) { - cv::Mat undistortedPoints; - cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); epnp PnP(cameraMatrix, opoints, undistortedPoints); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); PnP.compute_pose(R, tvec); - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); return true; } else if (flags == SOLVEPNP_P3P) { CV_Assert( npoints == 4); - cv::Mat undistortedPoints; - cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); p3p P3Psolver(cameraMatrix); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); if (result) - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); return result; } else if (flags == SOLVEPNP_ITERATIVE) @@ -97,24 +99,24 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, } /*else if (flags == SOLVEPNP_DLS) { - cv::Mat undistortedPoints; - cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); dls PnP(opoints, undistortedPoints); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); bool result = PnP.compute_pose(R, tvec); if (result) - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); return result; } else if (flags == SOLVEPNP_UPNP) { upnp PnP(cameraMatrix, opoints, ipoints); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); double f = PnP.compute_pose(R, tvec); - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); if(cameraMatrix.type() == CV_32F) cameraMatrix.at(0,0) = cameraMatrix.at(1,1) = (float)f; else @@ -131,7 +133,7 @@ class PnPRansacCallback : public PointSetRegistrator::Callback public: - PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::SOLVEPNP_ITERATIVE, + PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=SOLVEPNP_ITERATIVE, bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() ) : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess), rvec(_rvec), tvec(_tvec) {} @@ -142,12 +144,11 @@ public: { Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); - - bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs, + bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags ); Mat _local_model; - cv::hconcat(rvec, tvec, _local_model); + hconcat(rvec, tvec, _local_model); _local_model.copyTo(_model); return correspondence; @@ -166,7 +167,7 @@ public: Mat projpoints(count, 2, CV_32FC1); - cv::projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); + projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); const Point2f* ipoints_ptr = ipoints.ptr(); const Point2f* projpoints_ptr = projpoints.ptr(); @@ -175,7 +176,7 @@ public: float* err = _err.getMat().ptr(); for ( i = 0; i < count; ++i) - err[i] = (float)cv::norm( ipoints_ptr[i] - projpoints_ptr[i] ); + err[i] = (float)norm( ipoints_ptr[i] - projpoints_ptr[i] ); } @@ -188,7 +189,7 @@ public: Mat tvec; }; -bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, +bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, @@ -214,23 +215,45 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); - Ptr cb; // pointer to callback - cb = makePtr( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec); + int model_points = 5; + int ransac_kernel_method = SOLVEPNP_EPNP; - int model_points = 4; // minimum of number of model points - if( flags == cv::SOLVEPNP_ITERATIVE ) model_points = 6; - else if( flags == cv::SOLVEPNP_UPNP ) model_points = 6; - else if( flags == cv::SOLVEPNP_EPNP ) model_points = 5; + if( npoints == 4 ) + { + model_points = 4; + ransac_kernel_method = SOLVEPNP_P3P; + } + + Ptr cb; // pointer to callback + cb = makePtr( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec); double param1 = reprojectionError; // reprojection error double param2 = confidence; // confidence int param3 = iterationsCount; // number maximum iterations - cv::Mat _local_model(3, 2, CV_64FC1); - cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); + Mat _local_model(3, 2, CV_64FC1); + Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); // call Ransac - int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); + int result = createRANSACPointSetRegistrator(cb, model_points, + param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); + + if( result > 0 ) + { + vector opoints_inliers; + vector ipoints_inliers; + opoints.convertTo(opoints_inliers, CV_64F); + ipoints.convertTo(ipoints_inliers, CV_64F); + + const uchar* mask = _mask_local_inliers.ptr(); + int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints); + compressElems(&ipoints_inliers[0], mask, 1, npoints); + + opoints_inliers.resize(npoints1); + ipoints_inliers.resize(npoints1); + result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, + distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1; + } if( result <= 0 || _local_model.rows <= 0) { @@ -260,3 +283,5 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, } return true; } + +}