Vins-Mono系列代码和理论解读＜三＞. vins初始化流程和代码实现细节

原创

已于 2022-07-18 11:19:48 修改 · 1.8k 阅读

24 ·

CC 4.0 BY-SA版权

文章标签：

#计算机视觉

于 2022-06-18 18:33:50 首次发布

本文详细介绍了VINS系统初始化的过程，包括视觉SLAM、IMU预积分、相机IMU外参估计、陀螺仪Bias、重力方向和速度的求解。初始化通过纯视觉SFM、视觉IMU对齐等步骤，最终得到系统状态，如位姿、速度和尺度。同时，文章阐述了系统在不同阶段的数据需求和更新，以及重力方向优化的参数化方法。

VINS系统初始化触发的目的

VINS系统初始化是估计Body坐标系下的重力方向，IMU陀螺仪和加速度计的偏置Bias，以及各帧速度，同时对齐视觉位姿，恢复视觉位姿的尺度。在初始化完成后，系统可以实时导出有真实Pose的位姿数据，并且可以在后端执行联合优化进行状态最优估计。

系统初始化触发机制

系统初始化需要满足4个条件:
a.当前求解模式为INITIAL；
b.滑动窗口内图像帧数目为WINDOWS_SIZE+1，这个算是系统层面的原因了，因为整个VINS后端就是要维护这个WINDOWS_SIZE+1的窗口的状态；
c.相机到IMU的外参旋转项参数已知(已经标定好)，否则无法建立视觉测量值和IMU预积分测量值之间的连接；
d.当前最新图像帧的时间戳距离系统上一次初始化的时间超过0.1s(时间不能太短，在一次初始化失败之后短时间内没必要继续初始化)。

    if (solver_flag == INITIAL)
    {
   
   
        // 确保有足够的frame参与初始化，有外参，且当前帧时间戳大于系统刚运行时时间戳+0.1秒
        // 凑满窗口数的图像帧进行初始化
        if (frame_count == WINDOW_SIZE)
        {
   
   
            bool result = false;
            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
            {
   
   
                // 执行视觉惯性联合初始化
               result = initialStructure();
               initial_timestamp = header.stamp.toSec();
            }
         ...
         }
	｝

系统初始化触发时应具备的数据

站在设计者的角度考虑系统的初始化，可以更深入的了解理论和代码细节。比如我们需要想好在搭建一套VINS框架时，应该要储备什么数据用以支持完成系统初始化。回到正题，我们可以获取的数据包括视觉测量值(滑动窗口内各图像帧的数据关联——KLT跟踪建立的)以及IMU预积分测量值(连续图像帧之间的预积分测量值是由图像间IMU数据累加构成的)，在VINS-MONO中视觉/IMU测量值被封装成如下形式:

// 记录了所有图像帧信息，key为图像帧时间戳，value为图像帧视觉测量点和imu预积分测量数据
map<double, ImageFrame> all_image_frame;

需要注意的是，all_image_frame中存储的不仅仅是滑动窗口内的视觉/IMU数据，还包括窗口外的数据。

系统初始化触发并成功初始化后更新的数据

成功初始化后会更新滑动窗口内每一图像帧时刻的陀螺仪Bias，以及绝对的视觉位姿(拥有尺度scale，以及在世界惯性系的roll/pitch角)，同时还会计算滑动窗口内每一图像帧时刻的速度。在这个过程中，会利用估计的重力方向将视觉坐标系调整到世界惯性坐标系上。

系统初始化总流程概述

在这里插入图片描述区别于ORB-SLAM3，VINS-MONO的初始化属于松耦合的初始化，即首先执行纯视觉的SFM从而获得up-to-scale的各图像帧位姿，然后将视觉位姿和IMU位姿对齐从而达到初始化的目的。

系统初始化各阶段详解

纯视觉sfm

VINS-MONO的sfm算法比较简单，基本步骤如下所示:
1.汇总特征管理器记录的所有图像帧的特征信息，整理得到所有特征轨迹sfm_f；

    vector<SFMFeature> sfm_f;
    // 遍历特征管理器中的每一个特征点
    for (auto &it_per_id : f_manager.feature)
    {
   
   
        // 获取该特征点起始图像帧的上一帧图像
        int imu_j = it_per_id.start_frame - 1;
        // 创建用于sfm的特征点对象，即一条特征轨迹track
        // 状态设置为未三角化，保留特征点索引
        SFMFeature tmp_feature;
        tmp_feature.state = false;
        tmp_feature.id = it_per_id.feature_id;
        // 往该特征点对象中填充对应的观测数据(图像帧索引，图像特征点2d位置)
        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
   
   
            imu_j++;
            Vector3d pts_j = it_per_frame.point;
            tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{
   
   pts_j.x(), pts_j.y()}));
        }
        sfm_f.push_back(tmp_feature);
    }

2.执行纯视觉的两帧初始化，在滑动窗口内(共11帧)寻找合适的图像帧(和窗口最后一帧对应特征点的平均视差>视差阈值且特征点匹配数量>20且可利用solveRelativeRT解算出R/T)，记录下初始化图像对的索引(窗口内最后一帧和筛选得到的第L帧)和相对姿态(relative_R/relative_T)；

    // 保证具有足够的视差,由E矩阵恢复R、t
    // l是挑选出来的用来初始化的图像帧索引
    // 该函数判断每帧到窗口最后一帧对应特征点的平均视差大于30，且内点数目大于12则可进行初始化
    // 并同时返回当前最新帧到第l帧的坐标系变换R和T
    // l为0-10之间的一个值
    if (!relativePose(relative_R, relative_T, l))
    {
   
   
        ROS_INFO("Not enough features or parallax; Move device around");
        return false;
    }

3.执行GlobalSFM，恢复窗口内所有图像帧位姿和landmarks，这一步流程相对比较复杂，展开讲讲:

a.基于输入的初始化图像对和相对Pose，进行两帧内三角化triangulateTwoFrames
b.以第l帧为纯视觉坐标系原点，通过Pnp计算第l+1帧到窗口最后一帧中所有图像帧的姿态
c.以第l帧为纯视觉坐标系原点，通过Pnp计算窗口第0帧到第l-1帧中所有图像帧的姿态
d.基于滑动窗口内的所有带Pose的图像帧，三角化窗口内剩余的特征轨迹
e.执行全局BA，添加滑动窗口内各图像帧的四元数形式的旋转和向量形式的平移作为优化变量，遍历所有特征对建立残差项
f.将优化结果取出，计算滑动窗口内各帧到参考帧l的转换矩阵，并记录所有地图点sfm_tracked_points

GlobalSFM的实现定义在initial_sfm.cpp中，具体的调用如下

    GlobalSFM sfm;
    // param_1(in): 待求解位姿的图像帧总数目
    // param_2(out): 当前滑动窗口内的图像帧姿态，从滑动窗口内各图像帧坐标系到第l帧相机坐标系的旋转矩阵
    // param_3(out): 当前滑动窗口内的图像帧平移，从滑动窗口内各图像帧坐标系到第l帧相机坐标系的平移向量
    // param_4(in): 初始化图像对的左图索引，右图索引为窗口内最后一帧
    // param_5，param_6(in): 初始化图像对的相对位姿，注意relative_R表示从当前最新帧到第l帧的坐标系变换relative_R和relative_T
    // param_7(in,out): 特征管理器内获取的所有特征轨迹
    // param_8(out): sfm计算的到的稀疏点云
    if(!sfm.construct(frame_count + 1, Q, T, l,
              relative_R, relative_T,
              sfm_f, sfm_tracked_points))
    {
   
   
        ROS_DEBUG("global SFM failed!");
        marginalization_flag = MARGIN_OLD;
        return false;
    }

4.对所有图像帧(包括滑动窗口外的图像帧)，给定初始的R/T，然后执行solvePnp进行优化，这个过程修正了all_image_frame中存储的每一帧的R/T(使用相机到imu的标定外参做了转换)。

    //solve pnp for all frame
    // 对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行优化
    map<double, ImageFrame>::iterator frame_it;
    map<int, Vector3d>::iterator it;
    frame_it = all_image_frame.begin( );
    // 遍历所有frame数据
    // 这些frame数据有三种情况
    // a.时间戳小于滑动窗口第一帧时间戳
    // b.时间戳在滑动窗口内第i/i+1帧之间
    // c.时间戳大于滑动窗口最后一帧时间戳(不可能发生，因为滑动窗口内最后一帧就是当前最新帧)
    for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
    {
   
   
        // provide initial guess
        cv::Mat r, rvec, t, D, tmp_r;
        // 若当前frame在滑动窗口内(即关键帧keyframe)，则直接使用construct中更新的Q/T
        if((frame_it->first) == Headers[i].stamp.toSec())
        {
   
   
            frame_it->second.is_key_frame = true;
            frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
            frame_it->second.T = T[i];
            i++;
            continue;
        }
        if((frame_it->first) > Headers[i].stamp.toSec())
        {
   
   
            i++;
        }
        // 以距离当前普通帧时序上最近的滑动窗口内对应关键帧的Pose作为初值，执行Pnp求解当前普通帧的位姿
        Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
        Vector3d P_inital = - R_inital * T[i];
        cv::eigen2cv(R_inital, tmp_r);
        cv::Rodrigues(tmp_r, rvec);
        cv::eigen2cv(P_inital, t);
        // 标记当前图像帧为普通帧
        frame_it->second.is_key_frame = false;
        // 建立特征点3d位置和对应2d位置的向量，用以Pnp计算位姿
        vector<cv::Point3f> pts_3_vector;
        vector<cv::Point2f> pts_2_vector;
        for (auto &id_pts : frame_it->second.points)
        {
   
   
            int feature_id = id_pts.first;
            for (auto &i_p : id_pts.second)
            {
   
   
                it = sfm_tracked_points.find(feature_id);
                if(it != sfm_tracked_points.end())
                {
   
   
                    Vector3d world_pts = it->second;
                    cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
                    pts_3_vector.push_back(pts_3);
                    Vector2d img_pts = i_p.second.head<

最低0.47元/天解锁文章