$\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$
A Doppler LiDAR-Inertial Odometry

1. Introduction

$\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$ is a 4D Doppler LiDAR-inertial odometry framework that exploits intrinsic Doppler velocity measurements provided by FMCW Doppler LiDARs. To properly incorporate Doppler information, a motion compensation scheme is introduced together with a Doppler-aided observation model for on-manifold state estimation. Doppler-based criteria are further employed to effectively reject dynamic points, yielding more consistent geometric observations. As a result, $\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$ achieves accurate state estimation and static mapping even in structure-degenerated environments. Furthermore, a scan-free, motion-free, and correspondence-free initialization method, $\color{#FF577E}{𝙁𝙧𝙚𝙚\text{-}𝙄𝙣𝙞𝙩}$ (DOI | Project Page), is also integrated into $\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$.

$\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$ supports both 4D and 3D LiDARs and delivers accurate mapping across a wide range of challenging environments. The following examples highlight its performance in both structured and degenerated scenes.

2. Framework Highlights

• ⏱️ Doppler Transformation: In Section IV-C, we propose the Doppler transformation in Eq. (9), a closed-form formula that transforms the Doppler velocity of a point from one frame to any other. Based on it, the framework develops a Doppler compensation scheme that aligns all Doppler measurements into a unified spatiotemporal frame, enabling velocity estimation by the velocimeter. Beyond motion compensation, the Doppler transformation serves as a general formula for any task that transforms Doppler velocities across arbitrary different frames, and it subsumes existing applications as special cases. Specifically, ego-motion compensation for Doppler velocity is a special case of our proposed Doppler transformation (compensation) formula when the two coordinate frames coincide, whereas Doppler Correspondence is also a special case under the assumption of uniform motion.
• 🚀 Flexible Initialization: Multiple initialization algorithm and strategies are supported, including conventional stationary initialization and $\color{#FF577E}{𝙁𝙧𝙚𝙚\text{-}𝙄𝙣𝙞𝙩}$ (DOI | Project Page), which delivers accurate initial states even in the presence of aggressive motion, moving objects, and structure degeneracy.
• 🧭 Multiple Integration & Motion Compensation Schemes: Various integration and motion compensation methods are provided, including second-order Runge-Kutta method (RK2), third-order Runge-Kutta method (RK3), fourth-order Runge-Kutta method (RK4), mechanization, and ACI2.
• 🛡️ Robust Velocity Estimation: Multiple algorithms are integrated to improve the resilience and accuracy of velocity estimation, including least squares with RANSAC, covariance, and robust kernels.
• 🗺️ Multiple Map Structures: Different map structures are supported, including ikd-Tree and iVox, enabling flexible and efficient mapping under varying resources and environments.
• 📡 Multiple LiDAR Types: The framework is compatible with both 4D Doppler LiDARs and conventional 3D LiDARs, facilitating evaluation and deployment across different LiDAR sensing configurations.

3. Docker

All experimental results are obtained in the provided Docker environment.

3.1 Build Docker Image

mkdir -p ./fmcw_lio_ws/src/
cd ./fmcw_lio_ws/src/
git clone https://github.com/IMRL/FMCW-LIO.git
cd ./FMCW-LIO/docker/
docker build -t fmcw_lio:latest .

3.2 Launch Container

chmod +x run_docker.sh

# dataset path will be mounted to /datasets inside the container
./run_docker.sh /path/to/fmcw_lio_ws /path/to/dataset

3.3 Run

# at FMCW-LIO workspace
rm -rf ./devel/ ./build/ && catkin_make
source ./devel/setup.bash
# take FMCW-LIO dataset as an example
roslaunch fmcw_lio aeva64_fmcw_lio_structured.launch

# open another terminal to play bag
docker exec -it fmcw_lio bash
source /opt/ros/noetic/setup.bash
rosbag play --pause /path/to/bag

4. Native Installation

4.1 Prerequisites

• Ubuntu and ROS

  • Ubuntu 20.04.
  • ROS Noetic, please follow ROS Installation.

• PCL and Eigen

  • PCL 1.9, please follow PCL Installation.
  • Eigen 3.3.7, please follow Eigen Installation.

• Livox ROS Driver

  • Please follow livox_ros_driver.

4.2 Build

mkdir -p ./fmcw_lio_ws/src/
cd ./fmcw_lio_ws/src/
git clone https://github.com/IMRL/FMCW-LIO.git
cd ../ && rm -rf ./devel/ ./build/ && catkin_make
source ./devel/setup.bash

4.3 Run

# take FMCW-LIO dataset as an example
roslaunch fmcw_lio aeva64_fmcw_lio_structured.launch

# open another terminal to play bag
rosbag play --pause /path/to/bag

If launch is successful, terminal should display the $\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$ system information similar to the following:

5. Launch File Selection

$\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$ provides dedicated launch files for different 4D/3D LiDAR sensors, datasets, and sequence configurations. Please select the corresponding launch file according to the specific dataset and sequence.

Dataset	Sequence	Launch File
FMCW-LIO Dataset	FMCW-LIO_campus_01 FMCW-LIO_street_01 FMCW-LIO_stpaul_01	aeva64_fmcw_lio_structured.launch
	FMCW-LIO_tunnel_01 FMCW-LIO_tunnel_03	aeva64_fmcw_lio_tunnel.launch
Free-Init Dataset	Free-Init_handheld_campus	aeva64_fmcw_lio_structured.launch (set init_method: 3 in its YAML file)
	Free-Init_handheld_tunnel	aeva64_fmcw_lio_tunnel.launch (set init_method: 3 in its YAML file)
	Free-Init_vehicular_highway Free-Init_vehicular_tunnel	aeva64_freeinit_vehicular.launch
Boreas-RT Dataset	All	aeva64_boreas_rt.launch
HeRCULES Dataset	All	aeva64_hercules.launch
Newer College Dataset	Stereo Vision Lidar IMU Dataset	ouster64_newer_college_dataset.launch
	Multi-camera Vision Lidar IMU Dataset	ouster128_newer_college_dataset.launch
Oxford Spires Dataset	All	hesai64_oxford_spires_dataset.launch
Multi-Campus Dataset	KTH & TUHH Sequences	ouster64_mcd_kth_tuhh.launch
	NTU Sequences	ouster128_mcd_ntu.launch
NTU VIRAL Dataset	All	ouster16_ntu_viral.launch
Hilti SLAM Challenge Dataset 2022	All	hesai32_hilti_slam_2022.launch
UrbanNav Dataset	All	velodyne32_urbannav.launch
FAST-LIO2 Dataset	All	livox_avia_fast_lio2.launch
FAST-LIVO2 Dataset	All	livox_avia_fast_livo2.launch

6. FMCW-LIO Dataset

The collected FMCW Doppler LiDAR-inertial dataset (FMCW-LIO Dataset) is available on Hugging Face and Google Drive. The characteristics of each sequence are summarized in the following table. All sequences are start-end aligned, all returning to starting points, making end-to-end distance (E2E) a measure of drifted error.

Sequence	Preview	FMCW-LIO Map	Platform	Distance (m)	Duration (s)
FMCW-LIO_campus_01			Handheld	425	347
FMCW-LIO_street_01			Handheld	183	177
FMCW-LIO_tunnel_01			Handheld	1127	709
FMCW-LIO_tunnel_03			Handheld	280	171
FMCW-LIO_stpaul_01			Handheld	238	200

7. Free-Init Dataset

$\color{#FF577E}{𝙁𝙧𝙚𝙚\text{-}𝙄𝙣𝙞𝙩}$ (DOI | Project Page) is seamlessly integrated into the $\color{#F1261D}{𝗙}\color{#FF9200}{𝗠}\color{#FDDB2A}{𝗖}\color{#4DE42B}{𝗪}\color{#0FF692}{\text{-}}\color{#0BD8F9}{𝗟}\color{#2C22FB}{𝗜}\color{#C32DF3}{𝗢}$ framework. The dataset for qualitative test of dynamic initialization (Free-Init Dataset) is available on Hugging Face and Google Drive. The characteristics of each sequence are summarized in the following table.

Sequence	Preview	FMCW-LIO Map	Platform	Distance (m)	Duration (s)
Free-Init_handheld_campus			Handheld	30	30
Free-Init_handheld_tunnel			Handheld	50	30
Free-Init_vehicular_highway			Vehicular	553	30
Free-Init_vehicular_tunnel			Vehicular	562	30

8. Citation

If you find our algorithms, datasets, or frameworks helpful in your research, please consider citing our works.

@article{zhao2024fmcw-lio,
  title={FMCW-LIO: A Doppler LiDAR-Inertial Odometry},
  author={Zhao, Mingle and Wang, Jiahao and Gao, Tianxiao and Xu, Chengzhong and Kong, Hui},
  journal={IEEE Robotics and Automation Letters},
  volume={9},
  number={6},
  pages={5727--5734},
  year={2024},
  publisher={IEEE}
}

@article{zhao2024free-init,
  title={Free-Init: Scan-Free, Motion-Free, and Correspondence-Free Initialization for Doppler LiDAR-Inertial Systems},
  author={Zhao, Mingle and Wang, Jiahao and Gao, Tianxiao and Xu, Chengzhong and Kong, Hui},
  journal={IEEE Robotics and Automation Letters},
  volume={9},
  number={12},
  pages={11329--11336},
  year={2024},
  publisher={IEEE}
}