PL-VIO學習+註釋

1說明

本次是對賀一佳博士的PL-VIO進行學習並註釋.十分感謝賀博之前做的工作.
PL-VIO是在VINS-Mono的基礎上,添加了線特徵,並且取得了不錯的效果.缺點是線特徵的提取匹配所需要的時間比較長.
github: https://github.com/HeYijia/PL-VIO
論文:PL-VIO: Tightly-Coupled Monocular Visual-Inertial Odometry Using Point and Line Features.
同時,還少量借鑑了VINS-Mono的註釋版:
https://github.com/kuankuan-yue/VINS-Mono-Learning

主要註釋了其中的三個文件,其作用是對圖像進行線特徵的提取,匹配和發佈.

linefeature_tracker.cpp
linefeature_tracker.h
linefeature_tracker_node.cpp

註釋後的程序放在了
https://github.com/kuankuan-yue/PL-VIO-Learning

2註釋代碼

2.1linefeature_tracker_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/Imu.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>

#include "linefeature_tracker.h"

// #include "feature_tracker.h"

#define SHOW_UNDISTORTION 0

vector<uchar> r_status;
vector<float> r_err;
queue<sensor_msgs::ImageConstPtr> img_buf;

ros::Publisher pub_img,pub_match;

LineFeatureTracker trackerData;
double first_image_time;
int pub_count = 1;
bool first_image_flag = true;
double frame_cnt = 0;
double sum_time = 0.0;
double mean_time = 0.0;


/**
 * @brief   ROS的回調函數，對新來的圖像進行特徵追蹤，發佈
 * @Description readImage()函數對新來的圖像提取/匹配/發佈線特徵
 * @param[in]   img_msg 輸入的圖像
 * @return      void
*/
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{

    // 判斷是否是第一幀
    if(first_image_flag)
    {
        first_image_flag = false;
        first_image_time = img_msg->header.stamp.toSec();
    }

    // frequency control, 如果圖像頻率低於一個值
    // 發佈頻率控制
    // 並不是每讀入一幀圖像，就要發佈特徵點
    // 判斷間隔時間內的發佈次數
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
        PUB_THIS_FRAME = true;
        // reset the frequency control
        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;



    cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);//圖像的指針
    cv::Mat show_img = ptr->image;


//     cv::imshow("lineimg",show_img); //TODO:
//     cv::waitKey(1);

    // readImage
    TicToc t_r;
    frame_cnt++;
    trackerData.readImage(ptr->image.rowRange(0 , ROW));   //TODO: rowRange(i,j) 取圖像的i～j行


    // 發佈線特徵
    if (PUB_THIS_FRAME)
    {
        pub_count++;

        /* sensor_msgs::PointCloudPtr的格式
        std_msgs/Header header
        geometry_msgs/Point32[] points
        sensor_msgs/ChannelFloat32[] channels   */ 

        sensor_msgs::PointCloudPtr feature_lines(new sensor_msgs::PointCloud);
        sensor_msgs::ChannelFloat32 id_of_line;   //  feature id
        sensor_msgs::ChannelFloat32 u_of_endpoint;    //  u
        sensor_msgs::ChannelFloat32 v_of_endpoint;    //  v

        feature_lines->header = img_msg->header;
        feature_lines->header.frame_id = "world";

        vector<set<int>> hash_ids(NUM_OF_CAM);
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            if (i != 1 || !STEREO_TRACK)  // 單目
            {
                // 得到歸一化平面上的座標
                auto un_lines = trackerData.undistortedLineEndPoints();

                //auto &cur_lines = trackerData.curframe_->vecLine;
                auto &ids = trackerData.curframe_->lineID; //lineID

                for (unsigned int j = 0; j < ids.size(); j++)
                {

                    int p_id = ids[j]; //第j個線的id
                    hash_ids[i].insert(p_id);
                    geometry_msgs::Point32 p; //起始點的位置
                    p.x = un_lines[j].StartPt.x;
                    p.y = un_lines[j].StartPt.y;
                    p.z = 1;

                    feature_lines->points.push_back(p);
                    id_of_line.values.push_back(p_id * NUM_OF_CAM +i );
                    //std::cout<< "feature tracking id: " <<p_id * NUM_OF_CAM + i<<" "<<p_id<<"\n";
                    u_of_endpoint.values.push_back(un_lines[j].EndPt.x);
                    v_of_endpoint.values.push_back(un_lines[j].EndPt.y);
                    //ROS_ASSERT(inBorder(cur_pts[j]));
                }
            }

        }
        feature_lines->channels.push_back(id_of_line);
        feature_lines->channels.push_back(u_of_endpoint);
        feature_lines->channels.push_back(v_of_endpoint);
        ROS_DEBUG("publish %f, at %f", feature_lines->header.stamp.toSec(), ros::Time::now().toSec());
        pub_img.publish(feature_lines);

    }
    sum_time += t_r.toc();
    mean_time = sum_time/frame_cnt;
    ROS_INFO("whole Line feature tracker processing costs: %f", mean_time);
}

int main(int argc, char **argv)
{
    //ros初始化和設置句柄
    ros::init(argc, argv, "linefeature_tracker");
    ros::NodeHandle n("~");

    //設置logger的級別。 只有級別大於或等於level的日誌記錄消息纔會得到處理。
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

    //讀取yaml中的一些配置參數
    readParameters(n);

    //讀取每個相機實例對應的相機內參
    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData.readIntrinsicParameter(CAM_NAMES[i]);

    // TODO: 開始線特徵的檢測
    ROS_INFO("start line feature");
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

    // 發佈一些話題,給後端用
    pub_img = n.advertise<sensor_msgs::PointCloud>("linefeature", 1000);
    pub_match = n.advertise<sensor_msgs::Image>("linefeature_img",1000);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
    ros::spin();
    return 0;
}

2.2linefeature_tracker.cpp

#include "linefeature_tracker.h"


LineFeatureTracker::LineFeatureTracker()
{
    allfeature_cnt = 0;
    frame_cnt = 0;
    sum_time = 0.0;
}

void LineFeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());

    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
    K_ = m_camera->initUndistortRectifyMap(undist_map1_,undist_map2_);    

}


// 得到歸一化平面上的座標,得到un_lines
vector<Line> LineFeatureTracker::undistortedLineEndPoints()
{
    vector<Line> un_lines; //歸一化平面座標
    un_lines = curframe_->vecLine;
    float fx = K_.at<float>(0, 0);
    float fy = K_.at<float>(1, 1);
    float cx = K_.at<float>(0, 2);
    float cy = K_.at<float>(1, 2);
    for (unsigned int i = 0; i <curframe_->vecLine.size(); i++)
    {
        un_lines[i].StartPt.x = (curframe_->vecLine[i].StartPt.x - cx)/fx;
        un_lines[i].StartPt.y = (curframe_->vecLine[i].StartPt.y - cy)/fy;
        un_lines[i].EndPt.x = (curframe_->vecLine[i].EndPt.x - cx)/fx;
        un_lines[i].EndPt.y = (curframe_->vecLine[i].EndPt.y - cy)/fy;
    }
    return un_lines;
}


// 另一種線段匹配的方法
// 總體思想是,對當前幀forw_lines的每一個線,遍歷一遍上一幀所有線,尋找一個距離和角度最短的,之後在方向匹配.然後返回lineMatches<上一幀id,本幀id>
void LineFeatureTracker::NearbyLineTracking(const vector<Line> forw_lines, const vector<Line> cur_lines,
                                            vector<pair<int, int> > &lineMatches) {

    float th = 3.1415926/9;
    float dth = 30 * 30;
    // 遍歷當前幀的線
    for (size_t i = 0; i < forw_lines.size(); ++i) {
        Line lf = forw_lines.at(i); //當前幀的線
        Line best_match;
        size_t best_j = 100000;
        size_t best_i = 100000;
        float grad_err_min_j = 100000;
        float grad_err_min_i = 100000;
        vector<Line> candidate;

        // 從 forw --> cur 查找
        //遍歷上一幀的線
        for(size_t j = 0; j < cur_lines.size(); ++j) 
        {
            Line lc = cur_lines.at(j); //上一幀的線


            // condition 1 距離判斷
            Point2f d = lf.Center - lc.Center;
            float dist = d.dot(d);//
            if( dist > dth) continue;  //


            // condition 2 角度判斷
            float delta_theta1 = fabs(lf.theta - lc.theta);
            float delta_theta2 = 3.1415926 - delta_theta1;
        
            // 距離和角度都滿足的話,放入候選 candidate
            if( delta_theta1 < th || delta_theta2 < th)
            {
                //std::cout << "theta: "<< lf.theta * 180 / 3.14259 <<" "<< lc.theta * 180 / 3.14259<<" "<<delta_theta1<<" "<<delta_theta2<<std::endl;
                candidate.push_back(lc);
                //float cost = fabs(lf.image_dx - lc.image_dx) + fabs( lf.image_dy - lc.image_dy) + 0.1 * dist;
                float cost = fabs(lf.line_grad_avg - lc.line_grad_avg) + dist/10.0;

                //std::cout<< "line match cost: "<< cost <<" "<< cost - sqrt( dist )<<" "<< sqrt( dist ) <<"\n\n";
                if(cost < grad_err_min_j)
                {
                    best_match = lc;
                    grad_err_min_j = cost;
                    best_j = j;
                }
            }

        }
        if(grad_err_min_j > 50) continue;  // 沒找到

        //std::cout<< "!!!!!!!!! minimal cost: "<<grad_err_min_j <<"\n\n";

        // 如果 forw --> cur 找到了 best, 那我們反過來再驗證下
        if(best_j < cur_lines.size())
        {
            // 反過來，從 cur --> forw 查找
            Line lc = cur_lines.at(best_j);
            for (int k = 0; k < forw_lines.size(); ++k)
            {
                Line lk = forw_lines.at(k);

                // condition 1
                Point2f d = lk.Center - lc.Center;
                float dist = d.dot(d);
                if( dist > dth) continue;  //
                // condition 2
                float delta_theta1 = fabs(lk.theta - lc.theta);
                float delta_theta2 = 3.1415926 - delta_theta1;
                if( delta_theta1 < th || delta_theta2 < th)
                {
                    //std::cout << "theta: "<< lf.theta * 180 / 3.14259 <<" "<< lc.theta * 180 / 3.14259<<" "<<delta_theta1<<" "<<delta_theta2<<std::endl;
                    //candidate.push_back(lk);
                    //float cost = fabs(lk.image_dx - lc.image_dx) + fabs( lk.image_dy - lc.image_dy) + dist;
                    float cost = fabs(lk.line_grad_avg - lc.line_grad_avg) + dist/10.0;

                    if(cost < grad_err_min_i)
                    {
                        grad_err_min_i = cost;
                        best_i = k;
                    }
                }

            }
        }

        if( grad_err_min_i < 50 && best_i == i){

            //std::cout<< "line match cost: "<<grad_err_min_j<<" "<<grad_err_min_i <<"\n\n";
            lineMatches.push_back(make_pair(best_j,i));
        }
        /*
        vector<Line> l;
        l.push_back(lf);
        vector<Line> best;
        best.push_back(best_match);
        visualizeLineTrackCandidate(l,forwframe_->img,"forwframe_");
        visualizeLineTrackCandidate(best,curframe_->img,"curframe_best");
        visualizeLineTrackCandidate(candidate,curframe_->img,"curframe_");
        cv::waitKey(0);
        */
    }

}


// 下邊這個是採用NearbyLineTracking進行追蹤的?並不同於lsd+lbd
//#define NLT
#ifdef  NLT
void LineFeatureTracker::readImage(const cv::Mat &_img)
{
    cv::Mat img;
    TicToc t_p;
    frame_cnt++;
    cv::remap(_img, img, undist_map1_, undist_map2_, CV_INTER_LINEAR);
    //ROS_INFO("undistortImage costs: %fms", t_p.toc());
    if (EQUALIZE)   // 直方圖均衡化
    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        clahe->apply(img, img);
    }

    bool first_img = false;
    if (forwframe_ == nullptr) // 系統初始化的第一幀圖像
    {
        forwframe_.reset(new FrameLines);
        curframe_.reset(new FrameLines);
        forwframe_->img = img;
        curframe_->img = img;
        first_img = true;
    }
    else
    {
        forwframe_.reset(new FrameLines);  // 初始化一個新的幀
        forwframe_->img = img;
    }

    // step 1: line extraction
    TicToc t_li;

    int lineMethod = 2;
    bool isROI = false;


    lineDetector ld(lineMethod, isROI, 0, (float)img.cols, 0, (float)img.rows);
    //ROS_INFO("ld inition costs: %fms", t_li.toc());
    TicToc t_ld;
    forwframe_->vecLine = ld.detect(img);

    for (size_t i = 0; i < forwframe_->vecLine.size(); ++i) {
        if(first_img)
            forwframe_->lineID.push_back(allfeature_cnt++);
        else
            forwframe_->lineID.push_back(-1);   // give a negative id
    }
    ROS_INFO("line detect costs: %fms", t_ld.toc());

    // step 3: junction & line matching
    if(curframe_->vecLine.size() > 0)
    {
        TicToc t_nlt;
        vector<pair<int, int> > linetracker;//當前幀和上一陣的匹配情況 <上一幀的id,本幀的id>
        NearbyLineTracking(forwframe_->vecLine, curframe_->vecLine, linetracker);
        ROS_INFO("line match costs: %fms", t_nlt.toc());

        // 對新圖像上的line賦予id值
        for(int j = 0; j < linetracker.size(); j++)
        {
            // 匹配到的線id相同
            forwframe_->lineID[linetracker[j].second] = curframe_->lineID[linetracker[j].first];
        }

        // show NLT match
        visualizeLineMatch(curframe_->vecLine, forwframe_->vecLine, linetracker,
                           curframe_->img, forwframe_->img, "NLT Line Matches", 10, true,
                           "frame");
        visualizeLinewithID(forwframe_->vecLine,forwframe_->lineID,forwframe_->img,"forwframe_");
        visualizeLinewithID(curframe_->vecLine,curframe_->lineID,curframe_->img,"curframe_");
        stringstream ss;
        ss <<"/home/hyj/datasets/line/" <<frame_cnt<<".jpg";
        // SaveFrameLinewithID(forwframe_->vecLine,forwframe_->lineID,forwframe_->img,ss.str().c_str());
        waitKey(5);


        vector<Line> vecLine_tracked, vecLine_new;
        vector< int > lineID_tracked, lineID_new;
        // 將跟蹤的線和沒跟蹤上的線進行區分
        for (size_t i = 0; i < forwframe_->vecLine.size(); ++i)
        {
            if( forwframe_->lineID[i] == -1)
            {
                forwframe_->lineID[i] = allfeature_cnt++;
                vecLine_new.push_back(forwframe_->vecLine[i]);
                lineID_new.push_back(forwframe_->lineID[i]);
            }
            else
            {
                vecLine_tracked.push_back(forwframe_->vecLine[i]);
                lineID_tracked.push_back(forwframe_->lineID[i]);
            }
        }
        int diff_n = 30 - vecLine_tracked.size();  // 跟蹤的線特徵少於50了，那就補充新的線特徵, 還差多少條線
        if( diff_n > 0)    // 補充線條
        {
            for (int k = 0; k < vecLine_new.size(); ++k) {
                vecLine_tracked.push_back(vecLine_new[k]);
                lineID_tracked.push_back(lineID_new[k]);
            }
        }

        forwframe_->vecLine = vecLine_tracked;
        forwframe_->lineID = lineID_tracked;

    }
    curframe_ = forwframe_;
}
#endif

#define MATCHES_DIST_THRESHOLD 30//線特徵距離的閾值,30像素,匹配後距離大於這個值,就不要




/*TODO:
可視化線特徵匹配
imageMat1 當前圖像
imageMat2 上一個圖像
octave0_1 當前的LSD
octave0_2 上一個的LSD
good_matches 好的匹配 */
void visualize_line_match(Mat imageMat1, Mat imageMat2,
                          std::vector<KeyLine> octave0_1, std::vector<KeyLine>octave0_2,
                          std::vector<DMatch> good_matches)
{
    //	Mat img_1;
    cv::Mat img1,img2;
    if (imageMat1.channels() != 3){
        cv::cvtColor(imageMat1, img1, cv::COLOR_GRAY2BGR);
    }
    else{
        img1 = imageMat1;
    }

    if (imageMat2.channels() != 3){
        cv::cvtColor(imageMat2, img2, cv::COLOR_GRAY2BGR);
    }
    else{
        img2 = imageMat2;
    }


    //    srand(time(NULL));
    int lowest = 0, highest = 255;
    int range = (highest - lowest) + 1; //=256

    // 遍歷所有的 good_matches
    for (int k = 0; k < good_matches.size(); ++k) {
        DMatch mt = good_matches[k];

        KeyLine line1 = octave0_1[mt.queryIdx];  // trainIdx
        KeyLine line2 = octave0_2[mt.trainIdx];  //queryIdx


        unsigned int r = lowest + int(rand() % range);
        unsigned int g = lowest + int(rand() % range);
        unsigned int b = lowest + int(rand() % range);
        // rand 返回介於0和RAND_MAX（含）之間的隨機整數

        // 將線裏邊的起始點和終止點轉換爲opencv中的點
        cv::Point startPoint = cv::Point(int(line1.startPointX), int(line1.startPointY));
        cv::Point endPoint = cv::Point(int(line1.endPointX), int(line1.endPointY));
        // 在第一個圖像上劃線
        cv::line(img1, startPoint, endPoint, cv::Scalar(r, g, b),2 ,8);


        cv::Point startPoint2 = cv::Point(int(line2.startPointX), int(line2.startPointY));
        cv::Point endPoint2 = cv::Point(int(line2.endPointX), int(line2.endPointY));
        // 在第二個圖像上劃線
        cv::line(img2, startPoint2, endPoint2, cv::Scalar(r, g, b),2, 8);

        // 在第二個圖像上,畫出起始點的連接線-藍色
        cv::line(img2, startPoint, startPoint2, cv::Scalar(0, 0, 255),1, 8);
        // 在第二個圖像上,畫出終止點的連接線
        cv::line(img2, endPoint, endPoint2, cv::Scalar(0, 0, 255),1, 8);

    }
    /* plot matches */
    /*
    cv::Mat lsd_outImg;
    std::vector<char> lsd_mask( lsd_matches.size(), 1 );
    drawLineMatches( imageMat1, octave0_1, imageMat2, octave0_2, good_matches, lsd_outImg, Scalar::all( -1 ), Scalar::all( -1 ), lsd_mask,
    DrawLinesMatchesFlags::DEFAULT );

    imshow( "LSD matches", lsd_outImg );
    */
    imshow("LSD matches1", img1);
    imshow("LSD matches2", img2);
    waitKey(1);
}

void visualize_line_match(Mat imageMat1, Mat imageMat2,
                          std::vector<KeyLine> octave0_1, std::vector<KeyLine>octave0_2,
                          std::vector<bool> good_matches)
{
    //	Mat img_1;
    cv::Mat img1,img2;
    if (imageMat1.channels() != 3){
        cv::cvtColor(imageMat1, img1, cv::COLOR_GRAY2BGR);
    }
    else{
        img1 = imageMat1;
    }

    if (imageMat2.channels() != 3){
        cv::cvtColor(imageMat2, img2, cv::COLOR_GRAY2BGR);
    }
    else{
        img2 = imageMat2;
    }

    //    srand(time(NULL));
    int lowest = 0, highest = 255;
    int range = (highest - lowest) + 1;
    for (int k = 0; k < good_matches.size(); ++k) {

        if(!good_matches[k]) continue;

        KeyLine line1 = octave0_1[k];  // trainIdx
        KeyLine line2 = octave0_2[k];  //queryIdx

        unsigned int r = lowest + int(rand() % range);
        unsigned int g = lowest + int(rand() % range);
        unsigned int b = lowest + int(rand() % range);
        cv::Point startPoint = cv::Point(int(line1.startPointX), int(line1.startPointY));
        cv::Point endPoint = cv::Point(int(line1.endPointX), int(line1.endPointY));
        cv::line(img1, startPoint, endPoint, cv::Scalar(r, g, b),2 ,8);

        cv::Point startPoint2 = cv::Point(int(line2.startPointX), int(line2.startPointY));
        cv::Point endPoint2 = cv::Point(int(line2.endPointX), int(line2.endPointY));
        cv::line(img2, startPoint2, endPoint2, cv::Scalar(r, g, b),2, 8);
        cv::line(img2, startPoint, startPoint2, cv::Scalar(0, 0, 255),1, 8);
        cv::line(img2, endPoint, endPoint2, cv::Scalar(0, 0, 255),1, 8);

    }
    /* plot matches */
    /*
    cv::Mat lsd_outImg;
    std::vector<char> lsd_mask( lsd_matches.size(), 1 );
    drawLineMatches( imageMat1, octave0_1, imageMat2, octave0_2, good_matches, lsd_outImg, Scalar::all( -1 ), Scalar::all( -1 ), lsd_mask,
    DrawLinesMatchesFlags::DEFAULT );

    imshow( "LSD matches", lsd_outImg );
    */
    imshow("LSD matches1", img1);
    imshow("LSD matches2", img2);
    waitKey(1);
}


/*void LineFeatureTracker::readImage(const cv::Mat &_img)
線特徵LSD提取,LBD匹配描述,編號設置,match匹配,轉換爲幾何線特徵
*/
void LineFeatureTracker::readImage(const cv::Mat &_img)
{
    cv::Mat img;
    TicToc t_p;
    frame_cnt++;

    /* cv::remap
    重映射，就是把一幅圖像中某位置的像素放置到另一個圖片指定位置的過程。
    @param src源圖像。
    @param dst目標圖像。 它的大小與map1相同，類型與src相同。
    @param map1（x，y）點或僅x個值的第一個映射，類型爲CV_16SC2，CV_32FC1或CV_32FC2。 有關轉換浮點的詳細信息，請參見convertMaps以定點表示速度。
    @param map2類型爲CV_16UC1，CV_32FC1或無的y值的第二個映射（空映射如果map1是（x，y）個點）。
    @param插值插值方法（請參閱cv :: InterpolationFlags）。 INTER_AREA方法是此功能不支持。
    @param borderMode像素外推方法（請參閱cv :: BorderTypes）。 什麼時候borderMode = BORDER_TRANSPARENT，表示目標圖像中的像素與源圖像中的“異常值”相對應的值未被該功能修改。
    @param borderValue在恆定邊框的情況下使用的值。 默認情況下爲0。   */
    cv::remap(_img, img, undist_map1_, undist_map2_, CV_INTER_LINEAR);

//    cv::imshow("lineimg",img);//TODO:這個是將圖像進行取出畸變,需要查看一下他做了什麼
//    cv::waitKey(1);
    //ROS_INFO("undistortImage costs: %fms", t_p.toc());


    if (EQUALIZE)   // 直方圖均衡化
    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        clahe->apply(img, img);
    }

    bool first_img = false;
    if (forwframe_ == nullptr) // 系統初始化的第一幀圖像
    {
        forwframe_.reset(new FrameLines);
        curframe_.reset(new FrameLines);
        forwframe_->img = img;
        curframe_->img = img;
        first_img = true;
    }
    else
    {
        forwframe_.reset(new FrameLines);  // 初始化一個新的幀
        forwframe_->img = img;
    }

    //TODO: step 1: line extraction
    // 可以在這裏對提取到的線進行處理 FIXME:如何將兩個 KeyLine 合併成一個
    
    TicToc t_li;//線特徵提取時間

    std::vector<KeyLine> lsd, keylsd;
    Ptr<LSDDetector> lsd_; //LSD提取,除此之外,還有一個 EDLineDetector

    // 這裏設置其提取方式爲LSD
    lsd_ = cv::line_descriptor::LSDDetector::createLSDDetector();

    /* detect
    @brief檢測圖像中的線條。
    @param圖像輸入圖像
    @param關鍵點向量，將存儲一個或多個圖像的提取行在金字塔生成中使用的@param比例因子
    @param numOctaves金字塔內的八度數
    @param mask mask矩陣僅檢測感興趣的關鍵線*/
    lsd_->detect( img, lsd, 2, 2 );

    sum_time += t_li.toc();// 線特徵提取+描述的總時間

//    ROS_INFO("line detect costs: %fms", t_li.toc());

    //TODO: step 2: lbd descriptor
    Mat lbd_descr, keylbd_descr;
    TicToc t_lbd;//LBD描述子的時間

    Ptr<BinaryDescriptor> bd_ = BinaryDescriptor::createBinaryDescriptor( );
    bd_->compute( img, lsd, lbd_descr );

    for ( int i = 0; i < (int) lsd.size(); i++ )
    {
        if( lsd[i].octave == 0 && lsd[i].lineLength >= 30)
        {
            keylsd.push_back( lsd[i] );
            keylbd_descr.push_back( lbd_descr.row( i ) );
        }
    }
//    ROS_INFO("lbd_descr detect costs: %fms", keylsd.size() * t_lbd.toc() / lsd.size() );//添加描述子的時間
    sum_time += keylsd.size() * t_lbd.toc() / lsd.size();

    // TODO: 編號的設置
    forwframe_->keylsd = keylsd;
    forwframe_->lbd_descr = keylbd_descr;
    // 遍歷所有的LSD_line
    for (size_t i = 0; i < forwframe_->keylsd.size(); ++i) 
    {   
        // 如果是第一幀,全部給一個編號
        if(first_img)
            forwframe_->lineID.push_back(allfeature_cnt++);
        // 如果不是第一幀,編號全部記爲-1
        else
            forwframe_->lineID.push_back(-1);   // give a negative id
    }

//------------------遍歷所有的lsd 對其進行匹配的相關計算--------------------------------------------
    if(curframe_->keylsd.size() > 0)
    {

        //TODO:線特徵的匹配
        TicToc t_match; //匹配用的時間

        /* DMatch 用於匹配關鍵點描述符
        查詢描述符索引，訓練描述符索引，訓練圖像索引以及描述符之間的距離。*/
        std::vector<DMatch> lsd_matches;

        // 線特徵的匹配
        Ptr<BinaryDescriptorMatcher> bdm_;
        bdm_ = BinaryDescriptorMatcher::createBinaryDescriptorMatcher();
        bdm_->match(forwframe_->lbd_descr, curframe_->lbd_descr, lsd_matches);


//        ROS_INFO("lbd_macht costs: %fms", t_match.toc());
        sum_time += t_match.toc();
        mean_time = sum_time/frame_cnt;
        ROS_INFO("line feature tracker mean costs: %fms", mean_time);

        //TODO: 選擇最好的匹配
        std::vector<DMatch> good_matches; //裏邊放的是篩選後的匹配,匹配距離小於30.起始點和終止點距離小於60
        std::vector<KeyLine> good_Keylines;
        good_matches.clear();
        for ( int i = 0; i < (int) lsd_matches.size(); i++ )
        {
            // 如果距離大於閾值30,就不要
            if( lsd_matches[i].distance < MATCHES_DIST_THRESHOLD ){

                DMatch mt = lsd_matches[i];
                KeyLine line1 =  forwframe_->keylsd[mt.queryIdx] ;
                KeyLine line2 =  curframe_->keylsd[mt.trainIdx] ;
                Point2f serr = line1.getStartPoint() - line2.getStartPoint(); //起始點的誤差
                Point2f eerr = line1.getEndPoint() - line2.getEndPoint(); //終止點的誤差
                if((serr.dot(serr) < 60 * 60) && (eerr.dot(eerr) < 60 * 60))   // 線段在圖像裏不會跑得特別遠
                    // .dot 點乘
                    good_matches.push_back( lsd_matches[i] );
            }

        }


        //TODO: 保證匹配到的線特徵的 lineID 相同
        std::cout << forwframe_->lineID.size() <<" " <<curframe_->lineID.size();
        for (int k = 0; k < good_matches.size(); ++k) 
        {
            DMatch mt = good_matches[k];

            // 讓查詢到的線特徵的 lineID 等於,訓練集中的ID,保證相互匹配的線特徵的ID相同
            forwframe_->lineID[mt.queryIdx] = curframe_->lineID[mt.trainIdx];

        }


        visualize_line_match(forwframe_->img.clone(), curframe_->img.clone(), forwframe_->keylsd, curframe_->keylsd, good_matches);


        vector<KeyLine> vecLine_tracked, vecLine_new;
        vector< int > lineID_tracked, lineID_new;
        Mat DEscr_tracked, Descr_new;

        //TODO: 將跟蹤的線和沒跟蹤上的線進行區分
        for (size_t i = 0; i < forwframe_->keylsd.size(); ++i)
        {
            // 如果是新線
            if( forwframe_->lineID[i] == -1)
            {
                forwframe_->lineID[i] = allfeature_cnt++;//分配一個新的全局lineID
                vecLine_new.push_back(forwframe_->keylsd[i]);
                lineID_new.push_back(forwframe_->lineID[i]);
                Descr_new.push_back( forwframe_->lbd_descr.row( i ) );
            }
            // 跟蹤上的線
            else
            {
                vecLine_tracked.push_back(forwframe_->keylsd[i]);
                lineID_tracked.push_back(forwframe_->lineID[i]);
                DEscr_tracked.push_back( forwframe_->lbd_descr.row( i ) );
            }
        }

        // 跟蹤的線特徵少於50了，那就補充新的線特徵, 還差多少條線
        int diff_n = 50 - vecLine_tracked.size();
        if( diff_n > 0)    // 補充線條
        {
            // 距離20還差多少線,就從新線裏邊放入多少
            for (int k = 0; k < vecLine_new.size(); ++k) {
                vecLine_tracked.push_back(vecLine_new[k]);
                lineID_tracked.push_back(lineID_new[k]);
                DEscr_tracked.push_back(Descr_new.row(k));
            }

        }

        將20個線特徵放放入到forwframe_
        forwframe_->keylsd = vecLine_tracked;
        forwframe_->lineID = lineID_tracked;
        forwframe_->lbd_descr = DEscr_tracked;

    }

    //TODO: 將opencv的KeyLine數據轉爲幾何的Line
    // 或者在這裏進行線特徵的整合處理
    for (int j = 0; j < forwframe_->keylsd.size(); ++j) {
        Line l;
        KeyLine lsd = forwframe_->keylsd[j];
        l.StartPt = lsd.getStartPoint();
        l.EndPt = lsd.getEndPoint();
        l.length = lsd.lineLength;
        forwframe_->vecLine.push_back(l);
    }
    curframe_ = forwframe_;


}

2.3linefeature_tracker.h

#pragma once

#include <iostream>
#include <queue>

#include "camodocal/camera_models/CameraFactory.h"
#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"
#include "camodocal/camera_models/EquidistantCamera.h"

#include "parameters.h"
#include "tic_toc.h"

#include <opencv2/line_descriptor.hpp>
#include <opencv2/features2d.hpp>

using namespace cv::line_descriptor;
using namespace std;
using namespace cv;
using namespace camodocal;

struct Line
{
	Point2f StartPt;
	Point2f EndPt;
	float lineWidth;
	Point2f Vp;

	Point2f Center; //FIXME:
	Point2f unitDir; // [cos(theta), sin(theta)]
	float length;
	float theta;

	// para_a * x + para_b * y + c = 0
	float para_a;
	float para_b;
	float para_c;

	float image_dx;
	float image_dy;
    float line_grad_avg;

	float xMin;
	float xMax;
	float yMin;
	float yMax;
	unsigned short id;
	int colorIdx;
};


//TODO: 線特徵的類
class FrameLines
{
public:
    int frame_id;
    Mat img;
    
    vector<Line> vecLine;// 幾何線特徵
    vector< int > lineID; //線特徵中的編號,用於查找匹配的線

    // opencv3 lsd+lbd
    std::vector<KeyLine> keylsd;
    Mat lbd_descr;
};
typedef shared_ptr< FrameLines > FrameLinesPtr;

class LineFeatureTracker
{
  public:
    LineFeatureTracker();

    void readIntrinsicParameter(const string &calib_file);
    void NearbyLineTracking(const vector<Line> forw_lines, const vector<Line> cur_lines, vector<pair<int, int> >& lineMatches);

    vector<Line> undistortedLineEndPoints();

    void readImage(const cv::Mat &_img);

    FrameLinesPtr curframe_, forwframe_;//當前幀的所有線特徵

    cv::Mat undist_map1_, undist_map2_ , K_;

    camodocal::CameraPtr m_camera;       // pinhole camera

    int frame_cnt; //一共多少幀圖像
    vector<int> ids;                     // 每個特徵點的id
    vector<int> linetrack_cnt;           // 記錄某個特徵已經跟蹤多少幀了，即被多少幀看到了
    int allfeature_cnt; //用來統計整個地圖中有了多少條線，它將用來賦值

    double sum_time;
    double mean_time;
};