Tightly Coupled Range Inertial Odometry and Mapping with Exact Point Cloud Downsampling

TL;DR

The paper tackles the computational burden of dense registration-error minimization in LiDAR-SLAM by introducing exact point cloud downsampling via coresets, which preserves the quadratic objective for a given sampling pose with substantially fewer residuals. A deferred sampling strategy and point-correspondence updates enable real-time relinearization on standard CPUs while maintaining accuracy, implemented within a two-module SLAM pipeline: a sliding-window odometry estimator and a global submap-based trajectory optimizer. Key contributions include extending exact downsampling with correspondence updates, integrating fast overlap estimation, and demonstrating CPU-real-time performance with competitive accuracy on KITTI and MCD VIRAL datasets, compared to GPU-based baselines. The approach removes the GPU dependency for dense registration-factor graphs, delivering robust performance in degenerate scenes and enabling broader, real-time deployment on commodity hardware.

Abstract

In this work, to facilitate the real-time processing of multi-scan registration error minimization on factor graphs, we devise a point cloud downsampling algorithm based on coreset extraction. This algorithm extracts a subset of the residuals of input points such that the subset yields exactly the same quadratic error function as that of the original set for a given pose. This enables a significant reduction in the number of residuals to be evaluated without approximation errors at the sampling point. Using this algorithm, we devise a complete SLAM framework that consists of odometry estimation based on sliding window optimization and global trajectory optimization based on registration error minimization over the entire map, both of which can run in real time on a standard CPU. The experimental results demonstrate that the proposed framework outperforms state-of-the-art CPU-based SLAM frameworks without the use of GPU acceleration.

Figures (12)

• Figure 1: Odometry estimation factor graph
• Figure 2: Global trajectory optimization factor graph
• Figure 4: Odometry estimation module, which estimates sensor ego-motion using sliding window factor graph optimization, and global mapping module, which constructs factor graph to directly minimize matching cost errors across entire map. Both modules utilize the GICP scan matching factor accelerated with the exact point cloud downsampling algorithm.
• Figure 5: Flowchart of exact point cloud downsampling with deferred sampling strategy.
• Figure 6: Factor graph for odometry estimation.