gtsam_points

gtsam_points

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

Tested on Ubuntu 22.04 / 24.04 and CUDA 12.2 / 12.6 / 13.1 / 13.3, and NVIDIA Jetson Orin with GTSAM 4.2a9 and 4.3a1.

Updates

• 2026/07/12 : v1.2.2 released. Minor fix for GTSAM 4.3a1..
• 2026/06/14 : v1.2.1 released. Support for GTSAM 4.3a1 and CUDA 13.3.
• 2026/01/24 : v1.2.0 released. Support for both GTSAM 4.2a9 and GTSAM 4.3a0, and CUDA 13.1. Intensity support for Gaussian voxelmap.
• 2025/06/15 : The base GTSAM version has been changed. Make sure you have rebuilt and installed GTSAM 4.3a1.

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.
• FastOccupancyGrid Binary occupancy grid with bit blocks and flat hashing for efficient point cloud overlap estimation.

Point Features and Global Point Cloud Registration

• Point Feature Histogram (PFH) [13]
• Fast Point Feature Histogram (FPFH) [14]
• RANSAC RANSAC-based global point cloud registration. Supports 6DoF and 4DoF (XYZ + Yaw) estimation [15].
• Graduated Non-Convexity Graduated Non-Contexity-based global point cloud registration. Supports 6DoF and 4DoF (XYZ + Yaw) estimation [16].

Object Segmentation

• Region Growing Segmentation [17]
• Min-Cut Segmentation [18]

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.3a1

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 \                # Set ON to build demo programs
#   -DBUILD_TESTS=OFF \               # Set ON to build unit tests
#   -DBUILD_TOOLS=OFF \               # Set ON to build tools
#   -DBUILD_WITH_TBB=OFF \            # Set ON to enable TBB
#   -DBUILD_WITH_OPENMP=OFF \         # Set ON to enable OpenMP
#   -DBUILD_WITH_CUDA=OFF \           # Set ON to enable CUDA support
#   -DBUILD_WITH_CUDA_MULTIARCH=OFF \ # Set ON to enable multi-arch CUDA support
#   -DCMAKE_CUDA_ARCHITECTURES=89 \   # If not specified, "native" architecture is used
#   -DBUILD_WITH_MARCH_NATIVE=OFF     # Set ON to enable -march=native (recommended to keep it OFF)

make -j$(nproc)
sudo make install

Install from PPA [Ubuntu 22.04, 24.04] [AMD64, ARM64]

Setup PPA

curl -s https://koide3.github.io/ppa/setup_ppa.sh | sudo bash

Install GTSAM and gtsam_points

# Without CUDA
sudo apt install -y libgtsam-points-dev

# with CUDA 12.2 (for only Ubuntu 22.04)
sudo apt install -y libgtsam-points-cuda12.2-dev

# with CUDA 12.5
sudo apt install -y libgtsam-points-cuda12.5-dev

# with CUDA 13.1
sudo apt install -y libgtsam-points-cuda13.1-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

• Multi-scan registration of 5 frames (= A graph with 10 registration factors)
• Bundle adjustment factor
• Continuous-time ICP factor
• Colored ICP factor
• Incremental voxel mapping and normal estimation
• SE3 BSpline interpolation

License

This library is released under the MIT license.

Citation

@software{gtsam_points,
  author       = {Kenji Koide},
  title        = {gtsam_points : A collection of GTSAM factors and optimizers for point cloud SLAM},
  month        = Aug,
  year         = 2024,
  publisher    = {Zenodo},
  version      = {1.0.4},
  doi          = {10.5281/zenodo.13378352},
  url          = {https://github.com/koide3/gtsam_points)}}
}

Dependencies

• Eigen
• nanoflann
• GTSAM
• [optional] PCL
• [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
[13] Rusu et al., "ligning Point Cloud Views using Persistent Feature Histograms", IROS2008
[14] Rusu et al., "Fast Point Feature Histograms (FPFH) for 3D Registration", ICRA2009
[15] Buch et al., "Pose Estimation using Local Structure-Specific Shape and Appearance Context", ICRA2013
[16] Zhou et al., "Fast Global Registration", ECCV2016
[17] Rabbani et al., "Segmentation of Point Clouds Using Smoothness Constraint", Remote Sensing and Spatial Information Sciences, 2006
[18] Golovinskiy et al., "Min-Cut Based Segmentation of Point Clouds", S3DV-WS@ICCV2009