third_party/opencv
1
0
Fork 0
Code Issues Pull Requests Packages 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
This commit is contained in:
Tong Ke 2017-06-28 05:27:30 -07:00 committed by Alexander Alekhin
parent dcf3d988d5
commit 8088d6785a
7 changed files with 260 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
modules/calib3d/include/opencv2/calib3d.hpp
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
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
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
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
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
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
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(); }