Batch‑LIO

A batch‑wise extension of Point‑LIO: per‑~1 ms batch EKF update with in‑batch motion de‑skew — higher‑bandwidth, lower‑compute LiDAR‑inertial odometry.

🌐 English · 中文

---

Batch‑LIO reproduces innovation #1 of the USTC undergraduate thesis 《高带宽轮式激光惯性里程计》(Point‑LIWO — Batch‑based Direct Point LiDAR‑IMU‑Wheeled‑speed Odometry) by 张昊鹏. It is built directly on HKU‑MARS Point‑LIO and kept A/B‑comparable with it (CPU‑only; wheel‑speed / innovation #2 is out of scope).

Table of contents

	Section
1	What it changes vs Point‑LIO
2	Results
3	Build & Run (ROS 2 Humble)
4	Test
5	Repository layout
6	Legacy: ROS 1 Noetic
7	Attribution & license

---

What it changes vs Point‑LIO

Point‑LIO updates the EKF point‑wise (one update per distinct point timestamp) and already row‑stacks the measurement Jacobian over a group of same‑timestamp points. Batch‑LIO changes two things and adds OpenMP:

1. 1 ms time‑window grouping — points are grouped into fixed ~1 ms windows instead of by identical timestamp (time_compressing_batch, include/common_lib.h).
2. In‑batch de‑skew — because a window now spans many timestamps, each point is motion‑compensated to the window's reference (last‑point) time using the EKF state's angular/linear velocity, then all valid residuals are row‑stacked into a single EKF update per window (src/deskew.h, applied in src/laserMapping.cpp). This implements thesis eq. 3.44–3.47:
3. OpenMP on the per‑point KNN + plane‑fit loop (src/Estimator.cpp). OpenMP only pays off after batching enlarges the groups — on Point‑LIO's tiny per‑timestamp groups it is slower.

Δtⱼ = tⱼ − t_last                 (≤ 0 within a window)
Rⱼ  = Exp(ω · Δtⱼ)                (ω = state body angular velocity)
Tⱼ  = R_Iᵀ · v · Δtⱼ              (v = state world linear velocity; R_I = state rotation)
p'ⱼ = Rⱼ · pⱼ + Tⱼ

All changes are gated by ROS params (defaults make it equivalent to Point‑LIO when batch_dt ≤ 0).

param	default	meaning
batch_dt	0.001	batch window length in seconds (≤ 0 ⇒ point‑wise = Point‑LIO)
batch_omp	false	OpenMP on the KNN+plane‑fit loop
batch_deskew	true	in‑window de‑skew on/off (ablation toggle)

---

Results

A/B vs pristine Point‑LIO, same bag and parameters, CPU‑only on a 32‑core x86_64 machine. Full numbers in docs/RESULTS.md (ROS 1) and docs/RESULTS_ROS2.md (re‑run on the ROS 2 port).

No ground truth is used. "Accuracy" below means agreement with the Point‑LIO baseline (mean |Δpos|) plus loop‑closure drift on return‑loop bags — not error vs a true trajectory. The only comparison object is Point‑LIO.

Per‑frame compute: 2.3–4.7× faster

bag	point‑wise (ms)	batch 1 ms (ms)	speedup
quick‑shack (ROS 2)	12.42	3.51	3.5×
outdoor_run (ROS 2)	2.56	0.54	4.7×

Batch‑LIO matches the baseline trajectory closely and closes the outdoor_run loop better than baseline (0.020 m vs 0.073 m).

OpenMP only helps after batching ("batch enables parallelism")

batch_dt sweep: 1–2 ms is the sweet spot

Odometry rate depends on publish policy (not a bandwidth gain)

Batching reduces the EKF update count (windows ≪ points), so the raw max publish rate goes down, not up — the win is compute, not bandwidth. Point‑LIO isn't limited to 10 Hz either; its per‑point mode reaches ~6.7 kHz (noisy micro‑updates). For honesty:

scheme	publish policy	stable odom rate
Point‑LIO	per frame (default)	~10 Hz
Point‑LIO	per point	~6.7 kHz (noisy)
Batch‑LIO 1 ms	per window	~913 Hz

---

Build & Run (ROS 2 Humble)

Native ROS 2 Humble (/opt/ros/humble). The package depends on livox_ros_driver2 (its vendored SDK2 ships a prebuilt liblivox_lidar_sdk_shared.so, so no separate SDK build).

1 · Workspace + build

# livox_ros_driver2 as a sibling package; batch_lio symlinked from this repo
mkdir -p ~/batch_lio_ws/src
ln -sfn /path/to/livox_ros_driver2  ~/batch_lio_ws/src/livox_ros_driver2
ln -sfn /path/to/Batch-LIO         ~/batch_lio_ws/src/batch_lio
source /opt/ros/humble/setup.bash
cd ~/batch_lio_ws && colcon build --symlink-install

2 · Convert a ROS 1 Livox Avia bag → ROS 2 (mcap)

python3 -m pip install --user rosbags pyyaml
python3 scripts/convert_bag.py  your_avia.bag  ~/batch_lio_ws/bags/your_avia
# renames livox_ros_driver/CustomMsg -> livox_ros_driver2/msg/CustomMsg (byte-identical wire format)

3 · Run

source ~/batch_lio_ws/install/setup.bash
ros2 launch batch_lio mapping_avia.launch.py            # rviz2 on; rviz:=false for headless
ros2 bag play ~/batch_lio_ws/bags/your_avia             # in another shell
ros2 topic echo /aft_mapped_to_init                     # odometry

Works on standard Livox Avia bags (/livox/lidar = livox_ros_driver2/msg/CustomMsg, /livox/imu = sensor_msgs/msg/Imu), e.g. the HKU‑MARS / FAST‑LIO Avia sequences. Odometry on /aft_mapped_to_init; per‑frame stage timings as [ mapping ]: lines; trajectory logged to Log/pos_log.txt when runtime_pos_log_enable is set.

---

Test

cd ~/batch_lio_ws
colcon test --packages-select batch_lio          # deskew gtest (5) + smoke launch_test (1)
colcon test-result --all                         # 8 tests, 0 failures

test	what it checks
test_deskew (gtest)	the eq. 3.44–3.47 de‑skew transform (pure translation, pure rotation, body‑frame velocity)
test_smoke.py (launch_test)	launches the node, plays a converted bag, asserts odometry is published

A/B harness (speedup + de‑skew ablation):

bash scripts/ablations.sh
python3 scripts/compare_traj.py LABEL run/out/<run>/pos_log.txt run/out/<run>/node.log [baseline]

---

Repository layout

src/            modified Point‑LIO sources
  deskew.h          NEW: in‑batch de‑skew (eq 3.44‑3.47), header‑only + unit‑tested
  laserMapping.cpp  batch grouping + per‑window de‑skew before the EKF update + OMP control
  Estimator.cpp     OpenMP on the KNN + plane‑fit loop
  parameters.*      batch_dt / batch_omp / batch_deskew params (type‑tolerant loader)
include/common_lib.h  time_compressing_batch (1 ms windows) + ROS 2 time helpers
launch/*.py     ROS 2 python launch (mapping_{avia,horizon,ouster64,velody16}, avia_batch)
config/*.yaml   ROS 2 params files (wrapped in /**: {ros__parameters:})
scripts/        convert_bag.py (ROS1→ROS2), run_lio.sh, ablations.sh, compare_traj.py
test/           test_deskew.cpp (gtest), test_smoke.py (launch_test)
docs/           RESULTS.md, RESULTS_ROS2.md, superpowers/{specs,plans}/, figures/

---

Legacy: ROS 1 Noetic

The original ROS 1 Noetic (catkin) version is preserved on the ros1-noetic git tag. The ROS 2 port was a thin faithful port — same algorithm, same params; the only changes are the ROS plumbing (ament, rclcpp, tf2, livox_ros_driver2). See the design spec and implementation plan under docs/superpowers/.

git checkout ros1-noetic        # inspect the original ROS 1 version

---

Attribution & license

• Built on Point‑LIO (HKU‑MARS); please cite Point‑LIO and FAST‑LIO. The batch‑update idea reproduced here is from the USTC undergraduate thesis 《高带宽轮式激光惯性里程计》(Point‑LIWO) by 张昊鹏.
• De‑skew follows the FAST‑LIO / sr_lio motion‑compensation convention; the map uses an iVox‑style hashed‑voxel structure.
• Licensed under MIT (see LICENSE); portions derived from Point‑LIO / LOAM / Livox retain their BSD‑3 notices. This is a research reproduction.