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_TRIANGULATION_H_ 33 #define COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ 34 35 #include "base/camera.h" 36 37 #include <vector> 38 39 #include <Eigen/Core> 40 41 #include "optim/ransac.h" 42 #include "util/alignment.h" 43 #include "util/math.h" 44 #include "util/types.h" 45 46 namespace colmap { 47 48 // Triangulation estimator to estimate 3D point from multiple observations. 49 // The triangulation must satisfy the following constraints: 50 // - Sufficient triangulation angle between observation pairs. 51 // - All observations must satisfy cheirality constraint. 52 // 53 // An observation is composed of an image measurement and the corresponding 54 // camera pose and calibration. 55 class TriangulationEstimator { 56 public: 57 enum class ResidualType { 58 ANGULAR_ERROR, 59 REPROJECTION_ERROR, 60 }; 61 62 struct PointData { 63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW PointDataPointData64 PointData() {} PointDataPointData65 PointData(const Eigen::Vector2d& point_, const Eigen::Vector2d& point_N_) 66 : point(point_), point_normalized(point_N_) {} 67 // Image observation in pixels. Only needs to be set for REPROJECTION_ERROR. 68 Eigen::Vector2d point; 69 // Normalized image observation. Must always be set. 70 Eigen::Vector2d point_normalized; 71 }; 72 73 struct PoseData { 74 EIGEN_MAKE_ALIGNED_OPERATOR_NEW PoseDataPoseData75 PoseData() : camera(nullptr) {} PoseDataPoseData76 PoseData(const Eigen::Matrix3x4d& proj_matrix_, 77 const Eigen::Vector3d& pose_, const Camera* camera_) 78 : proj_matrix(proj_matrix_), proj_center(pose_), camera(camera_) {} 79 // The projection matrix for the image of the observation. 80 Eigen::Matrix3x4d proj_matrix; 81 // The projection center for the image of the observation. 82 Eigen::Vector3d proj_center; 83 // The camera for the image of the observation. 84 const Camera* camera; 85 }; 86 87 typedef PointData X_t; 88 typedef PoseData Y_t; 89 typedef Eigen::Vector3d M_t; 90 91 // Specify settings for triangulation estimator. 92 void SetMinTriAngle(const double min_tri_angle); 93 void SetResidualType(const ResidualType residual_type); 94 95 // The minimum number of samples needed to estimate a model. 96 static const int kMinNumSamples = 2; 97 98 // Estimate a 3D point from a two-view observation. 99 // 100 // @param point_data Image measurement. 101 // @param point_data Camera poses. 102 // 103 // @return Triangulated point if successful, otherwise none. 104 std::vector<M_t> Estimate(const std::vector<X_t>& point_data, 105 const std::vector<Y_t>& pose_data) const; 106 107 // Calculate residuals in terms of squared reprojection or angular error. 108 // 109 // @param point_data Image measurements. 110 // @param point_data Camera poses. 111 // @param xyz 3D point. 112 // 113 // @return Residual for each observation. 114 void Residuals(const std::vector<X_t>& point_data, 115 const std::vector<Y_t>& pose_data, const M_t& xyz, 116 std::vector<double>* residuals) const; 117 118 private: 119 ResidualType residual_type_ = ResidualType::REPROJECTION_ERROR; 120 double min_tri_angle_ = 0.0; 121 }; 122 123 struct EstimateTriangulationOptions { 124 // Minimum triangulation angle in radians. 125 double min_tri_angle = 0.0; 126 127 // The employed residual type. 128 TriangulationEstimator::ResidualType residual_type = 129 TriangulationEstimator::ResidualType::ANGULAR_ERROR; 130 131 // RANSAC options for TriangulationEstimator. 132 RANSACOptions ransac_options; 133 CheckEstimateTriangulationOptions134 void Check() const { 135 CHECK_GE(min_tri_angle, 0.0); 136 ransac_options.Check(); 137 } 138 }; 139 140 // Robustly estimate 3D point from observations in multiple views using RANSAC 141 // and a subsequent non-linear refinement using all inliers. Returns true 142 // if the estimated number of inliers has more than two views. 143 bool EstimateTriangulation( 144 const EstimateTriangulationOptions& options, 145 const std::vector<TriangulationEstimator::PointData>& point_data, 146 const std::vector<TriangulationEstimator::PoseData>& pose_data, 147 std::vector<char>* inlier_mask, Eigen::Vector3d* xyz); 148 149 } // namespace colmap 150 151 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( 152 colmap::TriangulationEstimator::PointData) 153 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( 154 colmap::TriangulationEstimator::PoseData) 155 156 #endif // COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ 157