1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. 2 // All rights reserved. 3 // 4 // Redistribution and use in source and binary forms, with or without 5 // modification, are permitted provided that the following conditions are met: 6 // 7 // * Redistributions of source code must retain the above copyright 8 // notice, this list of conditions and the following disclaimer. 9 // 10 // * Redistributions in binary form must reproduce the above copyright 11 // notice, this list of conditions and the following disclaimer in the 12 // documentation and/or other materials provided with the distribution. 13 // 14 // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 // its contributors may be used to endorse or promote products derived 16 // from this software without specific prior written permission. 17 // 18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 // POSSIBILITY OF SUCH DAMAGE. 29 // 30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 32 #ifndef COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ 33 #define COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ 34 35 #include <array> 36 #include <vector> 37 38 #include <Eigen/Core> 39 40 #include "util/alignment.h" 41 #include "util/types.h" 42 43 namespace colmap { 44 45 // Analytic solver for the P3P (Perspective-Three-Point) problem. 46 // 47 // The algorithm is based on the following paper: 48 // 49 // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang. Complete Solution 50 // Classification for the Perspective-Three-Point Problem. 51 // http://www.mmrc.iss.ac.cn/~xgao/paper/ieee.pdf 52 class P3PEstimator { 53 public: 54 // The 2D image feature observations. 55 typedef Eigen::Vector2d X_t; 56 // The observed 3D features in the world frame. 57 typedef Eigen::Vector3d Y_t; 58 // The transformation from the world to the camera frame. 59 typedef Eigen::Matrix3x4d M_t; 60 61 // The minimum number of samples needed to estimate a model. 62 static const int kMinNumSamples = 3; 63 64 // Estimate the most probable solution of the P3P problem from a set of 65 // three 2D-3D point correspondences. 66 // 67 // @param points2D Normalized 2D image points as 3x2 matrix. 68 // @param points3D 3D world points as 3x3 matrix. 69 // 70 // @return Most probable pose as length-1 vector of a 3x4 matrix. 71 static std::vector<M_t> Estimate(const std::vector<X_t>& points2D, 72 const std::vector<Y_t>& points3D); 73 74 // Calculate the squared reprojection error given a set of 2D-3D point 75 // correspondences and a projection matrix. 76 // 77 // @param points2D Normalized 2D image points as Nx2 matrix. 78 // @param points3D 3D world points as Nx3 matrix. 79 // @param proj_matrix 3x4 projection matrix. 80 // @param residuals Output vector of residuals. 81 static void Residuals(const std::vector<X_t>& points2D, 82 const std::vector<Y_t>& points3D, 83 const M_t& proj_matrix, std::vector<double>* residuals); 84 }; 85 86 // EPNP solver for the PNP (Perspective-N-Point) problem. The solver needs a 87 // minimum of 4 2D-3D correspondences. 88 // 89 // The algorithm is based on the following paper: 90 // 91 // Lepetit, Vincent, Francesc Moreno-Noguer, and Pascal Fua. 92 // "Epnp: An accurate o (n) solution to the pnp problem." 93 // International journal of computer vision 81.2 (2009): 155-166. 94 // 95 // The implementation is based on their original open-source release, but is 96 // ported to Eigen and contains several improvements over the original code. 97 class EPNPEstimator { 98 public: 99 // The 2D image feature observations. 100 typedef Eigen::Vector2d X_t; 101 // The observed 3D features in the world frame. 102 typedef Eigen::Vector3d Y_t; 103 // The transformation from the world to the camera frame. 104 typedef Eigen::Matrix3x4d M_t; 105 106 // The minimum number of samples needed to estimate a model. 107 static const int kMinNumSamples = 4; 108 109 // Estimate the most probable solution of the P3P problem from a set of 110 // three 2D-3D point correspondences. 111 // 112 // @param points2D Normalized 2D image points as 3x2 matrix. 113 // @param points3D 3D world points as 3x3 matrix. 114 // 115 // @return Most probable pose as length-1 vector of a 3x4 matrix. 116 static std::vector<M_t> Estimate(const std::vector<X_t>& points2D, 117 const std::vector<Y_t>& points3D); 118 119 // Calculate the squared reprojection error given a set of 2D-3D point 120 // correspondences and a projection matrix. 121 // 122 // @param points2D Normalized 2D image points as Nx2 matrix. 123 // @param points3D 3D world points as Nx3 matrix. 124 // @param proj_matrix 3x4 projection matrix. 125 // @param residuals Output vector of residuals. 126 static void Residuals(const std::vector<X_t>& points2D, 127 const std::vector<Y_t>& points3D, 128 const M_t& proj_matrix, std::vector<double>* residuals); 129 130 private: 131 bool ComputePose(const std::vector<Eigen::Vector2d>& points2D, 132 const std::vector<Eigen::Vector3d>& points3D, 133 Eigen::Matrix3x4d* proj_matrix); 134 135 void ChooseControlPoints(); 136 bool ComputeBarycentricCoordinates(); 137 138 Eigen::Matrix<double, Eigen::Dynamic, 12> ComputeM(); 139 Eigen::Matrix<double, 6, 10> ComputeL6x10( 140 const Eigen::Matrix<double, 12, 12>& Ut); 141 Eigen::Matrix<double, 6, 1> ComputeRho(); 142 143 void FindBetasApprox1(const Eigen::Matrix<double, 6, 10>& L_6x10, 144 const Eigen::Matrix<double, 6, 1>& rho, 145 Eigen::Vector4d* betas); 146 void FindBetasApprox2(const Eigen::Matrix<double, 6, 10>& L_6x10, 147 const Eigen::Matrix<double, 6, 1>& rho, 148 Eigen::Vector4d* betas); 149 void FindBetasApprox3(const Eigen::Matrix<double, 6, 10>& L_6x10, 150 const Eigen::Matrix<double, 6, 1>& rho, 151 Eigen::Vector4d* betas); 152 153 void RunGaussNewton(const Eigen::Matrix<double, 6, 10>& L_6x10, 154 const Eigen::Matrix<double, 6, 1>& rho, 155 Eigen::Vector4d* betas); 156 157 double ComputeRT(const Eigen::Matrix<double, 12, 12>& Ut, 158 const Eigen::Vector4d& betas, Eigen::Matrix3d* R, 159 Eigen::Vector3d* t); 160 161 void ComputeCcs(const Eigen::Vector4d& betas, 162 const Eigen::Matrix<double, 12, 12>& Ut); 163 void ComputePcs(); 164 165 void SolveForSign(); 166 167 void EstimateRT(Eigen::Matrix3d* R, Eigen::Vector3d* t); 168 169 double ComputeTotalReprojectionError(const Eigen::Matrix3d& R, 170 const Eigen::Vector3d& t); 171 172 const std::vector<Eigen::Vector2d>* points2D_ = nullptr; 173 const std::vector<Eigen::Vector3d>* points3D_ = nullptr; 174 std::vector<Eigen::Vector3d> pcs_; 175 std::vector<Eigen::Vector4d> alphas_; 176 std::array<Eigen::Vector3d, 4> cws_; 177 std::array<Eigen::Vector3d, 4> ccs_; 178 }; 179 180 } // namespace colmap 181 182 #endif // COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ 183