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},
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},
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},
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},
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},
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
{
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;
}
return makePtr<LMSolverImpl>(cb, maxIters);
}
+Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)
+{
+ return makePtr<LMSolverImpl>(cb, maxIters, eps);
+}
+
}
};
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
{
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);
+}
+
}
}
}
+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
projects / platform / upstream / opencv.git / commitdiff