PL-VIO学习+注释
文章目录
- 1说明
- 2注释代码
- 2.1`linefeature_tracker_node.cpp`
- 2.2`linefeature_tracker.cpp`
- 2.3`linefeature_tracker.h`
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
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<<" "<
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: "<
// 如果 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<<" "<
//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: "<
lineMatches.push_back(make_pair(best_j,i));
}
/*
vector l;
l.push_back(lf);
vector 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 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 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