7.2.1 Visual Odometry

The task of incrementally tracking the camera motion from video is commonly called visual odometry. It was established by Nister et al. [7]. They proposed to match a sparse set of features from frame to frame, yielding multiple observations of the same point in different images. The 3D camera motion is then computed by minimizing the reprojection error. Afterwards there were many other works based on this feature-based approach. The general idea is to extract and track features over the images and then minimize the reprojection error of them. Later works like MonoSLAM [8] and PTAM [9] added the ability to create a consistent map in real-time. A very recent example is ORB-SLAM [10]. It is capable of tracking the camera motion and creating a consistent map with the ability to close trajectory loops and re-localization, outperforming previous methods in accuracy and robustness.

Another approach to visual odometry are the direct methods. Instead of the tracking features and minimizing the reprojection error, they directly use the image data to obtain the camera motion by optimizing a photometric error. In the work of [11] this error was minimized in real-time in a stereo setting by formulating quadrifocal constraints on the pixels. Especially for tracking with RGB-D cameras direct solutions were proposed by Newcombe et al. [12] and Kerl et al. [13]. Recently, several direct approaches were proposed, which use only a monocular camera. They differ in the density of the image information used. Newcombe et al. [14] proposed a dense approach which uses the complete image for tracking. However, for monocular visual odometry mainly image regions with a sufficient gradient contribute information. Therefore Engel et al. [15] have presented a semi-dense SLAM system exploiting this fact. Although sparse methods have traditionally been keypoint-based, direct sparse methods have advantages discussed in two recent publications. SVO proposed by Forster et al. [16] uses a keypoint-based approach for mapping keyframes and performs sparse direct image alignment to track non-keyframes. In contrast DSO proposed by Engel et al. [4] is a purely direct and sparse system. By using a sparse set of points it can efficiently perform bundle adjustment on a window of keyframes yielding superior accuracy and robustness compared to the semi-dense approach.

7.2.2 Visual–Inertial Odometry

IMU sensors are in many ways complementary to vision. The accelerometer and gyroscope measurements provide good short-term motion information. Furthermore they make scale as well as the roll and pitch angles observable, which is relevant for some control tasks, like the navigation of drones. Therefore there were several methods which combine them with vision. They can be classified into two general approaches.

The first one is the so-called loosely coupled fusion. Here the vision system is used as a blackbox, providing pose measurements, which are then combined with the inertial data, usually using an (extended or unscented) Kalman filter. Examples of this are shown in [1], [17] and [18].

Recently, tightly coupled works became more popular. Here the motion of the camera is computed by jointly optimizing the parameters in a combined energy function consisting of a visual and an inertial error term. The advantage is that the two measurement sources are combined on a very deep level of the system yielding a very high accuracy and especially robustness. Especially for vision systems that are based on nonlinear optimization the inertial error term can help overcome nonlinearities in the energy functional. The disadvantage is that the tightly coupled fusion has to be done differently for different vision systems making it much harder to implement. Still a tightly-coupled fusion has been done for filtering-based approaches [19] [20] [21] as well as for energy-minimization based systems [22] [23] [24] [25] [26].

Especially for monocular energy-minimization based approaches it is important to start with a good visual–inertial initialization. In practical use-cases the system does not have any prior knowledge about initial velocities, poses and geometry. Therefore apart from the usual visual initialization procedure most visual–inertial systems need a specific visual–inertial initialization. However, the problem is even more difficult, because several types of motion do not allow one to uniquely determine all variables. In [27] a closed-form solution for visual–inertial initialization was presented, as well as an analysis of the exceptional cases. It was later extended to work with IMU biases by [28]. In [25] a visual–inertial initialization procedure was proposed that works on the challenging EuRoC dataset. They use 15 seconds of flight data for initialization to make sure that all variables are observable on this specific dataset. Also in [26] and [29] IMU initialization methods are proposed which work on the EuRoC dataset, the latter even works without prior initialization of IMU extrinsics.

7.3.1 Gauss–Newton Algorithm

The goal of the Gauss–Newton algorithm is to solve optimization problems of the form

$\underset{\mathit{x}}{\min }E(\mathit{x})=\underset{\mathit{x}}{\min }\sum _i{w}_i\cdot r_i{(\mathit{x})}^2$

where each $r_i$ is a residual and $w_i$ is the associated weight.

We will start with an explanation of the general Newton methods. The idea is based on a Taylor approximation of the energy function around ${\mathit{x}}_0$,

$E(\mathit{x})\approx E({\mathit{x}}_0)+{\mathit{b}}^T(\mathit{x}-{\mathit{x}}_0)+\frac{1}{2}{(\mathit{x}-{\mathit{x}}_0)}^T\mathbf{H}(\mathit{x}-{\mathit{x}}_0)$

where $\mathit{b}=\frac{dE}{dx}$ is the gradient and $\mathbf{H}=\frac{d^{2}E}{d{x}^2}$ is the Hessian.

Using this approximation the optimal solution is where

$0\stackrel{!}{=}\frac{dE}{d\mathit{x}}=\mathit{b}+\mathbf{H}(\mathit{x}-{\mathit{x}}_0).$

The optimal solution is therefore at

$\mathit{x}={\mathit{x}}_0-{\mathbf{H}}^{-1}\cdot \mathit{b}.$

Of course, this is not the exact solution because we used an approximation to derive it. Therefore the optimization is performed iteratively by starting with an initial solution ${\mathit{x}}_0$ and successively calculating

${\mathit{x}}_{t+1}={\mathit{x}}_t-\lambda \cdot {\mathbf{H}}^{-1}\cdot \mathit{b}$

where λ is the step size.

Calculating the Hessian is very time-intensive. Therefore the idea of the Gauss–Newton method is to use an approximation for it instead. In the case where the problem has the form of Eq. (7.1) we can compute

$b_j=2\sum _i{w}_i\cdot r_i(\mathit{x})\frac{\partial r_i}{\partial x_j},$

${\mathbf{H}}_{jk}=2\sum _i{w}_i(\frac{\partial r_i}{\partial x_j}\frac{\partial r_i}{\partial x_k}+r_i\frac{{\partial }^2{r}_i}{\partial x_j\partial x_k})\approx 2\sum _i{w}_i\left(\frac{\partial r_i}{\partial x_j}\frac{\partial r_i}{\partial x_k}\right).$

We define the stacked residual vector r by stacking all residuals $r_i$. The Jacobian J of r therefore contains at each element

${\mathbf{J}}_{ij}=\frac{\partial r_i}{x_j}.$

We also stack the weights into a diagonal matrix W.

Then the approximated Hessian can be calculated with

$\mathbf{H}\approx 2\sum _i{w}_i{\mathbf{J}}_{ij}{\mathbf{J}}_{ik}=2{\mathbf{J}}^T\mathbf{W}\mathbf{J}.$

Similarly the gradient of the energy function is

$\mathit{b}=2{\mathbf{J}}^T\mathbf{W}\mathit{r}.$

In summary the Gauss–Newton algorithm iteratively computes updates using Eq. (7.5), where the Hessian is approximated using Eq. (7.9).

7.3.2 Levenberg–Marquandt Algorithm

When initialized to far off from the optimum it can happen that the Gauss–Newton algorithm does not converge. The idea of the Levenberg–Marquandt algorithm is to dampen the Gauss–Newton updates. It still uses the same definition of H and b as the Gauss–Newton algorithm.

When computing the iterative update, however, it uses

${\mathit{x}}_{t+1}={\mathit{x}}_t-{(\mathbf{H}+\lambda \cdot \text{diag}(\mathbf{H}))}^{-1}\cdot \mathit{b}.$

The rough idea is to create a mixture between the Gauss–Newton algorithm and a simple gradient descent. Intuitively for a value of $\lambda =0$ this will degenerate to a normal Gauss–Newton step and for a high value it will be dampened to something similar to gradient descent.

Clearly there is no optimal value for λ so it is usually adapted on the fly. After each update it is calculated whether or not the update reduces the energy (and the update is discarded if not). When an update is discarded λ is increased and when it is accepted λ gets reduced.

Compared to the Gauss–Newton algorithm the Levenberg–Marquandt method is in general more robust to a bad initialization.

7.4.1 Notation

Throughout the chapter we will use the following notation: bold upper case letters H represent matrices, bold lower case x vectors and light lower case λ represent scalars. Transformations between coordinate frames are denoted as ${\mathbf{T}}_{i\mathrm{\_}j}\in \mathbf{SE}(3)$ where point in coordinate frame i can be transformed to the coordinate frame j using the following equation: ${\mathbf{p}}_i={\mathbf{T}}_{i\mathrm{\_}j}{\mathbf{p}}_j$. We denote Lie algebra elements as $\hat{\mathit{\xi }}\in \mathfrak{se}(3)$, where $\mathit{\xi }\in {\mathbb{R}}^6$, and use them to apply small increments to the 6D pose ${\mathit{\xi }}_{i\mathrm{\_}j}^{\prime }={\mathit{\xi }}_{i\mathrm{\_}j}⊞\mathit{\xi }:=\log {(e^{{\hat{\mathit{\xi }}}_{i\mathrm{\_}j}}\cdot e^{\hat{\mathit{\xi }}})}^{\vee }$. We define the world as a fixed inertial coordinate frame with gravity acting along the negative Z axis. We also assume that the transformation from camera to IMU frame $T_{\text{imu}\mathrm{\_}\text{cam}}$ is fixed and calibrated in advance. Factor graphs are expressed as a set G of factors and we use $G_1\cup G_2$ to denote a factor graph containing all factors that are either in $G_1$ or in $G_2$.

7.4.2 Photometric Error

The main idea behind direct methods is that the intensity of a point should be constant over the images it is observed in. However, changes in illumination and photometric camera parameters such as vignetting, auto-exposure and response function can produce a change of the pixel value even when the real-world intensity is constant. To account for this, DSO can perform a photometric calibration and account for illumination changes in the cost function.

To do this it uses the following error definition. The photometric error of a point $p\in {\mathrm{\Omega }}_i$ in reference frame i observed in another frame j is defined as follows:

$E_{\mathit{p}j}=\sum _{\mathbf{p}\in {\mathcal{N}}_{\mathit{p}}}{\omega }_{\mathit{p}}{\Vert (I_j[{\mathit{p}}^{\mathbf{\prime }}]-b_j)-\frac{t_j{e}^{a_j}}{t_i{e}^{a_i}}(I_i[\mathit{p}]-b_i)\Vert }_{\gamma },$

where ${\mathcal{N}}_{\mathit{p}}$ is a set of eight pixels around the point p, $I_i$ and $I_j$ are images of respective frames, $t_i$, $t_j$ are the exposure times, $a_i$, $b_i$, $a_j$, $b_j$ are the coefficients to correct for affine illumination changes, γ is the Huber norm, ${\omega }_p$ is a gradient-dependent weighting and ${\mathit{p}}^{\mathbf{\prime }}$ is the point projected into $I_j$.

The key idea here is that brightness constancy of a pixel is assumed when observed in different frames. However, the method compensates for illumination changes in the image, the vignette of the camera and the response function and the exposure time.

With that we can formulate the photometric error as

$E_{\text{photo}}=\sum _{i\in \mathcal{F}}\sum _{\mathit{p}\in {\mathcal{P}}_i}\sum _{j\in \text{obs}(\mathit{p})}{E}_{\mathit{p}j},$

where $\mathcal{F}$ is a set of keyframes that we are optimizing, ${\mathcal{P}}_i$ is a sparse set of points in keyframe i, and $\mathit{obs}(\mathbf{p})$ is a set of observations of the same point in other keyframes.

7.4.3 Interaction Between Coarse Tracking and Joint Optimization

For the accuracy of visual odometry it is important to optimize geometry and camera poses jointly. In DSO this can be done efficiently because the Hessian matrix of the system is sparse (see Sect. 7.4.5). However, the joint optimization still takes relatively much time which is why it cannot be done for each frame in real-time. Therefore the joint optimization is only performed for keyframes.

As we want to obtain poses for all frames however, we also perform a coarse tracking. Contrary to the joint estimation, we fix all point depths in the coarse tracking and only optimize the pose of the newest frame, as well as its affine illumination parameters ($a_i$ and $b_i$). This is done using normal direct image alignment in a pyramid scheme where we track against the latest keyframe (see Sect. 7.4.4).

In summary the system has the structure depicted in Fig. 7.3. For each frame we perform the coarse tracking yielding a relative pose of the newest frame with respect to the last keyframe. For each frame we also track candidate points (during the “trace points” procedure). This serves as an initialization when some of the candidate points are later activated during the joint optimization.

For some frames we create a new keyframe, depending on the distance and visual change since the last keyframe. Then the joint estimation is performed, optimizing the poses of all active keyframes as well as the geometry. In the real-time version of the system coarse tracking and joint optimization are performed in parallel in two separate threads. Active keyframes are all keyframes which have not been marginalized yet.

7.4.4 Coarse Tracking Using Direct Image Alignment

The coarse tracking is based on normal direct image alignment.

In order to speed up computation all active points are projected into the frame of the latest keyframe and tracking is performed only with reference to this keyframe.

As in the rest of the system the coarse tracking minimizes the photometric error in order to get a pose estimate. The optimized parameters include the 6 DOF pose which is represented as the transformation from the new frame to the reference frame.

This error is minimized using the Levenberg–Marquandt algorithm. In order to increase the convergence radius the well known pyramid scheme is used. This means that the image is scaled down multiple times (using Gaussian blurring). When tracking, at first the lowest resolution is used. After convergence the optimization is proceeded with the next level of the image pyramid (using the result from the previous level as an initialization). This is done until the original image is reached and the parameters are optimized using it.

This procedure has two advantages. First, tracking on a low resolution first is very fast, as less pixels have to be handled. The main advantage, however, is that it increases the convergence radius. The Taylor approximation of the energy, which in practice uses the image gradient, is only valid in the vicinity of the optimum. When using a smaller-scaled image the gradient contains information from a larger region (in terms of the original image), and therefore increases the convergence basin.

7.4.5 Joint Optimization

In contrast to the coarse tracking the joint optimization not only optimizes poses, but also the geometry, making the task significantly more difficult.

However, here all variables are initialized close to the optimum: The approximate pose of the newest frame is obtained from the previously executed coarse tracking. Newly activated points have a depth estimated by the tracing-procedure and all other variables have already been optimized in the previous execution of the joint optimization. Therefore DSO uses the Gauss–Newton algorithm for this task (instead of Levenberg–Marquandt), and no image pyramid.

When looking at the update step

$\delta \mathit{x}={\mathbf{H}}^{-1}\cdot \mathit{b}$

performed in each iteration it is clear that the most calculation-intensive part is inverting the Hessian matrix.

However, we can use the sparsity of the structure (see Fig. 7.4). The reason for this specific structure is that each residual only depends on the pose of two frames, the affine lighting parameters and one point depth (see also Eq. (7.12)). Therefore there is no residual depending on two different points, meaning that the off-diagonal part of ${\mathbf{J}}^T\mathbf{J}$ is 0 for all depths.

This information can be used to efficiently solve the problem. We can partition the linear system into

$\left[\begin{array}{cc}\hfill {\mathbf{H}}_{\alpha \alpha }\hfill & \hfill {\mathbf{H}}_{\alpha \beta }\hfill \\ \hfill {\mathbf{H}}_{\beta \alpha }\hfill & \hfill {\mathbf{H}}_{\beta \beta }\hfill \end{array}\right]\left[\begin{array}{c}\hfill \delta {\mathit{x}}_{\alpha }\hfill \\ \hfill \delta {\mathit{x}}_{\beta }\hfill \end{array}\right]=\left[\begin{array}{c}\hfill {\mathit{b}}_{\alpha }\hfill \\ \hfill {\mathit{b}}_{\beta }\hfill \end{array}\right]$

where ${\mathit{x}}_{\beta }$ are the depth variables and ${\mathit{x}}_{\alpha }$ are all other variables. Note that this means that ${\mathbf{H}}_{\beta \beta }$ is a diagonal matrix.

We can now apply the Schur complements, which yields the new linear system $\widehat{{\mathbf{H}}_{\alpha \alpha }}{\mathit{x}}_{\alpha }=\widehat{{\mathit{b}}_{\alpha }}$, with

$\widehat{{\mathbf{H}}_{\alpha \alpha }}={\mathbf{H}}_{\alpha \alpha }-{\mathbf{H}}_{\alpha \beta }{\mathbf{H}}_{\beta \beta }^{-1}{\mathbf{H}}_{\beta \alpha },$

$\widehat{{\mathit{b}}_{\alpha }}={\mathit{b}}_{\alpha }-{\mathbf{H}}_{\alpha \beta }{\mathbf{H}}_{\beta \beta }^{-1}{\mathit{b}}_{\beta }.$

In this procedure the only inversion needed is ${\mathbf{H}}_{\beta \beta }^{-1}$, which is trivial because ${\mathbf{H}}_{\beta \beta }$ is diagonal.

The resulting $\widehat{{\mathbf{H}}_{\alpha \alpha }}$ is much smaller than the original H because there are typically much more points, than other variables. With it we can efficiently compute the update for the non-depth variables with

$\delta {\mathit{x}}_{\alpha }={\widehat{{\mathbf{H}}_{\alpha \alpha }}}^{-1}\cdot \widehat{{\mathit{b}}_{\alpha }}.$

The update step of for the depth variables is done by reinserting

${\mathbf{H}}_{\beta \alpha }\delta {\mathit{x}}_{\alpha }+{\mathbf{H}}_{\beta \beta }\delta {\mathit{x}}_{\beta }={\mathit{b}}_{\beta }\iff \delta {\mathit{x}}_{\beta }={\mathbf{H}}_{\beta \beta }^{-1}({\mathit{b}}_{\beta }-{\mathbf{H}}_{\beta \alpha }\delta {\mathit{x}}_{\alpha }).$

7.5.1 Inertial Error

In this section we derive the nonlinear dynamic model that we use to construct the error term, which depends on rotational velocities measured by gyroscope and linear acceleration measured by accelerometer. We decompose the 6D pose from the state s into rotation and translational part:

${\mathbf{T}}_{w\mathrm{\_}imu}=\left(\begin{array}{cc}\hfill \mathbf{R}\hfill & \hfill \mathbf{p}\hfill \\ \hfill 0\hfill & \hfill 1\hfill \end{array}\right),$

with that we can write the following dynamic model:

$\dot{\mathit{p}}=\mathit{v},$

$\dot{\mathit{v}}=\mathbf{R}({\mathit{a}}_z+{\mathit{ϵ}}_a-{\mathit{b}}_a)+\mathit{g},$

$\dot{\mathbf{R}}=\mathbf{R}{[{\mathit{\omega }}_z+{\mathit{ϵ}}_{\omega }-{\mathit{b}}_{\omega }]}_{\times },$

${\dot{\mathit{b}}}_a={\mathit{ϵ}}_{b,a},$

${\dot{\mathit{b}}}_{\omega }={\mathit{ϵ}}_{b,\omega },$

where ${\mathit{ϵ}}_a$, ${\mathit{ϵ}}_{\omega }$, ${\mathit{ϵ}}_{b,a}$, and ${\mathit{ϵ}}_{b,\omega }$ denote the Gaussian white noise that affects the measurements and ${\mathit{b}}_a$ and ${\mathit{b}}_{\omega }$ denote slowly evolving biases. ${[\cdot ]}_{\times }$ is the skew-symmetric matrix such that, for vectors a, b, ${\left[\mathit{a}\right]}_{\times }\mathit{b}=\mathit{a}\times \mathit{b}$.

As IMU data is obtained with a much higher frequency than images, we follow the preintegration approach proposed in [30] and improved in [31] and [23]. We integrate the IMU measurements between timestamps i and j in the IMU coordinate frame and obtain pseudo-measurements $\mathrm{\Delta }{\mathit{p}}_{i\to j}$, $\mathrm{\Delta }{\mathit{v}}_{i\to j}$, and ${\mathbf{R}}_{i\to j}$.

We initialize pseudo-measurements with $\mathrm{\Delta }{\mathit{p}}_{i\to i}=0$, $\mathrm{\Delta }{\mathit{v}}_{i\to i}=0$, ${\mathbf{R}}_{i\to i}=\mathbf{I}$, and assuming the time between IMU measurements is Δt we integrate the raw measurements:

$\mathrm{\Delta }{\mathit{p}}_{i\to k+1}=\mathrm{\Delta }{\mathit{p}}_{i\to k}+\mathrm{\Delta }{\mathit{v}}_{i\to k}\mathrm{\Delta }t,$

$\mathrm{\Delta }{\mathit{v}}_{i\to k+1}=\mathrm{\Delta }{\mathit{v}}_{i\to k}+{\mathbf{R}}_{i\to k}({\mathit{a}}_z-{\mathit{b}}_a)\mathrm{\Delta }t,$

${\mathbf{R}}_{i\to k+1}={\mathbf{R}}_{i\to k}\exp ({[{\mathit{\omega }}_z-{\mathit{b}}_{\omega }]}_{\times }\mathrm{\Delta }t).$

Given the initial state and integrated measurements the state at the next time-step can be predicted:

${\mathit{p}}_j={\mathit{p}}_i+(t_j-t_i){\mathit{v}}_i+\frac{1}{2}{(t_j-t_i)}^2\mathit{g}+{\mathbf{R}}_i\mathrm{\Delta }{\mathit{p}}_{i\to j},$

${\mathit{v}}_j={\mathit{v}}_i+(t_j-t_i)\mathit{g}+{\mathbf{R}}_i\mathrm{\Delta }{\mathit{v}}_{i\to j},$

${\mathbf{R}}_j={\mathbf{R}}_i{\mathbf{R}}_{i\to j}.$

For the previous state ${\mathit{s}}_{i-1}$ (based on the state definition in Eq. (7.39)) and IMU measurements ${\mathit{a}}_{i-1}$, ${\mathit{\omega }}_{i-1}$ between frames i and $i-1$, the method yields a prediction

${\hat{\mathit{s}}}_i:=h({\mathit{\xi }}_{i-1},{\mathit{v}}_{i-1},{\mathit{b}}_{i-1},{\mathit{a}}_{i-1},{\mathit{\omega }}_{i-1})$

of the pose, velocity, and biases in frame i with associated covariance estimate ${\hat{\mathrm{\Sigma }}}_{s,i}$. Hence, the IMU error function terms are

$E_{\text{inertial}}({\mathit{s}}_i,{\mathit{s}}_j):={({\mathit{s}}_j\boxminus {\hat{\mathit{s}}}_j)}^T{\stackrel{\mathbf{ˆ}}{\mathbf{\Sigma }}}_{s,j}^{-1}({\mathit{s}}_j\boxminus {\hat{\mathit{s}}}_j).$

7.5.2 IMU Initialization and the Problem of Observability

In contrast to a purely monocular system the usage of inertial data enables us to observe more parameters such as scale and gravity direction. While this on the one hand is an advantage, on the other hand it also means that these values have to be correctly estimated as otherwise the IMU-integration will not work correctly. This is why other methods such as [25] need to estimate the scale and gravity direction in a separate initialization procedure before being able to use inertial measurements for tracking.

Especially the metric scale, however, imposes a problem as it is only observable after the camera has performed certain motions. An example of a degenerate case is when the system is initialized while the camera moves with a fixed speed that does not change for the first minute. Then the IMU will measure no acceleration (except gravity and the bias) for the first minute rendering it useless for scale estimation. This shows that in theory it can take an arbitrary amount of time until all parameters are observable. Reference [27, Tables I and II] contains an excellent summary, showing the different cases where a unique solution for the problem cannot be found. Although these completely degenerate scenarios usually do not occur, also in practice it can often take multiple seconds for the scale to be observable. In [25] the authors say that they wait for 15 seconds of camera motion in order to be sure that everything is observable when processing the popular EuRoC dataset. Obviously it is not optimal to wait for such a long time before using any inertial measurements for tracking especially as on other datasets it might take even longer. However, as explained previously, using a wrong scale will render the inertial data useless. Therefore in [6] a novel strategy for handling this issue has been proposed. We explicitly include scale (and gravity direction) as a parameter in our visual–inertial system and jointly optimize them together with the other values such as poses and geometry. This means that we can initialize with an arbitrary scale instead of waiting until it is observable. We initialize the various parameters as follows:

All these parameters are then jointly optimized during a bundle adjustment like optimization.

7.5.3 $\mathbf{SIM}(3)$-based Model

In order to be able to start tracking and mapping with a preliminary scale and gravity direction we need to include them into our model. Therefore in addition to the metric coordinate frame we define the DSO coordinate frame to be a scaled and rotated version of it. The transformation from the DSO frame to the metric frame is defined as ${\mathbf{T}}_{m\mathrm{\_}d}\in \{\mathbf{T}\in \mathbf{SIM}(3)|\text{translation}(\mathbf{T})=0\}$, together with the corresponding ${\mathit{\xi }}_{m\mathrm{\_}d}=\log ({\mathbf{T}}_{m\mathrm{\_}d})\in \mathfrak{sim}(3)$. We add a superscript D or M to all poses denoting in which coordinate frame they are expressed. In the optimization the photometric error is always evaluated in the DSO frame, making it independent of the scale and gravity direction. The inertial error on the other hand has to be evaluated in the metric frame.

7.5.4 Scale-Aware Visual–Inertial Optimization

We optimize the poses, IMU-biases and velocities of a fixed number of keyframes. Fig. 7.5A shows a factor graph of the problem. Note that there are in fact many separate visual factors connecting two keyframes each, which we have combined to one big factor connecting all the keyframes in this visualization. Each IMU-factor connects two subsequent keyframes using the preintegration scheme described in Sect. 7.5.1. As the error of the preintegration increases with the time between the keyframes we ensure that the time between two consecutive keyframes is not bigger than 0.5 seconds, which is similar to what [25] have done. Note that in contrast to their method, however, we allow the marginalization procedure described in Sect. 7.5.4.2 to violate this constraint which ensures that long-term relationships between keyframes can be properly observed.

An important property of our algorithm is that the optimized poses are not represented in the metric frame but in the DSO frame. This means that they do not depend on the scale of the environment. If we were optimizing them in the metric frame instead a change of the scale would mean that the translational part of all poses also changes which does not happen during nonlinear optimization.

7.5.4.3 Dynamic marginalization for delayed scale convergence

The marginalization procedure described in Sect. 7.5.4.2 has two purposes: reduce the computation complexity of the optimization by removing old states and maintain the information about the previous states of the system. This procedure fixes the linearization points of the states connected to the old states, so they should already have a good estimate. In our scenario this is the case for all variables except of scale.

The main idea of “dynamic marginalization” is to maintain several marginalization priors at the same time and reset the one we currently use when the scale estimate moves too far from the linearization point in the marginalization prior. To get an idea of the method we strongly recommend watching the animation at https://youtu.be/GoqnXDS7jbA?t=2m6s.

In our implementation we use three marginalization priors: $M_{\text{visual}}$, $M_{\text{curr}}$ and $M_{\text{half}}$. $M_{\text{visual}}$ contains only scale independent information from previous states of the vision and cannot be used to infer the global scale. $M_{\text{curr}}$ contains all information since the time we set the linearization point for the scale and $M_{\text{half}}$ contains only the recent states that have a scale close to the current estimate.

When the scale estimate deviates too much from the linearization point of $M_{\text{curr}}$, the value of $M_{\text{curr}}$ is set to $M_{\text{half}}$ and $M_{\text{half}}$ is set to $M_{\text{visual}}$ with corresponding changes in the linearization points. This ensures that the optimization always has some information about the previous states with consistent scale estimates. In the remaining part of the section we provide the details of our implementation.

We define $G_{\text{metric}}$ to contain only the visual–inertial factors (which depend on ${\mathit{\xi }}_{m\mathrm{\_}d}$) and $G_{\text{visual}}$ to contain all other factors, except the marginalization priors. Then

$G_{\text{full}}=G_{\text{metric}}\cup G_{\text{visual}}.$

(7.48)

Fig. 7.6 depicts the partitioning of the factor graph.

We define three different marginalization factors $M_{\text{curr}}$, $M_{\text{visual}}$ and $M_{\text{half}}$. For the optimization we always compute updates using the graph

$G_{ba}=G_{\text{metric}}\cup G_{\text{visual}}\cup M_{\text{curr}}.$

(7.49)

When keyframe i is marginalized we update $M_{\text{visual}}$ with the factor arising from marginalizing frame i in $G_{\text{visual}}\cup M_{\text{visual}}$. This means that $M_{\text{visual}}$ contains all marginalized visual factors and no marginalized inertial factors making it independent of the scale.

For each marginalized keyframe i we define

$s_i:=\text{scale estimate at the time, }i\text{ was marginalized.}$

(7.50)

We define $i\in M$ if and only if M contains an inertial factor that was marginalized at time i. Using this we enforce the following constraints for inertial factors contained in $M_{\text{curr}}$ and $M_{\text{half}}$:

$\forall i\in M_{\text{curr}}:s_i\in [s_{\text{middle}}/d_i,s_{\text{middle}}\cdot d_i],$

(7.51)

$\forall i\in M_{\text{half}}:s_i\in \{\begin{array}{cc}[s_{\text{middle}},s_{\text{middle}}\cdot d_i],\hfill & \text{if }{s}_{\text{curr}}>s_{\text{middle}},\hfill \\ [s_{\text{middle}}/d_i,s_{\text{middle}}],\hfill & \text{otherwise,}\hfill \end{array}$

(7.52)

where $s_{\text{middle}}$ is the current middle of the allowed scale interval (initialized with $s_0$), $d_i$ is the size of the scale interval at time i, and $s_{\text{curr}}$ is the current scale estimate.

We update $M_{\text{curr}}$ by marginalizing frame i in $G_{\text{ba}}$ and we update $M_{\text{half}}$ by marginalizing i in $G_{\text{metric}}\cup G_{\text{visual}}\cup M_{\text{half}}$.

In order to preserve the constraints in Eqs. (7.51) and (7.52) we apply Algorithm 1 every time a marginalization happens. By following these steps on the one hand we make sure that the constraints are satisfied which ensures that the scale difference in the currently used marginalization factor stays smaller than $d_i^2$. On the other hand the factor always contains some inertial factors so that the scale estimation works at all times. Note also that $M_{\text{curr}}$ and $M_{\text{half}}$ have separate first estimate Jacobians that are employed when the respective marginalization factor is used. Fig. 7.7 shows how the system works in practice.

An important part of this strategy is the choice of $d_i$. It should be small, in order to keep the system consistent, but not too small so that $M_{\text{curr}}$ always contains enough inertial factors. Therefore we chose to dynamically adjust the parameter as follows. At all time steps i we calculate

$d_i=\min \{d_{\text{min}}^j|j\in \mathbb{N}\setminus \{0\},\frac{s_i}{s_{i-1}}<d_i\}.$

(7.53)

This ensures that it cannot happen that the $M_{\text{half}}$ gets reset to $M_{\text{visual}}$ at the same time that $M_{\text{curr}}$ is exchanged with $M_{\text{half}}$. Therefore it prevents situations where $M_{\text{curr}}$ contains no inertial factors at all, making the scale estimation more reliable. On the one hand $d_{\text{min}}$ should not be too small as otherwise not enough inertial information is accumulated. On the other hand it should not be too big, because else the marginalization can get inconsistent. In our experiments we chose $d_{\text{min}}=\sqrt{1.1}$ which is a good tradeoff between the two.

7.5.5 Coarse Visual–Inertial Tracking

The coarse tracking is responsible for computing a fast pose estimate for each frame that also serves as an initialization for the joint estimation detailed in Sect. 7.5.4. We perform conventional direct image alignment between the current frame and the latest keyframe, while keeping the geometry and the scale fixed. The general factor graph for the coarse tracking is shown in Fig. 7.8A. Inertial residuals using the previously described IMU preintegration scheme are placed between subsequent frames. Note that in contrast to the factor graph for the joint optimization, the visual factor for the coarse tracking does not connect all keyframes and depths, but instead just connects each frame with the current keyframe.

Every time the joint estimation is finished for a new frame, the coarse tracking is reinitialized with the new estimates for scale, gravity direction, bias, and velocity as well as the new keyframe as a reference for the visual factors. Similar to the joint estimation we perform partial marginalization to keep the update time constrained. After estimating the variables for a new frame we marginalize out all variables except the keyframe pose and the variables of the newest frame. Also we immediately marginalize the photometric parameters $a_i$ and $b_i$ as soon as the pose estimation for frame i is finished. Fig. 7.8B–D shows how the coarse factor graph changes over time. In contrast to the joint optimization we do not need to use dynamic marginalization because the scale is not included in the optimization.

7.6.1 Proof of the Chain Rule

In this section we will prove the chain rule (7.57).

We define the multivariate derivatives implicitly:

$f(\mathrm{\Omega }(\mathit{\xi })⊞\mathit{\delta })-f(\mathrm{\Omega }(\mathit{\xi }))=\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot \mathit{\delta }+\mu (\mathit{\delta })\cdot \mathit{\delta },$

where the error function $\mu (\mathit{\delta })$ goes to 0 as δ goes to zero. We can rewrite Eq. (7.56) by multiplying $\mathrm{\Omega }(\mathit{\xi })$ from the left,

$\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})=\mathrm{\Omega }(\mathit{\xi })⊞(\frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}\cdot \mathit{ϵ}+\eta (\mathit{ϵ})\cdot \mathit{ϵ}).$

Using this we can compute

$$\begin{gathered} f(\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ}))-f(\mathrm{\Omega }(\mathit{\xi }))\stackrel{\text{(7.64)}}{=}f(\mathrm{\Omega }(\mathit{\xi })⊞\underset{=:{\mathit{\delta }}_{\mathit{ϵ}}}{\underbrace{(\frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}\cdot \mathit{ϵ}+\eta (\mathit{ϵ})\cdot \mathit{ϵ})}})-f(\mathrm{\Omega }(\mathit{\xi }))=f(\mathrm{\Omega }(\mathit{\xi })⊞{\mathit{\delta }}_{\mathit{ϵ}})-f(\mathrm{\Omega }(\mathit{\xi }))\stackrel{\text{(7.63)}}{=}\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot {\mathit{\delta }}_{\mathit{ϵ}}+\mu ({\mathit{\delta }}_{\mathit{ϵ}})\cdot {\mathit{\delta }}_{\mathit{ϵ}} \\ =\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot (\frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}\cdot \mathit{ϵ}+\eta (\mathit{ϵ})\cdot \mathit{ϵ})+\mu ({\mathit{\delta }}_{\mathit{ϵ}})\cdot (\frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}\cdot \mathit{ϵ}+\eta (\mathit{ϵ})\cdot \mathit{ϵ})=\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot \frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}\cdot \mathit{ϵ}+\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot \eta (\mathit{ϵ})\cdot \mathit{ϵ} \\ +\mu ({\mathit{\delta }}_{\mathit{ϵ}})\cdot \frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}\cdot \mathit{ϵ}+\mu ({\mathit{\delta }}_{\mathit{ϵ}})\cdot \eta (\mathit{ϵ})\cdot \mathit{ϵ} \\ =\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot \frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}\cdot \mathit{ϵ} \\ +\underset{=:\gamma (\mathit{ϵ})}{\underbrace{(\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot \eta (\mathit{ϵ})+\mu ({\mathit{\delta }}_{\mathit{ϵ}})\cdot \frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{d\mathit{ϵ}}+\mu ({\mathit{\delta }}_{\mathit{ϵ}})\cdot \eta (\mathit{ϵ}))}}\cdot \mathit{ϵ}. \end{gathered}$$

When ϵ goes to 0, then ${\mathit{\delta }}_{\mathit{ϵ}}$ also goes to 0 (as can be seen from its definition). Using this it follows by the definition of the derivative that $\eta (\mathit{ϵ})$ and $\mu ({\mathit{\delta }}_{\mathit{ϵ}})$ go to 0 as well. This shows that $\gamma (\mathit{ϵ})$ goes to 0 when ϵ goes to 0. Therefore the last line of Eq. (7.65) is in line with our definition of the derivative and

$\frac{df\left(\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})\right)}{d\mathit{ϵ}}=\frac{df(\mathrm{\Omega }\left(\mathit{\xi }\right)⊞\mathit{\delta })}{\mathit{\delta }}\cdot \frac{d_l\mathrm{\Omega }(\mathit{\xi }⊞\mathit{ϵ})}{\mathit{ϵ}}.$

 □

7.6.2 Derivation of the Jacobian with Respect to Pose in Eq. (7.58)

In this section we will show how to derive the Jacobians $\frac{\partial \mathrm{\Psi }({\mathit{\xi }}_{cam\mathrm{\_}w}^D⊞\mathit{ϵ},{\mathit{\xi }}_{m\mathrm{\_}d})}{\partial \mathit{ϵ}}$ using the implicit definition of the derivative shown in Eq. (7.56).

In order to do this we need the definition of the adjoint.

$\mathbf{T}\cdot \exp (\mathit{ϵ})=\exp ({\text{Adj}}_{\mathbf{T}}\cdot \mathit{ϵ})\cdot \mathbf{T}$

with $\mathbf{T}\in \mathbf{SIM}(3)$. It follows that

$\log (\mathbf{T}\cdot \exp (\mathit{ϵ})\cdot {\mathbf{T}}^{-1})={\text{Adj}}_{\mathbf{T}}\cdot \mathit{ϵ}.$

Using this we can compute

$$\begin{gathered} \mathrm{\Psi }{({\mathit{\xi }}_{cam\mathrm{\_}w}^D,{\mathit{\xi }}_{m\mathrm{\_}d})}^{-1}⊞\mathrm{\Psi }({\mathit{\xi }}_{cam\mathrm{\_}w}^D⊞\mathit{ϵ},{\mathit{\xi }}_{m\mathrm{\_}d})\stackrel{\text{(7.55)}}{=}{({\mathit{\xi }}_{m\mathrm{\_}d}⊞{\left({\mathit{\xi }}_{cam\mathrm{\_}w}^D\right)}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu}^M)}^{-1}⊞({\mathit{\xi }}_{m\mathrm{\_}d}⊞{({\mathit{\xi }}_{cam\mathrm{\_}w}^D⊞\mathit{ϵ})}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu}^M)={\left({\mathit{\xi }}_{cam\mathrm{\_}imu}^M\right)}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}^D⊞\underset{=0}{\underbrace{{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}}}⊞{\mathit{ϵ}}^{-1}⊞{\left({\mathit{\xi }}_{cam\mathrm{\_}w}^D\right)}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu}^M \\ =\log ({\left({\mathbf{T}}_{cam\mathrm{\_}imu}^M\right)}^{-1}\cdot {\mathbf{T}}_{m\mathrm{\_}d}\cdot {\mathbf{T}}_{cam\mathrm{\_}w}^D\cdot \exp {\left(\mathit{ϵ}\right)}^{-1}\cdot {\left({\mathbf{T}}_{cam\mathrm{\_}w}^D\right)}^{-1}\cdot {\mathbf{T}}_{m\mathrm{\_}d}^{-1}\cdot {\mathbf{T}}_{cam\mathrm{\_}imu}^M)\stackrel{\text{(7.68)}}{=}\text{Adj}({\left({\mathbf{T}}_{cam\mathrm{\_}imu}^M\right)}^{-1}\cdot {\mathbf{T}}_{m\mathrm{\_}d}\cdot {\mathbf{T}}_{cam\mathrm{\_}w}^D)\cdot (-\mathit{ϵ}). \end{gathered}$$

After moving the minus sign to the left this is in line with our definition of the derivative in Eq. (7.56) and it follows that

$\frac{\partial \mathrm{\Psi }({\mathit{\xi }}_{cam\mathrm{\_}w}^D⊞\mathit{ϵ},{\mathit{\xi }}_{m\mathrm{\_}d})}{\partial \mathit{ϵ}}=-\text{Adj}({\left({\mathbf{T}}_{cam\mathrm{\_}imu}^M\right)}^{-1}\cdot {\mathbf{T}}_{m\mathrm{\_}d}\cdot {\mathbf{T}}_{cam\mathrm{\_}w}^D).$

7.6.3 Derivation of the Jacobian with Respect to Scale and Gravity Direction in Eq. (7.59)

In order to derive the Jacobian $\frac{\partial \mathrm{\Psi }({\mathit{\xi }}_{cam\mathrm{\_}w}^D,{\mathit{\xi }}_{m\mathrm{\_}d}⊞\mathit{ϵ})}{\partial \mathit{ϵ}}$ we need the Baker–Campbell–Hausdorff formula: Let $\mathit{a},\mathit{b}\in \mathfrak{sim}(3)$, then

$\log (\exp (\mathit{a})\cdot \exp (\mathit{b}))=\mathit{a}+\mathit{b}+\frac{1}{2}[\mathit{a},\mathit{b}]+\frac{1}{12}([\mathit{a},[\mathit{a},\mathit{b}]]+[\mathit{b},[\mathit{b},\mathit{a}]])+\frac{1}{48}([\mathit{b},[\mathit{a},[\mathit{b},\mathit{a}]]]+[\mathit{a},[\mathit{b},[\mathit{b},\mathit{a}]]])+....$

Here $[\mathit{a},\mathit{b}]:=\mathit{a}\mathit{b}-\mathit{b}\mathit{a}$ denotes the Lie bracket.

In this section we will omit the superscripts to simplify the notation. We have

$$\begin{gathered} \mathrm{\Psi }{({\mathit{\xi }}_{cam\mathrm{\_}w},{\mathit{\xi }}_{m\mathrm{\_}d})}^{-1}⊞\mathrm{\Psi }({\mathit{\xi }}_{cam\mathrm{\_}w},{\mathit{\xi }}_{m\mathrm{\_}d}⊞\mathit{ϵ})\stackrel{\text{(7.55)}}{=}{({\mathit{\xi }}_{m\mathrm{\_}d}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu})}^{-1}⊞({\mathit{\xi }}_{m\mathrm{\_}d}⊞\mathit{ϵ}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}^{-1}⊞{({\mathit{\xi }}_{m\mathrm{\_}d}⊞\mathit{ϵ})}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu})={\mathit{\xi }}_{cam\mathrm{\_}imu}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}⊞\mathit{ϵ}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}^{-1}⊞{\mathit{ϵ}}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu} \\ ={\mathit{\xi }}_{cam\mathrm{\_}imu}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}⊞\mathit{ϵ}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}^{-1}⊞\underset{=0}{\underbrace{{({\mathit{\xi }}_{cam\mathrm{\_}imu}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d})}^{-1}⊞({\mathit{\xi }}_{cam\mathrm{\_}imu}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d})}}⊞{\mathit{ϵ}}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu} \\ =\underset{:=\mathit{a}}{\underbrace{{\mathit{\xi }}_{cam\mathrm{\_}imu}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}⊞\mathit{ϵ}⊞{\mathit{\xi }}_{cam\mathrm{\_}w}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu}}}⊞\underset{:=\mathit{b}}{\underbrace{{\mathit{\xi }}_{cam\mathrm{\_}imu}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}⊞{\mathit{ϵ}}^{-1}⊞{\mathit{\xi }}_{m\mathrm{\_}d}^{-1}⊞{\mathit{\xi }}_{cam\mathrm{\_}imu}}}. \end{gathered}$$