SVO學(xué)習(xí)筆記（二）

• 這篇文章
• 稀疏圖像對(duì)齊
• 地圖點(diǎn)投影(地圖與當(dāng)前幀間的關(guān)系)
• • reprojectMap
  • reprojectPoint
  • reprojectCell
• 特征點(diǎn)對(duì)齊中的非線性優(yōu)化
• 結(jié)尾

這篇文章

?這次仍是關(guān)于SVO系統(tǒng)的學(xué)習(xí)記錄（沖沖沖！）。這次的主要內(nèi)容是sparseimgalign、reproject和featurealign這三個(gè)代碼文件。首先介紹第一個(gè)代碼文件----稀疏圖像對(duì)齊。

稀疏圖像對(duì)齊

?這部分代碼是論文中“Sparse Model-based Image Alignment”部分的實(shí)現(xiàn)。它通過直接法在前后兩幀中尋找匹配點(diǎn)，然后估計(jì)出兩幀間的相對(duì)位姿變換矩陣（粗略估計(jì)）。這部分大家可以參考的這個(gè)博客。這篇博客對(duì)這部分代碼講解的十分詳細(xì)且容易理解（向前輩學(xué)習(xí)！）。此處只記錄一下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;//參考幀中所有特征點(diǎn)的像素塊（refpatches）。每一行對(duì)應(yīng)一個(gè)特征點(diǎn)像素塊內(nèi)所有的像素ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);//jacobian_cache_保存各像素的雅可比矩陣//使用resize能一次性給容器分配好空間，之后pushback就只是放入元素。這樣能節(jié)約一定時(shí)間jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);//記錄參考幀中哪些特征點(diǎn)被當(dāng)前幀觀測(cè)到了visible_fts_.resize(ref_patch_cache_.rows, false); SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());//先使用高層的金字塔圖像進(jìn)行位姿優(yōu)化（粗略優(yōu)化），然后逐步使用低層金字塔圖像進(jìn)行位姿優(yōu)化//（在高層找到匹配點(diǎn)后，再去低層優(yōu)化匹配點(diǎn)位置）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_;
}

地圖點(diǎn)投影(地圖與當(dāng)前幀間的關(guān)系)

?這個(gè)文件是將地圖中的地圖點(diǎn)投影到當(dāng)前幀中，然后建立這些地圖點(diǎn)的最初觀測(cè)幀（關(guān)鍵幀）與當(dāng)前幀間的聯(lián)系，以便實(shí)現(xiàn)論文中的特征點(diǎn)對(duì)齊部分的內(nèi)容。主要介紹文件中較為重要的幾個(gè)函數(shù)，首先是reprojectMap函數(shù)。該函數(shù)將地圖中的點(diǎn)投影到當(dāng)前幀中，幫助建立關(guān)鍵幀和當(dāng)前幀間的聯(lián)系。

reprojectMap

//將地圖點(diǎn)投影到當(dāng)前幀中，建立關(guān)鍵幀與當(dāng)前幀的聯(lián)系
void Reprojector::reprojectMap(FramePtr frame,//當(dāng)前幀//容器保存的是關(guān)鍵幀的指針和其與當(dāng)前幀的共視程度std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)//關(guān)聯(lián)上的關(guān)鍵幀
{resetGrid();SVO_START_TIMER("reproject_kfs");list< pair<FramePtr,double> > close_kfs;//獲得與當(dāng)前frame共視的KFsmap_.getCloseKeyframes(frame, close_kfs);//那五個(gè)特殊點(diǎn)派上了用場(chǎng)//根據(jù)KF與frame的距離對(duì)共視的KFs進(jìn)行排序close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <boost::bind(&std::pair<FramePtr, double>::second, _2));size_t n = 0;//只取最前面幾個(gè)KFsoverlap_kfs.reserve(options_.max_n_kfs);//統(tǒng)計(jì)與當(dāng)前幀有共視的KFs，以及每個(gè)KF與當(dāng)前frame有多少共視的地圖點(diǎn)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（與當(dāng)前frame有共視部分的KF，共視的特征點(diǎn)的數(shù)量）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中所有特征點(diǎn)（主要是看特征點(diǎn)對(duì)應(yīng)的地圖點(diǎn)是否在frame上可見）{if((*it_ftr)->point == NULL)continue;//防止對(duì)同一個(gè)地圖點(diǎn)進(jìn)行重復(fù)操作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))//投影判斷地圖點(diǎn)是否在能在當(dāng)前幀中overlap_kfs.back().second++;//共視點(diǎn)加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();//遍歷地圖中所有候選地圖點(diǎn)，并統(tǒng)計(jì)地圖點(diǎn)投影失敗的次數(shù)。失敗多的就從地圖中刪除。//剔除掉地圖中的壞點(diǎn)while(it!=map_.point_candidates_.candidates_.end()){if(!reprojectPoint(frame, it->first))//看候選地圖點(diǎn)是否在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函數(shù)中配置好的，記錄某地圖點(diǎn)在當(dāng)前圖像中哪一個(gè)網(wǎng)格內(nèi)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//使每個(gè)當(dāng)前幀中的網(wǎng)格內(nèi)只保留一個(gè)3D-2D匹配點(diǎn)對(duì)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");
}

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

reprojectPoint

//判斷某一個(gè)地圖點(diǎn)是否在當(dāng)前幀中
//如果在，則將該地圖點(diǎn)分配到對(duì)應(yīng)的網(wǎng)格中，并保存3D-2D匹配點(diǎn)對(duì)。
//此時(shí)網(wǎng)格中的所有3D-2D點(diǎn)對(duì)都只是最粗略的匹配，需要進(jìn)一步優(yōu)化匹配結(jié)果
bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
{Vector2d px(frame->w2c(point->pos_));// 保證特征點(diǎn)以及它周圍的像素塊（搜索匹配時(shí)要用）也能夠在frame的圖像范圍內(nèi)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

/*函數(shù)所需參數(shù)：cell--網(wǎng)格對(duì)象frame--當(dāng)前幀
功能：優(yōu)化網(wǎng)格中的地圖點(diǎn)與像素點(diǎn)（3D-2D點(diǎn)對(duì)）的匹配關(guān)系，并保證每個(gè)網(wǎng)格內(nèi)只有一對(duì)匹配點(diǎn)對(duì)。
*/
bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
{//按照點(diǎn)的好壞程度進(jìn)行排序cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));Cell::iterator it=cell.begin();//遍歷cell中的Candidates類對(duì)象（3D-2D點(diǎn)對(duì)）while(it!=cell.end()){++n_trials_;if(it->pt->type_ == Point::TYPE_DELETED)//剔除壞點(diǎn)的匹配{it = cell.erase(it);continue;}bool found_match = true;if(options_.find_match_direct)//為地圖點(diǎn)在網(wǎng)格中尋找更準(zhǔn)確的匹配點(diǎn)（使用非線性優(yōu)化的方式）found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);if(!found_match)//如果沒找到合適的匹配，則給該地圖點(diǎn)添加一次投影失敗的標(biāo)記{it->pt->n_failed_reproj_++;//地圖點(diǎn)投影失敗的次數(shù)過多時(shí)，就會(huì)被刪除掉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;}//更新地圖點(diǎn)的狀態(tài)?？梢蕴岣叩貓D中的好點(diǎn)數(shù)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增加新的特征點(diǎn)Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);frame->addFeature(new_feature);//new_feature->point = it->pt;//如果是邊緣類特征，需要進(jìn)行一些設(shè)置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);//網(wǎng)格中找出一對(duì)精確的匹配點(diǎn)對(duì)，就不再關(guān)心網(wǎng)格中的剩余部分點(diǎn)對(duì)return true;}return false;
}

特征點(diǎn)對(duì)齊中的非線性優(yōu)化

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

//點(diǎn)特征的非線性優(yōu)化搜索匹配
bool align2D(const cv::Mat& cur_img,//當(dāng)前幀圖像uint8_t* ref_patch_with_border,//參考幀中的像素塊(帶邊框)uint8_t* ref_patch,//不帶邊框const int n_iter,//迭代次數(shù)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語言中的字節(jié)對(duì)齊操作，用來提高訪問數(shù)據(jù)的效率float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];//保存patch中每個(gè)點(diǎn)在x、y方向上的梯度float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];Matrix3f H; H.setZero();//加2是因?yàn)檫@里處理的patch是帶邊框的const int ref_step = patch_size_+2;float* it_dx = ref_patch_dx;float* it_dy = ref_patch_dy;//論文中計(jì)算的是特征點(diǎn)那個(gè)框框中的光度誤差，所以要對(duì)patch中每個(gè)點(diǎn)都進(jìn)行誤差計(jì)算//這里所求解的最小二乘問題由反向LK光流法構(gòu)建。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;//用差分來計(jì)算x、y方向上的光度值的導(dǎo)數(shù)（離散）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;//用于中間插值。更新時(shí)的像素坐標(biāo)也許不是個(gè)整型數(shù)，所以需要通過其周圍的像素光度來計(jì)算該坐標(biāo)下的光度值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){//獲得匹配特征點(diǎn)在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;}}//計(jì)算坐標(biāo)的更新量update = Hinv * Jres;u += update[0];v += update[1];mean_diff += update[2];//坐標(biāo)更新量很小時(shí)，就不再優(yōu)化if(update[0]*update[0]+update[1]*update[1] < min_update_squared){converged=true;break;}}cur_px_estimate << u, v;return converged;
}
//省略了代碼中的一些輸出項(xiàng)

結(jié)尾

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。