JP2005353036A - 情報処理装置および情報処理方法 - Google Patents

Abstract

【課題】 物体に装着した指標の、対象物体に対する位置または位置及び姿勢を、簡便かつ正確に取得する。
【解決手段】 物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理方法であって、前記物体の位置姿勢情報を取得する物体位置姿勢取得工程と、前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力工程と、前記第１の画像から前記指標を検出する検出工程と、前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする。
【選択図】 図１

Description

本発明は、物体上に設置した指標の、物体座標系における位置または位置姿勢を求めるものに関する。

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

このような画像表示装置の応用としては、患者の体表面に体内の様子を重畳表示する手術支援や、現実空間に浮遊する仮想の敵と戦う複合現実感ゲームなど、今までのバーチャルリアリティとは異なった新たな分野が期待されている。

これらの応用に対して共通に要求されるのは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置及び姿勢の関係（以下ではこれを、撮像装置に対する対象物体の位置及び姿勢と称する）を求める問題に帰結される。

この問題を解決する方法として、対象物体上に、対象物体の座標系における配置が既知であるような複数の指標を設置あるいは設定し、既知の情報であるその指標の物体座標系における座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、撮像装置に対する対象物体の位置及び姿勢を求めることが一般的に行われている。

物体座標系における指標の座標とその投影像の画像座標の組から撮像装置に対する対象物体の位置及び姿勢を算出する方法は、写真測量等の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。

また、磁気センサのような６自由度位置姿勢センサを撮像装置と対象物体の双方に取り付け、撮像装置が撮像した画像内における指標の投影像の座標に加え、センサによって計測された（センサが基準としている基準座標系における）撮像装置と対象物体の夫々の位置及び姿勢の計測値もさらに利用して、撮像装置に対する対象物体の位置及び姿勢を求めるハイブリッドな位置姿勢計測方法も提案されている（例えば、特許文献１では、画像上で検出された指標の投影像の情報を用いて、センサ計測値の誤差が補正される）。

また、本出願人は、特願２００３−３２３１０１において、ある基準座標系における対象物体の位置及び姿勢を計測したいという目的において、対象物体に指標を設置し、基準座標系に固定したカメラによってこれを撮影することで、固定カメラに対する対象物体の位置及び姿勢を求め、これによって、基準座標系における対象物体の位置及び姿勢を得るという方法を提案している。
Ｒ．Ｍ．Ｈａｒａｌｉｃｋ，Ｃ．Ｌｅｅ，Ｋ．Ｏｔｔｅｎｂｅｒｇ，ａｎｄ Ｍ．Ｎｏｌｌｅ：Ｒｅｖｉｅｗ ａｎｄ ａｎａｌｙｓｉｓ ｏｆ ｓｏｌｕｔｉｏｎｓ ｏｆ ｔｈｅ ｔｈｒｅｅ ｐｏｉｎｔ ｐｅｒｓｐｅｃｔｉｖｅ ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｐｒｏｂｌｅｍ，Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｖｏｌ．１３，ｎｏ．３，ｐｐ．３３１−３５６，１９９４． Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ ｐａｒａｍｅｔｅｒｉｚｅｄ ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ ｍｏｄｅｌｓ ｔｏ ｉｍａｇｅｓ，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１． 内山，山本，田村："複合現実感のためのハイブリッド位置合わせ手法−６自由度センサとビジョン手法の併用−"，日本バーチャルリアリティ学会論文誌，ｖｏｌ．８，ｎｏ．１，ｐｐ．１１９−１２５，２００３． 藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９，ｎｏ．５７４，ｐｐ．１−８，１９９９． Ａ．Ｉ．Ｃｏｍｐｏｒｔ，Ｅ．Ｍａｒｃｈａｎｄ，Ｆ．Ｃｈａｕｍｅｔｔｅ，Ａ ｒｅａｌ−ｔｉｍｅ ｔｒａｃｋｅｒ ｆｏｒ ｍａｒｋｅｒｌｅｓｓ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ，Ｐｒｏｃ．Ｉｎｔ'ｌ Ｓｙｍｐ．ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ ２００４，ｐｐ．３６−４５，２００４．

ところで、撮像装置が撮影した画像内における指標の投影像の画像座標を検出することで対象物体の位置及び姿勢を算出する上記の方法においては、対象物体に設定した複数の指標と、対象物体との相対的な位置関係は、既知でなければならない。個々の指標の相対的な位置関係が未知である場合、例えば、着色された球状の指標や円形状の指標を用いてその画像上における指標の投影像の重心位置を特徴として利用する場合のように、特徴が一つの点で記述されるような指標が対象物体上に個々に設置されている場合には、対象物体座標系における個々の指標の３次元位置が予め計測されていなくてはならない。一方、指標同士の相対的な位置関係が既知である場合、例えば、上記と同様な指標を用いる場合であっても、それらの指標が予めベースとなる治具に装着されており、この治具の座標系（このように指標の相対的な位置を記述するための座標系を以下では指標座標系と呼ぶ）における各指標の位置が既に計測されており、この治具が対象物体に装着されているような場合には、対象物体座標系におけるこの治具の位置と姿勢が既知であれば良い（なお、特徴が複数の点で記述されるような指標、例えば、形状の既知な四角形や三角形の指標を用いてその画像上における指標の投影像の各頂点を特徴として利用する場合は、この指標自体が複数の指標の集まりであると解釈して、「指標同士の相対的な位置関係が既知である場合」の一種として考えることにする）。しかしながら、これらを正確に較正する方法は一般的に知られておらず、従来までは手動計測によって得られる不正確な位置または位置及び姿勢を既知の値として利用する以外に方策がなかった。その結果、上記の撮像装置が撮影した画像内における指標の投影像の画像座標を検出することで対象物体の位置及び姿勢を算出する方法においては、対象物体の位置及び姿勢を実用上低い精度でしか計測できないという改善の余地があった。

本発明は以上の問題を鑑みてなされたものであり、物体に装着した指標の、対象物体に対する（すなわち、対象物体座標系における）位置または位置及び姿勢を、簡便かつ正確に取得することを目的とする。

上記目的を達成するために、請求項１記載の発明は、物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理方法であって、前記物体の位置姿勢情報を取得する物体位置姿勢取得工程と、前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力工程と、前記第１の画像から前記指標を検出する検出工程と、前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする。

また、請求項９記載の発明は、物体に装着した指標の前記物体を基準とした位置または位置姿勢を、当該指標の較正情報として算出する情報処理方法であって、カメラによって前記物体を撮影することにより得られた画像を入力する画像入力工程と、前記指標の画像座標に関する情報を前記画像から検出する検出工程と、前記物体と前記カメラとの間の相対的な位置姿勢を算出する物体位置姿勢算出工程と、前記物体位置姿勢算出工程で算出された前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記検出工程で検出された指標の画像座標に関する情報を用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする。

本発明によれば物体に対する指標の位置または位置姿勢を、簡便かつ正確に取得することができる。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。

［第１の実施形態］
本実施形態に係る指標較正装置は、対象物体に装着した指標の較正を行うためのものであり、対象物体に装着した６自由度位置姿勢センサと、固定位置に設置した客観視点カメラを利用して、対象物体に対する（すなわち、対象物体座標系における）夫々の指標の位置を、指標ごとに算出する。以下、本実施形態に係る指標較正装置及び指標較正方法について説明する。なお、本実施形態では、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」によって、６自由度位置姿勢センサが構成されているものとする。また、その６自由度位置姿勢センサが取り付けられている対象物体は、「撮像装置」自体であるとする。すなわち、本実施形態に係る指標較正装置は、撮像装置に装着した（他の撮像装置によって観測されるための）指標の較正を行うためのものであり、撮像装置の座標系（以下、これを主観視点カメラ座標系と呼ぶ）における夫々の指標の位置を、指標ごとに算出するものといえる。

図１は、本実施形態に係る指標較正装置１００の概略構成を示す図である。図１に示したように、本実施形態に係る指標較正装置１００は、主観視点指標検出部１１０、位置姿勢算出部１２０、客観視点カメラ１４０、客観視点指標検出部１５０、指示部１６０、データ管理部１７０、較正情報算出部１８０によって構成されており、較正対象である指標が装着された撮像装置１３０に接続されている。

なお、撮像装置１３０上には、較正される対象である指標Ｐ^ｋ（ｋ＝１，，，Ｋ_２）（以下、これを客観視点指標と呼ぶ）が１つ以上設定されているものとする。主観視点カメラ座標系（＝対象物体座標系：撮像装置１３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における客観視点指標の位置は未知であり、この未知である客観視点指標の位置が、本実施形態に係る指標較正装置が較正する情報となる。

現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標（以下、主観視点指標（基準指標））として、世界座標系（現実空間の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における位置が既知である複数個の主観視点指標Ｑ^ｋ（ｋ＝１，，，Ｋ_１）が配置されている。主観視点指標Ｑ^ｋは、指標較正用のデータを取得する時の撮像装置１３０によって、少なくとも３個以上の指標が観測されるように設置されていることが望ましい。図１の例は、４個の主観視点指標Ｑ^１，Ｑ^２，Ｑ^３，Ｑ^４が配置されており、そのうちの３個Ｑ^１，Ｑ^３，Ｑ^４が撮像装置１３０の視野内に含まれている状況を示している。

主観視点指標Ｑ^ｋは、例えば、それぞれが異なる色を有する円形状のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する四角形領域によって形成されるような四角形指標を用いることも可能である。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが識別可能であるような指標であれば、何れの形態であってもよい。

撮像装置１３０が出力する画像（以下、これを主観視点画像と呼ぶ）は、主観視点指標検出部１１０に入力される。

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

さらに、主観視点指標検出部１１０は、検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。例えば図１の場合には、Ｎ＝３であり、識別子ｋ_１＝１，ｋ_２＝３，ｋ_３＝４とこれらに対応する画像座標ｕ^Ｑｋ１，ｕ^Ｑｋ２，ｕ^Ｑｋ３が出力される。本実施形態における主観視点指標検出部１１０は、撮像装置１３０からの画像の入力をトリガとして上記の指標検出処理を逐次実行しているが、位置姿勢算出部１２０からの要求に応じて（その時点で入力している主観視点画像を利用して）処理を実行する構成としても良い。

位置姿勢算出部１２０は、検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと、既知な情報として予め保持している指標の世界座標ｘ_Ｗ ^Ｑｋｎとの対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。主観視点指標の世界座標と画像座標の組から撮像装置の位置及び姿勢を算出する方法は、写真測量等の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。例えば、主観視点指標が同一平面上に配置されている場合には、４点以上の指標を検出することで、２次元ホモグラフィの計算に基づいて撮像装置の位置及び姿勢を得ることができる。また、同一平面上にない６点以上の指標を用いて撮像装置の位置と姿勢を用いる方法も良く知られている。また、撮像装置の位置と姿勢の推定値から算出される指標の画像座標の理論値と指標の画像座標の実測値との誤差を、画像ヤコビアンを用いたガウス・ニュートン法のような繰り返し計算によって最小化することにより、位置と姿勢の推定値を最適化する方法などを用いることもできる。算出した撮像装置１３０の世界座標系における位置及び姿勢は、データ管理部１７０の要求に従って、データ管理部１７０へと出力される。なお、本実施形態における位置姿勢算出部１２０は、主観視点指標検出部１１０からのデータの入力をトリガとして上記の位置及び姿勢の算出処理を逐次実行しているが、データ管理部１７０からの要求に応じて（その時点での入力データを利用して）処理を実行する構成としても良い。以下では、撮像装置１３０の位置及び姿勢は、それぞれ３値ベクトル［ｘ ｙ ｚ］^Ｔ及び［ξ ψ ζ］^Ｔによって保持されているものとする。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。このとき、位置及び姿勢は、６次元ベクトル［ｘ ｙ ｚ ξ ψ ζ］^Ｔで表される。

客観視点カメラ１４０は、指標較正用のデータ取得を行う時の撮像装置１３０を撮像可能な位置に固定して配置されている。世界座標系における客観視点カメラ１４０の位置及び姿勢は、既知の値として較正情報算出部１８０に予め保持されているものとする。

客観視点指標検出部１５０は、客観視点カメラ１４０が撮影した画像（客観視点画像）を入力し、主観視点指標検出部１１０と同様な処理によって画像中に撮影されている客観視点指標Ｐ^ｋの画像座標を検出し、データ管理部１７０の要求に従って検出された客観視点指標Ｐ^ｋｍの画像座標ｕ^Ｐｋｍとその識別子ｋ_ｍをデータ管理部１７０へと出力する。ここで、ｍ（ｍ＝１，，，Ｍ）は検出された指標夫々に付けられたインデックスであり、Ｍは検出された指標の総数を表している。例えば図１の場合には、Ｍ＝２であり、識別子ｋ_１＝１，ｋ_２＝２とこれらに対応する画像座標ｕ^Ｐｋ１，ｕ^Ｐｋ２が出力される。本実施形態における客観視点指標検出部１５０は、客観視点画像の入力をトリガとして上記の指標検出処理を逐次実行しているが、データ管理部１７０からの要求に応じて（その時点で入力している客観視点画像を利用して）処理を実行する構成としても良い。

指示部１６０は、不図示のオペレータからデータ取得コマンドが入力された時には「データ取得」の指示をデータ管理部１７０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部１８０に送信する。指示部１６０へのコマンド入力は、例えばキーボードを用いて、特定のコマンドを割り当てたキーを押すことによって行うことができる。また、コマンドの入力は、ディスプレイ上に表示されたＧＵＩで行うなどの、いずれの方法で行ってもよい。

データ管理部１７０は、指示部１６０から「データ取得」の指示を受けると、位置姿勢算出部１２０から撮像装置１３０の世界座標系における位置及び姿勢を入力し、客観視点指標検出部１５０から客観視点指標の画像座標とその識別子を入力し、［撮像装置１３０の位置及び姿勢−客観視点指標の画像座標］の組を、客観視点指標の識別子毎に用意したデータリストに追加しこれを保持する。ここで、位置姿勢算出部１２０から入力される撮像装置１３０の位置及び姿勢は、客観視点指標検出部１５０から入力される客観視点指標の画像座標を検出した画像の撮影時刻と、同一時刻のものである。また、データ管理部１７０は、較正情報算出部１８０からの要求にしたがって、生成した客観視点指標の識別子毎のデータリストを較正情報算出部１８０に出力する。

較正情報算出部１８０は、指示部１６０から「較正情報算出」の指示を受けると、データ管理部１７０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、各客観視点指標の主観視点カメラ座標系における位置）を出力する。

図２は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお、同フローチャートに従ったプログラムコードは、本実施形態の装置内の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。

ステップＳ２０１０において、指示部１６０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、指標較正用のデータ取得を行う位置に撮像装置１３０を配置した時に、データ取得コマンドを入力する。指示部１６０は、データ取得コマンドが入力されている場合には、ステップＳ２０２０へと処理を移行させる。

ステップＳ２０２０において、データ管理部１７０は、位置姿勢算出部１２０から、撮像装置１３０の世界座標系における位置及び姿勢ｔ＝［ｘ ｙ ｚ ξ ψ ζ］^Ｔを入力する。

ステップＳ２０３０において、データ管理部１７０は、客観視点指標検出部１５０から、客観視点指標検出部１５０によって検出された客観視点指標Ｐ^ｋｍの画像座標ｕ^Ｐｋｍとその識別子ｋ_ｍを入力する。客観視点指標検出部１５０は、入力された客観視点画像に対して常に指標の検出処理を行っており、以上のステップＳ２０２０，Ｓ２０３０の処理により、撮像装置の位置と姿勢がｔであるときの客観視点指標の画像座標を得ることができる。なお、客観視点指標検出部１５０から入力される情報は、必ずしも全ての客観視点指標に関するものである必要はなく、その時点で客観視点画像上において検出されている指標に関する情報であればよい。

次に、ステップＳ２０４０において、データ管理部１７０は、検出された客観視点指標Ｐ^ｋｍ毎に、入力したデータの組をデータリストＬ^Ｐｋに追加する。具体的には、位置姿勢算出部１２０から入力するｔをｔ_ｉ＝［ｘ_ｉ ｙ_ｉ ｚ_ｉ ξ_ｉ ψ_ｉ ζ_ｉ］^Ｔとし、客観視点指標検出部１５０から入力するｕ^Ｐｋｍをｕ_ｉ ^Ｐｋとして、［ｔ_ｉ，ｕ_ｉ ^Ｐｋ］の組を、客観視点指標検出部１５０から入力する識別子ｋ_ｍを参照することで振り分けながら、Ｐ^ｋに関するｉ番目のデータとして客観視点指標Ｐ^ｋに関するデータリストＬ^Ｐｋに登録する。ここでｉ（ｉ＝１，，，Ｉ^Ｐｋ）は、データリストＬ^Ｐｋに登録したデータの組夫々に対するインデックスであり、Ｉ^Ｐｋは、客観視点指標Ｐ^ｋに関して登録したデータの総組数を表している。

以上の処理によって、データの取得が行われる。

ステップＳ２０５０では、データ管理部１７０によって、それまでに取得された、全ての客観視点指標に関するデータリスト、もしくは、少なくとも１つの客観視点指標に関するデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。少なくとも１つのデータリスト、もしくは、全てのデータリストが条件を満たしていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。一方、全てのデータリスト、もしくは、少なくとも１つのデータリストが較正情報算出の条件を満たしている場合には、ステップＳ２０６０へと処理を移行させる。ある客観視点指標Ｐ^ｋに関するデータリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、データリストＬ^Ｐｋが異なるデータ［ｔ_ｉ，ｕ_ｉ ^Ｐｋ］を２組以上得ていること、が挙げられる。後述するように、１組のデータから２つの方程式が得られるため（式３参照）、２組以上のデータを得ていれば、４つの方程式から３パラメータである客観視点指標の主観視点カメラ座標系における位置を決定することができる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。

次にステップＳ２０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ２０７０へと処理を移行し、入力されていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。

較正情報算出部１８０は、求めるべき較正情報、すなわち客観視点指標の主観視点カメラ座標系における位置を３値ベクトル［ｘ ｙ ｚ］^Ｔとして扱う。以下では、この未知パラメータを状態ベクトルｓ^Ｐｋ＝［ｘ^Ｐｋ ｙ^Ｐｋ ｚ^Ｐｋ］^Ｔと記述する。以下、ある客観視点指標Ｐ^ｋに対する処理を想定して説明を続けるが、各処理は較正情報算出の条件を満たしたデータリストに対応する全ての客観視点指標に対して共通に行われるものとする。

ステップＳ２０７０において、較正情報算出部１８０は、状態ベクトルｓ^Ｐｋに適当な初期値（例えば［０ ０ ０］^Ｔ）を与える。

ステップＳ２０８０において、較正情報算出部１８０は、データリストＬ^Ｐｋ中の各データ［ｔ_ｉ，ｕ_ｉ ^Ｐｋ］（ｉ＝１，２，，，，Ｉ^Ｐｋ）および状態ベクトルｓ^Ｐｋから、全てのｉに対して、客観視点指標Ｐ^ｋの客観視点画像座標の理論値ｕ_ｉ ^Ｐｋ'＝［ｕ_ｘｉ ^Ｐｋ'，ｕ_ｙｉ ^Ｐｋ'］を算出する。ここで客観視点指標の客観視点画像座標の理論値とは、主観視点カメラ座標系における位置が与えられている客観視点指標Ｐ^ｋの、客観視点画像中に見えるべき位置（座標）のデータを指す。ｕ_ｉ ^Ｐｋ'の算出は、客観視点指標Ｐ^ｋの主観視点カメラ座標系における位置を表す状態ベクトルｓ^Ｐｋの関数

に基づいて行われる。

具体的には、関数Ｆ_ｉ（）は、ｉ番目のデータを取得した時の（すなわち、撮像装置１３０の位置と姿勢がｔ_ｉである時の）客観視点指標Ｐ^ｋの客観視点カメラ座標における位置ベクトルｂ_ｉ ^Ｐｋを、ｓ^Ｐｋから求める次式、

及び、ｂ_ｉ ^Ｐｋから客観視点指標Ｐ^ｋの客観視点画像上の座標ｕ_ｉ ^Ｐｋ'を求める次式、

によって構成されている。ここでｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ１４０の焦点距離であり、既知の値として予め保持されているものとする。また、Ｍ_ＷＢは客観視点カメラ座標系における座標を世界座標に変換する変換行列であり、既知の値として予め保持されている世界座標系における客観視点カメラ１４０の位置及び姿勢に基づいて、予め算出されているものとする。Ｍ_ＷＣｉはｔ_ｉによって定まるモデリング変換行列（主観視点カメラ座標系における座標を世界座標系における座標に変換する行列）であり、次式によって定義される。

ただし、

である。

ステップＳ２０９０において、較正情報算出部１８０は、全てのｉに対して、データリストＬ^Ｐｋ中の各データに含まれる客観視点指標Ｐ^ｋの実際の画像座標ｕ_ｉ ^Ｐｋと、それに対応する画像座標の理論値ｕ_ｉ ^Ｐｋ'との誤差△ｕ_ｉ ^Ｐｋを次式によって算出する。

ステップＳ２１００において、較正情報算出部１８０は、全てのｉに対して、状態ベクトルｓ^Ｐｋに関する画像ヤコビアン（すなわち、式１の関数Ｆ_ｉ（）を状態ベクトルｓ^Ｐｋの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列）Ｊ_ｕｉｓ ^Ｐｋ（＝∂ｕ_ｉ ^Ｐｋ／∂ｓ^Ｐｋ）を算出する。具体的には、式３の右辺を客観視点カメラ座標系上の位置ベクトルｂ_ｉ ^Ｐｋの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｉｂｉ ^Ｐｋ（＝∂ｕ_ｉ ^Ｐｋ／∂ｂ_ｉ ^Ｐｋ）と、式２の右辺を状態ベクトルｓ^Ｐｋの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列Ｊ_ｂｉｓ ^Ｐｋ（＝∂ｂ_ｉ ^Ｐｋ／∂ｓ^Ｐｋ）を算出し、次式によってＪ_ｕｉｓ ^Ｐｋを算出する。

ステップＳ２１１０において、較正情報算出部１８０は、以上のステップで算出した、全てのｉに対する誤差△ｕ_ｉ ^Ｐｋ及びヤコビ行列Ｊ_ｕｉｓ ^Ｐｋに基づいて、ｓ^Ｐｋの補正値△ｓ^Ｐｋを算出する。具体的には、全てのｉに対する誤差△ｕ_ｉ ^Ｐｋを垂直に並べた２Ｉ^Ｐｋ次元ベクトルの誤差ベクトル

及びヤコビ行列Ｊ_ｕｉｓ ^Ｐｋを垂直に並べた２Ｉ^Ｐｋ行×３列の行列

を作成し、Φの擬似逆行列Φ^＋を用いて、△ｓ^Ｐｋを次式より算出する。

ここで、△ｓ^Ｐｋは３次元ベクトルであるから、２Ｉ^Ｐｋが３以上、すなわちＩ^Ｐｋが２以上であれば、△ｓ^Ｐｋを求めることができる。なおΦ^＋は、例えばΦ^＋＝（Φ^ＴΦ）^−１Φ^Ｔによって求めることができるが、他の方法によって求めてもよい。

ステップＳ２１２０において、較正情報算出部１８０は、ステップＳ２１１０において算出した補正値△ｓ^Ｐｋを用いて、式１１に従って客観視点指標Ｐ_ｋの主観視点カメラ座標系における位置ベクトルｓ^Ｐｋを補正し、得られた値を新たなｓ^Ｐｋとする。

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

ステップＳ２１３０において計算が収束したと判定されると、ステップＳ２１４０において、較正情報算出部１８０は、得られた状態ベクトルｓ^Ｐｋを、客観視点指標Ｐ^ｋの主観視点カメラ座標系における位置を示すパラメータとして出力する。

最後にステップＳ２１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、指標較正装置１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。

以上の処理によって、対象物体（撮像装置）に装着した指標の対象物体に対する（すなわち、対象物体座標系における）位置を、簡便かつ正確に取得することができる。

＜変形例１−１＞
なお、本実施形態においては、状態ベクトルの補正値の算出に式１０で表現されるニュートン法を用いているが、補正値の算出は必ずしもニュートン法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。また、本実施形態では、ステップＳ２０７０からステップＳ２１３０までの処理において、求めるべき指標の位置に初期値を与え、画像ヤコビアンを利用した繰り返し演算によって最適値を求めたが、より簡便な計算方法によって指標の位置を求めることも可能である。例えば、式３を展開すると、

の関係が得られる。この関係を利用すると、

のようにして、ｓを直接得ることができる。ここで、

を表している。

＜変形例１−２＞
また、本実施形態では、６自由度位置姿勢センサとして、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」を組み合わせたものを用いたが、世界座標系における対象物体の位置と姿勢が計測可能であれば、それ以外方法によって６自由度位置姿勢センサを構成してもよい。例えば、磁気センサのような６自由度位置姿勢センサや、ジャイロセンサのような３自由度姿勢センサを撮像装置１３０に取り付け、撮像装置１３０の撮影画像から検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと、既知な情報として予め保持している指標の世界座標ｘ_Ｗ ^Ｑｋｎとの対応関係、ならびに、撮像値１３０に取り付けたセンサの計測値、の両方に基づいたハイブリッドな方法で、撮像装置１３０の位置及び姿勢を算出しても良い（具体的な算出方法は、例えば〔非特許文献３〕、〔非特許文献４〕を参照）。

＜変形例１−３＞
本実施形態では、対象物体が撮像装置の場合であったので、世界座標系における対象物体の位置と姿勢の計測に、対象物体自身が撮影する映像情報を利用することができた。対象物体が任意の物体である場合には、その位置と姿勢の計測に、磁気センサのような６自由度位置姿勢センサを用いることができる。その場合の構成を図５に示す。

図５に示した本実施形態の変形例に係る指標較正装置５００は、任意の対象物体５１０に装着した指標の較正を行うためのものであり、対象物体５１０に装着した６自由度位置姿勢センサ５２０と、固定位置に設置した客観視点カメラ１４０を利用して、対象物体５１０に対する（すなわち、対象物体座標系における）夫々の指標の位置を、指標ごとに算出する。６自由度位置姿勢センサ５２０としては、磁気式の位置姿勢センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ Ｔｅｃｈ．社のＦｌｏｃｋ ｏｆ Ｂｉｒｄｓ）や超音波センサなどを用いることが可能であるし、他の何れの６自由度位置姿勢センサを用いてもよい。指標較正装置５００は、図１における主観視点指標検出部１１０及び位置姿勢算出部１２０に代替する構成要素として、位置姿勢算出部５３０を有している。位置姿勢算出部５３０は、６自由度位置姿勢センサ５２０より位置姿勢計測値（センサ座標系におけるセンサ自身の位置及び姿勢）を入力し、その位置姿勢計測値から、既知の較正情報を用いて世界座標系における対象物体５１０の位置及び姿勢を算出し、データ管理部１７０の要求に従って、これをデータ管理部１７０へと出力する。それ以外の部分に関しては、指標較正装置１００とまったく同様である。

＜変形例１−４＞
なお、本実施形態において、対象物体を撮影するカメラとして固定位置に設置した客観視点カメラを用いていたが、世界座標系におけるカメラの位置及び姿勢が計測可能であれば客観視点カメラを固定しなくてもよい。

例えば、磁気センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ Ｔｅｃｈ．社のＦｌｏｃｋ ｏｆ Ｂｉｒｄｓ）や、光学式センサ（加ＮＤＩ社のＯｐｔｏｔｒａｋや英ＶＩＣＯＮ社のＶＩＣＯＮ）、超音波センサといった６自由度位置姿勢センサを客観視点カメラに取り付けて、画像撮影時に客観視点カメラの世界座標系に対する位置姿勢をセンサ計測値から得てもよい。また、客観視点カメラによって撮影される主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと予め既知な情報として保持されている指標の世界座標Ｘｗ^Ｑｋｎから客観視点カメラの位置姿勢を求めてもよい。また、客観視点カメラに前述の６自由度位置姿勢センサや３自由度姿勢センサを取り付け、客観視点画像とセンサの計測値の両方に基づいたハイブッドな手法（〔非特許文献３〕〔非特許文献４〕）によって客観視点カメラの位置姿勢を求めてもよい。

この場合、上記のような手段によって計測された客観視点カメラの位置姿勢は、ステップＳ２０２０において撮像装置１３０の位置姿勢と同時に取得され、ステップＳ２０４０においてデータリストに保存される。そして、ステップ２０８０において、式２におけるＭ_ＷＢ（世界座標系における客観視点カメラの位置姿勢を表している）として、既知の値として予め保持されている固定値ではなく、データリスト中に保持されている位置姿勢計測値に基づいて算出した値を使用すればよい。

［第２の実施形態］
本実施形態に係る指標較正装置は、対象物体に装着した複数の指標が指標座標系を有しており、指標座標系における各指標の座標が既知である場合において、対象物体に装着した指標の較正を行うためのものであり、対象物体に装着した６自由度位置姿勢センサと、固定位置に設置した客観視点カメラを利用して、対象物体に対する（すなわち、対象物体座標系における）指標座標系の位置と姿勢、あるいは、対象物体に対する各指標の位置を算出する。以下、本実施形態に係る指標較正装置及び指標較正方法について説明する。なお、本実施形態においても、第１の実施形態と同様に、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」によって、６自由度位置姿勢センサが構成されているものとする。また、その６自由度位置姿勢センサが取り付けられている対象物体は、「撮像装置」自体であるとする。

図３は、本実施形態に係る指標較正装置の概略構成を示す図である。図１と同じ部分については同じ番号、記号を付けており、その詳細な説明は省略する。図３に示したように、本実施形態に係る指標較正装置３００は、主観視点指標検出部１１０、位置姿勢算出部１２０、客観視点カメラ１４０、客観視点指標検出部１５０、指示部１６０、データ管理部３７０、較正情報算出部３８０によって構成されており、較正対象である指標が装着された撮像装置１３０に接続されている。

なお、撮像装置１３０上の複数の位置には、較正される対象である客観視点指標Ｐ^ｋ（ｋ＝１，，，Ｋ_２）が設定されているものとする。主観視点カメラ座標系（＝対象物体座標系）上における各指標の位置は未知であるが、夫々の相対的な位置関係は既知であるとする。例えば、４点の客観視点指標Ｐ^ｋ（ｋ＝１，２，３，４）の集合が、サイズが既知な１つの四角形指標Ｒ^１を形成する場合が相当する。図３は、その状況を示している。この時、四角形指標Ｒ^１上のある１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系を指標座標系と呼ぶと、指標座標系における各客観視点指標Ｐ^ｋの座標は既知である。よって、主観視点カメラ座標系における指標座標系の位置及び姿勢（＝指標座標系における座標を主観視点カメラ座標に変換する変換行列）が判明すれば、未知である客観視点指標Ｐ^ｋの主観視点カメラ座標系における位置を算出することができる。このことより、本実施形態に係る指標較正装置が較正する情報は、主観視点カメラ座標系における指標座標系の位置及び姿勢であるとする。

現実空間中の複数の位置には、第１の実施形態と同様に、撮像装置１３０によって撮影するための指標として、世界座標系における位置が既知である主観視点指標Ｑ^ｋが設定されている。

主観視点指標検出部１１０は、第１の実施形態と同様に、撮像装置１３０が撮影した主観視点画像を入力し、画像中に撮影されている主観視点指標Ｑ^ｋの画像座標を検出し、検出された主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを位置姿勢算出部１２０へと出力する。

位置姿勢算出部１２０は、第１の実施形態と同様に、検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと、既知な情報として予め保持している指標の世界座標ｘ_Ｗ ^Ｑｋｎとの対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。算出した撮像装置１３０の世界座標系における位置及び姿勢は、データ管理部３７０の要求に従って、データ管理部３７０へと出力される。

客観視点カメラ１４０は、第１の実施形態と同様に、指標較正用のデータ取得を行う時の撮像装置１３０を撮像可能な位置に固定して配置されている。世界座標系における客観視点カメラ１４０の位置及び姿勢は、既知の値として較正情報算出部３８０に予め保持されているものとする。

客観視点指標検出部１５０は、第１の実施形態と同様に、客観視点カメラ１４０が撮影した客観視点画像を入力し、主観視点指標検出部１１０と同様な処理によって画像中に撮影されている客観視点指標Ｐ^ｋの画像座標を検出し、データ管理部３７０の要求に従って検出された客観視点指標Ｐ^ｋｍの画像座標ｕ^Ｐｋｍとその識別子ｋ_ｍをデータ管理部３７０へと出力する。ただし、図３の例では、指標として四角形指標を用いているので、第１の実施形態で述べた四角形指標の検出処理が実行される。

指示部１６０は、第１の実施形態と同様に、不図示のオペレータからデータ取得コマンドが入力された時には「データ取得」の指示をデータ管理部３７０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部３８０に送信する。

データ管理部３７０は、指示部１６０から「データ取得」の指示を受けると、位置姿勢算出部１２０から撮像装置１３０の世界座標系における位置及び姿勢を入力し、客観視点指標検出部１５０から客観視点指標の画像座標とその識別子を入力し、［撮像装置１３０の位置及び姿勢−客観視点指標の画像座標−客観視点指標の識別子］の組を、一つのデータリストに追加しこれを保持する。ここで、位置姿勢算出部１２０から入力される撮像装置１３０の位置及び姿勢は、客観視点指標検出部１５０から入力される客観視点指標の画像座標を検出した画像の撮影時刻と、同一時刻のものである。また、データ管理部３７０は、較正情報算出部３８０からの要求にしたがって、生成したデータリストを較正情報算出部３８０に出力する。

較正情報算出部３８０は、指示部１６０から「較正情報算出」の指示を受けると、データ管理部３７０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、主観視点カメラ座標系における指標座標系の位置と姿勢）を出力する。

図４は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。図２と同じ部分については同じ番号、記号を付けており、その詳細な説明は省略する。なお、同フローチャートに従ったプログラムコードは、本実施形態の装置内の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。

ステップＳ２０１０からステップＳ２０３０までは、第１の実施形態と同様の処理を行う。ステップＳ２０３０を終えると、ステップＳ４０４０へと処理を移行する。

次に、ステップＳ４０４０において、データ管理部３７０は、検出された客観視点指標Ｐ^ｋｍ全てについて、入力したデータの組をデータリストＬにデータＤ_ｊとして追加する。具体的には、位置姿勢算出部１２０から入力するｔをｔ_ｊ＝［ｘ_ｊ ｙ_ｊ ｚ_ｊ ξ_ｊ ψ_ｊ ζ_ｊ］^Ｔとし、客観視点指標検出部１５０から入力する識別子ｋ_ｍをｋ_ｊとし、同じく客観視点指標検出部１５０から入力するｕ^Ｐｋｍをｕ_ｊ ^Ｐｋｊとして、Ｄ_ｊ＝［ｔ_ｊ，ｕ_ｊ ^Ｐｋｊ，ｋ_ｊ］の組を、ｊ番目のデータとしてデータリストＬに登録する。ここでｊ（ｊ＝１，，，Ｎ^Ｐｋ）は、データリストＬに登録したデータの組夫々に対するインデックスであり、Ｎ^Ｐｋは、登録したデータの総組数を表している。

以上の処理によって、データの取得が行われる。

ステップＳ４０５０では、データ管理部３７０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ２０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも異なる３つ以上の客観視点指標Ｐ^ｋに関するデータが、データリストＬに含まれていること、が挙げられる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。

次にステップＳ２０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ４０７０へと処理を移行し、入力されていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。

較正情報算出部３８０は、求めるべき較正情報、すなわち指標系座標の主観視点カメラ座標系における位置及び姿勢を６値ベクトル［ｘ ｙ ｚ ξ ψ ζ］^Ｔとして扱う。以下では、この未知パラメータを状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］^Ｔと記述する。

ステップＳ４０７０において、較正情報算出部３８０は、状態ベクトルｓに適当な初期値を与える。初期値としては、オペレータが指示部１６０を介しておおよその値を手入力しても良いし、ある同一時刻に入力された（すなわち、同一の客観視点画像から検出された）複数の客観視点指標の検出座標をリストＬから抽出し、このデータを用いて、客観視点座標系における指標座標系の同時刻における位置と姿勢を公知の方法によって算出し、得られた位置と姿勢から指標座標系における座標を客観視点カメラ座標系に変換する変換行列Ｍ_ＢＭを求め、さらに、リストＬ中に保持されている、同時刻において入力された撮像装置の位置と姿勢ｔに基づいて、数式４によって定まる変換行列Ｍ_ＷＣを求め、次式によって、主観視点カメラ座標系における指標系座標の位置及び姿勢を表す変換行列Ｍ_ＣＭを得て、この行列によって表される位置と姿勢をｓの初期値として利用しても良い。

ここで、Ｍ_ＷＢは、客観視点カメラ座標系における座標を世界座標に変換する変換行列であり、既知の値として予め保持されている世界座標系における客観視点カメラ１４０の位置及び姿勢に基づいて、予め算出されているものとする。

なお、客観視点座標系における指標座標系の位置と姿勢を算出する公知の手法としては、例えば、指標が同一平面上に配置されている場合には、４点以上の指標を用いて２次元ホモグラフィの計算に基づいて指標座標系の位置と姿勢を得る手法を用いることが可能である。また、同一平面上にない６点以上の指標を用いる手法や、これらの解を初期値にして、ガウス・ニュートン法のような繰り返し計算によって最適解を得る手法を用いることもできる。

ステップＳ４０８０において、較正情報算出部３８０は、データリストＬ中の各データＤ_ｊ＝［ｔ_ｊ，ｕ_ｊ ^Ｐｋｊ，ｋ_ｊ］（ｊ＝１，２，，，，Ｎ^Ｐｋ）および状態ベクトルｓから、全てのｊに対して、各客観視点指標Ｐ^ｋｊの客観視点画像座標の理論値ｕ_ｊ ^Ｐｋｊ'＝［ｕ_ｘｊ ^Ｐｋｊ'，ｕ_ｙｊ ^Ｐｋｊ'］を算出する。ここで客観視点指標の客観視点画像座標の理論値とは、指標座標系における位置ベクトルｈ_Ｍ ^Ｐｋｊが既知な客観視点指標Ｐ^ｋｊの、主観視点カメラ座標系における指標座標系の位置及び姿勢がｓである時に客観視点画像中に見えるべき位置（座標）のデータを指す。ｕ_ｊ ^Ｐｋｊ'の算出は、指標座標系の主観視点カメラ座標系における位置を表す状態ベクトルｓの関数

に基づいて行われる。

具体的には、関数Ｆ_ｊ（）は、ｊ番目のデータを取得した時の（すなわち、撮像装置１３０の位置と姿勢がｔ_ｊである時の）客観視点指標Ｐ^ｋｊの客観視点カメラ座標における位置ベクトルｂ_ｊ ^Ｐｋｊを、ｓから求める次式、

及び、ｂ_ｊ ^Ｐｋｊから客観視点指標Ｐ^ｋｊの客観視点画像上の座標ｕ_ｊ ^Ｐｋｊ'を求める次式、

によって構成されている。ここでｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ１４０の焦点距離であり、既知の値として予め保持されているものとする。また、Ｍ_ＷＣｊはｔ_ｊによって定まるモデリング変換行列であり、式４のｉをｊに読み替えた式によって定義される。また、Ｍ_ＣＭ（ｓ）はｓによって定まるモデリング変換行列（指標座標系における座標を主観視点カメラ座標系における座標に変換する行列）であり、次式によって定義される。

ただし、

である。

ステップＳ４０９０において、較正情報算出部３８０は、全てのｊに対して、データリストＬ中の各データに含まれる客観視点指標Ｐ^ｋｊの実際の画像座標ｕ_ｊ ^Ｐｋｊと、それに対応する画像座標の理論値ｕ_ｊ ^Ｐｋｊ'との誤差△ｕ_ｊ ^Ｐｋｊを次式によって算出する。

ステップＳ４１００において、較正情報算出部３８０は、全てのｊに対して、状態ベクトルｓに関する画像ヤコビアン（すなわち、式１７の関数Ｆ_ｊ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）Ｊ_ｕｊ ^Ｐｋｊ _ｓ（＝∂ｕ_ｊ ^Ｐｋｊ／∂ｓ）を算出する。具体的には、式１９の右辺を客観視点カメラ座標系上の位置ベクトルｂ_ｊ ^Ｐｋｊの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｊｂｊ ^Ｐｋｊ（＝∂ｕ_ｊ ^Ｐｋｊ／∂ｂ_ｊ ^Ｐｋｊ）と、数１８の右辺を状態ベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列Ｊ_ｂｊ ^Ｐｋｊ _ｓ（＝∂ｂ_ｊ ^Ｐｋｊ／∂ｓ）を算出し、次式によってＪ_ｕｊ ^Ｐｋｊ _ｓを算出する。

ステップＳ４１１０において、較正情報算出部３８０は、以上のステップで算出した、全てのｊに対する誤差△ｕ_ｊ ^Ｐｋｊ及びヤコビ行列Ｊ_ｕｊ ^Ｐｋｊ _ｓに基づいて、ｓの補正値△ｓを算出する。具体的には、全てのｊに対する誤差△ｕ_ｊ ^Ｐｋｊを垂直に並べた２Ｊ次元ベクトルの誤差ベクトル

及びヤコビ行列Ｊ_ｕｊ ^Ｐｋｊ _ｓを垂直に並べた２Ｊ行×６列の行列

を作成し、Φの擬似逆行列Φ^＋を用いて、△ｓを次式より算出する。

ここで、△ｓは６次元ベクトルであるから、２Ｊが６以上、すなわちＪが３以上であれば、△ｓを求めることができる。なおΦ^＋は、例えばΦ^＋＝（Φ^ＴΦ）^−１Φ^Ｔによって求めることができるが、他の方法によって求めてもよい。

ステップＳ４１２０において、較正情報算出部３８０は、ステップＳ４１１０において算出した補正値△ｓを用いて、式２７に従って指標系座標の主観視点カメラ座標系における位置及び姿勢を表す状態ベクトルｓを補正し、得られた値を新たなｓとする。

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

ステップＳ４１３０において計算が収束したと判定されると、ステップＳ４１４０において、較正情報算出部３８０は、得られた状態ベクトルｓを、較正情報、すなわち、主観視点カメラ座標系における指標座標系の位置及び姿勢として出力する。この時の出力の形態は、ｓそのものであっても良いし、ｓの位置成分を３値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓから生成した座標変換行列Ｍ_ＣＭであっても良い。

最後にステップＳ２１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、指標較正装置３００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。

以上の処理によって、対象物体（撮像装置）に装着した指標の対象物体に対する（すなわち、対象物体座標系における）位置または位置及び姿勢を、簡便かつ正確に取得することができる。

＜変形例２−１＞
なお、本実施形態においては、較正情報として、主観視点カメラ座標系における指標座標系の位置及び姿勢を出力していたが、得られた状態ベクトルｓから、主観視点カメラ座標系における客観視点指標Ｐ^ｋの位置を個々に算出して、これらを較正情報として出力しても良い。この場合、主観視点カメラ座標系における客観視点指標Ｐ^ｋの位置は、ｓから求められる座標変換行列Ｍ_ＣＭと、指標座標系における客観視点指標Ｐ^ｋの既知の位置ｈ_Ｍ ^Ｐｋとの積によって得ることができる。

＜変形例２−２＞
また、本実施形態では、６自由度位置姿勢センサとして、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」を組み合わせたものを用いたが、世界座標系における対象物体の位置と姿勢が計測可能であれば、それ以外の方法によって６自由度位置姿勢センサを構成してもよい。例えば、第１の実施例と同様に、磁気センサのような６自由度位置姿勢センサや、ジャイロセンサのような３自由度姿勢センサを撮像装置１３０に取り付け、撮像装置１３０の撮影画像から検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと、既知な情報として予め保持している指標の世界座標ｘ_Ｗ ^Ｑｋｎとの対応関係、ならびに、撮像値１３０に取り付けたセンサの計測値、の両方に基づいたハイブリッドな方法で、撮像装置１３０の位置及び姿勢を算出しても良い（具体的な算出方法は、例えば非特許文献３、非特許文献４を参照）。

＜変形例２−３＞
なお、本実施形態においては、状態ベクトルの補正値の算出に式２６で表現される最急降下法を用いているが、補正値の算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。また、本実施形態においては、主観視点カメラ座標系における指標座標系の位置及び姿勢ｓに適当な初期値を与え、画像ヤコビアンを用いた繰り返し計算によって、全入力データに対するｓの最適値を求める方法について説明したが、他の簡易的な算出方法を用いることでも、主観視点カメラ座標系における指標座標系の位置及び姿勢を得ることができる。例えば、ステップＳ４０７０の説明で述べたような処理手順によって、１枚の客観視点画像上における複数の客観視点指標の検出座標のみを用いて、主観視点カメラ座標系における指標系座標の位置及び姿勢を得て、これを較正情報として出力してもよい。

＜変形例２−４＞
なお、本実施形態においては、客観視点指標として四角形指標を用いていたが、指標座標系における各指標の位置が既知である指標群であれば、指標の種類は何れのものでもよい。例えば、第一の実施形態で用いたような複数の円形状指標の組であってもよいし、複数種の指標が混在していてもよい。また、複数の指標座標系を有する場合であっても、個々の指標座標系毎に上記の処理を個別に行うか、或いは、個々の指標座標系毎に上記の処理を並列的に行うことで、同様に較正することが可能である。

＜変形例２−５＞
本実施形態では、対象物体が撮像装置の場合であったので、世界座標系における対象物体の位置と姿勢の計測に、対象物体自身が撮影する映像情報を利用することができた。対象物体が任意の物体である場合には、その位置と姿勢の計測に、磁気センサのような６自由度位置姿勢センサを用いることができる。その場合の構成は、図５に示した第１の実施形態の＜変形例１−３＞と同様である。６自由度位置姿勢センサとしては、磁気式の位置姿勢センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ Ｔｅｃｈ．社のＦｌｏｃｋ ｏｆ Ｂｉｒｄｓ）や光学式センサ（加ＮＤＩ社のＯｐｔｏｔｒａｋや英ＶＩＣＯＮ社のＶＩＣＯＮ）、超音波センサなどを用いることが可能であるし、他の何れの６自由度位置姿勢センサを用いてもよい。

＜変形例２−６＞
なお、本実施形態において、対象物体を撮影するカメラとして固定位置に設置した客観視点カメラを用いていたが、第１の実施形態の＜変形例１−４＞と同様に、世界座標系におけるカメラの位置及び姿勢が計測可能であれば客観視点カメラを固定しなくてもよい。

この場合、計測された客観視点カメラの位置姿勢は、ステップＳ２０２０において撮像装置１３０の位置姿勢と同時に取得され、ステップＳ４０４０においてデータリストに保存される。そして、ステップ４０８０において、式１８におけるＭ_ＷＢ（世界座標系における客観視点カメラの位置姿勢を表している）として、既知の値として予め保持されている固定値ではなく、データリスト中に保持されている位置姿勢計測値に基づいて算出した値を使用すればよい。

［第３の実施形態］
本実施形態に係る指標較正装置では、対象物体に装着した複数の指標の較正を、６自由度位置姿勢センサを有する移動可能な撮像装置で対象物体を撮影することにより行う。第１または第２の実施形態では、指標の位置または位置姿勢のみを未知パラメータとして取り扱っていたが、本実施形態では指標の位置又は位置姿勢だけでなく物体に対するカメラの位置姿勢も未知パラメータとして取り扱う。以下、本実施形態に係る指標較正装置及び指標較正方法について説明する。

本実施形態の指標はは、同一直線上にない３個以上の点群から構成される。指標を構成する複数の点の相対位置は既知、つまり各指標において規定される指標座標系における指標を構成する各点の位置は既知であるとする。本実施形態では、前述の指標が物体上に複数配置されている場合に、各指標に規定される指標座標系の、物体を基準とした（つまり物体座標系に対する）位置姿勢を較正情報として求める。

図６は、本実施形態に係る指標較正装置１１００の概略構成を示す図である。図６に示すように、本実施形態に係る指標較正装置１１００は、指標検出部１０２０、位置姿勢算出部１０３０、撮像部１０１０、指示部１０５０、データ管理部１０４０、較正情報算出部１０６０によって構成されており、指標が装着された物体１０００に接続されている。

また、物体１０００には６自由度位置姿勢センサＡ２が装着されており、世界座標系に対する物体の位置姿勢を計測することが可能になっている。６自由度位置姿勢センサＡ２は位置姿勢算出部１０３０に接続されている。また、撮像部１０１０には６自由度位置姿勢センサＡ１が装着されており、撮像部を構成するカメラの世界座標系に対する位置姿勢を計測することが可能になっている。６自由度位置姿勢センサＡ１は位置姿勢算出部１０３０に接続されている。

なお、物体１０００上には、前述の定義に基づく指標が複数配置されている。ここで、物体に配置された指標をＰ^ｋ（ｋ＝１，，，Ｋ_ｏ）で表す。但し、Ｋ_ｏは物体上に配置された較正対象となる指標の数である。また、指標Ｐ^ｋは点ｐ^ｋｉ（（ｋ＝１，，，Ｋ_ｏ）、（ｉ＝１，，，Ｎ_ｋ））により構成される。但し、Ｎ_ｋは、指標Ｐ^ｋを構成する点の総数を表す。

前述したように、物体上に配置された指標は、夫々を構成する点の指標座標系における位置は既知であるが、物体座標系（物体上の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における位置姿勢は未知である。なお、物体上には、物体座標系を規定するための基準指標として、同一直線上にない物体座標系における位置が既知な指標が３点以上配置されている。すべての基準指標は、指標較正用のデータを取得する撮像部１０１０によって他の較正対象のマーカと同一画像内で観測される必要がある。また、指標Ｐ^ｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各点が識別可能であるような指標であれば、何れの形態であってもよい。

撮像部１０１０は、物体上に配置された指標を様々な位置や方向から撮影する。撮影された画像は、指標検出部１０２０に入力される。

指標検出部１０２０は、撮像部１０１０より画像を入力し、入力した画像中に撮影されている指標Ｐ^ｋを構成する各点の画像座標を検出する。

さらに、指標検出部１０２０は、検出された各々の指標Ｐ^ｋｎを構成する点ｐ^ｋｎｉの画像座標ｕ^Ｐｋｎｉとその識別子ｋ_ｎをデータ管理部１０４０へと出力する。ここで、ｎ（ｎ＝１，，，Ｍ）は検出された指標夫々に対するインデックスであり、Ｍは検出された指標の総数を表している。例えば図６の場合は、識別子が１、２，３である四角形形状の指標が撮影される場合を示しており、Ｍ＝３であり、識別子ｋ_１＝１、ｋ_２＝２、ｋ_３＝３とこれらに対応する画像座標ｕ^Ｐｋ１ｉ、ｕ^Ｐｋ２ｉ、ｕ^Ｐｋ３ｉ、（ｉ＝１、２、３、４）が出力される。

位置姿勢算出部１０３０は、カメラに装着された６自由度位置姿勢センサＡ１と、物体に装着された６自由度位置姿勢センサＡ２から得られるカメラと物体の世界座標系における位置姿勢から、物体座標系におけるカメラの位置姿勢を算出する。この物体座標系におけるカメラの位置姿勢を、データ管理部１０４０の要求に従って、データ管理部１０４０へ出力する。

また、位置姿勢算出部１０３０は、物体座標系におけるカメラの位置及び姿勢を、夫々３値ベクトル［ｘ ｙ ｚ］^Ｔ及び［ξ ψ ζ］^Ｔによって内部的に表現している。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。このとき、位置及び姿勢は夫々、６次元ベクトル［ｘ ｙ ｚ ξ ψ ζ］^Ｔで表される。

指示部１０５０は、不図示のオペレータからデータ取得コマンドが入力されたとき時には「データ取得」の指示をデータ管理部１０４０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部１０６０に送信する。

データ管理部１０４０は、指示部１０５０から「データ取得」の指示を受けると、位置姿勢算出部１０３０から物体座標系におけるカメラの位置と姿勢を入力し、指標検出部１０２０から指標の画像座標とその識別子を入力し、［カメラの物体座標系での位置姿勢−指標の画像座標−指標の識別子］の組を一つのデータリストに追加しこれを保持する。ここで、位置姿勢算出部から入力されるカメラの物体座標系での位置姿勢は、指標検出部１０２０から入力される指標の画像座標を検出した画像の撮影時刻と同一時刻のものである。また、データ管理部１０４０は、較正情報算出部１０６０からの要求に従って、生成したデータリストを較正情報算出部１０６０に出力する。

較正情報算出部１０６０は、指示部１０５０から「較正情報算出」の指示を受け取ると、データ管理部１０４０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、物体座標系における指標座標の位置と姿勢）を出力する。

図７は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお同フローチャートに従ったプログラムコードは、本実施形態の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。

ステップＳ６０１０において、指示部１０５０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、指標較正用のデータ取得を行う位置に物体１０００を配置した時に、データ取得コマンドを入力する。指示部１０５０は、データ取得コマンドが入力されている場合には、ステップＳ６０２０へと処理を移行させる。

ステップＳ６０２０において、データ管理部１０４０は、位置姿勢算出部１０３０から、物体座標系におけるカメラの位置及び姿勢ｔ_ＯＣ＝［ｘ_ＯＣ ｙ_ＯＣ ｚ_ＯＣ ξ_ＯＣ ψ_ＯＣ ζ_ＯＣ］^Ｔを入力する。位置姿勢算出部１０３０は常にｔ_ＯＣの算出を行っている。カメラに装着した６自由度位置姿勢センサＡ１の計測値ｓ_Ａ１＝［ｘ_Ａ１Ｃ ｙ_Ａ１Ｃ ｚ_Ａ１Ｃ ξ_Ａ１Ｃ ψ_Ａ１Ｃ ζ_Ａ１Ｃ］^Ｔから得られる世界座標系におけるカメラの位置姿勢を表す変換行列をＭ_ｗｃ（ｓ_Ａ１）、物体に装着した６自由度位置姿勢センサＡ２の計測値ｓ_Ａ２＝［ｘ_Ａ２Ｏ ｙ_Ａ２Ｏ ｚ_Ａ２Ｏ ξ_Ａ２Ｏ ψ_Ａ２Ｏ ζ_Ａ２Ｏ］^Ｔから得られる世界座標系における物体の位置姿勢を表す変換行列をＭ_ｗｏと表すと、物体座標系におけるカメラの位置姿勢を表す変換行列Ｍ_ｏｃは、式（３３）のように表される。

式２８で得られる行列Ｍ_ｏｃから並進成分と姿勢成分を分離することにより、物体座標系における撮像装置の位置と姿勢ｔ_ＯＣ＝［ｘ_ＯＣ ｙ_ＯＣ ｚ_ＯＣ ξ_ＯＣ ψ_ＯＣ ζ_ＯＣ］^Ｔを得ることができる。

ステップＳ６０３０において、データ管理部１０４０は、指標検出部１０２０から、指標検出部１０２０によって検出された指標Ｐ^ｋｎを構成する点の画像座標群ｕ^Ｐｋｎｉとその識別子ｋ_ｎを入力する。指標検出部１０２０は、入力された画像に対して常に指標の検出処理を行っており、本ステップの処理により、撮像装置の位置と姿勢がｔ_ＯＣであるときの指標の画像座標を得ることができる。なお、指標検出部１０２０から入力される情報は、必ずしも全ての指標に関するものである必要はなく、その時点で画像上において検出されている指標に関する情報であればよい。

次に、ステップＳ６０４０において、データ管理部１０４０は、検出された指標Ｐ^ｋｎ全てについて、入力したデータの組をデータリストＤＬにデータＤ_Ｌとして追加する。具体的には、位置姿勢算出部１０３０から入力するｔ_ＯＣをｔ_ＯＣｊ＝［ｘ_ＯＣｊ ｙ_ＯＣｊ ｚ_ＯＣｊ ξ_ＯＣｊ ψ_ＯＣｊ ζ_ＯＣｊ］^Ｔとし、指標検出部１０２０から入力する識別子ｋ_ｎをｋ_ｎｊとし、同じく指標検出部１０２０から入力するｕ^Ｐｋｎｉをｕ_Ｌ ^{Ｐｋｎｊｉ}として、Ｄ_Ｌ＝［ｔ_ＯＣｊ，ｕ_Ｌ ^{Ｐｋｎｊｉ}，ｋ_ｎｊ］の組を、Ｌ番目のデータとしてデータリストＤＬに登録する。ここでｊ（ｊ＝１，，，Ｎ_Ｊ）は、撮影した画像に対するインデックスであり、Ｌ（Ｌ＝１，，，Ｎ_Ｌ）は、データリストＤＬに登録したデータの組夫々に対するインデックスであり、Ｎ_Ｊは、撮影した画像の総数を表し、Ｎ_Ｌは、登録したデータの総組数を表している。

以上の処理によって、データの取得が行われる。

ステップＳ６０５０では、データ管理部１０４０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ６０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも異なる３つ以上の基準指標に関するデータが、データリストＤＬに含まれていること、が挙げられる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。

次にステップＳ６０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ６０７０へと処理を移行し、入力されていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。

較正情報算出部１０６０は、第２の実施形態と同様に求めるべき較正情報、すなわち物体座標系における指標の位置姿勢を６値ベクトルとして扱う。以下では、この未知パラメータを状態ベクトルｓ_ｍ＝［ｘ_ｍ ｙ_ｍ ｚ_ｍ ξ_ｍ ψ_ｍ ζ_ｍ］^Ｔと記述する。また、本実施形態では物体に対するカメラの位置姿勢ｔ_ＯＣｊも未知パラメータとして取り扱う。ｔ_ＯＣｊを状態ベクトルｓ_ｃｊ＝［ｘ_ｏｃｊ ｙ_ｏｃｊ ｚ_ｏｃｊ ξ_ｏｃｊ ψ_ｏｃｊ ζ_ｏｃｊ］^Ｔと記述する。

ステップＳ６０７０において、較正情報算出部１０６０は、夫々の状態ベクトルｓ_ｍ、ｓ_ｃｊに適当な初期値を与える。ｓ_ｃｊの初期値は、センサ出力から得られたｔ_ＯＣｊを用いる。ｓ_ｍの初期値としては、オペレータが指示部１０５０を介しておおよその値を手入力しても良い。カメラ座標系から物体座標系への変換行列をＭ_ＯＣ、物体座標系から物体に装着された指標座標系への変換行列をＭ_ＭＯとすると、カメラ座標系から指標座標への変換行列Ｍ_ＭＣを得る。

ステップＳ４０８０において、較正情報算出部１０６０は、データリストＤＬ中の各データＤ_Ｌ＝［ｔ_ＯＣｊ，ｕ_Ｌ ^{Ｐｋｎｊｉ}，ｋ_ｊ］、（（ｊ＝１，２，，，，Ｎ_Ｊ）（Ｌ＝１，２，，，，Ｎ_Ｌ）および状態ベクトルｓ_ｍ、ｓ_ｃｊ、から、全てのＬに対して、指標Ｐ^ｋｎｊの画像座標の計算値ｕ_ｌＬ ^{Ｐｋｎｊｉ}'＝［ｕ_ｘＬ ^{Ｐｋｎｊｉ}'，ｕ_ｙＬ ^{Ｐｋｎｊｉ}'］を求める。ここで指標の画像座標の計算値とは、物体座標系における位置及び姿勢がｓ_ｍである指標の指標座標系における位置ベクトルｈ_Ｍ ^Ｐｋｎｊが既知な点Ｐ^ｋｎｊの、画像中に見えるべき位置（座標）のデータを指す。ｕ_Ｌ ^{Ｐｋｎｊｉ}'の算出は、物体座標系における指標座標系の位置を表す状態ベクトルｓ＝［ｓ_ｍ ｓ_Ｃｊ］の関数

に基づいて行われる。

具体的には、関数Ｆ_Ｌ（）は、Ｌ番目のデータを取得した時の（すなわち、同データセット中の撮像装置１０１０の位置と姿勢がｔ_ＯＣｊである時の）指標Ｐ^ｋｎｊのカメラ座標における位置ベクトルｃ_Ｌ ^{Ｐｋｎｊｉ}を、ｓ_ｍ，ｓ_ｃｊから求める次式、

及び、ｃ_Ｌ ^{Ｐｋｎｊｉ}から指標Ｐ^ｋｎｊの画像上の座標ｕ_Ｌ ^{Ｐｋｎｊｉ}'を求める次式、

によって構成されている。ここでｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向におけるカメラ１０１０の焦点距離であり、既知の値として予め保持されているものとする。また、

によって定まる変換行列（指標座標系から物体座標系に変換する行列）であり、

はｓ_ｃｊによって定まるモデリング変換行列（物体座標系からカメラ座標系に変換する行列）である。

は次式によって定義される。

ただし、

である。また、

ただし、

である。

ステップＳ６０９０において、較正情報算出部１０６０は、全てのＬに対して、データリストＤＬ中の各データに含まれる指標Ｐ^ｋｎｊの実際の画像座標ｕ_Ｌ ^{Ｐｋｎｊｉ}と、それに対応する画像座標の計算値ｕ_Ｌ ^{Ｐｋｎｊｉ}'との誤差△ｕ_Ｌ ^{Ｐｋｎｊｉ}を次式によって算出する。

ステップＳ６１００において、較正情報算出部１０６０は、全てのＬに対して、状態ベクトルｓ＝［ｓ_ｍ ｓ_ｃｊ］に関する画像ヤコビアン（すなわち、式３０の関数Ｆ_ｊ（）を状態ベクトルｓ_ｍ、ｓ_ｃｊの各要素で偏微分した解を各要素に持つ（２×Ｎ_Ｌ）行×（６×Ｋ_ｏ＋６×Ｋ_ＮＪ）列のヤコビ行列）Ｊ_ｕＬ ^{Ｐｋｎｊｉ} _ｓ（＝∂ｕ_Ｌ ^{Ｐｋｎｊｉ}／∂ｓ）を算出する。ここで、Ｎ_Ｌは検出したすべての指標の総数であり、Ｋ_ｏは、較正対象となる指標の総数である。Ｋ_ＮＪは画像の撮影枚数である。

ステップＳ６１１０において、較正情報算出部１０６０は、以上のステップで算出した、全てのＬに対する誤差△ｕ_Ｌ ^{Ｐｋｎｊｉ}及びヤコビ行列Ｊ_ｕＬ ^{Ｐｋｎｊｉ} _ｓに基づいて、ｓの補正値△ｓを算出する。具体的には、全てのＬに対する誤差△ｕ_Ｌ ^{Ｐｋｎｄｊｉ}を垂直に並べた２Ｎ_Ｌ次元ベクトルの誤差ベクトル

及びヤコビ行列Ｊ_ｕＬ ^{Ｐｋｎｊｉ} _ｓを垂直に並べた（２Ｎ_Ｌ）行×（６×Ｋ_ｏ＋６×Ｋ_ＮＪ）列の行列

を作成し、Φの擬似逆行列Φ^＋を用いて、△ｓを次式より算出する。

ここで、△ｓは（６×Ｋ_ｏ＋６×Ｋ_ＮＪ）次元ベクトルであるから、２Ｎ_Ｌが（６×Ｋ_ｏ＋６×Ｋ_ＮＪ）以上であれば、△ｓを求めることができる。なおΦ^＋は、例えばΦ^＋＝（Φ^ＴΦ）^−１Φ^Ｔによって求めることができるが、他の方法によって求めてもよい。

ステップＳ６１２０において、較正情報算出部１０６０は、ステップＳ６１１０において算出した補正値△ｓを用いて、式４１に従ってｓを補正し、得られた値を新たなｓとする。ここで状態ベクトルｓ＝［ｓ_ｍ ｓ_ｃｊ］は、物体座標系における指標の位置姿勢の状態ベクトルｓ_ｍ、物体座標系におけるカメラ位置姿勢の状態ベクトルｓ_Ｃｊである。

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

ステップＳ６１３０において計算が収束したと判定されると、ステップＳ６１４０において、較正情報算出部１０６０は、得られた状態ベクトルｓのうちｓ_ｍを、較正情報、すなわち、物体座標系における指標座標系の位置及び姿勢として出力する。この時の出力の形態は、ｓ_ｍそのものであっても良いし、ｓ_ｍの位置成分を３値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓ_ｍから生成した座標変換行列Ｍ_ＯＭであっても良い。

最後にステップＳ６１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、指標較正装置１１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。

以上の処理によって、対象物体に対する（すなわち、対象物体座標系における）対象物体に装着した指標の位置または位置と姿勢を、簡便かつ正確に取得することができる。

＜変形例３−１＞
なお、本実施形態において、撮像部１０１０は自由に移動可能な構成であったが、第１及び第２の実施形態における客観視点カメラと同様に、撮像部１０１０を固定位置に設置しても良い。この場合、６自由度位置姿勢センサＡ１は不要であり、位置姿勢算出部１０３０は、既知の値として予め保持されている世界座標系における撮像部１０１０の位置姿勢と、６自由度位置姿勢センサＡ１から得られる世界座標系における物体の位置姿勢に基づいて、物体座標系における撮像部１０１０の位置姿勢を算出する。第１の実施形態や第２の実施形態においては、客観視点カメラの位置姿勢は既知の値として正確に得られている必要があったが、本実施形態では、位置姿勢算出部１０３０が出力する物体座標系におけるカメラの位置姿勢に補正処理が施されるため、既知の値としては大まかな位置姿勢のみが入力されていればよいという利点がある。

＜変形例３−２＞
なお、本実施形態において、カメラと物体との相対的な位置関係を求めるために６自由度位置姿勢センサを用いるが，物体座標系におけるカメラの位置及び姿勢が計測可能であればいずれのセンサでもよい。

例えば、磁気センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ Ｔｅｃｈ．社のＦｌｏｃｋ ｏｆ Ｂｉｒｄｓ）、光学式センサ（加ＮＤＩ社のＯｐｔｏｔｒａｋや英ＶＩＣＯＮ社のＶＩＣＯＮ）、超音波センサといった６自由度位置姿勢センサをカメラと物体に取り付けて、画像撮影時のカメラの物体座標系に対する位置姿勢をセンサ計測値から得てもよい。また、実施例１及び２と同様に，物体に装着されているカメラと世界座標系における位置が既知である基準指標によって構成される６自由度位置姿勢センサの計測値から，物体とそれを撮影するカメラの相対的な位置姿勢を求めてもよい。また、物体とそれを撮影するカメラに前述の６自由度位置姿勢センサや３自由度姿勢センサを取り付け、撮影画像とセンサの計測値の両方に基づいたハイブリッドな手法によって物体とそれを撮影するカメラの相対的な位置姿勢を求めてもよい。

＜変形例３−３＞
なお、本実施形態における指標較正装置によって物体に装着された指標の物体に対する位置と姿勢を較正した後、カメラに装着した６自由度位置姿勢センサＡ１のカメラに対する位置と姿勢を求めることも可能である．ここで位置と姿勢は、位置を表す３次元の並進ベクトルｔと、姿勢を表す３×３回転行列Ｒを使って、式４２のような４×４行列Ｍで表す。

第３の実施形態において説明したように、物体に装着された指標の物体に対する位置と姿勢を求める際に、較正に利用した画像を撮影した時点でのカメラの物体に対する位置と姿勢Ｍ_ＯＣも同時に求まる。また、カメラに装着した６自由度位置姿勢センサと物体に装着した６自由度位置姿勢センサの相対的な位置と姿勢Ｍ_Ａ２Ａ１はそれぞれのセンサ出力値から計算できる。物体に装着した６自由度位置姿勢センサの物体に対する位置と姿勢Ｍ_ＯＡ２が何らかの方法で求まっている場合に、式４３が成り立つ。

ここで既知の情報であるＭ_ＯＡ２とＭ_Ａ２Ａ１を次のようにまとめる。

式４４を用いて式４３を式４５のように表す。

Ｎ_Ｊ枚の画像を撮影すると、撮影画像ごとに存在するＭ_ＯＣ，Ｍ_ＯＡ１と、カメラに装着した６自由度位置姿勢センサのカメラに対する位置と姿勢Ｍ_Ａ１Ｃを用いて式４６のように表せる。

ここで、

とすると、式４６は式４９のように表せる。

撮影した画像が複数ある場合には、Ｍ_Ａ１Ｃを求めるためにＢの一般化逆行列を用いて式５０のように計算することが可能である。

また、ＡとＢを用いてＭ_Ａ１Ｃを求めるために他の手法で計算してもよい。

算出した位置と姿勢を表す４×４行列Ｍ_Ａ１Ｃより、式４２のように姿勢を表す３×３回転行列Ｒと位置を表す３次元並進ベクトルｔを分解し求める。以上のようにしてカメラに装着した６自由度位置姿勢センサのカメラに対する位置と姿勢を求めることが可能である。

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

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

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

第１の実施形態に係る指標較正装置の概略構成を示す図である。 第１の実施形態に係る指標較正方法の概略処理を示すフローチャートである。 第２の実施形態に係る指標較正装置の概略構成を示す図である。 第２の実施形態に係る指標較正方法の概略処理を示すフローチャートである。 第１の実施形態の＜変形例１―３＞および第２の実施形態の＜変形例２−５＞に係る指標較正装置の概略構成を示す図である。 第３の実施形態に係る指標構成装置の概略構成を示す図である。 第３の実施形態に係る指標較正方法の概略処理を示すフローチャートである。

Claims (17)

1. 物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理方法であって、
   前記物体の位置姿勢情報を取得する物体位置姿勢取得工程と、
   前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力工程と、
   前記第１の画像から前記指標を検出する検出工程と、
   前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする情報処理方法。
2. 前記物体は主観視点撮影部を有し、
   前記物体位置姿勢取得工程は、
   前記主観視点撮影部で撮影された第２の画像を取得し、
   前記第２の画像から、前記物体以外に存在する指標を抽出し、
   前記第２の画像から抽出された指標の画像座標に関する情報と、予め保持されている指標の位置情報とに基づき、前記主観視点撮影部の位置姿勢を取得することを特徴とする請求項１記載の情報処理方法。
3. 複数の指標の相対位置が既知であることを特徴とする請求項１記載の情報処理方法。
4. 指標座標系における前記指標の位置が既知であり、
   前記指標位置姿勢算出工程は、前記物体の位置姿勢情報の座標系と前記指標座標系とを変換する変換パラメータ算出することを特徴とする請求項１記載の情報処理方法。
5. さらに、前記物体と前記客観視点撮影部との相対的な位置姿勢を未知パラメータとすることを特徴とする請求項１記載の情報処理方法。
6. さらに、前記物体に対する前記客観視点撮像部の位置姿勢を算出することを特徴とする請求項５記載の情報処理方法。
7. 前記客観視点撮像部は、前記情景中に固定されていることを特徴とする請求項１に記載の指標較正装置。
8. 前記指標位置姿勢算出工程は、
   前記物体に対する前記指標の位置または位置姿勢の推定値に基づいて、前記画像内における前記指標の画像座標に関する情報を推定する推定工程と、
   前記検出手段が検出した指標の画像座標に関する情報と、前記推定手段が推定した指標の画像座標に関する情報との間の誤差に基づき、前記推定値を補正する補正工程とを有することを特徴とする請求項１に記載の情報処理装置。
9. 物体に装着した指標の前記物体を基準とした位置または位置姿勢を、当該指標の較正情報として算出する情報処理方法であって、
   カメラによって前記物体を撮影することにより得られた画像を入力する画像入力工程と、
   前記指標の画像座標に関する情報を前記画像から検出する検出工程と、
   前記物体と前記カメラとの間の相対的な位置姿勢を算出する物体位置姿勢算出工程と、
   前記物体位置姿勢算出工程で算出された前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記検出工程で検出された指標の画像座標に関する情報を用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする情報処理方法。
10. 前記カメラは情景中に固定されており、
    前記物体位置姿勢算出工程は、前記物体の位置姿勢を計測する計測工程をさらに有し、前記カメラの配置情報と前記計測工程で計測した前記物体の位置姿勢計測値とに基づいて、前記物体と前記カメラとの間の相対的な位置姿勢を算出することを特徴とする請求項９に記載の情報処理方法。
11. 前記物体位置姿勢算出工程は、前記物体の位置姿勢を計測する第一の計測工程と、前記カメラの位置姿勢を計測する第二の計測工程をさらに有し、前記第一の計測工程で計測した前記物体の位置姿勢計測値と、前記第二の計測工程で計測した前記カメラの位置姿勢計測値とに基づいて、前記物体と前記カメラとの間の相対的な位置姿勢を算出することを特徴とする請求項９に記載の情報処理方法。
12. 前記指標位置姿勢算出工程は、前記物体位置姿勢算出工程で算出した前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記物体を基準とした前記指標の位置または位置姿勢の推定値に基づいて、前記画像内における前記指標の画像座標に関する情報を推定する推定工程と、
    前記検出工程が検出した前記指標の画像座標に関する情報と、前記推定工程が推定した前記指標の画像座標に関する情報との間の誤差を軽減させるように、前記物体を基準とした前記指標の位置または位置姿勢の推定値を補正する補正工程とをさらに有することを特徴とする請求項７に記載の情報処理方法。
13. 前記指標位置姿勢算出工程は、前記物体位置姿勢算出工程で算出した前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記物体を基準とした前記指標の位置または位置姿勢の推定値に基づいて、前記画像内における前記指標の画像座標に関する情報を推定する推定工程と、
    前記検出工程が検出した前記指標の画像座標に関する情報と、前記推定工程が推定した前記指標の画像座標に関する情報との間の誤差を軽減させるように、前記物体を基準とした前記指標の位置または位置姿勢の推定値と、前記物体と前記カメラとの間の相対的な位置姿勢の算出値の夫々を補正する補正工程とをさらに有することを特徴とする請求項７記載の情報処理方法。
14. 前記指標の画像座標に関する情報は、前記指標によって定まる１点または複数点の画像座標であることを特徴とする請求項７記載の情報処理方法。
15. 請求項１乃至１４のいずれかに記載の情報処理方法をコンピュータにて実現するための情報処理方法。
16. 物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理装置であって、
    前記物体の位置姿勢情報を取得する物体位置姿勢取得手段と、
    前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力手段と、
    前記第１の画像から前記指標を検出する検出手段と、
    前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出手段とを有することを特徴とする情報処理装置。
17. 物体に装着した指標の前記物体を基準とした位置または位置姿勢を、当該指標の較正情報として算出する情報処理装置であって、
    カメラによって前記物体を撮影する撮像手段と、
    前記撮像工程において撮影される画像を入力する画像入力手段と、
    前記指標の画像座標に関する情報を前記画像から検出する検出手段と、
    前記物体と前記カメラとの間の相対的な位置姿勢を算出する物体位置姿勢算出手段と、
    前記物体位置姿勢算出手段で算出された前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記検出工程で検出された指標の画像座標に関する情報を用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出手段とを有することを特徴とする情報処理装置。
    

Images