VINS_Fusion 特征追踪策略

VINS_Fusion前端的特征追踪策略在feature_tracker.cpp中。主要是TrackImage这个函数。
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
这个函数是图片信息中特征追踪的函数需要好好研究

输入参数
* double _cur_time当前的时间戳
* img 左目图片
* img1 右目图片

输出结果
* featureframe 特征帧

基本流程

1. 图像处理

当接受到双目图片及时间信息之后，首先进行图像处理,VINS_Fusion中注释了这几行代码。

    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//createCLAHE 直方图均衡
        clahe->apply(cur_img, cur_img);
        if(!rightImg.empty())
            clahe->apply(rightImg, rightImg);
    }

2. 特征追踪

若上一帧没有特征点，则直接基于3层的金字塔进行搜索
如果上一帧检测到了特征点，则直接进行利用LK光流法进行特征点追踪。
如果匹配到的数目过少，则扩大搜索，提高金字塔层数，再次进行搜索。
提取的新特征点是Harris特征点.

反向检查
主要是从当前帧中提取的特征点，看是否能在前一帧也追踪到这些特征点，如果两帧都能找到这些特征点，且被找到的点的距离小于提前设置好的阈值，则把状态设置为1，否则为0.
然后剔除不在图像内的点。

if(FLOW_BACK)//
{
    vector<uchar> reverse_status;
    vector<cv::Point2f> reverse_pts = prev_pts;
    //注意！这里输入的参数和上边的前后是相反的
    cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1, 
    cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
    //cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3); 
    for(size_t i = 0; i < status.size(); i++)
    {
        //如果前后都能找到，并且找到的点的距离小于0.5
        if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
        {
            status[i] = 1;
        }
        else
            status[i] = 0;
    }
}

3. 设置Mask

利用setMask()函数设置一个mask，确定兴趣区域（即之后找特征点的区域）.
主要细节：

• 把追踪到的点进行标记。
• 设置遮挡部分（鱼眼相机）
• 对检测到的特征点按追踪到的次数排序
• 在mask图像中将追踪到点的地方设置为0，否则为255，目的是为了下面做特征点检测的时候可以选择没有特征点的区域进行检测。
• 在同一区域内，追踪到次数最多的点会被保留，其他的点会被删除

这一步之前其实还有个rejectWithF().主要是为了使用F矩阵进行拒绝，删除一些点，涉及到了FM_RANSAC，拒绝了一些光流跟踪错误的点。这个开启之后会影响效率，因此默认注释掉了。

4. 提取左目图片新特征

设置了mask之后，就可以在当前帧提取新的特征了。要提取多少个特征点由提前设置的特征点最大值与当前追踪到的特征点决定，主要是要补充一些特征点进去以免追踪丢失。如果追踪到的特征点小于一定数目，则利用cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask)提取新特征;
该函数的功能具体解析可以查看：
当追踪到一得数量的特征点之后，提取新的特征点存入到cur_pts中。

        for (auto &p : n_pts)
        {
            cur_pts.push_back(p);
            ids.push_back(n_id++);
            track_cnt.push_back(1);
        }    

然后将像素坐标系下的坐标转换为归一化相机坐标系下的坐标，即un_pts为归一化相机坐标系下的坐标。计算当前帧相对于前一帧的特征点沿x,y方向的像素移动速度。

cur_un_pts = undistortedPts(cur_pts, m_camera[0]); //当前帧不失真的点
pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);

5. 匹配右目图片特征

如果是双目相机，当在左目图片上提取足够的新特征，还需要计算这些特征在右目图片上的的像素速度。
首先，要把左目上的点在右目图片上找到。
也是利用cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);从右目图片上找特征。
再来一个反向检查，设置status向量。

if(FLOW_BACK)
{
    cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
    for(size_t i = 0; i < status.size(); i++)
    {
        if(status[i] && statusRightLeft[i] && inBorder(cur_right_pts[i]) && distance(cur_pts[i], reverseLeftPts[i]) <= 0.5)
            status[i] = 1;
        else
            status[i] = 0;
    }
}

6. 剔除特征点

主要为了保留双目图片都有且能与之前帧匹配上的特征点

reduceVector(cur_right_pts, status);
reduceVector(ids_right, status);

同样也需要把右目图片上的特征点归一化，且计算特征点沿x，y方向的像素速度

7. 展示轨迹

    // 展示轨迹 featureFrame是在这构造的
    if(SHOW_TRACK)
        drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);

构建一个map型的特征帧featureFrame，并把7维的特征点诸葛加入

    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
    for (size_t i = 0; i < ids.size(); i++)
    {
        int feature_id = ids[i];
        double x, y ,z;
        x = cur_un_pts[i].x;
        y = cur_un_pts[i].y;
        z = 1;
        double p_u, p_v;
        p_u = cur_pts[i].x;
        p_v = cur_pts[i].y;
        int camera_id = 0;//左目的特征点
        double velocity_x, velocity_y;
        velocity_x = pts_velocity[i].x;
        velocity_y = pts_velocity[i].y;

        Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
        xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
        featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
        // 特征点的id，相机id（0或1） 和 xyz_uv_velocity（特征点空间坐标，像素坐标和像素速度）
    }

如果是双目的，则需要把右目图片中的特征点也加入进去

最后返回FeatureFrame即可