論文閱讀：《TwistSLAM: Constrained SLAM in Dynamic Environment》

Motivation

現(xiàn)有的視覺(jué)SLAM一般假定環(huán)境是剛性的，這種假設(shè)很大程度上限制了算法的應(yīng)用，因?yàn)楝F(xiàn)實(shí)環(huán)境是動(dòng)態(tài)變化的(汽車，行人等)?，F(xiàn)有的解決方法一是通過(guò)動(dòng)態(tài)物體去除來(lái)緩解這個(gè)問(wèn)題，但是隨著動(dòng)態(tài)物體點(diǎn)占比的提升，直接采取去除的方式會(huì)大大降低定位的精度；二是利用深度學(xué)習(xí)來(lái)標(biāo)注動(dòng)態(tài)物體從而來(lái)維持靜態(tài)環(huán)境的假設(shè)，但是這樣會(huì)導(dǎo)致環(huán)境信息的丟失；三是基于語(yǔ)義來(lái)同時(shí)解決SLAM和目標(biāo)跟蹤問(wèn)題，這也是這篇文章的技術(shù)基礎(chǔ)之一。下圖是算法的示意圖: 算法同時(shí)估計(jì)相機(jī)位姿和場(chǎng)景中的移動(dòng)對(duì)象，場(chǎng)景中的地圖結(jié)構(gòu)(圖中為道路平面)會(huì)約束對(duì)象的移動(dòng)。算法框架基于的是ORB-SLAM2，在建圖時(shí)，會(huì)同時(shí)使用語(yǔ)義信息來(lái)構(gòu)建與場(chǎng)景中對(duì)象對(duì)應(yīng)的簇地圖，簇地圖構(gòu)建完成后，可以僅利用靜態(tài)簇(道路和房屋)來(lái)估計(jì)位姿。動(dòng)態(tài)簇可以通過(guò)速度的變化來(lái)進(jìn)行跟蹤和約束。

作者：晃晃悠悠的虛無(wú)周|來(lái)源：微信公眾號(hào)「3D視覺(jué)工坊」

另外，目前雖然也有可以跟蹤動(dòng)態(tài)物體的SLAM，但是他們絕大部分都是使用重投影誤差之類的優(yōu)化項(xiàng)，這些都是6個(gè)自由度。這其實(shí)與現(xiàn)實(shí)世界并不符合，比如說(shuō)，大部分汽車只有3個(gè)自由度，2個(gè)自由度在平移(道路平面)，1個(gè)自由度在旋轉(zhuǎn)(汽車自身法向量)。所以消除與物理意義不對(duì)應(yīng)的自由度也是有必要的。對(duì)應(yīng)這個(gè)問(wèn)題的解決辦法，作者是選擇將約束表示為機(jī)械關(guān)節(jié)，因?yàn)闄C(jī)械關(guān)節(jié)的運(yùn)動(dòng)是有限制的，從而減少了自由度。

Contribution

1. 提出了一種語(yǔ)義SLAM系統(tǒng)，可以在靜態(tài)和動(dòng)態(tài)場(chǎng)景中可靠地估計(jì)相機(jī)的位姿。

2. 提出了一個(gè)可以跟蹤場(chǎng)景中多個(gè)移動(dòng)對(duì)象的立體SLAM框架。

3. 提出了一種新的跟蹤和BA公式，該公式考慮了場(chǎng)景中對(duì)象之間的機(jī)械關(guān)節(jié)特性，從而約束運(yùn)動(dòng)的自由度。

Content

1. 符號(hào)聲明

剛性物體o關(guān)于世界坐標(biāo)系的位姿:

運(yùn)動(dòng)物體的速度可以表示為一個(gè)twist:

twist同時(shí)也可以表示為矩陣:

關(guān)于twist的指數(shù)映射:

那么運(yùn)動(dòng)物體在楨間的運(yùn)動(dòng)可以表示為:

同一個(gè)物體同一時(shí)間在不同坐標(biāo)系的表示可以通過(guò)伴隨映射矩陣來(lái)轉(zhuǎn)換:

伴隨映射矩陣可以通過(guò)不同坐標(biāo)系的相對(duì)位姿來(lái)構(gòu)建:

2.語(yǔ)義簇圖語(yǔ)義簇圖是作者提出的一個(gè)數(shù)據(jù)結(jié)構(gòu)，圖的頂點(diǎn)對(duì)應(yīng)于場(chǎng)景中的對(duì)象，邊對(duì)應(yīng)于對(duì)象之間存在的物理鏈接, 示意圖如下:

目標(biāo)是估計(jì)相機(jī)的位姿和每個(gè)移動(dòng)對(duì)象的位姿，同時(shí)使用對(duì)象之間的機(jī)械關(guān)節(jié)來(lái)改進(jìn)這些位姿估計(jì)，上圖的兩個(gè)汽車簇都通過(guò)一個(gè)平面約束鏈接到道路，該約束僅具有3自由度(1個(gè)關(guān)于平面法向量的旋轉(zhuǎn)自由度，2個(gè)平面內(nèi)的平移自由度)。這樣的簡(jiǎn)單表示允許具有高度通用性，因?yàn)閷?duì)于給定的語(yǔ)義類，只需要定義其靜態(tài)父類和優(yōu)化的機(jī)械關(guān)節(jié)的類型。3.算法框圖如下圖。使用語(yǔ)義信息創(chuàng)建與場(chǎng)景中的對(duì)象相對(duì)應(yīng)的點(diǎn)簇。然后使用靜態(tài)語(yǔ)義簇（例如道路、地板、房子）來(lái)魯棒地跟蹤相機(jī)。接下來(lái)，匹配與動(dòng)態(tài)對(duì)象（例如，汽車、自行車等）對(duì)應(yīng)的關(guān)鍵點(diǎn)，通過(guò)雙目圖片跟蹤或三角化新的3D點(diǎn)。然后，在BA中，使用靜態(tài)和動(dòng)態(tài)3D點(diǎn)對(duì)相機(jī)和對(duì)象的所有位姿估計(jì)進(jìn)行優(yōu)化。

4.從全景分割創(chuàng)建簇這一步，作者選擇基于S3LAM的全景分割方式來(lái)創(chuàng)建簇。全景圖像的優(yōu)點(diǎn)在于能夠知道圖像中每個(gè)像素的語(yǔ)義類，并為每個(gè)對(duì)象指定唯一的id。然后融合單個(gè)3D點(diǎn)的多個(gè)2D觀測(cè)值，以獲得其類別和id，從而獲得最終的語(yǔ)義地圖。因?yàn)殪o態(tài)對(duì)象是固定的，所以在直接表示他們的三維點(diǎn)；對(duì)于動(dòng)態(tài)對(duì)象，通過(guò)簇位姿和簇twist來(lái)表示。5.簇幾何這一部分的目標(biāo)是為計(jì)算機(jī)械關(guān)節(jié)約束動(dòng)態(tài)簇的速度和位姿作鋪墊。假設(shè)僅考慮平面簇，對(duì)于每一個(gè)平面簇都可以通過(guò)SVD估計(jì)出一個(gè)三維平面，然后通過(guò)RANSAC去除離群值。6.動(dòng)態(tài)SLAM首先是僅使用靜態(tài)點(diǎn)簇的點(diǎn)來(lái)計(jì)算相機(jī)位姿，代價(jià)函數(shù)如下:

考慮到如果場(chǎng)景中包含許多實(shí)際上是靜態(tài)的潛在移動(dòng)對(duì)象，例如停放的汽車，則位姿可能會(huì)變差。為了解決這個(gè)問(wèn)題，可以估計(jì)所有運(yùn)動(dòng)對(duì)象的位姿，并將它們集成到BA中，將速度接近0的動(dòng)態(tài)對(duì)象點(diǎn)也作為靜態(tài)點(diǎn)。7.動(dòng)態(tài)數(shù)據(jù)關(guān)聯(lián)和關(guān)鍵點(diǎn)動(dòng)態(tài)數(shù)據(jù)關(guān)聯(lián)的挑戰(zhàn)主要有兩個(gè)，一個(gè)是相機(jī)和對(duì)象運(yùn)動(dòng)的組合會(huì)在圖像空間中產(chǎn)生較大的位移，因此需要進(jìn)行大半徑搜索以匹配關(guān)鍵點(diǎn)，另一個(gè)是較大的移動(dòng)可能會(huì)導(dǎo)致圖像中對(duì)象的重要視覺(jué)變化（例如，對(duì)象的亮度變化或視點(diǎn)變化），這使得匹配過(guò)程更加困難。作者采用的是RAFT中所用的CNN產(chǎn)生光流估計(jì)的方法來(lái)解決上述問(wèn)題。目標(biāo)跟蹤的問(wèn)題是動(dòng)態(tài)對(duì)象通常占據(jù)圖像的一小部分，導(dǎo)致目標(biāo)點(diǎn)太少，無(wú)法獲得精確的估計(jì)。對(duì)于這個(gè)問(wèn)題，作者強(qiáng)制關(guān)鍵點(diǎn)提取過(guò)程從對(duì)象邊界框定義的區(qū)域中保留更多的關(guān)鍵點(diǎn)。然后使用關(guān)鍵點(diǎn)創(chuàng)建具有雙目三角剖分的新三維點(diǎn)，并且將這些點(diǎn)添加到現(xiàn)有簇中或用于創(chuàng)建新簇，或者直接用來(lái)進(jìn)行跟蹤。8.簇間約束的機(jī)械關(guān)節(jié)首先提出一個(gè)假設(shè):大部分移動(dòng)簇可以通過(guò)特定的機(jī)械關(guān)節(jié)鏈接到靜態(tài)父簇。根據(jù)ISO3952,存在有12個(gè)標(biāo)準(zhǔn)關(guān)節(jié)。作者主要使用的是平面關(guān)節(jié)(3個(gè)自由度：平面中的2個(gè)平移和圍繞其法線的1個(gè)旋轉(zhuǎn)，表示汽車相對(duì)于其靜態(tài)父對(duì)象道路的位移)和旋轉(zhuǎn)關(guān)節(jié)(1自由度。靜態(tài)父簇是墻，移動(dòng)簇是門)。為了更方便的建模關(guān)節(jié)，可以將twist的空間分解為兩個(gè)正交空間的和:

Fl表示通過(guò)關(guān)節(jié)l約束的twist的空間在坐標(biāo)系F下的表示。對(duì)于具有z軸的平面關(guān)節(jié)，F(xiàn)l定義為:

要使對(duì)象的位移在物理上精確，twist必須位于Fl空間內(nèi)。為此，將twist從其原始空間投影到Fl。因?yàn)镽6是歐幾里德矩陣，所以投影是一個(gè)6×6矩陣，定義如下：

舉個(gè)例子，如果是平面關(guān)節(jié)，那么:

那么,twist關(guān)于這個(gè)平面關(guān)節(jié)的變換可以表示為:

更多干貨

歡迎加入【3D視覺(jué)工坊】交流群，方向涉及3D視覺(jué)、計(jì)算機(jī)視覺(jué)、深度學(xué)習(xí)、vSLAM、激光SLAM、立體視覺(jué)、自動(dòng)駕駛、點(diǎn)云處理、三維重建、多視圖幾何、結(jié)構(gòu)光、多傳感器融合、VR/AR、學(xué)術(shù)交流、求職交流等。工坊致力于干貨輸出，為3D領(lǐng)域貢獻(xiàn)自己的力量！歡迎大家一起交流成長(zhǎng)~

添加小助手微信：dddvision，備注學(xué)校/公司+姓名+研究方向即可加入工坊一起學(xué)習(xí)進(jìn)步。