Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #8585 from tonyke1993:ap3p

Browse Source 
Enable p3p and ap3p in solvePnPRansac (#8585)

* add paper info

* allow p3p and ap3p being RANSAC kernel

* keep previous code

* apply catrees comment

* fix getMat

* add comment

* add solvep3p test

* test return value

* fix warnings
pull/9005/head
Tong Ke 8 years ago committed by Alexander Alekhin
parent dcf3d988d5
commit 8088d6785a
7 changed files with 260 additions and 7 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 30
      modules/calib3d/include/opencv2/calib3d.hpp
  2. 34
      modules/calib3d/src/ap3p.cpp
  3. 6
      modules/calib3d/src/ap3p.h
  4. 35
      modules/calib3d/src/p3p.cpp
  5. 6
      modules/calib3d/src/p3p.h
  6. 60
      modules/calib3d/src/solvepnp.cpp
  7. 96
      modules/calib3d/test/test_solvepnp_ransac.cpp

30
modules/calib3d/include/opencv2/calib3d.hpp
Unescape View File

@ -561,6 +561,9 @@ F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Leng
Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$
assuming that both have the same value. Then the cameraMatrix is updated with the estimated
focal length.
- **SOLVEPNP_AP3P** Method is based on the paper of Tong Ke and Stergios I. Roumeliotis.
"An Efficient Algebraic Solution to the Perspective-Three-Point Problem". In this case the
function requires exactly four object and image points.
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.
@ -631,6 +634,33 @@ CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoint
bool useExtrinsicGuess = false, int iterationsCount = 100,
float reprojectionError = 8.0, double confidence = 0.99,
OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );
/** @brief Finds an object pose from 3 3D-2D point correspondences.
@param objectPoints Array of object points in the object coordinate space, 3x3 1-channel or
1x3/3x1 3-channel. vector\<Point3f\> can be also passed here.
@param imagePoints Array of corresponding image points, 3x2 1-channel or 1x3/3x1 2-channel.
vector\<Point2f\> can be also passed here.
@param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ .
@param distCoeffs Input vector of distortion coefficients
\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
assumed.
@param rvecs Output rotation vectors (see Rodrigues ) that, together with tvecs , brings points from
the model coordinate system to the camera coordinate system. A P3P problem has up to 4 solutions.
@param tvecs Output translation vectors.
@param flags Method for solving a P3P problem:
- **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang
"Complete Solution Classification for the Perspective-Three-Point Problem".
- **SOLVEPNP_AP3P** Method is based on the paper of Tong Ke and Stergios I. Roumeliotis.
"An Efficient Algebraic Solution to the Perspective-Three-Point Problem".
The function estimates the object pose given 3 object points, their corresponding image
projections, as well as the camera matrix and the distortion coefficients.
*/
CV_EXPORTS_W int solveP3P( InputArray objectPoints, InputArray imagePoints,
InputArray cameraMatrix, InputArray distCoeffs,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
int flags );
/** @brief Finds an initial camera matrix from 3D-2D point correspondences.

34
modules/calib3d/src/ap3p.cpp
Unescape View File

@ -313,6 +313,38 @@ bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Ma
return result;
}
int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints) {
CV_INSTRUMENT_REGION()
double rotation_matrix[4][3][3], translation[4][3];
std::vector<double> points;
if (opoints.depth() == ipoints.depth()) {
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
} else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14]);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
Rs.push_back(R);
tvecs.push_back(tvec);
}
return solutions;
}
bool
ap3p::solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1,
double mv1,
@ -383,4 +415,4 @@ int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, doubl
return computePoses(featureVectors, worldPoints, R, t);
}
}
}

6
modules/calib3d/src/ap3p.h
Unescape View File

@ -17,8 +17,9 @@ private:
template<typename OpointType, typename IpointType>
void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
points.clear();
points.resize(20);
for (int i = 0; i < 4; i++) {
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*npoints);
for (int i = 0; i < npoints; i++) {
points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
points[i * 5 + 2] = opoints.at<OpointType>(i).x;
@ -39,6 +40,7 @@ public:
ap3p(cv::Mat cameraMatrix);
bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints);
int solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints);
int solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,

35
modules/calib3d/src/p3p.cpp
Unescape View File

@ -57,6 +57,41 @@ bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat
return result;
}
int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints)
{
CV_INSTRUMENT_REGION()
double rotation_matrix[4][3][3], translation[4][3];
std::vector<double> points;
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
}
else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14]);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
Rs.push_back(R);
tvecs.push_back(tvec);
}
return solutions;
}
bool p3p::solve(double R[3][3], double t[3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,

6
modules/calib3d/src/p3p.h
Unescape View File

@ -11,6 +11,7 @@ class p3p
p3p(cv::Mat cameraMatrix);
bool solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints);
int solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints);
int solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
@ -34,8 +35,9 @@ class p3p
void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
{
points.clear();
points.resize(20);
for(int i = 0; i < 4; i++)
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*npoints);
for(int i = 0; i < npoints; i++)
{
points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;

60
modules/calib3d/src/solvepnp.cpp
Unescape View File

@ -268,7 +268,12 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
int model_points = 5;
int ransac_kernel_method = SOLVEPNP_EPNP;
if( npoints == 4 )
if( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
{
model_points = 4;
ransac_kernel_method = flags;
}
else if( npoints == 4 )
{
model_points = 4;
ransac_kernel_method = SOLVEPNP_P3P;
@ -337,4 +342,57 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
return true;
}
int solveP3P( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) {
CV_INSTRUMENT_REGION()
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints == 3 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P );
Mat cameraMatrix0 = _cameraMatrix.getMat();
Mat distCoeffs0 = _distCoeffs.getMat();
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
Mat distCoeffs = Mat_<double>(distCoeffs0);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
std::vector<Mat> Rs, ts;
int solutions = 0;
if (flags == SOLVEPNP_P3P)
{
p3p P3Psolver(cameraMatrix);
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
}
else if (flags == SOLVEPNP_AP3P)
{
ap3p P3Psolver(cameraMatrix);
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
}
if (solutions == 0) {
return 0;
}
if (_rvecs.needed()) {
_rvecs.create(solutions, 1, CV_64F);
}
if (_tvecs.needed()) {
_tvecs.create(solutions, 1, CV_64F);
}
for (int i = 0; i < solutions; i++) {
Mat rvec;
Rodrigues(Rs[i], rvec);
_tvecs.getMatRef(i) = ts[i];
_rvecs.getMatRef(i) = rvec;
}
return solutions;
}
}

96
modules/calib3d/test/test_solvepnp_ransac.cpp
Unescape View File

@ -153,7 +153,7 @@ protected:
return isTestSuccess;
}
void run(int)
virtual void run(int)
{
ts->set_failed_test_info(cvtest::TS::OK);
@ -253,6 +253,100 @@ protected:
}
};
class CV_solveP3P_Test : public CV_solvePnPRansac_Test
{
public:
CV_solveP3P_Test()
{
eps[SOLVEPNP_P3P] = 1.0e-4;
eps[SOLVEPNP_AP3P] = 1.0e-4;
totalTestsCount = 1000;
}
~CV_solveP3P_Test() {}
protected:
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
{
std::vector<Mat> rvecs, tvecs;
Mat trueRvec, trueTvec;
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics, rng);
if (mode == 0)
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
else
generateDistCoeffs(distCoeffs, rng);
generatePose(trueRvec, trueTvec, rng);
std::vector<Point3f> opoints;
opoints = std::vector<Point3f>(points.begin(), points.begin()+3);
vector<Point2f> projectedPoints;
projectedPoints.resize(opoints.size());
projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
int num_of_solutions = solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, method);
if (num_of_solutions != (int) rvecs.size() || num_of_solutions != (int) tvecs.size() || num_of_solutions == 0)
return false;
double min_rvecDiff = DBL_MAX, min_tvecDiff = DBL_MAX;
for (unsigned int i = 0; i < rvecs.size(); ++i) {
double rvecDiff = norm(rvecs[i]-trueRvec);
min_rvecDiff = std::min(rvecDiff, min_rvecDiff);
}
for (unsigned int i = 0; i < tvecs.size(); ++i) {
double tvecDiff = norm(tvecs[i]-trueTvec);
min_tvecDiff = std::min(tvecDiff, min_tvecDiff);
}
bool isTestSuccess = min_rvecDiff < epsilon[method] && min_tvecDiff < epsilon[method];
double error = std::max(min_rvecDiff, min_tvecDiff);
if (error > maxError)
maxError = error;
return isTestSuccess;
}
virtual void run(int)
{
ts->set_failed_test_info(cvtest::TS::OK);
vector<Point3f> points, points_dls;
const int pointsCount = 500;
points.resize(pointsCount);
generate3DPointCloud(points);
const int methodsCount = 2;
int methods[methodsCount] = {SOLVEPNP_P3P, SOLVEPNP_AP3P};
RNG rng = ts->get_rng();
for (int mode = 0; mode < 2; mode++)
{
for (int method = 0; method < methodsCount; method++)
{
double maxError = 0;
int successfulTestsCount = 0;
for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
{
if (runTest(rng, mode, methods[method], points, eps, maxError))
successfulTestsCount++;
}
//cout << maxError << " " << successfulTestsCount << endl;
if (successfulTestsCount < 0.7*totalTestsCount)
{
ts->printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n",
method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
cout << "mode: " << mode << ", method: " << method << " -> "
<< ((double)successfulTestsCount / totalTestsCount) * 100 << "%"
<< " (err < " << maxError << ")" << endl;
}
}
}
};
TEST(Calib3d_SolveP3P, accuracy) { CV_solveP3P_Test test; test.safe_run();}
TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }

Write Preview
Loading…
Cancel
Save