g2o_a_general_framework_for_graph_optimaization

g2o: A General Framework for Graph Optimization

NONLINEAR GRAPH OPTIMIZATION USING LEAST-SQUARES

• 机器人和计算机视觉中的许多问题都可以用下列方程的最小化来求解

• $$\mathbf{F}(\mathbf{x})=\sum _{⟨i,j⟩\in \mathcal{C}}\underset{{\mathbf{F}}_{ij}}{\mathbf{e}{\left({\mathbf{x}}_i,{\mathbf{x}}_j,{\mathbf{z}}_{ij}\right)}^{\top }{\mathbf{\Omega }}_{ij}\mathbf{e}\left({\mathbf{x}}_i,{\mathbf{x}}_j,{\mathbf{z}}_{ij}\right)}$$

• $${\mathbf{x}}^{\ast }=\underset{\mathbf{x}}{\mathrm{argmin}}\mathbf{F}(\mathbf{x}).$$

  • $({\mathbf{x}}_1^{\mathbf{T}},\cdots ,{\mathbf{x}}_{\mathbf{n}}^{\mathbf{T}}{)}^{\mathbf{T}}$：参数向量， ${\mathbf{x}}_i$：表示一个一般的参数块

  • ${\mathbf{z}}_{ij}$和${\Omega }_{ij}$：分别表示与参数相关约束的均值和信息矩阵

  • $\mathbf{e}\left({\mathbf{x}}_i,{\mathbf{x}}_j,{\mathbf{z}}_{ij}\right)$ ：向量误差函数，度量参数块${\mathbf{x}}_i$和${\mathbf{x}}_j$ 满足约束 ${\mathbf{z}}_{ij}$ 的程度，完全满足约束条件时为$0$

• 简单起见:

• $$\mathbf{e}\left({\mathbf{x}}_i,{\mathbf{x}}_j,{\mathbf{z}}_{ij}\right)\stackrel{\text{ def. }}{=}{\mathbf{e}}_{ij}\left({\mathbf{x}}_i,{\mathbf{x}}_j\right)\stackrel{\text{ def. }}{=}{\mathbf{e}}_{ij}(\mathbf{x}).$$

  • 注意：每一个误差函数，每一个参数块，每一个误差函数都能张成不同的空间。在这种形式下，一个问题能用一个图来高效地表示。

  • 节点$i$ 表示参数块${\mathbf{x}}_i$ ，节点$i$ 和节点$j$ 之间的边表示两个参数块${\mathbf{x}}_j和{\mathbf{x}}_i$ 之间的有序约束。

  • [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ciloICLW-1648560327983)(g2o_a_general_framework_for_graph_optimaization.assets/image-20220328163210512.png)]

• A. 最小二乘优化

  • 如果已知参数$\breve{\mathbf{X}}$ 有一个良好的猜测值，可以用(2) 使用G-N或L-M算法求数值解，思想：用当前状态猜测值$\breve{\mathbf{X}}$ 附近的一阶泰勒展开近似误差函数。

  • $${\mathbf{e}}_{ij}\left({\breve{\mathbf{x}}}_i+\mathbf{\Delta }{\mathbf{x}}_i,{\breve{\mathbf{x}}}_j+\mathbf{\Delta }{\mathbf{x}}_j\right)={\mathbf{e}}_{ij}(\breve{\mathbf{x}}+\mathbf{\Delta }\mathbf{x})$$

  • $$\simeq {\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\Delta \mathbf{x}.$$

  • ${\mathbf{J}}_{ij}$ 是在$\breve{\mathbf{X}}$ 和${\mathbf{e}}_{ij}\stackrel{\text{ def. }}{=}{\mathbf{e}}_{ij}(\breve{\mathbf{X}})$ 处的雅克比，将(5) 带入(1)中${\mathbf{F}}_{ij}$​的误差项中得到：

  • $$\begin{array}{rl}{\mathbf{F}}_{ij}& (\breve{\mathbf{x}}+\mathbf{\Delta }\mathbf{x})\\ & ={\mathbf{e}}_{ij}(\breve{\mathbf{x}}+\mathbf{\Delta }\mathbf{x}{)}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}(\breve{\mathbf{x}}+\mathbf{\Delta }\mathbf{x})\\ & \simeq {\left({\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\mathbf{\Delta }\mathbf{x}\right)}^{\top }{\mathbf{\Omega }}_{ij}\left({\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\mathbf{\Delta }\mathbf{x}\right)\\ & =\underset{c_{ij}}{{\mathbf{e}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}}+2\underset{{\mathbf{b}}_{ij}}{{\mathbf{e}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{J}}_{ij}}\mathbf{\Delta }\mathbf{x}+\Delta {\mathbf{x}}^{\top }\underset{{\mathbf{H}}_{ij}}{{\mathbf{J}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{J}}_{ij}}\mathbf{\Delta }\mathbf{x}(9)\\ & ={\mathrm{c}}_{ij}+2{\mathbf{b}}_{ij}\mathbf{\Delta }\mathbf{x}+\mathbf{\Delta }{\mathbf{x}}^{\top }{\mathbf{H}}_{ij}\mathbf{\Delta }\mathbf{x}\end{array}$$

  • 有了这个局部近似后，重写(1)中的函数$\mathbf{F}(\mathbf{x})$​：

  • $$\begin{array}{rl}\mathbf{F}(\breve{\mathbf{x}}+\mathbf{\Delta }\mathbf{x})& =\sum _{⟨i,j⟩\in \mathcal{C}}{\mathbf{F}}_{ij}(\breve{\mathbf{x}}+\mathbf{\Delta }\mathbf{x})\\ & \simeq \sum _{⟨i,j⟩\in \mathcal{C}}{c}_{ij}+2{\mathbf{b}}_{ij}\mathbf{\Delta }\mathbf{x}+\Delta {\mathbf{x}}^{\top }{\mathbf{H}}_{ij}\mathbf{\Delta }\mathbf{x}(12)\\ & =\mathrm{c}+2{\mathbf{b}}^{\top }\mathbf{\Delta }\mathbf{x}+\mathbf{\Delta }{\mathbf{x}}^{\top }\mathbf{H}\mathbf{\Delta }\mathbf{x}.\end{array}$$

    • $c=\sum c_{ij},\mathbf{b}=\sum {\mathbf{b}}_{ij},\mathbf{H}=\sum {\mathbf{H}}_{ij}.$

  • 通过求解线性方程组来最小化$\Delta \mathbf{x}$​ :

  • $$\mathbf{H}\Delta {\mathbf{x}}^{\ast }=-\mathbf{b}.$$

  • $\mathbf{H}$是系统的信息矩阵，解为初始猜想加上增量$\Delta {\mathbf{x}}^{\ast }$​

  • $${\mathbf{x}}^{\ast }=\breve{\mathbf{x}}+\Delta {\mathbf{x}}^{\ast }.$$

  • L-M算法：

  • $$(\mathbf{H}+\lambda \mathbf{I})\mathbf{\Delta }{\mathbf{x}}^{\ast }=-\mathbf{b}.$$

• B. 可选参数化

  • 上面的问题假设参数$\mathbf{x}$处于欧几里得空间上，对于SLAM或BA等问题并不适用。

  • 为了处理非欧空间上的状态变量，常见方法是在一个不同于参数${\mathbf{x}}_i$ 的空间中表示增量$\Delta \mathbf{x}$

  • SLAM 中每个参数块${\mathbf{x}}_i=\{t_i,{\alpha }_i\}$ ，$t_i$ 为欧式空间中表示平移的向量，${\alpha }_i$ 为2D或3D旋转群$SO(2)或SO(3)$ 张成的非欧空间。

  • 为避免奇异点，这些空间通常采用过参数化的描述，如旋转矩阵或四元数。

  • 直接将公式(15) 应用到过参数化的表示会破坏由过参数化引起的约束，可以用最小化的旋转表示（如3维中的欧拉角），但是这样会有奇异点（万向锁）。

  • 另一个方法就是计算一种新的误差函数(扰动模型)，$\mathbf{\Delta }{\mathbf{x}}_i$ 是当前状态变量$\breve{\mathbf{X}}$ 周围的一个扰动，表示一个细小的旋转，${\mathbf{x}}_i$ 使用过参数化的表示。优化后的变量${\mathbf{x}}^{\ast }$ 通过非线性运算符$⊞$ 的增量求得：

  • $${\mathbf{x}}_i^{\ast }={\breve{\mathbf{x}}}_i⊞\mathbf{\Delta }{\mathbf{x}}_i^{\ast }.$$

  • 在三维SLAM中可用平移向量和归一化四元数的轴来表示增量$\mathbf{\Delta }{\mathbf{x}}_i$ ，位姿${\mathbf{x}}_i$ 可用旋转向量和完整四元数表示。在将增量转换为与状态变量相同的表示之后，操作符$⊞$ 通过使用标准运动组合操作符$\oplus$ 将增量$\mathbf{\Delta }{\mathbf{x}}_i$ 应用到${\mathbf{x}}_i$​ .

  • $${\breve{\mathbf{x}}}_i⊞\mathbf{\Delta }{\mathbf{x}}_i^{\ast }\stackrel{\text{ def. }}{=}{\breve{\mathbf{x}}}_i\oplus \mathbf{\Delta }{\mathbf{x}}_i^{\ast }.$$

  • 定义一个新的误差函数：

  • $$\begin{array}{rl}{\mathbf{e}}_{ij}\left(\mathbf{\Delta }{\mathbf{x}}_i,\mathbf{\Delta }{\mathbf{x}}_j\right)\stackrel{\text{ def. }}{=}& {\mathbf{e}}_{ij}\left({\breve{\mathbf{x}}}_i⊞\mathbf{\Delta }{\mathbf{x}}_i,{\breve{\mathbf{x}}}_j⊞\mathbf{\Delta }{\mathbf{x}}_j\right)\\ =& {\mathbf{e}}_{ij}(\breve{\mathbf{x}}⊞\mathbf{\Delta }\mathbf{x})\simeq {\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\mathbf{\Delta }\mathbf{x}.\end{array}$$

  • $\breve{\mathbf{X}}$ 张成原始的过参数化空间，雅克比${\mathbf{J}}_{ij}$​等于：

  • $${\mathbf{J}}_{ij}={\frac{\partial {\mathbf{e}}_{ij}(\breve{\mathbf{x}}⊞\mathbf{\Delta }\mathbf{x})}{\partial \mathbf{\Delta }\mathbf{x}}|}_{\Delta \mathbf{x}=0}.$$

  • 增量 $\Delta {\tilde{\mathrm{x}}}^{\ast }$ 是在初始猜想$\breve{\mathbf{X}}$ 的局部欧式空间中计算，故需要由$⊞$ 算子重新映射到冗余空间中。

  • g2o框架允许为增量和状态变量定义不同的空间，支持同一个问题中任意参数。无论参数化的选择如何，Hessian 矩阵$\mathbf{H}$ 的结构一般是不变的。

• 线性系统结构

  • 根据公式(13), $\mathbf{H}$矩阵和向量$\mathbf{b}$由一组矩阵和向量求和得到，对每个约束，设${\mathbf{b}}_{ij}={\mathbf{J}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}$ 和${\mathbf{H}}_{ij}={\mathbf{J}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{J}}_{ij}$ (公式9中也有这样的表达)，然后重写$\mathbf{b}$和$\mathbf{H}$​：

  • $$\mathbf{b}=\sum _{⟨i,j⟩\in \mathcal{C}}{\mathbf{b}}_{ij}\mathbf{H}=\sum _{⟨i,j⟩\in \mathcal{C}}{\mathbf{H}}_{ij}.$$

  • 每个约束都对系统有一个附加值的贡献，该值只取决于误差函数的雅克比，由于约束的误差函数只依赖两个节点的值，公式(5) 的雅克比有如下形式：

  • $${\mathbf{J}}_{ij}=(0\cdots 0\underset{i}{{\mathbf{A}}_{ij}}0\cdots 0\underset{j}{{\mathbf{B}}_{ij}}0\cdots 0).$$

  • ${\mathbf{A}}_{ij}$和${\mathbf{B}}_{ij}$分别是误差函数对$\mathbf{\Delta }{\mathbf{x}}_i$ 和$\mathbf{\Delta }{\mathbf{x}}_j$ 的导数，由(9)得到下边的${\mathbf{H}}_{ij}$​​矩阵块结构：${\mathbf{H}}_{ij}=\left(\begin{array}{cccc}\ddots & & & \\ {\mathbf{A}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{A}}_{ij}& \cdots & {\mathbf{A}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{B}}_{ij}& \\ ⋮& & ⋮& \\ {\mathbf{B}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{A}}_{ij}& \cdots & {\mathbf{B}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{B}}_{ij}& \\ & & & \ddots \end{array}\right){\mathbf{b}}_{ij}=\left(\begin{array}{c}⋮\\ {\mathbf{A}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}\\ ⋮\\ {\mathbf{B}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}\\ ⋮\end{array}\right).$

  • 注：${\mathbf{\Omega }}_{ij}$是高维空间上的流型吗？在这里当作一个常数来处理.本质为信息矩阵(有确定的望告知)

  • 为简单表示，省略了0块，$\mathbf{H}$矩阵的块结构是图的邻接矩阵(存放边的数据) ，因此它的非零块的数量与图中的边的数量成正比，通常会得到稀疏的$\mathbf{H}$矩阵。在g2o中利用$\mathbf{H}$的这一特性，采用最先进的方法求解(14)的线性系统。

• 特殊结构系统

  • 某些问题(如BA) 中会导致$\mathbf{H}$矩阵具有某些特殊结构，本系统可以利用这些特殊结构来提高性能。在BA中一般由两种变量：相机的位姿P和相机观测到的地标点的位姿 I ，通过对(14)中的变量重新排序，得到下列系统：

  • $$\left(\begin{array}{cc}{\mathbf{H}}_{\mathrm{p}\mathrm{p}}& {\mathbf{H}}_{\mathrm{p}\mathrm{l}}\\ {\mathbf{H}}_{\mathrm{p}\mathrm{l}}^{\top }& {\mathbf{H}}_{\mathrm{l}\mathrm{l}}\end{array}\right)\left(\begin{array}{l}\Delta {\mathbf{x}}_{\mathrm{p}}^{\ast }\\ \Delta {\mathbf{x}}_{\mathbf{l}}^{\ast }\end{array}\right)=\left(\begin{array}{l}-{\mathbf{b}}_{\mathrm{p}}\\ -{\mathbf{b}}_{\mathbf{l}}\end{array}\right)$$

  • 通过取$\mathbf{H}$​矩阵的Schur补得到一个等价的约化系统 [7]

  • $$\left({\mathbf{H}}_{\mathrm{p}\mathrm{p}}-{\mathbf{H}}_{\mathrm{p}\mathrm{l}}{\mathbf{H}}_{\mathrm{l}\mathrm{l}}^{-1}{\mathbf{H}}_{\mathrm{p}\mathrm{l}}^{\top }\right)\Delta {\mathrm{x}}_{\mathrm{p}}^{\ast }=-{\mathbf{b}}_{\mathrm{p}}+{\mathbf{H}}_{\mathrm{p}\mathrm{l}}{\mathbf{H}}_{1l}^{-1}{\mathbf{b}}_{\mathbf{l}}.$$

  • ${\mathbf{H}}_{11}$是一个块对角阵，${\mathbf{H}}_{11}^{-1}$很好求解，求解(25) 我们可以得到相机的增量$\Delta {\mathrm{x}}_{\mathrm{p}}^{\ast }$ ，然后利用这个来解

  • $${\mathbf{H}}_{11}\Delta {\mathbf{x}}_{\mathbf{l}}^{\ast }=-{\mathbf{b}}_{\mathbf{l}}-{\mathbf{H}}_{\mathbf{p}\mathbf{l}}^{\top }\mathbf{\Delta }{\mathbf{x}}_{\mathbf{p}}^{\ast }$$

  • 利用$\Delta {\mathrm{x}}_1^{\ast }$ 来调整观测到的世界特征。通常，世界特征的数量大于相机位姿数量，因此用(25) 的求解速度要快于(14). 尽管(25) 需要计算左边的矩阵需要额外的时间。

IMPLEMENTATION

• 通过实现途中顶点和边的抽象基类来实现目标，连个基类都提供了一组方便子类化的虚函数，大多数内部操作都使用模板参数来实现。运用了Eigen库。

• 系统框架，只由灰色方框需要自己定义来解决新的优化问题[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GKC6qto5-1648560327984)(g2o_a_general_framework_for_graph_optimaization.assets/image-20220329153511062.png)]

• 使用所提供的的基类，派生新的节点类型只需要定义 $⊞$ 算子应用与增量。

• 连接两个顶点${\mathbf{x}}_i$ 和${\mathbf{x}}_j$ 的边需要定义误差函数${\mathbf{e}}_{ij}(\cdot )$. 用数值计算雅克比矩阵${\mathbf{J}}_{ij}$ ，或者用户可以通过重写虚基类显示地指定${\mathbf{J}}_{ij}$ ，因此只需要编写几行代码就可以实现解决新优化问题或比较不同参数化的新类型。

• $\mathbf{H}$矩阵的计算可以使用动态尺寸矩阵(一般情况下)，也可使用固定尺寸的矩阵(已知变量 ${\mathbf{x}}_i$的维度)。利用已知的先验维度可以实现编译时的优化，如展开循环来执行矩阵乘法。

• 在实现(25)中舒尔补简化所需的矩阵乘法时，利用底层图的稀疏结构，只有乘非零项才会形成额外的${\mathbf{H}}_{PP}$矩阵。对底层矩阵块结构进行操作，相比于标量矩阵乘法更加高效的矩阵乘法。

• 该框架时未知的嵌入式线性求解器，可适用于不同问题。

• 实验中作者使用了两个求解器

  • 1、由于$\mathbf{H}$是半正定对称矩阵，稀疏Cholesky分解得到了一个有效的求解器。最小二乘迭代期间的非零模式是恒定的，因此能够复用第一次迭代中计算的符号分解，从而减少填充，并减少后继迭代中的总体计算时间。
  • 注意：这种Cholesky分解没有利用参数的块结构。
  • 2、第二种方法是带有块Jacobi预调节器的预条件共轭梯度(PCG)，由于PCG本身就是一种迭代方法，求解一个$n\times n$矩阵的线性系统需要$n$ 次迭代，使用PCG进行$n$次迭代通常比Cholesky分解慢，所以基于PCG平方残差的相对减少来限制迭代次数。

EXPERIMENTS

使用真实数据集和人工数据集比较g2o与其他先进算法。然后介绍了实现数据集、设备相关信息。

A. Runtime Comparison

与其他算法运行时间比较。

• 虽然g2o专注于批处理优化，但它可以通过向图中添加节点后进行优化来增量地处理数据。效率性能类似与为增量使用而设计的方法，可以作为更复杂在线系统的组件。
• g2o可以数值计算雅克比矩阵${\mathbf{J}}_{ij}$，可以快速建立一个新的优化问题或一个新的误差函数的原型。

测试不同的参数化

• 该框架只需要实现误差函数和更新步骤，能够轻松地比较不同的参数化。为此实现了BA中表示姿态的两个不同的参数化。

  1. 增量$\Delta x_i$ 由平移向量和单位四元数的轴表示。
  2. 增量$\Delta x_i$ 由李代数$se(3)$表示
  • 两个参数化都收敛到同一个解，但使用李代数$se(3)$ 更快。

线性求解器的比较

• 本系统可以使用不同的线性求解器来求解(14)或(25) ,作者已经实现了两个基于Cholesky分解的求解器CHOLMOD和CSpare. 还利用block-Jacobi预调节器将PCG实现为一种迭代方法。
• PCG 的收敛取决于初始猜想与最优解的接近程度。

利用有关结构的知识

• 如前文所述，某些问题尤其特有的结构。使用这种结构可能会导致线性系统的求解获得实质性的改进。基于地标的SLAM和BA有相同的线性结构：地标/ 点只能与机器人姿态/ 摄像机连接，因此 Hessian${\mathbf{H}}_{ll}$ 的地标点部分具有块对角结构。
• 评估了使用特定分解对基于地标的SLAM和BA的优势。
  • 当地标的数量超多姿态的数量时，预分解的结果会显著加快(BA中典型情况) ，当姿态数量占主导地位时，效率并不是很高。

CONCLUSIONS

• 使用g2o只需定义误差函数和对当前解施加扰动的过程。还可以很容易地在系统中嵌入新的线性求解器来验证特定求解器的特性，适用于广泛的共享该图结构的问题。