6. 源码走读：一帧激光数据的完整旅程

上一篇：5. gmapping的两大改进 ｜ 下一篇：7. 实战：跑起来与调参

原理懂了，现在把 第 5 篇 的算法图对号入座到真实代码。gmapping 分两层：外层 ROS 壳（slam_gmapping 包）+ 内核算法库（openslam_gmapping）。

一、代码地图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
slam_gmapping/            ← ROS 壳: 订阅话题、格式转换、发 TF 和地图
├── main.cpp              入口
├── slam_gmapping.cpp     startLiveSlam / laserCallback / addScan / updateMap
└── ...
openslam_gmapping/        ← 算法内核: RBPF 全在这
├── gridfastslam/
│   ├── gridslamprocessor.{h,cpp}   ⭐ GridSlamProcessor::processScan() 核心循环
│   └── particle.h                  粒子(位姿+权重+地图节点)
├── scanmatcher/
│   └── scanmatcher.cpp             ⭐ 扫描匹配器(打分+爬山优化)
├── grid/
│   ├── harray2d.h                  ⭐ 层次化栅格(地图树, 省内存的秘诀)
│   └── slam13d...
└── utils/ (point.h 等)

二、外层：ROS 壳的接力赛

2.1 startLiveSlam()：起跑线

1
2
3
4
5
6
7
8
9
void SlamGMapping::startLiveSlam() {
  // 发布: /map(锁存), /map_metadata, /entropy, dynamic_map 服务
  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  // 订阅: /scan, 经 tf::MessageFilter 等到 odom 可用才进回调
  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
  scan_filter_->registerCallback(... &SlamGMapping::laserCallback ...);
  // 单独线程周期发布 map->odom 变换
  transform_thread_ = new boost::thread(... publishLoop ...);
}

三个注意点：

  • /map 用 latched 发布（地图变化慢，新订阅者自动收最后一版，见第 2 篇）；
  • tf::MessageFilter：激光帧攒着，直到能查到它时间戳对应的里程计位姿才放行进回调（时间同步）；
  • throttle_scans_：每 N 帧处理一帧（降频）。

2.2 laserCallback()：每帧入口

1
2
3
4
5
6
7
8
9
10
11
12
void SlamGMapping::laserCallback(scan) {
  if ((laser_count_ % throttle_scans_) != 0) return;   // 降频
  if (!got_first_scan_) { initMapper(*scan); ... }     // 首帧: 初始化
  if (addScan(*scan, odom_pose)) {                     // ⭐ 核心处理
    // 取最优粒子位姿, 计算地图系修正量
    mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
    tf::Transform laser_to_map  = ...(mpose)... .inverse();
    tf::Transform odom_to_laser = ...(odom_pose)...;
    map_to_odom_ = (odom_to_laser * laser_to_map).inverse();  // ⭐ TF 修正量
    if (!got_map_ || 到了更新间隔) updateMap(*scan);           // 发布地图
  }
}

map_to_odom_ 那行就是第 2 篇 TF 树的落点：最优粒子位姿（map 系）与里程计位姿（odom 系）的差。

2.3 addScan()：格式桥

getOdomPose() 查 TF 拿该时刻里程计位姿 -> ROS 激光消息转成 GMapping::RangeReading（含位姿+距离数组）-> 调 gsp_->processScan(reading) 进入内核。

三、内核：processScan() – 第 5 篇算法图逐行对号 ⭐⭐

GridSlamProcessor::processScan() 是整个 gmapping 的心脏，流程与 第 5 篇 的 mermaid 图一一对应：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
processScan(read) {
  // [0] 里程计运动模型: 上一有效位姿 -> 当前位姿的相对运动
  m_motionModel.drawFromMotion(particles, relPose, m_odoPose);

  // [1] ⭐改进一: 每个粒子的"扫描匹配 + 高斯近似提议分布"
  for (每个粒子 it) {
    // (a) 扫描匹配: 在粒子地图上爬山找最优位姿(小于 minimumScore 则放弃)
    if (m_computeCovariance ...) { ... }
    matched = m_matcher.optimize(corrected, it->map, it->pose, plainReading);
    // (b) 在匹配点附近撒 K 个候选, 逐一打分
    for (j = 0; j < m_kernelSize; j++) {
      //   候选位姿 = 匹配点 + 高斯扰动
      //   似然 = 激光得分(likelihoodAndScore) × 里程计似然(invMeasurementModel...)
      ...
    }
    // (c) K 个带权候选 -> 拟合高斯 N(μ, Σ)
    sampleGaussian(...);
    // (d) 从该高斯采样新位姿 = 该粒子的"生" ⭐
    it->pose = drawfrom(...);
    // (e) 权重更新(重要性校正, 第5篇 1.4 公式)
    it->weight *= ...;
  }

  // [2] 权重归一化 + 体检
  normalize();
  neff = getNeff();               // ⭐ Neff 计算

  // [3] ⭐改进二: 自适应重采样
  if (neff < m_resampleThreshold) {
    resample();                   // 抽签重组粒子(地图节点随树复制)
    updateTreeWeights(false);
  }

  // [4] 更新活跃区域 + 每粒子地图逐格子更新
  updateActiveArea / registerScan ...   // Bresenham 光线投射
}

逐段对照：

代码段 对应原理 所在篇
drawFromMotion 里程计运动模型采样 3
m_matcher.optimize 扫描匹配”对齐” 5 §1.3
sampleGaussian + 采样 改进提议分布（高斯近似） 5 §1.3
weight *= ... 重要性权重校正 5 §1.4
neff < threshold -> resample 自适应重采样 5 §2
registerScan 逐格子贝叶斯建图 4 §2.3

四、彩蛋：地图树（HierarchicalArray2D）– 内存救命术

每个粒子一张地图，重采样时”复制地图”岂不内存爆炸？openslam 的解法：

  • 栅格不是平铺一个大数组，而是树状结构：叶子=格子块，粒子的地图 = 指向共享树的引用；
  • 重采样复制粒子时只复制指针（写时复制 copy-on-write）；
  • 只有当两个粒子后续观测不同、要改写同一块格子时，才真正分裂出私有副本；
  • 配合”活跃区域 active area”（只更新激光扫到的区域），进一步把更新量压到最小。

这是 gmapping 能在 2007 年的电脑上实时跑的工程支柱，也是面试常问点。

五、一帧激光的旅程（总览图）

flowchart TD
    A["🔊 /scan 激光帧"] -->|tf::MessageFilter 时间同步| B["laserCallback"]
    B -->|首帧| C["initMapper 初始化"]
    B -->|后续帧| D["addScan: 转格式+查TF"]
    D --> E["⭐ processScan (内核)"]
    E --> F["运动模型采样"]
    F --> G["扫描匹配+高斯提议分布"]
    G --> H["权重更新 + Neff"]
    H -->|Neff低| I["重采样(地图树共享)"]
    H -->|Neff高| J["跳过重采样"]
    I & J --> K["更新各粒子地图"]
    K --> L["最优粒子 -> map->odom TF"]
    K --> M["周期发布 /map 栅格图"]
    style E fill:#fef7e0,stroke:#b06000
    style G fill:#e8f0fe,stroke:#1a73e8
    style I fill:#fce8e6,stroke:#d93025

📚 参考：源代码解析（本文主线参考，含 startLiveSlam/laserCallback/addScan 源码）、ROS1系列（订阅/发布话题全表）