這是我們 SLAM 組同事高洪臣寫的文章，感謝分享??????～

位姿估計

代碼主框架在 FrameHandlerMono::processFrame()。

稀疏圖像對齊（直接法）

代碼主要在 SparseImgAlign。

使用稀疏直接法計算兩幀之間的初始相機位姿T_cur_from_ref：根據last_frame_的特徵點 和last_frame_與new_frame_的相對位姿，建立損失函數（兩幀之間稀疏的 4x4 的 patch 的光度誤差）SparseImgAlign::computeResiduals，使用 G-N 優化演算法獲得兩幀之間的位姿變換，沒有特徵匹配過程，效率較高。

其中，

  • patch 選取了 4x4 的大小，忽略 patch 的變形，並且沒有做仿射變換（affine warp），加快了計算速度
  • 在幾個圖像金字塔層之間迭代優化，從金字塔的最頂層（默認是第五層klt_max_level）逐層計算（默認計算到第 3 層klt_min_level）
  • 廣泛使用指針，比直接取值速度更快
  • 逆向組合(inverse compositional)演算法，預先計算雅克比矩陣precomputeReferencePatches，節省計算量
  • 幾處雙線性插值方法對提高精度有幫助

最後，cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;

特徵對齊（光流法）

代碼入口在 Reprojector::reprojectMap。

通過上一步的幀間匹配能夠得到當前幀相機的位姿，但是這種 frame to frame 估計位姿的方式不可避免的會帶來累計誤差從而導致漂移。

稀疏圖像對齊之後使用 特徵對齊，即通過地圖向當前幀投影，並使用 逆向組合光流 以稀疏圖像對齊的結果為初始值，完成基於 patch 的特徵匹配，得到更精確的特徵位置。

基於光度不變性假設，特徵塊在以前參考幀中的亮度應該和 new frame 中的亮度差不多，所以重新構造一個殘差，對特徵預測位置進行優化（代碼主要在Matcher::findMatchDirect）：

  • 優化變數是 像素位置
  • 光度誤差的前一部分是當前圖像中的亮度值；後一部分不是 I_{k-1} 而不是 I_{r} ，即它是根據投影的 3D 點追溯到其所在的 KF 中的像素值
  • 選取了 8x8 的 patch ，由於是特徵塊對比並且 3D 點所在的 KF 可能離當前幀 new frame 比較遠，所以光度誤差還加了一個仿射變換 A_{i} ，可以得到亞像素級別的精度
  • 對於每個特徵點單獨考慮，找到和當前幀視角最接近的共視關鍵幀getCloseViewObs（這個關鍵幀和當前幀的視角差別越小，patch的形變越小，越可以更精準地匹配）

該過程通過 inverse compositional Lucas-Kanade algorithm 求解，得到優化後更加準確的特徵點預測位置。

位姿和結構優化（特徵點法）

代碼主要在pose_optimizer::optimizeGaussNewton和FrameHandlerBase::optimizeStructure

motion-only BA

  • 優化變數是 相機位姿

structure-only BA

  • 優化變數是 三維點坐標

local BA

  • 附近關鍵幀和可見的地圖點都被優化了，這一小步在 fast 模式下是不做的

重定位

代碼主要在 FrameHandlerMono::relocalizeFrame。

重定位效果一般，有待改進。

更多學習筆記：

小覓智能：乾貨 | 一起快速上手 VINS-Fusion?

zhuanlan.zhihu.com圖標

PS：點贊??和分享??是對我們的最大鼓勵呀～??????


推薦閱讀：
相關文章