2.vinsfusion 数据流解读

继续上一节写到的诗句cv：：mat格式的图像数据和时间进入estimator.cpp的inputimage函数。之后的操作则是在trackImage函数中根据上一帧和提取的特征点信息、当前阵、预测的当前帧特征点信息进行光流追踪，确定特征点的配对关系，像素坐标、归一化坐标、uv方向的速度存储到frame中。然后开启processMeasurement。
1.输入图像和时间到trackImage当函数得到前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）。

    //featureTracker类在构建estimator类时构建
   featureFrame = featureTracker.trackImage(t, _img, _img1);//追踪双目
   //返回的数据是特征点id、相机 id 和归一化坐标系坐标、像素坐标和归一化相机坐标系速度
   //map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
   xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
            featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
            

注意：此时得到的featureFrame和订阅前端"/feature_tracker/feature"话题订阅的featureFrame结构是一样的，当前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）。

2.将得到的imgTrack通过pubTrackImage发布出去，然后可视化界面接收展示轨迹。

//将当前追踪的左右帧图像给到imgTrack
 cv::Mat imgTrack = featureTracker.getTrackImage();
//将得到的imgTrack通过pubTrackImage发布出去，然后可视化界面接收展示轨迹。
pubTrackImage(imgTrack, t);

话题名为：image_track

//image_track为话题名，1000表示：缓冲区中的消息在大于 1000 个的时候就会开始丢弃先前发布的消息
    pub_image_track = n.advertise<sensor_msgs::Image>("image_track", 1000);

3.将跟踪得到的featureFrame存储到featureBuf

featureBuf.push(make_pair(t, featureFrame));
现在featureFrame包含时间、特征点id 、相机 id 和归一化坐标系坐标、像素坐标和归一化相机坐标系速度。
queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf;

4.开启processMeasurement线程,处理全部量测的线程,IMU的预积分,特征点的处理

processMeasurements();

重点函数：
featureTracker.trackImage(t, _img, _img1)

     * 跟踪一帧图像，提取当前帧特征点
     * 1、用前一帧运动估计特征点在当前帧中的位置，如果特征点没有速度，就直接用前一帧该点位置
     * 2、LK光流跟踪前一帧的特征点，正反向，删除跟丢的点；如果是双目，进行左右匹配，只删右目跟丢的特征点
     * 3、对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩阵，用极线约束进一步剔除outlier点（代码注释掉了）
     * 4、如果特征点不够，剩余的用角点来凑；更新特征点跟踪次数
     * 5、计算特征点归一化相机平面坐标，并计算相对与前一帧移动速度
     * 6、保存当前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
     * 7、展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点

注：
角点：
通常意义上来说，角点就是极值点，即在某方面属性特别突出的点，是在某些属性上强度最大或者最小的孤立点、线段的终点。 对于图像而言，如图所示圆圈内的部分，即为图像的角点，其是物体轮廓线的连接点。

角点检测算法基本思想是使用一个固定窗口（取某个像素的一个邻域窗口）在图像上进行任意方向上的滑动，比较滑动前与滑动后两种情况，窗口中的像素灰度变化程度，如果存在任意方向上的滑动，都有着较大灰度变化，那么我们可以认为该窗口中存在角点。