SVO學習筆記（一）

• • 這篇文章
  • Frame
  • Featuredetection
  • Featrue_matcher
  • • 三角測量求深度
    • 特征匹配
    • • 非線性優化尋找匹配特征
      • 極線搜索匹配特征
  • 總結

這篇文章

?一個很年輕的叔叔踩進了SLAM的坑，現在正在學習視覺SLAM中的SVO系統。本著好記性不如爛筆頭的思想，我想記錄下整個學習過程。希望這個筆記能加深自己對系統的理解，也希望它能給讀者帶來啟發（水平一般，但還是要有這種夢想）。
?所謂一口吃不成胖子，啃代碼也是一個道理。所以這次先記錄Frame、Featuredetection和Featurematcher三部分的內容。為了控制篇幅，只針對我認為比較重要和特殊的部分進行記錄。

Frame

?Frame文件中比較特殊的是存儲了五個特殊的關鍵點的容器。這五個點用于判斷圖像間的共視情況(看有沒有重疊區域)。這部分的代碼如圖：

void Frame::setKeyPoints()
{//該循環確定圖中特征點（把不好的去掉）for(size_t i = 0; i < 5; ++i)if(key_pts_[i] != NULL)if(key_pts_[i]->point == NULL)//如果某個KeyPoints沒有指向任何地圖點，則重新尋找新的KeyPointsstd::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); });//checkKeyPoints函數用于找出那五個特殊的特征點
}

&esmp;Frame中保存的五個特殊關鍵點分別是：1、距離圖片中心最近的。用1)將圖片分成四等分，在分好后的四個角落各找出一個點（2，3，4，5），它們越接近圖片邊界越好(這些點都要有對應的3D點)。這五個特殊點會不斷地更新（我想這段話也能夠解釋checkKeyPoints函數了。這個函數就是不斷地尋找更符上述條件的五個點）。

Featuredetection

?SVO中的特征對象是作者自己定義的(以FAST角點為基礎)，即文件中的Corner類，它保存了角點的位置、得分、角度（梯度方向）和金字塔層數這些信息。
?不過特征點的提取策略有些特別，我們一步一步地來看。

void FastDetector::detect(Frame* frame,const ImgPyr& img_pyr,const double detection_threshold,Features& fts)
{
//初始化角點存儲容器，其下標與網格的數量對應Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));//遍歷金字塔中的每幅圖像，提取FAST角點：for(int L=0; L<n_pyr_levels_; ++L){const int scale = (1<<L);vector<fast::fast_xy> fast_corners;//提取FAST角點（可以使用不同的方法）//代碼就不貼了，勞煩讀者參考源碼。//..................................vector<int> scores, nm_corners;//計算各角點的得分fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);//對提取的角點進行非極大值抑制，nmcorner記錄那些被選出來的極大值的角點的下標fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);//整個過程中最重要的一步：控制特征點的分布，保證每個網格中只有一個特征點。for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it){fast::fast_xy& xy = fast_corners.at(*it);const int k = static_cast<int>((xy.y*scale)/cell_size_)*grid_n_cols_+ static_cast<int>((xy.x*scale)/cell_size_);if(grid_occupancy_[k])//只對那些還沒被占據的網格設置角點continue;const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y);if(score > corners.at(k).score)//每個網格中只能有一個角點，所以網格只存放出現在這里得分最高的角點corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);}}// 對網格中所有的特征點進行篩選，只保留得分大于閾值的點std::for_each(corners.begin(), corners.end(), [&](Corner& c) {if(c.score > detection_threshold)fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));});resetGrid();
}

?特征提取的特點就在于將圖片用網格進行了劃分，且每個網格中只保留一個特征點。這樣能夠讓特征點分布的比較均勻。

Featrue_matcher

?這個文件要講的是三角測量求深度、非線性優化提煉精確匹配特征、極線搜索匹配特征。這幾個是我認為比較重要的內容，其余部分，比如仿射變換等，都是比較容易明白的。

三角測量求深度

//功能：通過兩幀間匹配特征點的歸一化坐標，進行三角測量來獲得對應地圖點的坐標。
bool depthFromTriangulation(const SE3& T_search_ref,//從參考幀到當前幀的位姿變換矩陣const Vector3d& f_ref,//歸一化平面的坐標const Vector3d& f_cur,double& depth)
{Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;const Matrix2d AtA = A.transpose()*A;if(AtA.determinant() < 0.000001)return false;AtA.col(1) = -1*AtA.col(1);//這里源碼中沒有，但是我覺得應該是要加上的
//深度計算的公式是：d = - (ATA)^(-1) * AT * tconst Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();depth = fabs(depth2[0]);//返回的是參考幀中的深度return true;
}

?代碼中需要講解的就是深度的求解公式。它的具體推到過程可參考這個博客

特征匹配

?特征匹配是由最小化以關鍵點為中心的一個像素塊的光度誤差的方法實現的。這部分要注意的是像素塊的選取。將當前幀中的像素塊定義為curpatch(默認為一個正方形)，參考幀的定義為refpatch。考慮到兩幀間的仿射變換，與curpatch相對應的refpatch在形狀和方向可能與curpatch不同，比如下圖這個例子：

?P1和P2本是匹配的特征，但在旋轉作用下，它們的像素塊內像素剛好相反。這就導致它們的光度誤差較大，無法被判定為匹配。另一種情況是兩個匹配像素塊的形狀不一定相同（當前幀中正方形的像素塊在參考幀中的匹配可能是只是平行四邊形），若此時兩個像素塊仍取正方形，那么塊中的像素可能不具有匹配關系。
?上述兩種情況都會降低匹配的效果。因此要通過仿射變換，找到兩個正確的像素塊，以保證其中像素的正確匹配。這是最重要的前提條件（不過一般在論文中的featurealign部分才使用。因為sparseimgalign部分是針對前后兩幀匹配，所以兩個像素塊的形狀差異較小）。說完前提，接下來介紹各種匹配方法的函數。首先介紹非線性優化尋找匹配特征的函數。

非線性優化尋找匹配特征

//這個是論文當中的Relaxation Through Feature Alignment部分
//使用非線性優化的方式尋找匹配結果：px_cur是先前由極線搜索獲得的匹配點坐標，此時用高斯牛頓法將像素塊中的光度誤差最小化
//函數中會尋找與pt到cur的觀測方向間夾角最小（共視程度最高）的一個KF作為參考幀，
bool Matcher::findMatchDirect(const Point& pt,//匹配特征點對所對應的地圖點const Frame& cur_frame,//當前幀Vector2d& px_cur)//地圖點當前幀的匹配點坐標的初始估計
{
//找到觀測角度與當前幀很接近的參考幀（求地圖點到當前幀的向量和到參考幀的向量，這兩個向量間的夾角要最小）if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;//看特征點和窗口是否在其對應的金字塔層圖像中if(!ref_ftr_->frame->cam_->isInFrame(ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))return false;warp::getWarpMatrixAffine(//你懂的*ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,(ref_ftr_->frame->pos() - pt.pos_).norm(),//地圖點在ref系的深度cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);//找出當前幀中的匹配特征點最可能出現在curframe中第幾層金字塔上search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);//在參考幀中某一層金字塔上的圖像中尋找對應的refpatch//這里是為當前幀匹配特征點的patch在refframe中找到對應的像素塊。不過找的是帶邊框的。warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);createPatchFromPatchWithBorder();Vector2d px_scaled(px_cur/(1<<search_level_));bool success = false;//如果特征是邊緣型，那么只需要搜索方向只需要往一個方向進行就行（因為邊緣有一個方向上移動時光度值變化較小，易找匹配）if(ref_ftr_->type == Feature::EDGELET){Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);dir_cur.normalize();success = feature_alignment::align1D(cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);}else//主要使用這一部分{//patch是參考關鍵幀中的像素塊。在cur中搜索的像素塊都認為是正方形success = feature_alignment::align2D(cur_frame.img_pyr_[search_level_],patch_with_border_, patch_,options_.align_max_iter, px_scaled);}px_cur = px_scaled * (1<<search_level_);return success;
}

極線搜索匹配特征

?這部分是通過極限搜索的方式來找出參考幀與當前幀的匹配關系。

//極線搜索匹配點。搜索也是在某一層金字塔中進行
/*這一步的前提是已經獲得了當前幀的初始位姿估計，然后再來尋找相應的匹配特征
1、計算出當前幀curframe和參考幀refframe間的相對位姿
2、為refframe中那些深度值較好的特征點（地圖點）在curframe上進行極限搜索，尋找對應的匹配點（極線搜索是在某一層金字塔中完成）
3、找到匹配點后，進行三角測量，優化地圖點的深度，以便給地圖添加好點。*/
bool Matcher::findEpipolarMatchDirect(const Frame& ref_frame,const Frame& cur_frame,const Feature& ref_ftr,//參考幀中的特征點const double d_estimate,//該特征點的深度估計值const double d_min,//深度波動的范圍（最小和最大的深度值）const double d_max,double& depth)//找到匹配點對后，新估計的地圖點深度。用于之后的深度優化
{SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();int zmssd_best = PatchScore::threshold();//閾值//匹配方式是ZMSSD（Zero Mean Sum of Squared Differences）一種光度誤差計算的方法Vector2d uv_best;//匹配的最終結果//由dmin和dmax來確定投影在cur上的極線（在dmin、dmax這一距離上的所有地圖點都可能在ref_ftr上成像）Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
//注意：這里的A、B都是在歸一化平面上的點坐標epi_dir_ = A - B;//極線向量，主要用這個來確定極線的方向warp::getWarpMatrixAffine(//你懂的*ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);

?此時就找到了極線的方向和兩幀間仿射變換。之后分情況選擇極線搜索或者是非線性優化。如果極線的長度很短，那么就可通過非線性優化就能夠找到匹配特征點；否則就老老實實地在極線上一步步地找。同時搜索的方法還和特征的類型有關，此處主要講點特征的搜索（對不起，太懶了）。

search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
Vector2d px_A(cur_frame.cam_->world2cam(A));//極線上兩端的像素點
Vector2d px_B(cur_frame.cam_->world2cam(B));//在某層金字塔中的極線的長度（在這條極線上進行搜索）
epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
createPatchFromPatchWithBorder();
//極線長度很小時就意味著距離最終結果只差一點像素偏移。所以這就直接使用直接法求解就行
if(epi_length_ < 2.0){px_cur_ = (px_A+px_B)/2.0;//用極線的中點作為初始點Vector2d px_scaled(px_cur_/(1<<search_level_));bool res;if(options_.align_1d)res = feature_alignment::align1D(//邊緣型特征cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);elseres = feature_alignment::align2D(//角點型特征cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,options_.align_max_iter, px_scaled);//是在特征點出現的那層圖像金字塔圖像中進行極限搜索，px_scaled是那層圖像中的像素坐標if(res){px_cur_ = px_scaled*(1<<search_level_);//把像素坐標恢復到原始圖像坐標系中if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))//找到匹配點對后，新估計的地圖點深度。用于之后的深度優化return true;}return false;}size_t n_steps = epi_length_/0.7; Vector2d step = epi_dir_/n_steps;//每一步需要增加的像素坐標變化量if(n_steps > options_.max_epi_search_steps){printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",n_steps, epi_length_, d_min, d_max);return false;}int pixel_sum = 0;int pixel_sum_square = 0;PatchScore patch_score(patch_);//用來計算兩個patch間的誤差的一個類Vector2d uv = B-step;//這個uv也是在當前幀的歸一化平面上的坐標Vector2i last_checked_pxi(0,0);++n_steps;//沿著極線方向，搜索匹配窗口和匹配特征點//step的方向是從B->Afor(size_t i=0; i<n_steps; ++i, uv+=step){//原圖上的特征點的坐標Vector2d px(cur_frame.cam_->world2cam(uv));//在某個金字塔層圖像上的坐標Vector2i pxi(px[0]/(1<<search_level_)+0.5,px[1]/(1<<search_level_)+0.5);if(pxi == last_checked_pxi)//防止重復搜索continue;last_checked_pxi = pxi;// 看特征點和其周圍的窗口這些元素是否都在圖像的范圍內if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))continue;//找出pxi的像素塊(curpatch)uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data+ (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols+ (pxi[0]-halfpatch_size_);//計算當前選中的patch和參考幀中的patch的誤差int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);if(zmssd < zmssd_best) {//選一個誤差最小的zmssd_best = zmssd;uv_best = uv;}//判斷是否成功匹配if(zmssd_best < PatchScore::threshold()){if(options_.subpix_refinement)//進行高斯牛頓非線性優化來優化{px_cur_ = cur_frame.cam_->world2cam(uv_best);Vector2d px_scaled(px_cur_/(1<<search_level_));bool res;if(options_.align_1d)res = feature_alignment::align1D(cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);elseres = feature_alignment::align2D(cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,options_.align_max_iter, px_scaled);//在使用depthFromTriangulation函數求出地圖點深度后//這深度值可以幫助更新地圖點的狀態，讓地圖中好點增加if(res){px_cur_ = px_scaled*(1<<search_level_);if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))//每次找到匹配特征點后都進行三角化，獲得參考中特征點深度return true;}return false;}px_cur_ = cur_frame.cam_->world2cam(uv_best);if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))return true;}return false;
}

總結

?（長嘆口氣）以上就是這篇博客的全部內容。第一編用CSDN，所以寫博客的過程還是有點辛苦(代碼復制的時候還頻繁卡殼，博友們有沒有什么好的辦法？)。但年輕人相信以后會越寫越輕松！感謝聰明、美麗、有耐心的你看完這篇長文。如果文中有不正確的地方請在評論中指出（感謝博友的指點）。那么…就到這吧，祝大家開心每一天！

總結

以上是生活随笔為你收集整理的SVO学习笔记（一）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。