从零学习VINS-Mono/Fusion源代码(五):VIO初始化

本节分析VIO初始化部分

VINS-Mono/Fusion代码学习系列:
从零学习VINS-Mono/Fusion源代码(一):主函数
从零学习VINS-Mono/Fusion源代码(二):前端图像跟踪
从零学习VINS-Mono/Fusion源代码(三):IMU预积分公式推导
从零学习VINS-Mono/Fusion源代码(四):误差卡尔曼滤波
从零学习VINS-Mono/Fusion源代码(五):VIO初始化


从零学习VINS-Mono/Fusion源代码(五):VIO初始化

  • 1 为什么要做初始化?
  • 2 初始化流程
    • 2.1 参数读取
    • 2.2 回调函数
    • 2.3 Measurement_process{process} —— vio处理线程
      • 2.3.1 数据预处理
      • 2.3.2 VIO初始化任务
  • 3 VIO初始化实现
    • 3.1 特征点管理,检查是否是关键帧 addFeatureCheckParallax
    • 3.2 旋转外参初始化 CalibrationExRotation
    • 3.3 VIO初始化 initialStructure
      • 3.3.1 SFM部分
      • 3.3.2 视觉惯性对齐

1 为什么要做初始化?

提供初始值(良好的初始值,能够避免优化过程中系统陷入局部最小,减少迭代次数,降低计算量)

初始化的变量:
位姿、速度、零偏,硬件外参、地图点
从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第1张图片
其中,在没有先验条件的情况下,加速度计零偏和平移外参是比较难求解的,加计bias受到重力干扰,难以分离.
因此,平移外参一般是事先测量得到,加速度计零偏就设为0,在后续优化中进行处理.


2 初始化流程

estimator_node.cpp中找到主函数main(),主函数与光流部分类似,定义节点,设置参数,接收topic.
从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第2张图片

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vins_estimator");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    readParameters(n);
    estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    registerPub(n);

    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);//前端光流结果
    ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);//接收前端重启命令
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);//回环检测fast_relocalization响应

    std::thread measurement_process{process};
    ros::spin();
 
    return 0;
}

2.1 参数读取

使用readParameters(n)读取yaml文件中估计器迭代计算参数、IMU参数、旋转平移外参,以及时间延迟参数.

estimator.setParameter() 估计器外参预设
本质上就是一个外参的赋值过程,然后设置特征点的置信度,默认在虚拟相机下,特征点投影后的坐标差为1.5个像素.

void Estimator::setParameter()
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);

	//特征点置信度
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

2.2 回调函数

IMU_callback()
IMU回调函数主要完成两个任务:

  • 把imu消息存入buffer
  • 根据IMU频率预测并发送位姿,提高里程计频率(对应系统框图propagation)

消息存入buffer的过程中用到了进程锁,避免数据放入和取出发生访问冲突,具体概念参见:
[c++11]多线程编程(六)——条件变量(Condition Variable)

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();

    last_imu_t = imu_msg->header.stamp.toSec();

    {
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg);
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

feature_callback就是把前端光流数据丢进buffer
restart_callback做了一个状态器复位


2.3 Measurement_process{process} —— vio处理线程

从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第3张图片

2.3.1 数据预处理

  • getMeasurements()完成imu图像帧的时间同步(这个getMeasurements(),本质上就是把图像帧和imu数据分组,上一帧k到当前帧k+1之间的imu数据与当前图像帧k+1构成一组);
  • 之后,把imu送入estimator.processIMU做预积分,并且更新滑窗中的状态量Ps Vs Rs,给非线性优化提供更好的初值;
  • 光流结果(像素坐标、归一化坐标、速度)重新整理到7x1向量xyz_uv_velocity中去.

2.3.2 VIO初始化任务

estimator.processImage函数中完成了VIO初始化和后端优化求解,本节只讨论VIO初始化任务:

  1. 特征点管理,检查是否是关键帧 addFeatureCheckParallax
  2. 旋转外参初始化 CalibrationExRotation
  3. VIO初始化 initialStructure
    从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第4张图片

3 VIO初始化实现

3.1 特征点管理,检查是否是关键帧 addFeatureCheckParallax

把当前帧的特征点信息送入f_manager中进行维护,通过视差判断关键帧.

VINS特征点维护方式:
FeatureManager这个类中,有一项list< FeaturePerId > feature,用来维护下图中每一个特征点id对应的属性.
从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第5张图片

关键帧筛选条件:

  • 是第0帧或第1帧(frame_count<2)
  • 追踪到上一帧特征点数目少于20个(last_track_num<20)
  • 计算倒数第二帧和倒数第三帧之间的视差(本质是判断倒数第二帧是不是关键帧)compensatedParallax2
  • 与上一帧没有共视特征点(parallax_num==0)

如果倒数第二帧是关键帧,去掉最老帧;如果倒数第二帧不是关键帧,就把它去掉.


3.2 旋转外参初始化 CalibrationExRotation

只针对于 ESTIMATE_EXTRINSIC=2,才需要在初始化阶段计算旋转外参量.
思想就是利用k到k+n时刻中,各相邻两帧之间的旋转关系,构建超定方程求解.
这个旋转关系有两种途径得到:第一种是IMU预积分,第二种是特征点对极约束.

以k到k+1时刻为例,将外参 q c b q_{cb} qcb作为桥梁,可以构建下面这个等式:


q c b ⊗ q b k b k + 1 = q c k c k + 1 ⊗ q c b {q_{cb}} \otimes {q_{{b_k}{b_{k + 1}}}} = {q_{{c_k}{c_{k + 1}}}} \otimes {q_{cb}} qcbqbkbk+1=qckck+1qcb

[ q b k b k + 1 ] R q c b = [ q c k c k + 1 ] L q c b {\left[ {{q_{{b_k}{b_{k + 1}}}}} \right]_R}{q_{cb}} = {\left[ {{q_{{c_k}{c_{k + 1}}}}} \right]_L}{q_{cb}} [qbkbk+1]Rqcb=[qckck+1]Lqcb

对于k到k+n时刻:


( R k − L k ) q c b = 0 {\left( {{R_k} - {L_k}} \right){q_{cb}} = 0} (RkLk)qcb=0
( R k + 1 − L k + 1 ) q c b = 0 {\left( {{R_{k+1}} - {L_{k+1}}} \right){q_{cb}} = 0} (Rk+1Lk+1)qcb=0
⋮ \vdots
( R k + n − L k + n ) q c b = 0 {\left( {{R_{k+n}} - {L_{k+n}}} \right){q_{cb}} = 0} (Rk+nLk+n)qcb=0

然后就构建了Ax=0这样的问题,通过SVD奇异值分解来求解.


3.3 VIO初始化 initialStructure

initialStructure函数中完成的任务:检查imu能观性、SFM纯视觉三维重建、对所有帧pnp、视觉惯性对齐

3.3.1 SFM部分

假设滑窗中有11帧,先找一个枢纽帧(假设第4帧),要求它离最后一帧尽可能远(目的是避免纯旋转情况,便于求解t),通过对极约束求解枢纽帧和最后一帧之间的相对位姿;同时,距离太远会导致两帧共同观测到的特征点数目少,所以要做一个权衡。

然后,根据得到的位姿,通过三角化恢复观测到的特征点的3D世界坐标.

利用已经恢复的3D点,通过pnp(3D-2D)来恢复第5-9帧的图像帧相对位姿,在这个过程中也可以三角化出更多的3D点.

同样地,利用pnp求解第3-0帧的图像帧位姿.
从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第6张图片

然后,做一个global BA,来调整这些位姿和3D点.

3.3.2 视觉惯性对齐

  • solveGyroscopeBias求解陀螺零偏
    从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第7张图片

    理论上为单位四元数,取出虚部构造Ax=b问题,求解 δ b w \delta {b_w} δbw

从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第8张图片

  • LinearAlignment求解速度、尺度、重力方向

    论文中指出的待求解量:
    从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第9张图片
    构造的观测方程:
    从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第10张图片
    推导一下这个方程怎么来的:
    首先把式(5)换到 c 0 {c_0} c0系下面去(就是枢纽帧),然后带入式(14).
    从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第11张图片从零学习VINS-Mono/Fusion源代码(五):VIO初始化_第12张图片

    ①平移预积分量构成方程:

    R c 0 b k p b k + 1 c 0 = R c 0 b k ( p b k c 0 + v b k c 0 Δ t k − 1 2 g c 0 Δ t k 2 ) + α R_{{c_0}}^{{b_k}}p_{{b_{k + 1}}}^{{c_0}} = R_{{c_0}}^{{b_k}}(p_{{b_k}}^{{c_0}} +v_{{b_k}}^{{c_0}}\Delta {t_k} - \frac{1}{2}{g^{{c_0}}}\Delta t_k^2) + \alpha Rc0bkpbk+1c0=Rc0bk(pbkc0+vbkc0Δtk21gc0Δtk2)+α

    R c 0 b k p b k + 1 c 0 = R c 0 b k ( s p c k c 0 − R b k c 0 p b c + v b k c 0 Δ t k − 1 2 g c 0 Δ t k 2 ) + α R_{{c_0}}^{{b_k}}p_{{b_{k + 1}}}^{{c_0}} = R_{{c_0}}^{{b_k}}(sp_{{c_k}}^{{c_0}} - R_{{b_k}}^{{c_0}}p_b^c + v_{{b_k}}^{{c_0}}\Delta {t_k} - \frac{1}{2}{g^{{c_0}}}\Delta t_k^2) + \alpha Rc0bkpbk+1c0=Rc0bk(spckc0Rbkc0pbc+vbkc0Δtk21gc0Δtk2)+α

    R c 0 b k ( s p c k + 1 c 0 − R b k + 1 c 0 p b c ) = R c 0 b k ( s p c k c 0 − R b k c 0 p b c + v b k c 0 Δ t k − 1 2 g c 0 Δ t k 2 ) + α R_{{c_0}}^{{b_k}}(sp_{{c_{k + 1}}}^{{c_0}} - R_{{b_{k + 1}}}^{{c_0}}p_b^c) = R_{{c_0}}^{{b_k}}(sp_{{c_k}}^{{c_0}} - R_{{b_k}}^{{c_0}}p_b^c + v_{{b_k}}^{{c_0}}\Delta {t_k} - \frac{1}{2}{g^{{c_0}}}\Delta t_k^2) + \alpha Rc0bk(spck+1c0Rbk+1c0pbc)=Rc0bk(spckc0Rbkc0pbc+vbkc0Δtk21gc0Δtk2)+α

    α − R c 0 b k R b k c 0 p b c + R c 0 b k R b k + 1 c 0 p b c = R c 0 b k ( s p c k + 1 c 0 − s p c k c 0 − v b k c 0 Δ t k + 1 2 g c 0 Δ t k 2 ) \alpha - R_{{c_0}}^{{b_k}}R_{{b_k}}^{{c_0}}p_b^c + R_{{c_0}}^{{b_k}}R_{{b_{k + 1}}}^{{c_0}}p_b^c = R_{{c_0}}^{{b_k}}(sp_{{c_{k + 1}}}^{{c_0}} - sp_{{c_k}}^{{c_0}} - v_{{b_k}}^{{c_0}}\Delta {t_k} + \frac{1}{2}{g^{{c_0}}}\Delta t_k^2) αRc0bkRbkc0pbc+Rc0bkRbk+1c0pbc=Rc0bk(spck+1c0spckc0vbkc0Δtk+21gc0Δtk2)

    α − p b c + R c 0 b k R b k + 1 c 0 p b c = R c 0 b k ( s p c k + 1 c 0 − s p c k c 0 − v b k c 0 Δ t k + 1 2 g c 0 Δ t k 2 ) \alpha - p_b^c + R_{{c_0}}^{{b_k}}R_{{b_{k + 1}}}^{{c_0}}p_b^c = R_{{c_0}}^{{b_k}}(sp_{{c_{k + 1}}}^{{c_0}} - sp_{{c_k}}^{{c_0}} - v_{{b_k}}^{{c_0}}\Delta {t_k} + \frac{1}{2}{g^{{c_0}}}\Delta t_k^2) αpbc+Rc0bkRbk+1c0pbc=Rc0bk(spck+1c0spckc0vbkc0Δtk+21gc0Δtk2)

    ②旋转预积分量构成方程:

    R c 0 b k v b k + 1 c 0 = R c 0 b k ( v b k c 0 − g c 0 Δ t k ) + β R_{{c_0}}^{{b_k}}v_{{b_{k + 1}}}^{{c_0}} = R_{{c_0}}^{{b_k}}(v_{{b_k}}^{{c_0}} - {g^{{c_0}}}\Delta t_k^{}) + \beta Rc0bkvbk+1c0=Rc0bk(vbkc0gc0Δtk)+β

    R c 0 b k R b k + 1 c 0 v b k + 1 = R c 0 b k ( R b k c 0 v b k − g c 0 Δ t k ) + β R_{{c_0}}^{{b_k}}R_{{b_{k + 1}}}^{{c_0}}{v^{{b_{k + 1}}}} = R_{{c_0}}^{{b_k}}(R_{{b_k}}^{{c_0}}{v^{{b_k}}} - {g^{{c_0}}}\Delta t_k^{}) + \beta Rc0bkRbk+1c0vbk+1=Rc0bk(Rbkc0vbkgc0Δtk)+β

    β = R c 0 b k R b k + 1 c 0 v b k + 1 − v b k + R c 0 b k g c 0 Δ t k \beta = R_{{c_0}}^{{b_k}}R_{{b_{k + 1}}}^{{c_0}}{v^{{b_{k + 1}}}} - {v^{{b_k}}} + R_{{c_0}}^{{b_k}}{g^{{c_0}}}\Delta t_k^{} β=Rc0bkRbk+1c0vbk+1vbk+Rc0bkgc0Δtk

你可能感兴趣的:(计算机视觉)