一. 问题定义
首先需要清楚什么叫做PnP(Perspective-n-Point)呢？是为了解决什么问题？
已知信息：
-
n个3D点在A坐标系(可以认为是世界坐标系)的坐标 { p 1 , p 2 , . . . , p n } \{p_1, p_2, ..., p_n\} {p1,p2,...,pn}，以及这些3D点投影在图像上的2D点在图像坐标系的坐标 { u 1 , u 2 , . . . , u n } \{u_1, u_2, ..., u_n\} {u1,u2,...,un}
-
这n个3D参考点和图像上2D投影点的的匹配关系（3D位置通常由三角化或者RGBD的深度图确定，对于双目或RGBD的里程计，可以直接用PnP估计相机运动，而单目视觉里程计需要先初始化）
-
相机内参K，畸变系数
待求变量：
- A坐标系(可以是世界坐标系，也可以是另一个相机坐标系)到相机坐标系的位姿变换，即旋转和平移
插图来自opencv官网。这里的相机坐标系，x轴指向有，y轴指向下，z轴指向前方。
为什么要用3D-2D的方法进行姿态估计？
这种方法不需要对极约束，有可以在很少的匹配点中获得较好的运动估计。
可选方法以及前提条件：
这类问题有很多种求解方法，比如P3P，直接线性变换(DLT)，EPnP，UPnP等等，还可以用非线性优化来构建最小二乘问题来迭代求解，即光束平差法(BA)。
| 选项 | DLT | P3P | EPnP | 非线性优化(BA) |
|---|---|---|---|---|
求解思路 |
构建增广矩阵(R|t)，通过投影矩阵构建12维的方程组 | 通过三个点对的相似三角形和余弦定理联立方程 | 通过控制点来变换，复杂度为O(n) | 把相机位姿和空间点位置都看成优化变量，最小化重投影误差 |
概况 |
至少要6个点对(12个未知数，每个点对构建两个方程)，大于6个时，可以用SVD等对超定方程求最小二乘解 | 需要4个点对，3个用来求解，一个点对用来验证，返回唯一的解 | 对于有噪音的特征点效果较好；求的是闭式解，不需要迭代和初始估计值 | |
不足 |
忽略了T矩阵之间的联系，求出的R不一定满足SO(3)，需要找到旋转矩阵对它近似 | 难以运用更多匹配点对的信息，遇到噪声或误匹配就会失效。需要4个点对，且这些点要是共面的 | 可能陷入局部最小 |
二. 直接线性变换DLT
已知一个3D点在世界坐标系的齐次坐标，其对应的2D投影点的齐次坐标，相机内参（不知道也可以，可以用PnP去估计K,R,t, 未知数多一点结果稍微差一点），那么就可以写出3D点到2D点的投影：
λ
(
u
v
1
)
=
(
f
x
0
c
x
0
f
y
c
y
0
0
1
)
(
R
∣
t
)
(
X
Y
Z
1
)
\lambda \left( \begin{array}{ccc} u \\ v \\ 1 \end{array}\right) = \left( \begin{array}{ccc} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0& 0& 1 \end{array}\right) \left( \begin{array}{ccc}R \ | \ t \end{array}\right) \left( \begin{array}{ccc} X \\ Y \\ Z\\ 1 \end{array}\right)
λ⎝⎛uv1⎠⎞=⎝⎛fx000fy0cxcy1⎠⎞(R ∣ t)⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
在这个方法中，忽略了旋转矩阵自身的正交约束，R和t相当于就是12个未知数，
(
R
∣
t
)
(R | t)
(R∣t)就是3*4的矩阵。
这时就可以自己动动手，把这个式子全部展开，然后消去
λ
\lambda
λ项，写成矩阵形式：
(
X
f
x
Y
f
x
Z
f
x
f
x
0
0
0
0
X
c
x
−
u
x
Y
c
x
−
u
Y
Z
c
x
−
u
Z
c
x
−
u
0
0
0
0
X
f
y
Y
f
y
Z
f
y
f
y
X
c
y
−
v
x
Y
c
y
−
v
Y
Z
c
y
−
v
Z
c
y
−
v
)
(
a
1
a
2
.
.
.
a
12
)
=
0
\left( \begin{array}{ccc} Xf_x & Yf_x & Zf_x & f_x & 0 & 0& 0 &0 &Xc_x-ux & Yc_x-uY & Zc_x - uZ & c_x - u \\ 0 & 0& 0 &0 & Xf_y & Yf_y & Zf_y & f_y & Xc_y-vx & Yc_y-vY & Zc_y - vZ & c_y - v \end{array}\right) \left( \begin{array}{ccc} a_1 \\ a_2 \\ .\\.\\.\\ a_{12} \end{array}\right) = 0
(Xfx0Yfx0Zfx0fx00Xfy0Yfy0Zfy0fyXcx−uxXcy−vxYcx−uYYcy−vYZcx−uZZcy−vZcx−ucy−v)⎝⎜⎜⎜⎜⎜⎜⎛a1a2...a12⎠⎟⎟⎟⎟⎟⎟⎞=0
可以看到一个点对可以提供2个方程，那么12个未知数就至少需要6个点对。
当超过6个点对时，就会出现超定，这时就要用SVD求解：阅读相关
这里存在一个问题，假设未知数的时候，12个未知数之间没有相互关系，求出来的旋转矩阵不一定能满足正定的要求。所以此时还要找一个最好的旋转矩阵跟它近似，这里可以使用QR分解，相当于把结果从矩阵空间重新投影到SE(3)流形上，转换成旋转和平移两部分。
R
←
(
R
R
T
)
−
1
2
R
R \leftarrow (RR^T)^{-\frac{1}{2}} R
R←(RRT)−21R
参考资料：
三. P3P
已知信息：
-
A.B.C三点在世界坐标系下的坐标 ，就可以求出AB，BC，AC的长度
-
∠ B O A ( α ) , ∠ A O C ( β ) , ∠ A O B ( γ ) \angle{BOA}(\alpha), \angle{AOC(\beta)},\angle{AOB}(\gamma) ∠BOA(α),∠AOC(β),∠AOB(γ)：这三个角度是已知的量，我个人是这么理解的，首先不能直接用OA，OB，OC加余弦定理来求，因为并不知道光心O在世界坐标系下的位置。在已知3D点世界坐标和2D点匹配的情况下，可以画出上图，可以发现，2D点和对应的相机坐标系中的点也可以形成相同的角度，这样，已知内参和像素坐标就可以求出夹角。
此时，可以先把像素坐标转化为归一化后的相机坐标系坐标，比如
(
x
z
,
y
z
,
1
)
=
(
u
f
x
−
c
x
f
x
,
v
f
y
−
c
y
f
y
,
1
)
(\frac{x}{z}, \frac{y}{z},1)=(\frac{u}{f_x}-\frac{c_x}{f_x},\frac{v}{f_y}-\frac{c_y}{f_y}, 1 )
(zx,zy,1)=(fxu−fxcx,fyv−fycy,1)，然后我们就可以求从光心O到这个归一化坐标点的模长，然后把这个向量长度归为单位长度，即让
k
X
1
s
,
k
X
2
s
,
k
X
3
s
^kX_1^s,^kX_2^s,^kX_3^s
kX1s,kX2s,kX3s都是从光心指向相机坐标点的单位向量。有了单位向量，直接做内积，就可以求出余弦值了，就可以得到三个角度。
最终目的：
通过余弦定理，我们可以得到OA，OB，OC的长度，也就可以得到ABC三个点在相机坐标系下的坐标。之后就可以转化成了3D(相机系)-3D(世界系) 的点对问题，来计算出相机在世界坐标系下的位姿R,t。步骤如下：
由上面的已知条件（绿色），我们要计算的就是图中的s1,s2,s3。 
从第一个式子开始，定义
u
=
s
2
s
1
,
v
=
s
3
s
1
u=\frac{s_2}{s_1}, v=\frac{s_3}{s_1}
u=s1s2,v=s1s3并代入：
a
2
=
s
1
2
(
u
2
+
v
2
−
2
u
v
cos
α
)
a^2 = s_1^2(u^2 + v^2 - 2uv\cos\alpha)
a2=s12(u2+v2−2uvcosα), 这样可以得到
s
1
2
=
a
2
u
2
+
v
2
−
2
u
v
cos
α
s_1^2=\frac{a^2}{u^2+v^2-2uv\cos\alpha}
s12=u2+v2−2uvcosαa2.
同理，把u和v代入第二和第三个式子可以得到
s
1
2
s_1^2
s12的另外两种表示，代入后就得到了一个四次多项式：
求出v就可以得到
s
1
,
s
2
,
s
3
s_1,s_2,s_3
s1,s2,s3，这里存在一个问题，就是会有4组解，所以需要另外的措施消除这种模糊性：
- 使用已知的从GPS得到的近似解
- 使用第四个点对进行验证，得到唯一的解
举一个四组解的例子：三个夹角都相等，3D点互相的距离也都相等
此时，ABC三点在相机和世界坐标系下的坐标都已知，可以根据此求解3D-3D的ICP问题，从而求解出相机的位姿。
四. EPnP
对应论文《EPnP: Accurate Non-Iterative O(n) Solution to the PnP Problem》
把2D图像点通过内参转换到相机坐标系的3D点，然后用ICP来解3D-3D的变换就可以得到位姿。核心是：求解3D参考点在相机坐标系下的坐标
解决方法是：使用4个控制点作为参考基准，让所有世界坐标系下的3D点都可以表示为这4个参考点的线性组合，权重值和为1。这样，就可以用原来世界坐标系（或相机坐标系）下的4个控制点来表示所有的世界坐标系（或相机坐标系）下的3D点。核心就变成了：求解4个控制点在相机坐标系下的坐标。
-
控制点的选取
原本控制点可以随机选取，为了有一个稳定结果，论文推荐了下面这个固定的选法：其中一个控制点为世界坐标系中所有点的质心，再用所有3D点坐标减去重心坐标，做成 A T A A^TA ATA的形式，再用SVD求出三个主方向，并加在重心坐标上得到其他三个控制点。
4个控制点在世界坐标系下的坐标为 c j w , j = 1 , . . . , 4 c_j^w, j=1,...,4 cjw,j=1,...,4，这四个点是可以通过3D点直接求出来的。令这4个控制点在相机坐标系下的坐标为 c j c , j = 1 , . . . , 4 c_j^c, j=1,...,4 cjc,j=1,...,4，注意对于不同的3D点，每一个点所对应的坐标权重值是不一样的。 -
同一个3D点对应的相机坐标系和世界坐标系下的4个控制点的权重系数关系
权重 α i j \alpha_{ij} αij是相同的，证明过程如下：
p i c = R c w P i w + t = R c w ∑ j = 1 4 α i j c j w + ∑ j = 1 4 α i j t = ∑ j = 1 4 α i j ( R c w c i w + t ) = ∑ j = 1 4 α i j c j c \mathbf{p}_i^c = R_{cw}P_i^w + t =R_{cw}\sum^4_{j=1} \alpha_{ij} \mathbf{c}_j^w + \sum_{j=1}^4 \alpha_{ij}t = \sum^4_{j=1} \alpha_{ij}(R_{cw}c_i^w + t) = \sum^4_{j=1} \alpha_{ij} \mathbf{c}_j^c pic=RcwPiw+t=Rcwj=1∑4αijcjw+j=1∑4αijt=j=1∑4αij(Rcwciw+t)=j=1∑4αijcjc
为了求相机坐标系下控制点的坐标，可以根据相机模型写出下面的方程：
∀
i
,
w
i
(
u
i
v
i
1
)
=
K
p
i
c
=
(
f
x
0
c
x
0
f
y
c
y
0
0
1
)
∑
j
=
1
4
α
i
j
(
x
j
c
y
j
c
z
j
c
)
\forall i, w_i\begin{pmatrix} u_i \\ v_i \\ 1 \end{pmatrix} = K \mathbf{p}^c_i = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0&1\end{pmatrix} \sum_{j=1}^4 \alpha_{ij} \begin{pmatrix} x_j^c \\ y_j^c \\ z_j^c \end{pmatrix}
∀i,wi⎝⎛uivi1⎠⎞=Kpic=⎝⎛fx000fy0cxcy1⎠⎞j=1∑4αij⎝⎛xjcyjczjc⎠⎞
这里的
K
K
K是内参矩阵，每一个控制点的相机系坐标为
c
j
c
=
[
x
j
c
,
y
j
c
,
z
j
c
]
T
,
j
=
1
,
.
.
.
4
c_j^c = [x_j^c, y_j^c, z_j^c]^T, j = 1,...4
cjc=[xjc,yjc,zjc]T,j=1,...4。这里的
w
i
w_i
wi就是深度，而且有
w
i
=
∑
j
=
1
4
α
i
j
z
j
c
w_i = \sum_{j=1}^4 \alpha_{ij}z_j^c
wi=∑j=14αijzjc. 把前两行减去最后一行，可以得到：
∑
j
=
1
4
α
i
j
f
x
x
j
c
+
α
i
j
(
c
x
−
u
i
)
z
j
c
=
0
∑
j
=
1
4
α
i
j
f
y
y
j
c
+
α
i
j
(
c
y
−
v
i
)
z
j
c
=
0
\begin{array}{l} \sum_{j=1}^4 \alpha_{ij} f_x x_j^c + \alpha_{ij}(c_x - u_i)z_j^c = 0 \\ \sum_{j=1}^4 \alpha_{ij} f_y y_j^c + \alpha_{ij}(c_y - v_i)z_j^c = 0 \end{array}
∑j=14αijfxxjc+αij(cx−ui)zjc=0∑j=14αijfyyjc+αij(cy−vi)zjc=0
这个方程组中，有四个控制点的相机坐标，也就是有12个未知数，可以提取出未知数
x
\mathbf{x}
x变为列向量，写成如下矩阵形式。
M
M
M的维度是
2
n
∗
12
2n * 12
2n∗12，
n
n
n是点对个数。
M
x
=
0
M\mathbf{x} = 0
Mx=0
具体的求解可以看下面的epnp的第二个参考链接。
此时权重系数已知，控制点的相机系坐标也已知，就可以恢复出3D点在相机坐标系下的位置了，3D点的世界坐标一开始就知道，所以就可以利用ICP求解位姿了。
为什么算法的复杂度是
O
(
n
)
O(n)
O(n)?
体现在求解
M
x
=
0
M\mathbf{x}=0
Mx=0时，求
x
\mathbf{x}
x时要先求维度为
12
∗
12
12*12
12∗12的
M
T
M
M^TM
MTM，这里的复杂度就是
O
(
n
)
O(n)
O(n)。文章来源：https://www.toymoban.com/news/detail-413130.html
参考：
DLT： https://zhuanlan.zhihu.com/p/58648937
P3P：https://zhuanlan.zhihu.com/p/140077137
Bonn大学P3P课件：https://www.ipb.uni-bonn.de/html/teaching/msr2-2020/sse2-15-p3p.pdf
EpnP：https://blog.csdn.net/zkk9527/article/details/107939991
https://zhuanlan.zhihu.com/p/59070440文章来源地址https://www.toymoban.com/news/detail-413130.html
到了这里，关于2D-3D匹配问题的PnP算法对比：DLT，P3P，EPnP的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
![[点云配准]LCD(2D-3D特征配准算法)例程align_point_cloud.py解析](https://imgs.yssmx.com/Uploads/2024/02/481623-1.png)
![2D-3D配准指南[方法汇总]【入门指导向】（一）问题介绍+LCD跨域描述子+Triplet loss](https://imgs.yssmx.com/Uploads/2024/02/779509-1.png)
