优化库——ceres（二）深入探索ceres::Problem

1-1 🔖 Problem类函数总览

整个Problem函数内部的核心操作实际上是由类对象内部的internal::scoped_ptr<internal::ProblemImpl> problem_impl_;操作的，这层关系就好比STL 提供的 queue 和stack和deque的关系，queue 和stac的内部实际上都是deque实现的专业术语叫做配接器 (adapters)

秀一把C++概念（可不看）

• 容器 (containers) : 各种数据结构，如 vector, list, deque, set, map, 用来存放数据

• 算法 (algorithms) : 各种常用算法如 sort, search, copy, erase···

• 迭代器 (iterators) : 扮演容器与算法之间的胶合剂，是所谓的＂泛型指针" 章。共有五种类型，以及其它衍生变化。从实现的角度来看，迭代器是一种将 operator*, operator->, operator++, operator-- 等指针相关操作予以重载的 class template 。所有 STL 容器都附带有自己专属的迭代器—一是的，只有容器设计者才知道如何遍历自己的元素。

• 仿函数 (functors) : 行为类似函数，可作为算法的某种策略 (policy)

• 配接器 (adapters) : 一种用来修饰容器 (containers) 或仿函数 (functors)或迭代器 (iterators) 接口的东西。

  例如， STL 提供的 queue 和stack, 虽然看似容器，其实只能算是一种容器配接器，因为它们的底部完全借助 deque, 所有操作都由底层的 deque 供应。改变 functor 接口者，称为function adapter; 改变 container 接口者，称为 container adapter; 改变 iterator接口者，称为 iterator adapter 。

• 配置器 (allocators): 负责空间配置与管理。从实现的角度来看，配置器是一个实现了动态空间配置、空间管理、空间释放的 class template 。

---

STL 六大组件的交互关系： Container 通过 Allocator 取得数据储存空间， Algorithm 通过 Iterator 存取 Container 内容， Functor 可以协助 Algorithm完成不同的策略变化， Adapter 可以修饰或套接 Functor 。

ceres::Problem主要函数如下

重点看一下函数AddResidualBlock

1-2 🔖 ceres::LocalParameterization

参考链接：

Ceres详解（一） Problem类

Ceres LocalParameterization 理解

ceres-solver： From QuaternionParameterization to LocalParameterization 【推荐】

LocalParameterization类的作用是解决非线性优化中的过参数化问题。所谓过参数化，即待优化参数的实际自由度小于参数本身的自由度。例如在SLAM中，当采用四元数表示位姿时，由于四元数本身的约束（模长为1），实际的自由度为3而非4。此时，若直接传递四元数进行优化，冗余的维数会带来计算资源的浪费，需要使用Ceres预先定义的QuaternionParameterization对优化参数进行重构：

problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数
// QuaternionParameterization 继承于 LocalParameterization
ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量

关于函数QuaternionParameterization，源码细节后面会有专门小节解析。

四元数的使用问题：

​ 四元数表示的是一个SO3，四元数表示的这个东西是一个有三个自由度的东西，然而四元数却有四维也就是四个自由度，这显然是不合理的，所以也就产生了一个单位四元数这么一个东西，单位四元数顾名思义 就是说四元数的四个量的二范数是1.这个其实是一个约束，这个约束就约束了四元数的一个自由度，这样其实四元数就只剩下三个自由度了正好符合一个SO3的维数。

​ 然后在ceres里面，如果使用的是自动求导，然后再结合爬山法，那么每步迭代中都会产生一个四维的delta(迭代的增量，参考LM等算法)，那么根据常规的爬山法，这样就仅仅需要将 原四元数“加上”这个迭代产生的delta就能够得到新的四元数了，这里问题就来了，直接加上以后这个四元数就不再是一个单位四元数了，就没有意义了，如果非得这么用的话就得每次迭代过后都将这个四元数进行一个归一化处理。

解决方案：

​ 对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是不支持广义的加法（因为使用普通的加法就会打破其 constraint，比如旋转矩阵加旋转矩阵得到的就不再是旋转矩阵），所以我们在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个参数本地化的子类，需要继承于LocalParameterization，LocalParameterization是纯虚类，所以我们继承的时候要把所有的纯虚函数都实现一遍才能使用该类生成对象.

​ 除了不支持广义加法要自定义参数本地化的子类外，如果你要对优化变量做一些限制也可以如法炮制，比如ceres中slam2d example中对角度范围进行了限制.

后面小节将以LocalParameterization的一个派生类QuaternionParameterization为例进行解析LocalParameterization的使用：

1-2-1自定义LocalParameterization

参考链接：

[1] ceres-solver
[2]《A Tutorial on Graph-Based SLAM》
[3]《流形与几何初步》
[4]《Quaternion kinematics for the error-state Kalman filter》

[5] Ceres LocalParameterization 理解

LocalParameterization本身是一个虚基类，详细定义如下。用户可以自行定义自己需要使用的子类，或使用Ceres预先定义好的子类。

class LocalParameterization {
 public:
  virtual ~LocalParameterization() {}
  //
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;//参数正切空间上的更新函数
  virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; //雅克比矩阵
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;//一般不用
  virtual int GlobalSize() const = 0; // 参数的实际维数
  virtual int LocalSize() const = 0; // 正切空间上的参数维数
};

上述成员函数中，需要我们改写的主要为

• Plus(const double* x,const double* delta,double* x_plus_delta)：定义变量的加法（具体参考下面例子）
• ComputeJacobian()：x对delta的雅克比矩阵
• GlobalSize()：传入的实际参数的维度
• LocalSize()：参数实际的维度（自由度）

具体方式可参考下一小节的案例

1-2-2 案例：QuaternionParameterization解析

这里我们以ceres预先定义好的QuaternionParameterization为例具体说明LocalParameterization用法，类声明如下：

注意。

• 在 ceres 源码中没有明确说明之处都认为矩阵 raw memory 存储方式是 Row Major 的，这与 Eigen 默认的 Col Major 是相反的。

• ceres 默认的 Quaternion raw memory 存储方式是 w, x, y, z，而 Eigen Quaternion 的存储方式是 x, y, z, w，这就导致在 ceres 代码中除ceres::QuaternionParameterization 之外还有ceres::EigenQuaternionParameterization。

  Eigen Quaternion指的是eigen库中的函数Eigen::Quaternion(w,x,y,z)函数中，实数w在首；但是实际上它的内部存储顺序是[x y z w]，对其访问的时候最后一个元素才是w

  对三个函数内部存储顺序总结

  ceres::QuaternionParameterization：内部存储顺序为(w,x,y,z)

  ceres::EigenQuaternionParameterization：内部存储顺序为(x,y,z,w)

  Eigen::Quaternion(w,x,y,z)：内部存储顺序为(x,y,z,w)（与构造函数没有保持一致）

• ceres 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法则。

class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
 public:
  virtual ~QuaternionParameterization() {}
  //重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数【x】，用旋转矢量表示的增量【delta】，以及更新后的四元数【x_plus_delta】。
  //函数首先将增量【delta】由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;
  //GlobalSize 返回值为4，即四元数本身的实际维数。由于在内部优化时，ceres采用的是旋转矢量，维数为3，因此LocalSize()的返回值为3。
  //GlobalSize 就是表示他真正的维数是一个4维的
  virtual int GlobalSize() const { return 4; }
  //LocalSize是告诉Ceres他表示的东西是一个三维的
  virtual int LocalSize() const { return 3; }
};
//=============================================================================
//重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数【x】，用旋转矢量表示的增量【delta】，以及更新后的四元数【x_plus_delta】。
//函数首先将增量【delta】由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。
bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const {
  // 将旋转矢量转换为四元数形式
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
    double q_delta[4];
    q_delta[0] = cos(norm_delta);
    q_delta[1] = sin_delta_by_delta * delta[0];
    q_delta[2] = sin_delta_by_delta * delta[1];
    q_delta[3] = sin_delta_by_delta * delta[2];
    // 采用四元数乘法更新
    QuaternionProduct(q_delta, x, x_plus_delta);
  } else {
    for (int i = 0; i < 4; ++i) {
      x_plus_delta[i] = x[i];
    }
  }
  return true;
}
//=============================================================================

Plus()

实现了优化变量的更新，即使用GN法得到的$\Delta {\tilde{\mathbf{x}}}^{\ast }$更新原来优化变量$\check{\mathbf{x}}$，使用的是 ⊞ 广义加法运算符.
$${\mathbf{x}}^{\ast }=\breve{\mathbf{x}}\oplus \Delta {\tilde{\mathbf{x}}}^{\ast }$$
⊞运算符，首先将四元数的向量部分（与旋转向量相差一个系数）变成一个完整的四元数（纯虚四元数的指数），即得到过参数化的增量，然后将该增量应用到待估计变量上.

ComputeJacobian 函数给出了四元数相对于旋转矢量的雅克比矩阵计算方法, 即：
$${\mathbf{J}}_{4\times 3}=d\mathbf{q}/d\mathbf{v}=d{\left[q_w,q_x,q_y,q_z\right]}^T/d[[x,y,z]$$
对应Jacobian维数为4行3列，存储方式为行主序。

推导过程如下：

ComputeJacobian()

参考《A Tutorial on Graph-Based SLAM》中的公式24，使用链式法则我们知道${\tilde{\mathbf{J}}}_{ij}$由两部分构成，第一部分$\frac{\partial {\mathbf{e}}_{ij}(\breve{\mathbf{x}})}{\partial {\breve{\mathbf{x}}}_i}$是对原始过参数化的优化变量（比如，四元数）的导数，这个很容易求得，直接借助ceres的AutoDiffCostFunction() 计算即可，或者自己计算雅可比矩阵，实现一个costfunction，关键是第二部分是如何求得的呢？
$${\tilde{\mathbf{J}}}_{ij}={\frac{\partial {\mathbf{e}}_{ij}(\mathbf{x}\oplus \Delta \tilde{\mathbf{x}})}{\partial \Delta \tilde{\mathbf{x}}}|}_{\Delta \tilde{\mathbf{x}}=0}={\frac{\partial {\mathbf{e}}_{ij}(\breve{\mathbf{x}})}{\partial {\mathbf{x}}_i}\cdot \frac{{\breve{\mathbf{x}}}_i\oplus \mathbf{\Delta }{\tilde{\mathbf{x}}}_i}{\partial \mathbf{\Delta }{\tilde{\mathbf{x}}}_i}|}_{\Delta \tilde{\mathbf{x}}=0}$$
现在求解 ${\frac{\partial {\mathbf{x}}_i\oplus \Delta {\tilde{\mathbf{x}}}_i}{\partial \Delta {\hat{\mathbf{x}}}_i}|}_{\Delta \tilde{\mathbf{x}}=0}$，以四元数为例：
$$\frac{\partial {\mathfrak{q}}_i\oplus \mathbf{\Delta }{\tilde{\mathbf{q}}}_i}{\partial \mathbf{\Delta }{\tilde{\mathbf{q}}}_i}=\frac{\partial {\mathbf{q}}_i\otimes e^{\Delta {\tilde{\mathbf{q}}}_i}}{\partial \mathbf{\Delta }{\tilde{\mathbf{q}}}_i}\approx \frac{\partial {\mathbf{q}}_i\otimes \left[\begin{array}{c}1\\ \mathbf{\Delta }{\tilde{\mathbf{q}}}_i\end{array}\right]}{\partial \mathbf{\Delta }{\tilde{\mathbf{q}}}_i}=\frac{\partial {\left[{\mathbf{q}}_i\right]}_L\left[\begin{array}{c}1\\ \mathbf{\Delta }{\tilde{\mathbf{q}}}_i\end{array}\right]}{\partial \mathbf{\Delta }{\tilde{\mathbf{q}}}_i}$$

注意上面符号的变化，先从 ⊞ 变成四元数乘法 ⊗ ,最后变成普通的乘法，进一步得到，（wxyz）
$$\left[\begin{array}{llll}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right]\frac{\partial \left[\begin{array}{c}1\\ \Delta {\tilde{\mathbf{q}}}_i\end{array}\right]}{\partial \mathbf{\Delta }{\tilde{\mathbf{q}}}_i}=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right]\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_w& -q_z& q_y\\ q_z& q_w& -q_x\\ -q_y& q_x& q_w\end{array}\right]$$
最后得到雅可比矩阵是4*3维的，代码里使用一个size为12的数组存储.

1-2-3 ceres::LocalParameterization的其他用法

ceres::QuaternionParameterization 继承于 ceres::LocalParameterization，在了解 ceres::QuaternionParameterization 运行原理的情况下，可以根据自身需求继承实现自己的 ceres::LocalParameterization。

一个非常好用的简单例子是“固定模长优化”。“固定模长优化”是对待优化变量进行了模长限制，最常见的“固定模长优化”是重力加速度方向优化，参考 VINS [2] 的 Algorithm 1，初始化过程中的重力加速度方向优化。

1-3 🔖 关于添加残差块，参数块，设置参数化的区别和调用关系

• AddResidualBlock

• AddParameterBlock

  函数内部，只调用了一行代码。只有两个重载函数，都copy过来了

  void ProblemImpl::AddParameterBlock(double* values, int size)
  {
      InternalAddParameterBlock(values, size);
  }
  
  void ProblemImpl::AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization)
  {
      ParameterBlock* parameter_block = InternalAddParameterBlock(values, size);
      if (local_parameterization != NULL)
      {
          parameter_block->SetParameterization(local_parameterization);
      }
  }
  

• SetParameterization

再来看看优化步骤：

• 第一步就是构建对应的CostFunction。
• 完事后，调用AddResidualBlock函数添加残差块，这个函数里面直接调用函数InternalAddParameterBlock，对比上面内容，事实上会调用AddParameterBlock(double* values, int size)函数，而AddParameterBlock函数里面实际上会调用SetParameterization函数。

也就是说如果我们的参数属于正常的plus更新的话，也就是没有过参数（LocalParameterization），没有manifold space，那么就完全不需要调用AddParameterBlock或者SetParameterization函数

如果我们的参数需要自定义更新方式，我们可以调用AddParameterBlock或者SetParameterization函数任何一个都可以，调用方式如下

// 方法1
void AddParameterBlock(double* values,
                     int size,
                     LocalParameterization* local_parameterization);
// 方法2
void SetParameterization(double* values,
                       LocalParameterization* local_parameterization);

这里提一下既然程序中给了默认的参数化方法，我们自己添加的话，程序就会调用我们的自定义方法。
还有一个比较有意思的地方是程序虽然反复调用了AddParameterBlock，但是参数并不会添加重复，因为内部使用map管理，每次添加的时候，都会保证地址不重复。

总结一下

• 参数正常更新，只需要调用AddResidualBlock
• 参数自定义更新，需要调用AddParameterBlock或者SetParameterization，要注意，数量一定要添加对，因为比如vins-mono里面参数非常多，搞不清楚参数维度就会很容易出错。

参考链接

[1] ceres-solver
[2]《A Tutorial on Graph-Based SLAM》
[3]《流形与几何初步》
[4]《Quaternion kinematics for the error-state Kalman filter》

[5] Ceres LocalParameterization 理解