JP4136859B2 - 位置姿勢計測方法 - Google Patents

Description

本発明は、撮像装置あるいは撮像装置を装着した対象物体の位置及び姿勢を計測する技術に関するものである。

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

またその他にも、観察者の頭部に装着された光学シースルー型ディスプレイに、観察者の視点の位置及び姿勢に応じて生成した仮想空間の画像を表示する光学シースルー方式によって実現される。

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

これらの応用に対して共通に要求されるのは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、ビデオシースルー方式の場合は情景中における（すなわち、世界座標系における）撮像装置の位置及び姿勢を求める問題に帰結される。同様に、光学シースルー方式の場合は情景中における観察者の視点あるいはディスプレイの位置及び姿勢を求める問題に帰結される。

前者の問題を解決する方法として、情景中に複数の指標を配置あるいは設定し、撮像装置が撮像した画像内における指標の投影像の座標を検出することで、情景中における該撮像装置の位置及び姿勢を求めることが一般的に行われている。また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置を装着し、前者と同様な方法によって該撮像装置の位置及び姿勢を求め、それに基づいて計測対象物の位置及び姿勢を求めることが一般的に行われている。

図１を用いて、撮像装置が撮像した画像から指標を検出することで撮像装置の位置及び姿勢を計測する位置姿勢計測装置の従来例について説明する。図１は従来の位置姿勢計測装置の構成を示す図である。同図に示したように、本従来例における位置姿勢計測装置１００は、主観視点指標検出部１１０及び位置姿勢算出部１２０によって構成されており、撮像装置１３０に接続されている。

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

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

撮像装置１３０が出力する画像（以下、これを主観視点画像と呼ぶ）は、位置姿勢計測装置１００に入力される。

主観視点指標検出部１１０は、撮像装置１３０より主観視点画像を入力し、入力した画像中に撮影されている主観視点指標Ｑ^ｋの画像座標を検出する。例えば、主観視点指標Ｑ^ｋの各々が異なる色を有するマーカによって構成されている場合には、主観視点画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、主観視点指標Ｑ^ｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを主観視点画像上に施すことにより、指標の位置を検出する。

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

位置姿勢算出部１２０は、検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと、既知な情報として予め保持している指標の世界座標ｘ_Ｗ ^Ｑｋｎとの対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。

主観視点指標の世界座標と画像座標の組から撮像装置の位置及び姿勢を算出する方法は、写真測量等の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。

図２のフローチャートを用いて、本従来例における位置姿勢算出部１２０が行う処理について説明する。本従来例において、撮像装置１３０の位置及び姿勢は、繰り返し演算による数値計算的手法によって算出される。

位置姿勢算出部１２０は、算出すべき撮像装置１３０の位置及び姿勢を、それぞれ３値ベクトル［ｘ ｙ ｚ］^Ｔ及び［ξ ψ ζ］^Ｔによって内部的に表現している。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。このとき、求めるべき未知パラメータは、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］^Ｔと記述される。

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

ステップＳ２０１０において、位置姿勢算出部１２０は、主観視点指標検出部１１０において検出された各々の主観視点指標Ｑ^ｋｎの画像座標の実測値ｕ^Ｑｋｎとその識別子ｋ_ｎを入力する。

ステップＳ２０２０において、位置姿勢算出部１２０は、各々の主観視点指標Ｑ^ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を算出する。ｕ^Ｑｋｎ＊の算出は、既知な情報として予め保持している指標Ｑ^ｋｎ各々の世界座標ｘ_Ｗ ^Ｑｋｎと現在の状態ベクトルｓの関数：

に基づいて行われる。
具体的には、関数Ｆｃ（）は、ｘ_Ｗ ^Ｑｋｎとｓから当該指標のカメラ座標（カメラ上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘ_Ｃ ^Ｑｋｎを求める次式、

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

によって構成されている。ここでｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。また、Ｍ_ＷＣ（ｓ）はｓによって定まるモデリング変換行列（すなわち、カメラ座標系における座標を世界座標系における座標に変換する行列）であり、次式によって定義される。

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

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

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

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

を作成し、Φの擬似逆行列Φ’を用いて、

として算出する。図１の例ではＮ＝３であるので、Ｕは６次元ベクトル、Φは６行×６列の行列となる。
ステップＳ２０６０において、位置姿勢算出部１２０は、ステップＳ２０５０において算出した補正値Δｓを用いて式１０に従って状態ベクトルｓを補正し、得られた値をｓの新たな推定値とする。

ステップＳ２０７０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ２０２０以降の処理を行う。
ステップＳ２０８０において、位置姿勢算出部１２０は、得られた状態ベクトルｓを撮像装置１３０の位置及び姿勢の情報として出力する。位置及び姿勢の情報は、例えば、ｓから算出したビューイング変換行列の形態によって出力する。

以上の方法によって撮像装置１３０の位置及び姿勢を取得することが、従来からなされてきた。

また、計測対象物体に複数の指標を設定し、外部に設置した客観視点カメラによって対象物体を撮影し、撮像した客観視点画像内における指標の投影像の画像座標を検出することで、対象物体の位置及び姿勢を求めることも行われている（例えば、非特許文献２、特許文献１を参照）。
R. M. Haralick, C. Lee, K. Ottenberg, and M. Nolle: Review and analysis of solutions of the three point perspective pose estimation problem, International Journal of Computer Vision, vol.13, no.3, pp.331−356, 1994. D. G. Lowe: Fitting parameterized three−dimensional models to images, IEEE Transactions on PAMI, vol.13, no.5, pp.441−450, 1991. 特開平０９−３２３２８０号公報

しかし、計測対象である撮像装置から得た主観視点画像から情景中の指標を検出する前者の方法では、姿勢の変化に比べて位置の変化に対する画像上での指標の移動量が小さいために、位置計測の精度が十分に得られないという問題があった。また、広い範囲の見回しを実現しようとした場合、対応する広範囲の領域に多数の指標を設定しなくてはならないという問題があった。

一方、客観視点カメラから得た客観視点画像から計測対象物上のマーカを検出する後者の方法では、位置の変化に比べて姿勢の変化に対する画像上での指標の移動量が小さいために、姿勢計測の精度が十分に得られないという問題があった。

本発明は以上の問題に鑑みてなされたものであり、対象物体の位置姿勢及び姿勢の計測を、位置及び姿勢の何れについても高い精度で実現することを目的とする。

本発明の目的を達成する為に、例えば本発明の位置姿勢計測方法は以下の構成を備える。

即ち、主観視点撮像装置の位置及び姿勢を算出する位置姿勢計測方法であって、
前記主観視点撮像装置によって物理空間内の情景を撮像する主観視点撮像工程と、
前記主観視点撮像装置を前記物理空間に固定された客観視点撮像装置によって撮像する客観視点撮像工程と、
前記主観視点撮像工程で取得された第１の画像から、前記情景中に配置された第１の指標の画像座標を検出する第１の検出工程と、
前記客観視点撮像工程で取得された第２の画像から、前記主観視点撮像装置上に配置された第２の指標の画像座標を検出する第２の検出工程と、
前記第２の検出工程で検出した前記第２の指標の画像座標と、前記客観視点撮像装置の位置姿勢とに基づいて、前記第２の指標の物理空間中における位置を拘束する直線を求め、前記第２の指標が当該直線上に存在するという拘束の元に、前記第１の検出工程で検出した前記第１の指標の画像座標に基づいて、前記主観視点撮像装置の位置及び姿勢を算出する位置姿勢算出工程と
を備えることを特徴とする。

本発明の目的を達成する為に、例えば本発明の位置姿勢計測装置は以下の構成を備える。

即ち、物理空間内の情景を撮像する主観視点撮像装置の位置及び姿勢を算出する位置姿勢計測装置であって、
前記物理空間に固定され、前記主観視点撮像装置を撮像する客観視点撮像手段と、
前記主観視点撮像装置によって取得された第１の画像から、前記情景中に配置された第１の指標の画像座標を検出する第１の検出手段と、
前記客観視点撮像手段によって取得された第２の画像から、前記主観視点撮像装置上に配置された第２の指標の画像座標を検出する第２の検出手段と、
前記第２の検出手段が検出した前記第２の指標の画像座標と、前記客観視点撮像装置の位置姿勢とに基づいて、前記第２の指標の物理空間中における位置を拘束する直線を求め、前記第２の指標が当該直線上に存在するという拘束の元に、前記第１の検出手段が検出した前記第１の指標の画像座標に基づいて、前記主観視点撮像装置の位置及び姿勢を算出する位置姿勢算出手段と
を備えることを特徴とする。

本発明に係る構成により、対象物体の位置及び姿勢の計測を、位置及び姿勢の何れについても高い精度で実現することができる。

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

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

図３は、本実施形態に係る位置姿勢計測装置の概略構成を示す図である。なお図１と同じ部分については同じ番号、記号を付けており、その説明を省略する。図３に示したように、本実施形態に係る位置姿勢計測装置３００は、主観視点指標検出部１１０、客観視点指標検出部３１０、位置姿勢算出部３２０、及び客観視点カメラ３３０によって構成されており、計測対象である撮像装置１３０に接続されている。

客観視点カメラ３３０は、計測対象範囲内に撮像装置１３０が位置するときに、撮像装置１３０を撮像可能な位置に固定して配置されている。ここで、世界座標系における客観視点カメラ３３０の位置及び姿勢は、既知の値として予め客観視点指標検出部３１０に保持されているものとする。

現実空間中の複数の位置には、上記従来例と同様に、撮像装置１３０によって撮影するための指標として、世界座標系における位置が既知である主観視点指標Ｑｋが設定されている。また、撮像装置１３０上の複数の位置には、客観視点カメラ３３０によって撮影するための指標（以下、客観視点指標）として、主観視点カメラ座標系（撮像装置１３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）上における位置が既知である客観視点指標Ｐｋ（ｋ＝１，，，Ｋ２）が設定されている。

これらの指標は、位置及び姿勢を計測しようとする計測対象範囲内の各々の地点に撮像装置１３０を位置させた時に、撮像装置１３０によって取得される画像上において観測される主観視点指標と、客観視点カメラ３３０によって取得される画像上において観測される客観視点指標の総数が常に少なくとも３個以上となるように設置されていることが望ましい。図３の例は、３個の主観視点指標Ｑ１，Ｑ２，Ｑ３と２個の客観視点指標Ｐ１，Ｐ２が設定されており、そのうちの２個の主観視点指標Ｑ１，Ｑ３が撮像装置１３０の視野内に含まれており、２個の客観視点指標Ｐ１，Ｐ２が客観視点カメラ３３０の視野内に含まれている状況を示している。

なお、客観視点指標Ｐ^ｋは、主観視点指標Ｑ^ｋと同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが識別可能であるような指標であれば、何れの形態であってもよい。また主観視点指標、客観視点指標は共に、故意に設定されたものであっても良いし、故意に設定されたものではない、例えば、上述の通り、自然形状のものを用いても良い。

主観視点指標検出部１１０は、上記従来例と同様に、撮像装置１３０が撮影した主観視点画像を入力し、画像中に撮影されている主観視点指標Ｑ^ｋの画像座標を検出し、検出された主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを位置姿勢算出部３２０へと出力する。例えば図３の場合には、Ｎ＝２であり、識別子ｋ_１＝１，ｋ_２＝３とこれらに対応する画像座標ｕ^Ｑｋ１，ｕ^Ｑｋ２が出力される。

客観視点指標検出部３１０は、客観視点カメラ３３０が撮影した画像（以下、客観視点画像）を入力し、主観視点指標検出部１１０と同様な処理によって画像中に撮影されている客観視点指標Ｐ^ｋの画像座標を検出し、検出された客観視点指標Ｐ^ｋｍの画像座標ｕ^Ｐｋｍとその識別子ｋ^ｍを位置姿勢算出部３２０へと出力する。ここで、ｍ（ｍ＝１，，，Ｍ）は検出された指標夫々に付けられたインデックスであり、Ｍは検出された指標の総数を表している。例えば図３の場合には、Ｍ＝２であり、識別子ｋ_１＝１，ｋ_２＝２とこれらに対応する画像座標ｕ^Ｐｋ１，ｕ^Ｐｋ２が出力される。

位置姿勢算出部３２０は、検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと、既知な情報として予め保持している指標の世界座標ｘ_Ｗ ^Ｑｋｎとの対応関係と、検出された各々の客観視点指標Ｐ^ｋｍの画像座標ｕ^Ｐｋｍと、既知な情報として予め保持している主観視点カメラ座標系上における指標の位置ｘ_Ｃ ^Ｐｋｍとの対応関係に基づいて、撮像装置１３０の位置及び姿勢を示すパラメータを算出する。これにより、撮像装置１３０の位置及び姿勢の計測を行うことができる。

なお、図３に示した客観視点指標検出部３１０、位置姿勢算出部３２０、主観視点指標検出部１１０の夫々は独立した装置として扱っても良いし、夫々ソフトウェアとして１つもしくは複数のコンピュータにインストールし、夫々のコンピュータのＣＰＵにより実行することで、その機能を実現するようにしても良い。本実施形態では、各部（客観視点指標検出部３１０、位置姿勢算出部３２０、主観視点指標検出部１１０）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。

図１０は客観視点指標検出部３１０、位置姿勢算出部３２０、主観視点指標検出部１１０の夫々をソフトウェアとして実行するコンピュータの基本構成を示す図である。

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

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

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

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

１００７は外部記憶装置で、ハードディスクなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）や、客観視点指標検出部３１０、位置姿勢算出部３２０、主観視点指標検出部１１０の夫々として機能するソフトウェアのプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報もここに保存する。

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

１００９はＩ／Ｆで、撮像装置１３０、客観視点カメラ３３０を接続するためのものであって、夫々が撮像した画像はＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。１０１０は上述の各部を繋ぐバスである。

図４は、ＣＰＵ１００１が位置姿勢算出部３２０のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。なお、以下の処理を行う前段で、客観視点指標検出部３１０、位置姿勢算出部３２０、主観視点指標検出部１１０の夫々のソフトウェアのプログラムはＲＡＭ１００２に既にロードされているものとする。

位置姿勢算出部３２０では、上記従来例における位置姿勢算出部１２０と同様に、算出すべき撮像装置１３０の位置及び姿勢を、それぞれ３値ベクトル［ｘ ｙ ｚ］^Ｔ及び［ξ ψ ζ］^Ｔとして扱っている。このとき、求めるべき未知パラメータは、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］^Ｔと記述される。

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

ステップＳ４０１０において、位置姿勢算出部３２０は、主観視点指標検出部１１０において検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを入力する。

ステップＳ４０２０において、位置姿勢算出部３２０は、客観視点指標検出部３１０において検出された各々の客観視点指標Ｐ^ｋｍの画像座標ｕ^Ｐｋｍとその識別子ｋ_ｍを入力する。

ステップＳ４０３０において、位置姿勢算出部３２０は、入力された指標の総数（すなわちＮ＋Ｍ）が３以上であるかどうかの判定を行う。指標の総数が３に満たない場合には、ステップＳ４０４０からステップＳ４１２０によって実現されるｓの更新処理を行わずにステップＳ４１３０へと進む。

ステップＳ４０４０において、位置姿勢算出部３２０は、各々の主観視点指標Ｑ^ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を式１〜式４に基づいて算出する。すなわち、撮像装置１３０の位置及び姿勢が従前に求めた状態ベクトルｓに従っていると仮定して、撮像装置１３０と主観視点指標との位置姿勢関係に応じて、各々の主観視点指標の画像座標の推定値を求める。

ステップＳ４０５０において、位置姿勢算出部３２０は、各々の主観視点指標Ｑ^ｋｎに対して、画像座標の推定値ｕ^Ｑｋｎ＊と実測値ｕ^Ｑｋｎとの誤差Δｕ^Ｑｋｎを、式５に基づいて算出する。

ステップＳ４０６０において、位置姿勢算出部３２０は、各々の主観視点指標Ｑ^ｋｎに対して、式１の関数Ｆｃ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を、式６に基づいて算出する。

ステップＳ４０７０において、位置姿勢算出部３２０は、各々の客観視点指標Ｐ^ｋｍに対して、その画像座標の推定値ｕ^Ｐｋｍ＊を算出する。すなわち、撮像装置１３０の位置及び姿勢が従前に求めた状態ベクトルｓに従っていると仮定して、撮像装置１３０と客観視点指標との位置姿勢関係に応じて、各々の客観視点指標の画像座標の推定値を求める。

ｕ^Ｐｋｍ＊の算出は、既知な情報として予め保持している指標Ｐ^ｋｍ各々のカメラ座標（カメラ座標系における座標値）ｘ_Ｃ ^Ｐｋｍと、現在の状態ベクトルｓの関数

に基づいて行われる。
具体的には、関数Ｆ_Ｂ（）は、客観視点カメラ座標系（客観視点カメラ３３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）上における当該指標の座標ｘ_Ｂ ^Ｐｋｍを、ｘ_Ｃ ^Ｐｋｍとｓから求める次式、

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

によって構成されている。ここでｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ３３０の焦点距離であり、既知の値として予め保持されているものとする。また、Ｍ_ＷＢは客観視点カメラ座標系における座標を世界座標に変換する変換行列であり、既知の値として予め保持されている世界座標系における客観視点カメラ３３０の位置及び姿勢に基づいて、予め算出されているものとする。
ステップＳ４０８０において、位置姿勢算出部３２０は、各々の主観視点指標Ｐ^ｋｍに対して、画像座標の推定値ｕ^Ｐｋｍ＊と実測値ｕ^Ｐｋｍとの誤差Δｕ^Ｐｋｍを、次式によって算出する。

ステップＳ４０９０において、位置姿勢算出部３２０は、各々の主観視点指標Ｐ^ｋｍに対して、式１１の関数Ｆ_Ｂ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列Ｊ_ｕｓ ^Ｐｋｍ（＝∂ｕ／∂ｓ）を算出する。具体的には、式１３の右辺をカメラ座標ｘ_Ｂ ^Ｐｋｍの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｐｋｍ（＝∂ｕ／∂ｘ）と、式１２の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列Ｊ_ｘｓ ^Ｐｋｍ（＝∂ｘ／∂ｓ）を算出し、次式によってＪ_ｕｓ ^Ｐｋｍを算出する。

ステップＳ４１００において、位置姿勢算出部３２０は、以上のステップで算出した誤差Δｕ^Ｑｋｎ、ヤコビ行列Ｊ_ｕｓ ^Ｑｋｎ、誤差Δｕ_Ｐ ^ｋｍ、及びヤコビ行列Ｊ_ｕｓ ^Ｐｋｍに基づいて、誤差ベクトルＵ及び行列Φを定め、式９によって状態ベクトルｓの補正値Δｓを算出する。このとき、誤差ベクトルＵは、誤差Δｕ^Ｑｋｎ及びΔｕ^Ｐｋｍを垂直に並べた２（Ｎ＋Ｍ）次元のベクトル

として、また、行列Φは、ヤコビ行列Ｊ_ｕｓ ^Ｑｋｎ及びＪ_ｕｓ ^Ｐｋｍを垂直に並べた２（Ｎ＋Ｍ）行×６列の行列

として設定する。図３の例では、Ｍ＝２，Ｎ＝２であるので、Ｕは８次元ベクトル、Φは８行×６列の行列となる。
ステップＳ４１１０において、位置姿勢算出部３２０は、ステップＳ４１００において算出した補正値Δｓを用いて式１０によって状態ベクトルｓ、すなわち撮像装置１３０の位置及び姿勢を示すパラメータを補正し、得られた値をｓの新たな推定値とする。

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

ステップＳ４１２０において計算が収束したと判定されると、ステップＳ４１３０において、位置姿勢算出部３２０は、得られた状態ベクトルｓを撮像装置１３０の位置及び姿勢を示すパラメータとして出力する。なお、出力の形態はｓのうちの姿勢成分をオイラー角に変換したものでもよいし、ｓから算出したビューイング変換行列であってもよいし、他のいずれの位置姿勢記述方法によるものでもよい。

以上の処理によって、主観視点画像上における指標間の誤差を極小化するだけでなく、客観視点画像上における指標間の誤差をも極小化するような、撮像装置１３０の位置及び姿勢を得ることができる。これにより、主観視点画像、客観視点画像の何れかの画像のみを用いている場合よりも、撮像装置１３０の位置及び姿勢をより高い精度で計測し、そのパラメータを算出することができる。

以上によって、撮像装置の位置及び姿勢の計測が、位置と姿勢のいずれにおいても高い精度で実現される。

また、本実施形態では、主観視点画像と客観視点画像の双方を利用しているので、主観視点画像のみを用いる従来手法と比べて、情景中に配置する主観視点指標の総数を減少させることができる。

［第２の実施形態］
本実施形態に係る位置姿勢計測装置は、ステレオビデオシースルーＨＭＤ（Ｈｅａｄ Ｍｏｕｎｔ Ｄｉｓｐｌａｙ）に搭載された２台のカメラの位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図５は、本実施形態に係る位置姿勢計測装置の概略構成を示す図である。なお、図１，３と同じ部分については同じ番号、記号を付けている。図５に示したように、本実施形態に係る位置姿勢計測装置５００は、主観視点指標検出部１１０Ｌ，１１０Ｒ、客観視点指標検出部３１０、位置姿勢算出部５２０、及び客観視点カメラ３３０によって構成されている。

なお、主観視点指標検出部１１０Ｌ，１１０Ｒは夫々、図１に示した主観視点指標検出部１１０と同じものを左用、右用としたものである。主観視点指標検出部１１０Ｌ，１１０Ｒについての詳細は後述する。

主観視点指標Ｑ^ｋは第１の実施形態と同様に配置されており、また、客観視点カメラ３３０及び客観視点指標検出部３１０の機能は第１の実施形態と同様であるので、これらに関する詳細な説明は省略する。

ＨＭＤ５４０には、観察者の左右の視点に対応する現実空間の映像を撮影するために、２台のカメラ５５０Ｌ，５５０Ｒが内蔵されている。ここで、左カメラ座標系（カメラ５５０Ｌ上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）におけるカメラ５５０Ｒの位置及び姿勢は既知であるとする。また、ＨＭＤ５４０上の複数の位置には、左カメラ座標系における位置が既知である客観視点指標Ｐ^ｋ（ｋ＝１，，，Ｋ_２）が設定されている。

カメラ５５０Ｌ，５５０Ｒによって撮影された左右の主観視点画像は、各々主観視点指標検出部１１０Ｌ，１１０Ｒへと入力される。

主観視点指標検出部１１０Ｌ及び１１０Ｒは、第１の実施形態における主観視点指標検出部１１０と同様な処理によって、各々が入力する左右の主観視点画像に対して主観視点指標の検出処理を行い、検出された指標の画像座標とその識別子を位置姿勢算出部５２０へと出力する。以下では、主観視点指標検出部１１０Ｌ及び１１０Ｒによって検出された指標の画像座標を各々ｕ_Ｌ ^ＱｋＬｎ及びｕ_Ｒ ^ＱｋＲｎと表記する。また、検出された指標の総数を各々Ｎ_Ｌ及びＮ_Ｒと表記する。

図５の例では、３個の主観視点指標Ｑ^１，Ｑ^２，Ｑ^３が配置されており、そのうちの２個の主観視点指標Ｑ^１，Ｑ^２がカメラ５５０Ｌの視野内に、２個の主観視点指標Ｑ^１，Ｑ^３がカメラ５５０Ｌの視野内に含まれている状況を示している。このとき、Ｎ_Ｌ＝２であり、主観視点指標検出部１１０Ｌは、識別子ｋ_Ｌ１＝１，ｋ_Ｌ２＝２とこれらに対応する画像座標ｕ^ＱｋＬ１，ｕ^ＱｋＬ２を出力する。同様に、Ｎ_Ｒ＝２であり、主観視点指標検出部１１０Ｒは、識別子ｋ_Ｒ１＝１，ｋ_Ｒ２＝３とこれらに対応する画像座標ｕ^ＱｋＲ１，ｕ^ＱｋＲ２を出力する。

位置姿勢算出部５２０は、検出された主観視点指標Ｑ^ｋｎの画像座標ｕ^{ＬＱｋＬｎ}及びｕ^{ＲＱｋＲｎ}と客観視点指標Ｐ^ｋｍの画像座標ｕ_Ｐｋｍに基づいて、２台のカメラ５５０Ｌ，５５０Ｒの位置及び姿勢を算出する。

なお、図５に示した客観視点指標検出部３１０、位置姿勢算出部５２０、主観視点指標検出部１１０Ｒ、１１０Ｌの夫々は独立した装置として扱っても良いし、夫々ソフトウェアとして１つもしくは複数のコンピュータにインストールし、夫々のコンピュータのＣＰＵにより実行することで、その機能を実現するようにしても良い。本実施形態では、各部（客観視点指標検出部３１０、位置姿勢算出部５２０、主観視点指標検出部１１０Ｒ、１１０Ｌ）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。またこのコンピュータの基本構成は図１０に示した構成とする。

また本実施形態に関する説明において、既知の情報は全て外部記憶装置１００７に保存されているものとする。

位置姿勢算出部５２０の処理工程は第１の実施形態における位置姿勢算出部３２０と同様であり、カメラ５５０Ｌの位置及び姿勢を未知パラメータとして、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］^Ｔを算出する。ただし、左右双方の主観視点指標の検出結果を用いるため、以下の点で第１の実施形態とは処理が異なっている。

すなわち、誤差ベクトルＵを、誤差Δｕ_Ｌ ^ＱｋＬｎ，Δｕ_Ｒ ^ＱｋＲｎ，及びΔｕ^Ｐｋｍを垂直に並べた２（Ｎ_Ｌ＋Ｎ_Ｒ＋Ｍ）次元のベクトルとして設定する。また、行列Φを、ヤコビ行列Ｊ_ｕＬｓ ^ＱｋＬｎ，Ｊ_ｕＲｓ ^ＱｋＲｎ，及びＪ_ｕｓ ^Ｐｋｍを垂直に並べた２（Ｎ_Ｌ＋Ｎ_Ｒ＋Ｍ）行×６列の行列として設定する。図５の例では、Ｍ＝２，Ｎ_Ｌ＝２，Ｎ_Ｒ＝２であるので、Ｕは１２次元ベクトル、Φは１２行×６列の行列となる。

ここで、主観視点指標に関する誤差及びヤコビ行列の算出は、カメラ５５０Ｌ，５５０Ｒの各々の固有パラメータ（焦点距離）に基づいて、第１の実施形態における主観視点指標に関する誤差及びヤコビ行列の算出（ステップＳ４０４０〜Ｓ４０６０）と同様の処理によって行う。ただし、カメラ５５０Ｒに関しては、右カメラ座標系（カメラ５５０Ｒ上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における指標の座標ｘ_ＣＲ ^ＱｋＲｎをｓから求める式として、式２の代わりに次式を使用する。

ここで、Ｍ_ＣＬＣＲは右カメラ座標系から左カメラ座標系への変換行列であり、既知の値として予め保持されている左カメラ座標系におけるカメラ５５０Ｒの位置及び姿勢に基づいて、予め算出されているものとする。
以上によってｓが求まれば、カメラ５５０Ｒの位置及び姿勢は、ｓ及び既知の値であるＭ_ＣＬＣＲを用いることで、容易に算出できる。

本実施形態における位置姿勢計測装置５００の出力は、カメラ５５０Ｌ及び５５０Ｒそれぞれの位置及び姿勢を表すビューイング変換行列であってもよいし、カメラ５５０Ｌ及び５５０Ｒそれぞれの位置及び姿勢を表す２つの６次元ベクトルであってもよい。また、位置姿勢計測装置５００の出力を使用する装置（本実施形態の場合は、不図示の画像生成装置）がカメラ５５０Ｌとカメラ５５０Ｒの位置関係（例えば前述の行列Ｍ_ＣＬＣＲ）を保持している場合には、一方のカメラの位置及び姿勢を表すデータのみを出力してもよい。

以上によって、ステレオビデオシースルーＨＭＤに搭載された２台のカメラの位置及び姿勢の計測が、位置と姿勢のいずれにおいても高い精度で実現される。

［第３の実施形態］
第１の実施形態は１台の客観視点カメラを使用していたが、客観視点カメラの台数は１台に限定されるものではない。複数台の客観視点カメラを用いて位置姿勢計測装置を構成することることで、計測範囲の拡大や計測の安定性向上が期待できる。

図６は、本実施形態に係る位置姿勢計測装置の概略構成を示す図である。なお、図１，３と同じ部分については同じ番号、記号を付けている。図６に示したように、本実施形態に係る位置姿勢計測装置６００は、客観視点カメラ３３０ａ〜３３０ｄ、客観視点指標検出部３１０ａ〜３１０ｄ、主観視点指標検出部１１０、及び位置姿勢算出部６２０によって構成されている。

なお、主観視点指標Ｑ及び客観視点指標Ｐは第１の実施形態と同様に配置されており、撮像装置１３０が撮影した主観視点映像が、主観視点指標検出部１１０に入力される。主観視点指標検出部１１０の機能は第１の実施形態と同様であるので、説明は省略する。

客観視点カメラ３３０ａ〜３３０ｄは、計測対象範囲内に撮像装置１３０が位置するときに撮像装置１３０を撮像可能な位置に固定して配置されている。客観視点カメラ３３０ａ〜３３０ｄの配置は、隠蔽に対する安定性を向上させることを目的として、客観視点指標Ｐが同時に複数の客観視点カメラによって観察されるように、それぞれがオーバーラップする領域を撮影するように設定してもよいし、計測対象範囲の拡大を目的として、互いに異なる領域を撮影するように設定してもよい。尚、世界座標系における客観視点カメラ３３０ａ〜３３０ｄの各々の位置及び姿勢は、既知の値として予め保持されているものとする。

客観視点指標検出部３１０ａ〜３１０ｄの各々は、第１の実施形態における客観視点指標検出部３１０と同様な処理によって、客観視点カメラ３３０ａ〜３３０ｄが撮影した夫々の客観視点画像を入力し、客観視点指標の検出処理を行い、検出された指標の画像座標とその識別子を位置姿勢算出部６２０へと出力する。以下では、客観視点指標検出部３１０ａ〜３１０ｄによって検出された指標の画像座標を各々ｕ_ａ ^Ｐｋａｎ，ｕ_ｂ ^Ｐｋｂｎ，ｕ_ｃ ^Ｐｋｃｎ，ｕ_ｄ ^Ｐｋｄｎと表記する。また、検出された指標の総数を各々Ｍ_ａ，Ｍ_ｂ，Ｍ_ｃ，Ｍ_ｄと表記する。

位置姿勢算出部６２０は、検出された各々の主観視点指標の画像座標とこの指標の世界座標との対応関係と、客観視点指標検出部３１０ａ〜３１０ｄの各々によって検出された各々の客観視点指標の画像座標と主観視点カメラ座標系上における指標の位置との対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。

なお、図６に示した客観視点指標検出部３１０ａ〜３１０ｄ、主観視点指標検出部１１０、及び位置姿勢算出部６２０の夫々は独立した装置として扱っても良いし、夫々ソフトウェアとして１つもしくは複数のコンピュータにインストールし、夫々のコンピュータのＣＰＵにより実行することで、その機能を実現するようにしても良い。本実施形態では、各部（客観視点指標検出部３１０ａ〜３１０ｄ、主観視点指標検出部１１０、及び位置姿勢算出部６２０）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。またこのコンピュータの基本構成は図１０に示した構成とする。

また本実施形態に関する説明において、既知の情報は全て外部記憶装置１００７に保存されているものとする。

位置姿勢算出部６２０の処理工程は第１の実施形態における位置姿勢算出部３２０と同様であり、撮像装置１３０の位置及び姿勢を未知パラメータとして、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］^Ｔを算出する。ただし、複数台の客観視点カメラを用いるために、以下の点で第１の実施形態とは処理が異なっている。

すなわち、誤差ベクトルＵを、誤差Δｕ^Ｑｋｎ及びΔｕ_ａ ^Ｐｋａｍ，Δｕ_ｂ ^Ｐｋｂｍ，Δｕ_ｃ ^Ｐｋｃｍ，Δｕ_ｄ ^Ｐｋｄｍを垂直に並べた２（Ｎ＋Ｍ_ａ＋Ｍ_ｂ＋Ｍ_ｃ＋Ｍ_ｄ）次元のベクトルとして設定する。また、行列Φを、ヤコビ行列Ｊ_ｕｓ ^Ｑｋｎ及び、Ｊ_ｕａｓ ^Ｐｋａｍ，Ｊ_ｕｂｓ ^Ｐｋｂｍ，Ｊ_ｕｃｓ ^Ｐｋｃｍ，Ｊ_ｕｄｓ ^Ｐｋｄｍを垂直に並べた２（Ｎ＋Ｍ_ａ＋Ｍ_ｂ＋Ｍ_ｃ＋Ｍ_ｄ）行×６列の行列として設定する。ここで、客観視点指標に関する誤差及びヤコビ行列の算出は、客観視点カメラ３３０ａ〜３３０ｄの各々の固有パラメータ（世界座標系における位置や焦点距離）に基づいて、第１の実施形態における客観視点指標に関する誤差及びヤコビ行列の算出（ステップＳ４０７０〜Ｓ４０９０）と同様の処理によって行う。

以上のように、複数の客観視点カメラを用いることで、撮像装置が広い範囲を移動した場合や、いずれかの客観視点カメラに対して撮像装置が隠蔽されるような状況下においても、位置及び姿勢を算出するのに十分なデータを得ることが可能となり、移動可能範囲の拡大あるいは隠蔽等に対する安定性の向上が実現される。

なお、本実施形態における位置姿勢計測装置は４台の客観視点カメラを用いていたが、客観視点カメラの台数はこれに限定されるものではなく、何れの台数であってもよいことはいうまでもない。

［第４の実施形態］
第１乃至第３の実施形態は、空間中を移動する撮像手段自体の位置及び姿勢を計測することを目的としていた。本実施形態に係る位置姿勢計測装置は、任意の対象物体の位置及び姿勢を計測することを目的とした位置姿勢計測装置であって、第１の実施形態の位置姿勢計測装置に主観視点画像を撮像するカメラを追加した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図７は、本実施形態に係る位置姿勢計測装置の構成を示す図である。図１，３と同じ部分については同じ番号を付けており、その説明を省略する。図７に示したように、本実施形態に係る位置姿勢計測装置７００は、主観視点指標検出部１１０、客観視点指標検出部３１０、位置姿勢算出部７２０、客観視点カメラ３３０、及び主観視点カメラ７３０によって構成されている。

主観視点カメラ７３０は、計測対象物体７１０に固定して装着されている。ここで、主観視点カメラ座標系（主観視点カメラ７３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における計測対象物体７１０の位置及び姿勢は既知であるとする。

現実空間中の複数の位置には、主観視点カメラ７３０によって撮影するための指標として、世界座標系における位置が既知である主観視点指標Ｑ^ｋ（ｋ＝１，，，Ｋ_１）が配置されている。また、主観視点カメラ７３０上の複数の位置には、主観視点カメラ座標系上における位置が既知である客観視点指標Ｐｋ（ｋ＝１，，，Ｋ_２）が配置されている。

これらの指標は、位置及び姿勢を計測しようとする計測対象範囲内の各々の地点に計測対象物体７１０を位置させた時に、主観視点カメラ７３０によって取得される画像上において観測される主観視点指標と、客観視点カメラ３３０によって取得される画像上において観測される客観視点指標の総数が常に少なくとも３個以上となるように設置されていることが望ましい。

客観視点カメラ３３０、主観視点指標検出部１１０、及び客観視点指標検出部３１０の機能は第１の実施形態と同様であるので、詳細な説明は省略する。ただし、主観視点カメラ７３０によって取得された画像が主観視点画像として主観視点指標検出部１１０に入力されるという点が、第１の実施形態とは異なっている。

位置姿勢算出部７２０は、世界座標系における主観視点カメラ７３０の位置及び姿勢を未知のベクトルｓとして、図４で示した第１の実施形態における位置姿勢算出部３２０と同様な処理により、ｓを算出する。

位置姿勢算出部７２０はさらに、上記によって算出した世界座標系における主観視点カメラ７３０の位置及び姿勢と、既知の値である主観視点カメラ座標系における計測対象物体７１０の位置及び姿勢に基づいて、座標変換行列の積により世界座標系における計測対象物体７１０の位置及び姿勢を算出し、これを出力する。

以上によって、任意の計測対象物体の位置及び姿勢の計測が、位置と姿勢のいずれにおいても高い精度で実現される。

なお、図７に示した主観視点指標検出部１１０、客観視点指標検出部３１０、位置姿勢算出部７２０の夫々は独立した装置として扱っても良いし、夫々ソフトウェアとして１つもしくは複数のコンピュータにインストールし、夫々のコンピュータのＣＰＵにより実行することで、その機能を実現するようにしても良い。本実施形態では、各部（主観視点指標検出部１１０、客観視点指標検出部３１０、位置姿勢算出部７２０）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。またこのコンピュータの基本構成は図１０に示した構成とする。

また本実施形態に関する説明において、既知の情報は全て外部記憶装置１００７に保存されているものとする。

なお、本実施形態における位置姿勢算出部７２０は主観視点カメラ７３０の位置及び姿勢を状態ベクトルｓとしていたが、計測対象物体７１０の位置及び姿勢を状態ベクトルｓとして直接求めてもよい。この場合、ｓから主観視点指標の主観視点カメラ座標ｘ_Ｃ ^Ｑｋｎを求める式２と、客観視点指標の客観視点カメラ座標系における座標ｘ_Ｂ ^Ｐｋｍを求める式１２を、それぞれ、

に置き換えればよい。ここで、Ｍ_ＣＯは計測物体座標系（計測対象物体７１０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）から主観視点カメラ座標系への変換行列であり、既知の値として予め保持されている主観視点カメラ座標系における計測対象物体７１０の位置及び姿勢に基づいて、予め算出されているものとする。一方、Ｍ_ＷＯ（ｓ）はｓによって定まるモデリング変換行列（すなわち、物体座標系における座標を世界座標系における座標に変換する行列）であり、式４と同様にｓによって定義される。
［第５の実施形態］
第１乃至第４の実施形態において、位置姿勢算出部３２０，５２０，６２０，７２０は、いずれも、位置と姿勢を表す６値の状態ベクトルｓを未知数として、主観視点指標および客観視点指標の検出座標と、それらの点の計算値との間の誤差の総和を最小化するようなｓを求めていた。しかし、主観視点画像と客観視点画像の双方から得られる幾何的な拘束条件を併用することで計測対象物体の位置及び姿勢を取得する方法はこれに限定されるものではない。本実施形態に係る位置姿勢計測装置は、全体の誤差を最小化する手法とは異なる手法を用いた位置姿勢算出部をその構成要素として有していることを特徴とする。

本実施形態に係る位置姿勢計測装置の構成は基本的には第１の実施形態と同様であるが、位置姿勢算出部３２０が第１の実施形態のそれとは異なる位置姿勢算出部３２０'に置換された構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

本実施形態では、各部（客観視点指標検出部３１０、位置姿勢算出部３２０'、主観視点指標検出部１１０）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。またこのコンピュータの基本構成は図１０に示した構成とする。また本実施形態に関する説明において、既知の情報は全て外部記憶装置１００７に保存されているものとする。

図１１は、ＣＰＵ１００１が位置姿勢算出部３２０'のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。なお、以下の処理を行う前段で、客観視点指標検出部３１０、位置姿勢算出部３２０'、主観視点指標検出部１１０の夫々のソフトウェアのプログラムはＲＡＭ１００２に既にロードされているものとする。

ステップＳ１１０００において、位置姿勢算出部３２０'は、客観視点指標検出部３１０において検出された客観視点指標Ｐの画像座標ｕ^Ｐａ，ｕ^Ｐｂを入力する。本実施形態では、以降の処理に使用される客観視点指標は１点のみである。

また、複数の客観視点指標の画像座標が入力された場合（すなわち、撮像装置１３０上に２点以上の客観視点指標が設定されていて、かつ、それらのうちの複数が客観視点指標検出部３１０によって検出された場合）には、位置姿勢算出部３２０'は、適当な１点を客観視点指標Ｐの画像座標ｕ^Ｐとして選択する。

ステップＳ１１０１０において、位置姿勢算出部３２０'は、画像座標ｕ^Ｐに基づいて、世界座標系における客観視点指標Ｐの位置を拘束する一本の直線を表すパラメータを算出する。すなわち、世界座標系における直線の傾き（方向ベクトル）ｈｘ，ｈｙ，ｈｚを、

によって画像座標ｕ^Ｐに基づいて算出し、算出したこのｈｘ，ｈｙ，ｈｚを直線のパラメータとする。ここで、ｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ３３０の焦点距離であって、既知の値として予め外部記憶装置１００７に保持されているものとする。このとき、世界座標系における直線上の点は、媒介変数τの関数として

のように表すことができる。ここで、ｘ_ＷＢ，ｙ_ＷＢ，ｚ_ＷＢは世界座標系における客観視点カメラ３３０の位置であって、既知の値として予め外部記憶装置１００７に保持されているものとする。
従って式２２で表される直線は、世界座標系における客観視点カメラ３３０の位置と世界座標系における客観視点指標Ｐの位置とを通る直線であって、世界座標系における客観視点指標Ｐの位置は、上記媒介変数τが適当な値を取ることにより得られるものである。

以下、求めるべき未知パラメータを、世界座標系における客観視点指標Ｐの位置を定める媒介変数τと、撮像装置１３０の姿勢を表す３値（ξ，ψ，ζ）をあわせた４値の状態ベクトルｓ＝［τ ξ ψ ζ］^Ｔによって記述する。

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

ステップＳ１１０３０において、位置姿勢算出部３２０'は、主観視点指標検出部１１０において検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを入力する。

ステップＳ１１０４０において、位置姿勢算出部３２０'は、入力された主観視点指標数が２以上であるかどうかの判定を行う。指標の総数Ｎが２に満たない場合には、ステップＳ１１０５０からステップＳ１１１００によって実現されるｓの更新処理を行わずにステップＳ１１１１０へと進む。

ステップＳ１１０５０において、位置姿勢算出部３２０'は、各々の主観視点指標Ｑ^ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を式１〜式４に基づいて算出する。ただし、本実施形態では、式４におけるｘ，ｙ，ｚ（世界座標系における撮像装置１３０の位置を表している）がｓの構成要素として含まれていないので、次式により、ｓに基づいてこれらの値を算出する。

ここで、ｘ_Ｃ ^Ｐは、主観視点カメラ座標系における指標Ｐの座標値であり、既知な情報として予め外部記憶装置１００７に保持されている。また、Ｒ_ＷＣ（ｓ）は世界座標系における撮像装置１３０の姿勢を表す回転行列であり、ｓによって次式のように定義される。

すなわちステップＳ１１０５０では、撮像装置１３０の位置及び姿勢が従前に求めた状態ベクトルｓに従っていると仮定して、撮像装置１３０と主観視点指標との位置姿勢関係に応じて、各々の主観視点指標の画像座標の推定値を求める。
次に、ステップＳ１１０６０において、位置姿勢算出部３２０'は、各々の主観視点指標Ｑ^ｋｎに対して、画像座標の推定値ｕ^Ｑｋｎ＊と実測値ｕ^Ｑｋｎとの誤差Δｕ^Ｑｋｎを、式５に基づいて算出する。

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

ステップＳ１１０８０において、位置姿勢算出部３２０'は、式７によって２Ｎ次元の誤差ベクトルＵを、式８によって行列Φ（２Ｎ行×４列）を定め、式９によって状態ベクトルｓの補正値Δｓを算出する。

ステップＳ１１０９０において、位置姿勢算出部３２０'は、ステップＳ１１０８０において算出した補正値Δｓを用いて式１０によって状態ベクトルｓを補正し、得られた値をｓの新たな推定値とする。

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

ステップＳ１１１１０において、位置姿勢算出部３２０'は、撮像装置１３０の位置及び姿勢の情報を出力する。位置及び姿勢の情報は、例えば、ｓから算出したモデリング変換行列の形態によって出力する。なお、得られたｓから、世界座標系における撮像装置１３０の位置を式２３に基づいて算出し、ｓに含まれる姿勢を表す３値と共に位置および姿勢を表す６値のデータとして出力してもよいし、姿勢をオイラー角に変換した後に出力してもよいし、他のいずれの位置姿勢記述方法によって記述した位置及び姿勢の情報を出力してもよい。

以上の処理によって、客観視点カメラ３３０から得られる客観視点指標が存在すべき直線を拘束条件として、その拘束条件のもとで、主観視点画像上における主観視点指標の誤差を最小とするような撮像装置１３０の位置及び姿勢を得ることができる。

なお、本実施形態による位置及び姿勢の計測結果は、第１の実施形態による位置及び姿勢の計測結果と比較して、客観視点カメラ３３０から得られる情報を優先的に信頼したものとなる。したがって、客観視点カメラ３３０から得られる情報の信頼性が撮像装置１３０から得られる情報より相対的に勝っている状況、例えば、高解像度の客観視点カメラを利用可能な場合や、検出精度の非常に高いマーカを客観視点指標としてのみ利用可能な場合等において、本実施形態に係る位置姿勢計測装置は、第１の実施形態と比して有効に機能する。

［第６の実施形態］
本実施形態に係る位置姿勢計測装置は、第５の実施形態の位置姿勢計測装置にさらにもう１台の客観視点カメラを追加し、且つ位置姿勢算出部３２０’の代わりに、位置姿勢算出部３２０’とは異なる動作により撮像装置１３０の位置及び姿勢の情報を出力する位置姿勢算出部８２０を用いた構成を有するものである。

以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図８は、本実施形態に係る位置姿勢計測装置の構成を示す図である。なお、図１，３と同じ部分については同じ番号、記号を付けており、その説明を省略する。図８に示したように、本実施形態における位置姿勢計測装置８００は、客観視点カメラ３３０ａ，３３０ｂ、主観視点指標検出部１１０、客観視点指標検出部３１０ａ，３１０ｂ、及び位置姿勢算出部８２０によって構成されており、計測対象である撮像装置１３０に接続されている。

撮像装置１３０上には、主観視点カメラ座標系上における位置が既知である客観視点指標Ｐが配置されている。この指標は、位置及び姿勢を計測しようとする計測対象範囲内の各々の地点に撮像装置１３０を位置させた時に、客観視点カメラ３３０ａ，３３０ｂの各々によって取得される画像上において常に観測されるように設置されていることが望ましい。

客観視点カメラ３３０ａ，３３０ｂは、計測対象範囲内に撮像装置１３０が位置するときに客観視点指標Ｐを常に撮像可能な位置に、固定して配置されている。ここで、世界座標系における客観視点カメラ３３０ａ及び３３０ｂの位置及び姿勢は、既知の値として予め保持されているものとする。

現実空間中の複数の位置には、第１の実施形態と同様に主観視点指標Ｑ^ｋが配置されている。これらの指標は、位置及び姿勢を計測しようとする計測対象範囲内の各々の地点に撮像装置１３０を位置させた時に、撮像装置１３０によって取得される画像上において観測される総数Ｎが常に少なくとも２個以上となるように設置されていることが望ましい。図８の例は、３個の主観視点指標Ｑ^１，Ｑ^２，Ｑ^３が配置されており、そのうちの２個の主観視点指標Ｑ^１，Ｑ^３が撮像装置１３０の視野内に含まれている状況を示している。

客観視点指標検出部３１０ａは、客観視点カメラ３３０ａが撮影した客観視点画像を入力し、上記従来例と同様の処理によって客観視点指標Ｐの画像座標を検出し、その画像座標ｕ^Ｐａを位置姿勢算出部８２０へと出力する。同様に、客観視点指標検出部３１０ｂは、客観視点カメラ３３０ｂが撮影した客観視点画像を入力し、上記従来例と同様の処理によって客観視点指標Ｐの画像座標を検出し、その画像座標ｕ^Ｐｂを位置姿勢算出部８２０へと出力する。

位置姿勢算出部８２０は、主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎと、客観視点指標Ｐの画像座標ｕ^Ｐａ及びｕ^Ｐｂを入力として、撮像装置１３０の位置及び姿勢を算出し、これを出力する。

なお、図８に示した主観視点指標検出部１１０、客観視点指標検出部３１０ａ，３１０ｂ、及び位置姿勢算出部８２０の夫々は独立した装置として扱っても良いし、夫々ソフトウェアとして１つもしくは複数のコンピュータにインストールし、夫々のコンピュータのＣＰＵにより実行することで、その機能を実現するようにしても良い。本実施形態では、各部（主観視点指標検出部１１０、客観視点指標検出部３１０ａ，３１０ｂ、及び位置姿勢算出部８２０）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。またこのコンピュータの基本構成は図１０に示した構成とする。

また本実施形態に関する説明において、既知の情報は全て外部記憶装置１００７に保存されているものとする。

図９は、ＣＰＵ１００１が位置姿勢算出部８２０のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。なお、以下の処理を行う前段で、主観視点指標検出部１１０、客観視点指標検出部３１０ａ，３１０ｂ、及び位置姿勢算出部８２０の夫々のソフトウェアのプログラムはＲＡＭ１００２に既にロードされているものとする。

ステップＳ９０００において、位置姿勢算出部８２０は、客観視点指標検出部３１０ａ及び３１０ｂにおいて検出された客観視点指標Ｐの画像座標ｕ^Ｐａ，ｕ^Ｐｂを入力する。

ステップＳ９０１０において、位置姿勢算出部８２０は、三角測量の原理に基づいて、画像座標ｕ^Ｐａ及びｕ^Ｐｂを用いて、世界座標系における客観視点指標Ｐの位置ｘ_Ｗ ^Ｐを算出する。

以下、求めるべき未知パラメータを、算出すべき撮像装置１３０の姿勢を表す３値の状態ベクトルｓ＝［ξ ψ ζ］^Ｔによって記述する。

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

ステップＳ９０３０において、位置姿勢算出部８２０は、主観視点指標検出部１１０において検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを入力する。

ステップＳ９０４０において、位置姿勢算出部８２０は、入力された主観視点指標数が２以上であるかどうかの判定を行う。指標の総数Ｎが２に満たない場合には、ステップＳ９０５０からステップＳ９１００によって実現されるｓの更新処理を行わずにステップＳ９１１０へと進む。

ステップＳ９０５０において、位置姿勢算出部８２０は、各々の主観視点指標Ｑ^ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を式１〜式４に基づいて算出する。ただし、本実施形態では、式４におけるｘ，ｙ，ｚ（世界座標系における撮像装置１３０の位置を表している）がｓの構成要素として含まれていないので、次式により、ｓに基づいてこれらの値を算出する。

ここで、ｘ_Ｗ ^ＰはステップＳ９０１０において算出された世界座標系における客観視点指標Ｐの位置、ｘ_Ｃ ^Ｐは既知の値である主観視点カメラ座標系における客観視点指標Ｐの位置を表している。また、Ｒ_ＷＣ（ｓ）は世界座標系における撮像装置１３０の姿勢を表す回転行列であり、式２４によって定義される。
ステップＳ９０６０において、位置姿勢算出部８２０は、各々の主観視点指標Ｑ^ｋｎに対して、画像座標の推定値ｕ^Ｑｋｎ＊と実測値ｕ^Ｑｋｎとの誤差Δｕ^Ｑｋｎを、式５に基づいて算出する。

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

ステップＳ９０８０において、位置姿勢算出部８２０は、式７によって２Ｎ次元の誤差ベクトルＵを、式８によって行列Φ（２Ｎ行×３列）を定め、式９によって状態ベクトルｓの補正値Δｓを算出する。

ステップＳ９０９０において、位置姿勢算出部８２０は、ステップＳ９０８０において算出した補正値Δｓを用いて式１０によって状態ベクトルｓを補正し、得られた値をｓの新たな推定値とする。

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

ステップＳ９１１０において、位置姿勢算出部８２０は、撮像装置１３０の位置及び姿勢の情報を出力する。位置及び姿勢の情報は、例えば、ｓから算出したモデリング変換行列の形態によって出力する。なお、得られたｓから式２５に基づいて世界座標系における撮像装置１３０の位置を算出して、姿勢を表すｓそのものと共に６値のデータとして出力してもよいし、姿勢をオイラー角に変換した後に出力してもよいし、他のいずれの位置姿勢記述方法によって記述した位置及び姿勢の情報を出力してもよい。

以上の処理によって、客観視点カメラ３３０を用いて求めた客観視点指標の位置を拘束条件として、その拘束条件のもとで、主観視点画像上における主観視点指標の誤差を最小とするような撮像装置１３０の位置及び姿勢を得ることができる。したがって、第５の実施形態と同様に、客観視点カメラ３３０から得られる情報の信頼性が撮像装置１３０から得られる情報より相対的に勝っている状況、例えば、高解像度の客観視点カメラを利用可能な場合や、検出精度の非常に高いマーカを客観視点指標としてのみ利用可能な場合等において、本実施形態に係る位置姿勢計測装置は、第１の実施形態と比して有効に機能する。

［第７の実施形態］
第６の実施形態では、２台の客観視点カメラによって、客観視点指標Ｐの世界座標ｘ_Ｗ ^Ｐを算出していたが、ある測点の３次元位置を計測可能な手段であれば、何れの手段であっても、第６の実施形態における客観視点カメラの代わりに用いることができる。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図１２は、本実施形態に係る位置姿勢計測装置の構成を示す図である。図８と同じ部分については同じ番号、記号を付けており、その説明を省略する。図１２に示したように、本実施形態における位置姿勢計測装置１２００は、主観視点指標検出部１１０、位置センサ１２３０、測点１２４０、位置入力部１２１０、及び位置姿勢算出部８２０'によって構成されており、計測対象である撮像装置１３０に接続されている。

位置センサ１２３０は、所定の測点の３次元位置を計測するセンサであり、例えば、光学式の３次元位置センサであるＮｏｒｔｈｅｒｎ Ｄｉｇｉｔａｌ Ｉｎｃ.のＯＰＴＯＴＲＡＫによって構成されている。

測点１２４０は位置センサ１２３０によって計測される対象となる点であり、ＯＰＴＯＴＲＡＫの場合、位置センサ１２３０からの制御信号に応じて点滅する赤外ＬＥＤによって構成されている。測点１２４０は、第６の実施形態における客観視点指標Ｐの代わりに、撮像装置１３０に装着されている。測点１２４０は、位置及び姿勢を計測しようとする計測対象範囲内の各々の地点に撮像装置１３０を位置させた時に、位置センサ１２３０の計測範囲に常に含まれるように設置されていることが望ましい。また、主観視点カメラ座標系における測点１２４０の位置は既知であるものとして外部記憶装置１００７にデータとして記憶されており、必要に応じてＲＡＭ１００２に読み出されて用いられるものである。

位置入力部１２１０は、位置センサ１２３０が出力する測点１２４０の３次元座標を入力し、これを位置姿勢算出部８２０'へと出力する。なお、位置センサ１２３０が出力する測点１２４０の３次元座標が、位置センサ１２３０自身が定義するセンサ座標系における座標である場合には、位置入力部１２１０は、これを世界座標系における座標に変換した後に位置姿勢算出部８２０'へと出力する。センサ座標系から世界座標系への座標変換は、世界座標系における位置センサ１２３０の位置及び姿勢が既知の情報として与えられている場合に、公知の方法によって行うことができる。世界座標系における位置センサ１２３０の位置及び姿勢は既知の情報として外部記憶装置１００７にデータとして記憶されており、必要に応じてＲＡＭ１００２に読み出されて用いられる。

位置姿勢算出部８２０'は、位置入力部１２１０から入力される測点１２４０の３次元座標と、主観視点指標検出部１１０から入力される主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎを用いて撮像装置１３０の位置及び姿勢を算出し、これを出力する。

なお、位置姿勢算出部８２０'の動作は、第６の実施形態における位置姿勢算出部８２０とほぼ同様である。ただし、客観視点指標Ｐの画像座標ｕ^Ｐａ，ｕ^Ｐｂを入力し、世界座標系における客観視点指標Ｐの位置ｘ_Ｗ ^Ｐを算出するという位置姿勢算出部８２０の工程（ステップＳ９０００，Ｓ９０１０）の代わりに、位置姿勢算出部８２０'は、世界座標系における測点１２４０の位置ｘ_Ｗ ^Ｐを位置入力部１２１０から入力するという工程を有している。また、式２５におけるｘ_Ｗ ^Ｐとして、世界座標系における測点１２４０の位置を用いる点が第６の実施形態とは異なっている。

以上の処理によって、位置センサ１２３０を用いて求めた測点１２４０の位置を拘束条件として、その拘束条件のもとで、主観視点画像上における主観視点指標の誤差を最小とするような撮像装置１３０の位置及び姿勢を得ることができる。これにより、位置センサの利用によって得られる位置精度の安定性と、主観視点画像の利用によって得られる画像上での位置合わせ精度の安定性を両立した、撮像装置１３０の位置及び姿勢の計測が実現される。

なお、位置センサ１２３０として用いるセンサは、測点の３次元位置を計測できるものであれば何れのセンサを用いても良い。例えば、超音波ビーコンの３次元位置を測定する超音波センサ（例えば、ＩｎｔｅｒＳｅｎｓｅ社のＩＳ−６００）を位置センサ１２３０として用いても良い。この場合、測点１２４０として、超音波ビーコンが撮像装置１３０に装着されることになる。

なお、本実施形態は、測点を装着した計測対象物体に撮像装置１３０を装着し、この計測対象物体、もしくは撮像装置１３０の位置姿勢を計測する場合にも適用可能であることは明らかである。なお、撮像装置１３０の位置姿勢を求める場合には、撮像装置１３０と計測対象物体との位置姿勢関係は予め測定しておき、メモリに保持させておくことが必要となる。

［第８の実施形態］
第７の実施形態では、位置センサとして光学センサや超音波センサを用いていたが、屋外での利用を目的とする場合には、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を位置センサとして用いることが可能である。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図１３は、本実施形態に係る位置姿勢計測装置の構成を示す図である。図１２と同じ部分については同じ番号、記号を付けており、その説明を省略する。図１３に示したように、本実施形態における位置姿勢計測装置１３００は、主観視点指標検出部１１０、ＧＰＳ受信機１３３０、位置入力部１３１０、及び位置姿勢算出部１３２０によって構成されており、計測対象である撮像装置１３０に接続されている。

ＧＰＳ受信機１３３０は、撮像装置１３０に装着されており、ＧＰＳ衛星１３４０ａ〜１３４０ｄからの信号を受信することで地球上における自己の位置（緯度，経度，高度）を計測し、位置入力部１３１０へと出力する。ただし、十分な数のＧＰＳ衛星が捕捉できない状況下では緯度と経度のみの計測値が得られる場合があるので、この場合には、緯度と経度のみを出力する。なお、主観視点カメラ座標系におけるＧＰＳ受信機１３３０の位置は既知であるものとする。

位置入力部１３１０は、ＧＰＳ受信機１３３０が出力する緯度，経度，及び高度を入力し、これを周知のマトリクス変換処理によって世界座標系における３次元座標（ｘ^Ｐ，ｙ^Ｐ，ｚ^Ｐ）あるいは２次元座標（ｘ^Ｐ，ｙ^Ｐ）に変換した後に、位置姿勢算出部１３２０へと出力する。

位置姿勢算出部１３２０は、位置入力部１３１０から入力されるＧＰＳ受信機１３３０の３次元座標あるいは２次元座標と、主観視点指標検出部１１０から入力される主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎを用いて撮像装置１３０の位置及び姿勢を算出し、これを出力する。

位置姿勢算出部１３２０は、ＧＰＳ受信機１３３０の３次元座標が得られている場合には、第６の実施形態における位置姿勢算出部８２０'と同様な工程によって撮像装置１３０の位置及び姿勢を導出しこれを出力する。また、ＧＰＳ受信機１３３０の２次元座標のみが計測されている場合には、第５の実施形態における位置姿勢算出部８２０と同様な工程、すなわち、測点（本実施形態の場合はＧＰＳ受信機１３３０）の世界座標系における位置を拘束する直線を表す媒介変数τと、撮像装置１３０の姿勢を表す３値（ξ，ψ，ζ）をあわせた４値の状態ベクトルｓ＝［τ ξ ψ ζ］^Ｔを未知パラメータとしてそれを求める工程によって、撮像装置１３０の位置及び姿勢を導出しこれを出力する。なお、本実施形態の場合には、直線の拘束式（第５の実施形態における式２２に対応する式）を、次式のように構成すればよい。

以上の処理によって、ＧＰＳ受信機１３３０によって得られる位置を拘束条件として、その拘束条件のもとで、主観視点画像上における主観視点指標の誤差を最小とするような撮像装置１３０の位置及び姿勢を得ることができる。
なお、常にＧＰＳ受信機１３３０が３次元座標を出力可能な状態が期待できる場合には、２次元座標のみが得られた場合の処理分岐を有さない構成をとることができることはいうまでもない。また、撮影装置１３０の高さが常に一定と仮定できる場合（例えば、撮像装置１３０がＨＭＤの内蔵カメラであって、その高さが装着者の視点の高さと仮定できる場合等）には、ＧＰＳ受信機１３３０から得られる高度の情報を用いずに、高さ方向の位置として固定値を用いても良い。

＜変形例１＞
上記の実施形態の各々において、誤差ベクトルＵと行列Φに基づく状態ベクトルの補正値Δｓの算出に式９で表現される最急降下法を用いているが、補正値Δｓの算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の数値計算手法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。

＜変形例２＞
上記の実施形態の各々において、指標の画像座標に関わる特徴量として、点特徴、すなわち、指標の投影像を代表する１点の画像座標を用いていた。また、指標ＱおよびＰとして、このような点特徴を抽出可能な指標（以下、これを点指標と呼ぶ）を用いていた。しかし上記実施形態は、指標の種類や指標の画像座標に関わる特徴量の種類に限定されるものではない。

例えば、公知の位置姿勢計測装置（例えば、高橋，石井，牧野，中静：ＶＲインターフェースのための単眼による長方形マーカ位置・姿勢の高精度実時間推定法, ３次元画像コンファレンス'９６公演論文集，ｐｐ．１６７−１７２，１９９６．を参照）に用いられているような、特定の幾何形状をもつマーカを主観視点指標及び／あるいは客観視点指標として用いることも出来る。四角形のマーカを指標として用いる場合、四角形の各頂点の世界座標を既知の値として保持しておき、指標の画像座標に関わる特徴量として、画像から各頂点の画像座標を検出することで、上記実施形態における指標と同様の効果を得ることができる。

また、別の公知の位置姿勢計測装置（例えば、非特許文献２を参照）に用いられているような線特徴を指標の画像座標に関わる特徴量として用いて、線特徴を抽出可能な指標（以下、これを線指標と呼ぶ）を主観視点指標及び／あるいは客観視点指標として用いてもよい。例えば、直線の原点からの距離を誤差評価のための基準として、画像からの検出値ｄと状態ベクトルｓからの推定値ｄ＊から算出する誤差Δｄによって誤差ベクトルＵを構成し、ｄ＊の算出式を状態ベクトルｓの各要素で偏微分した解を各要素に持つ１行×６列のヤコビ行列Ｊ_ｄｓ（＝∂ｄ／∂ｓ）によって行列Φを構成することで、上記実施形態と同様な枠組みによって位置及び姿勢の計測を行うことが出来る。もちろん、線指標と点指標を併用してもよい。

＜変形例３＞
第３乃至第８の実施形態において、撮像装置１３０あるいは主観視点カメラ７３０は１台であったが、第２の実施形態と同様に、複数台の撮像装置あるいは主観視点カメラを用いることも可能である。

＜変形例４＞
第４の実施形態において、客観視点指標Ｐは主観視点カメラ７３０上に配置されていたが、客観視点指標Ｐの配置位置はこれに限定されるものではなく、計測対象物体７１０に配置しても同様の効果が得られることはいうまでもない。

＜変形例５＞
第５乃至第８の実施形態は、主観視点カメラ座標系における客観視点指標，測点，ＧＰＳ受信機の各々の位置を既知とし、計測対象である撮影装置１３０の位置との相違を考慮した構成となっていたが、これらを同一と近似する簡略化した構成を取ることも可能である。

なお、主観視点画像と客観視点画像の双方から得られる幾何的な拘束条件を併用することで計測対象物体の位置及び姿勢を計測することこそが本質的な特徴であり、適用範囲は、指標の種類、拘束条件の利用方法、解の算出方法に限定されるものではない。

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

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

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

従来の位置姿勢計測装置の構成を示す図である。 従来例における位置姿勢算出部１２０が行う処理のフローチャートである。 本発明の第１の実施形態に係る位置姿勢計測装置の概略構成を示す図である。 ＣＰＵ１００１が位置姿勢算出部３２０のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。 本発明の第２の実施形態に係る位置姿勢計測装置の概略構成を示す図である。 本発明の第３の実施形態に係る位置姿勢計測装置の概略構成を示す図である。 本発明の第４の実施形態に係る位置姿勢計測装置の概略構成を示す図である。 本発明の第６の実施形態に係る位置姿勢計測装置の概略構成を示す図である。 ＣＰＵ１００１が位置姿勢算出部８２０のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。 コンピュータの基本構成を示す図である。 ＣＰＵ１００１が位置姿勢算出部３２０'のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。 本発明の第７の実施形態に係る位置姿勢計測装置の概略構成を示す図である。 本発明の第８の実施形態に係る位置姿勢計測装置の概略構成を示す図である。

Claims (5)

1. 主観視点撮像装置の位置及び姿勢を算出する位置姿勢計測方法であって、
   前記主観視点撮像装置によって物理空間内の情景を撮像する主観視点撮像工程と、
   前記主観視点撮像装置を前記物理空間に固定された客観視点撮像装置によって撮像する客観視点撮像工程と、
   前記主観視点撮像工程で取得された第１の画像から、前記情景中に配置された第１の指標の画像座標を検出する第１の検出工程と、
   前記客観視点撮像工程で取得された第２の画像から、前記主観視点撮像装置上に配置された第２の指標の画像座標を検出する第２の検出工程と、
   前記第２の検出工程で検出した前記第２の指標の画像座標と、前記客観視点撮像装置の位置姿勢とに基づいて、前記第２の指標の物理空間中における位置を拘束する直線を求め、前記第２の指標が当該直線上に存在するという拘束の元に、前記第１の検出工程で検出した前記第１の指標の画像座標に基づいて、前記主観視点撮像装置の位置及び姿勢を算出する位置姿勢算出工程と
   を備えることを特徴とする位置姿勢計測方法。
2. 前記位置姿勢算出工程では、前記直線上における前記第２の指標の位置を表す媒介変数と、前記主観視点撮像装置の姿勢を表すパラメータを未知数とし、前記第１の検出工程で検出した前記第１の指標の画像座標に基づいてこれらの未知数を求めることにより、前記主観視点撮像装置の位置及び姿勢を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
3. 物理空間内の情景を撮像する主観視点撮像装置の位置及び姿勢を算出する位置姿勢計測装置であって、
   前記物理空間に固定され、前記主観視点撮像装置を撮像する客観視点撮像手段と、
   前記主観視点撮像装置によって取得された第１の画像から、前記情景中に配置された第１の指標の画像座標を検出する第１の検出手段と、
   前記客観視点撮像手段によって取得された第２の画像から、前記主観視点撮像装置上に配置された第２の指標の画像座標を検出する第２の検出手段と、
   前記第２の検出手段が検出した前記第２の指標の画像座標と、前記客観視点撮像装置の位置姿勢とに基づいて、前記第２の指標の物理空間中における位置を拘束する直線を求め、前記第２の指標が当該直線上に存在するという拘束の元に、前記第１の検出手段が検出した前記第１の指標の画像座標に基づいて、前記主観視点撮像装置の位置及び姿勢を算出する位置姿勢算出手段と
   を備えることを特徴とする位置姿勢計測装置。
4. コンピュータに請求項１または２に記載の位置姿勢計測方法を実行させることを特徴とするプログラム。
5. 請求項４に記載のプログラムを格納したことを特徴とする、コンピュータ読み取り可能な記憶媒体。

Images