generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added solvePnPRansac method

Browse Source
This commit is contained in:
Alexander Shishkov 2011-03-05 00:18:49 +00:00
parent c3b05cf398
commit 04461a53f1
5 changed files with 508 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst
View File

@ -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

13
modules/calib3d/include/opencv2/calib3d/calib3d.hpp
View File

@ -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,

22
modules/calib3d/src/calibration.cpp
View File

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

324
modules/calib3d/src/solvepnp.cpp Normal file
View File

@ -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<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;
}

139
modules/calib3d/test/test_solvepnp_ransac.cpp Normal file
View File

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