1. 背景
最近正在做姿態估計，簡單搜姿態估計的關鍵字得到的信息不夠完整，所以把搜到的信息加上自己的理解整合在這篇文章。
2. 正向3D到2D的過程
先上一張圖【已經自帶水印就偷懶省下引用】：
首先3D物體在世界坐標系，也就是我們現實世界，物體是現實中的絕對尺寸，以m為單位。現實世界的物體認為成剛體，那么這個剛體和相機之間會有一個相對位置。對於物體來說，是以自身的中心為原點，需要把自身的尺寸映射到以相機光心為原點的相機坐標系。也就是對物體的絕對坐標做一些平移和旋轉，變換坐標的圓心和軸向。
物體經過鏡頭（透鏡）在焦距位置成像，也就是圖中最左邊的像平面。這個像平面是以o1-xy為坐標系的。根據透鏡成像原理，這個成像和實際物體相比是倒像，然而我們希望得到的圖片是正像，所以就在對稱位置取了虛擬像平面，這個就是圖像坐標系。在圖像坐標系中圖片的尺寸大小是和感光元件（Sensor Size）相關的，以mm為單位。通常在相機說明書里以 μm 為單位給出，例如：Image Area：5856 μm x 3276 μm。看上圖的右半部分，相機坐標系的物體平面和圖像坐標系與相機光心形成金字塔結構，所以可以利用相似性得出變化坐標。下圖中的（Xc，Yc，Zc）是指物體在相機坐標系中的坐標。
由圖就很容易得出：
像素坐標系是指可視化后圖片的坐標系，也叫是通常左上角為原點，向右向下為正方向的坐標系。在圖像坐標系還需要變換到像素坐標系才能得到我們看得到的圖片。像素是離散化的點，而成像是連續的，因此從圖像坐標系到像素坐標系的轉換也叫離散化。在不考慮畸變和仿設的情況下，變換公式如下：
（u,v）是像素坐標系坐標，（x,y）是圖像坐標系坐標。dx和dy是計算出來的像素大小。
整個轉換過程如下：
圖像物理轉像素坐標系和相機坐標系到圖像物理坐標系的復合就是相機的內參矩陣，而世界坐標系到相機坐標系的轉換就是內參矩陣。
3. 2D到3D姿態估計
這個問題定義是：在有2D特征點的情況下找到3D旋轉的外參矩陣，也就是找到上圖中的世界坐標系到相機坐標系的變換矩陣。很顯然，我們需要知道內參矩陣，像素2D坐標和3D世界坐標才能求解外參矩陣。內參矩陣可以使用相機標定或者說明書參數估計方法。相機標定用棋盤格的方法很多博客教程，就不在這里重述，主要寫一下根據給定數據估算的方法【1】：
假設攝像頭的焦距為4mm,像片的尺寸大小為640x480，傳感器尺寸為5856 μm x 3276 μm，像素大小為3μm x 3μm。即f=4， dx = 640/5.856, dy=480/3.276。u0 = 640/2， v0 = 480/2。從而內參矩陣可以直接代入參數得出。
以頭部姿態估計為例，3D世界坐標就是給出實際物理模型（3DMM）的3D特征點，對應的2D特征點可以通過很多算法獲得。之后可以通過solvePnp的方法求解外參矩陣。
參考：