itinai content

←back to Blog

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

This paper introduces a SLAM framework that achieves real-time CPU-only performance in dense, registration-error-minimization-based odometry and mapping by leveraging exact point cloud downsampling via coreset extraction, eliminating the need for GPU acceleration.

Key Highlights

  • Exact Point Cloud Downsampling via Coresets – Selects a minimal subset of residuals that exactly preserve the quadratic registration error function for a given pose, enabling evaluation with only 0.5–5% of the original data without approximation error.
  • Deferred Sampling Strategy – Defers the coreset extraction until the linearization point significantly deviates from the sampling point, minimizing redundant computation during iterative optimization.
  • Sliding Window Odometry Optimization – Implements tightly coupled LiDAR-IMU odometry using iSAM2 with full GICP registration factors (exact nearest neighbors + exact downsampling), allowing accurate motion estimation even under rapid dynamics and point cloud degeneracy.
  • Global Trajectory Optimization without Pose Graphs – Constructs a dense factor graph using GICP registration error across all overlapping submap pairs, avoiding reliance on relative pose constraints (e.g., gtsam::BetweenFactor) for superior global consistency.
  • High-Fidelity GICP Registration – Incorporates full covariance modeling and exact KD-tree nearest neighbor search for precise point-to-point alignment, facilitated by the speedup from exact coreset sampling.
  • Real-Time Performance on Standard CPUs – Achieves ~52ms per-frame odometry and ~434ms per submap global optimization (on Intel i7-8700K), easily meeting real-time thresholds with no GPU.
  • Robust to Point Cloud Degeneration – Outperforms leading SLAM methods (FAST-LIO2, VoxelMap, SLICT) on flatwall datasets, maintaining low ATEs during scenes with minimal structure and severe scan degradation.
  • KITTI-Verified Downsampling Accuracy – Demonstrates that coreset-based downsampling yields zero error at the sampling point and outperforms random and quadratic approximation under pose perturbations.
  • Benchmark-Proven Generalization – Matches or exceeds the accuracy of GPU-accelerated GLIM on MCD VIRAL dataset (average ATE: 0.536m vs 0.508m) while running entirely on CPU.
  • Dense Loop Closure Without Sparsity Bottleneck – Maintains high overlap connectivity in global optimization, even for large loop closures – outperforms traditional pose-graph-based methods like SLICT and PGO on long sequences.

Paper Resources

Paper: https://arxiv.org/abs/2505.01017v1
Project: https://staff.aist.go.jp/k.koide/projects/icra2025_es/

1. Visual SLAM: https://learnopencv.com/monocular-slam-in-python/
    2. LiDAR SLAM: https://learnopencv.com/lidar-slam-with-ros2/
    3. MASt3R SLAM: https://learnopencv.com/mast3r-slam-realtime-dense-slam-explained/

The post Tightly Coupled Range Inertial Odometry and Mapping with Exact Point Cloud Downsampling appeared first on OpenCV.