[ICRA2024] Tightly Coupled Range Inertial Localization on a 3D Prior Map Based on Sliding Window Factor Graph Optimization

Tightly Coupled Range Inertial Localization on a 3D Prior Map Based on Sliding Window Factor Graph Optimization
Kenji Koide, Shuji Oishi, Masashi Yokozuka, and Atsuhiko Banno
National Institute of Advanced Industrial Science Technology (AIST)
IEEE International Conference on Robotics and Automation (ICRA2024)

Transcript

1. 1 Kenji Koide，Shuji Oishi，Masashi Yokozuka，and Atsuhiko Banno National Institute of Advanced Industrial Science Technology (AIST) Tightly Coupled Range Inertial Localization on a 3D Prior Map Based on Sliding Window Factor Graph Optimization
2. 2 Aim of this work Reducing the dependency on the map Existing map-based localization methods require a complete map They often become unstable when there are unmapped regions or changes in the map Unmapped
3. 3 Proposal We get rid of relative pose factors (i.e., gtsam::BetweenFactor or g2o::EdgeSE3) and introduce scan matching factors that directly minimize point cloud registration errors Tight coupling of scan-to-scan and scan-to-map registration factors This enables fusing sensor ego motion estimation and map-based pose correction smoothly Sliding window optimization for the robustness to momentary point cloud data corruption
4. 4 Proposed graph structure 𝑚 𝑥1 𝑥2 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 𝑥8 Matching cost factor IMU factor Marginalization factor Active sensor state Marginalized state Map points Scan-to-scan registration factors (GICP) Scan-to-map registration factors (GICP) IMU factors Optimization-based Keep past frames active (re-linearize) Tight coupling Marginalization factors
5. 5 Proposed graph structure 𝑚 𝑥1 𝑥2 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 𝑥8 Matching cost factor IMU factor Marginalization factor Active sensor state Marginalized state Map points Scan-to-scan registration factors (GICP) Scan-to-map registration factors (GICP) IMU factors Optimization-based Keep past frames active (re-linearize) Tight coupling Marginalization factors
6. Scan-to-map registration factors (GICP) 6 Proposed graph structure 𝑚 𝑥1 𝑥2 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 𝑥8 Matching cost factor IMU factor Marginalization factor Active sensor state Marginalized state Map points Scan-to-scan registration factors (GICP) IMU factors Optimization-based Keep past frames active (re-linearize) Tight coupling Marginalization factors
7. 7 Proposed graph structure 𝑚 𝑥1 𝑥2 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 𝑥8 Matching cost factor IMU factor Marginalization factor Active sensor state Marginalized state Map points Scan-to-scan registration factors (GICP) Scan-to-map registration factors (GICP) IMU factors Optimization-based Keep past frames active (re-linearize) Tight coupling Marginalization factors
8. 8 Localization on map boundaries 𝑚 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 On the map boundary Propagate per-point constraints with degenerate DoF Per-point constraints available!! The tight coupling enable accurately incorporating and propagating per-point uncertainty We can use map information even if there is only a very small scan-to-map overlap (< 5%)
9. 9 Localization out of the map 𝑚 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 Out of the map The factor graph falls back to a range inertial odometry estimation Once the sensor comes back to the map, the estimation drift is smoothly corrected
10. 10 Incorporating degenerate registration factors 𝑚 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 Degenerate frames Degenerate registration constraints can safely be incorporated into the factor graph
11. 11 Global localization for initialization CT-ICP + iVox Preintergration LiDAR IMU Loose coupling 2D BnB scan matching Localizer [Dellenbach, 2022] [Bai, 2022] [Forster, 2015] Initial state estimation Global localization Pose tracking [Hess, 2016] Points Inertia SE3 traj Delta Gravity-aligned point cloud Init pose on map Blue: Gravity-aligned points Green: Input points for 2D BnB Orange: Pose tracking result
12. 12 Experimental results Indoor sequences Outdoor sequences Processing time No corruptions Can run on Jetson nano i7-9700K + RTX1660Ti 17.2 ms per frame (58FPS)
13. 13 Conclusion Dataset：https://zenodo.org/records/10122133 Range inertial localization with factor graph optimization • Tightly coupled scan-to-scan and scan-to-map registration factors • Sliding window optimization • Loose coupling-based initial state estimation