情報処理装置、および情報処理方法、並びにプログラム
【課題】計算量を削減し効率的な３次元位置情報の生成を行う構成を実現する。
【解決手段】カメラによる撮影画像を観測データとして入力し、特徴点位置情報を含む多次元正規分布データからなる状態データを更新することで特徴点の３次元位置を求める例えばＳＬＡＭ処理において、３次元情報の生成対象領域を区分したサブマップを設定する。さらにサブマップに属しない特徴点に関するデータを含まないサブマップ対応の状態データを生成して、観測データに基づいてサブマップ対応状態データを更新する。本構成によれば状態データのデータ量が削減され、状態データ更新に伴う計算量を減少させることが可能であり、効率的な処理が実現される。
【解決手段】カメラによる撮影画像を観測データとして入力し、特徴点位置情報を含む多次元正規分布データからなる状態データを更新することで特徴点の３次元位置を求める例えばＳＬＡＭ処理において、３次元情報の生成対象領域を区分したサブマップを設定する。さらにサブマップに属しない特徴点に関するデータを含まないサブマップ対応の状態データを生成して、観測データに基づいてサブマップ対応状態データを更新する。本構成によれば状態データのデータ量が削減され、状態データ更新に伴う計算量を減少させることが可能であり、効率的な処理が実現される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、情報処理装置、および情報処理方法、並びにプログラムに関する。さらに詳細には、３次元空間の解析処理を実行する情報処理装置、および情報処理方法、並びにプログラムに関する。
【背景技術】
【０００２】
３次元空間の解析の処理手法として、３次元空間をカメラによって撮影し、その撮影画像の解析を行う手法がある。例えばカメラによって撮影された画像から複数の特徴点（ランドマーク（Ｌａｎｄｍａｒｋ）とも呼ばれる）を抽出し、これらの特徴点の３次元位置を推定する処理が行われる。
【０００３】
カメラを移動させながら画像を撮影し、複数の撮影画像に含まれる特徴点の軌跡を解析することで、特徴点の３次元位置を推定するとともに、カメラ（自己）の位置姿勢を推定（ローカリゼーション）することが可能となる。また特徴点の３次元位置情報を用いて周囲の地図（環境地図）を作成（ｍａｐｐｉｎｇ）することができる。このように、カメラ(自己)の位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を並行して実行する処理はＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）と呼ばれる。
【０００４】
ＳＬＡＭの手法の１つとして、ＥＫＦ（拡張カルマンフィルタ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を使用したＥＫＦベースのＳＬＡＭがある。なお、ＥＫＦベースのＳＬＡＭについては、例えば非特許文献１（Ｓ．Ｔｈｒｕｎ，Ｗ．Ｂｕｒｇａｒｄ，ａｎｄ Ｄ．Ｆｏｘ． Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｒｏｂｏｔｉｃｓ． ＭＩＴ Ｐｒｅｓｓ， Ｃａｍｂｒｉｄｇｅ，ＭＡ，２００５）に記載されている。
【０００５】
ＥＫＦベースのＳＬＡＭは、例えばカメラを移動させながら画像を連続的に撮影し、各画像に含まれる特徴点の軌跡（トラッキング情報）を求め、移動ステレオ法でカメラの移動量と特徴点の３次元位置を同時に推定する方法である。この処理には、例えば、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報、
これらの各情報を含む確率分布モデルとしての多次元正規分布データからなる「状態データ」を利用する。カルマンフィルタや拡張カルマンフィルタを用いて「状態データ」の更新処理を行って、特徴点位置、カメラ位置等を推定する。
【０００６】
「状態データ」は、カメラの位置、姿勢、速度、角速度と、各特徴点の位置情報を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。分散共分散行列は、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報、これらの固有状態値の［分散］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散］を含む行列である。なお、具体的なデータ構成については本発明の説明の欄で説明する。
【０００７】
ＥＫＦベースのＳＬＡＭにおける「状態データ」の更新処理では、移動するカメラの撮影画像を利用するが、その結果、「状態データ」には登録されているが、処理対象として入力した最新のカメラ撮影画像に写っていない特徴点情報についても更新対象として処理を行うことになる。すなわち先行して入力した画像に含まれる特徴点が、最新の入力画像には含まれなくなる場合がある。先行して入力した画像に含まれる特徴点に関する情報は「状態データ」に登録される。この結果、「状態データ」に含まれる「カメラに写っている特徴点」と「カメラに写っていない特徴点」との相関情報についてもカルマンフィルタを適用して更新する処理が実行されることになる。
【０００８】
すなわち、マップ作成対象領域が大きくなると特徴点の数も増大する。「状態データ」に多数の特徴点情報を含めた場合、カルマンフィルタや拡張カルマンフィルタを用いた「状態データ」の更新処理における計算量が大きくなる。
【０００９】
「状態データ」には前述のように分散共分散行列が含まれ、この行列に全ての特徴点とカメラの相関情報が保持されることになる。従って特徴点数が増えると分散共分散行列のサイズも大きくなる。カルマンフィルタや拡張カルマンフィルタを用いた「状態データ」の更新処理における計算量は、特徴点の数ｎに対して、計算量がＯ（９ｎ２）のオーダーで増加する。データ更新は、繰り返し行うことが必要であり、その結果、特徴点数の多い広い領域の３次元マップを作成する場合の計算処理の負荷、処理時間が過大になるという問題がある。
【先行技術文献】
【非特許文献】
【００１０】
【非特許文献１】Ｓ．Ｔｈｒｕｎ，Ｗ．Ｂｕｒｇａｒｄ，ａｎｄ Ｄ．Ｆｏｘ． Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｒｏｂｏｔｉｃｓ． ＭＩＴ Ｐｒｅｓｓ， Ｃａｍｂｒｉｄｇｅ，ＭＡ，２００５
【発明の概要】
【発明が解決しようとする課題】
【００１１】
本発明は、例えば上述の問題点に鑑みてなされたものであり、撮影画像解析による特徴点位置推定を行う構成において、処理負荷を軽減し、精度を低下させることなく特徴点の３次元位置を推定し、３次元マップの生成を可能とする情報処理装置、および情報処理方法、並びにプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１２】
本発明の第１の側面は、
空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することで、カメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成部と、
３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定部を有し、
前記３次元情報生成部は、
前記サブマップ設定部の設定したサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行して、カメラの位置、姿勢、または各特徴点の３次元位置を求める情報処理装置にある。
【００１３】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、前記サブマップ対応状態データとして、前記カメラの位置、姿勢と、前記サブマップ内の各特徴点の位置の状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データを、各サブマップに対応させて設定し、設定したサブマップ対応の多次元正規分布データを観測情報を適用して更新する。
【００１４】
さらに、本発明の情報処理装置の一実施態様において、前記分散共分散行列は、カメラの位置、姿勢、各特徴点の位置情報、これらの固有状態値の分散と、前記各状態値の異なる状態値の組み合わせの相関情報に対応する共分散を含む行列である。
【００１５】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、カルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、または拡張カルマンフィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を適用して前記サブマップ対応状態データの更新処理を実行する。
【００１６】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、旧サブマップから新サブマップへ、サブマップを切り換えて処理を行う場合、旧サブマップ対応状態データに含まれるカメラ情報をそのまま利用して新サブマップ対応状態データを生成する。
【００１７】
さらに、本発明の情報処理装置の一実施態様において、前記サブマップ設定部は、サブマップに含まれる特徴点数が予め既定した上限数Ｎ以上となった場合、重複領域を持つ２つのサブマップに分割するサブマップ分割処理を実行し、当該２つのサブマップには少なくとも一つは同一の特徴点を有することを特徴とする。
【００１８】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、サブマップ分割処理において生成した２つの分割サブマップ各々に対応する２つのサブマップ対応状態データを生成する処理を行う構成であり、分割前のサブマップのサブマップ対応状態データから、各分割サブマップに含まれない特徴点情報を各々削除して２つの分割サブマップ各々のサブマップ対応状態データを生成する。
【００１９】
さらに、本発明の情報処理装置の一実施態様において、前記サブマップ設定部は、３次元位置の算出対象となる領域を射影変換して生成した２次元平面を予め既定した大きさで、かつ重複領域を持つ複数のサブマップに分割するサブマップ分割処理を実行する。
【００２０】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、バンドル調整処理による特徴点位置修正処理を実行する。
【００２１】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を並行して実行するＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理を行う。
【００２２】
さらに、本発明の第２の側面は、
情報処理装置において実行する情報処理方法であり、
サブマップ設定部が、３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定ステップと、
３次元情報生成部が、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行してカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップである情報処理方法にある。
【００２３】
さらに、本発明の第３の側面は、
情報処理装置において情報処理を実行させるプログラムであり、
サブマップ設定部に、３次元位置の算出対象となる領域を分割したサブマップを設定させるサブマップ設定ステップと、
３次元情報生成部に、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求めさせる３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行させてカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップであるプログラムにある。
【００２４】
なお、本発明のプログラムは、例えば、様々なプログラム・コードを実行可能な画像処理装置やコンピュータ・システムに対して、コンピュータ可読な形式で提供する記憶媒体、通信媒体によって提供可能なプログラムである。このようなプログラムをコンピュータ可読な形式で提供することにより、画像処理装置やコンピュータ・システム上でプログラムに応じた処理が実現される。
【００２５】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施例や添付する図面に基づくより詳細な説明によって明らかになるであろう。なお、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。
【発明の効果】
【００２６】
本発明の一実施例の構成によれば、計算量を削減し効率的な３次元位置情報の生成を行う構成が実現される。カメラによる撮影画像を観測データとして入力し、特徴点位置情報を含む多次元正規分布データからなる状態データを更新することで特徴点の３次元位置を求める例えばＳＬＡＭ処理において、３次元情報の生成対象領域を区分したサブマップを設定する。さらにサブマップに属しない特徴点に関するデータを含まないサブマップ対応の状態データを生成して、観測データに基づいてサブマップ対応状態データを更新する。本構成によれば状態データのデータ量が削減され、状態データ更新に伴う計算量を減少させることが可能であり、効率的な処理が実現される。
【図面の簡単な説明】
【００２７】
【図１】ＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理の概要について説明する図である。
【図２】ＳＬＡＭ処理に適用する状態データの構成について説明する図である。
【図３】ピンホールカメラモデルについて説明する図である。
【図４】ピンホールカメラモデルについて説明する図である。
【図５】ＳＬＡＭ処理に適用する状態データの構成について説明する図である。
【図６】本発明の情報処理装置の処理に適用するサブマップと状態データについて説明する図である。
【図７】本発明の情報処理装置の処理に適用するサブマップと画像フレームとの対応について説明する図である。
【図８】本発明の情報処理装置の処理に適用するサブマップと状態データについて説明する図である。
【図９】本発明の一実施例の情報処理装置の構成例について説明する図である。
【図１０】本発明の一実施例の情報処理装置において実行する処理のシーケンスについて説明するフローチャートを示す図である。
【図１１】図１０に示すフローチャートのステップＳ１０１における注目サブマップ選択処理の詳細シーケンスについて説明するフローチャートを示す図である。
【図１２】図１０に示すフローチャートのステップＳ１０２における注目サブマップに対する「状態データ」を用意する処理について説明するフローチャートを示す図である。
【図１３】サブマップ対応の状態データについて説明する図である。
【図１４】注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理のシーケンスについて説明するフローチャートを示す図である。
【図１５】図１４のフローにおけるステップＳ３０３の「注目サブマップの分割処理」の詳細シーケンスについて説明するフローチャートを示す図である。
【図１６】図１５のフローにおけるステップＳ４５２〜Ｓ４５４のサブマップの分割処理の具体例について説明する図である。
【図１７】図１５のフローにおけるステップＳ４５５〜Ｓ４５６のサブマップ対応の状態データに対する処理の具体例について説明する図である。
【図１８】図１５のフローにおけるステップＳ４５５〜Ｓ４５６のサブマップ対応の状態データに対する処理の具体例について説明する図である。
【図１９】図１５のフローにおけるステップＳ４５５〜Ｓ４５６のサブマップ対応の状態データに対する処理の具体例について説明する図である。
【図２０】３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理の詳細シーケンスについて説明するフローチャートを示す図である。
【図２１】３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理の具体例について説明する図である。
【図２２】３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理の具体例について説明する図である。
【図２３】図１０のフローにおけるステップＳ１０８の全体最適化処理の詳細シーケンスについて説明するフローチャートを示す図である。
【図２４】バンドル調整処理について説明する図である。
【図２５】サブマップの重複領域の必要性について説明する図である。
【図２６】サブマップの重複領域の必要性について説明する図である。
【発明を実施するための形態】
【００２８】
以下、図面を参照しながら本発明の実施形態に係る情報処理装置、および情報処理方法、並びにプログラムの詳細について説明する。説明は以下の項目に従って行う。
１．ＳＬＡＭ処理の概要について
２．サプマップについて
３．情報処理装置の構成例およびサブマップを適用した処理例について
４．サブマップの設定手法の追加説明
【００２９】
［１．ＳＬＡＭ処理の概要について］
本発明の情報処理装置は、カメラによって撮影された画像に含まれる特徴点情報を適用して特徴点の３次元位置を推定する処理を実行する。例えば、カメラ位置の同定（ローカリゼーション）と周囲の地図作成（ｍａｐｐｉｎｇ）処理を並行して実行するＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理を行う。
【００３０】
具体的には、カメラを移動させながら画像を連続的に撮影し、各画像に含まれる特徴点の軌跡（トラッキング情報）を求め、移動ステレオ法でカメラの移動量と特徴点の３次元位置を同時に推定する。
【００３１】
前述したようにＳＬＡＭの手法の１つとして、カルマンフィルタを適用したＳＬＡＭや、ＥＫＦ（拡張カルマンフィルタ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を使用したＥＫＦベースのＳＬＡＭがある。ＳＬＡＭについて図１以下を参照して説明する。図１のシステムは、単眼カメラ１０１から得られる映像を入力し解析を行う。カメラ１０１は、移動しながら画像を連続的に撮影する。ＳＬＡＭは、映像内の特徴点のトラッキングを求め、特徴点の移動量（トラッキング結果）から移動ステレオ法でカメラの移動量と特徴点の３次元位置を同時に推定する。
【００３２】
ＳＬＡＭでは、図２に示す状態データを用いた処理を行う。すなわち、図１に示すカメラ１０１の移動量と、カメラ１０１によって撮影された画像に含まれる特徴点１０３の位置を、多次元正規分布で表現された「状態データ」を利用して推定する。
【００３３】
「状態データ」は、図２に示すように、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。分散共分散行列は、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報、これらの固有状態値の［分散（σａａ）］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散（σａｂ）］を含む行列である。
【００３４】
カメラは移動しながら画像を撮影し、順次、取得する画像から特徴点トラッキング情報（軌跡）を得て、カルマンフィルタや拡張カルマンフィルタ（ＥＫＦ）を適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行うものである。なお、単眼カメラはピンホールカメラモデルにフィットするものとし、全ての特徴点は静止していると仮定する。
【００３５】
ピンホールカメラモデルについて、図３、図４を参照して説明する。ピンホールカメラモデルにおいて、特徴点の画像フレーム中の位置は下式（数式１）によって計算することができる。
【００３６】
【数１】
・・・（数式１）
【００３７】
上記式の意味について、図３、図４を参照して説明する。上記式は、カメラの撮影画像１３０に含まれるオブジェクト１３１の点（ｍ）のカメラ撮影画像平面の画素位置１３２、すなわち、カメラ座標系によって表現されている位置と、世界座標系におけるオブジェクト１２０の３次元位置（Ｍ）１２１との対応関係を示す式である。
【００３８】
カメラ撮影画像平面の画素位置１３２はカメラ座標系によって表現されている。カメラ座標系は、カメラの焦点を原点Ｃとして、像平面がＸｃ，Ｙｃの二次元平面、奥行きをＺｃとした座標系であり、カメラの動きによって原点Ｃは移動する。
【００３９】
一方、オブジェクト１２０の３次元位置（Ｍ）１２１は、カメラの動きによって移動しない原点Ｏを有するＸＹＺ三軸からなる世界座標系によって示される。この異なる座標系でのオブジェクトの位置の対応関係を示す式が上述のピンホールカメラモデルとして定義される。
【００４０】
この式に含まれる値は、図４に示すように、
λ：正規化パラメータ
Ａ：カメラ内部パラメータ、
Ｃｗ：カメラ位置、
Ｒｗ：カメラ回転行列、
を意味している。
【００４１】
さらに、
【数２】
は、同次座標系で表現されたカメラの像平面上の位置である。
λは、正規化パラメータであり、
【数３】
の第３項を満足させるための値である。
【００４２】
なお、カメラ内部パラメータＡには、以下の値が含まれる。
ｆ：焦点距離
θ：画像軸の直交性（理想値は９０°）
ｋｕ：縦軸のスケール（３次元位置のスケールから二次元画像のスケールへの変換）
ｋｖ：横軸のスケール（３次元位置のスケールから二次元画像のスケールへの変換）
（ｕ０，ｖ０）：画像中心位置
【００４３】
このように、世界座標系にある特徴点は位置［Ｍ］で表現される。また、カメラは位置［Ｃｗ］と姿勢（回転行列）Ｒｗで表現される。カメラの焦点位置・画像中心等はカメラ内部パラメータ［Ａ］で表現される。これらのパラメータから、「世界座標系にある特徴点」から「カメラの像平面上」に射影されるそれぞれの位置の関係式が、上述した式（数式１）によって表現することができる。
【００４４】
また、カメラの位置・姿勢を回転行列Ｒｗと並進ベクトルｔｗで表現した場合は、以下に示す式（数式２）のようになる。但し、並進ベクトルｔｗとカメラ位置との関係は式（数式３）で表される。
【００４５】
【数４】
・・・（数式２）
【００４６】
【数５】
・・・（数式３）
【００４７】
図１のシステムにおける単眼カメラ１０１は、このようなピンホールカメラモデルにフィットするカメラである。カメラは移動しながら画像を撮影し、順次、取得する画像から特徴点トラッキング情報（軌跡）を得て、カルマンフィルタや拡張カルマンフィルタを適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行うものである。
【００４８】
カルマンフィルタ、および拡張カルマンフィルタについて説明する。カルマンフィルタは、カメラによって撮影される画像を構成するフレームを順次入力して処理を行う。処理対象フレームの１フレーム前の状態（推定値）と現在フレームから得られる観測値（特徴点トラッキング結果）から現在の状態（推定値）を与える逐次推定型（予測）フィルタである。なお、「状態」は、図２に示す（ａ）平均ベクトルと、（ｂ）分散共分散行列で構成される多次元正規分布モデルで表現される状態データに対応する。
【００４９】
以下にカルマンフィルタの数式を示す。
【数６】
但し、
Ｆｔは予測モデル（行列）
Ｈｔは観測モデル（行列）
を表し、平均０、分散共分散Ｑｔの多次元正規分布モデルに従う予測モデル誤差、平均０、分散共分散Ｒｔの多次元正規分布モデルに従う観測モデル誤差を表す。
また、[＾]は予測値を意味する。なお、各モデルの設定例は、例えば［ｈｔｔｐ：／／ｅｎ．ｗｉｋｉｐｅｄｉａ．ｏｒｇ／ｗｉｋｉ／Ｋａｌｍａｎ＿ｆｉｌｔｅｒ＃Ｅｘａｍｐｌｅｓ］を参照のこと。
【００５０】
カルマンフィルタは、ＦｔとＨｔが行列であるため、線形モデルしか扱えない。この問題を解決するために、拡張カルマンフィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）がある。拡張カルマンフィルタ（ＥＫＦ）は非線形の予測モデル・観測モデルを扱うことができる。拡張カルマンフィルタ（ＥＫＦ）の式は以下の通りである。
【００５１】
【数７】
但し、
Ｆは予測モデル関数、
Ｈは観測モデル関数、
を表し、
（▽Ｆ（ｘ））は関数Ｆのヤコビ行列、
（▽Ｈ（ｘ））は関数Ｈのヤコビ行列、
Ｑｔは予測モデル誤差（平均０，分散共分散Ｑｔの多次元正規分布モデルに従う）、
Ｒｔは観測モデル誤差（平均０，分散共分散Ｒｔの多次元正規分布モデルに従う）、
を表す。
【００５２】
図１に示すカメラ１０１は移動しながら画像を撮影し、順次、取得する画像から特徴点トラッキング情報（軌跡）を得て、上述したカルマンフィルタや拡張カルマンフィルタを適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行う。
【００５３】
先に図２を参照して説明したように、図１に示すカメラ１０１の移動量と、カメラ１０１によって撮影された画像に含まれる特徴点１０３の位置の推定は、図２に示す多次元正規分布で表現された「状態データ」を利用して行われる。
【００５４】
図２を参照して説明したように状態データは、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。図５（ａ）に示すように、分散共分散行列は、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報、
これらの固有状態値の［分散］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散］を含む行列である。
【００５５】
図２、図５に示す状態データにおいて特徴点＃１〜＃Ｎの位置はＰ１〜ＰＮとして示されている。分散共分散行列には、特徴点＃１〜＃Ｎの位置：Ｐ１〜ＰＮに関する共分散データが要素として含まれる。
【００５６】
カメラは移動しながら画像を連続的に撮影しており、撮影画像に含まれる特徴点は順次入れ替わる。すなわち、すべてのフレームに特徴点＃１〜＃Ｎが含まれるとは限らない。すなわち、ある画像フレームＦ１には含まれるが、次の画像フレームＦ２には含まれない特徴点や、画像フレームＦ２には含まれるが、前の画像フレームＦ１には含まれない特徴点が存在する。
【００５７】
すなわち、図５（ｂ）に示すように、カメラ１５１は移動ライン１５２に沿って移動して画像を連続撮影しており、各画像に含まれる特徴点は、順次入れ替わることになる。例えば時間ｔｎにおける撮影画像は撮影画像範囲１５３の画像であるとすると、その撮影画像に含まれる特徴点は、特徴点Ａ１〜Ａ３である。特徴点Ｂや特徴点Ｃは、その撮影画像には含まれず、その前の撮影画像やその後の撮影画像に含まれる特徴点となる。
【００５８】
従来型のＥＫＦベースのＳＬＡＭにおいては、処理時点で得られている特徴点の情報を全て含む「状態データ」を適用した処理が行われる。例えば、フレームｆ０１〜ｆｎｎの画像を利用したＳＬＡＭ処理を実行する場合、フレームｆ０１〜ｆｎｎにおいてカ検出される特徴点をすべて含む状態データを設定して、更新処理を行う。
【００５９】
従って、カルマンフィルタやＥＫＦ（拡張カルマンフィルタ）を適用した更新処理を行う状態データには登録されているが、観測値として入力される画像フレームに写っていない特徴点も存在し、この場合、状態データに含まれる観測値としての入力画像フレームにない特徴点情報の更新処理も行われる。状態データに含まれる共分散データである「カメラに写っている特徴点」と「カメラに写っていない特徴点」との相関情報に基づいてカルマンフィルタやＥＫＦを適用した更新処理が実行される。
【００６０】
結果として、ＳＬＡＭ処理では、多数の特徴点情報を含む更新処理が必要になり、現在の画像から得られていない特徴点の位置情報を用いた状態データの更新処理が行われることになる。このような処理は、計算コストの増大を発生させ処理の非効率化の要因になる。
【００６１】
１つ具体例をあげて説明する。現在の撮影画像に含まれる特徴点、すなわち、「見えている特徴点の位置」が正しく（正しい＝推定値と実際値があっている）、現在の撮影画像に含まれない特徴点、すなわち「カメラの位置姿勢・見えていない特徴点の位置」が正しくない場合を考える。「状態データ」に相関情報がない（＝分散共分散行列の共分散成分が全て０）モデルの場合はカメラ位置のみ更新される。「状態データ」に相関情報がある（＝分散共分散行列の共分散成分が全て０ではない）モデルの場合はカメラ位置姿勢の更新の度合と相関情報に応じて見えていない特徴点の位置も更新される。
【００６２】
ただし、分散共分散行列は全ての特徴点とカメラの相関情報を保持しているため、特徴点の数が増えると分散共分散行列のサイズも大きくなる。特徴点の数ｎに対して、計算量はＯ（９ｎ２）のオーダーで増加する。リアルタイム処理する場合は、特に大きな問題である。このように、状態データに特徴点情報を多く含む場合、処理時間および処理負荷が大きくなるという問題がある。
【００６３】
［２．サブマップについて］
そこで、本発明では、図６に示すように３次元位置解析対象となる全体領域を示すマップ２００を複数のサブマップ２１１，２２１に切り分けて処理を行う。各々のサブマップには、
（ａ）「サブマップの識別子としての番号（ＩＤ）」、
（ｂ）「ＳＬＡＭの計算で用いる「状態データ」」、
これらのデータが対応付けられる。
【００６４】
本実施例では、図６下段に示すサブマップＭ１，２１１，サブマップＭ２,２２１のようにＭ１，Ｍ２，・・・を各サブマップの識別子（ＩＤ）としている。サブマップ対応の状態データ２１２，２２２は、先に図２を参照して説明した状態データと同様のデータであり、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。分散共分散行列は、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報、これらの固有状態値の［分散（σａａ）］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散（σａｂ）］を含む行列である。
【００６５】
なお、各サブマップ対応の状態データに含まれる特徴点情報は、各々のサブマップ内に含まれる特徴点の情報のみである。すなわち、図６に示すサブマップＭ１，２１１対応の状態データ２１２は、
カメラの位置、姿勢、速度、角速度、
サブマップＭ１，２１１内の特徴点ａ，ｂ，ｃ，ｄ，ｅの位置情報、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
【００６６】
一方サブマップＭ２，２２１対応の状態データ２２２は、
カメラの位置、姿勢、速度、角速度、
サブマップＭ２，２２１内の特徴点ｄ，ｅ，ｆ，ｇ，ｈの位置情報、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
【００６７】
各サブマップ対応の状態データには、自己のサブマップのみに含まれる特徴点と、異なるサブマップにのみに含まれる特徴点との相互の相関情報、すなわち共分散データなど、自己のサブマップに含まれない特徴点に関する平均ベクトル、分散共分散データは含まれない。
【００６８】
例えばサブマップＭ１，２１１の状態データ２１２には、サブマップＭ１，２１１にのみ含まれる特徴点（ａ，ｂ，ｃ）と、サブマップＭ２，２２１のみに含まれる特徴点（ｆ，ｇ，ｈ）との相関情報、すなわち共分散データは含まれない。サブマップＭ２，２２１の状態データ２２２にも、サブマップＭ２，２２１にのみ含まれる特徴点（ｆ，ｇ，ｈ）と、サブマップＭ１，２１１のみに含まれる特徴点（ａ，ｂ，ｃ）との相関情報、すなわち共分散データは含まれない。
【００６９】
なお、サブマップ２１１，２２１ごとに対応して設定された「状態データ」２１２，２２２は全て世界座標系で表現される。ＳＬＡＭ処理においては、特定のサブマップに対応する「状態データ」のみを対象とした処理、例えばカルマンフィルタや拡張カルマンフィルタを適用した更新処理を行うことで計算量を低減する。
【００７０】
なお、サブマップは、ＳＬＡＭの実行単位として設定する領域である。図１に示すカメラ１０１によつて撮影される画像フレームに対応するものではない。カメラ１０１によって撮影される画像フレームとサブマップとの対応は例えば図７に示す対応となる。
【００７１】
カメラ１０１はポイントＡからポイントＢに移動しながら連続的な撮影処理を行っており、その間、例えば、フレームｆ００１〜ｆ１００、計１００フレームを撮影する。これらの多数の画像フレームから様々な特徴点が抽出され、特徴点のトラッキング（軌跡）データが取得される。
【００７２】
サブマップＭ１，２１１は、例えば画像フレームｆ００１〜ｆ０６０の６０フレーム分の画像を利用してＳＬＡＭ処理を行う領域である。また、サブマップＭ２，２２１は、例えば画像フレームｆ０４１〜ｆ１００の６０フレーム分の画像を利用してＳＬＡＭ処理を行う領域である。このように、サブマップＭ１，Ｍ２・・・は、ＳＬＡＭの実行単位として設定する領域である。なお、上述したフレーム数は一例であり、各サブマップに対応するフレーム数は任意の数でよい。
【００７３】
サブマップ領域の設定は、例えば、１つのサブマップに含まれる特徴点の数が、予め設定した上限数以下とするという条件を用いて行われる。なお、図６、図７には２つのサブマップのみを示しているが、サブマップ数は任意の数に設定できる。図８には、４つのサブマップＭ１〜Ｍ４の設定例を示している。図６〜図８に示すように、サブマップは別のサブマップと重なるように設定する。このとき重複した領域にある特徴点は各サブマップの状態変数で表現する。要するに、重複した領域にある特徴点は位置情報を複数保持する。複数ある特徴点位置は、後で位置情報が収束するように最適化処理する（図１０のフローチャートの「全体最適化」に該当）。詳しくは後述する。
【００７４】
［３．情報処理装置の構成例およびサブマップを適用した処理例について］
次に、本発明の情報処理装置の構成例およびサブマップを適用した処理例について説明する。図９は、本発明の情報処理装置の一実施例構成を示す図である。本発明の情報処理装置３００は、カメラ３０１によって撮影された画像データに基づいて、特徴点トラッキング情報（軌跡）を得て、カルマンフィルタや拡張カルマンフィルタを適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行うものである。なお、カメラ３０１はピンホールカメラモデルにフィットするものである。
【００７５】
本発明の一実施形態にかかる情報処理装置３００は図９に示すように、周囲環境を撮影するカメラ３０１の撮影した撮影画像をカメラ３０１から直接、あるいはカメラ３０１が撮影し、撮影画像記憶部３０２（ＤＢ０：データベース０）に格納された撮影画像を入力して処理を行う。
【００７６】
サブマップ設定部３０３は、３次元マップの生成対象となる処理対象領域（全体マップ）を任意の複数領域に区分したサブマップ領域を設定する。なお、３次元マップの生成対象となる領域は図９に示すカメラ３０１における撮影画像によってカバーされる領域となる。３次元マップの生成処理は、カメラ３０１の撮影画像をリアルタイムで順次入力して実行してもよいし、撮影済み画像を記憶した記憶部３０２から取得して実行してもよい。
【００７７】
撮影済み画像を記憶した記憶部から取得する場合は、３次元マップの生成対象となる処理対象領域（全体マップ）が予め確認できるので、確認された全体領域を複数のサブマップ領域に分割することができる。分割処理は任意の基準で行ってよい。例えば全体領域を２分割して初期的な２つのサブマップを設定する。あるいは４分割して４つのサブマップを設定するといった処理が可能である。あるいは一定の撮影画像フレーム数単位、撮影時間単位でサブマップを設定してもよい。たとえば先に図７を参照して説明したように、予め設定したフレーム数、例えば６０フレームの画像フレームによってカバーされる領域を１つのサブマップとする処理や、予め設定した所定のカメラ移動距離単位で１つのサブマップを設定するといった処理を行う構成としてもよい。
【００７８】
カメラ３０１の撮影画像をリアルタイムで順次入力して処理を行う場合も、予め設定したフレーム数の画像フレームによってカバーされる領域を１つのサブマップとする処理や、予め設定した所定のカメラ移動距離単位で１つのサブマップを設定するといった処理を行う構成とすることができる。
【００７９】
サブマップ設定部３０３は、このように、カメラ３０１または記憶部３０２から撮影画像を入力してサブマップの設定を行う。なお、初期的には１つのマップのみを設定し、そのマップを処理の進行に従って複数のサブマップに分割する処理を行う構成としてもよい。例えば処理対象とするマップまたはサブマップ内の特徴点数が予め既定した上限数以上になった場合に２つに分割するといった処理を行うことが可能である。あるいは処理対象のマップまたはサブマップの空間的な領域が既定サイズより大きくなった場合に、分割するといった処理が可能である。これらのサブマップ設定処理、分割処理については、後段で詳細に説明する。
【００８０】
情報処理装置３００は、さらに、カメラの位置姿勢を推定するカメラ位置姿勢検出部３０４、カメラ位置姿勢検出部３０４において検出されたカメラ位置姿勢情報を記憶するカメラ位置姿勢記憶部３０６（ＤＢ１：データベース１）を有する。
また、撮影画像に含まれる特徴点の追跡処理を実行する特徴点追跡部３０５、特徴点追跡部３０５の検出した特徴点の軌跡情報を記憶する特徴点軌跡（トラッキング）情報記憶部３０７（ＤＢ２：データベース２）を有する。
【００８１】
さらに、カメラ位置姿勢記憶部３０４に格納されたカメラ位置姿勢情報と、特徴点軌跡情報記憶部３０５に格納された特徴点軌跡情報を入力して、これらの入力情報に基づいてカメラの位置姿勢および特徴点の３次元位置を算出する３次元情報生成部３０８、さらに、自己（カメラ）位置と、特徴点３次元位置情報を含むサブマップ対応状態データ（存在確率分布データ）を格納した状態データ記憶部３０９を有する。
【００８２】
カメラ位置姿勢検出部３０４、特徴点追跡部３０５、および３次元情報生成部３０８は、サブマップ設定部３０３において設定されたサブマップ単位で処理を行なうことになる。
【００８３】
カメラ３０１は、例えば移動するロボットに装着され、あるいはユーザ（人）によって適宜、移動される。すなわちカメラ３０１の位置や姿勢は固定されたものではなく、変更された位置姿勢からの撮影画像、すなわち異なる視点から撮影された画像フレーム（例えば動画像）を順次入力する。なお、カメラ３０１は前述したようにピンホールカメラモデルに合うものを用いている。
【００８４】
カメラ位置姿勢検出部３０４は、カメラ３０１から入力する映像内の特徴点を利用して、特徴点の位置とカメラ位置を毎フレーム修正し、カメラ位置姿勢検出部３０４で決めた世界座標系で表されたカメラの推定位置および姿勢情報としてのカメラの位置（Ｃｗ）およびカメラの回転行列（Ｒｗ）を生成してカメラ位置姿勢記憶部３０６に格納する。
【００８５】
なお、この処理は、例えば論文［Ａｎｄｒｅｗ Ｊ．Ｄａｖｉｓｏｎ，"Ｒｅａｌ−ｔｉｍｅ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｓａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ ｗｉｔｈ ａ ｓｉｎｇｌｅ ｃａｍｅｒａ"，Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ９ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，Ｎｉｎｔｈ，（２００３）］に記載された方法を用いることができる。
【００８６】
なお、カメラ位置姿勢検出部３０４では、カメラの位置（Ｃｗ）およびカメラの回転行列（Ｒｗ）の各々について、正規分布に従った存在確率分布を持つカメラ位置姿勢情報を生成してカメラ位置姿勢記憶部３０６に格納する。
【００８７】
カメラ３０１の撮影画像は特徴点追跡部３０５にも入力される。特徴点追跡部３０５は、カメラ３０１によって取得されるフレーム画像内から選択される局所領域（特徴点）のフレーム間の移動量を求める。例えば、特徴点追跡部３０５は前フレームの局所領域画像とのウインドウマッチングを実行して特徴点のフレーム間の移動量を求める。
【００８８】
特徴点追跡部３０５で得られる特徴点追跡結果は、特徴点トラッキングデータとして特徴点軌跡（トラッキング）情報記憶部３０７に記録される。
【００８９】
図９のカメラ位置姿勢記憶部３０６と特徴点軌跡（トラッキング）情報記憶部３０７は、それぞれカメラ位置姿勢検出部３０４の検出したカメラ位置姿勢情報と、特徴点追跡部３０５の検出した特徴点軌跡情報について、全フレームの出力結果を保持している。なお、それぞれのデータのフレーム時間の同期はとれているものとする。
【００９０】
３次元情報生成部３０８は、カメラ位置姿勢記憶部３０６と特徴点軌跡（トラッキング）情報記憶部３０７に格納されたカメラ位置姿勢情報および特徴点軌跡（トラッキング）情報を入力し、前述のサブマップ対応の状態データを適用したＳＬＡＭ処理を実行して、カメラの位置姿勢の推定（ローカリゼーション）と特徴点の３次元位置の推定（マッピング）を実行する。この処理は、サブマップ対応の状態データの更新処理、すなわち先に説明したカルマンフィルタや拡張カルマンフィルタを適用した更新処理が含まれる。サブマップ対応の状態データは、状態データ記憶部３０９（ＤＢ３：データベース３）に格納され、随時更新される。
【００９１】
なお、図９に示す構成において、各処理ブロックは、固有のハードウェアとして設定してもよいし、例えばＰＣなどの情報処理装置において所定のプログラムを実行させて各処理ブロックに対応する処理を実行させる構成としてもよい。例えば後述する図１０に示す処理シーケンスを実行させるプログラムをメモリに格納し、プログラム実行部としてのＣＰＵを備えた制御部においてプログラムを実行させることで処理を行うことが可能である。
【００９２】
本発明の情報処理装置の実行する処理シーケンスについて、図１０に示すフローチャートを参照して説明する。
まず、処理の全体概要について説明し、その後、各ステップの処理の詳細について説明する。なお、図１０に示すフローチャートのステップＳ１０１〜Ｓ１０７は、１つの画像フレームの入力ごとに実行する処理である。ステップＳ１０８における全体最適化処理の実行単位としてのフレーム数を［Ｌ］フレーム、例えばＬ＝６０フレームとして、処理フレームがＬに至るまでは、ステップＳ１０１〜Ｓ１０７の処理を入力フレームごとに繰り返し実行する。
【００９３】
ステップＳ１０７において、フレーム数がＬとなった場合、ステップＳ１０８において全体最適化処理を実行して、ステップＳ１０９においてカウンタを０にリセットして、ステップＳ１０１に戻り、その後の入力フレームに対する処理を実行する。
【００９４】
なお、図１０に示す記憶部（ＤＢ）３２０は、図９に示す４つの記憶部ＤＢ０〜ＤＢ３に対応する。すなわち、
撮影画像記憶部３０２（ＤＢ０：データベース０）
カメラ位置姿勢記憶部３０６（ＤＢ１：データベース１）
特徴点軌跡（トラッキング）情報記憶部３０７（ＤＢ２：データベース２）
状態データ記憶部３０９（ＤＢ３：データベース３）
これら４つの記憶部を示している。
【００９５】
図１０に示すフローチャートの処理の全体概要について説明する。
ステップＳ１０１：注目サブマップを選択する。
ステップＳ１０２：注目サブマップに対する「状態データ」を用意する。
ステップＳ１０３：初期化処理（最初の処理フレームのみ）。
ステップＳ１０４：注目サブマップの状態データを適用した状態データ更新処理、例えばＥＫＦ ＳＬＡＭによる状態データ更新処理を実行する。なお、本実施例ではＥＫＦ（拡張カルマンフィルタ）を適用した処理例について説明するが、カルマンフィルタを適用した処理として実行することも可能である。
ステップＳ１０５：必要に応じて注目サブマップの切り分け処理を行う。予め設定した条件、例えばサブマップに含まれる特徴点が既定上限数を越えた場合などに注目サブマップを２つのサブマップに分割する。
ステップＳ１０６：処理回数（処理フレーム数）のカウント値の更新処理（＋１）を行う。
ステップＳ１０７：処理回数（処理フレーム数）が既定処理回数Ｌを超えたか否かを判定する。
ステップＳ１０８：全体最適化処理を行う。
ステップＳ１０９：カウント値を０にリセットする。
このような処理を繰り返し実行する。
【００９６】
なお、ステップＳ１０３の処理は、最初の処理フレームのみに対する処理であり、最初の処理フレームではＳＬＡＭ処理に必要となる初期化処理を行う。なお、初期化処理において生成する最初のサブマップは、「サブマップＭ１」とする。
また、予め設定したフレーム数、Ｌフレームに１回の割合で全体最適化（＝「バンドル調整処理」）を行う。これは、サブマップ同士の位置あわせを行い、重複した領域にある特徴点の位置が収束するようにする処理である。
【００９７】
以下、図１０に示すフローチャートの各ステップの処理の詳細について説明する。
（ステップＳ１０１）注目サブマップの選択処理について
まず、図１０に示すフローチャートのステップＳ１０１における注目サブマップ選択処理の詳細シーケンスについて、図１１に示すフローチャートを参照して説明する。
【００９８】
まずステップＳ１５１において、３次元マップの生成対象となる処理対象領域の少なくとも一部を含むサブマップを設定する。なお、初期的には、１つのサブマップのみを設定してもよいし、処理対象領域を複数領域に区分して複数のサブマップ領域を設定してもよい。
【００９９】
なお、３次元マップの生成対象となる領域は、図９に示すカメラ３０１における撮影画像によってカバーされる領域となる。３次元マップの生成処理は、カメラ３０１の撮影画像をリアルタイムで順次入力して実行してもよいし、撮影済み画像を記憶した記憶部から取得して実行してもよい。
【０１００】
サブマップの設定態様は様々な態様が可能である。例えば全体マップが把握できている場合は、初期的に全体マップを１つのサブマップとしてもよいし、予め２以上の任意数のサブマップを設定してもよい。複数のサブマップを設定する場合は、カメラの移動距離や撮影フレーム数、撮影時間などの様々な基準に従って全体マップを分割することができる。
【０１０１】
カメラ３０１の撮影画像をリアルタイムで順次入力して処理を行う場合は、順次入力する画像フレームによって取得される領域によって１つのサブマップが設定されることになる。この場合、初期的には１つのサブマップのみとなり、画像フレームの入力に応じてサブマップ領域が次第に大きくなる。大きくなったサブマップは、ステップＳ１０５のマップ切り分け処理によって、２つのサブマップに分割されることになる。この処理については後段で詳細に説明する。
【０１０２】
図１１のフローのステップＳ１５２では、ステップＳ１５１で設定したサブマップ毎にカメラに写っている特徴点をスキャンする。カメラ位置姿勢は、初期設定情報、または直前フレームでのカルマンフィルタはまたはＥＫＦを用いたＳＬＡＭによる推定結果を適用し、ピンホールカメラモデル（図３、図４参照）に従ってスキャンする。
【０１０３】
次に、ステップＳ１５３において、スキャンした特徴点がどのサブマップに属しているかチェックし、特徴点が一番多く属しているサブマップを注目サブマップとする。もし特徴点が一番多く属しているサブマップが複数ある場合は、番号（ＩＤ）が大きいサブマップを選択する。以上で、注目サブマップの選択が完了する。
【０１０４】
（ステップＳ１０２）注目サブマップに対する「状態データ」を用意する処理について、
次に、図１０に示すフローチャートのステップＳ１０２における注目サブマップに対する「状態データ」を用意する処理について図１２に示すフローチャートを参照して説明する。
【０１０５】
まず、ステップＳ２０１において、現在の注目サブマップ識別子（ＩＤ）を保存する。これは、例えば、現在の処理フレームの１フレーム前の処理フレームに対応する注目サブマップ識別子（ＩＤ（番号））を把握するために行う。
【０１０６】
ステップＳ２０２では、１フレーム前の注目サブマップと現フレームの注目サブマップが異なるか否かを判定し、異なる場合はステップＳ２０３において、１フレーム前の注目サブマップの「状態データ」のカメラ情報を現フレームの注目サブマップの「状態データ」のカメラ情報に上書きする。
【０１０７】
なお、「状態データ」は、先に図２を参照して説明したように、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
これらの状態データは、サブマップ対応の状態データとして設定され、特徴点に関する情報は各サブマップに含まれる特徴点情報のみが含まれている。
【０１０８】
ステップＳ２０３では、サブマップ対応の状態データに含まれるカメラ情報、すなわち、サブマップ対応の状態データに含まれるカメラの位置、姿勢、速度、角速度に関する情報については、新たなサブマップに対する処理を行う場合、１フレーム前の異なるサブマップ対応の状態データを利用する。
【０１０９】
図１３にサブマップ対応の状態データの例を示す。この状態データにおけるカメラ情報は、（ａ）平均ベクトルのデータ部分３２１と、（ｂ）分散共分散行列のデータ部分３３１の各データ部分である。これらのデータ部分については、１フレーム前の異なるサブマップ対応の状態データをそのままコピーして利用する。
【０１１０】
さらに、ステップＳ２０３では、状態データ中の分散共分散行列に含まれるカメラと特徴点の共分散の値を全て０にする。これは、前のサブマップで求めたカメラ位置は、前のサブマップ内の特徴点との関係から求めたものであり、現在のサブマップの特徴点とは関係ないため、データをリセットするものである。
【０１１１】
図１３に示す「状態データ」の（ｂ）分散共分散行列のデータ部分３３２，３３３の共分散の値を全て０にする。
以上でＥＫＦ ＳＬＡＭ用の「状態データ」の前処理が完了する。
【０１１２】
（ステップＳ１０４）注目サブマップの状態データを適用した状態データ更新処理について、
ステップＳ１０４の注目サブマップの状態データを適用した状態データ更新処理は、例えばカルマンフィルタ、あるいは拡張カルマンフィルタを適用した状態データの更新処理として行われる。注目サブマップ対応の状態データを、入力フレームから得られる情報、すなわち、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報を利用して、更新する処理を行う。
【０１１３】
拡張カルマンフィルタを適用した処理、すなわちＥＫＦベースのＳＬＡＭについては、例えば、［"Ｒｅａｌ−Ｔｉｍｅ Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｓａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ ｗｉｔｈ ａ Ｓｉｎｇｌｅ Ｃａｍｅｒａ"，Ａｎｄｒｅｗ Ｊ． Ｄａｖｉｓｏｎ， ＩＣＣＶ ２００３］、あるいは、本出願人が先に出願した［特開２００８−３０４２６９号公報］に記載されており、この記載方法を適用することが可能である。ただし、本願において適用する状態データ、すなわち更新処理対象とするのはサブマップ対応の状態データである。サブマップ対応の状態データには、サブマップに含まれる特徴点以外の特徴点についての位置情報、相関情報は含まれず、データ量が少ないため、極めて高速な計算処理が可能となる。
【０１１４】
なお、サブマップ対応の状態データの更新処理に適用するために入力した画像フレームに、状態データに登録されていない新たな特徴点が検出された場合は、その新規特徴点を状態データ中の変数として追加する処理を行う。
【０１１５】
（ステップＳ１０５）注目サブマップの切り分け処理について
次に、図１０に示すフローチャートのステップＳ１０５において実行する注目サブマップの切り分け処理について説明する。
【０１１６】
注目サブマップの切り分け処理は必要に応じて実行される。現在処理対象としている注目サブマップは、予め設定した条件に応じて分割する。
以下、具体例として以下の２つの分割処理例について説明する。
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理例
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理例
【０１１７】
まず、上記（ａ）の処理例、すなわち、
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理例について説明する。
【０１１８】
この分割処理の全体シーケンスを図１４に示すフローチャートを参照して説明する。
最初に、ステップＳ３０１において、注目サブマップの特徴点の総数をカウントする。
次に、ステップＳ３０２において、カウントした特徴点の総数が、予め設定した既定上限数［Ｎ］以上であるか否かを判定する。
【０１１９】
カウントした特徴点の総数が、予め設定した規定上限数［Ｎ］以上である場合は、ステップＳ３０３に進み、現在処理対象としている注目サブマップの分割処理を行う。Ｎ点未満であれば分割処理をしないで終了する。
【０１２０】
なお、予め設定する既定上限数［Ｎ］は、カメラなどの状況に応じて設定する。本例では、既定上限数：Ｎ＝１２５とする。
【０１２１】
図１４のフローにおけるステップＳ３０３の「注目サブマップの分割処理」の詳細シーケンスについて、図１５に示すフローチャートを参照して説明する。サブマップ分割処理は、図９に示す情報処理装置３００のサブマップ設定部３０３において実行される。なお、分割処理対象とするサブマップは、図１０に示すフローの最初のステップＳ１０１において選択した注目サブマップである。
【０１２２】
まず、ステップＳ４５１において、処理対象としているサブマップ、すなわち注目サブマップが、他のサブマップとの重なり領域の有無を判定する。重なり領域を持たない場合は、ステップＳ４５２に進み、重なり領域を持つ場合は、ステップＳ４５３に進む。
【０１２３】
注目サブマップが、他のサブマップとの重なり領域を持たない場合は、ステップＳ４５２に進み、注目サブマップの特徴点群の重心を求め，重心から最も遠い特徴点を点Ｇとする。
また、注目サブマップが、他のサブマップとの重なり領域を持つ場合は、ステップＳ４５３に進み、注目サブマップの特徴点で、他のサブマップに属する点を選び、選択した点群の重心を点Ｇとする。
これらの処理を実行する。この処理について図１６を参照して説明する。
【０１２４】
図１６には、
（ａ）サブマップ重複領域がない場合のサブマップ分割処理（Ｓ４５２の処理）
（ｂ）サブマップ重複領域がある場合のサブマップ分割処理（Ｓ４５３の処理）
これらの各処理の具体例を示している。分割処理対象としている注目サブマップはサブマップＭ４である。
【０１２５】
図１６（ａ）に示すように、サブマップ重複領域がない場合のサブマップ分割処理（Ｓ４５２の処理）は、注目サブマップＭ４の特徴点群の重心Ｐ１を求め，重心Ｐ１から最も遠い特徴点を点Ｇ１とする処理である。
一方、サブマップ重複領域がある場合のサブマップ分割処理（Ｓ４５３の処理）は図１６（ｂ）に示すように、注目サブマップＭ４の特徴点で、他の重複サブマップＭ３に属する特徴点を選び、選択した特徴点群の重心を点Ｇ２とする処理である。
【０１２６】
これらの処理の後、ステップＳ４５４に進む。
ステップＳ４５４では、注目サブマップに属する特徴点各々の点Ｇからの距離を算出し、距離の遠い順に並べて、以下の各サブマップ対応の特徴点設定処理を行う。
１番〜Ｍ番：新規サブマップの特徴点
Ｍ＋１番〜２Ｍ番：注目サブマップと新規サブマップの重複領域の特徴点
上記の処理によって、新規サブマップを設定する。
なお、Ｍは、注目サブマッフ内の特徴点数＝Ｎとしたとき、例えば、
Ｍ＝Ｎ×０．２
の設定とする。
Ｎ＝１２５の場合、Ｍ＝２５であり、新規追加サブマップにはサブマップ分割時点で、５０の特徴点が設定される。５０の特徴点中、２５の特徴点が新規追加サブマップのみに属する特徴点であり、残り２５の特徴点が新規追加サブマップと注目サブマップの重複領域に属する特徴点となる。
【０１２７】
この処理の具体例は、図１６に示す通りである。（ａ）サブマップ重複領域がない場合のサブマップ分割処理では、図１６（ａ）に示すように、新規追加サブマップＭ５が設定され、（ｂ）サブマップ重複領域がある場合のサブマップ分割処理では、図１６（ｂ）に示すように、新規追加サブマップＭ５が設定される。
【０１２８】
次に、ステップＳ４５５において、分割前の注目サブマップの「状態データ」を分割後の新規追加サブマップの「状態データ」とするコピー処理を行う。
最後に、ステップＳ４５６において、注目サブマップと新規追加サブマップの各「状態データ」から各サブマップに属さない特徴点」の情報を削除して、各サブマップの状態データとする処理を行う。
【０１２９】
図１７以下を参照してこれらの処理の具体例について説明する。図１７は、分割処理対象となる注目サブマップＭ４に対応する状態データのコピー処理について説明する図である。
【０１３０】
図１７（ａ）は、分割処理前のサブマップＭ４とサブマップＭ４対応状態データ３５１を示している。状態データは、先に図２を参照して説明したように、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
【０１３１】
ステップＳ４５５の処理は、図１７（ａ）に示すサブマップＭ４対応状態データ３５１を図１７（ｂ）に示すように、サブマップＭ４対応状態データ３５２、サブマップＭ５対応状態データ３５３として設定する処理である。これらは仮のサブマップ対応状態データである。
【０１３２】
次に、これらのサブマップＭ４対応状態データ３５２と、サブマップＭ５対応状態データ３５３から、それぞれのサブマップに属さない特徴点に関する情報を削除する処理を行う。
【０１３３】
図１８は、状態データ中の平均ベクトルからの情報削除処理、図１９は、分散共分散行列からの情報削除処理の具体例を示している。このように、これらのサブマップＭ４対応状態データ３５２と、サブマップＭ５対応状態データ３５３から、それぞれのサブマップに属さない特徴点に関する情報を削除する処理を行って、最終的な各サブマップの状態データを更新および設定する処理を行なう。
【０１３４】
この処理によって、図１４に示すフローチャートにおけるステップＳ３０３の注目サブマップの分割処理が終了する。なお、図１４のフローに示すサブマップ分割処理は、前述のように、図１０に示すフローチャートのステップＳ１０５において実行する注目サブマップの切り分け処理の一態様である。
【０１３５】
前述したように図１０に示すフローチャートのステップＳ１０５において実行する注目サブマップの切り分け処理は、必要に応じて実行される処理であり、例えば以下のいずれかの処理態様で行われる。
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理
図１４〜図１９を参照して説明した処理は、上記の（ａ）の処理に対応する。
【０１３６】
次に、図２０以下を参照して、上記の（ｂ）のサブマップ分割処理例について説明する。すなわち、
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理
である。
【０１３７】
このサブマップ分割処理の全体シーケンスを説明するフローチャートを図２０に示す。
図２０に示すフローチャートを参照して、３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割し手サブマップ分割を行う処理について説明する。
【０１３８】
なお、図２０に示すフローの処理の前処理として、ステップＳ５００の処理として、処理対象としている３次元空間に以下のパラメータを設定しておく。
＊ベクトル［ｋ］
＊ベクトル［ｒ１］
＊ベクトル［ｒ２］
＊分割中心［Ｏ］
＊距離［ｄ］，［ｅ］
【０１３９】
図２１を参照してこれらのパラメータについて説明する。
図２１は３次元マップを生成する対象となる空間（部屋）を示している。これらの空間に設定される多数の特徴点に対する３次元位置を算出する。
ベクトル［ｋ］は２次元マップへ射影する方向を表しており、状況に応じて設定する。例えば、図２１に示すような部屋に関するマップを扱うとした場合、図２１に示すように、床面に垂直なベクトルをベクトル［ｋ］と設定するのが適していると考える。
【０１４０】
ベクトル［ｒ１］と、ベクトル［ｒ２］は、図２１に示すように、３次元空間上の２次元マップの座標軸ベクトルを表す。ベクトル［ｒ１］と、ベクトル［ｒ２］は、以下の条件を満足するように設定する。
（条件１）ベクトル［ｋ］と、射影変換によって生成する２次元マップの座標軸ｒ１，ｒ２同士は直交する。
（条件２）２次元マップの座標軸［ｒ１］，［ｒ２］同士は直交する。
これらの条件を満たすように状況にあわせて設定する。
なお、これらのベクトル，［ｋ］，［ｒ１］，［ｒ２］は３×１の行列として表現される。
【０１４１】
分割中心［Ｏ］は、図２２に示すように、射影して生成した２次元マップにおける分割処理の基準点である。
距離［ｄ］は、図２２に示すように、射影して生成した２次元マップにおいて設定するサブマップ間隔である。
距離［ｅ］は、図２２に示すように、生成した２次元マップにおいて設定するサブマップの重複領域の重複幅である。
【０１４２】
図２０のステップＳ５０１では、３次元空間のマップに属する全特徴点を２次元マップに射影する。射影変換は、図２０に示すように、以下の式（数式Ａ）を適用することができる。
【０１４３】
【数８】
・・・・・（数式Ａ）
【０１４４】
但し、Ｘ，Ｙ，Ｚは特徴点の３次元空間上の位置、ｕ，ｖは特徴点の２次元マップ上の位置、λは任意の数を表す。
【０１４５】
次に、ステップＳ５０２において２次元マップに射影された特徴点マップを分割してサブマップを生成する。マップ分割処理は、図２２に示すように、分割中心［Ｏ］を中心に、距離［ｄ］，［ｅ］を利用して等間隔に分割する。
【０１４６】
図２２には、太線で示す６つのサブマップＭ１〜Ｍ６を示している。分割中心［Ｏ］を分割基準点として、射影して生成した２次元マップにおいて、サブマップ間隔を［ｄ］、サブマップ重複領域の重複幅を［ｅ］として６つのサブマップが設定される。この処理が、３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理である。なお、さらに、新しくサブマップを作る必要がある場合は、今まであるサブマップの識別子と異なる識別子、例えば番号の最大値に１を足したものを新しく割り当てて新規追加サブマップの識別子とする。
【０１４７】
このように、図１０のステップＳ１０５のサブマップ切り分け処理は、
図１４〜図１９を参照して説明した
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理
あるいは、図２０〜図２２を参照して説明した
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理
上記処理のいずれかを適用して実行される。なお、（ａ），（ｂ）の処理を組み合わせて実行する構成としてもよい。
【０１４８】
図１０のフローチャートの説明に戻る。ステップＳ１０５の処理、すなわち必要に応じたマップ切り分け処理が終了すると、ステップＳ１０６において、入力フレームのカウンタを更新する。すなわち、１つの処理フレームごとにカウンタを［＋１］とするカウンタ更新処理を実行する。
【０１４９】
次に、ステップＳ１０７において、予め設定したステップＳ１０８における全体最適化処理の実行単位としてのフレーム数［Ｌ］とカウンタ値を比較する。カウンタ値がＬ未満である場合は、処理フレームがＬにいたるまでは、ステップＳ１０１〜Ｓ１０７の処理を入力フレームごとに繰り返し実行する。Ｌは例えば６０フレームである。
【０１５０】
ステップＳ１０７において、フレーム数がＬとなった場合、ステップＳ１０８において全体最適化処理を実行して、ステップＳ１０９においてカウンタを０にリセットして、ステップＳ１０１に戻り、その後の入力フレームに対する処理を実行する。
【０１５１】
ステップＳ１０８の全体最適化処理について、図２３、図２４を参照して説明する。ステップＳ１０８の全体最適化処理の詳細シーケンスについて、図２３に示すフローチャートを参照して説明する。
【０１５２】
全体最適化処理は、バンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）を用いる。図２３に示すフローの説明の前にバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）の概要について図２４を参照して説明する。
【０１５３】
バンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）は、例えば図２４に示すように、３つの異なる位置からの撮影画像４１１〜４１３を利用し、その撮影画像中から検出した特徴点４２１〜４２３を利用して、特徴点の３次元位置を求める。複数の画像に含まれる特徴点中、対応する特徴点の位置は図２４に示す特徴点４２１のように、各画像の撮影ポイントから特徴点４２１を結ぶ線（Ｂｕｎｄｌｅ）が１つの特徴点４２１位置において交わるはずである。
【０１５４】
しかし、カメラの位置や特徴点の画像内の位置情報などは必ずしも正確に算出されず様々な要因による誤差が含まれる。従ってこのような誤差を取り除く必要がある。具体的には１つの対応する特徴点の３次元位置とカメラ位置とを結ぶ線が、その１つの特徴点において交わるように補正する。すなわち算出済みのカメラ位置や特徴点位置を修正する処理が必要となる。この処理がバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）として実行され、この調整処理によって修正された特徴点位置情報やカメラ位置情報を利用してより精度の高い３次元情報を生成することが可能となる。このバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）は従来から知られる一般的な方法で、例えば、文献［"Ｔｈｅ Ｄｅｓｉｇｎ ａｎｄ Ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｆ ａ Ｇｅｎｅｒｉｃ Ｓｐａｒｓｅ Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ Ｓｏｆｔｗａｒｅ Ｐａｃｋａｇｅ Ｂａｓｅｄ ｏｎ ｔｈｅ Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ Ａｌｇｏｒｉｔｈｍ"，ｂｙ Ｍ．Ｉ．Ａ．Ｌｏｕｒａｋｉｓ ａｎｄ Ａ．Ａ．Ａｒｇｙｒｏｓ，Ａｕｇ．（２００４）．］に記載された手法を適用できる。
【０１５５】
図２３に示すフローチャートの各ステップの処理について説明する。なお、この処理は、図９に示す３次元情報生成部３０８において行われる処理である。この処理における入力は以下の情報である。
＊カメラ位置姿勢の履歴
＊最新フレームでの特徴点位置全て
＊特徴点トラッキング結果（画像内の特徴点の位置。カメラ位置の履歴に対応する画像全て）
【０１５６】
なお、カメラ位置の履歴と特徴点トラッキング結果は、毎フレームごとに実行するＥＫＦ ＳＬＡＭ処理（図１０のフローのステップＳ１０４の処理）の結果から取得する。
【０１５７】
まず、ステップＳ６０１において、最新フレームの特徴点位置を、特徴点が属するサブマップの状態データから取得する。特徴点位置は、所属するサブマップの状態データの平均ベクトル（図２（ａ））から取得する。複数のサブマップに属する特徴点（図７のｄ、ｅ）は、所属するサブマップ各々の状態データの平均ベクトルから、所属するサブマップ数の位置情報を取得し、取得した特徴点位置の平均値をとる。この特徴点位置を初期値とする。
【０１５８】
ステップＳ６０２では、最新のカメラ位置姿勢情報を取得する。処理対象としているサブマップの状態データから取得することができる。
【０１５９】
ステップＳ６０３では、図２４を参照して説明したバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）による全体最適化処理を行う。すなわち、同じ特徴点が撮影されている複数の画像フレームを取得して、これら複数の画像フレームにおける共通する特徴点の位置を一致させるバンドル調整処理による全体最適化を実行する。
【０１６０】
この処理によって、特徴点位置の微調整が行われることになり、サブマップの状態データも、この微調整後の特徴点位置情報に基づいて更新することが必要となる。具体的には、ステップＳ６０４において、微調整された各特徴点が所属しているサブマップの特徴点位置を上書きする。複数のサブマップに属する特徴点（図７のｄ、ｅ）は、所属するサブマップ各々の状態データの平均ベクトルに（図２（ａ））に、最適化された位置情報を上書きする。
さらに、ステップＳ６０５において、最新のカメラ位置・姿勢を上書きする。
以上で、図１０に示すフローのステップＳ１０８の全体最適化処理が完了する。
この全体最適化処理は計算負担が重いため、図１０のフローに示すようにＬフレームに１回の割合で処理する。
【０１６１】
以上で本発明の３次元マップの生成に必要となる特徴点位置が算出される。
上述した説明から理解されるように、本発明の処理では、３次元マップの生成領域全体を複数のサブマップに区分し、各サブマップ対応の状態データを設定して、このサブマップ対応の状態データに対する更新処理をカルマンフィルタや拡張カルマンフィルタ（ＥＫＦ）を用いて実行する。この構成により、状態データ更新に伴う計算量が大幅に削減でき、効率的なＳＬＡＭ処理、すなわち、特徴点の３次元位置解析、カメラの位置姿勢算出、さらに３次元環境マップの作成を行うことができる。
【０１６２】
［４．サブマップの設定手法の追加説明］
上述した本発明の処理において、サブマップの設定に際して、隣接するサブマップは共通する特徴点を持つ重複領域を持つように設定することが好ましいと説明した。この理由について、図２５、図２６を参照して説明する。
【０１６３】
隣接するサブマップは共通する特徴点を持つ重複領域を持つように設定することが好ましい理由は、マップの全体最適化（バンドル調整処理）をより正確に行うためである。
【０１６４】
図２５はサブマップ同士が重複するように配置した例である。星印は特徴点、カメラは図に示す移動軌跡に沿って左から右に移動して、１ａ，１ｂ，１ｃ／２ａ，１ｄ／２ｂ，２ｃ，２ｄの各地点で画像を撮影しているものとする。
１ａでは、サブマップＭ１の領域の画像を撮影している。
１ｂ，１ｃ，２ａ，２ｂでは、サブマップＭ１とサブマップＭ２の重複領域の画像を撮影している。
２ｃでは、サブマップＭ２の領域の画像を撮影している。
１ａ，１ｂ，１ｃ，２ａ，２ｂ，２ｃの各カメラ位置と星印で示す特徴点を結ぶ直線は、特徴点のバンドル（トラッキング結果）を表している。
【０１６５】
この設定で、特徴点４５１，４５２のバンドル調整を行う場合について考察する。例えば、サブマップＭ１とサブマップＭ２に属する特徴点４５１に注目する。特徴点４５１を結ぶバンドル情報（トラッキング結果）は３つあり、それぞれ、カメラ１ｂ、カメラ１ｃ、カメラ２ａとを結んでいる。特徴点４５１と、カメラ１ｂ、カメラ１ｃ、カメラ２ａとを結ぶバンドル（トラッキング情報）は、前述のとおりＥＫＦＳＬＡＭ（図１０のフローのステップＳ１０４）が生成する。ただし、ＥＫＦＳＬＡＭには１つのサブマップのみ入力される。
カメラが１ｂの位置にある時と１ｃの位置にある時は、注目サブマップはサブマップＭ１であるので、特徴点４５１と、カメラ１ｂ、カメラ１ｃとを結ぶバンドル（トラッキング情報）は、サブマップＭ１からＥＫＦＳＬＡＭによって生成される。
一方、カメラが２ａの位置にある時は、注目サブマップはサブマップＭ２であるので、特徴点４５１と、カメラ２ａとを結ぶバンドル（トラッキング情報）は、サブマップＭ２からＥＫＦＳＬＡＭによって生成される。
特徴点４５１とカメラ１ｂ、カメラ１ｃ、カメラ２ａとを結ぶそれぞれのバンドルは、異なるサブマップから生成されているが、特徴点４５１はサブマップＭ１とサブマップＭ２に属していることがわかっているため、バンドルは特徴点４５１で交わることがわかる。
バンドル調整は、カメラ位置１ｂ，１ｃ，２ａの３つ異なるカメラ位置でのバンドル情報が適用でき、先に図２４を参照しいて説明したバンドル調整が可能となる。
【０１６６】
また、サブマップＭ１とサブマップＭ２に属する特徴点４５２に注目する。特徴点４５２を結ぶバンドル情報（トラッキング結果）は３つあり、それぞれ、カメラ１ｃ、カメラ２ａ、カメラ２ｂとを結んでいる。特徴点４５２と、カメラ１ｃ、カメラ２ａ、カメラ２ｂとを結ぶバンドル（トラッキング情報）は、前述のとおりＥＫＦＳＬＡＭ（図１０のフローのステップＳ１０４）が生成する。ただし、ＥＫＦＳＬＡＭには１つのサブマップのみ入力される。
カメラが１ｃの位置にある時は、注目サブマップはサブマップＭ１であるので、特徴点４５２と、カメラ１ｃとを結ぶバンドル（トラッキング情報）は、サブマップＭ１からＥＫＦＳＬＡＭによって生成される。
一方、カメラが２ａの位置にある時と２ｂの位置にあるは、注目サブマップはサブマップＭ２であるので、特徴点４５２と、カメラ２ａ、カメラ２ｂとを結ぶバンドル（トラッキング情報）は、サブマップＭ２からＥＫＦＳＬＡＭによって生成される。
特徴点４５２とカメラ１ｃ、カメラ２ａ、カメラ２ｂとを結ぶそれぞれのバンドルは、異なるサブマップから生成されているが、特徴点４５２はサブマップＭ１とサブマップＭ２に属していることがわかっているため、バンドルは特徴点４５２で交わることがわかる。
バンドル調整は、カメラ位置１ｃ，２ａ，２ｂの３つのカメラ位置でのバンドル情報が適用でき、先に図２４を参照しいて説明したバンドル調整が可能となる。
【０１６７】
これに対して、図２５と同じ特徴点配置において、サブマップ同士が重ならないようにサブマップを設定した場合の例を図２６に示す。図２５の例と比較すると、図２６の例では、特徴点４５１とカメラ２ａとを結ぶバンドル、特徴点４５２とカメラ１ｃを結ぶバンドルがない。
特徴点４５１とカメラ２ａとを結ぶバンドルがない理由を説明する。バンドル（トラッキング結果）を生成するＥＫＦＳＬＡＭには１つのサブマップのみ入力される。カメラが２ａの場所にあるとき、注目サブマップはＭ２である。このとき、サブマップＭ１にしか属さない特徴点４５１はＥＫＦＳＬＡＭには入力されない。従って、カメラ２ａと特徴点４５１とのバンドル（トラッキング結果）はＥＫＦＳＬＡＭで生成されないため、図２６の例にはない。
同様ではあるが、特徴点４５２とカメラ１ｃとを結ぶバンドルがない理由を説明する。バンドル（トラッキング結果）を生成するＥＫＦＳＬＡＭには１つのサブマップのみ入力される。カメラが１ｃの場所にあるとき、注目サブマップはＭ２である。このとき、サブマップＭ２にしか属さない特徴点４５２はＥＫＦＳＬＡＭには入力されない。従って、カメラ１ｃと特徴点４５２とのバンドル（トラッキング結果）はＥＫＦＳＬＡＭで生成されないため、図２６の例にはない。
図２６において、サブマップＭ１の特徴点４５１のバンドル調整処理を行おうとした場合、カメラ位置１ｂ，１ｃの２つのサブマップＭ１に対応する異なるカメラ位置でのバンドル情報のみしか利用できない。
【０１６８】
同様に、サブマップＭ２の特徴点４５２のバンドル調整処理を行おうとした場合、カメラ位置２ａ，２ｂの２つのサブマップＭ２に対応する異なるカメラ位置でのバンドル情報のみしか利用できない。
図２６では、サブマップＭ１とサブマップＭ２との関連性を表すバンドルが存在しない。そのため、図２４の全体最適化を行うと、サブマップＭ１、サブマップＭ２それぞれ独立に最適化され、サブマップの境界部分（例えば、図２６の特徴点４５１、特徴点４５２、カメラ１ｃ、カメラ２ａそれぞれの間の位置姿勢関係）はバンドル調整（全体最適化）されない問題が発生する。
【０１６９】
結果として、サブマップに重複領域を設定しない場合、サブマップの端部にある特徴点のバンドル調整が出来ない場合が発生する。この場合には、特徴点の３次元位置の精度が低下する可能性がある。このような理由から、サブマップの設定に際しては、隣接するサブマップは共通する特徴点を持つ重複領域を持つように設定することが好ましい。
【０１７０】
以上、特定の実施例を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施例の修正や代用を成し得ることは自明である。すなわち、例示という形態で本発明を開示してきたのであり、限定的に解釈されるべきではない。本発明の要旨を判断するためには、特許請求の範囲の欄を参酌すべきである。
【０１７１】
なお、明細書中において説明した一連の処理はハードウェア、またはソフトウェア、あるいは両者の複合構成によって実行することが可能である。ソフトウェアによる処理を実行する場合は、処理シーケンスを記録したプログラムを、専用のハードウェアに組み込まれたコンピュータ内のメモリにインストールして実行させるか、あるいは、各種処理が実行可能な汎用コンピュータにプログラムをインストールして実行させることが可能である。
【０１７２】
例えば、プログラムは記録媒体としてのハードディスクやＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）に予め記録しておくことができる。あるいは、プログラムはフレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ），ＭＯ（Ｍａｇｎｅｔｏ ｏｐｔｉｃａｌ）ディスク，ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）、磁気ディスク、半導体メモリなどのリムーバブル記録媒体に、一時的あるいは永続的に格納（記録）しておくことができる。このようなリムーバブル記録媒体は、いわゆるパッケージソフトウエアとして提供することができる。
【０１７３】
なお、プログラムは、上述したようなリムーバブル記録媒体からコンピュータにインストールする他、ダウンロードサイトから、コンピュータに無線転送や、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネットといったネットワークを介して、コンピュータに有線で転送し、コンピュータでは、そのようにして転送されてくるプログラムを受信し、内蔵するハードディスク等の記録媒体にインストールすることができる。
【０１７４】
なお、明細書に記載された各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。また、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。
【産業上の利用可能性】
【０１７５】
以上、説明したように、本発明の一実施例の構成によれば、計算量を削減し効率的な３次元位置情報の生成を行う構成が実現される。カメラによる撮影画像を観測データとして入力し、特徴点位置情報を含む多次元正規分布データからなる状態データを更新することで特徴点の３次元位置を求める例えばＳＬＡＭ処理において、３次元情報の生成対象領域を区分したサブマップを設定する。さらにサブマップに属しない特徴点に関するデータを含まないサブマップ対応の状態データを生成して、観測データに基づいてサブマップ対応状態データを更新する。本構成によれば状態データのデータ量が削減され、状態データ更新に伴う計算量を減少させることが可能であり、効率的な処理が実現される。
【符号の説明】
【０１７６】
１０１ カメラ
１０２ 撮影画像
１０３ 特徴点
１０４ トラッキング（軌跡）情報
１２０ オブジェクト
１２１ オブジェクト３次元位置
１３０ カメラ撮影平面画像
１３１ オブジェクト
１３２ オブジェクト画素位置
２１１，２２１ サブマップ
２１２，２２２ サブマップ対応状態データ
３００ 情報処理装置
３０１ カメラ
３０２ 撮影画像記憶部
３０３ サブマップ設定部
３０４ カメラ位置姿勢検出部
３０５ 特徴点追跡部
３０６ カメラ位置姿勢記憶部
３０７ 特徴点軌跡（トラッキング）情報記憶部
３０８ ３次元情報生成部
３０９ 状態データ記憶部
３２０ 記憶部
３５１〜３５３ サブマップ対応状態データ
４１１〜４１３ 撮影画像
４２１〜４２３ 特徴点
４５１，４５２ 特徴点
【技術分野】
【０００１】
本発明は、情報処理装置、および情報処理方法、並びにプログラムに関する。さらに詳細には、３次元空間の解析処理を実行する情報処理装置、および情報処理方法、並びにプログラムに関する。
【背景技術】
【０００２】
３次元空間の解析の処理手法として、３次元空間をカメラによって撮影し、その撮影画像の解析を行う手法がある。例えばカメラによって撮影された画像から複数の特徴点（ランドマーク（Ｌａｎｄｍａｒｋ）とも呼ばれる）を抽出し、これらの特徴点の３次元位置を推定する処理が行われる。
【０００３】
カメラを移動させながら画像を撮影し、複数の撮影画像に含まれる特徴点の軌跡を解析することで、特徴点の３次元位置を推定するとともに、カメラ（自己）の位置姿勢を推定（ローカリゼーション）することが可能となる。また特徴点の３次元位置情報を用いて周囲の地図（環境地図）を作成（ｍａｐｐｉｎｇ）することができる。このように、カメラ(自己)の位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を並行して実行する処理はＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）と呼ばれる。
【０００４】
ＳＬＡＭの手法の１つとして、ＥＫＦ（拡張カルマンフィルタ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を使用したＥＫＦベースのＳＬＡＭがある。なお、ＥＫＦベースのＳＬＡＭについては、例えば非特許文献１（Ｓ．Ｔｈｒｕｎ，Ｗ．Ｂｕｒｇａｒｄ，ａｎｄ Ｄ．Ｆｏｘ． Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｒｏｂｏｔｉｃｓ． ＭＩＴ Ｐｒｅｓｓ， Ｃａｍｂｒｉｄｇｅ，ＭＡ，２００５）に記載されている。
【０００５】
ＥＫＦベースのＳＬＡＭは、例えばカメラを移動させながら画像を連続的に撮影し、各画像に含まれる特徴点の軌跡（トラッキング情報）を求め、移動ステレオ法でカメラの移動量と特徴点の３次元位置を同時に推定する方法である。この処理には、例えば、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報、
これらの各情報を含む確率分布モデルとしての多次元正規分布データからなる「状態データ」を利用する。カルマンフィルタや拡張カルマンフィルタを用いて「状態データ」の更新処理を行って、特徴点位置、カメラ位置等を推定する。
【０００６】
「状態データ」は、カメラの位置、姿勢、速度、角速度と、各特徴点の位置情報を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。分散共分散行列は、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報、これらの固有状態値の［分散］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散］を含む行列である。なお、具体的なデータ構成については本発明の説明の欄で説明する。
【０００７】
ＥＫＦベースのＳＬＡＭにおける「状態データ」の更新処理では、移動するカメラの撮影画像を利用するが、その結果、「状態データ」には登録されているが、処理対象として入力した最新のカメラ撮影画像に写っていない特徴点情報についても更新対象として処理を行うことになる。すなわち先行して入力した画像に含まれる特徴点が、最新の入力画像には含まれなくなる場合がある。先行して入力した画像に含まれる特徴点に関する情報は「状態データ」に登録される。この結果、「状態データ」に含まれる「カメラに写っている特徴点」と「カメラに写っていない特徴点」との相関情報についてもカルマンフィルタを適用して更新する処理が実行されることになる。
【０００８】
すなわち、マップ作成対象領域が大きくなると特徴点の数も増大する。「状態データ」に多数の特徴点情報を含めた場合、カルマンフィルタや拡張カルマンフィルタを用いた「状態データ」の更新処理における計算量が大きくなる。
【０００９】
「状態データ」には前述のように分散共分散行列が含まれ、この行列に全ての特徴点とカメラの相関情報が保持されることになる。従って特徴点数が増えると分散共分散行列のサイズも大きくなる。カルマンフィルタや拡張カルマンフィルタを用いた「状態データ」の更新処理における計算量は、特徴点の数ｎに対して、計算量がＯ（９ｎ２）のオーダーで増加する。データ更新は、繰り返し行うことが必要であり、その結果、特徴点数の多い広い領域の３次元マップを作成する場合の計算処理の負荷、処理時間が過大になるという問題がある。
【先行技術文献】
【非特許文献】
【００１０】
【非特許文献１】Ｓ．Ｔｈｒｕｎ，Ｗ．Ｂｕｒｇａｒｄ，ａｎｄ Ｄ．Ｆｏｘ． Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｒｏｂｏｔｉｃｓ． ＭＩＴ Ｐｒｅｓｓ， Ｃａｍｂｒｉｄｇｅ，ＭＡ，２００５
【発明の概要】
【発明が解決しようとする課題】
【００１１】
本発明は、例えば上述の問題点に鑑みてなされたものであり、撮影画像解析による特徴点位置推定を行う構成において、処理負荷を軽減し、精度を低下させることなく特徴点の３次元位置を推定し、３次元マップの生成を可能とする情報処理装置、および情報処理方法、並びにプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１２】
本発明の第１の側面は、
空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することで、カメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成部と、
３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定部を有し、
前記３次元情報生成部は、
前記サブマップ設定部の設定したサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行して、カメラの位置、姿勢、または各特徴点の３次元位置を求める情報処理装置にある。
【００１３】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、前記サブマップ対応状態データとして、前記カメラの位置、姿勢と、前記サブマップ内の各特徴点の位置の状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データを、各サブマップに対応させて設定し、設定したサブマップ対応の多次元正規分布データを観測情報を適用して更新する。
【００１４】
さらに、本発明の情報処理装置の一実施態様において、前記分散共分散行列は、カメラの位置、姿勢、各特徴点の位置情報、これらの固有状態値の分散と、前記各状態値の異なる状態値の組み合わせの相関情報に対応する共分散を含む行列である。
【００１５】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、カルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、または拡張カルマンフィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を適用して前記サブマップ対応状態データの更新処理を実行する。
【００１６】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、旧サブマップから新サブマップへ、サブマップを切り換えて処理を行う場合、旧サブマップ対応状態データに含まれるカメラ情報をそのまま利用して新サブマップ対応状態データを生成する。
【００１７】
さらに、本発明の情報処理装置の一実施態様において、前記サブマップ設定部は、サブマップに含まれる特徴点数が予め既定した上限数Ｎ以上となった場合、重複領域を持つ２つのサブマップに分割するサブマップ分割処理を実行し、当該２つのサブマップには少なくとも一つは同一の特徴点を有することを特徴とする。
【００１８】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、サブマップ分割処理において生成した２つの分割サブマップ各々に対応する２つのサブマップ対応状態データを生成する処理を行う構成であり、分割前のサブマップのサブマップ対応状態データから、各分割サブマップに含まれない特徴点情報を各々削除して２つの分割サブマップ各々のサブマップ対応状態データを生成する。
【００１９】
さらに、本発明の情報処理装置の一実施態様において、前記サブマップ設定部は、３次元位置の算出対象となる領域を射影変換して生成した２次元平面を予め既定した大きさで、かつ重複領域を持つ複数のサブマップに分割するサブマップ分割処理を実行する。
【００２０】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、バンドル調整処理による特徴点位置修正処理を実行する。
【００２１】
さらに、本発明の情報処理装置の一実施態様において、前記３次元情報生成部は、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を並行して実行するＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理を行う。
【００２２】
さらに、本発明の第２の側面は、
情報処理装置において実行する情報処理方法であり、
サブマップ設定部が、３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定ステップと、
３次元情報生成部が、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行してカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップである情報処理方法にある。
【００２３】
さらに、本発明の第３の側面は、
情報処理装置において情報処理を実行させるプログラムであり、
サブマップ設定部に、３次元位置の算出対象となる領域を分割したサブマップを設定させるサブマップ設定ステップと、
３次元情報生成部に、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求めさせる３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行させてカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップであるプログラムにある。
【００２４】
なお、本発明のプログラムは、例えば、様々なプログラム・コードを実行可能な画像処理装置やコンピュータ・システムに対して、コンピュータ可読な形式で提供する記憶媒体、通信媒体によって提供可能なプログラムである。このようなプログラムをコンピュータ可読な形式で提供することにより、画像処理装置やコンピュータ・システム上でプログラムに応じた処理が実現される。
【００２５】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施例や添付する図面に基づくより詳細な説明によって明らかになるであろう。なお、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。
【発明の効果】
【００２６】
本発明の一実施例の構成によれば、計算量を削減し効率的な３次元位置情報の生成を行う構成が実現される。カメラによる撮影画像を観測データとして入力し、特徴点位置情報を含む多次元正規分布データからなる状態データを更新することで特徴点の３次元位置を求める例えばＳＬＡＭ処理において、３次元情報の生成対象領域を区分したサブマップを設定する。さらにサブマップに属しない特徴点に関するデータを含まないサブマップ対応の状態データを生成して、観測データに基づいてサブマップ対応状態データを更新する。本構成によれば状態データのデータ量が削減され、状態データ更新に伴う計算量を減少させることが可能であり、効率的な処理が実現される。
【図面の簡単な説明】
【００２７】
【図１】ＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理の概要について説明する図である。
【図２】ＳＬＡＭ処理に適用する状態データの構成について説明する図である。
【図３】ピンホールカメラモデルについて説明する図である。
【図４】ピンホールカメラモデルについて説明する図である。
【図５】ＳＬＡＭ処理に適用する状態データの構成について説明する図である。
【図６】本発明の情報処理装置の処理に適用するサブマップと状態データについて説明する図である。
【図７】本発明の情報処理装置の処理に適用するサブマップと画像フレームとの対応について説明する図である。
【図８】本発明の情報処理装置の処理に適用するサブマップと状態データについて説明する図である。
【図９】本発明の一実施例の情報処理装置の構成例について説明する図である。
【図１０】本発明の一実施例の情報処理装置において実行する処理のシーケンスについて説明するフローチャートを示す図である。
【図１１】図１０に示すフローチャートのステップＳ１０１における注目サブマップ選択処理の詳細シーケンスについて説明するフローチャートを示す図である。
【図１２】図１０に示すフローチャートのステップＳ１０２における注目サブマップに対する「状態データ」を用意する処理について説明するフローチャートを示す図である。
【図１３】サブマップ対応の状態データについて説明する図である。
【図１４】注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理のシーケンスについて説明するフローチャートを示す図である。
【図１５】図１４のフローにおけるステップＳ３０３の「注目サブマップの分割処理」の詳細シーケンスについて説明するフローチャートを示す図である。
【図１６】図１５のフローにおけるステップＳ４５２〜Ｓ４５４のサブマップの分割処理の具体例について説明する図である。
【図１７】図１５のフローにおけるステップＳ４５５〜Ｓ４５６のサブマップ対応の状態データに対する処理の具体例について説明する図である。
【図１８】図１５のフローにおけるステップＳ４５５〜Ｓ４５６のサブマップ対応の状態データに対する処理の具体例について説明する図である。
【図１９】図１５のフローにおけるステップＳ４５５〜Ｓ４５６のサブマップ対応の状態データに対する処理の具体例について説明する図である。
【図２０】３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理の詳細シーケンスについて説明するフローチャートを示す図である。
【図２１】３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理の具体例について説明する図である。
【図２２】３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理の具体例について説明する図である。
【図２３】図１０のフローにおけるステップＳ１０８の全体最適化処理の詳細シーケンスについて説明するフローチャートを示す図である。
【図２４】バンドル調整処理について説明する図である。
【図２５】サブマップの重複領域の必要性について説明する図である。
【図２６】サブマップの重複領域の必要性について説明する図である。
【発明を実施するための形態】
【００２８】
以下、図面を参照しながら本発明の実施形態に係る情報処理装置、および情報処理方法、並びにプログラムの詳細について説明する。説明は以下の項目に従って行う。
１．ＳＬＡＭ処理の概要について
２．サプマップについて
３．情報処理装置の構成例およびサブマップを適用した処理例について
４．サブマップの設定手法の追加説明
【００２９】
［１．ＳＬＡＭ処理の概要について］
本発明の情報処理装置は、カメラによって撮影された画像に含まれる特徴点情報を適用して特徴点の３次元位置を推定する処理を実行する。例えば、カメラ位置の同定（ローカリゼーション）と周囲の地図作成（ｍａｐｐｉｎｇ）処理を並行して実行するＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理を行う。
【００３０】
具体的には、カメラを移動させながら画像を連続的に撮影し、各画像に含まれる特徴点の軌跡（トラッキング情報）を求め、移動ステレオ法でカメラの移動量と特徴点の３次元位置を同時に推定する。
【００３１】
前述したようにＳＬＡＭの手法の１つとして、カルマンフィルタを適用したＳＬＡＭや、ＥＫＦ（拡張カルマンフィルタ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を使用したＥＫＦベースのＳＬＡＭがある。ＳＬＡＭについて図１以下を参照して説明する。図１のシステムは、単眼カメラ１０１から得られる映像を入力し解析を行う。カメラ１０１は、移動しながら画像を連続的に撮影する。ＳＬＡＭは、映像内の特徴点のトラッキングを求め、特徴点の移動量（トラッキング結果）から移動ステレオ法でカメラの移動量と特徴点の３次元位置を同時に推定する。
【００３２】
ＳＬＡＭでは、図２に示す状態データを用いた処理を行う。すなわち、図１に示すカメラ１０１の移動量と、カメラ１０１によって撮影された画像に含まれる特徴点１０３の位置を、多次元正規分布で表現された「状態データ」を利用して推定する。
【００３３】
「状態データ」は、図２に示すように、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。分散共分散行列は、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報、これらの固有状態値の［分散（σａａ）］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散（σａｂ）］を含む行列である。
【００３４】
カメラは移動しながら画像を撮影し、順次、取得する画像から特徴点トラッキング情報（軌跡）を得て、カルマンフィルタや拡張カルマンフィルタ（ＥＫＦ）を適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行うものである。なお、単眼カメラはピンホールカメラモデルにフィットするものとし、全ての特徴点は静止していると仮定する。
【００３５】
ピンホールカメラモデルについて、図３、図４を参照して説明する。ピンホールカメラモデルにおいて、特徴点の画像フレーム中の位置は下式（数式１）によって計算することができる。
【００３６】
【数１】
・・・（数式１）
【００３７】
上記式の意味について、図３、図４を参照して説明する。上記式は、カメラの撮影画像１３０に含まれるオブジェクト１３１の点（ｍ）のカメラ撮影画像平面の画素位置１３２、すなわち、カメラ座標系によって表現されている位置と、世界座標系におけるオブジェクト１２０の３次元位置（Ｍ）１２１との対応関係を示す式である。
【００３８】
カメラ撮影画像平面の画素位置１３２はカメラ座標系によって表現されている。カメラ座標系は、カメラの焦点を原点Ｃとして、像平面がＸｃ，Ｙｃの二次元平面、奥行きをＺｃとした座標系であり、カメラの動きによって原点Ｃは移動する。
【００３９】
一方、オブジェクト１２０の３次元位置（Ｍ）１２１は、カメラの動きによって移動しない原点Ｏを有するＸＹＺ三軸からなる世界座標系によって示される。この異なる座標系でのオブジェクトの位置の対応関係を示す式が上述のピンホールカメラモデルとして定義される。
【００４０】
この式に含まれる値は、図４に示すように、
λ：正規化パラメータ
Ａ：カメラ内部パラメータ、
Ｃｗ：カメラ位置、
Ｒｗ：カメラ回転行列、
を意味している。
【００４１】
さらに、
【数２】
は、同次座標系で表現されたカメラの像平面上の位置である。
λは、正規化パラメータであり、
【数３】
の第３項を満足させるための値である。
【００４２】
なお、カメラ内部パラメータＡには、以下の値が含まれる。
ｆ：焦点距離
θ：画像軸の直交性（理想値は９０°）
ｋｕ：縦軸のスケール（３次元位置のスケールから二次元画像のスケールへの変換）
ｋｖ：横軸のスケール（３次元位置のスケールから二次元画像のスケールへの変換）
（ｕ０，ｖ０）：画像中心位置
【００４３】
このように、世界座標系にある特徴点は位置［Ｍ］で表現される。また、カメラは位置［Ｃｗ］と姿勢（回転行列）Ｒｗで表現される。カメラの焦点位置・画像中心等はカメラ内部パラメータ［Ａ］で表現される。これらのパラメータから、「世界座標系にある特徴点」から「カメラの像平面上」に射影されるそれぞれの位置の関係式が、上述した式（数式１）によって表現することができる。
【００４４】
また、カメラの位置・姿勢を回転行列Ｒｗと並進ベクトルｔｗで表現した場合は、以下に示す式（数式２）のようになる。但し、並進ベクトルｔｗとカメラ位置との関係は式（数式３）で表される。
【００４５】
【数４】
・・・（数式２）
【００４６】
【数５】
・・・（数式３）
【００４７】
図１のシステムにおける単眼カメラ１０１は、このようなピンホールカメラモデルにフィットするカメラである。カメラは移動しながら画像を撮影し、順次、取得する画像から特徴点トラッキング情報（軌跡）を得て、カルマンフィルタや拡張カルマンフィルタを適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行うものである。
【００４８】
カルマンフィルタ、および拡張カルマンフィルタについて説明する。カルマンフィルタは、カメラによって撮影される画像を構成するフレームを順次入力して処理を行う。処理対象フレームの１フレーム前の状態（推定値）と現在フレームから得られる観測値（特徴点トラッキング結果）から現在の状態（推定値）を与える逐次推定型（予測）フィルタである。なお、「状態」は、図２に示す（ａ）平均ベクトルと、（ｂ）分散共分散行列で構成される多次元正規分布モデルで表現される状態データに対応する。
【００４９】
以下にカルマンフィルタの数式を示す。
【数６】
但し、
Ｆｔは予測モデル（行列）
Ｈｔは観測モデル（行列）
を表し、平均０、分散共分散Ｑｔの多次元正規分布モデルに従う予測モデル誤差、平均０、分散共分散Ｒｔの多次元正規分布モデルに従う観測モデル誤差を表す。
また、[＾]は予測値を意味する。なお、各モデルの設定例は、例えば［ｈｔｔｐ：／／ｅｎ．ｗｉｋｉｐｅｄｉａ．ｏｒｇ／ｗｉｋｉ／Ｋａｌｍａｎ＿ｆｉｌｔｅｒ＃Ｅｘａｍｐｌｅｓ］を参照のこと。
【００５０】
カルマンフィルタは、ＦｔとＨｔが行列であるため、線形モデルしか扱えない。この問題を解決するために、拡張カルマンフィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）がある。拡張カルマンフィルタ（ＥＫＦ）は非線形の予測モデル・観測モデルを扱うことができる。拡張カルマンフィルタ（ＥＫＦ）の式は以下の通りである。
【００５１】
【数７】
但し、
Ｆは予測モデル関数、
Ｈは観測モデル関数、
を表し、
（▽Ｆ（ｘ））は関数Ｆのヤコビ行列、
（▽Ｈ（ｘ））は関数Ｈのヤコビ行列、
Ｑｔは予測モデル誤差（平均０，分散共分散Ｑｔの多次元正規分布モデルに従う）、
Ｒｔは観測モデル誤差（平均０，分散共分散Ｒｔの多次元正規分布モデルに従う）、
を表す。
【００５２】
図１に示すカメラ１０１は移動しながら画像を撮影し、順次、取得する画像から特徴点トラッキング情報（軌跡）を得て、上述したカルマンフィルタや拡張カルマンフィルタを適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行う。
【００５３】
先に図２を参照して説明したように、図１に示すカメラ１０１の移動量と、カメラ１０１によって撮影された画像に含まれる特徴点１０３の位置の推定は、図２に示す多次元正規分布で表現された「状態データ」を利用して行われる。
【００５４】
図２を参照して説明したように状態データは、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。図５（ａ）に示すように、分散共分散行列は、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報、
これらの固有状態値の［分散］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散］を含む行列である。
【００５５】
図２、図５に示す状態データにおいて特徴点＃１〜＃Ｎの位置はＰ１〜ＰＮとして示されている。分散共分散行列には、特徴点＃１〜＃Ｎの位置：Ｐ１〜ＰＮに関する共分散データが要素として含まれる。
【００５６】
カメラは移動しながら画像を連続的に撮影しており、撮影画像に含まれる特徴点は順次入れ替わる。すなわち、すべてのフレームに特徴点＃１〜＃Ｎが含まれるとは限らない。すなわち、ある画像フレームＦ１には含まれるが、次の画像フレームＦ２には含まれない特徴点や、画像フレームＦ２には含まれるが、前の画像フレームＦ１には含まれない特徴点が存在する。
【００５７】
すなわち、図５（ｂ）に示すように、カメラ１５１は移動ライン１５２に沿って移動して画像を連続撮影しており、各画像に含まれる特徴点は、順次入れ替わることになる。例えば時間ｔｎにおける撮影画像は撮影画像範囲１５３の画像であるとすると、その撮影画像に含まれる特徴点は、特徴点Ａ１〜Ａ３である。特徴点Ｂや特徴点Ｃは、その撮影画像には含まれず、その前の撮影画像やその後の撮影画像に含まれる特徴点となる。
【００５８】
従来型のＥＫＦベースのＳＬＡＭにおいては、処理時点で得られている特徴点の情報を全て含む「状態データ」を適用した処理が行われる。例えば、フレームｆ０１〜ｆｎｎの画像を利用したＳＬＡＭ処理を実行する場合、フレームｆ０１〜ｆｎｎにおいてカ検出される特徴点をすべて含む状態データを設定して、更新処理を行う。
【００５９】
従って、カルマンフィルタやＥＫＦ（拡張カルマンフィルタ）を適用した更新処理を行う状態データには登録されているが、観測値として入力される画像フレームに写っていない特徴点も存在し、この場合、状態データに含まれる観測値としての入力画像フレームにない特徴点情報の更新処理も行われる。状態データに含まれる共分散データである「カメラに写っている特徴点」と「カメラに写っていない特徴点」との相関情報に基づいてカルマンフィルタやＥＫＦを適用した更新処理が実行される。
【００６０】
結果として、ＳＬＡＭ処理では、多数の特徴点情報を含む更新処理が必要になり、現在の画像から得られていない特徴点の位置情報を用いた状態データの更新処理が行われることになる。このような処理は、計算コストの増大を発生させ処理の非効率化の要因になる。
【００６１】
１つ具体例をあげて説明する。現在の撮影画像に含まれる特徴点、すなわち、「見えている特徴点の位置」が正しく（正しい＝推定値と実際値があっている）、現在の撮影画像に含まれない特徴点、すなわち「カメラの位置姿勢・見えていない特徴点の位置」が正しくない場合を考える。「状態データ」に相関情報がない（＝分散共分散行列の共分散成分が全て０）モデルの場合はカメラ位置のみ更新される。「状態データ」に相関情報がある（＝分散共分散行列の共分散成分が全て０ではない）モデルの場合はカメラ位置姿勢の更新の度合と相関情報に応じて見えていない特徴点の位置も更新される。
【００６２】
ただし、分散共分散行列は全ての特徴点とカメラの相関情報を保持しているため、特徴点の数が増えると分散共分散行列のサイズも大きくなる。特徴点の数ｎに対して、計算量はＯ（９ｎ２）のオーダーで増加する。リアルタイム処理する場合は、特に大きな問題である。このように、状態データに特徴点情報を多く含む場合、処理時間および処理負荷が大きくなるという問題がある。
【００６３】
［２．サブマップについて］
そこで、本発明では、図６に示すように３次元位置解析対象となる全体領域を示すマップ２００を複数のサブマップ２１１，２２１に切り分けて処理を行う。各々のサブマップには、
（ａ）「サブマップの識別子としての番号（ＩＤ）」、
（ｂ）「ＳＬＡＭの計算で用いる「状態データ」」、
これらのデータが対応付けられる。
【００６４】
本実施例では、図６下段に示すサブマップＭ１，２１１，サブマップＭ２,２２１のようにＭ１，Ｍ２，・・・を各サブマップの識別子（ＩＤ）としている。サブマップ対応の状態データ２１２，２２２は、先に図２を参照して説明した状態データと同様のデータであり、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。分散共分散行列は、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報、これらの固有状態値の［分散（σａａ）］と、上記各状態値の異なる状態値の組み合わせの相関情報に対応する［共分散（σａｂ）］を含む行列である。
【００６５】
なお、各サブマップ対応の状態データに含まれる特徴点情報は、各々のサブマップ内に含まれる特徴点の情報のみである。すなわち、図６に示すサブマップＭ１，２１１対応の状態データ２１２は、
カメラの位置、姿勢、速度、角速度、
サブマップＭ１，２１１内の特徴点ａ，ｂ，ｃ，ｄ，ｅの位置情報、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
【００６６】
一方サブマップＭ２，２２１対応の状態データ２２２は、
カメラの位置、姿勢、速度、角速度、
サブマップＭ２，２２１内の特徴点ｄ，ｅ，ｆ，ｇ，ｈの位置情報、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
【００６７】
各サブマップ対応の状態データには、自己のサブマップのみに含まれる特徴点と、異なるサブマップにのみに含まれる特徴点との相互の相関情報、すなわち共分散データなど、自己のサブマップに含まれない特徴点に関する平均ベクトル、分散共分散データは含まれない。
【００６８】
例えばサブマップＭ１，２１１の状態データ２１２には、サブマップＭ１，２１１にのみ含まれる特徴点（ａ，ｂ，ｃ）と、サブマップＭ２，２２１のみに含まれる特徴点（ｆ，ｇ，ｈ）との相関情報、すなわち共分散データは含まれない。サブマップＭ２，２２１の状態データ２２２にも、サブマップＭ２，２２１にのみ含まれる特徴点（ｆ，ｇ，ｈ）と、サブマップＭ１，２１１のみに含まれる特徴点（ａ，ｂ，ｃ）との相関情報、すなわち共分散データは含まれない。
【００６９】
なお、サブマップ２１１，２２１ごとに対応して設定された「状態データ」２１２，２２２は全て世界座標系で表現される。ＳＬＡＭ処理においては、特定のサブマップに対応する「状態データ」のみを対象とした処理、例えばカルマンフィルタや拡張カルマンフィルタを適用した更新処理を行うことで計算量を低減する。
【００７０】
なお、サブマップは、ＳＬＡＭの実行単位として設定する領域である。図１に示すカメラ１０１によつて撮影される画像フレームに対応するものではない。カメラ１０１によって撮影される画像フレームとサブマップとの対応は例えば図７に示す対応となる。
【００７１】
カメラ１０１はポイントＡからポイントＢに移動しながら連続的な撮影処理を行っており、その間、例えば、フレームｆ００１〜ｆ１００、計１００フレームを撮影する。これらの多数の画像フレームから様々な特徴点が抽出され、特徴点のトラッキング（軌跡）データが取得される。
【００７２】
サブマップＭ１，２１１は、例えば画像フレームｆ００１〜ｆ０６０の６０フレーム分の画像を利用してＳＬＡＭ処理を行う領域である。また、サブマップＭ２，２２１は、例えば画像フレームｆ０４１〜ｆ１００の６０フレーム分の画像を利用してＳＬＡＭ処理を行う領域である。このように、サブマップＭ１，Ｍ２・・・は、ＳＬＡＭの実行単位として設定する領域である。なお、上述したフレーム数は一例であり、各サブマップに対応するフレーム数は任意の数でよい。
【００７３】
サブマップ領域の設定は、例えば、１つのサブマップに含まれる特徴点の数が、予め設定した上限数以下とするという条件を用いて行われる。なお、図６、図７には２つのサブマップのみを示しているが、サブマップ数は任意の数に設定できる。図８には、４つのサブマップＭ１〜Ｍ４の設定例を示している。図６〜図８に示すように、サブマップは別のサブマップと重なるように設定する。このとき重複した領域にある特徴点は各サブマップの状態変数で表現する。要するに、重複した領域にある特徴点は位置情報を複数保持する。複数ある特徴点位置は、後で位置情報が収束するように最適化処理する（図１０のフローチャートの「全体最適化」に該当）。詳しくは後述する。
【００７４】
［３．情報処理装置の構成例およびサブマップを適用した処理例について］
次に、本発明の情報処理装置の構成例およびサブマップを適用した処理例について説明する。図９は、本発明の情報処理装置の一実施例構成を示す図である。本発明の情報処理装置３００は、カメラ３０１によって撮影された画像データに基づいて、特徴点トラッキング情報（軌跡）を得て、カルマンフィルタや拡張カルマンフィルタを適用して状態データの更新を実行し、カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を行うものである。なお、カメラ３０１はピンホールカメラモデルにフィットするものである。
【００７５】
本発明の一実施形態にかかる情報処理装置３００は図９に示すように、周囲環境を撮影するカメラ３０１の撮影した撮影画像をカメラ３０１から直接、あるいはカメラ３０１が撮影し、撮影画像記憶部３０２（ＤＢ０：データベース０）に格納された撮影画像を入力して処理を行う。
【００７６】
サブマップ設定部３０３は、３次元マップの生成対象となる処理対象領域（全体マップ）を任意の複数領域に区分したサブマップ領域を設定する。なお、３次元マップの生成対象となる領域は図９に示すカメラ３０１における撮影画像によってカバーされる領域となる。３次元マップの生成処理は、カメラ３０１の撮影画像をリアルタイムで順次入力して実行してもよいし、撮影済み画像を記憶した記憶部３０２から取得して実行してもよい。
【００７７】
撮影済み画像を記憶した記憶部から取得する場合は、３次元マップの生成対象となる処理対象領域（全体マップ）が予め確認できるので、確認された全体領域を複数のサブマップ領域に分割することができる。分割処理は任意の基準で行ってよい。例えば全体領域を２分割して初期的な２つのサブマップを設定する。あるいは４分割して４つのサブマップを設定するといった処理が可能である。あるいは一定の撮影画像フレーム数単位、撮影時間単位でサブマップを設定してもよい。たとえば先に図７を参照して説明したように、予め設定したフレーム数、例えば６０フレームの画像フレームによってカバーされる領域を１つのサブマップとする処理や、予め設定した所定のカメラ移動距離単位で１つのサブマップを設定するといった処理を行う構成としてもよい。
【００７８】
カメラ３０１の撮影画像をリアルタイムで順次入力して処理を行う場合も、予め設定したフレーム数の画像フレームによってカバーされる領域を１つのサブマップとする処理や、予め設定した所定のカメラ移動距離単位で１つのサブマップを設定するといった処理を行う構成とすることができる。
【００７９】
サブマップ設定部３０３は、このように、カメラ３０１または記憶部３０２から撮影画像を入力してサブマップの設定を行う。なお、初期的には１つのマップのみを設定し、そのマップを処理の進行に従って複数のサブマップに分割する処理を行う構成としてもよい。例えば処理対象とするマップまたはサブマップ内の特徴点数が予め既定した上限数以上になった場合に２つに分割するといった処理を行うことが可能である。あるいは処理対象のマップまたはサブマップの空間的な領域が既定サイズより大きくなった場合に、分割するといった処理が可能である。これらのサブマップ設定処理、分割処理については、後段で詳細に説明する。
【００８０】
情報処理装置３００は、さらに、カメラの位置姿勢を推定するカメラ位置姿勢検出部３０４、カメラ位置姿勢検出部３０４において検出されたカメラ位置姿勢情報を記憶するカメラ位置姿勢記憶部３０６（ＤＢ１：データベース１）を有する。
また、撮影画像に含まれる特徴点の追跡処理を実行する特徴点追跡部３０５、特徴点追跡部３０５の検出した特徴点の軌跡情報を記憶する特徴点軌跡（トラッキング）情報記憶部３０７（ＤＢ２：データベース２）を有する。
【００８１】
さらに、カメラ位置姿勢記憶部３０４に格納されたカメラ位置姿勢情報と、特徴点軌跡情報記憶部３０５に格納された特徴点軌跡情報を入力して、これらの入力情報に基づいてカメラの位置姿勢および特徴点の３次元位置を算出する３次元情報生成部３０８、さらに、自己（カメラ）位置と、特徴点３次元位置情報を含むサブマップ対応状態データ（存在確率分布データ）を格納した状態データ記憶部３０９を有する。
【００８２】
カメラ位置姿勢検出部３０４、特徴点追跡部３０５、および３次元情報生成部３０８は、サブマップ設定部３０３において設定されたサブマップ単位で処理を行なうことになる。
【００８３】
カメラ３０１は、例えば移動するロボットに装着され、あるいはユーザ（人）によって適宜、移動される。すなわちカメラ３０１の位置や姿勢は固定されたものではなく、変更された位置姿勢からの撮影画像、すなわち異なる視点から撮影された画像フレーム（例えば動画像）を順次入力する。なお、カメラ３０１は前述したようにピンホールカメラモデルに合うものを用いている。
【００８４】
カメラ位置姿勢検出部３０４は、カメラ３０１から入力する映像内の特徴点を利用して、特徴点の位置とカメラ位置を毎フレーム修正し、カメラ位置姿勢検出部３０４で決めた世界座標系で表されたカメラの推定位置および姿勢情報としてのカメラの位置（Ｃｗ）およびカメラの回転行列（Ｒｗ）を生成してカメラ位置姿勢記憶部３０６に格納する。
【００８５】
なお、この処理は、例えば論文［Ａｎｄｒｅｗ Ｊ．Ｄａｖｉｓｏｎ，"Ｒｅａｌ−ｔｉｍｅ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｓａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ ｗｉｔｈ ａ ｓｉｎｇｌｅ ｃａｍｅｒａ"，Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ９ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，Ｎｉｎｔｈ，（２００３）］に記載された方法を用いることができる。
【００８６】
なお、カメラ位置姿勢検出部３０４では、カメラの位置（Ｃｗ）およびカメラの回転行列（Ｒｗ）の各々について、正規分布に従った存在確率分布を持つカメラ位置姿勢情報を生成してカメラ位置姿勢記憶部３０６に格納する。
【００８７】
カメラ３０１の撮影画像は特徴点追跡部３０５にも入力される。特徴点追跡部３０５は、カメラ３０１によって取得されるフレーム画像内から選択される局所領域（特徴点）のフレーム間の移動量を求める。例えば、特徴点追跡部３０５は前フレームの局所領域画像とのウインドウマッチングを実行して特徴点のフレーム間の移動量を求める。
【００８８】
特徴点追跡部３０５で得られる特徴点追跡結果は、特徴点トラッキングデータとして特徴点軌跡（トラッキング）情報記憶部３０７に記録される。
【００８９】
図９のカメラ位置姿勢記憶部３０６と特徴点軌跡（トラッキング）情報記憶部３０７は、それぞれカメラ位置姿勢検出部３０４の検出したカメラ位置姿勢情報と、特徴点追跡部３０５の検出した特徴点軌跡情報について、全フレームの出力結果を保持している。なお、それぞれのデータのフレーム時間の同期はとれているものとする。
【００９０】
３次元情報生成部３０８は、カメラ位置姿勢記憶部３０６と特徴点軌跡（トラッキング）情報記憶部３０７に格納されたカメラ位置姿勢情報および特徴点軌跡（トラッキング）情報を入力し、前述のサブマップ対応の状態データを適用したＳＬＡＭ処理を実行して、カメラの位置姿勢の推定（ローカリゼーション）と特徴点の３次元位置の推定（マッピング）を実行する。この処理は、サブマップ対応の状態データの更新処理、すなわち先に説明したカルマンフィルタや拡張カルマンフィルタを適用した更新処理が含まれる。サブマップ対応の状態データは、状態データ記憶部３０９（ＤＢ３：データベース３）に格納され、随時更新される。
【００９１】
なお、図９に示す構成において、各処理ブロックは、固有のハードウェアとして設定してもよいし、例えばＰＣなどの情報処理装置において所定のプログラムを実行させて各処理ブロックに対応する処理を実行させる構成としてもよい。例えば後述する図１０に示す処理シーケンスを実行させるプログラムをメモリに格納し、プログラム実行部としてのＣＰＵを備えた制御部においてプログラムを実行させることで処理を行うことが可能である。
【００９２】
本発明の情報処理装置の実行する処理シーケンスについて、図１０に示すフローチャートを参照して説明する。
まず、処理の全体概要について説明し、その後、各ステップの処理の詳細について説明する。なお、図１０に示すフローチャートのステップＳ１０１〜Ｓ１０７は、１つの画像フレームの入力ごとに実行する処理である。ステップＳ１０８における全体最適化処理の実行単位としてのフレーム数を［Ｌ］フレーム、例えばＬ＝６０フレームとして、処理フレームがＬに至るまでは、ステップＳ１０１〜Ｓ１０７の処理を入力フレームごとに繰り返し実行する。
【００９３】
ステップＳ１０７において、フレーム数がＬとなった場合、ステップＳ１０８において全体最適化処理を実行して、ステップＳ１０９においてカウンタを０にリセットして、ステップＳ１０１に戻り、その後の入力フレームに対する処理を実行する。
【００９４】
なお、図１０に示す記憶部（ＤＢ）３２０は、図９に示す４つの記憶部ＤＢ０〜ＤＢ３に対応する。すなわち、
撮影画像記憶部３０２（ＤＢ０：データベース０）
カメラ位置姿勢記憶部３０６（ＤＢ１：データベース１）
特徴点軌跡（トラッキング）情報記憶部３０７（ＤＢ２：データベース２）
状態データ記憶部３０９（ＤＢ３：データベース３）
これら４つの記憶部を示している。
【００９５】
図１０に示すフローチャートの処理の全体概要について説明する。
ステップＳ１０１：注目サブマップを選択する。
ステップＳ１０２：注目サブマップに対する「状態データ」を用意する。
ステップＳ１０３：初期化処理（最初の処理フレームのみ）。
ステップＳ１０４：注目サブマップの状態データを適用した状態データ更新処理、例えばＥＫＦ ＳＬＡＭによる状態データ更新処理を実行する。なお、本実施例ではＥＫＦ（拡張カルマンフィルタ）を適用した処理例について説明するが、カルマンフィルタを適用した処理として実行することも可能である。
ステップＳ１０５：必要に応じて注目サブマップの切り分け処理を行う。予め設定した条件、例えばサブマップに含まれる特徴点が既定上限数を越えた場合などに注目サブマップを２つのサブマップに分割する。
ステップＳ１０６：処理回数（処理フレーム数）のカウント値の更新処理（＋１）を行う。
ステップＳ１０７：処理回数（処理フレーム数）が既定処理回数Ｌを超えたか否かを判定する。
ステップＳ１０８：全体最適化処理を行う。
ステップＳ１０９：カウント値を０にリセットする。
このような処理を繰り返し実行する。
【００９６】
なお、ステップＳ１０３の処理は、最初の処理フレームのみに対する処理であり、最初の処理フレームではＳＬＡＭ処理に必要となる初期化処理を行う。なお、初期化処理において生成する最初のサブマップは、「サブマップＭ１」とする。
また、予め設定したフレーム数、Ｌフレームに１回の割合で全体最適化（＝「バンドル調整処理」）を行う。これは、サブマップ同士の位置あわせを行い、重複した領域にある特徴点の位置が収束するようにする処理である。
【００９７】
以下、図１０に示すフローチャートの各ステップの処理の詳細について説明する。
（ステップＳ１０１）注目サブマップの選択処理について
まず、図１０に示すフローチャートのステップＳ１０１における注目サブマップ選択処理の詳細シーケンスについて、図１１に示すフローチャートを参照して説明する。
【００９８】
まずステップＳ１５１において、３次元マップの生成対象となる処理対象領域の少なくとも一部を含むサブマップを設定する。なお、初期的には、１つのサブマップのみを設定してもよいし、処理対象領域を複数領域に区分して複数のサブマップ領域を設定してもよい。
【００９９】
なお、３次元マップの生成対象となる領域は、図９に示すカメラ３０１における撮影画像によってカバーされる領域となる。３次元マップの生成処理は、カメラ３０１の撮影画像をリアルタイムで順次入力して実行してもよいし、撮影済み画像を記憶した記憶部から取得して実行してもよい。
【０１００】
サブマップの設定態様は様々な態様が可能である。例えば全体マップが把握できている場合は、初期的に全体マップを１つのサブマップとしてもよいし、予め２以上の任意数のサブマップを設定してもよい。複数のサブマップを設定する場合は、カメラの移動距離や撮影フレーム数、撮影時間などの様々な基準に従って全体マップを分割することができる。
【０１０１】
カメラ３０１の撮影画像をリアルタイムで順次入力して処理を行う場合は、順次入力する画像フレームによって取得される領域によって１つのサブマップが設定されることになる。この場合、初期的には１つのサブマップのみとなり、画像フレームの入力に応じてサブマップ領域が次第に大きくなる。大きくなったサブマップは、ステップＳ１０５のマップ切り分け処理によって、２つのサブマップに分割されることになる。この処理については後段で詳細に説明する。
【０１０２】
図１１のフローのステップＳ１５２では、ステップＳ１５１で設定したサブマップ毎にカメラに写っている特徴点をスキャンする。カメラ位置姿勢は、初期設定情報、または直前フレームでのカルマンフィルタはまたはＥＫＦを用いたＳＬＡＭによる推定結果を適用し、ピンホールカメラモデル（図３、図４参照）に従ってスキャンする。
【０１０３】
次に、ステップＳ１５３において、スキャンした特徴点がどのサブマップに属しているかチェックし、特徴点が一番多く属しているサブマップを注目サブマップとする。もし特徴点が一番多く属しているサブマップが複数ある場合は、番号（ＩＤ）が大きいサブマップを選択する。以上で、注目サブマップの選択が完了する。
【０１０４】
（ステップＳ１０２）注目サブマップに対する「状態データ」を用意する処理について、
次に、図１０に示すフローチャートのステップＳ１０２における注目サブマップに対する「状態データ」を用意する処理について図１２に示すフローチャートを参照して説明する。
【０１０５】
まず、ステップＳ２０１において、現在の注目サブマップ識別子（ＩＤ）を保存する。これは、例えば、現在の処理フレームの１フレーム前の処理フレームに対応する注目サブマップ識別子（ＩＤ（番号））を把握するために行う。
【０１０６】
ステップＳ２０２では、１フレーム前の注目サブマップと現フレームの注目サブマップが異なるか否かを判定し、異なる場合はステップＳ２０３において、１フレーム前の注目サブマップの「状態データ」のカメラ情報を現フレームの注目サブマップの「状態データ」のカメラ情報に上書きする。
【０１０７】
なお、「状態データ」は、先に図２を参照して説明したように、画像を撮影するカメラと特徴点の状態を示す状態値の多次元正規分布データによって構成される。すなわち、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
これらの状態データは、サブマップ対応の状態データとして設定され、特徴点に関する情報は各サブマップに含まれる特徴点情報のみが含まれている。
【０１０８】
ステップＳ２０３では、サブマップ対応の状態データに含まれるカメラ情報、すなわち、サブマップ対応の状態データに含まれるカメラの位置、姿勢、速度、角速度に関する情報については、新たなサブマップに対する処理を行う場合、１フレーム前の異なるサブマップ対応の状態データを利用する。
【０１０９】
図１３にサブマップ対応の状態データの例を示す。この状態データにおけるカメラ情報は、（ａ）平均ベクトルのデータ部分３２１と、（ｂ）分散共分散行列のデータ部分３３１の各データ部分である。これらのデータ部分については、１フレーム前の異なるサブマップ対応の状態データをそのままコピーして利用する。
【０１１０】
さらに、ステップＳ２０３では、状態データ中の分散共分散行列に含まれるカメラと特徴点の共分散の値を全て０にする。これは、前のサブマップで求めたカメラ位置は、前のサブマップ内の特徴点との関係から求めたものであり、現在のサブマップの特徴点とは関係ないため、データをリセットするものである。
【０１１１】
図１３に示す「状態データ」の（ｂ）分散共分散行列のデータ部分３３２，３３３の共分散の値を全て０にする。
以上でＥＫＦ ＳＬＡＭ用の「状態データ」の前処理が完了する。
【０１１２】
（ステップＳ１０４）注目サブマップの状態データを適用した状態データ更新処理について、
ステップＳ１０４の注目サブマップの状態データを適用した状態データ更新処理は、例えばカルマンフィルタ、あるいは拡張カルマンフィルタを適用した状態データの更新処理として行われる。注目サブマップ対応の状態データを、入力フレームから得られる情報、すなわち、カメラの位置、姿勢、速度、角速度、各特徴点の位置情報を利用して、更新する処理を行う。
【０１１３】
拡張カルマンフィルタを適用した処理、すなわちＥＫＦベースのＳＬＡＭについては、例えば、［"Ｒｅａｌ−Ｔｉｍｅ Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｓａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ ｗｉｔｈ ａ Ｓｉｎｇｌｅ Ｃａｍｅｒａ"，Ａｎｄｒｅｗ Ｊ． Ｄａｖｉｓｏｎ， ＩＣＣＶ ２００３］、あるいは、本出願人が先に出願した［特開２００８−３０４２６９号公報］に記載されており、この記載方法を適用することが可能である。ただし、本願において適用する状態データ、すなわち更新処理対象とするのはサブマップ対応の状態データである。サブマップ対応の状態データには、サブマップに含まれる特徴点以外の特徴点についての位置情報、相関情報は含まれず、データ量が少ないため、極めて高速な計算処理が可能となる。
【０１１４】
なお、サブマップ対応の状態データの更新処理に適用するために入力した画像フレームに、状態データに登録されていない新たな特徴点が検出された場合は、その新規特徴点を状態データ中の変数として追加する処理を行う。
【０１１５】
（ステップＳ１０５）注目サブマップの切り分け処理について
次に、図１０に示すフローチャートのステップＳ１０５において実行する注目サブマップの切り分け処理について説明する。
【０１１６】
注目サブマップの切り分け処理は必要に応じて実行される。現在処理対象としている注目サブマップは、予め設定した条件に応じて分割する。
以下、具体例として以下の２つの分割処理例について説明する。
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理例
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理例
【０１１７】
まず、上記（ａ）の処理例、すなわち、
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理例について説明する。
【０１１８】
この分割処理の全体シーケンスを図１４に示すフローチャートを参照して説明する。
最初に、ステップＳ３０１において、注目サブマップの特徴点の総数をカウントする。
次に、ステップＳ３０２において、カウントした特徴点の総数が、予め設定した既定上限数［Ｎ］以上であるか否かを判定する。
【０１１９】
カウントした特徴点の総数が、予め設定した規定上限数［Ｎ］以上である場合は、ステップＳ３０３に進み、現在処理対象としている注目サブマップの分割処理を行う。Ｎ点未満であれば分割処理をしないで終了する。
【０１２０】
なお、予め設定する既定上限数［Ｎ］は、カメラなどの状況に応じて設定する。本例では、既定上限数：Ｎ＝１２５とする。
【０１２１】
図１４のフローにおけるステップＳ３０３の「注目サブマップの分割処理」の詳細シーケンスについて、図１５に示すフローチャートを参照して説明する。サブマップ分割処理は、図９に示す情報処理装置３００のサブマップ設定部３０３において実行される。なお、分割処理対象とするサブマップは、図１０に示すフローの最初のステップＳ１０１において選択した注目サブマップである。
【０１２２】
まず、ステップＳ４５１において、処理対象としているサブマップ、すなわち注目サブマップが、他のサブマップとの重なり領域の有無を判定する。重なり領域を持たない場合は、ステップＳ４５２に進み、重なり領域を持つ場合は、ステップＳ４５３に進む。
【０１２３】
注目サブマップが、他のサブマップとの重なり領域を持たない場合は、ステップＳ４５２に進み、注目サブマップの特徴点群の重心を求め，重心から最も遠い特徴点を点Ｇとする。
また、注目サブマップが、他のサブマップとの重なり領域を持つ場合は、ステップＳ４５３に進み、注目サブマップの特徴点で、他のサブマップに属する点を選び、選択した点群の重心を点Ｇとする。
これらの処理を実行する。この処理について図１６を参照して説明する。
【０１２４】
図１６には、
（ａ）サブマップ重複領域がない場合のサブマップ分割処理（Ｓ４５２の処理）
（ｂ）サブマップ重複領域がある場合のサブマップ分割処理（Ｓ４５３の処理）
これらの各処理の具体例を示している。分割処理対象としている注目サブマップはサブマップＭ４である。
【０１２５】
図１６（ａ）に示すように、サブマップ重複領域がない場合のサブマップ分割処理（Ｓ４５２の処理）は、注目サブマップＭ４の特徴点群の重心Ｐ１を求め，重心Ｐ１から最も遠い特徴点を点Ｇ１とする処理である。
一方、サブマップ重複領域がある場合のサブマップ分割処理（Ｓ４５３の処理）は図１６（ｂ）に示すように、注目サブマップＭ４の特徴点で、他の重複サブマップＭ３に属する特徴点を選び、選択した特徴点群の重心を点Ｇ２とする処理である。
【０１２６】
これらの処理の後、ステップＳ４５４に進む。
ステップＳ４５４では、注目サブマップに属する特徴点各々の点Ｇからの距離を算出し、距離の遠い順に並べて、以下の各サブマップ対応の特徴点設定処理を行う。
１番〜Ｍ番：新規サブマップの特徴点
Ｍ＋１番〜２Ｍ番：注目サブマップと新規サブマップの重複領域の特徴点
上記の処理によって、新規サブマップを設定する。
なお、Ｍは、注目サブマッフ内の特徴点数＝Ｎとしたとき、例えば、
Ｍ＝Ｎ×０．２
の設定とする。
Ｎ＝１２５の場合、Ｍ＝２５であり、新規追加サブマップにはサブマップ分割時点で、５０の特徴点が設定される。５０の特徴点中、２５の特徴点が新規追加サブマップのみに属する特徴点であり、残り２５の特徴点が新規追加サブマップと注目サブマップの重複領域に属する特徴点となる。
【０１２７】
この処理の具体例は、図１６に示す通りである。（ａ）サブマップ重複領域がない場合のサブマップ分割処理では、図１６（ａ）に示すように、新規追加サブマップＭ５が設定され、（ｂ）サブマップ重複領域がある場合のサブマップ分割処理では、図１６（ｂ）に示すように、新規追加サブマップＭ５が設定される。
【０１２８】
次に、ステップＳ４５５において、分割前の注目サブマップの「状態データ」を分割後の新規追加サブマップの「状態データ」とするコピー処理を行う。
最後に、ステップＳ４５６において、注目サブマップと新規追加サブマップの各「状態データ」から各サブマップに属さない特徴点」の情報を削除して、各サブマップの状態データとする処理を行う。
【０１２９】
図１７以下を参照してこれらの処理の具体例について説明する。図１７は、分割処理対象となる注目サブマップＭ４に対応する状態データのコピー処理について説明する図である。
【０１３０】
図１７（ａ）は、分割処理前のサブマップＭ４とサブマップＭ４対応状態データ３５１を示している。状態データは、先に図２を参照して説明したように、
カメラの位置、姿勢、速度、角速度、
各特徴点の位置情報（Ｐ１〜ＰＮ）、
これらの状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データによって構成される。
【０１３１】
ステップＳ４５５の処理は、図１７（ａ）に示すサブマップＭ４対応状態データ３５１を図１７（ｂ）に示すように、サブマップＭ４対応状態データ３５２、サブマップＭ５対応状態データ３５３として設定する処理である。これらは仮のサブマップ対応状態データである。
【０１３２】
次に、これらのサブマップＭ４対応状態データ３５２と、サブマップＭ５対応状態データ３５３から、それぞれのサブマップに属さない特徴点に関する情報を削除する処理を行う。
【０１３３】
図１８は、状態データ中の平均ベクトルからの情報削除処理、図１９は、分散共分散行列からの情報削除処理の具体例を示している。このように、これらのサブマップＭ４対応状態データ３５２と、サブマップＭ５対応状態データ３５３から、それぞれのサブマップに属さない特徴点に関する情報を削除する処理を行って、最終的な各サブマップの状態データを更新および設定する処理を行なう。
【０１３４】
この処理によって、図１４に示すフローチャートにおけるステップＳ３０３の注目サブマップの分割処理が終了する。なお、図１４のフローに示すサブマップ分割処理は、前述のように、図１０に示すフローチャートのステップＳ１０５において実行する注目サブマップの切り分け処理の一態様である。
【０１３５】
前述したように図１０に示すフローチャートのステップＳ１０５において実行する注目サブマップの切り分け処理は、必要に応じて実行される処理であり、例えば以下のいずれかの処理態様で行われる。
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理
図１４〜図１９を参照して説明した処理は、上記の（ａ）の処理に対応する。
【０１３６】
次に、図２０以下を参照して、上記の（ｂ）のサブマップ分割処理例について説明する。すなわち、
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理
である。
【０１３７】
このサブマップ分割処理の全体シーケンスを説明するフローチャートを図２０に示す。
図２０に示すフローチャートを参照して、３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割し手サブマップ分割を行う処理について説明する。
【０１３８】
なお、図２０に示すフローの処理の前処理として、ステップＳ５００の処理として、処理対象としている３次元空間に以下のパラメータを設定しておく。
＊ベクトル［ｋ］
＊ベクトル［ｒ１］
＊ベクトル［ｒ２］
＊分割中心［Ｏ］
＊距離［ｄ］，［ｅ］
【０１３９】
図２１を参照してこれらのパラメータについて説明する。
図２１は３次元マップを生成する対象となる空間（部屋）を示している。これらの空間に設定される多数の特徴点に対する３次元位置を算出する。
ベクトル［ｋ］は２次元マップへ射影する方向を表しており、状況に応じて設定する。例えば、図２１に示すような部屋に関するマップを扱うとした場合、図２１に示すように、床面に垂直なベクトルをベクトル［ｋ］と設定するのが適していると考える。
【０１４０】
ベクトル［ｒ１］と、ベクトル［ｒ２］は、図２１に示すように、３次元空間上の２次元マップの座標軸ベクトルを表す。ベクトル［ｒ１］と、ベクトル［ｒ２］は、以下の条件を満足するように設定する。
（条件１）ベクトル［ｋ］と、射影変換によって生成する２次元マップの座標軸ｒ１，ｒ２同士は直交する。
（条件２）２次元マップの座標軸［ｒ１］，［ｒ２］同士は直交する。
これらの条件を満たすように状況にあわせて設定する。
なお、これらのベクトル，［ｋ］，［ｒ１］，［ｒ２］は３×１の行列として表現される。
【０１４１】
分割中心［Ｏ］は、図２２に示すように、射影して生成した２次元マップにおける分割処理の基準点である。
距離［ｄ］は、図２２に示すように、射影して生成した２次元マップにおいて設定するサブマップ間隔である。
距離［ｅ］は、図２２に示すように、生成した２次元マップにおいて設定するサブマップの重複領域の重複幅である。
【０１４２】
図２０のステップＳ５０１では、３次元空間のマップに属する全特徴点を２次元マップに射影する。射影変換は、図２０に示すように、以下の式（数式Ａ）を適用することができる。
【０１４３】
【数８】
・・・・・（数式Ａ）
【０１４４】
但し、Ｘ，Ｙ，Ｚは特徴点の３次元空間上の位置、ｕ，ｖは特徴点の２次元マップ上の位置、λは任意の数を表す。
【０１４５】
次に、ステップＳ５０２において２次元マップに射影された特徴点マップを分割してサブマップを生成する。マップ分割処理は、図２２に示すように、分割中心［Ｏ］を中心に、距離［ｄ］，［ｅ］を利用して等間隔に分割する。
【０１４６】
図２２には、太線で示す６つのサブマップＭ１〜Ｍ６を示している。分割中心［Ｏ］を分割基準点として、射影して生成した２次元マップにおいて、サブマップ間隔を［ｄ］、サブマップ重複領域の重複幅を［ｅ］として６つのサブマップが設定される。この処理が、３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理である。なお、さらに、新しくサブマップを作る必要がある場合は、今まであるサブマップの識別子と異なる識別子、例えば番号の最大値に１を足したものを新しく割り当てて新規追加サブマップの識別子とする。
【０１４７】
このように、図１０のステップＳ１０５のサブマップ切り分け処理は、
図１４〜図１９を参照して説明した
（ａ）注目サブマップに含まれる特徴点が、予め設定した既定上限数以上となった場合に分割する処理
あるいは、図２０〜図２２を参照して説明した
（ｂ）３次元マップを２次元マップに射影近似し、２次元マップ上で空間的に等間隔に分割する処理
上記処理のいずれかを適用して実行される。なお、（ａ），（ｂ）の処理を組み合わせて実行する構成としてもよい。
【０１４８】
図１０のフローチャートの説明に戻る。ステップＳ１０５の処理、すなわち必要に応じたマップ切り分け処理が終了すると、ステップＳ１０６において、入力フレームのカウンタを更新する。すなわち、１つの処理フレームごとにカウンタを［＋１］とするカウンタ更新処理を実行する。
【０１４９】
次に、ステップＳ１０７において、予め設定したステップＳ１０８における全体最適化処理の実行単位としてのフレーム数［Ｌ］とカウンタ値を比較する。カウンタ値がＬ未満である場合は、処理フレームがＬにいたるまでは、ステップＳ１０１〜Ｓ１０７の処理を入力フレームごとに繰り返し実行する。Ｌは例えば６０フレームである。
【０１５０】
ステップＳ１０７において、フレーム数がＬとなった場合、ステップＳ１０８において全体最適化処理を実行して、ステップＳ１０９においてカウンタを０にリセットして、ステップＳ１０１に戻り、その後の入力フレームに対する処理を実行する。
【０１５１】
ステップＳ１０８の全体最適化処理について、図２３、図２４を参照して説明する。ステップＳ１０８の全体最適化処理の詳細シーケンスについて、図２３に示すフローチャートを参照して説明する。
【０１５２】
全体最適化処理は、バンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）を用いる。図２３に示すフローの説明の前にバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）の概要について図２４を参照して説明する。
【０１５３】
バンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）は、例えば図２４に示すように、３つの異なる位置からの撮影画像４１１〜４１３を利用し、その撮影画像中から検出した特徴点４２１〜４２３を利用して、特徴点の３次元位置を求める。複数の画像に含まれる特徴点中、対応する特徴点の位置は図２４に示す特徴点４２１のように、各画像の撮影ポイントから特徴点４２１を結ぶ線（Ｂｕｎｄｌｅ）が１つの特徴点４２１位置において交わるはずである。
【０１５４】
しかし、カメラの位置や特徴点の画像内の位置情報などは必ずしも正確に算出されず様々な要因による誤差が含まれる。従ってこのような誤差を取り除く必要がある。具体的には１つの対応する特徴点の３次元位置とカメラ位置とを結ぶ線が、その１つの特徴点において交わるように補正する。すなわち算出済みのカメラ位置や特徴点位置を修正する処理が必要となる。この処理がバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）として実行され、この調整処理によって修正された特徴点位置情報やカメラ位置情報を利用してより精度の高い３次元情報を生成することが可能となる。このバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）は従来から知られる一般的な方法で、例えば、文献［"Ｔｈｅ Ｄｅｓｉｇｎ ａｎｄ Ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｆ ａ Ｇｅｎｅｒｉｃ Ｓｐａｒｓｅ Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ Ｓｏｆｔｗａｒｅ Ｐａｃｋａｇｅ Ｂａｓｅｄ ｏｎ ｔｈｅ Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ Ａｌｇｏｒｉｔｈｍ"，ｂｙ Ｍ．Ｉ．Ａ．Ｌｏｕｒａｋｉｓ ａｎｄ Ａ．Ａ．Ａｒｇｙｒｏｓ，Ａｕｇ．（２００４）．］に記載された手法を適用できる。
【０１５５】
図２３に示すフローチャートの各ステップの処理について説明する。なお、この処理は、図９に示す３次元情報生成部３０８において行われる処理である。この処理における入力は以下の情報である。
＊カメラ位置姿勢の履歴
＊最新フレームでの特徴点位置全て
＊特徴点トラッキング結果（画像内の特徴点の位置。カメラ位置の履歴に対応する画像全て）
【０１５６】
なお、カメラ位置の履歴と特徴点トラッキング結果は、毎フレームごとに実行するＥＫＦ ＳＬＡＭ処理（図１０のフローのステップＳ１０４の処理）の結果から取得する。
【０１５７】
まず、ステップＳ６０１において、最新フレームの特徴点位置を、特徴点が属するサブマップの状態データから取得する。特徴点位置は、所属するサブマップの状態データの平均ベクトル（図２（ａ））から取得する。複数のサブマップに属する特徴点（図７のｄ、ｅ）は、所属するサブマップ各々の状態データの平均ベクトルから、所属するサブマップ数の位置情報を取得し、取得した特徴点位置の平均値をとる。この特徴点位置を初期値とする。
【０１５８】
ステップＳ６０２では、最新のカメラ位置姿勢情報を取得する。処理対象としているサブマップの状態データから取得することができる。
【０１５９】
ステップＳ６０３では、図２４を参照して説明したバンドル調整処理（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ）による全体最適化処理を行う。すなわち、同じ特徴点が撮影されている複数の画像フレームを取得して、これら複数の画像フレームにおける共通する特徴点の位置を一致させるバンドル調整処理による全体最適化を実行する。
【０１６０】
この処理によって、特徴点位置の微調整が行われることになり、サブマップの状態データも、この微調整後の特徴点位置情報に基づいて更新することが必要となる。具体的には、ステップＳ６０４において、微調整された各特徴点が所属しているサブマップの特徴点位置を上書きする。複数のサブマップに属する特徴点（図７のｄ、ｅ）は、所属するサブマップ各々の状態データの平均ベクトルに（図２（ａ））に、最適化された位置情報を上書きする。
さらに、ステップＳ６０５において、最新のカメラ位置・姿勢を上書きする。
以上で、図１０に示すフローのステップＳ１０８の全体最適化処理が完了する。
この全体最適化処理は計算負担が重いため、図１０のフローに示すようにＬフレームに１回の割合で処理する。
【０１６１】
以上で本発明の３次元マップの生成に必要となる特徴点位置が算出される。
上述した説明から理解されるように、本発明の処理では、３次元マップの生成領域全体を複数のサブマップに区分し、各サブマップ対応の状態データを設定して、このサブマップ対応の状態データに対する更新処理をカルマンフィルタや拡張カルマンフィルタ（ＥＫＦ）を用いて実行する。この構成により、状態データ更新に伴う計算量が大幅に削減でき、効率的なＳＬＡＭ処理、すなわち、特徴点の３次元位置解析、カメラの位置姿勢算出、さらに３次元環境マップの作成を行うことができる。
【０１６２】
［４．サブマップの設定手法の追加説明］
上述した本発明の処理において、サブマップの設定に際して、隣接するサブマップは共通する特徴点を持つ重複領域を持つように設定することが好ましいと説明した。この理由について、図２５、図２６を参照して説明する。
【０１６３】
隣接するサブマップは共通する特徴点を持つ重複領域を持つように設定することが好ましい理由は、マップの全体最適化（バンドル調整処理）をより正確に行うためである。
【０１６４】
図２５はサブマップ同士が重複するように配置した例である。星印は特徴点、カメラは図に示す移動軌跡に沿って左から右に移動して、１ａ，１ｂ，１ｃ／２ａ，１ｄ／２ｂ，２ｃ，２ｄの各地点で画像を撮影しているものとする。
１ａでは、サブマップＭ１の領域の画像を撮影している。
１ｂ，１ｃ，２ａ，２ｂでは、サブマップＭ１とサブマップＭ２の重複領域の画像を撮影している。
２ｃでは、サブマップＭ２の領域の画像を撮影している。
１ａ，１ｂ，１ｃ，２ａ，２ｂ，２ｃの各カメラ位置と星印で示す特徴点を結ぶ直線は、特徴点のバンドル（トラッキング結果）を表している。
【０１６５】
この設定で、特徴点４５１，４５２のバンドル調整を行う場合について考察する。例えば、サブマップＭ１とサブマップＭ２に属する特徴点４５１に注目する。特徴点４５１を結ぶバンドル情報（トラッキング結果）は３つあり、それぞれ、カメラ１ｂ、カメラ１ｃ、カメラ２ａとを結んでいる。特徴点４５１と、カメラ１ｂ、カメラ１ｃ、カメラ２ａとを結ぶバンドル（トラッキング情報）は、前述のとおりＥＫＦＳＬＡＭ（図１０のフローのステップＳ１０４）が生成する。ただし、ＥＫＦＳＬＡＭには１つのサブマップのみ入力される。
カメラが１ｂの位置にある時と１ｃの位置にある時は、注目サブマップはサブマップＭ１であるので、特徴点４５１と、カメラ１ｂ、カメラ１ｃとを結ぶバンドル（トラッキング情報）は、サブマップＭ１からＥＫＦＳＬＡＭによって生成される。
一方、カメラが２ａの位置にある時は、注目サブマップはサブマップＭ２であるので、特徴点４５１と、カメラ２ａとを結ぶバンドル（トラッキング情報）は、サブマップＭ２からＥＫＦＳＬＡＭによって生成される。
特徴点４５１とカメラ１ｂ、カメラ１ｃ、カメラ２ａとを結ぶそれぞれのバンドルは、異なるサブマップから生成されているが、特徴点４５１はサブマップＭ１とサブマップＭ２に属していることがわかっているため、バンドルは特徴点４５１で交わることがわかる。
バンドル調整は、カメラ位置１ｂ，１ｃ，２ａの３つ異なるカメラ位置でのバンドル情報が適用でき、先に図２４を参照しいて説明したバンドル調整が可能となる。
【０１６６】
また、サブマップＭ１とサブマップＭ２に属する特徴点４５２に注目する。特徴点４５２を結ぶバンドル情報（トラッキング結果）は３つあり、それぞれ、カメラ１ｃ、カメラ２ａ、カメラ２ｂとを結んでいる。特徴点４５２と、カメラ１ｃ、カメラ２ａ、カメラ２ｂとを結ぶバンドル（トラッキング情報）は、前述のとおりＥＫＦＳＬＡＭ（図１０のフローのステップＳ１０４）が生成する。ただし、ＥＫＦＳＬＡＭには１つのサブマップのみ入力される。
カメラが１ｃの位置にある時は、注目サブマップはサブマップＭ１であるので、特徴点４５２と、カメラ１ｃとを結ぶバンドル（トラッキング情報）は、サブマップＭ１からＥＫＦＳＬＡＭによって生成される。
一方、カメラが２ａの位置にある時と２ｂの位置にあるは、注目サブマップはサブマップＭ２であるので、特徴点４５２と、カメラ２ａ、カメラ２ｂとを結ぶバンドル（トラッキング情報）は、サブマップＭ２からＥＫＦＳＬＡＭによって生成される。
特徴点４５２とカメラ１ｃ、カメラ２ａ、カメラ２ｂとを結ぶそれぞれのバンドルは、異なるサブマップから生成されているが、特徴点４５２はサブマップＭ１とサブマップＭ２に属していることがわかっているため、バンドルは特徴点４５２で交わることがわかる。
バンドル調整は、カメラ位置１ｃ，２ａ，２ｂの３つのカメラ位置でのバンドル情報が適用でき、先に図２４を参照しいて説明したバンドル調整が可能となる。
【０１６７】
これに対して、図２５と同じ特徴点配置において、サブマップ同士が重ならないようにサブマップを設定した場合の例を図２６に示す。図２５の例と比較すると、図２６の例では、特徴点４５１とカメラ２ａとを結ぶバンドル、特徴点４５２とカメラ１ｃを結ぶバンドルがない。
特徴点４５１とカメラ２ａとを結ぶバンドルがない理由を説明する。バンドル（トラッキング結果）を生成するＥＫＦＳＬＡＭには１つのサブマップのみ入力される。カメラが２ａの場所にあるとき、注目サブマップはＭ２である。このとき、サブマップＭ１にしか属さない特徴点４５１はＥＫＦＳＬＡＭには入力されない。従って、カメラ２ａと特徴点４５１とのバンドル（トラッキング結果）はＥＫＦＳＬＡＭで生成されないため、図２６の例にはない。
同様ではあるが、特徴点４５２とカメラ１ｃとを結ぶバンドルがない理由を説明する。バンドル（トラッキング結果）を生成するＥＫＦＳＬＡＭには１つのサブマップのみ入力される。カメラが１ｃの場所にあるとき、注目サブマップはＭ２である。このとき、サブマップＭ２にしか属さない特徴点４５２はＥＫＦＳＬＡＭには入力されない。従って、カメラ１ｃと特徴点４５２とのバンドル（トラッキング結果）はＥＫＦＳＬＡＭで生成されないため、図２６の例にはない。
図２６において、サブマップＭ１の特徴点４５１のバンドル調整処理を行おうとした場合、カメラ位置１ｂ，１ｃの２つのサブマップＭ１に対応する異なるカメラ位置でのバンドル情報のみしか利用できない。
【０１６８】
同様に、サブマップＭ２の特徴点４５２のバンドル調整処理を行おうとした場合、カメラ位置２ａ，２ｂの２つのサブマップＭ２に対応する異なるカメラ位置でのバンドル情報のみしか利用できない。
図２６では、サブマップＭ１とサブマップＭ２との関連性を表すバンドルが存在しない。そのため、図２４の全体最適化を行うと、サブマップＭ１、サブマップＭ２それぞれ独立に最適化され、サブマップの境界部分（例えば、図２６の特徴点４５１、特徴点４５２、カメラ１ｃ、カメラ２ａそれぞれの間の位置姿勢関係）はバンドル調整（全体最適化）されない問題が発生する。
【０１６９】
結果として、サブマップに重複領域を設定しない場合、サブマップの端部にある特徴点のバンドル調整が出来ない場合が発生する。この場合には、特徴点の３次元位置の精度が低下する可能性がある。このような理由から、サブマップの設定に際しては、隣接するサブマップは共通する特徴点を持つ重複領域を持つように設定することが好ましい。
【０１７０】
以上、特定の実施例を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施例の修正や代用を成し得ることは自明である。すなわち、例示という形態で本発明を開示してきたのであり、限定的に解釈されるべきではない。本発明の要旨を判断するためには、特許請求の範囲の欄を参酌すべきである。
【０１７１】
なお、明細書中において説明した一連の処理はハードウェア、またはソフトウェア、あるいは両者の複合構成によって実行することが可能である。ソフトウェアによる処理を実行する場合は、処理シーケンスを記録したプログラムを、専用のハードウェアに組み込まれたコンピュータ内のメモリにインストールして実行させるか、あるいは、各種処理が実行可能な汎用コンピュータにプログラムをインストールして実行させることが可能である。
【０１７２】
例えば、プログラムは記録媒体としてのハードディスクやＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）に予め記録しておくことができる。あるいは、プログラムはフレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ），ＭＯ（Ｍａｇｎｅｔｏ ｏｐｔｉｃａｌ）ディスク，ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）、磁気ディスク、半導体メモリなどのリムーバブル記録媒体に、一時的あるいは永続的に格納（記録）しておくことができる。このようなリムーバブル記録媒体は、いわゆるパッケージソフトウエアとして提供することができる。
【０１７３】
なお、プログラムは、上述したようなリムーバブル記録媒体からコンピュータにインストールする他、ダウンロードサイトから、コンピュータに無線転送や、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネットといったネットワークを介して、コンピュータに有線で転送し、コンピュータでは、そのようにして転送されてくるプログラムを受信し、内蔵するハードディスク等の記録媒体にインストールすることができる。
【０１７４】
なお、明細書に記載された各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。また、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。
【産業上の利用可能性】
【０１７５】
以上、説明したように、本発明の一実施例の構成によれば、計算量を削減し効率的な３次元位置情報の生成を行う構成が実現される。カメラによる撮影画像を観測データとして入力し、特徴点位置情報を含む多次元正規分布データからなる状態データを更新することで特徴点の３次元位置を求める例えばＳＬＡＭ処理において、３次元情報の生成対象領域を区分したサブマップを設定する。さらにサブマップに属しない特徴点に関するデータを含まないサブマップ対応の状態データを生成して、観測データに基づいてサブマップ対応状態データを更新する。本構成によれば状態データのデータ量が削減され、状態データ更新に伴う計算量を減少させることが可能であり、効率的な処理が実現される。
【符号の説明】
【０１７６】
１０１ カメラ
１０２ 撮影画像
１０３ 特徴点
１０４ トラッキング（軌跡）情報
１２０ オブジェクト
１２１ オブジェクト３次元位置
１３０ カメラ撮影平面画像
１３１ オブジェクト
１３２ オブジェクト画素位置
２１１，２２１ サブマップ
２１２，２２２ サブマップ対応状態データ
３００ 情報処理装置
３０１ カメラ
３０２ 撮影画像記憶部
３０３ サブマップ設定部
３０４ カメラ位置姿勢検出部
３０５ 特徴点追跡部
３０６ カメラ位置姿勢記憶部
３０７ 特徴点軌跡（トラッキング）情報記憶部
３０８ ３次元情報生成部
３０９ 状態データ記憶部
３２０ 記憶部
３５１〜３５３ サブマップ対応状態データ
４１１〜４１３ 撮影画像
４２１〜４２３ 特徴点
４５１，４５２ 特徴点
【特許請求の範囲】
【請求項１】
空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することで、カメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成部と、
３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定部を有し、
前記３次元情報生成部は、
前記サブマップ設定部の設定したサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行して、カメラの位置、姿勢、または各特徴点の３次元位置を求める情報処理装置。
【請求項２】
前記３次元情報生成部は、
前記サブマップ対応状態データとして、
前記カメラの位置、姿勢と、前記サブマップ内の各特徴点の位置の状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データを、各サブマップに対応させて設定し、設定したサブマップ対応の多次元正規分布データを観測情報を適用して更新する請求項１に記載の情報処理装置。
【請求項３】
前記分散共分散行列は、カメラの位置、姿勢、各特徴点の位置情報、これらの固有状態値の分散と、前記各状態値の異なる状態値の組み合わせの相関情報に対応する共分散を含む行列である請求項２に記載の情報処理装置。
【請求項４】
前記３次元情報生成部は、
カルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、または拡張カルマンフィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を適用して前記サブマップ対応状態データの更新処理を実行する請求項１〜３いずれかに記載の情報処理装置。
【請求項５】
前記３次元情報生成部は、
旧サブマップから新サブマップへ、サブマップを切り換えて処理を行う場合、旧サブマップ対応状態データに含まれるカメラ情報をそのまま利用して新サブマップ対応状態データを生成する請求項１に記載の情報処理装置。
【請求項６】
前記サブマップ設定部は、
サブマップに含まれる特徴点数が予め既定した上限数Ｎ以上となった場合、重複領域を持つ２つのサブマップに分割するサブマップ分割処理を実行し、
当該２つのサブマップには少なくとも一つは同一の特徴点を有することを特徴とする請求項１〜５いずれかに記載の情報処理装置。
【請求項７】
前記３次元情報生成部は、
サブマップ分割処理において生成した２つの分割サブマップ各々に対応する２つのサブマップ対応状態データを生成する処理を行う構成であり、
分割前のサブマップのサブマップ対応状態データから、各分割サブマップに含まれない特徴点情報を各々削除して２つの分割サブマップ各々のサブマップ対応状態データを生成する請求項６に記載の情報処理装置。
【請求項８】
前記サブマップ設定部は、
３次元位置の算出対象となる領域を射影変換して生成した２次元平面を予め既定した大きさで、かつ重複領域を持つ複数のサブマップに分割するサブマップ分割処理を実行する請求項１〜５いずれかに記載の情報処理装置。
【請求項９】
前記３次元情報生成部は、
バンドル調整処理による特徴点位置修正処理を実行する請求項１に記載の情報処理装置。
【請求項１０】
前記３次元情報生成部は、
カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を並行して実行するＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理を行う請求項１〜９いずれかに記載の情報処理装置。
【請求項１１】
情報処理装置において実行する情報処理方法であり、
サブマップ設定部が、３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定ステップと、
３次元情報生成部が、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行してカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップである情報処理方法。
【請求項１２】
情報処理装置において情報処理を実行させるプログラムであり、
サブマップ設定部に、３次元位置の算出対象となる領域を分割したサブマップを設定させるサブマップ設定ステップと、
３次元情報生成部に、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求めさせる３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行させてカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップであるプログラム。
【請求項１】
空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することで、カメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成部と、
３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定部を有し、
前記３次元情報生成部は、
前記サブマップ設定部の設定したサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行して、カメラの位置、姿勢、または各特徴点の３次元位置を求める情報処理装置。
【請求項２】
前記３次元情報生成部は、
前記サブマップ対応状態データとして、
前記カメラの位置、姿勢と、前記サブマップ内の各特徴点の位置の状態値を示す平均ベクトルと分散共分散行列を含む多次元正規分布データを、各サブマップに対応させて設定し、設定したサブマップ対応の多次元正規分布データを観測情報を適用して更新する請求項１に記載の情報処理装置。
【請求項３】
前記分散共分散行列は、カメラの位置、姿勢、各特徴点の位置情報、これらの固有状態値の分散と、前記各状態値の異なる状態値の組み合わせの相関情報に対応する共分散を含む行列である請求項２に記載の情報処理装置。
【請求項４】
前記３次元情報生成部は、
カルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、または拡張カルマンフィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を適用して前記サブマップ対応状態データの更新処理を実行する請求項１〜３いずれかに記載の情報処理装置。
【請求項５】
前記３次元情報生成部は、
旧サブマップから新サブマップへ、サブマップを切り換えて処理を行う場合、旧サブマップ対応状態データに含まれるカメラ情報をそのまま利用して新サブマップ対応状態データを生成する請求項１に記載の情報処理装置。
【請求項６】
前記サブマップ設定部は、
サブマップに含まれる特徴点数が予め既定した上限数Ｎ以上となった場合、重複領域を持つ２つのサブマップに分割するサブマップ分割処理を実行し、
当該２つのサブマップには少なくとも一つは同一の特徴点を有することを特徴とする請求項１〜５いずれかに記載の情報処理装置。
【請求項７】
前記３次元情報生成部は、
サブマップ分割処理において生成した２つの分割サブマップ各々に対応する２つのサブマップ対応状態データを生成する処理を行う構成であり、
分割前のサブマップのサブマップ対応状態データから、各分割サブマップに含まれない特徴点情報を各々削除して２つの分割サブマップ各々のサブマップ対応状態データを生成する請求項６に記載の情報処理装置。
【請求項８】
前記サブマップ設定部は、
３次元位置の算出対象となる領域を射影変換して生成した２次元平面を予め既定した大きさで、かつ重複領域を持つ複数のサブマップに分割するサブマップ分割処理を実行する請求項１〜５いずれかに記載の情報処理装置。
【請求項９】
前記３次元情報生成部は、
バンドル調整処理による特徴点位置修正処理を実行する請求項１に記載の情報処理装置。
【請求項１０】
前記３次元情報生成部は、
カメラの位置同定（ローカリゼーション）と周囲の地図（環境地図）の作成（ｍａｐｐｉｎｇ）を並行して実行するＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ）処理を行う請求項１〜９いずれかに記載の情報処理装置。
【請求項１１】
情報処理装置において実行する情報処理方法であり、
サブマップ設定部が、３次元位置の算出対象となる領域を分割したサブマップを設定するサブマップ設定ステップと、
３次元情報生成部が、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求める３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行してカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップである情報処理方法。
【請求項１２】
情報処理装置において情報処理を実行させるプログラムであり、
サブマップ設定部に、３次元位置の算出対象となる領域を分割したサブマップを設定させるサブマップ設定ステップと、
３次元情報生成部に、空間を移動するカメラが撮影した複数の異なる視点からの撮影画像を順次入力し、画像内の特徴点の３次元位置とカメラの位置姿勢情報を含む状態データを、特徴点のトラッキング情報を含む観測情報を適用して更新することでカメラの位置、姿勢、または特徴点の３次元位置を求めさせる３次元情報生成ステップとを有し、
前記３次元情報生成ステップは、
前記サブマップ設定ステップにおいて設定されたサブマップ各々に対して、サブマップ領域外の特徴点情報を含まないサブマップ対応状態データを設定し、設定したサブマップ対応状態データの更新処理を実行させてカメラの位置、姿勢、または各特徴点の３次元位置を求めるステップであるプログラム。
【図２】
【図３】
【図４】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２３】
【図２４】
【図２５】
【図２６】
【図１】
【図５】
【図１１】
【図２１】
【図２２】
【図３】
【図４】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２３】
【図２４】
【図２５】
【図２６】
【図１】
【図５】
【図１１】
【図２１】
【図２２】
【公開番号】特開２０１１−４３４１９（Ｐ２０１１−４３４１９Ａ）
【公開日】平成２３年３月３日（２０１１．３．３）
【国際特許分類】
【出願番号】特願２００９−１９２０９１（Ｐ２００９−１９２０９１）
【出願日】平成２１年８月２１日（２００９．８．２１）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
【公開日】平成２３年３月３日（２０１１．３．３）
【国際特許分類】
【出願日】平成２１年８月２１日（２００９．８．２１）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
[ Back to top ]