视觉惯性SLAM: VI ORB-SLAM
这篇博客
之前看了VI ORB-SLAM的论文,现在终于有时间来写论文的学习笔记(不容易啊)。因为这是第一次看视觉惯性SLAM论文,所以写这篇博客来详细地记录一下,以后发现问题会再做修改。话不多说,直接进入正题。
【转载声明】本篇文字均为原创,如转载请注明出处
视觉惯性SLAM
只使用相机或惯性元件(IMU)传感器搭建的SLAM系统会存在诸多问题。因为相机虽然可以很好地完成闭环检测和定位,但是它易受到模糊等影响,所以运动速度不能过快。而IMU却可以在速度较快,或周围环境纹理较少时完成定位操作。不过它不能实现闭环检测,且很容易产生漂移现象(因为存在加速度偏差和陀螺仪偏差:即使IMU元件不动,测量值也可能不为0)。
这么看来这两个传感器的功能刚好能够互补,用相机来帮助IMU消除累积误差,并完成闭环检测;用IMU帮助相机解决在纹理少、运动快的情况下的定位问题。因此,视觉惯性SLAM也就诞生了。
VI ORB-SLAM是由ORB-SLAM和IMU模块组成,它的大部分环节功能和ORB-SLAM是相似的,所以相似部分之后就不会过多地介绍。
预备知识
符号说明:
这里提前说一些之后会在公式中使用的符号和变量。
1、惯性传感器(IMU)的坐标系记为:
B
B
B (Body)
2、相机传感器的坐标系记为:
C
C
C(Camera)
3、世界坐标系记为:
W
W
W
4、IMU中的加速度偏差:
b
a
b_{a}
ba
5、IMU中的陀螺仪偏差:
b
g
b_{g}
bg
6、IMU中两次测量值之间的时间间隔:Δt
7、IMU在世界坐标系中的方向:
R
W
B
R_{WB}
RWB(从B坐标系到W坐标系的旋转矩阵)
8、将B坐标系的速度在W坐标系中表示:
W
V
B
_{W}V_{B}
WVB
9、将B坐标系的位置在W坐标系中表示:
W
P
B
_{W}P_{B}
WPB(B到W坐标系之间的平移变换)
10、重力加速度:
g
W
g_{W}
gW
11、重力方向:
g
W
^
=
g
W
∗
∥
g
W
∗
∥
\hat{g_{W}}=\frac{g_{W}^{*}}{\left \| g_{W}^{*} \right \|}
gW^=∥gW∗∥gW∗
12、重力的大小
G
G
G
13、IMU测得的加速度大小:
a
B
a_{B}
aB
14、IMU测得的角速度大小:
w
B
w_{B}
wB
15、B坐标系到相机坐标系的变换矩阵:
T
C
B
=
[
R
C
B
∣
C
P
B
]
T_{CB}=[R_{CB} | _{C}P_{B}]
TCB=[RCB∣CPB]
相机投影变换矩阵
基于小孔成像模型来算出相机坐标系与像素坐标系之间的变换矩阵,并定义投影函数
π
\pi
π:
π
(
X
C
)
=
[
f
u
X
C
Z
C
+
c
u
f
v
Y
C
Z
C
+
c
v
]
,
X
C
=
[
X
C
Y
C
Z
C
]
T
.
.
.
.
.
.
.
.
.
.
.
.
.
(
1
)
\pi(X_{C})=\begin{bmatrix} f_{u}\frac{X_{C}}{Z_{C}}+c_{u}\\ f_{v}\frac{Y_{C}}{Z_{C}}+c_{v} \end{bmatrix} ,X_{C}=[ X_{C} Y_{C} Z_{C}]^{T}.............(1)
π(XC)=[fuZCXC+cufvZCYC+cv],XC=[XCYCZC]T.............(1)
式中
[
f
u
,
f
v
]
T
[f_{u},f_{v}]^{T}
[fu,fv]T表示焦距长度,
[
c
u
,
c
v
]
[c_{u},c_{v}]
[cu,cv]表示主点位置。它们的单位都为像素。
IMU数据更新方程
更新的量有三个: R , p , v R,p,v R,p,v。更新的公式如下:
这个公式是从参考文献[1]中获得的,表示的是前后两帧之间的变量关系(和参考文献[1]不同的是,这里忽略了噪声对角速度、加速度测量值的影响)。这是之后实现IMU数据预积分的基础。IMU数据的预积分
预积分就是计算出两相邻帧或关键帧之间的IMU数据的总变化量
(
Δ
R
,
Δ
p
,
Δ
v
)
(ΔR,Δp,Δv)
(ΔR,Δp,Δv),它由相邻帧或关键帧之间所有的IMU测量数据叠加后获得。在VI ORB-SLAM中用这个公式表示预积分:
这个公式也是从参考文献[1]中获得的。其中
J
Δ
v
g
b
g
i
J_{\Delta v}^{g}b_{g}^{i}
JΔvgbgi表示第 i 帧的陀螺仪偏差对于
Δ
v
Δv
Δv的影响量,
J
Δ
v
g
J_{\Delta v}^{g}
JΔvg是
Δ
v
Δv
Δv关于
b
g
b_{g}
bg的雅可比矩阵。其他与此类似的变量也是这个意思。
VI ORB-SLAM各环节工作方式
因为是基于ORB-SLAM搭建的系统,所以VI ORB-SLAM的工作原理和ORB-SLAM十分相似,也是使用三个线程:Tracking、Local Mapping、Loop Close。不过它们各线程的具体工作内容有所不同,下面来介绍这些不同之处。
Initialization
在ORB-SLAM中,初始化的任务有:创建出初始地图、定义第一参考帧和轨迹的尺度。在VI ORB-SLAM中,除了上述任务外,初始化还要估计出IMU设备的重力方向、加速度和陀螺仪偏差等变量。它的步骤是:
1、先使用最原始的视觉初始化,获得初始地图和一定数量的关键帧;
2、进行IMU传感器的初始化,初始化其重力方向、加速度和陀螺仪偏差、速度,以及整个轨迹对应的尺度。
这个部分的内容放在最后再说。
Tracking
跟踪线程负责计算状态变量:
(
R
,
p
,
v
,
b
)
(R,p,v,b)
(R,p,v,b),即机器位姿、速度和IMU传感器的偏差。计算速率和相机的帧率相同(因为IMU数据的采集速率快,不能给每个数据找到其对应的图像数据。图像数据则刚好相反)。
在估计出状态变量后,对估计值进行优化。所以整个线程的工作流程如下:
1、估计出当前帧的状态变量;
2、将地图中的地图点投影到当前帧上,寻找匹配特征点;
3、计算这些匹配点的重投影误差以及相应帧之间的IMU误差;
4、求得总误差,通过最小化总误差来完成状态变量的优化。
这里需要注意的是:根据地图是否更新(有无新的关键帧产生或检测到闭环),上述流程中第3、4步会有两种不同的计算方法
(a)当地图发生了更新:只有优化当前帧的状态
使用更新后的地图和最后一个关键帧作为优化的参考,它们在优化过程中保持不变。如下图所示:
图中来自最后一个关键帧的变量用下标
i
i
i 表示,来自当前帧的则用下标
j
j
j 表示。重投影误差用蓝色矩形表示,IMU误差则是绿色矩形。红色框表示固定的变量。
在当前情况下,待优化的状态变量和优化方式为:
式中 E p r o j E_{proj} Eproj表示地图点与当前帧产生的重投影误差, E I M U E_{IMU} EIMU表示关键帧与当前帧产生的IMU误差。 E p r o j E_{proj} Eproj的计算公式:
式中
X
C
X_{C}
XC是地图点在相机坐标系下的坐标(计算原理可理解为:
W
p
B
_{W}p_{B}
WpB是IMU在世界坐标系下的坐标,用
X
W
X_{W}
XW(地图点的世界坐标)减去
W
p
B
_{W}p_{B}
WpB就能获得该地图点与
W
p
B
_{W}p_{B}
WpB之间的向量(世界坐标系)。然后对该向量进行旋转平移变换,就能获得地图点在B坐标系下的坐标)。
IMU误差的计算公式为:
式6是在文献[1]中获得的。
个人理解:感觉
e
v
,
e
p
e_{v}, e_{p}
ev,ep的计算和文献[1]中不符,使用的
b
(
.
)
j
b_{(.)}^{j}
b(.)j应该是
b
(
.
)
i
b_{(.)}^{i}
b(.)i。
之后使用非线性优化的方式优化误差。最后将当前帧的优化结果和Hessian矩阵设为一个先验信息,给后续帧做参考。
(b) 地图未更新:同时优化前后两帧,但保持地图点不变
从图中可看出,这个情况下只用地图点保持不变,且优化的对象从一个当前帧增加到前后两帧,对应的待优化状态变量也变成了:
式中
E
p
r
i
o
r
E_{prior}
Eprior是前一帧保留的先验信息产生的先验误差(图3中的小灰色正方形),它的计算公式为:
式中带横线的变量表示前一帧的先验信息,不带横线的表示前一帧在当前优化中的结果。
ρ
ρ
ρ是一类鲁棒核函数。
个人理解:
E
p
r
i
o
r
E_{prior}
Eprior:在当前优化过程中可以调节前一帧的状态变量,但不能使其变化太多(好像可以防止累积误差)
在完成优化后,上一帧就被边缘化(抛弃),而当前帧的优化结果又将作为新的先验信息。这个方法一直持续到地图发生更新或先验信息无效为止。
LocalMapping
VIORB-SLAM和ORB-SLAM的局部建图环节不同的地方在于:前者在进行LocalBA时要多考虑一个IMU的误差项约束。这两个系统在LocalBA上的对比图如下:
相同点:只保留和优化N个关键帧,以及它们观测到的所有地图点。不过其余能够观测到这些地图点的关键帧也会在LocalBA中被使用。它们给地图点提供重投影误差的约束,自身的状态是不变的(在红色框内的部分)。
不同点:VIORB-SLAM要考虑各关键帧之间的IMU约束,以及第N+1个关键帧(红框最右边)对其下一关键帧的IMU约束。
Loop Closing
VIORB-SLAM 和 ORB-SLAM在闭环检测环节上是差不多的。当检测到闭环之后,会对闭环中的关键帧进行位姿图优化(减少计算量),只优化位姿
(
R
,
p
)
(R,p)
(R,p),而不优化IMU变量
(
v
,
b
)
(v,b)
(v,b)。
值得注意的是,位姿图中优化的状态变量只有6个自由度,因为尺度信息可以在IMU初始化时估计出来。这和ORB-SLAM是不同的。
个人理解:因为闭环匹配上的两个关键帧之间很难有IMU的数据约束,所以位姿优化不优化IMU变量
Full BA
使用一个独立的线程进行Full BA,优化所有状态量:位姿,速度和偏差。
IMU初始化
这是整个VI ORB-SLAM的核心部分,用于估计重力方向、尺度(
s
s
s)、速度(
v
v
v)、IMU的偏差(
b
a
,
b
g
b_{a},b_{g}
ba,bg)。整个过程为:
1、估计陀螺仪偏差(
b
g
b_{g}
bg)
2、粗略估计尺度(
s
s
s)和重力向量(
g
W
g_{W}
gW)(不考虑加速度偏差的影响)
3、估计加速度偏差(
b
a
b_{a}
ba),并优化尺度(
s
s
s)和重力方向
4、估计速度(
v
v
v)
需要注意的是任两个相邻的关键帧的时间间隔要短,以减少测量噪声的影响。下面介绍各步骤的具体内容。
估计 b g b_{g} bg
可以通过每两个相邻关键帧之间的旋转测量值来估计陀螺仪偏差:
式中$R_{(.)}$表示由视觉求出的关键帧间的旋转变换,$\Delta R_{i,i+1}$表示通过预积分求出的关键帧间变换关系。优化过程中假设 b g b_{g} bg保持不变,通过最小化上述误差项来估计出 b g b_{g} bg。
估计尺度 s s s和重力向量 g W g_{W} gW(不考虑加速度偏差 b a b_{a} ba)
在求出
b
g
b_{g}
bg之后,可以预积分出速度(
v
v
v)和位置(
p
p
p)。
由于单目SLAM估计的轨迹存在一个尺度因子
s
s
s,所以在轨迹中描述IMU元件与相机之间的变换关系时要考虑
s
s
s,即构建如下变换公式:
将式(10)带入到式(3),同时忽视其中的 b a b_{a} ba后可得公式:
该式子用于求解
s
s
s 和
g
W
g_{W}
gW ,由求解线性方程来实现。为了避免求解速度变量,降低方程复杂度,使用三个连续关键帧之间存在的两个相对位姿关系,以及式子(3)中的速度计算公式,来构建线性方程组:
为了方便,将关键帧 i 、 i + 1 、 i + 2 i、i+1、i+2 i、i+1、i+2写作 1 、 2 、 3 1、2、3 1、2、3。方程组中各项的表达式为:
方程组(12)的形式为 A 3 ( N − 2 ) X 4 x 4 X 1 = B 3 ( N − 2 ) X 1 A_{3(N-2)X4}x_{4X1}=B_{3(N-2)X1} A3(N−2)X4x4X1=B3(N−2)X1。使用SVD求解方程组(12),获得 s ∗ , g W ∗ s^{*}, g_{W}^{*} s∗,gW∗。由于未知量的自由度为 4,所以至少需要有4个关键帧才能求解方程组。
估计加速度偏差( b a b_{a} ba),尺度( s s s)和重力方向
前面获得的
s
∗
,
g
W
∗
s^{*}, g_{W}^{*}
s∗,gW∗未考虑加速度偏差(
b
a
b_{a}
ba),原因是加速度偏差和重力很难区分开==(重力会影响到
b
a
b_{a}
ba,两者相关性较大)==。而重力的方向和加速度偏差区分性很好,所以可以同时估计这两者的量。
首先认为在地图的第一参考惯性坐标系I中,重力的方向为
g
I
^
=
{
0
,
0
,
−
1
}
\hat{g_{I}}=\{0,0,-1\}
gI^={0,0,−1},大小为
G
G
G。而之前计算出来的
g
W
∗
g_{W}^{*}
gW∗的方向为
g
W
^
=
g
W
∗
∥
g
W
∗
∥
\hat{g_{W}}=\frac{g_{W}^{*}}{\left \| g_{W}^{*} \right \|}
gW^=∥gW∗∥gW∗。这时计算
W
、
I
W、I
W、I坐标系之间的旋转变换:
个人理解:之前求出的
g
W
∗
g_{W}^{*}
gW∗是在地图第一参考帧中的重力在世界坐标系下的向量表示,而
g
I
g_{I}
gI是重力方向在第一参考帧中的表示。求出
R
W
I
R_{WI}
RWI后就能够获得所有轨迹与真实世界之间的旋转关系(修正位姿估计值的方向)
此时
g
W
g_{W}
gW可以表示成:
g
W
=
R
W
I
g
I
^
G
g_{W}=R_{WI}\hat{g_{I}}G
gW=RWIgI^G …(15)
式中的
R
W
I
R_{WI}
RWI只包含x、y两个轴上的旋转角度,因为z轴的旋转不会影响到重力的方向。
通过下面这个扰动来优化旋转
R
W
I
R_{WI}
RWI:
式(16)的一阶近似为:
(PS:关于扰动和一阶近似可以参考《视觉SLAM十四讲》P75、P110)将式(17)带入到式(11)中,并考虑 b a b_{a} ba的影响,可得:
仍是使用三个关键帧,得到如下线性方程组:
其中各项的表达式为($\lambda (i)$和方程组(12)中的一样):式中
[
]
(
:
,
1
:
2
)
[]_{(:,1:2)}
[](:,1:2)表示矩阵的前两列
方程组(19)的形式为
A
3
(
N
−
2
)
X
6
x
6
X
1
=
B
3
(
N
−
2
)
X
1
A_{3(N-2)X6}x_{6X1}=B_{3(N-2)X1}
A3(N−2)X6x6X1=B3(N−2)X1。通过SVD的方法求解(19),求出
s
∗
,
δ
θ
x
y
∗
,
b
a
∗
s^{*},\delta \theta _{xy}^{*} , b_{a}^{*}
s∗,δθxy∗,ba∗,以及条件数。由于待求变量的自由度为6,所以至少需要4个关键帧。
PS:条件数可用于检查此问题是否条件良好(如:IMU执行的运动使所有变量均可观察)[2]
速度( v v v)估计
可以通过式(18)求出(用来初始化使用的)关键帧的速度,因为此时 s 、 g W 、 b a 、 b g s、g_{W}、b _{a}、b_{g} s、gW、ba、bg都已知。此外,还可以使用公式(3)求出最近的关键帧的速度。
在重定位后重新初始化 b g , b a b_{g},b_{a} bg,ba
如果系统在长期运行后进行了重定位操作,则使用式(9)重新初始化 b g b_{g} bg。而 b a b_{a} ba则通过式(19)求出,此时 s 、 g W s、g_{W} s、gW都是已知的。此过程使用20个连续的帧来完成,这些帧都是由视觉估计位姿。
结尾
这篇博客是自己对视觉惯性SLAM领域的一个初步探索,里面也加入了一些自己的理解。虽然VI ORB-SLAM的作者没有开源代码,但是网上有前辈根据论文复现了系统(感谢前辈!!)。所以又有事情可做了/(-o-)/~~。
参考资料:
1、https://arxiv.org/pdf/1610.05949.pdf
2、https://blog.csdn.net/myarrow/article/details/54694472(这篇博客写的真不错!!)