1、SIFT特征提取

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

2、SIFT特征描述

特征提取和特征描述實(shí)際上是兩碼事。在上一節(jié)中特征提取已經(jīng)結(jié)束了。假如有兩幅圖片，那么相同的特征點(diǎn)肯定會被找到。特征描述的作用是為匹配做準(zhǔn)備，其以特征點(diǎn)局部區(qū)域信息為標(biāo)準(zhǔn)，將兩幅圖中相同的特征點(diǎn)聯(lián)系起來。特征的本質(zhì)是一個高維向量。要求尺度不變，旋轉(zhuǎn)不變。

1、最小二乘法擬合

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

2、RASAC擬合　

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

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

3、非線性擬合

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

　

　　

　　

　　

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

1、極幾何的研究前提

極幾何的研究對象是兩幅有重疊區(qū)域圖像。研究目標(biāo)是提取相機(jī)拍攝位姿之間的關(guān)系。一旦得到兩次拍攝位姿之間的關(guān)系，我們就可以對場景點(diǎn)進(jìn)行三維重建。

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

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

極點(diǎn)的本質(zhì)是另一臺相機(jī)光心在本圖像上的映射點(diǎn)。極線的本質(zhì)是另一臺相機(jī)光線在本圖像上的映射線。（極點(diǎn)和極線都是在圖像上的）

1.1、本征矩陣

本征矩陣攜帶了相機(jī)相對位置信息。其推導(dǎo)如下：

在相機(jī)2的坐標(biāo)系中，場景點(diǎn)坐標(biāo)：X2 = RX1+ t

相機(jī)1光心坐標(biāo)：t

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

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

其中，tx  代表 t 的叉乘矩陣。tx R 稱為本征矩陣E. 兩幅圖片一旦拍攝完成R與T都是確定的?？臻g中任何一組對應(yīng)點(diǎn)都必須滿足本征矩陣！

1.2、基本矩陣

空間中的點(diǎn)滿足E矩陣，則該點(diǎn)坐標(biāo)Zoom后，仍然必須滿足E矩陣。坐標(biāo)的Zoom顯然和相機(jī)內(nèi)部矩陣有關(guān)。

在相機(jī)坐標(biāo)系下：

x1 = KX1; x2 = KX2

其中，x1 ，x2 是齊次像素坐標(biāo)。那么，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  稱為基本矩陣。

基本矩陣所接受的是齊次像素坐標(biāo)。

基本矩陣的秩是2，因?yàn)樗?空間。同時，其自由度是8，因?yàn)樗邮艿氖驱R次坐標(biāo)。每組圖像點(diǎn)可以提供1個方程，所以由8組點(diǎn)就可以線性解出F矩陣。當(dāng)然，解法是化成Ax = 0，然后使用奇異值分解取v的最后一列。然后2次奇異值分解去掉最小奇異值正則化。

1.3、極點(diǎn)與極線

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

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

極點(diǎn)是多條極線的交點(diǎn)（最少兩條）

2、由本征矩陣恢復(fù)R,T

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

E的秩為2，因?yàn)槠溆?空間。同時，由于r1 r2 r3 是正交的，所以其叉乘之后必然也是正交的。所以不妨假設(shè)其叉乘完之后依然滿足旋轉(zhuǎn)矩陣的某些性質(zhì)。比如：每一列，模相等。

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

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

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

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

3、由空間位置關(guān)系恢復(fù)三維坐標(biāo)

在已知標(biāo)定信息，兩相機(jī)位置關(guān)系的情況下，就已知了兩個相機(jī)的投影矩陣P，對于空間中一點(diǎn)X1，有以下關(guān)系：

x1　=  P*X1

[x1]x P X1 = 0;

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

4、由RANSAC求 F 矩陣

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

SIFT算法提供了一種自動匹配的可能性，然而，匹配結(jié)果還有很多誤匹配的點(diǎn)。本節(jié)的目標(biāo)是利用RANSAC作為算法基礎(chǔ)，基礎(chǔ)矩陣作為方法，來對匹配結(jié)果進(jìn)行判斷。

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

F1表示F的第一列。只要誤差小于閾值，都認(rèn)為該點(diǎn)符合 F 方程。

算法流程如下：1、隨機(jī)取8個點(diǎn)；2、估計(jì)F；3、計(jì)算所有點(diǎn)的e，并求#inlier；4、回到1，2，3，如果#inlier變多則更新F_candidate；5、迭代很多次結(jié)束，F(xiàn)_candidate 為F的估計(jì)值。

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

（五）

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

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

所有問題有一個大前提就是知道相機(jī)內(nèi)部矩陣K。

1、由單應(yīng)矩陣進(jìn)行位姿估計(jì)

單應(yīng)矩陣原指從 R2--R2 的映射關(guān)系。

但在估計(jì)問題中，如果我們能獲得這種映射關(guān)系，就可以恢復(fù)從世界坐標(biāo)系 x_w 到相機(jī)坐標(biāo)系 x_c 的變換矩陣。此變換矩陣表達(dá)了相機(jī)相對于x_w 的位姿。

H = s*K*[r1 r2 t] —— 假設(shè)平面上z坐標(biāo)為0

s*[r1 r2 t] = k-1*H —— 利用單應(yīng)矩陣求取旋轉(zhuǎn)與平移向量

r3 = r1×r2 —— 恢復(fù)r3

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

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

這里要介紹的是一種優(yōu)雅到爆棚的方法?；诰仃囎儞Q與奇異值分解。JB SHI真不愧大牛。三兩句就把這個問題講的如此簡單。　　

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

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

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

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

2、由射影變換進(jìn)行位姿估計(jì)

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

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

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

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

3、由兩幅點(diǎn)云進(jìn)行位姿估計(jì)

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

此問題學(xué)名為：Procrustes Problem。來自希臘神話。用中文來比喻的話可以叫穿鞋問題。如何對腳進(jìn)行旋轉(zhuǎn)平移，最后塞進(jìn)鞋里。其數(shù)學(xué)描述如下:通過選擇合適的R，T，減小AB之間的差別?！　?/span>

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

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

由矩陣分析可知，最后兩項(xiàng)實(shí)際上是相等的（跡的循環(huán)不變性與轉(zhuǎn)置不變性）

那么優(yōu)化目標(biāo)又可以轉(zhuǎn)為：　　

跡是和奇異值相關(guān)的量（相似變換跡不變）

顯然，如果Z的跡盡可能大，那么只有一種情況，Z是單位陣，單位陣的跡是旋轉(zhuǎn)矩陣?yán)镒畲蟮摹Ｋ訰的解析解如下：　

　

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

（六）

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

1、基于非線性優(yōu)化的相機(jī)位姿估計(jì)

之前已經(jīng)在擬合一篇中，已經(jīng)補(bǔ)完了非線性最小二乘擬合問題。Bundle Adjustment，中文是光束平差法，就是利用非線性最小二乘法來求取相機(jī)位姿，三維點(diǎn)坐標(biāo)。在僅給定相機(jī)內(nèi)部矩陣的條件下，對四周物體進(jìn)行高精度重建。Bundle Adjustment的優(yōu)化目標(biāo)依舊是最小重復(fù)投影誤差?！　?/span>

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

2、雅克比矩陣

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

每一行是一個點(diǎn)在該位姿下的誤差，每一列代表f對x分量的偏導(dǎo)數(shù)。

q x c 均為變量，q是旋轉(zhuǎn)四元素，x 是三維點(diǎn)空間坐標(biāo)，c 是相機(jī)光心在世界坐標(biāo)系下的坐標(biāo)。J 可以分為三部分，前4列代表對旋轉(zhuǎn)求導(dǎo)，中間三列代表對c求導(dǎo)，最后三列代表對x求導(dǎo)。其中，對旋轉(zhuǎn)求導(dǎo)又可以分解為對旋轉(zhuǎn)矩陣求導(dǎo)X旋轉(zhuǎn)矩陣對四元素q求導(dǎo)。一旦獲得J的表達(dá)式，我們就可以使用Newton-Gaussian 迭代對x尋優(yōu)了。求導(dǎo)后的數(shù)學(xué)表達(dá)式如下：

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

通過同時迭代所有的q C X ，最終可以同時得到世界點(diǎn)坐標(biāo)，相機(jī)位姿 ——SLAM!