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_GENERALIZED_ABSOLUTE_POSE_H_ 33 #define COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_ 34 35 #include <vector> 36 37 #include <Eigen/Core> 38 39 #include "util/alignment.h" 40 #include "util/types.h" 41 42 namespace colmap { 43 44 // Solver for the Generalized P3P problem (NP3P or GP3P), based on: 45 // 46 // Lee, Gim Hee, et al. "Minimal solutions for pose estimation of a 47 // multi-camera system." Robotics Research. Springer International 48 // Publishing, 2016. 521-538. 49 // 50 // This class is based on an original implementation by Federico Camposeco. 51 class GP3PEstimator { 52 public: 53 // The generalized image observations, which is composed of the relative pose 54 // of the specific camera in the generalized camera and its image observation. 55 struct X_t { 56 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 // The relative transformation from the generalized camera to the camera 58 // frame of the observation. 59 Eigen::Matrix3x4d rel_tform; 60 // The 2D image feature observation. 61 Eigen::Vector2d xy; 62 }; 63 64 // The observed 3D feature points in the world frame. 65 typedef Eigen::Vector3d Y_t; 66 // The transformation from the world to the generalized camera frame. 67 typedef Eigen::Matrix3x4d M_t; 68 69 // The minimum number of samples needed to estimate a model. 70 static const int kMinNumSamples = 3; 71 72 // Estimate the most probable solution of the GP3P problem from a set of 73 // three 2D-3D point correspondences. 74 static std::vector<M_t> Estimate(const std::vector<X_t>& points2D, 75 const std::vector<Y_t>& points3D); 76 77 // Calculate the squared cosine distance error between the rays given a set of 78 // 2D-3D point correspondences and a projection matrix of the generalized 79 // camera. 80 static void Residuals(const std::vector<X_t>& points2D, 81 const std::vector<Y_t>& points3D, 82 const M_t& proj_matrix, std::vector<double>* residuals); 83 }; 84 85 } // namespace colmap 86 87 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::GP3PEstimator::X_t) 88 89 #endif // COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_ 90