【面试克星】【公式少代码少话多】Vins-Mono初始化部分概要说明

前言

vio面试很容易被问的一个环节。问的时候多是概念性问题，一般不要求展示代码细节和公式推导。所以针对这种case有了这篇博客。方便大家快速理解这个过程，便于后续进一步学习细节。

先搞个基础

Ax=0和Ax=b 两个最小二乘解的答案
推导反正知道大意，细节看一遍忘一遍，所以直接来结论吧。

Ax=0的解就是对A SVD分解之后最小特征值对应的特征向量
Ax=b的解为$x=(A^{T}A{)}^{-1}{A}^{T}b$
推导过程中有一步x的范数为1这个约束，因为如果x最小，则一定存在kx比x更小，所以增加x的范数为1这个约束，编程有约束的优化问题

四元数表示姿态，在状态量中使用四个状态来表示，即一个实部三个虚部。三个虚部扣掉虚数i，即表示旋转轴。实部为cos角，角度为实际旋转的一半，因此用四元数对一个点进行旋转时要前后各乘一遍，只乘一个才是旋转的一半。

IMU处理

前言

函数：processIMU
processIMU中IMU处理没有所谓的初始化，这点不同于processImage。
每一帧IMU的数据进来都会做这一步处理。
传入参数的时候包含了dt这个变量，所以内部进行积分的时候使用了中值积分的方式，或者说这个函数拿到的IMU数值是积分之前的，不能够直接拿来用。

作用

包含预积分也在里，这个留到下一个博客说。
这里只说这个函数的核心功能——计算两个图像之间的IMU姿态变换

部分细节

中值积分之后认为dt时间内匀速运动以及匀加速运动 然后运用初中学的公式求出距离、速度和姿态。
加速度减bias转到世界坐标系下再减g
角速度减bias

Image处理

前言

处理图像的核心函数，也是每一帧图像都会走一遍这个函数。
这里最主要的函数即为这篇博客要说的初始化函数
进入初始化之前，如果相机的外参没有标定，则先进行一步外参校准：CalibrationExRotation
计算外参中定义的四元数的算子，虽然没有封装成函数，但是也是直接使用了算子推倒的结果。

初始化

存储的图像数据的个数要满足滑动窗口的大小，才能进入真正初始化的阶段。
frame数量需要一直上升直到等于WINDOW_SIZE
如果等于WINDOW_SIZE前没有校正好外参，则会一直往里加数据。
如果等于WINDOW_SIZE了但还是没有校正好外参，则删除最早的图像。但如果
size - 1 帧图像不是关键帧，则删除第size - 1 帧的图像，重新进行初始化。

初始化函数initialStructure函数由两部分组成：

1. 纯视觉SFM估计滑动窗内帧位姿和路标点深度
2. 视觉惯性联合校准

纯视觉SFM估计滑动窗内帧位姿和路标点深度

流程：

1. 最后一帧与滑动窗之间寻找一帧，若这一帧跟踪的点数大于30个且视差超过20个，5点法计算基础矩阵恢复R，t
2. 先暂时随意设定尺度因子，三角化出相对应的特征点，PnP计算所有帧的位姿（是一个不断迭代重复的过程，直到恢复所有帧的位姿），然后BA重投影优化

相关函数：
首先要求imu要有足够的运动，线速度的方差要大于0.25
relativePose：恢复相对姿态，返回和最后一帧相对应的索引
getCorresponding：寻找两帧之间的特征点
cv::findFundamentalMat：求解基础矩阵
cv::recoverPose：从基础矩阵恢复R t
GlobalSFM::construct：作者自行编写，三角化特征点
solveFrameByPnP：求位姿，输入对应的三维点和二维点求解R t，内部调用cv::solvePnP
cv::Rodrigues：罗德里格斯公式，旋转向量和旋转矩阵之间相互转化

视觉惯性联合校准（我认为的重点）

函数：visualInitialAlign
校准目标：

1. 陀螺仪零偏标定bg
2. 速度v，重力g，尺度s初始化
3. 重力矢量修正

校准流程：

1. 旋转外参如果不可知，先估计外参
2. 利用旋转约束估计陀螺仪bias：${\mathbf{q}}_{c_0{b}_k}={\mathbf{q}}_{c_0{c}_k}\otimes {\mathbf{q}}_{bc}^{-1}$
3. 利用平移约束估计重力方向、速度以及尺度初始值：$s{\overline{\mathbf{p}}}_{c_0{b}_k}=s{\overline{\mathbf{p}}}_{c_0{c}_k}-{\mathbf{R}}_{c_0{b}_k}{\mathbf{p}}_{bc}$（实际重力估计出来之后还有一步重力的优化：参考这里）
4. 计算出了角计的偏置，设加计偏置为0，重新预积分计算了整个imu的相关量
5. 因为恢复了尺度，所以将没有尺度的三维坐标回复成有尺度的三维坐标，后进行ceres优化

相关问题：

1. 为什么只估计陀螺仪bias，而不估计加速度计的bias？

原作者通过仿真结果证明了，如果运动不够剧烈，加速度计的bias是很难被观测出来的。其次忽略加速度计的bias对估计其他初始值的影响并不大。

2. 为什么估计重力向量

我们只知道重力在东北天坐标系下的方向（无论在哪个坐标系，大小都是9.81，严格来说跟g一样），但是世界坐标系一般指的都是第一帧坐标系（camera或者body什么的都可以，因为有外参可以互相转换），重力方向在东北天坐标系下当然是竖直向下，但是在世界坐标系下经常不一定是竖直向下（xy平面并不平行于世界坐标系的水平面），所以我们要估计重力向量在世界坐标系，也就是第一帧坐标系下的方向。

3. 为什么只估计了速度初始值而不估计角度

这个目前用于估计的公式里其实也没有角度的约束，所以也是想估计没办法估计。另外估计初始值的目的之一是防止估计器在使用的过程中不收敛，可能作者测试的时候发现角度的初始值的设定并不影响估计器的收敛问题，因此也是没进行估计。

4. 为什么没有估计相机坐标系和IMU坐标系的平移外参？

一般进行刚体安装时两者相对平移量很小，我们可以设定为一个较小的值，后续再进行优化。