高博14讲--第七讲 视觉里程计-7.7 3D-2D:PnP

PnP的基本问题

   PnP（Perspective-n-Point）：已知3D点的空间位置（世界坐标）和相机上的投影点（像素坐标），求相机的旋转和平移（外参），估计相机的位姿。

   3D-2D就是指，已知空间中点的三维的世界坐标和二维的像素坐标，通过多对这样的点，求相机的旋转和平移（外参），估计相机的位姿。

  特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。
  因此：
     双目或RGB-D的视觉里程计，可直接使用PnP估计相机运动。
     单目视觉里程计，必须先进行初始化，然后才能使用PnP。

• 给定：一张已知特征点3D位置的图像，一张二维图像。
• 已知：特征点在世界坐标系(指的是相机最初位置的那个坐标系)下的三维坐标，特征点映射到图像上的二维坐标，三维- - 坐标系中的特征点与二维图像上点的匹配关系。
• 未知：相机坐标系在世界坐标系下的位姿，特征点在相机坐标系下的深度。
• 最初使用对极几何，初始化出特征点的深度。对于后续的二维图像（当前帧），当前帧与之前帧进行特征点匹配，匹配到的点若之前已经估计出深度，就知道了世界坐标系下的三维坐标和当前帧二维点的匹配关系，可以用PNP求解当前帧的相机，相对于世界坐标系的位姿变换。

直接线性变换（DTL）

  空间中一点P，齐次坐标为$P=(X,Y,Z,1{)}^T$，在图像中的投影坐标为$x_1=(u_1,v_1,1{)}^T$，相机的位姿R,t未知。定义增光矩阵[R|t]为3*4的矩阵,包含旋转和平移。
展开形式如下：
$$s\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}{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{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$
化简得：
$$\left[\begin{array}{c}s{u}_1\\ s{v}_1\\ s\end{array}\right]=\left[\begin{array}{c}{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}\end{array}\right]$$
用最后一行把s消去，得到两个约束：
$$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}},v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+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$$
所以$u_1,v_1$简化为：
$$u_1=\frac{t_1^{T}P}{t_3^{T}P},v_1=\frac{t_2^{T}P}{t_3^{T}P}$$
展开得：
$$t_1^{T}P-t_3^{T}P{u}_1=0,$$
和
$$t_2^{T}P-t_3^{T}P{v}_1=0.$$
t是待求的变量，由上式可知，每个特征点提供两个关于t的线性约束。


假设一共有N个特征点，则列出如下线性方程组：
$$\left[\begin{array}{ccc}{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\end{array}\right]\left[\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right]=0$$
t一共有12维，最少通过6对匹配点即可线性求解矩阵T，这种方法称为“直接线性变换”（Direct Linear Transform,DLT）。

P3P

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$。

P3P存在问题

• 只利用三个点的信息，多于三组点时，难以利用。
• 存在误匹配或噪声，则失效。