VIO 初始化系列2---VINS-Mono初始化

参考论文:VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
Robust initialization of monocular visual-inertial estimation on aerial robots
参考博客:VINS-FUSION源码框架及C++知识点总结
参考课程:从0开始手写VIO
在参考博客里有介绍过初始化部分的大致流程,趁着现在要做VIO方面的项目, 再来仔细梳理该初始化过程。深蓝学院的课件里对此有详细的内容,所以本博客主要引用课件内容。
VIO 初始化系列2---VINS-Mono初始化_第1张图片
估计相机-陀螺仪旋转外参
CalibrationExRotition: 该步骤估计相机与陀螺仪之间的外参,类似于手眼标定的过程, 就是求解一个AX=XB线性问题。
VIO 初始化系列2---VINS-Mono初始化_第2张图片
VIO 初始化系列2---VINS-Mono初始化_第3张图片
在此没有估计平移外参, 因为默认为该外参对系统的影响是线性的, 可以通过离线标定, 出场设定或者尺子测量得出。在VINS-Mono中如果设定要标定外参, 但该平移外参也会在初始化阶段最后的Full BA优化中进行优化。

估计陀螺仪bias
solveGyroscopeBias()
VIO 初始化系列2---VINS-Mono初始化_第4张图片
代码中构建了对应的最小二乘问题,没有使用求解器, 直接用高斯牛顿一步迭代求解出该bias, 初始化过程中滑窗内的陀螺仪bias都是认为相同的。

初始化速度、重力和尺度因子
非线性部分的陀螺仪bias已经在之前解决好,剩下的估计问题就是一个线性问题了。VIO 初始化系列2---VINS-Mono初始化_第5张图片
VIO 初始化系列2---VINS-Mono初始化_第6张图片
VIO 初始化系列2---VINS-Mono初始化_第7张图片
该线性求解过程中会求出重力在c0系下的值gc0 , 通过该值的模长与实际重力大小9.81的误差是否大于0.5(也就是约5%的重力值),来判断此次初始化是否成功。
优化重力向量
为何采用该方法?可否假设估计出来的gc0重力方向有效,赋予模长后重新估计? 也就是将下面的w1, w2强制设为0。或者在优化的时候加入模长限制?
VIO 初始化系列2---VINS-Mono初始化_第8张图片VIO 初始化系列2---VINS-Mono初始化_第9张图片
将相机坐标系对齐世界坐标系
VIO 初始化系列2---VINS-Mono初始化_第10张图片

你可能感兴趣的:(VINS,算法,状态估计)