VINS-Mono-VIO初始化 （二：旋转外参初始化）

第k时刻到k+1时刻，IMU预积分有 $q_{b_k{b}_{k+1}}$ 这个b代表在body系下面，相机有 $q_{c_k{c}_{k+1}}$（对极约束求解的）
各自乘上 $q_{cb}$ ，因为相机到 IMU 的旋转外参不会变化的，公式变成
$q_{cb}\oplus q_{b_k{b}_{k+1}}$= $q_{c_k{c}_{k+1}}\oplus q_{cb}$
$q_{cb}$ 的下标可以当作 $q_{c_k{b}_k}$ 和 $q_{c_{k+1}{b}_{k+1}}$ ，等式中一乘进去就等于 $q_{c_k{b}_{k+1}}$

通过之前把四元数乘法变成矩阵乘法的法则进行变换，则有
$[q_{b_k{b}_{k+1}}{]}_R\cdot q_{cb}=[q_{c_k{c}_{k+1}}{]}_L\cdot q_{cb}$
=> $R\cdot q_{cb}=L\cdot q_{cb}$
挪到一边
=> $(R-L)\cdot q_{cb}=0$
这个 $(R-L)$ 会有很多组，则变成
$\left[\begin{array}{c}(R_k-L_k)\\ (R_{k+1}-L_{k+1})\\ (R_{k+2}-L_{k+2})\\ ...\end{array}\right]$·$q_{cb}=0$
每一行都是 4×4 的矩阵，总共为 4(N+1)×4行，$q_{cb}$ 为 4×1
左边的矩阵可以定为 A 矩阵，问题则变成 Ax=0 ，A为超定方程，$A\in N\times 4，x\in 4\times 1$，这里只能求得最小二乘解

求解超定方程最小二乘解的最常用方法是对 A 矩阵进行 svd 分解
代码中的SVD分解是直接调用库函数，具体如何分解可以看这篇文章 奇异值分解（SVD）方法求解最小二乘问题

$=>Ax=UD{V}^T\cdot x=0，左奇异向量U\in N\times N，奇异值D\in N\times 4，右奇异向量V\in 4\times 4$， $U$ 和 $V$ 也是正交矩阵
问题变成$||UD{V}^T\cdot x|{|}_{min}$，即 $x$ 为什么的时候左边的 $||UD{V}^T||$ 为最小的

这个 $U$ 和旋转矩阵 $R$ 类似，是个正交矩阵且模为1，所以 $||U\cdot x||=x$，因为旋转矩阵不会改变大小，只会改变向量的方向
这样则问题等价成 $=>||D{V}^T\cdot x|{|}_{min}$，这里的 $V$ 也是正交矩阵，$V^T$ 也是正交矩阵，只是旋转的方向相反，和上面同理，这里定义 $V^T\cdot x=y$ ，把这一块一起设置成 $y$
则问题等价成 $=>||Dy|{|}_{min}$ ，这里还有一个约束，由于 $x$ 是四元数，所以 $x$ 的模为1， $||x||=1$，所以 $y$ 的模也为1 ，$||y||=1$
$D$ 奇异值矩阵为对角矩阵，但不是严格的N×N的对角，具体形式为$\left[\begin{array}{cccc}{\sigma }_0& 0& 0& 0\\ 0& {\sigma }_1& 0& 0\\ 0& 0& {\sigma }_2& 0\\ 0& 0& 0& {\sigma }_3\\ 0& 0& 0& 0\\ ...& ...& ...& ...\\ 剩下都是0& & & \end{array}\right]$
两个矩阵相乘$\left[\begin{array}{cccc}{\sigma }_0& 0& 0& 0\\ 0& {\sigma }_1& 0& 0\\ 0& 0& {\sigma }_2& 0\\ 0& 0& 0& {\sigma }_3\\ 0& 0& 0& 0\end{array}\right]\cdot \left[\begin{array}{c}{y}_0\\ y_1\\ y_2\\ y_3\end{array}\right]$，则问题变成 ${\Sigma }_{i=1}^3{\sigma }_i{y}_i$ ，现在要的就是这个累加值最小，由于奇异值矩阵里面 ${\sigma }_3$ 是最小的，所以当 $y=[0,0,0,1{]}^T$ 时整体累加值最小，所以问题变成 $V^{T}x=\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]$ 时整体最小时求解 $x$ 是多少，$V=[\overrightarrow{a_0},\overrightarrow{a_1},\overrightarrow{a_2},\overrightarrow{a_3}{]}^T\in 4\times 4$，其中每个向量 $||\vec{a}||=1$，且任何两个旋转向量的点乘都为0，$\overrightarrow{a_0}\cdot \overrightarrow{a_1}=0$，每个向量之间是正交的， $x$ 模为1的，$V$ 的向量模也为1，假设 $x=\overrightarrow{a_3}$ ，由于向量间正交 $\overrightarrow{a_0}\cdot \overrightarrow{a_3}=0，\overrightarrow{a_1}\cdot \overrightarrow{a_3}=0，\overrightarrow{a_2}\cdot \overrightarrow{a_3}=0，\overrightarrow{a_3}\cdot \overrightarrow{a_3}=1$
因为要满足$\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]$这样的形式才是最小值，所以当 $x$ 等于右奇异矩阵的最后一个向量 $\overrightarrow{a_3}$ 时整体最小，公式为$x=V\cdot y$。
所以代码中直接取右奇异矩阵的最后一个向量作为结果。

最后结果还要检查前三个奇异值，比0大才是有效的，最后一个奇异值一般在0附近，检查的前三个奇异值都接近0的话就证明可能在0空间附近运动，证明激励不足够

下面这句代码是用 $q_{cb}\oplus q_{b_k{b}_{k+1}}$= $q_{c_k{c}_{k+1}}\oplus q_{cb}$ 这个公式进行变换的，里面的 ric 等于 $q_{bc}$ ，$q_{bc}^{-1}=q_{cb}$
$q_{bc}^{-1}\cdot q_{b_k{b}_{k+1}}\cdot q_{bc}=q_{cb}\cdot q_{b_k{b}_{k+1}}\cdot q_{bc}=q_{c_k{c}_{k+1}}$
这样做的目的是把 IMU 的旋转变换到相机坐标系下面，然后后面通过角度差来进行判断

// 通过外参把imu的旋转转移到相机坐标系
// 这里的 ric 是 qbc
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);  // ric是上一次求解得到的外参