• VINS-Mono代码分析与总结(二) 系统初始化


    Vins-Mono完整版总结:https://www.zybuluo.com/Xiaobuyi/note/866099

    参考文献

    1 VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen (techincal report)
    2 Solà J. Quaternion kinematics for the error-state KF[M]// Surface and Interface Analysis. 2015.
    3 Solà J. Yang Z, Shen S. Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration[J]. IEEE Transactions on Automation Science & Engineering, 2017, 14(1):39-51.
    4 Engel J, Schöps T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[C]//European Conference on Computer Vision. Springer, Cham, 2014:834-849.
    5 Shen S, Michael N, Kumar V. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C]// IEEE International Conference on Robotics and Automation. IEEE, 2015:5303-5310.
    6 Shen S, Mulgaonkar Y, Michael N, et al. Initialization-Free Monocular Visual-Inertial State Estimation with Application to Autonomous MAVs[M]// Experimental Robotics. Springer International Publishing, 2016.
    7 Sibley G. A Sliding Window Filter for SLAM[J]. University of Southern California, 2006.
    8 CamOdoCal: https://github.com/hengli/camodocal


    3 系统初始化

      在提取的图像的Features和做完IMU的预积分之后,进入了系统的初始化环节,那么系统为什么要进行初始化呢,主要的目的有以下两个:

    • 系统使用单目相机,需要恢复尺度;
    • 要对IMU进行初始化,IMU会受到bias的影响,所以要得到IMU的bias(只估计了陀螺仪的Bias);
    • VIO的一些初始状态量未知。

    所以我们要从初始化中恢复出尺度、重力、速度以及IMU的bias,因为视觉(SFM)在初始化的过程中有着较好的表现,所以在初始化的过程中主要以SFM为主,然后将IMU的预积分结果与其对齐,即可得到较好的初始化结果。
      系统的初始化主要包括三个环节:求取相机与IMU之间的相对旋转、相机初始化(局部滑窗内的SFM,包括没有尺度的BA)、IMU与视觉的对齐(IMU预积分中的 (alpha)等和相机的translation)。

    3.1 相机与IMU之间的相对旋转

      这个地方相当于求取相机与IMU的一部分外参。相机与IMU之间的旋转标定非常重要,偏差1-2°系统的精度就会变的极低。这部分的内容参考文献[3]中Ⅴ-A部分,这里做简单的总结。
      设相机利用对极关系得到的旋转矩阵为 (R^{c_{k}}_{c_{k+1}}),IMU经过预积分得到的旋转矩阵为(R^{b_{k}}_{b_{k+1}}),相机与IMU之间的相对旋转为 (R^{b}_{c}),则对于任一帧满足,

    [R^{b_{k}}_{b_{k+1}}R^{b}_{c}=R^{b}_{c}R^{c_{k}}_{c_{k+1}} ag{3.1} ]

    对式(3.1)可以做简单的证明,在其两边同乘 (^{c}x_{k+1})

    [egin{align} onumber R^{b_{k}}_{b_{k+1}}R^{b}_{c}{^{c}x_{k+1}}&=R^{b}_{c}R^{c_{k}}_{c_{k+1}}{^{c}x_{k+1}} \ onumber R^{b_{k}}_{b_{k+1}}{^{b}x_{k+1}}&=R^{b}_{c}{^{c}x_{k}} \ onumber ^{b}x_{k}&=^{b}x_{k} end{align} ]

    将旋转矩阵写为四元数,则式(3.1)可以写为

    [q^{b_{k}}_{b{k+1}}otimes q^{b}_{c}=q^{b}_{c}otimes q^{c_{k}}_{c{k+1}} ]

    将其写为左乘和右乘的形式,综合为

    [[Q_{1}(q^{b_{k}}_{b{k+1}})-Q_{2}(q^{c_{k}}_{c{k+1}})]q^{b}_{c}=Q^{k}_{k+1}q^{b}_{c}=0 ag{3.2} ]

    其中 (Q_{1}(q^{b_{k}}_{b{k+1}}))(Q_{2}(q^{c_{k}}_{c{k+1}})) 分别表示四元数的左乘和右乘形式,

    [egin{align} onumber Q_{1}(q)&=egin{bmatrix} q_{w}I_{3}+[q_{xyz }]_{ imes} & q_{xyz}\ onumber -q_{xyz} & q_{w} end{bmatrix} \ Q_{2}(q)&=egin{bmatrix} q_{w}I_{3}-[q_{xyz }]_{ imes} & q_{xyz}\ onumber -q_{xyz} & q_{w} end{bmatrix} end{align} ag{3.3} ]

    这个地方因为四元数的实部在前在后的表达不一样,而左右乘的形式也不一样,所以文献[2]和文献[3]存在区别。Vins-Mono使用的是Eigen实部在后的表达方式。
    那么对于 (n)对测量值,则有

    [egin{bmatrix} w^{0}_{1}Q^{0}_{1}\ w^{1}_{2}Q^{1}_{2}\ vdots \ w^{N-1}_{N}Q^{N-1}_{N} end{bmatrix}q^{b}_{c}=Q_{N}q^{b}_{c}=0 ag{3.4} ]

    其中 (w^{N-1}_{N}) 为外点剔除权重,其与相对旋转求取得残差有关,(N)为最新的视觉帧的index,其由最终的终止条件决定。残差可以写为,

    [r^{k}_{k+1}=acos((tr(hat{R}^{b^{-1}}_{c}R^{b_{k}^{-1}}_{b_{k+1}}hat{R}^{b}_{c}R^{c_{k}}_{c_{k+1}} )-1)/2) ag{3.5} ]

    残差还是很好理解的,在具体的代码中可以计算公式(3.1)两边两个旋转的得角度差。在得到残差之后就可以进一步得到公式(3.4)中的权重,

    [w^{k}_{k+1}=left{egin{matrix} 1,qquad r^{k}_{k+1}<threshold\ frac{threshold}{r^{k}_{k+1}},qquad otherwise end{matrix} ight. ag{3.6} ]

    一般会将阈值 (threshold) 取做 (5°)。至此,就可以通过求解方程(3.4)得到相对旋转,式(3.4)的解为 (Q_{N}) 的左奇异向量中最小奇异值对应的特征向量。
      但是,在这里还要注意 求解的终止条件(校准完成的终止条件) 。在足够多的旋转运动中,我们可以很好的估计出相对旋转 (R^{b}_{c}),这时 (Q_{N}) 对应一个准确解,且其零空间的秩为1。但是在校准的过程中,某些轴向上可能存在退化运动(如匀速运动),这时 (Q_{N}) 的零空间的秩会大于1。判断条件就是 (Q_{N}) 的第二小的奇异值是否大于某个阈值,若大于则其零空间的秩为1,反之秩大于1,相对旋转(R^{b}_{c}) 的精度不够,校准不成功。

    3.2 相机初始化

      这一阶段的思路就是单目相机的初始化过程,先求取本质矩阵求解位姿,进而三角化特征点,然后PnP求解位姿,不断重复的过程,直到恢复出滑窗内的Features和相机位姿,代码比较清晰。要注意的就是坐标系的和位姿的变换,容易混乱。如以下几个函数:
    triangulateTwoFrames :输入是相机外参(世界到相机),求解出的3D点是在世界坐标系下。
    cv::solvePnP:该API输入是世界坐标系的点,求解出的是世界坐标系到相机坐标系的变换,所以一般需要将结果转置。

    3.3 视觉与IMU对齐

      视觉与IMU的对齐主要解决三个问题:
      (1) 修正陀螺仪的bias;
      (2) 初始化速度、重力向量 (g)和尺度因子(Metric scale);
      (3) 改进重力向量 (g)的量值;

    3.3.1 陀螺仪Bias修正

      发现校正部分使用的都是一系列的约束条件,思路很重要啊。陀螺仪Bias校正的时候也是使用了一个简单的约束条件:

    [underset{delta b_{w}}{min}sum_{kin B}^{ }left | q^{c_{0}^{-1}}_{b_{k+1}}otimes q^{c_{0}}_{b_{k}}otimesgamma _{b_{k+1}}^{b_{k}} ight |^{2} ag{3.7} ]

    其中

    [gamma _{b_{k+1}}^{b_{k}}approx hat{gamma}_{b_{k+1}}^{b_{k}}otimes egin{bmatrix} 1\ frac{1}{2}J^{gamma }_{b_{w}}delta b_{w} end{bmatrix} ag{3.8} ]

    公式(3.7)的最小值为单位四元数 ([1,0_{v}]^{T}) ,所以将式(3.7)进一步写为,

    [egin{align} onumber q^{c_{0}^{-1}}_{b_{k+1}}otimes q^{c_{0}}_{b_{k}}otimesgamma _{b_{k+1}}^{b_{k}}&=egin{bmatrix} 1\ 0 end{bmatrix} \ onumber hat{gamma}_{b_{k+1}}^{b_{k}}otimes egin{bmatrix} 1\ frac{1}{2}J^{gamma }_{b_{w}}delta b_{w} end{bmatrix}&=q^{c_{0}^{-1}}_{b_{k}}otimes q^{c_{0}}_{b_{k+1}} \ end{align} ]

    [egin{bmatrix} 1\ frac{1}{2}J^{gamma }_{b_{w}}delta b_{w} end{bmatrix}=hat{gamma}_{b_{k+1}}^{b_{k}^{-1}}otimes q^{c_{0}^{-1}}_{b_{k}}otimes q^{c_{0}}_{b_{k+1}} ag{3.9} ]

    只取式(3.9)式虚部,在进行最小二乘求解

    [J^{gamma^{T}}_{b_{w}}J^{gamma }_{b_{w}}delta b_{w}=2*J^{gamma^{T}}_{b_{w}}(hat{gamma}_{b_{k+1}}^{b_{k}^{-1}}otimes q^{c_{0}^{-1}}_{b_{k}}otimes q^{c_{0}}_{b_{k+1}}).vec ag{3.10} ]

    求解式(3.10)的最小二乘解,即可得到 (delta b_{w}),注意这个地方得到的只是Bias的变化量,需要在滑窗内累加得到Bias的准确值。

    3.3.2 初始化速度、重力向量 (g)和尺度因子

      在这个步骤中,要估计系统的速度、重力向量以及尺度因子。所以系统的状态量可以写为,

    [X_{I}=[v^{c_{0}}_{b_{0}},v^{c_{0}}_{b_{1}},cdots ,g^{c_{0}},s] ag{3.11} ]

    上面的状态量都是在 (c_{0}) 相机坐标系下。接着,有前面的由预积分部分,IMU的测量模型可知

    [egin{align} onumber alpha^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(s(ar{p}^{c_{0}}_{b_{k+1}}-ar{p}^{c_{0}}_{b_{k}})+frac{1}{2}g^{c_{0}} riangle t_{k}^{2}-v^{c_{0}}_{b_{k}} riangle t_{k}^{2}) \ onumber eta ^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(v^{c_{0}}_{b_{k+1}}+g^{c_{0}} riangle t_{k}-v^{c_{0}}_{b_{k}}) end{align} ag{3.12} ]

    在3.1小节,我们已经得到了IMU相对于相机的旋转 (q_{b}^{c}),假设IMU到相机的平移量(p_{b}^{c}) 那么可以很容易地将相机坐标系下的位姿转换到IMU坐标系下,

    [egin{align} onumber q_{b_{k}}^{c_{0}} &= q^{c_{0}}_{c_{k}}otimes q_{b}^{c} \ onumber sar{p}^{c_{0}}_{b_{k}}&=sar{p}^{c_{0}}_{c_{k}}+q^{c_{0}}_{c_{k}}p_{b}^{c} end{align} ag{3.13} ]

    综合式(3.12)和式(3.13)可得,

    [egin{align} onumber hat{z}^{b_{k}}_{b_{k+1}}&=egin{bmatrix} alpha^{b_{k}}_{b_{k+1}}-q^{c_{0}}_{c_{k+1}}p^{c}_{b}+q^{c_{0}}_{c_{k}}p^{c}_{b}&\ onumber eta ^{b_{k}}_{b_{k+1}} end{bmatrix}=H^{b_{k}}_{b_{k+1}}X_{I}+n^{b_{k}}_{b_{k+1}} \ onumber &approx egin{bmatrix} -q^{b_{k}}_{c_{0}} riangle t_{k} &0& 1/2q^{b_{k}}_{c_{0}} riangle t_{k}^{2} &q^{b_{k}}_{c_{0}}(ar{p}^{c_{0}}_{b_{k+1}}-ar{p}^{c_{0}}_{b_{k}}) \ -q^{b_{k}}_{c_{0}}& q^{b_{k}}_{c_{0}} &q^{b_{k}}_{c_{0}} riangle t_{k} & 0 end{bmatrix}egin{bmatrix} v^{c_{0}}_{b_{k}}\ v^{c_{0}}_{b_{k+!}}\ g^{c_{0}}\ s end{bmatrix} end{align} ag{3.14} ]

    这一步的推导也比较简单,以(H)矩阵的第一行,将式(3.13)的(2)式带入(3.12)的(1)式可得

    [egin{align} onumber alpha^{b_{k}}_{b_{k+1}}&=q^{b_{k}}_{c_{0}}(s(ar{p}^{c_{0}}_{c_{k+1}}-ar{p}^{c_{0}}_{c_{k}}+(q_{c_{k+1}}^{c_{0}}-q_{c_{k}}^{c_{0}})p^{b}_{c}+frac{1}{2}g^{c_{0}} riangle t_{k}^{2}-v^{c_{0}}_{b_{k}} riangle t_{k}^{2}) \ onumber alpha^{b_{k}}_{b_{k+1}}&-q^{b_{k}}_{c_{0}}(q_{c_{k+1}}^{c_{0}}-q_{c_{k}}^{c_{0}})p^{b}_{c}=q^{b_{k}}_{c_{0}}(s(ar{p}^{c_{0}}_{c_{k+1}}-ar{p}^{c_{0}}_{c_{k}}p^{b}_{c})+frac{1}{2}g^{c_{0}} riangle t_{k}^{2}-v^{c_{0}}_{b_{k}} riangle t_{k}^{2}) end{align} ]

    但是不知道为什么论文,在做了近似之后漏了上式等式左边的(q^{b_{k}}_{c_{0}})
    最后求解最小二乘问题

    [underset{delta b_{w}}{min}sum_{kin B}^{ }left | hat{z}^{b_{k}}_{b_{k+1}}-H^{b_{k}}_{b_{k+1}}X_{I} ight |^{2} ]

    至此即可求解出所有状态量,但是对于重力向量 (g^{c_{0}}) 还要做进一步的纠正。在纠正(g^{c_{0}}) 的过程中,会对速度也做进一步的优化。

    3.3.3 纠正重力向量

      这部分和上一小节的内容差不多,就是迭代不断更新重力向量的过程,要注意重力向量被表达为:

    [hat{g} = gcdot hat{ar{g}} + omega_{1}b_{1}+omega_{2}b_{2} ]

    VINS-Mono代码注释:https://github.com/gaochq/VINS-Mono/tree/comment
    注释不完整,可以一起交流。

  • 相关阅读:
    算法之路——插入排序篇3:希尔排序
    对enum的探讨
    算法之路——插入排序篇1
    算法之路——快速排序
    算法之路——插入排序篇2
    圆的角度DDA算法初试
    解决omnicppcomplete显示"pattern not found"
    汇编
    直线的dda算法
    equ定义的是符号
  • 原文地址:https://www.cnblogs.com/buxiaoyi/p/8203285.html
Copyright © 2020-2023  润新知