orbslam2-基礎理論（二)

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

這篇將要講orbslam2相機初始位置估計的另一種方法：單應矩陣Ｈ（使用場景：平面，視差小）和map point的求法

（一）：單應矩陣

在計算機視覺中，平面的單應性被定義爲一個平面到另外一個平面的投影映射。因此一個二維平面上的點映射到攝像機成像儀上的映射就是平面單應性的例子。如果點Q到成像儀上的點q的映射使用齊次座標，這種映射可以用矩陣相乘的方式表示。若有一下定義：

Q=[X Y Z  1]T

q=[x y  1]T

則可以將單應性簡單的表示爲：

q=sHQ

這裏引入參數s，它是任意尺度的比例（目的是使得單應性定義到該尺度比例）。通常根據習慣放在H的外面。

H有兩部分組成：用於定位觀察的物體平面的物理變換和使用攝像機內參數矩陣的投影。

物理變換部分是與觀測到的圖像平面相關的部分旋轉R和部分平移t的影響之和，表示如下

W=[R t]

這裏R爲3*3大小的矩陣，t表示一個一個3維的列矢量。

攝像機內參數矩陣用M表示，那麼我們重寫單應性如下：

q=sWMQ  where W是相機內參矩陣

我們知道單應性研究的是一個平面上到另外一個平面的映射，那麼上述公式中的~Q，就可以簡化爲平面座標中的~Q'，即我們使Z=0。即物體平面上的點我們用x,y表示，相機平面上的點，我們也是用二維點表示。我們去掉了Z方向的座標，那麼相對於旋轉矩陣R，R可以分解爲R=[r1 r2 r3]，那麼r3也就不要了。

其中H爲：

H=sM[r1 r2 t]

是一個3×3大小的矩陣.

 故最終的單應性矩陣可表示如下：

q=sHQ

我們可以從Ｈ矩陣中恢復Ｒ，Ｔ。但是同樣存在尺度問題（指Ｔ），所以只有在初始位置估計的時候纔用到Ｅ或Ｈ。以後一般都用opencv的ＰＮＰ求解也就是3Ｄ-2Ｄ的映射。

(二）我們除了要求相機的pose以外，還要求map point。下面來講解map point的求法。

   對於三維空間中的一點 P，我們假設第一個攝像機座標系C1就是世界座標系，P在世界座標系下的表示爲P=(x,y,z,1)T，這時，攝像機座標系C1的外參數矩陣M1爲單位矩陣。 點P和光心的連線交第一個圖像平面於點p1 ,注意這裏的p1是在攝像機座標系的座標表示，不是在圖像座標系下，這在上一篇博客中已經強調。同理，和第二個攝像機光心連線交第二幅圖像於點p2 。 pi在各自攝像機座標系中的表示爲：
p1=⎛⎝⎜x1y11⎞⎠⎟c1  和  p2=⎛⎝⎜x2y21⎞⎠⎟c2 攝像機座標系C2的外參數矩陣爲M2，由於攝像機座標系C1就是世界座標系，所以有 在推導本徵矩陣E的時候，我們說R是從座標系C2到座標系C1的旋轉變換矩陣，即12R。t是平移，更確卻的說是光心C2在C1中的座標表示,即1t2。所以在通過8點法求出的R,T以後，得到的從攝像機座標2變換到到攝像機座標系1的變換矩陣爲
12H=[R3×30t3×11] 而這裏的外參數矩陣M2是將世界座標系中的一點P投影到攝像機座標系C2。所以M2=21H=12H−1，只需去掉H矩陣的最下面一行齊次座標就行了。

所以，通過本徵矩陣得到R,t以後，要計算攝像機座標系C2外參數矩陣的程序如下：

有了外參矩陣，我們就可以得到這些點座標的關係：

由於光心Ci，三維座標點P，以及pi三點共線，所以向量Cipi、CiP的叉乘應該爲0，上述方程又可以轉化爲： 這又是一個要用最小二乘求解的線性方程方程組 ,和求本徵矩陣一樣，計算矩陣A的SVD分解，然後奇異值最小的那個奇異向量就是三維座標P的解。

在以上計算P三維座標的推導過程中，可以看到和本徵矩陣E是息息相關的，E和我們的尺度緊密相連，所以計算出來的深度和尺度scale也是直接相關的。同時，根據這種三角法(triangulation )計算的深度，其實是不怎麼靠譜的，一般只是拿這個做一個初始值。

兩個射線夾角越小，誤差協方差越大。所以點到光心連線組成的射線向量在orbslam中是有明確記錄的。