Ceres Solver Document学习笔记

Ceres Solver Document学习笔记

之前在学习Vins-Mono时，在Vins-Mono后端就是使用Ceres实现的滑窗优化，当时对Ceres的了解也仅仅是在简单使用的层面上，最近项目中有再次用到Ceres相关的内容，因此我把Ceres Solver的Document扫了一遍，这篇博客是相关的笔记。我个人觉得Ceres比较重要的优势主要有如下几点：

1. Ceres中提供了自动求导的功能，在优化算法推到的过程中，最麻烦的也就是求雅克比的过程，而Ceres Solver提供的自动求导功能直接为用户避免了这个麻烦，因此在工程中使用Ceres后除非需要优化系统性能才会采用手动求导雅克比；
2. Ceres中提供了局部参数化的功能接口，也就是LocalParameteriztion对象，这对于求解旋转变换相关问题有极大的帮助；

其他的诸如计算速度，求解器丰富等就不多提及了，下面开始展开细节

1. 基本概念

我看大多数博客都会用如下例子：
$$\underset{\mathbf{x}}{\min }\frac{1}{2}\sum _i{\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)$$$$\text{ s.t. }{l}_j\le x_j\le u_j$$其中，公式中的各个部分对应的概念是：
${\rho }_i\left({\Vert f_i\left(x_{i_1},\dots ,x_{i_k}\right)\Vert }^2\right)$被称为ResidualBlock，也就是残差;
$\left\{x_{i_1},\dots ,x_{i_k}\right\}$被成为ParameterBlock，也就是输入参数;
$f_i(\cdot )$被称为CostFunction，也就是代价函数，是根据输入参数直接计算残差的模块；
${\rho }_i(\cdot )$被称为LossFunction，也就是损失函数或者鲁棒核函数，通常用来减少输入参数中外点影响的模块；
以上都是Ceres中的定义，在其他不同的工具或者任务中定义可能不相同，Ceres所做的也就是根据输入参数计算损失，并通过凸优化的方法将损失降到局部最小，关于凸优化中的基本原理就不在此补充，下面主要就Ceres各个模块的用法进行介绍。

2. 基本方法

2.1 CostFunction

CostFunction负责计算残差和雅克比矩阵，CostFuntion类代码如下：

class CostFunction {
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) = 0;
  const vector<int32>& parameter_block_sizes();
  int num_residuals() const;

 protected:
  vector<int32>* mutable_parameter_block_sizes();
  void set_num_residuals(int num_residuals);
};

Evaluate成员函数负责根据输入的paramters计算residuals和jacobians，如果jacobians为nullptr，通常我们只需要在Evaluate函数中实现residuals的计算，jacobians后面可以通过Ceres提供的自动求导（数值求导）替代，否则，我们还需要在Evaluate函数中实现jacobians的计算，jacobians是一个大小为num_residuals * parameter_block_sizes_的行主数组，具体计算公式为：$$\text{jacobians}[i][r\ast \text{ parameter block sizes}[i]+c]=\frac{\partial \text{residual}[r]}{\partial \text{parameters}[i][c]}$$在CostFunction中用成员变量CostFunction::parameter_block_sizes_和CostFunction::num_residuals_分别用于记录输入parameters和residuals的尺寸，这两个信息可以通过继承该类后使用访问其设置，或者通过Problem::AddResidualBlock()函数添加。

此外，如果paramters大小和residuals大小在编译时是已知的，我们就可以使用SizeCostFunction，该函数可以将paramters大小和residuals大小设置为模板参数，用户只需要在使用的时候给模板参数赋值就可以，如下：

template<int kNumResiduals, int... Ns>
class SizedCostFunction : public CostFunction {
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const = 0;
};

2.2 AutoDiffCostFunction

该方法就是Ceres中提供的自动求导的方法，如果需要使用自动微分函数，必须在CostFunction中定义模板符号函数operator()，该函数根据paramters计算residuals，如下所示：

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] = k_ - x[0] * y[0] - x[1] * y[1];
    return true;
  }

 private:
  double k_;
};

然后带自动微分功能的CostFunction构造函数如下：

CostFunction* cost_function
    = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
        new MyScalarCostFunctor(1.0));              ^  ^  ^
                                                    |  |  |
                        Dimension of residual ------+  |  |
                        Dimension of x ----------------+  |
                        Dimension of y -------------------+

默认情况下，当AutoDiffCostFunction销毁时，其对应的CostFunction也会销毁，如果不希望存在这种从属关系，可以在实例化AutoDiffCostFunction时，传入参数DO_NOT_TAKE_OWNERSHIP，如下代码所示：

MyScalarCostFunctor functor(1.0)
CostFunction* cost_function
    = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
        &functor, DO_NOT_TAKE_OWNERSHIP);

AutoDiffCostFunction同时支持在运行时确定残差维度，此时需要在实例化时传入参数DYNAMIC，代码如下所示：

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 ---------------------------------------+

AutoDIffCostFunction在编译时就需要确定参数维度，但是在一些使用场景中需要在运行时才确定，那么这时就需要使用DynamicAutoDiffCostFunction，具体使用方法如下：

template <typename CostFunctor, int Stride = 4>
class DynamicAutoDiffCostFunction : public CostFunction {
};

struct MyCostFunctor {
  template<typename T>
  bool operator()(T const* const* parameters, T* residuals) const {
  }
}

DynamicAutoDiffCostFunction<MyCostFunctor, 4>* cost_function =
  new DynamicAutoDiffCostFunction<MyCostFunctor, 4>(
    new MyCostFunctor());
cost_function->AddParameterBlock(5);
cost_function->AddParameterBlock(10);
cost_function->SetNumResiduals(21);

2.3 NumericDiffCostFuntion

NumericDiffCostFunction作用和AutoDiffCostFunction作用是相同的，二者唯一的区别是NumericDiffCostFunction中不再使用模板类型，而是使用double类型代替模板类型。谷歌推荐类型为AutoDiffCostFunction，C++模板的使用使得自动求导效率更高，而数值的差花费更多，容易出现数字错误，导致收敛速度变慢。

2.4 LossFunction

LossFunction就是上文提到的代价计算公式中的鲁棒核函数${\rho }_i(\cdot )$部分，用于减少输入异常值对结果的影响，通常鲁棒核函数${\rho }_i(\cdot )$要求满足$$\rho (0)=0$$$${\rho }^{\prime }(0)=1$$$${\rho }^{\prime }(s)<1$$$${\rho }^{\prime \prime }(s)<0$$具体地，在Ceres中提供了如下核函数：
TrivialLoss$$\rho (s)=s$$HuberLoss$$\rho (s)=\{\begin{array}{ll}s& s\le 1\\ 2\sqrt{s}-1& s>1\end{array}$$SoftLOneLoss$$\rho (s)=2(\sqrt{1+s}-1)$$CauchyLoss$$\rho (s)=\log (1+s)$$ArctanLoss$$\rho (s)=\arctan (s)$$TolerantLoss$$\rho (s,a,b)=b\log \left(1+e^{(s-a)/b}\right)-b\log \left(1+e^{-a/b}\right)$$这里我们来补充推导下鲁棒核函数的作用原理，鲁棒核函数是直接作用于残差$f_i(\mathbf{x})$上，最小二乘的代价函数就变成如下形式：$$\underset{\mathbf{x}}{\min }\frac{1}{2}\sum _k\rho \left({\Vert f_k(\mathbf{x})\Vert }^2\right)$$将误差的平方项记作$s_k={\Vert f_k(\mathbf{x})\Vert }^2$，则鲁棒核函数进行二阶泰勒展开有：$$\frac{1}{2}\rho (s)=\frac{1}{2}\left(\text{ const }+{\rho }^{\prime }\Delta s+\frac{1}{2}{\rho }^{\prime \prime }{\Delta }^{2}s\right)$$其中$\Delta s_k$的计算如下：$$\begin{array}{rl}\Delta s_k& ={\Vert f_k(\mathbf{x}+\Delta \mathbf{x})\Vert }^2-{\Vert f_k(\mathbf{x})\Vert }^2\\ & \approx {\Vert f_k+{\mathbf{J}}_k\Delta \mathbf{x}\Vert }^2-{\Vert f_k(\mathbf{x})\Vert }^2\\ & =2f_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}+(\Delta \mathbf{x}{)}^{\top }{\mathbf{J}}_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}\end{array}$$结合以上两公式有$$\begin{array}{rl}\frac{1}{2}\rho (s)\approx & \frac{1}{2}({\rho }^{\prime }\left[2f_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}+(\Delta \mathbf{x}{)}^{\top }{\mathbf{J}}_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}\right]\\ & +\frac{1}{2}{\rho }^{\prime \prime }{\left[2f_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}+(\Delta \mathbf{x}{)}^{\top }{\mathbf{J}}_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}\right]}^2+\text{ const })\\ \approx & {\rho }^{\prime }{f}_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}+\frac{1}{2}{\rho }^{\prime }(\Delta \mathbf{x}{)}^{\top }{\mathbf{J}}_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}+{\rho }^{\prime \prime }(\Delta \mathbf{x}{)}^{\top }{\mathbf{J}}_k^{\top }{f}_k{f}_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}+\text{const}\\ =& {\rho }^{\prime }{f}_k^{\top }{\mathbf{J}}_k\Delta \mathbf{x}+\frac{1}{2}(\Delta \mathbf{x}{)}^{\top }{\mathbf{J}}_k^{\top }\left({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_k{f}_k^{\top }\right){\mathbf{J}}_k\Delta \mathbf{x}+\text{const}\end{array}$$对上式求和后再对$\Delta \mathrm{x}$进行求导，并令其等于零，有$$\sum _k{\mathbf{J}}_k^{\top }\left({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_k{f}_k^{\top }\right){\mathbf{J}}_k\Delta \mathbf{x}=-\sum _k{\rho }^{\prime }{\mathbf{J}}_k^{\top }{f}_k$$$$\sum _k{\mathbf{J}}_k^{\top }W{\mathbf{J}}_k\Delta \mathbf{x}=-\sum _k{\rho }^{\prime }{\mathbf{J}}_k^{\top }{f}_k$$熟悉优化算法的同学肯定注意到${\sum }_k{\mathbf{J}}_k^{\top }W{\mathbf{J}}_k$部分就是$\mathbf{H}$矩阵，而${\sum }_k{\rho }^{\prime }{\mathbf{J}}_k^{\top }{f}_k$部分就是$\mathbf{b}$向量，相对于没有核函数的优化流程中，加入核函数只是在求解$\Delta \mathbf{x}$过程中在$\mathbf{b}$向量中加入了${\rho }^{\prime }$这一项，以上就是鲁棒核函数作用与优化过程的原理推导。

2.5 LocalParameterization

LocalParameterization主要是用于优化过程中的表达形式的转化，最典型的例子就是就是在Bundle Adjustment或者Pose Graph中对四元数的处理，我们都知道，四元数是对SO3的一种表达，用四元数进行旋转变换的计算是方便的，但是用四个变量表达三个自由度时就必定存在另一个约束，也就是四元数的模长必须为1，因此如果我们使用四元数进行迭代优化的话，每进行一次迭代就需要对四元数进行一次归一化操作，这样显然很麻烦，更加简便的方法就是使用se3的表达方式李代数进行迭代优化，但是李代数对于计算旋转变化的操作又不友好，综上所述，我们最好的办法就是在迭代优化和计算旋转变换的过程中存在这么一个从四元数到李代数的转换，这也就是Ceres为我们提供的LocalParameterization类，LocalParameterization类其实是一个接口类，像四元数到李代数这样常见的变换，Ceres已经集成好了具体的实现，如EigenQuaternionParameterization类，该类是实现如下：

class CERES_EXPORT EigenQuaternionParameterization
    : public ceres::LocalParameterization {
 public:
  virtual ~EigenQuaternionParameterization() {}
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x, double* jacobian) const;
  virtual int GlobalSize() const { return 4; }
  virtual int LocalSize() const { return 3; }
};

除此之外，Ceres还提供了例如HomogeneousVectorParameterization、LineParameterization、ProductParameterization等一系列实现，当然也可以自己继承LocalParameterization类来实现，我之前在VINS-Mono加入线特征的实验中就有实现一个直线普吕克坐标的转换，还是非常方便的。

2.6 Problem

Problem类主要用于构建优化问题，其中Problem::AddResidualBlock()用于向Problem中添加CostFunction和LossFunction，以及一系列Parameter Block，具体用法如下：

ResidualBlockId Problem::AddResidualBlock(CostFunction*, LossFunction*, const vector<double*> parameter_blocks);
ResidualBlockId Problem::AddResidualBlock(CostFunction*, LossFunction*, double* x0, double* x1...);

此外Problem::AddParameterBlock()方法用于显示地向Problem对象中添加Parameter Block，这里要注意的是Problem::AddResidualBlock()是不可以重复添加Parameter Block的，而Problem::AddParameterBlock()是可以的，并且当用户想设置Parameter Block的LocalParameterization属性时也是使用该方法添加的，具体如下：

void AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization);

除了这两个方法之外，Problem::SetParameterBlockConstant()用于设置Parameter Block为常量，也就是在迭代优化时不优化这一块，这在Bundle Adjustment和Pose Graph中经常使用的一个属性，最后Problem::SetParameterBlockVariable()用于删除Parameter Block的常量属性。除了以上介绍的四个方法外，Problem可以通过Options方法对进行进一步配置，具体的方法介绍就不在此介绍了，需要的同学可以去仔细阅读下Ceres的官方文档，在正常使用过程中有以上四个方法就基本足够了。

2.7 Solver

Solver类主要通过Option方法对求解器进行配置，配置的内容主要包括求解器使用的优化方法和参数、线性求解器、预处理器等，这里不对配置的API进行过多的介绍了，我觉得需要了解的直接查文档就好，这里主要对各个具体的方法进行简单回顾

首先是求解器使用的优化方法，优化方法分为Linear Search Method和Trust Region Method，我们要解决的问题是：$$\arg \underset{x}{\min }\frac{1}{2}\Vert F(x){\Vert }^2$$我们对非线性代价函数进行线性化$$F(x+\Delta x)\approx F(x)+J(x)\Delta x$$我们要解决的问题便转化为$$\underset{\Delta x}{\min }\frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2$$我们通过迭代$x\leftarrow x+\Delta x$来解决该问题，那么解决方法就分为：
Linear Search Method：
该方法的步骤主要如下：

1. 给定一个初始化点$x$；
2. $\Delta x=-H^{-1}(x)g(x)$；
3. $\arg {\min }_{\mu }\frac{1}{2}\Vert F(x+\mu \Delta x){\Vert }^2$；
4. $x=x+\mu \Delta x$；
5. 重复第二步；

其中，$\Delta H(x)=J(x{)}^{T}J(x)$， $g=-J^T(x)F(x)$，$\mu$为迭代步长，这其实就是最经典的Gauss-Newton法。

Trust Region Method：

1. 给定一个初始点$x$和一个可信任区域半径$\mu$
2. $\arg {\min }_{\mu }\frac{1}{2}\Vert F(x+\mu \Delta x){\Vert }^2$；
3. $x=x+\mu \Delta x$；
4. 重复第二步；

其中，$\Delta H(x)=J(x{)}^{T}J(x)$， $g=-J^T(x)F(x)$，$\mu$为迭代步长，这其实就是最经典的
2. 求解带约束问题
$\arg {\min }_{\Delta x}\frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2$
such that $\Vert D(x)\Delta x{\Vert }^2\le \mu$
$L\le x+\Delta x\le U$
3. 计算增益比$\rho =\frac{\Vert F(x+\Delta x){\Vert }^2-\Vert F(x){\Vert }^2}{\Vert J(x)\Delta x+F(x){\Vert }^2-\Vert F(x){\Vert }^2}$
4. 如果$\rho >ϵ$则$x=x+\Delta x.$
5. 如果$\rho >{\eta }_1$则$\mu =2\mu$
6. 如果$\rho <{\eta }_2$则$\mu =0.5\ast \mu$
7. 重复第二步

其中，$\mu$为信任区域半径，在Trust Region Method中，最关键的步骤就是解决带约束的优化问题，也就是$$\begin{array}{rl}\arg \underset{\Delta x}{\min }& \frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2\\ \text{ such that }& \Vert D(x)\Delta x{\Vert }^2\le \mu \\ & L\le x+\Delta x\le U\end{array}$$解决该问题，Ceres又提供了两种方法，即著名的Levenberg-Marquardt法和Dogleg法:
在Levenberg-Marquardt法中，将该带约束的优化问题转化为无约束形式：$$\arg \underset{\Delta x}{\min }\frac{1}{2}\Vert J(x)\Delta x+F(x){\Vert }^2+\frac{1}{\mu }\Vert D(x)\Delta x{\Vert }^2$$其中$D(x)$是一个非负对角矩阵，这里我们可以从另外一个角度来理解$D(x)$的作用，在Gauss-Newton法中，当$\Delta H$为不可逆矩阵时，关键步骤$\Delta x=-H^{-1}(x)g(x)$就将求解失败，而如果我们将该式改写为$$\left(J^T(x)J(x)+uI\right)h=-J^T(x)F(x)$$后，其中$I$为单位对角阵，那么矩阵$\left(J^T(x)J(x)+uI\right)$ 一定可逆，而这里的$I$就等价于公式中的$D(x)$。除此之外，我们再来分析参数$u=1/\mu$，当$u$足够小时，Levenberg-Marquardt法即退化为Gauss-Newton法，当$u$足够大时，Levenberg-Marquardt法即退化为更简单的最速下降法，而在迭代过程中具体更加倾向于那种方法则有增益比$\rho$决定的。
在Dogleg法中，其实也是控制算法在Gauss-Newton法和最速下降法中切换，但是思路更加直接，是直接计算出Gauss-Newton法和最速下降法的结果，然后进行比较，具体流程可以参考我之前总结的一篇博客视觉SLAM总结——视觉SLAM面试题汇总中的第37题。

其次是线性求解器，线性求解器指的是求解求解问题$$H\Delta x=g$$Ceres Solver中主要提供了Dense OR、Dense Normal Cholesky、Sparse Normal Cholesky、Dense Schur、Sparse Schur等方法，其中QR分解就是将矩阵分解为一个三角矩阵和一个正交阵，如下$$\Delta x^{\ast }=-R^{-1}{Q}^{\top }g$$其中$Q$为正交阵，$R$为上三角矩阵。Cholesky分解是将矩阵分解为一个上三角矩阵和一个下三角矩阵，如下：$$\Delta x^{\ast }=R^{-1}{R}^{-\top }g$$其中，求解三角阵可以采用对矩阵$J$进行OR分解，然后$J^{\top }J=R^{\top }{Q}^{\top }QR=R^{\top }R$即可获得Cholescky分解形式。Schur求解通常是特指和Bundle Adjustment有关的求解方式，在Bundle Adjustment问题中，$H$矩阵通常具有如下形式：$$H=\left[\begin{array}{cc}B& E\\ E^{\top }& C\end{array}\right]$$求解方程变为：$$\left[\begin{array}{cc}B& E\\ E^{\top }& C\end{array}\right]\left[\begin{array}{c}\Delta y\\ \Delta z\end{array}\right]=\left[\begin{array}{c}v\\ w\end{array}\right]$$此时求解结果变为$$\Delta z=C^{-1}\left(w-E^{\top }\Delta y\right)$$$$\left[B-E{C}^{-1}{E}^{\top }\right]\Delta y=v-E{C}^{-1}w$$因为$C$是对角线矩阵，因此求解过程中最复杂的求逆过程就被简化，是的计算过程复杂度大大减小。

2.8 Covariance

Convariance类主要是用于评估非线性最小二乘求解器返回的解的质量，也就是协方差，对于非线性回归问题$$y=f(x)+N(0,I)$$我们对其求解获得$$x^{\ast }=\arg \underset{x}{\min }\Vert f(x)-y{\Vert }^2$$此时$x^{\ast }$的协方差估计为$$C\left(x^{\ast }\right)={\left(J^{\prime }\left(x^{\ast }\right)J\left(x^{\ast }\right)\right)}^{-1}$$$J^{\prime }\left(x^{\ast }\right)$为函数$J$在$x^{\ast }$处的雅克比，当$J^{\prime }\left(x^{\ast }\right)$非满秩时，可以根据Moore-Penrose逆给出$$C\left(x^{\ast }\right)={\left(J^{\prime }\left(x^{\ast }\right)J\left(x^{\ast }\right)\right)}^{†}$$以上公式的推导是默认协方差为单位阵，如果不符合这个条件，非线性回归问题变为：$$y=f(x)+N(0,S)$$那么最大似然问题被求解为：$$x^{\ast }=\arg \underset{x}{\min }{f}^{\prime }(x)S^{-1}f(x)$$相应地$x^{\ast }$的协方差变为：$$C\left(x^{\ast }\right)={\left(J^{\prime }\left(x^{\ast }\right)S^{-1}J\left(x^{\ast }\right)\right)}^{-1}$$如下代码展示了Convariance类的大致使用方法

在这里插入代码片double x[3];
double y[2];

Problem problem;
problem.AddParameterBlock(x, 3);
problem.AddParameterBlock(y, 2);
<Build Problem>
<Solve Problem>

Covariance::Options options;
Covariance covariance(options);

vector<pair<const double*, const double*> > covariance_blocks;
covariance_blocks.push_back(make_pair(x, x));
covariance_blocks.push_back(make_pair(y, y));
covariance_blocks.push_back(make_pair(x, y));

CHECK(covariance.Compute(covariance_blocks, &problem));

double covariance_xx[3 * 3];
double covariance_yy[2 * 2];
double covariance_xy[3 * 2];
covariance.GetCovarianceBlock(x, x, covariance_xx)
covariance.GetCovarianceBlock(y, y, covariance_yy)
covariance.GetCovarianceBlock(x, y, covariance_xy)

可以看到Ceres提供的接口是可以从求解的过程中直接获得变量间的残差，Convariance在我之前接触的项目中还没有使用到过，之后有机会要尝试使用下。

以上就是我学习Ceres Document的一个笔记，因为时间原因写得比较简单，有问题欢迎随时交流～

此外，对其他SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结