前言

本文主要介紹VINS的視覺處理前端的視覺跟蹤模組（feature_trackers）。
論文第四章A節（IV. MEASUREMENT PREPROCESSING——A. Vision Processing Front-end）
可以看到其演算法主要包括以下內容：
1、對於每一幅新影象，KLT稀疏光流演算法對現有特徵進行跟蹤；
2、檢測新的角點特徵以保證每個影象特徵的最小數目(100-300)；
3、通過設定兩個相鄰特徵之間畫素的最小間隔來執行均勻的特徵分佈；
4、利用基本矩陣模型的RANSAC演算法進行外點剔除；
5、對特徵點進行去畸變矯正，然後投影到一個單位球面上(對於cata-fisheye camera)。

而文章還提到的視覺處理前端對關鍵幀的選擇將在之後進行討論。
由於這部分基本沒有數學推導所以直接看程式碼實現吧！

之前看第5點的時候一直有一個誤區，為什麼要將特徵點投影到單位球面上，其實並不全是。程式碼：

m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);

是根據不同的相機模型將二維座標轉換到三維座標
對於CATA（卡特魚眼相機）才是將畫素座標投影到單位圓內，這裡涉及了魚眼相機模型
而對於PINHOLE（針孔相機）是將畫素座標直接轉換到歸一化平面（z=1）並採用逆畸變模型(k1,k2,p1,p2)去畸變

程式碼實現

輸入輸出

feature_trackers可被認為是一個單獨的模組
輸入：影象，即訂閱了感測器或者rosbag釋出的topic：“/cam0/image_raw”
輸出：
1、釋出topic：“/feature_trackers/feature_img”
即跟蹤的特徵點影象，主要是之後給RVIZ用和除錯用
2、釋出topic：“/feature_trackers/feature”
即跟蹤的特徵點資訊，由/vins_estimator訂閱並進行優化
3、釋出topic：“/feature_trackers/restart”
即判斷特徵跟蹤模組是否出錯，若有問題則進行復位，由/vins_estimator訂閱

程式碼結構

VINS對於視覺跟蹤相關的程式碼在feature_trackers資料夾中。該資料夾內src的檔案有：
feature_trackers.cpp/feature_trackers.h：構建feature_trackers類，實現特徵點跟蹤的函式

feature_trackers類的成員變數

	cv::Mat mask;//影象掩碼
    cv::Mat fisheye_mask;//魚眼相機mask，用來去除邊緣噪點

    // prev_img是上一次釋出的幀的影象資料
    // cur_img是光流跟蹤的前一幀的影象資料
    // forw_img是光流跟蹤的後一幀的影象資料
    cv::Mat prev_img, cur_img, forw_img;

    vector<cv::Point2f> n_pts;//每一幀中新提取的特徵點
    vector<cv::Point2f> prev_pts, cur_pts, forw_pts;//對應的影象特徵點
    vector<cv::Point2f> prev_un_pts, cur_un_pts;//歸一化相機座標系下的座標
    vector<cv::Point2f> pts_velocity;//當前幀相對前一幀特徵點沿x,y方向的畫素移動速度
    vector<int> ids;//能夠被跟蹤到的特徵點的id
    vector<int> track_cnt;//當前幀forw_img中每個特徵點被追蹤的時間次數

    map<int, cv::Point2f> cur_un_pts_map;
    map<int, cv::Point2f> prev_un_pts_map;

    camodocal::CameraPtr m_camera;//相機模型
    double cur_time;
    double prev_time;

    static int n_id;//用來作為特徵點id，每檢測到一個新的特徵點，就將n_id作為該特徵點的id，然後n_id加1

feature_trackers_node.cpp：main()函式 ROS的程式入口

parameters.cpp/parameters.h：實現引數的讀取和設定
以使用配置檔案config\euroc\euroc_config.yaml為例

tic_toc.h：TicToc類，記錄時間

可以看到，視覺跟蹤的主要實現方法集中在feature_trackers_node.cpp的回撥函式和feature_trackers類中，接下來將從main()函式開始對加粗的函式進行具體解讀

程式碼解讀(部分內容省略)

程式入口 int main(int argc, char **argv)

1、ros初始化和設定控制代碼，設定logger級別

ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

2、讀取如config->euroc->euroc_config.yaml中的一些配置引數

readParameters(n);

3、讀取每個相機例項讀取對應的相機內參，NUM_OF_CAM=1為單目

 for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

4、判斷是否加入魚眼mask來去除邊緣噪聲

5、訂閱話題IMAGE_TOPIC(如/cam0/image_raw)，執行回撥函式img_callback

ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

6、釋出feature，例項feature_points，跟蹤的特徵點，給後端優化用
釋出feature_img，例項ptr，跟蹤的特徵點圖，給RVIZ用和除錯用

pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);

回撥函式 void img_callback(const sensor_msgs::ImageConstPtr &img_msg)

/**
 * @brief   ROS的回撥函式，對新來的影象進行特徵點追蹤，釋出
 * @Description	readImage()函式對新來的影象使用光流法進行特徵點跟蹤
 *              追蹤的特徵點封裝成feature_points釋出到pub_img的話題下
 *              影象封裝成ptr釋出在pub_match下
 * @param[in]   輸入的影象
 * @return      void
*/

1、判斷是否是第一幀

if(first_image_flag)

2、判斷時間間隔是否正確，有問題則restart

if(img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time) 

3、釋出頻率控制，並不是每讀入一幀影象，就要釋出特徵點，通過判斷間隔時間內的釋出次數
（不釋出的時候也是會執行readImage() 讀取影象進行處理）

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
	PUB_THIS_FRAME = true;
	// 時間間隔內的釋出頻率十分接近設定頻率時，更新時間間隔起始時刻，並將資料釋出次數置0
   if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
	{
	first_image_time = img_msg->header.stamp.toSec();
	pub_count = 0;
	}
}
else
	PUB_THIS_FRAME = false;

4、將影象編碼8UC1轉換為mono8

5、單目時：FeatureTracker::readImage() 函式讀取影象資料進行處理

trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());

6、更新全域性ID

7、如果PUB_THIS_FRAME=1則進行釋出
將特徵點id，矯正後歸一化平面的3D點(x,y,z=1)，畫素2D點(u,v)，畫素的速度(vx,vy)，封裝成sensor_msgs::PointCloudPtr型別的feature_points例項中,釋出到pub_img;
將影象封裝到cv_bridge::cvtColor型別的ptr例項中釋出到pub_match

影象特徵跟蹤的主要處理函式 void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)

1、createCLAHE() 判斷並對影象進行自適應直方圖均衡化；
2、calcOpticalFlowPyrLK() 從cur_pts到forw_pts做LK金字塔光流法；
3、根據status，把跟蹤失敗的和位於影象邊界外的點剔除，剔除時不僅要從當前幀資料forw_pts中剔除，而且還要從cur_un_pts、prev_pts、cur_pts，記錄特徵點id的ids，和記錄特徵點被跟蹤次數的track_cnt中剔除；
4、setMask() 對跟蹤點進行排序並依次選點，設定mask：去掉密集點，使特徵點分佈均勻
5、rejectWithF() 通過基本矩陣F剔除outliers
6、goodFeaturesToTrack() 尋找新的特徵點(shi-tomasi角點)，新增(MAX_CNT - forw_pts.size())個點以確保每幀都有足夠的特徵點
7、addPoints()向forw_pts新增新的追蹤點，id初始化-1，track_cnt初始化為1
8、undistortedPoints() 對特徵點的影象座標根據不同的相機模型進行去畸變矯正和深度歸一化，計算每個角點的速度

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    //如果EQUALIZE=1，表示太亮或則太暗
    if (EQUALIZE)//判斷是否進行直方圖均衡化處理
    {
        //自適應直方圖均衡
        //createCLAHE(double clipLimit, Size tileGridSize)
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    if (forw_img.empty())
    {
        //如果當前幀的影象資料forw_img為空，說明當前是第一次讀入影象資料
        //將讀入的影象賦給當前幀forw_img
        //同時，還將讀入的影象賦給prev_img、cur_img，這是為了避免後面使用到這些資料時，它們是空的
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        //否則，說明之前就已經有影象讀入
        //所以只需要更新當前幀forw_img的資料
        forw_img = img;
    }

    forw_pts.clear();//此時forw_pts還儲存的是上一幀影象中的特徵點，所以把它清除

    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;

        //呼叫cv::calcOpticalFlowPyrLK()對前一幀的特徵點cur_pts進行LK金字塔光流跟蹤，得到forw_pts
        //status標記了從前一幀cur_img到forw_img特徵點的跟蹤狀態，無法被追蹤到的點標記為0
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

        //將位於影象邊界外的點標記為0
        for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;

        //根據status,把跟蹤失敗的點剔除
        //不僅要從當前幀資料forw_pts中剔除，而且還要從cur_un_pts、prev_pts和cur_pts中剔除
        //prev_pts和cur_pts中的特徵點是一一對應的
        //記錄特徵點id的ids，和記錄特徵點被跟蹤次數的track_cnt也要剔除
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

    //光流追蹤成功,特徵點被成功跟蹤的次數就加1
    //數值代表被追蹤的次數，數值越大，說明被追蹤的就越久
    for (auto &n : track_cnt)
        n++;

    //PUB_THIS_FRAME=1 需要釋出特徵點
    if (PUB_THIS_FRAME)
    {
        //通過基本矩陣剔除outliers
        rejectWithF();

        ROS_DEBUG("set mask begins");
        TicToc t_m;

        setMask();//保證相鄰的特徵點之間要相隔30個畫素,設定mask
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;

        //計算是否需要提取新的特徵點
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            /** 
            * cv::goodFeaturesToTrack()
            * @brief   在mask中不為0的區域檢測新的特徵點
            * @optional    ref:https://docs.opencv.org/3.1.0/dd/d1a/group__imgproc__feature.html#ga1d6bb77486c8f92d79c8793ad995d541
            * @param[in]    InputArray _image=forw_img 輸入影象
            * @param[out]   _corners=n_pts 存放檢測到的角點的vector
            * @param[in]    maxCorners=MAX_CNT - forw_pts.size() 返回的角點的數量的最大值
            * @param[in]    qualityLevel=0.01 角點質量水平的最低閾值（範圍為0到1，質量最高角點的水平為1），小於該閾值的角點被拒絕
            * @param[in]    minDistance=MIN_DIST 返回角點之間歐式距離的最小值
            * @param[in]    _mask=mask 和輸入影象具有相同大小，型別必須為CV_8UC1,用來描述影象中感興趣的區域，只在感興趣區域中檢測角點
            * @param[in]    blockSize：計算協方差矩陣時的視窗大小
            * @param[in]    useHarrisDetector：指示是否使用Harris角點檢測，如不指定，則計算shi-tomasi角點
            * @param[in]    harrisK：Harris角點檢測需要的k值
            * @return      void
            */
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;

        //添將新檢測到的特徵點n_pts新增到forw_pts中，id初始化-1,track_cnt初始化為1.
        addPoints();

        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }

    //當下一幀影象到來時，當前幀資料就成為了上一幀釋出的資料
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;

    //把當前幀的資料forw_img、forw_pts賦給上一幀cur_img、cur_pts
    cur_img = forw_img;
    cur_pts = forw_pts;

    //根據不同的相機模型進行去畸變矯正和深度歸一化，計算速度
    undistortedPoints();
    prev_time = cur_time;
}

void FeatureTracker::setMask()

/**
 * @brief   對跟蹤點進行排序並去除密集點
 * @Description	對跟蹤到的特徵點，按照被追蹤到的次數排序並依次選點
 *              使用mask進行類似非極大抑制的方法，半徑為30，去掉密集點，使特徵點分佈均勻            
 * @return      void
*/
void FeatureTracker::setMask()
{
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
    
    // prefer to keep features that are tracked for long time
    //構造(cnt，pts，id)序列
    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;

    for (unsigned int i = 0; i < forw_pts.size(); i++)
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));

    //對光流跟蹤到的特徵點forw_pts，按照被跟蹤到的次數cnt從大到小排序
    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
         {
            return a.first > b.first;
         });

    //清空cnt，pts，id並重新存入
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(it.second.first) == 255)
        {
            //當前特徵點位置對應的mask值為255，則保留當前特徵點，將對應的特徵點位置pts，id，被追蹤次數cnt分別存入
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);

            //在mask中將當前特徵點周圍半徑為MIN_DIST的區域設定為0，後面不再選取該區域內的點（使跟蹤點不集中在一個區域上）
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

void FeatureTracker::rejectWithF()

/**
 * @brief   通過F矩陣去除outliers
 * @Description	將影象座標畸變矯正後轉換為深度歸一化座標
 *              cv::findFundamentalMat()計算F矩陣
 *              reduceVector()去除outliers 
 * @return      void
*/
void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;

        //將影象座標轉換為深度歸一化相機座標
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {

            Eigen::Vector3d tmp_p;
            
            //根據不同的相機模型將二維座標轉換到三維座標
            //對於PINHOLE（針孔相機）將畫素座標轉換到歸一化平面並去畸變
            //對於CATA（卡特魚眼相機）將畫素座標投影到單位圓內
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            
            //進行深度歸一化
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;
        //呼叫cv::findFundamentalMat對un_cur_pts和un_forw_pts計算F矩陣
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}