当前位置:   article > 正文

COLMAP后端优化-代码阅读_rigbundleadjuster

rigbundleadjuster

转载于

【colmap】COLMAP/src/exe/colmap.cc - 知乎

  2021-07-13_我想静静,的博客-CSDN博客

稀疏重建基本流程

  1. 特征提取
  2. 增量式SfM选择无序影像进行特征匹配,
  3. 并进行几何纠正、三角测量恢复稀疏点云结构,
  4. 通过已有点云重新估计相对姿态,
  5. 再进行局部和全局的BA优化。
  6. 之后逐步向已有的结构中增加视角或影像,进行三角测量和姿态估计,再进行BA优化修正结构数据,最后输出全部的相机参数和稀疏三维点云。

代码结构

源码位于colmap/src路径下,包括下图所示几个子文件夹:

  1. 特征提取与匹配的基本操作在feature文件夹下,
  2. 最基本的数据缓存存储格式定义外还有整个SFM流程中的最底层操作在 base/util文件夹下,其中最关键的是correspondence_graph的建立,对于后续的最优初始化图像对的选取以及Next best view selection;
  3. 增量式重建的最基本操作在sfm文件夹下;
  4. 光束法平差BA的最基本操作在optim文件夹下;
  5. 最底层增量式重建和光束法平差BA操作的进一步封装在controllers文件夹下,其中automatic_reconstruction将前面分析的三步数据处理流程合并,一键式重建。
  6. ui文件夹与软件界面相关。

代码主体

代码入口在src/exe文件夹下的colmap.cc中:

  1. int main(int argc, char** argv) {
  2. using namespace colmap;
  3. InitializeGlog(argv);
  4. #ifdef GUI_ENABLED
  5. Q_INIT_RESOURCE(resources);
  6. #endif
  7. std::vector<std::pair<std::string, command_func_t>> commands;
  8. commands.emplace_back("gui", &RunGraphicalUserInterface);
  9. commands.emplace_back("automatic_reconstructor", &RunAutomaticReconstructor);
  10. commands.emplace_back("bundle_adjuster", &RunBundleAdjuster);
  11. commands.emplace_back("color_extractor", &RunColorExtractor);
  12. commands.emplace_back("database_cleaner", &RunDatabaseCleaner);
  13. commands.emplace_back("database_creator", &RunDatabaseCreator);
  14. commands.emplace_back("database_merger", &RunDatabaseMerger);
  15. commands.emplace_back("delaunay_mesher", &RunDelaunayMesher);
  16. commands.emplace_back("exhaustive_matcher", &RunExhaustiveMatcher);
  17. commands.emplace_back("feature_extractor", &RunFeatureExtractor);
  18. commands.emplace_back("feature_importer", &RunFeatureImporter);
  19. commands.emplace_back("hierarchical_mapper", &RunHierarchicalMapper);
  20. commands.emplace_back("image_deleter", &RunImageDeleter);
  21. commands.emplace_back("image_filterer", &RunImageFilterer);
  22. commands.emplace_back("image_rectifier", &RunImageRectifier);
  23. commands.emplace_back("image_registrator", &RunImageRegistrator);
  24. commands.emplace_back("image_undistorter", &RunImageUndistorter);
  25. commands.emplace_back("image_undistorter_standalone",
  26. &RunImageUndistorterStandalone);
  27. commands.emplace_back("mapper", &RunMapper);
  28. commands.emplace_back("matches_importer", &RunMatchesImporter);
  29. commands.emplace_back("model_aligner", &RunModelAligner);
  30. commands.emplace_back("model_analyzer", &RunModelAnalyzer);
  31. commands.emplace_back("model_comparer", &RunModelComparer);
  32. commands.emplace_back("model_converter", &RunModelConverter);
  33. commands.emplace_back("model_cropper", &RunModelCropper);
  34. commands.emplace_back("model_merger", &RunModelMerger);
  35. commands.emplace_back("model_orientation_aligner",
  36. &RunModelOrientationAligner);
  37. commands.emplace_back("model_splitter", &RunModelSplitter);
  38. commands.emplace_back("model_transformer", &RunModelTransformer);
  39. commands.emplace_back("patch_match_stereo", &RunPatchMatchStereo);
  40. commands.emplace_back("point_filtering", &RunPointFiltering);
  41. commands.emplace_back("point_triangulator", &RunPointTriangulator);
  42. commands.emplace_back("poisson_mesher", &RunPoissonMesher);
  43. commands.emplace_back("project_generator", &RunProjectGenerator);
  44. commands.emplace_back("rig_bundle_adjuster", &RunRigBundleAdjuster);
  45. commands.emplace_back("sequential_matcher", &RunSequentialMatcher);
  46. commands.emplace_back("spatial_matcher", &RunSpatialMatcher);
  47. commands.emplace_back("stereo_fusion", &RunStereoFuser);
  48. commands.emplace_back("transitive_matcher", &RunTransitiveMatcher);
  49. commands.emplace_back("vocab_tree_builder", &RunVocabTreeBuilder);
  50. commands.emplace_back("vocab_tree_matcher", &RunVocabTreeMatcher);
  51. commands.emplace_back("vocab_tree_retriever", &RunVocabTreeRetriever);
  52. if (argc == 1) {
  53. return ShowHelp(commands);
  54. }
  55. const std::string command = argv[1];
  56. if (command == "help" || command == "-h" || command == "--help") {
  57. return ShowHelp(commands);
  58. } else {
  59. command_func_t matched_command_func = nullptr;
  60. for (const auto& command_func : commands) {
  61. if (command == command_func.first) {
  62. matched_command_func = command_func.second;
  63. break;
  64. }
  65. }
  66. if (matched_command_func == nullptr) {
  67. std::cerr << StringPrintf(
  68. "ERROR: Command `%s` not recognized. To list the "
  69. "available commands, run `colmap help`.",
  70. command.c_str())
  71. << std::endl;
  72. return EXIT_FAILURE;
  73. } else {
  74. int command_argc = argc - 1;
  75. char** command_argv = &argv[1];
  76. command_argv[0] = argv[0];
  77. return matched_command_func(command_argc, command_argv);
  78. }
  79. }
  80. return ShowHelp(commands);
  81. }

其中各种指令分布在如下文件中:

  1. #include "exe/database.h"
  2. #include "exe/feature.h"
  3. #include "exe/gui.h"
  4. #include "exe/image.h"
  5. #include "exe/model.h"
  6. #include "exe/mvs.h"
  7. #include "exe/sfm.h"
  8. #include "exe/vocab_tree.h"
  9. #include "util/version.h"

colmap.cc是可执行文件的主文件,从app或者终端传入的命令会由该文件处理。

(比如打开UI界面的命令colmap gui,就是由这个文件处理)

从该文件中可以看出,colmap重建整体流程为:

colmap在automatic_reconstruct模式下,需要传入的参数为image_path以及images所在的文件夹。

colmap在不使用自动重建时,在不同阶段需要传入的参数分别为:

  • feature_extractor,需要指定image_path和一个空的database_path,在特征提取结束后,提取完的特征以及其他信息被记录在database_path指定的.db文件中
  • exhaustive_matcher,需要给入上一轮生成的database文件,这一步详尽匹配的结果会继续记录在该.db文件中
  • colmap mapper,需要给入image_path以及上一轮生成的.db文件,以及一个output_path用来存入生成的model

colmap.cc的主要函数有两个,main函数:接收传入参数,根据参数执行命令;ShowHelp函数:当参数出现错误或者参数为-h时输出帮助命令。

基于ceres的RunBundleAdjuster

commands.emplace_back("bundle_adjuster", &RunBundleAdjuster);为例,它定义在sfm文件里

  1. namespace colmap {
  2. int RunAutomaticReconstructor(int argc, char** argv);
  3. int RunBundleAdjuster(int argc, char** argv);
  4. int RunColorExtractor(int argc, char** argv);
  5. int RunMapper(int argc, char** argv);
  6. int RunHierarchicalMapper(int argc, char** argv);
  7. int RunPointFiltering(int argc, char** argv);
  8. int RunPointTriangulator(int argc, char** argv);
  9. int RunRigBundleAdjuster(int argc, char** argv);
  10. }
  1. int RunBundleAdjuster(int argc, char** argv) {
  2. std::string input_path;
  3. std::string output_path;
  4. OptionManager options;
  5. options.AddRequiredOption("input_path", &input_path);
  6. options.AddRequiredOption("output_path", &output_path);
  7. options.AddBundleAdjustmentOptions();
  8. options.Parse(argc, argv);
  9. if (!ExistsDir(input_path)) {
  10. std::cerr << "ERROR: `input_path` is not a directory" << std::endl;
  11. return EXIT_FAILURE;
  12. }
  13. if (!ExistsDir(output_path)) {
  14. std::cerr << "ERROR: `output_path` is not a directory" << std::endl;
  15. return EXIT_FAILURE;
  16. }
  17. Reconstruction reconstruction;
  18. reconstruction.Read(input_path);
  19. BundleAdjustmentController ba_controller(options, &reconstruction); // BA
  20. ba_controller.Start();
  21. ba_controller.Wait();
  22. reconstruction.Write(output_path);
  23. return EXIT_SUCCESS;
  24. }

可以看到,BundleAdjustmentController为关键

控制全局BA的BundleAdjustmentController

RunBundleAdjusterBundleAdjustmentController定义在bundle_adjustment.h里,它是控制全局BA的类,该类的run方法定义了BA过程。

  1. // Class that controls the global bundle adjustment procedure.
  2. class BundleAdjustmentController : public Thread {
  3. public:
  4. BundleAdjustmentController(const OptionManager& options,
  5. Reconstruction* reconstruction);
  6. private:
  7. void Run();
  8. const OptionManager options_;
  9. Reconstruction* reconstruction_;
  10. };

1.构造函数

  1. BundleAdjustmentController::BundleAdjustmentController(
  2. const OptionManager& options, Reconstruction* reconstruction)
  3. : options_(options), reconstruction_(reconstruction) {}

2.Run()函数

  1. void BundleAdjustmentController::Run() {
  2. CHECK_NOTNULL(reconstruction_);
  3. PrintHeading1("Global bundle adjustment");
  4. const std::vector<image_t>& reg_image_ids = reconstruction_->RegImageIds();
  5. if (reg_image_ids.size() < 2) {
  6. std::cout << "ERROR: Need at least two views." << std::endl;
  7. return;
  8. }
  9. // Avoid degeneracies in bundle adjustment.
  10. reconstruction_->FilterObservationsWithNegativeDepth();
  11. BundleAdjustmentOptions ba_options = *options_.bundle_adjustment;
  12. ba_options.solver_options.minimizer_progress_to_stdout = true;
  13. BundleAdjustmentIterationCallback iteration_callback(this);
  14. ba_options.solver_options.callbacks.push_back(&iteration_callback);
  15. // Configure bundle adjustment.
  16. BundleAdjustmentConfig ba_config;
  17. for (const image_t image_id : reg_image_ids) {
  18. ba_config.AddImage(image_id);
  19. }
  20. ba_config.SetConstantPose(reg_image_ids[0]);
  21. ba_config.SetConstantTvec(reg_image_ids[1], {0});
  22. // Run bundle adjustment.
  23. BundleAdjuster bundle_adjuster(ba_options, ba_config);
  24. bundle_adjuster.Solve(reconstruction_);
  25. GetTimer().PrintMinutes();
  26. }

Run()函数里调用了BundleAdjuster,BundleAdjuster是利用Ceres-Solver解决问题

  1. BundleAdjuster bundle_adjuster(ba_options, ba_config);
  2. bundle_adjuster.Solve(reconstruction_);

基于Ceres-Solver的BundleAdjuster

  1. class BundleAdjuster {
  2. public:
  3. BundleAdjuster(const BundleAdjustmentOptions& options,
  4. const BundleAdjustmentConfig& config);
  5. bool Solve(Reconstruction* reconstruction);
  6. // Get the Ceres solver summary for the last call to `Solve`.
  7. const ceres::Solver::Summary& Summary() const;
  8. private:
  9. void SetUp(Reconstruction* reconstruction,
  10. ceres::LossFunction* loss_function);
  11. void TearDown(Reconstruction* reconstruction);
  12. void AddImageToProblem(const image_t image_id, Reconstruction* reconstruction,
  13. ceres::LossFunction* loss_function);
  14. void AddPointToProblem(const point3D_t point3D_id,
  15. Reconstruction* reconstruction,
  16. ceres::LossFunction* loss_function);
  17. protected:
  18. void ParameterizeCameras(Reconstruction* reconstruction);
  19. void ParameterizePoints(Reconstruction* reconstruction);
  20. const BundleAdjustmentOptions options_;
  21. BundleAdjustmentConfig config_;
  22. std::unique_ptr<ceres::Problem> problem_;
  23. ceres::Solver::Summary summary_;
  24. std::unordered_set<camera_t> camera_ids_;
  25. std::unordered_map<point3D_t, size_t> point3D_num_observations_;
  26. };

私有成员

problem 和 Summary

  1. std::unique_ptr<ceres::Problem> problem_;
  2. ceres::Solver::Summary summary_;
  1. const ceres::Solver::Summary& Summary() const;
  2. const ceres::Solver::Summary& BundleAdjuster::Summary() const {
  3. return summary_;
  4. }

构造函数

  1. BundleAdjuster::BundleAdjuster(const BundleAdjustmentOptions& options,
  2. const BundleAdjustmentConfig& config)
  3. : options_(options), config_(config) {
  4. CHECK(options_.Check());
  5. }

构造函数里赋值了options_与config_,这里看一下他们的类型

BundleAdjustmentOptions

  1. struct BundleAdjustmentOptions {
  2. // Loss function types: Trivial (non-robust) and Cauchy (robust) loss.
  3. enum class LossFunctionType { TRIVIAL, SOFT_L1, CAUCHY };
  4. LossFunctionType loss_function_type = LossFunctionType::TRIVIAL;
  5. // Scaling factor determines residual at which robustification takes place.
  6. double loss_function_scale = 1.0;
  7. // Whether to refine the focal length parameter group.
  8. bool refine_focal_length = true;
  9. // Whether to refine the principal point parameter group.
  10. bool refine_principal_point = false;
  11. // Whether to refine the extra parameter group.
  12. bool refine_extra_params = true;
  13. // Whether to refine the extrinsic parameter group.
  14. bool refine_extrinsics = true;
  15. // Whether to print a final summary.
  16. bool print_summary = true;
  17. // Minimum number of residuals to enable multi-threading. Note that
  18. // single-threaded is typically better for small bundle adjustment problems
  19. // due to the overhead of threading.
  20. int min_num_residuals_for_multi_threading = 50000;
  21. // Ceres-Solver options.
  22. ceres::Solver::Options solver_options;
  23. BundleAdjustmentOptions() {
  24. solver_options.function_tolerance = 0.0;
  25. solver_options.gradient_tolerance = 0.0;
  26. solver_options.parameter_tolerance = 0.0;
  27. solver_options.minimizer_progress_to_stdout = false;
  28. solver_options.max_num_iterations = 100;
  29. solver_options.max_linear_solver_iterations = 200;
  30. solver_options.max_num_consecutive_invalid_steps = 10;
  31. solver_options.max_consecutive_nonmonotonic_steps = 10;
  32. solver_options.num_threads = -1;
  33. #if CERES_VERSION_MAJOR < 2
  34. solver_options.num_linear_solver_threads = -1;
  35. #endif // CERES_VERSION_MAJOR
  36. }
  37. // Create a new loss function based on the specified options. The caller
  38. // takes ownership of the loss function.
  39. ceres::LossFunction* CreateLossFunction() const;
  40. bool Check() const;
  41. };

其中 BundleAdjustmentOptions定义了诸多选项,
以及ceres::Solver::Options solver_options;ceres::LossFunction* CreateLossFunction() const;

BundleAdjustmentConfig

  1. // Configuration container to setup bundle adjustment problems.
  2. class BundleAdjustmentConfig {
  3. public:
  4. BundleAdjustmentConfig();
  5. size_t NumImages() const;
  6. size_t NumPoints() const;
  7. size_t NumConstantCameras() const;
  8. size_t NumConstantPoses() const;
  9. size_t NumConstantTvecs() const;
  10. size_t NumVariablePoints() const;
  11. size_t NumConstantPoints() const;
  12. // Determine the number of residuals for the given reconstruction. The number
  13. // of residuals equals the number of observations times two.
  14. size_t NumResiduals(const Reconstruction& reconstruction) const;
  15. // Add / remove images from the configuration.
  16. void AddImage(const image_t image_id);
  17. bool HasImage(const image_t image_id) const;
  18. void RemoveImage(const image_t image_id);
  19. // Set cameras of added images as constant or variable. By default all
  20. // cameras of added images are variable. Note that the corresponding images
  21. // have to be added prior to calling these methods.
  22. void SetConstantCamera(const camera_t camera_id);
  23. void SetVariableCamera(const camera_t camera_id);
  24. bool IsConstantCamera(const camera_t camera_id) const;
  25. // Set the pose of added images as constant. The pose is defined as the
  26. // rotational and translational part of the projection matrix.
  27. void SetConstantPose(const image_t image_id);
  28. void SetVariablePose(const image_t image_id);
  29. bool HasConstantPose(const image_t image_id) const;
  30. // Set the translational part of the pose, hence the constant pose
  31. // indices may be in [0, 1, 2] and must be unique. Note that the
  32. // corresponding images have to be added prior to calling these methods.
  33. void SetConstantTvec(const image_t image_id, const std::vector<int>& idxs);
  34. void RemoveConstantTvec(const image_t image_id);
  35. bool HasConstantTvec(const image_t image_id) const;
  36. // Add / remove points from the configuration. Note that points can either
  37. // be variable or constant but not both at the same time.
  38. void AddVariablePoint(const point3D_t point3D_id);
  39. void AddConstantPoint(const point3D_t point3D_id);
  40. bool HasPoint(const point3D_t point3D_id) const;
  41. bool HasVariablePoint(const point3D_t point3D_id) const;
  42. bool HasConstantPoint(const point3D_t point3D_id) const;
  43. void RemoveVariablePoint(const point3D_t point3D_id);
  44. void RemoveConstantPoint(const point3D_t point3D_id);
  45. // Access configuration data.
  46. const std::unordered_set<image_t>& Images() const;
  47. const std::unordered_set<point3D_t>& VariablePoints() const;
  48. const std::unordered_set<point3D_t>& ConstantPoints() const;
  49. const std::vector<int>& ConstantTvec(const image_t image_id) const;
  50. private:
  51. std::unordered_set<camera_t> constant_camera_ids_;
  52. std::unordered_set<image_t> image_ids_;
  53. std::unordered_set<point3D_t> variable_point3D_ids_;
  54. std::unordered_set<point3D_t> constant_point3D_ids_;
  55. std::unordered_set<image_t> constant_poses_;
  56. std::unordered_map<image_t, std::vector<int>> constant_tvecs_;
  57. };

然后我们来看BundleAdjuster的主体,BundleAdjuster::Solve()函数

1. 定义问题problem

 problem_.reset(new ceres::Problem());

2. 设置目标函数loss_function

ceres::LossFunction* loss_function = options_.CreateLossFunction();
  1. ceres::LossFunction* BundleAdjustmentOptions::CreateLossFunction() const {
  2. ceres::LossFunction* loss_function = nullptr;
  3. switch (loss_function_type) {
  4. case LossFunctionType::TRIVIAL:
  5. loss_function = new ceres::TrivialLoss();
  6. break;
  7. case LossFunctionType::SOFT_L1:
  8. loss_function = new ceres::SoftLOneLoss(loss_function_scale);
  9. break;
  10. case LossFunctionType::CAUCHY:
  11. loss_function = new ceres::CauchyLoss(loss_function_scale);
  12. break;
  13. }
  14. CHECK_NOTNULL(loss_function);
  15. return loss_function;
  16. }

3. 通过SetUp将图像和点加进去

  1. void BundleAdjuster::SetUp(Reconstruction* reconstruction,
  2. ceres::LossFunction* loss_function) {
  3. // Warning: AddPointsToProblem assumes that AddImageToProblem is called first.
  4. // Do not change order of instructions!
  5. for (const image_t image_id : config_.Images()) {
  6. AddImageToProblem(image_id, reconstruction, loss_function);
  7. }
  8. for (const auto point3D_id : config_.VariablePoints()) {
  9. AddPointToProblem(point3D_id, reconstruction, loss_function);
  10. }
  11. for (const auto point3D_id : config_.ConstantPoints()) {
  12. AddPointToProblem(point3D_id, reconstruction, loss_function);
  13. }
  14. ParameterizeCameras(reconstruction);
  15. ParameterizePoints(reconstruction);
  16. }

AddImageToProblem()AddPointToProblem()函数将图像和点加到problem里,这两个函数的具体代码就不展示了,这两个函数中设计误差项添加的代码如下:

  1. cost_function = BundleAdjustmentConstantPoseCostFunction<CameraModel>::Create(
  2. image.Qvec(), image.Tvec(), point2D.XY());
  3. problem_->AddResidualBlock(cost_function, loss_function,
  4. point3D.XYZ().data(), camera_params_data);

  1. // Set pose parameterization.
  2. if (!constant_pose) {
  3. ceres::LocalParameterization* quaternion_parameterization =
  4. new ceres::QuaternionParameterization;
  5. problem_->SetParameterization(qvec_data, quaternion_parameterization);
  6. if (config_.HasConstantTvec(image_id)) {
  7. const std::vector<int>& constant_tvec_idxs =
  8. config_.ConstantTvec(image_id);
  9. ceres::SubsetParameterization* tvec_parameterization =
  10. new ceres::SubsetParameterization(3, constant_tvec_idxs);
  11. problem_->SetParameterization(tvec_data, tvec_parameterization);
  12. }
  13. }

其中BundleAdjustmentConstantPoseCostFunction定义于base/cost_functions里,其中包括了大量其他代价函数。该文件涉及ceres的诸多方法:

  1. static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) {
  2. return (new ceres::AutoDiffCostFunction<
  3. RigBundleAdjustmentCostFunction<CameraModel>, 2, 4, 3, 4, 3, 3,
  4. CameraModel::kNumParams>(
  5. new RigBundleAdjustmentCostFunction(point2D)));
  6. }
  7. ceres::QuaternionProduct(rel_qvec, rig_qvec, qvec);
  8. ceres::UnitQuaternionRotatePoint(rel_qvec, rig_tvec, tvec);

4.在options里配置各种优化的选项(接着上面的3)

  1. ceres::Solver::Options solver_options = options_.solver_options;
  2. const bool has_sparse =
  3. solver_options.sparse_linear_algebra_library_type != ceres::NO_SPARSE;
  4. // Empirical choice.
  5. const size_t kMaxNumImagesDirectDenseSolver = 50;
  6. const size_t kMaxNumImagesDirectSparseSolver = 1000;
  7. const size_t num_images = config_.NumImages();
  8. if (num_images <= kMaxNumImagesDirectDenseSolver) {
  9. solver_options.linear_solver_type = ceres::DENSE_SCHUR;
  10. } else if (num_images <= kMaxNumImagesDirectSparseSolver && has_sparse) {
  11. solver_options.linear_solver_type = ceres::SPARSE_SCHUR;
  12. } else { // Indirect sparse (preconditioned CG) solver.
  13. solver_options.linear_solver_type = ceres::ITERATIVE_SCHUR;
  14. solver_options.preconditioner_type = ceres::SCHUR_JACOBI;
  15. }

5.多线程设计

  1. if (problem_->NumResiduals() <
  2. options_.min_num_residuals_for_multi_threading) {
  3. solver_options.num_threads = 1;
  4. #if CERES_VERSION_MAJOR < 2
  5. solver_options.num_linear_solver_threads = 1;
  6. #endif // CERES_VERSION_MAJOR
  7. } else {
  8. solver_options.num_threads =
  9. GetEffectiveNumThreads(solver_options.num_threads);
  10. #if CERES_VERSION_MAJOR < 2
  11. solver_options.num_linear_solver_threads =
  12. GetEffectiveNumThreads(solver_options.num_linear_solver_threads);
  13. #endif // CERES_VERSION_MAJOR
  14. }

6.求解

ceres::Solve(solver_options, problem_.get(), &summary_);

相关博客:关于Colmap中BA的Ceres源码_又决定放弃的博客-CSDN博客

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/人工智能uu/article/detail/768610
推荐阅读
相关标签
  

闽ICP备14008679号