SLAM前端：對極幾何、三角測量

對極幾何在ORBSLAM初始化過程中求解兩幀的R、t，輸入兩幀圖像的多對匹配點，對極幾何計算得到R、t

三角測量在單目ORBslam初始化過程中計算初始化點雲的3D座標，輸入兩幀的R、t+匹配特徵點的像素座標，輸出點的3D座標

1 對極幾何

對極幾何在兩幀的圖像中，其中包含n個匹配的特徵點，用於估計相機的位姿：旋轉矩陣R與平移向量t。一般只在單目SLAM初始化過程中使用。

 

情景如圖，兩幅圖分別爲相機在兩位置的圖像，共同觀測到P點。可以理解爲，相機從位置變換到位置，旋轉平移矩陣分別爲R、t。現在需要求解R、t。

空間中的一對匹配點如圖所示，左圖投影點像素座標，右圖投影點。以左相機相機座標系爲參考座標系有：

                                                                                                                          （1-1）

K爲相機內參矩陣，可逆，可得：

                                                                                                                         （1-2）

分別爲P點在兩相機座標系下的深度，即z座標。

取，則：

                                                                                                                                        （1-3）

兩邊同時左乘，並且得到：

                                                                                                                                       （1-4）

兩邊同時左乘得到：

                                                                                                                              （1-5）

與垂直，因此上式左邊爲0，得到：

                                                                                                                                        （1-6）

將代入可得：

                                                                                                                       （1-7）

上式即爲對極約束公式，簡潔的表述了兩幅圖像之間一組匹配點之間的關係。求解出上式即可求解出R、t。

2 三角測量

三角測量利用兩幀圖像匹配的像素點、以及兩幀圖像的位姿關係R、t，計算匹配點的3D座標。

已知兩幀圖像的匹配點P在兩幀圖像下的像素座標、兩幀圖像的旋轉矩陣R、平移矩陣t。求解匹配點P的3D座標。

由（1-2）（1-3）可以得到：

                                                                                                                                 （1-8）

同時左乘得到：

                                                                                                                              （1-9）

上式可以求解得到，代入式（1-8）可以求解得到。

但是由於觀測噪聲，求解會帶來誤差，一般用優化進行求解。如下圖：