第一天：SLAM整体算法框架简介

从零开始搭建一套SLAM系统

第一天：整体算法框架简介以及学习建议

SLAM是什么

• SLAM 和 SFM 是什么关系

  • 不同点：
    • SFM (Structure From Motion)，称之为传统三维重建，这是一门计算机视觉学科的分支，特点是把图片数据集集回来，离线慢慢精细化处理。常见应用就是重建某建筑物的3d地图，比较炫的成果可以见 rome_paper.pdf (washington.edu) (一天重建罗马城)。
    • 提起SLAM就要提到ROS。ROS是机器人操作系统，用一堆杂七杂八的传感器组装好一套硬件，采集各个传感器的数据，传送给ROS的处理节点。SLAM会处理这些数据进行实时建图与定位，要做到的是低运算量！2107.00822 (arxiv.org)
    • 二者所属学科不同，应用侧重点不同。
  • 相同点：
    • SFM为SLAM提供了强有力的理论支持，二者的数学理论相似。

• SLAM 和 Deep Learning 是什么关系

  • SLAM是和Deep Learning互补的算法。SLAM专注于几个问题，通过构建周围世界的3d或2d地图，并确定robot所在的位置和方向，Deep Learning主要解决感知与识别。
  • 如果想让机器人走到冰箱而不撞墙，你需要SLAM；如果要识别冰箱内的物体，你需要神经网络。

SLAM的前端后端

• 首先明确三个问题：

  • SLAM可以使用大量不同的硬件
  • SLAM是一个概念不是算法
  • SLAM包含多个模块，每个模块都有多种算法

• SLAM框架可以分为前端和后端。

  • 前端：涉及传感器的选择以及传感器数据处理，比如里程计时间戳和Lidar点云时间戳的对齐等。位姿粗估计。

    • 移动系统：里程计、GPS、IMU惯导等
    • 移动系统中的传感器可以选择一种或多种，甚至“没有”（参考纯视觉里程计VO或者纯激光里程计LO）。
      • 前端状态估计
        • IMU积分或轮速DR
        • 对极几何
        • 点云配准

  • 后端：综合前端信息，进行位姿优化。

    • 测量系统：Laser, sonar, vision等

      • 测量系统中，Laser激光精度高，就是价格昂贵；声呐雷达价格便宜，技术成熟，就是精度差，水下等特殊场景广泛采用。摄像机信息量丰富直观，但处理量大，对光照敏感。vision又分单目双目等。

    • 后端优化

      • 基于滤波的算法框架
      • 基于优化的算法框架

SLAM的一般过程

运动估计

• 由于运动估计传感器（移动系统）得到的位姿通常具有较大的误差，如果不使用测量系统得到的周围环境信息来优化位姿的话，其不确定性与累积误差会越来越大，如下图所示
  

• scan2scan

​ 渐进式的匹配方式，存在累计误差。如果有一帧错误，后续又基于这个错误的位置，后果不堪设想。

• scan2map单帧状态优化

  • 使用局部地图来优化当前帧位姿

    

  • 可以消除前端里程计的部分误差，但是同样也是渐进式的匹配方式，存在累计误差。如果有一帧错误，后续又基于这个错误的位置，后果不堪设想。

• 批量状态优化

  • 使用滑动窗来优化当前帧与历史帧位姿

    

  • 浅蓝⾊点代表机器⼈状态，深蓝⾊点表示地标。地标的选择是⼀个研究⽅向，简单的有特征点（静⽌点），更鲁棒更⾼级的是线段，平⾯都可以作为地标。

  • 优化方法和滤波器方法有根本上的不同。当前时刻的状态不仅仅与上一时刻的状态有关，而是考虑过去历史帧中的信息（局部地图）。通过优化，把误差平摊分到每一次观测当中。

回环检测与回环优化

• 回环检测：新来一帧数据，如何判断它在已有序列中是否出现过。

  回环检测简单来说，就是让机器人记住它走过的路。当机器人走到一个它之前已经走过的地方时，它能够认出这个地方，并且调整它的地图和位置。这就像我们走在一个迷宫里，如果我们能记住我们之前走过的路，就不容易迷路。

  回环检测是SLAM（同步定位与地图构建）系统中的一个关键组件，用于识别当前帧是否在之前的帧中已经出现过，以减少累积误差并提高定位和地图构建的精度。具体步骤如下：

  1. 特征提取：
     • 从新帧和已有序列帧中提取特征点，常用算法包括SIFT、SURF、ORB等。
     • 提取特征后，得到每个帧的关键点和描述子，用于后续的特征匹配。
  2. 特征匹配：
     • 使用KNN（K-最近邻）或FLANN（快速近似最近邻搜索）等算法对新帧与已有帧的特征点进行匹配。
     • 通过RANSAC（随机采样一致性）算法剔除错误匹配，只保留有效的特征点对。
  3. 回环验证：
     • 计算匹配点对之间的几何变换矩阵，如单应矩阵或基础矩阵，验证新帧与已有帧的几何一致性。
     • 如果变换矩阵通过验证，则认为检测到回环。
  4. 更新地图：
     • 利用回环信息优化全局地图，减少因累积误差导致的漂移。
     • 更新当前帧的位姿估计，提高SLAM系统的整体精度。

• 回环优化（增量优化）：实际的SLAM问题是一个增量问题，在基于后端优化的方法中，通常采用滑窗的方式来进行局部地图优化，而在GTSAM中，是通过SAM2构建的Bayers Tree来实现增量优化，GTSAM会对当前状态的临近状态进行优化，而对于即使产生回环，也只会在回环区域内进行优化。

一个完整的SLAM框架处理过程如下：

• 机器人使用观测系统测量地标相对于机器人的距离和角度。
• 运动一段距离后，达到新的位置，机器人通过运动方程预测新位置。
• 在新的位置，通过观测测量系统重新测量各个地标的距离和角度。
• 如此反复

学习建议

• 理论与实践结合，如果理论一时半会无法理解，则通过实践来加深理解

课程的整体算法框架

框架表

Core algorithm	Department	Role and responsibility
LINK3D-Keypoints	Front-End	Lidar Keypoint Extraction
Optical-Flow-Tracking	Front-End	Keypoint Projection And Tracking
IMP-Ceres	Front-End	Scan Registration
Cloth-Simulation-Filtering	Back-End	Ground Filter
SLD-Ground-Voxel	Back-End	Sliding Windows Opt On Ground Voxel
ScanContex-Descriptor	Loop	Loop Detection
ScanContex-ICP	Loop	Loop Correction

框架表解读

Core algorithm

这个框架表列出了在SLAM（Simultaneous Localization and Mapping，即时定位与地图构建）系统中使用的核心算法。每个算法都有不同的作用，并在不同的处理阶段中发挥重要作用。

Department

表格中按部门划分了各个算法，这些部门分别是“Front-End”（前端）、“Back-End”（后端）和“Loop”（回环检测与优化）。每个部门在SLAM系统中负责不同的任务：

• Front-End（前端）：负责初步的数据处理和特征提取。
• Back-End（后端）：负责数据的进一步处理和优化。
• Loop（回环）：负责检测和修正回环（即机器人重新经过之前走过的路径时进行的识别和校正）。

Role and responsibility

每个算法在SLAM系统中都有特定的角色和职责：

1. LINK3D-Keypoints（前端 - Lidar关键点提取）

   • 作用：从激光雷达（Lidar）数据中提取关键点。关键点是数据中具有显著特征的点，可以用来进行匹配和定位。

2. Optical-Flow-Tracking（前端 - 关键点投影和跟踪）

   • 作用：在连续的图像帧之间进行关键点的投影和跟踪。这有助于跟踪物体的移动，从而估计机器人的运动。

3. IMP-Ceres（前端 - 扫描配准）

   • 作用：进行扫描匹配和配准。通过配准，系统可以将来自不同时间点或不同传感器的数据对齐到同一个参考框架中。

4. Cloth-Simulation-Filtering（后端 - 地面滤波）

   • 作用：利用布料模拟进行地面点云的滤波。这样可以有效地去除地面噪声，提高地图的精度。

5. SLD-Ground-Voxel（后端 - 滑动窗口地面体素优化）

   • 作用：在地面体素上应用滑动窗口优化。这有助于在构建地图时对地面的特征进行更精确的表示和处理。

6. ScanContex-Descriptor（回环 - 回环检测）

   • 作用：进行回环检测。当机器人经过之前已经走过的路径时，这个算法帮助识别并确认回环，从而修正路径和位置。

7. ScanContex-ICP（回环 - 回环校正）

   • 作用：在回环被检测到后，使用ICP（Iterative Closest Point）算法进行回环校正。这有助于调整和优化整个路径，使地图更加精确。