[ICRA2021] Voxelized GICP for Fast and Accurate 3D Point Cloud Registration

Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno
National Institute of Advanced Industrial Science and Technology (AIST), Japan
IEEE International Conference on Robotics and Automation (ICRA2021)

Transcript

1. Voxelized GICP for Fast and Accurate 3D Point Cloud Registration Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno National Institute of Advanced Industrial Science and Technology (AIST), Japan Code available: https://github.com/SMRT-AIST/fast_gicp
2. Scan Matching for LiDAR SLAM Odometry estimation Pan et al., MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square, 2021 Tixiao et al., LIO-SAM: Tightly-coupled LiDAR Inertial Odometry via Smoothing and Mapping, 2020 Loop Detection Scan matching is inevitable for LiDAR SLAM We need a fast and accurate scan matching method
3. Popular Scan Matching Methods Generalized ICP (GICP) Normal Distributions Transform Accurate: Fine surface modeling Slow: Needs costly NN search ICP with D2D matching Fast: No NN search, Suitable for GPU Inaccurate: Accuracy largely depends on voxel resolution Voxel-based data association Voxelized GICP (VGICP) Combine GICP’s D2D matching scheme with NDT’s voxel-based data association Robust to voxel resolution changes / As accurate as GICP / Runs at 120Hz on a GPU
4. Generalized ICP Red: source 𝒂𝒊 Blue: target 𝒃𝒊 1. Estimate covariances 2. Find correspondences with NN search 3. Update s.t. D2D cost is minimized Point residual: Point residual distribution: Matching cost: D2D matching cost Point residual Fused cov
5. Matching cost: 1. Estimate covariances 2. Voxelize target points 3. Find correspondences with voxel lookup 4. Update s.t. distribution-to-multi-distribution cost is minimized Voxelized GICP Red: source 𝒂𝒊 Blue: target 𝒃𝒊 Point residual: GICP: VGICP: Point residual distribution: GICP: Weight (# of points) Mean of points/covs
6. 1. Estimate covariances 2. Voxelize target points (Calc means of targets) 3. Find correspondences with voxel lookup 4. Update s.t. distribution-to-multi-distribution cost is minimized Matching cost: Voxelized GICP Red: source 𝒂𝒊 Blue: target 𝒃𝒊 Point residual: GICP: VGICP: Point residual distribution: GICP: Weight (# of points) Mean of points/covs
7. The proposed approach can properly estimate the covariances of voxels in the distance Voxelization Approaches NDT algorithms: Aggregating points Proposed approach: Aggregating point distributions Deficient Valid
8. Implementation VGICP - Multi-threaded CPU - CUDA-based GPU GICP - Multi-threaded CPU Covariance Estimation - Multi-threaded KD-tree (CPU) - Brute-force NN search (GPU) Code available: https://github.com/SMRT-AIST/fast_gicp All the implementations use a Gauss-Newton-based optimizer
9. Evaluation in a Simulated Environment PCL implementations - ICP - NDT - GICP Our implementations - GICP - VGICP Intel Core i9-9900K NVIDIA GeForce RTX2080Ti Ray casting-based realistic LiDAR simulation Compared algorithms PC spec
10. Trajectory Estimation Accuracy ICP: - Baseline NDT: - Accuracy depends on the voxel resolution GICP (PCL): - Better accuracy than ICP and NDT GICP (ours): - Slightly better than PCL’s GICP - Maybe because of the optimizer choice VGICP (ours): - Almost as accurate as GICP
11. Trajectory Estimation Accuracy ICP: - Baseline NDT: - Accuracy depends on the voxel resolution GICP (PCL): - Better accuracy than ICP and NDT GICP (ours): - Slightly better than PCL’s GICP - Maybe because of the optimizer choice VGICP (ours): - Almost as accurate as GICP
12. Trajectory Estimation Accuracy ICP: - Baseline NDT: - Accuracy depends on the voxel resolution GICP (PCL): - Better accuracy than ICP and NDT GICP (ours): - Slightly better than PCL’s GICP - Maybe because of the optimizer choice VGICP (ours): - Almost as accurate as GICP
13. Trajectory Estimation Accuracy ICP: - Baseline NDT: - Accuracy depends on the voxel resolution GICP (PCL): - Better accuracy than ICP and NDT GICP (ours): - Slightly better than PCL’s GICP - Maybe because of the optimizer choice VGICP (ours): - Almost as accurate as GICP
14. Processing Time Our GICP: - Slightly faster than PCL’s implementation with single-threading - With multi-threading, it gets significantly faster VGICP: - Same preprocessing time - GPU-brute force NN search is slightly slower than CPU-based KD-tree - Significantly small optimization time Preprocess time can be improved with: - Sensor-specific NN search method - Reusing estimated covariances
15. Evaluation in a Real Environment GICP - 20FPS on CPU VGICP - As accurate as GICP - 30FPS on CPU - 120FPS on GPU Velodyne HDL32e Intel Core i9-9900K NVIDIA GeForce RTX2080Ti LiDAR: PC:
16. Conclusion Code available: https://github.com/SMRT-AIST/fast_gicp • Voxelized GICP, which combines GICP’s D2D matching cost and NDT’s voxel data association, is proposed • Multi point distribution aggregation approach is proposed to calculate valid distributions on voxels with few points • Evaluation results show that VGICP is as accurate as GICP while reducing the pose optimization time
17. Extension: Global Matching Cost Minimization