提及機(jī)器人視覺，不免會想到計算機(jī)視覺和機(jī)器視覺，很多人會把這三者弄混。

　　計算機(jī)視覺是以圖片認(rèn)知為基礎(chǔ)的科學(xué)，只通過圖片識別輸出結(jié)果，代表企業(yè)是谷歌。

　　機(jī)器視覺多用于生產(chǎn)線上的質(zhì)量檢測，普遍基于2D識別，被廣泛應(yīng)用于3C電子行業(yè)，代表企業(yè)是康耐視。

　　機(jī)器人視覺是指不僅要把視覺信息作為輸入，而且還要對這些信息進(jìn)行處理，進(jìn)而提取出有用的信息提供給機(jī)器人。是為了讓機(jī)器人真正變成“機(jī)器人”，而不是機(jī)器臂。

　　(一)

　　傳統(tǒng)的機(jī)器臂只是自動化設(shè)備，是通過編程處理固定的動作，是不能處理具有變動性事物的能力。機(jī)器人視覺這要求機(jī)器人要擁有3D視覺，能處理三維空間里的三維物體問題，并且具有復(fù)雜算法，支撐機(jī)器人對位置、動作、軌跡等復(fù)雜信息的捕捉，這必須要依賴人工智能和深度學(xué)習(xí)來完成。

　　機(jī)器人視覺是為認(rèn)知機(jī)器人服務(wù)，具備不斷學(xué)習(xí)的功能尤為關(guān)鍵，無論是做檢測還是定位引導(dǎo)，當(dāng)機(jī)器人做的次數(shù)越多，伴隨著數(shù)據(jù)的增長變化，機(jī)器人的準(zhǔn)確性也會越高，這跟人的學(xué)習(xí)成長能力是類似的。

　　機(jī)器人視覺是一種處理問題的研究手段。經(jīng)過長時間的發(fā)展，機(jī)器人視覺在定位，識別，檢測等多個方面發(fā)展出來各種方法。其以常見的相機(jī)作為工具，以圖像作為處理媒介，獲取環(huán)境信息。

　　1、相機(jī)模型

　　相機(jī)是機(jī)器人視覺的主要武器，也是機(jī)器人視覺和環(huán)境進(jìn)行通信的媒介。相機(jī)的數(shù)學(xué)模型為小孔模型，其核心在于相似三角形的求解。其中有三個值得關(guān)注的地方： 1.11/f = 1/a + 1/b

　　焦距等于物距加上像距。此為成像定理，滿足此條件時才能成清晰的像。 1.2X = x * f/Z

　　如果連續(xù)改變焦距f ，并同時移動相機(jī)改變Z，則可以使得物體x在圖像上所占像素數(shù)目不變(X)。此為DollyZoom原理。如果某個物體在該物體后方(更大的Z)，可利用此原理任意調(diào)整兩個物體在相片上的比例。 1.3

　　焦距越長，則視場越小，可以將遠(yuǎn)處的物體拍清晰。同時相片會有更大的景深。

　　2、消失點

　　消失點是相片中特有的。此點在相片中不直接存在，在現(xiàn)實中直接不存在。由于射影變換，相片中原本平行的線會有相交的趨勢。如果求的平行直線在圖像中的交點，則該點對應(yīng)現(xiàn)實中無窮遠(yuǎn)處的一點。該點的圖像坐標(biāo)為[X1 X1 1]。此點成為消失點。相機(jī)光心與消失點的連線指向消失點在攝像機(jī)坐標(biāo)系中的方向。

　　此外，同一平面上各個方向的消失點，會在圖像中組成一條直線，稱為水平線。該原理可以用于測量站在地上的人的高度。值得注意的是只有相機(jī)水平時，horizen的高度才是camera Height.

　　2.1 位姿估計

　　如果我們能獲得一幅圖中的2個消失點。且這2個消失點所對應(yīng)的方向是相互垂直的(網(wǎng)格)，那么我們就可以估計出相機(jī)相對于此圖像的姿態(tài)(靶標(biāo)位姿估計)。 在獲得相機(jī)相對于靶標(biāo)的旋轉(zhuǎn)向量后，如果相機(jī)內(nèi)部參數(shù)已知，且已知射影變換矩陣，則可計算相機(jī)相對于靶標(biāo)的距離，那么可以估計機(jī)器人的位置。H = K^-1*(H射影矩陣)

　　2.2 點線對偶

　　p1×p2 = L12 L12×L23 = p2

　　3、射影變換

　　射影變化是空間中平面---》平面的一種變換。對齊次坐標(biāo)，任意可逆矩陣H均表達(dá)了射影變換。簡而言之，可以表達(dá)為A = HB ，其中AB是[X Y 1]形式的其次坐標(biāo)。射影變換的一大作用就是將某一形狀投射成其他形狀。比如，制作相片中的廣告牌，或者比賽轉(zhuǎn)播中的廣告牌，或者游泳比賽運動員到達(dá)后那個biu的一下出現(xiàn)的國旗。射影變換也是增強(qiáng)現(xiàn)實技術(shù)的基礎(chǔ)。

　　射影變換的核心在于H的求取。普通的求解方法見機(jī)器視覺教材。

　　假設(shè)平面相片的四個點分別是A(0，0，1)，B(0，1，1)，C(1，1，1)，D(1，0，1)。顯然，這四個點需要投射到四個我們已知像素位坐標(biāo)的圖像區(qū)域中。

　　此外，我們還可以依據(jù)像素位置計算兩個有趣的點，V1(x1， y1， z1)，V2(x2，y2，z2)，這兩個點都是圖像點。他們對應(yīng)的實際坐標(biāo)假設(shè)是(0，1，0)，(1，0，0)。那么我們就有三個很有趣的實際點了。分別是(1，0，0)，(0，1，0)，(0，0，1)，恰好是一個Identity Matrix。這三個實際坐標(biāo)經(jīng)過射影變換會得到像素坐標(biāo)。像素坐標(biāo)又是已知的。那么H的第一列就應(yīng)該對應(yīng)beta*V2，第二列應(yīng)該對應(yīng)alpha*V1。

　　第三列應(yīng)該對應(yīng)gama*【A的像素坐標(biāo)】。alpha beta gama是常數(shù)?！旧溆白兓蟮淖鴺?biāo)應(yīng)為常數(shù)乘以其次坐標(biāo)】。

　　如果能解得alpha beta gama，那么我們就獲得了射影變換矩陣。顯然把C點的像素坐標(biāo)帶入方程，我們則有3個方程，4個未知數(shù)(引入了一個lamda)。但是lamda并不影響，除過去后我們只要

　　alpha/lamda，beta/lamda，gama/lamda當(dāng)作未知數(shù)即可解除射影矩陣。

　　所以，射影變換矩陣的第一列代表消失點V1，第二列代表消失點V2，第一列與第二列的叉乘，代表水平線方程(點線對偶)。

　　(二)

　　上回介紹了機(jī)器人視覺的一些基礎(chǔ)信息，說到機(jī)器人視覺的核心任務(wù)是estimation，理論框架是射影幾何理論。然而，整個estimation的首要條件是已知像素點坐標(biāo)，尤其是多幅圖中對應(yīng)點的像素坐標(biāo)。 單幅圖像的處理方法不贅述，想講講不變點檢測與不變特征。由于機(jī)器人在不斷運動，所以可能從不同方向?qū)ν晃矬w進(jìn)行拍攝。而拍攝的距離有遠(yuǎn)近，角度有titled. 由于射影變換本身的性質(zhì)，無法保證兩幅圖中的物體看上去一樣。所以我們需要一種特征提取方法(特征點檢測)，能夠保證檢測是旋轉(zhuǎn)，縮放不變的。除此之外還要一種特征描述方法，同樣對旋轉(zhuǎn)和縮放不變。

　　1、SIFT特征提取

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

　　多尺度卷積的作用是構(gòu)造一個由近及遠(yuǎn)的圖像。金字塔則由下采樣進(jìn)行構(gòu)造。

　　對于不同尺度的圖像同一個像素，我們可以跟蹤它“灰度”的變化。我們發(fā)現(xiàn)，如果某一點對不同 sigma 的模版響應(yīng)是不同的，最大響應(yīng)(卷積后的灰度)所對應(yīng)的scale 成為該點本征scale。這有點像對一個機(jī)械結(jié)構(gòu)給不同頻率的激勵，某一頻率下會發(fā)生共振，我們可以記錄此頻率一定程度上代表了此結(jié)構(gòu)(單擺頻率只和ml有關(guān)，有了f就可以重現(xiàn)系統(tǒng))。 所以，我們只要找到一個合適的模版(激勵方式)，再找到最大響應(yīng)，就可以獲取圖片中各個點的 Intrinsic Scale(本征尺度)。同一物體在不同距離拍攝后，都會統(tǒng)一在Intrinsic Scale下進(jìn)行響應(yīng)。由此解決了尺度不變的問題。 3D非極大值抑制是指在某點的3*3*3鄰域內(nèi)，僅取最大響應(yīng)，作為特征點。由于該點是空間鄰域中響應(yīng)最強(qiáng)的，所以該點也是旋轉(zhuǎn)不變的。從各個方向看，該點響應(yīng)最強(qiáng)。

　　2、SIFT特征描述

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

　　這里所使用的是HOG特征。特征描述可以分為兩步：(1)局部主方向確定;(2)計算梯度直方圖。 以sigma作為特征描述選擇范圍是一個合理的想法，因為sigma描述了尺度，特征點位置+尺度 = 特征點所代局部信息。在此基礎(chǔ)上，統(tǒng)計其領(lǐng)域內(nèi)所有像素的梯度方向，以方向統(tǒng)計直方圖作為特征向量，至此完成HOG特征構(gòu)造。重要的是，在統(tǒng)計方向之前，需要把圖像主方向和X軸方向?qū)R。示意圖如下：

　　圖中黃色的有點像時鐘的東西是特征點+scale，指針代表該片小圖像的主方向(PCA)。綠色的是直方圖的bin，用于計算特征向量。 最后，我們只要匹配特征向量就可以得到 圖像1 --- 圖像2 的對應(yīng)點對，通過單應(yīng)矩陣的計算就可以將兩幅圖拼接在一起。如果已知標(biāo)定信息則可進(jìn)行3D reconstruction。 (三)上篇文章說到從場景中提取特征點，并且對不同角度中的特征點進(jìn)行匹配。這次要先介紹一個工具 —— 擬合。 擬合本質(zhì)上是一個優(yōu)化問題，對于優(yōu)化問題，最基本的是線性最小二乘法。換言之，我們需要保證擬合誤差最小。

　　1、最小二乘法擬合

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

　　第一類以 因變量 誤差作為優(yōu)化目標(biāo)，該類問題往往是自變量---因變量模式，xy的單位不同。 第二類以 距離 作為優(yōu)化目標(biāo)，該類問題xy的單位往往相同，直線不代表趨勢，而是一種幾何模型。 由于優(yōu)化目標(biāo)不同，故建模方式與解均不同，但是解法思路是一樣的，都是講求和化作向量的模。而向量又是矩陣的運算結(jié)果，最終化為奇異值分解問題。

　　2、RASAC擬合

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

　　如果用最小二乘法是無法得到這樣的效果的，直線大約會在圖中直線偏上一點。關(guān)于隨機(jī)采樣一致性算法的原理，在wiki百科上講的很清楚，甚至給出了偽代碼和matlab，C代碼，想換一個不那么嚴(yán)肅或者說不那么學(xué)術(shù)的方式來解釋這個算法。 實際上這個算法就是從一堆數(shù)據(jù)里挑出自己最心儀的數(shù)據(jù)。所謂心儀當(dāng)然是有個標(biāo)準(zhǔn)(目標(biāo)的形式：滿足直線方程?滿足圓方程?以及能容忍的誤差e)。平面中確定一條直線需要2點，確定一個圓則需要3點。隨機(jī)采樣算法，其實就和小女生找男朋友差不多。

　　從人群中隨便找個男生，看看他條件怎么樣，然后和他談戀愛，(平面中隨機(jī)找兩個點，擬合一條直線，并計算在容忍誤差e中有多少點滿足這條直線)

　　第二天，再重新找個男生，看看他條件怎么樣，和男朋友比比，如果更好就換新的(重新隨機(jī)選兩點，擬合直線，看看這條直線是不是能容忍更多的點，如果是則記此直線為結(jié)果)

　　第三天，重復(fù)第二天的行為(循環(huán)迭代)

　　終于到了某個年齡，和現(xiàn)在的男朋友結(jié)婚(迭代結(jié)束，記錄當(dāng)前結(jié)果)

　　顯然，如果一個女生按照上面的方法找男朋友，最后一定會嫁一個好的(我們會得到心儀的分割結(jié)果)。只要這個模型在直觀上存在，該算法就一定有機(jī)會把它找到。優(yōu)點是噪聲可以分布的任意廣，噪聲可以遠(yuǎn)大于模型信息。 這個算法有兩個缺點，第一，必須先指定一個合適的容忍誤差e。第二，必須指定迭代次數(shù)作為收斂條件。 綜合以上特性，本算法非常適合從雜亂點云中檢測某些具有特殊外形的物體。

　　3、非線性擬合

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

　　在三維重建中，如果我們有2個以上視角，那么三條線很可能是不交于一點的。原因是我們選擇的旋轉(zhuǎn)矩陣有精度表達(dá)問題，位姿估計也存在誤差。使用奇異值分解的方法是求得到三條線距離最小的點，還有一種合適的估計，是使得該點在三個相機(jī)上的重復(fù)投影誤差最小。同時，R，T，P(X，Y，Z)進(jìn)行估計，最終保證Reprojection err 最小的方法————the state of the art BUNDLE ADJUST. 先回到最原始的問題，如何求解非線性最小二乘法。

　　由線性最小二乘法，我們可以得到非線性最小二乘法矩陣表達(dá)形式。如果要求得其局部最小值，則對 x 求導(dǎo)后，導(dǎo)數(shù)應(yīng)為 0。

　　然而，這個東西并不好解，我們考慮使用梯度下降迭代的方式。這里使用的是單純的梯度。

　　這里有個非常不好理解的地方，其假設(shè)detaX非常小，故表示成上述形式，以保證 f(x + deta_X)《f(x) ， 只要依次迭代 x 就能保證每次都向著f(x)減小的方向移動。實際上，這個解應(yīng)該由hessian矩陣給出?！?span》

　　以信標(biāo)定位為例。講道理，兩個信標(biāo)為圓心畫圓應(yīng)該給出位置的兩個解析解。但是如果有很多信標(biāo)，那么信標(biāo)就會畫出一塊區(qū)域。..。..。.這是SLAM里的經(jīng)典問題了，后面會有博客專門講BUNDLE ADJUST.

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

　　1、極幾何的研究前提

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

　　極幾何定義的物理量包括4個：1、極點;2、極線;3、基本矩陣;4、本征矩陣;定義如左圖。 極幾何研究的物理量包括4個：C1坐標(biāo)，C2坐標(biāo)，R，T，定義如右圖。

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

　　1.1、本征矩陣

　　本征矩陣攜帶了相機(jī)相對位置信息。其推導(dǎo)如下： 在相機(jī)2的坐標(biāo)系中，場景點坐標(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)點都必須滿足本征矩陣!

　　1.2、基本矩陣

　　空間中的點滿足E矩陣，則該點坐標(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，因為它有0空間。同時，其自由度是8，因為它接受的是齊次坐標(biāo)。每組圖像點可以提供1個方程，所以由8組點就可以線性解出F矩陣。當(dāng)然，解法是化成Ax = 0，然后使用奇異值分解取v的最后一列。然后2次奇異值分解去掉最小奇異值正則化。

　　1.3、極點與極線

　　從基本矩陣可知：x2 T F x1 = 0 顯然這里有熟悉的身影，由點線對偶可知，x2 在直線 F x1 上。該直線是極線在圖像2上的方程。x1 在直線 x2 T F 。該直線是極線在圖像1上的方程。 極點是多條極線的交點(最少兩條)

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

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

　　E的秩為2，因為其有0空間。同時，由于r1 r2 r3 是正交的，所以其叉乘之后必然也是正交的。所以不妨假設(shè)其叉乘完之后依然滿足旋轉(zhuǎn)矩陣的某些性質(zhì)。比如：每一列，模相等。 由 tT E = 0 可知，對E奇異值分解之后，t 為最小奇異值所對應(yīng)的 u(：，end)。 如下：

　　這里假設(shè)了 R = UYVT 。因為U，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，對于空間中一點X1，有以下關(guān)系： x1= P*X1 [x1]x P X1 = 0; 顯然，我們又有了Ax = 0的神奇形式。奇異值分解搞定之。

　　4、由RANSAC求 F 矩陣

　　有了8個對應(yīng)點，我們就可以求得F矩陣，再加上K，我們就可以對兩幅圖片進(jìn)行三維重建。然而想要自動的求取8個對應(yīng)點還是有一定難度。 SIFT算法提供了一種自動匹配的可能性，然而，匹配結(jié)果還有很多誤匹配的點。本節(jié)的目標(biāo)是利用RANSAC作為算法基礎(chǔ)，基礎(chǔ)矩陣作為方法，來對匹配結(jié)果進(jìn)行判斷。 首先，由于檢測誤差等因素，像素點不可能恰好滿足基本方程。所以點到極線會有一定的距離。我們采用垂直距離來建模，有以下表達(dá)式：

　　F1表示F的第一列。只要誤差小于閾值，都認(rèn)為該點符合 F 方程。 算法流程如下：1、隨機(jī)取8個點;2、估計F;3、計算所有點的e，并求#inlier;4、回到1，2，3，如果#inlier變多則更新F_candidate;5、迭代很多次結(jié)束，F(xiàn)_candidate 為F的估計值。 RANSAC算法又一次證明了其對噪聲超級好的控制能力。

　　(五)之前說到，機(jī)器人視覺的核心是Estimation，求取特征并配準(zhǔn)，也是為了Estimation做準(zhǔn)備。一旦配準(zhǔn)完成，我們就可以從圖像中估計機(jī)器人的位置，姿態(tài)。有了位置，姿態(tài)，我們可以把三維重建的東西進(jìn)行拼接。 從視覺信息估計機(jī)器人位姿的問題可以分為三個大類：1、場景點在同一平面上。2、場景點在三維空間中。3、兩幅點云的配準(zhǔn)。 所有問題有一個大前提就是知道相機(jī)內(nèi)部矩陣K。

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

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

　　但在估計問題中，如果我們能獲得這種映射關(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)。在前面我提過從消失點來求取單應(yīng)關(guān)系，但是如果不是從長方形 --- 四邊形的映射，我們并沒有消失點可以找。 這里要介紹的是一種優(yōu)雅到爆棚的方法?；诰仃囎儞Q與奇異值分解。JB SHI真不愧大牛。三兩句就把這個問題講的如此簡單。

　　由于H矩陣一共有8個自由度，每一對單應(yīng)點可以提供兩個方程，所以4個單應(yīng)點就可以唯一確定單應(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)行位姿估計

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

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

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

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

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

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

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

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

　　由矩陣分析可知，最后兩項實際上是相等的(跡的循環(huán)不變性與轉(zhuǎn)置不變性) 那么優(yōu)化目標(biāo)又可以轉(zhuǎn)為：

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

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

　　至此，我們獲得了3D--3D位姿估計的解析解! (六)最后一個話題是Bundle Adjustment. 機(jī)器人視覺學(xué)中，最頂尖的方法。

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

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

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

　　2、雅克比矩陣

　　雅克比矩陣的行代表信息，列代表約束。 每一行是一個點在該位姿下的誤差，每一列代表f對x分量的偏導(dǎo)數(shù)。

　　q x c 均為變量，q是旋轉(zhuǎn)四元素，x 是三維點空間坐標(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 ，最終可以同時得到世界點坐標(biāo)，相機(jī)位姿 ——SLAM!