【2D/3D-Lidar-SLAM】 2D/3D激光SLAM以及GMapping 与 Cartographer

这里写自定义目录标题

- [1. 激光SLAM分类](#1. 激光SLAM分类)
- [2. 2D Lidar SLAM](#2. 2D Lidar SLAM)
- [3. 3D Lidar SLAM](#3. 3D Lidar SLAM)
- [4. GMapping](#4. GMapping)
- - [**1. GMapping 系统架构**](#1. GMapping 系统架构)
  - - [1.1 **粒子滤波器Particle Filter**](#1.1 粒子滤波器Particle Filter)
    - [1.2 **运动模型Motion Model**](#1.2 运动模型Motion Model)
    - [1.3 **传感器模型Sensor Model**](#1.3 传感器模型Sensor Model)
    - [1.4 **地图更新Map Update**](#1.4 地图更新Map Update)
    - [1.5 **重采样Resampling**](#1.5 重采样Resampling)
    - [1.6 **闭环检测Loop Closure**](#1.6 闭环检测Loop Closure)
  - [**2. GMapping 的特点**](#2. GMapping 的特点)
  - - [2.1 **基于粒子滤波器的 SLAM**](#2.1 基于粒子滤波器的 SLAM)
    - [2.2 **高效的 2D 建图**](#2.2 高效的 2D 建图)
    - [2.3 **实时性**](#2.3 实时性)
    - [2.4 **闭环检测能力有限**](#2.4 闭环检测能力有限)
    - [2.5 **开源与 ROS 集成**](#2.5 开源与 ROS 集成)
  - [**3. GMapping 的优缺点总结**](#3. GMapping 的优缺点总结)
  - [**4. 典型应用场景**](#4. 典型应用场景)
- [5. Cartographer](#5. Cartographer)
- - [**1. 系统架构**](#1. 系统架构)
  - [**2. Cartographer 的特点**](#2. Cartographer 的特点)
  - [**3. 典型应用场景**](#3. 典型应用场景)
  - [**4. 优缺点总结**](#4. 优缺点总结)

1. 激光SLAM分类

激光雷达SLAM包括以下几种方案:

直接法, 直接法的典型代表是ICP和NDT系列，ICP可以直接计算出激光的位姿，多帧可以构成联合优化，这种方案比较简单但是比较有效，常用于激光雷达的多程对齐。
基于特征的匹配(LO)，该方案的典型代表是LOAM和后续的改进方案A-LOAM/F-LOAM,该方案通过寻找线面特征，并通过特征匹配来计算帧间的位姿，多个位姿可以做BA优化。
多传感器融合的方案。该方案的典型代表是LIO-Mapping,LINS和LIO-SAM.LIO-Mapping算法借鉴VINS-Mono的预积分和后端优化，前端视觉里程计改成激光里程计。
基于栅格的，该方案的代表是谷歌开源的cartography，这种方案在室内的机器人定位比较有优势。
基于面元，该方案的典型代表是suma。
基于语义信息，该方案的典型代表是segmap和suma++。

总结，激光SLAM的直接法比较简单，可以用于激光里程计的多程对齐或回环检测;

纯LO的算法目前工业圈很少使用，一般采用多传感器融合的方案，毕竟激光雷达都用了，IMU传感器。

多传感器融合的方案主要用于高精地图的制作，自动驾驶领域一般采用高精地图作为先验来定位，不会同时定位和建图。基于栅格的可以用于移动机器人，在室内环境栅格假设大多数还是有效的，在室外场景的话，一般会采用NDT Map来存储地图。基于语义信息和面元的话，个人不太了解，在工业圈的使用也不多。

2. 2D Lidar SLAM

一般将使用单线雷达建构二维地图的SLAM算法，称为2D Lidar SLAM。大家熟知的2D Lidar SLAM算法有：gmapping, hector, karto, cartographer。通常数据和运动都限制在2D平面内且运动平面与激光扫描平面平行。

gmapping[1]基于粒子滤波的2D激光雷达SLAM，构建二维栅格地图。融合里程计信息，没有回环检测。

优点是在小场景中，计算量小，速度较快。

缺点是每个粒子都携带一幅地图，无法应对大场景（内存和计算量巨大）；如果里程不准或标定参数不准，在长回廊等环境中容易把图建歪。

hector

hector SLAM是完全基于scan-matching的，使用迭代优化的方法来求匹配的最佳位置，为避免陷入局部极值，也采用多分辨率的地图匹配。由于完全依赖于scan matching，要求雷达的测量精度较高、角度范围大，扫描速度较高（或移动速度慢）。噪声多、边角特征点少的场景就很容易失败。原文所提出方法的特点还在于，加入IMU，使用EKF估计整体的6DoF位姿，并根据roll, pitch角将激光扫描数据投影到XY平面，因而支持激光雷达有一定程度的倾斜，比如手持或机器人运动在不是很平整的地面上。

karto

karto是基于scan-matching，回环检测和图优化SLAM算法，采用SPA（Sparse Pose Adjustment）进行优化。关于karto 和 cartographer 的比较，

对Cartographer和Karto两个2D激光SLAM算法的比较表格：

特性	Karto	Cartographer
图优化框架	使用spa (karto_slam) 或 g2o (nav2d) 作为优化库	使用Google的Ceres库构建问题进行优化
前端与后端处理	单线程进行	多线程后端优化（至少4线程）
运动预测	使用odom进行初始位置预测	使用IMU构建预测模型，结合scan matcher与odom构建观测模型，使用UKF进行运动预测
Scan Matcher	使用实时相关性扫描匹配器（实时correlative scan matcher），通过双分辨率的低分辨率和高分辨率两次搜索	先使用实时相关性扫描匹配器，再利用Ceres优化求解，考虑栅格概率、T的偏差、R的偏差
Submap概念	没有submap概念，以keyScan形式存储在sensorManager，无地图缓存，每次计算地图有计算消耗	采用submap概念，依据一定数量的scan初始化一个submap，有子图缓存，会占用内存
Loop-Closure	1. 依据pose和distance信息创建localMap 2. 从Graph中找到相邻的所有vertex 3. 广度优先搜索添加nearLinkedScans 4. 挑选candidateScans 5. loopScanMatcher进行scanTomap匹配 6. 添加调整边（全局闭环） 7. 触发correctPose: spa优化	1. 类似karto，使用实时相关性扫描匹配器 2. 引入branch and bound方式加快闭环查找 3. 基于多分辨率多层树型结构，使用深度优先搜索(DFS)确定闭环 4. 添加相应的闭环约束，利用Ceres优化

这个表格总结了Cartographer和Karto在SLAM算法实现上的一些关键差异，包括它们使用的优化库、处理方式、运动预测、扫描匹配、子图管理以及回环检测策略。这些差异反映了两个算法在设计和实现上的不同选择，以及它们在性能和应用场景上的潜在差异。

cartographer是谷歌开源的激光SLAM框架， 主要特点在于：引入submap，scan to submap matching，新到的一帧数据与最近的submap匹配，放到最优位置上。如果不再有新的scan更新到最近的submap，再封存该submap，再去创建新的submap。回环检测和优化。利用submap和当前scan作回环检测，如果当前scan与已经创建的submap在距离上足够近，则进行回环检测。检测到回环之后用ceres进行优化，调整submap之间的相对位姿。为了加快回环检测，采用分枝定界法。

Cartographer是一款由Google开发的SLAM（Simultaneous Localization and Mapping）算法，它既可以应用于2D激光雷达的SLAM，也可以应用于3D激光雷达的SLAM。尽管Cartographer在2D激光雷达SLAM方面表现出色，尤其是在实时闭环检测方面，但它也支持3D激光雷达SLAM，能够处理多线激光雷达的数据。

3. 3D Lidar SLAM

3D Lidar SLAM主要针对多线激光雷达，以下是一些比较出名的3D Lidar SLAM算法：

LOAM (Lidar Odometry and Mapping)
- LOAM是2014年提出的针对多线激光雷达的SLAM算法。
- 特点：
  - 前端抽取平面点和边缘点，利用scan-to-scan匹配计算帧间位姿，形成里程计。
  - 估计帧间运动，对scan中的每个点进行运动补偿。
  - 生成map时，使用里程计信息作为submap-to-map的初始估计，再通过submap和map之间的匹配进行优化。
- LOAM在提出时尚未加入回环优化。
A-LOAM
- A-LOAM是LOAM的高级实现，使用Eigen和ceres-solver简化代码实现。
LeGO-LOAM
- LeGO-LOAM在LOAM的基础上进行了改进，主要增加了：
  - 地面点分割和点云聚类去噪。
  - ICP回环检测和gtsam优化。
LOAM-livox
- 由大疆在2019年针对小视场（FOV）Lidar提出的算法。
- 特点：
  - 添加策略提取更鲁棒的特征点，包括忽略畸变区域、剔除反射异常点、剔除与平台夹角过小的点等。
  - 与LOAM相似，有运动补偿和剔除匹配度不高的点后，再优化求解相对位姿。
VLOAM
- VLOAM是视觉和激光雷达紧耦合的SLAM方案，结合了视觉信息和激光雷达数据，以提高SLAM的准确性和鲁棒性。

这些算法各有特点，适用于不同的应用场景和需求。LOAM及其衍生算法在多线激光雷达SLAM领域有着广泛的应用，而VLOAM等视觉与激光雷达融合的SLAM算法则在需要更高准确性和鲁棒性的场景中显示出优势。

4. GMapping

GMapping 是一种基于粒子滤波器的二维 SLAM 算法，全称为 Grid-based FastSLAM ，由 OpenSLAM 项目开发，专门用于机器人在未知环境中的同时定位与建图。GMapping 是 ROS 系统中常用的开源 SLAM 算法之一，主要适用于 2D 环境建图和定位。

1. GMapping 系统架构

GMapping 采用粒子滤波器的框架，结合了机器人运动模型和传感器观测数据，构建全局一致的地图。其系统架构主要由以下几个模块组成：

1.1 粒子滤波器Particle Filter

GMapping 使用 粒子滤波器 来跟踪机器人的位姿。每个粒子代表一个机器人位姿假设，并根据传感器数据和运动模型进行更新。
每个粒子都有自己对应的地图和机器人位姿估计。通过加权多个粒子的位姿，系统最终生成全局一致的地图。

1.2 运动模型Motion Model

在每次机器人运动之后，GMapping 根据运动模型（通常是里程计数据）来对所有粒子的位姿进行预测更新。粒子滤波器通过"预测-更新"的过程处理机器人的运动不确定性。
运动模型考虑了机器人运动中的噪声和误差，通常使用的是里程计模型，即通过轮子转动估计机器人位置。

1.3 传感器模型Sensor Model

GMapping 依赖于激光雷达传感器，通过扫描匹配的方式更新每个粒子的权重。传感器模型用于评估粒子所代表的位姿假设与实际传感器数据的匹配程度。
对于每个粒子，计算激光扫描与已生成地图的匹配分数。匹配分数越高，该粒子的权重越大。

1.4 地图更新Map Update

GMapping 使用 栅格地图（Grid Map），将环境离散化为二维网格，每个网格单元表示空间中的一个位置。
每个粒子都有自己的局部地图。根据激光雷达的观测结果和粒子的位姿，地图会在每一帧进行更新，逐步形成机器人的全局环境地图。

1.5 重采样Resampling

随着时间推移，某些粒子的权重会远远大于其他粒子。为了避免粒子退化问题（即大量粒子失效），GMapping 定期进行重采样，将权重较大的粒子复制更多副本，而权重较小的粒子会被删除。这确保了系统的稳定性和收敛性。

1.6 闭环检测Loop Closure

GMapping 支持简单的闭环检测，当机器人回到某个之前已经访问过的区域时，它会尝试通过匹配激光雷达数据和已有的地图，纠正位姿估计中的漂移。
由于闭环检测较为基础，GMapping 适合中小规模的场景，在大型或复杂场景中的表现可能不如后续的 SLAM 系统（如 Cartographer）。

2. GMapping 的特点

2.1 基于粒子滤波器的 SLAM

GMapping 使用 FastSLAM 算法，该算法是基于粒子滤波器的，能够同时跟踪多个位姿假设（通过多个粒子）并生成相应的地图。
该方法通过多个粒子来估计机器人位置和地图，同时应对位置的不确定性，尤其是在动态或噪声较大的环境中具有较强的鲁棒性。

2.2 高效的 2D 建图

GMapping 的设计初衷是为了高效的二维 SLAM，能够在具有噪声的传感器数据（如激光雷达）下生成精确的栅格地图。
相比于传统的 SLAM 算法，GMapping 在中小规模的环境中具有较高的精度，且适合资源受限的设备。

2.3 实时性

GMapping 在粒子滤波器的基础上进行了多项优化，可以实时处理激光雷达数据，适合移动机器人实时导航应用。
尽管实时性较好，但由于使用粒子滤波器，计算量会随粒子数的增加而增加，在大型或复杂环境中实时性会有所下降。

2.4 闭环检测能力有限

GMapping 支持简单的闭环检测，但相比于像 Cartographer 这样的系统，其闭环检测能力较为基础，适合较小或中等规模的场景。
当地图中的闭环较复杂或环境较大时，闭环检测的效果会显著下降，导致累积误差无法有效纠正。

2.5 开源与 ROS 集成

GMapping 是 ROS（Robot Operating System） 社区中的一个重要模块，易于在 ROS 环境中进行部署和使用。
其开源的特点使得开发者可以轻松集成到自己的机器人平台中，进行二次开发。

3. GMapping 的优缺点总结

特点	优点	缺点
基于粒子滤波器的 SLAM	能够处理带有较多噪声和不确定性的传感器数据，适用于具有不确定性和动态环境的场景。	随着粒子数增多，计算复杂度迅速增加，难以处理大型复杂环境。
二维栅格地图	提供高效的二维环境建图，在中小规模场景中具有较高的精度。	仅支持二维建图，不适合三维场景或需要三维建图的应用。
实时性强	适合资源受限的硬件平台，在大多数移动机器人应用中可以实现实时性要求。	在大型或复杂环境下，实时性可能下降，计算复杂度和延迟增加。
闭环检测能力有限	支持基础的闭环检测，可以在简单或中小规模的环境中纠正累积误差。	闭环检测功能较弱，难以在大型或复杂环境中有效纠正漂移，导致较大累积误差。
ROS 集成与开源性	完美集成在 ROS 系统中，易于部署和开发，适合多种机器人平台应用。	对于新手用户，调试和参数调整可能需要一定的学习曲线。

4. 典型应用场景

室内移动机器人：例如家庭机器人、仓库机器人等，通常在二维平面环境中移动，GMapping 能够实时生成精确的地图。
服务机器人导航：GMapping 提供的高效 2D SLAM 可以用于服务机器人在狭小或中等规模的环境中自主导航。
中小型场景下的 2D SLAM：在复杂度较低的环境中（如办公楼、商场等），GMapping 能够提供高效的定位和建图服务。

GMapping 由于其简单高效，适用于中小规模的 2D 环境，特别是在计算资源有限或对闭环检测要求不高的场景中表现优异。然而，在需要更复杂的全局优化或处理大型三维环境时，可能会显得力不从心。

5. Cartographer

Cartographer 是 Google 开源的一款用于 SLAM（Simultaneous Localization and Mapping） 的系统，能够同时使用激光雷达（LiDAR）和 IMU 进行二维或三维环境建图和定位。它的架构设计非常模块化，能够适应多种传感器输入，提供高精度的实时建图和定位。以下是 Cartographer 系统的主要架构和特点：

1. 系统架构

Cartographer 的系统架构主要包括以下几个模块：

前端系统（Front-End System）：
- 主要负责传感器数据的预处理与初步数据融合。
- 使用 IMU、LiDAR 和里程计Odometry等传感器的数据，来估计局部位姿并进行短时间尺度的建图。
- 具体包括：
  - 扫描匹配（Scan Matching）：对激光雷达数据进行匹配，通过与前一帧数据的匹配来估计机器人的移动。
  - 轨迹优化：使用 IMU 提供的加速度、角速度等数据来增强位姿估计的精度，减少短时间漂移。
后端系统（Back-End System）：
- 主要处理全局优化和大规模图结构的维护。
- 使用Pose Graph SLAM的技术来进行全局约束的优化。
- 全局回环检测（Loop Closure）：通过识别已经经过的地方，减少累积误差，形成全局一致的地图。
- 全局优化（Global Optimization）：在检测到回环时，对整个轨迹进行图优化，更新之前的位姿估计，减少误差。
传感器数据处理模块：
- 支持多种传感器（2D/3D LiDAR、IMU、里程计等）同步采集数据并融合处理。
- 对 IMU 数据进行积分，帮助纠正 LiDAR 数据中的短期漂移。
轨迹管理与优化（Pose Graph Management & Optimization）：
- Cartographer 使用 Pose Graph 的方式管理机器人的运动轨迹。每一帧激光雷达数据都作为图中的节点，边则代表机器人的运动变化。
- 每当检测到回环时（即机器人返回了先前已经到过的位置），通过图优化方法减少长期累计的误差。
局部与全局地图生成（Local and Global Map Generation）：
- 局部地图由前端系统生成，描述机器人短期内的环境变化。
- 全局地图则由后端系统负责，通过优化后的位姿信息进行融合，形成一致性更好的大规模地图。

2. Cartographer 的特点

传感器融合：
- 支持多种传感器，包括 LiDAR、IMU、Wheel Odometry，并能够灵活结合不同传感器的数据，提供更高精度的 SLAM 解决方案。
- IMU 和 LiDAR 数据的同步融合，有助于在短期内提升运动估计的精度。
回环检测与全局优化：
- 回环检测是 Cartographer 系统的核心特点之一，当机器人再次经过某个地方时，系统能够检测到并优化全局地图，减少累积误差。
- Pose Graph SLAM 的使用使得系统能够在全局范围内优化位姿，并通过闭环校正来减少漂移。
2D 和 3D SLAM 支持：
- Cartographer 支持二维和三维的 SLAM ，分别适用于不同的场景：
  - 2D SLAM 主要用于平面环境，例如室内机器人导航。
  - 3D SLAM 适用于更复杂的三维环境，例如无人车和无人机的应用。
模块化设计：
- Cartographer 的架构高度模块化，便于开发者根据自己的需求进行定制和扩展。无论是修改传感器数据处理的细节，还是更改全局优化策略，开发者都可以灵活调整系统架构。
实时性与精度：
- Cartographer 通过高效的前后端分离处理机制，能够实现实时 SLAM，同时保持较高的精度。通过前端的快速位姿估计，后端则能在稍后进行全局优化，减少系统延迟。
开源与社区支持：
- Cartographer 是一个开源项目 ，具有活跃的社区和良好的文档支持，便于开发者进行二次开发或研究。

3. 典型应用场景

室内机器人导航：Cartographer 的 2D SLAM 方案在平面环境中表现优异，适用于清洁机器人、配送机器人等。
无人车导航：3D SLAM 能够适应复杂的室外环境，适用于无人车、自动驾驶技术。
无人机：通过融合 IMU 和 LiDAR 数据，Cartographer 也适用于无人机的环境感知与自主飞行。

4. 优缺点总结

特点	优点	缺点
传感器融合	提供多种传感器融合，提高定位与建图精度	对传感器数据的时间同步要求较高
回环检测与全局优化	高效的回环检测和全局图优化，减少长时间漂移	全局优化需要较多的计算资源，回环检测有延迟
2D/3D SLAM 支持	支持多种环境，适应不同应用场景	在复杂三维环境中，计算开销较大
实时性	前后端分离设计，前端快速处理数据，保持较高的实时性	在较大场景中，实时性可能受到全局优化和图优化步骤的影响
开源与可扩展性	开源社区支持，便于研究和开发，架构模块化设计，容易进行二次开发	部分功能需要根据具体应用场景进行调整，默认配置可能不适合所有应用场景

Cartographer 的架构和特点使其成为适用于多种应用场景的强大 SLAM 系统，尤其适合在需要高精度地图和长期一致性位姿估计的环境中应用。