VINS-Fusion源码逐行解析：processImage函数

接上一讲内容继续介绍，processImage函数的入口在processMeasurements()中，是一个处理输入图像数据，并根据图像数据进行特征点的添加、初始化和优化等操作的函数，此函数很重要，并且内部包含的函数较多，我们这一讲首先捋一下processImage函数的执行流程和逻辑，并介绍一部分包含的函数

先整体看一下我注释的源码：

//处理输入图像数据，并根据图像数据进行特征点的添加、初始化和优化等操作
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
{
    //输出调试信息，表示接收到了新的图像
    ROS_DEBUG("new image coming ------------------------------------------");
    //输出调试信息，显示正在添加的特征点的数量
    ROS_DEBUG("Adding feature points %lu", image.size());
    //调用addFeatureCheckParallax函数检查特征点的视差变化，如果变化足够大，设置边缘化标志位为 MARGIN_OLD
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    {
        marginalization_flag = MARGIN_OLD;
        //printf("keyframe\n");
    }
    //否则，设置边缘化标志位为 MARGIN_SECOND_NEW
    else
    {
        marginalization_flag = MARGIN_SECOND_NEW;
        //printf("non-keyframe\n");
    }

    //输出当前帧是否为关键帧
    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;

    //创建一个 ImageFrame 对象来存储当前图像帧的数据
    ImageFrame imageframe(image, header);
    //将当前帧的预积分对象赋值给 imageframe
    imageframe.pre_integration = tmp_pre_integration;
    //将当前帧的数据插入到 all_image_frame 中
    all_image_frame.insert(make_pair(header, imageframe));
    //创建一个新的预积分对象，初始化参数包括当前帧的加速度、角速度和偏置。
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

    //如果正在进行外参标定
    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;
            //进行外参标定，如果成功则更新外参
            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;
                /将外参标定状态置为1
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

    //如果当前处于初始化阶段
    if (solver_flag == INITIAL)
    {
        // monocular + IMU initilization
        //如果是单目+IMU初始化
        if (!STEREO && USE_IMU)
        {
            //如果达到窗口大小，进行初始化
            if (frame_count == WINDOW_SIZE)
            {
                bool result = false;
                //如果不是在进行外参标定且时间间隔大于0.1秒，进行初始化
                if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
                {
                    result = initialStructure();
                    initial_timestamp = header;   
                }
                //如果初始化成功，进行优化、状态更新和窗口滑动操作
                if(result)
                {
                    optimization();
                    updateLatestStates();
                    solver_flag = NON_LINEAR;
                    slideWindow();
                    ROS_INFO("Initialization finish!");
                }
                //如果初始化失败，仅进行窗口滑动操作
                else
                    slideWindow();
            }
        }

        // stereo + IMU initilization
        //双目和imu模式
        if(STEREO && USE_IMU)
        {
            //首先通过initFramePoseByPnP函数初始化当前帧的位姿，然后对特征点进行三角化。
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            //检查帧计数是否达到窗口大小
            if (frame_count == WINDOW_SIZE)
            {
                //声明一个迭代器，用于遍历所有图像帧
                map<double, ImageFrame>::iterator frame_it;
                int i = 0;
                //遍历所有图像帧，更新其旋转矩阵 (R) 和平移向量 (T) 为 Rs 和 Ps 数组中对应的值，确保每个图像帧都使用最新的姿态信息。
                for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
                {
                    frame_it->second.R = Rs[i];
                    frame_it->second.T = Ps[i];
                    i++;
                }
                //计算陀螺仪偏置
                solveGyroscopeBias(all_image_frame, Bgs);
                //使用新的陀螺仪偏置，对每个帧的预积分重新传播
                for (int i = 0; i <= WINDOW_SIZE; i++)
                {
                    pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
                }
                //进行优化
                optimization();
                //更新最新的状态
                updateLatestStates();
                //将求解器标志设置为非线性
                solver_flag = NON_LINEAR;
                //滑动窗口
                slideWindow();
                //输出初始化完成的消息
                ROS_INFO("Initialization finish!");
            }
        }

        // stereo only initilization
        //仅是双目
        if(STEREO && !USE_IMU)
        {
            //初始化当前帧的位姿，对特征点进行三角化，并进行优化。
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            optimization();

            //当达到窗口大小时进行进一步处理
            if(frame_count == WINDOW_SIZE)
            {
                //进行优化、更新最新状态，并滑动窗口，将求解器标志设置为非线性
                optimization();
                updateLatestStates();
                solver_flag = NON_LINEAR;
                slideWindow();
                ROS_INFO("Initialization finish!");
            }
        }

        //frame_count 小于窗口大小 (WINDOW_SIZE) 时，将当前帧的状态初始化为上一帧的状态。
        if(frame_count < WINDOW_SIZE)
        {
            //增加 frame_count
            frame_count++;
            //将当前帧的位置、速度、旋转矩阵、加速度偏差和陀螺仪偏差初始化为上一帧的相应值
            int prev_frame = frame_count - 1;
            Ps[frame_count] = Ps[prev_frame];
            Vs[frame_count] = Vs[prev_frame];
            Rs[frame_count] = Rs[prev_frame];
            Bas[frame_count] = Bas[prev_frame];
            Bgs[frame_count] = Bgs[prev_frame];
        }

    }
    //如果不是初始化状态
    else
    {
        // 在非初始化状态下处理帧的状态估计和优化
        TicToc t_solve;
        //如果不使用IMU
        if(!USE_IMU)
            //调用 f_manager.initFramePoseByPnP() 初始化当前帧的位姿
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            //对特征点进行三角化
        f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
        //进行优化
        optimization();
        //存储需要移除的异常值索引
        set<int> removeIndex;
        //调用函数处理当前帧的异常值，并将结果存储在 removeIndex 中。
        outliersRejection(removeIndex);
        //特征管理器 f_manager 根据 removeIndex 中的异常值索引移除相应的特征点。
        f_manager.removeOutlier(removeIndex);
        //单线程下
        if (! MULTIPLE_THREAD)
        {
            //特征跟踪器根据 removeIndex 中的异常值索引移除跟踪的特征点。
            featureTracker.removeOutliers(removeIndex);
            //预测下一帧的特征点
            predictPtsInNextFrame();
        }
        //输出求解器运行耗时的调试信息，单位为毫秒    
        ROS_DEBUG("solver costs: %fms", t_solve.toc());

        //失败检测
        //调用 failureDetection() 函数检测系统是否出现故障
        if (failureDetection())
        {
            //如果检测到故障
            //输出警告信息 "failure detection!"。
            ROS_WARN("failure detection!");
            //将 failure_occur 设置为 1，表示发生了故障。
            failure_occur = 1;
            //调用 clearState() 清除当前状态
            clearState();
            //调用 setParameter() 设置系统参数
            setParameter();
            //输出系统重启的警告信息 "system reboot!"
            ROS_WARN("system reboot!");
            //立即返回，结束当前处理流程
            return;
        }

        //调用 slideWindow() 函数，执行窗口滑动操作，管理视觉惯性导航系统中的帧
        slideWindow();
        //移除失败的特征点
        //调用 f_manager.removeFailures() 函数，移除特征管理器中标记为失败的特征点
        f_manager.removeFailures();
        // prepare output of VINS
        //准备输出 VINS 的关键帧位置：
        //清空 key_poses 容器
        key_poses.clear();
        //遍历所有的窗口帧数 (WINDOW_SIZE)，将位置 (Ps) 数组中的每个关键帧位置添加到 key_poses 中。
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);

        //更新最后的旋转矩阵和平移向量

        //将最后一个窗口帧的旋转矩阵 (last_R) 和平移向量 (last_P) 设置为 Rs[WINDOW_SIZE] 和 Ps[WINDOW_SIZE]。
        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE];
        //将第一个窗口帧的旋转矩阵 (last_R0) 和平移向量 (last_P0) 设置为 Rs[0] 和 Ps[0]。
        last_R0 = Rs[0];
        last_P0 = Ps[0];
        //调用 updateLatestStates() 函数，更新系统中的最新状态信息
        updateLatestStates();
    }  
}

好的，看过源码了，现在我们捋一遍逻辑，首先通过调用addFeatureCheckParallax函数来判断是删除最旧的那帧，还是删除刚进来的那帧，

//调用addFeatureCheckParallax函数检查特征点的视差变化，如果变化足够大，设置边缘化标志位为 MARGIN_OLD
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    {
        marginalization_flag = MARGIN_OLD;
        //printf("keyframe\n");
    }
    //否则，设置边缘化标志位为 MARGIN_SECOND_NEW
    else
    {
        marginalization_flag = MARGIN_SECOND_NEW;
        //printf("non-keyframe\n");
    }

然后更新一下当前帧的的预积分

//记录当前帧的时间戳
    Headers[frame_count] = header;

    //创建一个 ImageFrame 对象来存储当前图像帧的数据
    ImageFrame imageframe(image, header);
    //将当前帧的预积分对象赋值给前一帧
    imageframe.pre_integration = tmp_pre_integration;
    //将当前帧的数据插入到 all_image_frame 中
    all_image_frame.insert(make_pair(header, imageframe));
    //创建一个新的预积分对象，初始化参数包括当前帧的加速度、角速度和偏置。
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

 再进行外参标定，通过getCorresponding函数获取当前帧与上一帧的特征对应关系，接着调用

CalibrationExRotation函数进行外参标定，更新外参矩阵

//如果正在进行外参标定
    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;
            //进行外参标定，如果成功则更新外参
            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;
                /将外参标定状态置为1
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

如果在初始化阶段，分三种情况，单目加imu、双目加imu和纯双目情况

单目加imu

如果当前帧的id等于WINDOW_SIZE，那么调用initialStructure()函数完成初始化，如果成功，就进行优化，更新状态，滑动窗口，失败就仅进行滑动窗口操作

//如果是单目+IMU初始化
        if (!STEREO && USE_IMU)
        {
            //如果达到窗口大小，进行初始化
            if (frame_count == WINDOW_SIZE)
            {
                bool result = false;
                //如果不是在进行外参标定且时间间隔大于0.1秒，进行初始化
                if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
                {
                    result = initialStructure();
                    initial_timestamp = header;   
                }
                //如果初始化成功，进行优化、状态更新和窗口滑动操作
                if(result)
                {
                    optimization();
                    updateLatestStates();
                    solver_flag = NON_LINEAR;
                    slideWindow();
                    ROS_INFO("Initialization finish!");
                }
                //如果初始化失败，仅进行窗口滑动操作
                else
                    slideWindow();
            }
        }

双目加imu

直接调用initFramePoseByPnP函数进行初始化，接着通过triangulate函数计算特征点的深度，然后更新所有图像帧的位姿，计算新的陀螺仪偏置，使用新的陀螺仪偏置，对每个帧的预积分重新传播，最后进行优化，更新状态，滑动窗口操作

//双目和imu模式
        if(STEREO && USE_IMU)
        {
            //首先通过initFramePoseByPnP函数初始化当前帧的位姿，然后对特征点进行三角化。
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            //检查帧计数是否达到窗口大小
            if (frame_count == WINDOW_SIZE)
            {
                //声明一个迭代器，用于遍历所有图像帧
                map<double, ImageFrame>::iterator frame_it;
                int i = 0;
                //遍历所有图像帧，更新其旋转矩阵 (R) 和平移向量 (T) 为 Rs 和 Ps 数组中对应的值，确保每个图像帧都使用最新的姿态信息。
                for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
                {
                    frame_it->second.R = Rs[i];
                    frame_it->second.T = Ps[i];
                    i++;
                }
                //计算陀螺仪偏置
                solveGyroscopeBias(all_image_frame, Bgs);
                //使用新的陀螺仪偏置，对每个帧的预积分重新传播
                for (int i = 0; i <= WINDOW_SIZE; i++)
                {
                    pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
                }
                //进行优化
                optimization();
                //更新最新的状态
                updateLatestStates();
                //将求解器标志设置为非线性
                solver_flag = NON_LINEAR;
                //滑动窗口
                slideWindow();
                //输出初始化完成的消息
                ROS_INFO("Initialization finish!");
            }
        }

仅双目

这个就简单了，调用initFramePoseByPnP函数进行初始化，通过triangulate函数计算特征点的深度，然后直接优化，达到窗口大小时则进行优化，更新状态，滑动窗口操作

//仅是双目
        if(STEREO && !USE_IMU)
        {
            //初始化当前帧的位姿，对特征点进行三角化，并进行优化。
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            optimization();

            //当达到窗口大小时进行进一步处理
            if(frame_count == WINDOW_SIZE)
            {
                //进行优化、更新最新状态，并滑动窗口，将求解器标志设置为非线性
                optimization();
                updateLatestStates();
                solver_flag = NON_LINEAR;
                slideWindow();
                ROS_INFO("Initialization finish!");
            }
        }

如果没有太多帧，没达到WINDOW_SIZE数量时，直接将当前帧的状态初始为上一帧的状态，包括位置、速度、旋转矩阵、加速度偏差和陀螺仪偏差

//frame_count 小于窗口大小 (WINDOW_SIZE) 时，将当前帧的状态初始化为上一帧的状态。
        if(frame_count < WINDOW_SIZE)
        {
            //增加 frame_count
            frame_count++;
            //将当前帧的位置、速度、旋转矩阵、加速度偏差和陀螺仪偏差初始化为上一帧的相应值
            int prev_frame = frame_count - 1;
            Ps[frame_count] = Ps[prev_frame];
            Vs[frame_count] = Vs[prev_frame];
            Rs[frame_count] = Rs[prev_frame];
            Bas[frame_count] = Bas[prev_frame];
            Bgs[frame_count] = Bgs[prev_frame];
        }

最后，如果不是初始化状态，在不使用imu的情况下 调用initFramePoseByPnP函数初始化当前帧的位姿，然后进行三角化，调用outliersRejection找到需要移除的异常值索引，接着调用removeOutlier函数对其移除，在单线程下，通过特征跟踪器根据 removeIndex 中的异常值索引移除跟踪的特征点，并会使用predictPtsInNextFrame函数预测下一帧的特征点，然后调用failureDetection函数进行故障检测，移除失败的特征点，最后更新最新状态信息，完

//如果不是初始化状态
    else
    {
        // 在非初始化状态下处理帧的状态估计和优化
        TicToc t_solve;
        //如果不使用IMU
        if(!USE_IMU)
            //调用 f_manager.initFramePoseByPnP() 初始化当前帧的位姿
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            //对特征点进行三角化
        f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
        //进行优化
        optimization();
        //存储需要移除的异常值索引
        set<int> removeIndex;
        //调用函数处理当前帧的异常值，并将结果存储在 removeIndex 中。
        outliersRejection(removeIndex);
        //特征管理器 f_manager 根据 removeIndex 中的异常值索引移除相应的特征点。
        f_manager.removeOutlier(removeIndex);
        //单线程下
        if (! MULTIPLE_THREAD)
        {
            //特征跟踪器根据 removeIndex 中的异常值索引移除跟踪的特征点。
            featureTracker.removeOutliers(removeIndex);
            //预测下一帧的特征点
            predictPtsInNextFrame();
        }
        //输出求解器运行耗时的调试信息，单位为毫秒    
        ROS_DEBUG("solver costs: %fms", t_solve.toc());

        //失败检测
        //调用 failureDetection() 函数检测系统是否出现故障
        if (failureDetection())
        {
            //如果检测到故障
            //输出警告信息 "failure detection!"。
            ROS_WARN("failure detection!");
            //将 failure_occur 设置为 1，表示发生了故障。
            failure_occur = 1;
            //调用 clearState() 清除当前状态
            clearState();
            //调用 setParameter() 设置系统参数
            setParameter();
            //输出系统重启的警告信息 "system reboot!"
            ROS_WARN("system reboot!");
            //立即返回，结束当前处理流程
            return;
        }

        //调用 slideWindow() 函数，执行窗口滑动操作，管理视觉惯性导航系统中的帧
        slideWindow();
        //移除失败的特征点
        //调用 f_manager.removeFailures() 函数，移除特征管理器中标记为失败的特征点
        f_manager.removeFailures();
        // prepare output of VINS
        //准备输出 VINS 的关键帧位置：
        //清空 key_poses 容器
        key_poses.clear();
        //遍历所有的窗口帧数 (WINDOW_SIZE)，将位置 (Ps) 数组中的每个关键帧位置添加到 key_poses 中。
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);

        //更新最后的旋转矩阵和平移向量

        //将最后一个窗口帧的旋转矩阵 (last_R) 和平移向量 (last_P) 设置为 Rs[WINDOW_SIZE] 和 Ps[WINDOW_SIZE]。
        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE];
        //将第一个窗口帧的旋转矩阵 (last_R0) 和平移向量 (last_P0) 设置为 Rs[0] 和 Ps[0]。
        last_R0 = Rs[0];
        last_P0 = Ps[0];
        //调用 updateLatestStates() 函数，更新系统中的最新状态信息
        updateLatestStates();
    }