SLAM算法知识荟萃

SLAM

自动驾驶八股

PID 控制算法

P 比例积分控制，对误差乘以 $K_p$，得到调节量。随着误差地减小，调节量会逐渐减小，并且 $K_p$ 越大表明到达指定状态越快，响应越快；但是会存在稳态误差，比如无人机上方存在向下吹的风时；
I 积分控制，对多阶段误差积分后乘以 $K_i$，得到调节量，这样即使存在稳态误差，仍会逐渐达到指定状态；
D 微分控制，令 $K_d\cdot \Delta err=K_d\cdot (er{r}_t-er{r}_{t-1})$，如果误差为负证明误差在逐渐减小，可以使调节量小一点，避免出现过冲的现象；

P 比例积分控制	I 积分控制	D 微分控制
现在	过去	将来
快速响应	消除误差	抑制过冲
存在稳态误差	出现超调量	易受到干扰

ROS 和 ROS2 区别

1. ROS只能在Linux上运行，ROS2支持Linux，Windows，MacOS
2. ROS2 没有 master 节点
3. ROS 基于 TCP/UDP 通讯，ROS2 内部提供了 DDS 的抽象层实现，有了这个抽象层，用户就不用关注底层的 DDS 使用了哪家 API。可以以零拷贝的方式传递消息，节省了CPU和内存资源。
   在 ROS2 中使用 export RMW_IMPLEMENTATION=... 更换 DDS（FastDDS, Cyclone, RTI connext）
4. ROS 使用 C++ 03，ROS2 使用 C++ 11 以上；ROS 使用 Python2.7，ROS2 使用 Python3.5
5. ROS 设置 Domain 需要区分主从，ROS2 设置 Domain 时只需要设置 ID 一致即可
6. 可以通过 rosbridge 在 ROS 和 ROS2 之间通讯
7. ROS2 中有 QoS Policy

四元数在表示空间旋转时的优势是什么？

1. 解决了其他表示三维空间旋转算法会遇到的问题，比如使用欧拉角会遇到的万向锁问题。
2. 参数少，计算效率高，四元数需要四个参数，旋转矩阵需要九个参数。
3. 可以被用来规划出高阶连续姿态运动以及在多姿态间插值。
4. 表示旋转时要求四元数是单位四元数，旋转矩阵是单位矩阵，而转化四元数为单位四元数的难度更低。

介绍自动驾驶系统

自动驾驶系统包括环境感知，决策规划，控制执行三部分。

1. 环境感知包括：高精度地图，多传感器信息，规则理解和行为预测等；
2. 决策规划包括：路径规划，循迹控制，车道保持等；
3. 控制执行包括：驱动系统，控制转向等。

介绍回环检测

回环检测的原因：在SLAM建图的过程中，位姿的估计仅考虑相邻时间上的关键帧，在这期间产生的误差会逐步累积，从而形成累积误差。所以需要通过回环检测方法，发现潜在的回环来修正漂移误差并构建全局一致的轨迹和地图。
比如，根据特征点描述子的相似程度，词袋模型（ORB，VINS，RTABMAP），深度学习模型等。

介绍词袋模型

1. 预先加载词袋字典树，将每一帧的描述子都转化成为一个单词。
2. 字典对所有单词计算一个词袋向量，词袋向量之间的距离表示单词（两帧）之间的差异性。
3. 在图像检索的过程中，会利用倒排索引的方式，找出相似度高的关键帧，并根据词袋向量之间距离由近及远排序。

手撕对极约束

对极几何：描述的是两幅视图之间的内在摄影关系，与外部场景无关，只依赖于摄像机内参和这两幅图之间相对姿态之间的关系。
$X_L,X_R$ 像点
$C_L,C_R$ 光心
$e_L,e_R$ 极点

E为本征矩阵，表征了同一物点在两个透视相机成像面上像点的几何约束关系
F为基本矩阵，表征了同一物点在两个透视相机像素面上像素点间的几何约束关系

使用OpenCV找到四边形的边界

1. 读取图像，并转为灰度图
2. 使用高斯滤波，对图像进行处理
3. 计算梯度幅值和方向
4. 使用阈值检测边缘
5. 使用霍夫变换检测直线
6. 使用直线的坐标点作为四边形的边界

介绍卡尔曼滤波

1. 基于前一时间点的系统状态的概率分布给出当前时间点的系统状态的概率分布预测P1
2. 基于当前时间点的观测量的概率分布Q给出另一组对当前时间点系统状态的概率分布预测P2
3. 最后将两个关于当前时间点系统状态的概率分布的预测P1和P2，计算它们的联合概率分布（把两个概率分布乘到一起去）。
   这个联合概率分布就是最终更正过后的当前时间点的系统状态概率分布
   有了它，接下来就可以进入下一个时间点继续迭代我们交替的预测-更正步骤了

推导卡尔曼增益

$$\mathcal{N}(x,{\mu }_0,{\sigma }_0)\cdot \mathcal{N}(x,{\mu }_1,{\sigma }_1)\stackrel{?}{=}\mathcal{N}(x,{\mu }^{\prime },{\sigma }^{\prime })$$
把该式按照一维方程进行扩展，可得：
$$\begin{gathered} {\mu }^{\prime }={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2} \\ {{\sigma }^{\prime }}^2={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2} \end{gathered}$$
使用k进行简化：
$$\begin{gathered} \mathbf{k}=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2} \\ {\mu }^{\prime }={\mu }_0+\mathbf{k}({\mu }_1-{\mu }_0) \\ {{\sigma }^{\prime }}^2={\sigma }_0^2-\mathbf{k}{\sigma }_0^2 \end{gathered}$$
扩展到多维空间，将这个式子转化为矩阵格式：
$$\begin{gathered} \mathbf{K}={\sum }_0({\sum }_0+{\sum }_1{)}^{-1} \\ {\mu }^{\prime }={\mu }_0+\mathbf{K}({\mu }_1-{\mu }_0) \\ {\sum }^{\prime }={\sum }_0-\mathbf{K}{\sum }_0 \end{gathered}$$
这个矩阵$\mathbf{K}$就是卡尔曼增益。

介绍PnP

PnP 全称 Perspective-n-Points，多点透视成像，描述了当知道n个3D空间点及其投影点的位置时，如何估计相机的位姿。
特征点的3D位置可以通过三角化或者RGB-D相机得到。因此在双目或RGB-D VIO中，可以直接使用PnP估计相机运动；在单目VIO中，需要先进行初始化才可以使用PnP。

PnP求解最少需要几个点

PnP求解最少需要7个3D空间点，3个点对和1个验证点，通过SVD分解求解位姿会得到4组解。

PnP的误差来源

1. 特征匹配的正确性（数据关联的正确性）
2. 测量数据的噪声（是否去畸变，坐标是否精确）
3. 光照程度
4. 运动变化

求解线性方程 Ax=b

1. LU方法：矩阵A分解为上下三角阵L和U，需要矩阵A可逆，结果的准确性依赖于A的数值形式
2. QR方法：矩阵A分解为正交阵Q和上三角阵R的乘积。QR方法数值稳定，计算时间复杂度高。
3. SVD方法：将矩阵A分解为U，Σ，V三个较小的矩阵组合的形式，则可计算出A的伪逆B。

SVD和QR方法哪个快

1. QR方法数值稳定，计算时间复杂度高。
2. SVD是最小二乘求解线性方程组最可靠的分解法，但计算代价很大。
3. QR方法比SVD方法更快。

介绍IMU预积分

IMU可以获得每一时刻的加速度和角速度，通过积分就可以得到两帧之间的由IMU测出的位移和旋转。但IMU的频率远高于相机。
在VIO算法中，当被估计的状态量不断调整时，每次调整都需要在它们之间重新积分，这个过程会导致计算量爆炸式增长。
IMU预积分是对IMU的相对测量进行处理，使它与绝对位姿解耦，或者只要线性运算就可以矫正，从而避免绝对位姿被优化时重复积分。
ORB-SLAM3中包含IMU预积分。

介绍AMCL

AMCL改进了MCL，解决了机器人绑架问题。在MCL基础上，当平均置信度下降时，增加随机粒子。

介绍描述子距离的匹配方法

1. 暴力匹配
2. 基于阈值：如果描述子之间的距离小于某个阈值，则认为他们相互匹配
3. 基于最近邻：如果两点的距离小于某个阈值且是最近邻，则认为他们是唯一的匹配
4. 基于距离比率的最近邻：该方法与最近邻类似，不同的是对该点的最近邻和次近邻进行了处理
5. 使用词袋模型进行初步筛选后，再进行匹配
6. 重投影匹配

为什么SLAM中要引入李群李代数

1. SLAM中需要求解什么样的位姿更符合当前的观测数据，一个方法就是将R，t构建为一个优化问题，求解最优的R，t使得误差最小化
2. 而旋转矩阵是带有约束的（正交且行列式为1）。当他们作为优化变量时，会引入额外的约束，使得优化变得困难。通过李群-李代数间的转化关系，可以把位姿估计变成无约束的优化问题，简化了求解过程。

介绍BA的过程

BA-bundle adjustment(光束法平差)，这种方法是将相机位姿和三维点放在一起进行优化。
该优化的误差项是重投影误差，即3D点投影的投影位置与观测位置的差。
求解思路是，先分别求出重投影误差对相机位姿和三维点的雅克比矩阵，最后使用LM算法求解出结果。

介绍关键帧

关键帧是信息最丰富的帧，比如新的特征最多的帧，主要用于解算当前位置并构建地图

选择关键帧的方法

1. 特征的数量
2. 使用统计特征来评估帧的质量
3. 运动的连续性

三维点云地图转二维栅格地图

先转化为八叉树地图，然后根据高度阈值筛选出部分点，最后垂直投影到二维平面上

介绍三角化

单目SLAM中使用三角化估计场景深度，并定位世界点在三维坐标系中的值。原理：在不同的位置观测同一个三维点P(x, y, z)，已知在不同位置处观察到的三维点的二维投影点X1(x1, y1), X2(x2, y2)，利用三角关系，恢复出三维点的深度信息z。

Fast，Harris，ORB特征点

Fast特征点

针对工程中对于特征点提取的高实时性的要求，提高特征点提取的速度。
全称为：Features From Accelerated Segment Test。
FAST角点：通过对比中心与周围点（半径为3的圆上的点）灰度的差别，即可确定是否为关键点。
例如，考虑灰度图像，若该点的灰度值比其周围领域内足够多的像素点的灰度值大或者小，则该点可能为角点。

Harris特征点

Harris角点的可以根据窗口滑动后的区域与滑动前的区域的相似性进行判断。
在图片中各个方向上移动一个小窗口，若窗口内的灰度发生较大变化，那么就认为在窗口内遇到了角点。反之，窗口内就不存在角点。
如果窗口在某一个方向上移动，灰度发生了较大的变化，而在另外一些方向上没有发生变化，那么窗口内的图像可能是一条直线。

ORB特征点

ORB特征点在FAST特征点上进行了改进。

1. 首先使用FAST特征点检测方法提取特征点，
2. 再从FAST特征点中挑选出Harris角点相应值较大的N个特征点，
3. 使用多层金字塔添加尺度不变性，使用灰度质心法添加旋转不变性。

介绍视觉SLAM框架

1. 传感器读取：相机图像信息和多种传感器的信息的读取和预处理；
2. 前端视觉里程计：估算相邻图像之间相机的运动，以及局部地图；
3. 后端优化：后端接受不同时刻相机里程计测算的相机位姿，以及回环检测的信息；
4. 回环检测：回环检测用于判断机器人是否到达过之前的位置，如果检测到回环，就把信息交给后端进行处理；
5. 建图：他根据估计的轨迹，建立与任务要求对应的地图。

介绍RANSAC原理

RANSAC通过反复选择数据中的一组随机子集来达成目标:

1. 随机选择数据中的一组作为随机子集，使用该随机子集建立模型。
2. 用该模型去测试其它的所有数据，如果某个点适用于估计的模型,认为它也是局内点。
3. 如果有足够多的点被归类为假设的局内点,那么估计的模型就足够合理。否则重新执行步骤1，2
4. 用所有假设的局内点去重新估计模型,因为它仅仅被初始的假设局内点估计过。
5. 最后,通过估计局内点与模型的错误率来评估模型。重复执行这个过程固定次数,舍弃局内点太少的模型,保留比现有的更好的模型。

视觉SLAM如何处理光暗变换场景

1. 在相机上放置滤光片
2. 采用多传感器融合技术

视觉SLAM如何处理动态场景

1. 分割前景和背景，通过像素灰度和目标变化分辨前景和背景，前景是动的，背景是静止的
2. 光流法，通过像素变化，推测物体的运动方向和速度（VDO-SLAM计算图像中每个点的运动，如果有很多点互相靠近，并且运动相同，可以视为一个刚体）
3. 多传感器融合
4. 目标识别，去除可能会移动的物体或人

激光SLAM

视觉SLAM

ORB-SLAM2

ORB-SLAM为什么没有边缘化的操作

1. 随着关键帧数目的增多，滑窗（或者说ORB-SLAM3的共视图）的计算量会越来越大。因此需要想办法去掉老的关键帧，来给新关键帧腾地方。但是最旧帧可能包含了很强的共视关系，直接删除可能会导致BA效果变差，所以无法直接剔除关键帧，需要执行边缘化，从滑窗/共视图里剔除了老的关键帧，又能保留这个关键帧的约束效果，其核心算法是舒尔补。
2. ORB-SLAM中地图点剔除机制和共视图选取local mapping大小的机制，都保证与被剔除的点和关键帧相关的协方差矩阵比较稀疏。所以直接丢弃它们，对优化结果影响很小。此时直接剔除的效率更高。

ORB-SLAM提取特征点

• 特征点的响应值描述的是该特征点的区分度大小，响应值越大的特征点越应该被保留；
• 特征点的描述子是特征点的哈希运算，其大小无意义，仅用来在数据库中快速找到特征点。

1. 分区域搜索特征点，若某区域内特征点响应值普遍较小的话就降低分数线再搜索一遍；
2. 对得到的所有特征点及进行八叉树筛选，若某区域内特征点数目过于密集，只取最大的一个。八叉树分裂：每次都对区域分为4份，如果不够期望保留的区域则继续分裂，其中没有特征点的区域不再进行分裂，直到大于阈值时就保留质量最好的一个点；
3. 计算主方向：对特征点周围半径大小为19的圆利用灰度质心法，计算中心方向作为特征点主方向，并将特征点周围像素旋转到主方向上
4. 计算描述子：对特征点周围256个点对进行逐个比较得到256位的描述子。

与OpenCV中的不同

1. 对图片进行分cell提取Fast特征点，如果该区域没有特征点就降低阈值再搜索一遍
2. 对特征点进行非极大值抑制
3. 计算特征点周围19像素的方向
4. 将特征点周围16的像素旋转到主方向上计算描述子

ORB-SLAM 初始化

1. 初始化各成员变量：读配置文件信息，创建orb词袋，创建关键帧数据库主要保存orb描述子倒排索引，创建地图
2. 创建3大线程：Tracking、LocalMapping和LoopClosing
3. 设置线程间通信
4. 初始化图像金字塔相关的变量：各层级之间的缩放关系
5. 初始化用于计算描述子的pattern：写死的256个坐标
6. 计算一个半径为16的圆的近似坐标，用于计算描述子时进行旋转操作

单目ORB-SLAM的初始化

单目VIO初始化是为了获得第一帧的特征点对应的三维点，以此作为地图点进行跟踪：

1. 设置RANSAC使用到的点对
2. 计算单应矩阵H及其卡方检验得分，计算基础矩阵F及其卡方检验得分
3. 如果H分数>0.4使用单应矩阵恢复运动；如果F分数<0.4使用基础矩阵恢复运动
4. 如果三角化得到的点数足够多且视差角足够大，则初始化成功

ORB-SLAM的哪个部分最耗时（LocalBA）

1. LocalBA最耗时，当前关键帧的一级共视关键帧位姿会被优化；二极共视关键帧会加入优化图，但其位姿不会被优化；所有局部地图点位姿都会被优化
2. 后端H矩阵求解的算法复杂度：直接对H矩阵求逆来计算增量方程，复杂度为$O(n^3)$
3. 如何加速后端H矩阵的求解：可以利用H的稀疏结构来加速运算，首先将H矩阵划分为4个块，然后利用Schur消元来分步求解

ORB-SLAM如何克服尺度漂移？

1. 单目VIO中的尺度漂移是因为尺度不确定，在位姿估计和地图点估计中计算误差和测量误差不断积累造成尺度不确定。
2. 双目和RGB-D的ORB-SLAM可以直接获得特征点的深度，所以地图点的误差就被固定在一定的范围中，从而限制了整个估计误差的传播
3. 单目ORB-SLAM中可以引入imu提供绝对尺度。虽然长时间下imu发散严重，但是短时间中的误差不影响限制位姿估计的误差传播。

ORB-SLAM的回环检测流程

找到与当前帧4相似的关键帧1，这样的话就可以根据1直接估计4的位姿，而不是1–2–3–4。减少了误差传递。
对于当前帧4的共视帧6，可以1—4—6来获得，而不是1–2–3–4–6

1. 检查闭环检测队列是否存在关键帧
2. 检测回环候选帧
   1. 从闭环检测队列中取出一个共视关键帧，作为当前检测的闭环关键帧
   2. 如果距离上一次闭环小于10帧，或者地图中关键帧没有超过10帧，不进行闭环检测
   3. 遍历当前检测的关键帧（红色点）的所有连接的共视关键帧，计算当前关键帧与每个共视关键帧的词袋相似度得分，并得到最低的分数（要保证闭环候选关键帧与当前关键帧的相似度得分大于当前关键帧与连接的共视关键帧得分的最低值）
   4. 在所有关键帧中能否找到与当前关键帧连接的闭环候选帧。闭环候选帧的判断条件：1. 上一次闭环检测的连续组与当前闭环候选帧构建的连续组是否存在连续性（有相同的关键帧）；2. 候选关键帧与相邻的关键帧构成子候选组是否存在于上一次回环检测构成的连续组中。满足这两个条件才是闭环候选帧
3. 考虑到单目相机的尺度漂移，计算当前帧与候选闭环帧的sim3变换
4. 闭环融合：第一步是融合重复的点云，并且在共视图中插入新的边以连接闭环。首先当前帧的位姿会根据相似变换(sim3结果)被矫正，同时所有与其相连的关键帧位姿也会被矫正。所有的被闭环处的关键帧观测到的地图点会通过映射在一个小范围里，然后去搜索它的近邻匹配。这样就可以对所有匹配的点云进行更加有效地数据融合，并更新关键帧位姿，以及在图中的边。
5. 图优化：为了有效完成闭环，使用本质图去优化位姿图（只优化位姿），这样可以将闭环的误差分散到整个图中。

在这整个流程中估计了当前帧与候选闭环帧的sim3变换，当前关键帧的位姿和地图点

如何关联当前帧和局部地图的数据

仅使用帧与帧之间运动估计运动，一般精度还不够。
一般会维护局部地图来提高跟踪精度和鲁棒性。
为了权衡精度和实时性，为了保证实时性，只能使用当前帧跟踪一部分帧，一般使用固定滑动窗口或ORB-SLAM2中使用的共视帧窗口，它们与当前帧共视点相对其他帧多很多。
前端主要用于跟踪（得到当前帧位姿），据此选择关键帧交给后端，后端再进行三角化新的路标点、删错冗余和错误的路标点和帧、优化各帧位姿和路标点位姿。

RTAB-MAP

1.RTAB-MAP更适合用来建图和导航，为什么很少用到？
SLAM是同步建图与定位，注重于“提供精准的定位约束”。
而RTAB-MAP能够提供稠密地图，更适合于导航，注重于“提供物体和场景体积信息”
2.内存设计
越经常被观测到的地图点就越重要，被放在工作内存中；不经常观测到的地图点，被放在长期内存中。
3.和ORB-SLAM的区别
orbslam提取orb算法提取特征点，rtabmap算法使用多种算法提取特征点；
orbl-slam基于特征点，rtabmap基于外观；
orb-slam注重于实时性，在纹理较少，光照变化大的场景下不佳，rtabmap更注重综合性能，在多种场景下表现良好。