高博14讲--第七讲视觉里程计-7.7 3D-2D:PnP（未完待续）

快来打我* 2021-12-19 03:23 290阅读 0赞

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

*  PnP的基本问题
 *  两种求解方法
 *   *  代数的解法
     *  优化的解法
     *  代数与优化方法的对比
 *  直接线性变换（DTL）
 *   *  P3P原理
     *  P3P推导过程
     *  P3P存在问题

# 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推导过程 ##

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3podWlxaXV6aHVveXVlNTgz_size_16_color_FFFFFF_t_70]  
 P3P仅用三对匹配点，和一个验证点。

*   已知：A,B,C在世界坐标系下的三维点坐标，a,b,c是其在像平面的投影，已知其归一化坐标。
 *  未知：A,B,C在相机坐标系下的坐标。

根据三角形的相似关系和余弦定理，△Oab-△OAB,△Obc-△OBC,△Oac-△OACA相似。可以推出：

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3podWlxaXV6aHVveXVlNTgz_size_16_color_FFFFFF_t_70 1]![在这里插入图片描述][20190704233424732.png]   其中x,y未知，也就是A,B,C在相机坐标系下的坐标，也就是OA,OB,OC的长度未知。  
  最后，方程组是关于x，y的二元二次方程，最多得到四个解，用验证点来计算最可能的解，得到A,B,C在相机坐标系下的3D坐标。  
  然后，根据3D-3D的点对，计算相机的运动   R , t \\ R,t  R,t。

## P3P存在问题 ##

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

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3podWlxaXV6aHVveXVlNTgz_size_16_color_FFFFFF_t_70]: /images/20211219/ed201fed97604a2d89771899b72d76bd.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3podWlxaXV6aHVveXVlNTgz_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/2019070423331216.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3podWlxaXV6aHVveXVlNTgz,size_16,color_FFFFFF,t_70
[20190704233424732.png]: /images/20211219/d502525e942c43c39c4442038e1e08da.png