(57)【要約】【課題】新しく得られたエッジの影響により変化する各エッジの状態の推定精度を向上させる技術を提供すること。【解決手段】連続するノードと、連続するノードを接続するエッジと、を含むグラフの構造を推定するグラフ構造推定装置は、ノードの位置を含むノード情報と、エッジの状態量とエッジの状態量の分散とを含むエッジ情報を生成する生成部と、ノード情報とエッジ情報とを記憶する記憶部と、を備え、生成部は、さらに、追加エッジ生成条件を満たす場合に、連続しないノードからの追加エッジのエッジ情報を生成すると共に、追加エッジのエッジ情報を用いて、連続するノードのノード情報および連続するノードを接続するエッジのエッジ情報を更新する。【選択図】図１
2020135595.tif 000002
【特許請求の範囲】
【請求項１】
連続するノードと、前記連続するノードを接続するエッジと、を含むグラフの構造を推定するグラフ構造推定装置であって、
前記ノードの位置を含むノード情報と、前記エッジの状態量と前記エッジの状態量の分散とを含むエッジ情報を生成する生成部と、
前記ノード情報と前記エッジ情報とを記憶する記憶部と、
を備え、
前記生成部は、
さらに、
追加エッジ生成条件を満たす場合に、連続しないノードからの追加エッジの前記エッジ情報を生成すると共に、前記追加エッジの前記エッジ情報を用いて、前記連続するノードの前記ノード情報および前記連続するノードを接続するエッジの前記エッジ情報を更新する、
グラフ構造推定装置。
【請求項２】
請求項１に記載のグラフ構造推定装置であって、
前記エッジは、有向エッジである、
グラフ構造推定装置。
【請求項３】
請求項１および請求項２のいずれか一項に記載のグラフ構造推定装置であって、
前記追加エッジは、ループ閉じ込みのエッジである、
グラフ構造推定装置。
【請求項４】
請求項１および請求項２のいずれか一項に記載のグラフ構造推定装置であって、
前記連続しないノードは、他のグラフを構成するノードである、
グラフ構造推定装置。
【請求項５】
請求項１から請求項４のいずれか一項に記載のグラフ構造推定装置であって、
前記生成部は、
ベイジアンフィルタリングを用いて、前記エッジ情報を更新する、
グラフ構造推定装置。
【請求項６】
請求項１から請求項５のいずれか一項に記載のグラフ構造推定装置であって、
前記生成部は、
前記追加ノードが生成され、複数の前記エッジが合流する前記ノードにおいて、前記ノード情報および前記エッジ情報を統合する、
グラフ構造推定装置。
【請求項７】
グラフ構造推定装置を備えるＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｐｉｎｇ）装置であって、
前記グラフ構造推定装置は、連続するノードと、前記連続するノードを接続するエッジと、を含むグラフの構造を推定するグラフ構造推定装置であって、
前記ノードの位置を含むノード情報と、前記エッジの状態量と前記エッジの状態量の分散とを含むエッジ情報を生成する生成する生成部と、
前記ノード情報と前記エッジ情報とが記憶される記憶部と、
を備え、
前記生成部は、
さらに、
追加エッジ生成条件を満たす場合に、連続しないノードからの追加エッジの前記エッジ情報を生成すると共に、前記追加エッジの前記エッジ情報を用いて、前記連続するノードの前記ノード情報および前記連続するノードを接続するエッジの前記エッジ情報を更新する、
ＳＬＡＭ装置。
【請求項８】
連続するノードと、前記連続するノードを接続するエッジと、を含むグラフの構造を推定するグラフ構造推定方法であって、
前記ノードの位置を含むノード情報と、前記エッジの状態量と前記エッジの状態量の分散とを含むエッジ情報を生成する工程と、
追加エッジ生成条件を満たす場合に、連続しないノードからの追加エッジの前記エッジ情報を生成する工程と、
前記追加エッジの前記エッジ情報を用いて、前記連続するノードの前記ノード情報および前記連続するノードを接続するエッジの前記エッジ情報を更新する工程と、
を備える、グラフ構造推定方法。
【請求項９】
連続するノードと、前記連続するノードを接続するエッジと、を含むグラフの構造を推定するためのコンピュータプログラムであって、
前記ノードの位置を含むノード情報と、前記エッジの状態量と前記エッジの状態量の分散とを含むエッジ情報を生成する機能と、
追加エッジ生成条件を満たす場合に、連続しないノードからの追加エッジの前記エッジ情報を生成する機能と、
前記追加エッジの前記エッジ情報を用いて、前記連続するノードの前記ノード情報および前記連続するノードを接続するエッジの前記エッジ情報を更新する機能と、
をコンピュータに、実現させるコンピュータプログラム。
【請求項１０】
記憶媒体であって、
請求項９に記載のコンピュータプログラムを記憶する、
記憶媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、グラフ構造推定装置に関する。
【背景技術】
【０００２】
従来のグラフベースＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｐｉｎｇ）では、グラフ上のエッジの状態を個々に推定した後、最適化を行うことにより、最終的なグラフを出力している。
【先行技術文献】
【特許文献】
【０００３】
【非特許文献１】Ｒ．  Ｍｕｒ−Ａｒｔａｌ他，  “ＯＲＢ−ＳＬＡＭ，”  ＩＥＥＥ  Ｔｒａｎｓ．  ｏｎ  Ｒｏｂｏｔｉｃｓ  ２０１５．
【非特許文献２】Ｒ．  Ｍｕｒ−Ａｒｔａｌ他，  “ＯＲＢ−ＳＬＡＭ２，”  ＩＥＥＥ  Ｔｒａｎｓ．  ｏｎ  Ｒｏｂｏｔｉｃｓ  ２０１７．
【非特許文献３】Ｇ．  Ｇｒｉｓｅｔｔｉ他，  “Ａ  Ｔｕｔｏｒｉａｌ  ｏｎ  Ｇｒａｐｈ−Ｂａｓｅｄ  ＳＬＡＭ，”  ＩＥＥＥ  ＩＴＳ  Ｍａｇａｚｉｎｅ  ２０１０．
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記非特許文献１〜３に開示された技術では、推定後の各エッジの状態（誤差分散など）を正しく評価することができない。そのため、例えば、他のセンサから得られた情報を統合する際等に、精度や信頼性が十分に得られない可能性がある。
【０００５】
本発明は、上述した課題の少なくとも一部を解決するためになされたものであり、新しく得られたエッジの影響により変化する各エッジの状態の推定精度を向上させる技術を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、上述の課題を解決するためになされたものであり、以下の形態として実現することが可能である。
【０００７】
（１）本発明の一形態によれば、連続するノードと、前記連続するノードを接続するエッジと、を含むグラフの構造を推定するグラフ構造推定装置が提供される。このグラフ構造推定装置は、前記ノードの位置を含むノード情報と、前記エッジの状態量と前記エッジの状態量の分散とを含むエッジ情報を生成する生成部と、前記ノード情報と前記エッジ情報とを記憶する記憶部と、を備え、前記生成部は、さらに、追加エッジ生成条件を満たす場合に、連続しないノードからの追加エッジの前記エッジ情報を生成すると共に、前記追加エッジの前記エッジ情報を用いて、前記連続するノードの前記ノード情報および前記連続するノードを接続するエッジの前記エッジ情報を更新する。
【０００８】
この構成によれば、エッジの状態量とエッジの状態量の分散とを含むエッジ情報が、記憶部に記憶される。そして、追加エッジが生成された際に、それまでに生成され、記憶部に記憶されている全てのエッジ情報を、追加エッジのエッジ情報を用いて、更新する。そのため、生成された各エッジの推定誤差分散を含む状態を更新することができる。その結果、得られたグラフ構造の信頼度を測ることができる。また、新しく得られたエッジの影響により変化する各エッジの状態の推定精度を向上させることができる。そのため、他の車（センサ）等により得られた情報と統合する場合の、ロバスト性、信頼度、および精度を向上させることができる。
【０００９】
（２）上記形態のグラフ構造推定装置であって、前記エッジは、有向エッジでもよい。このようにすると、始点ノードからエッジを辿ることにより、ノードの状態を推定することができる。
【００１０】
（３）上記形態のグラフ構造推定装置であって、前記追加エッジは、ループ閉じ込みのエッジでよい。このようにすると、ループ閉じ込みを行った際に、それまでに生成したノードとエッジの状態を更新することができる。
【００１１】
（４）上記形態のグラフ構造推定装置であって、前記連続しないノードは、他のグラフを構成するノードでもよい。このようにすると、例えば、自身のグラフを生成する際に用いているセンサ（例えば、画像生成装置等）と異なるセンサによる情報を用いて、自身が生成したグラフの状態を更新することができる。
【００１２】
（５）上記形態のグラフ構造推定装置であって、前記生成部は、ベイジアンフィルタリングを用いて、前記エッジ情報を更新してもよい。このようにすると、精度よくエッジの状態を更新することができる。
【００１３】
（６）上記形態のグラフ構造推定装置であって、前記生成部は、前記追加ノードが生成され、複数の前記エッジが合流する前記ノードにおいて、前記ノード情報および前記エッジ情報を統合してもよい。このようにすると、追加エッジも含めて、全てのノードとエッジの状態を適切に更新することができる。
【００１４】
なお、本発明は、種々の態様で実現することが可能であり、例えば、グラフ構造推定装置を備えるＳＬＡＭ装置、グラフ構造推定方法、この方法をコンピュータに実行させるコンピュータプログラム、そのプログラムを記憶する記憶媒体、および、そのコンピュータプログラムを含み搬送波内に具現化されたデータ信号等の形態で実現することができる。
【図面の簡単な説明】
【００１５】
【図１】本発明の一実施形態としてのグラフ構造推定装置の構成を機能的に示すブロック図である。
【図２】グラフ構造推定装置によって推定されるグラフを概念的に示す図である。
【図３】グラフ構造推定処理の流れを示すフローチャートである。
【図４】三次元ＯＲＢ特徴の計算を説明するための説明図である。
【図５】ループ閉じ込みにおける観測誤差を概念的に示す説明図である。
【発明を実施するための形態】
【００１６】
本実施形態では、ＳＬＡＭ装置がグラフ構造推定装置を備え、グラフ構造推定装置を用いて、車載のステレオカメラによるＶｉｓｕａｌ  ＳＬＡＭを行う例を用いて、説明する。本実施形態におけるＶｉｓｕａｌ  ＳＬＡＭでは、ステレオカメラから入力される画像シーケンスから、基準となるキーフレームを決定し、キーフレーム間の移動量を推定する。グラフ構造推定装置は、キーフレームの位置をノードとし、キーフレーム間の移動をエッジとしたグラフを推定する。このグラフによって、カメラの移動軌跡、すなわち車の移動軌跡を推定する。
【００１７】
図１は、本発明の一実施形態としてのグラフ構造推定装置１０の構成を機能的に示すブロック図である。グラフ構造推定装置１０は、例えばパーソナルコンピュータ等の情報処理装置であって、制御部１１、記憶部１２、通信Ｉ／Ｆ部１５、入力Ｉ／Ｆ部１３、出力Ｉ／Ｆ部１４、バス１６を含む。なお、本明細書において、インターフェースを「Ｉ／Ｆ」と称する。バス１６は、制御部１１、記憶部１２、通信Ｉ／Ｆ部１５、入力Ｉ／Ｆ部１３、出力Ｉ／Ｆ部１４を電気的に接続する信号線である。
【００１８】
制御部１１は、グラフ構造推定装置全体の動作を制御する電子装置であり、例えば中央演算装置（ＣＰＵ）であってもよい。制御部１１は、後述するプログラム１２３を実行することにより、生成部１１１として機能する。
【００１９】
生成部１１１は、グラフを構成するノードに関するノード情報１２１を生成し、記憶部１２に記憶させる。具体的には、生成部１１１は、画像生成装置（ステレオカメラ）２０から入力される画像シーケンスから基準となるキーフレームを決定し、キーフレームの位置をノードとする。ノード情報１２１は、位置情報を含む。
【００２０】
また、生成部１１１は、ノード間を接続するエッジに関するエッジ情報１２２を生成し、記憶部１２に記憶させる。エッジ情報は、エッジの状態量（ベクトル情報）と、状態量の分散を含む。具体的には、生成部１１１は、上記キーフレーム間の移動をエッジとする。生成部１１１は、移動量を算出する際に得られる移動の分散を、ベクトル情報に含める。すなわち、本実施形態のグラフ構造推定装置１０では、生成された全てのノードとエッジに関するノード情報１２１とエッジ情報１２２とが、記憶部１２に記憶される。
【００２１】
記憶部１２は、情報を記憶するメモリ装置の総称であり、リードオンリーメモリ（ＲＯＭ）、ランダムアクセスメモリ（ＲＡＭ）、ソリッドステートドライブ（ＳＳＤ）、ハードディスク、フラッシュメモリカードなど様々な形式の記憶装置を使用することができる。記憶部１２には、オペレーティングシステム（ＯＳ）や本実施形態に係るプログラム１２３が格納されている。また、上述のノード情報１２１、およびエッジ情報１２２が格納される。図１では、予め格納されているものと区別するために、ノード情報１２１とエッジ情報１２２とを、破線囲んで図示している。
【００２２】
通信Ｉ／Ｆ部１５は、無線または有線により画像生成装置２０やその他の装置と通信を可能にする通信インターフェースである。入力Ｉ／Ｆ部１３は、キーボード等の入力装置（不図示）や、ＵＳＢメモリ装置やフラッシュメモリ等の記録装置等から入力された指令やデータを、制御部１１や記憶部１２へ伝達するインターフェースである。出力Ｉ／Ｆ部１４は、表示装置やプリンタ等の出力装置（不図示）や、ＵＳＢメモリ装置及びフラッシュメモリ等の記録装置等に文字や画像等を出力するインターフェースである。
【００２３】
本実施形態で説明する処理を実現するプログラム１２３は、プログラム提供者側から通信ネットワーク及び通信Ｉ／Ｆ部１５を介して、例えば記憶部１２に格納されてもよい。また、上記実施形態で説明した処理を実現するプログラムは、市販され、流通している可搬型記憶媒体に格納されていてもよい。この場合、この可搬型記憶媒体は外付け又は内蔵の読取装置にセットされて、制御部１１によってそのプログラムが読み出されて、実行されてもよい。可搬型記憶媒体としてはＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、フレキシブルディスク、光ディスク、光磁気ディスク、ＩＣカード、ＵＳＢメモリ装置など様々な形式の記憶媒体を使用することができる。このような記憶媒体に格納されたプログラムが読取装置によって読み取られる。
【００２４】
図２は、グラフ構造推定装置１０によって推定されるグラフＧ１を概念的に示す図である。グラフＧ１は、連続するノードＮと、連続するノードＮを接続するエッジＥと、を含む。図２では、複数のノードＮを区別するために、連続するノードＮを、ノードＮが生成された順に、１から連番を符号の末尾に付して示している。ここで、連続して生成されたノードを、「連続するノード」と呼んでいる。また、エッジＥについては、始点ノードＮの番号を、符号の末尾に付している。例えば、ノードＮ１を始点とし、ノードＮ２を終点とするエッジＥに、「Ｅ１」の符号を付している。上述の通り、ノードＮがキーフレームの位置、エッジＥがキーフレーム間の移動に、それぞれ、対応する。
【００２５】
図３は、グラフ構造推定処理の流れを示すフローチャートである。本実施形態のＶｉｓｕａｌ  ＳＬＡＭでは、新しい画像が得られる度に最新のキーフレームからの移動量を更新し、条件を満たしていれば新しいキーフレームを生成する．新しいキーフレームを生成した際、ループ閉じ込みの判定を行い、ループ閉じ込みがある場合、その情報を用いてグラフ全体を更新する．
【００２６】
グラフ構造推定装置１０が、グラフ構造推定処理開始の指示を受け付けると、グラフ構造推定処理が開始される。
まず、制御部１１は、初期キーフレームを生成する（ステップＳ１０２）。詳しくは、画像生成装置２０から入力される、カメラの初期位置を示す画像を用いて、初期キーフレームを生成する。制御部１１は、初期キーフレームの位置を、ノードＮ１（図２）としてノード情報１２１を生成し、記憶部１２に記憶させる。
【００２７】
制御部１１は、画像生成装置２０から画像を取得し（ステップＳ１０４）、移動量を推定する（ステップＳ１０６）。
【００２８】
以下に、最新のキーフレームからの移動量の推定について図４を用いて説明する．本実施形態では、ＯＲＢ（Ｏｒｉｅｎｔｅｄ  ＦＡＳＴ  ａｎｄ  Ｒｏｔａｔｅｄ  ＢＲＩＥＦ）特徴量を用いたＥｘｔｅｎｄｅｄ  Ｉｎｆｏｒｍａｔｉｏｎ  Ｆｉｌｔｅｒ（ＥＩＦ）を用いて、移動量を推定する。
【００２９】
図４は、三次元ＯＲＢ特徴の計算を説明するための説明図である。図４では、画像生成装置２０から入力されるステレオ画像（左画像ＩＬと右画像ＩＲ）と、各画像のＯＲＢ特徴量ＶＬ、ＶＲと、計算された三次元上のＯＲＢ特徴Ｖ３の一例を図示している。
【００３０】
制御部１１は、まず、ステップＳ１０４において得られた左画像ＩＬ、右画像ＩＲそれぞれについてＯＲＢ特徴量を計算する。計算したＯＲＢ特徴量を左右の画像で対応付けることにより、ステレオ視を用いたＯＲＢ特徴量の三次元位置を推定する。図４に示すように、左画像ＩＬの特徴量ＶＬと右画像ＩＲの特徴ＶＲとを対応付けることにより、三次元上のＯＲＢ特徴Ｖ３の三次元位置を算出する。
【００３１】
次に、制御部１１は、得られた三次元ＯＲＢ特徴を最新のキーフレームの三次元ＯＲＢ特徴と対応付け、移動量の計算をする。移動量を表すキーフレーム間のエッジの状態量を以下のように定義する。
【数１】
2020135595.tif 000003
【００３２】
エッジの状態量の予測モデルを、以下のように定義する。
【数２】
2020135595.tif 000004
【００３３】
エッジの状態量の分散をＶとし、その予測モデルを、以下のように定義する。
【数３】
2020135595.tif 000005
【００３４】
移動量の推定は画像シーケンスのサンプリング間隔に基づいて状態量を予測した後、観測により更新することにより行われる。
【００３５】
【数４】
2020135595.tif 000006
【００３６】
これより、観測モデルを以下のように定義する。
【数５】
2020135595.tif 000007
【００３７】
ＥＩＦでは、以下のように状態量を変換する。
【数６】
2020135595.tif 000008
このＥＩＦは複数の特徴量が対応付けられた場合、それらを順に計算することができる。
以上説明した通り、制御部１１は、移動量を推定する。
【００３８】
次に、制御部１１は、新キーフレームを生成するか否かを判定する（ステップＳ１０８）。本実施形態において、制御部１１は、ステップＳ１０６で算出した移動量が閾値以上になった場合に、新キーフレームを生成すると判定する。他の実施形態では、例えば、画像シーケンスでのフレーム数が、閾値以上になった場合に、新キーフレームを生成すると判定してもよい。制御部１１は、新キーフレームを生成すると判定されるまで、ステップＳ１０４、ステップＳ１０６を繰り返す。
【００３９】
制御部１１は、ステップＳ１０８において、新キーフレームを生成すると判定すると、新キーフレーム生成の判定に用いられた移動量の算出に用いた画像（ステップＳ１０４において入力された画像）を用いて、新キーフレームを生成する（ステップＳ１１０）。例えば、初期キーフレームの次に生成された新キーフレームに対応するノードＮは、第２ノードＮ２（図２）である。そして、新キーフレーム生成の判定に用いられた移動量に対応するエッジＥ１を生成する。エッジＥ１のエッジ情報１２２は、エッジＥ１の状態量ｘと、分散Ｖを含む。
【００４０】
制御部１１は、新キーフレームを生成すると（ステップＳ１１０）、ループ閉じ込みを行うか否かを判定する（ステップＳ１１２）。具体的には、制御部１１は、前に生成されたキーフレームの中で新キーフレームと近い画像があれば、ループ閉じ込みを行うと判定する。近い画像の検索に関しては、例えば、Ｂａｇｓ  ｏｆ  Ｂｉｎａｒｙ  Ｗｏｒｄｓ等の手法を用いることができる。本実施形態における、「前に生成されたキーフレームの中で新キーフレームと近い画像があること」を、「追加エッジ生成条件」とも呼ぶ。
【００４１】
制御部１１は、ステップＳ１１２において、ループ閉じ込みを行うと判定すると、新フレームと近い画像の過去のキーフレーム（ノード）との間のループ閉じ込みエッジを計算する。例えば、対応づいた過去のキーフレームを基準として新しいキーフレームまでのエッジを上記のＥＩＦなどを用いて計算する。得られたエッジを観測としてＤｅｃｏｕｐｌｅｄ  Ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ  （ＤＥＫＦ）によりグラフ上のエッジすべてを更新する。
【００４２】
以下に、ループ閉じ込みエッジの生成について、図５を用いて説明する。
図５は、ループ閉じ込みにおける観測誤差を概念的に示す説明図である。図５では、ループ閉じ込みエッジを通るパスＰ１を、破線で図示し、ループ閉じ込みエッジを通らないパスＰ２を、一点鎖線で図示している。図５に示す例では、制御部１１は、ステップＳ１０２（図３）においてノードＮ１を生成した後、ステップＳ１０４〜１１２を繰り返し、ノードＮ２〜Ｎ１３、およびそれらのノードＮを接続するエッジＥ１〜Ｅ１３等を生成している。制御部１１は、ステップＳ１１０において、ノードＮ１４と、エッジＥ１３を生成した後、ループ閉じ込み判定（ステップＳ１１２）において、ループ閉じ込みを行うと判定し、ループエッジＲＥ３を生成する（ステップＳ１１４）。ここで、ループエッジＲＥ３は、ノードＮ３からノードＮ１４に向かうエッジである。
【００４３】
制御部１１は、ループ閉じ込みエッジ（ループエッジＲＥ３）を通るパスＰ１と、ループ閉じ込みエッジ（ループエッジＲＥ３）を通らないパスＰ２を用いて、それぞれ、ノードＮ１４の位置を算出し、その誤差を用いて、ノードＮ１〜Ｎ１４と、それらの間を繋ぐ全てのエッジＥ１〜Ｅ１３を更新する。すなわち、記憶部１２に記憶されているノードＮ１〜Ｎ１４に関するノード情報１２１と、ノードＮ１〜Ｎ１３の間を繋ぐエッジＥ１〜Ｅ１３に関するエッジ情報１２２を更新する。以下に、詳細に説明する。
【００４４】
観測誤差は、図５に示されるようにループ閉じ込みエッジ（ループエッジＲＥ３）を通るパスＰ１と、ループ閉じ込みエッジ（ループエッジＲＥ３）を通らないパスＰ２で、それぞれ計算した新しいキーフレームの位置（および姿勢）の誤差として得られる。
【００４５】
【数７】
2020135595.tif 000009
【００４６】
ただし、キーフレームがループ閉じ込みエッジを持つ場合、一つのノードに複数のエッジが接続される。制御部１１は、それぞれのエッジに基づいて計算した値を統合するためにＣｏｖａｒｉａｎｃｅ  Ｉｎｔｅｒｓｅｃｔｉｏｎ  （ＣＩ）を用いる。例えば、図５において、ノードＮ１４には、エッジＥ１３と、ループエッジＲＥ３が接続される。
【００４７】
【数８】
2020135595.tif 000010
【００４８】
【数９】
2020135595.tif 000011
これにより、ループ閉じ込みエッジの情報を用いて、グラフ上のすべてのエッジの状態量と、エッジの状態量の分散を更新することができる。
【００４９】
図５では、ループ閉じ込みエッジ（ループエッジＲＥ３）を通らないパスＰ２で計算した最新（１４番目）のノードＮ１４ｆと、ループ閉じ込みエッジ（ループエッジＲＥ３）を通るパスＰ１で計算した最新（１４番目）のノードＮ１４ｃの位置を破線で示す。
【００５０】
制御部１１は、得られた軌跡（グラフ）を出力する前に、各キーフレームの位置をスムージングして、軌跡を修正する（ステップＳ１１８）。例えば、スムージングの処理は一般的なカルマンスムージングを用いて以下のように行うことができる。
【００５１】
【数１０】
2020135595.tif 000012
【００５２】
制御部１１は、スムージングにより修正された軌跡を出力する（ステップＳ１２０）。制御部１１は、ステレオカメラ（画像生成装置２０）を搭載した車両の走行軌跡と共に、地図を出力する。
【００５３】
以上説明したように、本実施形態のグラフ構造推定装置１０によれば、グラフ構造推定装置１０によって生成された全てのエッジを、状態量（三次元ベクトル）と状態量の分散を含むエッジ情報として保持している。そして、エッジ情報に対して、Ｄｅｃｏｕｐｌｅｄ  Ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒによりフィルタリングすることで、過去に獲得されたエッジの状態（エッジ情報）を更新することができる。すなわち、本実施形態のグラフ構造推定装置１０によれば、各ノードＮの位置だけでなく、その分散や、各エッジの状態量とその分散を、同時に推定することができる。
【００５４】
また、過去に得られたループ閉じ込みエッジの影響はＣｏｖａｒｉａｎｃｅ  Ｉｎｔｅｒｓｅｃｔｉｏｎにより統合することで、これらのエッジも含めたすべてのエッジ状態の更新を行うことができる。
【００５５】
本実施形態のグラフ構造推定装置１０によれば、ループ閉じ込みエッジが得られた際に、これまでに獲得されたエッジの状態（エッジ情報）を更新する。すなわち、生成された各エッジの推定誤差分散を含む状態を更新することができるため、得られたグラフ構造の信頼度を測ることができる。その結果、例えば、他の車（センサ）等により得られた情報と統合する場合の、ロバスト性、信頼度、および精度を向上させることができる。
【００５６】
＜本実施形態の変形例＞
本発明は上記の実施形態に限られるものではなく、その要旨を逸脱しない範囲において種々の態様において実施することが可能であり、例えば次のような変形も可能である。
【００５７】
・上記実施形態において、グラフ構造推定装置１０を用いるグラフベースＳＬＡＭ装置を例示したが、上記実施形態に限定されない。例えば、ＳＮＳ（ｓｏｃｉａｌ  ｎｅｔｗｏｒｋｉｎｇ  ｓｅｒｖｉｃｅ，  ソーシャル・ネットワーキング・サービス）において、グラフ構造推定装置１０を用いてもよい。例えば、ワールドワイドウエブ（ＷＷＷ）のリンクの関係をグラフで表現する場合等に、用いることができる。
【００５８】
・上記実施形態では、有向グラフを更新する例を示したが、無向グラフに対して適用してもよい。無向グラフに対して適用する場合は、エッジに適切な方向（古いノードから新しいノードへ）を与えることにより、適用することができる。
【００５９】
・上記実施形態において、複数のエッジが合流するノードにおいてそれらの情報を、Ｃｏｖａｒｉａｎｃｅ  Ｉｎｔｅｒｓｅｃｔｉｏｎ  により統合する例を示したが、他の方法によって統合してもよい。
【００６０】
・上記実施形態において、状態更新のベイジアンフィルタリングに、Ｄｅｃｏｕｐｌｅｄ  Ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ  を用いる例を示したが、他のフィルターを用いても良い。さらに、ベイジアンフィルタリング以外を用いてもよい。
【００６１】
以上、実施形態、変形例に基づき本態様について説明してきたが、上記した態様の実施の形態は、本態様の理解を容易にするためのものであり、本態様を限定するものではない。本態様は、その趣旨並びに特許請求の範囲を逸脱することなく、変更、改良され得ると共に、本態様にはその等価物が含まれる。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することができる。
【符号の説明】
【００６２】
１０…グラフ構造推定装置
１１…制御部
１２…記憶部
１３…入力Ｉ／Ｆ部
１４…出力Ｉ／Ｆ部
１５…通信Ｉ／Ｆ部
１６…バス
２０…画像生成装置
１１１…生成部
１２１…ノード情報
１２２…エッジ情報
１２３…プログラム
Ｅ…エッジ
Ｇ１…グラフ
ＩＬ…左画像
ＩＲ…右画像
Ｐ１、Ｐ２…パス
ＲＥ３…ループエッジ
【図１】
2020135595.tif 000013
【図２】
2020135595.tif 000014
【図３】
2020135595.tif 000015
【図４】
2020135595.tif 000016
【図５】
2020135595.tif 000017