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

Merge pull request #14431 from catree:feat_solvePnPRefine

Browse Source
pull/14484/head
Alexander Alekhin 6 years ago
parent b7ca3d77a6 dac31e84fb
commit fc57129300
6 changed files with 711 additions and 6 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. 55
      doc/opencv.bib
  2. 59
      modules/calib3d/include/opencv2/calib3d.hpp
  3. 9
      modules/calib3d/src/levmarq.cpp
  4. 1
      modules/calib3d/src/precomp.hpp
  5. 267
      modules/calib3d/src/solvepnp.cpp
  6. 326
      modules/calib3d/test/test_solvepnp_ransac.cpp

55
doc/opencv.bib
Unescape View File

@ -195,6 +195,21 @@
volume = {9},
publisher = {Walter de Gruyter}
}
@article{Chaumette06,
author = {Chaumette, Fran{\c c}ois and Hutchinson, S.},
title = {{Visual servo control, Part I: Basic approaches}},
url = {https://hal.inria.fr/inria-00350283},
journal = {{IEEE Robotics and Automation Magazine}},
publisher = {{Institute of Electrical and Electronics Engineers}},
volume = {13},
number = {4},
pages = {82-90},
year = {2006},
pdf = {https://hal.inria.fr/inria-00350283/file/2006_ieee_ram_chaumette.pdf},
hal_id = {inria-00350283},
hal_version = {v1},
}
@article{Daniilidis98,
author = {Konstantinos Daniilidis},
title = {Hand-Eye Calibration Using Dual Quaternions},
@ -242,6 +257,12 @@
publisher = {IEEE},
url = {http://alumni.media.mit.edu/~jdavis/Publications/publications_402.pdf}
}
@misc{Eade13,
author = {Eade, Ethan},
title = {Gauss-Newton / Levenberg-Marquardt Optimization},
year = {2013},
url = {http://ethaneade.com/optimization.pdf}
}
@inproceedings{EM11,
author = {Gastal, Eduardo SL and Oliveira, Manuel M},
title = {Domain transform for edge-aware image and video processing},
@ -596,10 +617,14 @@
title = {ROF and TV-L1 denoising with Primal-Dual algorithm},
url = {http://znah.net/rof-and-tv-l1-denoising-with-primal-dual-algorithm.html}
}
@misc{VandLec,
author = {Vandenberghe, Lieven},
title = {QR Factorization},
url = {http://www.seas.ucla.edu/~vandenbe/133A/lectures/qr.pdf}
@misc{Madsen04,
author = {K. Madsen and H. B. Nielsen and O. Tingleff},
title = {Methods for Non-Linear Least Squares Problems (2nd ed.)},
year = {2004},
pages = {60},
publisher = {Informatics and Mathematical Modelling, Technical University of Denmark, {DTU}},
address = {Richard Petersens Plads, Building 321, {DK-}2800 Kgs. Lyngby},
url = {http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf}
}
@article{MHT2011,
author = {Getreuer, Pascal},
@ -645,6 +670,23 @@
title = {Deeper understanding of the homography decomposition for vision-based control},
year = {2007}
}
@article{Marchand16,
author = {Marchand, Eric and Uchiyama, Hideaki and Spindler, Fabien},
title = {{Pose Estimation for Augmented Reality: A Hands-On Survey}},
url = {https://hal.inria.fr/hal-01246370},
journal = {{IEEE Transactions on Visualization and Computer Graphics}},
publisher = {{Institute of Electrical and Electronics Engineers}},
volume = {22},
number = {12},
pages = {2633 - 2651},
year = {2016},
month = Dec,
doi = {10.1109/TVCG.2015.2513408},
keywords = {homography ; SLAM ; motion estimation ; Index Terms-Survey ; augmented reality ; vision-based camera localization ; pose estimation ; PnP ; keypoint matching ; code examples},
pdf = {https://hal.inria.fr/hal-01246370/file/survey-ieee-v2.pdf},
hal_id = {hal-01246370},
hal_version = {v1},
}
@article{Matas00,
author = {Matas, Jiri and Galambos, Charles and Kittler, Josef},
title = {Robust detection of lines using the progressive probabilistic hough transform},
@ -915,6 +957,11 @@
volume = {2},
publisher = {IEEE}
}
@misc{VandLec,
author = {Vandenberghe, Lieven},
title = {QR Factorization},
url = {http://www.seas.ucla.edu/~vandenbe/133A/lectures/qr.pdf}
}
@inproceedings{V03,
author = {Kwatra, Vivek and Sch{\"o}dl, Arno and Essa, Irfan and Turk, Greg and Bobick, Aaron},
title = {Graphcut textures: image and video synthesis using graph cuts},

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

@ -777,6 +777,65 @@ CV_EXPORTS_W int solveP3P( InputArray objectPoints, InputArray imagePoints,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
int flags );
/** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame
to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution.
@param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel,
where N is the number of points. vector\<Point3f\> can also be passed here.
@param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
where N is the number of points. vector\<Point2f\> can also be 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 rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec , brings points from
the model coordinate system to the camera coordinate system. Input values are used as an initial solution.
@param tvec Input/Output translation vector. Input values are used as an initial solution.
@param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm.
The function refines the object pose given at least 3 object points, their corresponding image
projections, an initial solution for the rotation and translation vector,
as well as the camera matrix and the distortion coefficients.
The function minimizes the projection error with respect to the rotation and the translation vectors, according
to a Levenberg-Marquardt iterative minimization @cite Madsen04 @cite Eade13 process.
*/
CV_EXPORTS_W void solvePnPRefineLM( InputArray objectPoints, InputArray imagePoints,
InputArray cameraMatrix, InputArray distCoeffs,
InputOutputArray rvec, InputOutputArray tvec,
TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON));
/** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame
to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution.
@param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel,
where N is the number of points. vector\<Point3f\> can also be passed here.
@param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
where N is the number of points. vector\<Point2f\> can also be 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 rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec , brings points from
the model coordinate system to the camera coordinate system. Input values are used as an initial solution.
@param tvec Input/Output translation vector. Input values are used as an initial solution.
@param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm.
@param VVSlambda Gain for the virtual visual servoing control law, equivalent to the \f$\alpha\f$
gain in the Gauss-Newton formulation.
The function refines the object pose given at least 3 object points, their corresponding image
projections, an initial solution for the rotation and translation vector,
as well as the camera matrix and the distortion coefficients.
The function minimizes the projection error with respect to the rotation and the translation vectors, using a
virtual visual servoing (VVS) @cite Chaumette06 @cite Marchand16 scheme.
*/
CV_EXPORTS_W void solvePnPRefineVVS( InputArray objectPoints, InputArray imagePoints,
InputArray cameraMatrix, InputArray distCoeffs,
InputOutputArray rvec, InputOutputArray tvec,
TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON),
double VVSlambda = 1);
/** @brief Finds an initial camera matrix from 3D-2D point correspondences.
@param objectPoints Vector of vectors of the calibration pattern points in the calibration pattern

9
modules/calib3d/src/levmarq.cpp
Unescape View File

@ -81,11 +81,11 @@ class LMSolverImpl CV_FINAL : public LMSolver
{
public:
LMSolverImpl() : maxIters(100) { init(); }
LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), maxIters(_maxIters) { init(); }
LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters) : cb(_cb), epsx(FLT_EPSILON), epsf(FLT_EPSILON), maxIters(_maxIters) { init(); }
LMSolverImpl(const Ptr<LMSolver::Callback>& _cb, int _maxIters, double _eps) : cb(_cb), epsx(_eps), epsf(_eps), maxIters(_maxIters) { init(); }
void init()
{
epsx = epsf = FLT_EPSILON;
printInterval = 0;
}
@ -214,4 +214,9 @@ Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters)
return makePtr<LMSolverImpl>(cb, maxIters);
}
Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)
{
return makePtr<LMSolverImpl>(cb, maxIters, eps);
}
}

1
modules/calib3d/src/precomp.hpp
Unescape View File

@ -95,6 +95,7 @@ public:
};
CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters);
CV_EXPORTS Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);
class CV_EXPORTS PointSetRegistrator : public Algorithm
{

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

@ -456,4 +456,271 @@ int solveP3P( InputArray _opoints, InputArray _ipoints,
return solutions;
}
class SolvePnPRefineLMCallback CV_FINAL : public LMSolver::Callback
{
public:
SolvePnPRefineLMCallback(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs)
{
objectPoints = _opoints.getMat();
imagePoints = _ipoints.getMat();
npoints = std::max(objectPoints.checkVector(3, CV_32F), objectPoints.checkVector(3, CV_64F));
imagePoints0 = imagePoints.reshape(1, npoints*2);
cameraMatrix = _cameraMatrix.getMat();
distCoeffs = _distCoeffs.getMat();
}
bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
{
Mat param = _param.getMat();
_err.create(npoints*2, 1, CV_64FC1);
if(_Jac.needed())
{
_Jac.create(npoints*2, param.rows, CV_64FC1);
}
Mat rvec = param(Rect(0, 0, 1, 3)), tvec = param(Rect(0, 3, 1, 3));
Mat J, projectedPts;
projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPts, _Jac.needed() ? J : noArray());
if (_Jac.needed())
{
Mat Jac = _Jac.getMat();
for (int i = 0; i < Jac.rows; i++)
{
for (int j = 0; j < Jac.cols; j++)
{
Jac.at<double>(i,j) = J.at<double>(i,j);
}
}
}
Mat err = _err.getMat();
projectedPts = projectedPts.reshape(1, npoints*2);
err = projectedPts - imagePoints0;
return true;
}
Mat objectPoints, imagePoints, imagePoints0;
Mat cameraMatrix, distCoeffs;
int npoints;
};
/**
* @brief Compute the Interaction matrix and the residuals for the current pose.
* @param objectPoints 3D object points.
* @param R Current estimated rotation matrix.
* @param tvec Current estimated translation vector.
* @param L Interaction matrix for a vector of point features.
* @param s Residuals.
*/
static void computeInteractionMatrixAndResiduals(const Mat& objectPoints, const Mat& R, const Mat& tvec,
Mat& L, Mat& s)
{
Mat objectPointsInCam;
int npoints = objectPoints.rows;
for (int i = 0; i < npoints; i++)
{
Mat curPt = objectPoints.row(i);
objectPointsInCam = R * curPt.t() + tvec;
double Zi = objectPointsInCam.at<double>(2,0);
double xi = objectPointsInCam.at<double>(0,0) / Zi;
double yi = objectPointsInCam.at<double>(1,0) / Zi;
s.at<double>(2*i,0) = xi;
s.at<double>(2*i+1,0) = yi;
L.at<double>(2*i,0) = -1 / Zi;
L.at<double>(2*i,1) = 0;
L.at<double>(2*i,2) = xi / Zi;
L.at<double>(2*i,3) = xi*yi;
L.at<double>(2*i,4) = -(1 + xi*xi);
L.at<double>(2*i,5) = yi;
L.at<double>(2*i+1,0) = 0;
L.at<double>(2*i+1,1) = -1 / Zi;
L.at<double>(2*i+1,2) = yi / Zi;
L.at<double>(2*i+1,3) = 1 + yi*yi;
L.at<double>(2*i+1,4) = -xi*yi;
L.at<double>(2*i+1,5) = -xi;
}
}
/**
* @brief The exponential map from se(3) to SE(3).
* @param twist A twist (v, w) represents the velocity of a rigid body as an angular velocity
* around an axis and a linear velocity along this axis.
* @param R1 Resultant rotation matrix from the twist.
* @param t1 Resultant translation vector from the twist.
*/
static void exponentialMapToSE3Inv(const Mat& twist, Mat& R1, Mat& t1)
{
//see Exponential Map in http://ethaneade.com/lie.pdf
/*
\begin{align*}
\boldsymbol{\delta} &= \left( \mathbf{u}, \boldsymbol{\omega} \right ) \in se(3) \\
\mathbf{u}, \boldsymbol{\omega} &\in \mathbb{R}^3 \\
\theta &= \sqrt{ \boldsymbol{\omega}^T \boldsymbol{\omega} } \\
A &= \frac{\sin \theta}{\theta} \\
B &= \frac{1 - \cos \theta}{\theta^2} \\
C &= \frac{1-A}{\theta^2} \\
\mathbf{R} &= \mathbf{I} + A \boldsymbol{\omega}_{\times} + B \boldsymbol{\omega}_{\times}^2 \\
\mathbf{V} &= \mathbf{I} + B \boldsymbol{\omega}_{\times} + C \boldsymbol{\omega}_{\times}^2 \\
\exp \begin{pmatrix}
\mathbf{u} \\
\boldsymbol{\omega}
\end{pmatrix} &=
\left(
\begin{array}{c|c}
\mathbf{R} & \mathbf{V} \mathbf{u} \\ \hline
\mathbf{0} & 1
\end{array}
\right )
\end{align*}
*/
double vx = twist.at<double>(0,0);
double vy = twist.at<double>(1,0);
double vz = twist.at<double>(2,0);
double wx = twist.at<double>(3,0);
double wy = twist.at<double>(4,0);
double wz = twist.at<double>(5,0);
Matx31d rvec(wx, wy, wz);
Mat R;
Rodrigues(rvec, R);
double theta = sqrt(wx*wx + wy*wy + wz*wz);
double sinc = std::fabs(theta) < 1e-8 ? 1 : sin(theta) / theta;
double mcosc = (std::fabs(theta) < 1e-8) ? 0.5 : (1-cos(theta)) / (theta*theta);
double msinc = (std::abs(theta) < 1e-8) ? (1/6.0) : (1-sinc) / (theta*theta);
Matx31d dt;
dt(0) = vx*(sinc + wx*wx*msinc) + vy*(wx*wy*msinc - wz*mcosc) + vz*(wx*wz*msinc + wy*mcosc);
dt(1) = vx*(wx*wy*msinc + wz*mcosc) + vy*(sinc + wy*wy*msinc) + vz*(wy*wz*msinc - wx*mcosc);
dt(2) = vx*(wx*wz*msinc - wy*mcosc) + vy*(wy*wz*msinc + wx*mcosc) + vz*(sinc + wz*wz*msinc);
R1 = R.t();
t1 = -R1 * dt;
}
enum SolvePnPRefineMethod {
SOLVEPNP_REFINE_LM = 0,
SOLVEPNP_REFINE_VVS = 1
};
static void solvePnPRefine(InputArray _objectPoints, InputArray _imagePoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
InputOutputArray _rvec, InputOutputArray _tvec,
SolvePnPRefineMethod _flags,
TermCriteria _criteria=TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON),
double _vvslambda=1)
{
CV_INSTRUMENT_REGION();
Mat opoints_ = _objectPoints.getMat(), ipoints_ = _imagePoints.getMat();
Mat opoints, ipoints;
opoints_.convertTo(opoints, CV_64F);
ipoints_.convertTo(ipoints, CV_64F);
int npoints = opoints.checkVector(3, CV_64F);
CV_Assert( npoints >= 3 && npoints == ipoints.checkVector(2, CV_64F) );
CV_Assert( !_rvec.empty() && !_tvec.empty() );
int rtype = _rvec.type(), ttype = _tvec.type();
Size rsize = _rvec.size(), tsize = _tvec.size();
CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
(ttype == CV_32FC1 || ttype == CV_64FC1) );
CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
(tsize == Size(1, 3) || tsize == Size(3, 1)) );
Mat cameraMatrix0 = _cameraMatrix.getMat();
Mat distCoeffs0 = _distCoeffs.getMat();
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
Mat distCoeffs = Mat_<double>(distCoeffs0);
if (_flags == SOLVEPNP_REFINE_LM)
{
Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
Mat rvec, tvec;
rvec0.convertTo(rvec, CV_64F);
tvec0.convertTo(tvec, CV_64F);
Mat params(6, 1, CV_64FC1);
for (int i = 0; i < 3; i++)
{
params.at<double>(i,0) = rvec.at<double>(i,0);
params.at<double>(i+3,0) = tvec.at<double>(i,0);
}
createLMSolver(makePtr<SolvePnPRefineLMCallback>(opoints, ipoints, cameraMatrix, distCoeffs), _criteria.maxCount, _criteria.epsilon)->run(params);
params.rowRange(0, 3).convertTo(rvec0, rvec0.depth());
params.rowRange(3, 6).convertTo(tvec0, tvec0.depth());
}
else if (_flags == SOLVEPNP_REFINE_VVS)
{
Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
Mat rvec, tvec;
rvec0.convertTo(rvec, CV_64F);
tvec0.convertTo(tvec, CV_64F);
vector<Point2d> ipoints_normalized;
undistortPoints(ipoints, ipoints_normalized, cameraMatrix, distCoeffs);
Mat sd = Mat(ipoints_normalized).reshape(1, npoints*2);
Mat objectPoints0 = opoints.reshape(1, npoints);
Mat imagePoints0 = ipoints.reshape(1, npoints*2);
Mat L(npoints*2, 6, CV_64FC1), s(npoints*2, 1, CV_64FC1);
double residuals_1 = std::numeric_limits<double>::max(), residuals = 0;
Mat err;
Mat R;
Rodrigues(rvec, R);
for (int iter = 0; iter < _criteria.maxCount; iter++)
{
computeInteractionMatrixAndResiduals(objectPoints0, R, tvec, L, s);
err = s - sd;
Mat Lp = L.inv(cv::DECOMP_SVD);
Mat dq = -_vvslambda * Lp * err;
Mat R1, t1;
exponentialMapToSE3Inv(dq, R1, t1);
R = R1 * R;
tvec = R1 * tvec + t1;
residuals_1 = residuals;
Mat res = err.t()*err;
residuals = res.at<double>(0,0);
if (std::fabs(residuals - residuals_1) < _criteria.epsilon)
break;
}
Rodrigues(R, rvec);
rvec.convertTo(rvec0, rvec0.depth());
tvec.convertTo(tvec0, tvec0.depth());
}
}
void solvePnPRefineLM(InputArray _objectPoints, InputArray _imagePoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
InputOutputArray _rvec, InputOutputArray _tvec,
TermCriteria _criteria)
{
CV_INSTRUMENT_REGION();
solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_LM, _criteria);
}
void solvePnPRefineVVS(InputArray _objectPoints, InputArray _imagePoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
InputOutputArray _rvec, InputOutputArray _tvec,
TermCriteria _criteria, double _VVSlambda)
{
CV_INSTRUMENT_REGION();
solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_VVS, _criteria, _VVSlambda);
}
}

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

@ -589,4 +589,330 @@ TEST(Calib3d_SolvePnP, iterativeInitialGuess3pts)
}
}
TEST(Calib3d_SolvePnP, refine3pts)
{
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<double>(3,1) << 0.2, -0.1, 0.6);
Mat tvec_est = (Mat_<double>(3,1) << 0.05, -0.05, 1.0);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<double>(3,1) << 0.2, -0.1, 0.6);
Mat tvec_est = (Mat_<double>(3,1) << 0.05, -0.05, 1.0);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
{
Matx33f intrinsics(605.4f, 0.0f, 317.35f,
0.0f, 601.2f, 242.63f,
0.0f, 0.0f, 1.0f);
float L = 0.1f;
vector<Point3f> p3d;
p3d.push_back(Point3f(-L, -L, 0.0f));
p3d.push_back(Point3f(L, -L, 0.0f));
p3d.push_back(Point3f(L, L, 0.0f));
Mat rvec_ground_truth = (Mat_<float>(3,1) << -0.75f, 0.4f, 0.34f);
Mat tvec_ground_truth = (Mat_<float>(3,1) << -0.15f, 0.35f, 1.58f);
vector<Point2f> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<float>(3,1) << -0.5f, 0.2f, 0.2f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.2f, 1.0f);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<float>(3,1) << -0.5f, 0.2f, 0.2f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.2f, 1.0f);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
}
TEST(Calib3d_SolvePnP, refine)
{
//double
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
p3d.push_back(Point3d(-L, L, L/2));
p3d.push_back(Point3d(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<double>(3,1) << 0.1, -0.1, 0.1);
Mat tvec_est = (Mat_<double>(3,1) << 0.0, -0.5, 1.0);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "\nmethod: Levenberg-Marquardt (C API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<double>(3,1) << 0.1, -0.1, 0.1);
Mat tvec_est = (Mat_<double>(3,1) << 0.0, -0.5, 1.0);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<double>(3,1) << 0.1, -0.1, 0.1);
Mat tvec_est = (Mat_<double>(3,1) << 0.0, -0.5, 1.0);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
//float
{
Matx33f intrinsics(605.4f, 0.0f, 317.35f,
0.0f, 601.2f, 242.63f,
0.0f, 0.0f, 1.0f);
float L = 0.1f;
vector<Point3f> p3d;
p3d.push_back(Point3f(-L, -L, 0.0f));
p3d.push_back(Point3f(L, -L, 0.0f));
p3d.push_back(Point3f(L, L, 0.0f));
p3d.push_back(Point3f(-L, L, L/2));
p3d.push_back(Point3f(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<float>(3,1) << -0.75f, 0.4f, 0.34f);
Mat tvec_ground_truth = (Mat_<float>(3,1) << -0.15f, 0.35f, 1.58f);
vector<Point2f> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<float>(3,1) << -0.1f, 0.1f, 0.1f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.0f, 1.0f);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "\nmethod: Levenberg-Marquardt (C API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<float>(3,1) << -0.1f, 0.1f, 0.1f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.0f, 1.0f);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<float>(3,1) << -0.1f, 0.1f, 0.1f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.0f, 1.0f);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
//refine after solvePnP
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
p3d.push_back(Point3d(-L, L, L/2));
p3d.push_back(Point3d(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
//add small Gaussian noise
RNG& rng = theRNG();
for (size_t i = 0; i < p2d.size(); i++)
{
p2d[i].x += rng.gaussian(5e-2);
p2d[i].y += rng.gaussian(5e-2);
}
Mat rvec_est, tvec_est;
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, false, SOLVEPNP_EPNP);
{
Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone();
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine, true, SOLVEPNP_ITERATIVE);
cout << "\nmethod: Levenberg-Marquardt (C API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est (EPnP): " << rvec_est.t() << std::endl;
cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est (EPnP): " << tvec_est.t() << std::endl;
cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3);
EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF));
EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF));
}
{
Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone();
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine);
cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3);
EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF));
EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF));
}
{
Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone();
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3);
EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF));
EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF));
}
}
}
}} // namespace

Write Preview
Loading…
Cancel
Save