diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 0cc265ff44..859e079a3c 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 b9bc4ae5b3..60ea577256 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())