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