五、VINS-mono 代码解析——VIO初始化之SFM详解

前言

我们知道VINS采用了视觉和IMU的松耦合初始化方案。我将初始化分为两部分, 第一部分就是本文主要讲的SFM. 第二部分为**视觉部分和IMU部分之间的关联**.由于单目紧耦合的VIO是一个高度非线性系统，单目视觉没有尺度信息，IMU的测量又存在偏置误差，如果没有良好的初始值很难将这两种测量结果有机融合，因而初始化是VIO最敏感的环节。

本文主要介绍运动恢复结构(SFM)得到纯视觉系统的初始化, 即滑动窗口中所有帧的位姿和所有路标点的3d位置 。

系统流程图

Estimator类

processImage()函数位于vins_estimator/src/estimator.cpp

• initialStructure()初始化函数
  • SFM初始化
  • relativePose()函数
  • getCorresponding()函数返回两帧匹配特征点3D坐标
  • solveRelativeRT()函数 利用五点法求解相机初始位姿
• GlobalSFM::construct () [重要]

processImage()函数

具体看一下实现的细节:

/**
 * @brief   处理图像特征数据
 * @Description addFeatureCheckParallax()添加特征点到feature中，计算点跟踪的次数和视差，判断是否是关键帧               
 *              判断并进行外参标定
 *              进行视觉惯性联合初始化或基于滑动窗口非线性优化的紧耦合VIO
 * @param[in]   image 某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]s构成的map,索引为feature_id
 * @param[in]   header 某帧图像的头信息
 * @return  void
*/
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
   
    ROS_DEBUG("new image coming ------------------------------------------");
    ROS_DEBUG("Adding feature points %lu", image.size());
 
    // 1. 通过检测两帧之间的视差决定次新帧是否作为关键帧
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))//添加之前检测到的特征点到feature容器中，计算每一个点跟踪的次数，以及它的视差
        marginalization_flag = MARGIN_OLD;//=0
    else
        marginalization_flag = MARGIN_SECOND_NEW;//=1
 
    ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
    ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
    ROS_DEBUG("Solving %d", frame_count);
    ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());
    Headers[frame_count] = header;
 
    // 2. 填充imageframe的容器以及更新临时预积分初始值
    ImageFrame imageframe(image, header.stamp.toSec());//ImageFrame类包括特征点、时间、位姿Rt、预积分对象pre_integration、是否关键帧
    imageframe.pre_integration = tmp_pre_integration;// tmp_pre_integration是之前IMU 预积分计算的
    all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));// map<double, ImageFrame> all_image_frame;
    
    tmp_pre_integration = new IntegrationBase{
   acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};//更新临时预积分初始值
 
    // 3. 如果没有外参则标定IMU到相机的外参
    if(ESTIMATE_EXTRINSIC == 2)
    {
   
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
   
            //得到两帧之间归一化特征点
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            //标定从camera到IMU之间的外参数
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
   
                ROS_WARN("initial extrinsic rotation calib success");
                ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;// 完成外参标定
            }
        }
    }
    // 4. 初始化
    if (solver_flag == INITIAL)//初始化
    {
   
        //frame_count是滑动窗口中图像帧的数量，一开始初始化为0，滑动窗口总帧数WINDOW_SIZE=10
        //确保有足够的frame参与初始化
        if (frame_count == WINDOW_SIZE)
        {
   
            bool result = false;
            //有外参且当前帧时间戳大于初始化时间戳0.1秒，就进行初始化操作
            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
            {
   
               // 4.1 重要！！！ 视觉惯性联合初始化
               result = initialStructure();
               // 4.2 更新初始化时间戳
               initial_timestamp = header.stamp.toSec();
            }
            if(result)//初始化成功
            {
   
                // 先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
                solver_flag = NON_LINEAR;// 初始化更改为非线性
                solveOdometry(); // 4.3 非线性化求解里程计
                slideWindow();// 4.4 滑动窗
                f_manager.removeFailures();// 4.5 剔除feature中估计失败的点（solve_flag == 2）0 haven't solve yet; 1 solve succ; 2 solve fail;
                ROS_INFO("Initialization finish!");
                // 4.6 初始化窗口中PVQ
                last_R = Rs[WINDOW_SIZE];
                last_P = Ps[WINDOW_SIZE];
                last_R0 = Rs[0];
                last_P0 = Ps[0];
                
            }
            else
                slideWindow();// 初始化失败则直接滑动窗口
        }
        else
            frame_count++;// 图像帧数量+1
    }
    else// 5. 紧耦合的非线性优化
    {
   
        TicToc t_solve;
        solveOdometry();// 5.1 非线性化求解里程计
        ROS_DEBUG("solver costs: %fms", t_solve.toc());
 
        //5.2 故障检测与恢复,一旦检测到故障，系统将切换回初始化阶段
        if (failureDetection())
        {
   
            ROS_WARN("failure detection!");
            failure_occur = 1;
            clearState();// 清空状态
            setParameter();// 重设参数
            ROS_WARN("system reboot!");
            return;
        }
 
        TicToc t_margin;
        slideWindow(); // 5.3 滑动窗口
        f_manager.removeFailures();// 5.4 移除失败的
        ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
        // prepare output of VINS
        key_poses.clear();
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);// 关键位姿的位置 Ps
 
        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE]; 
        last_R0 = Rs[0];
        last_P0 = Ps[0];
    }
}

initialStructure()初始化函数

觉结构初始化过程至关重要，多传感器融合过程中，当单个传感器数据不确定性较高，需要依赖其他传感器降低不确定性。先对纯视觉SFM初始化相机位姿，再和IMU对齐。

主要分为1、纯视觉SFM估计滑动窗内相机位姿和路标点逆深度。2、视觉惯性联合校准，SFM与IMU积分对齐。

代码实现部分:

首先纯视觉SFM初始化sfm.construct()函数，之后视觉惯性联合初始化visualInitialAlign()函数。

视觉初始化入口在

bool Estimator::initialStructure()

初始化变量

Quaterniond Q[frame_count + 1];//旋转四元数Q
Vector3d T[frame_count + 1]; // 平移矩阵T
map<int, Vector3d> sfm_tracked_points; //存储SFM重建出特征点的坐标
vector<SFMFeature> sfm_f;// SFMFeature三角化状态、特征点索引、平面观测、位置坐标、深度

首先定义视觉SFM最重要的几个变量，旋转Q和平移T，map容器sfm_tracked_points保存SFM重建出的路标3D坐标，最重要的是SFMFeature类型的vector容器sfm_f。SFMFeature数据结构为：

// SFM 数据结构
struct SFMFeature
{
   
    bool state;//特征点的状态（是否被三角化）
    int id;//特征点ID
    vector<pair<int,Vector2d>> observation;//所有观测到该特征点的图像帧ID和 2D像素坐标
    double position[3];//路标3D坐标
    double depth;//深度
};

SFM初始化

1、通过加速度标准差保证IMU有充分运动激励，可以初始化

    // 1. 通过加速度标准差判断IMU是否有充分运动以初始化。
    {
   
        map<double, ImageFrame>::iterator frame_it;// 迭代器
        // 1.1 先求均值 aver_g
        Vector3d sum_g;
        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
   
            double dt = frame_it->second.pre_integration->sum_dt;
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            sum_g += tmp_g;
        }
        Vector3d aver_g;
        aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);// 总帧数减1，因为all_image_frame第一帧不算
        // 1.2 再求标准差var,表示线加速度的标准差
        double var = 0;
        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
   
            double dt = frame_it->second.pre_integration->sum_dt;
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
            //cout << "frame g " << tmp_g.transpose() << endl;
        }
        var = sqrt(var / ((int)all_image_frame.size() - 1));
        //ROS_WARN("IMU variation %f!", var);
        // 判断，加速度标准差大于0.25则代表imu充分激励，足够初始化
        if(var < 0.25)
        {
   
            ROS_INFO("IMU excitation not enouth!");
            //return false;
        }
    }

求解标准差的过程需要先求解均值，再求每个值和均值的差，最后需要判断加速度标准差大于0.25即可满足imu充分激励，可以初始化。

ImageFrame图像帧的数据结构为，就是头文件initial_alignment.h全部内容。

image类型为：

class ImageFrame
{
   
    public: