这个系统从测量预处理开始。VIO;重定位;和姿态图优化这三个模块以多线程的方式同时运行。
(
⋅
)
w
(cdot)^w
(⋅)w;世界坐标系。重力方向和世界坐标系的z轴平行。
(
⋅
)
b
(cdot)^b
(⋅)b: 物体坐标系。VINS定义该坐标系和IMU坐标系相同。
(
⋅
)
c
(cdot)^c
(⋅)c: 相机坐标系。
VINS同时使用旋转矩阵
R
R
R和四元数
q
q
q来表示旋转;在状态向量中主要使用四元数。
q
b
w
q_b^w
qbw: body坐标系到世界坐标系的旋转。
p
b
w
p_b^w
pbw: body坐标系到世界坐标系的平移。
b
k
b_k
bk: 拍摄第
k
k
k帧图像时的body坐标系。
c
k
c_k
ck: 拍摄第
k
k
k帧图像时的相机坐标系。
⊗
otimes
⊗: 两个四元数之间做乘法。
g
w
=
[
0
,
0
,
g
]
T
g_w=[0,0,g]^T
gw=[0,0,g]T: 世界坐标系下的重力向量。
(
⋅
)
^
hat{(cdot)}
(⋅)^: 表示带噪声的测量结果;或者对一个确定量的估计值。
初始化过程提供所有必需值。包括姿态;速度;重力向量;偏差;3D特征位置。用来启动随后基于非线性优化的VIO。
测量预处理模块负责特征的提取和跟踪;相邻两帧之间的IMU测量预积分。
该模块融合IMU预积分结果;特征观测;以及来自回环检测模块的重检测特征。
执行全局优化;去除漂移。
