VINS-mono学习总结

Vins-mono是一个后端基于非线性优化的、单目与IMU紧耦合的融合定位算法。

整体：

1 预处理模块 视觉：特征点提取与追踪 IMU：惯性解算与误差状态分析、计算预积分量

2 初始化模块（旋转外参标定、基于图像的三维重建-纯视觉单目slam问题、陀螺仪零偏估计、视觉惯性对齐、利用重力的先验知识修正重力）

3 基于滑动窗口的非线性优化模块（预积分约束、视觉重投影约束、边缘化约束

4 回环检测模块（检测回环、校验回环、构建回环约束）

分模块：

1 预处理模块：

1.1特征点的提取与追踪

1.1.1利用灰度不变性假设进行光流追踪，计算出特征点在两帧之间的速度，由于有许多特征点，构建优化问题（采用最小二乘法）去得到被追踪的特征点。

1.1.2图像金字塔：对原始图像进行不同倍率的缩放。这是因为优化问题与像素梯度有关，像素梯度非线性化特别强，若优化起点与最优解相差太多像素，很容易陷入局部最小值。采用图像金字塔，若原本优化起点与最优解差10个像素，采用金字塔进行缩放就缩小为5个像素，容易追踪上来，故将上层金字塔追踪结果作为下层金字塔的初值逐层追踪，可以避免陷入最小值，避免像素精度下降，提升光流追踪的稳定性。

1.1.3对极约束去外点

采用RANSAC法求本质矩阵E，去除外点

原理：同一特征点在第二张图片的位置应该落在由第一个特征点以及外参所决定的极线上面，点与极线的距离超过门限就认为是个outlier，应为舍弃的误匹配。

1.1.4特征点均匀化

对追踪的特征点按照追踪次数进行排序，追踪次数越多，认为特征点的质量越好。

为了避免特征点过于集中，选中的特征点周围一定半径区域不允许别的特征点存在。

1.1.5提取一些新的特征点

由于去除outlier和均匀化的操作，特征点数目会越来越少，故要提取新的特征点。

1.1.5 逐次逼近式去畸变

相机投影模型是3D点经过外参从世界坐标系转换到相机坐标系，归一化，畸变参数计算畸变后的坐标，再经过内参得到他的像素坐标。

我们现在已知畸变后的2d像素坐标，想反推回3D点，就需要知道畸变前的相机坐标。Vins-mono采用逐次逼近法求解畸变前的相机坐标。

1.1.6选取关键帧

1）视差：两帧匹配的特征点像素差超过一定阈值判定为视差较大，认为新的一帧是关键帧。

2）追踪质量：跟踪的特征点明显表少，认为是关键帧。

1.2 IMU惯性解算与误差状态分析，状态预测与协方差预测、预积分量的计算

预积分及其作用？--将相对于上一帧状态进行积分转变为相对于上一帧时刻进行积分

使用视觉里程计与IMU数据联合进行后端优化时，对每个点云关键帧姿态的调整就意味着需要对其与前后两帧之间的IMU数据进行重新积分，这对计算时间与计算资源来说都是极大的浪费，因此我们使用IMU的预积分模型来简化冗余的重新积分过程，节省时间与计算量。把积分项变成相对与上一帧的起始时刻进行积分，而不是相对于上一个坐标系状态进行积分，且只需要积分一次，积分出的相对位姿保持不变，每次迭代优化调整的都是IMU坐标系相对世界坐标系的姿态，这样就不需要再重复进行积分运算。预积分量同时可以作为两个点云帧之间的位姿变换。

2初始化模块

优化变量：位置、速度、姿态、零偏、外参、地图点

初始化（值）的意义：优化问题求解是迭代下降的方式，建模优化问题后用G-N法或L-M法以初始值为起点找最小值，要保证初始值与最终结果足够接近，防止陷入局部最小值，另一方面，初始值离最终结果越近，迭代次数越少，每迭代一次重新计算雅可比和增量方程，计算量比较少，实时性能够得到保证。

2.1 旋转外参标定：

将两帧陀螺仪积分乘外参与两针图像对极约束乘旋转外参建等式关系，利用多帧数据构建超定方程AX=0，进行SVD分解求出旋转外参。

2.2基于图像的三维重建，纯视觉单目slam问题

选择枢纽帧，在滑窗中与最后一帧既不能太近也不能太远，要求共视多，视差大。

不能太近是因为要保证视差足够大，避免纯旋转，否则3自由度的变化要求解出6自由度的解会带来奇异性。

不能太近是因为要保证两帧匹配的特征点要足够多，特征点少解出的置信度不高。

（代码中共视大于20，总视差大于30个像素，从第一帧开始与最后一帧做比较，首个满足条件的是枢纽帧）

找到枢纽帧后，利用枢纽帧与最后一帧的对极约束求解本质矩阵求得R,T，三角化出地图点。

接下来其他帧与最后一帧、枢纽帧通过最小化重投影误差求解PnP，其他帧的初始位姿是上一帧的位姿，再三角化出地图点。

将10帧求解出来的位姿作为初始值，进行全局的BA，通过重投影误差构建优化问题求解出置信度更高的位姿和地图点。

采用ceres的自动求解决初始化全局BA问题。

2.3陀螺仪零偏估计

零偏建模方式为预积分量对零偏进行一阶泰勒展开。

根据全局BA求解出的位姿以及外参，将视觉计算出两帧的旋转作为准确值，与imu预积分量做对比，将零偏作为优化变量最小化视觉与imu两帧位姿变化的误差，求解出零偏。

初始化时，零偏的初始值为0，计算出零偏变化量较大，预积分量需要重新传播，滑窗优化时，零偏变化量较小，可通过零偏建模公式将零偏变化量补偿到预积分量中。

2.4视觉与惯性对齐

将每一帧图像的位姿与imu预积分构建等式、求解的线性方程，求解各帧的速度、尺度、枢纽帧的重力。

2.5利用重力的先验知识修正

重力大小可以通过查表获得先验，vins认为当地的重力大是准确的，故只需要优化重力的方向，它将重力方向分解为在切平面上的两个相互正交的单位向量，优化两单位向量的权重改变重力方向，从此，优化重力从3个自由度转变为2个自由度。

3基于滑动窗口的非线性优化模块

后端是非线性优化的框架，对于非线性优化来讲，初值是一个很重要的因素，当初值离真值特别远时，基于迭代的非线性优化很容易陷入局部最优解，很难收敛到全局最优解。因此初始化模块提供了良好的初始值，初始值离真值越近，迭代次数越少，每次迭代需要重新计算增量方程，计算量比较小，实时性得到保证。

雅可比矩阵：残差对于每一个优化变量求导

ceres自动求导：只需要定义残差的计算方式，自定义一个重载括号运算符的模板函数用来提供残差的计算方式。

ceres解析求导：既需要定义残差的计算方式，又需要定义残差对各个参数块的雅可比矩阵，重载Evaluate函数用来计算残差和雅可比。

Ceres loss function 鲁棒核函数：作用是筛出outlier。采用最小二乘问题求解优化问题时，我们将最小化误差项的二范数平方和作为目标函数，残差以二范数增长，误匹配、outlier会导致误差很大，往往会抹平其他正确边的影响，使优化算法专注于调整一个错误的值。核函数的做法是当误差超过一定的阈值时，将二次函数替换成一次函数，保证每条边的误差不会过大而掩盖其他的边，降低误匹配点对优化问题的影响。

3.1预积分约束：约束两针之间的位姿、速度和零偏

如果两帧状态都是imu预积分得到的，预积分等式完全成立。但在优化问题中，位姿不仅受预积分约束，还受其他约束影响，例如重投影约束，为了减小其他约束的误差，会对变量进行调整，此时预积分等式不成立。但位姿无论怎么调整都不能离预积分量太远，当预积分残差过大时，预积分量产生约束也会对变量进行调整。

两步：定义残差、雅可比矩阵计算

1、残差定义：两帧之间的位姿与预积分量之差，将协方差矩阵P分解为LTL，构造新的加权重的残差e=LTe

2、雅可比矩阵的计算：残差对两帧的位置、速度、姿态、零偏求导。

3.2视觉重投影约束

使用逆深度建模，将3D点与第一次看到他的帧绑定逆深度。

一方面优化地图点时3个自由度的3D点减小为1个自由度的逆深度，求解速度更快，另一方面特征点的深度不会离相机很近，深度大时逆深度小，数值稳定性会更好。

1、残差定义：第i帧通过光流追踪到第j帧的像素坐标与第i帧通过内参，外参投影到第j帧的像素坐标之差（观测值与估计值之差）

优化变量：位姿，相机与imu之间的外参，逆深度

2、雅可比矩阵求解：采用链式求导法则，残差对优化变量求导变为残差先对3D点求导，3D点再对优化变量求导。优化调整位姿时要固定华创中最老帧的位姿，防止零空间的漂移。

3.3边缘化约束

判断滑窗内倒数第二帧是否为关键帧，若为关键帧，则边缘化掉最老帧，若不为关键帧，则边缘化掉倒数第二帧。然而滑窗去掉的最老帧状态量对滑窗内部仍是有约束作用的。

被边缘化部分为：预积分约束的第0帧与第1帧位姿速度和零偏，重投影约束第0帧与其他帧的位姿外参逆深度

通过舒尔补消元实现边缘化，优化问题最终会变为求解增量方程的等式，将被边缘化的部分与保留部分分离，左右同乘矩阵进行消元，目的使左下角矩阵块变为0，此时将最老帧状态抹掉了，但与最老帧相关的信息仍然保留在矩阵快中，对保留下来的部分仍然有约束作用，被边缘化后的状态量对保留下来的状态量仍有贡献。

边缘化后矩阵体现的现象是原本无关的矩阵块，由于老帧被边缘化而变得相关。

边缘化会带来fill-in现象，把原来的稀疏矩阵变成了稠密矩阵。

vins的解决办法：边缘化掉第0帧看到的所有地图点，既然稠密矩阵无法避免，我们就减少矩阵的维度。通过边缘化后的增量方程得到边缘化约束的残差和雅可比矩阵。

随着后续的优化，滑窗内的状态量会被改变，故边缘化的残差会被改变，保持边缘化的雅可比矩阵不变，当残差变大时，要使当前状态量尽可能接近边缘化时的状态量。

优化时达到迭代次数或增量小于一定阈则认为收敛。

4回环检测模块

滑窗优化输出一个平滑的VIO位姿，经过回环检测优化出一个全局一致性更好的轨迹。

加载通过K-means聚类并且训练好的词袋。

在原图上计算已有特征点的描述子并额外提取Fast特征点。（特征点周围一定区域内的坐标点对，两个点对灰度值A>B为1）

降采样进行回环检测：间隔几帧，回环帧之间相隔必要的平移距离。

4.1找到回环关键帧

利用词袋模型，调用DBOW库接口进行回环检测，候选关键帧的得分大于0.05，其他附近候选关键帧也要大于0.015，候选关键帧id尽可能小，这样从回环关键帧到当前关键帧调整时可调整更多的位姿，全局一致性会更好。（索引尽可能小，得分尽可能高）

4.2回环校验：通过词袋找到候选，通过描述子和几何进行确认是否是真正意义上的回环。

描述子校验：对当前关键帧与回环帧描述子计算汉明距离，计算出两帧之间描述子的相似度，判断描述子匹配得分和汉明距离。

几何校验：利用RANSAC求解PnP，内点数大于一定的阈值，相对位姿变换小于一定阈值认为找到了回环。

4.3 4自由度的位姿图优化

由于重力在地球上是可观的，所以roll，pitch角是可观的，故只计算yaw角。

由于当前帧与回环帧一般是比较近的，所以计算相对位姿变换时，回环帧初始位姿设定为当前帧位姿（这里先固定当前帧，PnP求解相对位姿变换，再固定原始回环帧，根据相对位姿变换求解新当前帧位姿）

构建回环约束：地图点应该被当前帧和回环帧同时看到，构建当前帧与回环帧之间的视觉重投影约束。

4自由度优化的节点为最早回环帧到当前产生回环帧的位姿，最早回环帧之前的节点是不优化的。此时应固定第一帧，防止零空间上的漂移。

回环帧约束残差的定义为当前帧与回环帧回环节点下的相对位姿变换与VIO节点下相对位姿变换之差。

优化问题会使回环产生的残差均匀分配给其他边，通过动态调整其他帧位姿使得轨迹的一致性更好。

计算当前帧VIO位姿与优化后的位姿差，当前帧之后的所有位姿，根据得到的位姿差进行补偿。