相機位姿求解問題？

12-28

我目前知道的求解方法是3種:1)假如是平面, 可通過單應性變換求解R、t；2）通過求基本矩陣，得到本質矩陣，進而求R、t; 3）PnP求解R、t。我想弄清楚2個問題：1)還有別的方法求相機位姿？ 2）基本矩陣、PnP求解的區別？謝了

謝邀。我就是喜歡這種綜述類的問題，哈哈。
相機位姿估計是指給定若干圖像，估計其中相機運動的問題。求解方法通常分特徵點法和直接法兩種，這也是視覺里程計的兩類基本方法。分別講一下特徵點和直接法吧。
------------------------------------------------
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 = Kleft( {RP + t} ight)$ (1)
這裡 $d_1,d_2$ 是兩個點的距離， $p_1,p_2$ 要取齊次坐標， $P$ 取非齊次坐標。又說，既然左邊都取齊次了，乾脆齊次到底吧，於是去掉那倆深度：

${p_1} = KP, p_2 = Kleft( {RP + t} ight)$ (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 imes {x_2} = t imes R{x_1} + t imes t = t imes R{x_1}]$ (5)

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

$[x_2^Tleft( {t imes {x_2}} ight) = 0 = x_2^Tt imes R{x_1}]$ (6)

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

$[x_2^Tt imes R{x_1} =0]$ (7)

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

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

$[E = t imes 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拉成長條即可。比方說對極約束展開後是這樣的：

$egin{pmatrix} u_{1},v_{1},1 end{pmatrix} egin{pmatrix} e_{1} e_{2} e_{3}\ e_{4} e_{5} e_{6}\ e_{7} e_{8} e_{9} end{pmatrix} egin{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 m{e}=0.$ (11)

這裡：

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

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

$egin{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} egin{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給奇異值分解了：

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

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

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

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

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

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

$m{p}_2^T m{K}^{-T} m{t} imes m{R} m{K}^{-1} m{p}_1 = 0.$ (16)

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

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

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

這圖來自wikipedia.

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

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

然後對兩個點，有：

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

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

$m{p}_2 = m{H} m{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上有一個因子，而 $m{R} in SO(3)$ 自身具有約束，沒有關係。換言之，在分解過程中，對 $m{t}$ 乘以任意非零常數，分解都是成立的，這個叫做單目SLAM的尺度不確定性。因此，我們通常把t進行歸一化，讓它的長度等於1。或者讓場景中特徵點的平均深度等於1，總之是有個比♂例的。

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

卡卡西老師教你初始化.jpg
（手持單目時不能原地旋轉，必須像結印那樣有平移）

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

1.3 3D-3D，ICP：

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

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

$m{P} = { m{p}_1, ldots, m{p}_n }, quad m{P}$

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

$forall i, m{p}_i = m{R} m{p}_i$

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

$m{e}_i = m{p}_i - (m{R} m{p}_i$

最小化它：

$mathop {min }limits_{m{R}, m{t}} J = frac{1}{2} sumlimits_{i = 1}^n| {left( {{m{p}_i} - left( {m{R}{m{p}_i}$

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

$m{p} = frac{1}{n}sum_i^n ( m{p}_i ), quad m{p}$

然後處理一下目標函數：

$egin{align*} egin{array}{ll} frac{1}{2}sumlimits_{i = 1}^n {{{left| {{m{p}_i} - left( {m{R}{ m{p}_i}$

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

$mathop {min }limits_{m{R}, m{t}} J = frac{1}{2}sumlimits_{i = 1}^n {{left| {{m{p}_i} - m{p} - m{R}left( {{m{p}_i}$

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

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

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

$frac{1}{2}sumlimits_{i = 1}^n left| {{m{q}_i} - m{R} m{q}_i$

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

$sumlimits_{i = 1}^n - m{q}_i^T m{R} m{q}^prime_i = sumlimits_{i = 1}^n -mathrm{tr} left( m{R} m{q}_i^{prime} m{q}_i^T ight) = - mathrm{tr} left( m{R} sumlimits_{i = 1}^n m{q}_i^{prime} m{q}^{ T}_i ight).$

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

$m{W} = sumlimits_{i = 1}^n m{q}_i m{q}^{prime T}_i.$

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

$m{R} = m{U} m{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點：

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

然後你又知道了投影：

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

於是算一個誤差：

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

然後讓它們最小化：

${m{T} ^*} = arg mathop {min }limits_{m{T}} frac{1}{2}sumlimits_{i = 1}^n {left| {{{m{p}}_i} - frac{1}{s_i} m{K} (m{R}{{m{P}}_i}}+m{t}) ight|_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.

應該還有吧，你說的三種方法應該都算間接法吧(Indirect method), 先找匹配的特徵點(2d-2d或3d-2d或3d-3d)，然後根據特徵點對的位置求解位姿。前兩種是2d-2d, PnP應該是3d-2d，除此之外還是3d-3d(比如ICP, ICP需要迭代更新點匹配)。間接法一般假設點匹配已知，不需要位姿初值，通常有closed-form解。得到closed-form解後當然也可以進行進一步的非線性優化。

如果是直接法，則不需要找對應的特徵點，直接優化位姿，但需要給定位姿初值。常見的例子包括直接法算單應，或直接法算R，T。直接法的data assoication是跟位姿相關的，一般利用當前的R，T值predict匹配的特徵點，然後優化對應的cost function。可以認為在直接法中我們同時在優化位姿和data association。這點不同於間接法，間接法的data association是固定不變的。

比如KinectFusion利用深度圖進行tracking的時候, 採用的是projective data association, 根據當前的R，T估計值將點投到當前幀找對應點(可以說算是加速的近似的ICP，ICP嚴格要求clostest point), 不過這算是點到點的距離，通常會考慮投影點的法向量而使用點到面的距離。

由於直接法需要初值，如果是ordered sequence, 比如SLAM，一般可以用上一幀的結果，但如果是unordered sequence，則需要利用間接法進行bootstrap。間接法一般是sparse(ORBSLAM2)的，直接法則可以是sparse(比如最近的Direct Sparse Odometry)或dense(比如KinectFusion)的。

Essential matrix 就是可以算pose不同的camera的R，t的關係的，但是t＝0算不出來。pure translation好像也算不出來，不確定。。。而且算出來的t，沒有scale。。要自己算那個
PnP就是一堆3D點在另一個camera上面看到了對應的2D pixel，然後能算出來R，t，但是同理,點太少，t太小，算不準。。但是有scale，就是t是不需要變的了。。
具體的演算法上面說的太好了！

還有其他有意思的演算法：
Homography，能找出來8個pose，只有一個是所有的點在兩個camera前面的，適合看到的點在同一個major plan上，其實我用essential算也沒啥大問題。。
Bundle Adjustment，就是有一堆3D的點，找到了對應的在另一個camera C的2D pixel，和PnP很像，不過只需要給出camera C的大概的初始位置，BA直接算出來，很神奇，不過初始位置要准。大部分用於video，因為相鄰的camera pose幾乎差不多，幾十個pixel之差。
ICP是出於點陣之間的R，t， optimization後得出的，但是點陣要離得近，分布均勻，要不密集的點全都堆堆到一起去了。。說白了就是分布不均勻，R,t，會很sensitive
Direct method沒怎麼研究過。。好像是個通過estimate depth map然後直接放到一個optimization裡面算出來的。。