SLAM 十四讲（第一版）各章方法总结与理解

总结十四讲中各章各步骤提到的各种方法，以及具体方法在哪个 c++ 库中可以调用。目的在于能更直观地了解 slam 过程各步骤到底在做什么，以及是怎么联系在一起的。

2. 初识 SLAM

• SLAM（Simultaneous Localization and Mapping）：同时定位 + 建图
• 前端/视觉里程计 (Visual Odometry, VO)：视觉里程计任务是估算相邻图像间相机的运动，以及局部地图的样子。
• 后端优化（Optimization）：后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。
• 回环检测（Loop Closing）：回环检测判断机器人是否曾经到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
• 建图（Mapping）：它根据估计的轨迹，建立与任务要求对应的地图。

3. 三维空间刚体运动

• Eigen 库：用于矩阵运算。

4. 李群与李代数（用于非线性优化）

• Sophus 库：用于李代数运算。

5. 相机与图像

• Opencv 库：用于图像操作。
• PCL 库：用于点云操作。
• 现实点到像素点过程，涉及 4 个坐标系
  • 世界坐标系点 P -> 相机坐标系点 P’，用到 R，t；
  • 相机坐标系点 P’ -> 归一化平面（相机）坐标系点 P_c；
  • 归一化平面（相机）坐标系点 P_c -> 去畸变后的坐标；
  • 去畸变后的坐标 -> 像素坐标点 P_uv，用到相机内参。

6. 非线性优化（前端、后端都会用到）

非线性优化即求解最小二乘问题

• 非线性优化的物理意义：

  • 定位：通过估计观测到当前数据时传感器（雷达/相机）的状态（即位姿）实现的。
  • 优化定位：优化对传感器的状态估计，即求传感器最可能产生现在观测到当前数据的状态（最大似然估计）。
  • 最大似然估计 <== 等价 ==> 最小二乘问题的最优解，所以状态（位姿）估计可以转化为最小二乘/非线性优化问题（通过李群 - 李代数的转换关系）。
  • 即状态（位姿）可以通过变换矩阵来描述，然后用李代数来优化求解。

• 非迭代方法（仅能求解简单问题）

  • 导数为零法

• 迭代方法（可以求解复杂问题）

  • 一阶和二阶梯度法（直观）：最速下降法一阶，过于贪心、牛顿法二阶，大问题时计算困难。
  • G-N 法（高斯牛顿法 / Gauss-Newton）：简单，但特定场景稳定性差可能不收敛，迭代；
  • L-M 法（列文伯格-马夸尔特法 / Levenberg-Marquadt）：鲁棒性高，但收敛较慢， 迭代；

• c++ 优化库

  • Ceres 库：可用 LM 、 dogleg 方法。
  • g2o 库：图优化，可用 GN 、 LM 、 dogleg 方法。

7. 视觉里程计（特征点法）

• 特征点：关键点/Key-point + 描述子/Descriptor，如 ORB 特征/Oriented FAST and Rotated BRIEF = Oriented FAST + BRIEF。使用特征点法估计相机运动时，我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化 重投影误差（Reprojection error） 来优化相机运动。在这个过程中，我们需要精确地知道空间点在两个相机中投影后的像素位置——这也就是我们为何要对特征进行匹配或跟踪的理由。

• 特征匹配：暴力匹配（Brute-Force Matcher） 简单、FLANN（快速近似最近邻） 适用点数多的场景

• 三角测量：估计地图点的深度，OpenCV 的 triangulation 函数。

• RANSAC（随机采样一致性 / Random Sample Concensus）：存在错误数据的场景，代替最小二乘法。

• 估计相机位姿的变化

  • 对极几何：知道两组 2D 点信息的场景，需要八对以上的点；
  • PnP（Perspective-n-Point）：知道 3D 点 + （另一帧图像中对应 2D）点投影位置信息的场景，不需要用对极几何，可以通过更少对的点估计相机位姿，包括 P3P（三对点） 、 DLT（直接线性变换） 、 EPnP（Efficient PnP，OpenCV 库中可用） 、 UPnP 这些线性方法和 BA 法 这一非线性方法。
  • ICP（Iterative Closest Point）：知道两组 3D 点信息的场景，包括 SVD（奇异值分解，Eigen 库中可用） 这一线性方法和类似 BA 法 的非线性方法。

• BA 法（光束法平差/捆集调整 / Bundle Adjustment）：从视觉重建中提炼出最优的 3D 模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点空间位置做出最优的调整 (adjustment) 之后，最后收束到相机光心的这个过程。在本讲中具体表示为通过 PnP 方法线性部分的结果作为初始值，同时优化特征点和传感器位姿。其中优化过程可以用 Ceres 和 g2o 这两个库中的 G-N / L-M 等方法。

8. 视觉里程计（直接法）

• 光流法（Optical Flow）：只计算关键点，然后用光流法代替计算描述子来追踪特征点的运动，估计相机运动时仍使用“对极几何、PnP 或 ICP 算法”。
  • 稀疏光流：计算部分像素运动
  • 稠密光流：计算所有像素运动
  • LK 光流（Lucas-Kanade）：假设 1 - 同一个空间点的像素灰度值，在各个图像中是固定不变的（灰度不变假设）；假设 2 - 某一个窗口内的像素具有相同的运动。OpenCV 库中可用。
• 直接法（Direct Method）：只计算关键点，然后用直接法计算特征点在下一帧图像中的位置，即用像素灰度信息计算相机运动，不用“对极几何、PnP 或 ICP 算法”，速度更快。在直接法中，我们并不需要知道点与点之间之间的对应关系，而是通过最小化 光度误差（Photometric error） 来求得它们。转为优化问题后可用 ceres 库和 g2o 库。
  • 稀疏直接法：只使用稀疏关键点，只能计算稀疏的重构。
  • 半稠密（Semi-Dense）直接法：只使用带有梯度的像素点，可以重构一个半稠密结构。
  • 稠密直接法：使用所有像素。
• 直接：不计算关键点，直接根据像素灰度差异计算相机运动。

9. 实践：设计前端

• 数据：带有深度信息的、连续的各帧图像数据。
• 初步目标：估计两两帧之间（Pairwise）的相机运动，即无结构（Structureless）的 VO 前端。
• 原理：前一帧作参考（Reference）建立坐标系，当前帧（Current）与其做特征匹配，并估计运动关系。最后加上迭代优化位姿估计。
• 地图：自己实现的地图类，第一帧点全部放入。后续帧来到时优化地图（删除地图不在当前视野中的点、匹配率太低的点、匹配点数少时加入新的点），然后拿出视野内的点和当前帧特征描述子匹配。
• 实现：使用 OpenCV 的 ORB 类实现特征提取和匹配，OpenCV 的 solvePnPRansac 方法实现运动关系的估计， 并以 RANSAC PnP 结果为初值，用 g2o 的 L-M 方法优化位姿运动估计。
• 效果：对于局部的定位和低精度需求，VO 前端已经够用，无需完全的 SLAM。但容易丢失且可能轨迹漂移。

10. 后端1

• 后端的意义：前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。后端就在于构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。

• 后端的实时性：后端的优化没必要实时地响应前端的图像数据。人们倾向于把前端和后端分开，运行于两个独立线程之中。

• 滤波器方法：马尔可夫性假设（k 时刻状态只与 k −1 时刻状态有关）下的方法。小型场景下更简单。

  • 卡尔曼滤波器（KF / Kalman Filter）：线性系统使用
  • 扩展卡尔曼滤波器（EKF / Extended Kalman Filter）：非线性系统使用
  • 局限：无法处理回环场景、线性近似可能只在很小范围内成立、大型场景算力要求更高

• 非线性优化方法：依然考虑 k 时刻状态与之前所有状态的关系。

  • BA 和图优化：需要做的主要是构造 Bundle Adjustment 问题，设置 Schur 消元，然后调用稠密或者稀疏矩阵求解器对变量进行优化即可。

11. 后端2

• 位姿图（Pose Graph）优化：只优化相机位姿，抛弃特征点。节点是相机位姿，边是两个位姿节点之间相对运动的估计。使用 g2o 库完成位置图的优化。
• 因子图（Factor Graph）优化：由表示优化变量的变量节点，以及表示因子的因子节点组成。其过程为调整各变量的值，使它们的因子之乘积最大化。

12. 回环检测

• 回环检测的意义
  • 给出除了相邻帧之外的，一些时隔更加久远的约束，消除长期的累计误差，保证轨迹和地图全局一致（Global Consistent）。
  • 可用于进行重定位。
• 基于里程计的几何关系（Odometry based）：里程计回到相近位置时认为形成回环。但里程计可能不准，累计误差大时失效。
• 基于外观（Appearance based）：根据两张图像的相似度确定回环。
  • 词袋模型（BoW/Bag-of-Words）：用“图像上有哪几种特征”来描述一个图像
  • 字典：包含许多图像概念的单词，其生成问题类似一个聚类问题。
  • K-means（K 均值）：用于聚类，生成字典
  • K 叉树：用于表达字典，类似于层次聚类，是 k-means 的直接扩展。叶子层是单词，中间节点用于快速查找。
  • DBoW3 库：DBoW3::Vocabulary 对象用于生成字典、计算图像之间描述向量的相似度

13. 建图

• 单目稠密重建
  • 立体视觉
  • 极线搜索：在极线上搜索上一帧图片中的关键点位置
  • 块匹配：在极线上进行块匹配来搜索关键点，常用SAD（差的绝对值之和 / Sum of Absolute Difference）、SSD（平方和 / Sum of Squared Distance）、NCC（归一化互相关 / Normalized Cross Correlation）
  • 高斯分布的深度滤波器：用于优化不同帧图片中同一个点的深度估计，计算量小。
• RGB-D 稠密重建
  • 点云地图（Point Cloud Map）：可用 PCL 库实现
  • 八叉树地图：导航常用、压缩性能较好、可灵活更新，可用 Octomap 库实现