diff --git a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst index 8ce7d733bc..08a9b196f6 100644 --- a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst +++ b/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* 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`` 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`` 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 diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 60d63aaef3..3680ed08e3 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/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* inliers = NULL ); + //! initializes camera matrix from a few 3D points and the corresponding projections. CV_EXPORTS_W Mat initCameraMatrix2D( const vector >& objectPoints, const vector >& imagePoints, diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index aca6bc5506..695664e664 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/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 >& objectPoints, const vector >& imagePoints, Size imageSize, double aspectRatio ) diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp new file mode 100644 index 0000000000..ab0ed910d5 --- /dev/null +++ b/modules/calib3d/src/solvepnp.cpp @@ -0,0 +1,324 @@ +/*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& pointsMask, const Mat& objectPoints, const Mat& imagePoints, + const Parameters& params, vector& 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(0, i) - modelObjectPoints.at(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 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 localInliers; + for (size_t i = 0; i < objectPoints.cols; i++) + { + Point2f p(imagePoints.at(0, i)[0], imagePoints.at(0, i)[1]); + if ((norm(p - projected_points[i]) < params.reprojectionError) + && (rotatedPoints.at(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 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& 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& inliers; + Mat &rvec, &tvec; + Mat initRvec, initTvec; + + static RNG generator; + static Mutex syncMutex; + + void generateVar(vector& 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* 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 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; +} + diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp new file mode 100644 index 0000000000..ea306dd611 --- /dev/null +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -0,0 +1,139 @@ +/*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& 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 (0, 0) = 400.0; + intrinsics.at (1, 1) = 400.0; + intrinsics.at (0, 2) = 640 / 2; + intrinsics.at (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 (0, 0) = 1.0f; + tvec1.at (0, 0) = 1.0f; + tvec1.at (1, 0) = 1.0f; + vector points; + points.resize(500); + generate3DPointCloud(points); + + vector 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 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 (0, 0) - 1) < eps); + isTestSuccess = isTestSuccess && (abs(rvec.at (1, 0)) < eps); + isTestSuccess = isTestSuccess && (abs(rvec.at (2, 0)) < eps); + isTestSuccess = isTestSuccess + && (abs(tvec.at (0, 0) - 1) < eps); + isTestSuccess = isTestSuccess + && (abs(tvec.at (1, 0) - 1) < eps); + isTestSuccess = isTestSuccess && (abs(tvec.at (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(); } +