A. Visual SLAM

Monocular SLAM was first solved in MonoSLAM [13], [14], [52] using an extended Kalman filter (EKF) and Shi-Tomasi points that were tracked in subsequent images doing a guided search by correlation. Mid-term data association was significantly improved using techniques that guarantee that the feature matches used are consistent, achieving hand-held visual SLAM [53], [54].

In contrast, keyframe-based approaches estimate the map using only a few selected frames, discarding the information coming from intermediate frames. This allows to perform the more costly, but more accurate, BA optimization at keyframe rate. The most representative system was PTAM [16] that splits camera tracking and mapping into two parallel threads. Keyframe-based techniques are more accurate than filtering for the same computational cost [55], becoming the gold standard in visual SLAM and VO. Large-scale monocular SLAM was achieved in [56] using sliding-window BA and in [57] using a double-window optimization and a covisibility graph.

Building on these ideas, ORB-SLAM [2], [3] uses oriented fast and rotated brief (ORB) features, whose descriptor provides short-term and mid-term data association, builds a covisibility graph to limit the complexity of tracking and mapping, and performs loop closing and relocalization using the bag-of-words library DBoW2 [9], achieving long-term data association. To date, it is the only visual SLAM system integrating the three types of data association, which we believe is the key to its excellent accuracy. In this work, we improve its robustness in pure visual SLAM with the new Atlas system that starts a new map when tracking is lost and its accuracy in loopy scenarios with the new place recognition method with improved recall.

Direct methods do not extract features, but use directly the pixel intensities in the images, and estimate motion and structure by minimizing a photometric error. LSD-SLAM [20] was able to build large-scale semidense maps using high gradient pixels. However, map estimation was reduced to PG optimization, achieving lower accuracy than PTAM and ORB-SLAM [2]. The hybrid system semi-direct visual odometry (SVO) [23], [24] extracts FAST features, uses a direct method to track features and any pixel with nonzero intensity gradient from frame to frame, and optimizes camera trajectory and 3-D structure using reprojection error. SVO is extremely efficient, but, being a pure VO method, it only performs short-term data association, which limits its accuracy. Direct sparse odometry (DSO) [27] is able to compute accurate camera poses in situations where point detectors perform poorly, enhancing robustness in low textured areas or against blurred images. It introduces local photometric BA that simultaneously optimizes a window of seven recent keyframes and the inverse depth of the points. Extensions of this work include stereo [29], loop closing using features and DBoW2 [58], [59], and visual–inertial odometry [46]. Direct sparse mapping (DSM) [31] introduces the idea of map reusing in direct methods, showing the importance of mid-term data association. In all cases, the lack of integration of short-, mid-, and long-term data association results in lower accuracy than our proposal (see Section VII).

B. Visual–Inertial SLAM

The combination of visual and inertial sensors provides robustness to poor texture, motion blur, and occlusions and, in the case of monocular systems, makes scale observable.

Research in tightly coupled approaches can be traced back to multi-state constraint kalman filter (MSCKF) [33] where the EKF quadratic cost in the number of features is avoided by feature marginalization. The initial system was perfected in [34] and extended to stereo in [35] and [36]. The first tightly coupled VO system based on keyframes and BA was OKVIS [38], [39] which is also able to use monocular and stereo vision. While these systems rely on features, ROVIO [41], [42] feeds an EFK with photometric error using direct data association.

ORB-SLAM-VI [4] presented for the first time a visual–inertial SLAM system able to reuse a map with short-term, mid-term, and long-term data association, using them in an accurate local visual–inertial BA based on IMU preintegration [60], [61]. However, its IMU initialization technique was too slow, taking 15 s, which harmed robustness and accuracy. Faster initialization techniques were proposed in [62] and [63] based on a closed-form solution to jointly retrieve scale, gravity, accelerometer bias and initial velocity, as well as visual features depth. Crucially, they ignore IMU noise properties and minimize the 3-D error of points in space, and not their reprojection errors, that is the gold standard in feature-based computer vision. Our previous work [64] shows that this results in large unpredictable errors.

VINS-Mono [7] is a very accurate and robust monocular-inertial odometry system with loop closing that uses DBoW2 and 4 degrees of freedom (DoF) PG optimization and map-merging. Feature tracking is performed with Lucas–Kanade tracker, being slightly more robust than descriptor matching. In VINS-Fusion [44], it has been extended to stereo and stereo-inertial.

VI-DSO [46] extends DSO to visual–inertial odometry, proposing a BA that combines inertial observations with the photometric error of selected high gradient pixels, which renders very good accuracy. As the information from high gradient pixels is successfully exploited, the robustness in scene regions with poor texture is also boosted. Their initialization method relies on visual–inertial BA and takes 20–30 s to converge within 1% scale error.

The recent BASALT [47] is a stereo-inertial odometry system that extracts nonlinear factors from visual–inertial odometry to use them in BA and closes loops matching ORB features, achieving very good to excellent accuracy. Kimera [8] is a novel outstanding metric-semantic mapping system, but its metric part consists in stereo-inertial odometry plus loop closing with DBoW2 and PG optimization, achieving similar accuracy to VINS-Fusion.

In this work, we build on ORB-SLAM-VI and extend it to stereo-inertial SLAM. We propose a novel fast initialization method based on MAP estimation that properly takes into account visual and inertial sensor uncertainties and estimates the true scale with 5% error in 2 s, converging to 1% scale error in 15 s. All other systems discussed above are visual–inertial odometry methods, some of them extended with loop closing, and lack the capability of using mid-term data associations. We believe that this, together with our fast and precise initialization, is the key to the better accuracy consistently obtained by our system, even in sequences without loops.

C. Multimap SLAM

The idea of adding robustness to tracking losses during exploration by means of map creation and fusion was first proposed in [65] within a filtering approach. One of the first keyframe-based multimap systems was [66], but the map initialization was manual and the system was not able to merge or relate the different submaps. Multimap capability has been researched as a component of collaborative mapping systems, with several mapping agents and a central server that only receives information [67] or with bidirectional information flow as in C2TAM [68]. MOARSLAM [69] proposed a robust stateless client-server architecture for collaborative multidevice SLAM, but the main focus was the software architecture and did not report accuracy results.

More recently, CCM-SLAM [70], [71] proposes a distributed multimap system for multiple drones with bidirectional information flow, built on top of ORB-SLAM. Their focus is on overcoming the challenges of limited bandwidth and distributed processing, while ours is on accuracy and robustness, achieving significantly better results on the EuRoC dataset. SLAM [72] also proposes a multimap extension of ORB-SLAM2 but keeps submaps as separated entities while we perform seamless map merging, building a more accurate global map.

VINS-Mono [7] is a VO system with loop closing and multimap capabilities that rely on the place recognition library DBoW2 [9]. Our experiments show that ORB-SLAM3 is 2.6 times more accurate than VINS-Mono in monocular-inertial single-session operation on the EuRoC dataset, thanks to the ability to use mid-term data association. Our Atlas system also builds on DBoW2 but proposes a novel higher recall place recognition technique and performs a more detailed and accurate map merging using local BA, increasing the advantage to 3.2 times better accuracy than VINS-Mono in multisession operation on EuRoC.

A. Relocalization

A robust SLAM system needs the capability of relocalizing the camera when tracking fails. ORB-SLAM solves the relocalization problem by setting a perspective-n-points solver based on the ePnP algorithm [73], which assumes a calibrated pin-hole camera along all its formulation. To follow up with our approach, we need a PnP algorithm that works independently of the camera model used. For that reason, we have adopted maximum likelihood perspective-n-point algorithm [74] that is completely decoupled from the camera model as it uses projective rays as input. The camera model just needs to provide an unprojection function passing from pixels to projection rays, to be able to use relocalization.

B. Nonrectified Stereo SLAM

Most stereo SLAM systems assume that stereo frames are rectified, i.e., both images are transformed to pin-hole projections using the same focal length, with image planes coplanar, and are aligned with horizontal epipolar lines, such that a feature in one image can be easily matched by looking at the same row in the other image. However, the assumption of rectified stereo images is very restrictive and, in many applications, is neither suitable nor feasible. For example, rectifying a divergent stereo pair or a stereo fisheye camera would require severe image cropping, losing the advantages of a large FOV.

For that reason, our system does not rely on image rectification, considering the stereo rig as two monocular cameras having the following:

1. a constant relative SE(3) transformation between them;

2. optionally, a common image region that observes the same portion of the scene.

These constrains allow us to effectively estimate the scale of the map by introducing that information when triangulating new landmarks and in the BA optimization. Following up with this idea, our SLAM pipeline estimates a 6 DoF rigid body pose, whose reference system can be located in one of the cameras or in the IMU sensor, and represents the cameras with respect to the rigid body pose.

If both cameras have an overlapping area in which we have stereo observations, we can triangulate true scale landmarks the first time they are seen. The rest of both images still has a lot of relevant information that is used as monocular information in the SLAM pipeline. Features first seen in these areas are triangulated from multiple views, as in the monocular case.

A. Fundamentals

While, in pure visual SLAM, the estimated state only includes the current camera pose, in visual–inertial SLAM, additional variables need to be computed. These are the body pose Ti=[Ri,pi]∈SE(3) and velocity vi in the world frame, and the gyroscope and accelerometer biases, bgi and bai, which are assumed to evolve according to a Brownian motion. This leads to the state vector

View Source \begin{equation*} \mathcal {S}_i \doteq \lbrace \mathbf {T}_i, \mathbf {v}_i, \mathbf {b}^g_i, \mathbf {b}^a_i\rbrace . \tag{1} \end{equation*}

For visual–inertial SLAM, we preintegrate IMU measurements between consecutive visual frames, i and i+1, following the theory developed in [60] and formulated on manifolds in [61]. We obtain preintegrated rotation, velocity, and position measurements, denoted as ΔRi,i+1, Δvi,i+1, and Δpi,i+1, as well as a covariance matrix ΣIi,i+1 for the whole measurement vector. Given these preintegrated terms and states Si and Si+1, we adopt the definition of inertial residual rIi,i+1 from [61]

View Source \begin{equation*} \begin{split} \mathbf {r}_{\mathcal {I}_{i,i+1}}& =[{\bf r}_{\Delta \text{R}_{i,i+1}}, {\bf r}_{\Delta \text{v}_{i,i+1}}, {\bf r}_{\Delta \text{p}_{i,i+1}}]\\ {\bf r}_{\Delta \text{R}_{i,i+1}}& = \text{Log}\left(\Delta {\bf R}_{i,i+1}^\text{T} {\bf R}_i^\text{T} {\bf R}_{i+1}\right)\\ {\bf r}_{\Delta \text{v}_{i,i+1}}& = {\bf R}_i^\text{T} \left({\bf v}_{i+1} - {\bf v}_i - {\bf g} \Delta t_{i,i+1}\right) - \Delta {\bf v}_{i,i+1} \\ {\bf r}_{\Delta \text{p}_{i,i+1}} & = {\bf R}_i^\text{T} \left({{\bf p}_j} - {{\bf p}}_i - {{\bf v}}_i \Delta t_{i,i+1} - \frac{1}{2}{\bf g} \Delta t^2\right) - \Delta {\bf p}_{i,i+1} \end{split} \tag{2} \end{equation*}

rij=uij−Π(TCBT−1i⊕xj)(3)

View Source \begin{equation*} \mathbf {r}_{ij} = \mathbf {u}_{ij}-\Pi \left(\mathbf {T}_{\text{CB}}\mathbf {T}_i^{-1} \oplus \mathbf {x}_j\right) \tag{3} \end{equation*}

Combining inertial and visual residual terms, visual–inertial SLAM can be posed as a keyframe-based minimization problem [39]. Given a set of k+1 keyframes and its state S¯k≐{S0…Sk} and a set of l 3-D points and its state X≐{x0…xl−1}, the visual–inertial optimization problem can be stated as follows:

View Source \begin{equation*} \underset{\bar{\mathcal {S}}_{k}, \mathcal {X}}{\min } \left(\sum _{i = 1}^{k} \left\Vert \mathbf {r}_{\mathcal {I}_{i-1,i}}\right\Vert _{\Sigma ^{-1}_{\mathcal {I}_{i,i+1}}}^2 + \sum _{j = 0}^{l-1} \sum _{i \in \mathcal {K}^{j}} \rho _{\text{Hub}}\left(\left\Vert \mathbf {r}_{ij}\right\Vert _{\Sigma ^{-1}_{ij}}\right) \right) \tag{4} \end{equation*}

Fig. 2.

Factor graph representation for different optimizations along the system.

Show All

B. IMU Initialization

The goal of this step is to obtain good initial values for the inertial variables: body velocities, gravity direction, and IMU biases. Some systems like VI-DSO [46] try to solve from scratch visual–inertial BA, sidestepping a specific initialization process, obtaining slow convergence for inertial parameters (up to 30 s).

In this work, we propose a fast and accurate initialization method based on the following three key insights.

1. Pure monocular SLAM can provide very accurate initial maps [2], whose main problem is that scale is unknown. Solving first the vision-only problem will enhance IMU initialization.

2. As shown in [56], scale converges much faster when it is explicitly represented as an optimization variable, instead of using the implicit representation of BA.

3. Ignoring sensor uncertainties during IMU initialization produces large unpredictable errors [64].

So, taking properly into account sensor uncertainties, we state the IMU initialization as a MAP estimation problem, split into the following three steps.

1. Vision-Only MAP Estimation: We initialize pure monocular SLAM [2] and run it during 2 s, inserting keyframes at 4 Hz. After this period, we have an up-to-scale map composed of k=10 camera poses and hundreds of points, which is optimized using visual-only BA [Fig. 2(b)]. These poses are transformed to body reference, obtaining the trajectory T¯0:k=[R,p¯]0:k where the bar denotes up-to-scale variables in the monocular case.

2. Inertial-Only MAP Estimation: In this step, we aim to obtain the optimal estimation of the inertial variables, in the sense of MAP estimation, using only T¯0:k and inertial measurements between these keyframes. These inertial variables may be stacked in the inertial-only state vector

   Yk={s,Rwg,b,v¯0:k}(5)
   View Source \begin{equation*} \mathcal {Y}_k=\lbrace s, {\bf R}_{\text{w}g}, \mathbf {b}, \bar{\mathbf {v}}_{0:k} \rbrace \tag{5} \end{equation*} where s∈R+ is the scale factor of the vision-only solution; Rwg∈SO(3) is a rotation matrix used to compute gravity vector g in the world reference as g=RwggI, where gI=(0,0,G)T and G is the gravity magnitude; b=(ba,bg)∈R6 are the accelerometer and gyroscope biases assumed to be constant during initialization; and v¯0:k∈R3 is the up-to-scale body velocities from first to last keyframe, initially estimated from T¯0:k. At this point, we are only considering the set of inertial measurements I0:k≐{I0,1…Ik−1,k}. Thus, we can state an MAP estimation problem, where the posterior distribution to be maximized is
   p(Yk|I0:k)∝p(I0:k|Yk)p(Yk)(6)
   View Source \begin{equation*} p(\mathcal {Y}_k \vert \mathcal {I}_{0:k}) \propto p (\mathcal {I}_{0:k} \vert \mathcal {Y}_k) p(\mathcal {Y}_k) \tag{6} \end{equation*} where p(I0:k|Yk) stands for likelihood and p(Yk) for prior. Considering independence of measurements, the inertial-only MAP estimation problem can be written as
   Y∗k=argmaxYk(p(Yk)∏i=1kp(Ii−1,i|s,Rwg,b,v¯i−1,v¯i)).(7)
   View Source \begin{equation*} \mathcal {Y}_{k}^* = \underset{\mathcal {Y}_{k}}{\arg \max } \left(p(\mathcal {Y}_k) \prod _{i=1}^k p(\mathcal {I}_{i-1,i} \vert s, {\bf R}_{\text{w}g}, \mathbf {b}, \bar{\mathbf {v}}_{i-1}, \bar{\mathbf {v}}_{i}) \right). \tag{7} \end{equation*}

   Taking negative logarithm and assuming Gaussian error for IMU preintegration and prior distribution, this finally results in the optimization problem

   Y∗k=argminYk(∥b∥2Σ−1b+∑i=1k∥rIi−1,i∥2Σ−1Ii−1,i).(8)
   View Source \begin{equation*} \mathcal {Y}_k^* = \underset{\mathcal {Y}_{k}}{\arg \min } \left(\Vert \mathbf {b}\Vert _{\Sigma ^{-1}_b}^2 + \sum _{i=1}^{k} \Vert \mathbf {r}_{\mathcal {I}_{i-1,i}}\Vert _{\Sigma ^{-1}_{\mathcal {I}_{i-1,i}}}^2 \right) . \tag{8} \end{equation*}

   This optimization, represented in Fig. 2(c), differs from (4) in not including visual residuals, as the up-to-scale trajectory estimated by visual SLAM is taken as constant, and adding a prior residual that forces IMU biases to be close to zero. Covariance matrix Σb represents prior knowledge about the range of values IMU biases may take. Details for preintegration of IMU covariance ΣIi−1,i can be found at [61].

   As we are optimizing in a manifold, we need to define a retraction [61] to update Rwg during the optimization. Since rotation around gravity direction does not suppose a change in gravity, this update is parameterized with two angles (δαg,δβg)

   Rnewwg=RoldwgExp(δαg,δβg,0)(9)
   View Source \begin{equation*} {\bf R}_{ \text{wg}}^{\text{new}} = {\bf R}_{ \text{wg}}^{\text{old}} \text{Exp}(\delta \alpha _{{\bf g}}, \delta \beta _{{\bf g}}, 0) \tag{9} \end{equation*} with Exp(.) being the exponential map from R3 to SO(3). To guarantee that scale factor remains positive during optimization, we define its update as
   snew=soldexp(δs).(10)
   View Source \begin{equation*} s^{\text{new}} = s^{\text{old}} \exp {(\delta s).} \tag{10} \end{equation*}

   Once the inertial-only optimization is finished, the frame poses and velocities and the 3-D map points are scaled with the estimated scale factor and rotated to align the z-axis with the estimated gravity direction. Biases are updated and IMU preintegration is repeated, aiming to reduce future linearization errors.

3. Visual–Inertial MAP Estimation: Once we have a good estimation for inertial and visual parameters, we can perform a joint visual–inertial optimization for further refining the solution. This optimization may be represented as Fig. 2(a) but having common biases for all keyframes and including the same prior information for biases than in the inertial-only step.

Our exhaustive initialization experiments on the EuRoC dataset [6] show that this initialization is very efficient, achieving 5% scale error with trajectories of 2 s. To improve the initial estimation, visual–inertial BA is performed 5 and 15 s after initialization, converging to 1% scale error as shown in Section VII. After these BAs, we say that the map is mature, meaning that scale, IMU parameters, and gravity directions are already accurately estimated.

Our initialization is much more accurate than joint initialization methods that solve a set of algebraic equations [62]–​[64] and much faster than the initialization used in ORB-SLAM-VI [4] that needed 15 s to get the first scale estimation or that used in VI-DSO [46] that starts with a huge scale error and requires 20–30 s to converge to 1% error. Comparisons between different initialization methods may be found at [6].

In some specific cases, when slow motion does not provide good observability of the inertial parameters, initialization may fail to converge to accurate solutions in just 15 s. To get robustness against this situation, we propose a novel scale refinement technique based on a modified inertial-only optimization, where all inserted keyframes are included, but scale and gravity directions are the only parameters to be estimated [Fig. 2(d)]. Note that, in that case, the assumption of constant biases would not be correct. Instead, we use the values estimated from mapping, and we fix them. This optimization, which is very computationally efficient, is performed in the local mapping thread every 10 s until the map has more than 100 keyframes or more than 75 s have passed since initialization.

Finally, we have easily extended our monocular-inertial initialization to stereo-inertial by fixing the scale factor to one and taking it out from the inertial-only optimization variables, enhancing its convergence.

C. Tracking and Mapping

For tracking and mapping, we adopt the schemes proposed in [4]. Tracking solves a simplified visual–inertial optimization where only the states of the last two frames are optimized, while map points remain fixed.

For mapping, trying to solve the whole optimization from (4) would be intractable for large maps. We use as optimizable variables a sliding window of keyframes and their points, including also observations to these points from covisible keyframes but keeping their pose fixed.

D. Robustness to Tracking Loss

In pure visual SLAM or VO systems, temporal camera occlusion and fast motions result in losing track of visual elements, getting the system lost. ORB-SLAM pioneered the use of fast relocalization techniques based on bag-of-words place recognition, but they proved insufficient to solve difficult sequences in the EuRoC dataset [3]. Our visual–inertial system enters into visually lost state when less than 15 point maps are tracked and achieves robustness in the following two stages:

1. Short-term lost: The current body state is estimated from IMU readings, and map points are projected in the estimated camera pose and searched for matches within a large image window. The resulting matches are included in visual–inertial optimization. In most cases, this allows to recover visual tracking. Otherwise, after 5 s, we pass to the next stage.

2. Long-term lost: A new visual–inertial map is initialized as explained above, and it becomes the active map.

A. Place Recognition

To achieve higher recall, for every new active keyframe, we query the DBoW2 database for several similar keyframes in the Atlas. To achieve 100% precision, each of these candidates goes through several steps of geometric verification. The elementary operation of all the geometrical verification steps consists in checking whether there is an ORB keypoint inside an image window whose descriptor matches the ORB descriptor of a map point, using a threshold for the Hamming distance between them. If there are several candidates in the search window, to discard ambiguous matches, we check the distance ratio to the second-closest match [76]. The steps of our place recognition algorithm are as follows.

1. DBoW2 candidate keyframes. We query the Atlas DBoW2 database with the active keyframe Ka to retrieve the three most similar keyframes, excluding keyframes covisible with Ka. We refer to each matching candidate for place recognition as Km.

2. Local window. For each Km, we define a local window that includes Km, its best covisible keyframes, and the map points observed by all of them. The DBoW2 direct index provides a set of putative matches between keypoints in Ka and in the local window keyframes. For each of these 2-D–2-D matches, we have also the 3-D–3-D match available between their corresponding map points.

3. 3-D aligning transformation. We compute using RANSAC the transformation Tam that better aligns the map points in Km local window with those of Ka. In pure monocular, or in monocular-inertial when the map is still not mature, we compute Tam∈Sim(3); otherwise, Tam∈SE(3). In both cases, we use Horn algorithm [77] using a minimal set of three 3-D–3-D matches to find each hypothesis for Tam. The putative matches that, after transforming the map point in Ka by Tam, achieve a reprojection error in Ka below a threshold give a positive vote to the hypothesis. The hypothesis with more votes is selected, provided the number is over a threshold.

4. Guided matching refinement. All the map points in the local window are transformed with Tam to find more matches with the keypoints in Ka. The search is also reversed, finding matches for Ka map points in all the keyframes of the local window. Using all the matchings found, Tam is refined by nonlinear optimization, where the goal function is the bidirectional reprojection error, using Huber influence function to provide robustness to spurious matches. If the number of inliers after the optimization is over a threshold, a second iteration of guided matching and nonlinear refinement is launched, using a smaller image search window.

5. Verification in three covisible keyframes. To avoid false positives, DBoW2 waited for place recognition to fire in three consecutive keyframes, delaying or missing place recognition. Our crucial insight is that, most of the time, the information required for verification is already in the map. To verify place recognition, we search in the active part of the map two keyframes covisible with Ka where the number of matches with points in the local window is over a threshold. If they are not found, the validation is further tried with the new incoming keyframes, without requiring the bag-of-words to fire again. The validation continues until three keyframes verify Tam or two consecutive new keyframes fail to verify it.

6. VI gravity direction verification. In the visual–inertial case, if the active map is mature, we have estimated Tam∈SE(3). We further check whether the pitch and roll angles are below a threshold to definitively accept the place recognition hypothesis.

B. Visual Map Merging

When a successful place recognition produces multimap data association between keyframe Ka in the active map Ma, and a matching keyframe Km from a different map stored in the Atlas Mm, with an aligning transformation Tam, we launch a map merging operation. In the process, special care must be taken to ensure that the information in Mm can be promptly reused by the tracking thread to avoid map duplication. For this, we propose to bring the Ma map into Mm reference. As Ma may contain many elements and merging them might take a long time, merging is split into two steps. First, the merge is performed in a welding window defined by the neighbors of Ka and Km in the covisibility graph, and in a second stage, the correction is propagated to the rest of the merged map by a PG optimization. The detailed steps of the merging algorithm are as follows.

1. Welding window assembly. The welding window includes Ka and its covisible keyframes, Km and its covisible keyframes, and all the map points observed by them. Before their inclusion in the welding window, the keyframes and map points belonging to Ma are transformed by Tma to align them with respect to Mm.

2. Merging maps. Maps Ma and Mm are fused together to become the new active map. To remove duplicated points, matches are actively searched for Ma points in the Mm keyframes. For each match, the point from Ma is removed, and the point in Mm is kept accumulating all the observations of the removed point. The covisibility and essential graphs [2] are updated by the addition of edges connecting keyframes from Mm and Ma, thanks to the new mid-term point associations found.

3. Welding bundle adjustment. A local BA is performed optimizing all the keyframes from Ma and Mm in the welding window along with the map points which are observed by them [Fig. 3(a)]. To fix gauge freedom, the keyframes of Mm not belonging to the welding window but observing any of the local map points are included in the BA with their poses fixed. Once the optimization finishes, all the keyframes included in the welding area can be used for camera tracking, achieving fast and accurate reuse of map Mm.

4. Essential-graph optimization. A PG optimization is performed using the essential graph of the whole merged map, keeping fixed the keyframes in the welding area. This optimization propagates corrections from the welding window to the rest of the map.

Fig. 3.

Factor graph representation for the welding BA, with reprojection error terms (blue squares), IMU preintegration terms (yellow squares), and bias random walk (purple squares). (a) Visual welding BA. (b) Visual-Inertial welding BA.

Show All

C. Visual–Inertial Map Merging

The visual–inertial merging algorithm follows similar steps than the pure visual case. Steps 1) and 3) are modified to better exploit the inertial information.

1. VI welding window assembly: If the active map is mature, we apply the available Tma∈SE(3) to map Ma before its inclusion in the welding window. If the active map is not mature, we align Ma using the available Tma∈Sim(3).

2. VI welding bundle adjustment: Poses, velocities, and biases of keyframes Ka and Km and their five last temporal keyframes are included as optimizable. These variables are related by IMU preintegration terms, as shown in Fig. 3(b). For Mm, the keyframe immediately before the local window is included but fixed, while, for Ma, the similar keyframe is included, but its pose remains optimizable. All map points seen by the above-mentioned keyframes are optimized, together with poses from Km and Ka covisible keyframes. All keyframes and points are related by means of reprojection error.

D. Loop Closing

Loop closing correction algorithm is analogous to map merging but in a situation where both keyframes matched by place recognition belong to the active map. A welding window is assembled from the matched keyframes, and point duplicates are detected and fused creating new links in the covisibility and essential graphs. The next step is a PG optimization to propagate the loop correction to the rest of the map. The final step is a global BA to find the MAP estimate after considering the loop closure mid-term and long-term matches. In the visual–inertial case, the global BA is only performed if the number of keyframes is below a threshold to avoid a huge computational cost.

A. Single-Session SLAM on EuRoC

Table II compares the performance of ORB-SLAM3 using its four sensor configurations with the most relevant systems in the state of the art. Our reported values are the median after 10 executions. As shown in the table, ORB-SLAM3 achieves in all sensor configurations more accurate result than the best systems available in the literature, in most cases by a wide margin.

TABLE II Performance Comparison in the EuRoC Dataset (RMS ATE in M., Scale Error in %). Except Where Noted, We Show Results Reported by the Authors of Each System, for All the Frames in the Trajectory, Comparing With the Processed GT

In monocular and stereo configurations, our system is more precise than ORB-SLAM2 due to the better place recognition algorithm that closes loops earlier and provides more mid-term matches. Interestingly, the next best results are obtained by DSM that also uses mid-term matches, even though it does not close loops.

In monocular-inertial configuration, ORB-SLAM3 is five to ten times more accurate than MCSKF, OKVIS, and ROVIO and more than double the accuracy of VI-DSO and VINS-Mono, showing again the advantages of mid-term and long-term data associations. Compared with ORB-SLAM VI, our novel fast IMU initialization allows ORB-SLAM3 to calibrate the inertial sensor in a few seconds and uses it from the very beginning, being able to complete all EuRoC sequences, obtaining better accuracy.

In stereo-inertial configuration, ORB-SLAM3 is three to four times more accurate than Kimera and VINS-Fusion. Its accuracy is only approached by the recent BASALT that, being a native stereo-inertial system, was not able to complete sequence V203, where some frames from one of the cameras are missing. Comparing our monocular-inertial and stereo-inertial systems, the latter performs better in most cases. Only for two Machine Hall (MH) sequences, a lower accuracy is obtained. We hypothesize that greater depth scene for MH sequences may lead to less accurate stereo triangulation and, hence, a less precise scale.

To summarize performance, we have presented the median of ten executions for each sensor configuration. For a robust system, the median represents accurately the behavior of the system. But a nonrobust system will show high variance in its results. This can be analyzed using Fig. 4 that shows with colors the error obtained in each of the ten executions. Comparison with the figures for DSO, ROVIO, and VI-DSO published in [46] confirms the superiority of our method.

Fig. 4.

Colored squares represent the rms ATE for ten different executions in each sequence of the EuRoC dataset.

Show All

In pure visual configurations, the multimap system adds some robustness to fast motions by creating a new map when tracking is lost, which is merged later with the global map. This can be seen in sequences V103 monocular and V203 stereo that could not be solved by ORB-SLAM2 and are successfully solved by our system in most executions. As expected, stereo is more robust than monocular, thanks to its faster feature initialization, with the additional advantage that the real scale is estimated.

However, the big leap in robustness is obtained by our novel visual–inertial SLAM system, both in monocular and stereo configurations. The stereo-inertial system has a very slight advantage over monocular-inertial, particularly in the most challenging V203 sequence.

We can conclude that inertial integration not only boosts accuracy, reducing the median ATE error compared to pure visual solutions, but it also endows the system with excellent robustness, having a much more stable performance.

B. Visual–Inertial SLAM on TUM-VI Benchmark

The TUM-VI dataset [80] consists of 28 sequences in 6 different environments, recorded using a hand-held fisheye stereo-inertial rig. Ground-truth for the trajectory is only available at the beginning and at the end of the sequences, which, for most of them, represents a very small portion of the whole trajectory. Many sequences in the dataset do not contain loops. Even if the starting and ending points are in the same room, point of view directions are opposite and place recognition cannot detect any common region. Using this ground-truth for evaluation amounts to measuring the accumulated drift along the whole trajectory.

We extract 1500 ORB points per image in monocular-inertial setup and 1000 points per image in stereo-inertial, after applying CLAHE equalization to address under and over exposure found in the dataset. For outdoor sequences, our system struggles with very far points coming from the cloudy sky, which is very visible in fisheye cameras. These points may have slow motion that can introduce drift in the camera pose. For preventing this, we discard points further than 20 m from the current camera pose, only for outdoor sequences. A more sophisticated solution would be to use an image segmentation algorithm to detect and discard the sky.

The results obtained are compared with the most relevant systems in the literature in Table III, which clearly shows the superiority of ORB-SLAM3 both in monocular-inertial and stereo-inertial configurations. The closest systems are VINS-Mono and BASALT, which are essentially visual–inertial odometry systems with loop closures, and miss mid-term data associations.

TABLE III TUM-VI Benchmark [80]: RMS ATE (m) for Regions With Available Ground-Truth Data

Analyzing more in detail the performance of our system, it gets the lowest error in small and medium indoor environments, room and corridor sequences, with errors below 10 cm for most of them. In these trajectories, the system is continuously revisiting and reusing previously mapped regions, which is one of the main strengths of ORB-SLAM3. Also, tracked points are typically closer than 5 m, which makes it easier to estimate inertial parameters, preventing them from diverging.

In magistrale indoor sequences, which are up to 900 m long, most tracked points are relatively close, and ORB-SLAM3 obtains errors around 1 m except in one sequence that goes close to 5 m. In contrast, in some long outdoor sequences, the scarcity of close visual features may cause drift of the inertial parameters, notably scale and accelerometer bias, which leads to errors in the order of 10–70 m. However, ORB-SLAM3 is the best performing system in the outdoor sequences.

This dataset also contains three really challenging slides sequences, where the user descends though a dark tubular slide with almost total lack of visual features. In this situation, a pure visual system would be lost, but our visual–inertial system is able to process the whole sequence with competitive error, even if no loop-closures can be detected. Interestingly, VINS-Mono and BASALT, which track features using Lukas–Kanade, obtain, in some of these sequences, better accuracy than ORB-SLAM3, which matches ORB descriptors.

Finally, the room sequences can be representative of typical AR/VR applications, where the user moves with a hand-held or head-mounted device in a small environment. For these sequences, ground-truth is available for the entire trajectory. Table III shows that ORB-SLAM3 is significantly more accurate than competing approaches. The results obtained using our four sensor configurations are compared in Table IV. The better accuracy of pure monocular compared with stereo is only apparent: The monocular solution is up-to-scale and is aligned with ground-truth with 7 DoFs, while stereo provides the true scale and is aligned with 6 DoFs. Using monocular-inertial, we further reduce the average rms ATE error close to 1 cm, also obtaining the true scale. Finally, our stereo-inertial SLAM brings error below 1 cm, making it an excellent choice for AR/VR applications.

TABLE IV RMS ATE (m) Obtained by ORB-SLAM3 With Four Sensor Configurations in the Room Sequences, Representative of AR/VR Scenarios (Median of Three Executions)

C. Multisession SLAM

EuRoC dataset contains several sessions for each of its three environments: 5 in MH, 3 in Vicon1, and 3 in Vicon2. To test the multisession performance of ORB-SLAM3, we process sequentially all the sessions corresponding to each environment. Each trajectory in the same environment has ground-truth with the same world reference, which allows to perform a single global alignment to compute ATE.

The first sequence in each room provides an initial map. Processing the following sequences starts with the creation of a new active map, which is quickly merged with the map of the previous sessions, and from that point on, ORB-SLAM3 profits from reusing the previous map.

Table V reports the global multisession rms ATE for the four sensor configurations in the three rooms, comparing with the two only published multisession results in EuRoC dataset: CCM-SLAM [71] that reports pure monocular results in MH01-MH03, and VINS-Mono [7] in the five MH sequences, using monocular-inertial. In both cases, ORB-SLAM3 more than doubles the accuracy of competing methods. In the case of VINS-Mono, ORB-SLAM3 obtains 2.6 better accuracy in single-session, and the advantage goes up to 3.2 times in multisession, showing the superiority of our map merging operations.

TABLE V Multisession RMS ATE (m) on the EuRoC Dataset. For CCM-SLAM and VINS, We Show Results Reported by the Authors of Each System. Our Values Are the Median of Five Executions, Aligning the Trajectories With the Processed GT

Comparing these multisession performances with the single-session results reported in Table II, the most notable difference is that multisession monocular and stereo SLAM can robustly process the difficult sequences V103 and V203, thanks to the exploitation of the previous map.

We have also performed some multisession experiments on the TUM-VI dataset. Fig. 5 shows the result after processing several sequences inside the TUM building.¹ In this case, the small room sequence provides loop closures that were missing in the longer sequences, bringing all errors to centimeter level. Although ground-truth is not available outside the room, comparing the figure with the figures published in [82] clearly shows our point: Our multisession SLAM system obtains far better accuracy than existing visual–inertial odometry systems. This is further exemplified in Fig. 6. Although ORB-SLAM3 ranks higher in stereo inertial single-session processing of outdoors1, there is still a significant drift (≈60 m). In contrast, if outdoors1 is processed after magistrale2 in a multisession manner, this drift is significantly reduced, and the final map is much more accurate.

Fig. 5.

Multisession stereo-inertial result with several sequences from TUM-VI dataset (front, side, and top views).

Show All

Fig. 6.

Multisession stereo-inertial. In red, the trajectory estimated after single-session processing of outdoors1. In blue, multisession processing of magistrale2 first and then outdoors1.

Show All

D. Computing Time

Table VI summarizes the running time of the main operations performed in the tracking and mapping threads, showing that our system is able to run in real time at 30–40 frames and at 3–6 keyframes per second. The inertial part takes negligible time during tracking and, in fact, can render the system more efficient as the frame rate could be safely reduced. In the mapping thread, the higher number of variables per keyframe has been compensated with a smaller number of keyframes in the inertial local BA, achieving better accuracy, with similar running time. As the tracking and mapping threads work always in the active map, multimapping does not introduce significant overhead.

TABLE VI Running Time of the Main Parts of Our Tracking and Mapping Threads Compared to ORB-SLAM2, on EuRoC V202 (Mean Time and Standard Deviation in Ms)

Table VII summarizes the running time of the main steps for loop closing and map merging. The novel place recognition method only takes 10 ms per keyframe. Times for merging and loop closing remain below 1 s, running only a PG optimization. For loop closing, performing a full BA may increase times up to a few seconds, depending on the size of the involved maps. In any case, as both operations are executed in a separate thread (Fig. 1), they do not interfere with the real-time performance of the rest of the system. The visual–inertial systems perform just two map merges to join three sequences, while visual systems perform some additional merges to recover from tracking losses. Thanks to their lower drift, visual–inertial systems also perform less loop closing operations compared with pure visual systems.

TABLE VII Running Time of the Main Operations for Loop Closing and Map Merging for a Multisession Experiment on Sequences V201, V202, and V203 From EuRoC Dataset (Mean Time and Standard Deviation in Ms)

Although it would be interesting, we do not compare running time against other systems since this would require a significant effort that is beyond the scope of this work.