以下、添付図面を参照し、本発明の実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載した構成の具体的な実施例の1つである。
[第1の実施形態]
本実施形態では、シーン中に元来存在する特徴とは別の補助的な特徴(以下では補助特徴と称する)を人為的にシーン中に配置し、これを三次元マップの補正に利用するケースについて説明する。先ず、本実施形態に係るシステムの機能構成例について、図2のブロック図を用いて説明する。図2に示す如く、本実施形態に係るシステムは、撮像部170と情報処理装置1とを有する。
先ず、撮像部170について説明する。撮像部170はステレオカメラであり、第1のカメラ(例えば左側のカメラ)と第2のカメラ(例えば右側のカメラ)とを有する。第1のカメラ及び第2のカメラのそれぞれは、シーンの動画像を撮像し、該撮像したシーンの動画像(各フレームの撮像画像)を情報処理装置1に対して出力する。
次に、情報処理装置1について説明する。画像入力部120は、撮像部170から(第1のカメラ及び第2のカメラのそれぞれから)時系列に撮像画像を入力し(例えば毎秒30フレーム)、該入力した撮像画像を後段の第1の導出部130及び第2の導出部160に対して送出する。
第1の導出部130は、画像入力部120から入力された撮像画像と、保持部110に保持されている三次元マップと、に基づき、該撮像画像を撮像したときの撮像部170の世界座標系における位置及び姿勢を導出する(第1の取得処理)。世界座標系とは、例えば、現実空間(シーン)における1点を原点とし、該原点で互いに直交する3軸をそれぞれx軸、y軸、z軸とする座標系である。本実施形態では、撮像部170(ステレオカメラ)を構成する2つのカメラ(第1のカメラ及び第2のカメラ)のうち一方(第1のカメラ)の世界座標系における位置及び姿勢を「世界座標系における撮像部170の位置及び姿勢」として求める。本実施形態では、世界座標系における第1のカメラの位置及び姿勢は、世界座標系における第1のカメラの位置を表す三次元ベクトル、世界座標系における第1のカメラの姿勢を表す三次元ベクトル、を合わせた六次元ベクトルのことを表すものとする。第1のカメラの姿勢は、世界座標系の原点を通る回転軸とその回転軸まわりの回転角によって表す。姿勢を表す三次元ベクトルの方向が回転軸の向き、ノルムが回転角を表すものとする。また本実施形態では、第1のカメラの光軸をZ軸、第1のカメラの撮像面の水平方向をX軸、第1のカメラの撮像面の垂直方向をY軸とする三次元の座標系をカメラ座標系と呼び、第1のカメラの位置及び姿勢を撮像部170の位置及び姿勢と呼ぶ。
また、第1のカメラ及び第2のカメラのそれぞれの内部パラメータ(焦点距離fx(画像の水平方向)、fy(画像の垂直方向)、画像中心位置cx(画像の水平方向)、cy(画像の垂直方向)、レンズ歪みパラメータ)は既知であるとする。カメラの内部パラメータは、例えばZhangの方法(Z.Zhang,“A flexible new technique for camera calibration,” IEEE Trans. on Pattern Analysis and Machine Intelligence,vol.22,no.11,pp.1330-1334,2000.)により事前に校正しておく。また、撮像部170を構成する二台のカメラ間(第1のカメラと第2のカメラとの間)の相対的な位置及び姿勢(外部パラメータ)も事前に校正しておく。ステレオカメラの外部パラメータは、例えば三次元形状が既知のパターンを同時に左右のカメラで撮像してパターンを基準とした各カメラの位置及び姿勢を求め、夫々の位置及び姿勢をステレオカメラの左側カメラ(本実施形態の場合は第1のカメラ)を基準とした位置及び姿勢に変換して求める。撮像部170の内部パラメータと外部パラメータは情報処理装置1内の不図示のメモリ若しくは情報処理装置1がアクセス可能な外部メモリに保持されるものとする。
保持部110は、図3に示す如く現実空間(シーン)中に人為的に配置したそれぞれの補助特徴の配置情報(世界座標系における補助特徴の位置及び姿勢と該補助特徴の識別子)を保持している。本実施形態では、補助特徴として、図3に示すように内部に個別の識別子を持つ白黒の正方形の特徴を利用する。正方形中の所定の位置の白黒パターンを参照することにより、正方形の識別子を特定することができる。補助特徴の世界座標系における位置及び姿勢は、例えば特許第04532982号(または米国特許第7529387号)に開示される方法により事前に校正しておく。
保持部110は更に、第1の導出部130で利用するシーンの三次元マップを保持する。本実施形態では、非特許文献1と同様、三次元マップをキーフレームの集合として保持する。
各キーフレームは、撮像部170が撮像した撮像画像(濃淡画像)I、該撮像画像と同じ画像サイズのデプスマップD、分散マップVを保持する。デプスマップDにおける各画素は、撮像部170から該画素に対応する対応点までの距離(奥行き値)の逆数(inverse depth)、すなわちカメラ座標系における該対応点のZ座標の逆数を保持する。本実施形態では非特許文献1と同様、濃淡画像I上の画素のうち輝度勾配が閾値よりも大きい画素を特徴点とし、デプスマップDにおいて該特徴点に対応する位置の画素に、該特徴点に対応する奥行き値の逆数を登録するものとする。分散マップVの各画素は、inverse depthの分散を保持する。
さらに各キーフレームは、該キーフレームが保持する撮像画像の撮像時における撮像部170の世界座標系における位置及び姿勢を属性として保持する。以下では、キーフレームが保持する属性(世界座標系における撮像部170の位置及び姿勢)を「キーフレームの位置及び姿勢」と称する場合がある。
さらに各キーフレームは、該キーフレームが保持する撮像画像中の補助特徴に基づいて推定される撮像部170の世界座標系における位置及び姿勢が「参照位置及び参照姿勢」として保持可能である。
さらに各キーフレームは、マップ最適化で利用するための「キーフレーム対応情報」を保持可能である。キーフレーム対応情報は、キーフレームのペアを構成する2個のキーフレームのそれぞれの識別番号と、キーフレーム間の相対的な位置及び姿勢を保持する。キーフレーム対応情報は、マップ最適化において利用される。
更新部140は、撮像画像と、第1の導出部130によって導出された撮像部170の位置及び姿勢と、に基づいて、保持部110に保持されている三次元マップを更新する。
補正部150は、第2の導出部160が導出するキーフレーム間の相対的な位置及び姿勢を利用して、保持部110に保持されている三次元マップを補正する。このような補正により、三次元マップの蓄積誤差を低減し幾何的な整合がとれたマップにする。
第2の導出部160は、画像入力部120から入力された撮像画像から検出された補助特徴に基づいて撮像部170の位置及び姿勢を参照位置及び参照姿勢として導出する処理や、キーフレーム間の相対的な位置及び姿勢を導出する処理を行う。導出された相対的な位置及び姿勢は、補正部150で利用される。
制御部199は、情報処理装置1全体の動作制御を行う。例えば制御部199は、上記の画像入力部120、第1の導出部130、第2の導出部160、更新部140、補正部150の各機能部の動作制御を行う。次に、情報処理装置1の動作について、図4のフローチャートに従って説明する。
ステップS1010では、保持部110が保持する三次元マップの初期化処理が行われる。三次元マップの初期化処理では、更新部140は、画像入力部120が入力したステレオ画像(第1のカメラによる撮像画像及び第2のカメラによる撮像画像のセット)のうち第1のカメラによる撮像画像を基準画像とする。以下では、第1のカメラによる撮像画像を第1の撮像画像、第2のカメラによる撮像画像を第2の撮像画像と称する場合がある。そして更新部140は、基準画像上で輝度勾配が閾値以上の画素を特徴点とし、該基準画像と同セットの第2の撮像画像において該特徴点に対応する対応特徴点をステレオマッチングにより探索する。そして更新部140は、特徴点と対応特徴点とを用いた三角測量によりカメラ座標系における特徴点のinverse depthを算出する。そして更新部140は、該算出した特徴点のinverse depthを、デプスマップDにおいて該特徴点に対応する画素に登録する。さらに更新部140は、デプスマップD上の各画素について、左右画像(第1の撮像画像及び第2の撮像画像)の視差及び撮像部170の基線長、画像ノイズの分散を用いて算出したinverse depthの分散を推定し、分散マップVに保存する。そして更新部140は、基準画像を濃淡画像Iとし、該濃淡画像I、デプスマップD、分散マップVを含むキーフレームを生成して、三次元マップに追加する。なお、この時点で三次元マップに、濃淡画像Iの撮像時における撮像部170の世界座標系における位置及び姿勢を登録しても良い。濃淡画像Iの撮像時における撮像部170の世界座標系における位置及び姿勢はどのような方法で求めても良い。
ステップS1020では、画像入力部120は、撮像部170から入力されたステレオ画像を取得する。ステップS1030では、第1の導出部130は、ステップS1020で画像入力部120が入力したステレオ画像と、保持部110に保持されている三次元マップと、を用いて、撮像部170の世界座標系における位置及び姿勢を導出する。ステレオ画像と三次元マップとを用いて撮像部170の位置及び姿勢を導出する方法は周知であり、例えば、非特許文献1に開示される方法を利用することができる。
ステップS1040では、更新部140は、ステップS1030で導出した撮像部170の位置及び姿勢に基づいて、保持部110に保持されているキーフレームの更新、若しくは保持部110への新規のキーフレームの追加、を行う。ステップS1040における処理の詳細については後述する。
ステップS1050における処理は、ステップS1040において新規のキーフレームを保持部110に追加した場合に行われる。ステップS1050では、第2の導出部160は、三次元マップの補正で利用するためのキーフレーム間の相対的な位置及び姿勢を導出する。ステップS1050における処理の詳細については後述する。
ステップS1060では、補正部150は、ステップS1050で得られたキーフレーム間の相対的な位置及び姿勢を利用して三次元マップを補正する。キーフレームが保持するデプスマップDの補正まで行うと計算時間がかかるため、本実施形態ではキーフレームの世界座標系における位置及び姿勢のみを補正する。世界座標系におけるキーフレームの位置及び姿勢の補正は、世界座標系におけるキーフレームの位置及び姿勢から算出される相対的な位置及び姿勢とステップS1050で得られたキーフレーム間の相対的な位置及び姿勢の誤差の総和を評価関数として行う。ここで、キーフレームiとキーフレームjの世界座標系における位置及び姿勢を表す4×4行列をそれぞれM(i)、M(j)、キーフレームiとキーフレームjの相対的な位置及び姿勢を表す4×4行列をMrel(i,j)とする。行列M(i)、M(j)、Mrel(i,j)は、次式に示すように、左上の3×3行列が回転行列R、一番右の列が平行移動成分tを表す行列である。
行列M(i)、M(j)は、世界座標系における位置及び姿勢をカメラ座標系における位置及び姿勢に変換する行列であるとする。また行列Mrel(i,j)は、キーフレームjのカメラ座標系における位置及び姿勢をキーフレームiのカメラ座標系における三次元位置に変換する行列であるとする。ここでは、行列Mrel(i,j)を既知として、下記の式(1)が最小となるようなM(i)、M(j)を求める。
なお、||行列||Fはフロベニウスのノルム(行列の各要素の二乗和)を表す。また、(式1)は三次元マップに保存されたすべての「キーフレーム対応情報」についての総和である。M(i)、M(j)は非線形の変換であるため、例えば反復計算を行うGauss-Newton法により算出する。M(i)、M(j)の初期値としては、ステップS1030で導出された位置及び姿勢を用いるものとする。最適化されたM(i)、M(j)は再び三次元マップ中のキーフレームの属性として保存し、次にマップ最適化を行う際の初期値として利用する。
ステップS1070では、制御部199は、図4のフローチャートに従った処理の終了条件が満たされた否かを判断する。終了条件には様々なものが考えられる。例えば、ユーザが不図示の操作部を操作することで「図4のフローチャートに従った処理の終了指示」を入力したことを制御部199が検知した場合には、制御部199は終了条件が満たされたと判断する。終了条件が満たされた場合には、図4のフローチャートに従った処理は完了する。一方、終了条件が満たされていない場合には、処理はステップS1070を介してステップS1020に戻る。
次に、上記のステップS1040における処理の詳細について説明する。ステップS1040では、更新部140は、ステップS1030で導出した撮像部170の位置及び姿勢に基づいて、保持部110に保持されているキーフレームの更新、若しくは保持部110への新規のキーフレームの追加、を行う。
例えば更新部140は、保持部110に保持されているキーフレームのうち、ステップS1030で導出した撮像部170の位置及び姿勢に最も近い位置及び姿勢を属性として有するキーフレームを最近傍キーフレームとして特定する。そして更新部140は、最近傍キーフレームに含まれている濃淡画像Iにおける特徴点を求めると共に、ステップS1020で画像入力部120が入力した第1の撮像画像において該特徴点に対応する対応特徴点を求める。そして更新部140は、最近傍キーフレームに含まれている濃淡画像Iにおける特徴点の画素数に対する、ステップS1020で画像入力部120が入力した第1の撮像画像における対応特徴点の画素数の割合Rinを求める。そして更新部140は、Rinが閾値Tin未満の場合、最近傍キーフレームとのオーバーラップが少ないためキーフレームを追加すると判定する。一方、更新部140は、Rinが閾値Tin以上の場合、最近傍キーフレームとのオーバーラップが多いためキーフレームを追加しない(保持部110に保持されているキーフレームを更新する)と判定する。
キーフレームを追加すると判定した場合、更新部140は、以下のような処理を行うことで、新規のキーフレームを三次元マップに追加する。先ず更新部140は、ステップS1020で画像入力部120が入力したステレオ画像を用いてステップS1010で説明した処理を行うことで、カメラ座標系における特徴点のinverse depthを算出する。そして更新部140は、該算出したinverse depthにより既存のキーフレーム(三次元マップに登録済み(過去に登録した)のキーフレーム)から伝播されたinverse depthを更新することで新たなデプスマップDを生成する。また、更新部140は、既存のキーフレームが保持していたinverse depthの分散及び新たに算出されたinverse depthの分散を用いて分散マップVを生成する。そして更新部140は、ステップS1020で画像入力部120が入力した第1の撮像画像を濃淡画像Iとし、該濃淡画像I、ステップS1040で生成したデプスマップD及び分散マップV、を含み、且つステップS1030で導出した撮像部170の位置及び姿勢を属性として更に含むキーフレームを生成して三次元マップに追加する。
一方、キーフレームを追加しないと判定した場合、更新部140は、以下のような処理を行うことで、保持部110に保持されているキーフレームを更新する。より具体的には、更新部140は、ステップS1030で第1の導出部130が導出した位置及び姿勢、ステップS1020で入力されたステレオ画像を用いて既存のキーフレームのデプスマップD、分散マップVを更新する。非特許文献1に開示される方法と同様に、デプスマップD、分散マップVの更新は、ステレオ画像から推定されるinverse depthと、キーフレームが保持する濃淡画像とのモーションステレオから推定されるinverse depthを用いて行う。
次に、上記のステップS1050における処理の詳細について、図5のフローチャートに従って説明する。ステップS1110では、第2の導出部160は、第1の撮像画像及び第2の撮像画像のそれぞれについて、該撮像画像から補助特徴を検出する。図3に示すような補助特徴の検出は例えば次のようにして行う。まず、撮像画像を二値化して二値化画像を生成し、該二値化画像から四角形領域を検出し、該四角形領域の各頂点の画像座標をもとに正方形から画像座標への射影変換を算出し、四角形領域を正方形に変換する。そして正方形の所定の位置に相当する輝度値を画像から読み出すことにより補助特徴の識別子を特定する。このとき、該特定した識別子と保持部110に保持されている補助特徴の識別子との照合を行い、互いに識別子が一致する場合には各頂点の画像座標と世界座標系における三次元座標との対応を保存する。補助特徴の頂点の世界座標系における三次元座標は、補助特徴に規定された座標系における頂点の三次元座標と、補助特徴の世界座標系における位置及び姿勢より算出する。
ステップS1120では、第2の導出部160は、補助特徴から得られる情報が十分であるか否かを判断する。ここでは、ステップS1110で対応が保存された補助特徴の頂点の画像上の分布を利用して補助特徴の情報が十分にあるかの判定を行う。具体的には、第1の撮像画像及び第2の撮像画像のそれぞれについて、補助特徴の頂点の画像座標の共分散行列を算出する。第1の撮像画像及び第2の撮像画像のそれぞれから算出した共分散行列の最小固有値Eig(L)、Eig(R)の何れかが閾値T_Eigより小さいという条件が満たされた場合には、補助特徴の情報が不十分であると判断し、処理はステップS1130に進む。一方、この条件が満たされていない場合には、ステップS1150に進む。
ステップS1130では第2の導出部160は、ステップS1040で新たに追加されたキーフレーム(追加キーフレーム)の位置及び姿勢(属性)に近い位置及び姿勢を属性として有するキーフレームを保持部110に保持されている既存キーフレーム群から特定する。具体的には追加キーフレームに対し、世界座標系における視軸方向(カメラ座標系のZ軸)の角度差が閾値TAngle以内(閾値以内)である既存キーフレームのうち世界座標系における位置の差が閾値TDist以内である全ての既存キーフレームを特定する。そして第2の導出部160は、該特定したそれぞれの既存キーフレームと追加キーフレームとの相対的な位置及び姿勢を算出し(第2の取得処理)、該算出した相対的な位置及び姿勢と該それぞれの既存キーフレームの識別番号と追加キーフレームの識別番号とを追加キーフレームのキーフレーム対応情報として保持部110に登録する。
ステップS1140では、第2の導出部160は、追加キーフレームに含まれている濃淡画像Iとの類似度が高い濃淡画像Iを含む既存キーフレームを保持部110から特定し、該特定した既存キーフレームとの相対的な位置及び姿勢を導出する。具体的には、第2の導出部160は、追加キーフレーム及び既存キーフレームのそれぞれに含まれている濃淡画像Iの縮小画像I’を生成する。そして第2の導出部160は、追加キーフレームと既存キーフレームとの間で縮小画像I’の輝度値のSSD(Sum of Squared Distance)が最小かつ閾値TSSD以下となる既存キーフレームを特定する。そして第2の導出部160は、該特定したそれぞれの既存キーフレームと追加キーフレームとの相対的な位置及び姿勢を同様に算出する(第2の取得処理)。そして第2の導出部160は、該算出した相対的な位置及び姿勢と、該それぞれの既存キーフレームの識別番号と、追加キーフレームの識別番号と、を追加キーフレームのキーフレーム対応情報として保持部110に登録する。
一方、ステップS1150で第2の導出部160は、ステップS1110で得られた補助特徴の頂点の画像座標と世界座標系における三次元座標の対応に基づいて、追加キーフレームの参照位置及び参照姿勢を算出し、追加キーフレームに登録する。登録位置及び登録姿勢の算出は、例えばLepetitらの手法(V. Lepetit, F. Moreno-Noguer, and P. Fua, “EPnP: an accurate O(n) solution to the PnP problem,” International Journal of Computer Vision, vol.81,no.2,pp.155-166,2009.)により行う。
ステップS1160では、第2の導出部160は、追加キーフレームと、参照位置及び参照姿勢を保持する既存キーフレームと、の相対的な位置及び姿勢を算出する(第2の取得処理)。追加キーフレームの参照位置及び参照姿勢を表す4×4行列をMnew、既存キーフレームの参照位置及び参照姿勢を表す4×4行列をMoldとする(行列Mnew、Moldは、世界座標系における位置及び姿勢をカメラ座標系における位置及び姿勢に変換する行列であるとする)。このとき、キーフレーム間の相対的な位置及び姿勢Mrel(new、old)は以下の(式2)により算出する。
参照位置及び参照姿勢を保持する既存キーフレームが複数存在する場合には、追加キーフレームの前側にありかつ一定距離以上離れた既存キーフレームのうちキーフレームの視軸方向(カメラ座標系のZ軸)の間の角度が最も大きい既存キーフレームを選択する。これは、なるべく異なる視点を持つキーフレームをペアにすることで、三次元マップの補正によってより多くの累積誤差が削減されることを目的としている。選択されたペアの相対的な位置及び姿勢と、該選択されたペアにおけるそれぞれのキーフレームの識別番号と、は追加キーフレームにおけるキーフレーム対応情報として保持部110に登録する。
このように、本実施形態によれば、シーン中に配置した補助特徴を利用して導出されるキーフレーム間の相対的な位置及び姿勢を三次元マップの補正に利用することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。
<変形例>
第1の実施形態では、撮像部170はスレテオカメラであるものとして説明したが、シーンに配置した補助特徴を検出可能な画像を撮像することができる装置であれば、撮像部170としてスレテオカメラ以外の装置を用いても良い。例えば、撮像部170は単眼カメラであってもよいし、リジッドに固定されてカメラ間の相対的な位置及び姿勢が既知な三台以上のカメラであってもよい。また、撮像部170は、カラー画像と距離画像を撮像するRGBDカメラであってもよい。
また、補助特徴の形状は四角形に限るものではなく、画像から安定的に検出可能な特徴であれば如何なる特徴であっても良い。例えば、三角形、六角形、円形、楕円形の補助特徴を使用しても良い。また、補助特徴の識別子は画像から特定できなくてもよい。例えば、円形の補助特徴を用いる場合、第1の導出部130が導出する撮像部170の位置及び姿勢をもとに補助特徴の画像上の位置を算出し、近傍で検出された補助特徴と対応付けることで特定してもよい。また識別子つきの補助特徴と識別子のない補助特徴を併用してもよい。この場合、識別子つきの補助特徴から算出される撮像部170の位置及び姿勢をもとに識別子のない補助特徴の画像上の位置を算出し、近傍で検出された補助特徴と対応付けてもよい。
また、補助特徴の配置情報の校正は必ずしも事前に行わなくてもよい。例えば、撮像部170によって撮像される画像を用いて、シーンの三次元マップ作成と並行して行ってもよい。その際には、補助特徴を用いた三次元マップの補正は随時行わずに、補助特徴の配置情報の信頼度が十分高くなった場合(例えば信頼度が閾値以上の場合)に行うようにすることで三次元マップの精度が低下しないようにする。補助特徴の配置情報の信頼度の判定は、例えば校正に用いた画像群において各補助特徴が検出された回数の平均値や最小値に基づいて行い、検出回数の平均値や最小値が大きいほど信頼度が高いとする。
また、補助特徴の情報が十分にあるかどうかの判断基準は、補助特徴の画像上の分布に限るものではない。例えば、画像上で検出される補助特徴の個数が閾値以上の場合に、補助特徴の情報が十分にあると判断しても良い。また、補助特徴の個数と画像上の分布を組み合わせたものに基づいていてもよい。
また、参照位置及び参照姿勢を保持するキーフレームの中から相対的な位置及び姿勢を算出するために使用するキーフレームを特定する際には、キーフレームの間で共通の補助特徴が観察されているキーフレームのみを対象とするようにしてもよい。
[第2の実施形態]
本実施形態を含め、以下に説明する実施形態や変形例では第1の実施形態との差分について説明し、以下で特に触れない限りは第1の実施形態と同様であるものとする。本実施形態では、撮像部170に位置及び姿勢を計測するセンサを装着し、このセンサの計測値を三次元マップの補正に利用する場合について説明する。
本実施形態に係るシステムの機能構成例について、図6のブロック図を用いて説明する。図6において図2に示した機能部と同じ機能部には同じ参照番号を付しており、該機能部に係る説明は省略する。図6に示す如く、本実施形態に係るシステムは、撮像部170、センサ290、情報処理装置2、を有する。
先ず、センサ290について説明する。センサ290は、撮像部170の世界座標系における位置及び姿勢を計測し、その計測値を情報処理装置2に対して出力する。本実施形態では、センサ290はBauerらの文献(M. Bauer, M. Schlegel, D. Pustka, N. Navab, and G. Klinker, “Predicting and estimating the accuracy of n-occular optical tracking systems,” Proc. 5th IEEE and ACM International Symposium on Mixed and Augmented Reality, pp.43-51, 2006.)で開示されている光学式の位置姿勢センサであるとする。この光学式の位置姿勢センサは、計測対象(本実施形態では撮像部170)上に複数のマーカを配置し、配置されたマーカをシーン中に固定された複数のカメラによって撮影した画像を利用して計測対象の世界座標系における位置及び姿勢を計測するものである。
次に、情報処理装置2について説明する。センサ入力部280は、センサ290から出力された計測値を入力し、該計測値を後段の第2の導出部260に送出する。第2の導出部260は、計測値に基づいて撮像部170の位置及び姿勢を参照位置及び参照姿勢として導出する処理や、キーフレーム間の相対的な位置及び姿勢を導出する処理を行う。導出された相対的な位置及び姿勢は、補正部150で利用される。
次に、情報処理装置2の動作について、図7のフローチャートに従って説明する。図7において図4に示した処理ステップと同じ処理ステップには同じステップ番号を付しており、該処理ステップに係る説明は省略する。
ステップS2020では、画像入力部120は、撮像部170から入力されたステレオ画像を取得し、センサ入力部280は、センサ290から入力された計測値(世界座標系における撮像部170の位置及び姿勢)を取得する。
ステップS2040では、更新部140は上記のステップS1040と同様にして新規のキーフレームを三次元マップに追加する。ここで、追加するキーフレームの属性、参照位置及び参照姿勢としては、ステップS2020で取得した撮像部170の位置及び姿勢を適用する。
ステップS2050では、第2の導出部260は、追加キーフレームにおける参照位置及び参照姿勢として、ステップS2020で取得した計測値が示す位置及び姿勢を用いる。そして第2の導出部260は上記のステップS1160と同様にして、追加キーフレームと、参照位置及び参照姿勢を保持する既存キーフレームと、の相対的な位置及び姿勢を算出する。そして第2の導出部260は、参照位置及び参照姿勢を保持する既存キーフレームのうち追加キーフレームとの距離が最も大きい既存キーフレームを選択し、該選択した既存キーフレームとの相対的な位置及び姿勢と該選択した既存キーフレームの識別番号と追加キーフレームの識別番号とをキーフレーム対応情報として追加キーフレームに登録する。
このように、本実施形態によれば、位置及び姿勢を計測するセンサの計測値をもとにキーフレーム間の相対的な位置及び姿勢を導出することで、カメラの移動範囲が限定される場合であっても三次元マップに蓄積する誤差を低減できる。
<変形例>
センサ290は光学式の位置姿勢センサに限るものではなく、他の種類の位置姿勢センサであってもよい。例えば、磁気式の位置姿勢センサであってもよい。また、位置及び姿勢の双方を計測する必要はなく、位置または姿勢のいずれかを計測するセンサであってもよい。すなわち、ジャイロスコープや加速度センサ、磁気センサを利用して撮像部170の姿勢を計測する姿勢センサであってもよいし、GPS(Global Positioning System)のような位置を計測する位置センサであってもよい。位置または姿勢のいずれかを計測するセンサを利用する場合には、相対的な位置又は姿勢を取得することになり、該相対的な位置又は姿勢に基づいて三次元マップを補正する(センサが計測する成分のみを補正する)ことになる。また、位置を計測するセンサと姿勢を計測するセンサを組み合わせて位置姿勢センサとして利用してもよい。
また、シーンの三次元マップの作成に利用可能な画像を撮像することができる装置であれば、撮像部170としてスレテオカメラ以外の装置を用いても良い。例えば、撮像部170は単眼カメラであってもよいし、リジッドに固定されてカメラ間の相対的な位置及び姿勢が既知な三台以上のカメラであってもよい。また、撮像部170は、距離画像を撮像するデプスカメラであっても良いし、カラー画像と距離画像を撮像するRGBDカメラであってもよい。
[第3の実施形態]
本実施形態では、非特許文献1のようにマップ最適化において各キーフレームの位置及び姿勢のみを補正するのではなく、特徴点の三次元座標も補正する。本実施形態に係るシステムの機能構成例について、図8のブロック図を用いて説明する。図8において図2に示した機能部と同じ機能部には同じ参照番号を付しており、該機能部に係る説明は省略する。図8に示す如く、本実施形態に係るシステムは、撮像部170と、情報処理装置3と、を有する。
補正部350は、第2の導出部160が導出するキーフレーム間の相対的な位置及び姿勢を利用して三次元マップを補正する。第1の実施形態における補正部150では、計算時間を削減するためにキーフレームの位置及び姿勢のみ補正しており、三次元マップの精度は低かった。一方、本実施形態では、計算時間をかけてキーフレームの位置及び姿勢とともに各キーフレームが持つデプスマップも補正することで、三次元マップの精度を向上させる。
次に、情報処理装置3の動作について、図4のフローチャートに従って説明する。情報処理装置3は、図4のフローチャートに従った処理においてステップS1060の処理を図9のフローチャートに従った処理に置き換えた処理を実行する。
本実施形態に係るステップS1060では、補正部350は、ステップS1050で得られたキーフレーム間の相対的な位置及び姿勢を制約条件として三次元マップを補正する。三次元マップの補正では、(1)各キーフレームにおける特徴点の再投影誤差の総和、(2)キーフレームの相対的な位置及び姿勢の誤差の総和(式1)、の和を評価関数として各キーフレームの位置及び姿勢と各特徴点の三次元座標を補正する。
本実施形態に係るステップS1060では先ず、各キーフレームがデプスマップに保持する特徴点と他のキーフレームがデプスマップに保持する特徴点との対応付けを行う。次に、特徴点の対応付けの結果と、キーフレーム間の相対的な位置及び姿勢と、を用いて三次元マップを補正する。ここで、三次元マップが保持するキーフレームの総数をNkとする(つまり、キーフレームの識別番号は1~Nk)。
ステップS3110では、補正部350は、変数nの値を1に初期化する。ステップS3120では、補正部350は、n>Nkであるか否かを判断する。この判断の結果、n>Nkであれば、全てのキーフレームにおける特徴点の対応付けが完了したものとして処理はステップS3160に進み、n≦Nkの場合には、処理はステップS3130に進む。
ステップS3130で補正部350はステップS1130と同様の処理を行うことで、キーフレームn(識別番号がnのキーフレーム)の位置及び姿勢に近い位置及び姿勢を有するキーフレームを保持部110に保持されている既存キーフレーム群から特定する。更に補正部350は、上記のステップS1140と同様の処理を行うことで、キーフレームnに含まれている濃淡画像Iとの類似度が高い濃淡画像Iを含む既存キーフレームを保持部110から特定する。
ステップS3140では、補正部350は、キーフレームnにおける特徴点と、ステップS3130で特定したキーフレーム(特定キーフレーム)の特徴点と、の対応付けを行う。この対応付けの結果は、ステップS3160におけるパラメータ更新において、特徴点の三次元座標をキーフレーム毎に更新するのではなく、キーフレーム間で共通のパラメータとして更新するのに用いる。特徴点の対応付けは次のようにして行う。まず、各キーフレームの世界座標系における位置及び姿勢、デプスマップから算出される特徴点の三次元座標、撮像部170の内部パラメータ、を用いて、特定キーフレームにおいて、キーフレームnにおける特徴点に対応する位置(un、vn)を算出する。次に、特定キーフレームにおける特徴点のうち、位置(un、vn)との距離が閾値Tneighbour以下となる位置にある特徴点を選択する。そして、キーフレームnにおける特徴点に対し、選択された特徴点のうち、特徴点近傍のN×N画素の輝度値の二乗誤差の総和が閾値Tdiff以内となる特徴点が存在する場合には、そのうち最も二乗誤差の総和の小さい特徴点がキーフレームn上の特徴点と対応付くものとし、それぞれに同一の識別番号を割り振る。ステップS3150では、補正部350は、変数nの値を1つインクリメントする。そして処理はステップS3120に戻る。
一方、ステップS3160では、補正部350は、未知パラメータである「世界座標系における各キーフレームの位置及び姿勢」及び「各特徴点の三次元座標」を更新する。前述したように、(1)三次元マップを構成する各キーフレームにおける特徴点の再投影誤差の総和、(2)キーフレームの相対的な位置及び姿勢の誤差の総和、の和が最小となるように未知パラメータを推定する。ここで、再投影誤差とは、キーフレームの位置及び姿勢、特徴点の三次元座標、撮像部170の内部パラメータ、をもとに算出される画像上の位置(ucalc,vcalc)と実際の特徴点の画像上の検出位置(uobs,vobs)の差のことを言う。ここで再投影誤差の総和Ereprojを以下の(式3)のように表す。
(ucalc(i,j),vcalc(i,j))、はi番目のキーフレームにおけるj番目の特徴点の画像上の算出位置であるとする。(uobs(i,j),vobs(i,j))、はi番目のキーフレームにおけるj番目の特徴点の画像上の検出位置であるとする。またキーフレームの相対的な位置及び姿勢の誤差の総和Erelativeを以下の(式4)のように表す。
ここでM(s)、M(t)はそれぞれキーフレームsとキーフレームtの世界座標系における位置及び姿勢を表す4×4剛体変換行列である。Mrel(s,t)は、キーフレームsとキーフレームtの相対的な位置及び姿勢を表す4×4行列であり、ステップS1050から得られる。ステップS3160では、以下の(式5)に示す(式3)と(式4)の重み付け和Etotalが最小となるように未知パラメータを推定する。
(式5)を最小化する未知パラメータの推定はバンドル調整により行う。但し、一般にバンドル調整は再投影誤差を最小化する方法である。そのため、再投影誤差とキーフレーム間の位置及び姿勢の差分の和が最小となるように位置及び姿勢を推定するSkrypnykらの方法(I. Skrypnyk and D. G. Lowe, “Scene modelling, recognition and tracking with invariant image features,” Proc. 3rd IEEE and ACM International Symposium on Mixed and Augmented Reality, pp.110-119, 2004.)と組み合わせて推定を行う。
ステップS3170では、補正部350は、ステップS3160で推定された世界座標系における特徴点の三次元座標を各キーフレームのカメラ座標系における三次元座標に変換し、該変換した三次元座標のZ成分をinverse depthに変換する。
ステップS3180では、補正部350は、キーフレームの位置及び姿勢をステップS3160で推定された世界座標系におけるキーフレームの位置及び姿勢に更新する。また補正部350は、キーフレームに含まれているデプスマップDにおいて特徴点のinverse depthを、ステップS3170で変換された該特徴点のinverse depthに更新する。
このように、本実施形態によれば、シーン中に配置した補助特徴から得られるキーフレーム間の相対的な位置及び姿勢を用いてキーフレームの位置及び姿勢に加えて特徴点の三次元座標も補正することで、三次元マップを高精度化することができる。
<変形例>
三次元マップの保持方法は、キーフレームが個別にデプスマップDを保持する方法に限るものではない。例えば、Kleinらの方法(G. Klein and D. Murray,“Parallel Tracking and Mapping for Small AR Workspaces,” Proc.6th IEEE and ACM International Symposium on Mixed and Augmented Reality,pp.225-234,2007.)のように、三次元マップの中に唯一のデプスマップを保持していてもよい。
また、キーフレーム間の相対的な位置及び姿勢はシーン中に配置した補助特徴から導出する必要は必ずしもなく、第2の実施形態のように撮像装置の位置または姿勢を計測するセンサから取得してもよい。
[第4の実施形態]
図2,6,8に示した各機能部はハードウェアで実装しても良いが、保持部110を除く各機能部をソフトウェア(コンピュータプログラム)で実装しても良い。後者の場合、保持部110として機能するメモリを有し、該コンピュータプログラムを実行可能なコンピュータ装置は、上記の各実施形態や各変形例に係る情報処理装置に適用可能である。上記の各実施形態や各変形例に係る情報処理装置に適用可能なコンピュータ装置のハードウェア構成例について、図1のブロック図を用いて説明する。
CPU10は、ROM20やRAM30に格納されているコンピュータプログラムやデータを用いて処理を実行する。これによりCPU10は、コンピュータ装置全体の動作制御を行うと共に、情報処理装置が行うものとして上述した各処理を実行若しくは制御する。ROM20には、コンピュータ装置の設定データや各種の基本プログラムが格納されている。
RAM30は、外部記憶装置70からロードされたコンピュータプログラムやデータ、入力I/F(インターフェース)40を介して外部(撮像部170やセンサ290)から受信したデータ、を格納するためのエリアを有する。更にRAM30は、CPU10が各種の処理を実行する際に用いるワークエリアを有する。このようにRAM30は、各種のエリアを適宜提供することができる。
入力I/F40は、外部からの入力を受信するためのI/Fとして機能するものであり、例えば、撮像部170やセンサ290から出力される撮像画像や計測値は、この入力I/F40を介して受信される。
表示部60は、CRTや液晶画面などにより構成されており、CPU10による処理結果を画像や文字などでもって表示することができる。また表示部60はタッチパネル画面であっても良い。
外部記憶装置70は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置70には、OS(オペレーティングシステム)や、情報処理装置が行うものとして上述した各処理をCPU10に実行させるためのコンピュータプログラムやデータが保存されている。外部記憶装置70に保存されているコンピュータプログラムには、図2,6,8において保持部110を除く各機能部の機能をCPU10に実現させるためのコンピュータプログラムが含まれている。また、外部記憶装置70に保存されているデータには、上記の説明において既知の情報として説明したもの(例えば閾値や各種のパラメータ)が含まれている。外部記憶装置70に保存されているコンピュータプログラムやデータは、CPU10による制御に従って適宜RAM30にロードされ、CPU10による処理対象となる。なお、上記の保持部110や不図示のメモリとして説明したものは、ROM20やRAM30、外部記憶装置70によって実装可能である。
操作部80は、キーボードやマウスなどのユーザインターフェースにより構成されており、ユーザが操作することで各種の指示をCPU10に対して入力することができる。例えば、ユーザは操作部80を操作することで図4,7に示した処理の終了指示を入力することができる。
CPU10、ROM20、RAM30、入力I/F40、表示部60、外部記憶装置70、操作部80、は何れもバス90に接続されている。なお、図1に示したハードウェア構成は、上記の情報処理装置に適用可能なハードウェア構成の一例に過ぎない。
[第5の実施形態]
本実施形態では、上記の各実施形態に適用可能なGUI(グラフィカルユーザインターフェース)について説明する。例えば、図4のフローチャートに従って補正された三次元マップに基づいて仮想空間を構築し、該仮想空間を任意の視点(仮想カメラ)から見た画像(仮想空間画像)を提示するGUI(ウィンドウ)の一例を図10に示す。このGUIは、例えば、表示部60に表示される。
このGUIには、仮想カメラから見える仮想空間の仮想空間画像に加え、特徴点(特徴点の三次元座標を画面に投影した位置に表示)、キーフレームの位置及び姿勢を示すオブジェクト、を描画する。本実施形態では、特徴点は丸い点、キーフレームを表すオブジェクトはカメラを模した錐体により表現する。ユーザは操作部80を操作して仮想カメラの位置や姿勢、内部パラメータ等を変更し、作成された三次元マップに対応する仮想空間を様々な視点から観察して確認することができる。キーフレーム対応情報が三次元マップに存在する場合には、対応するキーフレームを表す錐体の頂点同士(オブジェクト間)を線分で結ぶことにより、どのキーフレームが三次元マップの補正に利用されたかを確認することができる。その際に、キーフレームの対応がどのように生成されたかによって線分の色を変えることで補助特徴が効果的に利用されているかを確認できるようにする。すなわち、キーフレームの対応(キーフレーム間の相対的な位置及び姿勢の導出)が、ステップS1130、ステップS1140、ステップS1160のどのステップで得られたかにより錐体を結ぶ線分の色を変更する。このGUIには、特徴点とキーフレームに加えて、補助特徴を表示してもよい。
また、第2の実施形態で説明したセンサの計測値に基づいてキーフレームの対応が得られた場合にはさらに別の色の線分を描画する。なお、本実施形態では、線分の色を変えることに加え/若しくは代えて線分のパターン(実線、点線、太線、細線など)を変えても良く、変更する表示形態は色に限らない。
[第6の実施形態]
第1の実施形態では、正方形、三角形、円形などの補助的な特徴を三次元マップの補正に利用していた。しかしながら、補助的な特徴はこれに限るものではなく、位置及び姿勢の推定に必要な幾何情報が既知である三次元的な物体を用いてもよい。このような場合、図4のフローチャートに従った処理において、ステップS1050において次のような処理を行うようにすればよい。
つまり本実施形態に係るステップS1050では、第2の導出部160は、シーンに存在する、または配置した三次元的な物体(以下、三次元物体)を用いてキーフレーム間の相対的な位置及び姿勢を算出する。第1の実施形態では補助特徴の頂点の画像座標と三次元座標との対応に基づいてキーフレームの参照位置及び参照姿勢を算出していた。本実施形態では、三次元物体の幾何情報とキーフレームが保持する画像をもとに参照位置及び参照姿勢を算出する。具体的には、三次元物体のエッジモデルを保持しておき、Drummondらの手法(T.Drummond and R.Cipolla,“Real-time Visual Tracking of Complex Structures,” IEEE Transactions on Pattern Analysis and Machine Intelligence,vol.24,no.7,pp.932-946,2002.)により参照位置及び参照姿勢を算出する。
このように、本実施形態によれば、位置及び姿勢の推定に必要な幾何情報が既知な任意の三次元物体を補助特徴として利用することが可能であるため、シーンの景観を損ねることを防止できる。
なお、三次元物体を用いた参照位置及び参照姿勢の算出方法はエッジモデルを用いた方法に限るものではなく、三次元物体上の特徴点を用いたり、輝度情報を用いたりしてもよい。また、通常の画像だけでなく距離画像を利用できる場合には、距離画像への三次元モデルのフィッティングにより参照位置及び参照姿勢を算出してもよい。さらには通常の画像と距離画像の双方への三次元モデルのフィッティングを行ってもよい。
[第7の実施形態]
画像を用いた三次元マップの作成及び撮像部170の位置及び姿勢の計測は、非特許文献1で開示される方法に限るものではない。例えば、Kleinらの方法(G. Klein and D. Murray,“Parallel Tracking and Mapping for Small AR Workspaces,” Proc.6th IEEE and ACM International Symposium on Mixed and Augmented Reality,pp.225-234,2007.)のように明示的に検出された特徴点を利用するものであってもよい。また、RGBDカメラやデプスカメラを用いる方法であってもよい。
また、新しく追加されたキーフレームとの相対的な位置及び姿勢を算出するために選択されるキーフレームは必ずしも一つである必要はなく、複数のキーフレームを選択してもよい。例えば、新しく追加されたキーフレームから一定距離以上離れたキーフレームすべてを選択してもよいし、新しく追加されたキーフレームからの距離が大きい順に複数個のキーフレームを選択してもよい。
また、三次元マップの補正は、位置及び姿勢の導出、三次元マップの更新の後に逐次的に実施する必要はない。三次元マップの補正には時間がかかるため、位置及び姿勢の導出、三次元マップの更新と並列に実施してもよい。
また、上記の実施形態や変形例では、撮像部170やセンサ290と情報処理装置(情報処理装置1、2,3)とを別個にするのではなく、撮像部170やセンサ290を情報処理装置に組み込んで一体化させても良い。なお、以上説明した各実施形態や各変形例はその一部若しくは全部を適宜組み合わせて使用しても構わないし、選択的に使用しても構わない。
<効果のまとめ>
シーン中に配置した補助特徴を利用して導出されるキーフレーム間の相対的な位置及び姿勢を三次元マップの補正に利用することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。
また、位置及び姿勢を計測するセンサの計測値をもとにキーフレーム間の相対的な位置及び姿勢を導出することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。
さらに、補助特徴やセンサの計測値をもとに導出されるキーフレーム間の相対的な位置及び姿勢を用いてキーフレームの位置及び姿勢に加えて特徴点の三次元座標も補正することで、さらに三次元マップを高精度化することができる。
<定義のまとめ>
保持部110が保持する三次元マップは、撮像画像、画像撮像時の撮像部170の位置及び姿勢、現実空間中の幾何特徴の三次元情報から構成されるキーフレーム群、によって表されるものであれば何でもよい。例えば、現実空間中の幾何特徴の三次元情報は、現実空間を基準とした座標系における三次元座標であってもよいし、キーフレームを基準とした座標における三次元座標やデプスマップであってもよい。
また撮像部170は、現実空間の画像を撮像するカメラであれば何でもよい。例えば、濃淡画像を撮影するカメラでもよいし、カラー画像を撮影するカメラであってもよい。さらには、カメラは1台であってもよいし、ステレオカメラなどの複数台のカメラによって構成される撮像装置であってもよい。また距離画像を撮影するデプスカメラや、カラー画像と距離画像を同時に撮影するRGBDカメラであってもよい。
また、第1の導出部130は、入力画像と三次元マップをもとに入力画像の撮像時の撮像部170の位置及び姿勢を導出するものであれば何でもよい。例えば、濃淡画像上の輝度値の勾配が大きい点を特徴点として位置及び姿勢を導出するものでもよいし、特徴点を画像から明示的に検出して位置及び姿勢を導出するものでもよい。
また、更新部140は、撮像時の撮像部170の位置及び姿勢と、第1の導出部130が位置及び姿勢を導出した画像と、を用いて三次元マップを更新するものであれば何でもよい。例えば、導出された位置及び姿勢をもとに、既存の三次元マップ中の特徴点の三次元座標やデプスマップを更新してもよい。また、新規のキーフレームを生成し、三次元マップに追加してもよい。
また、第2の導出部160は、キーフレーム間の相対的な位置及び姿勢を導出するものであれば何でもよい。例えば、シーン中に補助的な特徴を別途配置し、画像上で検出される特徴の情報をもとに導出されるキーフレームの位置及び姿勢を用いて相対的な位置及び姿勢を導出してもよい。また、撮像部170に位置または姿勢を計測するセンサを装着し、センサによって計測される撮像部170の位置または姿勢をもとに相対的な位置及び姿勢を導出してもよい。
また、補正部150は、第2の導出部160から得られるキーフレーム間の相対的な位置及び姿勢をもとに三次元マップを補正するものであれば何でもよい。例えば、キーフレームの位置及び姿勢のみを補正してもよいし、さらに特徴点の三次元座標やデプスマップを補正してもよい。
(その他の実施例)
本発明は、上述の実施形態の1以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける1つ以上のプロセッサがプログラムを読出し実行する処理でも実現可能である。また、1以上の機能を実現する回路(例えば、ASIC)によっても実現可能である。