VINS-Mono代码注释：https://github.com/gaochq/VINS-Mono/tree/comment
注释不完整，可以一起交流。
参考文献
1 VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen (techincal report)
2 Solà J. Quaternion kinematics for the error-state KF[M]// Surface and Interface Analysis. 2015.
3 Solà J. Yang Z, Shen S. Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration[J]. IEEE Transactions on Automation Science & Engineering, 2017, 14(1):39-51.
4 Engel J, Sch?ps T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[C]//European Conference on Computer Vision. Springer, Cham, 2014:834-849.
5 Shen S, Michael N, Kumar V. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C]// IEEE International Conference on Robotics and Automation. IEEE, 2015:5303-5310.
6 Shen S, Mulgaonkar Y, Michael N, et al. Initialization-Free Monocular Visual-Inertial State Estimation with Application to Autonomous MAVs[M]// Experimental Robotics. Springer International Publishing, 2016.
7 Sibley G. A Sliding Window Filter for SLAM[J]. University of Southern California, 2006.
8 CamOdoCal: https://github.com/hengli/camodocal
****
3 系统初始化
??在提取的图像的Features和做完IMU的预积分之后，进入了系统的初始化环节，那么系统为什么要进行初始化呢，主要的目的有以下两个：
- 系统使用单目相机，需要恢复尺度；
- 要对IMU进行初始化，IMU会受到bias的影响，所以要得到IMU的bias(只估计了陀螺仪的Bias);
- VIO的一些初始状态量未知。
所以我们要从初始化中恢复出尺度、重力、速度以及IMU的bias，因为视觉(SFM)在初始化的过程中有着较好的表现，所以在初始化的过程中主要以SFM为主，然后将IMU的预积分结果与其对齐，即可得到较好的初始化结果。
??系统的初始化主要包括三个环节：求取相机与IMU之间的相对旋转、相机初始化(局部滑窗内的SFM，包括没有尺度的BA)、IMU与视觉的对齐(IMU预积分中的 \(\alpha\)等和相机的translation)。
3.1 相机与IMU之间的相对旋转
??这个地方相当于求取相机与IMU的一部分外参。相机与IMU之间的旋转标定非常重要，偏差1-2°系统的精度就会变的极低。这部分的内容参考文献[3]中Ⅴ-A部分，这里做简单的总结。
??设相机利用对极关系得到的旋转矩阵为 \(R^{c_{k}}_{c_{k+1}}\)，IMU经过预积分得到的旋转矩阵为\(R^{b_{k}}_{b_{k+1}}\)，相机与IMU之间的相对旋转为 \(R^{b}_{c}\)，则对于任一帧满足，
\[
R^{b_{k}}_{b_{k+1}}R^{b}_{c}=R^{b}_{c}R^{c_{k}}_{c_{k+1}}
\tag{3.1}
\]
对式(3.1)可以做简单的证明，在其两边同乘 \(^{c}x_{k+1}\) 得
\[
\begin{align}\nonumber
R^{b_{k}}_{b_{k+1}}R^{b}_{c}{^{c}x_{k+1}}&=R^{b}_{c}R^{c_{k}}_{c_{k+1}}{^{c}x_{k+1}} \\\nonumber
R^{b_{k}}_{b_{k+1}}{^{b}x_{k+1}}&=R^{b}_{c}{^{c}x_{k}}
\\\nonumber
^{b}x_{k}&=^{b}x_{k}
\end{align}
\]
将旋转矩阵写为四元数，则式(3.1)可以写为
\[
q^{b_{k}}_{b{k+1}}\otimes q^{b}_{c}=q^{b}_{c}\otimes q^{c_{k}}_{c{k+1}}
\]
将其写为左乘和右乘的形式，综合为
\[
[Q_{1}(q^{b_{k}}_{b{k+1}})-Q_{2}(q^{c_{k}}_{c{k+1}})]q^{b}_{c}=Q^{k}_{k+1}q^{b}_{c}=0
\tag{3.2}
\]
其中 \(Q_{1}(q^{b_{k}}_{b{k+1}})\)，\(Q_{2}(q^{c_{k}}_{c{k+1}})\) 分别表示四元数的左乘和右乘形式，
\[
\begin{align}\nonumber
Q_{1}(q)&=\begin{bmatrix}
q_{w}I_{3}+[q_{xyz }]_{\times} & q_{xyz}\\\nonumber
-q_{xyz} & q_{w}
\end{bmatrix} \Q_{2}(q)&=\begin{bmatrix}
q_{w}I_{3}-[q_{xyz }]_{\times} & q_{xyz}\\\nonumber
-q_{xyz} & q_{w}
\end{bmatrix}
\end{align}
\tag{3.3}
\]
这个地方因为四元数的实部在前在后的表达不一样，而左右乘的形式也不一样，所以文献[2]和文献[3]存在区别。Vins-Mono使用的是Eigen实部在后的表达方式。
那么对于 \(n\)对测量值，则有
\[
\begin{bmatrix}
w^{0}_{1}Q^{0}_{1}\w^{1}_{2}Q^{1}_{2}\\vdots \w^{N-1}_{N}Q^{N-1}_{N}
\end{bmatrix}q^{b}_{c}=Q_{N}q^{b}_{c}=0
\tag{3.4}
\]
其中 \(w^{N-1}_{N}\) 为外点剔除权重，其与相对旋转求取得残差有关，\(N\)为最新的视觉帧的index，其由最终的终止条件决定。残差可以写为，
\[
r^{k}_{k+1}=acos((tr(\hat{R}^{b^{-1}}_{c}R^{b_{k}^{-1}}_{b_{k+1}}\hat{R}^{b}_{c}R^{c_{k}}_{c_{k+1}} )-1)/2)
\tag{3.5}
\]
残差还是很好理解的，在具体的代码中可以计算公式(3.1)两边两个旋转的得角度差。在得到残差之后就可以进一步得到公式(3.4)中的权重，
\[
w^{k}_{k+1}=\left\{\begin{matrix}
1,\qquad r^{k}_{k+1}<threshold\\frac{threshold}{r^{k}_{k+1}},\qquad otherwise
\end{matrix}\right.
\tag{3.6}
\]
一般会将阈值 \(threshold\) 取做 \(5°\)。至此，就可以通过求解方程(3.4)得到相对旋转，式(3.4)的解为 \(Q_{N}\) 的左奇异向量中最小奇异值对应的特征向量。
??但是，在这里还要注意 求解的终止条件(校准完成的终止条件) 。在足够多的旋转运动中，我们可以很好的估计出相对旋转 \(R^{b}_{c}\)，这时 \(Q_{N}\) 对应一个准确解，且其零空间的秩为1。但是在校准的过程中，某些轴向上可能存在退化运动(如匀速运动)，这时 \(Q_{N}\) 的零空间的秩会大于1。判断条件就是 \(Q_{N}\) 的第二小的奇异值是否大于某个阈值，若大于则其零空间的秩为1，反之秩大于1，相对旋转\(R^{b}_{c}\) 的精度不够，校准不成功。
3.2 相机初始化
??这一阶段的思路就是单目相机的初始化过程，先求取本质矩阵求解位姿，进而三角化特征点，然后PnP求解位姿，不断重复的过程，直到恢复出滑窗内的Features和相机位姿，代码比较清晰。要注意的就是坐标系的和位姿的变换，容易混乱。如以下几个函数：
triangulateTwoFrames ：输入是相机外参(世界到相机)，求解出的3D点是在世界坐标系下。
cv::solvePnP：该API输入是世界坐标系的点，求解出的是世界坐标系到相机坐标系的变换，所以一般需要将结果转置。
3.3 视觉与IMU对齐
??视觉与IMU的对齐主要解决三个问题：
??(1) 修正陀螺仪的bias；
??(2) 初始化速度、重力向量 \(g\)和尺度因子(Metric scale)；
??(3) 改进重力向量 \(g\)的量值；
3.3.1 陀螺仪Bias修正
??发现校正部分使用的都是一系列的约束条件，思路很重要啊。陀螺仪Bias校正的时候也是使用了一个简单的约束条件：
\[
\underset{\delta b_{w}}{min}\sum_{k\in B}^{ }\left \| q^{c_{0}^{-1}}_{b_{k+1}}\otimes q^{c_{0}}_{b_{k}}\otimes\gamma _{b_{k+1}}^{b_{k}} \right \|^{2}
\tag{3.7}
\]
其中
\[
\gamma _{b_{k+1}}^{b_{k}}\approx \hat{\gamma}_{b_{k+1}}^{b_{k}}\otimes \begin{bmatrix}
1\\frac{1}{2}J^{\gamma }_{b_{w}}\delta b_{w}
\end{bmatrix}
\tag{3.8}
\]
公式(3.7)的最小值为单位四元数 \([1,0_{v}]^{T}\) ,所以将式(3.7)进一步写为，
\[
\begin{align}\nonumber
q^{c_{0}^{-1}}_{b_{k+1}}\otimes q^{c_{0}}_{b_{k}}\otimes\gamma _{b_{k+1}}^{b_{k}}&=\begin{bmatrix}
1\0
\end{bmatrix} \\\nonumber
\hat{\gamma}_{b_{k+1}}^{b_{k}}\otimes \begin{bmatrix}
1\\frac{1}{2}J^{\gamma }_{b_{w}}\delta b_{w}
\end{bmatrix}&=q^{c_{0}^{-1}}_{b_{k}}\otimes q^{c_{0}}_{b_{k+1}} \\end{align}
\]
\[
\begin{bmatrix}
1\\frac{1}{2}J^{\gamma }_{b_{w}}\delta b_{w}
\end{bmatrix}=\hat{\gamma}_{b_{k+1}}^{b_{k}^{-1}}\otimes q^{c_{0}^{-1}}_{b_{k}}\otimes q^{c_{0}}_{b_{k+1}}
\tag{3.9}
\]
只取式(3.9)式虚部，在进行最小二乘求解
\[
J^{\gamma^{T}}_{b_{w}}J^{\gamma }_{b_{w}}\delta b_{w}=2*J^{\gamma^{T}}_{b_{w}}(\hat{\gamma}_{b_{k+1}}^{b_{k}^{-1}}\otimes q^{c_{0}^{-1}}_{b_{k}}\otimes q^{c_{0}}_{b_{k+1}}).vec
\tag{3.10}
\]
求解式(3.10)的最小二乘解，即可得到 \(\delta b_{w}\)，注意这个地方得到的只是Bias的变化量，需要在滑窗内累加得到Bias的准确值。
3.3.2 初始化速度、重力向量 \(g\)和尺度因子
??在这个步骤中，要估计系统的速度、重力向量以及尺度因子。所以系统的状态量可以写为，
\[
X_{I}=[v^{c_{0}}_{b_{0}},v^{c_{0}}_{b_{1}},\cdots ,g^{c_{0}},s]
\tag{3.11}
\]
上面的状态量都是在 \(c_{0}\) 相机坐标系下。接着，有前面的由预积分部分，IMU的测量模型可知
\[
\begin{align}\nonumber
\alpha^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(s(\bar{p}^{c_{0}}_{b_{k+1}}-\bar{p}^{c_{0}}_{b_{k}})+\frac{1}{2}g^{c_{0}}\triangle t_{k}^{2}-v^{c_{0}}_{b_{k}}\triangle t_{k}^{2}) \\\nonumber
\beta ^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(v^{c_{0}}_{b_{k+1}}+g^{c_{0}}\triangle t_{k}-v^{c_{0}}_{b_{k}})
\end{align}
\tag{3.12}
\]
在3.1小节，我们已经得到了IMU相对于相机的旋转 \(q_{b}^{c}\),假设IMU到相机的平移量\(p_{b}^{c}\) 那么可以很容易地将相机坐标系下的位姿转换到IMU坐标系下，
\[
\begin{align}\nonumber
q_{b_{k}}^{c_{0}} &= q^{c_{0}}_{c_{k}}\otimes q_{b}^{c} \\\nonumber
s\bar{p}^{c_{0}}_{b_{k}}&=s\bar{p}^{c_{0}}_{c_{k}}+q^{c_{0}}_{c_{k}}p_{b}^{c}
\end{align}
\tag{3.13}
\]
综合式(3.12)和式(3.13)可得，
\[
\begin{align}\nonumber
\hat{z}^{b_{k}}_{b_{k+1}}&=\begin{bmatrix}
\alpha^{b_{k}}_{b_{k+1}}-q^{c_{0}}_{c_{k+1}}p^{c}_{b}+q^{c_{0}}_{c_{k}}p^{c}_{b}&\\\nonumber
\beta ^{b_{k}}_{b_{k+1}}
\end{bmatrix}=H^{b_{k}}_{b_{k+1}}X_{I}+n^{b_{k}}_{b_{k+1}} \\\nonumber
&\approx \begin{bmatrix}
-q^{b_{k}}_{c_{0}}\triangle t_{k} &0& 1/2q^{b_{k}}_{c_{0}}\triangle t_{k}^{2} &q^{b_{k}}_{c_{0}}(\bar{p}^{c_{0}}_{b_{k+1}}-\bar{p}^{c_{0}}_{b_{k}}) \ -q^{b_{k}}_{c_{0}}& q^{b_{k}}_{c_{0}} &q^{b_{k}}_{c_{0}}\triangle t_{k} & 0
\end{bmatrix}\begin{bmatrix}
v^{c_{0}}_{b_{k}}\v^{c_{0}}_{b_{k+!}}\g^{c_{0}}\s
\end{bmatrix}
\end{align}
\tag{3.14}
\]
这一步的推导也比较简单，以\(H\)矩阵的第一行，将式(3.13)的(2)式带入(3.12)的(1)式可得
\[
\begin{align} \nonumber
\alpha^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(s(\bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}}+(q_{c_{k+1}}^{c_{0}}-q_{c_{k}}^{c_{0}})p^{b}_{c}+\frac{1}{2}g^{c_{0}}\triangle t_{k}^{2}-v^{c_{0}}_{b_{k}}\triangle t_{k}^{2}) \\ \nonumber
\alpha^{b_{k}}_{b_{k+1}}&-q^{b_{k}}_{c_{0}}(q_{c_{k+1}}^{c_{0}}-q_{c_{k}}^{c_{0}})p^{b}_{c}=q^{b_{k}}_{c_{0}}(s(\bar{p}^{c_{0}}_{c_{k+1}}-\bar{p}^{c_{0}}_{c_{k}}p^{b}_{c})+\frac{1}{2}g^{c_{0}}\triangle t_{k}^{2}-v^{c_{0}}_{b_{k}}\triangle t_{k}^{2})
\end{align}
\]
但是不知道为什么论文，在做了近似之后漏了上式等式左边的\(q^{b_{k}}_{c_{0}}\)。
最后求解最小二乘问题
\[
\underset{\delta b_{w}}{min}\sum_{k\in B}^{ }\left \|
\hat{z}^{b_{k}}_{b_{k+1}}-H^{b_{k}}_{b_{k+1}}X_{I}
\right \|^{2}
\]
至此即可求解出所有状态量，但是对于重力向量 \(g^{c_{0}}\) 还要做进一步的纠正。在纠正\(g^{c_{0}}\) 的过程中，会对速度也做进一步的优化。
3.3.3 纠正重力向量
??这部分和上一小节的内容差不多，就是迭代不断更新重力向量的过程，要注意重力向量被表达为：
\[
\hat{g} = g\cdot \hat{\bar{g}} + \omega_{1}b_{1}+\omega_{2}b_{2}
\]
