This is a collection of GTSAM factors and optimizers for range-based SLAM.

Tested on Ubuntu 22.04 / 24.04 and CUDA 12.2, and NVIDIA Jetson Orin with GTSAM 4.2a9.

Factors

Scan Matching Factors

IntegratedICPFactor & IntegratedPointToPlaneICPFactor
The conventional point-to-point and point-to-plane ICP [1].
IntegratedGICPFactor
Generalized ICP based on the distribution-to-distribution distance [2].
IntegratedVGICPFactor
GICP with voxel-based data association and multi-distribution-correspondence [3][4].
IntegratedVGICPFactorGPU
GPU implementation of VGICP [3][4].
To enable this factor, set -DBUILD_WITH_CUDA=ON.
IntegratedLOAMFactor
Matching cost factor based on the combination of point-to-plane and point-to-edge distances [5][6].

Colored Scan Matching Factors

IntegratedColorConsistencyFactor
Photometric ICP error [7].
IntegratedColoredGICPFactor
Photometric ICP error + GICP geometric error [2][7].

Continuous-time ICP Factors

IntegratedCT_ICPFactor
Continuous Time ICP Factor [8].
IntegratedCT_GICPFactor
Continuous Time ICP with GICP's D2D matching cost [2][8].

Bundle Adjustment Factors

PlaneEVMFactor and EdgeEVMFactor
Bundle adjustment factor based on Eigenvalue minimization [9].
LsqBundleAdjustmentFactor
Bundle adjustment factor based on EVM and EF optimal condition satisfaction [10].

Optimizers for GPU Factors

All the following optimizers were derived from the implementations in GTSAM.

LevenbergMarquardtOptimizerExt
ISAM2Ext
IncrementalFixedLagSmootherExt

Nearest Neighbor Search

KdTree KdTree with parallel tree construction. Derived from nanoflann.
IncrementalVoxelMap Incremental voxel-based nearest neighbor search (iVox) [11].
IncrementalCovarianceVoxelMap Incremental voxelmap with online normal and covariance estimation.

Continuous-Time Trajectory (Under development)

B-Spline Cubic B-Spline-based interpolation and linear acceleration and angular velocity expressions [12].
ContinuousTrajectory Cubic B-Spline-based continuous trajectory representation for offline batch optimization.

Installation

Install from source

# Install gtsam
git clone https://github.com/borglab/gtsam
cd gtsam
git checkout 4.2a9
 
mkdir build && cd build
cmake .. \
  -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
  -DGTSAM_BUILD_TESTS=OFF \
  -DGTSAM_WITH_TBB=OFF \
  -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF
 
make -j$(nproc)
sudo make install
 
# [optional] Install iridescence visualization library
# This is required for only demo programs
sudo apt install -y libglm-dev libglfw3-dev libpng-dev
git clone https://github.com/koide3/iridescence --recursive
mkdir iridescence/build && cd iridescence/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install
 
## Build gtsam_points
git clone https://github.com/koide3/gtsam_points
mkdir gtsam_points/build && cd gtsam_points/build
cmake .. -DCMAKE_BUILD_TYPE=Release
 
# Optional cmake arguments
# cmake .. \
#   -DBUILD_DEMO=OFF \
#   -DBUILD_TESTS=OFF \
#   -DBUILD_WITH_CUDA=OFF \
#   -DBUILD_WITH_MARCH_NATIVE=OFF
 
make -j$(nproc)
sudo make install

Install from <a href="https://github.com/koide3/ppa">PPA</a> [AMD64, ARM64]

Setup PPA

Ubuntu 24.04

curl -s --compressed "https://koide3.github.io/ppa/ubuntu2404/KEY.gpg" | gpg --dearmor | sudo tee /etc/apt/trusted.gpg.d/koide3_ppa.gpg >/dev/null

echo "deb [signed-by=/etc/apt/trusted.gpg.d/koide3_ppa.gpg] https://koide3.github.io/ppa/ubuntu2404 ./" | sudo tee /etc/apt/sources.list.d/koide3_ppa.list

Ubuntu 22.04

curl -s --compressed "https://koide3.github.io/ppa/ubuntu2204/KEY.gpg" | gpg --dearmor | sudo tee /etc/apt/trusted.gpg.d/koide3_ppa.gpg >/dev/null

echo "deb [signed-by=/etc/apt/trusted.gpg.d/koide3_ppa.gpg] https://koide3.github.io/ppa/ubuntu2204 ./" | sudo tee /etc/apt/sources.list.d/koide3_ppa.list

Ubuntu 20.04

curl -s --compressed "https://koide3.github.io/ppa/ubuntu2004/KEY.gpg" | gpg --dearmor | sudo tee /etc/apt/trusted.gpg.d/koide3_ppa.gpg >/dev/null

echo "deb [signed-by=/etc/apt/trusted.gpg.d/koide3_ppa.gpg] https://koide3.github.io/ppa/ubuntu2004 ./" | sudo tee /etc/apt/sources.list.d/koide3_ppa.list

Install GTSAM and gtsam_points

Without CUDA

sudo apt update && sudo apt install -y libgtsam-points-dev

With CUDA 12.2

sudo apt update && sudo apt install -y libgtsam-points-cuda12.2-dev

With CUDA 12.5

sudo apt update && sudo apt install -y libgtsam-points-cuda12.5-dev

Demo

cd gtsam_points
./build/demo_matching_cost_factors
./build/demo_bundle_adjustment
./build/demo_continuous_time
./build/demo_continuous_trajectory
./build/demo_colored_registration

Videos

License

This library is released under the MIT license.

Dependencies

Eigen
nanoflann
GTSAM
[optional] OpenMP
[optional] CUDA
[optional] iridescence

Disclaimer

The test data in data directory are generated from The KITTI Vision Benchmark Suite and The Newer College Dataset. Because they employ Creative Commons BY-NC-SA License 3.0 and 4.0, the test data must not be used for commercial purposes.

References

[1] Zhang, "Iterative Point Matching for Registration of Free-Form Curve", IJCV1994
[2] Segal et al., "Generalized-ICP", RSS2005
[3] Koide et al., "Voxelized GICP for Fast and Accurate 3D Point Cloud Registration", ICRA2021
[4] Koide et al., "Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors", RA-L2021
[5] Zhang and Singh, "Low-drift and real-time lidar odometry and mapping", Autonomous Robots, 2017
[6] Tixiao and Brendan, "LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain", IROS2018
[7] Park et al., "Colored Point Cloud Registration Revisited", ICCV2017
[8] Bellenbach et al., "CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure", 2021
[9] Liu and Zhang, "BALM: Bundle Adjustment for Lidar Mapping", IEEE RA-L, 2021
[10] Huang et al, "On Bundle Adjustment for Multiview Point Cloud Registration", IEEE RA-L, 2021
[11] Bai et al., "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels", IEEE RA-L, 2022
[12] Sommer et al., "Efficient Derivative Computation for Cumulative B-Splines on Lie Groups", CVPR2020