Simple Mapping and Localisation Estimation (SiMpLE) is a low-drift and low-configuration LiDAR odometry method providing accurate localization and mapping with a variety of sensors.
Demo_SiMpLE_compressed.mp4
The original paper code is available in the ./archive folder.
We perform among state-of-the-art LiDAR odometry methods with a simple and use case agnostic algorithm. The method is derived from using raw point clouds. We do not modify or develop a new variant of any existing algorithm. The code is written for readability and easy understanding with the methodology described in the paper. The software can be optimised for increased performance. Find our paper here.
We have tested the algorithm using LiDARs with various point cloud densities, fields of view, and scan patterns,
- Velodyne HDL-64 KITTI dataset
- OS1-64 from the MulRan dataset
- Velodyne VLP-16 using a self-created dataset
- Livox Horizon using a self-created dataset
The motivation stems from reducing the complexity and configuration burden of localisation algorithms.
The code is portable, easy to understand, and modify.
The following includes:
- A summary of the LiDAR odometry method.
- The hardware requirements and dependencies installation.
- Installation of the SiMpLE repository.
- An example of using SiMpLE.
- Sample results interpretation.
- The effect of threading on real-time performance.
- Results on different operating systems and processors.
- Extensive Parameter List
- Other datasets.
- References.
The method has five configuration parameters.
- rNew is the new point cloud spatial subsampling radius.
- rMap is the spatial subsampling radius for the local map.
- σ is the standard deviation of the Gaussian reward function - see the paper for more details.
- ϵ is the convergence tolerance of the optimisation solver.
- rMin is optional and for computational benefit only. The default value is zero where it has no effect. Increasing this value eliminates the rings commonly formed around the sensor to reduce the size of the point cloud without losing geometric information.
There are also hardware settings:
- rMax is the maximum radius of the point cloud. This is hardware-dependent and not a configuration parameter.
This implementation has been tested on Ubuntu 20.04.5/6 LTS (Focal Fossa) and Ubuntu 22.04.3 (Jammy Jellyfish) with an Intel Core i7-10700K CPU @ 3.80GHz x 16 and 62.5 GiB memory. SiMpLE uses a few open-source libraries for reading the algorithm configuration file, Kd-Trees, matrix operations, optimization functions, and CPU threading. The installation instructions are detailed below.
- Git is required to download the open-source libraries.
sudo apt install git- Require a g++ compiler.
sudo apt install g++- CMake is required to compile the libraries and the repository.
sudo apt install cmake- Install the Eigen library for math operations [1].
sudo apt install libeigen3-dev- Install Intel's Thread Building Blocks (TBB) library for CPU threading.
sudo apt install libtbb-devIf the SiMpLE repository build in the following section returns an error that it cannot find TBB for CMake, the following installation may help.
git clone https://github.com/oneapi-src/oneTBB
cd oneTBB
mkdir build && cd build
cmake ..
sudo make install- Clone and install the nanoflann library for KD-tree operations [2].
git clone https://github.com/jlblancoc/nanoflann.git
cd nanoflann
mkdir build && cd build
cmake ..
sudo make install- Install the Dlib library for the optimization solver [3, 4].
wget http://dlib.net/files/dlib-19.24.tar.bz2
tar xvf dlib-19.24.tar.bz2
cd dlib-19.24/
mkdir build && cd build
cmake ..
sudo make install- Install the yaml-cpp library for reading the configuration file [5].
git clone https://github.com/jbeder/yaml-cpp.git
cd yaml-cpp
mkdir build && cd build
cmake ..
sudo make installAlternative options for any of the libraries can be used if desired. The code is easy to change.
Clone the repository.
git clone https://github.com/vb44/SiMpLE.gitCreate a build folder in the repository.
cd SiMpLE
mkdir build && cd buildRun CMake.
cmake ../srcMake the executable.
makeThe code only works with .bin files in the KITTI format. However, the code is very easy to modify to suit the desired inputs and outputs. When compiled, the SiMpLE algorithm is run using a .yaml algorithm configuration file as shown below.
./simple config.yamlSample config files are included in the config folder.
An example is shown below.
Note that the --kitti argument is required to correct the scans for the KITTI dataset.
---
# Test setup.
scanPath: /path_to_scans/velodyne/
outputFileName: /path_to_output_files/fileName
kitti: true
verbose: true
# Algorithm configuration.
sigma: 0.3
rMap: 0.5
rNew: 0.3
convergenceTol: 1e-6
minSensorRange: 0
# Hardware configuration.
maxSensorRange: 120An example of interpreting the result file in the KITTI format is displayed in plotResultsMATLAB/interpretResults.m.
Sample results from the paper are available in the sampleResults folder for all reported datasets.
The estimated trajectories can be evaluated in the devkit folder, which is a reduced version of the devkit provided by KITTI.
It is important to note that the KITTI estimates must be transformed to the ground truth frame using the sensor registration before evaluating the results. An example is provided in the sampleResults folder.
SiMpLE's real-time performance is strongly reliant on performing CPU threading. The average execution times for varying the number of execution threads manually using OpenMP are displayed below. The performance of TBB is also included.
The implementation allows for deterministic results for a varying number of threads on a given platform. However, slight variations were observed when executing the algorithm on different machines. Upon investigation, this was found to be caused by the Dlib optimisation library having slight numerical precision differences when searching for the best pose hypothesis. Due to the nature of recursive pose estimation, the differences propagate and result in slight variations in the output trajectory.
Example results from the KITTI dataset executed on different operating systems and processors are displayed below.
- Ubuntu 20.04.5: Desktop with Intel Core i7 (10th gen, 16 cores).
- Ubuntu 20.04.6: Laptop with Intel Core i7 (8 cores).
- Ubuntu 22.04.3: Desktop virtual machine with Intel Core i7 (10th gen, 10 cores).
- Windows 11: Desktop with Intel Pentium (8 cores).
- macOS Sonoma 14.2.1: MacBook Air with M1 core.
The slight differences in the KITTI results on different machines are shown below.
| Sequence | Ubuntu 20.04.5 | Ubuntu 20.04.6 | Ubuntu 22.04.3 | Windows 11 | macOS 14.2.1 |
|---|---|---|---|---|---|
| 00 | 0.6667 | 0.6585 | 0.6599 | 0.6570 | 0.6606 |
| 01 | 0.7749 | 0.7750 | 0.7682 | 0.7786 | 0.7740 |
| 02 | 0.6263 | 0.6452 | 0.6290 | 0.6298 | 0.6208 |
| 03 | 0.7369 | 0.7378 | 0.7450 | 0.7507 | 0.7443 |
| 04 | 0.4067 | 0.4063 | 0.4043 | 0.4056 | 0.4052 |
| 05 | 0.3372 | 0.3298 | 0.3328 | 0.3213 | 0.3362 |
| 06 | 0.2613 | 0.2607 | 0.2621 | 0.2595 | 0.2599 |
| 07 | 0.4706 | 0.4768 | 0.4488 | 0.4622 | 0.4475 |
| 08 | 0.8236 | 0.8238 | 0.8215 | 0.8233 | 0.8290 |
| 09 | 0.5684 | 0.5420 | 0.5588 | 0.5476 | 0.5656 |
| 10 | 0.6454 | 0.6318 | 0.6622 | 0.6321 | 0.6186 |
| Average | 0.5744 | 0.5716 | 0.5720 | 0.5698 | 0.5693 |
The five parameters listed at the beginning of the page, namely, rNew, rMap, σ, ϵ, and rMin, are the algorithm configuration parameters. The use of opensource libraries such as nanoflann and Dlib introduce additional parameters into the implementation. We use the default parameters often hardcoded in the libraries and do not change them. Hence, these parameters are not included in the algorithm configuration list. SiMpLE has been extensively tested on numerous benchmark datasets in different environments and LiDARs with different characteristics, and we have not needed to change any default parameters. For complete transparency, an extensive list of all identifiable parameters and their source is displayed below.
| Parameter | Source | Use |
|---|---|---|
| rNew | Algorithm configuration. | New scan subsampling. |
| rMap | Algorithm configuration. | Local map subsampling. |
| σ | Algorithm configuration. | Reward standard deviation. |
| ϵ | Algorithm configuration. | Optimisation exit condition. |
| rMin | Algorithm configuration. | Minimum sensor range used for reducing point cloud size. |
| Parameter | Source | Use |
|---|---|---|
| rMax | Hardware specification. | Maintaining local map size. |
Other unchanged, default parameters that are introduced by opensource libraries as they appear in the code. These parameters have not been changed for all experiments,
| Parameter | Source | Value | Use |
|---|---|---|---|
| leafSize | KdTree library (nanoflann) | 10 | Used to construct the KD-tree. Default recommended value used. |
| derivativeEps | Dlib (API call to find ‘min using approximate derivatives’) | 1e-7 | Derivative step. Unchanged from the library. |
| wolfeRho | Dlib (API call to ‘bfgs search strategy’) | 0.01 | Set as a const parameter in the library (Nocedal and Wright (1999)). |
| wolfeSigma | Dlib (API call to ‘bfgs search strategy’) | 0.9 | Set as a const parameter in the library (Nocedal and Wright (1999)). |
| Line search, maxIterations | Dlib (API call to ‘bfgs search strategy’) | 100 | Set as a const parameter in the library. |
The UQ dataset scans can be downloaded from here. The folder contains the raw VLP-16 and Livox scans in the KITTI format for comparison with the sensor trajectory results shown in the paper.
- 05/04/2024: Refactor project for better readability and execution time improvement.
[1] eigen library
[2] nanoflann library
[3] Dlib library
[4] Dlib C++ install for CMake
[5] yaml-cpp library