SVO學習筆記（二）

• 這篇文章
• 稀疏圖像對齊
• 地圖點投影(地圖與當前幀間的關系)
• • reprojectMap
  • reprojectPoint
  • reprojectCell
• 特征點對齊中的非線性優化
• 結尾

這篇文章

?這次仍是關于SVO系統的學習記錄（沖沖沖！）。這次的主要內容是sparseimgalign、reproject和featurealign這三個代碼文件。首先介紹第一個代碼文件----稀疏圖像對齊。

稀疏圖像對齊

?這部分代碼是論文中“Sparse Model-based Image Alignment”部分的實現。它通過直接法在前后兩幀中尋找匹配點，然后估計出兩幀間的相對位姿變換矩陣（粗略估計）。這部分大家可以參考的這個博客。這篇博客對這部分代碼講解的十分詳細且容易理解（向前輩學習！）。此處只記錄一下run()的部分。

size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{reset();if(ref_frame->fts_.empty()){SVO_WARN_STREAM("SparseImgAlign: no features to track!");return 0;}ref_frame_ = ref_frame;cur_frame_ = cur_frame;//參考幀中所有特征點的像素塊（refpatches）。每一行對應一個特征點像素塊內所有的像素ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);//jacobian_cache_保存各像素的雅可比矩陣//使用resize能一次性給容器分配好空間，之后pushback就只是放入元素。這樣能節約一定時間jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);//記錄參考幀中哪些特征點被當前幀觀測到了visible_fts_.resize(ref_patch_cache_.rows, false); SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());//先使用高層的金字塔圖像進行位姿優化（粗略優化），然后逐步使用低層金字塔圖像進行位姿優化//（在高層找到匹配點后，再去低層優化匹配點位置）for(level_=max_level_; level_>=min_level_; --level_){mu_ = 0.1;jacobian_cache_.setZero();have_ref_patch_cache_ = false;if(verbose_)printf("\nPYRAMID LEVEL %i\n---------------\n", level_);optimize(T_cur_from_ref);}cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;return n_meas_/patch_area_;
}

地圖點投影(地圖與當前幀間的關系)

?這個文件是將地圖中的地圖點投影到當前幀中，然后建立這些地圖點的最初觀測幀（關鍵幀）與當前幀間的聯系，以便實現論文中的特征點對齊部分的內容。主要介紹文件中較為重要的幾個函數，首先是reprojectMap函數。該函數將地圖中的點投影到當前幀中，幫助建立關鍵幀和當前幀間的聯系。

reprojectMap

//將地圖點投影到當前幀中，建立關鍵幀與當前幀的聯系
void Reprojector::reprojectMap(FramePtr frame,//當前幀//容器保存的是關鍵幀的指針和其與當前幀的共視程度std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)//關聯上的關鍵幀
{resetGrid();SVO_START_TIMER("reproject_kfs");list< pair<FramePtr,double> > close_kfs;//獲得與當前frame共視的KFsmap_.getCloseKeyframes(frame, close_kfs);//那五個特殊點派上了用場//根據KF與frame的距離對共視的KFs進行排序close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <boost::bind(&std::pair<FramePtr, double>::second, _2));size_t n = 0;//只取最前面幾個KFsoverlap_kfs.reserve(options_.max_n_kfs);//統計與當前幀有共視的KFs，以及每個KF與當前frame有多少共視的地圖點for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)//遍歷距離近的所有KFs{FramePtr ref_frame = it_frame->first;//pair（與當前frame有共視部分的KF，共視的特征點的數量）overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();it_ftr!=ite_ftr; ++it_ftr)//遍歷KF中所有特征點（主要是看特征點對應的地圖點是否在frame上可見）{if((*it_ftr)->point == NULL)continue;//防止對同一個地圖點進行重復操作if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)continue;(*it_ftr)->point->last_projected_kf_id_ = frame->id_;if(reprojectPoint(frame, (*it_ftr)->point))//投影判斷地圖點是否在能在當前幀中overlap_kfs.back().second++;//共視點加1}}SVO_STOP_TIMER("reproject_kfs");SVO_START_TIMER("reproject_candidates");{boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);auto it=map_.point_candidates_.candidates_.begin();//遍歷地圖中所有候選地圖點，并統計地圖點投影失敗的次數。失敗多的就從地圖中刪除。//剔除掉地圖中的壞點while(it!=map_.point_candidates_.candidates_.end()){if(!reprojectPoint(frame, it->first))//看候選地圖點是否在frame中{it->first->n_failed_reproj_ += 3;if(it->first->n_failed_reproj_ > 30){map_.point_candidates_.deleteCandidate(*it);it = map_.point_candidates_.candidates_.erase(it);continue;}}++it;}} // unlock the mutex when out of scopeSVO_STOP_TIMER("reproject_candidates");SVO_START_TIMER("feature_align");//grid_：在reprojectPoint函數中配置好的，記錄某地圖點在當前圖像中哪一個網格內for(size_t i=0; i<grid_.cells.size(); ++i){// we prefer good quality points over unkown quality (more likely to match)// and unknown quality over candidates (position not optimized//使每個當前幀中的網格內只保留一個3D-2D匹配點對if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))++n_matches_;if(n_matches_ > (size_t) Config::maxFts())break;}SVO_STOP_TIMER("feature_align");
}

?上面的代碼將地圖點投影到當前幀中，為地圖點在當前幀中找到了更好的匹配點，并建立相應關鍵幀與當前幀之間的關系。代碼中要注意的兩個函數是：1）reprojectPoint：判斷地圖點是否在當前幀中，并記錄地圖點投影在圖像的哪個網格內；2）reprojectCell：保證每個網格內只保留一個地圖點以及它的匹配點。這寫操作能控制匹配點對的分布，同時減少計算量。下面來介紹這兩個函數。

reprojectPoint

//判斷某一個地圖點是否在當前幀中
//如果在，則將該地圖點分配到對應的網格中，并保存3D-2D匹配點對。
//此時網格中的所有3D-2D點對都只是最粗略的匹配，需要進一步優化匹配結果
bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
{Vector2d px(frame->w2c(point->pos_));// 保證特征點以及它周圍的像素塊（搜索匹配時要用）也能夠在frame的圖像范圍內if(frame->cam_->isInFrame(px.cast<int>(), 8)) {const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols+ static_cast<int>(px[0]/grid_.cell_size);grid_.cells.at(k)->push_back(Candidate(point, px));return true;}return false;
}

reprojectCell

/*函數所需參數：cell--網格對象frame--當前幀
功能：優化網格中的地圖點與像素點（3D-2D點對）的匹配關系，并保證每個網格內只有一對匹配點對。
*/
bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
{//按照點的好壞程度進行排序cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));Cell::iterator it=cell.begin();//遍歷cell中的Candidates類對象（3D-2D點對）while(it!=cell.end()){++n_trials_;if(it->pt->type_ == Point::TYPE_DELETED)//剔除壞點的匹配{it = cell.erase(it);continue;}bool found_match = true;if(options_.find_match_direct)//為地圖點在網格中尋找更準確的匹配點（使用非線性優化的方式）found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);if(!found_match)//如果沒找到合適的匹配，則給該地圖點添加一次投影失敗的標記{it->pt->n_failed_reproj_++;//地圖點投影失敗的次數過多時，就會被刪除掉if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)map_.safeDeletePoint(it->pt);if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)map_.point_candidates_.deleteCandidatePoint(it->pt);it = cell.erase(it);continue;}//更新地圖點的狀態?？梢蕴岣叩貓D中的好點數it->pt->n_succeeded_reproj_++;if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)it->pt->type_ = Point::TYPE_GOOD;//給frame增加新的特征點Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);frame->addFeature(new_feature);//new_feature->point = it->pt;//如果是邊緣類特征，需要進行一些設置if(matcher_.ref_ftr_->type == Feature::EDGELET){new_feature->type = Feature::EDGELET;new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;new_feature->grad.normalize();}it = cell.erase(it);//網格中找出一對精確的匹配點對，就不再關心網格中的剩余部分點對return true;}return false;
}

特征點對齊中的非線性優化

?這部分被featurematcher文件所調用。這里介紹align2D函數中迭代求解的步驟，它是針對點特征的求解方法。函數中迭代求解的最小二乘問題是根據Inverse Compositional Image Alignment算法（我愿稱它為逆向光流法）構建的。對這個算法的介紹可以參考這個博客。

//點特征的非線性優化搜索匹配
bool align2D(const cv::Mat& cur_img,//當前幀圖像uint8_t* ref_patch_with_border,//參考幀中的像素塊(帶邊框)uint8_t* ref_patch,//不帶邊框const int n_iter,//迭代次數Vector2d& cur_px_estimate,bool no_simd)
{
//使用的是8X8的patchconst int halfpatch_size_ = 4;const int patch_size_ = 8;const int patch_area_ = 64;bool converged=false;//__attribute是C語言中的字節對齊操作，用來提高訪問數據的效率float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];//保存patch中每個點在x、y方向上的梯度float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];Matrix3f H; H.setZero();//加2是因為這里處理的patch是帶邊框的const int ref_step = patch_size_+2;float* it_dx = ref_patch_dx;float* it_dy = ref_patch_dy;//論文中計算的是特征點那個框框中的光度誤差，所以要對patch中每個點都進行誤差計算//這里所求解的最小二乘問題由反向LK光流法構建。for(int y=0; y<patch_size_; ++y){//獲得帶邊界Patch中的每行頭指針uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)//{Vector3f J;//用差分來計算x、y方向上的光度值的導數（離散）J[0] = 0.5 * (it[1] - it[-1]);//dxJ[1] = 0.5 * (it[ref_step] - it[-ref_step]);//dyJ[2] = 1;*it_dx = J[0];*it_dy = J[1];//組成Hession矩陣H += J*J.transpose();}}Matrix3f Hinv = H.inverse();float mean_diff = 0;float u = cur_px_estimate.x();float v = cur_px_estimate.y();const float min_update_squared = 0.03*0.03;const int cur_step = cur_img.step.p[0];Vector3f update; update.setZero();//使用高斯牛頓法迭代for(int iter = 0; iter<n_iter; ++iter){int u_r = floor(u);int v_r = floor(v);if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)break;if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. checkreturn false;//亞像素float subpix_x = u-u_r;float subpix_y = v-v_r;//用于中間插值。更新時的像素坐標也許不是個整型數，所以需要通過其周圍的像素光度來計算該坐標下的光度值float wTL = (1.0-subpix_x)*(1.0-subpix_y);float wTR = subpix_x * (1.0-subpix_y);float wBL = (1.0-subpix_x)*subpix_y;float wBR = subpix_x * subpix_y;uint8_t* it_ref = ref_patch;float* it_ref_dx = ref_patch_dx;float* it_ref_dy = ref_patch_dy;Vector3f Jres; Jres.setZero();for(int y=0; y<patch_size_; ++y){//獲得匹配特征點在cur的patch中各元素的光度值uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy){float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];float res = search_pixel - *it_ref + mean_diff;//雅可比矩陣乘以誤差Jres[0] -= res*(*it_ref_dx);Jres[1] -= res*(*it_ref_dy);Jres[2] -= res;}}//計算坐標的更新量update = Hinv * Jres;u += update[0];v += update[1];mean_diff += update[2];//坐標更新量很小時，就不再優化if(update[0]*update[0]+update[1]*update[1] < min_update_squared){converged=true;break;}}cur_px_estimate << u, v;return converged;
}
//省略了代碼中的一些輸出項

結尾

?以上就是整篇博客的全部內容。這篇博客介紹了SVO論文中的“Sparse Model-based Image Alignment”部分、“Relaxation Through Feature Alignment”部分中的重投影操作和迭代優化求解的實現。之后計劃寫有關SVO的深度濾波等部分內容，也有會寫一下自己對于逆向光流法的理解（沖沖沖!）。

總結

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

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