点线结合PL-VINS：线特征提取（feature-tracker）代码整理笔记linefeature_tracker
本⽂重点关注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,(ls,0,(ws);
.
..
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.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::ws));
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( w( 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 += ();
mean_time = sum_time/frame_cnt;
/* select best matches */
rectangle函数opencv
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_->ainIdx];
Point2f serr = StartPoint()- EndPoint();
Point2f eerr = EndPoint()- EndPoint();
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_->ainIdx];
success_id.push_back(curframe_->ainIdx]);
}
⾸先构造⼆进制描述⼦匹配器并在前⼀帧和当前帧的线特征中进⾏匹配，其中
cv::DMatch::DMatch(
int  _queryIdx,//查询线的索引（当前要寻匹配结果的线在它所在图⽚上的索引）
int  _trainIdx,//被查询到的线的索引（存储库中的线的在存储库上的索引）
float  _distance  //两个描述⼦之间的距离
)
然后，根据DMatch的描述⼦距离进⾏阈值判断，进⼀步根据前后帧的线端点的移动距离判断是否为good matches，在success中保存对应的ID（但是后续代码没⽤上）。

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。