3D姿態估計-POSIT演算法

POSIT演算法，Pose from Orthography and Scaling with Iterations， 比例正交投影迭代變換演算法：

用於估計物體的3D姿態（相對於鏡頭的平移和旋轉量）。演算法正常工作的前提是物體在Z軸方向的“厚度”遠小於其在Z軸方向的平均深度，比如距離鏡頭10米遠的一張椅子。

演算法流程：

假設待求的姿態，包括旋轉矩陣R和平移向量T，分別為

透視投影變換為：

上式中的f是攝像機的焦距，它的具體值並不重要，重要的是f與x和y之間的比例，根據攝像頭內參數矩陣的fx和fy可以得到這個比例。實際的運算中可直接令f=1，但是相應的x和y也要按照比例設定。比如，對於內參數為[fx,fy,u0,v0]的攝像頭，如果一個畫素的位置是(u,v)，則對應的x和y應為

設世界座標系中的一點為(X_w,Y_w,Z_w)，則

有必要再解釋一下旋轉矩陣R和平移向量T的具體意義：

R的第i行表示攝像機座標系中的第i個座標軸方向的單位向量在世界座標系裡的座標；

R的第i列表示世界座標系中的第i個座標軸方向的單位向量在攝像機座標系裡的座標；

T正好是世界座標系的原點在攝像機座標系的座標，特別的，Tz就代表世界座標系的原點在攝像機座標系裡的“深度”。

根據前面的假設，物體在Z軸方向的‘厚度’，即物體表面各點在攝像機座標系中的Z座標變化範圍，遠小於該物體在Z軸方向的平均深度。一定要注意，“厚度”和“深度”都是相對於攝像機座標系的Z軸而言的。當世界座標系的原點在物體的中心附近時可以認為平均深度就是平移向量T中的Tz分量，即各點的Zc的平均值是Tz，而Zc的變化範圍相對於Tz又很小，因此可以認為，Zc始終在Tz附近，Zc≈Tz。

根據這個近似關係，可得

這就是我們的迭代初值。在這種初始狀態下，我們假設了物體的所有點在同一個深度上，這時的透視變換就退化為了一個比例正交投影POS。也就是，我們的迭代開始於一個比例正交投影，這也是POSIT演算法名字的由來。

我們前面得到了：

由於我們給了w一個估計值，因此可以先將其看做已知量，刪掉第三行（這樣方程中就少了4個未知量，更方便求解），得到

由於w被看做已知，因此上面的迭代方程可以看做有8個未知量，分別是

給定一對座標後（一個是世界座標系的座標，一個是影象座標系的座標，它們對應同一個點），我們就可以得到2個獨立的方程，一共需要8個獨立方程，因此至少需要給定4對座標，而且對應的這4個點在世界座標系中不能共面。為什麼不能共面？如果第4個點與前三個點共面，那麼該點的“齊次座標”就可以被其他三個點的“齊次座標”線性表示，而迭代方程的右側使用的就是齊次座標，這樣由第四個點得到的方程就不是獨立方程了。這裡之所以強調“齊次座標”是因為，只要三個點不共線，所有其他點（即使不共面）的“常規座標”都可以被這三個點的“常規座標”線性表示，但“齊次座標”則要求共面。

OK，假如我們獲得了4個不共面的點及其座標，並通過迭代方程求出了8個未知量。這時我們就可以算出向量sR₁和sR₂的模長。而由於R₁和R₂本身都是單位向量，即模長為1。因此我們可以求出s，進而求得R₁和R₂以及Tz=f/s：

有了R₁和R₂就可以求出R₃，後者為前兩個向量的叉積（兩兩垂直的單位向量）。

至此，整個旋轉矩陣R和平移向量T，共12個未知量，就都求出來了。不過，這只是近似值，因為我們一開始時假設了w=1（或Zc=Tz），即物體上所有的點的深度都是Tz。現在我們有了一個近似的轉換矩陣，可以利用它為各點計算一個新的深度，這個深度比Tz更準確。新的深度Zc和新的迭代係數w等於：

這時，由於每個點的有不同的深度，他們也就有了不同的迭代係數w。接著，將每個點的新w值代入迭代方程中，重新得到8個方程。由於這一次每個點的w（表徵了深度資訊）都比上一次迭代時更準確，因此會得到更精確的轉換矩陣，而更精確的轉換矩陣反過來又能讓我們求得各點更精確的深度資訊和w。如此往復迴圈反饋，就可逐步逼近精確解。

openCV裡用cvPOSIT()函式實現POSIT迭代，具體的函式用法網上有很多介紹不再重複了。順帶提一下openCV裡的另兩個函式solvePNP()和cvFindExtrinsicCameraParams2()，這兩個函式功能與POSIT類似，也是在已知一組點對應的影象座標和世界座標以及攝像頭內參數的情況下計算物體的3D姿態，不過與POSIT不同的是，它們不是求近似解，而是直接求精確解。既然可以直接求精確解了，那POSIT估計演算法還有什麼意義呢？

其實理論上，只要獲得3個點的資訊，就可以得出旋轉矩陣R和平移向量T了：

R和T共有12個未知量，每個點的座標代入前面的“---原始方程--”中，消去w，可得到2個獨立的方程，3個點就可以得到6個線性方程，再加上R自身的正交矩陣特徵（每行、每列都是單位向量，模長為1）又可以得到6個獨立的方程（非線性），共12個方程。

但實際中，解非線性方程很麻煩，所以openCV中應該是用了其他的優化方法。最無奈地，我們可以找6個點，每個點用“---原始方程--”消去w得到2個線性方程，最終也能得到12個方程，不過由於這種方法的求解過程中直接無視了正交矩陣R本身的特徵，最後得到的結果會由於點座標的測量誤差和計算誤差而稍微違反R自身的正交矩陣約束，當然這可以通過迭代彌補，但會增加演算法的複雜度。可能有人會疑惑，同樣是從3行的“---原始方程--”轉化成2行的方程，為什麼POSIT方法只需要四個點就可以求解，而這裡卻需要6個點？要知道，這裡只是利用線性關係消去了w，但保留了原來第三行中的未知量，因此未知量的數量保持12不變；而POSIT方法中，直接為w選取了一個估計值，並刪去了“---原始方程--”的第3行，這樣方程中才少了4個未知量只剩下8個，所以利用4個點的座標才得以求解。

於是，我們大概就能猜到既然有精確求解的演算法卻還要保留POSIT估計演算法的原因了：如果只有少數點的資訊（比如4個），又不想求解非線性方程，那就該POSIT上了。