【VSLAM系列】三：Vins-Mono论文笔记
VINs-Mono论文
1. VINS-Mono的特点：
- 1.未知初始状态的鲁棒性初始化过程
- 2.带imu-camera外参校准和imu校准的紧耦合，基于非线性优化的单目VIO系统
- 3.在线重定位和四个自由度的全局姿态图优化。
- 4.姿态图可以保存，加载，并和局部姿态图进行合并。
2. 传感器数据处理
摄像头和imu数据融合方法：
- 1.松耦合法，imu是独立于摄像头的模块，常使用EKF算法，imu数据此时用于状态传播，使用纯视觉位姿进行更新。
- 2.紧耦合，基于EKF或者优化的方式，此时imu数据和摄像头数据在原始数据层面一起进行优化。
摄像头数据处理
- 直接法：残差为光度误差，需要良好的初始值，在建立稠密地图上具有天然的优势。
- 间接法：残差为几何距离误差（即重投影误差），在计算和匹配特征点上需要耗费额外的时间，但工程上来说其技术成熟，鲁棒性好。建稠密地图比较麻烦
IMU数据处理
imu预积分
3. VINS-Mono系统框架
1.测量处理线程(measurement preprocessing (Section IV))：图像特征提取和光流追踪，在两个图像时间戳间的imu数据被进行预积分。
2.初始化线程（The initialization procedure (Section V)）：提供姿态（相机和imu）,速度（imu坐标系的速度），重力向量，陀螺仪偏置，特征点的三维坐标。这些值用于引导后续VIO的非线性优化。
3.视觉惯性里程计（滑动窗口内非线性优化）The VIO ( Section VI) with relocalization (see Section VII)：对特征观测和imu预积分值进行紧耦合优化，该模块带重定位功能。
4.姿态图优化模块（the pose graph optimization module (Section VIII)）：利用上面的重定位结果，进行姿态图优化，以消除漂移。
4. 测量预处理
1） 视觉数据处理
使用KLT稀疏光流追踪法对最新的图像进行光流追踪；该检测器通过设置两个相邻特征之间像素的最小间隔使特征均匀分布；保证图像特征的最小数目(100-300)
关键帧的判断：a)视差判断为主，如果追踪的特征点间视差大于某一个阈值，则认为是新的关键帧。为解决纯旋转问题，使用了短期imu预积分数据来补偿旋转。
b)为保证追踪的连续性，如果某一帧种追踪的特征点数少于一个阈值，则认为其是关键帧。
两帧的光流追踪优于描述子匹配，但是在遮挡和重定位的时候描述子匹配优
2）IMU预积分
IMU模型，imu测量结果都是自己坐标系
a ^ t = a t + b a t + R w t g w + n a ω ^ t = ω t + b g t + n w n a ∼ N ( 0 , σ a 2 ) , n w ∼ N ( 0 , σ w 2 ) â _t = a_t + b_{a t} + R^ t_w g^ w + n_ a\\ ω̂ _t = ω_ t + b _{gt} + n_ w\\ n_a∼N(0,σ_a^2), n_w∼N(0,σ _w^2) a^t=at+bat+Rwtgw+naω^t=ωt+bgt+nwna∼N(0,σa2),nw∼N(0,σw2)
偏置纠正：当偏置优化结果变化不大时，利用一阶泰勒展开对预积分量进行更新，（在优化过程中，bias会被更新，那么预积分量也要更新，用一阶近似直接在原来积分量上加数据而不是重新计算积分）
V5. 初始化
采用松耦合的方法得到初始值。纯视觉SLAM或者SfM，具有良好的初始化性质，多数情况下，纯视觉系统可以通过从相对运动方法中导出初始值。通过对齐IMU预积分与纯视觉SfM结果，粗略地恢复尺度、重力、速度，偏置。
外参估计
每来一帧做一次两两帧之间的估计
单目视觉运动估计：
找到合适的两帧图像，对应追踪的特征点不少于20个，特征点间位移大于30像素
对极几何五点法计算两帧相机平移与旋转
设置尺度，三角化所有路标点，使用pnp计算整个滑动窗口内所有帧的姿态
最小化重投影误差BA优化所有姿态与路标点（第一帧为参考帧）
相机坐标系转化到IMU坐标系
陀螺仪bais校准
使用imu预积分值，使纯视觉中的尺度信息得以和imu测量相匹配，估计外参
已求解滑动窗口中所有帧的姿态，从imu预积分中获得连续两帧的相对旋转。最小化如下代价函数：
相对旋转的更新使用第二个函数，可以得到一个初始化的偏置的校正值
速度、重力向量、尺度的初始化
通过最小化上面这个代价函数，可以获得滑动窗口中每一帧的速度。相对于相机坐标系的重力向量和尺度值。
重力方向优化，使用两自由度表示重力
δ g = w 1 b 1 + w 2 b 2 δg = w _1 b_ 1 + w_ 2 b _2 δg=w1b1+w2b2
将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转将所有变量从参考坐标系(·)c0 旋转到世界坐标系(·)w。
6. 紧耦合VIO 滑动窗口
状态变量与残差函数
X = [ x 0 , x 1 , . . . x n , x c b , λ 0 , λ 1 , . . . λ m ] x k = [ p b k w , v b k w , q b k w , b a , b g ] , k ∈ [ 0 , n ] x c b = [ p c b , q c b ] X = [x_0 , x_1 , . . . x _n , x^ b_c , λ _0 , λ_ 1 , . . . λ _m]\\ x _k =[p^w_{b_k} , v^w_{b_k} , q^w_{b_k} , b_a , b_g], k ∈ [0, n]\\ x^b_c =[p^b_c , q^b_c] X=[x0,x1,...xn,xcb,λ0,λ1,...λm]xk=[pbkw,vbkw,qbkw,ba,bg],k∈[0,n]xcb=[pcb,qcb]
m i n ( X ) { ∣ ∣ r p − H p X ∣ ∣ 2 + ∑ k ∈ B r B ( z ^ b k + 1 b k , X ) ∣ ∣ p b k + 1 b k 2 + ∑ ( l , j ) ∈ C ρ ( r C ( z ^ l c j , X ) ∣ ∣ P l c j 2 ) } min(X)\{||r_p − H_p X||^2+ \sum\limits_{k ∈B}^{}r_B (ẑ^{b_k}_{b_{k + 1}} , X )||^2_{p^{b_k}_{b_{k + 1}}} +\sum\limits_{(l,j )∈C}^{}ρ({ r_C (ẑ_l^{c_j} , X )||^2_{P^{c_j}_l}) } \} min(X){∣∣rp−HpX∣∣2+k∈B∑rB(z^bk+1bk,X)∣∣pbk+1bk2+(l,j)∈C∑ρ(rC(z^lcj,X)∣∣Plcj2)}
残差函数 = 先验 + IMU残差 + 视觉测量残差（球面相机模型）
IMU残差：基于状态量计算得到的相对位移，速度，旋转和imu预积分量的差值
视觉残差：观测点单位向量与路标单位向量相减，向量差在单位圆上一切平面分解
边缘化
如果第二新的帧是关键帧，则把滑动窗口中最旧的关键帧边缘化掉，如果第二新的帧不是关键帧，则把该帧对应的特征点舍弃掉（非关键帧的测量值不是所有的都会被边缘化掉），但保留其imu测量值。
备注：边缘化过程中残差的雅可比可能不是在同一点得到的，这是边缘化一直存在的问题，对于该问题VINS-Mono中认为漂移本身就存在，后续通过回环也可以来进行消除
执行边缘化过程中，我们需要不断迭代计算H矩阵和残差b，而迭代过程中，状态变量会被不断更新，计算边缘化相关的雅克比时需要注意固定线性化点。也就是计算雅克比时求导变量的值要固定，而不是用每次迭代更新以后的x去求雅克比，这就是FEJ（First Estimate Jacobians）。也被称为系统的一致性问题。
针对相机实时帧率的纯运动视觉惯性状态估计器
对于计算能力有限的平台，紧耦合单目VIO由于非线性优化沉重的计算量，在这些平台是难以达到相机实时帧率更新。为此，除了完全优化之外，VINS-Mono还采用了轻量级运动视觉惯性优化，将状态估计频率提高到相机率(30Hz)
纯运动状态估计并没有优化滑动窗口中的所有状态，而只是优化了固定数量的最新IMU状态的姿态和速度。将特征深度、外部参数、偏差和不想优化的旧IMU状态视为常数值。
故障检测与恢复
故障检测：
1、在最新帧中跟踪的特征数小于某一阈值；
2、最近两个估计器输出之间的位置或旋转有较大的不连续性；
3、偏置或外部参数估计有较大的变化；
一旦检测到故障，系统将切换回初始化阶段。一旦单目VIO被成功初始化，将新建一个独立的位姿图。
7. 重定位
四自由度drift 三维位置(x,y,z)和围绕重力方向的旋转(yaw)
1 利用DBoW2进行回环检测，除了用于单目VIO的角点特征外，生成关键帧的时候另外提取500个fast角点并由BRIEF描述子描述。
2 特征恢复，当检测到回环时，通过检索特征对应关系建立局部滑动窗口与回环候选帧之间的连接。
几何异常值剔除：1)、2D-2D：RANSAC的基本矩阵检验，利用当前图像中检索到的特征的二维观测和回环候选图像进行基础矩阵检验。2)、3D-2D：RANSAC的PNP检验，基于特征在局部滑动窗口中已知的三维位置，以及回环候选图像中的二维观测，进行PNP检验。当内点超过一定阈值时，我们将该候选帧视为正确的回环检测并执行重定位
4 紧耦合重定位 ，在重定位过程中，将所有回环帧的位姿作为常量利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口
残差函数中添加回环项
∑ ( l , j ) ∈ C ρ ( r C ( z ^ l v , X , q ^ v w , p ^ v w ) ∣ ∣ P l c j 2 ) \sum\limits_{(l,j )∈C}^{}ρ({ r_C (ẑ_l^{v} , X,q̂^w_v , p̂_v ^w)||^2_{P^{c_j}_l}) } (l,j)∈C∑ρ(rC(z^lv,X,q^vw,p^vw)∣∣Plcj2)
其中L是回环帧中检索到的特征的观测集，(l,v)是指在回环帧v中观察到的第l个特征
8. 全局位姿图优化
在重定位后，为保证过去的姿态能被正确加载到全局地图中，进行额外的位姿图优化
在位姿图中添加关键帧
当关键帧从滑动窗口被边缘化时，将被添加到位姿图中，关键帧在位姿图中作为顶点，它通过两种类型的边与其他顶点连接：
1）顺序边(Sequential Edge)：关键帧将建立与之前关键帧的若干顺序边。顺序边表示局部滑动窗口中两个关键帧之间的相对转换，其值直接从VIO中获取，顺序边只包含相对位置和偏航角。
2）回环边(Loop Closure Edge)：如果新边缘化的关键帧有一个回环连接，它将与回环帧通过一个回环边在位姿图中连接四自由度相对位姿变换，回环边的值由重定位结果得出。
四自由度位姿图优化
帧i和j之间边的残差，φ*i*，*θ*i是直接从单目VIO中得到的翻滚角和俯仰角的估计
最小化代价函数（顺序边 + 回环边）
m i n ( p , ψ ) { ∑ ( i , j ) ∈ S ∣ ∣ r i , j ∣ ∣ 2 + ∑ ( i , j ) ∈ L ρ ( ∣ ∣ r i , j ∣ ∣ 2 ) } min(p,\psi)\{\sum\limits_{(i,j)∈S}^{}||r_{ i,j}||^ 2 + \sum\limits_{(i,j)∈L}^{}ρ(||r_{i,j}||^ 2 )\} min(p,ψ){(i,j)∈S∑∣∣ri,j∣∣2+(i,j)∈L∑ρ(∣∣ri,j∣∣2)}
其中S是所有顺序边的集合，L是回环边的集合。尽管紧耦合的重定位已经有助于消除错误的回环，但还是添加另一个Huber范数 ρ(·)，以进一步减少任何可能的错误回环的影响。相反不对顺序边使用任何鲁棒范数，因为这些边是从VIO中提取出来的，VIO已经包含了足够多的外点排除机制。
位姿图管理
姿态图不仅可以优化当前地图，还可以将当前地图与以前构建的地图合并，加载了以前构建的姿态图，并检测到两个姿态图之间的闭环关系进行合并。
保存：只需要保存顶点和边，以及每个关键帧的词典查询向量
[ i , p ^ i w , q ^ i w , v , p ^ i v i , ψ ^ i v , D ( u , v , d e s ) ] [i, p̂^w_i , q̂^w_i , v, p̂^i_{iv} , ψ̂ _{iv} , D(u, v, des)] [i,p^iw,q^iw,v,p^ivi,ψ^iv,D(u,v,des)]
其中i为帧索引，p i 和q i w 分别为VIO的位置和方向。如果此帧有一个闭环帧，则v是闭环帧的索引。p i 和ψ ·i 是这两帧之间的相对位置和偏航角，通过重新定位得到。D（u、v、des）是关键帧的可视化查询向量，每个特性都包含二维位置及其简短的描述子
实行一个下采样过程，将位姿图数据库保持在有限的大小。所有具有回环约束的关键帧都将被保留，而其他与相邻帧过近或方向非常相似的关键帧可能会被删除。关键帧被移除的概率和其相邻帧的空间密度成正比。
V9. 实验
总轨迹超过700米，持续约10分，VINS-Mono无环闭路的最终漂移为[-5.47,2.76,-0.29]m，占整个轨迹长度的0.88%，经回环修正，最终漂移上界为[-0.032,0.09,-0.07]m
总轨迹长度为61.97 m。最终漂移为[0.08，0.09，0.13]m，为0.29%的位置漂移