JP4726191B2 - 位置姿勢補正装置、位置姿勢補正方法、そのプログラムおよび記憶媒体 - Google Patents

Description

本発明は、物体の位置及び姿勢、特に撮像装置の位置及び姿勢を計測する方法及び装置に関するものである。

近年、現実空間と仮想空間の繋ぎ目のない結合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）を重畳描画した画像を表示する装置として実現することができる。

このような画像表示装置を実現するには、現実空間中に定義した基準座標系（重畳しようとする仮想物体の位置及び姿勢を決定する基準となる現実空間中の座標系）と、撮像装置の座標系（カメラ座標系）との間の、相対的な位置及び姿勢を計測することが不可欠である。これは、仮想物体（仮想空間画像）を現実空間上の位置に合わせて描画するには、基準座標系に対する撮像装置の現実のカメラパラメータと同一のカメラパラメータを用いて仮想物体の画像を生成しなければならないからである。例えば、現実の室内のある位置に仮想物体を重畳表示する場合には、部屋に基準座標系を定義し、その基準座標系における撮像装置の位置及び姿勢を求めればよい。また、観察者が手に保持する現実の箱に何らかの仮想の模様やラベルを重畳表示する場合には、箱自身の物体座標系を基準座標系と考え、撮像装置に対する箱（基準座標系）の位置及び姿勢を求めればよい。

撮像装置の位置及び姿勢を計測する方法として、現実空間中に複数の指標（人為的なマーカや自然特徴など）を配置あるいは設定し、撮像装置によって撮像された画像内における指標の投影像の座標を検出して、指標の座標情報との関係に基づいて撮像装置の位置及び姿勢を求めることが一般的に行われている（例えば、非特許文献１）。しかし、このアプローチを用いる場合、常に指標が撮像されなくてはならないという制約がある。

一方、撮像装置に磁気センサや超音波センサなどを用いた６自由度位置姿勢センサを装着し、これによって計測した撮像装置の位置及び姿勢の誤差を、指標を撮像した画像を用いて得られる情報（画像情報）によって補正する試みがなされている（例えば、特許文献１，特許文献２）。特許文献２で開示されている方法では、撮像画像中に指標が検出された場合にはその情報に基づいてセンサ計測値の誤差が補正され、指標が検出されない場合には６自由位置姿勢センサの計測値がそのまま撮像装置の位置及び姿勢として使用される。指標の検出の有無に関わらずに撮像装置の位置及び姿勢が得られるため、複合現実感の提示を安定して行うことができる。

また、特許文献２の方法では、検出された指標の数が３点以上の場合には、撮像装置の位置及び姿勢の６自由度が画像情報に基づいて算出され、検出された指標の数が２点及び１点の場合には、センサによって計測された撮像装置の位置あるいは姿勢のいずれか（２または３自由度）のみを補正する処理が適用される。すなわち、検出された指標の数を判定基準として、撮像装置の位置及び姿勢の算出に利用するアルゴリズムを切り替えることが行われている。これにより、画像情報のみからでは撮像装置の位置及び姿勢を求められない場合（撮像された指標の数が３点に満たない場合）であっても、センサ計測値を基準としながら、その誤差を極力打ち消すような補正のなされた位置及び姿勢を取得することができる。

一方、特許文献１の方法では、検出された指標の数に関わらず、センサによって計測された撮像装置の位置あるいは姿勢のいずれかのみを画像情報に基づいて補正する処理が適用される。この補正方法では、姿勢を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような回転補正値を個別に求め、それらを平均化することで姿勢計測値に対する補正値が算出される。また、位置を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような平行移動補正値を個別に求め、それらを平均化することで位置計測値に対する補正値が算出される。指標の数に関わらず補正の自由度を２または３に限定するため、情報量が不十分な場合であっても安定した解を得ることができる。

加藤ら："マーカー追跡に基づく拡張現実感システムとそのキャリブレーション",日本バーチャルリアリティ学会論文誌, vol.4, no.4, pp.607-616, 1999. J. Park, B. Jiang, and U. Neumann: "Vision-based pose computation: robust and accurate augmented reality tracking," Proc. 2nd International Workshop on Augmented Reality (IWAR'99), pp.3-12, 1999. D. G. Lowe: "Fitting parameterized three−dimensional models to images," IEEE Transactions on PAMI, vol.13, no.5, pp.441−450, 1991. 特開2003-222509号公報 特開2003-279310号公報 特開2003-344018号公報

上記特許文献２の手法では、指標が３点以上観測されている際には必ず、位置及び姿勢の６自由度を画像情報から求めるアルゴリズムが選択される。しかし実際には、画像上の一部の領域に指標が偏在して観測されている場合のように、指標の数が３点以上であっても、位置及び姿勢の６自由度を安定して求めるのに十分な画像情報が入力画像中に含まれていない状況が存在する。特許文献２の手法は、このような状況下において、得られる解の精度や安定性が不十分となることがあった。

一方、上記特許文献１の手法は、精度よりも安定性を重視した手法であり、画像上の一部の領域に指標が偏在して観測されている場合のような画像情報が不十分な状況下であっても、特許文献２の手法と比較して安定した計測を行うことができる。しかし、十分な画像情報が得られている場合でも一部のパラメータの補正のみしか行わないので、画像情報を最大限に生かした精度を得ることができなかった。

また、上記特許文献１の手法では、個々の指標に対する２次元的な補正値の平均を求めているだけであるので、全指標上での誤差の和を最小とするような最適な補正を行うことができなかった。

本発明はこのような従来技術の問題点に鑑みてなされたものであり、計測対象物体の位置及び姿勢の計測を、安定性と精度とを両立しながら行うことが可能な位置姿勢測定方法及び装置を提供することを目的とする。

上述の目的は、撮像装置の位置姿勢を構成する複数のパラメータの１つ以上を補正する位置姿勢補正装置であって、複数のパラメータを入力する位置姿勢入力手段と、現実空間に複数配置された指標を撮像装置で撮像した画像を入力する画像入力手段と、画像から指標の画像座標の実測値を検出する検出手段と、検出手段による検出結果に基づいて、複数のパラメータのうち何れを補正対象とするか決定する決定手段と、複数のパラメータおよび実測値に基づいて、複数のパラメータのうち、決定手段によって補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正する補正手段と、を有し、補正手段が、指標の座標と複数のパラメータとに基づいて、指標の画像座標の理論値を算出する理論値算出手段と、実測値と理論値との誤差を小さくするように、補正対象と決定されたパラメータの補正値を算出する補正値算出手段と、補正値に基づいて、複数のパラメータのうち補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、を有することを特徴とする位置姿勢補正装置によって達成される。

また、上述の目的は、計測対象物体の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置であって、複数のパラメータを入力する位置姿勢入力手段と、計測対象物体に複数配置された指標を撮像装置で撮像した画像を入力する画像入力手段と、撮像画像から指標の画像座標の実測値を検出する検出手段と、検出手段による検出結果に基づいて、複数のパラメータのうち何れを補正対象とするか決定する決定手段と、複数のパラメータおよび実測値に基づいて、複数のパラメータのうち決定手段によって補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正する補正手段と、を有し、補正手段が、複数のパラメータに基づいて、指標の画像座標の理論値を算出する理論値算出手段と、実測値と理論値との誤差を小さくするように、補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出手段と、補正値に基づいて、複数のパラメータのうち補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、を有することを特徴とする位置姿勢補正装置によっても達成される。

また、上述の目的は、撮像装置の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置であって、複数のパラメータを入力する位置姿勢入力手段と、現実空間に複数配置され、位置が既知である指標を、撮像装置で撮像した画像を入力する画像入力手段と、画像から指標の画像座標の実測値を検出する検出手段と、検出手段による検出結果に基づいて、複数のパラメータのうち何れを補正対象とするか決定する決定手段と、複数のパラメータおよび実測値に基づいて、複数のパラメータのうち決定手段によって補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正する補正手段と、を有し、補正手段が、指標の位置と複数のパラメータとに基づいて、指標の画像座標の理論値を算出する理論値算出手段と、実測値と理論値との誤差を小さくするように、補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出手段と、補正値に基づいて、複数のパラメータのうち補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、を有し、複数のパラメータは、位置を示す３つの位置成分および姿勢を示す３つの姿勢成分の６成分に相当するパラメータであって、決定手段が、検出手段で検出された指標が１つである場合に、複数のパラメータのうち２成分以下に相当するパラメータを補正対象のパラメータとして決定し、検出手段で検出された指標が２つである場合に、検出された指標の画像座標間の距離に基づいて、複数のパラメータのうち３成分以下に相当するパラメータを補正対象のパラメータとして決定し、検出手段で検出された指標が３つ以上である場合に、検出された指標の画像座標から得られる領域の面積に基づいて、複数のパラメータのうち６成分以下に相当するパラメータを補正対象のパラメータとして決定することを特徴とする位置姿勢補正装置によっても達成される。

また、上述の目的は、計測対象物体の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置であって、複数のパラメータを入力する位置姿勢入力手段と、計測対象物体に複数配置された指標を撮像装置で撮像した画像を入力する画像入力手段と、撮像画像から指標の画像座標の実測値を検出する検出手段と、検出手段による検出結果に基づいて、複数のパラメータのうち何れを補正対象とするか決定する決定手段と、複数のパラメータおよび実測値に基づいて、複数のパラメータのうち決定手段によって補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正する補正手段と、を有し、補正手段が、複数のパラメータに基づいて、指標の画像座標の理論値を算出する理論値算出手段と、実測値と理論値との誤差を小さくするように、補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出手段と、補正値に基づいて、複数のパラメータのうち補正対象のパラメータと決定されなかったパラメータを補正せず、補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、を有し、複数のパラメータは、位置を示す３つの位置成分および姿勢を示す３つの姿勢成分の６成分に相当するパラメータであって、決定手段が、検出手段で検出された指標が１つである場合に、複数のパラメータのうち２成分以下に相当するパラメータを補正対象のパラメータとして決定し、検出手段で検出された指標が２つである場合に、検出された指標の画像座標間の距離に基づいて、複数のパラメータのうち３成分以下に相当するパラメータを補正対象のパラメータとして決定し、検出手段で検出された指標が３つ以上である場合に、検出された指標の画像座標から得られる領域の面積に基づいて、複数のパラメータのうち６成分以下に相当するパラメータを補正対象のパラメータとして決定することを特徴とする位置姿勢補正装置によっても達成される。

本発明に係る位置姿勢計測装置によれば、指標の画像座標の理論値と実測値との誤差を最小とする補正値を求め、この補正値によってセンサの計測値を補正することで、従来の方法と比べて安定性と精度に優れた計測が実現できる。

以下添付図面を参照して、本発明を好適な実施形態に従って詳細に説明する。
［第１の実施形態］
本実施形態に係る位置姿勢計測装置は、撮像装置と計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０によって構成される。そして、撮像装置１３０及び、撮像装置１３０と計測対象物体１７０に取り付けられた位置姿勢センサ１４０（１４０ａ，１４０ｂ）に接続されている。

計測対象物体１７０上の複数の位置には、物体座標系（計測対象物体１７０上に定義される座標系）における位置が既知である複数個の指標Ｑ_ｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。図１の例は、Ｋ＝９、すなわち９個の指標Ｑ_１〜Ｑ_９が配置されている状況を示している。指標Ｑ_ｋは、例えば、それぞれが異なる色を有する同一形状（図では円形）のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する四角形の単色領域によって形成されるような、四角形指標を用いることも可能である。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。また指標は故意に設定されたものであっても良いし、故意に設定されたものではない、自然形状のものを用いても良い。

例えばビデオカメラである撮像装置１３０が出力する画像（以下、これを撮影画像と呼ぶ）は、位置姿勢計測装置１００に入力される。

６自由度センサである位置姿勢センサ１４０ａ及び１４０ｂは、それぞれ撮像装置１３０及び計測対象物体１７０に装着されており、基準座標系における撮像装置１３０及び計測対象物体１７０の位置及び姿勢を計測する。位置姿勢センサ１４０が出力する計測値は、位置姿勢計測装置１００に入力される。位置姿勢センサ１４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。なお、位置姿勢センサ１４０によって計測される位置及び姿勢は、磁場の歪み等の影響によって誤差を含んだものとなっている。

画像入力部１６０は、位置姿勢装置１００へ入力される撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。

センサ計測値入力部１５０は、位置姿勢センサ１４０（１４０ａ，１４０ｂ）から夫々の計測値を入力し、位置姿勢算出部１２０へと出力する。

指標検出部１１０は、画像入力部１６０より撮影画像を入力し、入力した画像中に撮影されている指標Ｑ_ｋの画像座標を検出する。例えば、指標Ｑ_ｋの各々が異なる色を有するマーカによって構成されている場合には、撮影画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標Ｑ_ｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを撮影画像上に施すことにより、指標の位置を検出する。また、四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、４つの直線によって形成されている領域を指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の識別子を取得する。なお、このようにして検出される四角形指標は、本明細書では、４つの頂点の個々によって形成される４つの指標であると考える。

指標検出部１１０は、さらに、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。

位置姿勢算出部１２０は、センサ計測値入力部１５０の出力である撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、計測対象物体１７０あるいは撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

なお、図１に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。

図２は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々の機能を、ソフトウェアを実行することで実現するためのコンピュータの基本構成を示す図である。

ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々のソフトウェアの実行を制御して、各部の機能を実現する。

ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備えると共に、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。

ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データなどが格納されている。キーボード１００４、マウス１００５は入力デバイスであり、操作者はこれらを用いて、各種の指示をＣＰＵ１００１に入力することができる。

表示部１００６は、ＣＲＴや液晶ディスプレイなどにより構成されており、例えば、位置姿勢計測のために表示すべきメッセージ等を表示することができる。

外部記憶装置１００７は、ハードディスクドライブなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やＣＰＵ１００１が実行するプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。

記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出して、ＲＡＭ１００２や外部記憶装置１００７に出力する。

Ｉ／Ｆ１００９は、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、位置姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポート、また、算出した位置及び姿勢を外部へ出力するためのイーサネット（登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０及びセンサ計測値入力部１５０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
上述した各構成要素は、バス１０１０によって相互に接続される。

図３は、位置姿勢算出部１２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部１２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。

ステップＳ３０００において、位置姿勢算出部１２０は、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを指標抽出部１１０から入力する。なお、各指標の物体座標系における３次元座標ｘ_Ｏ ^Ｑｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。

ステップＳ３００５において、位置姿勢算出部１２０は、撮像装置１３０及び計測対象物体１７０の位置及び姿勢のセンサによる計測値をセンサ計測値入力部１５０から入力する。

ステップＳ３０１０において、位置姿勢算出部１２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ３０８０に処理を進め、それ以外の場合はステップＳ３０２０に処理を進める。

ステップＳ３０２０において、位置姿勢算出部１２０は、検出された指標の総数が一つか否かを判定する．指標の総数が一つの場合はステップＳ３０２５に処理を進め、それ以外の場合はステップＳ３０３０に処理を進める。

ステップＳ３０２５において、位置姿勢算出部１２０は、ステップＳ３００５で得た撮像装置１３０の姿勢の計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（物体座標系における指標の３次元座標ｘ_Ｏ ^Ｑｋｎと撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値から導出する投影座標の理論値ｕ^Ｑｋｎ＊と、実際の検出座標ｕ^Ｑｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。ステップＳ３０２５における補正処理としては、周知の処理を適用することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。

ステップＳ３０３０において、位置姿勢算出部１２０は、検出された指標の総数が二つか否かを判定する．指標の総数が二つの場合はステップＳ３０３３に処理を進め、それ以外の場合はステップＳ３０４０に処理を進める。

ステップＳ３０３３において、位置姿勢算出部１２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ_１（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ_１以上の場合にはステップＳ３０３５に処理を進め、それ以外の場合はステップＳ３０２５に処理を進め、上述した処理を行う。

ステップＳ３０３５において、位置姿勢算出部１２０は、ステップＳ３００５で得た計測対象物体１７０の位置及び姿勢の計測値と、撮像装置１３０の位置の計測値を固定値として、撮像装置１３０の姿勢の計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この処理工程の詳細は後述する。

ステップＳ３０４０において、位置姿勢算出部１２０は、検出された全指標（このステップが実行されるのは、検出された指標の総数が三つ以上の場合のみである）の画像座標を包含する凸包を算出する。画像上の点群に対する凸包の算出方法は画像処理の基本的事項として広く公知のものであるので、その詳細な説明は省略する。

図６，図７，及び図８は、撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示している。図６に示す撮影画像６００上では指標Ｑ１，Ｑ５，Ｑ６，Ｑ９が、図７の撮影画像７００上では指標Ｑ１，Ｑ２，Ｑ６，Ｑ７が，図８の撮影画像８００上では指標Ｑ２，Ｑ６，Ｑ７が観測されており、それ以外の指標は、他の物体（本実施形態においてはユーザの手６１０）によって隠蔽されているものとする。

これらの撮影画像に対して指標の検出を行い、図３のフローチャートで示した位置姿勢算出部１２０の処理ステップを実行すると、各画像において検出される指標の数は３以上であるので、ステップＳ３０４０以降の処理が実行されることになる。図９，図１０及び図１１は、それぞれ撮影画像６００，７００，８００に対してステップＳ３０４０の処理を実行した際に得られる、検出指標の成す凸包（９００，１０００，１１００）を示している。なお、以下では、各凸包の面積をそれぞれＡ_９００，Ａ_１０００，及びＡ_１１００と表記したときに、以下に述べる閾値Ｔ_２及び閾値Ｔ_３に対して、Ａ_１１００＜Ｔ_２＜Ａ_１０００＜Ｔ_３＜Ａ_９００という関係が満たされているものとする。

ステップＳ３０５０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ_２（例えば、撮影画像全体の面積の１／１６）との比較を行う。面積が閾値Ｔ_２以上の場合はステップＳ３０６０に処理を進め、それ以外の場合はステップＳ３０２５へと処理を進める。図６〜図１１の例では、撮影画像８００についてはステップＳ３０２５へ、撮影画像７００及び６００についてはステップＳ３０６０へと処理が進行する。

ステップＳ３０６０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ_３（例えば、撮影画像全体の面積の１／９）との比較を行う。面積が閾値Ｔ_３以上の場合はステップＳ３０７０に処理を進め、それ以外の場合はステップＳ３０３５へと処理を進める。図６〜図１１の例では、撮影画像７００についてはステップＳ３０３５へ、撮影画像６００についてはステップＳ３０７０へと処理が進行する。

ステップＳ３０７０において、位置姿勢算出部１２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程の詳細は後述する。

ステップＳ３０８０において、位置姿勢算出部１２０は、ステップＳ３０２５、Ｓ３０３５及びＳ３０７０で得られた撮像装置１３０及び計測対象物体１７０の位置及び姿勢を表すデータを、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

なお、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の各工程では、撮像装置１３０に関する計測値のみに補正を加えているので、ステップＳ３０８０で出力される位置及び姿勢のデータは、計測対象物体１７０に関しては、ステップＳ３００５で入力したセンサによる計測値そのものとなる。しかし、位置及び姿勢の出力形態はこれに限られるものではなく、逆に、撮像装置１３０の位置及び姿勢の計測値には補正を加えずにセンサによる計測値をそのまま出力し、計測対象物体１７０の位置及び姿勢の計測値に補正を加えて出力しても良い。この場合は、ステップＳ３０８０において、以下に述べる座標変換処理を行った後に、変換後の位置及び姿勢を出力する。

同次座標系を用いた４×４の座標変換行列によって位置及び姿勢を表記すると、計測対象物体１７０の補正後の位置及び姿勢M_WOは、センサ計測値として得られた撮像装置１３０の位置及び姿勢M^# _WC（以下、^# はセンサによる計測値を表す記号）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢M^# _WOと、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢M_WCに基づいて、次式によって算出される。

このとき、撮像装置１３０によって定義されるカメラ座標系における計測対象物体１７０の位置及び姿勢は、

となる。すなわち、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢（M_WC）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢（M^# _WO）とを出力する場合と、センサ計測値として得られた撮像装置１３０の位置及び姿勢（M^# _WC）と、式１によって求められる計測対象物体１７０の補正後の位置及び姿勢（M_WO）とを出力する場合では、計測対象物体１７０と撮像装置１３０の相対的な位置及び姿勢の関係は等価となる。したがって、本装置の出力を利用するアプリケーションが要求する形態に合わせて、出力形態を選択できるようになっていることが好ましい。

また、カメラ座標系における計測対象物体１７０の位置及び姿勢（M_CO）を式２から算出してこれを出力しても良いし、物体座標系における撮像装置１３０の位置及び姿勢（M_CO ^-1）を求めて、これを出力してもよい。

ステップＳ３０９０において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、ステップＳ３０００へと処理を進める。
以上によって、撮像装置の位置及び姿勢が計測される。

次に、図４のフローチャートを用いて、上記ステップＳ３０３５における撮像装置１３０の姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の姿勢を、３値ベクトル
s = ω_WC = [ξ_WC ψ_WC ζ_WC]^T
によって内部的に表現している。

姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。なお、姿勢ωは、次式、

によって、３×３の回転行列Rによっても表現可能であり、ωとRとは、互いに一意に変換することができる。Rからωへの変換方法は公知であるので、その詳細な説明は省略する。

ステップＳ４０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の姿勢を、sの初期値とする。

ステップＳ４０１０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を算出する。ｕ^Ｑｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、指標Ｑ_ｋｎ各々の物体座標系における座標ｘ_Ｏ ^Ｑｋｎ（既知な情報として予め保持している）から画像座標を算出する関数：

に基づいて行われる。具体的には、観測方程式Ｆｃ（）は、ｘ_Ｏ ^Ｑｋｎとsから当該指標のカメラ座標（撮像装置１３０の視点位置を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘ_Ｃ ^Ｑｋｎを求める次式、

及び、カメラ座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める次式、

によって構成されている。ここでt_WC及びt_WOは、撮像装置１３０と計測対象物体１７０の夫々の位置計測値（平行移動成分）を表す３次元ベクトルであり、ここでは固定値として取り扱われる。また、R_WOは計測対象物体１７０の姿勢計測値（回転成分）を表す３×３の回転行列であり、同様に固定値として取り扱われる。また、ｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。

ステップＳ４０２０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、画像座標の推定値ｕ^Ｑｋｎ＊と実測値ｕ^Ｑｋｎとの誤差Δｕ^Ｑｋｎを、次式によって算出する。

ステップＳ４０３０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、sに関する画像ヤコビアン（すなわち、式４の観測方程式Ｆｃ（）をsの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列）Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘ_Ｃ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｑｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列Ｊ_ｘｓ ^Ｑｋｎ（＝∂ｘ／∂ｓ）を算出し、次式によってＪ_ｕｓ ^Ｑｋｎを算出する。

ステップＳ４０４０において、位置姿勢算出部１２０は、ステップＳ４０２０及びステップＳ４０３０において算出した誤差Δｕ^Ｑｋｎ及び画像ヤコビアンＪ_ｕｓ ^Ｑｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差Δｕ^Ｑｋｎを垂直に並べた２Ｎ次元の誤差ベクトル

及び、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎを垂直に並べた２Ｎ行×３列の行列

を作成し、Θの擬似逆行列Θ⁺を用いて、

として算出する。このように、本実施形態では補正値Δsを、最急降下法を用いて算出している。

ステップＳ４０５０において、位置姿勢算出部１２０は、ステップＳ４０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。

ステップＳ４０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のsを用いて、再度ステップＳ４０１０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ４０７０へと処理を進める。

ステップＳ４０７０において、位置姿勢算出部１２０は、ステップＳ４０６０までの処理によって得られたsを、撮像装置１３０の姿勢の（補正後の）推定値 ω_WCとする。

以上によって、センサ計測値として得られた位置パラメータを固定して、全指標上での誤差の和を最小化するような姿勢パラメータを求めることができる。

次に、図５のフローチャートを用いて、上記ステップＳ３０７０における撮像装置１３０の位置及び姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の位置及び姿勢を、６値ベクトルs = [t_WC ^T ω_WC ^T]^T = [x_WC y_WC z_WC ξ_WC ψ_WC ζ_WC]^Tによって内部的に表現している。

ステップＳ５０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の位置及び姿勢を、sの初期値とする。

ステップＳ５０１０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を算出する。ｕ^Ｑｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、各指標Ｑ_ｋｎの物体座標系における座標ｘ_Ｏ ^Ｑｋｎから画像座標を算出する関数：

に基づいて行われる。具体的には、観測方程式Ｆ’ｃ（）は、ｘ_Ｏ ^Ｑｋｎとsから当該指標のカメラ座標ｘ_Ｃ ^Ｑｋｎを求める式５、及び、カメラ座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める式６によって構成されている。ただしここでは、式５におけるt_WC（撮像装置１３０の位置を表す）を、固定値としてではなく、sを構成するパラメータの一部として取り扱う。一方、計測対象物体１７０の位置t_WO及び姿勢R_WOに関しては、センサによる計測値をそのまま固定値として使用する。

ステップＳ５０２０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、画像座標の推定値ｕ^Ｑｋｎ＊と実測値ｕ^Ｑｋｎとの誤差Δｕ^Ｑｋｎを、式７によって算出する。

ステップＳ５０３０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、sに関する画像ヤコビアン（すなわち、式１３の観測方程式Ｆ’ｃ（）をsの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘ_Ｃ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｑｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列Ｊ_ｘｓ ^Ｑｋｎ（＝∂ｘ／∂ｓ）を算出し、式８によってＪ_ｕｓ ^Ｑｋｎを算出する。

ステップＳ５０４０において、位置姿勢算出部１２０は、ステップＳ５０２０及びステップＳ５０３０において算出した誤差Δｕ^Ｑｋｎ及び画像ヤコビアンＪ_ｕｓ ^Ｑｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差Δｕ^Ｑｋｎを垂直に並べた２Ｎ次元の誤差ベクトルＵ及び、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎを垂直に並べた２Ｎ行×６列の行列Θを作成し、Θの擬似逆行列Θ⁺を用いて、式１１によって算出する。

ステップＳ５０５０において、位置姿勢算出部１２０は、ステップＳ５０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。

ステップＳ５０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後のsを用いて、再度ステップＳ５０１０以降の処理を行う。一方、収束していると判定された場合には、ステップＳ５０７０へと処理を進める。

ステップＳ５０７０において、位置姿勢算出部１２０は、ステップＳ５０６０までの処理によって得られたsを、撮像装置１３０の補正後の位置及び姿勢の推定値とする。

なお、本実施形態では、ステップＳ３０７０の処理において、撮像装置１３０の位置及び姿勢を補正の対象としていたが、計測対象物体１７０の位置及び姿勢を補正の対象として処理を行っても良い。この場合は、算出すべき未知パラメータを、計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [t_WO ^T ω_WO ^T]^T = [x_WO y_WO z_WO ξ_WO ψ_WO ζ_WO]^Tによって内部的に表現し、センサ計測値として得られた計測対象物体１７０の位置及び姿勢をsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。

このとき、撮像装置１３０の位置t_WC及び姿勢R_WCに関しては、センサによる計測値をそのまま固定値として使用する。

また、カメラ座標系における計測対象物体１７０の位置及び姿勢や、物体座標系における撮像装置１３０の位置及び姿勢を未知のパラメータとして求めても良い。

前者の場合、算出すべき未知パラメータを、カメラ座標系における計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [t_CO ^T ω_CO ^T]^T = [x_CO y_CO z_CO ξ_CO ψ_CO ζ_CO]^Tによって内部的に表現する。また、カメラ座標系における計測対象物体１７０の位置及び姿勢（M^# _CO）を、次式、

によってセンサ計測値（M^# _WC及びM^# _WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。

一方、後者の場合は、算出すべき未知パラメータを、物体座標系における撮像装置１３０の位置及び姿勢を表す６値ベクトルs = [t_OC ^T ω_OC ^T]^T = [x_OC y_OC z_OC ξ_OC ψ_OC ζ_OC]^Tによって内部的に表現する。また、物体座標系における撮像装置１３０の位置及び姿勢（M^# _CO）を、次式、

によってセンサ計測値（M^# _WC及びM^# _WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。

以上のようにして、計測対象物体１７０と撮像装置１３０との間の相対的な位置及び姿勢の関係を未知のパラメータとして、センサによる位置及び姿勢計測値の誤差を補正することができる。

従来の位置姿勢計測装置では、指標の分布を考慮していないため、画像上の一部の領域に指標が偏在して観測されている場合に解が不安定になる。一方、本実施形態に係る位置姿勢計測装置によれば、ステップＳ３０３３や、ステップＳ３０４０，Ｓ３０５０，Ｓ３０６０によって指標の分布範囲の大きさを考慮した手法の選択が行われるため、例えば検出された指標の数が多くても、それらが画像上の小領域に偏在しているような場合に、位置及び姿勢の６自由度を安定して求めるのに不十分であるという判定を行うことができる。そのため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、不安定な解が得られる状況に陥る可能性が低下し、従来手法と比して安定した解を得ることができる。

また、本実施形態に係る位置姿勢計測装置によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行う場合に、その制約の中で指標上での誤差の和を最小化するような補正を行うことができる。したがって、未知とするパラメータを減らすことによって撮像装置の位置及び姿勢の計測を安定化させようとする場合であっても、従来と比べて高精度な計測を行うことができる。

［第２の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等に設定された基準座標系における任意の計測対象物体の位置及び姿勢の計測を行う。

本実施形態に係る位置姿勢計測装置は、撮像装置に位置姿勢センサが取り付けられておらず、三脚等によって既知の位置及び姿勢に固定して設置されていることが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。

図１２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１２００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。

撮像装置１３０は、三脚１２８０によって、空間中の既知の位置、姿勢に固定して設置されている。撮像装置１３０が出力する撮影画像は、位置姿勢計測装置１２００の画像入力部１６０に入力される。基準座標系における撮像装置１３０の位置及び姿勢は、既知の値として予め保持されているものとする。

センサ計測値入力部１５０は、計測対象物体１７０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１２２０へと出力する。

位置姿勢算出部１２２０は、センサ計測値入力部１５０の出力である計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、計測対象物体１７０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

なお、図１２に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。

本実施形態における位置姿勢算出部１２２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）とほぼ同一である。唯一の違いは、撮像装置１３０の位置及び姿勢が、計測値として与えられるのではなく、既知の値として予め保持されていることである。位置姿勢算出部１２２０は、撮像装置１３０の位置及び姿勢の既知の値を、撮像装置１３０の位置及び姿勢の仮の計測値とおき、第１の実施形態におけるステップＳ３０００からステップＳ３０７０までと同一の工程によって撮像装置１３０の位置及び姿勢の計測値の補正を行い、この補正結果に基づいて、ステップＳ３０８０において、第１の実施形態と同様に計測対象物体１７０の位置及び姿勢の計測値を補正する（式１）。本実施形態では、位置姿勢算出部１２２０がステップＳ３０８０において出力するのは、計測対象物体１７０の位置及び姿勢のみである。

以上によって、計測対象物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。

［第３の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。

本実施形態においては撮像装置以外の計測対象物体は存在せず、本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢の計測のみを行うことが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。

図１３は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１３００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。

また、現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標として、基準座標系における位置が既知である複数個の指標Ｑ_ｋが配置されている。指標Ｑ_ｋは、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、どのような形態であってもよい。

センサ計測値入力部１５０は、撮像装置１３０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１３２０へと出力する。

位置姿勢算出部１３２０は、センサ計測値入力部１５０の出力である撮像装置１３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

なお、図１３に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。

本実施形態における位置姿勢算出部１３２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）と類似しているため、図３を参照しながら、第１の実施形態との相違点のみについて説明する。

ステップＳ３００５において、位置姿勢算出部１３２０は、センサによって計測された撮像装置１３０の位置及び姿勢を、センサ計測値入力部１５０から入力する。

ステップＳ３０２５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の姿勢計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（基準座標系における指標の３次元座標ｘ_Ｗ ^Ｑｋｎと撮像装置１３０の位置及び姿勢の計測値から導出する投影座標の理論値ｕ^Ｑｋｎ＊と、実際の検出座標ｕ^Ｑｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。

このような補正処理としては、周知の方法を用いることができるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。

ステップＳ３０３５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の位置計測値を固定値として、撮像装置１３０の姿勢計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この補正処理工程は、第１の実施形態におけるステップＳ３０３５の補正処理工程（図４のフローチャートに対応）と類似したものとなる。ただし、本実施形態の場合、指標の観測方程式Ｆ’’ｃ（）が、基準座標系における指標Ｑ_ｋｎの座標ｘ_Ｗ ^Ｑｋｎ（既知な情報として予め保持している）から画像座標を算出する関数となる。すなわち、観測方程式Ｆ’’ｃ（）は次式によって表される。

具体的には、観測方程式Ｆ’’ｃ（）は、ｘ_Ｗ ^Ｑｋｎとsから当該指標のカメラ座標ｘ_Ｃ ^Ｑｋｎを求める次式、

及び、カメラ座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める式６によって構成されており、この点のみが異なっている。ここでt_WCは、撮像装置１３０の位置計測値を表す３次元ベクトルであり、ここでは固定値として取り扱われる。

ステップＳ３０７０において、位置姿勢算出部１３２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程での具体的な処理は、例えば特許文献２に開示されているような周知技術を用いて実現することが可能であるので、ここでの説明は省略する。

ステップＳ３０８０において、位置姿勢算出部１３２０は、ステップＳ３０７０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

以上によって、撮像装置の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。

［第４の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における任意の計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。本実施形態においては、撮像装置ではなく、任意の計測対象物体の位置及び姿勢を計測することが、第３の実施形態と異なっている。以下では、第３の実施形態との相違点のみを説明する。

図１４は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１４００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、撮像部１４３０、位置姿勢計測部１４４０、及び位置姿勢算出部１４２０によって構成されており、計測対象物体１４７０に接続されている。

撮像部１４３０は、位置及び姿勢を計測したい対象である計測対象物体１４７０に固定して取り付けられており、指標を含む現実空間の映像を撮影する。

位置姿勢計測部１４４０は、撮像部１４３０に固定して装着されており、基準座標系における撮像部１４３０の位置及び姿勢を計測してセンサ計測値入力部１５０へと出力する。位置姿勢計測部１４４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。

画像入力部１６０は、撮像部１４３０が撮影した撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。

センサ計測値入力部１５０は、撮像部１４３０の位置及び姿勢の計測値を位置姿勢計測部１４４０から入力し、位置姿勢算出部１４２０へと出力する。

位置姿勢算出部１４２０は、センサ計測値入力部１５０の出力である撮像部１４３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、撮像部１４３０の位置及び姿勢の計測値の誤差を補正する。
位置姿勢算出部１４２０はさらに、得られた撮像部１４３０の位置及び姿勢と、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係についての情報（具体的には、計測対象物体１４７０上のどの位置に撮像部１４３０が設置されているかという情報。計測対象物体１４７０によって定義される物体座標系における、撮像部１４３０の位置及び姿勢によって表現される）から、基準座標系における計測対象物体１４７０の位置及び姿勢を算出し、Ｉ／Ｆ１００９を介してこのデータを外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。なお、この変換工程は必ずしも必要ではなく、撮像部１４３０の位置及び姿勢をそのまま出力することも可能である。

なお、本実施形態では、位置姿勢計測部１４４０は撮像部１４３０の位置及び姿勢を計測していたが、位置姿勢計測部１４４０は計測対象物体１４７０の位置及び姿勢を計測するように構成されていても良い。この場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた計測対象物体１４７０の位置及び姿勢から撮像部１４３０の位置及び姿勢を算出し、これを初期値として上記と同様な処理を実行する。

また、位置姿勢算出部１４２０は、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の何れの工程においても、撮像部１４３０の姿勢あるいは位置及び姿勢を補正し、その後計測対象物体１４７０の位置及び姿勢を求めていた。しかし、ステップＳ３０３５及びＳ３０７０の工程においては、計測対象物体１４７０の位置及び姿勢を直接求めるような構成としても良い。

位置姿勢計測部１４４０が撮像部１４３０の位置及び姿勢を計測している場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた撮像部１４３０の位置及び姿勢から計測対象物体１４７０の位置及び姿勢を算出し、これを初期値とする。

ステップＳ３０３５においては、計測対象物体１４７０の姿勢を表す３値ベクトルs = ω_WO = [ξ_WO ψ_WO ζ_WO]^Tと未知パラメータとする。また、ステップＳ３０７０においては、計測対象物体１４７０の位置及び姿勢を表す６値ベクトルs = [t_WO ^T ω_WO ^T]^T = [x_WO y_WO z_WO ξ_WO ψ_WO ζ_WO]^Tを未知パラメータとする。そして、観測方程式Ｆ’’ｃ（）を定義するのに必要な式２０を、

に変更する。ここで、t_OC 及びR_OCは、計測対象物体１７０によって定義される物体座標系における撮像部１４３０の姿勢を表す３×３の回転行列及び位置を表す３次元ベクトルであり、撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を表す既知の値として、予め保持されているものとする。なお、ステップＳ３０３５では、t_WOは固定値として設定する。

以上によって、任意の物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。

［第５の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。

図１５は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１５００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５５０、及び位置姿勢算出部１５２０によって構成されており、撮像装置１３０及び姿勢センサ１５４０に接続されている。

本実施形態においては、撮像装置１３０に、位置姿勢センサではなく姿勢センサが装着されている点が、第３の実施形態と異なっている。姿勢センサを用いることで、位置姿勢センサを用いる場合に存在する、センサの計測範囲以外の領域を計測できないという制約を回避することができる。以下では、第３の実施形態との相違点のみを説明する。

姿勢センサ１５４０は、撮像装置１３０に装着されており、姿勢センサ１５４０自身の現在の姿勢を計測してセンサ計測値入力部１５５０へと出力する。姿勢センサ１５４０は、例えばジャイロセンサをベースとしたセンサユニットであり、具体的には、株式会社トキメックのTISS-5-40や、米国InterSense社のInertiaCube2等によって構成される。これらのセンサによって計測される姿勢計測値は、真の姿勢とは異なった、誤差を有する姿勢である。ただし、これらの姿勢センサは、地球の重力方向を観測する傾斜角センサを構成要素として有しており、傾斜角方向（ピッチ角及びロール角）のドリフト誤差の蓄積をキャンセルする機能を有しているので、傾斜角方向に関してはドリフト誤差を発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては時間の経過に伴って蓄積されるドリフト誤差を有している。

センサ計測値入力部１５５０は、方位ドリフト誤差補正値の更新値を位置姿勢算出部１５２０から入力し、姿勢センサ１５４０の現在の方位ドリフト誤差補正値を更新し保持する。また、姿勢センサ１５４０から姿勢計測値を入力し、現在の方位ドリフト誤差補正値によってこれを補正して、撮像装置１３０の姿勢の予測値として位置姿勢算出部１５２０へと出力する。

位置姿勢算出部１５２０は、センサ計測値入力部１５５０の出力である撮像装置１３０の姿勢の予測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力データとして、撮像装置１３０の位置及び姿勢を算出して出力する。また、位置及び姿勢の算出工程で導出される姿勢センサ１５４０の方位ドリフト誤差補正値の更新値を、センサ計測値入力部１５５０へと出力する。

なお、図１５に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。

図１６は、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部１５２０のソフトウェアのプログラムを実行することで行われる。

ステップＳ１５０００において、位置姿勢算出部１５２０は、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを指標抽出部１１０から入力する。なお、各指標の基準座標系における３次元座標ｘ^Ｑｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。

ステップＳ１５００５において、位置姿勢算出部１５２０は、撮像装置１３０の姿勢の予測値R^*をセンサ計測値入力部１５５０から入力し、これを、撮像装置１３０の姿勢の初期値として設定する。また、２回目以降の処理では、１ループ前の処理において算出された撮像装置１３０の位置を、撮像装置１３０の姿勢の初期値として設定する。

ステップＳ１５０１０において、位置姿勢算出部１５２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ１５０９０に処理を進め、それ以外の場合はステップＳ１５０２０に処理を進める。

ステップＳ１５０２０において、位置姿勢算出部１５２０は、検出された指標の総数が一つか否かを判定する．指標の総数が一つの場合はステップＳ１５０２５に処理を進め、それ以外の場合はステップＳ１５０３０に処理を進める。

ステップＳ１５０２５において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値の平行移動成分のうちの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加え、ステップＳ１５０９０に処理を進める。ステップＳ１５０２５での処理工程のとしては、周知の処理を提供することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの平行移動による補正方法（段落「００３０」〜「００３６」）およびランドマークを複数点利用したカメラの平行移動による補正方法（段落「００５１」〜「００５６」）に開示されている方法を用いることができる。ただし、特許文献１では６自由度位置姿勢センサの計測値を誤差補正の対象とした場合を説明しているが、本実施形態では、前フレームで求めた位置と姿勢センサの計測値とを組み合わせたものが補正対象となる点が異なっている。

ステップＳ１５０３０において、位置姿勢算出部１５２０は、検出された指標の総数が二つか否かを判定する．指標の総数が二つの場合はステップＳ１５０３２に処理を進め、それ以外の場合はステップＳ１５０４０に処理を進める。

ステップＳ１５０３２において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ_１（例えば、画像の対角線長の１／８）との比較を行う。距離が閾値Ｔ_１以上の場合にはステップＳ１５０３４に処理を進め、それ以外の場合はステップＳ１５０２５に処理を進める。

ステップＳ１５０３４において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離と所定の値として定められている閾値Ｔ_２（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ_２以上の場合にはステップＳ１５０３８に処理を進め、それ以外の場合はステップＳ１５０３６に処理を進める。

ステップＳ１５０３６において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうち、位置を表す３パラメータ（平行移動成分）を更新対象として（すなわち、姿勢計測値には誤差は含まれていないものと仮定して）、検出されている指標上での誤差を最小とするような補正を位置のパラメータのみに加え、ステップＳ１５０９０に処理を進める。この処理工程の詳細は、例えば特許文献３（第６の実施形態）に開示されているので、ここでの説明は省略する。また、位置を表す３パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置の補正を行うことができる。

ステップＳ１５０３８において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうちの位置を表す３パラメータ（平行移動成分）と、姿勢センサ１３４０の方位ドリフト誤差補正値の更新値φの合計４パラメータとを、算出すべき未知パラメータとして取り扱い、検出されている指標の情報を用いてこれらを算出する。この処理工程の詳細は、例えば特許文献３（第７の実施形態）に開示されているので、ここでの説明は省略する。また、これら４パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置と方位ドリフト誤差補正値の更新値の算出を行うことができる。さらに、ステップＳ１５０８０へと処理を進める。

ステップＳ１５０４０において、位置姿勢算出部１５２０は、検出された全指標（このステップが実行されるのは、指標の総数が三つ以上の場合のみである）の画像座標を包含する凸包を算出する。

ステップＳ１５０５０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ_３（例えば、画像の面積の１／１６）との比較を行う。面積が閾値Ｔ_３以上の場合はステップＳ１５０６０に処理を進め、それ以外の場合はステップＳ１５０３６へと処理を進め、上述した処理を行う。

ステップＳ１５０６０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ_４との比較を行う。面積が閾値Ｔ_４（例えば、画像の面積の１／９）以上の場合はステップＳ１５０７０に処理を進め、それ以外の場合はステップＳ１５０３８へと処理を進め、上述した処理を行う。

ステップＳ１５０７０において、位置姿勢算出部１５２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この算出は、例えば、撮像装置１３０の位置と姿勢を表す６パラメータを変数として、ステップＳ１５００５で設定した初期値を用いて、非線形方程式の反復解法によって誤差を最小化するパラメータを求めることによって実現される。すなわち、位置と姿勢を表す６パラメータを未知変数とした観測方程式を構成し、これに基づいて求められる指標投影位置の誤差と各パラメータに関する画像ヤコビアンとを用いて当該パラメータの補正値を求め、補正処理を反復的に行うことで画像上での誤差を最小とするような位置と姿勢を算出する。
位置姿勢算出部１５２０はさらに、ステップＳ１５００５で設定した姿勢の初期値と求められた姿勢との変化分を算出し、その方位成分を方位ドリフト誤差補正値の更新値φとする。

ステップＳ１５０８０において、位置姿勢算出部１５２０は、ステップＳ１５０３８あるいはステップＳ１５０７０において得られた方位ドリフト誤差補正値の更新値φを、センサ計測値入力部処理１５５０へと出力する。

ステップＳ１５０９０において、位置姿勢算出部１５２０は、ステップＳ１５０８０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

ステップＳ１５１００において、位置姿勢算出部１５２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ１５０００へと進める。

以上によって、撮像装置の位置及び姿勢が計測される。
本実施形態に係る位置姿勢計測装置によれば、ステップＳ１５０３４やステップＳ１５０５０によって指標の分布範囲の大きさを考慮した手法の選択（方位ドリフト誤差補正値の更新を行うか否かの判断）が行われるため、画像上の一部の領域に指標が偏在して観測されている場合に、方位ドリフト誤差補正値の更新を不正確に行ってしまうという状況を回避することができる。

また、本実施形態に係る位置姿勢計測装置によれば、第３の実施形態の効果に加え、ステップＳ１５０６０による手法の選択によって、画像情報が不足する場合であっても姿勢センサによる傾斜角情報を信頼することで位置及び姿勢を安定して導出できるほか、画像情報が十分得られる場合であれば位置及び姿勢の推定を高精度に行えるという、通常の画像情報に基づく位置姿勢推定手法が有する利点を両立することができる。

［他の実施形態］
（変形例１）
上記の実施形態の各々では、画像上における指標の分布に基づいて、適用する補正アルゴリズムが選択される。これらの補正アルゴリズム（第１の実施形態におけるステップＳ３０２５，Ｓ３０３５，Ｓ３０７０、第２乃至第４の実施形態におけるこれらの変形、及び、第５の実施形態におけるステップＳ１５０２５，Ｓ１５０３６，Ｓ１５０３８，Ｓ１５０７０）は、必ずしも上記の実施形態で示したものである必要はなく、指標の分布に従って見込まれる画像情報の情報量に応じて適切に設定されたものであれば、他の補正アルゴリズムに変更することも可能である。

例えば、第１乃至第４の実施形態におけるステップＳ３０２５やＳ３０３５において、姿勢の計測値を補正するのではなく、位置の計測値を補正する構成としてもよい。この場合、ステップＳ３０２５では、例えば、特許文献１の段落「００３０」〜「００３６」および「００５１」〜「００５６」に開示されているような、１つ又は複数の指標を利用したカメラの平行移動による補正方法に基づいて、位置計測値のうちのカメラの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加えることができる。

また、ステップＳ３０３５では、算出すべき未知パラメータを、撮像装置１３０の位置を表す３値ベクトルs = t_WC = [x_WC y_WC z_WC ]^Tとして、図５と同様な処理手順によって位置の補正を行う。第１の実施形態との相違点は、観測方程式を構成する式５を、次式のように変更することである。

ここで、R_WCは、固定値として用いる撮像装置１３０のセンサによる姿勢計測値を表している。また、R_WO 及びt_WOは、第１の実施形態における式５と同様に、計測対象物体１７０の位置及び姿勢のセンサによる計測値を表しており、固定値として取り扱われる。

なお、式２２は線形方程式であるので、式６と組み合わせた観測方程式から連立方程式を作成しこれを解くことで、繰り返し演算を用いなくても補正値を得ることができる。

なお、不図示のＵＩを介して、姿勢の補正と位置の補正の何れを優先させるかをユーザが自由に選択できるようにしてもよい。この場合、ステップＳ３０２５及びＳ３０３５の実行時に、ユーザの選択に応じて、姿勢あるいは位置の補正アルゴリズムが実行される。これにより、特性の異なる位置姿勢センサを用いた場合でも、より好ましい計測を行うことができる。

また、ステップＳ３０２５においては、計測値として得られる６パラメータのうちの任意の２パラメータを補正対象として選択し、これを未知数として上述した方法と記と同様にして求めてもよい。同様に、ステップＳ３０３５においては、計測値として得られる６パラメータのうちの任意の３パラメータを補正対象として選択し、これを未知数として上述した方法と同様な枠組みによって求めてもよい。

（変形例２）
上記第１乃至第４の実施形態では、画像上における指標の分布に基づいて、３種類のアルゴリズムへと処理を分岐させていた（ステップＳ３０３３、ステップＳ３０５０及びステップＳ３０６０）。しかし、指標の分布に基づいて行う分岐数は３通りに限られるものではない。例えば、ステップＳ３０５０の処理を省いて、閾値Ｔ_３と凸包の面積の大小関係によってステップＳ３０３５あるいはステップＳ３０７０へと処理を分岐させるだけでもよい。また、より多数の閾値を設けて、４通り以上の分岐を行ってもよい。例えば、補正対象とするパラメータの数（２〜６）を凸包の面積に応じて変更し、これを未知数として同様な方法により位置姿勢を算出してもよい。

（変形例３）
上記の実施形態の各々では、位置姿勢センサや姿勢センサによる計測値と、撮像装置によって得られる画像情報を併用することにより、計測対象（計測対象物体や撮像装置）の位置及び姿勢を求めていた。しかし、画像上における指標の分布に基づいて、位置及び姿勢を求めるために用いるアルゴリズムを選択するという基本的な技術思想は、これらのセンサによる計測値を用いずに、画像情報のみから計測対象の位置及び姿勢を得る場合であっても適用できる。
例えば、図１３に示した第３の実施形態の構成において、位置姿勢計測装置１３００がセンサ計測値入力部１５０を有さず、また、位置姿勢センサ１４０が撮像装置１３０に設置されていない場合を考える。この場合、位置姿勢算出部１３２０は、初期値として位置姿勢センサ１４０による撮像装置１３０の位置及び姿勢の計測値を用いるのではなく、前フレームで求めた撮像装置１３０の位置及び姿勢の算出値（あるいは、過去の算出値から予測した現フレームにおける位置及び姿勢）を用いることにすると、これを仮の計測値とおいて図３と同様な処理を行うことで、指標の分布に応じて適切な位置及び姿勢の算出アルゴリズムを選択することが可能となる。

（変形例４）
上記の実施形態の各々では、指標の分布を計る尺度として、指標が３点以上検出されている場合には、指標が成す凸包を求めてその面積を利用していた。しかし、指標の分布を測る尺度はこれに限られるものではなく、他の尺度を用いてもよい。例えば、画像中心から最も遠い位置にある指標を第１の指標として選択し、第１の指標から最も遠い位置にある指標を第２の指標として選択し、さらに、第１，第２の指標を結ぶ直線から最も遠い距離にある指標を第３の指標として選択し、これら第１，第２，第３の指標が成す三角形の面積を分布の尺度として、凸包の面積の変わりに用いても良い。また、簡易的には、このような第１，第２の指標間の距離を分布の尺度としても良いし、指標の画像座標の分散や標準偏差といった、分布を数値で表しうる他の任意の情報を分布の尺度として用いてもよい。

（変形例５）
第１の実施形態におけるステップＳ３０３５では、図４のステップＳ４０４０にて詳細に説明したように、誤差ベクトルＵと行列Θに基づく補正値Δsの算出に、式１１で表現される最急降下法を用いている。しかし、補正値Δsの算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を用いて求めてもよいし、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても発明の本質が損なわれないことは言うまでもない。また、上記の実施形態におけるsを状態ベクトル、式４を観測方程式とすることで、画像ヤコビアンを利用して位置姿勢センサによる計測値のうちの一部のパラメータのみを補正するという効果を有した拡張カルマンフィルタ（Extended Kalman Filter）や、反復拡張カルマンフィルタ（iterative Extended Kalman Filter）を構成することができる。なお、拡張カルマンフィルタ及び反復拡張カルマンフィルタについては非特許文献２に記載されるように周知であるので、その詳細に関するこれ以上の説明は省略する。

（変形例６）
上記の実施形態の各々においては、各々が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を用いていた。しかし、点指標以外の指標を用いることも可能である。例えば、公知の位置姿勢計測装置（例えば、非特許文献３を参照）に用いられているような、線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。例えば、原点から線指標までの距離を評価のための基準として画像からの検出値とsによる推定値から算出する誤差によって誤差ベクトルＵを構成し、観測方程式をsの各要素で偏微分した解を各要素に持つ１行×６列のヤコビ行列によって行列Θを構成することで、上記実施形態と同様にして位置及び姿勢の計測（補正）を行うことができる。また、線指標と点指標、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することも可能である。

（変形例７）
上記の実施形態の各々では、画像面積に対する指標の分布に応じて以降の処理を選択していた。しかし、計測対象物体に対する相対的な比率を尺度としてもよい。例えば、第１の実施形態において、ステップＳ３０４０で、全検出指標の成す凸包の面積Ｓ_Ｈに加えて、画像上への計測対象物体１７０（その大まかな形状を表す３次元データが記憶されているものとする）の投影像の面積Ｓ_Ｏを求め、これらの比率（Ｓ_Ｈ／Ｓ_Ｏ）を得る。そして、上記比率に対して適宜設定した閾値を用いてステップＳ３０５０とステップＳ３０６０での条件分岐を行えばよい。

また、画像中ではなく、物理空間における指標の分布を尺度とすることも可能である。例えば、第１の実施形態において、ステップＳ３０４０で、物体座標系における全検出指標の３次元座標によって構成される３次元凸包を求め、その体積Ｖ_Ｈを算出し、計測対象物体１７０の体積Ｖ_Ｏとの比率（Ｖ_Ｈ／Ｖ_Ｏ）を得ることで、計測対象物体１７０上における検出指標の分布の割合を導出する。そして、上記比率に対して適宜設定した閾値を用いてステップＳ３０５０とステップＳ３０６０での条件分岐を行えばよい。また、物体座標系における互いが最も遠い３点の検出指標によって構成される三角形の面積を求め、計測対象物体１７０のサイズ（例えば、外接直方体の最も長い２辺が成す矩形の面積）との比率を求め、これを尺度として用いることも可能である。

本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても達成される。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明したフローチャートに対応するプログラムコードが格納されることになる。

第１の実施形態における位置姿勢計測装置の構成を示す図である。 位置姿勢計測装置の各部をソフトウェアにより実現することのできるコンピュータの基本構成を示す図である。 実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。 図３のステップＳ３０３５における位置及び姿勢算出処理の詳細を説明するフローチャートである。 図３のステップＳ３０７０における位置及び姿勢算出処理の詳細を説明するフローチャートである。 撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。 撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。 撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。 図６の撮影画像（撮影画像６００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。 図７の撮影画像（撮影画像７００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。 図８の撮影画像（撮影画像８００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。 第２の実施形態における位置姿勢計測装置の構成を示す図である。 第３の実施形態における位置姿勢計測装置の構成を示す図である。 第４の実施形態における位置姿勢計測装置の構成を示す図である。 第５の実施形態における位置姿勢計測装置の構成を示す図である。 第５の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。

Claims (26)

1. 撮像装置の位置姿勢を構成する複数のパラメータの１つ以上を補正する位置姿勢補正装置であって、
   前記複数のパラメータを入力する位置姿勢入力手段と、
   現実空間に複数配置された指標を前記撮像装置で撮像した画像を入力する画像入力手段と、
   前記画像から前記指標の画像座標の実測値を検出する検出手段と、
   前記検出手段による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定手段と、
   前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち、前記決定手段によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正手段と、
   を有し、
   前記補正手段が、
   前記指標の座標と前記複数のパラメータとに基づいて、前記指標の画像座標の理論値を算出する理論値算出手段と、
   前記実測値と前記理論値との誤差を小さくするように、前記補正対象と決定されたパラメータの補正値を算出する補正値算出手段と、
   前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、
   を有することを特徴とする位置姿勢補正装置。
2. 前記指標は、前記現実空間に固定され、位置が既知であることを特徴とする請求項１に記載の位置姿勢補正装置。
3. 前記指標は、現実空間に存在する撮像対象物体に取り付けられ、
   前記位置姿勢補正装置は、
   前記撮像対象物体の位置姿勢を構成する複数のパラメータを入力する撮像対象位置姿勢入力手段
   を更に有することを特徴とする請求項１に記載の位置姿勢補正装置。
4. 計測対象物体の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置であって、
   前記複数のパラメータを入力する位置姿勢入力手段と、
   前記計測対象物体に複数配置された指標を撮像装置で撮像した画像を入力する画像入力手段と、
   前記撮像画像から前記指標の画像座標の実測値を検出する検出手段と、
   前記検出手段による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定手段と、
   前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち前記決定手段によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正手段と、
   を有し、
   前記補正手段が、
   前記複数のパラメータに基づいて、前記指標の画像座標の理論値を算出する理論値算出手段と、
   前記実測値と前記理論値との誤差を小さくするように、前記補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出手段と、
   前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、
   を有することを特徴とする位置姿勢補正装置。
5. 前記位置姿勢入力手段が入力する前記複数のパラメータは、位置姿勢センサによって計測した計測値であることを特徴とする請求項１乃至４の何れか１項に記載の位置姿勢補正装置。
6. 前記位置姿勢入力手段が入力する前記複数のパラメータのうち姿勢成分は、姿勢センサによって計測した計測値であって、
   前記決定手段が、前記検出手段による検出結果に基づいて、前記複数のパラメータの前記姿勢成分の全てを、前記複数のパラメータの位置成分よりも優先して前記補正対象のパラメータと決定することを特徴とする請求項１乃至４の何れか１項に記載の位置姿勢補正装置。
7. 前記位置姿勢入力手段が現実空間に固定され、既知の位置姿勢を構成する複数のパラメータを入力することを特徴とする請求項４に記載の位置姿勢補正装置。
8. 前記補正値算出手段が、前記実測値と前記理論値との誤差および、前記指標の画像ヤコビアンに基づいて、前記補正値を算出することを特徴とする請求項１乃至７の何れか１項に記載の位置姿勢計測装置。
9. 前記決定手段が、前記検出結果に基づく個数のパラメータを前記補正対象のパラメータとして決定することを特徴とする請求項１乃至８の何れか１項に記載の位置姿勢補正装置。
10. 前記複数のパラメータは、位置を示す３つの位置成分および姿勢を示す３つの姿勢成分の計６成分に相当するパラメータであることを特徴とする請求項１乃至９の何れか１項に記載の位置姿勢補正装置。
11. 前記決定手段が、前記検出手段で検出された指標が１つである場合に、前記複数のパラメータのうち２成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする請求項１０に記載の位置姿勢補正装置。
12. 前記決定手段が、前記検出手段で検出された指標が２つである場合に、前記複数のパラメータのうち３成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする請求項１０又は１１に記載の位置姿勢補正装置。
13. 前記決定手段が、前記検出手段で検出された指標が２つである場合に、該指標の画像座標間の距離に基づいて、前記複数のパラメータのうち３成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする請求項１２に記載の位置姿勢補正装置。
14. 前記決定手段が、前記検出手段で検出された指標が２つである場合に、該指標の画像座標間の距離が、予め設定された距離の閾値以上であれば前記複数のパラメータのうち３成分に相当するパラメータを、該距離の閾値未満であれば３成分未満に相当するパラメータを、前記補正対象のパラメータとして決定することを特徴とする請求項１３に記載の位置姿勢補正装置。
15. 前記決定手段が、前記検出手段で検出された指標が３つ以上である場合に、前記複数のパラメータのうち６成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする請求項１１乃至１４の何れか１項に記載の位置姿勢補正装置。
16. 前記決定手段が、前記検出手段で検出された指標が３つ以上である場合に、検出された指標の画像座標から得られる領域の面積に基づいて、前記複数のパラメータのうち６成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする請求項１５に記載の位置姿勢補正装置。
17. 前記決定手段が、前記検出手段で検出された指標が３つ以上である場合に、検出された指標の画像座標から得られる領域の面積が、予め設定された面積の閾値以上であれば前記複数のパラメータのうち６成分に相当するパラメータを補正対象とし、該距離の閾値未満であれば６成分未満に相当するパラメータを補正対象のパラメータとして決定することを特徴とする請求項１６に記載の位置姿勢補正装置。
18. 前記面積は、前記検出された指標の画像座標から得られる凸包の面積であることを特徴とする請求項１６又は１７に記載の位置姿勢補正装置。
19. 撮像装置の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置であって、
    前記複数のパラメータを入力する位置姿勢入力手段と、
    現実空間に複数配置され、位置が既知である指標を、前記撮像装置で撮像した画像を入力する画像入力手段と、
    前記画像から前記指標の画像座標の実測値を検出する検出手段と、
    前記検出手段による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定手段と、
    前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち前記決定手段によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正手段と、
    を有し、
    前記補正手段が、
    前記指標の位置と前記複数のパラメータとに基づいて、前記指標の画像座標の理論値を算出する理論値算出手段と、
    前記実測値と前記理論値との誤差を小さくするように、前記補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出手段と、
    前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、
    を有し、
    前記複数のパラメータは、位置を示す３つの位置成分および姿勢を示す３つの姿勢成分の６成分に相当するパラメータであって、
    前記決定手段が、
    前記検出手段で検出された指標が１つである場合に、前記複数のパラメータのうち２成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出手段で検出された指標が２つである場合に、該検出された指標の画像座標間の距離に基づいて、前記複数のパラメータのうち３成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出手段で検出された指標が３つ以上である場合に、該検出された指標の画像座標から得られる領域の面積に基づいて、前記複数のパラメータのうち６成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする位置姿勢補正装置。
20. 計測対象物体の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置であって、
    前記複数のパラメータを入力する位置姿勢入力手段と、
    前記計測対象物体に複数配置された指標を撮像装置で撮像した画像を入力する画像入力手段と、
    前記撮像画像から前記指標の画像座標の実測値を検出する検出手段と、
    前記検出手段による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定手段と、
    前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち前記決定手段によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正手段と、
    を有し、
    前記補正手段が、
    前記複数のパラメータに基づいて、前記指標の画像座標の理論値を算出する理論値算出手段と、
    前記実測値と前記理論値との誤差を小さくするように、前記補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出手段と、
    前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正手段と、
    を有し、
    前記複数のパラメータは、位置を示す３つの位置成分および姿勢を示す３つの姿勢成分の６成分に相当するパラメータであって、
    前記決定手段が、
    前記検出手段で検出された指標が１つである場合に、前記複数のパラメータのうち２成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出手段で検出された指標が２つである場合に、該検出された指標の画像座標間の距離に基づいて、前記複数のパラメータのうち３成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出手段で検出された指標が３つ以上である場合に、該検出された指標の画像座標から得られる領域の面積に基づいて、前記複数のパラメータのうち６成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする位置姿勢補正装置。
21. 撮像装置の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置が行う位置姿勢補正方法であって、
    前記位置姿勢補正装置の位置姿勢入力手段が、前記複数のパラメータを入力する位置姿勢入力工程と、
    前記位置姿勢補正装置の画像入力手段が、現実空間に複数配置され、位置が既知である指標を、前記撮像装置で撮像した画像を入力する画像入力工程と、
    前記位置姿勢補正装置の検出手段が、前記画像から前記指標の画像座標の実測値を検出する検出工程と、
    前記位置姿勢補正装置の決定手段が、前記検出工程による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定工程と、
    前記位置姿勢補正装置の補正手段が、前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち前記決定工程によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正工程とを有し、
    前記補正工程が、
    前記補正手段の理論値算出手段が、前記指標の位置と前記複数のパラメータとに基づいて、前記指標の画像座標の理論値を算出する理論値算出工程と、
    前記補正手段の補正値算出手段が、前記実測値と前記理論値との誤差を小さくするように、前記補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出工程と、
    前記補正手段のパラメータ補正手段が、前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正工程と、
    を有することを特徴とする位置姿勢補正方法。
22. 計測対象物体の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置が行う位置姿勢補正方法であって、
    前記位置姿勢補正装置の位置姿勢入力手段が、前記複数のパラメータを入力する位置姿勢入力工程と、
    前記位置姿勢補正装置の画像入力手段が、前記計測対象物体に複数配置された指標を撮像装置で撮像した画像を入力する画像入力工程と、
    前記位置姿勢補正装置の検出手段が、前記撮像画像から前記指標の画像座標の実測値を検出する検出工程と、
    前記位置姿勢補正装置の決定手段が、前記検出工程による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定工程と、
    前記位置姿勢補正装置の補正手段が、前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち前記決定工程によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正工程とを有し、
    前記補正工程が、
    前記補正手段の理論値算出手段が、前記複数のパラメータに基づいて、前記指標の画像座標の理論値を算出する理論値算出工程と、
    前記補正手段の補正値算出手段が、前記実測値と前記理論値との誤差を小さくするように、前記補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出工程と、
    前記補正手段のパラメータ補正手段が、前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正工程と、
    を有することを特徴とする位置姿勢補正方法。
23. 撮像装置の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置が行う位置姿勢補正方法であって、
    前記位置姿勢補正装置の位置姿勢入力手段が、前記複数のパラメータを入力する位置姿勢入力工程と、
    前記位置姿勢補正装置の画像入力手段が、現実空間に複数配置され、位置が既知である指標を、前記撮像装置で撮像した画像を入力する画像入力工程と、
    前記位置姿勢補正装置の検出手段が、前記画像から前記指標の画像座標の実測値を検出する検出工程と、
    前記位置姿勢補正装置の決定手段が、前記検出工程による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定工程と、
    前記位置姿勢補正装置の補正手段が、前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち前記決定工程によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正工程と
    を有し、
    前記補正工程が、
    前記補正手段の理論値算出手段が、前記指標の位置と前記複数のパラメータとに基づいて、前記指標の画像座標の理論値を算出する理論値算出工程と、
    前記補正手段の補正値算出手段が、前記実測値と前記理論値との誤差を小さくするように、前記補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出工程と、
    前記補正手段のパラメータ補正手段が、前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正工程と、
    を有し、
    前記複数のパラメータは、位置を示す３つの位置成分および姿勢を示す３つの姿勢成分の６成分に相当するパラメータであって、
    前記決定工程では、
    前記検出工程で検出された指標が１つである場合に、前記複数のパラメータのうち２成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出工程で検出された指標が２つである場合に、該検出された指標の画像座標間の距離に基づいて、前記複数のパラメータのうち３成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出工程で検出された指標が３つ以上である場合に、該検出された指標の画像座標から得られる領域の面積に基づいて、前記複数のパラメータのうち６成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする位置姿勢補正方法。
24. 計測対象物体の位置姿勢を構成する複数のパラメータを補正する位置姿勢補正装置が行う位置姿勢補正方法であって、
    前記位置姿勢補正装置の位置姿勢入力手段が、前記複数のパラメータを入力する位置姿勢入力工程と、
    前記位置姿勢補正装置の画像入力手段が、前記計測対象物体に複数配置された指標を撮像した画像を入力する画像入力工程と、
    前記位置姿勢補正装置の検出手段が、前記画像から前記指標の画像座標の実測値を検出する検出工程と、
    前記位置姿勢補正装置の決定手段が、前記検出工程による検出結果に基づいて、前記複数のパラメータのうち何れを補正対象とするか決定する決定工程と、
    前記位置姿勢補正装置の補正手段が、前記複数のパラメータおよび前記実測値に基づいて、前記複数のパラメータのうち前記決定工程によって前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正する補正工程と
    を有し、
    前記補正工程が、
    前記補正手段の理論値算出手段が、前記複数のパラメータに基づいて、前記指標の画像座標の理論値を算出する理論値算出工程と、
    前記補正手段の補正値算出手段が、前記実測値と前記理論値との誤差を小さくするように、前記補正対象のパラメータと決定されたパラメータの補正値を算出する補正値算出工程と、
    前記補正手段のパラメータ補正手段が、前記補正値に基づいて、前記複数のパラメータのうち前記補正対象のパラメータと決定されなかったパラメータを補正せず、前記補正対象のパラメータと決定されたパラメータを補正するパラメータ補正工程と、
    を有し、
    前記複数のパラメータは、位置を示す３つの位置成分および姿勢を示す３つの姿勢成分の６成分に相当するパラメータであって、
    前記決定工程では、
    前記検出工程で検出された指標が１つである場合に、前記複数のパラメータのうち２成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出工程で検出された指標が２つである場合に、該検出された指標の画像座標間の距離に基づいて、前記複数のパラメータのうち３成分以下に相当するパラメータを前記補正対象のパラメータとして決定し、
    前記検出工程で検出された指標が３つ以上である場合に、該検出された指標の画像座標から得られる領域の面積に基づいて、前記複数のパラメータのうち６成分以下に相当するパラメータを前記補正対象のパラメータとして決定することを特徴とする位置姿勢補正方法。
25. 請求項２１乃至２４のいずれか１項に記載の位置姿勢補正方法の各工程をコンピュータに実行させるためのプログラム。
26. 請求項２５に記載のプログラムを格納したコンピュータ読み取り可能な記録媒体。

Images