EnP演算法，論文：EPnP: Efficient Perspective-n-Point Camera Pose Estimation

直接給過程：

相機座標系用Fc，世界座標系用Fw表示，任何一點可以用四個控制點p表示：

對於相機座標系同樣成立：

對於上面的公式，首先需要說明的是α確實存在。因為cw或cc構成的方程組是欠定的，所以一定存在解。

理論上來說，控制點可以隨便選擇，這裡選擇控制點為參考點的中心，其他的點在PCA得到的主軸上單位長度處，從而提高演算法的穩定性。

如何求解？如下：

根據投影方程得到世界座標系中參考點座標和相機座標系中參考點的約束關係：

寫成矩陣的形式為：

A就是相機內參矩陣。

將等式的第三列代入第一二列，得到：

 因此，可以得到下面的線性方程組：

上面的方程中，四個控制點總共12個未知變數，M為2n×12的矩陣。因此，x屬於M的右零空間，vi為矩陣M的右奇異向量，可以通過求解MTM 的零空間特徵值得到。

 [說明]使用MTM比使用M計算量更少，因為MTM是求解是常數複雜度，而M是O(n3)的複雜度，但是計算MTM的複雜度是O(n)的。

上面求解的x中，需要確定βi，也就是確定合適的線性組合。根據參考點的位置不同，矩陣MTM的零空間維數可能為N=1→4維。求解β的策略是控制點在座標系Fw和Fc中，兩兩之間的距離是相同，而x的3k+1−3k分量表示分別表示不同的控制點在相機座標系中的座標，總共有C24=6個約束。
如果N=1，則根據約束有：

所以：

若N=2，則：

由於β1和β2只以二次項出現在方程中，記β=[β21,β1β2,β22]T, ρ的每一項為∥cwi−cwj∥2，得到相應的方程：L

其中L是由v1和v2構成的6×3的矩陣。
上面的方程可以通過β=(LTL)−1LTρ得到，然後通過選擇合適的符號從β21,β1β2,β22使得所有的pci有正的z座標。

如果N=3則和N=2差不多，唯一的區別在於使用的是L的逆，而不是偽逆，此時的L為6×6的矩陣。

共面的情況：

當點共面時，M減少為2n*9，有9個特徵向量，上面的結論仍然有效，唯一不同在於約束又6變成了3.

G-N優化

前面的步驟可以得到目標點在相機座標系中的閉式解，作為G-N優化的初始值，優化的變數為β=[β1,⋯,βN]T，目標函式為:

其中CW的兩點距離已知，控制點在相機中的值，用下式計算：

該優化過程和參考點的數目無關，優化步驟和時間是常數。

計算R，t

前面的兩步計算不同維數的零空間的誤差，選擇誤差最小維數對應的β，從而得到x，恢復出控制點在相機座標系中的座標並根據質心座標系數得到參考點在相機座標系的座標。剩下的工作就是已知一組點雲在兩個座標系中的座標，求兩個座標系的位姿變換。
步驟如下：

（1）求中心點，

（2）去中心

（3）計算H矩陣

（4）對H進行SVD

（5）計算X=VUT，如果det(x)=1，則R=X，t=Pcc−RPcw。否則R(2,⋅)=−R(2,⋅)