Ceres-Solver学习笔记(3)

Ceres的一个主要目的是解决大尺度bundle adjustment 问题。多说无益，直接上代码：

// file bal_problem.h#ifndef CERES_EXAMPLES_BAL_PROBLEM_H_#define CERES_EXAMPLES_BAL_PROBLEM_H_#include <string>namespace ceres {namespace examples {class BALProblem { public:  explicit BALProblem(const std::string& filename, bool use_quaternions);  ~BALProblem();  void WriteToFile(const std::string& filename) const;  void WriteToPLYFile(const std::string& filename) const;  // 将重建的“中心”移到原点，中心是通过计算点的marginal median来确定的。  // 然后对重构进行缩放，以便从原点处得到的点的 median absolute deviation 是100.0  //  // 问题的重投影误差保持不变  void Normalize();  // 用特定标准差的随机正态分布值扰乱相机姿态和几何结构  void Perturb(const double rotation_sigma,               const double translation_sigma,               const double point_sigma);    //相机参数的个数  int camera_block_size()      const { return use_quaternions_ ? 10 : 9; }  // 点参数的个数  int point_block_size()       const { return 3;                         }  int num_cameras()            const { return num_cameras_;              }  int num_points()             const { return num_points_;               }  int num_observations()       const { return num_observations_;         }  int num_parameters()         const { return num_parameters_;           }  const int* point_index()     const { return point_index_;              }  const int* camera_index()    const { return camera_index_;             }  const double* observations() const { return observations_;             }  const double* parameters()   const { return parameters_;               }  const double* cameras()      const { return parameters_;               }  double* mutable_cameras()          { return parameters_;               }  double* mutable_points() {    return parameters_  + camera_block_size() * num_cameras_;  } private:  void CameraToAngleAxisAndCenter(const double* camera,                                  double* angle_axis,                                  double* center) const;  void AngleAxisAndCenterToCamera(const double* angle_axis,                                  const double* center,                                  double* camera) const;  int num_cameras_;  int num_points_;  int num_observations_;  int num_parameters_;  bool use_quaternions_;  int* point_index_;  int* camera_index_;  double* observations_;  // The parameter vector is laid out as follows  // [camera_1, ..., camera_n, point_1, ..., point_m]  double* parameters_;};}  // namespace examples}  // namespace ceres#endif  // CERES_EXAMPLES_BAL_PROBLEM_H_

// file bundle_adjuster.cc#include <algorithm>#include <cmath>#include <cstdio>#include <cstdlib>#include <string>#include <vector>#include "bal_problem.h"#include "ceres/ceres.h"#include "gflags/gflags.h"#include "glog/logging.h"#include "snavely_reprojection_error.h"namespace ceres {namespace examples {void SetLinearSolver(Solver::Options* options) {  // 设置LinearSolver，可选项为："sparse_schur, dense_schur, iterative_schur,     // sparse_normal_cholesky, ""dense_qr, dense_normal_cholesky and cgnr."  CHECK(StringToLinearSolverType(FLAGS_linear_solver,                                 &options->linear_solver_type));  // 设置PreconditionerType，可选项为："identity, jacobi,   // schur_jacobi, cluster_jacobi, ""cluster_tridiagonal."  CHECK(StringToPreconditionerType(FLAGS_preconditioner,                                   &options->preconditioner_type));  // 设置VisibilityClusteringType，可选项为："single_linkage,   // canonical_views"  CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,                                         &options->visibility_clustering_type));  // 设置SparseLinearAlgebraLibraryType，可选项为："suite_sparse,  // cx_sparse"  CHECK(StringToSparseLinearAlgebraLibraryType(            FLAGS_sparse_linear_algebra_library,            &options->sparse_linear_algebra_library_type));  // 设置DenseLinearAlgebraLibraryType，可选项为："eigen,  // lapack."  CHECK(StringToDenseLinearAlgebraLibraryType(            FLAGS_dense_linear_algebra_library,            &options->dense_linear_algebra_library_type));  // 线程数  options->num_linear_solver_threads = FLAGS_num_threads;  // 是否使用舍尔补  options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;}void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {  // 3D点数量  const int num_points = bal_problem->num_points();  // 点的维度（3维）  const int point_block_size = bal_problem->point_block_size();  // 点数据起始地址  double* points = bal_problem->mutable_points();  // 相机姿态数  const int num_cameras = bal_problem->num_cameras();  // 相机参数数（9/10）  const int camera_block_size = bal_problem->camera_block_size();  // 相机参数起始地址  double* cameras = bal_problem->mutable_cameras();  // true（使用内迭代对非线性进行）  // false（细化每个成功的信任区域步骤）  if (options->use_inner_iterations) {    // 可选参数类型automatic, cameras, points，(points,cameras),(cameras,points)    if (FLAGS_blocks_for_inner_iterations == "cameras") {      LOG(INFO) << "Camera blocks for inner iterations";      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);      for (int i = 0; i < num_cameras; ++i) {        // 添加元素到一个组        options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);      }    } else if (FLAGS_blocks_for_inner_iterations == "points") {      LOG(INFO) << "Point blocks for inner iterations";      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);      for (int i = 0; i < num_points; ++i) {        options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);      }    } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {      LOG(INFO) << "Camera followed by point blocks for inner iterations";      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);      for (int i = 0; i < num_cameras; ++i) {        options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);      }      for (int i = 0; i < num_points; ++i) {        options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);      }    } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {      LOG(INFO) << "Point followed by camera blocks for inner iterations";      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);      for (int i = 0; i < num_cameras; ++i) {        options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);      }      for (int i = 0; i < num_points; ++i) {        options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);      }    } else if (FLAGS_blocks_for_inner_iterations == "automatic") {      LOG(INFO) << "Choosing automatic blocks for inner iterations";    } else {      LOG(FATAL) << "Unknown block type for inner iterations: "                 << FLAGS_blocks_for_inner_iterations;    }  }  // BA问题有一个稀疏的结构，使它们能够适应更专业、更高效的解决方案策略。斯帕塞舒尔、登斯舒尔和迭代器的解决者利用了这种特殊的结构。  //  // 这可以通过指定Options::orderingtype=ceres::SCHUR，  // 在这种情况下，Ceres将自动确定正确的参数块排序，  // 或者手动指定一个合适的排序向量，定义Options::num_eliminate_blocks。  if (FLAGS_ordering == "automatic") {    return;  }  ceres::ParameterBlockOrdering* ordering =      new ceres::ParameterBlockOrdering;  // The points come before the cameras.  for (int i = 0; i < num_points; ++i) {    ordering->AddElementToGroup(points + point_block_size * i, 0);  }  for (int i = 0; i < num_cameras; ++i) {    // When using axis-angle, there is a single parameter block for    // the entire camera.    ordering->AddElementToGroup(cameras + camera_block_size * i, 1);  }  options->linear_solver_ordering.reset(ordering);}void SetMinimizerOptions(Solver::Options* options) {  // 最大迭代次数  options->max_num_iterations = FLAGS_num_iterations;  // 迭代过程是否输出  options->minimizer_progress_to_stdout = true;  // 线程数  options->num_threads = FLAGS_num_threads;  // 每次迭代的精度  options->eta = FLAGS_eta;  // 求解时间  options->max_solver_time_in_seconds = FLAGS_max_solver_time;  // 信任区间算法/nonmonotic  options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;  if (FLAGS_line_search) {    options->minimizer_type = ceres::LINE_SEARCH;  }     CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,                                        &options->trust_region_strategy_type));  // 可选项：raditional_dogleg,subspace_dogleg  CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));  options->use_inner_iterations = FLAGS_inner_iterations;}void SetSolverOptionsFromFlags(BALProblem* bal_problem,                               Solver::Options* options) {  SetMinimizerOptions(options);  SetLinearSolver(options);  SetOrdering(bal_problem, options);}void BuildProblem(BALProblem* bal_problem, Problem* problem) {  const int point_block_size = bal_problem->point_block_size();  const int camera_block_size = bal_problem->camera_block_size();  double* points = bal_problem->mutable_points();  double* cameras = bal_problem->mutable_cameras();  // Observations是特征的坐标 u v  const double* observations = bal_problem->observations();  for (int i = 0; i < bal_problem->num_observations(); ++i) {       CostFunction* cost_function;    // Each Residual block takes a point and a camera as input and    // outputs a 2 dimensional residual.    // 加入约束 特征位置    cost_function =        (FLAGS_use_quaternions)        ? SnavelyReprojectionErrorWithQuaternions::Create(            observations[2 * i + 0],            observations[2 * i + 1])        : SnavelyReprojectionError::Create(            observations[2 * i + 0],            observations[2 * i + 1]);    // If enabled use Huber's loss function.    LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;    // 没一个特征对应这一个相机姿态和一个三维点    // 加入点 和 相机    double* camera =        cameras + camera_block_size * bal_problem->camera_index()[i];    double* point = points + point_block_size * bal_problem->point_index()[i];    problem->AddResidualBlock(cost_function, loss_function, camera, point);  }  if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {    LocalParameterization* camera_parameterization =        new ProductParameterization(            new QuaternionParameterization(),            new IdentityParameterization(6));    for (int i = 0; i < bal_problem->num_cameras(); ++i) {      // ？？？？      problem->SetParameterization(cameras + camera_block_size * i,                                   camera_parameterization);    }  }}void SolveProblem(const char* filename) {  // 实例化BALProblem 导入文件信息  BALProblem bal_problem(filename, FLAGS_use_quaternions);  if (!FLAGS_initial_ply.empty()) {    bal_problem.WriteToPLYFile(FLAGS_initial_ply);  }  Problem problem;  srand(FLAGS_random_seed);  bal_problem.Normalize();  // 添加噪声  bal_problem.Perturb(FLAGS_rotation_sigma,                      FLAGS_translation_sigma,                      FLAGS_point_sigma);  // 添加约束  BuildProblem(&bal_problem, &problem);  Solver::Options options;  // 设置优化选项  SetSolverOptionsFromFlags(&bal_problem, &options);  options.gradient_tolerance = 1e-16;  options.function_tolerance = 1e-16;  Solver::Summary summary;  //求解  Solve(options, &problem, &summary);  std::cout << summary.FullReport() << "\n";  if (!FLAGS_final_ply.empty()) {    bal_problem.WriteToPLYFile(FLAGS_final_ply);  }}}  // namespace examples} // mainint main(int argc, char** argv) {  CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);  google::InitGoogleLogging(argv[0]);  if (FLAGS_input.empty()) {    LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem";    return 1;  }  CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)      << "--use_local_parameterization can only be used with "      << "--use_quaternions.";  ceres::examples::SolveProblem(FLAGS_input.c_str());  return 0;}

// file snavely_reprojection_error.h#include "ceres/rotation.h"namespace ceres {namespace examples {// 小孔相机模型.  camera用9个参数表示， 3 个表示旋转, 3 个表示平移, 1个焦距，2个径向畸变// 中心点没有建模，假设为图片中心struct SnavelyReprojectionError {  SnavelyReprojectionError(double observed_x, double observed_y)      : observed_x(observed_x), observed_y(observed_y) {}  template <typename T>  bool operator()(const T* const camera,                  const T* const point,                  T* residuals) const {    // camera[0,1,2] 是angle-axis 旋转.    T p[3];    AngleAxisRotatePoint(camera, point, p);    // camera[3,4,5] are the translation.    // 点转换到相机坐标系    p[0] += camera[3];    p[1] += camera[4];    p[2] += camera[5];    // Compute the center of distortion. The sign change comes from    // the camera model that Noah Snavely's Bundler assumes, whereby    // the camera coordinate system has a negative z axis.    const T xp = - p[0] / p[2];    const T yp = - p[1] / p[2];    // Apply second and fourth order radial distortion.    const T& l1 = camera[7];    const T& l2 = camera[8];    const T r2 = xp*xp + yp*yp;    const T distortion = T(1.0) + r2  * (l1 + l2  * r2);    // Compute final projected point position.    const T& focal = camera[6];    // 得到3D点投影到图像上的位置    const T predicted_x = focal * distortion * xp;    const T predicted_y = focal * distortion * yp;    // 特征位置与3D点计算位置的差    residuals[0] = predicted_x - T(observed_x);    residuals[1] = predicted_y - T(observed_y);    return true;  }  // Factory to hide the construction of the CostFunction object from  // the client code.  static ceres::CostFunction* Create(const double observed_x,                                     const double observed_y) {    // 使用AutoDiffCostFunction，2个残差，参数1有9维，参数2有3维    return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(                new SnavelyReprojectionError(observed_x, observed_y)));  }  double observed_x;  double observed_y;};// Templated pinhole camera model for used with Ceres.  The camera is// parameterized using 10 parameters. 4 for rotation, 3 for// translation, 1 for focal length and 2 for radial distortion. The// principal point is not modeled (i.e. it is assumed be located at// the image center).struct SnavelyReprojectionErrorWithQuaternions {  // (u, v): the position of the observation with respect to the image  // center point.  SnavelyReprojectionErrorWithQuaternions(double observed_x, double observed_y)      : observed_x(observed_x), observed_y(observed_y) {}  template <typename T>  bool operator()(const T* const camera,                  const T* const point,                  T* residuals) const {    // camera[0,1,2,3] is are the rotation of the camera as a quaternion.    //    // We use QuaternionRotatePoint as it does not assume that the    // quaternion is normalized, since one of the ways to run the    // bundle adjuster is to let Ceres optimize all 4 quaternion    // parameters without a local parameterization.    T p[3];    QuaternionRotatePoint(camera, point, p);    p[0] += camera[4];    p[1] += camera[5];    p[2] += camera[6];    // Compute the center of distortion. The sign change comes from    // the camera model that Noah Snavely's Bundler assumes, whereby    // the camera coordinate system has a negative z axis.    const T xp = - p[0] / p[2];    const T yp = - p[1] / p[2];    // Apply second and fourth order radial distortion.    const T& l1 = camera[8];    const T& l2 = camera[9];    const T r2 = xp*xp + yp*yp;    const T distortion = T(1.0) + r2  * (l1 + l2  * r2);    // Compute final projected point position.    const T& focal = camera[7];    const T predicted_x = focal * distortion * xp;    const T predicted_y = focal * distortion * yp;    // The error is the difference between the predicted and observed position.    residuals[0] = predicted_x - T(observed_x);    residuals[1] = predicted_y - T(observed_y);    return true;  }  // Factory to hide the construction of the CostFunction object from  // the client code.  static ceres::CostFunction* Create(const double observed_x,                                     const double observed_y) {    return (new ceres::AutoDiffCostFunction<            SnavelyReprojectionErrorWithQuaternions, 2, 10, 3>(                new SnavelyReprojectionErrorWithQuaternions(observed_x,                                                            observed_y)));  }  double observed_x;  double observed_y;};

这个例程比较长，看起来很复杂，但是实际上只是将options的一些选项，列举了出来，具体的使用也没有很好的介绍，本质上并没有什么太难的。options的可设置参数非常之多，具体可以看solver.h文件。