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