[Paper Review] OKVIS2: Realtime Scalable Visual-Inertial SLAM with Loop Closure

Stefan Leutenegger|arXiv (Cornell University)|Feb 18, 2022

Robotics and Sensor-Based LocalizationEngineering24 citations

TL;DR

OKVIS2 presents a realtime scalable VI-SLAM system with loop closure that builds pose-graph edges from marginalised observations and revives landmarks after loop closure, enabling larger optimisation windows.

ABSTRACT

Robust and accurate state estimation remains a challenge in robotics, Augmented, and Virtual Reality (AR/VR), even as Visual-Inertial Simultaneous Localisation and Mapping (VI-SLAM) getting commoditised. Here, a full VI-SLAM system is introduced that particularly addresses challenges around long as well as repeated loop-closures. A series of experiments reveals that it achieves and in part outperforms what state-of-the-art open-source systems achieve. At the core of the algorithm sits the creation of pose-graph edges through marginalisation of common observations, which can fluidly be turned back into landmarks and observations upon loop-closure. The scheme contains a realtime estimator optimising a bounded-size factor graph consisting of observations, IMU pre-integral error terms, and pose-graph edges -- and it allows for optimisation of larger loops re-using the same factor-graph asynchronously when needed.

Motivation & Objective

• Address robustness and scalability of VI-SLAM in long and repeated loop-closure scenarios.
• Enable realtime operation while maintaining accuracy through a bounded-size factor graph.
• Allow asynchronous loop-closure optimisation that can revive old landmarks and observations.
• Incorporate a lightweight dynamic-object segmentation to improve robustness in dynamic scenes.

Proposed method

• Construct pose-graph factors from marginalised observations to enlarge the optimisation window.
• Maintain a realtime estimator with a bounded-size factor graph including observations, IMU pre-integrated terms, and pose-graph edges.
• Asynchronously perform loop-closure optimisation and convert pose-graph edges back into observations/landmarks.
• Use a lightweight segmentation CNN on keyframes to remove dynamic observations running on CPU for portability.
• Select new keyframes using a co-visibility heuristic to manage graph size while preserving long-term constraints.
• Employ Ceres Solver for nonlinear least squares optimisation of visual, inertial, and pose-graph terms.

Experimental results

Research questions

• RQ1 How can VI-SLAM maintain robustness and accuracy in the presence of long or frequent loop closures?
• RQ2 Can a realtime estimator operate on a bounded-size graph while still leveraging information from older observations via pose-graph factors?
• RQ3 What is the impact of marginalising observations on loop-closure quality and reactivation of landmarks?
• RQ4 Does a CPU-based lightweight segmentation of dynamic regions improve VI-SLAM robustness without sacrificing realtime performance?
• RQ5 How does OKVIS2 compare to state-of-the-art VI-SLAM/VIO systems on standard datasets when loop closure is enabled?

Key findings

• OKVIS2 achieves competitive to superior accuracy compared with state-of-the-art approaches on EuRoC and TUM VI benchmarks.
• In EuRoC, OKVIS2 shows on-par or slightly better Absolute Trajectory Error (ATE) compared with ORB-SLAM3 and other baselines (causal vs non-causal evaluation noted).
• The system demonstrates strong loop-closure performance by reviving old landmarks and re-integrating observations after loop-closure optimisation.
• A bounded-size realtime estimator with an asynchronously optimised full graph around detected loops maintains realtime operation while leveraging long-term constraints.
• A lightweight CPU-based segmentation CNN reduces dynamic-region observations, improving accuracy in scenes with clouds/humans.
• OKVIS2 provides detailed comparative results across multiple datasets, highlighting its scalability and robustness to long-term drifts when loops are handled effectively.