建圖（深度濾波器）

SVO 把像素的深度誤差模型看做概率分布，使用 高斯

——均勻混合分布的逆深度（深度值服從高斯分布，局外點的概率服從 Beta 分布），稱為 深度濾波器 Depth Filter，每個特徵點作為 種子 Seed（深度未收斂的像素點）有一個單獨的深度濾波器。

  • 初始化種子：如果進來一個 關鍵幀，就提取關鍵幀上的新特徵點，初始化深度濾波器，作為種子點放進一個種子隊列中
  • 更新種子：如果進來一個 普通幀，就用普通幀的信息，更新所有種子點的概率分布；如果某個種子點的深度分布已經收斂，就把它放到地圖中，供追蹤線程使用

初始化種子

代碼主要在 DepthFilter::initializeSeeds。

  • 當前幀上已經有的特徵點，佔據住網格
  • 多層金字塔 FAST 特徵檢測並進行非極大值抑制，映射到第 0 層網格，每個網格保留 Shi-Tomas 分數最大的點
  • 對於所有的新的特徵點，初始化成種子點 Seed
    • 高斯分布均值：mu = 1.0/depth_mean
    • 高斯分布方差：sigma2 = z_range*z_range/36，其中 z_range = 1.0/depth_min

更新種子（深度濾波）

深度濾波 DepthFilter::updateSeeds 主要過程：

  • 極線搜索與三角測量 Matcher::findEpipolarMatchDirect
  • 計算深度不確定度 DepthFilter::computeTau
  • 深度融合，更新種子 DepthFilter::updateSeed
  • 初始化新的地圖點，移除種子點

深度估計值（極線搜索）

  • 根據 深度均值mu和深度方差sigma2確定深度範圍 left[ d_{min},d_{max} ight] ,計算極線
  • 計算仿射矩陣 warp::getWarpMatrixAffine（幀間圖像發生旋轉）
  • 極線上搜索匹配：通過RANSAC計算ZMSSD獲取最佳匹配點坐標
    • 當計算的極線長度小於兩個像素時，直接採用下一步的圖像對齊
    • 否則，繼續沿極線搜索匹配
  • 光流法亞像素精度提取（圖像對齊） feature_alignment::align2D
  • 三角法恢復深度Z depthFromTriangulation （Triangulate in SVO）

深度估計不確定度

REMODE 對由於特徵定位不準導致的三角化深度誤差進行了分析

假設 焦距為 f ，像素擾動為px_noise = 1.0，角度變化量px_error_angle為 deltaeta 則

tanfrac{deltaeta}{2}=frac{1.0}{2f}

得 deltaeta=2arctanfrac{1.0}{2f}

則 eta^{+}=eta+deltaeta

gamma^{+}=pi-alpha-eta^{+}

已知 t 為 T_ref_cur.translation()，根據正弦定理

所以，深度不確定度tau為 gamma=Z^{+}-Z

逆深度不確定度 tau_inverse 為：

double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));

即逆深度標準差為

T_{inu}=frac{1}{2}left( frac{1}{Z-gamma} -frac{1}{Z+gamma} ight)

深度融合

代碼主要在 void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)。

SVO 的融合是不斷利用最新時刻深度的觀測值，來融合上一時刻深度最優值，直至深度收斂。

通過上面兩步得到逆深度測量值 x 1./z 和 逆深度不確定度 au_{inu} ,則 逆深度 服從高斯分布

Nleft( x, au_{inu}^{2} ight)

SVO 採用 Vogiatzis的論文 Video-based, real-time multi-view stereo 提到的概率模型，使用 高斯–均勻混合分布的深度濾波器，根據 x 和 au_{inu} 更新以下四個參數：

  • 逆深度高斯分布的均值 mu seed->mu
  • 逆深度高斯分布的方差 sigma^{2} seed->sigma2
  • Beta 分布的 a seed->a
  • Beta 分布的 b seed->b

更新過程（Vogiatzis的Supplementary matterial）如下：

其中，論文公式 19 有誤（代碼正確），應為

frac{1}{s^{2}}=frac{1}{sigma^{2}} + frac{1}{ au_{inu}^{2}}

最終，得到收斂的逆深度的最佳估計值 seed->mu。

新的地圖點

如果種子點的方差seed->sigma2，小於深度範圍 /200 的時候，就認為收斂了，它就不再是種子點，而是TYPE_CANDIDATE點。

TYPE_CANDIDATE點被成功觀察到 1 次，就變成TYPE_UNKNOWN點；TYPE_UNKNOWN被成功觀察到 10 次，就變成TYPE_GOOD點；如果多次應該觀察而沒有被觀察到，就變成TYPE_DELETED點。

總結與討論：

  • 特徵點提取在地圖線程，跟蹤使用光流法
  • 後端的特徵點只在關鍵幀上提取，用 FAST 加金字塔
  • 上一個關鍵幀的特徵點在這一個關鍵幀上找匹配點的方法，是用極線搜索，尋找亮度差最小的點，最後再用深度濾波器把這個地圖點準確地濾出來
  • 重定位比較簡單，沒有迴環
  • 深度濾波器使用的是高斯–均勻混合分布
  • SVO 對 PTAM 的改進主要在兩個方面：高效的特徵匹配、魯棒的深度濾波器
  • 在 Tracking 線程，當前幀的特徵點是從上一幀用光流法傳遞過來的，只有在 Mapping 線程DepthFilter插入新關鍵幀時才需要提取特徵點，不需要每一幀都提取特徵點
  • 在 Tracking 線程，當前幀的特徵點是從上一幀用光流法傳遞過來的，只有在 Mapping線程變了（除非BA階段調整）；而 SVO 的深度濾波器會根據多幀圖片不斷收斂地圖點的不確定度，從而得到更可靠的地圖點。因為地圖點更可靠，所以 SVO 只需要維護更少的地圖點（PTAM一般維護約 160 到 220 個特徵點，SVO 在 fast 模式下維護約 120 個地圖點），從而加快了計算速度。

參考文獻：

  • SVO: Fast Semi-Direct Monocular Visual Odometry
  • ivcj2010supp, Supplementary matterial
  • REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time
  • SVO 代碼筆記
  • svo： semi-direct visual odometry 論文解析
  • 能否具體解釋下svo的運動估計與深度估計兩方面？
  • SVO原理解析
  • svo的Supplementary matterial 推導過程
  • 深度濾波器詳細解讀

PS：歡迎留言評論分享你的觀點，共同學習（入坑） SLAM ??????


推薦閱讀：
相关文章