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_OPTIM_BUNDLE_ADJUSTMENT_H_ 33 #define COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ 34 35 #include <memory> 36 #include <unordered_set> 37 38 #include <Eigen/Core> 39 40 #include <ceres/ceres.h> 41 42 #include "PBA/pba.h" 43 #include "base/camera_rig.h" 44 #include "base/reconstruction.h" 45 #include "util/alignment.h" 46 47 namespace colmap { 48 49 struct BundleAdjustmentOptions { 50 // Loss function types: Trivial (non-robust) and Cauchy (robust) loss. 51 enum class LossFunctionType { TRIVIAL, SOFT_L1, CAUCHY }; 52 LossFunctionType loss_function_type = LossFunctionType::TRIVIAL; 53 54 // Scaling factor determines residual at which robustification takes place. 55 double loss_function_scale = 1.0; 56 57 // Whether to refine the focal length parameter group. 58 bool refine_focal_length = true; 59 60 // Whether to refine the principal point parameter group. 61 bool refine_principal_point = false; 62 63 // Whether to refine the extra parameter group. 64 bool refine_extra_params = true; 65 66 // Whether to refine the extrinsic parameter group. 67 bool refine_extrinsics = true; 68 69 // Whether to print a final summary. 70 bool print_summary = true; 71 72 // Minimum number of residuals to enable multi-threading. Note that 73 // single-threaded is typically better for small bundle adjustment problems 74 // due to the overhead of threading. 75 int min_num_residuals_for_multi_threading = 50000; 76 77 // Ceres-Solver options. 78 ceres::Solver::Options solver_options; 79 BundleAdjustmentOptionsBundleAdjustmentOptions80 BundleAdjustmentOptions() { 81 solver_options.function_tolerance = 0.0; 82 solver_options.gradient_tolerance = 0.0; 83 solver_options.parameter_tolerance = 0.0; 84 solver_options.minimizer_progress_to_stdout = false; 85 solver_options.max_num_iterations = 100; 86 solver_options.max_linear_solver_iterations = 200; 87 solver_options.max_num_consecutive_invalid_steps = 10; 88 solver_options.max_consecutive_nonmonotonic_steps = 10; 89 solver_options.num_threads = -1; 90 #if CERES_VERSION_MAJOR < 2 91 solver_options.num_linear_solver_threads = -1; 92 #endif // CERES_VERSION_MAJOR 93 } 94 95 // Create a new loss function based on the specified options. The caller 96 // takes ownership of the loss function. 97 ceres::LossFunction* CreateLossFunction() const; 98 99 bool Check() const; 100 }; 101 102 // Configuration container to setup bundle adjustment problems. 103 class BundleAdjustmentConfig { 104 public: 105 BundleAdjustmentConfig(); 106 107 size_t NumImages() const; 108 size_t NumPoints() const; 109 size_t NumConstantCameras() const; 110 size_t NumConstantPoses() const; 111 size_t NumConstantTvecs() const; 112 size_t NumVariablePoints() const; 113 size_t NumConstantPoints() const; 114 115 // Determine the number of residuals for the given reconstruction. The number 116 // of residuals equals the number of observations times two. 117 size_t NumResiduals(const Reconstruction& reconstruction) const; 118 119 // Add / remove images from the configuration. 120 void AddImage(const image_t image_id); 121 bool HasImage(const image_t image_id) const; 122 void RemoveImage(const image_t image_id); 123 124 // Set cameras of added images as constant or variable. By default all 125 // cameras of added images are variable. Note that the corresponding images 126 // have to be added prior to calling these methods. 127 void SetConstantCamera(const camera_t camera_id); 128 void SetVariableCamera(const camera_t camera_id); 129 bool IsConstantCamera(const camera_t camera_id) const; 130 131 // Set the pose of added images as constant. The pose is defined as the 132 // rotational and translational part of the projection matrix. 133 void SetConstantPose(const image_t image_id); 134 void SetVariablePose(const image_t image_id); 135 bool HasConstantPose(const image_t image_id) const; 136 137 // Set the translational part of the pose, hence the constant pose 138 // indices may be in [0, 1, 2] and must be unique. Note that the 139 // corresponding images have to be added prior to calling these methods. 140 void SetConstantTvec(const image_t image_id, const std::vector<int>& idxs); 141 void RemoveConstantTvec(const image_t image_id); 142 bool HasConstantTvec(const image_t image_id) const; 143 144 // Add / remove points from the configuration. Note that points can either 145 // be variable or constant but not both at the same time. 146 void AddVariablePoint(const point3D_t point3D_id); 147 void AddConstantPoint(const point3D_t point3D_id); 148 bool HasPoint(const point3D_t point3D_id) const; 149 bool HasVariablePoint(const point3D_t point3D_id) const; 150 bool HasConstantPoint(const point3D_t point3D_id) const; 151 void RemoveVariablePoint(const point3D_t point3D_id); 152 void RemoveConstantPoint(const point3D_t point3D_id); 153 154 // Access configuration data. 155 const std::unordered_set<image_t>& Images() const; 156 const std::unordered_set<point3D_t>& VariablePoints() const; 157 const std::unordered_set<point3D_t>& ConstantPoints() const; 158 const std::vector<int>& ConstantTvec(const image_t image_id) const; 159 160 private: 161 std::unordered_set<camera_t> constant_camera_ids_; 162 std::unordered_set<image_t> image_ids_; 163 std::unordered_set<point3D_t> variable_point3D_ids_; 164 std::unordered_set<point3D_t> constant_point3D_ids_; 165 std::unordered_set<image_t> constant_poses_; 166 std::unordered_map<image_t, std::vector<int>> constant_tvecs_; 167 }; 168 169 // Bundle adjustment based on Ceres-Solver. Enables most flexible configurations 170 // and provides best solution quality. 171 class BundleAdjuster { 172 public: 173 BundleAdjuster(const BundleAdjustmentOptions& options, 174 const BundleAdjustmentConfig& config); 175 176 bool Solve(Reconstruction* reconstruction); 177 178 // Get the Ceres solver summary for the last call to `Solve`. 179 const ceres::Solver::Summary& Summary() const; 180 181 private: 182 void SetUp(Reconstruction* reconstruction, 183 ceres::LossFunction* loss_function); 184 void TearDown(Reconstruction* reconstruction); 185 186 void AddImageToProblem(const image_t image_id, Reconstruction* reconstruction, 187 ceres::LossFunction* loss_function); 188 189 void AddPointToProblem(const point3D_t point3D_id, 190 Reconstruction* reconstruction, 191 ceres::LossFunction* loss_function); 192 193 protected: 194 void ParameterizeCameras(Reconstruction* reconstruction); 195 void ParameterizePoints(Reconstruction* reconstruction); 196 197 const BundleAdjustmentOptions options_; 198 BundleAdjustmentConfig config_; 199 std::unique_ptr<ceres::Problem> problem_; 200 ceres::Solver::Summary summary_; 201 std::unordered_set<camera_t> camera_ids_; 202 std::unordered_map<point3D_t, size_t> point3D_num_observations_; 203 }; 204 205 // Bundle adjustment using PBA (GPU or CPU). Less flexible and accurate than 206 // Ceres-Solver bundle adjustment but much faster. Only supports SimpleRadial 207 // camera model. 208 class ParallelBundleAdjuster { 209 public: 210 struct Options { 211 // Whether to print a final summary. 212 bool print_summary = true; 213 214 // Maximum number of iterations. 215 int max_num_iterations = 50; 216 217 // Index of the GPU used for bundle adjustment. 218 int gpu_index = -1; 219 220 // Number of threads for CPU based bundle adjustment. 221 int num_threads = -1; 222 223 // Minimum number of residuals to enable multi-threading. Note that 224 // single-threaded is typically better for small bundle adjustment problems 225 // due to the overhead of threading. 226 int min_num_residuals_for_multi_threading = 50000; 227 228 bool Check() const; 229 }; 230 231 ParallelBundleAdjuster(const Options& options, 232 const BundleAdjustmentOptions& ba_options, 233 const BundleAdjustmentConfig& config); 234 235 bool Solve(Reconstruction* reconstruction); 236 237 // Get the Ceres solver summary for the last call to `Solve`. 238 const ceres::Solver::Summary& Summary() const; 239 240 // Check whether PBA is supported for the given reconstruction. If the 241 // reconstruction is not supported, the PBA solver will exit ungracefully. 242 static bool IsSupported(const BundleAdjustmentOptions& options, 243 const Reconstruction& reconstruction); 244 245 private: 246 void SetUp(Reconstruction* reconstruction); 247 void TearDown(Reconstruction* reconstruction); 248 249 void AddImagesToProblem(Reconstruction* reconstruction); 250 void AddPointsToProblem(Reconstruction* reconstruction); 251 252 const Options options_; 253 const BundleAdjustmentOptions ba_options_; 254 BundleAdjustmentConfig config_; 255 ceres::Solver::Summary summary_; 256 257 size_t num_measurements_; 258 std::vector<pba::CameraT> cameras_; 259 std::vector<pba::Point3D> points3D_; 260 std::vector<pba::Point2D> measurements_; 261 std::unordered_set<camera_t> camera_ids_; 262 std::unordered_set<point3D_t> point3D_ids_; 263 std::vector<int> camera_idxs_; 264 std::vector<int> point3D_idxs_; 265 std::vector<image_t> ordered_image_ids_; 266 std::vector<point3D_t> ordered_point3D_ids_; 267 std::unordered_map<image_t, int> image_id_to_camera_idx_; 268 }; 269 270 class RigBundleAdjuster : public BundleAdjuster { 271 public: 272 struct Options { 273 // Whether to optimize the relative poses of the camera rigs. 274 bool refine_relative_poses = true; 275 276 // The maximum allowed reprojection error for an observation to be 277 // considered in the bundle adjustment. Some observations might have large 278 // reprojection errors due to the concatenation of the absolute and relative 279 // rig poses, which might be different from the absolute pose of the image 280 // in the reconstruction. 281 double max_reproj_error = 1000.0; 282 }; 283 284 RigBundleAdjuster(const BundleAdjustmentOptions& options, 285 const Options& rig_options, 286 const BundleAdjustmentConfig& config); 287 288 bool Solve(Reconstruction* reconstruction, 289 std::vector<CameraRig>* camera_rigs); 290 291 private: 292 void SetUp(Reconstruction* reconstruction, 293 std::vector<CameraRig>* camera_rigs, 294 ceres::LossFunction* loss_function); 295 void TearDown(Reconstruction* reconstruction, 296 const std::vector<CameraRig>& camera_rigs); 297 298 void AddImageToProblem(const image_t image_id, Reconstruction* reconstruction, 299 std::vector<CameraRig>* camera_rigs, 300 ceres::LossFunction* loss_function); 301 302 void AddPointToProblem(const point3D_t point3D_id, 303 Reconstruction* reconstruction, 304 ceres::LossFunction* loss_function); 305 306 void ComputeCameraRigPoses(const Reconstruction& reconstruction, 307 const std::vector<CameraRig>& camera_rigs); 308 309 void ParameterizeCameraRigs(Reconstruction* reconstruction); 310 311 const Options rig_options_; 312 313 // Mapping from images to camera rigs. 314 std::unordered_map<image_t, CameraRig*> image_id_to_camera_rig_; 315 316 // Mapping from images to the absolute camera rig poses. 317 std::unordered_map<image_t, Eigen::Vector4d*> image_id_to_rig_qvec_; 318 std::unordered_map<image_t, Eigen::Vector3d*> image_id_to_rig_tvec_; 319 320 // For each camera rig, the absolute camera rig poses. 321 std::vector<std::vector<Eigen::Vector4d>> camera_rig_qvecs_; 322 std::vector<std::vector<Eigen::Vector3d>> camera_rig_tvecs_; 323 324 // The Quaternions added to the problem, used to set the local 325 // parameterization once after setting up the problem. 326 std::unordered_set<double*> parameterized_qvec_data_; 327 }; 328 329 void PrintSolverSummary(const ceres::Solver::Summary& summary); 330 331 } // namespace colmap 332 333 #endif // COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ 334