orbslam2的基礎理論（一）

轉載聲明：本文轉載自  金木炎 的博客，僅供個人學習。感謝博主的無私分享，如有侵權，敬請告知。

本篇博客借鑑了許多經典博客的理論。

orbslam2相機pose估計和map point結構恢復

orbslam2的相機初始估計有兩種方法：本證矩陣Ｅ和單應矩陣兩種方法，具體可以看我之前的博客oerslam2-初始化

（1)再講相機pose估計之前，我要講一下相機的數學模型和對極幾何。

我覺得首先我們要理解相機模型中的四個平面座標系的關係：像素平面座標系（u,v）、像平面座標系（圖像物理座標第（x,y）、相機座標系（Xc,Yc,Zc）和世界座標系（Xw,Yw,Zw）。

1、像素座標與像平面座標系之間的關係 

    確定他們的關係之前，我們可以假設每一個像素在u軸和v軸方向上的物理尺寸爲dx和dy。仔細看下他們的模型可以推出以下公式（這個還是比較好理解的）：

 

                                            

                                                          公式1

    解釋：1、dx,dy,u0,v0其實都是我們假設出來的參數，最終是要我們求的內外參數。（dx，dy是一個像素點的在x,y像平面座標系下的長度，ｕ0,ｖ0是光心投影到圖片上點的像素座標）

得出這個公式後我們可以運用線性代數的知識把方程用矩陣形式表示：

                             

                                                       公式2

當然我們也可以用另一種矩陣形式表示：

                            

                                                           公式3

2、相機座標系與世界座標系之間的關係 

   這兩個座標系之間的關係我們可以旋轉矩陣R和平移矩陣T來得到以下關係：

                                

                                                        公式4

  解釋：1、在這個公式中，R爲3*3矩陣，T爲3*1，0爲（0，0，0），簡化用Lw表示後爲4*4矩陣。

3、成像投影關係（相機座標系與像平面座標系） 

    在相機模型中我們可以得到以下公式：

                                

                                                      公式5

         解釋：1、（其中f是相機焦距）

同樣我們用矩陣形式表示：

                                   

                                                           公式6

4、得到公式

而我們可以將以上公式綜合一下就可以得到：

  

（2)現在要講一下對極幾何的知識！

兩個攝像機的光心C0、C1，三維空間中一點P，在兩幅圖像中的位置爲p0、p1。如下圖所示： 
 
由於C0、C1、P三點共面，得到： 
 
p0在座標系C0中的表示,以及p1在座標系C1中的表示爲： （像平面座標系）
  p0=⎛⎝⎜x0y01⎞⎠⎟c0  和  p1=⎛⎝⎜x1y11⎞⎠⎟c1

這時，由共面得到的向量方程可寫成：

  (1) 其中，t是兩個攝像機光心的平移量;R是從座標系C1到座標系C0的旋轉變換，左乘旋轉矩陣R的目的是把向量C1p1→在座標系C1下的表示旋轉到座標系C0下，注意點p1是在座標系C1中的表示。 
  一個向量a [a1 a2 a3]叉乘一個向量b可以表示爲一個反對稱矩陣乘以向量b的形式這時由向量a表示的反對稱矩陣(skew symmetric matrix)如下: 
所以式1可以寫成： 我們用[t]×來表示t向量組成的反對稱矩陣。在之前那篇講lie的代數中，我們用t^表示的向量t的反對稱矩陣，在這邊博客中這種表示也會採用(寫起來方便)。 
  本徵矩陣 E 定義爲 E=[t]×R，它是一個3x3的矩陣： 

一個3x3的矩陣是本徵矩陣的充要條件是對它奇異值分解後，它有兩個相等的奇異值，並且第三個奇異值爲0

計算本徵矩陣E、尺度scale的來由

將矩陣相乘的形式拆開得到 
 
上面這個方程左邊進行任意縮放都不會影響方程的解： 
  (x0x1x0y1x0y0x1y0y1y0x1y11)E33⎛⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜E11/E33E12/E33E13/E33...1⎞⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟=0 
所以E雖然有9個未知數，但是有一個變量E33可以看做是縮放因子，因此實際只有8個未知量，這裏就是尺度scale的來由，後面會進一步分析這個尺度。 
  AX=0，x有8個未知量，需要A的秩等於8，所以至少需要8對匹配點。有了匹配點後，就只需要求解最小二乘問題了，上面這個方程的解就是矩陣A進行SVD分解A=UΣVT後，V矩陣最右邊那一列的值。另外如果這些匹配點都在一個平面上那就會出現A的秩小於8的情況，這時會出現多解，會讓你計算的E可能是錯誤的。

然而在實際計算過程中，匹配點座標存在誤差，這會使得計算出的E可能不會滿足之前提到的那條性質，所以我們需要把計算出的E投影到真正的本徵矩陣空間，也就是使得它的三個奇異值中兩個相等，一個爲0。

投影的方法如下：

從本徵矩陣恢復R、T，尺度scale的進一步分析

  我們知道本徵矩陣 E 定義爲 E=[t]×R或者t^R。可以用下面的計算式從本徵矩陣中恢復R和T，T^表示T的反對稱矩陣： 

其中RZ(π/2)表示繞Z軸旋轉90度得到的旋轉矩陣： 
從R,T的計算公式中可以看到R,T都有兩種情況，組合起來R,T有4種組合方式。由於一組R,T就決定了攝像機光心座標系C的位姿，所以選擇正確R、T的方式就是，把所有特徵點的深度計算出來，看深度值是不是都大於0，深度都大於0的那組R,T就是正確的。

這裏我們有必要進一步的分析下尺度scale，從R，T的計算公式中，可以發現平移向量t和E的奇異值Σ有關。再回顧一下之前求E時候的方程以及提到的伸縮: 

AE=AUΣV=0AsE=AUsΣV=0

所以，之前推導過程中所有對E進行的尺度伸縮實際上都是作用在奇異值Σ上。也就是說這個尺度scale實際作用的是兩個相機之間的平移，圖示如下： 

這個圖簡單明瞭的演示了這種平移縮放作用。從圖中也可以看出，由於尺度scale的關係，不同的t，決定了以後計算點P的深度也是不同的，所以恢復的物體深度也是跟尺度scale有關的，這就是論文中常說的結構恢復structure reconstruction，只是恢復了物體的結構框架，而不是實際意義的物體尺寸。並且要十分注意，每兩對圖像計算E並恢復R,T時，他們的尺度都不是一樣的，本來是同一點，在不同尺寸下，深度不一樣了，這時候地圖map它最頭痛了，所以這個尺度需要統一。 
  那麼如何讓scale之間統一呢？如果你一直採用這種2d-2d匹配計算位姿的方式，那每次計算的t都是在不同尺度下的，Davide Scaramuzza的論文《Visual odometry I》的computing the relative scale那部分講了一種方法使得相鄰位姿間的不同的尺度s經過縮放進行統一。我們已經知道出現尺度不一致是由於每次都是用這種計算本徵矩陣的方式，而尺度就是在計算E時產生的。所以尺度統一的另一種思路就是後續的位姿估計我不用這種2d-2d計算本徵E的方式了，也就說你通過最開始的兩幀圖像計算E恢復了R,T，並通過三角法計算出了深度，那我就有了場景點的3D座標，後續的視頻序列就可以通過3Dto2d(opencv裏的solvePnp)來進行位姿估計，這樣後續的視頻序列就不用計算本徵矩陣從而繞過了尺度，所以後續的視頻序列的位姿和最開始的兩幀的尺度是一樣的了。但是，這樣計算又會產生新的問題–scale drift。因爲，兩幀間的位姿總會出現誤差，這些誤差積累以後，使得尺度不再統一了。

下一篇的將會講單應矩陣的理論。