g2o:一种通用的图优化框架（g2o a general framework for graph optimization）

个人学习笔记，翻译自原始论文
g2o： a general framework for graph optimization

摘要

在机器人和计算机视觉领域的许多流行问题如SLAM和BA，都可以转化为对于误差函数的最小二乘优化问题，其误差函数可以通过图这种数据结构进行表达。本文描述了面对该问题的一种通用的结构，并且提供了开源的c++库g2o用于优化基于图的非线性误差函数。我们的系统被设计成为能够轻易的扩展到其他问题上并且对于一个新的问题，仅仅需要自定义几行代码去描述该问题即可。当前的实现提供了几种不同的SLAM和BA问题的解决方案。我们也提供了大量在真实世界和仿真环境中的数据集用以评估算法性能。这些评估表明，即使g2o提供了一个通用的解决方案，其在特定问题上性能依旧可以和最先进的专用算法相提并论。

简介

大量的机器人学和计算机视觉问题都牵涉到最小化一个可以用图来表达的非线性误差函数，如SLAM和BA。总的来说，这些问题都希望尽可能的从一系列受到高斯白噪声影响测量数据中估计真实的状态参数。例如对于基于图的slam问题来说，其状态变量是机器人在环境中的位置和机器人能观测到的地标点的位置。因此一个传感器测量数据仅仅依赖于和它相关联的两个状态变量中的位置信息。里程计测量信息也仅仅和其前后两次的状态变量中的位置信息相关。同样，对于一个地图点的位置也仅仅和能够看到它的那几帧对应的相机位姿相关。

所有的这些问题都可以用图来表达。其中每一个节点都代表了一个待优化的变量，连接两个顶点的每一条边都代表顶点之间的一次观测。在文献中，对于这样一个经典的问题提出了许多的方法去解决，例如有高斯-牛顿法、L-M算法，或者梯度下降的方法也能够在大多数情况下解决该问题。然而需要实现这些算法并达到良好的效果需要大量的知识储备。

在本篇文章中，我们针对这种可以用图表达的非线性最小二乘问题提出了一种通用的解决方案。我们命名为g2o。图1中给出了能够采用g2o作为后端解决的不同问题的一个概述。该系统能够和最先进的专用系统的性能所媲美，并且可以用于一般的通用系统。我们通过如下的算法实现高效性：

• 利用了图连接的稀疏性
• 利用了在上述问题中图的特殊性
• 利用先进的方法求解稀疏的线性系统
• 利用现代处理器的特性如SIMD指令优化内存的使用。

除了高效性，g2o非常的通用且易于扩展：

一个2D SLAM算法可以用少于30行的c++代码描述。用户只需要制定误差函数和他的参数即可。

在这篇文章中，我们将g2o应用于不同类别的最小二乘问题的求解并和对应的专用求解算法进行对比。我们展示了大量的真实和仿真数据用以评估算法，并和对应领域最先进算法进行对比，在一些情况下甚至超过了专用算法的性能。

论文接下来的结构如下：我们首先讨论了特别针对SLAM问题或者BA问题的求解方法。其次在第三节我们描述了我们提出的系统是如何处理基于图的最小二乘问题，并讨论通过LM算法或者GN算法的实现。在第四节中，我们讨论了系统的特性，最后在第五节我们将g2o系统和最先进的专用的方法进行对比。

相关工作

略（参考意义不大）

使用最小二乘的非线性图优化

许多在机器人和计算机视觉中的问题可以通过求解如下函数的最小值来解决：

$$\mathbf{F}(\mathbf{x})=\sum _{⟨i,j⟩\in \mathcal{C}}\underset{{\mathbf{F}}_{ij}}{\underbrace{\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})$$

这里的$x=(x_1^T,\cdots ,x_n^T{)}^T$是一个参数向量，其中每一个$x_i$代表着一个通用的参数块，$z_{ij}$和${\Omega }_{ij}$代表观测的均值和$x_i$和$x_j$之间的信息矩阵。$e(x_i,x_j,z_{ij})$是一个向量误差函数描述了$x_i$和$x_j$符合约束条件$z_{ij}$的程度。当其为零时代表完美的符合了约束条件。

为了简化记号，在论文的接下来部分将使用如下的简化符号代表误差函数：

$$\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})$$

注意到每一个误差函数，每一个参数块都可以跨越不同的空间。一个这样的问题能够通过有向图结构高效的表达。图中一个节点代表着一个参数块$x_i$，节点$i$和$j$之间的一条边代表着在两个参数块之间的有序约束。图二中是对建图过程的图描述。

A.最小二乘优化

如果已知参数的一个良好的初始值$\breve{x}$,便可以用流行的LM算法或G-N算法对其进行数值求解。其主要的思想在于使用误差函数的在初始位置的泰勒展开来近似误差函数。
$$\begin{array}{rl}{\mathbf{e}}_{ij}\left({\breve{\mathbf{x}}}_i+\Delta {\mathbf{x}}_i,{\breve{\mathbf{x}}}_j+\Delta {\mathbf{x}}_j\right)& ={\mathbf{e}}_{ij}(\breve{\mathbf{x}}+\Delta \mathbf{x})\\ & \simeq {\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\Delta \mathbf{x}\end{array}$$

在这里，$J_{ij}$是对$e_{ij}(x)$计算得到的雅克比矩阵。
定义残差项如下：
$$\begin{array}{l}{\mathbf{F}}_{ij}(\breve{\mathbf{x}}+\Delta \mathbf{x})\\ ={\mathbf{e}}_{ij}(\breve{\mathbf{x}}+\Delta \mathbf{x}{)}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}(\breve{\mathbf{x}}+\Delta \mathbf{x})\\ \simeq {\left({\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\Delta \mathbf{x}\right)}^{\top }{\mathbf{\Omega }}_{ij}\left({\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\Delta \mathbf{x}\right)\\ =\underset{{\mathrm{c}}_{ij}}{\underbrace{{\mathbf{e}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}}}+2\underset{{\mathbf{b}}_{ij}}{\underbrace{{\mathbf{e}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{J}}_{ij}}}\mathbf{\Delta }\mathbf{x}+\mathbf{\Delta }{\mathbf{x}}^{\top }\underset{{\mathbf{H}}_{ij}}{\underbrace{{\mathbf{J}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{J}}_{ij}}}\mathbf{\Delta }\mathbf{x}\\ ={\mathbf{c}}_{ij}+2{\mathbf{b}}_{ij}\mathbf{\Delta }\mathbf{x}+\mathbf{\Delta }{\mathbf{x}}^{\top }{\mathbf{H}}_{ij}\mathbf{\Delta }\mathbf{x}\end{array}$$
这只是对其中一个约束的残差项，对图中所有约束的残差项需要将所有的残差求和。
$$\begin{array}{rl}\mathbf{F}(\breve{\mathbf{x}}+\Delta \mathbf{x})& =\sum _{⟨i,j⟩\in \mathcal{C}}{\mathbf{F}}_{ij}(\breve{\mathbf{x}}+\Delta \mathbf{x})\\ & \simeq \sum _{⟨i,j⟩\in \mathcal{C}}{\mathrm{c}}_{ij}+2{\mathbf{b}}_{ij}\Delta \mathbf{x}+\Delta {\mathbf{x}}^{\top }{\mathbf{H}}_{ij}\Delta \mathbf{x}\\ & =\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}$,$b=\sum b_{ij}$,$H=\sum H_{ij}$.因此可以通过求解如下线性方程得到使误差最小的$\Delta x$。
$$\mathbf{H}\mathbf{\Delta }{\mathbf{x}}^{\ast }=-\mathbf{b}$$

$\mathbf{H}$矩阵是系统的信息矩阵，在初始值的基础上加上求解的增量即可得到下一次的迭代初始值。
$${\mathbf{x}}^{\ast }=\breve{\mathbf{x}}+\Delta {\mathbf{x}}^{\ast }$$

G-N法就是通过不断在当前点求解$\Delta x^{\ast }$来迭代$x^{\ast }$从而收敛到局部最优值。

LM算法在G-N算法的基础上引入了一个阻尼系统用来控制算法的收敛速度。
$$(\mathbf{H}+\lambda \mathbf{I})\mathbf{\Delta }{\mathbf{x}}^{\ast }=-\mathbf{b}$$

这里的$\lambda$是一个阻尼系数，其中$\lambda$越大前进的距离$\Delta x$越小。这在强非线性表面的情况下能够能够有效的控制步长。（避免步长太长，线性化后的误差太大）LM算法的主要思想就是如何动态的根据需要调节阻尼系数。在每一次迭代过程中会监视线性化误差函数与真实误差函数的误差。如果当前迭代比上一次的误差要小，说明模型能够较好的近似，于是在下一次迭代中减小$\lambda$的值以便快速搜索。否则将会减小$\lambda$的值并带回原式重新计算。

B.可选的参数化

程序中系统希望在面对不同函数最小化问题时能够采用一种通用的方法。他们假设参数空间是欧几里德的，但是这并不满足SLAM或则BA等问题的求解。为了处理状态变量在非欧几里德空间中的情况，一种通用的做法是在不同的参数空间中表达不同的参数。

例如，在上文中提到的SLAM问题中，每一个参数块中包含平移向量$t_i$和旋转参数${\alpha }_i$。其中平移向量是在欧几里德空间中的，而旋转参数是在非欧几里德空间中，如$SO(3),SO(2)$空间中。为了避免奇异性，这些空间常常采用超参数的方法描述，如四元数或旋转矩阵。如果直接将超参数带入原算法中，会使得计算结果不满足超参数自带的约束条件。为了避免这个问题，可以采用旋转的最小表达如欧拉角，但是欧拉角又会有奇异点。

现在的一个思路是，在当前位置对参数$\breve{x}$进行一个扰动，来求得增量$\Delta x$。其增量$\Delta x$采用最小参数的表达方式，而当前的位置$\breve{x}$依旧采用超参数的方法描述。由于增量一般都很小，离奇异点非常的远，因此不用考虑奇异点的问题。新的状态参数$x^{\ast }$可以通过一种算子田 $:\mathrm{Dom}\left({\mathbf{x}}_i\right)\times \mathrm{Dom}\left(\mathbf{\Delta }{\mathbf{x}}_i\right)\to \mathrm{Dom}\left({\mathbf{x}}_i\right)$的非线性方式进行更新（就是SO(3)的左\右扰动求导方法）：
$${\mathbf{x}}_i^{\ast }={\breve{\mathbf{x}}}_i\text{ 田 }\mathbf{\Delta }{\mathbf{x}}_i^{\ast }$$

例如在3D SLAM中可以采用四元数来表达位姿，但是更新量却是通过角速度向量来表达。然后通过四元数的跟新规则跟新当前状态（复杂公式太难打了，参考李群、李代数中如何通过对旋转矩阵采用扰动的方法求导即可理解。就是解决非欧空间内参数不能直接加，一个旋转矩阵加上一个旋转矩阵肯定不是旋转矩阵，那么如何求对应的雅可比，如何对参数跟新的问题）

C.线性系统的结构

对于$\text{H}$矩阵和向量$\text{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}$，然后定义$\text{H}$矩阵和向量$\text{b}$如下：
$$\mathbf{b}=\sum _{⟨i,j⟩\in \mathcal{C}}{\mathbf{b}}_{ij}\mathbf{H}=\sum _{⟨i,j⟩\in \mathcal{C}}{\mathbf{H}}_{ij}$$
对于每一个约束的误差都会用加法的形式构成系统整体的约束。经过累加之后的矩阵或者向量的结构取决于误差函数的雅可比结构。因为每一个约束仅仅和它连接的两个节点有关系，因此雅可比的大部分地方都是零，只有相关的两个节点对应的地方才不为零，表现为如下的结构。
$${\mathbf{J}}_{ij}=(0\cdots 0\underset{i}{\underbrace{{\mathbf{A}}_{ij}}}0\cdots 0\underset{j}{\underbrace{{\mathbf{B}}_{ij}}}0\cdots 0)$$

这里的${\text{A}}_{ij}$和${\text{B}}_{ij}$分别代表误差函数$e_{ij}$分别对节点i和节点j进行求导结果，可以表达为$\Delta x_i$和$\Delta x_j$.然后根据雅可比只有两项非零的特点得到矩阵$H_{ij}$和向量$b_{ij}$的表达式。
$${\mathbf{H}}_{ij}=\left(\begin{array}{ccccc}\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)$$
除了矩阵中给出的四项和向量中给出的两项，其他都为零，对于单个约束来说是非常稀疏的结构。即使在累加后，H矩阵依旧是稀疏的，因此将会利用其稀疏特性进行简化计算。

D.稀疏矩阵的处理

对于特定的应用如BA问题等，H矩阵拥有一些更加特殊的结构性质，我们将利用这些矩阵结构特性提高算法的性能。对于BA问题，需要求解的状态参数分为两类相机的位姿p和地图点的位置l。将参数分为两个部分看，然后对求解方程进行分块
$$\left(\begin{array}{cc}{\mathbf{H}}_{\mathbf{p}\mathbf{p}}& {\mathbf{H}}_{\mathbf{p}\mathbf{l}}\\ {\mathbf{H}}_{\mathbf{p}\mathbf{l}}^{\top }& {\mathbf{H}}_{\mathrm{l}\mathrm{l}}\end{array}\right)\left(\begin{array}{l}\mathbf{\Delta }{\mathbf{x}}_{\mathbf{p}}^{\ast }\\ \mathbf{\Delta }{\mathbf{x}}_{\mathbf{l}}^{\ast }\end{array}\right)=\left(\begin{array}{l}-{\mathbf{b}}_{\mathbf{p}}\\ -{\mathbf{b}}_{\mathbf{l}}\end{array}\right)$$

对这样一个分块系统去舒克补的方式可以简化计算，通过舒克补操作得到两个方程

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

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

第一个方程可以求得${\mathbf{x}}_{\mathbf{p}}^{\ast }$,然后带入到第二个方程就可以求出${\mathbf{x}}_1^{\ast }$。这样求解无需对整个大矩阵进行求逆的操作，而是转换成对分块小矩阵求逆，在稀疏矩阵的条件下能极大的降低运算复杂度。

实现

有关实现问题在g2o源码中给出的一份文档介绍的更为详细

其他

g2o中为了提高对outlier的鲁棒性，添加了一个Huber鲁棒核函数，但是没有在论文里面提及。