CN100338632C - 标记放置信息估计方法和信息处理设备 - Google Patents

Abstract

一种标记放置信息估计方法，包括限制条件设置步骤，设置可以用与标记的放置信息相关的几何条件定义的限制条件；捕捉步骤，捕捉包括标记图像的拍摄的图像；标记检测步骤，从所述拍摄的图像中检测标记；估计步骤，使用在图像上检测到的标记的位置和限制条件对标记的放置信息进行估计。这样，即使存在关于标记放置的限制条件，放置信息也可以被精确地获得以便满足该限制条件。

Description

标记放置信息估计方法和信息处理设备

技术领域

本发明涉及一种估计3-D空间内的标记放置信息的方法和设备。

背景技术

近年来进行了许多关于MR(混合现实)技术的研究。MR技术用于将现实空间和由计算机创建的虚拟空间进行无缝的合成。MR技术中将虚拟空间叠加到现实空间的AR(扩展现实；也称为增强现实)技术特别引人注意。

利用AR技术显示图像的图像显示设备通过显示通过将有关虚拟空间的计算机生成的图像叠加在现实空间的图像上获得的合成图像的视频透视方法得以实现，所述虚拟空间(用计算机制图绘制的虚拟物体、特征信息或类似信息)根据后面将进行说明的摄像装置的位置和方位生成，所述现实空间的图像用例如摄像机的摄像装置或者通过显示虚拟空间的图像的光学透视方法拍摄而成，该虚拟空间根据佩戴在观察者头上的透光式显示器的观察者的观察位置和方位生成。

利用AR技术显示图像的图像显示设备通过一显示通过将一关于虚拟空间(用计算机制图绘制的虚拟物体、特征信息或类似信息)的根据后面所述的摄像装置的位置和方位生成的一计算机生成图像叠加到一用例如摄影机的摄像装置拍摄而成的现实空间的图像上而获得的合成图像的视频透视方法，或是通过一将根据观察者的观察位置和方位而生成的虚拟空间的图像显示在佩戴在观察者的头部的透光式显示器上的光学透视方法，而得以实现。

期望能够将AR技术应用到各种领域中，例如将患者身体内的状态叠加到身体表面的外科辅助工具、将虚拟的建筑叠加到空地图像上的建筑仿真、叠加机器或其它设备的装配过程和配线的装配辅助工具或类拟辅助工具。

在AR技术中要解决的最重要的问题是怎样准确地完成现实空间和虚拟空间之间的对齐，迄今为止已经尝试了许多方法。AR技术中的对齐问题相当于在视频透视方法的情况下获得摄像装置在场景中(也就是说，在参照坐标系统中)的位置和方位的问题。类似地，在光学透视方法的情况下，对齐问题相当于获取观察者或显示设备在场景中的位置和方位的问题。

为解决前一问题所普遍采用的方法是，根据摄像装置拍摄而成的图像中的标记的投影位置和标记在参照坐标系统中的位置之间的对应关系，在场景中布置人工标记或创建自然特征点标记，以便获得摄像装置在参照坐标系统中的位置和方位。同样地，解决后一问题所普遍采用的方法是，将摄像装置放置在一个待测目标上(例如，观察者的头上或显示器上)，使用与前面方法同样的方式获取摄像装置的位置和方位，并且以此为基础获得待测目标的位置和方位。

接下来参照图1说明一个常规的位置-方位测量设备的例子，该设备通过将摄像装置拍摄而成的图像中检测到的标记的2-D坐标与参照坐标系统中的该标记的3-D位置相关联，对后面将会加以描述的摄像装置的位置和方位进行测量。如图1中所示，本常规例子中的位置-方位测量设备100包括标记检测单元110和位置-方位计算单元120，并连接至摄像装置130。

在参照坐标系统中的位置是已知的K个标记Q_k(k＝1，2到K)被作为标记放置到现实空间中以便获取摄像装置130的位置和方位。图1中的例子表示放置四个标记Q₁、Q₂、Q₃和Q₄的情况。其中，三个标记Q₁、Q₃和Q₄位于摄像装置130的观察范围之内，标记Q₂位于摄像装置130的观察范围之外。

标记Q_k可以为任何形状，如具有与其它标记不同颜色的圆形标记或者类似形状，只要拍摄的图像内的标记的投影位置可以被检测到并且该标记能够被识别。例如，可以采用3-D空间内的自然特征点，使用模板匹配的方法可以在所拍摄的图像中检测出这样的点。从摄像装置130输出的图像被输入到位置-方位测量设备100。标记检测单元110输入由摄像装置130输入的图像，并且检测该图像上拍摄的标记Q_k的图像坐标。例如，如果每个标记Q_k由分别具有不同颜色的圆形标记组成，标记检测单元110从输入的图像上检测对应于每个标记颜色的区域，并且将重心位置作为标记的检测坐标。

另外，标记检测单元110将每个检测到的标记Q_kn的图像坐标u^Mkn和标识符k_n输出到位置-方位计算单元120。这里，n(＝1，2到N)是表示所检测标记的序号的符号，N表示所检测标记的总数。例如，在图1的情况下，N＝3，所以标识符k₁＝1，k₂＝3，k₃＝4，输出相应的图像坐标u^Mk1、u^Mk2和u^Mk3。

位置-方位计算单元120根据作为预先已知信息的每个检测的标记Q_kn的图像坐标u^Mkn和标记Q_kn在参照坐标系统中的位置之间的关系计算摄像装置130的位置和方位。根据标记的一对3-D坐标和图像坐标计算摄像装置的位置和方位的旧的方法一直被建议用于摄影测量领域(例如，参见R.M.Haralick，C.Lee，K.Ottenberg，and M.Nolle：″Review and analysis of solutions of the three point perspective poseestimation problem″，Int′l.J.Computer Vision，vol.13，no.3，pp.331-356，1994和M.A.Fischler and R.C.Bolles：″Random sample consensus：aparadigm for model fitting with applications to image analysis andautomated cartography″，Comm.ACM，vol.24，no.6，pp.381-395，1981)。位置-方位计算单元120使用例如在R.M.Haralick，C.Lee，K.Ottenberg，和M.Nolle：″Review and analysis of solutions of the threepoint perspective pose estimation problem″，Int′l.J.Computer Vision，vol.13，no.3，pp.331-356，1994中描述的方法计算摄像装置130的位置和方位。

要注意的是，已经说明了关于使用作为3-D空间内的多点的标记(以下称为“点标记”)的情况，但是使用具有已知大小(尺寸)的正方形标记计算摄像装置的位置和方位的计算方法在例如J.Rekimoto：″Configuration method of augmented reality using 2-D matrixcode″，Interactive System and Software IV，Kindai Kagakusha，1996以及Kato，M.Billinghurst，Asano and和chibana：″Augmented realitybased on marker tracing and calibration thereof″，Japan Virtual RealityAcademic Journal，vol.4，no.4，pp.607-616，1999中被提出。同时使用正方形标记和点标记计算摄像装置的位置和方位(定位)的计算方法已经被提出，如在H.Kato，M.Billinghurst，I.Poupyrev，K.Imamoto andK.Tachibana：″Virtual object manipulation on a table-top ARenvironment″，Proc.ISAR2000，pp.111-119，2000中所公开的。使用这种计算方法，点标记具有甚至可以设置在狭窄的空间内的优点，正方形标记具有识别容易的优点，因为一个标记包括足够的信息，所以摄像装置的位置和方位可以仅通过一种标记获得，于是，就以一种补充的方式使用这两种标记。

根据上述方法，根据摄像装置拍摄的图像自过去就可以获得摄像装置的位置和方位。

另一方面，作了一种安排，即将6自由度的位置-方位传感器如磁传感器、超声传感器或类似产品附在作为测量的目标的摄像装置上，并且通过同时使用如前所述的图像处理标记检测对摄像装置的位置和方位进行测量，如在日本专利公开号No.11-084307、日本专利公开No.2000-041173和A.State，G.Hirota，D.T.Chen，W.F.Garrett和M.A.Livingston：″Superior augmented reality registration by integratinglandmark tracking and magnetic tracking″，Proc.SIGGRAPH′96，pp.429-438，1996所公开的。传感器输出的准确度依根据测量范围而发生变化，但是可以被鲁棒地获得，所以同时使用传感器和图像处理与仅使用图像处理方法相比可以改善鲁棒度。

对于使用标记的对齐方法，为了获得作为测量目标的摄像装置在参照坐标系统中的位置和方位，需要知道在点标记情况下在参照坐标系统中的位置和在正方形标记情况下在参照坐标系统中的位置和方位。在一个正方形标记的情况下，正方形标记自身经常被作为坐标系统的参照而不用单独地提供参照坐标系统，但是在采用多个正方形标记的情况下，需要得知相互之间的位置和方位关系，相应地，在需要采用参照坐标系统这一点上没有区别。

标记的位置和方位可以使用测量卷尺、标尺或量角器由手工进行测量，或者使用测量仪器进行测量，但是使用图像的测量技术可以用来提高准确度并且节约时间。点标记的位置可以通过称为束调整法进行测量。束调整法是这样的一种方法，其中大量的点标记被摄像装置拍摄，通过重复计算获取拍摄每个图像的摄像装置的位置和方位以及点标记的位置，从而使得在图像上实际观察到的标记的投影位置和从摄像装置的位置和方位以及标记的位置计算出来的投影位置之间的误差最小。

还有一种测量置于3-D空间中的多个正方形标记的位置和方位的方法在G.Baratoff，A.Neubeck和H.Regenbrecht：″Interactivemulti-marker calibration for augmented reality applications″，Proc.ISMAR2002，pp.107-116，2002中被披露。在G.Baratoff，A.Neubeck和H.Regenbrecht：″Interactive multi-marker calibration for augmentedreality applications″，Proc.ISMAR2002，pp.107-116，2002中，拍摄每个图像的摄像装置的位置和方位以及每个正方形标记的位置和方位通过使用以下方法进行测量，即大量的置于3-D空间内的多个正方形标记的图像被拍摄，并且重复计算从而使得投影误差最小。

使用上述用于测量标记的位置和方位的常规方法，在有一个关于标记位置的约束条件即多个标记位于同一个平面的情况下，测量结果在一些情况下便不能满足约束条件。这是因为上述测量方法是一种使标记在拍摄图像上的投影误差最小化的方法，相应地，如果用于标记投影位置计算的参数(照相摄像机的焦距、主点位置和透镜畸变校正参数)和图像上标记的观测位置包括一个误差余地，那么测量值也包括该误差余地。因此，在获得的测量结果中，本应在同一个平面上的多个标记不在同一个平面上。

发明内容

本发明的目的在于解决上述问题，并且提供一种获取标记的位置和方位的方法以便在存在关于标记放置的限制条件时满足该限制条件。

为达到这个目的，根据本发明的第一个方面，一种用于估计在几何限制下的空间内标记的空间放置信息的标记放置信息估计方法，包括：一标记检测步骤，用以从一包括标记的拍摄图像检测标记；一限制条件设置步骤，用以设置标记的一限制条件；标记投影位置计算步骤，用以根据拍摄图像的摄像单元的位置和方位的近似值和标记的放置信息的近似值，计算标记投影在图像表面上的投影位置；一校正值计算步骤，获取用于校正标记的放置信息的校正值，从而降低在标记检测步骤中检测到的图像表面的标记的位置和在标记投影位置计算步骤中获得的图像表面的标记的投影位置之间的总误差，并且满足限制条件设置步骤中设置的标记限制条件；一放置信息校正步骤，根据在校正值计算步骤中获取的校正值对标记的放置信息进行校正；以及一重复计算步骤，通过重复执行标记投影位置计算步骤、校正值计算步骤和放置信息校正步骤，并使用在放置信息校正步骤中校正后的标记放置信息而不是在标记投影位置计算步骤中计算出的投影位置对标记的放置信息进行计算。

根据本发明的第二个方面，一种用于估计来自拍摄图像的标记的空间放置信息的标记放置信息估计方法，包括：一限制条件设置步骤，用以设置可以用与标记的放置信息有关的几何条件来定义的限制条件；一捕捉步骤，捕捉包括标记图像的拍摄的图像；标记检测步骤，从拍摄的图像中检测标记；估计步骤，根据在图像上检测到的标记的位置和限制条件估计标记的放置信息。

从下面结合附图对典型实施例的描述，本发明的其它的特征和优点将会变得清楚，在整个附图中用类似的标记表示相同或相似的部分。

附图说明

图1为表示常规的位置-方位测量设备的示意图。

图2为表示根据第一实施例的一标记位置-方位估计设备的功能结构的方框图。

图3为表示根据第一实施例的标记位置-方位估计设备的硬件结构的方框图。

图4A为表示第一实施例中采用的点标记的示意图。

图4B为表示第一实施例子中采用的平面标记的示意图。

图5为表示用于在同一个平面上布置多个标记的GUI(图形用户界面)的示意图。

图6为表示照相摄像机坐标系统和图像坐标系统的示意图。

图7为表示一共线条件方程的示意图。

图8为表示平面定义方法的示意图。

图9为表示根据第一实施例获取点标记的位置和正方形标记的位置和方位的过程的流程图。

图10为表示存在两个限制标记的平面的情况下的示意图。

图11为表示可以适用第三实施例的标记放置的示意图。

图12为表示可以适用第四实施例的标记放置的示意图。

图13为表示被限制在圆柱体的侧面上的标记的示意图。

图14为表示被限制在球面上的标记的示意图。

图15A至图15C为表示根据第四实施例的GUI的示意图。

发明内容

下面参照附图说明本发明的实施例。

第一实施例

图2为表示根据本实施例的标记位置-方位估计设备2的功能结构的方框图。一摄像单元210为照相摄像机，拍摄布置有标记的场景。一图像捕捉单元220将摄像单元210拍摄的图像输入到计算机中。一标记检测-识别单元230在图像捕捉单元220输入到计算机中的图像中检测标记，并对每个检测到的标记进行识别。一标记放置条件设置单元240设置关于标记的位置的限制条件。一标记位置-方位计算单元250计算每个标记的位置和方位，从而满足标记放置条件设置单元240根据标记检测-识别单元230的标记检测结果所设置的有关标记的位置的限制条件。

图3为表示根据本实施例的标记位置-方位估计设备2的硬件结构的方框图。图3中所示的硬件结构和普通个人电脑的结构相同，但是还连接了图像捕捉设备310和照相摄像机311。图像捕捉设备310用于将照相摄像机311拍摄的图像输入到计算机中，并对应图像捕捉单元220。图像捕捉设备310的一个例子是视频捕捉板，但是并不仅限于此，只要可以输入照相摄像机拍摄的图像即可。CPU301通过执行存储在未显示的光媒体、外部存储设备或类似上的程序，用作标记检测-识别单元230、标记放置条件设置单元230和标记位置-方位计算单元250。

图3表示的标记位置-方位估计设备2的硬件结构也可以包括其它常规的计算机部件，例如和一头戴式显示器(HMD)相通讯的一图形加速器302，、一存储设备，如一盘305、一只读存储器(ROM)306、一随机存取存储器(RAM)307，和一个或多个输入设备，如一键盘309和一鼠标308。

接下来，根据具有上述结构的本实施例，说明标记位置-方位估计设备的操作概况。

首先，说明本实施例所使用的标记。图4A和图4B表示本实施例所使用的标记的示意图。图4A表示点标记，用3-D空间中的点的位置来表示其放置信息。假定点标记为具有单一颜色的圆形从而可以在图像中被检测到，每个点标记的重心表示其在3-D空间中的位置。图4B表示平面标记，其放置信息通过3-D空间中的平面的位置和方位表示。本实施例采用正方形标记作为平面标记，但是平面标记并不仅限于正方形标记，所以只要具有平面形状的任何形状的标记都可以使用。假设正方形的中心，也就是，两个对角线的交点，表示3-D空间中正方形标记的位置。如图4B所示，假设正方形标记的法线方向取作z轴，正方形标记的坐标系统被指定从而x轴和y轴可以和两侧相平行，相对于参照坐标系统的正方形标记的坐标系统的方向视为正方形标记的方向。根据本实施例的正方形标记周围是黑框，从而正方形标记可以很容易地在图像上被检测，所以黑框内的正方形被用作正方形标记。

(1.标记的拍摄)

用户使用摄像单元210拍摄布置有标记的多个图像。通过图像捕捉单元220将拍摄的图像输入到计算机中。

(2.标记的检测和识别)

标记检测和识别单元230从输入到计算机的拍摄的图像中检测标记并对之进行识别。这里，术语“标记的检测”是指在2-D图像上获取标记的图像坐标。在点标记的情况下，获取在图像上标记区域的重心的图像坐标，在正方形标记的情况下，获取每个顶点的图像坐标。标记的图像坐标可以自动获取，或者可以手工指定，例如，通过用户使用鼠标在图像上点击。在自动获取点标记的位置的情况下，例如，根据像素是否属于对应于该颜色的YUV颜色空间图像的特定区域提取出具有相同颜色的像素作为标记，通过将相连的像素标注为同样的颜色得到标记区域，并且计算图像内的标记区域的重心。同样，在自动获得正方形标记的每个顶点的位置的情况下，例如，将输入的图像转换为二值图像，检测正方形标记的黑框区域，得到内接于黑框区域的四方形。

术语“标记的识别”是指将图像内要检测的标记识别为唯一的标记。标记的识别可以自动完成，或者由用户手工完成。在自动识别点标记的情况下，例如，采用了具有不同颜色的多个标记，并且根据检测标记时标记区域的标注颜色完成识别。同样，在自动识别正方形标记的情况下，例如，通过给每个标记唯一的模式完成识别。

(3.标记放置条件的设置)

用户使用标记放置条件设置单元240设置有关标记放置的限制条件。在本实施例中，多个标记存在于同一平面上的限制条件作为与标记位置相关的限制条件被给出。假定这里使用的平面是未知平面，并且存在多个未知平面。图5表示用于设置在同个平面上的多个标记的GUI(图形用户界面)。拍摄的图像切换部分在GUI上，由摄像单元拍摄的图像显示在该部分。拍摄的图像的显示可以使用拍摄的图像切换部分上的按扭进行切换。当用户用鼠标点击Previous(前)按扭时，显示前幅拍摄的图像。当用户点击Next(后)按扭时，显示后一幅拍摄的图像。用户在观看拍摄的图像时在同个平面上设定标记。在拍摄的图像上，每个唯一的标记名称显示在每个识别的标记的旁边。图5表示拍摄了两个点标记(图中的POINT1(点1)和POINT2(点2))和两个正方形标记(图中的SQUARE1(正方形1)和SQUARE2(正方形2))的拍摄的图像显示在拍摄的图像的显示部分。假定这些标记中，点标记POINT1和正方形标记SQUARE1位于同一个平面上。用户通过用鼠标在任一个标记旁点击在拍摄的图像内选择出POINT1或SQUARE1。被选择的标记的名称变为粗体显示。然后，用户通过在压下shift键的同时用鼠标的左键点击同个平面上的附近的标记作为选择的标记从而从位于同一个平面上的标记中构成一个组。在点击属于同一平面上的组的标记的情况下，所有属于该组的标记的名称显示变为粗体。在这种状态中，用户通过在压下shift键的同时用鼠标左键点击另一个标记可以将该标记加入到该组中。同样，当用户在压下shift键的同时用鼠标右键在属于一个组的标记旁边点击时，从该组中释放该标记，当用户在不压下shift键时用右鼠标键点击同个标记时，释放整个组。

(4.标记的位置和方位的计算)

根据标记的检测和识别结果和关于标记放置的限制条件计算在参照坐标系统中标记的位置和方位。首先，说明有关的透视投影变换。图6为表示照相摄像机坐标系统和图像坐标系统的示意图。假定观察轴和图像表面之间的交点作为图像坐标系统的原点O_i，图像的水平方向作为x_i轴，图像的垂直方向作为y_i轴。同样，假定照相摄像机坐标系统的原点O_c和图像表面的焦距为f，照相摄像机坐标系统的z_c轴在观察轴的相反方向上，x_c轴在与图像的水平方向平行的方向上，y_c轴在与图像的垂直方向平行的方向上。

根据透视投影变换，照相摄像机坐标系统上的点x_c＝[x_c y_c z_c]^t被投影到图像坐标为表达式(2)中的u＝[u_x u_y]^t的点上。

$u_x=-f\frac{x_c}{z_c}$

$u_y=-f\frac{y_c}{z_c}---\left(2\right)$

在本实施例中，假定不存在透镜畸变或者已经被校正，并且照相摄像机为针孔相机。如图7所示，表达式1表示空间中的一个点，该点在图像上投影的点和照相摄像机位置(观察点)在同一条直线上，也称为共线条件。

在参照坐标系统中，照相摄像机位置为t＝[t_x t_y t_z]^t，照相摄像机方位(实际上是参照坐标系统相对于照相摄像机坐标系统的方位)为ω＝[ω_x ω_y ω_z]。要注意的是，ω是3自由度方位表达式，并且方位用旋转轴向量和旋转角表示。如果旋转角为r_a，用ω表示r_a如表达式(3)所示。

$r_a=\sqrt{{{\mathrm{\ω}}_x}^2+{{\mathrm{\ω}}_y}^2+{{\mathrm{\ω}}_z}^2}$

如果旋转轴向量为r_axis＝[r_x r_y r_z]^t，r_axis与ω之间的关系如表达式(4)中所示。

[ω_x ω_y ω_z]＝[r_ar_x r_ar_y r_ar_z]    ··(4)

表达式(5)表示ω(旋转角r_a和旋转轴向量r_axis)和3×3旋转变换矩阵R之间的关系。

$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]$

$=\left[\begin{array}{ccc}{r_x}^2(1-{\cos r}_a)+{\cos r}_a& r_x{r}_y(1-{\cos r}_a)-r_z{\sin r}_a& r_z{r}_x(1-\cos r_a)+r_y{\sin r}_a\\ r_x{r}_y(1-{\cos r}_a)+r_z{\sin r}_a& {r_y}^2(1-{\cos r}_a)+{\cos r}_a& r_y{r}_z(1-{\cos r}_a)-r_x\sin r_a\\ r_z{r}_x(1-{\cos r}_a)-r_y{\sin r}_a& r_y{r}_z(1-{\cos r}_a)+r_x{\sin r}_a& {r_z}^2(1-{\cos r}_a)+{\cos r}_a\end{array}\right]---\left(5\right)$

参照坐标系统上的点x_w＝[x_w y_w z_w]^t的照相摄像机坐标x_c用t和R表示，如表达式(6)所示。

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R\left[\begin{array}{c}{x}_w-t_x\\ y_w-t_y\\ z_w-t_z\end{array}\right]=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]\left[\begin{array}{c}{x}_w-t_x\\ y_w-t_y\\ z_w-t_z\end{array}\right]---\left(6\right)$

根据表达式(2)和(6)，即透视投影变换，在参照坐标系统上的点x_w＝[x_w y_w z_w]^t被投影到图像上的点u＝[u_x u_y]^t上，如表达式(7)所示。

$u_x=-f\frac{x_c}{z_c}=-f\frac{R_{11}(x_w-t_x)+R_{12}(y_w-t_y)+R_{13}(z_w-t_z)}{R_{31}(x_w-t_x)+R_{32}(y_w-t_y)+R_{33}(z_w-t_z)}$

$u_y=-f\frac{y_c}{z_c}=-f\frac{R_{21}(x_w-t_x)+R_{22}(y_w-t_y)+R_{23}(z_w-t_z)}{R_{31}(x_w-t_x)+R_{32}(y_w-t_y)+R_{33}(z_w-t_z)}---\left(7\right)$

理想地，基于t，ω，x_w从表达式(7)计算出的投影位置(计算的位置)和观察的位置(观察位置)一致。相应地，如果假定在图像的水平方向上的投影位置和观察位置之间的误差为F，在垂直方向上的误差为G，观察位置为u_o＝[u_ox u_oy]^t，则F和G为0，如表达式(8)所示。

$F=-f\frac{R_{11}(x_w-t_x)+R_{12}(y_w-t_y)+R_{13}(z_w-t_z)}{R_{31}(x_w-t_x)+R_{32}(y_w-t_y)+R_{33}(z_w-t_z)}-u_{\mathrm{ox}}=0$

$G=-f\frac{R_{21}(x_w-t_x)+R_{22}(y_w-t_y)+R_{23}(z_w-t_z)}{R_{31}(x_w-t_x)+R_{32}(y_w-t_y)+R_{33}(z_w-t_z)}-u_{\mathrm{oy}}=0---\left(8\right)$

F和G为关于照相摄像机位置t、照相摄像机方位ω和参照坐标上的观察目标点的位置x_w的函数。这里，参照坐标上的点的位置x_w依据标记的类型和是否限制平面上的标记为不同的可变函数，所以F和G也可依据标记的类型和否限制平面上的标记为不同的可变函数。下面分两种情况说明，即，限制平面上的标记的情况和不限制平面上的标记的情况。

(不限制平面上的标记的情况)

在点标记的情况下，如果假定参照坐标系统上点标记的位置为t_pn＝[t_pnx t_pny t_pnz]^t，则x_w和t_pn相等，如表达式(9)所示。

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{c}{t}_{\mathrm{pnx}}\\ t_{\mathrm{pny}}\\ t_{\mathrm{pnz}}\end{array}\right]---\left(9\right)$

相应地，如表达式(10)所示，F和G为关于照相摄像机位置t、照相摄像机方位ω和参照坐标系统上的点标记的位置t_pn的函数。

F(t_x，t_y，t_z，ω_x，ω_y，ω_z，t_pnx，t_pny，t_pnz)＝0

G(t_x，t_y，t_z，ω_x，ω_y，ω_z，t_pnx，t_pny，t_pnz)＝0    ··(10)

接下来说明关于正方形标记的情况。正方形标记用参照坐标系统中的位置t_s＝[t_sx t_sy t_sz]^t和关于参照坐标系统的方位ωs＝[ω_sx ω_sy ω_sz]表示(假定关于ω_s的3×3旋转转换矩阵为R_s)。假定在正方形标记坐标系统中正方形标记的顶点位置为x_s＝[x_s y_s O]^t。在参照坐标系统中正方形标记的顶点的位置x_w为关于t_s和ω_s(R_s)的函数，如表达式(11)所示。

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_s& t_s\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_s\\ y_s\\ 0\\ 1\end{array}\right]\left[\begin{array}{cccc}{R}_{s11}& R_{s12}& R_{s13}& t_{\mathrm{sx}}\\ R_{s21}& R_{s22}& R_{s23}& t_{\mathrm{sy}}\\ R_{s31}& R_{s32}& R_{s33}& t_{\mathrm{sz}}\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_s\\ y_s\\ 0\\ 1\end{array}\right]---\left(11\right)$

相应地，如表达式(12)所示，F和G为关于照相摄像机位置t、照相摄像机方位ω，正方形标记的位置t_p和正方形标记的方位ω_s的函数。

F(t_x，t_y，t_z，ω_x，ω_y，ω_z，t_sx，t_sy，t_sz，ω_sx，ω_sy，ω_sz)＝0

G(t_x，t_y，t_z，ω_x，ω_y，ω_z，t_sx，t_sy，t_sz，ω_sx，ω_sy，ω_sz)＝0  ··(12)

(限制平面上的标记的情况)

首先，说明关于定义平面的方法。如图8中所示，平面用极坐标表达式r＝[rθ]表示。平面坐标系统的z_p轴与平面的法线方向平行，并通过参照坐标系统的原点。平面的方位用绕y轴的旋转度θ和绕x轴的旋转度定义。r是平面和参照坐标系统的原点o_w之间的带正负号的距离。表示平面的方位的旋转矩阵R_p如表达式(13)所示。

$R_p=\left[\begin{array}{ccc}{R}_{p11}& R_{p12}& R_{p13}\\ R_{P21}& R_{p22}& R_{p23}\\ R_{p31}& R_{p32}& R_{p33}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& -\sin \mathrm{\φ}\\ 0& \sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& \sin \mathrm{\θ}\\ 0& 1& 0\\ -\sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]$

$=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& \sin \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\φ}& -\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}\cos \mathrm{\φ}& \sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]---\left(13\right)$

同样，在参照坐标系统中平面坐标系统的原点o_p的位置t_pl＝[t_plxt_ply t_plz]^t作为z_p轴和平面之间的交点被获得，如表达式(14)所示。

要注意的是，

表示平面的法线向量。

$t_{\mathrm{pl}}=\left[\begin{array}{c}{t}_{\mathrm{plx}}\\ t_{\mathrm{ply}}\\ t_{\mathrm{plz}}\end{array}\right]=r\·\stackrel{\→}{n}=r\·\left[\begin{array}{ccc}{R}_{p11}& R_{p12}& R_{p13}\\ R_{p21}& R_{p22}& R_{p23}\\ R_{p31}& R_{p32}& R_{p33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}r\sin \mathrm{\θ}\\ -r\cos \mathrm{\θ}\sin \mathrm{\φ}\\ r\cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]---\left(14\right)$

在参照坐标系统中用上述极坐标表达式r＝[rθ]表示的平面坐标系统上的点x_p＝[x_p y_p O]^t的位置x_w如表达式(15)所示。从表达式(15)中可以理解，x_w和x_p是r的函数。

$x_w=\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{ccc}{R}_{p11}& R_{p12}& t_{\mathrm{plx}}\\ R_{p21}& R_{p22}& t_{\mathrm{ply}}\\ R_{p31}& R_{P32}& t_{\mathrm{plz}}\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ 1\end{array}\right]=\left[\begin{array}{c}{x}_p\cos \mathrm{\θ}+r\sin \mathrm{\θ}\\ x_p\sin \mathrm{\θ}\sin \mathrm{\φ}+y_p\cos \mathrm{\φ}-r\cos \mathrm{\θ}\sin \mathrm{\φ}\\ -x_p\sin \mathrm{\θ}\cos \mathrm{\φ}+y_p\sin \mathrm{\φ}+r\cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]---\left(15\right)$

限制在平面上的点标记用位置t^pl _p＝[t^pl _px t^pl _py O]^t表示。如表达式(16)中所示，x_p等于t^pl _p。

$\left[\begin{array}{c}{x}_p\\ y_p\\ 0\end{array}\right]=\left[\begin{array}{c}{t^{\mathrm{pl}}}_{\mathrm{px}}\\ {t^{\mathrm{pl}}}_{\mathrm{py}}\\ 0\end{array}\right]---\left(16\right)$

相应地，x_w为t^pl _p和r的函数，F和G为照相摄像机位置t、照相摄像机方位ω、平面上的点标记的位置t^pl _p和平面的参数r的函数，如表达式(17)所示。

F(t_x，t_y，t_z，ω_x，ω_y，ω_x，t^pl _px，t^pl _py，r，θ，φ)＝0

G(t_x，t_y，t_z，ω_x，ω_y，ω_z，t^pl _px，t^pl _py，r，θ，φ)＝0··(17)

另一方面，在正方形标记的情况下，平面坐标系统上除位置t^pl _s＝[t^pl _sx t^pl _sy O]^t之外，限制在平面上的正方形标记用绕正方形标记的法线向量的旋转度θ^pl表示(即，绕平面坐标系统的z_p轴)。所以，如果假设在正方形标记坐标系统中正方形标记的顶点的位置为x_s＝[x_s y_sO]^t，其平面坐标x_p如表达式(18)所示。

$\left[\begin{array}{c}{x}_p\\ y_p\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\cos \mathrm{\θ}}^{\mathrm{pl}}& -{\sin \mathrm{\θ}}^{\mathrm{pl}}& {t^{\mathrm{pl}}}_{\mathrm{sx}}\\ {\sin \mathrm{\θ}}^{\mathrm{pl}}& {\cos \mathrm{\θ}}^{\mathrm{pl}}& {t^{\mathrm{pl}}}_{\mathrm{sx}}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_s\\ y_s\\ 1\end{array}\right]---\left(18\right)$

这样，x_w为t^pl _s、θ^pl和r的函数，F和G为照相摄像机位置t、照相摄像机方位ω、平面上的点标记的位置t^pl _s、平面上的正方形标记的方位θ^pl和平面的参数r的函数，如表达式(19)所示。

F(t_x，t_y，t_z，ω_x，ω_y，ω_z，t^pl _sx，t^pl _sy，θ^pl，r，θ，φ)＝0

G(t_x，t_y，t_z，ω_x，ω_y，ω_z，t^pl _sx，t^pl _sy，θ^pl，r，θ，φ)＝0  ··(19)

表达式(10)、(12)、(17)和(19)是关于照相摄像机位置和方位、标记的位置和方位和平面参数的非线性表达式。相应地，线性化在关于照相摄像机的位置和方位、标记的位置和方位以及使用一阶泰勒扩展的平面参数的近似值的附近完成，并且通过使用重复的计算获取照相摄像机的位置和方位、标记的位置和方位以及平面参数。

通过分别对相应的表达式(10)、(12)、(17)和(19)进行线性化获得表达式(20)、(21)、(22)和(23)。这里，Δt_x、Δt_y和Δt_z表示照相摄像机位置的近似值的校正值；Δω_x、Δω_y和Δω_z表示照相摄像机方位的近似值的校正值；Δt_px、Δt_py和Δt_pz表示没有被限制到在平面的点标记的位置的近似值的校正值；Δt_sx、Δt_sy和Δt_sz表示没有被限制到在平面的方形正方形标记的位置的近似值的校正值；Δω_sx、Δω_sy和Δω_sz表示没有被限制在到平面的方形正方形标记的方位的近似值的校正值；Δt^pl _px和Δt^pl _py表示在被限制在到平面的点标记的平面上的位置的近似值的校正值；Δt^pl _sx和Δt^pl _sy表示被限制在制到平面的方形正方形标记的平面上的位置的近似值的校正值；Δθ^pl表示用平面限制的方形正方形标记的平面上的方位的近似值的校正值；Δr、Δθ、和Δ表示关于平面参数的近似值的校正值。

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z+\frac{\∂F}{\∂t_{\mathrm{px}}}{\mathrm{\Δt}}_{\mathrm{px}}+\frac{\∂F}{\∂t_{\mathrm{py}}}\mathrm{\Δ}{t}_{\mathrm{py}}+\frac{\∂F}{\∂t_{\mathrm{pz}}}{\mathrm{\Δt}}_{\mathrm{pz}}=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z+\frac{\∂G}{\∂t_{\mathrm{px}}}{\mathrm{\Δt}}_{\mathrm{px}}+\frac{\∂G}{\∂t_{\mathrm{py}}}\mathrm{\Δ}{t}_{\mathrm{py}}+\frac{\∂G}{\∂t_{\mathrm{pz}}}{\mathrm{\Δt}}_{\mathrm{pz}}=0---\left(20\right)$

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{\∂t_{\mathrm{sx}}}{\mathrm{\Δt}}_{\mathrm{sx}}+\frac{\∂F}{\∂t_{\mathrm{sy}}}{\mathrm{\Δt}}_{\mathrm{sy}}+\frac{\∂F}{\∂t_{\mathrm{sz}}}{\mathrm{\Δt}}_{\mathrm{sz}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{sx}}}{\mathrm{\Δ\ω}}_{\mathrm{sx}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{sy}}}{\mathrm{\Δ\ω}}_{\mathrm{sy}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{sz}}}{\mathrm{\Δ\ω}}_{\mathrm{sz}}=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂G}{\∂t_{\mathrm{sx}}}{\mathrm{\Δt}}_{\mathrm{sx}}+\frac{\∂G}{\∂t_{\mathrm{sy}}}{\mathrm{\Δt}}_{\mathrm{sy}}+\frac{\∂G}{\∂t_{\mathrm{sz}}}{\mathrm{\Δt}}_{\mathrm{sz}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{sx}}}{\mathrm{\Δ\ω}}_{\mathrm{sx}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{sy}}}{\mathrm{\Δ\ω}}_{\mathrm{sy}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{sz}}}{\mathrm{\Δ\ω}}_{\mathrm{sz}}=0---\left(21\right)$

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{px}}^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{py}}^{\mathrm{pl}}+\frac{\∂F}{\∂r}\mathrm{\Δr}+\frac{\∂F}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂F}{\∂\mathrm{\φ}}\mathrm{\Δ\φ}=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂G}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{px}}^{\mathrm{pl}}+\frac{\∂G}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{py}}^{\mathrm{pl}}+\frac{\∂G}{\∂r}\mathrm{\Δ}r+\frac{\∂G}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂G}{\∂\mathrm{\φ}}\mathrm{\Δ\φ}=0---\left(22\right)$

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_x^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_y^{\mathrm{pl}}+\frac{\∂F}{\∂{\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}}+\frac{\∂F}{\∂r}\mathrm{\Δ}r+\frac{\∂F}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂F}{\∂\mathrm{\φ}}\mathrm{\Δ\φ}=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂G}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{sx}}^{\mathrm{pl}}+\frac{\∂G}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{sy}}^{\mathrm{pl}}+\frac{\∂G}{\∂{\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}}+\frac{\∂G}{\∂r}\mathrm{\Δ}r+\frac{\∂G}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂G}{\∂\mathrm{\φ}}\mathrm{\Δ\φ}=0---\left(23\right)$

这里，表达式(20)、(21)、(22)和(23)中的F⁰和G⁰为常数，表示从关于照相摄像机的位置和方位、点标记的位置或正方形标记的位置和方位以及平面参数的近似值得到的标记的观察位置u_O和投影位置(计算位置)之间的差。

表达式(20)、(21)、(22)和(23)是关于在某个图像上观察到的一个点标记或正方形标记的一个顶点的观察方程。事实上，多点标记或多正方形标记在多个图像上被观察到，所以得到多个表达式(20)、(21)、(22)和(23)。相应地，照相摄像机的位置和方位、没有被限制在平面上的点标记的位置、没有被限制在平面上的正方形标记的位置和方位、被限制到平面上的点标记的位置、被限制到平面上的正方形标记的位置和方位和平面参数通过求解作为联立方程的表达式(20)、(21)、(22)和(23)获得，该联立方程与关于照相摄像机的位置和方位、没有被限制在平面上的点标记的位置、没有被限制在平面上的正方形标记的位置和方位、被限制到平面上的点标记的位置、被限制到平面上的正方形标记的位置和方位和平面参数的近似值的校正值相关。

图9表示根据本实施例获得点标记的位置以及正方形标记的位置和方位的过程的流程图。现在，假定包含标记的场景的拍摄和从拍摄的图像中提取和识别标记已经完成。在步骤S100，设置关于载有每个图像的照相摄像机的位置和方位、点标记的位置、正方形标记的位置和方位和平面参数的近似值。在接下来的步骤中，得到用于对步骤S100中设置的近似值进行校正的校正值。在步骤S110，构成用于同时获取每个近似值的校正值的联立方程，以便根据照相摄像机的位置和方位、点标记的位置、正方形标记的位置和方位和平面参数的近似值使投影误差最小化。在步骤S120，从步骤S110中构成的联立方程中获得每个近似值的校正值。在步骤S130，通过用在步骤S120中获得的对应的校正值校正每个近似值从而获得新的近似值。在步骤S140，判断照相摄像机的位置和方位、点标记的位置、正方形标记的位置和方位和平面参数是否被收敛为步骤S130中的最优化值。如果是，处理结束。如果不是，处理返回步骤S110并且重复执行S110至S140。下面对每个步骤进行更详细的说明。

在步骤S100，输入关于载有图像的照相摄像机的位置和方位、点标记的位置、正方形标记的位置和方位和平面参数的近似值。这里，假定载有的图像的数量为N，载有每个图像的照相摄像机的位置为t_i＝[t_ix t_iy t_iz]^t(i＝1到N)，其方位为ω_i＝[ω_ixω_iyω_iz](i＝1到N)。对于需要获得其位置的点标记，假定没有被限制到平面的点标记的数量为K_p1，各个点标记的位置为t_pi＝[t_pix t_piy t_piz]^t(i＝1到K_p1)，则t^pl _pi＝[t^pl _pixt^pl _piy]t(i＝1到K_p2)。对于需要获得其位置和方位的正方形标记，假定没有被限制到平面的正方形标记的数量为K_s1，被限制到平面的正方形标记的数量为K_s2，各个正方形标记的位置为t_si＝[t_six t_siy t_siz]^t(i＝1到K_s1)，则t^pl _si＝[t^pl _six t^pl _siy]t(i＝1到K_s2)，其方位为ω_si＝[ω_sixω_siyω_siz](i＝1到K_s1)，θ^pl _i(i＝1到K_s2)。

每个照相摄像机的位置和方位的近似值可以从6自由度位置和方位传感器如佩带在照相摄像机上的电磁传感器的输出值获得，或者可以从参照坐标系统中已知位置的点和图像上的点的投影位置之间的对应中获得。在使用其位置在参照坐标系统中为已知的点的情况下，已知位置的点和将获得的位置为未知的标记在场景中混合在一起。另外，每个照相摄像机的位置和方位的近似值可以根据下面将描述的标记的位置和方位的近似值获得。

另外，每个照相摄像机的位置和方位的近似值可以通过机械的方式获得，例如，使用运动控制照相摄像机，或者使用预先校准过位置和方位的照相摄像机完成拍摄从而使用校准结果。

每个标记的位置和方位的近似值可以是使用测量带、尺或量角器，或者使用测量仪器手工测量出的粗略值，或者可以是使用一次根据本实施例的方式或类似方法估计出来的值。每个被限制到平面的标记的位置和方位的近似值可以是平面坐标系统中的标记的位置和方位的值。

如果在同个平面上的标记都是点标记，则平面参数的近似值可以通过从每个标记的位置的近似值中获得回归平面而获得。另一方面，如果在同个平面上的标记包括正方形标记，平面参数的近似值可以通过假定每个正方形标记为平面从每个正方形标记中获得。另外，用户可以直接指定平面参数而不用从每个标记的位置和方位的近似值中获得平面参数。

接下来，在步骤S110，构成表达式(20)、(21)、(22)和(23)的观察方程，所述观察方程的数量对应于在图像上观察到的标记的数量。表达式(20)、(21)、(22)和(23)是关于一个照相摄像机的位置和方位的校正值、一个点标记的位置的校正值或者一个正方形标记的位置和方位的校正值和一个平面参数的校正值的观察方程。这里，如表达式(24)所示，构成了关于N个照相摄像机的位置和方位、没有被限制到平面的K_p1点标记的位置、被限制到平面的K_p2点标记的位置、没有被限制到平面的K_s1正方形标记的位置、被限制到平面的K_s2正方形标记的位置和P平面参数的观察方程。在这种情况下，未知的校正值的数量为(6×N+3×K_p1+2×K_p2+6×K_s1+3×K_s2+3×P)。

$F^0+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(\frac{\∂F}{\∂t_{\mathrm{ix}}}{\mathrm{\Δt}}_{\mathrm{ix}}+\frac{\∂F}{\∂t_{\mathrm{iy}}}{\mathrm{\Δt}}_{\mathrm{iy}}+\frac{\∂F}{\∂t_{\mathrm{iz}}}{\mathrm{\Δt}}_{\mathrm{iz}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{ix}}}{\mathrm{\Δ\ω}}_{\mathrm{ix}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{iy}}}{\mathrm{\Δ\ω}}_{\mathrm{iy}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{iz}}}{\mathrm{\Δ\ω}}_{\mathrm{iz}})$

$+\underset{i=1}{\overset{K_{P1}}{\mathrm{\Σ}}}(\frac{\∂F}{{\∂t}_{\mathrm{pix}}}{\mathrm{\Δt}}_{\mathrm{pix}}+\frac{\∂F}{{\∂t}_{\mathrm{piy}}}{\mathrm{\Δt}}_{\mathrm{piy}}+\frac{\∂F}{{\∂t}_{\mathrm{piz}}}{\mathrm{\Δt}}_{\mathrm{piz}})$

$+\underset{i=1}{\overset{K_{s1}}{\mathrm{\Σ}}}(\frac{\∂F}{\∂t_{\mathrm{six}}}{\mathrm{\Δt}}_{\mathrm{six}}+\frac{\∂F}{\∂t_{\mathrm{siy}}}{\mathrm{\Δt}}_{\mathrm{siy}}+\frac{\∂F}{\∂t_{\mathrm{siz}}}{\mathrm{\Δt}}_{\mathrm{siz}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{six}}}{\mathrm{\Δ\ω}}_{\mathrm{six}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{siy}}}{\mathrm{\Δ\ω}}_{\mathrm{siy}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{siz}}}{\mathrm{\Δ\ω}}_{\mathrm{siz}})$

$+\underset{i=1}{\overset{K_{P2}}{\mathrm{\Σ}}}(\frac{\∂F}{\∂t_{\mathrm{pix}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{pix}}^{\mathrm{pl}}+\frac{\∂F}{\∂t_{\mathrm{piy}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{piy}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{K_{s2}}{\mathrm{\Σ}}}(\frac{\∂F}{{\∂t}_{\mathrm{six}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{six}}^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{siy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{siy}}^{\mathrm{pl}}+\frac{\∂F}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{P}{\mathrm{\Σ}}}(\frac{\∂F}{\∂r}\mathrm{\Δr}+\frac{\∂F}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂F}{\∂\mathrm{\φ}}\mathrm{\Δ\φ})=0$

$G^0+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(\frac{\∂G}{\∂t_{\mathrm{ix}}}{\mathrm{\Δt}}_{\mathrm{ix}}+\frac{\∂G}{\∂t_{\mathrm{iy}}}{\mathrm{\Δt}}_{\mathrm{iy}}+\frac{\∂G}{\∂t_{\mathrm{iz}}}{\mathrm{\Δt}}_{\mathrm{iz}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{ix}}}{\mathrm{\Δ\ω}}_{\mathrm{ix}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{iy}}}{\mathrm{\Δ\ω}}_{\mathrm{iy}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{iz}}}{\mathrm{\Δ\ω}}_{\mathrm{iz}})$

$+\underset{i=1}{\overset{K_{P1}}{\mathrm{\Σ}}}(\frac{\∂G}{{\∂t}_{\mathrm{pix}}}{\mathrm{\Δt}}_{\mathrm{pix}}+\frac{\∂G}{{\∂t}_{\mathrm{piy}}}{\mathrm{\Δt}}_{\mathrm{piy}}+\frac{\∂G}{{\∂t}_{\mathrm{piz}}}{\mathrm{\Δt}}_{\mathrm{piz}})$

$+\underset{i=1}{\overset{K_{s1}}{\mathrm{\Σ}}}(\frac{\∂G}{\∂t_{\mathrm{six}}}{\mathrm{\Δt}}_{\mathrm{six}}+\frac{\∂G}{\∂t_{\mathrm{siy}}}{\mathrm{\Δt}}_{\mathrm{siy}}+\frac{\∂G}{\∂t_{\mathrm{siz}}}{\mathrm{\Δt}}_{\mathrm{siz}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{six}}}{\mathrm{\Δ\ω}}_{\mathrm{six}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{siy}}}{\mathrm{\Δ\ω}}_{\mathrm{siy}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{siz}}}{\mathrm{\Δ\ω}}_{\mathrm{siz}})$

$+\underset{i=1}{\overset{K_{P2}}{\mathrm{\Σ}}}(\frac{\∂G}{\∂t_{\mathrm{pix}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{pix}}^{\mathrm{pl}}+\frac{\∂G}{\∂t_{\mathrm{piy}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{piy}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{K_{s2}}{\mathrm{\Σ}}}(\frac{\∂G}{{\∂t}_{\mathrm{six}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{six}}^{\mathrm{pl}}+\frac{\∂G}{{\∂t}_{\mathrm{siy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{siy}}^{\mathrm{pl}}+\frac{\∂G}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{P}{\mathrm{\Σ}}}(\frac{\∂G}{\∂r}\mathrm{\Δr}+\frac{\∂G}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂G}{\∂\mathrm{\φ}}\mathrm{\Δ\φ})=0---\left(24\right)$

如果假定从图像i(i＝1到N)中检测的点标记的数量为d_pi，正方形标记的数量为d_si，则从N个图像中检测出的点标记的数量D_p和从N个图像中检测出的正方形标记的数量D_s的如表达式(25)所示。

$D_p=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{d}_{\mathrm{pi}}$

$D_s=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{d}_{\mathrm{si}}---\left(25\right)$

如果从N个图像中检测的点标记的数量为D_p并且从N个图像中检测出的正方形标记的数量为D_s，则构成(D_p+4×D_s)套即2×(D_p+4×D_s)个观察方程。如果通过将表达式的左侧的常数项F⁰和G⁰变换到右侧，则以矩阵形式书写的联合方程如表达式(26)所示。

J·Δ＝E    ...(26)

J称为“雅可比矩阵”，其中列有F和G的关于照相摄像机位置和方位、点标记的位置、正方形标记的位置和方位以及平面参数的偏微分系数。雅可比矩阵J的行的数量为观察方程的数量，即2×(D_p+4×D_s)，其列的数量为未知数的数量，即(6×N+3×K_p1+2×K_p2+6×K_s1+3×K_s2+3×P)。Δ表示校正向量。校正向量的大小为未知数的数量，即(6×N+3×K_p1+2×K_p2+6×K_s1+3×K_s2+3×P)。E表示误差向量，具有表示投影位置的计算位置的近似值与观察位置之间的差的-F₀和-G₀。E的大小为观察方程的数量，即2×(D_p+4×D_s)。

需要注意的是，参照坐标系统的原点、刻度和方位可以通过在参照坐标系统中同时拍摄位置已知的点标记或者位置和方位已知的正方形标记得以清楚的指定。在表达式(24)中，关于这些标记的位置和方位的偏微分系数变为0。为了清楚的指定参照坐标系统的原点、刻度和方位，在点标记的情况下需要使用三个位置已知的点标记，或者在正方形标记的情况下需要使用一个位置和方位已知的正方形标记。

接下来，在步骤S120，关于照相摄像机的位置和方位、点标记的位置、正方形标记的位置和方位和平面参数的近似值的校正值通过使用表达式(26)获得。如果雅可比矩阵J为正方形矩阵，则校正值向量Δ通过用矩阵J的逆矩阵乘以表达式(26)的两端获得。如果矩阵J不为正方形矩阵，则校正值向量J用最小均方的方法获得，如表达式(27)所示。

Δ＝(J^t·J)^-1J^t·E    ...(27)

接下来说明关于获得雅可比矩阵J的每个因数的方法。首先，定义F＝[F G]^t。从表达式(7)和(8)，F和G可以写成如表达式(28)中所示，所以F和G为x_c、y_c和z_c的函数。

$F=-f\frac{x_c}{z_c}-u_{\mathrm{ox}}$

$G=-f\frac{y_c}{z_c}-u_{\mathrm{oy}}---\left(28\right)$

雅可比矩阵J_Fxc的因数具有根据F和G的x_c、y_c和z_c的偏微分系数，如表达式(29)所示。

$J_{{\mathrm{Fx}}_c}=\left[\begin{array}{ccc}\frac{\∂F}{{\∂x}_c}& \frac{\∂F}{{\∂y}_c}& \frac{\∂F}{{\∂z}_c}\\ \frac{\∂G}{{\∂x}_c}& \frac{\∂G}{{\∂y}_c}& \frac{\∂G}{{\∂z}_c}\end{array}\right]=\left[\begin{array}{ccc}-\frac{f}{z_c}& 0& \frac{{\mathrm{fx}}_c}{{z_c}^2}\\ 0& -\frac{f}{z_c}& \frac{{\mathrm{fy}}_c}{{z_c}^2}\end{array}\right]---\left(29\right)$

这里，如果x_c、y_c和z_c为变量S₁，S₂到S_m的函数，那么F和G也是变量S₁，S₂到S_m的函数。每个因数具有根据F和G的S₁，S₂到S_m的偏微分系数的雅可比矩阵J_Fs可以进行分解，如表达式(30)所示。

$J_{\mathrm{Fs}}=\left[\begin{array}{cccc}\frac{\∂F}{{\∂s}_1}& \frac{\∂F}{{\∂s}_2}& \·\·\·& \frac{\∂F}{{\∂s}_m}\\ \frac{\∂G}{{\∂s}_1}& \frac{\∂G}{{\∂s}_2}& \·\·\·& \frac{\∂G}{{\∂s}_m}\end{array}\right]$

$=J_{{\mathrm{Fx}}_c}\·J_{x_{c}s}=\left[\begin{array}{ccc}\frac{\∂F}{{\∂x}_c}& \frac{\∂F}{{\∂y}_c}& \frac{\∂F}{{\∂z}_c}\\ \frac{\∂G}{{\∂x}_c}& \frac{\∂G}{{\∂y}_c}& \frac{\∂G}{{\∂z}_c}\end{array}\right]\left[\begin{array}{cccc}\frac{{\∂x}_c}{{\∂s}_1}& \frac{{\∂x}_c}{{\∂s}_2}& \·\·\·& \frac{{\∂x}_c}{{\∂s}_m}\\ \frac{{\∂y}_c}{{\∂s}_1}& \frac{{\∂y}_c}{{\∂s}_2}& \·\·\·& \frac{{\∂y}_c}{{\∂s}_m}\\ \frac{{\∂z}_c}{{\∂s}_1}& \frac{{\∂z}_c}{{\∂s}_2}& \·\·\·& \frac{{\∂z}_c}{{\∂s}_m}\end{array}\right]---\left(30\right)$

通过用照相摄像机位置、照相摄像机方位、标记位置、标记方位和平面参数取代s，可以获得根据F和G的有关照相摄像机位置、照相摄像机方位、标记位置、标记方位和平面参数的偏微分系数。

每个因数具有根据F和G的t_x、t_y和t_z的偏微分系数的雅可比矩阵J_Ft可以写为如表达式(31)所示。

$J_{\mathrm{Ft}}=J_{{\mathrm{Fx}}_c}\·J_{x_{c}t}=J_{{\mathrm{Fx}}_c}\·\left[\begin{array}{ccc}\frac{{\∂x}_c}{{\∂t}_x}& \frac{{\∂x}_c}{{\∂t}_y}& \frac{{\∂x}_c}{{\∂t}_z}\\ \frac{{\∂y}_c}{{\∂t}_x}& \frac{{\∂y}_c}{{\∂t}_y}& \frac{{\∂y}_c}{{\∂t}_z}\\ \frac{{\∂z}_c}{{\∂t}_x}& \frac{{\∂z}_c}{{\∂t}_y}& \frac{{\∂z}_c}{{\∂t}_z}\end{array}\right]$

$=J_{{\mathrm{Fx}}_c}\·\left[\begin{array}{ccc}-R_{11}& -R_{12}& -R_{13}\\ -R_{21}& -R_{22}& -R_{23}\\ -R_{31}& -R_{32}& -R_{33}\end{array}\right]---\left(31\right)$

需要注意的是，方位ω和3×3旋转变换矩阵R之间的关系如表达式(5)所示。

每个因数具有根据F和G的根据照相摄像机方位ω_x、ω_y和ω_z的偏微分系数的雅可比矩阵J_Fω可以以一种分解的方式表示，如表达式(32)中所示。

这里，J_XcR为雅可比矩阵，如表达式(33)中所示。

$J_{x_{c}R}=\left[\begin{array}{ccccccccc}\frac{{\∂x}_c}{{\∂R}_{11}}& \frac{{\∂x}_c}{{\∂R}_{21}}& \frac{{\∂x}_c}{{\∂R}_{31}}& \frac{{\∂x}_c}{{\∂R}_{12}}& \frac{{\∂x}_c}{{\∂R}_{22}}& \frac{{\∂x}_c}{{\∂R}_{32}}& \frac{{\∂x}_c}{{\∂R}_{13}}& \frac{{\∂x}_c}{{\∂R}_{23}}& \frac{{\∂x}_c}{{\∂R}_{33}}\\ \frac{{\∂y}_c}{{\∂R}_{11}}& \frac{{\∂y}_c}{{\∂R}_{21}}& \frac{{\∂y}_c}{{\∂R}_{31}}& \frac{{\∂y}_c}{{\∂R}_{12}}& \frac{{\∂y}_c}{{\∂R}_{22}}& \frac{{\∂y}_c}{{\∂R}_{32}}& \frac{{\∂y}_c}{{\∂R}_{13}}& \frac{{\∂y}_c}{{\∂R}_{23}}& \frac{{\∂y}_c}{{\∂R}_{33}}\\ \frac{{\∂z}_c}{{\∂R}_{11}}& \frac{{\∂z}_c}{{\∂R}_{21}}& \frac{{\∂z}_c}{{\∂R}_{31}}& \frac{{\∂z}_c}{{\∂R}_{12}}& \frac{{\∂z}_c}{{\∂R}_{22}}& \frac{{\∂z}_c}{{\∂R}_{32}}& \frac{{\∂z}_c}{{\∂R}_{13}}& \frac{{\∂z}_c}{{\∂R}_{23}}& \frac{{\∂z}_c}{{\∂R}_{33}}\end{array}\right]$

$=\left[\begin{array}{ccccccccc}{x}_w-t_x& 0& 0& y_w-t_y& 0& 0& z_w-t_z& 0& 0\\ 0& x_w-t_x& 0& 0& y_w-t_y& 0& 0& z_w-t_z& 0\\ 0& 0& x_w-t_x& 0& 0& y_w-t_y& 0& 0& z_w-t_z\end{array}\right]---\left(33\right)$

J_Rraxis也是雅可比矩阵，如表达式(34)所示。

$J_{{\mathrm{Rr}}_{\mathrm{axis}}}=\left[\begin{array}{cccc}\frac{{\∂R}_{11}}{{\∂r}_x}& \frac{{\∂R}_{11}}{{\∂r}_y}& \frac{{\∂R}_{11}}{{\∂r}_z}& \frac{{\∂R}_{11}}{{\∂r}_a}\\ \frac{{\∂R}_{21}}{{\∂r}_x}& \frac{{\∂R}_{21}}{{\∂r}_y}& \frac{{\∂R}_{21}}{{\∂r}_z}& \frac{{\∂R}_{21}}{{\∂r}_a}\\ \frac{{\∂R}_{31}}{{\∂r}_x}& \frac{{\∂R}_{31}}{{\∂r}_y}& \frac{{\∂R}_{31}}{{\∂r}_z}& \frac{{\∂R}_{31}}{{\∂r}_a}\\ \frac{{\∂R}_{12}}{{\∂r}_x}& \frac{{\∂R}_{12}}{{\∂r}_y}& \frac{{\∂R}_{12}}{{\∂r}_z}& \frac{{\∂R}_{12}}{{\∂r}_a}\\ \frac{{\∂R}_{22}}{{\∂r}_x}& \frac{{\∂R}_{22}}{{\∂r}_y}& \frac{{\∂R}_{22}}{{\∂r}_z}& \frac{{\∂R}_{22}}{{\∂r}_a}\\ \frac{{\∂R}_{32}}{{\∂r}_x}& \frac{{\∂R}_{32}}{{\∂r}_y}& \frac{{\∂R}_{32}}{{\∂r}_z}& \frac{{\∂R}_{32}}{{\∂r}_a}\\ \frac{{\∂R}_{13}}{{\∂r}_x}& \frac{{\∂R}_{13}}{{\∂r}_y}& \frac{{\∂R}_{13}}{{\∂r}_z}& \frac{{\∂R}_{13}}{{\∂r}_a}\\ \frac{{\∂R}_{23}}{{\∂r}_x}& \frac{{\∂R}_{23}}{{\∂r}_y}& \frac{{\∂R}_{23}}{{\∂r}_z}& \frac{{\∂R}_{23}}{{\∂r}_a}\\ \frac{{\∂R}_{33}}{{\∂r}_x}& \frac{{\∂R}_{33}}{{\∂r}_y}& \frac{{\∂R}_{33}}{{\∂r}_z}& \frac{{\∂R}_{33}}{{\∂r}_a}\end{array}\right]$

$=\left[\begin{array}{cccc}{2r}_x(1-{\cos r}_a)& 0& 0& ({r_x}^2-1){\sin r}_a\\ r_y(1-{\cos r}_a)& r_x(1-{\cos r}_a)& {\sin r}_a& r_x{r}_y{\sin r}_a+r_z{\cos r}_a\\ r_z(1-{\cos r}_a)& -{\sin r}_a& r_x(1-{\cos r}_a)& r_z{r}_x{\sin r}_a-r_y{\cos r}_a\\ r_y(1-{\cos r}_a)& r_x(1-{\cos r}_a)& -{\sin r}_a& r_x{r}_y{\sin r}_a-r_z\cos r\\ 0& {2r}_y(1-{\cos r}_a)& 0& ({r_y}^2-1){\sin r}_a\\ {\sin r}_a& r_z(1-{\cos r}_a)& r_y(1-{\cos r}_a)& r_y{r}_z{\sin r}_a+r_x{\cos r}_a\\ r_z(1-{\cos r}_a)& {\sin r}_a& r_x(1-{\cos r}_a)& r_z{r}_x{\sin r}_a+r_y{\cos r}_a\\ -{\sin r}_a& r_z(1-{\cos r}_a)& r_y(1-{\cos r}_a)& r_y{r}_z{\sin r}_a-r_x{\cos r}_a\\ 0& 0& {2r}_z(1-{\cos r}_a)& ({r_z}^2-1){\sin r}_a\end{array}\right]---\left(34\right)$

如果定义

则

为如表达式(35)所示的雅可比矩阵。

更进一步的，

为如表达式(36)所示的雅可比矩阵。

每个因数具有根据没有被限制到F和G平面上的点标记的位置t_px、t_py和t_pz的偏微分系数的雅可比矩阵J_FtP可以表示为如表达式(37)所示。

$J_{{\mathrm{Ft}}_p}=J_{{\mathrm{Fx}}_c}\·J_{x_c{t}_p}=J_{{\mathrm{Fx}}_c}\·\left[\begin{array}{ccc}\frac{\∂x_c}{\∂t_{\mathrm{px}}}& \frac{\∂x_c}{\∂t_{\mathrm{py}}}& \frac{\∂x_c}{\∂t_{\mathrm{pz}}}\\ \frac{\∂y_c}{\∂t_{\mathrm{px}}}& \frac{\∂y_c}{\∂t_{\mathrm{py}}}& \frac{\∂y_c}{\∂t_{\mathrm{pz}}}\\ \frac{\∂z_c}{\∂t_{\mathrm{px}}}& \frac{\∂z_c}{\∂t_{\mathrm{py}}}& \frac{\∂z_c}{\∂t_{\mathrm{pz}}}\end{array}\right]=J_{{\mathrm{Fx}}_c}\·\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]---\left(37\right)$

每个因数具有根据没有被限制到F和G平面上的正方形标记的位置t_sx、t_sy和t_sz的偏微分系数的雅可比矩阵J_FtS可以表示为如表达式(38)所示。

$J_{{\mathrm{Ft}}_s}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·J_{x_w{t}_s}=J_{{\mathrm{Fx}}_c}\·\left[\begin{array}{ccc}\frac{{\∂x}_c}{{\∂x}_w}& \frac{{\∂x}_c}{{\∂y}_w}& \frac{{\∂x}_c}{{\∂z}_w}\\ \frac{{\∂y}_c}{{\∂x}_w}& \frac{{\∂y}_c}{{\∂y}_w}& \frac{{\∂y}_c}{{\∂z}_w}\\ \frac{{\∂z}_c}{{\∂x}_w}& \frac{{\∂z}_c}{{\∂y}_w}& \frac{{\∂z}_c}{{\∂z}_w}\end{array}\right]\·\left[\begin{array}{ccc}\frac{{\∂x}_w}{{\∂t}_{\mathrm{sx}}}& \frac{{\∂x}_w}{{\∂t}_{\mathrm{sy}}}& \frac{{\∂x}_w}{{\∂t}_{\mathrm{sz}}}\\ \frac{{\∂y}_w}{{\∂t}_{\mathrm{sx}}}& \frac{{\∂y}_w}{{\∂t}_{\mathrm{sy}}}& \frac{{\∂y}_w}{{\∂t}_{\mathrm{sz}}}\\ \frac{{\∂z}_w}{{\∂t}_{\mathrm{sx}}}& \frac{{\∂z}_w}{{\∂t}_{\mathrm{sy}}}& \frac{{\∂z}_w}{{\∂t}_{\mathrm{sz}}}\end{array}\right]$

$=J_{{\mathrm{Fx}}_c}\·\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]\·\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(38\right)$

每个因数具有根据没有被限制到F和G平面上的正方形标记的方位ω_s的偏微分系数的雅可比矩阵J_FωS可以分解为如表达式(39)所示。

$J_{{\mathrm{F\ω}}_s}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·J_{x_w{\mathrm{\ω}}_s}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·J_{x_w{R}_s}\·J_{R_s{\mathrm{\ω}}_s}$

$=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{ccccccccc}{x}_s& 0& 0& y_s& 0& 0& 0& 0& 0\\ 0& x_s& 0& 0& y_s& 0& 0& 0& 0\\ 0& 0& x_s& 0& 0& y_s& 0& 0& 0\end{array}\right]\·J_{R_s{\mathrm{\ω}}_s}---\left(39\right)$

J_RSωS可以以与表达式(34)至(36)同样的方式得到。

接下来，每个因数具有根据被限制到F和G平面上的点标记的位置t^pl _px和t^pl _py的偏微分系数的雅可比矩阵J_Ftppl可以表示为如表达式(40)所示。

J_xCxW可以以与表达式(38)同样的方式获得。

$J_{{\mathrm{Ft}}_p^{\mathrm{pl}}}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·J_{x_w{t}_p^{\mathrm{pl}}}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{cc}\frac{{\∂x}_w}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}& \frac{{\∂x}_w}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}\\ \frac{{\∂y}_w}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}& \frac{{\∂y}_w}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}\\ \frac{{\∂z}_w}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}& \frac{{\∂z}_w}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}\end{array}\right]$

$=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\φ}\\ -\sin \mathrm{\θ}\cos \mathrm{\φ}& \sin \mathrm{\φ}\end{array}\right]---\left(40\right)$

每个因数具有根据被限制到F和G平面上的正方形标记的位置t^pl _sx和t^pl _sy的偏微分系数的雅可比矩阵J_Ftspl可以表示为如表达式(41)所示。

$J_{{\mathrm{Ft}}_s^{\mathrm{pl}}}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·J_{x_w{x}_y}\·J_{x_p{t}_s^{\mathrm{pl}}}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{cc}\frac{{\∂x}_w}{{\∂x}_p}& \frac{{\∂x}_w}{{\∂x}_p}\\ \frac{{\∂y}_w}{{\∂x}_p}& \frac{{\∂y}_w}{{\∂y}_p}\\ \frac{{\∂z}_w}{{\∂x}_p}& \frac{{\∂z}_w}{{\∂z}_p}\end{array}\right]\·\left[\begin{array}{cc}\frac{{\∂x}_p}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}& \frac{{\∂x}_p}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}\\ \frac{{\∂y}_p}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}& \frac{{\∂y}_p}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}\end{array}\right]$

$=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_p}\·\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\φ}\\ -\sin \mathrm{\θ}\cos \mathrm{\φ}& \sin \mathrm{\φ}\end{array}\right]\·\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]---\left(41\right)$

每个因数具有根据被限制到F和G平面上的正方形标记的方位θ^pl的偏微分系数的雅可比矩阵J_Fθ ^pl可以表示为如表达式(42)所示。

$J_{{\mathrm{F\θ}}^{\mathrm{pl}}}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·J_{x_w{x}_p}\·J_{x_p{\mathrm{\θ}}^{\mathrm{pl}}}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{cc}\frac{{\∂x}_w}{{\∂x}_p}& \frac{{\∂x}_w}{{\∂y}_p}\\ \frac{{\∂y}_w}{{\∂x}_p}& \frac{{\∂y}_w}{{\∂y}_p}\\ \frac{{\∂z}_w}{{\∂x}_p}& \frac{{\∂x}_w}{{\∂z}_p}\end{array}\right]\·\left[\begin{array}{c}\frac{{\∂x}_p}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}\\ \frac{{\∂y}_p}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}\end{array}\right]$

$=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\φ}\\ -\sin \mathrm{\θ}\cos \mathrm{\φ}& \sin \mathrm{\φ}\end{array}\right]\·\left[\begin{array}{c}-x_s{\sin \mathrm{\θ}}^{\mathrm{pl}}-y_s{\cos \mathrm{\θ}}^{\mathrm{pl}}\\ x_s{\cos \mathrm{\θ}}^{\mathrm{pl}}-y_s{\sin \mathrm{\θ}}^{\mathrm{pl}}\end{array}\right]---\left(42\right)$

每个因数具有根据F和G的平面参数r、θ和的偏微分系数的雅可比矩阵J_Pr可以表示为如表达式(43)所示。

$J_{\mathrm{Fr}}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·J_{x_{w}r}=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{ccc}\frac{{\∂x}_w}{\∂r}& \frac{{\∂x}_w}{\∂\mathrm{\θ}}& \frac{{\∂x}_w}{\∂\mathrm{\φ}}\\ \frac{{\∂y}_w}{\∂r}& \frac{{\∂y}_w}{\∂\mathrm{\θ}}& \frac{{\∂y}_w}{\∂\mathrm{\φ}}\\ \frac{{\∂z}_w}{\∂r}& \frac{{\∂z}_w}{\∂\mathrm{\θ}}& \frac{{\∂z}_w}{\∂\mathrm{\φ}}\end{array}\right]$

$=J_{{\mathrm{Fx}}_c}\·J_{x_c{x}_w}\·\left[\begin{array}{ccc}\sin \mathrm{\θ}& -x_p\sin \mathrm{\θ}+r\cos \mathrm{\θ}& 0\\ -\cos \mathrm{\θ}\sin \mathrm{\φ}& x_p\cos \mathrm{\θ}\sin \mathrm{\φ}+r\sin \mathrm{\θ}\sin \mathrm{\φ}& x_p\sin \mathrm{\θ}\cos \mathrm{\φ}-y_p\sin \mathrm{\φ}-r\cos \mathrm{\θ}\cos \mathrm{\φ}\\ \cos \mathrm{\θ}\cos \mathrm{\φ}& -x_p\cos \mathrm{\θ}\cos \mathrm{\φ}-r\sin \mathrm{\θ}\cos \mathrm{\φ}& x_p\sin \mathrm{\θ}\sin \mathrm{\φ}+y_p\cos \mathrm{\φ}-r\cos \mathrm{\θ}\sin \mathrm{\φ}\end{array}\right]---\left(43\right)$

接下来，在步骤S140，判断收敛。收敛的判断通过判断一个条件完成，例如，校正值的绝对值是否小于特定的阀值，投影位置的计算位置和观察位置之间的差是否小于特定的阀值，投影位置的计算位置和观察位置是否变为最小值，或类似条件。如果判断结果是满足收敛，则完成标记的位置和方位的估计，否则，流程返回步骤S110，根据关于照相摄像机的位置和方位、点标记的位置、正方形标记的位置和方位和平面参数的校正后的近似值再次构成联立方程。

如前所述，在本实施例中，大量的布置有标记的场景的图像被拍摄，从拍摄的图像中检测和识别标记，指定同个平面上的多个标记，根据平面上的标记观察位置和标记位于同个平面的限制条件获得关于载有图像的照相摄像机的位置和方位、没有被限制在平面上的标记的位置和方位、被限制在平面上的标记的位置和方位和平面参数的对应于近似值的校正值，然后，用获得的校正值对近似值进行校正。重复用于获得对应于近似值的校正值并用所获得的校正值对近似值进行校正的操作循环，这样就得到了标记的位置和方位以便满足所述限制条件。

需要注意的是，在本实施例中，平面参数都是未知的，但是本发明并不仅限于此，即平面参数可以是可知的。例如，如果平面参数可以容易地获得，例如在参照坐标系统中x-y平面的情况，平面参数可以是已知的。如果平面参数是已知的，表达式(22)中被限制在平面上的点标记的观察方程如表达式(44)所示。

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z+\frac{\∂F}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{px}}^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{py}}^{\mathrm{pl}}=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z+\frac{\∂G}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{px}}^{\mathrm{pl}}+\frac{\∂G}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{py}}^{\mathrm{pl}}=0---\left(44\right)$

同样，在表达式(23)中被限制在平面上的正方形标记的观察方程如表达式(45)所示。

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}{\mathrm{\Δt}}_x^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_y^{\mathrm{pl}}+\frac{\∂F}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}}=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂G}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{sx}}^{\mathrm{pl}}+\frac{\∂G}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{sy}}^{\mathrm{pl}}+\frac{\∂G}{\∂{\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}}=0---\left(45\right)$

如上所述，根据本实施例，如果存在关于标记位置的限制条件，可以通过获得标记的位置和方位准确地获得标记的位置和方位，从而满足该限制条件。

第二实施例

对于第一实施例，如果多个标记位于同个平面上这一限制条件被作为关于标记位置的限制条件给出，一种用于估计标记的位置和方位从而满足该限制条件的方法已经被描述。对于第二实施例，将描述关于多个标记位于多个平面并且平面之间的角度为已知的情况。

需要注意的是，这里将要描述的是关于存在两个平面并且该两个平面之间的角度为已知的情况，但是平面的数量并不限于两个。

图10表示存在两个限制标记的平面的情况。正如在第一实施例中所描述的，在参照坐标系统中平面1用极性坐标表达式r₁＝[r₁θ₁φ_.1]表示。

另一方面，平面2也用一个极性坐标表达式r₂＝[r₂θ₂φ₂]表示，假定θ₂和φ₂表示与平面1相关的方位。更具体的，假定当平面1绕着y^pl轴旋转θ₂然后绕着x_p1轴旋转φ₂时获得的方位是平面2的方位。r₂是参照坐标系统中的原点和平面2之间带正负号的距离。这时，表示平面1和平面2的方位的旋转矩阵R^p1和R^p2可以分别表示为如表达式(47)和(48)所示。

$R^{p1}=\left[\begin{array}{ccc}{R}_{11}^{p1}& R_{12}^{p1}& R_{13}^{p1}\\ R_{21}^{p1}& R_{22}^{p1}& R_{23}^{p1}\\ R_{31}^{p1}& R_{32}^{p1}& R_{33}^{p1}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\cos \mathrm{\φ}}_1& {-\sin \mathrm{\φ}}_1\\ 0& {\sin \mathrm{\φ}}_1& {\cos \mathrm{\φ}}_1\end{array}\right]\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_1& 0& {\sin \mathrm{\θ}}_1\\ 0& 1& 0\\ {-\sin \mathrm{\θ}}_1& 0& {\cos \mathrm{\θ}}_1\end{array}\right]$

$=\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_1& 0& {\sin \mathrm{\θ}}_1\\ {\sin \mathrm{\θ}}_1{\sin \mathrm{\φ}}_1& {\cos \mathrm{\φ}}_1& {-\cos \mathrm{\θ}}_1{\sin \mathrm{\φ}}_1\\ {-\sin \mathrm{\θ}}_1{\cos \mathrm{\φ}}_1& {\sin \mathrm{\φ}}_1& {\cos \mathrm{\θ}}_1{\cos \mathrm{\φ}}_1\end{array}\right]---\left(47\right)$

$R^{p2}=\left[\begin{array}{ccc}{R}_{11}^{p2}& R_{12}^{p2}& R_{13}^{p2}\\ R_{21}^{p2}& R_{22}^{p2}& R_{23}^{p2}\\ R_{31}^{p2}& R_{32}^{p2}& R_{33}^{p2}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\cos \mathrm{\φ}}_2& {-\sin \mathrm{\φ}}_2\\ 0& {\sin \mathrm{\φ}}_2& {\cos \mathrm{\φ}}_2\end{array}\right]\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_2& 0& {\sin \mathrm{\θ}}_2\\ 0& 1& 0\\ {-\sin \mathrm{\θ}}_2& 0& {\cos \mathrm{\θ}}_2\end{array}\right]\·R^{p1}$

$=\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_2& 0& {\sin \mathrm{\θ}}_2\\ {\sin \mathrm{\θ}}_2{\sin \mathrm{\φ}}_2& {\cos \mathrm{\φ}}_2& {-\cos \mathrm{\θ}}_2{\sin \mathrm{\φ}}_2\\ {-\sin \mathrm{\θ}}_2{\cos \mathrm{\φ}}_2& {\sin \mathrm{\φ}}_2& {\cos \mathrm{\θ}}_2{\cos \mathrm{\φ}}_2\end{array}\right]\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_1& 0& {\sin \mathrm{\θ}}_1\\ {\sin \mathrm{\θ}}_1{\sin \mathrm{\φ}}_1& {\cos \mathrm{\φ}}_1& {-\cos \mathrm{\θ}}_1{\sin \mathrm{\φ}}_1\\ {-\sin \mathrm{\θ}}_1{\cos \mathrm{\φ}}_1& {\sin \mathrm{\φ}}_1& {\cos \mathrm{\θ}}_1{\cos \mathrm{\φ}}_1\end{array}\right]---\left(48\right)$

这里，平面1的法线向量

和平面2的法线向量

之间的角度为α，所以得出表达式(49)。

$\cos \mathrm{\α}=\frac{{\stackrel{\→}{n}}_1\·{\stackrel{\→}{n}}_2}{\left|{\stackrel{\→}{n}}_1\right|\·\left|{\stackrel{\→}{n}}_2\right|}={\cos \mathrm{\θ}}_2{\cos \mathrm{\φ}}_2=\mathrm{cons}\tan t---\left(49\right)$

这里，φ₂可以用θ₂和α从表达式(49)中得出。

根据第一实施例，被限制到平面1的点标记和正方形标记的观察方程和表达式(22)及(23)是一样的。另一方面，平面2为r₂和θ₂的函数，所以被限制到平面2的点标记和正方形标记的观察方程分别表示为如表达式(50)和(51)所示。

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{px}}^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{py}}^{\mathrm{pl}}+\frac{\∂F}{\∂r_2}{\mathrm{\Δr}}_2+\frac{\∂F}{\∂{\mathrm{\θ}}_2}\mathrm{\Δ}{\mathrm{\θ}}_2=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂G}{{\∂t}_{\mathrm{px}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{px}}^{\mathrm{pl}}+\frac{\∂G}{{\∂t}_{\mathrm{py}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{py}}^{\mathrm{pl}}+\frac{\∂G}{\∂r_2}{\mathrm{\Δr}}_2+\frac{\∂G}{\∂{\mathrm{\θ}}_2}\mathrm{\Δ}{\mathrm{\θ}}_2=0---\left(50\right)$

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}{\mathrm{\Δt}}_x^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_y^{\mathrm{pl}}+\frac{\∂F}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}}+\frac{\∂F}{\∂r_2}\mathrm{\Δ}{r}_2+\frac{\∂F}{\∂{\mathrm{\θ}}_2}\mathrm{\Δ}{\mathrm{\θ}}_2=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{{\∂t}_{\mathrm{sx}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{sx}}^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{sy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{sy}}^{\mathrm{pl}}+\frac{\∂F}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}}+\frac{\∂G}{\∂r_2}{\mathrm{\Δr}}_2+\frac{\∂G}{\∂{\mathrm{\θ}}_2}{\mathrm{\Δ\θ}}_2---\left(51\right)$

对于本实施例，如果获得标记的位置和方位，则根据观察方程(22)、(23)、(50)和(51)构成联立方程，获得关于照相摄像机位置和方位、标记的位置和方位、平面1的三个参数以及平面2的两个参数的关于近似值的校正值，并且使用重复的计算对近似值进行优化。要注意的是，如果从表达式(49)获得φ₂，则得到具有不同符号的两个解，但是在φ₂的这两个解中，每次重复计算得到的关于θ₂的投影误差减少得较多的一个解作为解。

如前所述，对于本实施例，如果多个标记位于多个平面作为关于标记位置的限制条件并且这些平面之间的角度为已知，则可以获得标记的位置和方位以便满足该限制条件。

第三实施例

对于第三实施例，下面将描述，在将例如多个平面标记的标记的法向向量方向相同作为与标记的位置相关的限制条件的情况下，标记的位置和方位的估计方法。

图11为表示可以应用第三实施例的标记的放置的示意图。在该附图中，假定布置有四个正方形标记，该四个正方形标记的法向向量即每个正方形标记的z_s轴的方向相同。

正方形标记的法向向量方向将用θ和φ表示。法向向量通过在参照坐标系统中的z轴的单位向量绕x轴旋转θ然后绕y轴旋转φ获得。对于具有相同法向向量方向的多个正方形标记，θ和φ具有共同的值。如果每个正方形标记绕z轴旋转角度为ψ，关于参照系统的正方形标记的方位R_s用表达式(52)表示。

$R_s=\left[\begin{array}{ccc}{R}_{s11}& R_{s12}& R_{s13}\\ R_{s21}& R_{s22}& R_{s23}\\ R_{s31}& R_{s32}& R_{s33}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\φ}& 0& \sin \mathrm{\φ}\\ 0& 1& 0\\ -\sin \mathrm{\φ}& 0& \cos \mathrm{\φ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ 0& \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]---\left(52\right)$

如果假定在参照坐标系统中每个正方形标记的位置为t_s＝[t_sx t_syt_sz]^t，则在正方形标记上的点x_s＝[x_s y_s]^t在参照坐标系统中的位置x_w用表达式(53)表示。

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{ccc}{R}_{s11}& R_{s12}& t_{\mathrm{sx}}\\ R_{s21}& R_{s22}& t_{\mathrm{sy}}\\ R_{s31}& R_{s32}& t_{\mathrm{sz}}\end{array}\right]\left[\begin{array}{c}{x}_s\\ y_s\\ 1\end{array}\right]---\left(53\right)$

对于本实施例，在每个正方形标记的每个顶点的参照坐标系统中的位置构成法向向量方向θ和φ、正方形标记ψ的z轴方向和每个正方形标记的参照坐标系统的位置t_s的函数。相应地，法向向量被限制的正方形标记的每个顶点的观察方程用表达式(54)表示。

$F^0+\frac{\∂F}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂F}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂F}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂F}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂F}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂F}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂F}{\∂t_{\mathrm{sx}}}{\mathrm{\Δt}}_{\mathrm{sx}}+\frac{\∂F}{\∂t_{\mathrm{sy}}}{\mathrm{\Δt}}_{\mathrm{sy}}+\frac{\∂F}{\∂t_{\mathrm{sz}}}{\mathrm{\Δt}}_{\mathrm{sz}}+\frac{\∂F}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂F}{\∂\mathrm{\φ}}\mathrm{\Δ\φ}+\frac{\∂F}{\∂\mathrm{\ψ}}\mathrm{\Δ\ψ}=0$

$G^0+\frac{\∂G}{\∂t_x}{\mathrm{\Δt}}_x+\frac{\∂G}{\∂t_y}{\mathrm{\Δt}}_y+\frac{\∂G}{\∂t_z}{\mathrm{\Δt}}_z+\frac{\∂G}{\∂{\mathrm{\ω}}_x}{\mathrm{\Δ\ω}}_x+\frac{\∂G}{\∂{\mathrm{\ω}}_y}{\mathrm{\Δ\ω}}_y+\frac{\∂G}{\∂{\mathrm{\ω}}_z}{\mathrm{\Δ\ω}}_z$

$+\frac{\∂G}{\∂t_{\mathrm{sx}}}{\mathrm{\Δt}}_{\mathrm{sx}}+\frac{\∂G}{\∂t_{\mathrm{sy}}}{\mathrm{\Δt}}_{\mathrm{sy}}+\frac{\∂G}{\∂t_{\mathrm{sz}}}{\mathrm{\Δt}}_{\mathrm{sz}}+\frac{\∂G}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂G}{\∂\mathrm{\φ}}\mathrm{\Δ\φ}+\frac{\∂G}{\∂\mathrm{\ψ}}\mathrm{\Δ\ψ}=0---\left(54\right)$

对于本实施例，如果获得标记的位置和方位，根据多个观察方程(54)构成联立方程，获得关于照相摄像机的位置和方位、标记的位置和方位和法向向量方向θ和φ的与近似值相关的校正值，然后使用重复的计算对近似值进行优化。

如上所述，对于本实施例，如果具有共同法向向量方向的多个标记作为关于标记放置的限制条件，则可以获得标记的位置和方位以便满足该限制条件。

第四实施例

对于第一个、第二个和第三实施例，在一个关于标记的放置的限制条件下获得标记的位置和方位。对于第四实施例，将说明在多个限制条件下获得标记的位置和方位的方法。

图12为表示可以使用本实施例的标记的放置的示意图。该附图说明以下情况，即点标记1和正方形标记1位于同一个平面上(平面1)，正方形标记2和正方形标记3具有共同的法向向量方向，并且点标记2和正方形标记4没有关于放置的限制条件。

对于用摄像装置拍摄的如图12所示的场景，观察方程(22)适用于点标记1，观察方程(23)适用于正方形标记1，观察方程(54)适用于正方形标记2和3，观察方程(20)适用于点标记2，观察方程(21)适用于正方形标记4。表达式(20)到(23)和表达式(54)为关于一个照相摄像机的位置和方位的校正值、一个标记的位置和方位的校正值、一个平面参数的校正值和一个法向向量方向的校正值的观察方程。对于本实施例，如表达式(55)中所示，所列出的观察方程与N个照相摄像机的位置和方位的校正值、没有被限制到平面的点标记K_p1的位置的校正值、被限制到平面的点标记K_p2的位置的校正值、没有被限制到平面的正方形标记K_s1的位置和方位的校正值、被限制到平面的正方形标记K_s2的位置和方位的校正值、限制法向向量的正方形标记K_s3的位置和方位的校正值和V法向向量方向的校正值相关。

$F^0+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(\frac{\∂F}{\∂t_{\mathrm{ix}}}{\mathrm{\Δt}}_{\mathrm{ix}}+\frac{\∂F}{\∂t_{\mathrm{iy}}}{\mathrm{\Δt}}_{\mathrm{iy}}+\frac{\∂F}{\∂t_{\mathrm{iz}}}{\mathrm{\Δt}}_{\mathrm{iz}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{ix}}}{\mathrm{\Δ\ω}}_{\mathrm{ix}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{iy}}}{\mathrm{\Δ\ω}}_{\mathrm{iy}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{iz}}}{\mathrm{\Δ\ω}}_{\mathrm{iz}})$

$+\underset{i=1}{\overset{K_{P1}}{\mathrm{\Σ}}}(\frac{\∂F}{{\∂t}_{\mathrm{pix}}}{\mathrm{\Δt}}_{\mathrm{pix}}+\frac{\∂F}{{\∂t}_{\mathrm{piy}}}{\mathrm{\Δt}}_{\mathrm{piy}}+\frac{\∂F}{{\∂t}_{\mathrm{piz}}}{\mathrm{\Δt}}_{\mathrm{piz}})$

$+\underset{i=1}{\overset{K_{s1}}{\mathrm{\Σ}}}(\frac{\∂F}{\∂t_{\mathrm{six}}}{\mathrm{\Δt}}_{\mathrm{six}}+\frac{\∂F}{\∂t_{\mathrm{siy}}}{\mathrm{\Δt}}_{\mathrm{siy}}+\frac{\∂F}{\∂t_{\mathrm{siz}}}{\mathrm{\Δt}}_{\mathrm{siz}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{six}}}{\mathrm{\Δ\ω}}_{\mathrm{six}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{siy}}}{\mathrm{\Δ\ω}}_{\mathrm{siy}}+\frac{\∂F}{\∂{\mathrm{\ω}}_{\mathrm{siz}}}{\mathrm{\Δ\ω}}_{\mathrm{siz}})$

$+\underset{i=1}{\overset{K_{P2}}{\mathrm{\Σ}}}(\frac{\∂F}{\∂t_{\mathrm{pix}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{pix}}^{\mathrm{pl}}+\frac{\∂F}{\∂t_{\mathrm{piy}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{piy}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{K_{s2}}{\mathrm{\Σ}}}(\frac{\∂F}{{\∂t}_{\mathrm{six}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{six}}^{\mathrm{pl}}+\frac{\∂F}{{\∂t}_{\mathrm{siy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{siy}}^{\mathrm{pl}}+\frac{\∂F}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{K_{s3}}{\mathrm{\Σ}}}(\frac{\∂F}{{\∂t}_{\mathrm{six}}^v}{\mathrm{\Δt}}_{\mathrm{six}}^v+\frac{\∂F}{{\∂t}_{\mathrm{siy}}^v}{\mathrm{\Δt}}_{\mathrm{siy}}^v+\frac{\∂F}{{\∂t}_{\mathrm{siy}}^v}{\mathrm{\Δt}}_{\mathrm{siy}}^v+\frac{\∂F}{\∂\mathrm{\Ψ}}\mathrm{\Δ\Ψ})$

$+\underset{i=1}{\overset{P}{\mathrm{\Σ}}}(\frac{\∂F}{\∂r}\mathrm{\Δr}+\frac{\∂F}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂F}{\∂\mathrm{\φ}}\mathrm{\Δ\φ})$

$+\underset{i=1}{\overset{V}{\mathrm{\Σ}}}(\frac{\∂F}{{\∂\mathrm{\θ}}^v}{\mathrm{\Δ\θ}}^v+\frac{\∂F}{{\∂\mathrm{\φ}}^v}{\mathrm{\Δ\φ}}^v)=0$

$G^0+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(\frac{\∂G}{\∂t_{\mathrm{ix}}}{\mathrm{\Δt}}_{\mathrm{ix}}+\frac{\∂G}{\∂t_{\mathrm{iy}}}{\mathrm{\Δt}}_{\mathrm{iy}}+\frac{\∂G}{\∂t_{\mathrm{iz}}}{\mathrm{\Δt}}_{\mathrm{iz}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{ix}}}{\mathrm{\Δ\ω}}_{\mathrm{ix}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{iy}}}{\mathrm{\Δ\ω}}_{\mathrm{iy}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{iz}}}{\mathrm{\Δ\ω}}_{\mathrm{iz}})$

$+\underset{i=1}{\overset{K_{P1}}{\mathrm{\Σ}}}(\frac{\∂G}{{\∂t}_{\mathrm{pix}}}{\mathrm{\Δt}}_{\mathrm{pix}}+\frac{\∂G}{{\∂t}_{\mathrm{piy}}}{\mathrm{\Δt}}_{\mathrm{piy}}+\frac{\∂G}{{\∂t}_{\mathrm{piz}}}{\mathrm{\Δt}}_{\mathrm{piz}})$

$+\underset{i=1}{\overset{K_{s1}}{\mathrm{\Σ}}}(\frac{\∂G}{\∂t_{\mathrm{six}}}{\mathrm{\Δt}}_{\mathrm{six}}+\frac{\∂G}{\∂t_{\mathrm{siy}}}{\mathrm{\Δt}}_{\mathrm{siy}}+\frac{\∂G}{\∂t_{\mathrm{siz}}}{\mathrm{\Δt}}_{\mathrm{siz}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{six}}}{\mathrm{\Δ\ω}}_{\mathrm{six}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{siy}}}{\mathrm{\Δ\ω}}_{\mathrm{siy}}+\frac{\∂G}{\∂{\mathrm{\ω}}_{\mathrm{siz}}}{\mathrm{\Δ\ω}}_{\mathrm{siz}})$

$+\underset{i=1}{\overset{K_{P2}}{\mathrm{\Σ}}}(\frac{\∂G}{\∂t_{\mathrm{pix}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{pix}}^{\mathrm{pl}}+\frac{\∂G}{\∂t_{\mathrm{piy}}^{\mathrm{pl}}}\mathrm{\Δ}{t}_{\mathrm{piy}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{K_{s2}}{\mathrm{\Σ}}}(\frac{\∂G}{{\∂t}_{\mathrm{six}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{six}}^{\mathrm{pl}}+\frac{\∂G}{{\∂t}_{\mathrm{siy}}^{\mathrm{pl}}}{\mathrm{\Δt}}_{\mathrm{siy}}^{\mathrm{pl}}+\frac{\∂G}{{\∂\mathrm{\θ}}^{\mathrm{pl}}}{\mathrm{\Δ\θ}}^{\mathrm{pl}})$

$+\underset{i=1}{\overset{K_{s3}}{\mathrm{\Σ}}}(\frac{\∂G}{{\∂t}_{\mathrm{six}}^v}{\mathrm{\Δt}}_{\mathrm{six}}^v+\frac{\∂G}{{\∂t}_{\mathrm{siy}}^v}{\mathrm{\Δt}}_{\mathrm{siy}}^v+\frac{\∂G}{{\∂t}_{\mathrm{siy}}^v}{\mathrm{\Δt}}_{\mathrm{siy}}^v+\frac{\∂G}{\∂\mathrm{\Ψ}}\mathrm{\Δ\Ψ})$

$+\underset{i=1}{\overset{P}{\mathrm{\Σ}}}(\frac{\∂G}{\∂r}\mathrm{\Δr}+\frac{\∂G}{\∂\mathrm{\θ}}\mathrm{\Δ\θ}+\frac{\∂G}{\∂\mathrm{\φ}}\mathrm{\Δ\φ})$

$+\underset{i=1}{\overset{V}{\mathrm{\Σ}}}(\frac{\∂G}{{\∂\mathrm{\θ}}^v}{\mathrm{\Δ\θ}}^v+\frac{\∂F}{{\∂\mathrm{\φ}}^v}{\mathrm{\Δ\φ}}^v)=0---\left(55\right)$

作为观察方程的表达式(55)为每个点标记和在拍摄的图像上检测到的每个正方形标记的每个顶点构成，并作为联立方程求解，这样获得对应于关于照相摄像机的位置t_i、照相摄像机的方位ω_i、放置未被限制的点标记的位置t_pi、放置未被限制的正方形标记的位置t_si、方位ω_si、被限制到平面的点标记的位置t^pl _pi、被限制到平面的正方形标记的位置t^pl _si、方位θ^pl _i、限制法向向量的正方形标记的位置t^v _si、方位ψ_i、平面参数r_i、法向向量方向θ^v _i和φ^v _i的近似值的校正值，并同时通过使用重复计算重复近似值的校正从而对近似值进行优化。

根据本实施例用于设置标记放置条件的GUI(图形用户接口)如图15A到15C所示。对于图15A到15C中的GUI，可以设置标记的编组和每个组的限制条件类型。图15A到15C中的GUI在图5中的GUI以外还包括用于设置限制条件的功能。当用户用鼠标右键点击选择的标记(在图15A中)时，显示一个用于选择限制条件的对话框(图15B)。然后，用户使用这个对话框设定一个关于选择的标记组的限制条件(例如平面限制、法向向量限制、直线限制)。要注意的是，用户接口的其它格式也可以使用，例如，可以用图15C中所示的菜单代替图15B中所示的对话框。

如上所述，对于本实施例，在存在多个关于标记的放置的限制条件的情况下，可以获得标记的位置和方位以便满足所有的限制条件。这样，可以精确地获得标志的位置和方位。

第一个修改

如图13所示，一个标记可以被限制在一个圆柱体的侧面。对于该圆柱体，原点位于圆柱体坐标系统的中心，圆柱体坐标系统如附图中所示。圆柱体的方位用绕x轴的θ和绕y轴的φ表示。表示圆柱体的方位的旋转矩阵R_cyl如表达式(56)所示。

$R_{\mathrm{cyl}}=\left[\begin{array}{ccc}{R}_{\mathrm{cyl}11}& R_{\mathrm{cyl}12}& R_{\mathrm{cyl}13}\\ R_{\mathrm{cyl}21}& R_{\mathrm{cyl}22}& R_{\mathrm{cyl}23}\\ R_{\mathrm{cyl}31}& R_{\mathrm{cyl}32}& R_{\mathrm{cyl}33}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\φ}& 0& \sin \mathrm{\φ}\\ 0& 1& 0\\ -\sin \mathrm{\φ}& 0& \cos \mathrm{\φ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ 0& \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]---\left(56\right)$

如果在参照坐标系统中圆柱体的原点的位置为t^cyl＝[t^cyl _x t^cyl _y t^cyl _z]^t，则在参照坐标系统中圆柱体坐标系统上的点x_cyl＝[x_cyl y_cyl z_cyl]^t的位置x_w如表达式(57)所示。

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{R}_{\mathrm{cyl}11}& R_{\mathrm{cyl}12}& R_{\mathrm{cyl}13}& t_x^{\mathrm{cyl}}\\ R_{\mathrm{cyl}21}& R_{\mathrm{cyl}22}& R_{\mathrm{cyl}23}& t_y^{\mathrm{cyl}}\\ R_{\mathrm{cyl}31}& R_{\mathrm{cyl}32}& R_{\mathrm{cyl}33}& t_z^{\mathrm{cyl}}\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{cyl}}\\ y_{\mathrm{cyl}}\\ z_{\mathrm{cyl}}\\ 1\end{array}\right]---\left(57\right)$

在圆柱体坐标系统中，表达式(58)用绕z_cyl轴的旋转角度β、坐标d和已知的圆柱体的半径r_cyl表示圆柱体侧面上的点。

$\left[\begin{array}{c}{x}_{\mathrm{cyl}}\\ y_{\mathrm{cyl}}\\ z_{\mathrm{cyl}}\end{array}\right]=\left[\begin{array}{c}{r}_{\mathrm{cyl}}\cos \mathrm{\β}\\ r_{\mathrm{cyl}}\sin \mathrm{\β}\\ d\end{array}\right]---\left(58\right)$

被限制在圆柱体的侧面上的标记的投影位置构成关于照相摄像机的位置和方位、圆柱体的位置和方位和圆柱体上的标记的位置β和d的函数，所以被限制在圆柱体的侧面上的标记的位置可以通过求解用于获得这些校正值的观察方程而得到。

第二次修改

如图14所示，标记可以被限制到球体的表面。球体坐标系统位于球体的中心。球体没有方向性并相应地仅用关于参照坐标系统的位置表示。如果在参照坐标系统中球体坐标系统的原点的位置为t^sph＝[t^sph _x t^sph _y t^sph _z]^t，则在参照坐标系统中球体坐标系统上的点x_sph＝[x_sph y_sph z_sph]^t的位置x_w如表达式(59)所示。

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{c}{x}_{\mathrm{sph}}+t_x^{\mathrm{sph}}\\ y_{\mathrm{sph}}+t_y^{\mathrm{sph}}\\ z_{\mathrm{sph}}+t_z^{\mathrm{sph}}\end{array}\right]---\left(59\right)$

具有已知半径r_sph以及角度θ和φ的球体表面上的点在球体坐标系统中用表达式(60)表示。

$\left[\begin{array}{c}{x}_{\mathrm{cyl}}\\ y_{\mathrm{cyl}}\\ z_{\mathrm{cyl}}\end{array}\right]=\left[\begin{array}{c}{r}_{\mathrm{sph}}\cos \mathrm{\θ}\cos \mathrm{\φ}\\ r_{\mathrm{sph}}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ r_{\mathrm{sph}}\sin \mathrm{\φ}\end{array}\right]---\left(60\right)$

被限制到球体表面上的标记的投影位置构成一个关于照相摄像机的位置和方位、球体的位置和球体表面上的标记的位置θ和φ的函数，所以，被限制到球体表面上的标记的位置可以通过求解用于获得这些校正值的观察方程而得到。

第三个修改

标记可以被限制到直线上。如果假定直线通过的点为tl＝[tlx tlytlz]t，并且方向向量为d＝[dx dy dz]t，则在参照坐标系统中位于直线上的标记的位置用表达式(61)表示。这里，s为在直线上表示标记位置的变量。

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{c}{t}_{\mathrm{lx}}+{\mathrm{sd}}_x\\ t_{\mathrm{ly}}+{\mathrm{sd}}_y\\ t_{\mathrm{lz}}+{\mathrm{sd}}_z\end{array}\right]---\left(61\right)$

被限制到直线上的标记的投影位置构成一个关于照相摄像机的位置和方位、直线通过的点t1、直线的方向向量d和直线上标记的位置s的函数，所以被限制到直线上的标记的位置可以通过求解用于获得这些校正值的观察方程而得到。

第四个修改

对于上述的实施例，对将泰勒扩展用于观察方程并且重复进行运用主要项的校正的方法作为校正标记放置信息的方法进行描述。这是一种在假定观察方程在一次处理循环中符合本地线性化的条件且其结果没有误差的情况下对未知参数的校正次数进行估计的方法，是一种相当于已知的通常称为牛顿方法的方法。所述牛顿方法是一种使用数值计算求解非线性方程的典型方法，但是用于上述实施例的重复计算方法并不限于此。例如，校正的次数可以根据在一次处理中估计的未知参数的校正次数的不同而动态的改变，例如使用同样是已知的Levenberg-Marquardt方法。另外，可以采用根据包括高阶项的泰勒扩展的结果完成重复的计算的方法。本发明的实质是使用各种类型的标记即使在混合了具有不同放置信息的多种类型的标记的情况下仍然可以获得最合适的解，相应地，作为数值解的重复计算的特定方法也不会离开本发明的实质。

当参照示意的实施例对本发明进行说明时，应该理解本发明并不限于披露的实施例。相反，本发明的意在覆盖包括在所附权利要求的精神和范围内的各种修改和等同的方案。下面的权利要求的范围应当给予最宽的解释从而包括所有这样的修改和等同的结构和功能。

Claims (7)

1.一种标记放置信息估计方法，用于估计位于几何限制的空间内的标记的空间信息，所述方法包括：

标记检测步骤，从拍摄的包括标记的图像中检测标记；

限制条件设置步骤，设置标记的限制条件；

标记投影位置计算步骤，根据所述拍摄的图像的摄像单元的位置和方位的近似值和标记的放置信息的近似值，计算标记被投影到图像表面上的位置；

校正值计算步骤，获得用于校正标记的放置信息的校正值，以便减少在所述标记检测步骤中检测到的图像表面上的标记的位置和在所述标记投影位置计算步骤中获得的图像表面上的标记的投影位置之间的总误差，并且满足在所述限制条件设置步骤中设置的限制条件；

放置信息校正步骤，根据在所述校正值计算步骤中获得的校正值对标记的放置信息进行校正；

重复计算步骤，使用在所述放置信息校正步骤中校正后的标记的放置信息代替所述标记投影位置计算步骤的所述标记的放置信息的近似值，通过重复执行所述标记投影位置计算步骤、所述校正值计算步骤和所述放置信息校正步骤对标记的放置信息进行计算。

2.根据权利要求1的标记放置信息估计方法，其中所述限制条件用相对于所述限制条件唯一的方程表示，并且满足所述限制条件的放置信息用相对于所述限制条件唯一的放置信息参数表示。

3.根据权利要求2的标记放置信息估计方法，其中所述限制条件为标记位于同一个未知平面上，相对于所述限制条件唯一的方程为平面方程，并且所述放置信息参数为平面上的标记的位置和旋转轴为平面的法向向量的旋转角。

4.根据权利要求1的标记放置信息估计方法，其中所述标记具有平面形状。

5.根据权利要求2的标记放置信息估计方法，其中位于空间内的所述标记为放置信息可以定义为位置和方位的标记，

其中所述限制条件为多个标记的法向向量的方向相同，相对于所述限制条件唯一的方程是法向向量方程，并且所述放置信息参数为标记的位置和旋转轴为所述法向向量的旋转角。

6.根据权利要求2的标记放置信息估计方法，其中存在多个所述限制条件，并且通过获得相对于每个限制条件唯一的方程和关于所述多个限制条件的所述标记的放置信息参数对所述标记的放置信息进行估计。

7.一种标记放置信息估计设备，用于估计位于几何限制的空间内的标记的空间信息，所述设备包括：

标记检测装置，从拍摄的包括标记的图像中检测标记；

限制条件设置装置，设置标记的限制条件；

标记投影位置计算装置，根据所述拍摄的图像的摄像单元的位置和方位的近似值和标记的放置信息的近似值，计算标记被投影到图像表面上的位置；

校正值计算装置，获得用于校正标记的放置信息的校正值，以便减少在所述标记检测装置中检测到的图像表面上的标记的位置和在所述标记投影位置计算装置中获得的图像表面上的标记的投影位置之间的总误差，并且满足在所述限制条件设置装置中设置的限制条件；

放置信息校正装置，根据在所述校正值计算装置中获得的校正值对标记的放置信息进行校正；

重复计算装置，使用在所述放置信息校正装置中校正后的标记的放置信息代替所述标记投影位置计算装置的所述标记的放置信息的近似值，重复使用所述标记投影位置计算装置、所述校正值计算装置和所述放置信息校正装置对标记的放置信息进行计算。

Images