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

前言

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

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

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

IV. SEMANTIC SLAM Using EM

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

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

下面是求解步驟：

初始化：初始的位姿 $mathcal{X}^{(0)}$ 通過集成里程計數據得到。路標的初始化 $mathcal{L}^{(0)}$ 可以通過在位姿 $mathcal{X}^{(0)}$ 結合相機觀測的物體相對方向，計算對應射線空間方向並初始路標得到。
求解過程

給出命題2：假如概率分佈是均勻的(uniform)，而且語義測量數據關聯在關鍵幀之間相對獨立。那麼有： $p(mathcal{D}|mathcal{S},mathcal{X},mathcal{L})=Pi_{t=1}^Tp(mathcal{D}_t|mathcal{S}_t,mathcal{X},mathcal{L})$

接著迭代求解兩步：

1) 數據關聯權重 $w_{ij}^t$ （「E」Step）

2) 連續的相機姿態和路標位置 $ell_{1:M}^p$ （「M」Step）

權重的求解公式如下：

$w_{kj}^{t,(i)}=sum_{ell^cinmathcal{C}}sum_{mathcal{D}_tinmathbb{D}_t(k,j)}mathcal{k}^{(i)}(mathcal{D}_t,ell^c) forall t,k,j ag{6}$

其中

$mathcal{k}^{(i)}(mathcal{D}_t,ell^c)=frac{p(mathcal{S}_t|mathcal{X}^{(i)},mathcal{L}^{(i)},mathcal{D}_t)}{sum_{ell^c}^{}{sum_{mathcal{D}_tinmathbb{D}_t}^{}{p(mathcal{S}_t|mathcal{X}^{(i)},mathcal{L}^{(i)},mathcal{D}_t)}}}$

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

獲得權重之後需要代入這個公式中獲得我們想要的兩個大寶貝： $mathcal{X}^{(i+1)},ell_{1:M}^{(i+1)}$

$mathcal{X}^{(i+1)},ell_{1:M}^{(i+1)}=mathop{argmin}_{mathcal{X},ell_{1:M}^p }sum_{t=1}^{T}{}sum_{s_kin S_t}^{}{}sum_{j=1}^{M}{-w_{kj}^{t,(i)}mathop{log}p(s_k | mathbb{x}_t,ell_j)}\ -logp(mathcal{Y}|mathcal{X})- logp(mathcal{I}|mathcal{X}) ag{7}$

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

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

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

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

$hatell_{1:M}^{c} =mathop{argmax}_{ell^c }p(ell_{1:M}^c| heta,mathcal{Z}) =mathop{argmax}_{ell^c }prod_{t=1}^{T}sum_{mathcal{D}_tinmathbb{D}_t}mathcal{k}(mathcal{D}_t,ell^c)$

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

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

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

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

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

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

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

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

$hat{mathcal{V}}=mathop{argmin}_mathcal{V}sum_{finmathcal{F}}f(mathcal{V})$

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

1) 語義Factors:

在相機姿態下的每一次測量都將給可見的路標 j 定義一個factors $f_{kj}^s(x_i,ell_j)$ ，其中的s代表語義；k代表第k次測量。

回憶各項目含義：

: 檢測置信度 score
: 矩形框 bounding box，即圖像中的物體位置
：檢測分類結果，是香蕉還是蘋果還是梨子

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

因此 $mathop{log}p(s|mathbb{x},ell)=mathop{log}p(s^b|mathbb{x},ell^p)+mathop{log}p(s^s|ell^c,s^c)p(s^c|ell^c)$ ，而且後一項可以被捨去。

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

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

下面是很神奇的部分。讓 $h_pi(mathbb{x},ell^p)$ 表示路標到當前相機位姿能看到的平面圖像中的投影。假定其為高斯分佈，協方差為。因此，與感測器姿態t，測量k和路標j有關的相機factor: $f_{kj}^s$ 成為：

$f_{kj}^s(mathcal{X},mathcal{L})=-w_{kj}^{t,(i)}mathop{log}p(s_k^b|mathbb{x}_t,ell_j^p) ag{9}$ $=||s_k^b-h_pi(mathbb{x}_t,ell_j)||_{R_s/w_{kj}^{t,(i)}}^2 ag{10}$

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