COLMAP后端优化-代码阅读

转载于

【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中：

int main(int argc, char** argv) {
  using namespace colmap;

  InitializeGlog(argv);
#ifdef GUI_ENABLED
  Q_INIT_RESOURCE(resources);
#endif

  std::vector> commands;
  commands.emplace_back("gui", &RunGraphicalUserInterface);
  commands.emplace_back("automatic_reconstructor", &RunAutomaticReconstructor);
  commands.emplace_back("bundle_adjuster", &RunBundleAdjuster);
  commands.emplace_back("color_extractor", &RunColorExtractor);
  commands.emplace_back("database_cleaner", &RunDatabaseCleaner);
  commands.emplace_back("database_creator", &RunDatabaseCreator);
  commands.emplace_back("database_merger", &RunDatabaseMerger);
  commands.emplace_back("delaunay_mesher", &RunDelaunayMesher);
  commands.emplace_back("exhaustive_matcher", &RunExhaustiveMatcher);
  commands.emplace_back("feature_extractor", &RunFeatureExtractor);
  commands.emplace_back("feature_importer", &RunFeatureImporter);
  commands.emplace_back("hierarchical_mapper", &RunHierarchicalMapper);
  commands.emplace_back("image_deleter", &RunImageDeleter);
  commands.emplace_back("image_filterer", &RunImageFilterer);
  commands.emplace_back("image_rectifier", &RunImageRectifier);
  commands.emplace_back("image_registrator", &RunImageRegistrator);
  commands.emplace_back("image_undistorter", &RunImageUndistorter);
  commands.emplace_back("image_undistorter_standalone",
                        &RunImageUndistorterStandalone);
  commands.emplace_back("mapper", &RunMapper);
  commands.emplace_back("matches_importer", &RunMatchesImporter);
  commands.emplace_back("model_aligner", &RunModelAligner);
  commands.emplace_back("model_analyzer", &RunModelAnalyzer);
  commands.emplace_back("model_comparer", &RunModelComparer);
  commands.emplace_back("model_converter", &RunModelConverter);
  commands.emplace_back("model_cropper", &RunModelCropper);
  commands.emplace_back("model_merger", &RunModelMerger);
  commands.emplace_back("model_orientation_aligner",
                        &RunModelOrientationAligner);
  commands.emplace_back("model_splitter", &RunModelSplitter);
  commands.emplace_back("model_transformer", &RunModelTransformer);
  commands.emplace_back("patch_match_stereo", &RunPatchMatchStereo);
  commands.emplace_back("point_filtering", &RunPointFiltering);
  commands.emplace_back("point_triangulator", &RunPointTriangulator);
  commands.emplace_back("poisson_mesher", &RunPoissonMesher);
  commands.emplace_back("project_generator", &RunProjectGenerator);
  commands.emplace_back("rig_bundle_adjuster", &RunRigBundleAdjuster);
  commands.emplace_back("sequential_matcher", &RunSequentialMatcher);
  commands.emplace_back("spatial_matcher", &RunSpatialMatcher);
  commands.emplace_back("stereo_fusion", &RunStereoFuser);
  commands.emplace_back("transitive_matcher", &RunTransitiveMatcher);
  commands.emplace_back("vocab_tree_builder", &RunVocabTreeBuilder);
  commands.emplace_back("vocab_tree_matcher", &RunVocabTreeMatcher);
  commands.emplace_back("vocab_tree_retriever", &RunVocabTreeRetriever);

  if (argc == 1) {
    return ShowHelp(commands);
  }

  const std::string command = argv[1];
  if (command == "help" || command == "-h" || command == "--help") {
    return ShowHelp(commands);
  } else {
    command_func_t matched_command_func = nullptr;
    for (const auto& command_func : commands) {
      if (command == command_func.first) {
        matched_command_func = command_func.second;
        break;
      }
    }
    if (matched_command_func == nullptr) {
      std::cerr << StringPrintf(
                       "ERROR: Command `%s` not recognized. To list the "
                       "available commands, run `colmap help`.",
                       command.c_str())
                << std::endl;
      return EXIT_FAILURE;
    } else {
      int command_argc = argc - 1;
      char** command_argv = &argv[1];
      command_argv[0] = argv[0];
      return matched_command_func(command_argc, command_argv);
    }
  }

  return ShowHelp(commands);
}

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

#include "exe/database.h"
#include "exe/feature.h"
#include "exe/gui.h"
#include "exe/image.h"
#include "exe/model.h"
#include "exe/mvs.h"
#include "exe/sfm.h"
#include "exe/vocab_tree.h"
#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文件里

namespace colmap {

int RunAutomaticReconstructor(int argc, char** argv);
int RunBundleAdjuster(int argc, char** argv);
int RunColorExtractor(int argc, char** argv);
int RunMapper(int argc, char** argv);
int RunHierarchicalMapper(int argc, char** argv);
int RunPointFiltering(int argc, char** argv);
int RunPointTriangulator(int argc, char** argv);
int RunRigBundleAdjuster(int argc, char** argv);

} 

int RunBundleAdjuster(int argc, char** argv) {
  std::string input_path;
  std::string output_path;

  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
  options.AddBundleAdjustmentOptions();
  options.Parse(argc, argv);

  if (!ExistsDir(input_path)) {
    std::cerr << "ERROR: `input_path` is not a directory" << std::endl;
    return EXIT_FAILURE;
  }

  if (!ExistsDir(output_path)) {
    std::cerr << "ERROR: `output_path` is not a directory" << std::endl;
    return EXIT_FAILURE;
  }

  Reconstruction reconstruction;
  reconstruction.Read(input_path);

  BundleAdjustmentController ba_controller(options, &reconstruction); //   BA
  ba_controller.Start();
  ba_controller.Wait();

  reconstruction.Write(output_path);

  return EXIT_SUCCESS;
}

可以看到，BundleAdjustmentController为关键

控制全局BA的BundleAdjustmentController

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

// Class that controls the global bundle adjustment procedure.
class BundleAdjustmentController : public Thread {
 public:
  BundleAdjustmentController(const OptionManager& options,
                             Reconstruction* reconstruction);

 private:
  void Run();

  const OptionManager options_;
  Reconstruction* reconstruction_;
};

1.构造函数

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

2.Run()函数

void BundleAdjustmentController::Run() {
  CHECK_NOTNULL(reconstruction_);

  PrintHeading1("Global bundle adjustment");

  const std::vector& reg_image_ids = reconstruction_->RegImageIds();

  if (reg_image_ids.size() < 2) {
    std::cout << "ERROR: Need at least two views." << std::endl;
    return;
  }

  // Avoid degeneracies in bundle adjustment.
  reconstruction_->FilterObservationsWithNegativeDepth();

  BundleAdjustmentOptions ba_options = *options_.bundle_adjustment;
  ba_options.solver_options.minimizer_progress_to_stdout = true;

  BundleAdjustmentIterationCallback iteration_callback(this);
  ba_options.solver_options.callbacks.push_back(&iteration_callback);

  // Configure bundle adjustment.
  BundleAdjustmentConfig ba_config;
  for (const image_t image_id : reg_image_ids) {
    ba_config.AddImage(image_id);
  }
  ba_config.SetConstantPose(reg_image_ids[0]);
  ba_config.SetConstantTvec(reg_image_ids[1], {0});

  // Run bundle adjustment.
  BundleAdjuster bundle_adjuster(ba_options, ba_config);
  bundle_adjuster.Solve(reconstruction_);

  GetTimer().PrintMinutes();
}

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

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

基于Ceres-Solver的BundleAdjuster

class BundleAdjuster {
 public:
  BundleAdjuster(const BundleAdjustmentOptions& options,
                 const BundleAdjustmentConfig& config);

  bool Solve(Reconstruction* reconstruction);

  // Get the Ceres solver summary for the last call to `Solve`.
  const ceres::Solver::Summary& Summary() const;

 private:
  void SetUp(Reconstruction* reconstruction,
             ceres::LossFunction* loss_function);
  void TearDown(Reconstruction* reconstruction);

  void AddImageToProblem(const image_t image_id, Reconstruction* reconstruction,
                         ceres::LossFunction* loss_function);

  void AddPointToProblem(const point3D_t point3D_id,
                         Reconstruction* reconstruction,
                         ceres::LossFunction* loss_function);

 protected:
  void ParameterizeCameras(Reconstruction* reconstruction);
  void ParameterizePoints(Reconstruction* reconstruction);

  const BundleAdjustmentOptions options_;
  BundleAdjustmentConfig config_;
  std::unique_ptr problem_;
  ceres::Solver::Summary summary_;
  std::unordered_set camera_ids_;
  std::unordered_map point3D_num_observations_;
};

私有成员

problem 和 Summary

std::unique_ptr problem_;
ceres::Solver::Summary summary_;

const ceres::Solver::Summary& Summary() const;

const ceres::Solver::Summary& BundleAdjuster::Summary() const {
  return summary_;
}

构造函数

BundleAdjuster::BundleAdjuster(const BundleAdjustmentOptions& options,
                               const BundleAdjustmentConfig& config)
    : options_(options), config_(config) {
  CHECK(options_.Check());
}

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

BundleAdjustmentOptions

struct BundleAdjustmentOptions {
  // Loss function types: Trivial (non-robust) and Cauchy (robust) loss.
  enum class LossFunctionType { TRIVIAL, SOFT_L1, CAUCHY };
  LossFunctionType loss_function_type = LossFunctionType::TRIVIAL;

  // Scaling factor determines residual at which robustification takes place.
  double loss_function_scale = 1.0;

  // Whether to refine the focal length parameter group.
  bool refine_focal_length = true;

  // Whether to refine the principal point parameter group.
  bool refine_principal_point = false;

  // Whether to refine the extra parameter group.
  bool refine_extra_params = true;

  // Whether to refine the extrinsic parameter group.
  bool refine_extrinsics = true;

  // Whether to print a final summary.
  bool print_summary = true;

  // Minimum number of residuals to enable multi-threading. Note that
  // single-threaded is typically better for small bundle adjustment problems
  // due to the overhead of threading.
  int min_num_residuals_for_multi_threading = 50000;

  // Ceres-Solver options.
  ceres::Solver::Options solver_options;

  BundleAdjustmentOptions() {
    solver_options.function_tolerance = 0.0;
    solver_options.gradient_tolerance = 0.0;
    solver_options.parameter_tolerance = 0.0;
    solver_options.minimizer_progress_to_stdout = false;
    solver_options.max_num_iterations = 100;
    solver_options.max_linear_solver_iterations = 200;
    solver_options.max_num_consecutive_invalid_steps = 10;
    solver_options.max_consecutive_nonmonotonic_steps = 10;
    solver_options.num_threads = -1;
#if CERES_VERSION_MAJOR < 2
    solver_options.num_linear_solver_threads = -1;
#endif  // CERES_VERSION_MAJOR
  }

  // Create a new loss function based on the specified options. The caller
  // takes ownership of the loss function.
  ceres::LossFunction* CreateLossFunction() const;

  bool Check() const;
};

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

BundleAdjustmentConfig

// Configuration container to setup bundle adjustment problems.
class BundleAdjustmentConfig {
 public:
  BundleAdjustmentConfig();

  size_t NumImages() const;
  size_t NumPoints() const;
  size_t NumConstantCameras() const;
  size_t NumConstantPoses() const;
  size_t NumConstantTvecs() const;
  size_t NumVariablePoints() const;
  size_t NumConstantPoints() const;

  // Determine the number of residuals for the given reconstruction. The number
  // of residuals equals the number of observations times two.
  size_t NumResiduals(const Reconstruction& reconstruction) const;

  // Add / remove images from the configuration.
  void AddImage(const image_t image_id);
  bool HasImage(const image_t image_id) const;
  void RemoveImage(const image_t image_id);

  // Set cameras of added images as constant or variable. By default all
  // cameras of added images are variable. Note that the corresponding images
  // have to be added prior to calling these methods.
  void SetConstantCamera(const camera_t camera_id);
  void SetVariableCamera(const camera_t camera_id);
  bool IsConstantCamera(const camera_t camera_id) const;

  // Set the pose of added images as constant. The pose is defined as the
  // rotational and translational part of the projection matrix.
  void SetConstantPose(const image_t image_id);
  void SetVariablePose(const image_t image_id);
  bool HasConstantPose(const image_t image_id) const;

  // Set the translational part of the pose, hence the constant pose
  // indices may be in [0, 1, 2] and must be unique. Note that the
  // corresponding images have to be added prior to calling these methods.
  void SetConstantTvec(const image_t image_id, const std::vector& idxs);
  void RemoveConstantTvec(const image_t image_id);
  bool HasConstantTvec(const image_t image_id) const;

  // Add / remove points from the configuration. Note that points can either
  // be variable or constant but not both at the same time.
  void AddVariablePoint(const point3D_t point3D_id);
  void AddConstantPoint(const point3D_t point3D_id);
  bool HasPoint(const point3D_t point3D_id) const;
  bool HasVariablePoint(const point3D_t point3D_id) const;
  bool HasConstantPoint(const point3D_t point3D_id) const;
  void RemoveVariablePoint(const point3D_t point3D_id);
  void RemoveConstantPoint(const point3D_t point3D_id);

  // Access configuration data.
  const std::unordered_set& Images() const;
  const std::unordered_set& VariablePoints() const;
  const std::unordered_set& ConstantPoints() const;
  const std::vector& ConstantTvec(const image_t image_id) const;

 private:
  std::unordered_set constant_camera_ids_;
  std::unordered_set image_ids_;
  std::unordered_set variable_point3D_ids_;
  std::unordered_set constant_point3D_ids_;
  std::unordered_set constant_poses_;
  std::unordered_map> constant_tvecs_;
};

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

1. 定义问题problem

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

2. 设置目标函数loss_function

ceres::LossFunction* loss_function = options_.CreateLossFunction();

ceres::LossFunction* BundleAdjustmentOptions::CreateLossFunction() const {
  ceres::LossFunction* loss_function = nullptr;
  switch (loss_function_type) {
    case LossFunctionType::TRIVIAL:
      loss_function = new ceres::TrivialLoss();
      break;
    case LossFunctionType::SOFT_L1:
      loss_function = new ceres::SoftLOneLoss(loss_function_scale);
      break;
    case LossFunctionType::CAUCHY:
      loss_function = new ceres::CauchyLoss(loss_function_scale);
      break;
  }
  CHECK_NOTNULL(loss_function);
  return loss_function;
}

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

void BundleAdjuster::SetUp(Reconstruction* reconstruction,
                           ceres::LossFunction* loss_function) {
  // Warning: AddPointsToProblem assumes that AddImageToProblem is called first.
  // Do not change order of instructions!
  for (const image_t image_id : config_.Images()) {
    AddImageToProblem(image_id, reconstruction, loss_function);
  }
  for (const auto point3D_id : config_.VariablePoints()) {
    AddPointToProblem(point3D_id, reconstruction, loss_function);
  }
  for (const auto point3D_id : config_.ConstantPoints()) {
    AddPointToProblem(point3D_id, reconstruction, loss_function);
  }

  ParameterizeCameras(reconstruction);
  ParameterizePoints(reconstruction);
}

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

cost_function = BundleAdjustmentConstantPoseCostFunction::Create( 
            image.Qvec(), image.Tvec(), point2D.XY());

problem_->AddResidualBlock(cost_function, loss_function,
                                 point3D.XYZ().data(), camera_params_data);

和

// Set pose parameterization.
    if (!constant_pose) {
      ceres::LocalParameterization* quaternion_parameterization =
          new ceres::QuaternionParameterization;
      problem_->SetParameterization(qvec_data, quaternion_parameterization);
      if (config_.HasConstantTvec(image_id)) {
        const std::vector& constant_tvec_idxs =
            config_.ConstantTvec(image_id);
        ceres::SubsetParameterization* tvec_parameterization =
            new ceres::SubsetParameterization(3, constant_tvec_idxs);
        problem_->SetParameterization(tvec_data, tvec_parameterization);
      }
    }

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

static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) {
    return (new ceres::AutoDiffCostFunction<
            RigBundleAdjustmentCostFunction, 2, 4, 3, 4, 3, 3,
            CameraModel::kNumParams>(
        new RigBundleAdjustmentCostFunction(point2D)));
  }

ceres::QuaternionProduct(rel_qvec, rig_qvec, qvec);


ceres::UnitQuaternionRotatePoint(rel_qvec, rig_tvec, tvec);

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

ceres::Solver::Options solver_options = options_.solver_options;

const bool has_sparse =
      solver_options.sparse_linear_algebra_library_type != ceres::NO_SPARSE;

// Empirical choice.
const size_t kMaxNumImagesDirectDenseSolver = 50;
const size_t kMaxNumImagesDirectSparseSolver = 1000;
const size_t num_images = config_.NumImages();
if (num_images <= kMaxNumImagesDirectDenseSolver) {
  solver_options.linear_solver_type = ceres::DENSE_SCHUR;
} else if (num_images <= kMaxNumImagesDirectSparseSolver && has_sparse) {
  solver_options.linear_solver_type = ceres::SPARSE_SCHUR;
} else {  // Indirect sparse (preconditioned CG) solver.
  solver_options.linear_solver_type = ceres::ITERATIVE_SCHUR;
  solver_options.preconditioner_type = ceres::SCHUR_JACOBI;
}

5.多线程设计

if (problem_->NumResiduals() <
      options_.min_num_residuals_for_multi_threading) {
    solver_options.num_threads = 1;
#if CERES_VERSION_MAJOR < 2
    solver_options.num_linear_solver_threads = 1;
#endif  // CERES_VERSION_MAJOR
  } else {
    solver_options.num_threads =
        GetEffectiveNumThreads(solver_options.num_threads);
#if CERES_VERSION_MAJOR < 2
    solver_options.num_linear_solver_threads =
        GetEffectiveNumThreads(solver_options.num_linear_solver_threads);
#endif  // CERES_VERSION_MAJOR
  }

6.求解

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

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