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

Description

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

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

非特許文献１では、画像から検出される特徴点を用いて画像撮像時の撮像装置の位置及び姿勢を計測する方法が開示されている。この方法では、シーン中に規定された世界座標系における特徴点の三次元座標とともに、仮想的に設定された複数の視点（以下、仮想視点と呼ぶ）において観察可能な特徴点の情報を予め三次元マップとして保持している。撮像装置が撮像した画像が入力されると、撮像画像から検出される特徴点に対応する三次元マップ中の特徴点を各仮想視点が保持する特徴点の情報をもとに探索し、特徴点の画像座標と三次元座標との対応をもとに撮像装置の位置及び姿勢を算出する。

Ａ．Ｗｅｎｄｅｌ，Ａ．Ｉｒｓｃｈａｒａ，ａｎｄ Ｈ．Ｂｉｓｃｈｏｆ， "Ｎａｔｕｒａｌ ｌａｎｄｍａｒｋ−ｂａｓｅｄ ｍｏｎｏｃｕｌａｒ ｌｏｃａｌｉｚａｔｉｏｎ ｆｏｒ ＭＡＶｓ，" Ｐｒｏｃ． ２０１１ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ ２０１１）， ｐｐ．５７９２−５７９９， ２０１１．

非特許文献１では、シーン中の予め決められた範囲内に仮想視点を配置するため、撮像装置がその範囲から外れてしまう場合には、位置及び姿勢計測の精度や安定性を損ねてしまうことがある。

本発明の１態様によれば、情報処理装置に、撮像された画像と、該画像を撮像したときの撮像装置の位置及び姿勢と、該画像に対応する空間中の特徴の三次元情報とを対応付けた、複数の参照データから構成される三次元マップを予め保持する保持手段と、撮像装置により撮像された画像を入力する入力手段と、前記複数の参照データと、前記入力された画像とに基づいて、該画像を前記撮像装置が撮像した位置及び姿勢を導出する導出手段と、導出された前記位置及び姿勢と前記複数の参照データとに基づいて、新たな参照データを追加するかを判定する判定手段と、前記判定手段により新たな参照データを追加すると判定された場合に、前記入力された画像と、導出された前記位置及び姿勢と、前記複数の参照データの１つにおける特徴の三次元データとに基づいて新たな参照データに登録する特徴点を決定する決定手段と、前記決定された特徴点を登録した前記新たな参照データを追加して前記複数の参照データを更新する更新手段とを備える。

本発明によれば、撮像装置があらかじめ仮想視点が生成された範囲から外れた場合であっても、精度や安定性を損ねることなく位置及び姿勢の計測ができる。

本実施形態における情報処理装置のハードウェア構成図である。 第１の実施形態における情報処理装置の機能構成を示す図である。 第１の実施形態における位置及び姿勢の導出、三次元マップ更新の処理手順を示すフローチャートである。 位置及び姿勢の導出処理における処理手順を示すフローチャートである。 三次元マップ更新処理における処理手順を示すフローチャートである。 第１の実施形態における仮想キーフレームの生成の処理手順を示すフローチャートである。 変形例における仮想キーフレーム生成の処理手順を示すフローチャートである。 本実施形態における現実空間の三次元マップを示す図である。 三次元マップ表示部が表示する画面の一例を示す図である。

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

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

（第１の実施形態）
第１の実施形態では、複合現実感システムにおける現実空間と仮想物体との位置合わせ、すなわち仮想物体の描画に利用するための現実空間における撮像装置（カメラ）の位置及び姿勢の計測に本発明の方法を適用した場合について説明する。複合現実感を体験するユーザは、頭部装着型表示装置ＨＭＤ（Ｈｅａｄ−Ｍｏｕｎｔｅｄ Ｄｉｓｐｌａｙ）を装着し、ＨＭＤを通して仮想物体が重畳された現実空間を観察するものとする。ＨＭＤには撮像装置としてステレオカメラが搭載されており、ステレオカメラが撮像した画像上にステレオカメラの現実空間における位置及び姿勢に基づいて描画された仮想物体が重畳されてユーザに提示される。撮像装置の位置及び姿勢の計測に利用する現実空間中の特徴点の三次元マップは事前に生成しておき、体験時には撮像装置が撮像した画像と三次元マップを用いて撮像装置の位置及び姿勢の計測を算出する。

本実施形態では、複合現実感の体験中に撮像された画像を三次元マップにキーフレームとして追加し、以後の位置及び姿勢計測で利用できるようにする。これにより、予め生成された三次元マップによって位置及び姿勢の計測が可能な範囲から撮像装置が外れる場合であっても精度や安定性を損なわずに位置及び姿勢の計測ができるようになる。キーフレームについては後述する。

本実施形態における撮像装置の位置及び姿勢とは、現実空間中に規定された世界座標系における撮像装置の位置を表す３パラメータ、及び撮像装置の姿勢を表す３パラメータを合わせた６パラメータのことを表す。ここでは、姿勢は回転軸と回転角によって表すものとし、姿勢を表す３パラメータは回転軸の傾きを表す２パラメータと該回転軸まわりの回転角を表す１パラメータであるとする。本実施形態では、特に断りがない限りステレオカメラのうち左側のカメラの位置及び姿勢を撮像装置の位置及び姿勢と呼ぶ。また、撮像装置の光軸をＺ軸、画像の水平方向をＸ軸、垂直方向をＹ軸とする撮像装置上に規定される三次元の座標系を撮像装置座標系と呼ぶ。

図２は、本実施形態における情報処理装置１の構成を示す図である。情報処理装置１は、三次元情報保持部１０、画像入力部２０、位置姿勢導出部３０、三次元マップ更新部４０から構成されている。画像入力部２０はＨＭＤに搭載されている撮像部５０と接続されている。撮像装置５０はステレオカメラである。画像入力部２０、位置姿勢導出部３０は画像合成部６０と接続されている。画像合成部６０は表示部７０と接続されている。

三次元情報保持部１０は、位置姿勢導出部３０で利用する現実空間の三次元マップを保持する。図８に示すように、本実施形態では、現実空間の三次元マップはキーフレーム（ｋｅｙｆｒａｍｅ）の集合として保持されるものとする。キーフレームは、撮像装置が撮像した撮像画像（濃淡画像）Ｉ、撮像画像と同じ画像サイズのデプスマップＤ、分散マップＶを対応付けて保持するものとする。デプスマップＤの各画素には、撮像装置から見た奥行きの逆数、すなわち撮像座標系におけるＺ座標の逆数が保持されているものとする。分散マップＶの各画素には、撮像装置から見た奥行きの逆数の分散が保持されているものとする。

以下では、Ｉ（ｕ，ｖ）により画像座標（ｕ，ｖ）における画素の輝度値を、Ｄ（ｕ，ｖ）により画像座標（ｕ，ｖ）における画素の奥行きの逆数をそれぞれ参照するものとする。また、Ｖ（ｕ，ｖ）により画像座標（ｕ，ｖ）における画素の奥行きの逆数の分散を参照するものとする。なおＤ（ｕ，ｖ）に０が登録されている画素は無効な点として位置及び姿勢の導出に利用されないものとする。さらにキーフレームは、画像撮像時の撮像装置の世界座標系における位置及び姿勢を持つものとする。

以下では、この位置及び姿勢を「キーフレームの位置及び姿勢」と呼ぶ。撮像装置が撮像する画像がカラー画像である場合には濃淡画像に変換して保持する。図８に示すように、三次元情報保持部１０は、キーフレームとして実キーフレーム及び仮想キーフレームの２種類のキーフレームを保持する。実キーフレームは、ユーザが複合現実感を体験する前に予め生成しておく。

本実施形態では、Ｅｎｇｅｌらの方法（Ｊ．Ｅｎｇｅｌ，Ｊ．Ｓｔｕｃｋｌｅｒ（ｕはウムラウトつき），Ｄ．Ｃｒｅｍｅｒｓ，“Ｌａｒｇｅ−Ｓｃａｌｅ Ｄｉｒｅｃｔ ＳＬＡＭ ｗｉｔｈ Ｓｔｅｒｅｏ Ｃａｍｅｒａｓ，” Ｐｒｏｃ．２０１５ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ，ｐｐ．１９３５−１９４２，２０１５．）を用いる。これにより事前の三次元マップ生成を行い、実キーフレームを生成するものとする。

Ｅｎｇｅｌらの方法では、濃淡画像上の画素のうち輝度勾配が大きい画素のみ奥行き値がデプスマップＤに登録される。一方仮想キーフレームは、ユーザが複合現実感を体験する際に生成されるキーフレームである。仮想キーフレームの生成方法は後述する。

画像入力部２０は、ステレオカメラ５０が撮像する画像を位置姿勢導出部３０に入力する。本実施形態では、撮像画像に仮想物体を重畳してＨＭＤに表示するため、ステレオカメラ５０が撮像する画像はカラー画像であるとする。カラー画像が入力される場合には濃淡画像に変換する。なお、ロボットや自動車の自己位置推定のように撮像画像がカラーである必要がない場合には、入力する画像は濃淡画像であってもよい。

画像入力部２０は、ステレオカメラ５０から時系列（例えば毎秒３０フレーム）に画像を入力し、情報処理装置１は画像入力部２０から入力される各々のステレオ画像を撮像した時の世界座標系におけるステレオカメラの位置及び姿勢を計測する。以下では、情報処理装置１に入力された最新の画像を現フレームと呼ぶ。また、現フレームの直前に入力された画像を前フレームと呼ぶ。

ステレオカメラ５０を構成する二台のカメラの内部パラメータ（焦点距離ｆ_ｘ（画像の水平方向）、ｆ_ｙ（画像の垂直方向）、画像中心位置ｃ_ｘ（画像の水平方向）、ｃ_ｙ（画像の垂直方向）、レンズ歪みパラメータ）は既知であるとする。簡単のため、本実施形態では二台のカメラの内部パラメータは共通であるとする。

カメラの内部パラメータは、例えばＺｈａｎｇの方法（Ｚ．Ｚｈａｎｇ，“Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，” ＩＥＥＥ Ｔｒａｎｓ． ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．１１，ｐｐ．１３３０−１３３４，２０００．）によって事前に校正する。

また、ステレオカメラ５０を構成する二台のカメラ間の相対的な位置及び姿勢（以下、外部パラメータと呼ぶ）も既知であるとする。ステレオカメラの外部パラメータは、例えば三次元形状が既知のパターンを同時に左右のカメラで撮像してパターンを基準とした各カメラの位置及び姿勢を求め、夫々の位置及び姿勢をステレオカメラの左側カメラを基準とした位置及び姿勢に変換して求める。ステレオカメラ５０の内部パラメータと外部パラメータは不図示の記憶手段に保持されるものとする。

位置姿勢導出部３０は、画像入力部２０によって入力された画像、三次元情報保持部１０が保持する三次元マップに基づいて世界座標系におけるステレオカメラの位置及び姿勢を導出する。位置及び姿勢の導出方法については後述する。

三次元マップ更新部４０は、撮像画像及び位置姿勢導出部３０によって導出されたステレオカメラの位置及び姿勢を用いて仮想キーフレームを生成し、三次元マップに追加する。仮想キーフレームの生成方法については後述する。生成された仮想キーフレームは、予め三次元マップに保持されていた実キーフレームとともにステレオカメラの位置及び姿勢の導出に利用される。

画像合成部６０は、位置姿勢導出部３０が導出したステレオカメラの位置及び姿勢とステレオカメラの内部・外部パラメータを用いて仮想物体のＣＧ画像をレンダリングし、撮像されたステレオ画像上に重畳した画像を合成する。

表示部７０はＨＭＤであり、画像合成部６０が合成した画像を表示する。本実施形態ではＨＭＤはステレオＨＭＤであり、左右の表示デバイスに異なる画像を表示することでユーザによる現実空間と仮想物体の立体視が可能であるものとする。

これらの各機能部は、ＣＰＵ１１０が、ＲＯＭ１２０に格納されたプログラムをＲＡＭ１３０に展開し、後述する各フローチャートに従った処理を実行することで実現されている。また例えば、ＣＰＵ１１０を用いたソフトウェア処理の代替としてハードウェアを構成する場合には、ここで説明する各機能部の処理に対応させた演算部や回路を構成すればよい。

次に、本実施形態における位置及び姿勢の導出、三次元マップ更新の処理手順について説明する。図３は、本実施形態における位置及び姿勢の導出、三次元マップ更新の処理手順を示すフローチャートである。

ステップＳ１０００では、システムの初期化を行う。システムの初期化では、不図示の記憶装置から三次元マップを三次元情報保持部１０に読み込む。また、位置姿勢導出部３０は未計測フラグＦをＴＲＵＥに設定する。未計測フラグＦがＴＲＵＥであることはステレオカメラの位置及び姿勢が不明であることを意味する。

ステップＳ１０１０では、画像入力部２０は、ステレオカメラ５０が撮像したステレオ画像を入力する。本実施形態ではステレオカメラはカラー画像を撮像するため、入力されたカラー画像を濃淡画像に変換し、さらに左右のカメラの内部パラメータを用いてレンズ歪みの影響を除去した画像に変換する。

ステップＳ１０２０では、画像入力部２０によって入力されたステレオ画像、三次元情報保持部１０が保持する三次元マップに基づいてステレオカメラの位置及び姿勢を導出する。位置及び姿勢の導出方法については後述する。

ステップＳ１０３０では、必要に応じて仮想キーフレームを追加することで三次元マップの更新を行う。三次元マップの更新方法については後述する。

ステップＳ１０４０では、ステップＳ１０５０で導出した撮像装置の位置及び姿勢を用いて仮想物体のＣＧ画像をレンダリングし、ステレオ画像の各画像に重畳合成してＨＭＤの表示部に表示する。

ステップＳ１０５０では、システムを終了するか否かの判定を行う。マウスやキーボードなどを介してユーザからシステムを終了するコマンドが入力されている場合にはシステムを終了し、さもなければステップＳ１０１０に戻り位置及び姿勢の計測処理を継続する。
（位置及び姿勢の導出処理の詳細）
次に、ステップＳ１０２０における位置及び姿勢の導出処理について詳細に説明する。図４は、ステップＳ１０２０における処理手順を示すフローチャートである。

ステップＳ１１１０において未計測フラグＦがＴＲＵＥであればステップＳ１１２０に進んでリローカライゼーション（ｒｅｌｏｃａｌｉｚａｔｉｏｎ）を行い、さもなければステップＳ１１３０に進む。

ステップＳ１１２０では、リローカライゼーションを行う。入力された画像に基づいて初期化処理を行って撮像装置の位置及び姿勢を導出する。リローカライゼーションとは、位置及び姿勢に関する事前情報（例えば前フレームでの撮像装置の位置及び姿勢）なしに撮像装置の位置及び姿勢を求める処理のことである。リローカライゼーションはシステム起動時と、システム動作中に位置及び姿勢の計測に失敗した場合に行う。リローカライゼーションに成功したら、未計測フラグＦをＦＡＬＳＥに変更してステップＳ１１３０に進む。リローカライゼーションで得られる撮像装置の位置及び姿勢は概略の位置及び姿勢であることが多いため、さらにステップＳ１１３０以降で高精度な位置及び姿勢を導出する。リローカライゼーションの詳細については後述する。

ステップＳ１１３０では、位置姿勢導出部３０は、現フレームの位置及び姿勢の予測値に最も近い位置及び姿勢を保持するキーフレーム（以下、最近傍キーフレーム）を実キーフレーム、仮想キーフレームの中から選択する。このキーフレーム選択において、現フレームの位置及び姿勢の予測値としては、前フレームで導出された位置及び姿勢を用いる。なお、前フレームでの位置及び姿勢の導出に失敗し、現フレームでリローカライゼーションを行った場合は、リローカライゼーションで得られた位置及び姿勢を現在の位置及び姿勢の予測値とする。

本実施形態では、予測値の姿勢との差が閾値Ｔｒ以内の姿勢を保持するキーフレームのうち、予測値の位置との差が最小となる位置を保持するキーフレームを最近傍キーフレームとして選択する。本実施形態では、予測値の姿勢との差を、姿勢の予測値とキーフレームが保持する姿勢との相対姿勢を回転軸・回転角で表した時の回転角であるとする。予測値の姿勢との差がＴｒ以内の姿勢を保持するキーフレームがない場合は閾値Ｔｒを増やして再度選択する。閾値Ｔｒを増やしても見つからない場合、前フレームで用いたキーフレームを利用する。

ステップＳ１１４０では、位置姿勢導出部３０は、ステップＳ１１３０で選択したキーフレームから、位置及び姿勢の導出に利用する特徴点を選択する。ここで「特徴点」とは、選択されたキーフレームのデプスマップＤにおいて有効な値が登録されている画素のことを表すものとする。またこれらの特徴点のことを「キーフレーム上の特徴点」と呼ぶ。複合現実感システムには実時間処理が要求されるため、位置及び姿勢の導出に利用できる時間は限られている。また、位置及び姿勢の導出にかかる時間は位置及び姿勢の導出に利用する特徴点の個数に依存する。そのため、予め位置及び姿勢の導出に利用する特徴点の個数の最大値Ｐｍａｘを定めておき、選択したキーフレームから最大Ｐｍａｘ個の特徴点を選択することで実時間での位置及び姿勢の導出を行えるようにする。なお、キーフレーム上の特徴点の数がＰｍａｘより小さい場合には全ての特徴点を選択し利用することで、定められた最大の個数以内の特徴点を使用するようにする。

ステップＳ１１５０では、位置姿勢導出部３０は、ステップＳ１１４０で選択された特徴点を用いて位置及び姿勢の予測値の修正を行う。本実施形態では、前述のＥｎｇｅｌらの方法と同様の方法により位置及び姿勢の導出を行う。すなわち、導出対象である位置及び姿勢を用いて特徴点の画像上での位置を算出した時に、算出された位置の入力画像上の輝度値とキーフレームの濃淡画像上での輝度値との誤差が小さくなるように位置及び姿勢の予測値を修正する。

ステップＳ１１６０では、位置姿勢導出部３０は、位置及び姿勢導出の収束判定を行う。収束判定は、例えば前述の輝度値の誤差が十分小さくなったかどうかに基づき行う（例えば、誤差が閾値よりも小さくなったかどうかに基づいて判断する）。また、例えば一つ前の繰返しステップ後の誤差と現繰返しステップ後の誤差の差分値が閾値よりも小さくなったかどうかに基づいて判断してもよい。収束したと判断されたら図４のフローチャートの処理を終了する。収束していないと判断された場合、ステップＳ１１５０に処理を戻す。なお、ステップＳ１１６０では、さらに位置及び姿勢の導出の失敗判定を行ってもよい。位置及び姿勢の導出の失敗判定は、例えば位置及び姿勢の導出に用いる評価関数の値をもとに行う。位置及び姿勢の導出に失敗していると判断された場合には未計測フラグＦをＴＲＵＥに設定する。

（リローカライゼーション処理の詳細）
本実施形態では、Ｋｌｅｉｎらの方法（Ｇ．Ｋｌｅｉｎ，Ｄ．Ｍｕｒｒａｙ，“Ｉｍｐｒｏｖｉｎｇ ｔｈｅ Ａｇｉｌｉｔｙ ｏｆ Ｋｅｙｆｒａｍｅ−Ｂａｓｅｄ ＳＬＡＭ，” Ｐｒｏｃ．１０ｔｈ Ｅｕｒｏｐｅａｎ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｖｏｌ．２，ｐｐ．８０２−８１５，２００８．）によりリローカライゼーションを行う。

Ｋｌｅｉｎらの方法では、入力画像の各画素の輝度値と三次元マップが持つ各キーフレームの濃淡画像の各画素の輝度値とを比較し、最も差が小さいキーフレームが保持する撮像装置の位置及び姿勢を入力画像撮像時の撮像装置の位置及び姿勢とする。

本実施形態では、予め生成された実キーフレームだけでなく複合現実感体験時に生成された仮想キーフレームも比較の対象とすることで、リローカライゼーション可能な範囲を拡大する。なお、リローカライゼーションを高速に行うため、キーフレーム画像は予め所定のサイズに縮小しておき、入力画像も同じサイズに縮小して比較を行う。比較の尺度としては輝度差の二乗和を利用し、輝度差の二乗和が所定の閾値以下の場合にリローカライゼーションに成功したものとする。リローカライゼーションに失敗した場合には次に入力される画像を用いて再度リローカライゼーションを行う。

（三次元マップの更新処理の詳細）
次に、ステップＳ１０３０における三次元マップの更新方法について詳細に説明する。図５は、ステップＳ１０３０における処理手順を示すフローチャートである。

ステップＳ１２１０では、ステップＳ１０２０で導出された位置及び姿勢に最も近い位置及び姿勢を保持するキーフレーム（最近傍キーフレーム）を実キーフレームの中から選択する。最近傍キーフレームの選択はステップＳ１１３０と同一の方法により行う。

ステップＳ１２２０では、ステップＳ１０２０で導出された現フレーム撮像時の撮像装置の位置と最近傍キーフレーム撮像時の撮像装置の位置の差をもとに仮想キーフレームの追加の判断を行う。具体的には、ステップＳ１０２０で導出された位置と最近傍キーフレームとの位置との距離算出を行い、距離ｄｉｓｔが閾値Ｔｔ以上であればステップＳ１２４０に進み仮想キーフレームを追加する。さもなければステップＳ１２３０に進みステップＳ１２２０とは異なる指標で仮想キーフレーム追加の判断を行う。

ステップＳ１２３０では、最近傍キーフレームと現フレームの間に十分なオーバーラップがあるかに基づいて仮想キーフレームの追加判断を行う。具体的には、最近傍キーフレーム上の全ての特徴点を現フレームの画像に投影したときに、画像内部に投影される特徴点の割合算出を行い、割合が閾値Ｔｐ未満であれば、仮想キーフレームを追加すると判断する。ここで現フレームの画像とは、ステレオカメラの左側カメラの画像であるとする。

ここでは、最近傍キーフレーム上の各特徴点について次に示す処理を行うことで現フレームの画像内部に投影されるか判断する。処理対象の特徴点の最近傍キーフレーム上の画像座標を（ｕ_ｋ，ｖ_ｋ）とする。まず、デプスマップＤを参照して該特徴点の奥行きＺの逆数１／Ｚを読み出す。次に画像座標（ｕ_ｋ，ｖ_ｋ）とカメラの内部パラメータ（ｆ_ｘ、ｆ_ｙ、ｃ_ｘ、ｃ_ｙ）を用いて該特徴点の最近傍キーフレームの撮像装置座標系における三次元座標（Ｘ_ｋ，Ｙ_ｋ，Ｚ_ｋ）を算出する。（Ｘ_ｋ，Ｙ_ｋ，Ｚ_ｋ）は数１により算出する。

次に、数２により最近傍キーフレームの撮像装置座標系における三次元座標（Ｘ_ｋ，Ｙ_ｋ，Ｚ_ｋ）を現フレームの撮像装置座標系における三次元座標（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）に変換する。

ここでｔ_ｗｋ、Ｒ_ｗｋはそれぞれ世界座標系における最近傍キーフレームの位置を表す３次元ベクトル、姿勢を表す３×３回転行列である。また、ｔ_ｗｃ、Ｒ_ｗｃはそれぞれ世界座標系における現フレーム撮像時の撮像装置の位置を表す３次元ベクトル、姿勢を表す３×３回転行列である。次に、数３により現フレームの撮像装置座標系における三次元座標（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）を現フレームの画像座標（ｕ_ｃ，ｖ_ｃ）に変換する。

このようにして算出される画像座標（ｕ_ｃ，ｖ_ｃ）が現フレームの画像内部にある特徴点の割合Ｒｐを算出する。割合Ｒｐが閾値Ｔｐ未満の場合には最近傍キーフレームと現フレームとの間のオーバーラップが不十分であるため、位置及び姿勢計測の精度や安定性が低下しないように、ステップＳ１２４０に進んで仮想キーフレームを追加する。さもなければ処理を終了する。なおステップＳ１２３０の処理は、ステップＳ１２２０の処理よりも時間がかかるため、ステップＳ１２２０で仮想キーフレームを追加しないと判断した場合にのみステップＳ１２３０で追加判断を行っている。

ステップＳ１２４０では、仮想キーフレームの生成を行う。ステップＳ１２４０における仮想キーフレームの生成処理の詳細については後述する。

ステップＳ１２５０では、生成した仮想キーフレーム（現フレームの濃淡画像、デプスマップ、分散マップ、画像撮像時の撮像装置の位置及び姿勢）を三次元情報保持手段１０が保持する三次元マップに追加し、図５のフローチャートの処理を終了する。

（仮想キーフレーム生成処理の詳細）
図６は仮想キーフレームの生成（ステップＳ１２４０）の処理手順を示すフローチャートである。ステップＳ１２１０で選択された最近傍キーフレームが保持する特徴点の数をＮｐとする。

ステップＳ１３１０では、仮想キーフレームの初期化を行う。具体的には、仮想キーフレームが保持するデプスマップの各画素の奥行きの逆数の値を０に、分散マップの各画素の奥行きの逆数の分散の値を所定のデフォルト値に設定する。ステップＳ１３２０では、最近傍キーフレーム上の特徴点を指定する変数ｉを１にセットする。

ステップＳ１３３０では、最近傍キーフレームのｉ番目の特徴点の現フレームの撮像装置座標系における三次元座標（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）及び現フレームの画像座標（ｕ_ｃ，ｖ_ｃ）をステップＳ１２３０で述べた方法により座標算出を行う。なお、ステップＳ１２３０で算出した値を保持しておき、ステップＳ１３３０ではその値を流用してもよい。

ステップＳ１３４０では、Ｓ１３３０で画像座標を算出した特徴点を仮想キーフレームに追加するか否かの判定を行う。本実施形態では
（１）（ｕ_ｃ，ｖ_ｃ）が現フレームの画像内部にあり、
（２）かつ奥行きＺ_ｃが予め設定される奥行きの上限値・下限値の間にあり、
（３）かつ現フレームの画像座標（ｕ_ｃ，ｖ_ｃ）における特性が所定の条件を満たす
場合に特徴点を仮想キーフレームに追加する。

本実施形態では、前述の特性として画像上での輝度値を利用する。ここでは、処理対象の特徴点の最近傍キーフレームの濃淡画像上での輝度値と、現フレームの濃淡画像の画像座標（ｕ_ｃ，ｖ_ｃ）における輝度値の差が予め決められた閾値Ｔｉ未満であることを（３）の条件とする。

特徴点の画像座標（ｕ_ｃ，ｖ_ｃ）が整数値である場合には該当する画素の輝度値を比較に利用する。画像座標が整数値でない場合には、周辺の画素の輝度値から線形補間により算出した輝度値を比較に利用する。本ステップで特徴点を追加すると判断された場合、ステップＳ１３５０に進み、さもなければステップＳ１３６０に進む。

ステップＳ１３５０では、仮想キーフレーム画像に特徴点を追加する。まず、現フレームにおける画像座標（ｕ_ｃ，ｖ_ｃ）を四捨五入により整数化（（ｕ_ｃ’，ｖ_ｃ’））し、該当する画素の奥行きの逆数の値Ｄ（ｕ_ｃ’，ｖ_ｃ’）を参照する。

（Ａ）Ｄ（ｕ_ｃ’，ｖ_ｃ’）が０の場合には、ステップＳ１３３０で算出した現フレームの撮像装置座標系を基準とした奥行き値Ｚ_ｃの逆数をＤ（ｕ_ｃ’，ｖ_ｃ’）に登録する。また分散マップのＶ（ｕ_ｃ’，ｖ_ｃ’）には最近傍キーフレームにおける該特徴点の分散から算出される分散を登録する。

（Ｂ）Ｄ（ｕ_ｃ’，ｖ_ｃ’）が０以外の場合、すなわち別の特徴点によって該当画素のデプスマップ、分散マップが更新されている場合、以下のようにする。すなわち１／Ｚ_ｃがＤ（ｕ_ｃ’，ｖ_ｃ’）の１σの範囲内にある場合には確率的に同じ点とみなして分散の逆数を重みとした１／Ｚ_ｃの加重平均をＤ（ｕ_ｃ’，ｖ_ｃ’）に登録する。Ｖ（ｕ_ｃ’，ｖ_ｃ’）にも同様に分散の加重平均を登録する。１／Ｚ_ｃが１σの範囲外でかつＤ（ｕ_ｃ’，ｖ_ｃ’）より大きい（すなわち手前にある）場合には、１／Ｚ_ｃでＤ（ｕ_ｃ’，ｖ_ｃ’）を上書きする。Ｖ（ｕ_ｃ’，ｖ_ｃ’）には最近傍キーフレームにおける該特徴点の分散から算出される分散を登録する。

ステップＳ１３６０では、ｉに１を加算しステップＳ１３７０に進む。ステップＳ１３７０では、ｉがＮｐより大きい場合には全ての特徴点に対する処理を完了したと判断し、ステップＳ１３８０に進む。さもなければステップＳ１３３０に戻り仮想キーフレームの生成処理を継続する。

以上述べたように第１の実施形態では、複合現実感の体験中に撮像装置によって撮像された画像を仮想キーフレームとして三次元マップに追加することで、精度や安定性を損なわずに位置及び姿勢の計測が可能な範囲を広げることができる。

（第１の実施形態バリエーション）
なお、画像を撮像する撮像装置はステレオカメラに限るものではなく、現実空間の画像を撮像するカメラであれば何でもよい。例えば、単眼カメラであってもよいし、リジッドに固定されてカメラ間の相対的な位置及び姿勢が既知な三台以上のカメラであってもよい。また、撮像装置はカラー画像を撮像するカメラに限るものではなく、濃淡画像を撮像するカメラであってもよい。また、特徴点を仮想キーフレームに追加するか否かの判定に用いる特性は輝度値に限るものではなく、撮像装置がカラー画像を撮像するカメラである場合にはカラー値を比較することにより特徴点の追加の判断を行ってもよい。

キーフレーム上の特徴点は、位置及び姿勢の導出に利用可能であれば何でもよい。例えば、キーフレーム上の特徴点は濃淡画像上の画素のうち輝度勾配が大きい画素であってもよい。また、例えば、Ｎｅｗｃｏｍｂｅらの方法（Ｒ．Ａ．Ｎｅｗｃｏｍｂｅ，Ｓ．Ｊ．Ｌｏｖｅｇｒｏｖｅ，Ａ．Ｊ．Ｄａｖｉｓｏｎ，“ＤＴＡＭ：Ｄｅｎｓｅ Ｔｒａｃｋｉｎｇ ａｎｄ Ｍａｐｐｉｎｇ ｉｎ Ｒｅａｌ−Ｔｉｍｅ，” Ｐｒｏｃ． １３ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｐｐ．２３２０−２３２７，２０１１．）のようにしてもよい。すなわち、キーフレーム上の全画素を特徴点としてもよい。

位置及び姿勢計測のリローカライゼーション方法は、位置及び姿勢の事前情報なしに撮像装置の位置及び姿勢の方法を計測するものであれば、撮像画像とキーフレームとの画像マッチングに限るものではない。例えば、撮像画像上で検出される特徴点毎にキーフレーム上で検出される特徴点とマッチングを行い、得られる特徴点の対応を用いて初期化を行ってもよい。また、撮像画像上で検出される特徴点や領域を複数のクラスに分類し、各クラスに分類された特徴点や領域の頻度をもとに類似したキーフレームを選択することで初期化を行ってもよい。

第１の実施形態では、現フレームを用いた三次元マップの更新処理が終わってから次フレームの位置及び姿勢の導出する逐次的な処理を行っていた。しかしながら、三次元マップ更新は必ずしも逐次的に行う必要はなく、次フレームの位置及び姿勢の導出と並行して行ってもよい。具体的には、各フレームの位置及び姿勢の導出を行うスレッドと三次元マップの更新を行うスレッドを分けて並列に処理を行い、位置及び姿勢の導出はその時点で得られる最新の三次元マップを用いて行ってもよい。

第１の実施形態では、デプスマップには奥行き値の逆数、分散マップには奥行き値の逆数の分散を保持していた。しかしながら、デプスマップに保持するデータは、キーフレームの撮像座標系における三次元座標に変換可能であれば、いかなるデータであってもよい。例えば、デプスマップに保持するデータは奥行きであってもよいし、キーフレームの撮像座標系における三次元座標を直接保持してもよい。

第１の実施形態では、仮想キーフレームを生成する際に選択するキーフレームは実キーフレームのみであった。しかしながら、選択するキーフレームはキーフレームであれば何でもよく、実キーフレームであっても、仮想キーフレームであってもよい。但し、位置及び姿勢の精度低下を抑えるため、仮想キーフレームには最近傍キーフレームとして選択したキーフレームの種類を属性として保持させる。

そして、最近傍キーフレームが仮想キーフレームである仮想キーフレームは、他の仮想キーフレームの最近傍キーフレームとして選択されないようにしてもよい。それ以外にも、最近傍キーフレームの最近傍キーフレームが仮想キーフレームであるキーフレームは最近傍キーフレームとして選択しないなど、仮想キーフレームの世代を考慮してもよい。

仮想キーフレーム上の特徴点の位置及び姿勢の計測における寄与度は、特徴点が仮想キーフレームに追加されるための条件を満たした特徴点の重みは大きく、満たしていない特徴点の重みは小さくなるようにするものであれば何でもよい。例えば第１の実施形態のように、特徴点が仮想キーフレームに追加されるための条件を満たさない場合には仮想キーフレームに登録しないようにしてもよい。また、最近傍キーフレームと現フレームにおける該特徴点の輝度差が大きいほど重みを小さくするようにしてもよい。

第１の実施形態では、現フレームの位置及び姿勢の予測値として前フレームの位置及び姿勢を用いた。しかしながら、現フレームの位置及び姿勢の予測値は、実際の現フレームの位置及び姿勢に近い値であれば何でもよい。例えば、等速度運動、等加速度運動などの運動モデルから予測される位置及び姿勢であってもよい。また、位置や位置変化を計測するセンサや、姿勢や姿勢変化を計測するセンサなどのカメラ以外のセンサによって計測される位置や位置変化、姿勢や姿勢変化から予測してもよい。

（第２の実施形態）
第１の実施形態では、特徴点を仮想キーフレームに追加すべきかどうか判断する際に最近傍キーフレーム、仮想キーフレームにおける特徴点の輝度値の差を用いることで光学的に整合する特徴点のみを仮想キーフレームに追加するようにしていた。第２の実施形態では、撮像装置がステレオカメラであることを利用し、特徴点の奥行きの差を用いることで幾何的に整合する特徴点のみを仮想キーフレームに追加できるようにする。これによって、仮想キーフレームを用いた場合の位置及び姿勢の計測を高精度化・安定化が可能になる。

第２の実施形態における情報処理装置の構成は第１の実施形態で説明した情報処理装置１と同一であるため説明を省略する。また、第１の実施形態と第２の実施形態では図６におけるステップＳ１３４０における処理だけが異なるため、以下ではステップＳ１３４０についてのみ説明する。

第２の実施形態におけるステップＳ１３４０では、
（１）（ｕ_ｃ，ｖ_ｃ）が現フレームの画像内部にあり、
（２）かつ奥行きＺ_ｃ予め設定される奥行きの上限値・下限値の間にあり、
（３）かつ現フレームの画像座標（ｕ_ｃ，ｖ_ｃ）における特性が所定の条件を満たす
場合に特徴点を仮想キーフレームに追加する。

第２の実施形態では、前述の特性として特徴点の奥行きを利用する。特徴点の奥行きはステレオカメラが撮像するステレオ画像をもとに算出する。具体的には、現フレームの左側カメラの画像における画像座標（ｕ_ｃ，ｖ_ｃ）に対応する右側カメラの画像上の点（ｕ_Ｒ，ｖ_Ｒ）を探索する。画像座標（ｕ_ｃ，ｖ_ｃ）とステレオカメラの内部・外部パラメータから算出される右側カメラの画像上のエピポーラ線上において、Ｎ×Ｎ画素領域の輝度の二乗誤差の和が最も小さい点を探索することにより対応点の座標（ｕ_Ｒ，ｖ_Ｒ）が得られる。

さらに（ｕ_ｃ，ｖ_ｃ）、（ｕ_Ｒ，ｖ_Ｒ）、ステレオカメラの内部・外部パラメータをもとに、三角測量によって現フレームの撮像装置座標系における特徴点の奥行きＺ_ｃ’を求める。このようにして得られた特徴点の奥行きＺ_ｃ’と、最近傍キーフレームにおける特徴点の現フレームの撮像装置座標系における奥行きＺ_ｃとの差が予め決められた閾値Ｔｄ未満であることを（３）の条件とする。本ステップで特徴点を追加すると判断された場合、ステップＳ１３５０に進み、さもなければステップＳ１３６０に進む。

以上述べたように、第２の実施形態では、複合現実感の体験中に撮像装置によって撮像された画像を仮想キーフレームとして三次元マップに追加することで、精度や安定性を損なわずに位置及び姿勢の計測が可能な範囲を広げることができる。さらに、最近傍キーフレームに保持されている奥行きと現フレームで得られる奥行きの差が小さい点のみを仮想キーフレームに追加することで、仮想キーフレームを用いた場合の位置及び姿勢の計測を高精度化・安定化が可能になる。

（第２の実施形態バリエーション）
第２の実施形態では、特徴点の奥行きの差が小さい点を仮想キーフレームに追加した。しかしながら、仮想キーフレームに追加する特徴点の選択方法はこれに限るものではなく、特徴点の奥行きの差が小さくかつ第１の実施例で述べたように輝度値の差が小さい特徴点を仮想キーフレームに追加してもよい。この場合、現フレームにおける特徴点の奥行きＺ_ｃ’と、最近傍キーフレームにおける特徴点の現フレームの撮像装置座標系における奥行きＺ_ｃとの差が閾値Ｔｄ未満であり、かつ輝度値の差が閾値Ｔｉ未満である点を仮想キーフレームに追加する。

第２の実施形態では撮像装置としてステレオカメラの利用を想定した。しかしながら、利用する撮像装置はステレオカメラに限るものではなく、特徴点の奥行きを計測できる撮像装置であれば他の撮像装置であってもよい。例えば、デプス画像を撮像するデプスカメラであってもよいし、カラー画像とデプス画像を取得できるＲＧＢＤカメラであってもよい。さらには、デプス画像ではなく計測対象の表面の点群の三次元座標を計測するＬｉＤＡＲなどの三次元計測装置を利用してもよい。また、濃淡画像やカラー画像を撮影するカメラに、デプスカメラや三次元計測装置を固定した撮像装置であってもよい。

（第３の実施形態）
以上述べた実施形態では、仮想キーフレームを生成する時に最近傍のキーフレームをもとに生成していた。第３の実施形態では、特徴点データベースに保持される特徴点の世界座標系における三次元座標を直接現フレームの三次元座標に変換することで仮想キーフレームを生成する。これによって、最近傍キーフレームの位置及び姿勢の計測誤差の影響を受けずに仮想キーフレームを生成することで、位置及び姿勢計測の精度低下を防止する。

第３の実施形態における情報処理装置の構成は第１の実施形態で説明した情報処理装置１と同一であるため説明を省略する。また、第１、第２の実施形態と第３の実施形態では、三次元情報保持部１０の三次元マップの保持方法と、図６におけるステップＳ１３２０、Ｓ１３３０の処理だけが異なる。以下では三次元マップの保持方法と、第３の実施形態におけるステップＳ１３２０、Ｓ１３３０の処理手順について詳細に説明する。

（三次元マップの保持方法）
三次元情報保持部１０は、キーフレームの集合に加えて特徴点の世界座標系における三次元座標を保持する特徴点データベースを保持する。特徴点データベースは、例えばＦｕｒｕｋａｗａらの方法（Ｙ．Ｆｕｒｕｋａｗａ，Ｂ．Ｃｕｒｌｅｓｓ，Ｓ．Ｍ．Ｓｅｉｔｚ，Ｒ．Ｓｚｅｌｉｓｋｉ，“Ｒｅｃｏｎｓｔｒｕｃｔｉｎｇ Ｂｕｉｌｄｉｎｇ Ｉｎｔｅｒｉｏｒｓ ｆｒｏｍ Ｉｍａｇｅｓ，” Ｐｒｏｃ． １２ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｐｐ．８０−８７，２００９．）によって生成する。

Ｆｕｒｕｋａｗａらの方法では多数の画像を入力として特徴点ベースのＳｔｒｕｃｔｕｒｅ−ｆｒｏｍ−ｍｏｔｉｏｎを行い、さらにＭｕｌｔｉ−Ｖｉｅｗ Ｓｔｅｒｅｏを行うことで密な三次元復元を行っている。これらの復元された密な三次元点を三次元座標とともに特徴点データベースに登録する。

（仮想キーフレーム生成）
第３の実施形態では、特徴点データベースに登録されている特徴点の数をＮｐとする。ステップＳ１３２０では、特徴点データベース中の特徴点を指定する変数ｉを１にセットする。

ステップＳ１３３０では、特徴点データベース中のｉ番目の特徴点の現フレームの撮像装置座標系における三次元座標（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）及び現フレームの画像座標（ｕ_ｃ，ｖ_ｃ）を算出する。まず、数４により該特徴点の世界座標系における三次元座標（Ｘ_ｗ，Ｙ_ｗ，Ｚ_ｗ）を現フレームの撮像装置座標系における三次元座標（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）に変換する。

前述したように、ｔ_ｗｃ、Ｒ_ｗｃはそれぞれ世界座標系における現フレーム撮像時の撮像装置の位置を表す３次元ベクトル、姿勢を表す３×３回転行列である。次に数３により現フレームの撮像装置座標系における三次元座標（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）を現フレームの画像座標（ｕ_ｃ，ｖ_ｃ）に変換する。

以上述べたように、第３の実施形態では、複合現実感の体験中に撮像装置によって撮像された画像を仮想キーフレームとして三次元マップに追加することで、精度や安定性を損なわずに位置及び姿勢の計測が可能な範囲を広げることができる。さらに、第３の実施形態では、特徴点の世界座標系における三次元座標を保持する特徴点データベースをもとに仮想キーフレームを生成することで、キーフレームの位置及び姿勢の計測誤差による精度低下を防止できる。

（第３の実施形態バリエーション）
三次元マップにさらにシーンのメッシュモデルを保持してもよい。メッシュモデルを保持する場合、仮想キーフレームへの特徴点追加可否の判定の際に特徴点の可視判定を行い、該特徴点がメッシュに隠蔽されないことを追加可能な条件として利用してもよい。

特徴点データベースは必ずしも多数の画像をもとに生成しなくてもよく、特徴点の三次元座標を計測可能であれば何でもよい。例えば、ＬｉＤＡＲなどの三次元スキャナによって直接シーンの三次元計測を行ってもよい。また、三次元計測を行わずに設計データをもとに生成してもよい。さらには、カメラによって撮影された画像を三次元計測や設計データから得られる三次元データにテクスチャマッピングして生成される三次元モデルから特徴点データベースを生成してもよい。

（変形例）
以上述べた実施形態では、仮想キーフレームは単一のキーフレーム（最近傍キーフレーム）を用いて生成した。しかしながら、仮想キーフレームの生成に用いるキーフレームは一つである必要はなく、複数のキーフレームを使って生成してもよい。

図７は本変形例における仮想キーフレーム生成の処理手順を示すフローチャートである。このフローチャートは第１の実施形態における図６に相当する。

ステップＳ４３１０、Ｓ４３２０、Ｓ４３４０、Ｓ４３５０、Ｓ４３６０、Ｓ４３７０は図６のステップＳ１３１０、Ｓ１３２０、Ｓ１３４０、Ｓ１３５０、Ｓ１３６０、Ｓ１３７０と同一であるので説明を省略する。

ステップＳ４３１５では、ステップＳ１０５０で導出された位置及び姿勢に近い位置及び姿勢を保持する実キーフレーム（近傍キーフレーム）を一つ以上選択する。ここでは、位置の差が閾値Ｔｓ未満でかつ第１の実施形態のステップＳ１２３０で説明した方法により現フレームに投影されるキーフレーム上の特徴点がＮｓ個以上であるキーフレームを選択する。本変形例では、選択されたキーフレーム上の特徴点の総数をＮｐとする。

ステップＳ４３３０では、各近傍キーフレームの各特徴点の現フレームの撮像装置座標系における三次元座標（Ｘ_ｃ，Ｙ_ｃ，Ｚ_ｃ）及び現フレームの画像座標（ｕ_ｃ，ｖ_ｃ）をステップＳ１２３０で述べた方法により算出する。但し、最近傍キーフレームの位置及び姿勢ではなく、各近傍キーフレームの位置及び姿勢を用いて算出する。

以上述べたように、本変形例では仮想キーフレームを複数のキーフレームを使って生成する。これによって、既存のキーフレームの間にキーフレームを生成する場合であっても十分な特徴点を追加することが可能になるため、位置及び姿勢の計測を高精度化・安定化することができる。

（変形例）
以上述べた実施形態における情報処理装置は、さらに三次元マップ表示部を備えていてもよい。三次元マップ表示部は、表示部７０であるユーザが装着するＨＭＤでもよいし、他のディスプレイであってもよい。三次元マップ表示部は、三次元マップに含まれるキーフレームの情報を表示する。

図９は、三次元マップ表示部が表示する画面の一例を示す図である。図９に示すように、画面には三次元マップに含まれるキーフレームを表す仮想物体とキーフレーム上の特徴点が表示される。キーフレームを表す仮想物体は、現実空間におけるキーフレーム撮像時の撮像装置の位置及び姿勢を表すように表示され、実キーフレームと仮想キーフレームでは色を変えて表示される。

また、現フレームで位置及び姿勢の導出に使われた最近傍キーフレームはさらに別の色で表示してもよい。さらには、現フレーム撮像時の撮像装置の位置及び姿勢を表す仮想物体を表示してもよい。キーフレーム上の特徴点の三次元座標は現実空間に規定される世界座標系における三次元座標に変換し、世界座標系における点として描画する。描画される特徴点の色として、撮像画像上の色や輝度を直接利用してもよいし、キーフレームの撮像装置座標系における奥行き（Ｚ座標）を色に変換した値を利用してもよい。描画される特徴点は、現フレームの最近傍キーフレームなど一部のキーフレーム上の特徴点だけでもよいし、全てのキーフレーム上の特徴点でもよい。さらには、仮想キーフレームと仮想キーフレームの生成に用いた近傍／最近傍キーフレームとの接続関係を仮想物体の位置を結ぶ線分によって表してもよい。

以上説明した実施形態によれば、事前に用意されたキーフレームに加えて撮像装置によって撮像された画像を仮想キーフレーム画像として三次元マップに追加することで、精度や安定性を損なわずに位置及び姿勢の計測が可能な範囲を広げることができる。

また、仮想キーフレームに特徴点を追加する際に、特徴点の画像上の輝度や距離を比較することで、光学的・幾何的な整合性の高い特徴点のみを追加することで、高精度かつ安定した撮像装置の位置及び姿勢の計測が可能になる。

さらに、特徴点の世界座標系における三次元座標を保持する特徴点データベースをもとに仮想キーフレームを生成することで、近傍キーフレームの位置及び姿勢の計測誤差に起因する位置及び姿勢計測の精度低下を防止できる。

上述した実施形態における三次元情報保持部が保持する三次元マップは、撮像画像、画像撮像時の撮像装置の位置及び姿勢、現実空間中の幾何特徴の三次元情報から構成されるキーフレーム群によって表されるものであれば何でもよい。例えば、現実空間中の幾何特徴の三次元情報は、現実空間を基準とした座標系における三次元座標であってもよいし、撮像時の現実空間における位置及び姿勢が既知な撮像装置を基準とした座標における三次元座標であってもよい。

上述した実施形態における画像入力部が入力する画像を撮像するカメラは、現実空間の画像を撮像するカメラであれば何でもよい。例えば、濃淡画像を撮影するカメラでもよいし、カラー画像を撮影するカメラであってもよい。さらには、カメラは１台であってもよいし、ステレオカメラなどの複数台のカメラによって構成される撮像装置であってもよい。

上述した実施形態における位置姿勢導出部は、入力画像と三次元マップをもとに入力画像撮像時のカメラの位置及び姿勢を導出するものであれば何でもよい。例えば、輝度値の差を評価関数として位置及び姿勢を導出するものでもよいし、特徴点の画像上の位置の差を評価関数として位置及び姿勢を導出するものでもよい。

上述した実施形態における三次元マップ更新部は、撮像時の撮像装置の位置及び姿勢を位置姿勢導出部が位置及び姿勢を導出した画像を用いて仮想キーフレームを生成し三次元マップに加えるものであれば何でもよい。例えば、導出された位置及び姿勢をもとに、既存の三次元マップ中の特徴点の三次元座標を変換することによって仮想キーフレーム上の特徴点の三次元座標を算出してもよい。既存の三次元マップ中の特徴点の三次元座標は、現実空間に設定された世界座標系を基準としたものでもよいし、キーフレーム撮像時の撮像装置の座標系を基準としたものでもよい。

また、導出された位置及び姿勢をもとに算出される入力画像上の位置を算出し、入力画像上の属性に基づいて特徴点の位置及び姿勢導出における寄与度を決定してもよい。入力画像上の属性として、輝度を用いてもよいし距離を用いてもよい。

（その他の実施形態）
以上、実施形態を詳述したが、本発明は、例えば、システム、装置、方法、コンピュータ読み取り可能なプログラムもしくは記憶媒体等としての実施態様をとることが可能である。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。

更に、本発明は、ソフトウェアのプログラムをシステム或いは装置に直接或いは遠隔から供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムコードを読み出して実行することによって前述した実施形態の機能が達成される場合を含む。この場合、供給されるプログラムは実施形態で図に示したフローチャートに対応したコンピュータプログラムである。

また、コンピュータが、読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているＯＳなどとの協働で実施形態の機能が実現されてもよい。この場合、ＯＳなどが、実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される。

１０ 三次元情報保持部
２０ 画像入力部
３０ 位置姿勢導出部
４０ 三次元マップ更新部
５０ 撮像装置
６０ 画像合成部
７０ 表示部

Claims (14)

1. 撮像された画像と、該画像を撮像したときの撮像装置の位置及び姿勢と、該画像に対応する空間中の特徴の三次元情報とを対応付けた、複数の参照データから構成される三次元マップを予め保持する保持手段と、
   撮像装置により撮像された画像を入力する入力手段と、
   前記複数の参照データと、前記入力された画像とに基づいて、該画像を前記撮像装置が撮像した位置及び姿勢を導出する導出手段と、
   導出された前記位置及び姿勢と前記複数の参照データとに基づいて、新たな参照データを追加するかを判定する判定手段と、
   前記判定手段により新たな参照データを追加すると判定された場合に、前記入力された画像と、導出された前記位置及び姿勢と、前記複数の参照データの１つにおける特徴の三次元データとに基づいて新たな参照データに登録する特徴点を決定する決定手段と、
   前記決定された特徴点を登録した前記新たな参照データを追加して前記複数の参照データを更新する更新手段と、を備える情報処理装置。
2. 前記導出手段は、
   前記撮像装置の現在の位置及び姿勢の予測値を求める予測手段と、
   前記複数の参照データから前記予測値に基づいて少なくとも１つの参照データを選択する選択手段と、
   選択された前記参照データにおける特徴点を用いて前記予測値を修正することで、前記導出される位置及び姿勢を求める修正手段と、を備える請求項１に記載の情報処理装置。
3. 前記予測手段は、前フレームにおける前記撮像装置の位置及び姿勢が得られていれば、当該前フレームにおける位置及び姿勢を前記予測値とする請求項２に記載の情報処理装置。
4. 前記予測手段は、前フレームにおける前記撮像装置の位置及び姿勢が得られていなければ、リローカライゼーションを行って前記予測値を求める請求項２に記載の情報処理装置。
5. 前記修正手段は、選択された前記参照データから所定の個数以内の特徴点を選択し、該選択された特徴点を用いて前記予測値を修正する請求項２に記載の情報処理装置。
6. 導出された前記撮像装置の位置と、選択された前記参照データにおける前記撮像装置の位置との距離を算出する距離算出手段を備え、
   前記判定手段は、前記距離に基づいて、前記新たな参照データを追加するか否かを判定する請求項２に記載の情報処理装置。
7. 選択された前記参照データにおける特徴点を前記入力された画像に投影した場合に、画像内部に投影される割合を算出する割合算出手段を備え、
   前記判定手段は、前記割合に基づいて、前記新たな参照データを追加するか否かを判定する請求項２に記載の情報処理装置。
8. 前記決定手段は、
   選択された前記参照データにおいて特徴点を順に指定する指定手段と、
   指定された特徴点の前記撮像装置を基準とした座標系における現フレームの三次元座標及び画像座標を算出する座標算出手段と、
   前記三次元座標及び画像座標に基づいて前記指定された特徴点を追加するか否かを判定する特徴点判定手段と、
   追加すると判定された前記特徴点を前記参照データに追加する追加手段とを備える請求項２に記載の情報処理装置。
9. 前記選択手段は、前記複数の参照データから２以上の参照データを順次選択し、
   前記修正手段は、順次選択された参照データにおける特徴点を用いて前記予測値を順次修正することで、前記導出される位置及び姿勢を求める請求項２に記載の情報処理装置。
10. 前記参照データは前記特徴の属性を更に含み、
    導出された前記位置及び姿勢と、選択された前記参照データにおける特徴の三次元情報とに基づいて、該特徴の前記入力された画像上での位置を算出する位置算出手段と、前記特徴の属性と前記入力された画像上での位置の属性とを比較して、生成される前記参照データの寄与度を決定する決定手段とを更に備え、
    前記導出手段は、前記更新手段により更新された複数の参照データと、決定された前記寄与度とを用いて、前記撮像装置の位置及び姿勢を導出する請求項１に記載の情報処理装置。
11. 前記特徴の属性は輝度または色を含む請求項１０に記載の情報処理装置。
12. 前記特徴の属性は前記撮像装置を基準とした距離を含む請求項１０に記載の情報処理装置。
13. 撮像された画像と、該画像を撮像したときの撮像装置の位置及び姿勢と、該画像に対応する空間中の特徴の三次元情報とを対応付けた、複数の参照データから構成される三次元マップを予め保持する保持手段を備えた情報処理装置における情報処理方法であって、
    撮像装置により撮像された画像を入力する入力工程と、
    前記複数の参照データと、前記入力された画像とに基づいて、該画像を前記撮像装置が撮像した位置及び姿勢を導出する導出工程と、
    導出された前記位置及び姿勢と前記複数の参照データとに基づいて、新たな参照データを追加するかを判定する判定工程と、
    新たな参照データを追加すると判定された場合に、前記入力された画像と、導出された前記位置及び姿勢と、前記複数の参照データの１つにおける特徴の三次元データとに基づいて新たな参照データに登録する特徴点を決定する決定工程と、
    前記決定された特徴点を登録した前記新たな参照データを追加して前記複数の参照データを更新する更新工程と、を備える情報処理方法。
14. コンピュータを、
    撮像された画像と、該画像を撮像したときの撮像装置の位置及び姿勢と、該画像に対応する空間中の特徴の三次元情報とを対応付けた、複数の参照データから構成される三次元マップを予め保持する保持手段と、
    撮像装置により撮像された画像を入力する入力手段と、
    前記複数の参照データと、前記入力された画像とに基づいて、該画像を前記撮像装置が撮像した位置及び姿勢を導出する導出手段と、
    導出された前記位置及び姿勢と前記複数の参照データとに基づいて、新たな参照データを追加するかを判定する判定手段と、
    前記判定手段により新たな参照データを追加すると判定された場合に、前記入力された画像と、導出された前記位置及び姿勢と、前記複数の参照データの１つにおける特徴の三次元データとに基づいて新たな参照データに登録する特徴点を決定する決定手段と、
    前記決定された特徴点を登録した前記新たな参照データを追加して前記複数の参照データを更新する更新手段として機能させるプログラム。

Images