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/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(); }
+