added solvePnPRansac method

modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst

@@ -480,6 +480,38 @@ cv::solvePnP
 The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients. This function finds such a pose that minimizes reprojection error, i.e. the sum of squared distances between the observed projections ``imagePoints`` and the projected (using
 :ref:`ProjectPoints2` ) ``objectPoints`` .
 
+.. index:: solvePnPRansac
+
+cv::solvePnPRansac
+------------
+.. c:function:: void solvePnPRansac( const Mat& objectPoints,               const Mat& imagePoints,               const Mat& cameraMatrix,               const Mat& distCoeffs,               Mat& rvec,               Mat& tvec,               bool useExtrinsicGuess=false,               int iterationsCount = 100,               float reprojectionError = 8.0,               int minInliersCount = 100,               vector<int>* inliers = NULL  )
+
+    Finds the object pose from the 3D-2D point correspondences
+
+    :param objectPoints: The array of object points in the object coordinate space, 3xN or Nx3 1-channel, or 1xN or Nx1 3-channel, where N is the number of points.  Can also pass  ``vector<Point3f>``  here.
+
+    :param imagePoints: The array of corresponding image points, 2xN or Nx2 1-channel or 1xN or Nx1 2-channel, where N is the number of points.  Can also pass  ``vector<Point2f>``  here.
+
+    :param cameraMatrix: The input camera matrix  :math:`A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}`
+    :param distCoeffs: The input vector of distortion coefficients  :math:`(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]])`  of 4, 5 or 8 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
+
+    :param rvec: The output rotation vector (see  :ref:`Rodrigues2` ) that (together with  ``tvec`` ) brings points from the model coordinate system to the camera coordinate system
+
+    :param tvec: The output translation vector
+
+    :param useExtrinsicGuess: If true (1), the function will use the provided  ``rvec``  and  ``tvec``  as the initial approximations of the rotation and translation vectors, respectively, and will further optimize them.
+
+    :param iterationsCount: The number of iterations 
+    
+    :param reprojectionError: If distance between image point and object point projected with using found rvec and tvec less reprojectionError, it is inlier.
+   
+    :param minInliersCount: If the algorithm at some stage finds inliers more than minInliersCount it finishs.
+    
+    :param inliers: The output vector that contained indices of inliers in objectPoints and imagePoints
+
+The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients. This function finds such a pose that minimizes reprojection error, i.e. the sum of squared distances between the observed projections ``imagePoints`` and the projected (using
+:ref:`ProjectPoints2` ) ``objectPoints`` . Through the use of RANSAC function is resistant to outliers.
+
 .. index:: findFundamentalMat
 
 cv::findFundamentalMat

modules/calib3d/include/opencv2/calib3d/calib3d.hpp

@@ -519,6 +519,19 @@ CV_EXPORTS_W void solvePnP( const Mat& objectPoints,
                             CV_OUT Mat& rvec, CV_OUT Mat& tvec,
                             bool useExtrinsicGuess=false );
 
+//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible.
+CV_EXPORTS_W void solvePnPRansac( const Mat& objectPoints,
+                                  const Mat& imagePoints,
+                                  const Mat& cameraMatrix,
+                                  const Mat& distCoeffs,
+                                  CV_OUT Mat& rvec,
+                                  CV_OUT Mat& tvec,
+                                  bool useExtrinsicGuess = false,
+                                  int iterationsCount = 100,
+                                  float reprojectionError = 8.0,
+                                  int minInliersCount = 100,
+                                  CV_OUT vector<int>* inliers = NULL );
+
 //! initializes camera matrix from a few 3D points and the corresponding projections.
 CV_EXPORTS_W Mat initCameraMatrix2D( const vector<vector<Point3f> >& objectPoints,
                                    const vector<vector<Point2f> >& imagePoints,

modules/calib3d/src/calibration.cpp

@@ -3276,28 +3276,6 @@ void cv::projectPoints( const Mat& opoints,
                       &_imagePoints, &_dpdrot, &_dpdt, &_dpdf, &_dpdc, &_dpddist, aspectRatio );
 }
 
-void cv::solvePnP( const Mat& opoints, const Mat& ipoints,
-                   const Mat& cameraMatrix, const Mat& distCoeffs,
-                   Mat& rvec, Mat& tvec, bool useExtrinsicGuess )
-{
-    CV_Assert(opoints.isContinuous() && opoints.depth() == CV_32F &&
-              ((opoints.rows == 1 && opoints.channels() == 3) ||
-               opoints.cols*opoints.channels() == 3) &&
-              ipoints.isContinuous() && ipoints.depth() == CV_32F &&
-              ((ipoints.rows == 1 && ipoints.channels() == 2) ||
-              ipoints.cols*ipoints.channels() == 2));
-    
-    rvec.create(3, 1, CV_64F);
-    tvec.create(3, 1, CV_64F);
-    CvMat _objectPoints = opoints, _imagePoints = ipoints;
-    CvMat _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
-    CvMat _rvec = rvec, _tvec = tvec;
-    cvFindExtrinsicCameraParams2(&_objectPoints, &_imagePoints, &_cameraMatrix,
-                                 distCoeffs.data ? &_distCoeffs : 0,
-                                 &_rvec, &_tvec, useExtrinsicGuess );
-}
-
-
 cv::Mat cv::initCameraMatrix2D( const vector<vector<Point3f> >& objectPoints,
                                 const vector<vector<Point2f> >& imagePoints,
                                 Size imageSize, double aspectRatio )

modules/calib3d/src/solvepnp.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "precomp.hpp"
+using namespace cv;
+
+void cv::solvePnP( const Mat& opoints, const Mat& ipoints,
+                   const Mat& cameraMatrix, const Mat& distCoeffs,
+                   Mat& rvec, Mat& tvec, bool useExtrinsicGuess )
+{
+    CV_Assert(opoints.isContinuous() && opoints.depth() == CV_32F &&
+              ((opoints.rows == 1 && opoints.channels() == 3) ||
+               opoints.cols*opoints.channels() == 3) &&
+              ipoints.isContinuous() && ipoints.depth() == CV_32F &&
+              ((ipoints.rows == 1 && ipoints.channels() == 2) ||
+              ipoints.cols*ipoints.channels() == 2));
+
+    rvec.create(3, 1, CV_64F);
+    tvec.create(3, 1, CV_64F);
+    CvMat _objectPoints = opoints, _imagePoints = ipoints;
+    CvMat _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
+    CvMat _rvec = rvec, _tvec = tvec;
+    cvFindExtrinsicCameraParams2(&_objectPoints, &_imagePoints, &_cameraMatrix,
+                                 distCoeffs.data ? &_distCoeffs : 0,
+                                 &_rvec, &_tvec, useExtrinsicGuess );
+}
+
+namespace cv
+{
+  namespace pnpransac
+  {
+    const int MIN_POINTS_COUNT = 4;
+
+    void project3dPoints(const Mat& points, const Mat& rvec, const Mat& tvec, Mat& modif_points)
+    {
+      modif_points.create(1, points.cols, CV_32FC3);
+      Mat R(3, 3, CV_64FC1);
+      Rodrigues(rvec, R);
+      Mat transformation(3, 4, CV_64F);
+      Mat r = transformation.colRange(0, 2);
+      R.copyTo(r);
+      Mat t = transformation.colRange(3, 4);
+      tvec.copyTo(t);
+      transform(points, modif_points, transformation);
+    }
+
+    class Mutex
+    {
+    public:
+        Mutex() {}
+        void lock()
+        {
+        #ifdef HAVE_TBB
+          slock.acquire(resultsMutex);
+        #endif
+        }
+
+        void unlock()
+        {
+        #ifdef HAVE_TBB
+          slock.release();
+        #endif
+        }
+
+    private:
+    #ifdef HAVE_TBB
+      tbb::mutex resultsMutex;
+      tbb::mutex::scoped_lock slock;
+    #endif
+    };
+
+    struct CameraParameters
+    {
+        void init(Mat _intrinsics, Mat _distCoeffs)
+        {
+            _intrinsics.copyTo(intrinsics);
+            _distCoeffs.copyTo(distortion);
+        }
+
+        Mat intrinsics;
+        Mat distortion;
+    };
+
+    struct Parameters
+    {
+        int iterationsCount;
+        float reprojectionError;
+        int minInliersCount;
+        bool useExtrinsicGuess;
+        CameraParameters camera;
+    };
+
+    void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
+                 const Parameters& params, vector<int>& inliers, Mat& rvec, Mat& tvec,
+                 const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex)
+    {
+      Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_32FC3), modelImagePoints(1, MIN_POINTS_COUNT, CV_32FC2);
+      for (size_t i = 0, colIndex = 0; i < pointsMask.size(); i++)
+      {
+        if (pointsMask[i])
+        {
+          Mat colModelImagePoints = modelImagePoints(Rect(colIndex, 0, 1, 1));
+          imagePoints.col(i).copyTo(colModelImagePoints);
+          Mat colModelObjectPoints = modelObjectPoints(Rect(colIndex, 0, 1, 1));
+          objectPoints.col(i).copyTo(colModelObjectPoints);
+          colIndex = colIndex+1;
+        }
+      }
+
+      //filter same 3d points, hang in solvePnP
+      double eps = 1e-10;
+      int num_same_points = 0;
+      for (int i = 0; i < MIN_POINTS_COUNT; i++)
+        for (int j = i + 1; j < MIN_POINTS_COUNT; j++)
+        {
+          if (norm(modelObjectPoints.at<Vec3f>(0, i) - modelObjectPoints.at<Vec3f>(0, j)) < eps)
+            num_same_points++;
+        }
+      if (num_same_points > 0)
+        return;
+
+      Mat localRvec, localTvec;
+      rvecInit.copyTo(localRvec);
+      tvecInit.copyTo(localTvec);
+
+      solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, params.useExtrinsicGuess);
+
+      vector<Point2f> projected_points;
+      projected_points.resize(objectPoints.cols);
+      projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points);
+
+      Mat rotatedPoints;
+      project3dPoints(objectPoints, localRvec, localTvec, rotatedPoints);
+
+      vector<int> localInliers;
+      for (size_t i = 0; i < objectPoints.cols; i++)
+      {
+        Point2f p(imagePoints.at<Vec2f>(0, i)[0], imagePoints.at<Vec2f>(0, i)[1]);
+        if ((norm(p - projected_points[i]) < params.reprojectionError)
+            && (rotatedPoints.at<Vec3f>(0, i)[2] > 0)) //hack
+        {
+          localInliers.push_back(i);
+        }
+      }
+
+      if (localInliers.size() > inliers.size())
+      {
+        resultsMutex.lock();
+
+        inliers.clear();
+        inliers.resize(localInliers.size());
+        memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
+        localRvec.copyTo(rvec);
+        localTvec.copyTo(tvec);
+
+        resultsMutex.unlock();
+      }
+    }
+
+    class PnPSolver
+    {
+    public:
+        void operator()( const BlockedRange& r ) const
+        {
+            vector<char> pointsMask(objectPoints.cols, 0);
+            memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
+            for( size_t i=r.begin(); i!=r.end(); ++i )
+            {
+               generateVar(pointsMask);
+               pnpTask(pointsMask, objectPoints, imagePoints, parameters,
+                       inliers, rvec, tvec, initRvec, initTvec, syncMutex);
+               if ((int)inliers.size() > parameters.minInliersCount)
+               {
+                    #ifdef HAVE_TBB
+                      tbb::task::self().cancel_group_execution();
+                    #else
+                      break;
+                     #endif
+               }
+            }
+        }
+        PnPSolver(const Mat& objectPoints, const Mat& imagePoints, const Parameters& parameters,
+                  Mat& rvec, Mat& tvec, vector<int>& inliers):
+                    objectPoints(objectPoints), imagePoints(imagePoints), parameters(parameters),
+                    rvec(rvec), tvec(tvec), inliers(inliers)
+        {
+          rvec.copyTo(initRvec);
+          tvec.copyTo(initTvec);
+        }
+    private:
+        const Mat& objectPoints;
+        const Mat& imagePoints;
+        const Parameters& parameters;
+        vector<int>& inliers;
+        Mat &rvec, &tvec;
+        Mat initRvec, initTvec;
+
+        static RNG generator;
+        static Mutex syncMutex;
+
+        void generateVar(vector<char>& mask) const
+        {
+          size_t size = mask.size();
+          for (size_t i = 0; i < size; i++)
+          {
+                int i1 = generator.uniform(0, size);
+                int i2 = generator.uniform(0, size);
+                char curr = mask[i1];
+                mask[i1] = mask[i2];
+                mask[i2] = curr;
+          }
+        }
+    };
+
+    Mutex PnPSolver::syncMutex;
+    RNG PnPSolver::generator;
+
+  }
+}
+
+void cv::solvePnPRansac(const Mat& opoints, const Mat& ipoints,
+                    const Mat& cameraMatrix, const Mat& distCoeffs, Mat& rvec, Mat& tvec, bool useExtrinsicGuess,
+                    int iterationsCount, float reprojectionError, int minInliersCount, vector<int>* inliers)
+{
+
+  CV_Assert(opoints.isContinuous());
+  CV_Assert(opoints.depth() == CV_32F);
+  CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
+  CV_Assert(ipoints.isContinuous());
+  CV_Assert(ipoints.depth() == CV_32F);
+  CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
+
+  rvec.create(3, 1, CV_64FC1);
+  tvec.create(3, 1, CV_64FC1);
+
+  Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
+
+  if (minInliersCount <= 0)
+    minInliersCount = objectPoints.cols;
+  cv::pnpransac::Parameters params;
+  params.iterationsCount = iterationsCount;
+  params.minInliersCount = minInliersCount;
+  params.reprojectionError = reprojectionError;
+  params.useExtrinsicGuess = useExtrinsicGuess;
+  params.camera.init(cameraMatrix, distCoeffs);
+
+  vector<int> localInliers;
+  Mat localRvec, localTvec;
+  rvec.copyTo(localRvec);
+  tvec.copyTo(localTvec);
+
+  if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
+  {
+     parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params,
+        localRvec, localTvec, localInliers));
+  }
+
+  if (localInliers.size() >= pnpransac::MIN_POINTS_COUNT)
+  {
+    size_t pointsCount = localInliers.size();
+    Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
+    int index;
+    for (size_t i = 0; i < localInliers.size(); i++)
+    {
+      index = localInliers[i];
+      Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1));
+      imagePoints.col(index).copyTo(colInlierImagePoints);
+      Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
+      objectPoints.col(index).copyTo(colInlierObjectPoints);
+    }
+    solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true);
+    localRvec.copyTo(rvec);
+    localTvec.copyTo(tvec);
+    if (inliers)
+       *inliers = localInliers;
+  }
+  else
+  {
+    tvec.setTo(Scalar(0));
+    Mat R = Mat::ones(3, 3, CV_64F);
+    Rodrigues(R, rvec);
+  }
+  return;
+}
+

modules/calib3d/test/test_solvepnp_ransac.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+
+using namespace cv;
+using namespace std;
+
+class CV_solvePnPRansac_Test : public cvtest::BaseTest
+{
+public:
+    CV_solvePnPRansac_Test() {}
+    ~CV_solvePnPRansac_Test() {}
+protected:
+    void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1,
+                    -1, 5), Point3f pmax = Point3f(1, 1, 10))
+    {
+            const Point3f delta = pmax - pmin;
+            for (size_t i = 0; i < points.size(); i++)
+            {
+                    Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX,
+                                    float(rand()) / RAND_MAX);
+                    p.x *= delta.x;
+                    p.y *= delta.y;
+                    p.z *= delta.z;
+                    p = p + pmin;
+                    points[i] = p;
+            }
+    }
+
+    void run(int)
+    {
+        cvtest::TS& ts = *this->ts;
+        ts.set_failed_test_info(cvtest::TS::OK);
+        Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
+        intrinsics.at<float> (0, 0) = 400.0;
+        intrinsics.at<float> (1, 1) = 400.0;
+        intrinsics.at<float> (0, 2) = 640 / 2;
+        intrinsics.at<float> (1, 2) = 480 / 2;
+        Mat dist_coeffs = Mat::zeros(5, 1, CV_32FC1);
+        Mat rvec1 = Mat::zeros(3, 1, CV_64FC1);
+        Mat tvec1 = Mat::zeros(3, 1, CV_64FC1);
+        rvec1.at<double> (0, 0) = 1.0f;
+        tvec1.at<double> (0, 0) = 1.0f;
+        tvec1.at<double> (1, 0) = 1.0f;
+        vector<Point3f> points;
+        points.resize(500);
+        generate3DPointCloud(points);
+
+        vector<Point2f> points1;
+        points1.resize(points.size());
+        projectPoints(Mat(points), rvec1, tvec1, intrinsics, dist_coeffs, points1);
+        for (size_t i = 0; i < points1.size(); i++)
+        {
+            if (i % 20 == 0)
+            {
+                    points1[i] = points1[rand() % points.size()];
+            }
+        }
+        double eps = 1.0e-7;
+        for (int testIndex = 0; testIndex<  10; testIndex++)
+        {
+            try
+            {
+                Mat rvec, tvec;
+                vector<int> inliers;
+
+                solvePnPRansac(Mat(points), Mat(points1), intrinsics, dist_coeffs, rvec, tvec,
+                                false, 1000, 2.0, -1, &inliers);
+
+                bool isTestSuccess = inliers.size() == 475;
+
+                isTestSuccess = isTestSuccess
+                                && (abs(rvec.at<double> (0, 0) - 1) < eps);
+                isTestSuccess = isTestSuccess && (abs(rvec.at<double> (1, 0)) < eps);
+                isTestSuccess = isTestSuccess && (abs(rvec.at<double> (2, 0)) < eps);
+                isTestSuccess = isTestSuccess
+                                && (abs(tvec.at<double> (0, 0) - 1) < eps);
+                isTestSuccess = isTestSuccess
+                                && (abs(tvec.at<double> (1, 0) - 1) < eps);
+                isTestSuccess = isTestSuccess && (abs(tvec.at<double> (2, 0)) < eps);
+                if (!isTestSuccess)
+                {
+                    ts.printf( cvtest::TS::LOG, "Invalid accuracy, inliers.size = %d\n", inliers.size());
+                    ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+                    break;
+                }
+
+            }
+            catch(...)
+            {
+                    ts.printf(cvtest::TS::LOG, "Exception in solvePnPRansac\n");
+                    ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+            }
+        }
+    }
+};
+
+TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
+