点线结合 PL-VINS ：线特征提取(feature-tracker) 代码整理笔记

PL-VIO代码地址：https://github.com/HeYijia/PL-VIO
PL-VINS代码地址：https://github.com/cnqiangfu/PL-VINS.

本文重点关注PL-VINS该框架下的LSD线特征提取部分的代码整理解读，感谢原作者老师们的分享。

在PL-VIO中直接调用了OpenCV旧版本的LSD线段检测器

#include <opencv2/line_descriptor/descriptor.hpp>
using namespace cv::line_descriptor;

但可能由于著作权等大人的原因，OpenCV3.4.6及以后版本已经将LSD线特征提取算法从代码中剔除了。
因此我们可以选择使用旧版本的OpenCV，亦或者基于新版本OpenCV把原LSD库代码打包成库，加入我们的工程代码中即可直接调用。

linefeature_tracker.h

// #include <opencv2/line_descriptor.hpp>
#include "line_descriptor_custom.hpp"

在项目中include自己生成的 line_descriptor_custom.hpp 库即可使用。

---

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

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

虽然代码中给线特征定义了许多属性，但仅有两个端点的坐标和线段长度被用到，在运行LineFeatureTracker::readImage(const cv::Mat &_img) 读取图片并提取线段时，将由LSD库的线段检测算子所提取出的KeyLine类线特征转换成本框架下所定义的到的Line类线特征，并赋值端点坐标和线段长度。

---

class FrameLines
{
public:
    int frame_id;
    Mat img;
    
    vector<Line> vecLine;
    vector< int > lineID;

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

包括一帧图像中所提取出的所有KeyLine线特征和对应的LBD描述子，以及转换后的Line线特征及它们对应的ID号。

---

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

见注释。

---

linefeature_tracker.cpp

Tips： 该cpp代码中还留有贺博他们内部自己写的线段检测算法（非OpenCV的LSD库），即

//#define NLT
#ifdef  NLT
void LineFeatureTracker::readImage(const cv::Mat &_img)
{
	...
	lineDetector ld(lineMethod, isROI, 0, (float)img.cols, 0, (float)img.rows); 
	...
	NearbyLineTracking(forwframe_->vecLine, curframe_->vecLine, linetracker);
	...
}
#endif

但公开代码中用到的实际上还是OpenCV的LSD库，既没有lineDetector的定义，也没有用到NearbyLineTracking这个函数，因此#define NLT到#endif之间包括的代码无视即可。

---

1. 在读取图像并提取线段特征之前，会先进行内参读取和图像去畸变

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

读取内参矩阵，生成图像去畸变的映射表 undist_map1_ 和 undist_map2_， 用于后续的 cv::remap(_img, img, undist_map1_, undist_map2_, CV_INTER_LINEAR);
其中，

void cv::remap(
	InputArray src,// 输入图像
	OutputArray dst,// 输出图像
	InputArray map1,// x 映射表 元素值的数据类型需要是CV_32FC1/CV_32FC2
	InputArray map2,// y 映射表
	int interpolation,// 选择的插值方法，常见线性插值，可选择立方等
	int borderMode,// BORDER_CONSTANT
	const Scalar borderValue// borderMode设置为BORDER_CONSTANT时才有用，填充INTER_LINEAR插值时没有计算到的像素点
)

---

2. 从图像中进行线特征的提取、跟踪和补充

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);
 	...
 	(省略一些初始化操作)
 	...
 	Ptr<line_descriptor::LSDDetectorC> lsd_ = line_descriptor::LSDDetectorC::createLSDDetectorC();
    // lsd parameters
    line_descriptor::LSDDetectorC::LSDOptions opts;
    opts.refine       = 1;     //1     	The way found lines will be refined
    opts.scale        = 0.5;   //0.8   	The scale of the image that will be used to find the lines. Range (0..1].
    opts.sigma_scale  = 0.6;	//0.6  	Sigma for Gaussian filter. It is computed as sigma = _sigma_scale/_scale.
    opts.quant        = 2.0;	//2.0   Bound to the quantization error on the gradient norm
    opts.ang_th       = 22.5;	//22.5	Gradient angle tolerance in degrees
    opts.log_eps      = 1.0;	//0		Detection threshold: -log10(NFA) > log_eps. Used only when advance refinement is chosen
    opts.density_th   = 0.6;	//0.7	Minimal density of aligned region points in the enclosing rectangle.
    opts.n_bins       = 1024;	//1024 	Number of bins in pseudo-ordering of gradient modulus.
    double min_line_length = 0.125;  // Line segments shorter than that are rejected
    opts.min_length   = min_line_length*(std::min(img.cols,img.rows));
    std::vector<KeyLine> lsd, keylsd;
    lsd_->detect( img, lsd, 2, 1, opts);
}

• Ptr<line_descriptor::LSDDetectorC> lsd_ = line_descriptor::LSDDetectorC::createLSDDetectorC();
  生成一个LSD的线段检测器lsd_， 并设置相应的参数opt（见注释，通常采用默认即可）
• std::vector lsd, keylsd;
  构造存储线特征的容器 lsd 和关键线特征的容器 keylsd。
• lsd_->detect( img, lsd, 2, 1, opts);
  函数如下，

void cv::line_descriptor::LSDDetector::detect(	
	const std::vector< Mat > & 	images, 
	std::vector< std::vector< KeyLine > > & 	keylines,
	int 	scale,   //图像金字塔构的尺度因子
	int 	numOctaves, // 图像金字塔的octave阶数
	LSDOptions opts  // 所用参数 
)	

---

	...（省略）
	Mat lbd_descr, keylbd_descr;
    // step 2: lbd descriptor
    TicToc t_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 >= 60)
        {
            keylsd.push_back( lsd[i] );
            keylbd_descr.push_back( lbd_descr.row( i ) );
        }
    }

进一步计算线特征对应描述子，并根据octave和线段长度生成关键线特征的集合，同时添加描述子。

---

	forwframe_->keylsd = keylsd;
    forwframe_->lbd_descr = keylbd_descr;

    for (size_t i = 0; i < forwframe_->keylsd.size(); ++i) {
        if(first_img)
            forwframe_->lineID.push_back(allfeature_cnt++);
        else
            forwframe_->lineID.push_back(-1);   // give a negative id
    }

赋值给当前帧forw_frame， 若为第一帧则为每条线特征赋予id, 否则先给一个-1的id。

---

若前一帧的关键线特征个数大于0（不是第一帧）

	if(curframe_->keylsd.size() > 0)

则开始进行线特征跟踪和匹配。

	{
		std::vector<DMatch> lsd_matches;
        Ptr<BinaryDescriptorMatcher> bdm_;
        bdm_ = BinaryDescriptorMatcher::createBinaryDescriptorMatcher();
        bdm_->match(forwframe_->lbd_descr, curframe_->lbd_descr, lsd_matches);

        sum_time += t_match.toc();
        mean_time = sum_time/frame_cnt;

        /* select best matches */
        std::vector<DMatch> good_matches;
        std::vector<KeyLine> good_Keylines;
        good_matches.clear();
        for ( int i = 0; i < (int) lsd_matches.size(); i++ )
        {
            if( lsd_matches[i].distance < 30 ){

                DMatch mt = lsd_matches[i];
                KeyLine line1 =  forwframe_->keylsd[mt.queryIdx] ;
                KeyLine line2 =  curframe_->keylsd[mt.trainIdx] ;
                Point2f serr = line1.getStartPoint() - line2.getEndPoint();
                Point2f eerr = line1.getEndPoint() - line2.getEndPoint();

                if((serr.dot(serr) < 200 * 200) && (eerr.dot(eerr) < 200 * 200)&&abs(line1.angle-line2.angle)<0.1)   // 线段在图像里不会跑得特别远
                    good_matches.push_back( lsd_matches[i] );
            }
        }
        vector< int > success_id;
        // std::cout << forwframe_->lineID.size() <<" " <<curframe_->lineID.size();
        for (int k = 0; k < good_matches.size(); ++k) {
            DMatch mt = good_matches[k];
            forwframe_->lineID[mt.queryIdx] = curframe_->lineID[mt.trainIdx];
            success_id.push_back(curframe_->lineID[mt.trainIdx]);
        }

首先构造二进制描述子匹配器并在前一帧和当前帧的线特征中进行匹配，其中

cv::DMatch::DMatch	(	
	int 	_queryIdx, //查询线的索引（当前要寻找匹配结果的线在它所在图片上的索引）
	int 	_trainIdx, //被查询到的线的索引（存储库中的线的在存储库上的索引）
	float 	_distance  //两个描述子之间的距离
)

然后，根据DMatch的描述子距离进行阈值判断，进一步根据前后帧的线端点的移动距离判断是否为good matches，在success中保存对应的ID（但是后续代码没用上）。

---

		//把没追踪到的线存起来

        vector<KeyLine> vecLine_tracked, vecLine_new;
        vector< int > lineID_tracked, lineID_new;
        Mat DEscr_tracked, Descr_new;
        // 将跟踪的线和没跟踪上的线进行区分
        for (size_t i = 0; i < forwframe_->keylsd.size(); ++i)
        {
            if( forwframe_->lineID[i] == -1) //若没跟踪到
            {
                forwframe_->lineID[i] = allfeature_cnt++;
                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 ) );
            }
        }

如注释，将跟踪到的线和未跟踪到的线（新线）分别加入到vecLine_tracked 和 vecLine_new中。

---

		vector<KeyLine> h_Line_new, v_Line_new;
        vector< int > h_lineID_new,v_lineID_new;
        Mat h_Descr_new,v_Descr_new;
        for (size_t i = 0; i < vecLine_new.size(); ++i)
        {
            if((((vecLine_new[i].angle >= 3.14/4 && vecLine_new[i].angle <= 3*3.14/4))||(vecLine_new[i].angle <= -3.14/4 && vecLine_new[i].angle >= -3*3.14/4)))
            {
                h_Line_new.push_back(vecLine_new[i]);
                h_lineID_new.push_back(lineID_new[i]);
                h_Descr_new.push_back(Descr_new.row( i ));
            }
            else
            {
                v_Line_new.push_back(vecLine_new[i]);
                v_lineID_new.push_back(lineID_new[i]);
                v_Descr_new.push_back(Descr_new.row( i ));
            }      
        }

通过一个简单的线段角度判断（相对于图像x轴的斜率）来识别未跟踪到的线（新线）为水平线（horizontal line, $[\pi ,3\pi /4]$ or $[-3\pi /4,-\pi /4]$）还是竖直线（vertical line, 其他角度）。

---

		int h_line,v_line;
        h_line = v_line =0;
        for (size_t i = 0; i < vecLine_tracked.size(); ++i)
        {
            if((((vecLine_tracked[i].angle >= 3.14/4 && vecLine_tracked[i].angle <= 3*3.14/4))||(vecLine_tracked[i].angle <= -3.14/4 && vecLine_tracked[i].angle >= -3*3.14/4)))
            {
                h_line ++;
            }
            else
            {
                v_line ++;
            }
        }
        int diff_h = 35 - h_line;
        int diff_v = 35 - v_line;

        if( diff_h > 0)    // 补充线条
        {
            int kkk = 1;
            if(diff_h > h_Line_new.size())
                diff_h = h_Line_new.size();
            else 
                kkk = int(h_Line_new.size()/diff_h);
            for (int k = 0; k < diff_h; ++k) 
            {
                vecLine_tracked.push_back(h_Line_new[k]);
                lineID_tracked.push_back(h_lineID_new[k]);
                DEscr_tracked.push_back(h_Descr_new.row(k));
            }
        }
        if( diff_v > 0)    // 补充线条
        {
            int kkk = 1;
            if(diff_v > v_Line_new.size())
                diff_v = v_Line_new.size();
            else 
                kkk = int(v_Line_new.size()/diff_v);
            for (int k = 0; k < diff_v; ++k)  
            {
                vecLine_tracked.push_back(v_Line_new[k]);
                lineID_tracked.push_back(v_lineID_new[k]);
                DEscr_tracked.push_back(v_Descr_new.row(k));
            }    
        }
        forwframe_->keylsd = vecLine_tracked;
        forwframe_->lineID = lineID_tracked;
        forwframe_->lbd_descr = DEscr_tracked;
    }

同理，把已跟踪的线进行水平线和竖直线的分类，进一步判断是否满足所要求的数量，不满足的话则补充新线。
最后，更新当前帧的 关键线特征及其描述子和ID。

---

	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_; //若是第一帧，则不进行任何操作并赋当前帧给前一帧， continue

把KeyLine线特征转化为本框架下使用的Line线特征，可见只使用了端点坐标和线段长度。

---

3. 线端点坐标转换为归一化坐标，最终发布

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

简单的用内参矩阵把端点的像素坐标转化为归一化屏幕坐标。
在原代码的linefeature_tracker_node.cpp中，使用的仍是sensor_msgs::PointCloudPtr的点云消息格式，把线的end point坐标存储到点的速度中去了而已，通过ros进行发布。

这样，结合特征点的提取，我们就可以同时对连续帧图像的点线特征进行处理，进一步的利用了图像信息，如图所示。

线的参数化和基于线的优化等，待后续整理

---

新建了一个点线SLAM/结构化SLAM交流群，可以私信我加入，欢迎新人同学讨论和行业大佬指导