該類是關於雙目幀的類，包含了提取特征點、線，兩種特征各自的匹配，繪制特征的方法等。
定義的類參數均為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要達到一個比值才算好。