一文详解PnP算法原理

一时失言乱红尘 2022-09-16 11:19 406阅读 0赞

PnP(Perspective-n-Point)问题的几何结构如图1所示，给定3D点的坐标、对应2D点坐标以及内参矩阵，求解相机的位姿。

数学语言描述如下：

![0bad42902c3269b44c659bae1da65d03.png][]

![7e0fd11c0b02e52a0ff830ed4b75539c.png][]

图1.PnP几何结构

## **1.直接线性变换法(Direct Linear Transform，DLT)** ##

假设：摄像机已经校准过了。

已知：

![e33fda630a35258296b239017ea3d8c1.png][]

求解相机的外参：R、t

透视投影模型为：

![0e8b7ff02c2d4d7e239977dd571e037d.png][]

每组3D-2D匹配点对应两个方程，一共有12个未知数，至少需要6组匹配点。

设有N组匹配点，则：

![9038de54e0c9bd7585f4062d5c473d32.png][]

上式写成矩阵形式：

AF=0

当N=6时，可以直接求解线性方程组。

![0c36bc4e807900f578cf9f01fb6c98a3.png][]

因此, 旋转矩阵, 平移矩阵求得:

![5779861e1fc45992633ea0bfae7ba88c.png][]

## **2.P3P** ##

P3P问题是已知三个3D目标点与其2D投影之间的对应关系，来确定标定相机的位姿问题。

![684ffd25d293d98191be4a1f61db95f4.png][]

![c234f226c139f367e301448302a3344b.png][]

图2.两点约束

![4c75ee84e8c76b99a44a4e1979e309a2.png][]

![11e45407454ff04ce94fa3ce02e70292.png][]

注：*直接线性变换法，只考虑了线性意义下的最优解，没有考虑几何约束。而P3P考虑了三角约束，给出三角约束意义下的最优解*

### **2.1 Zero Structure for the P3P Equation System** ###

文章\[1\]：Complete Solution Classification for the Perspective-Three-Point Problem

![d8da2bbed4e8045926ad7100660e5d19.png][]

图3.三点约束

对于公式（16）的变量有一些真实的约束：

![08ff004b523a3c6568278090ca87f6de.png][]

![5f1ffe61dad604a5e5d40c130662ab22.png][]

图4.三点约束--重定义边长

公式(17)消去C、v，得ES：

![885782ad83f16af581c91c29f6b3a271.png][]

### **2.2 PST** ###

文章\[2\]：A Stable Direct Solution of Perspective-Three-Point Problem

使用相似三角形，利用几何约束来减少未知参数的个数，把P3P方程组转化为四次方程，该称为透视相似三角形方法(Perspective Similar Triangle ，PST)。

#### （1）P3P问题转为PST问题 ####

![ad7a67750fe48dab7183eaba7f636f48.png][]

![284e7e4d04d0764ba567f5b8ae6b9c34.png][]

图5.P3P几何结构

#### （2）PST的求解 ####

![a729139f0f2bfc723cf0f8c445d9e153.png][]

![597d15ec4f63a63fd37f55435c5e5d51.png][]

图6.PST几何结构

**约束1：**相似三角形对应边成比例

![26790089b1d92a35d15048a0af5ab39d.png][]

所以：

![3625ffe0026826ea18138c31f6eb9c79.png][]

![85014346d438d788e80a5886ab20329f.png][]

![66154f3370568df045e32fd66935ca27.png][]

![e5ed80e8b3c217d5b8c3ef7471aca9e8.png][]

#### （3）PST多解和缺解问题 ####

由PST(perspective similar Triangle)求解，可得方程组等效转换为四次多项式

![16b0328b98f05c07f898d35502727b1b.png][]

多解问题：

由于存在多组解，相机位姿不能从3点集唯一确定的。解的个数直接对应于四次多项式实根的个数。要得到唯一的解，至少还应引入一点，构建2个三角形，进行求解。另一种方法是RANSAC算法，该算法将点集划分为3个点子集，检查这些子集的一致性。

RANSAC算法参考文献：Random Sample Consensus: A Paradigm for Model Fitting with Apphcatlons to Image Analysis and Automated Cartography

缺解问题：

![938da51ca202928b2b092892a25eb085.png][]

缺解问题是由P3P的固有结构决定的，其他P3P方法，如迭代解法、几何解法和分类法，也有同样的问题。

![03964db02e78135e97e1055576955e2c.png][]

![24084166033d6e53d302e7c7adc60af5.png][]

## **3.RPnP** ##

文章\[3\]：A Robust O(n) Solution to the Perspective-n-Point Problem

![9663929e4e26065eefe919156cde2800.png][]

下面来看一下，如何建立新的正交坐标系，以及如何求解正交坐标系到相机坐标系之间\[R T\]。

### 3.1确定旋转轴 ###

![7c88cd739fee869d9115bdc6f75847d1.png][]

![1b6366d095f315aac3d8327b48b1fa37.png][]

**当确定旋转轴时，只需求解剩余的旋转和三个平移参数，减少了未知变量的数量，来提高方程组的数值精度。**

### 3.2求解旋转角和平移矢量的方程 ###

相机坐标系与新坐标系绕之间的旋转矩阵：

![4003ea1f20b420968064e83a5acc1aea.png][]

![62788d6f802d61ea6bc8f86c5ed407d3.png][]

其中,

![874b35b4e714b0c664182fddf89c9bb7.png][]

### 3.3获取相机的位姿 ###

![f621449c632508d83996f194bbde5d67.png][]

再获取到相机坐标系与新坐标系绕之间的旋转和平移矩阵，进而可直接相机坐标系与世界坐标系绕之间旋转和平移矩阵，即相机的位姿。

## **引文：** ##

\[1\] Gao, X. S. , et al. "Complete solution classification for the perspective-three-point problem." IEEE Transactions on Pattern Analysis & Machine Intelligence 25.8(2003):930-943.

\[2\] Li, S. , and X. Chi . "A Stable Direct Solution of Perspective-Three-Point Problem." International Journal of Pattern Recognition and Artificial Intelligence 25.5(2011):627-642.

\[3\] Li, S. , X. Chi , and X. Ming . "A Robust O(n) Solution to the Perspective-n-Point Problem." IEEE Transactions on Pattern Analysis and Machine Intelligence (2012).

\[4\]W. Press, S. Teukolsky, W. Vetterling, and B. Flannery,Numerical Recipes:The Art of Scientific Computing.Cambridge Univ. Press, 2007.

\[5\]S. Umeyama, “Least-Squares Estimation of Transformation Parameters between Two Point Patterns,”IEEE Trans. Pattern Analysis and Machine Intelligence,vol. 13, no. 4, pp. 376-380, Apr. 1991.

**备注：**作者也是我们**「3D视觉从入门到精通」知识**特邀嘉宾：**[一个超干货的3D视觉学习社区][3D]**

**原创征稿**

**初衷**  
3D视觉工坊是基于**优质原创文章**的自媒体平台，创始人和合伙人致力于发布3D视觉领域最干货的文章，然而少数人的力量毕竟有限，知识盲区和领域漏洞依然存在。为了能够更好地展示领域知识，现向全体粉丝以及阅读者征稿，如果您的文章是**3D视觉**、**CV&深度学习**、**SLAM**、**三维重建**、**点云后处理**、**自动驾驶、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、硬件选型、求职分享等方向，**欢迎砸稿过来~文章内容可以为**paper reading、资源总结、项目实战总结**等形式，公众号将会对每一个投稿者提供相应的**稿费**，我们支持知识有价！

**投稿方式**

邮箱：vision3d@yeah.net 或者加下方的小助理微信，另请注明原创投稿。

![2385426b19cb6925803ed4c6d6f108a2.png][]

▲长按加微信联系

![6b26baa9230109a964cc6bfa67e2ddf0.png][]

▲长按关注公众号

[0bad42902c3269b44c659bae1da65d03.png]: /images/20220828/ebc0705b7d0d48cc939bffb4409e8888.png
[7e0fd11c0b02e52a0ff830ed4b75539c.png]: /images/20220828/9b5eb2317ea9455e924843e086bcd71c.png
[e33fda630a35258296b239017ea3d8c1.png]: /images/20220828/b4e3d77d7a3549238addd059a9c945c3.png
[0e8b7ff02c2d4d7e239977dd571e037d.png]: /images/20220828/e5e5af7ddeeb4949a220eaa284ed656d.png
[9038de54e0c9bd7585f4062d5c473d32.png]: /images/20220828/59763d9d95fd4adca7906c7df687bc3c.png
[0c36bc4e807900f578cf9f01fb6c98a3.png]: /images/20220828/b8b051270f8d494ebdfa4766166a94d7.png
[5779861e1fc45992633ea0bfae7ba88c.png]: /images/20220828/dffde98c2047496d838b1ff32cb65840.png
[684ffd25d293d98191be4a1f61db95f4.png]: /images/20220828/3a68bb18ef6844219a6b330be6230090.png
[c234f226c139f367e301448302a3344b.png]: /images/20220828/629912e0d64e4cb7ac16a83c2554f4b2.png
[4c75ee84e8c76b99a44a4e1979e309a2.png]: /images/20220828/2d2e283d575b4578a5317990ec56a1a2.png
[11e45407454ff04ce94fa3ce02e70292.png]: /images/20220828/d5f909f552be432c9e83091435213928.png
[d8da2bbed4e8045926ad7100660e5d19.png]: /images/20220828/54956367ab1a4a8eaf4acbca5e6aeccb.png
[08ff004b523a3c6568278090ca87f6de.png]: /images/20220828/b0a11e16da8140f7bfba5f45232a5fe4.png
[5f1ffe61dad604a5e5d40c130662ab22.png]: /images/20220828/1acc358768db4c9e8b9b22a358c408f0.png
[885782ad83f16af581c91c29f6b3a271.png]: /images/20220828/2000d77f66954480a490e474dc4b9899.png
[ad7a67750fe48dab7183eaba7f636f48.png]: /images/20220828/0bfc0a48b2b3405aafa640a90f3a2aa6.png
[284e7e4d04d0764ba567f5b8ae6b9c34.png]: /images/20220828/af4430f205ed4e26ad1f9eeab47bed6b.png
[a729139f0f2bfc723cf0f8c445d9e153.png]: /images/20220828/f7fc39f7242642f4946e5628c7dc8e7f.png
[597d15ec4f63a63fd37f55435c5e5d51.png]: /images/20220828/72f58e9d3b4f41c1816f7542f177da0b.png
[26790089b1d92a35d15048a0af5ab39d.png]: /images/20220828/4f1ba7549e254010980fb71d4ae929d1.png
[3625ffe0026826ea18138c31f6eb9c79.png]: /images/20220828/e98d8ec7e3a14bf189efb74838fe157d.png
[85014346d438d788e80a5886ab20329f.png]: /images/20220828/b83da9fd1fe24a3f9bd9c9cbef5e9147.png
[66154f3370568df045e32fd66935ca27.png]: /images/20220828/b329742ed62341e5b56e895006a3d273.png
[e5ed80e8b3c217d5b8c3ef7471aca9e8.png]: /images/20220828/4111ded8a04a4d23942b6c1252f2f694.png
[16b0328b98f05c07f898d35502727b1b.png]: /images/20220828/31c59c7867254bddb73e60ec9b6f1f74.png
[938da51ca202928b2b092892a25eb085.png]: /images/20220828/59dec7ace9824c7b8540c7c0ba2fc294.png
[03964db02e78135e97e1055576955e2c.png]: /images/20220828/48d8a2278f904f99b5c80e901077b097.png
[24084166033d6e53d302e7c7adc60af5.png]: /images/20220828/c24c68417eb44e179753794e96b93ee8.png
[9663929e4e26065eefe919156cde2800.png]: /images/20220828/c2a669b4ea784ba2a08e1ddd667cbc03.png
[7c88cd739fee869d9115bdc6f75847d1.png]: /images/20220828/b9b8e74bd7b148b4aac426c88eabbb81.png
[1b6366d095f315aac3d8327b48b1fa37.png]: /images/20220828/ce3517ee6c174bada1bc91e103143072.png
[4003ea1f20b420968064e83a5acc1aea.png]: /images/20220828/ab377d59e77648c7ab221ba0a9b2cb42.png
[62788d6f802d61ea6bc8f86c5ed407d3.png]: /images/20220828/9896077914154434936105303f2cd605.png
[874b35b4e714b0c664182fddf89c9bb7.png]: /images/20220828/a700827d384c488f81df498414fdf42b.png
[f621449c632508d83996f194bbde5d67.png]: /images/20220828/bceca232034748a983b64a552c20a9e2.png
[3D]: https://blog.csdn.net/Yong_Qi2015/article/details/111055882
[2385426b19cb6925803ed4c6d6f108a2.png]: /images/20220828/8067a7037d1743e7ae70a2f6f10ccf80.png
[6b26baa9230109a964cc6bfa67e2ddf0.png]: /images/20220828/b37f9325445d4c25b394c6661c0317c9.png