PnP算法详解
- PnP概述
- PnP数学模型
- PnP求解方法
- DLT直接线性变换法
- EPnP
- EPnP的特点
- 步骤
- 理论推倒
- 1.控制点及齐次重心坐标系
- 2.控制点的选择
- 3.计算控制点在相机坐标系下的坐标
- 4.求解R，t（ICP方法）
- 参考文章
博主缺粉丝希望大家能给个关注！！！
PnP概述
PnP(Perspective-n-Point)是求解3D到2D点的对应方法。它描述了当知道n个3D空间点及其位置，如何估计相机的位姿。如果两张图像中的一张特征点3D位置已知，那么至少需要3个点对(以及至少一个额外验证点验证结果)就可以计算相机的运动。
- PnP的应用范围很广比如两阶段法的6D姿态估计以及视觉SLAM等等。
- 特征点的3D位置可以由三角化或者RGB-D相机的深度图确定，当然还有其他方法。
PnP数学模型
PnP问题的几何结构如下图所示，给定3D点的坐标以及对应2D点的坐标以及内参矩阵，求解相机的姿态。
已知:n个点在世界坐标系下的坐标
P
1
P_{1}
P1、
P
2
P_{2}
P2、…、
P
i
P_{i}
Pi、…、
P
n
P_{n}
Pn
对应像素的坐标
p
1
p_{1}
p1、
p
2
p_{2}
p2、…、
p
i
p_{i}
pi、…、
p
n
p_{n}
pn
相机内参
K
K
K
求解:相机坐标系(
O
c
X
c
Y
c
Z
c
O_{c}X_{c}Y_{c}Z_{c}
OcXcYcZc)相对于世界坐标系(
O
w
X
w
Y
w
Z
w
O_{w}X_{w}Y_{w}Z_{w}
OwXwYwZw)的位姿，公式中(1)中的[R t]
[
X
c
Y
c
Z
c
]
=
[
R
t
]
[
X
w
Y
w
Z
w
]
(
1
)
begin{bmatrix} X_{c}\ Y_{c}\ Z_{c}\ end{bmatrix} = [R t]begin{bmatrix} X_{w}\ Y_{w}\ Z_{w}\ end{bmatrix} (1)
⎣⎡XcYcZc⎦⎤=[R t]⎣⎡XwYwZw⎦⎤(1)
PnP求解方法
- DLT直接线性变换
- P3P三对点估计位姿
- EPnP(Efficient Pnp)
- BA(Bundle Adjustment)光速法平差
DLT直接线性变换法
假设：相机经过标定，也就是相机的内参K已知。
已知：空间中的3D点坐标：
[
X
w
Y
w
Z
w
]
T
begin{bmatrix} X_{w} & Y_{w} &Z_{w} \ end{bmatrix}^{T}
[XwYwZw]T 齐次坐标表示为
[
X
w
Y
w
Z
w
1
]
T
begin{bmatrix} X_{w} & Y_{w} &Z_{w} & 1 \ end{bmatrix}^{T}
[XwYwZw1]T
投影点的坐标：$begin{bmatrix}
u & v
end{bmatrix}^{T} $ 齐次坐标的表示为：
[
u
v
1
]
T
begin{bmatrix} u & v & 1\ end{bmatrix}^{T}
[uv1]T
详解的内参矩阵：
K
K
K
求解：相机外参
R
、
t
R、t
R、t
以下在Ipad上进行手写推倒
注意实际的R和t还应该乘以
K
−
1
K^{-1}
K−1
字有点丑，大家见谅吧!!!
这
里
我
有
个
问
题
:
color{red}{这里我有个问题:}
这里我有个问题:我推倒的时候只推出了
R
=
U
V
T
R=UV^{T}
R=UVT,没有推出正负解，很多博客这里给的是正负解，
R
=
±
U
V
T
R=pm UV^{T}
R=±UVT，这里面怎么出现的负解呢？
EPnP
原论文:EPnP: An Accurate O(n) Solution to the PnP Problem
EPnP的特点
- EPnP的复杂度是 O ( n ) O(n) O(n),其他算法的复杂度基本上是 O ( n 3 ) O(n^{3}) O(n3)，所以对于特征点较多的PnP问题，非常高效。
- 核心思想是将3D点表示为4个控制点的组合，优化也只针对4个控制点，所以速度很快，在求解 M x = 0 Mx=0 Mx=0时，最多考虑了4个奇异向量，因此精度也很高。
步骤
1.在世界坐标系下确定4个控制点
c
j
w
,
j
=
1
,
2
,
3
,
4
c^{w}_{j},j=1,2,3,4
cjw,j=1,2,3,4,理论上可以任意取这四个控制点，只要不共面就行(因为共面无法组成坐标系),但原论文给了一种方法，取所有点的质心为
c
1
w
c^{w}_{1}
c1w并作为原点，通过主成分分析PCA得到另外的三个点
c
2
w
c^{w}_{2}
c2w、
c
3
w
c^{w}_{3}
c3w、
c
4
w
c^{w}_{4}
c4w建立坐标系。
2.已知参考点(特征点)在世界坐标系的坐标，
P
i
w
,
j
=
1
,
.
.
.
,
n
P^{w}_{i},j=1,...,n
Piw,j=1,...,n,以及控制点在世界坐标系下的坐标，计算权重因子$alpha $
3.计算四个控制点在相机坐标系下的坐标
c
j
c
,
j
=
1
,
2
,
3
,
4
c^{c}_{j},j=1,2,3,4
cjc,j=1,2,3,4（核心）
4.计算参考点在相机坐标系下的坐标
P
i
c
,
j
=
1
,
.
.
.
,
n
P^{c}_{i},j=1,...,n
Pic,j=1,...,n
5.根据ICP方法，计算R，t。
