Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization1
- 把尺度,重力方向放进模型中进行联合优化
- 由于尺度不是立刻就可观的,因此我们以任一尺度进行初始化,而不是延迟初始化到量都变得可观之后(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}, ξ m _ d ∈ log ( T m _ d ) ∈ s i m ( 3 ) \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
每个关键帧优化的变量为:
s
i
:
=
[
(
ξ
c
a
m
i
−
w
D
)
T
,
v
i
T
,
b
i
T
,
a
i
,
b
i
,
d
i
1
,
d
i
2
,
…
,
d
i
m
]
T
(1)
\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−wD)T,viT,biT,ai,bi,di1,di2,…,dim]T(1)
所有的向量为:
s
=
[
c
T
,
ξ
m
−
d
T
,
s
1
T
,
s
2
T
,
…
,
s
n
T
]
T
(2)
\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是没有重叠部分的,因此可以独立开:
H
=
H
p
h
o
t
o
+
H
i
m
u
and
b
=
b
p
h
o
t
o
+
b
i
m
u
(3)
\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坐标系,因此定义新的状态:
s
i
′
:
=
[
ξ
w
−
i
m
u
i
M
,
v
i
,
b
i
]
T
and
s
′
=
[
s
1
′
T
,
s
2
′
T
,
…
,
s
n
′
T
]
T
(4)
\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−imuiM,vi,bi]T and s′=[s1′T,s2′T,…,sn′T]T(4)
惯性的残差导出:
H
i
m
u
′
=
J
i
m
u
′
T
W
i
m
u
J
i
m
u
′
and
b
i
m
u
′
=
−
J
i
m
u
′
T
W
i
m
u
r
i
m
u
(5)
\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′TWimuJimu′ and bimu′=−Jimu′TWimurimu(5)
这个之前的位姿表示定义不同,需要一个
J
r
e
l
\mathbf {J_{rel} }
Jrel
H
i
m
u
=
J
r
e
l
T
⋅
H
i
m
u
′
⋅
J
r
e
l
and
b
i
m
u
=
J
r
e
l
T
⋅
b
i
m
u
′
(6)
\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,把 J r e l \mathbf {J_{rel} } Jrel固定线性化点,他是和 ξ m _ d \xi_{m\_d} ξm_d相关的。
3)Dynamic Marginalization for Delayed Scale Convergence
边缘化时不固定尺度的线性化点。
动态边缘化就是保持几个边缘化先验,当尺度估计离线性化点交远时,重置当前使用的。保留三个边缘化先验, M v i s u a l , M c u r r , M h a l f M_{visual},M_{curr},M_{half} Mvisual,Mcurr,Mhalf
M v i s u a l M_{visual} Mvisual包含之前视觉状态的与尺度无关的信息,不能用来推断全局尺度。
M c u r r M_{curr} Mcurr包含设置线性点以来的所有尺度信息。
M h a l f M_{half} Mhalf包含最近尺度接近当前值的状态信息。
当尺度估计和线性点差很多时,设置 M c u r r = M h a l f , M c u r r = M v i s u a l M_{curr}=M_{half},M_{curr}=M_{visual} Mcurr=Mhalf,Mcurr=Mvisual,确保优化有之前的信息。
定义 G m e t r i c G_{metric} Gmetric包含VI factor, G v i s u a l G_{visual} Gvisual包含其它的factor,不包括边缘化先验
G
full
=
G
metric
∪
G
visual
(7)
G_{\text {full }}=G_{\text {metric }} \cup G_{\text {visual }} \tag{7}
Gfull =Gmetric ∪Gvisual (7)
优化使用的因子图集是:
G
b
a
=
G
metric
∪
G
visual
∪
M
curr
(8)
G_{b a}=G_{\text {metric }} \cup G_{\text {visual }} \cup M_{\text {curr }} \tag{8}
Gba=Gmetric ∪Gvisual ∪Mcurr (8)
当一个关键帧被边缘化掉,我们在因子图 G visual ∪ M visual G_{\text {visual }}\cup M_{\text {visual }} Gvisual ∪Mvisual 上进行边缘化操作来更新 M visual M_{\text {visual }} Mvisual ,意味着它包含所有的边缘化的视觉因子,没有惯性因子,因此和尺度无关。
对于边缘化的关键帧,定义
i
i
i帧被边缘化时的尺度估计值为
s
i
s_i
si,包含惯性因子被边缘化时,强制下面的约束:
∀
i
∈
M
c
u
r
r
:
s
i
∈
[
s
m
i
d
d
l
e
/
d
i
,
s
middle
⋅
d
i
]
(9)
\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)
∀ i ∈ M half : s i ∈ { [ s middle , s middle ⋅ d i ] , if s curr > s middle [ s middle / d i , s middle ] , otherwise (10) \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 m i d d l e s_{middle} smiddle是当前允许尺度范围的中间值, d i d_i di是时间 i i i尺度区间大小, s c u r r s_{curr} scurr是当前的尺度估计
我们在因子图
G
b
a
G_{ba}
Gba上面边缘化更新
M
c
u
r
r
M_{curr}
Mcurr,在因子图
G
metric
∪
G
visual
∪
M
half
G_{\text {metric }} \cup G_{\text {visual }} \cup M_{\text {half }}
Gmetric ∪Gvisual ∪Mhalf 上边缘化更新
M
h
a
l
f
M_{half}
Mhalf.
为了保证尺度约束,边缘化后使用算法:
作用:
- 确保当前使用的边缘化因子中尺度的差异小于 d i 2 d_i^2 di2的约束条件被满足
- 另一方面,因子中总是包含惯性因子,因此可以持续进行尺度估计
- FEJ被分别使用在 M c u r r M_{curr} Mcurr和 M h a l f M_{half} Mhalf中
这里的
d
i
d_i
di选择应该足够小来保证一致性,也不能太小需要确保包含惯性因子,因此使用动态调整:
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−1si<di}(11)
理解:
d
i
d_i
di太小则容易按照算法1进行更新,视觉把curr给占据了。
d
i
d_i
di太大了则覆盖的范围小,则无法保证和之前的状态有一致性。
公式(11)确保不会发生, M h a l f M_{half} Mhalf被 M v i s u a l M_{visual} Mvisual給重置,然后 M h a l f M_{half} Mhalf又赋值给了 M c u r r M_{curr} Mcurr,我们选择 d min = 1.1 d_{\min }=\sqrt{1.1} dmin=1.1
每个新帧联合优化完成后,跟踪会使用新估计的尺度、重力方向、bias和速度重新初始化,以及设置新的关键帧为参考帧。当估计完新的一帧变量,边缘化掉所有变量除了关键帧pose和最新帧的变量。因为没有涉及尺度,不需要动态边缘化。
Supplementary Material
对参数的评价
- 由于IMU的加入,点可以少一些,而且点少还会提高精度,因为可靠地点数目变多了
公式推导
使用以下公式来判断尺度是否收敛,n是队列里最大数目取60:
c
=
max
j
:
=
i
−
n
+
1
n
s
j
min
j
:
=
i
−
n
+
1
n
s
j
−
1
(12)
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+1nsjmaxj:=i−n+1nsj−1(12)
当
c
<
0.005
c<0.005
c<0.005时认为收敛,固定
ξ
m
_
d
\xi_{m\_d}
ξm_d。
计算 J r e l \mathbf{{J}_{rel}} Jrel
相机和IMU是在两个不同的坐标系下表示的,因此位姿变换有
T
w
_
i
m
u
M
=
T
m
_
d
(
T
c
a
m
_
w
D
)
−
1
(
T
m
_
d
)
−
1
T
c
a
m
_
i
m
u
M
ξ
w
−
i
m
u
M
=
ξ
m
−
d
⊞
(
ξ
c
a
m
−
w
D
)
−
1
⊞
ξ
m
−
d
−
1
⊞
ξ
c
a
m
−
i
m
u
M
=
:
Ψ
(
ξ
c
a
m
−
w
D
,
ξ
m
_
d
)
(13)
\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系得到结果
对于函数
Ω
(
ξ
)
:
s
i
m
(
3
)
→
s
i
m
(
3
)
\Omega(\xi):\mathfrak {sim}(3) \to \mathfrak{sim}(3)
Ω(ξ):sim(3)→sim(3),我们定义导数:
Ω
(
ξ
)
−
1
⊞
Ω
(
ξ
⊞
ϵ
)
=
d
Ω
(
ξ
⊞
ϵ
)
d
ϵ
⋅
ϵ
+
η
(
ϵ
)
⋅
ϵ
(14)
\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)
η
(
ϵ
)
→
0
\eta(\epsilon) \to 0
η(ϵ)→0当
∥
ϵ
∥
→
0
\| \epsilon \| \to 0
∥ϵ∥→0
对于函数
f
(
ξ
)
:
s
i
m
(
3
)
→
R
f(\boldsymbol{\xi}): \mathfrak{sim}(3) \rightarrow \mathbb{R}
f(ξ):sim(3)→R,导数为
d
f
(
Ω
(
ξ
⊞
ϵ
)
)
d
ϵ
=
d
f
(
Ω
(
ξ
)
⊞
δ
)
δ
⋅
d
Ω
(
ξ
⊞
ϵ
)
ϵ
(15)
\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相对于位姿导数
∂
Ψ
(
ξ
c
a
m
−
w
D
⊞
ϵ
,
ξ
m
_
d
)
∂
ϵ
\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)
其中伴随性质有:
T
⋅
exp
(
ϵ
)
=
exp
(
A
d
j
T
⋅
ϵ
)
⋅
T
log
(
T
⋅
exp
(
ϵ
)
⋅
T
−
1
)
=
Adj
T
⋅
ϵ
(16)
\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)
其中
T
∈
S
I
M
(
3
)
\mathbf{T} \in \mathbf {SIM}(3)
T∈SIM(3),使用公式(13)(16)的结果
Ψ
(
ξ
c
a
m
−
w
D
,
ξ
m
−
d
)
−
1
⊞
Ψ
(
ξ
c
a
m
−
w
D
⊞
ϵ
,
ξ
m
−
d
)
=
(
ξ
m
−
d
⊞
(
ξ
c
a
m
−
w
D
)
−
1
⊞
ξ
m
−
d
−
1
⊞
ξ
c
a
m
−
i
m
u
M
)
−
1
⊞
(
ξ
m
−
d
⊞
(
ξ
c
a
m
−
w
D
⊞
ϵ
)
−
1
⊞
ξ
m
−
d
−
1
⊞
ξ
c
a
m
−
i
m
u
M
)
=
(
ξ
c
a
m
−
i
m
u
M
)
−
1
⊞
ξ
m
−
d
⊞
ξ
c
a
m
−
w
D
⊞
ξ
m
−
d
−
1
⊞
ξ
m
−
d
⏟
=
0
⊞
ϵ
−
1
⊞
(
ξ
c
a
m
−
w
D
)
−
1
⊞
ξ
m
−
d
−
1
⊞
ξ
c
a
m
−
i
m
u
M
=
log
(
(
T
c
a
m
−
i
m
u
M
)
−
1
⋅
T
m
−
d
⋅
T
c
a
m
−
w
D
⋅
exp
(
ϵ
)
−
1
⋅
(
T
c
a
m
_
w
D
)
−
1
⋅
T
m
−
d
−
1
⋅
T
c
a
m
−
i
m
u
M
)
=
Adj
(
(
T
c
a
m
−
i
m
u
M
)
−
1
⋅
T
m
−
d
⋅
T
c
a
m
−
w
D
)
⋅
(
−
ϵ
)
(17)
\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−wD,ξ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)
∂
Ψ
(
ξ
c
a
m
_
w
D
⊞
ϵ
,
ξ
m
_
d
)
∂
ϵ
=
−
Adj
(
(
T
c
a
m
−
i
m
u
M
)
−
1
⋅
T
m
−
d
⋅
T
c
a
m
−
w
D
)
(18)
\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相对于尺度和重力方向的导数
∂
Ψ
(
ξ
c
a
m
−
w
D
,
ξ
m
−
d
⊞
ϵ
)
∂
ϵ
\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
(
a
)
⋅
exp
(
b
)
)
=
a
+
b
+
1
2
[
a
,
b
]
+
1
12
(
[
a
,
[
a
,
b
]
]
+
[
b
,
[
b
,
a
]
]
)
+
1
48
(
[
b
,
[
a
,
[
b
,
a
]
]
]
+
[
a
,
[
b
,
[
b
,
a
]
]
]
)
+
…
(19)
\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
]
=
a
b
−
b
a
[a,b]=ab-ba
[a,b]=ab−ba
同公式(17)的推导一样凑成Adj形式:
根据公式(16)得到:
a
=
Adj
(
T
c
a
m
−
i
m
u
−
1
⋅
T
m
_
d
⋅
T
c
a
m
−
w
)
⋅
ϵ
(19)
\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)
b = − Adj ( T c a m − i m u − 1 ⋅ T m − d ) ⋅ ϵ (20) \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)
a
⋅
b
=
Adj
(
T
c
a
m
_
i
m
u
−
1
⋅
T
m
_
d
⋅
T
c
a
m
_
w
)
⋅
ϵ
⋅
(
−
Adj
(
T
c
a
m
_
i
m
u
−
1
⋅
T
m
_
d
)
)
⋅
ϵ
⏟
μ
1
(
ϵ
)
(21)
\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)
b ⋅ a = − Adj ( T c a m _ i m u − 1 ⋅ T m _ d ) ⋅ ϵ ⋅ Adj ( T c a m _ i m u − 1 ⋅ T m _ d ⋅ T c a m _ w ) ⏟ μ 2 ( ϵ ) ⋅ ϵ (22) \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)
[ a , b ] = a b − b a = ( μ 1 ( ϵ ) + μ 2 ( ϵ ) ) ⋅ ϵ (23) [\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)在 ϵ → 0 \boldsymbol \epsilon \to 0 ϵ→0时, μ 1 μ 2 \mu_1 \mu_2 μ1μ2为0
所以Jacobian相对于尺度和重力方向的导数为:
∂
Ψ
(
ξ
c
a
m
_
w
D
,
ξ
m
_
d
⊞
ϵ
)
∂
ϵ
=
Adj
(
T
c
a
m
−
i
m
u
−
1
⋅
T
m
−
d
⋅
T
c
a
m
−
w
)
−
Adj
(
T
c
a
m
−
i
m
u
−
1
⋅
T
m
−
d
)
(24)
\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
Supplementary Material to: Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization ↩︎