6.初始化
第一个问题，为什么要初始化？
对于单目系统而言，
(1)视觉系统只能获得二维信息，损失了一维信息(深度),所以需要动一下，也就是三角化才能重新获得损失的深度信息；
(2)但是，这个三角化恢复的深度信息，是个“伪深度”，它的尺度是随机的，不是真实的，所以就需要IMU来标定这个尺度；
(3)要想让IMU标定这个尺度，IMU也需要动一下，得到PVQ的P；
(4)另外，IMU存在bias，视觉获得的旋转矩阵不存在bias，所以可以用视觉来标定IMU的旋转bias；
(5)需要获得世界坐标系这个先验信息，通过初始化能借助g来确定；
对于双目系统而言，
初始化的难度会低一些，因为双目可以一下确定深度，只需要通过g的方向(先验)来确定世界坐标系。
注意，这个初始化只进行一次就够了，或者是在系统重启时执行一次，大部分时候，系统都处于NON_LINEAR的状态。因为，初始化的时候，就能确定尺度scaler和bias初始值，scaler确定后，在初始化获得的这些路标点都是准的了，后续通过PnP或者BA得到的特征点都是真实尺度的了。而bias初始值确定以后，在后续的非线性优化过程中，会实时更新。
6.1 基础原理
初始化的逻辑图如下：
6.1.1 如果旋转外参数 qbc 未知, 则先估计旋转外参数
实际上讲，这部分并不是vins代码中初始化的内容，而是初始化之前就需要完成的判断和操作，见5.2-4。根据IMU和视觉部分的旋转关系，可以得到下面的关系式：将多个帧之间的等式关系一起构建超定方程Ax=0。对A进行svd分解，其中最小奇异值对应的奇异向量便为需要求解的qbc。
虽然这里IMU的旋转部分并没有标定，得到的外参数可能不太准。但是问题不大，因为初始化所占用的总运行时间不长，而更长生命周期的后端会持续的优化这部分的值。
6.1.2 利用SfM确定各个pose和特征点的相对于c0帧的位置关系
这一部分和基于图像的三维重建比较像，可以用三角化和PnP把这一串的ck帧的位姿和特征点位置确定下来(特征点是伪深度)，在加上外参数qbc和pbc，一系列bk帧的位姿也确定下来。注意，这里把c0帧作为基础帧，实际上，c0帧旋转一下使gc0和gw方向一致时获得的坐标系就是vins的世界坐标系，也就是先验。在vins代码中，这一部分篇幅很长，但是这一部分也是很重要的部分。
首先我们先推导论文式(14)，所有帧的位姿(Rc0ck,qc0ck)表示相对于第一帧相机坐标系(·)c0。相机到IMU的外参为(Rbc,qbc)，得到姿态从相机坐标系转换到IMU坐标系的关系。
将T展开有成R与p有：
左侧矩阵的两项写开：
6.1.3 利用相机旋转约束标定IMU角速度bias
求解的目标函数如下公式所示：
6.1.3 利用相机旋转约束标定IMU角速度bias
求解的目标函数如下公式所示：
在SfM完成且外参数标定完之后，头两个值是已知的了，而且我们假设头两个值是准的。理想状态下，这三个数乘积应该是单位四元数，很可惜，第三个值是IMU预积分得到的，而预积分里面是有bias的。所以，通过最小化这个目标函数的，可以把旋转bias标定出来！
在IMU预积分部分，也就是4.1.1最后的那个公式，有：
带入到损失函数里，可以得到：
或者是：
带入bias的残差后，得到，
实部没有需要标定的量，所以只用考虑虚部，也就是：
两侧再乘以 ，可以构造出Ax=B的形式，在采用LDLT分解，就可以求出状态量：
其实这里不像那种用高斯牛顿法迭求解，而更像是用直接法,也就是矩阵运算的方式来求待优化的状态量，6.1.4也是一样的思路。代具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias()。
6.1.4 利用IMU的平移估计重力/各bk帧速度/尺度scaler
首先要明确需要优化的状态量是什么，是各帧在bk坐标系下的速度，c0帧下的g和SfM的尺度scaler：
这块有个遗留问题，就是为什么要优化速度呢？
在IMU预积分部分，已经有如下的公式：
但是w坐标系我们不知道，只知道c0坐标系，所以需要把上面的公式转到c0坐标系上：
上式中，等号左边减去等号右边就是残差，理想状态下，这个残差是0，那么带入上式得：
把这个 XX = 0的等式也转为 A x = b这种线性方程组的形式，如下式：
或者矩阵的形式：
对于beta而言，也是类似的，
那么，把这两个矩阵合体，就能得到论文中的等式：
同样采用LDLT分解，就能求出状态量：
6.1.5 利用gw的模长已知这个先验条件进一步优化gc0
但是，这里有一个bug，就是把g当成3自由度的向量优化，但是实际上g只有2自由度，因为它的模长是已知的。所以，在这里，需要想一个办法，就是怎么样用一个2自由度的表达式来表示3维的向量呢？
在这里，采用球面坐标进行参数化，也就是用g的模长作为半径画一个半球，上图蓝色线对应的是gc0的测量值的方向(也就是优化前的方向)，在这个交点上找到一个切平面，用gc0，b1，b2构造一个坐标系，那么在轴b1和b2上坐标值w1和w2就是我们需要求的量。求出来之后，把等号右边加在一起，就是优化后的gc0值。
注意，b1的方向是由gc0的测量值的方向与[1,0,0]作叉乘得到的，b2的方向是由gc0的测量值的方向与b1作叉乘得到的。
这样，6.1.4公式得到了更新，此时带优化量减少了1维：
即 ，同样使用LDLT分解：
其实，在这里，我们可以发现这一部分做的工作和6.1.4是相似的，但是却是在6.1.4的基础上进一步做的工作。
6.1.6 利用gc0和gw确定世界坐标系
(1) 找到 c0 到 w 系的旋转矩阵 Rwc0 = exp([θu])
gc0已经求出来了，而gw一直就是一个已知的量，因此它们之间的夹角θ是能很快求出来的；
所以我们然后可以用gc0和gw作叉乘得到一个旋转轴u；
最后把c0坐标系，绕着转轴旋转一个θ，就能找到c0 到 w 系的对齐关系，也就是Rwc0 = exp([θu])。
(2) 把所有 c0 坐标系下的变量旋转到 w系下
所有量都乘上Rwc0就可以了。我们定义的c0 与 w 系的原点坐标是重合的。
(3) 把相机平移和特征点尺度恢复到米制单位
初始化大功告成！