The official implementation of KCP: K-Closest Points and Maximum Clique Pruning for Efficient and Effective 3D Laser Scan Matching, accepted for publication in the IEEE Robotics and Automation Letters (RA-L).

KCP is an efficient and effective local point cloud registration approach targeting for real-world 3D LiDAR scan matching problem. A simple (and naive) understanding is: ICP iteratively considers the closest point of each source point, but KCP considers the k closest points of each source point in the beginning, and outlier correspondences are mainly rejected by the maximum clique pruning method. KCP is written in C and we also support Python binding of KCP (pykcp).

For more, please refer to our paper:

• Yu-Kai Lin, Wen-Chieh Lin, Chieh-Chih Wang, K-Closest Points and Maximum Clique Pruning for Efficient and Effective 3-D Laser Scan Matching. IEEE Robotics and Automation Letters (RA-L), vol.7, no. 2, pp. 1471 -- 1477, Apr. 2022. (paper) (preprint) (code) (video)

If you use this project in your research, please cite:

@article{lin2022kcp,
  title={K-Closest Points and Maximum Clique Pruning for Efficient and Effective 3-D Laser Scan Matching},
  author={Lin, Yu-Kai and Lin, Wen-Chieh and Wang, Chieh-Chih},
  journal={IEEE Robotics and Automation Letters},
  volume={7},
  number={2},
  pages={1471--1477},
  year={2022},
  doi={10.1109/LRA.2021.3140130},
}

and if you find this project helpful or interesting, please ⭐Star the repository. Thank you!

Table of Contents

• KCP
  • 📦 Resources
  • ⚙️ Installation
    • Step 1. Preparing the Dependencies
      • GCC, CMake, Git, and Eigen3
      • nanoflann
      • TEASER
    • Step 2. Preparing Dependencies of Python Binding (Optional)
    • Step 3. Building KCP
      • Without Python Binding
      • With Python Binding
    • Step 4. Installing KCP to the System (Optional)
  • 🌱 Examples
  • 📝 Some Remarks
    • Tuning Parameters
    • Controlling Computational Cost
    • Torwarding Global Registration Approaches
  • 🎁 Acknowledgement

• API Documentation
• Complete result of the experiment of robustness

The project is originally developed in Ubuntu 18.04, and the following instruction supposes that you are using Ubuntu 18.04 as well. I am not sure if it also works with other Ubuntu versions or other Linux distributions, but maybe you can give it a try 👍

Also, please feel free to open an issue if you encounter any problems of the following instruction.

You have to prepare the following packages or libraries used in KCP:

1. A C compiler supporting C 14 and OpenMP (e.g. GCC 7.5).
2. CMake ≥ 3.11
3. Git
4. Eigen3 ≥ 3.3
5. nanoflann
6. TEASER ≥ d79d0c67

sudo apt update
sudo apt install -y g   build-essential libeigen3-dev git software-properties-common lsb-release

# If you want to obtain newer version of cmake, run the following commands: (Ref: https://apt.kitware.com/)
# wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null
# echo "deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ $(lsb_release -sc) main" | sudo tee /etc/apt/sources.list.d/kitware.list >/dev/null
# sudo apt update

sudo apt install cmake

cd ~
git clone https://github.com/jlblancoc/nanoflann
cd nanoflann
mkdir build && cd build
cmake .. -DNANOFLANN_BUILD_EXAMPLES=OFF -DNANOFLANN_BUILD_TESTS=OFF
make
sudo make install

cd ~
git clone https://github.com/MIT-SPARK/TEASER-plusplus
cd TEASER-plusplus
git checkout d79d0c67
mkdir build && cd build
cmake .. -DBUILD_TESTS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_DOC=OFF
make
sudo make install

The Python binding of KCP (pykcp) uses pybind11 to achieve operability between C and Python. KCP will automatically download and compile pybind11 during the compilation stage. However, you need to prepare a runable Python environment with header files for the Python C API (python3-dev):

sudo apt install -y python3 python3-dev

Execute the following commands to build KCP:

git clone https://github.com/StephLin/KCP
cd KCP
mkdir build && cd build
cmake ..
make

git clone https://github.com/StephLin/KCP
cd KCP
mkdir build && cd build
cmake .. -DKCP_BUILD_PYTHON_BINDING=ON -DPYTHON_EXECUTABLE=$(which python3)
make

This will make the KCP library available in the system, and any C (CMake) project can find the package by find_package(KCP). Think twice before you enter the following command!

# Under /path/to/KCP/build
sudo make install

We provide two examples (one for C and the other for Python 3) These examples take nuScenes' LiDAR data to perform registration. Please check

• C example with CMake, and
• Python example

for more information.

The major parameters are

• kcp::KCP::Params::k and
• kcp::KCP::Params::teaser::noise_bound,

where k is the number of nearest points of each source point selected to be part of initial correspondences, and noise_bound is the criterion to determine if a correspondence is correct. In our paper, we suggest k=2 and noise_bound the 3-sigma (we use noise_bound=0.06 meters for nuScenes data), and those are default values in the library.

There is also a boolean parameter to enable debug messages called kcp::KCP::Params::verbose (default: false).

To use different parameters to the KCP solver, please refer to the following snippets:

C

#include <kcp/solver.hpp>

auto params = kcp::KCP::Params();

params.k                  = 2;
params.verbose            = false;
params.teaser.noise_bound = 0.06;

auto solver = kcp::KCP(params);

Python

import pykcp

params = pykcp.KCPParams()
params.k = 2
params.verbose = False
params.teaser.noise_bound = 0.06

solver = pykcp.KCP(params)

Instead of correspondence-free registration in TEASER , KCP considers k closest point correspondences to reduce the major computational cost of the maximum clique algorithm, and we have expressed the ability for real-world scenarios without any complicate or learning-based feature descriptor in the paper. However, it is still possible to encounter computational time or memory issue if there are too many correspondences fed to the solver.

We suggest controlling your keypoints around 500 for k=2 (in this way the computational time will be much closer to the one presented in the paper).

It is promising that KCP can be extended to a global registration approach if a fast and reliable sparse feature point representation method is employed.

In this way, the role of RANSAC, a fast registration approach usually used in learning based approaches, is similar to KCP's, but the computation results of KCP are deterministic, and also, KCP has better theoretical supports.

This project refers to the computation of the smoothness term defined in LOAM (implemented in Tixiao Shan's excellent project LIO-SAM, which is licensed under BSD-3). We modified the definition of the smoothness term (and it is called the multi-scale curvature in this project).