[opencv] 22/251: 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.

[Nobuhiro Iwamatsu]

This is an automated email from the git hooks/post-receive script.

iwamatsu pushed a commit to annotated tag 3.3.0
in repository opencv.

commit 98c78e0acd8b6016affa36df02c3871b2dd9626f
Author: catree <catree.catreus at outlook.com>
Date:   Mon Jul 3 21:55:11 2017 +0200

    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<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())

-- 
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/opencv.git