參考：
高翔——《視覺SLAM十四講》第7講 視覺里程計1
高翔——《視覺SLAM十四講》系列講解視頻及PPT ch7_1 & ch7_2

• 目錄：
  1 相機運動估計之 2D-2D: 對極幾何
  2 相機運動估計之 3D-2D: PnP
  3 相機運動估計之 3D-3D: ICP
  4 特征點位置估計之 三角測量

• 綜述（可能有錯，需檢查）
  （1）（僅單目相機初始化時服用）2D-2D，已知匹配好的若干特征點在兩幀圖像上的位置，得到估計的初始R、t；
  （2）（單目初始化時2D-2D之后立即服用）三角測量，已知匹配好的若干特征點在兩幀圖像上的位置，以及估計的R、t，得到估計的特征點空間位置P；
  （3）（單目三角測量后服用；以后每來一幀圖像都服用）3D-2D，已知前一幀的特征點的空間位置P（之前的三角測量實際上給了單目相機深度信息，變成Depth不準(zhǔn)確的RGBD），以及后一幀對應(yīng)到前一幀特征點的特征點在該幀圖像的位置，得到估計的R、t；
  （4）（雙目或RGBD相機服用）3D-3D，已知匹配好的若干特征點在兩幀圖像對應(yīng)的空間中的位置，得到估計的R、t。

1 相機運動估計之 2D-2D: 對極幾何

1. 對極約束

• 對極幾何中的幾何關(guān)系認(rèn)知：

  （1）O1、O2是光心，p1、p2是投影，e1、e2是極點，l1、l2是極線，PO1O2是極平面
  （2）約束關(guān)系：若p1已知、p2未知，可判斷p2在極線l2上；反之亦然。

• 實踐中對問題的分析：
  （1）條件：p1、p2已知，P未知
  （2）待求：T12

• 數(shù)學(xué)描述：
  

  基于“圖1的p1經(jīng)R、t到達圖2的p2”的想法，經(jīng)過一系列轉(zhuǎn)換：
  得到簡單的對極約束形式：
  定義E和F并進一步化簡對極約束的表示：
  其中E為Essential本質(zhì)矩陣，F(xiàn)為Fundamental基礎(chǔ)矩陣。
  E和F差的是內(nèi)參的關(guān)系。p和x差的也是內(nèi)參的關(guān)系。
  對極約束本質(zhì)上刻畫的是O1O2P共面的關(guān)系。
• 進一步分析：
  E共有5個自由度（R有3個，t有3個；但是由于對極約束中，等式兩遍同乘以一個非零常數(shù)后，照樣成立，即E乘以一個非零常數(shù)后等式依然成立，故自由度減1），這就意味著至少可由5對點求得E，但是這樣的五點法很麻煩（因為要用到什么非線性什么鬼的，反正就是麻煩）；而若當(dāng)作普通矩陣，則有8個自由度（3*3矩陣9個自由度，原因同上再減1個）。
• 解決問題流程：
  （1）由匹配點計算E矩陣；
  （2）由E恢復(fù)R、t。

2. 求解E本質(zhì)矩陣（然后使用E或F恢復(fù)R、t）：八點法+SVD

• 八點法的整體意思：
  

  E是(3 * 3)的，對極約束就是(1 * 3) * (3 * 3) * (3 * 1) = (1 * 1)的，可以把E轉(zhuǎn)成向量形式的e進行計算，那么左邊是1對點的形式，右邊是8對點的形式，相當(dāng)于是解Ax=0的矩陣，這樣解得E。

• SVD解R、t：
  

  反正解出來之后，可能性有4種，但只有一個是合理的，這樣就解完了。
• 八點法的討論和分析：
  （1）只用于單目的初始化，后面就用PnP和ICP了。
  （2）尺度不確定性：E=t^ R，E乘以任意倍數(shù)仍可行，也就是t^ R乘以任意倍數(shù)仍可行，R是正交矩陣，本身有約束（？），而t這個平移向量乘以任意倍數(shù)，解仍可行。所以直觀上，PO1O2這個三角（軌跡和地圖），縮放任意倍，得到的觀測值都是一樣的，這就是單目的尺度不確定性。所以實踐上，一般將t的模或特征點的平均深度設(shè)為1，不過求得的t跟真實的t差的倍數(shù)還是無從得知的。
  （3）對于純旋轉(zhuǎn)問題（t=0），由于E定義為t^ R，故此時E為0矩陣，那就沒法分解E求得R、t了，所以實際上沒有平移是沒法單目初始化的，單目初始化時一定要帶有平移。
  （4）多于8對點時，就成了算最小二乘解，或者是用RANSAC算法（好像上課學(xué)過，記得思路很神奇）。

3. 求解H單應(yīng)矩陣（然后使用H恢復(fù)R、t）

• 什么時候用到單應(yīng)矩陣：
  若場景中的特征點都落在同一平面上（比如墻、地面等），則可以通過單應(yīng)矩陣來進行運動估計。這種情況在無人機攜帶的俯視相機、掃地機攜帶的頂式相機中比較常見。
• 好像跟用E時是差不多的，先不看了。

2 相機運動估計之 3D-2D: PnP

• 綜述：
  （1）3D-2D的情況是已知n個3D空間點以及其投影位置的情況；
  （2）PnP，Perspective-n-Point；
  （3）具體方法分兩類：代數(shù)方法（直接線性變換 DLT、用三對點估計位姿的P3P）；非線性優(yōu)化方法（光束法平差 Bundle Adjustment），一般用優(yōu)化的方法，因為代數(shù)方法對噪聲不太魯棒

2.1 直接線性變換 DLT

• 分析：

  
  
  展開后最后一行左邊只有s，那就可以用這一行消去前兩行中的s。則一個特征點提供兩個方程：
  為了求解12個未知數(shù)，需要6對點。
• 討論：
  （1）由于求解時將12個元素看成是獨立的，而R要有正交矩陣的約束，所以就要用QR分解什么鬼的；
  （2）其他的先空著

2.2 P3P

而解這個二元二次方程組需要用到吳消元法，求解析解。但解出的是4個解，再用一對驗證點D、d計算最可能解。最終得到ABC三點在相機坐標(biāo)系下的3D坐標(biāo)。

2.3 Bundle Adjustment

參考：Bundle Adjustment---即最小化重投影誤差（高翔slam---第七講）

• 這個詞很抽象的樣子，先整體感受一下：
  （1）一般翻譯成“光束法平差”，很久沒有讀通，現(xiàn)在覺得“平”是個動詞，“平差”指擺平誤差。依據(jù)什么去“平差”？是用“光束”的方法，而“光束法”本身并不是理解成一個現(xiàn)成的方法，“光束”只是指相機拍照時三維空間點投影到二維圖像的連線集合。合起來，就是“利用三維到二維投影的光束，消除誤差”。
  （2）光束法平差的本質(zhì)是優(yōu)化，目標(biāo)函數(shù)是（min重投影誤差）。
  （3）“第一次投影”指相機拍照時三維空間點投影到圖像；“第二次投影 / 重投影”指計算得到的三維坐標(biāo)點（非真實）投影到計算得到的相機位姿（非真實）。

3 相機運動估計之 3D-3D: ICP

• 綜述：

  （1）首先，3D-3D的情況是雙目相機或RGBD相機的情況；
  （2）要搞清楚，所謂“ICP（Iterative Closest Point 迭代最近點）”并不是指解決這類問題的方法。對于激光SLAM來說，由于激光中數(shù)據(jù)特征不夠豐富，我們無從得知兩個點集之間的匹配關(guān)系，只能認(rèn)為距離最近的兩個點為同一個，這是“迭代最近點”；而在視覺SLAM中，特征點提供了較好的匹配關(guān)系（其實就省了“ICP”），而ICP在這里指代匹配好的兩組點間的運動估計問題（其實還是感覺很奇怪，ICP就理解成“兩組點的特征匹配已經(jīng)做好的這種狀態(tài)下，去做運動估計”吧）；
  （3）具體方法分兩種：線性代數(shù)求解（SVD）；非線性優(yōu)化求解（類似Bundle Adjustment）。
  （4）注意，在3D-3D問題中，并沒有出現(xiàn)相機模型，只與匹配好的兩組點有關(guān)。

3.1 SVD

• 分析：
  

  （1）構(gòu)建最小二乘問題，找使其最小的R、t：
  變換后，目標(biāo)函數(shù)變成：

  左邊只與R有關(guān)，而且與每個點的去質(zhì)心坐標(biāo)有關(guān)；右邊與R、t有關(guān)，而且只與兩個質(zhì)心有關(guān)。
  

  （2）而左邊部分變成了優(yōu)化這個：
  在這個東西里用SVD求解R。
  （3）用右邊部分就能直接求出t了：
• 真正的SVD部分：
  就不想看了。

4 特征點位置估計之 三角測量

• 綜述：
  有了相機的運動估計，下一步需要通過三角測量來估計特征點的空間位置。

• 分析1：
  理論上O1p1與O2p2會交于一點P，然而由于噪聲影響，往往無法相交，因此可以通過最小二乘法求解。按照對極幾何：
  其中x1、x2為特征點在兩個像平面成像的歸一化坐標(biāo)，s1、s2為特征點的深度。
  這兩個深度是可以分開求的，例如算s2，則對上式兩側(cè)左乘x1^：

  其中左側(cè)為0，右側(cè)可以看做關(guān)于s2的方程。如此可分別求得s1和s2，即可求得P的空間坐標(biāo)。

• 分析2：
  

  但是上面單獨求解s1、s2的過程中，由于噪聲的存在，導(dǎo)致估得的R、t不能使得s1x1=s2Rx2+t這個式子精確成立。所以更常見的方法是求最小二乘解而不是零解：
• 討論：
  先空著。