視覺里程計 第一部分：前30年的發展和理論基礎

視覺里程計 第一部分：前30年的發展和理論基礎

VisualOdometry -Part I The First 30 Years and Fundamentals

TaylorGuo, 2017年1月27日星期五

視覺里程計是通過單個相機或多個相機作爲輸入估計自身運動的過程。應用領域涵蓋機器人、可穿戴計算、增強現實和自動駕駛。Visual Odometry (VO)視覺里程計由Nister在2004年路標論文中創造。這個名詞和車輪測距非常類似，增量式地通過對車輪轉過的圈數積分估計車輛的運動。同樣的，視覺里程計使用板載相機檢測圖像運動變化增量式地估計車輛位姿。爲了使視覺里程更有效，必須有足夠的環境圖像，具有足夠紋理的靜態圖像可以提取運動特徵。另外，需要捕捉連續的圖像使場景重疊。

相比車輛測距，視覺里程計的優勢是不受車輪在不平整地面或其他不良狀況下滑動的影響。視覺里程計還提供了更爲精確的軌跡估計，相對位置誤差範圍爲0.1%到2%。這使得視覺里程成爲車輪測距有益補充，還有其他導航系統，比如全球衛星定位系統GPS，慣性測量單元IMU，和鐳射測距（與視覺里程類似，鐳射測距通過連續的鐳射掃描匹配車輛估計自身運動）。在GPS失效的環境下，比如水下或太空中，視覺里程計非常重要。

這個指南和綜述分爲兩部分，詳細介紹視覺里程計，包含從1980年到2011年間的研究。前20年大多是離線執行，在視覺里程計第一次用於火星探索時，第3個10年實時系統開始大量出現。第一部分指南，回顧了頭30年該領域的研究及基礎。簡要地討論了相機模型和標定，介紹了單目和雙目的主要運動估計方法，分析了每種方法的優勢和劣勢。第二部分主要講述特徵匹配，魯棒性和應用。它主要回顧了視覺里程計中的點特徵檢測和不同的無效數據（離羣點）去除方案。重點講述了RANSAC隨機抽樣一致性算法，討論採用獨特設計方法提高計算速度。其他的主題涵蓋誤差模型構建，位置識別（或閉環迴路檢測），捆集調整。

這個手冊給沒有經驗和非專業用戶提供了算法指南和參考用於構建完整的視覺里程系統。既然理想的或獨一無二的適用各種工作環境的視覺里程計方案並不存在，那麼最佳的方案應該要根據特定的導航環境和給定的運算資源仔細地選擇。

視覺里程的歷史

計算機視覺領域中，從一組相機圖像（校準的或沒有校準的）中重構相關相機位姿和3維結構的問題，衆所周知，採用運動恢復結構structure from motion (SfM)方法。它源自於論文2和論文3的工作。視覺里程計是運動恢復結構SfM方法的特例。運動恢復結構SfM方法更通用，處理從連續圖像序列中的結構和相機位姿進行3D重建問題。最終結構和相機位姿通過一個離線優化（比如，捆集調整）來精煉，它的計算時間隨圖像的數量增長，如論文4。相反地，視覺里程計主要估計相機序列的3D運動，以新的圖像幀進入的方式，它是實時的。捆集調整用於精煉軌跡的局部估計。

單獨利用視覺輸入估計車輛自身運動的問題始於1980年代早期，論文5中有描述。早期的視覺里程計研究，如論文5到論文9，用於行星探路車，受到NASA火星探索計劃的激勵，致力於提供可以在各種地形下探路車可以測量6自由度的運動的能力，即使是在不平整路面和粗糙地形下出現輪子打滑。

Moravec論文5的方法出衆，不僅僅因爲第一次提供了運動估計方法，它們的主要功能模塊至今仍在使用，還提供了一種最早期的角點檢測方法（第一次由Hannah論文10在1974年提出）如今大家熟知的Moravec角點檢測，如論文11，是Forstner論文12和Harris和Stephens論文3和論文82的先驅。

Moravec在一個行星探路車上測試了他的工作，這個車子配備了稱爲“sliderstereo”的一個單目相機在軌道上滑動。機器人以停止-運動的方式前進，在每個位置獲取和分析圖像。每停一次，相機水平滑動、在相同的間隔下拍9張照片。圖像上角點檢測採用他的操作方法，沿極對線用歸一化互相關係數方法匹配其他8幀圖像。如果有大尺度的場景變化採用coarse-to-fine策略（金字塔）通過相關係數查找機器人下一個位置的潛在匹配。無效數據（離羣點）通過檢測8個立體圖像的深度值的不一致性去除。最後，從兩個連續的機器人位置對比三角化的3D點雲計算運動狀況作爲粗略的移動變換。系統可以通過一個加權的最小二乘法處理，權值與3D點雲的距離呈相反關係。

儘管Moravec採用了單目滑動相機，但他的工作仍然屬於立體視覺里程計算法的一種。相關的3D位置特徵通過每個位置的三角化方法直接測得，用於獲取相對運動測量。三目的方法也屬於同一類算法問題。相對立體視覺，單目相機也是一種方法。在這種情況下，只有輪子滾動的信息可以採用。不足之處是運動信息只能在一個維度上獲取。絕對尺度信息可以通過直接測量的方法（比如，測量場景中的某一個元素的尺寸大小），運動約束，或集成其它傳感器，如IMU，氣壓傳感器，和距離傳感器，來獲得。當相機到場景的距離過大、大於立體相機的基線（兩個相機之間的距離），stereoVO的效果就退化成單目。在這種情況下，立體視覺無效，必須採用單目視覺方法。經過多年發展，單目和立體視覺里程計分別發展成兩個獨立的研究路線。接下來，我們將瞭解相關領域的工作。

立體視覺里程計

大部分視覺里程計研究採用的立體相機。基於Moravec的工作，Matthies和Shafer論文6和論文7採用雙目系統和Moravec的方法檢測和追蹤角點。與Moravec不確定概率的數量表示不同的是，他們採用三角化特徵的誤差協方差矩陣，並將其整合進運動估計的步驟中去。他們還演示了探路車運動軌跡的效果，5.5米的路上2%的相關誤差。Olson的論文9和論文13，後來擴展了這些工作，引入了絕對方向傳感器（比如，電子羅盤或全景相機），採用Forstner角點檢測，明顯加快了計算速度。他們發現單獨採用相機自身運動估計會產生累積誤差，該誤差隨着移動距離的增加呈超線性增加，最後導致航向誤差。如果加入一個絕對方向傳感器，誤差的增長會隨着移動距離的增加呈線性增長。他們測出在20米的道路上相對位置的誤差爲1.2%。

Lacroix論文8在探路車上採用了和前面類似的立體視覺里程方法。差別是關鍵點的選擇。沒有用Forstner角點檢測，他們採用稠密立體視覺，通過分析峯值處的相關函數選取候選關鍵點，方法如論文14，論文15所示。這種選擇方法基於觀測值，在相關曲線的形狀和特徵深度值的標準偏移之間有非常強的相關性。這種觀測方法後來被Cheng論文16和論文17應用在火星探路車上的視覺里程計上。他們在兩個方面改進了早期Olson論文9和論文13裏的實現方法。第一，採用Harris角點檢測方法，使用特徵點相關函數的曲率，如Lacroix提到的，去定義圖像點雲的誤差協方差矩陣。第二，如Nister論文1，他們採用RANSAC隨機抽樣一致性算法在最小二乘法運動估計步驟中去除離羣點。

另外一種用於運動估計和離羣點去除的方法由Milella和Siegwart論文14中提出。他們採用論文19中Shi-Tomasi方法進行角點檢測，與Lacroix，他們保留了那些在立體差別地圖中高質量的雲點。運動估計第一次用最小二乘法解決，後來採用迭代最近點算法IterativeClosestPoints（ICP）論文20，該算法廣泛應用於激光鐳射掃描的3D融合，用來做位姿優化。考慮到魯棒性，離羣點的移除步驟也整合進ICP算法。

上面提到的工作，都是3D點三角化用於立體圖像對，相對運動問題通過3D到3D點的融合（對比）問題進行處理。一種完全不同的方法是2004年Nister在論文1提到的。這篇論文不僅創造性地提出了“視覺里程計”的概念，還第一次提出並實現了實時長距離魯棒性的離羣點去除的算法流程。Nister在幾個方面進行了改進。第一，與之前所有的工作相比，他們沒有在所有圖像幀上跟蹤特徵，而是單獨使用Harris角點檢測特徵，在特徵之間進行匹配。在基於交叉相關的跟蹤中的優勢是可以避免特徵偏移。第二，他們沒有計算3D到3D雲點融合問題的相關運動模型，但計算的是3D到2D相機位姿估計問題（這個問題稱爲“運動估計”）。最後，他們將RANSAC離羣點去除算法整合進運動估計的步驟中。

Comport在論文21中介紹了一種不同運動估計算法。並沒有採用3D到3D點雲融合或3D到2D相機位姿估計技術，他們採用四視點張量幾何關係，可以通過2D到2D圖像匹配，而不需要三角化3D點雲。直接使用原始2D點雲替代三角化的3D點雲可以更精確地計算運動信息。

關鍵幀的選擇是非常重要的一步，一般應該在更新運動之前完成。

單目視覺里程計

雙目和單目視覺里程計不同的是，相關運動和3D結構必須從2D數據中計算出來。既然絕對尺度不知道，最開始的兩個相機位姿的距離設置爲1。當一個新的圖像幀進來的時候，相對於前兩個圖像幀的相對尺度和相機位姿采用3D結構或三焦點張量方法，如《多視圖幾何》裏面所講。

長距離（大概幾公里）單目相機採用透視和全景相機在過去10年中獲得了巨大的成功。相關的工作分爲3類：基於特徵的方法，基於表面外觀的方法，和混合方法。特徵方法，跟蹤圖像幀間顯著可重複的特徵；表面外觀方法，使用圖像或圖像子區域的所有像素稠密信息；混合方法，是將兩種方法合併使用。

第一類方法，有論文1，24，25，27和論文30-32。第一次使用單目相機的實時大場景視覺里程計是Nister的論文1。他們採用RANSAC去除離羣點，用3D到2D相機位姿估計計算新的相機位姿。採用了5點法的最小解決方法計算RANSAC中的運動假設，如論文33。之後，5點RANSAC在視覺里程計裏變得非常流行，並應用於論文23，25，27中。Corke在論文24中提出了一種使用反射相機和光流基於全景拍攝的單目視覺里程計。Lhuillier論文25和Mouragnon論文30提出了一種基於局部窗口的捆集調整方法重構運動和3D地圖（這意味着捆集調整可以在前m幀圖像子窗口中執行）。同樣的，他們也採用了5點RANSAC方法去除離羣點。Tardif論文27提供了在2.5公里的視覺里程計方法，他沒有采用捆集調整。與之前的工作不同的是，他們解耦了旋轉和平移估計。旋轉通過無窮遠點計算，平移通過重構3D地圖計算。通過5點RANSAC方法去除錯誤的對應。

基於表面外觀或混合的方法，如論文26，28，29。Goecke論文26採用Fourier-Mellin變換融合汽車上的基準透視圖像。Milford和Wyeth論文28採用RatSLAM算法從汽車上的單透視相機提取粗略的旋轉和平移速度信息。採用模板跟蹤場景中心位置。基於場景外觀的方法在遮擋的魯棒性上不好。因此，Scaramuzza和Siegwart論文29使用場景外觀估計汽車的旋轉，從基準估計旋轉和絕對尺度中估計特徵。基於特徵的方法也用於檢測基於場景外觀方法是否失效。

上面提到的這些方法的設計都沒有6自由度的運動約束。然而，有幾種車用視覺里程計採用了運動約束。它的優勢是減少了計算時間，增強了運動估計精度。例如，Liang和Pears論文35，Ke和Kanade論文36，Wang論文37，Guerrero論文38採用了單應射在一個基準上估計自身運動。Scaramuzza在論文31，39介紹了單點RANSAC離羣點去除方法，它基於汽車非完整的約束加速自身運動估計到400Hz。在後續的工作中，他們用非完整約束從單目相機中恢復汽車轉彎的絕對尺度。根據他的工作，Pretto論文32也採用了非完整約束增強了特徵跟蹤，Fraundorfer論文41採用了子窗口捆集調整。

減少偏移

視覺里程計增量式地計算相機軌跡（一個位姿一個位姿地），每一個新的幀到幀運動計算隨着時間會累積誤差。這樣在實際路徑和估計的軌跡之間就會產生偏移。對有些應用，儘可能地減少偏移是非常重要的，可以通過前m個相機位姿做局部優化。這種方法稱爲滑動窗口捆集調整或子窗口捆集調整，它應用於論文41-44。例如，在一個10公里的視覺里程計的實驗中，Konolige論文43演示了子窗口捆集調整通過因子2-5減少最終位置誤差。顯然，視覺里程計可以通過融合其他傳感器，如GPS和鐳射激光，甚至是IMU，減少誤差，如論文43,45,46。

V-SLAM

儘管本文主要講視覺里程計，但視覺SLAM同樣值得在這裏提一下。具體更深入的SLAM問題的研究，可以參考Durrant-Whyte和Bailey的論文47和48。V-SLAM中兩種方法佔主導地位：1）基於濾波器的圖像信息融合方法，採用概率分佈，論文49；2）非濾波器方法（也稱爲關鍵幀方法）對挑選的關鍵幀使用全局優化，如論文50。論文51評估和總結了它們的主要優點。

過去的幾年中，單目和立體相機都取得了成功，如論文49,52-62。大部分的工作都侷限於小的室內場景，最近有些工作是大場景的，如論文54,60,62。早期的V-SLAM採用了全協方差卡爾曼方法，如論文52,53,49。Davison論文49的優勢是在任何時間上可重複定位。Handa論文59基於概率論框架採用了主動匹配技術優化了這個工作。Civera論文60採用了RANSAC模型假設階段的先驗信息的卡爾曼濾波和單點RANSAC融合。最後，Strasdat論文61提供了一個大場景V-SLAM框架採用關鍵幀優化方法，如論文50，這成爲SLAM的一個特色。

視覺里程計只關注局部軌跡的一致性，SLAM關注全局軌跡的一致性。

VO與V-SLAM

這裏會詳細分析視覺里程計和V-SLAM。SLAM（特指V-SLAM）的目的是要對機器路徑有全局一致性的估計。這意味着要追蹤環境地圖（有些情況下地圖可能不需要）因爲當機器人走過之前去過的地方它需要能夠識別。（這稱作閉環控制。當迴環檢測到後，這個信息用於減少地圖和相機路徑的漂移。理解什麼時候生成迴環和有效地將這個新的約束集成到當前地圖中，是SLAM兩個主要問題。）而視覺里程計主要是增量式地重構路徑，一個位姿接一個位姿地，優化前n個路徑位姿（也稱爲子窗口捆集調整）。這個滑動的窗口優化相當於在SLAM中建立一個局部地圖；然而，其思想方法是不同的：在視覺里程計中我們只關心軌跡的局部一致性，局部地圖用來更精確地估計局部軌跡（比如捆集調整），但SLAM關心的是全局地圖的一致性。

視覺里程計用於構建SLAM算法的一個模塊，用於重構相機的增量運動；然而，爲了設計完整的SLAM方法，需要添加回環檢測，全局優化以獲得精確一致的地圖（沒有這一步，地圖在邏輯上是一致的）。

如果用戶只對相機路徑感興趣，但不需要環境地圖，也可以用完整的V-SLAM方法，而不是視覺里程計技術。V-SLAM方法可以更精確，因爲它增加了很多路徑上約束，但不一定更具魯棒性（比如，離羣點可能嚴重影響地圖一致性）。另外，它更復雜，計算代價也大。

最後，關於視覺里程計和V-SLAM的選擇取決於性能，一致性和簡單性的選擇。儘管，需要相機路徑全局一致性，視覺里程計可以爲了實時性而犧牲一致性，不需要追蹤之前相機的位姿。

視覺里程計問題的公式推導

假設在時間k，通過相機移動，追蹤環境圖像。如果是單目相機，k時間上拍攝的一組圖像表示爲I_0:n={I₀，…，I_n}。如果是立體相機，每個時刻有左右兩幅圖像，記爲I_l,0:n={I_l,0，…，I_l,n}和I_r,0:n={I_r,0，…，I_r,n}。圖1表示了這個具體問題。

圖1，演示了視覺里程計問題。

相鄰的相機位置的相對位姿T_k,K-1（或相機系統位置），通過視覺特徵進行計算，將位姿連起來獲得關於初始座標幀k=0時的絕對位姿C_k。

假設相機的圖像幀座標爲機器人的座標。立體相機，爲不失一般性，左眼的座標爲原點。

在連續時刻k-1和k，相機位置關係通過變換T_k，k-1∈R^4x4記爲：

其中R_k，k-1∈SO(3)是旋轉矩陣，t_k，k-1∈R^3x1是平移向量。這組向量T1:n={T1,0，…，Tn,n-1}包括連續的相機運動。爲了簡便，我們將T_k，k-1記爲T_k。最後，相機位姿C_0:n={C₀,… ,C_n}包含了相機相對初始座標k=0的變換。當前位姿C_n可以通過聯立變換T_k（k=1…n）計算，因此，C_n=C_n-1T_n，其中C₀是相機k=0時的位姿，用戶可以任意指定。

視覺里程計的主要任務就是計算圖像Ik到Ik-1的相對變換，然後聯立變換重建相機的C0:n的軌跡。這意味着視覺里程計一個位姿一個位姿的增量式地重建軌跡。這個步驟之後，前m個位姿迭代優化可以獲得局部軌跡更精確的估計。這個迭代優化的過程，是計算重構前m個圖像3D點雲重投影誤差開方的和的最小化值（稱爲窗口化捆集調整，因爲它在m幀圖像上的子窗口執行。捆集調整將在第二部分詳細介紹）。三角化圖像上的點獲得3D點雲（參考“三角化和關鍵幀的選擇”）。

單目視覺里程計有兩種方法計算相對運動Tk：基於場景外觀的（或全局的）方法，採用兩個輸入圖像的所有像素的亮度信息；基於特徵的方法，只採用從圖像中提取（追蹤）的顯著的、可重複的特徵。全局方法的精度比基於特徵的方法低，計算複雜度更高一些。（在之前“視覺里程計的歷史”章節中，大部分基於場景外觀的方法應用於單目視覺里程計中。主要是由於相比立體相機更容易執行。）基於特徵的方法要求對圖像幀的特徵提取（追蹤）具有魯棒性，它比全局方法更快、更精確。因此，大部分視覺里程計的執行都採用基於特徵的方法。

視覺里程計的主要流程方法如圖2所示。對每個新的圖像Ik（如果是立體相機，就是圖像對），前兩步包括檢測和匹配2D特徵，是通過與之前的圖像幀做對比而進行的。從不同的圖像幀上提取的3D特徵是2維特徵的重投影，這是圖像的對應關係。（我們將在第二部分詳細講解特徵匹配和特徵跟蹤的區別。第一個特徵匹配是在圖像中單獨檢測特徵，然後基於相似的特點再將它們進行匹配；第二個特徵跟蹤是先查找一幅圖像中的特徵，然後用局部搜索方法，比如關聯程度，在下一幅圖像中跟蹤它們。）第三步是計算兩個時刻間k-1和k的相對運動Tk。根據對應關係是3維或2維的不同，有3種不同的方法處理這個問題（如“運動估計”章節所示）。相機位姿Ck就可以通過聯立位姿Tk和之前的位姿計算獲得。最後，通過對前m幀圖像進行迭代優化（捆集調整）可以更精確地估計局部軌跡。

圖2：視覺里程計的主要流程

本文對運動估計也有詳細說明（參考“運動估計”章節）。特徵檢測和匹配，捆集調整將在第二部分詳細說明。同樣的，一個精確地運動計算，特徵對應不應該包含離羣點（比如錯誤的數據關聯）。即使出現離羣點也要確保精確的運動估計，是魯棒估計的主要任務，也將在第二部分詳細說明。大部分視覺里程計的執行都假設相機已經校準過。我們將瀏覽一下透視相機或全景相機的標準模型和校準過程。

相機模型和校準

視覺里程計可以採用透視相機和全景相機。我們將在本章主要討論一下相機的模型。

透視相機模型

透視相機常用模型是針孔射影系統：物體反射的光線通過透鏡中心（投影中心）在焦點平面形成圖像，如圖3a所示。令X=[x,y,z]T爲相機參考幀上場景中的一個點，p=[u,v]T爲圖像屏幕上的一個像素的投影。從3D世界到2D圖像之間的對應關係可以通過一下透視投影等式表示：

其中，λ是深度因子，αu和αv是焦距，u0和v0是圖像投影中心的座標。這些參數稱爲內參。當相機的視角大於45^o時，就會產生光線扭曲，可以通過二次或更高階多項式構建模型。完整的模型推到可以參考計算機視覺的教科書，如論文22和論文63所示。令p=[u,v,1]^T=K^-1[u,v,1]^T爲類似的圖像座標。歸一化座標將在後面的章節中大量使用。

全景相機模型

全景相機的視角非常寬（甚至超過180度），可以用魚眼鏡頭或通過鏡像合併標準相機來製造[也稱爲反折射相機，如圖3b所示]。反折射相機中典型的鏡頭形狀是旋轉的二次曲面（比如拋物面或雙曲線體），因爲它們產生一個投影中心，使得采用運動估計成爲可能，這將在“運動估計”中詳細講解。

圖3（a）透視投影，（b）反射投影，和（c）透視相機和全景相機地球形模型。圖像點朝向觀察點，在單位圓球中標準化。

目前，有兩種全景相機的模型。第一個是論文64中的反折射相機（拋物面或雙曲線的），第二個是論文65中的針對魚眼和反折射相機統一的模型。論文66和論文67對着兩種模型進行了研究。統一模型的投影等式爲：

其中，和a0,a1,…,an是相機內參，取決於它是鏡面鏡頭還是魚眼鏡頭。如論文65所示，n=4對鏡面或魚眼鏡頭來說是一個比較恰當的選擇。最後，這個模型假設圖像平面滿足理想特性，相機感光平面的軸心和鏡面軸心是對齊的。儘管這個假設只是針對魚眼鏡頭和反折射相機的，但可以通過引入理想平面和圖像平面的透視投影構建偏移模型，如論文66所示。

圓球模型

我們希望相機有一個有效單視點(singleeffective viewpoint)，即在視覺系統中只存在一個投影中心。其主要原因是單視點可從全方位攝像頭採集的圖像中產生正確的幾何透視圖像，同時還適用於成熟的外極線幾何理論。反折射相機中，當光線通過鏡面相交於一個點C時，纔會形成投影中心。如果存在這個點，可以使我們能夠從一個單視點到圓球通過對應關係構建全景投影模型。爲了方便，通常會採用單位球體。

值得注意的是球體模型不僅可以應用於全景相機中，還可以應用於透視相機。如果相機校準過，透視圖像或全景圖像上的任何點就可以映射到單位球體上的一個向量。如圖3（C）所示，單位向量指向觀察到的圖像場景中的點。這些向量是單位球體上歸一化圖像點。

相機校準

相機校準的目的是爲了獲得相機系統更精確的內部參數和外部參數。在多相機系統中（比如立體或三相機），外部參數描述了每個相機對的彼此位置關係和方向信息。最常用的模型是採用平面棋盤模型。棋盤上方塊的位置已知。爲了精確地計算校準參數，用戶需要在不同的位置和方向拍幾張棋盤的圖片，拍的時候要確保相機視角儘可能地覆蓋整個場景。相機的內部參數和外部參數可以通過最小二乘法計算。輸入數據是棋盤方格的四角的2D位置和它們對應的每幅圖像的像素座標。

許多相機校準工具都有MATLAB和C版本。最新的可以在網頁68中找到。在這些工具中，最常用的是論文69中的Matlab和論文70到72的透視和全景相機。透視相機的C語言校準版本OpenCV，網頁73中有提供一個開源計算機視覺庫OpenCV。

運動估計

運動估計對每幅圖像來說在視覺里程計中是一個核心步驟。精確的說，是計算運動估計中當前圖像和之前的圖像間的相機運動。將這些單次運動鏈接起來，就可以重構相機的運動軌跡。本章詳細解釋如何從時刻K-1和K的兩組對應的特徵fk-1和fk中計算兩幅圖像Ik-1和Ik的變換Tk。根據特徵對應關係是2維還是3維，有3中不同的方法：

• 2D到2D：fk-1和fk都是2維圖像座標；

• 3D到3D：fk-1和fk都是3維圖像座標。需要三角化每個時刻的3維雲點，比如使用立體相機；

• 3D到2D：fk-1是3維，fk是它們對應的圖像上的2D重投影。單目相機情況下，3D結構需要從2個相鄰的相機視圖（比如Ik-2和Ik-1）進行三角化，然後和第3幅視圖中的2D圖像特徵進行匹配（比如，Ik）。在單目算法中，需要匹配至少3幅視圖。

注意，特徵可能是點或者線。通常，在無結構的場景中由於缺乏直線，就在視覺里程計中使用點特徵。一個深入分析這三種點和線特徵方法的論文，在參考論文74中可見。本文只講解點特徵的推導。

2D到2D：圖像特徵匹配中的運動

估計本徵矩陣

校準過的相機圖像Ik和Ik-1之間的幾何關係可以通過本徵矩陣E表示。E中包含了相機運動參數具有一個未知的平移變換因子：

圖4.對極約束示意圖

用對極約束計算2D到2D特徵匹配的本徵矩陣。最小化方案是採用5個2D到2D的對應關係，如論文75，Nister的論文76提供了一種實現方法。Nister的5點算法成爲標準的2D到2D的運動估計方法，包括對離羣點的處理。對於n>=8的非共麪點的一種簡單直接的計算方法是Longuet-Higgins的8點算法，如論文2中總結。每個特徵匹配都給出一個約束：

8點法得到的這些約束進行排列可以得到一個線性方程組AE=0，通過解方程，就可以計算出E的值。這個齊次方程組可以很容易通過奇異值分解（SVD）計算，如論文2。如果超過8個點會產生超定方程組（方程個數大於未知量個數的方程組），如果超定方程組給定的條件（限制）過於嚴格，導致解不存在，就用最小二乘法進行擬合，求出最小二乘解，該方法對噪聲具有魯棒性。矩陣A的奇異值分解（SVD）形式爲A=USV^T，E的最小二乘估計爲||E||=1可以用V的最後一行查找。然而，E的線性估計並不滿足本徵矩陣的特徵約束，它是旋轉矩陣R和反對稱平移t的乘積。在本徵矩陣的奇異值裏面可以看到這些約束。經過奇異值分解（SVD）分解後的本徵矩陣是E=USV^T，提取主對角線上的值diag(S)={s,s,0}，第一個和第二個奇異值相等，第三個是0。爲了得到滿足約束的有效的本徵矩陣E，需要將它投影到具有有效的本徵矩陣的空間中去。投影本徵矩陣爲。

當3維空間中點共面時，8點算法方案退化。相應的，5點算法則可以應用於計算共麪點。最後，8點算法對校準過的（透視或全景）相機和沒有校準的相機都可適用，5點算法只適用於校準過的（透視或全景）相機。

從本徵矩陣E提取旋轉矩陣R和平移矩陣t

從計算估計出來的中，可以提取旋轉和平移矩陣。通常，對同一本徵矩陣有4種不同的方法求解R，t；其實，只要三角化一個點，就可以求出一對R和t。這4種方法是：

一種將E有效分解成R和t的方法在論文76中有詳細介紹。

通過三角化點雲選擇正確的方案，選擇點的時候這個點要在兩個相機的正前方，用估計的R，t作爲初始值，可以採用非線性優化方法對旋轉和平移參數進行優化。最小化重投影誤差的方程用公式10定義。

捆集調整可用於優化相機軌跡的局部估計。

計算相關尺度

爲了重構圖像序列的軌跡，需要將不同的變換T0:n進行鏈接。爲了達到這個目標，兩幅圖像的平移絕對尺度無法計算，需要計算它們的相對尺度。圖像子集變換的相對尺度是可以計算出來的。其中一種方法是三角化兩個圖像子集的一對圖像集Xk-1和Xk的3維點。從對應的3維點座標可以計算兩個3維點之間的距離。計算圖像對Xk-1和Xk之間的距離的比值r可以得出相應的尺度。

考慮到魯棒性，計算了很多的比例因子，採用平均值，如果出現離羣值就取中值。平移向量t也可以通過這個距離比值計算。相對尺度的計算要求在多個圖像幀上的特徵已經匹配好（或被追蹤），至少是三幅圖像幀。與採用外部三角化3維點不同的是，比例因子還可以通過尋找2維特徵的三視圖匹配之間的三焦點約束來計算，如《多視圖幾何》所示。

2D到2D對應關係的視覺里程計算法如算法1所示。

  

捆集調整可以精煉軌跡的局部估計。

3D到3D：3D結構對應中的運動

對於3D到3D的特徵對應，可以通過兩組3D特徵的一致變換計算相機的運動Tk。3D到3D的特徵對應只適用於立體視覺中。

論文77中，最小化方案採用3組3D到3D非共線對應關係，可以用於出現離羣點的魯棒估計，將在第2部分講解。當n>=3的對應點，可能的方法，論文78，是分別用3D特徵組的不同質心計算平移部分，和用奇異值分解（SVD）計算旋轉部分。平移部分通過下式計算：其中-表示算術平均值。

如果3D點的測量不確定性已知，可以通過論文17的方法添加權重。變換的計算具有絕對尺度，因此，圖像序列的軌跡可以直接連接變換得到。

3D到3D對應關係的視覺里程計算法如算法2所示。

爲了計算變換，也可以不用立體相機3D點的三角化，而採用4焦點約束。論文21講解了這個方法。4焦點張量也可以直接從2D到2D的立體對應關係中計算變換。

3D到2D：3D結構和圖像特徵對應中的運動

Nister的論文1指出，3D到2D對應的運動估計比3D到3D對應的更精確，因爲它的圖像重投影誤差更小（如論文10），而3D到3D的特徵位置誤差估計不是最小，如論文9。通過3D到2D的對應關係Xk-1和pk計算變換Tk。Xk-1可以用立體數據估計，或在單目視覺中，可以三角化圖像量測pk-1和pk-2獲得。然後，再使用三視圖的圖像對應關係。

衆所周知，這個問題是n點透視問題，有很多不同的方法可以用，如論文79。論文18中，最小化方法採用3個3D到2D的對應。稱之爲3點透視（P3P）方法，有4種方案用1個或更多點來消除歧義。（一種優化加速P3P算法如論文80所示，C代碼可以在作者的網站上下載。）在3D到2D的狀況下，P3P是一種用於在離羣點出現的情況下魯棒運動估計的標準方法，如論文18所示。魯棒估計將在第2部分詳細介紹。

一種簡單直接的解決n>=6點的PnP問題的方法是直接線性變換算法，如論文22所示。一個3D到2D點的對應提供了Pk=[R|t]的2個約束，如下所示。

其中，每個PjT都是一個4維向量（j表示PK的第j行），x,y,z是3D點Xk-1的三個座標值。

講6個點的對應關係的約束排列產生一個線性方程組爲AP=0。A中包含0向量，採用SVD奇異值分解，可以計算出P。旋轉和平移部分通過Pk=[R|t]提取。生成的旋轉矩陣R不需要正則化。然而，R和t都可以通過論文10中重投影誤差非線性優化來處理，所以問題不大。

3D到2D的運動估計採用的2D圖像點來自於單目相機。這意味着對立體相機，2D圖像點要麼是左邊的相機要麼是右邊的相機生成。顯然，我們希望同時使用兩個相機的圖像點。Nister在論文81中提出了一種對於外參校準（相機間的相對位置和方向已知）的相機的一般化的3D到2D運動估計算法，可應用於非共點的射線（比如，多相機的2D圖像點）。

對於單目相機，需要三角化3D圖像點，用一種獨特的方法從3D到2D的匹配中估計位姿。這種獨特的算法常常被稱爲SfM。從2個視圖開始，通過2D到2D特徵匹配計算3D點的初始點集和第一個變換。接下來的變換用3D到2D的特徵匹配計算。爲了達成這種做法，特徵的匹配或跟蹤，需要在多個圖像幀中處理（至少是3個圖像幀）。當新變換計算好後，將被加入到3D特徵點集中，新的3D特徵會被三角化。這種方法的挑戰是需要維護一個一致的、精確的三角化的3D特徵，在至少3個連續圖像幀上創建3D到2D的特徵匹配。

3D到2D對應的視覺里程計算法總結如算法3所示。

2D到2D和3D到2D運動估計比3D到3D方法更精確。

三角化和關鍵幀的旋轉

之前有些運動估計方法要求從2D圖像對應中三角化3D點（結構）。捆集調整BundleAdjustment也需要計算運動結構，從而計算出局部軌跡更精確的估計。

從至少2幅圖像幀的2D圖像對應的射線反向交匯，進行3D點三角化。理想情況下，這些射線將會在一個3D點處交匯。然而，由於圖像噪聲，相機模型和標定誤差，還有特徵匹配的不確定性，它們可能不會交匯。因此，在最小二乘情況下，最小距離的點，其所有射線交匯，可以作爲3D點的位置的估計。所有射線交匯的三角化的3D點的距離的標準偏差是一種理想的3D點。具有較大不確定的3維點將會被丟棄。這種情況在圖像幀非常靠近被拍攝的場景時會發生。這種情況發生時，3D點表現出非常大的不確定性。避免這種問題的方法是忽略這些圖像幀，直到這些3D點的不確定性減小到一個設定的閾值。這些挑選的圖像幀被稱爲關鍵幀。關鍵幀的選擇是視覺里程計中非常重要的一步，它是在更新運動之前必須完成。

視覺里程計在一致性和實時性能方面需要做取捨。

==========不忘初心 方得始終 [email protected]===2017年4月20日編輯===EJU in Shanghai===

討論

根據Nister論文1，對於運動的計算，與3D到3D方法相比2D到2D和3D到2D的方法具有優勢。對於立體相機，Nister對比了視覺里程計的性能，3D到3D的和3D到2D的情況，發現3D到2D要比3D到3D好很多。主要原因是三角化的3D點在深度方面具有更多的不確定性。3D到3D的特徵對應用於運動計算，不確定性會減少運動估計的效果。事實上，3D到3D的運動估計，3D位置誤差，論文9，可以在3D到2D圖像重投影誤差下最小化。

在單目算法中，相對3D到2D，更推薦使用2D到2D方法，因爲它避免了點雲的三角化。但實際應用中，3D到2D方法使用的比2D到2D方法多。主要原因是它快速的數據關聯。由於需要更精確的運動估計計算，對於輸入數據排除離羣點非常重要。去除離羣點是非常微秒的一步，它的計算時間與估計運動的最少點數密切相關。如前所述，2D到2D的計算要求最少5點對應關係（參考5點算法）；然而，在3D到2D運動估計中只有3個對應關係（參考P3P）。點數越少，運動估計計算越快。

立體相機對比單目相機的優勢，除了3D特徵可以在絕對尺度下直接計算之外，特徵匹配可以直接從2幅視圖中計算而不是單目相機算法中的3幅視圖。另外，3D結構從一對立體圖像對中計算得出而不是單目相機中的聯立多幅圖像，在小運動中，立體算法比單目算法的漂移更小。單目方法更有意思，因爲立體方法可能會退化成單目，當相機到場景點的距離遠遠大於立體相機間的基線（兩個相機間的距離）時就會出現這種情況。這種情況下，立體相機方法無效，只能使用單目方法。

無論採用哪種運動估計算法，都需要執行局部捆集調整（前m個圖像上）估計更精確的軌跡。捆集調整之後，運動估計的效果就得到非常大的緩解。

結論

本文討論了視覺里程計的歷史，問題的推導，不同的運動估計方法。視覺里程計是機器人上必要的一部分，比較容易理解。第二部分將會總結視覺里程計的其他部分：如何檢測和匹配圖像間顯著的可重複的特徵，出現無效數據後的魯棒估計，和捆集調整。另外，還有誤差迭代，應用，和開源代碼。

參考文獻

[1]D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry,” inProc.Int.Conf. Computer Vision and Pattern Recognition,2004, pp. 652–659.

[2]H. Longuet-Higgins, “Acomputer algorithm for reconstructing ascene from twoprojections,”Nature,vol. 293, no. 10, pp. 133–135,1981.

[3]C. Harris and J. Pike, “3d positional integration from imagesequences,” inProc.Alvey Vision Conf.,1988, pp. 87–90.

[4]J.-M. Frahm, P. Georgel, D. Gallup, T. Johnson, R. Raguram, C.Wu,Y.-H. Jen, E. Dunn, B. Clipp, S. Lazebnik, and M. Pollefeys,“Buildingrome on a cloudless day,” in Proc.European Conf. Computer Vision,2010,pp. 368–381.

[5]H. Moravec, “Obstacle avoidance and navigation in the real world byaseeing robot rover,” Ph.D. dissertation, Stanford Univ., Stanford,CA, 1980.

[6]L. Matthies and S. Shafer, “Error modeling in stereonavigation,”IEEEJ.Robot. Automat.,vol. 3, no. 3, pp. 239–248,1987.

[7]L. Matthies, “Dynamic stereo vision,” Ph.D. dissertation,CarnegieMellonUniv., Pittsburgh, PA, 1989.

[8]S. Lacroix, A. Mallet, R. Chatila, and L. Gallo, “Rover selflocalizationin planetary-like environments,” in Proc.Int. Symp. Articial Intelligence,Robotics, and Automation for Space(i-SAIRAS),1999, pp. 433–440.

[9]C. Olson, L. Matthies, M. Schoppers, and M. W. Maimone, “Robuststereoego-motion for long distance navigation,” in Proc.IEEE Conf.Computer Vision and Pattern Recognition,2000, pp. 453–458.

[10]M. Hannah, “Computer matching of areas in stereo images,”Ph.D.dissertation, Stanford Univ., Stanford, CA, 1974.

[11]H. Moravec, “Towards automatic visual obstacle avoidance,” inProc.5thInt. Joint Conf. Artificial Intelligence,Aug. 1977, p. 584.

[12]W. Forstner, “A feature based correspondence algorithm forimagematching,”Int.Arch. Photogrammetry,vol. 26, no. 3, pp. 150–166,1986.

[13]C. Olson, L. Matthies, M. Schoppers, and M. Maimone, “Rovernavigationusing stereo ego-motion,”Robot.Autonom. Syst.,vol. 43, no. 4,pp. 215–229,2003.

[14]A. Milella and R. Siegwart, “Stereo-based ego-motionestimationusing pixel tracking and iterative closest point,” inProc.IEEE Int. Conf.Vision Systems,pp. 21–24,2006.

[15]A. Howard, “Real-time stereo visual odometry for autonomousgroundvehicles,” inProc.IEEE/RSJ Int. Conf. Intelligent Robots and Systems,2008,pp. 3946–3952.

[16]Y. Cheng, M. W. Maimone, and L. Matthies, “Visual odometry onthemars exploration rovers,”IEEERobot. Automat. Mag.,vol. 13, no. 2,pp. 54–62,2006.

[17]M. Maimone, Y. Cheng, and L. Matthies, “Two years of visualodometryon the mars exploration rovers: Field reports,”J.Field Robot.,vol. 24,no. 3, pp. 169–186,2007.

[18]M. A. Fischler and R. C. Bolles, “Random sample consensus: Aparadigmfor model fitting with applications to image analysis andautomatedcartography,”Commun.ACM,vol. 24, no. 6, pp. 381–395,1981.

[19]C. Tomasi and J. Shi, “Good features to track,” inProc.ComputerVision and Pattern Recognition (CVPR ’94),1994, pp. 593–600.

[20]P. Besl and N.McKay, “Amethod for registration of 3-dshapes,”IEEETrans.Pattern Anal. Machine Intell.,vol. 14, no. 2, pp. 239–256,1992.

[21]A. Comport, E. Malis, and P. Rives, “Accuratequadrifocal trackingfor robust 3d visual odometry,”inProc.IEEE Int. Conf. Robotics andAutomation,2007, pp. 40–45.

[22]R. Hartley and A. Zisserman,MultipleView Geometry in ComputerVision,2nd ed. Cambridge U.K.: Cambridge Univ. Press, 2004.

[23]D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry forgroundvehicle applications,”J.Field Robot.,vol. 23, no. 1, pp. 3–20,2006.

[24]P. I. Corke, D. Strelow, and S. Singh, “Omnidirectional visualodometryfor a planetary rover,” in Proc.IEEE/RSJ Int. Conf. Intelligent Robotsand Systems,2005, pp. 4007–4012.

[25]M. Lhuillier, “Automatic structure and motion using acatadioptriccamera,” inProc.IEEE Workshop Omnidirectional Vision,2005, pp. 1–8.

[26]R. Goecke, A. Asthana, N. Pettersson, and L. Petersson, “Visualvehicleegomotion estimation using the Fourier-Mellin transform,” inProc.IEEEIntelligent Vehicles Symp.,2007, pp. 450–455.

[27]J. Tardif, Y. Pavlidis, and K. Daniilidis, “Monocular visualodometryin urban environments using an omnidirectional camera,” inProc.IEEE/RSJ Int. Conf. Intelligent Robots and Systems,2008, pp. 2531–2538.

[28]M. J. Milford and G.Wyeth, “Single camera vision-only SLAM onasuburban road network,” inProc.IEEE Int. Conf. Robotics and Automation(ICRA ’08),2008, pp. 3684–3689.

[29]D. Scaramuzza and R. Siegwart, “Appearance-guidedmonocularomnidirectional visual odometry for outdoor groundvehicles,”IEEETrans.Robot. (Special Issue on Visual SLAM),vol. 24, no. 5, pp. 1015–1026,Oct. 2008.

[30]E. Mouragnon, M. Lhuillier, M. Dhome, F. Dekeyser, and P. Sayd,“Realtime localization and 3d reconstruction,” in Proc.Int. Conf.Computer Vision and Pattern Recognition,2006, pp. 363–370.

[31]D. Scaramuzza, F. Fraundorfer, and R. Siegwart, “Real-timemonocularvisual odometry for on-road vehicles with 1-point RANSAC,”inProc.IEEE Int. Conf. Robotics and Automation (ICRA ’09),2009,pp. 4293–4299.

[32]A. Pretto, E. Menegatti, and E. Pagello, “Omnidirectionaldenselarge-scale mapping and navigation based on meaningfultriangulation,”in Proc.IEEE Int. Conf. Robotics and Automation,2011, pp. 3289–3296.

[33]D. Nister, “An efficient solution to the five-point relative poseproblem,”inProc.Int. Conf. Computer Vision and Pattern Recognition,2003,pp. 195–202.

[34]M. Milford, G. Wyeth, and D. Prasser, “RatSLAM: A hippocampalmodelfor simultaneous localization and mapping,” in Proc.IEEE Int.Conf. Robotics and Automation (ICRA ’04),2004, pp. 403–408.

[35]B. Liang and N. Pears, “Visual navigation using planarhomographies,”inProc.IEEE Int. Conf. Robotics and Automation (ICRA ’02),2002,pp. 205–210.

[36]Q. Ke and T. Kanade, “Transforming camera geometry to avirtualdownward-looking camera: Robust ego-motion estimation andgroundlayerdetection,” in Proc.Computer Vision and Pattern Recognition(CVPR),June 2003, pp. 390–397.

[37]H. Wang, K. Yuan, W. Zou, and Q. Zhou, “Visual odometry basedonlocally planar ground assumption,” in Proc.IEEE Int. Conf. InformationAcquisition,2005, pp. 59–64.

[38]J. Guerrero, R. Martinez-Cantin, and C. Sagues, “Visualmap-lessnavigation based on homographies,”J.Robot. Syst.,vol. 22, no. 10,pp. 569–581,2005.

[39]D. Scaramuzza, “1-point-RANSAC structure from motion forvehicle-mounted cameras by exploiting non-holonomic constraints,”Int.J.Comput. Vis.,vol. 95, no. 1, pp. 74–85,2011.

[40]D. Scaramuzza, F. Fraundorfer, M. Pollefeys, and R.Siegwart,“Absolute scale in structure from motion from a singlevehicle mountedcamera by exploiting nonholonomic constraints,” inProc.IEEE Int. Conf.Computer Vision (ICCV),Kyoto, Oct. 2009, pp. 1413–1419.

[41]F. Fraundorfer, D. Scaramuzza, and M. Pollefeys, “Aconstrictedbundle adjustment parameterization for relative scaleestimation in visualodometry,” in Proc.IEEE Int. Conf. Robotics and Automation,2010,pp. 1899–1904.

[42]N. Sunderhauf, K. Konolige, S. Lacroix, and P. Protzel,“Visualodometry using sparse bundle adjustment on an autonomousoutdoorvehicle,” in TagungsbandAutonome Mobile Systeme, Reihe Informatikaktuell.Levi, Schanz, Lafrenz, and Avrutin, Eds. Berlin,Springer-Verlag,2005, pp. 157–163.

[43]K. Konolige, M. Agrawal, and J. Sol, “Large scale visual odometryforrough terrain,” inProc.Int. Symp. Robotics Research,2007.

[44]J. Tardif, M. G. M. Laverne, A. Kelly, and M. Laverne, “Anewapproach to vision-aided inertial navigation,” in Proc.IEEE/RSJ Int. Conf.Intelligent Robots and Systems,2010, pp. 4161–4168.

[45]A. I. Mourikis and S. Roumeliotis, “A multi-state constraintkalmanfilter for vision-aided inertial navigation,” in Proc.IEEE Int. Conf. Roboticsand Automation,2007, pp. 3565–3572.

[46]E. Jones and S. Soatto, “Visual-inertial navigation, mappingandlocalization: A scalable real-time causal approach,”Int.J. Robot. Res.,vol.30, no. 4, pp. 407–430,2010.

[47]H. Durrant-Whyte and T. Bailey, “Simultaneouslocalization andmapping (SLAM): Part I. The essentialalgorithms,”Robot.Automat.Mag.,vol. 13, no. 2, pp. 99–110,2006.

[48]T. Bailey and H. Durrant-Whyte, “Simultaneouslocalisation andmapping (SLAM): Part II. State of the art,”Robot.Automat. Mag.,vol. 13,no. 3, pp. 108–117,2006.

[49]A. Davison, “Real-timesimultaneous localisation and mapping witha single camera,”inProc.Int. Conf. Computer Vision,2003, pp. 1403–1410.

[50]G. Klein and D. Murray, “Paralleltracking and mapping for small arworkspaces,”inProc.Int. Symp. Mixed and Augmented Reality,2007,pp. 225–234.

[51]H. Strasdat, J. Montiel, and A. Davison, “Realtime monocularSLAM: Why filter?”inProc.IEEE Int. Conf. Robotics and Automation,2010,pp. 2657–2664.

[52]A. Chiuso, P. Favaro, H. Jin, and S. Soatto, “3-D motion andstructurefrom 2-D motion causally integrated over time:Implementation,” inProc.European Conf. Computer Vision,2000, pp. 734–750.

[53]M. C. Deans, “Bearing-only localization and mapping,” Ph.D.dissertation,Carnegie Mellon Univ., Pittsburgh, 2002.

[54]L. A. Clemente, A. J. Davison, I. Reid, J. Neira, and J. D.Tardos,“Mapping large loops with a single hand-held camera,” inProc.RoboticsScience and Systems,2007.

[55]T. Lemaire and S. Lacroix, “Vision-based SLAM: Stereo andmonocularapproaches,”Int.J. Computer Vision,vol. 74, no. 3, pp. 343–364,2006.

[56]E. Eade and T. Drummond, “Monocular SLAM as a graph ofcoalescedobservations,” inProc.IEEE Int. Conf. Computer Vision,2007,pp. 1–8.

[57]G. Klein and D. Murray, “Improving the agility ofkeyframe-basedSLAM,” inProc.European Conf. Computer Vision,2008, pp. 802–815.

[58]K. Konolige and M. Agrawal, “FrameSLAM:From bundle adjustmentto real-time visual mappping,”IEEETrans. Robot.,vol. 24, no. 5,pp. 1066–1077,2008.

[59]A. Handa, M. Chli, H. Strasdat, and A. J. Davison, “Scalableactivematching,” inProc.Int. Conf. Computer Vision and Pattern Recognition,2010,pp. 1546–1553.

[60]J. Civera, O. Grasa, A. Davison, and J. Montiel, “1-point RANSACforekf filtering: Application to real-time structure from motionandvisual odometry,”J.Field Robot.,vol. 27, no. 5, pp. 609–631,2010.

[61]H. Strasdat, J. Montiel, and A. J. Davison, “Scale drift-awarelargescale monocular SLAM,” inProc.Robotics Science and Systems,2010.

[62]C. Mei, G. Sibley, M. Cummins, P. Newman, and I. Reid, “RSLAM:Asystem for large-scale mapping in constant-time using stereo,”Int.J.Computer Vision,vol. 94, no. 2, pp. 198–214,2010.

[63]Y. Ma, S. Soatto, J. Kosecka, and S. Sastry,AnInvitation to 3DVision, from Images to Models.Berlin: Springer-Verlag, 2003.

[64]C. Geyer and K. Daniilidis, “A unifying theory for centralpanoramicsystems and practical applications,” in Proc.European Conf. ComputerVision,2000, pp. 445–461.

[65]D. Scaramuzza, A. Martinelli, and R. Siegwart, “A flexibletechniquefor accurate omnidirectional camera calibration andstructure frommotion,” in Proc.IEEE Int. Conf. Computer Vision Systems (ICVS) 2006,Jan.2006, pp. 45–53.

[66]D. Scaramuzza, “Omnidirectional vision: From calibration torobotmotion estimation” Ph.D. dissertation, ETH Zurich, 2008.

[67]D. Scaramuzza, “Omnidirectional camera,” inEncyclopediaof ComputerVision,K. Ikeuchi, Ed. Berlin: Springer-Verlag, 2012.

[68]J. Bouguet (2011). List of camera calibration toolboxes.[Online].Available:http://www.vision.caltech.edu/bouguetj/calib.doc/htmls/links.html

[69]J. Bouguet. Camera calibration toolbox for Matlab.[Online].Available:http://www.vision.caltech.edu/bouguetj/calib.doc/index.html

[70]D. Scaramuzza. (2006). Ocamcalib toolbox: Omnidirectionalcameracalibration toolbox for Matlab (uses planar grids) google for“ocamcalib.”[Online].Available: https://sites.google.com/site/scarabotix/ocamcalibtoolbox

[71]C. Mei. (2006). Omnidirectional camera calibration toolbox forMatlab(uses planar grids). [Online]. Available:http://homepages.laas.fr/~cmei/index.php/Toolbox

[72]J. Barreto. Omnidirectional camera calibration toolbox forMatlab(uses lines). [Online]. Available:http://www.isr.uc.pt/~jpbar/CatPack/pag1.htm

[73]Opencv: Open-source computer vision library. [Online].Available:http://opencv.willowgarage.com/wiki/

[74]T. Huang and A. Netravalli, “Motionand structure from feature correspondences: A review,”Proc.IEEE,vol. 82, no. 2, pp. 252–268,1994.

[75]E. Kruppa, “Zur ermittlung eines objektes aus zwei perspektivenmitinnerer orientierung,”Sitzungsberichteder Akademie der Wissenschaften,Wien,Mathematisch-Naturwissenschaftlichen Klasse, Abteilung IIa,vol.122, pp. 1939–1948,1913.

[76]D. Nister, “Anefficient solution to the five-point relative pose problem,”inProc.Computer Vision and Pattern Recognition (CVPR ’03),2003,pp. II: 195–202.

[77]H. Goldstein,ClassicalMechanics.Reading, MA:Addison-Wesley, 1981.

[78]K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fittingoftwo 3-d point sets,”IEEETrans. Pattern Anal. Machine Intell.,vol. 9, no. 5,pp. 698–700,1987.

[79]F. Moreno-Noguer, V. Lepetit, and P. Fua, “Accuratenon-iterativeO(n) solution to the PnP problem,”inProc.IEEE Int. Conf. ComputerVision,2007, pp. 1–8.

[80]L. Kneip, D. Scaramuzza, and R. Siegwart, “A novelparameterizationof the perspective-three-point problem for a directcomputation of absolutecamera position and orientation,” in Proc.IEEE Conf. ComputerVision and Pattern Recognition,2011, pp. 2969–2976.

[81]D. Nister, “A minimal solution to the generalised 3-point poseproblem,”inProc.IEEE Conf. Computer Vision and Pattern Recognition,2004,pp. 560–567.

[82]C. Harris and M. Stephens, “Acombined corner and edge detector,”inProc.Alvey Vision Conf.,1988, pp. 147–151.