[Review] VI-DSO (Eng)

Introduction

IMU measurement is accumulated and updated when new keyframe is added.

Dynamic marginalization - partial marginalization when scale is not accurate. Instead, some initial guess is used until scale become converged.

Remark

• a direct sparse visual-inertial odometry system.
• a novel initialization strategy where scale and gravity direction are included into the model and jointly opti- mized after initialization.
• we introduce ”dynamic marginalization” as a technique to adaptively employ marginalization strategies even in cases where certain variables undergo drastic changes.

Direct Sparse Visual-Inertial Odometry (VI-DSO)

To make the problem computationally feasible the optimization is performed on a window of recent frames while all older frames get marginalized out.

The system runs two parallel loops:

• The coarse tracking is executed for every frame and uses direct image alignment combined with an inertial error term to estimate the pose of the most recent frame.
• When a new keyframe is created we perform a visual- inertial bundle adjustment like optimization that esti- mates the geometry and poses of all active keyframes.

Minimize:
$$
E_{total}=\lambda\cdot E_{photo}+E_{inertial}
$$

Inertial error

IMU preintegration allows adding a single IMU factor describing the pose between two camera frames. Inertial error is in form of normalized-squared sum.
$$
E_{inertial}(s_i,s_j):=(s_j\boxminus\hat{s}j)^T\hat{\Sigma}^{-1}{s,j}(s_j\boxminus\hat{s}_j)
$$
where the operator $\boxminus$ applies $\xi_j\boxplus(\hat{\xi_j})^{-1}$ for poses, and a normal subtraction for other components.

IMU Initialization and the problem of observability

Integration of IMU provides metric scale instead of relative camera scale, as well as gravity direction.

: Source: Martinelli, A. (2014). Closed-form solution of visual-inertial structure from motion. International Journal of Computer Vision, 106(2), 138–152. https://doi.org/10.1007/s11263-013-0647-7

For self-driving car, it is possible to have situations of

• Rotation around a single axis Varying Acceleration
• No rotation

In certain situation, immediate initialization could not be possible. The state-of-the-art SLAM cosumes first 15s for initialization.

In this paper, scale is initialized with arbitrary value. Initialization strategy is as following:

• Compute rough pose estimate between two frames and approximate depth for several points.
• Initial gravity direction is computed by averaging up to 40 acc. measurements.
• Initialize the velocity and IMU biases with zero and the scale with 1.0

All these parameters will be optimized.

SIM(3)-based Representation of the World

To incorporate scale and gravity direction into state vector, define DSO coordinate:
$$
T_{m_d} \in {T \in \text{SIM}(3)\ | \ \text{translation}(T) = 0}
$$
with corresponding
$$
\xi_{m_d}=log(T_{m_d})\in \mathfrak{sim}(3)
$$
Hence DSO coordinate is a scaled and rotated frame of the metric frame. Note that photometric error is evalutated in DSO frame, to be independent of scale and gravity, while inertial error is in metric frame.

Scale-aware Visual-Inertial Optimization

As IMU error between two subsequent keyframes increases, the time between the two is limited up to 0.5s. Optimized poses are represented in the DSO frame, so they do not depend on the scale.

Nonlinear optimization

Gauss-Newton algorithm is used. State vector is

$$ s_i:=[(\xi^D_{cam_{i\_w}})^T, v_i^T, b_i^T, a_i,b_i,d_i^1,d_i^2,\cdots,d_i^m]^T $$

where $v_i\in\mathbb{R}^3$ is the velocity, $b_i\in\mathbb{R}^6$ is the current IMU bias, $a_i, b_i$ are the affine illumination parameters, $d_i^j$ are the inverse depths of the points hosted in this keyframe. Therefore, the full state vector is defined as
$$
s=[C^T,\xi^T_{m_d},s^T_1,s^T_2,\cdots,s^T_n]^T
$$

where $c$ contains the geometric camera parameters and $\xi_{m_d}$ denotes the translation-free transformation between the DSO and the metric frame.

Now, using the stacked residual vector $r$ we have

$$ J=\dfrac{dr(s\boxplus\epsilon)}{d\epsilon}\bigg|_{\epsilon=0} \\\\\\ H=J^TWJ \\\\\\ b = -J^TWr $$

as we got Gauss-Newton system $\delta=H^{-1}b$.

Because there is no overapping residuals between $E_{photo}$ and $E_{inertial}$, we can divide $H, b$ as

$$ H=H_{photo}+H_{imu}\\\\\\ b=b_{photo}+b_{imu} $$

Recall that IMU is represented in the metric frame. So additional state vector is needed.

$$ s'_i:=[\xi^M_{w\_imu_i}, v_i, b_i]^T\\\\\\ s'=[s'^T_1,\cdots,s'^T_n]^T $$

The inertial residuals lead to

$$ H_{imu}'=J_{imu}'^T W_{imu}J_{imu}' \\\\\\ b'_{imu}=-J_{imu}'^T W_{imu} r_{imu} $$

For joint optimization,

$$ H_{imu}=J_{rel}^T H_{imu}'J_{rel} \\\\\\ b_{imu}=-J_{rel}^T b'_{imu} $$

Marginalization using the Schur-complement

Same as DSO.

Dynamic Marginalization for Delayed Scale Convergence

Marginalization priors

Maintaining priors $M_{visual}, \ M_{curr}, \ M_{half}$ and reset the one when the scale estimation is too far from the linearization point.

• $M_{curr}$ : used for state estimation from the moment that linearization point is set
• $M_{half}$ : contains only recent states
• $M_{visual}$ : scale independent information from previous states of vision

Priors visualization

Define factor graph

$$
G_{full}=G_{metric}\cup G_{visual}
$$
$G_{metric}$ contains only visual-inertial factors (depending on $\xi_{m_d}$), and $G_{visual}$ contains all others.

For optimization, the graph is computed as
$$
G_{ba}=G_{metric}\cup G_{visual}\cup M_{curr}
$$
When keyframe $i$ is marginalized, $M_{visual}$ is updated with $G_{visual}\cup M_{visual}$.

Scale estimates

$s_i:=$ scale estimate at the time, $i$ was marginalized. $i\in M$ iff $M$ contains inertial factor. This leads to this constraint:
$$
\forall i \in M_{curr}:s_i\in [s_{middle}/d_i, s_{middle}\cdot d_i]
$$

$$
\forall i\in M_{half}:s_i \in \left{
\begin{eqnarray}
[s_{middle}, s_{middle}\cdot d_i], & \text{if} \ s_{curr} > s_{middle} \
[s_{middle}/d_i, s_{middle}], & \text{otherwise}
\end{eqnarray}
\right.
$$

In order to preserve those constraints, below algorithm must be applied everytime when marginalization happens.

Coarse Visual-Inertial Tracking

Fast computation of pose estimation for each frame

IMU preintegration is used between keyframe ~ current frame

After Joint optimization is finished, the coarse tracking is re-initialized.

Conclusion

Pros:

• The system can estimate scale and gravity direction.

• Improved precision and accuracy by using IMU, compared to original DSO
• In presence of extreme dynamic, VI-DSO shows robust result, but in little-to-moderate conditions, it does not have significant improvements compared to LSD-SLAM and ORB-SLAM.

Cons:

• Scale is not clearly observable, hence initial guess is needed. This induces dynamic marginalization.
• For certain situations, initial states cannot be initialized properly (previous studies spends 15 ~ 30 seconds for this initialization).