1、SIFT特征提取

SIFT特征提取可以分為以下幾個步驟：（1）多尺度卷積；（2）構造金字塔；（3）3D非極大值抑制。

2、SIFT特征描述

特征提取和特征描述實際上是兩碼事。在上一節中特征提取已經結束了。假如有兩幅圖片，那么相同的特征點肯定會被找到。特征描述的作用是為匹配做準備，其以特征點局部區域信息為標準，將兩幅圖中相同的特征點聯系起來。特征的本質是一個高維向量。要求尺度不變，旋轉不變。

1、最小二乘法擬合

基本的最小二乘法擬合解決的是 點 --- 模型 的擬合問題。以點到直線的擬合為例，按照擬合誤差的建模，該問題可以分為兩類。　

2、RASAC擬合　

RanSaC算法（隨機采樣一致）原本是用于數據處理的一種經典算法，其作用是在大量噪聲情況下，提取物體中特定的成分。下圖是對RanSaC算法效果的說明。圖中有一些點顯然是滿足某條直線的，另外有一團點是純噪聲。目的是在大量噪聲的情況下找到直線方程，此時噪聲數據量是直線的3倍。

1. 從人群中隨便找個男生，看看他條件怎么樣，然后和他談戀愛，（平面中隨機找兩個點，擬合一條直線，并計算在容忍誤差e中有多少點滿足這條直線）
2. 第二天，再重新找個男生，看看他條件怎么樣，和男朋友比比，如果更好就換新的（重新隨機選兩點，擬合直線，看看這條直線是不是能容忍更多的點，如果是則記此直線為結果）
3. 第三天，重復第二天的行為（循環迭代）
4. 終于到了某個年齡，和現在的男朋友結婚（迭代結束，記錄當前結果）

3、非線性擬合

線性最小二乘法已經有了很好的解釋。但是生活總是如此不易，能化成上述標準矩陣形式的問題畢竟還是少數，大部分情況下，我們面對的不是min(||Ax - b||)，而是 min(||f(x)-b||) !!!

　

　　

　　

　　

極幾何是機器人視覺分支——雙目視覺中，最為重要的概念。與結構光視覺不同，雙目視覺是“主動測量”方法。

1、極幾何的研究前提

極幾何的研究對象是兩幅有重疊區域圖像。研究目標是提取相機拍攝位姿之間的關系。一旦得到兩次拍攝位姿之間的關系，我們就可以對場景點進行三維重建。

極幾何定義的物理量包括4個：1、極點；2、極線；3、基本矩陣；4、本征矩陣；定義如左圖。

極幾何研究的物理量包括4個：C1坐標，C2坐標，R，T，定義如右圖。

極點的本質是另一臺相機光心在本圖像上的映射點。極線的本質是另一臺相機光線在本圖像上的映射線。（極點和極線都是在圖像上的）

1.1、本征矩陣

本征矩陣攜帶了相機相對位置信息。其推導如下：

在相機2的坐標系中，場景點坐標：X2 = RX1+ t

相機1光心坐標：t

極線在空間中的映射 ：X2 - t = RX1

此時，三個向量在同一個平面上，則有：X2 T tx RX1  = 0

其中，tx  代表 t 的叉乘矩陣。tx R 稱為本征矩陣E. 兩幅圖片一旦拍攝完成R與T都是確定的。空間中任何一組對應點都必須滿足本征矩陣！

1.2、基本矩陣

空間中的點滿足E矩陣，則該點坐標Zoom后，仍然必須滿足E矩陣。坐標的Zoom顯然和相機內部矩陣有關。

在相機坐標系下：

x1 = KX1; x2 = KX2

其中，x1 ，x2 是齊次像素坐標。那么，X1  =  K-1x1 ；X2  =  K-1x2

帶入本征矩陣可得：

x2 T K-Ttx RK-1 x1  = 0　　======>      K-TEK-1  = 0  =========>  x2 T F  x1  = 0 

F  =   K-TEK-1  稱為基本矩陣。

基本矩陣所接受的是齊次像素坐標。

基本矩陣的秩是2，因為它有0空間。同時，其自由度是8，因為它接受的是齊次坐標。每組圖像點可以提供1個方程，所以由8組點就可以線性解出F矩陣。當然，解法是化成Ax = 0，然后使用奇異值分解取v的最后一列。然后2次奇異值分解去掉最小奇異值正則化。

1.3、極點與極線

從基本矩陣可知：x2 T F  x1  = 0 

顯然這里有熟悉的身影，由點線對偶可知，x2 在直線 F  x1  上。該直線是極線在圖像2上的方程。x1 在直線 x2 T F   。該直線是極線在圖像1上的方程。

極點是多條極線的交點（最少兩條）

2、由本征矩陣恢復R,T

E =  tx R = [ tx r1  tx r2  tx r3 ]

E的秩為2，因為其有0空間。同時，由于r1 r2 r3 是正交的，所以其叉乘之后必然也是正交的。所以不妨假設其叉乘完之后依然滿足旋轉矩陣的某些性質。比如：每一列，模相等。

由  tT E = 0 可知，對E奇異值分解之后，t 為最小奇異值所對應的 u(:,end). 如下：

這里假設了 R = UYVT .因為U,V和R是同族的。所以必然由矩陣Y使得上式成立。V是相互垂直的，R的作用是旋轉，U則必然是相互垂直的。所以這里R一定有解，不妨設一個中間變量Y。并很容易解得：

綜合來看，由4組可能的解，對應以下四種情況，其中只有第一種是可能的。故det(R) = 1 則猜z中了正確的解，如果det(R) = -1

則解為：t = -t ；R = -R　　

3、由空間位置關系恢復三維坐標

在已知標定信息，兩相機位置關系的情況下，就已知了兩個相機的投影矩陣P，對于空間中一點X1，有以下關系：

x1　=  P*X1

[x1]x P X1 = 0;

顯然，我們又有了Ax = 0的神奇形式。奇異值分解搞定之。

4、由RANSAC求 F 矩陣

有了8個對應點，我們就可以求得F矩陣，再加上K，我們就可以對兩幅圖片進行三維重建。然而想要自動的求取8個對應點還是有一定難度。

SIFT算法提供了一種自動匹配的可能性，然而，匹配結果還有很多誤匹配的點。本節的目標是利用RANSAC作為算法基礎，基礎矩陣作為方法，來對匹配結果進行判斷。

首先，由于檢測誤差等因素，像素點不可能恰好滿足基本方程。所以點到極線會有一定的距離。我們采用垂直距離來建模，有以下表達式：　　

F1表示F的第一列。只要誤差小于閾值，都認為該點符合 F 方程。

算法流程如下：1、隨機取8個點；2、估計F；3、計算所有點的e，并求#inlier；4、回到1，2，3，如果#inlier變多則更新F_candidate；5、迭代很多次結束，F_candidate 為F的估計值。

RANSAC算法又一次證明了其對噪聲超級好的控制能力。

（五）

之前說到，機器人視覺的核心是Estimation，求取特征并配準，也是為了Estimation做準備。一旦配準完成，我們就可以從圖像中估計機器人的位置，姿態。有了位置，姿態，我們可以把三維重建的東西進行拼接。

從視覺信息估計機器人位姿的問題可以分為三個大類：1、場景點在同一平面上。2、場景點在三維空間中。3、兩幅點云的配準。

所有問題有一個大前提就是知道相機內部矩陣K。

1、由單應矩陣進行位姿估計

單應矩陣原指從 R2--R2 的映射關系。

但在估計問題中，如果我們能獲得這種映射關系，就可以恢復從世界坐標系 x_w 到相機坐標系 x_c 的變換矩陣。此變換矩陣表達了相機相對于x_w 的位姿。

H = s*K*[r1 r2 t] —— 假設平面上z坐標為0

s*[r1 r2 t] = k-1*H —— 利用單應矩陣求取旋轉與平移向量

r3 = r1×r2 —— 恢復r3

s 并不重要，只需要對k-1*h1 進行歸一化就能求出來。

所以，最重要的就是如何求取兩個場景中的單應。在前面我提過從消失點來求取單應關系，但是如果不是從長方形 --- 四邊形的映射，我們并沒有消失點可以找。

這里要介紹的是一種優雅到爆棚的方法。基于矩陣變換與奇異值分解。JB SHI真不愧大牛。三兩句就把這個問題講的如此簡單。　　

由于H矩陣一共有8個自由度，每一對單應點可以提供兩個方程，所以4個單應點就可以唯一確定單應矩陣H。Ax = 0，我們在擬合一章中已經了解過了。x 是最小奇異值對于的V矩陣的列。這里是奇異值分解的第一次出現。

至此，我們恢復了H矩陣。按照正常的思路就可以解除[r1 r2 t]了。但是，我們的H矩陣是用奇異值分解優化出來的，反解的r1 r2 并不一定滿足正交條件，也不一定滿足等長條件。所以，我們還要擬合一次RT矩陣。

此次的擬合目標是 min(ROS3 - R')。

其中R' = [k-1H(:,1:2)  x ]. 方法依舊是奇異值分解，R = UV'. 這是奇異值分解的第二次出現。 

2、由射影變換進行位姿估計

由單應矩陣進行位姿估計的前提是所有點都在一個平面上。而由射影變換進行位姿估計則舍棄了此前提，故上一節是本節的一個特例。此問題學名為PnP問題：perspective-n-point。

仿造上面的思路，我們依舊可以寫成以下形式：　　

此處射影矩陣一共有12個未知數，9來自旋轉矩陣，3來自平移向量。每個點可以提供2個方程。故只要6個場景點，我們就可以用奇異值分解獲得P矩陣的值。同樣，在獲得P矩陣后求T = k-1*P,最后利用奇異值分解修正T.

不過按照常理，此問題只有6個自由度（3平移，3旋轉）。我們使用6個點其實是一種dirty method。

3、由兩幅點云進行位姿估計

對于現在很火的RGBD相機而言，可能這種情況會比較多。從不同角度獲得了同一物體的三維圖像，如何求取兩個位姿之間的變換關系。這個問題有解析解的前提是點能夠一一對應上。如果點不能一一對應，那就是ICP算法問題了。

此問題學名為：Procrustes Problem。來自希臘神話。用中文來比喻的話可以叫穿鞋問題。如何對腳進行旋轉平移，最后塞進鞋里。其數學描述如下:通過選擇合適的R，T，減小AB之間的差別。　　

T 其實很好猜，如果兩個點團能重合，那么其重心肯定是重合的。所以T代表兩個點團重心之間的向量。此問題則有如下變形：　　

由矩陣分析可知，向量的2范數有以下變形：

由矩陣分析可知，最后兩項實際上是相等的（跡的循環不變性與轉置不變性）

那么優化目標又可以轉為：　　

跡是和奇異值相關的量（相似變換跡不變）

顯然，如果Z的跡盡可能大，那么只有一種情況，Z是單位陣，單位陣的跡是旋轉矩陣里最大的。所以R的解析解如下：　

　

至此，我們獲得了3D--3D位姿估計的解析解！

（六）

最后一個話題是Bundle Adjustment. 機器人視覺學中，最頂尖的方法。

1、基于非線性優化的相機位姿估計

之前已經在擬合一篇中，已經補完了非線性最小二乘擬合問題。Bundle Adjustment，中文是光束平差法，就是利用非線性最小二乘法來求取相機位姿，三維點坐標。在僅給定相機內部矩陣的條件下，對四周物體進行高精度重建。Bundle Adjustment的優化目標依舊是最小重復投影誤差。　　

與利用non-linear mean square 解三角同，bundle adjustment 中所有的參數，RCX均為變量。N幅圖則有N個位姿，X個點，我們會得到非常大的jacobbian Matrix.本質上，需要使用雅克比矩陣進行梯度下降搜索。詳細見之前介紹過的“擬合”篇。

2、雅克比矩陣

雅克比矩陣的行代表信息，列代表約束。

每一行是一個點在該位姿下的誤差，每一列代表f對x分量的偏導數。

q x c 均為變量，q是旋轉四元素，x 是三維點空間坐標，c 是相機光心在世界坐標系下的坐標。J 可以分為三部分，前4列代表對旋轉求導，中間三列代表對c求導，最后三列代表對x求導。其中，對旋轉求導又可以分解為對旋轉矩陣求導X旋轉矩陣對四元素q求導。一旦獲得J的表達式，我們就可以使用Newton-Gaussian 迭代對x尋優了。求導后的數學表達式如下：

如果有兩個相機，則總的雅克比矩陣如下：

通過同時迭代所有的q C X ，最終可以同時得到世界點坐標，相機位姿 ——SLAM!