相機位姿求解 3種方法

阿新 • • 發佈：2018-12-09

目前在做通過雙目攝像頭獲得R t的部分，通過閱讀《Multiple View Geometry in Computer Vision II》瞭解到有三種形式：

1. 2D-2D: 特徵點在同一平面或近平面時，通過Homograpy Matrix可以恢復相機位姿；當特徵點的深度明顯不同時則需要利用對極幾何，通過求得Fundamental Matrix或者Essential Matrix來恢復相機位姿。

2.2D-3D: 通過已知的三維點，以及這些點與給定特徵點的關係，恢復相機位姿，常見的是PnP。

3.3D-3D: 使用ICP對前後兩幀各自的三維點進行匹配，得到兩幀間的相對位姿。

1. 特徵點法

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

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

你用的是單目相機，於是只有2D-2D的配對點；
你用的是RGBD或雙目相機，於是你有3D-3D的配對點；
你只知道一張圖中3D資訊，另一張圖只有2D資訊，於是有3D-2D的配對點。

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

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

$d_1 {p_1} = KP,d_2 p_2 = K\left( {RP + t} \right)$ (1) 這裡 $d_1,d_2$ 是兩個點的距離， $p_1,p_2$ 要取齊次座標， $P$ 取非齊次座標。又說，既然左邊都取齊次了，乾脆齊次到底吧，於是去掉那倆深度：

${p_1} = KP, p_2 = K\left( {RP + t} \right)$ (2)

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

${x_1} = {K^{ - 1}}{p_1},{x_2} = {K^{ - 1}}{p_2}$ (3)

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

${x_2} = R{x_1} + t$ (4)

就這樣。所以相機位姿估計問題變為：你有很多個 $x_1,x_2$

，怎麼去算這裡的 $R,t$ ？

------------------------------------------------

1.1 2D-2D，分解E和F的情況：

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

$t \times {x_2} = t \times R{x_1} + t \times t = t \times R{x_1}$ (5)

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

$x_2^T\left( {t \times {x_2}} \right) = 0 = x_2^Tt \times R{x_1}$ (6)

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

$x_2^Tt \times R{x_1} =0$ (7)

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

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

$E = t \times R$ (8)

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

$x_2^TE{x_1} = 0$ (9)

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

用一坨配對點算E；
用E算R,t

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

從配對點算E：

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

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

$\begin{pmatrix} u_{1},v_{1},1 \end{pmatrix} \begin{pmatrix} e_{1} & e_{2} & e_{3}\\ e_{4} & e_{5} & e_{6}\\ e_{7} & e_{8} & e_{9} \end{pmatrix} \begin{pmatrix} u_{2}\\v_{2}\\1 \end{pmatrix} =0.$ (10)

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

$[u_{1}u_{2},u_{1}v_{2},u_{1},v_{1}u_{2},v_{1}v_{2},u_{2},v_{2},1] \cdot \bm{e}=0.$ (11)

這裡：

$\bm{e}= [e_{1},e_{2},e_{3},e_{4},e_{5},e_{6},e_{7},e_{8},e_{9}]^{T}$ (12)

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

$\begin{pmatrix} u_{1}^{1}u_{2}^{1}& u_{1}^{1}v_{2}^{1}& u_{1}^{1}& v_{1}^{1}u_{2}^{1}& v_{1}^{1}v_{2}^{1}& v_{1}^{1} &u_{2}^{1} &v_{2}^{1}&1\\ u_{1}^{2}u_{2}^{2}& u_{1}^{2}v_{2}^{2}& u_{1}^{2}& v_{1}^{2}u_{2}^{2}& v_{1}^{2}v_{2}^{2}& v_{1}^{2} &u_{2}^{2} &v_{2}^{2}&1\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ u_{1}^{8}u_{2}^{8}& u_{1}^{8}v_{2}^{8}& u_{1}^{8}& v_{1}^{8}u_{2}^{8}& v_{1}^{8}v_{2}^{8}& v_{1}^{8} &u_{2}^{8}&v_{2}^{8}&1\\ \end{pmatrix} \begin{pmatrix} e_{1}\\ e_{2}\\ e_{3}\\ e_{4}\\ e_{5}\\ e_{6}\\ e_{7}\\ e_{8}\\ e_{9} \end{pmatrix} =0.$ (13)

然後就是愛怎麼解就怎麼解了。可逆時求逆，不可逆時求偽逆和最小二乘解，矩陣論裡都有，不說了。這個方法最少用八對點，所以叫什麼？對，八點法（你倒是取個好聽點的名字啊喂）。

然後就是用E算R,t的問題了。

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

$\bm{E} = \bm{U} \bm{\Sigma} \bm{V}^T$ (14)

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

$\begin{array}{l} \bm{t}_1^ \wedge = \bm{U}{\bm{R}_Z}(\frac{\pi }{2}) \bm{\Sigma} {\bm{U}^T}, \quad {\bm{R}_1} = \bm{U} \bm{R}_Z^T(\frac{\pi }{2}){ \bm{V}^T}\\ \bm{t}_2^ \wedge = \bm{U}{\bm{R}_Z}( - \frac{\pi }{2})\bm{\Sigma} {\bm{U}^T}, \quad {\bm{R}_2} = \bm{U} \bm{R}_Z^T( - \frac{\pi }{2}){\bm{V}^T}. \end{array}$ (15)

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

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

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

$\bm{p}_2^T \bm{K}^{-T} \bm{t} \times \bm{R} \bm{K}^{-1} \bm{p}_1 = 0.$ (16)

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

------------------------------------------------

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

這圖來自wikipedia.

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

$\bm{n}^T \bm{P} + d = 0.$ (17)

然後對兩個點，有：

$\begin{align*} \bm{p}_2 &= \bm{K} ( \bm{R} \bm{P} + \bm{t} ) \\ &= \bm{K} \left( \bm{R} \bm{P} + \bm{t} \cdot (- \frac{\bm{n}^T \bm{P} }{d}) \right) \\ &= \bm{K} \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{P} \\ &= \bm{K} \left( \bm{R} - \frac{\bm{t} \bm{n}^T }{d} \right) \bm{K}^{-1} \bm{p}_1. \end{align*}$ (18)

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

$\bm{p}_2 = \bm{H} \bm{p}_1.$ (19)

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

怎麼用給定的一堆匹配點算H；
怎麼用H算出R,t,n,d

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

------------------------------------------------

1.2 2D-2D，討論

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

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

1.3 3D-3D，ICP：

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

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

$\bm{P} = \{ \bm{p}_1, \ldots, \bm{p}_n \}, \quad \bm{P}' = \{ \bm{p}_1', \ldots, \bm{p}_n'\}$

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

$\forall i, \bm{p}_i = \bm{R} \bm{p}_i' + \bm{t}.$

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

$\bm{e}_i = \bm{p}_i - (\bm{R} \bm{p}_i' + \bm{t} ) .$

最小化它：

$\mathop {\min }\limits_{\bm{R}, \bm{t}} J = \frac{1}{2} \sum\limits_{i = 1}^n\| {\left( {{\bm{p}_i} - \left( {\bm{R}{\bm{p}_i}' + \bm{t}} \right)} \right)} \|^2_2.$

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

$\bm{p} = \frac{1}{n}\sum_i^n ( \bm{p}_i ), \quad \bm{p}' = \frac{1}{n} \sum_i^n ( \bm{p}_i' ).$

然後處理一下目標函式：

$\begin{align*} \begin{array}{ll} \frac{1}{2}\sum\limits_{i = 1}^n {{{\left\| {{\bm{p}_i} - \left( {\bm{R}{ \bm{p}_i}' + \bm{t}} \right)} \right\|}^2}} & = \frac{1}{2}\sum\limits_{i = 1}^n {{{\left\| {{\bm{p}_i} - \bm{R}{\bm{p}_i}' - \bm{t} - \bm{p} + \bm{Rp}' + \bm{p} - \bm{Rp}'} \right\|}^2}} \\ & = \frac{1}{2}\sum\limits_{i = 1}^n {{{\left\| {\left( {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right) + \left( {\bm{p} - \bm{Rp}' - \bm{t}} \right)} \right\|}^2}} \\ & = \frac{1}{2}\sum\limits_{i = 1}^n {{\left\| {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right\|}^2} + {{\left\| {\bm{p} - \bm{Rp}' - \bm{t}} \right\|}^2} +\\ & \quad \quad 2{{\left( {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right)}^T}\left( {\bm{p} - \bm{Rp}' - \bm{t}} \right). \end{array} \end{align*}$

這裡的技巧無非是先加一項再減一項而已。注意到交叉項部分中， $\left( {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right)$ 在求和之後是為零的，因此優化目標函式可以簡化為：

$\mathop {\min }\limits_{\bm{R}, \bm{t}} J = \frac{1}{2}\sum\limits_{i = 1}^n {{\left\| {{\bm{p}_i} - \bm{p} - \bm{R}\left( {{\bm{p}_i}' - \bm{p}'} \right)} \right\|}^2} + {{\left\| {\bm{p} - \bm{Rp}' - \bm{t}} \right\|}^2} .$

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

計算兩組點的質心；
計算去質心座標： $\bm{q}_i = \bm{p}_i - \bm{p}, \quad \bm{q}_i' = \bm{p}_i' - \bm{p}'.$
求解旋轉R；
根據旋轉和質心解t： $\bm{t}^* = \bm{p} - \bm{R} \bm{p}'.$

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

$\frac{1}{2}\sum\limits_{i = 1}^n \left\| {{\bm{q}_i} - \bm{R} \bm{q}_i' } \right\|^2 = \frac{1}{2}\sum\limits_{i = 1}^n \bm{q}_i^T \bm{q}_i + \bm{q}_i^{ \prime T} \bm{R}^T \bm{R} \bm{q}^\prime_i - 2\bm{q}_i^T \bm{R} \bm{q}^\prime_i.$

注意到第一項和R無關，第二項由於 $\bm{R}^T\bm{R}=\bm{I}$ ，亦與R無關。因此，實際上優化目標函式變為：

$\sum\limits_{i = 1}^n - \bm{q}_i^T \bm{R} \bm{q}^\prime_i = \sum\limits_{i = 1}^n -\mathrm{tr} \left( \bm{R} \bm{q}_i^{\prime} \bm{q}_i^T \right) = - \mathrm{tr} \left( \bm{R} \sum\limits_{i = 1}^n \bm{q}_i^{\prime} \bm{q}^{ T}_i \ \right).$

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

$\bm{W} = \sum\limits_{i = 1}^n \bm{q}_i \bm{q}^{\prime T}_i.$

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

$\bm{R} = \bm{U} \bm{V}^T.$

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

------------------------------------------------

1.4 3D-2D，PnP

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

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

$\bm{P}_i=[X_i,Y_i,Z_i]^T$

然後你又知道了投影：

$d_i \bm{p}_i = \bm{K} (\bm{RP}_i + \bm{t})$

於是算一個誤差：

$\bm{e}_i = \bm{p}_i - \frac{1}{d_i} \bm{K} (\bm{RP}_i+\bm{t})$

然後讓它們最小化：

${\bm{T} ^*} = \arg \mathop {\min }\limits_{\bm{T}} \frac{1}{2}\sum\limits_{i = 1}^n {\left\| {{{\bm{p}}_i} - \frac{1}{s_i} \bm{K} (\bm{R}{{\bm{P}}_i}}+\bm{t}) \right\|_2^2} .$

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

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

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

好了，特徵點法就說到這裡。直接法的話……有點更不動，可以參考我講直接法的部落格：直接法 - 半閒居士 - 部落格園

就這樣，躹躬。

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

相機位姿求解 3種方法

相機位姿求解 3種方法

相機位姿估計3：根據兩幅影象的位姿估計結果求某點的世界座標_0

相機位姿問題的特徵點法求解

hibernate update 只更新部分欄位的3種方法(轉載)

3種方法求解斐波那契數列

hibernate update 只更新部分欄位的3種方法

相機位姿問題的特徵點法求解——高翔（轉載自泡泡機器人）

Mysql字串欄位判斷是否包含某個字串的3種方法

Python：字符串中引用外部變量的3種方法

閉包的3種方法以及如何從外部讀取局部變量

scala中跳出循環的3種方法

jQuery判斷checkbox是否選中的3種方法

用jQuery來綁定事件的3種方法和區別

python 修改文件內容3種方法

mysql分表的3種方法

c++中字符串反轉的3種方法

hibernate update 只更新部分字段的3種方法（其實我只想說第二種）

黑客教父郭盛華：3種方法可以入侵你的汽車

Java中獲取類名的3種方法！

頁面內跳轉到相應位置的3種方法

相機位姿求解 3種方法

相關推薦