ORB-SLAM3 论文笔记
这篇博客
最近ORB-SLAM3全能王横空出世,在看完ORBSLAM-Atlas系统论文后,就直接莽ORB-SLAM3的论文了。关于系统中
I
M
U
IMU
IMU初始化的相关论文之后在慢慢理解(概率好头疼),剩下的就直接莽了!
【转载声明】本篇文字均为原创,如转载请注明出处
ORB-SLAM3系统
ORB-SLAM3是基于 VI ORB-SLAM 和 ORBSLAM-Atlas 两个系统(点击可查看我写的关于这两个系统论文的笔记)实现的,它相当于是这两个系统的融合升级版,是在视觉、视觉+惯性SLAM中的全能王。下图是ORB-SLAM3的流程框图:
这里大致介绍一下各个线程的工作内容:
a) Atlas:Atlas相当于一个子系统,它保存了许多ORB-SLAM3所创建的子地图。所有子地图可以分成两类:Active Map(跟踪定位所使用的地图),Non-active Map(之前保留下来的地图)。
b) Tracking:处理传感器的输入信息,通过Active Map来实时地对机器进行跟踪定位,同时还会决定是否创建新的关键帧 KF 。如果跟踪丢失,跟踪线程会先在 Atlas 的所有地图上进行重定位:重定位成功,则继续跟踪;否则将当前 Active Map 保存为 Non-active Map ,并重新创建地图。
c) Local Mapping:维护 Active Map 中的一小部分地图,可以帮助提高跟踪定位的精度。如果是惯性系统,则该线程还承担着 IMU 初始化的任务。
d) Loop and Map Merging:检测地图内或地图之间存在的闭环,根据闭环对地图进行修正,降低系统累积误差。
通过整个工作流程图以及各线程的功能,我们可以看出,相比于普通的视觉SLAM、视觉惯性SLAM(这里可用ORB-SLAM2和VI ORB-SLAM作为对比对象),ORB-SLAM3主要有以下几个创新点:
(1)能够进行单目、双目的视觉、视觉+惯性跟踪定位,使用最大后验估计(MAP)方法计算位姿。同时在IMU初始化方面也采用了一种精度、效率更好的新方法;
(2)使用一种新的位置识别方法,来完成重定位和闭环检测操作。这个方法耗时更少,且在保证100%准确率的同时,能够提高闭环的召回率;
(3)加入了 ORBSLAM-Atlas 系统的多地图特性,并使用(2)中的方法完成地图的位置识别+闭环检测;
(4)系统对相机模型进行了抽象表达,把系统中与相机模型有关的所有特性、函数(如投影、反投影函数等)都提取出来,组成一个独立的相机类。这样系统就能适用于不同的相机模型。
相机模型的抽象(Camera Model)
为了让ORB-SLAM3能适应于不同类型相机,这里将相机模型从系统中独立了出来,构建抽象类对象(代码实现是这样的)。用户可通过类继承的方法,创建想要的相机模型类对象。之后就能在ORB-SLAM3代码里生成相应实例并使用。论文里有针孔模型和鱼眼相机模型。这种作法会导致系统使用的相机模型不再是确定的,因此在算法等方面会存在一些问题。
重定位的问题
在重定位方面,ORB-SLAM 是通过 EPnP 算法来计算位姿的。而 EPnP 算法的公式是基于针孔相机模型的。如果系统使用的是鱼眼相机那么 EPnP 就失效了。因此系统需要使用一个不依赖某一个相机模型的PnP算法,即最大似然 PnP(MLPnP) 。
图片矫正的问题
在多数双目SLAM中,都假设双目图片能够矫正好,然后就能通过极线搜索的方法轻松地找到图片间的匹配特征。但这种假设具有较大局限性,且图片矫正在某些时候并不实用,比如矫正鱼眼相机的图片要去掉其中大部分区域。这就丢掉了很多环境信息,会降低系统的鲁棒性。所以ORB-SLAM3不依靠图片矫正,而将双目看作是两个满足下述条件的单目相机:
1)两个单目相机间存在一个固定的相对位姿变换矩阵。
2)(可选)两个相机间存在共视区域(当两个相机一前一后时就没有共视了)。
由于不依靠矫正后的图片,所以 ORB-SLAM3 采用的是暴力匹配的方式,在全局使用knn搜索为两个相机寻找匹配特征。
在跟踪和建图时,使用一个单目相机作为视觉上的参考坐标系,另一个则用来帮助构建地图点。可以通过三角化构建两相机间的共视部分中特征点对应的地图点。而共视部分以外的图片区域则按照单目相机图像的方式来处理(单目建图)。
视觉惯性SLAM的工作原理
相关公式
在视觉惯性SLAM中的状态变量有(用下标C(Camera)表示相机坐标系,下标B(Body)表示惯性元件坐标系):
1)
T
i
=
[
R
i
,
p
i
]
∈
S
E
(
3
)
T_{i}=[R_{i}, p_{i}] \in SE(3)
Ti=[Ri,pi]∈SE(3):第 i 时刻Body的位姿;
2)
v
i
v_{i}
vi:每帧的速度(在地图中第一参考坐标系下表示);
3)
b
i
g
b_{i}^{g}
big:第 i 时刻 IMU 元件的陀螺仪偏差;
4)
b
i
a
b_{i}^{a}
bia:第 i 时刻 IMU 元件的加速度偏差;
将这些变量整合起来,用一个向量 S 来公式 (1)表示:
其中
Δ
R
i
,
i
+
1
,
Δ
v
i
,
i
+
1
,
Δ
p
i
,
i
+
1
ΔR_{i,i+1}, Δv_{i,i+1}, Δp_{i,i+1}
ΔRi,i+1,Δvi,i+1,Δpi,i+1 是两帧间 IMU 测量值预积分后的结果。
是所有测量向量的信息矩阵。
R
i
,
p
i
R_{i}, p_{i}
Ri,pi 是系统估计(视觉估计)出来的结果,
v
i
v_{i}
vi 由 IMU 的速度计算公式求出。
视觉误差就是重投影误差,如公式 (3)所示:
其中
r
i
j
r_{ij}
rij 是第 j 个地图点到第 i 帧上的重投影误差。Π 是相应的相机投影模型,
u
i
j
u_{ij}
uij是地图点对应的像素坐标,它具有一个信息矩阵
Σ
i
j
Σ_{ij}
Σij。
T
C
B
T_{CB}
TCB 表示Body–>Camera坐标系的变换矩阵,可以提前标定。
将公式(2)和公式(3)的误差联合起来,就是系统所优化的总误差,如公式 (4)所示:
其中
K
j
K^{j}
Kj 是能够观测到第 j 个地图点的所有 KFs 。
ρ
H
u
b
ρ_{Hub}
ρHub 是鲁棒核函数。公式 (4) 的优化问题可由下图表示。
(图中的展示的优化方法在跟踪和建图过程中,会依据情况做出相应调整)
IMU初始化
因为有些IMU的参数:速度
v
v
v、加速度偏差
b
a
b^{a}
ba、陀螺仪偏差
b
g
b^{g}
bg、重力方向
g
g
g 是未知的,所以要通过初始化将它们估计出来。如果用的是单目相机,还需要估计出尺度
s
s
s。
论文用的初始化方法速度更快、精度更高。由于考虑了传感器的不确定性,所以将初始化当作一个最大后验估计(MAP)问题来求解。以单目系统为例,初始化的步骤为:
1)Vision-only MAP Estimation(这里和普通单目初始化一样):将系统以 4Hz 插入 KF 的频率运行 2s ,构建一个尺度不确定的视觉地图。该地图内应含有 10 个 KFs 和几百个地图点。然后通过Visual-Only BA 来优化各帧的位姿(如图 2b 所示)。通过各帧位姿,获得对应的 Body 位姿
T
ˉ
0
:
k
=
[
R
,
p
ˉ
]
\bar{T}_{0:k}=[R, \bar{p}]
Tˉ0:k=[R,pˉ]。
2)Inertial-only MAP Estimation:通过
T
ˉ
0
:
k
\bar{T}_{0:k}
Tˉ0:k和 KFs 间 IMU 预积分的值,来估计 IMU 的参数和尺度。将这些待估计变量用下式表示:
其中
s
s
s 是地图的尺度。
R
w
g
∈
S
O
(
3
)
R_{wg}\in SO(3)
Rwg∈SO(3) 是重力的旋转矩阵,它表示的是地图中第一参考坐标系到真实世界坐标系的旋转关系。
b
b
b 则是加速度和陀螺仪偏差(在初始化过程中被认为是定值)。
v
ˉ
0
:
k
\bar{v}_{0:k}
vˉ0:k 是基于尺度的速度。
(下面部分涉及到论文:Inertial-Only Optimization for Visual-Inertial Initialization)
此时,如果只考虑相邻两帧间的惯性测量值:
那么,根据就能建立以下 MAP 估计问题:
考虑到每个测量值的独立性,MAP 问题可以写成 公式 (7):
再进行一些假设,就能获得最终的优化问题,如公式 (8) 所示:
公式中不包含视觉误差,仅优化惯性误差。
r
p
r_{p}
rp 表示先验误差。这个优化过程如 图2c 所示:
其中关于
R
w
g
,
s
R_{wg}, s
Rwg,s 两个待估计参数的更新公式如下:
优化完后,地图的尺度就确定了。之后通过估计的重力方向,将地图中所有元素变换到真实世界坐标系下表示。
3)Visual-Inertial MAP Estimation:完成上述两步后,同时优化视觉误差和惯性误差。这一步和图 2c 很相似,但要保证各KFs都有相同的偏差
b
a
,
b
g
b^{a}, b^{g}
ba,bg,以及和第二步具有一样的先验信息。
通过上述步骤就能仅根据 2s 内产生的轨迹,获得尺度误差为 5% 的估计结果。在初始化后的 5、15s 会各执行一次视觉 + 惯性 BA(代码中类似于又执行了两次初始化,但稍微有些不同)。这样可将尺度误差降低到 1% 。完成上述所有操作后,就认为地图已经成熟。
双目情况下的初始化和上述类似,只需要将尺度
s
s
s 固定为 1,且不对它进行优化。
跟踪和建图
跟踪定位当前帧可以采用两种方法:通过运动模型跟踪(相邻两帧间的运动矩阵已知)和通过当前参考 KF 跟踪。在完成跟踪之后,需要进行位姿的优化,其方法和VI ORB-SLAM类似(单目系统是一样的优化方法,而双目系统需要考虑地图点是在哪一个相机上成像的。如果在右边相机,那在构建g2o误差边时要考虑两相机间的相对变换矩阵。关于VI ORB-SLAM可以参考这篇博客)。优化过程中只优化两帧的状态变量,且保持地图点不变。但当地图刚更新完毕时,优化方法会有所改变,具体可见VI ORB-SLAM中相关部分。
建图环节的优化主要考虑局部地图优化,优化局部地图中的 KFs 和地图点,其他的共视 KFs (局部地图以外)可以提供约束(重投影误差),但不会被优化(这里和代码实现不一样,也许是自己理解没有到位。代码中共视 KFs 的位姿也会被优化。)
在某些特殊情况下,比如机器移动很慢,IMU参数的可观测性较差。此时使用之前的初始化方法无法在15s内获得精度较好的结果。所以需要采用一个新的尺度提炼方法来预防这种特殊情况。这个方法是 Inertial-only 优化的修改版,它只优化尺度和重力方向。此时不再认为各 KFs 的偏差相同。该优化方法如下图所示(即图 2d ):
这个尺度提炼的方法将在 Local Mapping 线程中每隔10s执行一次,直到地图中已有100个以上KFs或者在初始之后系统已经运行了75s。
系统对跟踪丢失的应对
一般的视觉SLAM,如ORB-SLAM2在跟踪丢失时会直接进行重定位操作。此时地图停止更新,直到重定位完成。而ORB-SLAM3有两种方法来应对跟踪丢失的情况:
1)在视觉上发生跟踪丢失时,则只使用IMU进行位姿估计。根据估计的位姿,将地图点投影到当前图像上以寻找匹配点。然后通过视觉+惯性优化获得最终位姿结果。
2)如果通过上面方法在5s内无法重新定位,则重新构建地图。当前使用的地图根据情况决定是否保存。
(如果当前地图中信息较多( KFs 数量多)则认为其比较重要,将其保留;否则直接残忍抛弃)
多地图的闭环检测和地图融合
由于系统具有多地图特性,所以会构建出多个子地图。为了更有效地检测地图内和地图之间的闭环,ORB-SLAM3使用一种新的位置识别方法和闭环融合方法。
位置识别
将跟踪时正在使用的地图称为Active Map,其余子地图为Non-active Map。在Active Map中的关键帧记为
K
a
K_{a}
Ka 。整个位置识别的大致步骤为:
1)为当前关键帧
K
a
K_{a}
Ka ,找出三个候选者
K
m
K_{m}
Km(不包括之前就与
K
a
K_{a}
Ka 有共视关系的 KFs );
2)为每个
K
m
K_{m}
Km 构建一个局部窗口,其中包含了
K
m
K_{m}
Km 和与之共视程度较高的一些 KFs ,以及它们观测到的所有地图点;
3)先进行几何一致性检测。通过
R
A
N
S
A
C
RANSAC
RANSAC 算法,估计
K
a
K_{a}
Ka 和
K
m
K_{m}
Km 间适应性最高的变换矩阵
T
a
m
T_{am}
Tam;
4)将
K
m
K_{m}
Km 的局部窗口中所有地图点通过
T
a
w
=
T
a
m
T
m
w
T_{aw}=T_{am}T_{mw}
Taw=TamTmw投影到
K
a
K_{a}
Ka 中,寻找其匹配点。反过来也一样,可以把
K
a
K_{a}
Ka 观测到的地图点投影到
K
m
K_{m}
Km 的局部窗口中的KFs上。根据这些匹配关系,通过非线性优化来提高
T
a
m
T_{am}
Tam 的精度。
5)在 Active Map 中找 3 个和
K
a
K_{a}
Ka 共视程度大于一定阈值的 KFs(与
K
a
K_{a}
Ka观测到相同的局部地图中的地图点的数量较多) 。如果没找到,就等待在
K
a
K_{a}
Ka 后面的两个连续KFs**(不需要对这两个KFs进行DBoW2方式的位置识别,所以能节约时间)**。此时用
K
a
K_{a}
Ka 和其他 3 个KFs来验证
T
a
m
T_{am}
Tam。(验证方法应该就是看这两个KFs在经过
T
a
m
T_{am}
Tam变换后能否看到一定数量的地图点(地图点在
K
m
K_{m}
Km 的局部地图中))
6)基于重力方向的验证。在视觉惯性系统中,如果地图已初始化完毕,那么
T
a
m
∈
S
E
(
3
)
T_{am}\in SE(3)
Tam∈SE(3)。检查
T
a
m
T_{am}
Tam中
p
i
t
c
h
、
r
o
l
l
pitch、roll
pitch、roll 方向的旋转角度是否低于一定阈值。低于则认为这次位置识别成功。
(对第(6)条的个人理解:如果 K a K_{a} Ka 和 K m K_{m} Km形成闭环,那么他们的重力方向应该是差不多的,因为他们只有在 p i t c h 、 r o l l pitch、roll pitch、roll 方向上差不多的位置观测同一区域时,才会形成闭环)
视觉地图融合方法
将系统正在使用的地图定义为Active map,记为
M
a
M_{a}
Ma,其中的 KF 记为
K
a
K_{a}
Ka;其余子地图记为Non-active map,记为
M
m
M_{m}
Mm;与
K
a
K_{a}
Ka匹配的闭环帧记为
K
m
K_{m}
Km;
K
a
K_{a}
Ka和
K
m
K_{m}
Km间相对变换矩阵为
T
m
a
T_{ma}
Tma。
1)用
K
a
K_{a}
Ka+
K
m
K_{m}
Km+他们俩的共视图(系统默认
K
a
K_{a}
Ka选取 15个,
K
m
K_{m}
Km也一样)和观察到的地图点来构建接合窗口。构建前,先将属于
M
a
M_{a}
Ma的元素通过
T
m
a
T_{ma}
Tma变换到
M
m
M_{m}
Mm 地图坐标系中。
2)第一步后,Ma和Mm就完成了融合,一起作为新的Active map。此时会有重复的地图点产生,它们是
M
a
M_{a}
Ma与
M
m
M_{m}
Mm间的匹配点。对它们进行融合:把在
M
a
M_{a}
Ma地图中的地图点信息(它的观测KFs和描述子之类的)转移到与其匹配的
M
m
Mm
Mm中的地图点上。最后再更新融合后地图的共视图和本质图。
3)对融合地图中的接合窗口部分进行局部 BA 优化。其中,对于
M
m
M_{m}
Mm地图中剩余的KFs,只要能观测到接合窗口中的地图点,就能参与局部 BA 优化。但它们只是提供约束,不会被优化。这个过程如下面的 图(a) 所示:
4)以优化好的接合窗口为基础(固定其中KFs),通过位姿图优化来优化融合地图中剩余的 KFs 。
PS:融合后的地图直接用于跟踪定位,成为新的Active Map。在融合之前的Active Map中所使用的Local Window(跟踪线程使用的局部窗口),在融合之后的地图中继续使用(仍然是那些 KFs 构建局部窗口)。
视觉+惯性地图的融合方法
这里和视觉地图融合的方法相似,只是原来的第(1)和(3)步做了修改(接合窗口中KFs的数量和选取方法变了):
1)(与上面的第(1)步相比较)如果当前地图成熟了(尺度、重力方向估计优化完成),
T
m
a
∈
S
E
(
3
)
T_{ma} \in SE(3)
Tma∈SE(3),否则
T
m
a
∈
S
i
m
(
3
)
T_{ma} \in Sim(3)
Tma∈Sim(3)。之后的操作和之前一样。
2)视觉惯性接合窗口的局部 BA 优化:将
K
a
+
K
a
K_{a}+K_{a}
Ka+Ka前面的5个 KFs记为
K
a
K_{a}
Ka 的局部窗口、
K
m
+
K
m
K_{m}+K_{m}
Km+Km附近5个 KFs 记为
K
m
K_{m}
Km 的局部窗口。将在
K
m
K_{m}
Km 局部窗口之前的 KF 记为
K
b
m
K_{bm}
Kbm,同理
K
a
K_{a}
Ka 那边则记为
K
b
a
K_{ba}
Kba 。找出上面 KFs 观测到的地图点,记为
M
p
M_{p}
Mp,并在
M
a
M_{a}
Ma 和
M
m
M_{m}
Mm 地图中找到这些地图点其余的观测帧 KFs,记为
K
c
K_{c}
Kc 。
最后将上述所有 KFs 和地图点都包含到优化算法中,其中各部分的优化情况为:
1)
K
a
K_{a}
Ka局部窗口中的 KFs 的所有状态变量都会优化;
2)
K
m
K_{m}
Km局部窗口中的 KFs 的所有状态变量都会优化;
3)
K
c
K_{c}
Kc中的 KFs 只会优化位姿状态变量会被优化;
4)
M
p
M_{p}
Mp中的所有地图点都优化;
5)
K
b
a
K_{ba}
Kba只优化其位姿状态变量,其余状态变量保持不变;
6)
K
b
m
K_{bm}
Kbm提供约束,但不会被优化。
这个优化过程的示意图如下:
疑问:在查看代码时,代码中进行视觉惯性地图融合的函数应该是 MergeLocal2()
,但是该函数是将
M
m
M_{m}
Mm 地图中的元素变换到了
M
a
M_{a}
Ma 中,而且最后只完成了接合窗口部分优化,而没有对融合地图中剩余部分进行全局位姿优化。看来仍需要继续深入学习。
单个地图中的闭环融合
这里形成闭环的两个KFs都属于Active map。处理方式和上面的地图融合是相似的,即:
在这之后也会进行一次全局的BA优化(优化地图中所有KFs),以获得 MAP 估计结果。如果是视觉地图,最后的全局 BA 优化一定会使用;如果是视觉惯性地图,只有当地图中的 KFs 数量较少时 (代码中默认为200以内) 才会执行最后一步(为了防止计算量过大)。
结尾
上面是自己结合代码对 ORB-SLAM3 论文的初步理解 ,如有错误,请大家指出,谢谢!之后有新的思路会对这篇博客进行更新(路漫漫其修远兮,吾将上下而求索!)。
参考资料:
1、ORB_SLAM3和之前版本有什么不同?
2、ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM