Ceres Manifolds/LocalParameterization 类

Reference：

1. Ceres Solver
2. 【Ceres】（二）LocalParameterization参数化

系列文章：

1. Ceres-Solver 官方文档-其一
2. Ceres-Solver 官方文档-其二
3. Ceres Manifolds/LocalParameterization 类

在老版本中，想实现流形的建模，使用的是 LocalParameterization 类。在 V2.1.0 版本时，新增了 Manifolds 类并在 V2.2.0 时，Ceres-Solver 官方将 LocalParameterization 类删除了。两个类在用法上区别不大。但不得不说，老版本的命名方式确实不如新版，至少新版又短又一眼能看出其用途。

---

过参数化，即待优化参数的实际自由度小于参数本身的自由度。对于四元数或者旋转矩阵这种使用 过参数化 表示旋转的方式，不支持广义的加法（因为使用普通的加法就会打破其约束，比如旋转矩阵加旋转矩阵得到的就不再是旋转矩阵），所以我们在使用 Ceres 对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个参数本地化的子类，需要继承于Manifolds/LocalParameterization，Manifolds/LocalParameterization 是纯虚类，所以我们继承的时候要把所有的纯虚函数都实现一遍才能使用该类生成对象，一些常用的派生类将在第三节中描述。

除了不支持广义加法要自定义参数本地化的子类外，如果你要对优化变量做一些限制也可以如法炮制，比如在 Ceres-Solver 官方文档-其一 一文中提到的 slam/pose_graph_2d/pose_graph_2d.cc 对角度范围进行了限制。

在 Ceres 中，如果说CostFunction 中 operator() 与 g2o 中 computeError 相对(计算残差)；那么 AddParameterBlock 与 g2o 中添加顶点的方法 optimizer.addVertex 相对(添加优化变量)。在我们求得了增量 $\Delta x$ 后，需要将其更新至当前优化变量上，有的像欧几里得空间内的就不需要额外配置了。g2o 内的顶点更新函数为顶点内的成员函数 oplusImpl，而在 Ceres 中与之对应的就是 Manifolds/LocalParameterization 类了。

文中有几个名词需要留意：

1. ambient space：没找到准确的翻译，这里翻译为环境空间，上下文较为合适；
2. tangent space：切空间；
3. functor：拟函数。

1. Introduction

在传感器融合问题中，我们经常需要对 流形 空间中的量进行建模，例如由四元数表示的传感器的旋转/方向。(旋转矩阵和四元数是流形)

流形是局部看起来像欧几里德空间的空间。更准确地说，流形上的每一点都有一个与流形相切的线性空间。这个线性空间的维数等于流形本身的本征维度(Intrinsic dimension)(切空间)，小于或等于流形嵌入的环境空间。

例如，三维球面上一点的切空间是与球面在该点相切的二维平面(如李群与李代数)。切线空间之所以有趣，有两个原因：

1. 它们是欧几里德空间，所以通常的向量空间运算也适用于此，这使得数值运算变得简单。
2. 切空间中的运动转化为沿流形的运动。垂直于切空间的运动不能转化为流形上的运动。

然而，沿着与球体相切的二维平面移动并投射回球体会使你远离你开始时的点，但在同一点沿着法线移动并投射回球体会使你回到该点。

除了数学上的优点外，正确地建模流形数值并注意其几何形状也有实际的好处：

1. 在整个优化过程中，它自然地将量限制为流形，从而将用户从四元数的 normalization 之类的 hack 中解放出来。

2. 它将优化问题的维度降低到其自然大小。例如，限制在一条线上的量是一维对象，而不管这条线所在的环境空间的维度如何。

   在切空间中工作不仅降低了优化算法的计算复杂度，而且提高了算法的数值性能。

可以在流形上执行的一个基本操作是 $⊞$ 操作，它计算沿 $\delta$ 在切线空间中在 $x$ 处移动的结果，然后投影回 $x$ 所属的流形上。也称为收缩(Retraction)，$⊞$ 是欧几里德空间中向量加法的泛化。

与 $⊞$ 相反的是 $\boxminus$，它给出了流形上的两个点 $y$ 和 $x$，计算在$x$ 处的切向量 $\Delta$，也就是 $⊞(x,\Delta )=y$。

现在来考虑两个例子：

• 欧几里得空间 ${\mathbb{R}}^n$ 是流形最简单的例子。它的维数为 $n$(它的切空间也是如此)且 $⊞$/$\boxminus$ 是我们熟悉的向量 和/差 运算符。
  $$\begin{array}{rl}⊞(x,\Delta )& =x+\Delta =y\\ \boxminus (y,x)& =y-x=\Delta .\end{array}$$

• 一个更有趣的例子是 $SO(3)$，它是三维空间中的特殊正交群-------$3\times 3$ 旋转矩阵的空间。$SO(3)$是嵌入在 ${\mathbb{R}}^9$ 或者说 ${\mathbb{R}}^{3\times 3}$ 中的三维流形。所以 $SO(3)$ 上的点是用 $9$ 维向量或者 $3\times 3$ 矩阵表示的，而切空间中的点是用 $3$ 维向量表示的。

  对于$SO(3)$，$⊞$ 和 $\boxminus$ 根据矩阵 $\exp$ 和 $\log$ 运算定义如下(对数映射和指数映射公式)：
  给定一个三维向量 $\Delta =\left[\begin{array}{ll}p,& q,r\end{array}\right]$，我们有：
  $$\exp (\Delta )=\left[\begin{array}{ccc}\cos \theta +c{p}^2& -sr+cpq& sq+cpr\\ sr+cpq& \cos \theta +c{q}^2& -sp+cqr\\ -sq+cpr& sp+cqr& \cos \theta +c{r}^2\end{array}\right]$$其中，
  $$\begin{array}{rl}& \theta =\sqrt{p^2+q^2+r^2},\\ & s=\frac{\sin \theta }{\theta },\\ & c=\frac{1-\cos \theta }{{\theta }^2}.\end{array}$$给定一个 $x\in SO(3)$，我们有：
  $$\log (x)=1/(2\sin (\theta )/\theta )\left[x_{32}-x_{23},x_{13}-x_{31},x_{21}-x_{12}\right]$$其中，
  $$\theta ={\cos }^{-1}((\mathrm{Trace}(x)-1)/2)$$那么：
  $$\begin{array}{rl}⊞(x,\Delta )& =x\exp (\Delta )\\ \boxminus (y,x)& =\log \left(x^{T}y\right)\end{array}$$为了使 $⊞$ 和 $\boxminus$ 在数学上一致，必须让流形上的所有点 $x$ 满足以下特性：

  1. $⊞(x,0)=x$。这确保了切空间以 $x$ 为中心，零向量是单位元。
  2. 对于在流形上的所有 $y$，$⊞(x,\boxminus (y,x))=y$。这确保了可以从 $x$ 到任何 $y$。
  3. 对于所有的 $\Delta$，$\boxminus (⊞(x,\Delta ),x)=\Delta$。这确保 $⊞$ 是一个单一(一对一)映射。
  4. 对于所有的 ${\Delta }_1$、${\Delta }_2$，$\left|\boxminus \left(⊞\left(x,{\Delta }_1\right),⊞\left(x,{\Delta }_2\right)\right)\le \right|{\Delta }_1-{\Delta }_2\mid$。允许我们在流形上定义度量。

  此外，我们要求 $⊞$ 和 $\boxminus$ 足够平滑。特别地，它们需要在流形上处处可微。

  Ceres 的 Manifold 接口允许用户定义一个流形来实现优化目的的 Plus 和 Minus 运算 及 它们的求导(对应 $⊞$ 和 $\boxminus$)。

2. class Manifold

类 Manifold 定义方式如下，可以看到 Manifold 是一个 纯虚函数：

class Manifold {
 public:
  virtual ~Manifold();
  virtual int AmbientSize() const = 0;
  virtual int TangentSize() const = 0;
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;
  virtual bool PlusJacobian(const double* x, double* jacobian) const = 0;
  virtual bool RightMultiplyByPlusJacobian(const double* x,
                                           const int num_rows,
                                           const double* ambient_matrix,
                                           double* tangent_matrix) const;
  virtual bool Minus(const double* y,
                     const double* x,
                     double* y_minus_x) const = 0;
  virtual bool MinusJacobian(const double* x, double* jacobian) const = 0;
};

2.1 int Manifold::AmbientSize() const;

流形嵌入到的 环境空间 维度。
LocalParameterization 内为 GlobalSize()。

2.2 int Manifold::TangentSize() const;

流形/切空间 维度。
LocalParameterization 内为 LocalSize()。

2.3 bool Plus(const double *x, const double *delta, double *x_plus_delta) const;

该成员函数为流形实现 $⊞(x,\Delta )$ 运算。

欧几里得空间中向量加法的推广(广义加)，Plus 计算在切空间中 x 沿 delta 移动的结果，然后投影到 $x$ 所属的流形上。

x 和 x_plus_delta 是 Manifold::AmbientSize() 向量。delta 是一个 Manifold::TangentSize() 向量。
(比如说 x 和 x_plus_delta 属于 $SO(3)$，而 delta 属于 $\mathfrak{so}(3)$)

返回值指示运算是否成功。

2.4 bool PlusJacobian(const double *x, double *jacobian) const;

计算 $⊞(x,\Delta )$ 的导数，关于 $\Delta$ 在 $\Delta =0$，也就是 $\left(D_2⊞\right)(x,0)$。

jacobian 是一个 row-major 的大小为 Manifold::AmbientSize() $\times$ Manifold::TangentSize() 矩阵。

LocalParameterization 内为 ComputeJacobian()。

2.5 bool RightMultiplyByPlusJacobian(const double * $x$, const int num_rows, const double *ambient_matrix, double *tangent_matrix) const;

tangent_matrix$=$ambient_matrix $\times$ plus_jacobian.
ambient_matrix 是一个 row-major num_rows $\times$ Manifold::AmbientSize() 的矩阵。
tangent_matrix 是一个 row-major num_rows $\times$ Manifold::TangentSize() 的矩阵。

这个函数只被 GradientProblemSolver 使用，其中参数块的维度可能很大，直接计算这个乘积可能更有效，而不是先将雅可比矩阵计算成矩阵，然后再做矩阵向量乘积。

由于这不是一个经常使用的函数，因此为了方便起见，Ceres 内提供了一个默认实现。如果性能成为一个问题，那么用户应该考虑实现专门化。

LocalParameterization 内为 MultiplyByJacobian()，本身的写法就是右乘雅可比，这样写定义更明确。

2.6 bool Minus(const double *y, const double *x, double *y_minus_x) const;

实现流形的 $\boxminus (y,x)$ 运算。

欧几里得空间中向量减法的推广(广义减)，Minus 计算在切空间中 x 在 x 处的变化，这将使它到达 y。

x 和 y 是 Manifold::AmbientSize() 维度向量。y_minus_x 是一个 Manifold::TangentSize() 维度向量。

2.7 bool MinusJacobian(const double *x, double *jacobian) const = 0;

计算 $\boxminus (y,x)$ 的导数，关于 $y$ 在 $y=x$，也就是 $\left(D_1\boxminus \right)(x,x)$。

jacobian 是一个 row-major 的大小为 Manifold::TangentSize() $\times$ Manifold::AmbientSize() 矩阵。

3. Manifold 派生类

在上一节中可以获知，Manifold 是一个纯虚函数，其实现留给了该基类的派生类去做。Ceres Solver 配置了大量常用的 Manifold 实例。现在来介绍一下这些派生类(当前主要介绍 QuaternionManifold 和 AutoDiffManifold)：

3.1 EuclideanManifold

3.2 SubsetManifold

3.3 ProductManifold

3.4 QuaternionManifold/QuaternionParameterization

注意：如果使用的是 Eigen 的四元数，那么应该使用 3.5小节的 EigenQuaternionManifold 来替代，因为 Eigen 为其四元数使用不同的内存布局。(QuaternionParameterization 中表示四元数中四个量在内存中的存储顺序是[w, x, y, z]，而 Eigen 内部四元数在内存中的存储顺序是 [x, y, z, w]，但是其构造顺序是 [w, x, y, z])

对于一个 Hamilton 四元数流形(比如矩阵旋转使用的四元数)，四元数是用单位四维向量表示的三维流形，即：
$$q=\left[q_0,q_1,q_2,q_3\right],\Vert q\Vert =1$$这是个环境空间表示。这里 $q_0$ 为标量部分，即通常说的 $w$。$q_1$、$q_2$、$q_3$ 分别为 $i$、$j$、$k$ 的系数，其中：
$$\begin{array}{rl}i\times j& =k,\\ j\times k& =i,\\ k\times i& =j,\\ i\times i& =-1,\\ j\times j& =-1,\\ k\times k& =-1.\end{array}$$切空间是三维的，$⊞$ 和 $\boxminus$ 运算符是根据 $\exp$ 和 $\log$ 运算定义的(上面公式还是右乘的，这里写成左乘了，就挺随意的)。
$$\begin{array}{rl}⊞(x,\Delta )& =\exp (\Delta )\otimes x\\ \boxminus (y,x)& =\log \left(y\otimes x^{-1}\right)\end{array}$$其中 $\otimes$ 为四元数乘法且 $x$ 是一个单位矩阵，$x^{-1}=\left[\begin{array}{ll}{q}_0,& -q_1,-q_2,-q_3\end{array}\right]$。给定一个向量 $\Delta \in {\mathbb{R}}^3$(这里的 $\Delta$ 被称为纯虚四元数，可参考《自动驾驶与机器人中的SLAM技术》p30)，
$$\exp (\Delta )=\left[\begin{array}{c}\cos (\Vert \Delta \Vert )\\ \frac{\sin (\mid \Delta \Vert )}{\Vert \Delta \Vert }\Delta \end{array}\right]$$给定一个单位四元数 $q=\left[\begin{array}{llll}{q}_0,& q_1,& q_2,& q_3\end{array}\right]$
$$\log (q)=\frac{\mathrm{atan}2\left(\sqrt{1-q_0^2},q_0\right)}{\sqrt{1-q_0^2}}\left[q_1,q_2,q_3\right]$$

class CERES_EXPORT QuaternionManifold final : public Manifold {
 public:
  int AmbientSize() const override { return 4; }
  int TangentSize() const override { return 3; }

  bool Plus(const double* x,
            const double* delta,
            double* x_plus_delta) const override;
  bool PlusJacobian(const double* x, double* jacobian) const override;
  bool Minus(const double* y,
             const double* x,
             double* y_minus_x) const override;
  bool MinusJacobian(const double* x, double* jacobian) const override;
};

3.5 EigenQuaternionManifold

3.6 SphereManifold

3.7 LineManifold

3.8 AutoDiffManifold/AutoDiffLocalParameterization

创建一个通过自动求导计算雅可比矩阵的流形。

template <typename Functor, int kAmbientSize, int kTangentSize>
class AutoDiffManifold final : public Manifold {
 public:
  AutoDiffManifold() : functor_(std::make_unique<Functor>()) {}

  // Takes ownership of functor.
  explicit AutoDiffManifold(Functor* functor) : functor_(functor) {}

  int AmbientSize() const override { return kAmbientSize; }
  int TangentSize() const override { return kTangentSize; }

  bool Plus(const double* x,
            const double* delta,
            double* x_plus_delta) const override {
    return functor_->Plus(x, delta, x_plus_delta);
  }

  bool PlusJacobian(const double* x, double* jacobian) const override;

  bool Minus(const double* y,
             const double* x,
             double* y_minus_x) const override {
    return functor_->Minus(y, x, y_minus_x);
  }

  bool MinusJacobian(const double* x, double* jacobian) const override;

  const Functor& functor() const { return *functor_; }

 private:
  std::unique_ptr<Functor> functor_;
};

为了得到一个自动求导流形，需要定义一个带有模版 Plus 和 Minus 函数的 拟函数，其计算：

x_plus_delta = Plus(x, delta);
y_minus_x    = Minus(y, x);

其中，x、y 和 x_plus_y 是环境空间中流形上的向量(因此它们是 kAmbientSize 向量)，而 delta、y_minus_x 是切空间中的向量(因此它们是 kTangentSize 向量)。

拟函数的签名应包括：

struct Functor {
 template <typename T>
 bool Plus(const T* x, const T* delta, T* x_plus_delta) const;

 template <typename T>
 bool Minus(const T* y, const T* x, T* y_minus_x) const;
};

注意，Plus 和 Minus 操作是在参数 T 上模板化的。自动求导框架为 T 替换适当的 Jet 对象，以便在必要时计算导数。这和使用 AutoDiffCostFunction 计算导数的原理是一样的。

如果计算成功，Plus 和 Minus 应该返回 true，否则返回 false，在这种情况下，结果将不被使用。

给定这个拟函数，相应的流形可以构造为：

AutoDiffManifold<Functor, kAmbientSize, kTangentSize> manifold;

需要注意的是，老版本 AutoDiffLocalParameterization 在定义上有些区别：

template <typename Functor, int kGlobalSize, int kLocalSize>
class AutoDiffLocalParameterization : public LocalParameterization {
 public:
  AutoDiffLocalParameterization() :
      functor_(new Functor()) {}

  // Takes ownership of functor.
  explicit AutoDiffLocalParameterization(Functor* functor) :
      functor_(functor) {}

  virtual ~AutoDiffLocalParameterization() {}
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const {
    return (*functor_)(x, delta, x_plus_delta);
  }

  virtual bool ComputeJacobian(const double* x, double* jacobian) const {
    double zero_delta[kLocalSize];
    for (int i = 0; i < kLocalSize; ++i) {
      zero_delta[i] = 0.0;
    }

    double x_plus_delta[kGlobalSize];
    for (int i = 0; i < kGlobalSize; ++i) {
      x_plus_delta[i] = 0.0;
    }

    const double* parameter_ptrs[2] = {x, zero_delta};
    double* jacobian_ptrs[2] = { NULL, jacobian };
    return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize>
        ::Differentiate(*functor_,
                        parameter_ptrs,
                        kGlobalSize,
                        x_plus_delta,
                        jacobian_ptrs);
  }

  virtual int GlobalSize() const { return kGlobalSize; }
  virtual int LocalSize() const { return kLocalSize; }

 private:
  internal::scoped_ptr<Functor> functor_;
};

可以注意到，AutoDiffLocalParameterization 内函数 Plus 的使用方式是 return (*functor_)(x, delta, x_plus_delta)，而 AutoDiffManifold 内为 return functor_->Plus(x, delta, x_plus_delta)。所以在使用上，AutoDiffManifold 定义的拟函数需要定义 Plus 和 Minus；而 AutoDiffLocalParameterization 是做运算符 () 重载：template <typename T> bool operator()(const T* rad, const T* d_rad, T* rad_result) const {};。

四元数示例

以下仅用于说明目的。Ceres Solver 提供了一个优化的、生产级的 QuaternionManifold 实现。

作为一个具体的例子，考虑四元数的情况。四元数形成嵌入在 ${\mathbb{R}}^4$ 中的三维流形，即它们的环境维数为 $4$，它们的切空间维数为 $3$。下面的拟函数定义了四元数流形上的 Plus 和 Minus 操作。它假设四元数在内存中以 [w,x,y,z] 的形式布局，即实部或标量部分是第一个坐标。

struct QuaternionFunctor {
  template <typename T>
  bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
    const T squared_norm_delta =
        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];

    T q_delta[4];
    if (squared_norm_delta > T(0.0)) { // 可以做除法
      T norm_delta = sqrt(squared_norm_delta);
      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
      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];
    } else { // 不能做除法
      // We do not just use q_delta = [1,0,0,0] here because that is a
      // constant and when used for automatic differentiation will
      // lead to a zero derivative. Instead we take a first order
      // approximation and evaluate it at zero.
      q_delta[0] = T(1.0);
      q_delta[1] = delta[0];
      q_delta[2] = delta[1];
      q_delta[3] = delta[2];
    }

    QuaternionProduct(q_delta, x, x_plus_delta);
    return true;
  }

  template <typename T>
  bool Minus(const T* y, const T* x, T* y_minus_x) const {
    T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
    T ambient_y_minus_x[4];
    QuaternionProduct(y, minus_x, ambient_y_minus_x);
    T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
                    ambient_y_minus_x[2] * ambient_y_minus_x[2] +
                    ambient_y_minus_x[3] * ambient_y_minus_x[3]);
    if (u_norm > 0.0) {
      T theta = atan2(u_norm, ambient_y_minus_x[0]);
      y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
      y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
      y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
    } else {
      We do not use [0,0,0] here because even though the value part is
      a constant, the derivative part is not.
      y_minus_x[0] = ambient_y_minus_x[1];
      y_minus_x[1] = ambient_y_minus_x[2];
      y_minus_x[2] = ambient_y_minus_x[3];
    }
    return true;
  }
};

然后给定这个结构，自动微分四元数流形现在可以构造为：

Manifold* manifold = new AutoDiffManifold<QuaternionFunctor, 4, 3>;