FAST-LIO 部署（二）——脚本解析和ROS2升级

夜幕龙2025-12-09 8:33

FAST-LIO 在宇树 G1 上部署可以分为两种方法：

  1. 源码部署

  2. docker 部署

由于 FAST-LIO 使用环境为 ubuntu 20.04 + ROS1，而 G1 通用环境为 ubuntu 22.04 + ROS2，所以使用第二种方法可以直接上手，但是考虑到后续源码升级，选择第一种思路的升级版本

第一种思路又可以采用两种方法：

  1. FastLIO-ROS2 + Nav2：参考 FASTLIO2_ROS2 升级 FastLIO 为 FastLIO-ROS2，然后 ROS2 + Nav2 基本思路如下：

  2. 用 FastLIO(ROS2) 做 LIO 里程计 + 累积点云

  3. 用 octomap_server 把 3D 点云转成 2D 投影占据栅格 /projected_map

  4. 用 map_saver_cli 把 2D 栅格保存为 mymap.yaml/.pgm

  5. 用 Nav2 加载 mymap.yaml 导航

整套思路已经完成部署测试，记录在了宇树 G1 部署（六）笔记中

  1. 第二种是这几天正好看到的，高翔老师新出的 CPU 友好型 3D 激光建图与定位模块：Lightning-LM（Lightning-Speed Lidar Localization and Mapping）

🐾 Github：https://github.com/gaoxiang12/lightning-lm

这个项目可以当成"一体化 LIO-SLAM + 2D 栅格 + 闭环 + 轻量后端"的工程

由于： Lightning-LM is a complete laser mapping and localization module，其前端（LIO）：以 AA-FasterLIO 为核心（Faster-LIO 系列的加速版），完成去畸变、特征/体素匹配、紧耦合 IMU 预积分等，实时输出里程计

因此：此笔记在 Lightning-LM 框架中，分析 FasterLIO，方便后续替换

如果后续想继续轻量化自己搭建架构，则继续 1. FastLIO-ROS2 + Nav2 中的研发

首先强调一下，此笔记全是理论逻辑，初看可能一头雾水，但是部署完后再回头看一遍，对于升级优化发 paper 确实很有用

Faster-LIO 是在 FAST‑LIO2、FAST‑LIO 基础上进一步加速、轻量化的 LiDAR-IMU 里程计（LIO）系统

🐾 Github：https://github.com/gaoxiang12/faster-lio

📄 Paper：

https://github.com/gaoxiang12/faster-lio/blob/main/doc/faster-lio.pdf

https://ieeexplore.ieee.org/document/9718203

目录

[1 Lightning-LM 框架](#1 Lightning-LM 框架)

[2 Lightning-LM 架构拆解](#2 Lightning-LM 架构拆解)

[2.1 文件架构拆解](#2.1 文件架构拆解)

[2.1.1 应用层：src/app](#2.1.1 应用层：src/app)

[2.1.2 系统调度层：src/system](#2.1.2 系统调度层：src/system)

[***2.1.3 核心算法层：src/core](#***2.1.3 核心算法层：src/core)

[2.1.4 数据与公共类型：src/common](#2.1.4 数据与公共类型：src/common)

[2.1.5 IO / 适配层](#2.1.5 IO / 适配层)

[2.1.6 可视化 UI：src/ui](#2.1.6 可视化 UI：src/ui)

[2.1.7 杂项工具：src/utils](#2.1.7 杂项工具：src/utils)

[2.2 核心数据流和模块协作](#2.2 核心数据流和模块协作)

[2.2.1 LIO 前端（core/lio）](#2.2.1 LIO 前端（core/lio）)

[2.2.2 建图后端：地图 + 回环 + 图优化](#2.2.2 建图后端：地图 + 回环 + 图优化)

[2.2.3 定位模块（core/localization）](#2.2.3 定位模块（core/localization）)

[2.2.4 3D → 2D 栅格地图（core/g2p5）](#2.2.4 3D → 2D 栅格地图（core/g2p5）)

[2.3 运行模式 & 系统层逻辑](#2.3 运行模式 & 系统层逻辑)

[2.3.1 在线 / 离线程序入口（src/app）](#2.3.1 在线 / 离线程序入口（src/app）)

[2.3.2 system::Slam 和 system::LocSystem](#2.3.2 system::Slam 和 system::LocSystem)

[***2.4 配置体系 YAML → Options / Params](#***2.4 配置体系 YAML → Options / Params)

[2.4 数据流总结](#2.4 数据流总结)

[2.4.1 离线建图（run_slam_offline）](#2.4.1 离线建图（run_slam_offline）)

[2.4.2 在线纯定位（run_loc_online）](#2.4.2 在线纯定位（run_loc_online）)

[3 单独模块升级策略](#3 单独模块升级策略)

[3.1 Lightning-LM 的模块化程度（结构层级）](#3.1 Lightning-LM 的模块化程度（结构层级）)

[3.2 System --- "胶水层"](#3.2 System — “胶水层”)

[3.3 模块升级方向（挖坑向）](#3.3 模块升级方向（挖坑向）)

[3 Lightning-LM 效果展示](#3 Lightning-LM 效果展示)

1 Lightning-LM 框架

Lightning-LM 是一个完整的激光建图+定位模块

Lightning-LM特性：

  1. done\] 完整的3D Lidar SLAM，快速的LIO前端（AA-FasterLIO），标配

  2. done\] 实时回环检测，标配，选上的话会进行后端回环检测并闭环

  3. done\] 地图分区动态加载方案，适用大场景

  4. done\] 高频率IMU平滑输出，标配，100Hz

  5. 车辆里程计输入，选配 (TODO)
    
    更多

  6. done\] 轻量优化库miao以及增量式优化（来自g2o，但更轻更快，支持增量优化，不需要重新构建优化模型），标配，在回环、定位中均有用到

  7. done\] 基于外推器和平滑器的高频率输出，平滑因子可调

2 Lightning-LM 架构拆解

Lightning-LM 本质上是一套完整的 3D 激光 SLAM + 定位系统，围绕一个 LIO 前端（AA-FasterLIO）和一个轻量图优化库（miao）搭出整套建图/定位流水线，目标是：

  • 一个核心算法库，同时支撑：

    • 在线 / 离线建图（SLAM）

    • 在线 / 离线纯定位

  • 多线程 + 增量优化 + 分块地图，保证在纯 CPU 上也能 0.8～1.2 个核跑起来

  • ROS2 + Pangolin UI 封装，方便工程集成与可视化

从目录树可以看出，它是一个层次性很强的结构：应用入口 → 系统调度层 → 算法核心 → 公共类型/工具 → 第三方库

Lightning-LM 以 AA-FasterLIO 为前端、以自研 miao 为图优化核心，围绕"分块地图 + 回环 + 动静态分层 + 3D→2D 栅格"的完整 Lidar SLAM/定位系统；上层由 system::Slam/LocSystem 统一调度，下层通过 core/ 与 miao/ 实现算法细节，中间通过 ROS2/bag 封装和 Pangolin UI 做工程化落地

2.1 文件架构拆解

核心文件 src 的目录架构可以拆解概括成这样：

2.1.1应用层：src/app

  • run_slam_online.cc / run_slam_offline.cc

  • run_loc_online.cc / run_loc_offline.cc

  • run_loop_offline.cc, run_frontend_offline.cc, test_ui.cc

  • 这些是不同模式下的主程序入口（ROS2 node / 离线程序），负责：

    • 解析参数和 YAML 配置

    • 创建 system::Slam 或 system::LocSystem

    • 连接 bag 或真实传感器

2.1.2 系统调度层：src/system

  • slam.{cc,h}：建图系统

  • loc_system.{cc,h}：纯定位系统

  • async_message_process.h：系统级异步消息调度

  • 它们把各个算法模块（LIO、回环、地图、定位、UI...）"拼成一条流水线"

***2.1.3 核心算法层：src/core

  1. lio/：LIO 前端

  • eskf.{cc,hpp}：IMU 误差状态卡尔曼滤波 / IEKF

  • imu_processing.hpp：IMU 预积分、插值

  • laser_mapping.{cc,h}：激光因子更新 + 地图维护

  • pointcloud_preprocess.{cc,h}：点云去畸变、滤波、采样

  • pose6d.h：状态表示

  1. ivox3d/：稀疏增量体素地图（FasterLIO 用的 IVox 结构）

  2. g2p5/：3D → 2D 栅格转换（g2p5）

  • g2p5_map, g2p5_grid_data, g2p5_subgrid 等封装 2D 地图块
  1. localization/：

  • localization.{cpp,h}：整体定位流程

  • localization_result.*：定位结果结构

  • pose_graph/：pgo.*, pose_extrapolator.*, smoother.h

  • lidar_loc/：激光定位（scan-to-map，对应在线定位前端）

  • pclomp/ 里是一套多线程 NDT (ndt_omp.*)

  1. loop_closing/：闭环检测与优化
  • loop_closing.{cc,h}
  1. maps/：
  • tiled_map.{cc,h}, tiled_map_chunk.{cc,h}：分块地图管理与动态加载
  1. miao/：轻量图优化库（类 g2o）

  • core/graph/*, solver/*, robust_kernel/*, types/*...

  • 提供边/顶点结构、求解器、鲁棒核等

  1. lightning_math.hpp：一些公用数学工具

2.1.4 数据与公共类型：src/common

  • imu.h, odom.h, nav_state.{cc,h}：传感器和状态建模

  • keyframe.h, loop_candidate.h, measure_group.h：关键帧 / 回环候选 / 对齐好的测量批

  • point_def.h, functional_points.h：点类型定义（带强度、时间等）

  • options.{cc,h}, params.{cc,h}：将 YAML 配置解析成结构化参数

  • timed_pose.h, pose_rpy.h, eigen_types.h, std_types.h 等：基础类型

2.1.5 IO / 适配层

  1. src/io：

  • yaml_io.{cc,h}：读取配置

  • file_io.{cc,h}：读写 PCD、地图等

  • dataset_type.h：数据集枚举

  1. src/wrapper：

  • bag_io.{cc,h}：对 ROS2 bag 的遍历/读取封装

  • ros_utils.h：一些 ROS 相关工具

  1. srv/LocCmd.srv, srv/SaveMap.srv：暴露服务接口（定位命令、保存地图）

2.1.6 可视化 UI：src/ui

  • pangolin_window.* / pangolin_window_impl.*

  • ui_car.*, ui_cloud.*, ui_trajectory.*

  • 使用 Pangolin 做 3D UI，用来展示点云、轨迹和车辆模型，可在 YAML 中开关 system.with_ui / system.with_2dui

2.1.7 杂项工具：src/utils

  • timer.{cc,h}：性能计时（Issues 里大家贴的 log 就是它）

  • pointcloud_utils.*, sync.h：点云操作和时间同步

  • async_message_process.h：更底层的异步队列，供 system 和 app 使用

整体来看，算法核心被放在 core/ ，和 ROS / UI / 数据集强解耦，方便以后复用

2.2 核心数据流和模块协作

2.2.1 LIO 前端（core/lio）

主要职责：把 IMU + LiDAR 同步后，输出流畅的 6 DoF 位姿，并维护局部地图

典型流程：

python 复制代码 
MeasureGroup{IMU, LiDAR}  -->  pointcloud_preprocess
                          -->  IMU Processing / ESKF (eskf)
                          -->  laser_mapping (scan-to-map + map update)
                          -->  nav_state + keyframe + local map(IVox3d)

关键点：

  • 使用 AA-FasterLIO 前端（Anderson Acceleration 强化的 FasterLIO）作为 LIO 算法，结合 ivox3d 稀疏体素地图提高效率

  • MeasureGroup 把时间同步后的激光帧与若干 IMU 数据打包，保证预积分时序正确

  • eskf 实现的是误差状态 Kalman / IEKF，laser_mapping 构建观测 Jacobian 并更新状态，对应 Issues 中 ObsModel (IEKF Build Jacobian) 的计时项

2.2.2 建图后端：地图 + 回环 + 图优化

建图模式下，系统更像：

python 复制代码 
LIO(front-end) --> keyframes & local map
                  |
                  +--> loop_closing (检测闭环、构建约束)
                  +--> pose_graph (pgo + miao optimizer)
                  +--> maps::TiledMap / g2p5 生成全局地图
  1. 地图层（maps/tiled_map*）

  • 地图被划分为多个 TiledMapChunk，支持按需加载/卸载，适合大规模场景

  • 对 LIO 和定位模块暴露统一接口（如：给定位姿取邻域点云）

  1. 回环检测（loop_closing）

  • 使用 loop_candidate, keyframe 这些数据结构，从关键帧集合中寻找回环候选

  • 检测到回环后，构造 "回环边" 交给 pose graph

  1. 图优化（core/localization/pose_graph + core/miao）

  • pgo.{cc,h} + pgo_impl.*：封装了带有增量特性的位姿图优化逻辑

  • 底层依赖 miao：

    • 仿 g2o 的图结构和线性求解器，但更轻、更适配当前项目

    • 支持增量优化，无需重建整个图模型，这对在线回环修正非常关键

Lightning-LM 最新加入的"全局高度约束"也是通过后端图优化实现的：在 loop_closing 里可以配置 with_height，对 Z 轴加 soft constraint 抑制大场景 z 漂移

2.2.3 定位模块（core/localization）

纯定位和 SLAM 的前端是不一样的：

  • 建图时：使用 LIO（点云 + IMU + ivox3d）

  • 定位时：更多使用 scan-to-map / NDT 对现有地图匹配

目录结构里可以看到：

  • lidar_loc/：

    • lidar_loc.{cc,h}：整体激光定位算法封装

    • pclomp/：多线程的 NDT 实现（OpenMP 加速）

  • localization.{cpp,h}：把地图、初值、观测等 glue 在一起，形成完整的定位 pipeline

  • localization_result.*：封装定位结果（位姿、协方差、质量指标等）

  • pose_extrapolator.* + smoother.h：

    • 前者根据 IMU / wheel odom /历史 pose 为下一帧提供预测

    • 后者负责生成高频、平滑的输出轨迹（项目指出 100 Hz IMU 滤波输出）

配合 README 里的说明，定位系统还支持：

  • 动静态图层分离：地图分为静态层 + 动态层，动态层可以短期/中期/永久三种策略：

    • 短期：只保留最近的一段动态点

    • 中期：缓存更长时间

    • 永久：一直积累，用于长期存在的"动态但稳定"结构（如路边停的车）

这部分逻辑主要在 maps/ + localization/ 里实现，通过不同 map chunk 的管理和更新策略来体现

2.2.4 3D → 2D 栅格地图（core/g2p5）

g2p5 可以理解为一个独立的小模块：

python 复制代码 
3D 点云地图 --> g2p5_map/g2p5_subgrid --> 2D grid (map.pgm)

3D 点云地图 --> g2p5_map/g2p5_subgrid --> 2D grid (map.pgm)

  • g2p5_map.*：对整幅 2D 栅格地图的封装

  • g2p5_subgrid.*：局部子栅格块

  • g2p5_grid_data.h：每个栅格的数据结构（占据概率等）

系统可以通过配置 system.with_g2p5 来开启/关闭 2D 地图输出，实时生成并可保存 pgm

2.3 运行模式 & 系统层逻辑

2.3.1 在线 / 离线程序入口（src/app）

几类入口程序：

  1. 建图：

  • run_slam_online.cc

  • run_slam_offline.cc

  1. 定位：

  • run_loc_online.cc

  • run_loc_offline.cc

  1. 只跑前端：
  • run_frontend_offline.cc
  1. 回环测试：
  • run_loop_offline.cc
  1. UI 测试：
  • test_ui.cc

这些文件做的事情大体类似：

  1. 通过 io::yaml_io 读取配置（config/default_*.yaml 等）

  2. 构造 system::Slam 或 system::LocSystem

  3. 对于 在线模式：

    • 使用 wrapper::ros_utils 订阅 LiDAR/IMU 话题

    • 推入系统的消息队列（async_message_process）

  4. 对于 离线模式：

    • 使用 wrapper::bag_io 遍历 ROS2 bag（db3）

    • 将每条消息按时间顺序喂给系统

2.3.2 system::Slam 和 system::LocSystem

它们是整个系统的"中枢神经"：

  1. 管理各个子模块生命周期：
  • LIO / maps / loop_closing / localization / UI / IO / pose graph...
  1. 在内部维护一个或多个处理线程：

  • 消息接收线程：把 ROS/bag 数据转换为 MeasureGroup 或传感器帧

  • 计算线程：按时间顺序取消息，调用 LIO / 定位 / 回环 / 2D 地图等过程

  1. 对外提供接口：

  • topic：发布 TF / Pose / Odometry 等

  • service：

    • SaveMap：保存地图

    • LocCmd：控制定位（重定位、重置等）

多线程相关的关键工具包括：async_message_process.h（系统级）、utils/sync.h（时间同步）、utils/timer.*（性能统计）

***2.4 配置体系 YAML → Options / Params

配置文件在 config/（如 default_nclt.yaml 等），通过 common::options / params 解析成结构体

README 中列了一些核心开关：

  • system.with_loop_closing：是否启用回环

  • system.with_ui / system.with_2dui：是否开启 3D/2D UI

  • system.with_g2p5：是否生成 2D 栅格地图

  • system.map_path：地图存储目录

  • fasterlio.point_filter_num：点云下采样系数

  • g2p5.esti_floor：是否动态估计地面参数

  • g2p5.grid_map_resolution：栅格地图分辨率

  • loop_closing.with_height：开启高度约束，抑制 Z 漂移（2025-11-13 新增）

2.4 数据流总结

2.4.1 离线建图（run_slam_offline）

python 复制代码 
ROS2 bag(db3)
   |
   v (wrapper::bag_io)
MeasureGroup{IMU+LiDAR}
   |
   v (system::Slam)
LIO front-end (core/lio) ----> IVox3d local map
   |                                 |
   |                                 v
   |                          maps::TiledMap
   |                                 |
   +--> keyframes --> loop_closing --+
                    --> pose_graph(pgo + miao)
                                      |
                                      v
                        优化后的位姿图 / 修正地图
                                      |
                                      v
                         g2p5: 3D->2D 栅格地图 (map.pgm)

最终在 data/new_map 目录下得到：

  • global.pcd：可视化用的全局点云

  • map.pgm：2D 栅格

  • 以及块状地图、动态层数据等

2.4.2 在线纯定位（run_loc_online）

python 复制代码 
实时 LiDAR + IMU 话题
    |
    v
wrapper/ros_utils + utils/sync
    |
    v
system::LocSystem
    |
    +--> pose_extrapolator (高频预测)
    |
    +--> lidar_loc (NDT_OMP) 对 maps::TiledMap 扫描匹配
           |
           +--> localization_result (位姿 + 质量)
           |
           +--> smoother (轨迹平滑，100 Hz 输出)

同时根据配置选择：

  • 是否使用动静态图层分离（更新 dynamic layer）

  • 是否启用回环和图优化（对于特大场景，可以缓解累计漂移）

3 单独模块升级策略

Lightning-LM 的架构高度模块化，而且作者在设计时刻意把"算法核心"与"系统调度 / IO / 框架"做了彻底分离，所以可以单独替换、升级某个模块，而不破坏其他部分

3.1 Lightning-LM 的模块化程度（结构层级）

Lightning-LM 可以粗略分为 5 层：

python 复制代码 
App 层（online/offline main）
↓
System 层（Slam / LocSystem 流水线调度）
↓
Core 算法层（LIO / Localization / LoopClosing / Maps / g2p5）
↓
Math & Optimization（miao 图优化）
↓
IO、Wrapper、Utils

其中 核心算法层 就是想升级模块的主要战场

每个模块几乎都是目录级隔离 + 接口隔离 + 数据结构隔离，例如：

  • core/lio：LIO 前端

  • core/localization：定位系统（NDT + 匹配框架）

  • core/loop_closing：回环检测

  • core/maps：分块地图 / 动静态地图

  • core/g2p5：3D → 2D 栅格地图

  • core/miao：图优化

这些模块之间的依赖关系非常清晰，主要通过以下数据结构沟通：

  • NavState

  • MeasureGroup

  • KeyFrame

  • TiledMapChunk

  • LocalizationResult

  • PoseGraph / Vertex / Edge

👉 因此替换某个模块并不会影响整套 SLAM 运行，只要维持输入输出类型一致

3.2 System --- "胶水层"

System 层（system::Slam / system::LocSystem） 做的事情是：

  • 维护线程

  • 调用 LIO / Loop / PGO / Map

  • 发布结果 / 更新地图

它不关心模块内部实现，只关心模块的"输入输出 API"，所以在修改某个模块时，一般不会动 system 层。也就是说：模块替换不会破坏 main flow，只要保持接口一致

3.3 模块升级方向（挖坑向）

  1. 升级LIO（core/lio），耦合情况：

  • 输入：MeasureGroup

  • 输出：NavState + local map

  • 下游：loop_closing 与 map 层依赖关键帧与局部地图

➡ 可以独立替换，只要保持 laser_mapping 的输出格式一致

  1. 升级 定位模块（core/localization），耦合情况：

  • 输入：点云 + 地图块（tiled map）

  • 输出：LocalizationResult（pose、质量、协方差）

➡ 完全可插拔，是 Lightning-LM 最容易独立替换的模块

  1. 升级 地图模块（core/maps）

➡ 只要对外仍然提供 "给定 pose → 返回周围点云块" 的接口，就是可替换的

  1. 升级 回环检测（core/loop_closing）

➡ 回环只要求：输入是关键帧，输出是loop_constraints（通常是 RelativePose + 信息矩阵）。因此是完全可 modular 的

  1. 升级 图优化（core/miao）

➡ 因为 miao 的接口非常接近 g2o，只要保持 PoseGraph 的输入接口一致，就是可替换的

  1. 升级 3D→2D 栅格地图（core/g2p5）

➡ g2p5 是完全独立的小模块，不依赖 SLAM 主流程：​​​​​​

3 Lightning-LM 效果展示

相关推荐
HenryLiuu4 小时前
ROS NOETIC 安装教程
机器人
秋刀鱼 ..6 小时前
第七届国际科技创新学术交流大会暨机械工程与自动化国际学术会议（MEA 2025）
运维·人工智能·python·科技·机器人·自动化
万俟淋曦18 小时前
【论文速递】2025年第33周(Aug-10-16)(Robotics/Embodied AI/LLM)
人工智能·深度学习·ai·机器人·论文·robotics·具身智能
ModestCoder_21 小时前
强化学习 Policy 的 Tracking 能力全解析，以Legged_gym为例解说Policy的训练流程
人工智能·算法·自然语言处理·机器人·具身智能
强化学习与机器人控制仿真21 小时前
ProtoMotions 3 入门教程（一）开源 GPU 加速人形机器人强化学习仿真训练框架
人工智能·stm32·深度学习·机器人·强化学习·人形机器人·模仿学习
沫儿笙1 天前
川崎焊接机器人保护气体省气
人工智能·机器人
一颗小树x1 天前
【机器人】复现 RoboBrain2.0 具身大脑模型 | 统一感知、推理和规划能力
机器人·复现·robobrain2.0·具身大脑模型
万俟淋曦1 天前
【论文速递】2025年第34周(Aug-17-23)(Robotics/Embodied AI/LLM)
人工智能·深度学习·机器学习·ai·机器人·论文·具身智能
热门推荐
01GitHub 镜像站点02【超详细教程】手把手教你从微软官网免费下载Windows 10官方原版ISO镜像（2025最新版）03安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）04UV安装并设置国内源05React CVE-2025-55182漏洞排查与修复指南06Linux下V2Ray安装配置指南07BongoCat - 跨平台键盘猫动画工具08智能库存管理的需求预测模型：从业务痛点到落地代码的完整实践09从入门到实战：Gemini 3 使用指南速览10在VSCode配置Java开发环境的保姆级教程（适配各类AI编程IDE）