LIVO SLAM: FAST-LIVO2#
FAST-LIVO2 is a direct (feature-less) LiDAR-Inertial-Visual Odometry system: it fuses a LiDAR-Inertial pose estimate with dense visual-inertial tracking on raw image patches, avoiding explicit feature extraction/matching. It targets real-time onboard localization and mapping, including in visually- or geometrically-degraded environments where either sensor alone struggles.
Paper: FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry (accepted, T-RO’24)
Upstream: hku-mars/FAST-LIVO2
In Robotics AI Suite, the upstream tree is a pristine git submodule and Intel changes ship as patches on top, so FAST-LIVO2 can be evaluated as an alternative SLAM backend without forking the reference navigation stack.
[!IMPORTANT] FAST-LIVO2 is released under GPLv2. For commercial use, contact the upstream authors (see FAST-LIVO2/README.md) for an alternative license before shipping it in a product.
Changes to 3rd party source#
This work is based on the open-source FAST-LIVO2 repository, pinned in .gitmodules at the upstream commit the patches below apply to.
Patch |
Enhancement |
|---|---|
0001-Stop-VIO-waiting-on-the-LiDAR-buffer-in-LIVO-mode.patch |
Removes a LiDAR-buffer precondition that gated every VIO update even though the VIO step never reads the LiDAR queue. Measured: ~30 ms of gate wait removed per VIO update at 10 Hz LiDAR input (src/LIVMapper.cpp). |
Ports the codebase and launch files from ROS1/catkin to ROS2/ament (validated on Humble and Jazzy); adds a Livox Mid-360 + RealSense D415 sensor profile; adds an optional per-frame LIO/VIO timing CSV export gated behind |
|
Sizes the LIO/VIO OMP thread count from the process’s actual CPU affinity at startup ( |
|
Reformats |
|
0005-Fix-crash-race-risks-and-an-info-leak-in-the-ROS2-no.patch |
Wraps |
Environment setup (Ubuntu 24.04 / ROS 2 Jazzy, Intel Core Ultra / PTL)#
The following scripts automate the one-time host prerequisites and repository-specific setup:
# 1. Fetch the pristine upstream submodule
git submodule update --init robotics-ai-suite/pipelines/fast-livo2-demo/FAST-LIVO2
cd robotics-ai-suite/pipelines/fast-livo2-demo/scripts
# 2. One-time host dependencies (needs sudo; safe to re-run)
./install_deps.sh
# 3. Apply the Intel patches from the table above
./apply_patches.sh
# 4. Build fast_livo2 with colcon
./build.sh
All paths, the ROS distro, and the dataset sequence used below are centralized in scripts/env.sh — edit that one file to retarget a different workspace/sequence; nothing else needs to change.
Manual reproduction (no scripts)#
Everything above is what scripts/*.sh automate. This section spells out the
same steps by hand — for anyone who’d rather not run scripts, or who’s
forking this pipeline and wants to see exactly what each step does before
changing it. Every path/value below is one of scripts/env.sh’s own
defaults; run these commands from inside pipelines/fast-livo2-demo (all
relative paths are relative to that directory).
1. Host dependencies#
scripts/install_deps.sh installs the required system packages, Livox-SDK2, Sophus, and vikit_common; run it once (needs sudo, safe to re-run):
./scripts/install_deps.sh
2. Apply the Intel patches#
cd FAST-LIVO2
git am --keep-cr ../patches/0001-Stop-VIO-waiting-on-the-LiDAR-buffer-in-LIVO-mode.patch
git am --keep-cr ../patches/0002-Port-to-ROS2-and-bring-up-Mid-360-D415-on-A2W.patch
git am --keep-cr ../patches/0003-Size-OMP-thread-count-from-runtime-CPU-affinity.patch
git am --keep-cr ../patches/0004-Reformat-sources-to-the-real-clang-format-style.patch
git am --keep-cr ../patches/0005-Fix-crash-race-risks-and-an-info-leak-in-the-ROS2-no.patch
cd ..
(git am fails on a dirty or already-patched tree — apply_patches.sh’s
extra safety is only needed if you’re re-running this against an edited
.patch file.)
3. Build with colcon#
mkdir -p ~/fast_livo2_ws/src
ln -sfn "$(pwd)/FAST-LIVO2" ~/fast_livo2_ws/src/fast_livo2
source /opt/ros/jazzy/setup.bash
cd ~/fast_livo2_ws
colcon build --packages-select fast_livo2 # add --cmake-args -DENABLE_PERFRAME_TIMING=ON for the latency CSV
source install/setup.bash
cd -
If you don’t already have livox_ros_driver2/vikit_ros built elsewhere,
point UNDERLAY_SETUP in scripts/env.sh at an existing
install space, or let scripts/build.sh build them from scratch instead of
doing so by hand here.
5. Run fast_livo2 against the bag#
NTU_VIRAL.yaml’s tracked evo/seq_name param (which controls the output
trajectory filename, Log/result/<seq_name>.txt) already defaults to
eee_01, so no scratch-copy/sed step is needed — run directly against the
checked-in config.
Two terminals. Terminal A — the algorithm:
source /opt/ros/jazzy/setup.bash
source ~/fast_livo2_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_DOMAIN_ID=199
ros2 launch fast_livo2 mapping_ouster_ntu.launch.py \
use_rviz:=false \
avia_params_file:=FAST-LIVO2/config/NTU_VIRAL.yaml
Terminal B — bag playback (start once Terminal A is up and printing):
source /opt/ros/jazzy/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_DOMAIN_ID=199
ros2 bag play ~/ntu_viral_dataset/eee_01
Once playback finishes, stop fast_livo2 (Ctrl-C in Terminal A — a clean
SIGTERM, not kill -9, so it flushes the trajectory file) and check
FAST-LIVO2/Log/result/eee_01.txt was written. The core-pinning/SCHED_FIFO
wrapping run_ntu_viral.sh applies on PTL (taskset/chrt) is an optional
performance extra, not required for a correctness repro — see “Reference:
running on Intel PTL” above if you want that too.
Optional — the CycloneDDS+iceoryx shared-memory transport, by hand
(equivalent to scripts/setup_dds_shm.sh start — run that script instead if
you don’t need to customize this):
sudo apt-get install -y \
ros-jazzy-cyclonedds ros-jazzy-rmw-cyclonedds-cpp \
ros-jazzy-iceoryx-posh ros-jazzy-iceoryx-hoofs ros-jazzy-iceoryx-binding-c
MY_IP=$(ip route get 1.1.1.1 | awk '/src/{for(i=1;i<=NF;i++) if ($i=="src") print $(i+1)}')
mkdir -p scripts/generated
cat > scripts/generated/cyclonedds_shm.xml <<EOF
<CycloneDDS><Domain><General>
<AllowMulticast>true</AllowMulticast>
</General><Discovery><Peers><Peer Address="$MY_IP"/></Peers></Discovery>
<SharedMemory>
<Enable>true</Enable>
<LogLevel>warn</LogLevel>
</SharedMemory>
</Domain></CycloneDDS>
EOF
AllowMulticast must be true, not false — false plus a unicast Peer
pointing at your own IP reliably breaks same-host node discovery on some
machines (confirmed on Orin).
cat > scripts/generated/roudi_config.toml <<'EOF'
[general]
version = 1
[[segment]]
[[segment.mempool]]
size = 128
count = 10000
[[segment.mempool]]
size = 1024
count = 5000
[[segment.mempool]]
size = 16384
count = 1000
[[segment.mempool]]
size = 131072
count = 200
[[segment.mempool]]
size = 524288
count = 50
[[segment.mempool]]
size = 1048576
count = 30
[[segment.mempool]]
size = 4194304
count = 20
EOF
source /opt/ros/jazzy/setup.bash
iox-roudi -c scripts/generated/roudi_config.toml --monitoring-mode off &
sleep 2
pgrep -x iox-roudi && echo "RouDi is up"
The mempool sizes above are sized for full PointCloud2 scans — RouDi’s own
stock example config is too small and silently drops SHM segments instead of
erroring. --monitoring-mode off is required: RouDi’s default liveness
monitor evicts any participant that misses a ~1.5s heartbeat, which
CPU-isolation/governor/SCHED_FIFO changes can trigger even on a healthy
process.
Then, in every shell that needs to see the algorithm node (Terminal A,
Terminal B, and any rviz2/ros2 node list shell), export one more variable
before sourcing the ROS setup files:
export CYCLONEDDS_URI="file://$(pwd)/scripts/generated/cyclonedds_shm.xml"
Verify with ros2 node list (should show the FAST-LIVO2 node within ~1s of
launching it). When done: stop fast_livo2/ros2 bag play, then
pkill -x iox-roudi.
6. Evaluate RMSE#
The estimated trajectory (FAST-LIVO2/Log/result/eee_01.txt) needs
converting to the surveyed PRISM reflector frame before comparing against
ground truth — evaluate_viral.py,
already checked into the repo, provides both conversions:
python3 - <<'PY'
import sys
sys.path.insert(0, "FAST-LIVO2/Log/result/ntu_viral")
from evaluate_viral import convert_slam_to_prism, convert_leica_to_tum
convert_slam_to_prism("FAST-LIVO2/Log/result/eee_01.txt",
"FAST-LIVO2/Log/result/ntu_viral/eee_01_prism_repro.txt")
convert_leica_to_tum("$HOME/ntu_viral_dataset/leica_pose_eee_01.csv",
"FAST-LIVO2/Log/result/ntu_viral/eee_01_gt_repro.txt")
PY
pip install --user --break-system-packages evo # if not already installed
PATH="${HOME}/.local/bin:${PATH}" evo_ape tum \
FAST-LIVO2/Log/result/ntu_viral/eee_01_gt_repro.txt \
FAST-LIVO2/Log/result/ntu_viral/eee_01_prism_repro.txt -a
evo_ape’s summary table reports RMSE in meters; multiply by 100 to compare
against the documented eee_01 baseline of 2.71 cm
(FAST-LIVO2/Log/result/ntu_viral/README.md).
A fresh measurement that does not exceed the baseline by more than
RMSE_TOLERANCE_PCT (20% by default,
scripts/env.sh) is an expected pass — the check
exists to catch regressions, not to require beating the paper’s own number.
Limitations / non-goals#
Validated here: functional SLAM operation and pose-tracking accuracy (RMSE) against the public NTU VIRAL baseline.
Sensor assumption: a synchronized LiDAR + camera + IMU stream (native Livox format for the Mid-360 profile, or a standard rosbag as in the NTU VIRAL flow above).
Real-robot bring-up (Mid-360 + D415 on A2W) uses the config shipped in patch 0002 (
config/mid360-a2w*.yaml) but requires that physical hardware and is not exercised by this reproduce flow.GPLv2 licensing (see callout above) applies to the upstream code as-is; this integration does not change that.