Ceres Solver使用总结

最新推荐文章于 2024-09-25 11:27:39 发布

Arrow

最新推荐文章于 2024-09-25 11:27:39 发布

阅读量9.6k

点赞数 2

分类专栏： SLAM

本文链接：https://blog.csdn.net/MyArrow/article/details/82906738

版权

SLAM 专栏收录该内容

24 篇文章 23 订阅

订阅专栏

1. 基本概念

Ceres Solver是一个开源C ++库，用于建模和解决大型复杂的优化问题。
用途
- 具有边界约束的非线性最小二乘问题
  $\min \limits_x \frac{1}{2} \sum \limits_i \rho_i( \Vert f_i(x_{i_1}, ..., x_{i_k}) \Vert^2)$
  $\quad l_j < x_j < u_j$
- 一般无约束优化问题(等价于 $\rho_i(x)=x$ )　
  $\min \limits_x \frac{1}{2} \sum \limits_i \Vert f_i(x_{i_1}, ..., x_{i_k}) \Vert^2$
元素描述
- $f_i(x_{i_1}, ..., x_{i_k})$ : 残差函数(Residual Function)
- $x_{i_1}, ..., x_{i_k}]$ ：参数块 (Parameter Block)
- $\rho_i( \Vert f_i(x_{i_1}, ..., x_{i_k}) \Vert^2)$ ：残差块(Residual Block)
- $\rho_i$ ：损失函数（Loss Function），它是一个标量函数，用于减少异常值对非线性最小二乘问题解的影响

2. 使用流程

第一步：定义残差函数 $f_i$ ：定义一个代价函数的结构体( $s t r u c t$ )，在结构体内重载（）运算符
第二步：创建一个 $C o s t F u n c t i o n$ （把 $f_i$ 作为其参数）
第三步：增加一个残差块（Problem.AddResidualBlock）
第四步：问题求解（Problem.Solve）
下面以曲线拟合 $y=e^{mx+c}$ 为例，有很多对(x,y)的观测值，求参数m和c

2.1 定义残差函数

struct ExponentialResidual {
  ExponentialResidual(double x, double y)
      : x_(x), y_(y) {}
  // ()函数的参数为待求值和残差值    
  template <typename T> bool operator()(const T* const m,
                                        const T* const c,
                                        T* residual) const {
    residual[0] = y_ - exp(m[0] * x_ + c[0]);
    return true;
  }
 private:
  const double x_;  // 保存观测值，本结构体创建进传入
  const double y_;
};

2.2 创建CostFunction 并增加残差块

为每个观测值创建一个CostFunction并加入Problem中
class CostFunction (残差对象)
这是用户和Ceres优化器之间的建模层。函数的签名（输入参数块的数量和大小以及输出的数量）分别存储在parameter_block_sizes_和num_residuals_中。从此类继承的用户代码应该使用相应的访问器设置这两个成员。添加AddResidualBlock（）时，将通过问题验证此信息。
class AutoDiffCostFunction
是CostFunction的派生类，它自动计算残差对每个参数的层数（即jacobian），它是一个模板类

template <typename CostFunctor,
          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
          int N0,       // Number of parameters in block 0.
          int N1 = 0,   // Number of parameters in block 1.
          int N2 = 0,   // Number of parameters in block 2.
          int N3 = 0,   // Number of parameters in block 3.
          int N4 = 0,   // Number of parameters in block 4.
          int N5 = 0,   // Number of parameters in block 5.
          int N6 = 0,   // Number of parameters in block 6.
          int N7 = 0,   // Number of parameters in block 7.
          int N8 = 0,   // Number of parameters in block 8.
          int N9 = 0>   // Number of parameters in block 9.
  // Takes ownership of functor. Uses the template-provided value for the
  // number of residuals ("kNumResiduals").
  // 支持残差个数为动态的：ceres::DYNAMIC
  explicit AutoDiffCostFunction(CostFunctor* functor)          

  // Takes ownership of functor. Ignores the template-provided
  // kNumResiduals in favor of the "num_residuals" argument provided.
  // This allows for having autodiff cost functions which return varying
  // numbers of residuals at runtime.
  //  不支持残差个数为动态的：ceres::DYNAMIC
  AutoDiffCostFunction(CostFunctor* functor, int num_residuals)

Problem.AddResidualBlock

  // Add a residual block to the overall cost function. The cost
  // function carries with it information about the sizes of the
  // parameter blocks it expects. The function checks that these match
  // the sizes of the parameter blocks listed in parameter_blocks. The
  // program aborts if a mismatch is detected. loss_function can be
  // NULL, in which case the cost of the term is just the squared norm
  // of the residuals.
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   const std::vector<double*>& parameter_blocks);

  // Convenience methods for adding residuals with a small number of
  // parameters. This is the common case. Instead of specifying the
  // parameter block arguments as a vector, list them as pointers.
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3, double* x4, double* x5,
                                   double* x6, double* x7, double* x8,
                                   double* x9);

Problem.AddParameterBlock
- 参数即为要求值的变量
- 也就是残差函数的「bool operator()」中除residual之外的参数

  // Add a parameter block with appropriate size to the problem.
  // Repeated calls with the same arguments are ignored. Repeated
  // calls with the same double pointer but a different size results
  // in undefined behaviour.
  void AddParameterBlock(double* values, int size);

  // Add a parameter block with appropriate size and parameterization
  // to the problem. Repeated calls with the same arguments are
  // ignored. Repeated calls with the same double pointer but a
  // different size results in undefined behaviour.
  void AddParameterBlock(double* values,
                         int size,
                         LocalParameterization* local_parameterization);
                         
  // Hold the indicated parameter block constant during optimization.
  void SetParameterBlockConstant(double* values);

  // Allow the indicated parameter block to vary during optimization.
  void SetParameterBlockVariable(double* values);

实例

  double m = 0.0;
  double c = 0.0;
  Problem problem;
  for (int i = 0; i < kNumObservations; ++i) {
    CostFunction* cost_function =
        new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
            new ExponentialResidual(data[2 * i], data[2 * i + 1]));
    problem.AddResidualBlock(cost_function,
                             new CauchyLoss(0.5),
                             &m, &c);
  }

2.3 问题求解

  Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  Solver::Summary summary;
 
  Solve(options, &problem, &summary);
 
  std::cout << summary.BriefReport() << "\n";
  std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n";
  std::cout << "Final   m: " << m << " c: " << c << "\n";

3. 完整描述

// For example, consider a scalar error e = k - x'y, where both x and y are
// two-dimensional column vector parameters, the prime sign indicates
// transposition, and k is a constant. The form of this error, which is the
// difference between a constant and an expression, is a common pattern in least
// squares problems. For example, the value x'y might be the model expectation
// for a series of measurements, where there is an instance of the cost function
// for each measurement k.
//
// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
// the squaring is implicitly done by the optimization framework.
//
// To write an auto-differentiable cost function for the above model, first
// define the object

   class MyScalarCostFunctor {
     MyScalarCostFunctor(double k): k_(k) {}

     template <typename T>
     bool operator()(const T* const x , const T* const y, T* e) const {
       e[0] = T(k_) - x[0] * y[0] + x[1] * y[1];
       return true;
     }

    private:
     double k_;
   };

// Note that in the declaration of operator() the input parameters x and y come
// first, and are passed as const pointers to arrays of T. If there were three
// input parameters, then the third input parameter would come after y. The
// output is always the last parameter, and is also a pointer to an array. In
// the example above, e is a scalar, so only e[0] is set.
//
// Then given this class definition, the auto differentiated cost function for
// it can be constructed as follows.

     CostFunction* cost_function
         = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
              new MyScalarCostFunctor(1.0));             
//                                                       |  |  |
//                            Dimension of residual -----+  |  |
//                            Dimension of x ---------------+  |
//                            Dimension of y ------------------+
//
// In this example, there is usually an instance for each measumerent of k.
//
// In the instantiation above, the template parameters following
// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a
// 1-dimensional output from two arguments, both 2-dimensional.
//
// AutoDiffCostFunction also supports cost functions with a
// runtime-determined number of residuals. For example:

     CostFunction* cost_function
         = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
             new CostFunctorWithDynamicNumResiduals(1.0),   ^     ^  ^
                        runtime_number_of_residuals);  
                                                ^           |     |  |                   
//                                              |           |     |  |
//                                              |           |     |  |
//             Actual number of residuals ------+           |     |  |
//             Indicate dynamic number of residuals --------+     |  |
//             Dimension of x ------------------------------------+  |
//             Dimension of y ---------------------------------------+
//
// The framework can currently accommodate cost functions of up to 10
// independent variables, and there is no limit on the dimensionality
// of each of them.