[ceres-solver] From QuaternionParameterization to LocalParameterization

Posted 2021-03-25 jingetu

本文，从 ceres::QuaternionParameterization 入手，在理解 ceres::QuaternionParameterization 的基础上形成对 ceres::LocalParameterization 的认识。

ceres-solver 中对 LocalParameterization 的解释如下。

Sometimes the parameters (mathbf{x}) can overparameterize a problem. In that case it is desirable to choose a parameterization to remove the null directions of the cost. More generally, if (mathbf{x}) lies on a manifold of a smaller dimension than the ambient space that it is embedded in, then it is numerically and computationally more effective to optimize it using a parameterization that lives in the tangent space of that manifold at each point.

简单地说。LocalParameterization 是在优化 Manifold 上的变量时需要考虑的，Manifold 上变量是 over parameterized，即 Manifold 上变量的维度大于其自由度。这会导致 Manifold 上变量各个量之间存在约束，如果直接对这些量求导、优化，那么这就是一个有约束的优化，实现困难。为了解决这个问题，在数学上是对 Manifold 在当前变量值处形成的切空间（Tangent Space）求导，在切空间上优化，最后投影回 Manifold。

对于 SLAM 问题，广泛遇到的 Manifold 是旋转，旋转仅需使用 3 个量（so(3)）即可表达，但是实际应用中因为涉及到万向锁的问题，在更高维度表达旋转。四元数就是在维度 4 表达 3 个自由度的三维空间的旋转。

QuaternionParameterization 的方法 int GlobalSize() 返回 4，表示 Manifold 空间（四元数）是 4 维的，方法 int LocalSize() 返回 3，表示切空间是 3 维的。

QuaternionParameterization 的方法 bool ComputeJacobian(const double* x, double* jacobian) 计算得到一个 4x3 的矩阵。这些由 ComputeJacobian 计算得到的矩阵在 ceres 代码中被称作 "global_to_local"，含义是 Manifold 上变量对 Tangent Space 上变量的导数。在 ceres::CostFunction 处提供 residuals 对 Manifold 上变量的导数（({partial mathbf{e} over partial mathbf{X}^{(G)}})），乘以这个矩阵（({partial mathbf{X}^{(G)} over partial mathbf{X}^{(L)}})）之后变成对 Tangent Space 上变量的导数（({partial mathbf{e} over partial mathbf{X}^{(L)}})）。

QuaternionParameterization 的方法 bool Plus(const double* x, const double* delta, double* x_plus_delta) 将优化得到的增量（delta，在 Tangent Space）加到待优化变量（x，在 Manifold）中形成优化结果（x_plus_delta，在 Manifold），完成一次优化迭代。

注意。1. 在 ceres 源码中没有明确说明之处都认为矩阵 raw memory 存储方式是 Row Major 的，这与 Eigen 默认的 Col Major 是相反的。2. ceres 默认的 Quaternion raw memory 存储方式是 w, x, y, z，而 Eigen Quaternion 的存储方式是 x, y, z, w，这就导致在 ceres 代码中除 ceres::QuaternionParameterization 之外还有 ceres::EigenQuaternionParameterization 。3. ceres 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法则。

1. ceres::QuaternionParameterization 与 AutoDiff 的配合

首先，将方法 QuaternionParameterization::ComputeJacobian 的源码摘抄如下。

bool QuaternionParameterization::ComputeJacobian(const double* x,
                                                 double* jacobian) const {
  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
  return true;
}

按照 Row Major 的存储方式，按照 Manifold 上变量（x）的存储方式 w, x, y, z，可以将以上代码转换为数学公式。

[egin{align} {partial mathbf{X}^{(G)} over partial mathbf{X}^{(L)}} = egin{bmatrix} -x & -y & -z w & z & -y -z & w & x y & -x & w end{bmatrix} end{align}]

需要理解这个矩阵的由来。参考文献 [1] 的公式 (18)，以上矩阵对应 ([mathbf{q}]_R) 的右侧三列。如果将 Manifold 上变量 (mathbf{X}^{(G)}) 写作 (mathbf{q})，将 Tangent Space 上变量写作 (mathbf{ heta})，那么以上矩阵 ({partial mathbf{X}^{(G)} over partial mathbf{X}^{(L)}} = {partial mathbf{ heta} otimes mathbf{q} over partial mathbf{ heta}})。这是一个左扰动形式，检查方法 QuaternionParameterization::Plus，源码摘抄如下。

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;
}

可以看到，QuaternionProduct(q_delta, x, x_plus_delta);，优化得到的增量 q_delta 是左乘原变量 x，得到新变量 x_plus_delta，这与 QuaternionParameterization::ComputeJacobian 中的左扰动雅克比相对应。

实验，将 QuaternionParameterization::ComputeJacobian 计算的雅克比改为参考文献 [1] 的公式 (18) ([mathbf{q}]_L) 的右三列，将 QuaternionParameterization::Plus 中增量加到原变量过程改为右乘，即改变之后 QuaternionProduct(x, q_delta, x_plus_delta);。随后将这个右扰动形式的 QuaternionParameterization 与 AutoDiff 结合，解决实际问题，可以看到得到的结果在误差允许范围内与左扰动结果一致。

结论。QuaternionParameterization 的 Plus 与 ComputeJacobian 共同决定使用左扰动或使用右扰动形式。

得到以上关于 QuaternionParameterization 的好处是在不使用 AutoDiff 或 NumericDiff，用户代码提供雅克比时，能够提供指导。按照已有资料，一般都是对 so(3) 求导，很难找到对四元数求导。如果使用 ceres::QuaternionParameterization，ceres 期望用户代码提供对四元数的导数，用户提供这种导数比较麻烦。

如果用户自行提供一种 QuaternionParameterization，获得对 parameterization 的控制。用户就可以在 CostFunction 中提供对 so(3) 的求导，定义 QuaternionParameterization::ComputeJacobian 返回矩阵 (egin{bmatrix}mathbf{I}_{3 imes3} \ (mathbf{0}_{3 imes1})^Tend{bmatrix})。矩阵 (egin{bmatrix}mathbf{I}_{3 imes3} \ (mathbf{0}_{3 imes1})^Tend{bmatrix}) 将用户返回的 (N(e) imes4) 的矩阵，截取为 (N(e) imes3) 的矩阵。注，(N(e)) 表示误差 residuals 的个数。并且 QuaternionParameterization::Plus 中增量的加法应与 CostFunction 中对 so(3) 导数的左右扰动一致。在这个过程中，用户在 CostFunction 中返回的对 so(3) 导数应当是最后一列留 0，前三列填充值。（需要注意，ceres::QuaternionParameterization::Plus 操作的是半轴角，而不是轴角，如果直接套用，会使得计算出的雅克比差一个系数 2。）

当然以上这段文字描述的操作可以实现是有前提条件的，“所有用户对 Manifold 变量的导数在进入 ceres 之后会先经过 LocalParameterization::ComputeJacobian 转换为对 Tangent Space 变量的导数，再于其他处使用”。在当前对 ceres 的使用范围内，以上假设没有被打破。.

现在证明使用 AutoDiff 计算得到的误差对四元数的导数乘以 ceres::QuaternionParameterization::ComputeJacobian 返回矩阵的结果就是误差对 so(3) 的左扰动导数。

Quaternion 在误差计算中最广泛的用法是将一个三维点坐标从某个坐标系转换到另一个坐标系，如将世界坐标系下的坐标转换到相机坐标系下的坐标，({}^{C}mathbf{X}=mathbf{R}{{}^Cmathbf{q}_W}{}^Wmathbf{X}+{}^Cmathbf{t}_W)。按照链式法则，仅需证明 ({partial {}^{C}mathbf{X} over partial {}^Cmathbf{q}_W}{partial mathbf{ heta} otimes {}^Cmathbf{q}_W over partial mathbf{ heta}} = {partial {}^{C}mathbf{X} over partial mathbf{ heta}})。

参考文献 [1] 公式 (115)，得到 ({}^{C}mathbf{X})。

[egin{align} {}^{C}mathbf{X} &= egin{bmatrix} [1-2(q_y^2+q_z^2)]X + 2(q_xq_y-q_wq_z)Y + 2(q_xq_z+q_wq_y)Z + t_x 2(q_xq_y+q_wq_z)X + [1-2(q_x^2+q_z^2)]Y + 2(q_yq_z-q_wq_x)Z + t_y 2(q_xq_z-q_wq_y)X + 2(q_yq_z+q_wq_x)Y + [1-2(q_x^2+q_y^2)]Z + t_z end{bmatrix} {partial {}^{C}mathbf{X} over partial {}^Cmathbf{q}_W} &= egin{bmatrix} {partial {}^{C}X over partial q_w} & {partial {}^{C}X over partial q_x} & {partial {}^{C}X over partial q_y} & {partial {}^{C}X over partial q_z} {partial {}^{C}Y over partial q_w} & {partial {}^{C}Y over partial q_x} & {partial {}^{C}Y over partial q_y} & {partial {}^{C}Y over partial q_z} {partial {}^{C}Z over partial q_w} & {partial {}^{C}Z over partial q_x} & {partial {}^{C}Z over partial q_y} & {partial {}^{C}Z over partial q_z} end{bmatrix} otag &= 2 egin{bmatrix} q_yZ-q_zY & q_yY+q_zZ & q_wZ+q_xY-2q_yX & -q_wY+q_xZ-2q_zX -q_xZ+q_zX & -q_wZ-2q_xY+q_yX & q_xX+q_zZ & q_wX+q_yZ-2q_zY q_xY-q_yX & q_wY-2q_xZ+q_zX & -q_wX-2q_yZ+q_zY & q_xX+q_yY end{bmatrix} {partial {}^{C}mathbf{X} over partial {}^Cmathbf{q}_W}{partial mathbf{ heta} otimes {}^Cmathbf{q}_W over partial mathbf{ heta}} &= 2egin{bmatrix} q_yZ-q_zY & q_yY+q_zZ & q_wZ+q_xY-2q_yX & -q_wY+q_xZ-2q_zX -q_xZ+q_zX & -q_wZ-2q_xY+q_yX & q_xX+q_zZ & q_wX+q_yZ-2q_zY q_xY-q_yX & q_wY-2q_xZ+q_zX & -q_wX-2q_yZ+q_zY & q_xX+q_yY end{bmatrix} otag &phantom{=} {1 over 2}egin{bmatrix} -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{bmatrix} otag &= egin{bmatrix} 0 & 2(-q_wq_y+q_xq_z)+2(q_wq_x+q_yq_z)Y+[1-2(q_x^2+q_y^2)]Z & -2(q_wq_z+q_xq_y)X+[1-2(q_w^2+q_y^2)]Y+2(q_wq_x-q_yq_z)Z 2(q_wq_y-q_xq_z)-2(q_wq_x+q_yq_z)Y-[1-2(q_x^2+q_y^2)]Z & 0 & [1-2(q_y^2+q_z^2)]X+2(-q_wq_z+q_xq_y)Y+2(q_wq_y+q_xq_z)Z -2(q_wq_z+q_xq_y)X-[1-2(q_w^2+q_y^2)]Y+2(-q_wq_x+q_yq_z)Z & -[1-2(q_y^2+q_z^2)]X+2(q_wq_z-q_xq_y)Y-2(q_wq_y+q_xq_z)Z & 0 end{bmatrix} end{align}]

参考《视觉 SLAM 十四讲》 4.3.5，知道对 so(3) 的左扰动导数是 (-({}^Cmathbf{X})^{wedge})，这与以上推导得到的结果是基本对应的，差了一个位移 ({}^Cmathbf{t}_W)。这个差异可以解释，使用 se(3) 的扰动模型是 (Deltamathbf{R}(mathbf{R}mathbf{p}+mathbf{t})+Delta mathbf{t})，所以得到的对 so(3) 的导数中含有位移。而以上公式中的扰动模型是 ((Deltamathbf{R})mathbf{R}mathbf{p}+mathbf{t}+Deltamathbf{t})，应当应用《视觉 SLAM 十四讲》 4.3.4，在 so(3) 上进行扰动。总结，如果是对整个 se(3) 做一个 Parameterization（参考郑帆的博客《On-Manifold Optimization Demo using Ceres Solver》），那么就需要使用《视觉 SLAM 十四讲》 4.3.5 的导数模型，如果仅仅只是 QuaternionParameterization，那么仅需要使用《视觉 SLAM 十四讲》 4.3.4 的导数模型。在确定 Parameterization 的导数模型时，也应该关注在 CostFunction 中 residual 是如何计算的。

2. ceres::LocalParameterization 的其他用法

以上内容已对 ceres::QuaternionParameterization 中最重要的两个函数 ceres::QuaternionParameterization::ComputeJacobian 与 ceres::QuaternionParameterization::Plus 进行解释。ceres::QuaternionParameterization 继承于 ceres::LocalParameterization，在了解 ceres::QuaternionParameterization 运行原理的情况下，可以根据自身需求继承实现自己的 ceres::LocalParameterization。

一个非常好用的简单例子是“固定模长优化”。如前面的解释 ceres::LocalParameterization 用于 Manifold 上变量的优化，在 Manifold 上变量之间存在相互约束，所以可以看做是存在限制条件的优化。“固定模长优化”是对待优化变量进行了模长限制，最常见的“固定模长优化”是重力加速度方向优化，参考 VINS [2] 的 Algorithm 1，初始化过程中的重力加速度方向优化。

在实现过程中，可以在 CostFunction 中计算对 gravity 三维变量的导数，在 ceres::LocalParameterization::ComputeJacobian 中确定主方向，计算文中的 (mathbf{b}_1, mathbf{b}_2)，将对 gravity 三维变量的导数投影到 (mathbf{b}_1, mathbf{b}_2) 这两个方向上。优化出来的 (mathbf{b}_1, mathbf{b}_2) 两个方向上的增量，在 ceres::LocalParameterization::Plus 中合并到 gravity 三维坐标中，并且在合并过程中，注意归一化，实现固定模长。这个方法，经过本人实验，是成功的。VINS 代码没有细看，粗看其中函数 void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x) 并没有使用 ceres::LocalParameterization 处理。

参考文献

[1] Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).

[2] VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen, IEEE Transactions on Robotics.