VINS-Mono代码学习记录（八）--processImage( )续(包含初始化）

最新推荐文章于 2026-06-26 09:04:47 发布

原创

最新推荐文章于 2026-06-26 09:04:47 发布 · 714 阅读

本文详细解析VINS-Mono中的processImage()函数，涉及视觉惯导联合初始化、相机相对位姿求解、SFM构建以及初始速度、重力和尺度因子的计算。通过initialStructure()函数，分析imu可观性、求解相机相对位姿、构建全局SFM，并探讨visualInitialAlign()中求解陀螺仪bias、速度和重力矢量的方法。

上一节其实已经学到了初始化的部分，就是求解相机和IMU的外参数：旋转。
接下来继续吧…
在processImage( )中，涉及到初始化的代码为：

 //[4]判断是初始化还是非线性优化
    if (solver_flag == INITIAL)//初始化
    {
        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();
            }
            if(result)
            {
                solver_flag = NON_LINEAR;
                solveOdometry();
                slideWindow();
                f_manager.removeFailures();
                ROS_INFO("Initialization finish!");
                last_R = Rs[WINDOW_SIZE];
                last_P = Ps[WINDOW_SIZE];
                last_R0 = Rs[0];
                last_P0 = Ps[0];
                
            }
            else
                slideWindow();
        }
        else//否则，就往滑动窗口中添加帧
            frame_count++;
    }

其中的initialStructure( )是视觉惯导联合初始化，注释如下：
分三部分来注释，第一部分为imu的可观性：

 TicToc t_sfm;
    //[1]check imu observibility
    {
        map<double, ImageFrame>::iterator frame_it;
        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);//平均重力加速度
        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);
        if(var < 0.25)
        {
            ROS_INFO("IMU excitation not enouth!");
            //return false;
        }
    }

第二部分为求解相机的相对位姿

 //【2】 global sfm
    Quaterniond Q[frame_count + 1];
    Vector3d T[frame_count + 1];
    map<int, Vector3d> sfm_tracked_points;//用于存储后面SFM重建出来3D点的坐标
    vector<SFMFeature> sfm_f;
    for (auto &it_per_id : f_manager.feature)//FeatureManager f_manager;//滑窗内所有点 定义在feature_manager.h中
    {
        int imu_j = it_per_id.start_frame - 1;
        SFMFeature tmp_feature;
        tmp_feature.state = false;//是否三角化
        tmp_feature.id = it_per_id.feature_id;
        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()}));//取归一化的二维坐标和特征点的起始帧数打包push进SFMFeature类型的变量tmp_feature中
        }
        sfm_f.push_back(tmp_feature);//sfm_f存储的是每个特征对应的所有的被观测数据
    } 
    Matrix3d relative_R;
    Vector3d relative_T;
    int l;
    if (!relativePose(relative_R, relative_T, l))//对帧间的相对位姿进行求解
    {
        ROS_INFO("Not enough features or parallax; Move device around");
        return false;
    }
    GlobalSFM sfm;//参考链接 https://blog.csdn.net/jiweinanyi/article/details/100095460
    if(!sfm.construct(frame_count + 1, Q, T, l,
              relative_R, relative_T,
              sfm_f, sfm_tracked_points))//l表示上面选出的参考帧的索引
    {
        ROS_DEBUG("global SFM failed!");
        marginalization_flag = MARGIN_OLD;
        return false;
    }

在这一部分首先定义旋转量和平移量，建立一个map容器sfm_tracked_points用于存储后面SFM重建出来的特征点的坐标，建立一个SFMFeature类型的vector容器sfm_f，更新sfm_f，再调用relativePose(relative_R,relative_T,l)求解相对位姿，最后是sfm.construct(frame_count + 1, Q, T, l， relative_R, relative_T， sfm_f, sfm_tracked_points)构建全局SFM。
relativePose(re