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即可