1 什么是P3P问题P3P 问题是经典的多视图几何问题之一，其中标定的相机的绝对位姿由三个 2D-3D 对应关系决定。由于这是许多视觉系统的关键（例如定位和SfM），因此过去有很多研究关注于如何开发更快、更稳定的P3P算法。虽然当前SOTA的求解器既非常快又稳定，但仍然存在可能崩溃的配置。 本文将问题代数化为寻找两个圆锥的交点。通过这个方式，我们能够分析表征多项式系统的实根，并为每个问题实例采用量身定制的解决方案。这导出了一个快速稳定的P3P求解器，它能够正确解决其它方法可能会失败的情况。实验评估表明，该方法在速度和成功率方面都优于当前的SOTA方法。
PnP是指根据2D-3D对应关系集合估计相机绝对位姿，集合最小的情况是P3P问题。P3P是将2D-3D对应关系通过相机内参转换为3D-3D对应关系进行求解。 给定世界坐标系中的3个3D点以及它们对应的归一化图像点，两个点集通过刚体变换关联：
其中是某个正数。P3P的目标是求解其中的旋转和平移。
2 P3P问题发展史P3P作为一个几何问题历史悠久，比计算机视觉领域的出现都要早很久。早在1773年Lagrange就在研究这个问题，Lagrange证明该问题最多可能有4个实数解，可以转化为4次多项式问题求解。大约1个世纪后，1841年德国数学家Grunert重新研究了该问题，给出了一种直接求解方法。20世纪早期，该问题在摄影测量领域内受到关注，但主要关注点在于精调而不是从头求解。Finstenvalder和Scheufele在1937年证明P3P问题只需要找到1个三次多项式的1个根和2个二次多项式的根。该问题后来在1981年Fischler和Bolles的RANSAC论文重新露面，由于RANSAC的成功，该问题也开始受到很大的关注。
根据最后需要求解的一元多项式的阶次，P3P问题可分为两大类：求解1个四次方程和求解1个三次方程。
最近的大多数工作关注于将P3P问题转化为求解四次方程问题。Gao等人在2003年用吴零点分解法第一次给出了P3P的完成解析解。Kneip等人2011年提出了一种直接由计算相机绝对位置和旋转的方式求解P3P问题的方法，避免了特征值分解或奇异值分解。Ke等人2017年提出用相应的几何约束确定相机的旋转。Banno和Nakano分别于2018和2019提出了P3P的直接求解法，通过估计中间坐标系中的距离，使得旋转矩阵可以形式化为距离的线性表示。
与基于四次方程的方法不同，基于三次方程的方法在P3P问题的文献中没有得到太多关注。自Finstenvalder和Scheufele在1937年的工作以来，Grafarend等人在1989年也使用了三次方程，他们试图将（3）简化为齐次形式，然后使用与Finstenvalder和Scheufele相同的技术求解。Haralick等人在1991年回顾了P3P问题的主要基于三次方程的解法，并讨论了数值精度。最近，Persson和Nordberg在2018年展示了关于寻找旋转和平移的更多细节，并提出了一种使用三次方程的一个根的有效算法，该方法比以前的方法有更好的数值精度，并且更快。
3 P3P问题转化为两个圆锥曲线相交问题参照Persson和Nordberg在2018年的解法，为了消除旋转和平移参数，如图1所示，有如下约束：
根据余弦定理，
其中。我们的目标是找到的解，从而求解旋转和平移。我们可以假设，不然3D点就和相机中心一样了。将(3)中前两个式子除以第三个式子，并通过变量替换，可以得到以下二元二次方程组：
其中
现在问题变为求两个二次方程的实数解，也就是说找到两个圆锥曲线的实数交点。
4 本文的方法本文的方法的基本思路也是求解两个圆锥曲线的相交问题。 (4)和(5)的两个二次方程可以重写为：
其中为的矩阵。为了找到交点，首先构造一个矩阵
交点可通过构建一个与真正的解相交的退化圆锥曲线找到。退化圆锥曲线由以下命题给出：
命题1（退化圆锥曲线，见《计算机视觉中的多视图几何》）如果矩阵 不满秩，则圆锥曲线退化。退化点的圆锥曲线是两条线(秩为2)或一条重复线(秩为1)，可以写为：
其中。
退化圆锥曲线被构造出来后，可以被分解为（至多）两条直线(和)，可以进一步很容易地与原来的两个圆锥曲线相交。
4.1 寻找退化圆锥曲线根据定理1，退化圆锥曲线需要非满秩，即行列式为0：
得到关于的三次方程。求解(11)可以得到的值，并得到矩阵。注意原始方程组的任何解(同时属于圆锥曲线)也在退化圆锥曲线上。
对于(11)中的每个解，都可以得到一个退化圆锥曲线。根据(10)，退化圆锥曲线是两条直线和的组合。那么如何将退化圆锥曲线分解为两条直线呢？
4.1.1 方法一：直接求解直线这里展示一种寻找直线的直接方法。假定已经找到了一个退化圆锥曲线，写为如下形式：
由于，假设，矩阵也可以写为：
假定，令，则
\tilde{p}_2+\tilde{q}_2& =2c_{12}/c_{11}, \\ \tilde{p}_2\tilde{q}_2& =c_{22}/c_{11}, \\ \tilde{p}_3+\tilde{q}_3& =2c_{13}/c_{11}, \\ \tilde{p}_2\tilde{q}_3+\tilde{p}_3\tilde{q}_2& =2c_{23}/c_{11},
根据(14)和(15)可以解出, 进而根据(16)和(17)可求得。在这种情况下，可以得到一对直线和。为了避免的情况，可以找到的绝对值最大的对角线元素，更稳定地计算出直线对。
4.1.2 方法二：通过求两直线相交求解直线由于两直线参数的叉乘可得交点，可以进而从中提取出两直线。对于交点，这里展示两种求法：
(1) 零空间法: 根据(10)可知, 交点在的零空间内， 对于的任意零空间向量，有。我们现在必须找到，使得的尺度与(以及)一致。由于，我们有
结合(12),(13)和(18)，可以推导出的范数和的元素之间的关系：
因此，可以将以正确的尺度适当地重新缩放得到交点。
(2) 伴随矩阵法:矩阵的伴随矩阵应满足：
证明：通过(13)可以得到
与相等。
给定一个矩阵，可以得到。为了避免0元素，可以找到其对角线最大的元素和对应的列，交点可以通过将该列除以对角线的平方根得到。
恢复直线：得到交点之后，根据其反对称矩阵
定义一个新的矩阵
结合(22)和(10)可得。直线对可以通过的行和列得到。
秩为1的情况： 如果退化圆锥曲线包括一对重复的直线，则矩阵的秩为1。在这种情况下，可以直接从一行或一列中恢复重复的直线。
4.2 P3P问题求解退化圆锥曲线中获得的直线过原二次方程组(7)与(8)的解（交点），因此可以通过求直线与两个圆锥曲线的交点进行求解。 假设第一条直线为：
将(23)代入(5)可得一个关于的二次方程，至多有2个解。需要注意的是，我们只关心正的实数解。得到后，根据(23)可以得到。由可得。代入（3）可得关于的二次方程
由于, 可以得到的解。这种情况下，可以得到的值。由于有一对直线，的解有4种可能。知道后，可以用Gauss-Newton优化(3)的平方和对结果进行细化。之前的工作也使用了类似的细化方法。
求解旋转和平移：对于每个，首先用(1)消除平移，得到以下方程组
为了找到另一个非共面向量量对应关系，与前人工作一样，可以使用由三个3D点和图像点定义的平面的法线（见图2）。法向量也满足
其中
结合(25)和(26)，可以解得旋转
得到旋转后由(1)可以求解平移。
图2：从向量对应关系到旋转
以上展示了P3P问题求解的一个通用算法，主要包括2个步骤：
*博客内容为网友个人发布，仅代表博主个人观点，如有侵权请联系工作人员删除。
发布文章
