mirror of
https://github.com/opencv/opencv.git
synced 2025-01-19 15:04:01 +08:00
added P3P method
added test for solvePnP changed test for solvePnPRansac fixed bug with mutex solvePnPRansac
This commit is contained in:
Alexander Shishkov
parent
6d8f4c6b82
commit
c11551a510
@ -86,6 +86,13 @@ CVAPI(void) cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst );
|
||||
#define CV_FM_RANSAC_ONLY CV_RANSAC
|
||||
#define CV_FM_LMEDS CV_LMEDS
|
||||
#define CV_FM_RANSAC CV_RANSAC
|
||||
|
||||
enum
|
||||
{
|
||||
CV_ITERATIVE = 0,
|
||||
CV_EPNP = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation"
|
||||
CV_P3P = 2 // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
|
||||
};
|
||||
|
||||
CVAPI(int) cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
|
||||
CvMat* fundamental_matrix,
|
||||
@ -431,10 +438,6 @@ public:
|
||||
|
||||
namespace cv
|
||||
{
|
||||
CV_EXPORTS_W double ePnP( InputArray _opoints, InputArray _ipoints,
|
||||
InputArray _cameraMatrix, InputArray _distCoeffs,
|
||||
OutputArray _rvec, OutputArray _tvec);
|
||||
|
||||
//! converts rotation vector to rotation matrix or vice versa using Rodrigues transformation
|
||||
CV_EXPORTS_W void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian=noArray());
|
||||
|
||||
@ -491,10 +494,16 @@ CV_EXPORTS_W void projectPoints( InputArray objectPoints,
|
||||
double aspectRatio=0 );
|
||||
|
||||
//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are not handled.
|
||||
CV_EXPORTS_W void solvePnP( InputArray objectPoints, InputArray imagePoints,
|
||||
enum
|
||||
{
|
||||
ITERATIVE=CV_ITERATIVE,
|
||||
EPNP=CV_EPNP,
|
||||
P3P=CV_P3P
|
||||
};
|
||||
CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints,
|
||||
InputArray cameraMatrix, InputArray distCoeffs,
|
||||
OutputArray rvec, OutputArray tvec,
|
||||
bool useExtrinsicGuess=false );
|
||||
bool useExtrinsicGuess=false, int flags=0);
|
||||
|
||||
//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible.
|
||||
CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints,
|
||||
@ -503,11 +512,12 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints,
|
||||
InputArray distCoeffs,
|
||||
OutputArray rvec,
|
||||
OutputArray tvec,
|
||||
bool useExtrinsicGuess = false,
|
||||
bool useExtrinsicGuess = false,
|
||||
int iterationsCount = 100,
|
||||
float reprojectionError = 8.0,
|
||||
int minInliersCount = 100,
|
||||
OutputArray inliers = noArray() );
|
||||
OutputArray inliers = noArray(),
|
||||
int flags = 0);
|
||||
|
||||
//! initializes camera matrix from a few 3D points and the corresponding projections.
|
||||
CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints,
|
||||
|
||||
@ -5,27 +5,36 @@
|
||||
|
||||
class epnp {
|
||||
public:
|
||||
epnp(void);
|
||||
epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
|
||||
~epnp();
|
||||
|
||||
void set_internal_parameters(const double uc, const double vc,
|
||||
const double fu, const double fv);
|
||||
|
||||
void set_maximum_number_of_correspondences(const int n);
|
||||
void reset_correspondences(void);
|
||||
void add_correspondence(const double X, const double Y, const double Z,
|
||||
const double u, const double v);
|
||||
|
||||
double compute_pose(double R[3][3], double T[3]);
|
||||
|
||||
void relative_error(double & rot_err, double & transl_err,
|
||||
const double Rtrue[3][3], const double ttrue[3],
|
||||
const double Rest[3][3], const double test[3]);
|
||||
|
||||
void print_pose(const double R[3][3], const double t[3]);
|
||||
double reprojection_error(const double R[3][3], const double t[3]);
|
||||
|
||||
void compute_pose(cv::Mat& R, cv::Mat& t);
|
||||
private:
|
||||
template <typename T>
|
||||
void init_camera_parameters(const cv::Mat& cameraMatrix)
|
||||
{
|
||||
uc = cameraMatrix.at<T> (0, 2);
|
||||
vc = cameraMatrix.at<T> (1, 2);
|
||||
fu = cameraMatrix.at<T> (0, 0);
|
||||
fv = cameraMatrix.at<T> (1, 1);
|
||||
}
|
||||
template <typename OpointType, typename IpointType>
|
||||
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
|
||||
{
|
||||
for(int i = 0; i < number_of_correspondences; i++)
|
||||
{
|
||||
pws[3 * i ] = opoints.at<OpointType>(0,i).x;
|
||||
pws[3 * i + 1] = opoints.at<OpointType>(0,i).y;
|
||||
pws[3 * i + 2] = opoints.at<OpointType>(0,i).z;
|
||||
|
||||
us[2 * i ] = ipoints.at<IpointType>(0,i).x*fu + uc;
|
||||
us[2 * i + 1] = ipoints.at<IpointType>(0,i).y*fv + vc;
|
||||
}
|
||||
}
|
||||
double reprojection_error(const double R[3][3], const double t[3]);
|
||||
void choose_control_points(void);
|
||||
void compute_barycentric_coordinates(void);
|
||||
void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
|
||||
@ -47,7 +56,7 @@ class epnp {
|
||||
|
||||
void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
|
||||
void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
|
||||
double cb[4], CvMat * A, CvMat * b);
|
||||
const double cb[4], CvMat * A, CvMat * b);
|
||||
|
||||
double compute_R_and_t(const double * ut, const double * betas,
|
||||
double R[3][3], double t[3]);
|
||||
@ -57,13 +66,10 @@ class epnp {
|
||||
void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
|
||||
double R_src[3][3], double t_src[3]);
|
||||
|
||||
void mat_to_quat(const double R[3][3], double q[4]);
|
||||
|
||||
|
||||
double uc, vc, fu, fv;
|
||||
|
||||
double * pws, * us, * alphas, * pcs;
|
||||
int maximum_number_of_correspondences;
|
||||
std::vector<double> pws, us, alphas, pcs;
|
||||
int number_of_correspondences;
|
||||
|
||||
double cws[4][3], ccs[4][3];
|
||||
|
||||
416
modules/calib3d/src/p3p.cpp
Normal file
416
modules/calib3d/src/p3p.cpp
Normal file
@ -0,0 +1,416 @@
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
#include "polynom_solver.h"
|
||||
#include "p3p.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
void p3p::init_inverse_parameters()
|
||||
{
|
||||
inv_fx = 1. / fx;
|
||||
inv_fy = 1. / fy;
|
||||
cx_fx = cx / fx;
|
||||
cy_fy = cy / fy;
|
||||
}
|
||||
|
||||
p3p::p3p(cv::Mat cameraMatrix)
|
||||
{
|
||||
if (cameraMatrix.depth() == CV_32F)
|
||||
init_camera_parameters<float>(cameraMatrix);
|
||||
else
|
||||
init_camera_parameters<double>(cameraMatrix);
|
||||
init_inverse_parameters();
|
||||
}
|
||||
|
||||
p3p::p3p(double _fx, double _fy, double _cx, double _cy)
|
||||
{
|
||||
fx = _fx;
|
||||
fy = _fy;
|
||||
cx = _cx;
|
||||
cy = _cy;
|
||||
init_inverse_parameters();
|
||||
}
|
||||
|
||||
bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints)
|
||||
{
|
||||
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 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,
|
||||
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 p3p::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 distances[3];
|
||||
distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
|
||||
distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
|
||||
distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) );
|
||||
|
||||
// Calculate angles
|
||||
double cosines[3];
|
||||
cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
|
||||
cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
|
||||
cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
|
||||
|
||||
double lengths[4][3];
|
||||
int n = solve_for_lengths(lengths, distances, cosines);
|
||||
|
||||
int nb_solutions = 0;
|
||||
for(int i = 0; i < n; i++) {
|
||||
double M_orig[3][3];
|
||||
|
||||
M_orig[0][0] = lengths[i][0] * mu0;
|
||||
M_orig[0][1] = lengths[i][0] * mv0;
|
||||
M_orig[0][2] = lengths[i][0] * mk0;
|
||||
|
||||
M_orig[1][0] = lengths[i][1] * mu1;
|
||||
M_orig[1][1] = lengths[i][1] * mv1;
|
||||
M_orig[1][2] = lengths[i][1] * mk1;
|
||||
|
||||
M_orig[2][0] = lengths[i][2] * mu2;
|
||||
M_orig[2][1] = lengths[i][2] * mv2;
|
||||
M_orig[2][2] = lengths[i][2] * mk2;
|
||||
|
||||
if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
|
||||
continue;
|
||||
|
||||
nb_solutions++;
|
||||
}
|
||||
|
||||
return nb_solutions;
|
||||
}
|
||||
|
||||
/// Given 3D distances between three points and cosines of 3 angles at the apex, calculates
|
||||
/// the lentghs of the line segments connecting projection center (P) and the three 3D points (A, B, C).
|
||||
/// Returned distances are for |PA|, |PB|, |PC| respectively.
|
||||
/// Only the solution to the main branch.
|
||||
/// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
|
||||
/// IEEE Trans. on PAMI, vol. 25, No. 8, August 2003
|
||||
/// \param lengths3D Lengths of line segments up to four solutions.
|
||||
/// \param dist3D Distance between 3D points in pairs |BC|, |AC|, |AB|.
|
||||
/// \param cosines Cosine of the angles /_BPC, /_APC, /_APB.
|
||||
/// \returns Number of solutions.
|
||||
/// WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED
|
||||
|
||||
int p3p::solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3])
|
||||
{
|
||||
double p = cosines[0] * 2;
|
||||
double q = cosines[1] * 2;
|
||||
double r = cosines[2] * 2;
|
||||
|
||||
double inv_d22 = 1. / (distances[2] * distances[2]);
|
||||
double a = inv_d22 * (distances[0] * distances[0]);
|
||||
double b = inv_d22 * (distances[1] * distances[1]);
|
||||
|
||||
double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r;
|
||||
double pr = p * r, pqr = q * pr;
|
||||
|
||||
// Check reality condition (the four points should not be coplanar)
|
||||
if (p2 + q2 + r2 - pqr - 1 == 0)
|
||||
return 0;
|
||||
|
||||
double ab = a * b, a_2 = 2*a;
|
||||
|
||||
double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2;
|
||||
|
||||
// Check reality condition
|
||||
if (A == 0) return 0;
|
||||
|
||||
double a_4 = 4*a;
|
||||
|
||||
double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab);
|
||||
double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2;
|
||||
double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2);
|
||||
double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2;
|
||||
|
||||
double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr);
|
||||
double b0 = b * temp * temp;
|
||||
// Check reality condition
|
||||
if (b0 == 0)
|
||||
return 0;
|
||||
|
||||
double real_roots[4];
|
||||
int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]);
|
||||
|
||||
if (n == 0)
|
||||
return 0;
|
||||
|
||||
int nb_solutions = 0;
|
||||
double r3 = r2*r, pr2 = p*r2, r3q = r3 * q;
|
||||
double inv_b0 = 1. / b0;
|
||||
|
||||
// For each solution of x
|
||||
for(int i = 0; i < n; i++) {
|
||||
double x = real_roots[i];
|
||||
|
||||
// Check reality condition
|
||||
if (x <= 0)
|
||||
continue;
|
||||
|
||||
double x2 = x*x;
|
||||
|
||||
double b1 =
|
||||
((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) *
|
||||
(((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x +
|
||||
|
||||
(r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 +
|
||||
|
||||
(r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x +
|
||||
|
||||
2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) +
|
||||
p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1)));
|
||||
|
||||
// Check reality condition
|
||||
if (b1 <= 0)
|
||||
continue;
|
||||
|
||||
double y = inv_b0 * b1;
|
||||
double v = x2 + y*y - x*y*r;
|
||||
|
||||
if (v <= 0)
|
||||
continue;
|
||||
|
||||
double Z = distances[2] / sqrt(v);
|
||||
double X = x * Z;
|
||||
double Y = y * Z;
|
||||
|
||||
lengths[nb_solutions][0] = X;
|
||||
lengths[nb_solutions][1] = Y;
|
||||
lengths[nb_solutions][2] = Z;
|
||||
|
||||
nb_solutions++;
|
||||
}
|
||||
|
||||
return nb_solutions;
|
||||
}
|
||||
|
||||
bool p3p::align(double M_end[3][3],
|
||||
double X0, double Y0, double Z0,
|
||||
double X1, double Y1, double Z1,
|
||||
double X2, double Y2, double Z2,
|
||||
double R[3][3], double T[3])
|
||||
{
|
||||
// Centroids:
|
||||
double C_start[3], C_end[3];
|
||||
for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;
|
||||
C_start[0] = (X0 + X1 + X2) / 3;
|
||||
C_start[1] = (Y0 + Y1 + Y2) / 3;
|
||||
C_start[2] = (Z0 + Z1 + Z2) / 3;
|
||||
|
||||
// Covariance matrix s:
|
||||
double s[3 * 3];
|
||||
for(int j = 0; j < 3; j++) {
|
||||
s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0];
|
||||
s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1];
|
||||
s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2];
|
||||
}
|
||||
|
||||
double Qs[16], evs[4], U[16];
|
||||
|
||||
Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];
|
||||
Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];
|
||||
Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0];
|
||||
Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1];
|
||||
|
||||
Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1];
|
||||
Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2];
|
||||
Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0];
|
||||
Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1];
|
||||
Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2];
|
||||
Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2];
|
||||
|
||||
jacobi_4x4(Qs, evs, U);
|
||||
|
||||
// Looking for the largest eigen value:
|
||||
int i_ev = 0;
|
||||
double ev_max = evs[i_ev];
|
||||
for(int i = 1; i < 4; i++)
|
||||
if (evs[i] > ev_max)
|
||||
ev_max = evs[i_ev = i];
|
||||
|
||||
// Quaternion:
|
||||
double q[4];
|
||||
for(int i = 0; i < 4; i++)
|
||||
q[i] = U[i * 4 + i_ev];
|
||||
|
||||
double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3];
|
||||
double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3];
|
||||
double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3];
|
||||
double q2_3 = q[2] * q[3];
|
||||
|
||||
R[0][0] = q02 + q12 - q22 - q32;
|
||||
R[0][1] = 2. * (q1_2 - q0_3);
|
||||
R[0][2] = 2. * (q1_3 + q0_2);
|
||||
|
||||
R[1][0] = 2. * (q1_2 + q0_3);
|
||||
R[1][1] = q02 + q22 - q12 - q32;
|
||||
R[1][2] = 2. * (q2_3 - q0_1);
|
||||
|
||||
R[2][0] = 2. * (q1_3 - q0_2);
|
||||
R[2][1] = 2. * (q2_3 + q0_1);
|
||||
R[2][2] = q02 + q32 - q12 - q22;
|
||||
|
||||
for(int i = 0; i < 3; i++)
|
||||
T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool p3p::jacobi_4x4(double * A, double * D, double * U)
|
||||
{
|
||||
double B[4], Z[4];
|
||||
double Id[16] = {1., 0., 0., 0.,
|
||||
0., 1., 0., 0.,
|
||||
0., 0., 1., 0.,
|
||||
0., 0., 0., 1.};
|
||||
|
||||
memcpy(U, Id, 16 * sizeof(double));
|
||||
|
||||
B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15];
|
||||
memcpy(D, B, 4 * sizeof(double));
|
||||
memset(Z, 0, 4 * sizeof(double));
|
||||
|
||||
for(int iter = 0; iter < 50; iter++) {
|
||||
double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]);
|
||||
|
||||
if (sum == 0.0)
|
||||
return true;
|
||||
|
||||
double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0;
|
||||
for(int i = 0; i < 3; i++) {
|
||||
double * pAij = A + 5 * i + 1;
|
||||
for(int j = i + 1 ; j < 4; j++) {
|
||||
double Aij = *pAij;
|
||||
double eps_machine = 100.0 * fabs(Aij);
|
||||
|
||||
if ( iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) && fabs(D[j]) + eps_machine == fabs(D[j]) )
|
||||
*pAij = 0.0;
|
||||
else if (fabs(Aij) > tresh) {
|
||||
double h = D[j] - D[i], t;
|
||||
if (fabs(h) + eps_machine == fabs(h))
|
||||
t = Aij / h;
|
||||
else {
|
||||
double theta = 0.5 * h / Aij;
|
||||
t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
|
||||
if (theta < 0.0) t = -t;
|
||||
}
|
||||
|
||||
h = t * Aij;
|
||||
Z[i] -= h;
|
||||
Z[j] += h;
|
||||
D[i] -= h;
|
||||
D[j] += h;
|
||||
*pAij = 0.0;
|
||||
|
||||
double c = 1.0 / sqrt(1 + t * t);
|
||||
double s = t * c;
|
||||
double tau = s / (1.0 + c);
|
||||
for(int k = 0; k <= i - 1; k++) {
|
||||
double g = A[k * 4 + i], h = A[k * 4 + j];
|
||||
A[k * 4 + i] = g - s * (h + g * tau);
|
||||
A[k * 4 + j] = h + s * (g - h * tau);
|
||||
}
|
||||
for(int k = i + 1; k <= j - 1; k++) {
|
||||
double g = A[i * 4 + k], h = A[k * 4 + j];
|
||||
A[i * 4 + k] = g - s * (h + g * tau);
|
||||
A[k * 4 + j] = h + s * (g - h * tau);
|
||||
}
|
||||
for(int k = j + 1; k < 4; k++) {
|
||||
double g = A[i * 4 + k], h = A[j * 4 + k];
|
||||
A[i * 4 + k] = g - s * (h + g * tau);
|
||||
A[j * 4 + k] = h + s * (g - h * tau);
|
||||
}
|
||||
for(int k = 0; k < 4; k++) {
|
||||
double g = U[k * 4 + i], h = U[k * 4 + j];
|
||||
U[k * 4 + i] = g - s * (h + g * tau);
|
||||
U[k * 4 + j] = h + s * (g - h * tau);
|
||||
}
|
||||
}
|
||||
pAij++;
|
||||
}
|
||||
}
|
||||
|
||||
for(int i = 0; i < 4; i++) B[i] += Z[i];
|
||||
memcpy(D, B, 4 * sizeof(double));
|
||||
memset(Z, 0, 4 * sizeof(double));
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
62
modules/calib3d/src/p3p.h
Normal file
62
modules/calib3d/src/p3p.h
Normal file
@ -0,0 +1,62 @@
|
||||
#ifndef P3P_H
|
||||
#define P3P_H
|
||||
|
||||
|
||||
#include "precomp.hpp"
|
||||
|
||||
class p3p
|
||||
{
|
||||
public:
|
||||
p3p(double fx, double fy, double cx, double cy);
|
||||
p3p(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);
|
||||
|
||||
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>(0,i).x*fx + cx;
|
||||
points[i*5+1] = ipoints.at<IpointType>(0,i).y*fy + cy;
|
||||
points[i*5+2] = opoints.at<OpointType>(0,i).x;
|
||||
points[i*5+3] = opoints.at<OpointType>(0,i).y;
|
||||
points[i*5+4] = opoints.at<OpointType>(0,i).z;
|
||||
}
|
||||
}
|
||||
void init_inverse_parameters();
|
||||
int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]);
|
||||
bool align(double M_start[3][3],
|
||||
double X0, double Y0, double Z0,
|
||||
double X1, double Y1, double Z1,
|
||||
double X2, double Y2, double Z2,
|
||||
double R[3][3], double T[3]);
|
||||
|
||||
bool jacobi_4x4(double * A, double * D, double * U);
|
||||
|
||||
double fx, fy, cx, cy;
|
||||
double inv_fx, inv_fy, cx_fx, cy_fy;
|
||||
};
|
||||
|
||||
#endif // P3P_H
|
||||
|
||||
170
modules/calib3d/src/polynom_solver.cpp
Normal file
170
modules/calib3d/src/polynom_solver.cpp
Normal file
@ -0,0 +1,170 @@
|
||||
#include <math.h>
|
||||
#include <iostream>
|
||||
using namespace std;
|
||||
|
||||
#include "polynom_solver.h"
|
||||
|
||||
int solve_deg2(double a, double b, double c, double & x1, double & x2)
|
||||
{
|
||||
double delta = b * b - 4 * a * c;
|
||||
|
||||
if (delta < 0) return 0;
|
||||
|
||||
double inv_2a = 0.5 / a;
|
||||
|
||||
if (delta == 0) {
|
||||
x1 = -b * inv_2a;
|
||||
x2 = x1;
|
||||
return 1;
|
||||
}
|
||||
|
||||
double sqrt_delta = sqrt(delta);
|
||||
x1 = (-b + sqrt_delta) * inv_2a;
|
||||
x2 = (-b - sqrt_delta) * inv_2a;
|
||||
return 2;
|
||||
}
|
||||
|
||||
|
||||
/// Reference : Eric W. Weisstein. "Cubic Equation." From MathWorld--A Wolfram Web Resource.
|
||||
/// http://mathworld.wolfram.com/CubicEquation.html
|
||||
/// \return Number of real roots found.
|
||||
int solve_deg3(double a, double b, double c, double d,
|
||||
double & x0, double & x1, double & x2)
|
||||
{
|
||||
if (a == 0) {
|
||||
// Solve second order sytem
|
||||
if (b == 0) {
|
||||
// Solve first order system
|
||||
if (c == 0)
|
||||
return 0;
|
||||
|
||||
x0 = -d / c;
|
||||
return 1;
|
||||
}
|
||||
|
||||
x2 = 0;
|
||||
return solve_deg2(b, c, d, x0, x1);
|
||||
}
|
||||
|
||||
// Calculate the normalized form x^3 + a2 * x^2 + a1 * x + a0 = 0
|
||||
double inv_a = 1. / a;
|
||||
double b_a = inv_a * b, b_a2 = b_a * b_a;
|
||||
double c_a = inv_a * c;
|
||||
double d_a = inv_a * d;
|
||||
|
||||
// Solve the cubic equation
|
||||
double Q = (3 * c_a - b_a2) / 9;
|
||||
double R = (9 * b_a * c_a - 27 * d_a - 2 * b_a * b_a2) / 54;
|
||||
double Q3 = Q * Q * Q;
|
||||
double D = Q3 + R * R;
|
||||
double b_a_3 = (1. / 3.) * b_a;
|
||||
|
||||
if (Q == 0) {
|
||||
if(R == 0) {
|
||||
x0 = x1 = x2 = - b_a_3;
|
||||
return 3;
|
||||
}
|
||||
else {
|
||||
x0 = pow(2 * R, 1 / 3.0) - b_a_3;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (D <= 0) {
|
||||
// Three real roots
|
||||
double theta = acos(R / sqrt(-Q3));
|
||||
double sqrt_Q = sqrt(-Q);
|
||||
x0 = 2 * sqrt_Q * cos(theta / 3.0) - b_a_3;
|
||||
x1 = 2 * sqrt_Q * cos((theta + 2 * CV_PI)/ 3.0) - b_a_3;
|
||||
x2 = 2 * sqrt_Q * cos((theta + 4 * CV_PI)/ 3.0) - b_a_3;
|
||||
|
||||
return 3;
|
||||
}
|
||||
|
||||
// D > 0, only one real root
|
||||
double AD = pow(fabs(R) + sqrt(D), 1.0 / 3.0) * (R > 0 ? 1 : (R < 0 ? -1 : 0));
|
||||
double BD = (AD == 0) ? 0 : -Q / AD;
|
||||
|
||||
// Calculate the only real root
|
||||
x0 = AD + BD - b_a_3;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// Reference : Eric W. Weisstein. "Quartic Equation." From MathWorld--A Wolfram Web Resource.
|
||||
/// http://mathworld.wolfram.com/QuarticEquation.html
|
||||
/// \return Number of real roots found.
|
||||
int solve_deg4(double a, double b, double c, double d, double e,
|
||||
double & x0, double & x1, double & x2, double & x3)
|
||||
{
|
||||
if (a == 0) {
|
||||
x3 = 0;
|
||||
return solve_deg3(b, c, d, e, x0, x1, x2);
|
||||
}
|
||||
|
||||
// Normalize coefficients
|
||||
double inv_a = 1. / a;
|
||||
b *= inv_a; c *= inv_a; d *= inv_a; e *= inv_a;
|
||||
double b2 = b * b, bc = b * c, b3 = b2 * b;
|
||||
|
||||
// Solve resultant cubic
|
||||
double r0, r1, r2;
|
||||
int n = solve_deg3(1, -c, d * b - 4 * e, 4 * c * e - d * d - b2 * e, r0, r1, r2);
|
||||
if (n == 0) return 0;
|
||||
|
||||
// Calculate R^2
|
||||
double R2 = 0.25 * b2 - c + r0, R;
|
||||
if (R2 < 0)
|
||||
return 0;
|
||||
|
||||
R = sqrt(R2);
|
||||
double inv_R = 1. / R;
|
||||
|
||||
int nb_real_roots = 0;
|
||||
|
||||
// Calculate D^2 and E^2
|
||||
double D2, E2;
|
||||
if (R < 10E-12) {
|
||||
double temp = r0 * r0 - 4 * e;
|
||||
if (temp < 0)
|
||||
D2 = E2 = -1;
|
||||
else {
|
||||
double sqrt_temp = sqrt(temp);
|
||||
D2 = 0.75 * b2 - 2 * c + 2 * sqrt_temp;
|
||||
E2 = D2 - 4 * sqrt_temp;
|
||||
}
|
||||
}
|
||||
else {
|
||||
double u = 0.75 * b2 - 2 * c - R2,
|
||||
v = 0.25 * inv_R * (4 * bc - 8 * d - b3);
|
||||
D2 = u + v;
|
||||
E2 = u - v;
|
||||
}
|
||||
|
||||
double b_4 = 0.25 * b, R_2 = 0.5 * R;
|
||||
if (D2 >= 0) {
|
||||
double D = sqrt(D2);
|
||||
nb_real_roots = 2;
|
||||
double D_2 = 0.5 * D;
|
||||
x0 = R_2 + D_2 - b_4;
|
||||
x1 = x0 - D;
|
||||
}
|
||||
|
||||
// Calculate E^2
|
||||
if (E2 >= 0) {
|
||||
double E = sqrt(E2);
|
||||
double E_2 = 0.5 * E;
|
||||
if (nb_real_roots == 0) {
|
||||
x0 = - R_2 + E_2 - b_4;
|
||||
x1 = x0 - E;
|
||||
nb_real_roots = 2;
|
||||
}
|
||||
else {
|
||||
x2 = - R_2 + E_2 - b_4;
|
||||
x3 = x2 - E;
|
||||
nb_real_roots = 4;
|
||||
}
|
||||
}
|
||||
|
||||
return nb_real_roots;
|
||||
}
|
||||
12
modules/calib3d/src/polynom_solver.h
Normal file
12
modules/calib3d/src/polynom_solver.h
Normal file
@ -0,0 +1,12 @@
|
||||
#ifndef POLYNOM_SOLVER_H
|
||||
#define POLYNOM_SOLVER_H
|
||||
|
||||
int solve_deg2(double a, double b, double c, double & x1, double & x2);
|
||||
|
||||
int solve_deg3(double a, double b, double c, double d,
|
||||
double & x0, double & x1, double & x2);
|
||||
|
||||
int solve_deg4(double a, double b, double c, double d, double e,
|
||||
double & x0, double & x1, double & x2, double & x3);
|
||||
|
||||
#endif // POLYNOM_SOLVER_H
|
||||
@ -41,25 +41,62 @@
|
||||
//M*/
|
||||
|
||||
#include "precomp.hpp"
|
||||
#include "epnp.h"
|
||||
#include "p3p.h"
|
||||
#include <iostream>
|
||||
using namespace cv;
|
||||
|
||||
void cv::solvePnP( InputArray _opoints, InputArray _ipoints,
|
||||
bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
|
||||
InputArray _cameraMatrix, InputArray _distCoeffs,
|
||||
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess )
|
||||
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
|
||||
{
|
||||
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
|
||||
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
|
||||
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
|
||||
|
||||
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
|
||||
_rvec.create(3, 1, CV_64F);
|
||||
_tvec.create(3, 1, CV_64F);
|
||||
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
|
||||
CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
|
||||
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
|
||||
CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
|
||||
cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
|
||||
c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
|
||||
&c_rvec, &c_tvec, useExtrinsicGuess );
|
||||
|
||||
if (flags == CV_EPNP)
|
||||
{
|
||||
cv::Mat undistortedPoints;
|
||||
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
|
||||
epnp PnP(cameraMatrix, opoints, undistortedPoints);
|
||||
|
||||
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
|
||||
PnP.compute_pose(R, tvec);
|
||||
cv::Rodrigues(R, rvec);
|
||||
return true;
|
||||
}
|
||||
else if (flags == CV_P3P)
|
||||
{
|
||||
CV_Assert( npoints == 4);
|
||||
cv::Mat undistortedPoints;
|
||||
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
|
||||
p3p P3Psolver(cameraMatrix);
|
||||
|
||||
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
|
||||
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
|
||||
if (result)
|
||||
cv::Rodrigues(R, rvec);
|
||||
return result;
|
||||
}
|
||||
else if (flags == CV_ITERATIVE)
|
||||
{
|
||||
CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
|
||||
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
|
||||
CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
|
||||
cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
|
||||
c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
|
||||
&c_rvec, &c_tvec, useExtrinsicGuess );
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE or CV_EPNP");
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
namespace cv
|
||||
@ -84,25 +121,25 @@ namespace cv
|
||||
class Mutex
|
||||
{
|
||||
public:
|
||||
Mutex() {}
|
||||
Mutex() {
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
#ifdef HAVE_TBB
|
||||
slock.acquire(resultsMutex);
|
||||
resultsMutex.lock();
|
||||
#endif
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
#ifdef HAVE_TBB
|
||||
slock.release();
|
||||
resultsMutex.unlock();
|
||||
#endif
|
||||
}
|
||||
|
||||
private:
|
||||
#ifdef HAVE_TBB
|
||||
tbb::mutex resultsMutex;
|
||||
tbb::mutex::scoped_lock slock;
|
||||
#endif
|
||||
};
|
||||
|
||||
@ -124,6 +161,7 @@ namespace cv
|
||||
float reprojectionError;
|
||||
int minInliersCount;
|
||||
bool useExtrinsicGuess;
|
||||
int flags;
|
||||
CameraParameters camera;
|
||||
};
|
||||
|
||||
@ -159,8 +197,10 @@ namespace cv
|
||||
Mat localRvec, localTvec;
|
||||
rvecInit.copyTo(localRvec);
|
||||
tvecInit.copyTo(localTvec);
|
||||
|
||||
solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, params.useExtrinsicGuess);
|
||||
|
||||
solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec,
|
||||
params.useExtrinsicGuess, params.flags);
|
||||
|
||||
|
||||
vector<Point2f> projected_points;
|
||||
projected_points.resize(objectPoints.cols);
|
||||
@ -206,7 +246,7 @@ namespace cv
|
||||
generateVar(pointsMask);
|
||||
pnpTask(pointsMask, objectPoints, imagePoints, parameters,
|
||||
inliers, rvec, tvec, initRvec, initTvec, syncMutex);
|
||||
if ((int)inliers.size() > parameters.minInliersCount)
|
||||
if ((int)inliers.size() >= parameters.minInliersCount)
|
||||
{
|
||||
#ifdef HAVE_TBB
|
||||
tbb::task::self().cancel_group_execution();
|
||||
@ -261,7 +301,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
|
||||
InputArray _cameraMatrix, InputArray _distCoeffs,
|
||||
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
|
||||
int iterationsCount, float reprojectionError, int minInliersCount,
|
||||
OutputArray _inliers)
|
||||
OutputArray _inliers, int flags)
|
||||
{
|
||||
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
|
||||
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
|
||||
@ -288,6 +328,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
|
||||
params.reprojectionError = reprojectionError;
|
||||
params.useExtrinsicGuess = useExtrinsicGuess;
|
||||
params.camera.init(cameraMatrix, distCoeffs);
|
||||
params.flags = flags;
|
||||
|
||||
vector<int> localInliers;
|
||||
Mat localRvec, localTvec;
|
||||
@ -302,18 +343,21 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
|
||||
|
||||
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
|
||||
{
|
||||
int i, pointsCount = (int)localInliers.size();
|
||||
Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
|
||||
for (i = 0; i < pointsCount; i++)
|
||||
{
|
||||
int 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);
|
||||
if (flags != CV_P3P)
|
||||
{
|
||||
int i, pointsCount = (int)localInliers.size();
|
||||
Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
|
||||
for (i = 0; i < pointsCount; i++)
|
||||
{
|
||||
int 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, flags);
|
||||
}
|
||||
localRvec.copyTo(rvec);
|
||||
localTvec.copyTo(tvec);
|
||||
if (_inliers.needed())
|
||||
Mat(localInliers).copyTo(_inliers);
|
||||
|
||||
@ -48,100 +48,194 @@ using namespace std;
|
||||
class CV_solvePnPRansac_Test : public cvtest::BaseTest
|
||||
{
|
||||
public:
|
||||
CV_solvePnPRansac_Test() {}
|
||||
CV_solvePnPRansac_Test()
|
||||
{
|
||||
eps[CV_ITERATIVE] = 1.0e-2;
|
||||
eps[CV_EPNP] = 1.0e-2;
|
||||
eps[CV_P3P] = 1.0e-2;
|
||||
totalTestsCount = 10;
|
||||
}
|
||||
~CV_solvePnPRansac_Test() {}
|
||||
protected:
|
||||
void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1,
|
||||
-1, 5), Point3f pmax = Point3f(1, 1, 10))
|
||||
-1, 5), Point3f pmax = Point3f(1, 1, 10))
|
||||
{
|
||||
const Point3f delta = pmax - pmin;
|
||||
for (size_t i = 0; i < points.size(); i++)
|
||||
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 generateCameraMatrix(Mat& cameraMatrix, RNG& rng)
|
||||
{
|
||||
const double fcMinVal = 1e-3;
|
||||
const double fcMaxVal = 100;
|
||||
cameraMatrix.create(3, 3, CV_64FC1);
|
||||
cameraMatrix.setTo(Scalar(0));
|
||||
cameraMatrix.at<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal);
|
||||
cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal);
|
||||
cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal);
|
||||
cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal);
|
||||
cameraMatrix.at<double>(2,2) = 1;
|
||||
}
|
||||
|
||||
void generateDistCoeffs(Mat& distCoeffs, RNG& rng)
|
||||
{
|
||||
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
|
||||
for (int i = 0; i < 3; i++)
|
||||
distCoeffs.at<double>(i,0) = rng.uniform(0.0, 1.0e-6);
|
||||
}
|
||||
|
||||
void generatePose(Mat& rvec, Mat& tvec, RNG& rng)
|
||||
{
|
||||
const double minVal = 1.0e-3;
|
||||
const double maxVal = 1.0;
|
||||
rvec.create(3, 1, CV_64FC1);
|
||||
tvec.create(3, 1, CV_64FC1);
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
rvec.at<double>(i,0) = rng.uniform(minVal, maxVal);
|
||||
tvec.at<double>(i,0) = rng.uniform(minVal, maxVal/10);
|
||||
}
|
||||
}
|
||||
|
||||
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* eps, double& maxError)
|
||||
{
|
||||
Mat rvec, tvec;
|
||||
vector<int> inliers;
|
||||
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);
|
||||
|
||||
vector<Point2f> projectedPoints;
|
||||
projectedPoints.resize(points.size());
|
||||
projectPoints(Mat(points), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
|
||||
for (size_t i = 0; i < projectedPoints.size(); i++)
|
||||
{
|
||||
if (i % 20 == 0)
|
||||
{
|
||||
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;
|
||||
projectedPoints[i] = projectedPoints[rng.uniform(0,points.size()-1)];
|
||||
}
|
||||
}
|
||||
|
||||
solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
|
||||
false, 500, 0.5, -1, inliers, method);
|
||||
|
||||
bool isTestSuccess = inliers.size() >= points.size()*0.95;
|
||||
|
||||
double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
|
||||
isTestSuccess = isTestSuccess && rvecDiff < eps[method] && tvecDiff < eps[method];
|
||||
double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
|
||||
//cout << error << " " << inliers.size() << " " << eps[method] << endl;
|
||||
if (error > maxError)
|
||||
maxError = error;
|
||||
|
||||
return isTestSuccess;
|
||||
}
|
||||
|
||||
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);
|
||||
const int pointsCount = 500;
|
||||
points.resize(pointsCount);
|
||||
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++)
|
||||
|
||||
const int methodsCount = 3;
|
||||
RNG rng = ts.get_rng();
|
||||
|
||||
|
||||
for (int mode = 0; mode < 2; mode++)
|
||||
{
|
||||
if (i % 20 == 0)
|
||||
for (int method = 0; method < methodsCount; method++)
|
||||
{
|
||||
points1[i] = points1[rand() % points.size()];
|
||||
}
|
||||
}
|
||||
double eps = 1.0e-7;
|
||||
|
||||
int successfulTestsCount = 0;
|
||||
int totalTestsCount = 10;
|
||||
|
||||
for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
|
||||
{
|
||||
try
|
||||
{
|
||||
Mat rvec, tvec;
|
||||
vector<int> inliers;
|
||||
|
||||
solvePnPRansac(points, 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)
|
||||
successfulTestsCount++;
|
||||
|
||||
}
|
||||
catch(...)
|
||||
{
|
||||
ts.printf(cvtest::TS::LOG, "Exception in solvePnPRansac\n");
|
||||
double maxError = 0;
|
||||
int successfulTestsCount = 0;
|
||||
for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
|
||||
{
|
||||
if (runTest(rng, mode, 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
double eps[3];
|
||||
int totalTestsCount;
|
||||
};
|
||||
|
||||
if (successfulTestsCount < 0.8*totalTestsCount)
|
||||
class CV_solvePnP_Test : public CV_solvePnPRansac_Test
|
||||
{
|
||||
public:
|
||||
CV_solvePnP_Test()
|
||||
{
|
||||
eps[CV_ITERATIVE] = 1.0e-6;
|
||||
eps[CV_EPNP] = 1.0e-6;
|
||||
eps[CV_P3P] = 1.0e-4;
|
||||
totalTestsCount = 1000;
|
||||
}
|
||||
|
||||
~CV_solvePnP_Test() {}
|
||||
protected:
|
||||
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* eps, double& maxError)
|
||||
{
|
||||
Mat rvec, tvec;
|
||||
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;
|
||||
if (method == 2)
|
||||
{
|
||||
ts.printf( cvtest::TS::LOG, "Invalid accuracy, failed %d tests from %d\n",
|
||||
totalTestsCount - successfulTestsCount, totalTestsCount);
|
||||
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
|
||||
opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
|
||||
}
|
||||
else
|
||||
opoints = points;
|
||||
|
||||
vector<Point2f> projectedPoints;
|
||||
projectedPoints.resize(opoints.size());
|
||||
projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
|
||||
|
||||
solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
|
||||
false, method);
|
||||
|
||||
double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
|
||||
bool isTestSuccess = rvecDiff < eps[method] && tvecDiff < eps[method];
|
||||
|
||||
double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
|
||||
if (error > maxError)
|
||||
maxError = error;
|
||||
|
||||
return isTestSuccess;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
|
||||
TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user