Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization

Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization¹

• 把尺度，重力方向放进模型中进行联合优化
• 由于尺度不是立刻就可观的，因此我们以任一尺度进行初始化，而不是延迟初始化到量都变得可观之后（VIORB）。
• 对旧的量进行部分边缘化，为了保证一致性，提出“动态边缘化”策略。因此在尺度远离最优值时也可以采用边缘化。

DIRECT SPARSE VISUAL-INERTIAL ODOMETRY

DSVIO

初始化和可观性

零速和恒速运动都会导致无法进行初始化，VIORB使用15s初始化来保证所有量都可观。

我们是把scale，gravity都放入优化中使其和pose一起优化。

• 使用DSO的初始化方式，得到粗略位姿估计，和近似的逆深度，保证平均逆深度为1.
• 初始重力方向采用40个加速度测量数据的平均值。
• 初始化速度和IMU-bias的值为0.

基于Sim(3)的世界表示方式

为了能够以尺度，重力初始值来优化。除了度量坐标系，定义一个DSO坐标系，它是度量系的一个被尺度放缩，和旋转的版本。定义为 T m _ d ∈ { T ∈ SIM ⁡ ( 3 ) ∣  translation  ( T ) = 0 } \mathbf{T}_{m\_d} \in\{\mathbf{T} \in \operatorname{SIM}(3) | \text { translation }(\mathbf{T})=0\} Tm_d​∈{T∈SIM(3)∣ translation (T)=0}，${\xi }_{m\_d}\in \log ({\mathbf{T}}_{m\_d})\in \mathfrak{s}im(3)$.

光度误差使用DSO系，独立于尺度和重力方向，惯导误差使用度量系（metric frame）

Scale-aware Visual-inertial Optimization

关键帧之间不能超过0.5s，边缘化中可以违反这个部分，确保长期关联。优化的位姿是在DSO坐标系下的，不依赖于环境尺度。

1) Nonlinear Optimization

每个关键帧优化的变量为：
$${\mathbf{s}}_i:={\left[{\left({\mathbf{\xi }}_{ca{m}_{i-w}}^D\right)}^T,{\mathbf{v}}_i^T,{\mathbf{b}}_i^T,a_i,b_i,d_i^1,d_i^2,\dots ,d_i^m\right]}^T\text{\hspace{1em}(1)}$$
所有的向量为：
$$\mathbf{s}={\left[{\mathbf{c}}^T,{\mathbf{\xi }}_{m_{-}d}^T,{\mathbf{s}}_1^T,{\mathbf{s}}_2^T,\dots ,{\mathbf{s}}_n^T\right]}^T\text{\hspace{1em}(2)}$$
相机和IMU的的residual是没有重叠部分的，因此可以独立开：
$$\mathbf{H}={\mathbf{H}}_{\mathrm{p}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}}+{\mathbf{H}}_{\mathrm{i}\mathrm{m}\mathrm{u}}\text{ and }\mathbf{b}={\mathbf{b}}_{\mathrm{p}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}}+{\mathbf{b}}_{\mathrm{i}\mathrm{m}\mathrm{u}}\text{\hspace{1em}(3)}$$
惯性器件的残差需要使用相对IMU的metric坐标系，因此定义新的状态：
$${\mathbf{s}}_i^{\prime }:={\left[{\mathbf{\xi }}_{w_{-}im{u}_i}^M,{\mathbf{v}}_i,{\mathbf{b}}_i\right]}^T\text{ and }{\mathbf{s}}^{\prime }={\left[{\mathbf{s}}_1^{\prime T},{\mathbf{s}}_2^{\prime T},\dots ,{\mathbf{s}}_n^{\prime T}\right]}^T\text{\hspace{1em}(4)}$$
惯性的残差导出：
$${\mathbf{H}}_{\mathrm{i}\mathrm{m}\mathrm{u}}^{\prime }={\mathbf{J}}_{\mathrm{i}\mathrm{m}\mathrm{u}}^{\prime T}{\mathbf{W}}_{\mathrm{i}\mathrm{m}\mathrm{u}}{\mathbf{J}}_{\mathrm{i}\mathrm{m}\mathrm{u}}^{\prime }\text{ and }{\mathbf{b}}_{\mathrm{i}\mathrm{m}\mathrm{u}}^{\prime }=-{\mathbf{J}}_{\mathrm{i}\mathrm{m}\mathrm{u}}^{\prime T}{\mathbf{W}}_{\mathrm{i}\mathrm{m}\mathrm{u}}{\mathbf{r}}_{\mathrm{i}\mathrm{m}\mathrm{u}}\text{\hspace{1em}(5)}$$
这个之前的位姿表示定义不同，需要一个${\mathbf{J}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$
$${\mathbf{H}}_{\mathrm{i}\mathrm{m}\mathrm{u}}={\mathbf{J}}_{\mathrm{r}\mathrm{e}\mathrm{l}}^T\cdot {\mathbf{H}}_{\mathrm{i}\mathrm{m}\mathrm{u}}^{\prime }\cdot {\mathbf{J}}_{\mathrm{r}\mathrm{e}\mathrm{l}}\text{ and }{b}_{\mathrm{i}\mathrm{m}\mathrm{u}}={\mathbf{J}}_{\mathrm{r}\mathrm{e}\mathrm{l}}^T\cdot {\mathbf{b}}_{\mathrm{i}\mathrm{m}\mathrm{u}}^{\prime }\text{\hspace{1em}(6)}$$
详细推导见补充材料。

2）Marginalization using the Schur-Complement:

这里为了保证零空间，会使用FEJ，把${\mathbf{J}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$固定线性化点，他是和${\xi }_{m\_d}$相关的。

3）Dynamic Marginalization for Delayed Scale Convergence

边缘化时不固定尺度的线性化点。

动态边缘化就是保持几个边缘化先验，当尺度估计离线性化点交远时，重置当前使用的。保留三个边缘化先验，$M_{visual},M_{curr},M_{half}$

$M_{visual}$包含之前视觉状态的与尺度无关的信息，不能用来推断全局尺度。

$M_{curr}$包含设置线性点以来的所有尺度信息。

$M_{half}$包含最近尺度接近当前值的状态信息。

当尺度估计和线性点差很多时，设置$M_{curr}=M_{half}，M_{curr}=M_{visual}$，确保优化有之前的信息。

定义$G_{metric}$包含VI factor，$G_{visual}$包含其它的factor，不包括边缘化先验

$$G_{\text{full }}=G_{\text{metric }}\cup G_{\text{visual }}\text{\hspace{1em}(7)}$$
优化使用的因子图集是：
$$G_{ba}=G_{\text{metric }}\cup G_{\text{visual }}\cup M_{\text{curr }}\text{\hspace{1em}(8)}$$

当一个关键帧被边缘化掉，我们在因子图$G_{\text{visual }}\cup M_{\text{visual }}$上进行边缘化操作来更新$M_{\text{visual }}$，意味着它包含所有的边缘化的视觉因子，没有惯性因子，因此和尺度无关。

对于边缘化的关键帧，定义$i$帧被边缘化时的尺度估计值为$s_i$，包含惯性因子被边缘化时，强制下面的约束：
$$\forall i\in M_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{r}}:s_i\in \left[s_{\mathrm{m}\mathrm{i}\mathrm{d}\mathrm{d}\mathrm{l}\mathrm{e}}/d_i,s_{\text{middle }}\cdot d_i\right]\text{\hspace{1em}(9)}$$

$$\forall i\in M_{\text{half }}:s_i\in \{\begin{array}{ll}\left[s_{\text{middle }},s_{\text{middle }}\cdot d_i\right],& \text{ if }{s}_{\text{curr }}>s_{\text{middle }}\\ \left[s_{\text{middle }}/d_i,s_{\text{middle }}\right],& \text{ otherwise }\end{array}\text{\hspace{1em}(10)}$$

$s_{middle}$是当前允许尺度范围的中间值，$d_i$是时间$i$尺度区间大小，$s_{curr}$是当前的尺度估计

我们在因子图$G_{ba}$上面边缘化更新$M_{curr}$，在因子图
$G_{\text{metric }}\cup G_{\text{visual }}\cup M_{\text{half }}$上边缘化更新 $M_{half}$.

为了保证尺度约束，边缘化后使用算法：

作用：

• 确保当前使用的边缘化因子中尺度的差异小于$d_i^2$的约束条件被满足
• 另一方面，因子中总是包含惯性因子，因此可以持续进行尺度估计
• FEJ被分别使用在$M_{curr}$和$M_{half}$中

这里的$d_i$选择应该足够小来保证一致性，也不能太小需要确保包含惯性因子，因此使用动态调整：
d i = min ⁡ { d min ⁡ j ∣ j ∈ N \ { 0 } , s i s i − 1 < d i } (11) d_{i}=\min \left\{d_{\min }^{j} | j \in \mathbb{N} \backslash\{0\}, \frac{s_{i}}{s_{i-1}}<d_{i}\right\} \tag{11} di​=min{dminj​∣j∈N\{0},si−1​si​​<di​}(11)
理解：$d_i$太小则容易按照算法1进行更新，视觉把curr给占据了。$d_i$太大了则覆盖的范围小，则无法保证和之前的状态有一致性。

公式（11）确保不会发生，$M_{half}$被$M_{visual}$給重置，然后$M_{half}$又赋值给了$M_{curr}$，我们选择$d_{\min }=\sqrt{1.1}$

每个新帧联合优化完成后，跟踪会使用新估计的尺度、重力方向、bias和速度重新初始化，以及设置新的关键帧为参考帧。当估计完新的一帧变量，边缘化掉所有变量除了关键帧pose和最新帧的变量。因为没有涉及尺度，不需要动态边缘化。

Supplementary Material

对参数的评价

• 由于IMU的加入，点可以少一些，而且点少还会提高精度，因为可靠地点数目变多了

公式推导

使用以下公式来判断尺度是否收敛，n是队列里最大数目取60：
$$c=\frac{{\max }_{j:=i-n+1}^n{s}_j}{{\min }_{j:=i-n+1}^n{s}_j}-1\text{\hspace{1em}(12)}$$
当$c<0.005$时认为收敛，固定${\xi }_{m\_d}$。

计算${\mathbf{J}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$

相机和IMU是在两个不同的坐标系下表示的，因此位姿变换有
$$\begin{array}{rl}{\mathbf{T}}_{w\_imu}^M& ={\mathbf{T}}_{m\_d}({\mathbf{T}}_{cam\_w}^D{)}^{-1}({\mathbf{T}}_{m\_d}{)}^{-1}{\mathbf{T}}_{cam\_imu}^M\\ {\xi }_{w_{-}imu}^M& ={\mathbf{\xi }}_{m_{-}d}⊞{\left({\mathbf{\xi }}_{ca{m}_{-}w}^D\right)}^{-1}⊞{\mathbf{\xi }}_{m_{-}d}^{-1}⊞{\xi }_{ca{m}_{-}imu}^M\\ & =:\Psi \left({\mathbf{\xi }}_{ca{m}_{-}w}^D,{\mathbf{\xi }}_{m\_d}\right)\end{array}\text{\hspace{1em}(13)}$$
公式含义是：Metric系下imu转到cam变换矩阵，先转到DSO系下，DSO系下转为imu到world变换矩阵，再把DSO系转为Metric系得到结果

对于函数$\Omega (\xi )：\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)\to \mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$，我们定义导数：
$$\Omega (\mathbf{\xi }{)}^{-1}⊞\Omega (\mathbf{\xi }⊞\mathbf{ϵ})=\frac{d\Omega (\mathbf{\xi }⊞\mathbf{ϵ})}{d\mathbf{ϵ}}\cdot \mathbf{ϵ}+\eta (\mathbf{ϵ})\cdot \mathbf{ϵ}\text{\hspace{1em}(14)}$$
$\eta (ϵ)\to 0$当$\Vert ϵ\Vert \to 0$

对于函数$f(\mathbf{\xi }):\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)\to \mathbb{R}$，导数为
$$\frac{df(\Omega (\mathbf{\xi }⊞\mathbf{ϵ}))}{d\mathbf{ϵ}}=\frac{df(\Omega (\mathbf{\xi })⊞\mathbf{\delta })}{\delta }\cdot \frac{d\Omega (\mathbf{\xi }⊞\mathbf{ϵ})}{ϵ}\text{\hspace{1em}(15)}$$
1） 下面推导Jacobian相对于位姿导数
$$\frac{\partial \Psi \left({\mathbf{\xi }}_{ca{m}_{-}w}^D⊞\mathbf{ϵ},{\mathbf{\xi }}_{m\_d}\right)}{\partial \mathbf{ϵ}}$$
其中伴随性质有：
$$\begin{gathered} \mathbf{T}\cdot \exp (\mathbf{ϵ})=\exp \left({\mathrm{A}\mathrm{d}\mathrm{j}}_{\mathbf{T}}\cdot \mathbf{ϵ}\right)\cdot \mathbf{T} \\ \log \left(\mathbf{T}\cdot \exp (\mathbf{ϵ})\cdot {\mathbf{T}}^{-1}\right)={\mathrm{Adj}}_{\mathbf{T}}\cdot \mathbf{ϵ}\text{\hspace{1em}(16)} \end{gathered}$$
其中$\mathbf{T}\in \mathbf{S}\mathbf{I}\mathbf{M}(3)$，使用公式（13）（16）的结果
$$\begin{array}{rl}& \Psi {\left({\mathbf{\xi }}_{ca{m}_{-w}}^D,{\mathbf{\xi }}_{m_{-d}}\right)}^{-1}⊞\Psi \left({\mathbf{\xi }}_{ca{m}_{-}w}^D⊞\mathbf{ϵ},{\mathbf{\xi }}_{m_{-}d}\right)\\ & ={\left({\mathbf{\xi }}_{m_{-}d}⊞{\left({\mathbf{\xi }}_{ca{m}_{-}w}^D\right)}^{-1}⊞{\mathbf{\xi }}_{m_{-}d}^{-1}⊞{\mathbf{\xi }}_{ca{m}_{-}imu}^M\right)}^{-1}⊞\left({\mathbf{\xi }}_{m_{-}d}⊞{\left({\mathbf{\xi }}_{ca{m}_{-}w}^D⊞\mathbf{ϵ}\right)}^{-1}⊞{\mathbf{\xi }}_{m_{-}d}^{-1}⊞{\mathbf{\xi }}_{ca{m}_{-}imu}^M\right)\\ & ={\left({\mathbf{\xi }}_{ca{m}_{-}imu}^M\right)}^{-1}⊞{\mathbf{\xi }}_{m_{-}d}⊞{\mathbf{\xi }}_{ca{m}_{-}w}^D⊞\underset{=0}{\underbrace{{\mathbf{\xi }}_{m_{-}d}^{-1}⊞{\mathbf{\xi }}_{m_{-}d}}}⊞{\mathbf{ϵ}}^{-1}⊞{\left({\mathbf{\xi }}_{ca{m}_{-}w}^D\right)}^{-1}⊞{\mathbf{\xi }}_{m_{-}d}^{-1}⊞{\mathbf{\xi }}_{ca{m}_{-}imu}^M\\ & =\log \left({\left({\mathbf{T}}_{ca{m}_{-}imu}^M\right)}^{-1}\cdot {\mathbf{T}}_{m_{-}d}\cdot {\mathbf{T}}_{ca{m}_{-}w}^D\cdot \exp (ϵ{)}^{-1}\cdot {\left({\mathbf{T}}_{cam\_w}^D\right)}^{-1}\cdot {\mathbf{T}}_{m_{-}d}^{-1}\cdot {\mathbf{T}}_{ca{m}_{-}imu}^M\right)\\ & =\mathrm{Adj}\left({\left({\mathbf{T}}_{ca{m}_{-}imu}^M\right)}^{-1}\cdot {\mathbf{T}}_{m_{-}d}\cdot {\mathbf{T}}_{ca{m}_{-}w}^D\right)\cdot (-ϵ)\end{array}\text{\hspace{1em}(17)}$$
所以最后得到Jacobian相对于位姿导数公式（17）
$$\frac{\partial \Psi \left({\mathbf{\xi }}_{cam\_w}^D⊞\mathbf{ϵ},{\mathbf{\xi }}_{m\_d}\right)}{\partial \mathbf{ϵ}}=-\mathrm{Adj}\left({\left({\mathbf{T}}_{ca{m}_{-}imu}^M\right)}^{-1}\cdot {\mathbf{T}}_{m_{-}d}\cdot {\mathbf{T}}_{ca{m}_{-}w}^D\right)\text{\hspace{1em}(18)}$$
**2）**推导Jacobian相对于尺度和重力方向的导数
$$\frac{\partial \Psi \left({\mathbf{\xi }}_{ca{m}_{-}w}^D,{\mathbf{\xi }}_{m_{-}d}⊞\mathbf{ϵ}\right)}{\partial \mathbf{ϵ}}$$
根据BCH公式：
$$\log (\exp (\mathbf{a})\cdot \exp (\mathbf{b}))=\mathbf{a}+\mathbf{b}+\frac{1}{2}[\mathbf{a},\mathbf{b}]+\frac{1}{12}([\mathbf{a},[\mathbf{a},\mathbf{b}]]+[\mathbf{b},[\mathbf{b},\mathbf{a}]])+\frac{1}{48}([\mathbf{b},[\mathbf{a},[\mathbf{b},\mathbf{a}]]]+[\mathbf{a},[\mathbf{b},[\mathbf{b},\mathbf{a}]]])+\dots \text{\hspace{1em}(19)}$$
李括号：$[a,b]=ab-ba$

同公式（17）的推导一样凑成Adj形式：

根据公式（16）得到：
$$\mathbf{a}=\mathrm{Adj}\left({\mathbf{T}}_{ca{m}_{-}imu}^{-1}\cdot {\mathbf{T}}_{m\_d}\cdot {\mathbf{T}}_{ca{m}_{-}w}\right)\cdot ϵ\text{\hspace{1em}(19)}$$

$$\mathbf{b}=-\mathrm{Adj}\left({\mathbf{T}}_{ca{m}_{-}imu}^{-1}\cdot {\mathbf{T}}_{m_{-}d}\right)\cdot ϵ\text{\hspace{1em}(20)}$$

把公式（19）（20）带入BCH公式，即公式（19）
$$\mathbf{a}\cdot \mathbf{b}=\underset{{\mu }_1(ϵ)}{\underbrace{\mathrm{Adj}\left({\mathbf{T}}_{cam\_imu}^{-1}\cdot {\mathbf{T}}_{m\_d}\cdot {\mathbf{T}}_{cam\_w}\right)\cdot ϵ\cdot \left(-\mathrm{Adj}\left({\mathbf{T}}_{cam\_imu}^{-1}\cdot {\mathbf{T}}_{m\_d}\right)\right)\cdot ϵ}}\text{\hspace{1em}(21)}$$

$$\mathbf{b}\cdot \mathbf{a}=\underset{{\mu }_2(ϵ)}{\underbrace{-\mathrm{Adj}\left({\mathbf{T}}_{cam\_imu}^{-1}\cdot {\mathbf{T}}_{m\_d}\right)\cdot \mathbf{ϵ}\cdot \mathrm{Adj}\left({\mathbf{T}}_{cam\_imu}^{-1}\cdot {\mathbf{T}}_{m\_d}\cdot {\mathbf{T}}_{cam\_w}\right)}}\cdot ϵ\text{\hspace{1em}(22)}$$

$$[\mathbf{a},\mathbf{b}]=\mathbf{a}\mathbf{b}-\mathbf{b}\mathbf{a}=\left({\mu }_1(\mathbf{ϵ})+{\mu }_2(\mathbf{ϵ})\right)\cdot \mathbf{ϵ}\text{\hspace{1em}(23)}$$

公式（23）在$\mathbf{ϵ}\to 0$时，${\mu }_1{\mu }_2$为0

所以Jacobian相对于尺度和重力方向的导数为：
$$\frac{\partial \Psi \left({\mathbf{\xi }}_{cam\_w}^D,{\mathbf{\xi }}_{m\_d}⊞\mathbf{ϵ}\right)}{\partial \mathbf{ϵ}}=\mathrm{Adj}\left({\mathbf{T}}_{ca{m}_{-}imu}^{-1}\cdot {\mathbf{T}}_{m_{-}d}\cdot {\mathbf{T}}_{ca{m}_{-}w}\right)-\mathrm{Adj}\left({\mathbf{T}}_{ca{m}_{-}imu}^{-1}\cdot {\mathbf{T}}_{m_{-}d}\right)\text{\hspace{1em}(24)}$$

Reference

---

1. Supplementary Material to: Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization ↩︎