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

[ICRA2025] Tightly Coupled Range Inertial Odometry and Mapping with Exact Point Cloud Downsampling
https://staff.aist.go.jp/k.koide/

Transcript

1. Tightly Coupled Range Inertial Odometry and Mapping with Exact Point Cloud Downsampling Kenji Koide, Aoki Takanose, Shuji Oishi, and Masashi Yokozuka National Institute of Advanced Industrial Science and Technology (AIST), Japan
2. • LiDAR-IMU SLAM framework that directly minimizes multi-scan registration errors without a GPU (No gtsam::BetweenFactor). • The multi-scan registration approach showed good robustness but required GPU acceleration in previous work (GLIM) [RAS, 2024] • The exact-coreset-based downsampling drastically reduces the number of points (10 – 150) while retaining the accuracy. Proposal Odometry estimation factor graph Global trajectory optimization factor graph Optimizing extremely dense graphs using only an affordable CPU (e.g., Intel N150) Reducing the number of points (10 – 150) while retaining the accuracy
3. Downsampling for Point Cloud Registration How to reduce the effort for the registration error computation? → Point Cloud Downsampling Reducing # pts leads to corruption (at least 1k pts required) Trade-off between accuracy and speed Random Sampling Geometric Sampling Reijgwart+, RA-L, 2020 Yokozuka+, ICRA2021 Tuna+, T-RO, 2023 Jiao+, ICRA2021 Shiquan+, RA-L, 2024 Greedy Sampling Randomly selecting a certain number of points Selecting points such that that geometric props are retained Selecting points greedy based on a quality metric Good in a geometry rich env Deteriorates in degeneration Easy to implement and fast No accuracy guarantee Ensures the quality somehow Deteriorates depending on # pts
4. Exact Coreset 𝑓𝑓 𝒳𝒳 ≈ 𝑓𝑓( � 𝒳𝒳) Coreset � 𝒳𝒳：A subset of 𝒳𝒳 that approximates the output of a function 𝑓𝑓 Original set Coreset 𝑠𝑠. 𝑡𝑡. � 𝒳𝒳 ⊂ 𝒳𝒳 and � 𝒳𝒳 ≪ 𝒳𝒳 𝑓𝑓 𝒳𝒳 = 𝑓𝑓( � 𝒳𝒳) Exact coreset Exact coreset � 𝒳𝒳：A subset of 𝒳𝒳 that yields the same output through a function 𝑓𝑓 𝑠𝑠. 𝑡𝑡. � 𝒳𝒳 ⊂ 𝒳𝒳 and � 𝒳𝒳 ≪ 𝒳𝒳 Original set
5. Downsampling with Exact Corset Extraction Gauss-Newton optimization models an error function in the quadratic form Registration error function Information matrix / vector Constant Source / Target points No approximation error at the sampling point!! Given the current estimate (called sampling point), we select a weighted subset (coreset) of input data s.t. it yields the same quadratic function Weights / Residual indices Quadratic function parameters calculated using original set / extracted coreset Evaluation point s.t. During optimization, we re-linearize 𝑓𝑓𝑅𝑅𝑅𝑅𝑅𝑅 using the selected subset (coreset)
6. Nonlinear Approximation Error Evaluation Displacement vector error KL divergence 𝑯𝑯−1𝒃𝒃 − � 𝑯𝑯−1� 𝒃𝒃 KLD 𝑯𝑯, � 𝑯𝑯 Method Accuracy at the sampling point Accuracy under displacements Random sampling × (Corrupted) ✓ (can consider nonlinearity) Quadratic approximation (linear factor) ✓ (Zero errors) × (cannot consider nonlinearity) Exact Downsampling (Proposed) ✓ (Zero errors) ✓ (can consider nonlinearity) Zero errors at the sampling point
7. MCD VIRAL Dataset [Nguyen+, CVPR2024]  LiDAR : Ouster OS1-128 & Livox Mid-70 (Only Ouster was used for the proposed method) IMU : VN100 & VN200  On vehicle (ntu) and On-held (kth & tuhh) setups  Evaluating odometry estimation and global trajectory optimization
8. Quantitative Comparison ATEs [m] for odometry estimation methods ATEs [m] for global trajectory optimization methods The proposed system outperformed other CPU-based methods and was comparable to the GPU-based one (GLIM)
9. Global Optimization Factor Graphs SLICT PGO (a reference implementation in GLIM) Proposed Densely connected multi-scan registration factor graphs were created
10. Robustness to Point Cloud Degeneration The long optimization window (5s) helped overcome point cloud degeneration Flatwall Dataset [Koide+, RAS, 2024]  Involves a few seconds of complete degeneration of point clouds  Challenging for existing methods with state filtering and a short optimization window
11. Real-time Processing on an Intel N150 GMKTec G3 Plus Intel N150 (4 Core) Running at 1.5x ~ 2.0x of real-time
12. Conclusion  Point cloud downsampling based on exact coreset extraction  The downsampled points (10 ~ 150 points) yield the same quadratic function as that of the original set at a sampling point  The 3D LiDAR-IMU SLAM framework with the proposed downsampling method can even run on an Intel N150 in real time Mapping results on the MCD VIRAL dataset