Closed-Loop Benchmarking of Stereo Visual-Inertial SLAM Systems: Understanding the Impact of Drift and Latency on Tracking Accuracy

There is a long history of using filters in visual-inertial state estimation for mobile robots (e.g., EKF [2]; MSCKF [3]; [4]). The combination of sparse optical flow (e.g., KLT [5]) and MSCKF has been recognized as an efficient VI-SLAM solution [6, 7, 8]. A downside of most filter-based methods is the low mapping quality, which affects long-term navigation with location revisits.

VI-SLAM running Bundle Adjustment (BA) retains an explicit map, which promotes higher accuracy and long-term robustness of state estimation. To bound the cubic computational cost of BA, BA-based VI-SLAM typically works with a subset of historical information (keyframes and landmarks) sub-selected using a sliding window [9] or a covisibility graph [10]. Representative BA-based VI-SLAM includes feature-based OKVIS [11], KLT-based VINS-Fusion [12] and Kimera [13]. Closed-loop navigation with OKVIS has been demonstrated on both ground [14] and aerial robots [15]. Full navigation has been demonstrated with VINS-Fusion on a micro-air vehicle (MAV) [16]. Kimera [13] estimates 3D mesh on-the-fly, which benefits navigation.

Recently direct VI-SLAM systems have been derived; they do not require explicit feature extraction and matching. Direct systems jointly solve data association and state estimation by optimizing an objective functional using raw image readings. Direct VI-SLAM systems such as SVO [17] and ROVIO [18] have been integrated into closed-loop navigation systems [19, 20]. While both KLT and direct VI-SLAM are computationally cheaper than feature-based VI-SLAM, they are more sensitive to navigation-based conditions: e.g. they require accurate pose prediction (from inertial) and minimal light condition changes. Furthermore, both KLT and direct methods are mostly characterized by short-baseline feature matches. Feature descriptor matching, on the other hand, can find reliable long-baseline feature matches for improved state estimation (see Fig. 2).

Open-loop evaluation of different VI-SLAM methods has been extensively conducted in the literature, e.g. on multiple datasets [23, 24], on multiple computation devices [25, 26, 27], and for multiple synthetic environments [28, 29]. Closed-loop evaluation of different VI-SLAM methods in navigation tasks, however, is investigated less. On-board evaluation tends to be reported for individual implementations [30, 4]. One challenge of closed-loop evaluation is that closed-loop navigation is not just a software problem; the performance of the full system is affected by sensor choice, computational resources, system dynamics, and target environment. All these factors need to be experimentally controlled to comprehensively evaluate the performance of closed-loop navigation using VI-SLAM.

One way to conduct comprehensive and repeatable closed-loop evaluation is via simulation. Several existing simulators are commonly used in the robotics community. Gazebo [31] is one of the most popular simulators, with MAV-specific extensions such as RotorS [32]. AirSim [33] is another choice, with photorealistic renderings of visual data via Unreal Engine. A more recent development incorporates hardware in the loop [34]. The approach captures the trajectory of the actual robot on the fly, while rendering virtual visual data on a remote workstation to collect actual data under real physics and virtual data from an easy-to-extend renderer. However, ground truth acquisition relies on a MoCap device, which is hard to scale beyond room-sized environment. To properly benchmarking VI-SLAM in closed-loop navigation, the benchmarking framework needs to be re-configurable to cover a variety of factors, such as sensor configurations, computational & robot platforms, and target environments. Furthermore, ground truth coverage is required over the entire course of navigation. This work aims to fill a existing gap by presenting an open-source, closed-loop benchmarking framework that supports the above requirements, and serves to provide performance insights on representative VI-SLAM systems based on the closed-loop evaluation results.

Several publicly available stereo(-inertial) SLAM implementations were selected for integration into the closed-loop benchmarking system. The five implementations are:

1. 1.

   MSC: MSCKF-VIO [7] + MSF [36]. MSCKF-VIO is a tightly-coupled VIO system, with KLT-based front-end and MSCKF back-end. EKF-based sensor fusion, MSF [36], densifies the low-rate estimation output from MSCKF-VIO, before sending it to controller. No loop closing is included.

2. 2.

   VINS: VINS-Fusion [12] is a tightly-coupled VI-SLAM with KLT-based front-end and BA-based back-end. VINS has a large latency due to the BA. It does provide a low-latency, high-rate IMU propagation signal, which is sent to controller. VINS comes with loop closing, which is preferred in long-term revisit scenarios. A circular motion is executed to initialize VINS prior to starting the SLAM estimation process.

3. 3.

   SVO: SVO [17] + MSF. An efficient, loosely-coupled VIO system that consists of direct SVO and MSF fusion. No loop closing module. SVO has the lowest pose tracking latency of the methods listed.

4. 4.

   ORB: ORB-SLAM [22] + MSF. ORB-SLAM has a feature-based front-end and BA-based back-end. Due to the computational-costly feature extraction and matching, ORB-SLAM has a large visual estimation latency. Similar with MSC and SVO, MSF is integrated into ORB to generate a high-rate estimation signal.

5. 5.

   GF: Lazy-GF-ORB-SLAM [24] + MSF. A loosely-coupled modified version of ORB with two efficiency modifications: good feature and lazy stereo. Good feature matching performs targeted map-to-frame matching under an upper bounded matching budget. The lazy stereo modification partitions the stereo ORB-SLAM computations into those necessary for immediate pose estimation versus those that assist future pose estimation computations. The former is prioritized to run first, therefore enabling more rapidly output of pose estimation. These two modifications lower the latency without significant impact on the accuracy of pose estimation.

If no initialization approach is described, then the default is to keep the robot static for 10 seconds before starting a closed-loop/open-loop run.

The desired trajectory $d^{*}(t)\in\mathbb{R}^{2}$ is constructed from a series of specified waypoints using splines. An exponentially stabilizing trajectory tracking controller for Hilare-style robots [37] generates a kinematically feasible trajectory for the robot to follow. In the following discussion, constraints on accelerations and velocities are omitted for clarity, though they exist within the actual implementation.

The robot pose as a function of time $g(t)\in SE(2)$ obeys follows the control equations:

$$\dot{}g=g\cdot\begin{bmatrix}\nu\\ 0\\ \omega\end{bmatrix}\quad\text{and}\quad\begin{matrix}&\dot{\nu}={u}^{1}\\ &\dot{\omega}={u}^{2}\end{matrix}$$

(1)

where $\nu$ is the forward velocity and $\omega$ is the angular velocity, both in the body frame. The signal $u=(u^{1},u^{2})^{T}$ coordinates are the forward and angular acceleration (in body frame).

The controller used relies on the differential flatness of the robot motion to achieve exponential stabilization of a virtual point in front of the robot (by a distance $\lambda$) [37]. Define the $\lambda$-adjusted rotation matrix and angular velocity matrix,

$$R_{\lambda}=R\cdot\operatorname{diag}(1,\lambda)\quad\text{and}\quad\hat{\omega}(\lambda,\dot{\lambda})=\begin{bmatrix}0&-\lambda\omega\\ \frac{1}{\lambda}\omega&\frac{\dot{\lambda}}{\lambda}\end{bmatrix},$$

(2)

where $R$ is the rotation matrix given by the orientation in $g$. For $e_{1}$ the unit body $\hat{x}$-vector in the world frame, the trajectory tracking control law is

$${u}=c_{p}R^{-1}_{\lambda}\left(d^{*}-d-\lambda*Re_{1}\right)+c_{d}\left(R^{-1}_{\lambda}\dot{d}^{*}-{V}\right)\\ -c_{d}\dot{\lambda}e_{1}-\hat{\omega}(\lambda,\dot{\lambda}){V}-(\hat{\omega}(\lambda,\dot{\lambda})-c_{\lambda}I)\dot{\lambda}e_{1},$$

(3)

where $c_{p},c_{d},c_{\lambda}$ are feedback gains and ${V}=\big{[}\nu\,|\,\omega\big{]}^{T}$. The additional offset dynamics are

$$\dot{\lambda}=-c_{\lambda}(\lambda-\epsilon),\ \text{where}\ \lambda(0)>\epsilon>0,\ c_{\lambda}>0.$$

(4)

The dynamical system represented by Eqs 1-4 yields a reference trajectory of robot poses $g^{*}(t)$ and body velocity components ${V}^{*}(t)$ for tracking the desired trajectory $d^{*}(t)$. The offset variable $\lambda^{*}(t)$ can be ignored.

The real time trajectory controller drives the robot to track the reference trajectory based on feedback of the robot’s estimated state (an $SE(2)$ substate of the $SE(3)$ state estimate). These control commands are:

$$\begin{split}\nu_{cmd}&=k_{x}*\widetilde{x}+\nu^{*}\\ w_{cmd}&=k_{\theta}*\widetilde{\theta}+k_{y}*\widetilde{y}+\omega^{*}\\ \end{split}$$

(5)

where $[\widetilde{x},\widetilde{y},\widetilde{\theta}]^{T}\simeq\widetilde{g}={g}^{-1}g^{*}$ is the relative pose error between the current state $g$ and the desired state $g^{*}$ in body frame. In the absence of error, the control signal is ${V}^{*}(t)$.

This section describes the Gazebo-simulated environment for testing closed-loop trajectory tracking with VI-SLAM systems. The scene created for robot navigation is a virtual office world (Fig. 4). The world is based on the floor-plan of an actual office, with texture-mapped surfaces. The walls are placed 1m above the ground plane since collision checking and path planning is outside the scope of this paper. Introducing collision avoidance would add another coupling factor to the closed-loop system, which would introduce unneeded difficulty in identifying the source of tracking error (i.e., was it to avoid a collision or due to poor estimation?).

Six test trajectories were created for the closed-loop navigation experiments, each with different characteristics (Fig. 5). The first two are relatively short ($\sim$50m), with few to no revisits. The 3rd and 4th trajectories are both of medium length ($\sim$120m). The 3rd has many revisits as it retraces the trajectory once, whereas the 4th crosses earlier trajectory segments facing the opposite direction or transverse to them. The last 2 trajectories are long ($\sim$240m). The 5th retraces trajectory segments, while the 6th does so facing in the opposite direction. All trajectories have the same start point for the robot, the origin of the world. Three desired linear velocities are tested: 0.5m/s, 1.0m/s, and 1.5m/s. Based on these velocities, the navigation course in simulation lasted from 30 seconds up to 480 seconds.

Given that simulation and recorded open-loop benchmark data may not align, this section conducts a comparison for verification of the domain of applicability for the simulation. The comparison shows that simulated scenes have some overlap with existing benchmarks though they do not span the entire domain. Based on the similarity, closed-loop implementations should have predictive power when the closed-loop system is deployed in equivalent real-world settings.

A subset of the EuRoC sequences and the simulated open-loop sequences are characterized in Table I, using the benchmark properties evaluated in [39]. The duration profile is defined with medium describing an interval of $[2,10]$ minutes. The motion profile is categorized from low (0.5 m/s) to medium (1.0 m/s) to high (1.5 m/s). The revisit frequency is a function of the trajectory followed and how often there is co-visibility of features across trajectory segments that are temporally distant. One additional statistic captured is the average number of features tracked per frame using ORB (last column). Simulation sequences exhibit less texture than EuRoC ones. There is sufficient overlap in the characteristics of the two benchmarks, with the simulation reflecting slightly more diverse scenarios. Qualitatively, the simulation sequences are comparable or harder benchmarking cases relative to the EuRoC sequences.

To compare further, we ran open-loop benchmarking against ground truth to get a sense for the pose estimation properties of the VI-SLAM algorithms and whether the two benchmark sets agree in terms of relative ordering. The results averaged from 5-run repeats of the medium motion profile are summarized in Tables III and III, where the track loss cases are omitted (dashes). According to the tables, the ATE between VI-SLAM estimation and ground truth is usually lower for EuRoC sequences. Both SVO and VINS exhibit outliers in EuRoC relative to the prevailing values across all methods, with SVO having one and VINS having three. They occur for the MH sequences, suggesting that these might be more problematic in general for VINS and SVO. However, SVO and VINS also have one track failure for the simulated cases. Overall, the outcomes align with the previous claim that the simulation sequences are comparable to or harder than EuRoC. If track failure is added as a penalty to the simulation performance outcomes, then the rank ordering of the algorithms agree between EuRoC and simulation. GF typically has the lowest ATE, while VINS has the highest ATE. Furthermore, the relative orders of latencies for different VI-SLAM agree: SVO is lowest, and VINS is highest (MSC is unique in that is has mismatch). The comparisons support using simulation to benchmark VI-SLAM, with validity for specific deployment conditions.

Trajectory tracking performance is quantified in Tables V and V. The tracking RMSE between the desired and actual trajectories reflects an average of the 5-run repeats. Cases with average RMSE over 10m are considered navigation failures and omitted (dashes). The average latency of visual estimation in each VI-SLAM is reported in the bottom row of each table (algorithms sorted to be in ascending order).

According to Tables V and V, both VINS and ORB fail under multiple configurations. Compared to ORB, the success rate and RMS of GF are significantly improved. The reduction of visual estimation latency contributes to the improvement, since the open-loop outcomes of ORB and GF are similar in terms of drift, but are quite different in terms of latency. The outcomes suggest that meeting standard frame-rate latency levels ($\sim\!\!30$ms) is best, and quite possibly essential for good closed-loop trajectory tracking performance. The filter-based MSC is significantly affected by the IMU data quality, as it fails to navigate for multiple low-end IMU cases and higher velocity. The outcomes suggest an over-reliance on the IMU for pose estimation, which is supported by Fig. 2 where MSC has poor long-term data association for detected features. Being able to re-associate to lost tracks improves performance by linking against a known static point in the world and improving absolute position estimates. Otherwise, systems such as MSC rely on integrated estimates which have poor observability properties.

The last two approaches to review are $SVO$ and $GF$, which both successfully track the camera pose for all but one sequence, each. These are the two strongest performing methods. Interestingly they have different run-time properties. For both the open-loop and closed-loop evaluations, SVO has the lowest latency but the highest drift, while GF is the opposite. Their relative performance remained the same from open-loop to closed-loop, modulo a small fraction of sequences. It appears that low latencies are tolerant to higher drift, whereas lower drift permits higher latency. Overall, however, it appears that once the latency is low enough, it is better to target accuracy enhancements over latency enhancements for closed-loop trajectory tracking (for ground vehicles in mostly static, feature sufficient, environments). Comparing SVO and GF across the two IMU types indicates that high-end IMUs provide the best error scaling to ground speed, with GF being more consistent as the speed increased.

These quantitative outcomes can be seen qualitatively in Fig. 7 and 7, which trace the closed-loop robot trajectories for the different VI-SLAM methods. VINS goes out of bounds for many runs. Focusing on the traces of SVO and GF, it is clear that SVO has a higher estimation variance across the runs for a given sequence, while GF trajectories are more closely clustered. The properties hold irrespective of the IMU type. Overall, GF appears to be the strongst performer. As a modification of ORB, it seeks to reduce pose estimation latency while preserving the beneficial properties of ORB. The findings of this paper imply that prioritizing accuracy while striving to achieve sufficiently small latencies is an effective means to identifying a high performing VI-SLAM for autonomous, mobile robot applications. Some work is still needed to resolve the outlier cases for GF.