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;
};