From 98c78e0acd8b6016affa36df02c3871b2dd9626f Mon Sep 17 00:00:00 2001 From: catree Date: Mon, 3 Jul 2017 21:55:11 +0200 Subject: [PATCH] Use directly solvePnP when the number of input points is equal to the number of model points. Enable useExtrinsicGuess parameter. Return rvec and tvec estimated using all the inliers instead of the best rvec and tvec estimated during the Minimal Sample Sets step. Document the behavior of solvePnPRansac. --- modules/calib3d/include/opencv2/calib3d.hpp | 7 +++ modules/calib3d/src/solvepnp.cpp | 77 ++++++++++++++++++++--------- 2 files changed, 62 insertions(+), 22 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 0cc265f..859e079 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -627,6 +627,13 @@ makes the function resistant to outliers. @note - An example of how to use solvePNPRansac for object detection can be found at opencv_source_code/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/ + - The default method used to estimate the camera pose for the Minimal Sample Sets step + is #SOLVEPNP_EPNP. Exceptions are: + - if you choose #SOLVEPNP_P3P or #SOLVEPNP_AP3P, these methods will be used. + - if the number of input points is equal to 4, #SOLVEPNP_P3P is used. + - The method used to estimate the camera pose using all the inliers is defined by the + flags parameters unless it is equal to #SOLVEPNP_P3P or #SOLVEPNP_AP3P. In this case, + the method #SOLVEPNP_EPNP will be used instead. */ CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index b9bc4ae..60ea577 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -249,7 +249,7 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, ipoints = ipoints0; int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); + CV_Assert( npoints >= 4 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); CV_Assert(opoints.isContinuous()); CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F); @@ -279,6 +279,31 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, ransac_kernel_method = SOLVEPNP_P3P; } + if( model_points == npoints ) + { + bool result = solvePnP(opoints, ipoints, cameraMatrix, distCoeffs, _rvec, _tvec, useExtrinsicGuess, ransac_kernel_method); + + if(!result) + { + if( _inliers.needed() ) + _inliers.release(); + + return false; + } + + if(_inliers.needed()) + { + _inliers.create(npoints, 1, CV_32S); + Mat _local_inliers = _inliers.getMat(); + for(int i = 0; i < npoints; i++) + { + _local_inliers.at(i) = i; + } + } + + return true; + } + Ptr cb; // pointer to callback cb = makePtr( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec); @@ -293,26 +318,6 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, 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 = opoints.reshape(3); - ipoints = ipoints.reshape(2); - 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 || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1; - } - if( result <= 0 || _local_model.rows <= 0) { _rvec.assign(rvec); // output rotation vector @@ -323,10 +328,38 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, return false; } - else + + vector opoints_inliers; + vector ipoints_inliers; + opoints = opoints.reshape(3); + ipoints = ipoints.reshape(2); + 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, useExtrinsicGuess, + (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1; + + if( result <= 0 ) { _rvec.assign(_local_model.col(0)); // output rotation vector _tvec.assign(_local_model.col(1)); // output translation vector + + if( _inliers.needed() ) + _inliers.release(); + + return false; + } + else + { + _rvec.assign(rvec); // output rotation vector + _tvec.assign(tvec); // output translation vector } if(_inliers.needed()) -- 2.7.4