EPnP: An Accurate O(n) Solution to the PnP Problem

PnP：目的：求解相機位姿，旋轉與位置
已知：相機內參矩陣，3D點和2D點
作用：AR，相機位置跟蹤等
方法有：DLT，EPnP，等
DLT參考：DLT原理
EPnP:
1.四個控制點求解（PCA）
2.3D點集用四個控制點表示的權重
3.四個控制點在相機座標系下的座標，12個未知數
4.3D點集在世界座標系座標，及相機座標系座標，類似於ICP解算出R，t
四個控制點求解


參考EPnP
矩陣構建


b求解




高斯牛頓優化：
將上一步b做初值

參考EPnP原理
代碼代碼