第6章-摄像机未标定的P5P问题.doc

第九章 摄像机未标定的P5P问题所谓PNP问题是指通过控制点和它们的图像点求解摄像机的方位问题，也称为透视N点问题 (Perspective N Point)或N点定位问题，是计算机视觉、摄影测量学乃至数学领域的一个重要研究课题，其研究成果可直接应用于机器人定位、导航等。自从PNP问题首次于1981年提出后1，由于其在物体定位方面的重要应用价值，引起了人们的广泛重视，已有很多相关的文章问世2-14。在传统的研究方法中，均假定摄像机的内参数是已知的且在运动过程中是不变的。为了获取高质量的图像，目前数字摄像机都具有自动变焦的功能，也就是说机器人在运动过程中摄像机内参数可能经常发生变化。因此，要求摄像机内参数是已知的而且在运动过程中保持不变，已很难满足实际应用的要求。本章针对五参数针孔摄像机模型，主要讨论摄像机在运动（运动参数未知）过程中其内参数是未知的且可以发生变化时，如何通过5个控制点以及它们的图像点，来求解所对应的内参数、方位以及运动参数。证明了下述结论：已知5个控制点在世界坐标系中的坐标，以及它们在摄像机作一般刚体运动前、后两幅图像中的图像坐标，当5个控制点中任意4个点均不共面且摄像机运动前、后的两光心的连线不通过任一个控制点时，则可线性地确定摄像机关于世界坐标系的方位、运动前、后所对应的内参数以及运动参数。本章的研究成果在机器人视觉中具有一定的理论意义和应用价值。9.1 PNP问题概述9.1.1 PNP问题的定义PNP问题的定义是：已知n个控制点在世界坐标系中的坐标与对应的图像点在像平面中的坐标，求摄像机的内参数矩阵以及摄像机坐标系关于世界坐标系的方向与位置，如图6-1所示。其中R为旋转矩阵，表示摄像机的方向；t是平移向量，表示摄像机的位置。（1）当摄像机内参数矩阵是已知的，则上述问题是经典的PNP问题。（2）摄像机内参数矩阵是未知的，上述问题称为摄像机未标定的PNP问题。图9-1 PNP问题示意图9.1.2 已有结论目前，对于摄像机内参数已知且在运动过程中保持不变的P3P问题、P4P问题和P5P问题有如下结论：P3P问题最多有4个解且解的上限可以达到；P4P问题当4个控制点共面时，问题有唯一解, 当4个控制点不共面时，问题最多可能有4个解且解的上限可以达到；P5P问题至多有两个解且解的上限可以达到。对于摄像机内参数未知且在运动过程中是变化的PNP问题，相关的研究报导还很少23。在许多实际问题中（如基于主动视觉系统的任务），摄像机内参数需要经常被调节而发生变化，因此不能假定摄像机内参数已知且在运动过程中是不变的。这样，研究未标定的PNP问题就具有十分重要的意义。如果摄像机使用五参数模型，对于未标定的P3P、P4P问题，其约束方程的个数均少于待求未知参数的个数，所以未标定的P3P、P4P问题总是有无穷多解。对于摄像机为四参数模型的未标定P5P问题，文献2给出了下述结果：当5个控制点中任意4个不共面，或者存在4个共面的控制点但任意三个图像点不共线时，未标定的P5P问题的解仅有两种可能：（1）至多有4个解；（2）有无穷多解。无穷多解的情形是可以发生的，但极其少见。该文同时给出了至多有4个解和有无穷多解的代数条件，以及求解未标定P5P问题的具体算法。9.1.3 本章所研究的问题不论是经典的PNP问题还是未标定的PNP问题，大都是使用单幅图像。由于机器人在运动过程中通常可以获得控制点的多幅图像，我们可以通过两幅图像来求解PNP问题吗？这正是本章所要研究的问题。确切地说本章讨论下述问题：假定摄像机作一般刚体运动，运动参数未知，且在运动过程中内参数可以发生变化。已知5个控制点在世界坐标系中的坐标，以及它们在运动前、后两幅图像中的图像坐标，求摄像机运动前、后关于世界坐标系的方位，运动前、后所对应的内参数以及运动参数。本章假定摄像机模型为针孔五参数模型，即摄像机内参数矩阵为：其中分别称为图像平面u、v轴的尺度因子，为主点坐标，s表示摄像机的畸变因子。我们先分析一下问题求解的可能性。两个内参数矩阵有10个独立的参数、两个旋转矩阵有6个独立的参数、两个平移向量有6个独立的参数，一共有22个参数需要待定。每个控制点可提供4个二次约束方程，一共有20个二次约束方程。方程的个数比待定参数的个数少两个，如果运动前、后的场景图像中不能提供其它信息的话，该问题总有无穷多解，是不可解的。那么，图像中提供怎么样的信息就可以变为可解的问题呢？通过研究我们发现，如果两幅图像中包含有特征点或特征线（当然它们的空间几何信息是未知的）等信息，该问题就成为一个可解的问题。更准确地说，如果运动前、后两幅图像间的基本矩阵是已知的，则该问题是可解的，这是因为基本矩阵可提供8个约束方程，一共可得到28个非线性约束方程。直接求解由这28个（22个未知数）非线性方程所构成的方程组是极其困难的。本章采取一种新的求解方法，整个求解过程都是线性的。首先我们利用运动前、后两幅图像中的特征对应信息以及三维仿射变换保持体积比不变的性质，仿射重构5个控制点，然后利用5个控制点和它们的仿射重构求解三维仿射变换矩阵，最后由三维仿射变换矩阵求解摄像机运动前、后所对应的内参数以及运动前、后关于世界坐标系的方位和运动参数。在本章其它几节，我们将详细讨论这种方法。9.2问题的数学描述与求解基本思路9.2.1问题的数学描述令世界坐标系为，控制点关于世界坐标系的坐标记为x，摄像机运动前后所对应的坐标系分别为与，控制点关于这两个坐标系的坐标分别记为、。令摄像机运动前关于世界坐标系的方位为，即 （9.1）摄像机作一般刚体运动，即 （9.2）摄像机运动示意图如图9-2所示，于是，摄像机运动后关于世界坐标系的方位为，即 （9.3）记摄像机运动前、后所对应的内参数矩阵分别为：问题的数学描述：已知空间5个控制点关于世界坐标系的坐标为(i=1,2,5)，以及运动前的图像坐标为(i=1,2,5)、运动后的图像坐标为(i=1,2,5)，求解。命题：如果已知摄像机运动前、后两幅图像间的基本矩阵，当5个控制点满足下述条件（I）、（II）时，则可线性地确定。在本章中，我们总假定下述条件（I）、（II）成立：（I）在5个控制点中，任意4个点均不共面；（II）摄像机运动前、后的两光心的连线不通过任一个控制点。OwC1C2(R t)(Rc tc)xi图9-2 摄像机运动示意图9.2.2 求解基本思路对于运动前后的图像坐标、，我们有（9.4）（9.5）其中是未知的非零常数。记 （9.6.1） （9.6.2） （9.6.3） （9.7.1） （9.7.2）则式（9.4）、（9.5）可化为 （9.8） （9.9）式（9.6.3）有明显的几何意义：是三维空间中的仿射变换，即是控制点的仿射重构。下面给出命题证明的基本思路：由基本矩阵求解A；由三维仿射变换保持体积比不变的性质确定的仿射重构；由控制点和它的仿射重构确定仿射变换；由仿射变换确定。以下几节将详细实现上述各个步骤。9.3 命题的证明1.由基本矩阵F求解A假定由图像匹配点已计算出基本矩阵为 （9.10）通过方程，可以计算出第二幅图像中的极点（9.11）于是，存在非零常数使得 （9.12）其中从方程（9.12）可直接计算出（9.13）其中是未知的非零向量。2.由仿射重构确定s与将A代入（9.9）式，并注意到（9.14）于是我们有 （9.15）其中是未知的非零常数。令，于是（9.8）、（9.15）两式可分别化为（9.16） （9.17）将式（9.16）代入式（9.17），有（9.18）从式（9.18）可计算出（9.19）其中 （9.20） （9.21）由假定（II），必有否则必有即这表明控制点必在摄像机运动前、后两光心的连线上，与假定（II）矛盾。因此式（9.19）可写成：（9.22）其中 （9.23）将式（9.22）代入式（9.16），有（9.24）下面确定未知非零向量s：由（9.6.3）式知，与之间相差一个三维仿射变换，换句话说必是的仿射重构，由与相差一个公共的非零常数因子，所以与之间也相差一个三维仿射变换，即也必是仿射重构。令，分别是以，为顶点的四面体的体积。由假定（I），对1，2，3，4，5的任两个不相同的4元素组合与，必有。因为三维仿射变换保持体积比不变，所以我们有 （9.25）不难计算 （9.26）其中（9.27）（9.28）（9.29）下面计算将式（9.22）代入式（9.28），有 而 所以 （9.30）是与无关的常数，并简记为。对1，2，3，4，5的任两个不相同的4元素组合与，其中必有三个元素相同，不妨设，于是从式（9.25）可知 （9.31）其中 （9.32）是的一次函数。由于，所以方程（9.25）与下述线性方程等价： （9.33）这样，我们可以通过求解下述线性方程组来确定向量。 （9.34）将代入（9.24）式，有 （9.35）将代入（9.13）式，有A （9.36）3.由控制点和它的仿射重构确定仿射变换将式（9.35）代入式（9.6.3），有 （9.37）令并记，则有 （9.38）其中： 这样，可得到下述线性方程组： （9.39）其中 当5个控制点不共面时，必有，所以 （9.40）其中，表示矩阵C的广义逆。于是（9.41）4.由仿射变换确定由式（9.6.1）、（9.6.2）和（9.41），我们有 （9.42） （9.43）令，将（9.42）式两边转置再右乘，我们有因矩阵的最后一个元素必为1，所以对进行Cholesky分解，再将最后一个元素归一化，可唯一计算出（9.44）由式（9.7.1）和（9.36），有（9.45）用上面相同的方法，可唯一确定由式（9.42）和（9.44），有（9.46）因, 所以必有 （9.47）代入式（9.46），可得（9.48）由式（9.43）、（9.47），可知（9.49）同理，可求出（9.50） （9.51）这样，我们就从代数上构造性地证明了9.2节所给的命题。9.4问题的求解算法对于9.2节所描述的问题，根据以上证明过程，我们可以给出以下求解算法： 求基本矩阵与极点； 计算： 计算：； 求解线性方程组确定向量； ； 解线性方程组，在相差一个非零常数因子的意义下确定仿射变换，其中： 用式（6.42）（6.51）分别计算。9.5 仿真实验在仿真实验中，第一、二个视点的摄像机内参数矩阵的理论值分别为：，第一、 二个视点的摄像机坐标系关于世界坐标系运动的理论值分别为仿真的两幅图像如图9-3所示，其中图像点1、2、3、5、7所对应的空间点在世界坐标系中的坐标是已知的。应用本章所提出的算法，计算结果如下：这与相应的理论值是一致的。 200150100500-50-100-150-200-2502003004005006007008009001000xy70060050040030020010002003004005006007008009001000xy图9-3 仿真图像参 考 文 献1M.A.Fishler, R.C.Bolles, Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography, Comm. ACM, Vol.24, No.6, pp.381-395, 1981.2吴福朝、胡占义，摄像机未标定的P5P问题研究，计算机学报，vol.24，No.11，pp.1222-1226，2001.3胡占义、雷成、吴福朝，关于P4P问题的一点讨论，自动化学报，Vol.27，No.6，pp.770-776，2001.4W.J.Wolfe, D.Mathis, The perspective view of three points, IEEE-T PAMI, Vol.13, No.1, pp66-73, 1991.5M.L.Liu, K.H.Wong, Pose estimation using four corresponding points, Pattern Recognition Letters 20, pp.69-74, 1999.6C.Su, Y.Q.Xu, H.Li, Necessary and sufficient condition of positive root number of perspective-tree-point problem, Chinese Journal of Computers，Vol.21,No.12, pp1084- 1095,1998.7M.A.Penna, Determining camera parameters from the perspective projection of a quadrilateral, Pattern Recognition, Vol.24, No.6, pp.533-541, 1991. 8L.Quan, Z.D.LAN, Linear N= 4 point pose determination, ICCV98, pp.778-783, 1998.9吴福朝、胡占义，关于P5P问题的研究，软件学报，Vol.12，No.5，pp.768-775，2001. 10M.Dhome, M.Richetin, J.T.Lapreste, Determination of the attitude of 3-D objects from a single perspective view, IEEE-T PAMI, Vol.11, No.12, pp.1265-1278, 1989.11M.A.Abidi and T. Chandra, A new efficient and direct solution for pose estimation using quadrangular targets: Algorithm and evaluation, IEEE-T PAMI Vol.17, No.5, pp.534-538, 1995.12R.Horaud, B.Conio, An analytic solution for the perspective 4-point problem, CVGIP47, pp.33-44, 1989.13R.M.Haralick, Determining camera parameters from the perspective projection of a rectangle, Pattern Recognition, Vol.22, No. 3, pp.225-230, 1989.14R.M.Haralick, C.N.Lee, K.Ottenberg, Analysis and solution of the three point perspective pose estimation problem, CVPR91, pp.592- 98, 1991.15M.J.Brooks, L.Agapito, Direct methods for self-calibration for a moving stereo head, In Proc. ECCV96, Vol2, pp.415