實戰(zhàn) | 如何制作一個SLAM軌跡真值獲取裝置？
點擊下方卡片，關(guān)注“新機(jī)器視覺”公眾號
重磅干貨，第一時間送達(dá)
本文知乎作者楊小東授權(quán)轉(zhuǎn)載，未經(jīng)授權(quán)禁止二次轉(zhuǎn)載
原文：
SLAM性能的一個很重要的指標(biāo)是定位精度，公開的室內(nèi)數(shù)據(jù)集一般會采用動捕設(shè)備獲取設(shè)備軌跡真值。但是動捕設(shè)備價格昂貴，OptiTrack百萬以上，國產(chǎn)的Nokov >20W。沒有動捕設(shè)備，我們就很難用自己采集的數(shù)據(jù)評估SLAM的定位性能。
為此，我們搞了一個廉價的軌跡真值捕獲裝置。用一個固定的單目相機(jī)獲取平面運動的軌跡真值。我們主要是用這種方法來測室內(nèi)移動機(jī)器人的運動軌跡。
本文給出了原理和代碼。
注意：只能測2D平面內(nèi)的運動，不是3D。
1. 原理
如下圖，支架上固定一個下視的單目相機(jī)，用來捕捉機(jī)器人頂端平貼的一張ArUco碼。ArUco碼相對的機(jī)器人的位姿是已知的（這個應(yīng)該很容以做到吧）。我們要解決的問題就是利用單目相機(jī)測量ArUco碼在平面上的位姿。實際上，OpenCV提供的ArUco的庫，可以實現(xiàn)ArUco碼的識別，并提供碼四個角點的像素坐標(biāo)。如果我們能計算出到四個角點在平面上的坐標(biāo)，就可以計算出機(jī)器人在平面上的位姿（位置和朝向）。下面就來說一下如何計算一個點在平面上的位置。
世界坐標(biāo)系下，一個3D的點
，投影到圖像坐標(biāo)下，可表示為
T 是外參。我們想要測量的是2D平面內(nèi)的坐標(biāo)，為了方便，可以把世界坐標(biāo)系的原點設(shè)在平面之上，Z 軸垂直于被這個平面（就是ArUco那個平面）。那么
的 z =0 。式（1）就可以變?yōu)?/p>
平面上的點
和圖像上的像素
之間就建立了聯(lián)系。它們之間就是一個單應(yīng)矩陣H 。
我們只要知道這個單應(yīng)矩陣H ，就可以通過圖像像素
2. 現(xiàn)在的問題是單應(yīng)矩陣怎么來的呢？
從式（2）可以看出，只要知道攝像機(jī)的內(nèi)參數(shù)K 和外參數(shù)T ，H就有了。內(nèi)參數(shù) K好說，大家應(yīng)該都很熟悉。關(guān)鍵是 T，我們這里用平面靶標(biāo)來標(biāo)定。具體的，如下圖把一張棋盤格靶標(biāo)放在ArUco碼之上，通過PnP方法可以計算出靶標(biāo)坐標(biāo)系與攝像機(jī)坐標(biāo)系之間的位姿變換 T'。注意，棋盤格靶標(biāo)這個平面并不是ArUco碼的平面，因為棋盤格靶標(biāo)有厚度 t。這個厚度 t我們很容易測量，認(rèn)為是已知的。那么，相機(jī)坐標(biāo)系和ArUco面上的世界坐標(biāo)系之間的坐標(biāo)變換就是
至此，我們就得到了外參數(shù) T 。通過（2）式也就可以得到單應(yīng)矩陣 H。
3. 下面的問題是計算機(jī)器人的位姿
如上圖，剛才我們提到，在圖像上可以得到ArUco碼四個角點的像素坐標(biāo)，這里記為
使用（3）式，可以分別計算出這四個點在世界坐標(biāo)系下的坐標(biāo)（就是平面內(nèi)的坐標(biāo)）
關(guān)于機(jī)器人的位置，其實我們只需要一個點的坐標(biāo)就好了，就是ArUco碼中心點的坐標(biāo)。因此我們把四個點合成為一個點：
就是機(jī)器人的位置。
下面還要計算一下機(jī)器人的姿態(tài)，在2D平面就是朝向了。用兩個點就可以計算出朝向，現(xiàn)在我們手里有四個點，那就平均一下好了。θ就是機(jī)器人的朝向。
4. 測量的精度問題
既然要作為真值去用，那就要求要有足夠的測量精度。下面我們分析一下測量精度。我們把式（3）展開
消去 λ可以得到
如果圖像u方向上像素提取的精度-方差為
，v方向的精度-方差為
。也就是像素點的協(xié)方差為
那么測得的平面上點的協(xié)方差就是
至此就精度分析完了，也就是說標(biāo)出來單應(yīng)矩陣 H之后，通過（12）式就可以計算出大致的測量精度。
實際上，也可以把攝像機(jī)的內(nèi)外參帶入到（3）式，再結(jié)合（12）式，可以獲得一個解析的精度表達(dá)。這個比較復(fù)雜。如果相機(jī)安裝的時候相對于測量平面僅是繞相機(jī)坐標(biāo)系的x軸旋轉(zhuǎn)一個角度的話，測量區(qū)域內(nèi)的精度分布如下圖。如果攝像機(jī)正對測量平面的話，測量區(qū)域的測量精度是相同的。當(dāng)然，相機(jī)的分辨率越高精度越高。焦距越長精度越高，但是同時可測量的區(qū)域也會變小。
我自己使用的相機(jī)是大恒MER-302-56U3C相機(jī)，2048×1536像素；鏡頭是kowa LM3NC1M，3.5mm廣角鏡頭；攝像機(jī)離測量平面的距離大概是2m。可測量范圍大概是4m×3m。如果認(rèn)為像素的提取精度為1個pixel的話，測量精度約為2mm。這個精度足夠用于SLAM軌跡評估。
5. 關(guān)于測量范圍
如果相機(jī)正對于測量平面，測量區(qū)域是一個矩形。如果相機(jī)傾斜放置的話，測量區(qū)域就是一般的四邊形了。上圖就是一種。測量區(qū)域的大小跟鏡頭的視場角、相機(jī)安裝的高度、角度都有關(guān)系，比較復(fù)雜。總的來說，為了使測量平面內(nèi)的測量精度一致，應(yīng)該盡量讓相機(jī)正對測量平面。那焦距越短，測量視場越大，但是測量精度隨之降低。為了獲取足夠的測量精度，就需要一個高分辨率的相機(jī)。為了降低懸掛高度，就需要一個短焦鏡頭。
6. 代碼
標(biāo)定和測量的全部代碼如下
https://github.com/ydsf16
本文僅做學(xué)術(shù)分享，如有侵權(quán)，請聯(lián)系刪文。