PL-VIO学习+注释
2024-05-14 07:57:09
文章目录
- 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 <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 0vector<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 controlif (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;}}elsePUB_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);// readImageTicToc 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 headergeometry_msgs/Point32[] pointssensor_msgs/ChannelFloat32[] channels */ sensor_msgs::PointCloudPtr feature_lines(new sensor_msgs::PointCloud);sensor_msgs::ChannelFloat32 id_of_line; // feature idsensor_msgs::ChannelFloat32 u_of_endpoint; // usensor_msgs::ChannelFloat32 v_of_endpoint; // vfeature_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; //lineIDfor (unsigned int j = 0; j < ids.size(); j++){int p_id = ids[j]; //第j个线的idhash_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;// 距离和角度都满足的话,放入候选 candidateif( 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 1Point2f d = lk.Center - lc.Center;float dist = d.dot(d);if( dist > dth) continue; //// condition 2float 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 extractionTicToc 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++);elseforwframe_->lineID.push_back(-1); // give a negative id}ROS_INFO("line detect costs: %fms", t_ld.toc());// step 3: junction & line matchingif(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 matchvisualizeLineMatch(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_matchesfor (int k = 0; k < good_matches.size(); ++k) {DMatch mt = good_matches[k];KeyLine line1 = octave0_1[mt.queryIdx]; // trainIdxKeyLine line2 = octave0_2[mt.trainIdx]; //queryIdxunsigned 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]; // trainIdxKeyLine line2 = octave0_2[k]; //queryIdxunsigned 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// 这里设置其提取方式为LSDlsd_ = 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 descriptorMat 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_linefor (size_t i = 0; i < forwframe_->keylsd.size(); ++i) { // 如果是第一帧,全部给一个编号if(first_img)forwframe_->lineID.push_back(allfeature_cnt++);// 如果不是第一帧,编号全部记为-1elseforwframe_->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.起始点和终止点距离小于60std::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++;//分配一个新的全局lineIDvecLine_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 = 0float 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+lbdstd::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 cameraint frame_cnt; //一共多少帧图像vector<int> ids; // 每个特征点的idvector<int> linetrack_cnt; // 记录某个特征已经跟踪多少帧了，即被多少帧看到了int allfeature_cnt; //用来统计整个地图中有了多少条线，它将用来赋值double sum_time;double mean_time;
};
PL-VIO学习+注释相关推荐
- 判断题:oracle自带的sql语言环境是pl/sql,Oracle之PL/SQL学习笔记之数据类型(三)
Oracle之PL/SQL学习笔记之数据类型(三) 所有的编程语言中变量是使用最频繁的.PL/SQL作为一个面向过程的数据库编程语言同样少不了变量,利用变量可以把PL/SQL块需要的参数传递进来,做到 ...
- oracle pl sql示例,oracle PL SQL学习案例（一）
oracle PL SQL学习案例(一) [示例1.1] 查询雇员编号为7788的雇员姓名和工资. 步骤1:用SCOTT/TIGER账户登录SQL*Plus. 步骤2:在输入区输入以下程序: /*这 ...
- Oracle之PL/SQL学习笔记之有名块练习
2019独角兽企业重金招聘Python工程师标准>>> Oracle之PL/SQL学习笔记之有名块练习 存储过程案例: 案例1: 根据雇员姓名跟新雇员工资,如果雇员不存在输出没有该雇 ...
- 手写VIO学习总结（二）
文章目录 1. 作业1 1.1 方法1 2. 作业2 系列笔记: 手写VIO学习总结(一) 1. 作业1 1.1 方法1 1 利用vio_data_simulation-ros_version生成im ...
- VIO学习笔记（二）—— IMU 传感器
学习资料是深蓝学院的<从零开始手写VIO>课程,对课程做一些记录,方便自己以后查询,如有错误还请斧正.由于习惯性心算公式,所以为了加深理解,文章公式采用手写的形式. VIO学习笔记(一)- ...
- 蜘蛛纸牌java注释_GitHub - tangguangyao/SpiderSolitaire: 蜘蛛纸牌源码学习+注释
SpiderSolitaire 蜘蛛纸牌源码学习+注释 源码分析思路: 首先,用了2个构造函数Spider和Poker Spider构造函数拥有以下方法: init:函数初始化 start:游戏开始 ...
- oracle - - 注释符,Oracle数据库扩展语言PL/SQL之注释、分隔符和标识符
点击蓝字关注我吧 [本文详细介绍了Oracle数据库扩展语言PL/SQL的注释.分隔符和标识符,欢迎读者朋友们阅读.转发和收藏!] 1 基本概念 1.1 注释 注释不会被数据库编译,只是给开发人提供一 ...
- 小猿圈python学习-注释
随着学习的深入,用不了多久,你就可以写复杂的上千甚至上万行的代码啦,有些代码你花了很久写出来,过了些天再回去看,发现竟然看不懂了,哈哈,这太正常了. 另外,你以后在工作中会发现,一个项目多是由几个甚至 ...
- python学习-注释、语法、整数、浮点数初步接触
文章目录 注释 编码特点 自定义函数 题外话 注释 井号(#),规范写法是"# " 井号+空格 python的代码风格是PEP8 python能够根据赋值自己定义数据类型,不需要单 ...
- linux下Makefile学习--注释很好
什么是makefile?或许很多Winodws的程序员都不知道这个东西,因为那些Windows的IDE都 为你做了这个工作,但我觉得要作一个好的和 professional的程序员,makefile还 ...
最新文章
- QCustomPlot的简单用法总结
- 数据同步关于去除乱码插入mysql数据库
- vs目录(继承的值)配置
- iOS 高效的分页加载
- Mac OS使用技巧之十五:快捷方便的Mini Dock
- [AtCoder Educational DP Contest] J - Sushi（期望dp）
- python模拟超市抹零_Python基础练习二超市存包柜模拟
- word文本框被锁定解锁_如何在Word中锁定文本框
- Python 之 PyCharm + selenium 163邮箱登录详解
- 【硬件深似海】磁珠选型规范
- 为什么mac Safari提示:打不开该网页,因为无法与服务器建立安全的链接？
- jeeplus框架代码生成器功能的使用
- 1468 - 平方矩阵 Python
- vue循环jq渲染网页页面
- ABC157 D - Friend Suggestions 题解
- ODBC 编程API
- python 大智慧股池_跟我从零开始学会大智慧股票池自动交易
- ViewPager添加圆点
- 哪些“星战”技术会变为现实
- 便于查询增加索引文件 c语言,英汉电子词典小项目总结