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.

catree · catree · commit 98c78e0acd8b · 2017-07-05T13:17:38.000+02:00
diff --git 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
@@ -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<int>(i) = i;
+            }
+        }
+
+        return true;
+    }
+
     Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
     cb = makePtr<PnPRansacCallback>( 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<Point3d> opoints_inliers;
-        vector<Point2d> 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<uchar>();
-        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<Point3d> opoints_inliers;
+    vector<Point2d> 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<uchar>();
+    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())
