JP4566786B2 - 位置姿勢計測方法および情報処理装置 - Google Patents

Description

本発明は、物体の位置及び姿勢、特に撮像装置の位置及び姿勢を計測するものに関する。

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

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

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

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

前者の問題を解決する方法として、情景中に複数の指標を配置あるいは設定し、撮像装置によって撮像された画像内における指標の投影像の座標を検出することで、情景中における撮像装置の位置及び姿勢を求めることが一般的に行われている。また、撮像装置に慣性センサを装着して、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用したり、同推定結果を画像に基づく位置及び姿勢算出の初期値として用いたり、さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いることで、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている（例えば、非特許文献１）。

また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置（及び慣性センサ）を装着し、前者と同様な方法によって撮像装置の位置及び姿勢を求め、撮像装置と計測対象物体との既知の相対的な位置と姿勢の関係から、計測対象物の位置及び姿勢を求めることが一般的に行われている。
藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９，ｎｏ．５７４，ｐｐ．１−８）

しかし、上記の従来手法では、安定した位置合わせを実現するのに十分な画像情報が主観視点画像中に含まれていない状況下、例えば、画像上の一部の領域に指標が偏在して観測されている場合や、指標が３点のみしか観測されていない場合、指標の検出に誤差が含まれている場合等に、得られる解の精度や安定性が不十分となることがあった。また、観測されている指標が２点以下の場合には、解を求めることができなかった。これらの問題を回避するためには情景中に多数の指標を満遍なく設定する必要があり、指標同士の識別が困難になるという問題や、現実空間の美観を損ねてしまうという問題が生じていた。また、主観視点画像上における指標の像が観察者の手等によって遮蔽されてしまうような状況下において、位置合わせを行うことが全くできなくなるという問題があった。

上記の課題は、以下の装置によって解決される。

現実空間に配置された主観視点点指標を撮像した主観視点画像を取得する主観視点撮像手段と、前記主観視点撮像手段に伴って配置された客観視点点指標を、前記現実空間の世界座標系において固定されて既知である客観視点位置姿勢から撮像した客観視点画像を取得する客観視点撮像手段と、前記主観視点撮像手段に備えられ、前記主観視点撮像手段の姿勢計測値を計測する姿勢計測手段と、前記姿勢計測手段の方位ドリフト誤差補正値、前記主観視点点指標の前記世界座標系における位置、前記客観視点点指標の前記主観視点撮像手段に対する位置、前記主観視点撮像手段の焦点距離および前記客観視点撮像手段の焦点距離を予め保持する保持手段と、前記姿勢計測値と前記方位ドリフト誤差補正値に基づいて、前記主観視点撮像手段の姿勢予測値を算出する姿勢予測手段と、前記主観視点画像から少なくとも１点の主観視点点指標の画像座標を検出する第１の検出手段と、前記客観視点画像から少なくとも１点の客観視点点指標の画像座標を検出する第２の検出手段と、前記客観視点位置姿勢、前記姿勢予測値、前記検出された主観視点点指標の画像座標、当該主観視点点指標の世界座標系における位置、前記検出された客観視点点指標の画像座標、および当該客観視点点指標の前記主観視点撮像手段に対する位置に基づいて、前記主観視点撮像手段の位置姿勢を算出する位置姿勢算出手段と、前記算出された前記主観視点撮像装置の位置姿勢に基づいて、前記保持された方位ドリフト誤差補正値を更新する方位誤差更新手段とを備え、前記位置姿勢算出手段が、前記主観視点撮像手段の位置姿勢の設定値を設定する設定値設定手段と、前記設定値、前記主観視点点指標の世界座標系における位置、および前記主観視点撮像手段の焦点距離に基づいて、前記主観視点画像の前記主観視点点指標の画像座標の主観座標推定値を算出する主観推定値算出手段と、前記設定値、前記客観視点点指標の前記主観視点撮像手段に対する位置、前記客観視点位置姿勢、および前記客観視点撮像手段の焦点距離に基づいて、前記客観視点画像の前記客観視点点指標の画像座標の客観座標推定値を算出する客観推定値算出手段と、前記検出された主観視点点指標の画像座標と前記主観座標推定値との主観誤差を算出する主観画像誤差算出手段と、前記検出された客観視点点指標の画像座標と前記客観座標推定値との客観誤差を算出する客観画像誤差算出手段と、前記算出された主観誤差および客観誤差に基づいて、前記主観視点撮像手段の位置および姿勢の方位角成分のみの補正値を算出する補正値算出手段と、前記算出された補正値および前記設定値に基づいて、前記主観視点撮像手段の位置姿勢を補正する補正手段とを備えることを特徴とする情報処理装置。

本発明によれば、撮像装置によって撮像される画像上で観測される指標の数が１点の場合であっても、撮像装置の位置及び姿勢を計測することが可能となる。また、３点以上の指標が得られた場合においても、求めるべきパラメータ数が少ないため、情報の冗長性が相対的に増加し、従来手法と比して安定した解を得ることができる。この効果は、特に、指標の数が３点の場合に発揮される。したがって、解が得られない状況に陥る可能性が低下し、さらに、（同じ安定性を有する）解を得るために必要となる指標の数が減少する。その結果、シーン中に設定する指標数を減らせられるという利点が生じ、これが、誤認識率の低下につながり、また、美観を損ねるという問題の軽減にも貢献する。

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

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

図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、客観視点カメラ１８０（１８０ａ，１８０ｂ，１８０ｃ，１８０ｄ）、画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢センサ１４０、姿勢予測部１５０、及び位置姿勢算出部１２０によって構成されており、撮像装置１３０に接続されている。

現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標（以下、主観視点指標）として、世界座標系（現実空間の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における位置ｘ_Ｗ ^Ｑｋが既知である複数個の主観視点指標Ｑ_ｋ（ｋ＝１，，，Ｋ_Ｑ）が配置されている。また、姿勢センサ１４０上の複数の位置には、客観視点カメラ１８０によって撮影するための指標（以下、客観視点指標）として、主観視点カメラ座標系（撮像装置１３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）上における位置ｘ_Ｃ ^Ｐｋが既知である客観視点指標Ｐ_ｋ（ｋ＝１，，，Ｋ_Ｐ）が設定されている。

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

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

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

客観視点カメラ１８０（１８０ａ，１８０ｂ，１８０ｃ，１８０ｄ）は、計測対象範囲内に撮像装置１３０が位置するときに、そのいずれかが撮像装置１３０を撮像可能な位置に固定して配置されている。ここで、各客観視点カメラ１８０の世界座標系における位置及び姿勢は、データ記憶部１７０に既知の値として予め保持されているものとする。客観視点カメラ１８０が出力する画像（以下、これを客観視点画像と呼ぶ）は、画像入力部１６０に入力される。

画像入力部１６０は、位置姿勢装置１００へ入力される主観視点画像及び客観視点画像の夫々をデジタルデータに変換し、データ記憶部１７０に保存する。

姿勢センサ１４０は、撮像装置１３０に装着されており、姿勢センサ１４０自身の現在の姿勢を計測して姿勢予測部１５０へと出力する。姿勢センサ１４０は、ジャイロセンサをベースとしたセンサユニットであり、例えば、株式会社トキメックのＴＩＳＳ−５−４０や、米国ＩｎｔｅｒＳｅｎｓｅ社のＩｎｅｒｔｉａＣｕｂｅ２等によって構成される。これらのセンサによって計測される姿勢計測値は、真の姿勢とは異なった、誤差を有する姿勢である。ただし、これらの姿勢センサは、地球の重力方向を観測する傾斜角センサを構成要素として有しており、傾斜角方向のドリフト誤差の蓄積をキャンセルする機能を有しているので、傾斜角方向（ピッチ角及びロール角）に関してはドリフト誤差を発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては時間の経過に伴って蓄積されるドリフト誤差を有している。

姿勢予測部１５０は、方位ドリフト誤差補正値φをデータ記憶部１７０から入力し、姿勢センサ１４０から入力した姿勢計測値を補正して撮像装置１３０の姿勢を予測し、データ記憶部１７０へと出力する。

指標検出部１１０は、データ記憶部１７０より主観視点画像及び客観視点画像の夫々を入力し、入力した画像中に撮影されている指標の画像座標を検出する。例えば、指標の各々が異なる色を有するマーカによって構成されている場合には、画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標の各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを画像上に施すことにより、指標の位置を検出する。なお、撮像装置１３０の位置の算出値（位置姿勢算出部１２０の出力）及び姿勢の予測値（姿勢予測部１５０の出力）をデータ記憶部１７０からさらに入力し、この予測値に基づいて画像上における指標の位置を予測し探索範囲を限定することにより、指標検出処理の計算負荷を軽減したり、指標の誤検出や誤同定を低減させたりすることが可能である。

指標検出部１１０は、さらに、検出された指標の画像座標とその指標の識別子をデータ記憶部１７０へと出力する。なお、以下では、主観視点画像上で検出された指標を、検出された指標の夫々に付けられた識別子ｎ（ｎ＝１，，，Ｎ）を用いて、Ｑ_ｋｎと表記する。また、客観視点画像上で検出された指標を、カメラの識別子ｘ（ｘ＝ａ，ｂ，ｃ，ｄ）と、検出された指標の夫々に付けられた識別子ｍ（ｍ＝１，，，Ｍ_ｘ）を用いて、Ｐ_ｋｘｍと表記する。ここで、Ｎは主観視点画像上で検出された指標の数を、Ｍ_ｘは夫々の客観視点画像上で検出された指標の数を表している。また、Ｍを、各画像上で検出された指標の総和とする。また、検出された指標Ｑ_ｋｎの画像座標をｕ^Ｑｋｎと表記し、検出された指標Ｐ_ｋｘｍの画像座標を、その画像を撮影した客観視点カメラの識別子に応じて、各々ｕ_ａ ^Ｐｋａｍ，ｕ_ｂ ^Ｐｋｂｍ，ｕ_ｃ ^Ｐｋｃｍ，ｕ_ｄ ^Ｐｋｄｍと表記する。例えば図１の場合には、Ｎ＝２，Ｍ_ａ＝０，Ｍ_ｂ＝０，Ｍ_ｃ＝２，Ｍ_ｄ＝１，Ｍ＝３であり、指標の識別子ｋ_１＝１，ｋ_２＝３，ｋ_ｃ１＝１，ｋ_ｃ２＝２，ｋ_ｄ１＝２と、これらを撮影したカメラの識別子と、これらに対応する画像座標ｕ^Ｑｋ１，ｕ^Ｑｋ２，ｕ_ｃ ^Ｐｋｃ１，ｕ_ｃ ^Ｐｋｃ２，ｕ_ｄ ^Ｐｋｄ１が出力される。

位置姿勢算出部１２０は、撮像装置１３０の姿勢の予測値と、指標検出部１１０によって検出された各々の主観視点指標の画像座標ｕ^Ｑｋｎと世界座標ｘ_Ｗ ^Ｑｋｎの組と、各々の客観視点指標の画像座標ｕ^Ｐｋａｍ，ｕ^Ｐｋｂｍ，ｕ^Ｐｋｃｍ，ｕ^Ｐｋｄｍとそれに対応する主観視点カメラ座標（主観視点カメラ座標系における座標値）ｘ_Ｃ ^Ｐｋａｍ，ｘ_Ｃ ^Ｐｋｂｍ，ｘ_Ｃ ^Ｐｋｃｍ，ｘ_Ｃ ^Ｐｋｄｍの組とをデータ記憶部１７０から入力し、これらの情報に基づいて撮像装置１３０の位置及び姿勢を算出し、Ｉ／Ｆを介して外部へ出力する。また、算出した撮像装置１３０の位置をデータ記憶部１７０に出力し、さらに、位置及び姿勢の算出工程で導出される姿勢センサ１４０の方位ドリフト誤差補正値の更新値によって、データ記憶部１７０が保持する方位ドリフト誤差補正値を更新する。

データ記憶部１７０は、方位ドリフト誤差補正値、画像入力部１６０から入力される画像、姿勢予測部１５０から入力される姿勢の予測値、位置姿勢算出部１２０から入力される位置の算出値、指標検出部１１０から入力される各々の指標の画像座標及び識別子、さらに、既知の値である主観視点指標の世界座標、客観視点指標の物体座標（物体座標系における座標値）、客観視点カメラ１８０のカメラパラメータ等のデータを保持し、必要に応じてこれらを入出力する。

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

図２は画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部１５０、及び位置姿勢算出部１２０の夫々をソフトウェアとして実行するコンピュータの基本構成を示す図である。

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

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

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

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

１００７は外部記憶装置で、ハードディスクなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やソフトウェアのプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。

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

１００９はＩ／Ｆで、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢシリアルポート、また、算出した撮像装置１３０の位置及び姿勢を外部へ出力するためのイーサネット（登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０の機能の一部は、Ｉ／Ｆ１００９によって実現される。

１０１０は上述の各部を繋ぐバスである。

図３は、姿勢予測部１５０の処理手順を示すフローチャートであり、ＣＰＵ１００１が姿勢予測部１５０のソフトウェアのプログラムを実行することで行われる。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、ＲＡＭ１００２に既にロードされているものとする。

なお、姿勢を表現する方法には様々なものが存在するが、ここでは、３×３の回転行列Ｒによって表現されているものとする。

ステップＳ３０００において、姿勢予測部１５０は、姿勢センサ１４０から姿勢計測値Ｒ^＃（ ^＃ はセンサによる計測値を表す記号）を入力する。

ステップＳ３０１０において、姿勢予測部１５０は、データ記憶部１７０から方位ドリフト誤差補正値φ^＊を入力する。

ステップＳ３０２０において、姿勢予測部１５０は、姿勢計測値Ｒ^＃（姿勢センサ１４０の姿勢を表している）に、姿勢センサ１４０から計測対象物体１３０への姿勢変換と、方位ドリフト誤差補正値φ^＊によるドリフト誤差補正を反映させ、方位ドリフト誤差補正後の撮像装置１３０の姿勢を算出し、これを、姿勢の予測値Ｒ^＊とする。

ここで、△Ｒ（φ）は、方位方向にφだけの回転を加える回転行列であり、φの関数として次式によって定義される。

ここで、ｌ＝（ｌ_１，ｌ_２，ｌ_３）は、世界座標系における鉛直上方向（地球の重力の反対方向）を表す既知のベクトルを表している。

また、Ｒ_ＳＣは、主観視点カメラ座標系（撮像装置１３０の位置及び姿勢を表す座標系）からセンサ座標系（姿勢センサ１４０の位置及び姿勢を表す座標系）へ姿勢を変換する３×３の行列であり、固定値である姿勢センサ１４０と撮像装置１３０の相対姿勢に基づいて、既知の値として予め設定されている。

ステップＳ３０３０において、姿勢予測部１５０は、姿勢の予測値Ｒ^＊をデータ記憶部１７０へと出力する。

ステップＳ３０４０において、姿勢予測部１５０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ３０００へと進める。

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

位置姿勢算出部１２０は、撮像装置１３０の位置ｔ＝［ｘｙｚ］^Ｔと、姿勢センサ１４０の方位ドリフト誤差補正値の更新値φの合計４パラメータを、算出すべき未知パラメータとして取り扱う。すなわち、本実施形態では、姿勢についてはその全てを未知とするのではなく、姿勢の予測値Ｒ^＊には方位方向のドリフト誤差のみが含まれており、その方位ドリフト誤差補正値の更新値φのみを求めることで、撮像装置１３０の姿勢が得られるというモデルを適用する。以下では、求めるべき未知パラメータを４値の状態ベクトルｓ＝［ｘｙｚφ］^Ｔによって記述する。

ステップＳ４０００において、位置姿勢算出部１２０は、撮像装置１３０の姿勢の予測値Ｒ^＊（姿勢予測部１５０の出力）をデータ記憶部１７０から入力する。

ステップＳ４０１０において、位置姿勢算出部１２０は、状態ベクトルｓの初期値を、ｓ＝［ｘ_−１ｙ_−１ｚ_−１０］^Ｔと設定する。ここで、ｘ_−１，ｙ_−１，ｚ_−１は、１ループ前の処理におけるステップＳ４１１０において算出された計測対象物体１３０の位置を表している。

ステップＳ４０２０において、位置姿勢算出部１２０は、指標検出部１１０によって検出された主観視点指標の画像座標とその世界座標の組と、客観視点指標の画像座標とその世界座標の組を、データ記憶部１７０から入力する。例えば図１の場合には、主観視点指標の画像座標ｕ^Ｑ１，ｕ^Ｑ３及びそれらに対応する世界座標ｘ_Ｗ ^Ｑ１，ｘ_Ｗ ^Ｑ３と、客観視点指標の画像座標ｕ_ｃ ^Ｐ１，ｕ_ｃ ^Ｐ２，ｕ_ｄ ^Ｐ２及びそれらに対応する主観視点カメラ座標（主観視点カメラ座標系における座標値）ｘ_Ｃ ^Ｐ１，ｘ_Ｃ ^Ｐ２が入力される。

ステップＳ４０３０において、位置姿勢算出部１２０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力した指標の実体の総数が２点以上の場合はステップＳ４０４０に処理を進め、２点未満の場合はステップＳ４１００に処理を進める。例えば図４の場合には、２つの主観視点指標と２つの客観視点指標（投影像は３点であるが，指標の実体は２つ）が検出されており，総数は４となるので，ステップＳ４０４０へと処理が進められる。

ステップＳ４０４０において、位置姿勢算出部１２０は、ステップＳ４０２０で入力した各々の主観視点指標及び客観視点指標の画像座標の推定値を、現在の状態ベクトルｓに基づいて算出する。

主観視点指標Ｑ^ｋｎの画像座標の推定値ｕ^Ｑｋｎ＊の算出は、指標Ｑ^ｋｎの世界座標ｘ_Ｗ ^Ｑｋｎと現在の状態ベクトルｓの関数：

・・・式（３）
を用いて行う。具体的には、関数Ｆｃ（）は、世界座標ｘ_Ｗ ^Ｑｋｎとｓから主観視点カメラ座標（主観視点カメラ座標系における座標）ｘ_Ｃ ^Ｑｋｎを求める次式、

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

・・・式（５）
によって構成されている。ここで、Ｒ^＊は、ステップＳ４０００で入力した姿勢の予測値を表している。また、△Ｒ（φ）は，方位方向にφだけの回転を加える回転行列を表しており、式２によって定義される。また、ｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。

一方、客観視点指標Ｐ_ｋｍの画像座標の推定値ｕ^Ｐｋｍ＊の算出は、客観視点指標Ｐ_ｋｍ各々の主観視点カメラ座標（主観視点カメラ座標系における座標値）ｘ_Ｃ ^Ｐｋｍと現在の状態ベクトルｓの関数：

・・・式（６）

を用いて行う。具体的には、関数Ｆ_Ｂ（ ）は、世界座標系上における当該指標の座標ｘ_Ｗ ^Ｐｋｍをｘ_Ｃ ^Ｐｋｍとｓから求める次式、

及び、世界座標系ｘ_Ｗ ^Ｐｋｍから客観視点カメラ座標（客観視点カメラ座標系（客観視点カメラ１８０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）上における当該指標の座標）ｘ_Ｂ ^Ｐｋｍを求める次式、

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

・・・式（９）

によって構成されている。ここで、ｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは夫々の客観視点カメラ１８０のｘ軸方向及びｙ軸方向における焦点距離、Ｒ_ＷＢは夫々の客観視点カメラ１８０の世界座標系における姿勢を表す３ｘ３行列、ｔ_ＷＢは夫々の客観視点カメラ１８０の世界座標系における位置を表す３次元ベクトルであり、客観視点カメラ１８０の夫々に対して既知の値として予め保持されている。

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

・・・式（１０）

・・・式（１１）

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

・・・式（１２）

一方、客観視点指標Ｐ_ｋｍの画像ヤコビアンは、式６の関数Ｆｂ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×４列のヤコビ行列Ｊ_ｕｓ ^Ｐｋｍ（＝∂ｕ／∂ｓ）であり、具体的には、式９の右辺を客観視点カメラ座標ｘ_Ｂ ^Ｐｋｍの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘＢ ^Ｐｋｍ（＝∂ｕ／∂ｘ_Ｂ）と、式８の右辺を世界座標ｘ_Ｗ ^Ｐｋｍの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列Ｊ_ｘＢｘＷ ^Ｐｋｍ（＝∂ｘ_Ｂ／∂ｘ_Ｗ）と、式７の右辺を状態ベクトルｓの各要素で偏微分した解を各要素に持つ３行×４列のヤコビ行列Ｊ_ｘＷｓ ^Ｐｋｍ（＝∂ｘ_Ｗ／∂ｓ）を求めた後に、これらを用いて次式によって算出される。

ステップＳ４０７０において、位置姿勢算出部１２０は、ステップＳ４０５０及びステップＳ４０６０において算出した各々の指標に対する誤差△ｕ及び画像ヤコビアンＪ_ｕｓに基づいて、状態ベクトルｓの補正値△ｓを次式によって算出する。

ここでＵは、各々の指標（主観視点指標及び客観視点指標）に対して求めた誤差△ｕを垂直に並べた２（Ｎ＋Ｍ）次元の誤差ベクトル

であり、θ’は、各々の指標（主観視点指標及び客観視点指標）に対して求めた画像ヤコビアンＪ_ｕｓを垂直に並べた２（Ｎ＋Ｍ）行×４列の行列

の擬似逆行列を表している。図１の例では、Ｎ＝２及びＭ＝３であるので、Ｕは１０次元ベクトル、θは１０行×４列の行列となる。

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

・・・式（１７）

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

ステップＳ４０９０において計算が収束したと判定されると、ステップＳ４１００において、位置姿勢算出部１２０は、得られた状態ベクトルｓから、撮像装置１３０の姿勢を算出する。具体的には、前ステップまでで得られた状態ベクトルｓの算出値から、ドリフト誤差補正値の更新値φを得て、次式：

によって、撮像装置１３０の姿勢Ｒを算出する。

ステップＳ４１１０において、位置姿勢算出部１２０は、得られた撮像装置１３０の位置及び姿勢の情報をＩ／Ｆ１００９を介して外部へ出力する。また、撮像装置１３０の位置ｔをデータ記憶部１７０へと出力する。なお、位置及び姿勢の出力の形態は例えば姿勢を表す３×３行列Ｒと位置を表す３次元ベクトルｔの組であってもよいし、姿勢をオイラー角に変換したものでもよいし、位置及び姿勢から算出したビューイング変換行列でもよいし、他のいずれの位置姿勢記述方法によるものでもよい。

ステップＳ４１２０において、位置姿勢算出部１２０は、以上の算出工程で得られた方位ドリフト誤差補正値の更新値φを用いて、データ記憶部１７０が保持している方位ドリフト誤差補正値φ^＊を次式によって更新する。

ステップＳ４１３０において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ４０００へと進み、次フレーム以降の入力データに対して同様の処理を実行する。

以上によって、撮像装置の位置及び姿勢が計測される。

本実施形態に係る位置姿勢計測装置によれば、主観視点画像及び客観視点画像上で観測される指標の数の合計が２点以上であれば、撮像装置の位置及び姿勢を計測することができる。したがって、主観視点の映像が（手等による隠蔽によって）遮蔽される場合であっても、客観視点画像の情報（２点以上の客観視点指標の観測）によって位置及び姿勢の計測を行うことが可能であるし、反対に、客観視点指標が全て遮蔽されてしまうような状況下であっても、主観視点画像の情報（２点以上の主観視点指標の観測）によって、位置及び姿勢の計測を継続することが可能となる。

なお、上記の実施形態では複数台の客観視点カメラ１８０を用いていたが、必ずしも複数台の客観視点カメラが必要ではなく、客観視点カメラが１台の場合であっても本実施形態と同様の効果が得られることはいうまでもない。

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

図５は、本実施形態に係る位置姿勢計測装置の構成を示す図である。図５に示したように、本実施形態に係る位置姿勢計測装置５００は、画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢センサ１４０、姿勢予測部１５０、位置姿勢算出部５２０、及び主観視点カメラ５３０によって構成されている。

第１の実施形態と同じ機能を有する部分については図１と同じ番号を付けており、その説明を省略する。ただし、主観視点カメラ５３０によって取得された画像が主観視点画像として画像入力部１６０に入力されるという点、姿勢予測部１５０による姿勢の予測値が主観視点カメラ５３０の姿勢であるという点、姿勢センサ１４０が主観視点カメラ５３０に装着されている点は、第１の実施形態とは異なっている。

主観視点カメラ５３０は、計測対象物体５８０に固定して装着されている。ここで、主観視点カメラ座標系における計測対象物体５８０の位置及び姿勢は既知であるとする。

位置姿勢算出部５２０は、主観視点カメラ５３０の姿勢の予測値Ｒ^＊と、指標検出部１１０によって検出された各々の主観視点指標の画像座標と世界座標の組と、各々の客観視点指標の画像座標とそれに対応する主観視点カメラ座標の組とをデータ記憶部１７０から入力し、これらの情報に基づいて、第１の実施形態における位置姿勢算出部１２０と同様な処理によって、主観視点カメラ５３０の位置及び姿勢を算出する。また、算出した位置をデータ記憶部１７０に出力し、さらに、位置及び姿勢の算出工程で導出される姿勢センサ１４０の方位ドリフト誤差補正値の更新値φによって、データ記憶部１７０が保持する方位ドリフト誤差補正値を更新する。

位置姿勢算出部５２０はさらに、上記によって算出した（世界座標系における）主観視点カメラ５３０の位置及び姿勢と、既知の値であるカメラ座標系における計測対象物体５８０の位置及び姿勢とに基づいて、計測対象物体５８０の位置及び姿勢を算出する。算出された位置及び姿勢は、Ｉ／Ｆ１００９を介して外部へ出力される。

以上によって、任意の計測対象物体の位置及び姿勢の計測が実現される。

なお、本実施形態における位置姿勢算出部５２０は主観視点カメラ５３０の位置及び姿勢を一旦求めた後に計測対象物体５８０の位置及び姿勢を求めていたが、計測対象物体５８０の位置及び姿勢を直接求めてもよい。この場合、姿勢予測部１５０は計測対象物体５８０の位置姿勢を推定するようにし（式１のＲ_ＳＣを、計測物体座標系からセンサ座標系へ姿勢を変換する３×３の行列とする）、計測対象物体５８０の位置を状態ベクトルｓの構成要素として設定し、主観視点指標の主観視点カメラ座標ｘ_Ｃ ^Ｑｋｎをｓから求める式４を、

に、客観視点指標の世界座標ｘ_Ｗ ^Ｐｋｍをｓから求める式７を、

に置き換えればよい。ここで、Ｒ_ＣＯは計測物体座標系（計測対象物体５８０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）から主観視点カメラ座標系へ姿勢を変換する行列、ｔ_ＣＯは同座標系間の位置を変換するベクトルであり、既知の値として予め保持されている主観視点カメラ座標系における計測対象物体５８０の位置及び姿勢に基づいて、予め算出されているものとする。

なお、本実施形態における計測対象物体５８０は、情景を撮像する撮像装置であってもよい。そして、主観視点カメラ５３０を、情景を撮像する撮像装置の視野とは異なる視野となるように例えば上向きに配置し、それに伴って主観視点カメラ５３０の視野内に主観視点指標Ｑ_ｋを配置してもよい。そうすることにより、情景を撮像する撮像装置の視野内に主観視点指標Ｑ_ｋが入らなくなるため、美観を損ねるという問題の軽減等に貢献する。

また、本実施形態において、計測対象物体５８０に複数の主観視点カメラ５３０を装着することによって、計測対象物体５８０の位置及び姿勢の計測を、位置と姿勢のいずれにおいても高い精度で実現されるようにしてもよい。

［第３の実施形態］
上記の実施形態において、位置姿勢算出部１２０及び５２０は、いずれも、方位ドリフト誤差補正値の更新値と位置とを表す４値の状態ベクトルｓを未知数として、主観視点指標および客観視点指標の検出座標（実測値）と、それらの点の計算値との間の誤差の総和を最小化するようなｓを求めていた。しかし、主観視点画像と客観視点画像の双方から得られる幾何的な拘束条件を併用することで計測対象物体の位置及び姿勢を取得する方法はこれに限定されるものではない。本実施形態に係る位置姿勢計測装置は、全体の誤差を最小化する手法とは異なる手法を用いた位置姿勢算出部をその構成要素として有していることを特徴とする。

本実施形態に係る位置姿勢計測装置の構成は基本的には第１の実施形態と同様であるが、位置姿勢算出部１２０が第１の実施形態のそれとは異なる位置姿勢算出部１２０’に置換された構成となっている（不図示）。つまり、位置姿勢算出部が行う処理が異なる。

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

本実施形態では、各部（画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部１５０、及び位置姿勢算出部１２０’）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。またこのコンピュータの基本構成は図２に示した構成とする。

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

ステップＳ６０００において、位置姿勢算出部１２０’は、撮像装置１３０の姿勢の予測値Ｒ^＊（姿勢予測部１５０の出力）をデータ記憶部１７０から入力する。

ステップＳ６００３において、位置姿勢算出部１２０’は、指標検出部１１０において検出された客観視点指標について、その画像座標とカメラ座標の組を入力する。

なお、複数の客観視点指標が設置されている場合や、複数の客観視点カメラが設置されている場合には、複数の客観視点指標の投影像が検出され、各々の画像座標が入力される状況が発生する。本実施形態では、このような場合であっても、以降の処理に使用される客観視点指標は１点のみであり、位置姿勢算出部１２０’は、適当な１点を客観視点指標Ｐの画像座標ｕ^Ｐとして選択する。

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

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

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

以下、世界座標系における客観視点指標Ｐの位置を定める媒介変数τと、姿勢センサ１４０の方位ドリフト誤差補正値の更新値φの合計２パラメータを、算出すべき未知パラメータとして取り扱う。以下では、求めるべき未知パラメータを２値の状態ベクトルｓ’＝［τφ］^Ｔによって記述する。

ステップＳ６０１０において、位置姿勢算出部１２０’は、状態ベクトルｓ’に適当な初期値ｓ’＝［τ_−１０］^Ｔを設定する。ここで、τ_−１としては、例えば、１ループ前の処理において得られた撮像装置１３０の位置から客観視点指標Ｐの世界座標を求め、得られた世界座標に最も近い直線ｌ_Ｗ上の点を表すτを設定する。

ステップＳ６０２０において、位置姿勢算出部１２０’は、指標検出部１１０において検出された各々の主観視点指標の画像座標とその世界座標の組を入力する。

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

ステップＳ６０４０において、位置姿勢算出部１２０’は、各々の主観視点指標Ｑ^ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を、世界座標ｘ_Ｗ ^Ｑｋｎとｓ’から主観視点カメラ座標（主観視点カメラ座標系における座標）ｘ_Ｃ ^Ｑｋｎを求める次式、

及び、カメラ座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める式５に基づいて算出する。ここで、ｘ_Ｃ ^Ｐは、主観視点カメラ座標系における指標Ｐの座標値であり、既知な情報として予め外部記憶装置１００７に保持されている。

すなわち、撮像装置１３０の位置及び姿勢が従前に求めた状態ベクトルｓ’に従っていると仮定して、撮像装置１３０と主観視点指標との位置姿勢関係に応じて、各々の主観視点指標の画像座標の推定値を求める。

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

ステップＳ６０６０において、位置姿勢算出部１２０’は、各々の主観視点指標Ｑ^ｋｎに対して、状態ベクトルｓ’に関する画像ヤコビアン（すなわち、式１の関数Ｆｃ（）（本実施形態では、式２４と式５によって構成されるものであり、ｓはｓ’となる）をｓ’の各要素で偏微分した解を各要素に持つ２行×２列のヤコビ行列）Ｊ_ｕｓ’ ^Ｑｋｎ（＝∂ｕ／∂ｓ’）を算出する。具体的には、式５の右辺をカメラ座標ｘ_Ｃ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｑｋｎ（＝∂ｕ／∂ｘ）と、式２４の右辺をベクトルｓ’の各要素で偏微分した解を各要素に持つ３行×２列のヤコビ行列Ｊ_ｘｓ’ ^Ｑｋｎ（＝∂ｘ／∂ｓ’）とを算出した後に、これらを用いて式１２（ただしｓはｓ’とおきかえる）によって算出する。

ステップＳ６０７０において、位置姿勢算出部１２０’は、式１４（ただしｓはｓ’とおきかえる）を用いて、状態ベクトルｓ’の補正値△ｓ’を算出する。ただし、本実施形態においては、Ｕは各々の主観視点指標に対して求めた誤差△ｕ^Ｑｋｎを垂直に並べた２Ｎ次元の誤差ベクトルであり、θは各々の主観視点指標に対して求めた画像ヤコビアンＪ_ｕｓ’ ^Ｑｋｎを垂直に並べた２Ｎ行×２列の行列を表している。

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

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

ステップＳ６０９０において計算が収束したと判定されると、ステップＳ６１００において、位置姿勢算出部１２０’は、撮像装置１３０の位置及び姿勢を算出する。姿勢Ｒの算出は、前ステップまでで得られたドリフト誤差補正値の更新値φを用いて、式１８によって行う。一方、位置ｔの算出は、前ステップまでで得られた媒介変数τ及び姿勢Ｒを用いて、次式によって行う。

ステップＳ６１１０において、位置姿勢算出部１２０’は、得られた撮像装置１３０の位置及び姿勢の情報をＩ／Ｆ１００９を介して外部へ出力する。また、撮像装置１３０の位置ｔをデータ記憶部１７０へと出力する。なお、位置及び姿勢の出力の形態は例えば姿勢を表す３×３行列Ｒと位置を表す３次元ベクトルｔの組であってもよいし、姿勢をオイラー角に変換したものでもよいし、位置及び姿勢から算出したビューイング変換行列でもよいし、他のいずれの位置姿勢記述方法によるものでもよい。

ステップＳ６１２０において、位置姿勢算出部１２０’は、以上の算出工程で得られた方位ドリフト誤差補正値の更新値φを用いて、データ記憶部１７０が保持している方位ドリフト誤差補正値φ^＊を式１９によって更新する。

ステップＳ６１３０において、位置姿勢算出部１２０’は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ６０００へと進み、次フレーム以降の入力データに対して同様の処理を実行する。

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

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

［第４の実施形態］
上記の実施形態において、位置姿勢算出部１２０，５２０，及び１２０’は、いずれも、姿勢センサ１４０の方位ドリフト誤差補正値の更新値を未知数として求めていた。しかし、方位方向のみに姿勢の補正項目を限定せずに、姿勢センサ１４０の３軸方向に対する誤差の補正値を求めることももちろん可能である。本実施形態に係る位置姿勢計測装置の構成は、第１の実施形態に係る位置姿勢計測装置の構成とほぼ同じであるので、以下では、第１の実施形態とは異なる部分についてのみを説明する。

本実施形態におけるデータ記憶部１７０は、姿勢センサ１４０の方位ドリフト誤差補正値φ^＊の代わりに、姿勢センサ１４０の誤差補正回転行列△Ｒ^＊を記憶している。

本実施形態における姿勢予測部１５０は、姿勢センサ１４０の方位ドリフト誤差補正値φ^＊を入力する代わりに姿勢センサ１４０の誤差補正回転行列△Ｒ^＊を入力し（ステップＳ３０１０）、式１の代わりに次式によって姿勢の予測値Ｒ^＊を算出する（ステップＳ３０２０）。

本実施形態における位置姿勢算出部１２０では、撮像装置１３０の位置ｔ＝［ｘｙｚ］^Ｔと、撮像装置１３０の姿勢Ｒの３値表現ω＝［ξψζ］^Ｔの合計６パラメータを、算出すべき未知パラメータとして取り扱う。以下では、求めるべき未知パラメータを６値の状態ベクトルｓ’’＝［ｘｙｚξψζ］^Ｔによって記述する。

姿勢（回転行列）を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。なお、姿勢ωは、次式

（ただし、

である。）
によって、３×３の回転行列Ｒによっても表現可能であり、ωとＲとは、互いに一意に変換することができる。Ｒからωへの変換方法は公知であるので、その詳細な説明は省略する。

本実施形態における位置姿勢算出部１２０では、状態ベクトルｓ’’の初期値を、ｓ’’＝ｘ_−１ｙ_−１ｚ_−１ξ^＊ψ^＊ζ^＊］^Ｔと設定する（ステップＳ４０１０）。ここで、ｘ_−１，ｙ_−１，ｚ_−１は、１ループ前の処理において算出された計測対象物体１３０の位置を表しており、ξ^＊，ψ^＊，ζ^＊は、姿勢の予測値Ｒ^＊から得られる姿勢の３値表現を表している。

本実施形態における位置姿勢算出部１２０では、入力した指標数による処理の分岐（ステップＳ４０３０）を、入力した指標の実体の総数が３点以上か否かで判定する。

本実施形態における位置姿勢算出部１２０では、主観視点指標Ｑ_ｋｎの主観視点カメラ座標ｘ_Ｃ ^Ｑｋｎを当該指標の世界座標ｘ_Ｗ ^Ｑｋｎとｓ（本実施形態ではｓ’’）とから求める関係式と、客観視点指標Ｐ_ｋｍの世界座標ｘ_Ｗ ^Ｐｋｍを当該指標の主観視点カメラ座標ｘ_Ｃ ^Ｐｋｍとｓ（本実施形態ではｓ’’）とから求める関係式が、第１の実施形態における式４と式７から、それぞれ次式（式２８及び式２９）に変更される。したがって、各指標に対する画像ヤコビアンは、２行×６列のヤコビ行列Ｊ_ｕｓ’’ ^Ｑｋｎ（＝∂ｕ／∂ｓ’’）となる。

本実施形態における位置姿勢算出部１２０では、撮像装置１３０の姿勢Ｒの算出（ステップＳ４１００）を、得られた状態ベクトルｓ’’を用いて式２７によって行う。

本実施形態における位置姿勢算出部１２０は、ステップＳ４１２０において、以上の算出工程で得られた撮像装置１３０の姿勢Ｒを用いて姿勢センサ１４０の誤差補正回転行列△Ｒ^＊を次式によって求め、データ記憶部１７０が保持している値を更新する。

以上によって、撮像装置の位置及び姿勢が計測される。

［第５の実施形態］
第１の実施形態では、世界座標系に固定された客観視点カメラ１８０によって，撮像装置１３０（に取り付けられた姿勢センサ）上に配置された客観視点指標Ｐ_ｋを撮影していた。しかし、撮像装置１３０の位置及び姿勢を取得するための構成はこれに限られるものではない。本実施形態に係る位置姿勢計測装置は、撮像装置１３０上に固定された客観視点カメラ１８０によって、世界座標系に配置された客観視点指標Ｐ_ｋを撮影する構成となっていることを特徴とする。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図７は、本実施形態に係る位置姿勢計測装置の構成を示す図である。図７に示したように、本実施形態に係る位置姿勢計測装置７００は、客観視点カメラ１８０（１８０ａ，１８０ｂ）、画像入力部１６０、データ記憶部７７０、指標検出部１１０、姿勢センサ１４０、姿勢予測部１５０、及び位置姿勢算出部７２０によって構成されており、撮像装置１３０に接続されている。なお、第１の実施形態と同じ機能を有する部分については図１と同じ番号を付けており、その説明を省略する。

撮像装置１３０上には、主観視点カメラ座標系における位置及び姿勢が既知である客観視点カメラ１８０（１８０ａ，１８０ｂ）が固定して配置されている。

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

図７の例は、３個の主観視点指標Ｑ_１，Ｑ_２，Ｑ_３と２個の客観視点指標Ｐ_１，Ｐ_２が設定されており、そのうちの２個の主観視点指標Ｑ_１，Ｑ_３が撮像装置１３０の視野内に含まれており、客観視点指標Ｐ_１が客観視点カメラ１８０ａおよび１８０ｂの視野内に、客観視点指標Ｐ_２が客観視点カメラ１８０ｂの視野内に含まれている状況を示している。

図７の場合には、主観視点画像上で検出された指標の数および夫々の客観視点画像上で検出された指標の数は、Ｎ＝２，Ｍ_ａ＝１，Ｍ_ｂ＝２となる。指標検出部１１０からは、指標の識別子ｋ_１＝１，ｋ_２＝３，ｋ_ａ１＝１，ｋ_ｂ１＝１，ｋ_ｂ２＝２と、これらを撮影したカメラの識別子と、これらに対応する画像座標ｕ^Ｑｋ１，ｕ^Ｑｋ２，ｕ_ａ ^Ｐｋａ１，ｕ_ｂ ^Ｐｋｂ１，ｕ_ｂ ^Ｐｋｂ２が出力される。

客観視点カメラ１８０（１８０ａ，１８０ｂ）は、計測対象範囲内に撮像装置１３０が位置するときに、そのいずれかが客観視点指標Ｐ_ｋを撮像可能となるように配置されている。ここで、各客観視点カメラ１８０の主観視点カメラ座標系における位置及び姿勢は、データ記憶部７７０に既知の値として予め保持されているものとする。

位置姿勢算出部７２０は、撮像装置１３０の姿勢の予測値と、指標検出部１１０によって検出された各々の主観視点指標の画像座標ｕ^Ｑｋｎと世界座標ｘ_Ｗ ^Ｑｋｎの組と、各々の客観視点指標の画像座標ｕ^Ｐｋａｍ，ｕ^Ｐｋｂｍとそれに対応する世界座標ｘ_Ｗ ^Ｐｋａｍ，ｘ_Ｗ ^Ｐｋｂｍの組とをデータ記憶部７７０から入力し、これらの情報に基づいて撮像装置１３０の位置及び姿勢を算出し、Ｉ／Ｆを介して外部へ出力する。また、算出した撮像装置１３０の位置をデータ記憶部７７０に出力し、さらに、位置及び姿勢の算出工程で導出される姿勢センサ１４０の方位ドリフト誤差補正値の更新値によって、データ記憶部７７０が保持する方位ドリフト誤差補正値を更新する。

データ記憶部７７０は、方位ドリフト誤差補正値、画像入力部１６０から入力される画像、姿勢予測部１５０から入力される姿勢の予測値、位置姿勢算出部７２０から入力される位置の算出値、指標検出部１１０から入力される各々の指標の画像座標及び識別子、さらに、既知の値である主観視点指標の世界座標、客観視点指標の世界座標、客観視点カメラ１８０の主観視点カメラ座標系における位置及び姿勢等のデータを保持し、必要に応じてこれらを入出力する。

本実施形態に係る、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートは，第１の実施形態に係るフローチャート（図４）とほぼ同じであるので、以下では、第１の実施形態とは異なる部分についてのみ説明する。

ステップＳ４０２０において、位置姿勢算出部７２０は、主観視点カメラ座標（主観視点カメラ座標系における座標値）ｘ_Ｃ ^Ｐ１，ｘ_Ｃ ^Ｐ２を入力する代わりに、世界座標ｘ_Ｗ ^Ｐ１，ｘ_Ｗ ^Ｐ２を入力する。

ステップＳ４０４０において、客観視点指標Ｐ_ｋｍの画像座標の推定値ｕ^Ｐｋｍ＊の算出は、客観視点指標Ｐ_ｋｍ各々の世界座標ｘ_Ｗ ^Ｐｋｍと現在の状態ベクトルｓの関数：

を用いて行う。具体的には、関数Ｆ_Ｄ（ ）は、世界座標ｘ_Ｗ ^Ｐｋｍとｓから主観視点カメラ座標（主観視点カメラ座標系における座標）ｘ_Ｃ ^Ｐｋｍを求める次式、

及び、主観視点カメラ座標ｘ_Ｃ ^Ｐｋｍから客観視点カメラ座標（客観視点カメラ座標系（客観視点カメラ１８０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）上における当該指標の座標）ｘ_Ｂ ^Ｐｋｍを求める次式、

及び、客観視点カメラ座標ｘ_Ｂ ^Ｐｋｍから画像座標ｕ^Ｐｋｍ＊を求める式９によって構成されている。ここで、ｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは夫々の客観視点カメラ１８０のｘ軸方向及びｙ軸方向における焦点距離、Ｒ_ＣＢは夫々の客観視点カメラ１８０の主観視点カメラ座標系における姿勢を表す３ｘ３行列、ｔ_ＣＢは夫々の客観視点カメラ１８０の主観視点カメラ座標系における位置を表す３次元ベクトルであり、客観視点カメラ１８０の夫々に対して既知の値として予め保持されている。

以上によって、撮像手段の位置及び姿勢が計測される。

なお、上記の実施形態では複数台の客観視点カメラ１８０を用いていたが、必ずしも複数台の客観視点カメラが必要ではなく、客観視点カメラが１台の場合であっても本実施形態と同様の効果が得られることは言うまでもない。

また、第２乃至第４の実施形態においても、本実施形態で説明した、撮像装置１３０上に固定された客観視点カメラ１８０によって世界座標系に配置された客観視点指標Ｐｋを撮影する構成を用いることができることは言うまでもない。

（変形例１）
上記の実施形態の各々において、誤差ベクトルＵと行列Θに基づく状態ベクトルの補正値△ｓの算出に式１１で表現される最急降下法を用いているが、補正値△ｓの算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても構わない。

（変形例２）
上記の実施形態の各々において、指標として一つの指標が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を用いていた。しかし、指標の種類に限定されるものではなく、他の種類の指標を用いても構わない。

例えば、公知の位置姿勢計測装置（例えば、”高橋，石井，牧野，中静，ＶＲインターフェースのための単眼による長方形マーカ位置・姿勢の高精度実時間推定法，３次元画像コンファレンス９６公演論文集，ｐｐ．１６７−１７２，１９９６．”を参照）に用いられているような、特定の幾何形状をもつマーカを主観視点指標及び／あるいは客観視点指標として用いることも出来る。例えば、四角形のマーカを指標として用いる場合、四角形の各頂点の世界座標を既知の値として保持しておき（あるいは、マーカの位置、向き、サイズからこれらの値を演算し）、画像から各頂点の画像座標を検出することで、上記実施形態における４点の指標と同様の効果を得ることができる。特に、ＩＤ情報を有する一つの四角形マーカ（例えば、”加藤，Ｍ．ＢｉｌｌｉｎｇＨｕｒｓｔ，浅野，橘，マーカー追跡に基づく拡張現実感システムとそのキャリブレーション，日本バーチャルリアリティ学会論文誌，ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１６，１９９９．”を参照）を客観視点指標として撮像装置に設置する構成（あるいは、姿勢センサに予め一つの四角形マーカを設置しておく構成）は、画像からのマーカの検出精度及び同定精度の良さが期待できるため、特に好適な形態といえる。

また、別の公知の位置姿勢計測装置（例えば、”Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ ｐａｒａｍｅｔｅｒｉｚｅｄ ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ ｍｏｄｅｌｓ ｔｏ ｉｍａｇｅｓ，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１．”を参照）に用いられているような、線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。例えば、直線の原点からの距離を評価のための基準として、画像からの検出値ｄと状態ベクトルｓからの推定値ｄ^＊から算出する誤差△ｄによって誤差ベクトルＵを構成し、ｄ^＊の算出式を状態ベクトルｓの各要素で偏微分した解を各要素に持つ１行×６列のヤコビ行列Ｊ_ｄｓ（＝∂ｄ／∂ｓ）によって行列θを構成することで、上記実施形態と同様な枠組みによって位置及び姿勢の計測を行うことが出来る。また、線指標と点指標、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することが可能である。

（変形例３）
第２の実施形態における主観視点カメラ５３０は１台であったが、複数台の主観視点カメラを計測対象物体５８０に設置し、位置及び姿勢の計測に用いることも可能である。この場合、画像入力部１５０、指標検出部１１０はそれぞれのカメラからの入力画像を処理する。また、姿勢予測部１５０及び位置姿勢算出部５２０は、計測対象物体５８０の位置及び姿勢を基準とした演算を行う。位置姿勢算出部５２０では、計測対象物体５８０の位置及び方位ドリフト補正値によって状態ベクトルｓを構成し、各画像から得られる指標の情報から、式２０（カメラ毎にＲ_ＣＯとｔ_ＣＯが異なる）に基づいて各指標の誤差と画像ヤコビアンを求め、それらを（客観視点指標の情報と共に）積み重ねて誤差ベクトルＵ及び行列θを構成することで、上記実施形態と同様な枠組みによって位置及び姿勢の推定を行うことができる。

また、第１、第３，第４の各実施形態における計測対象である撮像装置１３０は１台であったが、ステレオ・ビデオシースルー・ヘッドマウントディスプレイのような２台の撮像装置を計測する場合であっても、一方の撮像装置（例えば左眼用の撮像装置）を基準とすることで、同様な手法によって位置及び姿勢の計測を行うことができる。

（変形例４）
上記の実施形態の夫々では、方位ドリフト誤差を有する姿勢センサを用いていたが、他の姿勢センサを用いることも可能である。例えば、加速度センサによって傾斜方向の角度を計測し、地磁気センサによって方位方向の角度を計測するタイプの姿勢センサにおいても、位置と方位方向の誤差の更新値を未知パラメータとして、上記実施形態と同様な処理によって計測対象の位置と姿勢の計測を行うことができる。また、傾斜方向のみを計測する姿勢センサを用いる場合でも、方位方向の計測値が常に０となる３軸の姿勢センサと仮定すると、同様な処理によって計測対象の位置と姿勢の計測を行うことができる。

（変形例５）
上記の実施形態の夫々で用いる主観視点カメラまたは客観視点カメラには、可視光とは異なる波長の光を撮像するカメラを用いることも可能である。例えば、客観視点カメラとして赤外光を撮像するカメラを用い、客観視点指標として赤外光を発光または反射する指標を用いてもよい。この場合、主観視点指標は客観視点カメラには写らないため、客観視点画像上で主観視点指標を誤検出することがなくなるという効果がある。

ここで、客観視点指標の各々を、異なるタイミングで赤外光を発光するマーカによって構成すれば、いずれの指標であるかを識別することができる。すなわち、指標検出部において、客観視点画像上からマーカに対応する領域を抽出し、その重心位置を、客観視点カメラによる撮像と同一のタイミングで発光された指標の検出座標とすればよい。もちろん、客観視点指標が一つの場合には発光タイミングの制御は不要である。

なお、客観視点カメラとして赤外光を撮像するカメラを用い、客観視点指標として赤外光を発光または反射する指標を用いることも可能である。さらに、主観視点カメラと客観視点カメラの両方に赤外光を撮像するカメラを用い、主観視点指標と客観視点指標の両方に赤外光を発光または反射する指標を用いてもよい。

また、可視光とは異なる波長の光を撮像するカメラとしては、赤外光を撮像するカメラに限らず、紫外光を撮像するカメラなどを用いてもよい。さらに、可視光とは異なる波長の光と可視光の両方を同時に撮像するカメラを用いてもよい。

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

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

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

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

Claims (6)

1. 現実空間に配置された主観視点点指標を撮像した主観視点画像を取得する主観視点撮像手段と、
   前記主観視点撮像手段に伴って配置された客観視点点指標を、前記現実空間の世界座標系において固定されて既知である客観視点位置姿勢から撮像した客観視点画像を取得する客観視点撮像手段と、
   前記主観視点撮像手段に備えられ、前記主観視点撮像手段の姿勢計測値を計測する姿勢計測手段と、
   前記姿勢計測手段の方位ドリフト誤差補正値、前記主観視点点指標の前記世界座標系における位置、前記客観視点点指標の前記主観視点撮像手段に対する位置、前記主観視点撮像手段の焦点距離および前記客観視点撮像手段の焦点距離を予め保持する保持手段と、
   前記姿勢計測値と前記方位ドリフト誤差補正値に基づいて、前記主観視点撮像手段の姿勢予測値を算出する姿勢予測手段と、
   前記主観視点画像から少なくとも１点の主観視点点指標の画像座標を検出する第１の検出手段と、
   前記客観視点画像から少なくとも１点の客観視点点指標の画像座標を検出する第２の検出手段と、
   前記客観視点位置姿勢、前記姿勢予測値、前記検出された主観視点点指標の画像座標、当該主観視点点指標の世界座標系における位置、前記検出された客観視点点指標の画像座標、および当該客観視点点指標の前記主観視点撮像手段に対する位置に基づいて、前記主観視点撮像手段の位置姿勢を算出する位置姿勢算出手段と、
   前記算出された前記主観視点撮像装置の位置姿勢に基づいて、前記保持された方位ドリフト誤差補正値を更新する方位誤差更新手段と
   を備え、
   前記位置姿勢算出手段が、
   前記主観視点撮像手段の位置姿勢の設定値を設定する設定値設定手段と、
   前記設定値、前記主観視点点指標の世界座標系における位置、および前記主観視点撮像手段の焦点距離に基づいて、前記主観視点画像の前記主観視点点指標の画像座標の主観座標推定値を算出する主観推定値算出手段と、
   前記設定値、前記客観視点点指標の前記主観視点撮像手段に対する位置、前記客観視点位置姿勢、および前記客観視点撮像手段の焦点距離に基づいて、前記客観視点画像の前記客観視点点指標の画像座標の客観座標推定値を算出する客観推定値算出手段と、
   前記検出された主観視点点指標の画像座標と前記主観座標推定値との主観誤差を算出する主観画像誤差算出手段と、
   前記検出された客観視点点指標の画像座標と前記客観座標推定値との客観誤差を算出する客観画像誤差算出手段と、
   前記算出された主観誤差および客観誤差に基づいて、前記主観視点撮像手段の位置および姿勢の方位角成分のみの補正値を算出する補正値算出手段と、
   前記算出された補正値および前記設定値に基づいて、前記主観視点撮像手段の位置姿勢を補正する補正手段と
   を備えることを特徴とする情報処理装置。
2. 前記位置姿勢算出手段が、
   前記誤差もしくは前記補正値が予め設定された閾値よりも小さいか否かを判定する判定手段
   を更に有し、
   前記設定値設定手段が、前記誤差もしくは前記補正値が前記閾値よりも小さくない場合に、前記補正された前記主観視点撮像手段の位置姿勢を前記設定値として再設定することを特徴とする請求項１に記載の情報処理装置。
3. 前記主観視点撮像手段は、現実空間に存在する計測対象物体に装着されており、
   前記保持手段が、前記主観視点撮像手段と前記計測対象物体との相対位置姿勢を更に保持し、
   前記位置姿勢算出手段が、算出した前記主観視点撮像装置の位置姿勢、および前記相対位置姿勢に基づいて、前記計測対象物体の位置姿勢を算出することを特徴とする請求項１または２に記載の情報処理装置。
4. 情報処理装置が行う位置姿勢計測方法であって、
   前記情報処理装置が有する主観視点撮像手段が、現実空間に配置された主観視点点指標を撮像した主観視点画像を入力する工程と、
   前記情報処理装置が有する客観視点撮像手段が、前記主観視点撮像手段に伴って配置された客観視点点指標を、前記現実空間の世界座標系において固定されて既知である客観視点位置姿勢から撮像した客観視点画像を入力する客観視点撮像工程と、
   前記情報処理装置が有する姿勢計測手段が、前記主観視点撮像装置に備えられ、前記主観視点撮像手段の姿勢計測値を計測する姿勢計測工程と、
   前記情報処理装置が有する姿勢予測手段が、前記姿勢計測値と予め保持された前記姿勢計測手段の方位ドリフト誤差補正値に基づいて、前記主観視点撮像手段の姿勢予測値を算出する姿勢予測工程と、
   前記情報処理装置が有する第１の検出手段が、前記主観視点画像から少なくとも１点の前記主観視点点指標の画像座標を検出する第１の検出工程と、
   前記情報処理装置が有する第２の検出手段が、前記客観視点画像から少なくとも１点の前記主観視点点指標の画像座標を検出する第２の検出工程と、
   前記情報処理装置が有する位置姿勢算出手段が、前記客観視点位置姿勢、前記姿勢予測値、前記検出された主観視点点指標の画像座標、当該主観視点点指標の世界座標系における位置、前記検出された客観視点点指標の画像座標、および当該客観視点点指標の前記主観視点撮像手段に対する位置に基づいて、前記主観視点撮像装置の位置姿勢を算出する位置姿勢算出工程と、
   前記情報処理装置が有する方位誤差更新手段が、前記算出された前記主観視点撮像装置の位置姿勢に基づいて、前記保持された方位ドリフト誤差補正値を更新する方位誤差更新工程と
   を有し、
   前記位置姿勢算出工程では、
   前記位置姿勢算出手段の設定値設定手段が、前記主観視点撮像手段の位置姿勢の設定値を設定する設定値設定工程と、
   前記位置姿勢算出手段の主観推定値手段が、前記設定値、前記主観視点点指標の世界座標系における位置、および予め保持された前記主観視点撮像手段の焦点距離に基づいて、前記主観視点画像の前記主観視点点指標の画像座標の主観座標推定値を算出する主観推定値算出工程と、
   前記位置姿勢算出手段の客観推定値手段が、前記設定値、前記客観視点点指標の前記主観視点撮像手段に対する位置、前記客観視点位置姿勢、および予め保持された前記客観視点撮像手段の焦点距離に基づいて、前記客観視点画像の前記客観視点点指標の画像座標の客観座標推定値を算出する客観推定値算出工程と、
   前記位置姿勢算出手段の主観画像誤差算出手段が、前記検出された主観視点点指標の画像座標と前記主観座標推定値との主観誤差を算出する主観画像誤差算出工程と、
   前記位置姿勢算出手段の客観画像誤差算出手段が、前記検出された客観視点点指標の画像座標と前記客観座標推定値との客観誤差を算出する客観画像誤差算出工程と、
   前記位置姿勢算出手段の補正値算出手段が、前記算出された主観誤差および客観誤差に基づいて、前記主観視点撮像手段の位置および姿勢の方位角成分のみの補正値を算出する補正値算出工程と、
   前記位置姿勢算出手段の補正手段が、前記算出された補正値および前記設定値に基づいて、前記主観視点撮像手段の位置姿勢を補正する補正工程と、
   を有することを特徴とする位置姿勢計測方法。
5. 請求項４に記載の位置姿勢計測方法の各工程をコンピュータに実行させるためのプログラム。
6. 請求項５に記載のプログラムを格納するコンピュータ読み取り可能な記録媒体。

Images