Multi-camera Visual-Inertial Simultaneous Localization and Mapping for Autonomous Valet Parking

Marcus Abate, Ariel Schwartz, Xue Iuan Wong, Wangdong Luo, Rotem Littman, Marc Klinger, Lars Kuhnert, Douglas Blue, Luca Carlone

TL;DR

This paper presents an enhanced multi-camera visual-inertial SLAM system, built upon the Kimera framework, specifically designed for autonomous valet parking. It achieves robust localization and dense free-space mapping by integrating multiple cameras, external odometry, and a novel loop-closure method, outperforming existing state-of-the-art systems.

What Problem Does This Paper Address?

Autonomous valet parking, while a crucial application for self-driving vehicles, presents unique challenges for localization and mapping. Unlike highway driving, parking scenarios often involve low speeds, dynamic obstacles, and GPS-denied environments like multi-story parking garages. Traditional visual-inertial SLAM (VI-SLAM) systems, often relying on a single camera, struggle with the accuracy and robustness needed for these complex situations. Furthermore, generating dense 3D maps of free space, essential for obstacle avoidance, is difficult with limited sensor configurations and in visually repetitive indoor environments. Existing multi-camera SLAM approaches often make simplifying assumptions (e.g., planar motion) or require expensive sensors like LiDAR, limiting their applicability.

Methods

The core contribution is an extension of the Kimera VI-SLAM framework to handle multiple cameras and external odometry sources. Key modifications include:

  • Multi-Camera Input: The system can now process asynchronous data from multiple monocular cameras in parallel, feeding them into a unified backend for state estimation.
  • External Odometry Integration: Wheel odometry measurements are incorporated as a factor in the VIO backend's factor graph, enhancing robustness.
  • Novel Loop Closure: A new loop-closure scheme is introduced that circumvents the limitations of traditional Perspective-n-Point (PnP) methods. This method uses a "scale-less" approach, focusing on rotation and translation direction, which proves more robust with sparse feature correspondences. They also explore a "rotation-only" variant.
  • Dense Free-Space Mapping: A novel method for dense 3D free-space mapping is developed. It combines a semantic segmentation network to identify ground planes with a homography-based technique to project these pixels into 3D. This point cloud is then fed into Kimera-Semantics for volumetric reconstruction and mesh generation.

Datasets

The system was evaluated on:

  • Simulated Datasets: Photo-realistic urban outdoor environments.
  • Real-World Datasets: Collected by Ford Motor Company using a prototype car in both indoor and outdoor parking scenarios. These datasets cover a variety of conditions, including occlusions, traffic, and visually similar indoor environments.

Key Insights and Ideas

  • Multi-camera Fusion is Key for Robustness: While individual camera performance varies, integrating data from multiple cameras generally improves overall SLAM accuracy, though careful extrinsic calibration is crucial.
  • PnP Loop Closure is Fragile: The traditional PnP method for loop closure proved to be less accurate and robust in their experiments, likely due to mismatches between detected 2D features and backend-tracked 3D landmarks.
  • Scale-less Loop Closure Offers a Better Alternative: The proposed scale-less loop closure method, focusing on rotation and translation direction, significantly reduces drift and improves trajectory estimation, especially in challenging scenarios.
  • Homography for Free-Space Mapping is Efficient: Using a homography to map segmented ground planes into 3D is an efficient way to generate dense free-space maps without requiring depth estimation networks or LiDAR.

Key Results

  • Outperforms State-of-the-Art: The multi-camera Kimera system consistently outperformed Vins-Fusion and ORB-SLAM3 in terms of trajectory accuracy (ATE RMSE and drift) across various datasets.
  • Low Trajectory Error: The system achieved an average trajectory error under 1% of the trajectory length across over 8 km of traveled distance.
  • Improved Loop Closure: The scale-less loop closure method significantly reduced drift, often by a factor of four, and achieved drift less than 1% of trajectory length in most datasets.
  • Accurate Free-Space Reconstruction: Geometric reconstruction accuracy for the free-space maps was comparable to using ground-truth poses, demonstrating the effectiveness of their mapping approach.

Limitations and Future Work

  • Extrinsic Calibration Sensitivity: The performance of the multi-camera system was sensitive to extrinsic calibration errors, particularly with side-mirror mounted cameras. The authors suggest automatic calibration in the backend factor graph as a potential solution.
  • Semantic Mapping Extension: While the system can handle semantic annotations, future work could extend the mapping to include other relevant semantic classes like parking spots and pedestrians for more comprehensive autonomous parking solutions.

Summary updated

Abstract

Localization and mapping are key capabilities for self-driving vehicles. In this paper, we build on Kimera and extend it to use multiple cameras as well as external (e.g., wheel) odometry sensors, to obtain accurate and robust odometry estimates in real-world problems. Additionally, we propose an effective scheme for closing loops that circumvents the drawbacks of common alternatives based on the Perspective-n-Point method and also works with a single monocular camera. Finally, we develop a method for dense 3D mapping of the free space that combines a segmentation network for free-space detection with a homography-based dense mapping technique. We test our system on photo-realistic simulations and on several real datasets collected on a car prototype developed by the Ford Motor Company, spanning both indoor and outdoor parking scenarios. Our multi-camera system is shown to outperform state-of-the art open-source visual-inertial-SLAM pipelines (Vins-Fusion, ORB-SLAM3), and exhibits an average trajectory error under 1% of the trajectory length across more than 8 km of distance traveled (combined across all datasets). A video showcasing the system is available at: youtu.be/H8CpzDpXOI8.
BibTeX
@inproceedings{abate2023multi,
  author = {Marcus Abate and Ariel Schwartz and Xue Iuan Wong and Wangdong Luo and Rotem Littman and Marc Klinger and Lars Kuhnert and Douglas Blue and Luca Carlone},
  title = {{Multi-camera Visual-Inertial Simultaneous Localization and Mapping for Autonomous Valet Parking}},
  booktitle = {ISER},
  year = {2023},
  url = {https://openreview.net/forum?id=G2yU6wjr1Z},
}
Figure