（點開，進入微信，重新整理下就可以看見了）

歡迎大家在週日來到泡泡機器人講堂，本次我們將為大家介紹相機位姿問題的求解，相機位姿估計是指給定若干影象，估計其中相機運動的問題。求解方法通常分特徵點法和直接法兩種，這也是視覺里程計的兩類基本方法。本次主要為大家講解特徵點法。

特徵點法的思路，是先從影象當中提取許多特徵，然後在影象間進行特徵匹配，這樣就得到許多匹配好的點，再根據這些點進行相機位姿的求解。相機位姿求解部分則和影象本身關係不大了。比方說下圖是ORB特徵匹配的結果。

特徵匹配之後，你得到了一組配對點，以及它們的畫素座標。剩下的問題是說，怎麼用這組配對點去計算相機的運動。這裡，根據感測器形式的不同，分成三種情況：

• 你用的是單目相機，於是只有2D-2D的配對點；

• 你用的是RGBD或雙目相機，於是你有3D-3D的配對點；

• 你只知道一張圖中3D資訊，另一張圖只有2D資訊，於是有3D-2D的配對點。
  

第三種情況可能出現在單目SLAM中，你通過之前的資訊計算了3D的地圖點，現在又來了一張2D影象，所以會有3D-2D的情況。或者，在RGBD和雙目中，你也可以忽略其中一張圖的3D資訊，使用3D-2D的配對點。 無論如何，這三種情況都是現實存在的，所以處理方式也分為三種。

為保持行文清楚，先來把變數設一下。假設某個左邊的特徵點叫，右邊配對的點叫，它們都以畫素座標給出。同時，以左邊圖為參考系，設右邊圖相對它的運動由旋轉和平移描述，相機內參為。由於這兩個點肯定是同一個空間點P的投影，那麼顯然:

(1)

這裡是兩個點的距離，要取齊次座標，取非齊次座標。又說，既然左邊都取齊次了，乾脆齊次到底吧，於是去掉那倆深度：

  (2)

該等式在齊次意義下成立，也就是說乘以任意非零常數仍然是等的。不懂的同學請去學習小孔成像原理。我們覺得右邊的內參挺煩人的，於是記

(3)

這倆貨叫歸一化相機座標，也就是去掉內參之後的東西，剩下的就簡單了：

  (4)

就這樣。所以相機位姿估計問題變為：你有很多個
，怎麼去算這裡的？

1.1.分解E和F的情況

在2D-2D情況下，你只有兩個點的2D座標，這種情況出現在單目SLAM的初始化過程中。這時我們只有一個 (4) 式，還是乘任意常數都成立的操蛋情形。沒辦法，兩邊叉乘吧：

(5)

這東西右邊的兩個，自己叉乘自己就沒了。然後，再同時兩邊左乘一個：

(6)

發現左邊的乘了一個和自己垂直的東西，當然是零了，於是就只剩下：

  (7)

一個東西等於零，看起來很帥哦。這個牛逼的玩意叫做對極約束（Epipolar Constraint）。簡而言之，隨便你出一組匹配點，都會有這麼個約束成立。

對極約束這東西在幾何上的意思就是這仨貨的混合積為零（從第二個影象角度來看），所以它們是共面的向量。既然兩個匹配點是同一個點的投影，那不共面還能上天麼？當然是共面的了。於是，為了好看，又把中間那倆定義成一個：

 (8)

這個E叫做Essential（本質）矩陣（別問我為什麼叫Essential，就是這麼叫的）。所以（7）變為：

  (9)

這個約束只有E，但我們的目標是求呀，於是求解變成了兩步：

• 用一坨配對點算E；

• 用E算R,t

不妨再說說這兩步怎麼算。

1.1.1.從配對點算E

最簡單的方式是把E看成單純的一個數值矩陣，忽略它裡面各元素的內在聯絡。當然這麼做的時候你實際要清楚E是有內在性質的，我就直接告訴你這貨的奇異值是一個零加倆一樣的數就完了，證明不寫了。E由t和R的叉積組成，t是仨自由度，R是仨自由度，一共六個。又由於等式為零這樣的約束，乘任意非零常數都成立，也就是對E隨便乘個數，對極約束還是成立的，所以自由度減一，一共五個。因為E有五個自由度，所以最少拿五對匹配點可以把它算出來，這個乃是“五點法”。

又，五點法用了E的奇異值這種奇怪的性質，對E引入了非線性約束，解起來麻煩。所以另一個法子是把E看作數值矩陣，然後解它每一個元素就行了唄。E一個九個數，去掉一個非零常數的因子，還剩八個自由度，所以最少拿八對匹配點就可以算出E，粗暴地把E拉成長條即可。比方說對極約束展開後是這樣的：

 (10)

把E拉成一個向量扔到右邊：

  (11)

這裡：

  (12)

簡單吧，現在你搞出了一個線性方程。當你有八對點時，就變成了方程組，磊起來是這樣的：

  (13)

然後就是愛怎麼解就怎麼解了。可逆時求逆，不可逆時求偽逆和最小二乘解，矩陣論裡都有，不說了。這個方法最少用八對點，所以叫什麼？對，八點法。

1.1.2.用E算R,t

這個推導也沒啥好說的，直接給答案吧，推起來太麻煩。先把E給奇異值分解了：

 (14)

完了之後這麼一算就得到了R,t：

 (15)

這裡對任意一個t加個相反號，對極約束仍然滿足，所以你會得到四個解。這四個解畫出來是這樣的：

怎麼看這個圖呢？兩個小紅點是我們找的配對點，它們都是P的投影。你會看到這四個解裡小紅點的位置都是不變的。那麼哪種情況是真實的呢？廢話，當然是第一種。因為只有第一種情況裡，P出現在相機的前面。什麼？你的相機還能看到身後的東西？你確實不是在逗我？

於是，在驗證之後，就能確定唯一的解了。另外再囉嗦一句，當你不知道內參時，只有畫素座標，那麼對極約束為：

  (16)

這時中間那貨叫做F（Fundamental，基本矩陣），和E大同小異但是性質比E麻煩點。因為SLAM裡通常認為相機已經標定好了所以也用不著它了。

 1.2.分解H的情況

另一種情況是你找的那些點都位於一個平面上，比如說你的相機是朝天花板或地板看的，這時候分解E和F會出現退化，要用另一種方式來解。

這圖來自wikipedia.

你們不是在平面上嗎？來啊，我們就把平面搞出來。平面方程為：

  (17)

然後對兩個點，有：

  (18)

這個式子的好處是直接推出了兩個座標之間的關係。把中間那坨東西記為
（Homography，單應矩陣），於是：

  (19)

這貨也沒啥大不了的。和之前一樣，問題變為：

• 怎麼用給定的一堆匹配點算H；

• 怎麼用H算出R,t,n,d

講起來又是一堆麻煩事。總之第一步比較容易，把H拉成一長條扔到一邊求個線性方程組就行了；第二步比較麻煩，要用到SVD和QR分解。最後你會得到八組解，然後有一串步驟告訴你如何從這八組解裡選出最好的。步驟實在是比較長我就懶得寫了。總之你要知道，在特徵點位於平面上時，分解H；否則分解E或F。就這樣.

 1.3.討論

稍微說幾句。2D-2D的情況出現在單目SLAM的初始化中，你沒有別的資訊，只能這樣子做。其中，分解E或F的過程中存在幾個問題。E這個東西具有尺度等價性，隨便乘個數仍是同一個。所以拿它分解得到的R,t也有一個尺度等價性，特別是t上有一個因子，而自身具有約束，沒有關係。換言之，在分解過程中，對乘以任意非零常數，分解都是成立的，這個叫做單目SLAM的尺度不確定性。因此，我們通常把t進行歸一化，讓它的長度等於1。或者讓場景中特徵點的平均深度等於1，總之是有個比例的。

此外，分解E的過程中，如果相機發生的是純旋轉，導致t為零，那麼，得到的E也將為零。於是，另一個結論是，單目初始化不能只有純旋轉，必須要有一定程度的平移！必須要有一定程度的平移！必須要有一定程度的平移！

程度的平移！

（手持單目時不能原地旋轉，必須像結印那樣有平移）

2.已知3D-3D配對點，求解位姿

下面來討論簡單點的情況：你不光得到了匹配點，還知道這兩組匹配點的深度，於是有了3D-3D的匹配。因為你知道匹配，這種情況下 R,t 的估計是有解析解（閉式解）的。否則，如果只有兩堆點而不知道匹配，則要用迭代最近點（Iterative Closest Point, ICP）求解。閉式解可以稍加推導，不喜歡看推導的同學可以跳過。

假定你找的兩組點是這樣的：

配對好之後，每個點滿足關係：

一開始不知道R,t，所以算一個誤差再求它最小化。誤差為：

最小化它：

簡單吧。這裡可以用一個技巧，先把兩組點的質心設出來，記住不帶下標的是質心：

然後處理一下目標函式：

這裡的技巧無非是先加一項再減一項而已。注意到交叉項部分中，在求和之後是為零的，因此優化目標函式可以簡化為：

嘛，這兩項裡，左邊只和旋轉矩陣R相關，而右邊既有R也有t，但只和質心相關。因此，只要我們獲得了R，令第二項為零就能得到t。於是，ICP可以分為以下幾個步驟求解：

• 計算兩組點的質心；

• 計算去質心座標：
  

• 求解旋轉R；

• 根據旋轉和質心解t：
  

t很簡單，問題是R怎麼解？這東西的平方誤差展開為：

注意到第一項和R無關，第二項由於，亦與R無關。因此，實際上優化目標函式變為：

這個優化問題的解法見文獻[1]，這裡只給結果。首先定義：

對W進行SVD分解，然後令：

於是就得到了旋轉。

總之就是有閉式解，很簡單，因為有匹配。在不知道匹配的時候，情況比較麻煩，通常你要假設最近點是配對點，所以叫迭代最近點。但是既然我在講特徵點法，匹配就是知道的，什麼迭代最近見鬼去吧。

3.已知3D-2D配對點，求解位姿

PnP（Perspective n Points）就是你有n個點的3D位置和它們的投影，然後要算相機的位姿。這個倒是SLAM裡最常見的情況，因為你會有一堆的地圖點和畫素點等著你算。PnP的做法有好多種：直接線性變換，P3P，EPnP，UPnP等等，基本你去找OpenCV的SolvePnP中的引數即可，好用的都在那裡。除此之外，通常認為線性方程解PnP，在有噪聲的情況下表現不佳，所以一般以EPnP之類的解為初始值，構建一個Bundle Adjustment（BA）去做優化。上面那堆演算法題主自己查文獻比較好，有大量的實現細節。當然你也可以完全不鳥他們，直接調cv的函式，反正人家早實現好
了……

扯到BA不妨多說幾句，BA其實蠻容易理解的，只是名字聽上去不那麼直觀。首先，你有3D點：

然後你又知道了投影：

於是算一個誤差：

然後讓它們最小化：

就行了。這就叫最小化重投影誤差，也叫BA。當然實際算的時候，由於R,t自身帶有約束，所以要轉到李代數上算，這裡不展開。

直觀的解釋如上圖。我們通過特徵匹配，知道了和是同一個空間點P的投影，但是不知道相機的位姿。在初始值中，P的投影與實際的之間有一定的距離。於是我們調整相機的位姿，使得這個距離變小。不過，由於這個調整需要考慮很多個點，所以最後每個點的誤差通常都不會精確為零。總之，我們就寄希望於這個誤差會越調越小了。為什麼越調越小呢？因為我們往往會沿著負梯度方向去調唄。當然解釋起來又得涉及一些非線性優化的東西，什麼高斯牛頓之類的，請查非線性優化教材。

BA是萬金油，你看哪個問題不爽就把它扔到優化目標裡，然後讓計算機幫你優化就行。當然這東西非凸的時候要當心初值，否則一不小心就掉在區域性坑裡爬不出來……。

參考文獻： 

[1] Arun, K Somani and Huang, Thomas S and Blostein, Steven D, Least-squares fitting of two 3-D point sets, PAMI, 1987.