这个笔记会整理网上和论文里的一些参考资料,也会有一些自己的心得,欢迎交流╰( ̄▽ ̄)╮。
一、概述
视觉惯性导航系统(VINS)相比较传统的视觉SLAM有更大的优势。对于相机快速运动和环境剧烈光照变化导致相机导航失效的情况,IMU能在短时间内提供较精确的定位;对于IMU随着时间的增长会积累大量的导航误差的情况,相机识别过去的位置能帮助减少甚至消除IMU累积的误差。显然,VINS比视觉SLAM有更高的鲁棒性,最近几年也受到较大的关注。
二、坐标系
VINS系统里会用到的坐标系比较多,弄清楚每个坐标系对理解目前开源的VINS非常重要。
首先是利用外参进行坐标变换可能会用到的三大坐标系:
- 体坐标系,坐标系原点固定在惯性导航单元(IMU)的位置,方向与IMU测量的方向一致。从IMU获取的加速度和角速度都是基于这个坐标系,记为B;
- 相机坐标系,坐标系原点固定在相机光心位置(一般把凸透镜的中心看作光心),x,y轴与图像的X,Y轴平行,z轴为相机光轴,从光心出发指向前方,记为C;
- 世界坐标系,通常而言,坐标系原点固定在第一帧图像对应的相机坐标系原点,x,y,z轴的方向与对应的第一帧相机坐标系方向平行,记为W;
除了上述的几个坐标系外,还有一个惯性坐标系。惯性坐标系z轴方向与重力方向一致。
然后是利用相机模型(这里指针孔相机模型),也可以说是相机的测量模型,进行坐标变换可能会用到的三个空间点坐标:
- 空间点的相机坐标,空间点在相机坐标系下的3D坐标,记为P=[X,Y,Z];
- 空间点的归一化坐标,从空间点出发的光线经过光心,投影在相机前z=1的平面上,就得到了空间点的归一化坐标,记为Pc=[X/Z,Y/Z,1]。如果考虑畸变校正的话,一般是代入归一化坐标计算;
- 空间点的像素坐标,图像左上角为原点,沿着图像长的方向为u轴,沿着图像宽的方向为v轴,记为 p=[u,v],u,v一般为整数;
*物理成像坐标是指从空间点出发经过光心的光线,投影在相机前z=f的平面上(实际上是z=-f平面),以z=f平面与光轴的交点为原点,一般不使用;
这里引用《视觉SLAM十四讲》的图5-1,图中是针孔相机模型的示意图。
三、IMU预积分
这部分更详细的内容可以参考IMU 预积分推导和【泡泡机器人原创专栏】IMU预积分总结与公式推导,也可以直接看论文On-Manifold Preintegration for Real-Time Visual–Inertial Odometry。
IMU通常指由3个加速度计和3个陀螺仪组成的组合单元,能测量对应轴上的加速度和角速度,当然测量值受到测量噪声和bias的影响,长时间运行的系统会无限累积误差。
发展至今,VINS大致分为两类:依靠滤波融合视觉和惯性导航的数据;依靠优化全局位姿来降低误差。后者能获取更高的精度,但是计算量大。
当采用全局优化的方法时,每执行一次全局优化,所有关键帧的位姿都发生改变,这时就需要重新积分IMU测量获取新的状态,再利用估计残差进行非线性优化。这一过程消耗大量的计算资源,而IMU预积分的作用就是避免IMU积分的重复运算。
因为偏置也被优化了,所以预积分还是要更新,不过相比重新计算所有状态量,计算量要小。
3.1 IMU测量模型
首先从IMU能获取其角速度和加速度,测量值是真实值、bias和噪声的和。
陀螺仪测量模型(不考虑地球自转):
ω
~
W
B
B
(
t
)
=
ω
W
B
B
(
t
)
+
b
g
(
t
)
+
η
g
(
t
)
\widetilde{\omega} _{WB}^{B}(t)=\omega _{WB}^{B}(t)+b^{g}(t)+\eta ^{g}(t)
ω
WBB(t)=ωWBB(t)+bg(t)+ηg(t)
加速度计测量模型(不考虑地球自转):
a
~
B
(
t
)
=
R
W
B
T
(
a
W
(
t
)
−
g
W
)
+
b
a
(
t
)
+
η
a
(
t
)
\widetilde{a} ^{B}(t)=R_{WB}^{T}(a ^{W}(t)-g^{W})+b^{a}(t)+\eta ^{a}(t)
a
B(t)=RWBT(aW(t)−gW)+ba(t)+ηa(t)
其中 ω ~ W B B ( t ) \widetilde{\omega} _{WB}^{B}(t) ω WBB(t)和 a ~ B ( t ) \widetilde{a} ^{B}(t) a B(t)分别表示角速度和加速度的测量值, ω W B B ( t ) {\omega} _{WB}^{B}(t) ωWBB(t)和 a B ( t ) {a} ^{B}(t) aB(t)分别表示角速度和加速度的真实值, b g ( t ) b^{g}(t) bg(t)和 b a ( t ) b^{a}(t) ba(t)分别表示陀螺仪和加速度计的偏置, η \eta η为噪声。
3.2 IMU预积分状态量
t时刻的三个状态量:B系到W系旋转
R
W
B
(
t
)
R_{WB}(t)
RWB(t)、W系下IMU速度
v
W
(
t
)
v^{W}(t)
vW(t)和IMU位置
p
W
(
t
)
p^{W}(t)
pW(t)。假设在之后的
Δ
t
\Delta t
Δt时间内,IMU测量值保持不变,由欧拉积分能获取三个状态量在
t
+
Δ
t
t+\Delta t
t+Δt时刻的值。考虑离散情况,令i表示t时刻,j表示
t
+
N
Δ
t
t+N\Delta t
t+NΔt时刻,则用IMU测量值表示的j时刻的状态量为:
R
j
=
R
i
∏
k
=
i
j
−
1
E
x
p
[
(
ω
~
k
−
b
k
g
−
η
k
g
d
)
Δ
t
]
R_{j}=R_{i}\prod _{k=i}^{j-1}Exp[(\widetilde{\omega} _{k}-b_{k}^{g}-\eta _{k}^{gd})\Delta t]
Rj=Rik=i∏j−1Exp[(ω
k−bkg−ηkgd)Δt]
v
j
=
v
i
+
∑
k
=
i
j
−
1
[
R
k
(
a
~
k
−
b
k
a
−
η
k
a
d
)
]
Δ
t
+
g
Δ
t
i
j
v_{j}=v_{i}+\sum_{k=i}^{j-1}[R_{k}(\widetilde{a} _{k}-b_{k}^{a}-\eta _{k}^{ad})]\Delta t+g\Delta t_{ij}
vj=vi+k=i∑j−1[Rk(a
k−bka−ηkad)]Δt+gΔtij
p
j
=
p
i
+
∑
k
=
i
j
−
1
v
k
Δ
t
+
1
2
∑
k
=
i
j
−
1
g
Δ
t
2
+
1
2
∑
k
=
i
j
−
1
R
k
(
a
~
k
−
b
k
a
−
η
k
a
d
)
Δ
t
2
p_{j}=p_{i}+\sum_{k=i}^{j-1}v_{k}\Delta t+\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^{2}+\frac{1}{2}\sum_{k=i}^{j-1}R_{k}(\widetilde{a} _{k}-b_{k}^{a}-\eta _{k}^{ad})\Delta t^{2}
pj=pi+k=i∑j−1vkΔt+21k=i∑j−1gΔt2+21k=i∑j−1Rk(a
k−bka−ηkad)Δt2
其中
E
x
p
(
)
Exp()
Exp()表示
E
x
p
:
s
o
(
3
)
→
S
O
(
3
)
Exp: so(3) \rightarrow SO(3)
Exp:so(3)→SO(3),
s
o
(
3
)
so(3)
so(3)是李代数,就是一个3维向量;
S
O
(
3
)
SO(3)
SO(3)是李群,就是3*3的旋转矩阵;
E
x
p
(
)
Exp()
Exp()表示从李代数换算到李群。
通过上述几个公式可以定义出IMU预积分状态量计算值(下面第一个等于后)和测量值(下面第二个等于后)两组值。
Δ
R
i
j
=
R
i
T
R
j
=
∏
k
=
i
j
−
1
E
x
p
[
(
ω
~
k
−
b
k
g
−
η
k
g
d
)
Δ
t
]
\Delta R_{ij}=R_{i}^{T}R_{j}=\prod _{k=i}^{j-1}Exp[(\widetilde{\omega} _{k}-b_{k}^{g}-\eta _{k}^{gd})\Delta t]
ΔRij=RiTRj=k=i∏j−1Exp[(ω
k−bkg−ηkgd)Δt]
Δ
v
i
j
=
R
i
T
(
v
j
−
v
i
−
g
Δ
t
i
j
)
=
∑
k
=
i
j
−
1
[
Δ
R
i
k
(
a
~
k
−
b
k
a
−
η
k
a
d
)
]
Δ
t
\Delta v_{ij}=R_{i}^{T}(v_{j}-v_{i}-g\Delta t_{ij})=\sum_{k=i}^{j-1}[\Delta R_{ik}(\widetilde{a} _{k}-b_{k}^{a}-\eta _{k}^{ad})]\Delta t
Δvij=RiT(vj−vi−gΔtij)=k=i∑j−1[ΔRik(a
k−bka−ηkad)]Δt
Δ
p
i
j
=
R
i
T
(
p
j
−
p
i
−
v
i
Δ
t
i
j
−
1
2
∑
k
=
i
j
−
1
g
Δ
t
2
)
=
∑
k
=
i
j
−
1
[
Δ
v
i
k
Δ
t
+
1
2
Δ
R
i
k
(
a
~
k
−
b
k
a
−
η
k
a
d
)
Δ
t
2
]
\Delta p_{ij}=R_{i}^{T}(p_{j}-p_{i}-v_{i}\Delta t_{ij}-\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^{2})=\sum_{k=i}^{j-1}[\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}(\widetilde{a} _{k}-b_{k}^{a}-\eta _{k}^{ad})\Delta t^{2}]
Δpij=RiT(pj−pi−viΔtij−21k=i∑j−1gΔt2)=k=i∑j−1[ΔvikΔt+21ΔRik(a
k−bka−ηkad)Δt2]
为什么这里我要写成计算值和测量值?因为计算值是通过非IMU测量数据得到的,比如纯视觉SfM计算的IMU位姿;测量值顾名思义,就是用IMU测量数据得到的;这两个值的差作为目标函数
3.3 IMU预积分测量噪声
对预积分状态量测量值分离噪声,方便后续作为非线性优化迭代时的状态量增量。
假设ij时间段内的bias不变,推导过程这里省略,直接给出分离出的预积分测量噪声的递推形式结果:
δ
ϕ
i
j
=
Δ
R
~
j
−
1
j
δ
ϕ
i
j
−
1
+
J
r
j
−
1
η
j
−
1
g
d
Δ
t
\delta \phi _{ij}=\Delta \widetilde R_{j-1j}\delta \phi _{ij-1}+J_{r}^{j-1}\eta_{j-1}^{gd}\Delta t
δϕij=ΔR
j−1jδϕij−1+Jrj−1ηj−1gdΔt
δ
v
i
j
=
δ
v
i
j
−
1
+
Δ
R
~
i
j
−
1
η
j
−
1
a
d
Δ
t
−
Δ
R
~
i
j
−
1
(
a
~
j
−
1
−
b
j
−
1
a
)
∧
δ
ϕ
i
j
−
1
Δ
t
\delta v_{ij}=\delta v_{ij-1}+\Delta \widetilde R_{ij-1}\eta_{j-1}^{ad}\Delta t-\Delta \widetilde R_{ij-1}(\widetilde{a} _{j-1}-b_{j-1}^{a})^{\wedge }\delta \phi _{ij-1}\Delta t
δvij=δvij−1+ΔR
ij−1ηj−1adΔt−ΔR
ij−1(a
j−1−bj−1a)∧δϕij−1Δt
δ
p
i
j
=
δ
p
i
j
−
1
+
δ
v
i
j
−
1
Δ
t
+
1
2
Δ
R
~
i
j
−
1
η
j
−
1
a
d
Δ
t
2
−
1
2
Δ
R
~
i
j
−
1
(
a
~
j
−
1
−
b
j
−
1
a
)
∧
δ
ϕ
i
j
−
1
Δ
t
2
\delta p_{ij}=\delta p_{ij-1}+\delta v_{ij-1}\Delta t+\frac{1}{2}\Delta \widetilde R_{ij-1}\eta_{j-1}^{ad}\Delta t^{2}-\frac{1}{2}\Delta \widetilde R_{ij-1}(\widetilde{a} _{j-1}-b_{j-1}^{a})^{\wedge }\delta \phi _{ij-1}\Delta t^{2}
δpij=δpij−1+δvij−1Δt+21ΔR
ij−1ηj−1adΔt2−21ΔR
ij−1(a
j−1−bj−1a)∧δϕij−1Δt2
IMU预积分理想值、测量值、测量噪声之间的关系为:
Δ
R
i
j
=
Δ
R
~
i
j
E
x
p
(
−
δ
ϕ
i
j
)
\Delta R_{ij}=\Delta \widetilde R_{ij}Exp(-\delta \phi_{ij})
ΔRij=ΔR
ijExp(−δϕij)
Δ
v
i
j
=
Δ
v
~
i
j
−
δ
v
i
j
\Delta v_{ij}=\Delta \widetilde v_{ij}-\delta v_{ij}
Δvij=Δv
ij−δvij
Δ
p
i
j
=
Δ
p
~
i
j
−
δ
p
i
j
\Delta p_{ij}=\Delta \widetilde p_{ij}-\delta p_{ij}
Δpij=Δp
ij−δpij
其中
δ
ϕ
i
j
\delta \phi _{ij}
δϕij、
δ
v
i
j
\delta v_{ij}
δvij、
δ
p
i
j
\delta p_{ij}
δpij分别表示旋转矩阵,速度,位置在ij时间内预积分状态量的测量噪声;
J
r
j
−
1
J_{r}^{j-1}
Jrj−1是右乘雅克比矩阵,矩阵解释参考《视觉SLAM十四讲》,具体推导参考【泡泡机器人原创专栏】IMU预积分总结与公式推导(二);^表示计算反对称矩阵;
Δ
R
i
j
,
Δ
v
i
j
,
Δ
p
i
j
\Delta R_{ij},\Delta v_{ij},\Delta p_{ij}
ΔRij,Δvij,Δpij表示IMU预积分理想值;
Δ
R
~
i
j
,
Δ
v
~
i
j
,
Δ
p
~
i
j
\Delta \widetilde R_{ij},\Delta \widetilde v_{ij},\Delta \widetilde p_{ij}
ΔR
ij,Δv
ij,Δp
ij表示预积分测量值。
把这些预积分测量噪声合成一个向量,并记为
η
i
j
=
[
δ
ϕ
i
j
δ
v
i
j
δ
p
i
j
]
\eta _{ij}=[\delta \phi _{ij}\: \delta v_{ij}\: \delta p_{ij}]
ηij=[δϕijδvijδpij],将IMU测量噪声合成一个向量,并记为
η
j
d
=
[
η
j
g
d
η
j
a
d
]
\eta_{j}^{d}=[\eta_{j}^{gd}\: \eta_{j}^{ad}]
ηjd=[ηjgdηjad],则上述结果可以用矩阵表示,系数简单用字母代替
η
i
j
=
A
j
−
1
η
i
j
−
1
+
B
j
−
1
η
j
−
1
d
\eta _{ij}=A_{j-1}\eta _{ij-1}+B_{j-1}\eta_{j-1}^{d}
ηij=Aj−1ηij−1+Bj−1ηj−1d
于是预积分测量噪声的协方差矩阵
Σ
i
j
\Sigma_{ij}
Σij也可以通过递推得到
Σ
i
j
=
A
j
−
1
Σ
i
j
−
1
A
j
−
1
T
+
B
j
−
1
Σ
η
B
j
−
1
T
\Sigma _{ij}=A_{j-1}\Sigma _{ij-1}A_{j-1}^{T}+B_{j-1}\Sigma_{\eta}B_{j-1}^{T}
Σij=Aj−1Σij−1Aj−1T+Bj−1ΣηBj−1T
3.4 IMU预积分状态量修正(考虑bias变化)
之前的推导基于一个假设:在ij时间段内bias不变。但实际情况是bias会不断累积,因此在VINS进行优化时也需要优化bias。bias的改变也会带来IMU状态量的改变,为了避免重新积分IMU状态量,就需要考虑IMU预积分当bias改变时的修正。为简化运算,采用关于bias的预积分的一阶近似更新。
Δ
R
~
i
j
(
b
^
i
g
)
≈
Δ
R
~
i
j
(
b
ˉ
i
g
)
E
x
p
(
∂
Δ
R
ˉ
i
j
∂
b
ˉ
g
δ
b
i
g
)
\Delta \widetilde R_{ij}(\hat{b}_{i}^{g})\approx \Delta \widetilde R_{ij}(\bar{b}_{i}^{g})Exp(\frac{\partial \Delta \bar{R}_{ij}}{\partial \bar{b}^{g}}\delta b_{i}^{g})
ΔR
ij(b^ig)≈ΔR
ij(bˉig)Exp(∂bˉg∂ΔRˉijδbig)
Δ
v
i
j
(
b
^
i
g
,
b
^
i
a
)
≈
Δ
v
i
j
(
b
ˉ
i
g
,
b
ˉ
i
a
)
+
∂
Δ
v
ˉ
i
j
∂
b
ˉ
g
δ
b
i
g
+
∂
Δ
v
ˉ
i
j
∂
b
ˉ
a
δ
b
i
a
\Delta v_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})\approx \Delta v_{ij}(\bar{b}_{i}^{g},\bar{b}_{i}^{a})+\frac{\partial \Delta \bar{v}_{ij}}{\partial \bar{b}^{g}}\delta b_{i}^{g}+\frac{\partial \Delta \bar{v}_{ij}}{\partial \bar{b}^{a}}\delta b_{i}^{a}
Δvij(b^ig,b^ia)≈Δvij(bˉig,bˉia)+∂bˉg∂Δvˉijδbig+∂bˉa∂Δvˉijδbia
Δ
p
i
j
(
b
^
i
g
,
b
^
i
a
)
≈
Δ
p
i
j
(
b
ˉ
i
g
,
b
ˉ
i
a
)
+
∂
Δ
p
ˉ
i
j
∂
b
ˉ
g
δ
b
i
g
+
∂
Δ
p
ˉ
i
j
∂
b
ˉ
a
δ
b
i
a
\Delta p_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})\approx \Delta p_{ij}(\bar{b}_{i}^{g},\bar{b}_{i}^{a})+\frac{\partial \Delta \bar{p}_{ij}}{\partial \bar{b}^{g}}\delta b_{i}^{g}+\frac{\partial \Delta \bar{p}_{ij}}{\partial \bar{b}^{a}}\delta b_{i}^{a}
Δpij(b^ig,b^ia)≈Δpij(bˉig,bˉia)+∂bˉg∂Δpˉijδbig+∂bˉa∂Δpˉijδbia
其中
b
^
i
g
\hat{b}_{i}^{g}
b^ig和
b
^
i
a
\hat{b}_{i}^{a}
b^ia表示bias更新后的估计值,
b
ˉ
i
g
\bar{b}_{i}^{g}
bˉig和
b
ˉ
i
a
\bar{b}_{i}^{a}
bˉia表示bias更新前的估计值,
δ
b
i
g
\delta b_{i}^{g}
δbig和
δ
b
i
a
\delta b_{i}^{a}
δbia表示bias更新量。
上式中的IMU预积分状态量关于bias的偏导参考【泡泡机器人原创专栏】IMU预积分总结与公式推导(三)。
3.5 优化
到这里已经定义了IMU预积分状态量,噪声,并且考虑了bias变化后带来的状态量修正。之后需要考虑的是如何利用ij时间段内的预积分优化时间段内的状态。
在ij时间段内待优化的状态量包括: R i , v i , p i , R j , v j , p j , δ b i g , δ b i a R_{i},v_{i},p_{i},R_{j},v_{j},p_{j},\delta b_{i}^{g},\delta b_{i}^{a} Ri,vi,pi,Rj,vj,pj,δbig,δbia。考虑 δ b i g , δ b i a \delta b_{i}^{g},\delta b_{i}^{a} δbig,δbia的原因是需要用bias更新量修正预积分状态量。
待优化的目标函数是残差的加权平方和。残差是在3.2中定义的两个预积分状态量的差别,其中IMU预积分状态量计算值是通过视觉或者其他非IMU的方式计算的预积分值,预积分状态量测量值则是通过IMU测量值计算的预积分值;计算加权平方和的原因是考虑到状态量单位的影响,权重为预积分测量噪声协方差的逆。
残差定义:
r
Δ
R
i
j
=
l
o
g
[
(
Δ
R
~
i
j
(
b
^
i
g
)
)
T
Δ
R
i
j
]
r_{\Delta R_{ij}}=log[(\Delta \widetilde R_{ij}(\hat{b}_{i}^{g}))^{T}\Delta R_{ij}]
rΔRij=log[(ΔR
ij(b^ig))TΔRij]
r
Δ
v
i
j
=
Δ
v
i
j
−
Δ
v
i
j
(
b
^
i
g
,
b
^
i
a
)
r_{\Delta v_{ij}}=\Delta v_{ij}-\Delta v_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})
rΔvij=Δvij−Δvij(b^ig,b^ia)
r
Δ
p
i
j
=
Δ
p
i
j
−
Δ
p
i
j
(
b
^
i
g
,
b
^
i
a
)
r_{\Delta p_{ij}}=\Delta p_{ij}-\Delta p_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})
rΔpij=Δpij−Δpij(b^ig,b^ia)
优化方法采用GN或者LM,具体参考《视觉SLAM十四讲》。每个状态量的更新方法如下:
R
i
←
R
i
⋅
E
x
p
(
δ
ϕ
i
)
,
v
i
←
v
i
+
δ
v
i
,
p
i
←
p
i
+
δ
p
i
R_{i}\leftarrow R_{i}\cdot Exp(\delta \phi_{i}),v_{i}\leftarrow v_{i}+\delta v_{i},p_{i}\leftarrow p_{i}+\delta p_{i}
Ri←Ri⋅Exp(δϕi),vi←vi+δvi,pi←pi+δpi
R
j
←
R
j
⋅
E
x
p
(
δ
ϕ
j
)
,
v
j
←
v
j
+
δ
v
j
,
p
j
←
p
j
+
δ
p
j
R_{j}\leftarrow R_{j}\cdot Exp(\delta \phi_{j}),v_{j}\leftarrow v_{j}+\delta v_{j},p_{j}\leftarrow p_{j}+\delta p_{j}
Rj←Rj⋅Exp(δϕj),vj←vj+δvj,pj←pj+δpj
δ
b
i
g
←
δ
b
i
g
+
δ
b
i
g
~
,
δ
b
i
a
←
δ
b
i
a
+
δ
b
i
a
~
\delta b_{i}^{g}\leftarrow \delta b_{i}^{g}+\widetilde{\delta b_{i}^{g}},\delta b_{i}^{a}\leftarrow \delta b_{i}^{a}+\widetilde{\delta b_{i}^{a}}
δbig←δbig+δbig
,δbia←δbia+δbia
要计算状态量更新中的更新量,就需要计算目标函数对状态量的雅克比矩阵。矩阵计算结果参考【泡泡机器人原创专栏】IMU预积分总结与公式推导(四)