论文A Tutorial on Graph-Based SLAM 学习笔记

整理一下SLAM的内容：
20200514
论文地址：http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.232.7301&rep=rep1&type=pdf

2021年9月3日 再次重新整理文章

Graph-based SLAM（基于图优化的算法)

A Tutorial on Graph-Based SLAM
这个论文写的很好。

看了：https://blog.csdn.net/plateros/article/details/103498039 的文章，写的很好。稍微总结一下：
这个文章把主要的point列了出来：https://blog.csdn.net/cyjwcbe/article/details/86666239

1、用图G（V, E）的方式表述了SLAM问题，

• 图优化中，顶点是优化项，而边是约束项。
• Pose(x)与Landmark(m)构成了图的顶点集。
• 边(Edge)存在两种构成。第一种是Pose到Pose，另一种是Pose到Landmark。
• 状态转移：$g(control,pose)$ ，即$g(u_1,x_0)$, 该值与“真实”的Pose $x_1$的差，构成了这条边的约束。
• 约束写成马氏距离：
• 完整的约束为前两个约束的和，（SLAM的图优化问题就是寻找合适的Pose(x), Landmark(m)，让如下的公式最小）

2、信息矩阵

分解上面的信息矩阵，消去了landmark节点(m)

3、 节点定义为机器人的Pose

• 令 节 点 x = { x 1 , . . . , x n } 表 示 节 点 向 量 。 令节点x=\left \{ x_{1},...,x_{n} \right \}表示节点向量。 令节点x={x1​,...,xn​}表示节点向量。

4、边可以统称为一种"虚拟观测"(virtural measurement)
边有两种，

• 时间上相邻的节点存在一条边$e_{(i,i+1)}$。这条边通常是运动模型提供的约束（如里程计，轮速仪等）。

• 如果在$x_i和x_j$看到了同样的路标，那么在这两个节点之间存在一条边$e_{(i,j)}$。这条边的约束由对路标的观测模型提供。

• $z_{(i,j)}$是虚拟观测的均值，

• 方差由信息矩阵${\Omega }_{i,j}$表示。

• ${\hat{z}}_{i,j}(x_i,x_j)$是已知$x_i和x_j$，对观测的预测。

5、$l_{(i,j)}是z_{(i,j)}$似然函数的对数。
可由预测的观测与观测的实际值的差求出

简化这个公式

问题的求解变成了寻找合适的节点向量x^{*}以使得F(x)得值最小。非线性最优化可采用最小二乘，牛顿高斯，Levenberg-Marquardt法。

论文公式推到20210903

1. 作者首先说明了SLAM属于概率问题，然后观测z(t)是多模的，因此高斯假设不成立

其中，单个观察 z(t) :

• 可能具有多条潜在的edge，他们连接graph中不同pose
• 并且graph连通性本身需要被描述为概率分布。

两个节点之间的edge被标记为两个姿势的相对位置的概率分布，条件是它们的相互测量。

2.作者说我们明白了上述道理后，我们接下来要做“数据关联”，这个属于概率问题

我们需要将地图变成node和edge的拓扑结构。因此，需要确定不同node之间的edge约束。

作者说，这个属于数据关联，通常由 SLAM 前端解决。为了计算正确的数据关联，前端通常需要对机器人轨迹 $p(x_{1:T}|z_{1:T},u_{1:T})$上的条件先验进行一致估计。

这需要在机器人探索环境时交错执行前端和后端。

3. 作者说数据关联不是这个论文的重点，建议大家自己解决

我们不会描述解决数据关联问题的复杂方法。这些方法通过谱聚类 [27]、联合兼容性分支和边界 [23] 或回溯 [13] 来处理关联。我们宁愿假设给定的前端提供一致的估计。
Such methods tackle association by means of spectral clustering [27], joint compatibility branch and bound [23], or backtracking [13].

[27] E. Olson, J. Leonard, and S. Teller. Fast iterative optimization of pose graphs with poor initial estimates. In Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), pages 2262–2269, 2006.

[23] J. Neira and J.D. Tardós. Data association in stochastic mapping using the joint compatibility test. IEEE Transactions on Robotics and Automation, 17(6):890–897, 2001.

[13] D. Hähnel, W. Burgard, B. Wegbreit, and S. Thrun. Towards lazy data association in slam. In Proc. of the Int. Symposium of Robotics Research (ISRR), pages 421–431, Siena, Italy, 2003

这些论文可以再去读读，把前端给弄了。

4. 作者说，graph建图算法就是去计算机器人轨迹上后验posterior的高斯近似（如果噪声是高斯的）

计算这个高斯的均值作为 最大值 maximizes the likelihood of the observations.

在下文中，我们将找到该最大值的任务描述为约束优化问题。

找到这个高斯均值（最大似然），就获得高斯的信息矩阵

5. 建立方程表述

1、令 $x=(x_1,...,x_T{)}^T$ 为参数向量，其中$x_i$描述节点 $i$ 的姿态。

2、设 z ij 和 Ω ij 分别是节点 i 和节点 j 之间虚拟测量的均值和信息矩阵。这样就方便写马氏距离$(x-z{)}^T\Omega (x-z)$.。

3、设 $\widehat{z_{ij}}(x_i,x_j)$ 是给定节点 i 和 j 的虚拟测量的预测值。通常这种预测是两个节点之间的相对变换

测量值 $z_{ij}$ 的对数似然 $l_{ij}$ 为 (马氏距离)

5. 作者引入了误差函数来对上述的对数似然进行简化

设 $e(xi,xj,zij)$ 是一个函数，它计算预期观测值 $\hat{z}$ 与机器人收集的真实观测值 $z$ 之间的差值。

6.作者说，全部有约束的点之间都要计算误差，还要把误差加起来，对数似然最大似然的最大值，就是负对数似然的最小值

令 C 为存在约束（观察）z 的索引对的集合。 最大似然方法的目标是找到使所有观测值的负对数似然 F(x) 的最小值

问题就是求解最小是负对数似然

公式5：

这就是 计算机器人轨迹上后验的高斯近似。

7. 作者说在本节的其余部分，我们将描述一种解决方程的方法。

所提出的方法使用标准优化方法，如高斯-牛顿或 Levenberg-Marquardt 算法，它特别有效，因为它有效地利用了问题的结构

我们首先描述了传统非线性最小二乘优化。
随后，我们介绍一种允许处理奇点的解决方法以优雅的方式表示机器人姿势。

A. 通过迭代局部线性化的误差最小化

如果已知机器人位姿的一个好的初始猜测$\hat{x}$，则方程5的数值解 可以通过使用
流行的 Gauss-Newton 或 Levenberg-Marquardt 算法。

这个想法是通过其围绕当前初始$\hat{x}$猜测的一阶泰勒展开来近似误差函数

泰勒展开了，就一句话，用多项式函数去逼近一个光滑的损失函数。

我们的损失函数 ,对其进行一阶泰勒展开

推到过程：
其中$J$是雅可比矩阵。



方程14中，我们设：

把常数项c去掉，所以可以得到：

作者解释矩阵 H 是系统的信息矩阵，因为

它是通过雅可比矩阵在轨迹空间中投影测量误差而获得的。

它在构造上是稀疏的，在通过约束连接的姿势之间具有非零值。 它的非零块数是约束数加上节点数的两倍。 这允许解决方程(15) 通过稀疏 Cholesky 分解。 可以在库 CSparse [4] 中找到稀疏 Cholesky 分解的高效而紧凑的实现。

然后通过将计算增量添加到初始猜测来获得线性化解

Gauss-Newton 算法迭代：

• 方程 (14) 中的线性化、
• 方程 (15) 中的解
• 以及方程 (16) 中的更新步骤。

在每次迭代中，前一个解被用作线性化点和初始猜测。

作者说，这是假设在参数 x 的空间是欧几里德空间，但是SLAM可能在非欧空间，可能导致次优解

上面描述的过程是多元函数最小化的一般方法，这里是针对 SLAM 问题的特殊情况导出的。

欧几里德空间(Euclidean Space)，一句话总结：欧几里得空间就是在对现实空间的规则抽象和推广（从n<=3推广到有限n维空间）。
欧几里得几何就是中学学的平面几何、立体几何，在欧几里得几何中，平行线任何位置的间距相等。
而中学学的几何空间一般是2维，3维（所以，我们讨论余弦值、点间的距离、内积都是在低纬空间总结的），如果将这些低维空间所总结的规律推广到有限的n维空间，那这些符合定义的空间则被统称为欧几里得空间（欧式空间，Euclidean Space）。
而欧几里得空间主要是定义了内积、距离、角（没错，就是初中的那些定义），理解了这些再去理解数学定义就很明确了

关于非欧式空间：

非欧几何，爱因斯坦曾经形象地说明过：假定存在一种二维扁平智能生物，但它们不是生活在绝对的平面上，而是生活在一个球面上，那么，当它们在小范围研究圆周率的时候，会跟我们一样发现圆周率是3.14159……可是，如果它们画一个很大的圆，去测量圆的周长和半径，就会发现周长小于2πr，圆越大，周长比2πr小得越多，为了能够适用于大范围的研究，它们就必须修正它们的几何方法。如果空间有四维，而我们生活在三维空间中，而这个三维空间在空间的第四个维度中发生了弯曲，我们的几何就会象那个球面上的扁平智能生物一样，感受不到第四维的存在，但我们的几何必须进行修正，这就是非欧几何。在非欧几何中，平行的直线只在局部平行，就象地球的经线只在赤道上平行。闵可夫斯基空间属于欧几里得几何的扩展，它是把时间也作为一个维度进行量化，再添加光速系数，跟洛伦兹变换一样，使得不同惯性系中的运动问题计算得以简化。

B. 关于线性化系统结构的考虑

作者分析了结构，认为雅可比矩阵比较稀疏

这里A ij 和B ij 是误差函数关于x i 和x j 的导数。

• 首先认为，矩阵 H 和向量 b 是通过对一组矩阵和向量求和获得的

• 该加数的结构取决于误差函数的雅可比矩阵。

• 由于约束的误差函数仅取决于两个节点的值，因此方程（7）中的雅可比矩阵稀疏的很，就像公式17一样

作者也写出了矩阵 H ij 的结构

为简单起见，我们省略了零块。

作者应该是把A高斯牛顿的算法写出来给大家看，说了一种稀疏性的节约内存的想法

算法 1 总结了迭代高斯-牛顿过程，以确定机器人姿态上后验的均值和信息矩阵。由于系统中的大多数结构都是稀疏的，我们建议使用内存高效的表示来存储系统的 Hessian H。

• 由于 Hessian 的结构是从图的连通性预先知道的，我们建议在迭代开始时预先分配 Hessian 一次，并在需要新的线性化时通过循环所有边来更新它。每条边对系数向量的块H [ii] 、H [ij] 、H [ji] 和H [jj] 以及块b [i] 和b [j] 有贡献。

• 另一个优化是仅计算 H 的上三角部分，因为它是对称的。

作者还补充了一点，如果约束太少，导致误差方程无法求解，应该添加0来增加方程的一般性。

请注意，约束 e ij 的误差仅取决于连接姿势 x i 和 x j 的相对位置。因此，在所有姿态的刚性变换下，姿态x的特定配置的误差F(x)是不变的。这导致 Eq15 未确定。

因此，为了对该系统进行数值求解，通常的做法是将增量 $\Delta x_k$ 之一约束为零。这可以通过将单位矩阵添加到第 k 个对角块 H[kk] 来完成。

在不失算法 1 中的一般性的情况下，我们固定第一个节点 $x_1$ 。修复姿势图的特定节点的另一种方法是抑制方程 15 中线性系统的第 k 个块行和第 k 个块列

C. 流形上的最小二乘法

作者开始讲非欧式空间了，我还不是很懂，说用流形来求解，大概会设计李群与李代数吧

数值处理非欧空间的一种常用方法是对流形进行优化。

流形是一个数学空间，在全局尺度上不一定是欧几里得，但在局部尺度上可以看作是欧几里得[20]。 请注意，此处描述的基于流形的方法类似于 Taylor 和 Kriegman [30] 描述的在 SO(3) 中最小化函数的方法。

作者提到了singularities 奇异点

这个联想到动态欧拉角的万向锁问题。

欧拉角的一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock x ）：在俯仰角为±90 ◦ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要我们想用三个实数来表达三维旋转时，都会不可避免地碰到奇异性问题。

由于这种原理，欧拉角不适于插值和迭代，往往只用于人机交互中。

万向锁（Gimbal lock）： 一旦选择±90°作为pitch角，就会导致第一次旋转和第三次旋转等价，整个旋转表示系统被限制在只能绕竖直轴旋转，丢失了一个表示维度。对于万向锁的问题，只有在动态欧拉角中出现，而静态欧拉角不存在万向锁的问题！！！

• 静态： 即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态，此时各个变换顺序的旋转矩阵是左乘的
• 动态： 即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态，此时各个变换顺序的旋转矩阵是右乘的
• 内旋Intrinsic rotations：绕运动轴
• 外旋 Extrinsic rotations： 绕固定轴

作者说，为了避免奇异点，采用过度参数化的方式描述，导致不好求解优化方程

简单地讲，就是旋转或者旋转平移矩阵对加法不封闭（两个旋转矩阵相加不是旋转矩阵，两个旋转平移矩阵相加不是旋转平移矩阵）

在 SLAM 问题的上下文中，每个参数块 x i 由平移向量 t i 和旋转分量 α i 组成。平移 t i 显然形成了欧几里得空间，而旋转分量 α i 跨越非欧几里得 2D 或 3D 旋转群 SO(2) 或 SO(3)。

为了避免奇点，这些空间通常以过度参数化的方式描述，例如，通过旋转矩阵或四元数。将方程（16）直接应用于这些过度参数化的表示打破了由过度参数化引起的约束。

过度参数化会导致额外的自由度，从而在解决方案中引入错误。

为了方便优化损失函数，采用最小旋转表示（3D中欧拉角），有奇异点问题，需要引入流形来解决

为了克服这个问题，可以使用最小的旋转表示（例如，3D 中的欧拉角）。然而，这受制于奇点。 2D 情况下的奇点可以通过对角度进行归一化来轻松恢复，但是在 3D 中，此过程并不简单。

另一种想法是将底层空间视为一个流形，并定义一个算子⊞，它将欧几里德空间中的局部变化 ∆x 映射到流形上的一个变化，∆x → x⊞∆x。

作者说，二维情况下的奇点可以通过对角度进行归一化来轻松恢复，然而，在 3D 中，这个过程并不简单。所以用流形

另一种想法是将底层空间视为一个流形并定义一个运算符 ⊞ 来映射一个局部
欧几里得空间中的变化 ∆x 到流形，∆x → x⊞∆x。我们向读者推荐
Hertzberg [14] 了解数学细节。

然后用流形去处理$H∆x=-b$。

流形问题的 H 不再表示轨迹的信息矩阵

流形问题的 H 不再表示轨迹的信息矩阵，而是轨迹增量 Δ~ x 的信息矩阵。 为了获得轨迹的信息矩阵， 在姿势 x 的原始空间中计算 H。

这里的处理在ceres中具有自动求导的库中可以实现。

Define a operator map 3D transformation to manifold

• 平移变量t显然是欧式空间，
• 而旋转变量的表达式（通常是欧拉角）属于非欧式空间。

简单地讲，就是旋转或者旋转平移矩阵对加法不封闭（两个旋转矩阵相加不是旋转矩阵，两个旋转平移矩阵相加不是旋转平移矩阵）

• 牛顿高斯的最优求解遇到了问题

• 可以使用四元组替代欧拉角表示3维旋转。

• 借用流形的概念(Manifolds），再结合四元组的表达

  • 例如，当计算两个点在流形（非欧式空间）上的距离时，首先在其中一个点的附近建立欧式空间的映射，然后把另一个点映射过去，再计算他们的距离。只需要在原算法中定义“加”，“减”两个操作（映射），将两个操作封装，而这两个封装可以使用四元组的对数实现。更详细的内容可以参考[4]。[5]给出了关于Smooth Manifold的详细说明。[4] A Framework for Sparse, Non-Linear Least Squares Problems on Manifolds [5] INTRODUCTION TO SMOOTH MANIFOLDS
  • 流形(Manifolds)是局部欧几里得空间化的一个拓扑空间。欧式空间就是最简单的流形。

接下来作者写了两个SLAM实践，我们一起来看看

该数据包括描述：连续时间帧之间移动的 2D 变换的里程计测量值，2D 激光数据。

通过以下方法进行图构建：

• 每当机器人移动超过 0.5 米或旋转超过 0.5 弧度时，该算法就会在图形中添加一个新顶点，并用当前的激光观察对其进行标记。

• 此激光扫描与先前获取的扫描相匹配，以改进里程计估计，并将相应的边添加到图形中。我们使用 Olson [26] 描述的扫描匹配器的变体。

[26] E. Olson. Real-time correlative scan matching. In Proc. of the IEEE
Int. Conf. on Robotics & Automation (ICRA), 2009.

• 当机器人在先前未知的区域中长时间行进后重新进入已知区域时，算法会寻找当前扫描与过去测量值的匹配（闭环）。如果当前观察结果与另一个节点的观察结果匹配成功，则算法向图中添加一条新边。

• 当机器人在先前未知的区域中长时间行进后重新进入已知区域时，算法会寻找当前扫描与过去测量值的匹配（闭环）。如果当前观察结果与另一个节点的观察结果匹配成功，则算法向图中添加一条新edge。edge用相对变换标记，使两次扫描重叠得最好。

  • 将当前测量值与所有先前的扫描相匹配将非常低效且容易出错，因为它不考虑关于机器人位置的已知先验。
  • 相反，作者建议的算法选择过去的候选节点作为其 3σ 边际协方差marginal covariances包含当前机器人姿势的节点。（换句话说，作者建议在3σ误差范围marginal covariances内寻找闭环约束），这些协方差marginal covariances可以作为简化的 Hessian $H_{red}$ 的逆的对角块获得。
  • $H_{red}$ 是通过移除新插入的机器人位姿的行和列从 H 中获得的。 $H_{red}$ 是假设当前位置固定时所有轨迹的信息矩阵。

• 每当检测到闭环时，该算法就会执行优化。

作者说，2D的 ，SE(2) 中的姿势不需要过度参数化，因此在这种情况下，利用流形没有优势。

作者的3D SLAM

将 SLAM 算法扩展到 3D 相当简单。 只需要替代二维扫描匹配和闭环检测

在我们的实施中，我们利用流行的 ICP 算法 [1] 并确定循环闭包我们使用 Steder 等人的算法 [29]。

[1] Paul J. Besl and Neil D. McKay. A method for registration of
3-d shapes. IEEE Transactions on Pattern Analysis and Machine
Intelligence, 14(2):239–256, 1992.

[29] B. Steder, G. Grisetti, and W. Burgard. Robust place recognition for 3D
range data based on point features. In Proc. of the IEEE Int. Conf. on
Robotics & Automation (ICRA), 2010

此外，图中的每个节点和每个约束都存在在 SE(3) 中

关于SE(3)的参数化表示问题：参数化的选择主要影响收敛速度

表示 SE(3) 元素所需的最小参数数为 6，可能的选择包括 3D 平移向量加上三个欧拉角。利用这种参数化导致算法 1 但是，这种最小表示受到奇点的影响，可以通过利用过度参数化的状态空间来避免。

或者，可以用最小表示描述优化问题 Δ~ x 的相对扰动，同时将姿态留在原始过度参数化的空间中。这导致了算法 2 在本节中，我们在模拟机器人获得的位姿图上比较了优化算法的这两种变体。请注意，Hessian 的稀疏模式在两种情况下都是相同的。此外，与求解线性系统的时间相比，计算线性系统的时间可以忽略不计。

因此，参数化的选择主要影响收敛速度，而不是执行一次迭代所需的时间。为了突出这种效果，我们使用两种算法展示了在一次优化运行期间每次迭代的误差演变。

球体表面上行走的机器人的模拟 3D 数据集

我们使用在球体表面上行走的机器人的模拟 3D 数据集。 测量受到显着误差的影响，使用里程计信息初始化系统导致图 11 左侧部分所示的图形。从这个初始猜测开始，我们执行了带有和不带有流形线性化的高斯牛顿算法， 即，这里使用欧拉角。

图 12 显示了两种方法迭代过程中误差的演变。 首先，两种方法都能够减少误差。 然而，不适当地考虑奇点会导致算法 1 发散，而算法 2 收敛到正确的解决方案

SLAM(1)概率模型与EKF

该博主写的很好：https://blog.csdn.net/plateros/article/details/103461233

需要注意：数据关联(data association)，此处需要额外的算法将预测的观测和传感器的观测建立对应关系。常见的算法有基于2D/3D点的NN, ICP算法，基于图像的特征匹配算法(Sift, Surf, ORB)等。

MonoSLAM

[3]MonoSLAM采单目摄像机，基于上文的EKF算法，实现了实时的SLAM。
[3] MonoSLAM: Real-Time Single Camera SLAM
https://github.com/hanmekim/SceneLib2

其中星标最多的是Hanme Kim的版本。

https://github.com/hanmekim/SceneLib2.git

由于MonoSLAM是一个典型的EKF算法

SLAM资料整理

这个写的不错：https://blog.csdn.net/hiddleson/article/details/25914569
可以：https://www.jianshu.com/p/43706a18ddc0

大杂烩：http://www.coin163.com/it/3768835166305779491/SLAM

g2o的实现：

文献资料：

Globally Consistent Range Scan Alignment for Environment Mapping.

Efficient Sparse Pose Adjustment for 2D Mapping.

A Tutorial on Graph-Based SLAM.：
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.232.7301&rep=rep1&type=pdf