Semantic Mapping with Simultaneous Object Detection and Localization
論文位址：https://arxiv.org/abs/1810.11525
論文視訊：https://www.youtube.com/watch?v=W-6ViSlrrZg
簡介
作者提出同時進行目标檢測和位姿估計，利用一段連續的圖像幀，這個圖像幀和slam不同之處在于它是對一個場景的掃描，運動的幅度可能很小，就在一個場景附近各個角度掃的一段圖形序列，然後在機器人運動的時候通過ORBSLAM定位機器人自己的位置，利用faster-rcnn對物體進行目标檢測，通過粒子濾波進行物體的位姿估計，使用提出的CT-Map方法來對檢測結果進行糾正，得到更準确的檢測結果和物體的位姿。
目标
使機器人能夠在物體層面上對世界進行語義映射，其中世界的表示是對物體類别和位姿的置信度。随着神經網絡物體檢測的不斷進步，作者為語義映射提供了更強大的構模組化塊。然而，由于訓練資料集的偏差和多樣性不足，這種物體檢測在野外經常是嘈雜的。并且對來自此類網絡的錯誤檢測保持魯棒性。作者将物體類别模組化為生成推理的隐藏狀态的一部分，而不是對檢測器給出的類标簽做出選擇。
考慮到運動式RGB-D觀測，作者的目标是推斷解釋觀測結果的物體類别和位姿，同時考慮物體之間的上下文關系和物體位姿的時間一緻性。在語義映射期間顯式地模組化物體-物體上下文關系，而不是假設每個物體在環境中是獨立的。簡單來說就是相同類别的物體比不同類别的物體更容易共同出現。此外，應加強實體合理性，以防止物體互相交叉，以及漂浮在空中。
物體位姿的時間一緻性在語義映射中也起着重要作用。物體可以保留在過去觀察的位置，或者随着時間的推移逐漸改變它們的語義位置。在遮擋的情況下，模組化時間一緻性可能有助于部分觀察物體的定位。通過時間一緻性模組化，機器人可以獲得物體永久性的概念，例如，即使沒有直接觀察物體，也相信物體仍然存在。
圖1 每列顯示環境的RGB圖像，以及由檢測和定位物體組成的相應語義映射。
創新點
考慮到語義映射中的環境和時間因素，作者提出了環境時間映射（CT-MAP）方法來同時檢測物體并通過運動的RGB-D相機觀測定位它們的6D位姿。将語義映射問題利用機率表示為物體類别和位姿的置信度估計問題。使用條件随機場（CRF）來模組化物體之間的上下文關系和物體位姿的時間一緻性。 CRF模型中的依賴性包括以下方面：
1）潛在語義映射變量和觀測結果之間的相容性，
2）物體之間的上下文關系
3）物體位姿的時間一緻性。
作者提出了一種基于粒子濾波的算法，在CT-MAP中進行生成推理。
核心思想
使用一個個向量（物體的位置和機率）來代表場景中的物體。針對每個物體向量存在的機率，作者用粒子濾波和CRF來更新。基本的思想是利用不同幀之間的同一物體在空間位置的一緻性來更新物體存在的置信度。用機率來表示物體存在的置信度的好處就是，即使機器人即使一段時間内沒有識别出這個物體，這個物體還是一定機率和粒子的形式再場景中存在的。
方法
作者提出CT-Map的方法。 CT-Map方法保持了對觀察場景中物體類别和位姿的置信度。 假設機器人通過外部定位程式（例如ORB-SLAM）在環境中定位。該語義地圖由一組N個物體組成 O O O= { o 1 o^{1} o1 ， o 2 o^{2} o2 ，···， o N o^{N} oN }。 每個物體 o i o^{i} oi= { o c o^{c} oc， o g o^{g} og， o ψ o^{ψ} oψ}包含物體類别 o c ∈ C o^{c}∈C oc∈C，物體幾何結構 o g o^{g} og和物體位姿 o ψ o^{ψ} oψ，其中 C C C是物體類别的集合 C C C = { c 1 c_{1} c1， c 2 c_{2} c2，…， c n c_{n} cn}。
在t時刻，機器人的位置為 x t x_{t} xt。機器人的觀測值為 z t z_{t} zt = { I t I_{t} It， S t S_{t} St}，其中 I t I_{t} It是觀察到的RGB-D圖像， S t S_{t} St是語義測量值。語義測量值 s k s_{k} sk= { s k s s^{s}_{k} sks， s k b s^{b}_{k} skb } ∈ S t S_{t} St，由物體探測器得到，其中包含：
1）物體檢測記錄矢量 s k s s^{s}_{k} sks， s k s s^{s}_{k} sks中的每個元素表示每個物體類别的檢測置信度，
2）二維邊界框 s k b s^{b}_{k} skb
機器人位置 x t x_{t} xt和觀測值 z t z_{t} zt是已知的。物體集合 O O O是未知變量。作者對物體之間的環境依賴關系以及每個獨立物體随時間變換的時間一緻性模組化。說白了就是在不同時刻，不同的位置對相同的物體進行觀測。
圖2 語義映射問題的模型
作者是如何模組化的？
語義映射的後驗機率表示為：
其中Z是歸一化常數，并且在t時刻應用于物體 o i o^{i} oi的動作由 u t i u^{i}_{t} uti表示。 Φ p Φ_{p} Φp是預測勢能（模組化物體姿态的時間一緻性）。 Φ m Φ_{m} Φm是測量勢能（給定物體的三維網格的觀察模型）。 Φ c Φ_{c} Φc是環境勢能（捕獲物體之間的上下文關系）。
如何模組化時間一緻性？
具體取決于物體是否在視野中。如果物體在視野中，将動作 u u u模組化為連續随機變量，其遵循具有零均值和小方差Σ的高斯分布。 該假設将三維中的小物體移動預測模組化為：
是以預測勢能表達為：
當物體 o i o^{i} oi很長一段時間内不在視野中時，它可以位于相同位置或移動到不同位置。物體 o i o^{i} oi仍然處于最後一次觀測到的位置的機率是時間的函數。為了考慮物體 o i o^{i} oi可以移動到其他位置的事實，作者用離散随機變量{ u s t a y u_{stay} ustay; u m o v e u_{move} umove}對時間動作 u i u^{i} ui進行模組化。具體來說， u s t a y u_{stay} ustay表示沒有操作，物體停留在同一位置， u m o v e u_{move} umove表示使用移動操作，物體被移動到其他位置。而這些高層次的動作遵循一定的分布 p ( u i ， Δ t ） p(u^{i}，Δt） p(ui，Δt）。
其中 r 1 ， r 2 r_{1}，r_{2} r1，r2是常數， Δ t Δt Δt是沒有觀察到物體 o i o^{i} oi的持續時間。 随着Δt的增加， u s t a y u_{stay} ustay的機率衰減，最終 p （ u s t a y ; Δ t ） = r 1 p（u_{stay}; Δt）= r_{1} p（ustay;Δt）=r1，當 Δ t Δt Δt趨于無窮大時。 對于不同的物體 o i o^{i} oi，控制衰減速度的系數 μ i μ^{i} μi是不同的。作者在實驗中為不同的物體提供了探索式 μ i μ^{i} μi，而這些系數也可以使用Toris等人的介紹的方法來學習。
如何給定物體三維網格的觀察模型？
物體的測量勢能表示為：
作者使用非零常數δ來說明物體不在視野中的情況。 g ( o t i ; x t ; z t ) g(o^{i}_{t};x_{t} ;z_{t} ) g(oti;xt;zt)測量觀測值 z t z_{t} zt和 o t i o^{i}_{t} oti， x t x_{t} xt之間的相容性。
其中 h （ o t i ; s k s ） h（o^{i}_{t}; s^{s}_{k}） h（oti;sks）是檢測置信度向量 s k s s^{s}_{k} sks對類别 o t c o^{c}_{t} otc置信度的打分。函數 I I I評估兩個邊界框的最小面積上的交集。 b （ o t i ; x t ） b（o^{i}_{t}; x_{t}） b（oti;xt）是基于 x t x_{t} xt的圖像空間中投影 o t i o^{i}_{t} oti的最小封閉邊界框。
假設已知物體的三維網格模型。函數 f （ o t i ; x t ; I t ） f（o^{i}_{t}; x_{t};I_{t}） f（oti;xt;It）計算投影的 o t i o^{i}_{t} oti和 I t I_{t} It在邊界框 b （ o t i ; x t ） b（o^{i}_{t}; x_{t}） b（oti;xt）内的相似性。如果機器人之前觀測過物體 o i o^{i} oi，對 o i o^{i} oi的置信度表明它處于機器人目前的視野範圍内。如果機器人無法檢測到物體 o i o^{i} oi，則該物體可能被遮擋，在這種情況下，使用 g （ o t i ; x t ; z t ） = f （ o t i ; x t ; I t ） g（o^{i}_{t};x_{t}; z_{t}）= f（o^{i}_{t}; x_{t}; I_{t}） g（oti;xt;zt）=f（oti;xt;It）來估計物體潛在的位置。
如何捕獲物體之間的上下文關系？
所有環境中的物體類别之間存在共同的上下文關系。例如，杯子出現在桌子上會比出現在地闆上更頻繁，滑鼠在鍵盤旁邊會比在咖啡機旁邊出現得更頻繁。作者将這些共同的上下文關系稱為類别級的上下文關系。在特定環境中，某些物體執行個體之間存在上下文關系。例如，電視總是放在某個桌子上，麥片盒通常存儲在特定的櫃子中。作者将特定環境中的這些上下文關系稱為執行個體級上下文關系。
作者手動将類别級上下文關系編碼為模型的先驗知識，也可以從公共場景資料集中學習。由于執行個體級上下文關系在不同環境中是變化的，是以必須随着時間的推移學習特定環境的關系。環境勢能由類别級勢能 Φ c a t Φ_{cat} Φcat和執行個體級勢能 Φ i n s Φ_{ins} Φins組成。
将 Φ c Φ_{c} Φc（ o t i o^{i}_{t} oti; o t j o^{j}_{t} otj）模組化為混合高斯模型， Φ c a t Φ_{cat} Φcat（ o t i o^{i}_{t} oti; o t j o^{j}_{t} otj）和 Φ i n s Φ_{ins} Φins（ o t i o^{i}_{t} oti; o t j o^{j}_{t} otj）各自為高斯分量。在實驗中，手動設計 Φ c a t Φ_{cat} Φcat作為先驗知識，并通過貝葉斯更新 Φ i n s Φ_{ins} Φins。設計 Φ c a t Φ_{cat} Φcat時應遵循兩個原則：
1）簡單的實體限制，例如不允許物體交叉，物體不應該懸浮在空中，
2）屬于同一類别的物體對，比不同類别的物體對更經常同時出現。
粒子濾波的算法
實驗和效果
使用Faster R-CNN 作為物體探測器。給定RGB-D觀測的RGB通道，應用物體檢測器并從區域提出網絡中擷取邊界框，以及相應的類别得分向量。然後對這些邊界框使用非最大抑制，并合并具有大于0.5的交叉聯合（IoU）的邊界框。
在所有實驗中，作者使用等式5中的 w 1 = w 2 = 0.5 w_{1} = w_{2} = 0.5 w1=w2=0.5來同等地處理類别級和執行個體級的勢能。 如果一個物體在無限長時間内沒有被觀察到，假設物體在相同的位置或不同位置的機率是相同。是以，在等式3中使用 r 1 = r 2 = 0.5 r_{1}= r_{2} = 0.5 r1=r2=0.5。
物體檢測
作者使用Faster R-CNN物體探測器的噪聲物體探測，而CT-Map可以通過将物體類别模組化隐藏狀态的一部分來糾正一些錯誤探測。為了評估CT-Map的物體檢測性能，在資料集中的每個RGB-D序列的末尾處，對場景中所有物體進行六自由度位姿估計，并将它們投射回該序列中的每個相機幀上，以生成帶有類别标簽的邊界框。通過考慮不同的勢能集合來進行兩個語義映射過程：
1）時間映射（T-Map）：考慮CRF模型中的預測勢能;
2）環境時間映射（CT-Map）：考慮CRF模型中的預測和環境勢能。
在觀測中，T-Map和CT-Map均包括測量勢能。
作者使用mAP作為物體檢測名額。如下表所示，T-Map通過結合預測和觀察勢能改進了Faster R-CNN，并且CT-Map通過另外結合環境勢能進一步改善了性能。Faster R-CNN在測試場景中表現不佳，因為訓練資料不一定涵蓋測試時遇到的差别。雖然通過提供更多的訓練資料可以進一步提高Faster R-CNN的性能，但是CT-Map在訓練受限的情況下提供了更具有魯棒性的目标檢測
作者場景資料集上的mAP:
在某些情況下，由于遮擋，Faster R-CNN檢測到的物體不可靠。如果之前在環境中觀測過一個物體，通過對物體的時間一緻性進行模組化，來預測物體可以去的位置。是以，即使由于遮擋而未觸發對物體的檢測，該方法仍然可以定位物體并聲明檢測。但是，如果物體存在很嚴重的遮擋且深度觀測缺少物體足夠的幾何資訊，無法對物體進行定位。
位姿估計
對于資料集中的每個RGB-D序列，定位最後看到每個物體的幀，并使用已知的相機矩陣将深度幀投影回3D點雲。然後，手動标記物體的ground truth六自由度位姿。之後将每個RGB-D序列末尾估計出的物體位姿與ground truth進行比較。
位姿估計精度由 a c c u r a c y = N c o r r e c t / N t o t a l accuracy=N_{correct}/N_{total} accuracy=Ncorrect/Ntotal測量，其中 N c o r r e c t N_{correct} Ncorrect是正确定位的物體數量， N t o t a l N_{total} Ntotal是資料集中存在的物體總數。如果物體位姿估計誤差小于在某個位置誤差門檻值 Δ t Δt Δt和旋轉誤差門檻值 Δ θ Δθ Δθ，則稱該物體被正确地定位。 Δ t Δt Δt是歐幾裡德距離的平移誤差， Δ θ Δθ Δθ是方向上的絕對角度差。對于對稱物體，忽略關于對稱軸的旋轉誤差。
作者應用疊代最近點（ICP）和快點特征直方圖（FPFH）算法作為六自由度物體位姿估計的基準。對于資料集中的每個RGB-D序列，作者采用标記幀的3D點雲，并根據ground truth邊界框裁剪它們。這些裁剪點雲作為觀察結果與物體3D網格模型一起被提供給基準。 ICP和FPFH用于将物體模型登記到裁剪的觀測點雲。允許最大疊代次數為50000次。
圖3 CT-Map的物體位姿估計
CT-Map顯着優于ICP和FPFH。由于生成推斷反複地對物體位姿假設進行采樣并根據觀察結果對其進行評估，是以CT-Map不會像ICP和FPFH這樣的判别方法受到局部最小值的影響。
作者提供的視訊，機器人帶動RGBD相機移動的過程，采集到多幀資料（RGBD序列），粒子在不斷運動，在資料末尾，粒子收斂到物體的範圍内。
本文作者：耗子
github：https://github.com/lh641446825
知乎：https://www.zhihu.com/people/hao-zi-meng-jian-mao/activities
部落格園：https://www.cnblogs.com/lh641446825/
歡迎關注和轉載！