(57)【要約】【課題】  物体に装着した指標の、対象物体に対する位置または位置及び姿勢を、簡便かつ正確に取得する。【解決手段】  物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理方法であって、前記物体の位置姿勢情報を取得する物体位置姿勢取得工程と、前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力工程と、前記第１の画像から前記指標を検出する検出工程と、前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする。【選択図】  図１
2005353036.tif 000002
【特許請求の範囲】
【請求項１】
物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理方法であって、
前記物体の位置姿勢情報を取得する物体位置姿勢取得工程と、
前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力工程と、
前記第１の画像から前記指標を検出する検出工程と、
前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする情報処理方法。
【請求項２】
前記物体は主観視点撮影部を有し、
前記物体位置姿勢取得工程は、
前記主観視点撮影部で撮影された第２の画像を取得し、
前記第２の画像から、前記物体以外に存在する指標を抽出し、
前記第２の画像から抽出された指標の画像座標に関する情報と、予め保持されている指標の位置情報とに基づき、前記主観視点撮影部の位置姿勢を取得することを特徴とする請求項１記載の情報処理方法。
【請求項３】
複数の指標の相対位置が既知であることを特徴とする請求項１記載の情報処理方法。
【請求項４】
指標座標系における前記指標の位置が既知であり、
前記指標位置姿勢算出工程は、前記物体の位置姿勢情報の座標系と前記指標座標系とを変換する変換パラメータ算出することを特徴とする請求項１記載の情報処理方法。
【請求項５】
さらに、前記物体と前記客観視点撮影部との相対的な位置姿勢を未知パラメータとすることを特徴とする請求項１記載の情報処理方法。
【請求項６】
さらに、前記物体に対する前記客観視点撮像部の位置姿勢を算出することを特徴とする請求項５記載の情報処理方法。
【請求項７】
前記客観視点撮像部は、前記情景中に固定されていることを特徴とする請求項１に記載の指標較正装置。
【請求項８】
前記指標位置姿勢算出工程は、
前記物体に対する前記指標の位置または位置姿勢の推定値に基づいて、前記画像内における前記指標の画像座標に関する情報を推定する推定工程と、
前記検出手段が検出した指標の画像座標に関する情報と、前記推定手段が推定した指標の画像座標に関する情報との間の誤差に基づき、前記推定値を補正する補正工程とを有することを特徴とする請求項１に記載の情報処理装置。
【請求項９】
物体に装着した指標の前記物体を基準とした位置または位置姿勢を、当該指標の較正情報として算出する情報処理方法であって、
カメラによって前記物体を撮影することにより得られた画像を入力する画像入力工程と、
前記指標の画像座標に関する情報を前記画像から検出する検出工程と、
前記物体と前記カメラとの間の相対的な位置姿勢を算出する物体位置姿勢算出工程と、
前記物体位置姿勢算出工程で算出された前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記検出工程で検出された指標の画像座標に関する情報を用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする情報処理方法。
【請求項１０】
前記カメラは情景中に固定されており、
前記物体位置姿勢算出工程は、前記物体の位置姿勢を計測する計測工程をさらに有し、前記カメラの配置情報と前記計測工程で計測した前記物体の位置姿勢計測値とに基づいて、前記物体と前記カメラとの間の相対的な位置姿勢を算出することを特徴とする請求項９に記載の情報処理方法。
【請求項１１】
前記物体位置姿勢算出工程は、前記物体の位置姿勢を計測する第一の計測工程と、前記カメラの位置姿勢を計測する第二の計測工程をさらに有し、前記第一の計測工程で計測した前記物体の位置姿勢計測値と、前記第二の計測工程で計測した前記カメラの位置姿勢計測値とに基づいて、前記物体と前記カメラとの間の相対的な位置姿勢を算出することを特徴とする請求項９に記載の情報処理方法。
【請求項１２】
前記指標位置姿勢算出工程は、前記物体位置姿勢算出工程で算出した前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記物体を基準とした前記指標の位置または位置姿勢の推定値に基づいて、前記画像内における前記指標の画像座標に関する情報を推定する推定工程と、
前記検出工程が検出した前記指標の画像座標に関する情報と、前記推定工程が推定した前記指標の画像座標に関する情報との間の誤差を軽減させるように、前記物体を基準とした前記指標の位置または位置姿勢の推定値を補正する補正工程とをさらに有することを特徴とする請求項７に記載の情報処理方法。
【請求項１３】
前記指標位置姿勢算出工程は、前記物体位置姿勢算出工程で算出した前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記物体を基準とした前記指標の位置または位置姿勢の推定値に基づいて、前記画像内における前記指標の画像座標に関する情報を推定する推定工程と、
前記検出工程が検出した前記指標の画像座標に関する情報と、前記推定工程が推定した前記指標の画像座標に関する情報との間の誤差を軽減させるように、前記物体を基準とした前記指標の位置または位置姿勢の推定値と、前記物体と前記カメラとの間の相対的な位置姿勢の算出値の夫々を補正する補正工程とをさらに有することを特徴とする請求項７記載の情報処理方法。
【請求項１４】
前記指標の画像座標に関する情報は、前記指標によって定まる１点または複数点の画像座標であることを特徴とする請求項７記載の情報処理方法。
【請求項１５】
請求項１乃至１４のいずれかに記載の情報処理方法をコンピュータにて実現するための情報処理方法。
【請求項１６】
物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理装置であって、
前記物体の位置姿勢情報を取得する物体位置姿勢取得手段と、
前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力手段と、
前記第１の画像から前記指標を検出する検出手段と、
前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出手段とを有することを特徴とする情報処理装置。
【請求項１７】
物体に装着した指標の前記物体を基準とした位置または位置姿勢を、当該指標の較正情報として算出する情報処理装置であって、
カメラによって前記物体を撮影する撮像手段と、
前記撮像工程において撮影される画像を入力する画像入力手段と、
前記指標の画像座標に関する情報を前記画像から検出する検出手段と、
前記物体と前記カメラとの間の相対的な位置姿勢を算出する物体位置姿勢算出手段と、
前記物体位置姿勢算出手段で算出された前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記検出工程で検出された指標の画像座標に関する情報を用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出手段とを有することを特徴とする情報処理装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体上に設置した指標の、物体座標系における位置または位置姿勢を求めるものに関する。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない結合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）の画像を重畳描画してそれを表示するビデオシースルー方式によって実現される。
【０００３】
このような画像表示装置の応用としては、患者の体表面に体内の様子を重畳表示する手術支援や、現実空間に浮遊する仮想の敵と戦う複合現実感ゲームなど、今までのバーチャルリアリティとは異なった新たな分野が期待されている。
【０００４】
これらの応用に対して共通に要求されるのは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置及び姿勢の関係（以下ではこれを、撮像装置に対する対象物体の位置及び姿勢と称する）を求める問題に帰結される。
【０００５】
この問題を解決する方法として、対象物体上に、対象物体の座標系における配置が既知であるような複数の指標を設置あるいは設定し、既知の情報であるその指標の物体座標系における座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、撮像装置に対する対象物体の位置及び姿勢を求めることが一般的に行われている。
【０００６】
物体座標系における指標の座標とその投影像の画像座標の組から撮像装置に対する対象物体の位置及び姿勢を算出する方法は、写真測量等の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。
【０００７】
また、磁気センサのような６自由度位置姿勢センサを撮像装置と対象物体の双方に取り付け、撮像装置が撮像した画像内における指標の投影像の座標に加え、センサによって計測された（センサが基準としている基準座標系における）撮像装置と対象物体の夫々の位置及び姿勢の計測値もさらに利用して、撮像装置に対する対象物体の位置及び姿勢を求めるハイブリッドな位置姿勢計測方法も提案されている（例えば、特許文献１では、画像上で検出された指標の投影像の情報を用いて、センサ計測値の誤差が補正される）。
【０００８】
また、本出願人は、特願２００３−３２３１０１において、ある基準座標系における対象物体の位置及び姿勢を計測したいという目的において、対象物体に指標を設置し、基準座標系に固定したカメラによってこれを撮影することで、固定カメラに対する対象物体の位置及び姿勢を求め、これによって、基準座標系における対象物体の位置及び姿勢を得るという方法を提案している。
【非特許文献１】Ｒ．Ｍ．Ｈａｒａｌｉｃｋ，Ｃ．Ｌｅｅ，Ｋ．Ｏｔｔｅｎｂｅｒｇ，ａｎｄ  Ｍ．Ｎｏｌｌｅ：Ｒｅｖｉｅｗ  ａｎｄ  ａｎａｌｙｓｉｓ  ｏｆ  ｓｏｌｕｔｉｏｎｓ  ｏｆ  ｔｈｅ  ｔｈｒｅｅ  ｐｏｉｎｔ  ｐｅｒｓｐｅｃｔｉｖｅ  ｐｏｓｅ  ｅｓｔｉｍａｔｉｏｎ  ｐｒｏｂｌｅｍ，Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｊｏｕｒｎａｌ  ｏｆ  Ｃｏｍｐｕｔｅｒ  Ｖｉｓｉｏｎ，ｖｏｌ．１３，ｎｏ．３，ｐｐ．３３１−３５６，１９９４．
【非特許文献２】Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ  ｐａｒａｍｅｔｅｒｉｚｅｄ  ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ  ｍｏｄｅｌｓ  ｔｏ  ｉｍａｇｅｓ，ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１．
【非特許文献３】内山，山本，田村：“複合現実感のためのハイブリッド位置合わせ手法−６自由度センサとビジョン手法の併用−”，日本バーチャルリアリティ学会論文誌，ｖｏｌ．８，ｎｏ．１，ｐｐ．１１９−１２５，２００３．
【非特許文献４】藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９，ｎｏ．５７４，ｐｐ．１−８，１９９９．
【非特許文献５】Ａ．Ｉ．Ｃｏｍｐｏｒｔ，Ｅ．Ｍａｒｃｈａｎｄ，Ｆ．Ｃｈａｕｍｅｔｔｅ，Ａ  ｒｅａｌ−ｔｉｍｅ  ｔｒａｃｋｅｒ  ｆｏｒ  ｍａｒｋｅｒｌｅｓｓ  ａｕｇｍｅｎｔｅｄ  ｒｅａｌｉｔｙ，Ｐｒｏｃ．Ｉｎｔ'ｌ  Ｓｙｍｐ．ｏｎ  Ｍｉｘｅｄ  ａｎｄ  Ａｕｇｍｅｎｔｅｄ  Ｒｅａｌｉｔｙ  ２００４，ｐｐ．３６−４５，２００４．
【発明の開示】
【発明が解決しようとする課題】
【０００９】
ところで、撮像装置が撮影した画像内における指標の投影像の画像座標を検出することで対象物体の位置及び姿勢を算出する上記の方法においては、対象物体に設定した複数の指標と、対象物体との相対的な位置関係は、既知でなければならない。個々の指標の相対的な位置関係が未知である場合、例えば、着色された球状の指標や円形状の指標を用いてその画像上における指標の投影像の重心位置を特徴として利用する場合のように、特徴が一つの点で記述されるような指標が対象物体上に個々に設置されている場合には、対象物体座標系における個々の指標の３次元位置が予め計測されていなくてはならない。一方、指標同士の相対的な位置関係が既知である場合、例えば、上記と同様な指標を用いる場合であっても、それらの指標が予めベースとなる治具に装着されており、この治具の座標系（このように指標の相対的な位置を記述するための座標系を以下では指標座標系と呼ぶ）における各指標の位置が既に計測されており、この治具が対象物体に装着されているような場合には、対象物体座標系におけるこの治具の位置と姿勢が既知であれば良い（なお、特徴が複数の点で記述されるような指標、例えば、形状の既知な四角形や三角形の指標を用いてその画像上における指標の投影像の各頂点を特徴として利用する場合は、この指標自体が複数の指標の集まりであると解釈して、「指標同士の相対的な位置関係が既知である場合」の一種として考えることにする）。しかしながら、これらを正確に較正する方法は一般的に知られておらず、従来までは手動計測によって得られる不正確な位置または位置及び姿勢を既知の値として利用する以外に方策がなかった。その結果、上記の撮像装置が撮影した画像内における指標の投影像の画像座標を検出することで対象物体の位置及び姿勢を算出する方法においては、対象物体の位置及び姿勢を実用上低い精度でしか計測できないという改善の余地があった。
【００１０】
本発明は以上の問題を鑑みてなされたものであり、物体に装着した指標の、対象物体に対する（すなわち、対象物体座標系における）位置または位置及び姿勢を、簡便かつ正確に取得することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、請求項１記載の発明は、物体に装着した指標の前記物体に対する位置または位置姿勢を算出する情報処理方法であって、前記物体の位置姿勢情報を取得する物体位置姿勢取得工程と、前記物体を俯瞰する視点から撮影する客観視点撮像部から第１の画像を入力する第１画像入力工程と、前記第１の画像から前記指標を検出する検出工程と、前記物体の位置姿勢情報と前記検出された指標の画像座標に関する情報とを用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする。
【００１２】
また、請求項９記載の発明は、物体に装着した指標の前記物体を基準とした位置または位置姿勢を、当該指標の較正情報として算出する情報処理方法であって、カメラによって前記物体を撮影することにより得られた画像を入力する画像入力工程と、前記指標の画像座標に関する情報を前記画像から検出する検出工程と、前記物体と前記カメラとの間の相対的な位置姿勢を算出する物体位置姿勢算出工程と、前記物体位置姿勢算出工程で算出された前記物体と前記カメラとの間の相対的な位置姿勢の算出値と、前記検出工程で検出された指標の画像座標に関する情報を用いて、前記物体を基準とした前記指標の位置または位置姿勢を算出する指標位置姿勢算出工程とを有することを特徴とする。
【発明の効果】
【００１３】
本発明によれば物体に対する指標の位置または位置姿勢を、簡便かつ正確に取得することができる。
【発明を実施するための最良の形態】
【００１４】
以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
【００１５】
［第１の実施形態］
本実施形態に係る指標較正装置は、対象物体に装着した指標の較正を行うためのものであり、対象物体に装着した６自由度位置姿勢センサと、固定位置に設置した客観視点カメラを利用して、対象物体に対する（すなわち、対象物体座標系における）夫々の指標の位置を、指標ごとに算出する。以下、本実施形態に係る指標較正装置及び指標較正方法について説明する。なお、本実施形態では、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」によって、６自由度位置姿勢センサが構成されているものとする。また、その６自由度位置姿勢センサが取り付けられている対象物体は、「撮像装置」自体であるとする。すなわち、本実施形態に係る指標較正装置は、撮像装置に装着した（他の撮像装置によって観測されるための）指標の較正を行うためのものであり、撮像装置の座標系（以下、これを主観視点カメラ座標系と呼ぶ）における夫々の指標の位置を、指標ごとに算出するものといえる。
【００１６】
図１は、本実施形態に係る指標較正装置１００の概略構成を示す図である。図１に示したように、本実施形態に係る指標較正装置１００は、主観視点指標検出部１１０、位置姿勢算出部１２０、客観視点カメラ１４０、客観視点指標検出部１５０、指示部１６０、データ管理部１７０、較正情報算出部１８０によって構成されており、較正対象である指標が装着された撮像装置１３０に接続されている。
【００１７】
なお、撮像装置１３０上には、較正される対象である指標Ｐｋ（ｋ＝１，，，Ｋ２）（以下、これを客観視点指標と呼ぶ）が１つ以上設定されているものとする。主観視点カメラ座標系（＝対象物体座標系：撮像装置１３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における客観視点指標の位置は未知であり、この未知である客観視点指標の位置が、本実施形態に係る指標較正装置が較正する情報となる。
【００１８】
現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標（以下、主観視点指標（基準指標））として、世界座標系（現実空間の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における位置が既知である複数個の主観視点指標Ｑｋ（ｋ＝１，，，Ｋ１）が配置されている。主観視点指標Ｑｋは、指標較正用のデータを取得する時の撮像装置１３０によって、少なくとも３個以上の指標が観測されるように設置されていることが望ましい。図１の例は、４個の主観視点指標Ｑ１，Ｑ２，Ｑ３，Ｑ４が配置されており、そのうちの３個Ｑ１，Ｑ３，Ｑ４が撮像装置１３０の視野内に含まれている状況を示している。
【００１９】
主観視点指標Ｑｋは、例えば、それぞれが異なる色を有する円形状のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する四角形領域によって形成されるような四角形指標を用いることも可能である。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが識別可能であるような指標であれば、何れの形態であってもよい。
【００２０】
撮像装置１３０が出力する画像（以下、これを主観視点画像と呼ぶ）は、主観視点指標検出部１１０に入力される。
【００２１】
主観視点指標検出部１１０は、撮像装置１３０より主観視点画像を入力し、入力した画像中に撮影されている主観視点指標Ｑｋの画像座標を検出する。例えば、主観視点指標Ｑｋの各々が異なる色を有するマーカによって構成されている場合には、主観視点画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、主観視点指標Ｑｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを主観視点画像上に施すことにより、指標の位置を検出する。基準指標として四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の方向と識別子を取得する。なお、このようにして検出される四角形指標は、本明細では、４つの頂点の個々によって形成される４つの指標であると考える。
【００２２】
さらに、主観視点指標検出部１１０は、検出された各々の主観視点指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。例えば図１の場合には、Ｎ＝３であり、識別子ｋ１＝１，ｋ２＝３，ｋ３＝４とこれらに対応する画像座標ｕＱｋ１，ｕＱｋ２，ｕＱｋ３が出力される。本実施形態における主観視点指標検出部１１０は、撮像装置１３０からの画像の入力をトリガとして上記の指標検出処理を逐次実行しているが、位置姿勢算出部１２０からの要求に応じて（その時点で入力している主観視点画像を利用して）処理を実行する構成としても良い。
【００２３】
位置姿勢算出部１２０は、検出された各々の主観視点指標Ｑｋｎの画像座標ｕＱｋｎと、既知な情報として予め保持している指標の世界座標ｘＷＱｋｎとの対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。主観視点指標の世界座標と画像座標の組から撮像装置の位置及び姿勢を算出する方法は、写真測量等の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。例えば、主観視点指標が同一平面上に配置されている場合には、４点以上の指標を検出することで、２次元ホモグラフィの計算に基づいて撮像装置の位置及び姿勢を得ることができる。また、同一平面上にない６点以上の指標を用いて撮像装置の位置と姿勢を用いる方法も良く知られている。また、撮像装置の位置と姿勢の推定値から算出される指標の画像座標の理論値と指標の画像座標の実測値との誤差を、画像ヤコビアンを用いたガウス・ニュートン法のような繰り返し計算によって最小化することにより、位置と姿勢の推定値を最適化する方法などを用いることもできる。算出した撮像装置１３０の世界座標系における位置及び姿勢は、データ管理部１７０の要求に従って、データ管理部１７０へと出力される。なお、本実施形態における位置姿勢算出部１２０は、主観視点指標検出部１１０からのデータの入力をトリガとして上記の位置及び姿勢の算出処理を逐次実行しているが、データ管理部１７０からの要求に応じて（その時点での入力データを利用して）処理を実行する構成としても良い。以下では、撮像装置１３０の位置及び姿勢は、それぞれ３値ベクトル［ｘ  ｙ  ｚ］Ｔ及び［ξ  ψ  ζ］Ｔによって保持されているものとする。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。このとき、位置及び姿勢は、６次元ベクトル［ｘ  ｙ  ｚ  ξ  ψ  ζ］Ｔで表される。
【００２４】
客観視点カメラ１４０は、指標較正用のデータ取得を行う時の撮像装置１３０を撮像可能な位置に固定して配置されている。世界座標系における客観視点カメラ１４０の位置及び姿勢は、既知の値として較正情報算出部１８０に予め保持されているものとする。
【００２５】
客観視点指標検出部１５０は、客観視点カメラ１４０が撮影した画像（客観視点画像）を入力し、主観視点指標検出部１１０と同様な処理によって画像中に撮影されている客観視点指標Ｐｋの画像座標を検出し、データ管理部１７０の要求に従って検出された客観視点指標Ｐｋｍの画像座標ｕＰｋｍとその識別子ｋｍをデータ管理部１７０へと出力する。ここで、ｍ（ｍ＝１，，，Ｍ）は検出された指標夫々に付けられたインデックスであり、Ｍは検出された指標の総数を表している。例えば図１の場合には、Ｍ＝２であり、識別子ｋ１＝１，ｋ２＝２とこれらに対応する画像座標ｕＰｋ１，ｕＰｋ２が出力される。本実施形態における客観視点指標検出部１５０は、客観視点画像の入力をトリガとして上記の指標検出処理を逐次実行しているが、データ管理部１７０からの要求に応じて（その時点で入力している客観視点画像を利用して）処理を実行する構成としても良い。
【００２６】
指示部１６０は、不図示のオペレータからデータ取得コマンドが入力された時には「データ取得」の指示をデータ管理部１７０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部１８０に送信する。指示部１６０へのコマンド入力は、例えばキーボードを用いて、特定のコマンドを割り当てたキーを押すことによって行うことができる。また、コマンドの入力は、ディスプレイ上に表示されたＧＵＩで行うなどの、いずれの方法で行ってもよい。
【００２７】
データ管理部１７０は、指示部１６０から「データ取得」の指示を受けると、位置姿勢算出部１２０から撮像装置１３０の世界座標系における位置及び姿勢を入力し、客観視点指標検出部１５０から客観視点指標の画像座標とその識別子を入力し、［撮像装置１３０の位置及び姿勢−客観視点指標の画像座標］の組を、客観視点指標の識別子毎に用意したデータリストに追加しこれを保持する。ここで、位置姿勢算出部１２０から入力される撮像装置１３０の位置及び姿勢は、客観視点指標検出部１５０から入力される客観視点指標の画像座標を検出した画像の撮影時刻と、同一時刻のものである。また、データ管理部１７０は、較正情報算出部１８０からの要求にしたがって、生成した客観視点指標の識別子毎のデータリストを較正情報算出部１８０に出力する。
【００２８】
較正情報算出部１８０は、指示部１６０から「較正情報算出」の指示を受けると、データ管理部１７０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、各客観視点指標の主観視点カメラ座標系における位置）を出力する。
【００２９】
図２は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお、同フローチャートに従ったプログラムコードは、本実施形態の装置内の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。
【００３０】
ステップＳ２０１０において、指示部１６０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、指標較正用のデータ取得を行う位置に撮像装置１３０を配置した時に、データ取得コマンドを入力する。指示部１６０は、データ取得コマンドが入力されている場合には、ステップＳ２０２０へと処理を移行させる。
【００３１】
ステップＳ２０２０において、データ管理部１７０は、位置姿勢算出部１２０から、撮像装置１３０の世界座標系における位置及び姿勢ｔ＝［ｘ  ｙ  ｚ  ξ  ψ  ζ］Ｔを入力する。
【００３２】
ステップＳ２０３０において、データ管理部１７０は、客観視点指標検出部１５０から、客観視点指標検出部１５０によって検出された客観視点指標Ｐｋｍの画像座標ｕＰｋｍとその識別子ｋｍを入力する。客観視点指標検出部１５０は、入力された客観視点画像に対して常に指標の検出処理を行っており、以上のステップＳ２０２０，Ｓ２０３０の処理により、撮像装置の位置と姿勢がｔであるときの客観視点指標の画像座標を得ることができる。なお、客観視点指標検出部１５０から入力される情報は、必ずしも全ての客観視点指標に関するものである必要はなく、その時点で客観視点画像上において検出されている指標に関する情報であればよい。
【００３３】
次に、ステップＳ２０４０において、データ管理部１７０は、検出された客観視点指標Ｐｋｍ毎に、入力したデータの組をデータリストＬＰｋに追加する。具体的には、位置姿勢算出部１２０から入力するｔをｔｉ＝［ｘｉ  ｙｉ  ｚｉ  ξｉ  ψｉ  ζｉ］Ｔとし、客観視点指標検出部１５０から入力するｕＰｋｍをｕｉＰｋとして、［ｔｉ，ｕｉＰｋ］の組を、客観視点指標検出部１５０から入力する識別子ｋｍを参照することで振り分けながら、Ｐｋに関するｉ番目のデータとして客観視点指標Ｐｋに関するデータリストＬＰｋに登録する。ここでｉ（ｉ＝１，，，ＩＰｋ）は、データリストＬＰｋに登録したデータの組夫々に対するインデックスであり、ＩＰｋは、客観視点指標Ｐｋに関して登録したデータの総組数を表している。
【００３４】
以上の処理によって、データの取得が行われる。
【００３５】
ステップＳ２０５０では、データ管理部１７０によって、それまでに取得された、全ての客観視点指標に関するデータリスト、もしくは、少なくとも１つの客観視点指標に関するデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。少なくとも１つのデータリスト、もしくは、全てのデータリストが条件を満たしていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。一方、全てのデータリスト、もしくは、少なくとも１つのデータリストが較正情報算出の条件を満たしている場合には、ステップＳ２０６０へと処理を移行させる。ある客観視点指標Ｐｋに関するデータリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、データリストＬＰｋが異なるデータ［ｔｉ，ｕｉＰｋ］を２組以上得ていること、が挙げられる。後述するように、１組のデータから２つの方程式が得られるため（式３参照）、２組以上のデータを得ていれば、４つの方程式から３パラメータである客観視点指標の主観視点カメラ座標系における位置を決定することができる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。
【００３６】
次にステップＳ２０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ２０７０へと処理を移行し、入力されていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。
【００３７】
較正情報算出部１８０は、求めるべき較正情報、すなわち客観視点指標の主観視点カメラ座標系における位置を３値ベクトル［ｘ  ｙ  ｚ］Ｔとして扱う。以下では、この未知パラメータを状態ベクトルｓＰｋ＝［ｘＰｋ  ｙＰｋ  ｚＰｋ］Ｔと記述する。以下、ある客観視点指標Ｐｋに対する処理を想定して説明を続けるが、各処理は較正情報算出の条件を満たしたデータリストに対応する全ての客観視点指標に対して共通に行われるものとする。
【００３８】
ステップＳ２０７０において、較正情報算出部１８０は、状態ベクトルｓＰｋに適当な初期値（例えば［０  ０  ０］Ｔ）を与える。
【００３９】
ステップＳ２０８０において、較正情報算出部１８０は、データリストＬＰｋ中の各データ［ｔｉ，ｕｉＰｋ］（ｉ＝１，２，，，，ＩＰｋ）および状態ベクトルｓＰｋから、全てのｉに対して、客観視点指標Ｐｋの客観視点画像座標の理論値ｕｉＰｋ'＝［ｕｘｉＰｋ'，ｕｙｉＰｋ'］を算出する。ここで客観視点指標の客観視点画像座標の理論値とは、主観視点カメラ座標系における位置が与えられている客観視点指標Ｐｋの、客観視点画像中に見えるべき位置（座標）のデータを指す。ｕｉＰｋ'の算出は、客観視点指標Ｐｋの主観視点カメラ座標系における位置を表す状態ベクトルｓＰｋの関数
【００４０】
【数１】
2005353036.tif 000003
【００４１】
に基づいて行われる。
【００４２】
具体的には、関数Ｆｉ（）は、ｉ番目のデータを取得した時の（すなわち、撮像装置１３０の位置と姿勢がｔｉである時の）客観視点指標Ｐｋの客観視点カメラ座標における位置ベクトルｂｉＰｋを、ｓＰｋから求める次式、
【００４３】
【数２】
2005353036.tif 000004
【００４４】
及び、ｂｉＰｋから客観視点指標Ｐｋの客観視点画像上の座標ｕｉＰｋ'を求める次式、
【００４５】
【数３】
2005353036.tif 000005
【００４６】
によって構成されている。ここでｆＢｘ及びｆＢｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ１４０の焦点距離であり、既知の値として予め保持されているものとする。また、ＭＷＢは客観視点カメラ座標系における座標を世界座標に変換する変換行列であり、既知の値として予め保持されている世界座標系における客観視点カメラ１４０の位置及び姿勢に基づいて、予め算出されているものとする。ＭＷＣｉはｔｉによって定まるモデリング変換行列（主観視点カメラ座標系における座標を世界座標系における座標に変換する行列）であり、次式によって定義される。
【００４７】
【数４】
2005353036.tif 000006
【００４８】
ただし、
【００４９】
【数５】
2005353036.tif 000007
である。
【００５０】
ステップＳ２０９０において、較正情報算出部１８０は、全てのｉに対して、データリストＬＰｋ中の各データに含まれる客観視点指標Ｐｋの実際の画像座標ｕｉＰｋと、それに対応する画像座標の理論値ｕｉＰｋ'との誤差△ｕｉＰｋを次式によって算出する。
【００５１】
【数６】
2005353036.tif 000008
【００５２】
ステップＳ２１００において、較正情報算出部１８０は、全てのｉに対して、状態ベクトルｓＰｋに関する画像ヤコビアン（すなわち、式１の関数Ｆｉ（）を状態ベクトルｓＰｋの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列）ＪｕｉｓＰｋ（＝∂ｕｉＰｋ／∂ｓＰｋ）を算出する。具体的には、式３の右辺を客観視点カメラ座標系上の位置ベクトルｂｉＰｋの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｉｂｉＰｋ（＝∂ｕｉＰｋ／∂ｂｉＰｋ）と、式２の右辺を状態ベクトルｓＰｋの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列ＪｂｉｓＰｋ（＝∂ｂｉＰｋ／∂ｓＰｋ）を算出し、次式によってＪｕｉｓＰｋを算出する。
【００５３】
【数７】
2005353036.tif 000009
【００５４】
ステップＳ２１１０において、較正情報算出部１８０は、以上のステップで算出した、全てのｉに対する誤差△ｕｉＰｋ及びヤコビ行列ＪｕｉｓＰｋに基づいて、ｓＰｋの補正値△ｓＰｋを算出する。具体的には、全てのｉに対する誤差△ｕｉＰｋを垂直に並べた２ＩＰｋ次元ベクトルの誤差ベクトル
【００５５】
【数８】
2005353036.tif 000010
【００５６】
及びヤコビ行列ＪｕｉｓＰｋを垂直に並べた２ＩＰｋ行×３列の行列
【００５７】
【数９】
2005353036.tif 000011
【００５８】
を作成し、Φの擬似逆行列Φ＋を用いて、△ｓＰｋを次式より算出する。
【００５９】
【数１０】
2005353036.tif 000012
【００６０】
ここで、△ｓＰｋは３次元ベクトルであるから、２ＩＰｋが３以上、すなわちＩＰｋが２以上であれば、△ｓＰｋを求めることができる。なおΦ＋は、例えばΦ＋＝（ΦＴΦ）−１ΦＴによって求めることができるが、他の方法によって求めてもよい。
【００６１】
ステップＳ２１２０において、較正情報算出部１８０は、ステップＳ２１１０において算出した補正値△ｓＰｋを用いて、式１１に従って客観視点指標Ｐｋの主観視点カメラ座標系における位置ベクトルｓＰｋを補正し、得られた値を新たなｓＰｋとする。
【００６２】
【数１１】
2005353036.tif 000013
【００６３】
ステップＳ２１３０において、較正情報算出部１８０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値△ｓＰｋが予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓＰｋを用いて、再度ステップＳ２０８０以降の処理を行う。
【００６４】
ステップＳ２１３０において計算が収束したと判定されると、ステップＳ２１４０において、較正情報算出部１８０は、得られた状態ベクトルｓＰｋを、客観視点指標Ｐｋの主観視点カメラ座標系における位置を示すパラメータとして出力する。
【００６５】
最後にステップＳ２１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、指標較正装置１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。
【００６６】
以上の処理によって、対象物体（撮像装置）に装着した指標の対象物体に対する（すなわち、対象物体座標系における）位置を、簡便かつ正確に取得することができる。
【００６７】
＜変形例１−１＞
なお、本実施形態においては、状態ベクトルの補正値の算出に式１０で表現されるニュートン法を用いているが、補正値の算出は必ずしもニュートン法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。また、本実施形態では、ステップＳ２０７０からステップＳ２１３０までの処理において、求めるべき指標の位置に初期値を与え、画像ヤコビアンを利用した繰り返し演算によって最適値を求めたが、より簡便な計算方法によって指標の位置を求めることも可能である。例えば、式３を展開すると、
【００６８】
【数１２】
2005353036.tif 000014
【００６９】
【数１３】
2005353036.tif 000015
【００７０】
の関係が得られる。この関係を利用すると、
【００７１】
【数１４】
2005353036.tif 000016
【００７２】
のようにして、ｓを直接得ることができる。ここで、
【００７３】
【数１５】
2005353036.tif 000017
【００７４】
を表している。
【００７５】
＜変形例１−２＞
また、本実施形態では、６自由度位置姿勢センサとして、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」を組み合わせたものを用いたが、世界座標系における対象物体の位置と姿勢が計測可能であれば、それ以外方法によって６自由度位置姿勢センサを構成してもよい。例えば、磁気センサのような６自由度位置姿勢センサや、ジャイロセンサのような３自由度姿勢センサを撮像装置１３０に取り付け、撮像装置１３０の撮影画像から検出された各々の主観視点指標Ｑｋｎの画像座標ｕＱｋｎと、既知な情報として予め保持している指標の世界座標ｘＷＱｋｎとの対応関係、ならびに、撮像値１３０に取り付けたセンサの計測値、の両方に基づいたハイブリッドな方法で、撮像装置１３０の位置及び姿勢を算出しても良い（具体的な算出方法は、例えば〔非特許文献３〕、〔非特許文献４〕を参照）。
【００７６】
＜変形例１−３＞
本実施形態では、対象物体が撮像装置の場合であったので、世界座標系における対象物体の位置と姿勢の計測に、対象物体自身が撮影する映像情報を利用することができた。対象物体が任意の物体である場合には、その位置と姿勢の計測に、磁気センサのような６自由度位置姿勢センサを用いることができる。その場合の構成を図５に示す。
【００７７】
図５に示した本実施形態の変形例に係る指標較正装置５００は、任意の対象物体５１０に装着した指標の較正を行うためのものであり、対象物体５１０に装着した６自由度位置姿勢センサ５２０と、固定位置に設置した客観視点カメラ１４０を利用して、対象物体５１０に対する（すなわち、対象物体座標系における）夫々の指標の位置を、指標ごとに算出する。６自由度位置姿勢センサ５２０としては、磁気式の位置姿勢センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ  Ｔｅｃｈ．社のＦｌｏｃｋ  ｏｆ  Ｂｉｒｄｓ）や超音波センサなどを用いることが可能であるし、他の何れの６自由度位置姿勢センサを用いてもよい。指標較正装置５００は、図１における主観視点指標検出部１１０及び位置姿勢算出部１２０に代替する構成要素として、位置姿勢算出部５３０を有している。位置姿勢算出部５３０は、６自由度位置姿勢センサ５２０より位置姿勢計測値（センサ座標系におけるセンサ自身の位置及び姿勢）を入力し、その位置姿勢計測値から、既知の較正情報を用いて世界座標系における対象物体５１０の位置及び姿勢を算出し、データ管理部１７０の要求に従って、これをデータ管理部１７０へと出力する。それ以外の部分に関しては、指標較正装置１００とまったく同様である。
【００７８】
＜変形例１−４＞
なお、本実施形態において、対象物体を撮影するカメラとして固定位置に設置した客観視点カメラを用いていたが、世界座標系におけるカメラの位置及び姿勢が計測可能であれば客観視点カメラを固定しなくてもよい。
【００７９】
例えば、磁気センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ  Ｔｅｃｈ．社のＦｌｏｃｋ  ｏｆ  Ｂｉｒｄｓ）や、光学式センサ（加ＮＤＩ社のＯｐｔｏｔｒａｋや英ＶＩＣＯＮ社のＶＩＣＯＮ）、超音波センサといった６自由度位置姿勢センサを客観視点カメラに取り付けて、画像撮影時に客観視点カメラの世界座標系に対する位置姿勢をセンサ計測値から得てもよい。また、客観視点カメラによって撮影される主観視点指標Ｑｋｎの画像座標ｕＱｋｎと予め既知な情報として保持されている指標の世界座標ＸｗＱｋｎから客観視点カメラの位置姿勢を求めてもよい。また、客観視点カメラに前述の６自由度位置姿勢センサや３自由度姿勢センサを取り付け、客観視点画像とセンサの計測値の両方に基づいたハイブッドな手法（〔非特許文献３〕〔非特許文献４〕）によって客観視点カメラの位置姿勢を求めてもよい。
【００８０】
この場合、上記のような手段によって計測された客観視点カメラの位置姿勢は、ステップＳ２０２０において撮像装置１３０の位置姿勢と同時に取得され、ステップＳ２０４０においてデータリストに保存される。そして、ステップ２０８０において、式２におけるＭＷＢ（世界座標系における客観視点カメラの位置姿勢を表している）として、既知の値として予め保持されている固定値ではなく、データリスト中に保持されている位置姿勢計測値に基づいて算出した値を使用すればよい。
【００８１】
［第２の実施形態］
本実施形態に係る指標較正装置は、対象物体に装着した複数の指標が指標座標系を有しており、指標座標系における各指標の座標が既知である場合において、対象物体に装着した指標の較正を行うためのものであり、対象物体に装着した６自由度位置姿勢センサと、固定位置に設置した客観視点カメラを利用して、対象物体に対する（すなわち、対象物体座標系における）指標座標系の位置と姿勢、あるいは、対象物体に対する各指標の位置を算出する。以下、本実施形態に係る指標較正装置及び指標較正方法について説明する。なお、本実施形態においても、第１の実施形態と同様に、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」によって、６自由度位置姿勢センサが構成されているものとする。また、その６自由度位置姿勢センサが取り付けられている対象物体は、「撮像装置」自体であるとする。
【００８２】
図３は、本実施形態に係る指標較正装置の概略構成を示す図である。図１と同じ部分については同じ番号、記号を付けており、その詳細な説明は省略する。図３に示したように、本実施形態に係る指標較正装置３００は、主観視点指標検出部１１０、位置姿勢算出部１２０、客観視点カメラ１４０、客観視点指標検出部１５０、指示部１６０、データ管理部３７０、較正情報算出部３８０によって構成されており、較正対象である指標が装着された撮像装置１３０に接続されている。
【００８３】
なお、撮像装置１３０上の複数の位置には、較正される対象である客観視点指標Ｐｋ（ｋ＝１，，，Ｋ２）が設定されているものとする。主観視点カメラ座標系（＝対象物体座標系）上における各指標の位置は未知であるが、夫々の相対的な位置関係は既知であるとする。例えば、４点の客観視点指標Ｐｋ（ｋ＝１，２，３，４）の集合が、サイズが既知な１つの四角形指標Ｒ１を形成する場合が相当する。図３は、その状況を示している。この時、四角形指標Ｒ１上のある１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系を指標座標系と呼ぶと、指標座標系における各客観視点指標Ｐｋの座標は既知である。よって、主観視点カメラ座標系における指標座標系の位置及び姿勢（＝指標座標系における座標を主観視点カメラ座標に変換する変換行列）が判明すれば、未知である客観視点指標Ｐｋの主観視点カメラ座標系における位置を算出することができる。このことより、本実施形態に係る指標較正装置が較正する情報は、主観視点カメラ座標系における指標座標系の位置及び姿勢であるとする。
【００８４】
現実空間中の複数の位置には、第１の実施形態と同様に、撮像装置１３０によって撮影するための指標として、世界座標系における位置が既知である主観視点指標Ｑｋが設定されている。
【００８５】
主観視点指標検出部１１０は、第１の実施形態と同様に、撮像装置１３０が撮影した主観視点画像を入力し、画像中に撮影されている主観視点指標Ｑｋの画像座標を検出し、検出された主観視点指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを位置姿勢算出部１２０へと出力する。
【００８６】
位置姿勢算出部１２０は、第１の実施形態と同様に、検出された各々の主観視点指標Ｑｋｎの画像座標ｕＱｋｎと、既知な情報として予め保持している指標の世界座標ｘＷＱｋｎとの対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。算出した撮像装置１３０の世界座標系における位置及び姿勢は、データ管理部３７０の要求に従って、データ管理部３７０へと出力される。
【００８７】
客観視点カメラ１４０は、第１の実施形態と同様に、指標較正用のデータ取得を行う時の撮像装置１３０を撮像可能な位置に固定して配置されている。世界座標系における客観視点カメラ１４０の位置及び姿勢は、既知の値として較正情報算出部３８０に予め保持されているものとする。
【００８８】
客観視点指標検出部１５０は、第１の実施形態と同様に、客観視点カメラ１４０が撮影した客観視点画像を入力し、主観視点指標検出部１１０と同様な処理によって画像中に撮影されている客観視点指標Ｐｋの画像座標を検出し、データ管理部３７０の要求に従って検出された客観視点指標Ｐｋｍの画像座標ｕＰｋｍとその識別子ｋｍをデータ管理部３７０へと出力する。ただし、図３の例では、指標として四角形指標を用いているので、第１の実施形態で述べた四角形指標の検出処理が実行される。
【００８９】
指示部１６０は、第１の実施形態と同様に、不図示のオペレータからデータ取得コマンドが入力された時には「データ取得」の指示をデータ管理部３７０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部３８０に送信する。
【００９０】
データ管理部３７０は、指示部１６０から「データ取得」の指示を受けると、位置姿勢算出部１２０から撮像装置１３０の世界座標系における位置及び姿勢を入力し、客観視点指標検出部１５０から客観視点指標の画像座標とその識別子を入力し、［撮像装置１３０の位置及び姿勢−客観視点指標の画像座標−客観視点指標の識別子］の組を、一つのデータリストに追加しこれを保持する。ここで、位置姿勢算出部１２０から入力される撮像装置１３０の位置及び姿勢は、客観視点指標検出部１５０から入力される客観視点指標の画像座標を検出した画像の撮影時刻と、同一時刻のものである。また、データ管理部３７０は、較正情報算出部３８０からの要求にしたがって、生成したデータリストを較正情報算出部３８０に出力する。
【００９１】
較正情報算出部３８０は、指示部１６０から「較正情報算出」の指示を受けると、データ管理部３７０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、主観視点カメラ座標系における指標座標系の位置と姿勢）を出力する。
【００９２】
図４は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。図２と同じ部分については同じ番号、記号を付けており、その詳細な説明は省略する。なお、同フローチャートに従ったプログラムコードは、本実施形態の装置内の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。
【００９３】
ステップＳ２０１０からステップＳ２０３０までは、第１の実施形態と同様の処理を行う。ステップＳ２０３０を終えると、ステップＳ４０４０へと処理を移行する。
【００９４】
次に、ステップＳ４０４０において、データ管理部３７０は、検出された客観視点指標Ｐｋｍ全てについて、入力したデータの組をデータリストＬにデータＤｊとして追加する。具体的には、位置姿勢算出部１２０から入力するｔをｔｊ＝［ｘｊ  ｙｊ  ｚｊ  ξｊ  ψｊ  ζｊ］Ｔとし、客観視点指標検出部１５０から入力する識別子ｋｍをｋｊとし、同じく客観視点指標検出部１５０から入力するｕＰｋｍをｕｊＰｋｊとして、Ｄｊ＝［ｔｊ，ｕｊＰｋｊ，ｋｊ］の組を、ｊ番目のデータとしてデータリストＬに登録する。ここでｊ（ｊ＝１，，，ＮＰｋ）は、データリストＬに登録したデータの組夫々に対するインデックスであり、ＮＰｋは、登録したデータの総組数を表している。
【００９５】
以上の処理によって、データの取得が行われる。
【００９６】
ステップＳ４０５０では、データ管理部３７０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ２０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも異なる３つ以上の客観視点指標Ｐｋに関するデータが、データリストＬに含まれていること、が挙げられる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。
【００９７】
次にステップＳ２０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ４０７０へと処理を移行し、入力されていない場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。
【００９８】
較正情報算出部３８０は、求めるべき較正情報、すなわち指標系座標の主観視点カメラ座標系における位置及び姿勢を６値ベクトル［ｘ  ｙ  ｚ  ξ  ψ  ζ］Ｔとして扱う。以下では、この未知パラメータを状態ベクトルｓ＝［ｘ  ｙ  ｚ  ξ  ψ  ζ］Ｔと記述する。
【００９９】
ステップＳ４０７０において、較正情報算出部３８０は、状態ベクトルｓに適当な初期値を与える。初期値としては、オペレータが指示部１６０を介しておおよその値を手入力しても良いし、ある同一時刻に入力された（すなわち、同一の客観視点画像から検出された）複数の客観視点指標の検出座標をリストＬから抽出し、このデータを用いて、客観視点座標系における指標座標系の同時刻における位置と姿勢を公知の方法によって算出し、得られた位置と姿勢から指標座標系における座標を客観視点カメラ座標系に変換する変換行列ＭＢＭを求め、さらに、リストＬ中に保持されている、同時刻において入力された撮像装置の位置と姿勢ｔに基づいて、数式４によって定まる変換行列ＭＷＣを求め、次式によって、主観視点カメラ座標系における指標系座標の位置及び姿勢を表す変換行列ＭＣＭを得て、この行列によって表される位置と姿勢をｓの初期値として利用しても良い。
【０１００】
【数１６】
2005353036.tif 000018
【０１０１】
ここで、ＭＷＢは、客観視点カメラ座標系における座標を世界座標に変換する変換行列であり、既知の値として予め保持されている世界座標系における客観視点カメラ１４０の位置及び姿勢に基づいて、予め算出されているものとする。
【０１０２】
なお、客観視点座標系における指標座標系の位置と姿勢を算出する公知の手法としては、例えば、指標が同一平面上に配置されている場合には、４点以上の指標を用いて２次元ホモグラフィの計算に基づいて指標座標系の位置と姿勢を得る手法を用いることが可能である。また、同一平面上にない６点以上の指標を用いる手法や、これらの解を初期値にして、ガウス・ニュートン法のような繰り返し計算によって最適解を得る手法を用いることもできる。
【０１０３】
ステップＳ４０８０において、較正情報算出部３８０は、データリストＬ中の各データＤｊ＝［ｔｊ，ｕｊＰｋｊ，ｋｊ］（ｊ＝１，２，，，，ＮＰｋ）および状態ベクトルｓから、全てのｊに対して、各客観視点指標Ｐｋｊの客観視点画像座標の理論値ｕｊＰｋｊ'＝［ｕｘｊＰｋｊ'，ｕｙｊＰｋｊ'］を算出する。ここで客観視点指標の客観視点画像座標の理論値とは、指標座標系における位置ベクトルｈＭＰｋｊが既知な客観視点指標Ｐｋｊの、主観視点カメラ座標系における指標座標系の位置及び姿勢がｓである時に客観視点画像中に見えるべき位置（座標）のデータを指す。ｕｊＰｋｊ'の算出は、指標座標系の主観視点カメラ座標系における位置を表す状態ベクトルｓの関数
【０１０４】
【数１７】
2005353036.tif 000019
【０１０５】
に基づいて行われる。
【０１０６】
具体的には、関数Ｆｊ（）は、ｊ番目のデータを取得した時の（すなわち、撮像装置１３０の位置と姿勢がｔｊである時の）客観視点指標Ｐｋｊの客観視点カメラ座標における位置ベクトルｂｊＰｋｊを、ｓから求める次式、
【０１０７】
【数１８】
2005353036.tif 000020
【０１０８】
及び、ｂｊＰｋｊから客観視点指標Ｐｋｊの客観視点画像上の座標ｕｊＰｋｊ'を求める次式、
【０１０９】
【数１９】
2005353036.tif 000021
【０１１０】
によって構成されている。ここでｆＢｘ及びｆＢｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ１４０の焦点距離であり、既知の値として予め保持されているものとする。また、ＭＷＣｊはｔｊによって定まるモデリング変換行列であり、式４のｉをｊに読み替えた式によって定義される。また、ＭＣＭ（ｓ）はｓによって定まるモデリング変換行列（指標座標系における座標を主観視点カメラ座標系における座標に変換する行列）であり、次式によって定義される。
【０１１１】
【数２０】
2005353036.tif 000022
【０１１２】
ただし、
【０１１３】
【数２１】
2005353036.tif 000023
【０１１４】
である。
【０１１５】
ステップＳ４０９０において、較正情報算出部３８０は、全てのｊに対して、データリストＬ中の各データに含まれる客観視点指標Ｐｋｊの実際の画像座標ｕｊＰｋｊと、それに対応する画像座標の理論値ｕｊＰｋｊ'との誤差△ｕｊＰｋｊを次式によって算出する。
【０１１６】
【数２２】
2005353036.tif 000024
【０１１７】
ステップＳ４１００において、較正情報算出部３８０は、全てのｊに対して、状態ベクトルｓに関する画像ヤコビアン（すなわち、式１７の関数Ｆｊ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）ＪｕｊＰｋｊｓ（＝∂ｕｊＰｋｊ／∂ｓ）を算出する。具体的には、式１９の右辺を客観視点カメラ座標系上の位置ベクトルｂｊＰｋｊの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｊｂｊＰｋｊ（＝∂ｕｊＰｋｊ／∂ｂｊＰｋｊ）と、数１８の右辺を状態ベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｂｊＰｋｊｓ（＝∂ｂｊＰｋｊ／∂ｓ）を算出し、次式によってＪｕｊＰｋｊｓを算出する。
【０１１８】
【数２３】
2005353036.tif 000025
【０１１９】
ステップＳ４１１０において、較正情報算出部３８０は、以上のステップで算出した、全てのｊに対する誤差△ｕｊＰｋｊ及びヤコビ行列ＪｕｊＰｋｊｓに基づいて、ｓの補正値△ｓを算出する。具体的には、全てのｊに対する誤差△ｕｊＰｋｊを垂直に並べた２Ｊ次元ベクトルの誤差ベクトル
【０１２０】
【数２４】
2005353036.tif 000026
【０１２１】
及びヤコビ行列ＪｕｊＰｋｊｓを垂直に並べた２Ｊ行×６列の行列
【０１２２】
【数２５】
2005353036.tif 000027
【０１２３】
を作成し、Φの擬似逆行列Φ＋を用いて、△ｓを次式より算出する。
【０１２４】
【数２６】
2005353036.tif 000028
【０１２５】
ここで、△ｓは６次元ベクトルであるから、２Ｊが６以上、すなわちＪが３以上であれば、△ｓを求めることができる。なおΦ＋は、例えばΦ＋＝（ΦＴΦ）−１ΦＴによって求めることができるが、他の方法によって求めてもよい。
【０１２６】
ステップＳ４１２０において、較正情報算出部３８０は、ステップＳ４１１０において算出した補正値△ｓを用いて、式２７に従って指標系座標の主観視点カメラ座標系における位置及び姿勢を表す状態ベクトルｓを補正し、得られた値を新たなｓとする。
【０１２７】
【数２７】
2005353036.tif 000029
【０１２８】
ステップＳ４１３０において、較正情報算出部３８０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値△ｓが予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ４０８０以降の処理を行う。
【０１２９】
ステップＳ４１３０において計算が収束したと判定されると、ステップＳ４１４０において、較正情報算出部３８０は、得られた状態ベクトルｓを、較正情報、すなわち、主観視点カメラ座標系における指標座標系の位置及び姿勢として出力する。この時の出力の形態は、ｓそのものであっても良いし、ｓの位置成分を３値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓから生成した座標変換行列ＭＣＭであっても良い。
【０１３０】
最後にステップＳ２１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、指標較正装置３００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ２０１０へと戻り、データ取得コマンドの入力を待つ。
【０１３１】
以上の処理によって、対象物体（撮像装置）に装着した指標の対象物体に対する（すなわち、対象物体座標系における）位置または位置及び姿勢を、簡便かつ正確に取得することができる。
【０１３２】
＜変形例２−１＞
なお、本実施形態においては、較正情報として、主観視点カメラ座標系における指標座標系の位置及び姿勢を出力していたが、得られた状態ベクトルｓから、主観視点カメラ座標系における客観視点指標Ｐｋの位置を個々に算出して、これらを較正情報として出力しても良い。この場合、主観視点カメラ座標系における客観視点指標Ｐｋの位置は、ｓから求められる座標変換行列ＭＣＭと、指標座標系における客観視点指標Ｐｋの既知の位置ｈＭＰｋとの積によって得ることができる。
【０１３３】
＜変形例２−２＞
また、本実施形態では、６自由度位置姿勢センサとして、「対象物体上に装着されている撮像装置と、世界座標系における位置が既知である基準指標」を組み合わせたものを用いたが、世界座標系における対象物体の位置と姿勢が計測可能であれば、それ以外の方法によって６自由度位置姿勢センサを構成してもよい。例えば、第１の実施例と同様に、磁気センサのような６自由度位置姿勢センサや、ジャイロセンサのような３自由度姿勢センサを撮像装置１３０に取り付け、撮像装置１３０の撮影画像から検出された各々の主観視点指標Ｑｋｎの画像座標ｕＱｋｎと、既知な情報として予め保持している指標の世界座標ｘＷＱｋｎとの対応関係、ならびに、撮像値１３０に取り付けたセンサの計測値、の両方に基づいたハイブリッドな方法で、撮像装置１３０の位置及び姿勢を算出しても良い（具体的な算出方法は、例えば非特許文献３、非特許文献４を参照）。
【０１３４】
＜変形例２−３＞
なお、本実施形態においては、状態ベクトルの補正値の算出に式２６で表現される最急降下法を用いているが、補正値の算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。また、本実施形態においては、主観視点カメラ座標系における指標座標系の位置及び姿勢ｓに適当な初期値を与え、画像ヤコビアンを用いた繰り返し計算によって、全入力データに対するｓの最適値を求める方法について説明したが、他の簡易的な算出方法を用いることでも、主観視点カメラ座標系における指標座標系の位置及び姿勢を得ることができる。例えば、ステップＳ４０７０の説明で述べたような処理手順によって、１枚の客観視点画像上における複数の客観視点指標の検出座標のみを用いて、主観視点カメラ座標系における指標系座標の位置及び姿勢を得て、これを較正情報として出力してもよい。
【０１３５】
＜変形例２−４＞
なお、本実施形態においては、客観視点指標として四角形指標を用いていたが、指標座標系における各指標の位置が既知である指標群であれば、指標の種類は何れのものでもよい。例えば、第一の実施形態で用いたような複数の円形状指標の組であってもよいし、複数種の指標が混在していてもよい。また、複数の指標座標系を有する場合であっても、個々の指標座標系毎に上記の処理を個別に行うか、或いは、個々の指標座標系毎に上記の処理を並列的に行うことで、同様に較正することが可能である。
【０１３６】
＜変形例２−５＞
本実施形態では、対象物体が撮像装置の場合であったので、世界座標系における対象物体の位置と姿勢の計測に、対象物体自身が撮影する映像情報を利用することができた。対象物体が任意の物体である場合には、その位置と姿勢の計測に、磁気センサのような６自由度位置姿勢センサを用いることができる。その場合の構成は、図５に示した第１の実施形態の＜変形例１−３＞と同様である。６自由度位置姿勢センサとしては、磁気式の位置姿勢センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ  Ｔｅｃｈ．社のＦｌｏｃｋ  ｏｆ  Ｂｉｒｄｓ）や光学式センサ（加ＮＤＩ社のＯｐｔｏｔｒａｋや英ＶＩＣＯＮ社のＶＩＣＯＮ）、超音波センサなどを用いることが可能であるし、他の何れの６自由度位置姿勢センサを用いてもよい。
【０１３７】
＜変形例２−６＞
なお、本実施形態において、対象物体を撮影するカメラとして固定位置に設置した客観視点カメラを用いていたが、第１の実施形態の＜変形例１−４＞と同様に、世界座標系におけるカメラの位置及び姿勢が計測可能であれば客観視点カメラを固定しなくてもよい。
【０１３８】
この場合、計測された客観視点カメラの位置姿勢は、ステップＳ２０２０において撮像装置１３０の位置姿勢と同時に取得され、ステップＳ４０４０においてデータリストに保存される。そして、ステップ４０８０において、式１８におけるＭＷＢ（世界座標系における客観視点カメラの位置姿勢を表している）として、既知の値として予め保持されている固定値ではなく、データリスト中に保持されている位置姿勢計測値に基づいて算出した値を使用すればよい。
【０１３９】
［第３の実施形態］
本実施形態に係る指標較正装置では、対象物体に装着した複数の指標の較正を、６自由度位置姿勢センサを有する移動可能な撮像装置で対象物体を撮影することにより行う。第１または第２の実施形態では、指標の位置または位置姿勢のみを未知パラメータとして取り扱っていたが、本実施形態では指標の位置又は位置姿勢だけでなく物体に対するカメラの位置姿勢も未知パラメータとして取り扱う。以下、本実施形態に係る指標較正装置及び指標較正方法について説明する。
【０１４０】
本実施形態の指標はは、同一直線上にない３個以上の点群から構成される。指標を構成する複数の点の相対位置は既知、つまり各指標において規定される指標座標系における指標を構成する各点の位置は既知であるとする。本実施形態では、前述の指標が物体上に複数配置されている場合に、各指標に規定される指標座標系の、物体を基準とした（つまり物体座標系に対する）位置姿勢を較正情報として求める。
【０１４１】
図６は、本実施形態に係る指標較正装置１１００の概略構成を示す図である。図６に示すように、本実施形態に係る指標較正装置１１００は、指標検出部１０２０、位置姿勢算出部１０３０、撮像部１０１０、指示部１０５０、データ管理部１０４０、較正情報算出部１０６０によって構成されており、指標が装着された物体１０００に接続されている。
【０１４２】
また、物体１０００には６自由度位置姿勢センサＡ２が装着されており、世界座標系に対する物体の位置姿勢を計測することが可能になっている。６自由度位置姿勢センサＡ２は位置姿勢算出部１０３０に接続されている。また、撮像部１０１０には６自由度位置姿勢センサＡ１が装着されており、撮像部を構成するカメラの世界座標系に対する位置姿勢を計測することが可能になっている。６自由度位置姿勢センサＡ１は位置姿勢算出部１０３０に接続されている。
【０１４３】
なお、物体１０００上には、前述の定義に基づく指標が複数配置されている。ここで、物体に配置された指標をＰｋ（ｋ＝１，，，Ｋｏ）で表す。但し、Ｋｏは物体上に配置された較正対象となる指標の数である。また、指標Ｐｋは点ｐｋｉ（（ｋ＝１，，，Ｋｏ）、（ｉ＝１，，，Ｎｋ））により構成される。但し、Ｎｋは、指標Ｐｋを構成する点の総数を表す。
【０１４４】
前述したように、物体上に配置された指標は、夫々を構成する点の指標座標系における位置は既知であるが、物体座標系（物体上の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における位置姿勢は未知である。なお、物体上には、物体座標系を規定するための基準指標として、同一直線上にない物体座標系における位置が既知な指標が３点以上配置されている。すべての基準指標は、指標較正用のデータを取得する撮像部１０１０によって他の較正対象のマーカと同一画像内で観測される必要がある。また、指標Ｐｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各点が識別可能であるような指標であれば、何れの形態であってもよい。
【０１４５】
撮像部１０１０は、物体上に配置された指標を様々な位置や方向から撮影する。撮影された画像は、指標検出部１０２０に入力される。
【０１４６】
指標検出部１０２０は、撮像部１０１０より画像を入力し、入力した画像中に撮影されている指標Ｐｋを構成する各点の画像座標を検出する。
【０１４７】
さらに、指標検出部１０２０は、検出された各々の指標Ｐｋｎを構成する点ｐｋｎｉの画像座標ｕＰｋｎｉとその識別子ｋｎをデータ管理部１０４０へと出力する。ここで、ｎ（ｎ＝１，，，Ｍ）は検出された指標夫々に対するインデックスであり、Ｍは検出された指標の総数を表している。例えば図６の場合は、識別子が１、２，３である四角形形状の指標が撮影される場合を示しており、Ｍ＝３であり、識別子ｋ１＝１、ｋ２＝２、ｋ３＝３とこれらに対応する画像座標ｕＰｋ１ｉ、ｕＰｋ２ｉ、ｕＰｋ３ｉ、（ｉ＝１、２、３、４）が出力される。
【０１４８】
位置姿勢算出部１０３０は、カメラに装着された６自由度位置姿勢センサＡ１と、物体に装着された６自由度位置姿勢センサＡ２から得られるカメラと物体の世界座標系における位置姿勢から、物体座標系におけるカメラの位置姿勢を算出する。この物体座標系におけるカメラの位置姿勢を、データ管理部１０４０の要求に従って、データ管理部１０４０へ出力する。
【０１４９】
また、位置姿勢算出部１０３０は、物体座標系におけるカメラの位置及び姿勢を、夫々３値ベクトル［ｘ  ｙ  ｚ］Ｔ及び［ξ  ψ  ζ］Ｔによって内部的に表現している。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。このとき、位置及び姿勢は夫々、６次元ベクトル［ｘ  ｙ  ｚ  ξ  ψ  ζ］Ｔで表される。
【０１５０】
指示部１０５０は、不図示のオペレータからデータ取得コマンドが入力されたとき時には「データ取得」の指示をデータ管理部１０４０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部１０６０に送信する。
【０１５１】
データ管理部１０４０は、指示部１０５０から「データ取得」の指示を受けると、位置姿勢算出部１０３０から物体座標系におけるカメラの位置と姿勢を入力し、指標検出部１０２０から指標の画像座標とその識別子を入力し、［カメラの物体座標系での位置姿勢−指標の画像座標−指標の識別子］の組を一つのデータリストに追加しこれを保持する。ここで、位置姿勢算出部から入力されるカメラの物体座標系での位置姿勢は、指標検出部１０２０から入力される指標の画像座標を検出した画像の撮影時刻と同一時刻のものである。また、データ管理部１０４０は、較正情報算出部１０６０からの要求に従って、生成したデータリストを較正情報算出部１０６０に出力する。
【０１５２】
較正情報算出部１０６０は、指示部１０５０から「較正情報算出」の指示を受け取ると、データ管理部１０４０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、物体座標系における指標座標の位置と姿勢）を出力する。
【０１５３】
図７は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお同フローチャートに従ったプログラムコードは、本実施形態の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。
【０１５４】
ステップＳ６０１０において、指示部１０５０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、指標較正用のデータ取得を行う位置に物体１０００を配置した時に、データ取得コマンドを入力する。指示部１０５０は、データ取得コマンドが入力されている場合には、ステップＳ６０２０へと処理を移行させる。
【０１５５】
ステップＳ６０２０において、データ管理部１０４０は、位置姿勢算出部１０３０から、物体座標系におけるカメラの位置及び姿勢ｔＯＣ＝［ｘＯＣ  ｙＯＣ  ｚＯＣ  ξＯＣ  ψＯＣ  ζＯＣ］Ｔを入力する。位置姿勢算出部１０３０は常にｔＯＣの算出を行っている。カメラに装着した６自由度位置姿勢センサＡ１の計測値ｓＡ１＝［ｘＡ１Ｃ  ｙＡ１Ｃ  ｚＡ１Ｃ  ξＡ１Ｃ  ψＡ１Ｃ  ζＡ１Ｃ］Ｔから得られる世界座標系におけるカメラの位置姿勢を表す変換行列をＭｗｃ（ｓＡ１）、物体に装着した６自由度位置姿勢センサＡ２の計測値ｓＡ２＝［ｘＡ２Ｏ  ｙＡ２Ｏ  ｚＡ２Ｏ  ξＡ２Ｏ  ψＡ２Ｏ  ζＡ２Ｏ］Ｔから得られる世界座標系における物体の位置姿勢を表す変換行列をＭｗｏと表すと、物体座標系におけるカメラの位置姿勢を表す変換行列Ｍｏｃは、式（３３）のように表される。
【０１５６】
【数２８】
2005353036.tif 000030
【０１５７】
式２８で得られる行列Ｍｏｃから並進成分と姿勢成分を分離することにより、物体座標系における撮像装置の位置と姿勢ｔＯＣ＝［ｘＯＣ  ｙＯＣ  ｚＯＣ  ξＯＣ  ψＯＣ  ζＯＣ］Ｔを得ることができる。
【０１５８】
ステップＳ６０３０において、データ管理部１０４０は、指標検出部１０２０から、指標検出部１０２０によって検出された指標Ｐｋｎを構成する点の画像座標群ｕＰｋｎｉとその識別子ｋｎを入力する。指標検出部１０２０は、入力された画像に対して常に指標の検出処理を行っており、本ステップの処理により、撮像装置の位置と姿勢がｔＯＣであるときの指標の画像座標を得ることができる。なお、指標検出部１０２０から入力される情報は、必ずしも全ての指標に関するものである必要はなく、その時点で画像上において検出されている指標に関する情報であればよい。
【０１５９】
次に、ステップＳ６０４０において、データ管理部１０４０は、検出された指標Ｐｋｎ全てについて、入力したデータの組をデータリストＤＬにデータＤＬとして追加する。具体的には、位置姿勢算出部１０３０から入力するｔＯＣをｔＯＣｊ＝［ｘＯＣｊ  ｙＯＣｊ  ｚＯＣｊ  ξＯＣｊ  ψＯＣｊ  ζＯＣｊ］Ｔとし、指標検出部１０２０から入力する識別子ｋｎをｋｎｊとし、同じく指標検出部１０２０から入力するｕＰｋｎｉをｕＬＰｋｎｊｉとして、ＤＬ＝［ｔＯＣｊ，ｕＬＰｋｎｊｉ，ｋｎｊ］の組を、Ｌ番目のデータとしてデータリストＤＬに登録する。ここでｊ（ｊ＝１，，，ＮＪ）は、撮影した画像に対するインデックスであり、Ｌ（Ｌ＝１，，，ＮＬ）は、データリストＤＬに登録したデータの組夫々に対するインデックスであり、ＮＪは、撮影した画像の総数を表し、ＮＬは、登録したデータの総組数を表している。
【０１６０】
以上の処理によって、データの取得が行われる。
【０１６１】
ステップＳ６０５０では、データ管理部１０４０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ６０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも異なる３つ以上の基準指標に関するデータが、データリストＤＬに含まれていること、が挙げられる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。
【０１６２】
次にステップＳ６０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ６０７０へと処理を移行し、入力されていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。
【０１６３】
較正情報算出部１０６０は、第２の実施形態と同様に求めるべき較正情報、すなわち物体座標系における指標の位置姿勢を６値ベクトルとして扱う。以下では、この未知パラメータを状態ベクトルｓｍ＝［ｘｍ  ｙｍ  ｚｍ  ξｍ  ψｍ  ζｍ］Ｔと記述する。また、本実施形態では物体に対するカメラの位置姿勢ｔＯＣｊも未知パラメータとして取り扱う。ｔＯＣｊを状態ベクトルｓｃｊ＝［ｘｏｃｊ  ｙｏｃｊ  ｚｏｃｊ  ξｏｃｊ  ψｏｃｊ  ζｏｃｊ］Ｔと記述する。
【０１６４】
ステップＳ６０７０において、較正情報算出部１０６０は、夫々の状態ベクトルｓｍ、ｓｃｊに適当な初期値を与える。ｓｃｊの初期値は、センサ出力から得られたｔＯＣｊを用いる。ｓｍの初期値としては、オペレータが指示部１０５０を介しておおよその値を手入力しても良い。カメラ座標系から物体座標系への変換行列をＭＯＣ、物体座標系から物体に装着された指標座標系への変換行列をＭＭＯとすると、カメラ座標系から指標座標への変換行列ＭＭＣを得る。
【０１６５】
【数２９】
2005353036.tif 000031
【０１６６】
ステップＳ４０８０において、較正情報算出部１０６０は、データリストＤＬ中の各データＤＬ＝［ｔＯＣｊ，ｕＬＰｋｎｊｉ，ｋｊ］、（（ｊ＝１，２，，，，ＮＪ）（Ｌ＝１，２，，，，ＮＬ）および状態ベクトルｓｍ、ｓｃｊ、から、全てのＬに対して、指標Ｐｋｎｊの画像座標の計算値ｕｌＬＰｋｎｊｉ'＝［ｕｘＬＰｋｎｊｉ'，ｕｙＬＰｋｎｊｉ'］を求める。ここで指標の画像座標の計算値とは、物体座標系における位置及び姿勢がｓｍである指標の指標座標系における位置ベクトルｈＭＰｋｎｊが既知な点Ｐｋｎｊの、画像中に見えるべき位置（座標）のデータを指す。ｕＬＰｋｎｊｉ'の算出は、物体座標系における指標座標系の位置を表す状態ベクトルｓ＝［ｓｍ  ｓＣｊ］の関数
【０１６７】
【数３０】
2005353036.tif 000032
【０１６８】
に基づいて行われる。
【０１６９】
具体的には、関数ＦＬ（）は、Ｌ番目のデータを取得した時の（すなわち、同データセット中の撮像装置１０１０の位置と姿勢がｔＯＣｊである時の）指標Ｐｋｎｊのカメラ座標における位置ベクトルｃＬＰｋｎｊｉを、ｓｍ，ｓｃｊから求める次式、
【０１７０】
【数３１】
2005353036.tif 000033
【０１７１】
及び、ｃＬＰｋｎｊｉから指標Ｐｋｎｊの画像上の座標ｕＬＰｋｎｊｉ'を求める次式、
【０１７２】
【数３２】
2005353036.tif 000034
【０１７３】
によって構成されている。ここでｆＢｘ及びｆＢｙは、それぞれｘ軸方向及びｙ軸方向におけるカメラ１０１０の焦点距離であり、既知の値として予め保持されているものとする。また、
【０１７４】
【化１】
2005353036.tif 000035
【０１７５】
によって定まる変換行列（指標座標系から物体座標系に変換する行列）であり、
【０１７６】
【化２】
2005353036.tif 000036
【０１７７】
はｓｃｊによって定まるモデリング変換行列（物体座標系からカメラ座標系に変換する行列）である。
【０１７８】
【化３】
2005353036.tif 000037
【０１７９】
は次式によって定義される。
【０１８０】
【数３３】
2005353036.tif 000038
【０１８１】
ただし、
【０１８２】
【数３４】
2005353036.tif 000039
【０１８３】
である。また、
【０１８４】
【数３５】
2005353036.tif 000040
【０１８５】
ただし、
【０１８６】
【数３６】
2005353036.tif 000041
【０１８７】
である。
【０１８８】
ステップＳ６０９０において、較正情報算出部１０６０は、全てのＬに対して、データリストＤＬ中の各データに含まれる指標Ｐｋｎｊの実際の画像座標ｕＬＰｋｎｊｉと、それに対応する画像座標の計算値ｕＬＰｋｎｊｉ'との誤差△ｕＬＰｋｎｊｉを次式によって算出する。
【０１８９】
【数３７】
2005353036.tif 000042
【０１９０】
ステップＳ６１００において、較正情報算出部１０６０は、全てのＬに対して、状態ベクトルｓ＝［ｓｍ  ｓｃｊ］に関する画像ヤコビアン（すなわち、式３０の関数Ｆｊ（）を状態ベクトルｓｍ、ｓｃｊの各要素で偏微分した解を各要素に持つ（２×ＮＬ）行×（６×Ｋｏ＋６×ＫＮＪ）列のヤコビ行列）ＪｕＬＰｋｎｊｉｓ（＝∂ｕＬＰｋｎｊｉ／∂ｓ）を算出する。ここで、ＮＬは検出したすべての指標の総数であり、Ｋｏは、較正対象となる指標の総数である。ＫＮＪは画像の撮影枚数である。
【０１９１】
ステップＳ６１１０において、較正情報算出部１０６０は、以上のステップで算出した、全てのＬに対する誤差△ｕＬＰｋｎｊｉ及びヤコビ行列ＪｕＬＰｋｎｊｉｓに基づいて、ｓの補正値△ｓを算出する。具体的には、全てのＬに対する誤差△ｕＬＰｋｎｄｊｉを垂直に並べた２ＮＬ次元ベクトルの誤差ベクトル
【０１９２】
【数３８】
2005353036.tif 000043
【０１９３】
及びヤコビ行列ＪｕＬＰｋｎｊｉｓを垂直に並べた（２ＮＬ）行×（６×Ｋｏ＋６×ＫＮＪ）列の行列
【０１９４】
【数３９】
2005353036.tif 000044
【０１９５】
を作成し、Φの擬似逆行列Φ＋を用いて、△ｓを次式より算出する。
【０１９６】
【数４０】
2005353036.tif 000045
【０１９７】
ここで、△ｓは（６×Ｋｏ＋６×ＫＮＪ）次元ベクトルであるから、２ＮＬが（６×Ｋｏ＋６×ＫＮＪ）以上であれば、△ｓを求めることができる。なおΦ＋は、例えばΦ＋＝（ΦＴΦ）−１ΦＴによって求めることができるが、他の方法によって求めてもよい。
【０１９８】
ステップＳ６１２０において、較正情報算出部１０６０は、ステップＳ６１１０において算出した補正値△ｓを用いて、式４１に従ってｓを補正し、得られた値を新たなｓとする。ここで状態ベクトルｓ＝［ｓｍ  ｓｃｊ］は、物体座標系における指標の位置姿勢の状態ベクトルｓｍ、物体座標系におけるカメラ位置姿勢の状態ベクトルｓＣｊである。
【０１９９】
【数４１】
2005353036.tif 000046
【０２００】
ステップＳ６１３０において、較正情報算出部１０６０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値△ｓが予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ６０８０以降の処理を行う。
【０２０１】
ステップＳ６１３０において計算が収束したと判定されると、ステップＳ６１４０において、較正情報算出部１０６０は、得られた状態ベクトルｓのうちｓｍを、較正情報、すなわち、物体座標系における指標座標系の位置及び姿勢として出力する。この時の出力の形態は、ｓｍそのものであっても良いし、ｓｍの位置成分を３値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓｍから生成した座標変換行列ＭＯＭであっても良い。
【０２０２】
最後にステップＳ６１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、指標較正装置１１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。
【０２０３】
以上の処理によって、対象物体に対する（すなわち、対象物体座標系における）対象物体に装着した指標の位置または位置と姿勢を、簡便かつ正確に取得することができる。
【０２０４】
＜変形例３−１＞
なお、本実施形態において、撮像部１０１０は自由に移動可能な構成であったが、第１及び第２の実施形態における客観視点カメラと同様に、撮像部１０１０を固定位置に設置しても良い。この場合、６自由度位置姿勢センサＡ１は不要であり、位置姿勢算出部１０３０は、既知の値として予め保持されている世界座標系における撮像部１０１０の位置姿勢と、６自由度位置姿勢センサＡ１から得られる世界座標系における物体の位置姿勢に基づいて、物体座標系における撮像部１０１０の位置姿勢を算出する。第１の実施形態や第２の実施形態においては、客観視点カメラの位置姿勢は既知の値として正確に得られている必要があったが、本実施形態では、位置姿勢算出部１０３０が出力する物体座標系におけるカメラの位置姿勢に補正処理が施されるため、既知の値としては大まかな位置姿勢のみが入力されていればよいという利点がある。
【０２０５】
＜変形例３−２＞
なお、本実施形態において、カメラと物体との相対的な位置関係を求めるために６自由度位置姿勢センサを用いるが，物体座標系におけるカメラの位置及び姿勢が計測可能であればいずれのセンサでもよい。
【０２０６】
例えば、磁気センサ（米Ｐｏｌｈｅｍｕｓ社のＦＡＳＴＲＡＫや米Ａｓｃｅｎｓｉｏｎ  Ｔｅｃｈ．社のＦｌｏｃｋ  ｏｆ  Ｂｉｒｄｓ）、光学式センサ（加ＮＤＩ社のＯｐｔｏｔｒａｋや英ＶＩＣＯＮ社のＶＩＣＯＮ）、超音波センサといった６自由度位置姿勢センサをカメラと物体に取り付けて、画像撮影時のカメラの物体座標系に対する位置姿勢をセンサ計測値から得てもよい。また、実施例１及び２と同様に，物体に装着されているカメラと世界座標系における位置が既知である基準指標によって構成される６自由度位置姿勢センサの計測値から，物体とそれを撮影するカメラの相対的な位置姿勢を求めてもよい。また、物体とそれを撮影するカメラに前述の６自由度位置姿勢センサや３自由度姿勢センサを取り付け、撮影画像とセンサの計測値の両方に基づいたハイブリッドな手法によって物体とそれを撮影するカメラの相対的な位置姿勢を求めてもよい。
【０２０７】
＜変形例３−３＞
なお、本実施形態における指標較正装置によって物体に装着された指標の物体に対する位置と姿勢を較正した後、カメラに装着した６自由度位置姿勢センサＡ１のカメラに対する位置と姿勢を求めることも可能である．ここで位置と姿勢は、位置を表す３次元の並進ベクトルｔと、姿勢を表す３×３回転行列Ｒを使って、式４２のような４×４行列Ｍで表す。
【０２０８】
【数４２】
2005353036.tif 000047
【０２０９】
第３の実施形態において説明したように、物体に装着された指標の物体に対する位置と姿勢を求める際に、較正に利用した画像を撮影した時点でのカメラの物体に対する位置と姿勢ＭＯＣも同時に求まる。また、カメラに装着した６自由度位置姿勢センサと物体に装着した６自由度位置姿勢センサの相対的な位置と姿勢ＭＡ２Ａ１はそれぞれのセンサ出力値から計算できる。物体に装着した６自由度位置姿勢センサの物体に対する位置と姿勢ＭＯＡ２が何らかの方法で求まっている場合に、式４３が成り立つ。
【０２１０】
【数４３】
2005353036.tif 000048
【０２１１】
ここで既知の情報であるＭＯＡ２とＭＡ２Ａ１を次のようにまとめる。
【０２１２】
【数４４】
2005353036.tif 000049
【０２１３】
式４４を用いて式４３を式４５のように表す。
【０２１４】
【数４５】
2005353036.tif 000050
【０２１５】
ＮＪ枚の画像を撮影すると、撮影画像ごとに存在するＭＯＣ，ＭＯＡ１と、カメラに装着した６自由度位置姿勢センサのカメラに対する位置と姿勢ＭＡ１Ｃを用いて式４６のように表せる。
【０２１６】
【数４６】
2005353036.tif 000051
【０２１７】
ここで、
【０２１８】
【数４７】
2005353036.tif 000052
【０２１９】
【数４８】
2005353036.tif 000053
【０２２０】
とすると、式４６は式４９のように表せる。
【０２２１】
【数４９】
2005353036.tif 000054
【０２２２】
撮影した画像が複数ある場合には、ＭＡ１Ｃを求めるためにＢの一般化逆行列を用いて式５０のように計算することが可能である。
【０２２３】
【数５０】
2005353036.tif 000055
【０２２４】
また、ＡとＢを用いてＭＡ１Ｃを求めるために他の手法で計算してもよい。
【０２２５】
算出した位置と姿勢を表す４×４行列ＭＡ１Ｃより、式４２のように姿勢を表す３×３回転行列Ｒと位置を表す３次元並進ベクトルｔを分解し求める。以上のようにしてカメラに装着した６自由度位置姿勢センサのカメラに対する位置と姿勢を求めることが可能である。
【０２２６】
［他の実施形態］
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても、達成されることは言うまでもない。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０２２７】
さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０２２８】
本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明した（図２及び／又は図４に示す）フローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０２２９】
【図１】第１の実施形態に係る指標較正装置の概略構成を示す図である。
【図２】第１の実施形態に係る指標較正方法の概略処理を示すフローチャートである。
【図３】第２の実施形態に係る指標較正装置の概略構成を示す図である。
【図４】第２の実施形態に係る指標較正方法の概略処理を示すフローチャートである。
【図５】第１の実施形態の＜変形例１―３＞および第２の実施形態の＜変形例２−５＞に係る指標較正装置の概略構成を示す図である。
【図６】第３の実施形態に係る指標構成装置の概略構成を示す図である。
【図７】第３の実施形態に係る指標較正方法の概略処理を示すフローチャートである。
【図１】
2005353036.tif 000056
【図２】
2005353036.tif 000057
【図３】
2005353036.tif 000058
【図４】
2005353036.tif 000059
【図５】
2005353036.tif 000060
【図６】
2005353036.tif 000061
【図７】
2005353036.tif 000062