Voxelized GICP for Fast and Accurate 3D Point Cloud Registration
Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno
Proc. IEEE International Conference on Robotics and Automation (ICRA2021), pp. 11054-11059, Xi'an, China, May, 2021
https://staff.aist.go.jp/k.koide/
Voxelized GICP for Fast and Accurate 3D Point Cloud Registration [ICRA2021]
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. 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
12. 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:
13. 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