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坐标系，它是度量系的一个被尺度放缩，和旋转的版本。定义为$\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 sim(3)$ξm_d​∈log(Tm_d​)∈sim(3).

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

Scale-aware Visual-inertial Optimization

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

1) Nonlinear Optimization

每个关键帧优化的变量为：
$\boldsymbol{s}_{i}:=\left[\left(\boldsymbol{\xi}_{c a m_{i-w}}^{D}\right)^{T}, \boldsymbol{v}_{i}^{T}, \boldsymbol{b}_{i}^{T}, a_{i}, b_{i}, d_{i}^{1}, d_{i}^{2}, \ldots, d_{i}^{m}\right]^{T} \tag{1}$si​:=[(ξcami−w​D​)T,viT​,biT​,ai​,bi​,di1​,di2​,…,dim​]T(1)
所有的向量为：
$\boldsymbol{s}=\left[\boldsymbol{c}^{T}, \boldsymbol{\xi}_{m_{-} d}^{T}, \boldsymbol{s}_{1}^{T}, \boldsymbol{s}_{2}^{T}, \ldots, \boldsymbol{s}_{n}^{T}\right]^{T} \tag{2}$s=[cT,ξm−​dT​,s1T​,s2T​,…,snT​]T(2)
相机和IMU的的residual是没有重叠部分的，因此可以独立开：
$\mathbf{H}=\mathbf{H}_{\mathrm{photo}}+\mathbf{H}_{\mathrm{imu}} \text { and } \boldsymbol{b}=\boldsymbol{b}_{\mathrm{photo}}+\boldsymbol{b}_{\mathrm{imu}} \tag{3}$H=Hphoto​+Himu​ and b=bphoto​+bimu​(3)
惯性器件的残差需要使用相对IMU的metric坐标系，因此定义新的状态：
$\boldsymbol{s}_{i}^{\prime}:=\left[\boldsymbol{\xi}_{w_{-}i m u_{i}}^{M}, \boldsymbol{v}_{i}, \boldsymbol{b}_{i}\right]^{T} \text { and } \boldsymbol{s}^{\prime}=\left[\boldsymbol{s}_{1}^{\prime T}, \boldsymbol{s}_{2}^{\prime T}, \ldots, \boldsymbol{s}_{n}^{\prime T}\right]^{T} \tag{4}$si′​:=[ξw−​imui​M​,vi​,bi​]T and s′=[s1′T​,s2′T​,…,sn′T​]T(4)
惯性的残差导出：
$\mathbf{H}_{\mathrm{imu}}^{\prime}=\mathbf{J}_{\mathrm{imu}}^{\prime T} \mathbf{W}_{\mathrm{imu}} \mathbf{J}_{\mathrm{imu}}^{\prime} \text { and } \boldsymbol{b}_{\mathrm{imu}}^{\prime}=-\mathbf{J}_{\mathrm{imu}}^{\prime T} \mathbf{W}_{\mathrm{imu}} \boldsymbol{r}_{\mathrm{imu}} \tag{5}$Himu′​=Jimu′T​Wimu​Jimu′​ and bimu′​=−Jimu′T​Wimu​rimu​(5)
这个之前的位姿表示定义不同，需要一个$\mathbf {J_{rel} }$Jrel​
$\mathbf{H}_{\mathrm{imu}}=\mathbf{J}_{\mathrm{rel}}^{T} \cdot \mathbf{H}_{\mathrm{imu}}^{\prime} \cdot \mathbf{J}_{\mathrm{rel}} \text { and } b_{\mathrm{imu}}=\mathbf{J}_{\mathrm{rel}}^{T} \cdot \boldsymbol{b}_{\mathrm{imu}}^{\prime} \tag{6}$Himu​=JrelT​⋅Himu′​⋅Jrel​ and bimu​=JrelT​⋅bimu′​(6)
详细推导见补充材料。

2）Marginalization using the Schur-Complement:

这里为了保证零空间，会使用FEJ，把$\mathbf {J_{rel} }$Jrel​固定线性化点，他是和$\xi_{m\_d}$ξm_d​相关的。

3）Dynamic Marginalization for Delayed Scale Convergence

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

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

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

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

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

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

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

$G_{\text {full }}=G_{\text {metric }} \cup G_{\text {visual }} \tag{7}$Gfull ​=Gmetric ​∪Gvisual ​(7)
优化使用的因子图集是：
$G_{b a}=G_{\text {metric }} \cup G_{\text {visual }} \cup M_{\text {curr }} \tag{8}$Gba​=Gmetric ​∪Gvisual ​∪Mcurr ​(8)

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

对于边缘化的关键帧，定义$i$i帧被边缘化时的尺度估计值为$s_i$si​，包含惯性因子被边缘化时，强制下面的约束：
$\forall i \in M_{\mathrm{curr}}: s_{i} \in\left[s_{\mathrm{middle}} / d_{i}, s_{\text {middle }} \cdot d_{i}\right] \tag{9}$∀i∈Mcurr​:si​∈[smiddle​/di​,smiddle ​⋅di​](9)

$\forall i \in M_{\text {half }}: s_{i} \in\left\{\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}\right. \tag{10}$∀i∈Mhalf ​:si​∈{[smiddle ​,smiddle ​⋅di​],[smiddle ​/di​,smiddle ​],​ if scurr ​>smiddle ​ otherwise ​(10)

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

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

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

作用：

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

这里的$d_i$di​选择应该足够小来保证一致性，也不能太小需要确保包含惯性因子，因此使用动态调整：
$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$di​太小则容易按照算法1进行更新，视觉把curr给占据了。$d_i$di​太大了则覆盖的范围小，则无法保证和之前的状态有一致性。

公式（11）确保不会发生，$M_{half}$Mhalf​被$M_{visual}$Mvisual​給重置，然后$M_{half}$Mhalf​又赋值给了$M_{curr}$Mcurr​，我们选择$d_{\min }=\sqrt{1.1}$dmin​=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 \tag{12}$c=minj:=i−n+1n​sj​maxj:=i−n+1n​sj​​−1(12)
当$c<0.005$c<0.005时认为收敛，固定$\xi_{m\_d}$ξm_d​。

计算$\mathbf{{J}_{rel}}$Jrel​

相机和IMU是在两个不同的坐标系下表示的，因此位姿变换有
$\begin{aligned} \mathbf{T}^{M}_{w\_imu} &= \mathbf{T}_{m\_d}(\mathbf{T}^D_{cam\_w})^{-1}(\mathbf{T}_{m\_d})^{-1}\mathbf{T}^M_{cam\_imu} \\\xi^M_{w_{-}imu} &=\boldsymbol{\xi}_{m_{-} d} \boxplus \left(\boldsymbol{\xi}_{c a m_{-} w}^{D}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \xi_{c a m_{-} imu}^{M} \\&=: \Psi\left(\boldsymbol{\xi}_{cam_{-} w}^{D}, \boldsymbol{\xi}_{m\_{d}}\right) \end{aligned}\tag{13}$Tw_imuM​ξw−​imuM​​=Tm_d​(Tcam_wD​)−1(Tm_d​)−1Tcam_imuM​=ξm−​d​⊞(ξcam−​wD​)−1⊞ξm−​d−1​⊞ξcam−​imuM​=:Ψ(ξcam−​wD​,ξm_d​)​(13)
公式含义是：Metric系下imu转到cam变换矩阵，先转到DSO系下，DSO系下转为imu到world变换矩阵，再把DSO系转为Metric系得到结果

对于函数$\Omega(\xi)：\mathfrak {sim}(3) \to \mathfrak{sim}(3)$Ω(ξ)：sim(3)→sim(3)，我们定义导数：
$\Omega(\boldsymbol{\xi})^{-1} \boxplus \Omega(\boldsymbol{\xi} \boxplus\boldsymbol{\epsilon})=\frac{d \Omega(\boldsymbol{\xi} \boxplus\boldsymbol{\epsilon})}{d \boldsymbol{\epsilon}} \cdot \boldsymbol{\epsilon}+\eta(\boldsymbol{\epsilon}) \cdot \boldsymbol{\epsilon} \tag{14}$Ω(ξ)−1⊞Ω(ξ⊞ϵ)=dϵdΩ(ξ⊞ϵ)​⋅ϵ+η(ϵ)⋅ϵ(14)
$\eta(\epsilon) \to 0$η(ϵ)→0当$\| \epsilon \| \to 0$∥ϵ∥→0

对于函数$f(\boldsymbol{\xi}): \mathfrak{sim}(3) \rightarrow \mathbb{R}$f(ξ):sim(3)→R，导数为
$\frac{d f(\Omega(\boldsymbol{\xi} \boxplus \boldsymbol \epsilon ))}{d \boldsymbol{\epsilon}}=\frac{d f(\Omega(\boldsymbol{\xi}) \boxplus\boldsymbol \delta)}{\delta} \cdot \frac{d \Omega(\boldsymbol{\xi} \boxplus \boldsymbol{\epsilon})}{\epsilon} \tag{15}$dϵdf(Ω(ξ⊞ϵ))​=δdf(Ω(ξ)⊞δ)​⋅ϵdΩ(ξ⊞ϵ)​(15)
1） 下面推导Jacobian相对于位姿导数
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \boldsymbol{\epsilon}, \boldsymbol{\xi}_{m\_d}\right)}{\partial \boldsymbol{\epsilon}}$∂ϵ∂Ψ(ξcam−​wD​⊞ϵ,ξm_d​)​
其中伴随性质有：
$\mathbf{T} \cdot \exp (\boldsymbol{\epsilon})=\exp \left(\mathrm{Adj}_{\mathbf{T}} \cdot \boldsymbol{\epsilon}\right) \cdot \mathbf{T}\\ \log \left(\mathbf{T} \cdot \exp (\boldsymbol{\epsilon}) \cdot \mathbf{T}^{-1}\right)=\operatorname{Adj}_{\mathbf{T}} \cdot \boldsymbol{\epsilon} \tag{16}$T⋅exp(ϵ)=exp(AdjT​⋅ϵ)⋅Tlog(T⋅exp(ϵ)⋅T−1)=AdjT​⋅ϵ(16)
其中$\mathbf{T} \in \mathbf {SIM}(3)$T∈SIM(3)，使用公式（13）（16）的结果
$\begin{aligned} &\Psi\left(\boldsymbol{\xi}_{c a m_{-w}}^{D}, \boldsymbol{\xi}_{m_{-d}}\right)^{-1} \boxplus \Psi\left(\boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \boldsymbol{\epsilon}, \boldsymbol{\xi}_{m_{-} d}\right) \\ &=\left(\boldsymbol{\xi}_{m_{-} d} \boxplus\left(\boldsymbol{\xi}_{c a m_-w}^{D}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \boldsymbol{\xi}_{c a m_{-}i m u}^{M}\right)^{-1} \boxplus\left(\boldsymbol{\xi}_{m_{-} d} \boxplus\left(\boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \boldsymbol{\epsilon}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \boldsymbol{\xi}_{c a m_{-}i m u}^{M}\right) \\ &=\left(\boldsymbol{\xi}_{c a m_{-}i m u}^{M}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d} \boxplus \boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \underbrace{\boldsymbol{\xi}_{m_{-}d}^{-1} \boxplus \boldsymbol{\xi}_{m_{-}d}}_{=0} \boxplus \boldsymbol{\epsilon}^{-1} \boxplus \left(\boldsymbol{\xi}_{c a m_{-}w}^{D}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \boldsymbol{\xi}_{c a m_{-}i m u}^{M}\\ &=\log \left(\left(\mathbf{T}_{c a m_{-}i m u}^{M}\right)^{-1} \cdot \mathbf{T}_{m_{-} d} \cdot \mathbf{T}_{c a m_{-}w}^{D} \cdot \exp (\epsilon)^{-1} \cdot\left(\mathbf{T}_{c a m\_w}^{D}\right)^{-1} \cdot \mathbf{T}_{m_{-}d}^{-1} \cdot \mathbf{T}_{c a m_{-}i m u}^{M}\right) \\ &=\operatorname{Adj}\left(\left(\mathbf{T}_{c a m_{-}i m u}^{M}\right)^{-1} \cdot \mathbf{T}_{m_{-} d} \cdot \mathbf{T}_{c a m_{-} w}^{D}\right) \cdot(-\epsilon) \end{aligned} \tag{17}$​Ψ(ξcam−w​D​,ξm−d​​)−1⊞Ψ(ξcam−​wD​⊞ϵ,ξm−​d​)=(ξm−​d​⊞(ξcam−​wD​)−1⊞ξm−​d−1​⊞ξcam−​imuM​)−1⊞(ξm−​d​⊞(ξcam−​wD​⊞ϵ)−1⊞ξm−​d−1​⊞ξcam−​imuM​)=(ξcam−​imuM​)−1⊞ξm−​d​⊞ξcam−​wD​⊞=0ξm−​d−1​⊞ξm−​d​​​⊞ϵ−1⊞(ξcam−​wD​)−1⊞ξm−​d−1​⊞ξcam−​imuM​=log((Tcam−​imuM​)−1⋅Tm−​d​⋅Tcam−​wD​⋅exp(ϵ)−1⋅(Tcam_wD​)−1⋅Tm−​d−1​⋅Tcam−​imuM​)=Adj((Tcam−​imuM​)−1⋅Tm−​d​⋅Tcam−​wD​)⋅(−ϵ)​(17)
所以最后得到Jacobian相对于位姿导数公式（17）
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m\_w}^{D} \boxplus \boldsymbol{\epsilon}, \boldsymbol{\xi}_{m\_ d}\right)}{\partial \boldsymbol{\epsilon}}=-\operatorname{Adj}\left(\left(\mathbf{T}_{c a m_{-}i m u}^{M}\right)^{-1} \cdot \mathbf{T}_{m_{-}d} \cdot \mathbf{T}_{c a m_{-}w}^{D}\right) \tag{18}$∂ϵ∂Ψ(ξcam_wD​⊞ϵ,ξm_d​)​=−Adj((Tcam−​imuM​)−1⋅Tm−​d​⋅Tcam−​wD​)(18)
**2）**推导Jacobian相对于尺度和重力方向的导数
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m_{-} w}^{D}, \boldsymbol{\xi}_{m_{-} d} \boxplus\boldsymbol{\epsilon}\right)}{\partial \boldsymbol{\epsilon}}$∂ϵ∂Ψ(ξcam−​wD​,ξm−​d​⊞ϵ)​
根据BCH公式：
$\log (\exp (\boldsymbol{a}) \cdot \exp (\boldsymbol{b}))=\boldsymbol{a}+\boldsymbol{b}+\frac{1}{2}[\boldsymbol{a}, \boldsymbol{b}]+\frac{1}{12}([\boldsymbol{a},[\boldsymbol{a}, \boldsymbol{b}]]+[\boldsymbol{b},[\boldsymbol{b}, \boldsymbol{a}]])+\frac{1}{48}([\boldsymbol{b},[\boldsymbol{a},[\boldsymbol{b}, \boldsymbol{a}]]]+[\boldsymbol{a},[\boldsymbol{b},[\boldsymbol{b}, \boldsymbol{a}]]])+\ldots \tag{19}$log(exp(a)⋅exp(b))=a+b+21​[a,b]+121​([a,[a,b]]+[b,[b,a]])+481​([b,[a,[b,a]]]+[a,[b,[b,a]]])+…(19)
李括号：$[a,b]=ab-ba$[a,b]=ab−ba

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

根据公式（16）得到：
$\boldsymbol{a}=\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m\_{d}} \cdot \mathbf{T}_{c a m_{-}w}\right) \cdot \epsilon \tag{19}$a=Adj(Tcam−​imu−1​⋅Tm_d​⋅Tcam−​w​)⋅ϵ(19)

$\boldsymbol{b}=-\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m_{-}d}\right) \cdot \epsilon \tag{20}$b=−Adj(Tcam−​imu−1​⋅Tm−​d​)⋅ϵ(20)

把公式（19）（20）带入BCH公式，即公式（19）
$\boldsymbol{a} \cdot \boldsymbol{b}=\underbrace{\operatorname{Adj}\left(\mathbf{T}_{c a m\_ i m u}^{-1} \cdot \mathbf{T}_{m\_d} \cdot \mathbf{T}_{c a m\_ w}\right) \cdot \epsilon \cdot\left(-\operatorname{Adj}\left(\mathbf{T}_{c a m\_ i m u}^{-1} \cdot \mathbf{T}_{m\_ d}\right)\right) \cdot \epsilon}_{\mu_{1}(\epsilon)} \tag{21}$a⋅b=μ1​(ϵ)Adj(Tcam_imu−1​⋅Tm_d​⋅Tcam_w​)⋅ϵ⋅(−Adj(Tcam_imu−1​⋅Tm_d​))⋅ϵ​​(21)

$\boldsymbol{b} \cdot \boldsymbol{a}=\underbrace{-\operatorname{Adj}\left(\mathbf{T}_{c a m\_ i m u}^{-1} \cdot \mathbf{T}_{m\_d}\right) \cdot \boldsymbol{\epsilon} \cdot \operatorname{Adj}\left(\mathbf{T}_{c a m\_i m u}^{-1} \cdot \mathbf{T}_{m \_ d} \cdot \mathbf{T}_{c a m\_w}\right)}_{\mu_{2}(\epsilon)} \cdot \epsilon \tag{22}$b⋅a=μ2​(ϵ)−Adj(Tcam_imu−1​⋅Tm_d​)⋅ϵ⋅Adj(Tcam_imu−1​⋅Tm_d​⋅Tcam_w​)​​⋅ϵ(22)

$[\boldsymbol{a}, \boldsymbol{b}]=\boldsymbol{a} \boldsymbol{b}-\boldsymbol{b} \boldsymbol{a}=\left(\mu_{1}(\boldsymbol{\epsilon})+\mu_{2}(\boldsymbol{\epsilon})\right) \cdot \boldsymbol{\epsilon}\tag{23}$[a,b]=ab−ba=(μ1​(ϵ)+μ2​(ϵ))⋅ϵ(23)

公式（23）在$\boldsymbol \epsilon \to 0$ϵ→0时，$\mu_1 \mu_2$μ1​μ2​为0

所以Jacobian相对于尺度和重力方向的导数为：
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m\_w}^{D}, \boldsymbol{\xi}_{m\_d} \boxplus \boldsymbol{\epsilon}\right)}{\partial \boldsymbol{\epsilon}}=\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m_{-} d} \cdot \mathbf{T}_{c a m_{-} w}\right)-\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m_{-} d}\right) \tag{24}$∂ϵ∂Ψ(ξcam_wD​,ξm_d​⊞ϵ)​=Adj(Tcam−​imu−1​⋅Tm−​d​⋅Tcam−​w​)−Adj(Tcam−​imu−1​⋅Tm−​d​)(24)

Reference

---

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