VIO第7讲:VINS初始化与VINS系统
文章目录
1 VINS初始化
VINS
初始化采用视觉和 IMU 的松耦合方案,首先用 SFM
求解滑窗内所有帧的位姿,和所有路标点的 3D 位置,然后跟 IMU 预积分的值对齐,求解重力方向、尺度因子、陀螺仪 bias
及每一帧对应的速度。初始化的流程图如下所示:
1.1 视觉初始化
1.1.1 relativePose
relativePose
:字面意思即相对位姿,说明这里求解的并不是世界系下绝对位姿。在滑窗内寻找与当前帧的匹配特征点数较多的关键帧作为参考帧,并通过求基础矩阵
cv::findFundamentalMat
计算出当前帧到参考帧的姿态T_cr
;
1.1.2 GlobalSFM与BA优化
参考帧为上一步选出来的最共视帧,最后得到的sfm_tracked_points
为三角化出来的 3D 路标点。
用一个图来表示
1.1.3 visualInitialAlign
纯视觉初始化时,我们采用第一帧 c0
作为基准坐标系。类似于纯视觉SLAM中,以第一帧作为世界系,后续所有姿态以其为基准。
1.2 VisualIMUAlignment
视觉惯性对齐,如何对齐?
考虑以下几个问题
1 IMU怎么和世界坐标系对齐,计算初始时刻的 q w b 0 q_{wb_{0}} qwb0?
2 单目视觉姿态如何和IMU轨迹对齐,尺度如何获取?
3 VIO系统的初始速度v,传感器
bias
等如何估计?
4 IMU和相机之间的外参?
1.2.1 视觉和IMU之间的联系
考虑相机坐标系 c 0 c_{0} c0为世界坐标系,利用外参可以构建下面的等式(VINS
论文):
q c 0 b k = q c 0 c k ⊗ q b c − 1 s p ‾ c 0 b k = s p ‾ c 0 c k − R c 0 b k p b c \begin{aligned}\mathbf{q}_{c_0b_k}&=\mathbf{q}_{c_0c_k}\otimes\mathbf{q}_{bc}^{-1}\\s\overline{\mathbf{p}}_{c_0b_k}&=s\overline{\mathbf{p}}_{c_0c_k}-\mathbf{R}_{c_0b_k}\mathbf{p}_{bc}\end{aligned} qc0bkspc0bk=qc0ck⊗qbc−1=spc0ck−Rc0bkpbc
旋转关系是显而易见的,主要写下关于平移。上面式子中s
是视觉与IMU尺度转换因子。单目视觉没有一个比较准确的尺度,所以所有计算的距离等都是一个相对量! p ‾ \overline{\mathbf{p}} p表示非米制单位的轨迹,即视觉下表示平移的量。
为什么会有这么一个尺度转换,这个是因为 p b c \mathbf{p}_{bc} pbc是相机与IMU平移量的外参,是一个可以确定的米制量!还有一点, p b c \mathbf{p}_{bc} pbc是IMU
坐标系下的量,我们把 c 0 c_{0} c0看作世界系,所以在坐标变换中需要将向量 p b c \mathbf{p}_{bc} pbc转换到世界系! 参考下面绘制图理解。
[ P ‾ c 0 c k 1 ] = [ R c 0 b k p c 0 b k → O 1 ] ⋅ [ 1 s P ‾ b c 1 ] \begin{bmatrix}\overline{{P}}_{c_0c_k}\\1\end{bmatrix}=\begin{bmatrix}R_{c_{0}b_{k}}&p_{c_{0}b_{k}}\\\rightarrow\\O&1\end{bmatrix}\cdot\begin{bmatrix}\frac1s\overline{{P}}_{bc}\\1\end{bmatrix} [Pc0ck1]=
Rc0bk→Opc0bk1
⋅[s1Pbc1]
上面的式子还可以变换为下面的形式,本质上都是一样的。总之,s
是一个非常关键的尺度转换量。
我们也能通过旋转矩阵来推导:
T c 0 ← b k = T c 0 ← c k T b ← c − 1 ⇔ T c 0 ← b k T b ← c = T c 0 ← c k ⇔ R c 0 ← b k ( R b ← c P c k + t b ← c ) + t c 0 ← b k = R c 0 ← c k P c k + t c 0 ← c k \begin{gathered} T_{c_{0}\leftarrow b_{k}}=T_{c_{0}\leftarrow c_{k}}T_{b\leftarrow c}{}^{-1} \\ \Leftrightarrow T_{c_{0}\leftarrow b_{k}}T_{b\leftarrow c}=T_{c_{0}\leftarrow c_{k}} \\ \Leftrightarrow R_{c_{0}\leftarrow b_{k}}\big(R_{b\leftarrow c}P_{c_{k}}+t_{b\leftarrow c}\big)+t_{c_{0}\leftarrow b_{k}}=R_{c_{0}\leftarrow c_{k}}P_{c_{k}}+t_{c_{0}\leftarrow c_{k}} \end{gathered} Tc0←bk=Tc0←ckTb←c−1⇔Tc0←bkTb←c=Tc0←ck⇔Rc0←bk(Rb←cPck+tb←c)+tc0←bk=Rc0←ckPck+tc0←ck
然后就可以得到上面的结果,这里t
就是上面的平移p
⟹ { R c 0 ← b k = R c 0 ← c k R b ← c − 1 t c 0 ← b k = t c 0 ← c k − R c 0 ← b k t b ← c \left.\implies\left\{\begin{matrix}R_{c_0\leftarrow b_k}=R_{c_0\leftarrow c_k}R_{b\leftarrow c}^{-1}\\t_{c_0\leftarrow b_k}=t_{c_0\leftarrow c_k}-R_{c_0\leftarrow b_k}t_{b\leftarrow c}\end{matrix}\right.\right. ⟹{Rc0←bk=Rc0←ckRb←c−1tc0←bk=tc0←ck−Rc0←bktb←c
1.2.2 视觉IMU对齐流程
估计外参----估计偏置bg
----估计相机系的重力 g c 0 g^{c0} gc0、尺度s、速度v----优化重力方向—将VIO
坐标系与世界系对齐
① 旋转约束估计外参 q b c q_{bc} qbc
initial_ex_rotation.cpp 函数 CalibrationExRotation()
这里的旋转约束估计是为了哪些不知道外参的情况,有的时候可以通过事先的标定去解决,如kalibar工具;或者说,买的机器人厂家已经给出了。
1 利用相同时间戳的两帧IMU和cam数据进行旋转约束(一个平行四边形的两条路径)
q b k b k + 1 ⊗ q b c = q b c ⊗ q c k c k + 1 \mathbf{q}_{b_kb_{k+1}}\otimes\mathbf{q}_{bc}=\mathbf{q}_{bc}\otimes\mathbf{q}_{c_kc_{k+1}} qbkbk+1⊗qbc=qbc⊗qckck+1
2 [ ⋅ ] L , [ ⋅ ] R \left[\cdot\right]_L,\left[\cdot\right]_R [⋅]L,[⋅]R是
left and right quaternion multiplication
,就是左右乘算子
( [ q b k b k + 1 ] L − [ q c k c k + 1 ] R ) q b c = Q k + 1 k ⋅ q b c = 0 \left(\begin{bmatrix}\mathbf{q}_{b_kb_{k+1}}\end{bmatrix}\right._L-\begin{bmatrix}\mathbf{q}_{c_kc_{k+1}}\end{bmatrix}_R)\mathbf{q}_{bc}=\mathbf{Q}_{k+1}^k\cdot\mathbf{q}_{bc}=\mathbf{0} ([qbkbk+1]L−[qckck+1]R)qbc=Qk+1k⋅qbc=0
3 如果累计多个时间段的旋转约束,并加上鲁棒核权重,很明显类似于三角化的一个最小二乘问题!
[ w 1 0 ⋅ Q 1 0 w 2 1 ⋅ Q 2 1 ⋮ w N N − 1 ⋅ Q N N − 1 ] q b c = Q N ⋅ q b c = 0 \left.\left[\begin{array}{c}w_1^0\cdot\mathbf{Q}_1^0\\w_2^1\cdot\mathbf{Q}_2^1\\\vdots\\w_N^{N-1}\cdot\mathbf{Q}_N^{N-1}\end{array}\right.\right]\mathbf{q}_{bc}=\mathbf{Q}_N\cdot\mathbf{q}_{bc}=\mathbf{0} w10⋅Q10w21⋅Q21⋮wNN−1⋅QNN−1 qbc=QN⋅qbc=0
其中
w k + 1 k = { 1 , r k + 1 k < threshold threshold r k + 1 k , otherwise \left.w_{k+1}^k=\left\{\begin{array}{cc}1,&\quad r_{k+1}^k<\text{threshold}\\\frac{\text{threshold}}{r_{k+1}^k},&\quad\text{otherwise}\end{array}\right.\right. wk+1k={1,rk+1kthreshold,rk+1k<thresholdotherwise
求解同样采用 SVD
分解,即最小奇异值对应的奇异向量,即V
的最后一列,也即 V T V^{T} VT的最后一行
Q N = D S V T \mathbf{Q}_N = DSV^{T} QN=DSVT
4 四元数实部表示了旋转角,虚部表示了旋转轴–$\operatorname{tr}(\mathbf{R})=1+2\cos\theta $,利用这个公式,下面式子理论上乘积应该为单位矩阵,对应角度为0,即没有旋转。所以利用角度误差也能够优化外参!
r k + 1 k = acos ( ( tr ( R ^ b c − 1 R b k b k + 1 − 1 R ^ b c R c k c k + 1 ) − 1 ) / 2 ) r_{k+1}^{k}=\operatorname{acos}\left(\left(\operatorname{tr}\left(\hat{\mathbf{R}}_{bc}^{-1}\mathbf{R}_{b_{k}b_{k+1}}^{-1}\hat{\mathbf{R}}_{bc}\mathbf{R}_{c_{k}c_{k+1}}\right)-1\right)/2\right) rk+1k=acos((tr(R^bc−1Rbkbk+1−1R^bcRckck+1)−1)/2)
补充:求迹tr()
即求矩阵的对角线元素之和。
② 陀螺仪 bias 校正—旋转约束
initial_aligment.cpp 函数 solveGyroscopeBias().
如果外参数 q b c q_{bc} qbc已标定好,利用旋转约束,可估计陀螺仪
bias
,约束:视觉给出的相邻帧间的旋转应该等于IMU预积分的旋转值
理论上下面旋转为0,应该对应一个单位阵(单位四元数)
a r g min δ b w ∑ k ∈ B ∥ 2 ⌊ q c 0 b k + 1 − 1 ⊗ q c 0 b k ⊗ q b k b k + 1 ⌋ x y z ∥ 2 arg\min_{\delta\mathbf{b}^w}\sum_{k\in B}\left\|2\left\lfloor\mathbf{q}_{c_0b_{k+1}}^{-1}\otimes\mathbf{q}_{c_0b_k}\otimes\mathbf{q}_{b_kb_{k+1}}\right\rfloor_{xyz}\right\|^2 argδbwmink∈B∑
2⌊qc0bk+1−1⊗qc0bk⊗qbkbk+1⌋xyz
2
其中, B 表示所有的图像关键帧集合,其中,更新偏置后的预积分的一阶泰勒近似:
q b k b k + 1 ≈ q ^ b k b k + 1 ⊗ [ 1 1 2 J b g q δ b g ] \mathbf{q}_{b_kb_{k+1}}\approx\hat{\mathbf{q}}_{b_kb_{k+1}}\otimes\begin{bmatrix}1\\\frac12\mathbf{J}_{b^g}^\mathbf{q}\delta\mathbf{b}^g\end{bmatrix} qbkbk+1≈q^bkbk+1⊗[121Jbgqδbg]
最终其实就是求解Ax=b的优化问题
③ 初始化速度、重力 g c 0 g^{c0} gc0和尺度因子—平移约束
initial_aligment.cpp 函数 LinearAlignment()
g c 0 \mathbf{g}^{c_0} gc0为重力向量在第 0 帧相机坐标系下的表示,s表示尺度因子,将视觉轨迹拉伸到米制单位, v k b k \mathbf{v}_k^{b_k} vkbk表示k时刻速度在body系下的表示!
X I = [ v 0 b 0 , v 1 b 1 , ⋯ v n b n , g c 0 , s ] ⊤ \mathcal{X}_I=\left[\mathbf{v}_0^{b_0},\mathbf{v}_1^{b_1},\cdots\mathbf{v}_n^{b_n},\mathbf{g}^{c_0},s\right]^\top XI=[v0b0,v1b1,⋯vnbn,gc0,s]⊤
前面预积分观测量(左)和状态预测值(右)的关系如下,w表示世界系,后续用 c 0 c_{0} c0表示
α b i b j = q b i w ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) β b i b j = q b i w ( v j w − v i w + g w Δ t ) \begin{aligned} &\boldsymbol{\alpha}_{b_ib_j} =\mathbf{q}_{b_iw}(\mathbf{p}_{wb_j}-\mathbf{p}_{wb_i}-\mathbf{v}_i^w\Delta t+\frac12\mathbf{g}^w\Delta t^2) \\ &\boldsymbol{\beta}_{b_ib_j} =\mathbf{q}_{b_iw}(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w\Delta t) \\ \end{aligned} αbibj=qbiw(pwbj−pwbi−viwΔt+21gwΔt2)βbibj=qbiw(vjw−viw+gwΔt)
要注意的一点就是,重力加速度 g w g^{w} gw是知道的,就是我们常知的9.8,但是我们一开始把第一帧 c 0 c_{0} c0表示世界系,这个坐标系并不一定与世界系相同,所以我们需要去估计 g c 0 \mathbf{g}^{c_0} gc0
将世界坐标系
w
换成相机初始时刻坐标系 c 0 c_{0} c0有(cam到imu存在尺度s关系)
α b k b k + 1 = R b k c 0 ( s ( p ‾ c 0 b k + 1 − p ‾ c 0 b k ) + 1 2 g c 0 Δ t k 2 − R c 0 b k v k b k Δ t k ) β b k b k + 1 = R b k c 0 ( R c 0 b k + 1 v k + 1 b k + 1 + g c 0 Δ t k − R c 0 b k v k b k ) \begin{aligned} &\boldsymbol{\alpha}_{b_{k}b_{k+1}} =\mathbf{R}_{b_kc_0}\left(s\left(\overline{\mathbf{p}}_{c_0b_{k+1}}-\overline{\mathbf{p}}_{c_0b_k}\right)+\frac{1}{2}\mathbf{g}^{c_0}\Delta t_k^2-\mathbf{R}_{c_0b_k}\mathbf{v}_k^{b_k}\Delta t_k\right) \\ &\beta_{b_kb_{k+1}} =\mathbf{R}_{b_kc_0}\left(\mathbf{R}_{c_0b_{k+1}}\mathbf{v}_{k+1}^{b_{k+1}}+\mathbf{g}^{c_0}\Delta t_k-\mathbf{R}_{c_0b_k}\mathbf{v}_k^{b_k}\right) \end{aligned} αbkbk+1=Rbkc0(s(pc0bk+1−pc0bk)+21gc0Δtk2−Rc0bkvkbkΔtk)βbkbk+1=Rbkc0(Rc0bk+1vk+1bk+1+gc0Δtk−Rc0bkvkbk)
把 q c 0 b k = q c 0 c k ⊗ q b c − 1 s p ‾ c 0 b k = s p ‾ c 0 c k − R c 0 b k p b c \begin{aligned}\mathbf{q}_{c_0b_k}&=\mathbf{q}_{c_0c_k}\otimes\mathbf{q}_{bc}^{-1}\\s\overline{\mathbf{p}}_{c_0b_k}&=s\overline{\mathbf{p}}_{c_0c_k}-\mathbf{R}_{c_0b_k}\mathbf{p}_{bc}\end{aligned} qc0bkspc0bk=qc0ck⊗qbc−1=spc0ck−Rc0bkpbc代入,然后就可以得到下面等式
α b k b k + 1 = s R b k c 0 ( p ‾ c 0 c k + 1 − p ‾ c 0 c k ) − R b k c 0 R c 0 b k + 1 p b c + p b c + 1 2 R b k c 0 g c 0 Δ t k 2 − v k b k Δ t k \begin{aligned} \boldsymbol{\alpha}_{b_kb_{k+1}}=& s\mathbf{R}_{b_kc_0}\left(\overline{\mathbf{p}}_{c_0c_{k+1}}-\overline{\mathbf{p}}_{c_0c_k}\right) \\ &-\mathbf{R}_{b_kc_0}\mathbf{R}_{c_0b_{k+1}}\mathbf{p}_{bc}+\mathbf{p}_{bc}+\frac12\mathbf{R}_{b_kc_0}\mathbf{g}^{c_0}\Delta t_k^2-\mathbf{v}_k^{b_k}\Delta t_k \end{aligned} αbkbk+1=sRbkc0(pc0ck+1−pc0ck)−Rbkc0Rc0bk+1pbc+pbc+21Rbkc0gc0Δtk2−vkbkΔtk
问题求解
④ 修正重力矢量
上面求解重力 g c 0 \mathbf{g}^{c_0} gc0的过程中,没有加入模长的限制 ∥ g c 0 ∥ = 9.81 \|\mathbf{g}^{c_0}\|=9.81 ∥gc0∥=9.81。一个三维变量,有了模长限制,说明它可以用确定两个数,就知道第三个值,也称重力 g c 0 \mathbf{g}^{c_0} gc0只有两个自由度。
⑤ 相机坐标系对齐世界坐标系
通过利用世界系 g w g^{w} gw与相机系的重力 g c 0 g^{c0} gc0,我们可以得到两个坐标系之间的旋转!因为旋转可以用角轴表示,所以只需要计算出旋转轴和旋转角即可!
这部分那个视频还是讲的挺好的!
2 VINS系统
VINS系统框架
VINS状态变量和残差
特别要注意的是先验残差,如何构建?如何更新?VIO第4讲提到过,基于一阶泰勒近似。
优化马氏距离,即带有了信息矩阵。但Ceres中只接收最小二乘(残差与雅可比),所以会把信息矩阵进行
LLT
分解,构建新的残差。
至于为什么信息矩阵通常放在海塞矩阵里面,这个十四讲第六讲很久之前就推导过了。