雪花臺灣

[論文]語義SLAM的概率數據關聯 解析（三）EM法求解

前言

很不容易大家能看到這一章。在前面的章節對語義SLAM的數學模型做了詳細說明，那麼我們如何求解並得到所要的相機、路標的估計姿態呢。

在看論文的過程中發現底層的概率學理論、非線性優化方法實際都能在各領域通用，比如在機器學習領域，神經網路的訓練也在不斷應用非線性優化。

所以雖然理論枯燥，一旦打通便能達到視野廣闊的境界。

IV. SEMANTIC SLAM Using EM

論文的求解過程，除了數據關聯外，我們還把離散路標的類別也當作優化中的潛在變數，使得整體清晰高效地分割了離散和連續變數[resulting in a clean and efficient separation between discrete and continuous variables.]。（什麼叫清晰分割?)

在論文III部分中所說（見解析二），幾何測量的數據關聯由特徵跟蹤演算法提供，交給了前端演算法不用我們管。所以我們需要考慮的重點只剩下語義測量的數據關聯 和物體類別 。

下面是求解步驟：

  1. 初始化：初始的位姿 通過集成里程計數據得到。路標的初始化 可以通過在位姿 結合相機觀測的物體相對方向，計算對應射線空間方向並初始路標得到。
  2. 求解過程

給出命題2：假如概率分佈 是均勻的(uniform)，而且語義測量數據關聯在關鍵幀之間相對獨立。那麼有：

接著迭代求解兩步：

1) 數據關聯權重 （「E」Step）

2) 連續的相機姿態和路標位置（「M」Step）

權重的求解公式如下：

其中

這部分就是個權重而已，分母部分是所有概率的和，可以當成歸一化係數。

獲得權重之後需要代入這個公式中獲得我們想要的兩個大寶貝：

該公式整體是一個優化問題，我們需要求得讓整體值最小的兩個 作為i+1幀狀態的 。先別被這一大坨公式嚇壞，我們將在下面的B. 圖優化小節中慢慢拆解各部分所代表的含義。

A. 物體類別和數據關聯（E Step）

單個幀的權重需要對所有可能數據關聯取和，然而，由於幀與幀之間的獨立數據關聯假設，以及僅僅很少的物體可以在感測器視野內被看到，所以有可能全部遍歷，因此對於所有幀t，測量k，路標j的可以被很高效地實際計算。

一旦 被計算出來，將被用於連續的感測器姿態和路標位置最優化中。此外，最大化似然的路標類估計 也可以從被計算的 值中被獲得。（這裡SLAM反過來幫助語義了）

腦海中一直有思路說既然SLAM綜合了環境中這麼多信息，如何幫助原本只靠一張圖片的圖像檢測來識別物體呢。這個數學公式一句話就解決了這個問題。確實感覺到數學簡潔之美。

我需要求解得到的路標類別，是能夠讓該式子值取最大時的解。該公式的證明論文中單獨附出了一個Appendix II. 有興趣的同學我將單開一節敘述這個公式。

B. 位姿圖優化（「M」Step）

該部分就是教如何求解公式（7），我們把它再搬過來。

直觀理解，我們開始搜索這個最優值，這個式子的值在隨著相機姿態、路標位置和路標類別瘋狂變化的狀態下值在空間中變化，直到發現一個全局最優值能讓整體值最小。那麼我們就找到了這樣的最優解組合。

我們選擇用圖優化的方式來求解，位姿圖是反映有清晰物理結構和稀疏約束的優化問題的一種簡便方式。圖由一羣頂點V組成，每一個頂點與一個待優化變數相關聯，一組因子F在頂點之間，與單個代價函數的成分相關聯。我們的圖對每一個感測器姿態和每一個路標位置都有一個頂點，他們是待優化的變數。同時圖的求解有很多現成的庫，非常方便直接調用。

比起先前工作，感測器姿態和路標之間的連接是硬的數據關聯，論文的工作則用軟的語義數據關聯累乘因子。而軟關係的體現則用因子圖來表示。最後的總表達式將是各個Factor所表示的概率分佈函數的乘積。而單個Factor式子中涉及到的變數，作為因子圖中的節點會通過邊與對應的f相連接。

一個factor 與一個代價函數相關聯，以如下形式整體求解：

公式（7）中的Factor式子分為以下幾個部分：

1) 語義Factors:

在相機姿態 下的每一次測量都將給可見的路標 j 定義一個factors ，其中的s代表語義；k代表第k次測量。

回憶各項目含義：

由於 在公式（7）中是固定的（在E Step中算完了置信度和類別，M Step不改變），則 和 是常量。注意這裡是地圖中存放的路標，則是觀測。

因此 ，而且後一項可以被捨去。

這個公式分別包含了：已知地標種類，觀測是該種類的測量概率（僅與前端有關），已知地標種類和觀測種類的置信度（僅與前端有關），最後的是已知相機姿態和地標位置求觀測中矩形框的位置概率（這個涉及到相機和地標的位置估計）。

在實際計算中將該表達式輸入到因子圖求解的庫中，直接獲得結果。

下面是很神奇的部分。讓 表示路標 到當前相機位姿能看到的平面圖像中的投影。假定其為高斯分佈，協方差為 。因此，與感測器姿態t，測量k和路標j有關的相機factor: 成為：

實際上反映了地圖中物體投影到當前相機觀測中的位置的距離（即YouTube視頻中的藍色虛擬車輛），與實際觀測的物體矩形框位置之間的像素距離。該距離結合了範數，右下角 代表一個與其權重和協方差有關的範數，以及右上角的平方，整體作用產生一個衡量匹配關係的數值。

由於重新看到了之前記錄的路標產生的語義factors將成為我們迴環檢測約束的根據。

2）幾何Factors:

即公式（7）中的 。

其中 是清晰跟蹤到的特徵總數量，如觀測到的幾何路標的總數量。

讓 成為通過測量 生成的全局框架下路標的3D位置，並如之前般假設投影有高斯像素雜訊 ，於是得到：

該公式中測量與投影都是特徵點的位置，如ORB特徵點。其中條件 代表所有能讓「：」後面成立的k。將所有地圖上的特徵點投影到當前圖像中計算距離。

對於單個觀測：

由於使用迭代方式來最優化完整姿態圖，我們需要線性化上述公式。得到：

詳細的線性化過程在此就不論述了。

3）慣性Factors:

該論文基於引文[40]提出的模型來考慮幀間接收的多個IMU測量數據，具體不多論述，公式如下：

到此，可以看到完整的姿態圖優化模型了，就是個非線性最小二乘問題，包含三項：語義觀測項（10），幾何觀測項（15）和慣性項（17）。

到此，論文核心數學模型已經介紹完畢。總體來講本論文提出了一種框架，而很多部分實際上組合的是已經現成的模型：如特徵檢測ORB、物體檢測deformable parts model detection algorithm [38]、還有因子圖的求解框架等等。

感覺很像神經網路中提出了一種新的網路模型。

不過按照作者的大量實驗，確實能夠做到語義輔助定位，所以在這個框架確實能成為當今語義SLAM的基本問題：如何融合語義信息和幾何信息的一個解決方案。接下來語義SLAM還有很多路要走，比如作者在論文最後提出了一個方向，即將檢測物體的姿態（朝向）引入到模型中，而非現在一樣只有物體的位置。

筆者特意搜索了作者2017年以來文章，並沒有發現該方向的發表。所以要麼作者還在做要麼就卡住了。

感謝大家讀到這裡。知乎的公式編輯器真好用，只是沒法可視化改

原野尋蹤：[論文]語義SLAM的概率數據關聯 解析（一）簡介?

zhuanlan.zhihu.com原野尋蹤：[論文]語義SLAM的概率數據關聯 解析（二）語義數學模型?

zhuanlan.zhihu.com原野尋蹤：[論文]語義SLAM的概率數據關聯 解析（三）EM法求解?

zhuanlan.zhihu.com

推薦閱讀：

查看原文 >>
相關文章