[ICRA2022] Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping

Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping
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 (ICRA2022)

Transcript

1. Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno National Institute of Advanced Industrial Science and Technology (AIST), Japan https://twitter.com/MR2T_AIST https://staff.aist.go.jp/k.koide/ k.koide@aist.go.jp
2. Key Concepts Global Matching Cost Minimization LiDAR-IMU Tight Coupling • Directly minimize the matching cost over the entire map with GPU acceleration (unlike pose graph opt.) • Enables to accurately constrain frames with a very small overlap PGO [SuMa, RSS2018] Proposed • Employ the LiDAR-IMU tight coupling scheme for all the estimation stages (from frontend to backend) • Robust to quick sensor motion and environments without sufficient geometrical features • Reduce estimation drift in 4 DoF
3. Global Trajectory Optimization Global Matching Cost Minimization • We directly minimize the matching cost over the entire map = Global multi-scan registration • It enables to accurately constrain the relative pose between frames with a small overlap • This approach can easily be extended to the LiDAR-IMU tight coupling scheme Koide et al., “Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors”, IEEE RA-L, 2021 PGO [SuMa, RSS2018] Proposed Constraints between small overlapping frames
4. LiDAR-IMU Fusion Tight Coupling of LiDAR and IMU constraints • We build all the estimation stages (from frontend to backend) with tightly coupled LiDAR and IMU constraints • For global optimization, IMU factors are created between endpoints of submaps to strongly constrain submap poses Frontend factor graph (Fixed-lag smoothing) Only factors related to the latest frame (𝑥9 ) are shown Backend factor graph (iSAM2)
5. LiDAR-IMU Tight Coupling The proposed method aggressively creates loop constraints Constraints between small overlapping frames Environment without sufficient geometries KAIST Urban Dataset
6. Conclusions ※最新フレーム(𝑥9 )に関わるファクタのみ表示 • Global matching cost minimization for backend optimization • LiDAR-IMU tight coupling is employed for all the estimation stages • The proposed method can be applied to any kind of range sensors We are planning to release an extended version of this work as open source
8. LiDAR-IMU Fusion Tight Coupling of LiDAR and IMU constraints • Because our approach directly computes point cloud matching costs on the graph, it can easily be extended to the LiDAR-IMU tight coupling scheme with the IMU factor • We build all the estimation stages (from frontend to backend) with tightly coupled LiDAR and IMU constraints Frontend factor graph (Fixed-lag smoothing) • Keyframe-based point cloud matching to suppress estimation drift Only factors related to the latest frame (𝑥9 ) are shown • IMU factors for robustness to quick sensor motion • Fixed-lag smoothing to bound the computation cost
9. LiDAR-IMU Fusion Tight Coupling of LiDAR and IMU constraints • Because our approach directly computes point cloud matching costs on the graph, it can easily be extended to the LiDAR-IMU tight coupling scheme with the IMU factor • We build all the estimation stages (from frontend to backend) with tightly coupled LiDAR and IMU constraints Backend factor graph (iSAM2) • Submap optimization with fully connected matching cost factors • For global mapping, create a matching cost factor between every frame pair with an overlap rate larger than 5% • Create an IMU factor between the first and last frame states between submaps (endpoints)
10. Evaluation results ※最新フレーム(𝑥9 )に関わるファクタのみ表示 Accuracy (Newer College Dataset) Processing Time (KAIST Urban Dataset) https://ori-drs.github.io/newer-college-dataset/ https://sites.google.com/view/complex-urban-dataset Optimization converged in < 1 sec Proposed LIO-SAM
11. Key Concepts Global Matching Cost Minimization LiDAR-IMU Tight Coupling • Directly minimize the matching cost over the entire map with GPU acceleration (unlike pose graph opt.) • Enables to accurately constrain frames with a very small overlap PGO [SuMa, RSS2018] Proposed • Employ the LiDAR-IMU tight coupling scheme for all the estimation stages (from frontend to backend) • Robust to quick sensor motion and environments without sufficient geometrical features • Reduce estimation drift in 4 DoF
12. Global Optimization for LiDAR SLAM (1/3) Pose Graph Optimization • PGO minimizes errors in the pose space to correct estimation drift • Each relative pose constraint is modeled as a Gaussian distribution 𝑡0 𝑡1 𝑡2 𝑡𝑖 Loop constraint (SE3 relative pose) 𝐞𝑖𝑗 = log ෢ 𝐓𝑖𝑗 −1𝐓𝑖 −1𝐓𝑗 𝑓 ℱ, 𝒯 = ෍ 𝑖,𝑗∈ℱ 𝜌(𝐞𝑖𝑗 𝑇 𝛀𝑖𝑗 𝐞𝑖𝑗 ) Relative pose mean Current relative pose estimate Information matrix = Covariance matrix-1 Objective function Gaussian distribution
13. Global Optimization for LiDAR SLAM (2/3) Covariance Estimation Covariance Estimation • Estimating the uncertainty of a scan matching result is difficult in practice [CELLO-3D, ICRA2019] • Most frameworks do not properly set covariances • To create a SE3 relative pose constraint, the mean relative pose is necessary • It is difficult to create loop constraints between small overlapping frames where scan matching can fail ?