CN1517902A - 位置姿势测量方法、位置姿势测量装置 - Google Patents

Abstract

提供一种位置姿势测量方法。求出第1标识的图像坐标和第1标识的推测图像坐标的第1误差坐标，该第1标识的图像坐标配置于现实空间中，在第1摄像单元所拍摄的第1图像上；该第1标识的推测图像坐标，根据具有按照以前计算出的位置姿势参数的位置姿势的第1摄像机构和第1标识的位置姿势关系，被推测为位于第1图像上。另一方面，求出第2标识的图像坐标和第2标识的推测图像坐标的第2误差坐标，该第2标识的图像坐标配置于第1摄像机构中，在包含第1摄像机构的第2图像上；该第2标识的推测图像坐标，根据具有按照上述位置姿势参数的位置姿势的第1摄像机构和第2标识的位置姿势关系，被推测为位于第2图像上。用第1、第2误差坐标来修正上述位置姿势参数。

Description

位置姿势测量方法、位置姿势测量装置

技术领域

本发明涉及测量摄像装置或安装了摄像装置的对象物体的位置和姿势的技术。

背景技术

近年来，广泛进行着以现实空间和假想空间的无缝结合为目的的、关于复合现实感的研究。进行复合现实感的呈示的图像显示装置通过视频透视(video see-through)方式来实现，该视频透视方式为：在由摄像机等摄像装置所拍摄的现实空间的图像上，重叠描绘依照摄像装置的位置和姿势所生成的假想空间(例如通过计算机图形所描绘的假想物体或字符信息等)的图像，并将其显示。

另外除此之外还可通过光学透视方式来实现，该光学透视方式为：在安装于观察者头部的光学透视型显示器上，显示依照观察者的视点的位置和姿势所生成的假想空间的图像。

作为这样的图像显示装置的应用，期待在患者的身体表面重叠显示体内的情况的手术辅助、与漂浮在现实空间中的假想敌作战的复合现实感游戏等与迄今为止的虚拟现实所不同的新领域。

对于这些应用所共同要求的是如何精确地进行现实空间与假想空间之间的位置对合(alignment)，以往已进行了许多尝试。复合现实感中的位置对合问题在视频透视方式下归结为求得场景(scene)中(即世界坐标系中)的摄像装置的位置和姿势的问题。同样地，在光学透视方式下，归结为求得场景中的观察者的视点或显示器的位置和姿势的问题。

作为解决前一问题的方法，一般是在场景中配置或设定多个标识(indicator)，通过检测出在摄像装置所拍摄的图像内的标识的投影图像的坐标，来求得场景中的该摄像装置的位置和姿势。另外，作为解决后一问题的方法，一般是在测量对象物(即观察者的头部或显示器)上安装摄像装置，通过与前者相同的方法来求得该摄像装置的位置和姿势，基于此来求得测量对象物的位置和姿势。

使用图1，对通过从摄像装置所拍摄的图像中检测出标识来测量摄像装置的位置和姿势的位置姿势测量装置的以往例进行说明。图1是表示以往的位置姿势测量装置的结构的图。如该图所示那样，该以往例中的位置姿势测量装置100由主观视点标识检测单元110和位置姿势计算单元120构成，并连接到摄像装置130。

另外，在现实空间中的多个位置上，配置有世界坐标系(将现实空间的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)中位置为已知的多个主观视点标识Q^k(k＝1，，，K₁)，作为用于由摄像装置130所拍摄的标识(下面称为主观视点标识)。最好是对主观视点标识Q^k进行设置，以便在使摄像装置130位于将要测量位置和姿势的测量对象范围内的各个地点时，在由摄像装置130所取得的图像上，至少3个标识始终被观测到。图1的例子表示配置有4个主观视点标识Q¹、Q²、Q³、Q⁴，其中3个Q¹、Q³、Q⁴被包含在摄像装置130的视场内的情况。

主观视点标识Q^k例如既可以由各自具有不同颜色的圆形标记构成，也可以由各自具有不同纹理特征的自然特征等的特征点来构成。只要是可检测摄影图像上的投影图像的图像坐标的，且可用某种方法识别是哪个标识这样的标识，则任何形式都可以。

摄像装置130输出的图像(下面将此称为主观视点图像)被输入给位置姿势测量装置100。

主观视点标识检测单元110从摄像装置130输入主观视点图像，检测被拍摄在所输入的图像中的主观视点标识Q^k的图像坐标。例如，在各主观视点标识Q^k由具有不同颜色的标记构成时，从主观视点图像上检测出与各标记颜色对应的区域，并将其重心位置设为标识的检测坐标。另外，各主观视点标识Q^k由具有不同纹理特征的特征点构成时，通过在主观视点图像上实施利用作为已知信息被预先保持的各标识的模板图像的模板匹配，来检测出标识的位置。

进而，主观视点标识检测单元110将检测出的各主观视点标识Q^kn的图像坐标u^Qkn及其标识符k_n输出给位置姿势计算单元120。此处，n(n＝1，，，N)是对于检测出的各标识的索引，N表示检测出的标识的总数。例如在图1的情况下N＝3，输出标识符k₁＝1、k₂＝3、k₃＝4和与它们对应的图像坐标u^Qk1、u^Qk2、u^Qk3。

位置姿势计算单元120基于检测出的各主观视点标识Q^kn的图像坐标u^Qkn，和作为已知信息被预先保持的标识的世界坐标X_W ^Qkn的对应关系，计算出摄像装置130的位置和姿势。

从主观视点标识的世界坐标和图像坐标的组计算出摄像装置的位置和姿势的方法，在照相测量等领域中很早以来就得以提案(例如，参照R.M.Haralick，C.Lee，K.Ottenberg，and M.Nolle：Review andanalysis of solutions of the three point perspective pose estimationproblem，International Journal of Computer Vision，vol.13，no.3，pp.311-356，1994.和D.G.Lowe：Fitting parameterized three-dimensionalmodels to images，IEEE Transactions on PAMI，vol.13，no.5pp.441-450，1991.)。

用图2的流程来说明本以往例中的位置姿势计算单元120进行的处理。在本以往例中，摄像装置130的位置和姿势通过利用递归运算的数值计算方法来计算出。

位置姿势计算单元120分别通过3值向量[x y z]^T和[ξψζ]^T来内部地表达待计算的摄像装置130的位置和姿势。通过3值来表达姿势的方法有各种各样，在此采用通过向量大小来定义旋转角、通过向量的方向来定义旋转轴方向这样的通过3值向量来表达的方法。此时，待求得的未知参数被记作6值状态向量s＝[x y z ξψζ]^T。

在步骤S2000中，位置姿势计算单元120在状态向量s中设定适当的初始值(位置和姿势的推测值)。例如，既可以将在前一帧(时刻t_k-1)处理中所导出的s作为初始值，也可以设定通过基于来自过去的s的导出值的变化的预测而得到的值。

在步骤S2010中，位置姿势计算单元120输入在主观视点标识检测单元110中检测出的各主观视点标识Q^kn的图像坐标的实测值u^Qkn及其标识符k_n。

在步骤S2020中，位置姿势计算单元120对各主观视点标识Q^kn计算出其图像坐标的推测值u^Qkn*。u^Qkn*的计算，基于作为已知信息被预先保持的各标识Q^kn的世界坐标X_W ^Qkn和当前的状态向量s的函数：

[式1]

$u^{{Q_{k_n}}^{*}}=F_C(x_W^{Q_{k_n}},s)$

来进行。

具体来讲，就是函数F_C()由从X_W ^Qkn和s求得该标识的摄像机坐标(将摄像机上的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)X_C ^Qkn的下式

[式2]

$x_C^{Q_{k_n}}={\left[\begin{array}{cccc}{x}_C^{Q_{k_n}}& y_C^{Q_{k_n}}& z_C^{Q_{k_n}}& 1\end{array}\right]}^T=M_{\mathrm{WC}}{\left(s\right)}^{-1}\·x_W^{Q_{k_n}}$

和从摄像机坐标X_C ^Qkn求得图像坐标u^Qkn*的下式[式3]

$u^{{Q_{k_n}}^{*}}={\left[\begin{array}{cc}{u}_x^{{Q_{k_n}}^{*}}& u_y^{{Q_{k_n}}^{*}}\end{array}\right]}^T={\left[\begin{array}{cc}-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}}}\end{array}\right]}^T$

构成。此处f^C _x和f^C _y分别是x轴方向和y轴方向上的摄像装置130的焦点距离，作为已知值被预先保持。另外，M_WC(s)是由s确定的建模变换矩阵(即，将摄像机坐标系中的坐标变换成世界坐标系中的坐标的矩阵)，由下式来定义。

[式4]

$M_{\mathrm{WC}}\left(s\right)=\left[\begin{array}{cccc}\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{\θ}& x\\ \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{\θ}& y\\ \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{\θ}& z\\ 0& 0& 0& 1\end{array}\right]$

其中， $\mathrm{\θ}=\sqrt{{\mathrm{\ξ}}^2+{\mathrm{\ψ}}^2+{\mathrm{\ζ}}^2}.$

在步骤S2030中，位置姿势计算单元120对各主观视点标识Q^kn，通过下式计算出图像坐标的推测值u^Qkn*和实测值u^Qkn的误差Δu^Qkn。[式5]

$\mathrm{\Δ}{u}^{Q_{k_n}}=u^{Q_{k_n}}-u^{{Q_{k_n}}^{*}}$

在步骤S2040中，位置姿势计算单元120对各主观视点标识Q^kn，计算出作为各元素具有用状态向量s的各元素对式1的函数F_C()进行了偏微分的解的2行×6列的雅可比矩阵J_us ^Qkn(＝u/s)。具体来讲，就是计算出作为各元素具有用摄像机坐标X_C ^Qkn的各元素对式3的右边进行了偏微分的解的2行×3列的雅可比矩阵J_ux ^Qkn(＝u/x)，和作为各元素具有用向量s的各元素对式2的右边进行了偏微分的解的3行×6列的雅可比矩阵J_xs ^Qkn(＝x/s)，并通过下式计算出J_us ^Qkn。

[式6]

$J_{\mathrm{us}}^{Q_{k_n}}=J_{\mathrm{ux}}^{Q_{k_n}}\·J_{\mathrm{xs}}^{Q_{k_n}}$

在步骤S2050中，位置姿势计算单元120基于在步骤S2030和步骤S2040中计算出的误差Δu^Qkn和雅可比矩阵J_us ^Qkn，计算出状态向量s的修正值Δs。具体来讲，就是生成垂直排列了误差Δu^Qkn的2N维误差向量，

[式7]

$U=\left[\begin{array}{c}\mathrm{\Δ}{u}^{Q_{k_1}}\\ \underset{.}{\overset{.}{.}}\\ \mathrm{\Δ}{u}^{Q_{k_N}}\end{array}\right]$

和垂直排列了雅可比矩阵J_us ^Qkn的2N行×6列的矩阵

[式8]

$\mathrm{\Φ}=\left[\begin{array}{c}{J}_{\mathrm{us}}^{Q_{k_1}}\\ \underset{.}{\overset{.}{.}}\\ J_{\mathrm{us}}^{Q_{k_N}}\end{array}\right]$

并用Φ的伪逆矩阵Φ’，计算出

[式9]

ΔS＝Φ’U

在图1的例子中N＝3，所以U是6维向量，Φ是6行×6列的矩阵。

在步骤S2060中，位置姿势计算单元120使用在步骤S2050中计算出的修正值Δs，按照式10修正状态向量s，并将得到的值设为s的新推测值。

[式10]

S+ΔS→S

在步骤S2070中，位置姿势计算单元120使用误差向量U是否比预先确定的阈值小，或者修正值Δs是否比预先确定的阈值小这样的一些判断标准来判断计算是否收敛。如果不收敛，则用修正后的状态向量s再次进行步骤S2020以后的处理。

在步骤S2080中，位置姿势计算单元120将得到的状态向量s作为摄像装置130的位置和姿势信息进行输出。位置和姿势信息，例如用从s计算出的观察变换矩阵的形式来进行输出。

以往就是通过以上方法来取得摄像装置130的位置和姿势的。

另外，还可以在测量对象物体上设定多个标识，由设置在外部的客观视点摄像机来拍摄对象物体，并检测出所拍摄的客观视点图像内的标识的投影图像的图像坐标，由此求出对象物体的位置和姿势(例如，参照D.G.Lowe：Fitting parameterized three-dimensional models toimages，IEEE Transactions on PAMI，vol.13，no.5 pp.441-450，1991.和日本专利申请公开特开平09-323280号公报)。

但是，在从由作为测量对象的摄像装置得到的主观视点图像检测场景中的标识的前一方法中，与姿势的变化相比对于位置的变化的在图像上的标识的移动量要小，所以存在不能充分获得位置测量的精度的问题。另外，在欲实现宽广范围的环视时，就存在必须在对应的宽广范围区域内设定多个标识的问题。

另一方面，在从由客观视点摄像机得到的客观视点图像中检测测量对象物体上的标记(marker)的后一方法中，与位置的变化相比对于姿势的变化的在图像上的标识的移动量要小，所以存在不能充分获得姿势测量的精度的问题。

发明内容

本发明就是鉴于以上问题而完成的，以对于位置和姿势的任何一个，都能够以高的精度来实现对象物体的位置和姿势的测量为目的。

为了达到此目的，本发明的第1技术方案提供一种计算对象物体的位置和姿势的位置姿势测量方法，包括：主观视点取得步骤，取得第1图像，该第1图像是通过安装在上述对象物体上的主观视点摄像单元拍摄场景而得到的；客观视点图像取得步骤，取得第2图像，该第2图像是通过客观视点摄像单元拍摄上述对象物体而得到的；第1检测步骤，从上述第1图像中检测出与配置在上述场景中的第1标识的图像坐标相关的特征量；第2检测步骤，从上述第2图像中检测出与配置在上述对象物体上或上述主观视点摄像单元上的第2标识的图像坐标相关的特征量；以及位置姿势计算步骤，用在上述第1检测步骤中检测出的与上述第1标识的图像坐标相关的特征量，和在上述第2检测步骤中检测出的与上述第2标识的图像坐标相关的特征量，计算出上述对象物体的位置和姿势。

进而，本发明的第2技术方案提供一种位置姿势测量方法，计算安装在拍摄现实空间的第1摄像单元中，与该第1摄像单元的位置姿势关系已知的对象物体的第1位置姿势参数，包括：第1图像坐标检测步骤，从由上述第1摄像单元所拍摄的第1图像中检测出配置在上述现实空间中的第1标识的图像坐标；第1推测图像坐标检测步骤，在上述第1摄像单元的位置和姿势遵照以前计算出的第2位置姿势参数这样的假定的基础上，根据上述第1摄像单元和上述第1标识的位置姿势关系，来求得上述第1标识的推测图像坐标；第1误差坐标计算步骤，求得上述第1图像上的、在上述第1图像坐标检测步骤中检测出的图像坐标和在上述第1推测图像坐标检测步骤中计算出的推测图像坐标的第1误差坐标；第2图像坐标检测步骤，从由第2摄像单元所拍摄的、包含上述对象物体的第2图像中，检测出配置在上述第1摄像单元或上述对象物体中的第2标识的图像坐标；第2推测图像坐标检测步骤，在上述第1摄像单元的位置和姿势遵照上述以前计算出的第2位置姿势参数这样的假定的基础上，根据上述第1摄像单元和上述第2标识的位置姿势关系，来求得上述第2标识的推测图像坐标；第2误差坐标计算步骤，求得上述第2图像上的、在上述第2图像坐标检测步骤中检测出的图像坐标和在上述第2推测图像坐标检测步骤中计算出的推测图像坐标的第2误差坐标；修正值计算步骤，用上述第1误差坐标和上述第2误差坐标，求得用于修正上述以前计算出的第2位置姿势参数的修正值；修正步骤，用上述修正值修正上述以前计算出的第2位置姿势参数；以及参数计算步骤，用在上述修正步骤中所修正的上述第2位置姿势参数，求得与上述第1摄像单元有已知的位置姿势关系的上述对象物体的上述第1位置姿势参数。

进而，本发明的第3技术方案提供一种位置姿势测量方法，将拍摄场景的摄像装置作为测量对象物体，计算出该对象物体的位置和姿势，包括：检测步骤，从由上述摄像装置所取得的图像中，检测出与配置在上述场景中的第1标识的图像坐标相关的特征量；测量步骤，测量配置在上述摄像装置中的第2标识的位置；以及位置姿势计算步骤，用在上述检测步骤中检测出的与上述第1标识的图像坐标相关的特征量，和在上述测量步骤中测量出的上述第2标识的位置，计算出上述摄像装置的位置和姿势。

进而，本发明的第4技术方案提供一种计算对象物体的位置和姿势的位置姿势测量装置，包括：主观视点摄像单元，安装在上述对象物体上来拍摄场景；客观视点摄像单元，拍摄上述对象物体；第1检测单元，从由上述主观视点摄像单元所取得的第1图像中，检测出与配置在上述场景中的第1标识的图像坐标相关的特征量；第2检测单元，从由上述客观视点摄像单元所取得的第2图像中，检测出与配置在上述对象物体上或上述主观视点摄像单元上的第2标识的图像坐标相关的特征量；以及位置姿势计算单元，用上述第1检测单元检测出的与上述第1标识的图像坐标相关的特征量，和上述第2检测单元检测出的与上述第2标识的图像坐标相关的特征量，计算出上述对象物体的位置和姿势。

本发明的其他特征以及优点，通过以附图为参照的下面的说明将会弄明白。此外，在附图中对相同或相似的结构附加相同的参照标号。

附图说明

附图包含在说明书中，构成其一部分，表示本发明的实施形式，并与说明书的记述一起用于说明本发明的原理。

图1是表示以往的位置姿势测量装置的结构的图。

图2是以往例中的位置姿势计算单元120进行的处理的流程图。

图3是表示本发明的第1实施形式的位置姿势测量装置的概略结构的图。

图4是通过CPU1001执行位置姿势计算单元320的软件程序而进行的、计算表示摄像装置130的位置和姿势的参数的处理的流程图。

图5是表示本发明的第2实施形式的位置姿势测量装置的概略结构的图。

图6是表示本发明的第3实施形式的位置姿势测量装置的概略结构的图。

图7是表示本发明的第4实施形式的位置姿势测量装置的概略结构的图。

图8是表示本发明的第6实施形式的位置姿势测量装置的概略结构的图。

图9是通过CPU1001执行位置姿势计算单元820的软件程序而进行的、计算表示摄像装置130的位置和姿势的参数的处理的流程图。

图10是表示计算机的基本结构的图。

图11是通过CPU1001执行位置姿势计算单元320’的软件程序而进行的、计算表示摄像装置130的位置和姿势的参数的处理的流程图。

图12是表示本发明的第7实施形式的位置姿势测量装置的概略结构的图。

图13是表示本发明的第8实施形式的位置姿势测量装置的概略结构的图。

具体实施形式

下面参照附图对本发明的优选实施形式进行详细说明。

[第1实施形式]

本实施形式的位置姿势测量装置进行摄像装置的位置和姿势的测量。下面，说明本实施形式的位置姿势测量装置和位置姿势测量方法。

图3是表示本实施形式的位置姿势测量装置的概略结构的图。对于与图1相同的部分，附加相同的编号、记号，并省略其说明。如图3所示那样，本实施形式的位置姿势测量装置300由主观视点标识(viewpoint indicator)检测单元110、客观视点标识检测单元310、位置姿势计算单元320和客观视点摄像机330构成，并连接到作为测量对象的摄像装置130。

在摄像装置130位于测量对象范围内时，客观视点摄像机330被固定配置于可以拍摄摄像装置130的位置。这里，设世界坐标系中的客观视点摄像机330的位置和姿势，作为已知的值被预先保持在客观视点标识检测单元310中。

在现实空间中的多个位置上，与上述以往例一样，设定了在世界坐标系中的位置为已知的主观视点标识Q^k，作为用于由摄像装置130进行拍摄的标识。另外，在摄像装置130的多个位置上，设定了在主观视点摄像机坐标系(将摄像装置130上的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)上的位置为已知的客观视点标识P^k(k＝1，，，K₂)，作为用于由客观视点摄像机330进行拍摄的标识(下面称为客观视点标识)。

最好是，这样来设置这些标识，以便在使摄像装置130位于将要测量位置和姿势的测量对象范围内的各个地点时，在由摄像装置130所取得的图像上被观测到的主观视点标识，和在由客观视点摄像机330所取得的图像上被观测到的客观视点标识的总数，始终至少为3个。图3的例子表示设定了3个主观视点标识Q¹、Q²、Q³和2个客观视点标识P¹、P²，其中2个主观视点标识Q¹、Q³包含在摄像装置130的视场内，2个客观视点标识P¹、P²包含在客观视点摄像机330的视场内的情况。

另外，客观视点标识P^k和主观视点标识Q^k一样，只要是可检测出摄影图像上的投影图像的图像坐标，并且可识别出是哪个标识这样的标识，则任何形式都可以。另外，主观视点标识和客观视点标识都既可以特意地进行设定，也可以不是特意地进行设定，例如也可以，如上述那样使用自然形状。

主观视点标识检测单元110与上述以往例一样，输入摄像装置130所拍摄的主观视点图像，检测被拍摄在图像中的主观视点标识Q^k的图像坐标，并将检测出的主观视点标识Q^kn的图像坐标u^Qkn及其标识符k_n输出给位置姿势计算单元320。例如，在图3的情况下，是N＝2，输出标识符k₁＝1、k₂＝3和与它们对应的图像坐标u^Qk1、u^Qk2。

客观视点标识检测单元310输入客观视点摄像机330所拍摄的图像(下面称为客观视点图像)，通过与主观视点标识检测单元110同样的处理，检测被拍摄在图像中的客观视点标识P^k的图像坐标，并将检测出的客观视点标识P^km的图像坐标u^Pkm及其标识符k_m输出给位置姿势计算单元320。此处，m(m＝1，，，M)是附加给检测出的各标识的索引，M表示检测出的标识的总数。例如，在图3的情况下，为M＝2，输出标识符k₁＝1、k₂＝2和与它们对应的图像坐标u^Pk1、u^Pk2。

位置姿势计算单元320基于检测出的各主观视点标识Q^kn的图像坐标u^Qkn与作为已知信息而预先保持的标识的世界坐标X_W ^Qkn的对应应关系，以及检测出的各个客观视点标识P^kn的图像坐标u^Pkm与作为已知信息而预先保持的主观视点摄像机坐标系上的标识的位置X_C ^Pkm的对应关系，计算出表示摄像装置130的位置和姿势的参数。据此，就能够进行摄像装置130的位置和姿势的测量。

另外，图3所示的客观视点标识检测单元310、位置姿势计算单元320、主观视点标识检测单元110既可以分别作为独立的装置来处理，也可以分别作为软件安装在1个或多个计算机中，并由各个计算机的CPU进行执行，由此来实现其功能。在本实施形式中，设各单元(客观视点标识检测单元310、位置姿势计算单元320、主观视点标识检测单元110)作为在1台计算机内成为执行对象的软件来处理。

图10是表示将客观视点标识检测单元310、位置姿势计算单元320、主观视点标识检测单元110分别作为软件来执行的计算机的基本结构的图。

1001是CPU，使用存储在RAM1002和ROM1003中的程序和数据，进行计算机整体的控制，并且控制客观视点标识检测单元310、位置姿势计算单元320、主观视点标识检测单元110各自软件的执行，以实现各单元的功能。

1002是RAM，具有暂时存储从外部存储装置1007和存储介质驱动1008装入的程序和数据的区域，并且具有CPU1001进行各种处理所必需的工作区。

1003是ROM，一般保存着计算机的引导程序和设定数据等。1004、1005分别是键盘、鼠标，操作者可以用它们对CPU1001输入各种指示。

1006是显示单元，由CRT或液晶画面等构成，例如。能够显示为了摄像装置130的位置姿势测量而应显示的消息等。

1007是外部存储装置，是作为硬盘等大容量信息存储装置而起作用的装置，将分别作为OS(操作***)、客观视点标识检测单元310、位置姿势计算单元320、主观视点标识检测单元110而起作用的软件的程序等保存于此。另外，在本实施形式的说明中，说明为已知的信息也保存于此。

1008是存储介质驱动，按照来自CPU1001的指示读出存储在CD-ROM或DVD-ROM等存储介质中的程序和数据，并输出给RAM1002和外部存储装置1007。

1009是I/F，用于连接摄像装置130、客观视点摄像机330，它们各自所拍摄的图像经由I/F1009被取入到RAM1002。1010是联结上述各单元的总线。

图4是通过CPU1001执行位置姿势计算单元320的软件程序而进行的、计算表示摄像装置130的位置和姿势的参数的处理的流程图。另外，假定在进行以下处理之前，客观视点标识检测单元310、位置姿势计算单元320、主观视点标识检测单元110各自的软件程序已经被装入到RAM1002中。

在位置姿势计算单元320中，与上述以往例中的位置姿势计算单元120一样，将待计算的摄像装置130的位置和姿势分别作为3值向量[x y z]^T和[ξψ ζ]^T来处理。此时，待求得的参数被表示为6值状态向量s＝[x y z ξ ψ ζ]^T。

在步骤S4000中，位置姿势计算单元320对状态向量s设定适当的初始值(位置和姿势的推测值)。例如，既可以将在前一帧(时刻t_k-1)处理中所导出的s作为初始值，也可以设定通过基于来自过去的s的导出值的变化的预测所得到的值。

在步骤S4010中，位置姿势计算单元320输入在主观视点标识检测单元110中检测出的各主观视点标识Q^kn的图像坐标u^Qkn及其标识符k_n。

在步骤S4020中，位置姿势计算单元320输入在客观视点标识检测单元310中检测出的各客观视点标识P^km的图像坐标u^Pkm及其标识符k_m。

在步骤S4030中，位置姿势计算单元320判断所输入的标识的总数(即N+M)是否为3个以上。如果标识总数不足3个，则不进行由步骤S4040到步骤S4120所实现的s的更新处理而进入步骤S4130。

在步骤S4040中，位置姿势计算单元320对各个主观视点标识Q^kn，基于式1～式4计算出其图像坐标的推测值u^Qkn*。即，假定为摄像装置130的位置和姿势遵照以前所求得的状态向量s，并根据摄像装置130和主观视点标识的位置姿势关系来求得各个主观视点标识的图像坐标的推测值。

在步骤S4050中，位置姿势计算单元320对各个主观视点标识Q^kn，基于式5计算出图像坐标的推测值u^Qkn*和实测值u^Qkn的误差Δu^Qkn。

在步骤S4060中，位置姿势计算单元320对各个主观视点标识Q^kn，基于式6计算出作为各元素具有用状态向量s的各元素对式1的函数F_C()进行了偏微分的解的2行×6列的雅可比矩阵J_us ^Qkn(＝u/s)。

在步骤S4070中，位置姿势计算单元320，对各个客观视点标识P^km，计算出其图像坐标的推测值u^Pkm*。即，假定为摄像装置130的位置和姿势遵照以前所求得的状态向量s，并根据摄像装置130和客观视点标识的位置姿势关系来求得各个客观视点标识的图像坐标的推测值。

u^Pkm*的计算，基于作为已知信息而预先保持的标识P^km各个摄像机坐标(摄像机坐标系中的坐标值)X_C ^Pkm和当前的状态向量s的函数来进行。

[式11]

$u^{{P_{k_m}}^{*}}=F_B(x_C^{P_{k_m}},s)$

具体来讲，函数F_B()由从X_C ^Pkm和s求得客观视点摄像机坐标系(将客观视点摄像机330上的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)上的该标识的坐标X_B ^Pkm的下式，

[式12]

$x_B^{P_{k_m}}={\left[\begin{array}{cccc}{x}_B^{P_{k_m}}& y_B^{P_{k_m}}& z_B^{P_{k_m}}& 1\end{array}\right]}^T=M_{\mathrm{WB}}^{-1}\·M_{\mathrm{WC}}\left(s\right)\·x_C^{P_{k_m}}$

和从摄像机坐标X_B ^Pkm求得图像坐标u^Pkm*的下式

[式13]

$u^{{P_{k_m}}^{*}}={\left[\begin{array}{cc}{u}_x^{{P_{k_m}}^{*}}& u_y^{{P_{k_m}}^{*}}\end{array}\right]}^T={\left[\begin{array}{cc}-f_x^B\frac{x_B^{P_{k_m}}}{z_B^{P_{k_m}}}& -f_y^B\frac{y_B^{P_{k_m}}}{z_B^{P_{k_m}}}\end{array}\right]}^T$

构成。此处f^B _x和f^B _y分别是x轴方向和y轴方向上的客观视点摄像机330的焦点距离，并作为已知的值被预先保持。另外，M_WB是将客观视点摄像机坐标系中的坐标变换成世界坐标的变换矩阵，并基于作为已知的值被预先保持的世界坐标系中的客观视点摄像机330的位置和姿势，而预先计算出。

在步骤S4080中，位置姿势计算单元320对各个主观视点标识P^km，通过下式计算出图像坐标的推测值u^Pkm*和实测值u^Pkm的误差Δu^Pkm。

[式14]

$\mathrm{\Δ}{u}^{P_{k_m}}=u^{P_{k_m}}-u^{{P_{k_m}}^{*}}$

作为各元素具有用状态向量s的各元素对式1的函数F_C()进行了偏微分的解

在步骤S4090中，位置姿势计算单元320对各个主观视点标识P^km，计算出作为各元素具有用状态向量s的各元素对式11的函数F_B()进行了偏微分的解的2行×6列的雅可比矩阵J_us ^Pkm(＝u/s)。具体来讲，就是计算出作为各元素具有用摄像机坐标X_B ^Pkm的各元素对式13的右边进行了偏微分的解的2行×3列的雅可比矩阵J_ux ^Pkm(＝u/x)，和作为各元素具有用向量s的各元素对式12的右边进行了偏微分的解的3行×6列的雅可比矩阵J_xs ^Pkm(＝x/s)，并通过下式计算出J_us ^Pkm。

[式15]

$J_{\mathrm{us}}^{P_{k_m}}=J_{\mathrm{ux}}^{P_{k_m}}\·J_{\mathrm{xs}}^{P_{k_m}}$

在步骤S4100中，位置姿势计算单元320基于在以上步骤中计算出的误差Δu^Qkn、雅可比矩阵J_us ^Qkn、误差Δu^Pkm和雅可比矩阵J_us ^Pkm，来确定误差向量U和矩阵Φ，通过式9计算出状态向量s的修正值Δs。此时，将误差向量U设定为垂直排列了误差Δu^Qkn和Δu^Pkm的2(N+M)维向量，

[式16]

$U=\left[\begin{array}{c}{\mathrm{\Δu}}^{Q_{k_1}}\\ \underset{.}{\overset{.}{.}}\\ {\mathrm{\Δu}}^{Q_{k_N}}\\ \mathrm{\Δ}{u}^{P_{k_1}}\\ \underset{.}{\overset{.}{.}}\\ \mathrm{\Δ}{u}^{P_{k_M}}\end{array}\right]$

另外，将矩阵Φ设定为垂直排列了雅可比矩阵J_us ^Qkn和J_us ^Pkm的2(N+M)行×6列的矩阵。在图3的例子中，由于M＝2、N＝2，所以U是8维向量，Φ是8行×6列的矩阵。

[式17]

$\mathrm{\Φ}=\left[\begin{array}{c}{J}_{\mathrm{us}}^{Q_{k_1}}\\ \underset{.}{\overset{.}{.}}\\ J_{\mathrm{us}}^{Q_{k_N}}\\ J_{\mathrm{us}}^{P_{k_1}}\\ \underset{.}{\overset{.}{.}}\\ J_{\mathrm{us}}^{P_{k_M}}\end{array}\right]$

在步骤S4110中，位置姿势计算单元320用在步骤S4100中计算出的修正值Δs，通过式10对状态向量s，即表示摄像装置130的位置和姿势的参数进行修正，并将所得到的值设为s的新推测值。

在步骤S4120中，位置姿势计算单元320使用，误差向量U是否比预先确定的阈值小，或者修正值Δs是否比预先确定的阈值小这样的一些判断标准，来判断计算是否收敛。如果不收敛，则用修正后的状态向量s，再次进行步骤S4040以后的处理。

若在步骤S4120中判断为计算已收敛，则在步骤S4130中，位置姿势计算单元320将所得到的状态向量s作为表示摄像装置130的位置和姿势的参数进行输出。此外，输出的形式既可以是将s中的姿势成分变换成欧拉角的形式，也可以是从s计算出的观察变换矩阵，还可以利用其他各种位置姿势记述方法。

通过以上处理，就能够得到不仅使主观视点图像上的标识间的误差极小化，还使客观视点图像上的标识间的误差极小化这样的摄像装置130的位置和姿势。据此，与仅使用主观视点图像、客观视点图像的某一个图像的情况相比，就能够以更高的精度来测量摄像装置130的位置和姿势，并计算出其参数。

通过以上处理，摄像装置的位置和姿势的测量，在位置和姿势的任何一个上都能以高的精度来实现。

另外，在本实施形式中，由于利用了主观视点图像和客观视点图像两者，所以与仅使用主观视点图像的以往方法相比，就能够减少配置在场景中的主观视点标识的总数。

[第2实施形式]

本实施形式的位置姿势测量装置，测量搭载于立体视频透视(stereo video see-through)HMD(Head Mount Display)上的2台摄像机的位置和姿势。下面，对本实施形式的位置姿势测量装置和位置姿势测量方法进行说明。

图5是表示本实施形式的位置姿势测量装置的概略结构的图。对于与图1、图3相同的部分，附加相同的编号、记号。如图5所示，本实施形式的位置姿势测量装置500由主观视点标识检测单元110L和110R、客观视点标识检测单元310、位置姿势计算单元520、以及客观视点摄像机330构成。

另外，主观视点标识检测单元110L和110R是分别将与图1所示主观视点标识检测单元110相同的单元设为左用、右用后的单元。关于主观视点标识检测单元110L和110R的细节将在后面进行描述。

主观视点标识Q^k与第1实施形式同样地进行配置，另外由于客观视点摄像机330和客观视点标识检测单元310的功能也与第1实施形式相同，所以省略关于它们的详细说明。

为了拍摄与观察者的左右视点相对应的现实空间的图像，在HMD540中内置有2台摄像机550L、550R。此处设左摄像机坐标系(将摄像机550L上的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)中的摄像机550R的位置和姿势为已知。另外，在HMD540上的多个位置上，设定有左摄像机坐标系中的位置为已知的客观视点标识P^k(k＝1，，，K₂)。

由摄像机550L、550R所拍摄的左右主观视点图像，被分别输入到主观视点标识检测单元110L、110R中。

主观视点标识检测单元110L和110R，通过与第1实施形式中的主观视点标识检测单元110相同的处理，对分别输入的左右主观视点图像进行主观视点标识的检测处理，并将检测出的标识的图像坐标及其标识符输出给位置姿势计算单元520。以下，将由主观视点标识检测单元110L和110R检测出的标识的图像坐标分别表示为u_L ^QkLn和u_R ^QkRn。另外，将检测出的标识的总数分别表达为N_L和N_R。

在图5的例子中，表示配置有3个主观视点标识Q¹、Q²、Q³，其中2个主观视点标识Q¹、Q²包含在摄像机550L的视场内，2个主观视点标识Q¹、Q³包含在摄像机550R的视场内的状况。此时，N_L＝2，主观视点标识检测单元110L输出标识符k_L1＝1、k_L2＝2和与它们对应的图像坐标u^QkL1、u^QkL2。同样地，N_R＝2，主观视点标识检测单元110R输出标识符k_R1＝1、k_R2＝3和与它们对应的图像坐标u^QkR1、u^QkR2。

位置姿势计算单元520基于检测出的主观视点标识Q^kn的图像坐标u^LQkLn和u^RQkRn以及客观视点标识P^km的图像坐标u^Pkm，计算出2台摄像机550L、550R的位置和姿势。

另外，图5所示的客观视点标识检测单元310、位置姿势计算单元520、主观视点标识检测单元110R、110L既可以分别作为独立的装置来处理，也可以分别作为软件安装在1个或多个计算机中，并由各个计算机的CPU进行执行，由此来实现其功能。在本实施形式中，各单元(客观视点标识检测单元310、位置姿势计算单元520、主观视点标识检测单元110R、110L)作为在1台计算机内成为执行对象的软件来处理。另外该计算机的基本结构采用图10所示的结构。

另外，在与本实施形式相关的说明中，设已知的信息全部保存在外部存储装置1007中。

位置姿势计算单元520的处理过程与第1实施形式中的位置姿势计算单元320相同，将摄像机550L的位置和姿势作为未知参数，计算出6值状态向量s＝[x y z ξ ψ ζ]^T。但是，因为使用左右双方的主观视点标识的检测结果，所以在以下点上与第1实施形式处理不同。

即，将误差向量U设定为垂直排列了误差Δu_L ^QkLn、Δu_R ^QkRn和Δu^Pkm的2(N_L+N_R+M)维的向量。另外，将矩阵Φ设定为垂直排列了雅可比矩阵J_uLs ^QkLn、J_uRs ^QkRn和J_us ^Pkm的2(N_L+N_R+M)行×6列的矩阵。在图5的例子中，M＝2、N_L＝2、N_R＝2，所以U是12维向量，Φ是12行×6列的矩阵。

在此，关于主观视点标识的误差和雅可比矩阵的计算，基于摄像机550L、550R的各自的固有参数(焦点距离)，通过与第1实施形式中的关于主观视点标识的误差和雅可比矩阵的计算(步骤S4040～S4060)相同的处理来进行。但是，关于摄像机550R，作为从s求得右摄像机坐标系(将摄像机550R上的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)中的标识的坐标X_CR ^QkRn的公式，取代式2而使用下式。

[式18]

$x_{C_R}^{Q_{k{R}_n}}={M_{C_L{C}_R}}^{-1}\·M_{{\mathrm{WC}}_L}{\left(s\right)}^{-1}\·x_W^{Q_{k{R}_n}}$

此处，M_CLCR是从右摄像机坐标系向左摄像机坐标系的变换矩阵，基于作为已知的值被预先保持的左摄像机坐标系中的摄像机550R的位置和姿势，而预先计算出。

根据以上公式，如果已求得s，则摄像机550R的位置和姿势，通过使用s和作为已知值的M_CLCR就能够容易地计算出。

本实施形式中的位置姿势测量装置500的输出，既可以是表示摄像机550L和550R各自的位置和姿势的观察变换矩阵，也可以是表示摄像机550L和550R各自的位置和姿势的2个6维向量。另外，在使用位置姿势测量装置500的输出的装置(本实施形式中为未图示的图像生成装置)保持有摄像机550L和摄像机550R的位置关系(例如上述矩阵M_CLCR)时，也可以仅输出表示一个摄像机的位置和姿势的数据。

通过以上处理，搭载于立体视频透视HMD上的2台摄像机的位置和姿势的测量，在位置和姿势的任何一个上都能以高的精度来实现。

[第3实施形式]

尽管第1实施形式使用了1台客观视点摄像机，但客观视点摄像机的台数并不限于1台。通过使用多台客观视点摄像机来构成位置姿势测量装置，就有望扩大测量范围和提高测量的稳定性。

图6是表示本实施形式的位置姿势测量装置的概略结构的图。对与图1、图3相同的部分附加相同的编号、记号。如图6所示那样，本实施形式的位置姿势测量装置600由客观视点摄像机330a～330d、客观视点标识检测单元310a～310d、主观视点标识检测单元110、以及位置姿势计算单元620构成。

另外，主观视点标识Q和客观视点标识P与第1实施形式同样地进行配置，摄像装置130所拍摄的主观视点映像被输入给主观视点标识检测单元110。主观视点标识检测单元110的功能与第1实施形式相同，故省略其说明。

在摄像装置130处于测量对象范围内时，客观视点摄像机330a～330d被固定配置于可以拍摄摄像装置130的位置。客观视点摄像机330a～330d的配置，可以为了提高对于遮挡的稳定性，进行设定以便为了使客观视点标识P同时由多个客观视点摄像机来进行观察，而使其分别拍摄重叠的区域，也可以以扩大测量对象范围为目的，进行设定以便拍摄互不相同的区域。此外，设世界坐标系中的客观视点摄像机330a～330d各自的位置和姿势，作为已知值被预先保持。

客观视点标识检测单元310a～310d分别通过与第1实施形式中的客观视点标识检测单元3 10相同的处理，输入客观视点摄像机330a～330d所拍摄的各个客观视点图像，进行客观视点标识的检测处理，将检测出的标识的图像坐标及其标识符输出给位置姿势计算单元620。下面，将由客观视点标识检测单元310a～310d检测出的标识的图像坐标分别表达为u_a ^Pkan、u_b ^Pkbn、u_c ^Pkcn、u_d ^Pkdn。另外，将检测出的标识的总数分别表达为M_a、M_b、M_c、M_d。

位置姿势计算单元620基于检测出的各主观视点标识的图像坐标和该标识的世界坐标的对应关系、以及由客观视点标识检测单元310a～310d分别检测出的各客观视点标识的图像坐标和主观视点摄像机坐标系上的标识的对应关系，计算出摄像装置130的位置和姿势。

另外，图6所示客观视点标识检测单元310a～310d、主观视点标识检测单元110、以及位置姿势计算单元620既可以分别作为独立的装置来处理，也可以分别作为软件安装在1个或多个计算机中，并由各个计算机的CPU进行执行，由此来实现其功能。在本实施形式中，各单元(客观视点标识检测单元310a～310d、主观视点标识检测单元110、以及位置姿势计算单元620)作为在1台计算机内成为执行对象的软件来处理。另外该计算机的基本结构采用图10所示的结构。

另外，在有关本实施形式的说明中，设已知信息全部保存在外部存储装置1007中。

位置姿势计算单元620的处理过程与第1实施形式中的位置姿势计算单元320相同，将摄象装置130的位置和姿势作为未知参数，计算出6值状态向量s＝[x y z ξ ψ ζ]^T。但是，因为使用多台客观视点摄像机，所以在以下点上与第1实施形式处理不同。

即，将误差向量U设定为垂直排列了误差Δu^Qkn、Δu_a ^Pkam、Δu_b ^Pkbm、Δu_c ^Pkcm和Δu_d ^Pkdm的2(N+M_a+M_b+M_c+M_d)维的向量。另外，将矩阵Φ设定为垂直排列了雅可比矩阵J_us ^Qkn、J_uas ^Pkam、J_ubs ^Pkbm、J_ucs ^Pkcm和J_uds ^Pkdm的2(N+M_a+M_b+M_c+M_d)行×6列的矩阵。在此，与客观视点标识有关的误差和雅可比矩阵的计算，基于客观视点摄像机330a～330d的各固有参数(世界坐标系中的位置和焦点距离)，通过与第1实施形式中的与客观视点标识有关的误差和雅可比矩阵的计算(步骤S4070～S4090)相同的处理来进行。

如以上那样，通过使用多个客观视点摄像机，即使在摄像装置在较广范围内移动的情况，或对于任何一个客观视点摄像机，摄像装置被遮挡住这样的状况下，也可以得到对计算位置和姿势所足够的数据，从而实现扩大可移动范围或者提高对遮挡等的稳定性。

另外，尽管本实施形式中的位置姿势测量装置使用了4台客观视点摄像机，但不言而喻客观视点摄像机的台数并不限于此，也可以是任意的台数。

[第4实施形式]

第1至第3实施形式是以测量在空间中移动的摄像装置自身的位置和姿势为目的的。本实施形式的位置姿势测量装置是以测量任意对象物体的位置和姿势为目的的位置姿势测量装置，是在第1实施形式的位置姿势测量装置上追加了拍摄主观视点图像的摄像机的结构。下面，说明本实施形式的位置姿势测量装置和位置姿势测量方法。

图7是表示本实施形式的位置姿势测量装置的结构的图。对于与图1、图3相同的部分附加相同的编号、记号，并省略其说明。如图7所示那样，本实施形式的位置姿势测量装置700由主观视点标识检测单元110、客观视点标识检测单元310、位置姿势计算单元720、客观视点摄像机330和主观视点摄像机730构成。

主观视点摄像机730固定安装于测量对象物体710。此处，设主观视点摄像机坐标系(将主观视点730上的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)中的测量对象物体710的位置和姿势是已知的。

在现实空间中的多个位置上，配置有在世界坐标系中的位置为已知的主观视点标识Q^k(k＝1，，，K₁)，作为用于由摄像装置130拍摄的标识(下面称为主观视点标识)。另外，在主观视点摄像机730上的多个位置处，配置有在主观视点摄像机坐标系上的位置为已知的客观视点标识P^k(k＝1，，，K₂)。

最好是，这样设置这些标识，以便在使测量对象物体710位于将要测量位置和姿势的测量对象范围内的各个地点时，在由主观视点摄像机730所取得的图像上被观测到的主观视点标识，和在由客观视点摄像机330所取得的图像上被观测到的客观视点标识的总数，始终至少为3个。

客观视点摄像机330、主观视点标识检测单元110、以及客观视点标识检测单元310的功能与第1实施形式相同，故省略其详细说明。但是，在将由主观视点摄像机730所取得的图像作为主观视点图像输入到主观视点标识检测单元110中这一点上，与第1实施形式不同。

位置姿势计算单元720将世界坐标系中的主观视点摄像机730的位置和姿势作为未知的向量s，通过与图4所示的第1实施形式中的位置姿势计算单元320相同的处理，计算出s。

位置姿势计算单元720进而基于由上述过程算出的世界坐标系中的主观视点摄像机730的位置和姿势、以及作为已知值的主观视点摄像机坐标系中的测量对象物体710的位置和姿势，通过坐标变换矩阵的积计算出世界坐标系中的测量对象物体710的位置和姿势，并将其输出。

通过以上处理，任意的测量对象物体的位置和姿势的测量，在位置和姿势的任何一个上都能以高的精度来实现。

另外，图7所示主观视点标识检测单元110、客观视点标识检测单元310、以及位置姿势计算单元720既可以分别作为独立的装置来处理，也可以分别作为软件安装在1个或多个计算机中，由各个计算机的CPU进行执行，由此来实现其功能。在本实施形式中，各单元(主观视点标识检测单元110、客观视点标识检测单元310、以及位置姿势计算单元720)作为在1台计算机内成为执行对象的软件来处理。另外该计算机的基本结构采用图10所示的结构。

另外，在有关本实施形式的说明中，设已知的信息全部保存在外部存储装置1007中。

另外，本实施形式中的位置姿势计算单元720将主观视点摄像机730的位置和姿势作为状态向量s，但也可以将测量对象物体710的位置和姿势作为状态向量s直接来求得。此时，将由s来求得主观视点标识的主观视点摄像机坐标X_C ^Qkn的式2和求得客观视点标识的客观视点摄像机坐标系中的坐标X_B ^Pkm的式12分别替换成

[式19]

$x_C^{Q_{k_n}}=M_{\mathrm{CO}}\·M_{\mathrm{WO}}{\left(s\right)}^{-1}\·x_W^{Q_{k_n}}$

[式20]

$x_B^{P_{k_m}}=M_{\mathrm{WB}}^{-1}\·M_{\mathrm{WO}}\left(s\right)\·M_{\mathrm{CO}}^{-1}\·x_C^{P_{k_m}}$

即可。此处，M_CO是从测量物体坐标系(将测量对象物体710上的1点定义为原点，进而将相互正交的3个轴分别定义为X轴、Y轴、Z轴的坐标系)向主观视点摄像机坐标系的变换矩阵，基于作为已知的值被预先保持的主观视点摄像机坐标系中的测量对象物体710的位置和姿势，而预先计算出。另一方面，M_WO(s)是由s确定的建模变换矩阵(即，将物体坐标系中的坐标变换成世界坐标系中的坐标的矩阵)，与式4一样由s来进行定义。

[第5实施形式]

在第1至第4实施形式中，位置姿势计算单元320、520、620、720的任一个都是，将表示位置和姿势的6值状态向量s作为未知数，来求得使主观视点标识和客观视点标识的检测坐标，与这些点的计算值之间的误差的总和最小的s。但是，通过并用从主观视点图像和客观视点图像两者得到的几何约束条件来取得测量对象物体的位置和姿势的方法并不限于此。本实施形式的位置姿势测量装置的特征在于，作为其构成要素，具有使用了与使整体的误差最小的方法不同的方法的位置姿势计算单元。

本实施形式的位置姿势测量装置的结构基本上与第1实施形式相同，为将位置姿势计算单元320替换成与第1实施形式中的不同的位置姿势计算单元320’的结构。下面，说明本实施形式的位置姿势测量装置和位置姿势测量方法。

在本实施形式中，各单元(客观视点标识检测单元310、位置姿势计算单元320’、主观视点标识检测单元110)作为在1台计算机内成为执行对象的软件来处理。另外该计算机的基本结构采用图10所示的结构。另外，在有关本实施形式的说明中，设已知的信息全部保存在外部存储装置1007中。

图11是通过CPU1001执行位置姿势计算单元320’的软件的程序而进行的、计算表示摄像装置130的位置和姿势的参数的处理的流程图。另外，假定在进行以下处理之前，客观视点标识检测单元310、位置姿势计算单元320’、主观视点标识检测单元110各自的软件的程序已经被装入到RAM1002中。

在步骤S11000中，位置姿势计算单元320’输入在客观视点标识检测单元310中检测出的客观视点标识P的图像坐标u^Pa、u^Pb。在本实施形式中，以后的处理所使用的客观视点标识仅是1点。

另外，输入了多个客观视点标识的图像坐标时(即，在摄像装置130上设定了2点以上的客观视点标识，且这些中的多个被客观视点标识检测单元310检测出来时)，位置姿势计算单元320’选择适当的1点作为客观视点标识P的图像坐标u^P。

在步骤S11010中，位置姿势计算单元320’基于图像坐标u^P，计算出表示约束世界坐标系中的客观视点标识P的位置的一条直线的参数。即，基于图像坐标u^P，通过

[式21]

$\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\\ 0\end{array}\right]=M_{\mathrm{WB}}\·\left[\begin{array}{c}{u}_x^P/f_x^B\\ u_y^P/f_y^B\\ 1\\ 0\end{array}\right]$

计算出世界坐标系中的直线的倾斜度(方向向量)hx、hy、hz，将计算出的该hx、hy、hz作为直线的参数。此处，f^B _x和f^B _y分别是x轴方向和y轴方向的客观视点摄像机330的焦点距离，并作为已知值被预先保持在外部存储装置1007中。此时，世界坐标系中的直线上的点，能够作为媒介变量τ的函数，如

[式22]

$l_W\left(\mathrm{\τ}\right)=\left[\begin{array}{c}{h}_x\mathrm{\τ}+x_{\mathrm{WB}}\\ h_y\mathrm{\τ}+y_{\mathrm{WB}}\\ h_z\mathrm{\τ}+z_{\mathrm{WB}}\\ 1\end{array}\right]$

那样来表示。此处，xWB、yWB、zWB是世界坐标系中的客观视点摄像机330的位置，作为已知值被预先保持在外部存储装置1007中。

从而，用式22表示的直线是通过世界坐标系中的客观视点摄像机330的位置和世界坐标系中的客观视点标识P的位置的直线，世界坐标系中的客观视点标识P的位置是通过上述媒介变量τ取适当值而得到的。

下面，利用组合了确定世界坐标系中的客观视点标识P的位置的媒介变量τ，和表示摄像装置130的姿势的3值(ξ ψ ζ)的4值状态向量s＝[τ ξ ψ ζ]^T来描述待求得的未知参数。

在步骤S11020中，位置姿势计算单元320’在状态向量s上设定适当的初始值。作为s的初始值，例如，既可以使用在前一帧(时刻t_k-1)处理中所导出的s，也可以设定通过基于来自过去的s的导出值的变化的预测而得到的值。

在步骤S11030中，位置姿势计算单元320’输入在主观视点标识检测单元110中检测出的各主观视点标识Q^kn的图像坐标u^Qkn及其标识符k_n。

在步骤S11040中，位置姿势计算单元320’判断所输入的主观视点标识数是否为2个以上。如果标识总数N不足2个，则不进行由步骤S11050到步骤S11100所实现的s的更新处理而进入步骤S11110。

在步骤S11050中，位置姿势计算单元320’对各主观视点标识Q^kn，基于式1～式4计算出其图像坐标的推测值u^Qkn*。但是，在本实施形式中，作为s的构成要素，不包含式4中的x、y、z(表示世界坐标系中的摄像装置130的位置)，所以通过下式基于s计算出这些值。

[式23]

${\left[\begin{array}{cccc}x& y& z& 1\end{array}\right]}^T=l_W\left(\mathrm{\τ}\right)-R_{\mathrm{WC}}\left(s\right)\·x_C^P$

此处，X_C ^P是主观视点摄像机坐标系中的标识P的坐标值，作为已知信息被预先保持在外部存储装置1007中。另外，R_WC(s)是表示世界坐标系中的摄像装置130的姿势的旋转矩阵，根据s如下式那样进行定义。

[式24]

$R_{\mathrm{WC}}\left(s\right)=\left[\begin{array}{cccc}\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{\θ}& 0\\ \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{\θ}& 0\\ \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{\θ}& 0\\ 0& 0& 0& 1\end{array}\right]$

即，在步骤S11050中，假定为摄像装置130的位置和姿势遵照以前所求得的状态向量s，并根据摄像装置130和主观视点标识的位置姿势关系，求得各主观视点标识的图像坐标的推测值。

接着，在步骤S11060中，位置姿势计算单元320’对各主观视点标识Q^kn，基于式5计算出图像坐标的推测值u^Qkn*和实测值u^Qkn的误差Δu^Qkn。

在步骤S11070中，位置姿势计算单元320’对各主观视点标识Q^kn，计算出作为各元素具有用s的各元素对式1的函数F_C()进行了偏微分的解的2行×3列的雅可比矩阵J_us ^Qkn(＝u/s)。具体来讲，就是计算出作为各元素具有用向量s的各元素对式2的右边进行了偏微分的解的3行×4列的雅可比矩阵J_xs ^Qkn(＝x/s)，和作为各元素具有用摄像机坐标X_C ^Qkn的各元素对式3的右边进行了偏微分的解的2行×3列的雅可比矩阵J_ux ^Qkn(＝u/x)，并通过式6算出J_us ^Qkn。

在步骤S11080中，位置姿势计算单元320’通过式7确定2N维的误差向量U，通过式8确定矩阵Φ(2N行×4列)，通过式9算出状态向量s的修正值Δs。

在步骤S11090中，位置姿势计算单元320’通过式10用在步骤S11080中计算出的修正值Δs来修正状态向量s，并将所得到的值设为s的新推测值。

在步骤S11100中，位置姿势计算单元320’使用误差向量U是否比预先确定的阈值小，或者修正值Δs是否比预先确定的阈值小这样的一些判断标准，来判断计算是否收敛。如果不收敛，则用修正后的状态向量s再次进行步骤S11050以后的处理。

在步骤S11110中，位置姿势计算单元320’输出摄像装置130的位置和姿势的信息。位置和姿势信息，例如以从s计算出的建模变换矩阵的形式输出。另外，既可以基于式23从所得到的s计算出世界坐标系中的摄像装置130的位置，与包含在s中的表示姿势的3值一起作为表示位置和姿势的6值的数据进行输出，也可以将姿势变换成欧拉角后进行输出，还可以输出由其他任何位置姿势记述方法所记述的位置和姿势的信息。

通过以上处理，将应存在从客观视点摄像机330得到的客观视点标识的直线作为约束条件，在该约束条件基础上，就能够得到使主观视点图像上的主观视点标识的误差最小那样的摄像装置130的位置和姿势。

另外，本实施形式的位置和姿势的测量结果与第1实施形式的位置和姿势的测量结果相比较，就优先信赖从客观视点摄像机330得到的信息。从而，在从客观视点摄像机330得到的信息的可靠性比从摄像装置130得到的信息相对要高的状况下，例如，在可利用高分辨率的客观视点摄像机时，或仅可将检测精度非常高的标记作为客观视点标识进行利用时等，本实施形式的位置姿势测量装置与第1实施形式相比就更有效地发挥作用。

[第6实施形式]

本实施形式的位置姿势测量装置具有如下结构：在第5实施形式的位置姿势测量装置上再追加1台客观视点摄像机，且取代位置姿势计算单元320’，而使用通过与位置姿势计算单元320’不同的动作来输出摄像装置130的位置和姿势的信息的位置姿势计算单元820。

下面，说明本实施形式的位置姿势测量装置和位置姿势测量方法。

图8是表示本实施形式的位置姿势测量装置的结构的图。另外，对于与图1、图3相同的部分，附加相同编号、记号，并省略其说明。如图8所示那样，本实施形式中的位置姿势测量装置800由客观视点摄像机330a、330b、主观视点标识检测单元110、客观视点标识检测单元310a、310b、以及位置姿势计算单元820构成，并连接到作为测量对象的摄像装置130。

在摄像装置130上配置有主观视点摄像机坐标系上的位置为已知的客观视点标识P。最好是对该标识的进行配置，以便在使摄像装置130位于将要测量的位置和姿势的测量对象范围内的各地点时，在由客观视点摄像机330a、330b各自所得到的图像上始终被观测到。

客观视点摄像机330a、330b在摄像装置130位于测量对象范围内时，被固定配置在始终可拍摄客观视点标识P的位置。此处，设世界坐标系中的客观视点摄像机330a和330b的位置和姿势作为已知值被预先保持。

在现实空间中的多个位置处，与第1实施形式同样配置有主观视点标识Q^k。最好是对这些标识进行配置，以便在使摄像装置130位于将要测量的位置和姿势的测量对象范围内的各地点时，在由摄像装置130所取得的图像上被观测到的总数N至少为2个。图8的例子表示配置有3个主观视点标识Q¹、Q²、Q³，其中2个主观视点标识Q¹、Q²被包含在摄像装置130的视场内的状况。

客观视点标识检测单元310a输入客观视点摄像机330a所拍摄的客观视点图像，通过与上述以往例相同的处理，检测出客观视点标识P的图像坐标，并将其图像坐标u^Pa输出给位置姿势计算单元820。同样地，客观视点标识检测单元310b输入客观视点摄像机330b所拍摄的客观视点图像，通过与上述以往例相同的处理，检测出客观视点标识P的图像坐标，将其图像坐标u^Pb输出给位置姿势计算单元820。

位置姿势计算单元820将主观视点标识Q^kn的图像坐标u^Qkn、客观视点标识P的图像坐标u^Pa和u^Pb作为输入，计算出摄像装置130的位置和姿势，并将其输出。

另外，图8所示的主观视点标识检测单元110、客观视点标识检测单元310a、310b、以及位置姿势计算单元820既可以分别作为独立的装置来处理，也可以分别作为软件安装在1个或多个计算机中，由各自的计算机的CPU进行执行，由此来实现其功能。在本实施形式中，各单元(主观视点标识检测单元110、客观视点标识检测单元310a、310b、以及位置姿势计算单元820)作为在1台计算机内成为执行对象的软件来处理。另外，该计算机的基本结构采用图10所示的结构。

另外，在关于本实施形式的说明中，设已知的信息全部保存在外部存储装置1007中的信息。

图9是通过CPU1001执行位置姿势计算单元820的软件的程序而进行的、计算表示摄像装置130的位置和姿势的参数的处理的流程图。假定在进行以下处理之前，主观视点标识检测单元110、客观视点标识检测单元310a、310b、以及位置姿势计算单元820的各软件的程序已经被装入到RAM1002中。

在步骤S9000中，位置姿势计算单元820输入在客观视点标识检测单元310a和310b中检测出的客观视点标识P的图像坐标u^Pa、u^Pb。

在步骤S9010中，位置姿势计算单元820基于三角测量的原理，使用图像坐标u^Pa和u^Pb，计算出世界坐标系中的客观视点标识P的位置X_W ^P。

下面，通过表示待计算的摄像装置130的姿势的3值状态向量s＝[ξ ψ ζ]^T，来描述待求得的未知参数。

在步骤S9020中，位置姿势计算单元820在状态向量s上设定适当的初始值(姿势的推测值)。作为s的初始值，例如既可以使用在前一帧(时刻t_k-1)的处理中所导出的s，也可以设定通过基于来自过去的s的导出值的变化的预测而得到的值。

在步骤S9030中，位置姿势计算单元820输入在主观视掉标识110中检测出的各主观视点标识Q^kn的图像坐标u^Qkn及其标识符k_n。

在步骤S9040中，位置姿势计算单元820判断所输入的主观视点标识数是否为2个以上。在标识总数N不足2个时，不进行由步骤S9050至步骤S9100实现的s的更新处理，进入步骤S9110。

在步骤S9050中，位置姿势计算单元820对各主观视点标识Q^kn，基于式1～式4计算出其图像坐标的推测值u^Qkn*。但是，在本实施形式中，由于作为s的构成要素，不包含式4中的x、y、z(表示世界坐标系中的摄像装置130的位置)，所以通过下式，基于s计算出这些值。

[式25]

${\left[\begin{array}{cccc}x& y& z& 1\end{array}\right]}^T=x_W^P-R_{\mathrm{WC}}\left(s\right)\·x_C^P$

这里，X_W ^P表示在步骤S9010中计算出的世界坐标系中的客观视点标识P的位置，X_C ^P表示作为已知值的主观视点摄像机坐标系中的客观视点标识P的位置。另外，R_WC(s)是表示世界坐标系中的摄像装置130的姿势的旋转矩阵，由式24进行定义。

在步骤S9060中，位置姿势计算单元820对各主观视点标识Q^kn，基于式5计算出图像坐标的推测值u^Qkn*和实测值u^Qkn的误差Δu^Qkn。

在步骤S9070中，位置姿势计算单元820对各主观视点标识Q^kn，计算出作为各元素具有用状态向量s的各元素对式1的函数F_C()进行了偏微分的解的2行×3列的雅可比矩阵J_us ^Qkn(＝u/s)。具体来讲，就是计算出作为各元素具有用摄像机坐标X_C ^Qkn的各元素对式3的右边进行了偏微分的解的2行×3列的雅可比矩阵J_ux ^Qkn(＝u/x)，和作为各元素具有用向量s的各元素对式2的右边进行了偏微分的解的3行×3列的雅可比矩阵J_xs ^Qkn(＝x/s)，并通过式6计算出J_us ^Qkn。

在步骤S9080中，位置姿势计算单元820通过式7确定2N维的误差向量U，通过式8确定矩阵Φ(2N行×3列)，通过式9计算出状态向量s的修正值Δs。

在步骤S9090中，位置姿势计算单元820用在步骤S9080中计算出的修正值Δs，通过式10修正状态向量s，并将所得到的值设为s的新推测值。

在步骤S9100中，位置姿势计算单元820使用误差向量U是否比预先确定的阈值小，或者修正值Δs是否比预先确定的阈值小这样的一些判断标准，来判断计算是否收敛。如果不收敛，则用修正后的状态向量s再次进行步骤S9050以后的处理。

在步骤S9110中，位置姿势计算单元820输出摄像装置130的位置和姿势的信息。位置和姿势的信息例如以从s计算出的建模变换矩阵的形式进行输出。另外，既可以从所得到的s基于式25计算出世界坐标系中的摄像装置130的位置，与表示姿势的s本身一起作为6值数据进行输出，也可以将姿势变换成欧拉角后进行输出，还可以输出由其他任何位置姿势记述方法所记述的位置和姿势的信息。

通过以上处理，将用客观视点摄像机330所求得的客观视点标识的位置作为约束条件，在该约束条件的基础上，就能够得到使主观视点图像上的主观视点标识的误差最小那样的摄像装置130的位置和姿势。从而，与第5实施形式同样，在从客观视点摄像机330得到的信息的可靠性比从摄像装置130得到的信息相对要高的状况下，例如在可利用高分辨率的客观视点摄像机时，或仅可将检测精度非常高的标记作为客观视点标识进行利用时等，本实施形式的位置姿势测量装置与第1实施形式相比就更有效地发挥作用。

[第7实施形式]

尽管在第6实施形式中，是通过2台客观视点摄像机，来计算出客观视点标识P的世界坐标X_W ^P，但只要是能测量某测量点的3维位置的装置，则任何装置都可取代第6实施形式中的客观视点摄像机来使用。下面，说明本实施形式的位置姿势测量装置和位置姿势测量方法。

图12是表示本实施形式的位置姿势测量装置的结构的图。对于与图8相同的部分，附加相同的编号、记号，并省略其说明。如图12所示，本实施形式中的位置姿势测量装置1200由主观视点标识检测单元110、位置传感器1230、测量点1240、位置输入单元1210、以及位置姿势计算单元820’构成，并连接到作为测量对象的摄像装置130上。

位置传感器1230是测量预定的测量点的3维位置的传感器，例如，由作为光学式3维位置传感器的Northern Digital lnc.的OPTOTRAK构成。

测量点1240是作为由位置传感器1230所测量的对象的点，在使用OPTOTRAK的情况下，由依照来自位置传感器1230的控制信号而忽亮忽灭的红外LED构成。测量点1240取代第6实施形式中的客观视点标识P，安装在摄像装置130上。最好是对测量点1240进行配置，以便在使摄像装置130位于将要测量位置和姿势的测量对象范围内的各地点时，始终被包含在位置传感器1230的测量范围内。另外，主观视点摄像机坐标系中的测量点1240的位置作为已知的数据被存储在外部存储装置1007中，并根据需要被读出到RAM1002进行使用。

位置输入单元1210输入位置传感器1230输出的测量点1240的3维坐标，并将其输出给位置姿势计算单元820’。此外，在位置传感器1230输出的测量点1240的3维坐标是位置传感器1230自身定义的传感器坐标系中的坐标时，位置输入单元1210在将其变换成世界坐标系中的坐标后，输出给位置姿势计算单元820’。如果世界坐标系中的位置传感器1230的位置和姿势作为已知信息被给出，则从传感器坐标系向世界坐标系的坐标变换能够通过公知方法来进行。世界坐标系中的位置传感器1230的位置和姿势作为已知的信息被存储在外部存储装置1007中，根据需要被读出到RAM1002进行使用。

位置姿势计算单元820’使用从位置输入单元1210所输入的测量点1240的3维坐标和从主观视点标识检测单元110输入的主观视点标识Q^kn的图像坐标u^Qkn，计算出摄像装置130的位置和姿势，并将其输出。

另外，位置姿势计算单元820’的动作与第6实施形式中的位置姿势计算单元820大致相同。但是，取代位置姿势计算单元820的步骤(步骤S9000、S9010)，即输入客观视点标识P的图像坐标u^Pa、u^Pb，计算出世界坐标系中的客观视点标识P的位置X_W ^P，而是位置姿势计算单元820’具有从位置输入单元1210输入世界坐标系中的测量点1240的位置X_W ^P的步骤。另外，在将世界坐标系中的测量点1240的位置用作式25中的X_W ^P这一点上，与第6实施形式不同。

通过以上处理，将用位置传感器1230所求得的测量点1240的位置作为约束条件，在该约束条件基础上，就能够得到使主观视点图像上的主观视点标识的误差最小那样的摄像装置130的位置和姿势。据此，就能够实现兼顾了通过使用位置传感器所得到的位置精度的稳定性，和通过使用主观视点图像所得到的图像上的位置对准精度的稳定性的，摄像装置130的位置和姿势的测量。

另外，作为位置传感器1230所用的传感器，只要能够测量测试点的3维位置，则任何传感器都可以使用。例如，也可以将测量超声波信标(beacon)的3维位置的超声波传感器(例如Inter Sense公司的IS-600)用作位置传感器1230。在此情况下，超声波信标就作为测量点1240被安装在摄像装置130中。

另外，显然本实施形式可以适用于将摄像装置130装在安装了测量点的测量对象物体上，测量该测量对象物体或者摄像装置130的位置姿势的情况。此外，在求摄像装置130的位置姿势时，摄像装置130和测量对象物体的位置姿势关系必须预先测定，并保存在存储器中。

[第8实施形式]

在第7实施形式中，作为位置传感器是使用光学传感器或超声波传感器，但在以室外使用为目的的情况下，就可将GPS(GlobalPositioning System)用作位置传感器。下面，说明本实施形式的位置姿势测量装置和位置姿势测量方法。

图13是表示本实施形式的位置姿势测量装置的结构的图。对于与图12相同的部分，附加相同编号、符号，并省略其说明。如图13所示那样，本实施形式中的位置姿势测量装置1300由主观视点标识检测单元110、GPS接收机1330、位置输入单元1310、以及位置姿势计算单元1320构成，并连接到作为测量对象的摄像装置130。

GPS接收机1330被安装在摄像装置130上，通过接收来自GPS卫星1340a～1340d的信号，来测量在地球上的自己的位置(纬度、经度、高度)，向位置输入单元1310输出。但是，在不能捕捉足够数量的GPS卫星的情况下，有时只能得到纬度和经度的测量值，所以在此情况下只输出纬度和精度。此外，设主观视点摄像机坐标系中的GPS接收机1330的位置为已知。

位置输入单元1310输入GPS接收机1330输出的纬度、经度和高度，在通过公知的矩阵变换处理将其变换成世界坐标系中的3维坐标(x^P、y^P、z^P)或2维坐标(x^P、y^P)后，向位置姿势计算单元1320输出。

位置姿势计算单元1320使用从位置输入单元1310输入的GPS接收机1330的3维坐标或2维坐标，和从主观视点标识检测单元110输入的主观视点标识Q^kn的图像坐标u^Qkn，计算出摄像装置130的位置和姿势，并将其输出。

在得到GPS接收机1330的3维坐标时，位置姿势计算单元1320通过与第6实施形式中的位置姿势计算单元820’相同的处理过程，导出摄像装置130的位置和姿势并将其输出。另外，在仅测量到GPS接收机1330的2维坐标时，通过与第5实施形式中的位置姿势计算单元820相同的处理过程，即，组合了表示约束测量点(本实施形式中为GPS接收机1330)在世界坐标系中的位置的直线的媒介变量τ，和表示摄像装置130的姿势的3值(ξψζ)的4值状态向量s＝[τξψζ]T作为未知参数来对其进行求解的处理过程，导出摄像装置130的位置和姿势并将其输出。另外，在本实施形式的情况下，如下式那样构成直线的约束式(与第5实施形式中的式22对应的式子)即可。

[式26]

$l_W\left(\mathrm{\τ}\right)=\left[\begin{array}{c}{x}^P\\ y^P\\ \mathrm{\τ}\\ 1\end{array}\right]$

通过以上处理，将由GPS接收机1330得到的位置作为约束条件，在该约束条件的基础上，就能够得到使主观视点图像上的主观视点标识的误差最小那样的摄像装置130的位置和姿势。

另外，不言而喻在始终能期待GPS接收机1330可输出3维坐标的状态的情况下，可以采用没有仅获得2维坐标的情况的处理分支的结构。另外，如果能够假定摄像装置130的高度始终恒定(例如摄像装置130是HMD的内置摄像机，其高度能够假定为装戴者的视点的高度的情况等)，则也可以不使用从GPS接收机1330得到的高度的信息，而使用固定值作为高度方向的位置。

<变形例1>

尽管在上述各实施形式中，对基于误差向量U和矩阵Φ的状态向量的修正值Δs的计算，使用由式9表达的最陡下降法，但修正值Δs的计算未必非要通过最快下降法来进行。例如也可以使用作为公知的数值计算方法的LM法(Levenberg-Marquardt法)来计算，不言而喻即使应用其他任何数值计算方法也无损本发明的实质。

<变形例2>

在上述各实施形式中，作为与标识的图像坐标相关的特征量，使用了点特征，即代表标识的投影图像的1点的图像坐标。另外，作为标识Q和P，使用了可抽取这样的点特征的标识(以下将其称为点标识)。但是上述实施形式并不限于标识的种类和与标识的图像坐标相关的特征量的种类。

例如，还能够将在公知的位置姿势测量装置(例如，参照高桥、石井、牧野、中静：用于VR接口的利用单眼的长方形标记位置·姿势的高精度实时推测法、3维图像会议’96讲演论文集、PP.167-172，1996.)中所使用那样的、具有特定几何形状的标记，作为主观视点标识和/或客观视点标识来使用。在将四角形标记用作标识时，将四角形的各顶点的世界坐标作为已知值来保持，从图像中检测出各顶点的图像坐标作为与标识的图像坐标相关的特征量，由此就能够取得与上述实施形式中的标识相同的效果。

另外，也可以将在其他公知的位置姿势测量装置(例如，参照非专利文献2)中所使用那样的线特征作为与标识的图像坐标相关的特征量来使用，将可抽取线特征的标识(以下将其称为线标识)作为主观视点标识和/或客观视点标识来使用。例如，将直线距原点的距离作为用于误差评价的标准，由从来自图像的检测值d和来自状态向量s的推测值d^*计算出的误差Δd，构成误差向量U，由作为各元素具有用状态向量s的各元素对d^*的计算式进行了偏微分的解的1行×6列的雅可比矩阵J_ds(＝d/s)构成矩阵Φ，由此，就能够通过与上述实施形式相同的方案来进行位置和姿势的测量。当然，还可以并用线标识和点标识。

<变形例3>

尽管在第3至第8实施形式中，摄像装置130或主观视点摄像机730用了1台，但也可与第2实施形式一样，使用多台摄像装置或主观视点摄像机。

<变形例4>

尽管在第4实施形式中，客观视点标识P被配置在主观视点摄像机730上，但客观视点标识P的配置位置并不限于此，不言而喻即使配置在测量对象物体710上也可取得同样的效果。

<变形例5>

尽管第5至第8实施形式是将主观视点摄像机坐标系中的客观视点标识、测量点、GPS接收机的各自的位置设为已知，并考虑到与作为测量对象的摄像装置130的位置的差异的结构，但也可采取将它们近似为相同的进行了简化的结构。

此外，通过并用从主观视点图像和客观视点图像两者得到的几何约束条件来测量测量对象物体的位置和姿势本身才是本质特征，适用范围并不限于标识的种类、约束条件的使用方法、解的计算方法。

由于在不脱离本发明的精神和范围内，能够实现本发明大量明显且广泛的不同实施形式，应该理解为本发明并不限于特定的实施形式，而是由附加的权利要求所规定。

Claims (17)

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

取得第1图像的主观视点取得步骤，该第1图像是通过安装在上述对象物体上的主观视点摄像单元拍摄场景而得到的；

取得第2图像的客观视点图像取得步骤，该第2图像是通过客观视点摄像单元拍摄上述对象物体而得到的；

第1检测步骤，从上述第1图像中检测出与配置在上述场景中的第1标识的图像坐标相关的特征量；

第2检测步骤，从上述第2图像中检测出与配置在上述对象物体上或上述主观视点摄像单元上的第2标识的图像坐标相关的特征量；以及

位置姿势计算步骤，用在上述第1检测步骤中检测出的与上述第1标识的图像坐标相关的特征量，和在上述第2检测步骤中检测出的与上述第2标识的图像坐标相关的特征量，计算出上述对象物体的位置和姿势。

2.根据权利要求1所述的位置姿势测量方法，其特征在于：

上述对象物体是摄像装置，具有上述主观视点摄像单元。

3.根据权利要求1所述的位置姿势测量方法，其特征在于：

上述客观视点摄像单元被固定在上述场景中。

4.根据权利要求1所述的位置姿势测量方法，其特征在于：

上述客观视点摄像单元由多个摄像机组成。

5.根据权利要求1所述的位置姿势测量方法，其特征在于：

上述位置姿势计算步骤还包括，

推测步骤，基于上述对象物体的位置和姿势的推测值，分别推测出与上述第1图像内的上述第1标识的图像坐标相关的特征量，和与上述第2图像内的上述第2标识的图像坐标相关的特征量；以及

修正步骤，为了使在上述第1和第2检测步骤中检测出的与上述第1和第2标识的图像坐标相关的特征量，和在上述推测步骤中推测出的与该标识各自的图像坐标相关的特征量之间的误差减小，而修正上述对象物体的位置和姿势的推测值。

6.根据权利要求1所述的位置姿势测量方法，其特征在于：

与上述第1标识的图像坐标相关的特征量是由上述第1标识所确定的1个点或多个点的图像坐标，与上述第2标识的图像坐标相关的特征量是由上述第2标识所确定的1个点或多个点的图像坐标。

7.根据权利要求6所述的位置姿势测量方法，其特征在于：

作为上述第1标识和上述第2标识，分别设定至少1个点的可检测出图像坐标的标识，在上述第1检测步骤和上述第2检测步骤中检测出的图像坐标的总数为3个点以上。

8.根据权利要求6所述的位置姿势测量方法，其特征在于：

上述位置姿势计算步骤包括，

位置计算步骤，用在上述第2检测步骤中检测出的上述第2标识的图像坐标，计算出与上述对象物体的位置相关的信息；以及

姿势计算步骤，用在上述第1检测步骤中检测出的上述第1标识的图像坐标，计算出与上述对象物体的姿势相关的信息。

9.根据权利要求6所述的位置姿势测量方法，其特征在于：

作为上述第2标识，设定1个点的标识，作为上述第1标识，设定至少2个点的可检测出图像坐标的标识。

10.一种位置姿势测量方法，计算安装在拍摄现实空间的第1摄像单元中，与该第1摄像单元的位置姿势关系已知的对象物体的第1位置姿势参数，其特征在于，包括：

第1图像坐标检测步骤，从由上述第1摄像单元所拍摄的第1图像中检测出配置在上述现实空间中的第1标识的图像坐标；

第1推测图像坐标检测步骤，在上述第1摄像单元的位置和姿势遵照以前计算出的第2位置姿势参数这样的假定的基础上，根据上述第1摄像单元和上述第1标识的位置姿势关系，来求得上述第1标识的推测图像坐标；

第1误差坐标计算步骤，求得上述第1图像上的、在上述第1图像坐标检测步骤中检测出的图像坐标和在上述第1推测图像坐标检测步骤中计算出的推测图像坐标的第1误差坐标；

第2图像坐标检测步骤，从由第2摄像单元所拍摄的、包含上述对象物体的第2图像中，检测出配置在上述第1摄像单元或上述对象物体中的第2标识的图像坐标；

第2推测图像坐标检测步骤，在上述第1摄像单元的位置和姿势遵照上述以前计算出的第2位置姿势参数这样的假定的基础上，根据上述第1摄像单元和上述第2标识的位置姿势关系，来求得上述第2标识的推测图像坐标；

第2误差坐标计算步骤，求得上述第2图像上的、在上述第2图像坐标检测步骤中检测出的图像坐标和在上述第2推测图像坐标检测步骤中计算出的推测图像坐标的第2误差坐标；

修正值计算步骤，用上述第1误差坐标和上述第2误差坐标，求得用于修正上述以前计算出的第2位置姿势参数的修正值；

修正步骤，用上述修正值修正上述以前计算出的第2位置姿势参数；以及

参数计算步骤，用在上述修正步骤中所修正的上述第2位置姿势参数，求得与上述第1摄像单元有已知的位置姿势关系的上述对象物体的上述第1位置姿势参数。

11.根据权利要求10所述的位置姿势测量方法，其特征在于：

上述对象物体是摄像装置，具有上述主观视点摄像单元。

12.根据权利要求1所述的位置姿势测量方法，其特征在于：

在上述第2检测步骤中检测出的与上述第2标识的图像坐标相关的特征量是上述第2标识的图像坐标；

上述位置姿势计算步骤，基于在上述第2检测步骤中检测出的上述第2标识的图像坐标和上述客观视点摄像单元的位置姿势，求得约束上述第2标识的3维空间中的位置的直线，在上述第2标识存在于上述直线上这样的约束的基础上，基于在上述第1检测步骤中检测出的与上述第1标识的图像坐标相关的特征量，计算出上述对象物体的位置和姿势。

13.根据权利要求12所述的位置姿势测量方法，其特征在于：

上述位置姿势计算步骤，将表示上述直线上的上述第2标识的位置的媒介变量和表示上述对象物体的姿势的参数作为未知数，并基于在上述第1检测步骤中检测出的与上述第1标识的图像坐标相关的特征量，来求出这些未知数，由此计算出上述对象物体的位置和姿势。

14.一种位置姿势测量方法，将拍摄场景的摄像装置作为测量对象物体，计算出该对象物体的位置和姿势，其特征在于，包括：

检测步骤，从由上述摄像装置所取得的图像中，检测出与配置在上述场景中的第1标识的图像坐标相关的特征量；

测量步骤，测量配置在上述摄像装置中的第2标识的位置；以及

位置姿势计算步骤，用在上述检测步骤中检测出的与上述第1标识的图像坐标相关的特征量，和在上述测量步骤中测量出的上述第2标识的位置，计算出上述摄像装置的位置和姿势。

15.一种计算对象物体的位置和姿势的位置姿势测量装置，其特征在于，包括：

主观视点摄像单元，安装在上述对象物体上来拍摄场景；

客观视点摄像单元，拍摄上述对象物体；

第1检测单元，从由上述主观视点摄像单元所取得的第1图像中，检测出与配置在上述场景中的第1标识的图像坐标相关的特征量；

第2检测单元，从由上述客观视点摄像单元所取得的第2图像中，检测出与配置在上述对象物体上或上述主观视点摄像单元上的第2标识的图像坐标相关的特征量；以及

位置姿势计算单元，用上述第1检测单元检测出的与上述第1标识的图像坐标相关的特征量，和上述第2检测单元检测出的与上述第2标识的图像坐标相关的特征量，计算出上述对象物体的位置和姿势。

16.一种程序，其特征在于：

使计算机执行权利要求1所述的位置姿势测量方法。

17.一种计算机可读取的存储介质，其特征在于：存储权利要求16所述的程序。

Images