編輯：OAK中國
首發：oakchina.cn
喜歡的話，請多多👍⭐️✍
前言
Hello，大家好，這裡是OAK中國，我是助手君。
本文來自OAK人工智能俱樂部成員@阮PY，PDF下載下傳位址，代碼下載下傳位址。
在許多應用中，我們需要知道頭部相對于相機是如何傾斜的。例如，在虛拟現實應用程式中，可以使用頭部的姿勢來渲染場景的正确視圖。在駕駛員輔助系統中，汽車上的攝像頭可以觀察駕駛員的面部，通過頭部姿态估計來判斷駕駛員是否在關注道路。當然，人們也可以使用基于頭部姿勢的手勢來控制免提應用程式。
本文中我們約定使用下面術語，以免混淆。
位姿： 英文是pose， 包括位置和姿态。
位置： 英文是location 。
圖檔： 英文是photo ，本文中用來指一幅照片。
圖像： 英文是image， 本文中用在平面或坐标系中，例如image plane 指圖像平面， image coordinate system 指圖像坐标系統。
旋轉： 英文是rotation 。
平移： 英文是translation。
變換： 英文是transform 。
投影： 英文是project 。
1、什麼是位姿估計？
在計算機視覺中，物體的姿态是指物體相對于相機的相對方向和位置。你可以通過物體相對于相機移動，或者相機相對于物體移動來改變位姿。—— 這二者對于改變位姿是等價的，因為它們之間的關系是相對的。
本文中描述的位姿估計問題通常被稱為“Perspective-n-Point” 問題，或計算機視覺中的PnP 問題。PnP 問題的目标是找到一個物體的位姿，我們需要具備兩個條件：
條件1： 有一個已經校準了的相機；
條件2： 我們知道物體上的n 個3D 點的位置locations 和這些3D 點在圖像中相應的2D 投影。
2、如何在數學上描述相機的運動
一個3D 剛體(rigid object) 僅有2 種類型的相對于相機的運動。
第一種： 平移運動（Translation）。平移運動是指相機從目前的位置location 其坐标為（X， Y， Z）移動到新的坐标位置（X‘， Y’，Z‘）。平移運動有3 個自由度——各沿着X，Y，Z 三個軸的方向。平移運動可以用向量t = (X’-X, Y’-Y, Z’-Z)來描述。
第二種：旋轉運動（Rotation）。是指将相機繞着X，Y，Z 軸旋轉。旋轉運動也有3個自由度。有多種數學上的方法描述旋轉運動。使用歐拉角（橫搖roll, 縱搖pitch, 偏航yaw）描述， 使用3X3 的旋轉矩陣描述，或者使用旋轉方向和角度（directon of rotation and angle）。
是以，3D 物體的位姿估計其實就是指尋找描述平移和旋轉的6 個數。
3、進行位姿估計時你需要什麼？
為了計算一幅圖像中一個剛體的3D 位姿， 需要下面的資訊：
圖1：2D 點和3D 點必須一一對應
資訊1： 若幹個點的2D 坐标。你需要一幅圖像中若幹個點的2D（x, y）位置locations。在人的面部這個例子中，你可以選擇：眼角、鼻尖、嘴角等。在本文中，我們選擇：鼻尖、下巴、左眼角、右眼角、左嘴角、右嘴角等6 個點。
資訊2： 與2D 坐标點一一對應的3D 位置locations。你需要2D 特征點的3D 位置locations。你也許認為，你需要圖檔中那個人的3D 模型，以便獲得3D 位置locations。理想情況下的确如此，但是現實中，情況并非如此。一個3D 模型的确足夠了，但是你從哪裡搞到一個頭部的3D 模型呢？——我們并非需要一個完整的3D 模型！我們僅僅需要在某個任意參考系下若幹個點的3D 位置【Just need the 3D locations of a few
points in some arbitrary reference frame】。注意：這裡的“某個任意參考系in some arbitrary reference frame”是重點。
在本文中，我們選取的是如下3D 點：
（1）鼻尖(0.0, 0.0, 0.0)。X、Y、Z 坐标都為0.0 意味着這是原點。請思考，這是那個坐标系的原點呢？是相機坐标系？還是世界坐标系？
（2）下巴(0.0, -330.0, -65.0)。
（3）左眼角(-225.0, 170.0, -135.0) 。
（4）右眼角(225.0, 170.0, -135.0) 。
（5）左嘴角(-150.0, -150.0, -125.0) 。
（6）右嘴角(150.0, -150.0, -125.0) 。
注意： 上面的點是在“某個任意參考系/坐标系統in some arbitrary reference frame / coordinate system”中的，該坐标系就算所謂的“世界坐标系World Coordinates”。在OpenCV 文檔中，也被稱為“模型坐标系Model Coordinates”。
資訊3： 相機的内參。正如前文說提到的，在這個PnP 問題中，我們假定相機已經被标定了。換句話來說，你需要知道相機的焦距focal length、圖像的光學中心、徑向畸變參數。—— 是以，你需要标定你的相機。當然，對我們這些喜歡偷懶的人來說，這個工作太繁瑣。有沒有能偷懶的辦法呢？的确有！
由于沒有使用精确的3D 模型，我們已經處于近似狀态下。我們還可以進一步進行近似處理：（1）我們可以用圖像中心近似光學中心；（2）用圖像的像素寬度近似相機的焦距；（3）假定不存在徑向畸變。
4、位姿估計算法是如何工作的？
有很多的位姿估計算法，最有名的可以追溯到1841 年。該算法的詳細讨論超出了本文的讨論範圍。這裡隻給出其簡要的核心思想。
該位姿估計PnP 問題涉及到3 個坐标系統。（1）世界坐标系。前面給出的各個面部特征的3D 坐标就是在世界坐标系之中；（2）如果我們知道了旋轉矩陣R 和平移向量t ，我們就能将世界坐标系下的3D 點“變換Transform” 到相機坐标系中的3D 點。（3）使用相機内參矩陣，能将相機坐标系中的3D 點能被投影到圖像平面image plane, 也就算圖像坐标系統image coordinate system。
整個問題就是在3 個坐标系統中玩耍： 3D 的世界坐标系World coordiantes、3D的相機坐标系Camera coordinates、2D 的圖像坐标系Image coordinates。
圖2：三個坐标系統之間的關系
下面，我們來深入研究圖像生成方程，以了解上述三個坐标系是如何工作的。
在上述圖檔中，左下角的O 是相機的中心，中間的平面Image Plane 就是像平面，我們感興趣的是找出“将3D 點P 投影到像平面中點p 的方程式”。
首先，我們假設已經知道了位于世界坐标系中3D 點P 的位置（U，V，W ），如果我們還知道了世界坐标系相對于相機坐标系之間的旋轉矩陣R 和平移向量t, 通過下面方程式，就能計算出點P 在相機坐标系下的位置（X， Y， Z）。
将上面的方程式（1）展開，我們得到下面的形式：
如果學習過線性代數，就應該知道：對于上面方程，如果知道了足夠數量的點的對應關系【（X， Y， Z ）和（U，V，W ）之間的對應關系】，那麼上面的方程（2）就算一個線性方程組，其中， rij 和(tx, ty, tz) 就是未知數。運用線性代數的知識，就能解出這些未知數。
正如将在下面章節講述的，我們知道(X, Y, Z) 隻在一個未知的尺度上【或者說(X, Y, Z)僅由一個未知的尺度所決定】，是以我們沒有一個簡單的線性系統。
5、直接線性變換（Direct Linear Transform）
我們已經知道了3D 模型【世界坐标系】中的很多點【也就是（U，V，W ）】，但是，我們不知道(X, Y, Z) 。我們隻知道這些3D 點對應的2D 點【在圖像平面Image Plane中】的位置【也就是(x, y) 】。在不考慮畸變參數的情況下， 像平面中點p 的坐标(x,y) 由下面的方程式（3）給出。
這裡， fx 和fy 分别是焦距f 在x 軸和y 軸方向上的長度。(cx, cy) 是相機的光學中心。如果考慮徑向畸變參數，那麼事情将變得稍微有點複雜了。是以，為了簡單起見，我們忽略了徑向畸變參賽。
方程式（3）中的s 是什麼？ 它是一個未知的尺度因子scale factor 。由于在圖像中我們沒有點的depth 資訊， 是以這個s 必須存在于方程中。引入s 是為了表示：圖2 中射線O-P 上的任何一點，無論遠近，在像平面Image Plane 上都是同一個點p 。
也就是說：如果我們将世界坐标系中的任何一點P 與相機坐标系的中心點O 連接配接起來，射線O-P 與像平面Image Plane 的交點就是點P 在像平面上的像點p， 該射線上的任何一點P ，都将在像平面上産生同一個像點p 。
現在，上面這些讨論已經将方程式（2）搞複雜了。因為這已經不是我們所熟悉的、能解決的一個“好的線性方程”了。我們方程看起來更像下面的形式。
不過，幸運的是，上面形式的方程，可以使用一些“代數魔法”來解決—— 直接線性變換（DLT）。當你發現一個問題的方程式“幾乎是線性的，但又由于存在未知的尺度因子，造成該方程不完全線性”， 那麼你就可以使用DLT 方法來求解。
6、列文伯格- 馬誇爾特優化算法（ Levenberg-Marquardt Optimization）
由于下面的一些原因，前面闡述的DLT 解決方案并不能非常精确地求解。第一： 旋轉向量R 有3 個自由度，但是DLT 方案中使用的矩陣描述有9 個數，DLT 方案中沒有任何措施“強迫估計後得到的3X3 的矩陣變為一個旋轉矩陣”。更重要的是：DLT 方案沒有“正确的目标函數”。的确，我們希望能最小化“重投影誤差reprojection error”，正如下面将要講的。
對于方程式（2）和方程式（3），如果我們知道正确的位姿（矩陣R 和向量t），通過将3D 點投影到2D 像平面中， 我們能預測到3D 面部點的2D 點在圖像中的位置locations 。換而言之，如果我們知道R 和t，對于每一個3D 點P，我們都能在像平面上找到對于的點p。
我們也知道了2D 面部特征點【通過Dlib 或者手工點選給出】。我們可以觀察被投影的3D 點和2D 面部特征之間的距離。當位姿估計結果是準确的時候，被投影到像平面Image Plane 中的3D 點将與2D 面部特征點幾乎完美地對齊。但是，當位姿估計不準确時，我們可以計算“重投影誤差reprojection error” —— 被投影的3D 點和2D 面部特征點之間的距離平方和。
位姿（R 和t）的近似估計可以使用DLT 方案。改進DLT 解決方案的一個簡單方法是随機“輕微”改變姿态(R 和t)，并檢查重投影誤差是否減小。如果的确減小了，我們就采用新的估計結果。我們可以不斷地擾動R 和t 來找到更好的估計。盡管這種方法可以工作，但是很慢。可以證明，有一些基本性的方法可以通過疊代地改變R 和t 的值，進而降低重投影誤差。—— 其中之一就是所謂的“列文伯格-馬誇爾特優化算法”。
7、OpenCV 中的位姿估計
在OpenCV 中，有兩種用于位姿估計的API： solvePnP 和solvePnPRansac 。
solvePnP 實作了幾種姿态估計算法，可以使用參數進行選擇不同的算法。預設情況下，它使用标志SOLVEPNP_ITERATIVE，其本質上是DLT 解決方案，然後是列文伯格-馬誇爾特算法進行優化。SOLVEPNP_P3P 隻使用3 個點來計算姿勢，并且應該隻在使用solvePnPRansac 時使用。在OpenCV 3 中， 引入了SOLVEPNP_DLS 和SOLVEPNP_UPNP 兩種新方法。關于SOLVEPNP_UPNP 有趣的事情是，它在估計位姿是，也試圖估計相機内部參數。
solvePnPRansac 中的“Ransac” 是“随機抽樣一緻性算法Random Sample Consensus” 的意思。引入Ransac 是為了位姿估計的魯棒性。當你懷疑一些資料點是噪聲資料的時候，使用RANSAC 是很有用的。
8、樣例
CMakeLists.txt 檔案：
圖檔檔案：
源代碼：
參考資料
https://docs.oakchina.cn/en/latest/
https://www.oakchina.cn/selection-guide/
OAK中國
| OpenCV AI Kit在中國區的官方代理商和技術服務商
| 追蹤AI技術和産品新動态
戳「+關注」擷取最新資訊↗↗