VINS-Mono代码学习记录（七）----processImage( )

一、重定位帧的处理

在process( )函数中，在处理完IMU之后，接着就是对重定位帧的设置，代码片段如下：

  //[4] 设置重定位帧setReloFrame( )
            sensor_msgs::PointCloudConstPtr relo_msg = NULL;//去看传感器数据的definition
            while (!relo_buf.empty())
            {
                relo_msg = relo_buf.front();// 返回队首元素的值
                relo_buf.pop();//删除队列首元素
            }
            if (relo_msg != NULL)
            {
                vector<Vector3d> match_points;
                double frame_stamp = relo_msg->header.stamp.toSec();
                //遍历relo_msg中的points特征点
                for (unsigned int i = 0; i < relo_msg->points.size(); i++)
                {
                    Vector3d u_v_id;
                    u_v_id.x() = relo_msg->points[i].x;
                    u_v_id.y() = relo_msg->points[i].y;
                    u_v_id.z() = relo_msg->points[i].z;
                    match_points.push_back(u_v_id);
                }
                //[重定位帧的平移向量T的x,y,z，旋转四元数w,x,y,z和索引值]

                Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);//平移
                Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);//四元数表示的旋转
                Matrix3d relo_r = relo_q.toRotationMatrix();
                int frame_index;
                frame_index = relo_msg->channels[0].values[7];//索引
                estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
            }

relo_buf用来接收relo_msg，从relo_msg中取出点push到match_points,再取出帧的时间戳、帧的id、旋转以及平移量传递给估计器的setReloFrame( )函数。

void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
    //[1]将传入的各参数进行赋值给重定位相关的变量
    relo_frame_stamp = _frame_stamp;
    relo_frame_index = _frame_index;
    match_points.clear();
    match_points = _match_points;
    prev_relo_t = _relo_t;
    prev_relo_r = _relo_r;

    //【2】遍历滑动窗口，将当前传入的重定位帧的时间戳和滑动窗口中的进行对比
    for(int i = 0; i < WINDOW_SIZE; i++)
    {
        if(relo_frame_stamp == Headers[i].stamp.toSec())
        {
            relo_frame_local_index = i;//记录滑窗中是那一帧重定位
            relocalization_info = 1;//时间戳相等,重定位标志置1
            for (int j = 0; j < SIZE_POSE; j++)//SIZE_POSE=7
                relo_Pose[j] = para_Pose[i][j];// 把两个匹配的帧的位置和旋转四元数存储起来
        }
    }
}

二、processImage（ ）函数

先来看张图：

代码部分为：

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
  //image的数据类型分别表示feature_id,camera_id,点的x,y,z坐标，u,v坐标，在x,y方向上的跟踪速度
    ROS_DEBUG("new image coming ------------------------------------------");
    ROS_DEBUG("Adding feature points %lu", image.size());

    //[1]根据视差来决定marg掉哪一帧，如果次新帧是关键帧，marg掉最老帧，如果次新帧不是关键帧,marg掉次新帧
    if (f_manager.addFeatu