[pl-slam]stereoFrameHandler
Posted 奋斗的妮子
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了[pl-slam]stereoFrameHandler相关的知识，希望对你有一定的参考价值。
初始化并运行StVO时，创建了一个新的StereoFrameHandler的指针类：
StereoFrameHandler* StVO = new StereoFrameHandler(cam_pin);
该类的参数是一个双目相机的相机模型。
有以下变量：
1 // adaptative fast 2 int orb_fast_th; //orb特征提取FAST的threshold 3 4 list<PointFeature*> matched_pt; //匹配的特征点集合 5 list<LineFeature*> matched_ls; //匹配的线特征集合 6 7 StereoFrame* prev_frame; //前一帧 8 StereoFrame* curr_frame; //当前帧 9 PinholeStereoCamera* cam; //针孔相机模型 10 11 int n_inliers, n_inliers_pt, n_inliers_ls; //内点数量 12 13 // slam-specific variables 14 bool prev_f_iskf; //一个flag，前一帧是否为关键帧 15 double entropy_first_prevKF; //熵，论文中提到过的一个变量，将代表不确定性的协方差转换为一个标量，称之为entropy。 16 Matrix4d T_prevKF; //前一关键帧的位姿 17 Matrix6d cov_prevKF_currF;
有以下函数：
1.构造函数和析构函数：
定义：
StereoFrameHandler( PinholeStereoCamera* cam_ );
~StereoFrameHandler();
函数体：
StereoFrameHandler::StereoFrameHandler( PinholeStereoCamera *cam_ ) : cam(cam_) {}
StereoFrameHandler::~StereoFrameHandler(){}
2.初始化函数：
定义： void initialize( const Mat img_l_, const Mat img_r_, const int idx_);
参数：左、右相机img，帧数索引
就是把左右相机的首帧作为前一帧，并把它们的位姿初始化为单位矩阵。同时从config中获取ORB提取特征的threshold。
1 void StereoFrameHandler::initialize(const Mat img_l_, const Mat img_r_ , const int idx_) 2 { 3 prev_frame = new StereoFrame( img_l_, img_r_, idx_, cam ); //新建一个双目帧 4 prev_frame->extractInitialStereoFeatures(); //提取初始化双目图像特征点、线 5 prev_frame->Tfw = Matrix4d::Identity(); //前一帧的转换矩阵，定位初始位置，用单位矩阵 6 prev_frame->Tfw_cov = Matrix6d::Identity(); //Tfw_cov是什么？ 7 prev_frame->DT = Matrix4d::Identity(); //DT是什么？ 8 // variables for adaptative FAST 9 orb_fast_th = Config::orbFastTh(); //orb提取特征的threshold 10 // SLAM variables for KF decision 11 T_prevKF = Matrix4d::Identity(); 12 cov_prevKF_currF = Matrix6d::Zero(); 13 prev_f_iskf = true; //前一帧是否为关键帧 14 }
3.更新图像帧
定义： void updateFrame();
（1）根据优化结果或者提取的特征数量跟新FAST角点提取时的threshold；
（2）清除并更新变量，变量是：prev_frame和curr_frame。
4.插入双目图像对
定义： void insertStereoPair(const Mat img_l_, const Mat img_r_, const int idx_);
1 void StereoFrameHandler::insertStereoPair(const Mat img_l_, const Mat img_r_ , const int idx_) 2 { 3 curr_frame = new StereoFrame( img_l_, img_r_, idx_, cam ); 4 curr_frame->extractStereoFeatures( orb_fast_th ); //提取图像对特征 5 f2fTracking(); //帧间跟踪 6 }
5.帧间跟踪
定义： void f2fTracking();
分为基于点特征的帧间跟踪和基于线特征的帧间跟踪。
（1）基于点特征的帧间跟踪：
①匹配描述子时，使用暴力匹配，Hamming距离；
②分别计算前一帧到当前帧的匹配 pmatches_12，和当前帧到前一帧的匹配 pmatches_21；
③把以上两种匹配距离都以描述子距离从小到大的的方式排序；
④然后，遍历匹配 pmatches_12，如果 pmatches_12 的询问点和 pmatches_21的训练点是一样的，并且 pmatches_12 的最佳匹配距离比次优匹配的距离大于Config::minRatio12P()，则认为这个点特征是内点，并把该点放到匹配点集 matched_pt中。
当然，用不用这种互匹配方式来选择内点，可以根据参数 Config::bestLRMatches() 来选择。
1 // points f2f tracking 2 matched_pt.clear(); 3 if( Config::hasPoints() && !(curr_frame->stereo_pt.size()==0) && !(prev_frame->stereo_pt.size()==0) ) 4 { 5 BFMatcher* bfm = new BFMatcher( NORM_HAMMING, false ); // cross-check 6 Mat pdesc_l1, pdesc_l2; 7 vector<vector<DMatch>> pmatches_12, pmatches_21; 8 // 12 and 21 matches 9 pdesc_l1 = prev_frame->pdesc_l; 10 pdesc_l2 = curr_frame->pdesc_l; 11 if( Config::bestLRMatches() ) 12 { 13 if( Config::lrInParallel() ) 14 { 15 auto match_l = async( launch::async, &StereoFrame::matchPointFeatures, prev_frame, bfm, pdesc_l1, pdesc_l2, ref(pmatches_12) ); 16 auto match_r = async( launch::async, &StereoFrame::matchPointFeatures, prev_frame, bfm, pdesc_l2, pdesc_l1, ref(pmatches_21) ); 17 match_l.wait(); 18 match_r.wait(); 19 } 20 else 21 { 22 bfm->knnMatch( pdesc_l1, pdesc_l2, pmatches_12, 2); 23 bfm->knnMatch( pdesc_l2, pdesc_l1, pmatches_21, 2); 24 } 25 } 26 else 27 bfm->knnMatch( pdesc_l1, pdesc_l2, pmatches_12, 2); 28 // sort matches by the distance between the best and second best matches 29 double nn12_dist_th = Config::minRatio12P(); 30 // resort according to the queryIdx 31 sort( pmatches_12.begin(), pmatches_12.end(), sort_descriptor_by_queryIdx() ); 32 if( Config::bestLRMatches() ) 33 sort( pmatches_21.begin(), pmatches_21.end(), sort_descriptor_by_queryIdx() ); 34 // bucle around pmatches 35 for( int i = 0; i < pmatches_12.size(); i++ ) 36 { 37 // check if they are mutual best matches 38 int lr_qdx = pmatches_12[i][0].queryIdx; 39 int lr_tdx = pmatches_12[i][0].trainIdx; 40 int rl_tdx; 41 if( Config::bestLRMatches() ) 42 rl_tdx = pmatches_21[lr_tdx][0].trainIdx; 43 else 44 rl_tdx = lr_qdx; 45 // check if they are mutual best matches and the minimum distance 46 double dist_nn = pmatches_12[i][0].distance; 47 double dist_12 = pmatches_12[i][0].distance / pmatches_12[i][1].distance; 48 if( lr_qdx == rl_tdx && dist_12 > nn12_dist_th ) 49 { 50 PointFeature* point_ = prev_frame->stereo_pt[lr_qdx]; 51 point_->pl_obs = curr_frame->stereo_pt[lr_tdx]->pl; 52 point_->inlier = true; 53 matched_pt.push_back( point_ ); 54 curr_frame->stereo_pt[lr_tdx]->idx = prev_frame->stereo_pt[lr_qdx]->idx; // prev idx 55 } 56 } 57 }
（2）基于线特征的帧间跟踪：
①提取线特征方法：LSD——Line Segment Detector，具有高精确性和可重复性；
②线特征描述子：用LBD——Line Band Descriptor方法，原文中是说：allows us to find correspondences between lines based on their local appearance。
③确定内点的方法和点特征的类似，互相匹配检测，以及这个线特征是否有足够的意义；
④利用线段提供的有用的几何信息来过滤出不同方向和长度的线条。
1 // line segments f2f tracking 2 matched_ls.clear(); 3 if( Config::hasLines() && !(curr_frame->stereo_ls.size()==0) && !(prev_frame->stereo_ls.size()==0) ) 4 { 5 BFMatcher* bfm = new BFMatcher( NORM_HAMMING, false ); // cross-check 6 Mat ldesc_l1, ldesc_l2; 7 vector<vector<DMatch>> lmatches_12, lmatches_21; 8 // 12 and 21 matches 9 ldesc_l1 = prev_frame->ldesc_l; 10 ldesc_l2 = curr_frame->ldesc_l; 11 if( Config::bestLRMatches() ) 12 { 13 if( Config::lrInParallel() ) 14 { 15 auto match_l = async( launch::async, &StereoFrame::matchLineFeatures, prev_frame, bfm, ldesc_l1, ldesc_l2, ref(lmatches_12) ); 16 auto match_r = async( launch::async, &StereoFrame::matchLineFeatures, prev_frame, bfm, ldesc_l2, ldesc_l1, ref(lmatches_21) ); 17 match_l.wait(); 18 match_r.wait(); 19 } 20 else 21 { 22 bfm->knnMatch( ldesc_l1,ldesc_l2, lmatches_12, 2); 23 bfm->knnMatch( ldesc_l2,ldesc_l1, lmatches_21, 2); 24 } 25 } 26 else 27 bfm->knnMatch( ldesc_l1,ldesc_l2, lmatches_12, 2); 28 // sort matches by the distance between the best and second best matches 29 double nn_dist_th, nn12_dist_th; 30 curr_frame->lineDescriptorMAD(lmatches_12,nn_dist_th, nn12_dist_th); 31 nn12_dist_th = nn12_dist_th * Config::descThL(); 32 // resort according to the queryIdx 33 sort( lmatches_12.begin(), lmatches_12.end(), sort_descriptor_by_queryIdx() ); 34 if( Config::bestLRMatches() ) 35 sort( lmatches_21.begin(), lmatches_21.end(), sort_descriptor_by_queryIdx() ); 36 // bucle around pmatches 37 for( int i = 0; i < lmatches_12.size(); i++ ) 38 { 39 // check if they are mutual best matches 40 int lr_qdx = lmatches_12[i][0].queryIdx; 41 int lr_tdx = lmatches_12[i][0].trainIdx; 42 int rl_tdx; 43 if( Config::bestLRMatches() ) 44 rl_tdx = lmatches_21[lr_tdx][0].trainIdx; 45 else 46 rl_tdx = lr_qdx; 47 // check if they are mutual best matches and the minimum distance 48 double dist_12 = lmatches_12[i][1].distance - lmatches_12[i][0].distance; 49 if( lr_qdx == rl_tdx && dist_12 > nn12_dist_th ) 50 { 51 LineFeature* line_ = prev_frame->stereo_ls[lr_qdx]; 52 line_->sdisp_obs = curr_frame->stereo_ls[lr_tdx]->sdisp; 53 line_->edisp_obs = curr_frame->stereo_ls[lr_tdx]->edisp; 54 line_->spl_obs = curr_frame->stereo_ls[lr_tdx]->spl; 55 line_->epl_obs = curr_frame->stereo_ls[lr_tdx]->epl; 56 line_->le_obs = curr_frame->stereo_ls[lr_tdx]->le; 57 line_->inlier = true; 58 matched_ls.push_back( line_ ); 59 curr_frame->stereo_ls[lr_tdx]->idx = prev_frame->stereo_ls[lr_qdx]->idx; // prev idx 60 } 61 } 62 }
最后内点的数量是点特征内点数量+线特征内点数量：
1 n_inliers_pt = matched_pt.size(); 2 n_inliers_ls = matched_ls.size(); 3 n_inliers = n_inliers_pt + n_inliers_ls;
6.优化位姿：
是有两个重载的优化位姿函数：
void optimizePose(); void optimizePose(Matrix4d DT_ini);
两个优化位姿函数过程类似，最主要的都是用高斯牛顿法来优化位姿。
7.计算线特征重合的部分：
定义： double lineSegmentOverlap( Vector2d spl_obs, Vector2d epl_obs, Vector2d spl_proj, Vector2d epl_proj );
计算线特征的观测线段和投影线段的重合部分比率。
跟slam尤其相关的两个函数：
// slam-specific functions bool needNewKF(); void currFrameIsKF();
1.判断是否需要一个新的关键帧： bool needNewKF();
（1）将代表不确定性的协方差转化为一个标量，称之为entropy：
double entropy_curr = 3.0*(1.0+log(2.0*acos(-1))) + 0.5*log( cov_prevKF_currF.determinant() );
（2）计算当前帧的entropy和前面第一个关键帧的entropy的比值entropy_ratio：
double entropy_ratio = entropy_curr / entropy_first_prevKF;
（3）如果比值entropy_ratio是在实数范围内或者小于Config::minEntropyRatio()，就把当前帧作为一个新的关键帧插入到系统中：
1 // decide if a new KF is needed 2 if( entropy_ratio < Config::minEntropyRatio() || std::isnan(entropy_ratio) || std::isinf(entropy_ratio) || 3 ( curr_frame->DT_cov == Matrix6d::Zero() && curr_frame->DT == Matrix4d::Identity() ) ) 4 { 5 cout << endl << "Entropy ratio: " << entropy_ratio << endl; 6 return true; 7 } 8 else 9 { 10 cout << endl << "No new KF needed" << endl << endl; 11 return false; 12 }
2.把当前帧设为关键帧： void currFrameIsKF();
1 void StereoFrameHandler::currFrameIsKF() 2 { 3 4 // restart point indices 5 int idx_pt = 0; 6 for( vector<PointFeature*>::iterator it = curr_frame->stereo_pt.begin(); it != curr_frame->stereo_pt.end(); it++) 7 { 8 (*it)->idx = idx_pt; 9 idx_pt++; 10 } 11 12 // restart line indices 13 int idx_ls = 0; 14 for( vector<LineFeature*>::iterator it = curr_frame->stereo_ls.begin(); it != curr_frame->stereo_ls.end(); it++) 15 { 16 (*it)->idx = idx_ls; 17 idx_ls++; 18 } 19 20 // update KF 21 // 把当前帧的位姿和协方差矩阵都设置为单位矩阵 22 curr_frame->Tfw = Matrix4d::Identity(); 23 curr_frame->Tfw_cov = Matrix6d::Identity(); 24 25 // update SLAM variables for KF decision 26 // 更新前一个关键帧的位姿，以及前一个关键帧到当前关键帧的协方差矩阵 27 T_prevKF = curr_frame->Tfw; 28 cov_prevKF_currF = Matrix6d::Zero(); 29 prev_f_iskf = true; 30 31 }
以上是关于[pl-slam]stereoFrameHandler的主要内容，如果未能解决你的问题，请参考以下文章