高博14讲--第七讲 视觉里程计-7.7 3D-2D:PnP
PnP的基本问题
PnP(Perspective-n-Point):已知3D点的空间位置(世界坐标)和相机上的投影点(像素坐标),求相机的旋转和平移(外参),估计相机的位姿,求出相机在世界坐标系下的位置和姿态。
3D-2D就是指,已知空间中点的三维的世界坐标和二维的像素坐标,通过多对这样的点,求相机的旋转和平移(外参),估计相机的位姿,求出相机在世界坐标系下的位置和姿态。
特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。
因此:
双目或RGB-D的视觉里程计,可直接使用PnP估计相机运动。
单目视觉里程计,必须先进行初始化,然后才能使用PnP。
- 给定: 一张已知特征点3D位置的图像,一张二维图像。
- 已知: 特征点在世界坐标系(指的是相机最初位置的那个坐标系)下的三维坐标,特征点映射到图像上的二维坐标,三维- - 坐标系中的特征点与二维图像上点的匹配关系。
- 未知: 相机坐标系在世界坐标系下的位姿,特征点在相机坐标系下的深度。
- 最初使用对极几何,初始化出特征点的深度。对于后续的二维图像(当前帧),当前帧与之前帧进行特征点匹配,匹配到的点若之前已经估计出深度,就知道了世界坐标系下的三维坐标和当前帧二维点的匹配关系,可以用PNP求解当前帧的相机,相对于世界坐标系的位姿变换。
两种求解方法
代数的解法
需要构造一些线性方程,用线性代数求解
例如: DLT,P3P,EPnP/UPnP/…
优化的解法
根据问题构建一个优化问题,一般定义一个误差,希望在某个估计值下面,把误差最小化,就得到了优化后的估计,这个估计可以认为是真正的值。
例如: Bundle Adjustment
代数与优化方法的对比
当运动比较连续时,用优化的方法,而线性代数的解法,解出的值比较粗糙,并对于噪声不够鲁棒,当观测量中出现一定的噪声,解的变化比较大。
当这个解完全不知道时,用代数的方法;
当解有一个比较好的初始值时,用优化的方法,从初始值开始,构建一个优化问题,通过迭代的方式,解这个优化问题。
直接线性变换(DTL)
空间中一点P,齐次坐标为
P
=
(
X
,
Y
,
Z
,
1
)
T
\ P=(X,Y,Z,1) ^ T
P=(X,Y,Z,1)T,在图像中的投影坐标为
x
1
=
(
u
1
,
v
1
,
1
)
T
\ x_1=(u_1,v_1,1)^T
x1=(u1,v1,1)T,相机的位姿R,t未知。定义增光矩阵[R|t]为3*4的矩阵,包含旋转和平移。
展开形式如下:
s
[
u
1
v
1
1
]
=
[
t
1
t
2
t
3
t
4
t
5
t
6
t
7
t
8
t
9
t
10
t
11
t
12
]
[
X
Y
Z
1
]
s \left[ \begin{matrix} u_1 \\ v_1 \\ 1 \end{matrix} \right]= \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} X \\ Y \\ Z \\ 1 \end{matrix} \right]
s⎣⎡u1v11⎦⎤=⎣⎡t1t5t9t2t6t10t3t7t11t4t8t12⎦⎤⎣⎢⎢⎡XYZ1⎦⎥⎥⎤
化简得:
[
s
u
1
s
v
1
s
]
=
[
t
1
X
+
t
2
Y
+
t
3
Z
+
t
4
t
5
X
+
t
6
Y
+
t
7
Z
+
t
8
t
9
X
+
t
10
Y
+
t
11
Z
+
t
12
]
\left[ \begin{matrix} su_1 \\ sv_1 \\ s \end{matrix} \right]= \left[ \begin{matrix} t_1X + t_2Y + t_3Z + t_4 \\ t_5X + t_6Y + t_7Z + t_8 \\ t_9X + t_{10}Y + t_{11}Z + t_{12} \end{matrix} \right]
⎣⎡su1sv1s⎦⎤=⎣⎡t1X+t2Y+t3Z+t4t5X+t6Y+t7Z+t8t9X+t10Y+t11Z+t12⎦⎤
用最后一行把s消去,得到两个约束:
u
1
=
t
1
X
+
t
2
Y
+
t
3
Z
+
t
4
t
9
X
+
t
10
Y
+
t
11
Z
+
t
12
,
v
1
=
t
5
X
+
t
6
Y
+
t
7
Z
+
t
8
t
9
X
+
t
10
Y
+
t
11
Z
+
t
12
u_1=\frac{t_1X+t_2Y+t_3Z+t_4}{t_9X+t_{10}Y+t_{11}Z+t_{12}},v_1=\frac{t_5X+t_6Y+t_7Z+t_8}{t_9X+t_{10}Y+t_{11}Z+t_{12}}
u1=t9X+t10Y+t11Z+t12t1X+t2Y+t3Z+t4,v1=t9X+t10Y+t11Z+t12t5X+t6Y+t7Z+t8
定义T的行向量:
t
1
=
(
t
1
,
t
2
,
t
3
,
t
4
)
T
,
t
2
=
(
t
5
,
t
6
,
t
7
,
t
8
)
T
,
t
3
=
(
t
9
,
t
10
,
t
11
,
t
12
)
T
t_1=(t_1,t_2,t_3,t_4)^T,t_2=(t_5,t_6,t_7,t_8)^T,t_3=(t_9,t_{10},t_{11},t_{12})^T
t1=(t1,t2,t3,t4)T,t2=(t5,t6,t7,t8)T,t3=(t9,t10,t11,t12)T
所以
u
1
,
v
1
\ u_1,v_1
u1,v1简化为:
u
1
=
t
1
T
P
t
3
T
P
,
v
1
=
t
2
T
P
t
3
T
P
u_1=\frac{t_1^TP}{t_3^TP},v_1=\frac{t_2^TP}{t_3^TP}
u1=t3TPt1TP,v1=t3TPt2TP
展开得:
t
1
T
P
−
t
3
T
P
u
1
=
0
,
t_1^TP-t_3^TPu_1=0,
t1TP−t3TPu1=0,
和
t
2
T
P
−
t
3
T
P
v
1
=
0.
t_2^TP-t_3^TPv_1=0 .
t2TP−t3TPv1=0.
t是待求的变量,由上式可知,每个特征点提供两个关于t的线性约束。
假设一共有N个特征点,则列出如下线性方程组:
[
P
1
T
0
−
u
1
P
1
T
0
P
1
T
−
v
1
P
1
T
.
.
.
.
.
.
P
N
T
0
−
u
N
P
N
T
0
P
N
T
−
u
N
P
N
T
]
[
t
1
t
2
t
3
]
=
0
\left[ \begin{matrix} P_1^T & 0 & -u_1P_1^T \\ 0 & P_1^T & -v_1P_1^T \\ .. & ..& .. \\ P_N^T & 0 & -u_NP_N^T \\ 0 & P_N^T & -u_NP_N^T \end{matrix} \right] \left[ \begin{matrix} t_1 \\ t_2 \\ t_3 \end{matrix} \right] =0
⎣⎢⎢⎢⎢⎡P1T0..PNT00P1T..0PNT−u1P1T−v1P1T..−uNPNT−uNPNT⎦⎥⎥⎥⎥⎤⎣⎡t1t2t3⎦⎤=0
t一共有12维,最少通过6对匹配点即可线性求解矩阵T,这种方法称为“直接线性变换”(Direct Linear Transform,DLT)。
P3P原理
利用三角形相似,求解投影点a,b,c在相机坐标系下的3D坐标,最后把问题转化为一个3D到3D的位姿估计问题。
P3P推导过程
P3P仅用三对匹配点,和一个验证点。
- 已知:A,B,C在世界坐标系下的三维点坐标,a,b,c是其在像平面的投影,已知其归一化坐标。
- 未知:A,B,C在相机坐标系下的坐标。
根据三角形的相似关系和余弦定理,△Oab-△OAB,△Obc-△OBC,△Oac-△OACA相似。可以推出:
其中x,y未知,也就是A,B,C在相机坐标系下的坐标,也就是OA,OB,OC的长度未知。
最后,方程组是关于x,y的二元二次方程,最多得到四个解,用验证点来计算最可能的解,得到A,B,C在相机坐标系下的3D坐标。
然后,根据3D-3D的点对,计算相机的运动
R
,
t
\ R,t
R,t。
P3P存在问题
- 只利用三个点的信息,多于三组点时,难以利用。
- 存在误匹配或噪声,则失效。