Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction

[ IEEE ICRA ] [ arXiv ] [ Video ] [ Presentation ]

DLIO is a new lightweight LiDAR-inertial odometry algorithm with a novel coarse-to-fine approach in constructing continuous-time trajectories for precise motion correction. It features several algorithmic improvements over its predecessor, DLO, and was presented at the IEEE International Conference on Robotics and Automation (ICRA) in London, UK in 2023.

本仓库二次开发/优化记录（2026-03-10）

本节是对一次“从能编译到能稳定跑 bag 并产出结果”的完整修复复盘，重点解决崩溃、越界、并发未定义行为（UB）、以及离线回放参数误配导致的空点云/低点数问题。

目标

• 让 DLIO 在 ROS1 Noetic 离线 rosbag 回放场景下稳定运行（不崩溃、不刷屏致命错误）。
• 提供可重复的崩溃捕获方式（自动 backtrace），便于后续继续优化。
• 记录改动点与原因，避免同类问题回归。

过程中的主要问题（按出现顺序）

1. 点云预处理过强导致低点数/无法进入里程计

• 现象：反复出现 "Low number of points in the cloud!" 或无法正常推进。
• 原因：默认 crop=3m + voxel=1m 对离线数据过于激进；同时 GICP 最小点数阈值偏高。
• 处理：在 launch 层覆盖参数，便于回退（见下方修改列表）。

2. 运行时 SIGSEGV（exit code -11），定位到 deskewPointcloud

• 现象：节点在 IMU 标定阶段或刚初始化时崩溃。
• 根因：deskewPointcloud 假设 timestamps 非空，在空 scan/空 timestamps 情况下发生越界访问。
• 处理：增加空输入保护 + 节流告警，跳过异常帧而不是崩溃。

3. IMU 缓冲区窗口提取存在并发/迭代器失效风险（UB）

• 现象：偶发异常、难复现的内存错误风险（尤其在多线程与 circular_buffer 迭代器混用时）。
• 根因：在锁外使用基于 boost::circular_buffer 的迭代器窗口，缓冲区在其他线程 push 时可能导致迭代器失效。
• 处理：采用 "lock-copy-unlock"：锁内拷贝出 vector 快照，锁外做时间窗口裁剪与积分。

4. computeSpaciousness 越界访问

• 现象：存在 OOB 读（循环条件 <= size）。
• 处理：修正循环边界，增加空 scan 保护，并做小幅性能改进（reserve、避免 pow）。

5. TF_REPEATED_DATA 刷屏

• 现象：tf2 报 "TF_REPEATED_DATA ignoring data with redundant timestamp"。
• 根因：动态线程重复广播同一时间戳的 TF；同时 baselink->imu/lidar 外参属于静态 TF，不应按帧率重复发布。
• 处理：外参改为 /tf_static 一次性发布；动态 odom->base_link 增加按时间戳去重。

修改点清单（文件 -> 改了什么 -> 为什么）

1) 点云/IMU 离线回放与调参入口

• launch/dlio_copy.launch

  • 覆盖参数：cropBoxFilter/size=1.0、voxelFilter/res=0.2、gicp/minNumPoints=64。
  • 原因：离线 bag 默认参数过于激进；且该实现 CropBox 使用 setNegative(true)，size 过大会“删除更多点”，必须避免误设。

• launch/dlio_copy_gdb.launch

  • 新增：用 gdb batch 模式启动 dlio_odom_node，崩溃自动打印 backtrace。
  • 原因：解决 "exit code -11" 时需要稳定获取栈回溯，而 ROS 日志不一定完整。

2) 运行时稳定性修复（核心）

• src/dlio/odom.cc

  • deskewPointcloud：增加空点云与空 timestamps 的保护与节流告警。
  • imuMeasFromTimeRange / integrateImu / integrateImuInternal：改为 vector 快照窗口，消除 circular_buffer 迭代器 UB 风险；对 dt<=0 做鲁棒处理。
  • computeSpaciousness：修复越界与空 scan 情况；减少不必要开销。
  • TF 发布：静态外参发布到 /tf_static；动态 TF 按 stamp 去重。

• include/dlio/odom.h

  • 新增 StaticTransformBroadcaster 成员；新增动态 TF stamp 去重原子变量。
  • 原因：配合 odom.cc 的 TF 发布策略调整。

• include/dlio/dlio.h

  • 补充 include。
  • 原因：上述 IMU 窗口接口改为使用 std::vector，需要头文件保证独立编译。

推荐验证流程（离线 bag 回放）

以本仓库测试 bag 为例：bags/T3F2-2021-08-02-15-00-12.bag

1. 编译

cd ~/dlio_ws
source /opt/ros/noetic/setup.bash
catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash

2. 启动

roslaunch direct_lidar_inertial_odometry dlio_copy.launch

3. 任务开关（必须）

rostopic pub -1 /state_machine/mission std_msgs/Int16 "data: 1"

4. 回放（建议带 /clock）

rosbag play --clock ~/dlio_ws/src/direct_lidar_inertial_odometry/bags/T3F2-2021-08-02-15-00-12.bag -r 1.0

5. 检查输出

rostopic hz -w 3 /dlio/odom_node/odom
rostopic echo -n 1 /tf_static

PCD 导出与查看（不依赖仓库内置服务）

本仓库上游的 save_pcd 服务在当前代码中是注释状态。离线保存 map 更简单的方式是使用 pcl_ros 的 pointcloud_to_pcd：

rosrun pcl_ros pointcloud_to_pcd input:=/dlio/map_node/map _prefix:=/tmp/dlio_map_

查看 PCD 可用：

pcl_viewer /tmp/dlio_map_*.pcd

后续建议（下一阶段优化方向）

• 对 "Bad time sync between LiDAR and IMU" 做系统化诊断：统一时间基准（/clock vs wall time）、记录点云/IMU header.stamp 分布、对齐延迟与缓冲策略。
• 将点云预处理参数做成 profile：online 实车 / offline bag 两套，避免误配。

上游说明（简版）

本仓库基于上游 DLIO（论文/演示链接见页首）。上游 README 内容较长，这里只保留你实际跑通与二次开发必需的信息；更多截图/细节可直接参考上游仓库：https://github.com/vectr-ucla/direct_lidar_inertial_odometry

传感器与话题

• LiDAR：sensor_msgs::PointCloud2；IMU：sensor_msgs::Imu。
• 已覆盖/测试：Ouster、Velodyne、Hesai；Livox 需要 feature/livox-support 分支 + livox_ros_driver2（并使用对应消息类型）。
• 强约束：LiDAR 与 IMU 必须时间同步，否则会直接影响 deskew/里程计稳定性。

标定与配置

• 外参（LiDAR/IMU 到机体重心）与 IMU 内参建议写入 cfg/dlio.yaml。
• 没有精确外参也可先用粗略值跑通流程，但精度会下降。

依赖（最小集合）

• Ubuntu 20.04 + ROS Noetic + C++14。
• 常见系统依赖：

sudo apt install libomp-dev libpcl-dev libeigen3-dev

编译与运行（示例）

在已有 catkin 工作空间中：

cd ~/dlio_ws
source /opt/ros/noetic/setup.bash
catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash

运行（按你的话题名改参数/或直接改 launch）：

roslaunch direct_lidar_inertial_odometry dlio.launch \
  rviz:=true \
  pointcloud_topic:=/robot/lidar \
  imu_topic:=/robot/imu

Livox 示例：

roslaunch direct_lidar_inertial_odometry dlio.launch \
  rviz:=true \
  livox_topic:=/livox/lidar \
  imu_topic:=/robot/imu

地图保存

• 上游提供保存 .pcd 的服务接口；本仓库当前更推荐用 pcl_ros/pointcloud_to_pcd（见上方“PCD 导出与查看”），更直接、对改动更不敏感。

上游测试数据

• 上游 README 提供了公开测试数据下载链接（Google Drive），可用于对比 deskew on/off 的效果。

Citation

If you found this work useful, please cite our manuscript:

BibTeX

@article{chen2022dlio,
  title={Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction},
  author={Chen, Kenny and Nemiroff, Ryan and Lopez, Brett T},
  journal={2023 IEEE International Conference on Robotics and Automation (ICRA)},
  year={2023},
  pages={3983-3989},
  doi={10.1109/ICRA48891.2023.10160508}
}

Acknowledgements

致谢（展开）

We thank the authors of the FastGICP and NanoFLANN open-source packages:

• Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration,” in IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2021, pp. 11 054–11 059.
• Jose Luis Blanco and Pranjal Kumar Rai, “NanoFLANN: a C++ Header-Only Fork of FLANN, A Library for Nearest Neighbor (NN) with KD-Trees,” https://github.com/jlblancoc/nanoflann, 2014.

We would also like to thank Helene Levy and David Thorne for their help with data collection.

Many thanks to @shrijitsingh99 for porting DLIO to ROS2!

License

This work is licensed under the terms of the MIT license.