VIO第7讲：VINS初始化与VINS系统

1 VINS初始化

  VINS初始化采用视觉和 IMU 的松耦合方案，首先用 SFM 求解滑窗内所有帧的位姿，和所有路标点的 3D 位置，然后跟 IMU 预积分的值对齐，求解重力方向、尺度因子、陀螺仪 bias 及每一帧对应的速度。初始化的流程图如下所示：

​

1.1 视觉初始化

1.1.1 relativePose

  relativePose：字面意思即相对位姿，说明这里求解的并不是世界系下绝对位姿。在滑窗内寻找与当前帧的匹配特征点数较多的关键帧作为参考帧，并通过求基础矩阵
cv::findFundamentalMat 计算出当前帧到参考帧的姿态T_cr；

1.1.2 GlobalSFM与BA优化

  参考帧为上一步选出来的最共视帧，最后得到的sfm_tracked_points 为三角化出来的 3D 路标点。

用一个图来表示

1.1.3 visualInitialAlign

  纯视觉初始化时，我们采用第一帧 c0 作为基准坐标系。类似于纯视觉SLAM中，以第一帧作为世界系，后续所有姿态以其为基准。

1.2 VisualIMUAlignment

  视觉惯性对齐，如何对齐？

考虑以下几个问题

1 IMU怎么和世界坐标系对齐，计算初始时刻的$q_{w{b}_0}$?

2 单目视觉姿态如何和IMU轨迹对齐，尺度如何获取？

3 VIO系统的初始速度v，传感器bias等如何估计？

4 IMU和相机之间的外参？

1.2.1 视觉和IMU之间的联系

  考虑相机坐标系$c_0$为世界坐标系，利用外参可以构建下面的等式（VINS论文）：
$$\begin{array}{rl}{\mathbf{q}}_{c_0{b}_k}& ={\mathbf{q}}_{c_0{c}_k}\otimes {\mathbf{q}}_{bc}^{-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}}_{bc}\end{array}$$
  旋转关系是显而易见的，主要写下关于平移。上面式子中s是视觉与IMU尺度转换因子。单目视觉没有一个比较准确的尺度，所以所有计算的距离等都是一个相对量！$\overline{\mathbf{p}}$表示非米制单位的轨迹，即视觉下表示平移的量。

  为什么会有这么一个尺度转换，这个是因为${\mathbf{p}}_{bc}$是相机与IMU平移量的外参，是一个可以确定的米制量！还有一点，${\mathbf{p}}_{bc}$是IMU坐标系下的量，我们把$c_0$看作世界系，所以在坐标变换中需要将向量${\mathbf{p}}_{bc}$转换到世界系！ 参考下面绘制图理解。

$$\left[\begin{array}{c}{\overline{P}}_{c_0{c}_k}\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{c_0{b}_k}& p_{c_0{b}_k}\\ \to & \\ O& 1\end{array}\right]\cdot \left[\begin{array}{c}\frac{1}{s}{\overline{P}}_{bc}\\ 1\end{array}\right]$$

  上面的式子还可以变换为下面的形式，本质上都是一样的。总之，s是一个非常关键的尺度转换量。

  我们也能通过旋转矩阵来推导：
$$\begin{array}{c}{T}_{c_0\leftarrow b_k}=T_{c_0\leftarrow c_k}{T}_{b\leftarrow c}{}^{-1}\\ \iff T_{c_0\leftarrow b_k}{T}_{b\leftarrow c}=T_{c_0\leftarrow c_k}\\ \iff R_{c_0\leftarrow b_k}(R_{b\leftarrow c}{P}_{c_k}+t_{b\leftarrow c})+t_{c_0\leftarrow b_k}=R_{c_0\leftarrow c_k}{P}_{c_k}+t_{c_0\leftarrow c_k}\end{array}$$
  然后就可以得到上面的结果，这里t就是上面的平移p
$$⟹\{\begin{array}{c}{R}_{c_0\leftarrow b_k}=R_{c_0\leftarrow c_k}{R}_{b\leftarrow c}^{-1}\\ t_{c_0\leftarrow b_k}=t_{c_0\leftarrow c_k}-R_{c_0\leftarrow b_k}{t}_{b\leftarrow c}\end{array}$$

1.2.2 视觉IMU对齐流程

  估计外参----估计偏置bg----估计相机系的重力$g^{c0}$、尺度s、速度v----优化重力方向—将VIO坐标系与世界系对齐

① 旋转约束估计外参$q_{bc}$

initial_ex_rotation.cpp 函数 CalibrationExRotation()

这里的旋转约束估计是为了哪些不知道外参的情况，有的时候可以通过事先的标定去解决，如kalibar工具；或者说，买的机器人厂家已经给出了。

​

1 利用相同时间戳的两帧IMU和cam数据进行旋转约束（一个平行四边形的两条路径）

$${\mathbf{q}}_{b_k{b}_{k+1}}\otimes {\mathbf{q}}_{bc}={\mathbf{q}}_{bc}\otimes {\mathbf{q}}_{c_k{c}_{k+1}}$$

2 ${\left[\cdot \right]}_L,{\left[\cdot \right]}_R$是left and right quaternion multiplication，就是左右乘算子

$${(\left[\begin{array}{c}{\mathbf{q}}_{b_k{b}_{k+1}}\end{array}\right]}_L-{\left[\begin{array}{c}{\mathbf{q}}_{c_k{c}_{k+1}}\end{array}\right]}_R){\mathbf{q}}_{bc}={\mathbf{Q}}_{k+1}^k\cdot {\mathbf{q}}_{bc}=0$$

3 如果累计多个时间段的旋转约束，并加上鲁棒核权重，很明显类似于三角化的一个最小二乘问题！

$$[\begin{array}{c}{w}_1^0\cdot {\mathbf{Q}}_1^0\\ w_2^1\cdot {\mathbf{Q}}_2^1\\ ⋮\\ w_N^{N-1}\cdot {\mathbf{Q}}_N^{N-1}\end{array}]{\mathbf{q}}_{bc}={\mathbf{Q}}_N\cdot {\mathbf{q}}_{bc}=0$$

  其中
$$w_{k+1}^k=\{\begin{array}{cc}1,& r_{k+1}^k<\text{threshold}\\ \frac{\text{threshold}}{r_{k+1}^k},& \text{otherwise}\end{array}$$
  求解同样采用 SVD 分解，即最小奇异值对应的奇异向量，即V的最后一列，也即$V^T$的最后一行
$${\mathbf{Q}}_N=DS{V}^T$$

4 四元数实部表示了旋转角，虚部表示了旋转轴–$\operatorname{tr}(\mathbf{R})=1+2\cos\theta $，利用这个公式，下面式子理论上乘积应该为单位矩阵，对应角度为0，即没有旋转。所以利用角度误差也能够优化外参！

$$r_{k+1}^k=\mathrm{acos}\left(\left(\mathrm{tr}\left({\hat{\mathbf{R}}}_{bc}^{-1}{\mathbf{R}}_{b_k{b}_{k+1}}^{-1}{\hat{\mathbf{R}}}_{bc}{\mathbf{R}}_{c_k{c}_{k+1}}\right)-1\right)/2\right)$$

补充：求迹tr()即求矩阵的对角线元素之和。

② 陀螺仪 bias 校正—旋转约束

initial_aligment.cpp 函数 solveGyroscopeBias().

如果外参数$q_{bc}$已标定好，利用旋转约束，可估计陀螺仪 bias，

约束：视觉给出的相邻帧间的旋转应该等于IMU预积分的旋转值

理论上下面旋转为0，应该对应一个单位阵（单位四元数）

$$arg\underset{\delta {\mathbf{b}}^w}{\min }\sum _{k\in B}{\Vert 2{\lfloor {\mathbf{q}}_{c_0{b}_{k+1}}^{-1}\otimes {\mathbf{q}}_{c_0{b}_k}\otimes {\mathbf{q}}_{b_k{b}_{k+1}}\rfloor }_{xyz}\Vert }^2$$
其中, B 表示所有的图像关键帧集合，其中，更新偏置后的预积分的一阶泰勒近似:
$${\mathbf{q}}_{b_k{b}_{k+1}}\approx {\hat{\mathbf{q}}}_{b_k{b}_{k+1}}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b^g}^{\mathbf{q}}\delta {\mathbf{b}}^g\end{array}\right]$$

最终其实就是求解Ax=b的优化问题

③ 初始化速度、重力$g^{c0}$和尺度因子—平移约束

initial_aligment.cpp 函数 LinearAlignment()

  ${\mathbf{g}}^{c_0}$为重力向量在第 0 帧相机坐标系下的表示，s表示尺度因子，将视觉轨迹拉伸到米制单位，${\mathbf{v}}_k^{b_k}$表示k时刻速度在body系下的表示！
$${\mathcal{X}}_I={\left[{\mathbf{v}}_0^{b_0},{\mathbf{v}}_1^{b_1},\cdots {\mathbf{v}}_n^{b_n},{\mathbf{g}}^{c_0},s\right]}^{\top }$$

  前面预积分观测量（左）和状态预测值（右）的关系如下，w表示世界系，后续用$c_0$表示
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_j}={\mathbf{q}}_{b_{i}w}({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2)\\ & {\mathbf{\beta }}_{b_i{b}_j}={\mathbf{q}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)\end{array}$$
  要注意的一点就是，重力加速度$g^w$是知道的，就是我们常知的9.8，但是我们一开始把第一帧$c_0$表示世界系，这个坐标系并不一定与世界系相同，所以我们需要去估计${\mathbf{g}}^{c_0}$

将世界坐标系 w 换成相机初始时刻坐标系$c_0$有（cam到imu存在尺度s关系）

$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_k{b}_{k+1}}={\mathbf{R}}_{b_k{c}_0}\left(s\left({\overline{\mathbf{p}}}_{c_0{b}_{k+1}}-{\overline{\mathbf{p}}}_{c_0{b}_k}\right)+\frac{1}{2}{\mathbf{g}}^{c_0}\Delta t_k^2-{\mathbf{R}}_{c_0{b}_k}{\mathbf{v}}_k^{b_k}\Delta t_k\right)\\ & {\beta }_{b_k{b}_{k+1}}={\mathbf{R}}_{b_k{c}_0}\left({\mathbf{R}}_{c_0{b}_{k+1}}{\mathbf{v}}_{k+1}^{b_{k+1}}+{\mathbf{g}}^{c_0}\Delta t_k-{\mathbf{R}}_{c_0{b}_k}{\mathbf{v}}_k^{b_k}\right)\end{array}$$

把$\begin{array}{rl}{\mathbf{q}}_{c_0{b}_k}& ={\mathbf{q}}_{c_0{c}_k}\otimes {\mathbf{q}}_{bc}^{-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}}_{bc}\end{array}$代入，然后就可以得到下面等式

$$\begin{array}{rl}{\mathbf{\alpha }}_{b_k{b}_{k+1}}=& s{\mathbf{R}}_{b_k{c}_0}\left({\overline{\mathbf{p}}}_{c_0{c}_{k+1}}-{\overline{\mathbf{p}}}_{c_0{c}_k}\right)\\ & -{\mathbf{R}}_{b_k{c}_0}{\mathbf{R}}_{c_0{b}_{k+1}}{\mathbf{p}}_{bc}+{\mathbf{p}}_{bc}+\frac{1}{2}{\mathbf{R}}_{b_k{c}_0}{\mathbf{g}}^{c_0}\Delta t_k^2-{\mathbf{v}}_k^{b_k}\Delta t_k\end{array}$$

问题求解

④ 修正重力矢量

  上面求解重力${\mathbf{g}}^{c_0}$的过程中，没有加入模长的限制$\Vert {\mathbf{g}}^{c_0}\Vert =9.81$。一个三维变量，有了模长限制，说明它可以用确定两个数，就知道第三个值，也称重力${\mathbf{g}}^{c_0}$只有两个自由度。

⑤ 相机坐标系对齐世界坐标系

  通过利用世界系$g^w$与相机系的重力$g^{c0}$，我们可以得到两个坐标系之间的旋转！因为旋转可以用角轴表示，所以只需要计算出旋转轴和旋转角即可！

这部分那个视频还是讲的挺好的！

2 VINS系统

VINS系统框架

VINS状态变量和残差

特别要注意的是先验残差，如何构建？如何更新？VIO第4讲提到过，基于一阶泰勒近似。

优化马氏距离，即带有了信息矩阵。但Ceres中只接收最小二乘（残差与雅可比），所以会把信息矩阵进行LLT分解，构建新的残差。

至于为什么信息矩阵通常放在海塞矩阵里面，这个十四讲第六讲很久之前就推导过了。