VINS-mono代码阅读 -- processImage

1. 对特征点信息的封装

relocalization frame这部分的处理，我准备放到后面再写，现在就先看看ProcessImage这部分代码。在进行图像处理之前，先对接收到的特征点信息进行封装，image是建立每个特征点和(camera_id,[x,y,z,u,v,vx,vy])构成的map，索引为feature_id。然后调用processImage。

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
// 遍历img_msg 中的特征点
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
    int v = img_msg->channels[0].values[i] + 0.5;
    int feature_id = v / NUM_OF_CAM;
    int camera_id = v % NUM_OF_CAM;
    //归一化坐标
    double x = img_msg->points[i].x;
    double y = img_msg->points[i].y;
    double z = img_msg->points[i].z;
	//像素坐标
    double p_u = img_msg->channels[1].values[i];
    double p_v = img_msg->channels[2].values[i];
	//像素速度
    double velocity_x = img_msg->channels[3].values[i];
    double velocity_y = img_msg->channels[4].values[i];
    ROS_ASSERT(z == 1);//如果z != 1,就出错了
    Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
    xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
    image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);

2. processImage

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
//1. 检测视差，根据视差决定marg掉新帧还是老帧
    ROS_DEBUG("new image coming ------------------------------------------");
    ROS_DEBUG("Adding feature points %lu", image.size());
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
        marginalization_flag = MARGIN_OLD;
    else 
        marginalization_flag = MARGIN_SECOND_NEW;

    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;

    ImageFrame imageframe(image, header.stamp.toSec());
    imageframe.pre_integration = tmp_pre_integration;//tmp_pre_integration 是在processIMU() 时进行的  IntegrationBase *tmp_pre_integration;
	//存储所有的imageFrame
    all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

//2. 外参标定
    if(ESTIMATE_EXTRINSIC == 2)
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
			// 1. 获取两帧之间的匹配的点对
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
			// 2. 计算相机到IMU的旋转矩阵
			//pre_integrations[frame_count-1]->delta_q 是IMU预积分得到的旋转  calib_ric是要计算的相机到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], RIC[0]
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

//3. 初始化
    if (solver_flag == INITIAL)
    {
        if (frame_count == WINDOW_SIZE)	//WINDOW_SIZE=10
        {
            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 ++;
    }
//4. 非线性优化
    else
    {
        TicToc t_solve;
        solveOdometry();
        ROS_DEBUG("solver costs: %fms", t_solve.toc());

        if (failureDetection())
        {
            ROS_WARN("failure detection!");
            failure_occur = 1;
            clearState();
            setParameter();
            ROS_WARN("system reboot!");
            return;
        }

        TicToc t_margin;
        slideWindow();
        f_manager.removeFailures();
        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]);

        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
    }
}

processImage主要包含了以下几个过程：
（1）计算视差
（2）外参标定
（3）初始化
（4）非线性优化

3. 视差计算

//TRUE 在processImage()中会将marginalization_flag设置为MARGIN_OLD。FALSE在processImage()中将marginalization_flag设置为MARGIN_NEW
//平均视差>MIN_PARALLAX  true，小于，false
//平均视差大，marg old， 平均视差小， Marg new
//image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    ROS_DEBUG("input feature: %d", (int)image.size());
    ROS_DEBUG("num of feature: %d", getFeatureCount());
    double parallax_sum = 0;// 所有特征点的视差总和
    int parallax_num = 0; //满足某些条件的跟踪点的个数
    last_track_num = 0;//被跟踪点的个数

	//遍历image中所有的特征点，和已经记录了特征点的容器feature中进行比较
    for (auto &id_pts : image)
    {
		//特征点管理器，存储特征点格式：首先按照特征点ID, 一个一个存储，每个ID会包含其在不同帧上的位置
		//id_pts.second[0].second获取的信息为：xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);// .first 得到key， .second 得到value
		//获取feature_id
        int feature_id = id_pts.first;
        
		//在feature中查找该feature_id的feature是否存在
		//find_if 返回第一个结果为true的值
		//这是一个lambda函数  [捕获列表] (参数列表) {函数体}  捕获列表捕获的是外面的参数
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)		//list<FeaturePerId> feature
                          {
            return it.feature_id == feature_id;
                          });

        if (it == feature.end())
        {
			//没有找到该feature 的id， 则把该特征点放入feature的list容器中，frame_count 是该特征点的起始帧
            feature.push_back(FeaturePerId(feature_id, frame_count));
			//feature是个list类型的容器，里面每个元素类型为FeaturePerId, feature_per_frame 表示每个FeaturePerId类型元素
			//back 返回最后一个元素，
            feature.back().feature_per_frame.push_back(f_per_fra);//同一个特征点在不同的帧下的坐标
        }
        else if (it->feature_id == feature_id)
        {
			/**
			*如果找到了相同id的特征点，就在其feature_per_frame中添加此特征点在此帧的位置及其他信息
			*it 的feature_per_frame存放的是该feature能被哪些帧看到，存放的是在这些帧中该特征点的信息
			*所以，feature_per_frame.size的大小就是有多少帧可以看到该特征点
			*/
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++;
        }
    }
	//加入到窗口中的帧个数取值为1或者0
	//或者能够跟踪到的特征点数目小于20个  
    if (frame_count < 2 || last_track_num < 20)
        return true;
	//遍历每一个feature
    for (auto &it_per_id : feature)
    {
		//计算能被当前帧和其前两帧共视的特征点的视差
		//it_per_if.feature_per_frame.size()表示该特征点能被多少帧共视
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {//计算特征点it_per_id在倒数第二帧和倒数第三帧的视差，并求所有视差的累加和
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;
        }
    }

    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
		//视差总和除以参与计算视差的特征点的个数，表示平均视差
        return parallax_sum / parallax_num >= MIN_PARALLAX;//MIN_PARALLAX = 10.0/460
    }
}

视差的计算还是比较简单的，难点就在于厘清前面特征点在这几个类之间的传递，这几个类到底表示的是什么意义。首先把这里用到的几个容易混淆的变量画个图出来：

接下来我们来看一下程序执行的过程：

1. 对image种的每一个特征点，按照feature_id，feature(list)中查找，
2. 如果没有找到这个feature_id，说明之前没有帧看到过这个特征点，那么就用这个点的id，frame_count(start_frame)新建一个FeaturePerId，然后把这个特征点的观测(f_per_fra)push到feature->feature_per_frame中。
3. 如果找到了这个feature_id,说明前面的帧已经看到了这个点，就只需要把当前帧观测到的信息(f_per_fra)push到这个特征点对应的feature->feature_per_frame中。然后跟踪到的点的数目++。
4. 加到滑窗中的帧的数目小于2，或者跟踪到的点的数目小于20个，返回true。
5. 遍历每一个feature，对能被当前帧和前两帧观测到的点进行视差计算，调用compensatedParallax2
6. 总视差等于0，返回true，
7. 返回平均视差 >= 最小视差

4. compensatedParallax2

/**
* it_per_id		从特征点list上取下来的一个feature
* frame_count	当前滑动窗口中的frame个数
*/
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
    //check the second last frame is keyframe or not
    //parallax betwwen seconde last frame and third last frame
	//计算该特征点在倒数第二帧和倒数第三帧的视差
    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];

    double ans = 0;
    Vector3d p_j = frame_j.point;

    double u_j = p_j(0);
    double v_j = p_j(1);

    Vector3d p_i = frame_i.point;
    Vector3d p_i_comp;

    //int r_i = frame_count - 2;
    //int r_j = frame_count - 1;
    //p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
    p_i_comp = p_i;
	//p_i中存放的是该point的x, y, z坐标值，z难道不是1吗？
    double dep_i = p_i(2);
    double u_i = p_i(0) / dep_i;
    double v_i = p_i(1) / dep_i;

    double du = u_i - u_j, dv = v_i - v_j;

    double dep_i_comp = p_i_comp(2);
    double u_i_comp = p_i_comp(0) / dep_i_comp;
    double v_i_comp = p_i_comp(1) / dep_i_comp;
    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;

    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));

    return ans;
}

这个函数整体看起来就简单一些了，没有什么比较费解的地方，注释掉的地方确实比较费解，但是没注释的地方是没什么难点的。计算视差的部分到这里就结束了。

下一步，就是外参的标定和初始化、非线性优化部分了。