VIO学习笔记（二）：估计器的初始化问题

VIO学习笔记（二）：估计器的初始化问题

继上篇学习了IMU传感器的测量原理之后，今天来说说VIO估计器的初始化问题。
1.什么是VIO估计器的初始化？
初始化就是把变量赋值为默认值，把控件设置为默认状态，把每准备好的准备好。这么看来，初始化问题似乎并不那么困难，但不幸的是，把初始化这个定义用在整个系统层面来讲就不是那么简单的了，系统的初始化是指系统在初次使用时，根据实际情况进行参数设置，涉及到实际情况，那么情况就比较复杂了，操作起来也就不那么容易了。
什么是VIO估计器的初始化呢？简单来说，就是把VIO这个视觉惯性系统工作所需要的参数提供给VIO这个系统。学习过VIO相关知识的朋友们都知道，要是VIO系统顺利工作，需要提供给系统速度，重力矢量、尺度等重要参数。VIO估计器的初始化就是干这个工作的。
2.为什么要做VIO的初始化工作？
大家都知道VIO系统是一个高度的非线性系统，由于单目相机无法直接观测到尺度，这样就导致如果没有良好的初始化值，很难直接将两种传感得测量值融合到一起。同时要知道的是，初始化工作在整个系统运行过程中是经常被用到的，不仅仅在系统的启动阶段需要用到初始化，在故障重启的时候也需要用到初始化这个工作。因此，VIO的初始化是重要的，也是必要的。
3.如何进行VIO系统的初始化工作？
这里介绍港科大沈老师团队的一种VIO初始化方法，也就是VINS-mono这篇论文中提到的方法。
沈老师的团队考虑到从单纯的单目相机无法得到尺度信息，但是VSLAM或者说的更加直白点，从运动中恢复结构（SFM），具有良好的初始化性能，什么叫SFM呢？学过视觉SLAM的同学们都知道，单纯从单目相机上我们无法得到尺度等重要信息，但是我们通过相应的方法可以从连续运动的关键帧图像中解算出相机的运动信息。没错，就是高博14讲上所讲的那一套流程。主要分为以下几个步骤：
（1）纯视觉SFM
比如从连续关键帧中使用5点法或者8点法来恢复旋转和带有尺度的平移，然后使用三角化等方法恢复实际的尺度，这样就得到3D-2D的特征，有了这些特征，采用PNP算法就可以解算出实际的旋转和平移，最后对所有的帧及3D点进行BA优化。这里我们要知道，纯视觉SFM是没有世界坐标系的认识的，所有位姿都是基于第一帧图像。如果我们通过联合标定知道相机和IMU的外参，就可以将姿态从相机坐标系转为物体坐标系（imu坐标系）

这里所有真的位姿用：

来表示。

相机和IMU之间的外部参数用

来表示。
（2）视觉惯性校准
这一步及是将视觉SFM和IMU进行对齐。
首先是陀螺仪偏置的标定，这个参数如果是IMU传感器较好，偏置一定的话，是可以直接用六面法将偏置误差标定出来的，但沈老师团队采用了更牛的做法，在初始化阶段对IMU的偏置也初始化，方法比较简单，就是整个视觉解算出的旋转变化量与IMU预积分得到的两帧间位姿的乘积理论上应该相等，也就是原文中优化代价函数的出发点：

这里备注一下，在IMU的预积分项中是有IMU偏置这一项的。即上式中的bw。
接下来是速度、重力矢量、尺度等参数的初始化，其中，待优化的状态向量定义为：

bn表示第n帧图像本体坐标系下的速度，gc0为相机坐标系下的重力向量，s的尺度。
这样我们通过IMU和视觉的对齐思路可以得到下式：

等号左边为IMU预积分量，等号右面为通过视觉得到的量。理论上两者应该也是相等的，再用上文的第一个将相机坐标系转到IMU坐标系的公式来处理上式，可以得到：

这样就建立了预积分和视觉SFM之间的关系式，我们就可以定义一下优化目标函数来进行优化：

这样整个就可以得到所有帧在第一帧坐标系下的速度、尺度以及重力矢量因子。
第三步是对重力矢量进行微调，由于重力大小已知，那么重力矢量就只剩两个自由度了，作者在其切线空间上用两个变量重新参数化了重力，将其表示为：

至此，VIO的初始化工作就完成了。