[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     }
View Code

(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     }
View Code

最后内点的数量是点特征内点数量+线特征内点数量:

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的主要内容,如果未能解决你的问题,请参考以下文章

构建 MRPT 库时 cmake 配置失败