(57)【要約】【課題】  カメラ等の撮像装置の移動範囲が限られている場合であっても三次元マップに蓄積する誤差を低減させるための技術を提供すること。【解決手段】  撮像装置によるシーンの撮像画像と該シーンの三次元マップとに基づいて該撮像装置の位置及び姿勢を取得する第１の取得処理とは異なる第２の取得処理により撮像装置の位置及び姿勢を取得する。取得した位置及び姿勢と、該位置及び姿勢よりも過去に取得した位置及び姿勢と、の間の相対的な位置及び姿勢を取得する。取得した相対的な位置及び姿勢に基づいて、三次元マップを補正する。【選択図】  図２
2019020778.tif 000002
【特許請求の範囲】
【請求項１】
撮像装置によるシーンの撮像画像と該シーンの三次元マップとに基づいて該撮像装置の位置及び姿勢を取得する第１の取得処理とは異なる第２の取得処理により前記撮像装置の位置及び姿勢を取得する第１の取得手段と、
前記第１の取得手段が取得した位置及び姿勢と、該位置及び姿勢よりも過去に前記第１の取得手段が取得した位置及び姿勢と、の間の相対的な位置及び姿勢を取得する第２の取得手段と、
前記第２の取得手段が取得した相対的な位置及び姿勢に基づいて、前記三次元マップを補正する補正手段と
を備えることを特徴とする情報処理装置。
【請求項２】
前記第１の取得手段は、前記撮像画像に含まれている特徴に基づいて前記撮像装置の位置及び姿勢を取得する前記第２の取得処理を行うことを特徴とする請求項１に記載の情報処理装置。
【請求項３】
前記特徴は、前記シーン中に元来存在する特徴とは別の補助的な特徴であり、人為的にシーン中に配置した特徴であることを特徴とする請求項２に記載の情報処理装置。
【請求項４】
前記特徴は、幾何情報が既知である三次元物体の特徴であることを特徴とする請求項２に記載の情報処理装置。
【請求項５】
前記第１の取得手段は、前記第１の取得処理によって前記撮像装置の位置及び姿勢を取得し、
前記第２の取得手段は、前記第１の取得処理によって取得した位置及び姿勢と、該位置及び姿勢との差が閾値以内となる位置及び姿勢と、の間の相対的な位置及び姿勢を取得することを特徴とする請求項１乃至４の何れか１項に記載の情報処理装置。
【請求項６】
前記第１の取得手段は、前記第１の取得処理によって前記撮像装置の位置及び姿勢を取得し、
前記第２の取得手段は、前記撮像装置が類似した撮像画像を撮像したときの前記第１の取得処理によって取得したそれぞれの位置及び姿勢の間の相対的な位置及び姿勢を取得することを特徴とする請求項１乃至５の何れか１項に記載の情報処理装置。
【請求項７】
前記第１の取得手段は、前記撮像装置の位置及び／又は姿勢を計測するセンサから前記撮像装置の位置及び／又は姿勢を取得し、
前記第２の取得手段は、前記第１の取得手段が取得した位置及び／又は姿勢と、該位置及び／又は姿勢よりも過去に前記第１の取得手段が取得した位置及び／又は姿勢と、の間の相対的な位置及び／又は姿勢を取得し、
前記補正手段は、前記第２の取得手段が取得した相対的な位置及び／又は姿勢に基づいて、前記三次元マップを補正する
ことを特徴とする請求項１に記載の情報処理装置。
【請求項８】
前記三次元マップには、前記第１の取得処理によって取得された複数の位置及び姿勢が登録されており、
前記補正手段は、前記三次元マップに登録されている位置及び姿勢を、前記第２の取得手段が取得した相対的な位置及び姿勢に基づいて補正することを特徴とする請求項１乃至７の何れか１項に記載の情報処理装置。
【請求項９】
前記三次元マップには、前記撮像装置により撮像されたシーンの複数の撮像画像と、該撮像画像を撮像したときの前記第１の取得処理によって取得した位置及び姿勢と、該撮像画像に基づく距離画像と、が登録されており、
前記補正手段は、前記三次元マップに登録されている撮像画像の間で特徴点の対応付けを行い、該対応付けの結果と、前記第２の取得手段が取得した相対的な位置及び／又は姿勢と、に基づいて、前記三次元マップに登録されている位置及び姿勢と、前記距離画像と、を補正することを特徴とする請求項１乃至７の何れか１項に記載の情報処理装置。
【請求項１０】
更に、
前記補正手段により補正した三次元マップに基づく仮想空間の画像上に、前記第１の取得手段が取得した位置及び姿勢を示すオブジェクトと、オブジェクト間を結ぶ線分と、を表示する表示手段を備え、
前記表示手段は、前記線分を、該線分に対応するオブジェクト間の相対的な位置及び姿勢を取得した方法に応じた表示形態で表示することを特徴とする請求項１乃至９の何れか１項に記載の情報処理装置。
【請求項１１】
情報処理装置が行う情報処理方法であって、
前記情報処理装置の第１の取得手段が、撮像装置によるシーンの撮像画像と該シーンの三次元マップとに基づいて該撮像装置の位置及び姿勢を取得する第１の取得処理とは異なる第２の取得処理により前記撮像装置の位置及び姿勢を取得する第１の取得工程と、
前記情報処理装置の第２の取得手段が、前記第１の取得工程で取得した位置及び姿勢と、該位置及び姿勢よりも過去に前記第１の取得工程で取得した位置及び姿勢と、の間の相対的な位置及び姿勢を取得する第２の取得工程と、
前記情報処理装置の補正手段が、前記第２の取得工程で取得した相対的な位置及び姿勢に基づいて、前記三次元マップを補正する補正工程と
を備えることを特徴とする情報処理方法。
【請求項１２】
コンピュータを、請求項１乃至１０の何れか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像装置の位置及び姿勢の取得技術に関するものである。
【背景技術】
【０００２】
画像を用いてシーンの三次元マップ作成とカメラの位置及び姿勢計測とを同時に行う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軸とする三次元の座標系をカメラ座標系と呼び、第１のカメラの位置及び姿勢を撮像部１７０の位置及び姿勢と呼ぶ。
【００１４】
また、第１のカメラ及び第２のカメラのそれぞれの内部パラメータ（焦点距離ｆｘ（画像の水平方向）、ｆｙ（画像の垂直方向）、画像中心位置ｃｘ（画像の水平方向）、ｃｙ（画像の垂直方向）、レンズ歪みパラメータ）は既知であるとする。カメラの内部パラメータは、例えばＺｈａｎｇの方法（Ｚ．Ｚｈａｎｇ，“Ａ  ｆｌｅｘｉｂｌｅ  ｎｅｗ  ｔｅｃｈｎｉｑｕｅ  ｆｏｒ  ｃａｍｅｒａ  ｃａｌｉｂｒａｔｉｏｎ，”  ＩＥＥＥ  Ｔｒａｎｓ．  ｏｎ  Ｐａｔｔｅｒｎ  Ａｎａｌｙｓｉｓ  ａｎｄ  Ｍａｃｈｉｎｅ  Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．１１，ｐｐ．１３３０−１３３４，２０００．）により事前に校正しておく。また、撮像部１７０を構成する二台のカメラ間（第１のカメラと第２のカメラとの間）の相対的な位置及び姿勢（外部パラメータ）も事前に校正しておく。ステレオカメラの外部パラメータは、例えば三次元形状が既知のパターンを同時に左右のカメラで撮像してパターンを基準とした各カメラの位置及び姿勢を求め、夫々の位置及び姿勢をステレオカメラの左側カメラ（本実施形態の場合は第１のカメラ）を基準とした位置及び姿勢に変換して求める。撮像部１７０の内部パラメータと外部パラメータは情報処理装置１内の不図示のメモリ若しくは情報処理装置１がアクセス可能な外部メモリに保持されるものとする。
【００１５】
保持部１１０は、図３に示す如く現実空間（シーン）中に人為的に配置したそれぞれの補助特徴の配置情報（世界座標系における補助特徴の位置及び姿勢と該補助特徴の識別子）を保持している。本実施形態では、補助特徴として、図３に示すように内部に個別の識別子を持つ白黒の正方形の特徴を利用する。正方形中の所定の位置の白黒パターンを参照することにより、正方形の識別子を特定することができる。補助特徴の世界座標系における位置及び姿勢は、例えば特許第０４５３２９８２号（または米国特許第７５２９３８７号）に開示される方法により事前に校正しておく。
【００１６】
保持部１１０は更に、第１の導出部１３０で利用するシーンの三次元マップを保持する。本実施形態では、非特許文献１と同様、三次元マップをキーフレームの集合として保持する。
【００１７】
各キーフレームは、撮像部１７０が撮像した撮像画像（濃淡画像）Ｉ、該撮像画像と同じ画像サイズのデプスマップＤ、分散マップＶを保持する。デプスマップＤにおける各画素は、撮像部１７０から該画素に対応する対応点までの距離（奥行き値）の逆数（ｉｎｖｅｒｓｅ  ｄｅｐｔｈ）、すなわちカメラ座標系における該対応点のＺ座標の逆数を保持する。本実施形態では非特許文献１と同様、濃淡画像Ｉ上の画素のうち輝度勾配が閾値よりも大きい画素を特徴点とし、デプスマップＤにおいて該特徴点に対応する位置の画素に、該特徴点に対応する奥行き値の逆数を登録するものとする。分散マップＶの各画素は、ｉｎｖｅｒｓｅ  ｄｅｐｔｈの分散を保持する。
【００１８】
さらに各キーフレームは、該キーフレームが保持する撮像画像の撮像時における撮像部１７０の世界座標系における位置及び姿勢を属性として保持する。以下では、キーフレームが保持する属性（世界座標系における撮像部１７０の位置及び姿勢）を「キーフレームの位置及び姿勢」と称する場合がある。
【００１９】
さらに各キーフレームは、該キーフレームが保持する撮像画像中の補助特徴に基づいて推定される撮像部１７０の世界座標系における位置及び姿勢が「参照位置及び参照姿勢」として保持可能である。
【００２０】
さらに各キーフレームは、マップ最適化で利用するための「キーフレーム対応情報」を保持可能である。キーフレーム対応情報は、キーフレームのペアを構成する２個のキーフレームのそれぞれの識別番号と、キーフレーム間の相対的な位置及び姿勢を保持する。キーフレーム対応情報は、マップ最適化において利用される。
【００２１】
更新部１４０は、撮像画像と、第１の導出部１３０によって導出された撮像部１７０の位置及び姿勢と、に基づいて、保持部１１０に保持されている三次元マップを更新する。
【００２２】
補正部１５０は、第２の導出部１６０が導出するキーフレーム間の相対的な位置及び姿勢を利用して、保持部１１０に保持されている三次元マップを補正する。このような補正により、三次元マップの蓄積誤差を低減し幾何的な整合がとれたマップにする。
【００２３】
第２の導出部１６０は、画像入力部１２０から入力された撮像画像から検出された補助特徴に基づいて撮像部１７０の位置及び姿勢を参照位置及び参照姿勢として導出する処理や、キーフレーム間の相対的な位置及び姿勢を導出する処理を行う。導出された相対的な位置及び姿勢は、補正部１５０で利用される。
【００２４】
制御部１９９は、情報処理装置１全体の動作制御を行う。例えば制御部１９９は、上記の画像入力部１２０、第１の導出部１３０、第２の導出部１６０、更新部１４０、補正部１５０の各機能部の動作制御を行う。次に、情報処理装置１の動作について、図４のフローチャートに従って説明する。
【００２５】
ステップＳ１０１０では、保持部１１０が保持する三次元マップの初期化処理が行われる。三次元マップの初期化処理では、更新部１４０は、画像入力部１２０が入力したステレオ画像（第１のカメラによる撮像画像及び第２のカメラによる撮像画像のセット）のうち第１のカメラによる撮像画像を基準画像とする。以下では、第１のカメラによる撮像画像を第１の撮像画像、第２のカメラによる撮像画像を第２の撮像画像と称する場合がある。そして更新部１４０は、基準画像上で輝度勾配が閾値以上の画素を特徴点とし、該基準画像と同セットの第２の撮像画像において該特徴点に対応する対応特徴点をステレオマッチングにより探索する。そして更新部１４０は、特徴点と対応特徴点とを用いた三角測量によりカメラ座標系における特徴点のｉｎｖｅｒｓｅ  ｄｅｐｔｈを算出する。そして更新部１４０は、該算出した特徴点のｉｎｖｅｒｓｅ  ｄｅｐｔｈを、デプスマップＤにおいて該特徴点に対応する画素に登録する。さらに更新部１４０は、デプスマップＤ上の各画素について、左右画像（第１の撮像画像及び第２の撮像画像）の視差及び撮像部１７０の基線長、画像ノイズの分散を用いて算出したｉｎｖｅｒｓｅ  ｄｅｐｔｈの分散を推定し、分散マップＶに保存する。そして更新部１４０は、基準画像を濃淡画像Ｉとし、該濃淡画像Ｉ、デプスマップＤ、分散マップＶを含むキーフレームを生成して、三次元マップに追加する。なお、この時点で三次元マップに、濃淡画像Ｉの撮像時における撮像部１７０の世界座標系における位置及び姿勢を登録しても良い。濃淡画像Ｉの撮像時における撮像部１７０の世界座標系における位置及び姿勢はどのような方法で求めても良い。
【００２６】
ステップＳ１０２０では、画像入力部１２０は、撮像部１７０から入力されたステレオ画像を取得する。ステップＳ１０３０では、第１の導出部１３０は、ステップＳ１０２０で画像入力部１２０が入力したステレオ画像と、保持部１１０に保持されている三次元マップと、を用いて、撮像部１７０の世界座標系における位置及び姿勢を導出する。ステレオ画像と三次元マップとを用いて撮像部１７０の位置及び姿勢を導出する方法は周知であり、例えば、非特許文献１に開示される方法を利用することができる。
【００２７】
ステップＳ１０４０では、更新部１４０は、ステップＳ１０３０で導出した撮像部１７０の位置及び姿勢に基づいて、保持部１１０に保持されているキーフレームの更新、若しくは保持部１１０への新規のキーフレームの追加、を行う。ステップＳ１０４０における処理の詳細については後述する。
【００２８】
ステップＳ１０５０における処理は、ステップＳ１０４０において新規のキーフレームを保持部１１０に追加した場合に行われる。ステップＳ１０５０では、第２の導出部１６０は、三次元マップの補正で利用するためのキーフレーム間の相対的な位置及び姿勢を導出する。ステップＳ１０５０における処理の詳細については後述する。
【００２９】
ステップＳ１０６０では、補正部１５０は、ステップＳ１０５０で得られたキーフレーム間の相対的な位置及び姿勢を利用して三次元マップを補正する。キーフレームが保持するデプスマップＤの補正まで行うと計算時間がかかるため、本実施形態ではキーフレームの世界座標系における位置及び姿勢のみを補正する。世界座標系におけるキーフレームの位置及び姿勢の補正は、世界座標系におけるキーフレームの位置及び姿勢から算出される相対的な位置及び姿勢とステップＳ１０５０で得られたキーフレーム間の相対的な位置及び姿勢の誤差の総和を評価関数として行う。ここで、キーフレームｉとキーフレームｊの世界座標系における位置及び姿勢を表す４×４行列をそれぞれＭ（ｉ）、Ｍ（ｊ）、キーフレームｉとキーフレームｊの相対的な位置及び姿勢を表す４×４行列をＭｒｅｌ（ｉ，ｊ）とする。行列Ｍ（ｉ）、Ｍ（ｊ）、Ｍｒｅｌ（ｉ，ｊ）は、次式に示すように、左上の３×３行列が回転行列Ｒ、一番右の列が平行移動成分ｔを表す行列である。
【００３０】
【数１】
2019020778.tif 000003
【００３１】
行列Ｍ（ｉ）、Ｍ（ｊ）は、世界座標系における位置及び姿勢をカメラ座標系における位置及び姿勢に変換する行列であるとする。また行列Ｍｒｅｌ（ｉ，ｊ）は、キーフレームｊのカメラ座標系における位置及び姿勢をキーフレームｉのカメラ座標系における三次元位置に変換する行列であるとする。ここでは、行列Ｍｒｅｌ（ｉ，ｊ）を既知として、下記の式（１）が最小となるようなＭ（ｉ）、Ｍ（ｊ）を求める。
【００３２】
【数２】
2019020778.tif 000004
【００３３】
なお、｜｜行列｜｜Ｆはフロベニウスのノルム（行列の各要素の二乗和）を表す。また、（式１）は三次元マップに保存されたすべての「キーフレーム対応情報」についての総和である。Ｍ（ｉ）、Ｍ（ｊ）は非線形の変換であるため、例えば反復計算を行うＧａｕｓｓ−Ｎｅｗｔｏｎ法により算出する。Ｍ（ｉ）、Ｍ（ｊ）の初期値としては、ステップＳ１０３０で導出された位置及び姿勢を用いるものとする。最適化されたＭ（ｉ）、Ｍ（ｊ）は再び三次元マップ中のキーフレームの属性として保存し、次にマップ最適化を行う際の初期値として利用する。
【００３４】
ステップＳ１０７０では、制御部１９９は、図４のフローチャートに従った処理の終了条件が満たされた否かを判断する。終了条件には様々なものが考えられる。例えば、ユーザが不図示の操作部を操作することで「図４のフローチャートに従った処理の終了指示」を入力したことを制御部１９９が検知した場合には、制御部１９９は終了条件が満たされたと判断する。終了条件が満たされた場合には、図４のフローチャートに従った処理は完了する。一方、終了条件が満たされていない場合には、処理はステップＳ１０７０を介してステップＳ１０２０に戻る。
【００３５】
次に、上記のステップＳ１０４０における処理の詳細について説明する。ステップＳ１０４０では、更新部１４０は、ステップＳ１０３０で導出した撮像部１７０の位置及び姿勢に基づいて、保持部１１０に保持されているキーフレームの更新、若しくは保持部１１０への新規のキーフレームの追加、を行う。
【００３６】
例えば更新部１４０は、保持部１１０に保持されているキーフレームのうち、ステップＳ１０３０で導出した撮像部１７０の位置及び姿勢に最も近い位置及び姿勢を属性として有するキーフレームを最近傍キーフレームとして特定する。そして更新部１４０は、最近傍キーフレームに含まれている濃淡画像Ｉにおける特徴点を求めると共に、ステップＳ１０２０で画像入力部１２０が入力した第１の撮像画像において該特徴点に対応する対応特徴点を求める。そして更新部１４０は、最近傍キーフレームに含まれている濃淡画像Ｉにおける特徴点の画素数に対する、ステップＳ１０２０で画像入力部１２０が入力した第１の撮像画像における対応特徴点の画素数の割合Ｒｉｎを求める。そして更新部１４０は、Ｒｉｎが閾値Ｔｉｎ未満の場合、最近傍キーフレームとのオーバーラップが少ないためキーフレームを追加すると判定する。一方、更新部１４０は、Ｒｉｎが閾値Ｔｉｎ以上の場合、最近傍キーフレームとのオーバーラップが多いためキーフレームを追加しない（保持部１１０に保持されているキーフレームを更新する）と判定する。
【００３７】
キーフレームを追加すると判定した場合、更新部１４０は、以下のような処理を行うことで、新規のキーフレームを三次元マップに追加する。先ず更新部１４０は、ステップＳ１０２０で画像入力部１２０が入力したステレオ画像を用いてステップＳ１０１０で説明した処理を行うことで、カメラ座標系における特徴点のｉｎｖｅｒｓｅ  ｄｅｐｔｈを算出する。そして更新部１４０は、該算出したｉｎｖｅｒｓｅ  ｄｅｐｔｈにより既存のキーフレーム（三次元マップに登録済み（過去に登録した）のキーフレーム）から伝播されたｉｎｖｅｒｓｅ  ｄｅｐｔｈを更新することで新たなデプスマップＤを生成する。また、更新部１４０は、既存のキーフレームが保持していたｉｎｖｅｒｓｅ  ｄｅｐｔｈの分散及び新たに算出されたｉｎｖｅｒｓｅ  ｄｅｐｔｈの分散を用いて分散マップＶを生成する。そして更新部１４０は、ステップＳ１０２０で画像入力部１２０が入力した第１の撮像画像を濃淡画像Ｉとし、該濃淡画像Ｉ、ステップＳ１０４０で生成したデプスマップＤ及び分散マップＶ、を含み、且つステップＳ１０３０で導出した撮像部１７０の位置及び姿勢を属性として更に含むキーフレームを生成して三次元マップに追加する。
【００３８】
一方、キーフレームを追加しないと判定した場合、更新部１４０は、以下のような処理を行うことで、保持部１１０に保持されているキーフレームを更新する。より具体的には、更新部１４０は、ステップＳ１０３０で第１の導出部１３０が導出した位置及び姿勢、ステップＳ１０２０で入力されたステレオ画像を用いて既存のキーフレームのデプスマップＤ、分散マップＶを更新する。非特許文献１に開示される方法と同様に、デプスマップＤ、分散マップＶの更新は、ステレオ画像から推定されるｉｎｖｅｒｓｅ  ｄｅｐｔｈと、キーフレームが保持する濃淡画像とのモーションステレオから推定されるｉｎｖｅｒｓｅ  ｄｅｐｔｈを用いて行う。
【００３９】
次に、上記のステップＳ１０５０における処理の詳細について、図５のフローチャートに従って説明する。ステップＳ１１１０では、第２の導出部１６０は、第１の撮像画像及び第２の撮像画像のそれぞれについて、該撮像画像から補助特徴を検出する。図３に示すような補助特徴の検出は例えば次のようにして行う。まず、撮像画像を二値化して二値化画像を生成し、該二値化画像から四角形領域を検出し、該四角形領域の各頂点の画像座標をもとに正方形から画像座標への射影変換を算出し、四角形領域を正方形に変換する。そして正方形の所定の位置に相当する輝度値を画像から読み出すことにより補助特徴の識別子を特定する。このとき、該特定した識別子と保持部１１０に保持されている補助特徴の識別子との照合を行い、互いに識別子が一致する場合には各頂点の画像座標と世界座標系における三次元座標との対応を保存する。補助特徴の頂点の世界座標系における三次元座標は、補助特徴に規定された座標系における頂点の三次元座標と、補助特徴の世界座標系における位置及び姿勢より算出する。
【００４０】
ステップＳ１１２０では、第２の導出部１６０は、補助特徴から得られる情報が十分であるか否かを判断する。ここでは、ステップＳ１１１０で対応が保存された補助特徴の頂点の画像上の分布を利用して補助特徴の情報が十分にあるかの判定を行う。具体的には、第１の撮像画像及び第２の撮像画像のそれぞれについて、補助特徴の頂点の画像座標の共分散行列を算出する。第１の撮像画像及び第２の撮像画像のそれぞれから算出した共分散行列の最小固有値Ｅｉｇ（Ｌ）、Ｅｉｇ（Ｒ）の何れかが閾値Ｔ＿Ｅｉｇより小さいという条件が満たされた場合には、補助特徴の情報が不十分であると判断し、処理はステップＳ１１３０に進む。一方、この条件が満たされていない場合には、ステップＳ１１５０に進む。
【００４１】
ステップＳ１１３０では第２の導出部１６０は、ステップＳ１０４０で新たに追加されたキーフレーム（追加キーフレーム）の位置及び姿勢（属性）に近い位置及び姿勢を属性として有するキーフレームを保持部１１０に保持されている既存キーフレーム群から特定する。具体的には追加キーフレームに対し、世界座標系における視軸方向（カメラ座標系のＺ軸）の角度差が閾値ＴＡｎｇｌｅ以内（閾値以内）である既存キーフレームのうち世界座標系における位置の差が閾値ＴＤｉｓｔ以内である全ての既存キーフレームを特定する。そして第２の導出部１６０は、該特定したそれぞれの既存キーフレームと追加キーフレームとの相対的な位置及び姿勢を算出し（第２の取得処理）、該算出した相対的な位置及び姿勢と該それぞれの既存キーフレームの識別番号と追加キーフレームの識別番号とを追加キーフレームのキーフレーム対応情報として保持部１１０に登録する。
【００４２】
ステップＳ１１４０では、第２の導出部１６０は、追加キーフレームに含まれている濃淡画像Ｉとの類似度が高い濃淡画像Ｉを含む既存キーフレームを保持部１１０から特定し、該特定した既存キーフレームとの相対的な位置及び姿勢を導出する。具体的には、第２の導出部１６０は、追加キーフレーム及び既存キーフレームのそれぞれに含まれている濃淡画像Ｉの縮小画像Ｉ’を生成する。そして第２の導出部１６０は、追加キーフレームと既存キーフレームとの間で縮小画像Ｉ’の輝度値のＳＳＤ（Ｓｕｍ  ｏｆ  Ｓｑｕａｒｅｄ  Ｄｉｓｔａｎｃｅ）が最小かつ閾値ＴＳＳＤ以下となる既存キーフレームを特定する。そして第２の導出部１６０は、該特定したそれぞれの既存キーフレームと追加キーフレームとの相対的な位置及び姿勢を同様に算出する（第２の取得処理）。そして第２の導出部１６０は、該算出した相対的な位置及び姿勢と、該それぞれの既存キーフレームの識別番号と、追加キーフレームの識別番号と、を追加キーフレームのキーフレーム対応情報として保持部１１０に登録する。
【００４３】
一方、ステップＳ１１５０で第２の導出部１６０は、ステップＳ１１１０で得られた補助特徴の頂点の画像座標と世界座標系における三次元座標の対応に基づいて、追加キーフレームの参照位置及び参照姿勢を算出し、追加キーフレームに登録する。登録位置及び登録姿勢の算出は、例えばＬｅｐｅｔｉｔらの手法（Ｖ．  Ｌｅｐｅｔｉｔ，  Ｆ．  Ｍｏｒｅｎｏ−Ｎｏｇｕｅｒ，  ａｎｄ  Ｐ．  Ｆｕａ，  “ＥＰｎＰ：  ａｎ  ａｃｃｕｒａｔｅ  Ｏ（ｎ）  ｓｏｌｕｔｉｏｎ  ｔｏ  ｔｈｅ  ＰｎＰ  ｐｒｏｂｌｅｍ，”  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｊｏｕｒｎａｌ  ｏｆ  Ｃｏｍｐｕｔｅｒ  Ｖｉｓｉｏｎ，  ｖｏｌ．８１，ｎｏ．２，ｐｐ．１５５−１６６，２００９．）により行う。
【００４４】
ステップＳ１１６０では、第２の導出部１６０は、追加キーフレームと、参照位置及び参照姿勢を保持する既存キーフレームと、の相対的な位置及び姿勢を算出する（第２の取得処理）。追加キーフレームの参照位置及び参照姿勢を表す４×４行列をＭｎｅｗ、既存キーフレームの参照位置及び参照姿勢を表す４×４行列をＭｏｌｄとする（行列Ｍｎｅｗ、Ｍｏｌｄは、世界座標系における位置及び姿勢をカメラ座標系における位置及び姿勢に変換する行列であるとする）。このとき、キーフレーム間の相対的な位置及び姿勢Ｍｒｅｌ（ｎｅｗ、ｏｌｄ）は以下の（式２）により算出する。
【００４５】
【数３】
2019020778.tif 000005
【００４６】
参照位置及び参照姿勢を保持する既存キーフレームが複数存在する場合には、追加キーフレームの前側にありかつ一定距離以上離れた既存キーフレームのうちキーフレームの視軸方向（カメラ座標系のＺ軸）の間の角度が最も大きい既存キーフレームを選択する。これは、なるべく異なる視点を持つキーフレームをペアにすることで、三次元マップの補正によってより多くの累積誤差が削減されることを目的としている。選択されたペアの相対的な位置及び姿勢と、該選択されたペアにおけるそれぞれのキーフレームの識別番号と、は追加キーフレームにおけるキーフレーム対応情報として保持部１１０に登録する。
【００４７】
このように、本実施形態によれば、シーン中に配置した補助特徴を利用して導出されるキーフレーム間の相対的な位置及び姿勢を三次元マップの補正に利用することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。
【００４８】
＜変形例＞
第１の実施形態では、撮像部１７０はスレテオカメラであるものとして説明したが、シーンに配置した補助特徴を検出可能な画像を撮像することができる装置であれば、撮像部１７０としてスレテオカメラ以外の装置を用いても良い。例えば、撮像部１７０は単眼カメラであってもよいし、リジッドに固定されてカメラ間の相対的な位置及び姿勢が既知な三台以上のカメラであってもよい。また、撮像部１７０は、カラー画像と距離画像を撮像するＲＧＢＤカメラであってもよい。
【００４９】
また、補助特徴の形状は四角形に限るものではなく、画像から安定的に検出可能な特徴であれば如何なる特徴であっても良い。例えば、三角形、六角形、円形、楕円形の補助特徴を使用しても良い。また、補助特徴の識別子は画像から特定できなくてもよい。例えば、円形の補助特徴を用いる場合、第１の導出部１３０が導出する撮像部１７０の位置及び姿勢をもとに補助特徴の画像上の位置を算出し、近傍で検出された補助特徴と対応付けることで特定してもよい。また識別子つきの補助特徴と識別子のない補助特徴を併用してもよい。この場合、識別子つきの補助特徴から算出される撮像部１７０の位置及び姿勢をもとに識別子のない補助特徴の画像上の位置を算出し、近傍で検出された補助特徴と対応付けてもよい。
【００５０】
また、補助特徴の配置情報の校正は必ずしも事前に行わなくてもよい。例えば、撮像部１７０によって撮像される画像を用いて、シーンの三次元マップ作成と並行して行ってもよい。その際には、補助特徴を用いた三次元マップの補正は随時行わずに、補助特徴の配置情報の信頼度が十分高くなった場合（例えば信頼度が閾値以上の場合）に行うようにすることで三次元マップの精度が低下しないようにする。補助特徴の配置情報の信頼度の判定は、例えば校正に用いた画像群において各補助特徴が検出された回数の平均値や最小値に基づいて行い、検出回数の平均値や最小値が大きいほど信頼度が高いとする。
【００５１】
また、補助特徴の情報が十分にあるかどうかの判断基準は、補助特徴の画像上の分布に限るものではない。例えば、画像上で検出される補助特徴の個数が閾値以上の場合に、補助特徴の情報が十分にあると判断しても良い。また、補助特徴の個数と画像上の分布を組み合わせたものに基づいていてもよい。
【００５２】
また、参照位置及び参照姿勢を保持するキーフレームの中から相対的な位置及び姿勢を算出するために使用するキーフレームを特定する際には、キーフレームの間で共通の補助特徴が観察されているキーフレームのみを対象とするようにしてもよい。
【００５３】
［第２の実施形態］
本実施形態を含め、以下に説明する実施形態や変形例では第１の実施形態との差分について説明し、以下で特に触れない限りは第１の実施形態と同様であるものとする。本実施形態では、撮像部１７０に位置及び姿勢を計測するセンサを装着し、このセンサの計測値を三次元マップの補正に利用する場合について説明する。
【００５４】
本実施形態に係るシステムの機能構成例について、図６のブロック図を用いて説明する。図６において図２に示した機能部と同じ機能部には同じ参照番号を付しており、該機能部に係る説明は省略する。図６に示す如く、本実施形態に係るシステムは、撮像部１７０、センサ２９０、情報処理装置２、を有する。
【００５５】
先ず、センサ２９０について説明する。センサ２９０は、撮像部１７０の世界座標系における位置及び姿勢を計測し、その計測値を情報処理装置２に対して出力する。本実施形態では、センサ２９０はＢａｕｅｒらの文献（Ｍ．  Ｂａｕｅｒ，  Ｍ．  Ｓｃｈｌｅｇｅｌ，  Ｄ．  Ｐｕｓｔｋａ，  Ｎ．  Ｎａｖａｂ，  ａｎｄ  Ｇ．  Ｋｌｉｎｋｅｒ，  “Ｐｒｅｄｉｃｔｉｎｇ  ａｎｄ  ｅｓｔｉｍａｔｉｎｇ  ｔｈｅ  ａｃｃｕｒａｃｙ  ｏｆ  ｎ−ｏｃｃｕｌａｒ  ｏｐｔｉｃａｌ  ｔｒａｃｋｉｎｇ  ｓｙｓｔｅｍｓ，”  Ｐｒｏｃ．  ５ｔｈ  ＩＥＥＥ  ａｎｄ  ＡＣＭ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｓｙｍｐｏｓｉｕｍ  ｏｎ  Ｍｉｘｅｄ  ａｎｄ  Ａｕｇｍｅｎｔｅｄ  Ｒｅａｌｉｔｙ，  ｐｐ．４３−５１，  ２００６．）で開示されている光学式の位置姿勢センサであるとする。この光学式の位置姿勢センサは、計測対象（本実施形態では撮像部１７０）上に複数のマーカを配置し、配置されたマーカをシーン中に固定された複数のカメラによって撮影した画像を利用して計測対象の世界座標系における位置及び姿勢を計測するものである。
【００５６】
次に、情報処理装置２について説明する。センサ入力部２８０は、センサ２９０から出力された計測値を入力し、該計測値を後段の第２の導出部２６０に送出する。第２の導出部２６０は、計測値に基づいて撮像部１７０の位置及び姿勢を参照位置及び参照姿勢として導出する処理や、キーフレーム間の相対的な位置及び姿勢を導出する処理を行う。導出された相対的な位置及び姿勢は、補正部１５０で利用される。
【００５７】
次に、情報処理装置２の動作について、図７のフローチャートに従って説明する。図７において図４に示した処理ステップと同じ処理ステップには同じステップ番号を付しており、該処理ステップに係る説明は省略する。
【００５８】
ステップＳ２０２０では、画像入力部１２０は、撮像部１７０から入力されたステレオ画像を取得し、センサ入力部２８０は、センサ２９０から入力された計測値（世界座標系における撮像部１７０の位置及び姿勢）を取得する。
【００５９】
ステップＳ２０４０では、更新部１４０は上記のステップＳ１０４０と同様にして新規のキーフレームを三次元マップに追加する。ここで、追加するキーフレームの属性、参照位置及び参照姿勢としては、ステップＳ２０２０で取得した撮像部１７０の位置及び姿勢を適用する。
【００６０】
ステップＳ２０５０では、第２の導出部２６０は、追加キーフレームにおける参照位置及び参照姿勢として、ステップＳ２０２０で取得した計測値が示す位置及び姿勢を用いる。そして第２の導出部２６０は上記のステップＳ１１６０と同様にして、追加キーフレームと、参照位置及び参照姿勢を保持する既存キーフレームと、の相対的な位置及び姿勢を算出する。そして第２の導出部２６０は、参照位置及び参照姿勢を保持する既存キーフレームのうち追加キーフレームとの距離が最も大きい既存キーフレームを選択し、該選択した既存キーフレームとの相対的な位置及び姿勢と該選択した既存キーフレームの識別番号と追加キーフレームの識別番号とをキーフレーム対応情報として追加キーフレームに登録する。
【００６１】
このように、本実施形態によれば、位置及び姿勢を計測するセンサの計測値をもとにキーフレーム間の相対的な位置及び姿勢を導出することで、カメラの移動範囲が限定される場合であっても三次元マップに蓄積する誤差を低減できる。
【００６２】
＜変形例＞
センサ２９０は光学式の位置姿勢センサに限るものではなく、他の種類の位置姿勢センサであってもよい。例えば、磁気式の位置姿勢センサであってもよい。また、位置及び姿勢の双方を計測する必要はなく、位置または姿勢のいずれかを計測するセンサであってもよい。すなわち、ジャイロスコープや加速度センサ、磁気センサを利用して撮像部１７０の姿勢を計測する姿勢センサであってもよいし、ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）のような位置を計測する位置センサであってもよい。位置または姿勢のいずれかを計測するセンサを利用する場合には、相対的な位置又は姿勢を取得することになり、該相対的な位置又は姿勢に基づいて三次元マップを補正する（センサが計測する成分のみを補正する）ことになる。また、位置を計測するセンサと姿勢を計測するセンサを組み合わせて位置姿勢センサとして利用してもよい。
【００６３】
また、シーンの三次元マップの作成に利用可能な画像を撮像することができる装置であれば、撮像部１７０としてスレテオカメラ以外の装置を用いても良い。例えば、撮像部１７０は単眼カメラであってもよいし、リジッドに固定されてカメラ間の相対的な位置及び姿勢が既知な三台以上のカメラであってもよい。また、撮像部１７０は、距離画像を撮像するデプスカメラであっても良いし、カラー画像と距離画像を撮像するＲＧＢＤカメラであってもよい。
【００６４】
［第３の実施形態］
本実施形態では、非特許文献１のようにマップ最適化において各キーフレームの位置及び姿勢のみを補正するのではなく、特徴点の三次元座標も補正する。本実施形態に係るシステムの機能構成例について、図８のブロック図を用いて説明する。図８において図２に示した機能部と同じ機能部には同じ参照番号を付しており、該機能部に係る説明は省略する。図８に示す如く、本実施形態に係るシステムは、撮像部１７０と、情報処理装置３と、を有する。
【００６５】
補正部３５０は、第２の導出部１６０が導出するキーフレーム間の相対的な位置及び姿勢を利用して三次元マップを補正する。第１の実施形態における補正部１５０では、計算時間を削減するためにキーフレームの位置及び姿勢のみ補正しており、三次元マップの精度は低かった。一方、本実施形態では、計算時間をかけてキーフレームの位置及び姿勢とともに各キーフレームが持つデプスマップも補正することで、三次元マップの精度を向上させる。
【００６６】
次に、情報処理装置３の動作について、図４のフローチャートに従って説明する。情報処理装置３は、図４のフローチャートに従った処理においてステップＳ１０６０の処理を図９のフローチャートに従った処理に置き換えた処理を実行する。
【００６７】
本実施形態に係るステップＳ１０６０では、補正部３５０は、ステップＳ１０５０で得られたキーフレーム間の相対的な位置及び姿勢を制約条件として三次元マップを補正する。三次元マップの補正では、（１）各キーフレームにおける特徴点の再投影誤差の総和、（２）キーフレームの相対的な位置及び姿勢の誤差の総和（式１）、の和を評価関数として各キーフレームの位置及び姿勢と各特徴点の三次元座標を補正する。
【００６８】
本実施形態に係るステップＳ１０６０では先ず、各キーフレームがデプスマップに保持する特徴点と他のキーフレームがデプスマップに保持する特徴点との対応付けを行う。次に、特徴点の対応付けの結果と、キーフレーム間の相対的な位置及び姿勢と、を用いて三次元マップを補正する。ここで、三次元マップが保持するキーフレームの総数をＮｋとする（つまり、キーフレームの識別番号は１〜Ｎｋ）。
【００６９】
ステップＳ３１１０では、補正部３５０は、変数ｎの値を１に初期化する。ステップＳ３１２０では、補正部３５０は、ｎ＞Ｎｋであるか否かを判断する。この判断の結果、ｎ＞Ｎｋであれば、全てのキーフレームにおける特徴点の対応付けが完了したものとして処理はステップＳ３１６０に進み、ｎ≦Ｎｋの場合には、処理はステップＳ３１３０に進む。
【００７０】
ステップＳ３１３０で補正部３５０はステップＳ１１３０と同様の処理を行うことで、キーフレームｎ（識別番号がｎのキーフレーム）の位置及び姿勢に近い位置及び姿勢を有するキーフレームを保持部１１０に保持されている既存キーフレーム群から特定する。更に補正部３５０は、上記のステップＳ１１４０と同様の処理を行うことで、キーフレームｎに含まれている濃淡画像Ｉとの類似度が高い濃淡画像Ｉを含む既存キーフレームを保持部１１０から特定する。
【００７１】
ステップＳ３１４０では、補正部３５０は、キーフレームｎにおける特徴点と、ステップＳ３１３０で特定したキーフレーム（特定キーフレーム）の特徴点と、の対応付けを行う。この対応付けの結果は、ステップＳ３１６０におけるパラメータ更新において、特徴点の三次元座標をキーフレーム毎に更新するのではなく、キーフレーム間で共通のパラメータとして更新するのに用いる。特徴点の対応付けは次のようにして行う。まず、各キーフレームの世界座標系における位置及び姿勢、デプスマップから算出される特徴点の三次元座標、撮像部１７０の内部パラメータ、を用いて、特定キーフレームにおいて、キーフレームｎにおける特徴点に対応する位置（ｕｎ、ｖｎ）を算出する。次に、特定キーフレームにおける特徴点のうち、位置（ｕｎ、ｖｎ）との距離が閾値Ｔｎｅｉｇｈｂｏｕｒ以下となる位置にある特徴点を選択する。そして、キーフレームｎにおける特徴点に対し、選択された特徴点のうち、特徴点近傍のＮ×Ｎ画素の輝度値の二乗誤差の総和が閾値Ｔｄｉｆｆ以内となる特徴点が存在する場合には、そのうち最も二乗誤差の総和の小さい特徴点がキーフレームｎ上の特徴点と対応付くものとし、それぞれに同一の識別番号を割り振る。ステップＳ３１５０では、補正部３５０は、変数ｎの値を１つインクリメントする。そして処理はステップＳ３１２０に戻る。
【００７２】
一方、ステップＳ３１６０では、補正部３５０は、未知パラメータである「世界座標系における各キーフレームの位置及び姿勢」及び「各特徴点の三次元座標」を更新する。前述したように、（１）三次元マップを構成する各キーフレームにおける特徴点の再投影誤差の総和、（２）キーフレームの相対的な位置及び姿勢の誤差の総和、の和が最小となるように未知パラメータを推定する。ここで、再投影誤差とは、キーフレームの位置及び姿勢、特徴点の三次元座標、撮像部１７０の内部パラメータ、をもとに算出される画像上の位置（ｕｃａｌｃ，ｖｃａｌｃ）と実際の特徴点の画像上の検出位置（ｕｏｂｓ，ｖｏｂｓ）の差のことを言う。ここで再投影誤差の総和Ｅｒｅｐｒｏｊを以下の（式３）のように表す。
【００７３】
【数４】
2019020778.tif 000006
【００７４】
（ｕｃａｌｃ（ｉ，ｊ），ｖｃａｌｃ（ｉ，ｊ））、はｉ番目のキーフレームにおけるｊ番目の特徴点の画像上の算出位置であるとする。（ｕｏｂｓ（ｉ，ｊ），ｖｏｂｓ（ｉ，ｊ））、はｉ番目のキーフレームにおけるｊ番目の特徴点の画像上の検出位置であるとする。またキーフレームの相対的な位置及び姿勢の誤差の総和Ｅｒｅｌａｔｉｖｅを以下の（式４）のように表す。
【００７５】
【数５】
2019020778.tif 000007
【００７６】
ここでＭ（ｓ）、Ｍ（ｔ）はそれぞれキーフレームｓとキーフレームｔの世界座標系における位置及び姿勢を表す４×４剛体変換行列である。Ｍｒｅｌ（ｓ，ｔ）は、キーフレームｓとキーフレームｔの相対的な位置及び姿勢を表す４×４行列であり、ステップＳ１０５０から得られる。ステップＳ３１６０では、以下の（式５）に示す（式３）と（式４）の重み付け和Ｅｔｏｔａｌが最小となるように未知パラメータを推定する。
【００７７】
【数６】
2019020778.tif 000008
【００７８】
（式５）を最小化する未知パラメータの推定はバンドル調整により行う。但し、一般にバンドル調整は再投影誤差を最小化する方法である。そのため、再投影誤差とキーフレーム間の位置及び姿勢の差分の和が最小となるように位置及び姿勢を推定するＳｋｒｙｐｎｙｋらの方法（Ｉ．  Ｓｋｒｙｐｎｙｋ  ａｎｄ  Ｄ．  Ｇ．  Ｌｏｗｅ，  “Ｓｃｅｎｅ  ｍｏｄｅｌｌｉｎｇ，  ｒｅｃｏｇｎｉｔｉｏｎ  ａｎｄ  ｔｒａｃｋｉｎｇ  ｗｉｔｈ  ｉｎｖａｒｉａｎｔ  ｉｍａｇｅ  ｆｅａｔｕｒｅｓ，”  Ｐｒｏｃ．  ３ｒｄ  ＩＥＥＥ  ａｎｄ  ＡＣＭ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｓｙｍｐｏｓｉｕｍ  ｏｎ  Ｍｉｘｅｄ  ａｎｄ  Ａｕｇｍｅｎｔｅｄ  Ｒｅａｌｉｔｙ，  ｐｐ．１１０−１１９，  ２００４．）と組み合わせて推定を行う。
【００７９】
ステップＳ３１７０では、補正部３５０は、ステップＳ３１６０で推定された世界座標系における特徴点の三次元座標を各キーフレームのカメラ座標系における三次元座標に変換し、該変換した三次元座標のＺ成分をｉｎｖｅｒｓｅ  ｄｅｐｔｈに変換する。
【００８０】
ステップＳ３１８０では、補正部３５０は、キーフレームの位置及び姿勢をステップＳ３１６０で推定された世界座標系におけるキーフレームの位置及び姿勢に更新する。また補正部３５０は、キーフレームに含まれているデプスマップＤにおいて特徴点のｉｎｖｅｒｓｅ  ｄｅｐｔｈを、ステップＳ３１７０で変換された該特徴点のｉｎｖｅｒｓｅ  ｄｅｐｔｈに更新する。
【００８１】
このように、本実施形態によれば、シーン中に配置した補助特徴から得られるキーフレーム間の相対的な位置及び姿勢を用いてキーフレームの位置及び姿勢に加えて特徴点の三次元座標も補正することで、三次元マップを高精度化することができる。
【００８２】
＜変形例＞
三次元マップの保持方法は、キーフレームが個別にデプスマップＤを保持する方法に限るものではない。例えば、Ｋｌｅｉｎらの方法（Ｇ．  Ｋｌｅｉｎ  ａｎｄ  Ｄ．  Ｍｕｒｒａｙ，“Ｐａｒａｌｌｅｌ  Ｔｒａｃｋｉｎｇ  ａｎｄ  Ｍａｐｐｉｎｇ  ｆｏｒ  Ｓｍａｌｌ  ＡＲ  Ｗｏｒｋｓｐａｃｅｓ，”  Ｐｒｏｃ．６ｔｈ  ＩＥＥＥ  ａｎｄ  ＡＣＭ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｓｙｍｐｏｓｉｕｍ  ｏｎ  Ｍｉｘｅｄ  ａｎｄ  Ａｕｇｍｅｎｔｅｄ  Ｒｅａｌｉｔｙ，ｐｐ．２２５−２３４，２００７．）のように、三次元マップの中に唯一のデプスマップを保持していてもよい。
【００８３】
また、キーフレーム間の相対的な位置及び姿勢はシーン中に配置した補助特徴から導出する必要は必ずしもなく、第２の実施形態のように撮像装置の位置または姿勢を計測するセンサから取得してもよい。
【００８４】
［第４の実施形態］
図２，６，８に示した各機能部はハードウェアで実装しても良いが、保持部１１０を除く各機能部をソフトウェア（コンピュータプログラム）で実装しても良い。後者の場合、保持部１１０として機能するメモリを有し、該コンピュータプログラムを実行可能なコンピュータ装置は、上記の各実施形態や各変形例に係る情報処理装置に適用可能である。上記の各実施形態や各変形例に係る情報処理装置に適用可能なコンピュータ装置のハードウェア構成例について、図１のブロック図を用いて説明する。
【００８５】
ＣＰＵ１０は、ＲＯＭ２０やＲＡＭ３０に格納されているコンピュータプログラムやデータを用いて処理を実行する。これによりＣＰＵ１０は、コンピュータ装置全体の動作制御を行うと共に、情報処理装置が行うものとして上述した各処理を実行若しくは制御する。ＲＯＭ２０には、コンピュータ装置の設定データや各種の基本プログラムが格納されている。
【００８６】
ＲＡＭ３０は、外部記憶装置７０からロードされたコンピュータプログラムやデータ、入力Ｉ／Ｆ（インターフェース）４０を介して外部（撮像部１７０やセンサ２９０）から受信したデータ、を格納するためのエリアを有する。更にＲＡＭ３０は、ＣＰＵ１０が各種の処理を実行する際に用いるワークエリアを有する。このようにＲＡＭ３０は、各種のエリアを適宜提供することができる。
【００８７】
入力Ｉ／Ｆ４０は、外部からの入力を受信するためのＩ／Ｆとして機能するものであり、例えば、撮像部１７０やセンサ２９０から出力される撮像画像や計測値は、この入力Ｉ／Ｆ４０を介して受信される。
【００８８】
表示部６０は、ＣＲＴや液晶画面などにより構成されており、ＣＰＵ１０による処理結果を画像や文字などでもって表示することができる。また表示部６０はタッチパネル画面であっても良い。
【００８９】
外部記憶装置７０は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置７０には、ＯＳ（オペレーティングシステム）や、情報処理装置が行うものとして上述した各処理をＣＰＵ１０に実行させるためのコンピュータプログラムやデータが保存されている。外部記憶装置７０に保存されているコンピュータプログラムには、図２，６，８において保持部１１０を除く各機能部の機能をＣＰＵ１０に実現させるためのコンピュータプログラムが含まれている。また、外部記憶装置７０に保存されているデータには、上記の説明において既知の情報として説明したもの（例えば閾値や各種のパラメータ）が含まれている。外部記憶装置７０に保存されているコンピュータプログラムやデータは、ＣＰＵ１０による制御に従って適宜ＲＡＭ３０にロードされ、ＣＰＵ１０による処理対象となる。なお、上記の保持部１１０や不図示のメモリとして説明したものは、ＲＯＭ２０やＲＡＭ３０、外部記憶装置７０によって実装可能である。
【００９０】
操作部８０は、キーボードやマウスなどのユーザインターフェースにより構成されており、ユーザが操作することで各種の指示をＣＰＵ１０に対して入力することができる。例えば、ユーザは操作部８０を操作することで図４，７に示した処理の終了指示を入力することができる。
【００９１】
ＣＰＵ１０、ＲＯＭ２０、ＲＡＭ３０、入力Ｉ／Ｆ４０、表示部６０、外部記憶装置７０、操作部８０、は何れもバス９０に接続されている。なお、図１に示したハードウェア構成は、上記の情報処理装置に適用可能なハードウェア構成の一例に過ぎない。
【００９２】
［第５の実施形態］
本実施形態では、上記の各実施形態に適用可能なＧＵＩ（グラフィカルユーザインターフェース）について説明する。例えば、図４のフローチャートに従って補正された三次元マップに基づいて仮想空間を構築し、該仮想空間を任意の視点（仮想カメラ）から見た画像（仮想空間画像）を提示するＧＵＩ（ウィンドウ）の一例を図１０に示す。このＧＵＩは、例えば、表示部６０に表示される。
【００９３】
このＧＵＩには、仮想カメラから見える仮想空間の仮想空間画像に加え、特徴点（特徴点の三次元座標を画面に投影した位置に表示）、キーフレームの位置及び姿勢を示すオブジェクト、を描画する。本実施形態では、特徴点は丸い点、キーフレームを表すオブジェクトはカメラを模した錐体により表現する。ユーザは操作部８０を操作して仮想カメラの位置や姿勢、内部パラメータ等を変更し、作成された三次元マップに対応する仮想空間を様々な視点から観察して確認することができる。キーフレーム対応情報が三次元マップに存在する場合には、対応するキーフレームを表す錐体の頂点同士（オブジェクト間）を線分で結ぶことにより、どのキーフレームが三次元マップの補正に利用されたかを確認することができる。その際に、キーフレームの対応がどのように生成されたかによって線分の色を変えることで補助特徴が効果的に利用されているかを確認できるようにする。すなわち、キーフレームの対応（キーフレーム間の相対的な位置及び姿勢の導出）が、ステップＳ１１３０、ステップＳ１１４０、ステップＳ１１６０のどのステップで得られたかにより錐体を結ぶ線分の色を変更する。このＧＵＩには、特徴点とキーフレームに加えて、補助特徴を表示してもよい。
【００９４】
また、第２の実施形態で説明したセンサの計測値に基づいてキーフレームの対応が得られた場合にはさらに別の色の線分を描画する。なお、本実施形態では、線分の色を変えることに加え／若しくは代えて線分のパターン（実線、点線、太線、細線など）を変えても良く、変更する表示形態は色に限らない。
【００９５】
［第６の実施形態］
第１の実施形態では、正方形、三角形、円形などの補助的な特徴を三次元マップの補正に利用していた。しかしながら、補助的な特徴はこれに限るものではなく、位置及び姿勢の推定に必要な幾何情報が既知である三次元的な物体を用いてもよい。このような場合、図４のフローチャートに従った処理において、ステップＳ１０５０において次のような処理を行うようにすればよい。
【００９６】
つまり本実施形態に係るステップＳ１０５０では、第２の導出部１６０は、シーンに存在する、または配置した三次元的な物体（以下、三次元物体）を用いてキーフレーム間の相対的な位置及び姿勢を算出する。第１の実施形態では補助特徴の頂点の画像座標と三次元座標との対応に基づいてキーフレームの参照位置及び参照姿勢を算出していた。本実施形態では、三次元物体の幾何情報とキーフレームが保持する画像をもとに参照位置及び参照姿勢を算出する。具体的には、三次元物体のエッジモデルを保持しておき、Ｄｒｕｍｍｏｎｄらの手法（Ｔ．Ｄｒｕｍｍｏｎｄ  ａｎｄ  Ｒ．Ｃｉｐｏｌｌａ，“Ｒｅａｌ−ｔｉｍｅ  Ｖｉｓｕａｌ  Ｔｒａｃｋｉｎｇ  ｏｆ  Ｃｏｍｐｌｅｘ  Ｓｔｒｕｃｔｕｒｅｓ，”  ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  Ｐａｔｔｅｒｎ  Ａｎａｌｙｓｉｓ  ａｎｄ  Ｍａｃｈｉｎｅ  Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２４，ｎｏ．７，ｐｐ．９３２−９４６，２００２．）により参照位置及び参照姿勢を算出する。
【００９７】
このように、本実施形態によれば、位置及び姿勢の推定に必要な幾何情報が既知な任意の三次元物体を補助特徴として利用することが可能であるため、シーンの景観を損ねることを防止できる。
【００９８】
なお、三次元物体を用いた参照位置及び参照姿勢の算出方法はエッジモデルを用いた方法に限るものではなく、三次元物体上の特徴点を用いたり、輝度情報を用いたりしてもよい。また、通常の画像だけでなく距離画像を利用できる場合には、距離画像への三次元モデルのフィッティングにより参照位置及び参照姿勢を算出してもよい。さらには通常の画像と距離画像の双方への三次元モデルのフィッティングを行ってもよい。
【００９９】
［第７の実施形態］
画像を用いた三次元マップの作成及び撮像部１７０の位置及び姿勢の計測は、非特許文献１で開示される方法に限るものではない。例えば、Ｋｌｅｉｎらの方法（Ｇ．  Ｋｌｅｉｎ  ａｎｄ  Ｄ．  Ｍｕｒｒａｙ，“Ｐａｒａｌｌｅｌ  Ｔｒａｃｋｉｎｇ  ａｎｄ  Ｍａｐｐｉｎｇ  ｆｏｒ  Ｓｍａｌｌ  ＡＲ  Ｗｏｒｋｓｐａｃｅｓ，”  Ｐｒｏｃ．６ｔｈ  ＩＥＥＥ  ａｎｄ  ＡＣＭ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｓｙｍｐｏｓｉｕｍ  ｏｎ  Ｍｉｘｅｄ  ａｎｄ  Ａｕｇｍｅｎｔｅｄ  Ｒｅａｌｉｔｙ，ｐｐ．２２５−２３４，２００７．）のように明示的に検出された特徴点を利用するものであってもよい。また、ＲＧＢＤカメラやデプスカメラを用いる方法であってもよい。
【０１００】
また、新しく追加されたキーフレームとの相対的な位置及び姿勢を算出するために選択されるキーフレームは必ずしも一つである必要はなく、複数のキーフレームを選択してもよい。例えば、新しく追加されたキーフレームから一定距離以上離れたキーフレームすべてを選択してもよいし、新しく追加されたキーフレームからの距離が大きい順に複数個のキーフレームを選択してもよい。
【０１０１】
また、三次元マップの補正は、位置及び姿勢の導出、三次元マップの更新の後に逐次的に実施する必要はない。三次元マップの補正には時間がかかるため、位置及び姿勢の導出、三次元マップの更新と並列に実施してもよい。
【０１０２】
また、上記の実施形態や変形例では、撮像部１７０やセンサ２９０と情報処理装置（情報処理装置１、２，３）とを別個にするのではなく、撮像部１７０やセンサ２９０を情報処理装置に組み込んで一体化させても良い。なお、以上説明した各実施形態や各変形例はその一部若しくは全部を適宜組み合わせて使用しても構わないし、選択的に使用しても構わない。
【０１０３】
＜効果のまとめ＞
シーン中に配置した補助特徴を利用して導出されるキーフレーム間の相対的な位置及び姿勢を三次元マップの補正に利用することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。
【０１０４】
また、位置及び姿勢を計測するセンサの計測値をもとにキーフレーム間の相対的な位置及び姿勢を導出することで、カメラの移動範囲が限定される場合であっても高精度な三次元マップを作成できる。
【０１０５】
さらに、補助特徴やセンサの計測値をもとに導出されるキーフレーム間の相対的な位置及び姿勢を用いてキーフレームの位置及び姿勢に加えて特徴点の三次元座標も補正することで、さらに三次元マップを高精度化することができる。
【０１０６】
＜定義のまとめ＞
保持部１１０が保持する三次元マップは、撮像画像、画像撮像時の撮像部１７０の位置及び姿勢、現実空間中の幾何特徴の三次元情報から構成されるキーフレーム群、によって表されるものであれば何でもよい。例えば、現実空間中の幾何特徴の三次元情報は、現実空間を基準とした座標系における三次元座標であってもよいし、キーフレームを基準とした座標における三次元座標やデプスマップであってもよい。
【０１０７】
また撮像部１７０は、現実空間の画像を撮像するカメラであれば何でもよい。例えば、濃淡画像を撮影するカメラでもよいし、カラー画像を撮影するカメラであってもよい。さらには、カメラは１台であってもよいし、ステレオカメラなどの複数台のカメラによって構成される撮像装置であってもよい。また距離画像を撮影するデプスカメラや、カラー画像と距離画像を同時に撮影するＲＧＢＤカメラであってもよい。
【０１０８】
また、第１の導出部１３０は、入力画像と三次元マップをもとに入力画像の撮像時の撮像部１７０の位置及び姿勢を導出するものであれば何でもよい。例えば、濃淡画像上の輝度値の勾配が大きい点を特徴点として位置及び姿勢を導出するものでもよいし、特徴点を画像から明示的に検出して位置及び姿勢を導出するものでもよい。
【０１０９】
また、更新部１４０は、撮像時の撮像部１７０の位置及び姿勢と、第１の導出部１３０が位置及び姿勢を導出した画像と、を用いて三次元マップを更新するものであれば何でもよい。例えば、導出された位置及び姿勢をもとに、既存の三次元マップ中の特徴点の三次元座標やデプスマップを更新してもよい。また、新規のキーフレームを生成し、三次元マップに追加してもよい。
【０１１０】
また、第２の導出部１６０は、キーフレーム間の相対的な位置及び姿勢を導出するものであれば何でもよい。例えば、シーン中に補助的な特徴を別途配置し、画像上で検出される特徴の情報をもとに導出されるキーフレームの位置及び姿勢を用いて相対的な位置及び姿勢を導出してもよい。また、撮像部１７０に位置または姿勢を計測するセンサを装着し、センサによって計測される撮像部１７０の位置または姿勢をもとに相対的な位置及び姿勢を導出してもよい。
【０１１１】
また、補正部１５０は、第２の導出部１６０から得られるキーフレーム間の相対的な位置及び姿勢をもとに三次元マップを補正するものであれば何でもよい。例えば、キーフレームの位置及び姿勢のみを補正してもよいし、さらに特徴点の三次元座標やデプスマップを補正してもよい。
【０１１２】
（その他の実施例）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【符号の説明】
【０１１３】
１１０：保持部  １２０：画像入力部  １３０：第１の導出部  １４０：更新部  １５０：補正部  １６０：第２の導出部  １７０：撮像部  １９９：制御部
【図１】
2019020778.tif 000009
【図２】
2019020778.tif 000010
【図３】
2019020778.tif 000011
【図４】
2019020778.tif 000012
【図５】
2019020778.tif 000013
【図６】
2019020778.tif 000014
【図７】
2019020778.tif 000015
【図８】
2019020778.tif 000016
【図９】
2019020778.tif 000017
【図１０】
2019020778.tif 000018