in solvePnPRansac call the solvePnP in the end with all the inliers to get more precise estimate

modules/calib3d/perf/perf_pnp.cpp

@@ -20,7 +20,7 @@ typedef perf::TestBaseWithParam<int> 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<Point2f> points2d(pointsNum);
     vector<Point3f> points3d(pointsNum);

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<float>(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<cv::Point3f,cv::Point2f>(opoints, ipoints);
+      init_points<Point3f,Point2f>(opoints, ipoints);
     else
-      init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
+      init_points<Point3d,Point2d>(opoints, ipoints);
   }
   else if (opoints.depth() == CV_32F)
-    init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
+    init_points<Point3f,Point2d>(opoints, ipoints);
   else
-    init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
+    init_points<Point3d,Point2f>(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];
   }
 }
+
+}

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

modules/calib3d/src/fundam.cpp

@@ -69,20 +69,6 @@ static bool haveCollinearPoints( const Mat& m, int count )
 }
 
 
-template<typename T> 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<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
-        npoints = compressPoints( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
+        compressElems( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
+        npoints = compressElems( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
         if( npoints > 0 )
         {
             Mat src1 = src.rowRange(0, npoints);

modules/calib3d/src/precomp.hpp

@@ -102,6 +102,19 @@ CV_EXPORTS Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<Po
 CV_EXPORTS Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& cb,
                                                                    int modelPoints, double confidence=0.99, int maxIters=1000 );
 
+template<typename T> 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

modules/calib3d/src/solvepnp.cpp

@@ -48,11 +48,13 @@
 #include "opencv2/calib3d/calib3d_c.h"
 
 #include <iostream>
-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<float>(0,0) = cameraMatrix.at<float>(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<Point2f>();
         const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
@@ -175,7 +176,7 @@ public:
         float* err = _err.getMat().ptr<float>();
 
         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<PointSetRegistrator::Callback> cb; // pointer to callback
-    cb = makePtr<PnPRansacCallback>( 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<PointSetRegistrator::Callback> cb; // pointer to callback
+    cb = makePtr<PnPRansacCallback>( 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<Point3d> opoints_inliers;
+        vector<Point2d> ipoints_inliers;
+        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 ? 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;
 }
+
+}