lidar_odometry

Fast LiDAR SLAM

A high-performance real-time LiDAR SLAM system (~400 FPS on KITTI).

Features

• 2-Level hierarchical voxel map with precomputed surfels
• Point-to-plane ICP with Gauss-Newton optimization on Lie manifold
• Adaptive M-estimator for robust estimation (PKO)
• Loop closure detection with LiDAR Iris
• Configurable correspondence method: Surfel-based (O(1) lookup) or KDTree-based (dynamic plane fitting)

Correspondence Method Selection

use_surfel_correspondence: false  # true: O(1) surfel lookup, false: KDTree + plane fitting

• Surfel-based (default): Pre-computed surfels enable O(1) correspondence lookup. Best for outdoor environments with large planar structures.
• KDTree-based: Dynamic plane fitting at query time. Recommended for unstructured environments or narrow indoor spaces where pre-computed surfels may not represent local geometry accurately.

Build

git clone https://github.com/93won/lidar_odometry
cd lidar_odometry
./build.sh

Run

Before run algorithm on a test data do not foget update paths in config yaml files

# KITTI dataset
./build/kitti_lidar_odometry config/kitti.yaml

# MID360 / PLY files
./build/lidar_odometry config/mid360.yaml

Sample Data

• KITTI Sequence 07
• MID360 Dataset

ROS Wrapper

https://github.com/93won/lidar_odometry_ros_wrapper

References

@article{choi2025surfel,
  title={Surfel-LIO: Fast LiDAR-Inertial Odometry with Pre-computed Surfels and Hierarchical Z-order Voxel Hashing},
  author={Choi, Seungwon and Park, Dong-Gyu and Hwang, Seo-Yeon and Kim, Tae-Wan},
  journal={arXiv preprint arXiv:2512.03397},
  year={2025}
}

@article{choi2025pko,
  author={Choi, Seungwon and Kim, Tae-Wan},
  journal={IEEE Robotics and Automation Letters}, 
  title={Probabilistic Kernel Optimization for Robust State Estimation}, 
  year={2025},
  volume={10},
  number={3},
  pages={2998-3005},
  doi={10.1109/LRA.2025.3536294}
}

@inproceedings{wang2020iris,
  title={LiDAR Iris for Loop-Closure Detection},
  author={Wang, Ying and Sun, Zezhou and Xu, Cheng-Zhong and Sarma, Sanjay and Yang, Jian and Kong, Hui},
  booktitle={IROS},
  year={2020}
}

License

MIT License