mirror of
https://github.com/opencv/opencv.git
synced 2024-11-24 03:00:14 +08:00
Add solvePnPRefineLM to refine a pose according to a Levenberg-Marquardt iterative minimization process. Add solvePnPRefineVVS to refine a pose using a virtual visual servoing scheme.
This commit is contained in:
catree
parent
d17699363c
commit
dac31e84fb
@ -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},
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user