2
| 本文作者： 大牛講堂 | 2016-09-28 10:52 |
雷鋒網(wǎng)按：本文作者高翔博士。Simultaneous Localization and Mapping，即時(shí)定位與地圖構(gòu)建技術(shù)。無論在室內(nèi)、野外、空中還是水下，SLAM是機(jī)器人進(jìn)入未知環(huán)境遇到的第一個(gè)問題。本期將給大家介紹SLAM的視覺里程計(jì)。
本講主要關(guān)注視覺里程計(jì)。為了講清楚它的來龍去脈，我們會(huì)進(jìn)行一些公式的推導(dǎo)。視覺里程計(jì)算法需要大量的幾何知識(shí)，我們將在必要的時(shí)候，以Tips的形式介紹一些背景知識(shí)。
一、里程計(jì)
在里程計(jì)問題中，我們希望測量一個(gè)運(yùn)動(dòng)物體的軌跡。這可以通過許多不同的手段來實(shí)現(xiàn)。例如，我們在汽車輪胎上安裝計(jì)數(shù)碼盤，就可以得到輪胎轉(zhuǎn)動(dòng)的距離，從而得到汽車的估計(jì)?；蛘撸部梢詼y量汽車的速度、加速度，通過時(shí)間積分來計(jì)算它的位移。完成這種運(yùn)動(dòng)估計(jì)的裝置（包括硬件和算法）叫做里程計(jì)（Odometry）。
里程計(jì)一個(gè)很重要的特性，是它只關(guān)心局部時(shí)間上的運(yùn)動(dòng)，多數(shù)時(shí)候是指兩個(gè)時(shí)刻間的運(yùn)動(dòng)。當(dāng)我們以某種間隔對時(shí)間進(jìn)行采樣時(shí)，就可估計(jì)運(yùn)動(dòng)物體在各時(shí)間間隔之內(nèi)的運(yùn)動(dòng)。由于這個(gè)估計(jì)受噪聲影響，先前時(shí)刻的估計(jì)誤差，會(huì)累加到后面時(shí)間的運(yùn)動(dòng)之上，這種現(xiàn)象稱為漂移（Drift）。
漂移是我們不希望看到的，它們擾亂全局軌跡的估計(jì)。但是，如果沒有其他校正機(jī)制，而只有局部運(yùn)動(dòng)的情況下，這也是所有里程計(jì)都不可避免的現(xiàn)象之一。
二、視覺里程計(jì)
如果一個(gè)里程計(jì)主要依靠視覺傳感器，比如單目、雙目相機(jī)，我們就叫它視覺里程計(jì)。
和傳統(tǒng)里程計(jì)一樣，視覺里程計(jì)最主要的問題是如何從幾個(gè)相鄰圖像中，估計(jì)相機(jī)的運(yùn)動(dòng)。
相鄰圖像間的相似性，為我們估計(jì)相機(jī)運(yùn)動(dòng)提供了依據(jù)。目前，視覺里程計(jì)的主要方法分為基于特征點(diǎn)的方法和不使用特征點(diǎn)的直接法兩種。特征點(diǎn)方法也叫稀疏方法，而使用特征點(diǎn)描述的也叫稠密方法。
Tips
我們知道，相機(jī)能夠把三維空間中的信息變成一張二維的照片。這件事情是怎么做到的呢？我們需要一個(gè)數(shù)學(xué)模型來清楚地描述它，也就是相機(jī)模型。其中，針孔模型是最常用，也是最簡單的相機(jī)模型。
在這個(gè)模型中，我們考慮一個(gè)空間點(diǎn)X=[x,y,z]，它投影在相機(jī)平面，并產(chǎn)生了一個(gè)像素p，位于照片中的[u,v]位置。假設(shè)相機(jī)光圈中心對準(zhǔn)z軸，成像平面位于z=1處，那么根據(jù)下圖描述的關(guān)系，投影方程即為：
寫成慣用的矩陣形式，則有：
把z寫在左邊，寫成zp的形式，表示z為p的深度值。我們可以看到，投影公式的形式是相當(dāng)簡單的。實(shí)際當(dāng)中，每個(gè)相機(jī)的焦距、光圈中心都有所不同，這些稱為它們的內(nèi)參，一般用C表示。于是投影方程變?yōu)椋?/p>
又，因?yàn)橄鄼C(jī)本身相對外部世界存在著一個(gè)運(yùn)動(dòng)。這個(gè)運(yùn)動(dòng)包括三維空間里的旋轉(zhuǎn)和位移，由一個(gè)旋轉(zhuǎn)矩陣R和平移向量T描述，所以最后的投影關(guān)系為：
這個(gè)方程描述了相機(jī)內(nèi)參、位姿和像素之間的關(guān)系。其中內(nèi)參可以通過標(biāo)定相機(jī)求取，p可以在圖像中觀察到，而z,X,R,T則是待估計(jì)的變量。
三、特征匹配
基于特征的方法是當(dāng)前視覺里程計(jì)的主流方式，有很長時(shí)間的研究歷史。特征方法認(rèn)為，對于兩張圖像，應(yīng)該首先選取一些具有代表性的點(diǎn)，稱為特征點(diǎn)。之后，僅針對這些特征點(diǎn)估計(jì)相機(jī)的運(yùn)動(dòng)，同時(shí)估計(jì)特征點(diǎn)的空間位置。圖像里其他非特征點(diǎn)的信息，則被丟棄了。
特征點(diǎn)方法把一個(gè)對圖像的運(yùn)動(dòng)估計(jì)轉(zhuǎn)換為對兩組點(diǎn)之間的運(yùn)動(dòng)估計(jì)。于是，它的主要問題為：
我們?nèi)绾潍@取圖像特征點(diǎn)？如何匹配它們？
如何根據(jù)已知特征點(diǎn)，計(jì)算相機(jī)的運(yùn)動(dòng)？
1、計(jì)算特征點(diǎn)
第一個(gè)問題屬于計(jì)算機(jī)視覺的研究范圍，和幾何關(guān)系不大，我們在此簡單地加以介紹。過去的研究中，人們設(shè)計(jì)了很多特征點(diǎn)提取方法，包括圖像中的角點(diǎn)、色塊等。近年來流行的特征點(diǎn)計(jì)算則更為復(fù)雜，主要的目的是，在圖像發(fā)生一定的改變后，特征點(diǎn)提取算法仍能提取出相同的點(diǎn)，并能判別它們之間的相關(guān)性。
常用的特征點(diǎn)有Harris角點(diǎn)、SIFT、SURF、ORB等。它們能夠標(biāo)識(shí)出，計(jì)算機(jī)認(rèn)為圖像里哪些點(diǎn)比較特殊，具有較重要的意義。
一張圖像中的Sift特征點(diǎn)
對于每一個(gè)特征點(diǎn)，為了說明它與其他點(diǎn)的區(qū)別，人們還使用“描述子”（Descriptor）對它們加以描述。描述子通常是一個(gè)向量，含有特征點(diǎn)和周圍區(qū)域的信息。如果兩個(gè)特征點(diǎn)的描述子相似，我們就可以認(rèn)為它們是同一個(gè)點(diǎn)。根據(jù)特征點(diǎn)和描述子的信息，我們可以計(jì)算出兩張圖像中的匹配點(diǎn)。
匹配點(diǎn)的示意圖
2、根據(jù)匹配好的特征點(diǎn)估計(jì)相機(jī)運(yùn)動(dòng)
在匹配好特征點(diǎn)后，我們可以得到兩個(gè)一一對應(yīng)的像素點(diǎn)集。接下來要做的，就是根據(jù)兩組匹配好的點(diǎn)集，計(jì)算相機(jī)的運(yùn)動(dòng)了。在普通的單目成像中，我們只知道這兩組點(diǎn)的像素坐標(biāo)。而在雙目和RGBD配置中，我們還知道該特征點(diǎn)離相機(jī)的距離。因此，該問題就出現(xiàn)了多種形式：
· 2D-2D形式：通過兩個(gè)圖像的像素位置來估計(jì)相機(jī)的運(yùn)動(dòng)。
· 3D-2D形式：假設(shè)已知其中一組點(diǎn)的3D坐標(biāo)，以及另一組點(diǎn)的2D坐標(biāo)，求相機(jī)運(yùn)動(dòng)。
· 3D-3D形式:兩組點(diǎn)的3D坐標(biāo)均已知，估計(jì)相機(jī)的運(yùn)動(dòng)。
那么問題就來了：是否需要為這三種情況設(shè)計(jì)不同的計(jì)算方法呢？答案是：既可以單獨(dú)做，也可以統(tǒng)一到一個(gè)大框架里去做。
單獨(dú)做的時(shí)候，2D-2D使用對極幾何的方法，3D-2D使用PnP求解算法，而3D-3D則稱為ICP方法（準(zhǔn)確地說，ICP不需要各點(diǎn)的配對關(guān)系）。
統(tǒng)一的框架，就是指把所有未知變量均作為優(yōu)化變量，而幾何關(guān)系則是優(yōu)化變量之間的約束。由于噪聲的存在，幾何約束通常無法完美滿足。于是，我們把與約束不一致的地方寫進(jìn)誤差函數(shù)。通過最小化誤差函數(shù)，來求得各個(gè)變量的估計(jì)值。這種思路也稱為Bundle Adjustment(BA，中文亦稱捆集優(yōu)化或光束法平差)。
代數(shù)方法簡潔優(yōu)美，但是它們對于噪聲的容忍性較差。存在誤匹配，或者像素坐標(biāo)存在較大誤差時(shí)，它給出的解會(huì)不可靠。而在優(yōu)化方法中，我們先猜測一個(gè)初始值，然后根據(jù)梯度方向進(jìn)行迭代，使誤差下降。Bundle Adjustment非常通用，適用于任意可以建模的模型。但是，由于優(yōu)化問題本身非凸、非線性，使得迭代方法往往只能求出局部最優(yōu)解，而無法獲得全局最優(yōu)解。也就是說，只有在初始值足夠好的情況下，我們才能希望得到一個(gè)滿意的解。
因此，在實(shí)際的VO中，我們會(huì)結(jié)合這兩種方法的優(yōu)點(diǎn)。先使用代數(shù)方法估計(jì)一個(gè)粗略的運(yùn)動(dòng)，然后再用Bundle Adjustment進(jìn)行優(yōu)化，求得可精確的值。
3、重建
在2D-2D情形下，我們通過兩個(gè)圖像的像素位置，估計(jì)了相機(jī)的運(yùn)動(dòng)。在此過程中，并沒有用到這些像素點(diǎn)的3D空間位置信息（而且我們也不知道）。不過，在得到相機(jī)運(yùn)動(dòng)之后，就可以根據(jù)這個(gè)運(yùn)動(dòng)信息，計(jì)算各個(gè)特征點(diǎn)的空間位置，該問題也稱為三角化（Triangularization）。在原理上，只要我們在空間中的不同位置拍攝到了同一個(gè)點(diǎn)，就可以通過照片中的像素位置，來估計(jì)這個(gè)點(diǎn)在真實(shí)空間中的位置。
（圖片來自Wikipedia）
在SLAM中，三角化的結(jié)果，可以視為一種地圖。這樣，我們建立了對地圖的一種點(diǎn)云描述:即通過大量的空間點(diǎn)，來估計(jì)整個(gè)地圖的樣貌。這雖然是一種較粗略的描述，但我們也能看出地圖里究竟有些什么東西。在特征點(diǎn)方法中，因?yàn)槲覀冎恢貥?gòu)特征點(diǎn)的空間位置，這種方法也常常被稱為稀疏重構(gòu)。
除了使用特征點(diǎn)以外，直接法為我們提供了另一種思路。直接法認(rèn)為，對圖像提取特征點(diǎn)的過程中，丟棄了圖像里大量有用的信息。因?yàn)橹蟮倪\(yùn)動(dòng)估計(jì)、重建過程并未使用除特征點(diǎn)以外的信息。舉例來說，對于一張640x480的圖像，原本有30萬左右的像素，但是我們只用了其中幾百個(gè)特征點(diǎn)而已。此外，由于特征點(diǎn)的設(shè)計(jì)缺陷，我們無法保證對于每個(gè)圖，都能提以足夠的、有效的特征點(diǎn)。經(jīng)驗(yàn)告訴我們，這只是在大部分時(shí)候是可行的。
直接法跳過了提取特征點(diǎn)的步驟。它構(gòu)建一個(gè)優(yōu)化問題，直接根據(jù)像素信息（通常是亮度），來估計(jì)相機(jī)的運(yùn)動(dòng)。這種方法省去了提特征的時(shí)間，然而代價(jià)則是，利用了所有信息之后，使得優(yōu)化問題規(guī)模遠(yuǎn)遠(yuǎn)大于使用特征點(diǎn)的規(guī)模。因此，基于直接法的VO，多數(shù)需要GPU加速，才能做到實(shí)時(shí)化。此外，直接方法沒有提特征點(diǎn)，它假設(shè)相機(jī)運(yùn)動(dòng)是連續(xù)的、緩慢的。只有在圖像足夠相似時(shí)才有效。而特征點(diǎn)方法在圖像差異較大時(shí)也能工作。
因?yàn)槔昧藞D像中所有的信息，直接法重構(gòu)的地圖是稠密的，這與基于稀疏特征點(diǎn)的VO有很大不同。在稠密地圖里，你可以看到每處的細(xì)節(jié)，而不是離散的點(diǎn)。
本節(jié)介紹了VO的基本原理。
VO的目標(biāo)是根據(jù)拍攝的圖像估計(jì)相機(jī)的運(yùn)動(dòng)。它的主要方式分為特征點(diǎn)法和直接方法。其中，特征點(diǎn)方法目前占據(jù)主流，能夠在噪聲較大、相機(jī)運(yùn)動(dòng)較快時(shí)工作，但地圖則是稀疏特征點(diǎn)；直接方法不需要提特征，能夠建立稠密地圖，但存在著計(jì)算量大、魯棒性不好的缺陷。
敬請期待下一期地平線大牛講堂！
雷鋒網(wǎng)注：本文由大牛講堂授權(quán)雷鋒網(wǎng)發(fā)布，如需轉(zhuǎn)載請聯(lián)系原作者并注明作者和出處，不得刪減內(nèi)容。如有興趣可關(guān)注公號“地平線機(jī)器人技術(shù)”，了解最新消息。
雷峰網(wǎng)原創(chuàng)文章，未經(jīng)授權(quán)禁止轉(zhuǎn)載。詳情見轉(zhuǎn)載須知。
