JP2015215191A - 位置姿勢計測装置、方法及びプログラム - Google Patents

Abstract

【課題】第１の撮像装置の位置及び姿勢を算出可能な状態を維持できるようにする。【解決手段】位置姿勢計測装置１００は、主観視点カメラ１６０で撮像した主観視点画像から、現実空間中の座標が既知である指標の画像座標を検出し、また、追加視点カメラ１７０で撮像した追加視点画像から、現実空間中の座標が既知である指標の画像座標を検出して、指標の画像座標と、指標の現実空間中の座標と、主観視点カメラ１６０と追加視点カメラ１７０との相対的な位置及び姿勢の情報とに基づいて、主観視点カメラ１６０の位置及び姿勢を算出する。また、位置姿勢計測装置１００は、主観視点画像及び追加視点画像中の指標数が、主観視点カメラ１６０の位置及び姿勢を算出するのに必要な数となるように、追加視点カメラ１７０の姿勢及び焦点距離のうち少なくともいずれか一方を制御する。【選択図】図１

Description

本発明は、第１の撮像装置と、前記第１の撮像装置との相対的な位置が固定である第２の撮像装置とを用いて、前記第１の撮像装置の位置及び姿勢を算出する位置姿勢計測装置、方法及びプログラムに関する。

現実の空間に文字やＣＧ映像を重畳して提示する複合現実感（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ）に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラ等で撮影した画像にカメラの位置及び姿勢に応じて生成した画像を重畳描画し、これを表示する装置として実現できる。
このような画像表示装置を実現するには、現実空間中に定義した基準座標系と、カメラ座標系との間の、相対的な位置及び姿勢を計測することが不可欠である。例として、室内やテーブル上といった現実環境の所定位置に仮想物体を重畳表示する場合を考える。この場合は、その環境の適切な場所（例えば部屋の床面やテーブル面）に基準座標系を定義し、基準座標系におけるカメラの位置及び姿勢を求めればよい。

＜従来技術１＞
このような計測を実現するために、カメラが撮影した画像を利用して、カメラの位置及び姿勢を推定することが一般的に行われている（非特許文献１）。例えば、以下の手順によって、基準座標系におけるカメラの位置及び姿勢を計測することができる。
（１）室内の床や壁、テーブル面等に、基準座標系における位置（基準座標）が既知である複数の指標を配置あるいは設定する。ここで、指標とは、計測のために意図的に設置された人為的なマーカであってもよいし、元々その環境に存在している自然特徴等であってもよい。
（２）主観視点カメラが撮影した撮像画像内における指標の投影像の座標を検出する。
（３）検出された指標の画像座標と、当該指標の基準座標との対応関係に基づいて、カメラの位置及び姿勢を求める。
（４）固定された客観視点カメラを設置し、その客観視点カメラによって前記主観視点カメラを撮像し、主観視点カメラの位置を検出する。

＜従来技術２＞
特許文献１では、第１の撮像装置と、第１の撮像装置との相対的な位置及び姿勢が固定で、かつ第１の撮像装置と空間分解能の異なる第２の撮像装置とを用いて指標を検出し、第１の撮像装置の位置及び姿勢を算出することが開示されている。

特許第５０３６２６０号公報

佐藤，内山，山本：ＵＧ＋Ｂ法：主観及び客観視点カメラと姿勢センサを用いた位置合わせ手法，日本バーチャルリアリティ学会論文誌，vol.10, no.3, pp.391-400, 2005.

しかしながら、従来技術１の方法では、客観視点カメラを設置しなければならず、設置するための空間、設置にかかる手間、資材をそろえるコスト等が必要となる点が課題となる。
また、従来技術２の方法では、従来技術１の課題は解消されるが、指標の検出が第１の撮像装置と第２の撮像装置の設置方向に依存される。すなわち、第１の撮像装置又は第２の撮像装置が撮影している画像に指標が入っていなければ第１の撮像装置の位置及び姿勢を算出できないので、お互いの撮像装置の相対的な位置姿勢が固定されている状態では撮像装置の位置姿勢が制限される。

本発明は上記のような点に鑑みてなされたものであり、第１の撮像装置の位置及び姿勢を算出可能な状態を維持できるようにすることを目的とする。

本発明の位置姿勢計測装置は、第１の撮像装置と、前記第１の撮像装置との相対的な位置が固定である第２の撮像装置とを用いて、前記第１の撮像装置の位置及び姿勢を算出する位置姿勢計測装置であって、前記第１の撮像装置で撮像した第１の画像を入力する第１の画像入力手段と、前記第２の撮像装置で撮像した第２の画像を入力する第２の画像入力手段と、前記第１の画像から、現実空間中の座標が既知である指標の第１の画像座標を検出する第１の検出手段と、前記第２の画像から、現実空間中の座標が既知である指標の第２の画像座標を検出する第２の検出手段と、前記第１の画像座標と、前記第２の画像座標と、前記指標の現実空間中の座標と、前記第１の撮像装置と前記第２の撮像装置との相対的な位置及び姿勢の情報とに基づいて、前記第１の撮像装置の位置及び姿勢を算出する位置姿勢算出手段と、前記第１の画像及び前記第２の画像中の指標数が、前記第１の撮像装置の位置及び姿勢を算出するのに必要な数となるように、前記第２の撮像装置の姿勢及び焦点距離のうち少なくともいずれか一方を制御する制御手段とを備えたことを特徴とする。

本発明によれば、第１の撮像装置の位置及び姿勢を算出可能な状態を維持することができる。

実施形態に係る位置姿勢計測装置の構成例を示す図である。 実施形態に係る位置姿勢計測装置として機能しうるコンピュータの基本構成例を示す図である。 実施形態に係る位置姿勢計測装置の位置姿勢算出部の動作を説明するフローチャートである。 実施形態に係る位置姿勢計測装置の位置姿勢推測部、最適状態算出部及び追加視点カメラ制御部の動作を説明するフローチャートである。 追加視点画像中の指標の状態を示す図である。 追加視点画像中の指標の状態を示す図である。 追加視点画像中の指標の状態を示す図である。 追加視点画像中の指標の状態を示す図である。 追加視点画像中の指標の状態を示す図である。 追加視点画像中の指標の状態を示す図である。 追加視点画像中の指標の状態を示す図である。 ９個の姿勢による追加視点画像を示す図である。

以下、添付図面を参照して、本発明の好適な実施形態について説明する。
（第１の実施形態）
第１の実施形態に係る位置姿勢計測装置１００は、主観視点カメラ１６０及び追加視点カメラ１７０で撮像した画像から検出する指標に基づいて、主観視点カメラ１６０の位置及び姿勢を算出する。
図１は、本実施形態に係る位置姿勢計測装置１００の構成例を示す図である。位置姿勢計測装置１００は、画像入力部１１０、データ記憶部１２０、主観視点指標検出部１３０、追加視点指標検出部１４０、位置姿勢算出部１５０、位置姿勢推測部１８０、最適状態算出部１９０、追加視点カメラ制御部２００を備える。

位置姿勢計測装置１００には、主観視点カメラ１６０及び追加視点カメラ１７０が接続する。
本実施形態において、位置姿勢の計測対象の撮像装置である主観視点カメラ１６０は、例えば観察者が装着する頭部搭載型表示装置（ＨＭＤ）に固定的に設置され、ほぼ観察者の視点（主観視点）位置から観察される現実空間を撮像するために用いられる。
また、追加撮像装置である追加視点カメラ１７０は、例えば主観視点カメラ１６０が設置されたＨＭＤに設置され、主観視点カメラ１６０との相対的な位置は固定で既知であり、姿勢及び焦点距離の変更が制御可能な構造をしている。追加視点カメラ１７０の撮像範囲は、主観視点カメラ１６０の撮像範囲と重複してもよいし、独立していてもよい。
主観視点カメラ１６０及び追加視点カメラ１７０は、市販のカラービデオカメラにより実現できる。
なお、本実施形態では便宜上「主観視点」カメラを用いているが、計測対象の撮像装置が必ずしも観察者の視点位置から観察される現実空間を撮影するカメラである必要はない。

現実空間中の複数の位置には、主観視点カメラ１６０又は追加視点カメラ１７０で撮像するための指標が配置される。ここで、指標Ｑ_k（ｋ＝１、・・・、Ｋ）は、世界座標系における位置Ｘ_W ^Qkが既知である。世界座標系とは、現実空間の１点を原点として定義し、更に互いに直交する３軸をそれぞれＸ軸、Ｙ軸、Ｚ軸として定義した座標系である。
これらの指標は、主観視点カメラ１６０が計測対象範囲内で移動したときに、主観視点カメラ１６０又は追加視点カメラ１７０が撮像できるように設置されることが望ましい。以下、主観視点カメラ１６０が撮像する画像を主観視点画像、追加視点カメラ１７０が撮像する画像を追加視点画像と呼ぶ。
図１に示す例では、指標Ｑ１、Ｑ２、Ｑ３、Ｑ４が配置されている。そして、指標Ｑ１、Ｑ２が主観視点カメラ１６０の視野内に、指標Ｑ３が追加視点カメラ１７０の視野内に含まれている。
なお、指標Ｑ_kは、例えばそれぞれが異なる色を有する円形状のマーカであってもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点であってもよい。また、ある程度の面積を有する四角形の単色領域によって形成されるような、四角形指標を用いることも可能である。すなわち、撮像画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で同定可能であるような指標であれば、任意の形態の指標を用いることができる。指標は、操作者により設定されたものであってもよいし、操作者により設定されたものではない、自然形状のものを用いてもよい。

主観視点画像上において観測される指標の数が３個以上であれば、主観視点カメラ１６０の位置及び姿勢を算出することができる。指標の世界座標と、主観視点画像中の座標とから、主観視点カメラ１６０の位置及び姿勢を算出する方法は、写真測量等の分野において知られているため、その詳細については省略する。
主観視点画像からは検出不可能な位置にある指標であっても、別の方向を向いている追加視点カメラ１７０で撮像した追加視点画像からは検出できる場合がある。従って、追加主観視点カメラ１７０の向いている方向に適した追加視点指標を主観視点指標とは別に設定してもよい。

位置姿勢計測装置１００において、画像入力部１１０は、主観視点カメラ１６０及び追加視点カメラ１７０から主観視点画像及び追加視点画像を入力してデジタルデータに変換し、データ記憶部１２０に保存する。

主観視点指標検出部１３０は、データ記憶部１２０より主観視点画像を読み出し、主観視点画像中の指標の画像座標を検出する。例えば指標の各々が異なる色を有するマーカである場合には、画像上から各々のマーカ色に対応する領域を検出し、その重心位置を各指標の画像座標とする。指標の各々が異なるテクスチャ特徴を有する特徴点である場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを行うことにより、指標の画像座標を検出する。また、四角形指標を用いる場合には、画像に２値化処理を施した後にラベリングを行い、４つの直線によって形成されている領域を指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出をなくし、また、指標の識別子を取得する。なお、このようにして検出される四角形指標は、本実施形態では、４つの頂点の個々によって特定される４つの指標であると考える。
主観視点指標検出部１３０は、検出された指標の画像座標とその指標の識別子をデータ記憶部１２０に出力する。以下では、主観視点画像上で検出された指標を、検出された指標のそれぞれに付けられた識別子ｎ（ｎ＝１、・・・、Ｎ）を用いて、Ｑ_knと表記する。ここで、Ｎは主観視点画像上で検出された指標の数を表している。また、検出された指標Ｑ_knの画像座標をｕ^Qknと表記する。例えば図１の場合、Ｎ＝２であり、指標の識別子ｋ₁＝１、ｋ₂＝２と、これらに対応する画像座標ｕ^Qk1、ｕ^Qk2が出力される。

追加視点指標検出部１４０は、主観視点指標検出部１３０と同様に、データ記憶部１２０より追加視点画像を読み出し、追加視点画像中の指標の画像座標を検出する。
追加視点指標検出部１４０は、検出された指標の画像座標とその指標の識別子をデータ記憶部１２０に出力する。以下では、追加視点画像上で検出された指標を、検出された指標のそれぞれに付けられた識別子ｍ（ｍ＝１、・・・、Ｍ）を用いて、Ｑ_kmと表記する。ここで、Ｍは追加視点画像上で検出された指標の数を表している。また、検出された指標Ｑ_kmの画像座標をｕ^Qkmと表記する。例えば図１の場合には、Ｍ＝１であり、指標の識別子ｋ１＝１と、これらに対応する画像座標ｕ^Qk1が出力される。

位置姿勢算出部１５０は、
・主観視点指標検出部１３０によって検出された各々の指標の画像座標ｕ^Qkn
・追加視点指標検出部１４０によって検出された各々の指標の画像座標ｕ^Qkm
・既知な情報として予め保持されている指標の世界座標ｘ_W ^Qkの組を、データ記憶部１２０から取得する。そして、これらの情報に基づいて、主観視点カメラ１６０の位置及び姿勢を算出（推定）する。算出した主観視点カメラ１６０の位置及び姿勢は、例えば位置を表す３次元ベクトルｘ_W ^C1と姿勢を表す３×３行列Ｒ_WC1との組の形態によってデータ記憶部１２０に出力される。

データ記憶部１２０は、既知の値である指標の世界座標、主観視点カメラ１６０及び追加視点カメラ１７０のカメラパラメータ等のデータを予め保持する。また、データ記憶部１２０は、画像入力部１１０から入力される主観視点画像及び追加視点画像、主観視点指標検出部１３０及び追加視点指標検出部１４０から入力される各々の指標の画像座標及び識別子を保持する。さらに、データ記憶部１２０は、位置姿勢算出部１５０から入力される主観視点カメラ１６０の位置及び姿勢の値を保持する。さらにまた、データ記憶部１２０は、追加視点カメラ１７０の変動する焦点距離の値、姿勢の値を保持する。

位置姿勢推測部１８０、最適状態算出部１９０及び追加視点カメラ制御部２００は、詳細は後述するが、主観視点カメラ１６０の位置及び姿勢を算出可能な状態を維持できるように、すなわち主観視点画像及び追加視点画像中の指標数が、主観視点カメラ１６０の位置及び姿勢を算出するのに必要な数となるように、追加視点カメラ１７０の姿勢及び焦点距離のうち少なくともいずれか一方を制御する。

なお、図１に示した各部１１０〜１５０、１８０〜２００のそれぞれは、それぞれ独立したハードウェアで構成することができる。また、図示しないＣＰＵがプログラムを実行することによりソフトウェア的に各部の機能を実現するようにしてもよい。また、各部をソフトウェア的に実現するＣＰＵは、共通のＣＰＵであっても、複数の異なるＣＰＵであってもよい。また、ネットワーク接続された複数のコンピュータにより分散処理して各部の機能を実現してもよい。本実施形態では、各部１１０〜１５０、１８０〜２００は一のＣＰＵがプログラムを実行することにより、各部の機能を実現する構成を有する。

図２は、実施形態に係る位置姿勢計測装置１００として機能しうるコンピュータの基本構成例を示す図である。
上述の通り、本実施形態では、各部１１０〜１５０、１８０〜２００のそれぞれを、プログラムの実行によりソフトウェア的に実現する。
ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行う。また、各部１１０〜１５０、１８０〜２００のそれぞれの機能を実現するプログラムを実行し、各部として機能する。
ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備える。また、ＲＡＭ１００２は、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。本実施形態において、データ記憶部１２０の機能は、ＲＡＭ１００２によって実現される。
ＲＯＭ１００３は、一般に、ＣＰＵ１００１がコンピュータの起動時に実行するプログラムや、各種設定データ等が格納されている。

１００４はキーボード、１００５はマウスであり、入力装置を構成する。操作者はこれら入力装置を用いて、各種の指示をコンピュータに入力することができる。入力装置による入力は、ＣＰＵ１００１によって検知される。
表示部１００６は、ＣＲＴモニタやＬＣＤ等であり、例えば主観視点カメラ１６０の位置姿勢計測のために表示すべきメッセージ等を表示することができる。
外部記憶装置１００７はハードディスク等の大容量情報記憶装置であり、ＯＳ（オペレーティングシステム）やアプリケーションプログラム等を保存する。また、本実施形態において、既知の情報として説明される情報は外部記憶装置１００７に保存されており、必要に応じてＲＡＭ１００２にロードされる。
記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭ等の記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出し、ＲＡＭ１００２や外部記憶装置１００７に出力する。
Ｉ／Ｆ１００９は、外部機器を接続するための各種インタフェースである。本実施形態において、／Ｆ１００９は、主観視点カメラ１６０や追加視点カメラ１７０を接続するためのインタフェースや、計測結果等を出力するためのネットワークインタフェースを含む。具体的には、アナログビデオインタフェース、デジタル入出力インタフェース（ＵＳＢ、ＩＥＥＥ１３９４等）や、イーサネット（登録商標）インタフェース等によって構成される。Ｉ／Ｆ１００９を介して入力されたデータはＲＡＭ１００２に取り込まれる。画像入力部１１０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
バス１０１０は、上述の各部を相互に通信可能に接続する。

（位置姿勢算出処理）
図３は、実施形態に係る位置姿勢計測装置１００の位置姿勢算出部１５０の動作を説明するフローチャートである。上述のように、本実施形態では、ＣＰＵ１００１がプログラムを実行することで位置姿勢算出部１５０として機能する。なお、以下に説明する処理をＣＰＵ１００１が実現するために必要なプログラムコードは、ＲＡＭ１００２にロードされている。
本実施形態において、主観視点カメラ１６０の位置及び姿勢は、繰り返し演算による数値計算的手法によって算出される。
位置姿勢算出部１５０は、算出すべき主観視点カメラ１６０の位置及び姿勢を、それぞれ３値ベクトル［ｘ ｙ ｚ］^T及び［ξ ψ ζ］^Tによって内部的に表現している。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。このとき、求めるべき未知パラメータは、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］^Tと記述される。

ステップＳ３００で、位置姿勢算出部１５０は、状態ベクトルｓに適当な初期値（位置及び姿勢の推定値）を設定する。例えば主観視点画像及び追加視点画像の各画像フレームに対して位置及び姿勢を算出する場合、前フレーム（時刻ｔ_k-1）の処理において導出された状態ベクトルｓを初期値とすることができる。また、直近の所定期間内における状態ベクトルｓの変化に基づいた予測値を初期値としてもよい。

ステップＳ３０１で、位置姿勢算出部１５０は、主観視点指標検出部１３０で検出された各々の指標Ｑ^knの画像座標の実測値ｕ^Qknとその識別子ｋ_nをデータ記憶部１２０から取得する。また、位置姿勢算出部１５０は、追加視点指標検出部１４０で検出された各々の指標Ｑ^kmの画像座標の実測値ｕ^Qkmとその識別子ｋ_mについてもデータ記憶部１２０から取得する。

ステップＳ３０２で、位置姿勢算出部１５０は、主観視点画像から検出された指標Ｑ^kn及び、追加視点画像から検出された指標Ｑ^kmに対し、画像座標の推定値ｕ^Qkn*及びｕ^Qkm*を算出する。
ｕ^Qkn*の算出は、下式（１）に示すように、データ記憶部１２０に既知な情報として予め保持している指標Ｑ^kn各々の世界座標ｘ_W ^Qknと現在の状態ベクトルｓの関数に基づいて行われる。

また、ｕ^Qkm*の算出は、下式（２）に示すように、データ記憶部１２０に既知な情報として予め保持している指標Ｑ^km各々の世界座標ｘ_W ^Qkmと現在の状態ベクトルｓの関数に基づいて行われる。

具体的には、関数Ｆｃ₁（）は、下式（３）及び下式（４）によって構成される。下式（３）は、ｘ_W ^Qknとｓから当該指標の主観視点カメラ座標（主観視点カメラ１６０上の１点を原点として定義し、更に互いに直交する３軸をそれぞれＸ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘ_C1 ^Qknを求める。また、下式（４）は、主観視点カメラ座標ｘ_c1 ^Qknから画像座標の推定値ｕ^Qkn*を求める。

ここで、ｆ^C1 _x及びｆ^C1 _yは、それぞれｘ軸方向及びｙ軸方向における主観視点カメラ１６０の焦点距離であり、データ記憶部１２０に既知の値として予め保持されている。また、Ｍ_WC1（ｓ）は状態ベクトルｓによって定まるモデリング変換行列（すなわち、主観視点カメラ座標系における座標を世界座標系における座標に変換する行列）であり、下式（５）によって定義される。

一方、関数Ｆｃ₂（）は、下式（６）及び下式（７）によって構成される。下式（６）は、ｘ_W ^Qkmとｓから当該指標の追加視点カメラ座標（追加視点カメラ１７０上の１点を原点として定義し、更に互いに直交する３軸をそれぞれＸ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘ_C2 ^Qkmを求める。下式（７）は、追加視点カメラ座標ｘ_C2 ^Qkmから画像座標の推定値ｕ^Qkm*を求める。

ここで、Ｍ_C1C2は、追加視点カメラ座標系から主観視点カメラ座標系への変換行列である。Ｍ_C1C2は、主観視点カメラ座標系における追加視点カメラ１７０の位置及び現在の姿勢（データ記憶部１２０に保存されている）に基づいて、予め算出されている。また、ｆ^C2 _x及びｆ^C2 _yは、それぞれｘ軸方向及びｙ軸方向における追加視点カメラ１７０の現在の焦点距離であり、データ記憶部１２０に保存されている。

ステップＳ３０３で、位置姿勢算出部１５０は、各々の指標Ｑ^knに対して、画像座標の推定値ｕ^Qkn*と実測値ｕ^Qknとの誤差Δｕ^Qknを、下式（８）によって算出する。

また、各々の指標Ｑ^kmに対して、画像座標の推定値ｕ^Qkm*と実測値ｕ^Qkmとの誤差Δｕ^Qkmを、下式（９）によって算出する。

ステップＳ３０４で、位置姿勢算出部１５０は、各々の指標Ｑ^knに対して、式（１）の関数Ｆｃ₁（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列Ｊ_us ^Qkn（＝∂ｕ^Qkn／∂ｓ）を求める。また、位置姿勢算出部１５０は、各々の指標Ｑ^kmに対して、式（２）の関数Ｆｃ₂（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列Ｊ_us ^Qkm（＝∂ｕ^Qkm／∂ｓ）を算出する。
具体的には、式（４）の右辺を主観視点カメラ座標ｘ_C1 ^Qknの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ux ^Qkn（＝∂ｕ^Qkn／∂ｘ^Qkn）と、式（３）の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列Ｊ_xs ^Qkn（＝∂ｘ^Qkn／∂ｓ）を算出し、下式（１０）によってヤコビ行列Ｊ_us ^Qknを算出する。

また、式（７）の右辺を追加視点カメラ座標ｘ_C2 ^Qkmの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ux ^Qkm（＝∂ｕ^Qkm／∂ｘ^Qkm）と、式（６）の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列Ｊ_xs ^Qkm（＝∂ｘ^Qkm／∂ｓ）を算出し、下式（１１）によってヤコビ行列Ｊ_us ^Qkmを算出する。

ステップＳ３０５で、位置姿勢算出部１５０は、ステップＳ３０３及びステップＳ３０４において算出した誤差Δｕ^Qkn及びΔｕ^Qkm、及び、ヤコビ行列Ｊ_us ^Qkn及びＪ_us ^Qkmに基づいて、状態ベクトルｓの補正値Δｓを算出する。具体的には、下式（１２）に示すように、誤差Δｕ^Qkn及びΔｕＱｋｍを垂直に並べた２（Ｎ＋Ｍ）次元の誤差ベクトル及び、下式（１３）に示すように、ヤコビ行列Ｊ_us ^Qkn及びＪ_us ^Qkmを垂直に並べた２（Ｎ＋Ｍ）行×６列の行列を作成し、Θの擬似逆行列Θ⁺を用いて、下式（１４）として算出する。図１の例ではＮ＝３、Ｍ＝２であるので、Ｕは１０次元ベクトル、Θは１０行×６列の行列となる。

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

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

ステップＳ３０７において計算が収束したと判定される場合、ステップＳ３０８で、位置姿勢算出部１５０は、得られた状態ベクトルｓを主観視点カメラ１６０の位置及び姿勢の情報としてデータ記憶部１２０に出力する。位置及び姿勢の情報は、例えば位置を表す３次元ベクトルｘ_W ^C1と姿勢を表す３×３行列Ｒ_WC1（ｓ）との組の形態によって出力する。

以上のように、主観視点画像及び追加視点画像の中に合計３個以上の指標が存在すれば、それを用いて主観視点カメラ１６０の位置及び姿勢を算出することができる。

（３個以上の指標を捉える処理）
次に、主観視点画像及び追加視点画像中に３個以上の指標を収める方法を説明する。
既に説明したように、追加視点カメラ１７０は姿勢及び焦点距離の変更が制御可能であり、その変動値をデータ記憶部１２０に保存している。ここで、追加視点カメラ１７０の姿勢ベクトルと焦点距離情報を定義する。姿勢ベクトルをｏ＝［ξ ψ ζ］^T、焦点距離情報をｆとする。なお、姿勢ベクトルの表現方法は前記の主観視点カメラと同様である。
データ記憶部１２０には、現在及び過去の時刻ｔにおける主観視点カメラ１６０の状態ベクトルｓ_tと、追加視点カメラ１７０の姿勢情報ｏ_t及び焦点距離情報ｚ_tが保存されている。そこで、直近の所定期間の主観視点カメラ１６０の状態ベクトルｓの推移を利用し、未来の主観視点カメラ１６０の位置及び姿勢を推測することができる。また、未来の主観視点カメラ１６０の位置及び姿勢の推測値から相対的に未来の追加視点カメラ１７０の位置及び姿勢を算出することができる。

図４は、実施形態に係る位置姿勢計測装置１００の位置姿勢推測部１８０、最適状態算出部１９０及び追加視点カメラ制御部２００の動作を説明するフローチャートであり、次のフレームの主観視点カメラ１６０の位置及び姿勢を推測し、次のフレームにおいて指標が３点以上検出できるように追加視点カメラ１７０を制御する処理を示す。この処理は、例えば図３のステップＳ３０８の終了後に行われる。
ステップＳ４００で、位置姿勢推測部１８０は、データ記憶部１２０から直近の所定期間の主観視点カメラ１６０の位置及び姿勢の状態ベクトルｓ_tを入力する。

ステップＳ４０１で、位置姿勢推測部１８０は、状態ベクトルｓ_tの推移から次のフレームである時間ｔ₁での主観視点カメラ１６０の位置及び姿勢を推測する。その位置及び姿勢の推測値（以下、位置姿勢推測値と呼ぶ）をｓ_t1とする。過去の位置及び姿勢の推移から未来の位置及び姿勢を推測する方法は様々な方法が知られており、本件ではどのような方法を用いても構わない。例えば時刻ｔ_aの位置及び姿勢Ｖ_ta、次フレームの時刻ｔ_bの位置及び姿勢Ｖ_tbでΔＶ＝Ｖ_tb−Ｖ_taのとき、次々フレーム時刻ｔ_cの位置及び姿勢Ｖ_tc＝Ｖ_tb＋ΔＶと推測する。また、未来の位置及び姿勢の推測は次のフレームまででもよいし、所定の未来までの複数のフレームの推測でも構わない。以降の判断には、本実施形態では次のフレームにしているが、所定の未来のフレームを判断基準にしても構わない。
また、位置姿勢推測部１８０は、主観視点カメラ１６０の位置姿勢推測値ｓ_t1、データ記憶部１２０に記憶されている固定の主観視点カメラ１６０との位置情報、及び現在の姿勢情報を用いて、追加視点カメラ１７０の位置姿勢推測値ｓａ_t1を算出する。

ステップＳ４０２で、位置姿勢推測部１８０は、位置姿勢推測値ｓ_t1における主観視点画像中の指標の数と、位置姿勢推測値ｓａ_t1における追加視点画像中の指標の数を算出する。指標のデータをデータ記憶部１２０から位置姿勢推測部１８０に入力する。
まず主観視点カメラ１６０の位置姿勢推測値ｓ_t1とカメラパラメータからカメラの撮像範囲を算出する。カメラの撮像範囲と指標の位置を比較し、位置姿勢推測値ｓ_t1における主観視点画像中の指標の数を算出する。同様に、位置姿勢推測値ｓａ_t1における追加視点画像中の指標の数を算出する。そして、主観視点画像中の指標の数と追加視点画像中の指標の数を合計し、総可視指標数を算出する。

ステップＳ４０３で、位置姿勢推測部１８０は、総可視指標数が３点以上であるか否かを判定し、３点以上であれば処理を終了し、３点未満であればステップＳ４０４に処理を移す。

ステップＳ４０４で、最適状態算出部１９０は、総可視指標数が３点以上になるような追加視点カメラ１７０の姿勢、焦点距離の算出を行う。
以下、ケース１からケース３に分けて続きの処理を説明する。
・ケース１：焦点距離だけを変更する場合
最新フレームの一つ前のフレームＦ１における追加視点画像中において指標Ｑ５、Ｑ６、Ｑ７が図５のように存在している。そして、最新フレームではフレームＦ１より指標Ｑ５、Ｑ６、Ｑ７に近づき、追加視点画像中の指標Ｑ５、Ｑ６、Ｑ７が図６のように存在する。そして、位置姿勢推測値ｓａ_t1におけるフレームでは最新フレームより指標Ｑ５、Ｑ６、Ｑ７に近づき、追加視点画像は図７のように指標Ｑ５、Ｑ７だけが存在し、指標Ｑ６が存在しない。
そこで、最適状態算出部１９０は、指標Ｑ６が追加視点画像中に存在するような追加視点カメラ１７０の姿勢及び焦点距離の算出を行う。この場合は、追加視点カメラ１７０が指標に近づきすぎたことにより指標が追加視点画像中から存在しなくなってしまった。そこで、位置姿勢推測値ｓａ_t1において指標Ｑ５、Ｑ６、Ｑ７が追加視点画像中に存在するような焦点距離の値を算出し、追加視点カメラ制御部２００に出力する。

ステップＳ４０５で、追加視点カメラ制御部２００は、追加視点カメラ１７０の焦点距離を最適状態算出部１９０で算出した値に変更する。また、新規焦点距離をデータ記憶部１２０に出力し、データ記憶部１２０は新規焦点距離を記録する。

・ケース２：姿勢だけを変更する場合
最新フレームの一つ前のフレームＦ１における追加視点画像中において指標Ｑ５、Ｑ６、Ｑ７が図５のように存在している。そして、最新フレームでは、追加主観視点カメラ１７０の位置及び姿勢が変更され、追加視点画像中の指標Ｑ５、Ｑ６、Ｑ７が図８のように存在する。そして、位置姿勢推測値ｓａ_t1におけるフレームでは、追加視点画像は図９のように指標Ｑ５、Ｑ６だけが存在し、指標Ｑ７が存在しない。
そこで、最適状態算出部１９０は、指標Ｑ７が追加視点画像中に存在するような追加視点カメラ１７０の姿勢及び焦点距離の算出を行う。この場合は、追加視点カメラ１７０の位置及び姿勢が変化したことにより指標が追加視点画像中から存在しなくなってしまった。そこで、位置及び焦点距離を変更せずに指標Ｑ５、Ｑ６、Ｑ７が追加視点画像中に存在するような姿勢の値を算出し、追加視点カメラ制御部２００に出力する。

ステップＳ４０５で、追加視点カメラ制御部２００は、追加視点カメラ１７０の姿勢を最適状態算出部１９０で算出した値に変更する。また、新姿勢ベクトルをデータ記憶部１２０に出力し、データ記憶部１２０は新姿勢ベクトルを記録する。

・ケース３：焦点距離と姿勢を変更する場合
最新フレームの一つ前のフレームＦ１における追加視点画像中において指標Ｑ５、Ｑ６、Ｑ７が図５のように存在している。そして、最新フレームでは、追加主観視点カメラ１７０の位置及び姿勢が変更され、追加視点画像中の指標Ｑ５、Ｑ６、Ｑ７が図１０のように存在する。そして、位置姿勢推測値ｓａ_t1におけるフレームでは、追加視点画像は図１１のように指標Ｑ５、Ｑ７だけが存在し、指標Ｑ６が存在しない。
そこで、最適状態算出部１９０は、指標Ｑ７が追加視点画像中に存在するような追加視点カメラ１７０の姿勢及び焦点距離の算出を行う。この場合は、追加視点カメラ１７０の位置及び姿勢が変化したことにより指標が追加視点画像中から存在しなくなってしまった。そこで、位置を変更させずに指標Ｑ５、Ｑ６、Ｑ７が追加視点画像中に存在するような姿勢及び焦点距離の値を算出し、追加視点カメラ制御部２００に出力する。

ステップＳ４０５で、追加視点カメラ制御部２００は、追加視点カメラ１７０の姿勢及び焦点距離を最適状態算出部１９０で算出した値に変更する。また、新焦点距離、新姿勢ベクトルをデータ記憶部１２０に出力し、データ記憶部１２０は新焦点距離、新姿勢ベクトルを記録する。

以上のように、位置姿勢の計測対象である第１の撮像装置（主観視点カメラ１６０）に加え、姿勢及び焦点距離を制御可能な第２のカメラ（追加視点カメラ１７０）を併用し、両方のカメラで撮像した画像から検出した指標の情報を用いて第１のカメラの位置及び姿勢を算出する。第２のカメラの焦点距離の変更、姿勢の変更、又は姿勢及び焦点距離の変更によって３点以上の指標を捉え、第１のカメラの位置及び姿勢を算出できる状態を維持することができる。
例えば観察者がＨＭＤを身に付け、現実空間映像にＣＧを重畳して体験する複合現実間システムのように、第１のカメラが観察者に表示するための主観視点映像を撮像する必要があり、第１のカメラの画角等を変更することができない場合がある。この場合でも、姿勢及び焦点距離が制御可能な第２のカメラを追加し、第２のカメラの姿勢、焦点距離を制御しつつ両方のカメラで撮像した画像を用いて第１のカメラの位置及び姿勢を算出することで、第１のカメラの位置及び姿勢の測定可能範囲を拡大することができる。

（第２の実施形態）
第２の実施形態では、位置姿勢計測装置１００の構成及び位置姿勢算出処理は第１の実施形態と同様であり、第１の実施形態とは、ステップＳ４０４、ステップＳ４０５の処理を変更している。以下では、第１の実施形態との相違点を中心に説明する。
ステップＳ４０４で、最適状態算出部１９０は、総可視指標数が３点以上になるような追加視点カメラ１７０の姿勢、焦点距離を全体の指標情報を分析して算出する。追加視点カメラ１７０の位置姿勢推測値ｓａ_t1の位置における、姿勢の変動可能な範囲と、焦点距離の変動可能な範囲の中で最適な追加視点画像を撮像できる姿勢及び焦点距離を算出する。最適な追加視点画像とは、以下の条件を満たすものとなる。
（１）指標が３点以上あること
（２）指標に近づいても近未来の所定フレームにおいては指標が追加視点画像中に存在し認識できること
（３）指標から遠ざかっても近未来の所定フレームにおいては指標が追加視点画像中に存在し認識できること
（４）姿勢の変更があっても近未来の所定フレームにおいては指標が追加視点画像中に存在し認識できること

以上の条件を満たすものとして、以下のような具体的な追加視点画像になる。
Ａ．３点以上のより多くの指標が存在する追加視点画像であること
Ｂ．追加視点画像の近接領域に別の指標が存在すること
Ｃ．追加視点画像中の指標が認識できる下限のサイズより一定割合大きいこと
Ｄ．追加視点画像中の指標の位置が画像領域の中心に寄っていること
最適状態算出部１９０は、Ａ，Ｂ，Ｃ，Ｄの条件を満たす姿勢及び焦点距離を算出する。なお、Ａ，Ｂ，Ｃ，Ｄ全ての条件を満たすのが最も望ましいが、そのような条件が見つからなければ一部の条件を満たす姿勢及び焦点距離を算出する。算出した後は追加視点カメラ制御部２００に出力する。

以上の条件を満たす追加視点カメラ１７０の姿勢及び焦点距離を算出するために、例えば以下の方法がある。
探索する焦点距離を可動範囲内でｎ個設定する。ｎ個の各焦点距離において姿勢を変更することで撮影可能な全範囲を撮影するような最小の個数Ｋ_nの姿勢を算出する。そのＫ_n個の各姿勢における撮像画像の中に含まれる追加視点画像をＰ_Knとする。
Ｐ_Knにおける指標の数と各指標の認識状態をポイント化しその値をＱ_PKnとする。さらに周囲の指標のポイントに係数を掛けて加えた値をＰ_Knの最終ポイントＺ_PKnとする。
図１２は、ある焦点距離において９個の姿勢における追加視点画像を並べたものである。追加視点画像Ｐ０における最終ポイントＺ_P0はＰ１、Ｐ２、Ｐ３、Ｐ４、Ｐ５、Ｐ６、Ｐ７、Ｐ８の影響をうける。その結果Ｚ_P0は下式（１６）のようになる。

このように各焦点距離と姿勢における最終ポイントを計算し、そのポイントが最も大きいものを選択する。

ステップＳ４０５で、追加視点カメラ制御部２００は、追加視点カメラ１７０の姿勢及び焦点距離を最適状態算出部１９０で算出した値に変更する。また、新焦点距離、新姿勢ベクトルをデータ記憶部１２０に出力し、データ記憶部１２０は新焦点距離、新姿勢ベクトルを記録する。

（第３の実施形態）
第３の実施形態では、位置姿勢計測装置１００の構成及び位置姿勢算出処理は第１の実施形態と同様であり、第１の実施形態とは、ステップＳ４０４、ステップＳ４０５の処理を変更している。以下では、第１の実施形態との相違点を中心に説明する。
主観視点カメラ１６０の位置姿勢推測値ｓ_t1における指標数が１、追加視点カメラ１７０の位置姿勢推測値ｓａ_t1における指標数が１になる場合、合計で指標数が３以上になればいいので、追加視点カメラ１７０が撮像する追加視点画像中の指標数を２以上にすればよい。

最適状態算出部１９０は、総可視指標数が３点以上になるような追加視点カメラ１７０の姿勢、焦点距離の算出を行う。
ステップＳ４０４で、最適状態算出部１９０は、追加視点画像中の指標数が２以上になる追加視点カメラ１７０の姿勢、焦点距離の算出を行う。

ステップＳ４０５で、追加視点カメラ制御部２００は、追加視点カメラ１７０の姿勢及び焦点距離を最適状態算出部１９０で算出した値に変更する。また、新焦点距離、新姿勢ベクトルをデータ記憶部１２０に出力し、データ記憶部１２０は新焦点距離、新姿勢ベクトルを記録する。

（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１００ 位置姿勢計測装置
１１０ 画像入力部
１２０ データ記憶部
１３０ 主観視点指標検出部
１４０ 追加視点指標検出部
１５０ 位置姿勢算出部
１６０ 主観視点カメラ
１７０ 追加視点カメラ
１８０ 位置姿勢推測部
１９０ 最適状態算出部
２００ 追加視点カメラ制御部

Claims (8)

1. 第１の撮像装置と、前記第１の撮像装置との相対的な位置が固定である第２の撮像装置とを用いて、前記第１の撮像装置の位置及び姿勢を算出する位置姿勢計測装置であって、
   前記第１の撮像装置で撮像した第１の画像を入力する第１の画像入力手段と、
   前記第２の撮像装置で撮像した第２の画像を入力する第２の画像入力手段と、
   前記第１の画像から、現実空間中の座標が既知である指標の第１の画像座標を検出する第１の検出手段と、
   前記第２の画像から、現実空間中の座標が既知である指標の第２の画像座標を検出する第２の検出手段と、
   前記第１の画像座標と、前記第２の画像座標と、前記指標の現実空間中の座標と、前記第１の撮像装置と前記第２の撮像装置との相対的な位置及び姿勢の情報とに基づいて、前記第１の撮像装置の位置及び姿勢を算出する位置姿勢算出手段と、
   前記第１の画像及び前記第２の画像中の指標数が、前記第１の撮像装置の位置及び姿勢を算出するのに必要な数となるように、前記第２の撮像装置の姿勢及び焦点距離のうち少なくともいずれか一方を制御する制御手段とを備えたことを特徴とする位置姿勢計測装置。
2. 前記制御手段は、前記第１の画像及び前記第２の画像中の指標数の合計が所定の数となるように、前記第２の撮像装置の姿勢及び焦点距離のうち少なくともいずれか一方を制御することを特徴とする請求項１に記載の位置姿勢計測装置。
3. 前記所定の数は３であることを特徴とする請求項２に記載の位置姿勢計測装置。
4. 前記第１の撮像装置の位置及び姿勢の過去の推移から未来の推移を推測し、前記第１の画像及び前記第２の画像中の指標数が、前記第１の撮像装置の位置及び姿勢を算出するのに必要な数に対して不足するか否かを予測する予測手段を備え、
   前記制御手段は、前記予測手段で不足すると予測された場合に、前記第２の撮像装置の姿勢及び焦点距離のうち少なくともいずれか一方を制御することを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測装置。
5. 前記第１の撮像装置は頭部に装着する頭部搭載型表示装置に設けられていることを特徴とする請求項１乃至４のいずれか１項に記載の位置姿勢計測装置。
6. 前記第２の撮像装置は前記頭部搭載型表示装置に設けられていることを特徴とする請求項５に記載の位置姿勢計測装置。
7. 第１の撮像装置と、前記第１の撮像装置との相対的な位置が固定である第２の撮像装置とを用いて、前記第１の撮像装置の位置及び姿勢を算出する位置姿勢計測方法であって、
   前記第１の撮像装置で撮像した第１の画像から、現実空間中の座標が既知である指標の第１の画像座標を検出するステップと、
   前記第２の撮像装置で撮像した第２の画像から、現実空間中の座標が既知である指標の第２の画像座標を検出するステップと、
   前記第１の画像座標と、前記第２の画像座標と、前記指標の現実空間中の座標と、前記第１の撮像装置と前記第２の撮像装置との相対的な位置及び姿勢の情報とに基づいて、前記第１の撮像装置の位置及び姿勢を算出するステップと、
   前記第１の画像及び前記第２の画像中の指標数が、前記第１の撮像装置の位置及び姿勢を算出するのに必要な数となるように、前記第２の撮像装置の姿勢及び焦点距離のうち少なくともいずれか一方を制御するステップとを有することを特徴とする位置姿勢計測方法。
8. 第１の撮像装置と、前記第１の撮像装置との相対的な位置が固定である第２の撮像装置とを用いて、前記第１の撮像装置の位置及び姿勢を算出するためのプログラムであって、
   前記第１の撮像装置で撮像した第１の画像を入力する第１の画像入力手段と、
   前記第２の撮像装置で撮像した第２の画像を入力する第２の画像入力手段と、
   前記第１の画像から、現実空間中の座標が既知である指標の第１の画像座標を検出する第１の検出手段と、
   前記第２の画像から、現実空間中の座標が既知である指標の第２の画像座標を検出する第２の検出手段と、
   前記第１の画像座標と、前記第２の画像座標と、前記指標の現実空間中の座標と、前記第１の撮像装置と前記第２の撮像装置との相対的な位置及び姿勢の情報とに基づいて、前記第１の撮像装置の位置及び姿勢を算出する位置姿勢算出手段と、
   前記第１の画像及び前記第２の画像中の指標数が、前記第１の撮像装置の位置及び姿勢を算出するのに必要な数となるように、前記第２の撮像装置の姿勢及び焦点距離のうち少なくともいずれか一方を制御する制御手段としてコンピュータを機能させるためのプログラム。

Images