對極幾何、基本矩陣、本質矩陣

對極約束相關介紹可以在《計算機視覺中的多檢視幾何》一書的185頁找到；

1 對極約束

1.2 對極約束的理解

對極幾何是兩幅檢視之間內在的射影幾何；

對極約束：已知某一3D點\(X\)在第一張影象上的投影是\(x\),那麼在同樣觀測到點\(X\)的第二幅影象上的投影\(x'\)是如何被約束的？

首先，有相機投影模型 \(Zx = PX\) ，可以推斷點\(X\)在射線\(Cx\)上，（C是第一個相機的光心）

由於無法確定點\(X\)的深度，因此，第二幅影象上成像點的在面\(CC'X\)與成像平面的交線\(l'\)上,在對極幾何中稱為極線

則有第一個攝像機光心在第二幅影象上的投影\(e'\)稱為極點

因此：對極約束實際上是將\(x'\)約束在直線\(l'\)上，表示是由點\(x\)到直線\(l'\)的對映

假設 該對映為 \(F\) , 則有 \(l' = Fx\),由於點\(x'\)在直線\(l'\)上，可得

其中，對映\(F\)就是基本矩陣，表示的是點到線的對映；

對極幾何的前提：基線長度不能為0；初始化，純旋轉退化，對極約束就不存在了。

2 基本矩陣-Fundamental(對極約束的代數表示)

2.1 基本矩陣的幾何推導

基本矩陣的幾何推導的詳細內容可以參考《計算機視覺中的多檢視幾何》

將對極約束的過程分成兩步來分析：

1. 假設有虛擬平面\(\pi\)，將影象平面1上的點\(x\)，投影到平面\(\pi\)上，在轉移到影象平面2上的點\(x^{'}\)

1. 第二步是構造對極線，即攝影機\(P_1\)的光心在影象平面2上的投影-極點\(e^{'}\)和投影點\(x^{'}\)構成的直線\(l^{'}\),兩個點的叉乘得到直線方程；

那麼可以定義點到極線的對映\(F\)滿足：\(I^{'} = Fx\) ，因此，\(F = [e^{'}]_{\times}H_{\pi}\)，由於\([e^{'}]_{\times}\)的秩為2，基本矩陣\(F\) 的秩為2。

另外，由於點\(x\)的投影點\(x^{'}\)一定在直線\(I^{'}\)上，通過內積為0可以得到一個約束，也就是對極約束。

2.2 基本矩陣的代數推導

2.3 基本矩陣的重要性質

3 基本矩陣的計算方法

3.1 最小化代數距離方法（DLT）

3.2 退化情形

無平移，兩個檢視，攝影機中心重合。對極幾何的定義不存在了

4 本質矩陣

4.1 對本質矩陣的理解

首先本質矩陣是基本矩陣在歸一化平面上的特殊表示；具有形式: \(E = [t]_{*}R = R[R^{T}t]_{*}\)

4.2 本質矩陣的性質

(1) 本質矩陣的自由度

本質矩陣的自由度是5，旋轉矩陣和平移向量分別有三個自由度，但是本質矩陣有一個全域性尺度因子的多義性；

(2) 本質矩陣的內在性質

一個矩陣是本質矩陣的充要條件是它的奇異值中有兩個相等而第三個是0.其奇異值一定是\([\sigma,\sigma,0]^{T}\)的形式

該結論的證明在《計算機視覺中的多檢視幾何》的9.6.1小節有介紹；

4.3 本質矩陣的求解

本質矩陣的求解也可以使用八點法，構建線性方程組來求解。具體的形式不再推導。可以參考《視覺SLAM十四講》

值得提一下的是，八點法構建的線性方程組，最後求解完，可能沒辦法滿足本質矩陣的內在性質

那有沒有解決辦法呢？

可以進行如下操作：

1. 首先對E做奇異值分解：假設得到E的奇異值矩陣 \(\Sigma = diag(\sigma_1,\sigma_2,\sigma_3)\),且有\(\sigma_1 > \sigma_2 > \sigma_3\)
2. 那麼可以取新的E為

4.4 本質矩陣的分解（由本質矩陣恢復攝像機矩陣）

對本質矩陣的分解，可以參考《計算機視覺中的多檢視幾何》和《視覺SLAM十四講》一起看；

在ORB-SLAM2的地圖初始化時，會分別計算H和F，統計特徵點重投影誤差及點到極線的距離計算得分，選擇得分高的對映。

當選擇F矩陣時，需要從F矩陣中恢復位姿，ORB-SLAM2中會通過F計算E，再從E中恢復pose。

總結一下流程：

1. 對本質矩陣進行奇異值分解
2. 左奇異值矩陣U的最後一列就是t，對其進行歸一化
3. 構造一個繞Z軸旋轉pi/2的旋轉矩陣W，按照下式組合得到旋轉矩陣 R1 = uWvt，旋轉矩陣有行列式為+1的約束，所以如果算出來為負值，需要取反
4. 矩陣W取轉置來按照相同的公式計算旋轉矩陣 R2 = uW.t()vt，同理，旋轉矩陣有行列式為+1的約束，所以如果算出來為負值，需要取反
5. 獲得本質矩陣分解結果，形成四組解,分別是： (R1, t) (R1, -t) (R2, t) (R2, -t)
6. 四組接分別進行三角化，取有效3D點最多的一組解。

// step 2 : 根據基礎矩陣和相機的內引數矩陣計算本質矩陣
  cv::Mat E21 = K.t()*F21*K;

  // 定義本質矩陣分解結果，形成四組解,分別是： (R1, t) (R1, -t) (R2, t) (R2, -t)
  cv::Mat R1, R2, t;

  // step 3 : 從本質矩陣求解兩個R解和兩個t解，共四組解
  // Note : 參考：Multiple View Geometry in Computer Vision - Result 9.19 p259
  // 不過由於兩個t解互為相反數，因此這裡先只獲取一個
  // 雖然這個函數對t有歸一化，但並沒有決定單目整個SLAM過程的尺度.
  // 因為 CreateInitialMapMonocular 函數對3D點深度會縮放，然後反過來對 t 有改變.
  //注意下文中的符號「'」表示矩陣的轉置
  //                          |0 -1  0|
  // E = U Sigma V'   let W = |1  0  0|
  //                          |0  0  1|
  // 得到4個解 E = [R|t]
  // R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3
  DecomposeE(E21,R1,R2,t);
  cv::Mat t1 = t;
  cv::Mat t2 = -t;

/**
 * @brief 分解Essential矩陣得到R,t
 * 分解E矩陣將得到4組解，這4組解分別為[R1,t],[R1,-t],[R2,t],[R2,-t]
 * 參考：Multiple View Geometry in Computer Vision - Result 9.19 p259
 * @param[in] E                 本質矩陣
 * @param[in & out] R1          旋轉矩陣1
 * @param[in & out] R2          旋轉矩陣2
 * @param[in & out] t           平移向量，另外一個取相反數
 */
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{
  // 準備儲存對本質矩陣進行奇異值分解的結果
  cv::Mat u,w,vt;
  // 對本質矩陣進行奇異值分解
  cv::SVD::compute(E,w,u,vt);

  // 左奇異值矩陣U的最後一列就是t，對其進行歸一化
  u.col(2).copyTo(t);
  t=t/cv::norm(t);

  // 構造一個繞Z軸旋轉pi/2的旋轉矩陣W，按照下式組合得到旋轉矩陣 R1 = u*W*vt
  // 計算完成後要檢查一下旋轉矩陣行列式的數值，使其滿足行列式為1的約束
  cv::Mat W(3,3,CV_32F,cv::Scalar(0));
  W.at<float>(0,1)=-1;
  W.at<float>(1,0)=1;
  W.at<float>(2,2)=1;

  // 計算
  R1 = u*W*vt;
  // 旋轉矩陣有行列式為+1的約束，所以如果算出來為負值，需要取反
  if(cv::determinant(R1)<0)
    R1=-R1;

  // 同理將矩陣W取轉置來按照相同的公式計算旋轉矩陣R2 = u*W.t()*vt
  R2 = u*W.t()*vt;
  //旋轉矩陣有行列式為1的約束
  if(cv::determinant(R2)<0)
    R2=-R2;
}