Robust Lidar-based Odometry and Mapping

kilo-map is a real-time LiDAR-based SLAM system with the following features:

• Hybrid Feature Gaussian Voxel Map: Incrementally maintains uncertain hybrid features (planar and NDT-variant), improving feature utilization across both structured and unstructured environments.

• Two-Stage ESKF Frontend: Fuses LiDAR and IMU in an Error-State Kalman Filter with a two-stage lidar update for high-dynamic motion: Stage 1 performs incremental per-point ESKF updates along the scan timeline to compensate for motion distortion (analogous to Point-LIO); Stage 2 backpropagates the distortion-corrected points to body frame and runs iterated ESKF (IESKF) over the full frame for global consistency refinement. This design makes the system robust against aggressive platform dynamics (e.g., legged robots) while preserving IESKF's fast convergence property.

• Factor Graph Backend: Uses Ceres-based factor graph optimization to tightly couple loop-closure constraints with odometry factors, reducing global drift. small_gicp and KISS-Matcher serve as verification modules for loop closure, improving matching accuracy.

• Real-Time Visualization: Built on Iridescence for real-time 3D visualization and debugging. Outputs frontend and backend trajectories for algorithm evaluation, and supports saving global maps (as a single map or tiled blocks) for localization systems.

• Cross-Platform Support: Supports both ROS 1 and ROS 2, validated on multiple public datasets with different LiDAR sensors. Most configurations share the same YAML structure and require little to no parameter tuning across different platforms and sensor setups.

Prerequisites

This project supports both ROS 1 and ROS 2, and has been tested on these distributions(melodic, noetic, foxy). It should also work with ROS 2 distributions on Ubuntu 22.04 and 24.04 (e.g., Humble, Jazzy).

All third-party libraries and ROS message packages are bundled in this repository. You only need to install the following system dependencies via apt:

# required
sudo apt update && sudo apt install -y libeigen3-dev libpcl-dev libgoogle-glog-dev libgflags-dev libyaml-cpp-dev libboost-filesystem-dev libboost-system-dev libtbb-dev liblz4-dev libceres-dev libglm-dev libglfw3-dev

# optional
sudo apt install -y libpng-dev libjpeg-dev libassimp-dev

Build

cd ~/kilo_map_ws/src
git clone https://github.com/ouguangjun/kilo-map.git
cd ..

# ros1
catkin_make # or catkin build

# ros2
colcon build

Run

Public dataset

The system has been validated on NCLT, SuperLoc, m3dgr, diter and other public datasets. You can download these datasets and test with the corresponding launch files (make sure to match the LiDAR type, topic, and extrinsic parameters in the YAML config).

Taking the legged robot legkilo dataset as an example:

# ROS 1
source devel/setup.bash
roslaunch legkilo legkilo_go1_velodyne.launch
rosbag play slope.bag

# ROS 2
source install/setup.bash
ros2 launch legkilo legkilo_go1_velodyne.py
# use rosbags-convert to convert ROS 1 bag to ROS 2 format
ros2 bag play ./slope_ros2

Custom Dataset

To run with your own dataset, make sure the following YAML parameters are correctly configured:

1. lidar_topic / imu_topic: ROS topic names for the LiDAR and IMU data.
2. lidar_type: The LiDAR model. Currently supported: velodyne, ouster, hesai, livox. To add a new type, refer to legkilo/src/preprocess/lidar_processing.h.
3. time_scale: The scale factor to convert each LiDAR point's raw timestamp to seconds (e.g., 1e-9 for nanosecond timestamps, 1e-6 for microseconds). This varies across LiDAR models and even across different driver configurations for the same sensor.
4. sensor_type: The fusion mode. Use LIO for typical LiDAR-IMU setups.
5. extrinsic_T / extrinsic_R: The translation vector and rotation matrix of the IMU-to-LiDAR extrinsic transformation.
6. For best results, start mapping from a stationary state to allow the system to initialize IMU biases and the initial pose. Setting init_type to 2 enables gravity-aligned initialization.

Save Map

After the run completes, you can save the global map and trajectory via the Save Result button in the upper-left corner of the viewer.

Related Publications

This project originated from the following paper, but has been substantially refactored and redesigned over time. It is no longer a direct implementation of the original work.

Leg-KILO (RA-L 2024)

@ARTICLE{legkilo,
  author={Ou, Guangjun and Li, Dong and Li, Hanmin},
  journal={IEEE Robotics and Automation Letters}, 
  title={Leg-KILO: Robust Kinematic-Inertial-Lidar Odometry for Dynamic Legged Robots}, 
  year={2024},
  volume={9},
  number={10},
  pages={8194-8201},
  doi={10.1109/LRA.2024.3440730}
}

Contact

If you have questions, make an issue or contact me at ouguangjun98@gmail.com

If you have ideas or suggestions for improvement, feel free to submit a PR or reach out!

Maintainers

License

This project is licensed under the MIT License - see the LICENSE file for details

Acknowledgments

We gratefully acknowledge the following open-source projects:

• Iridescence — real-time 3D visualization

• small_gicp — point cloud registration for loop-closure verification

• KISS-Matcher — point cloud registration for loop-closure verification

• HKU-MaRS Lab — for inspiration from their outstanding publications

• Xiang Gao — for his excellent open-source projects

免责声明

本项目为个人学习项目，出于兴趣分享，学术上不必过度细究😄