JP6762913B2 - 情報処理装置、情報処理方法 - Google Patents

Description

本発明は、撮像装置の位置及び姿勢の取得技術に関するものである。

画像を用いてシーンの三次元マップ作成とカメラの位置及び姿勢計測とを同時に行うSLAM（Simultaneous Localization and Mapping）技術がある。この技術は、複合／拡張現実感（MR／AR）における現実空間と仮想物体との位置合わせやロボットの自己位置推定等に利用される。

画像を用いるSLAM技術では、計測されたカメラの位置及び姿勢をもとに逐次三次元マップを更新・拡張する。画像をもとに計測されるカメラの位置及び姿勢には誤差が含まれるため、三次元マップも誤差を含んだものとなる。さらに、カメラの位置及び姿勢は誤差を含んだ三次元マップに基づいて計測されるため、カメラの位置及び姿勢にさらに誤差が含まれることになり、その結果、三次元マップには大きな誤差が蓄積される。これに対し、非特許文献１では、カメラがほぼ同じ位置に戻ってきたこと（ループ・クロージャー、loop closure）を画像の類似性を用いて認識し、該認識の結果をもとに三次元マップを補正することで蓄積誤差を低減している。

J. Engel, J. Stuckler, and D. Cremers, "Large-Scale Direct SLAM with Stereo Cameras," Proc. 2015 IEEE／RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1935-1942, 2015.

非特許文献１で開示される方法では、カメラの移動範囲が限られている場合には蓄積誤差が十分低減されないため、作成される三次元マップの精度が低いという課題がある。本発明はこのような問題に鑑みてなされたものであり、カメラ等の撮像装置の移動範囲が限られている場合であっても三次元マップに蓄積する誤差を低減させるための技術を提供する。

本発明の一様態は、撮像装置により撮像されるシーンの三次元情報と該撮像装置の位置及び姿勢とを対応付けた三次元マップを保持する保持手段と、
前記三次元マップと前記撮像装置により撮像された画像とに基づいて該画像の撮像時における前記撮像装置の第１の位置及び姿勢を導出する第１の導出手段と、
前記撮像された画像と前記導出された第１の位置及び姿勢とに基づいて前記三次元マップを更新する更新手段と、
前記第１の位置及び姿勢と、該第１の位置及び姿勢とは異なる手法または時点で導出された前記撮像装置の第２の位置及び姿勢との間の相対的な位置及び姿勢を導出する第２の導出手段と、
前記相対的な位置及び姿勢に基づいて前記三次元マップを補正する補正手段と
を備えることを特徴とする。

本発明の構成によれば、カメラ等の撮像装置の移動範囲が限られている場合であっても三次元マップに蓄積する誤差を低減させることができる。

コンピュータ装置のハードウェア構成例を示すブロック図。 システムの機能構成例を示すブロック図。 現実空間に配置された補助特徴を説明する図。 情報処理装置１が行う処理のフローチャート。 ステップＳ１０５０における処理のフローチャート。 システムの機能構成例を示すブロック図。 情報処理装置２が行う処理のフローチャート。 システムの機能構成例を示すブロック図。 ステップＳ１０６０の処理のフローチャート。 ＧＵＩ（ウィンドウ）の一例を示す図。

以下、添付図面を参照し、本発明の実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載した構成の具体的な実施例の１つである。

［第１の実施形態］
本実施形態では、シーン中に元来存在する特徴とは別の補助的な特徴（以下では補助特徴と称する）を人為的にシーン中に配置し、これを三次元マップの補正に利用するケースについて説明する。先ず、本実施形態に係るシステムの機能構成例について、図２のブロック図を用いて説明する。図２に示す如く、本実施形態に係るシステムは、撮像部１７０と情報処理装置１とを有する。

先ず、撮像部１７０について説明する。撮像部１７０はステレオカメラであり、第１のカメラ（例えば左側のカメラ）と第２のカメラ（例えば右側のカメラ）とを有する。第１のカメラ及び第２のカメラのそれぞれは、シーンの動画像を撮像し、該撮像したシーンの動画像（各フレームの撮像画像）を情報処理装置１に対して出力する。

次に、情報処理装置１について説明する。画像入力部１２０は、撮像部１７０から（第１のカメラ及び第２のカメラのそれぞれから）時系列に撮像画像を入力し（例えば毎秒３０フレーム）、該入力した撮像画像を後段の第１の導出部１３０及び第２の導出部１６０に対して送出する。

第１の導出部１３０は、画像入力部１２０から入力された撮像画像と、保持部１１０に保持されている三次元マップと、に基づき、該撮像画像を撮像したときの撮像部１７０の世界座標系における位置及び姿勢を導出する（第１の取得処理）。世界座標系とは、例えば、現実空間（シーン）における１点を原点とし、該原点で互いに直交する３軸をそれぞれｘ軸、ｙ軸、ｚ軸とする座標系である。本実施形態では、撮像部１７０（ステレオカメラ）を構成する２つのカメラ（第１のカメラ及び第２のカメラ）のうち一方（第１のカメラ）の世界座標系における位置及び姿勢を「世界座標系における撮像部１７０の位置及び姿勢」として求める。本実施形態では、世界座標系における第１のカメラの位置及び姿勢は、世界座標系における第１のカメラの位置を表す三次元ベクトル、世界座標系における第１のカメラの姿勢を表す三次元ベクトル、を合わせた六次元ベクトルのことを表すものとする。第１のカメラの姿勢は、世界座標系の原点を通る回転軸とその回転軸まわりの回転角によって表す。姿勢を表す三次元ベクトルの方向が回転軸の向き、ノルムが回転角を表すものとする。また本実施形態では、第１のカメラの光軸をZ軸、第１のカメラの撮像面の水平方向をX軸、第１のカメラの撮像面の垂直方向をY軸とする三次元の座標系をカメラ座標系と呼び、第１のカメラの位置及び姿勢を撮像部１７０の位置及び姿勢と呼ぶ。

また、第１のカメラ及び第２のカメラのそれぞれの内部パラメータ（焦点距離ｆｘ（画像の水平方向）、ｆｙ（画像の垂直方向）、画像中心位置ｃｘ（画像の水平方向）、ｃｙ（画像の垂直方向）、レンズ歪みパラメータ）は既知であるとする。カメラの内部パラメータは、例えばＺｈａｎｇの方法（Ｚ．Ｚｈａｎｇ，“Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，” ＩＥＥＥ Ｔｒａｎｓ． ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．１１，ｐｐ．１３３０−１３３４，２０００．）により事前に校正しておく。また、撮像部１７０を構成する二台のカメラ間（第１のカメラと第２のカメラとの間）の相対的な位置及び姿勢（外部パラメータ）も事前に校正しておく。ステレオカメラの外部パラメータは、例えば三次元形状が既知のパターンを同時に左右のカメラで撮像してパターンを基準とした各カメラの位置及び姿勢を求め、夫々の位置及び姿勢をステレオカメラの左側カメラ（本実施形態の場合は第１のカメラ）を基準とした位置及び姿勢に変換して求める。撮像部１７０の内部パラメータと外部パラメータは情報処理装置１内の不図示のメモリ若しくは情報処理装置１がアクセス可能な外部メモリに保持されるものとする。

保持部１１０は、図３に示す如く現実空間（シーン）中に人為的に配置したそれぞれの補助特徴の配置情報（世界座標系における補助特徴の位置及び姿勢と該補助特徴の識別子）を保持している。本実施形態では、補助特徴として、図３に示すように内部に個別の識別子を持つ白黒の正方形の特徴を利用する。正方形中の所定の位置の白黒パターンを参照することにより、正方形の識別子を特定することができる。補助特徴の世界座標系における位置及び姿勢は、例えば特許第０４５３２９８２号（または米国特許第７５２９３８７号）に開示される方法により事前に校正しておく。

保持部１１０は更に、第１の導出部１３０で利用するシーンの三次元マップを保持する。本実施形態では、非特許文献１と同様、三次元マップをキーフレームの集合として保持する。

各キーフレームは、撮像部１７０が撮像した撮像画像（濃淡画像）Ｉ、該撮像画像と同じ画像サイズのデプスマップＤ、分散マップＶを保持する。デプスマップＤにおける各画素は、撮像部１７０から該画素に対応する対応点までの距離（奥行き値）の逆数（ｉｎｖｅｒｓｅ ｄｅｐｔｈ）、すなわちカメラ座標系における該対応点のＺ座標の逆数を保持する。本実施形態では非特許文献１と同様、濃淡画像Ｉ上の画素のうち輝度勾配が閾値よりも大きい画素を特徴点とし、デプスマップＤにおいて該特徴点に対応する位置の画素に、該特徴点に対応する奥行き値の逆数を登録するものとする。分散マップＶの各画素は、ｉｎｖｅｒｓｅ ｄｅｐｔｈの分散を保持する。

さらに各キーフレームは、該キーフレームが保持する撮像画像の撮像時における撮像部１７０の世界座標系における位置及び姿勢を属性として保持する。以下では、キーフレームが保持する属性（世界座標系における撮像部１７０の位置及び姿勢）を「キーフレームの位置及び姿勢」と称する場合がある。

さらに各キーフレームは、該キーフレームが保持する撮像画像中の補助特徴に基づいて推定される撮像部１７０の世界座標系における位置及び姿勢が「参照位置及び参照姿勢」として保持可能である。

さらに各キーフレームは、マップ最適化で利用するための「キーフレーム対応情報」を保持可能である。キーフレーム対応情報は、キーフレームのペアを構成する２個のキーフレームのそれぞれの識別番号と、キーフレーム間の相対的な位置及び姿勢を保持する。キーフレーム対応情報は、マップ最適化において利用される。

更新部１４０は、撮像画像と、第１の導出部１３０によって導出された撮像部１７０の位置及び姿勢と、に基づいて、保持部１１０に保持されている三次元マップを更新する。

補正部１５０は、第２の導出部１６０が導出するキーフレーム間の相対的な位置及び姿勢を利用して、保持部１１０に保持されている三次元マップを補正する。このような補正により、三次元マップの蓄積誤差を低減し幾何的な整合がとれたマップにする。

第２の導出部１６０は、画像入力部１２０から入力された撮像画像から検出された補助特徴に基づいて撮像部１７０の位置及び姿勢を参照位置及び参照姿勢として導出する処理や、キーフレーム間の相対的な位置及び姿勢を導出する処理を行う。導出された相対的な位置及び姿勢は、補正部１５０で利用される。

制御部１９９は、情報処理装置１全体の動作制御を行う。例えば制御部１９９は、上記の画像入力部１２０、第１の導出部１３０、第２の導出部１６０、更新部１４０、補正部１５０の各機能部の動作制御を行う。次に、情報処理装置１の動作について、図４のフローチャートに従って説明する。

ステップＳ１０１０では、保持部１１０が保持する三次元マップの初期化処理が行われる。三次元マップの初期化処理では、更新部１４０は、画像入力部１２０が入力したステレオ画像（第１のカメラによる撮像画像及び第２のカメラによる撮像画像のセット）のうち第１のカメラによる撮像画像を基準画像とする。以下では、第１のカメラによる撮像画像を第１の撮像画像、第２のカメラによる撮像画像を第２の撮像画像と称する場合がある。そして更新部１４０は、基準画像上で輝度勾配が閾値以上の画素を特徴点とし、該基準画像と同セットの第２の撮像画像において該特徴点に対応する対応特徴点をステレオマッチングにより探索する。そして更新部１４０は、特徴点と対応特徴点とを用いた三角測量によりカメラ座標系における特徴点のｉｎｖｅｒｓｅ ｄｅｐｔｈを算出する。そして更新部１４０は、該算出した特徴点のｉｎｖｅｒｓｅ ｄｅｐｔｈを、デプスマップＤにおいて該特徴点に対応する画素に登録する。さらに更新部１４０は、デプスマップＤ上の各画素について、左右画像（第１の撮像画像及び第２の撮像画像）の視差及び撮像部１７０の基線長、画像ノイズの分散を用いて算出したｉｎｖｅｒｓｅ ｄｅｐｔｈの分散を推定し、分散マップＶに保存する。そして更新部１４０は、基準画像を濃淡画像Ｉとし、該濃淡画像Ｉ、デプスマップＤ、分散マップＶを含むキーフレームを生成して、三次元マップに追加する。なお、この時点で三次元マップに、濃淡画像Ｉの撮像時における撮像部１７０の世界座標系における位置及び姿勢を登録しても良い。濃淡画像Ｉの撮像時における撮像部１７０の世界座標系における位置及び姿勢はどのような方法で求めても良い。

ステップＳ１０２０では、画像入力部１２０は、撮像部１７０から入力されたステレオ画像を取得する。ステップＳ１０３０では、第１の導出部１３０は、ステップＳ１０２０で画像入力部１２０が入力したステレオ画像と、保持部１１０に保持されている三次元マップと、を用いて、撮像部１７０の世界座標系における位置及び姿勢を導出する。ステレオ画像と三次元マップとを用いて撮像部１７０の位置及び姿勢を導出する方法は周知であり、例えば、非特許文献１に開示される方法を利用することができる。

ステップＳ１０４０では、更新部１４０は、ステップＳ１０３０で導出した撮像部１７０の位置及び姿勢に基づいて、保持部１１０に保持されているキーフレームの更新、若しくは保持部１１０への新規のキーフレームの追加、を行う。ステップＳ１０４０における処理の詳細については後述する。

ステップＳ１０５０における処理は、ステップＳ１０４０において新規のキーフレームを保持部１１０に追加した場合に行われる。ステップＳ１０５０では、第２の導出部１６０は、三次元マップの補正で利用するためのキーフレーム間の相対的な位置及び姿勢を導出する。ステップＳ１０５０における処理の詳細については後述する。

ステップＳ１０６０では、補正部１５０は、ステップＳ１０５０で得られたキーフレーム間の相対的な位置及び姿勢を利用して三次元マップを補正する。キーフレームが保持するデプスマップＤの補正まで行うと計算時間がかかるため、本実施形態ではキーフレームの世界座標系における位置及び姿勢のみを補正する。世界座標系におけるキーフレームの位置及び姿勢の補正は、世界座標系におけるキーフレームの位置及び姿勢から算出される相対的な位置及び姿勢とステップＳ１０５０で得られたキーフレーム間の相対的な位置及び姿勢の誤差の総和を評価関数として行う。ここで、キーフレームｉとキーフレームｊの世界座標系における位置及び姿勢を表す４×４行列をそれぞれＭ（ｉ）、Ｍ（ｊ）、キーフレームｉとキーフレームｊの相対的な位置及び姿勢を表す４×４行列をＭ_ｒｅｌ（ｉ，ｊ）とする。行列Ｍ（ｉ）、Ｍ（ｊ）、Ｍ_ｒｅｌ（ｉ，ｊ）は、次式に示すように、左上の３×３行列が回転行列Ｒ、一番右の列が平行移動成分ｔを表す行列である。

行列Ｍ（ｉ）、Ｍ（ｊ）は、世界座標系における位置及び姿勢をカメラ座標系における位置及び姿勢に変換する行列であるとする。また行列Ｍ_ｒｅｌ（ｉ，ｊ）は、キーフレームｊのカメラ座標系における位置及び姿勢をキーフレームｉのカメラ座標系における三次元位置に変換する行列であるとする。ここでは、行列Ｍ_ｒｅｌ（ｉ，ｊ）を既知として、下記の式（１）が最小となるようなＭ（ｉ）、Ｍ（ｊ）を求める。

なお、｜｜行列｜｜_Ｆはフロベニウスのノルム（行列の各要素の二乗和）を表す。また、（式１）は三次元マップに保存されたすべての「キーフレーム対応情報」についての総和である。Ｍ（ｉ）、Ｍ（ｊ）は非線形の変換であるため、例えば反復計算を行うＧａｕｓｓ−Ｎｅｗｔｏｎ法により算出する。Ｍ（ｉ）、Ｍ（ｊ）の初期値としては、ステップＳ１０３０で導出された位置及び姿勢を用いるものとする。最適化されたＭ（ｉ）、Ｍ（ｊ）は再び三次元マップ中のキーフレームの属性として保存し、次にマップ最適化を行う際の初期値として利用する。

ステップＳ１０７０では、制御部１９９は、図４のフローチャートに従った処理の終了条件が満たされた否かを判断する。終了条件には様々なものが考えられる。例えば、ユーザが不図示の操作部を操作することで「図４のフローチャートに従った処理の終了指示」を入力したことを制御部１９９が検知した場合には、制御部１９９は終了条件が満たされたと判断する。終了条件が満たされた場合には、図４のフローチャートに従った処理は完了する。一方、終了条件が満たされていない場合には、処理はステップＳ１０７０を介してステップＳ１０２０に戻る。

次に、上記のステップＳ１０４０における処理の詳細について説明する。ステップＳ１０４０では、更新部１４０は、ステップＳ１０３０で導出した撮像部１７０の位置及び姿勢に基づいて、保持部１１０に保持されているキーフレームの更新、若しくは保持部１１０への新規のキーフレームの追加、を行う。

例えば更新部１４０は、保持部１１０に保持されているキーフレームのうち、ステップＳ１０３０で導出した撮像部１７０の位置及び姿勢に最も近い位置及び姿勢を属性として有するキーフレームを最近傍キーフレームとして特定する。そして更新部１４０は、最近傍キーフレームに含まれている濃淡画像Ｉにおける特徴点を求めると共に、ステップＳ１０２０で画像入力部１２０が入力した第１の撮像画像において該特徴点に対応する対応特徴点を求める。そして更新部１４０は、最近傍キーフレームに含まれている濃淡画像Ｉにおける特徴点の画素数に対する、ステップＳ１０２０で画像入力部１２０が入力した第１の撮像画像における対応特徴点の画素数の割合Ｒｉｎを求める。そして更新部１４０は、Ｒｉｎが閾値Ｔｉｎ未満の場合、最近傍キーフレームとのオーバーラップが少ないためキーフレームを追加すると判定する。一方、更新部１４０は、Ｒｉｎが閾値Ｔｉｎ以上の場合、最近傍キーフレームとのオーバーラップが多いためキーフレームを追加しない（保持部１１０に保持されているキーフレームを更新する）と判定する。

キーフレームを追加すると判定した場合、更新部１４０は、以下のような処理を行うことで、新規のキーフレームを三次元マップに追加する。先ず更新部１４０は、ステップＳ１０２０で画像入力部１２０が入力したステレオ画像を用いてステップＳ１０１０で説明した処理を行うことで、カメラ座標系における特徴点のｉｎｖｅｒｓｅ ｄｅｐｔｈを算出する。そして更新部１４０は、該算出したｉｎｖｅｒｓｅ ｄｅｐｔｈにより既存のキーフレーム（三次元マップに登録済み（過去に登録した）のキーフレーム）から伝播されたｉｎｖｅｒｓｅ ｄｅｐｔｈを更新することで新たなデプスマップＤを生成する。また、更新部１４０は、既存のキーフレームが保持していたｉｎｖｅｒｓｅ ｄｅｐｔｈの分散及び新たに算出されたｉｎｖｅｒｓｅ ｄｅｐｔｈの分散を用いて分散マップＶを生成する。そして更新部１４０は、ステップＳ１０２０で画像入力部１２０が入力した第１の撮像画像を濃淡画像Ｉとし、該濃淡画像Ｉ、ステップＳ１０４０で生成したデプスマップＤ及び分散マップＶ、を含み、且つステップＳ１０３０で導出した撮像部１７０の位置及び姿勢を属性として更に含むキーフレームを生成して三次元マップに追加する。

一方、キーフレームを追加しないと判定した場合、更新部１４０は、以下のような処理を行うことで、保持部１１０に保持されているキーフレームを更新する。より具体的には、更新部１４０は、ステップＳ１０３０で第１の導出部１３０が導出した位置及び姿勢、ステップＳ１０２０で入力されたステレオ画像を用いて既存のキーフレームのデプスマップＤ、分散マップＶを更新する。非特許文献１に開示される方法と同様に、デプスマップＤ、分散マップＶの更新は、ステレオ画像から推定されるｉｎｖｅｒｓｅ ｄｅｐｔｈと、キーフレームが保持する濃淡画像とのモーションステレオから推定されるｉｎｖｅｒｓｅ ｄｅｐｔｈを用いて行う。

次に、上記のステップＳ１０５０における処理の詳細について、図５のフローチャートに従って説明する。ステップＳ１１１０では、第２の導出部１６０は、第１の撮像画像及び第２の撮像画像のそれぞれについて、該撮像画像から補助特徴を検出する。図３に示すような補助特徴の検出は例えば次のようにして行う。まず、撮像画像を二値化して二値化画像を生成し、該二値化画像から四角形領域を検出し、該四角形領域の各頂点の画像座標をもとに正方形から画像座標への射影変換を算出し、四角形領域を正方形に変換する。そして正方形の所定の位置に相当する輝度値を画像から読み出すことにより補助特徴の識別子を特定する。このとき、該特定した識別子と保持部１１０に保持されている補助特徴の識別子との照合を行い、互いに識別子が一致する場合には各頂点の画像座標と世界座標系における三次元座標との対応を保存する。補助特徴の頂点の世界座標系における三次元座標は、補助特徴に規定された座標系における頂点の三次元座標と、補助特徴の世界座標系における位置及び姿勢より算出する。

ステップＳ１１２０では、第２の導出部１６０は、補助特徴から得られる情報が十分であるか否かを判断する。ここでは、ステップＳ１１１０で対応が保存された補助特徴の頂点の画像上の分布を利用して補助特徴の情報が十分にあるかの判定を行う。具体的には、第１の撮像画像及び第２の撮像画像のそれぞれについて、補助特徴の頂点の画像座標の共分散行列を算出する。第１の撮像画像及び第２の撮像画像のそれぞれから算出した共分散行列の最小固有値Ｅｉｇ（Ｌ）、Ｅｉｇ（Ｒ）の何れかが閾値Ｔ＿Ｅｉｇより小さいという条件が満たされた場合には、補助特徴の情報が不十分であると判断し、処理はステップＳ１１３０に進む。一方、この条件が満たされていない場合には、ステップＳ１１５０に進む。

ステップＳ１１３０では第２の導出部１６０は、ステップＳ１０４０で新たに追加されたキーフレーム（追加キーフレーム）の位置及び姿勢（属性）に近い位置及び姿勢を属性として有するキーフレームを保持部１１０に保持されている既存キーフレーム群から特定する。具体的には追加キーフレームに対し、世界座標系における視軸方向（カメラ座標系のＺ軸）の角度差が閾値ＴＡｎｇｌｅ以内（閾値以内）である既存キーフレームのうち世界座標系における位置の差が閾値ＴＤｉｓｔ以内である全ての既存キーフレームを特定する。そして第２の導出部１６０は、該特定したそれぞれの既存キーフレームと追加キーフレームとの相対的な位置及び姿勢を算出し（第２の取得処理）、該算出した相対的な位置及び姿勢と該それぞれの既存キーフレームの識別番号と追加キーフレームの識別番号とを追加キーフレームのキーフレーム対応情報として保持部１１０に登録する。

ステップＳ１１４０では、第２の導出部１６０は、追加キーフレームに含まれている濃淡画像Ｉとの類似度が高い濃淡画像Ｉを含む既存キーフレームを保持部１１０から特定し、該特定した既存キーフレームとの相対的な位置及び姿勢を導出する。具体的には、第２の導出部１６０は、追加キーフレーム及び既存キーフレームのそれぞれに含まれている濃淡画像Ｉの縮小画像Ｉ’を生成する。そして第２の導出部１６０は、追加キーフレームと既存キーフレームとの間で縮小画像Ｉ’の輝度値のＳＳＤ（Ｓｕｍ ｏｆ Ｓｑｕａｒｅｄ Ｄｉｓｔａｎｃｅ）が最小かつ閾値ＴＳＳＤ以下となる既存キーフレームを特定する。そして第２の導出部１６０は、該特定したそれぞれの既存キーフレームと追加キーフレームとの相対的な位置及び姿勢を同様に算出する（第２の取得処理）。そして第２の導出部１６０は、該算出した相対的な位置及び姿勢と、該それぞれの既存キーフレームの識別番号と、追加キーフレームの識別番号と、を追加キーフレームのキーフレーム対応情報として保持部１１０に登録する。

一方、ステップＳ１１５０で第２の導出部１６０は、ステップＳ１１１０で得られた補助特徴の頂点の画像座標と世界座標系における三次元座標の対応に基づいて、追加キーフレームの参照位置及び参照姿勢を算出し、追加キーフレームに登録する。登録位置及び登録姿勢の算出は、例えばＬｅｐｅｔｉｔらの手法（Ｖ． Ｌｅｐｅｔｉｔ， Ｆ． Ｍｏｒｅｎｏ−Ｎｏｇｕｅｒ， ａｎｄ Ｐ． Ｆｕａ， “ＥＰｎＰ： ａｎ ａｃｃｕｒａｔｅ Ｏ（ｎ） ｓｏｌｕｔｉｏｎ ｔｏ ｔｈｅ ＰｎＰ ｐｒｏｂｌｅｍ，” Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ， ｖｏｌ．８１，ｎｏ．２，ｐｐ．１５５−１６６，２００９．）により行う。

ステップＳ１１６０では、第２の導出部１６０は、追加キーフレームと、参照位置及び参照姿勢を保持する既存キーフレームと、の相対的な位置及び姿勢を算出する（第２の取得処理）。追加キーフレームの参照位置及び参照姿勢を表す４×４行列をＭ_ｎｅｗ、既存キーフレームの参照位置及び参照姿勢を表す４×４行列をＭ_ｏｌｄとする（行列Ｍ_ｎｅｗ、Ｍ_ｏｌｄは、世界座標系における位置及び姿勢をカメラ座標系における位置及び姿勢に変換する行列であるとする）。このとき、キーフレーム間の相対的な位置及び姿勢Ｍ_ｒｅｌ（ｎｅｗ、ｏｌｄ）は以下の（式２）により算出する。

参照位置及び参照姿勢を保持する既存キーフレームが複数存在する場合には、追加キーフレームの前側にありかつ一定距離以上離れた既存キーフレームのうちキーフレームの視軸方向（カメラ座標系のＺ軸）の間の角度が最も大きい既存キーフレームを選択する。これは、なるべく異なる視点を持つキーフレームをペアにすることで、三次元マップの補正によってより多くの累積誤差が削減されることを目的としている。選択されたペアの相対的な位置及び姿勢と、該選択されたペアにおけるそれぞれのキーフレームの識別番号と、は追加キーフレームにおけるキーフレーム対応情報として保持部１１０に登録する。

このように、本実施形態によれば、シーン中に配置した補助特徴を利用して導出されるキーフレーム間の相対的な位置及び姿勢を三次元マップの補正に利用することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。

＜変形例＞
第１の実施形態では、撮像部１７０はスレテオカメラであるものとして説明したが、シーンに配置した補助特徴を検出可能な画像を撮像することができる装置であれば、撮像部１７０としてスレテオカメラ以外の装置を用いても良い。例えば、撮像部１７０は単眼カメラであってもよいし、リジッドに固定されてカメラ間の相対的な位置及び姿勢が既知な三台以上のカメラであってもよい。また、撮像部１７０は、カラー画像と距離画像を撮像するＲＧＢＤカメラであってもよい。

また、補助特徴の形状は四角形に限るものではなく、画像から安定的に検出可能な特徴であれば如何なる特徴であっても良い。例えば、三角形、六角形、円形、楕円形の補助特徴を使用しても良い。また、補助特徴の識別子は画像から特定できなくてもよい。例えば、円形の補助特徴を用いる場合、第１の導出部１３０が導出する撮像部１７０の位置及び姿勢をもとに補助特徴の画像上の位置を算出し、近傍で検出された補助特徴と対応付けることで特定してもよい。また識別子つきの補助特徴と識別子のない補助特徴を併用してもよい。この場合、識別子つきの補助特徴から算出される撮像部１７０の位置及び姿勢をもとに識別子のない補助特徴の画像上の位置を算出し、近傍で検出された補助特徴と対応付けてもよい。

また、補助特徴の配置情報の校正は必ずしも事前に行わなくてもよい。例えば、撮像部１７０によって撮像される画像を用いて、シーンの三次元マップ作成と並行して行ってもよい。その際には、補助特徴を用いた三次元マップの補正は随時行わずに、補助特徴の配置情報の信頼度が十分高くなった場合（例えば信頼度が閾値以上の場合）に行うようにすることで三次元マップの精度が低下しないようにする。補助特徴の配置情報の信頼度の判定は、例えば校正に用いた画像群において各補助特徴が検出された回数の平均値や最小値に基づいて行い、検出回数の平均値や最小値が大きいほど信頼度が高いとする。

また、補助特徴の情報が十分にあるかどうかの判断基準は、補助特徴の画像上の分布に限るものではない。例えば、画像上で検出される補助特徴の個数が閾値以上の場合に、補助特徴の情報が十分にあると判断しても良い。また、補助特徴の個数と画像上の分布を組み合わせたものに基づいていてもよい。

また、参照位置及び参照姿勢を保持するキーフレームの中から相対的な位置及び姿勢を算出するために使用するキーフレームを特定する際には、キーフレームの間で共通の補助特徴が観察されているキーフレームのみを対象とするようにしてもよい。

［第２の実施形態］
本実施形態を含め、以下に説明する実施形態や変形例では第１の実施形態との差分について説明し、以下で特に触れない限りは第１の実施形態と同様であるものとする。本実施形態では、撮像部１７０に位置及び姿勢を計測するセンサを装着し、このセンサの計測値を三次元マップの補正に利用する場合について説明する。

本実施形態に係るシステムの機能構成例について、図６のブロック図を用いて説明する。図６において図２に示した機能部と同じ機能部には同じ参照番号を付しており、該機能部に係る説明は省略する。図６に示す如く、本実施形態に係るシステムは、撮像部１７０、センサ２９０、情報処理装置２、を有する。

先ず、センサ２９０について説明する。センサ２９０は、撮像部１７０の世界座標系における位置及び姿勢を計測し、その計測値を情報処理装置２に対して出力する。本実施形態では、センサ２９０はＢａｕｅｒらの文献（Ｍ． Ｂａｕｅｒ， Ｍ． Ｓｃｈｌｅｇｅｌ， Ｄ． Ｐｕｓｔｋａ， Ｎ． Ｎａｖａｂ， ａｎｄ Ｇ． Ｋｌｉｎｋｅｒ， “Ｐｒｅｄｉｃｔｉｎｇ ａｎｄ ｅｓｔｉｍａｔｉｎｇ ｔｈｅ ａｃｃｕｒａｃｙ ｏｆ ｎ−ｏｃｃｕｌａｒ ｏｐｔｉｃａｌ ｔｒａｃｋｉｎｇ ｓｙｓｔｅｍｓ，” Ｐｒｏｃ． ５ｔｈ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ， ｐｐ．４３−５１， ２００６．）で開示されている光学式の位置姿勢センサであるとする。この光学式の位置姿勢センサは、計測対象（本実施形態では撮像部１７０）上に複数のマーカを配置し、配置されたマーカをシーン中に固定された複数のカメラによって撮影した画像を利用して計測対象の世界座標系における位置及び姿勢を計測するものである。

次に、情報処理装置２について説明する。センサ入力部２８０は、センサ２９０から出力された計測値を入力し、該計測値を後段の第２の導出部２６０に送出する。第２の導出部２６０は、計測値に基づいて撮像部１７０の位置及び姿勢を参照位置及び参照姿勢として導出する処理や、キーフレーム間の相対的な位置及び姿勢を導出する処理を行う。導出された相対的な位置及び姿勢は、補正部１５０で利用される。

次に、情報処理装置２の動作について、図７のフローチャートに従って説明する。図７において図４に示した処理ステップと同じ処理ステップには同じステップ番号を付しており、該処理ステップに係る説明は省略する。

ステップＳ２０２０では、画像入力部１２０は、撮像部１７０から入力されたステレオ画像を取得し、センサ入力部２８０は、センサ２９０から入力された計測値（世界座標系における撮像部１７０の位置及び姿勢）を取得する。

ステップＳ２０４０では、更新部１４０は上記のステップＳ１０４０と同様にして新規のキーフレームを三次元マップに追加する。ここで、追加するキーフレームの属性、参照位置及び参照姿勢としては、ステップＳ２０２０で取得した撮像部１７０の位置及び姿勢を適用する。

ステップＳ２０５０では、第２の導出部２６０は、追加キーフレームにおける参照位置及び参照姿勢として、ステップＳ２０２０で取得した計測値が示す位置及び姿勢を用いる。そして第２の導出部２６０は上記のステップＳ１１６０と同様にして、追加キーフレームと、参照位置及び参照姿勢を保持する既存キーフレームと、の相対的な位置及び姿勢を算出する。そして第２の導出部２６０は、参照位置及び参照姿勢を保持する既存キーフレームのうち追加キーフレームとの距離が最も大きい既存キーフレームを選択し、該選択した既存キーフレームとの相対的な位置及び姿勢と該選択した既存キーフレームの識別番号と追加キーフレームの識別番号とをキーフレーム対応情報として追加キーフレームに登録する。

このように、本実施形態によれば、位置及び姿勢を計測するセンサの計測値をもとにキーフレーム間の相対的な位置及び姿勢を導出することで、カメラの移動範囲が限定される場合であっても三次元マップに蓄積する誤差を低減できる。

＜変形例＞
センサ２９０は光学式の位置姿勢センサに限るものではなく、他の種類の位置姿勢センサであってもよい。例えば、磁気式の位置姿勢センサであってもよい。また、位置及び姿勢の双方を計測する必要はなく、位置または姿勢のいずれかを計測するセンサであってもよい。すなわち、ジャイロスコープや加速度センサ、磁気センサを利用して撮像部１７０の姿勢を計測する姿勢センサであってもよいし、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）のような位置を計測する位置センサであってもよい。位置または姿勢のいずれかを計測するセンサを利用する場合には、相対的な位置又は姿勢を取得することになり、該相対的な位置又は姿勢に基づいて三次元マップを補正する（センサが計測する成分のみを補正する）ことになる。また、位置を計測するセンサと姿勢を計測するセンサを組み合わせて位置姿勢センサとして利用してもよい。

また、シーンの三次元マップの作成に利用可能な画像を撮像することができる装置であれば、撮像部１７０としてスレテオカメラ以外の装置を用いても良い。例えば、撮像部１７０は単眼カメラであってもよいし、リジッドに固定されてカメラ間の相対的な位置及び姿勢が既知な三台以上のカメラであってもよい。また、撮像部１７０は、距離画像を撮像するデプスカメラであっても良いし、カラー画像と距離画像を撮像するＲＧＢＤカメラであってもよい。

［第３の実施形態］
本実施形態では、非特許文献１のようにマップ最適化において各キーフレームの位置及び姿勢のみを補正するのではなく、特徴点の三次元座標も補正する。本実施形態に係るシステムの機能構成例について、図８のブロック図を用いて説明する。図８において図２に示した機能部と同じ機能部には同じ参照番号を付しており、該機能部に係る説明は省略する。図８に示す如く、本実施形態に係るシステムは、撮像部１７０と、情報処理装置３と、を有する。

補正部３５０は、第２の導出部１６０が導出するキーフレーム間の相対的な位置及び姿勢を利用して三次元マップを補正する。第１の実施形態における補正部１５０では、計算時間を削減するためにキーフレームの位置及び姿勢のみ補正しており、三次元マップの精度は低かった。一方、本実施形態では、計算時間をかけてキーフレームの位置及び姿勢とともに各キーフレームが持つデプスマップも補正することで、三次元マップの精度を向上させる。

次に、情報処理装置３の動作について、図４のフローチャートに従って説明する。情報処理装置３は、図４のフローチャートに従った処理においてステップＳ１０６０の処理を図９のフローチャートに従った処理に置き換えた処理を実行する。

本実施形態に係るステップＳ１０６０では、補正部３５０は、ステップＳ１０５０で得られたキーフレーム間の相対的な位置及び姿勢を制約条件として三次元マップを補正する。三次元マップの補正では、（１）各キーフレームにおける特徴点の再投影誤差の総和、（２）キーフレームの相対的な位置及び姿勢の誤差の総和（式１）、の和を評価関数として各キーフレームの位置及び姿勢と各特徴点の三次元座標を補正する。

本実施形態に係るステップＳ１０６０では先ず、各キーフレームがデプスマップに保持する特徴点と他のキーフレームがデプスマップに保持する特徴点との対応付けを行う。次に、特徴点の対応付けの結果と、キーフレーム間の相対的な位置及び姿勢と、を用いて三次元マップを補正する。ここで、三次元マップが保持するキーフレームの総数をＮｋとする（つまり、キーフレームの識別番号は１〜Ｎｋ）。

ステップＳ３１１０では、補正部３５０は、変数ｎの値を１に初期化する。ステップＳ３１２０では、補正部３５０は、ｎ＞Ｎｋであるか否かを判断する。この判断の結果、ｎ＞Ｎｋであれば、全てのキーフレームにおける特徴点の対応付けが完了したものとして処理はステップＳ３１６０に進み、ｎ≦Ｎｋの場合には、処理はステップＳ３１３０に進む。

ステップＳ３１３０で補正部３５０はステップＳ１１３０と同様の処理を行うことで、キーフレームｎ（識別番号がｎのキーフレーム）の位置及び姿勢に近い位置及び姿勢を有するキーフレームを保持部１１０に保持されている既存キーフレーム群から特定する。更に補正部３５０は、上記のステップＳ１１４０と同様の処理を行うことで、キーフレームｎに含まれている濃淡画像Ｉとの類似度が高い濃淡画像Ｉを含む既存キーフレームを保持部１１０から特定する。

ステップＳ３１４０では、補正部３５０は、キーフレームｎにおける特徴点と、ステップＳ３１３０で特定したキーフレーム（特定キーフレーム）の特徴点と、の対応付けを行う。この対応付けの結果は、ステップＳ３１６０におけるパラメータ更新において、特徴点の三次元座標をキーフレーム毎に更新するのではなく、キーフレーム間で共通のパラメータとして更新するのに用いる。特徴点の対応付けは次のようにして行う。まず、各キーフレームの世界座標系における位置及び姿勢、デプスマップから算出される特徴点の三次元座標、撮像部１７０の内部パラメータ、を用いて、特定キーフレームにおいて、キーフレームｎにおける特徴点に対応する位置（ｕｎ、ｖｎ）を算出する。次に、特定キーフレームにおける特徴点のうち、位置（ｕｎ、ｖｎ）との距離が閾値Ｔ_{ｎｅｉｇｈｂｏｕｒ}以下となる位置にある特徴点を選択する。そして、キーフレームｎにおける特徴点に対し、選択された特徴点のうち、特徴点近傍のＮ×Ｎ画素の輝度値の二乗誤差の総和が閾値Ｔｄｉｆｆ以内となる特徴点が存在する場合には、そのうち最も二乗誤差の総和の小さい特徴点がキーフレームｎ上の特徴点と対応付くものとし、それぞれに同一の識別番号を割り振る。ステップＳ３１５０では、補正部３５０は、変数ｎの値を１つインクリメントする。そして処理はステップＳ３１２０に戻る。

一方、ステップＳ３１６０では、補正部３５０は、未知パラメータである「世界座標系における各キーフレームの位置及び姿勢」及び「各特徴点の三次元座標」を更新する。前述したように、（１）三次元マップを構成する各キーフレームにおける特徴点の再投影誤差の総和、（２）キーフレームの相対的な位置及び姿勢の誤差の総和、の和が最小となるように未知パラメータを推定する。ここで、再投影誤差とは、キーフレームの位置及び姿勢、特徴点の三次元座標、撮像部１７０の内部パラメータ、をもとに算出される画像上の位置（ｕ_ｃａｌｃ，ｖ_ｃａｌｃ）と実際の特徴点の画像上の検出位置（ｕ_ｏｂｓ，ｖ_ｏｂｓ）の差のことを言う。ここで再投影誤差の総和Ｅ_{ｒｅｐｒｏｊ}を以下の（式３）のように表す。

（ｕ_ｃａｌｃ（ｉ，ｊ），ｖ_ｃａｌｃ（ｉ，ｊ））、はｉ番目のキーフレームにおけるｊ番目の特徴点の画像上の算出位置であるとする。（ｕ_ｏｂｓ（ｉ，ｊ），ｖ_ｏｂｓ（ｉ，ｊ））、はｉ番目のキーフレームにおけるｊ番目の特徴点の画像上の検出位置であるとする。またキーフレームの相対的な位置及び姿勢の誤差の総和Ｅ_{ｒｅｌａｔｉｖｅ}を以下の（式４）のように表す。

ここでＭ（ｓ）、Ｍ（ｔ）はそれぞれキーフレームｓとキーフレームｔの世界座標系における位置及び姿勢を表す４×４剛体変換行列である。Ｍ_ｒｅｌ（ｓ，ｔ）は、キーフレームｓとキーフレームｔの相対的な位置及び姿勢を表す４×４行列であり、ステップＳ１０５０から得られる。ステップＳ３１６０では、以下の（式５）に示す（式３）と（式４）の重み付け和Ｅ_{ｔｏｔａｌ}が最小となるように未知パラメータを推定する。

（式５）を最小化する未知パラメータの推定はバンドル調整により行う。但し、一般にバンドル調整は再投影誤差を最小化する方法である。そのため、再投影誤差とキーフレーム間の位置及び姿勢の差分の和が最小となるように位置及び姿勢を推定するＳｋｒｙｐｎｙｋらの方法（Ｉ． Ｓｋｒｙｐｎｙｋ ａｎｄ Ｄ． Ｇ． Ｌｏｗｅ， “Ｓｃｅｎｅ ｍｏｄｅｌｌｉｎｇ， ｒｅｃｏｇｎｉｔｉｏｎ ａｎｄ ｔｒａｃｋｉｎｇ ｗｉｔｈ ｉｎｖａｒｉａｎｔ ｉｍａｇｅ ｆｅａｔｕｒｅｓ，” Ｐｒｏｃ． ３ｒｄ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ， ｐｐ．１１０−１１９， ２００４．）と組み合わせて推定を行う。

ステップＳ３１７０では、補正部３５０は、ステップＳ３１６０で推定された世界座標系における特徴点の三次元座標を各キーフレームのカメラ座標系における三次元座標に変換し、該変換した三次元座標のＺ成分をｉｎｖｅｒｓｅ ｄｅｐｔｈに変換する。

ステップＳ３１８０では、補正部３５０は、キーフレームの位置及び姿勢をステップＳ３１６０で推定された世界座標系におけるキーフレームの位置及び姿勢に更新する。また補正部３５０は、キーフレームに含まれているデプスマップＤにおいて特徴点のｉｎｖｅｒｓｅ ｄｅｐｔｈを、ステップＳ３１７０で変換された該特徴点のｉｎｖｅｒｓｅ ｄｅｐｔｈに更新する。

このように、本実施形態によれば、シーン中に配置した補助特徴から得られるキーフレーム間の相対的な位置及び姿勢を用いてキーフレームの位置及び姿勢に加えて特徴点の三次元座標も補正することで、三次元マップを高精度化することができる。

＜変形例＞
三次元マップの保持方法は、キーフレームが個別にデプスマップＤを保持する方法に限るものではない。例えば、Ｋｌｅｉｎらの方法（Ｇ． Ｋｌｅｉｎ ａｎｄ Ｄ． Ｍｕｒｒａｙ，“Ｐａｒａｌｌｅｌ Ｔｒａｃｋｉｎｇ ａｎｄ Ｍａｐｐｉｎｇ ｆｏｒ Ｓｍａｌｌ ＡＲ Ｗｏｒｋｓｐａｃｅｓ，” Ｐｒｏｃ．６ｔｈ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ，ｐｐ．２２５−２３４，２００７．）のように、三次元マップの中に唯一のデプスマップを保持していてもよい。

また、キーフレーム間の相対的な位置及び姿勢はシーン中に配置した補助特徴から導出する必要は必ずしもなく、第２の実施形態のように撮像装置の位置または姿勢を計測するセンサから取得してもよい。

［第４の実施形態］
図２，６，８に示した各機能部はハードウェアで実装しても良いが、保持部１１０を除く各機能部をソフトウェア（コンピュータプログラム）で実装しても良い。後者の場合、保持部１１０として機能するメモリを有し、該コンピュータプログラムを実行可能なコンピュータ装置は、上記の各実施形態や各変形例に係る情報処理装置に適用可能である。上記の各実施形態や各変形例に係る情報処理装置に適用可能なコンピュータ装置のハードウェア構成例について、図１のブロック図を用いて説明する。

ＣＰＵ１０は、ＲＯＭ２０やＲＡＭ３０に格納されているコンピュータプログラムやデータを用いて処理を実行する。これによりＣＰＵ１０は、コンピュータ装置全体の動作制御を行うと共に、情報処理装置が行うものとして上述した各処理を実行若しくは制御する。ＲＯＭ２０には、コンピュータ装置の設定データや各種の基本プログラムが格納されている。

ＲＡＭ３０は、外部記憶装置７０からロードされたコンピュータプログラムやデータ、入力Ｉ／Ｆ（インターフェース）４０を介して外部（撮像部１７０やセンサ２９０）から受信したデータ、を格納するためのエリアを有する。更にＲＡＭ３０は、ＣＰＵ１０が各種の処理を実行する際に用いるワークエリアを有する。このようにＲＡＭ３０は、各種のエリアを適宜提供することができる。

入力Ｉ／Ｆ４０は、外部からの入力を受信するためのＩ／Ｆとして機能するものであり、例えば、撮像部１７０やセンサ２９０から出力される撮像画像や計測値は、この入力Ｉ／Ｆ４０を介して受信される。

表示部６０は、ＣＲＴや液晶画面などにより構成されており、ＣＰＵ１０による処理結果を画像や文字などでもって表示することができる。また表示部６０はタッチパネル画面であっても良い。

外部記憶装置７０は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置７０には、ＯＳ（オペレーティングシステム）や、情報処理装置が行うものとして上述した各処理をＣＰＵ１０に実行させるためのコンピュータプログラムやデータが保存されている。外部記憶装置７０に保存されているコンピュータプログラムには、図２，６，８において保持部１１０を除く各機能部の機能をＣＰＵ１０に実現させるためのコンピュータプログラムが含まれている。また、外部記憶装置７０に保存されているデータには、上記の説明において既知の情報として説明したもの（例えば閾値や各種のパラメータ）が含まれている。外部記憶装置７０に保存されているコンピュータプログラムやデータは、ＣＰＵ１０による制御に従って適宜ＲＡＭ３０にロードされ、ＣＰＵ１０による処理対象となる。なお、上記の保持部１１０や不図示のメモリとして説明したものは、ＲＯＭ２０やＲＡＭ３０、外部記憶装置７０によって実装可能である。

操作部８０は、キーボードやマウスなどのユーザインターフェースにより構成されており、ユーザが操作することで各種の指示をＣＰＵ１０に対して入力することができる。例えば、ユーザは操作部８０を操作することで図４，７に示した処理の終了指示を入力することができる。

ＣＰＵ１０、ＲＯＭ２０、ＲＡＭ３０、入力Ｉ／Ｆ４０、表示部６０、外部記憶装置７０、操作部８０、は何れもバス９０に接続されている。なお、図１に示したハードウェア構成は、上記の情報処理装置に適用可能なハードウェア構成の一例に過ぎない。

［第５の実施形態］
本実施形態では、上記の各実施形態に適用可能なＧＵＩ（グラフィカルユーザインターフェース）について説明する。例えば、図４のフローチャートに従って補正された三次元マップに基づいて仮想空間を構築し、該仮想空間を任意の視点（仮想カメラ）から見た画像（仮想空間画像）を提示するＧＵＩ（ウィンドウ）の一例を図１０に示す。このＧＵＩは、例えば、表示部６０に表示される。

このＧＵＩには、仮想カメラから見える仮想空間の仮想空間画像に加え、特徴点（特徴点の三次元座標を画面に投影した位置に表示）、キーフレームの位置及び姿勢を示すオブジェクト、を描画する。本実施形態では、特徴点は丸い点、キーフレームを表すオブジェクトはカメラを模した錐体により表現する。ユーザは操作部８０を操作して仮想カメラの位置や姿勢、内部パラメータ等を変更し、作成された三次元マップに対応する仮想空間を様々な視点から観察して確認することができる。キーフレーム対応情報が三次元マップに存在する場合には、対応するキーフレームを表す錐体の頂点同士（オブジェクト間）を線分で結ぶことにより、どのキーフレームが三次元マップの補正に利用されたかを確認することができる。その際に、キーフレームの対応がどのように生成されたかによって線分の色を変えることで補助特徴が効果的に利用されているかを確認できるようにする。すなわち、キーフレームの対応（キーフレーム間の相対的な位置及び姿勢の導出）が、ステップＳ１１３０、ステップＳ１１４０、ステップＳ１１６０のどのステップで得られたかにより錐体を結ぶ線分の色を変更する。このＧＵＩには、特徴点とキーフレームに加えて、補助特徴を表示してもよい。

また、第２の実施形態で説明したセンサの計測値に基づいてキーフレームの対応が得られた場合にはさらに別の色の線分を描画する。なお、本実施形態では、線分の色を変えることに加え／若しくは代えて線分のパターン（実線、点線、太線、細線など）を変えても良く、変更する表示形態は色に限らない。

［第６の実施形態］
第１の実施形態では、正方形、三角形、円形などの補助的な特徴を三次元マップの補正に利用していた。しかしながら、補助的な特徴はこれに限るものではなく、位置及び姿勢の推定に必要な幾何情報が既知である三次元的な物体を用いてもよい。このような場合、図４のフローチャートに従った処理において、ステップＳ１０５０において次のような処理を行うようにすればよい。

つまり本実施形態に係るステップＳ１０５０では、第２の導出部１６０は、シーンに存在する、または配置した三次元的な物体（以下、三次元物体）を用いてキーフレーム間の相対的な位置及び姿勢を算出する。第１の実施形態では補助特徴の頂点の画像座標と三次元座標との対応に基づいてキーフレームの参照位置及び参照姿勢を算出していた。本実施形態では、三次元物体の幾何情報とキーフレームが保持する画像をもとに参照位置及び参照姿勢を算出する。具体的には、三次元物体のエッジモデルを保持しておき、Ｄｒｕｍｍｏｎｄらの手法（Ｔ．Ｄｒｕｍｍｏｎｄ ａｎｄ Ｒ．Ｃｉｐｏｌｌａ，“Ｒｅａｌ−ｔｉｍｅ Ｖｉｓｕａｌ Ｔｒａｃｋｉｎｇ ｏｆ Ｃｏｍｐｌｅｘ Ｓｔｒｕｃｔｕｒｅｓ，” ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２４，ｎｏ．７，ｐｐ．９３２−９４６，２００２．）により参照位置及び参照姿勢を算出する。

このように、本実施形態によれば、位置及び姿勢の推定に必要な幾何情報が既知な任意の三次元物体を補助特徴として利用することが可能であるため、シーンの景観を損ねることを防止できる。

なお、三次元物体を用いた参照位置及び参照姿勢の算出方法はエッジモデルを用いた方法に限るものではなく、三次元物体上の特徴点を用いたり、輝度情報を用いたりしてもよい。また、通常の画像だけでなく距離画像を利用できる場合には、距離画像への三次元モデルのフィッティングにより参照位置及び参照姿勢を算出してもよい。さらには通常の画像と距離画像の双方への三次元モデルのフィッティングを行ってもよい。

［第７の実施形態］
画像を用いた三次元マップの作成及び撮像部１７０の位置及び姿勢の計測は、非特許文献１で開示される方法に限るものではない。例えば、Ｋｌｅｉｎらの方法（Ｇ． Ｋｌｅｉｎ ａｎｄ Ｄ． Ｍｕｒｒａｙ，“Ｐａｒａｌｌｅｌ Ｔｒａｃｋｉｎｇ ａｎｄ Ｍａｐｐｉｎｇ ｆｏｒ Ｓｍａｌｌ ＡＲ Ｗｏｒｋｓｐａｃｅｓ，” Ｐｒｏｃ．６ｔｈ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ，ｐｐ．２２５−２３４，２００７．）のように明示的に検出された特徴点を利用するものであってもよい。また、ＲＧＢＤカメラやデプスカメラを用いる方法であってもよい。

また、新しく追加されたキーフレームとの相対的な位置及び姿勢を算出するために選択されるキーフレームは必ずしも一つである必要はなく、複数のキーフレームを選択してもよい。例えば、新しく追加されたキーフレームから一定距離以上離れたキーフレームすべてを選択してもよいし、新しく追加されたキーフレームからの距離が大きい順に複数個のキーフレームを選択してもよい。

また、三次元マップの補正は、位置及び姿勢の導出、三次元マップの更新の後に逐次的に実施する必要はない。三次元マップの補正には時間がかかるため、位置及び姿勢の導出、三次元マップの更新と並列に実施してもよい。

また、上記の実施形態や変形例では、撮像部１７０やセンサ２９０と情報処理装置（情報処理装置１、２，３）とを別個にするのではなく、撮像部１７０やセンサ２９０を情報処理装置に組み込んで一体化させても良い。なお、以上説明した各実施形態や各変形例はその一部若しくは全部を適宜組み合わせて使用しても構わないし、選択的に使用しても構わない。

＜効果のまとめ＞
シーン中に配置した補助特徴を利用して導出されるキーフレーム間の相対的な位置及び姿勢を三次元マップの補正に利用することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。

また、位置及び姿勢を計測するセンサの計測値をもとにキーフレーム間の相対的な位置及び姿勢を導出することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。

さらに、補助特徴やセンサの計測値をもとに導出されるキーフレーム間の相対的な位置及び姿勢を用いてキーフレームの位置及び姿勢に加えて特徴点の三次元座標も補正することで、さらに三次元マップを高精度化することができる。

＜定義のまとめ＞
保持部１１０が保持する三次元マップは、撮像画像、画像撮像時の撮像部１７０の位置及び姿勢、現実空間中の幾何特徴の三次元情報から構成されるキーフレーム群、によって表されるものであれば何でもよい。例えば、現実空間中の幾何特徴の三次元情報は、現実空間を基準とした座標系における三次元座標であってもよいし、キーフレームを基準とした座標における三次元座標やデプスマップであってもよい。

また撮像部１７０は、現実空間の画像を撮像するカメラであれば何でもよい。例えば、濃淡画像を撮影するカメラでもよいし、カラー画像を撮影するカメラであってもよい。さらには、カメラは１台であってもよいし、ステレオカメラなどの複数台のカメラによって構成される撮像装置であってもよい。また距離画像を撮影するデプスカメラや、カラー画像と距離画像を同時に撮影するＲＧＢＤカメラであってもよい。

また、第１の導出部１３０は、入力画像と三次元マップをもとに入力画像の撮像時の撮像部１７０の位置及び姿勢を導出するものであれば何でもよい。例えば、濃淡画像上の輝度値の勾配が大きい点を特徴点として位置及び姿勢を導出するものでもよいし、特徴点を画像から明示的に検出して位置及び姿勢を導出するものでもよい。

また、更新部１４０は、撮像時の撮像部１７０の位置及び姿勢と、第１の導出部１３０が位置及び姿勢を導出した画像と、を用いて三次元マップを更新するものであれば何でもよい。例えば、導出された位置及び姿勢をもとに、既存の三次元マップ中の特徴点の三次元座標やデプスマップを更新してもよい。また、新規のキーフレームを生成し、三次元マップに追加してもよい。

また、第２の導出部１６０は、キーフレーム間の相対的な位置及び姿勢を導出するものであれば何でもよい。例えば、シーン中に補助的な特徴を別途配置し、画像上で検出される特徴の情報をもとに導出されるキーフレームの位置及び姿勢を用いて相対的な位置及び姿勢を導出してもよい。また、撮像部１７０に位置または姿勢を計測するセンサを装着し、センサによって計測される撮像部１７０の位置または姿勢をもとに相対的な位置及び姿勢を導出してもよい。

また、補正部１５０は、第２の導出部１６０から得られるキーフレーム間の相対的な位置及び姿勢をもとに三次元マップを補正するものであれば何でもよい。例えば、キーフレームの位置及び姿勢のみを補正してもよいし、さらに特徴点の三次元座標やデプスマップを補正してもよい。

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

１１０：保持部 １２０：画像入力部 １３０：第１の導出部 １４０：更新部 １５０：補正部 １６０：第２の導出部 １７０：撮像部 １９９：制御部

Claims (20)

1. 撮像装置により撮像されるシーンの三次元情報と該撮像装置の位置及び姿勢とを対応付けた三次元マップを保持する保持手段と、
   前記三次元マップと前記撮像装置により撮像された画像とに基づいて該画像の撮像時における前記撮像装置の第１の位置及び姿勢を導出する第１の導出手段と、
   前記撮像された画像と前記導出された第１の位置及び姿勢とに基づいて前記三次元マップを更新する更新手段と、
   前記第１の位置及び姿勢と、該第１の位置及び姿勢とは異なる手法または時点で導出された前記撮像装置の第２の位置及び姿勢との間の相対的な位置及び姿勢を導出する第２の導出手段と、
   前記相対的な位置及び姿勢に基づいて前記三次元マップを補正する補正手段と
   を備えることを特徴とする情報処理装置。
2. 前記第２の導出手段は、前記三次元マップを参照せずに前記撮像装置により撮像された画像内の特徴に基づいて前記第２の位置及び姿勢を導出することを特徴とする請求項１に記載の情報処理装置。
3. 前記特徴は前記シーンに人為的に配置された補助的な特徴であることを特徴とする請求項２に記載の情報処理装置。
4. 前記特徴は幾何情報が既知である三次元物体の特徴であることを特徴とする請求項２に記載の情報処理装置。
5. 前記第２の導出手段は、前記第１の導出手段により過去に導出され、前記三次元マップに含まれている前記撮像装置の位置及び姿勢を、前記第２の位置及び姿勢として用いることを特徴とする請求項２に記載の情報処理装置。
6. 前記第２の導出手段は、前記三次元マップに含まれている前記位置及び姿勢のうち、前記第１の導出手段により新たに導出された第１の位置及び姿勢と近似する位置及び姿勢を、前記第２の位置及び姿勢として用いることを特徴とする請求項５に記載の情報処理装置。
7. 前記第２の導出手段は、前記三次元マップに含まれている前記位置及び姿勢のうち、前記第１の導出手段により新たに導出された第１の位置及び姿勢と対応する撮像された画像と、類似する画像と対応付けられた位置及び姿勢を、前記第２の位置及び姿勢として用いることを特徴とする請求項５に記載の情報処理装置。
8. 前記第２の導出手段は、前記撮像された画像内の特徴が十分でない場合に、前記三次元マップに含まれている前記撮像装置の位置及び姿勢を、前記第２の位置及び姿勢として用いることを特徴とする請求項５に記載の情報処理装置。
9. 前記保持手段は、前記三次元マップとして複数のキーフレーム情報を保持し、該複数のキーフレーム情報の各々は、前記撮像装置により撮像される画像と、該画像中の複数の特徴点の奥行情報とを含み、該撮像装置の位置及び姿勢と対応付けられていることを特徴とする請求項１に記載の情報処理装置。
10. 前記複数の特徴点は前記画像中で輝度勾配が閾値以上の画素であることを特徴とする請求項９に記載の情報処理装置。
11. 前記更新手段は、前記撮像された画像と前記導出された第１の位置及び姿勢とを含む情報を新たなキーフレーム情報として前記三次元マップに追加することを特徴とする請求項９に記載の情報処理装置。
12. 前記更新手段は、前記撮像された画像と前記導出された第１の位置及び姿勢とを含む情報に基づいて前記三次元マップ中のキーフレーム情報を修正することを特徴とする請求項９に記載の情報処理装置。
13. 前記更新手段は、前記導出された第１の位置及び姿勢と最も近傍の位置及び姿勢に対応するキーフレーム情報を最近傍キーフレーム情報として前記三次元マップから求め、当該最近傍キーフレーム情報における画像と前記撮像された画像とが所定の類似条件を満たせば、当該最近傍キーフレーム情報を修正することを特徴とする請求項１２に記載の情報処理装置。
14. 前記所定の類似条件は、前記撮像された画像において前記最近傍キーフレーム情報の特徴点と対応する特徴点の画素数の割合が閾値以上であることを特徴とする請求項１３に記載の情報処理装置。
15. 前記補正手段は、前記三次元マップ中の前記キーフレーム情報における前記撮像装置の位置及び姿勢を補正することを特徴とする請求項１２に記載の情報処理装置。
16. 前記補正手段は、前記三次元マップ中の前記キーフレーム情報における前記撮像装置の位置及び姿勢と前記特徴点の奥行情報とを補正することを特徴とする請求項１２に記載の情報処理装置。
17. 更に、
    前記三次元マップに基づいて仮想空間の画像を表示し、該仮想空間の画像上に前記キーフレームに対応するオブジェクトを表示する表示手段を備えることを特徴とする請求項９に記載の情報処理装置。
18. 前記第２の導出手段は、前記撮像装置の位置または姿勢を計測するセンサから入力された計測値を前記第２の位置及び姿勢の少なくとも一部として用いることを特徴とする請求項１に記載の情報処理装置。
19. 情報処理装置が行う情報処理方法であって、
    撮像装置により撮像されるシーンの三次元情報と該撮像装置の位置及び姿勢とを対応付けた三次元マップと、前記撮像装置により撮像された画像とに基づいて該画像の撮像時における前記撮像装置の第１の位置及び姿勢を導出する第１の導出工程と、
    前記撮像された画像と前記導出された第１の位置及び姿勢とに基づいて前記三次元マップを更新する更新工程と、
    前記第１の位置及び姿勢と、該第１の位置及び姿勢とは異なる手法または時点で導出された前記撮像装置の第２の位置及び姿勢との間の相対的な位置及び姿勢を導出する第２の導出工程と、
    前記相対的な位置及び姿勢に基づいて前記三次元マップを補正する補正工程と
    を備えることを特徴とする情報処理方法。
20. コンピュータを、請求項１乃至１８の何れか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。

Images