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)$ 会有很多组，则变成
[ ( R k − L k ) ( R k + 1 − L k + 1 ) ( R k + 2 − L k + 2 ) . . . ] \begin{bmatrix} (R_{k}-L_{k}) \\ (R_{k+1}-L_{k+1})\\ (R_{k+2}-L_{k+2})\\ ... \end{bmatrix} ​(Rk​−Lk​)(Rk+1​−Lk+1​)(Rk+2​−Lk+2​)...​ ​·$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的对角，具体形式为 [ σ 0 0 0 0 0 σ 1 0 0 0 0 σ 2 0 0 0 0 σ 3 0 0 0 0 . . . . . . . . . . . . 剩下都是 0 ] \begin{bmatrix} σ_{0}&0&0&0\\ 0&σ_{1}&0&0\\ 0&0&σ_{2}&0\\ 0&0&0&σ_{3}\\ 0&0&0&0\\ ...&...&...&...\\ 剩下都是0 \end{bmatrix} ​σ0​0000...剩下都是0​0σ1​000...​00σ2​00...​000σ3​0...​ ​
两个矩阵相乘 [ σ 0 0 0 0 0 σ 1 0 0 0 0 σ 2 0 0 0 0 σ 3 0 0 0 0 ] ⋅ [ y 0 y 1 y 2 y 3 ] \begin{bmatrix} σ_{0}&0&0&0\\ 0&σ_{1}&0&0\\ 0&0&σ_{2}&0\\ 0&0&0&σ_{3}\\ 0&0&0&0\\ \end{bmatrix}·\begin{bmatrix} y_{0}\\ y_{1}\\ y_{2}\\ y_{3}\\ \end{bmatrix} ​σ0​0000​0σ1​000​00σ2​00​000σ3​0​ ​⋅ ​y0​y1​y2​y3​​ ​，则问题变成 ${\Sigma }_{i=1}^3{\sigma }_i{y}_i$ ，现在要的就是这个累加值最小，由于奇异值矩阵里面 ${\sigma }_3$ 是最小的，所以当 $y=[0,0,0,1{]}^T$ 时整体累加值最小，所以问题变成 V T x = [ 0 0 0 1 ] V^{T}x=\begin{bmatrix} 0\\ 0\\ 0\\ 1\\ \end{bmatrix} VTx= ​0001​ ​ 时整体最小时求解 $x$ 是多少，$V=[a_0,a_1,a_2,a_3{]}^T\in 4\times 4$，其中每个向量 $||a||=1$，且任何两个旋转向量的点乘都为0，$a_0\cdot a_1=0$，每个向量之间是正交的， $x$ 模为1的，$V$ 的向量模也为1，假设 $x=a_3$ ，由于向量间正交 $a_0\cdot a_3=0，a_1\cdot a_3=0，a_2\cdot a_3=0，a_3\cdot a_3=1$
因为要满足 [ 0 0 0 1 ] \begin{bmatrix} 0\\ 0\\ 0\\ 1\\ \end{bmatrix} ​0001​ ​这样的形式才是最小值，所以当 $x$ 等于右奇异矩阵的最后一个向量 $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是上一次求解得到的外参