JP5132138B2 - 位置姿勢計測方法、位置姿勢計測装置 - Google Patents

Description

本発明は、視点位置姿勢計測の精度向上と安定性を向上させる技術に関する。

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

複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置の位置姿勢と称する）を求める問題に帰結される。

この問題を解決する方法として次のような試みがなされている（非特許文献１を参照）。先ず、環境中や対象物体上に、対象の座標系における配置が既知であるような複数の指標を設置あるいは設定する。そして、既知の情報であるその指標の対象座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、対象座標系に対する撮像装置の位置姿勢を求める。

また、撮像装置に慣性センサを装着し、この慣性センサのセンサ計測値を利用することにより、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている。例えば、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用する方法が提案されている。同推定結果を画像に基づく位置及び姿勢算出の初期値として用いる方法も提案されている。さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いる方法も提案されている（特許文献１、非特許文献２を参照）。

従来の画像情報を用いた位置合わせ手法は、指標の検出結果がすべて正しいことを前提としていた。さらに、指標検出結果はすべて均等な重みとして計算されていた。そのため、誤検出や検出精度の低い指標による影響を大きく受け、正しい位置姿勢計測が行えない場面が起こり得る。そこで近年、Ｍ推定のような統計的推定手法を導入し、検出した指標（特徴点）の画像上での観察座標と撮像装置の位置姿勢や指標の位置から推定した指標の画像座標（再投影座標）との間の誤差（再投影誤差）を求める。そして、この誤差をもとに検出指標の信頼度を算出し、誤検出指標を排除することや、その影響を下げる手法が提案されている（非特許文献３を参照）。
佐藤, 田村："複合現実感における位置合わせ手法", 画像の認識・理解シンポジウム(MIRU 2002)論文集I, 情報処理学会シンポジウムシリーズ, vol.2002, no.11, pp.I.61-I.68, July 2002 藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告PRMU99-192（信学技報vol.99, no.574, pp.1-8） 佐藤，神原，横矢，竹村："マーカと自然特徴点の追跡による動画像からのカメラ移動パラメータの復元"，電子情報通信学会論文誌，Ｄ−II，ｖｏｌ．Ｊ８６−Ｄ−II，ｎｏ．１０，ｐｐ．１４３１−１４４０，２００３． 特開２００５−３３３１９号公報

しかし、再投影誤差に基づいて信頼度を算出し、指標に重み付けすることで誤検出を排除する手法は、統計的に例外値を求めることで誤検出指標と判定する仕組みである。そのため、画像上で検出される指標の検出誤差はガウス分布等の誤差モデルにあてはまると仮定し、それを基に確からしく検出されている指標が多数ある場合にしか成立しない。したがって、指標が少ない場合などでは誤検出の影響を受けやすく、誤検出排除手法として十分とはいえない。また、例えば照明条件等により指標の検出結果が小刻みに変わる（あるフレームでは検出されていた指標が次のフレームでは検出されない）ような状況では、フレーム間で位置姿勢計測結果の不整合が生じてしまい、安定した位置姿勢計測を行うことができない。

本発明は以上の問題に鑑みてなされたものであり、誤検出指標や検出精度の低い指標による影響を減少させ、位置姿勢計測を高精度かつ安定して行う為の技術を提供することを目的とする。

本発明の目的は、例えば、以下の位置姿勢計測装置によって達成される。

すなわち、位置が既知である複数の指標が配された現実空間を撮像装置によって撮像した動画像を入力する入力手段と、前記入力された動画像に含まれる各フレームに映る前記指標の画像座標を検出する検出手段と、現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに連続で検出された回数が多い指標ほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する算出手段とを有することを特徴とする位置姿勢計測装置。

本発明の構成により、誤検出指標や検出精度の低い指標の影響を下げ、高精度かつより安定な撮像装置の位置姿勢計測を行うことができる。

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

［第１の実施形態］
本実施形態では、複数フレームの指標検出状況によって各指標の信頼度を変えて重み付けを行い、誤検出指標や精度の低い指標の排除または影響を下げ、位置合わせの精度を向上させる。以下、本実施形態に係る指標の検出状況に応じた重み付けによる撮像装置の位置姿勢測定方法について説明する。

図１は、本実施形態に係る撮像装置の位置姿勢計測を行う位置姿勢計測装置２０００の機能構成を示すブロック図である。

位置姿勢計測装置２０００は、画像入力部２０２０、指標検出部２０３０、指標検出状況取得部２０４０、信頼度算出部２０５０、撮像装置初期位置姿勢入力部２０６０、データ管理部２０７０、位置姿勢算出部２０８０によって構成されている。また、画像入力部２０２０には、３次元現実空間の動画像を撮像する撮像装置２０１０が接続されている。

本実施形態では、画像入力部２０２０、指標検出部２０３０、指標検出状況取得部２０４０、信頼度算出部２０５０、撮像装置初期位置姿勢入力部２０６０、データ管理部２０７０、位置姿勢算出部２０８０の各部は、ハードウェアで構成されているものとする。従ってこれら各部によっては、ＣＰＵなどの制御部やメモリなどが搭載されている。

また、３次元現実空間中の背景、若しくは同空間中に配置されている対象物体上には、指標が複数配置されている。ここで、物体に配置された指標をＰｋ（ｋ＝１，，，Ｋo）で表す。但し、Ｋoは配置された指標の数（図３（ａ）の例ではＫo＝３）である。

図３は、本実施形態で用いる指標を説明する図である。図３（ｂ）に示すように指標Ｐｋは頂点ｐ^ｋｉ（ｉ＝１，，，Ｎｋ）により構成される。但し、Ｎｋは、指標Ｐｋを構成する頂点の総数（本実施形態の場合はＮｋ＝４）を表す。

以下では、計測の基準とする座標系（ここでは３次元現実空間中の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）を世界座標系と呼称する。また、世界座標系における各指標の位置姿勢は既知であるとする。また、３次元現実空間中に存在する夫々の指標を構成する各頂点は、世界座標系において位置が既知であるとする。

指標Ｐｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各頂点が識別可能であるような指標であれば、何れの形態であってもよい。例えば、図３（ｂ）に示すような四角形指標は内部に自身に固有の識別子を表すパターンを持ち、一意に同定可能である。

このような指標を検出する際には、撮影画像に２値化処理を施した後にラベリング処理を行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として抽出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって候補領域が指標領域であるかを判定し、内部のパターンを読み出すことによって指標の方向と識別子を取得する。

以下、位置姿勢計測装置２０００を構成する各部、及び撮像装置２０１０の動作について説明する。

上述の通り撮像装置２０１０は３次元現実空間の動画像を撮像する。撮像した各フレームの画像（現実空間画像）は、画像入力部２０２０を介して位置姿勢計測装置２０００に入力され、指標検出部２０３０に送信される。

指標検出部２０３０は、画像入力部２０２０から受けた画像中に撮影されている指標Ｐｋを構成する各頂点ｐ^ｋｉの画像座標を検出する。さらに、指標検出部２０３０は、検出された各々の指標Ｐ^ｋnの識別（同定）を行う。そして、指標の識別子ｋnと各頂点p^ｋnｉの画像座標ｕ^Ｐｋnｉとを、指標検出部２０３０による検出結果として、データ管理部２０７０及び指標検出状況取得部２０４０に出力する。

ここで、n（n＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。また、Ｎ個の指標から成る構成点（頂点）の総数をＮ_Totalとする。例えば図１の場合は、識別子が１、２、３である四角形形状の指標（Ｐ^１，Ｐ^２，Ｐ^３）が撮影される場合を示している。従って、Ｎ＝３であり、識別子ｋ１＝１、ｋ２＝２、ｋ３＝３とこれらに対応する画像座標ｕ^Pｋ１ｉ、ｕ^Pｋ２ｉ、ｕ^Pｋ３ｉ（ｉ＝１、２、３、４）が指標検出部２０３０から出力される。Ｎ_Totalは、１２（＝３×４）になる。

指標検出状況取得部２０４０は、指標検出部２０３０から入力した各フレームにおける指標の検出結果を蓄積・管理する。そして、現在のフレーム（最新フレームであり、以下、第ｆフレームと呼称する）で検出されているそれぞれの指標の検出状況Ｖ^Ｐｋｎを信頼度算出部２０５０に出力する。

例えば、第ｆフレームで検出されている夫々の指標について、過去のフレームから連続して検出されているフレーム数（連続検出フレーム数）a^Ｐｋｎを、指標Ｐ^ｋnの検出状況Ｖ^Ｐｋｎとして出力する。本実施形態では、連続検出フレーム数に上限を設け、ある一定フレーム数（例えば１秒間であれば３０フレームであり、以下ではｈフレームとする）以上はカウントしないものとする。

信頼度算出部２０５０は、指標検出状況取得部２０４０が求めた夫々の指標の検出状況Ｖ^Ｐｋｎに基づいて夫々の指標の信頼度ω^Ｐｋｎを算出し、算出した信頼度ω^Ｐｋｎをデータ管理部２０７０に出力する。

ここで、信頼度算出部２０５０における信頼度算出について説明する。指標の信頼度ω^Ｐｋｎは、検出状況Ｖ^Ｐｋｎを引数とする重み関数

によって算出する。重み関数ω（Ｖ^Ｐｋｎ）は、正（＞０）であり、ロバスト推定手法のひとつであるＭ推定で用いられるような重み関数（確率論的モデルを作り、観測データとよくあてはまるものにより大きな重み付けを行う関数）であってもよい。また、実験的、経験的に求めた関数であってもよい。

本実施形態では、指標ごとの連続検出フレーム数a^Ｐｋｎを、この指標の検出状況Ｖ^Ｐｋｎと定義している。「より連続したフレームで検出されている指標ほど、信頼できる指標である」という基準を考え、より連続して検出された指標の信頼度を高くするような重み関数を定義する。例えば、それぞれの指標ごとの連続検出フレーム数a^Ｐｋｎと、すべての指標の中で最も多く検出された指標の連続検出フレーム数a_max ^Ｐｋｎにより、信頼度ω^Ｐｋｎを次式のように求める。

または、次式のように求めてもよい。

また、指標ごとの連続検出フレーム数を反映する信頼度であれば、上記以外の関数を用いてもよい。

撮像装置初期位置姿勢入力部２０６０は、世界座標系における撮像装置２０１０の位置姿勢の初期値Ｍ_ＷＣを求め、これをデータ管理部２０７０に出力する。本実施形態では、検出された夫々の指標の信頼度、各頂点の画像座標、各頂点の世界座標系における位置をデータ管理部２０７０から入力し、信頼度が閾値以上の指標の情報のみを用いて撮像装置２０１０の位置姿勢を算出する。

複数の点の画像座標と世界座標系における位置との対応から撮像装置の位置と姿勢とを求める方法（一般にＰｎＰ問題と呼ばれる）は、写真測量やコンピュータビジョンの分野で広く公知であるので、その詳細な説明は省略する。

なお、初期位置姿勢の取得方法はこれに限定するものではなく、例えば、１フレーム前の処理において位置姿勢算出部２０８０によって算出された位置姿勢を保持しておき、これを初期位置姿勢として出力しても良い。また、６自由度位置姿勢センサを撮像装置に装着させてその出力値として得られる概略の位置姿勢を初期位置姿勢として用いてもよい。また、３自由度姿勢センサを撮像装置に装着させることによって得られる撮像装置の姿勢と信頼度が閾値以上となる指標を用いて初期位置姿勢を求めてもよい。あるいは、その他の何れかの公知技術を用いて撮像装置の概略位置姿勢を初期位置姿勢として求めても良い。

ここで、ある三次元座標系Bに対する三次元の座標系Aの位置を表す三次元ベクトルをｔ、姿勢を表す3×3回転行列をＲとする。この場合、A上での位置がｘ_A（三次元ベクトル）で表される点のB上での座標ｘ_B（三次元ベクトル）は、以下に示す４×４行列Ｍ_ＢＡを用いて同次座標表現によって以下の式５のように表される。

本実施形態では、Ｍ_ＢＡを座標系Ｂに対する座標系Ａの位置姿勢を表す手段として用いる。

データ管理部２０７０は、予め求めておいた既知の情報である世界座標系における夫々の指標Ｐ^ｋnの位置姿勢Ｍ_ＷＭや、各頂点の世界座標系における位置（世界座標）を保持している。また、指標検出部２０３０によって検出された各々の指標Ｐ^ｋnの識別子ｋnと各頂点p^ｋnｉの画像座標ｕ^Ｐｋnｉとを指標検出部２０３０から入力すると共に、夫々の指標の信頼度を信頼度算出部２０５０から入力する。また、撮像装置初期位置姿勢入力部２０６０が求めた世界座標系における撮像装置２０１０の位置姿勢の初期値Ｍ_ＷＣをも入力する。そしてこれら入力されたデータを保持し、必要に応じて適宜出力する。

位置姿勢算出部２０８０は、データ管理部２０７０から、あるフレームにおける撮像装置２０１０の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標を取得する。そして取得したこれらを用いて撮像装置２０１０の位置姿勢算出処理を行い、その結果として得られた撮像装置２０１０の位置姿勢情報（即ち、世界座標系における撮像装置２０１０の位置と姿勢）を出力する。

図２は、位置姿勢計測装置２０００が行う撮像装置２０１０の位置姿勢算出処理のフローチャートである。

先ずステップＳ６０１０において指標検出部２０３０は、撮像装置２０１０から画像入力部２０２０を介して入力した最新の画像（第ｆフレーム）から指標の検出処理を行う。そして検出結果（検出された各指標の識別子ｋnと各頂点の画像座標群ｕ^Ｐｋnｉ）を、データ管理部２０７０および指標検出状況取得部２０４０に出力する。指標検出部２０３０は、時系列に入力される画像の夫々に対して、常にこの検出処理を実行する。

次にステップＳ６０２０において指標検出状況取得部２０４０は、指標検出部２０３０から検出結果を入力し、夫々の検出指標について第ｆフレームよりも過去のフレームからの検出状況を算出し、算出した検出状況を信頼度算出部２０５０に出力する。検出状況の算出は、例えば、夫々の指標についてカウンタを用意し、現在のフレームで検出されていれば（上限ｈを超えない範囲で）カウンタを１増加させ、検出されていなければカウンタを０にリセットするという処理によって実現できる。

次にステップＳ６０３０において信頼度算出部２０５０は、指標検出状況取得部２０４０から夫々の検出指標の検出状況を入力し、上記重み関数を用いてそれぞれの指標の信頼度を算出する。それぞれの指標について算出した信頼度をデータ管理部２０７０に出力する。

次にステップＳ６０４０において撮像装置初期位置姿勢入力部２０６０は、第ｆフレームにおける世界座標系における撮像装置２０１０の初期位置姿勢Ｍ_ＷＣを算出する。そして算出した初期位置姿勢Ｍ_ＷＣをデータ管理部２０７０に出力する。

次にステップＳ６０５０において、位置姿勢算出部２０８０は、データ管理部２０７０から第ｆフレームにおける撮像装置２０１０の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標を取得する。そして取得したこれらを用いて、撮像装置２０１０の位置姿勢算出処理を行い、その結果として得られた撮像装置２０１０の位置姿勢情報を出力する。本ステップにおける処理の詳細については図４を用いて後述する。

最後にステップＳ６０６０では、位置姿勢算出処理を終了するか否かの判定が行われる。オペレータが、位置姿勢計測装置２０００に対して、位置姿勢算出処理の終了を指示した場合には、本処理を終了させる。一方、位置姿勢算出処理の継続を指示した場合には、再びステップＳ６０１０へと戻り、次フレーム（第（ｆ＋１）フレーム）の画像に対する処理が実行される。

図４は、上記ステップＳ６０５０における処理の詳細を示すフローチャートである。

先ずステップＳ４０１０において位置姿勢算出部２０８０は、第ｆフレームにおける撮像装置２０１０の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標を、データ管理部２０７０から取得する。

次にステップＳ４０２０において位置姿勢算出部２０８０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力された指標の頂点の総数Ｎ_Totalの数が３点以上の場合はステップＳ４０３０に処理を進め、３点未満の場合はステップＳ４０９０に処理を進める。例えば、四角形の４頂点で構成される指標１つであればＮ_Totalは４、三角形の３頂点で構成される指標２つならばＮ_Totalは６となる。

ここで、位置姿勢算出部２０８０は、世界座標系（あるいは任意の座標系）における撮像装置２０１０の位置姿勢を、求めるべき未知パラメータとして扱う。ここで、姿勢の表現方法として、３値のベクトルα＝［ξ φ ζ］^Ｔを用いる。αは回転軸・回転角による姿勢の表現方法であり、回転行列Ｒは次式に示すようなαの関数となる。

このとき、算出すべき撮像装置２０１０の位置姿勢は、位置ｔ=[x y z]^Ｔ及び姿勢α＝［ξ φ ζ］^Ｔによって表現され、求める未知パラメータは、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ φ ζ］^Ｔと記述される。

次にステップＳ４０３０において位置姿勢算出部２０８０は、各々の指標の頂点ｐ^ｋｎｉに対して、その画像座標の推定値（再投影座標）u^{Ｐｋｎｉ’}を算出する。u^{Ｐｋｎｉ’}の算出は、指標の頂点ｐ^ｋｎｉの世界座標ｘ_ｗ ^Ｐｋｎｉと現在の状態ベクトルｓの関数（観測方程式）

に基づいて行われる。

具体的には、関数Ｆ_ｃ（）は、ｘ_ｗ ^Ｐｋｎｉからカメラ座標における位置ベクトルｘ_Ｃ ^Ｐｋnｉを求める次式

及び、ｘ_Ｃ ^Ｐｋnｉから画像上の座標ｕ^{Ｐｋnｉ’}を求める次式

によって構成されている。ここでｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置２０１０の焦点距離であり、既知の値として予めデータ管理部２０７０によって保持されているものとする。

次にステップＳ４０４０において位置姿勢算出部２０８０は、各々の指標の頂点ｐ^ｋnｉの実測値ｕ^Ｐｋnｉと、それに対応する画像座標の計算値ｕ^{Ｐｋnｉ’}との誤差（再投影誤差）Δｕ^Ｐｋnｉを次式によって算出する。

次にステップＳ４０５０において位置姿勢算出部２０８０は、各々の指標の頂点ｐ^ｋnｉに対して、状態ベクトルｓに関する画像ヤコビアンＪ_ｕｓ ^Ｐｋnｉ（＝∂ｕ^Ｐｋnｉ／∂ｓ）を求める。係る画像ヤコビアンとは即ち、式７の関数Ｆｃ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列のことである。

具体的には、式９の右辺をカメラ座標ｘ_Ｃ ^Ｐｋnｉの各要素で変微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｐｋnｉ（＝∂ｕ^Ｐｋnｉ／∂ｘ）と、式８の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列Ｊ_ｘｓ ^Ｐｋnｉ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＰｋnｉを算出する。

次にステップＳ４０６０において位置姿勢算出部２０８０は、各指標の頂点の誤差Δｕ^Ｐｋnｉ、画像ヤコビアンＪ_ｕｓ ^Ｐｋnｉ、各指標の信頼度ω^Ｐｋｎに基づいて、状態ベクトルｓの補正値Δｓを算出する。以下、補正値Δｓの算出処理の詳細について説明する。

はじめに、各指標の頂点における再投影誤差を垂直に並べた２Ｎ_Total次元の誤差ベクトルＵを作成する。

また、各指標の頂点におけるヤコビ行列Ｊ_ｕｓ ^Ｐｋnｉを垂直に並べた（２Ｎ_Total）行×６列の行列Φを作成する。

次に、指標Ｐ^ｋｎの各頂点ｐ^ｋnｉに対応する要素（頂点毎にｘ座標とｙ座標の２要素を有する）に対して、この指標の信頼度ω^Ｐｋｎを対角成分に持つような、２Ｎ_Total行×２Ｎ_Total列の対角行列Ｗを作成する。

行列Ｗを重みとした最小二乗法によってΔｓを求めることを考えると、次の正規方程式

が得られるので、Δｓを次式より算出する。

このように、指標の検出の連続性を信頼度として表す行列Ｗを重みとしてΔｓの算出に利用することで、検出の連続性に応じて各指標のΔｓの算出への寄与度が変化するという効果を得ることができる。すなわち、過去のフレームから連続的に検出されている指標（より信頼できる指標である可能性が高い）をΔｓの算出に積極的に利用し、連続性の乏しい指標（誤検出指標である可能性が高い）から受ける悪影響を軽減するという効果を得ることができる。

なお、Ｍ推定のような統計的推定手法によって指標の再投影誤差Δｕ^Ｐｋnｉに応じた重みを求め、ω^Ｐｋｎとの積を得てこれを重みとして用いてもよい。

ここで、Δｓは６次元ベクトルであるから、２Ｍ_Totalが６以上であれば、Δｓを求めることができる。なお、本実施形態では式１６に示すようにΔｓを求めているが、他の方法によって求めてもよい。例えば、式１５は連立一次方程式であるので、ΔｓをＧａｕｓｓの消去法で解いてもよいし、その他のいずれの手法で解いてもよい。

次にステップＳ４０７０において位置姿勢算出部２０８０は、ステップＳ４０６０で算出した補正値Δｓを用いて、以下の式１７に従ってｓを補正し、得られた値を新たなｓとする。

なお、状態ベクトルｓの初期値としては、撮像装置初期位置姿勢入力部２０６０が求めた「世界座標系における撮像装置２０１０の位置姿勢の初期値Ｍ_ＷＣ」を用いる。

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

一方、ステップＳ４０８０において計算が収束したと判定されると、ステップＳ４０９０において位置姿勢算出部２０８０は、世界座標系における撮像装置２０１０の位置及び姿勢ｓを出力する。この時の出力の形態は、ｓそのものであっても良いし、ｓの位置成分を3値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓから生成した座標変換行列Ｍであっても良い。

以上の説明により、世界座標系に対する（すなわち、世界座標系における）撮像装置２０１０の位置及び姿勢、若しくは位置のみを取得することができる。

また、本実施形態によれば、指標の検出の連続性を考慮して位置姿勢推定を行うので、誤検出指標（連続フレーム間で同一の指標として検出されない場合が多い）の影響を軽減し、撮像装置２０１０の位置姿勢計測を安定して行うことができる。

＜変形例１＞
上記説明では、信頼度ω^Ｐｋｎは、過去のフレームから現フレームまでに連続して多く検出された指標に信頼度を高くするように求めていた。しかし、複数フレームにおける指標の検出状況Ｖ^Ｐｋｎに応じて信頼度を求めるのであれば、検出情報の種類はこれに限定されるものではないし、信頼度の算出方法はその他の方法でもよい。

例えば、現フレームから遡って過去ｈフレームにおける指標の検出回数b^Ｐｋｎを検出状況Ｖ^Ｐｋｎ _freqとし、それに基づいて信頼度ω^Ｐｋｎを求めてもよい。この場合は、検出回数が多い指標に信頼度を高く、検出回数の少ない指標に小さな信頼度を与えるような関数ω_freq（Ｖ^Ｐｋｎ _freq）を重み関数として定義する。例えば、それぞれの指標における検出回数b^Ｐｋｎと、検出された指標のうち最も多く検出された指標の最大検出回数b_max ^Ｐｋｎによって、信頼度を

として算出してもよい。または、

として算出してもよい。指標の検出回数に基づく信頼度であれば上記以外の計算方法で求めてもよい。また、このようにして得られた過去ｈフレームにおける指標の検出回数に基づく信頼度をそのまま信頼度ω^Ｐｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得るようにしても良い。

＜変形例２＞
上記変形例１では、過去ｈフレームにおける指標の検出回数b^Ｐｋｎに基づいて信頼度を算出していた。しかし、複数フレームにおける指標の検出状況Ｖ^Ｐｋｎに応じて信頼度を求めるのであれば、検出回数b^Ｐｋｎを過去ｈフレームにおける連続性と検出回数の両方に基づいて求め、信頼度を計算しても良い。

例えば、現在のフレームからある過去フレームまでの間で一定回数以上連続して検出された検出回数の合計を検出回数b^Ｐｋｎとする。ここで求めた検出回数b^Ｐｋｎを検出状況Ｖ^Ｐｋｎとし、それに基づいて信頼度ω^Ｐｋｎを求めてもよい。この場合は、一定回数以上連続して検出された指標の検出回数の合計が多い指標に信頼度を高く、一定回数以上連続して検出された指標の検出回数の合計が少ない指標に小さな信頼度を与えるような関数ω_sum（Ｖ_sum ^Ｐｋｎ）を重み関数として定義する。例えば、それぞれの指標における検出回数b^Ｐｋｎと、検出された指標のうち最も多く検出された指標の最大検出回数b_max ^Ｐｋｎによって、信頼度を

として算出してもよい。または、

として算出してもよい。また、このようにして得られた現在のフレームからある過去フレームまでの間で一定回数以上連続して検出された検出回数の合計に基づく信頼度をそのまま信頼度ω^Ｐｋｎとして用いるだけではなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得てもよい。

＜変形例３＞
また、上記説明では、図３（ｂ）に示すような複数の頂点を有する指標を用いた。しかし、それぞれが異なる色を有する円形状や球状のマーカを指標として用いる場合や、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点を指標として用いる場合であっても、第１の実施形態と同様の効果を奏することができる。即ち、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、指標は何れの形態であってもよい。

円形状や球状のマーカや自然特徴を用いる場合には、指標の情報は世界座標系（基準座標系）における位置ｘ_ｗ ^Ｐｋｎ＝［ｘ_ｗ、ｙ_ｗ、ｚ_ｗ］で表され、第１の実施形態における指標の頂点と同様に扱うことができる。また、これらの指標（多角形形状の指標、円形状の指標、自然特徴等）を混在して用いる場合であっても、第１の実施形態と同様の効果を得ることが可能であることは、いうまでもない。

＜変形例４＞
上記説明では、最適化演算において非線形方程式をテーラー展開し、１次近似で線形化することで補正値を求め、繰り返し補正していくことで最適解を求めるＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法を用いている。しかし、補正値の算出は必ずしもＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、最急降下法を用いても求めてもよい。即ち、他の何れの数値計算手法を適用しても良い。

［第２の実施形態］
第１の実施形態では、位置姿勢計測装置２０００を構成する各部はハードウェアでもって構成されているものとして説明した。しかし、これらの各部のうち一部若しくは全部をソフトウェアでもって構成し、残りを例えば画像処理ボードなどのハードウェアでもって構成するようにしても良い。この場合、このソフトウェアは、一般のＰＣ等のコンピュータにインストールされる。そして、このコンピュータが有するＣＰＵがこのインストールされたプログラムを実行することで、このコンピュータは、位置姿勢計測装置２０００を構成する各部が有する機能を実現することになる。

図５は、係るコンピュータのハードウェア構成を示すブロック図である。

ＣＰＵ５０１は、ＲＡＭ５０２やＲＯＭ５０３に格納されているプログラムやデータを用いて本コンピュータ全体の制御を行う。更に、本コンピュータを位置姿勢計測装置２０００に適用した場合には、位置姿勢計測装置２０００が行うものとして上述した各処理（図２，４に示したフローチャートに従った処理）を実行する。

ＲＡＭ５０２には、外部記憶装置５０６からロードされたプログラムやデータ、Ｉ／Ｆ（インターフェース）５０７を介して撮像装置２０１０から受信した各フレームの画像を一時的に記憶するためのエリアを有する。また、ＲＡＭ５０２は、ＣＰＵ５０１が各処理を実行する際に用いるワークエリアも有する。即ち、ＲＡＭ５０２は、各種のエリアを適宜提供することができる。

ＲＯＭ５０３には、本コンピュータの設定データや、ブートプログラムなどが格納されている。

操作部５０４は、キーボードやマウスなどにより構成されており、本コンピュータの操作者が操作することで、各種の指示をＣＰＵ５０１に対して入力することができる。

表示部５０５は、ＣＲＴや液晶画面等により構成されており、ＣＰＵ５０１による処理結果を画像や文字などでもって表示することができる。

外部記憶装置５０６は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。ここには、ＯＳ（オペレーティングシステム）や、位置姿勢計測装置２０００を構成する各部（データ管理部２０７０を除く）が有する機能をＣＰＵ５０１に実行させるためのプログラムやデータ等が保存されている。また、データ管理部２０７０が管理しているものとして説明した情報や、既知の情報として説明したものについてもこの外部記憶装置５０６に保存されている。外部記憶装置５０６に保存されているプログラムやデータは、ＣＰＵ５０１による制御に従って適宜ＲＡＭ５０２にロードされ、ＣＰＵ５０１による処理対象となる。

Ｉ／Ｆ５０７は、上記撮像装置２０１０を本コンピュータに接続するためのもので、撮像装置２０１０が撮像した各フレームの画像は、このＩ／Ｆ５０７を介して外部記憶装置５０６やＲＡＭ５０２に送出される。

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

［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。

また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

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

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

本発明の第１の実施形態に係る撮像装置の位置姿勢計測を行う位置姿勢計測装置２０００の機能構成を示すブロック図である。 位置姿勢計測装置２０００が行う撮像装置２０１０の位置姿勢算出処理のフローチャートである。 本発明の第１の実施形態で用いる指標を説明する図である。 ステップＳ６０５０における処理の詳細を示すフローチャートである。 コンピュータのハードウェア構成を示すブロック図である。

Claims (13)

1. 位置が既知である複数の指標が配された現実空間を撮像装置によって撮像した動画像を入力する入力手段と、
   前記入力された動画像に含まれる各フレームに映る前記指標の画像座標を検出する検出手段と、
   現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに連続で検出された回数が多い指標ほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する算出手段と
   を有することを特徴とする位置姿勢計測装置。
2. 前記複数の指標ごとにカウンタを有し、前記過去のフレームから前記現在のフレームへと当該指標が検出されるたびに当該指標に対応するカウンタのカウント値をカウントアップし、前記過去のフレームから前記現在のフレームへと当該指標が検出されないごとにカウンタのカウント値をリセットし、
   前記算出手段が、前記各指標に対応するカウンタのカウント値が高いほど、当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出することを特徴とする請求項１に記載の位置姿勢計測装置。
3. 前記算出手段が、
   前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに、連続で検出された回数が多い指標ほど信頼度が高くなるように、該現在のフレームから検出された各指標の信頼度を算出する信頼度算出手段と、
   前記現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記算出された信頼度が高いほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する位置姿勢算出手段と
   を有することを特徴とする請求項１又は２に記載の位置姿勢計測装置。
4. 前記現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに連続で検出された回数が多い、かつ、累積で検出された回数が多い指標ほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出することを特徴とする請求項１又は２に記載の位置姿勢計測装置。
5. 前記算出手段が、
   前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに、連続で検出された回数が多い、かつ、累積で検出された回数が多い指標ほど信頼度が高くなるように、該現在のフレームから検出された各指標の信頼度を算出する信頼度算出手段と、
   前記現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記算出された信頼度が高いほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する位置姿勢算出手段と
   を有することを特徴とする請求項４に記載の位置姿勢計測装置。
6. 位置が既知である複数の指標が配された現実空間を撮像装置によって撮像した動画像を入力する入力手段と、
   前記入力された動画像に含まれる各フレームに映る前記指標の画像座標を検出する検出手段と、
   現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに累積で検出された回数が多い指標ほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する算出手段と
   を有することを特徴とする位置姿勢計測装置。
7. 前記算出手段が、
   前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに、累積で検出された回数が多い指標ほど信頼度が高くなるように、該現在のフレームから検出された各指標の信頼度を算出する信頼度算出手段と、
   前記現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記算出された信頼度が高いほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する位置姿勢算出手段と
   を有することを特徴とする請求項６に記載の位置姿勢計測装置。
8. 前記位置姿勢算出手段が、
   前記撮像装置の位置姿勢の初期値を入力する初期値入力手段と、
   前記初期値と前記画像座標を検出された指標の既知である位置とに基づいて、前記各指標の前記現在のフレームの面における画像座標を算出する座標算出手段と、
   前記検出手段で検出された指標の画像座標と前記座標算出手段で算出された画像座標との誤差に、前記算出された信頼度が高いほど重い重み付けを与えて、前記撮像装置の位置姿勢の補正値を算出する補正値算出手段と、
   前記算出された補正値に基づいて、前記初期値を補正する補正手段と
   を有することを特徴とする請求項３、５、又は７に記載の位置姿勢計測装置。
9. 前記補正値算出手段が、画像ヤコビアンを用いて、前記補正値を算出することを特徴とする請求項８に記載の位置姿勢計測装置。
10. 請求項１乃至９の何れか１項に記載の位置姿勢計測装置の各手段としてコンピュータを機能させるためのプログラム。
11. 請求項１乃至９の何れか１項に記載の位置姿勢計測装置の各手段としてコンピュータを機能させるためのプログラムを格納した、コンピュータ読み取り可能な記憶媒体。
12. 位置姿勢計測装置の入力手段が、位置が既知である複数の指標が配された現実空間を撮像装置によって撮像した動画像を入力する入力工程と、
    前記位置姿勢計測装置の検出手段が、前記入力された動画像に含まれる各フレームに映る前記指標の画像座標を検出する検出工程と、
    前記位置姿勢計測装置の算出手段が、現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに連続で検出された回数が多い指標ほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する算出工程と
    を有することを特徴とする位置姿勢計測方法。
13. 位置姿勢計測装置の入力手段が、位置が既知である複数の指標が配された現実空間を撮像装置によって撮像した動画像を入力する入力工程と、
    前記位置姿勢計測装置の検出手段が、前記入力された動画像に含まれる各フレームに映る前記指標の画像座標を検出する検出工程と、
    前記位置姿勢計測装置の算出手段が、現在のフレームから検出された指標の画像座標および当該指標の既知である位置に基づいて、前記動画像に含まれる前記現在のフレームよりも過去のフレームから該現在のフレームまでに累積で検出された回数が多い指標ほど当該指標に対応する重み付けを重くして、前記撮像装置の位置姿勢を算出する算出工程と
    を有することを特徴とする位置姿勢計測方法。

Images