(57)【要約】【課題】  撮像画像を用いて計測対象物の位置や姿勢などを計測する際に、フレーム間の整合性と移動への追従性とを両立した計測技術を提供すること。【解決手段】  注目フレーム画像中の全指標の座標位置を用いて撮像装置の位置姿勢の第１推定値を求める。注目フレーム画像中の第１指標群と前フレーム画像中の第２指標群とで同じ固有情報を有する指標を共通指標として検出する。第２指標群の固有情報のうち第１指標群の固有情報とは異なる固有情報が存在する場合、前フレーム画像中の共通指標の座標位置を用いて撮像装置の位置姿勢の第２推定値を求める。第１指標群の固有情報のうち第２指標群の固有情報と異なる固有情報が存在する場合、注目フレーム画像中の共通指標の座標位置を用いて撮像装置の位置姿勢の第３推定値を求める。第１乃至３推定値のうち求められたものに基づいて求める補正量を用いて第１の推定値を補正する。【選択図】    図１
2007271564.tif 000002
【特許請求の範囲】
【請求項１】
撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標の画像座標に関する情報を用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程と、
過去のフレームにおいて前記撮像装置の位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置姿勢推定値のフレーム間での不連続性を緩和する位置及び姿勢の補正値を算出する補正値算出工程と、
前記補正値算出工程で算出した補正値を用いて、前記位置姿勢算出工程で算出した位置及び姿勢を補正する位置姿勢補正工程と
を有することを特徴とする位置姿勢計測方法。
【請求項２】
前記補正値算出工程では、指標の出現／消失を検出しなかった場合は、指標の出現／消失を検出した際に算出した補正値に、その時点からの時間経過に応じて減衰する重みを掛け合わせたものを現在のフレームにおける補正値とすることを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項３】
撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標と、過去のフレームにおいて当該撮像装置の位置及び姿勢の算出に使用した過去の指標とを比較し、両者に共通して含まれている共通指標を選択する指標選択工程と、
前記現在の指標の画像座標に関する情報を用いて、前記撮像装置の第一の位置及び姿勢を算出する第一の位置姿勢算出工程と、
前記現在の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第二の位置及び姿勢を算出する第二の位置姿勢算出工程と、
前記第一の位置及び姿勢と前記第二の位置及び姿勢とを用いて、現在のフレームにおける前記撮像装置の位置及び姿勢を算出する位置姿勢統合工程と
を有することを特徴とする位置姿勢計測方法。
【請求項４】
前記位置姿勢統合工程では、前記第一の位置及び姿勢と前記第二の位置及び姿勢との間の変化分を求め、得られた変化分を前記第一の位置及び姿勢に加重して付加することで前記撮像装置の位置及び姿勢を算出することを特徴とする、請求項３に記載の位置姿勢計測方法。
【請求項５】
撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標と、過去のフレームにおいて当該撮像装置の位置及び姿勢の算出に使用した過去の指標とを比較し、両者に共通して含まれている共通指標を選択する指標選択工程と、
前記現在の指標の画像座標に関する情報を用いて、前記撮像装置の第一の位置及び姿勢を算出する第一の位置姿勢算出工程と、
前記過去の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第二の位置及び姿勢を算出する第二の位置姿勢算出工程と、
前記第一の位置及び姿勢と前記第二の位置及び姿勢とを用いて、現在のフレームにおける前記撮像装置の位置及び姿勢を算出する位置姿勢統合工程と
を有することを特徴とする位置姿勢計測方法。
【請求項６】
前記位置姿勢統合工程では、前記第二の位置及び姿勢と前記過去のフレームにおいて適用されている位置及び姿勢との間の変化分を求め、得られた変化分を前記第一の位置及び姿勢に付加することで前記撮像装置の位置及び姿勢を算出することを特徴とする、請求項５に記載の位置姿勢計測方法。
【請求項７】
撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標と、過去のフレームにおいて当該撮像装置の位置及び姿勢の算出に使用した過去の指標とを比較し、両者に共通して含まれている共通指標を選択する指標選択工程と、
前記現在の指標の画像座標に関する情報を用いて、前記撮像装置の第一の位置及び姿勢を算出する第一の位置姿勢算出工程と、
前記現在の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第二の位置及び姿勢を算出する第二の位置姿勢算出工程と、
前記過去の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第三の位置及び姿勢を算出する第三の位置姿勢算出工程と、
前記第一、第二、第三の位置及び姿勢を用いて、現在のフレームにおける前記撮像装置の位置及び姿勢を算出する位置姿勢統合工程と
を有することを特徴とする位置姿勢計測方法。
【請求項８】
前記位置姿勢統合工程では、前記第一の位置及び姿勢と前記第二の位置及び姿勢との間の変化分と、前記第三の位置及び姿勢と前記過去のフレームにおいて適用されている位置及び姿勢との間の変化分を求め、得られた夫々の変化分を前記第一の位置及び姿勢に加重して付加することで前記撮像装置の位置及び姿勢を算出することを特徴とする、請求項７に記載の位置姿勢計測方法。
【請求項９】
計測対象を撮影する１台以上の撮像装置から画像を入力する画像入力工程と、
前記計測対象上に存在する指標を前記画像から検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標の画像座標に関する情報を用いて、前記計測対象の位置あるいは位置及び姿勢を算出する位置姿勢算出工程と、
過去のフレームにおいて前記計測対象の位置あるいは位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置あるいは位置姿勢推定値のフレーム間での不連続性を緩和する位置あるいは位置及び姿勢の補正値を算出する補正値算出工程と、
前記補正値算出工程で算出した補正値を用いて、前記位置姿勢算出工程で算出した位置あるいは位置及び姿勢を補正する位置姿勢補正工程と
を有することを特徴とする位置姿勢計測方法。
【請求項１０】
撮像装置から画像を入力する画像入力手段と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出手段と、
現在のフレームにおいて前記指標検出手段で検出された現在の指標の画像座標に関する情報を用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出手段と、
過去のフレームにおいて前記撮像装置の位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置姿勢推定値のフレーム間での不連続性を緩和する位置及び姿勢の補正値を算出する補正値算出手段と、
前記補正値算出手段が算出した補正値を用いて、前記位置姿勢算出手段が算出した位置及び姿勢を補正する位置姿勢補正手段と
を有することを特徴とする位置姿勢計測装置。
【請求項１１】
計測対象を撮影する１台以上の撮像装置と、
前記撮像装置から画像を入力する画像入力手段と、
前記計測対象上に存在する指標を前記画像から検出し、当該指標の画像座標に関する情報を取得する指標検出手段と、
現在のフレームにおいて前記指標検出手段で検出された現在の指標の画像座標に関する情報を用いて、前記計測対象の位置あるいは位置及び姿勢を算出する位置姿勢算出手段と、
過去のフレームにおいて前記計測対象の位置あるいは位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置あるいは位置姿勢推定値のフレーム間での不連続性を緩和する位置あるいは位置及び姿勢の補正値を算出する補正値算出手段と、
前記補正値算出手段が算出した補正値を用いて、前記位置姿勢算出手段が算出した位置あるいは位置及び姿勢を補正する位置姿勢補正手段と
を有することを特徴とする位置姿勢計測装置。
【請求項１２】
請求項１乃至請求項９のいずれか１項に記載の位置姿勢計測方法をコンピュータに実行させるための制御プログラム。
【請求項１３】
請求項１２に記載の制御プログラムを格納するコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像装置の位置姿勢を求めるための技術に関するものである。
【背景技術】
【０００２】
現実空間に文字やＣＧ映像を重畳して提示する複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラ等で撮影した画像にカメラの位置及び姿勢に応じて生成した画像を重畳描画し、これを表示する。
【０００３】
このような画像表示装置を実現するには、現実空間中に定義した基準座標系と、カメラ座標系との間の、相対的な位置及び姿勢を計測することが不可欠である。例として、室内やテーブル上といった現実環境の所定位置に仮想物体を重畳表示する場合を考える。この場合は、その環境の適切な場所（例えば部屋の床面やテーブル面）に基準座標系を定義し、基準座標系におけるカメラの位置及び姿勢を求めればよい。
【０００４】
このような計測を実現するために、カメラが撮影した画像を利用して、カメラの位置及び姿勢を推定することが一般的に行われている（非特許文献１，非特許文献２，非特許文献３を参照）。例えば、以下の手順によって、基準座標系におけるカメラの位置及び姿勢を計測することができる。
【０００５】
（１）  室内の床や壁、テーブル面等に、基準座標系における位置（基準座標）が既知である複数の指標を配置あるいは設定する。ここで、指標とは、計測のために意図的に設置された人為的なマーカであってもよいし、元々その環境に存在している自然特徴などであってもよい。
【０００６】
（２）  カメラが撮影した撮像画像内における指標の投影像の座標を検出する。
【０００７】
（３）  検出された指標の画像座標と、この指標の基準座標との対応関係に基づいて、カメラの位置及び姿勢を求める。
【０００８】
（従来例１）
上記の（３）の工程を実現する代表的な方法のひとつに、繰り返し演算によって指標の再投影誤差を最小化する非線形最適化手法がある（非特許文献１を参照）。この手法では、カメラの位置及び姿勢の推定値と各指標の基準座標に基づいて、この指標の画像座標の計算上の値（理論座標）が算出される。そして、この理論座標が指標の実際の検出座標に極力近づくように、カメラの位置及び姿勢の推定値に補正が加えられる。この理論座標の算出と推定値の補正を繰り返し行うことで、最適な位置及び姿勢が算出される。連続的に撮影される時系列画像に対してカメラの位置及び姿勢を求める場合には、入力されたフレーム毎に上述の最適化処理を実行する。これによって、個々のフレームにおいて最適な位置及び姿勢を算出する。
【０００９】
（従来例２）
一方、時系列画像におけるフレーム間の連続性を加味しながら上記の（３）の工程を実現する方法も提案されている。例えば、「前フレームでの位置及び姿勢から大きく外れない」という拘束条件を前述の非線形最適化手法に組み込むことで、フレーム間の連続性を極力維持しながら誤差最小化を行う手法が提案されている（非特許文献２を参照）。
【００１０】
（従来例３）
また、拡張カルマンフィルタを用いてカメラの位置及び姿勢を連続的に求めることが一般的に行われている（非特許文献３を参照）。拡張カルマンフィルタを利用する方法では、カメラの運動モデルが、等速度・等角速度運動、あるいは、等加速度・等角加速度運動といった仮定を用いてモデル化される。拡張カルマンフィルタの状態変数の構成要素としては、カメラの位置、姿勢、速度、加速度、角速度等が設定される。また、観測値として、各フレームにおける指標の画像座標と基準座標の組が入力される。拡張カルマンフィルタを用いた手法では、あるフレームにおいて出力される位置及び姿勢は、そのフレームの観測値だけでなく、仮定した運動モデルに基づいて、過去の状態の影響を受けたものとなる。仮定した運動モデルに対して実際のカメラの動きが大きく異なっていなければ、フレーム間の整合性が取れた状態で位置及び姿勢の推定値が変動し、時系列上でスムーズな動きを得ることができる。
【００１１】
（従来例４）
また、位置姿勢センサをカメラに装着することでカメラの位置及び姿勢を直接計測し、さらに、撮影した指標を利用してその誤差を補正することが行われている。特許文献１では、フレーム間で異なる補正値が得られた場合の不連続性を回避するために、複数フレームで求めた補正値を平均化した後に適用する工夫がなされている。
【非特許文献１】佐藤，内山，山本：ＵＧ＋Ｂ法：主観及び客観視点カメラと姿勢センサを用いた位置合わせ手法，日本バーチャルリアリティ学会論文誌，vol.10, no.3, pp.391-400, 2005.
【非特許文献２】I. Skrypnyk and D. Lowe: "Scene modeling, recognition and tracking with invariant image features," Proc. 3rd International Symposium on Mixed and Augmented Reality (ISMAR’04), pp.110-119, 2004.
【非特許文献３】J. Park, B. Jiang, and U. Neumann: "Vision-based pose computation: robust and accurate augmented reality tracking," Proc. 2nd International Workshop on Augmented Reality (IWAR’99), pp.3-12, 1999.
【特許文献１】特開2005-107247号公報
【発明の開示】
【発明が解決しようとする課題】
【００１２】
指標の再投影誤差を最小化する位置及び姿勢をフレーム毎に算出するアプローチ（従来例１）には、フレーム間の整合性の面で改善の余地がある。特に、撮影されている指標の組み合わせがフレーム間で異なる状況下において、前後のフレームで出力される位置及び姿勢の間に不連続性が生じる場合がある。これは、個々のフレームで得られる位置及び姿勢は誤差を含むものであり、入力される指標の組み合わせが変化することで、誤差の振る舞いが変化するためと考えられる。
【００１３】
一方、フレーム間の連続性を考慮する最適化手法（従来例２）では、過去に得られた位置及び姿勢に対する慣性力が働くので、カメラが急激に移動する状況に追従できない場合がある。拡張カルマンフィルタを用いる方法（従来例３）においても、カメラの速度や加速度が急激に変化する状況では、追従が困難であったり、オーバーシュートが発生したりする場合がある。これは、実際のカメラの動きが、仮定した運動モデルから大きく外れてしまうためである。このような状況が生じている区間においては、位置及び姿勢の計測精度が低下してしまう。
【００１４】
また、従来例４は、位置姿勢センサの計測誤差を「補正」することが目的なので、指標のみを利用して位置姿勢を計測する場合には適用できない。
【００１５】
本発明は以上の問題に鑑みてなされたものであり、撮像画像を用いて計測対象物の位置や姿勢などを計測する際に、フレーム間の整合性と移動への追従性とを両立した計測技術を提供することを目的とする。
【課題を解決するための手段】
【００１６】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。
【００１７】
即ち、撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標の画像座標に関する情報を用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程と、
過去のフレームにおいて前記撮像装置の位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置姿勢推定値のフレーム間での不連続性を緩和する位置及び姿勢の補正値を算出する補正値算出工程と、
前記補正値算出工程で算出した補正値を用いて、前記位置姿勢算出工程で算出した位置及び姿勢を補正する位置姿勢補正工程と
を有することを特徴とする。
【００１８】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。
【００１９】
即ち、撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標と、過去のフレームにおいて当該撮像装置の位置及び姿勢の算出に使用した過去の指標とを比較し、両者に共通して含まれている共通指標を選択する指標選択工程と、
前記現在の指標の画像座標に関する情報を用いて、前記撮像装置の第一の位置及び姿勢を算出する第一の位置姿勢算出工程と、
前記現在の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第二の位置及び姿勢を算出する第二の位置姿勢算出工程と、
前記第一の位置及び姿勢と前記第二の位置及び姿勢とを用いて、現在のフレームにおける前記撮像装置の位置及び姿勢を算出する位置姿勢統合工程と
を有することを特徴とする。
【００２０】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。
【００２１】
即ち、撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標と、過去のフレームにおいて当該撮像装置の位置及び姿勢の算出に使用した過去の指標とを比較し、両者に共通して含まれている共通指標を選択する指標選択工程と、
前記現在の指標の画像座標に関する情報を用いて、前記撮像装置の第一の位置及び姿勢を算出する第一の位置姿勢算出工程と、
前記過去の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第二の位置及び姿勢を算出する第二の位置姿勢算出工程と、
前記第一の位置及び姿勢と前記第二の位置及び姿勢とを用いて、現在のフレームにおける前記撮像装置の位置及び姿勢を算出する位置姿勢統合工程と
を有することを特徴とする。
【００２２】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。
【００２３】
即ち、撮像装置から画像を入力する画像入力工程と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標と、過去のフレームにおいて当該撮像装置の位置及び姿勢の算出に使用した過去の指標とを比較し、両者に共通して含まれている共通指標を選択する指標選択工程と、
前記現在の指標の画像座標に関する情報を用いて、前記撮像装置の第一の位置及び姿勢を算出する第一の位置姿勢算出工程と、
前記現在の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第二の位置及び姿勢を算出する第二の位置姿勢算出工程と、
前記過去の指標のうち、共通指標として選択された指標のみの画像座標に関する情報を用いて、前記撮像装置の第三の位置及び姿勢を算出する第三の位置姿勢算出工程と、
前記第一、第二、第三の位置及び姿勢を用いて、現在のフレームにおける前記撮像装置の位置及び姿勢を算出する位置姿勢統合工程と
を有することを特徴とする。
【００２４】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。
【００２５】
即ち、計測対象を撮影する１台以上の撮像装置から画像を入力する画像入力工程と、
前記計測対象上に存在する指標を前記画像から検出し、当該指標の画像座標に関する情報を取得する指標検出工程と、
現在のフレームにおいて前記指標検出工程で検出された現在の指標の画像座標に関する情報を用いて、前記計測対象の位置あるいは位置及び姿勢を算出する位置姿勢算出工程と、
過去のフレームにおいて前記計測対象の位置あるいは位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置あるいは位置姿勢推定値のフレーム間での不連続性を緩和する位置あるいは位置及び姿勢の補正値を算出する補正値算出工程と、
前記補正値算出工程で算出した補正値を用いて、前記位置姿勢算出工程で算出した位置あるいは位置及び姿勢を補正する位置姿勢補正工程と
を有することを特徴とする。
【００２６】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は以下の構成を備える。
【００２７】
即ち、撮像装置から画像を入力する画像入力手段と、
前記画像中に撮像されている指標を検出し、当該指標の画像座標に関する情報を取得する指標検出手段と、
現在のフレームにおいて前記指標検出手段で検出された現在の指標の画像座標に関する情報を用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出手段と、
過去のフレームにおいて前記撮像装置の位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置姿勢推定値のフレーム間での不連続性を緩和する位置及び姿勢の補正値を算出する補正値算出手段と、
前記補正値算出手段が算出した補正値を用いて、前記位置姿勢算出手段が算出した位置及び姿勢を補正する位置姿勢補正手段と
を有することを特徴とする。
【００２８】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は以下の構成を備える。
【００２９】
即ち、計測対象を撮影する１台以上の撮像装置と、
前記撮像装置から画像を入力する画像入力手段と、
前記計測対象上に存在する指標を前記画像から検出し、当該指標の画像座標に関する情報を取得する指標検出手段と、
現在のフレームにおいて前記指標検出手段で検出された現在の指標の画像座標に関する情報を用いて、前記計測対象の位置あるいは位置及び姿勢を算出する位置姿勢算出手段と、
過去のフレームにおいて前記計測対象の位置あるいは位置及び姿勢の算出に使用した過去の指標と、前記現在の指標との間における指標の出現及び／または消失を判定し、該指標の出現及び／または消失に起因する位置あるいは位置姿勢推定値のフレーム間での不連続性を緩和する位置あるいは位置及び姿勢の補正値を算出する補正値算出手段と、
前記補正値算出手段が算出した補正値を用いて、前記位置姿勢算出手段が算出した位置あるいは位置及び姿勢を補正する位置姿勢補正手段と
を有することを特徴とする。
【発明の効果】
【００３０】
本発明の構成により、計測対象物体の位置や姿勢の計測を、フレーム間における指標の出現や消失を考慮しながら実行することができる。また、整合性を維持するための処理を、指標の組み合わせの変化分によって生じるずれに対してのみ行うので、急激な移動にも追従可能であり、フレーム間の整合性と移動への追従性を両立した計測を実現できる。
【発明を実施するための最良の形態】
【００３１】
以下添付図面を参照して、本発明を好適な実施形態に従って詳細に説明する。
【００３２】
［第１の実施形態］
本実施形態では、撮像装置が撮像した現実空間の動画像を用いて、この撮像装置の位置姿勢の計測を行う。図１は、撮像装置の位置姿勢を計測するための本実施形態に係るシステムの機能構成を示すブロック図である。同図に示す如く、本実施形態に係るシステムは、撮像装置１５０、情報処理装置１００により構成されている。更に、情報処理装置１００は、画像入力部１１０、指標検出部１２０、指標選択部１３０、位置姿勢算出部１４０により構成されている。
【００３３】
先ず、本実施形態に係るシステムが配置される環境（現実空間）について説明する。同図に示す如く、現実空間中には、所定の１点を原点とし、この原点で互いに直交する３軸をそれぞれｘ、ｙ、ｚ軸とする基準座標系が定義されている。更に、この現実空間中には、基準座標系における位置が既知である複数個の指標Ｑｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。ここで、ｋは個々の指標に固有の識別子を表している。また、Ｋは配置されている指標の総数を表している。指標Ｑｋは、例えば、それぞれが異なる色を有する同一形状のマーカによって構成してもよい。また、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する矩形領域によって形成されるような、四角形指標を用いることも可能である。
【００３４】
即ち、指標として用いることができるものは、指標が存在する現実空間の画像を撮像した場合に、この画像上に位置するそれぞれの指標の識別子が識別可能であり、且つ画像上における指標座標位置が検出可能であれば、如何なるものを用いても良い。
【００３５】
また、指標は故意に設定されたものであっても良いし、故意に設定されたものではない自然形状のものを用いても良い。また、指標は、異なる性質のものを混在して用いてもよい。図１の場合には、７個の四角形指標Ｑ１〜Ｑ７が配置されている。
【００３６】
次に、本実施形態に係るシステムを構成している各部について説明する。
【００３７】
撮像装置１５０は、現実空間の動画像が撮像可能なカメラであり、例えば、ディジタルビデオカメラである。ここで、撮像装置１５０には、所定の１点（例えばレンズ焦点）を原点とし、視線方向を−Ｚ軸、Ｚ軸に直交してかつ互いに直交する２軸をＸ軸、Ｙ軸とするカメラ座標系が定義されている。詳しくは後述するが、情報処理装置１００は、基準座標系に対するこのカメラ座標系の位置姿勢を、撮像装置１５０の位置姿勢として計測する。
【００３８】
撮像装置１５０が撮像した各フレームの画像（現実空間の画像）は、画像信号として情報処理装置１００が有する画像入力部１１０に入力される。画像入力部１１０は、入力された各フレームの画像信号をディジタルデータに変換し、後段の指標検出部１２０に送出する。即ち、画像入力部１１０は、各フレームの画像データを指標検出部１２０に供給することになる。
【００３９】
指標検出部１２０は、画像入力部１１０から入力された各フレームについて、画像中に位置している全ての指標を検出する処理を行う。より詳しくは、画像中に位置している指標（指標Ｑ）を識別し、その指標の識別子を特定する。更に、画像中に位置する指標の画像座標を検出する。そして指標検出部１２０は、検出されたそれぞれの指標の識別子とその画像座標を、指標選択部１３０へと出力する。
【００４０】
指標の検出は、使用する指標の種類に応じた方法で行われる。例えば、指標の各々が異なる色を有するマーカによって構成されている場合には、画像上から各々のマーカ色に対応する領域を検出する。そして、その重心位置を指標の検出座標とする。また、指標の各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、テンプレート画像を用いたテンプレートマッチングによって指標の位置を検出する。この場合、各々の指標のテンプレート画像は、既知の情報として予め保持しておく。
【００４１】
また、四角形指標を用いる場合は、画像に２値化処理及びラベリング処理を施し、４つの直線によって形成されているラベル領域を指標候補として検出する。さらに、各指標候補の矩形領域の中に特定のパターンがあるか否かを判定することによって誤検出の排除を行う。また、矩形領域中のパターンによって、指標の識別子を取得する。矩形領域の４頂点の座標が、指標の位置として出力される。
【００４２】
何れにせよ、画像中における指標の識別子、及びその画像座標を特定する方法については従来より様々な方法があるので、何れの方法を用いても良い。なお、以下では、検出指標の夫々に通し番号ｎ（ｎ＝１，，，Ｎ）をつけ、画像上で検出された指標の識別子をｋｎと表記する。ここで、Ｎは画像上で検出された指標の総数を表している。
【００４３】
図３は、撮像装置１５０が撮像した連続する２つのフレームの画像の例を示す図である。画像４１０をｉ番目のフレームの画像、画像４２０を（ｉ＋１）番目のフレームの画像と考える。画像４１０上には指標Ｑ２，Ｑ３，Ｑ４が、画像４２０上には指標Ｑ２，Ｑ４，Ｑ６が位置している。指標検出部１２０に画像４２０が入力された場合を考えると、指標検出部１２０からは、検出された夫々の指標の識別子（ｋ１＝２，ｋ２＝４，ｋ３＝６）と、画像４２０上における画像座標（四角形指標の場合は、夫々の指標毎に４点の座標）が出力される。
【００４４】
図１に戻って、指標選択部１３０は、指標検出部１２０から検出指標の識別子と画像座標を入力し、フレーム間での指標の消失・出現を監視し、撮像装置１５０の位置姿勢の計算のために必要な情報を選択し、位置姿勢算出部１４０へと出力する。
【００４５】
図２は、指標選択部１３０の機能構成を示すブロック図である。同図に示す如く、指標選択部１３０は、データ蓄積部２１０、データ比較部２２０によって構成されている。
【００４６】
データ蓄積部２１０は、指標検出部１２０から検出指標の識別子と画像座標のセットを受けると、これを複数のフレームに渡って格納する。従って、データ蓄積部２１０には、画像上におけるそれぞれの指標の識別子、画像座標のセットが、各フレームの画像毎に保持されていることになる。
【００４７】
データ比較部２２０は、データ蓄積部２１０から、注目フレームの画像上に位置する指標の識別子と、注目フレームよりも従前のフレーム（例えば１つ前のフレームであり、以下では前フレームと呼称する）の画像上に位置する指標の識別子とを読み出す。そして、これらの識別子を比較し、注目フレームの画像上に位置する指標の識別子のうち、前フレームの画像上に位置する指標の識別子と同じ識別子を有する指標（以下、共通指標と呼称する）を特定する。更に、前フレームの画像上には存在しないが、注目フレームの画像上には存在する指標（出現指標）、前フレームの画像上には存在するものの、注目フレームの画像上には存在しない指標（消失指標）を特定する。
【００４８】
この特定処理についてより詳細に説明すると、先ず、消失指標が存在するか否かを表すフラグDisappFlgと、出現指標が存在するか否かを表すフラグAppFlgを0に初期化する。そして、前フレームの画像上に位置する指標の識別子のうち、注目フレームの画像上に位置する指標の識別子とは異なる識別子が存在する場合には、前フレームから注目フレームに移行した場合に、画像上で消失した指標があると判断し、DisappFlgを1に設定する。また、注目フレームの画像上に位置する指標の識別子のうち、前フレームの画像上に位置する指標の識別子とは異なる識別子が存在する場合には、前フレームから注目フレームに移行した場合に、画像上で新たに出現した指標があると判断し、AppFlgを1に設定する。
【００４９】
また、注目フレームの画像中に位置している指標と、前フレームの画像中に位置している指標とで、同じ識別子を有する指標を共通指標として検出し、検出した共通指標の画像座標、識別子のセットを保持する。
【００５０】
そして、その後、データ比較部２２０は、下記の如く、条件に基づいて以下のようなデータを位置姿勢算出部１４０に出力する。
【００５１】
（１）  注目フレームの画像上に位置する全ての指標の識別子と画像座標
（２）  フラグAppFlgの値（０または１）
（３）  フラグDisappFlgの値（０または１）、
（４）  AppFlg=1である場合にのみ、注目フレームの画像上に位置する共通指標の識別子と画像座標
（５）  DisappFlg=1である場合にのみ、前フレームの画像上に位置する共通指標の識別子と画像座標
図３に示した画像４１０，４２０を例に取り説明すると、注目フレームの画像が画像４２０、前フレームの画像が画像４１０である場合、前フレームの画像上には指標Ｑ２，Ｑ３，Ｑ４が、注目フレームの画像上には指標Ｑ２，Ｑ４，Ｑ６が検出されている。データ比較部２３０は、それぞれのフレームの画像上に位置する指標の識別子を比較し、前フレームから注目フレームに移行した場合に、画像４２０上でＱ３が消失、Ｑ６が出現したと判定し、Ｑ２，Ｑ４を共通指標として検出する。その結果、データ比較部２２０は、上記（１）〜（５）に対応する情報として、以下の情報を位置姿勢算出部１４０へ出力する。
【００５２】
（１）  注目フレームの画像上に位置する全ての指標の識別子（２，４，６）と画像座標
（２）  AppFlg=1
（３）  DisappFlg=1
（４）  注目フレームの画像上に位置する共通指標の識別子（２，４）と画像座標
（５）  前フレームの画像上に位置する共通指標の識別子（２，４）と画像座標
位置姿勢算出部１４０は、データ比較部２２０から受けた指標の画像座標を用いて、撮像装置１５０の位置姿勢を算出する処理を行う。図４は、位置姿勢算出部１４０の機能構成を示すブロック図である。同図に示す如く、位置姿勢算出部１４０は、データ入力部４１０、第一位置姿勢演算部４２０、第二位置姿勢演算部４３０、第三位置姿勢演算部４４０、位置姿勢統合部４５０によって構成されている。
【００５３】
データ入力部４１０は、指標選択部１３０から出力された情報を受けると、これを振り分ける処理を行う。より詳しく説明すると、データ入力部４１０は、注目フレームの画像上に位置する全ての指標の画像座標、識別子のセットについては第一位置姿勢演算部４２０に出力する。また、データ入力部４１０は、AppFlgを受けるとこの値を参照し、AppFlg=1である場合には、注目フレームの画像上に位置する共通指標の識別子と画像座標のセットを第二位置姿勢演算部４３０に出力する。また、データ入力部４１０は、DisappFlgを受けるとこの値を参照し、DisappFlg=1である場合には、前フレームの画像上に位置する共通指標の識別子、画像座標のセット第三位置姿勢演算部４４０に出力する。
【００５４】
第一位置姿勢演算部４２０、第二位置姿勢演算部４３０、第三位置姿勢演算部４４０はそれぞれ、受けた識別子、画像座標、及び対応する指標の基準座標系における位置を用いて、撮像装置１５０の位置姿勢の推定値を算出する。複数の指標の基準座標系における位置と画像座標の組から撮像装置１５０の位置姿勢を算出する方法として、本実施形態では、非線形最適化による再投影誤差最小化手法（非特許文献１参照）を利用する。なお、複数の指標の基準座標系における位置と画像座標の組から撮像装置１５０の位置姿勢を算出する方法については、従来から様々なものが提案されており、何れの方法を用いても、以下の説明の本質は変わらない。また本実施形態では、３種の入力データに対して別々の位置姿勢算出部を用いているが、同一の位置姿勢算出部を時間分割で利用してもよい。
【００５５】
位置姿勢統合部４５０は、第一位置姿勢演算部４２０、第二位置姿勢演算部４３０、第三位置姿勢演算部４４０のそれぞれ得られた位置姿勢を入力する。また、データ入力部４１０から、AppFlg、DisappFlgを入力する。そして位置姿勢統合部４５０は、これらの情報を用いて、撮像装置１５０の位置姿勢を、前フレームとの整合性を加味ながら算出（決定）する。そして位置姿勢算出部１４０は、決定した撮像装置１５０の位置姿勢を外部に出力する。なお、出力先については特に限定するものではなく、ここでの説明の趣旨ではないので、説明は省略する。位置姿勢算出部１４０が行う処理の詳細については以下で、図６を用いて説明する。
【００５６】
図５は、情報処理装置１００に適用可能なコンピュータのハードウェア構成を示すブロック図である。
【００５７】
ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１１０、指標検出部１２０、指標選択部１３０、位置姿勢算出部１４０として機能する。なお、指標選択部１３０が有するデータ蓄積部２１０に関しては、後述するＲＡＭ１００２や外部記憶装置１００７内に設ける。
【００５８】
ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータ、Ｉ／Ｆ１００９を介して外部（ここでは撮像装置１５０）から入力したデータ等を一時的に記憶するエリアを備える。また、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。
【００５９】
ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データなどを格納する。
【００６０】
キーボード１００４、マウス１００５は入力デバイスであり、本コンピュータの操作者がこれらの入力デバイスを操作することで、各種の指示をＣＰＵ１００１に対して入力することができる。
【００６１】
表示部１００６は、ＣＲＴや液晶ディスプレイなどにより構成されており、ＣＰＵ１００１による処理結果を画像や文字などでもって表示することができる。例えば、撮像装置１５０の位置姿勢計測のために表示すべきメッセージ等を表示する。
【００６２】
外部記憶装置１００７は、ハードディスクドライブなどの大容量情報記憶装置として機能する装置である。ここには、ＯＳ（オペレーティングシステム）や、画像入力部１１０、指標検出部１２０、指標選択部１３０、位置姿勢算出部１４０が行う各処理をＣＰＵ１００１に実行させるためのプログラムやデータが保存されている。また、外部記憶装置１００７には、本実施形態に係る処理の説明上、既知の情報として説明したものも保存されている。例えば、外部記憶装置１００７には、現実空間中に存在する全ての指標について、識別子と基準座標系における位置とがセットになって保持されている。
【００６３】
そして、外部記憶装置１００７に保存されているプログラムやデータはＣＰＵ１００１による制御に従って適宜ＲＡＭ１００２にロードされる。そしてＣＰＵ１００１がＲＡＭ１００２にロードされたプログラムやデータを用いて処理を実行することで、本コンピュータは、本実施形態で情報処理装置１００が行う処理として説明した各処理を実行することになる。
【００６４】
記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出し、ＲＡＭ１００２や外部記憶装置１００７に出力する。
【００６５】
Ｉ／Ｆ１００９は、撮像装置１５０をバス１０１０に接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、算出した撮像装置１５０の位置姿勢のデータを外部へ出力するためのイーサネット（登録商標）ポート等によって構成される。画像入力部１１０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
【００６６】
上述した各構成要素は、バス１０１０によって相互に接続される。
【００６７】
図９は、注目フレームにおける撮像装置１５０の位置姿勢を求めるために情報処理装置１００が行う処理のフローチャートである。従って、撮像装置１５０が撮像した各フレームの画像について同様の処理を行う場合には、同図のフローチャートに従った処理をこの各フレームについて行えば良い。
【００６８】
なお、同図のフローチャートに従った処理をＣＰＵ１００１に実行させるためのプログラムやデータは外部記憶装置１００７に保存されている。そしてこれをＣＰＵ１００１による制御に従ってＲＡＭ１００２にロードし、ＣＰＵ１００１がこれを用いて処理を実行することで、後述の各処理を実現する。
【００６９】
先ずステップＳ９００では、画像入力部１１０により、注目フレームの画像データが指標検出部１２０に供給される。次にステップＳ９０１では、指標検出部１２０が、注目フレームの画像中に位置している指標を識別し、その指標の識別子、画像座標を検出する。そして指標検出部１２０は、検出されたそれぞれの指標の識別子とその画像座標を、指標選択部１３０へと出力する。
【００７０】
次にステップＳ９０２では、データ蓄積部２１０は、指標検出部１２０から注目フレームの画像上に位置する指標の識別子と画像座標のセットを受けると、これを保持する。次にステップＳ９０３では、データ比較部２２０は、上記フラグDisappFlgを設定すると共に、ステップＳ９０４では、AppFlgを設定する。
【００７１】
DisappFlgフラグの設定については上述の通り、データ比較部２２０は先ず、データ蓄積部２１０から、注目フレームの画像上に位置する指標の識別子と、前フレームの画像上に位置する指標の識別子とを比較する。そして、前フレームの画像上に位置する指標の識別子のうち、注目フレームの画像上に位置する指標の識別子とは異なる識別子が存在する場合には、DisappFlgを1に設定する。また、AppFlgの設定についても上述の通り、注目フレームの画像上に位置する指標の識別子のうち、前フレームの画像上に位置する指標の識別子とは異なる識別子が存在する場合には、AppFlgを1に設定する。
【００７２】
次にステップＳ９０５では、データ比較部２２０は、注目フレームの画像中に位置している指標と、前フレームの画像中に位置している指標とで、同じ識別子を有する指標を共通指標として検出し、検出した共通指標の画像座標、識別子のセットを取得する。
【００７３】
次にステップＳ９０６では、データ比較部２２０は、注目フレームの画像上に位置する全ての指標の識別子と画像座標、フラグAppFlg、フラグDisappFlgを位置姿勢算出部１４０に出力する。
【００７４】
次にステップＳ９０７では、データ比較部２２０は、AppFlgの値を参照し、AppFlg=1であれば、処理をステップＳ９０８に進め、注目フレームの画像上に位置する共通指標の識別子と画像座標を位置姿勢算出部１４０に出力する。
【００７５】
次にステップＳ９０９では、データ比較部２２０は、DisappFlgの値を参照し、DisappFlg=1であれば処理をステップＳ９１０に進め、前フレームの画像上に位置する共通指標の識別子と画像座標を位置姿勢算出部１４０に出力する。
【００７６】
そして最後にステップＳ９１１では、位置姿勢算出部１４０が、注目フレームにおける撮像装置１５０の位置姿勢を求める処理を行う。
【００７７】
図６は、上記ステップＳ９１１における処理の詳細を示すフローチャートである。
【００７８】
ここで以下の説明では、撮像装置１５０の位置を、３値のベクトルｔ＝［ｘ  ｙ  ｚ］Ｔによって内部的に表現しているものとする。また、撮像装置１５０の姿勢を、カメラ座標系から基準座標系への回転変換を行う３×３の回転行列Ｒによって内部的に表現しているものとする。また、姿勢の情報は、必要に応じて、回転軸を表す３値のベクトルω＝［ξ  ψ  ζ］Ｔと、その軸まわりの回転角θの形式で利用する。また、Ｒ（ω，θ）は、回転軸ω，回転角θの姿勢に対応する回転行列を表すものとする。回転行列Ｒによる姿勢表現と、回転軸ωと回転角θによる姿勢表現との間の相互変換方法は公知であるので、その説明は省略する。
【００７９】
先ず、ステップＳ６０００では、データ入力部４１０が、指標選択部１３０から出力されたデータ（指標検出データ）を受ける。ステップＳ６０１０では先ず、第一位置姿勢演算部４２０が、データ入力部４１０から注目フレームの画像上に位置する全ての指標の画像座標を受ける。更に第一位置姿勢演算部４２０は、注目フレームの画像上に位置する全ての指標の識別子を用いて、注目フレームの画像上に位置する全ての指標の基準座標系における位置を外部記憶装置１００７から取得する。
【００８０】
そして第一位置姿勢演算部４２０は、データ入力部４１０から受けた全ての指標の画像座標と、注目フレームの画像上に位置する全ての指標の基準座標系における位置とを用いて、撮像装置１５０の位置姿勢の推定値を算出する。ここで、算出した位置の推定値をｔ１，姿勢の推定値をＲ１とする。なお、本ステップで得られる位置姿勢の推定値は、フレーム間の連続性を考慮せずにフレーム毎に最適な位置姿勢を推定する従来方法の出力に相当する。
【００８１】
次にステップＳ６０２０では、位置姿勢統合部４５０が、経過フレーム数カウンタＣｔ（初期値は０）に１を加算する。ここで、経過フレーム数カウンタとは、フレーム間での指標の出現や消失が生じたフレームからの経過フレーム数を示すカウンタである。
【００８２】
次にステップＳ６０３０では、位置姿勢統合部４５０が、経過フレーム数カウンタＣｔの値に基づいて、補正値の基準値（後述）に乗算する重みｗを算出する。例えば、次式によって重みｗを算出する。
【００８３】
ｗ＝｛ｃｏｓ（Ｃｔ／Ｄｒ×π）＋１｝／２      （式１）
ここで、Ｄｒは補正期間を表している。補正期間とは、あるフレーム間で指標の出現や消失が生じた際に、その現象に起因する不整合の回避処理（補正処理）をどの程度の期間行うかを表すパラメータである。例えば、３０フレームの期間で補正処理を行う場合は、Ｄｒを３０に設定する。この値は予め設定された所定値としても良いし、情報処理装置１００の使用者がキーボード１００４やマウス１００５を用いて対話的に調整してもよい。（式１）において、Ｃｔ＝０の時にｗ＝１となり、Ｃｔ＝Ｄｒの時にｗ＝０となる。このようにｗの値が徐々に減衰していく関数であれば、（式１）以外の関数によってｗを決定してもよい。例えば、次式を用いてもよい。
【００８４】
ｗ＝（Ｄｒ−Ｃｔ）／Ｄｒ      （式２）
次に、ステップＳ６０４０では、位置姿勢統合部４５０が、ステップＳ６０３０で求めた重みｗを、現在の補正値の基準値に乗算することで、補正値の基準値を更新する。ここで、補正値とは、ステップＳ６０１０で得た位置姿勢の推定値（位置推定値ｔ１、姿勢推定値Ｒ１）を、フレーム間での整合性が維持できるように補正するためのパラメータである。従って、ステップＳ６０４０では、位置の補正値Δtと、姿勢の補正値の回転成分Δθを、次式によって算出する。
【００８５】
Δｔ＝ｗ×ｔｂ      （式３）
Δθ＝ｗ×θｂ      （式４）
ここで、ｔｂは、位置の補正値の基準値を表している。また、θｂは、姿勢の補正値の基準値Ｒｂの回転角を表している。さらにステップＳ６０４０では、（式４）で求めたΔθと、姿勢の補正値の基準値Ｒｂの回転軸ωｂ＝［ξｂ  ψｂ  ζｂ］Ｔを用いて以下の（式５）に従った計算処理を行い、姿勢の補正値ΔＲを算出する。
【００８６】
ΔＲ＝Ｒ（ωｂ、Δθ）      （式５）
重みｗは時間経過と共に減衰していくので、位置の補正値の値は、時間経過に応じて徐々に０向かって減少していくことになる。同様に、姿勢の補正値は、時間経過に応じて徐々に単位行列へと近づいていくことになる。
【００８７】
次に、前フレームの画像上には存在しないが、注目フレームの画像上には存在する指標、前フレームの画像上には存在するものの、注目フレームの画像上には存在しない指標の何れかが存在する場合（DisappFlg + AppFlg ≠ 0）には、処理をステップＳ６０５０を介してステップＳ６０６０に進める。一方、前フレームから注目フレームに移行した場合に、画像上で新たに出現した指標、消失した指標が存在しない場合（DisappFlg + AppFlg = 0）には、処理をステップＳ６０５０を介してステップＳ６１５０に進める。
【００８８】
ステップＳ６０６０では、データ入力部４１０がDisappFlgの値を参照し、DisappFlg=1である場合には処理をステップＳ６０７０に進め、DisappFlg=0である場合には処理をステップＳ６０９０に進める。
【００８９】
ステップＳ６０７０では先ず、第三位置姿勢演算部４４０が、データ入力部４１０から、前フレームの画像上に位置する共通指標の識別子、画像座標のセットを受ける。更に、第三位置姿勢演算部４４０は、前フレームの画像上に位置する共通指標の識別子を用いて、前フレームの画像上に位置する共通指標の基準座標系における位置を外部記憶装置１００７から取得する。そして第三位置姿勢演算部４４０は、データ入力部４１０から受けた前フレームの画像上に位置する共通指標の画像座標と、前フレームの画像上に位置する全ての指標の基準座標系における位置とを用いて、撮像装置１５０の位置姿勢の推定値を算出する。ここで、算出した位置の推定値をｔ３，姿勢の推定値をＲ３とする。なお、本ステップで得られる位置姿勢の推定値は、注目フレームで消失した指標が、仮に、前フレームの段階で既に消失していたと仮定したときの、撮像装置１５０の位置姿勢を表している。
【００９０】
次にステップＳ６０８０では、位置姿勢統合部４５０は、前フレームから注目フレームに進めた場合に、画像上で消失指標が存在したことによる撮像装置１５０の位置の変化量、姿勢の変化量を算出する。この算出には、ステップＳ６０７０で求めた位置の推定値ｔ３と姿勢の推定値Ｒ３と、前フレームにおける撮像装置１５０の位置姿勢として最終的に求めた位置ｔ０，姿勢Ｒ０を用いる。位置の変化量をΔｔｄ、姿勢の変化量をΔＲｄとすると、これらは以下の式に従って求める。
【００９１】
Δｔｄ＝ｔ０−ｔ３                    （式６）
ΔＲｄ＝Ｒ０×Ｒ３−１      （式７）
なお、本ステップで得られる変化量は、指標の消失によって生じる位置及び姿勢の不連続性を直接的に表したものとなる。
【００９２】
次に、ステップＳ６０９０では、データ入力部４１０がAppFlgの値を参照し、AppFlg=1である場合には処理をステップＳ６１００に進め、AppFlg=0である場合には処理をステップＳ６１３０に進める。
【００９３】
ステップＳ６１００では、第二位置姿勢演算部４３０が、データ入力部４１０から、注目フレームの画像上に位置する共通指標の識別子、画像座標のセットを受ける。更に、第二位置姿勢演算部４３０は、注目フレームの画像上に位置する共通指標の識別子を用いて、注目フレームの画像上に位置する共通指標の基準座標系における位置を外部記憶装置１００７から取得する。そして第二位置姿勢演算部４３０は、データ入力部４１０から受けた注目フレームの画像上に位置する共通指標の画像座標と、注目フレームの画像上に位置する全ての指標の基準座標系における位置とを用いて、撮像装置１５０の位置姿勢の推定値を算出する。ここで、算出した位置の推定値をｔ２，姿勢の推定値をＲ２とする。
【００９４】
次に、ステップＳ６１１０では、第二位置姿勢演算部４３０は、ステップＳ６１００で求めた位置の推定値ｔ２，姿勢の推定値Ｒ２を、ステップＳ６０４０で更新した補正値でもって補正する処理を行う。より具体的には、以下の式に従って、位置の推定値ｔ２、姿勢の推定値Ｒ２を補正する。
【００９５】
ｔ２’＝ｔ２＋Δｔ    （式８）
Ｒ２’＝ΔＲ×Ｒ２    （式９）
ｔ２’は補正後の位置の推定値、Ｒ２’は補正後の姿勢の推定値である。
【００９６】
次にステップＳ６１２０では、位置姿勢統合部４５０は、前フレームから注目フレームに進めた場合に、画像上で出現指標が存在したことによる撮像装置１５０の位置の変化量、姿勢の変化量を算出する。この算出には、ステップＳ６０１０で求めた位置の推定値ｔ１と姿勢の推定値Ｒ１と、ステップＳ６１１０で求めたｔ２’、Ｒ２’とを用いる。位置の変化量をΔｔａ、姿勢の変化量をΔＲａとすると、これらは以下の式に従って求める。
【００９７】
Δｔａ＝ｔ２’−ｔ１                    （式１０）
ΔＲａ＝Ｒ２’×Ｒ１−１    （式１１）
なお、本ステップで得られる変化量は、指標の出現によって生じる撮像装置１５０の位置姿勢の不連続性を直接的に表したものとなる。
【００９８】
次に、ステップＳ６１３０では、位置姿勢統合部４５０は、注目フレーム以降のフレームについて本フローチャートに従った処理を行う際に用いる補正値の基準値を求める。具体的には、以下の式に従った計算処理を行うことで、補正値の基準値を求める。
【００９９】
ｔｂ＝ΔＲａ×Δｔｄ＋Δｔａ    （式１２）
Ｒｂ＝ΔＲａ×ΔＲｄ            （式１３）
補正値の基準値とは、あるフレーム間で指標の出現や消失が生じた場合に、そのフレーム間における不整合を回避するための補正値である。注目フレームにおけるｔ１，Ｒ１にそれぞれｔｂ、Ｒｂを付加することで、このフレームにおけるフレーム間の不整合を回避することができる。さらに、その後に続くフレームでは、時間経過に応じて減少していく重みをこの値に乗算することで得た補正値を付加していく。補正値を減衰させていくことで、補正を行わない状態（すなわち、フレーム内で最適な位置及び姿勢が得られている状態）に徐々に近づけていく。
【０１００】
次に、ステップＳ６１４０では、位置姿勢統合部４５０が、ステップＳ６１３０で算出した補正値の基準値を、注目フレームに対する補正値として以下のように設定する。
【０１０１】
Δｔ＝ｔｂ    （式１４）
Δθ＝θｂ    （式１５）
以上説明したステップＳ６０６０からステップＳ６１４０における処理を行うことで、前フレームと注目フレームとの間で消失指標、出現指標が存在した場合に対処するために、補正値の基準値を設定することができる。
【０１０２】
次にステップＳ６１５０では、位置姿勢統合部４５０が、ステップＳ６０１０で求めた位置の推定値ｔ１と姿勢の推定値Ｒ１をそれぞれ、ステップＳ６０４０、又はステップＳ６１４０で求めた補正値Δｔ、ΔＲを用いて補正し、「注目フレームにおける撮像装置１５０の位置姿勢」として最終的に求めるべき位置ｔ１’、姿勢Ｒ１’を求める処理を行う。この処理は以下の計算式を計算することで行う。
【０１０３】
ｔ１’＝ｔ１＋Δｔ    （式１６）
Ｒ１’＝ΔＲ×Ｒ１    （式１７）
ここで、指標の見え隠れがフレーム間で生じない期間がＤｒフレーム以上継続すると、Δｔが０、ΔＲが単位行列となるので、ｔ１’、Ｒ１’としてｔ１，Ｒ１がそのまま用いられることになる。
【０１０４】
次にステップＳ６１６０では、位置姿勢統合部４５０が、ステップＳ６１５０で求めた位置ｔ１’、姿勢Ｒ１’を出力する。出力先としては、外部記憶装置１００７のように、コンピュータ内部に対する出力でも良いが、Ｉ／Ｆ１００９を介して外部に出力するようにしても良い。
【０１０５】
なお、本実施形態では、撮像装置１５０の位置を３値のベクトルｔ１’で、姿勢を３×３の回転行列Ｒ１’によって表現した。しかし、位置姿勢の表現方法はこれに限定されるものではない。例えば、姿勢を、回転軸ベクトルと回転角、オイラー角、４元数等に変換して出力してもよい。また、同次座標表現による４×４の位置姿勢変換行列やその逆行列によって、位置及び姿勢を表してもよい。また、撮像装置１５０に対する基準座標系の位置及び姿勢を求めてこれを出力してもよい。もちろん、これらのいずれかの組み合わせを同時に出力してもよい。
【０１０６】
最後にステップＳ６１７０では、ステップＳ６１５０で求めた、位置ｔ１’、姿勢Ｒ１’を、注目フレーム以降のフレームについて同図のフローチャートに従った処理を行うために、それぞれｔ０，Ｒ０として外部記憶装置１００７やＲＡＭ１００２に格納する。
【０１０７】
以上説明したように、本実施形態によれば、フレーム間における指標の出現・消失の状況を監視することで、指標の見え隠れに起因する撮像装置１５０の位置姿勢計測に係る不整合を解消することが可能となる。その結果、フレーム間の整合性と動きに対する追従性を両立しながら、撮像装置１５０の位置及び姿勢を計測することができる。
【０１０８】
なお、本実施形態では、図１に示した画像入力部１１０、指標検出部１２０、指標選択部１３０、位置姿勢算出部１４０は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとした。しかし、一部をハードウェアでもって実現するようにしても良い。例えば、指標検出部１２０をグラフィックボードでもって構成するようにしても良い。
【０１０９】
更に、本実施形態では、画像入力部１１０、指標検出部１２０、指標選択部１３０、位置姿勢算出部１４０の各部を１つの装置内にソフトウェアとしてインストールしていた。しかしこれに限定するものではなく、２以上のコンピュータに分けてインストールし、それぞれのコンピュータ間で共同動作を行うことで、上記各処理を実現させても良い。これは、画像入力部１１０、指標検出部１２０、指標選択部１３０、位置姿勢算出部１４０のうち１以上をハードウェアでもって実現するようにしても同様である。
【０１１０】
［第２の実施形態］
本実施形態では、現実空間中に配した複数の撮像装置を用いて、計測対象物体の位置を計測する。図７は、複数の撮像装置を用いて計測対象物体の位置を計測するための本実施形態に係るシステムの機能構成を示すブロック図である。同図に示す如く、本実施形態に係るシステムは、撮像装置７５０ａ〜７５０ｄ、情報処理装置７００により構成されている。更に、情報処理装置７００は、画像入力部７１０、指標検出部７２０、指標選択部７３０、位置算出部７４０により構成されている。
【０１１１】
更に７６０は、本実施形態で計測対象物体の一例として用いるものであり、計測対象物体７６０には指標７７０が取り付けられている。情報処理装置７００は、基準座標系に対するこの指標７７０の位置を、計測対象物体７６０の位置として計測する。
【０１１２】
撮像装置７５０ａ〜７５０ｄは、現実空間の動画像が撮像可能なカメラであり、例えば、ディジタルビデオカメラである。それぞれの撮像装置７５０ａ〜７５０ｄが撮像した各フレームの画像は情報処理装置７００が有する画像入力部７１０に入力される。なお、それぞれの撮像装置７５０ａ〜７５０ｄのカメラ内部パラメータ（焦点距離やフォーカス値など）や、基準座標系における位置姿勢は既知であるものとする。
【０１１３】
画像入力部７１０は、入力された各フレームの画像信号をディジタルデータに変換し、後段の指標検出部７２０に送出する。即ち、画像入力部７１０は、各フレームの画像データを指標検出部７２０に供給することになる。なお、本実施形態では、撮像装置７５０ａ〜７５０ｄのそれぞれは、画像入力部７１０がそれぞれの撮像装置７５０ａ〜７５０ｄを識別可能なように画像入力部７１０に接続されているものとする。しかし、画像入力部７１０が撮像装置７５０ａ〜７５０ｄからの画像を受けた場合に、受けた画像がどの撮像装置からのものであるのかを画像入力部７１０が把握可能な構成であれば、その構成は如何なるものであっても良い。例えば、撮像装置７５０ａ〜７５０ｄは、撮像したフレーム画像と共に、自身に固有の識別情報を画像入力部７１０に送出するようにしても良い。
【０１１４】
指標検出部７２０は、画像入力部７１０から入力された各フレームについて、画像中に位置している指標７７０を検出する処理を行う。より詳しくは、画像中に位置している指標７７０を識別し、その画像座標を検出する。そして指標検出部７２０は、検出された指標７７０の画像座標を、指標選択部７３０へと出力する。
【０１１５】
ここで、指標検出部７２０が、画像から指標７７０を検出することができなかった場合には、その旨を指標選択部７３０に通知する。例えば、図７の例において、撮像装置７５０ａ，７５０ｃ，７５０ｄから受けた画像からは指標７７０を検出したものの、撮像装置７５０ｂから受けた画像からは指標７７０が検出されなかったとする。この場合、指標検出部７２０からは、撮像装置７５０ａ，７５０ｃ，７５０ｄのそれぞれから受けた画像における指標７７０の画像座標と、撮像装置７５０ｂから受けた画像からは指標７７０が検出されなかったという情報が出力される。なお、指標７７０の検出方法については第１の実施形態に記載の如く、如何なる手法を用いても良い。
【０１１６】
指標選択部７３０は、指標検出部１２０から、「指標７７０の検出ができなかった旨」若しくは指標７７０の画像座標を入力する。そして、指標７７０の画像座標を受けた場合には、これを用いてフレーム間での指標７７０の消失・出現を監視し、計測対象物体７６０の位置の計算のために必要な情報を選択し、位置算出部７４０へと出力する。
【０１１７】
ここで、指標選択部７３０が行う処理について、より詳細に説明する。指標選択部７３０は先ず、撮像装置７５０ａ〜７５０ｄのそれぞれから受けた前フレーム、注目フレームの両方のフレームから指標７７０が検出されたかをチェックする。そして、前フレーム、注目フレーム共に指標７７０の検出が成功した場合には、この２つのフレームの画像を撮像した撮像装置のリストを作成する。
【０１１８】
図７の構成の場合、撮像装置７５０ａから受けた前フレーム、注目フレームの両方のフレームから指標７７０を検出した場合には、撮像装置７５０ａ固有の識別情報を保持する。撮像装置７５０ｂから受けた前フレーム、注目フレームの両方のフレームから指標７７０を検出した場合には、撮像装置７５０ｂ固有の識別情報を保持する。撮像装置７５０ｃから受けた前フレーム、注目フレームの両方のフレームから指標７７０を検出した場合には、撮像装置７５０ｃ固有の識別情報を保持する。撮像装置７５０ｄから受けた前フレーム、注目フレームの両方のフレームから指標７７０を検出した場合には、撮像装置７５０ｄ固有の識別情報を保持する。
【０１１９】
このように、前フレーム、注目フレームの両方のフレームから指標７７０が検出された場合には、この２つのフレームを撮像した撮像装置固有の識別情報のリストを作成し、保持する。
【０１２０】
更に、指標選択部７３０は、撮像装置７５０ａ〜７５０ｄから受けた注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標７７０が検出されなかった場合には、この撮像装置に固有の識別情報を保持する。図７の構成の場合、撮像装置７５０ａから受けた注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標７７０が検出されなかった場合には、撮像装置７５０ａ固有の識別情報を保持する。撮像装置７５０ｂから受けた注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標が検出されなかった場合には、撮像装置７５０ｂ固有の識別情報を保持する。撮像装置７５０ｃから受けた注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標が検出されなかった場合には、撮像装置７５０ｃ固有の識別情報を保持する。撮像装置７５０ｄから受けた注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標が検出されなかった場合には、撮像装置７５０ｄ固有の識別情報を保持する。
【０１２１】
このように、注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標７７０が検出されなかった場合には、この２つのフレームを撮像した撮像装置固有の識別情報のリストを作成し、保持する。
【０１２２】
更に、指標選択部７３０は、撮像装置７５０ａ〜７５０ｄから受けた前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった場合には、この撮像装置に固有の識別情報を保持する。図７の構成の場合、撮像装置７５０ａから受けた前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった場合には、撮像装置７５０ａ固有の識別情報を保持する。撮像装置７５０ｂから受けた前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった場合には、撮像装置７５０ｂ固有の識別情報を保持する。撮像装置７５０ｃから受けた前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった場合には、撮像装置７５０ｃ固有の識別情報を保持する。撮像装置７５０ｄから受けた前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった場合には、撮像装置７５０ｄ固有の識別情報を保持する。
【０１２３】
このように、前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった場合には、この２つのフレームを撮像した撮像装置固有の識別情報のリストを作成し、保持する。
【０１２４】
そして、指標選択部７３０は、下記の如く、条件に基づいて以下のようなデータを位置算出部７４０に出力する。
【０１２５】
（１）  撮像装置７５０ａ〜７５０ｄが撮像した注目フレームの画像上に位置する指標７７０の画像座標、若しくは指標の検出ができなかった旨を示す情報
（２）  フラグAppFlgの値（０または１）
（３）  フラグDisappFlgの値（０または１）、
（４）  AppFlg=1である場合  →  前フレーム、注目フレームの両方のフレームから指標７７０が検出された場合に、この２つのフレームの画像を撮像した撮像装置から受けた注目フレームの画像上に位置する指標７７０の画像座標
（５）  DisappFlg=1である場合  →  前フレーム、注目フレームの両方のフレームから指標７７０が検出された場合に、この２つのフレームの画像を撮像した撮像装置から受けた前フレームの画像上に位置する指標７７０の画像座標
ここで、AppFlgは、注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標７７０が検出されなかった、というような両方のフレームを撮像した撮像装置が１以上の存在した場合に１となるものである。AppFlgはデフォルトでは０に初期化されている。
【０１２６】
また、DisappFlgは、前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった、というような両方のフレームを撮像した撮像装置が１以上の存在した場合に１となるものである。DisappFlgはデフォルトでは０に初期化されている。
【０１２７】
位置算出部７４０は、指標選択部７３０から受けた指標の画像座標を用いて、計測対象物体７６０の位置を算出する処理を行う。
【０１２８】
なお、本実施形態においても、情報処理装置７００のハードウェア構成については第１の実施形態と同様、即ち、図５に示した構成を有するコンピュータを適用する。更に、本実施形態においても、上記画像入力部７１０、指標検出部７２０、指標選択部７３０、位置算出部７４０は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。しかし、一部をハードウェアでもって実現するようにしても良い。例えば、指標検出部７２０をグラフィックボードでもって構成するようにしても良い。
【０１２９】
更に、画像入力部７１０、指標検出部７２０、指標選択部７３０、位置算出部７４０を２以上のコンピュータに分けてインストールし、それぞれのコンピュータ間で共同動作を行うことで、計測対象物体７６０の位置計測処理を実現させても良い。これは、画像入力部７１０、指標検出部７２０、指標選択部７３０、位置算出部７４０のうち１以上をハードウェアでもって実現するようにしても同様である。
【０１３０】
図１０は、注目フレームにおける計測対象物体７６０の位置を求めるために情報処理装置７００が行う処理のフローチャートである。従って、各フレームについて同様の処理を行う場合には、同図のフローチャートに従った処理をこの各フレームについて行えば良い。
【０１３１】
なお、同図のフローチャートに従った処理をＣＰＵ１００１に実行させるためのプログラムやデータは外部記憶装置１００７に保存されている。そしてこれをＣＰＵ１００１による制御に従ってＲＡＭ１００２にロードし、ＣＰＵ１００１がこれを用いて処理を実行することで、後述の各処理を実現する。
【０１３２】
先ずステップＳ１０００では、画像入力部７１０により、各撮像装置７５０ａ〜７５０ｄが撮像した注目フレームの画像データを指標検出部７２０に供給する。次にステップＳ１００１では、指標検出部７２０が、それぞれの注目フレームの画像中に位置している指標７７０を識別し、その画像座標を検出する。そして指標検出部７２０は、検出された指標７７０の画像座標を、指標選択部７３０へと出力する。なお、検出ができなかった場合には、その旨を指標選択部７３０に通知する。
【０１３３】
次にステップＳ１００２では、指標選択部７３０は、指標検出部７２０から注目フレームの画像上に位置する指標の画像座標、若しくは指標７７０が検出できなかった旨を受けると、これを保持する。次にステップＳ１００３では、指標選択部７３０は、上記フラグDisappFlgを設定すると共に、ステップＳ１００４では、AppFlgを設定する。
【０１３４】
DisappFlgフラグの設定については上述の通り、注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標７７０が検出されなかった、というような両方のフレームを撮像した撮像装置が１以上の存在した場合には１に設定する。AppFlgはデフォルトでは０に初期化されている。また、DisappFlgについても上述の通り、前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった、というような両方のフレームを撮像した撮像装置が１以上の存在した場合には１を設定する。DisappFlgはデフォルトでは０に初期化されている。
【０１３５】
そしてステップＳ１００５では、指標選択部７３０は、前フレーム、注目フレーム共に指標７７０の検出が成功した場合には、この２つのフレームの画像を撮像した撮像装置固有の識別情報のリストを作成し、保持する。更に、指標選択部７３０は、注目フレームの画像からは指標７７０が検出されたものの、前フレームの画像からは指標７７０が検出されなかった場合には、この２つのフレームの画像を撮像した撮像装置に固有の識別情報のリストを作成し、保持する。更に、指標選択部７３０は、前フレームの画像からは指標７７０が検出されたものの、注目フレームの画像からは指標７７０が検出されなかった場合には、この２つのフレームの画像を撮像した撮像装置に固有の識別情報のリストを作成し、保持する。
【０１３６】
次に、ステップＳ１００６では、指標選択部７３０は、それぞれの撮像装置７５０ａ〜７５０ｄからの注目フレームの画像上に位置する指標７７０の画像座標若しくは指標の検出ができなかった旨を示す情報、フラグAppFlg、フラグDisappFlgを位置算出部７４０に出力する。
【０１３７】
次にステップＳ１００７では、指標選択部７３０は、AppFlgの値を参照する。そしてAppFlg=1であれば、処理をステップＳ１００８に進める。ステップＳ１００８では、前フレーム、注目フレームの両方のフレームから指標７７０が検出された場合には、この２つのフレームの画像を撮像した撮像装置から受けた注目フレームの画像上に位置する指標７７０の画像座標を位置算出部７４０に出力する。
【０１３８】
次にステップＳ１００９では、指標選択部７３０は、DisappFlgの値を参照する。そして、DisappFlg=1であれば処理をステップＳ１０１０に進める。ステップＳ１０１０では、前フレーム、注目フレームの両方のフレームから指標７７０が検出された場合には、この２つのフレームの画像を撮像した撮像装置から受けた前フレームの画像上に位置する指標７７０の画像座標を位置算出部７４０に出力する。
【０１３９】
そして最後にステップＳ１０１１では、位置算出部７４０が、注目フレームにおける計測対象物体７６０の位置を求める処理を行う。
【０１４０】
図８は、上記ステップＳ１０１１における処理の詳細を示すフローチャートである。以下では、計測対象物体７６０の位置を、３値のベクトルｔ＝［ｘ  ｙ  ｚ］Ｔによって内部的に表現しているものとする。
【０１４１】
先ず、ステップＳ８０００では、指標選択部７３０から出力された、２以上の撮像装置による注目フレーム上における指標７７０の座標位置データ（指標検出データ）を受ける。ステップＳ８０１０では、各注目フレームの画像上に位置する指標７７０の座標位置を用いて、三角測量の原理に基づいて、計測対象物体７６０の位置の推定値を算出する。もちろん、この三角測量に基づく計算には、それぞれの撮像装置の基準座標系における位置も用いる。
【０１４２】
ここで、算出した位置の推定値をｔ１とする。具体的には、各注目フレームの画像上における指標７７０の再投影誤差の和が最小となるように、最小二乗法によって推定値ｔ１を決定する。
【０１４３】
次にステップＳ８０２０では、経過フレーム数カウンタＣｔ（初期値は０）に１を加算する。次にステップＳ８０３０では、経過フレーム数カウンタＣｔの値に基づいて、補正値の基準値（後述）に乗算する重みｗを算出する。ステップＳ８０２０，Ｓ８０３０における処理内容はそれぞれ、上記ステップＳ６０２０，Ｓ６０３０と同様にして行う。
【０１４４】
次に、ステップＳ８０４０では、ステップＳ８０３０で求めた重みｗを、現在の補正値の基準値に乗算することで、補正値の基準値を更新する。この処理は、上記（式３）に従った計算処理を行うことでなされるものである。
【０１４５】
次に、DisappFlg + AppFlg ≠ 0の場合には、処理をステップＳ８０５０を介してステップＳ８０６０に進める。一方、DisappFlg + AppFlg = 0の場合には、処理をステップＳ８０５０を介してステップＳ８１５０に進める。
【０１４６】
ステップＳ８０６０では、DisappFlgの値を参照し、DisappFlg=1である場合には処理をステップＳ８０７０に進め、DisappFlg=0である場合には処理をステップＳ８０９０に進める。
【０１４７】
ステップＳ８０７０では、それぞれの撮像装置から得られた前フレームの画像上における指標７７０の画像座標を用いて、三角測量の原理でもって、計測対象物体７６０の位置の推定値を算出する。ここで、算出した位置の推定値をｔ３とする。
【０１４８】
次にステップＳ８０８０では、ステップＳ８０７０で求めた位置の推定値ｔ３と、前フレームにおける計測対象物体７６０の位置として最終的に求めた位置ｔ０との差分値Δｔｄを上記（式６）に従って求める。
【０１４９】
次に、ステップＳ８０９０では、AppFlgの値を参照し、AppFlg=1である場合には処理をステップＳ８１００に進め、AppFlg=0である場合には処理をステップＳ８１３０に進める。
【０１５０】
ステップＳ８１００では、それぞれの撮像装置から得られた注目フレームの画像上における指標７７０の画像座標を用いて、三角測量の原理でもって、計測対象物体７６０の位置の推定値を算出する。ここで、算出した位置の推定値をｔ２とする。
【０１５１】
次にステップＳ８１１０では、ステップＳ８１００で求めた位置の推定値ｔ２を、ステップＳ８０４０で更新した補正値でもって補正する処理を行う。より具体的には、上記（式８）に従った処理を行う。補正後の位置の推定値をｔ２’とする。
【０１５２】
次にステップＳ８１２０では、ステップＳ８０１０で求めた位置の推定値ｔ１、ステップＳ８１１０で求めたｔ２’を用い、上記（式１０）を用いてΔｔａを求める。
【０１５３】
次に、ステップＳ８１３０では、注目フレーム以降のフレームについて本フローチャートに従った処理を行う際に用いる補正値の基準値を求める。具体的には、以下の式に従った計算処理を行うことで、補正値の基準値を求める。
【０１５４】
ｔｂ＝Δｔｄ＋Δｔａ    （式１８）
次に、ステップＳ８１４０では、ステップＳ８１３０で算出した補正値の基準値を、注目フレームに対する補正値として、上記（式１４）に示す如く設定する。
【０１５５】
以上説明したステップＳ８０６０からステップＳ８１４０における処理を行うことで、前フレームと注目フレームとの間で消失指標、出現指標が存在した場合に対処するために、補正値の基準値を設定することができる。
【０１５６】
次にステップＳ８１５０では、ステップＳ８０１０で求めた位置の推定値ｔ１を、ステップＳ８０４０、又はステップＳ８１４０で求めた補正値Δｔを用いて補正することで、「注目フレームにおける計測対象物体７６０の位置」として最終的に求めるべき位置ｔ１’を求める。この処理は上記（式１６）に従った計算処理を行うことでなされる。
【０１５７】
次にステップＳ８１６０では、ステップＳ８１５０で求めた位置ｔ１’を出力する。出力先としては、外部記憶装置１００７のように、コンピュータ内部に対する出力でも良いが、Ｉ／Ｆ１００９を介して外部に出力するようにしても良い。
【０１５８】
最後にステップＳ８１７０では、ステップＳ８１５０で求めた、位置ｔ１’を、注目フレーム以降のフレームについて同図のフローチャートに従った処理を行うためにｔ０として外部記憶装置１００７やＲＡＭ１００２に格納する。
【０１５９】
以上説明したように、本実施形態によれば、フレーム間における指標の出現・消失の状況を監視することで、指標の見え隠れに起因する計測対象物体７６０の位置計測に係る不整合を解消することが可能となる。その結果、フレーム間の整合性と動きに対する追従性を両立しながら、計測対象物体７６０の位置を計測することができる。
【０１６０】
［第３の実施形態］
＜変形例１＞
第１の実施形態では、撮像装置により撮像された画像のみを用いて、この撮像装置の位置姿勢の計測を行った。換言すれば、第１の実施形態に係る位置姿勢計測方法は、撮像装置により撮像された画像のみを用いて位置姿勢の計測を行う一般的な位置姿勢計測方法を、スムーズかつ高精度な計測が可能なように改良したものといえる。しかし、指標を利用して位置姿勢を求める方法であれば、これ以外の方法であっても、第１の実施形態に係る計測方法との組み合わせによって同様な改良を行うことができる。
【０１６１】
＜変形例２＞
上記非特許文献１には、計測対象物体に装着した１台以上の主観視点カメラと、天井や壁に配置した１台以上の客観視点カメラを用いて、物体の位置及び姿勢を計測する方法が開示されている。この方法では、主観視点カメラの撮影画像から環境中の指標が検出され、客観視点カメラの撮影画像から計測対象物体上の指標が検出される。そして、これらの全ての検出指標の情報を利用して、計測対象物体の位置及び姿勢が算出される。
【０１６２】
第１の実施形態で示した手法は、画像入力部１１０、指標検出部１２０、指標選択部１３０、位置姿勢算出部１４０の一部の機能を変更することで、このような複数の撮像装置を用いる方法と容易に組み合わせることができる。この場合、画像入力部１１０は、夫々のカメラからの画像を入力する。また、指標検出部１２０は、夫々のカメラの入力画像から指標を検出し、識別子と検出座標を出力する。そして、指標選択部１３０は、各カメラの検出結果に対して、連続フレーム間における共通指標の選択と、指標の出現・消失の有無の判定を行う。複数の撮像装置を用いる場合には、新たな指標を検出したカメラが１台でもあれば、AppFlgを1に設定する。同様に、前フレームまで検出していた指標を見失ったカメラが１台でもあれば、DisappFlgを1に設定する。そして、以下のデータを出力する。
【０１６３】
（Ａ）  注目フレームの入力画像に対する、全カメラの全検出指標の識別子と画像座標（指標の出現・消失の状況によらずに出力）
（Ｂ）  フラグAppFlg（０または１）
（Ｃ）  注目フレームの入力画像に対する、全カメラの共通指標の識別子と画像座標（出現した指標が存在する場合のみ）
（Ｄ）  フラグDisappFlg（０または１）
（Ｅ）  前フレームの入力画像に対する、全カメラの共通指標の識別子と画像座標（消失した指標が存在する場合のみ）
位置姿勢算出部１４０の動作は、位置姿勢演算部４２０、４３０、４４０の夫々が複数のカメラからの情報を利用すること以外は、第１の実施形態と同様である。なお、複数のカメラからの情報を利用して計測対象物体の位置及び姿勢を求める方法は、非特許文献１に開示されているので、ここでの詳細な説明は省略する。
【０１６４】
なお、１台以上の客観視点カメラを用いて複数の指標を配置した物体の位置及び姿勢を求める場合にも、同様な改良により発明の効果を得ることができる。この場合は、上記の変形例から、主観視点カメラを除去して考えればよい。
【０１６５】
＜変形例３＞
一方、撮像装置１５０に姿勢センサを装着し、その姿勢計測値と指標の検出座標とを併用することで、撮像装置１５０の位置及び姿勢を求めることがなされている。例えば、非特許文献１では、姿勢センサで計測した姿勢を、撮像装置１５０の姿勢としてそのまま利用する方法が開示されている。この方法では、撮像装置１５０の位置のみが未知パラメータであり、この未知パラメータが指標の検出座標を用いて算出される。また、非特許文献１には、撮像装置１５０の位置と方位角を未知パラメータとして、指標の検出座標を用いて該未知パラメータを算出する方法も開示されている。
【０１６６】
第１の実施形態で示した手法は、位置姿勢算出部１４０の一部の機能を変更することで、このような他の計測手段を利用する方法と容易に組み合わせることができる。この場合、データ入力部４１０は、姿勢センサからの姿勢計測値をさらに入力する。また、第一位置姿勢演算部４２０は、現フレームの姿勢計測値と現フレームの全検出指標の情報に基づいて、撮像装置１５０の位置（及び方位角）を推定する。また、第二位置姿勢演算部４３０は、現フレームの姿勢計測値と現フレームの共通指標の情報に基づいて、撮像装置１５０の位置（及び方位角）を推定する。また、第三位置姿勢演算部４４０は、前フレームの姿勢計測値と前フレームの共通指標の情報に基づいて、撮像装置１５０の位置（及び方位角）を算出する。
【０１６７】
そして、位置姿勢統合部４５０は、夫々の位置姿勢演算部で得られた位置（及び方位角）に基づいて、現フレームにおける撮像装置１５０の位置（及び方位角）を算出する。この位置の統合処理は、第１の実施形態におけるフローチャートにおいて、位置のみの補正値を考えた場合と同様である。また、方位角の統合処理も位置と同様であり、指標が出現した場合の不連続性と消失した場合の不連続性をそれぞれ求め、その和として方位角補正値の基準値を求めてこれを用いればよい。
【０１６８】
このように、フレーム間における指標の出現と消失を監視し、共通指標を用いて求めた位置及び姿勢（あるいはそれに準ずる方位角等のパラメータ）を利用して指標の見え隠れに起因する不整合を検知し、これを解消する補正を加えることが本発明の本質である。従って、指標を監視するカメラの数や種類（客観／主観視点）、指標の種類や検出方法、センサ等の付加情報の有無、位置及び姿勢の計算方法によらず、本発明の効果を得ることができる。
【０１６９】
＜変形例４＞
上記の実施形態では、前フレームで検出された全ての指標と、注目フレームで検出された全ての指標とを比較していた。しかし、指標の比較方法はこれに限定されるものではない。例えば、上記の実施形態で示した提案手法は、誤検出や誤同定された指標の情報（アウトライア）を除去するような位置及び姿勢の算出方法と組み合わせることができる。この場合、フレーム毎の処理においてアウトライアと判定された指標は、比較の対象として使用されない。
【０１７０】
また、検出指標数が非常に多い場合に、計算量を削減するために、一部の指標のみを使用して位置及び姿勢を推定する場合がある。上記の実施形態で示した提案手法は、このような方法と組み合わせることができる。この場合、位置及び姿勢の推定に実際に使用された指標のみが、フレーム間での比較対象となる。
【０１７１】
［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。
【０１７２】
また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１７３】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれたとする。その後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１７４】
本発明を上記記録媒体に適用する場合、その記録媒体には、先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０１７５】
【図１】撮像装置の位置姿勢を計測するための本発明の第１の実施形態に係るシステムの機能構成を示すブロック図である。
【図２】指標選択部１３０の機能構成を示すブロック図である。
【図３】撮像装置１５０が撮像した連続する２つのフレームの画像の例を示す図である。
【図４】位置姿勢算出部１４０の機能構成を示すブロック図である。
【図５】情報処理装置１００に適用可能なコンピュータのハードウェア構成を示すブロック図である。
【図６】ステップＳ９１１における処理の詳細を示すフローチャートである。
【図７】複数の撮像装置を用いて計測対象物体の位置を計測するための本発明の第２の実施形態に係るシステムの機能構成を示すブロック図である。
【図８】ステップＳ１０１１における処理の詳細を示すフローチャートである。
【図９】注目フレームにおける撮像装置１５０の位置姿勢を求めるために情報処理装置１００が行う処理のフローチャートである。
【図１０】注目フレームにおける計測対象物体７６０の位置を求めるために情報処理装置７００が行う処理のフローチャートである。
【図１】
2007271564.tif 000003
【図２】
2007271564.tif 000004
【図３】
2007271564.tif 000005
【図４】
2007271564.tif 000006
【図５】
2007271564.tif 000007
【図６】
2007271564.tif 000008
【図７】
2007271564.tif 000009
【図８】
2007271564.tif 000010
【図９】
2007271564.tif 000011
【図１０】
2007271564.tif 000012