[pl-slam]stereoFrame

Posted 奋斗的妮子

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了[pl-slam]stereoFrame相关的知识,希望对你有一定的参考价值。

该类是关于双目帧的类,包含了提取特征点、线,两种特征各自的匹配,绘制特征的方法等。

定义的类参数均为public,有:

 1     int frame_idx;      //双目帧索引
 2     Mat img_l, img_r;   //双目的左右图像
 3     Matrix4d Tfw;       //双目帧的三维变换
 4     Matrix4d DT;        //未知,但在其他类中也经常出现
 5 
 6     Matrix6d Tfw_cov;   //三维变换的协方差矩阵
 7     Vector6d Tfw_cov_eig;   //三维变换的协方差矩阵的对数映射
 8     double   entropy_first;
 9 
10     Matrix6d DT_cov;    //DT的协方差矩阵
11     Vector6d DT_cov_eig;    //DT的协方差矩阵的对数映射
12     double   err_norm;      //误差的范数
13 
14     vector<PointFeature*> stereo_pt;    //双目帧的点特征集
15     vector<LineFeature*>  stereo_ls;    //双目帧的线特征集
16 
17     Mat pdesc_l, pdesc_r, ldesc_l, ldesc_r;     //左右相机的点特征、线特征描述子
18 
19     PinholeStereoCamera* cam;   //相机模型

定义了以下类函数:

 1     StereoFrame();
 2     StereoFrame(const Mat img_l_, const Mat img_r_, const int idx_, PinholeStereoCamera* cam_ );
 3     ~StereoFrame();
 4 
 5     void extractInitialStereoFeatures( int fast_th = 20 );  //对双目初始帧的提取
 6     void extractStereoFeatures( int fast_th = 20 );         //对双目帧的提取
 7     //检测特征,包括线特征和点特征
 8     void detectFeatures(Mat img, vector<KeyPoint> &points, Mat &pdesc, vector<KeyLine> &lines, Mat &ldesc, double min_line_length, int fast_th = 20);
 9     //检测点特征
10     void detectPointFeatures( Mat img, vector<KeyPoint> &points, Mat &pdesc, int fast_th = 20 );
11     //检测线特征
12     void detectLineFeatures( Mat img, vector<KeyLine> &lines, Mat &ldesc, double min_line_length );
13     //匹配点特征
14     void matchPointFeatures(BFMatcher* bfm, Mat pdesc_1, Mat pdesc_2, vector<vector<DMatch>> &pmatches_12);
15     //匹配线特征
16     void matchLineFeatures(BFMatcher* bfm, Mat ldesc_1, Mat ldesc_2, vector<vector<DMatch>> &lmatches_12 );
17     //点特征的MAD中位数绝对偏差
18     void pointDescriptorMAD( const vector<vector<DMatch>> matches, double &nn_mad, double &nn12_mad );
19     //线特征的MAD中位数绝对偏差
20     void lineDescriptorMAD( const vector<vector<DMatch>> matches, double &nn_mad, double &nn12_mad );
21     //绘制双目帧的点特征和线特征
22     Mat  plotStereoFrame();
23     //绘制双目帧的特征匹配
24     Mat  plotStereoMatches();
25 
26     //求线特征的观察线段和重投影线段的重合率
27     double lineSegmentOverlapStereo(double spl_obs, double epl_obs, double spl_proj, double epl_proj  );

 1.提取双目初始帧的特征点和特征线

定义: void extractInitialStereoFeatures( int fast_th = 20 ); //对双目初始帧的提取 

技术分享图片
  1 void StereoFrame::extractInitialStereoFeatures( int fast_th )
  2 {
  3 
  4     // Feature detection and description
  5     // 特征检测和描述
  6     vector<KeyPoint> points_l, points_r;    //左右点特征集
  7     vector<KeyLine>  lines_l, lines_r;      //左右线特征集
  8     double min_line_length_th = Config::minLineLength() * std::min( cam->getWidth(), cam->getHeight() );
  9     if( Config::lrInParallel() )
 10     {
 11         auto detect_l = async(launch::async, &StereoFrame::detectFeatures, this, img_l, ref(points_l), ref(pdesc_l), ref(lines_l), ref(ldesc_l), min_line_length_th, fast_th  );
 12         auto detect_r = async(launch::async, &StereoFrame::detectFeatures, this, img_r, ref(points_r), ref(pdesc_r), ref(lines_r), ref(ldesc_r), min_line_length_th, fast_th );
 13         detect_l.wait();
 14         detect_r.wait();
 15     }
 16     else
 17     {
 18         //检测左右img的点特征和线特征
 19         detectFeatures(img_l,points_l,pdesc_l,lines_l,ldesc_l,min_line_length_th,fast_th);
 20         detectFeatures(img_r,points_r,pdesc_r,lines_r,ldesc_r,min_line_length_th,fast_th);
 21     }
 22 
 23     // Points stereo matching
 24     // 双目点特征匹配
 25     if( Config::hasPoints() && !(points_l.size()==0) && !(points_r.size()==0) )
 26     {
 27         BFMatcher* bfm = new BFMatcher( NORM_HAMMING, false );
 28         vector<vector<DMatch>> pmatches_lr, pmatches_rl, pmatches_lr_;
 29         Mat pdesc_l_;
 30         stereo_pt.clear();  //清除匹配点特征集,因为每一帧需要计算匹配,所以需要做清除处理
 31         // LR and RL matches
 32         if( Config::bestLRMatches() )
 33         {
 34             if( Config::lrInParallel() )
 35             {
 36                 auto match_l = async( launch::async, &StereoFrame::matchPointFeatures, this, bfm, pdesc_l, pdesc_r, ref(pmatches_lr) );
 37                 auto match_r = async( launch::async, &StereoFrame::matchPointFeatures, this, bfm, pdesc_r, pdesc_l, ref(pmatches_rl) );
 38                 match_l.wait();
 39                 match_r.wait();
 40             }
 41             else
 42             {
 43                 bfm->knnMatch( pdesc_l, pdesc_r, pmatches_lr, 2);
 44                 bfm->knnMatch( pdesc_r, pdesc_l, pmatches_rl, 2);
 45             }
 46         }
 47         else
 48             bfm->knnMatch( pdesc_l, pdesc_r, pmatches_lr, 2);
 49         // sort matches by the distance between the best and second best matches
 50         double nn12_dist_th  = Config::minRatio12P();
 51         // resort according to the queryIdx
 52         sort( pmatches_lr.begin(), pmatches_lr.end(), sort_descriptor_by_queryIdx() );
 53         if(Config::bestLRMatches())
 54             sort( pmatches_rl.begin(), pmatches_rl.end(), sort_descriptor_by_queryIdx() );
 55         // bucle around pmatches
 56         int pt_idx = 0;     //初始化提取双目特征时独有的
 57         for( int i = 0; i < pmatches_lr.size(); i++ )
 58         {
 59             int lr_qdx, lr_tdx, rl_tdx;
 60             lr_qdx = pmatches_lr[i][0].queryIdx;
 61             lr_tdx = pmatches_lr[i][0].trainIdx;
 62             if( Config::bestLRMatches() )
 63             {
 64                 // check if they are mutual best matches
 65                 rl_tdx = pmatches_rl[lr_tdx][0].trainIdx;
 66             }
 67             else
 68                 rl_tdx = lr_qdx;
 69             // check if they are mutual best matches and the minimum distance
 70             double dist_12 = pmatches_lr[i][0].distance / pmatches_lr[i][1].distance;
 71             if( lr_qdx == rl_tdx  && dist_12 > nn12_dist_th )
 72             {
 73                 // check stereo epipolar constraint
 74                 // 检测极线约束,因为双目两个相机水平放置,因此如果满足极线约束,两个特征点的y坐标应该相等
 75                 if( fabsf( points_l[lr_qdx].pt.y-points_r[lr_tdx].pt.y) <= Config::maxDistEpip() )
 76                 {
 77                     // check minimal disparity
 78                     double disp_ = points_l[lr_qdx].pt.x - points_r[lr_tdx].pt.x;
 79                     if( disp_ >= Config::minDisp() ){
 80                         pdesc_l_.push_back( pdesc_l.row(lr_qdx) );
 81                         PointFeature* point_;
 82                         Vector2d pl_; pl_ << points_l[lr_qdx].pt.x, points_l[lr_qdx].pt.y;
 83                         Vector3d P_;  P_ = cam->backProjection( pl_(0), pl_(1), disp_);
 84                         stereo_pt.push_back( new PointFeature(pl_,disp_,P_,pt_idx,points_l[lr_qdx].octave) );
 85                         pt_idx++;   //初始化提取双目特征时独有的
 86                     }
 87                 }
 88             }
 89         }
 90         pdesc_l_.copyTo(pdesc_l);
 91     }
 92 
 93     // Line segments stereo matching
 94     // 线特征匹配
 95     if( Config::hasLines() && !lines_l.empty() && !lines_r.empty() )
 96     {
 97         stereo_ls.clear();  //和点特征类似,在每一次检测匹配之前,都需要先清除线特征匹配集
 98         BFMatcher* bfm = new BFMatcher( NORM_HAMMING, false );
 99         vector<vector<DMatch>> lmatches_lr, lmatches_rl;
100         Mat ldesc_l_;
101         // LR and RL matches
102         if( Config::bestLRMatches() )
103         {
104             if( Config::lrInParallel() )
105             {
106                 auto match_l = async( launch::async, &StereoFrame::matchLineFeatures, this, bfm, ldesc_l, ldesc_r, ref(lmatches_lr) );
107                 auto match_r = async( launch::async, &StereoFrame::matchLineFeatures, this, bfm, ldesc_r, ldesc_l, ref(lmatches_rl) );
108                 match_l.wait();
109                 match_r.wait();
110             }
111             else
112             {
113                 bfm->knnMatch( ldesc_l,ldesc_r, lmatches_lr, 2);
114                 bfm->knnMatch( ldesc_r,ldesc_l, lmatches_rl, 2);
115             }
116         }
117         else
118             bfm->knnMatch( ldesc_l,ldesc_r, lmatches_lr, 2);
119         // sort matches by the distance between the best and second best matches
120         double nn_dist_th, nn12_dist_th;
121         lineDescriptorMAD(lmatches_lr,nn_dist_th, nn12_dist_th);        
122         nn12_dist_th  = nn12_dist_th * Config::descThL();
123         // bucle around pmatches
124         sort( lmatches_lr.begin(), lmatches_lr.end(), sort_descriptor_by_queryIdx() );
125         if( Config::bestLRMatches() )
126             sort( lmatches_rl.begin(), lmatches_rl.end(), sort_descriptor_by_queryIdx() );
127         int n_matches;
128         if( Config::bestLRMatches() )
129             n_matches = min(lmatches_lr.size(),lmatches_rl.size());
130         else
131             n_matches = lmatches_lr.size();
132         int ls_idx = 0;     //初始化提取双目特征时独有的
133         for( int i = 0; i < n_matches; i++ )
134         {
135             // check if they are mutual best matches ( if bestLRMatches() )
136             int lr_qdx = lmatches_lr[i][0].queryIdx;
137             int lr_tdx = lmatches_lr[i][0].trainIdx;
138             int rl_tdx;
139             if( Config::bestLRMatches() )
140                 rl_tdx = lmatches_rl[lr_tdx][0].trainIdx;
141             else
142                 rl_tdx = lr_qdx;
143             // check if they are mutual best matches and the minimum distance
144             double dist_12 = lmatches_lr[i][1].distance - lmatches_lr[i][0].distance;
145             double length  = lines_r[lr_tdx].lineLength;
146             if( lr_qdx == rl_tdx && dist_12 > nn12_dist_th )
147             {
148                 // estimate the disparity of the endpoints
149                 // 计算左img线特征重投影后的端点距离
150                 Vector3d sp_l; sp_l << lines_l[lr_qdx].startPointX, lines_l[lr_qdx].startPointY, 1.0;
151                 Vector3d ep_l; ep_l << lines_l[lr_qdx].endPointX,   lines_l[lr_qdx].endPointY,   1.0;
152                 // le_l是sp_l和ep_l的叉乘的单位向量
153                 Vector3d le_l; le_l << sp_l.cross(ep_l); le_l = le_l / sqrt( le_l(0)*le_l(0) + le_l(1)*le_l(1) );
154 
155                 // 计算右img线特征重投影后的端点距离
156                 Vector3d sp_r; sp_r << lines_r[lr_tdx].startPointX, lines_r[lr_tdx].startPointY, 1.0;
157                 Vector3d ep_r; ep_r << lines_r[lr_tdx].endPointX,   lines_r[lr_tdx].endPointY,   1.0;
158                 // le_r是sp_r和ep_r的叉乘向量
159                 Vector3d le_r; le_r << sp_r.cross(ep_r);
160 
161                 // 计算左右img的投影线段的重合比率
162                 double overlap = lineSegmentOverlapStereo( sp_l(1), ep_l(1), sp_r(1), ep_r(1) );
163 
164                 // 计算过程还没明白,但是如果按照论文的意思,应该是计算线段的投影点到其投影直线的距离,将其作为偏差
165                 sp_r << - (le_r(2)+le_r(1)*lines_l[lr_qdx].startPointY )/le_r(0) , lines_l[lr_qdx].startPointY ,  1.0;
166                 ep_r << - (le_r(2)+le_r(1)*lines_l[lr_qdx].endPointY   )/le_r(0) , lines_l[lr_qdx].endPointY ,    1.0;
167                 double disp_s = lines_l[lr_qdx].startPointX - sp_r(0);
168                 double disp_e = lines_l[lr_qdx].endPointX   - ep_r(0);
169                 // check minimal disparity
170                 if( disp_s >= Config::minDisp() && disp_e >= Config::minDisp()
171                         && fabsf(le_r(0)) > Config::lineHorizTh()
172                         && overlap > Config::stereoOverlapTh() )
173                 {
174                     ldesc_l_.push_back( ldesc_l.row(lr_qdx) );
175                     Vector3d sP_; sP_ = cam->backProjection( sp_l(0), sp_l(1), disp_s);
176                     Vector3d eP_; eP_ = cam->backProjection( ep_l(0), ep_l(1), disp_e);
177                     double angle_l = lines_l[lr_qdx].angle;
178                     stereo_ls.push_back( new LineFeature(Vector2d(sp_l(0),sp_l(1)),disp_s,sP_,Vector2d(ep_l(0),ep_l(1)),
179                                                          disp_e,eP_,le_l,angle_l,ls_idx, lines_l[lr_qdx].octave) );
180                     ls_idx++;       //初始化提取双目特征时独有的
181                 }
182             }
183         }
184         ldesc_l_.copyTo(ldesc_l);
185     }
186 
187 }
View Code

其中的点、线特征匹配和stereoFrameHandler中的f2fTracking函数差不多,用互匹配选择较好的特征。

2.提取双目帧的特征点和特征线

定义: void extractStereoFeatures( int fast_th = 20 ); //对双目帧的提取 

过程和提取初始帧的特征点和特征线基本一样,不同:

(1)对于初始帧,帧数为0,所以在对双目初始帧提取线特征和点特征时的索引都有变化,在上一个代码中有标示出来;

(2)在这个函数中,添加双目帧的线特征前,需要做一次判断,当线特征的两个端点的最大协方差矩阵的特征值小于设定值时,才添加这次的线特征。

3.检测img特征,包括特征点和特征线段

定义:  void detectFeatures(Mat img, vector<KeyPoint> &points, Mat &pdesc, vector<KeyLine> &lines, Mat &ldesc, double min_line_length, int fast_th = 20); 

根据Config::plInParallel()判断是否同时提取特征点和特征线段来决定采用哪段程序:

技术分享图片
 1 /**@brief 提取特征:包括点特征和线特征
 2  * @param img 待检测的图像
 3  * @param points 检测到的特征点集
 4  * @param pdesc 特征点的描述子
 5  * @param lines 检测到的特征线集
 6  * @param ldesc 特征线的描述子
 7  * @param min_line_length 特征线段的最小长度
 8  * @param fast_th 特征点检测的参数
 9  */
10 void StereoFrame::detectFeatures(Mat img, vector<KeyPoint> &points, Mat &pdesc, vector<KeyLine> &lines, Mat &ldesc, double min_line_length, int fast_th)
11 {
12 
13     if( Config::plInParallel() && Config::hasPoints() && Config::hasLines() )
14     {
15         //如果是并行计算特征点和特征线段,就需要分别调用检测特征点和特征线段的函数
16         auto detect_l = async(launch::async, &StereoFrame::detectLineFeatures,  this, img, ref(lines),  ref(ldesc), min_line_length );
17         auto detect_p = async(launch::async, &StereoFrame::detectPointFeatures, this, img, ref(points), ref(pdesc), fast_th );
18         detect_l.wait();
19         detect_p.wait();
20     }
21     else
22     {
23         // Detect point features
24         if( Config::hasPoints() )
25         {
26             int fast_th_ = Config::orbFastTh();
27             if( fast_th != 0 )
28                 fast_th_ = fast_th;
29             Ptr<ORB> orb = ORB::create( Config::orbNFeatures(), Config::orbScaleFactor(), Config::orbNLevels(),
30                                         Config::orbEdgeTh(), 0, Config::orbWtaK(), Config::orbScore(),
31                                         Config::orbPatchSize(), fast_th_ );
32             orb->detectAndCompute( img, Mat(), points, pdesc, false);
33         }
34         // Detect line features
35         Ptr<BinaryDescriptor>   lbd = BinaryDescriptor::createBinaryDescriptor();
36         if( Config::hasLines() )
37         {
38             Ptr<line_descriptor::LSDDetectorC> lsd = line_descriptor::LSDDetectorC::createLSDDetectorC();
39             // lsd parameters
40             line_descriptor::LSDDetectorC::LSDOptions opts;
41             opts.refine       = Config::lsdRefine();
42             opts.scale        = Config::lsdScale();
43             opts.sigma_scale  = Config::lsdSigmaScale();
44             opts.quant        = Config::lsdQuant();
45             opts.ang_th       = Config::lsdAngTh();
46             opts.log_eps      = Config::lsdLogEps();
47             opts.density_th   = Config::lsdDensityTh();
48             opts.n_bins       = Config::lsdNBins();
49             opts.min_length   = min_line_length;
50             lsd->detect( img, lines, Config::lsdScale(), 1, opts);  //检测特征线段
51             // filter lines
52             // 滤出一些特征线
53             if( lines.size()>Config::lsdNFeatures() && Config::lsdNFeatures()!=0  )
54             {
55                 // sort lines by their response
56                 sort( lines.begin(), lines.end(), sort_lines_by_response() );
57                 //sort( lines.begin(), lines.end(), sort_lines_by_length() );
58                 lines.resize(Config::lsdNFeatures());
59                 // reassign index
60                 for( int i = 0; i < Config::lsdNFeatures(); i++  )
61                     lines[i].class_id = i;
62             }
63             lbd->compute( img, lines, ldesc);   //计算特征线段的描述子
64         }
65     }
66 
67 }
View Code

其中,如果采用的是并行提取特征,需要分别调用检测特征点的代码:  void detectPointFeatures( Mat img, vector<KeyPoint> &points, Mat &pdesc, int fast_th = 20 ); 

以及检测特征线段的代码: void detectLineFeatures( Mat img, vector<KeyLine> &lines, Mat &ldesc, double min_line_length ); 

过程和方法与上面 detectFeatures()函数一样。

4.匹配特征点和匹配特征线段

 1 void StereoFrame::matchPointFeatures(BFMatcher* bfm, Mat pdesc_1, Mat pdesc_2, vector<vector<DMatch>> &pmatches_12  )
 2 {
 3     // 对每一个pdesc_1,在pdesc_2中寻找最近的2个描述子
 4     bfm->knnMatch( pdesc_1, pdesc_2, pmatches_12, 2);
 5 }
 6 
 7 void StereoFrame::matchLineFeatures(BFMatcher* bfm, Mat ldesc_1, Mat ldesc_2, vector<vector<DMatch>> &lmatches_12  )
 8 {
 9     // 对每一个ldesc_1,在ldesc_2中寻找最近的2个描述子
10     bfm->knnMatch( ldesc_1, ldesc_2, lmatches_12, 2);
11 }

需要注意的就是两个匹配都采用的是knnMatch()方法,该方法是用于对query集合中每一个描述子,在train集合中寻找最好的k个匹配,这段代码里面的k=2。是用于后面筛选匹配,最优/次优的ratio要达到一个比值才算好。

 

以上是关于[pl-slam]stereoFrame的主要内容,如果未能解决你的问题,请参考以下文章

微信小程序代码片段

VSCode自定义代码片段——CSS选择器

谷歌浏览器调试jsp 引入代码片段,如何调试代码片段中的js

片段和活动之间的核心区别是啥?哪些代码可以写成片段?

VSCode自定义代码片段——.vue文件的模板

VSCode自定义代码片段6——CSS选择器