I. INTRODUCTION

很多的SLAM框架都在努力地解決一些很難的問(wèn)題。像重新辨認(rèn)之前見過(guò)的landmark，另一個(gè)是如何保存與維護(hù)landmark（這個(gè)回很費(fèi)內(nèi)存空間）。維護(hù)這些點(diǎn)都是為了能夠?qū)崿F(xiàn)loop closeure 。
但是在很多的估計(jì)實(shí)驗(yàn)中是可以使用一些方法來(lái)溫和地避開一些與環(huán)境的交互問(wèn)題的。
通過(guò)放置一些Tag是可以創(chuàng)建一小撮的landmark，特別是在特征點(diǎn)小于100個(gè)時(shí)候尤其有用。TagSLAM是專門對(duì)一些特征點(diǎn)不多的場(chǎng)景，內(nèi)存與計(jì)算速度的要求不高，而且對(duì)于回環(huán)檢測(cè)的要求很高的場(chǎng)景。如果有至少一個(gè)Tag被camera觀察到，就可以進(jìn)行一次回環(huán)處理了。

Tag可以為觀察到的實(shí)體提供幾何上的約束。比如說(shuō)可以預(yù)先確定好的放置的Tag的位置。或者說(shuō)可以使用一些laser工具來(lái)測(cè)算實(shí)體與tag之間的距離。當(dāng)一個(gè)tag被移動(dòng)的相機(jī)認(rèn)出來(lái)之后，會(huì)在相應(yīng)的位置創(chuàng)建一個(gè)軌跡。這個(gè)tag的方法可以大大增強(qiáng)現(xiàn)有的SLAM系統(tǒng)和VIO系統(tǒng)。

這個(gè)TagSLAM是使用GTSAM來(lái)實(shí)現(xiàn)優(yōu)化的，需要估計(jì)一個(gè)良好的初值，不然的話可能會(huì)收斂失敗。

值得注意的是，SLAM是一個(gè)想當(dāng)通用的方法。如果你知道了相機(jī)的位姿，那么相對(duì)的，你就可以知道landmark的位置，同理 如果你知道了landmark 的位置，那么就可以知道相機(jī)的位姿。TagSlam繼承了這個(gè)特征，可以實(shí)現(xiàn)相機(jī)是靜止的情況下，可以估計(jì)出移動(dòng)Tag 的位置。

II. RELATED WORK

重點(diǎn)提到了一個(gè)SPM-SLAM作用與其相近。
SPM-SLAM是和一般的SLAM一樣使用過(guò)的是關(guān)鍵幀模式，而TagSLAM是所有幀都是關(guān)鍵幀然后再利用iSAM2進(jìn)行稀疏化（這里不是很懂）。

對(duì)于長(zhǎng)路程的話SPM-SLAM是優(yōu)于TagSLAM，TagSLAM可以滿足更加多的場(chǎng)景需求。
SPM-SLAM不可以包含關(guān)鍵點(diǎn)的測(cè)量，但是這個(gè)缺點(diǎn)在后來(lái)UcoSLAM中被解決了。
SPM-SLAM使用兩幀之間的關(guān)系測(cè)量出Tag 的位姿，TagSLAM則是不會(huì)立刻使用Tag 的關(guān)系，直到?jīng)]有二義性地確定了Tag 的位姿。

SPM-SLAM只注意他的模糊的測(cè)量，TagSLAM則是會(huì)額外考慮Tag 的觀測(cè)形狀等。

III. MODEL SETUP

符號(hào)表示：

A坐標(biāo)系下的X，乘上A坐標(biāo)系下到B坐標(biāo)的變換矩陣，得到B坐標(biāo)系下的X坐標(biāo)

body：是與tag和camera所相聯(lián)系的。body的位姿可以是靜態(tài)的，也可以是動(dòng)態(tài)的。
Tag：必須是獨(dú)一無(wú)二的，同時(shí)每一個(gè)Tag必須與body相聯(lián)系，沒有相聯(lián)系的tag將會(huì)無(wú)視它。不同于body，tag的位姿只能是靜態(tài)的。
camera pose：就像tag位姿是靜態(tài)的。是來(lái)源于body所鏈接的相機(jī)位姿的（應(yīng)該是先驗(yàn)的意思）。所來(lái)源于的body，對(duì)于camera來(lái)說(shuō)被稱為rig。

IV. FACTOR GRAPH

這個(gè)優(yōu)化是才用貝葉斯公式進(jìn)行最大似然估計(jì)的方法來(lái)優(yōu)化的，利用最大似然估計(jì)的值作為優(yōu)化值。默認(rèn)所有的概率分布都是高斯分布。

優(yōu)化的對(duì)象是各種位姿，一共包括了4種約束：

每一幀相機(jī)位姿及其相連的body之間的約束。（body類似于關(guān)鍵幀，但是body只有在遇到了tag才有）

每個(gè)tag位姿及其相連的body之間的約束。

body和師姐坐標(biāo)之間的約束（這個(gè)不是很懂）

動(dòng)態(tài)的body坐標(biāo)和世界坐標(biāo)之間的約束。（這個(gè)也不懂）

Absolute Pose Prior

利用一個(gè)特定的T0幀（可能就是前面說(shuō)的body）來(lái)形成約束：

Relative Pose Prior.

利用相鄰的AB幀之間重投影誤差來(lái)形成約束 。
動(dòng)態(tài)的body幀之間的也是這樣形成約束：

Tag Projection Factor.

利用tag的四個(gè)頂點(diǎn)來(lái)重投影誤差來(lái)確定相機(jī)的位姿。S是tag角點(diǎn)坐標(biāo)，c表示的是4個(gè)角點(diǎn)。

圖優(yōu)化表示：

方塊是約束，圓是優(yōu)化點(diǎn)。PT是4元邊，Pr是二元邊。

V. ROBUST INITIALIZATION

非線性優(yōu)化使用的是GSTAM庫(kù)。
很多的認(rèn)為因素都會(huì)導(dǎo)致優(yōu)化出錯(cuò)。所以初始化的過(guò)程是很重要的。

重要：實(shí)驗(yàn)表明，如果只能看到一個(gè)tag，其實(shí)對(duì)于確定位姿是很不準(zhǔn)的。至少在畫面中需要出現(xiàn)兩個(gè)tag或者是多個(gè)相機(jī)觀察到tag并且直到他們之間的內(nèi)參的時(shí)候，這個(gè)約束才是有用的。（所以定位起碼需要兩個(gè)tag）。

單純的PnP得出來(lái)的結(jié)果是沒有魯棒性的。優(yōu)于這個(gè)原因，論文中所有的基于單位來(lái)獲取位姿初始化都只能來(lái)自于一組最小的tag，也就是需要有最小的tag數(shù)，并且需要考慮這些tag的是使用與他的使用順序。（這里的初始化表示的是某一個(gè)位姿的創(chuàng)建）

為了盡可能地利用歷史的觀測(cè)，這個(gè)系統(tǒng)維護(hù)了兩個(gè)graph：保存了所有factor的完全圖 和 直到當(dāng)前時(shí)間變量使用的所有測(cè)量值，也就是說(shuō)是只保存滿足當(dāng)前變量?jī)?yōu)化約束的factor的優(yōu)化圖。

A. Subgraph discovery

當(dāng)新測(cè)量值來(lái)到的時(shí)候可能會(huì)導(dǎo)致當(dāng)前的某個(gè)位姿可以確定下來(lái)，也可能會(huì)導(dǎo)致以前的某個(gè)位姿可以確定下來(lái)。所以可以確定產(chǎn)生確定下來(lái)的位姿的fator和變量都會(huì)加入到subgraph中。并且按照這些約束的產(chǎn)生順序，加入到initialization list中。這個(gè)順學(xué)在后面的處理中將會(huì)有很大的作用。

已經(jīng)發(fā)現(xiàn)的subgraph是會(huì)依賴于新發(fā)現(xiàn)的factor的，一個(gè)新的factor 會(huì)可以加入到subgraph中。在通常情況下，subgraph是相連的，一個(gè)組新的factor 會(huì)僅產(chǎn)生一個(gè)subgraph。

本文的方法中會(huì)嚴(yán)格控制tag的位姿約束進(jìn)入到subgraph中，除非是tag的位姿已經(jīng)確定了或者是已經(jīng)給了一個(gè)位姿先驗(yàn)。，亦或者是觀察他的角度小于60度（這樣的重投影誤差會(huì)比較明顯）。

所有進(jìn)入到subgraph中過(guò)的變量都應(yīng)該是已經(jīng)確定的（有初值），對(duì)于那棟動(dòng)態(tài)的位姿，也應(yīng)該有個(gè)約束或者是先驗(yàn)信息。上面這個(gè)圖就是和Fig2對(duì)比，t時(shí)刻的動(dòng)態(tài)位姿雖然沒有確定，但是也會(huì)有先驗(yàn)位姿。

B. Order of subgraph discovery

進(jìn)入到subgraph是會(huì)按照下面的順序的：
1、任何與先驗(yàn)位姿相關(guān)聯(lián)的factor
2、任何與tag的重投影相關(guān)聯(lián)的factor
3、任何不建立位姿的factor

讀后感想

相較于常規(guī)的SLAM系統(tǒng)，用了一些新的設(shè)計(jì)思路。
1、subgraph系統(tǒng)，tag只會(huì)在確定了位姿之后才會(huì)加進(jìn)去因子圖中。
2、所有幀都會(huì)加入進(jìn)去，同時(shí)也是用了body節(jié)點(diǎn)的概念。
3、優(yōu)化的思想是基于最大似然估計(jì)，而不是常規(guī)的最小二乘。
4、參考系可以相互轉(zhuǎn)轉(zhuǎn)化，可以在相機(jī)不動(dòng)的情況下，獲得觀察的tag的位姿。
缺點(diǎn)：
1、因?yàn)榧軜?gòu)的不同，加入到別的SLAM框架里面的難度加大。
2、所有幀都會(huì)參與優(yōu)化，計(jì)算量比較大，不適合于移動(dòng)平臺(tái)。

總結(jié)

以上是生活随笔為你收集整理的【论文】TagSLAM: Robust SLAM with Fiducial Markers的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。