VINS-Mono-VIO初始化 (二:旋转外参初始化)

第k时刻到k+1时刻,IMU预积分有 q b k b k + 1 q_{b_{k}b_{k+1}} qbkbk+1 这个b代表在body系下面,相机有 q c k c k + 1 q_{c_{k}c_{k+1}} qckck+1(对极约束求解的)
各自乘上 q c b q_{cb} qcb ,因为相机到 IMU 的旋转外参不会变化的,公式变成
q c b ⊕ q b k b k + 1 q_{cb}⊕q_{b_{k}b_{k+1}} qcbqbkbk+1= q c k c k + 1 ⊕ q c b q_{c_{k}c_{k+1}}⊕q_{cb} qckck+1qcb
q c b q_{cb} qcb 的下标可以当作 q c k b k q_{c_{k}b_{k}} qckbk q c k + 1 b k + 1 q_{c_{k+1}b_{k+1}} qck+1bk+1 ,等式中一乘进去就等于 q c k b k + 1 q_{c_{k}b_{k+1}} qckbk+1

通过之前把四元数乘法变成矩阵乘法的法则进行变换,则有
[ q b k b k + 1 ] R ⋅ q c b = [ q c k c k + 1 ] L ⋅ q c b [q_{b_{k}b_{k+1}}]_{R}·q_{cb}=[q_{c_{k}c_{k+1}}]_{L}·q_{cb} [qbkbk+1]Rqcb=[qckck+1]Lqcb
=> R ⋅ q c b = L ⋅ q c b R·q_{cb}=L·q_{cb} Rqcb=Lqcb
挪到一边
=> ( R − L ) ⋅ q c b = 0 (R-L)·q_{cb}=0 (RL)qcb=0
这个 ( R − L ) (R-L) (RL) 会有很多组,则变成
[ ( 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} (RkLk)(Rk+1Lk+1)(Rk+2Lk+2)... · q c b = 0 q_{cb}=0 qcb=0
每一行都是 4×4 的矩阵,总共为 4(N+1)×4行, q c b q_{cb} qcb 为 4×1
左边的矩阵可以定为 A 矩阵,问题则变成 Ax=0 ,A为超定方程, A ∈ N × 4 , x ∈ 4 × 1 A∈N×4,x∈4×1 AN×4x4×1,这里只能求得最小二乘解

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

= > A x = U D V T ⋅ x = 0 ,左奇异向量 U ∈ N × N ,奇异值 D ∈ N × 4 ,右奇异向量 V ∈ 4 × 4 =>Ax=UDV^{T}·x=0,左奇异向量U∈N×N,奇异值D∈N×4,右奇异向量V∈4×4 =>Ax=UDVTx=0,左奇异向量UN×N,奇异值DN×4,右奇异向量V4×4 U U U V V V 也是正交矩阵
问题变成 ∣ ∣ U D V T ⋅ x ∣ ∣ m i n ||UDV^{T}·x||_{min} ∣∣UDVTxmin,即 x x x 为什么的时候左边的 ∣ ∣ U D V T ∣ ∣ ||UDV^{T}|| ∣∣UDVT∣∣ 为最小的

这个 U U U 和旋转矩阵 R R R 类似,是个正交矩阵且模为1,所以 ∣ ∣ U ⋅ x ∣ ∣ = x ||U·x||=x ∣∣Ux∣∣=x,因为旋转矩阵不会改变大小,只会改变向量的方向
这样则问题等价成 = > ∣ ∣ D V T ⋅ x ∣ ∣ m i n =>||DV^{T}·x||_{min} =>∣∣DVTxmin,这里的 V V V 也是正交矩阵, V T V^{T} VT 也是正交矩阵,只是旋转的方向相反,和上面同理,这里定义 V T ⋅ x = y V^{T}·x=y VTx=y ,把这一块一起设置成 y y y
则问题等价成 = > ∣ ∣ D y ∣ ∣ m i n =>||Dy||_{min} =>∣∣Dymin ,这里还有一个约束,由于 x x x 是四元数,所以 x x x 的模为1, ∣ ∣ x ∣ ∣ = 1 ||x||=1 ∣∣x∣∣=1,所以 y y y 的模也为1 , ∣ ∣ y ∣ ∣ = 1 ||y||=1 ∣∣y∣∣=1
D D 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} σ00000...剩下都是00σ1000...00σ200...000σ30...
两个矩阵相乘 [ σ 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} σ000000σ100000σ200000σ30 y0y1y2y3 ,则问题变成 Σ i = 1 3 σ i y i Σ^{3}_{i=1}σ_{i}y_{i} Σi=13σiyi ,现在要的就是这个累加值最小,由于奇异值矩阵里面 σ 3 σ_{3} σ3 是最小的,所以当 y = [ 0 , 0 , 0 , 1 ] T y=[0,0,0,1]^{T} 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 x x 是多少, V = [ a 0 ⃗ , a 1 ⃗ , a 2 ⃗ , a 3 ⃗ ] T ∈ 4 × 4 V=[\vec{a_{0}},\vec{a_{1}},\vec{a_{2}},\vec{a_{3}}]^{T}∈4×4 V=[a0 ,a1 ,a2 ,a3 ]T4×4,其中每个向量 ∣ ∣ a ⃗ ∣ ∣ = 1 ||\vec{a}||=1 ∣∣a ∣∣=1,且任何两个旋转向量的点乘都为0, a 0 ⃗ ⋅ a 1 ⃗ = 0 \vec{a_{0}}·\vec{a_{1}}=0 a0 a1 =0,每个向量之间是正交的, x x x 模为1的, V V V 的向量模也为1,假设 x = a 3 ⃗ x=\vec{a_{3}} x=a3 ,由于向量间正交 a 0 ⃗ ⋅ a 3 ⃗ = 0 , a 1 ⃗ ⋅ a 3 ⃗ = 0 , a 2 ⃗ ⋅ a 3 ⃗ = 0 , a 3 ⃗ ⋅ a 3 ⃗ = 1 \vec{a_{0}}·\vec{a_{3}}=0,\vec{a_{1}}·\vec{a_{3}}=0,\vec{a_{2}}·\vec{a_{3}}=0,\vec{a_{3}}·\vec{a_{3}}=1 a0 a3 =0a1 a3 =0a2 a3 =0a3 a3 =1
因为要满足 [ 0 0 0 1 ] \begin{bmatrix} 0\\ 0\\ 0\\ 1\\ \end{bmatrix} 0001 这样的形式才是最小值,所以当 x x x 等于右奇异矩阵的最后一个向量 a 3 ⃗ \vec{a_{3}} a3 时整体最小,公式为 x = V ⋅ y x=V·y x=Vy
所以代码中直接取右奇异矩阵的最后一个向量作为结果。

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

下面这句代码是用 q c b ⊕ q b k b k + 1 q_{cb}⊕q_{b_{k}b_{k+1}} qcbqbkbk+1= q c k c k + 1 ⊕ q c b q_{c_{k}c_{k+1}}⊕q_{cb} qckck+1qcb 这个公式进行变换的,里面的 ric 等于 q b c q_{bc} qbc q b c − 1 = q c b q^{-1}_{bc}=q_{cb} qbc1=qcb
q b c − 1 ⋅ q b k b k + 1 ⋅ q b c = q c b ⋅ q b k b k + 1 ⋅ q b c = q c k c k + 1 q^{-1}_{bc}·q_{b_{k}b_{k+1}}·q_{bc}=q_{cb}·q_{b_{k}b_{k+1}}·q_{bc}=q_{c_{k}c_{k+1}} qbc1qbkbk+1qbc=qcbqbkbk+1qbc=qckck+1
这样做的目的是把 IMU 的旋转变换到相机坐标系下面,然后后面通过角度差来进行判断

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

你可能感兴趣的:(VINS系列专栏,数码相机,线性代数,自动驾驶,算法,矩阵,c++)