在OpenCV中使用單應(yīng)性進(jìn)行姿勢(shì)估計(jì)

重磅干貨，第一時(shí)間送達(dá)

單應(yīng)性是一種平面關(guān)系，可將點(diǎn)從一個(gè)平面轉(zhuǎn)換為另一個(gè)平面。它是3乘3的矩陣，轉(zhuǎn)換3維向量表示平面上的2D點(diǎn)。這些向量稱為同構(gòu)坐標(biāo)，下面將進(jìn)行討論。下圖說(shuō)明了這種關(guān)系。這四個(gè)點(diǎn)在紅色平面和圖像平面之間相對(duì)應(yīng)。單應(yīng)性存儲(chǔ)相機(jī)的位置和方向，這可以通過(guò)分解單應(yīng)性矩陣來(lái)檢索。

針孔相機(jī)型號(hào)

針孔相機(jī)模型是相機(jī)的數(shù)學(xué)表示。它接收3D點(diǎn)并將其投影到像上圖所示的圖像平面。該模型的一些重要參數(shù)是焦點(diǎn)，像平面（上圖中的灰度平面），主點(diǎn)（上圖中的像面上的粗體點(diǎn)），焦距（像平面與圖之間的距離）。焦點(diǎn)）和光軸（垂直于穿過(guò)焦點(diǎn)的像平面的線）。該變換可以在投影矩陣中編碼，該投影矩陣將表示3D點(diǎn)的4維均勻向量轉(zhuǎn)換為表示平面上2D點(diǎn)的3維均勻向量。

同構(gòu)坐標(biāo)是表示計(jì)算機(jī)視覺(jué)中的點(diǎn)的投影坐標(biāo)。由于拍攝照片時(shí)會(huì)從3D轉(zhuǎn)換為2D，因此深度范圍會(huì)丟失。因此，可以將無(wú)限數(shù)量的3D點(diǎn)投影到相同的2D點(diǎn)，從而使均勻坐標(biāo)在描述時(shí)可以通用，因?yàn)樗鼈兊谋壤嗨啤｝R次坐標(biāo)只需采用正常的笛卡爾坐標(biāo)，并將維數(shù)增大到末端。

均勻坐標(biāo)在比例上也相等。

給定同構(gòu)坐標(biāo)，將所有元素除以矢量的最后一個(gè)元素（比例因子），然后笛卡爾坐標(biāo)是一個(gè)矢量，該矢量由除最后一個(gè)元素之外的所有元素組成。

投影矩陣是與相機(jī)屬性相關(guān)的其他兩個(gè)矩陣的乘積。它們是外部和內(nèi)部相機(jī)矩陣。這些矩陣分別存儲(chǔ)攝像機(jī)的外部參數(shù)和固有參數(shù)（因此命名）。

投影矩陣

外在矩陣

外在矩陣存儲(chǔ)攝像機(jī)在全局空間中的位置。該信息存儲(chǔ)在旋轉(zhuǎn)矩陣以及平移矢量中。旋轉(zhuǎn)矩陣存儲(chǔ)相機(jī)的3D方向，而平移矢量將其位置存儲(chǔ)在3D空間中。

旋轉(zhuǎn)矩陣

然后將旋轉(zhuǎn)矩陣和平移向量連接起來(lái)以創(chuàng)建外部矩陣。從功能上講，外部矩陣將3D同構(gòu)坐標(biāo)從全局坐標(biāo)系轉(zhuǎn)換為相機(jī)坐標(biāo)系。因此，所有變換后的矢量將相對(duì)于焦點(diǎn)在空間中表示相同的位置。

內(nèi)在矩陣

本征矩陣存儲(chǔ)相機(jī)的特征，例如焦距和主點(diǎn)。焦距（f 1和f 6）是從焦點(diǎn)到像面的距離，可以用像素寬度或像素高度（因此為何有2個(gè)焦距）來(lái)度量。每個(gè)像素都不是一個(gè)完美的正方形，因此每個(gè)邊都有不同的邊長(zhǎng)。主點(diǎn)（c?和c?）是光軸和像平面（像平面的功能中心）的交點(diǎn)。該矩陣將相對(duì)于焦點(diǎn)的3D坐標(biāo)轉(zhuǎn)換到圖像平面上。可以將其視為拍攝照片的矩陣。與外在矩陣結(jié)合使用時(shí)，將創(chuàng)建針孔相機(jī)模型。

針孔相機(jī)型號(hào)

現(xiàn)在單應(yīng)性是針孔相機(jī)模型的一種特殊情況，其中投影到相機(jī)上的所有現(xiàn)實(shí)世界坐標(biāo)都位于z坐標(biāo)為0的平面上。這是單應(yīng)性的派生詞。

H是單應(yīng)性矩陣，這是一個(gè)3 x 3的矩陣，可將點(diǎn)從一個(gè)平面轉(zhuǎn)換為另一個(gè)平面。在這里，轉(zhuǎn)換是在Z = 0的平面和指向該點(diǎn)的圖像平面之間進(jìn)行的投影。單應(yīng)性矩陣通常通過(guò)4點(diǎn)算法求解。本質(zhì)上，它使用來(lái)自2個(gè)平面的4個(gè)點(diǎn)對(duì)應(yīng)來(lái)求解單應(yīng)矩陣。在OpenCV中，我們可以使用cv2.findHomography方法找到單應(yīng)矩陣：

cv2.findHomography(<points from plane1>, <points from plane 2>)

此方法需要某種形式的特征點(diǎn)跟蹤，以便實(shí)現(xiàn)上面方法的結(jié)果。坐標(biāo)測(cè)量的質(zhì)量將有助于上述方法的準(zhǔn)確性。一旦有了單應(yīng)性矩陣，就可以將其分解為攝像機(jī)的平移和旋轉(zhuǎn)。

我們可以通過(guò)將求解矩陣中的前兩列用作旋轉(zhuǎn)矩陣中的前兩列，然后使用叉積來(lái)找到旋轉(zhuǎn)矩陣的最后一列，從而得出旋轉(zhuǎn)矩陣。

'''H is the homography matrixK is the camera calibration matrixT is translationR is rotation'''H = H.Th1 = H[0]h2 = H[1]h3 = H[2]K_inv = np.linalg.inv(K)L = 1 / np.linalg.norm(np.dot(K_inv, h1))r1 = L * np.dot(K_inv, h1)r2 = L * np.dot(K_inv, h2)r3 = np.cross(r1, r2)T = L * (K_inv @ h3.reshape(3, 1))R = np.array([[r1], [r2], [r3]])R = np.reshape(R, (3, 3))

使用Homography比其他算法簡(jiǎn)單得多，因?yàn)樗浅：?jiǎn)單直觀。利用基本矩陣或基本矩陣的其他方法需要復(fù)雜的算法和更多的實(shí)現(xiàn)精力。由于所有視覺(jué)定位方法都在做相同的事情，因此最好在可能的情況下使用Homography，以節(jié)省時(shí)間和精力。

由于僅在Z坐標(biāo)等于0的情況下才有可能進(jìn)行同構(gòu)照相，因此僅在所需目標(biāo)位于平面上的情況下才能使用同構(gòu)照相。否則，需要其他方法來(lái)定位，例如不具有此類約束的對(duì)極幾何。如果期望的目標(biāo)移出視野，則單應(yīng)性也將變得無(wú)用。結(jié)果，有必要對(duì)攝像機(jī)進(jìn)行定向，使其始終可以注視目標(biāo)，這在許多機(jī)器人上可能都不可行。