P3P求解相机姿态原理介绍

简介

首先，PnP算法是利用相机内参、瞄点在图像坐标系以及对应世界坐标系下的位置 ，求取相机在真实坐标系下姿态的问题 ；
其本质上是求解相机坐标系到世界坐标系的变换；

p3p

利用三个点对，余弦定理来求解 ，可以求出4个解，所以至少需要四个点对才能求取出没有歧义的解；
其求解过程可以分为两个步骤
1.根据余弦定理估计投射线的长度；
2.计算相机坐标系到世界坐标系的变换；

我们首先需要知道的是P3P并不是直接根据3D-2D点求出相机位姿矩阵，而是先求出对应的2D点在当前相机坐标系下的3D坐标，然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标求解相机位姿的。P3P的求解是从余弦定理开始的，设相机坐标中心为点P，A、B、C为不共线的三个3D点，D为验证3D点，根据余弦定理有如下公式：


以上公式中未知的只有PA、PB、PC三个变量；

求解上述三元二次方程，具体化简过程不表，我们可以得到PA、PB、PC，利用这个值 我们继而可以得到A、B、C在相机坐标系下的位置，然后根据A、B、C在相机坐标系下的位置和世界坐标系下的对应位置，求解两个坐标系的变换，进而可以得到相机在世界坐标系下的姿态；

DLT

DLT主要是通过构建一个增广矩阵（R|t），然后通过投影矩阵构建一个方程： 微信图片_20201207104140 通过最后一行，消去s，最后构建一个12维的线性方程组，通过6对匹配点（一对点两个方程）来求解中间的矩阵。 这种方式是通过2D-3D的关系直接构建方程，直接求解。

通过最后一行，消去s，最后构建一个12维的线性方程组，通过6对匹配点（一对点两个方程）来求解中间的矩阵。 这种方式是通过2D-3D的关系直接构建方程，直接求解。