CN101116101A - 位置/姿势测量方法和设备 - Google Patents

Abstract

一种用于稳定准确地测量位置和姿势的位置姿势测量装置。拍摄示出在空间上排列的一个或多个指标的图像，从拍摄到的图像中检测该指标。当检测到多个指标时，确定分布范围，并根据尺寸选择应用于位置姿势计算的算法(S3033，S3050，S3060)。例如，如果指标分布在足够宽的范围上，则作为未知数计算成像装置的位置和姿势的六个参数(S3070)。当指标的分布范围越小时，使要确定的未知参数的自由度越小(S3035，S3025)。

Description

位置/姿势测量方法和设备

技术领域

本发明涉及一种测量物体的位置和姿势、尤其是摄像装置的位置和姿势的方法和设备。

背景技术

近年来，关于针对现实空间和虚拟空间的无缝融合的混和现实已经进行了广泛研究。呈现混和现实的图像显示设备可以作为显示通过将根据摄像装置的位置和姿势所生成的虚拟空间(由计算机图形绘制的虚拟物体、文本信息等)的图像叠加在由摄像机等摄像装置拍摄的现实空间的图像上而获得的图像的设备来实现。

实现这种图像显示设备的要点是对在现实空间上定义的基准坐标系(作为确定要叠加的虚拟物体的位置和姿势时的基准的现实空间上的坐标系)和摄像装置的坐标系(照相机坐标系)之间的相对位置和姿势的测量。这是因为为了绘制虚拟物体(虚拟空间图像)以符合现实空间上的位置，必须使用与相对于基准坐标系的摄像装置的物理照相机参数相同的照相机参数生成虚拟物体的图像。例如，当将虚拟物体的图像叠加在现实房间中的某个位置处时，在该房间上定义基准坐标系，并且可以计算摄像装置在该基准坐标系中的位置和姿势。当要将任意的虚拟图案或标签叠加在观察者的手所保持的现实箱子上时，箱子本身的物体坐标系被认为是基准坐标系，并且可以计算箱子(基准坐标系)相对于摄像装置的位置和姿势。

作为测量摄像装置的位置和姿势的方法，通常的作法是在现实空间上安排或设置多个指标(人为标记、自然特征等)以检测由摄像装置拍摄的图像中的指标的投影图像的坐标，并基于与指标的坐标信息的关系计算摄像装置的位置和姿势(例如，非专利文献1)。然而，使用这种方法具有指标必须总被拍摄的约束。

另一方面，已经尝试在摄像装置上安装使用磁传感器、超声传感器等的6自由度的位置/姿势传感器，并使用从通过拍摄指标得到的图像中获得的信息(图像信息)来校正由该传感器测得的摄像装置的位置和姿势的误差(例如，专利文献1和专利文献2)。利用专利文献2中所公布的方法，当在所拍摄的图像中检测到指标时，基于该信息校正传感器测量值的误差。当没有检测到指标时，不改变6自由度位置/姿势传感器的测量值，并将其用作摄像装置的位置和姿势。因为可以获得摄像装置的位置和姿势而与是否检测到指标无关，所以可以稳定地呈现混和现实。

利用专利文献2的方法，当检测到的指标的数量是三个或更多时，基于图像信息计算摄像装置的位置和姿势的六个自由度。当检测到的指标的数量是两个或一个时，应用用于校正由传感器测得的摄像装置的位置和姿势中的一个(两个或三个自由度)的处理。更具体来说，切换用于计算摄像装置的位置和姿势的算法以将检测到的指标的数量作为判断标准。这样，即使当仅基于图像信息无法计算摄像装置的位置和姿势时(当拍摄到的指标的数量少于三个时)，可以参考传感器测量值获得位置和姿势，该位置和姿势已经过校正以尽可能地消除传感器测量值中的误差。

然而，利用专利文献1的方法，应用用于基于图像信息仅校正传感器测得的摄像装置的位置和姿势中的一个而与检测到的指标的数量无关的处理。利用这种校正方法，当校正姿势时，针对各个检测到的指标单独计算用于消除指标的误差的旋转校正值，并将该转动校正值平均以计算姿势测量值的校正值。当校正位置时，针对各个检测到的指标单独计算用于消除指标的误差的平移校正值，并将该平移校正值平均以计算位置测量值的校正值。由于校正的自由度限于两个或三个而与指标的数量无关，所以即使当信息量不充足时也可以获得稳定的解决。

非专利文献1：Kato等：An Augmented Reality System and itsCalibration based on Marker Tracking，TVRSJ，vol.4，no.4，pp.607-616，1999。

非专利文献2：J.Park，B.Jiang和U.Neumann：Vision-basedpose computation：robust and accurate augmented reality tracking，Proc.2nd International Workshop on Augmented Reality(IWAR′99)，pp.3-12，1999。

非专利文献3：D.G.Lowe：Fitting parameterizedthree-dimensional models to images，IEEE Transactions on PAMI，vol.13，no.5，pp.441-450，1991。

非专利文献4：Satoh，Uchiyama和Yamamoto：UG+B：ARegistration Framework Using User′s View，Gyroscope，andBird′s-Eye View，TVRSJ，vol.10，no.3，pp.391-400，2005。

非专利文献5：I.S krypnyk和D.Lowe：Scene modeling，recognition and tracking with invariant image features，Proc.3rdInternational Symposium on Mixed and Augmented Reality(ISMAR′04)，pp.110-119，2004。

非专利文献6：D.Kotake，K.Satoh，S.Uchiyama和H.Yamamoto：A hybrid and linear registration method utilizinginclination constraint，Proc. 4th International Symposium onMixed and Augmented Reality(ISMAR′05)，pp.140-149，2005。

非专利文献7：D.G.Lowe，Fitting Parameterized three-dimensionalmodels to images，IEEE Transactions on PAMI，vol.13，no.5，pp.441-450，1991。

专利文献1：日本特开2003-222509号公报

专利文献2：日本特开2003-279310号公报

专利文献3：日本特开2003-344018号公报

专利文献4：日本特开2004-233334号公报

发明内容

本发明要解决的问题

利用专利文献2的方法，当观测到三个或更多个指标时，总是选择用于计算位置和姿势的六个自由度的算法。然而，在实践中，存在这样的情况：在指标不均匀地分布在图像上的局部区域中并且在该区域中观测到该指标的情形中，即使当指标的数量是三个或更多个时，输入图像不包括充足的图像信息以稳定地计算位置的六个自由度。利用专利文献2的方法，所获得的解决方案的准确度和稳定性在这种情况下通常变得不足，存在改进的空间。

另一方面，专利文献1的方法把重点放在稳定性而不是准确度上，即使在指标不均匀地分布在图像上的局部区域中并且在该区域中观测到该指标的情形中图像信息不充足的情况下，与专利文献2的方法相比较一般可以进行稳定的测量。这是因为实际的执行系统使用具有弱准确度但具有高稳定性的磁传感器等六自由度传感器。然而，由于即使在获得了充足的图像信息时，该方法也只校正部分参数，因此不是通过最佳地利用图像信息来获得高准确度的方法。具体来说，作为用于混和现实的摄像装置的位置/姿势推定方法，更希望通过最佳地利用在所拍摄的图像中捕捉到的指标来推定摄像装置的位置和姿势。这是因为：由于这种方法是针对将虚拟物体叠加在所拍摄的图像上，因此需要将图像平面的偏差最小化。换句话说，专利文献2的方法在使用图像信息获得准确度方面还有改进的余地。

如上所述，难以同时实现达到专利文献1的高稳定性的效果和专利文献2的使用图像信息获得高准确度的效果。

此外，由于专利文献1的方法仅计算各个指标的二维校正值的平均值，因此在最佳校正上不能够将所有指标上的误差的总和最小化有改进的余地。

本发明是考虑到这种现有技术的问题而做出的，其目的是提供一种能够测量测量对象物体的位置和姿势同时实现高稳定性和高准确度的位置/姿势测量方法和设备。

解决问题的手段

为了实现以上目的，本发明具有以下配置。

根据权利要求1的发明针对一种用于测量测量对象物体的位置和姿势的位置/姿势测量方法，其特征在于，包括：拍摄图像获取步骤，用于获取拍摄图像；指标检测步骤，用于检测所述拍摄图像中所包括的指标；分布范围计算步骤，用于计算与所述指标检测步骤中检测到的指标的分布范围有关的评价值；以及位置/姿势计算步骤，用于使用与所述检测到的指标的图像坐标有关的信息和与所述分布范围有关的所述评价值来计算所述测量对象物体的位置和姿势。

根据权利要求15的发明针对一种用于测量测量对象物体的位置和姿势的位置/姿势测量方法，其特征在于，包括：姿势测量值输入步骤，用于从安装到所述测量对象物体的姿势传感器输入姿势测量值；拍摄图像获取步骤，用于获取拍摄图像；指标检测步骤，用于检测所述拍摄图像中所包括的指标；以及位置/姿势计算步骤，用于使用与所述检测到的指标的图像坐标有关的信息和所述姿势测量值来计算所述摄像装置的所述位置和姿势，其中，在所述位置/姿势计算步骤中，使用以下差异作为评价基准来计算所述摄像装置的位置和姿势，所述差异是：与所述检测到的指标的所述图像坐标有关的信息的实际测量值和基于所述摄像装置的所述位置和姿势的推定值计算出的与所述检测到的指标的所述图像坐标有关的信息的理论值之间的差异，以及所述推定值和所述姿势测量值的至少部分参数之间的差异。

根据权利要求20的发明针对一种用于测量作为测量对象物体的摄像装置的位置和姿势的位置/姿势测量方法，其特征在于，包括：姿势测量值输入步骤，用于从安装到所述摄像装置的姿势传感器输入姿势测量值；图像输入步骤，用于输入由所述摄像装置拍摄的拍摄图像；指标检测步骤，用于检测包括在所述拍摄图像中的指标；权重设置步骤，用于设置由操作者输入的权重；以及位置/姿势计算步骤，用于使用与所述指标检测步骤中检测到的所述指标的图像坐标有关的信息以及所述姿势测量值来计算所述摄像装置的位置和姿势，其中，在所述位置/姿势计算步骤中，通过根据在所述权重设置步骤中设置的权重改变与所述指标的所述图像坐标有关的信息和所述姿势测量值的影响来计算所述摄像装置的位置和姿势。

本发明的效果

根据本发明的位置/姿势测量设备，由于可以使用不仅考虑到检测到的指标的数量而且还考虑到与它们的分布范围相关的评价值的更好的方法来测量测量对象物体的位置和姿势，所以可以实现与传统方法相比较具有高稳定性和准确度的测量。

从以下结合附图的说明，本发明的其它特征和优点将变得明显。注意，在所有附图中相同的附图标记表示相同或相似的组件。

附图说明

包含在说明书中并构成说明书的一部分的附图示出本发明的实施例，并与说明书一起用于解释本发明的原理。

图1是示出根据第一实施例的位置/姿势测量设备的配置的框图；

图2是示出可通过软件实现位置/姿势测量设备的各个单元的计算机的基本配置的框图；

图3是用于说明根据第一实施例的用于计算摄像装置130和要测量的物体170的位置和姿势的处理的流程图；

图4是用于说明图3中的步骤S3035中的位置和姿势计算处理的细节的流程图；

图5是用于说明图3中的步骤S3070中的位置和姿势计算处理的细节的流程图；

图6示出通过由摄像装置130拍摄要测量的物体170的图像而获得的拍摄图像的例子；

图7示出通过由摄像装置130拍摄要测量的物体170的图像而获得的拍摄图像的例子；

图8示出通过由摄像装置130拍摄要测量的物体170的图像而获得的拍摄图像的例子；

图9示出将步骤S3040中的处理应用于图6中所示的拍摄图像(拍摄图像600)时获得的凸包(convex hull)；

图10示出将步骤S3040中的处理应用于图7中所示的拍摄图像(拍摄图像700)时获得的凸包；

图11示出将步骤S3040中的处理应用于图8中所示的拍摄图像(拍摄图像800)时获得的凸包；

图12是示出根据第二实施例的位置/姿势测量设备的配置的框图；

图13是示出根据第三实施例的位置/姿势测量设备的配置的框图；

图14是示出根据第四实施例的位置/姿势测量设备的配置的框图；

图15是示出根据第五实施例的位置/姿势测量设备的配置的框图；

图16是用于说明根据第五实施例的用于计算摄像装置130的位置和姿势的处理的流程图；

图17是用于说明根据第六实施例的用于计算摄像装置130和要测量的物体170的位置和姿势的处理的流程图；

图18是示出检测到的指标之间的距离和通过公式(28)获得的权重w₁之间的关系的图；

图19是示出凸包的面积和通过公式(26)和(28)获得的权重w₁和w₂之间的关系的图；

图20是示出根据第七实施例的位置/姿势测量设备的配置的框图；

图21是用于说明根据第七实施例的用于计算摄像装置130的位置和姿势的处理的流程图；

图22是示出根据第八实施例的位置/姿势测量设备的配置的框图；

图23是用于说明根据第八实施例的用于计算要测量的物体2270的位置和姿势的处理的流程图；以及

图24是用于说明使用指标的再投影误差和姿势传感器的误差作为评价基准的位置和姿势推定处理的流程图。

具体实施方式

下面将参照附图详细说明本发明的优选实施例。

第一实施例

根据本实施例的位置/姿势测量设备测量摄像装置和测量对象物体的位置和姿势。下面说明根据本实施例的位置/姿势测量设备和位置/姿势测量设备。

图1示出根据本实施例的位置/姿势测量设备的配置。如图1所示，根据本实施例的位置/姿势测量设备100包括图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元120。设备100连接到摄像装置130和位置/姿势传感器140，即安装到摄像装置130和要测量的物体170的传感器140a和140b。

在要测量的物体170上的多个位置处安排多个已知其在物体坐标系(在要测量的物体170上定义的坐标系)上的位置的指标Q_k(k＝1，2，...，K)。图1的例子示出K＝9即安排九个指标Q₁～Q₉的情况。例如，可以由具有不同颜色和相同形状的的标记(图1中的圆形)来构成指标Q_k，也可以由具有不同纹理特征的自然特征等的特征点来构成指标Q_k。作为选择，可以使用由每个具有特定面积的矩形单色区域形成的矩形指标。指标Q_k可以具有任何形式，只要它们在拍摄图像上的投影图像的图像坐标是可检测的并且可以通过任意方法来识别这些指标Q_k中的每一个即可。此外，指标可以是特意设置的，也可以是自然形状的非特意设置的指标。

将从作为例如摄像机的摄像装置130输出的图像(以下称为拍摄图像)输入到位置/姿势测量设备100。

作为六自由度传感器的位置/姿势传感器140a和140b分别安装到摄像装置130和要测量的物体170，并测量摄像装置130和要测量的物体170在基准坐标系上的位置和姿势。将从位置/姿势传感器140输出的测量值输入到位置/姿势测量设备100。每个位置/姿势传感器140包括例如可以从美国Polhemus得到的FASTRAK等。注意，由位置/姿势传感器140测得的位置和姿势包括由磁场的畸变等的影响而产生的误差。然而，这些传感器可以在它们的测量范围内连续稳定地测量位置和姿势。

图像输入单元160将输入到位置/姿势设备100的拍摄图像转换为数字数据，并将该数字数据输出到指标检测单元110。

传感器测量值输入单元150从位置/姿势传感器140即传感器140a和140b接收测量值，并将它们输出到位置/姿势计算单元120。

指标检测单元110从图像输入单元160接收拍摄图像，并检测在输入的图像中捕捉到的指标Q_k的图像坐标。例如，当由具有不同颜色的标记构成指标Q_k时，单元110从拍摄图像中检测对应于每个单独标记颜色的区域，并确定其重心位置作为关注的指标的检测到的坐标。另一方面，当由具有不同纹理特征的特征点构成指标Q_k时，单元110将使用作为已知信息预先保持的每个单独指标的模板图像的模板匹配应用到拍摄图像，从而检测关注指标的位置。此外，当使用矩形指标时，单元110对图像施加二值化处理，并标注二值图像以检测每个由四条直线形成的区域作为候选指标。此外，单元110检查每个候选区域是否包括特定图案以消除检测误差并获取该指标的标识符。注意，在本说明书中以这种方式检测到的矩形指标被视为由四个顶点单独形成的四个指标。

指标检测单元110还将检测到的指标Q_kn的图像坐标u^Qkn和它们的标识符k_n输出到位置/姿势计算单元120。注意，n(n＝1，...，N)是每个检测到的指标的指标号，N代表检测到的指标的总数。

位置/姿势计算单元120从传感器测量值输入单元150接收作为输出的摄像装置130和要测量的物体170的位置和姿势的测量值，并从指标检测单元110接收作为输出的单独指标Q_kn的图像坐标u^Qkn。单元120校正要测量的物体170或摄像装置130的位置和姿势的测量值的误差，并输出校正后的位置和姿势数据。

注意，图1中所示的图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元120中的至少一部分可以作为独立装置来实施，也可以作为通过将软件程序安装在一个或多个计算机中并由每个计算机的CPU执行它们来实现上述功能的软件程序来实施。在本实施例中，假定各单元(图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元120)由软件来实现并安装在单个计算机中。

图2是示出可通过执行软件来实现图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元120的功能的计算机的基本配置的框图。

CPU 1001使用存储在RAM 1002和ROM 1003中的程序和数据来控制整个计算机，并控制图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元120的软件程序的执行以实现各个单元的功能。

RAM 1002包括用于临时存储从外部存储装置1007或存储介质驱动1008加载的程序和数据的区、以及CPU 1001执行各种处理所需的工作区。

ROM 1003一般存储计算机的程序、设置数据等。键盘1004和鼠标1005是输入装置。操作者可以这些输入装置将各种指令输入到CPU 1001。

显示单元1006包括CRT、液晶显示器等，并且可以显示为了例如位置/姿势测量而显示的消息等。

外部存储装置1007是硬盘驱动等作为大容量信息存储装置的装置，并保存OS(操作系统)、由CPU 1001执行的程序等等。在本实施例的说明中，外部存储装置1007保存作为已知信息描述的信息，并根据需要将该信息加载到RAM 1002中。

存储介质驱动1008根据来自CPU 1001的指令读出存储在CD-ROM、DVD-ROM等存储介质中的程序和数据，并将所读出的程序和数据输出到RAM 1002和外部存储装置1007。

I/F 1009包括用于连接摄像装置130的模拟视频端口、或IEEE1394等数字输入/输出端口、用于连接位置/姿势传感器140的RS232C、USB等的串行端口、用于向外部输出计算后的位置和姿势的以太网(Ethernet)端口等等。从各装置输入的数据经由I/F1002存储在RAM 1002中。由I/F1009来实现图像输入单元160和传感器测量值输入单元150的部分功能。

通过总线1010相互连接上述组件。

图3是示出位置/姿势计算单元120的处理序列的流程图。在本实施例中，当CPU 1001执行用于实现位置/姿势计算单元120的功能的程序时执行该处理序列。假定在执行以下处理之前已经从例如外部存储装置1007将根据该流程图的程序代码加载到RAM 1002上。

在步骤S3000，位置/姿势计算单元120从指标提取单元110接收检测到的指标Q_kn的图像坐标u^Qkn和它们的标识符k_n。假定事先将各指标在物体坐标系上的三维(3D)坐标x_o ^Qkn作为已知值加载到RAM 1002上。

在步骤S3005，位置/姿势计算单元120从传感器测量值输入单元150接收由传感器测量的摄像装置130和物体170的位置和姿势的测量值。

在步骤S3010中，位置/姿势计算单元120检查是否检测到指标。如果没有检测到指标，则处理跳至步骤S3080；否则，处理前进至步骤S3020。

在步骤S3020中，位置/姿势计算单元120检查检测到的指标的总数是否是一个。如果指标的总数是一个，则处理前进至步骤S3025；否则，处理前进至步骤S3030。

在步骤S3025，位置/姿势计算单元120对在步骤S3005中获得的摄像装置130的姿势的测量值的平转和倾斜角度施加校正以消除检测到的指标的误差(指标在物体坐标系上的3D坐标x_o ^Qkn、从摄像装置130和要测量的物体170的位置和姿势的测量值得出的投影坐标的理论值x_o ^Qkn*以及实际检测到的坐标u^Qk之间的误差)。然后处理前进到步骤S3080。由于可以采用已知的处理作为步骤S3025中的校正处理，所以将不给出进一步的说明。更具体来说，可以使用例如专利文献1中公开的方法(使用一个标志通过照相机的转动的校正方法(“0019”段至“0029”段)以及使用多个标志通过照相机的转动的校正方法(“0039”段至“0050”段))。

在步骤S3030中，位置/姿势计算单元120检查检测到的指标的总数是否是两个。如果指标的总数是两个，则处理前进到步骤S3033；否则，处理前进到步骤S3040。

在步骤S3033中，位置/姿势计算单元120计算图像上两个检测到的指标之间的距离，并将计算后的值与被定义为预定值的阈值T₁(例如，图像对角线长度的1/4)进行比较。如果该距离等于或大于阈值T₁，则处理前进到步骤S3035；否则处理前进到步骤S3025以执行上述处理。

在步骤S3035中，位置/姿势计算单元120对表示摄像装置130的姿势的测量值的三个参数(平转、倾斜和旋转角)进行校正以将所有检测到的指标的误差的总和最小化，同时将要测量的物体170的位置和姿势的测量值和在步骤S3005中获得的摄像装置130的位置的测量值设定为固定值。然后，处理前进到步骤S3080。下面说明该处理步骤的细节。

仅当检测到的指标的总数是三个或更多个时执行步骤S3040。在该步骤中，作为所有检测到的指标的分布范围的评价值的例子，位置/姿势计算单元120计算包括所有检测到的指标的图像坐标的凸包。由于作为图像处理的基本问题计算图像上的点群的凸包的方法是普通的现有技术，因此将不给出其详细说明。

图6、图7和图8示出当摄像装置130拍摄要测量的物体170的图像时获得的拍摄图像的例子。在图6所示的拍摄图像600上，观测到指标Q1、Q5、Q6和Q9。在图7所示的拍摄图像700上，观测到指标Q1、Q2、Q6和Q7。在图8所示的拍摄图像800上，观测到指标Q2、Q6和Q7。假定其它指标隐藏在另一物体(在本实施例中为用户的手610)的后面。

当检测到这些拍摄图像的指标并执行图3的流程图中所示的位置/姿势计算单元120的处理步骤时，由于从每个图像检测到的指标的数量是三个或者更多，所以执行步骤S3040和随后步骤中的处理。图9、图10和图11分别示出由对拍摄图像600、700和800执行步骤S3040中的处理时获得的检测到的指标形成的凸包900、1000和1100。在以下说明中，假定将A₉₀₀、A₁₀₀₀和A₁₁₀₀设为各个凸包的面积，以下所述的阈值T₂和T₃满足关系A₁₁₀₀＜T₂＜A₁₀₀₀＜T₃＜A₉₀₀。

在步骤S3050中，位置/姿势计算单元120将步骤S3040中计算出的凸包的面积与定义为预定值的阈值T₂(例如，整个拍摄图像的面积的1/16)进行比较。如果该面积等于或大于阈值T₂，则处理前进到步骤S3060；否则，处理前进到步骤S3025。在图6至图11中所示的例子中，针对拍摄图像800处理前进到步骤S3025，针对拍摄图像700和600处理前进到步骤S3060。

在步骤S3060中，位置/姿势计算单元120将步骤S3040中计算出的凸包的面积与定义为预定值的阈值T₃(例如，整个拍摄图像的面积的1/9)进行比较。如果该面积等于或大于阈值T₃，则处理前进到步骤S3070；否则，处理前进到步骤S3035。在图6至图11中所示的例子中，针对拍摄图像700处理前进到步骤S3035，针对拍摄图像600处理前进到步骤S3070。

在步骤S3070中，位置/姿势计算单元120计算摄像装置130的位置和姿势以将所有检测到的指标的误差的总和最小化。下面说明该处理步骤的细节。

在步骤S3080中，位置/姿势计算单元120经由I/F1009向外部输出代表在步骤S3025、S3035或S3070中获得的摄像装置130和要测量的物体170的位置和姿势的数据。作为选择，单元120将这些数据转换为其它应用程序可使用的状态之后，将它们存储在RAM 1002中。

注意，由于在步骤S3025、S3035或S3070中只校正与摄像装置130相关的测量值，因此在步骤S3080中输出的位置和姿势数据包括与要测量的物体170相关的在步骤S3005中输入的传感器的测量值本身。然而，位置和姿势的输出模式不限于此。相反，可以不校正摄像装置130的测量值，并按原样输出传感器的测量值，可以校正并输出要测量的物体170的测量值。在此情况下，在步骤S3080中，位置/姿势计算单元120执行以下说明的坐标转换处理，然后输出转换后的位置和姿势。

在使用齐次坐标系通过4×4坐标转换矩阵描述位置和姿势时，基于摄像装置130的位置和姿势M^# _WC(^#是表示传感器的测量值的符号；它适用于以下说明)、作为传感器测量值获得的要测量的物体170的位置和姿势M^# _WO以及作为直到步骤S3070的处理结果而得到的摄像装置130的校正后的位置和姿势M_WC，通过下式计算要测量的物体170的校正后的位置和姿势M_WO：

$M_{\mathrm{WO}}={M^{\#}}_{\mathrm{WC}}\·{M_{\mathrm{WC}}}^{-1}\·{M^{\#}}_{\mathrm{WO}}---\left(1\right)$

这时，由下式给出要测量的物体170在由摄像装置130定义的照相机坐标系上的位置和姿势：

$M_{\mathrm{CO}}={M_{\mathrm{WC}}}^{-1}\·{M^{\#}}_{\mathrm{WO}}={{M^{\#}}_{\mathrm{WC}}}^{-1}\·M_{\mathrm{WO}}---\left(2\right)$

更具体来说，要测量的物体170和摄像装置130的相对位置和姿势之间的关系在以下两种情况之间相互等价：一种情况是输出作为直到步骤S3070的处理结果获得的摄像装置130的校正后的位置和姿势(M_WC)和作为传感器测量值获得的要测量的物体170的位置和姿势(M^# _WO)的情况，另一种情况是输出作为传感器测量值获得的摄像装置130的位置和姿势(M^# _WC)和通过公式(1)算出的要测量的物体170的校正后的位置和姿势(M_WO)的情况。因此，优选选择与使用该设备的输出的应用程序所需的模式相对应的输出模式。

可以使用公式(2)计算要测量的物体170在照相机坐标系上的位置和姿势(M_CO)并将其输出，也可以计算并输出摄像装置130在物体坐标系上的位置和姿势(M_CO ^-1)。

在步骤S3090，位置/姿势计算单元120检查处理是否要结束。如果处理没有要结束，则处理返回到步骤S3000。

下面使用图4的流程图说明步骤S3035中摄像装置130的姿势计算处理步骤。在以下说明中，由三值矢量S＝ω_WC＝[ξ_WCψ_WCζ_WC]^T内部地表示作为要计算的未知参数的摄像装置130的姿势。

可以使用通过三值表示姿势的各种方法。在这种情况下，使用由矢量的大小定义旋转角、由矢量的方向定义旋转轴方向的三值矢量来表示姿势。注意，可以由下式给出的3×3旋转矩阵R来表示姿势ω：

$R\left(\mathrm{\ω}\right)=\left[\begin{array}{ccc}\frac{{\mathrm{\ξ}}^2}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& \frac{\mathrm{\ξ\ψ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})-\frac{\mathrm{\ζ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{\mathrm{\ξ\ζ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\frac{\mathrm{\ψ}}{\mathrm{\θ}}\sin \mathrm{\θ}\\ \frac{\mathrm{\ψ\ξ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\frac{\mathrm{\ζ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{{\mathrm{\ψ}}^2}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& \frac{\mathrm{\ψ\ζ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})-\frac{\mathrm{\ξ}}{\mathrm{\θ}}\sin \mathrm{\θ}\\ \frac{\mathrm{\ζ\ξ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})-\frac{\mathrm{\ψ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{\mathrm{\ζ\ψ}}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\frac{\mathrm{\ξ}}{\mathrm{\θ}}\sin \mathrm{\θ}& \frac{{\mathrm{\ζ}}^2}{{\mathrm{\θ}}^2}(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}\end{array}\right]$

$(\mathrm{for\θ}=\sqrt{{\mathrm{\ξ}}^2+{\mathrm{\ψ}}^2+{\mathrm{\ζ}}^2})---\left(3\right)$

ω和R可以唯一地相互转换。由于对本领域的技术人员来说从R到ω的转换方法是已知的，所以不给出其详细说明。

在步骤S4000中，位置/姿势计算单元120将作为传感器测量值获得的摄像装置130的姿势设置为初始值s。

在步骤S4010中，位置/姿势计算单元120计算每个单独指标Q_kn的图像坐标的推定值u^Qkn*。基于由s定义的指标的观测方程，即从每个指标Q_kn在物体坐标系上的坐标X_O ^Qkn(作为已知信息预先保持)计算图像坐标的函数来计算u^Qkn*：

$U^{{Q_{k_n}}^{*}}=F_C(x_O^{Q_{k_n}},s)---\left(4\right)$

更具体来说，由用于从X_O ^Qkn和s计算关注指标的照相机坐标(将摄像装置130的视点位置定义为原点，将三个正交轴分别定义为X轴、Y轴Z轴的坐标系)X_C ^Qkn的公式：

$x_C^{Q_{k_n}}=\left[\begin{array}{c}{x}_C^{Q_{k_n}}\\ y_C^{Q_{k_n}}\\ z_C^{Q_{k_n}}\end{array}\right]={R\left({\mathrm{\ω}}_{\mathrm{WC}}\right)}^{-1}\·(R_{\mathrm{WO}}\·x_O^{Q_{k_n}}+t_{\mathrm{WO}}-t_{\mathrm{WC}})---\left(5\right)$

以及用于从照相机坐标X_C ^Qkn计算图像坐标u^Qkn*的公式：

$u^{{Q_{k_n}}^{*}}=[u_x^{{Q_{k_n}}^{*}}{u}_y^{{Q_{k_n}}^{*}}{]}^T=[-f_x^C\frac{x_C^{Q_{k_n}}}{z_C^{Q_{k_n}}}-f_y^C\frac{y_C^{Q_{k_n}}}{z_C^{Q_{k_n}}}{]}^T---\left(6\right)$

来构成观测方程Fc()。

其中，t_WC和t_WO是表示摄像装置130和要测量的物体170的位置测量值(平移成分)的3D矢量，并且在此情况中作为固定值来处理t_WC和t_WO。R_WO是表示要测量的物体170的姿势测量值(旋转成分)的3×3旋转矩阵，并同样作为固定值来处理R_WO。f^C _x和f^C _y是摄像装置130在x轴和y轴方向上的焦距，作为已知值预先保持f^C _x和f^C _y。

在步骤S4020中，位置/姿势计算单元120通过下式计算每个独立指标Q_kn的图像坐标的推定值u^Qkn*和实际测量值u^Qkn之间的误差Δu^Qkn：

$\mathrm{\Δ}{u}^{Q_{k_n}}=u^{Q_{k_n}}-u^{{Q_{k_n}}^{*}}---\left(7\right)$

在步骤S4030中，位置/姿势计算单元120关于每个独立指标Q_kn计算s的图像雅克比(Jacobian)(即，用s的各元素对公式(4)给出的观测方程Fc()进行偏微分而获得的解作为各元素的2行×3列的雅克比矩阵)J_us ^Qkn(＝u/s)。更具体来说，单元120计算用照相机坐标x_C ^Qkn的各元素对公式(6)的右侧进行偏微分而获得的解作为各元素的2行×3列的雅克比矩阵J_ux ^Qkn(＝u/x)以及用矢量s的各元素对公式(5)的右侧进行偏微分而获得的解作为各元素的3行×3列的雅克比矩阵J_xs ^Qkn(＝x/s)，然后用下式计算J_us ^Qkn：

$J_{\mathrm{us}}^{Q_{k_n}}=J_{\mathrm{ux}}^{Q_{k_n}}\·J_{\mathrm{xs}}^{Q_{k_n}}---\left(8\right)$

在步S4040中，位置/姿势计算单元120基于在步骤S4020和S4030中计算出的误差Δu^Qkn和图像雅克比J_us ^Qkn来计算s的校正值Δs。更具体来说，位置/姿势计算单元120生成通过纵向排列误差Δu^Qkn来定义的2N维误差矢量：

$U=\left[\begin{array}{c}\mathrm{\Δ}{u}^{Q_{k_1}}\\ \·\\ \·\\ \·\\ \mathrm{\Δ}{u}^{Q_{k_N}}\end{array}\right]---\left(9\right)$

以及通过纵向排列图像雅克比J_us ^Qkn来定义的2N行×3列的矩阵：

$\mathrm{\Θ}=\left[\begin{array}{c}{J}_{\mathrm{us}}^{Q_{k_1}}\\ \·\\ \·\\ \·\\ J_{\mathrm{us}}^{Q_{k_N}}\end{array}\right]---\left(10\right)$

然后，单元120使用Θ的伪逆矩阵Θ⁺如下计算Δs：

Δs＝Θ⁺U                        (11)

这样，本实施例通过局部线性近似来计算校正值Δs。

在步骤S4050，位置/姿势计算单元120根据公式(12)使用在步骤S4040中计算出的校正值Δs来校正s，并将所获得的值设置为s的新推定值：

S+ΔS→S                                (12)

在步骤S4060中，位置/姿势计算单元120使用例如误差矢量U是否小于预定阈值或校正值Δs是否小于预定阈值的某些标准来检查该计算是否收敛。如果该计算不收敛，则单元120使用校正后的s再次执行步骤S4010及随后步骤中的处理。如果判定计算收敛，则处理前进到步骤S4070。

上述步骤即步骤S4000至S4060用牛顿(Newton)方法解决了包括多个观测方程的针对s的非线性联立方程。如下面在变形例5中所述，该一系列步骤可以使用任何其它非线性解法。

在步骤S4070中，位置/姿势计算单元120将通过直到步骤S4060的处理所获得的s设置为摄像装置130的姿势的推定值ω_WC(校正后)。

如上所述，可以在作为传感器测量值固定所获得的位置参数的同时计算使所有指标的误差的总和最小化的姿势参数。

下面使用图5中的流程图说明步骤S3070中的摄像装置130的位置和姿势计算处理步骤。在以下说明中，由六值矢量s＝[t_WC ^Tω_WC ^T]^T＝[x_WC y_WC z_WCξ_WC ψ_WCζ_WC]^T来内部地表示作为要计算的未知参数的摄像装置130的位置和姿势。

在步骤S5000中，位置/姿势计算单元120将作为传感器测量值获得的摄像装置130的位置和姿势设置为s的初始值。

在步骤S5010中，位置/姿势计算单元120计算每个单独指标Q_kn的图像坐标的推定值u^Qkn*。基于由s定义的指标的观测方程，即从每个指标Q_kn在物体坐标系上的坐标X_O ^Qkn计算图像坐标的函数，来计算u^Qkn*：

$u^{{Q_{k_n}}^{*}}=F_C^{\′}(x_O^{Q_{k_n}},s)---\left(13\right)$

更具体来说，由用于从X_O ^Qkn和s计算关注指标的照相机坐标X_C ^Qkn的公式(5)和用于从照相机坐标X_C ^Qkn计算图像坐标u^Qkn*的公式(6)构成观测方程F′c()。然而，在此情况下，公式(5)中的t_WC(表示摄像装置130的位置)不是作为固定值而是作为形成s的部分参数来处理的。另一方面，对于要测量的物体170的位置t_WO和姿势R_WO，将传感器测量值作为固定值原样使用。

在步骤S5020中，位置/姿势计算单元120通过公式(7)针对每个单独的指标Q_kn计算图像坐标的推定值u^Qkn*和实际测量值u^Qkn之间的误差Δu^Qkn。

在步骤S5030，位置/姿势计算单元120关于每个独立指标Q_kn计算s的图像雅克比(即，将用s的元素对公式(13)给出的观测方程F′c()进行偏微分而获得的解作为各元素的2行×6列的雅克比矩阵)J_ux ^Qkn(＝u/s)。更具体来说，单元120计算将用照相机坐标x_C ^Qkn的各元素对公式(6)的右侧进行偏微分而获得的解作为各元素的2行×3列的雅克比矩阵J_ux ^Qkn(＝u/x)、以及将用矢量s的各元素对公式(5)的右侧进行偏微分而获得的解作为各元素的3行×6列的雅克比矩阵J_xs ^Qkn(＝x/s)，然后用公式(8)计算J_us ^Qkn。

在步骤S5040中，位置/姿势计算单元120基于在步骤S5020和S5030中计算出的误差Δu^Qkn和图像雅克比J_us ^Qkn来计算s的校正值Δs。更具体来说，位置/姿势计算单元120生成通过纵向排列误差Δu^Qkn来定义的2N维误差矢量U以及通过纵向排列图像雅克比J_us ^Qkn来定义的2N行×6列的矩阵Θ，然后通过公式(11)使用Θ的伪逆矩阵Θ⁺计算Δs。

在步骤S5050，位置/姿势计算单元120根据公式(12)使用在步骤S5040中计算出的校正值Δs来校正s，并将所获得的值设置为s的新推定值。

在步骤S5060中，位置/姿势计算单元120使用例如误差矢量U是否小于预定阈值或校正值Δs是否小于预定阈值等一些标准来检查该计算是否收敛。如果该计算不收敛，则单元120使用校正后的s再次执行步骤S5010及随后步骤中的处理。如果判定计算收敛，则处理前进到步骤S5070。

在步骤S5070中，位置/姿势计算单元120将通过直到步骤S5060的处理获得的s设置为摄像装置130的位置和姿势校正后的推定值。

在本实施例中，在步骤S3070的处理中，选择摄像装置130的位置和姿势作为校正对象。作为选择，在执行该处理时，可以选择要测量的物体170的位置和姿势作为校正对象。在此情况下，由六值矢量s＝[t_WO ^Tω_WO ^T]^T＝[x_WO y_WO z_WOξ_WO ψ_WOζ_WO]^T来内部地表示要计算的未知参数，并将作为传感器测量值得到的要测量的物体170的位置和姿势设置为s的初始值。形成观测方程F′c()的公式(5)修改为：

$x_C^{Q_{k_n}}={R_{\mathrm{WC}}}^{-1}\·(R\left({\mathrm{\ω}}_{\mathrm{WO}}\right)\·x_O^{Q_{k_n}}+t_{\mathrm{WO}}{-t}_{\mathrm{WC}})---\left(14\right)$

此时，关于摄像装置130的位置t_WC和姿势R_WC，原样使用传感器测量值作为固定值。

要测量的物体170在照相机坐标系上的位置和姿势或者摄像装置130在物体坐标系上的位置和姿势可以作为未知参数来计算。

在前一种情况下，由表示要测量的物体170在照相机坐标系上的位置和姿势的六值矢量s＝[t_CO ^Tω_CO ^T]^T＝[x_CO y_CO z_COξ_COψ_COζ_CO]^T来内部表示要计算的未知参数。位置/姿势计算单元120通过下式基于传感器测量值(M^# _WC和M^# _WO)计算要测量的物体170在照相机坐标系上的位置和姿势(M^# _CO)：

${M^{\#}}_{\mathrm{CO}}={{M^{\#}}_{\mathrm{WC}}}^{-1}\·{M^{\#}}_{\mathrm{WO}}---\left(15\right)$

单元120将其设置为s的初始值。此外，形成观测方程F′c()的公式(5)被修改为：

$x_C^{Q_{k_n}}=R\left({\mathrm{\ω}}_{\mathrm{CO}}\right)\·x_O^{Q_{k_n}}+t_{\mathrm{CO}}---\left(16\right)$

另一方面，在后一种情况下，由表示摄像装置130在物体坐标系上的位置和姿势的六值矢量s＝[t_OC ^Tω_OC ^T]^T＝[x_OC y_OC z_OCξ_OC ψ_OCζ_OC]^T来内部表示要计算的未知参数。位置/姿势计算单元120通过下式基于传感器测量值(M^# _WC和M^# _WO)计算摄像装置130在物体坐标系上的位置和姿势(M^# _CO)：

${M^{\#}}_{\mathrm{OC}}={{M^{\#}}_{\mathrm{WO}}}^{-1}\·{M^{\#}}_{\mathrm{WC}}---\left(17\right)$

单元120将其设置为s的初始值。此外，将形成观测方程F′c()的公式(5)修改为：

$x_C^{Q_{k_n}}=R{\left({\mathrm{\ω}}_{\mathrm{OC}}\right)}^{-1}\·(x_O^{Q_{k_n}}-t_{\mathrm{OC}})---\left(18\right)$

这样，使用要测量的物体170和摄像装置130之间的相对位置和姿势的关系作为未知参数可以校正传感器测得的位置和姿势的误差。

由于传统的位置/姿势测量设备不考虑指标的分布，所以当指标不均匀地分布在图像上的局部区域中并且在该区域中感测到该指标时，解变得不稳定。另一方面，根据本实施例的位置/姿势测量设备，由于在步骤S3033和步骤S3040、S3050以及S3060中考虑到指标的分布范围的大小来选择方法，所以即使当例如检测到大量指标时，如果它们不均匀地分布在图像上的小区域中时，可以判定这些指标不足以稳定地计算6自由度的位置和姿势。因此，即使当指标不均匀地分布在图像上的局部区域中并且在该区域中观测到该指标时，选择适当的位置/姿势推定方法，以降低获得不稳定的解的情况的可能性，并且与传统方法相比可以获得稳定的解。

根据本实施例的位置/姿势测量设备，当只选择形成摄像装置的位置和姿势的部分参数来校正传感器测量值时，可以在这种约束下进行将指标的误差总和最小化的校正。因此，即使当通过减少未知参数的数量来稳定地测量摄像装置的位置和姿势时，也可以实现具有比传统方法更高准确度的测量。

第二实施例

根据本实施例的位置/姿势测量设备测量任意测量对象物体在房间等中定义的基准坐标系上的位置和姿势。

根据本实施例的位置/姿势测量设备与第一实施例中的位置/姿势测量设备的不同之处在于，在摄像设备上没有安装位置/姿势传感器，使用三角架等以已知的位置和姿势固定摄像装置。下面只说明与第一实施例的不同之处。

图12是示出本实施例的位置/姿势测量设备的配置的框图。如图12所示，本实施例的位置/姿势测量设备1200包括图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1220，位置/姿势测量设备1200连接到摄像装置130和位置/姿势传感器140。

通过三角架1280将摄像装置130以已知的位置和姿势固定到空间中。将从摄像装置130输出的拍摄图像输入到位置/姿势测量设备1200的图像输入单元160中。假定摄像装置130在基准坐标系上的位置和姿势作为已知值预先保持。

传感器测量值输入单元150从位置/姿势传感器140接收要测量的物体170的位置和姿势的测量值，并将其输出到位置/姿势计算单元1220。

位置/姿势计算单元1220接收作为传感器测量值输入单元150的输出的要测量的物体170的位置和姿势的测量值、以及作为指标检测单元110的输出的各指标Q_kn的图像坐标u^Qkn，基于输入信息校正要测量的物体170的位置和姿势的测量值的误差，并输出校正后的位置和姿势数据。

注意，图12中所示的图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1220中的至少一部分可以作为独立装置来实现，也可以通过将用于实现所述功能的软件程序安装在一个或多个计算机中并由每个计算机的CPU来执行它们，从而作为软件程序来实现。在本实施例中，假定各个单元(图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1220)由软件来实现并安装在单个计算机中。通过执行软件来实现各个单元的功能的计算机的基本配置与第一实施例中相同，避免其重复说明。

本实施例的位置/姿势计算单元1220的处理序列基本上与第一实施例中的位置/姿势计算单元120的处理序列(对应于图3中的流程图)相同。唯一的不同之处在于摄像装置130的位置和姿势不是作为测量值给出的，而是作为已知值预先保持的。位置/姿势计算单元1220将摄像装置130的位置和姿势的已知值设置为摄像装置130的位置和姿势的临时测量值，以通过与第一实施例中的步骤S3000～S3070相同的步骤来校正摄像装置130的位置和姿势的测量值。然后单元1220按第一实施例中的步骤S3080(公式(1))校正要测量的物体170的位置和姿势的测量值。在本实施例中，位置/姿势计算单元1220在步骤S3080中仅输出要测量的物体170的位置和姿势。

利用上述处理，测量测量对象物体的位置和姿势。如上所述，同样根据本实施例的位置/姿势测量设备，由于考虑到指标的分布范围的大小来选择方法，所以即使当指标不均匀地分布在图像上的局部区域中并且在该区域中观测到该指标时，也可以选择适当的位置/姿势推定方法，并且与传统方法相比较可以获得稳定的解。

注意，即使当将安装到测量对象物体上的传感器改变为姿势传感器时，也可以实现由固定在已知位置处的摄像装置测量任意测量对象物体的位置和姿势的方法。例如，当检测到的指标的分布小于阈值时，作为固定值使用姿势传感器的姿势测量值，并且只从指标的信息来推定位置。另一方面，当指标的分布等于或大于阈值时，从指标的信息来推定位置和姿势的六个参数。在此情况下，摄像装置的数量不限于一个，可以使用多个摄像装置。

第三实施例

根据本实施例的位置/姿势测量设备测量摄像装置在房间等的空间上的位置和姿势。下面说明根据本实施例的位置/姿势测量设备。

在本实施例中，不存在摄像装置以外的测量对象物体，并且与第一实施例中不同，根据本实施例的位置/姿势测量设备只测量摄像装置的位置和姿势。下面只说明与第一实施例的不同之处。

图13是示出本实施例的位置/姿势测量设备的配置的框图。如图13中所示，本实施例的位置/姿势测量设备1300包括图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1320，位置/姿势测量设备1300连接到摄像装置130和位置/姿势传感器140。

在现实空间上的多个位置处布置在基准坐标系上的位置已知的多个指标Q_k作为要由摄像装置130拍摄的指标。指标Q_k可以采用任何形式，只要可以检测它们的投影图像在拍摄图像上的图像坐标、并且每个指标可以由任意方法来识别即可。

传感器测量值输入单元150从位置/姿势传感器140接收摄像装置130的位置和姿势的测量值，并将其输出到位置/姿势计算单元1320。

位置/姿势计算单元1320接收作为传感器测量值输入单元150的输出的摄像装置130的位置和姿势的测量值、以及作为指标检测单元110的输出的各指标Q_kn的图像坐标u^Qkn，校正摄像装置130的位置和姿势的测量值的误差，并输出校正后的位置和姿势数据。

注意，图13中所示的图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1320中的至少一部分可以作为独立装置来实现，也可以通过将用于实现所述功能的软件程序安装在一个或多个计算机中并由每个计算机的CPU执行它们，从而作为软件程序来实现。在本实施例中，假定各个单元(图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1320)由软件来实现并安装在单个计算机中。通过执行软件来实现各个单元的功能的计算机的基本配置与第一实施例中相同，不给出其重复说明。

由于本实施例中的位置/姿势计算单元1320的处理序列与第一实施例中的位置/姿势计算单元120的处理序列(对应于图3中的流程图)类似，所以以下参考图3只说明与第一实施例的不同之处。

在步骤S3005中，位置/姿势计算单元1320从传感器测量值输入单元150接收由传感器测得的摄像装置130的位置和姿势。

在步骤S3025中，位置/姿势计算单元1320对在步骤S3005中获得的摄像装置130的姿势的测量值的平转和倾斜角度施加校正，以消除检测到的指标的误差(指标在基准坐标系上的3D坐标x_W ^Qkn、从摄像装置130的位置和姿势的测量值取得的投影坐标的理论值u^Qkn*以及实际检测的坐标u^Qkn之间的误差)。然后处理前进到步骤S3080。

由于可以使用已知的处理作为该校正处理，所以将不给出进一步的说明。更具体来说，可以使用例如专利文献1中公开的方法(使用一个标志通过转动照相机的校正方法(“0019”段至“0029”段)以及使用多个标志通过转动照相机的校正方法(“0039”段至“0050”段))。

在步骤S3035中，位置/姿势计算单元1320对表示摄像装置130的姿势的测量值的三个参数(平转、倾斜和旋转角)进行校正，以将所有检测到的指标的误差的总和最小化，同时将在步骤S3005中获得的摄像装置130的位置测量值设定为固定值。然后处理前进到步骤S3080。该校正处理步骤与第一实施例中的步骤S3035(对应于图4的流程图)中的处理相同。然而，在本实施例的情况下，指标的观测方程F″c()表示用于从指标Q_kn在基准坐标系上的坐标x_W ^Qkn(预先作为已知信息保持)计算图像坐标的函数。即，观测方程F″c()为：

$u^{{Q_{k_n}}^{*}}=F_C^{\′\′}(x_W^{Q_{k_n}},s)---\left(19\right)$

更具体来说，由用于从x_W ^Qkn和s计算关注指标的照相机坐标x_C ^Qkn、由下式给出的公式：

$x_C^{Q_{k_n}}=R{\left({\mathrm{\ω}}_{\mathrm{WC}}\right)}^{-1}\·(x_W^{Q_{k_n}}-t_{\mathrm{WC}})---\left(20\right)$

和用于从照相机坐标x_C ^Qkn计算图像坐标u^Qkn*的公式(6)来配置观测方程F″c()。应当注意，t_wc是代表摄像装置130的位置测量值3D矢量，在该公式中作为固定值来处理t_wc。

在步骤S3070中，位置/姿势计算单元1320计算摄像装置130的位置和姿势以将所有检测到的指标的误差的总和最小化。由于例如可以使用专利文献2中公开的现有技术来来实现该处理步骤的详细处理，所以将不给出其说明。

在步骤S3080中，位置/姿势计算单元1320经由I/F 1009向外部输出作为直到S3070的结果获得的摄像装置130的位置和姿势。作为选择，单元1320通过其它应用程序将这些数据转换为可使用的状态之后，将它们存储在RAM 1002中。

利用上述处理，测量了摄像装置的位置和姿势。如上所述，同样根据本实施例的位置/姿势测量设备，由于考虑到指标的分布范围的大小来选择方法，所以即使当指标被不均匀被分布并且在图像上的局部区域中被观测到时，也可以选择适当的微行子/姿势推定方法，并且与传统方法相比较可以获得稳定的解。

第四实施例

根据本实施例的位置/姿势测量设备测量任意测量对象物体在房间等空间上的位置和姿势。下面说明根据本实施例的位置/姿势测量设备。在本实施例中，与第三实施例中不同，代替摄像装置测量任意测量对象物体的位置和姿势。下面只说明与第三实施例的不同之处。

图1 4是示出根据本实施例的位置/姿势测量设备的配置的框图。如图14中所示，本实施例的位置/姿势测量设备1400包括图像输入单元160、指标检测单元110、传感器测量值输入单元150、摄像单元1430、位置/姿势测量单元1440、位置/姿势计算单元1420，并且连接到要测量的物体1470。

摄像单元1430被固定并安装到作为要测量其位置和姿势的物体的要测量的物体1470上，并拍摄包括指标的现实空间的视频。

位置/姿势测量单元1440被固定并安装到摄像单元1430上，测量摄像单元1430在基准坐标系上的位置和姿势，并将其输出到传感器测量值输入单元150。位置/姿势测量单元1440包括例如可以从美国Polhemus得到的FASTRAK等。

图像输入单元160将摄像单元1430拍摄的拍摄图像转换为数字数据，并将该数字数据输出到指标检测单元110。

传感器测量值输入单元150从位置/姿势测量单元1440接收摄像单元1430的位置和姿势的测量值，并将其输出到位置/姿势计算单元1420。

位置/姿势计算单元1420接收作为传感器测量值输入单元150的输出的摄像单元1430的位置和姿势的测量值、和作为指标检测单元110的输出的各指标Q_kn的图像坐标u^Qkn，并校正摄像单元1430的位置和姿势的测量值的误差。

位置/姿势计算单元1420还根据所获得的摄像单元1430的位置和姿势、关于摄像单元1430和要测量的物体1470之间的相对位置/姿势关系的信息(更具体来说，表示摄像单元1430在要测量的物体1470上设置的位置的信息；该信息由摄像单元1430在由要测量的物体1470定义的物体坐标系上的位置和姿势来表示)来计算要测量的物体1470在基准坐标系上的位置和姿势。然后，单元1420经由I/F 1009向外部输出计算后的数据。作为选择，单元1420通过其它应用程序将该数据转换为可以使用的状态之后将其存储在RAM 1002中。注意，该转换处理不是总是需要的，可以原样输出摄像单元1430的位置和姿势。

在本实施例中，位置/姿势测量单元1440测量摄像单元1430的位置和姿势。作为选择，可以配置位置/姿势测量单元1440以测量要测量的物体1470的位置和姿势。在此情况下，位置/姿势计算单元1420使用作为已知值保持的摄像单元1430和要测量的物体1470之间的相对位置/姿势关系，根据作为测量值获得的要测量的物体1470的位置和姿势来计算摄像单元1430的位置和姿势，并使用该值作为初始值执行与上述相同的处理。

位置/姿势计算单元1420校正摄像单元1430的姿势或位置和姿势，然后在步骤S3025、S3035、以及S3070中的任何一个中计算要测量的物体1470的位置和姿势。然而，可以配置位置/姿势计算单元1420以在步骤S3035和S3070中直接计算要测量的物体1470的位置和姿势。

当位置/姿势测量单元1440测量摄像单元1430的位置和姿势时，位置/姿势计算单元1420使用作为已知值预先保持的摄像单元1430和要测量的物体1470之间的相对位置/姿势关系，根据作为测量值获得的摄像单元1430的位置和姿势来计算要测量的物体1470的位置和姿势，并使用它们作为初始值。

在步骤S3035中，位置/姿势计算单元1420作为未知参数设置表示要测量的物体1470的姿势的三值矢量s＝ω_WO＝[ξ_WOψ_WOζ_WO]^T。在步骤S3070中，位置/姿势计算单元1420作为未知参数设置表示要测量的物体1470的位置和姿势的六值矢量s＝[t_WO ^Tω_WO ^T]^T＝[x_WO y_WO z_WOξ_WOψ_WOζ_WO]^T。此外，单元1420将定义观测方程F″c()所需的公式(20)修改为：

$x_C^{Q_{k_n}}={R_{\mathrm{OC}}}^{-1}\·({R\left({\mathrm{\ω}}_{\mathrm{WO}}\right)}^{-1}\·(x_W^{Q_{k_n}}-t_{\mathrm{WO}})-t_{\mathrm{OC}})---\left(21\right)$

其中，t_OC和R_OC是表示摄像装置1430在由要测量的物体170定义的物体坐标系上的姿势的3×3旋转矩阵和表示姿势的3D矢量，并且作为表示摄像装置1430和要测量的物体1470之间的相对位置/姿势关系的已知值预先保持。注意，在步骤S3035中将t_WO设置为固定值。

利用上述处理，测量了任意物体的位置和姿势。如上所述，同样根据本实施例的位置/姿势测量设备，由于考虑到指标分布范围的大小来选择方法，所以即使当指标被不均匀地分布并且在图像上的局部区域中被观测到，与传统方法相比，也可以获得稳定的解。

第五实施例

根据本实施例的位置/姿势测量设备测量摄像装置在房间等空间上的位置和姿势。下面说明根据本实施例的位置/姿势测量设备。

图15是示出本实施例的位置/姿势测量设备的配置的框图。如图15中所示，本实施例的位置/姿势测量设备1500包括包括图像输入单元160、指标检测单元110、传感器测量值输入单元1550、以及位置/姿势计算单元1520，并连接到摄像装置130和姿势传感器1540。

在本实施例中，与第三实施例中不同，姿势传感器代替位置/姿势传感器安装到摄像装置130。使用姿势传感器可以避免使用位置/姿势时施加的约束以及传感器测量范围以外的区域不能被测量的约束。下面只说明与第三实施例的不同之处。

姿势传感器1540安装到摄像装置130，测量姿势传感器1540本身的当前姿势，并将其输出到传感器测量值输入单元1550。姿势传感器1540是基于例如陀螺仪传感器和加速传感器的传感器单元，包括从TOKIMEC可以获得的TISS-5-40、从美国InterSense可以获得的InertiaCube2等。由于这些传感器中每一个所测量的姿势测量值是作为对惯性测量值进行时间积分而获得的值的测量值，所以其具有不同于真实姿势的误差。然而，由于这些姿势传感器通过内部加速传感器观测地球重力的方向，所以它们具有消除倾斜角方向(螺距角和横摇角)中漂移误差累积的功能。因此，这些传感器具有在倾斜角度方向上不出现漂移误差的本性。换句话说，这些传感器具有随着时间的推移在方位方向(偏航角方向)上累积的漂移误差。

传感器测量值输入单元1550从位置/姿势计算单元1520接收方位漂移误差校正值的更新值，并更新和保持姿势传感器1540的当前方位漂移误差校正值。此外，单元1550从姿势传感器1540接收姿势测量值，通过当前的方位漂移误差校正值来校正该姿势测量值，并将校正后的值输出到位置/姿势计算单元1520作为摄像装置130的姿势的预测值。

位置/姿势计算单元1520作为输入值接收作为传感器测量值输入单元1550的输出的摄像装置130的姿势的预测值、作为指标检测单元110的输出的各指标Q_kn的图像坐标u^Qkn，并计算和输出摄像装置130的位置和姿势。此外，单元1520将位置和姿势计算步骤中得出的姿势传感器1540的方位漂移误差校正值的更新值输出到传感器测量值输入单元1550。

注意，图15中所示的图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1520中的至少一些可以作为独立装置来实施，也可以作为通过将软件程序安装在一个或多个计算机中并由每个计算机的CPU执行它们来实现上述功能的软件程序来实施。在本实施例中，假定各单元(图像输入单元160、指标检测单元110、传感器测量值输入单元150、以及位置/姿势计算单元1520)由软件来实现并安装在单个计算机中。通过执行软件来实现各单元的功能的计算机的基本配置与第一实施例中的相同，并取消其重复说明。

图16是示出用于计算表示摄像装置130的位置和姿势的参数的处理的流程图。当CPU 1001执行位置/姿势计算单元1520的软件程序时执行该处理。

在步骤S15000中，位置/姿势计算单元1520从指标提取单元110接收检测到的指标Q_kn的图像坐标u^Qkn和它们的标识符k_n。假定各指标在基准坐标系上的3D坐标x^Qkn作为已知值预先加载到RAM 1002上。

在步骤S15005中，位置/姿势计算单元1520从传感器测量值输入单元1550接收摄像装置130的姿势的预测值R^*，并将其设置为摄像装置130的姿势的初始值。在第二或随后的处理中，单元1520将紧前一循环的处理中计算出的摄像装置130的位置设置为摄像装置130的姿势的初始值。

在步骤S15010中，位置/姿势计算单元1520检查是否检测到指标。如果没有检测到指标，择处理跳至步骤S15090；否则，处理前进至步骤S15020。

在步骤S15020中，位置/姿势计算单元1520检查检测到的指标的总数是否为一个。如果指标的总数是一个，则处理前进至步骤S15025；否则，处理前进至步骤S15030。

在步骤S15025中，位置/姿势计算单元1520对作为校正对象的垂直于步骤S15005中设置的初始值的平移成分的视轴的两个轴方向上的值进行校正以消除检测到的指标上的误差。然后处理前进到步骤S15090。由于可以提供已知的处理作为步骤S15025中的处理步骤，所以不给出进一步的说明。更具体来说，可以使用例如专利文献1中公开的方法(使用一个标志借助于照相机的平移的校正方法(“0030”段至“0036”段)以及使用多个标志借助于照相机的平移的校正方法(“0051”段至“0056”段))。注意，专利文献1说明了这样的情况：六自由度位置/姿势传感器的测量值被用作要进行误差校正的对象。然而，在本实施例中，与专利文献1中不同，在上述构架中计算的位置与姿势传感器的测量值的组合被用作校正对象。

在步骤S15030中，位置/姿势计算单元1520检查检测到的指标的总数是否为两个。如果指标的总数是两个，则处理前进到步骤S15032；否则，处理前进到步骤S15040。

在步骤S15032中，位置/姿势计算单元1520计算图像上两个检测到的指标之间的距离，并比较计算后的值与作为预定值定义的阈值T₁(例如，图像对角线长度的1/8)。如果该距离等于或大于阈值T₁，则处理前进到步骤S15034；否则，处理前进到步骤S15025。

在步骤S15034中，位置/姿势计算单元1520比较图像上两个检测到的指标之间的距离与作为预定值定义的阈值T₂(例如，图像对角线长度的1/4)。如果该距离等于或大于阈值T₂，则处理前进到步骤S15038；否则，处理前进到步骤S15036。

在步骤S15036中，位置/姿势计算单元1520只对位置的参数进行使检测到的指标的误差最小化的校正，使代表步骤S15005中设置的初始值的位置的三个参数(平移成分)作为更新目标(即，在姿势测量值不包括任何误差的假定下)。然后，处理前进到步骤S15090。由于该处理步骤的细节在例如专利文献3(第六实施例)中进行了说明，因此取消其重复说明。作为选择，可以配置包括作为未知变量的、表示位置的三个参数的观测方程，并且使用图像雅克比进行误差最小化计算，这样也可以在将图像上的误差最小化的同时校正位置。

在步骤S15038中，位置/姿势计算单元1520处理作为要计算的未知参数的、表示步骤S15005中设置的初始值的位置的三个参数(平移成分)和总数传感器1340的方位漂移误差校正值的更新值φ的总共四个参数，并使用检测到的指标的信息计算它们。由于该处理步骤的细节在例如专利文献3(第七实施例)中进行了说明，因此取消其重复说明。作为选择，可以配置包括作为未知变量的、包括这四个参数的观测方程，并且使用图像雅克比进行误差最小化计算，这样也可以在将图像上的误差最小化的同时计算位置和方位漂移误差校正值的更新值。然后，处理前进到步骤S15080。

仅当检测到的指标的总数为三个或更多时才执行步骤S15040。在该步骤中，作为与所有检测到的指标的分布范围相关的评价值的例子，位置/姿势计算单元1520计算包括所有检测到的指标的图像坐标的凸包。

在步骤S15050中，位置/姿势计算单元1520比较在步骤S15040中计算出的凸包的面积与作为预定值定义的阈值T₃(例如，整个拍摄图像的面积的1/16)。如果该面积等于或大于阈值T₃，则处理前进到步骤S15060；否则，处理前进到步骤S15036以执行上述处理。

在步骤S15060中，位置/姿势计算单元1520在步骤S15040中计算出的凸包的面积与作为预定值定义的阈值T₄。如果该面积等于或大于阈值T₃(例如，整个拍摄图像的面积的1/9)，则处理前进到步骤S15070；否则，处理前进到步骤S15038以执行上述处理。

在步骤S15070中，位置/姿势计算单元1520计算摄像装置130的位置和姿势以将所有检测到的指标的误差的总和最小化。通过计算参数来执行该计算，该计算是使用在步骤S15005中设置的初始值通过非线性方程的迭代解法使误差最小化，表示摄像装置130的位置和姿势的六个参数作为变量。即，配置包括作为未知变量的、表示位置和姿势的六个参数的观测方程，并使用基于该方程算出的指标投影位置的误差以及与该参数相关的图像雅克比来计算每个参数的校正值，由此通过重复校正处理来计算使图像上的误差最小化的位置和姿势。

位置/姿势计算单元1520进一步计算在步骤S15005中设置的姿势的初始值与计算后的姿势之间的变化，并将其方位成分设置为方位漂移误差校正值的更新值φ。

在步骤S15080中，位置/姿势计算单元1520将在步骤S15038或S15070中获得的方位漂移误差校正值的更新值φ输出到传感器测量值输入单元1550。

在步骤S15090，位置/姿势计算单元1520经由I/F 1009向外部输出作为直到步骤S15080的结果获得的摄像装置130的位置和姿势。作为选择，单元1520通过其它应用程序将这些数据转换成可使用的状态后将这些数据存储在RAM 1002上。

在步骤S15100中，位置/姿势计算单元1520检查该处理是否结束。如果该处理没有结束，则处理返回到步骤S15000。

利用上述处理，可以测量摄像装置的位置和姿势。

根据本实施例的位置/姿势测量设备，由于在步骤S15034和S15050中考虑指标分布范围的大小(即，检查是否更新方位漂移误差校正值)来选择方法，所以当指标被不均匀分布并且在图像上的局部区域中被观测到时，可以消除不准确地更新方位漂移误差校正值的情况。

根据本实施例的位置/姿势测量设备，除了第三实施例中的效果以外，通过在步骤S15060中选择方法，即使当图像信息不足时，通过姿势传感器信赖倾斜角度信息，从而可以稳定地得出位置和姿势。另外，当获得了足够的图像信息时，可以高精确度地推定位置和姿势。这样，可以同时实现基于通常图像信息的位置/姿势推定方法的优点。

第一～第五实施例的变形例

变形例1

在上述实施例的每一个中，基于指标在图像上的分布来选择要应用的校正算法。这些算法(第一实施例中的步骤S3025、S3035、S3070，第二至第四实施例中的这些步骤的修改、以及第五实施例中的步骤S15025、S15038、以及S15070)不局限于上述实施例中所公开的算法，可以由其它校正算法来替换这些算法，只要根据依照指标的分布假定的图像信息的信息大小来适当设置这些方法即可。

例如，在第一至第四实施例中的步骤S3025或S3035中，可以校正位置的测量值而不是姿势的测量值。在此情况下，在步骤S3025中，如专利文献1的“0030”段至“0036”段和“0051”段至“0056”段中所公开的，位置/姿势计算单元可以对基于使用一个或多个指标借助于照相机的平移的校正方法对垂直于位置测量值的照相机的视轴的两个轴方向上的值施加校正以消除检测到的指标上的误差。

在步骤S3035中，位置/姿势计算单元以与图5中相同的处理序列校正作为要计算的未知参数的、具有三值矢量s＝t_WC＝[x_WC y_WC z_WC]^T的位置。与第一实施例中不同，配置观测方程的公式(5)被修改为：

$x_C^{Q_{k_n}}={R_{\mathrm{WC}}}^{-1}\·(R_{\mathrm{WO}}\·x_O^{Q_{k_n}}+t_{\mathrm{WO}}-t_{\mathrm{WC}})---\left(22\right)$

其中，R_WC是作为固定值的由传感器测得的摄像装置130的姿势测量值。此外，R_WO和t_WO是作为固定值来处理的、如第一实施例中公式(5)的要测量的物体170的位置和姿势的传感器测量值。

由于公式(22)是线性方程，所以由与公式(6)组合的观测方程形成联立方程，并求解该联立方程，从而不使用迭代操作而获得校正值。

注意，可以允许用户自由选择以通过UI(未示出)向姿势或位置的校正赋予优先级。在此情况下，在执行步骤S3025和S3035时，根据用户的选择执行姿势或位置校正算法。在此情况下，即使当使用具有不同特征的位置/姿势传感器，也可以进行更优化的测量。

在步骤S3025中，位置/姿势计算单元可以从作为测量值获得的六个参数中选择任意两个作为校正目标，并且可以以与上述方法相同的方式使用这些参数作为未知数进行计算。在步骤S3035中，位置/姿势计算单元同样可以从作为测量值获得的六个参数中选择任意三个作为校正目标，并且可以通过以与上述方法中相同的构架使用这些参数作为未知数来行进计算。

变形例2

在第一至第四实施例中，基于对指标在图像上的分布的测量，处理分支到三个不同的算法(步骤S3033、S3050、S3060)。然而，基于指标分布的测量进行分支的数量不限于三个。例如，可以省略步骤S3050中的处理，并且基于阈值T₃与凸包面积之间的大小关系，处理可以分支到步骤S3035或S3070。作为选择，可以设置更多数量的阈值，并且处理可以分支到四个或更多个路线。例如，可以根据凸包的面积改变要校正的参数的数量(2至6个)，并且可以通过相同的方法将位置和姿势作为未知数进行计算。

变形例3

在上述实施例的每一个中，使用位置/姿势传感器或姿势传感器的测量值计算测量对象(测量对象物体或摄像装置)的位置或姿势，并通过摄像装置一起拍摄图像信息。然而，基于指标在图像上的分布选择用于计算位置和姿势的算法的基本技术思想也可以应用到仅根据图像信息而不使用这些传感器的测量值的计算测量对象的位置和姿势的情况。

例如，下面考虑这样的情况：位置/姿势测量设备1300不具有传感器测量值输入单元150，位置/姿势传感器140不安装到图13所示的第三实施例的配置中的摄像装置130上。在此情况下，假定位置/姿势计算单元1320使用在前一帧中计算出的位置和姿势(或者从前一计算值中预测的当前帧中的位置和姿势)的计算值作为初始值，而不是使用位置/姿势传感器140对摄像装置130的位置和姿势的测量值，它执行与图3中相同的处理，将这些值作为临时测量值，从而根据指标的分布选择位置和姿势的适当计算算法。

变形例4

在上述每一个实施例中，当检测到三个或更多个指标时，计算由这些指标定义的凸包，并将其面积作为用于评价指标分布的评价值。然而，评价指标分布时使用的测量不局限于此，还可以使用其它测量。例如，选择位于距离图像中心最远处的指标作为第一指标，选择位于距离第一指标最远处的指标作为第二指标，选择距离第一和第二指标的连线最远的指标作为第三指标。然后，使用由第一、第二、第三指标形成的三角形的面积代替凸包的面积作为分布的测量。更简单地，可以使用该第一和第二指标之间的距离作为分布的测量，还可以使用表示分布的任意信息，例如作为指标的图像坐标的方差、标准偏差等的数字值，作为分布的测量。

作为使用其它指标的图像坐标的方差的实际方法，可以考虑使用指标的图像坐标的协方差的方法。即，该方法计算检测到的指标的方差-协方差矩阵，并使用其特征值(该值对应于主要成分分析中的主要成分的方差)。在此情况下，使用第二特征值作为测量值，可以推定指标在图像上的展开。使用第一特征值作为测量值也可以推定图像的线性展开。

此外，可以使用凸包面积以外的特征量，例如凸包的周长等。还可以使用从所有检测到的指标的外接四边形、外接圆等获得的特征量。可以使用指标的图像坐标的矩(moments)的分析值(例如，惯性等效椭圆的长轴和短轴)等。当使用单个多边形指标时，可以原样使用检测到该指标时获得的指标区域的面积。

代替直接测量检测到的指标的分布，可以基于任意信息来推定指标分布的大小。例如，可以基于从前一帧获得的照相机或物体的位置和坐标的推定值(初始值)或来自传感器的信息来计算图像上要观测的指标的坐标以获得指标的计算分布。此外，可以基于检测到的指标的外观大小等来近似推定到指标的距离，并且可以基于这些距离来推定分布。

变形例5

如图4中的步骤S4040中所详细说明的，在第一实施例的步骤S3035中，由公式(11)说明的方法被用于基于误差矢量U和矩阵Θ来计算校正值Δs。此外，使用作为结果的Δs，通过用s+Δs代替s的牛顿方法来计算s。例如，然而，计算s的方法不局限于该牛顿方法。例如，可以使用作为已知的非线性方程的迭代解法的例如LM方法(Levenberg-Marquardt方法)来计算s，也可以结合作为已知的鲁棒评价方法的M评价等统计方法。因此，即使当应用任何其它数字计算方法时也不会削弱本发明的要旨。在上述实施例中通过将s定义为状态矢量并将公式(4)定义为观测方程，可以配置具有使用图像雅克比只校正位置/姿势传感器的测量值的部分参数的效果的扩展卡尔曼过滤器(ExtendedKalman Filter)或者迭代扩展卡尔曼过滤器。由于如非专利文献2中所述，扩展卡尔曼过滤器和迭代扩展卡尔曼过滤器对本领域的技术人员来说是已知的，所以不给出关于它们的细节的进一步说明。

变形例6

在上述每一个实施例中，使用了每个代表一个坐标位置的指标(在下文中称为点指标)。然而，可以使用点指标以外的指标。例如，可以使用在已知的位置/姿势测量设备(例如，参见非专利文献3)中所使用的由线特征配置的指标(在下文中称为线指标)。例如，当使用从原点到线指标的距离作为基于s的评价和推定值的标准，通过从图像的检测值计算出的误差配置误差矢量U，并且通过由利用s的元素对观测方程进行偏微分而获得的解作为元素的1行×6列的雅克比矩阵配置矩阵Θ时，可以如上述实施例中那样测量(校正)位置和姿势。通过累积从线指标、点指标和其它指标获得的误差以及图像雅克比，可以一起使用这些特征。

变形例7

在上述每个实施例中，根据指标关于图像面积的分布来选择随后的处理。然而，也可以使用与测量对象物体的相对比作为测量值。例如，在第一实施例中，在步骤S3040中，处理由所有检测到的指标形成的凸包的面积S_H以外，位置/姿势计算单元还计算要测量的物体(假定存储了代表物体170的大致形状的3D数据)在图像上的投影图像的面积S_O，并获得它们的比(S_H/S_O)。然后，可以使用针对该比适当设置的阈值来执行步骤S3050和S3060中的条件分支。

可以使用指标在现实空间上而不是在图像上的分布作为测量值。例如，在第一实施例中，在步骤S3040中，位置/姿势计算单元计算由所有检测到的指标在物体坐标系上的3D坐标形成的3D凸包，计算其体积V_H，并获得与要测量的物体170的体积V_O的比(V_H/V_O)。然后，可以使用针对该比适当设置的阈值来执行步骤S3050和S3060中的条件分支。此外，计算由在物体坐标系上相互距离最远的三个检测到的指标形成的三角形的面积，计算与要测量的物体170的大小的比(例如，由外接矩形平行六面体的两个最长的边形成的矩形的面积)，并且可以使用该比作为测量值。

第六实施例

在第一至第五实施例中，根据指标的分布选择用于计算位置和姿势的一些方法中的一个。本实施例通过不同于选择一种方法的方法考虑到指标的分布来实现位置和姿势的稳定测量。

根据本实施例的位置/姿势测量设备象第一实施例中那样测量摄像装置和测量对象物体的位置和姿势。下面说明根据本实施例的位置/姿势测量设备和位置/姿势测量方法。

除了位置/姿势计算单元120的内部操作以外，根据本实施例的位置/姿势测量设备的配置与第一实施例中的基本相同。因此，以下只说明本实施例的位置/姿势计算单元与第一实施例不同的操作。

图17是示出本实施例的位置/姿势计算单元1720的处理序列的流程图。在本实施例中，当CPU执行用于实现位置/姿势计算单元1720的功能的程序时执行以下序列。

在步骤S17000中，位置/姿势计算单元1720从指标提取单元110接收检测到的指标Q_kn的图像坐标u^Qkn和它们的标识符k_n。假定各指标在物体坐标系上的3D坐标x_O ^Qkn作为已知值被预先加载到RAM 1002上。

在步骤S17005中，位置/姿势计算单元1720从传感器测量值输入单元150接收摄像装置130和要测量的物体170的位置和姿势的传感器测量值。在以下说明中，摄像装置130的位置测量值由三值矢量t^# _WC表示，其位置测量值由3×3旋转矩阵R^# _WC表示。

在步骤S17010中，位置/姿势计算单元1720检查是否检测到指标。如果没有检测到指标(N＝0)，则处理跳至步骤S17120；否则(N≥1)，处理前进至步骤S17020。

在步骤S17020中，位置/姿势计算单元1720检查检测到的指标的总数是否为一个。如果指标的总数是一个，则处理前进至步骤S17025；否则，处理前进至步骤S17030。

在步骤S17025中，位置/姿势计算单元1720对摄像装置130的姿势测量值的两个参数(平转和倾斜角)施加校正以消除检测到的指标上的误差。然后处理跳至步骤S17120。由于步骤S17025中的校正处理与第一实施例中的步骤S305中的相同，所以不给出其详细说明。

在步骤S17030中，位置/姿势计算单元1720计算用于对摄像装置130的姿势测量值的三个参数(平转、倾斜和旋转角度)施加校正的校正矩阵ΔR_{WC_1}，从而消除检测到的指标上的误差。首先，单元1720通过与第一实施例中的步骤S3035相同的方法校正姿势的三个参数以获得摄像装置的校正后的姿势R_{WC_1}。然后，单元1720基于所获得的R_{WC_1}和传感器测得的姿势测量值R^# _WC通过下式计算ΔR_{WC_1}：

$\mathrm{\Δ}{R}_{\mathrm{WC}\_1}=R_{\mathrm{WC}\_1}\·{R_{\mathrm{WC}}^{\#}}^{-1}---\left(23\right)$

尽管在步骤S17030中没有校正位置，但是为了说明方便，用三值矢量Δt_{WC_1}描述位置的校正值。然后，我们有Δt_{WC_1}＝[000]^T。

在步骤S17040中，位置/姿势计算单元1720检查检测到的指标的总数N是否为2个。如果指标的总数N是2个，则处理前进至步骤S17043；否则，处理前进至步骤S17050。

在步骤S17043中，位置/姿势计算单元1720计算用于对摄像装置130的姿势测量值的两个参数(平转和倾斜角度)施加校正的校正矩阵ΔR_{WC_2}，从而消除检测到的指标上的误差。首先，单元1720通过与步骤S17025相同的方法校正姿势以获得摄像装置的校正后的姿势R_{WC_2}。然后，单元1720基于获得的R_{WC_2}和姿势测量值R^# _WC以与公式(23)相同的方式计算ΔR_{WC_2}。

在步骤S17047中，位置/姿势计算单元1720通过组合两个姿势校正值(校正矩阵ΔR_{WC_1}和ΔR_{WC_2})计算实际要施加的姿势校正值(校正矩阵ΔR_WC)。由于没有校正位置，所以位置的校正矩阵Δt_WC等于[000]^T。然后处理前进至步骤S17111。

下面说明步骤S17047中姿势校正值的组合处理的细节。位置/姿势计算单元1720通过下式根据两个检测到的指标之间的距离dist确定两个校正值的权重w₁：

$w_1=\left\{\begin{array}{cc}0& \mathrm{dist}\&le;{T_1}^{\min }\\ \frac{\mathrm{dist}-{T_1}^{\min }}{{T_1}^{\max }-{T_1}^{\min }}& {T_1}^{\min }<\mathrm{dist}<\\ 1& {T_1}^{\max }\&le;\mathrm{dist}\end{array}{T_1}^{\max }\right.---\left(24\right)$

其中，T₁ ^min和T₁ ^max是用于标准化指标之间的距离的参数，并且事先设置适当的值。图18示出由公式(24)获得的权重w₁和dist之间的关系。

接下来，位置/姿势计算单元1720获得校正矩阵ΔR_{WC_1}和ΔR_{WC_2}的姿势成分作为四元素表达式(由四值矢量ΔH_{WC_1}和ΔH_{WC_2}表示)，并通过下式计算它们的权重和ΔH_WC：

ΔH_WC＝(1-w₁)ΔH_{WC_2}+w₁ΔH_{WC_1}    (25)

位置/姿势计算单元1720将所获得的权重和ΔH_WC转换为ΔR_WC作为旋转矩阵表达式。由于旋转矩阵和作为四元素的权重和之间的相互转换方法是计算机图形学等领域中的基本点，所以将不给出其详细说明。

当权重w₁＝0时，公式(25)选择两个参数的校正，当权重w₁＝1时，公式(25)选择三个参数的校正。当权重w₁范围从0到1时，根据权重w₁的大小生成两个参数的校正和三个参数的校正之间的中间转换矩阵。

下面说明步骤S17040中N≠2的情况，即检测到三个或更多个指标。在步骤S17050中，位置/姿势计算单元1720计算包括所有检测到的指标的图像坐标的凸包。

在步骤S17060中，位置/姿势计算单元1720比较在步骤S17050中计算出的凸包的面积A和作为预定值定义的阈值T₄(例如，整个拍摄图像的面积的1/12)。如果面积A等于或大于阈值T₄，则处理前进至步骤S17090；否则，处理前进至步骤S17070。注意，阈值T₄可以采用任何值，只要它对于稍后说明的两个参数T₂ ^max和T₃ ^min满足T₂ ^max≤T₄≤T₃ ^min即可。

在步骤S17070中，如步骤S17043中那样，位置/姿势计算单元1720计算用于对摄像装置130的姿势测量值的平转和倾斜角施加校正的校正矩阵ΔR_{WC_2}。

在步骤S17080中，如步骤S17047中那样，位置/姿势计算单元1720通过组合两个校正矩阵ΔR_{WC_1}和ΔR_{WC_2}来计算校正矩阵ΔR_WC。然后处理前进至步骤S17110。注意，在步骤S17080中，单元1720通过下式根据步骤S17050中计算出的凸包的面积A而不是根据指标之间的距离dist来确定权重w₁：

$w_1=\left\{\begin{array}{cc}0& A\&le;{T_2}^{\min }\\ \frac{A-{T_2}^{\min }}{{T_2}^{\max }-{T_2}^{\min }}& {T_2}^{\min }<A<{T_2}^{\max }\\ 1& {T_2}^{\max }\&le;A\end{array}\right.---\left(26\right)$

其中，T₂ ^min和T₂ ^max是用于标准化凸包的面积的参数，并且设置满足T₂ ^max≤T₄的适当值。

在步骤S17090中，位置/姿势计算单元1720计算摄像装置130的位置和姿势的校正值以将所有检测到的指标上的误差最小化。注意，位置的校正值由三值矢量Δt_{WC_3}表示，姿势的校正值由3×3旋转矩阵ΔR_{WC_3}表示。首先，单元1720通过与第一实施例中的步骤S3070相同的方法校正摄像装置的位置和姿势，以获得摄像装置的校正后的位置t_{WC_3}和姿势R_{WC_3}。然后，单元1720基于所获得的R_{WC_3}和传感器测得得姿势测量值R^# _WC按公式(23)计算ΔR_{WC_3}。此外，单元1720通过下式，基于所获得的t_{WC_3}和位置测量值t^# _WC计算Δt_{WC_3}：

$\mathrm{\&Delta;}{t}_{\mathrm{WC}\_3}=t_{\mathrm{WC}\_3}-t_{\mathrm{WC}}^{\#}---\left(27\right)$

在步骤S17100中，位置/姿势计算单元1720通过组合两对校正值(ΔR_{WC_1}和Δt_{WC_1}，以及ΔR_{WC_3}和Δt_{WC_3})来计算实际对传感器测量值施加的校正值。然后处理前进至步骤S17110。

下面说明步骤S17100中的处理细节。位置/姿势计算单元1720通过下式根据步骤S17050中计算出的凸包的面积A来确定权重w₂：

$w_2=\left\{\begin{array}{cc}0& A\&le;{T_3}^{\min }\\ \frac{A-{T_3}^{\min }}{{T_3}^{\max }-{T_3}^{\min }}& {T_3}^{\min }<A<{T_3}^{\max }\\ 1& {T_3}^{\max }\&le;A\end{array}\right.---\left(28\right)$

其中，T₂ ^min和T₂ ^max是用于标准化凸包面积的参数，预先设置适当值以满足T₄≤T₃ ^min。图19示出凸包的面积A和通过公式(26)和(28)获得的权重w₁和w₂之间的关系。

接下来，位置/姿势计算单元1720针对每个位置和姿势计算两个校正值的权重和。对于姿势，单元1720获得校正矩阵ΔR_{WC_1}和ΔR_{WC_3}的姿势成分作为四值表达式(由四值矢量ΔH_{WC_1}和ΔH_{WC_3}表示)，并通过下式计算它们的权重和ΔH_WC：

ΔH_WC＝(1-w₂)ΔH_{WC_1}+w₂ΔH_{WC_3}    (29)

位置/姿势计算单元1720将所获得的权重和ΔH_WC转换为ΔR_WC作为旋转矩阵。另一方面，对于位置的校正值，单元1720通过下式直接计算两个矢量的权重和ΔT_WC：

Δt_WC＝(1-w₂)Δt_{WC_1}+w₂Δt_{WC_3}    (30)

如上所述，Δt_{WC_1}＝[000]^T。因此，当权重W₂＝0时，公式(29)和(30)只选择姿势的三个值的校正，当权重W₂＝1时，通过添加位置的三个值来选择六个自由度的校正。

在步骤S17110中，位置/姿势计算单元1720通过下面的公式(31)和(32)对摄像装置的位置和姿势的传感器测量值施加校正以获得校正后的位置t_WC和姿势R_WC：

$R_{\mathrm{WC}}=\mathrm{\&Delta;}{R}_{\mathrm{WC}}\&CenterDot;R_{\mathrm{WC}}^{\#}---\left(31\right)$

$t_{\mathrm{WC}}=\mathrm{\&Delta;}{t}_{\mathrm{WC}}+t_{\mathrm{WC}}^{\#}---\left(32\right)$

在步骤S17120中，位置/姿势计算单元1720经由I/F 1009向外部输出所获得的表示摄像装置130和要测量的物体170的位置和姿势的数据。作为选择，单元1720通过其它应用程序将这些数据转换为可使用状态后将它们存储在RAM 1002中。

注意，可以象第一实施例中那样校正和输出要测量的物体170的位置和姿势的测量值。可以计算和输出要测量的物体在照相机坐标系上的位置和姿势。还可以计算和输出摄像装置130在物体坐标系上的位置和姿势。

在步骤S1720中，位置/姿势计算单元1720检查处理是否结束。如果处理没有结束，则处理返回到步骤S17000。

利用上述处理，测量了摄像装置的位置和姿势。

在本实施例中，根据检测到的指标的分布条件改变多个校正方法的使用比。因此，  当检测到的指标在时间序列图像上的分布逐渐改变时，多个方法对评价值或校正值的影响程度逐渐改变，并无缝地切换这些方法。因此，与通过阈值处理完全切换方法的第一实施例相比，在时间轴上的平滑方面，本实施例具有优势。

在本实施例中，结合了重点放在图像上的方法和重点放在传感器测量值上的方法。结果，图像信息对推定的位置和姿势的影响程度(贡献程度)根据检测到的指标的分布而改变。更具体来说，可以获得以下效果：当获得了足够的图像信息时，优先使用图像信息；否则，图像信息的影响程度降低。

注意，在上述实施例中，总是通过权重值W独立的两种方法，即权重w1和w2，来计算校正值。然而，首先计算权重w，当其值为0或1时，可以跳过其权重值为零的校正值的计算处理。可以减少当权重＝0或1时的计算量。

在上述说明中，通过两种方法获得的校正值(校正矢量和校正矩阵)的权重和可以被计算并添加到传感器测量值。然而，通过计算由两种方法获得的位置和姿势本身的位置和姿势的权重和也可以获得相同的效果。以这种方式，在步骤S17047中，单元1720按照公式(25)计算在步骤S17030中算出的姿势R_{WC_1}和在步骤S17043中算出的姿势R_{WC_2}的权重和。在步骤S17070中，单元1720按照公式(25)计算在步骤S17030中算出的姿势R_{WC_1}和在步骤S17070中算出的姿势R_{WC_2}的权重和。在步骤S17100中，单元1720按照公式(29)和(30)计算在步骤S17030中算出的姿势R_{WC_1}和位置t^# _WC与在步骤S17090中算出的姿势R_{WC_3}和位置t_{WC_3}的权重和。然后，单元1720输出所获得的位置和姿势作为权重和的结果而不执行步骤S17110。

本实施例中所述的技术思想的要旨是根据检测到的指标的分布组合通过多个方法获得的解。因此，在第二至第五实施例中，通过对两个结果的权重和根据指标的分布改变方法的选择步骤，可以配置具有与第一实施例相同效果的位置/姿势测量设备。

例如，下面将要说明将本实施例的思想应用到第五实施例的情况。例如，执行步骤S15070和S15038二者的处理，以通过这些处理获得位置和姿势。按照公式(28)计算凸包的面积A的权重w₂，按照公式(29)和(30)计算位置和姿势的权重和。同时，对于姿势，可以计算通过各方法获得的校正值的权重和，并且可以将该权重和添加到传感器测量值。由于同样应用于对权重和改变另一方法的选择步骤的方法，因此不再给出进一步说明。

在本实施例中，作为指标分布的测量值，由检测到的指标形成的凸包的面积的使用同样不是必不可少的，并且可以如上述实施例中的变形例4使用各种其它测量值。

如公式(24)、(26)和(28)以及图18和图19中所示，在本实施例中，当指标分布的测量值从下限值转变到上限值时，权重，即，权重w₁和w₂被设置为从0到1的线性转变。然而，权重计算方法不局限于这种特定方法。也就是说，当指标分布的测量值是下限值时，权重值是0；当指标分布的测量值是上限值时，权重值为1；并且假定权重值是在这两个值之间单调递增的值，由此获得相同的效果。例如，公式(26)可以修改为：

$w_1=\left\{\begin{array}{cc}0& A\&le;{T_2}^{\min }\\ (\cos \left(\frac{A-{T_2}^{\min }}{{T_2}^{\max }-{T_2}^{\min }}\mathrm{\&pi;}\right)+1)/2& {T_2}^{\min }<A<{T_2}^{\max }\\ 1& {T_2}^{\max }\&le;A\end{array}\right.---\left(33\right)$

当然，可以使用其它相似的单调递增函数来代替。

作为计算两个姿势的权重平均的方法，在本实施例中使用由公式(25)和(29)给出的四元素的权重和。然而，姿势的权重平均方法不限于此，可以使用其它计算方法。例如，可以代替使用公式(25)而使用以下方法。首先，计算表示姿势-校正矩阵ΔR_{WC_2}和ΔR_{WC_1}之间差异的姿势作为旋转轴和旋转角度θ的表达式。接下来，使权重w₁乘以旋转角度θ获得的角度等于0′。最后，计算由旋转轴和旋转角度θ表示的姿势，并将其乘以ΔR_{WC_2}。当w₁＝0时，该结果是ΔR_{WC_2}，当w1＝₁时，该结果是ΔR_{WC_1}。当w₁假定是0和1之间的中间值时，可以获得通过加权和插值这两个校正矩阵而获得的校正矩阵。当然，可以使用例如加权和插值这两个姿势的各种其它计算方法。

第七实施例

在上述实施例中，用数值明确地表示检测到的指标的分布情况，并基于该值控制所关注的帧中检测到的指标以外的信息的影响程度。本实施例除了不执行指标分布的明确数值转换以外，由于它提供根据指标的分布情况控制指标以外的信息的影响程度的方法，所以它与上述实施例相同。

根据本实施例的位置/姿势测量设备通过使用由作为测量对象物体的摄像装置所拍摄的时间序列图像来测量摄像装置的位置和姿势。下面说明根据本实施例的位置/姿势测量设备和位置/姿势测量方法。

图20是示出根据本实施例的位置/姿势测量设备的配置的框图。如图20中所示，本实施例的位置/姿势测量设备2000包括图像输入单元2050、指标检测单元2010、位置/姿势计算单元2020、权重设置单元2060、以及姿势传感器2040。设备2000连接到作为测量对象物体的摄像装置2030。根据本实施例的位置/姿势测量设备2000测量摄像装置2030的位置和姿势。

在场景(摄像装置2030要拍摄的空间)中定义基准坐标系(由符号W表示)。在该场景中的多个位置处，安排在基准坐标系上的位置(基准坐标)已知的多个指标Q_k(k＝1，2，...，K)。注意，k表示每个指标的标识符。K表示所安排的指标的总数。优选调整指标Qk的布局，使得摄像装置2030在测量范围内移动时都可以在拍摄图像中观测到这些指标。

图20的例子示出了K＝6的情况，即在场景中安排有六个指标Q₁至Q₆。例如，指标Q_k可以由颜色不同而形状相同(图20中为圆形)的标记来配置，也可以由具有不同纹理特征的自然特征等特征点来配置。作为选择，可以使用由每个具有一定面积的矩形单色区域形成的矩形指标。指标Q_k可以具有任何形式，只要它们的投影图像在拍摄图像上的坐标可以检测，并且这些指标Q_k中的每一个可以由任意方法来识别即可。此外，指标可以是故意设置的，也可以是自然形状的指标而不是故意设置的。

摄像装置2030包括例如摄像机。由摄像装置2030拍摄的图像被输入到位置/姿势测量设备2000。在摄像装置2030上定义照相机坐标系。在以下说明中，当需要时，用符号C表示照相机坐标系。位置/姿势测量设备2000测量该照相机坐标系关于基准坐标系的位置和姿势作为该摄像装置的位置和姿势。照相机坐标系是以摄像装置2030的视点位置为原点、以视轴为Z轴、以垂直于Z轴的两个值交轴为X轴和Y轴的坐标系。假定摄像装置2030的照相机内部参数是已知的。

姿势传感器2040安装到摄像装置2030，并测量摄像装置2030在基准坐标系上的姿势。从姿势传感器2040输出的测量值被输入到位置/姿势计算单元2020。姿势传感器2040包括例如可从美国InterSense获得的InertiaCube2等。

图像输入单元2050将输入到位置/姿势测量设备2000的拍摄图像转换为数字数据，并将该数字数据输出到指标检测单元2010。

指标检测单元2010从图像输入单元2050接收拍摄图像。指标检测单元2010分析输入图像，并检测(识别)在该图像中拍摄到的指标。注意，在本说明书中，除非另外说明，“检测到的指标”是指“识别到的指标”。指标检测单元2010还将对应于检测到的指标(在下文中当需要时也被称为检测指标)的图像坐标的基准坐标输出到位置/姿势计算单元2020。

注意，序号n(n＝1，2，...，N)被分配给各个检测指标，在图像上检测到的每个指标被描述为Q_kn。注意，N表示在图像上检测到的指标的总数。每个检测指标Q_kn的图像坐标(实际测量值)被描述为u_n，当需要时，由u_C ^Qk描述每个指标Q_k的图像坐标(即，u_n＝u_C ^Qkn)。作为已知值的每个指标Q_k的基准坐标被描述为x_W ^Qk。

由根据要使用的指标类型的方法检测指标。例如，如图20的例子中，当由具有不同颜色的标记配置指标时，从拍摄图像中检测对应于每个单独标记的区域。然后，将其中心位置确定为关注指标的检测到的坐标。另一方面，  当由具有不同纹理特征的特征点配置指标时，使用其模板图像通过模板匹配来检测每个指标的位置。在此情况下，将每个单独指标的模板图像作为已知值预先保持。

当使用矩形指标时，图像经过二值化处理和加标签处理以检测作为指标候选的由四条直线形成的标签区域。此外，通过检查每个候选区域的矩形区域是否包括特定图案来消除检测误差。此外，基于矩形区域中的图案来获取该指标的标识符。最后，输出矩形区域的四个顶点坐标作为指标的位置。注意，在本说明书中以这种方式检测到的矩形指标被认为是由四个顶点单独形成的四个指标。

权重设置单元2060提供用户接口(UI)，其用于设置表示“对姿势测量值的重视程度”的参数(在下文中根据需要称之为“权重参数”)。权重设置单元2060经由例如键盘1004、鼠标1005(稍后说明)的输入装置等接收权重参数，并将其输出到位置/姿势计算单元2020。

作为操作者在UI上表达权重参数的方法，可以原样使用在位置/姿势计算单元2020内使用的加权系数w的值。作为选择，可以使用加权系数w的指数部分的数值(10的N次幂)。此外，可以输入标准化后的值(0和1之间的值)，并且标准化后的值可以被内部地映射到加权系数w上。可以通过从键盘直接键入值来输入这些数值，也可以通过使用滑动块等GUI选择值来输入这些数值。

当使用滑动块等GUI时，可以使用数字值以外的概念表达式来输入参数，并且可以将该输入值内部地映射到加权系数w上。作为概念表达式，可以使用允许操作者从“最小”、“最大”、以及它们之间的几个水平中选择参数的UI。此外，还可以使用允许操作者从“指标重视”、“传感器重视”、以及它们之间的几个水平中选择参数的UI。

位置/姿势计算单元2020从姿势传感器2040接收摄像装置2030的姿势的测量值，并且还从指标检测单元2010接收检测指标Q_kn的图像坐标u^Qkn和基准坐标x_W ^Qkn对。单元2020基于输入信息计算并输出摄像装置2030的位置和姿势。稍后使用流程图说明位置/姿势计算单元2020的处理细节。

注意，图像输入单元2050、指标检测单元2010、权重设置单元2060、以及位置/姿势计算单元2020中的至少一些可以作为独立装置来实现，也可以作为通过安装在一个或多个计算机中并由每个计算机的CPU来执行从而实现该功能的软件程序来实现。在本实施例中，假定各单元(图像输入单元2050、指标检测单元2010、权重设置单元2060、以及位置/姿势计算单元2020)由软件来实现，并安装在单个计算机中。通过执行软件来实现各单元的功能的计算机的基本配置与第一实施例中的相同，并且将不给出其重复说明。

图21是示出位置/姿势计算单元2020的处理序列的流程图。在本实施例中，当CPU 1001执行用于实现位置/姿势计算单元2020的功能的程序时执行该处理序列。假定在执行以下处理前已经将执行根据图21的流程图的序列的程序代码从例如外部存储装置1007加载到RAM 1002上。

在以下说明中，由三值矢量t＝[x y z]^T内部地表达摄像装置2030的位置。此外，由三值矢量ω＝[ξψζ]^T表达摄像装置2030的姿势，该矢量通过矢量大小来定义旋转角度，通过矢量方向来定义旋转轴。由六值矢量s＝[t^Tω^T]^T＝[x y zξψζ]^T来表达摄像装置2030的位置和姿势。

注意，姿势信息被变换为3×3旋转矩阵R，在使用时旋转矩阵R获得从照相机坐标系到基准坐标系的旋转变换。ω和R可以被唯一地相互转换。可以通过公式(3)获得从ω到R的转换。由于对本领域的技术人员来说从R到ω的转换方法是已知的，所以将不给出其详细说明。

在步骤S21000中，位置/姿势计算单元2020在时间τ从姿势传感器2040接收摄像装置2030的姿势测量值ω^* _τ(＝[ξ^* _τψ^* _τζ^* _τ]^T)。

在步骤S21010中，位置/姿势计算单元2020在时间τ指标检测单元2010接收从拍摄图像中检测到的指标Q_kn(n＝1，2，...，N)的图像坐标u_n(＝u_C ^Qkn)和基准坐标x_W ^Qkn对。

在步骤S21020中，位置/姿势计算单元2020检查检测指标的输入信息是否足以推定位置和姿势，并根据该检查结果分支该处理。更具体来说，如果输入指标的数量是三个或更多时，处理前进到步骤S21030；否则，处理结束。

在步骤S21030中，位置/姿势计算单元2020处理作为规定值的与姿势测量值的倾斜角有关的两个参数，并使用各检测指标的信息推定剩余四个参数。注意，剩余的四个参数包括摄像装置2030的方位和位置。由于可以通过例如非专利文献6中所公开的方法来实现该处理，因此将不给出其详细说明。在以下说明中，在步骤S21030中获得的位置和姿势被表记为在随后处理中的位置和姿势的处始值s₀＝[t₀ ^T ω₀ ^T]^T＝[x₀ y₀ z₀ξ₀ψ₀ζ₀]^T。注意，所获得的姿势ω₀和姿势测量值ω^* _τ之间的差异只是方位分量。也就是说，ω₀可以被认为是通过校正ω^* _τ的方位漂移误差而获得的姿势。

在步骤S21040中，位置/姿势计算单元2020计算每个指标Q_kn的图像坐标的推定值(理论值)u_n ^*(＝u_C ^Qkn*)。基于由s定义的指标的观测方程，即从指标的基准坐标x_W ^Qkn计算图像坐标的公式(19)来计算u_n ^*。在公式(19)中，图像坐标的推定值u_n ^*被表记为u^Qkn*。

在步骤S21050中，位置/姿势计算单元2020通过下式计算每个指标Q_kn的图像坐标的推定值(理论值)u_n ^*和实际测量值u_n之间的差异(再投影误差)Δu_n：

$\mathrm{\&Delta;}{u}_n=u_n-u_n^{*}---\left(34\right)$

在步骤S21060中，位置/姿势计算单元2020关于每个指标Q_kn计算s的图像雅克比J_us ^Qkn(＝u/s)。注意，该图像雅克比是以利用s的元素对由公式(19)给出的观测方程进行偏微分而获得的解作为各元素的2行×3列的雅克比矩阵。更具体来说，单元2020计算雅克比矩阵J_ux ^Qkn(＝u/x)和雅克比矩阵J_xs ^Qkn(＝x/s)，然后通过下式计算J_us ^Qkn：

$J_{\mathrm{us}}^{Q_{k_n}}=J_{\mathrm{ux}}^{Q_{K_n}}\&CenterDot;J_{\mathrm{xs}}^{Q_{K_n}}---\left(35\right)$

其中，J_ux ^Qkn是以用照相机坐标x_C ^Qkn的各元素对公式(6)的右首侧进行偏微分而获得的解作为各元素的2行×3列的雅克比矩阵。J_xs ^Qkn是以用矢量s的各元素对公式(20)的右手侧进行偏微分而获得的解作为各元素的3行×6列的雅克比矩阵。

在步骤S21070中，位置/姿势计算单元2020使用下式计算当前姿势推定值ω与在步骤S21030中获得的姿势初始值ω₀的差Δω(＝[ΔξΔψΔζ]^T)：

Δω＝ω₀-ω            (36)

在步骤S21080中，位置/姿势计算单元2020基于在上述步骤中计算出的再投影误差Δu_n、图像雅克比J_us ^Qkn、以及姿势差异Δω计算s的校正值Δs。更具体来说，单元2020生成通过排列雅克比形成的矩阵Θ，并通过下式计算Δs：

ΔS＝(Θ′Θ)^-1Θ′U    (37)

其中，Θ是通过将与姿势参数ξ、ψ和ζ的s有关的雅克比垂直添加到由排列雅克比J_us ^Qkn形成的矩阵而生成的。Θ由下式定义：

$\mathrm{\&Theta;}=\begin{array}{c}\left[\begin{array}{ccccccc}& & & J_{\mathrm{us}}^{Q_{k_1}}& & & \\ & & & .& & & \\ & & & .& & & \\ & & & .& & & \\ & & & J_{\mathrm{us}}^{Q_{k_N}}& & & \\ 0& 0& 0& & 1& 0& 0\\ 0& 0& 0& & 0& 1& 0\\ 0& 0& 0& & 0& 0& 1\end{array}\right]\end{array}---\left(38\right)$

此外，Θ′是通过将加权系数w添加到Θ的转置矩阵而生成的，并且由下式定义：

${\mathrm{\&Theta;}}^{\&prime;}=\left[\begin{array}{cccccccc}& & & & & 0& 0& 0\\ & & & & & 0& 0& 0\\ & & & & & 0& 0& 0\\ J_{\mathrm{us}}^{{Q_{k_1}}^T}& .& .& .& J_{\mathrm{us}}^{{Q_{k_N}}^T}& & & \\ & & & & & w& 0& 0\\ & & & & & 0& w& 0\\ & & & & & 0& 0& w\end{array}\right]---\left(39\right)$

其中，w是用于在图像信息和姿势测量值之间平衡的加权系数，并代表“对姿势测量值的重视程度”。值w确定姿势测量值对最终获得的位置和姿势的影响的基准值。由权重设置单元2060给出加权系数w的值。注意，根据从指标获得的信息量被动地改变姿势测量值的实际影响。

另一方面，误差矢量U是通过排列Δu_n和Δω的各项而形成的(2N+3)维矢量，并由下式来定义：

$U=\left[\begin{array}{c}\mathrm{\&Delta;}{u}_1\\ .\\ .\\ .\\ \mathrm{\&Delta;}{u}_N\\ \mathrm{\&Delta;\&xi;}\\ \mathrm{\&Delta;\&psi;}\\ \mathrm{\&Delta;\&zeta;}\end{array}\right]---\left(40\right)$

下面说明由公式(37)至(40)计算出的Δs的含义。公式(36)的目的是计算Δs以直观地减少误差U。例如，在仅使用如第五实施例的步骤S15070中的处理的指标信息计算照相机的位置和姿势的方法中，由下式定义误差矢量：

$U=\left[\begin{array}{c}\mathrm{\&Delta;}{u}_1\\ .\\ .\\ .\\ \mathrm{\&Delta;}{u}_N\end{array}\right]---\left(41\right)$

该公式意图使用它们作为评价基准来最小化指标的再投影误差。

另一方面，通过将姿势差异Δω添加到由公式(41)定义的误差矢量U来形成由公式(40)定义的误差矢量U。这表明根据本实施例的位置/姿势计算单元2020使用指标的再投影误差和姿势测量值之间的差异作为评价基准。直观上，单元2020计算Δs，Δs用于考虑到姿势一定不能过分偏离姿势测量值(严格来说是通过校正姿势测量值的方位误差而获得的值)的约束条件将指标的再投影误差最小化。

在步骤S21090中，位置/姿势计算单元2020使用在步骤S21080中计算出的校正值Δs根据公式(12)来校正s，并将所获得的值设置为s的新的推定值。

在步骤S21100中，位置/姿势计算单元2020使用一些标准，如误差矢量U是否小于预定阈值或者校正值Δs是否小于预定阈值，来检查该计算是否收敛。如果该计算不收敛，则单元2020在校正后再次使用s执行步骤S21040及随后步骤中的处理。另一方面，如果判定该计算收敛，则处理前进到步骤S21110。

在步骤S21110中，位置/姿势计算单元2020输出通过直到步骤S21100的处理获得的s作为摄像装置2030的位置和姿势的推定值。注意，位置和姿势的输出形式不限于s本身。例如，当输出时，可以将姿势转换为旋转矩阵、旋转轴矢量和旋转角、欧拉角、四元素等。此外，可以由基于同次坐标表达的4×4位置/姿势转换矩阵或其逆矩阵表示位置和姿势。可以计算和输出摄像装置2030在基准坐标系上的位置和姿势。当然，可以同时输出这些输出形式的组合。

利用上述处理，计算出摄像装置2030的位置和姿势。当针对连续拍摄的时间序列图像计算摄像装置2030的位置和姿势时，可以针对每个输入帧执行上述处理。

根据本实施例的位置/姿势测量设备，计算这样的位置和姿势，该位置和姿势考虑到姿势一定不能过分偏离姿势测量值(严格来说是通过校正姿势测量值的方位误差而获得的值)的约束条件将指标的再投影误差最小化。

根据本实施例的位置/姿势测量设备，当从图像获得的信息不充足时，由于检测指标的各项的影响变得很小，所以可以获得重视姿势测量值的位置和姿势。结果，可以获得与信赖姿势测量值的同时约束未知参数的方法中相同的使解稳定的效果。另一方面，当获得充足的图像信息时，检测指标的各项的影响变得很大。结果，可以获得高精确度解而不受姿势测量值的误差的影响。

也就是说，根据本实施例的位置/姿势测量设备，可以有效利用从图像和姿势传感器获得的信息，并且与传统方法相比较可以实现稳定性和精确度二者皆优的测量。

第八实施例

根据本实施例的姿势测量设备使用配置在环境侧上的多个摄像装置和指标以及设置在测量对象物体上的多个摄像装置、指标和姿势传感器来测量任意测量对象物体的位置和姿势。下面说明根据本实施例的位置测量设备和位置测量方法。

图22是示出根据本实施例的位置/姿势测量设备的配置。如图22中所示，根据本实施例的位置/姿势测量设备2200包括图像输入单元2250、指标检测单元2210、位置/姿势计算单元2220、模式选择单元2280、一个或更多个主观视点照相机2230、一个或更多个客观视点照相机2260、以及姿势传感器2040。位置/姿势测量设备2200连接到要测量的物体2270。根据本实施例的位置/姿势测量设备2200测量要测量的物体2270在基准坐标系上的位置和姿势。

在场景中，与第一实施例中相同地配置在基准坐标系上的位置已知的一个或更多个指标Q_k(k＝1，2，...，K_W)。在以下说明中，根据需要将配置在环境侧上的这些指标称为主观视点指标。与第一实施例中相同，主观视点指标可以采用每种形式，只要它们的投影图像在拍摄图像上的图像坐标是可以检测的并且可以由任意方法识别每个指标即可。在图22中，K_W＝6，即，配置六个主观视点指标。

在要测量的物体2270上，配置在物体坐标系上的位置(物体坐标)已知的一个或更多个指标Q_KW+k(k＝1，...，K_O)。在以下说明中，根据需要将配置在物体侧上的这些指标称为客观视点指标。与主观视点指标相同，客观视点指标可以采用每种形式，只要它们的投影图像在拍摄图像上的图像坐标是可以检测的并且可以由任意方法识别每个指标即可。在图22中，K_O＝4，即配置四个客观视点指标。注意，客观视点指标不可以直接配置在要测量的物体2270上。客观视点指标可以配置在例如主观视点照相机2230上或姿势传感器2040上。

主观视点照相机2230a和2230b(也被通称为主观视点照相机2230)例如是视频照相机。主观视点照相机2230配置在要测量的物体2270上。由主观视点照相机2230拍摄的图像(主观视点图像)被输入到图像输入单元2250。假定主观视点照相机2230的照相机内部参数和它们在物体坐标系上的位置和姿势是已知的。主观视点指标的配置和主观视点照相机2230被优选调节成当要测量的物体2270在测量范围内移动时在拍摄图像中总可以观测到主观视点指标。在图22中，配置两个主观视点2230a和2230b。在以下说明中，根据需要，由符号C₁和C₂来表达由主观视点照相机2230分别定义的主观视点照相机坐标系。

客观视点照相机2260a至2260d(也被通称为客观视点照相机2260)是例如摄像机。客观视点照相机2260被设置在包围在测量范围内的位置处，以便于拍摄要测量的问题2270的图像。由客观视点照相机拍摄的图像(客观视点图像)被输入到图像输入单元2250。假定客观视点照相机2260的照相机内部参数和它们在基准坐标系上的位置和姿势是已知的。客观视点指标的配置和客观视点照相机2260被优选调节成当要测量的物体2270在测量范围内移动时在拍摄图像中总可以观测到客观视点指标。在图22中，配置两个主观视点2230a和2230b。在以下说明中，根据需要，由符号C1和C2来表达由主观视点照相机2230分别定义的主观视点照相机坐标系。在图22中，配置了四个客观指标照相机2260a、2260b、2260c、以及2260d。在以下说明中，由符号B₁、B₂、B₃和B₄来表达由客观视点照相机2260a至2260d分别定义的客观视点照相机坐标系。

姿势传感器2040配置在要测量的物体2270上。姿势传感器2040测量要测量的物体2270的姿势，并将其输出到位置/姿势计算单元2220。

图像输入单元2250将输入到位置/姿势测量设备2000的拍摄图像转换为数字数据，并将该数字数据输出到指标检测单元2210。

指标检测单元2210从图像输入单元2250接收拍摄图像。指标检测单元2210分析每个输入图像，并检测(识别)每个图像中的所拍摄到的指标。指标检测单元2210针对每个单独检测指标向位置/姿势计算单元2220输出一组检测到该指标的照相机的标识符、图像坐标的实际测量值、对应的3D坐标。注意，当指标是主观视点指标时3D坐标意味着基准坐标，当指标是客观视点指标时3D坐标以为着物体坐标。注意，指标检测方法与第一实施例中的一样不受具体限制，并且将不给出其详细说明。

在以下说明中，注意，序号n(n＝1，2，...，N)被分配给各检测指标，在每个图像上被检测到的每个指标被描述为Q_kn。注意，N表示在每个图像上检测到的指标的总数。此外，N＝∑N_d，N_d代表在每个照相机的拍摄图像上检测到的指标数。此外，d表示照相机的标识符(d＝C₁，C₂，B₁，B₂，B₃，B₄)。每个指标Q_kn的图像坐标被描述为u_n，拍摄该指标的照相机的标识符被描述为d_n，由具有该标识符的照相机检测到的每个指标Q_k的图像坐标被描述为u_d ^Qk(即，u_n＝u_dn ^Qkn)。当每个指标Q_k是主观视点指标时，其基准坐标作为已知值被描述为x_W ^Qk。同样，当每个指标Q_k是客观视点指标时，其物体坐标作为已知值被描述为x_O ^Qk。

模式选择单元2280允许用户用通过键盘1004、鼠标1005等输入装置选择在位置/姿势计算单元2220中使用的算法。例如，单元2280允许用户使用无线按钮等GUI在“重视指标信息”模式、“重视姿势传感器测量值”模式、以及“中间”模式中选择一种模式。描述选择单元2280将表示所选中的模式的信息输出到位置/姿势计算单元2220。

位置/姿势计算单元2220从姿势传感器2040接收要测量的物体2270的姿势测量值。此外，单元2220从指标检测单元2210接收针对每个检测指标的一组检测到该指标的照相机的标识符d_n、图像坐标的实际测量值u_n、对应的3D坐标(x_W ^Qkn或x_O ^Qkn)。单元2220基于输入信息计算并输出要测量的物体2270的位置和姿势。下面使用流程图说明位置/姿势计算单元2220的处理细节。

注意，图像输入单元2250、指标检测单元2210、模式选择单元2280、以及位置/姿势计算单元2220中的至少一些可以作为独立装置来实现，也可以作为通过安装到一个或多个计算机中并通过每个计算机的CPU的执行来实现所述功能的软件程序来实现。在本实施例中，假定各单元(图像输入单元2250、指标检测单元2210、模式选择单元2280、以及位置/姿势计算单元2220)由软件来实现并安装在单个计算机中。通过软件来实现各单元的功能的计算机的基本配置与第一实施例中的相同，并省略其重复说明。

图23是示出位置/姿势计算单元2220的处理序列的流程图。在本实施例中，当CPU 1001执行用于实现位置/姿势计算单元2220的功能的程序时，执行该处理序列。假定在执行该处理序列之前，用于执行根据图23的流程图的序列的程序代码已经从例如外部存储装置1007加载到RAM 1002上。

在以下说明中，与第一实施例中相同，由三值矢量t＝[x yz]^T和ω＝[ξψζ]^T内部地表达要测量的物体2270的位置和姿势。此外，由六值矢量s＝[t^T ω^T]^T＝[x y zξψζ]^T表达要测量的物体2270的位置和姿势。

在步骤S23000中，位置/姿势计算单元2220从姿势传感器2040接收要测量的物体2270的姿势测量值ω^* _τ(＝[ξ^* _τψ^* _τζ^* _τ]^T)。

在步骤S23010中，位置/姿势计算单元2220从指标检测单元2210接收在时间τ从各拍摄图像中检测到的检测指标Q_kn的信息。更具体来说，单元2220接收多组检测到对应指标的照相机的标识符d_n、图像坐标的实际测量值u_n、以及对应的3D坐标(x_W ^Qkn或x_O ^Qkn)。

在步骤S23020中，位置/姿势计算单元2220检查检测指标的输入信息是否足以推定位置和姿势，并根据检查结果分支处理。更具体来说，如果输入指标的实体总数为三个或更多个(N≥3)时，处理前进到步骤S23030；否则，处理结束。注意，实体总数意味着当在多个图像中检测到同一指标并将其确定为一个指标时计数的总数。

在步骤S23020中，位置/姿势计算单元2220检查模式选择单元2280所选择的模式是否为“重视指标信息”模式。如果“重视指标信息”模式被选中，则处理前进到步骤S23035；否则，处理前进到步骤S23040。

在步骤S23035中，位置/姿势计算单元2220通过使用各检测指标的再投影误差作为评价基准的方法来推定要测量的物体2270的位置和姿势。作为在将从主观和客观视点照相机获得的检测指标的再投影误差最小化的同时计算物体的位置和姿势的方法，由于在非专利文献1等中公开了基于非线性优化计算的方法，所以将不给出其详细说明。然后，处理前进到步骤S23070。

在步骤S23070中，位置/姿势计算单元2220处理作为规定值的与姿势测量值的倾斜角度有关的两个参数，并使用各检测指标的信息来推定剩余的四个参数。注意，所述剩余的四个参数包括要测量的物体2270的方位和姿势。由于可以通过例如非专利文献1中所公开的方法来实现该处理，所以将不给出其详细说明。在以下说明中，在步骤S23040中获得的位置和姿势被描述为随后步骤中的位置和姿势的初始值s₀＝[t₀ ^T ω₀ ^T]^T＝[x₀ y₀ z₀ξ₀ ψ₀ ζ₀]^T。

在步骤S23050中，位置/姿势计算单元2220检查模式选择单元2280所选择的模式是否为“重视姿势传感器测量值”模式。如果“重视姿势传感器测量值”模式被选中，则确定在步骤S23040中获得的位置和姿势被原样输出，并且处理跳至步骤S23070。否则，处理前进到步骤S23060。

在步骤S23060中，位置/姿势计算单元2220通过使用各检测指标的再投影误差和所推定的姿势参数和姿势测量值之间的差异作为评价基准的方法来推定要测量的物体2270的位置和姿势。稍后使用流程图说明步骤S23060中的处理细节。

在步骤S23070中，位置/姿势计算单元2220输出通过上述处理获得的要测量的物体2270的位置和姿势。

下面使用图24的流程图说明步骤S23060中的处理细节。

在步骤S24010中，位置/姿势计算单元2220计算每个单独指标Q_kn的图像坐标的推定值(理论值)u_n ^*。如果指标Q_kn是主观视点坐标，则基于如下式描述的基于主观视点照相机指标C_i的观测方程计算u_n ^*：

$u_n^{*}=u_{C_i}^{{Q_{k_n}}^{*}}=F_{C_i}(x_W^{Q_{k_n}},s)---\left(42\right)$

由用于根据x_W ^Qkn和s计算关注指标的照相机坐标x_Ci ^Qkn的公式(43)和用于根据x_Ci ^Qk计算图像坐标u_n ^*的公式(44)来配置观测方程Fc_i()：

$u_n^{*}=u_{C_i}^{{Q_{k_n}}^{*}}=[u_x^{{Q_{k_n}}^{*}}{u}_y^{{Q_{k_n}}^{*}}{]}^T=[-f_x^{C_i}\frac{x_{C_i}^{Q_{k_n}}}{z_{C_i}^{Q_{k_n}}}-f_y^{C_i}\frac{y_{C_i}^{Q_{k_n}}}{z_{C_i}^{Q_{k_n}}}{]}^T---\left(44\right)$

其中，f^Ci _x和f^Ci _y是主观视点照相机指标C_i分别在x轴方向和y轴方向上的焦距。R_CiO是用于将姿势从物体坐标系O转换到主观视点照相机坐标系C_i上的矩阵，t_CiO是用于在同一坐标系之间转换位置的矢量。假定与主观视点照相机2230a和2230b相对应地预先保持所有这些值作为位置的值。

另一方面，如果指标Q_kn是客观视点，则基于由下式描述的基于客观视点照相机B_i的观测方程计算u_n ^*：

$u_n^{*}=u_{B_i}^{{Q_{k_n}}^{*}}=F_{B_i}(x_O^{Q_{k_n}},s)---\left(45\right)$

由用于根据x_O ^Qkn和s计算关注指标的世界坐标x_W ^Qkn的公式(46)、用于计算每个客观视点照相机坐标系上的坐标x_Bi ^Qkn的公式(47)、以及用于根据x_Bi ^Qkn计算图像坐标u_n ^*的公式(48)来配置观测方程F_Bi()：

$x_{B_i}^{Q_{k_n}}=\left[\begin{array}{c}{x}_{B_i}^{Q_{k_n}}\\ y_{B_i}^{Q_{k_n}}\\ z_{B_i}^{Q_{k_n}}\end{array}\right]=R_{W{B}_i}^{-1}(x_W^{Q_{k_n}}-t_{W{B}_i})---\left(47\right)$

$u_n^{*}=u_{B_i}^{{{Q_k}_n}^{*}}=[u_x^{{Q_{k_n}}^{*}}{u}_y^{{Q_{k_n}}^{*}}{]}^T=[-f_x^{B_i}\frac{x_{B_i}^{Q_{k_n}}}{z_{B_i}^{Q_{k_n}}}-f_y^{B_i}\frac{y_{B_i}^{Q_{k_n}}}{z_{B_i}^{Q_{k_n}}}{]}^T---\left(48\right)$

在步骤S24020中，位置/姿势计算单元2220使用公式(34)计算每个指标Q_kn的图像坐标的推定值u_n ^*和实际测量值u_n之间的差(再投影误差)Δu_n。

在步骤S24030中，位置/姿势计算单元2220关于每个指标Q_kn计算s的图像雅克比J_us ^Qkn(＝u/s)。注意，该图像雅克比是以用s的各元素对由公式(42)或(4 5)给出的观测方程进行偏微分而获得的解作为各元素的2行×6列的雅克比矩阵。可以以与公式(35)相同的方式获得观测方程Fc_i()的图像雅克比。另一方面，通过计算雅克比矩阵J_uxB ^Qkn(＝u/x_B)、雅克比矩阵J_xBxW ^Qkn(＝x_B/x_W)以及雅克比矩阵J_xWs ^Qkn(＝x_W/s)，并使用下式来计算观测方程F_Bi()的图像雅克比：

$J_{\mathrm{us}}^{Q_{k_n}}=J_{u{x}_B}^{Q_{k_n}}\&CenterDot;J_{x_B{x}_W}^{Q_{k_n}}\&CenterDot;J_{x_{W}s}^{Q_{k_n}}---\left(49\right)$

其中，J_uxB ^Qkn是以用客观视点照相机坐标x_Bi ^Qkn的各元素对公式(48)的右手侧进行偏微分而获得的解作为元素的2行×3列的雅克比矩阵。J_xBxW ^Qkn是以用世界坐标x_W ^Qkn的各元素对公式(47)的右手侧进行偏微分而获得的解作为元素的3行×3列的雅克比矩阵。J_xWs ^Qkn是以用状态矢量s的各元素对公式(46)的右手侧进行偏微分而获得的解作为元素的3行×6列的雅克比矩阵。

在步骤S24040中，位置/姿势计算单元2220使用公式(35)计算当前姿势推定值ω关于在步骤S23040中获得的姿势的初始值ω₀的差Δω(＝[ΔξΔψΔζ]^T)。

在步骤S24050中，位置/姿势计算单元2220基于在以上步骤中计算出的再投影误差Δu_n、图像雅克比J_us ^Qkn以及姿势差异Δω来计算s的校正值Δs。更具体来说，单元2220生成通过安排所述雅克比和误差矢量U形成的矩阵Θ，并使用公式(37)计算Δs。注意，矩阵Θ、Θ′和U的定义与公式(38)、(39)和(40)中相同。然而，与第七实施例中不同，每个矩阵包括与不同照相机(主观和客观视点照相机)的检测指标Q^kn有关的所有信息。在本实施例中，作为加权系数w，将事先计算的适当值设置为固定值。

在步骤S24060中，位置/姿势计算单元2220使用在步骤S24050中计算的校正值Δs根据公式(12)校正s，并将所获得的值设置为s的新推定值。

在步骤S24070中，位置/姿势计算单元2220使用一些标准，如误差矢量U是否小于预定阈值或者校正值Δs是否小于预定阈值，来检查该计算是否收敛。如果该计算不收敛，则单元2220在校正后再次使用s执行步骤S21040及随后步骤中的处理。另一方面，如果判定该计算收敛，则处理前进到步骤S24010。

利用上述处理，计算出要测量的物体2270的位置和姿势。根据本实施例的位置/姿势测量设备，计算这样的位置和姿势，该位置和姿势考虑到姿势一定不能过分偏离姿势测量值(严格来说是通过校正姿势测量值的方位误差而获得的值)的约束条件将指标的再投影误差最小化。

在本实施例中，使用主观视点照相机2230和客观视点照相机2260二者。然而，用于测量要测量的物体2270的位置和姿势的照相机的配置不限于此。例如，可以只使用主观视点照相机2230，也可以只使用客观视点照相机2260。

注意，当目的是计算主观视点照相机2230的位置和姿势时，可以使用与本实施例中相同的方法。在此情况下，主观视点照相机2230本身(中的一个)可以被看作是要测量的物体2270。

在本实施例中，使用预定加权系数作为固定值。然而，与第一实施例中相同，可以提供权重设置单元2060以允许操作者设置加权系数w。此外，可以建议使用单个UI来设置权重设置单元2060和模式选择单元2280。例如，作为权重设置单元2060的输入部件，将考虑数值输入标准化参数(例如，0和1之间的值)的UI。在此情况下，如果输入参数是0，则可以设置“重视指标信息”模式；如果输入参数是1，则可以设置“重视姿势传感器测量值”模式；如果输入参数是0和1之间的中间值，则可以设置“中间”模式。只有在“中间”模式下，才可以在使用时将输入参数转换为加权系数w。

第七和第八实施例的变形例

变形例7

在第七和第八实施例中，考虑到姿势一定不能过分偏离姿势传感器2040的姿势测量值的约束条件来推定测量对象物体(摄像装置2030或要测量的物体2270)的位置和姿势。然而，在上述实施例中说明的技术思想的应用范围不限于只考虑姿势一定不能过分偏离姿势测量值的约束条件。

例如，当针对连续拍摄的时间序列图像计算测量对象物体的位置和姿势时，可以考虑与前一帧的连续性。在考虑到位置和姿势一定不能过分偏离在前一帧中计算出的位置和姿势的约束条件时，可以获得在帧之间连续性很好的位置和姿势。

本变形例中的处理步骤与第七和第八实施例中的处理步骤基本相同。下面参考图21说明与第七实施例的不同之处。基本上，与第七实施例的不同之处仅在于步骤S21110中保持的位置和姿势的推定结果(s_τ-1)，并且在校正值的计算步骤(步骤S21080)中考虑关于前一帧的约束条件。注意，第八实施例可以进行类似的修改。

在本变形例中，矩阵Θ(公式(38))，Θ′(公式(39))以及误差矢量U(公式(40))被分别修改为：

$\mathrm{\&Theta;}=\left[\begin{array}{ccccccc}& & & J_{\mathrm{us}}^{Q_{k_1}}& & & \\ & & & .& & & \\ & & & .& & & \\ & & & .& & & \\ & & & J_{\mathrm{us}}^{Q_{k_N}}& & & \\ 0& 0& 0& & 1& 0& 0\\ 0& 0& 0& & 0& 1& 0\\ 0& 0& 0& & 0& 0& 1\\ 1& 0& 0& & 0& 0& 0\\ 0& 1& 0& & 0& 0& 0\\ 0& 0& 1& & 0& 0& 0\\ 0& 0& 0& & 1& 0& 0\\ 0& 0& 0& & 0& 1& 0\\ 0& 0& 0& & 0& 0& 1\end{array}\right]---\left(50\right)$

${\mathrm{\&Theta;}}^{\&prime;}=\left[\begin{array}{cccccccccccccc}& & & & & 0& 0& 0& w_2& 0& 0& 0& 0& 0\\ & & & & & 0& 0& 0& 0& w_2& 0& 0& 0& 0\\ & & & & & 0& 0& 0& 0& 0& w_2& 0& 0& 0\\ J_{\mathrm{us}}^{{Q_{k_1}}^T}& .& .& .& J_{\mathrm{us}}^{{Q_{k_N}}^T}& & & & & & & & & \\ & & & & & w_1& 0& 0& 0& 0& 0& w_3& 0& 0\\ & & & & & 0& w_1& 0& 0& 0& 0& 0& w_3& 0\\ & & & & & 0& 0& w_1& 0& 0& 0& 0& 0& w_3\end{array}\right]---\left(51\right)$

$\left[\begin{array}{c}\mathrm{\&Delta;}{u}_1\\ .\\ .\\ .\\ \mathrm{\&Delta;}{u}_N\\ \mathrm{\&Delta;\&xi;}\\ \mathrm{\&Delta;\&psi;}\\ \mathrm{\&Delta;\&zeta;}\\ {(s_{\mathrm{\&tau;}-1}-s)}^T\end{array}\right]---\left(52\right)$

也就是说，新添加代表针对前一帧的结果的约束条件的六个评价值。注意，w₁、w₂，和w₃是定义姿势测量值的影响、前一帧位置的影响以及前一帧姿势的影响的权重参数。在本变形例中，假定针对各权重参数预先设置适当的值。可以根据对传感器的可靠性的重视程度和帧之间的连续性来独立设置这些值，也可以以简单的方式设置相同的值。。可以针对各参数设置不同的值。与上述实施例中相同，用户可以通过权重设置单元2060对值进行调节。

利用上述处理，可以同时考虑姿势一定不能过分偏离姿势测量值的约束条件以及位置和姿势一定不能过分偏离前一帧的推定结果的约束条件。利用这些约束条件的范围，可以计算使指标的再投影误差最小化的位置和姿势。

注意，作为在前一帧中的位置和姿势的推定结果的s_τ-1可以不总是步骤S21110中推定的结果。例如，可以基于测量对象物体的移动历史来使用当前帧中的测量对象物体的位置和姿势的推定值。在此情况下，可以通过基于测量对象物体的速度或角速度的简单线性近似或者通过各种其它方法来进行预测。

在本变形例中，使用前一帧中的全部位置和姿势参数(六个自由度)作为约束条件。然而，全部参数不必总是被用作约束条件。例如，对于姿势，可以只约束姿势传感器的测量值。在此情况下，可以从矩阵Θ(公式(50))，Θ′(公式(51))和误差矢量U(公式(52))中排除与前一帧中的姿势相关的项。同样，可以省略与前一帧的位置相关的项。还可以只约束前一帧中位置和姿势参数的任意组合。

变形例8

下面将考虑这样的情况，其中以添加到第七和第八实施例的形式将位置传感器进一步设置在测量对象物体(摄像装置2030或要测量的物体2270上)。在此情况下，可以使用从姿势传感器获得的姿势测量值和从位置传感器获得的位置测量值二者作为约束条件。

本变形例中的处理步骤与第七和第八实施例中的处理步骤基本相同。下面参考图21说明与第七实施例的不同之处。基本上，与第七实施例的不同之处仅在于在步骤S21010之后立即插入由位置传感器输入位置测量值的步骤作为步骤S21015，并且在校正值的计算步骤(步骤S21080)中考虑对位置测量的约束。注意，第八实施例也可以进行类似的修改(参见图23)。

在作为新添加的步骤的步骤S21015中，位置/姿势计算单元2020在时间τ从位置传感器接收摄像装置2030的位置姿势值t^* _τ。

在步骤S21030中，位置/姿势计算单元2020计算位置和姿势的初始值s₀。在本变形例中，单元2020使将步骤S21010中输入的姿势测量值ω^* _τ乘以由前一帧的处理推定的姿势传感器的误差校正值Δω_τ-1而获得的姿势等于姿势的初始值ω₀。此外，单元2020使用在步骤S21015中输入的姿势测量值t^* _τ并使用下式来计算位置的初始值t₀：

$t_0=t_{\mathrm{\&tau;}}^{*}-R\left({\mathrm{\&omega;}}_0\right)\&CenterDot;x_O---\left(53\right)$

其中，x_O作为已知值表示位置传感器的测量点在测量对象物体的坐标系上的坐标。更具体来说，公式(53)代表从位置传感器的测量值(其代表测量点的位置)到测量对象物体的测量值的转换方程。

在本变形例中，用于在步骤S21070中计算姿势的初始值和当前推定的姿势之间的差异的方程(公式(36))被修改为如下式的包括位置和姿势两者：

ΔS＝S₀-S                                (54)

此外，在步骤S21080中要计算的矩阵Θ(公式(38))、Θ(公式(39))以及误差矢量U(公式(40))被分别修改为：

$\mathrm{\&Theta;}=\left[\begin{array}{ccccccc}& & & J_{\mathrm{us}}^{Q_{k_1}}& & & \\ & & & .& & & \\ & & & .& & & \\ & & & .& & & \\ & & & J_{\mathrm{us}}^{Q_{k_N}}& & & \\ 1& 0& 0& & 0& 0& 0\\ 0& 1& 0& & 0& 0& 0\\ 0& 0& 1& & 0& 0& 0\\ 0& 0& 0& & 1& 0& 0\\ 0& 0& 0& & 0& 1& 0\\ 0& 0& 0& & 0& 0& 1\end{array}\right]---\left(55\right)$

${\mathrm{\&Theta;}}^{\&prime;}=\left[\begin{array}{ccccccccccc}& & & & & w_1& 0& 0& 0& 0& 0\\ & & & & & 0& w_1& 0& 0& 0& 0\\ & & & & & 0& 0& w_1& 0& 0& 0\\ J_{\mathrm{us}}^{{Q_{k_1}}^T}& .& .& .& J_{\mathrm{us}}^{{Q_{k_N}}^T}& & & & & & \\ & & & & & 0& 0& 0& w_2& 0& 0\\ & & & & & 0& 0& 0& 0& w_2& 0\\ & & & & & 0& 0& 0& 0& 0& w_2\end{array}\right]---\left(56\right)$

$U=\left[\begin{array}{c}\mathrm{\&Delta;}{u}_1\\ .\\ .\\ .\\ \mathrm{\&Delta;}{u}_N\\ \mathrm{\&Delta;}{s}^T\end{array}\right]---\left(57\right)$

更具体来说，添加了针对位置测量值的新的约束条件。注意，w₁和w₂是分别定义位置测量值的影响和姿势测量值的影响的权重参数。在本变形例中，针对w₁和w₂预先设置适当的值。可以根据传感器的可靠性单独设置这些值，也可以以简单的方式设置相同的值。可以针对各参数设置不同的值。与上述实施例中相同，用户可以通过权重设置单元2060调整各值。

在步骤S21110中，位置/姿势计算单元2020输出推定的位置和姿势。此时，单元2020计算姿势测量值ω^* _τ和最终获得的姿势ω之间的差，并保持其作为误差校正值Δω_τ-1。

利用上述处理，可以考虑到位置和姿势一定不能分别过分偏离位置测量值和姿势测量值的约束条件来计算使指标的再投影误差最小化的位置和姿势。

根据本变形例的位置/姿势测量设备，当从图像获得的信息不充分时，由于指标的各项的影响很小，所以可以获得重视各传感器的测量值的位置和姿势。另一方面，当获得了充分的图像信息时，指标各项的影响变得很大。结果，可以获得高精确度的解而不受各传感器测量值误差的影响。

在本变形例中，同样可以考虑帧之间的连续性。在此情况下，与变形例7中相同，可以将针对在前一帧中算出的位置和姿势的约束添加到校正值的计算步骤中。更具体来说，可以将用于评价前一帧的位置和姿势参数的各项添加到矩阵Θ(公式(55))、Θ′(公式(56))以及误差矢量U(公式(57))。注意，可以选择和使用全部12个约束条件(针对前一帧的六个约束条件和针对传感器测量值的六个约束条件)的任意组合。

变形例9

在第七和第八实施例以及变形例7和8的每一个中，推定了测量对象物体(摄像装置2030或要测量的物体2270)的位置和姿势的六个自由度的参数。然而，上述实施例中说明的技术思想的应用范围不局限于要推定的参数为位置和姿势的六个值的情况。

例如，下面将考虑具有高精确度的位置传感器被设置在测量对象物体上的情况。在此情况下，作为已知的方法，可以采用根据图像信息只计算测量对象物体的姿势参数的方法同时将从位置传感器获得的位置设置为已知值。与例如专利文献4的第六实施例中的说明相同，可以通过只将测量对象物体的姿势参数作为未知数的非线性优化处理来实现该方法。当采用该方法时，同样可以原样应用上述实施例中说明的技术思想。也就是说，可以在考虑针对姿势传感器2040的姿势测量值的约束条件的同时推定测量对象物体的姿势参数。

本变形例的处理步骤与第七和第八实施例中的基本相同。下面将参考图21说明与第七实施例的不同之处。基本上，与第七实施例的不同之处仅在于添加了输入姿势传感器的姿势测量值的步骤(步骤S21015)，并且只有测量对象物体的姿势ω变为未知参数。注意，也可以对第八实施例进行类似修改。

步骤S21015、S21030以及S21110中的处理与变形例7中的相同。

在本变形例中，将测量对象物体的位置作为已知值来处理，未知参数只是代表姿势的ω。在步骤S21060中，位置/姿势计算单元2020只针对ω的各参数计算图像雅克比。此外，根据未知参数的变化将步骤S21080中的矩阵Θ(公式(38))和Θ′(公式(39))分别修改为：

$\mathrm{\&Theta;}=\left[\begin{array}{ccc}& J_{\mathrm{us}}^{Q_{k_1}}& \\ & .& \\ & .& \\ & .& \\ & J_{\mathrm{us}}^{Q_{k_N}}& \\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(58\right)$

${\mathrm{\&Theta;}}^{\&prime;}=\left[\begin{array}{cccccccc}& & & & & w& 0& 0\\ J_{\mathrm{us}}^{{Q_{k_1}}^T}& .& .& .& J_{\mathrm{us}}^{{Q_{k_N}}^T}& 0& w& 0\\ & & & & & 0& 0& w\end{array}\right]---\left(59\right)$

利用上述处理，即使当六个自由度的位置和姿势没有被用作未知参数时，也可以考虑到姿势一定不能过分偏离姿势传感器的测量值的约束条件来计算使指标的再投影误差最小化的姿势。

在本变形例中，同样可以考虑帧之间的连续性。在此情况下，与变形例7中相同，可以将针对前一帧中算出的姿势的约束添加到校正值计算步骤中。更具体来说，可以将用于评价前一帧的姿势参数的各项添加到矩阵Θ(公式(58))、Θ′(公式(59))以及误差矢量U(公式(40))。

变形例10

在第七和第八实施例以及变形例7至9的每一个中，权重参数w使预先确定的固定值，或者是用户通过权重设置单元2060设置的值。然而，权重参数值的设置方法不局限于这种特定方法。在本变形例中，根据检测到的指标的分布主动调整权重参数，从而根据从指标获得的信息量更积极地实现调整指标影响的效果。

本变形例的处理步骤与第七和第八实施例中的基本相同。下面将参考图21说明与第七实施例的不同之处。基本上，与第七实施例的不同之处仅在于添加了计算检测到的指标的分布状况的步骤(步骤S21025)和根据计算出的检测到的指标的分布状况确定权重参数w的步骤(步骤S21027)。注意，对第八实施例也可以进行类似的修改。

在作为新添加步骤的步骤S21025中，位置/姿势计算单元2020计算包括所有检测到的指标的图像坐标的凸包。该处理与第六实施例中的步骤S17050中的处理相同。

接下来，在作为新添加步骤的步骤S21027中，位置/姿势计算单元2020通过下式根据在步骤S21025中计算出的凸包的面积A确定权重调整参数w₀：

$w_0=\left\{\begin{array}{cc}1& A\&le;T^{\min }\\ \frac{T^{\max }-A}{T^{\max }-T^{\min }}& T^{\min }<A<T^{\max }\\ 0& T^{\max }\&le;A\end{array}---\left(60\right)\right.$

其中，T^min和T^max是标准化凸包的面积所需的参数，针对这些参数预先设置适当的值。假定w₀是标准化为范围0和1之间的值。如果指标的分布是充分的，则w₀假定为0；如果指标的分布是不充分的，则w₀假定为靠近1的值。位置/姿势计算单元2020基于所获得的w₀通过下式计算在校正值Δs的计算步骤中所使用的加权系数w：

w＝w₀·w′                            (61)

其中，w′是由权重设置单元2060设置的加权系数。也就是说，根据指标的分布增加/减少由权重设置单元2060设置的加权系数。注意，w′不必使用权重设置单元2060来设置，可以预先设置适当的值。

利用上述处理，如果指标的分布是充分的，则加权系数w的值变小，并且姿势一定不能过分偏离姿势测量值的约束条件的影响也被减小。因此，可以通过充分采用检测到的图像信息来测量位置和姿势，而不受传感器误差的约束。

另一方面，如果指标的分布是不充分的，则加权系数w的值变大，并且姿势一定不能过分偏离姿势测量值的约束条件的影响增强。因此，即使当检测到的图像信息的信息量不充分时，也可以获得推定结果而不会偏离从传感器获得的姿势。

如上所述，根据本实施例，同样可以获得根据指标的分布控制从图像获得的信息的影响的效果。

注意，在本变形例中，使用由指标形成的凸包的面积作为指标分布的测量不是必不可少的。与实施例的变形例4中相同，可以使用各种其它测量。此外，加权系数w的偏差不必总是关于指标分布的测量成线性，并且与第六实施例中相同，可以使用在两个区间单调增加的任意值。

即使当像变形例7中那样同时考虑位置和姿势一定不能过分偏离在前一帧中计算出的位置和姿势的约束条件时，也可以根据指标的分布调整加权系数。在此情况下，w₁′、w₂′和w₃′被保持作为基本加权系数，并乘以w₀以获得要被实际应用的加权系数，即w₁、w₂和w₃。此外，当像变形例8中那样采用姿势传感器的约束时，可以根据指标的分布类似地调整加权系数。

注意，当使用上述12个约束条件(针对前一帧的6个约束条件以及针对传感器测量值的6个约束条件)的任意组合时，也可以获得根据指标的分布调整检测到的图像信息的影响的效果。例如，可以采用只使用位置和姿势一定不能过分偏离前一帧的推定结果(其至少一个参数)的约束条件而不使用任何传感器的配置。

在此情况下，如果指标的分布是充分的，则加权系数w的值变小，然后位置和姿势一定不能过分偏离前一帧中的推定值的约束条件的影响被减小。因此，通过充分利用检测到的图像信息可以测量位置和姿势而不受可以进行帧之间的平滑化的约束的影响。

另一方面，如果指标的分布不充分，则加权系数w的值变大，然后位置和姿势一定不能过分偏离前一帧中的推定值的约束条件的影响被增强。因此，即使当检测到的图像信息的信息量不充分时，也可以避免帧之间不自然的抖动，并且可以获得稳定的位置和姿势。

变形例11

第七和第八实施例以及变形例7至10的每一个使用又公式(37)表达的方法计算校正值Δs。此外，通过用s+Δs替换s来更新推定值。然而，基于矩阵Θ、矩阵Θ′以及误差矢量U计算s的方法不限于这种特定方法。例如，可以使用作为已知的非线性方程迭代解法的例如LM方法(Levenberg-Marquardt方法)来计算s。或者可以结合作为已知的鲁棒推定方法的M推定(M-estimation)等统计方法。因此，即使当应用任何其它数值计算方法时本发明的本质也不受损害。

变形例12

在上述实施例和变形例的每一个中，姿势由公式(3)描述的三个值来表达，并且在优化计算步骤中被使用。然而，在优化计算步骤中使用的位置和姿势不限于此。例如，可以使用欧拉角由三个值来表达姿势，并且可以使用该欧拉角作为未知参数进行优化计算。在此情况下，代替公式(3)获得了用于从欧拉角计算3×3旋转矩阵R的公式，并且可以在每个单独观测方程中建立该公式。当施加基于姿势测量值或前一帧的姿势推定值的约束时，可以将每个单独参数类似地改变为欧拉角。当然，即使当使用其它参数表达方法作为未知参数时，也可以类似地解决它们，并且参数表达方法不会造成任何本质问题。由于在例如非专利文献7中公开了使用欧拉角基于非线性优化计算推定照相机位置和姿势的方法，所以将不给出其详细说明。

通过将记录有可以实现上述实施例功能的软件程序的程序代码的存储介质(或记录介质)提供给系统或设备，并由系统或设备的计算机(或CPU或MPU)读出并执行存储在该存储介质中的程序代码也可以实现本发明的目的。在此情况下，从存储介质中读出的程序代码本身实现上述实施例的功能，并且存储有该程序代码的存储介质构成本发明。此外，本发明包括以下情况：不仅通过计算机执行所读出的程序代码而且还通过运行在计算机上的操作系统(OS)基于该程序代码的指令执行的一些或全部实际处理来实现上述实施例的功能。

此外，本发明还包括以下情况：在将从存储介质中读出的程序代码写入插入在计算机中或连接到计算机的功能扩展板或功能扩展单元之后，由设置在该功能扩展板或功能扩展单元中的CPU等执行的一些或全部实际处理来实现上述实施例的功能。

当本发明被应用于上述存储介质时，该存储介质优先存储对应于上述流程图的程序代码。

本发明不局限于上述实施例，在本发明的精神和范围内可以进行各种改变和修改。因此，为了公开本发明的范围，给出了所附的权利要求。

本申请要求在2005年2月4日提交的日本专利申请2005-029827和2006年2月2日提交的日本专利申请2006-026177的优先权，其全部内容通过引用包含于此。

Claims (27)

1.一种用于测量测量对象物体的位置和姿势的位置/姿势测量方法，其特征在于，包括：

拍摄图像获取步骤，用于获取拍摄图像；

指标检测步骤，用于检测所述拍摄图像中所包括的指标；

分布范围计算步骤，用于计算与所述指标检测步骤中检测到的指标的分布范围有关的评价值；以及

位置/姿势计算步骤，用于使用与所述检测到的指标的图像坐标有关的信息和与所述分布范围有关的所述评价值来计算所述测量对象物体的位置和姿势。

2.根据权利要求1所述的位置/姿势测量方法，其特征在于，所述测量对象物体是摄像装置，以及

所述拍摄图像是所述摄像装置拍摄的图像。

3.根据权利要求1所述的位置/姿势测量方法，其特征在于，所述拍摄图像是由与所述测量对象物体的相对位置关系已知的摄像装置拍摄的图像。

4.根据权利要求1所述的位置/姿势测量方法，其特征在于，所述拍摄图像是由拍摄所述测量对象物体的摄像装置拍摄的图像。

5.根据权利要求1所述的位置/姿势测量方法，其特征在于，在所述位置/姿势计算步骤中，根据所述分布范围的所述评价值从多个计算方法中选择一个。

6.根据权利要求1所述的位置/姿势测量方法，其特征在于，与所述分布范围有关的所述评价值是与所述检测到的指标的三维分布范围有关的评价值。

7.根据权利要求1所述的位置/姿势测量方法，其特征在于，与所述分布范围有关的所述评价值是与所述检测到的指标在所述拍摄图像上的分布范围有关的评价值。

8.根据权利要求1所述的位置/姿势测量方法，其特征在于，在所述位置/姿势计算步骤中，根据与所述指标的所述分布范围有关的所述评价值来控制与所述检测到的指标的所述图像坐标有关的信息对计算所述测量对象物体的所述位置和姿势的影响。

9.根据权利要求1所述的位置/姿势测量方法，其特征在于，在所述位置/姿势计算步骤中，

利用与所述指标检测步骤中检测到的指标的图像坐标有关的所述信息，使用不同的方法来计算所述测量对象物体的第一位置/姿势和第二位置/姿势；以及

基于与所述分布范围计算步骤中计算出的所述指标的分布范围有关的所述评价值，从所述第一位置/姿势和所述第二位置/姿势计算所述测量对象物体的所述位置和姿势。

10.根据权利要求1所述的位置/姿势测量方法，其特征在于，还包括测量结果获取步骤，所述测量结果获取步骤用于从测量单元获取代表所述测量对象物体的位置和姿势的参数中的至少一个的测量结果，

其中，在所述位置/姿势计算步骤中，通过基于与所述指标的分布范围有关的所述评价值控制与所述指标的所述图像坐标有关的信息的影响和所述测量结果的影响，来计算所述测量对象物体的位置和姿势。

11.根据权利要求10所述的位置/姿势测量方法，其特征在于，所述测量单元是安装到所述测量对象物体的位置传感器和/或姿势传感器。

12.根据权利要求1所述的位置/姿势测量方法，其特征在于，还包括保持步骤，所述保持步骤用于保持代表所述测量对象物体的位置和姿势的、从前一帧的拍摄图像中计算出的参数中的至少一个，

其中，在所述位置/姿势计算步骤中，

通过基于与所述指标的分布范围有关的所述评价值控制与所述指标的所述图像坐标有关的信息的影响和代表所述测量对象物体的位置和姿势的、从所述前一帧拍摄图像中计算出的所述参数的影响，来计算所述测量对象物体的位置和姿势。

13.根据权利要求1所述的位置/姿势测量方法，其特征在于，还包括姿势测量值获取步骤，所述姿势测量值获取步骤用于从测量所述测量对象物体的姿势的姿势传感器获取姿势测量值，

其中，在所述位置/姿势计算步骤中，根据与所述指标的图像坐标有关的信息的信息量的大小来设置与所述指标的图像坐标有关的信息的影响和所述姿势测量值的影响，并计算所述摄像装置的位置和姿势。

14.根据权利要求3所述的位置/姿势测量方法，其特征在于，还包括姿势测量值获取步骤，所述姿势测量值获取步骤用于从测量所述测量对象物体的所述姿势的姿势传感器获取姿势测量值，

其中，在所述位置/姿势计算步骤中，使用与所述指标检测步骤中检测到的所述指标的所述图像坐标有关的信息的实际测量值和基于所述摄像装置的位置和姿势的推定值计算出的与所述图像坐标有关的信息的理论值之间的差异、以及所述推定值和所述姿势测量值的至少部分参数之间的差异二者作为评价基准，来计算所述摄像装置的所述位置和姿势。

15.一种用于测量测量对象物体的位置和姿势的位置/姿势测量方法，其特征在于，包括：

姿势测量值输入步骤，用于从安装到所述测量对象物体的姿势传感器输入姿势测量值；

拍摄图像获取步骤，用于获取拍摄图像；

指标检测步骤，用于检测所述拍摄图像中所包括的指标；以及

位置/姿势计算步骤，用于使用与所述检测到的指标的图像坐标有关的信息和所述姿势测量值来计算所述摄像装置的所述位置和姿势，

其中，在所述位置/姿势计算步骤中，

使用以下差异作为评价基准来计算所述摄像装置的位置和姿势，所述差异是：与所述检测到的指标的所述图像坐标有关的信息的实际测量值和基于所述摄像装置的所述位置和姿势的推定值计算出的与所述检测到的指标的所述图像坐标有关的信息的理论值之间的差异，以及

所述推定值和所述姿势测量值的至少部分参数之间的差异。

16.根据权利要求15所述的位置/姿势测量方法，其特征在于，所述测量对象物体是摄像装置，以及

所述拍摄图像是所述摄像装置拍摄的图像。

17.根据权利要求15所述的位置/姿势测量方法，其特征在于，所述拍摄图像是由与所述测量对象物体的相对位置关系已知的摄像装置拍摄的图像。

18.根据权利要求15所述的位置/姿势测量方法，其特征在于，所述拍摄图像是由拍摄所述测量对象物体的摄像装置拍摄的图像。

19.根据权利要求15所述的位置/姿势测量方法，其特征在于，还包括保持步骤，所述保持步骤用于保持代表所述测量对象物体的位置和姿势的、从前一帧拍摄图像计算出的参数中的至少一个，

其中，在所述位置/姿势计算步骤中，

使用以下差异作为评价机关枪来计算所述测量对象物体的位置和姿势，所述差异是：与所述指标的所述图像坐标有关的信息的实际测量值和基于与所述检测到的指标的所述图像坐标有关的信息的实际测量值和所述摄像装置的所述位置和姿势的推定值计算出的与所述检测到的指标的所述图像坐标有关的信息的理论值之间的差异，

所述推定值和所述姿势测量值的至少部分参数之间的差异以及所述推定值和代表由所述保持部件所保持的前一位置和姿势的参数中的至少一个之间的差异；以及

所述推定值和代表位置和姿势的从所述前一帧拍摄图像计算出的参数中的至少一个之间的差异。

20.一种用于测量作为测量对象物体的摄像装置的位置和姿势的位置/姿势测量方法，其特征在于，包括：

姿势测量值输入步骤，用于从安装到所述摄像装置的姿势传感器输入姿势测量值；

图像输入步骤，用于输入由所述摄像装置拍摄的图像；

指标检测步骤，用于检测包括在所述拍摄图像中的指标；

权重设置步骤，用于设置由操作者输入的权重；以及

位置/姿势计算步骤，用于使用与所述指标检测步骤中检测到的所述指标的图像坐标有关的信息以及所述姿势测量值来计算所述摄像装置的位置和姿势，

其中，在所述位置/姿势计算步骤中，通过根据在所述权重设置步骤中设置的权重改变与所述指标的所述图像坐标有关的信息和所述姿势测量值的影响来计算所述摄像装置的位置和姿势。

21.根据权利要求20所述的位置/姿势测量方法，其特征在于，在所述位置/姿势计算步骤中，根据在所述权重设置步骤中设置的权重对以下差异中的至少一个进行加权，所述差异是：与所述指标的所述图像坐标有关的信息的实际测量值和基于所述摄像装置的位置和姿势的所述推定值计算出的与所述图像坐标有关的信息的理论值之间的差异，以及所述推定值和所述姿势测量值的至少部分参数之间的差异，并使用所述差异作为评价基准来计算所述摄像装置的位置和姿势。

22.一种用于测量作为测量对象物体的摄像装置的位置和姿势的位置/姿势测量方法，其特征在于，包括：

姿势测量值输入步骤，用于从安装到所述摄像装置的姿势传感器输入姿势测量值；

图像输入步骤，用于输入由所述摄像装置拍摄的图像；

指标检测步骤，用于检测包括在所述拍摄图像中的指标；

模式选择步骤，用于从以下模式中选择一个：重视与所述指标的图像坐标有关的信息的第一模式，重视所述姿势测量值的第二模式，以及作为所述第一模式和所述第二模式之间的中间模式的第三模式；以及

位置/姿势计算步骤，用于通过使用与所述指标检测步骤中检测到的所述指标的图像坐标有关的信息以及所述姿势测量值、应用根据所述模式选择步骤中选择的模式的方法，来计算所述摄像装置的位置和姿势。

23.根据权利要求22所述的位置/姿势测量方法，其特征在于，在所述位置/姿势计算步骤中，

当在所述模式选择步骤中选择所述第一模式时，基于与在所述指标检测步骤中检测到的所述指标的所述图像坐标有关的信息来计算所述摄像装置的位置和姿势，而不使用所述姿势测量值作为规定值或评价基准，

当在所述模式选择步骤中选择所述第三模式时，使用以下两种差异作为评价基准来计算所述摄像装置的位置和姿势，所述差异是：与在所述指标检测步骤中检测到的所述指标的所述图像坐标有关的信息的实际测量值和基于所述摄像装置的位置和姿势的所述推定值计算出的与所述图像坐标有关的信息的理论值之间的差异，以及所述推定值和所述姿势测量值的至少部分参数之间的差异，以及

当在所述模式选择步骤中选择所述第二模式时，基于与所述指标检测步骤中检测到的所述指标的所述图像坐标有关的信息，并设置所述姿势测量值的至少部分参数作为规定值来计算所述摄像装置的位置和姿势的未知参数。

24.一种用于使计算机执行根据权利要求1～23中任何一项所述的位置/姿势测量方法的控制程序。

25.一种用于存储根据权利要求24所述的控制程序的计算机可读记录介质。

26.一种用于测量测量对象物体的位置和姿势的位置/姿势测量设备，其特征在于，包括：

拍摄图像获取部件，用于获取拍摄图像；

指标检测部件，用于检测所述拍摄图像中所包括的指标；

分布范围计算部件，用于计算与所述指标检测部件检测到的指标的分布范围有关的评价值；以及

位置/姿势计算部件，用于使用与所述检测到的指标的图像坐标有关的信息和与所述分布范围有关的所述评价值来计算所述测量对象物体的位置和姿势。

27.一种用于测量测量对象物体的位置和姿势的位置/姿势测量设备，其特征在于，包括：

姿势测量值输入部件，用于从安装到所述测量对象物体的姿势传感器输入姿势测量值；

拍摄图像获取部件，用于获取拍摄图像；

指标检测部件，用于检测所述拍摄图像中所包括的指标；以及

位置/姿势计算部件，用于使用与所述检测到的指标的图像坐标有关的信息和所述姿势测量值来计算所述摄像装置的所述位置和姿势，

其中，所述位置/姿势计算部件使用以下差异作为评价基准来计算所述摄像装置的位置和姿势，所述差异是：

与所述检测到的指标的所述图像坐标有关的信息的实际测量值和基于所述摄像装置的所述位置和姿势的推定值计算出的与所述检测到的指标的所述图像坐标有关的信息的理论值之间的差异，以及

所述推定值和所述姿势测量值的至少部分参数之间的差异。

Images