1、89第九章 摄像机未标定的 P5P 问题所谓 PNP 问题是指通过控制点和它们的图像点求解摄像机的方位问题，也称为透视 N 点问题 (Perspective N Point)或 N 点定位问题，是计算机视觉、摄影测量学乃至数学领域的一个重要研究课题，其研究成果可直接应用于机器人定位、导航等。自从 PNP 问题首次于 1981 年提出后 1，由于其在物体定位方面的重要应用价值，引起了人们的广泛重视，已有很多相关的文章问世 2-14。在传统的研究方法中，均假定摄像机的内参数是已知的且在运动过程中是不变的。为了获取高质量的图像，目前数字摄像机都具有自动变焦的功能，也就是说机器人在运动过程中摄像机内参
2、数可能经常发生变化。因此，要求摄像机内参数是已知的而且在运动过程中保持不变，已很难满足实际应用的要求。本章针对五参数针孔摄像机模型，主要讨论摄像机在运动（运动参数未知）过程中其内参数是未知的且可以发生变化时，如何通过 5 个控制点以及它们的图像点，来求解所对应的内参数、方位以及运动参数。证明了下述结论：已知 5 个控制点在世界坐标系中的坐标，以及它们在摄像机作一般刚体运动前、后两幅图像中的图像坐标，当 5 个控制点中任意4 个点均不共面且摄像机运动前、后的两光心的连线不通过任一个控制点时，则可线性地确定摄像机关于世界坐标系的方位、运动前、后所对应的内参数以及运动参数。本章的研究成果在机器人视觉
3、中具有一定的理论意义和应用价值。9.1 PNP 问题概述9.1.1 PNP 问题的定义PNP 问题的定义是：已知 n 个控制点在世界坐标系中的坐标 与对n21x,.应的图像点在像平面中的坐标 ，求摄像机的内参数矩阵以及摄像机坐n21m,.标系关于世界坐标系的方向与位置 ，如图 6-1 所示。其中 R 为旋转矩阵，)(tR表示摄像机的方向；t 是平移向量，表示摄像机的位置。90（1）当摄像机内参数矩阵是已知的，则上述问题是经典的 PNP 问题。（2）摄像机内参数矩阵是未知的，上述问题称为摄像机未标定的 PNP 问题。 摄 像 机 坐 标 系 世 界 坐 标 系 x4 x2 Ow C (R t)
4、x1 x5 x3 图 9-1 PNP 问题示意图9.1.2 已有结论目前，对于摄像机内参数已知且在运动过程中保持不变的 P3P 问题、P4P 问题和 P5P 问题有如下结论：P3P 问题最多有 4 个解且解的上限可以达到；P4P 问题当 4 个控制点共面时，问题有唯一解 , 当 4 个控制点不共面时，问题最多可能有 4 个解且解的上限可以达到；P5P 问题至多有两个解且解的上限可以达到。对于摄像机内参数未知且在运动过程中是变化的 PNP 问题，相关的研究报导还很少 23。在许多实际问题中（如基于主动视觉系统的任务） ，摄像机内参数需要经常被调节而发生变化，因此不能假定摄像机内参数已知且在运动过
5、程中是不变的。这样，研究未标定的 PNP 问题就具有十分重要的意义。如果摄像机使用五参数模型，对于未标定的 P3P、 P4P 问题，其约束方程的个数均少于待求未知参数的个数，所以未标定的 P3P、P4P 问题总是有无穷多解。对于摄像机为四参数模型的未标定 P5P 问题，文献 2给出了下述结果：当 5个控制点中任意 4 个不共面，或者存在 4 个共面的控制点但任意三个图像点不共线时，未标定的 P5P 问题的解仅有两种可能：（ 1）至多有 4 个解；（2）有无穷91多解。无穷多解的情形是可以发生的，但极其少见。该文同时给出了至多有 4 个解和有无穷多解的代数条件，以及求解未标定 P5P 问题的具体
6、算法。9.1.3 本章所研究的问题不论是经典的 PNP 问题还是未标定的 PNP 问题，大都是使用单幅图像。由于机器人在运动过程中通常可以获得控制点的多幅图像，我们可以通过两幅图像来求解 PNP 问题吗？这正是本章所要研究的问题。确切地说本章讨论下述问题：假定摄像机作一般刚体运动，运动参数未知，且在运动过程中内参数可以发生变化。已知 5 个控制点在世界坐标系中的坐标，以及它们在运动前、后两幅图像中的图像坐标，求摄像机运动前、后关于世界坐标系的方位，运动前、后所对应的内参数以及运动参数。本章假定摄像机模型为针孔五参数模型，即摄像机内参数矩阵为： 10vfusuK其中 分别称为图像平面 u、v 轴
7、的尺度因子， 为主点坐标，s 表vuf, Tvu),(0示摄像机的畸变因子。我们先分析一下问题求解的可能性。两个内参数矩阵有 10 个独立的参数、两个旋转矩阵有 6 个独立的参数、两个平移向量有 6 个独立的参数，一共有 22 个参数需要待定。每个控制点可提供 4 个二次约束方程，一共有 20 个二次约束方程。方程的个数比待定参数的个数少两个，如果运动前、后的场景图像中不能提供其它信息的话，该问题总有无穷多解，是不可解的。那么，图像中提供怎么样的信息就可以变为可解的问题呢？通过研究我们发现，如果两幅图像中包含有特征点或特征线（当然它们的空间几何信息是未知的）等信息，该问题就成为一个可解的问题。
8、更准确地说，如果运动前、后两幅图像间的基本矩阵是已知的，则该问题是可解的，这是因为基本矩阵可提供 8 个约束方程，一共可得到 28 个非线性约束方程。直接求解由这 28 个（22 个未知数）非线性方程所构成的方程组是极其困难的。本章采取一种新的求解方法，整个求解过程都是线性的。首先我们利用运动前、后两幅图像中的特征对应信息以及三维仿射变换保持体积比不变的性质，92仿射重构 5 个控制点，然后利用 5 个控制点和它们的仿射重构求解三维仿射变换矩阵，最后由三维仿射变换矩阵求解摄像机运动前、后所对应的内参数以及运动前、后关于世界坐标系的方位和运动参数。在本章其它几节，我们将详细讨论这种方法。9.2
9、问题的数学描述与求解基本思路9.2.1 问题的数学描述令世界坐标系为 ，控制点关于世界坐标系的坐标记为 x，摄像机运动xyzOw前后所对应的坐标系分别为 与 ，控制点关于这两个坐)()(11zC)()(22zyx标系的坐标分别记为 、 。令摄像机运动前关于世界坐标系的方位为 ，)()(2 tR即（9.1）tRx(1)摄像机作一般刚体运动 ，即ctR（9.2）cct)1()2(摄像机运动示意图如图 9-2 所示，于是，摄像机运动后关于世界坐标系的方位为，即cctR（9.3）cc2tRx)(记摄像机运动前、后所对应的内参数矩阵分别为： 10vfus11u)()()()(K10vfus22u2)()
10、()()(问题的数学描述：93已知空间 5 个控制点关于世界坐标系的坐标为 (i=1,2,5)，以Tiiizyx),(及运动前的图像坐标为 (i=1,2,5)、运动后的图像坐标为Tiivu)1,()1m(i=1,2,5)，求解 。T2iivu),()2)mctRK,)2()命题：如果已知摄像机运动前、后两幅图像间的基本矩阵，当 5 个控制点满足下述条件（I ） 、 （II ）时，则可线性地确定 。ctRK,)2()1在本章中，我们总假定下述条件（I） 、 （II）成立：（I）在 5 个控制点中，任意 4 个点均不共面；（II）摄像机运动前、后的两光心的连线不通过任一个控制点。图 9-2 摄像机
11、运动示意图9.2.2 求解基本思路对于运动前后的图像坐标 、 ，T1i1ivu),()m),.(),() 521ivuT2i2i m我们有（9.4）)1(itKRx)1()(iOwC1 C2(R t)(Rc tc)1(imxi (2)im94（9.5）)2(im)()2)( cictRKx其中 是未知的非零常数。,1;5,.2()jij记（9.6.1）RKQ)1()(（9.6.2）tq)()(（9.6.3）1i1ai xx)()()(（9.7.1）)()2(KRAc（9.7.2）tq)()(则式（9.4） 、 （9.5）可化为（9.8）)1(im)(aix（9.9）)2(i )()(2qAi式
12、（9.6.3）有明显的几何意义： 是三维空间中的仿射变换，即 是控)1()(Q)(ajx制点 的仿射重构。jx下面给出命题证明的基本思路：由基本矩阵求解 A；由三维仿射变换保持体积比不变的性质确定 的仿射重构 ；jx)(ajx由控制点 和它的仿射重构 确定仿射变换 ；jx)(ajx)1()(qQ由仿射变换 确定 。)1()(qQtRK,)2()1c以下几节将详细实现上述各个步骤。9.3 命题的证明1.由基本矩阵 F 求解 A假定由图像匹配点已计算出基本矩阵为95（9.10）32131ffffF通过方程 ，可以计算出第二幅图像中的极点0TeF（9.11）T21e,于是，存在非零常数 使得0s（9
13、.12）FAe10s其中 0e1212e从方程（9.12）可直接计算出（9.13） 321 211200 sssefefefs)(A其中 是未知的非零向量。Ts),(321s2.由仿射重构确定 s 与 )(aix将 A 代入（9.9）式，并注意到)(0s（9.14）eqk2)(于是我们有（9.15）)(2imxsAai0)(其中 是未知的非零常数。k令 ，于是（9.8） 、 （9.15）两式,)2(1)2(1)( iiii kc)(10)( kscaiix可分别化为（9.16）)1(im)(aix96（9.17）)2(iimexsAai)(将式（9.16）代入式（9.17） ，有（9.18）)
14、2(ii si)()(11i从式（9.18）可计算出（9.19）iT1iiT1i1ii babasmss)()()()(其中（9.20）2i12ii eveu)()(,（9.21） 13i1i1i23i21i2i ffufbfvffb )()()()(由假定（II） ，必有 ),.()(5i01is否则必有 aii即 2i12i eveu)()(,这表明控制点 必在摄像机运动前、后两光心的连线上，与假定（II）矛盾。因ix此式（9.19）可写成：（9.22）iTiii csms)1()1()(其中（9.23）i2i2311ii eufvffabc)()(将式（9.22）代入式（9.16） ，有
15、（9.24）)(aix)1()(iims下面确定未知非零向量 s：由（9.6.3）式知， 与 之间相差一个三维仿射变换，换句话说 必是i)(ai )(aix97的仿射重构，由 与 相差一个公共的非零常数因子，所以 与 之间也ix)(aix)(i ix)(ai相差一个三维仿射变换，即 也必是 仿射重构。)(ajjx令 ， 分ijklV,(lkji ),()()( alkajiijklVx )51lkji别是以 ， 为顶点的四面体的体积。由假定（I） ，,lkjix,)()(lajai对1，2，3，4，5的任两个不相同的 4 元素组合 与 ，必有,lkji,lkji。因为三维仿射变换保持体积比不变
16、，所以我们有0,lkjiijklV（9.25）0)()(lkjiaijklV不难计算 11d1V ll1kkjj1iiijklijlak )()()()()()()()()( et mssmss （9.26）)()()(11 sssijklijklljid其中（9.27）11lkjiijkl xxet（9.28）)()()()()( ss1laijk1kaijl1jaikl1iajklijkl dds （9.29）)()()()(et1tr1sasrt m下面计算 )(sijkl将式（9.22）代入式（9.28） ，有)(sijklsmTlaijkTkaijlTjaiklTiajkl ddd
17、)1()1()1()1( laijkaijljaikliajkl cc)()()()(而98TlaijkTkaijlTjaiklTiajkl ddd )1()1()1()1( mm)()()()()( )()()( aijkijlaikljl 1l11j1iajkl ijkkijlikl ddvvuu011vvuuTlkji 11lkji 11lji lji lj )()()()( )()()()( )()()()( m所以 )(sijkllaijkaijljaiklijkl cdcd)()()()( （9.30）lkji11)()()()(etm是与 无关的常数，并简记为 。Ts),(321
18、s ijkl对1，2，3，4，5的任两个不相同的 4 元素组合 与 ，,lkji,lkji其中必有三个元素相同，不妨设 ，于是从式（9.25）可知lkji,lkjiaijklV)()( )()()()()1(1(11 ssss jklijliijklijllkj dd（9.31）)()()()( ;111 jkliiilkj p其中（9.32）)(;sjklip ijklijklT1ijkli1iijkl cddsm)()(是 的一次函数。Ts),(321s由于 ，所以方程（9.25）与下述线性方程等0)()(11)()( ssiilkj 99价：（9.33）0)(;sjklip这样，我们可以
19、通过求解下述线性方程组来确定向量 。*s（9.34）0)()(1234;51234;5ssp将 代入（9.24）式，有*s（9.35）)(aix)1(*)1(iicms将 代入（9.13）式，有*sA （9.36）)(*s03.由控制点 和它的仿射重构 确定仿射变换ix)(aix)1()(qQ将式（9.35）代入式（9.6.3） ，有（9.37）1i1qQ)()( )(1)(*)1( aiiicscxm令 )()()()( )()()()()()( 1210987654321qqc并记 ，则有Tq),.(1(2)(1q（9.38）)(aiixC)5,.1(i其中： 1zyx0001zyx1zy
20、x iiiiiii这样，可得到下述线性方程组：100（9.39）xqC其中T5T21.aaa)()()( .xx当 5 个控制点不共面时，必有 ，所以rnkC（9.40）q*其中， 表示矩阵 C 的广义逆。于是（9.41） *)()(*)()( )()()()(*)()()()( 121098765432111 qqccQq4.由仿射变换 确定)1()(qctRK,)2()1由式（9.6.1） 、 （9.6.2）和（9.41） ，我们有（9.42）)*1()1(Qc（9.43）)()(qtK令 ，将（9.42）式两边转置再右乘，我们有T)*1()(QWW2)1(cT因矩阵 的最后一个元素必为
21、1，所以对 进行 Cholesky 分T)1(KT)*1()(Q解，再将最后一个元素归一化，可唯一计算出（9.44）)*1()(K由式（9.7.1）和（9.36） ，有（9.45）)*1(0)2(ARsc用上面相同的方法，可唯一确定 )*2()(K由式（9.42）和（9.44） ，有101（9.46）*)()(11cQKR因 , 所以必有1)det(R（9.47）311c*)()(det代入式（9.46） ，可得（9.48）R311*)()(tQK*)()(1由式（9.43） 、 （9.47） ，可知（9.49）311*)()(det *)()(1q同理，可求出（9.50）*)(*)(*)(*
22、)( t 1123112c ssKAKAR（9.51）3112c *)(*)(det )(*)(21e这样，我们就从代数上构造性地证明了 9.2 节所给的命题。9.4 问题的求解算法对于 9.2 节所描述的问题，根据以上证明过程，我们可以给出以下求解算法： 求基本矩阵 与极点 ；32131ffffFT21e, 计算 ：)(aix 计算 ：)(,;sjkliijklijlpdc；)2()(3112eufvffciii ；11dlkjiijkl xxt102；ijkllkji 11cc)()()()(detm；)(;sjklip ijklijklT1ijkli1iijkl cddsm)()( 求解
23、线性方程组 确定向量 ；0)()(1234;51234;5ssp*s ；)(aixiT1ic*)(sm 解线性方程组 ，在相差一个非零常数因子的意义下确定仿射变换xqC，其中：)1()(qQT5T21C.a5a2a1)()()( .xx 1zyx0001zyzy iiiiiiiC 用式（6.42）（6.51）分别计算 。ctRK,)2()19.5 仿真实验在仿真实验中，第一、二个视点的摄像机内参数矩阵的理论值分别为：，108452K1)( 10584K2)(103第一、二个视点的摄像机坐标系关于世界坐标系运动的理论值分别为 103R1cosini)(T5(1)t106R2cosini)(T1(
24、2)t仿真的两幅图像如图 9-3 所示，其中图像点 1、2、3、5、7 所对应的空间点在世界坐标系中的坐标是已知的。应用本章所提出的算法，计算结果如下： 1028450K1 .)( 10352814K2 .)( 11082531073 0498686265R 11 .)( T194.)(t104 110532104602948655368R77 72 .)(T2 .)(t这与相应的理论值是一致的。图 9-3 仿真图像参 考 文 献1M.A.Fishler, R.C.Bolles, Random sample consensus: A paradigm for model fitting wit
25、h 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.200150100500-50-100-150-200-250200 300 400 500 600 700 800 900 1000xy70060
26、05004003002001000200 300 400 500 600 700 800 900 1000xy1054W.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, Nece
27、ssary 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
28、.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
29、.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, Det
30、ermining 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
31、 methods for self-calibration for a moving stereo head, In Proc. ECCV96, Vol2, pp.415-426, 1996. 16P.Sturm. Self-calibration of a moving zoom-lens camera by pre-calibration, Image and Vision Computing, 15, pp.583-589,1997.10617R.Enciso,T.Vieville. Self-calibration from four views with possibly varyi
32、ng intrinsic parameters, Image and Vision Computing, 15,pp.293-305,1997.18R.Hartley, Estimation of relative camera positions for uncalibrated cameras, ECCV92, pp.579-387, 1992.19R.Hartley, A.Zisserman, Multiple view geometry in computer vision, Cambridge University Press 2000. 20R.M.Haralick, L.G.Shapiro, Computer and Robot Vision. Addison-Wesley, 1993.21D.R.Zhang, O.Faugeras, Robust recovery of the epipolar geometry for an uncalibra- ted stereo rig. In Proc. ECCV94, pp.565-576, 1994.
