SVO 學習筆記（深度濾波）

• 這篇博客
• 論文中的深度濾波
• 深度濾波的代碼流程
• • 更新Seed對象
  • 初始化Seed對象
• 結尾

這篇博客

?這篇博客將介紹SVO論文中的Mapping部分，主要介紹深度濾波器在獲取新的圖像幀后，更新相應地圖點深度的過程。（2020-6-18：修改“更新Seed對象”中的內容）

論文中的深度濾波

?單目SLAM構建地圖點是通過三角測量操作實現的。在SVO中，認為通過三角測量產生的地圖點的深度值服從某種概率分布。在地圖點剛構建時，這個概率分布的不確定性很大，即地圖點深度值的不確定性較大（因為單目相機較難獲得圖中像素的真實深度）。這一點體現為下圖中的淺綠色部分，即深度d在[dmin,dmax]中波動：

圖中左邊的方框是地圖點的第一個觀測幀（即產生該地圖點的關鍵幀），右邊是地圖點的其他觀測幀。將這兩類幀分別記為 Ir 、Ik。
?從圖可以看出，降低地圖點深度不確定性的方法是：針對一個由 Ir 產生的新地圖點，在它的其他觀測幀 Ik 上找到匹配的特征點（使用極線搜索的方法，極線為[dmin,dmax]在 Ik 上的投影線段）。以此構建 Ir 和 Ik 上的一對匹配特征，并使用這對匹配特征進行三角測量，獲得該地圖點新的深度測量值。將這新的測量值與地圖點已有的深度信息融合，更新該深度值的分布情況，使[dmin,dmax]的范圍變小。
?當地圖點的觀測幀越多，它的深度值的波動范圍就會越小。當其小到一定程度時，就可將該地圖點放進地圖中使用了。
?上述就是SVO中深度濾波的大致概念，下面介紹它的代碼流程。

深度濾波的代碼流程

?在SVO中，深度濾波被定義為一個單獨的類對象。它將每個新構建的地圖點設定為一個Seed對象（種子對象）。Seed對象包括了待優化的地圖點、地圖點深度的平均值和方差等信息。
?深度濾波器通過觀測幀來優化種子對象。當種子對象中深度值的方差足夠小時，將該種子對應的地圖點添加到地圖中使用，否則繼續進行優化。
?在優化過程中，普通幀和關鍵幀對深度濾波的作用不同：
?1、如果是普通幀，就用它來優化深度值未收斂的地圖點；
?2、如果是關鍵幀，還需要提取幀上新的特征點，并為這些特征點產生新的種子對象。
?代碼中使用 addFrame 和 addKeyFrame 兩個函數處理不同的幀。在處理普通幀時會在addFrame函數中使用 updateSeeds 函數來更新種子對象：

更新Seed對象

//使用普通幀更新所有能夠更新的Seed對象
//只更新一定數量的Seeds，不能在此消耗過多時間
void DepthFilter::updateSeeds(FramePtr frame)
{size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();lock_t lock(seeds_mut_);std::list<Seed>::iterator it=seeds_.begin();const double focal_length = frame->cam_->errorMultiplier2();//像素點的測量偏差double px_noise = 1.0;double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0;while( it!=seeds_.end()){//判斷種子更新是否需要終止if(seeds_updating_halt_)return;//不去更新那些很老的Seed（產生這些Seed的關鍵幀ID比較老）if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {it = seeds_.erase(it);continue;}//檢查待優化的地圖點能否在當前幀上觀測到SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );if(xyz_f.z() < 0.0)  {++it; // 投影點在相機成像面的后面continue;}if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {++it; // 投影點不再相機觀測范圍內continue;}// we are using inverse depth coordinates使用逆深度進行參數化float z_inv_min = it->mu + sqrt(it->sigma2);float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);double z;if(!matcher_.findEpipolarMatchDirect(//因為是逆深度，所以用分式*it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))//z是找到匹配后，三角化計算出來該地圖點在參考幀中的深度{it->b++; // increase outlier probability when no match was found++it;++n_failed_matches;continue;}// compute tau計算由一個像素測量偏差所帶來的不確定度double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);//計算測量值的標準差double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));// update the estimate更新種子的概率分布。更新的是逆深度的概率分布updateSeed(1./z, tau_inverse*tau_inverse, &*it);++n_updates;if(frame->isKeyframe()){//如果當前幀是關鍵幀，特征點檢測時，不能在這個位置上（網格內）提取特征點//因為這個位置已經找到了與地圖點的匹配點，即已存在了一個特征feature_detector_->setGridOccpuancy(matcher_.px_cur_);}//如果種子收斂了，就初始化一個候選地圖點，并刪除掉這個種子if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh){assert(it->ftr->point == NULL); // TODO this should not happen anymore//注意是逆深度！！Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));//創建新地圖點Point* point = new Point(xyz_world, it->ftr);//將參考幀中對應特征點指向這個新的地圖點it->ftr->point = point;{seed_converged_cb_(point, it->sigma2); // put in candidate list}it = seeds_.erase(it);}//種子的深度值的下限不確定，是個壞種子else if(isnan(z_inv_min)){SVO_WARN_STREAM("z_min is NaN");it = seeds_.erase(it);}else++it;}
}

?在使用普通幀進行種子更新的過程中，有兩個重要函數：1、updateSeed函數完成多個測量值的融合；2、computeTau函數計算觀測幀中特征點的測量誤差（代碼中認為有一個像素坐標的測量誤差）對地圖點深度不確定性的影響。這兩個函數是相關計算公式的實現，前者對應的公式和推導過程可以參考這篇博客；后者的公式推導可以參考《視覺SLAM十四講》P326中的內容。（2020-6-18更新：）這里主要介紹updateSeed函數，因為computeTau和參考資料中的代碼相似。

// 完成逆深度分布的融合。參數：新估計的逆深度 x 、x對應的方差 tau2 、x對應的 seed
void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
{float norm_scale = sqrt(seed->sigma2 + tau2);if(std::isnan(norm_scale))return;// 構建以norm_scale為方差的正態分布boost::math::normal_distribution<float> nd(seed->mu, norm_scale);// 1/(s^2) = 1/(sigma2)+ 1/(tau2)float s2 = 1./(1./seed->sigma2 + 1./tau2);// m = (s^2) * (mu/sigma2 + x/tau2)float m = s2*(seed->mu/seed->sigma2 + x/tau2);// C1 = a/(a+b) * N(x|mu,sigma2 + tau2) ，pdf()概率密度：正態分布nd在x處的概率密度float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);// C2 = b/(a+b) * U(x)float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;// C1、C2的歸一化float normalization_constant = C1 + C2;C1 /= normalization_constant;C2 /= normalization_constant;// 計算 f、e 來進行a、b的更新// f = C1*（a+1)/(a+b+1) + C2*(a)/(a+b+1)float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.))+ C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f));// update parameters 參數更新// mu = C1*m+C2*mu;float mu_new = C1*m+C2*seed->mu;seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;seed->mu = mu_new;// a 和 b 的更新seed->a = (e-f)/(f-e/f);seed->b = seed->a*(1.0f-f)/f;
}

初始化Seed對象

?在處理關鍵幀時，會在addKeyFrame函數中調用initializeSeed函數來初始化新的Seed對象：

//當有關鍵幀產生時，初始化新的Seed
void DepthFilter::initializeSeeds(FramePtr frame)
{//從幀中提取特征Features new_features;feature_detector_->setExistingFeatures(frame->fts_);feature_detector_->detect(frame.get(), frame->img_pyr_,Config::triangMinCornerScore(), new_features);//為每個新的特征初始化一個Seed對象seeds_updating_halt_ = true;lock_t lock(seeds_mut_); // by locking the updateSeeds function stops++Seed::batch_counter;//初始化新的Seed對象時，使用的初始深度是當前幀中觀測到的地圖點的平均深度std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));});if(options_.verbose)//顯示產生了多少個新的SeedsSVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");seeds_updating_halt_ = false;
}

結尾

?感謝耐心的博友讀到最后，希望這篇博客能幫助到各位。如果這篇博客的內容存在不足，希望大家指出，感謝大家的糾正。

總結

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

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