glass-LIO：为“被读懂”而写的激光惯性里程计——10 篇教学文档、200 行解算器与被完整记录的 bug

大多数 SLAM 代码是为"跑起来"而写的，而开源项目 glass-LIO 的目标恰好相反：它是为"被读懂"而写的。这是一套针对 Livox MID-360 激光雷达的扫描到地图（scan-to-map）激光惯性里程计（LiDAR-Inertial Odometry, LIO），作者 Timothée Fréville 在代码之外写了 10 篇配套文档，把每一个不那么显而易见的决策——包括那些后来被证明是错的决策——都白纸黑字地记录下来并给出理由。用作者的话说："真正的主题是流形上的李代数与最小二乘，通过一个系统来教学，在这个系统里即使把它们搞错了，输出看起来仍然是合理的。"

这个领域为什么难：所有严重的 bug 都产生"合理的输出"

glass-LIO 的 README 开篇就点破了状态估计领域最反直觉的事实："这个项目里每一个严重的 bug 都产生了看起来合理的输出，没有一个会让程序崩溃。"

• 符号翻转的雅可比矩阵，照样收敛；
• 悄悄丢失了一阶项的雅可比矩阵，照样能跑；
• 把平面拟合成垂直于真实墙面的平面，ICP 依然有东西可以"啃"。

没有堆栈轨迹、没有 NaN、没有红色报错——只有一条安静而自信地错误的轨迹。正是这个事实决定了整个项目的架构、测试与文档的写法。

系统做什么：六阶段流水线

Livox 的一次扫描是约 100 毫秒的连续扫描（sweep），而不是一张瞬时快照。glass-LIO 的流水线因此设计为六个阶段：

整个循环首尾相接——这既是它能工作的原因，也是它的核心风险所在。当前状态：在测试 bag 上实时（10 Hz）运行，零丢帧、零发散，RMSE 稳定在约 0.13 m。

为什么值得读：五个与众不同的设计选择

1. 每个阶段都有配套文档，按执行顺序排列。六篇阶段文档加一篇解算器文档，每一篇解释的是该阶段埋下的陷阱，而不仅仅是它运行的代码。

2. bug 被记录下来而不是藏起来——包括那些至今仍存在的：紧耦合（tight coupling）已经构建完成、单元验证通过，但"在真实 bag 上会发散"，而且"为什么发散"被写了下来。

3. 不用 Ceres、不用 GTSAM、不用 g2o。流形最小二乘解算器用不到 200 行 Eigen 写成（glass_core/include/glass_core/gauss_newton.hpp），它就是让你读的。

4. 测试是"神谕"（oracle），不是冒烟测试。有限差分法钉死每一个雅可比矩阵；还做了变异测试（mutation testing）来确认测试真的能发现问题。

5. 一条命令跑起整个节点。Dockerfile + devcontainer 锁定 ROS 2 Jazzy，附带校验和验证的测试 bag。

解算器：流形上的高斯-牛顿

这是整个仓库数学含量最高的部分，也是作者最想让你读的部分。问题表述：给定状态 $\mathbf{T} \in SE(3)$ 和一簇标量残差 $r_i(\mathbf{T})$，求最小化鲁棒加权平方和的状态：

$$\mathbf{T}^\star = \arg\min_{\mathbf{T}} \; F(\mathbf{T}), \qquad F(\mathbf{T}) = \tfrac{1}{2}\sum_i w_i \, r_i(\mathbf{T})^2 \tag{1}$$

它比统计学教材里的最小二乘难在两点：$r_i$ 关于 $\mathbf{T}$ 非线性（没有闭式解，必须迭代）；$\mathbf{T}$ 生活在流形上而非 $\mathbb{R}^n$ 中（你甚至不能直接给位姿"加上"一个修正量）。

在每个残差处线性化，$r_i(\boldsymbol{\xi}) \approx r_i + \mathbf{J}_i \boldsymbol{\xi}$，代入代价函数后对 $\boldsymbol{\xi}$ 是二次的，可精确最小化。求导置零得到法方程（normal equations）：

$$\mathbf{H}\,\boldsymbol{\xi} = \mathbf{b}, \qquad \mathbf{H} = \sum_i w_i \,\mathbf{J}_i^\top \mathbf{J}_i, \qquad \mathbf{b} = -\sum_i w_i \,\mathbf{J}_i^\top r_i \tag{2}$$

这里形状本身就是重点：无论堆多少残差——3000 个对应点还是 20000 个——$\mathbf{H}$ 永远是 $n \times n$（SE(3) 上是 6×6，紧耦合是 18×18）。每个残差贡献一个秩 1 外积 $\mathbf{J}_i^\top\mathbf{J}_i$，折叠进固定尺寸的累加器。内存是常数级的。

高斯-牛顿到底丢掉了什么？真实海森矩阵是 $\nabla^2 F = \underbrace{\sum_i w_i \mathbf{J}_i^\top \mathbf{J}_i}_{\text{保留}} + \underbrace{\sum_i w_i r_i \nabla^2 r_i}_{\text{丢弃}}$。被丢弃的项被 $r_i$ 缩放，因此当解处残差很小时它就消失了——这正是收敛后 ICP 的状态：几厘米的残差对着米级的几何，$r_i\nabla^2 r_i$ 相对 $\mathbf{J}_i^\top\mathbf{J}_i$ 可以忽略。于是我们用"一个雅可比的代价"换到了牛顿法的曲率。文档同时诚实地指出反面：大残差问题（错误关联、糟糕的初始猜测）会让丢弃项变得重要，此时高斯-牛顿可能走出使代价上升的一步。

配准：为什么是点到平面

配准阶段的预测用恒速模型加陀螺仪积分给出初值，把初值送进 ICP 的收敛盆：

$$\mathbf{R}_{\text{guess}} = \mathbf{R}_{\text{pose}} \cdot \Delta\mathbf{R}_{\text{imu}}, \qquad \mathbf{t}_{\text{guess}} = \mathbf{t}_{\text{pose}} + \mathbf{v}\,\Delta t \tag{3}$$

ICP 是局部优化器，收敛盆很小；盆外它不会大声失败，而是锁定到错误的墙上，报告一个自信、低残差、完全错误的配准结果。残差采用点到平面形式——因为激光雷达永远不会重采样同一个物理点，点到点的"对应"根本没有正确答案；点到平面只惩罚沿法线的距离：

$$r_i(\mathbf{T}) = \mathbf{n}_i^\top \left( \mathbf{T}\,\mathbf{p}_i - \mathbf{c}_i \right) \tag{4}$$

每个对应点一个标量而非三个分量，这也是收敛更快的原因：代价曲面没有来自切向错配的虚假极小。左扰动线性化给出 $1\times 6$ 雅可比：

$$\mathbf{J}_i = \begin{bmatrix} \mathbf{n}_i^\top & (\mathbf{q}_i \times \mathbf{n}_i)^\top \end{bmatrix} \tag{5}$$

文档还特意警告：旋转块是 $(\mathbf{q} \times \mathbf{n})$ 而不是 $(\mathbf{n} \times \mathbf{q})$——调换它们，所有旋转方向的更新都会反向，而这照样会收敛到某个地方。对应关系搜索不用 KD 树，而是哈希查表：地图在每个体素缓存一个平面，把点哈希到体素、扫描 27 邻域取最近有效平面，每点常数工作量——这是配准足够快的最大原因。

去畸变：先算一笔账

为什么必须先做去畸变？文档给出了实测数据：一次扫描内的角度变化 $\Delta\theta \approx 2.8°$（0.049 rad），40 米处的误差近似弧长 $\text{error} \approx r \cdot \Delta\theta \approx 40 \times 0.049 \approx 2.0\ \text{m}$。两米的涂抹——配准阶段根本无法从这样的畸变中恢复，因为它是在把一张扭曲的云匹配到一张扭曲的地图。

紧耦合：一场被完整记录的教学失败

第 7 篇文档记录了把 IMU 作为残差放进同一组法方程（而非仅仅当作初值提示）的尝试：流形上的预积分、15 自由度状态、$J_r^{-1}$ 右雅可比逆。结局诚实地写在 README 里——紧耦合构建完成、单元测试通过，却在真实 bag 上发散；两个已修复的 bug（接错线的先验、把速度推到约 40 m/s 的激光雷达欠信任：$lidar_sigma$ 设成 5 cm 而 Livox 真实噪声约 2 cm，且点到平面残差没有速度列）修复后，紧耦合路径与松耦合路径在测试序列上打平（429 m 对 434 m），但因为"在这里只是打平而不是可证明地更好"，它默认保持关闭，直到在真正退化的数据上验证。作者总结："这次失败教给我的，比成功会教给我的更多。"

三个烧掉真实调试时间的传感器地雷

每一个都产生"合理输出"而非报错：

• 逐点时间戳是纳秒——而且时间不在 $intensity$ 字段里（在这个驱动里那是真实反射率）。搞混单位则每次查询都被钳制，去畸变悄悄变成空操作。
• $linear\_acceleration$ 的单位是 g 而非 m/s²——违反 $sensor\_msgs/Imu$ 规范，静止时实测 $|\mathbf{a}| = 0.997$。搞错则每个加速度小 9.81 倍。
• Release 构建定义 NDEBUG，会删除所有 assert()。基于 assert 的测试套件曾经"在什么都不检查的情况下通过"，直到 CMake 强制 -UNDEBUG。

工程结构：ROS 无关的估计引擎

估计引擎被拆进 glass_core/——纯 CMake、无 ROS 的库，只包含流形上的数学（高斯-牛顿解算器、SO(3) 右雅可比、Forster 式 IMU 预积分、15 自由度导航状态、陀螺零偏/重力初始化），这样未来的第二个前端（视觉惯性版 $glassvio$）可以共享完全相同的解算器而不是复制它。glasslio 是建在其上的激光雷达前端，通过 $add\_subdirectory(glass\_core)$ 引入。依赖仅 ROS 2 Jazzy、Eigen 3、PCL（common/io/filters）与内置的 Sophus——没有 Ceres，没有 GTSAM。

测试 bag 也不是作者自己的数据，而是日本产业技术综合研究所（AIST）Kenji Koide 发布在 Zenodo 上的"Driving SLAM Test with Livox MID360"（CC-BY-4.0），一段驾驶场景序列，由 scripts/download_bag.sh 下载（约 1.4 GB，可断点续传、校验和验证）。

对机器人开发者的启发

• "看起来合理的输出"是状态估计最大的敌人。glass-LIO 给出的对策是：用有限差分神谕钉死每个雅可比、用变异测试验证测试本身、把每个决策和每个 bug 都写成文档。
• 教学型代码可以是真的。这个项目既不是玩具也不是论文附录：实时 10 Hz、RMSE 0.13 m、完整的工程化（Docker、devcontainer、校验和数据集）。
• 失败记录与成功记录同等重要。紧耦合章节完整保留了"诊断本身就是陷阱"的过程，这对所有做传感器融合的工程师都是稀缺教材。

来源：GitHub — Tim-HW/glass-lio（MIT 许可证）。测试数据集：Koide, K. (2025). Driving SLAM Test with Livox MID360 [Data set]. Zenodo. https://doi.org/10.5281/zenodo.14841855（CC-BY-4.0）。