From 33cb9c5ca54b99c934cafacc960548f6b9cc4060 Mon Sep 17 00:00:00 2001 From: catree Date: Thu, 18 Apr 2019 19:07:19 +0200 Subject: [PATCH] Add SOLVEPNP_IPPE for planar pose estimation. Add solvePnPGeneric function that returns all the pose solutions and the reprojection errors. --- doc/opencv.bib | 16 +- modules/calib3d/include/opencv2/calib3d.hpp | 290 +++- modules/calib3d/src/ap3p.cpp | 104 +- modules/calib3d/src/ap3p.h | 18 +- modules/calib3d/src/ippe.cpp | 1100 +++++++++++++++ modules/calib3d/src/ippe.hpp | 259 ++++ modules/calib3d/src/p3p.cpp | 67 +- modules/calib3d/src/p3p.h | 12 +- modules/calib3d/src/solvepnp.cpp | 567 ++++++-- modules/calib3d/test/test_solvepnp_ransac.cpp | 1230 +++++++++++++++-- 10 files changed, 3291 insertions(+), 372 deletions(-) create mode 100644 modules/calib3d/src/ippe.cpp create mode 100644 modules/calib3d/src/ippe.hpp diff --git a/doc/opencv.bib b/doc/opencv.bib index fd1b60dfd1..e861e5b756 100644 --- a/doc/opencv.bib +++ b/doc/opencv.bib @@ -209,7 +209,21 @@ hal_id = {inria-00350283}, hal_version = {v1}, } - +@article{Collins14 + year = {2014}, + issn = {0920-5691}, + journal = {International Journal of Computer Vision}, + volume = {109}, + number = {3}, + doi = {10.1007/s11263-014-0725-5}, + title = {Infinitesimal Plane-Based Pose Estimation}, + url = {http://dx.doi.org/10.1007/s11263-014-0725-5}, + publisher = {Springer US}, + keywords = {Plane; Pose; SfM; PnP; Homography}, + author = {Collins, Toby and Bartoli, Adrien}, + pages = {252-286}, + language = {English} +} @article{Daniilidis98, author = {Konstantinos Daniilidis}, title = {Hand-Eye Calibration Using Dual Quaternions}, diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 16ab3342b7..c3139572b8 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -231,13 +231,25 @@ enum { LMEDS = 4, //!< least-median of squares algorithm RHO = 16 //!< RHO algorithm }; -enum { SOLVEPNP_ITERATIVE = 0, - SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp - SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete - SOLVEPNP_DLS = 3, //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct - SOLVEPNP_UPNP = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive - SOLVEPNP_AP3P = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17 - SOLVEPNP_MAX_COUNT //!< Used for count +enum SolvePnPMethod { + SOLVEPNP_ITERATIVE = 0, + SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp + SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete + SOLVEPNP_DLS = 3, //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct + SOLVEPNP_UPNP = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive + SOLVEPNP_AP3P = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17 + SOLVEPNP_IPPE = 6, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n + //!< Object points must be coplanar. + SOLVEPNP_IPPE_SQUARE = 7, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n + //!< This is a special case suitable for marker pose estimation.\n + //!< 4 coplanar object points must be defined in the following order: + //!< - point 0: [-squareLength / 2, squareLength / 2, 0] + //!< - point 1: [ squareLength / 2, squareLength / 2, 0] + //!< - point 2: [ squareLength / 2, -squareLength / 2, 0] + //!< - point 3: [-squareLength / 2, -squareLength / 2, 0] +#ifndef CV_DOXYGEN + SOLVEPNP_MAX_COUNT //!< Used for count +#endif }; enum { CALIB_CB_ADAPTIVE_THRESH = 1, @@ -540,6 +552,17 @@ Check @ref tutorial_homography "the corresponding tutorial" for more details */ /** @brief Finds an object pose from 3D-2D point correspondences. +This function returns the rotation and the translation vectors that transform a 3D point expressed in the object +coordinate frame to the camera coordinate frame, using different methods: +- P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): need 4 input points to return a unique solution. +- @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar. +- @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation. +Number of input points must be 4. Object points must be defined in the following order: + - point 0: [-squareLength / 2, squareLength / 2, 0] + - point 1: [ squareLength / 2, squareLength / 2, 0] + - point 2: [ squareLength / 2, -squareLength / 2, 0] + - point 3: [-squareLength / 2, -squareLength / 2, 0] +- for all the other flags, number of input points must be >= 4 and object points can be in any configuration. @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\ can be also passed here. @@ -550,14 +573,14 @@ where N is the number of points. vector\ can be also passed here. \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 Output rotation vector (see @ref Rodrigues ) that, together with tvec , brings points from +@param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from the model coordinate system to the camera coordinate system. @param tvec Output translation vector. @param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses the provided rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them. @param flags Method for solving a PnP problem: -- **SOLVEPNP_ITERATIVE** Iterative method is based on Levenberg-Marquardt optimization. In +- **SOLVEPNP_ITERATIVE** Iterative method is based on a Levenberg-Marquardt optimization. In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances between the observed projections imagePoints and the projected (using projectPoints ) objectPoints . @@ -567,18 +590,24 @@ In this case the function requires exactly four object and image points. - **SOLVEPNP_AP3P** Method is based on the paper of T. Ke, S. Roumeliotis "An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17). In this case the function requires exactly four object and image points. -- **SOLVEPNP_EPNP** Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the +- **SOLVEPNP_EPNP** Method has been introduced by F. Moreno-Noguer, V. Lepetit and P. Fua in the paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" (@cite lepetit2009epnp). -- **SOLVEPNP_DLS** Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. +- **SOLVEPNP_DLS** Method is based on the paper of J. Hesch and S. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP" (@cite hesch2011direct). -- **SOLVEPNP_UPNP** Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, -F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length +- **SOLVEPNP_UPNP** Method is based on the paper of A. Penate-Sanchez, J. Andrade-Cetto, +F. Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$ assuming that both have the same value. Then the cameraMatrix is updated with the estimated focal length. -- **SOLVEPNP_AP3P** Method is based on the paper of Tong Ke and Stergios I. Roumeliotis. -"An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17). In this case the -function requires exactly four object and image points. +- **SOLVEPNP_IPPE** Method is based on the paper of T. Collins and A. Bartoli. +"Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method requires coplanar object points. +- **SOLVEPNP_IPPE_SQUARE** Method is based on the paper of Toby Collins and Adrien Bartoli. +"Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method is suitable for marker pose estimation. +It requires 4 coplanar object points defined in the following order: + - point 0: [-squareLength / 2, squareLength / 2, 0] + - point 1: [ squareLength / 2, squareLength / 2, 0] + - point 2: [ squareLength / 2, -squareLength / 2, 0] + - point 3: [-squareLength / 2, -squareLength / 2, 0] The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients, see the figure below @@ -634,7 +663,7 @@ using the perspective projection model \f$ \Pi \f$ and the camera intrinsic para \end{align*} \f] -The estimated pose is thus the rotation (`rvec`) and the translation (`tvec`) vectors that allow to transform +The estimated pose is thus the rotation (`rvec`) and the translation (`tvec`) vectors that allow transforming a 3D point expressed in the world frame into the camera frame: \f[ @@ -695,6 +724,13 @@ a 3D point expressed in the world frame into the camera frame: - With **SOLVEPNP_ITERATIVE** method and `useExtrinsicGuess=true`, the minimum number of points is 3 (3 points are sufficient to compute a pose but there are up to 4 solutions). The initial solution should be close to the global solution to converge. + - With **SOLVEPNP_IPPE** input points must be >= 4 and object points must be coplanar. + - With **SOLVEPNP_IPPE_SQUARE** this is a special case suitable for marker pose estimation. + Number of input points must be 4. Object points must be defined in the following order: + - point 0: [-squareLength / 2, squareLength / 2, 0] + - point 1: [ squareLength / 2, squareLength / 2, 0] + - point 2: [ squareLength / 2, -squareLength / 2, 0] + - point 3: [-squareLength / 2, -squareLength / 2, 0] */ CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, @@ -712,10 +748,10 @@ where N is the number of points. vector\ can be also passed here. \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 Output rotation vector (see Rodrigues ) that, together with tvec , brings points from +@param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from the model coordinate system to the camera coordinate system. @param tvec Output translation vector. -@param useExtrinsicGuess Parameter used for SOLVEPNP_ITERATIVE. If true (1), the function uses +@param useExtrinsicGuess Parameter used for @ref SOLVEPNP_ITERATIVE. If true (1), the function uses the provided rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them. @param iterationsCount Number of iterations. @@ -724,12 +760,12 @@ is the maximum allowed distance between the observed and computed point projecti an inlier. @param confidence The probability that the algorithm produces a useful result. @param inliers Output vector that contains indices of inliers in objectPoints and imagePoints . -@param flags Method for solving a PnP problem (see solvePnP ). +@param flags Method for solving a PnP problem (see @ref solvePnP ). The function estimates an object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients. This function finds such a pose that minimizes reprojection error, that is, the sum of squared distances between the observed -projections imagePoints and the projected (using projectPoints ) objectPoints. The use of RANSAC +projections imagePoints and the projected (using @ref projectPoints ) objectPoints. The use of RANSAC makes the function resistant to outliers. @note @@ -749,6 +785,7 @@ CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoint bool useExtrinsicGuess = false, int iterationsCount = 100, float reprojectionError = 8.0, double confidence = 0.99, OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE ); + /** @brief Finds an object pose from 3 3D-2D point correspondences. @param objectPoints Array of object points in the object coordinate space, 3x3 1-channel or @@ -760,17 +797,20 @@ CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoint \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 rvecs Output rotation vectors (see Rodrigues ) that, together with tvecs , brings points from +@param rvecs Output rotation vectors (see @ref Rodrigues ) that, together with tvecs, brings points from the model coordinate system to the camera coordinate system. A P3P problem has up to 4 solutions. @param tvecs Output translation vectors. @param flags Method for solving a P3P problem: - **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang "Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete). -- **SOLVEPNP_AP3P** Method is based on the paper of Tong Ke and Stergios I. Roumeliotis. +- **SOLVEPNP_AP3P** Method is based on the paper of T. Ke and S. Roumeliotis. "An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17). The function estimates the object pose given 3 object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients. + +@note +The solutions are sorted by reprojection errors (lowest to highest). */ CV_EXPORTS_W int solveP3P( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, @@ -789,7 +829,7 @@ where N is the number of points. vector\ can also be passed here. \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 +@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. @@ -817,12 +857,12 @@ where N is the number of points. vector\ can also be passed here. \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 +@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. +gain in the Damped 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, @@ -836,6 +876,202 @@ CV_EXPORTS_W void solvePnPRefineVVS( InputArray objectPoints, InputArray imagePo TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON), double VVSlambda = 1); +/** @brief Finds an object pose from 3D-2D point correspondences. +This function returns a list of all the possible solutions (a solution is a +couple), depending on the number of input points and the chosen method: +- P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): 3 or 4 input points. Number of returned solutions can be between 0 and 4 with 3 input points. +- @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar. Returns 2 solutions. +- @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation. +Number of input points must be 4 and 2 solutions are returned. Object points must be defined in the following order: + - point 0: [-squareLength / 2, squareLength / 2, 0] + - point 1: [ squareLength / 2, squareLength / 2, 0] + - point 2: [ squareLength / 2, -squareLength / 2, 0] + - point 3: [-squareLength / 2, -squareLength / 2, 0] +- for all the other flags, number of input points must be >= 4 and object points can be in any configuration. +Only 1 solution is returned. + +@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\ can be also 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\ can be also 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 rvecs Vector of output rotation vectors (see @ref Rodrigues ) that, together with tvecs, brings points from +the model coordinate system to the camera coordinate system. +@param tvecs Vector of output translation vectors. +@param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses +the provided rvec and tvec values as initial approximations of the rotation and translation +vectors, respectively, and further optimizes them. +@param flags Method for solving a PnP problem: +- **SOLVEPNP_ITERATIVE** Iterative method is based on a Levenberg-Marquardt optimization. In +this case the function finds such a pose that minimizes reprojection error, that is the sum +of squared distances between the observed projections imagePoints and the projected (using +projectPoints ) objectPoints . +- **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang +"Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete). +In this case the function requires exactly four object and image points. +- **SOLVEPNP_AP3P** Method is based on the paper of T. Ke, S. Roumeliotis +"An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17). +In this case the function requires exactly four object and image points. +- **SOLVEPNP_EPNP** Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the +paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" (@cite lepetit2009epnp). +- **SOLVEPNP_DLS** Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. +"A Direct Least-Squares (DLS) Method for PnP" (@cite hesch2011direct). +- **SOLVEPNP_UPNP** Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, +F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length +Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$ +assuming that both have the same value. Then the cameraMatrix is updated with the estimated +focal length. +- **SOLVEPNP_IPPE** Method is based on the paper of T. Collins and A. Bartoli. +"Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method requires coplanar object points. +- **SOLVEPNP_IPPE_SQUARE** Method is based on the paper of Toby Collins and Adrien Bartoli. +"Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method is suitable for marker pose estimation. +It requires 4 coplanar object points defined in the following order: + - point 0: [-squareLength / 2, squareLength / 2, 0] + - point 1: [ squareLength / 2, squareLength / 2, 0] + - point 2: [ squareLength / 2, -squareLength / 2, 0] + - point 3: [-squareLength / 2, -squareLength / 2, 0] +@param rvec Rotation vector used to initialize an iterative PnP refinement algorithm, when flag is SOLVEPNP_ITERATIVE +and useExtrinsicGuess is set to true. +@param tvec Translation vector used to initialize an iterative PnP refinement algorithm, when flag is SOLVEPNP_ITERATIVE +and useExtrinsicGuess is set to true. +@param reprojectionError Optional vector of reprojection error, that is the RMS error +(\f$ \text{RMSE} = \sqrt{\frac{\sum_{i}^{N} \left ( \hat{y_i} - y_i \right )^2}{N}} \f$) between the input image points +and the 3D object points projected with the estimated pose. + +The function estimates the object pose given a set of object points, their corresponding image +projections, as well as the camera matrix and the distortion coefficients, see the figure below +(more precisely, the X-axis of the camera frame is pointing to the right, the Y-axis downward +and the Z-axis forward). + +![](pnp.jpg) + +Points expressed in the world frame \f$ \bf{X}_w \f$ are projected into the image plane \f$ \left[ u, v \right] \f$ +using the perspective projection model \f$ \Pi \f$ and the camera intrinsic parameters matrix \f$ \bf{A} \f$: + +\f[ + \begin{align*} + \begin{bmatrix} + u \\ + v \\ + 1 + \end{bmatrix} &= + \bf{A} \hspace{0.1em} \Pi \hspace{0.2em} ^{c}\bf{M}_w + \begin{bmatrix} + X_{w} \\ + Y_{w} \\ + Z_{w} \\ + 1 + \end{bmatrix} \\ + \begin{bmatrix} + u \\ + v \\ + 1 + \end{bmatrix} &= + \begin{bmatrix} + f_x & 0 & c_x \\ + 0 & f_y & c_y \\ + 0 & 0 & 1 + \end{bmatrix} + \begin{bmatrix} + 1 & 0 & 0 & 0 \\ + 0 & 1 & 0 & 0 \\ + 0 & 0 & 1 & 0 + \end{bmatrix} + \begin{bmatrix} + r_{11} & r_{12} & r_{13} & t_x \\ + r_{21} & r_{22} & r_{23} & t_y \\ + r_{31} & r_{32} & r_{33} & t_z \\ + 0 & 0 & 0 & 1 + \end{bmatrix} + \begin{bmatrix} + X_{w} \\ + Y_{w} \\ + Z_{w} \\ + 1 + \end{bmatrix} + \end{align*} +\f] + +The estimated pose is thus the rotation (`rvec`) and the translation (`tvec`) vectors that allow transforming +a 3D point expressed in the world frame into the camera frame: + +\f[ + \begin{align*} + \begin{bmatrix} + X_c \\ + Y_c \\ + Z_c \\ + 1 + \end{bmatrix} &= + \hspace{0.2em} ^{c}\bf{M}_w + \begin{bmatrix} + X_{w} \\ + Y_{w} \\ + Z_{w} \\ + 1 + \end{bmatrix} \\ + \begin{bmatrix} + X_c \\ + Y_c \\ + Z_c \\ + 1 + \end{bmatrix} &= + \begin{bmatrix} + r_{11} & r_{12} & r_{13} & t_x \\ + r_{21} & r_{22} & r_{23} & t_y \\ + r_{31} & r_{32} & r_{33} & t_z \\ + 0 & 0 & 0 & 1 + \end{bmatrix} + \begin{bmatrix} + X_{w} \\ + Y_{w} \\ + Z_{w} \\ + 1 + \end{bmatrix} + \end{align*} +\f] + +@note + - An example of how to use solvePnP for planar augmented reality can be found at + opencv_source_code/samples/python/plane_ar.py + - If you are using Python: + - Numpy array slices won't work as input because solvePnP requires contiguous + arrays (enforced by the assertion using cv::Mat::checkVector() around line 55 of + modules/calib3d/src/solvepnp.cpp version 2.4.9) + - The P3P algorithm requires image points to be in an array of shape (N,1,2) due + to its calling of cv::undistortPoints (around line 75 of modules/calib3d/src/solvepnp.cpp version 2.4.9) + which requires 2-channel information. + - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of + it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints = + np.ascontiguousarray(D[:,:2]).reshape((N,1,2)) + - The methods **SOLVEPNP_DLS** and **SOLVEPNP_UPNP** cannot be used as the current implementations are + unstable and sometimes give completely wrong results. If you pass one of these two + flags, **SOLVEPNP_EPNP** method will be used instead. + - The minimum number of points is 4 in the general case. In the case of **SOLVEPNP_P3P** and **SOLVEPNP_AP3P** + methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions + of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error). + - With **SOLVEPNP_ITERATIVE** method and `useExtrinsicGuess=true`, the minimum number of points is 3 (3 points + are sufficient to compute a pose but there are up to 4 solutions). The initial solution should be close to the + global solution to converge. + - With **SOLVEPNP_IPPE** input points must be >= 4 and object points must be coplanar. + - With **SOLVEPNP_IPPE_SQUARE** this is a special case suitable for marker pose estimation. + Number of input points must be 4. Object points must be defined in the following order: + - point 0: [-squareLength / 2, squareLength / 2, 0] + - point 1: [ squareLength / 2, squareLength / 2, 0] + - point 2: [ squareLength / 2, -squareLength / 2, 0] + - point 3: [-squareLength / 2, -squareLength / 2, 0] + */ +CV_EXPORTS_W int solvePnPGeneric( InputArray objectPoints, InputArray imagePoints, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, + bool useExtrinsicGuess = false, SolvePnPMethod flags = SOLVEPNP_ITERATIVE, + InputArray rvec = noArray(), InputArray tvec = noArray(), + OutputArray reprojectionError = noArray() ); + /** @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 @@ -933,7 +1169,7 @@ CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSiz @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 empty, the zero distortion coefficients are assumed. -@param rvec Rotation vector (see @ref Rodrigues ) that, together with tvec , brings points from +@param rvec Rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from the model coordinate system to the camera coordinate system. @param tvec Translation vector. @param length Length of the painted axes in the same unit than tvec (usually in meters). diff --git a/modules/calib3d/src/ap3p.cpp b/modules/calib3d/src/ap3p.cpp index 7b86834db8..11171f81a6 100644 --- a/modules/calib3d/src/ap3p.cpp +++ b/modules/calib3d/src/ap3p.cpp @@ -1,3 +1,4 @@ +#include "precomp.hpp" #include "ap3p.h" #include @@ -154,10 +155,11 @@ ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) { // worldPoints: The positions of the 3 feature points stored as column vectors // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame -int ap3p::computePoses(const double featureVectors[3][3], - const double worldPoints[3][3], +int ap3p::computePoses(const double featureVectors[3][4], + const double worldPoints[3][4], double solutionsR[4][3][3], - double solutionsT[4][3]) { + double solutionsT[4][3], + bool p4p) { //world point vectors double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]}; @@ -246,6 +248,13 @@ int ap3p::computePoses(const double featureVectors[3][3], double b3p[3]; vect_scale((delta / k3b3), b3, b3p); + double X3 = worldPoints[0][3]; + double Y3 = worldPoints[1][3]; + double Z3 = worldPoints[2][3]; + double mu3 = featureVectors[0][3]; + double mv3 = featureVectors[1][3]; + double reproj_errors[4]; + int nb_solutions = 0; for (int i = 0; i < 4; ++i) { double ctheta1p = s[i]; @@ -290,9 +299,29 @@ int ap3p::computePoses(const double featureVectors[3][3], solutionsR[nb_solutions][1][2] = R[2][1]; solutionsR[nb_solutions][2][2] = R[2][2]; + if (p4p) { + double X3p = solutionsR[nb_solutions][0][0] * X3 + solutionsR[nb_solutions][0][1] * Y3 + solutionsR[nb_solutions][0][2] * Z3 + solutionsT[nb_solutions][0]; + double Y3p = solutionsR[nb_solutions][1][0] * X3 + solutionsR[nb_solutions][1][1] * Y3 + solutionsR[nb_solutions][1][2] * Z3 + solutionsT[nb_solutions][1]; + double Z3p = solutionsR[nb_solutions][2][0] * X3 + solutionsR[nb_solutions][2][1] * Y3 + solutionsR[nb_solutions][2][2] * Z3 + solutionsT[nb_solutions][2]; + double mu3p = X3p / Z3p; + double mv3p = Y3p / Z3p; + reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3); + } + nb_solutions++; } + //sort the solutions + if (p4p) { + for (int i = 1; i < nb_solutions; i++) { + for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) { + std::swap(reproj_errors[j], reproj_errors[j-1]); + std::swap(solutionsR[j], solutionsR[j-1]); + std::swap(solutionsT[j], solutionsT[j-1]); + } + } + } + return nb_solutions; } @@ -311,9 +340,10 @@ bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Ma else extract_points(opoints, ipoints, points); - bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5], - points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], - points[14], + bool result = solve(rotation_matrix, translation, + points[0], points[1], points[2], points[3], points[4], + points[5], points[6], points[7], points[8], points[9], + points[10], points[11], points[12], points[13],points[14], points[15], points[16], points[17], points[18], points[19]); cv::Mat(3, 1, CV_64F, translation).copyTo(tvec); cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R); @@ -335,10 +365,13 @@ int ap3p::solve(std::vector &Rs, std::vector &tvecs, const cv: else extract_points(opoints, ipoints, points); + const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4; int solutions = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5], points[6], points[7], points[8], points[9], - points[10], points[11], points[12], points[13], points[14]); + points[10], points[11], points[12], points[13], points[14], + points[15], points[16], points[17], points[18], points[19], + p4p); for (int i = 0; i < solutions; i++) { cv::Mat R, tvec; @@ -353,42 +386,33 @@ int ap3p::solve(std::vector &Rs, std::vector &tvecs, const cv: } bool -ap3p::solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1, - double mv1, - double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2, double mu3, - double mv3, double X3, double Y3, double Z3) { +ap3p::solve(double R[3][3], double t[3], + double mu0, double mv0, double X0, double Y0, double Z0, + double mu1, double mv1, double X1, double Y1, double Z1, + double mu2, double mv2, double X2, double Y2, double Z2, + double mu3, double mv3, double X3, double Y3, double Z3) { double Rs[4][3][3], ts[4][3]; - int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2); + const bool p4p = true; + int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p); if (n == 0) return false; - int ns = 0; - double min_reproj = 0; - for (int i = 0; i < n; i++) { - double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0]; - double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1]; - double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2]; - double mu3p = cx + fx * X3p / Z3p; - double mv3p = cy + fy * Y3p / Z3p; - double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3); - if (i == 0 || min_reproj > reproj) { - ns = i; - min_reproj = reproj; - } - } - for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) - R[i][j] = Rs[ns][i][j]; - t[i] = ts[ns][i]; + R[i][j] = Rs[0][i][j]; + t[i] = ts[0][i]; } return true; } -int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1, - double mv1, double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2) { +int ap3p::solve(double R[4][3][3], double t[4][3], + double mu0, double mv0, double X0, double Y0, double Z0, + double mu1, double mv1, double X1, double Y1, double Z1, + double mu2, double mv2, double X2, double Y2, double Z2, + double mu3, double mv3, double X3, double Y3, double Z3, + bool p4p) { double mk0, mk1, mk2; double norm; @@ -413,13 +437,17 @@ int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, doubl mu2 *= mk2; mv2 *= mk2; - double featureVectors[3][3] = {{mu0, mu1, mu2}, - {mv0, mv1, mv2}, - {mk0, mk1, mk2}}; - double worldPoints[3][3] = {{X0, X1, X2}, - {Y0, Y1, Y2}, - {Z0, Z1, Z2}}; + mu3 = inv_fx * mu3 - cx_fx; + mv3 = inv_fy * mv3 - cy_fy; + double mk3 = 1; //not used + + double featureVectors[3][4] = {{mu0, mu1, mu2, mu3}, + {mv0, mv1, mv2, mv3}, + {mk0, mk1, mk2, mk3}}; + double worldPoints[3][4] = {{X0, X1, X2, X3}, + {Y0, Y1, Y2, Y3}, + {Z0, Z1, Z2, Z3}}; - return computePoses(featureVectors, worldPoints, R, t); + return computePoses(featureVectors, worldPoints, R, t, p4p); } } diff --git a/modules/calib3d/src/ap3p.h b/modules/calib3d/src/ap3p.h index df44198115..c044c6fd32 100644 --- a/modules/calib3d/src/ap3p.h +++ b/modules/calib3d/src/ap3p.h @@ -1,7 +1,7 @@ #ifndef P3P_P3P_H #define P3P_P3P_H -#include "precomp.hpp" +#include namespace cv { class ap3p { @@ -18,7 +18,7 @@ private: void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector &points) { points.clear(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - points.resize(5*npoints); + points.resize(5*4); //resize vector to fit for p4p case for (int i = 0; i < npoints; i++) { points[i * 5] = ipoints.at(i).x * fx + cx; points[i * 5 + 1] = ipoints.at(i).y * fy + cy; @@ -26,6 +26,12 @@ private: points[i * 5 + 3] = opoints.at(i).y; points[i * 5 + 4] = opoints.at(i).z; } + //Fill vectors with unused values for p3p case + for (int i = npoints; i < 4; i++) { + for (int j = 0; j < 5; j++) { + points[i * 5 + j] = 0; + } + } } void init_inverse_parameters(); @@ -45,7 +51,9 @@ public: int solve(double R[4][3][3], double t[4][3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1, double mv1, double X1, double Y1, double Z1, - double mu2, double mv2, double X2, double Y2, double Z2); + double mu2, double mv2, double X2, double Y2, double Z2, + double mu3, double mv3, double X3, double Y3, double Z3, + bool p4p); bool solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, @@ -59,8 +67,8 @@ public: // worldPoints: Positions of the 3 feature points stored as column vectors // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame - int computePoses(const double featureVectors[3][3], const double worldPoints[3][3], double solutionsR[4][3][3], - double solutionsT[4][3]); + int computePoses(const double featureVectors[3][4], const double worldPoints[3][4], double solutionsR[4][3][3], + double solutionsT[4][3], bool p4p); }; } diff --git a/modules/calib3d/src/ippe.cpp b/modules/calib3d/src/ippe.cpp new file mode 100644 index 0000000000..74a2864525 --- /dev/null +++ b/modules/calib3d/src/ippe.cpp @@ -0,0 +1,1100 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This file is based on file issued with the following license: + +/*============================================================================ + +Copyright 2017 Toby Collins +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "precomp.hpp" +#include "ippe.hpp" + +namespace cv { +namespace IPPE { +PoseSolver::PoseSolver() : IPPE_SMALL(1e-3) +{ +} + +void PoseSolver::solveGeneric(InputArray _objectPoints, InputArray _imagePoints, OutputArray _rvec1, OutputArray _tvec1, + float& err1, OutputArray _rvec2, OutputArray _tvec2, float& err2) +{ + Mat normalizedImagePoints; + if (_imagePoints.getMat().type() == CV_32FC2) + { + _imagePoints.getMat().convertTo(normalizedImagePoints, CV_64F); + } + else + { + normalizedImagePoints = _imagePoints.getMat(); + } + + //solve: + Mat Ma, Mb; + solveGeneric(_objectPoints, normalizedImagePoints, Ma, Mb); + + //the two poses computed by IPPE (sorted): + Mat M1, M2; + + //sort poses by reprojection error: + sortPosesByReprojError(_objectPoints, normalizedImagePoints, Ma, Mb, M1, M2, err1, err2); + + //fill outputs + rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1); + rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2); + + M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1); + M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2); +} + +void PoseSolver::solveGeneric(InputArray _objectPoints, InputArray _normalizedInputPoints, + OutputArray _Ma, OutputArray _Mb) +{ + //argument checking: + size_t n = static_cast(_objectPoints.rows() * _objectPoints.cols()); //number of points + int objType = _objectPoints.type(); + int type_input = _normalizedInputPoints.type(); + + CV_CheckType(objType, objType == CV_32FC3 || objType == CV_64FC3, + "Type of _objectPoints must be CV_32FC3 or CV_64FC3" ); + CV_CheckType(type_input, type_input == CV_32FC2 || type_input == CV_64FC2, + "Type of _normalizedInputPoints must be CV_32FC3 or CV_64FC3" ); + CV_Assert(_objectPoints.rows() == 1 || _objectPoints.cols() == 1); + CV_Assert(_objectPoints.rows() >= 4 || _objectPoints.cols() >= 4); + CV_Assert(_normalizedInputPoints.rows() == 1 || _normalizedInputPoints.cols() == 1); + CV_Assert(static_cast(_objectPoints.rows() * _objectPoints.cols()) == n); + + Mat normalizedInputPoints; + if (type_input == CV_32FC2) + { + _normalizedInputPoints.getMat().convertTo(normalizedInputPoints, CV_64F); + } + else + { + normalizedInputPoints = _normalizedInputPoints.getMat(); + } + + Mat objectInputPoints; + if (type_input == CV_32FC3) + { + _objectPoints.getMat().convertTo(objectInputPoints, CV_64F); + } + else + { + objectInputPoints = _objectPoints.getMat(); + } + + Mat canonicalObjPoints; + Mat MmodelPoints2Canonical; + + //transform object points to the canonical position (zero centred and on the plane z=0): + makeCanonicalObjectPoints(objectInputPoints, canonicalObjPoints, MmodelPoints2Canonical); + + //compute the homography mapping the model's points to normalizedInputPoints + Matx33d H; + HomographyHO::homographyHO(canonicalObjPoints, _normalizedInputPoints, H); + + //now solve + Mat MaCanon, MbCanon; + solveCanonicalForm(canonicalObjPoints, normalizedInputPoints, H, MaCanon, MbCanon); + + //transform computed poses to account for canonical transform: + Mat Ma = MaCanon * MmodelPoints2Canonical; + Mat Mb = MbCanon * MmodelPoints2Canonical; + + //output poses: + Ma.copyTo(_Ma); + Mb.copyTo(_Mb); +} + +void PoseSolver::solveCanonicalForm(InputArray _canonicalObjPoints, InputArray _normalizedInputPoints, const Matx33d& H, + OutputArray _Ma, OutputArray _Mb) +{ + _Ma.create(4, 4, CV_64FC1); + _Mb.create(4, 4, CV_64FC1); + + Mat Ma = _Ma.getMat(); + Mat Mb = _Mb.getMat(); + + //initialise poses: + Ma.setTo(0); + Ma.at(3, 3) = 1; + Mb.setTo(0); + Mb.at(3, 3) = 1; + + //Compute the Jacobian J of the homography at (0,0): + double j00 = H(0, 0) - H(2, 0) * H(0, 2); + double j01 = H(0, 1) - H(2, 1) * H(0, 2); + double j10 = H(1, 0) - H(2, 0) * H(1, 2); + double j11 = H(1, 1) - H(2, 1) * H(1, 2); + + //Compute the transformation of (0,0) into the image: + double v0 = H(0, 2); + double v1 = H(1, 2); + + //compute the two rotation solutions: + Mat Ra = Ma.colRange(0, 3).rowRange(0, 3); + Mat Rb = Mb.colRange(0, 3).rowRange(0, 3); + computeRotations(j00, j01, j10, j11, v0, v1, Ra, Rb); + + //for each rotation solution, compute the corresponding translation solution: + Mat ta = Ma.colRange(3, 4).rowRange(0, 3); + Mat tb = Mb.colRange(3, 4).rowRange(0, 3); + computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Ra, ta); + computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Rb, tb); +} + +void PoseSolver::solveSquare(InputArray _objectPoints, InputArray _imagePoints, OutputArray _rvec1, OutputArray _tvec1, + float& err1, OutputArray _rvec2, OutputArray _tvec2, float& err2) +{ + //allocate outputs: + _rvec1.create(3, 1, CV_64FC1); + _tvec1.create(3, 1, CV_64FC1); + _rvec2.create(3, 1, CV_64FC1); + _tvec2.create(3, 1, CV_64FC1); + + Mat objectPoints2D; + + //generate the object points: + objectPoints2D.create(1, 4, CV_64FC2); + Mat objectPoints = _objectPoints.getMat(); + double squareLength; + if (objectPoints.depth() == CV_32F) + { + objectPoints2D.ptr(0)[0] = Vec2d(objectPoints.ptr(0)[0](0), objectPoints.ptr(0)[0](1)); + objectPoints2D.ptr(0)[1] = Vec2d(objectPoints.ptr(0)[1](0), objectPoints.ptr(0)[1](1)); + objectPoints2D.ptr(0)[2] = Vec2d(objectPoints.ptr(0)[2](0), objectPoints.ptr(0)[2](1)); + objectPoints2D.ptr(0)[3] = Vec2d(objectPoints.ptr(0)[3](0), objectPoints.ptr(0)[3](1)); + + squareLength = sqrt( (objectPoints.ptr(0)[1](0) - objectPoints.ptr(0)[0](0))* + (objectPoints.ptr(0)[1](0) - objectPoints.ptr(0)[0](0)) + + (objectPoints.ptr(0)[1](1) - objectPoints.ptr(0)[0](1))* + (objectPoints.ptr(0)[1](1) - objectPoints.ptr(0)[0](1)) ); + } + else + { + objectPoints2D.ptr(0)[0] = Vec2d(objectPoints.ptr(0)[0](0), objectPoints.ptr(0)[0](1)); + objectPoints2D.ptr(0)[1] = Vec2d(objectPoints.ptr(0)[1](0), objectPoints.ptr(0)[1](1)); + objectPoints2D.ptr(0)[2] = Vec2d(objectPoints.ptr(0)[2](0), objectPoints.ptr(0)[2](1)); + objectPoints2D.ptr(0)[3] = Vec2d(objectPoints.ptr(0)[3](0), objectPoints.ptr(0)[3](1)); + + squareLength = sqrt( (objectPoints.ptr(0)[1](0) - objectPoints.ptr(0)[0](0))* + (objectPoints.ptr(0)[1](0) - objectPoints.ptr(0)[0](0)) + + (objectPoints.ptr(0)[1](1) - objectPoints.ptr(0)[0](1))* + (objectPoints.ptr(0)[1](1) - objectPoints.ptr(0)[0](1)) ); + } + + Mat H; //homography from canonical object points to normalized pixels + + Mat normalizedInputPoints; + if (_imagePoints.getMat().type() == CV_32FC2) + { + _imagePoints.getMat().convertTo(normalizedInputPoints, CV_64F); + } + else + { + normalizedInputPoints = _imagePoints.getMat(); + } + + //compute H + homographyFromSquarePoints(normalizedInputPoints, squareLength / 2.0, H); + + //now solve + Mat Ma, Mb; + solveCanonicalForm(objectPoints2D, normalizedInputPoints, H, Ma, Mb); + + //sort poses according to reprojection error: + Mat M1, M2; + sortPosesByReprojError(_objectPoints, normalizedInputPoints, Ma, Mb, M1, M2, err1, err2); + + //fill outputs + rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1); + rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2); + + M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1); + M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2); +} + +void PoseSolver::generateSquareObjectCorners3D(double squareLength, OutputArray _objectPoints) +{ + _objectPoints.create(1, 4, CV_64FC3); + Mat objectPoints = _objectPoints.getMat(); + objectPoints.ptr(0)[0] = Vec3d(-squareLength / 2.0, squareLength / 2.0, 0.0); + objectPoints.ptr(0)[1] = Vec3d(squareLength / 2.0, squareLength / 2.0, 0.0); + objectPoints.ptr(0)[2] = Vec3d(squareLength / 2.0, -squareLength / 2.0, 0.0); + objectPoints.ptr(0)[3] = Vec3d(-squareLength / 2.0, -squareLength / 2.0, 0.0); +} + +void PoseSolver::generateSquareObjectCorners2D(double squareLength, OutputArray _objectPoints) +{ + _objectPoints.create(1, 4, CV_64FC2); + Mat objectPoints = _objectPoints.getMat(); + objectPoints.ptr(0)[0] = Vec2d(-squareLength / 2.0, squareLength / 2.0); + objectPoints.ptr(0)[1] = Vec2d(squareLength / 2.0, squareLength / 2.0); + objectPoints.ptr(0)[2] = Vec2d(squareLength / 2.0, -squareLength / 2.0); + objectPoints.ptr(0)[3] = Vec2d(-squareLength / 2.0, -squareLength / 2.0); +} + +double PoseSolver::meanSceneDepth(InputArray _objectPoints, InputArray _rvec, InputArray _tvec) +{ + CV_CheckType(_objectPoints.type(), _objectPoints.type() == CV_64FC3, + "Type of _objectPoints must be CV_64FC3" ); + + size_t n = static_cast(_objectPoints.rows() * _objectPoints.cols()); + Mat R; + Mat q; + Rodrigues(_rvec, R); + double zBar = 0; + + for (size_t i = 0; i < n; i++) + { + Mat p(_objectPoints.getMat().at(static_cast(i))); + q = R * p + _tvec.getMat(); + double z; + if (q.depth() == CV_64F) + { + z = q.at(2); + } + else + { + z = static_cast(q.at(2)); + } + zBar += z; + } + return zBar / static_cast(n); +} + +void PoseSolver::rot2vec(InputArray _R, OutputArray _r) +{ + CV_CheckType(_R.type(), _R.type() == CV_64FC1, + "Type of _R must be CV_64FC1" ); + CV_Assert(_R.rows() == 3); + CV_Assert(_R.cols() == 3); + + _r.create(3, 1, CV_64FC1); + + Mat R = _R.getMat(); + Mat rvec = _r.getMat(); + + double trace = R.at(0, 0) + R.at(1, 1) + R.at(2, 2); + double w_norm = acos((trace - 1.0) / 2.0); + double eps = std::numeric_limits::epsilon(); + double d = 1 / (2 * sin(w_norm)) * w_norm; + if (w_norm < eps) //rotation is the identity + { + rvec.setTo(0); + } + else + { + double c0 = R.at(2, 1) - R.at(1, 2); + double c1 = R.at(0, 2) - R.at(2, 0); + double c2 = R.at(1, 0) - R.at(0, 1); + rvec.at(0) = d * c0; + rvec.at(1) = d * c1; + rvec.at(2) = d * c2; + } +} + +void PoseSolver::computeTranslation(InputArray _objectPoints, InputArray _normalizedImgPoints, InputArray _R, OutputArray _t) +{ + //This is solved by building the linear system At = b, where t corresponds to the (unknown) translation. + //This is then inverted with the associated normal equations to give t = inv(transpose(A)*A)*transpose(A)*b + //For efficiency we only store the coefficients of (transpose(A)*A) and (transpose(A)*b) + + CV_CheckType(_objectPoints.type(), _objectPoints.type() == CV_64FC2, + "Type of _objectPoints must be CV_64FC2" ); + CV_CheckType(_normalizedImgPoints.type(), _normalizedImgPoints.type() == CV_64FC2, + "Type of _normalizedImgPoints must be CV_64FC2" ); + CV_CheckType(_R.type(), _R.type() == CV_64FC1, + "Type of _R must be CV_64FC1" ); + CV_Assert(_R.rows() == 3 && _R.cols() == 3); + CV_Assert(_objectPoints.rows() == 1 || _objectPoints.cols() == 1); + CV_Assert(_normalizedImgPoints.rows() == 1 || _normalizedImgPoints.cols() == 1); + + size_t n = static_cast(_normalizedImgPoints.rows() * _normalizedImgPoints.cols()); + CV_Assert(n == static_cast(_objectPoints.rows() * _objectPoints.cols())); + + Mat objectPoints = _objectPoints.getMat(); + Mat imgPoints = _normalizedImgPoints.getMat(); + + _t.create(3, 1, CV_64FC1); + + Mat R = _R.getMat(); + + //coefficients of (transpose(A)*A) + double ATA00 = static_cast(n); + double ATA02 = 0; + double ATA11 = static_cast(n); + double ATA12 = 0; + double ATA20 = 0; + double ATA21 = 0; + double ATA22 = 0; + + //coefficients of (transpose(A)*b) + double ATb0 = 0; + double ATb1 = 0; + double ATb2 = 0; + + //now loop through each point and increment the coefficients: + for (int i = 0; i < static_cast(n); i++) + { + const Vec2d& objPt = objectPoints.at(i); + double rx = R.at(0, 0) * objPt(0) + R.at(0, 1) * objPt(1); + double ry = R.at(1, 0) * objPt(0) + R.at(1, 1) * objPt(1); + double rz = R.at(2, 0) * objPt(0) + R.at(2, 1) * objPt(1); + + const Vec2d& imgPt = imgPoints.at(i); + double a2 = -imgPt(0); + double b2 = -imgPt(1); + + ATA02 = ATA02 + a2; + ATA12 = ATA12 + b2; + ATA20 = ATA20 + a2; + ATA21 = ATA21 + b2; + ATA22 = ATA22 + a2 * a2 + b2 * b2; + + double bx = -a2 * rz - rx; + double by = -b2 * rz - ry; + + ATb0 = ATb0 + bx; + ATb1 = ATb1 + by; + ATb2 = ATb2 + a2 * bx + b2 * by; + } + + double detAInv = 1.0 / (ATA00 * ATA11 * ATA22 - ATA00 * ATA12 * ATA21 - ATA02 * ATA11 * ATA20); + + //S gives inv(transpose(A)*A)/det(A)^2 + //construct S: + double S00 = ATA11 * ATA22 - ATA12 * ATA21; + double S01 = ATA02 * ATA21; + double S02 = -ATA02 * ATA11; + double S10 = ATA12 * ATA20; + double S11 = ATA00 * ATA22 - ATA02 * ATA20; + double S12 = -ATA00 * ATA12; + double S20 = -ATA11 * ATA20; + double S21 = -ATA00 * ATA21; + double S22 = ATA00 * ATA11; + + //solve t: + Mat t = _t.getMat(); + t.at(0) = detAInv * (S00 * ATb0 + S01 * ATb1 + S02 * ATb2); + t.at(1) = detAInv * (S10 * ATb0 + S11 * ATb1 + S12 * ATb2); + t.at(2) = detAInv * (S20 * ATb0 + S21 * ATb1 + S22 * ATb2); +} + +void PoseSolver::computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2) +{ + //This is fairly optimized code which makes it hard to understand. The matlab code is certainly easier to read. + _R1.create(3, 3, CV_64FC1); + _R2.create(3, 3, CV_64FC1); + + Matx33d Rv; + Matx31d v(p, q, 1); + rotateVec2ZAxis(v,Rv); + Rv = Rv.t(); + + //setup the 2x2 SVD decomposition: + double rv00 = Rv(0,0); + double rv01 = Rv(0,1); + double rv02 = Rv(0,2); + + double rv10 = Rv(1,0); + double rv11 = Rv(1,1); + double rv12 = Rv(1,2); + + double rv20 = Rv(2,0); + double rv21 = Rv(2,1); + double rv22 = Rv(2,2); + + double b00 = rv00 - p * rv20; + double b01 = rv01 - p * rv21; + double b10 = rv10 - q * rv20; + double b11 = rv11 - q * rv21; + + double dtinv = 1.0 / ((b00 * b11 - b01 * b10)); + + double binv00 = dtinv * b11; + double binv01 = -dtinv * b01; + double binv10 = -dtinv * b10; + double binv11 = dtinv * b00; + + double a00 = binv00 * j00 + binv01 * j10; + double a01 = binv00 * j01 + binv01 * j11; + double a10 = binv10 * j00 + binv11 * j10; + double a11 = binv10 * j01 + binv11 * j11; + + //compute the largest singular value of A: + double ata00 = a00 * a00 + a01 * a01; + double ata01 = a00 * a10 + a01 * a11; + double ata11 = a10 * a10 + a11 * a11; + + double gamma2 = 0.5 * (ata00 + ata11 + sqrt((ata00 - ata11) * (ata00 - ata11) + 4.0 * ata01 * ata01)); + if (gamma2 < 0) + CV_Error(Error::StsNoConv, "gamma2 is negative."); + + double gamma = sqrt(gamma2); + + if (std::fabs(gamma) < std::numeric_limits::epsilon()) + CV_Error(Error::StsNoConv, "gamma is zero."); + + //reconstruct the full rotation matrices: + double rtilde00 = a00 / gamma; + double rtilde01 = a01 / gamma; + double rtilde10 = a10 / gamma; + double rtilde11 = a11 / gamma; + + double rtilde00_2 = rtilde00 * rtilde00; + double rtilde01_2 = rtilde01 * rtilde01; + double rtilde10_2 = rtilde10 * rtilde10; + double rtilde11_2 = rtilde11 * rtilde11; + + double b0 = sqrt(-rtilde00_2 - rtilde10_2 + 1); + double b1 = sqrt(-rtilde01_2 - rtilde11_2 + 1); + double sp = (-rtilde00 * rtilde01 - rtilde10 * rtilde11); + + if (sp < 0) + { + b1 = -b1; + } + + //store results: + Mat R1 = _R1.getMat(); + Mat R2 = _R2.getMat(); + + R1.at(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (b0)*rv02; + R1.at(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (b1)*rv02; + R1.at(0, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv00 + (b0 * rtilde01 - b1 * rtilde00) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02; + R1.at(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (b0)*rv12; + R1.at(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (b1)*rv12; + R1.at(1, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv10 + (b0 * rtilde01 - b1 * rtilde00) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12; + R1.at(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (b0)*rv22; + R1.at(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (b1)*rv22; + R1.at(2, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv20 + (b0 * rtilde01 - b1 * rtilde00) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22; + + R2.at(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (-b0) * rv02; + R2.at(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (-b1) * rv02; + R2.at(0, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv00 + (b1 * rtilde00 - b0 * rtilde01) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02; + R2.at(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (-b0) * rv12; + R2.at(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (-b1) * rv12; + R2.at(1, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv10 + (b1 * rtilde00 - b0 * rtilde01) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12; + R2.at(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (-b0) * rv22; + R2.at(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (-b1) * rv22; + R2.at(2, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv20 + (b1 * rtilde00 - b0 * rtilde01) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22; +} + +void PoseSolver::homographyFromSquarePoints(InputArray _targetPoints, double halfLength, OutputArray H_) +{ + CV_CheckType(_targetPoints.type(), _targetPoints.type() == CV_32FC2 || _targetPoints.type() == CV_64FC2, + "Type of _targetPoints must be CV_32FC2 or CV_64FC2" ); + + Mat pts = _targetPoints.getMat(); + + double p1x, p1y; + double p2x, p2y; + double p3x, p3y; + double p4x, p4y; + + if (_targetPoints.type() == CV_32FC2) + { + p1x = -pts.at(0)(0); + p1y = -pts.at(0)(1); + + p2x = -pts.at(1)(0); + p2y = -pts.at(1)(1); + + p3x = -pts.at(2)(0); + p3y = -pts.at(2)(1); + + p4x = -pts.at(3)(0); + p4y = -pts.at(3)(1); + } + else + { + p1x = -pts.at(0)(0); + p1y = -pts.at(0)(1); + + p2x = -pts.at(1)(0); + p2y = -pts.at(1)(1); + + p3x = -pts.at(2)(0); + p3y = -pts.at(2)(1); + + p4x = -pts.at(3)(0); + p4y = -pts.at(3)(1); + } + + //analytic solution: + double det = (halfLength * (p1x * p2y - p2x * p1y - p1x * p4y + p2x * p3y - p3x * p2y + p4x * p1y + p3x * p4y - p4x * p3y)); + if (abs(det) < 1e-9) + CV_Error(Error::StsNoConv, "Determinant is zero!"); + double detsInv = -1 / det; + + Matx33d H; + H(0, 0) = detsInv * (p1x * p3x * p2y - p2x * p3x * p1y - p1x * p4x * p2y + p2x * p4x * p1y - p1x * p3x * p4y + p1x * p4x * p3y + p2x * p3x * p4y - p2x * p4x * p3y); + H(0, 1) = detsInv * (p1x * p2x * p3y - p1x * p3x * p2y - p1x * p2x * p4y + p2x * p4x * p1y + p1x * p3x * p4y - p3x * p4x * p1y - p2x * p4x * p3y + p3x * p4x * p2y); + H(0, 2) = detsInv * halfLength * (p1x * p2x * p3y - p2x * p3x * p1y - p1x * p2x * p4y + p1x * p4x * p2y - p1x * p4x * p3y + p3x * p4x * p1y + p2x * p3x * p4y - p3x * p4x * p2y); + H(1, 0) = detsInv * (p1x * p2y * p3y - p2x * p1y * p3y - p1x * p2y * p4y + p2x * p1y * p4y - p3x * p1y * p4y + p4x * p1y * p3y + p3x * p2y * p4y - p4x * p2y * p3y); + H(1, 1) = detsInv * (p2x * p1y * p3y - p3x * p1y * p2y - p1x * p2y * p4y + p4x * p1y * p2y + p1x * p3y * p4y - p4x * p1y * p3y - p2x * p3y * p4y + p3x * p2y * p4y); + H(1, 2) = detsInv * halfLength * (p1x * p2y * p3y - p3x * p1y * p2y - p2x * p1y * p4y + p4x * p1y * p2y - p1x * p3y * p4y + p3x * p1y * p4y + p2x * p3y * p4y - p4x * p2y * p3y); + H(2, 0) = -detsInv * (p1x * p3y - p3x * p1y - p1x * p4y - p2x * p3y + p3x * p2y + p4x * p1y + p2x * p4y - p4x * p2y); + H(2, 1) = detsInv * (p1x * p2y - p2x * p1y - p1x * p3y + p3x * p1y + p2x * p4y - p4x * p2y - p3x * p4y + p4x * p3y); + H(2, 2) = 1.0; + + Mat(H, false).copyTo(H_); +} + +void PoseSolver::makeCanonicalObjectPoints(InputArray _objectPoints, OutputArray _canonicalObjPoints, OutputArray _MmodelPoints2Canonical) +{ + int objType = _objectPoints.type(); + CV_CheckType(objType, objType == CV_32FC3 || objType == CV_64FC3, + "Type of _objectPoints must be CV_32FC3 or CV_64FC3" ); + + int n = _objectPoints.rows() * _objectPoints.cols(); + + _canonicalObjPoints.create(1, n, CV_64FC2); + + Mat objectPoints = _objectPoints.getMat(); + Mat canonicalObjPoints = _canonicalObjPoints.getMat(); + + Mat UZero(3, n, CV_64FC1); + + double xBar = 0; + double yBar = 0; + double zBar = 0; + bool isOnZPlane = true; + for (int i = 0; i < n; i++) + { + double x, y, z; + if (objType == CV_32FC3) + { + x = static_cast(objectPoints.at(i)[0]); + y = static_cast(objectPoints.at(i)[1]); + z = static_cast(objectPoints.at(i)[2]); + } + else + { + x = objectPoints.at(i)[0]; + y = objectPoints.at(i)[1]; + z = objectPoints.at(i)[2]; + } + + if (abs(z) > IPPE_SMALL) + { + isOnZPlane = false; + } + + xBar += x; + yBar += y; + zBar += z; + + UZero.at(0, i) = x; + UZero.at(1, i) = y; + UZero.at(2, i) = z; + } + xBar = xBar / static_cast(n); + yBar = yBar / static_cast(n); + zBar = zBar / static_cast(n); + + for (int i = 0; i < n; i++) + { + UZero.at(0, i) -= xBar; + UZero.at(1, i) -= yBar; + UZero.at(2, i) -= zBar; + } + + Matx44d MCenter = Matx44d::eye(); + MCenter(0, 3) = -xBar; + MCenter(1, 3) = -yBar; + MCenter(2, 3) = -zBar; + + if (isOnZPlane) + { + //MmodelPoints2Canonical is given by MCenter + Mat(MCenter, false).copyTo(_MmodelPoints2Canonical); + for (int i = 0; i < n; i++) + { + canonicalObjPoints.at(i)[0] = UZero.at(0, i); + canonicalObjPoints.at(i)[1] = UZero.at(1, i); + } + } + else + { + Mat UZeroAligned(3, n, CV_64FC1); + Matx33d R; //rotation that rotates objectPoints to the plane z=0 + + if (!computeObjextSpaceR3Pts(objectPoints,R)) + { + //we could not compute R, problably because there is a duplicate point in {objectPoints(0),objectPoints(1),objectPoints(2)}. + //So we compute it with the SVD (which is slower): + computeObjextSpaceRSvD(UZero,R); + } + + UZeroAligned = R * UZero; + + for (int i = 0; i < n; i++) + { + canonicalObjPoints.at(i)[0] = UZeroAligned.at(0, i); + canonicalObjPoints.at(i)[1] = UZeroAligned.at(1, i); + if (abs(UZeroAligned.at(2, i)) > IPPE_SMALL) + CV_Error(Error::StsNoConv, "Cannot transform object points to the plane z=0!"); + } + + Matx44d MRot = Matx44d::zeros(); + MRot(3, 3) = 1; + + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + MRot(i,j) = R(i,j); + } + } + Matx44d Mb = MRot * MCenter; + Mat(Mb, false).copyTo(_MmodelPoints2Canonical); + } +} + +void PoseSolver::evalReprojError(InputArray _objectPoints, InputArray _imagePoints, InputArray _M, float& err) +{ + Mat projectedPoints; + Mat imagePoints = _imagePoints.getMat(); + Mat r; + rot2vec(_M.getMat().colRange(0, 3).rowRange(0, 3), r); + + Mat K = Mat::eye(3, 3, CV_64FC1); + Mat dist; + projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), K, dist, projectedPoints); + + err = 0; + int n = _objectPoints.rows() * _objectPoints.cols(); + + float dx, dy; + const int projPtsDepth = projectedPoints.depth(); + for (int i = 0; i < n; i++) + { + if (projPtsDepth == CV_32F) + { + dx = projectedPoints.at(i)[0] - static_cast(imagePoints.at(i)[0]); + dy = projectedPoints.at(i)[1] - static_cast(imagePoints.at(i)[1]); + } + else + { + dx = static_cast(projectedPoints.at(i)[0] - imagePoints.at(i)[0]); + dy = static_cast(projectedPoints.at(i)[1] - imagePoints.at(i)[1]); + } + + err += dx * dx + dy * dy; + } + err = sqrt(err / (2.0f * n)); +} + +void PoseSolver::sortPosesByReprojError(InputArray _objectPoints, InputArray _imagePoints, InputArray _Ma, InputArray _Mb, + OutputArray _M1, OutputArray _M2, float& err1, float& err2) +{ + float erra, errb; + evalReprojError(_objectPoints, _imagePoints, _Ma, erra); + evalReprojError(_objectPoints, _imagePoints, _Mb, errb); + if (erra < errb) + { + err1 = erra; + _Ma.copyTo(_M1); + + err2 = errb; + _Mb.copyTo(_M2); + } + else + { + err1 = errb; + _Mb.copyTo(_M1); + + err2 = erra; + _Ma.copyTo(_M2); + } +} + +void PoseSolver::rotateVec2ZAxis(const Matx31d& a, Matx33d& Ra) +{ + double ax = a(0); + double ay = a(1); + double az = a(2); + + double nrm = sqrt(ax*ax + ay*ay + az*az); + ax = ax/nrm; + ay = ay/nrm; + az = az/nrm; + + double c = az; + + if (abs(1.0+c) < std::numeric_limits::epsilon()) + { + Ra = Matx33d::zeros(); + Ra(0,0) = 1.0; + Ra(1,1) = 1.0; + Ra(2,2) = -1.0; + } + else + { + double d = 1.0/(1.0+c); + double ax2 = ax*ax; + double ay2 = ay*ay; + double axay = ax*ay; + + Ra(0,0) = -ax2*d + 1.0; + Ra(0,1) = -axay*d; + Ra(0,2) = -ax; + + Ra(1,0) = -axay*d; + Ra(1,1) = -ay2*d + 1.0; + Ra(1,2) = -ay; + + Ra(2,0) = ax; + Ra(2,1) = ay; + Ra(2,2) = 1.0 - (ax2 + ay2)*d; + } +} + +bool PoseSolver::computeObjextSpaceR3Pts(InputArray _objectPoints, Matx33d& R) +{ + bool ret; //return argument + double p1x,p1y,p1z; + double p2x,p2y,p2z; + double p3x,p3y,p3z; + + Mat objectPoints = _objectPoints.getMat(); + if (objectPoints.type() == CV_32FC3) + { + p1x = objectPoints.at(0)[0]; + p1y = objectPoints.at(0)[1]; + p1z = objectPoints.at(0)[2]; + + p2x = objectPoints.at(1)[0]; + p2y = objectPoints.at(1)[1]; + p2z = objectPoints.at(1)[2]; + + p3x = objectPoints.at(2)[0]; + p3y = objectPoints.at(2)[1]; + p3z = objectPoints.at(2)[2]; + } + else + { + p1x = objectPoints.at(0)[0]; + p1y = objectPoints.at(0)[1]; + p1z = objectPoints.at(0)[2]; + + p2x = objectPoints.at(1)[0]; + p2y = objectPoints.at(1)[1]; + p2z = objectPoints.at(1)[2]; + + p3x = objectPoints.at(2)[0]; + p3y = objectPoints.at(2)[1]; + p3z = objectPoints.at(2)[2]; + } + + double nx = (p1y - p2y)*(p1z - p3z) - (p1y - p3y)*(p1z - p2z); + double ny = (p1x - p3x)*(p1z - p2z) - (p1x - p2x)*(p1z - p3z); + double nz = (p1x - p2x)*(p1y - p3y) - (p1x - p3x)*(p1y - p2y); + + double nrm = sqrt(nx*nx+ ny*ny + nz*nz); + if (nrm > IPPE_SMALL) + { + nx = nx/nrm; + ny = ny/nrm; + nz = nz/nrm; + Matx31d v(nx, ny, nz); + rotateVec2ZAxis(v,R); + ret = true; + } + else + { + ret = false; + } + return ret; +} + +void PoseSolver::computeObjextSpaceRSvD(InputArray _objectPointsZeroMean, OutputArray _R) +{ + _R.create(3, 3, CV_64FC1); + Mat R = _R.getMat(); + + //we could not compute R with the first three points, so lets use the SVD + SVD s; + Mat W, U, VT; + s.compute(_objectPointsZeroMean.getMat() * _objectPointsZeroMean.getMat().t(), W, U, VT); + double s3 = W.at(2); + double s2 = W.at(1); + + //check if points are coplanar: + CV_Assert(s3 / s2 < IPPE_SMALL); + + R = U.t(); + if (determinant(R) < 0) + { + //this ensures R is a rotation matrix and not a general unitary matrix: + R.at(2, 0) = -R.at(2, 0); + R.at(2, 1) = -R.at(2, 1); + R.at(2, 2) = -R.at(2, 2); + } +} +} //namespace IPPE + +namespace HomographyHO { +void normalizeDataIsotropic(InputArray _Data, OutputArray _DataN, OutputArray _T, OutputArray _Ti) +{ + Mat Data = _Data.getMat(); + int numPoints = Data.rows * Data.cols; + CV_Assert(Data.rows == 1 || Data.cols == 1); + CV_Assert(Data.channels() == 2 || Data.channels() == 3); + CV_Assert(numPoints >= 4); + + int dataType = _Data.type(); + CV_CheckType(dataType, dataType == CV_32FC2 || dataType == CV_32FC3 || dataType == CV_64FC2 || dataType == CV_64FC3, + "Type of _Data must be one of CV_32FC2, CV_32FC3, CV_64FC2, CV_64FC3"); + + _DataN.create(2, numPoints, CV_64FC1); + + _T.create(3, 3, CV_64FC1); + _Ti.create(3, 3, CV_64FC1); + + Mat DataN = _DataN.getMat(); + Mat T = _T.getMat(); + Mat Ti = _Ti.getMat(); + + _T.setTo(0); + _Ti.setTo(0); + + int numChannels = Data.channels(); + double xm = 0; + double ym = 0; + for (int i = 0; i < numPoints; i++) + { + if (numChannels == 2) + { + if (dataType == CV_32FC2) + { + xm = xm + Data.at(i)[0]; + ym = ym + Data.at(i)[1]; + } + else + { + xm = xm + Data.at(i)[0]; + ym = ym + Data.at(i)[1]; + } + } + else + { + if (dataType == CV_32FC3) + { + xm = xm + Data.at(i)[0]; + ym = ym + Data.at(i)[1]; + } + else + { + xm = xm + Data.at(i)[0]; + ym = ym + Data.at(i)[1]; + } + } + } + xm = xm / static_cast(numPoints); + ym = ym / static_cast(numPoints); + + double kappa = 0; + double xh, yh; + + for (int i = 0; i < numPoints; i++) + { + + if (numChannels == 2) + { + if (dataType == CV_32FC2) + { + xh = Data.at(i)[0] - xm; + yh = Data.at(i)[1] - ym; + } + else + { + xh = Data.at(i)[0] - xm; + yh = Data.at(i)[1] - ym; + } + } + else + { + if (dataType == CV_32FC3) + { + xh = Data.at(i)[0] - xm; + yh = Data.at(i)[1] - ym; + } + else + { + xh = Data.at(i)[0] - xm; + yh = Data.at(i)[1] - ym; + } + } + + DataN.at(0, i) = xh; + DataN.at(1, i) = yh; + kappa = kappa + xh * xh + yh * yh; + } + double beta = sqrt(2 * numPoints / kappa); + DataN = DataN * beta; + + T.at(0, 0) = 1.0 / beta; + T.at(1, 1) = 1.0 / beta; + + T.at(0, 2) = xm; + T.at(1, 2) = ym; + + T.at(2, 2) = 1; + + Ti.at(0, 0) = beta; + Ti.at(1, 1) = beta; + + Ti.at(0, 2) = -beta * xm; + Ti.at(1, 2) = -beta * ym; + + Ti.at(2, 2) = 1; +} + +void homographyHO(InputArray _srcPoints, InputArray _targPoints, Matx33d& H) +{ + Mat DataA, DataB, TA, TAi, TB, TBi; + + HomographyHO::normalizeDataIsotropic(_srcPoints, DataA, TA, TAi); + HomographyHO::normalizeDataIsotropic(_targPoints, DataB, TB, TBi); + + int n = DataA.cols; + CV_Assert(n == DataB.cols); + + Mat C1(1, n, CV_64FC1); + Mat C2(1, n, CV_64FC1); + Mat C3(1, n, CV_64FC1); + Mat C4(1, n, CV_64FC1); + + double mC1 = 0, mC2 = 0, mC3 = 0, mC4 = 0; + + for (int i = 0; i < n; i++) + { + C1.at(0, i) = -DataB.at(0, i) * DataA.at(0, i); + C2.at(0, i) = -DataB.at(0, i) * DataA.at(1, i); + C3.at(0, i) = -DataB.at(1, i) * DataA.at(0, i); + C4.at(0, i) = -DataB.at(1, i) * DataA.at(1, i); + + mC1 += C1.at(0, i); + mC2 += C2.at(0, i); + mC3 += C3.at(0, i); + mC4 += C4.at(0, i); + } + + mC1 /= n; + mC2 /= n; + mC3 /= n; + mC4 /= n; + + Mat Mx(n, 3, CV_64FC1); + Mat My(n, 3, CV_64FC1); + + for (int i = 0; i < n; i++) + { + Mx.at(i, 0) = C1.at(0, i) - mC1; + Mx.at(i, 1) = C2.at(0, i) - mC2; + Mx.at(i, 2) = -DataB.at(0, i); + + My.at(i, 0) = C3.at(0, i) - mC3; + My.at(i, 1) = C4.at(0, i) - mC4; + My.at(i, 2) = -DataB.at(1, i); + } + + Mat DataAT, DataADataAT; + + transpose(DataA, DataAT); + DataADataAT = DataA * DataAT; + double dt = DataADataAT.at(0, 0) * DataADataAT.at(1, 1) - DataADataAT.at(0, 1) * DataADataAT.at(1, 0); + + Mat DataADataATi(2, 2, CV_64FC1); + DataADataATi.at(0, 0) = DataADataAT.at(1, 1) / dt; + DataADataATi.at(0, 1) = -DataADataAT.at(0, 1) / dt; + DataADataATi.at(1, 0) = -DataADataAT.at(1, 0) / dt; + DataADataATi.at(1, 1) = DataADataAT.at(0, 0) / dt; + + Mat Pp = DataADataATi * DataA; + + Mat Bx = Pp * Mx; + Mat By = Pp * My; + + Mat Ex = DataAT * Bx; + Mat Ey = DataAT * By; + + Mat D(2 * n, 3, CV_64FC1); + + for (int i = 0; i < n; i++) + { + D.at(i, 0) = Mx.at(i, 0) - Ex.at(i, 0); + D.at(i, 1) = Mx.at(i, 1) - Ex.at(i, 1); + D.at(i, 2) = Mx.at(i, 2) - Ex.at(i, 2); + + D.at(i + n, 0) = My.at(i, 0) - Ey.at(i, 0); + D.at(i + n, 1) = My.at(i, 1) - Ey.at(i, 1); + D.at(i + n, 2) = My.at(i, 2) - Ey.at(i, 2); + } + + Mat DT, DDT; + transpose(D, DT); + DDT = DT * D; + + Mat S, U; + eigen(DDT, S, U); + + Mat h789(3, 1, CV_64FC1); + h789.at(0, 0) = U.at(2, 0); + h789.at(1, 0) = U.at(2, 1); + h789.at(2, 0) = U.at(2, 2); + + Mat h12 = -Bx * h789; + Mat h45 = -By * h789; + + double h3 = -(mC1 * h789.at(0, 0) + mC2 * h789.at(1, 0)); + double h6 = -(mC3 * h789.at(0, 0) + mC4 * h789.at(1, 0)); + + H(0, 0) = h12.at(0, 0); + H(0, 1) = h12.at(1, 0); + H(0, 2) = h3; + + H(1, 0) = h45.at(0, 0); + H(1, 1) = h45.at(1, 0); + H(1, 2) = h6; + + H(2, 0) = h789.at(0, 0); + H(2, 1) = h789.at(1, 0); + H(2, 2) = h789.at(2, 0); + + H = Mat(TB * H * TAi); + double h22_inv = 1 / H(2, 2); + H = H * h22_inv; +} +} +} //namespace cv diff --git a/modules/calib3d/src/ippe.hpp b/modules/calib3d/src/ippe.hpp new file mode 100644 index 0000000000..6dc76f59a6 --- /dev/null +++ b/modules/calib3d/src/ippe.hpp @@ -0,0 +1,259 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This file is based on file issued with the following license: + +/*============================================================================ + +Copyright 2017 Toby Collins +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may + be used to endorse or promote products derived from this software without + specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef OPENCV_CALIB3D_IPPE_HPP +#define OPENCV_CALIB3D_IPPE_HPP + +#include + +namespace cv { +namespace IPPE { + +class PoseSolver { +public: + /** + * @brief PoseSolver constructor + */ + PoseSolver(); + + /** + * @brief Finds the two possible poses of a planar object given a set of correspondences and their respective reprojection errors. + * The poses are sorted with the first having the lowest reprojection error. + * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. + * 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates. + * @param rvec1 First rotation solution (3x1 rotation vector) + * @param tvec1 First translation solution (3x1 vector) + * @param reprojErr1 Reprojection error of first solution + * @param rvec2 Second rotation solution (3x1 rotation vector) + * @param tvec2 Second translation solution (3x1 vector) + * @param reprojErr2 Reprojection error of second solution + */ + void solveGeneric(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1, + float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2); + + /** + * @brief Finds the two possible poses of a square planar object and their respective reprojection errors using IPPE. + * The poses are sorted so that the first one is the one with the lowest reprojection error. + * + * @param objectPoints Array of 4 coplanar object points defined in the following object coordinates: + * - point 0: [-squareLength / 2.0, squareLength / 2.0, 0] + * - point 1: [squareLength / 2.0, squareLength / 2.0, 0] + * - point 2: [squareLength / 2.0, -squareLength / 2.0, 0] + * - point 3: [-squareLength / 2.0, -squareLength / 2.0, 0] + * 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. Points are in normalized pixel coordinates. + * @param rvec1 First rotation solution (3x1 rotation vector) + * @param tvec1 First translation solution (3x1 vector) + * @param reprojErr1 Reprojection error of first solution + * @param rvec2 Second rotation solution (3x1 rotation vector) + * @param tvec2 Second translation solution (3x1 vector) + * @param reprojErr2 Reprojection error of second solution + */ + void solveSquare(InputArray objectPoints, InputArray imagePoints, OutputArray rvec1, OutputArray tvec1, + float& reprojErr1, OutputArray rvec2, OutputArray tvec2, float& reprojErr2); + +private: + /** + * @brief Finds the two possible poses of a planar object given a set of correspondences in normalized pixel coordinates. + * These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms. + * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double). + * @param normalizedImagePoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (float or double). + * @param Ma First pose solution (unsorted) + * @param Mb Second pose solution (unsorted) + */ + void solveGeneric(InputArray objectPoints, InputArray normalizedImagePoints, OutputArray Ma, OutputArray Mb); + + /** + * @brief Finds the two possible poses of a planar object in its canonical position, given a set of correspondences in normalized pixel coordinates. + * These poses are **NOT** sorted on reprojection error. Note that the returned poses are object-to-camera transforms, and not camera-to-object transforms. + * @param canonicalObjPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (double) where N is the number of points + * @param normalizedInputPoints Array of corresponding image points in normalized pixel coordinates, 1xN/Nx1 2-channel (double) where N is the number of points + * @param H Homography mapping canonicalObjPoints to normalizedInputPoints. + * @param Ma + * @param Mb + */ + void solveCanonicalForm(InputArray canonicalObjPoints, InputArray normalizedInputPoints, const Matx33d& H, + OutputArray Ma, OutputArray Mb); + + /** + * @brief Computes the translation solution for a given rotation solution + * @param objectPoints Array of corresponding object points, 1xN/Nx1 3-channel where N is the number of points + * @param normalizedImagePoints Array of corresponding image points (undistorted), 1xN/Nx1 2-channel where N is the number of points + * @param R Rotation solution (3x1 rotation vector) + * @param t Translation solution (3x1 rotation vector) + */ + void computeTranslation(InputArray objectPoints, InputArray normalizedImgPoints, InputArray R, OutputArray t); + + /** + * @brief Computes the two rotation solutions from the Jacobian of a homography matrix H at a point (ux,uy) on the object plane. + * For highest accuracy the Jacobian should be computed at the centroid of the point correspondences (see the IPPE paper for the explanation of this). + * For a point (ux,uy) on the object plane, suppose the homography H maps (ux,uy) to a point (p,q) in the image (in normalized pixel coordinates). + * The Jacobian matrix [J00, J01; J10,J11] is the Jacobian of the mapping evaluated at (ux,uy). + * @param j00 Homography jacobian coefficent at (ux,uy) + * @param j01 Homography jacobian coefficent at (ux,uy) + * @param j10 Homography jacobian coefficent at (ux,uy) + * @param j11 Homography jacobian coefficent at (ux,uy) + * @param p The x coordinate of point (ux,uy) mapped into the image (undistorted and normalized position) + * @param q The y coordinate of point (ux,uy) mapped into the image (undistorted and normalized position) + */ + void computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2); + + /** + * @brief Closed-form solution for the homography mapping with four corner correspondences of a square (it maps source points to target points). + * The source points are the four corners of a zero-centred squared defined by: + * - point 0: [-squareLength / 2.0, squareLength / 2.0] + * - point 1: [squareLength / 2.0, squareLength / 2.0] + * - point 2: [squareLength / 2.0, -squareLength / 2.0] + * - point 3: [-squareLength / 2.0, -squareLength / 2.0] + * + * @param targetPoints Array of four corresponding target points, 1x4/4x1 2-channel. Note that the points should be ordered to correspond with points 0, 1, 2 and 3. + * @param halfLength The square's half length (i.e. squareLength/2.0) + * @param H Homograhy mapping the source points to the target points, 3x3 single channel + */ + void homographyFromSquarePoints(InputArray targetPoints, double halfLength, OutputArray H); + + /** + * @brief Fast conversion from a rotation matrix to a rotation vector using Rodrigues' formula + * @param R Input rotation matrix, 3x3 1-channel (double) + * @param r Output rotation vector, 3x1/1x3 1-channel (double) + */ + void rot2vec(InputArray R, OutputArray r); + + /** + * @brief Takes a set of planar object points and transforms them to 'canonical' object coordinates This is when they have zero mean and are on the plane z=0 + * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param canonicalObjectPoints Object points in canonical coordinates 1xN/Nx1 2-channel (double) + * @param MobjectPoints2Canonical Transform matrix mapping _objectPoints to _canonicalObjectPoints: 4x4 1-channel (double) + */ + void makeCanonicalObjectPoints(InputArray objectPoints, OutputArray canonicalObjectPoints, OutputArray MobjectPoints2Canonical); + + /** + * @brief Evaluates the Root Mean Squared (RMS) reprojection error of a pose solution. + * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. + * @param M Pose matrix from 3D object to camera coordinates: 4x4 1-channel (double) + * @param err RMS reprojection error + */ + void evalReprojError(InputArray objectPoints, InputArray imagePoints, InputArray M, float& err); + + /** + * @brief Sorts two pose solutions according to their RMS reprojection error (lowest first). + * @param objectPoints Array of 4 or more coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param imagePoints Array of corresponding image points, 1xN/Nx1 2-channel. This can either be in pixel coordinates or normalized pixel coordinates. + * @param Ma Pose matrix 1: 4x4 1-channel + * @param Mb Pose matrix 2: 4x4 1-channel + * @param M1 Member of (Ma,Mb} with lowest RMS reprojection error. Performs deep copy. + * @param M2 Member of (Ma,Mb} with highest RMS reprojection error. Performs deep copy. + * @param err1 RMS reprojection error of _M1 + * @param err2 RMS reprojection error of _M2 + */ + void sortPosesByReprojError(InputArray objectPoints, InputArray imagePoints, InputArray Ma, InputArray Mb, OutputArray M1, OutputArray M2, float& err1, float& err2); + + /** + * @brief Finds the rotation _Ra that rotates a vector _a to the z axis (0,0,1) + * @param a vector: 3x1 mat (double) + * @param Ra Rotation: 3x3 mat (double) + */ + void rotateVec2ZAxis(const Matx31d& a, Matx33d& Ra); + + /** + * @brief Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points. + * @param objectPoints Array of N>=3 coplanar object points defined in object coordinates. 1xN/Nx1 3-channel (float or double) where N is the number of points + * @param R Rotation Mat: 3x3 (double) + * @return Success (true) or failure (false) + */ + bool computeObjextSpaceR3Pts(InputArray objectPoints, Matx33d& R); + + /** + * @brief computeObjextSpaceRSvD Computes the rotation _R that rotates the object points to the plane z=0. This uses the cross-product method with the first three object points. + * @param objectPointsZeroMean Zero-meaned coplanar object points: 3xN matrix (double) where N>=3 + * @param R Rotation Mat: 3x3 (double) + */ + void computeObjextSpaceRSvD(InputArray objectPointsZeroMean, OutputArray R); + + /** + * @brief Generates the 4 object points of a square planar object + * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.) + * @param objectPoints Set of 4 object points (1x4 3-channel double) + */ + void generateSquareObjectCorners3D(double squareLength, OutputArray objectPoints); + + /** + * @brief Generates the 4 object points of a square planar object, without including the z-component (which is z=0 for all points). + * @param squareLength The square's length (which is also it's width) in object coordinate units (e.g. millimeters, meters, etc.) + * @param objectPoints Set of 4 object points (1x4 2-channel double) + */ + void generateSquareObjectCorners2D(double squareLength, OutputArray objectPoints); + + /** + * @brief Computes the average depth of an object given its pose in camera coordinates + * @param objectPoints: Object points defined in 3D object space + * @param rvec: Rotation component of pose + * @param tvec: Translation component of pose + * @return: average depth of the object + */ + double meanSceneDepth(InputArray objectPoints, InputArray rvec, InputArray tvec); + + //! a small constant used to test 'small' values close to zero. + double IPPE_SMALL; +}; +} //namespace IPPE + +namespace HomographyHO { + +/** +* @brief Computes the best-fitting homography matrix from source to target points using Harker and O'Leary's method: +* Harker, M., O'Leary, P., Computation of Homographies, Proceedings of the British Machine Vision Conference 2005, Oxford, England. +* This is not the author's implementation. +* @param srcPoints Array of source points: 1xN/Nx1 2-channel (float or double) where N is the number of points +* @param targPoints Array of target points: 1xN/Nx1 2-channel (float or double) +* @param H Homography from source to target: 3x3 1-channel (double) +*/ +void homographyHO(InputArray srcPoints, InputArray targPoints, Matx33d& H); + +/** +* @brief Performs data normalization before homography estimation. For details see Hartley, R., Zisserman, A., Multiple View Geometry in Computer Vision, +* Cambridge University Press, Cambridge, 2001 +* @param Data Array of source data points: 1xN/Nx1 2-channel (float or double) where N is the number of points +* @param DataN Normalized data points: 1xN/Nx1 2-channel (float or double) where N is the number of points +* @param T Homogeneous transform from source to normalized: 3x3 1-channel (double) +* @param Ti Homogeneous transform from normalized to source: 3x3 1-channel (double) +*/ +void normalizeDataIsotropic(InputArray Data, OutputArray DataN, OutputArray T, OutputArray Ti); + +} +} //namespace cv +#endif diff --git a/modules/calib3d/src/p3p.cpp b/modules/calib3d/src/p3p.cpp index 7521e6b167..8ee0f490c7 100644 --- a/modules/calib3d/src/p3p.cpp +++ b/modules/calib3d/src/p3p.cpp @@ -49,9 +49,11 @@ bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat else extract_points(opoints, ipoints, points); - bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5], - points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14], - points[15], points[16], points[17], points[18], points[19]); + bool result = solve(rotation_matrix, translation, + points[0], points[1], points[2], points[3], points[4], + points[5], points[6], points[7], points[8], points[9], + points[10], points[11], points[12], points[13], points[14], + points[15], points[16], points[17], points[18], points[19]); cv::Mat(3, 1, CV_64F, translation).copyTo(tvec); cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R); return result; @@ -75,10 +77,13 @@ int p3p::solve(std::vector& Rs, std::vector& tvecs, const cv:: else extract_points(opoints, ipoints, points); + const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4; int solutions = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5], points[6], points[7], points[8], points[9], - points[10], points[11], points[12], points[13], points[14]); + points[10], points[11], points[12], points[13], points[14], + points[15], points[16], points[17], points[18], points[19], + p4p); for (int i = 0; i < solutions; i++) { cv::Mat R, tvec; @@ -100,39 +105,27 @@ bool p3p::solve(double R[3][3], double t[3], { double Rs[4][3][3], ts[4][3]; - int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2); + const bool p4p = true; + int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p); if (n == 0) return false; - int ns = 0; - double min_reproj = 0; - for(int i = 0; i < n; i++) { - double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0]; - double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1]; - double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2]; - double mu3p = cx + fx * X3p / Z3p; - double mv3p = cy + fy * Y3p / Z3p; - double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3); - if (i == 0 || min_reproj > reproj) { - ns = i; - min_reproj = reproj; - } - } - for(int i = 0; i < 3; i++) { for(int j = 0; j < 3; j++) - R[i][j] = Rs[ns][i][j]; - t[i] = ts[ns][i]; + R[i][j] = Rs[0][i][j]; + t[i] = ts[0][i]; } return true; } int p3p::solve(double R[4][3][3], double t[4][3], - double mu0, double mv0, double X0, double Y0, double Z0, - double mu1, double mv1, double X1, double Y1, double Z1, - double mu2, double mv2, double X2, double Y2, double Z2) + double mu0, double mv0, double X0, double Y0, double Z0, + double mu1, double mv1, double X1, double Y1, double Z1, + double mu2, double mv2, double X2, double Y2, double Z2, + double mu3, double mv3, double X3, double Y3, double Z3, + bool p4p) { double mk0, mk1, mk2; double norm; @@ -152,6 +145,9 @@ int p3p::solve(double R[4][3][3], double t[4][3], norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1); mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2; + mu3 = inv_fx * mu3 - cx_fx; + mv3 = inv_fy * mv3 - cy_fy; + double distances[3]; distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) ); distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) ); @@ -167,6 +163,7 @@ int p3p::solve(double R[4][3][3], double t[4][3], int n = solve_for_lengths(lengths, distances, cosines); int nb_solutions = 0; + double reproj_errors[4]; for(int i = 0; i < n; i++) { double M_orig[3][3]; @@ -185,9 +182,29 @@ int p3p::solve(double R[4][3][3], double t[4][3], if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions])) continue; + if (p4p) { + double X3p = R[nb_solutions][0][0] * X3 + R[nb_solutions][0][1] * Y3 + R[nb_solutions][0][2] * Z3 + t[nb_solutions][0]; + double Y3p = R[nb_solutions][1][0] * X3 + R[nb_solutions][1][1] * Y3 + R[nb_solutions][1][2] * Z3 + t[nb_solutions][1]; + double Z3p = R[nb_solutions][2][0] * X3 + R[nb_solutions][2][1] * Y3 + R[nb_solutions][2][2] * Z3 + t[nb_solutions][2]; + double mu3p = X3p / Z3p; + double mv3p = Y3p / Z3p; + reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3); + } + nb_solutions++; } + if (p4p) { + //sort the solutions + for (int i = 1; i < nb_solutions; i++) { + for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) { + std::swap(reproj_errors[j], reproj_errors[j-1]); + std::swap(R[j], R[j-1]); + std::swap(t[j], t[j-1]); + } + } + } + return nb_solutions; } diff --git a/modules/calib3d/src/p3p.h b/modules/calib3d/src/p3p.h index 9c7f7ec987..93e867d479 100644 --- a/modules/calib3d/src/p3p.h +++ b/modules/calib3d/src/p3p.h @@ -15,7 +15,9 @@ class p3p int solve(double R[4][3][3], double t[4][3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1, double mv1, double X1, double Y1, double Z1, - double mu2, double mv2, double X2, double Y2, double Z2); + double mu2, double mv2, double X2, double Y2, double Z2, + double mu3, double mv3, double X3, double Y3, double Z3, + bool p4p); bool solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1, double mv1, double X1, double Y1, double Z1, @@ -36,7 +38,7 @@ class p3p { points.clear(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - points.resize(5*npoints); + points.resize(5*4); //resize vector to fit for p4p case for(int i = 0; i < npoints; i++) { points[i*5] = ipoints.at(i).x*fx + cx; @@ -45,6 +47,12 @@ class p3p points[i*5+3] = opoints.at(i).y; points[i*5+4] = opoints.at(i).z; } + //Fill vectors with unused values for p3p case + for (int i = npoints; i < 4; i++) { + for (int j = 0; j < 5; j++) { + points[i * 5 + j] = 0; + } + } } void init_inverse_parameters(); int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]); diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 82a1604e46..0fdca0d510 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -46,12 +46,44 @@ #include "epnp.h" #include "p3p.h" #include "ap3p.h" +#include "ippe.hpp" #include "opencv2/calib3d/calib3d_c.h" -#include - namespace cv { +#if defined _DEBUG || defined CV_STATIC_ANALYSIS +static bool isPlanarObjectPoints(InputArray _objectPoints, double threshold) +{ + CV_CheckType(_objectPoints.type(), _objectPoints.type() == CV_32FC3 || _objectPoints.type() == CV_64FC3, + "Type of _objectPoints must be CV_32FC3 or CV_64FC3"); + Mat objectPoints; + if (_objectPoints.type() == CV_32FC3) + { + _objectPoints.getMat().convertTo(objectPoints, CV_64F); + } + else + { + objectPoints = _objectPoints.getMat(); + } + + Scalar meanValues = mean(objectPoints); + int nbPts = objectPoints.checkVector(3, CV_64F); + Mat objectPointsCentred = objectPoints - meanValues; + objectPointsCentred = objectPointsCentred.reshape(1, nbPts); + + Mat w, u, vt; + Mat MM = objectPointsCentred.t() * objectPointsCentred; + SVDecomp(MM, w, u, vt); + + return (w.at(2) < w.at(1) * threshold); +} + +static bool approxEqual(double a, double b, double eps) +{ + return std::fabs(a-b) < eps; +} +#endif + void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs, InputArray rvec, InputArray tvec, float length, int thickness) { @@ -80,120 +112,24 @@ void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray d line(image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), thickness); } -bool solvePnP( InputArray _opoints, InputArray _ipoints, - InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) +bool solvePnP( InputArray opoints, InputArray ipoints, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags ) { CV_INSTRUMENT_REGION(); - Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); - int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) - && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); - - Mat rvec, tvec; - if( flags != SOLVEPNP_ITERATIVE ) - useExtrinsicGuess = false; - - if( useExtrinsicGuess ) - { - int rtype = _rvec.type(), ttype = _tvec.type(); - Size rsize = _rvec.size(), tsize = _tvec.size(); - CV_Assert( (rtype == CV_32F || rtype == CV_64F) && - (ttype == CV_32F || ttype == CV_64F) ); - CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) && - (tsize == Size(1, 3) || tsize == Size(3, 1)) ); - } - else - { - int mtype = CV_64F; - // use CV_32F if all PnP inputs are CV_32F and outputs are empty - if (_ipoints.depth() == _cameraMatrix.depth() && _ipoints.depth() == _opoints.depth() && - _rvec.empty() && _tvec.empty()) - mtype = _opoints.depth(); - - _rvec.create(3, 1, mtype); - _tvec.create(3, 1, mtype); - } - rvec = _rvec.getMat(); - tvec = _tvec.getMat(); - - Mat cameraMatrix0 = _cameraMatrix.getMat(); - Mat distCoeffs0 = _distCoeffs.getMat(); - Mat cameraMatrix = Mat_(cameraMatrix0); - Mat distCoeffs = Mat_(distCoeffs0); - bool result = false; - - if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) - { - Mat undistortedPoints; - undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); - epnp PnP(cameraMatrix, opoints, undistortedPoints); + vector rvecs, tvecs; + int solutions = solvePnPGeneric(opoints, ipoints, cameraMatrix, distCoeffs, rvecs, tvecs, useExtrinsicGuess, (SolvePnPMethod)flags, rvec, tvec); - Mat R; - PnP.compute_pose(R, tvec); - Rodrigues(R, rvec); - result = true; - } - else if (flags == SOLVEPNP_P3P) - { - CV_Assert( npoints == 4); - Mat undistortedPoints; - undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); - p3p P3Psolver(cameraMatrix); - - Mat R; - result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); - if (result) - Rodrigues(R, rvec); - } - else if (flags == SOLVEPNP_AP3P) - { - CV_Assert( npoints == 4); - Mat undistortedPoints; - undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); - ap3p P3Psolver(cameraMatrix); - - Mat R; - result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); - if (result) - Rodrigues(R, rvec); - } - else if (flags == SOLVEPNP_ITERATIVE) - { - CvMat c_objectPoints = cvMat(opoints), c_imagePoints = cvMat(ipoints); - CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs); - CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec); - cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, - (c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0, - &c_rvec, &c_tvec, useExtrinsicGuess ); - result = true; - } - /*else if (flags == SOLVEPNP_DLS) + if (solutions > 0) { - Mat undistortedPoints; - undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); - - dls PnP(opoints, undistortedPoints); - - Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); - bool result = PnP.compute_pose(R, tvec); - if (result) - Rodrigues(R, rvec); - return result; + int rdepth = rvec.empty() ? CV_64F : rvec.depth(); + int tdepth = tvec.empty() ? CV_64F : tvec.depth(); + rvecs[0].convertTo(rvec, rdepth); + tvecs[0].convertTo(tvec, tdepth); } - else if (flags == SOLVEPNP_UPNP) - { - upnp PnP(cameraMatrix, opoints, ipoints); - Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); - PnP.compute_pose(R, tvec); - Rodrigues(R, rvec); - return true; - }*/ - else - CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); - return result; + return solutions > 0; } class PnPRansacCallback CV_FINAL : public PointSetRegistrator::Callback @@ -258,10 +194,10 @@ public: }; bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, - InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, - int iterationsCount, float reprojectionError, double confidence, - OutputArray _inliers, int flags) + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, + int iterationsCount, float reprojectionError, double confidence, + OutputArray _inliers, int flags) { CV_INSTRUMENT_REGION(); @@ -410,7 +346,8 @@ int solveP3P( InputArray _opoints, InputArray _ipoints, Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - CV_Assert( npoints == 3 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); + CV_Assert( npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); + CV_Assert( npoints == 3 || npoints == 4 ); CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P ); Mat cameraMatrix0 = _cameraMatrix.getMat(); @@ -420,7 +357,7 @@ int solveP3P( InputArray _opoints, InputArray _ipoints, Mat undistortedPoints; undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); - std::vector Rs, ts; + std::vector Rs, ts, rvecs; int solutions = 0; if (flags == SOLVEPNP_P3P) @@ -438,19 +375,91 @@ int solveP3P( InputArray _opoints, InputArray _ipoints, return 0; } - if (_rvecs.needed()) { - _rvecs.create(solutions, 1, CV_64F); - } - - if (_tvecs.needed()) { - _tvecs.create(solutions, 1, CV_64F); + Mat objPts, imgPts; + opoints.convertTo(objPts, CV_64F); + ipoints.convertTo(imgPts, CV_64F); + if (imgPts.cols > 1) + { + imgPts = imgPts.reshape(1); + imgPts = imgPts.t(); } + else + imgPts = imgPts.reshape(1, 2*imgPts.rows); - for (int i = 0; i < solutions; i++) { + vector reproj_errors(solutions); + for (size_t i = 0; i < reproj_errors.size(); i++) + { Mat rvec; Rodrigues(Rs[i], rvec); - _tvecs.getMatRef(i) = ts[i]; - _rvecs.getMatRef(i) = rvec; + rvecs.push_back(rvec); + + Mat projPts; + projectPoints(objPts, rvec, ts[i], _cameraMatrix, _distCoeffs, projPts); + + projPts = projPts.reshape(1, 2*projPts.rows); + Mat err = imgPts - projPts; + + err = err.t() * err; + reproj_errors[i] = err.at(0,0); + } + + //sort the solutions + for (int i = 1; i < solutions; i++) + { + for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) + { + std::swap(reproj_errors[j], reproj_errors[j-1]); + std::swap(rvecs[j], rvecs[j-1]); + std::swap(ts[j], ts[j-1]); + } + } + + int depthRot = _rvecs.fixedType() ? _rvecs.depth() : CV_64F; + int depthTrans = _tvecs.fixedType() ? _tvecs.depth() : CV_64F; + _rvecs.create(solutions, 1, CV_MAKETYPE(depthRot, _rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); + _tvecs.create(solutions, 1, CV_MAKETYPE(depthTrans, _tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); + + for (int i = 0; i < solutions; i++) + { + Mat rvec0, tvec0; + if (depthRot == CV_64F) + rvec0 = rvecs[i]; + else + rvecs[i].convertTo(rvec0, depthRot); + + if (depthTrans == CV_64F) + tvec0 = ts[i]; + else + ts[i].convertTo(tvec0, depthTrans); + + if (_rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR) + { + Mat rref = _rvecs.getMat_(); + + if (_rvecs.depth() == CV_32F) + rref.at(0,i) = Vec3f(rvec0.at(0,0), rvec0.at(1,0), rvec0.at(2,0)); + else + rref.at(0,i) = Vec3d(rvec0.at(0,0), rvec0.at(1,0), rvec0.at(2,0)); + } + else + { + _rvecs.getMatRef(i) = rvec0; + } + + if (_tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR) + { + + Mat tref = _tvecs.getMat_(); + + if (_tvecs.depth() == CV_32F) + tref.at(0,i) = Vec3f(tvec0.at(0,0), tvec0.at(1,0), tvec0.at(2,0)); + else + tref.at(0,i) = Vec3d(tvec0.at(0,0), tvec0.at(1,0), tvec0.at(2,0)); + } + else + { + _tvecs.getMatRef(i) = tvec0; + } } return solutions; @@ -723,4 +732,314 @@ void solvePnPRefineVVS(InputArray _objectPoints, InputArray _imagePoints, solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_VVS, _criteria, _VVSlambda); } +int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, + bool useExtrinsicGuess, SolvePnPMethod flags, + InputArray _rvec, InputArray _tvec, + OutputArray reprojectionError) { + CV_INSTRUMENT_REGION(); + + Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); + int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); + CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) + && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); + + if( flags != SOLVEPNP_ITERATIVE ) + useExtrinsicGuess = false; + + if (useExtrinsicGuess) + CV_Assert( !_rvec.empty() && !_tvec.empty() ); + + if( useExtrinsicGuess ) + { + 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_(cameraMatrix0); + Mat distCoeffs = Mat_(distCoeffs0); + + vector vec_rvecs, vec_tvecs; + if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) + { + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + epnp PnP(cameraMatrix, opoints, undistortedPoints); + + Mat rvec, tvec, R; + PnP.compute_pose(R, tvec); + Rodrigues(R, rvec); + + vec_rvecs.push_back(rvec); + vec_tvecs.push_back(tvec); + } + else if (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) + { + vector rvecs, tvecs; + solveP3P(_opoints, _ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags); + vec_rvecs.insert(vec_rvecs.end(), rvecs.begin(), rvecs.end()); + vec_tvecs.insert(vec_tvecs.end(), tvecs.begin(), tvecs.end()); + } + else if (flags == SOLVEPNP_ITERATIVE) + { + Mat rvec, tvec; + if (useExtrinsicGuess) + { + rvec = _rvec.getMat(); + tvec = _tvec.getMat(); + } + else + { + rvec.create(3, 1, CV_64FC1); + tvec.create(3, 1, CV_64FC1); + } + + CvMat c_objectPoints = cvMat(opoints), c_imagePoints = cvMat(ipoints); + CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs); + CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec); + cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, + (c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0, + &c_rvec, &c_tvec, useExtrinsicGuess ); + + vec_rvecs.push_back(rvec); + vec_tvecs.push_back(tvec); + } + else if (flags == SOLVEPNP_IPPE) + { + CV_DbgAssert(isPlanarObjectPoints(opoints, 1e-3)); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + + IPPE::PoseSolver poseSolver; + Mat rvec1, tvec1, rvec2, tvec2; + float reprojErr1, reprojErr2; + try + { + poseSolver.solveGeneric(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2); + + if (reprojErr1 < reprojErr2) + { + vec_rvecs.push_back(rvec1); + vec_tvecs.push_back(tvec1); + + vec_rvecs.push_back(rvec2); + vec_tvecs.push_back(tvec2); + } + else + { + vec_rvecs.push_back(rvec2); + vec_tvecs.push_back(tvec2); + + vec_rvecs.push_back(rvec1); + vec_tvecs.push_back(tvec1); + } + } + catch (...) { } + } + else if (flags == SOLVEPNP_IPPE_SQUARE) + { + CV_Assert(npoints == 4); + +#if defined _DEBUG || defined CV_STATIC_ANALYSIS + double Xs[4][3]; + if (opoints.depth() == CV_32F) + { + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 3; j++) + { + Xs[i][j] = opoints.ptr(0)[i](j); + } + } + } + else + { + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 3; j++) + { + Xs[i][j] = opoints.ptr(0)[i](j); + } + } + } + + const double equalThreshold = 1e-9; + //Z must be zero + for (int i = 0; i < 4; i++) + { + CV_DbgCheck(Xs[i][2], approxEqual(Xs[i][2], 0, equalThreshold), "Z object point coordinate must be zero!"); + } + //Y0 == Y1 && Y2 == Y3 + CV_DbgCheck(Xs[0][1], approxEqual(Xs[0][1], Xs[1][1], equalThreshold), "Object points must be: Y0 == Y1!"); + CV_DbgCheck(Xs[2][1], approxEqual(Xs[2][1], Xs[3][1], equalThreshold), "Object points must be: Y2 == Y3!"); + //X0 == X3 && X1 == X2 + CV_DbgCheck(Xs[0][0], approxEqual(Xs[0][0], Xs[3][0], equalThreshold), "Object points must be: X0 == X3!"); + CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[2][0], equalThreshold), "Object points must be: X1 == X2!"); + //X1 == Y1 && X3 == Y3 + CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[1][1], equalThreshold), "Object points must be: X1 == Y1!"); + CV_DbgCheck(Xs[3][0], approxEqual(Xs[3][0], Xs[3][1], equalThreshold), "Object points must be: X3 == Y3!"); +#endif + + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + + IPPE::PoseSolver poseSolver; + Mat rvec1, tvec1, rvec2, tvec2; + float reprojErr1, reprojErr2; + try + { + poseSolver.solveSquare(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2); + + if (reprojErr1 < reprojErr2) + { + vec_rvecs.push_back(rvec1); + vec_tvecs.push_back(tvec1); + + vec_rvecs.push_back(rvec2); + vec_tvecs.push_back(tvec2); + } + else + { + vec_rvecs.push_back(rvec2); + vec_tvecs.push_back(tvec2); + + vec_rvecs.push_back(rvec1); + vec_tvecs.push_back(tvec1); + } + } catch (...) { } + } + /*else if (flags == SOLVEPNP_DLS) + { + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + + dls PnP(opoints, undistortedPoints); + + Mat rvec, tvec, R; + bool result = PnP.compute_pose(R, tvec); + if (result) + { + Rodrigues(R, rvec); + vec_rvecs.push_back(rvec); + vec_tvecs.push_back(tvec); + } + } + else if (flags == SOLVEPNP_UPNP) + { + upnp PnP(cameraMatrix, opoints, ipoints); + + Mat rvec, tvec, R; + PnP.compute_pose(R, tvec); + Rodrigues(R, rvec); + vec_rvecs.push_back(rvec); + vec_tvecs.push_back(tvec); + }*/ + else + CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); + + CV_Assert(vec_rvecs.size() == vec_tvecs.size()); + + int solutions = static_cast(vec_rvecs.size()); + + int depthRot = _rvecs.fixedType() ? _rvecs.depth() : CV_64F; + int depthTrans = _tvecs.fixedType() ? _tvecs.depth() : CV_64F; + _rvecs.create(solutions, 1, CV_MAKETYPE(depthRot, _rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); + _tvecs.create(solutions, 1, CV_MAKETYPE(depthTrans, _tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1)); + + for (int i = 0; i < solutions; i++) + { + Mat rvec0, tvec0; + if (depthRot == CV_64F) + rvec0 = vec_rvecs[i]; + else + vec_rvecs[i].convertTo(rvec0, depthRot); + + if (depthTrans == CV_64F) + tvec0 = vec_tvecs[i]; + else + vec_tvecs[i].convertTo(tvec0, depthTrans); + + if (_rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR) + { + Mat rref = _rvecs.getMat_(); + + if (_rvecs.depth() == CV_32F) + rref.at(0,i) = Vec3f(rvec0.at(0,0), rvec0.at(1,0), rvec0.at(2,0)); + else + rref.at(0,i) = Vec3d(rvec0.at(0,0), rvec0.at(1,0), rvec0.at(2,0)); + } + else + { + _rvecs.getMatRef(i) = rvec0; + } + + if (_tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR) + { + + Mat tref = _tvecs.getMat_(); + + if (_tvecs.depth() == CV_32F) + tref.at(0,i) = Vec3f(tvec0.at(0,0), tvec0.at(1,0), tvec0.at(2,0)); + else + tref.at(0,i) = Vec3d(tvec0.at(0,0), tvec0.at(1,0), tvec0.at(2,0)); + } + else + { + _tvecs.getMatRef(i) = tvec0; + } + } + + if (reprojectionError.needed()) + { + int type = reprojectionError.type(); + reprojectionError.create(solutions, 1, type); + CV_CheckType(reprojectionError.type(), type == CV_32FC1 || type == CV_64FC1, + "Type of reprojectionError must be CV_32FC1 or CV_64FC1!"); + + Mat objectPoints, imagePoints; + if (_opoints.depth() == CV_32F) + { + _opoints.getMat().convertTo(objectPoints, CV_64F); + } + else + { + objectPoints = _opoints.getMat(); + } + if (_ipoints.depth() == CV_32F) + { + _ipoints.getMat().convertTo(imagePoints, CV_64F); + } + else + { + imagePoints = _ipoints.getMat(); + } + + for (size_t i = 0; i < vec_rvecs.size(); i++) + { + vector projectedPoints; + projectPoints(objectPoints, vec_rvecs[i], vec_tvecs[i], cameraMatrix, distCoeffs, projectedPoints); + double rmse = norm(projectedPoints, imagePoints, NORM_L2) / sqrt(2*projectedPoints.size()); + + Mat err = reprojectionError.getMat(); + if (type == CV_32F) + { + err.at(0,static_cast(i)) = static_cast(rmse); + } + else + { + err.at(0,static_cast(i)) = rmse; + } + } + } + + return solutions; +} + } diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index adf7758c92..77a5d5df8d 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -44,10 +44,161 @@ namespace opencv_test { namespace { +//Statistics Helpers +struct ErrorInfo +{ + ErrorInfo(double errT, double errR) : errorTrans(errT), errorRot(errR) + { + } + + bool operator<(const ErrorInfo& e) const + { + return sqrt(errorTrans*errorTrans + errorRot*errorRot) < + sqrt(e.errorTrans*e.errorTrans + e.errorRot*e.errorRot); + } + + double errorTrans; + double errorRot; +}; + +//Try to find the translation and rotation thresholds to achieve a predefined percentage of success. +//Since a success is defined by error_trans < trans_thresh && error_rot < rot_thresh +//this just gives an idea of the values to use +static void findThreshold(const std::vector& v_trans, const std::vector& v_rot, double percentage, + double& transThresh, double& rotThresh) +{ + if (v_trans.empty() || v_rot.empty() || v_trans.size() != v_rot.size()) + { + transThresh = -1; + rotThresh = -1; + return; + } + + std::vector error_info; + error_info.reserve(v_trans.size()); + for (size_t i = 0; i < v_trans.size(); i++) + { + error_info.push_back(ErrorInfo(v_trans[i], v_rot[i])); + } + + std::sort(error_info.begin(), error_info.end()); + size_t idx = static_cast(error_info.size() * percentage); + transThresh = error_info[idx].errorTrans; + rotThresh = error_info[idx].errorRot; +} + +static double getMax(const std::vector& v) +{ + return *std::max_element(v.begin(), v.end()); +} + +static double getMean(const std::vector& v) +{ + if (v.empty()) + { + return 0.0; + } + + double sum = std::accumulate(v.begin(), v.end(), 0.0); + return sum / v.size(); +} + +static double getMedian(const std::vector& v) +{ + if (v.empty()) + { + return 0.0; + } + + std::vector v_copy = v; + size_t size = v_copy.size(); + + size_t n = size / 2; + std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end()); + double val_n = v_copy[n]; + + if (size % 2 == 1) + { + return val_n; + } else + { + std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end()); + return 0.5 * (val_n + v_copy[n - 1]); + } +} + +static void generatePose(const vector& points, Mat& rvec, Mat& tvec, RNG& rng, int nbTrials=10) +{ + const double minVal = 1.0e-3; + const double maxVal = 1.0; + rvec.create(3, 1, CV_64FC1); + tvec.create(3, 1, CV_64FC1); + + bool validPose = false; + for (int trial = 0; trial < nbTrials && !validPose; trial++) + { + for (int i = 0; i < 3; i++) + { + rvec.at(i,0) = rng.uniform(minVal, maxVal); + tvec.at(i,0) = (i == 2) ? rng.uniform(minVal*10, maxVal) : rng.uniform(-maxVal, maxVal); + } + + Mat R; + cv::Rodrigues(rvec, R); + bool positiveDepth = true; + for (size_t i = 0; i < points.size() && positiveDepth; i++) + { + Matx31d objPts(points[i].x, points[i].y, points[i].z); + Mat camPts = R*objPts + tvec; + if (camPts.at(2,0) <= 0) + { + positiveDepth = false; + } + } + validPose = positiveDepth; + } +} + +static void generatePose(const vector& points, Mat& rvec, Mat& tvec, RNG& rng, int nbTrials=10) +{ + vector points_double(points.size()); + + for (size_t i = 0; i < points.size(); i++) + { + points_double[i] = Point3d(points[i].x, points[i].y, points[i].z); + } + + generatePose(points_double, rvec, tvec, rng, nbTrials); +} + +static std::string printMethod(int method) +{ + switch (method) { + case 0: + return "SOLVEPNP_ITERATIVE"; + case 1: + return "SOLVEPNP_EPNP"; + case 2: + return "SOLVEPNP_P3P"; + case 3: + return "SOLVEPNP_DLS (remaped to SOLVEPNP_EPNP)"; + case 4: + return "SOLVEPNP_UPNP (remaped to SOLVEPNP_EPNP)"; + case 5: + return "SOLVEPNP_AP3P"; + case 6: + return "SOLVEPNP_IPPE"; + case 7: + return "SOLVEPNP_IPPE_SQUARE"; + default: + return "Unknown value"; + } +} + class CV_solvePnPRansac_Test : public cvtest::BaseTest { public: - CV_solvePnPRansac_Test() + CV_solvePnPRansac_Test(bool planar_=false, bool planarTag_=false) : planar(planar_), planarTag(planarTag_) { eps[SOLVEPNP_ITERATIVE] = 1.0e-2; eps[SOLVEPNP_EPNP] = 1.0e-2; @@ -61,10 +212,10 @@ public: ~CV_solvePnPRansac_Test() {} protected: void generate3DPointCloud(vector& points, - Point3f pmin = Point3f(-1, -1, 5), - Point3f pmax = Point3f(1, 1, 10)) + Point3f pmin = Point3f(-1, -1, 5), + Point3f pmax = Point3f(1, 1, 10)) { - RNG rng = cv::theRNG(); // fix the seed to use "fixed" input 3D points + RNG& rng = theRNG(); // fix the seed to use "fixed" input 3D points for (size_t i = 0; i < points.size(); i++) { @@ -75,6 +226,44 @@ protected: } } + void generatePlanarPointCloud(vector& points, + Point2f pmin = Point2f(-1, -1), + Point2f pmax = Point2f(1, 1)) + { + RNG& rng = theRNG(); // fix the seed to use "fixed" input 3D points + + if (planarTag) + { + const float squareLength_2 = rng.uniform(0.01f, pmax.x) / 2; + points.clear(); + points.push_back(Point3f(-squareLength_2, squareLength_2, 0)); + points.push_back(Point3f(squareLength_2, squareLength_2, 0)); + points.push_back(Point3f(squareLength_2, -squareLength_2, 0)); + points.push_back(Point3f(-squareLength_2, -squareLength_2, 0)); + } + else + { + Mat rvec_double, tvec_double; + generatePose(points, rvec_double, tvec_double, rng); + + Mat rvec, tvec, R; + rvec_double.convertTo(rvec, CV_32F); + tvec_double.convertTo(tvec, CV_32F); + cv::Rodrigues(rvec, R); + + for (size_t i = 0; i < points.size(); i++) + { + float x = rng.uniform(pmin.x, pmax.x); + float y = rng.uniform(pmin.y, pmax.y); + float z = 0; + + Matx31f pt(x, y, z); + Mat pt_trans = R * pt + tvec; + points[i] = Point3f(pt_trans.at(0,0), pt_trans.at(1,0), pt_trans.at(2,0)); + } + } + } + void generateCameraMatrix(Mat& cameraMatrix, RNG& rng) { const double fcMinVal = 1e-3; @@ -95,32 +284,34 @@ protected: distCoeffs.at(i,0) = rng.uniform(0.0, 1.0e-6); } - void generatePose(Mat& rvec, Mat& tvec, RNG& rng) + virtual bool runTest(RNG& rng, int mode, int method, const vector& points, double& errorTrans, double& errorRot) { - const double minVal = 1.0e-3; - const double maxVal = 1.0; - rvec.create(3, 1, CV_64FC1); - tvec.create(3, 1, CV_64FC1); - for (int i = 0; i < 3; i++) + if ((!planar && method == SOLVEPNP_IPPE) || method == SOLVEPNP_IPPE_SQUARE) { - rvec.at(i,0) = rng.uniform(minVal, maxVal); - tvec.at(i,0) = rng.uniform(minVal, maxVal/10); + return true; } - } - virtual bool runTest(RNG& rng, int mode, int method, const vector& points, const double* epsilon, double& maxError) - { Mat rvec, tvec; vector inliers; Mat trueRvec, trueTvec; Mat intrinsics, distCoeffs; generateCameraMatrix(intrinsics, rng); - if (method == 4) intrinsics.at(1,1) = intrinsics.at(0,0); + //UPnP is mapped to EPnP + //Uncomment this when UPnP is fixed +// if (method == SOLVEPNP_UPNP) +// { +// intrinsics.at(1,1) = intrinsics.at(0,0); +// } if (mode == 0) + { distCoeffs = Mat::zeros(4, 1, CV_64FC1); + } else + { generateDistCoeffs(distCoeffs, rng); - generatePose(trueRvec, trueTvec, rng); + } + + generatePose(points, trueRvec, trueTvec, rng); vector projectedPoints; projectedPoints.resize(points.size()); @@ -138,11 +329,9 @@ protected: bool isTestSuccess = inliers.size() >= points.size()*0.95; double rvecDiff = cvtest::norm(rvec, trueRvec, NORM_L2), tvecDiff = cvtest::norm(tvec, trueTvec, NORM_L2); - isTestSuccess = isTestSuccess && rvecDiff < epsilon[method] && tvecDiff < epsilon[method]; - double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff; - //cout << error << " " << inliers.size() << " " << eps[method] << endl; - if (error > maxError) - maxError = error; + isTestSuccess = isTestSuccess && rvecDiff < eps[method] && tvecDiff < eps[method]; + errorTrans = tvecDiff; + errorRot = rvecDiff; return isTestSuccess; } @@ -152,68 +341,184 @@ protected: ts->set_failed_test_info(cvtest::TS::OK); vector points, points_dls; - points.resize(pointsCount); - generate3DPointCloud(points); + points.resize(static_cast(pointsCount)); - RNG rng = ts->get_rng(); + if (planar || planarTag) + { + generatePlanarPointCloud(points); + } + else + { + generate3DPointCloud(points); + } + RNG& rng = ts->get_rng(); for (int mode = 0; mode < 2; mode++) { for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++) { - double maxError = 0; + //To get the same input for each methods + RNG rngCopy = rng; + std::vector vec_errorTrans, vec_errorRot; + vec_errorTrans.reserve(static_cast(totalTestsCount)); + vec_errorRot.reserve(static_cast(totalTestsCount)); + int successfulTestsCount = 0; for (int testIndex = 0; testIndex < totalTestsCount; testIndex++) { - if (runTest(rng, mode, method, points, eps, maxError)) + double errorTrans, errorRot; + if (runTest(rngCopy, mode, method, points, errorTrans, errorRot)) { successfulTestsCount++; } + vec_errorTrans.push_back(errorTrans); + vec_errorRot.push_back(errorRot); } + + double maxErrorTrans = getMax(vec_errorTrans); + double maxErrorRot = getMax(vec_errorRot); + double meanErrorTrans = getMean(vec_errorTrans); + double meanErrorRot = getMean(vec_errorRot); + double medianErrorTrans = getMedian(vec_errorTrans); + double medianErrorRot = getMedian(vec_errorRot); + if (successfulTestsCount < 0.7*totalTestsCount) { - ts->printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n", - method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode); + ts->printf(cvtest::TS::LOG, "Invalid accuracy for %s, failed %d tests from %d, %s, " + "maxErrT: %f, maxErrR: %f, " + "meanErrT: %f, meanErrR: %f, " + "medErrT: %f, medErrR: %f\n", + printMethod(method).c_str(), totalTestsCount - successfulTestsCount, totalTestsCount, printMode(mode).c_str(), + maxErrorTrans, maxErrorRot, meanErrorTrans, meanErrorRot, medianErrorTrans, medianErrorRot); ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); } - cout << "mode: " << mode << ", method: " << method << " -> " + cout << "mode: " << printMode(mode) << ", method: " << printMethod(method) << " -> " << ((double)successfulTestsCount / totalTestsCount) * 100 << "%" - << " (err < " << maxError << ")" << endl; + << " (maxErrT: " << maxErrorTrans << ", maxErrR: " << maxErrorRot + << ", meanErrT: " << meanErrorTrans << ", meanErrR: " << meanErrorRot + << ", medErrT: " << medianErrorTrans << ", medErrR: " << medianErrorRot << ")" << endl; + double transThres, rotThresh; + findThreshold(vec_errorTrans, vec_errorRot, 0.7, transThres, rotThresh); + cout << "approximate translation threshold for 0.7: " << transThres + << ", approximate rotation threshold for 0.7: " << rotThresh << endl; } + cout << endl; + } + } + std::string printMode(int mode) + { + switch (mode) { + case 0: + return "no distortion"; + case 1: + default: + return "distorsion"; } } double eps[SOLVEPNP_MAX_COUNT]; int totalTestsCount; int pointsCount; + bool planar; + bool planarTag; }; class CV_solvePnP_Test : public CV_solvePnPRansac_Test { public: - CV_solvePnP_Test() + CV_solvePnP_Test(bool planar_=false, bool planarTag_=false) : CV_solvePnPRansac_Test(planar_, planarTag_) { eps[SOLVEPNP_ITERATIVE] = 1.0e-6; eps[SOLVEPNP_EPNP] = 1.0e-6; eps[SOLVEPNP_P3P] = 2.0e-4; eps[SOLVEPNP_AP3P] = 1.0e-4; - eps[SOLVEPNP_DLS] = 1.0e-4; - eps[SOLVEPNP_UPNP] = 1.0e-4; + eps[SOLVEPNP_DLS] = 1.0e-6; //DLS is remapped to EPnP, so we use the same threshold + eps[SOLVEPNP_UPNP] = 1.0e-6; //UPnP is remapped to EPnP, so we use the same threshold + eps[SOLVEPNP_IPPE] = 1.0e-6; + eps[SOLVEPNP_IPPE_SQUARE] = 1.0e-6; + totalTestsCount = 1000; + + if (planar || planarTag) + { + if (planarTag) + { + pointsCount = 4; + } + else + { + pointsCount = 30; + } + } + else + { + pointsCount = 500; + } } ~CV_solvePnP_Test() {} protected: - virtual bool runTest(RNG& rng, int mode, int method, const vector& points, const double* epsilon, double& maxError) + virtual bool runTest(RNG& rng, int mode, int method, const vector& points, double& errorTrans, double& errorRot) { - Mat rvec, tvec; + if ((!planar && (method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE)) || + (!planarTag && method == SOLVEPNP_IPPE_SQUARE)) + { + errorTrans = -1; + errorRot = -1; + //SOLVEPNP_IPPE and SOLVEPNP_IPPE_SQUARE need planar object + return true; + } + + //Tune thresholds... + double epsilon_trans[SOLVEPNP_MAX_COUNT]; + memcpy(epsilon_trans, eps, SOLVEPNP_MAX_COUNT * sizeof(*epsilon_trans)); + + double epsilon_rot[SOLVEPNP_MAX_COUNT]; + memcpy(epsilon_rot, eps, SOLVEPNP_MAX_COUNT * sizeof(*epsilon_rot)); + + if (planar) + { + if (mode == 0) + { + epsilon_trans[SOLVEPNP_EPNP] = 5.0e-3; + epsilon_trans[SOLVEPNP_DLS] = 5.0e-3; + epsilon_trans[SOLVEPNP_UPNP] = 5.0e-3; + + epsilon_rot[SOLVEPNP_EPNP] = 5.0e-3; + epsilon_rot[SOLVEPNP_DLS] = 5.0e-3; + epsilon_rot[SOLVEPNP_UPNP] = 5.0e-3; + } + else + { + epsilon_trans[SOLVEPNP_ITERATIVE] = 1e-4; + epsilon_trans[SOLVEPNP_EPNP] = 5e-3; + epsilon_trans[SOLVEPNP_DLS] = 5e-3; + epsilon_trans[SOLVEPNP_UPNP] = 5e-3; + epsilon_trans[SOLVEPNP_P3P] = 1e-4; + epsilon_trans[SOLVEPNP_AP3P] = 1e-4; + epsilon_trans[SOLVEPNP_IPPE] = 1e-4; + epsilon_trans[SOLVEPNP_IPPE_SQUARE] = 1e-4; + + epsilon_rot[SOLVEPNP_ITERATIVE] = 1e-4; + epsilon_rot[SOLVEPNP_EPNP] = 5e-3; + epsilon_rot[SOLVEPNP_DLS] = 5e-3; + epsilon_rot[SOLVEPNP_UPNP] = 5e-3; + epsilon_rot[SOLVEPNP_P3P] = 1e-4; + epsilon_rot[SOLVEPNP_AP3P] = 1e-4; + epsilon_rot[SOLVEPNP_IPPE] = 1e-4; + epsilon_rot[SOLVEPNP_IPPE_SQUARE] = 1e-4; + } + } + Mat trueRvec, trueTvec; Mat intrinsics, distCoeffs; generateCameraMatrix(intrinsics, rng); - if (method == SOLVEPNP_DLS) - { - intrinsics.at(1,1) = intrinsics.at(0,0); - } + //UPnP is mapped to EPnP + //Uncomment this when UPnP is fixed +// if (method == SOLVEPNP_UPNP) +// { +// intrinsics.at(1,1) = intrinsics.at(0,0); +// } if (mode == 0) { distCoeffs = Mat::zeros(4, 1, CV_64FC1); @@ -222,7 +527,8 @@ protected: { generateDistCoeffs(distCoeffs, rng); } - generatePose(trueRvec, trueTvec, rng); + + generatePose(points, trueRvec, trueTvec, rng); std::vector opoints; switch(method) @@ -231,9 +537,18 @@ protected: case SOLVEPNP_AP3P: opoints = std::vector(points.begin(), points.begin()+4); break; - case SOLVEPNP_UPNP: - opoints = std::vector(points.begin(), points.begin()+50); - break; + //UPnP is mapped to EPnP + //Uncomment this when UPnP is fixed +// case SOLVEPNP_UPNP: +// if (points.size() > 50) +// { +// opoints = std::vector(points.begin(), points.begin()+50); +// } +// else +// { +// opoints = points; +// } +// break; default: opoints = points; break; @@ -243,20 +558,19 @@ protected: projectedPoints.resize(opoints.size()); projectPoints(opoints, trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints); + Mat rvec, tvec; bool isEstimateSuccess = solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec, false, method); - if (isEstimateSuccess == false) + + if (!isEstimateSuccess) { - return isEstimateSuccess; + return false; } double rvecDiff = cvtest::norm(rvec, trueRvec, NORM_L2), tvecDiff = cvtest::norm(tvec, trueTvec, NORM_L2); - bool isTestSuccess = rvecDiff < epsilon[method] && tvecDiff < epsilon[method]; + bool isTestSuccess = rvecDiff < epsilon_rot[method] && tvecDiff < epsilon_trans[method]; - double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff; - if (error > maxError) - { - maxError = error; - } + errorTrans = tvecDiff; + errorRot = rvecDiff; return isTestSuccess; } @@ -264,95 +578,129 @@ protected: class CV_solveP3P_Test : public CV_solvePnPRansac_Test { - public: - CV_solveP3P_Test() - { - eps[SOLVEPNP_P3P] = 2.0e-4; - eps[SOLVEPNP_AP3P] = 1.0e-4; - totalTestsCount = 1000; - } - - ~CV_solveP3P_Test() {} - protected: - virtual bool runTest(RNG& rng, int mode, int method, const vector& points, const double* epsilon, double& maxError) - { - std::vector rvecs, tvecs; - Mat trueRvec, trueTvec; - Mat intrinsics, distCoeffs; - generateCameraMatrix(intrinsics, rng); - if (mode == 0) - distCoeffs = Mat::zeros(4, 1, CV_64FC1); - else - generateDistCoeffs(distCoeffs, rng); - generatePose(trueRvec, trueTvec, rng); - - std::vector opoints; - opoints = std::vector(points.begin(), points.begin()+3); - - vector projectedPoints; - projectedPoints.resize(opoints.size()); - projectPoints(opoints, trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints); - - int num_of_solutions = solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, method); - if (num_of_solutions != (int) rvecs.size() || num_of_solutions != (int) tvecs.size() || num_of_solutions == 0) - return false; - - bool isTestSuccess = false; - double error = DBL_MAX; - for (unsigned int i = 0; i < rvecs.size() && !isTestSuccess; ++i) { - double rvecDiff = cvtest::norm(rvecs[i], trueRvec, NORM_L2); - double tvecDiff = cvtest::norm(tvecs[i], trueTvec, NORM_L2); - isTestSuccess = rvecDiff < epsilon[method] && tvecDiff < epsilon[method]; - error = std::min(error, std::max(rvecDiff, tvecDiff)); - } - - if (error > maxError) - maxError = error; - - return isTestSuccess; - } - - virtual void run(int) - { - ts->set_failed_test_info(cvtest::TS::OK); - - vector points; - points.resize(pointsCount); - generate3DPointCloud(points); - - const int methodsCount = 2; - int methods[] = {SOLVEPNP_P3P, SOLVEPNP_AP3P}; - RNG rng = ts->get_rng(); - - for (int mode = 0; mode < 2; mode++) - { - for (int method = 0; method < methodsCount; method++) - { - double maxError = 0; - int successfulTestsCount = 0; - for (int testIndex = 0; testIndex < totalTestsCount; testIndex++) - { - if (runTest(rng, mode, methods[method], points, eps, maxError)) - successfulTestsCount++; - } - if (successfulTestsCount < 0.7*totalTestsCount) - { - ts->printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n", - method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode); - ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); - } - cout << "mode: " << mode << ", method: " << method << " -> " - << ((double)successfulTestsCount / totalTestsCount) * 100 << "%" - << " (err < " << maxError << ")" << endl; - } - } - } +public: + CV_solveP3P_Test() + { + eps[SOLVEPNP_P3P] = 2.0e-4; + eps[SOLVEPNP_AP3P] = 1.0e-4; + totalTestsCount = 1000; + } + + ~CV_solveP3P_Test() {} +protected: + virtual bool runTest(RNG& rng, int mode, int method, const vector& points, double& errorTrans, double& errorRot) + { + std::vector rvecs, tvecs; + Mat trueRvec, trueTvec; + Mat intrinsics, distCoeffs; + generateCameraMatrix(intrinsics, rng); + if (mode == 0) + { + distCoeffs = Mat::zeros(4, 1, CV_64FC1); + } + else + { + generateDistCoeffs(distCoeffs, rng); + } + generatePose(points, trueRvec, trueTvec, rng); + + std::vector opoints; + opoints = std::vector(points.begin(), points.begin()+3); + + vector projectedPoints; + projectedPoints.resize(opoints.size()); + projectPoints(opoints, trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints); + + int num_of_solutions = solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, method); + if (num_of_solutions != (int) rvecs.size() || num_of_solutions != (int) tvecs.size() || num_of_solutions == 0) + { + return false; + } + + bool isTestSuccess = false; + for (size_t i = 0; i < rvecs.size() && !isTestSuccess; i++) { + double rvecDiff = cvtest::norm(rvecs[i], trueRvec, NORM_L2); + double tvecDiff = cvtest::norm(tvecs[i], trueTvec, NORM_L2); + isTestSuccess = rvecDiff < eps[method] && tvecDiff < eps[method]; + + errorTrans = std::min(errorTrans, tvecDiff); + errorRot = std::min(errorRot, rvecDiff); + } + + return isTestSuccess; + } + + virtual void run(int) + { + ts->set_failed_test_info(cvtest::TS::OK); + + vector points; + points.resize(static_cast(pointsCount)); + generate3DPointCloud(points); + + const int methodsCount = 2; + int methods[] = {SOLVEPNP_P3P, SOLVEPNP_AP3P}; + RNG rng = ts->get_rng(); + + for (int mode = 0; mode < 2; mode++) + { + //To get the same input for each methods + RNG rngCopy = rng; + for (int method = 0; method < methodsCount; method++) + { + std::vector vec_errorTrans, vec_errorRot; + vec_errorTrans.reserve(static_cast(totalTestsCount)); + vec_errorRot.reserve(static_cast(totalTestsCount)); + + int successfulTestsCount = 0; + for (int testIndex = 0; testIndex < totalTestsCount; testIndex++) + { + double errorTrans = 0, errorRot = 0; + if (runTest(rngCopy, mode, methods[method], points, errorTrans, errorRot)) + { + successfulTestsCount++; + } + vec_errorTrans.push_back(errorTrans); + vec_errorRot.push_back(errorRot); + } + + double maxErrorTrans = getMax(vec_errorTrans); + double maxErrorRot = getMax(vec_errorRot); + double meanErrorTrans = getMean(vec_errorTrans); + double meanErrorRot = getMean(vec_errorRot); + double medianErrorTrans = getMedian(vec_errorTrans); + double medianErrorRot = getMedian(vec_errorRot); + + if (successfulTestsCount < 0.7*totalTestsCount) + { + ts->printf(cvtest::TS::LOG, "Invalid accuracy for %s, failed %d tests from %d, %s, " + "maxErrT: %f, maxErrR: %f, " + "meanErrT: %f, meanErrR: %f, " + "medErrT: %f, medErrR: %f\n", + printMethod(methods[method]).c_str(), totalTestsCount - successfulTestsCount, totalTestsCount, printMode(mode).c_str(), + maxErrorTrans, maxErrorRot, meanErrorTrans, meanErrorRot, medianErrorTrans, medianErrorRot); + ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + } + cout << "mode: " << printMode(mode) << ", method: " << printMethod(methods[method]) << " -> " + << ((double)successfulTestsCount / totalTestsCount) * 100 << "%" + << " (maxErrT: " << maxErrorTrans << ", maxErrR: " << maxErrorRot + << ", meanErrT: " << meanErrorTrans << ", meanErrR: " << meanErrorRot + << ", medErrT: " << medianErrorTrans << ", medErrR: " << medianErrorRot << ")" << endl; + double transThres, rotThresh; + findThreshold(vec_errorTrans, vec_errorRot, 0.7, transThres, rotThresh); + cout << "approximate translation threshold for 0.7: " << transThres + << ", approximate rotation threshold for 0.7: " << rotThresh << endl; + } + } + } }; TEST(Calib3d_SolveP3P, accuracy) { CV_solveP3P_Test test; test.safe_run();} TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); } TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); } +TEST(Calib3d_SolvePnP, accuracy_planar) { CV_solvePnP_Test test(true); test.safe_run(); } +TEST(Calib3d_SolvePnP, accuracy_planar_tag) { CV_solvePnP_Test test(true, true); test.safe_run(); } TEST(Calib3d_SolvePnPRansac, concurrency) { @@ -367,6 +715,7 @@ TEST(Calib3d_SolvePnPRansac, concurrency) camera_mat.at(1, 0) = 0.f; camera_mat.at(2, 0) = 0.f; camera_mat.at(2, 1) = 0.f; + camera_mat.at(2, 2) = 1.f; Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0)); @@ -420,7 +769,7 @@ TEST(Calib3d_SolvePnPRansac, input_type) { const int numPoints = 10; Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0., - 5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.); + 5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.); std::vector points3d; std::vector points2d; @@ -455,7 +804,7 @@ TEST(Calib3d_SolvePnPRansac, input_type) EXPECT_LE(cvtest::norm(t1, t4, NORM_INF), 1e-6); } -TEST(Calib3d_SolvePnP, double_support) +TEST(Calib3d_SolvePnPRansac, double_support) { Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0., 5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.); @@ -466,15 +815,15 @@ TEST(Calib3d_SolvePnP, double_support) for (int i = 0; i < 10 ; i+=2) { points3d.push_back(cv::Point3d(5+i, 3, 2)); - points3dF.push_back(cv::Point3d(5+i, 3, 2)); + points3dF.push_back(cv::Point3f(static_cast(5+i), 3, 2)); points3d.push_back(cv::Point3d(5+i, 3+i, 2+i)); - points3dF.push_back(cv::Point3d(5+i, 3+i, 2+i)); + points3dF.push_back(cv::Point3f(static_cast(5+i), static_cast(3+i), static_cast(2+i))); points2d.push_back(cv::Point2d(0, i)); - points2dF.push_back(cv::Point2d(0, i)); + points2dF.push_back(cv::Point2f(0, static_cast(i))); points2d.push_back(cv::Point2d(-i, i)); - points2dF.push_back(cv::Point2d(-i, i)); + points2dF.push_back(cv::Point2f(static_cast(-i), static_cast(i))); } - Mat R,t, RF, tF; + Mat R, t, RF, tF; vector inliers; solvePnPRansac(points3dF, points2dF, intrinsics, cv::Mat(), RF, tF, true, 100, 8.f, 0.999, inliers, cv::SOLVEPNP_P3P); @@ -484,6 +833,367 @@ TEST(Calib3d_SolvePnP, double_support) EXPECT_LE(cvtest::norm(t, Mat_(tF), NORM_INF), 1e-3); } +TEST(Calib3d_SolvePnP, input_type) +{ + Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0., + 5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.); + vector points3d_; + vector points3dF_; + //Cube + const float l = -0.1f; + //Front face + points3d_.push_back(Point3d(-l, -l, -l)); + points3dF_.push_back(Point3f(-l, -l, -l)); + points3d_.push_back(Point3d(l, -l, -l)); + points3dF_.push_back(Point3f(l, -l, -l)); + points3d_.push_back(Point3d(l, l, -l)); + points3dF_.push_back(Point3f(l, l, -l)); + points3d_.push_back(Point3d(-l, l, -l)); + points3dF_.push_back(Point3f(-l, l, -l)); + //Back face + points3d_.push_back(Point3d(-l, -l, l)); + points3dF_.push_back(Point3f(-l, -l, l)); + points3d_.push_back(Point3d(l, -l, l)); + points3dF_.push_back(Point3f(l, -l, l)); + points3d_.push_back(Point3d(l, l, l)); + points3dF_.push_back(Point3f(l, l, l)); + points3d_.push_back(Point3d(-l, l, l)); + points3dF_.push_back(Point3f(-l, l, l)); + + Mat trueRvec = (Mat_(3,1) << 0.1, -0.25, 0.467); + Mat trueTvec = (Mat_(3,1) << -0.21, 0.12, 0.746); + + for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++) + { + vector points3d; + vector points2d; + vector points3dF; + vector points2dF; + + if (method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE) + { + const float tagSize_2 = 0.05f / 2; + points3d.push_back(Point3d(-tagSize_2, tagSize_2, 0)); + points3d.push_back(Point3d( tagSize_2, tagSize_2, 0)); + points3d.push_back(Point3d( tagSize_2, -tagSize_2, 0)); + points3d.push_back(Point3d(-tagSize_2, -tagSize_2, 0)); + + points3dF.push_back(Point3f(-tagSize_2, tagSize_2, 0)); + points3dF.push_back(Point3f( tagSize_2, tagSize_2, 0)); + points3dF.push_back(Point3f( tagSize_2, -tagSize_2, 0)); + points3dF.push_back(Point3f(-tagSize_2, -tagSize_2, 0)); + } + else if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P) + { + points3d = vector(points3d_.begin(), points3d_.begin()+4); + points3dF = vector(points3dF_.begin(), points3dF_.begin()+4); + } + else + { + points3d = points3d_; + points3dF = points3dF_; + } + + projectPoints(points3d, trueRvec, trueTvec, intrinsics, noArray(), points2d); + projectPoints(points3dF, trueRvec, trueTvec, intrinsics, noArray(), points2dF); + + //solvePnP + { + Mat R, t, RF, tF; + + solvePnP(points3dF, points2dF, Matx33f(intrinsics), Mat(), RF, tF, false, method); + solvePnP(points3d, points2d, intrinsics, Mat(), R, t, false, method); + + //By default rvec and tvec must be returned in double precision + EXPECT_EQ(RF.type(), tF.type()); + EXPECT_EQ(RF.type(), CV_64FC1); + + EXPECT_EQ(R.type(), t.type()); + EXPECT_EQ(R.type(), CV_64FC1); + + EXPECT_LE(cvtest::norm(R, RF, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t, tF, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, RF, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, tF, NORM_INF), 1e-3); + } + { + Mat R1, t1, R2, t2; + + solvePnP(points3dF, points2d, intrinsics, Mat(), R1, t1, false, method); + solvePnP(points3d, points2dF, intrinsics, Mat(), R2, t2, false, method); + + //By default rvec and tvec must be returned in double precision + EXPECT_EQ(R1.type(), t1.type()); + EXPECT_EQ(R1.type(), CV_64FC1); + + EXPECT_EQ(R2.type(), t2.type()); + EXPECT_EQ(R2.type(), CV_64FC1); + + EXPECT_LE(cvtest::norm(R1, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, t2, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t2, NORM_INF), 1e-3); + } + { + Mat R1(3,1,CV_32FC1), t1(3,1,CV_64FC1); + Mat R2(3,1,CV_64FC1), t2(3,1,CV_32FC1); + + solvePnP(points3dF, points2d, intrinsics, Mat(), R1, t1, false, method); + solvePnP(points3d, points2dF, intrinsics, Mat(), R2, t2, false, method); + + //If not null, rvec and tvec must be returned in the same precision + EXPECT_EQ(R1.type(), CV_32FC1); + EXPECT_EQ(t1.type(), CV_64FC1); + + EXPECT_EQ(R2.type(), CV_64FC1); + EXPECT_EQ(t2.type(), CV_32FC1); + + EXPECT_LE(cvtest::norm(Mat_(R1), R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, Mat_(t2), NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, Mat_(R1), NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, Mat_(t2), NORM_INF), 1e-3); + } + { + Matx31f R1, t2; + Matx31d R2, t1; + + solvePnP(points3dF, points2d, intrinsics, Mat(), R1, t1, false, method); + solvePnP(points3d, points2dF, intrinsics, Mat(), R2, t2, false, method); + + Matx31d R1d(R1(0), R1(1), R1(2)); + Matx31d t2d(t2(0), t2(1), t2(2)); + + EXPECT_LE(cvtest::norm(R1d, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, t2d, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R1d, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t2d, NORM_INF), 1e-3); + } + + //solvePnPGeneric + { + vector Rs, ts, RFs, tFs; + + int res1 = solvePnPGeneric(points3dF, points2dF, Matx33f(intrinsics), Mat(), RFs, tFs, false, (SolvePnPMethod)method); + int res2 = solvePnPGeneric(points3d, points2d, intrinsics, Mat(), Rs, ts, false, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Mat R = Rs.front(), t = ts.front(), RF = RFs.front(), tF = tFs.front(); + + //By default rvecs and tvecs must be returned in double precision + EXPECT_EQ(RF.type(), tF.type()); + EXPECT_EQ(RF.type(), CV_64FC1); + + EXPECT_EQ(R.type(), t.type()); + EXPECT_EQ(R.type(), CV_64FC1); + + EXPECT_LE(cvtest::norm(R, RF, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t, tF, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, RF, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, tF, NORM_INF), 1e-3); + } + { + vector R1s, t1s, R2s, t2s; + + int res1 = solvePnPGeneric(points3dF, points2d, intrinsics, Mat(), R1s, t1s, false, (SolvePnPMethod)method); + int res2 = solvePnPGeneric(points3d, points2dF, intrinsics, Mat(), R2s, t2s, false, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Mat R1 = R1s.front(), t1 = t1s.front(), R2 = R2s.front(), t2 = t2s.front(); + + //By default rvecs and tvecs must be returned in double precision + EXPECT_EQ(R1.type(), t1.type()); + EXPECT_EQ(R1.type(), CV_64FC1); + + EXPECT_EQ(R2.type(), t2.type()); + EXPECT_EQ(R2.type(), CV_64FC1); + + EXPECT_LE(cvtest::norm(R1, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, t2, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t2, NORM_INF), 1e-3); + } + { + vector > R1s, t2s; + vector > R2s, t1s; + + int res1 = solvePnPGeneric(points3dF, points2d, intrinsics, Mat(), R1s, t1s, false, (SolvePnPMethod)method); + int res2 = solvePnPGeneric(points3d, points2dF, intrinsics, Mat(), R2s, t2s, false, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Mat R1 = R1s.front(), t1 = t1s.front(); + Mat R2 = R2s.front(), t2 = t2s.front(); + + //If not null, rvecs and tvecs must be returned in the same precision + EXPECT_EQ(R1.type(), CV_32FC1); + EXPECT_EQ(t1.type(), CV_64FC1); + + EXPECT_EQ(R2.type(), CV_64FC1); + EXPECT_EQ(t2.type(), CV_32FC1); + + EXPECT_LE(cvtest::norm(Mat_(R1), R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, Mat_(t2), NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, Mat_(R1), NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, Mat_(t2), NORM_INF), 1e-3); + } + { + vector R1s, t2s; + vector R2s, t1s; + + int res1 = solvePnPGeneric(points3dF, points2d, intrinsics, Mat(), R1s, t1s, false, (SolvePnPMethod)method); + int res2 = solvePnPGeneric(points3d, points2dF, intrinsics, Mat(), R2s, t2s, false, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Matx31f R1 = R1s.front(), t2 = t2s.front(); + Matx31d R2 = R2s.front(), t1 = t1s.front(); + Matx31d R1d(R1(0), R1(1), R1(2)), t2d(t2(0), t2(1), t2(2)); + + EXPECT_LE(cvtest::norm(R1d, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, t2d, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R1d, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t2d, NORM_INF), 1e-3); + } + + if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P) + { + //solveP3P + { + vector Rs, ts, RFs, tFs; + + int res1 = solveP3P(points3dF, points2dF, Matx33f(intrinsics), Mat(), RFs, tFs, (SolvePnPMethod)method); + int res2 = solveP3P(points3d, points2d, intrinsics, Mat(), Rs, ts, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Mat R = Rs.front(), t = ts.front(), RF = RFs.front(), tF = tFs.front(); + + //By default rvecs and tvecs must be returned in double precision + EXPECT_EQ(RF.type(), tF.type()); + EXPECT_EQ(RF.type(), CV_64FC1); + + EXPECT_EQ(R.type(), t.type()); + EXPECT_EQ(R.type(), CV_64FC1); + + EXPECT_LE(cvtest::norm(R, RF, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t, tF, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, RF, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, tF, NORM_INF), 1e-3); + } + { + vector R1s, t1s, R2s, t2s; + + int res1 = solveP3P(points3dF, points2d, intrinsics, Mat(), R1s, t1s, (SolvePnPMethod)method); + int res2 = solveP3P(points3d, points2dF, intrinsics, Mat(), R2s, t2s, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Mat R1 = R1s.front(), t1 = t1s.front(), R2 = R2s.front(), t2 = t2s.front(); + + //By default rvecs and tvecs must be returned in double precision + EXPECT_EQ(R1.type(), t1.type()); + EXPECT_EQ(R1.type(), CV_64FC1); + + EXPECT_EQ(R2.type(), t2.type()); + EXPECT_EQ(R2.type(), CV_64FC1); + + EXPECT_LE(cvtest::norm(R1, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, t2, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t2, NORM_INF), 1e-3); + } + { + vector > R1s, t2s; + vector > R2s, t1s; + + int res1 = solveP3P(points3dF, points2d, intrinsics, Mat(), R1s, t1s, (SolvePnPMethod)method); + int res2 = solveP3P(points3d, points2dF, intrinsics, Mat(), R2s, t2s, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Mat R1 = R1s.front(), t1 = t1s.front(); + Mat R2 = R2s.front(), t2 = t2s.front(); + + //If not null, rvecs and tvecs must be returned in the same precision + EXPECT_EQ(R1.type(), CV_32FC1); + EXPECT_EQ(t1.type(), CV_64FC1); + + EXPECT_EQ(R2.type(), CV_64FC1); + EXPECT_EQ(t2.type(), CV_32FC1); + + EXPECT_LE(cvtest::norm(Mat_(R1), R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, Mat_(t2), NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, Mat_(R1), NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, Mat_(t2), NORM_INF), 1e-3); + } + { + vector R1s, t2s; + vector R2s, t1s; + + int res1 = solveP3P(points3dF, points2d, intrinsics, Mat(), R1s, t1s, (SolvePnPMethod)method); + int res2 = solveP3P(points3d, points2dF, intrinsics, Mat(), R2s, t2s, (SolvePnPMethod)method); + + EXPECT_GT(res1, 0); + EXPECT_GT(res2, 0); + + Matx31f R1 = R1s.front(), t2 = t2s.front(); + Matx31d R2 = R2s.front(), t1 = t1s.front(); + Matx31d R1d(R1(0), R1(1), R1(2)), t2d(t2(0), t2(1), t2(2)); + + EXPECT_LE(cvtest::norm(R1d, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(t1, t2d, NORM_INF), 1e-3); + + EXPECT_LE(cvtest::norm(trueRvec, R1d, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3); + EXPECT_LE(cvtest::norm(trueTvec, t2d, NORM_INF), 1e-3); + } + } + } +} + TEST(Calib3d_SolvePnP, translation) { Mat cameraIntrinsic = Mat::eye(3,3, CV_32FC1); @@ -548,13 +1258,16 @@ TEST(Calib3d_SolvePnP, iterativeInitialGuess3pts) solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE); - std::cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; - std::cout << "rvec_est: " << rvec_est.t() << std::endl; - std::cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; - std::cout << "tvec_est: " << tvec_est.t() << std::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); + + EXPECT_EQ(rvec_est.type(), CV_64FC1); + EXPECT_EQ(tvec_est.type(), CV_64FC1); } { @@ -579,13 +1292,230 @@ TEST(Calib3d_SolvePnP, iterativeInitialGuess3pts) solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE); - std::cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl; - std::cout << "rvec_est: " << rvec_est.t() << std::endl; - std::cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl; - std::cout << "tvec_est: " << tvec_est.t() << std::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); + + EXPECT_EQ(rvec_est.type(), CV_32FC1); + EXPECT_EQ(tvec_est.type(), CV_32FC1); + } +} + +TEST(Calib3d_SolvePnP, iterativeInitialGuess) +{ + { + 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 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_(3,1) << 0.3, -0.2, 0.75); + Mat tvec_ground_truth = (Mat_(3,1) << 0.15, -0.2, 1.5); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + Mat rvec_est = (Mat_(3,1) << 0.1, -0.1, 0.1); + Mat tvec_est = (Mat_(3,1) << 0.0, -0.5, 1.0); + + solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE); + + 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); + + EXPECT_EQ(rvec_est.type(), CV_64FC1); + EXPECT_EQ(tvec_est.type(), CV_64FC1); + } + + { + 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 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_(3,1) << -0.75f, 0.4f, 0.34f); + Mat tvec_ground_truth = (Mat_(3,1) << -0.15f, 0.35f, 1.58f); + + vector p2d; + projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d); + + Mat rvec_est = (Mat_(3,1) << -0.1f, 0.1f, 0.1f); + Mat tvec_est = (Mat_(3,1) << 0.0f, 0.0f, 1.0f); + + solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE); + + 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); + + EXPECT_EQ(rvec_est.type(), CV_32FC1); + EXPECT_EQ(tvec_est.type(), CV_32FC1); + } +} + +TEST(Calib3d_SolvePnP, generic) +{ + { + 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 p3d_; + p3d_.push_back(Point3d(-L, L, 0)); + p3d_.push_back(Point3d(L, L, 0)); + p3d_.push_back(Point3d(L, -L, 0)); + p3d_.push_back(Point3d(-L, -L, 0)); + p3d_.push_back(Point3d(-L, L, L/2)); + p3d_.push_back(Point3d(0, 0, -L/2)); + + const int ntests = 10; + for (int numTest = 0; numTest < ntests; numTest++) + { + Mat rvec_ground_truth; + Mat tvec_ground_truth; + generatePose(p3d_, rvec_ground_truth, tvec_ground_truth, theRNG()); + + vector p2d_; + projectPoints(p3d_, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d_); + + for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++) + { + vector rvecs_est; + vector tvecs_est; + + vector p3d; + vector p2d; + if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P || + method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE) + { + p3d = vector(p3d_.begin(), p3d_.begin()+4); + p2d = vector(p2d_.begin(), p2d_.begin()+4); + } + else + { + p3d = p3d_; + p2d = p2d_; + } + + vector reprojectionErrors; + solvePnPGeneric(p3d, p2d, intrinsics, noArray(), rvecs_est, tvecs_est, false, (SolvePnPMethod)method, + noArray(), noArray(), reprojectionErrors); + + EXPECT_TRUE(!rvecs_est.empty()); + EXPECT_TRUE(rvecs_est.size() == tvecs_est.size() && tvecs_est.size() == reprojectionErrors.size()); + + for (size_t i = 0; i < reprojectionErrors.size()-1; i++) + { + EXPECT_GE(reprojectionErrors[i+1], reprojectionErrors[i]); + } + + bool isTestSuccess = false; + for (size_t i = 0; i < rvecs_est.size() && !isTestSuccess; i++) { + double rvecDiff = cvtest::norm(rvecs_est[i], rvec_ground_truth, NORM_L2); + double tvecDiff = cvtest::norm(tvecs_est[i], tvec_ground_truth, NORM_L2); + const double threshold = method == SOLVEPNP_P3P ? 1e-2 : 1e-4; + isTestSuccess = rvecDiff < threshold && tvecDiff < threshold; + } + + EXPECT_TRUE(isTestSuccess); + } + } + } + + { + 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 p3f_; + p3f_.push_back(Point3f(-L, L, 0)); + p3f_.push_back(Point3f(L, L, 0)); + p3f_.push_back(Point3f(L, -L, 0)); + p3f_.push_back(Point3f(-L, -L, 0)); + p3f_.push_back(Point3f(-L, L, L/2)); + p3f_.push_back(Point3f(0, 0, -L/2)); + + const int ntests = 10; + for (int numTest = 0; numTest < ntests; numTest++) + { + Mat rvec_ground_truth; + Mat tvec_ground_truth; + generatePose(p3f_, rvec_ground_truth, tvec_ground_truth, theRNG()); + + vector p2f_; + projectPoints(p3f_, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2f_); + + for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++) + { + vector rvecs_est; + vector tvecs_est; + + vector p3f; + vector p2f; + if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P || + method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE) + { + p3f = vector(p3f_.begin(), p3f_.begin()+4); + p2f = vector(p2f_.begin(), p2f_.begin()+4); + } + else + { + p3f = p3f_; + p2f = p2f_; + } + + vector reprojectionErrors; + solvePnPGeneric(p3f, p2f, intrinsics, noArray(), rvecs_est, tvecs_est, false, (SolvePnPMethod)method, + noArray(), noArray(), reprojectionErrors); + + EXPECT_TRUE(!rvecs_est.empty()); + EXPECT_TRUE(rvecs_est.size() == tvecs_est.size() && tvecs_est.size() == reprojectionErrors.size()); + + for (size_t i = 0; i < reprojectionErrors.size()-1; i++) + { + EXPECT_GE(reprojectionErrors[i+1], reprojectionErrors[i]); + } + + bool isTestSuccess = false; + for (size_t i = 0; i < rvecs_est.size() && !isTestSuccess; i++) { + double rvecDiff = cvtest::norm(rvecs_est[i], rvec_ground_truth, NORM_L2); + double tvecDiff = cvtest::norm(tvecs_est[i], tvec_ground_truth, NORM_L2); + const double threshold = method == SOLVEPNP_P3P ? 1e-2 : 1e-4; + isTestSuccess = rvecDiff < threshold && tvecDiff < threshold; + } + + EXPECT_TRUE(isTestSuccess); + } + } } }