一. 问题定义

首先需要清楚什么叫做PnP(Perspective-n-Point)呢？是为了解决什么问题？
已知信息：

n个3D点在A坐标系(可以认为是世界坐标系)的坐标 { p 1 , p 2 , . . . , p n } \{p_1, p_2, ..., p_n\} {p1,p2,...,pn}，以及这些3D点投影在图像上的2D点在图像坐标系的坐标 { u 1 , u 2 , . . . , u n } \{u_1, u_2, ..., u_n\} {u1,u2,...,un}
这n个3D参考点和图像上2D投影点的的匹配关系（3D位置通常由三角化或者RGBD的深度图确定，对于双目或RGBD的里程计，可以直接用PnP估计相机运动，而单目视觉里程计需要先初始化）
相机内参K，畸变系数

待求变量：

A坐标系(可以是世界坐标系，也可以是另一个相机坐标系)到相机坐标系的位姿变换，即旋转和平移

插图来自opencv官网。这里的相机坐标系，x轴指向有，y轴指向下，z轴指向前方。

为什么要用3D-2D的方法进行姿态估计？
这种方法不需要对极约束，有可以在很少的匹配点中获得较好的运动估计。

可选方法以及前提条件：
这类问题有很多种求解方法，比如P3P，直接线性变换(DLT)，EPnP，UPnP等等，还可以用非线性优化来构建最小二乘问题来迭代求解，即光束平差法(BA)。

选项	DLT	P3P	EPnP	非线性优化(BA)
`求解思路`	构建增广矩阵(R\|t)，通过投影矩阵构建12维的方程组	通过三个点对的相似三角形和余弦定理联立方程	通过控制点来变换，复杂度为O(n)	把相机位姿和空间点位置都看成优化变量，最小化重投影误差
`概况`	至少要6个点对(12个未知数，每个点对构建两个方程)，大于6个时，可以用SVD等对超定方程求最小二乘解	需要4个点对，3个用来求解，一个点对用来验证，返回唯一的解	对于有噪音的特征点效果较好；求的是闭式解，不需要迭代和初始估计值
`不足`	忽略了T矩阵之间的联系，求出的R不一定满足SO(3)，需要找到旋转矩阵对它近似	难以运用更多匹配点对的信息，遇到噪声或误匹配就会失效。需要4个点对，且这些点要是共面的		可能陷入局部最小

二. 直接线性变换DLT

已知一个3D点在世界坐标系的齐次坐标，其对应的2D投影点的齐次坐标，相机内参（不知道也可以，可以用PnP去估计K,R,t, 未知数多一点结果稍微差一点），那么就可以写出3D点到2D点的投影：
λ ( u v 1 ) = ( f x 0 c x 0 f y c y 0 0 1 ) ( R ∣ t ) ( X Y Z 1 ) \lambda \left( \begin{array}{ccc} u \\ v \\ 1 \end{array}\right) = \left( \begin{array}{ccc} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0& 0& 1 \end{array}\right) \left( \begin{array}{ccc}R \ | \ t \end{array}\right) \left( \begin{array}{ccc} X \\ Y \\ Z\\ 1 \end{array}\right) λ⎝⎛uv1⎠⎞=⎝⎛fx000fy0cxcy1⎠⎞(R ∣ t)⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
在这个方法中，忽略了旋转矩阵自身的正交约束，R和t相当于就是12个未知数， ( R ∣ t ) (R | t) (R∣t)就是3*4的矩阵。
这时就可以自己动动手，把这个式子全部展开，然后消去 λ \lambda λ项，写成矩阵形式：
( X f x Y f x Z f x f x 0 0 0 0 X c x − u x Y c x − u Y Z c x − u Z c x − u 0 0 0 0 X f y Y f y Z f y f y X c y − v x Y c y − v Y Z c y − v Z c y − v ) ( a 1 a 2 . . . a 12 ) = 0 \left( \begin{array}{ccc} Xf_x & Yf_x & Zf_x & f_x & 0 & 0& 0 &0 &Xc_x-ux & Yc_x-uY & Zc_x - uZ & c_x - u \\ 0 & 0& 0 &0 & Xf_y & Yf_y & Zf_y & f_y & Xc_y-vx & Yc_y-vY & Zc_y - vZ & c_y - v \end{array}\right) \left( \begin{array}{ccc} a_1 \\ a_2 \\ .\\.\\.\\ a_{12} \end{array}\right) = 0 (Xfx0Yfx0Zfx0fx00Xfy0Yfy0Zfy0fyXcx−uxXcy−vxYcx−uYYcy−vYZcx−uZZcy−vZcx−ucy−v)⎝⎜⎜⎜⎜⎜⎜⎛a1a2...a12⎠⎟⎟⎟⎟⎟⎟⎞=0
可以看到一个点对可以提供2个方程，那么12个未知数就至少需要6个点对。

当超过6个点对时，就会出现超定，这时就要用SVD求解：阅读相关

这里存在一个问题，假设未知数的时候，12个未知数之间没有相互关系，求出来的旋转矩阵不一定能满足正定的要求。所以此时还要找一个最好的旋转矩阵跟它近似，这里可以使用QR分解，相当于把结果从矩阵空间重新投影到SE(3)流形上，转换成旋转和平移两部分。
R ← ( R R T ) − 1 2 R R \leftarrow (RR^T)^{-\frac{1}{2}} R R←(RRT)−21R

参考资料：

三. P3P

已知信息：

A.B.C三点在世界坐标系下的坐标，就可以求出AB，BC，AC的长度
∠ B O A ( α ) , ∠ A O C ( β ) , ∠ A O B ( γ ) \angle{BOA}(\alpha), \angle{AOC(\beta)},\angle{AOB}(\gamma) ∠BOA(α),∠AOC(β),∠AOB(γ)：这三个角度是已知的量，我个人是这么理解的，首先不能直接用OA，OB，OC加余弦定理来求，因为并不知道光心O在世界坐标系下的位置。在已知3D点世界坐标和2D点匹配的情况下，可以画出上图，可以发现，2D点和对应的相机坐标系中的点也可以形成相同的角度，这样，已知内参和像素坐标就可以求出夹角。

此时，可以先把像素坐标转化为归一化后的相机坐标系坐标，比如 ( x z , y z , 1 ) = ( u f x − c x f x , v f y − c y f y , 1 ) (\frac{x}{z}, \frac{y}{z},1)=(\frac{u}{f_x}-\frac{c_x}{f_x},\frac{v}{f_y}-\frac{c_y}{f_y}, 1 ) (zx,zy,1)=(fxu−fxcx,fyv−fycy,1)，然后我们就可以求从光心O到这个归一化坐标点的模长，然后把这个向量长度归为单位长度，即让 k X 1 s , k X 2 s , k X 3 s ^kX_1^s,^kX_2^s,^kX_3^s kX1s,kX2s,kX3s都是从光心指向相机坐标点的单位向量。有了单位向量，直接做内积，就可以求出余弦值了，就可以得到三个角度。

最终目的：
通过余弦定理，我们可以得到OA，OB，OC的长度，也就可以得到ABC三个点在相机坐标系下的坐标。之后就可以转化成了3D(相机系)-3D(世界系) 的点对问题，来计算出相机在世界坐标系下的位姿R,t。步骤如下：

由上面的已知条件（绿色），我们要计算的就是图中的s1,s2,s3。
从第一个式子开始，定义 u = s 2 s 1 , v = s 3 s 1 u=\frac{s_2}{s_1}, v=\frac{s_3}{s_1} u=s1s2,v=s1s3并代入：
a 2 = s 1 2 ( u 2 + v 2 − 2 u v cos ⁡ α ) a^2 = s_1^2(u^2 + v^2 - 2uv\cos\alpha) a2=s12(u2+v2−2uvcosα), 这样可以得到 s 1 2 = a 2 u 2 + v 2 − 2 u v cos ⁡ α s_1^2=\frac{a^2}{u^2+v^2-2uv\cos\alpha} s12=u2+v2−2uvcosαa2.
同理，把u和v代入第二和第三个式子可以得到 s 1 2 s_1^2 s12的另外两种表示，代入后就得到了一个四次多项式：

求出v就可以得到 s 1 , s 2 , s 3 s_1,s_2,s_3 s1,s2,s3，这里存在一个问题，就是会有4组解，所以需要另外的措施消除这种模糊性：

使用已知的从GPS得到的近似解
使用第四个点对进行验证，得到唯一的解

举一个四组解的例子：三个夹角都相等，3D点互相的距离也都相等

此时，ABC三点在相机和世界坐标系下的坐标都已知，可以根据此求解3D-3D的ICP问题，从而求解出相机的位姿。

四. EPnP

对应论文《EPnP: Accurate Non-Iterative O(n) Solution to the PnP Problem》

把2D图像点通过内参转换到相机坐标系的3D点，然后用ICP来解3D-3D的变换就可以得到位姿。核心是：求解3D参考点在相机坐标系下的坐标

解决方法是：使用4个控制点作为参考基准，让所有世界坐标系下的3D点都可以表示为这4个参考点的线性组合，权重值和为1。这样，就可以用原来世界坐标系（或相机坐标系）下的4个控制点来表示所有的世界坐标系（或相机坐标系）下的3D点。核心就变成了：求解4个控制点在相机坐标系下的坐标。

控制点的选取
原本控制点可以随机选取，为了有一个稳定结果，论文推荐了下面这个固定的选法：其中一个控制点为世界坐标系中所有点的质心，再用所有3D点坐标减去重心坐标，做成 A T A A^TA ATA的形式，再用SVD求出三个主方向，并加在重心坐标上得到其他三个控制点。
4个控制点在世界坐标系下的坐标为 c j w , j = 1 , . . . , 4 c_j^w, j=1,...,4 cjw,j=1,...,4，这四个点是可以通过3D点直接求出来的。令这4个控制点在相机坐标系下的坐标为 c j c , j = 1 , . . . , 4 c_j^c, j=1,...,4 cjc,j=1,...,4，注意对于不同的3D点，每一个点所对应的坐标权重值是不一样的。
同一个3D点对应的相机坐标系和世界坐标系下的4个控制点的权重系数关系
权重 α i j \alpha_{ij} αij是相同的，证明过程如下：
p i c = R c w P i w + t = R c w ∑ j = 1 4 α i j c j w + ∑ j = 1 4 α i j t = ∑ j = 1 4 α i j ( R c w c i w + t ) = ∑ j = 1 4 α i j c j c \mathbf{p}_i^c = R_{cw}P_i^w + t =R_{cw}\sum^4_{j=1} \alpha_{ij} \mathbf{c}_j^w + \sum_{j=1}^4 \alpha_{ij}t = \sum^4_{j=1} \alpha_{ij}(R_{cw}c_i^w + t) = \sum^4_{j=1} \alpha_{ij} \mathbf{c}_j^c pic=RcwPiw+t=Rcwj=1∑4αijcjw+j=1∑4αijt=j=1∑4αij(Rcwciw+t)=j=1∑4αijcjc

为了求相机坐标系下控制点的坐标，可以根据相机模型写出下面的方程：
∀ i , w i ( u i v i 1 ) = K p i c = ( f x 0 c x 0 f y c y 0 0 1 ) ∑ j = 1 4 α i j ( x j c y j c z j c ) \forall i, w_i\begin{pmatrix} u_i \\ v_i \\ 1 \end{pmatrix} = K \mathbf{p}^c_i = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0&1\end{pmatrix} \sum_{j=1}^4 \alpha_{ij} \begin{pmatrix} x_j^c \\ y_j^c \\ z_j^c \end{pmatrix} ∀i,wi⎝⎛uivi1⎠⎞=Kpic=⎝⎛fx000fy0cxcy1⎠⎞j=1∑4αij⎝⎛xjcyjczjc⎠⎞

这里的 K K K是内参矩阵，每一个控制点的相机系坐标为 c j c = [ x j c , y j c , z j c ] T , j = 1 , . . . 4 c_j^c = [x_j^c, y_j^c, z_j^c]^T, j = 1,...4 cjc=[xjc,yjc,zjc]T,j=1,...4。这里的 w i w_i wi就是深度，而且有 w i = ∑ j = 1 4 α i j z j c w_i = \sum_{j=1}^4 \alpha_{ij}z_j^c wi=∑j=14αijzjc. 把前两行减去最后一行，可以得到：
∑ j = 1 4 α i j f x x j c + α i j ( c x − u i ) z j c = 0 ∑ j = 1 4 α i j f y y j c + α i j ( c y − v i ) z j c = 0 \begin{array}{l} \sum_{j=1}^4 \alpha_{ij} f_x x_j^c + \alpha_{ij}(c_x - u_i)z_j^c = 0 \\ \sum_{j=1}^4 \alpha_{ij} f_y y_j^c + \alpha_{ij}(c_y - v_i)z_j^c = 0 \end{array} ∑j=14αijfxxjc+αij(cx−ui)zjc=0∑j=14αijfyyjc+αij(cy−vi)zjc=0

这个方程组中，有四个控制点的相机坐标，也就是有12个未知数，可以提取出未知数 x \mathbf{x} x变为列向量，写成如下矩阵形式。 M M M的维度是 2 n ∗ 12 2n * 12 2n∗12， n n n是点对个数。
M x = 0 M\mathbf{x} = 0 Mx=0
具体的求解可以看下面的epnp的第二个参考链接。

此时权重系数已知，控制点的相机系坐标也已知，就可以恢复出3D点在相机坐标系下的位置了，3D点的世界坐标一开始就知道，所以就可以利用ICP求解位姿了。

为什么算法的复杂度是 O ( n ) O(n) O(n)?
体现在求解 M x = 0 M\mathbf{x}=0 Mx=0时，求 x \mathbf{x} x时要先求维度为 12 ∗ 12 12*12 12∗12的 M T M M^TM MTM，这里的复杂度就是 O ( n ) O(n) O(n)。

参考：
DLT： https://zhuanlan.zhihu.com/p/58648937
P3P：https://zhuanlan.zhihu.com/p/140077137
Bonn大学P3P课件：https://www.ipb.uni-bonn.de/html/teaching/msr2-2020/sse2-15-p3p.pdf
EpnP：https://blog.csdn.net/zkk9527/article/details/107939991
https://zhuanlan.zhihu.com/p/59070440

2D-3D匹配问题的PnP算法对比：DLT，P3P，EPnP相关推荐

3D重建传统算法对比深度学习，SFU谭平：更需要的是二者的融合
点击我爱计算机视觉标星,更快获取CVML新技术机器之心原创作者:一鸣近年来,深度学习在计算机视觉的重要领域--三维重建中取得了一系列成果.然而,最近有论文指出,深度学习的 3D 重建表现甚至不如 ...
3D-2D：PnP算法原理
3D-2D:PnP算法原理 1.问题背景-- 什么是PnP问题 ? 2.PnP问题的求解方法 2.1 P3P 2.1.1 算法的实际理解 2.1.2 算法的数学推导 2.1.3 算法的缺陷 2.2 直 ...
3D-BoNet：比3D点云实例分割算法快10倍！代码已开源
点击我爱计算机视觉标星,更快获取CVML新技术本文转载自新智元(AI_era) 新智元报道来源:投稿编辑:元子 [新智元导读]本文提出了一种基于边界框回归的高效点云实例分割算法,通过最小 ...
单目车辆3Dbox检测算法对比
现有的3dBox的检测算法大部分都是直接回归pose a)将3D模型遍历姿态参数,投影到2D,进行HOG特征匹配: b)将3D模型遍历姿态参数,投影到2D,进行shape匹配: c)CNN直接回归p ...
3D视觉(六)：PnP问题（pespective-n-point）
3D视觉(六):PnP问题(pespective-n-point) PnP问题,是指已知3D点(x, y, z)及其在相机上的投影(u,v),求解相机位姿变换R.T. 投影方程可表示为: 这里K为相机 ...
SLAM | 激光SLAM中开源算法对比
点击上方"AI算法修炼营",选择加星标或"置顶" 标题以下,全是干货前面的话好久没有更新SLAM系列的文章了,前面我们讲到了激光SLAM技术.基于激光雷达的 ...
Waymo 2020 | 2D/3D目标检测、跟踪和域自适应性冠军解决方案解析
©PaperWeekly 原创 · 作者|黄飘学校|华中科技大学硕士研究方向|多目标跟踪随着最近 Waymo Open Dataset Challenges 2020 的落幕,其中关于 2D/3 ...
PnP算法详解（超详细公式推导）
PnP算法详解 PnP概述 PnP数学模型 PnP求解方法 DLT直接线性变换法 EPnP EPnP的特点步骤理论推倒 1.控制点及齐次重心坐标系 2.控制点的选择 3.计算控制点在相机坐标系下的 ...
PnP算法简介与代码解析-柴政
PnP算法简介与代码解析-柴政 PnP求解算法是指通过多对3D与2D匹配点,在已知或者未知相机内参的情况下,利用最小化重投影误差来求解相机外参的算法.PnP求解算法是SLAM前端位姿跟踪部分中常用的算 ...

2D-3D匹配问题的PnP算法对比：DLT，P3P，EPnP

一. 问题定义

二. 直接线性变换DLT

三. P3P

四. EPnP

2D-3D匹配问题的PnP算法对比：DLT，P3P，EPnP相关推荐

最新文章

热门文章