From 0a63ab36bb10b133ded9496aca0058ff630fa2a8 Mon Sep 17 00:00:00 2001
From: Tong Ke <tonyke1993@gmail.com>
Date: Fri, 7 Apr 2017 01:48:34 -0500
Subject: [PATCH] Merge pull request #8301 from tonyke1993:p3p_alg
New p3p algorithm (accepted by CVPR 2017) (#8301)
* add p3p source code
* indent 4
* update publication info
* fix filename
* interface done
* plug in done, test needed
* debugging
* for test
* a working version
* clean p3p code
* test
* test
* fix warning, blank line
* apply patch from @catree
* add reference info
* namespace, indent 4
* static solveQuartic
* put small functions to anonymous namespace
---
doc/opencv.bib | 7 +
modules/calib3d/include/opencv2/calib3d.hpp | 4 +-
modules/calib3d/src/ap3p.cpp | 383 ++++++++++++++++++
modules/calib3d/src/ap3p.h | 65 +++
modules/calib3d/src/solvepnp.cpp | 16 +-
modules/calib3d/test/test_solvepnp_ransac.cpp | 8 +-
6 files changed, 477 insertions(+), 6 deletions(-)
create mode 100644 modules/calib3d/src/ap3p.cpp
create mode 100644 modules/calib3d/src/ap3p.h
diff --git a/doc/opencv.bib b/doc/opencv.bib
index 29a2ae4512..f4bb2512d1 100644
--- a/doc/opencv.bib
+++ b/doc/opencv.bib
@@ -897,3 +897,10 @@
year={2010},
publisher={Springer}
}
+@INPROCEEDINGS{Ke17,
+ author = {Ke, Tong and Roumeliotis, Stergios},
+ title = {An Efficient Algebraic Solution to the Perspective-Three-Point Problem},
+ booktitle = {Computer Vision and Pattern Recognition (CVPR), 2017 IEEE Conference on},
+ year = {2017},
+ organization = {IEEE}
+}
diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp
index b5a56ec1b6..c58daab027 100644
--- a/modules/calib3d/include/opencv2/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d.hpp
@@ -236,8 +236,8 @@ enum { SOLVEPNP_ITERATIVE = 0,
SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp
SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete
SOLVEPNP_DLS = 3, //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct
- SOLVEPNP_UPNP = 4 //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive
-
+ SOLVEPNP_UPNP = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive
+ SOLVEPNP_AP3P = 5 //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17
};
enum { CALIB_CB_ADAPTIVE_THRESH = 1,
diff --git a/modules/calib3d/src/ap3p.cpp b/modules/calib3d/src/ap3p.cpp
new file mode 100644
index 0000000000..ad2841fe98
--- /dev/null
+++ b/modules/calib3d/src/ap3p.cpp
@@ -0,0 +1,383 @@
+#include "ap3p.h"
+
+#include <cmath>
+#include <complex>
+
+using namespace std;
+
+namespace {
+void solveQuartic(const double *factors, double *realRoots) {
+ const double &a4 = factors[0];
+ const double &a3 = factors[1];
+ const double &a2 = factors[2];
+ const double &a1 = factors[3];
+ const double &a0 = factors[4];
+
+ double a4_2 = a4 * a4;
+ double a3_2 = a3 * a3;
+ double a4_3 = a4_2 * a4;
+ double a2a4 = a2 * a4;
+
+ double p4 = (8 * a2a4 - 3 * a3_2) / (8 * a4_2);
+ double q4 = (a3_2 * a3 - 4 * a2a4 * a3 + 8 * a1 * a4_2) / (8 * a4_3);
+ double r4 = (256 * a0 * a4_3 - 3 * (a3_2 * a3_2) - 64 * a1 * a3 * a4_2 + 16 * a2a4 * a3_2) / (256 * (a4_3 * a4));
+
+ double p3 = ((p4 * p4) / 12 + r4) / 3; // /=-3
+ double q3 = (72 * r4 * p4 - 2 * p4 * p4 * p4 - 27 * q4 * q4) / 432; // /=2
+
+ double t; // *=2
+ complex<double> w;
+ if (q3 >= 0)
+ w = -sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
+ else
+ w = sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
+ if (w.imag() == 0.0) {
+ w.real(cbrt(w.real()));
+ t = 2.0 * (w.real() + p3 / w.real());
+ } else {
+ w = pow(w, 1.0 / 3);
+ t = 4.0 * w.real();
+ }
+
+ complex<double> sqrt_2m = sqrt(static_cast<complex<double> >(-2 * p4 / 3 + t));
+ double B_4A = -a3 / (4 * a4);
+ double complex1 = 4 * p4 / 3 + t;
+ complex<double> complex2 = 2 * q4 / sqrt_2m;
+ double sqrt_2m_rh = sqrt_2m.real() / 2;
+ double sqrt1 = sqrt(-(complex1 + complex2)).real() / 2;
+ realRoots[0] = B_4A + sqrt_2m_rh + sqrt1;
+ realRoots[1] = B_4A + sqrt_2m_rh - sqrt1;
+ double sqrt2 = sqrt(-(complex1 - complex2)).real() / 2;
+ realRoots[2] = B_4A - sqrt_2m_rh + sqrt2;
+ realRoots[3] = B_4A - sqrt_2m_rh - sqrt2;
+}
+
+void polishQuarticRoots(const double *coeffs, double *roots) {
+ const int iterations = 2;
+ for (int i = 0; i < iterations; ++i) {
+ for (int j = 0; j < 4; ++j) {
+ double error =
+ (((coeffs[0] * roots[j] + coeffs[1]) * roots[j] + coeffs[2]) * roots[j] + coeffs[3]) * roots[j] +
+ coeffs[4];
+ double
+ derivative =
+ ((4 * coeffs[0] * roots[j] + 3 * coeffs[1]) * roots[j] + 2 * coeffs[2]) * roots[j] + coeffs[3];
+ roots[j] -= error / derivative;
+ }
+ }
+}
+
+inline void vect_cross(const double *a, const double *b, double *result) {
+ result[0] = a[1] * b[2] - a[2] * b[1];
+ result[1] = -(a[0] * b[2] - a[2] * b[0]);
+ result[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+inline double vect_dot(const double *a, const double *b) {
+ return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+}
+
+inline double vect_norm(const double *a) {
+ return sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
+}
+
+inline void vect_scale(const double s, const double *a, double *result) {
+ result[0] = a[0] * s;
+ result[1] = a[1] * s;
+ result[2] = a[2] * s;
+}
+
+inline void vect_sub(const double *a, const double *b, double *result) {
+ result[0] = a[0] - b[0];
+ result[1] = a[1] - b[1];
+ result[2] = a[2] - b[2];
+}
+
+inline void vect_divide(const double *a, const double d, double *result) {
+ result[0] = a[0] / d;
+ result[1] = a[1] / d;
+ result[2] = a[2] / d;
+}
+
+inline void mat_mult(const double a[3][3], const double b[3][3], double result[3][3]) {
+ result[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0];
+ result[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1];
+ result[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2];
+
+ result[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0];
+ result[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1];
+ result[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2];
+
+ result[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0];
+ result[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1];
+ result[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2];
+}
+}
+
+namespace cv {
+void ap3p::init_inverse_parameters() {
+ inv_fx = 1. / fx;
+ inv_fy = 1. / fy;
+ cx_fx = cx / fx;
+ cy_fy = cy / fy;
+}
+
+ap3p::ap3p(cv::Mat cameraMatrix) {
+ if (cameraMatrix.depth() == CV_32F)
+ init_camera_parameters<float>(cameraMatrix);
+ else
+ init_camera_parameters<double>(cameraMatrix);
+ init_inverse_parameters();
+}
+
+ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) {
+ fx = _fx;
+ fy = _fy;
+ cx = _cx;
+ cy = _cy;
+ init_inverse_parameters();
+}
+
+// This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
+// See https://arxiv.org/pdf/1701.08237.pdf
+// featureVectors: The 3 bearing measurements (normalized) stored as column vectors
+// worldPoints: The positions of the 3 feature points stored as column vectors
+// solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
+// solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
+int ap3p::computePoses(const double featureVectors[3][3],
+ const double worldPoints[3][3],
+ double solutionsR[4][3][3],
+ double solutionsT[4][3]) {
+
+ //world point vectors
+ double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]};
+ double w2[3] = {worldPoints[0][1], worldPoints[1][1], worldPoints[2][1]};
+ double w3[3] = {worldPoints[0][2], worldPoints[1][2], worldPoints[2][2]};
+ // k1
+ double u0[3];
+ vect_sub(w1, w2, u0);
+
+ double nu0 = vect_norm(u0);
+ double k1[3];
+ vect_divide(u0, nu0, k1);
+ // bi
+ double b1[3] = {featureVectors[0][0], featureVectors[1][0], featureVectors[2][0]};
+ double b2[3] = {featureVectors[0][1], featureVectors[1][1], featureVectors[2][1]};
+ double b3[3] = {featureVectors[0][2], featureVectors[1][2], featureVectors[2][2]};
+ // k3,tz
+ double k3[3];
+ vect_cross(b1, b2, k3);
+ double nk3 = vect_norm(k3);
+ vect_divide(k3, nk3, k3);
+
+ double tz[3];
+ vect_cross(b1, k3, tz);
+ // ui,vi
+ double v1[3];
+ vect_cross(b1, b3, v1);
+ double v2[3];
+ vect_cross(b2, b3, v2);
+
+ double u1[3];
+ vect_sub(w1, w3, u1);
+ // coefficients related terms
+ double u1k1 = vect_dot(u1, k1);
+ double k3b3 = vect_dot(k3, b3);
+ // f1i
+ double f11 = k3b3;
+ double f13 = vect_dot(k3, v1);
+ double f15 = -u1k1 * f11;
+ //delta
+ double nl[3];
+ vect_cross(u1, k1, nl);
+ double delta = vect_norm(nl);
+ vect_divide(nl, delta, nl);
+ f11 *= delta;
+ f13 *= delta;
+ // f2i
+ double u2k1 = u1k1 - nu0;
+ double f21 = vect_dot(tz, v2);
+ double f22 = nk3 * k3b3;
+ double f23 = vect_dot(k3, v2);
+ double f24 = u2k1 * f22;
+ double f25 = -u2k1 * f21;
+ f21 *= delta;
+ f22 *= delta;
+ f23 *= delta;
+ double g1 = f13 * f22;
+ double g2 = f13 * f25 - f15 * f23;
+ double g3 = f11 * f23 - f13 * f21;
+ double g4 = -f13 * f24;
+ double g5 = f11 * f22;
+ double g6 = f11 * f25 - f15 * f21;
+ double g7 = -f15 * f24;
+ double coeffs[5] = {g5 * g5 + g1 * g1 + g3 * g3,
+ 2 * (g5 * g6 + g1 * g2 + g3 * g4),
+ g6 * g6 + 2 * g5 * g7 + g2 * g2 + g4 * g4 - g1 * g1 - g3 * g3,
+ 2 * (g6 * g7 - g1 * g2 - g3 * g4),
+ g7 * g7 - g2 * g2 - g4 * g4};
+ double s[4];
+ solveQuartic(coeffs, s);
+ polishQuarticRoots(coeffs, s);
+
+ double temp[3];
+ vect_cross(k1, nl, temp);
+
+ double Ck1nl[3][3] =
+ {{k1[0], nl[0], temp[0]},
+ {k1[1], nl[1], temp[1]},
+ {k1[2], nl[2], temp[2]}};
+
+ double Cb1k3tzT[3][3] =
+ {{b1[0], b1[1], b1[2]},
+ {k3[0], k3[1], k3[2]},
+ {tz[0], tz[1], tz[2]}};
+
+ double b3p[3];
+ vect_scale((delta / k3b3), b3, b3p);
+
+ int nb_solutions = 0;
+ for (int i = 0; i < 4; ++i) {
+ double ctheta1p = s[i];
+ if (abs(ctheta1p) > 1)
+ continue;
+ double stheta1p = sqrt(1 - ctheta1p * ctheta1p);
+ stheta1p = (k3b3 > 0) ? stheta1p : -stheta1p;
+ double ctheta3 = g1 * ctheta1p + g2;
+ double stheta3 = g3 * ctheta1p + g4;
+ double ntheta3 = stheta1p / ((g5 * ctheta1p + g6) * ctheta1p + g7);
+ ctheta3 *= ntheta3;
+ stheta3 *= ntheta3;
+
+ double C13[3][3] =
+ {{ctheta3, 0, -stheta3},
+ {stheta1p * stheta3, ctheta1p, stheta1p * ctheta3},
+ {ctheta1p * stheta3, -stheta1p, ctheta1p * ctheta3}};
+
+ double temp_matrix[3][3];
+ double R[3][3];
+ mat_mult(Ck1nl, C13, temp_matrix);
+ mat_mult(temp_matrix, Cb1k3tzT, R);
+
+ // R' * p3
+ double rp3[3] =
+ {w3[0] * R[0][0] + w3[1] * R[1][0] + w3[2] * R[2][0],
+ w3[0] * R[0][1] + w3[1] * R[1][1] + w3[2] * R[2][1],
+ w3[0] * R[0][2] + w3[1] * R[1][2] + w3[2] * R[2][2]};
+
+ double pxstheta1p[3];
+ vect_scale(stheta1p, b3p, pxstheta1p);
+
+ vect_sub(pxstheta1p, rp3, solutionsT[nb_solutions]);
+
+ solutionsR[nb_solutions][0][0] = R[0][0];
+ solutionsR[nb_solutions][1][0] = R[0][1];
+ solutionsR[nb_solutions][2][0] = R[0][2];
+ solutionsR[nb_solutions][0][1] = R[1][0];
+ solutionsR[nb_solutions][1][1] = R[1][1];
+ solutionsR[nb_solutions][2][1] = R[1][2];
+ solutionsR[nb_solutions][0][2] = R[2][0];
+ solutionsR[nb_solutions][1][2] = R[2][1];
+ solutionsR[nb_solutions][2][2] = R[2][2];
+
+ nb_solutions++;
+ }
+
+ return nb_solutions;
+}
+
+bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints) {
+ CV_INSTRUMENT_REGION()
+
+ double rotation_matrix[3][3], translation[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);
+
+ bool result = 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],
+ points[15], points[16], points[17], points[18], points[19]);
+ cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
+ cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
+ return result;
+}
+
+bool
+ap3p::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, double mu2, double mv2, double X2, double Y2, double Z2, double mu3,
+ double mv3, double X3, double Y3, double Z3) {
+ double Rs[4][3][3], ts[4][3];
+
+ int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
+ if (n == 0)
+ return false;
+
+ int ns = 0;
+ double min_reproj = 0;
+ for (int i = 0; i < n; i++) {
+ double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
+ double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
+ double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
+ double mu3p = cx + fx * X3p / Z3p;
+ double mv3p = cy + fy * Y3p / Z3p;
+ double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
+ if (i == 0 || min_reproj > reproj) {
+ ns = i;
+ min_reproj = reproj;
+ }
+ }
+
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++)
+ R[i][j] = Rs[ns][i][j];
+ t[i] = ts[ns][i];
+ }
+
+ return true;
+}
+
+int ap3p::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, double mu2, double mv2, double X2, double Y2, double Z2) {
+ double mk0, mk1, mk2;
+ double norm;
+
+ mu0 = inv_fx * mu0 - cx_fx;
+ mv0 = inv_fy * mv0 - cy_fy;
+ norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
+ mk0 = 1. / norm;
+ mu0 *= mk0;
+ mv0 *= mk0;
+
+ mu1 = inv_fx * mu1 - cx_fx;
+ mv1 = inv_fy * mv1 - cy_fy;
+ norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
+ mk1 = 1. / norm;
+ mu1 *= mk1;
+ mv1 *= mk1;
+
+ mu2 = inv_fx * mu2 - cx_fx;
+ mv2 = inv_fy * mv2 - cy_fy;
+ norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
+ mk2 = 1. / norm;
+ mu2 *= mk2;
+ mv2 *= mk2;
+
+ double featureVectors[3][3] = {{mu0, mu1, mu2},
+ {mv0, mv1, mv2},
+ {mk0, mk1, mk2}};
+ double worldPoints[3][3] = {{X0, X1, X2},
+ {Y0, Y1, Y2},
+ {Z0, Z1, Z2}};
+
+ return computePoses(featureVectors, worldPoints, R, t);
+}
+}
\ No newline at end of file
diff --git a/modules/calib3d/src/ap3p.h b/modules/calib3d/src/ap3p.h
new file mode 100644
index 0000000000..37c7be12b4
--- /dev/null
+++ b/modules/calib3d/src/ap3p.h
@@ -0,0 +1,65 @@
+#ifndef P3P_P3P_H
+#define P3P_P3P_H
+
+#include "precomp.hpp"
+
+namespace cv {
+class ap3p {
+private:
+ template<typename T>
+ void init_camera_parameters(const cv::Mat &cameraMatrix) {
+ cx = cameraMatrix.at<T>(0, 2);
+ cy = cameraMatrix.at<T>(1, 2);
+ fx = cameraMatrix.at<T>(0, 0);
+ fy = cameraMatrix.at<T>(1, 1);
+ }
+
+ 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++) {
+ 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;
+ points[i * 5 + 3] = opoints.at<OpointType>(i).y;
+ points[i * 5 + 4] = opoints.at<OpointType>(i).z;
+ }
+ }
+
+ void init_inverse_parameters();
+
+ double fx, fy, cx, cy;
+ double inv_fx, inv_fy, cx_fx, cy_fy;
+public:
+ ap3p() {}
+
+ ap3p(double fx, double fy, double cx, double cy);
+
+ ap3p(cv::Mat cameraMatrix);
+
+ bool solve(cv::Mat &R, cv::Mat &tvec, 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,
+ double mu2, double mv2, double X2, double Y2, double Z2);
+
+ bool 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,
+ double mu2, double mv2, double X2, double Y2, double Z2,
+ double mu3, double mv3, double X3, double Y3, double Z3);
+
+ // This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
+ // See https://arxiv.org/pdf/1701.08237.pdf
+ // featureVectors: 3 bearing measurements (normalized) stored as column vectors
+ // worldPoints: Positions of the 3 feature points stored as column vectors
+ // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
+ // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
+ int computePoses(const double featureVectors[3][3], const double worldPoints[3][3], double solutionsR[4][3][3],
+ double solutionsT[4][3]);
+
+};
+}
+#endif //P3P_P3P_H
diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index 1e9b8ec6e4..a9be4c848d 100644
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@ -45,6 +45,7 @@
#include "dls.h"
#include "epnp.h"
#include "p3p.h"
+#include "ap3p.h"
#include "opencv2/calib3d/calib3d_c.h"
#include <iostream>
@@ -118,6 +119,18 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
if (result)
Rodrigues(R, rvec);
}
+ else if (flags == SOLVEPNP_AP3P)
+ {
+ CV_Assert( npoints == 4);
+ Mat undistortedPoints;
+ undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
+ ap3p P3Psolver(cameraMatrix);
+
+ Mat R;
+ result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
+ if (result)
+ Rodrigues(R, rvec);
+ }
else if (flags == SOLVEPNP_ITERATIVE)
{
CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
@@ -291,7 +304,8 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
opoints_inliers.resize(npoints1);
ipoints_inliers.resize(npoints1);
result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
- distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1;
+ distCoeffs, rvec, tvec, false,
+ (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
}
if( result <= 0 || _local_model.rows <= 0)
diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp
index e6e9b6b22f..86634a0b45 100644
--- a/modules/calib3d/test/test_solvepnp_ransac.cpp
+++ b/modules/calib3d/test/test_solvepnp_ransac.cpp
@@ -57,6 +57,7 @@ public:
eps[SOLVEPNP_ITERATIVE] = 1.0e-2;
eps[SOLVEPNP_EPNP] = 1.0e-2;
eps[SOLVEPNP_P3P] = 1.0e-2;
+ eps[SOLVEPNP_AP3P] = 1.0e-2;
eps[SOLVEPNP_DLS] = 1.0e-2;
eps[SOLVEPNP_UPNP] = 1.0e-2;
totalTestsCount = 10;
@@ -161,7 +162,7 @@ protected:
points.resize(pointsCount);
generate3DPointCloud(points);
- const int methodsCount = 5;
+ const int methodsCount = 6;
RNG rng = ts->get_rng();
@@ -189,7 +190,7 @@ protected:
}
}
}
- double eps[5];
+ double eps[6];
int totalTestsCount;
};
@@ -201,6 +202,7 @@ public:
eps[SOLVEPNP_ITERATIVE] = 1.0e-6;
eps[SOLVEPNP_EPNP] = 1.0e-6;
eps[SOLVEPNP_P3P] = 1.0e-4;
+ eps[SOLVEPNP_AP3P] = 1.0e-4;
eps[SOLVEPNP_DLS] = 1.0e-4;
eps[SOLVEPNP_UPNP] = 1.0e-4;
totalTestsCount = 1000;
@@ -222,7 +224,7 @@ protected:
generatePose(trueRvec, trueTvec, rng);
std::vector<Point3f> opoints;
- if (method == 2)
+ if (method == 2 || method == 5)
{
opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
}