学习P3P_p3p算法_Gone_float的博客-程序员秘密

P3P 是另一种解 PnP 的方法。它仅使用三对匹配点，对数据要求较少。

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点为 $A, B, C$ ， 2D 点为 $a, b, c$ ，其中小写字母代表的点为大写字母在相机成像平面上的投影。此外， P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 $D - d$ ，相机光心为 $O$ 。请注意，我们知道的是 $A, B, C$ 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。一旦 3D 点在相机坐标系下的坐标能够算出，我们就得到了 3D-3D 的对应点，把 PnP 问题转换为了 ICP 问题。

在这里插入图片描述
显然，三角形之间存在对应关系：

$\Delta O a b-\Delta O A B, \quad \Delta O b c-\Delta O B C, \quad \Delta O a c-\Delta O A C$

来考虑 $O a b$ 和 $O A B$ 的关系，利用余弦定理，有：

$A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2}$

对于其他两个三角形亦有类似性质，于是有：

$A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2}$
$B^{2}+O C^{2}-2 O B \cdot O C \cdot \cos \langle b, c\rangle=B C^{2}$
$A^{2}+O C^{2}-2 O A \cdot O C \cdot \cos \langle a, c\rangle=A C^{2}$

对上面三式全体除以 $O C^{2}$ ，并且记 $x = O A / O C, y = O B / O C$ ，得：

$x^{2}+y^{2}-2 x y \cos \langle a, b\rangle=A B^{2} / O C^{2}$
$y^{2}+1^{2}-2 y \cos \langle b, c\rangle=B C^{2} / O C^{2}$
$x^{2}+1^{2}-2 x \cos \langle a, c\rangle=A C^{2} / O C^{2}$

记 $v=A B^{2} / O C^{2}, u v=B C^{2} / O C^{2}, w v=A C^{2} / O C^{2}$ ，有：

$x^{2}+y^{2}-2 x y \cos \langle a, b\rangle-v=0$
$y^{2}+1^{2}-2 y \cos \langle b, c\rangle-u v=0$
$x^{2}+1^{2}-2 x \cos \langle a, c\rangle-w v=0$

我们可以把第一个式子中的 $v$ 放到等式一边，并代入第 $2 ， 3$ 两式，得：

$y^{2}-u x^{2}-\cos \langle b, c\rangle y+2 u x y \cos \langle a, b\rangle+1=0$
$x^{2}-w y^{2}-\cos \langle a, c\rangle x+2 w x y \cos \langle a, b\rangle+1=0$

( $x, y$ 是未知量， $u, w, c o s ⟨ a, b ⟩, c o s ⟨ b, c ⟩, c o s ⟨ a, c ⟩$ 是已知量)

注意这些方程中的已知量和未知量。由于我们知道 $2 D$ 点的图像位置，三个余弦角 $c o s ⟨ a, b ⟩, c o s ⟨ b, c ⟩, c o s ⟨ a, c ⟩$ 是已知的。同时， $u=B C^{2} / A B^{2}, w=A C^{2} / A B^{2}$ 可以通过 $A, B, C$ 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的 $x, y$ 是未知的，随着相机移动会发生变化。因此，该方程组是关于 $x, y$ 的一个二元二次方程（多项式方程）。解析地求解该方程组是一个复杂的过程，需要用吴消元法。

这里不展开对该方程解法的介绍，感兴趣的读者请参照。类似于分解 $E$ 的情况，该方程最多可能得到四个解，但我们可以用验证点来计算最可能的解，得到 $A, B, C$ 在相机坐标系下 $3 D$ 坐标。然后，根据 $3 D - 3 D$ 的点对，计算相机的运动 $R, t$ 。

从 $P 3 P$ 的原理上可以看出，为了求解 $P n P$ ，我们利用了三角形相似性质，求解投影点 $a, b, c$ 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。

后文将看到，带有匹配信息的 3D-3D 位姿求解非常容易，所以这种思路是非常有效的。其他的一些方法，例如 EPnP，亦采用了这种思路。然而， P3P 也存在着一些问题：

P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。
如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。

所以后续人们还提出了许多别的方法，如 EPnP、 UPnP 等。它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响。不过，相对于 P3P 来说，原理会更加复杂一些，所以我们建议读者阅读原始的论文，或通过实践来理解 PnP 过程。

在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。在相机运动足够连续时，也可以假设相机不动或匀速运动，用推测值作为初始值进行优化。

视觉SLAM十四讲

本文链接：https://blog.csdn.net/qq_38766208/article/details/120082049

原作者删帖不实内容删帖广告或垃圾文章投诉

智能推荐

solidity教程（一）搭建僵尸工厂_僵尸工厂solidity_Clown95的博客-程序员秘密

title: solidity教程（一）基础tags: solidity,区块链第1章: 课程概述第一课你将创造一个&amp;quot;僵尸工厂&amp;quot;，用它建立一支僵尸部队。我们的工厂会把我们部队中所有的僵尸保存到数据库中工厂会有一个函数能产生新的僵尸每个僵尸会有一个随机的独一无二的面孔在后面的课程里，我们会增加功能。比如，让僵尸能攻击人类或其它僵尸! 但是在实现这些好玩的功能之前，我们先要实现创...

程序的本质在于逻辑_程序开发语言和程序逻辑哪个重要_dog250的博客-程序员秘密

有时候，特定的场合下，你会发现写一个bash脚本都会带来这样那样的问题，有些地方没有考虑到，而有些则过于冗余。即使你熟悉N中高级语言，同时精通底层汇编语言，又精通网络协议，配置各类网络设备已经到了炉火纯青的地步，即使如此，如果你是一个毫无逻辑的人，或者一时半会儿没有彻底理解需求导致逻辑不清晰，你都无法正确的编写出代码，哪怕是一段很短的bash脚本-如果不是冗长的C代码的话。因此程序

Java Web 2.2 CSS样式表_Boyka_大头的博客-程序员秘密

2.2 CSS样式表定义——CSS：（cascading style sheets，层叠样式表），通常说的样式表。CSS是一种美化网页的技术，通过使用CSS可以方便、灵活地设置网页中不同元素的外观属性，使网页在外观上达到一个更好的效果。CSS由三个基本部分组成：对象，属性，值。2.2.1 CSS样式表的几种定义方式：

计算机茶包装设计论文,创意设计论文茶叶包装设计论文_weixin_39899776的博客-程序员秘密

第37卷第2期2010年6月TEACOMMUNICATION茶叶通讯Vol．37，No．2June，2010茶叶包装设计研究徐茂刘玲(西南大学食品科学学院·重庆·400715)摘要：包装设计的重要功能是激发消费者的购买欲。这种购买心理可以通过包装的视觉冲击而触发。茶叶的包装作为商品与消费者之间的信息纽带，对其文化性的要求更显得突出。好的茶叶包装不仅可以提高销售份额，更可以提升品牌形象，使我国传统的...

睡后收益 -- 程序员秘密打赏功能及自定义模块以及代码分享_csdn博文打赏_AI架构师易筋的博客-程序员秘密

睡后收益 – 是指睡觉????的时候也有收益，比如出书的作者，各种互联网的产品等。很多朋友喜欢在网络上分享自己的学习心得、工作经验、技术见解，自己呕心沥血写的好文章当然是非常希望得到认可。本文介绍怎么为csdn博客增加打赏功能。修改步骤1. 点击右上角的创作2. 左侧栏滑动到创作权益 > 自定义模块笔者的内容如下<div> <p>邮箱：[email protected]</p> <p>商务合作VX：zgp

uni-app 实现锚点跳转方法_杨葱儿的博客-程序员秘密

核心代码：//从当前位置到达目标位置uni.createSelectorQuery().select('.comment-content').boundingClientRect(data=>{//目标位置的节点：类或者id uni.createSelectorQuery().select(".arc-content").boundingClientRect(res=>{//最外层盒子的节点：类或者id uni.pageScrollTo({ ...