PNP算法
在視覺SLAM中，我們通常會有不同的觀測數據。
如果我們有RGB-D相機，我們還可以獲得匹配像素對應的深度信息，那么我們就得到了兩組三維匹配點，這時就可以通過
ICP（迭代最近點）來進行運動估計，ICP又分利用SVD分解和Bundle Adjustment兩種方式，但是本質上並無區別，畢竟SVD也是構建非線性最小二乘問題。不過，Bundle Adjustment問題可以同時優化點和位姿。
如果我們有一組三維點匹配二維點，這種類型的數據獲取方式有多種。典型的是可以通過RBG-D來獲取，前面我們提到RGB-D采集的數據可以用ICP進行優化位姿，實際上由於RGB-D相機本身的噪聲，深度值估計是有誤差的。因此在SLAM中通常會用PNP的方法來做估計，PNP也就是我們本講要介紹的內容，利用一個帶噪聲的深度值進行位姿估計，總比用兩個都帶噪聲的深度值來得合適些。另一種渠道是，單目三角化得到的地圖點重投影到新幀中構建的PNP問題，原理一致。此外，還有雙目圖像。
由於歷史上的大牛眾多，提供了多種千奇百怪的PNP算法，因此篇幅會有點長。筆者這也算是嘔心瀝血之作了，先把幾個方法寫出來先，然后再進入主題吧：
1. 直接線性變換（DLT）；
2. P3P;
3. EPNP;
4. UPNP；
5. 光束平差法（Bundle Adjustment）。
已知條件
匹配的 $n$ 組三維點-二維點：
$$P^{r} = \{P_{1}^{r}, P_{2}^{r}, \dots, P_{n}^{r}\}, p^{c} = \{p_{1}^{c}, p_{2}^{c}, \dots, p_{n}^{c}\}$$
其中，$P^{r}$ 表示參考幀中的三維點，$p^{c}$ 表示當前幀匹配的二維點（像素坐標）。數據獲取方式如前面介紹的。
問題
在已知條件下，求解參考幀到當前幀的相對位姿。（旋轉矩陣$R$ 和位移向量$t$）
方法一：直接線性變換（DLT）
直接線性變換與我們前面介紹的對極幾何，單應矩陣的計算是類似的，都是忽略相對位姿本身的性質，直接將其視為一個數值矩陣，優化完才利用某些約束條件恢復相對運動。
假設我們有一對匹配對：$P_{i}^{r} = [X, Y, Z, 1]^{T}$ 和 $p_{i}^{c} = [u, v, 1]^{T}$，兩個坐標均為歸一化坐標。假設相對位姿為：$T = [R | t]$，$p_{i}^{r}$ 的深度值為 $s$，則我們有：
$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} t_{1} & t_{2} & t_{3} & t_{4} \\ t_{5} & t_{6} & t_{7} & t_{8} \\ t_{9} & t_{10} & t_{11} & t_{12} \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} $$
通過令 $T = \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix}$，則上式可以變成：
$$ s\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} = \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} P_{i}^{c} $$
可以得到：
$$ \begin{aligned} u = \frac{t_{1}^{T}P_{i}^{c}}{t_{3}^{T}P_{i}^{c}} \\ v = \frac{t_{2}^{T}P_{i}^{c}}{t_{3}^{T}P_{i}^{c}} \end{aligned} $$
通過上述式子，我們去除了尺度因子的約束。進一步簡化，我們得到兩個約束條件：
$$ \begin{aligned} -t_{3}^{T} u P_{i}^{c} + t_{1}^{T}P_{i}^{c} = 0 \Rightarrow \begin{bmatrix}P_{i}^{c} & 0 & -uP_{i}^{c} \end{bmatrix} \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} \\ -t_{3}^{T} v P_{i}^{c} + t_{2}^{T}P_{i}^{c} = 0 \Rightarrow \begin{bmatrix}0 & P_{i}^{c} & -vP_{i}^{c} \end{bmatrix} \begin{bmatrix} t_{1}^{T} \\ t_{2}^{T} \\ t_{3}^{T} \end{bmatrix} \\ \end{aligned} $$
由於相對位姿共有 $12$ 個未知數，因此至少需要六組匹配點，才能提供 $12$ 個約束條件用以解決這個問題。於是，我們構建了一個齊次方程：
$$AT = 0$$
矩陣 $A$ 是多組匹配點構成的約束條件，如果只有六組匹配點，則我們可以直接求得滿足要求的解。如果不止六組點，那么我們就需要構建一個非線性方程來求解。這與我們先前介紹的對極幾何，單應矩陣等利用SVD求解的方法一致。通過SVD分解，我們得到了 $12$ 維的 $T$ 矩陣。
利用直接線性變換（DLT）估計的是一個射影變換，因為對於我們上述構建的問題，僅僅需要其滿足一定的空間變換關系，而對於矩陣 $T$ 並沒有更多的約束。實際上，由於 $T = [R | t]$，旋轉矩陣需要滿足一定的約束性質，因此需要進一步對解進行分析，比如QR分解,SVDf分解，將結果從矩陣空間重新投影到 $SE(3)$ 流形上，轉換成旋轉和平移兩部分。
很多博文基本上到這一步就沒有后續了，默認大家都會了，可是實際上大部分的讀者還是一臉懵逼，甚至人雲亦雲，就這樣。實際上，后續的求解基本不清楚怎么做，筆者先前便是如此。通過查找資料，找到了一個知乎回答：
[PnP] PnP問題之DLT解法，較好地解釋了這個求解的問題。筆者簡單的記錄一下。
通過SVD分解，我們得到了 $T$ 的數值解，這個解是不帶尺度的。“旋轉矩陣”為：
$$\hat{R} = \begin{bmatrix} t_{1} & t_{2} & t_{3} \\ t_{5} & t_{6} & t_{7} \\ t_{9} & t_{10} & t_{11} \end{bmatrix}$$
同樣，通過SVD分解：
$$\begin{bmatrix} U & \Sigma & V \end{bmatrix} = SVD(\hat{R})$$
於是我們可以得到旋轉矩陣：
$$ \begin{aligned} R_{1} = UV^{T} \\ R_{2} = -UV^{T} \end{aligned} $$
理論上，$\Sigma$ 的對角線應該非常接近，取均值，則尺度因子 $\beta$ 可以由下式得到：
$$\beta = \pm 1 / (tr(\Sigma) / 3)$$
於是，我們可以計算平移向量：
$$ \begin{aligned} t_{1} = \beta_{1} \begin{bmatrix} t_{4} & t_{8} & t_{12} \end{bmatrix} \\ t_{2} = \beta_{2} \begin{bmatrix} t_{4} & t_{8} & t_{12} \end{bmatrix} \end{aligned} $$
通過上面的計算，我們得到了四組解，一說到有四組解，我們是不是想起了對極幾何的操作？沒錯，我們還是一樣利用點必須位於相機前方，即深度值必須為正這么一個約束對四組解進行驗證，找到正確的解。
方法二：P3P
接下來，筆者要介紹另一種PNP方法：P3P，只用三組3D-2D匹配點加一組用於驗證，對數據要求較少。
P3P的思路比較朴素，就是利用點的空間分布關系在不同相機坐標系下保持不變，以及相似三角形原理計算出參考幀三維點 $P_{i}^{c}$ 對應的參考幀三維點 $P_{i}^{r}$（實際上，參考幀只有二維點 $p_{i}^{r}$，因此才需要算三維點）。有了參考幀的三維空間點以及匹配的當前幀的三維空間點，我們可以構建一個類似於對極幾何一般的約束問題，比如 $AE = b$，再分解出四組解，利用驗證組選擇正確的解。
首先引入一個圖：
上述描繪的是空間點$A_{w}, B_{w}, C_{w}$在當前相機坐標系下的對應點，對應像素坐標分別為$u, v, w$。設當前相機光心為 $O$，則可以得到三組相似三角形：
$$ \Delta Ouv \sim \Delta OAB, \Delta Ouw \sim \Delta OAC, \Delta Ovw \sim \Delta OBC $$
利用余弦定理可以得到：
$$ \begin{aligned} OA^{2} + OB^{2} - 2OA\cdot OB \cdot cos<ou, ov> = AB^{2} \\ OA^{2} + OC^{2} - 2OA\cdot OC \cdot cos<ou, ow> = AC^{2} \\ OB^{2} + OC^{2} - 2OB\cdot OC \cdot cos<ov, ow> = BC^{2} \end{aligned} $$
對上述所有式子除以 $OC^{2}$，並令$x = \frac{OA}{OC}, y = \frac{OB}{OC}$，可以得到：
$$ \begin{aligned} x^{2} + y^{2} - 2x\cdot y \cdot cos<ou, ov> = \frac{AB^{2}}{OC^{2}} \\ x^{2} + 1 - 2x\cdot cos<ou, ow> = \frac{AC^{2}}{OC^{2}} \\ y^{2} + 1 - 2y\cdot cos<ov, ow> = \frac{BC^{2}}{OC^{2}} \end{aligned} $$
上述式子中，由於像素坐標已知，顯然余弦值可求，而右邊的式子中，$A, B, C$ 的空間關系也是已知的，但是我們不知道 $OA, OB, OC$ 是未知的，前兩項是待求，最后一項我們需要將他消除，則我們通過令：$a = \frac{AB^{2}}{OC^{2}}, ab = \frac{AC^{2}}{OC^{2}}, ac = \frac{BC^{2}}{OC^{2}}$，可以得到 $b = \frac{AC^{2}}{AB^{2}}, c = \frac{BC^{2}}{AB^{2}}$，我們可以將第一個式子帶入其余兩個式子中，以此消去 $OA$，即：
$$ \begin{aligned} (1 - b)x^{2} -by^{2} + 1 - 2x\cdot cos<ou, ow> + 2b\cdot x\cdot y\cdot cos<ou, ov> = 0 \\ -cx^{2} + (1 - c)\cdot y^{2} + 1 - 2y\cdot cos<ov, ow> + 2b\cdot x\cdot y\cdot cos<ou, ov> = 0 \end{aligned} $$
最后我們得到了兩個二元二次方程，該方程比較復雜，需要使用一些特殊的求解方法，視覺SLAM十四講中推薦的是使用吳消元法。具體的操作，筆者搜索到一篇博文[相機位姿求解——P3P問題](https://www.cnblogs.com/mafuqiang/p/8302663.html)提供了求解方法。得到 $x, y$ 后，我們將其代回原式子中，可以計算得到 $OA, OB, OC$。但我們需要求的是一個坐標點，而不單單是長度，因此還需要乘上一個單位方向向量。（筆者認為這個單位向量可以通過歸一化平面的坐標與像素點坐標來計算）。
OK，通過上述方法，我們得到了三組點，於是我們利用類對極幾何的方法計算出四組解，一組點可以提供三個約束條件，三組點剛好滿足約束要求。於是我們利用第四組點進行驗證，選擇正確的解。至此，P3P的方法我們就介紹完畢了。
有一點值得注意的是，P3P算法只要求3組點用於計算，1組點用於驗證，再多的數據也用不了。很顯然，如果我們選擇的數據對存在噪聲太大甚至是錯誤的，都會直接影響到最終結果。因此，實際上P3P也不是很好嘛，計算還那么難。筆者也提供一篇大家都在推薦的文章：Complete solution classification for the perspective-three-point problem。實話說，筆者大概瀏覽了一下，就看不下去了。如果以后不是非得用到的情況下，筆者肯定不會再去碰它的了。
方法三：EPNP
前面我們介紹了P3P，接下來我們介紹EPNP。筆者EPNP部分主要參考兩篇博客，其一是
深入EPnP算法，其二是
PNP(pespective-n-point)算法學習筆記。第二篇博客主要是第一篇博客的整理版本，嗯，類似於我這一版差不多，加入了自己的理解。
EPNP相比P3P在模型上稍微復雜了一些，首先我們需要在參考幀中選擇 $4$ 個不共面的控制點 $[c_{1}^{r}, c_{2}^{r}, c_{3}^{r}, c_{4}^{r}]$，假設參考幀中有一個點 $P_{i}^{r}$，那么該點可以用我們先前選出的控制點進行線性表出：
$$P_{i}^{r} = \sum_{j=1}^{4}(\alpha_{ij}\cdot c_{j}^{r})$$
且滿足$\sum_{j=1}^{4}\alpha_{ij} = 1$，$\alpha_{ij}$ 是齊次barycentric坐標。同理，在當前幀坐標系下也滿足：
$$P_{i}^{c} = \sum_{j=1}^{4}(\alpha_{ij}\cdot c_{j}^{c})$$
參考幀和當前幀之間的空間點滿足約束關系：
$$P_{j}^{c} = \begin{bmatrix}R & t\end{bmatrix} \begin{bmatrix}P_{j}^{r} \\ 1\end{bmatrix} \Rightarrow \sum_{j=1}^{4}\alpha_{ij}c_{j}^{c} = \sum_{j=1}^{4}\alpha_{ij}\begin{bmatrix}R & t\end{bmatrix} \begin{bmatrix}c_{j}^{r} \\ 1\end{bmatrix}$$
為了得到精確解，而不是最小二乘擬合得到的解，於是我們需要引入 $4$ 組控制點。為了更加直觀的看到空間點與我們選擇的控制點間的關系：
$$\begin{bmatrix}P_{i}^{r} \\ 1 \end{bmatrix} = C \begin{bmatrix}\alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix} = \begin{bmatrix}c_{1}^{r} & c_{2}^{r} & c_{3}^{r} & c_{4}^{r} \\ 1 & 1 & 1 & 1 \end{bmatrix} \begin{bmatrix}\alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix}$$
OK,進一步分析得到：
$$\begin{bmatrix}\alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix} = C^{-1}\begin{bmatrix}P_{i}^{r} \\ 1 \end{bmatrix}$$
重點來了！那么我們在往下走之前，先分析一下我們目前可以得到的所有數據。已知數據是參考幀中有三維空間點一堆 $P^{r}$ 以及當前幀中對應的匹配像素坐標 $p^{c}$， 好像...沒了？？嗯，是的。但是我們可以先假設，比如上面的東西我們部分已知呢？
首先，參考幀中的四個控制點 $c_{i}^{r}$ 我們是可以選擇的，因此可以視為已知的。那么我們知道控制點，也知道參考幀中的空間點，很顯然我們可以求出barycentric坐標，即 $[\alpha_{i1}, \alpha_{i2}, \alpha_{i3}, \alpha_{i4}]^{T}$。就這么多已知的東西，沒了吧？這次是真沒了。而且我們還需要利用這些量，來計算當前幀相機坐標系下的四個控制點 $[c_{i1}^{c}, c_{i2}^{c}, c_{i3}^{c}, c_{i4}^{c}]$。
我們知道針孔相機的投影模型為：
$$s_{i}p_{i}^{c} = KP_{i}^{r} \Rightarrow s_{i}\begin{bmatrix}u_{i} \\ v_{i} \\ 1 \end{bmatrix} = \begin{bmatrix}f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}X_{i}^{r} \\ Y_{i}^{r} \\ Z_{i}^{r} \end{bmatrix}$$
將控制點關系帶入上述方程，並消去深度值 $s_{i}$，我們可以得到兩個約束方程。不過值得注意的是，我們需要求解當前幀相機坐標系下的四個控制點，共計 $12$ 個未知數。於是我們又用老方法，把這些未知數拉成長條，解數值方程。最后還得利用空間點在外參變換下，空間關系不改變的約束，結合非線性優化來得到最終的控制點坐標。
最后，我們得到了當前幀和參考幀各自四個控制點，並且可以利用參考幀的空間點來計算barycentric坐標，那么我們顯然可以利用：
$$P_{i}^{c} = \sum_{j=1}^{4}(\alpha_{ij}\cdot c_{j}^{c})$$
得到當前幀中匹配的空間點。於是乎，我們有了參考幀的三維坐標 $P^{r}$ 還有當前幀匹配的三維坐標 $P^{c}$，那么ICP走起！至此，我們EPNP的問題就解決了。
ICP的內容我們之前介紹過了，這里就不贅述了。
還有一點，實際上參考幀坐標系下的四個控制點只要不共面就行了，不過博文中還提供了另外一種選擇控制點的方法，具體參考上述提到的兩篇博文即可。
方法四：UPNP
UPNP實際上跟EPNP十分類似，唯一不同的地方在於UPNP不需要知道相機的焦距。我們在EPNP中需要用到相機內參的地方是：
$$s_{i}p_{i}^{c} = KP_{i}^{r} \Rightarrow s_{i}\begin{bmatrix}u_{i} \\ v_{i} \\ 1 \end{bmatrix} = \begin{bmatrix}f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}X_{i}^{r} \\ Y_{i}^{r} \\ Z_{i}^{r} \end{bmatrix}$$
由於一般情況下，相機的焦距 $f_{x} = f_{y}$，因此可以直接記為 $f$。UPNP中通過將未知焦距 $f$ 移到待求解的方程中，於是整個問題又變成了EPNP的操作了。通過求解出當前坐標系下的控制點，進而計算出當前幀坐標下的三維坐標。於是我們可以通過ICP求解的方法計算參考幀和當前幀的相對位姿。
再詳細的內容可以觀看我們泡泡的公開課：
【泡泡機器人公開課】第三十九課：PnP 算法簡介&代碼解析-柴政。
方法五：光束平差法（Ｂundle Adjustment）
前面那些理論都太難了，我們來點簡單的。畢竟BA的套路還是比較固定的，通過構建一個非線性最小二乘法問題，選擇優化方法對目標函數進行優化，最后就可以得到我們想要的結果。
BA方法唯一的障礙應該還是在求雅可比上。那我們還是先把問題描述一下吧。目標函數可以記為:
$$f(\xi) = \sum_{i}^{n}|| p_{i}^{c} - K\cdot exp(\xi^{\wedge})\cdot P_{i}^{r} ||^{2} = \sum_{i}^{n}|| e_{i}(\xi) ||^{2} $$
其中上述的點均采用齊次坐標，相機內參 $K \in R^{3 \times 4}$，$exp(\xi^{\wedge})$ 表示從參考幀坐標系到當前幀坐標系的相對變換。為了求解上述問題，我們需要先對該目標函數計算雅可比，首先簡化問題：
$$e(\xi) = p_{i}^{c} - K\cdot exp(\xi^{\wedge})\cdot P_{i}^{r} = p_{i}^{c} - K\cdot P_{i}^{C} = p_{i}^{r} - p_{i}^{c}$$
其中，$P_{i}^{C}$ 表示通過將參考幀坐標系的點轉換到當前坐標系下得到的，於是我們的雅可比可以通過鏈式法則計算：
$$\frac{\partial{e}}{\partial{\xi}} = \frac{\partial{e}}{\partial{p_{i}^{c}}} \cdot \frac{\partial{p_{i}^{c}}}{\partial{P_{i}^{C}}} \cdot \frac{\partial{P_{i}^{C}}}{\partial{\xi}}$$
第一步求解 $\frac{\partial{e}}{\partial{P_{i}^{C}}}$，設 $P_{i}^{C} = [x_{i}^{C}, y_{i}^{C}, z_{i}^{C}, 1]^{T}$，通過針孔模型相機的投影模型，可以很簡單的計算出雅可比：
$$\frac{\partial{e}}{\partial{P_{i}^{C}}} = \frac{\partial{e}}{\partial{p_{i}^{c}}} \cdot \frac{\partial{p_{i}^{c}}}{\partial{P_{i}^{C}}} = (-1) \cdot \begin{bmatrix} x_{i}^{C}f_{x} & 0 & -\frac{x_{i}^{C}\cdot f_{x}}{(z_{i}^{C})^{2}} \\ 0 & y_{i}^{C}f_{y} & -\frac{y_{i}^{C}\cdot f_{y}}{(z_{i}^{C})^{2}} \end{bmatrix}$$
接下來是計算 $\frac{\partial{P_{i}^{C}}}{\partial{\xi}}$，由於旋轉矩陣不具備加法封閉性，利用直接求導的話，盡管有BCH近似可以用，但是會引入一個計算比較復雜的雅可比。因此，本文還是更傾向於使用擾動模型，這也是大部分程序中使用的方法：
$$ \begin{aligned} \frac{\partial{P_{i}^{C}}}{\partial{\xi}} =& \frac{exp(\delta \xi^{\wedge})exp(\xi^{\wedge})P_{i}^{r} - exp(\xi^{\wedge})P_{i}^{r}}{\delta \xi} \\ \approx & \frac{(1 + \delta \xi^{\wedge})exp(\xi^{\wedge})P_{i}^{r} - exp(\xi^{\wedge})P_{i}^{r}}{\delta \xi} \\ =& \frac{(\delta \xi^{\wedge})exp(\xi^{\wedge})P_{i}^{r}}{\delta \xi} \\ =& \frac{\begin{bmatrix} \delta \phi^{\wedge} & \delta \rho \\ 0^{T} & 0 \end{bmatrix} \begin{bmatrix} RP_{i}^{r} + t \\ 1 \end{bmatrix} }{\delta \xi} \\ =& \frac{\begin{bmatrix} \delta \phi^{\wedge}(RP_{i}^{r} + t) + \delta \rho \\ 0 \end{bmatrix} }{\delta \xi} \\ =& \frac{\begin{bmatrix} -(RP_{i}^{r} + t)^{\wedge}\delta \phi + \delta \rho \\ 0 \end{bmatrix} }{\delta \xi} \\ =& \frac{\begin{bmatrix} -(RP_{i}^{r} + t)^{\wedge}\delta \phi + \delta \rho \\ 0 \end{bmatrix} }{\delta \xi} \\ =& \frac{\begin{bmatrix} -(RP_{i}^{r} + t)^{\wedge} & I \\ 0^{T} & 0^{T} \end{bmatrix} \begin{bmatrix} \delta \phi \\ \delta \rho \end{bmatrix} }{\delta \xi} \\ =& \begin{bmatrix} -(RP_{i}^{r} + t)^{\wedge} & I \\ 0^{T} & 0^{T} \end{bmatrix} \\ =& (TP_{i}^{r})^{\odot} \end{aligned} $$
其中，有 $exp(\xi^{\wedge}) = T = \begin{bmatrix}R & t \\ 0^{T} & 1\end{bmatrix}$，$\delta \xi = \begin{bmatrix} \delta \phi \\ \delta \rho \end{bmatrix}$。當然你也可以選擇位移在前，旋轉在后，那么雅可比的順序就需要調整一下。
通過上述的計算，我們可以知道$\frac{\partial{e}}{\partial{\xi}}$：
$$\frac{\partial{e}}{\partial{\xi}} = - \begin{bmatrix} x_{i}^{C}f_{x} & 0 & -\frac{x_{i}^{C}\cdot f_{x}}{(z_{i}^{C})^{2}} \\ 0 & y_{i}^{C}f_{y} & -\frac{y_{i}^{C}\cdot f_{y}}{(z_{i}^{C})^{2}} \end{bmatrix} (TP_{i}^{r})^{\odot}$$
具體的筆者就不展開了，反正就那么代進去算就對了。於是我們這里就算出了位姿的雅可比矩陣。那么優化問題就很簡單了，通過對目標函數中的各個殘差進行一階泰勒展開，並對展開后的殘差進行一階導數即可得到一個正規方程：$H\Delta x = b$，具體也沒啥好說的，非線性優化的內容就那樣推唄。具體可以參考
學習筆記之——P3P與ICP位姿估計算法及實驗。OK，那么非線性優化的方法也介紹完了。
總結
在前面，我們介紹了 $5$ 種PNP的求解方法，盡管有些不夠具體，但是筆者也提供了足夠多的參考資料，這些參考資料都是筆者在整理筆記時篩選的精華博文，可以幫助讀者們省去大量的時間。可以說，筆者這個博文已經基本上包含了大部分的PNP算法了，也算是筆者的嘔心瀝血之作。前后花去幾天的時間，來閱讀資料並整理。在此之前，筆者實現了兩個簡單的PNP函數，一個是基於OpenCV提供的solverPnpRansac來求解相對位姿，另外一個是筆者自己實現的Bundle Adjustment版本的PNP。詳情可以查看我的github:
PNP。
參考資料
1.
g2o定義邊
2.
SLAM之李群李代數工具
3.
李代數求導
4.
李代數的導數
5.
相機位姿求解問題？
6.
3D-2D相機位姿估計
7.
特征法前端（二）
8. [視覺SLAM十四講]
10.
相機位姿求解——P3P問題
12. X.-S. Gao, X.-R. Hou, J. Tang, and H.-F. Cheng, “Complete solution classification for the perspective-three-point problem,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 25, pp. 930–943, Aug 2003.
13.
深入EPnP算法