GenZ-ICP (Optimized)

Demo   •   Video   •   Install   •   ROS   •   Paper   •   Contact Us

GenZ-ICP is a Generalizable and Degeneracy-Robust LiDAR Odometry Using an Adaptive Weighting

---

💢 Optimizations:

File	Optimization	Description	Estimated CPU Usage Reduction
Registration.cpp	Vectorized TransformPoints	Use Eigen matrix operations instead of per-point std::transform	5–15% of total runtime
Registration.cpp	Precomputed kernel_squared	Compute kernel * kernel once instead of repeatedly in Weight	0.5–1% of total runtime
Registration.cpp	Improved Weight Function	Reduce operations in weight computation from 4 to 2 multiplications + 1 addition	2–5% of total runtime
VoxelHashMap.cpp	Squared Distance in GetClosestNeighbor	Use .squaredNorm() for comparisons, with one sqrt at the end	10–20% of total runtime
VoxelHashMap.cpp	Precomputed Statistics in GetClosestNeighbor	Use sum_points and sum_outer for O(27) computation vs. O(27 * P) per query	15–25% of total runtime
VoxelHashMap.cpp	Efficient AddPoints	Simplify with for loop and map_.at(), maintaining precomputed stats	1–3% of total runtime
VoxelHashMap.cpp	Optimized RemovePointsFarFromLocation	Use iterator-based erase for safe and efficient removal	1–2% of total runtime
Overall	Combined Effect	Cumulative impact across all optimizations, varying with point cloud size and voxel density	20–50% of total runtime

---

⚙️ How to build & run

ROS1

How to build

You should not need any extra dependency, just clone and build:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone https://github.com/cocel-postech/genz-icp.git
cd ..
catkin build genz_icp --cmake-args -DCMAKE_BUILD_TYPE=Release
source ~/catkin_ws/devel/setup.bash

How to run

Option 1

If you want to use a pre-tuned parameter set, you need to provide the config file with the topic name as arguments:

roslaunch genz_icp odometry.launch topic:=<topic_name> config_file:=<config_file_name>.yaml

rosbag play <rosbag_file_name>.bag

Examples and download links for demo datasets can be found here

Option 2

Otherwise, the only required argument to provide is the topic name:

roslaunch genz_icp odometry.launch topic:=<topic_name>

rosbag play <rosbag_file_name>.bag

Check out the tuning guide for the parameters of GenZ-ICP at this link

ROS2

How to build

You should not need any extra dependency, just clone and build:

mkdir -p ~/colcon_ws/src
cd ~/colcon_ws/src
git clone https://github.com/cocel-postech/genz-icp.git
cd ..
colcon build --packages-select genz_icp --cmake-args -DCMAKE_BUILD_TYPE=Release
source ~/colcon_ws/install/setup.bash

How to run

The only required argument to provide is the topic name:

ros2 launch genz_icp odometry.launch.py topic:=<topic_name>

and then,

ros2 bag play <rosbag_file_name>.mcap

Check out the tuning guide for the parameters of GenZ-ICP at this link

📌 Todo list

✔️ Code optimization to reduce CPU load

• Python support for GenZ-ICP

📝 Citation

If you use our codes, please cite our paper (arXiv, IEEE Xplore)

@ARTICLE{lee2024genzicp,
  author={Lee, Daehan and Lim, Hyungtae and Han, Soohee},
  journal={IEEE Robotics and Automation Letters (RA-L)}, 
  title={{GenZ-ICP: Generalizable and Degeneracy-Robust LiDAR Odometry Using an Adaptive Weighting}}, 
  year={2025},
  volume={10},
  number={1},
  pages={152-159},
  keywords={Localization;Mapping;SLAM},
  doi={10.1109/LRA.2024.3498779}
}

🙏 Acknowledgement

Many thanks to KISS team—Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch—to provide outstanding LiDAR odometry codes!

Please refer to KISS-ICP for more information

📫 Contact information

If you have any questions, please do not hesitate to contact us

• Daehan Lee ✉️ daehanlee at postech dot ac dot kr
• Hyungtae Lim ✉️ shapelim at mit dot edu
• Ali Pahlevani ✉️ a.pahlevani1998 at gmail dot com (Not a main author - Contributor)