前言

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

先搞个基础

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

Ax=0的解就是对A SVD分解之后最小特征值对应的特征向量
Ax=b的解为\(x=(A^TA)^{-1}A^Tb\)
推导过程中有一步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函数由两部分组成：

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

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

流程：

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

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

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

函数：visualInitialAlign
校准目标：

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

校准流程：

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

相关问题：

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

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

为什么估计重力向量

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