位姿估計 | SVO 論文與代碼分析分講
這是我們 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
):
- 優化變數是 像素位置
- 光度誤差的前一部分是當前圖像中的亮度值;後一部分不是
而不是 ,即它是根據投影的 3D 點追溯到其所在的 KF 中的像素值 - 選取了 8x8 的 patch ,由於是特徵塊對比並且 3D 點所在的 KF 可能離當前幀 new frame 比較遠,所以光度誤差還加了一個仿射變換
,可以得到亞像素級別的精度 - 對於每個特徵點單獨考慮,找到和當前幀視角最接近的共視關鍵幀
getCloseViewObs
(這個關鍵幀和當前幀的視角差別越小,patch的形變越小,越可以更精準地匹配)
該過程通過 inverse compositional Lucas-Kanade algorithm 求解,得到優化後更加準確的特徵點預測位置。
位姿和結構優化(特徵點法)
代碼主要在pose_optimizer::optimizeGaussNewton
和FrameHandlerBase::optimizeStructure
motion-only BA
- 優化變數是 相機位姿
structure-only BA
- 優化變數是 三維點坐標
local BA
- 附近關鍵幀和可見的地圖點都被優化了,這一小步在 fast 模式下是不做的
重定位
代碼主要在 FrameHandlerMono::relocalizeFrame
。
重定位效果一般,有待改進。
更多學習筆記:
小覓智能:乾貨 | 一起快速上手 VINS-FusionPS:點贊??和分享??是對我們的最大鼓勵呀~??????
推薦閱讀: