【PL-SLAM】原理部分
1. Overview
A. map
B. Feature Tracking
C. Local Mapping
D. Loop Closure
2. Feature Tracking
A. Point Features
B. Line Segment Features（LSD线段特征+LBD描述子）
C. Motion Estimation
D. Keyframe Selection
3. Local mapping
A. Keyframe Insertion
B. Local Bundle Adjustment
4. Loop closure
A. Loop-Closure Detection
B. Loop Correction

1. Overview
PL-SLAM的原理如下图所示，主要包括三部分：视觉里程计VO，局部建图，闭环。


A. map
map 包括以下，

a set of KFs（关键帧）
the detected 3-D landmarks (both keypoints and line segments)
a covisibility graph
a spanning tree

其中，

关键帧包含了，观测到的立体特征和其描述子（利用字典计算出的左图描述子）、相机3D位姿信息。
路标包含了，一系列观测、每个被观测到的路标最具代表性的描述子、路标点的3D位置、路标线段的方向和其端点的坐标。
共视图建立了共视信息的模型，节点表示关键帧，边表示关键帧之间共视路标（文中设定共视路标的数目≥20）。如下图所示，用绿边表示的共视关系。
为了快速闭环优化，采用了本征图（essential graph），比共视图稀疏些，因其控制共视路标的数目≥100。
生成树表示了包含关键帧的图的最小连接表示。与本征图一起用于闭环。



B. Feature Tracking
注意，此处 feature 包括 point 和 line segment。
从一系列双目立体帧中跟踪特征，并计算它们的3D位置和其关联的协方差矩阵表示的不确定性。接着，3D路标被投影到新的相机位姿，并最小化投影误差以获取相机位姿增量和其估计的协方差。在每一帧重复这个过程，简单执行  frame-to-frame VO，直到新的关键帧插入地图map。
一旦关键帧插入地图，局部建图和闭环检测便同时进行（并行但不同线程）。
C. Local Mapping
局部建图，在共视图中，在新关键帧、上一关键帧和连接上一关键帧的其余关键帧之间，寻找新的特征对应关系。这样，构建了局部地图，其中包括与当前关键帧共视至少20个路标的所有关键帧，以及它们所观测到的所有路标。最后对局部地图执行优化。
D. Loop Closure
与局部建图并行，闭环检测采用基于BoW方法。在相机运动期间，捕获的 KFs 的所有视觉描述子都存储在数据库中，该数据库随之用于查找与当前帧相似的帧。仅当围绕此 KF 的局部序列也相似时，最佳匹配才被视为闭环候选。 最后，估算当前 KF 与闭环候选者之间的相对 *SE（3）*转换，以便在找到正确的估计的情况下，通过位姿图优化（PGO）过程校正闭环中涉及的所有 KFs 姿态 。
notice

这的局部建图和闭环，与ORB-SLAM中的方法一样，其目的是为了减小BA优化的计算量。

在BA优化时，采用近年流行的 relative techniques，而非消耗更大的 global approaches。
2. Feature Tracking
简而言之，沿着一连串的立体帧跟踪点和线段，然后通过最小化投影误差来计算相机的3D运动及其不确定性。 注意，此过程仅执行相机位姿的优化，而不优化被跟踪特征的3D位置，后者在后续的 LBA 过程中优化。
由于实验中使用的双目立体相机是经过预先标定的，因此可以通过直接使用其外部参数来确定观察到的关键点和直线的3D位置，从而在单个立体镜头中对这些特征进行初始化。
A. Point Features
采用ORB特征，关键点检测性能好，描述子匹配快速。为了减少匹配的离群点，我们仅考虑满足以下条件的测量：左图像中的最佳匹配对应于右图像中的最佳匹配，即它们是互为最佳的匹配。此外，还过滤掉出，在描述子空间中，那些 “ 第二最佳匹配的距离 ” 小于 “ 最佳匹配的距离 ” 的两倍的匹配，以确保对应关系足够有意义。（更鲁棒吧）
B. Line Segment Features（LSD线段特征+LBD描述子）
LSD方法已用于提取线段，从而提供了高精度和可重复性。对于立体匹配和帧到帧跟踪，使用 LBD 方法 （line band descriptor）提供的二进制描述子来增强线段，这能够根据线的局部外观找到线之间的对应关系。 与点的情况类似，我们检查两个候选特征是否互为最佳匹配，并且特征是否足够有意义。 最后，我们利用线段提供的有用的几何信息来滤除那些具有不同方向和长度的线匹配，以及那些在端点视差上有很大差异的线匹配。 注意，此过滤器有助于系统保留大量的结构线，从而允许基于点和线形成更一致的地图[见下图]。


C. Motion Estimation
将关键点和线段从第一帧反向投影到下一帧。 然后，采用鲁棒的高斯-牛顿法，最小化线和关键点投影误差来迭代估计相机的运动。为了处理离群值，我们采用了 Pseudo-Huberr 损失函数并执行两步最小化。最后，获得两个连续帧之间的增量运动估计，可以通过以下正态分布来建模：

$\boldsymbol{\xi}_{t, t+1} \sim \mathcal{N}\left(\boldsymbol{\xi}_{t, t+1}^{*}, \boldsymbol{\Sigma}_{\boldsymbol{\xi}_{t, t+1}^{*}}\right)（1）$

其中 $\boldsymbol{\xi}_{t, t+1}^{*} \in \mathfrak{s e}(3)$ 是在 $t$ 和 $t+1$之间的相机运动的6维向量 , 而 $\boldsymbol{\Sigma}_{\boldsymbol{\xi}_{t, t+1}^{*}}$ 估计的运动的协方差。
D. Keyframe Selection
为了确定何时在地图中插入新的KF，采用了 relative motion estimation 的不确定性。 因此，根据（1），通过以下表达式将不确定性从协方差矩阵转化为标量，称为熵：

$h(\boldsymbol{\xi})=3(1+\log (2 \pi))+0.5 \log \left(\left|\boldsymbol{\Sigma}_{\xi}\right|\right)（2）$

然后，对于给定的 KF i，我们检查 “ KF i与当前帧 i + u 之间的运动估计熵 ” 与 “ KF i与其首个连续帧 i + 1之间的熵 ” 之间的比率，即

$\alpha=\frac{h\left(\boldsymbol{\xi}_{i, i+u}\right)}{h\left(\boldsymbol{\xi}_{i, i+1}\right)}（3）$

如果 α 值低于某个预先确定的阈值（在实验中已将其设置为0.9），则将帧 i + u 作为新的 KF 插入到系统中。注意，要计算（2）中的表达式，需要非连续帧之间的位姿增量的不确定性。 由于（1）仅估计连续帧之间的增量运动，因此通过一阶高斯传播技术来组合一系列此类估计，以获得两个非连续 KF 之间的协方差。
3. Local mapping
本节描述了在插入新的 KF 时系统的行为，该行为主要包括执行局部地图的 BA 优化，该局部地图就是，“ 通过共视图与当前 KF 连接的那些 KFs ” + “ 由这些局部 KFs 所观察到的路标 ”。
A. Keyframe Insertion
每次 VO 选择一个KF，都会将其插入 SLAM 系统并优化局部地图。首先，我们优化当前和先前KF之间的相对位姿变化的估计，因为由 VO 提供的位姿是通过合成中间帧之间的相对运动来估计的。为此，我们考虑了第2节中描述的几何约束，并在 KFs 之间执行了数据关联，并获得了在其中观察到的一致的共同特征集。 然后，执行与第2-C节中介绍的优化类似的优化，为此，我们将VO 线程提供的位姿用作高斯-牛顿最小化的初始估计。计算完 KFs 之间的相对位姿变化后，将当前的 KF 插入系统，包括以下内容。

1）关键帧的索引。
2）其 3D 位姿的信息，包括绝对位姿、与前一个 KF 的相对位姿及其相关的不确定性。
3）新的 3D 路标，通过存储其 2D 图像坐标和其描述子进行初始化。已存在的路标的新观测值也将添加到地图中。

最后，还在当前帧的未匹配特征观测值与局部地图中的地标之间寻找新的对应关系。
B. Local Bundle Adjustment
插入 $\mathrm{KF}$ 之后, 接下来执行局部地图的 BA 优化。定义一个向量 $\psi$ ，其包含了待优化的变量， 它们是  每个关键帧 KF 的 $\operatorname{se}(3)$ 位姿$\boldsymbol{\xi}_{i w}$， 每个 $3\mathrm{D}$ 点的位置 $\mathbf{X}_{w j}，$ 和每个线段的端点的3D坐标: $\left\{\mathbf{P}_{w k}, \mathbf{Q}_{w k}\right\} .$ 然后，将观测值和投影到观测帧的路标之间的投影误差最小化

$\boldsymbol{\psi}^{*}=\underset{\boldsymbol{\psi}}{\operatorname{argmin}} \sum_{i \in \mathcal{K}_{l}}\left[\sum_{j \in \mathcal{P}_{l}} \mathbf{e}_{i j}^{\top} \boldsymbol{\Sigma}_{\mathbf{e}_{i j}}^{-1} \mathbf{e}_{i j}+\sum_{k \in \mathcal{L}_{l}} \mathbf{e}_{i k}^{\top} \boldsymbol{\Sigma}_{\mathbf{e}_{i k}}^{-1} \mathbf{e}_{i k}\right]（4）$
其中 $\mathcal{K}_{l}, \mathcal{P}_{l},$ 和 $\mathcal{L}_{l}$ 分别指的是局部 KFs 集合 ，点集, 和线段集。
在（4）中，点的重投影误差$\mathbf{e}_{i j}$，按如下计算，

$\mathbf{e}_{i j}=\mathbf{x}_{i j}-\boldsymbol{\pi}\left(\boldsymbol{\xi}_{i w}, \mathbf{X}_{w j}\right)（5）$

其中函数 $\boldsymbol{\pi}: \mathfrak{s e}(3) \times \mathbb{R}^{3} \mapsto \mathbb{R}^{2}$ 表示重投影，先将世界坐标系下第 $j$ 个 3D 点 $\mathbf{X}_{w j}$  放入第$i$ 个 $\mathrm{KF}$的局部参考坐标系下，即, $\mathbf{X}_{i j},$ 再把这个点投射到图像中。
关于线段，将3D线段的投影端点与其在图像平面中对应的无限线之间的距离作为误差函数。在第 i 帧观测到的第 k 条线段的重投影误差$\mathbf{e}_{i k}$，有

$\mathbf{e}_{i k}=\left[\begin{array}{l}
\mathbf{l}_{i k} \cdot \boldsymbol{\pi}\left(\boldsymbol{\xi}_{i w}, \mathbf{P}_{w k}\right) \\
\mathbf{l}_{i k} \cdot \boldsymbol{\pi}\left(\boldsymbol{\xi}_{i w}, \mathbf{Q}_{w k}\right)
\end{array}\right]（6）$

其中 $\mathbf{P}_{w k}$ 和 $\mathbf{Q}_{w k}$ 指的是世界坐标下线段端点的 3D  坐标，$\mathbf{l}_{i k}$是与第 i 个KF 中的第 k 个线段相对应的无穷线的方程式，可以用齐次坐标中的线段的二维端点之间的叉积获得，即：$\mathbf{l}_{i k}=\mathbf{p}_{i k} \times \mathbf{q}_{i k}$。
利用Levenberg-Marquardt优化方法可以迭代地解决（4）中的问题，为此我们需要估计Jacobian矩阵和Hessian矩阵，

$\Delta \boldsymbol{\psi}=[\mathbf{H}+\lambda \operatorname{diag}(\mathbf{H})]^{-1} \mathbf{J}^{\top} \mathbf{W} \mathbf{e}（7）$

其中误差向量$\mathbf{e}$包含了点和线段的重投影误差。（7）和更新步骤（8），

$\boldsymbol{\psi}^{\prime}=\boldsymbol{\psi} \oplus \Delta \boldsymbol{\psi}（8）$

可以递归应用，直到收敛为止，从而得到最佳$\boldsymbol{\psi}$，从中可以更新局部 KF 和路标的位置。 注意，鉴于$\boldsymbol{\psi}$ 中变量的不同性质，更新方程无法直接应用于整个矢量。
应强调的是，在（4）中，点和线中的误差影响分别由 $\boldsymbol{\Sigma}_{\mathbf{e}_{i j}}^{-1}$ 和 $\boldsymbol{\Sigma}_{\mathbf{e}_{i k}}^{-1}$ 加权，代表与每个投影误差不确定性相关的协方差矩阵的逆 。但实际上，将此类协方差设置为单位矩阵并遵循与2-C节中所述方法类似的方法更为有效，因为它引入了稳健的权重，并且还处理了离群值的存在。
最后，移除地图中观测少于三次的路标点。
4. Loop closure
采用基于所提取的关键点和线段的二进制描述子的 BoW 方法，以鲁棒地应对数据关联和闭环检测。
简而言之，BoW技术将从图像中提取的所有信息（关键点和线段的描述子）汇总到一个单词向量中，为此，它使用了从不同图像数据集离线构建的字典表。 然后，随着相机的移动，从捕捉的图像中计算出单词并存储在数据库中，从而随后将用于搜索与当前关键帧最相似的图像。
A. Loop-Closure Detection
闭环检测既要找到类似于当前正在处理的图像，又要估算它们之间的相对位姿变化，如下所述。
1）Visual Place Recognition：
针对点和线段，建立两个特定的字典和数据库。这样，在每个时间步，并行检索关键点和线段数据库中最相似的图像，以查找闭环。双重搜索的动机是，某些场景可能用线条而不是关键点来描述，反之亦然。 因此，同时使用这两种方法并合并它们的结果使我们能够精确数据库查询的输出，从而减少计算量。
为了说明这一点，首先定义一个相似度矩阵，该矩阵在每一行中都包含相似度值，其值的范围为[0，1]，于是计算相似度矩阵。
具体地，如下图4(a) 中的矩阵仅使用ORB关键点来计算，以建立字典和数据库，而图4（b）中的矩阵仅依赖于线段。调色板从蓝色（分数= 0）到红色（分数= 1）。注意到，根据关键点，图像看起来相似的地方，在第一个矩阵中会出现一些淡黄色区域。这表明存在潜在的闭环，但实际上它们只是假阳闭环。但是，第二个矩阵没有这种现象。另一方面，基于关键点的检测，由于不相似图像的分数差异通常更大，因此第一个矩阵具有更大的区别性。因此，当检测潜在的闭环时，可以将组合查询两个特征数据库而产生的图像相似度，以提高鲁棒性。


作者制定了一套点线特征的加权求和的方式（$s_{k}$表示关键点分数，$s_{l}$表示线段分数），来计算图像之间的相似度得分，根据的两个标准，分别是 strength 和 dispersion。 前者根据图像中检测到的一组特征中关键点或线条的特征数量，按比例对相似性评分加权，而后者则考虑特征在图像中的离散程度（ 分散度越高，权重越大）。得到鲁棒的相似度评分如下：

$\begin{aligned}
s_{t}=& 0.5\left(n_{k} /\left(n_{k}+n_{l}\right)+d_{k} /\left(d_{k}+d_{l}\right)\right) s_{k} \\
&+0.5\left(n_{l} /\left(n_{k}+n_{l}\right)+d_{l} /\left(d_{k}+d_{l}\right)\right) s_{l}
\end{aligned}（10）$

其中，$n_{k}$，$n_{l}$为提取的关键点和线段的数量，$d_{k}$，$d_{l}$表示离散值——分别计算检测到特征的 x 、y 坐标的方差之和的平方根。针对线段特征，x 和 y 为线段中点。
此公式给予两种特征都相同的重要性（因此采用0.5系数）。
将上述策略与其余四种方法作比较，采用四种数据集测试。策略3即对应（10），表现最好。


2）估计相对运动
一旦有了闭环候选，需要滤掉假阳的闭环。 恢复形成闭环的两个KF之间的相对姿态（我们把其中一个命名为当前帧，另一个为旧帧），来实现筛除假阳。首先，先在这两个关键帧寻找特征匹配，接着在当前帧与旧帧关联的局部地图之间寻找新对应关系。接着估计两个KF之间的刚性变换 $\hat{\boldsymbol{\xi}}_{i j} \in \mathfrak{s e}(3)$。最后，我们再检查闭环候选的一致性，检测方法如下：

协方差矩阵 $\Sigma_{\tilde{\xi}_{i j}}$ 的最大特征值小于0.01
获得的平移和旋转不能分别超过0.50米和3度。
估计计算时，内点所占比例大于50%;

关于第一个标准，不确定性矩阵的特征值的较大值（参见（1））通常指出了病态Hessian矩阵，这很可能是由于特征匹配集中存在大量离群值 。确保协方差矩阵的最大特征值低于某个阈值，能够检测到可能不正确的闭环候选并将其丢弃。
关于第二个标准，为估计的姿态设置了最大平移和旋转限制，因为基于BoW的方法通常会提供外观和位姿非常相似的匹配，因此若两帧之间的位姿有很大变化，通常为错误的闭环检测。
关于第三个标准，由于运动估计会受到离群值以及视觉场景识别的错误关联的强烈影响，因此在优化之后设置检测到的离群值的最小比率。
B. Loop Correction
在估计了轨迹中所有连续的闭环之后，融合闭环的两侧，校正沿环分布的误差。将问题作为PGO（位姿图优化？）来解决，其中节点为关键帧，边由本征图和生成树给定。定义以下误差函数为$\mathfrak{s e}(3)$差：（不太懂）

$\mathbf{r}_{i j}\left(\boldsymbol{\xi}_{i w}, \boldsymbol{\xi}_{j w}\right)=\log \left(\exp \left(\hat{\boldsymbol{\xi}}_{i j}\right) \cdot \exp \left(\boldsymbol{\xi}_{j w}\right) \cdot \exp \left(\boldsymbol{\xi}_{i w}\right)^{-1}\right)（11）$

其中，log 映射表示 $SE(3) \mapsto \mathfrak{s e}(3)$，exp 映射表示 $\mathfrak{s e}(3) \mapsto$ $S E(3)$。
用 g2o 库来解决这个G2O问题，从而生成关键帧的最优位姿。我们更新KFs的位姿，以及它们所观测到的地标的位姿，并且，还要将回环两边的局部地图进行融合，方法是首先融合在估计它们的相对位姿时所匹配的地标点，然后再在未匹配的地标之间寻找新的匹配关系。

参考

https://zhuanlan.zhihu.com/p/41371645

SLAM系统原理推导
1.SLAM 基础知识
1.1 SLAM 系统概率模型
1.2 里程计运动模型
1.3 激光雷达测距模型
1.4 系统观测模型
2 SLAM实例
2.1 粒子滤波算法原理

1.SLAM 基础知识
  SLAM 系统框架如图1 所示。包括两个主要传感器：激光雷达与轮式里程计。其中激光雷达用来获取环境信息，通过激光扫描可以测量周围 360°环境的障碍物信息。轮式里程计则通过移动底盘的电机编码器持续获取机器人的里程信息。
  激光雷达的作用主要有两个方面：一方面可以为构图算法提供点云数据。当构图算法获取足够的点云数据后，即可构建出以机器人为中心，以雷达射程为半径的局部地图。另一方面可以通过系统观测模型修正贝叶斯滤波器的预测位姿，提高滤波估计的机器人位姿精度。轮式里程计主要作用则是为 SLAM 系统提供里程信息。通过轮式里程计运动学模型，可以将里程信息转化为机器人位姿变化信息，送入贝叶斯滤波器初步计算出预测位姿。 预测精度完全依赖于轮式里程计提供的数据精度。而轮式里程计的测量精度对于获取精确的机器人位姿来讲远远不够。因此，需要用到系统观测模型对预测结果进行修正。经过修正并加权求和得到的机器人位姿即为当前估计的最佳位姿。在已知最佳位姿的条件下，局部地图即可更新至全局地图中。全局地图进一步进行高斯模糊，构建新的观测模型。至此整个 SLAM系统形成完整的闭环系统。


1.1 SLAM 系统概率模型
  在 SLAM 系统框架中， 采用贝叶斯滤波器为 SLAM 系统的后端优化算法提供概率模型。 贝叶斯滤波的原理是基于已经发生的事件对未发生事件进行估计，求解未发生事件的最可能概率分布。贝叶斯滤波的推导如图 2 所示。



  在贝叶斯滤波器构建的 SLAM 概率模型基础上， 发展出了两大分支： 基于卡尔曼滤波的 SLAM 算法， 主要代表有 EKF-SLAM， UKF-SLAM 等； 以及基于粒子滤波的 SLAM 算法， 主要代表有 RBPF-SLAM， FastSLAM 等。 其中基于卡尔曼滤波的 SLAM 算法要求噪声必须服从高斯分布且系统非线性不能太大；相反基于粒子滤波的 SLAM 算法适用于非线性较大的系统， 但受限于粒子退化与粒子耗散这一对耦合问题。
1.2 里程计运动模型
  机器人底盘分为两轮差速底盘和三轮全向底盘。两轮差速底盘其结构简单，造价较低，并且建立的模型相对简单，因此被应用于各种类型的机器人生产中。两轮差速底盘的运动学模型如图 3 所示。



该模型将机器人的任意移动视为某一圆心做圆弧运动，其中 d 为两轮距离的二分之一， r 为底盘中心做圆弧运动的半径。设左轮速度为 $v_{L}$，右轮速度为$v_{R}$, 底盘中心的角速度为 ω, 若已知 $v_{L}$,$v_{R}$,可以由式(1)计算出底盘运动中心的角速度 ω 和线速度 v。



  假设机器人在里程计数据更新的两时刻之间的极短时间 $\Delta t$内作匀速运动，则相邻两时刻之间机器人位移由式(2)给出。



式中： dx ,dy ,$d\theta$ 分别为机器人坐标系下沿 x 轴方向位移，沿 y 轴方向位移，以及以 z 轴为旋转轴的逆时针旋转角度。式(2)给出的是在机器人坐标系下的位移。机器人坐标系与世界坐标系的关系是由机器人位姿联系起来的，如图 2-4 所示。 图中$\left ( x,y,\theta  \right )$为机器人在世界坐标系下的位姿表示。



  可以通过机器人位姿 $\left ( x,y,\theta  \right )$将机器人在机器人坐标系的位移转换到世界坐标系中。最终计算的机器人运动模型由式(3)给出：


1.3 激光雷达测距模型
  激光雷达的测距模型主要有以下两种：三角测距与 TOF（ Time of Flight）测距。其中基于三角测距模型的激光雷达实物图如图 5 a 所示。图中左边小孔为激光发射器，右边稍大的孔为接受激光的光学摄像头。 三角测距模型主要原理为三角形的角边角定理，激光发射器位置与摄像头光心以及扫描点构成的三角形如图 5 b 所示。



  图 5b 中 $O_{1}$点为激光发射器， $O_{2}$点为摄像头光心轴，光心轴旁边的三角形代表用来捕捉反射光斑的相机模型。激光发射器发射的激光由红色虚线表示，相机接收的反射激光由蓝色虚线表示。 A 点表示激光击中的障碍物位置,障碍物A 在成像平面上的位置用 $A_{1}$ 表示。其中线段$O_{1}$ $O_{2}$长度已知；由于激光垂直于直线 $O_{1}$ $O_{2}$ 发射，$\angle AO_{1}O_{2}$ 为 90°；$\angle O_{1}O_{2}A$  可以通过成像点 $A_{1}$ 的位置获取。在三角形 $O_{1}O_{2}A$中已知两内角与一条边长，即可通过正弦定理求出三角形另外一边 $O_{1}A$的长度，见式(4)。 线段 $O_{1}A$ 即为障碍物 A 到雷达的距离。

单线激光雷达通过电机驱动雷达测距核心进行顺时针旋转，最高可达每秒10Hz。旋转过程中不断发射激光束，基于三角测距模型获取 360°方向的障碍物距离信息，完成激光扫描数据的封装。
1.4 系统观测模型
  激光雷达的观测模型包括光束模型和似然场模型。其中似然场模型的基本原理是对地图进行高斯模糊（如图6 所示），利用 SLAM算法输出的环境地图， 对于地图上存在障碍物的位置， 以该点为中心， 以地图上其他点到该点的欧氏距离为变量来构造高斯分布函数，设 d 表示激光测量终点坐标$\left ( x_{zt},y_{zt} \right )$与地图 m 上最近的障碍物之间的欧式距离，那么激光雷达测量的概率分布可以由一个以 0 为中心，方差为 d 的高斯函数$\varepsilon (d)$给出，见式(2-5)：



  依据地图提供的障碍物信息可以获取地图上全部点对应的高斯分布函数值，并将该值作为地图上的似然评分，评分越高则代表扫描点与障碍物点越接近，其测量结果也就越准确。

  地图经高斯模糊后， 环境$z_{t}$的期望值相对于位姿来讲都是连续的，相比于光束模型，似然场模型不会因为环境形状变化不规律导致观测期望突变，因此似然场模型适合结构化与非结构化的环境，增加了该模型的适用场景。此外，对于似然场每一点的概率都可以预先计算，在实际 SLAM 算法执行过程中计算式仅需要对预先计算的似然场概率分布进行查表即可，计算量可以忽略不计，提高了算法的实时性。


2 SLAM实例
2.1 粒子滤波算法原理
  粒子滤波算法使用贝叶斯估计的思想， 用粒子分布去近似机器人位姿的概率分布，如图7 所示。

  首先将一定数量的粒子均匀撒在可能分布的空间中，作为粒子滤波的初始状态$x_{0}$ 的概率分布。按照贝叶斯估计的流程，可以通过里程计运动模型求得t时刻的机器人位姿 $x_{t}$ ，作为预测粒子集。 再根据激光雷达观测模型对预测粒子集中每一个粒子赋予权值， 粒子权值越高， 则代表该粒子与实际位置的误差越小。最终对所有粒子根据其权值进行加权平均，即可得到滤波后的位姿估计结果。

  SLAM 中主要任务是机器人的自主定位与环境地图的建立。 其中在已知机器人的位姿信息的条件下计算环境地图存在闭式解。 因此 SLAM 问题的主要难点在于如何获取机器人位姿信息。通过粒子滤波得到的机器人位姿$x_{1:t}$，与激光雷达的历史观测信息$z_{1:t}$可以直接计算得到环境的栅格地图，下面主要介绍基于粒子滤波器获取机器人位姿的算法步骤。

  粒子滤波算法的基本流程如下：

用粒子集表示机器人的位姿信息： $X_{t}=\left \{ \left (x_{t}^i,w_{t}^i{}  \right )|i=1,...,n\right \}$， 其中每一个粒子包含两个参数： 机器人位姿 $x_{t}^i$ ，粒子权重 $w_{t}^i$。
将总数为 N 的粒子集随机且均匀地撒在所有可能的位姿空间内，作为粒子滤波算法的初始状态 。
已知前一时刻的粒子集状态 $X_{t-1}=\left \{ \left (x_{t-1}^i,w_{t-1}^i{}  \right )|i=1,...,n\right \}$， 每一个粒子通过建议分布 $x_{t}^{i}\sim p(x_{t}|u_{t},x_{t-1}^{i})$ 计算当前时刻的位姿信息。 这里建议分布采用里程计运动模型。其中 Δx, Δy, Δθ 为里程计信息 $u_{t}$；$N_{x}$, $N_{y}$, $N_{\theta }$为里程计的测量噪声，噪声服从均值为 0 的高斯分布。最后粒子在建议分布进行随机采样，见式(6)。



4. 评估每一个粒子的权重：

  机器人实际位姿分布未知，粒子集内每一个位姿都可能是机器人的实际位姿。定义粒子权重$w_{t}^{i}$，用来描述粒子的预测分布与实际分布的匹配程度，粒子权重定义见式(7)，权重越大，表明粒子的预测分布实际分布匹配程度越高。



  经贝叶斯公式推导，可以得到权重的计算式，见式(8):



  由于粒子权重与机器人在$x_{t}$位姿下获取观测数据$z_{t}$的置信度成正比，其系数$\eta$为归一化因子。因此首先将每个粒子对应的$p(z_{t}|x_{t}^{i})$ 求出，计算出归一化因子$\eta$ （见式）即可通过式()求出归一化后的粒子权重。



计算出机器人位姿估计结果，见式(10)。



6. 将得到的当前时刻粒子集${X_{t }}$ 替换前一时刻粒子集${X_{t - 1}}$ ，返回步骤（3）进行迭代计算。

7. 通过上述步骤可以迭代计算出机器人在每一时刻的位姿分布情况，并通过式(11)计算出每一时刻下的机器人位姿估计结果。通过此结果可以进一步计算出环境地图，完成即时定位与地图构建的任务。

1、hector-slam代码框架概述
下载源码：git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git
原理解读参照：https://blog.csdn.net/weixin_40047925/article/details/80679496
其中包含了许多用于仿真的文件，hector slam算法主要集中在hector mapping包中，同时核心的位姿估计和点云建立栅格图操作主要在文件HectorSlamProcessor.h的函数update中。如下：
函数newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov))根据输入的激光点云数据和现有的栅格图进行对齐，这部分是整个代码的核心，主要思想是使用高斯牛顿优化代价函数，求取让代价函数达到最优的位姿偏量，从而求出当前机器人的位姿态。
当然，如果已经知道机器人的准确位姿不需要进行上面的位姿解算，直接用函数mapRep->updateByScan(dataContainer, newPoseEstimateWorld)更新栅格地图。这部分算法原理比较简单，主要两个方面：一是，用对数表达每一个子栅格的概率值；二是，用Bresenham 画线算法来更新子栅格的概率值。
为了说明上述算法原理，先介绍一部分的预备知识，然后再切入正题。
2、hector-slam预备知识
2-1 占用栅格地图
这里直接使用上述参考文章进行说明，假设机器人的位姿已知，那么经过每一次激光数据扫描，真实障碍物在栅格地图上的占用概率不断提升最终无限接近1，而没有障碍物的地方则趋近于0。整个栅格地图被分成了一定分辨率的子栅格，最终地图的概率分布如下：
p(m|z_{1:t},x_{1:t})=\prod p(m_{i}|z_{1:t},x_{1:t})
整个栅格图构建需要计算每一个子栅格的占用概率值。实际在表达栅格占用概率使用对数形式表达，好处是避免在0和1附近概率值不稳定的问题。具体如下：
l_{t,i}=log\frac{p(m_{i}|z_{1:t},x_{1:t})}{1-p(m_{i}|z_{1:t},x_{1:t})}                         p(m_{i}|z_{1:t},x_{1:t})=1-\frac{1}{1+e^{l_{t,i}}}
在实际使用栅格地图进行导航或者路径规划时，每一个栅格仅有三个状态：占用、空闲和未知，该三种状态仅需对p(m_{i}|z_{1:t},x_{1:t})或者l_{t,i}设定范围进行判断即可，比如hector slam中是l_{t,i}值大于0为占用，小于0为空闲，等于0为未知。
2-2 非线性优化——高斯牛顿法
在处理优化问题时经常会求去某个指标的最值，比如最小二乘、最大似然、最小方差等，在hector slam求解位姿时也构造了一个最小二乘问题，但是使用的模型是关于位姿的非线性函数，所以需要使用非线性优化的方法，hector slam使用的是高斯牛顿法。这里对高斯牛顿法做个简单说明。
对于非线性最小二乘问题，可以用如下数学表达式来描述：
x = argminF(x)=argmin\frac{1}{2}\left \|f(x))\right \|_{2}^{2}
如果f(x)数学表达式比较简单，可以直接对F(x)求导，并领其等于0，可以求出极值。对于无法求导情况，一般用迭代求解\Delta x，求解的迭代值不断的趋使F(x)趋近最小值，直到\Delta x变化很小或者达到最大迭代次数。那么关键问题在于求取\Delta x，这里有很多方法，如：梯度下降法、牛顿法、牛顿高斯法和LM法。简单比较一下：梯度下降法——对代价函数F(x)进行泰勒展开，仅保留一阶导数部分，按照梯度反方向进行迭代，即\Delta x=-J(x)，但是这种方向并不一定是最合适的方向，有时可能迭代过头，反而会绕路；牛顿法——其实是上面F(x)泰勒展开，保留到二阶，增加了海森矩阵，因此其迭代的路径一般是比较短的，但是需要计算F(x)的二阶导数，这个是很难求得的，因此实际应用受限；牛顿高斯法——为了避免牛顿法求解海塞矩阵难的问题，牛顿高斯法先对f(x)进行一节泰勒展开，然后得到F(x)近似表达式，然后另其导数为0，求取F(x)局部最小时的\Delta x，然后不断迭代，直至达到收敛条件；LM法是在牛顿高斯法上进行了修改，避免了牛顿高斯法求解正规方程时可能出现的奇异解，被广泛使用。
话不多说，直接介绍高斯牛顿法。
f(x+\Delta x) \approx f(x) + J(x)\Delta x \Rightarrow
F(x) = \frac{1}{2}( f(x) + J(x)\Delta x )^{T}( f(x) + J(x)\Delta x ) \Rightarrow
F{}'(x) = J^{T}(x)( f(x) + J(x)\Delta x ) =0 \Rightarrow
J^{T}(x)J(x)\Delta x = - J^{T}f(x)\overset{H = J^{T}(x)J(x)}{\rightarrow}\Delta x=-H^{-1}J^{T}f(x)
和牛顿法比较实际上是用雅克比矩阵二次来代替了海塞矩阵，省去了计算海塞矩阵。
3、hector-slam位姿解算
当我们传感器第一次接收到数据，我们可以人为设定当前位置，以此来第一次建立栅格地图m，可以认为是初始化操作。之后，我们在根据前一时刻建立的地图求解的位姿都是相对初始化时刻的位姿而言的，这一点需要明白。那么这个问题可以用贝叶斯估计描述为：
p(x_{t}|m_{t-1},z_t)=\beta p(m_{t-1}|x_{t},z_{t})*p(x_{t}|x_{t-1})
这里假设状态x符合马尔科夫过程，但是对于先验p(x_{t}|x_{t-1})概率很多情况下是不知道的，如果有这部分的先验的话可以添加这部分代码。在没有这部分信息情况下，hector-slam是让似然概率p(m_{t-1}|x_{t},z_{t})达到最大的最大似然估计。hector-slam通过最小二乘的方式来表达x_{t}=argmaxP(m_{t-1}|x_{t},z_{t})最大似然问题(最小二乘实际上就是最大似然估计的一种)：
x=argmin\sum [1-M(S_{i}(x))]^{2}
未完待续。。。。

1、hector-slam代码框架概述

      下载源码：git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

      原理解读参照：https://blog.csdn.net/weixin_40047925/article/details/80679496

       其中包含了许多用于仿真的文件，hector slam算法主要集中在hector mapping包中，同时核心的位姿估计和点云建立栅格图操作主要在文件HectorSlamProcessor.h的函数update中。如下：



       函数newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov))根据输入的激光点云数据和现有的栅格图进行对齐，这部分是整个代码的核心，主要思想是使用高斯牛顿优化代价函数，求取让代价函数达到最优的位姿偏量，从而求出当前机器人的位姿态。

       当然，如果已经知道机器人的准确位姿不需要进行上面的位姿解算，直接用函数mapRep->updateByScan(dataContainer, newPoseEstimateWorld)更新栅格地图。这部分算法原理比较简单，主要两个方面：一是，用对数表达每一个子栅格的概率值；二是，用Bresenham 画线算法来更新子栅格的概率值。

       为了说明上述算法原理，先介绍一部分的预备知识，然后再切入正题。

2、hector-slam预备知识

      2-1 占用栅格地图

       这里直接使用上述参考文章进行说明，假设机器人的位姿已知，那么经过每一次激光数据扫描，真实障碍物在栅格地图上的占用概率不断提升最终无限接近1，而没有障碍物的地方则趋近于0。整个栅格地图被分成了一定分辨率的子栅格，最终地图的概率分布如下：

     

       整个栅格图构建需要计算每一个子栅格的占用概率值。实际在表达栅格占用概率使用对数形式表达，好处是避免在0和1附近概率值不稳定的问题。具体如下：

                              

        在实际使用栅格地图进行导航或者路径规划时，每一个栅格仅有三个状态：占用、空闲和未知，该三种状态仅需对或者设定范围进行判断即可，比如hector slam中是值大于0为占用，小于0为空闲，等于0为未知。



           2-2 非线性优化——高斯牛顿法

        在处理优化问题时经常会求去某个指标的最值，比如最小二乘、最大似然、最小方差等，在hector slam求解位姿时也构造了一个最小二乘问题，但是使用的模型是关于位姿的非线性函数，所以需要使用非线性优化的方法，hector slam使用的是高斯牛顿法。这里对高斯牛顿法做个简单说明。

         对于非线性最小二乘问题，可以用如下数学表达式来描述：

          

         如果f(x)数学表达式比较简单，可以直接对F(x)求导，并领其等于0，可以求出极值。对于无法求导情况，一般用迭代求解，求解的迭代值不断的趋使F(x)趋近最小值，直到变化很小或者达到最大迭代次数。那么关键问题在于求取，这里有很多方法，如：梯度下降法、牛顿法、牛顿高斯法和LM法。简单比较一下：梯度下降法——对代价函数F(x)进行泰勒展开，仅保留一阶导数部分，按照梯度反方向进行迭代，即，但是这种方向并不一定是最合适的方向，有时可能迭代过头，反而会绕路；牛顿法——其实是上面F(x)泰勒展开，保留到二阶，增加了海森矩阵，因此其迭代的路径一般是比较短的，但是需要计算F(x)的二阶导数，这个是很难求得的，因此实际应用受限；牛顿高斯法——为了避免牛顿法求解海塞矩阵难的问题，牛顿高斯法先对f(x)进行一节泰勒展开，然后得到F(x)近似表达式，然后另其导数为0，求取F(x)局部最小时的，然后不断迭代，直至达到收敛条件；LM法是在牛顿高斯法上进行了修改，避免了牛顿高斯法求解正规方程时可能出现的奇异解，被广泛使用。

        话不多说，直接介绍高斯牛顿法。

               

        

        

        

        和牛顿法比较实际上是用雅克比矩阵二次来代替了海塞矩阵，省去了计算海塞矩阵。

3、hector-slam位姿解算

       当我们传感器第一次接收到数据，我们可以人为设定当前位置，以此来第一次建立栅格地图m，可以认为是初始化操作。之后，我们在根据前一时刻建立的地图求解的位姿都是相对初始化时刻的位姿而言的，这一点需要明白。那么这个问题可以用贝叶斯估计描述为：

                                   

        这里假设状态x符合马尔科夫过程，但是对于先验概率很多情况下是不知道的，如果有这部分的先验的话可以添加这部分代码。在没有这部分信息情况下，hector-slam是让似然概率达到最大的最大似然估计。hector-slam通过最小二乘的方式来表达最大似然问题(最小二乘实际上就是最大似然估计的一种)：

                                     

   未完待续。。。。