JP4785416B2 - 位置姿勢計測方法及び装置 - Google Patents

Description

本発明は、物体の位置及び姿勢を計測するものに関する。

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

このような画像表示装置を実現するには、現実空間中に定義した基準座標系（重畳しようとする仮想物体の位置及び姿勢を決定する基準となる現実空間中の座標系）と、カメラの座標系（カメラ座標系）との間の、相対的な位置及び姿勢を計測することが不可欠である。これは、仮想物体（仮想空間画像）を現実空間上の位置に合わせて描画するには、基準座標系に対するカメラの現実のカメラパラメータと同一のカメラパラメータを用いて仮想物体の画像を生成しなければならないからである。例えば、室内やテーブル上といった現実環境の所定位置に仮想物体を重畳表示する場合には、その環境の適切な場所（例えば部屋の床面やテーブル面）に基準座標系を定義し、基準座標系におけるカメラの位置及び姿勢を求めればよい。

一方、移動可能な現実物体上に何らかの情報を重畳表示する（例えば、観察者が手に把持する現実の箱に仮想の模様やラベルを重畳表示する）アプリケーションを実現するためには、被重畳対象物体である箱自身の物体座標系を基準座標系と考え、箱（基準座標系）に対するカメラの位置及び姿勢を求めればよい。

このような計測を実現するための方法として、被重畳対象物体である箱の表面に複数の指標（人為的なマーカや自然特徴など）を配置あるいは設定し、計測対象であるカメラによる撮像画像内における当該指標の投影像の座標を検出し、それらの物体座標系における位置との対応関係に基づいて、カメラに対する被重畳対象物体の位置及び姿勢を求めることが一般的に行われている（第一のアプローチ）。

あるいは、環境側に基準座標系を定義して、カメラと、被重畳対象物体である箱のそれぞれの、基準座標系における位置及び姿勢を求めてもよい。共通の座標系における位置及び姿勢が求まれば、それらの間の相対的な位置及び姿勢の関係は容易に得ることができる（第二のアプローチ）。例えば、計測対象であるカメラと被重畳対象物体である箱のそれぞれを撮影する客観視点カメラを環境側に別途設置し、客観視点カメラによる撮像画像から、カメラ及び箱のぞれぞれの上に配置あるいは設定した複数の指標の投影像座標を検出する。そして、カメラ上の指標の投影像とそれらのカメラ座標系における位置との対応関係に基づいて、環境に設定した基準座標系におけるカメラの位置及び姿勢を算出し、さらに、物体上の指標の投影像とそれらの物体座標系における位置との対応関係に基づいて、基準座標系における箱の位置及び姿勢を算出することで、カメラに対する被重畳対象物体の位置及び姿勢を計測することがなされている。

また、第二のアプローチは、基準座標系における計測対象物体の位置及び姿勢を計測するような従来手法を夫々の計測対象物体の位置姿勢計測に適用することによっても実現できる。例えば、環境中の既知の位置に複数の指標を配置し、計測対象カメラによる撮像画像から検出した当該指標の画像座標を用いることで、基準座標系における計測対象カメラの位置及び姿勢を求めることができるし、同様に、計測対象物体である被重畳物体に装着した主観視点カメラによって同指標を撮像／検出することで、基準座標系における物体の位置及び姿勢を得ることができる。また、計測対象物体であるカメラ（あるいは計測対象物体に装着した主観視点カメラ）によって観測した環境中の指標の情報と、環境に設置した客観視点カメラによって観測した計測対象物体上の指標の情報を併用して、基準座標系における計測対象物体の位置及び姿勢を求める方法も利用可能である（非特許文献３，非特許文献４参照）。
Ｊ．Ｐａｒｋ，Ｂ．Ｊｉａｎｇ，ａｎｄ Ｕ．Ｎｅｕｍａｎｎ："Ｖｉｓｉｏｎ−ｂａｓｅｄ ｐｏｓｅ ｃｏｍｐｕｔａｔｉｏｎ：ｒｏｂｕｓｔ ａｎｄ ａｃｃｕｒａｔｅ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ ｔｒａｃｋｉｎｇ，"Ｐｒｏｃ．２ｎｄ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｗｏｒｋｓｈｏｐ ｏｎ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＷＡＲ’９９），ｐｐ．３−１２，１９９９． Ｄ．Ｇ．Ｌｏｗｅ："Ｆｉｔｔｉｎｇ ｐａｒａｍｅｔｅｒｉｚｅｄ ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ ｍｏｄｅｌｓ ｔｏ ｉｍａｇｅｓ，"ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１． Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，Ｈ．Ｙａｍａｍｏｔｏ，ａｎｄ Ｈ．Ｔａｍｕｒａ："Ｒｏｂｕｓｔ ｖｉｓｉｏｎ−ｂａｓｅｄ ｒｅｇｉｓｔｒａｔｉｏｎ ｕｔｉｌｉｚｉｎｇ ｂｉｒｄ’ｓ−ｅｙｅ ｖｉｅｗ ｗｉｔｈ ｕｓｅｒ’ｓ ｖｉｅｗ，"Ｐｒｏｃ．２ｎｄ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ’０３），ｐｐ．４６−５５，２００３． Ｅ．Ｆｏｘｌｉｎ，Ｙ．Ａｌｔｓｈｕｌｅｒ，Ｌ．Ｎａｉｍａｒｋ ａｎｄ Ｍ．Ｈａｒｒｉｎｇｔｏｎ："ＦｌｉｇｈｔＴｒａｃｋｅｒ：Ａ ｎｏｖｅｌ ｏｐｔｉｃａｌ／ｉｎｅｒｔｉａｌ ｔｒａｃｋｅｒ ｆｏｒ ｃｏｃｋｐｉｔ ｅｎｈａｎｃｅｄ ｖｉｓｉｏｎ，"Ｐｒｏｃ．３ｒｄ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ’０４），ｐｐ．２１２−２２１，２００４．

しかし、第一のアプローチを用いる場合、被重畳対象物体である箱上の指標が、計測対象であるカメラから常に十分に観測されていなくてはならないという制約がある。したがって、箱の表面の大部分が観察者の手で隠蔽されているような状況において、情報の重畳を行うことができない。また、第一のアプローチでは、基準座標系における夫々の計測対象物体の位置及び姿勢を得ることができないので、基準座標系に定位した仮想情報を同時に表示するような用途には用いることができない。

また、第二のアプローチを用いる場合、計測対象であるカメラが撮像した被重畳対象物体の情報が計測に利用されないので、カメラと被重畳対象物体である箱のそれぞれの位置姿勢計測誤差の影響が、重畳される仮想物体の位置のずれとして画像上に直接的に現れてしまう。

本発明はこのような従来技術の課題を鑑みてなされたものであり、位置姿勢計測を安定性と精度とを両立しながら行うことが可能にすることを目的とする。

上記目的を達成するために本発明は以下の構成を有することを特徴とする。

本願請求項１記載の発明は、第一の撮像装置の位置及び姿勢と、該第一の撮像装置による被観測物体の位置及び姿勢とを計測する位置姿勢計測方法であって、前記第一の撮像装置が撮像した第一の撮像画像から、前記被観測物体上の指標を検出する第一の指標検出工程と、環境中に設置された第二の撮像装置が撮像した第二の撮像画像から、前記被観測物体上の指標を検出する第二の指標検出工程と、前記第二の撮像画像から前記第一の撮像装置上の指標を検出する第三の指標検出工程と、前記第一、第二、及び第三の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記第一の撮像装置の位置及び姿勢と前記被観測物体の位置及び姿勢とを算出する位置姿勢算出工程とを有することを特徴とする。

本願請求項２記載の発明は、第一の撮像装置の位置及び姿勢と、該第一の撮像装置による被観測物体の位置及び姿勢とを計測する位置姿勢計測方法であって、前記第一の撮像装置が撮像した第一の撮像画像から、前記被観測物体上の指標を検出する第一の指標検出工程と、前記被観測物体に装着された第二の撮像装置が撮像した第二の撮像画像から、環境中の指標を検出する第二の指標検出工程と、前記第一の撮像画像または前記第一の撮像装置に装着された第三の撮像装置が撮像した第三の撮像画像から、環境中の指標を検出する第三の指標検出工程と、前記第一、第二、及び第三の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記第一の撮像装置の位置及び姿勢と前記被観測物体の位置及び姿勢とを算出する位置姿勢算出工程とを有することを特徴とする。

本願請求項３記載の発明は、複数の計測対象物体の夫々の位置及び姿勢あるいは当該計測対象物体間の相対的な位置及び姿勢の関係を計測する位置姿勢計測方法であって、前記計測対象物体のいずれか一つ以上に装着された第一の撮像装置によって、他の計測対象物体上の指標を観測し検出する第一の指標検出工程と、環境中に設置された第二の撮像装置によって、前記夫々の計測対象物体上の指標を観測し検出する第二の指標検出工程と、前記第一及び第二の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記計測対象物体の夫々の位置及び姿勢あるいは前記計測対象物体間の相対的な位置及び姿勢の関係を算出する位置姿勢算出工程とを有することを特徴とする。

本願請求項４記載の発明は、複数の計測対象物体の夫々の位置及び姿勢あるいは当該計測対象物体間の相対的な位置及び姿勢の関係を計測する位置姿勢計測方法であって、前記計測対象物体のいずれか一つ以上に装着された第一の撮像装置によって、他の計測対象物体上の指標を観測し検出する第一の指標検出工程と、前記計測対象物体の夫々に装着した第二の撮像装置によって、環境中の指標を観測し検出する第二の指標検出工程と、前記第一及び第二の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記計測対象物体の夫々の位置及び姿勢あるいは前記計測対象物体間の相対的な位置及び姿勢の関係を算出する位置姿勢算出工程とを有することを特徴とする。

本発明によれば、カメラと被重畳対象物体との配置の関係を直接計測する画像情報と、環境に対するカメラ及び被重畳対象物体との配置の関係を計測する画像情報の双方に矛盾の無い位置姿勢計測を行うことができ、従来の方法と比べて安定性と精度に優れた計測を実現することができる。

以下添付図面を参照して、好適な実施形態の１例を説明する。

［第１の実施形態］
本実施形態に係る位置姿勢計測装置は、撮像装置と計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、画像入力部１１０（主観視点画像入力部１１０ａ及び客観視点画像入力部１１０ｂ）、指標検出部１２０（主観視点指標検出部１２０ａ及び客観視点指標検出部１２０ｂ）、位置姿勢算出部１３０、及び客観視点カメラ１４０（１４０ａ，１４０ｂ，１４０ｃ）によって構成される。そして、撮像装置１５０に接続されている。撮像装置１５０と計測対象物体１６０の位置及び姿勢が、本実施形態に係る位置姿勢計測装置１００によって計測される。

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

撮像装置１５０は例えばビデオカメラである。計測対象物体１６０と同様に、撮像装置１５０上の複数の位置には、主観視点カメラ座標系（撮像装置の視点位置を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系。以下では記号Ｃによってこの座標系を表現する）における位置が既知である複数個の指標Ｑ_ＫＯ＋ｋ（ｋ＝１，・・・，Ｋ_Ｃ）が配置されている。図１の例は、Ｋ_Ｃ＝２、すなわち２個の指標Ｑ_７，Ｑ_８が配置されている状況を示している。撮像装置１５０が撮像する画像（以下、これを主観視点画像と呼ぶ）は、位置姿勢計測装置１００に入力される。

客観視点カメラ１４０（１４０ａ，１４０ｂ，１４０ｃ）は例えば１台以上のビデオカメラであり、計測対象範囲内に撮像装置１５０及び計測対象物体１６０が位置するときに、そのいずれかによって撮像装置１５０及び計測対象物体１６０が撮像されるような位置に固定して配置されている。客観視点カメラ１４０が出力する画像（以下、これを客観視点画像と呼ぶ）は、客観視点画像入力部１１０ｂに入力される。ここで、基準座標系（以下では記号Ｗによってこの座標系を表現する）における客観視点カメラ１４０の位置及び姿勢は、既知の値として予め保持されているものとする。以下では、客観視点カメラ１４０の夫々によって定義される客観視点カメラ座標系を、それぞれ記号Ｂ_１，Ｂ_２，Ｂ_３で表現する。

画像入力部１１０は、主観視点画像入力部１１０ａと客観視点画像入力部１１０ｂからなる。主観視点画像入力部１１０ａは、位置姿勢計測装置１００へ入力される主観視点画像をデジタルデータに変換し、指標検出部１２０ａへと出力する。同様に、客観視点画像入力部１１０ｂは、客観視点カメラ１４０から入力される客観視点画像をデジタルデータに変換し、指標検出部１２０ｂへと出力する。

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

指標検出部１２０は、さらに、検出された各々の指標の画像座標とその識別子を、位置姿勢算出部１３０へと出力する。

なお、以下では、検出された指標の夫々に通し番号ｎ（ｎ＝１，，，Ｎ）をつけ、画像上で検出された夫々の指標をＱ_ｋｎと表記する。ここで、Ｎは各画像上で検出された指標の総数であり、Ｎ＝ΣＮ_ｄ、Ｎ_ｄは夫々のカメラの撮影画像上で検出された指標の数、ｄはカメラの識別子（ｄ＝Ｂ_１，Ｂ_２，Ｂ_３，Ｃ）を表している。また、指標Ｑ_ｋｎの画像座標をｕ_ｎ、その指標を撮影したカメラの識別子をｄ_ｎと表記し、識別子ｄのカメラで検出された指標Ｑ_ｋの画像座標をｕ_ｄ ^Ｑｋと表記する（すなわち、ｕ_ｎ＝ｕ_ｄｎ ^Ｑｋｎ）。

例えば、図１の例で、客観視点カメラ１４０ａが指標Ｑ_４，Ｑ_５，Ｑ_６を、客観視点カメラ１４０ｃが指標Ｑ_７，Ｑ_８を、撮像装置１５０が指標Ｑ_２，Ｑ_３，Ｑ_５，Ｑ_６をそれぞれ観測し、それぞれの撮影画像から各指標が検出されているものとする。この場合、Ｎ＝９，Ｎ_Ｂ１＝３，Ｎ_Ｂ２＝０，Ｎ_Ｂ３＝２，Ｎ_Ｃ＝４であり、指標の識別子（ｋ_１＝４，ｋ_２＝５，ｋ_３＝６，ｋ_４＝７，ｋ_５＝８，ｋ_６＝２，ｋ_７＝３，ｋ_８＝５，ｋ_９＝６）と、これらを撮影したカメラの識別子（ｄ_１＝Ｂ_１，ｄ_２＝Ｂ_１，ｄ_３＝Ｂ_１，ｄ_４＝Ｂ_３，ｄ_５＝Ｂ_３，ｄ_６＝Ｃ，ｄ_７＝Ｃ，ｄ_８＝Ｃ，ｄ_９＝Ｃ）と、検出された画像座標（ｕ_Ｂ１ ^Ｑ４，ｕ_Ｂ１ ^Ｑ５，ｕ_Ｂ１ ^Ｑ６，ｕ_Ｂ３ ^Ｑ７，ｕ_Ｂ３ ^Ｑ８，ｕ_Ｃ ^Ｑ２，ｕ_Ｃ ^Ｑ３，ｕ_Ｃ ^Ｑ５，ｕ_Ｃ ^Ｑ６）が出力される。

位置姿勢算出部１３０は、指標検出部１２０から各々の指標の画像座標を入力し、撮像装置１５０及び計測対象物体１６０の位置及び姿勢を算出して出力する。

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

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

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

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

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

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

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

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

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

上述した各構成要素は、バス１０１０によって相互に接続される。

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

以下では、基準座標系における計測対象物体１６０の位置を３値のベクトルｔ_ＷＯ＝［ｘ_ＷＯ ｙ_ＷＯ ｚ_ＷＯ］^Ｔ、姿勢を３値のベクトルω_ＷＯ＝［ξ_ＷＯ ψ_ＷＯ ζ_ＷＯ］^Ｔ、位置と姿勢を６値のベクトルｓ_ＷＯ＝［ｔ_ＷＯ ^Ｔ ω_ＷＯ ^Ｔ］^Ｔ、撮像装置１５０の位置を３値のベクトルｔ_ＷＣ＝［ｘ_ＷＣ ｙ_ＷＣ ｚ_ＷＣ］^Ｔ、姿勢を３値のベクトルω_ＷＣ＝［ξ_ＷＣ ψ_ＷＣ ζ_ＷＣ］^Ｔ、位置と姿勢を６値のベクトルｓ_ＷＣ＝［ｔ_ＷＣ ^Ｔ ω_ＷＣ ^Ｔ］^Ｔと表すことにする。以下の処理では、これらのパラメータを１次元に並べた１２値の状態ベクトルｓ＝［ｓ_ＷＯ ^Ｔ ｓ_ＷＣ ^Ｔ］^Ｔ＝［ｘ_ＷＯ ｙ_ＷＯ ｚ_ＷＯ ξ_ＷＯ ψ_ＷＯ ζ_ＷＯ ｘ_ＷＣ ｙ_ＷＣ ｚ_ＷＣ ξ_ＷＣ ψ_ＷＣ ζ_ＷＣ］^Ｔを、導出すべき未知パラメータとする。

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

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

ステップＳ３０００において、位置姿勢算出部１３０は、検出された各々の指標Ｑ_ｋｎ（ｎ＝１，，，Ｎ）について、画像座標ｕ_ｎ（＝ｕ_ｄｎ ^Ｑｋｎ）と、指標識別子ｋ_ｎ及びカメラ識別子ｄ_ｎを指標抽出部１２０から入力する。なお、各指標の物体座標系における３次元座標ｘ_Ｏ ^Ｑｋｎあるいは主観視点カメラ座標系における３次元座標ｘ_Ｃ ^Ｑｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。

ステップＳ３０１０において、位置姿勢算出部１３０は、状態ベクトルｓに適当な初期値（位置及び姿勢の推定値）を設定する。例えば、前フレーム（時刻ｔ_ｋ−１）の処理において導出されたｓを初期値としてもよいし、過去からのｓの導出値の変化に基づいた予測によって得た値を設定してもよい。

ステップＳ３０２０において、位置姿勢算出部１３０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値ｕ_ｎ ^＊を算出する。ｕ_ｎ ^＊の算出は、ｓによって定義される指標の観測方程式に基づいて行われる。指標の観測方程式は、観測するカメラの種類（撮像装置１５０または客観視点カメラ１４０）と、観測される指標が属する物体（計測対象物体１６０または撮像装置１５０）の組み合わせ応じて、以下のように定義される。

（１）計測対象物体１６０上の指標を、撮像装置１５０で観測している場合：
観測方程式を、

とおくと、この式は、指標の物体座標ｘ_Ｏ ^Ｑｋｎとｓから、当該指標の主観視点カメラ座標ｘ_Ｃ ^Ｑｋｎを求める次式、

及び、主観視点カメラ座標ｘ_Ｃ ^Ｑｋｎから主観視点画像座標ｕ_Ｃ ^Ｑｋｎ＊を求める透視投影変換を表す次式、

によって構成される。ここでｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１５０の焦点距離であり、既知の値としてＲＡＭ１００２に予めロードされているものとする。

（２）計測対象物体１６０上の指標を、客観視点カメラ１４０で観測している場合：
観測方程式を、

とおくと、この式は、指標の物体座標ｘ_Ｏ ^Ｑｋｎとｓから、当該指標の客観視点カメラ座標ｘ_Ｂ ^Ｑｋｎを求める次式、

及び、客観視点カメラ座標ｘ_Ｂ ^Ｑｋｎから客観視点画像座標ｕ_Ｂ ^Ｑｋｎ＊を求める透視投影変換を表す次式、

によって構成される。ここでＲ_ＷＢは基準座標系における客観視点カメラ１４０の姿勢を表す３×３の回転行列、ｔ_ＷＢは基準座標系における客観視点カメラ１４０の位置を表す３値のベクトル、ｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ１４０の焦点距離であり、既知の値としてＲＡＭ１００２に予めロードされているものとする。なお、ここでは夫々の客観視点カメラ１４０を代表して記号Ｂを用いているが、実際には、何れの客観視点カメラによる観測かに応じて、焦点距離や位置や姿勢には、異なったパラメータが使用される。

（３）撮像装置１５０上の指標を、客観視点カメラ１４０で観測している場合：
観測方程式を

とおくと、この式は、指標の主観視点カメラ座標ｘ_Ｃ ^Ｑｋｎとｓから、当該指標の客観視点カメラ座標ｘ_Ｂ ^Ｑｋｎを求める次式、

及び、透視投影変換を行う式７によって構成される。

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

ステップＳ３０４０において、位置姿勢算出部１３０は、各々の指標Ｑ_ｋｎに対して、ｓに関する画像ヤコビアン（すなわち、式２，式５，または式８の観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×１２列のヤコビ行列）Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。

具体的には、撮像装置１５０による計測対象物体１６０上の指標の観測（ステップＳ３０３０の（１））に対しては、式４の右辺を主観視点カメラ座標の各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｑｋｎ（＝∂ｕ／∂ｘ）と、式３の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×１２列のヤコビ行列Ｊ_ｘｓ ^Ｑｋｎ（＝∂ｘ／∂ｓ）を算出し、次式によってＪ_ｕｓ ^Ｑｋｎを算出する。

同様に、客観視点カメラ１４０による計測対象物体１６０上の指標の観測（ステップＳ３０３０の（２））に対しては、式７の右辺を客観視点カメラ座標の各要素で偏微分したヤコビ行列と、式６の右辺をベクトルｓの各要素で偏微分したヤコビ行列を算出し、式１１と同様にこれらの積によってＪ_ｕｓ ^Ｑｋｎを算出する。なお、式６には撮像装置１５０の位置と姿勢に関するパラメータが含まれないことから、この観測で得られる画像ヤコビアンＪ_ｕｓ ^Ｑｋｎのうちのｓ_ＷＣに関する要素は、全て０となる。

同様に、客観視点カメラ１４０による撮像装置１５０上の指標の観測（ステップＳ３０３０の（３））に対しては、式７の右辺を客観視点カメラ座標の各要素で偏微分したヤコビ行列と、式９の右辺をベクトルｓの各要素で偏微分したヤコビ行列を算出し、式１１と同様にこれらの積によってＪ_ｕｓ ^Ｑｋｎを算出する。なお、式９には計測対象物体１６０の位置と姿勢に関するパラメータが含まれないことから、この観測で得られる画像ヤコビアンＪ_ｕｓ ^Ｑｋｎのうちのｓ_ＷＯに関する要素は、全て０となる。

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

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

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

として算出する。

ステップＳ３０６０において、位置姿勢算出部１３０は、ステップＳ３０５０において算出した補正値△ｓを用いて式１５に従ってｓを補正し、得られた値をｓの新たな推定値とする。
ｓ＋△ｓ→ｓ ・・・式（１５）

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

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

なお、位置及び姿勢を出力する際のデータの表現方法は、各物体（撮像装置１５０と計測対象物体１６０）の位置及び姿勢をそれぞれ６値のパラメータで表したものであってもよいし、他の表現形態をとってもよい。例えば、各物体の姿勢に関して式１によって姿勢の３値ベクトルから３×３の回転行列を求め、位置の３値ベクトルと姿勢の３×３行列を出力してもよいし、オイラー角や４元数による表現に姿勢を変換して出力してもよいし、同時座標表現による４×４の位置姿勢変換行列やその逆行列を出力してもよい。また、撮像装置１５０に対する計測対象物体１６０の位置及び姿勢を求めてこれを出力してもよいし、反対に、計測対象物体１６０に対する撮像装置１５０の位置及び姿勢を求めてこれを出力してもよい。もちろん、これらのいずれかの組み合わせを同時に出力してもよい。

ステップＳ３０９０において、位置姿勢算出部１３０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ３０００へと進み、次フレーム以降の入力データに対して同様の処理を実行する。

以上のようにして、撮像装置１５０と客観視点カメラ１４０から得られる情報を同時に利用して、計測対象物体１６０と撮像装置１５０の位置及び姿勢を同時に推定することができる。

本実施形態に係る位置姿勢計測装置によれば、撮像装置１５０の撮影画像から得られる撮像装置１５０と計測対象物体１６０との間の相対的位置姿勢に関する情報と、客観視点カメラ１４０の撮影画像から得られる夫々の計測対象物体の絶対的位置姿勢に関する情報の、双方に矛盾の無い解を得ることができる。したがって、絶対精度の信頼性を保ちつつ重畳画像の位置ずれを最小限に抑えるような、複合現実感の提示に好適な位置姿勢計測が実現できる。

［第２の実施形態］
本実施形態に係る位置姿勢計測装置は、二つの計測対象物体の位置及び姿勢の計測を行う。例えば、現実空間の像を透過させつつ情報を重畳表示することが可能な光学シースルーＨＭＤを用いて、手持ちの物体の上に何らかの情報を重畳表示するアプリケーションを実現する場合に、当該ＨＭＤと手持ち物体の双方の位置及び姿勢を計測する目的に、本実施形態に係る位置姿勢計測装置を用いることができる。本実施形態に係る位置姿勢計測装置は、主観視点画像を撮影する複数の主観視点カメラと、物体の姿勢を計測する複数の姿勢センサを有している点が、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点に焦点を絞って、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図４は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置４００は、画像入力部１１０、指標検出部１２０、位置姿勢算出部４３０、客観視点カメラ１４０、主観視点カメラ４５０（４５０ａ，４５０ｂ）、姿勢センサ４７０（４７０ａ，４７０ｂ）、及びセンサ計測値入力部４８０によって構成されている。二つの計測対象物体１６０（１６０ａ，１６０ｂ）の位置及び姿勢が、本実施形態に係る位置姿勢計測装置４００によって計測される。例えば、計測対象物体１６０ｂとして光学シースルーＨＭＤを、計測対象物体１６０ａとして情報を重畳表示する対象である被観測物体を設定することができる。

第１の実施形態と同じ機能を有する部分（１１０，１２０，１４０）については図１と同じ番号を付けており、その説明を省略する。ただし、２台の主観視点カメラ４５０（４５０ａ，４５０ｂ）によって取得された画像が主観視点画像として主観視点画像入力部１１０ａに入力されるという点と、これらの画像に対して主観視点指標検出部１２０ａにおいて指標の検出処理が実行されるという点は、第１の実施形態とは異なっている。

夫々の計測対象物体１６０上の複数の位置には、夫々の物体座標系（以下では、記号Ｏ_１，Ｏ_２によって表現する）における位置が既知である複数個の指標Ｑ_ｋがそれぞれ配置されている。第一の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、指標は何れの形態であってもよい。

第１の実施形態では、計測対象物体１６０上の指標は全て同種のものであり、客観視点画像と主観視点画像から共通の指標を検出していたが、本実施形態では、客観視点カメラ１４０によって観測／検出するための指標と、主観視点カメラ４５０によって観測／検出するための指標として、異なる種類の指標を用いている。図４の例では、客観視点カメラによる観測を意図した球形の指標Ｑ_１〜Ｑ_８と、主観視点カメラ４５０による観測を意図した四角形指標Ｑ_Ｓ１及びＱ_Ｓ２（それぞれの頂点をＱ_９〜Ｑ_１２及びＱ_１３〜Ｑ_１６とする）が配置されている。このように複数種類の指標を使い分けることは、カメラから指標までの距離等が異なる際に有効な場合がある。もちろん、第一の実施形態と同様に共通の指標を用いることも可能である。なお、図４では、指標Ｑ_１〜Ｑ_８が計測対象物体１６０の上ではなく主観視点カメラ４５０の上に配置されているが、物体座標系における位置が既知であれば、指標の配置場所に限定は無い。

主観視点カメラ４５０（４５０ａ，４５０ｂ）は、例えば１台以上のビデオカメラであり、計測対象物体１６０の夫々に設置されている。夫々の物体座標系上における主観視点カメラ４５０の位置及び姿勢は既知であり、主観視点カメラ４５０は、夫々が装着されている計測対象物体１６０の主観視点位置からもう一方の計測対象物体を観測した画像を得て、主観視点画像入力部１１０ａへと出力する。以下では、主観視点カメラ４５０の夫々によって定義される主観視点カメラ座標系を、それぞれ記号Ｃ_１，Ｃ_２で表現する。

姿勢センサ４７０（４７０ａ，４７０ｂ）は、計測対象物体１６０の夫々に設置されている。姿勢センサ４７０は、例えばジャイロセンサをベースとしたセンサユニットであり、具体的には、株式会社トキメックのＴＩＳＳ−５−４０や、米国ＩｎｔｅｒＳｅｎｓｅ社のＩｎｅｒｔｉａＣｕｂｅ２等によって構成される。夫々の物体座標系上における姿勢センサ４７０の姿勢は既知であり、姿勢センサ４７０は、夫々が装着されている計測対象物体１６０の基準座標系における姿勢を計測して、センサ計測値入力部４８０へ出力する。

センサ計測値入力部４８０は、夫々の姿勢センサ４７０から姿勢計測値を入力し、位置姿勢算出部４３０へ出力する。

位置姿勢算出部４３０は、各々の指標の画像座標を指標検出部１２０から入力し、姿勢計測値をセンサ計測値入力部４８０から入力し、これらの情報に基づいて、夫々の計測対象物体１６０の位置及び姿勢を算出して出力する。

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

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

ステップＳ５０００において、位置姿勢算出部４３０は、夫々の計測対象物体１６０の姿勢計測値（以下ではそれぞれを、３×３の回転行列Ｒ_ＷＯ１，Ｒ_ＷＯ２と記す）を、センサ計測値入力部４８０から入力する。

本実施形態では、姿勢センサ４７０によって計測された姿勢は十分信頼できるものであると考え、ステップＳ５０００において入力した物体の姿勢Ｒ_ＷＯ１，Ｒ_ＷＯ２を正しいものとして取り扱う。第一の実施形態では二つの物体の位置と姿勢を表す１２のパラメータを未知パラメータとして扱っていたが、本実施形態では、夫々の計測対象物体１６０の姿勢は既知であり、位置のみを画像情報から求めればよい。以下では、これらのパラメータ（計測対象物体１６０ａと１６０ｂの位置）を１次元に並べた６値の状態ベクトルｓ＝［ｔ_ＷＯ１ ^Ｔ ｔ_ＷＯ２ ^Ｔ］^Ｔ＝［ｘ_ＷＯ１ ｙ_ＷＯ１ ｚ_ＷＯ１ ｘ_ＷＯ２ ｙ_ＷＯ２ ｚ_ＷＯ２］^Ｔを、導出すべき未知パラメータとする。

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

ステップＳ５０２０において、位置姿勢算出部４３０は、状態ベクトルｓに適当な初期値（位置の推定値）を設定する。例えば、前フレーム（時刻ｔ_ｋ−１）の処理において導出されたｓを初期値としてもよいし、過去からのｓの導出値の変化に基づいた予測によって得た値を設定してもよい。

ステップＳ５０３０において、位置姿勢算出部４３０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値ｕ_ｎ ^＊を算出する。ｕ_ｎ ^＊の算出は、ｓによって定義される指標の観測方程式に基づいて行われる。指標の観測方程式は、観測するカメラと観測される指標が属する物体の組み合わせ応じて、以下のように定義される。

計測対象物体１６０ａ上の指標を主観視点カメラ４５０ｂで観測している場合、観測方程式は、指標の物体座標ｘ_Ｏ１ ^Ｑｋｎとｓから、当該指標の主観視点カメラ座標ｘ_Ｃ２ ^Ｑｋｎを求める次式、

及び、透視投影変換を表す式４によって構成される。計測対象物体１６０ｂ上の指標を主観視点カメラ４５０ａで観測している場合も同様であり、式１６の代わりに次式を使用する。

ここで、Ｒ_Ｏ２Ｃ２及びｔ_Ｏ２Ｃ２は物体座標系Ｏ_２における主観視点カメラ４５０ｂの姿勢及び位置、Ｒ_Ｏ１Ｃ１及びｔ_Ｏ１Ｃ１は物体座標系Ｏ_１における主観視点カメラ４５０ａの姿勢及び位置、式４におけるｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは夫々のカメラの焦点距離であり、既知の値としてＲＡＭ１００２に予めロードされているものとする。なお、一つの計測対象物体１６０に複数の主観視点カメラが設置されている場合には、夫々の主観視点カメラに固有のパラメータが使用される。

一方、計測対象物体１６０上の指標を客観視点カメラ１４０で観測している場合については、第一の実施形態（式５〜式７）と同様であるので説明を省略する。ただし、姿勢センサにより計測対象物体１６０の姿勢としてＲ_ＷＯが得られているので、式６におけるＲ（ω_ＷＯ）の項を、計測値Ｒ_ＷＯに置き換える必要がある。

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

ステップＳ５０５０において、位置姿勢算出部４３０は、各々の指標Ｑ_ｋｎに対して、ｓに関する画像ヤコビアン（すなわち、観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。なお、画像ヤコビアンの算出方法は、式１６や式１７を用いて観測方程式を構成する点以外は第一の実施形態と同様であるので、詳細な説明は省略する。

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

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

ステップＳ５０８０において、位置姿勢算出部４３０は、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ５０３０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ５０９０へと処理を進める。

ステップＳ５０９０において、位置姿勢算出部４３０は、ステップ５０８０までの処理によって得られたｓを、計測対象物体１６０ａ及び１６０ｂの位置として、ステップＳ５０００で入力した姿勢計測値と合わせて、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

ステップＳ５１００において、位置姿勢算出部４３０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ５０００へと進み、次フレーム以降の入力データに対して同様の処理を実行する。

以上によって、計測対象物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、計測対象物体１６０間の相対的位置姿勢に関する情報と計測対象物体の絶対的位置姿勢に関する情報の双方に矛盾の無い解を得ることが可能であり、精度と安定性を両立した位置及び姿勢の計測が実現できる。

なお、主観視点カメラ４５０ａ及び４５０ｂは、必ずしも双方が必要ではない。両者の相対的位置姿勢に関する情報を得るために最低限必要なのはいずれかの１台であり、本実施形態のより簡易的な構成として、４５０ａあるいは４５０ｂのいずれかを有さない構成をとることも可能である。

［第３の実施形態］
本実施形態に係る位置姿勢計測装置は、撮像装置と計測対象物体の位置及び姿勢の計測を行う。以下では、第１及び第２の実施形態との相違点に焦点を絞って、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

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

第１の実施形態と同じ機能を有する部分（１１０，１２０）については図１と同じ番号を付けており、その説明を省略する。ただし、本実施形態では、客観視点画像入力部１１０ｂ及び客観視点指標検出部１２０ｂが存在しない点が第１の実施形態とは異なっている。また、２台の主観視点カメラ４５０（４５０ａ，４５０ｂ）および撮像装置１５０の夫々によって取得された画像が主観視点画像として画像入力部１１０に入力されるという点と、これらの画像に対して指標検出部１２０において指標の検出処理が実行されるという点も、第１の実施形態とは異なっている。

同様に、第２の実施形態と同じ機能を有する部分（４５０，４７０，４８０）については図４と同じ番号を付けており、その説明を省略する。ただし、本実施形態では、主観視点カメラ４５０（４５０ａ，４５０ｂ）及び姿勢センサ４７０（４７０ａ，４７０ｂ）が、計測対象物体１６０及び撮像装置１５０の夫々に設置されているという点が、第２の実施形態とは異なっている。また、主観視点カメラ４５０の夫々が、計測対象物体１６０及び撮像装置１５０が標準的な姿勢である際に、夫々が鉛直上方向を撮影するような位置関係となるように設置されている点も、第２の実施形態とは異なっている。

計測対象物体１６０上の複数の位置には、計測対象物体１６０上の物体座標系における位置が既知である複数個の指標Ｑ_ｋが配置されている。他の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、指標は何れの形態であってもよい。

撮像装置１５０は例えばビデオカメラである。撮像装置１５０が撮像する画像は、位置姿勢計測装置６００に入力される。以下では、撮像装置１５０によって定義されるカメラ座標系を、記号Ｃで表現する。

現実空間中の複数の位置には、撮像装置１５０及び主観視点カメラ４５０によって撮影するための指標として、基準座標系における位置が既知である複数個の指標が配置されている。図６の例では、９個の四角形指標Ｑ_Ｓ２〜Ｑ_Ｓ１０が天井面に、５個の四角形指標Ｑ_Ｓ１１〜Ｑ_Ｓ１５が壁面及び床面に設定されている状況を示している。図６では、指標Ｑ_Ｓ２〜Ｑ_Ｓ１０が主観視点カメラ４５０によって撮影されることを意図して、指標Ｑ_Ｓ１１〜Ｑ_Ｓ１５が撮像装置１５０によって観測されることを意図して配置されているが、必ずしも別々の指標を用いる必要は無い。なお、指標の種類は、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。撮像装置１５０で検出するための指標と主観視点カメラ４５０で検出するための指標として別種の指標を用いてもよいし、図６ように同種の指標を用いてもよい。

位置姿勢算出部６３０は、各々の指標の画像座標を指標検出部１２０から入力し、姿勢計測値をセンサ計測値入力部４８０から入力し、これらの情報に基づいて、撮像装置１５０及び計測対象物体１６０の位置及び姿勢を算出して出力する。

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

以下、図５のフローチャートに従って、位置姿勢算出部６３０の処理手順を説明する。

ステップＳ５０００において、位置姿勢算出部６３０は、撮像装置１５０と計測対象物体１６０の姿勢計測値（以下ではそれぞれを、３×３の回転行列Ｒ_ＷＣ，Ｒ_ＷＯと記す）を、センサ計測値入力部４８０から入力する。

本実施形態では、ステップＳ５０００において入力した物体の姿勢を正しい値と考え、計測対象物体１６０と撮像装置１５０の位置を１次元に並べた６値の状態ベクトルｓ＝［ｔ_ＷＣ ^Ｔ ｔ_ＷＯ ^Ｔ］^Ｔ＝［ｘ_ＷＣ ｙ_ＷＣ ｚ_ＷＣ ｘ_ＷＯ ｙ_ＷＯ ｚ_ＷＯ］^Ｔを、導出すべき未知パラメータとする。

ステップＳ５０１０において、位置姿勢算出部６３０は、検出された各々の指標Ｑ_ｋｎ（ｎ＝１，，，Ｎ）について、画像座標ｕ_ｎ（＝ｕ_ｄｎ ^Ｑｋｎ）と、指標識別子ｋ_ｎ及びカメラ識別子ｄ_ｎを指標抽出部１２０から入力する。なお、各指標の物体座標系における３次元座標ｘ_Ｏ ^Ｑｋｎあるいは基準座標系における３次元座標ｘ_Ｗ ^Ｑｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。

ステップＳ５０２０において、位置姿勢算出部６３０は、状態ベクトルｓに適当な初期値（位置の推定値）を設定する。

ステップＳ５０３０において、位置姿勢算出部６３０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値ｕ_ｎ ^＊を算出する。ｕ_ｎ ^＊の算出は、ｓによって定義される指標の観測方程式に基づいて行われる。指標の観測方程式は、観測するカメラと観測される指標が属する物体の組み合わせ応じて、以下のように定義される。

計測対象物体１６０上の指標を撮像装置１５０で観測している場合の観測方程式は、第一の実施形態（式２〜式４）と同様であるので説明を省略する。ただし、姿勢センサにより計測対象物体１６０と撮像装置１５０の姿勢が得られているので、式３におけるＲ（ω_ＷＯ）及びＲ（ω_ＷＣ）の項を、それぞれ計測値Ｒ_ＷＯ及びＲ_ＷＣに置き換える必要がある。

一方、基準座標系の指標を撮像装置１５０、主観視点カメラ４５０ａ及び主観視点カメラ４５０ｂで観測している場合の観測方程式は、それぞれ、

及び、透視投影変換を表す式４によって構成される。ここで、Ｒ_ＯＣ１及びｔ_ＯＣ１は物体座標系Ｏにおける主観視点カメラ４５０ａの姿勢及び位置、Ｒ_ＣＣ２及びｔ_ＣＣ２はカメラ座標系Ｃにおける主観視点カメラ４５０ｂの姿勢及び位置であり、それぞれ既知の値としてＲＡＭ１００２に予めロードされているものとする。また、式４におけるｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは、夫々のカメラの焦点距離であり、既知の値としてＲＡＭ１００２に予めロードされているものとする。

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

ステップＳ５０５０において、位置姿勢算出部６３０は、各々の指標Ｑ_ｋｎに対して、ｓに関する画像ヤコビアン（すなわち、観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。なお、画像ヤコビアンの算出方法は、式１８，式１９，式２０を用いて観測方程式を構成する点以外は第一の実施形態と同様であるので、詳細な説明は省略する。

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

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

ステップＳ５０８０において、位置姿勢算出部６３０は、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ５０３０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ５０９０へと処理を進める。

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

ステップＳ５１００において、位置姿勢算出部６３０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ５０００へと進み、次フレーム以降の入力データに対して同様の処理を実行する。

以上によって、計測対象物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、撮像装置１５０と計測対象物体１６０との間の相対的位置姿勢に関する情報と、撮像装置１５０及び計測対象物体１６０の環境中における絶対的位置姿勢に関する情報の双方に矛盾の無い解を得ることが可能であり、精度と安定性を両立した位置及び姿勢の計測が実現できる。

なお、主観視点カメラ４５０ｂ及び撮像装置１５０の双方による環境側の指標の観測は必ずしも必須ではなく、両者のいずれかによって行われていればよい。すなわち、本実施形態のより簡易的な構成として、主観視点カメラ４５０ｂを有さない構成を取ることも可能であるし、あるいは、撮像装置１５０による環境側の指標の観測を行わなくてもよい。また、主観視点カメラ４５０ｂによって計測対象物体１６０上の指標を観測してもよいし、この場合は、撮像装置１５０による計測対象物体１６０上の指標の観測は必須ではない。また、撮像装置１５０上に複数の主観視点カメラを設置し、計測対象物体１６０上の指標を観測するための主観視点カメラと、環境側の指標を観測するための主観視点カメラを使い分けてもよい。

［他の実施形態］
（変形例１）
上記実施形態の各々では、誤差ベクトルＵと行列Θに基づく補正値△ｓの算出に、式１４で表現されるＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法を用いている。しかし、補正値△ｓの算出は必ずしもＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。また、上記の実施形態における未知パラメータｓを含むようなカルマンフィルタの状態ベクトルを定義し、式２，式５，及び式８などで定義される観測方程式を利用し、さらに、例えば等加速度などの予測モデルを用いることで、計測対象間の相対位置姿勢に関する情報と各計測対象の絶対位置姿勢に関する情報の双方に矛盾しないように各計測対象の位置と姿勢を同時に更新するという効果を有した、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）や、反復拡張カルマンフィルタ（ｉｔｅｒａｔｉｖｅ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を構成することができる。なお、拡張カルマンフィルタ及び反復拡張カルマンフィルタについては非特許文献１に記載されるように周知であるので、その詳細に関するこれ以上の説明は省略する。

また、上記実施形態の各々では、ステップＳ３０１０またはステップＳ５０２０において過去の算出結果から未知パラメータの初期値を設定しているが、初期値の取得方法はこれに限るものではない。例えば、第１及び第２の実施形態においては、客観視点カメラによる計測対象上の指標の検出情報に基づいて公知の技術を用いて夫々の計測対象の位置及び姿勢を得てこれを初期値としてもよいし、第３の実施形態においては、夫々の主観視点カメラによる指標の検出情報に基づいて公知の技術を用いて夫々の計測対象の位置及び姿勢を得てこれを初期値としてもよい。

また、上記第２及び第３の実施形態では、ステップＳ５０３０からステップＳ５０８０で表される反復解法によって未知パラメータを求めていたが、必ずしもこの解法を用いなくてもよい。第２及び第３の実施形態のように未知パラメータが位置情報のみの場合には、各々の観測方程式を展開することで未知パラメータに関する線型方程式を得ることが可能であるので、検出された夫々の指標の画像座標と３次元座標を入力とした線形連立方程式を作成し、これを解くことによって解を得てもよい。

（変形例２）
上記第２及び第３の実施形態では、夫々の計測対象（撮像装置１５０及び計測対象物体１６０）に姿勢センサ４７０が装着されていたが、姿勢センサは必ずしも必要ではない。姿勢センサ４７０を装着せずに、第１の実施形態と同様に夫々の計測対象の位置及び姿勢の１２パラメータを未知として、画像情報のみからこれらを算出することも可能である。この場合、夫々の観測方程式における姿勢に関するパラメータを、いずれも未知数として取り扱えばよい。例えば、式１６及び式１７ではＲ_ＷＯ２及びＲ_ＷＯ１を姿勢センサから得ていたが、これをそれぞれＲ（ω_ＷＯ２）及びＲ（ω_ＷＯ１）とおき、未知数としてω_ＷＯ２，ω_ＷＯ１を追加すればよい。同様に、式１８，式１９，式２０ではＲ_ＷＯ及びＲ_ＷＣを姿勢センサから得ていたが、これをそれぞれＲ（ω_ＷＯ）及びＲ（ω_ＷＣ）とおき、未知数としてω_ＷＯ，ω_ＷＣを追加すればよい。

反対に、上記第１の実施形態における夫々の計測対象に姿勢センサを装着することで、未知パラメータを夫々の計測対象の位置情報のみに限定することもできる。この場合は、第３の実施形態と同様に姿勢センサによって計測対象物体１６０の姿勢Ｒ_ＷＯ及び撮像装置１５０の姿勢Ｒ_ＷＣを得て、式３，式６，式９におけるＲ（ω_ＷＯ）及びＲ（ω_ＷＣ）を、計測値Ｒ_ＷＯ及びＲ_ＷＣに置き換えればよい。

また、姿勢センサを用いる場合であっても、センサによる姿勢計測値の全てを信頼するのではなく、より信頼できるパラメータのみを固定値として取り扱うこともできる。姿勢センサの多くは鉛直重力軸まわりの計測値（すなわち方位角）の精度に信頼性がないという性質を有しているので、姿勢センサから得られる傾斜角のみを固定値として用いてもよい。言い換えると、３軸の姿勢センサの替わりに傾斜センサを用いてもよい。また、姿勢センサは姿勢の初期値を得るためにのみ利用して、位置及び姿勢の全パラメータを未知として求めてもよい。また、姿勢センサ以外の他のセンサ（例えば６自由度位置姿勢センサや位置センサ）を計測対象に装着し、そのセンサによる計測値の少なくとも一部を固定パラメータとして用いることや、あるいは、そのセンサによる計測値を初期値として用いることも、同様の技術思想の範疇において実現可能である。また、複数の計測対象物体に対して一方のみにセンサを装着してその出力を利用することも、もちろん可能である。

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

（変形例４）
第１の実施形態に係る位置姿勢計測装置は、第２の実施形態における主観視点カメラ４５０に相当する構成要素を有していなかった。しかし、第２の実施形態と同様に、計測対象物体１６０に主観視点カメラ４５０をさらに装着し、撮像装置１５０上に設定した指標をこれによって観測することで、撮像装置１５０及び計測対象物体１６０の位置及び姿勢の算出にその情報を用いてもよい。この場合、主観視点カメラ４５０による撮影画像を画像入力部１１０へ入力し、指標検出部１２０で座標を検出し、位置姿勢算出部１３０による解の算出工程に第２の実施形態と同様な観測方程式（式１７及び式４）を組み込めばよい。同様に、撮像装置１５０に主観視点カメラ４５０を装着してもよいし、この場合は、撮像装置１５０による指標の観測は必須とはならない。

また、第１及び第２の実施形態に係る位置姿勢計測装置は、第３の実施形態において利用されているような環境側に設置された指標を用いていなかった。一方、第３の実施形態に係る位置姿勢計測装置は、第１及び第２の実施形態において利用されているような客観視点カメラを用いていなかった。しかし、客観視点カメラによって観測した計測対象物体（撮像装置１５０及び計測対象物体１６０）上の指標の情報と、主観視点カメラ（撮像装置１５０及び主観視点カメラ４５０）によって観測した環境側の指標の情報を併用してもよい。

すなわち、第１及び第２の実施形態において、第３の実施形態と同様に環境側に指標を設定し、撮像装置１５０及び主観視点カメラ４５０によって撮影したこれらの指標の情報を撮像装置１５０及び計測対象物体１６０の位置及び姿勢の算出に用いてもよい。この場合、位置姿勢算出部１３０あるいは４３０による解の算出工程に、第３の実施形態と同様な観測方程式（式１８〜式２０及び式４）を組み込めばよい。また、一方の計測対象物体のみが両者（主観視点カメラによる環境側の指標の観測と客観視点カメラによる物体上の指標の観測）を併用するという構成も可能であるし、一方の計測対象物体には環境側の指標を観測する主観視点カメラを設置せずにその物体上の指標を客観視点カメラで観測し、さらに、もう一方の計測対象物体上には指標を設定せずにその物体に設置した主観視点カメラによって環境側の指標の観測するという構成も可能であるし、発明の本質を逸脱しなければ、どのような組み合わせを取ることも可能である。

（変形例５）
上記の実施形態の各々に係る位置姿勢計測装置は２つの物体を計測対象としていたが、計測する物体の数は２つに限定されるものではない。例えば第１の実施形態において、１台の撮像装置と複数の計測対象物体を計測する場合には、夫々の計測対象物体上に指標を設定し、これを撮像装置１５０及び客観視点カメラ１４０によって観測し、各物体の位置及び姿勢を未知パラメータとして解けばよい。第２、第３の実施形態においても同様であり、夫々に必要な主観視点カメラや姿勢センサを設置することで、前述した実施形態における技術思想の範疇において実現可能である。

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

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

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

第１の実施形態における位置姿勢計測装置の構成を示す図である。 位置姿勢計測装置の各部をソフトウェアにより実現することのできるコンピュータの基本構成を示す図である。 第１の実施形態における、撮像装置１５０及び計測対象物体１６０の位置及び姿勢を算出する処理を説明するフローチャートである。 第２の実施形態における位置姿勢計測装置の構成を示す図である。 第２の実施形態における、計測対象物体１６０の位置及び姿勢を算出する処理を説明するフローチャートである。 第３の実施形態における位置姿勢計測装置の構成を示す図である。

Claims (14)

1. 第一の撮像装置の位置及び姿勢と、該第一の撮像装置による被観測物体の位置及び姿勢とを計測する位置姿勢計測方法であって、
   前記第一の撮像装置が撮像した第一の撮像画像から、前記被観測物体上の指標を検出する第一の指標検出工程と、
   環境中に設置された第二の撮像装置が撮像した第二の撮像画像から、前記被観測物体上の指標を検出する第二の指標検出工程と、
   前記第二の撮像画像から前記第一の撮像装置上の指標を検出する第三の指標検出工程と、
   前記第一、第二、及び第三の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記第一の撮像装置の位置及び姿勢と前記被観測物体の位置及び姿勢とを算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
2. 第一の撮像装置の位置及び姿勢と、該第一の撮像装置による被観測物体の位置及び姿勢とを計測する位置姿勢計測方法であって、
   前記第一の撮像装置が撮像した第一の撮像画像から、前記被観測物体上の指標を検出する第一の指標検出工程と、
   前記被観測物体に装着された第二の撮像装置が撮像した第二の撮像画像から、環境中の指標を検出する第二の指標検出工程と、
   前記第一の撮像画像または前記第一の撮像装置に装着された第三の撮像装置が撮像した第三の撮像画像から、環境中の指標を検出する第三の指標検出工程と、
   前記第一、第二、及び第三の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記第一の撮像装置の位置及び姿勢と前記被観測物体の位置及び姿勢とを算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
3. 複数の計測対象物体の夫々の位置及び姿勢あるいは当該計測対象物体間の相対的な位置及び姿勢の関係を計測する位置姿勢計測方法であって、
   前記計測対象物体のいずれか一つ以上に装着された第一の撮像装置によって、他の計測対象物体上の指標を観測し検出する第一の指標検出工程と、
   環境中に設置された第二の撮像装置によって、前記夫々の計測対象物体上の指標を観測し検出する第二の指標検出工程と、
   前記第一及び第二の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記計測対象物体の夫々の位置及び姿勢あるいは前記計測対象物体間の相対的な位置及び姿勢の関係を算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
4. 複数の計測対象物体の夫々の位置及び姿勢あるいは当該計測対象物体間の相対的な位置及び姿勢の関係を計測する位置姿勢計測方法であって、
   前記計測対象物体のいずれか一つ以上に装着された第一の撮像装置によって、他の計測対象物体上の指標を観測し検出する第一の指標検出工程と、
   前記計測対象物体の夫々に装着した第二の撮像装置によって、環境中の指標を観測し検出する第二の指標検出工程と、
   前記第一及び第二の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記計測対象物体の夫々の位置及び姿勢あるいは前記計測対象物体間の相対的な位置及び姿勢の関係を算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
5. 前記位置姿勢算出工程は、前記夫々の指標検出工程において検出された指標の画像座標に関する情報と、前記計測対象物体の夫々の位置及び姿勢のパラメータの推定値を用いて求められた当該画像座標に関する情報の計算値との間の誤差に基づき、前記計測対象物体の夫々の位置及び姿勢のパラメータの推定値を補正することを特徴とする請求項１乃至請求項４のいずれかに記載の位置姿勢計測方法。
6. 前記位置姿勢算出工程は、前記推定値の補正を行う際の補正値の算出処理を、前記計測対象物体の夫々に関して同時に行うことを特徴とする請求項５に記載の位置姿勢計測方法。
7. 前記計測対象物体の少なくとも一つ以上の位置または／及び姿勢を計測するセンサから位置または／及び姿勢の計測値を入力するセンサ計測値入力工程をさらに有し、
   前記位置姿勢算出工程は、前記夫々の指標検出工程において検出された指標の画像座標に関する情報に加えて、前記センサ計測値入力工程において入力された前記計測対象物体の位置または／及び姿勢の計測値を用いて、前記計測対象物体の夫々の位置及び姿勢を算出することを特徴とする請求項１乃至請求項６のいずれかに記載の位置姿勢計測方法。
8. 前記位置姿勢算出工程は、前記センサ計測値入力工程において入力された前記計測対象物体の位置または／及び姿勢の計測値のうちの少なくともいずれかのパラメータを既知の値として利用して、前記夫々の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記計測対象物体の夫々の位置及び姿勢のうちの残されたパラメータを算出することを特徴とする請求項７に記載の位置姿勢計測方法。
9. 第一の撮像装置の位置及び姿勢と、該第一の撮像装置による被観測物体の位置及び姿勢とを計測する位置姿勢計測装置であって、
   前記第一の撮像装置が撮像した第一の撮像画像から、前記被観測物体上の指標を検出する第一の指標検出手段と、
   環境中に設置された第二の撮像装置が撮像した第二の撮像画像から、前記被観測物体上の指標を検出する第二の指標検出手段と、
   前記第二の撮像画像から前記第一の撮像装置上の指標を検出する第三の指標検出手段と、
   前記第一、第二、及び第三の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記第一の撮像装置の位置及び姿勢と前記被観測物体の位置及び姿勢とを算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。
10. 第一の撮像装置の位置及び姿勢と、該第一の撮像装置による被観測物体の位置及び姿勢とを計測する位置姿勢計測装置であって、
    前記第一の撮像装置が撮像した第一の撮像画像から、前記被観測物体上の指標を検出する第一の指標検出手段と、
    前記被観測物体に装着された第二の撮像装置が撮像した第二の撮像画像から、環境中の指標を検出する第二の指標検出手段と、
    前記第一の撮像画像または前記第一の撮像装置に装着された第三の撮像装置が撮像した第三の撮像画像から、環境中の指標を検出する第三の指標検出手段と、
    前記第一、第二、及び第三の指標検出手段において検出された指標の画像座標に関する情報を用いて、前記第一の撮像装置の位置及び姿勢と前記被観測物体の位置及び姿勢とを算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。
11. 複数の計測対象物体の夫々の位置及び姿勢あるいは当該計測対象物体間の相対的な位置及び姿勢の関係を計測する位置姿勢計測装置であって、
    前記計測対象物体のいずれか一つ以上に装着された第一の撮像装置によって、他の計測対象物体上の指標を観測し検出する第一の指標検出手段と、
    環境中に設置された第二の撮像装置によって、前記夫々の計測対象物体上の指標を観測し検出する第二の指標検出手段と、
    前記第一及び第二の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記計測対象物体の夫々の位置及び姿勢あるいは前記計測対象物体間の相対的な位置及び姿勢の関係を算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。
12. 複数の計測対象物体の夫々の位置及び姿勢あるいは当該計測対象物体間の相対的な位置及び姿勢の関係を計測する位置姿勢計測装置であって、
    前記計測対象物体のいずれか一つ以上に装着された第一の撮像装置によって、他の計測対象物体上の指標を観測し検出する第一の指標検出手段と、
    前記計測対象物体の夫々に装着した第二の撮像装置によって、環境中の指標を観測し検出する第二の指標検出手段と、
    前記第一及び第二の指標検出工程において検出された指標の画像座標に関する情報を用いて、前記計測対象物体の夫々の位置及び姿勢あるいは前記計測対象物体間の相対的な位置及び姿勢の関係を算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。
13. 請求項１乃至請求項８のいずれか１項に記載の位置姿勢計測方法をコンピュータに実行させるための制御プログラム。
14. 請求項１３に記載の制御プログラムを格納するコンピュータ読み取り可能な記録媒体。
    

Images