论文分享-VINS论文解读_vins解析-CSDN博客

本文链接：https://blog.csdn.net/zz_0000000/article/details/132282808

概述

VINS论文中主要讲解的就是视觉与IMU之间的耦合，使用了怎样的方法将两种传感器数据融合一起，实现了一个完整且易使用的slam系统。VINS代码的整体流程如图：

在这里插入图片描述

图中红色字体代表着论文中的不同部分：

Measurement Preprocessing测量预处理：对于视觉测量来说，VINS中的相机数据是如何识别跟踪特征点；对于IMU测量需要对两个连续帧之间的测量进行预积分
Initialization初始化：在初始过程中如何通过视觉和IMU提供必要的姿态、速度、重力向量、偏置等基础状态量
VIO with Relocalization紧耦合VIO：视觉和IMU使用紧耦合的方式在重定位和后端非线性优化过程中的应用
Global Pose Graph Optimization全局位姿图优化：使用了4自由度的位姿图优化全局

测量预处理

由两个部分组成：视觉信息和IMU信息。

视觉测量

对于每一帧图像，使用了KLT稀疏光流算法对特征点进行跟踪，并且设置了特征像素点之间的最小像素阈值，使得特征点均匀化，利用基本矩阵模型的RANSAC算法进行外点的剔除

对于关键帧的选取主要有两个标准：

⏩ 如果跟踪的所有特征在当前帧和上一帧之间大额平均视差，超过了某一个阈值，那么这个当前帧就作为新的关键帧处理（视差太大了，可能就跟丢了）
⏩ 如果跟踪的特征数量低于某一个阈值，就将当前帧建立为新的关键帧

IMU预积分

暂时不考虑预积分，对于原始的陀螺仪和加速度计的观测数据是：

在这里插入图片描述

即 $\hat{a_t}$ 是加速度计的观测值（包括了 $a_{t}$ 真实值、 $b_{a_{t}}$ 加速度计偏置、 $g^w$ 重力向量和 $n_a$ 噪声）， $\hat{w_{t}}$ 是陀螺仪的观测值（包括了 $w_t$ 真实值、 $b_{w_{t}}$ 陀螺仪偏置和 $n_w$ 噪声），而对于相邻k、k+1两帧，如果直接使用IMU在帧k的位姿上进行积分，就会求得k+1的位姿（这里都是相对于世界坐标系）：

在这里插入图片描述

上面公式就是将IMU积分直接添加到视觉图像帧中的做法，但是上面公式中计算的时候需要依赖第k帧的 $v$ 和 $R$ ，当使用非线性优化时，每次迭代更新这个第k帧的数据，就会导致需要每次迭代后重新进行积分计算，非常耗时。因此提出预积分的方式，使得积分解算的没有第k帧的数据，就是解算的是两帧之间的相对位姿，这样就不需要全部重新进行积分，同时解算的还不包括前一帧的数据（预积分就是利用两帧之间的IMU观测值解算了由k->k+1过程中，相对于k的位姿变换或者说增量）。

由于上面公式都是相对于世界坐标系的转换，那么想要得到相对于前一帧k的变换，就要对上式两边同时乘以坐标转换 $R_{w}^{b_{k}}$

在这里插入图片描述

而其中的 $\alpha _{b{k+1}}^{b_{k}}$ 、 $\beta _{b_{k+1}}^{b_{k}}$ 、 $\gamma _{b_{k+1}}^{b_{k}}$ 就是IMU的预积分项，可以看到其只与不同时刻的观测值有关

对于上面的IMU预积分公式，发现其与偏置 $b$ 有关，而偏置也是需要优化的变量，那么就又是之前的问题，每次迭代都要重新计算预积分，非常耗时。因此这里假设了预积分的变化量与偏置是线性关系（也是由于偏置是缓慢变化的），对预积分项进行一阶泰勒展开：

在这里插入图片描述

至此，预积分项的计算公式已经得到，接下来就是为了计算这个预积分项。

上面预积分项公式中，分为两个部分：预积分的主体和一阶偏导部分

预积分的主体：还是原来的积分形式，但是对于离散时间的实现，就需要采用不同的数值积分方法，在论文中介绍了使用欧拉积分的方法，但是代码中使用了中值积分，因此下面介绍的也是使用中值积分方式：

在这里插入图片描述

一阶偏导部分：这部分计算较为复杂，计算过程在附录1，简单说明下流程：

– 对于上面的这些状态量 $\alpha、\beta、\gamma 、b_a 、b_w$ 的误差（指的是这些状态量的变化）使用 $\delta z_{t}^{b_{k}}$ 表示，计算两个时刻间的误差表示： $\delta z_{t+\delta t}^{b_{k}}=F\delta z_{t}^{b_{k}} + Vn_t$ ，这意味着下个时刻的IMU测量误差与上个时刻成线性关系
– $\delta z_{b_{k+1}}^{b_{k}}$ 相对于 $\delta z_{b_{k}}^{b_{k}}$ 的一阶Jacobian矩阵 $J_{b_{k+1}}$ 可以使用初始的Jacobian矩阵 $J_{b_{k}}=I$ 进行递归计算： $J_{t+\delta t}=FJ_t$
– 其中 $J_{b_{a}}^{\alpha}$ 就是 $J_{b_{k+1}}$ 中的子块矩阵，位置对应于 $\frac{\delta \alpha_{b_{k+1}}^{b_{k}}}{\delta b_{a_{k}}}$ ，其他的 $J_{b_{w}}^{\alpha}、J_{b_{a}}^\beta 、J_{b_{w}}^{\beta}、J_{b_{w}}^{\gamma}$ 也是同样的含义

初始化

初始化阶段使用的是松耦合的传感器融合方法，将视觉和IMU分别单独求解初始状态量，由于单目视觉中不可避免出现的没有尺度的情况，因此还需要使用IMU求解的状态量进行数据对齐，得到尺度信息

滑动窗口-纯视觉

VINS中设置了一个滑动窗口限制计算的复杂度。初始化过程中，寻找最新帧与之前所有帧之间的特征对应，如果找到了稳定的特征跟踪（超过30个跟踪特征）和足够的视差，就使用对极约束恢复位姿，否则就将当前帧放入滑动窗口，等待新的帧。由于没有尺度信息，那么在三角化的过程中任意设置尺度，然后基于这些3D地图点使用PnP估计窗口中其他所有帧的位姿，最后使用全局BA优化所有的特征观测（纯视觉）

VINS中，将第一帧相机坐标系作为基准坐标系（c0），假设相机和IMU之间有个外部参数，就可以将相机坐标系（c0）转换到IMU坐标系（body），下面公式推导在附录2：

在这里插入图片描述

IMU校准

IMU在初始化过程中，也需要对多个状态量进行计算，包括：陀螺仪bias、速度、重力向量和尺度信息

陀螺仪bias

对于连续两帧，可以从纯视觉的初始化过程得到旋转值 $q_{b_{k}}^{c_{0}}$ 和 $q_{b_{k+1}}^{c_{0}}$ ，从IMU预积分得到的相对旋转 $\gamma_{b_{k+1}}^{b_{k}}$ ，那么通过求解下面的代价函数，就能够得到陀螺仪bias。第一项和第二项作四元数旋转的广义乘积就可以得到相机从bk到bk+1下的相对旋转（bk+1坐标系下），第三项是陀螺仪从bk+1到bk下的相对旋转（bk坐标系下），两者在做广义乘积，就是首先从bk到bk+1旋转，然后再从bk+1到bk旋转，相当于做差（OA+AO=0）

在这里插入图片描述

注意：上面公式求解后，得到的是 $\delta b_w$ ，也就是偏置的变化量，需要对滑动窗口内累加才能得到偏置的准确值（求解公式的推导在附录3）

求解陀螺仪bias后，将其重新传递给IMU预积分，得到新的IMU预积分项 $\alpha、\beta、\gamma$

速度、重力向量和尺度

这三个状态量的求解使用的是IMU预积分计算过程中的等式（这时候还没有世界坐标系，只是以c0为基准，后面重力确定了，才有世界坐标系）

在这里插入图片描述

经过陀螺仪bias的计算后，能够得到新的IMU预积分项 $\alpha、\beta、\gamma$ ，在上面的等式中，包含了速度 $v$ 、重力向量 $g$ 和尺度 $s$ （尺度在 $p$ 中，由于初始化过程中单目无尺度，因此这里所有的 $p$ 都是带有 $s$ 的），而其他的变量都是已知了，那么将这个式子转换成：

在这里插入图片描述

然后将坐标系变换的公式带入，并进行整理变成Hx=b的形式（推导过程在附录4）：

在这里插入图片描述

最后就只需要求解最小二乘解：

在这里插入图片描述

重力细化

因为上面计算的重力向量中，没有对重力的模长进行约束（重力的大小是已知的），并且重力的计算包括了重力加速度的影响，因此需要对其进行再次的细化求解：

将重力向量参数化，并添加模长约束，即

在这里插入图片描述

其中的 $\hat{\vec{g}}^{c_{0}}$ 就是上面求解IMU重力向量的单位方向向量， $b_1,b_2$ 也是由其决定：

在这里插入图片描述

那么只需要求解 $w_1,w_2$ 即可求解重力向量，将这个重力向量公式，重新带入之前的最小二乘求解中：

在这里插入图片描述

初始化完成

计算了这些状态量的初始值，最后将这些初始值融合到其他的变量中，就完成了初始化的全部流程：

⏩将重力向量旋转到z轴上，得到世界坐标系与相机坐标系 $c_0$ 之间的旋转 $q_{c_{0}}^{w}$

在这里插入图片描述

⏩将所有的变量从相机坐标系 $c_{0}$ 旋转到世界坐标系
⏩相机的平移和特征点的尺度恢复成计算的尺度单位 $s$

紧耦合VIO

非线性优化

在上面的初始化过程是松耦合过程，为了给系统提供优化初值和状态量，而在后端优化的过程中，就是VINS的重点：视觉与惯性的观测量进行数据融合，统一进行优化，也就是紧耦合过程。VINS中将滑动窗口内的状态量整合到一起，也就是需要优化的所有状态量，包括n+1个相机的状态（位置p、速度v、朝向q、加速度计偏置ba、陀螺仪偏置bg（在预积分那块是bw，相同意义））、相机坐标系到IMU坐标系的外参（位移 $p_c^b$ 、旋转 $q_c^b$ ）、m+1个3D点的逆深度（ $\lambda$ 是特征点的逆深度（标量），指的是第一次观测到该特征点的深度的逆（与第i帧绑定））：

在这里插入图片描述

根据IMU与视觉的融合，建立BA紧耦合模型分为三个部分：边缘化部分（从滑动窗口去掉的位姿和特征点的约束）、IMU误差部分（滑动窗口内相邻帧之间的IMU残差）和视觉测量误差（视觉重投影误差，使用了Huber核函数）

在这里插入图片描述

对于上式的求解最小二乘问题，需要的整体目标函数的增量方程就是：

在这里插入图片描述

由此可以看出，对于IMU和视觉的测量误差，都需要矩阵和协方差矩阵，下面分别对这两项中的残差、Jacobian矩阵和协方差矩阵进行说明

IMU约束

残差：两帧之间的IMU预积分状态量和偏置的误差，在之前计算预积分时就已经求得了：

在这里插入图片描述

优化变量：需要优化的量，这主要用于后面Jacobian矩阵的求解：

在这里插入图片描述

Jacobian矩阵：求残差对于优化变量的偏导，计算流程在附录5：

在这里插入图片描述

协方差矩阵：在计算预积分时，已经将协方差计算出来了： $P_{k+1}=FP_kF^T+VQV^T$

视觉约束

残差：视觉的残差使用的就是重投影误差，但是有所不同的是在单位球面上做的相机观测残差（实际VINS代码好像使用的依然是归一化平面计算），即比较地图点 $p$ 从第一次观测到它的第 $i$ 个相机坐标系转换到当前第 $j$ 个相机坐标系下的坐标与当前观测到它的相机坐标系的坐标进行对比，形成残差（这里没有使用ORBSLAM2中重投影误差，即使用像素坐标的误差，而是这里选择使用归一化平面坐标的误差，主要原因可能是像素坐标最小误差是1个像素，而归一化坐标没有刻度限制，会更加精确）：

在这里插入图片描述

⏩第一个式子就是残差的表达式
⏩第二个式子是相机反投影函数， $\pi_c^{-1}$ 指的是将像素坐标通过相机内参转换为单位向量的观测值数据（可以理解为归一化平面）
⏩第三个式子是重投影的估计模型，推导过程在附录6

优化变量：需要优化的量，这主要用于后面Jacobian矩阵的求解：

在这里插入图片描述

Jacobian矩阵：求残差对于优化变量的偏导，计算流程在附录7：

在这里插入图片描述

协方差矩阵：视觉约束的噪声协方差与标定相机内参时的重投影误差有关（就是偏离了几个像素），这里取得1.5个像素差，则信息矩阵等于协方差矩阵的逆：

在这里插入图片描述

边缘化

VINS使用了滑动窗口，那么不可避免出现帧的加入和退出，也就形成了边缘化问题，即将帧或特征点移出窗口时，如果直接删除掉所有相关的约束，就会导致求解的精度下降，所以将这些相关的约束转变成一个约束项作为先验放在优化问题中，这就是边缘化需要做的事情。

VINS中根据次新帧是否为关键帧，判断边缘化条件：

⏩如果次新帧是关键帧，那么就将滑动窗口内最老的帧边缘化掉（边缘化最老帧）
⏩如果次新帧不是关键帧，就将次新帧丢弃，只保留关联的IMU数据（不边缘化）

注意关键帧的选择标准：特征点足够多并且视差足够大。因此如果次新帧不是关键帧，那么这个次新帧其实与次次新帧很像，完全可以用次次新帧代替这个次新帧，而不会造成很大的影响，但是IMU数据需要保留，是为了保证IMU预积分的连贯性

边缘化marg策略：

在非线性优化理论中， $H\delta x=b$ 可以写成：

在这里插入图片描述

根据边缘化要做的事情：不再计算需要marg的变量，但是希望保留其相关联的约束关系，因此可以使用Schur进行消元：

在这里插入图片描述

即需要保留的变量是 $\delta x_b$ ，而不再求解 $\delta x_a$ ，同时能够在求解的过程中保留了 $\delta x_a$ 相关的约束。由此就可以直接计算保留的变量 $\delta x_b$

在这里插入图片描述

将上式改写成： $H_p \delta x = b_p$ ，即 $H_p、b_p$ 就是为保留变量 $\delta x_b$ 的先验信息，对应的就是整体目标函数的增量方程中的变量。

重定位

为了消除累计漂移，提出了一种紧耦合的重定位模块，依然先进行回环检测，建立回环帧和当前帧之间的连接，再将这些特征连接集成到紧耦合的重定位模块，实现最小计算代价得到状态估计

回环检测

VINS中使用了DBoW2词袋位置识别进行回环检测。除了前端用于跟踪使用的角点特征外，在回环检测模块中还提取了500个角点并由BRIEF描述子描述（应该与ORBSLAM2一样，先使用DBoW2进行特征搜索，再使用BRIEF描述子进行特征匹配）。

回环检测不断对比当前帧与过去帧之间的特征，当计算了描述子后，就丢弃了原始图像来减少内存消耗。

特征恢复

当经过回环检测模块后，通过特征对应关系建立局部滑动窗口与回环候选帧之间的连接。但是仅仅使用描述子去做特征匹配可能会造成大量异常值，因此使用以下两步剔除异常值（ORBSLAM2中好像使用的是旋转直方图剔除不合群的匹配点）：

⏩2D-2D：RANSAC的基础矩阵检验。使用对极约束中的基础矩阵F对当前帧特征的二维观测与回环候选帧进行检验
⏩3D-2D：RANSAC的PNP检验。基于特征在局部滑动窗口中已知的三维位置，以及回环候选帧中的二维观测，进行PNP检验

当内点超过一定阈值，才认为该候选帧是正确的回环并开始执行重定位。下图是分别使用不同方法对匹配点进行检测的结果：

在这里插入图片描述

紧耦合重定位

所谓紧耦合的重定位就是将回环帧的位姿和相关特征点作为视觉约束项，加入到后端非线性优化的整体目标函数中，但是并没有固定回环帧的位姿，只是用滑动窗口来更加精确的计算回环帧位姿，从而计算出与回环帧之间的相对位姿关系，再对滑动窗口内的所有帧进行调整，完成重定位。目标函数为：

在这里插入图片描述

全局位姿图优化

位姿图指的是只有每帧的位姿组成的图，优化这样的图就需要知道优化的变量是什么，对于位姿和位姿之间的连接，如果是纯视觉中，需要6个自由度（旋转+位移），但是在VIO中，由于之前的重力对齐，根据重力方向可以知道俯仰pitch和翻滚roll，因此仅需要优化位置 $x, y, z$ 和偏航角yaw这4个自由度

位姿图中添加关键帧

当关键帧从滑动窗口被边缘化时，它将被添加到位姿图中。这个关键帧在位姿图中作为顶点，它通过两种类型的边与其他顶点连接，而每种边都是由4自由度的变量组成的残差：

⏩序列边：关键帧将建立与之前关键帧的若干边。序列边表示局部滑动窗口中两个关键帧之间的相对转换，其值直接从VIO中获取
⏩回环边：如果新边缘化的关键帧有一个回环连接，它将与回环帧通过一个回环边在位姿图中连接，回环边的值由重定位结果得出

4自由度位姿图优化

对于第 i 帧和第 j 帧的残差可写成：

在这里插入图片描述

所有的回环边和序列边的整体目标函数是：

在这里插入图片描述

其中 $S$ 为序列边， $L$ 为回环边。为了减少任何可能的错误回环造成的影响，为回环边添加了Huber核函数。

参考

VINS知识点汇总
 VINS-Mono论文中文记录
 VINS技术路线与代码详解
 白话VINS-Mono之边缘化（五）
VINS-Mono 代码解析六、边缘化（1）
VINS-Mono 论文解读（IMU预积分残差+Marg边缘化）
VINS-Mono预积分
 VINS-MONO边缘化策略
 Github-StevenCui/VIO-Doc:主流VIO论文推导及代码解析