VINS的初始化

首先，明确在系统初始化阶段可以利用的已知量，以及待求解的未知量。

已知：1.各帧相对于第1帧的位姿 $q_{c_1{c}_i},p_{c_1{c}_i}$；（根据SfM求得）
2.各相邻帧之间的位姿变化 $q_{b_k{b}_{k+1}},p_{b_k{b}_{k+1}}$；（根据IMU的预积分求得）

待求解：1.相机和IMU之间的外参 $q_{bc},p_{bc}$；
2.传感器的偏置$b$；
3.初始速度 $v$，尺度 $s$，重力矢量 $g^{c_1}$；
4.计算初始时刻IMU系相对于世界坐标系的位姿 $q_{w{b}_1}$，。

---

1.相机和IMU之间的外参

几何约束：
对于任意相邻两个时刻，$t_{k+1}$时刻的相机坐标系在$t_k$时刻的IMU系下的表示，都有两种表示路径（如图蓝线和绿线）即：
$$q_{b_k{c}_{k+1}}=q_{b_k{c}_k}\otimes q_{c_k{c}_{k+1}}=q_{b_k{b}_{k+1}}\otimes q_{b_{k+1}{c}_{k+1}}$$

【注意】相机和IMU之间的外参 $q_{bc}$ 可以认为是不变化的！有：
$$\begin{array}{rl}{q}_{bc}\otimes q_{c_k{c}_{k+1}}& =q_{b_k{b}_{k+1}}\otimes q_{bc}\\ [q_{c_k{c}_{k+1}}{]}_R{q}_{bc}& =[q_{b_k{b}_{k+1}}{]}_L{q}_{bc}\\ ([q_{c_k{c}_{k+1}}{]}_R-[q_{b_k{b}_{k+1}}{]}_L)q_{bc}& =0\\ Q_{k+1}^k{q}_{bc}& =0\end{array}$$

$q_{c_k{c}_{k+1}},q_{b_k{b}_{k+1}}$是已知量，则$Q_{k+1}^k$可计算得到。

将多个时刻的线性方程累积起来，得到：
$$\left[\begin{array}{c}{Q}_1^0\\ Q_2^1\\ ⋮\\ Q_N^{N-1}\end{array}\right]q_{bc}=Q_N\cdot q_{bc}=0$$

如果加上鲁棒权重则有：
$$\left[\begin{array}{c}{w}_1^0\cdot Q_1^0\\ w_2^1\cdot Q_2^1\\ ⋮\\ w_N^{N-1}\cdot Q_N^{N-1}\end{array}\right]q_{bc}=Q_N\cdot q_{bc}=0$$

而权重计算的依据为两条路径的接近程度：即$\Delta R\triangleq {\hat{R}}_{bc}^{-1}{R}_{b_k{b}_{k+1}}^{-1}{\hat{R}}_{bc}{R}_{c_k{c}_{k+1}}$与单位阵$I$的接近程度。
这里使用旋转轴与旋转角来衡量（单位阵对应的旋转角为0，如果$\Delta R$对应的旋转角越大，认为越不可靠，将其对应的项非常小的权重），即：
$$\begin{array}{rl}{r}_{k+1}^k& =\arccos (tr(\Delta R)-1)/2\\ w_{k+1}^k& =\{\begin{array}{ll}1& ,r_{k+1}^k<threshold\\ \frac{threshold}{r_{k+1}^k}& ,otherwise\end{array}\end{array}$$

至此，求解$Q_N$进行SVD分解后最小奇异值对应的奇异向量，即可求出 $q_{bc}$。

---

2.传感器的偏置 $b$

几何约束：
当$q_{bc}$已知，$q_{b_k{b}_{k+1}}$既可以由IMU预积分得到，又可以通过相邻两帧相机位姿结合(IMU及相机之间的外参)得到，即：
$$q_{b_k{b}_{k+1}}=q_{c_1{b}_k}^{-1}\otimes q_{c_1{b}_{k+1}}$$

但是，由于传感器偏置的存在，二者不会完全相等！优化如下目标函数：
$$\underset{\delta b_g}{\min }\Vert \lfloor q_{c_1{b}_{k+1}}^{-1}\otimes q_{c_1{b}_k}\otimes q_{b_k{b}_{k+1}}{\rfloor }_{xyz}{\Vert }^2$$

迭代过程中需要求残差项关于$\delta b_g$的雅可比矩阵（实际上仅 $q_{b_k{b}_{k+1}}$与之有关），即：
$$q_{b_k{b}_{k+1}}\approx {\hat{q}}_{b_k{b}_{k+1}}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_g}^q\delta b_g\end{array}\right]$$

求解最小二乘问题，只需完成两件事！1.明确残差函数的雅可比；2.构造Hessian矩阵。

---

3.初始速度 $v$，尺度 $s$，重力矢量 $g^{c_1}$

几何约束：位移矢量将满足
$$s{\bar{p}}_{c_1{b}_k}=s{\bar{p}}_{c_1{c}_k}-R_{c_1{b}_k}{p}_{bc}\text{\hspace{1em}(1)}$$

IMU预积分的约束（只需将之前的世界坐标系换作此时的$c_1$系）：
$$\begin{array}{rl}{\alpha }_{b_k{b}_{k+1}}& =R_{b_k{c}_1}(p_{c_1{b}_{k+1}}-p_{c_1{b}_k}-v_k^{c_1}+\frac{1}{2}{g}^{c_1}\Delta t^2)\\ {\beta }_{b_k{b}_{k+1}}& =R_{b_k{c}_1}(v_{b_{k+1}}^{c_1}-v_{b_k}^{c_1}+g^{c_1}\Delta t)\end{array}\text{\hspace{1em}(2)}$$

将公式（1）代入公式（2）有：
$$\begin{array}{rl}{\alpha }_{b_k{b}_{k+1}}& =R_{b_k{c}_1}(s({\bar{p}}_{c_1{b}_{k+1}}-{\bar{p}}_{c_1{b}_k})-R_{c_1{b}_k}{v}_k^{b_k}\Delta t+\frac{1}{2}{g}^{c_1}\Delta t^2)\\ & =R_{b_k{c}_1}(s{\bar{p}}_{c_1{b}_{k+1}}-s{\bar{p}}_{c_1{b}_k}-R_{c_1{b}_k}{v}_k^{b_k}\Delta t+\frac{1}{2}{g}^{c_1}\Delta t^2)\\ & =R_{b_k{c}_1}[(s{\bar{p}}_{c_1{c}_{k+1}}-R_{c_1{b}_{k+1}}{p}_{bc})-(s{\bar{p}}_{c_1{c}_k}-R_{c_1{b}_k}{p}_{bc})-R_{c_1{b}_k}{v}_k^{b_k}\Delta t+\frac{1}{2}{g}^{c_1}\Delta t^2]\\ & =s{R}_{b_k{c}_1}({\bar{p}}_{c_1{c}_{k+1}}-{\bar{p}}_{c_1{c}_k})-R_{b_k{c}_1}{R}_{c_1{b}_{k+1}}{p}_{bc}+p_{bc}-v_k^{b_k}\Delta t+\frac{1}{2}{R}_{b_k{c}_1}{g}^{c_1}\Delta t^2\\ {\beta }_{b_k{b}_{k+1}}& =R_{b_k{c}_1}(R_{c_1{b}_{k+1}}{v}_{k+1}^{b_{k+1}}-R_{c_1{b}_k}{v}_k^{b_k}+g^{c_1}\Delta t)\\ & =R_{b_k{c}_1}{R}_{c_1{b}_{k+1}}{v}_{k+1}^{b_{k+1}}-v_k^{b_k}+R_{b_k{c}_1}{g}^{c_1}\Delta t\end{array}\text{\hspace{1em}(2)}$$

和待估计量${\mathrm{X}}_I^k=[v_k^{b_k},v_{k+1}^{b_{k+1}},g^{c_1},s]$相关的放到右边，整理得：
$$\begin{array}{rl}{\alpha }_{b_k{b}_{k+1}}+R_{b_k{c}_1}{R}_{c_1{b}_{k+1}}{p}_{bc}-p_{bc}& =-v_k^{b_k}\Delta t+\frac{1}{2}{R}_{b_k{c}_1}{g}^{c_1}\Delta t^2+s{R}_{b_k{c}_1}({\bar{p}}_{c_1{c}_{k+1}}-{\bar{p}}_{c_1{c}_k})\\ {\beta }_{b_k{b}_{k+1}}& =-v_k^{b_k}+R_{b_k{c}_1}{R}_{c_1{b}_{k+1}}{v}_{k+1}^{b_{k+1}}+R_{b_k{c}_1}{g}^{c_1}\Delta t\end{array}\text{\hspace{1em}(3)}$$

写成矩阵形式：
$$\begin{array}{rl}\left[\begin{array}{c}{\hat{\alpha }}_{b_k{b}_{k+1}}+R_{b_k{c}_1}{R}_{c_1{b}_{k+1}}{p}_{bc}-p_{bc}\\ {\hat{\beta }}_{b_k{b}_{k+1}}\end{array}\right]& =\left[\begin{array}{cccc}-I\Delta t& 0& \frac{1}{2}{R}_{b_k{c}_1}\Delta t^2& R_{b_k{c}_1}({\bar{p}}_{c_1{c}_{k+1}}-{\bar{p}}_{c_1{c}_k})\\ -I& R_{b_k{c}_1}{R}_{c_1{b}_{k+1}}& R_{b_k{c}_1}\Delta t& 0\end{array}\right]\left[\begin{array}{c}{v}_k^{b_k}\\ v_{k+1}^{b_{k+1}}\\ g^{c_1}\\ s\end{array}\right]\\ {\hat{Z}}_{b_{k+1}}^{b_k}& =H_{b_{k+1}}^{b_k}{\mathrm{X}}_I^k\end{array}$$

即可转化为线性最小二乘问题求解：
$$\underset{{\mathrm{X}}_I^k}{\min }\Vert {\hat{Z}}_{b_{k+1}}^{b_k}-H_{b_{k+1}}^{b_k}{\mathrm{X}}_I^k{\Vert }^2$$