PnP
算法是指已知3D点及其在相机上的投影,求相机姿态的问题。
投影方程可以表示为:
λ
[
u
v
1
]
=
K
[
R
t
]
[
x
y
z
1
]
\lambda \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} R & t \end{bmatrix}\begin{bmatrix} x \\ y \\ z\\1 \end{bmatrix}
λ⎣⎡uv1⎦⎤=K[Rt]⎣⎢⎢⎡xyz1⎦⎥⎥⎤
这里的K
为相机内参矩阵,是已知的。我们要做的就是从n
对这样的对应关系中恢复出相机姿态。在SLAM应用中,系统初始化后,后面的跟踪就是基于求解PnP
问题。
1、求解PnP
问题的DLT
解法
首先,将
[
R
t
]
\begin{bmatrix} R & t \end{bmatrix}
[Rt]看作是一个包含12个未知量的3x4
矩阵,而将投影方程消去尺度因子
λ
\lambda
λ后,可以获得两个方程,在联立不少于6个的投影方程后,即可求出12个未知量的最小二乘解,最后恢复出
[
R
t
]
\begin{bmatrix} R & t \end{bmatrix}
[Rt]。
2、求解PnP
问题的EPnP
解法
它的思路是先将世界坐标系下的三维点转换到相机坐标系下,再用icp
的方式得到相机位姿。但是,如何在位姿未知的情况下转换?这里引入了控制点的概念,所有的三维点都可以由四个控制点表示,在世界坐标系和相机坐标系下只有基底的变化,系数是相同的。而这就要求获得控制点在相机坐标系下的坐标,这里同样利用的是投影方程联立求解。
以上具体推到过程参考:
https://zhuanlan.zhihu.com/p/58648937
https://zhuanlan.zhihu.com/p/59070440