論文：A Direct Least-Squares (DLS) Method for PnP

求解過程看著挺吃力的，看懂的還請多多指正。

本文針對PnP問題，採用非線性最小二乘直接計算，主要貢獻如下：

1.提出通用的n點（n≥3）姿態（all pose）計算方法，設計了獨特的非線性最小二乘代價函式。

2.無初始化要求，效能接近極大似然（MLE）方法的結果

3.非線性最小二乘代價函式不依賴點數變化。

具體過程如下：

1.問題建模

這裡的定義，跟其他如AP3P是一樣的，就是換了個字元，S是sensor的意思，就是相機座標系，G還是參考座標系。z是畫素座標系中測量點的方向向量，後面

整體的PnP非線性最小二乘求解建模如下：

其中代價函式J是均方誤差和：

難點在於如何求解J的最小值，通常有給定一個引數的初始猜測值，然後不斷迭代如（如GN演算法），但這不能保證一定會獲得精確值，還有利用KKT條件（SVM中的支援向量的求解演算法中有用到）來求非線性系統，但是，這裡難點在於位置引數過多（6+n）,而本文提出的方法如下：

1.修改LS模型：

先考慮無噪聲情況：

其中未知引數有α，C,P，我們重寫成矩陣運算形式：

根據上面，計算P和α：

 注意，P和α，都是旋轉矩陣C的線性函式：

因此，我們重寫式子4：

 這樣，我們將未知引數，從6+n減少到了3。

接著，我們採用CGR 引數化旋轉矩陣C，使得3個CGR變數成為無約束優化變數。同時滿足：

CGR引數化C（s=[s1,s2,s3]^T）：

 因此，9式可以表示 為：

由於C（s）導致線性化，因此，繼續簡化：

式13就是最終化簡後的約束條件。

2.修改代價函式

利用13式，我們重新定義代價函式J，來計算CGR的旋轉引數s,先增加噪聲：

 基於15式，PnP問題，重構為下面無約束的LSM問題：

其中，新的

 注意，這裡是一個4階多項式，包含如1，s1,s2,s3,...s1^4,s2^4,s3^4等項。

下面介紹作者直接計算多項式最小值的過程：

考慮到目標函式3個位置引數的4階多項式，因此，優化方程定義為：

為了構造Maculy矩陣，作者對目標函式增加了i=0項，F0= u0 + u1s1 + u2s2+ u3s3,這裡u是隨機生成的，這樣，原始目標函式被擴充套件為7個單項式組成：

然後 ，作者將引數S集合，劃分為4個集合，其中，S3集合包含所有可以被 s3^3整除的項，S2集合包含所有可以被s2^3整除但不能是s3^3，以此類推，S0集合包含所有未劃分的剩餘項。

之後，擴充套件多項式Fi為Gi:

假定原始系統的解為p，則所有擴充套件自F的多項式G，都為0，即F(p) = 0.但是，F0不會是0:

矩陣分解式21:

最終：

 其中：

從式23可以看出，F0（p）是Mf0的一個特徵值，對應的特徵向量p^α。我們可以直接獲得式18的27個解。

因為S0是1，因此將特徵向量以第一個項歸一化，以此求解出S1,S2,S3。

實際上，這27組解，只有4個是真實區域性最優，大多數情況下，n≥6時，我們只能獲取一個真實解。之後，根據該解計算代價，如此實現直接計算最優化。

另外，DLS-LM是在上面求解時，採用LM迭代演算法，其他與DLS類似。

作者對比了NPL,SDP,EPnP,DLS,DLS-LM等演算法，點的分佈視角45°*45°，並模擬了高斯噪聲。效果如下：

從這裡可以看出，NPL,SDP基本不用考慮了。當點數超過6時，DLS與EPnP的差距也不明顯了，剩下的可以對比下時間。