[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 }
其中的点、线特征匹配和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 }
其中，如果采用的是并行提取特征，需要分别调用检测特征点的代码： 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的主要内容，如果未能解决你的问题，请参考以下文章