JP6584208B2 - 情報処理装置、情報処理方法、プログラム - Google Patents

Description

本発明は撮像装置の位置及び姿勢を計測する方法に関する。

画像情報に基づく撮像装置の位置及び姿勢の計測は、拡張現実感／複合現実感における現実空間と仮想物体の位置合わせ、ロボットや自動車の自己位置推定、物体やシーンの三次元モデリングに利用される。

非特許文献１では、シーン中にある特徴点の情報を三次元マップとして保持しておき、画像上で検出される特徴点と三次元マップ中の特徴点の対応をもとに撮像装置の位置及び姿勢を推定する方法が開示されている。この方法では、前フレームにおいて計測された位置及び姿勢を現フレームにおける位置及び姿勢の計測に利用する。例えば、前フレームで計測された位置及び姿勢と動きモデルに基づいて現フレームの位置及び姿勢を予測し、三次元マップ中の特徴点と画像上の特徴点との対応付けに利用する。また、予測された位置及び姿勢は現フレームの位置及び姿勢を求めるための繰返し計算の初期値として利用する。

フレーム間での撮像装置の動きが大きい場合や画像上で検出される特徴点の数が極めて少ない場合には位置及び姿勢の計測は失敗する場合がある。そのため、位置及び姿勢の計測の失敗を検出し失敗から回復する処理が必要となる。非特許文献１では、正しく対応付けられた特徴点の割合に基づいて位置及び姿勢計測の質を判定し、質が低い計測が一定フレーム数継続する場合に位置及び姿勢の計測に失敗したと判定している。

Ｇ．Ｋｌｅｉｎ ａｎｄ Ｄ．Ｍｕｒｒａｙ，"Ｐａｒａｌｌｅｌ Ｔｒａｃｋｉｎｇ ａｎｄ Ｍａｐｐｉｎｇ ｆｏｒ Ｓｍａｌｌ ＡＲ Ｗｏｒｋｓｐａｃｅｓ，" Ｐｒｏｃ．６ｔｈ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ’０７），２００７．

非特許文献１における位置及び姿勢計測の失敗判定方法は画像上の見た目に基づく方法であるため、見た目が類似した部分がシーン中の別の箇所に存在する場合、推定される位置及び姿勢が正しいか否かの判定を誤るという課題がある。また、特徴点の数が極めて少ない場合には、推定される位置及び姿勢が正しくても正しく対応付けられた特徴点の割合が小さいために失敗判定を誤るという課題がある。

本発明は上記課題に鑑みてなされたものであり、位置姿勢の推定処理が失敗したかを判定し安定的に位置姿勢を導出することを目的とする。

上記課題を解決するために、本発明の情報処理装置は、例えば、現実空間の特徴の三次元情報を保持する三次元情報保持手段と、撮像装置によって前記現実空間を撮像した画像を入力する画像入力手段と、前記撮像装置に取り付けられたセンサによって計測される前記撮像装置の位置姿勢に関する計測値を入力する計測値入力手段と、前記三次元情報と前記入力された画像とに基づいて、前記撮像装置の位置姿勢を導出する位置姿勢導出手段と、前記計測値から得られる位置姿勢に関する情報と前記位置姿勢導出手段によって導出された前記撮像装置の位置姿勢に関する情報との差分に基づいて前記位置姿勢導出手段による前記撮像装置の位置姿勢の導出が失敗したか否かを判定する判定手段と、前記判定手段の結果を出力する出力手段とを備える。

本発明によれば、位置姿勢の推定処理が失敗したかを判定し安定的に位置姿勢を導出することができる。

第１の実施形態における情報処理装置１の構成を示す図である。 姿勢センサがカメラに固定されている様子を示す図である。 第１の実施形態における位置及び姿勢計測の処理手順を示すフローチャートである。 第１の実施形態のステップＳ１０４０における処理手順を示すフローチャートである。 第１の実施形態のステップＳ１０６０における処理手順を示すフローチャートである。 第２の実施形態のステップＳ１０６０における処理手順を示すフローチャートである。 第３の実施形態のステップＳ１０６０における処理手順を示すフローチャートである。 本発明の情報処理装置のハードウェア構成の例を示す図である。

本発明にかかる各実施形態を説明するのに先立ち、各実施形態に示す情報処理装置が実装されるハードウェア構成について、図８を用いて説明する。

図８は、本実施形態における情報装置のハードウェア構成図である。同図において、ＣＰＵ８１０は、バス８００を介して接続する各デバイスを統括的に制御する。ＣＰＵ９１０は、読み出し専用メモリ（ＲＯＭ）８２０に記憶された処理ステップやプログラムを読み出して実行する。オペレーティングシステム（ＯＳ）をはじめ、本実施形態に係る各処理プログラム、デバイスドライバ等はＲＯＭ８２０に記憶されており、ランダムアクセスメモリ（ＲＡＭ）８３０に一時記憶され、ＣＰＵ８１０によって適宜実行される。また、入力Ｉ／Ｆ８４０は、外部の装置（表示装置や操作装置など）から情報処理装置１で処理可能な形式で入力信号として入力する。また、出力Ｉ／Ｆ８５０は、外部の装置（表示装置）へ表示装置が処理可能な形式で出力信号として出力する。

（第１の実施形態）
本実施形態では、現実空間に仮想物体を重ね合わせて表示する複合現実感システムにおいて仮想物体を描画するために必要な現実空間におけるカメラの位置及び姿勢の計測に本発明の方法を適用した場合について述べる。

具体的には、カメラに装着した姿勢センサが計測する姿勢と、カメラが撮影する画像を用いて算出されるカメラの姿勢との比較により画像を用いた位置及び姿勢の計測の失敗を判定し、失敗と判定された場合には復帰処理を行って位置及び姿勢の計測を再開する。これによって、位置及び姿勢は正しく計測されておらず失敗しているにも関わらず画像としては整合性が取れているために位置及び姿勢が正しく計測されていると誤判定されることを回避する。本実施形態において、カメラの位置姿勢とは、世界座標系におけるカメラの撮像視点位置の位置座標（ｘ、ｙ、ｚ）の３パラメータおよび、カメラの姿勢、すなわちカメラ座標系のＸ軸、Ｙ軸、Ｚ軸の方向を表す３パラメータを合わせた６パラメータのことをいう。本実施形態において、姿勢を表す３パラメータは、世界座標系における回転軸の傾きを表す２パラメータと、該回転軸まわりの回転角を表す１パラメータであるとする。なお、姿勢を表す３パラメータはこれに限るものではなく、ロール角・ピッチ角・ヨー角によって表してもよいし、オイラー角によって表してもよい。

なお、本実施形態では、３次元空間におけるカメラの姿勢のうち、傾斜角を用いて失敗判定を行う。ここでカメラの傾斜角とは、重力軸に対するカメラの傾きを表す。姿勢の傾斜角以外の成分は方位角であり、重力軸まわりの回転を表す。一般的な姿勢センサには角速度センサ（ジャイロセンサ）とともに加速度センサが内蔵されており、重力軸の方向を傾斜角の絶対的なリファレンスとして利用できるため、失敗判定に傾斜角を用いる。

図１は、本実施形態における情報処理装置１の構成を示す図である。情報処理装置１は、三次元情報保持部１０、画像入力部２０、センサ計測値入力部３０、位置姿勢算出部４０、失敗判定部５０から構成されている。画像入力部２０はカメラ７０と接続されている。また、センサ計測値入力部３０は姿勢センサ８０と接続されている。また情報処理装置１には失敗回復部６０が接続されている。

三次元情報保持部１０は、画像に基づく位置及び姿勢の推定に利用するシーン（現実空間）の三次元情報を保持する。非特許文献１に開示されるように、シーンの三次元情報はマップデータとして保持する。マップデータには特徴点データベースを保持する。特徴点データベースには、シーン中に規定された世界座標系における特徴点の三次元座標と特徴点を表す画像パッチを保持する。さらにマップデータには、撮像した時の位置及び姿勢が属性として付与されたシーンの撮像画像群を保持する。これらの画像群を構成する画像それぞれを、以下ではキーフレーム画像と呼ぶ。マップデータは、公知のＳｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ技術により予め作成してもよいし、非特許文献１のように位置及び姿勢の計測と並行して作成してもよい。なお本実施形態では、世界座標系のＺ軸が重力軸と平行に、Ｘ軸、Ｙ軸が重力軸と直交（地面に平行）になるように座標軸を設定する。

画像入力部２０は、カメラ７０によって撮像された画像を位置姿勢算出部４０に入力する。本実施形態では、画像入力部２０はカメラ７０から時系列に入力するものとし、情報処理装置１は、カメラ７０が逐次撮像する画像の各々について撮像時のカメラの位置及び姿勢を計測する。本実施形態では、カメラ７０が撮像する画像に仮想物体のＣＧを重畳描画した画像をユーザが観察するため画像は入力するカラー画像であるとする。なお、例えば自動車やロボットの自己位置推定では撮像した画像をユーザが観察する必要はないため、入力する画像は濃淡画像であってもよい。カメラ７０の内部パラメータ（焦点距離、画像中心、レンズ歪みパラメータ）は既知であるとする。カメラの内部パラメータは、例えばＺｈａｎｇの方法（Ｚ．Ｚｈａｎｇ，“Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｉｌｂｒａｔｉｏｎ，” ＩＥＥＥ Ｔｒａｎｓ．ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．１１，ｐｐ．１３３０−１３３４，２０００．）によって校正する。

センサ計測値入力部３０は、姿勢センサ８０によって計測された３自由度の姿勢を位置姿勢算出部４０、失敗判定部５０に入力する。図２に示すように、姿勢センサ８０はカメラ７０に固定されているものとする。カメラ７０に対する姿勢センサ８０の姿勢は既知であるとし、世界座標系における姿勢センサ８０の姿勢はカメラ７０の姿勢に変換可能であるとする。カメラ７０に対する姿勢センサ８０の姿勢は、例えばＳａｔｏｈらの方法（Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，ａｎｄ Ｈ．Ｙａｍａｍｏｔｏ，“Ａ Ｈｅａｄ Ｔｒａｃｋｉｎｇ Ｍｅｔｈｏｄ Ｕｓｉｎｇ Ｂｉｒｄ’ｓ−Ｅｙｅ Ｖｉｅｗ Ｃａｍｅｒａ ａｎｄ Ｇｙｒｏｓｃｏｐｅ，” Ｐｒｏｃ．３ｒｄ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ’０４），２００４．）によって校正する。

位置姿勢算出部４０は、画像入力部２０によって入力された画像、センサ計測値入力部３０によって入力された姿勢計測値及び三次元情報保持部１０が保持するマップデータに基づいて画像を撮像した時の世界座標系におけるカメラの位置及び姿勢を算出（位置姿勢導出）する。

失敗判定部５０は、センサ計測値入力部３０によって入力された姿勢計測値及び位置姿勢算出部４０によって算出されたカメラの姿勢に基づいて、位置姿勢算出部４０による位置及び姿勢の算出が失敗したかどうか判定し、その結果を出力する。

失敗回復部６０は、画像入力部２０によって入力された画像及び三次元情報保持部１０が保持するマップデータに基づいて失敗回復を行い、位置及び姿勢の計測を再開する。

次に、本実施形態における位置及び姿勢計測の処理手順について説明する。図３は、本実施形態における位置及び姿勢計測の処理手順を示すフローチャートである。

ステップＳ１０１０では、位置姿勢算出部４０は失敗フラグＦをＴＲＵＥに設定する。これは、位置及び姿勢の計測を始める時には位置及び姿勢は不明であり、位置及び姿勢計測に失敗した時と同じ状態にあるためである。

ステップＳ１０２０では、位置姿勢算出部４０は、カメラ７０が撮像した画像及び姿勢センサ８０が計測したカメラの姿勢をそれぞれ画像入力部２０、センサ計測値入力部３０を介して取得する。

ステップＳ１０３０において失敗フラグＦがＴＲＵＥである場合にはステップＳ１０５０に進み、ＦＡＬＳＥである場合にはステップＳ１０４０に進む。

ステップＳ１０４０では、位置姿勢算出部４０はカメラ７０によって撮像された画像、姿勢センサ８０によって計測されたカメラの姿勢及び三次元情報保持部１０が保持するマップデータに基づいて画像を撮像した時の世界座標系におけるカメラの位置及び姿勢を算出する。ステップＳ１０４０では前フレームでの位置及び姿勢計測が失敗していないので、前フレームで計測された位置及び姿勢を用いて位置及び姿勢を算出する。位置及び姿勢の詳細な算出方法については後述する。

ステップＳ１０５０では、失敗回復部６０は、前フレームでの位置及び姿勢計測が失敗しているため、位置及び姿勢計測の失敗回復処理を行う。位置及び姿勢計測の失敗回復は、Ｋｌｅｉｎらの方法（Ｇ．Ｋｌｅｉｎ ａｎｄ Ｄ．Ｍｕｒｒａｙ，“Ｉｍｐｒｏｖｉｎｇ ｔｈｅ Ａｇｉｌｉｔｙ ｏｆ Ｋｅｙｆｒａｍｅ−ｂａｓｅｄ ＳＬＡＭ，” Ｐｒｏｃ．１０ｔｈ Ｅｕｒｏｐｅａｎ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，２００８．）を用いて行う。すなわち、入力画像を縮小した画像と、マップデータ中のキーフレーム画像を縮小した画像とのマッチングを行い、最も誤差の小さいキーフレーム画像が属性として持つ位置及び姿勢を入力画像撮像時のカメラの位置及び姿勢とする。

ステップＳ１０６０では、失敗判定部５０は、位置姿勢算出部４０によって算出されたカメラの姿勢と、姿勢センサ８０によって計測されたカメラの姿勢を比較することにより位置姿勢算出部４０における位置及び姿勢算出の失敗を判定する。本実施例では、カメラの姿勢のうち、姿勢センサによって高精度に計測が可能な傾斜角成分を比較することによって失敗を判定する。失敗判定の詳細については後述する。

ステップＳ１０７０では、位置及び姿勢計測を終了するか否かの判定を行う。マウスやキーボードなどを介してユーザから位置及び姿勢計測を終了するコマンドが入力されている場合、位置及び姿勢計測を終了する。そうでない場合は位置及び姿勢計測を継続するためステップＳ１０２０に戻る。

（ステップＳ１０４０の処理の詳細）
次に、ステップＳ１０４０における位置及び姿勢算出処理について詳細に説明する。図４は、ステップＳ１０４０における処理手順を示すフローチャートである。

ステップＳ１１１０では、位置姿勢算出部４０は、姿勢センサの計測値と前フレームで算出された位置及び姿勢から現フレームの位置及び姿勢を予測する。予測位置は前フレームで計測された位置をそのまま用いる。前フレームで算出された姿勢を表す３自由度回転行列をＲ（ｔ−１）、前フレーム、現フレームの姿勢センサによって計測された姿勢を表す３自由度回転行列をそれぞれＲｓ（ｔ−１）、Ｒｓ（ｔ）とする。Ｒ（ｔ−１）、Ｒｓ（ｔ−１）、Ｒｓ（ｔ）は世界座標系を基準とした時のカメラの姿勢を表すものとする。現フレームの予測姿勢を表す３×３回転行列Ｒ（ｔ）を式１のように算出する。
Ｒ（ｔ）＝Ｒ（ｔ−１）・（Ｒ_ｓ（ｔ−１））^−１・Ｒ_ｓ（ｔ） （式１）
ステップＳ１１２０では、位置姿勢算出部４０は、入力された画像から特徴点を検出する。特徴点の検出は非特許文献１と同様の方法で検出し、以降のステップで利用するために検出された特徴点の入力画像上での座標（ｕ，ｖ）を保持する。

ステップＳ１１３０では、位置姿勢算出部４０は、ステップＳ１１２０で検出された特徴点と、マップデータ中の特徴点との対応付けを行う。具体的には、まずステップＳ１１１０で得られた現フレームの予測位置及び予測姿勢を用いてマップデータ中の各特徴点の世界座標を入力画像上の座標に変換する。次に、変換された座標を中心とした一定範囲内に存在するステップＳ１１２０で検出された特徴点の画像パッチとマップデータ中の特徴点の画像パッチとの類似度を算出する。最もパッチの類似度が大きい特徴点とマップ中の特徴点が対応するとして、マップ中の特徴点の世界座標と入力画像上で検出された特徴点の座標の組を対応付けデータとして以降のステップで利用する。

ステップＳ１１４０では、位置姿勢算出部４０は、ステップＳ１１３０で得られた複数の対応付けデータを利用して現フレームを撮像した時のカメラの位置及び姿勢を算出する。カメラの位置及び姿勢の算出は、非特許文献１と同様の方法で行う。すなわち、算出する位置及び姿勢によってマップ中の特徴点の世界座標を画像座標に変換して得られる座標と、該特徴点に対応付けられた入力画像上で検出された特徴点の画像座標との誤差（再投影誤差）が小さくなるように、繰返し位置及び姿勢の算出を行う。算出したカメラの姿勢を表す３×３回転行列をＲｖ（ｔ）とする。Ｒｖ（ｔ）は後述するステップＳ１０６０で用いる。

（ステップＳ１０６０の処理の詳細）
次に、ステップＳ１０６０における失敗判定処理について詳細に説明する。図５は、ステップＳ１０６０における処理手順を示すフローチャートである。

ステップＳ１２１０では、失敗判定部５０は、現フレームの姿勢センサによって計測された姿勢Ｒｓ（ｔ）から傾斜角成分を抽出する。Ｒｓ（ｔ）は式２のように傾斜角成分Ｒｉｓと方位角成分Ｒａｓに分解できる。
Ｒ_ｓ（ｔ）＝Ｒ_ａｓ・Ｒ_ｉｓ （式２）
前述したように、世界座標系のＺ軸の方向を重力軸の方向にとったため、Ｒａｓは世界座標系のＺ軸周りの回転Ｒｚで表される。また、Ｒｉｓは重力軸に直交する平面（地面）上のベクトル周りの回転であるため、世界座標系のＸ軸周りの回転ＲｘとＹ軸周りの回転Ｒｙを合成した回転ＲｙＲｘで表される。Ｒｚの回転角をα、Ｒｙの回転角をβ、Ｒｚの回転角をγとするとＲｓ（ｔ）は式３のように表すことができる。

数３を展開すると数４のようになる。

センサ計測値から得られるＲｓ（ｔ）の各要素の値と式４をもとに、ｃｏｓβ、ｓｉｎβ、ｃｏｓγ、ｓｉｎγを算出することでＲｓ（ｔ）から傾斜角成分Ｒｉｓを抽出する。

ステップＳ１２２０では、失敗判定部５０は、ステップＳ１０４０で算出したカメラの姿勢Ｒｖ（ｔ）をステップＳ１２１０と同じ方法により方位角Ｒａｖと傾斜角Ｒｉｖに分解することで傾斜角成分Ｒｉｖを抽出する。

ステップＳ１２３０では、失敗判定部５０は、ステップＳ１２１０、ステップＳ１２２０で抽出した傾斜角成分Ｒｉｓ、Ｒｉｖを比較することで、ステップＳ１０４０において位置及び姿勢の算出に失敗したかどうかを判定する。具体的には、カメラを基準とした重力軸方向を表すベクトルｇｓ、ｇｖをそれぞれ傾斜角成分Ｒｉｓ、Ｒｉｖから算出し、二つのベクトルの角度差が閾値以上であれば位置及び姿勢の算出に失敗したと判断する。前述したように本実施形態では世界座標系のＺ軸を重力軸方向と平行にとっている。そのため、式５のように傾斜角Ｒｉｓ、Ｒｉｖを用いて世界座標系のＺ軸を表すベクトルをカメラ座標系における重力軸方向を表すベクトルｇｓ、ｇｖに変換する。

ｇｓ、ｇｖから算出される二つのベクトルがなす角度が閾値以上であれば失敗フラグＦをＴＲＵＥにセットし、それ以外の場合には失敗フラグをＦＡＬＳＥにセットする。

以上述べたように第１の実施形態では、姿勢センサが計測する傾斜角と画像をもとに算出された傾斜角との比較によって位置及び姿勢計測の失敗を判定することで、適切なタイミングで位置及び姿勢計測の失敗から回復させ、位置及び姿勢計測を再開させることができる。

なお、位置及び姿勢計測の失敗判定に利用する情報は姿勢の傾斜角成分に限るものではなく、センサが計測する値と直接比較できるものであれば何でもよい。例えば、屋外など姿勢センサが方位角も正確に計測することができる場合には、３自由度の姿勢を比較対象にしてもよい。また、姿勢のうち方位角を比較対象にしてもよい。さらに、利用するセンサは姿勢センサに限るものではなく、位置または姿勢を計測するものであれば何でもよい。例えば、傾斜角を直接計測する傾斜センサであってもよい。また、ＧＰＳなどの位置センサを利用し、位置を比較対象にしてもよい。また、気圧センサのような高さを計測するセンサを利用し、位置のうち高さ成分を比較対象にしてもよい。さらには、気圧センサの気圧を高さに変換せずに、計測される気圧を比較対象にしてもよい。他にも、磁気センサやモーションキャプチャシステムのような位置及び姿勢を計測するセンサを利用してもよい。

なお，位置及び姿勢計測の失敗判定を行う際には、本実施形態で述べたセンサの計測値と画像に基づく計測値との比較による失敗判定と、非特許文献１で開示されているような画像情報のみによる失敗判定とを組み合わせて用いてもよい。すなわち、画像情報のみによる失敗判定を行い、失敗と判定されない場合に本実施形態で述べた失敗判定を行ってもよい。

なお、位置及び姿勢を計測するための画像を撮影するカメラは一台のカメラである必要はなく、複数台のカメラ（例えばステレオカメラ）であってもよい。また、通常の濃淡画像やカラー画像を撮像するカメラではなく、距離画像を撮像するカメラであってもよい。

位置姿勢算出部は、画像とマップデータに基づいて位置及び姿勢を算出するものであれば何でもよい。ステレオカメラである場合には、ステレオカメラが撮像する画像を用いて位置及び姿勢を算出してもよい。距離画像を撮像するカメラを利用する場合には、距離画像を用いて位置及び姿勢を算出してもよい。また、位置及び姿勢の算出に用いる特徴は、特徴点に限るものではなく、画像から検出可能でマップデータとして登録可能な特徴であれば他の特徴であってもよい。例えば、濃淡画像において輝度勾配が大きい点であるエッジでもよい。

失敗回復部における失敗回復処理は、画像マッチングによって行う方法に限るものではなく、位置及び姿勢の事前情報なしに入力された画像から位置及び姿勢を算出する方法であればいかなる方法であってもよい。例えば、非特許文献１に開示されているように、マップデータ中の特徴点と画像上の特徴点との対応を識別器によって求め、得られた対応から位置及び姿勢を算出してもよい。

三次元情報保持部が保持するシーンの三次元情報は、特徴点の世界座標系における三次元座標のデータベースに限るものではなく、世界座標系における三次元座標に変換可能であればいかなる表現形態であってもよい。例えば、撮像した時の世界座標系における位置及び姿勢が属性として付与されたシーンの撮像画像群を保持し、特徴点の座標は撮像画像群のカメラ座標系における特徴点の三次元座標として保持しておき、属性に含まれる位置及び姿勢を用いて世界座標系における特徴点の三次元座標に変換してもよい。

（第２の実施形態）
第１の実施形態では、センサが計測する位置及び姿勢と画像をもとに算出される位置及び姿勢を直接比較することで位置及び姿勢計測の失敗判定を行っていた。本実施形態では、センサから得られる計測値の差分と、画像をもとに算出される位置及び姿勢の差分を比較することで位置及び姿勢計測の失敗判定を行う。

具体的には、異なるフレームにおいて姿勢センサによって計測された姿勢の差分と、カメラが撮影する画像を用いて算出した姿勢の差分とを比較することにより、画像を用いた位置及び姿勢の計測の失敗を判定する。これによって、例えばセンサの傾斜角や方位角等特定の成分が高精度に計測できなくても、位置及び姿勢の計測の失敗を判定することができる。

本実施形態における情報処理装置の構成は第１の実施形態で説明した情報処理装置１と同一であるため説明を省略する。また第２の実施形態における位置及び姿勢計測の処理手順も第１の実施形態と同一であるため説明を省略する。第１の実施形態と第２の実施形態で異なる部分は、図３のフローチャートにおけるステップＳ１０６０である。

図６は、本実施形態のステップＳ１０６０における処理手順を示すフローチャートである。

ステップＳ２２１０では、失敗判定部５０は、前フレームの姿勢センサによって計測された姿勢Ｒｓ（ｔ−１）と現フレームの姿勢センサによって計測された姿勢Ｒｓ（ｔ）からフレーム間の姿勢変化ΔＲｓを算出する。ΔＲｓは式６のように算出する。
ΔＲ_ｓ＝（Ｒ_ｓ（ｔ−１））^−１・Ｒ_ｓ（ｔ） （式６）
ステップＳ２２２０では、失敗判定部５０は、ステップＳ１０４０で前フレームを用いて算出したときのカメラの姿勢Ｒｖ（ｔ−１）と現フレームを用いて算出したカメラの姿勢Ｒｖ（ｔ）からフレーム間の姿勢変化ΔＲｖを算出する。ΔＲｖはΔＲｓと同様に算出する。

ステップＳ１２３０では、失敗判定部５０は、ステップＳ１２１０、ステップＳ１２２０で抽出した姿勢変化ΔＲｓ、ΔＲｖを比較することで、ステップＳ１０４０において位置及び姿勢の算出に失敗したかどうかを判定する。具体的にはまず式７より姿勢変化ΔＲｓとΔＲｖの差であるΔＲｓｖを算出する。ΔＲｓｖは３×３回転行列である。ΔＲｓｖの回転角が所定の閾値以上であれば失敗と判断する。
ΔＲ_ｓｖ＝（ΔＲ_ｓ）^−１ΔＲ_ｖ （式７）
ΔＲｓｖの回転角θは、３×３回転行列ΔＲｓｖのトレースが１＋２ｃｏｓθであることを利用して求める。回転角θが閾値以上であれば失敗フラグＦをＴＲＵＥにセットし、それ以外の場合には失敗フラグをＦＡＬＳＥにセットする。

以上述べたように本実施形態では、センサから得られる姿勢計測値の差分と、画像をもとに算出される姿勢の差分を比較することで、例えばセンサの傾斜角や方位角等特定の成分が高精度に計測できなくても、位置及び姿勢の計測の失敗を判定することができる。

本実施形態では現フレームと前フレームから姿勢の差分を算出していた。しかしながら、姿勢の差分は時間的に隣接したフレームから算出しなくてもよい。例えば、数フレーム前の姿勢と現フレームの姿勢から差分を算出してもよい。

なお、位置及び姿勢計測の失敗判定に利用する情報は姿勢計測値の差分に限るものではなく、差分を比較できるものであれば何でもよい。例えば、例えば、ＧＰＳなどの位置センサを利用し、位置の差分を比較対象にしてもよい。また、気圧センサのような高さを計測するセンサを利用し、高さの差分を比較対象にしてもよい。さらには、気圧センサの気圧を高さに変換せずに、計測される気圧の差を比較対象にしてもよい。

なお，位置及び姿勢計測の失敗判定を行う際には、本実施形態で述べたセンサの計測値の差分と画像に基づく計測値の差分との比較による失敗判定と、非特許文献１で開示されているような画像情報のみによる失敗判定とを組み合わせて用いてもよい。すなわち、画像情報のみによる失敗判定を行い、失敗と判定されない場合に本実施形態で述べた失敗判定を行ってもよい。

さらには、利用するセンサは位置や姿勢を計測するものに限るものではなく、速度センサ、加速度センサ、角速度センサなどの位置や姿勢の変化を計測するセンサであってもよい。このようなセンサを利用する場合、センサ計測値は差分を求める必要はなく、画像をもとに算出される位置及び姿勢の差分からセンサ計測値に相当する物理量（速度、加速度、角速度）を算出すればよい。

（第３の実施形態）
第１の実施形態及び第２の実施形態ではセンサ計測値を位置及び姿勢計測の失敗判定に利用していた。これに対し第３の実施形態では、センサ計測値を位置及び姿勢計測の失敗回復に利用する。センサ計測値を失敗回復に利用することで、高速な失敗回復が可能になる。

第３の実施形態における情報処理装置の構成１は第１の実施形態で説明した情報処理装置１と同一であるため説明を省略する。また第３の実施形態における位置及び姿勢計測の処理手順も第１の実施形態と同一であるため説明を省略する。第１の実施形態と第３の実施形態で異なる部分は、図３のフローチャートにおけるステップＳ１０５０である。

図７は、第３の実施形態のステップＳ１０５０における処理手順を示すフローチャートである。

ステップＳ３１０では、失敗回復部６０は、姿勢センサが計測する姿勢計測値から実施例１で述べた方法により傾斜角成分Ｒｉｓを抽出する。

ステップＳ３２０では、失敗回復部６０は、マップデータが保持するキーフレーム画像毎に、キーフレーム画像が属性として持つ姿勢から傾斜角成分Ｒｉｋを抽出し、ＲｉｓとＲｉｋの差を第１の実施形態で述べたカメラを基準とした時の重力軸方向のベクトルの角度差として求める。そして角度差が所定の閾値以内のキーフレーム画像を失敗回復における画像マッチングの対象として選択する。

ステップＳ３３０では、失敗回復部６０は、第１の実施形態のステップＳ１０５０の説明で述べたように、入力画像を縮小した画像とマップデータ中のキーフレーム画像を縮小した画像とのマッチングを行う。このとき、全てのキーフレーム画像とのマッチングを行うのではなく、ステップＳ３２０で選択したキーフレーム画像のみを対象としてマッチングを行う。そして、最もマッチング誤差の小さいキーフレーム画像が属性として持つ位置及び姿勢を入力画像撮像時のカメラの位置及び姿勢とする。

以上述べたように、本実施形態では姿勢センサの計測値を用いて失敗回復の際の画像マッチング対象のキーフレーム画像を限定することで、高速な失敗回復が可能になる。

なお、キーフレーム画像を限定するのに利用するセンサ計測値は傾斜角に限るものではなく、キーフレーム画像が属性として持つ位置及び姿勢と比較できるものであればいかなるものであってもよい。例えば、３自由度の姿勢を用いて限定してもよいし、姿勢の方位角成分のみを用いてもよい。また、利用するセンサは姿勢センサに限るものではなく、ＧＰＳのような位置センサや気圧センサのような高さセンサであってもよい。

（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。

即ち、上述した各実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（又はＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。

また、このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。

＜効果のまとめ＞
本発明により、姿勢センサが計測する傾斜角と画像をもとに算出された傾斜角との比較によって位置及び姿勢計測の失敗を判定することで、適切なタイミングで位置及び姿勢計測の失敗から回復させ、位置及び姿勢計測を再開させることができる。

また、センサから得られる姿勢計測値の差分と、画像をもとに算出される姿勢の差分を比較することで、例えばセンサの傾斜角や方位角等特定の成分が高精度に計測できなくても、位置及び姿勢の計測の失敗を判定することができる。

さらに姿勢センサの計測値を失敗回復に利用することで、高速な失敗回復が可能になる。

＜定義のまとめ＞
本発明における情報処理装置を構成する三次元情報保持部は、画像に基づく位置及び姿勢の推定に利用するシーンの三次元情報を保持する。シーンの三次元情報として、シーン中に規定された世界座標系における特徴点の三次元座標を保持する特徴点データベースを保持してもよい。また、撮像した時の世界座標系における位置及び姿勢が属性として付与されたシーンの撮像画像群を保持し、撮像画像群のカメラ座標系における特徴点の三次元座標を保持し、属性に含まれる位置及び姿勢を用いて世界座標系における特徴点の三次元座標に変換してもよい。

画像入力部はカメラによって撮像された画像を入力する。カメラは、濃淡画像を撮影するカメラであってもよいし、カラー画像を撮影するカメラであってもよい。また、距離画像を撮影するカメラであってもよい。

センサ計測値入力部は位置または姿勢に関する情報を計測するセンサの計測値を入力する。センサは、姿勢センサや位置センサ、傾斜センサ、高さセンサなど位置及び姿勢を直接計測するセンサであってもよいし、速度センサ、加速度センサ、角速度センサなど位置及び姿勢の変化を計測するセンサであってもよい。

位置姿勢算出部は、画像とシーンの三次元情報を元に画像を撮像した時のカメラの位置及び姿勢を算出するものであれば何でもよい。位置及び姿勢の算出に用いる画像は、単眼カメラによって撮影された画像でもステレオカメラのように複数のカメラによって撮影された画像でもよい。また、濃淡／カラー画像を撮影するカメラではなく距離画像を撮像するカメラによって撮像された距離画像を用いてもよい。また、位置及び姿勢の算出に用いる特徴は、特徴点に限るものではなく、画像から検出可能でマップデータとして登録可能な特徴であれば他の特徴であってもよい。例えば、濃淡画像における輝度勾配が大きい点であるエッジでもよい。

位置姿勢算出部は画像上で検出される特徴点の再投影誤差が小さくなるように位置及び姿勢の算出を行う。

失敗判定部は、センサ計測値を用いて画像を用いた位置及び姿勢算出の失敗判定を行う。センサが計測する位置または姿勢と画像を用いた算出した位置及び姿勢を直接比較してもよいし、位置または姿勢の変化量を比較してもよい。

Claims (11)

1. 現実空間の特徴の三次元情報を保持する三次元情報保持手段と、
   撮像装置によって前記現実空間を撮像した画像を入力する画像入力手段と、
   前記撮像装置に取り付けられたセンサによって計測される前記撮像装置の位置姿勢に関する計測値を入力する計測値入力手段と、
   前記三次元情報と前記入力された画像とに基づいて、前記撮像装置の位置姿勢を導出する位置姿勢導出手段と、
   前記計測値から得られる位置姿勢に関する情報と前記位置姿勢導出手段によって導出された前記撮像装置の位置姿勢に関する情報との差分に基づいて前記位置姿勢導出手段による前記撮像装置の位置姿勢の導出が失敗したか否かを判定する判定手段と、
   前記判定手段の結果を出力する出力手段とを備える情報処理装置。
2. 前記撮像装置の位置姿勢に関する計測値は、前記撮像装置の位置姿勢を示すパラメータのうちのいずれかであることを特徴とする請求項１に記載の情報処理装置。
3. 前記判定手段によって前記位置姿勢導出手段の位置姿勢の導出が失敗したと判定された場合、前記三次元情報と前記入力された画像とに基づいて前記撮像装置の位置姿勢を導出することにより、前記位置姿勢導出手段による前記撮像装置の位置姿勢の導出の失敗からの復帰を行う復帰手段をさらに有することを特徴とする請求項１または２に記載の情報処理装置。
4. 前記三次元情報保持手段は、更に、前記現実空間を撮像した複数の画像と、該複数の画像それぞれを撮像した時の撮像装置の位置姿勢を示すパラメータとを対応づけて保持し、前記復帰手段は、前記計測値と前記三次元情報保持手段が保持する位置姿勢を示すパラメータとに基づき前記複数の画像のうちから選択された画像を使って前記復帰を行うことを特徴とする請求項３に記載の情報処理装置。
5. 前記計測値は前記撮像装置の姿勢を示すパラメータであり、
   前記判定手段は、前記姿勢を示すパラメータと、前記位置姿勢導出手段によって導出される撮像装置の位置姿勢のうちの姿勢を示すパラメータとを比較することにより前記判定を行うことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記計測値は前記撮像装置の姿勢を示すパラメータであり、
   前記判定手段は、前記センサによって計測された姿勢を示すパラメータの変化量と、前記位置姿勢導出手段によって導出された撮像装置の姿勢の差分とを比較することにより位置及び姿勢導出の失敗を判定することを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
7. 前記姿勢を示すパラメータは傾斜角成分または方位角のいずれかであることを特徴とする請求項５または６に記載の情報処理装置。
8. 前記計測値は前記撮像装置の位置を示すパラメータであり、
   前記判定手段は、前記位置を示すパラメータと、前記位置姿勢導出手段によって導出される撮像装置の位置姿勢のうちの位置を示すパラメータとを比較することにより前記判定を行うことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
9. 前記計測値は前記撮像装置の位置を示すパラメータであり、
   前記判定手段は、前記センサによって計測された位置を示すパラメータの変化量と、前記位置姿勢導出手段によって導出された撮像装置の位置の差分とを比較することにより位置及び姿勢導出の失敗を判定することを特徴とする請求項１乃至４、または８のいずれか１項に記載の情報処理装置。
10. 撮像装置によって現実空間を撮像した画像を入力する画像入力工程と、
    前記撮像装置に取り付けられたセンサによって計測される前記撮像装置の位置姿勢に関する計測値を入力する計測値入力工程と、
    前記現実空間に含まれる特徴の三次元情報と前記入力された画像とに基づいて、前記撮像装置の位置姿勢を導出する位置姿勢導出工程と、
    前記計測値から得られる位置姿勢に関する情報と前記位置姿勢導出工程において導出された前記撮像装置の位置姿勢に関する情報との差分に基づいて前記位置姿勢導出手段による前記撮像装置の位置姿勢の導出が失敗したか否かを判定する判定工程と、
    前記判定手段の結果を出力する出力工程とを備える情報処理方法。
11. コンピュータを、請求項１乃至９のいずれか１項に記載の情報処理装置の各手段として機能させるためのプログラム。

Images