Manifold

class Manifold

在多传感器融合问题中，我们经常需要对处在流形上的量进行建模和参数化，例如使用四元数表示的旋转。流形空间在局部类似于欧式空间，更确切地说，在流形上的每一点都有一个与流形相切的线性空间。它的维度等于流形本身的内在维度，小于或等于流形所嵌入的局部空间。注意：ambient space没找到准确的翻译，这里翻译为局部空间 或者叫做 本地空间。

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

• 它们是欧几里得空间，因此适用于通常的向量空间运算，这使得数值运算变得简单。

• 切线空间中的运动转化为沿流形的运动。垂直于切线空间的运动不会转化为流形上的运动。

然而，沿着与球面相切的二维平面移动并投影到球面上会使你远离起始点，但沿着同一点的法线移动并投影到球面上会使你回到起始点。除了数学上的有点之外，正确的对流行进行数值上的建模并关注其几何特征对实际使用也有很大帮助。

• 在整个优化过程中，它自然而然地将变量约束在流形上，使用户无需再使用四元数归一化等技巧。

• 它将优化问题的维度缩小到自然大小。例如，一个限制在一条线上的量是一个一维对象，而不管这条线所处的局部空间的维度是多少。

• 在切线空间工作不仅能降低优化算法的计算复杂度，还能改善算法的数值表现。

一个可以在流形上进行的基本操作是 $\boxplus$，它计算在切空间上从 $x$ 处移动 $\delta$ 的结果，然后投影回 $x$ 所属的流形上，也称为 Retraction，$\boxplus$ 是欧几里德空间中向量加法的泛化。$\boxplus$ 操作的逆运算是 $\boxminus$，给定两个在流形上的点 $x,y$，计算在 $x$ 处的切向量$\Delta$，也就是 $\boxplus(x, \Delta)=y$。现在来考虑如下的两个例子

欧式空间 $\mathbb{R}^n$ 是一个最简单的流形空间，它的维度为 $n$ ，同时它的局部空间的维度也为 $n$。$\boxminus$，$\boxplus$ 操作都是我们所熟悉的正常加减法。

$$\begin{aligned} \boxplus(x,\Delta)&=x+\Delta = y\\ \boxminus(y,x)&=y-x = \Delta \end{aligned}$$

一个更有趣的例子是 $SO(3)$，特殊正交群（ special orthogonal group），在流形上为 3 维，表示为 $3\times3$ 的旋转矩阵，简单来说，$SO(3)$ 是嵌入在 $\mathbb{R}^9$ 或者说是 $\mathbb{R}^{3\times 3}$ 中的三维流形。所以在 $SO(3)$ 中的点一般用 9 维向量或者 $3\times3$的矩阵表示，而它切空间上，只需要 3 个维度就可以表示。

对于$SO(3)$ ，$\boxminus, \boxplus$ 操作被定义为矩阵的 $\log, \exp$ 映射，给定一个三维向量 $\Delta = [p,q,r]$

$$\exp (\Delta)=\left[\begin{array}{ccc} \cos \theta+c p^{2} & -s r+c p q & s q+c p r \\ s r+c p q & \cos \theta+c q^{2} & -s p+c q r \\ -s q+c p r & s p+c q r & \cos \theta+c r^{2} \end{array}\right]$$

其中

$$\begin{array}{l} \theta=\sqrt{p^{2}+q^{2}+r^{2}}, \\ s=\frac{\sin \theta}{\theta} \\ c=\frac{1-\cos \theta}{\theta^{2}} . \end{array}$$

$x \in S O(3)$ ，我们有

$$\log (x)=1 /(2 \sin (\theta) / \theta)\left[x_{32}-x_{23}, \quad x_{13}-x_{31}, \quad x_{21}-x_{12}\right]$$

其中

$$\theta=\cos ^{-1}((\operatorname{Trace}(x)-1) / 2)$$

有：

$$\begin{aligned} \boxplus(x, \Delta) & =x \exp (\Delta) \\ \boxminus(y, x) & =\log \left(x^{T} y\right) \end{aligned}$$

为使 $\boxplus$ 和 $\boxminus$ 在数学上保持一致，流形上的所有点 $x$ 必须满足以下等式：

1. $\boxplus(x, 0)=x$ 。这就确保了切空间以 x 为中心，而零向量是恒等映射。

2. 对于流形上的所有 $y$，$\boxplus(x, \boxminus(y, x))=y$ 。这确保了任何 $y$ 都可以从 $x$ 到达。

3. 对于所有的 $\Delta, \boxminus(\boxplus(x, \Delta), x)=\Delta$ 。这确保了 $\boxplus$ 是一个一一对应映射。

4. 对于所有 $\Delta_{1}, \Delta_{2}\left|\boxminus\left(\boxplus\left(x, \Delta_{1}\right), \boxplus\left(x, \Delta_{2}\right)\right)\right| \leq \left|\Delta_{1}-\Delta_{2}\right|$. \中间。允许我们在流形上定义一个度量。

此外，我们要求 $\boxminus, \boxplus$ 足够平滑。特别地，它们需要在流形上处处可微。Ceres 的 Manifold 接口允许用户定义一个流形来实现优化目的的 Plus 和 Minus 运算及它们的求导

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

int Manifold::AmbientSize() const;

流形嵌入到的局部空间维度。比如在用四元数表示时其值为 4。对应老版本 Ceres 的 GlobalSize()。

int Manifold::TangentSize() const;

流形空间上的维度，或者说切空间中的维度。

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

实现流形上的 $\boxplus(x,\Delta)$ 操作，欧几里得空间中向量加法的推广（广义加），Plus 计算在切空间中 x 沿 delta 移动的结果，然后投影到 x 所属的流形上。x 和 x_plus_delta 是 Manifold::AmbientSize() 向量。delta 是一个 Manifold::TangentSize() 向量。 (比如说 x 和 x_plus_delta 属于 $SO(3)$，而 delta 属于 $\mathfrak{se}(3)$。返回值指示运算是否成功。

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

Compute the derivative of $\boxplus (x,\Delta)$ w.r.t $\Delta=0$ at $\Delta=0$ , i.e. $(D_2\boxplus)(x,0)$，jacobian 是一个行为主的 Manifold::AmbientSize() $\times$ Manifold::TangentSize() 的矩阵。返回值表示计算是否成功。

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

tangent_matrix $=$ ambient_matix $\times$ plus_jacobian。ambient_matrix 是一个行为主的 num_rows $\times$ Manifold::AmbientSize() 大小的矩阵。tangent_matrix 是一个行为主的 num_rows $\times$ Manifold::TangentSize() 的矩阵。返回值表示计算是否成功。

This function is only used by the GradientProblemSolver, where the dimension of the parameter block can be large and it may be more efficient to compute this product directly rather than first evaluating the Jacobian into a matrix and then doing a matrix vector product.

由于这个函数并不常用，为了方便起见，Ceres 提供了一个默认实现。如果考虑性能，那么用户应该考虑自行实现一个。

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

实现流形上的 $\boxminus (y,x)$ 的操作，Minus 是欧几里得空间中向量减法的一种概括。给定流形上的两个点 x 和 y，Minus 会计算一个在 x 处的切线空间中对 x 的增量，使其到达 y。x 和 y 是Manifold::AmbientSize() 向量，y_minus_x 是 Manifold::TangentSize() 向量。返回值表示操作是否成功。

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

Compute the derivative of $\boxminus (y,x)$ w.r.t. $y$ at $y=x$, i.e. $(D_1\boxminus)(x,x)$ ，jacobian 是一个行为主的 Manifold::TangentSize() $\times$ Manifold::AmbientSize() 的矩阵。返回值表示计算是否成功。

Ceres 提供了大量的实际中经常用到的 Manifold 实例，对于 Lie Groups，Sophus 是一个不错的选择。

class EuclideanManifold

EuclideanManifold 代表欧式空间上的操作，其 $\boxplus,\boxminus$ 都是我们平常所熟知的向量加减法。

$$\begin{aligned} \boxplus(x,\Delta)&=x+\Delta\\ \boxminus(y,x)&=y-x \end{aligned}$$

默认情况下，参数都被认为是欧式空间上的，所以无需使用，提供 EuclideanManifold 的目的是为了测试，也是为了和其他流形上的实现一起实用，比如 ProductManifold，我们在后面会作出说明。

该类适用于动态和静态的局部空间维度。如果编译时已知道局部空间维度，则使用

EuclideanManifold<3> manifold;

如果在编译时不知道局部空间尺寸，则需要将模板参数设置为 ceres::DYNAMIC，并将实际尺寸作为构造函数参数提供：

EuclideanManifold<ceres::DYNAMIC> manifold(ambient_dim);

class SubsetManifold

假设 $x$ 是一个两维的向量，并且用户希望让第一个维度为常数，不优化它，那么此时的 $\Delta$ 是一个标量，$\boxplus$ 可以定义为：

$$\boxplus(x,\Delta)=x+ \begin{bmatrix} 0\\ 1\\ \end{bmatrix} \Delta$$

给定两个维度为 2 的向量 $x,y$，第一个维度的值相同，$\boxminus$ 定义为

$$\boxminus(y,x)=y[1]-x[1]$$

SubsetManifold 将这种构造推广到通过指定坐标集来保持参数块的任何部分不变。

It is legal to hold all coordinates of a parameter block to constant using a SubsetManifold. It is the same as calling Problem::SetParameterBlockConstant() on that parameter block.

class ProductManifold

In cases, where a parameter block is the Cartesian product of a number of manifolds and you have the manifold of the individual parameter blocks available, ProductManifold can be used to construct a Manifold of the Cartesian product.

在刚性变换的情况下，假设有一个大小为 7 的参数块，其中前四个条目表示四元数的旋转，后三个条目表示平移，那么流形就可以这样构建：

ProductManifold<QuaternionManifold, EuclideanManifold<3>> se3;

Manifolds 能够被移动赋值到 ProductManifold:

SubsetManifold manifold1(5, {2});
SubsetManifold manifold2(3, {0, 1});
ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1,
                                                         manifold2);

在高级用例中，流形可以动态分配，并作为（智能）指针传递：

ProductManifold<std::unique_ptr<QuaternionManifold>, EuclideanManifold<3>> se3
{std::make_unique<QuaternionManifold>(), EuclideanManifold<3>{}};

模板参数也可以省略，因为它们是自动推导出来的，使初始化简单得多：

ProductManifold se3{QuaternionManifold{}, EuclideanManifold<3>{}};

class QuaternionManifold

If you are using Eigen quaternions, then you should use EigenQuaternionManifold instead because Eigen uses a different memory layout for its Quaternions.

四元数是用 4 个维度的单位向量表示的三维流形，即

$$q=[q_0,q_1,q_2,q_3],||q||=1$$

是局部空间上的表示形式，$q_0$ 是实部，$q_1$ 是 $i$ 的系数，$q_2$ 是 $j$ 的系数，$q_3$ 是 $k$ 的系数，其中

$$\begin{aligned} 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{aligned}$$

正切空间是 3 维的，其加减操作可以定义为

$$\begin{aligned} \boxplus(x,\Delta)&=\exp(\Delta)\otimes x\\ \boxminus(y,x)&=\log(y\otimes x^{-1}) \end{aligned}$$

其中 $\otimes$ 是 Quaternion product，$x$ 是一个单位四元数，$x^{-1}=[q_0,-q_1,-q_2,-q_3]$，给定一个三维空间中的向量 $\Delta \in \mathbb{R}^3$

$$\exp(\Delta)= \begin{bmatrix} \cos (||\Delta||)\\ \frac{\sin(\Delta)}{||\Delta||}\Delta \end{bmatrix}$$

给定单位四元数 $q=[q_0,q_1,q_2,q_3]$

\log(q)=\frac{\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 EigenQuaternionManifold

Eigen 实现的 Hamilton quaternion 表示方法，在几何上，它与上面定义的四元数流形（QuaternionManifold）完全相同。不过，Eigen 为四元数元素使用的内部存储器布局与常用的不同。Eigen 中存储的是 $q=[q_1,q_2,q_3,q_0]$ 或者是 $[x,y,z,w]$，实部为最后一个。由于 Ceres 对原始指针的参数块进行操作，这种差异非常重要，需要定义不同的流形。

class SphereManifold

这提供了一个球面上的流形，意味着矢量的法线保持不变。这种情况经常出现在 Structure for Motion 问题中。比如用它来表示三角剖分条件不佳的点。在这种情况下，使用过参数化是有利的，因为 homogeneous vectors 可以表示无穷远处的点。局部空间维度需要大于1。

该类适用于动态和静态的局部空间维度。如果编译时已知道局部空间维度，则使用

SphereManifold<3> manifold;

如果在编译时不知道局部空间尺寸，则需要将模板参数设置为 ceres::DYNAMIC，并将实际尺寸作为构造函数参数提供：

SphereManifold<ceres::DYNAMIC> manifold(ambient_dim);

class LineManifold

该类为线提供了一个流形，其中线的定义使用了一个原点和一个方向向量。因此，局部空间大小需要是线所在空间维度的两倍。参数块的前半部分为原点，后半部分为方向。This manifold is a special case of the Affine Grassmannian manifold) for the case $\text{Graff}_1(R^n)$.

请注意，这是一条直线的流形，而不受限于直线上的一个点。当我们想对直线空间进行优化时，它就会派上用场。例如，给定三维空间中的 n 个不同点（测量值），我们希望找到一条使所有点的距离平方和最小的直线。