JP2020067439A - 移動体位置推定システムおよび移動体位置推定方法 - Google Patents

Abstract

【課題】高精度な位置推定をおこなうこと。【解決手段】Ｖ−ＳＬＡＭを用いた移動体位置推定をするにあたり、撮影された時系列画像の任意の画像につき、任意の画像の第１の撮影位置（ＧＮＳＳ値Ｇ１〜Ｇ７）を測位により取得し、任意の画像の特徴から、当該任意の画像の第２の撮影位置（キーフレームＫＦ１〜ＫＦ７）を推定し、第１の撮影位置（ＧＮＳＳ値Ｇ１〜Ｇ７）と、第２の撮影位置（キーフレームＫＦ１〜ＫＦ７）とのずれを最小化する拘束条件（Ｃ７）を用いて、任意の画像の撮影位置・姿勢や、任意の画像の中で抽出された特徴点の推定された３次元位置を調整する。ＫＦ１とＫＦ２は、姿勢推定対象最新ＫＦ７から時間的に遠く共通特徴点もないので、最適化補正計算の対象外とし、ＫＦ３は、時間的に近いが、共通特徴点がないので、位置固定で補正計算に入れる参考ＫＦとし、ＫＦ４〜ＫＦ７は、位置が近く、共通特徴点があるので、補正対象ＫＦ群とする。【選択図】図３

Description

本発明は、移動体位置推定システムおよび移動体位置推定方法に関する。

従来、移動体が移動中に取得した周辺状況に関するデータを入力とし、移動体の走行経路と周辺環境地図を同時に作成するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）という技術がある。また、ＳＬＡＭ技術の中でも、移動体が撮影した映像を入力として、移動体の走行時のカメラ位置・姿勢を推定する技術は、Ｖｉｓｕａｌ−ＳＬＡＭ（以後、「Ｖ−ＳＬＡＭ」という）と呼ばれる。

関連する先行技術としては、ＳＬＡＭ技術を用いて移動体に搭載されるカメラで撮像した画像に基づき移動体の位置を推定する場合に、ＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）などで取得した情報を参照し、ＳＬＡＭ技術で算出した移動体の位置・姿勢のうちの位置を後補正する技術がある。

特開２０１８−０２８４８９号公報

しかしながら、従来技術では、マップが歪み、正しい環境地図を作成できず、環境地図を元にした位置・姿勢推定に誤りが生じる、いわゆる「スケールドリフト」の問題を十分に解消できず、また、処理コストが高く、補正した推定撮影位置・姿勢の精度が低いという問題がある。

一つの側面では、本発明は、高精度な位置姿勢推定をおこなうことを目的とする。

一つの実施態様では、撮影された時系列画像のうちの任意の画像について、当該任意の画像の第１の撮影位置を測位により取得し、前記任意の画像の特徴から、当該任意の画像の第２の撮影位置を推定し、前記第１の撮影位置と、前記第２の撮影位置とのずれを最小化する拘束条件を用いて、前記任意の画像の撮影位置、前記任意の画像の撮影姿勢、および、前記任意の画像の中で抽出された特徴点の推定された３次元位置、の少なくともいずれか一つを調整する、情報処理装置を有する移動体位置推定システムが提供される。

本発明の一側面によれば、高精度な位置姿勢推定をおこなうことができる。

図１は、スケールドリフトの一例を示す説明図である。 図２は、ループクローズの一例を模式的に示す説明図である。 図３は、実施の形態にかかる移動体位置推定方法における姿勢グラフと最適化の一例を模式的に示す説明図である。 図４は、実施の形態にかかる移動体位置推定方法における姿勢グラフと最適化の別の一例を模式的に示す説明図である。 図５は、実施の形態にかかる移動体位置推定システムのシステム構成の一例を示す説明図である。 図６は、移動体位置推定装置（サーバ）のハードウェア構成の一例を示すブロック図である。 図７は、車載機のハードウェア構成の一例を示すブロック図である。 図８は、実座標環境マップのデータ構成の一例を示す説明図である。 図９は、全画像位置姿勢データのデータ構成の一例を示す説明図である。 図１０は、実施の形態にかかる移動体位置推定システム、移動体位置推定方法の内容の一例を示す説明図である。 図１１Ａは、初期姿勢・座標系設定部における変換行列算出の一例を示す説明図（その１）である。 図１１Ｂは、初期姿勢・座標系設定部における変換行列算出の一例を示す説明図（その２）である。 図１１Ｃは、初期姿勢・座標系設定部における変換行列算出の一例を示す説明図（その３）である。 図１１Ｄは、スケール変換行列Ｍ１の算出の一例を示す説明図である。 図１１Ｅは、回転変換行列Ｍ２の算出の一例を示す説明図である。 図１２は、ＫＦ（キーフレーム）更新部の処理の手順の一例を示すフローチャートである。 図１３は、グラフ制約生成部の処理の手順の一例を示すフローチャートである。

以下に図面を参照して、本発明にかかる移動体位置推定システムおよび移動体位置推定方法の実施の形態を詳細に説明する。

（実施の形態）
まず、図１〜図４を用いて、本実施の形態にかかる移動体位置推定システムおよび移動体位置推定方法の概要について説明する。

移動する一般車の車載機のデータ（たとえば映像データなど）は、大量に収集（プローブ）され、車載データ解析に用いられているが、一般的に搭載されているＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）装置では誤差の大きな自車位置測定しかできないため、詳細な自車位置を必要とするサービスには適用することができない。

この一般車のデータに対し、たとえば走行中の撮影時の位置・姿勢を高精度に推定してその車載画像に付加するならば、画像から走路周辺の地物を抽出して自動運転向け地図などの地図を作成更新したり、自動運転向けに撮影時の周辺状況を解析したりする新しいサービス分野に応用することができる。このため、これら一般車映像を使った新しいサービス利用の前提として、車載画像を撮影したカメラ位置・姿勢（映像の撮影位置・姿勢）を正確に推定する技術が必要とされている。

ＳＬＡＭは、移動中に取得した周辺状況に関する車載データ、たとえばＬＩＤＡＲ（Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）データなどを入力とし、自車走行経路（自車の位置と姿勢）と周辺環境地図（周辺物の３次元位置マップなど）を同時に作成する技術の総称である。なかでも、Ｖ−ＳＬＡＭは、車載カメラで撮影した映像を入力とし、撮影した映像に映る被写体の変化を用いて、自車走行経路（自車位置・姿勢）と周辺環境地図（周辺の被写体の画像特徴点群の３次元位置マップ、以下、環境地図という）を推定作成することができる技術であり、一般車の映像から自車位置と姿勢を推定することができる。

ところで、一般的にＳＬＡＭは、環境地図を同時作成しながら姿勢推定を逐次的におこなう技術であるため、長距離での誤差蓄積や、大きく見えが変化する場所の調整失敗でスケール（縮尺）変動が発生する。それにより、上述のような、マップが歪み、正しい環境地図を作成できず、環境地図を元にした位置・姿勢推定に誤りが生じる、いわゆる「スケールドリフト」と呼ばれる問題が発生する。

図１は、スケールドリフトの一例を示す説明図である。図１において、左右の図はいずれも経路図を示している。左図は、（ａ）ＧＰＳで実際に測定した、地図上の実座標系１０１を示す経路図であり、右図は、（ｂ）ＳＬＡＭ実行結果１０２に基づく経路図である。

地図上の実座標系１０１を示す経路図において、走行経路１１１は実線で示されているのに対して、ＳＬＡＭ実行結果１０２に基づく経路図においては、走行経路１１２は、当該走行経路１１２上で撮影した各画像の推定撮影位置を結んだ棒線で示されている。また、走行経路１１２の周辺に散らばる細かい点群は、周辺の被写体の画像特徴群の３次元位置マップ、すなわち、周辺環境地図である。このＳＬＡＭ実行結果１０２は、特に実座標系とは無関係なＳＬＡＭローカル座標系での推定走行軌跡を、走行経路１１１に合わせて全体拡大および回転させて並べて示したものである。

図１からもわかるように、地図上の実座標系１０１を示す走行経路１１１に比べて、ＳＬＡＭ実行結果１０２に基づく走行経路１１２、すなわち、Ｖ−ＳＬＡＭで推定した走行経路（走行画像の撮影位置を繋いだもの）が大きく異なっている。特に、経路で囲まれたエリアＡとＢの大きさが両者で異なっており、ＳＬＡＭ実行結果１０２において、Ｖ−ＳＬＡＭ実行時には、エリアＡとエリアＢの近傍で、局所的にスケールが大きく異なっている（スケールドリフトが発生している）ことがわかる。

ＳＬＡＭには、このスケールドリフトの課題に対し、走行中に同じ場所（ループ）を通過することを検知し、該場所で複数回の通過時の推定位置・姿勢や環境地図がすべて整合性がとれるようＳＬＡＭ推定結果を補正してスケールドリフトを改善する「ループクローズ」という技術がある。

Ｖ−ＳＬＡＭでは、すべての画像の撮影位置・姿勢推定を実施するが、特に主要な画像をキーフレーム（「ＫＦ」）と呼ぶ。そして、このＫＦに対してのみ、撮影位置・姿勢を大域／局所の双方で矛盾が無いように、環境地図を用いた詳細な解析技法で、環境地図自体も特徴点群の追加や位置変更などの更新調整をおこないながら、その撮影位置・姿勢を推定する。残る画像群の撮影位置・姿勢は、環境地図を更新せずに、ＫＦからの相対関係を使って簡易に推定する、という方法が取られることが多い。

図２は、ループクローズの一例を模式的に示す説明図であり、ＫＦの位置・姿勢推定時のループクローズ技術の適用例を示している。図２において、各ＫＦを三角形で示している。符号２０１に示すように、（１）ＳＬＡＭでＫＦどうしの類似性、たとえばＶ−ＳＬＡＭならＫＦ画像どうしの類似性などから同一場所であるループを検出する。具体的には、たとえば、新たに作成されたＫＦと、過去のＫＦとの間で画像類似などからループを検出する。符号２０１では、ループを検出した状態を示している。

つぎに、符号２０２に示すように、（２）連続するＫＦどうしの関係を適度の近さで隣り合うよう拘束する通常のＳＬＡＭの位置・姿勢推定の最適化計算時の拘束条件に加えて、発見したループ箇所のＫＦどうしが近くなるような拘束条件を追加する。符号２０２において、点線で示した部分が拘束条件を示している。そして、符号２０３に示すように、（３）追加した拘束条件を含めた新たな拘束条件群で、ループ近傍のＫＦ群および環境地図の補正をおこなう。

この結果、ループ近傍のＫＦの位置・姿勢が補正されるため、ループ近傍のスケール誤差は解消するが、ループから離れたＫＦのスケール誤差は解消されないし、必ずしも走行時にループが発生するとは限らないため、ループクローズによるスケールドリフト改善効果は限定的であるという課題（第１の課題）がある。すなわち、ループの辻褄が合うように全体が補正されるが、局所的な縮尺変動は補正されないという問題点がある。また、そもそも、ループ走行しないと適用できないという問題点がある。

また、ＳＬＡＭは、初期画像からの差分で環境地図を作成するため、実座標位置（緯度経度、実世界スケール）とは無関係な、その映像独自のローカル系（ＳＬＡＭローカル系）での値となる。そのため、実世界の地図と対応付けた活用が困難であり、上述した一般車映像を使ったサービス利用に活用するのには難しいという課題（第２の課題）がある。

従来、ＳＬＡＭは、ロボットなどの自律移動のために開発され、使い捨ての環境地図を作成しながら周辺物体を避けた移動ができることが重要で、移動経路自体の正確さ（絶対値としての大きさの把握や、スケール固定など）はあまり重要でなかった、という経緯があるため、一般車映像を使ったサービス利用向けの撮影位置・姿勢推定に適用するには、新たに上記第１の課題および第２の課題の２つを解決することが必要である。

これに対し、先行技術におけるＶ−ＳＬＡＭによる撮影位置・姿勢推定では、Ｖ−ＳＬＡＭ特有の第１の課題であるスケールドリフトを防ぐため、Ｖ−ＳＬＡＭを実行後に、Ｖ−ＳＬＡＭで算出した環境マップや推定撮影位置を、Ｖ−ＳＬＡＭの入力映像と同時に別途取得したＧＮＳＳ高精度位置を用いて、局所的な線形変換を用いて逐次的に補正する。

しかしながら、この方法では、補正に用いる位置座標変換は、規定距離離れた２地点の位置対による局所的な線形変換（アフィン変換）になるため、大まかな補正しかできず、右左折のような右左折後に特に大きくスケールが変わるようなシーンであっても、右左折前後の２地点を選定してしまう可能性があり、右左折前の比較的スケールが正しい画像に対しても、大きくスケールを間違えた右左折後の画像向けの大きな補正を含んだ位置座標変換がかかるため、補正精度が低くなる。

また、本来は撮影位置が変われば、見え方も変化するため、撮影位置の変更は姿勢にも影響するが、Ｖ−ＳＬＡＭ後の補正は、実際には位置補正しかおこなわないため、推定した姿勢はまったく補正できないという問題がある。

さらに、Ｖ−ＳＬＡＭの算出後に補正処理をおこなう必要があるため、Ｖ−ＳＬＡＭ処理以外の処理コストがかかる。

このように、Ｖ−ＳＬＡＭを用いた従来技術では、実座標系の撮影位置・姿勢を補正算出する処理コストが高く、補正した推定撮影位置・姿勢の精度も低いという問題があった。本発明は、これらの処理コストと推定精度悪化という課題を解決することを目的とする。

本発明は、図３と図４に示す実施の形態にかかる移動体位置推定方法により、３Ｄ特徴点と走行軌跡の最適化（誤差最小化）計算にＧＮＳＳ情報を含めて、このＧＮＳＳ情報を用いて、（さらに、シーン変化に基づいた適切なＫＦを使って、）逐次補正をおこなうものであり、これにより、走行距離が伸びても縮尺変動しない補正処理をおこなうことができるため、広域でのスケールドリフトを精度良く回避することができる。

本実施では、画像解析による見えの変化と、ＧＮＳＳ位置の変化の双方を、無理なくＶ−ＳＬＡＭのキーフレーム（ＫＦ）の位置・姿勢および周辺の環境マップの特徴点群位置の最適化計算に反映させるために、まずはＧＮＳＳ位置を用いたＫＦの位置・姿勢最適化（図３）で大まかにＧＮＳＳ位置に合わせこみをおこない、続けてＧＮＳＳ位置を用いて再度のＫＦの位置・姿勢と環境マップの特徴点群の位置の双方に対する最適化（図４）で詳細にＧＮＳＳ位置に合わせる、二段階の処理をおこなっている。しかし、後述するようにこれは一例であって、逐次的に最適化を実施するときに、前段のＫＦの位置・姿勢のみの最適化を常に、または時折省略して、そのときは後段のマップ特徴点群も含めた最適化だけを実施するようにしても構わない。

また、とくに記載がなければ、以後のＫＦの最適化はＫＦの位置・姿勢の最適化を示すが、位置だけを最適化しても構わない。たとえば、前段は位置だけを最適化し、後段では位置・姿勢を最適化してもよい。

以下では、この二段階の処理を、図３、図４を用いて順に説明する。

図３は、実施の形態にかかる移動体位置推定方法における姿勢グラフと最適化の一例を模式的に示す説明図である。図３では、後述するグラフ制約生成部５３２において新たに作成する２つの最適化に用いる姿勢グラフと、後述するＫＦ姿勢・特徴点マップ最適化部５３３における新たな追加処理である、該姿勢グラフを用いた最適化の一例を示している。また図３は、前述した二段階処理の、ＧＮＳＳ位置を用いて大まかにＫＦの位置・姿勢を最適化する前段の処理の例を示す。

図３において、三角形で示すキーフレーム（ＫＦ）は、ＫＦ１、ＫＦ２、ＫＦ３、・・・と、点線の矢印で示す進行方向に追加される。また、四角形は、ＧＮＳＳ位置に関するＧＮＳＳ値を示している。符号３０１で示す状況では、ＫＦ１→ＫＦ２→ＫＦ３→ＫＦ４→ＫＦ５の順で、時系列に追加されたことを示しており、ＫＦ５は、姿勢推定対象最新ＫＦである。また、Ｇ１、Ｇ３、Ｇ５は、それぞれＫＦ１、ＫＦ３、ＫＦ５におけるＧＮＳＳ値を示している。

ＧＮＳＳ位置に関するＧＮＳＳ値を用いたキーフレーム（ＫＦ）位置・姿勢の逐次補正の処理手順を、符号３０１〜３０４の順で説明する。

まず、符号３０１に示すように、移動経路の始まりのころは、（１）新たに求めたキーフレーム（ＫＦ）であるＫＦ５を含め、今までに求めた過去のＫＦ群（ＫＦ１〜ＫＦ４）すべてに対し、隣り合うＫＦどうしを結ぶ実線で示す、ＫＦ画像間の見えの変化に合わせたＫＦの相対的な位置・姿勢変化量からのずれを最小化する、従来のＶ−ＳＬＡＭで使われているものと同じ拘束条件を設定する。符号３０１では、ＫＦ１〜ＫＦ５の隣どうしを互いに結ぶ拘束条件を設定する。

本実施では、この従来のＫＦどうしを結ぶ拘束条件に加えて、ＫＦにＧＮＳＳ位置があるならば、さらにＫＦとＧＮＳＳ値とのずれを最小化する新たな拘束条件を設定する。符号３０１において、ＫＦ１とＧ１を結ぶ実線は、この拘束条件Ｃ１を示している。拘束条件Ｃ３、Ｃ５も同様に、それぞれＫＦ３とＧ３、ＫＦ５とＧ５のこの拘束条件である。

符号３０１に示すように、これらＫＦどうしの見えに関する拘束条件と、ＫＦとＧＮＳＳ値との拘束条件との２種類の拘束条件を設定するが、さらにそのどちらをより優先するかを、各拘束条件の重みパラメータを設定して全体最適化への影響力を変えるなどで、調整してもよい。たとえば、用いるＧＮＳＳ位置の精度が非常に良いことがあらかじめ分かっている場合は、ＧＮＳＳ値との拘束条件の影響力をより強めるよう、重みを重く調整する。

そして、符号３０２に示すように、（２）過去のキーフレーム（ＫＦ）群を含めて、符号３０１で設定した拘束条件に基づく最適化により、ＫＦの位置・姿勢を補正する。符号３０２からもわかるように、ＧＮＳＳ値との拘束条件により、ＫＦ１をＧ１に、ＫＦ３をＧ３に、ＫＦ５をＧ５に近づくように補正されている。このように拘束条件に基づいて、ＫＦどうしの見えの変化にも合わせながら、拘束条件全体の誤差を最小化するよう、ＫＦをＧＮＳＳ値に近づけるように補正する。このため、この補正による結果として、必ずしもＫＦの位置がＧＮＳＳ値と完全に一致する結果になるとは限らない。

つぎに、移動体がさらに経路を進み、最初のキーフレーム（ＫＦ）であるＫＦ１から十分離れたＫＦ７を追加する位置まで進んだときの処理手順を符号３０３に示す。符号３０３において、移動体の進行方向への移動（時間の経過）にともない、ＫＦ６およびＫＦ７（姿勢推定対象最新ＫＦ）があらたに追加されている。

符号３０３に示すように、（３）新たに求めたキーフレーム（ＫＦ）に対し、符号３０１と同様に、隣り合うＫＦとの見えの変化に関する従来の拘束条件と、ＫＦにＧＮＳＳ位置がある場合にはＫＦとＧＮＳＳ値とのずれを最小化する拘束条件を、それぞれ追加する。

ここで、符号３０１と異なり、十分経路が伸びたので、最適化補正計算に使うＫＦ、すなわち拘束条件を設定するＫＦの選定をおこなう。最適化補正計算に使うＫＦの選定は、以下の２つの選定方法のうちのいずれかを使うことができる。第１の選定方法は、時間的、または距離的な近さに基づく方法である。たとえば、撮影時間差やＧＮＳＳ位置などから概算できる距離差が大きすぎるＫＦを使わないようにすることができる。

また、第２の選定方法は、共通する特徴点の有無に基づく方法である。たとえば、共通する特徴点がある場合は、位置・姿勢調整対象のＫＦ、すなわち、補正対象ＫＦ群とする。

一方、共通する特徴点が無い場合は、位置・姿勢は調整しない（固定）が、調整には使う参考ＫＦとする。

あるいは、最新ＫＦとの共通特徴数、時間的な近さなどから、補正対象ＫＦを選定したり、調整に用いる参考ＫＦを選定する代わりに、調整時の重みを変えて同様の処理をおこなってもよい。たとえば、共通特徴数が少なかったり、時間、または距離的に遠いＫＦは、調整影響が少なくなるよう、重みづけをする。

符号３０３において、ＫＦ１とＫＦ２は、時間的に遠く共通特徴点もないので、最適化補正計算の対象外とし、拘束条件は設定しない。また、ＫＦ３は、時間的に近いが、共通特徴点がないＫＦなので、位置固定で補正計算に入れる参考ＫＦとし、そのＫＦに関する拘束条件は設定するが、調整で位置・姿勢が変化することがないようにその位置・姿勢を固定する。また、ＫＦ４〜ＫＦ７は、位置が近く、共通特徴点がある補正対象ＫＦ群であるため、そのＫＦに関する拘束条件を設定するとともに調整で位置・姿勢を変化するよう設定する。その際、より新しいＫＦほど、より位置・姿勢を変化しやすいように、拘束条件に重みづけしてもよい。このように、ＫＦの選定をおこないながら、姿勢推定対象最新ＫＦ７の位置・姿勢を調整するための各拘束条件を設定する。

そして、符号３０４に示すように、（４）過去Ｎ（Ｎ＝４）フレーム含めて、拘束条件に基づく最適化により、キーフレーム（ＫＦ）の位置・姿勢を補正する。符号３０４からもわかるように、特にＫＦ７がＧ７に基づいて大きく補正し、それと共にＫＦ４〜ＫＦ６も補正されている。以上に示した手順で、ＫＦが追加されるごとに、ＧＮＳＳ値を用いた逐次補正の処理をおこなうことができる。

図３に示すような模式的なＫＦ位置・姿勢の最適化は、たとえば、既存の任意のグラフ構造を用いた最適化処理を用いて実施することができる。具体的な姿勢グラフとして、ＫＦ群をノードとして互いに拘束条件で結ぶとともに、入力ＧＮＳＳ情報に該当するＫＦに関するＧＮＳＳ位置があれば、そのＧＮＳＳ位置も新たなノードとして該当ＫＦに接続し、ＧＮＳＳ位置に該当ＫＦ位置との差がより小さくなるような拘束条件を設定する。このように作成したグラフ構造を用いて、最適化を実施する。

なお、このＧＮＳＳ位置のノードの初期値として、ＧＮＳＳ位置（実座標位置）のみを設定してもよいが、ＧＮＳＳ位置に対応するＫＦの現在の推定姿勢情報を加えて位置・姿勢の値を持つノードとして設定してもよい。また、これらの値（ＧＮＳＳ位置に関するＧＮＳＳ値およびＫＦから得た姿勢情報）は、適宜補正して用いてもよい。たとえば、明らかにＫＦ姿勢の値が間違っていると思われる場合などに、ＫＦから得た姿勢を補正して使うようにしてもよい。

また、ＧＮＳＳ位置を保持していないＫＦに対しては、ＧＮＳＳ値の補間計算をおこなうことで、ＧＮＳＳ位置のノードを、すべてのＫＦで保持するようにしてもよい。すべてのＫＦノードで接続するノードや保持するデータが同じ（位置・姿勢値）となるようにすることで、より簡単にグラフ最適化を計算することができる。

また、映像に対する入力姿勢情報が取得できる場合には、推定済のＫＦ姿勢の代わりに、入力したカメラ姿勢情報を用いるようにしてもよい。Ｖ−ＳＬＡＭによって映像変化から撮影姿勢は推定できるが、直路走行などの場合に、進行方向軸に対する回転のような姿勢変化（ロール角）に誤差が生じた場合は、該誤差からの復帰（値改善）が難しいため、別途センサで測定したカメラ姿勢情報を入力することで、より精度良い撮影位置・姿勢推定が可能となる。このとき、入力姿勢情報を使うタイミングも任意でよく、たとえば、ＫＦに対応する入力姿勢情報が有れば常に使うようにしてもよいし、新ＫＦに対してだけ入力姿勢情報を使い、他のＫＦ群は推定済の姿勢を使ってもよい。または、推定したＫＦ姿勢が明らかに間違っていると思われる場合だけ、入力姿勢情報を使うようにしてもよい。

図３では、たとえばグラフ構造に使うＫＦ群として、新ＫＦと映像時系列的、または空間的に近いＫＦ群を使う。たとえば、ＫＦ画像の映像時刻が新ＫＦと近かったり（時刻差が規定時間以内だったり）、保持するＧＮＳＳ位置が新ＫＦと近かったり（ＧＮＳＳ位置差が規定距離以内だったり）する任意のＫＦ群を、規定閾値を使って選定する。加えて、共有するマップ特徴点の最低数の閾値を決めて、後述するＣｏｖｉｓｉｖｉｌｉｔｙグラフとして保持しているＫＦどうしの特徴点の共有関係を有効に用いて、新ＫＦと３次元位置を持つマップ特徴点群を規定数以上共有しているＫＦ群も選定する。

これら２つの方法で選定したＫＦ群双方とも、グラフ構造のノードとし、ノード（ＫＦ）間に共有するマップ特徴点の共有数が多い程、相手の位置・姿勢変化の影響を受けるような拘束条件（たとえば、より強く相対位置・姿勢を保つようなエッジ）を設定する。

さらに、図３では、共通特徴点のないＫＦは、グラフ最適化による位置・姿勢変化の影響を受けないノード、すなわち、位置・姿勢固定で変更を受けないノードとして設定するようにしてもよい。

このように、図３では、逐次的に新ＫＦに合わせて、位置・姿勢の滑らかな補正のために、映像時刻的に近いＫＦを選定する。さらに、図３では、映像時刻による時間制限の閾値設定が難しい場合を考慮して、映像時刻と切り離した共有するマップ特徴点の多さそのものを使って、ＫＦ群を選定している。映像時刻的な近さは、往々にして共有するマップ特徴点も多く、空間的にも近いことが多いが、これらのいずれかを用いてＫＦ群を選定することで、より新ＫＦの位置・姿勢との関連の深いＫＦ群を補正対象として選定することができる。

そして、後述するＫＦ姿勢・特徴点マップ最適化部で、ノードとするＫＦ群の自動選定を通して補正最適化の影響範囲を調整した姿勢グラフを用いて、ＧＮＳＳ位置を使って新ＫＦとＫＦ群の位置・姿勢を最適化することで、従来のＶ−ＳＬＡＭの課題であった、画像特徴変化のみの位置・姿勢推定で生じるスケールドリフトを、シーン変化（画像上に映り込む被写体の変化）を考慮しながらも精度良く回避して、ＫＦ位置・姿勢を推定することが可能となる。また、この移動体位置推定方法を用いた補正処理は、ループ走行しなくても適用することができる。

図４は、実施の形態にかかる移動体位置推定方法における姿勢グラフと最適化の別の一例を模式的に示す説明図である。図４では、図３のＧＮＳＳ位置による最適化で変化したＫＦ群の位置・姿勢に合わせて、周辺のマップ特徴点の３次元位置も変化させながら、全体整合性が良くなるようにＫＦ群の位置・姿勢も再度同時に最適化して微調整することを目的としておこなう、前述した二段階処理の後段の詳細なＧＮＳＳ位置を用いた最適化の例を示す。

図４も図３と同様に、姿勢グラフを使った最適化として実施する例を示す。

図４において、符号４０１は、（ａ）ＧＮＳＳによる調整前の状態を示しており、符号４０２は、（ｂ）ＧＮＳＳによる調整後の状態を示している。符号４０２からわかるように、（ｂ）ＧＮＳＳによる調整後は、ＫＦとともに特徴点も動いていることがわかる。

図４において、三角形で示すキーフレーム（ＫＦ）は、ＫＦ１１、ＫＦ１２、ＫＦ１３、・・・ＫＦ１６と、点線の矢印で示す進行方向に追加される。ＫＦ１６は、最新ＫＦである。また、Ｇ１３、Ｇ１５、Ｇ１６は、それぞれＫＦ１３、ＫＦ１５、ＫＦ１６におけるＧＮＳＳ位置から得たＧＮＳＳ値を示している。

図４の姿勢グラフは、図３の姿勢グラフのような、ＫＦ群とＧＮＳＳ位置のノードに加えて、新ＫＦ周辺のマップ特徴点群をグラフのノードとして追加する。具体的には、図４の符号４０１に示すように、姿勢グラフのＫＦ群（ＫＦ１１〜ＫＦ１６）から閲覧されるマップ特徴点群を白丸で示すノード（Ｎ１１〜Ｎ１６）として追加する。

また、図４でも、図３と同じように、ＧＮＳＳ値のノードとＫＦのノードとの間に任意の重みをもった拘束条件（Ｃ１５、Ｃ１６）のエッジを追加する。さらに、マップ特徴点群のノード群と閲覧しているＫＦのノードとの間にも、任意の重みをもった拘束条件のエッジを追加する。このとき、マップ特徴点群のノードは、ＫＦ群から閲覧されるマップ特徴点群のすべてを使ってもよいし、一部を間引いて使ってもよい。このように、シーン変化に合わせて、最適化（誤差最小化）計算に使うキーフレーム（ＫＦ）や特徴点群を自動制御することで、精度のよい位置・姿勢調整を実現する。

調整に使う特徴点群の選定は、ＫＦ選定から、自動的に以下のように決定することができる。たとえば、位置・姿勢調整対象ＫＦ群に存在する特徴点を、位置調整対象の特徴点とすることができる。また、位置固定ＫＦ（調整に参考利用するか否かに関わらず）のみに存在する特徴点を、位置調整しない（固定の）特徴点とすることができる。

具体的には、図４において、ＫＦ１１およびＫＦ１２は、最適化計算の対象としないＫＦであるため、ＫＦ１１およびＫＦ１２の位置・姿勢は変化しない。ＫＦ１３は、位置を固定して最適化調整に用いる参考ＫＦであり、ＫＦ１４、ＫＦ１５、ＫＦ１６は、最適化計算で位置・姿勢調整の対象となるＫＦである。また、Ｎ１１およびＮ１２は、最適化計算の対象としない、位置変化しない特徴点である。一方、Ｎ１３〜Ｎ１６は、位置調整対象の特徴点である。そのうち、Ｎ１４〜Ｎ１６は、最新の位置・姿勢推定対象である最新ＫＦ１６から閲覧できたＫＦ１６に関連する特徴点である。

ＫＦとマップ特徴点を結ぶエッジには、たとえば該ＫＦ画像上に該マップ特徴点を投影した画像位置と、画像特徴量から該特徴点と同じと思われるＫＦ画像内で抽出した画像特徴の画像位置との差（再投影誤差）が大きい程、位置または位置・姿勢変化をおこなうような拘束条件を設定する。

さらに、図４の姿勢グラフは、新たにマップ特徴点をノード追加することで、多くのマップ特徴点を共有するＫＦどうしは同じ特徴点ノードとのエッジを多数保持して似た補正影響を受けるようになるため、図３で設定していたＫＦ間に設定したエッジ（共有する特徴点数の多さを示す拘束条件のエッジ）はすべて削除してよい。

なお、姿勢グラフ全体として、エッジの種類別に全体最適化への影響力を変えるため、図３と同様に、拘束条件に重みを設定してもよい。ＧＮＳＳ位置による調整を示すＧＮＳＳ位置とＫＦとのエッジ拘束条件と、画像ベースの画像特徴変化による調整を示すマップ特徴点とＫＦのエッジ拘束条件とに対し、任意の割合で最適化に影響させるよう、重みを設定することで、簡単にシステム調整をおこなうことができる。たとえば、マップ特徴点ノード数が非常に多い場合は、該ノードを使った画像ベースの影響力が非常に強くなるので、適宜影響を減らすよう、もう片方の重みよりも重みをより小さく調整してもよい。

このように、本実施の形態にかかる移動体位置推定方法にあっては、図４で示す姿勢グラフを用いたグラフ最適化により、図３で示すようにＧＮＳＳ位置で最適化済のＫＦ群に合わせて、必要ならＫＦ群の位置を微調整しながら、無理なく周辺のマップ特徴点群の位置も補正することができる。このため、従来のＶ−ＳＬＡＭの出力結果の画像撮影位置を修正しただけでは得られない、Ｖ−ＳＬＡＭとしての画像的なシーン変化にも、ＧＮＳＳ位置にも適度に補正した画像撮影位置と姿勢を得ることができ、さらに、同様に補正した特徴点群の３次元位置（実座標マップ）を得ることもできる。

（システム構成例）
図５は、実施の形態にかかる移動体位置推定システムのシステム構成の一例を示す説明図である。

図５において、実施の形態にかかる移動体位置推定システム５００は、移動体位置推定装置の一例であるサーバ５０１と、移動体５０３に搭載された、映像および衛星５０５からのＧＮＳＳ情報を収集する情報収集装置の一例である車載機５０２とを備える。そして、サーバ５０１と車載機５０２とが、ネットワーク５０４によって接続されることにより、移動体位置推定システム５００を構成する。また、移動体位置推定システム５００は、図示は省略するが、クラウドコンピューティングシステムによって、その機能を実現するようにしてもよい。

サーバ５０１は、初期姿勢・座標系設定部５１１と、フレーム姿勢推定部５２１と、ＫＦ（キーフレーム）更新部５２２と、３Ｄマップ特徴点更新部５３１と、グラフ制約生成部５３２と、ＫＦ姿勢・特徴点マップ最適化部５３３と、ループ検出・クロージング部５４１の各機能部を有する。各構成部５１１、５２１、５２２、５３１〜５３３、５４１によって、サーバ５０１の制御部を構成することができる。これらの構成部の詳細については、後述する。

また、サーバ５０１は、ＫＦ群情報５５１および特徴点群情報５５２などを記憶する実座標環境マップ５５０を備えている。あるいは、サーバ５０１は、実座標環境マップ５５０とアクセス可能に接続されている。

すなわち、実座標環境マップ５５０は、サーバ５０１内に設けられて（記憶されて）いてもよく、また、実座標環境マップ５５０は、図示を省略する別のサーバ内に設けられ、ネットワーク５０４などのネットワークによってサーバ５０１と接続されていてもよい。実座標環境マップ５５０の詳細については、後述する。

そして、上記構成部は、大きく分けて４つの機能部に分けることができる。初期姿勢・座標系設定部５１１によって、システムの初期化処理機能５１０を実現することができる。また、フレーム姿勢推定部５２１と、ＫＦ更新部５２２と、によって、位置姿勢推定（トラッキング）処理機能５２０を実現することができる。また、３Ｄマップ特徴点更新部５３１と、グラフ制約生成部５３２と、ＫＦ姿勢・特徴点マップ最適化部５３３と、によって、環境マップ作成（ローカルマッピング）処理機能５３０を実現することができる。また、ループ検出・クロージング部５４１によって、ループクローズ処理機能５４０を実現することができる。

移動体５０３は、具体的には、たとえば、情報を収集するコネクテッドカーであるが、これには限定されない。一般乗用車やタクシーなどの営業車、二輪車（自動二輪や自転車）、大型車（バスやトラック）などであってもよい。また、移動体５０３には、水上を移動する船舶や上空を移動する航空機、無人航空機（ドローン）、自動走行ロボットなどであってもよい。

車載機５０２は、撮影映像に関する情報およびＧＮＳＳ情報を含む移動体５０３の情報を収集する。移動体５０３の情報には、移動体５０３から収集する、移動体５０３の姿勢情報なども含まれる。

移動体５０３には、車載機５０２が備えられている。車載機５０２は、移動体５０３に搭載された専用の装置であってもよく、取り外し可能な機器であってもよい。また、スマートフォンやタブレットなどの通信機能を備えた携帯端末装置を移動体５０３において利用するものであってもよい。また、車載機５０２の機能を、移動体５０３が備えている機能を用いて実現するようにしてもよい。

したがって、車載機５０２の『車載』という表現は、移動体に搭載された専用装置という意味には限定されない。車載機５０２は、移動体５０３における情報を収集し、収集した情報をサーバ５０１に対して送信できる機能を持った装置であれば、どのような形態の装置であってもよい。

車載機５０２は、撮影映像に関する情報およびＧＮＳＳ情報を含む移動体５０３の情報（車載データ）を取得し、取得した車載データを保存する。そして、保存した車載データを、無線通信によって、ネットワーク５０４を介して、サーバ５０１へ送信する。また、サーバ５０１から配信されたプログラムを含む各種データを、ネットワーク５０４を介して、無線通信により受信する。

また、車載機５０２は、近距離通信機能により、近くを走行中の別の移動体５０３の情報を取得し、サーバ５０１へ送信するようにしてもよい。また、車載機５０２どうしが、近距離通信機能により、通信をおこない、他の車載機５０２を介して、サーバ５０１との通信をおこなうようにしてもよい。

このようにして、移動体位置推定システム５００において、サーバ５０１は、移動体５０３に搭載された車載機５０２から車載データを取得するとともに、各車載機５０２へ各種データを配信することができる。

また、車載機５０２は、通信手段を備えていなくてよい。すなわち、車載機５０２は、サーバ５０１とは、ネットワーク５０４を介して接続されていなくてもよい。その場合は、車載機５０２に蓄積されたデータは、オフラインで（たとえば、記録メディアを介して人手などにより）、サーバ５０１に入力することができる。

図５において、サーバ５０１が、初期姿勢・座標系設定部５１１と、フレーム姿勢推定部５２１と、ＫＦ更新部５２２と、３Ｄマップ特徴点更新部５３１と、グラフ制約生成部５３２と、ＫＦ姿勢・特徴点マップ最適化部５３３と、ループ検出・クロージング部５４１の各機能部を有する構成とした。図示は省略するが、これらの各機能部の少なくとも一つを、サーバ５０１に加えて、あるいは、サーバに代えて、車載機５０２が有するようにしてもよい。

車載機５０２が、各機能部５１１、５２１、５２２、５３１、５３２、５３３、５４１の少なくとも一つを有する場合は、サーバ５０１が実施する処理の内容と同じであってもよい。ただし、３Ｄ地図マップ情報は、任意の媒体（ＤＶＤ／ＢＬディスク、ＨＤＤなど）に保持していて利用する以外にも、適宜、図示を省略する外部サーバから無線ネットなどを経由して取得するようにしてもよい。

（移動体位置推定装置のハードウェア構成例）
図６は、移動体位置推定装置のハードウェア構成の一例を示すブロック図である。移動体位置推定装置の一例であるサーバ５０１は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）６０１と、メモリ６０２と、ネットワークＩ／Ｆ（Ｉｎｔｅｒｆａｃｅ）６０３と、記録媒体Ｉ／Ｆ６０４と、記録媒体６０５と、を有する。また、各構成部は、バス６００によってそれぞれ接続される。

ここで、ＣＰＵ６０１は、サーバ（移動体位置推定装置）５０１の全体の制御を司る。メモリ６０２は、たとえば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）およびフラッシュＲＯＭなどを有する。具体的には、たとえば、フラッシュＲＯＭやＲＯＭが各種プログラムを記憶し、ＲＡＭがＣＰＵ６０１のワークエリアとして使用される。メモリ６０２に記憶されるプログラムは、ＣＰＵ６０１にロードされることで、コーディングされている処理をＣＰＵ６０１に実行させる。

ネットワークＩ／Ｆ６０３は、通信回線を通じてネットワーク５０４に接続され、ネットワーク５０４を介して他の装置（たとえば、車載機５０２、実座標環境マップ５５０や全画像位置姿勢データ５６０が格納される装置、あるいは、他のサーバやシステム）に接続される。そして、ネットワークＩ／Ｆ６０３は、ネットワーク５０４と自装置内部とのインターフェースを司り、他の装置からのデータの入出力を制御する。ネットワークＩ／Ｆ６０３には、たとえば、モデムやＬＡＮアダプタなどを採用することができる。

記録媒体Ｉ／Ｆ６０４は、ＣＰＵ６０１の制御にしたがって記録媒体６０５に対するデータのリード／ライトを制御する。記録媒体６０５は、記録媒体Ｉ／Ｆ６０４の制御で書き込まれたデータを記憶する。記録媒体６０５としては、たとえば、磁気ディスク、光ディスクなどが挙げられる。

なお、サーバ５０１は、上述した構成部のほかに、たとえば、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）、キーボード、ポインティングデバイス、ディスプレイなどを有していてもよい。

（車載機のハードウェア構成例）
図７は、車載機のハードウェア構成の一例を示すブロック図である。情報収集装置の一例である車載機５０２は、ＣＰＵ７０１と、メモリ７０２と、無線通信装置７０３と、移動体Ｉ／Ｆ７０４と、受信装置７０５と、撮像装置７０６と、を有する。また、各構成部は、バス７００によってそれぞれ接続される。

ＣＰＵ７０１は、車載機５０２の全体の制御を司る。メモリ７０２は、たとえば、ＲＯＭ、ＲＡＭおよびフラッシュＲＯＭなどを有する。具体的には、たとえば、フラッシュＲＯＭやＲＯＭが各種プログラムを記憶し、ＲＡＭがＣＰＵ７０１のワークエリアとして使用される。メモリ７０２に記憶されるプログラムは、ＣＰＵ７０１にロードされることで、コーディングされている処理をＣＰＵ７０１に実行させる。

無線通信装置７０３は、発信された電波を受信したり、電波を発信したりする。アンテナと受信装置とを含む構成であり、各種通信規格による移動通信（具体的には、たとえば、３Ｇ、４Ｇ、５Ｇ、ＰＨＳ通信など）、Ｗｉ−Ｆｉ（登録商標）などの通信を送受信する機能を備えている。

移動体Ｉ／Ｆ７０４は、移動体５０３と車載機５０２の自装置内部とのインターフェースを司り、移動体５０３からのデータの入出力を制御する、したがって、車載機５０２は、移動体Ｉ／Ｆ７０４を介して移動体５０３が備えるＥＣＵ（各種センサなどを含む）７０７から情報を収集する。移動体Ｉ／Ｆ７０４は、具体的には、たとえば、有線により接続する際に用いるコネクタや近距離無線通信（具体的には、たとえば、Ｂｌｕｅｔｏｏｔｈ（登録商標））装置などであってもよい。

受信装置（たとえばＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信装置などのＧＮＳＳ受信装置）７０５は、複数の衛星５０５からの電波を受信し、受信した電波に含まれる情報から、地球上の現在位置を算出する。

撮像装置（たとえばカメラなど）７０６は、静止画や動画を撮像する機器である。具体的には、たとえば、レンズと撮像素子とを備える構成である。撮像装置７０６による撮像画像は、メモリ７０２に保存される。また、カメラなどの撮像装置７０６は、画像認識機能や、バーコードやＱＲコード（登録商標）を読み取る機能や、ＯＭＲ（Ｏｐｔｉｃａｌ Ｍａｒｋ Ｒｅａｄｅｒ）、ＯＣＲ（Ｏｐｔｉｃａｌ Ｃｈａｒａｃｔｅｒ Ｒｅａｄｅｒ）機能などを備えていてもよい。

図７に示したように、ＧＮＳＳ受信装置７０５および撮像装置７０６は、車載機５０２が備えていてもよく、また、移動体５０３が備えていたり、別途、外付けされたものを用いるようにしてもよい。その際、ＧＮＳＳ受信装置７０５あるいは撮像装置７０６と、車載機５０２とのデータのやりとりは、有線または無線通信によりおこなうようにしてもよい。

ＧＮＳＳ受信装置７０５や撮像装置７０６を、車載機５０２が備えていない場合は、移動体Ｉ／Ｆ７０４などを介して、それらの情報を取得するようにしてもよい。また、車載機５０２は、図示は省略するが、各種入力装置、ディスプレイ、メモリカードなどの記録媒体の読み書き用のインターフェース、各種入力端子などを備えていてもよい。

（実座標環境マップの内容）
図８は、実座標環境マップのデータ構成の一例を示す説明図である。図８において、実座標環境マップ５５０は、ＫＦ群情報５５１と、特徴点群情報（３次元位置情報）５５２ａと、特徴点群情報（ＫＦ画像内位置情報）５５２ｂと、を有する。

ここで、既存技術の環境地図に相当するものが、実座標環境マップ５５０であり、各画像特徴点の３次元位置（特徴点群情報（３次元位置情報）５５２ａ）の他に、どの画像特徴点はどの画像で閲覧されたか（用いるか）に関する情報を持っている。これを、特徴点群情報（ＫＦ画像内位置情報）５５２ｂと、ＫＦ群情報５５１の２つで表している。主要な映像内の画像（ＫＦ）群の情報であるＫＦ群情報５５１と、該ＫＦ画像上に各画像特徴が映っている２次元位置に関する情報である特徴点群情報（ＫＦ画像内位置情報）５５２ｂは、環境地図を任意の画像の位置・姿勢推定に用いるために必須な情報である。

図８に示すように、ＫＦ群情報５５１は、「ＩＤ」、「親ＫＦ ＩＤ」、「子ＫＦ ＩＤ」、「ループＫＦ ＩＤ」、「姿勢情報」、「位置情報」、「特徴量」、「ＧＮＳＳ位置」および「映像フレーム番号」を含む各種情報を持っている。

ここで、「ＩＤ」は、当該ＫＦの情報を識別する一意の識別情報であり、「親ＫＦ ＩＤ」および「子ＫＦ ＩＤ」は、ＫＦどうしをつなぐ情報であり、「ループＫＦ ＩＤ」は、ループクローズ処理などで使う、再度同じ場所（ループ）を通過した時に通過時其々のＫＦ群を互いに参照保持する情報である。

また、「姿勢情報」・「位置情報」は、ＫＦの推定撮影位置・姿勢情報であり、「特徴量」は、任意の画像に対して似た画像か否かを判断するのに用いる画像全体としての特徴量であり、「ＧＮＳＳ位置」は、新たに入力ＧＮＳＳ情報に相当する該ＫＦの撮影時のＧＮＳＳ位置であり、「映像フレーム番号」は、対応する映像のフレーム番号である。

図８に示すように、特徴点群情報（３次元位置情報）５５２ａは、「ＩＤ」、「位置座標」、「特徴量」および「観測ＫＦのＩＤ群」を含む各種情報を持っている。

ここで、「ＩＤ」は、当該特徴点情報を識別する一意の識別情報であり、「位置座標」は、推定した特徴点の実座標位置座標であり、「特徴量」は、画像特徴であり、「観測ＫＦのＩＤ群」は、当該特徴点が映っているＫＦの情報であり、ＫＦ群情報５５１の中の該当するＫＦ情報の「ＩＤ」が関連付けされる。なお、実座標位置座標は、初期姿勢・座標系設定部で作成した任意の実座標変換を利用して実座標化するものとして、ローカル値で保持していてもよい。

特徴点群情報（ＫＦ画像内位置情報）５５２ｂは、ＫＦ画像から抽出された画像特徴点群の情報であり、複数のＫＦ画像から同時閲覧されて選定されて３次元位置を持つ特徴点群と、３次元位置を持たない特徴点群の二種類が存在する。３次元位置を持たないＫＦ特徴点群は、任意の画像が該ＫＦ画像と似ているかを詳細評価するのに使ったり、将来新たなＫＦ画像が得られた時に新しく選定されて３次元位置を持つ特徴点群になるのに備えて、保持しておく。

図８に示すように、特徴点群情報（ＫＦ画像内位置情報）５５２ｂは、「ＩＤ」、「ＫＦ ＩＤ」、「マップ点ＩＤ」、「特徴点位置」、「特徴点角度」および「縮小階層番号」を含む各種情報を持っている。

ここで、「ＩＤ」は、当該特徴点情報を識別する一意の識別情報である。「ＫＦ ＩＤ」は、当該ＫＦ特徴点を抽出したＫＦを特定するための情報であり、ＫＦ群情報５５１の中の該当するＫＦ情報の「ＩＤ」が関連付けされる。「マップ点ＩＤ」は、特徴点群情報（３次元位置情報）５５２ａへの参照情報であり、特徴点群情報（３次元位置情報）５５２ａの中の該当する特徴点情報の「ＩＤ」が関連付けされる。この「マップ点ＩＤ」は、複数のＫＦ画像から同時閲覧されて選定されて３次元位置を持つ特徴点群だけが持っており、３次元位置を持たない特徴点群は持っていない。

また、「特徴点位置」・「特徴点角度」は、たとえば、ＯＲＢ（Ｏｒｉｅｎｔｅｄ ＦＡＳＴ ａｎｄ Ｒｏｔａｔｅｄ ＢＲＩＥＦ）特徴の重心位置および方向ベクトルに関する情報である。また、「縮小階層番号」は、当該ＫＦ画像内での抽出状況に関する情報である。この「縮小階層番号」は、たとえば、画像特徴として縮小率を変えてピラミッド階層的に求めた縮小画像群を用いて算出したＯＲＢ特徴点を想定している場合に、縮小画像群のどれで抽出したのかに関する情報である。これら「特徴点位置」「特徴点角度」「縮小階層番号」などは、他の画像特徴を使う場合は、その特徴に合わせた情報にしてよい。

このようにして、実座標環境マップ５５０が形成され、ＫＦ群情報と特徴点群情報とが関連付けされて、記憶される。なお、一般的に特徴点ベースのＶ−ＳＬＡＭの環境マップは、画像特徴点群の画像特徴と３次元位置、該特徴点群を閲覧している画像ＫＦの情報、また、画像ＫＦと似た画像を検索できるようにするための画像ＫＦ内の画像特徴群を含むが、実座標環境マップ５５０は、ＫＦ群情報５５１に「ＧＮＳＳ位置」情報を新たに保持する以外は、従来のＶ−ＳＬＡＭの環境マップと同じデータであってもよい。

（全画像位置姿勢データの内容）
図９は、全画像位置姿勢データのデータ構成の一例を示す説明図である。全画像位置姿勢データ５６０は、主要な画像で構成するＫＦとは異なり、すべての映像中の画像に対し、推定した撮影位置と姿勢を保持する。ここで、既存技術の映像内の全画像の撮影位置・姿勢に相当するものが、全画像位置姿勢データ５６０である。

図９に示すように、全画像位置姿勢データ５６０は、「ＩＤ」、「親ＫＦ ＩＤ」、「姿勢情報」、「位置情報」、「映像フレーム番号」を含む各種情報を持っている。ここで、「ＩＤ」は、当該位置姿勢データを識別する一意の識別情報である。「親ＫＦ ＩＤ」は、映像的に近く位置・姿勢を参照するＫＦの情報である。「姿勢情報」・「位置情報」は、親ＫＦからの相対位置および姿勢であり、「映像フレーム番号」は、対応する映像のフレーム番号である。

位置姿勢情報は、たとえば、映像的に近いＫＦに対する相対位置・姿勢として保持しておき、最終的にＶ−ＳＬＡＭ結果を出力する際に、ＫＦの位置・姿勢を反映させながら、実座標値にする。このようにすることで、逐次的にＶ−ＳＬＡＭを処理する際に、ＫＦの位置・姿勢が最適化処理で途中で変化することを気にせずに、全画像の位置・姿勢を最終的なＫＦの位置・姿勢に合わせて簡単に算出することができる。また、位置姿勢情報は、ＫＦと同様に、実座標値でもローカル値で保持するようにしてもよい。

なお、図８および図９からもわかるように、この例では、ＫＦの位置・姿勢に関する情報は、ＫＦの他の情報とともに全画像位置姿勢とは別に保持するものとしている。全画像位置姿勢は映像の全画像フレームの撮影位置・姿勢であり、実座標環境マップ５５０のＫＦ群情報５５１に含まれるＫＦの位置姿勢情報は、映像中の一部画像であるＫＦ画像の撮影位置・姿勢であるため、全画像位置姿勢データ５６０に含めるようにしてもよい。

また、全画像位置姿勢データ５６０は、従来のＶ−ＳＬＡＭと同じデータであってもよい。

また、図５の移動体位置推定システム５００は、図示を省略するが、図８、図９で示した実座標環境マップ５５０、全画像位置姿勢データ５６０の各種情報の他に、従来と同様に、実座標環境マップを用いたＶ−ＳＬＡＭ計算を高速化するための様々な情報を追加で保持するようにしてもよい。たとえば、画像ＫＦ群内で３次元位置を持つマップ特徴点群を共有しているＫＦどうし、さらにその中でも最も特徴点群の共有数の多いＫＦ群、などの関係を保持して、各ＫＦどうしで互いに参照できてもよい。

より具体的には、たとえば、ＯＲＢ−ＳＬＡＭでは、Ｃｏｖｉｓｉｖｉｌｉｔｙグラフであり、各ＫＦをノードとしてエッジにマップ特徴点を共有するＫＦ群、エッジの重みを共有するマップ特徴点数とする、グラフ構造のデータとして保持してよい。これらは、後述するローカルマッピング処理などで、ＫＦ位置・姿勢や環境マップの最適化計算対象を求めたり、ループクローズ処理などで現在の画像フレームに似た画像を探索したりするのを高速化するのに利用することができる。

（移動体位置推定システムの内容）
図１０は、実施の形態にかかる移動体位置推定システム、移動体位置推定方法の内容の一例を示す説明図である。

図１０において、カメラなどによる映像１００１、映像１００１と同時に取得したＧＮＳＳ情報１００２、の各入力データと、初期姿勢・座標系設定部５１１、フレーム姿勢推定部５２１、キーフレーム（ＫＦ）更新部５２２、３Ｄマップ特徴点更新部５３１、グラフ制約生成部５３２、ＫＦ姿勢・特徴点マップ最適化部５３３、ループ検出・クロージング部５４１、の７つのＶ−ＳＬＡＭをベースとする処理部と、実座標環境マップ５５０（ＫＦ群情報５５１、特徴点群情報５５２）、全画像位置姿勢データ５６０、の２つの内部保持データ、さらに、初期環境マップ１０１０のデータを持っていてもよい。また、このうち内部保持データの少なくともどちらかを出力データ（実座標環境マップ５５０’、全画像位置姿勢データ５６０’）として出力することができる。

なお、本実施の形態にかかる移動体位置推定システム５００は、従来のＶ−ＳＬＡＭ技術をベースとしているため、各処理部の処理の一部で、従来のＶ−ＳＬＡＭの処理と同じ処理をおこなうようにしてもよい。本実施の形態では、従来のＶ−ＳＬＡＭとして特徴点ベースのＶ−ＳＬＡＭ、特にＯＲＢ特徴を用いたＯＲＢ−ＳＬＡＭの基本的な処理例をあげ、従来のＶ−ＳＬＡＭ処理との差を示すようにして、以下に説明する。

（入力される情報の内容）
移動体位置推定システム５００には、映像１００１、ＧＮＳＳ情報１００２、姿勢情報１００３の各情報が入力される。映像１００１およびＧＮＳＳ情報１００２は、初期姿勢・座標系設定部５１１に入力され、姿勢情報１００３は、グラフ制約生成部５３２に入力される。ただし、グラフ制約生成部５３２に入力される姿勢情報１００３については、必須の入力情報でなくてもよい。

映像１００１は、車両などの移動体５０３に搭載した車載機５０２が有する撮像装置７０６によって撮影された映像である。車載機５０２などの車両の通信手段を用いたり、記録メディアを介して人手を使ったり、任意の方法で入手し、本システム５００の入力とすることができる。また、映像の歪み補正などで用いるため、映像を撮影した撮像装置７０６の内部パラメータは既知とし、適宜歪み補正を実施するものとする。

ＧＮＳＳ情報１００２は、映像撮影時の移動体５０３の位置であり、ＧＰＳなどの任意の既存の測位手段によるデータであり、映像と同等の任意の方法で入手して、本システム５００の入力とする。

なお、ＧＮＳＳ情報１００２は、映像によるＶ−ＳＬＡＭのスケールドリフトを補正するために新たに利用するものであり、できるだけ映像の全フレームで保持することが望ましいが、必ずしも全フレームで保持していなくてもよい。保持するフレームが多い程、本システムで出力する全画像位置姿勢、および、実座標環境マップの位置および姿勢精度を改善することができる。

また、後述するように、本システムの初期化で利用する少なくとも映像解析開始地点付近の２つの画像フレームは、ＧＮＳＳ情報を保持している必要があり、なるべく密にＧＮＳＳ情報を保持しているほど、映像開始から早い段階で初期化処理が終了でき、撮影位置・姿勢推定処理を実施することができる。

同様に、ＧＮＳＳ情報１００２は、なるべく正確な位置であることが望ましく、精度が高い程、本システムの出力結果の位置および姿勢精度を改善することができる。また、ＧＮＳＳ情報は、ＧＰＳ受信機などの位置になることが多いが、ＧＰＳ受信機とカメラの相対位置関係を用いて、できるだけカメラの位置情報に変換してあることが望ましい。

また、姿勢情報１００３は、任意のＩＭＵ（ｉｎｅｒｔｉａｌ ｍｅａｓｕｒｅｍｅｎｔ ｕｎｉｔ）などから取得する、映像を撮影した時のカメラ姿勢情報である。ＩＭＵは、具体的には、加速度センサ、ジャイロセンサなどである。たとえば、カメラを中心とし、自車前方、右方、鉛直上方、などの座標軸に対する回転角、ロール、ピッチ、ヨー角などである。ＧＮＳＳ情報と同様に、映像の画像すべてに対して保持してもよく、任意画像にだけ保持していてもよい。

なお、ＧＮＳＳ情報１００２および姿勢情報１００３は、上述したように別途センサ群から入手するのではなく、一度、Ｖ−ＳＬＡＭで推定した各カメラ撮影位置・姿勢を、手作業などの任意の手法で補正し、補正した各カメラ撮影位置・姿勢を、再度、同じ映像のＧＮＳＳ情報１００２および姿勢情報１００３として読み込ませるようにしてもよい。

従来のＶ−ＳＬＡＭの出力結果であるカメラ撮影位置を手修正しても、周辺の環境マップ（特徴点群）をそれに合わせて補正することはできない。しかしながら、本システムでは、後述する独自のローカルマッピング機能により、入力されたＧＮＳＳ情報１００２による位置と、映像１００１を解析した結果による位置の双方を適切にマージ反映させた推定ができる。したがって、本システムによれば、手修正した出力結果を入力とする再実施を通して、手修正結果に合わせて特徴点群を含めた実座標環境マップを滑らかに作成することが可能となる。

なお、最初の実施時は姿勢情報を入力しなくても位置情報とともに姿勢情報も推定出力するので、再実行では、該推定姿勢情報も位置情報とともに入力して使えるが、姿勢情報は使わずに位置情報だけを入力として使ってもよい。たとえば、手修正した位置情報と異なり、まったく姿勢情報を手修正できなかった場合などでは、２つの情報は確からしさが異なっているため、精度の低い推定姿勢情報は使わずに位置情報だけを再実施時に入力利用して、実座標環境マップを作成することができる。

また、一度出力した実座標環境マップ５５０’を再び入力として利用してもよい。たとえば、ある走路の最初の走行映像の撮影位置・姿勢を推定する際には、実座標環境マップが存在しないので、当該実座標環境マップの入力無しで本システムにおける処理を実行し、つぎに同じ走路を走行した二回目以降の走行映像の撮影位置・姿勢を推定する場合には、最初の走行映像の処理結果として出力した実座標環境マップ５５０’を入力して、あたかも当該映像の処理で作成した内部データかのように利用することができる。このとき、一回目と二回目以降の走行映像では、撮影する車両やカメラ、走行レーン内の位置などが異なっていてもよい。なお、実座標環境マップ５５０’を入力する場合には、ＧＮＳＳ情報１００２の入力を省略してもよい。

なお、車載機５０２が、各機能部５１１、５２１、５２２、５３１、５３２、５３３、５４１の少なくとも一つを有する場合は、映像１００１やＧＮＳＳ情報１００２は、車載機５０２の内部に保持してＶ−ＳＬＡＭを処理するようにしてもよい。

以後の本システムの説明では、特に記載がない場合には、実座標環境マップ入力がなく、一から実座標環境マップを作成する場合（ＧＮＳＳ情報１００２の入力を必須とする場合）について説明をおこなう。また、ＧＮＳＳ情報１００２として、平面直角座標系の値を例として説明をおこなう。

（初期姿勢・座標系設定部５１１の内容）
システムの初期化処理機能５１０を担当する初期姿勢・座標系設定部５１１は、計算する座標系の決定とともに、初期化処理として以後のトラッキングなどの処理機能で必要とする内部データの作成をおこなう。具体的には、映像開始時の場所近傍にある特徴点群の３次元位置を推定するとともに、初期ＫＦ位置・姿勢の推定をおこない、以後の処理で最低限必要となる、映像開始時の場所近傍の実座標環境マップを作成する。この初期姿勢・座標系設定部の処理のうち、計算に用いる座標系の決定処理以外は、従来のＶ−ＳＬＡＭの初期処理と同じ処理でもよい。この初期化処理が完了しないと、以後のフレーム姿勢推定を含めた処理は実行されないことも、従来のＶ−ＳＬＡＭと同じである。

初期姿勢・座標系設定部５１１は、具体的には、たとえば、図６に示した、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。

初期姿勢・座標系設定部５１１は、まず、歪み補正した映像の各画像に対し、任意の画像特徴群を取得する。つぎに、最初の２フレームで同時に映っている特徴点（各画像の特徴点のペア）を各画像特徴群から探索する。ペアの探索方法は、利用する画像特徴に依存し、既存の２画像の同特徴対の探索方法を利用してもよい。算出したペア数が十分多い場合には、特徴点群の画像変化を用いて、カメラの位置・姿勢変化と２画像に共通で映る各特徴点群の３次元位置を推定する。

すなわち、２画像に映る各特徴点ペアの位置・姿勢変化から既存の方法、たとえば、平面を想定したＨｏｍｏｇｒａｐｈｙや、非平面を仮定した基礎行列などの幾何モデルを使った手法を用いて、２画像のカメラの位置・姿勢の変化を表す変換を推定するとともに、推定した２画像のカメラの位置・姿勢と各特徴ペアの各画像上の位置から、既存の三角測量などの手法を用いて、各特徴の３次元位置を推定する。

なお、ペア数が不足する場合には、２画像のどちらかの画像（たとえば、後時刻の画像）を他の画像（たとえば、さらにその後の時刻の画像）に変えてこの処理をおこなう。また、利用する最初の２フレームは、厳密に映像開始時点の画像でなくてもよく、同じ被写体が映っている可能性のある任意の２フレームであってもよい。たとえば、停車中映像であることがわかっているのであれば、カメラ撮影位置が変わったと思われる画像を、後時刻の画像として選ぶようにしてもよい。

また、このとき、すべての特徴点群ペアの３次元位置を算出するのではなく、他の特徴点群と比べて誤差の大きな特徴点を省いたり、画像全体でまんべんなく規定数の特徴点が得られるように、特徴点群が集中する画像部分では特徴点群を間引いたり、２カメラ位置と当該特徴点の成す角（交会角）が小さな特徴点を省いたり、というように、任意の特徴点選別をおこなってもよい。

また、初期姿勢・座標系設定部５１１は、従来のＶ−ＳＬＡＭと同様にさらに最適化計算をおこなって、算出した初期値をより正確な値へと更新する処理を付加してもよい。具体的には、２画像の各画像に対し、カメラ位置と特徴点群の３次元位置とがわかっており、各画像に特徴点群がどう映り込むのかを計算することができるため、各画像への特徴点群の映り込み位置と、実際のカメラ画像での該特徴点の位置との差（再投影誤差と呼ぶ）を調べ、特徴点群の再投影誤差がなるべく少なくなるよう、特徴点やカメラ位置および姿勢を微調整する最適化補正（ＢＡ（Ｂｕｎｄｌｅ Ａｄｊｕｓｔｍｅｎｔ））をするようにしてもよい。

続いて、初期姿勢・座標系設定部５１１は、算出結果から初期環境マップ１０１０を作成する。すなわち、用いた２画像をＫＦとして推定した該画像の撮影位置・姿勢とともに初期環境マップ１０１０に登録して、同じく推定した特徴点群の情報（２画像上の位置や、３次元位置）も、初期環境マップ１０１０に登録する。この初期環境マップ１０１０は、初期化処理ということで、以後のトラッキングやローカルマッピングなどの機能部でおこなう方法とはやや異なる方法でＫＦ位置・姿勢や特徴点群の３次元位置を推定しており、若干精度が低い。

なお、これらの２画像のカメラ位置・姿勢、および車両周辺の特徴点群の３次元位置の初期値の算出処理では、従来のＶ−ＳＬＡＭと同様に２画像のうち片方の画像（多くは、より時間の早い画像。以後、「初期カメラの画像」と呼ぶ）のカメラ位置・姿勢（以後、「初期カメラ位置・姿勢」と呼ぶ）を原点および基準座標系としたローカル系で算出してよい。

たとえば、一般的に画像処理で用いる画像上の画素位置を示すための画素座標系は、撮影画像の画像横方向をＸ、画像下方向をＹとすることが多い。このため、従来のＶ−ＳＬＡＭもこれと似た基準座標系定義とするために、初期フレームのカメラ位置を原点（０，０，０）とし、自車右手方向Ｘ、自車鉛直下方向Ｙ、自車前方方向Ｚ、という右手系（ＳＬＡＭローカル系）定義とすることが多い。本システムにおいても、このＳＬＡＭローカル系で２画像のカメラ位置・姿勢と、２画像に共通で映る特徴点群の３次元位置を算出する。

このように、初期姿勢・座標系設定部５１１は、従来のＶ−ＳＬＡＭと同様に、初期環境マップ１０１０の作成（ＫＦ位置・姿勢の推定、および特徴点群の３次元位置の推定）処理をおこなう。

つぎに、初期姿勢・座標系設定部５１１は、ＳＬＡＭローカル座標系で算出した環境マップのＫＦ撮影位置・姿勢、および特徴点群の３次元位置を実座標系対応にするために、入力ＧＮＳＳ情報から２画像に対応するＧＮＳＳ位置座標値を得て、ＳＬＡＭローカル（座標）系と実座標系の変換行列を算出する。

図１１Ａ〜１１Ｃは、初期姿勢・座標系設定部５１１における変換行列算出の一例を示す説明図である。図１１Ａに示すように、本システム５００では、実座標系として平面直角座標系を用いる。具体的には、符号１１０１は、（ａ）ＳＬＡＭローカル系（右手系）を示している。具体的には、原点（初期カメラ）に対して、Ｘ方向が右手方向を示しており、Ｙ方向が下方向を示しており、Ｚ方向が進行方向を示している。

これに対して、符号１１０２は、（ｂ）実座標系、すなわち、平面直角座標系（左手系）を示している。具体的には、平面直角座標系原点（０，０，０）に対して、Ｘ方向が「北」、すなわち、平面直角座標系Ｘ値［ｍ］を示しており、Ｙ方向が「東」、すなわち、平面直角座標系Ｙ値［ｍ］を示しており、Ｚ方向が「上」、すなわち、標高値［ｍ］を示している。

ただし、これは一例であって、従来のＶ−ＳＬＡＭの右手系のＳＬＡＭローカル系とは異なる左手系の平面直角座標系を用いるのではなく、右手系の任意の座標系を用いるようにしてもよい。

図１１Ｂは、移動ベクトルを示している。符号１１０３は、ローカル系の移動ベクトルＡであり、符号１１０４は、実座標系の移動ベクトルＢである。移動ベクトルとは、「任意２時刻の画像フレームの遅い時刻の方のフレーム（Ｆ２）の位置」と、「任意２時刻の画像フレームの早い時刻の方のフレーム（Ｆ１）の位置」の差分（Ｆ２−Ｆ１）を示す進行方向ベクトルである。図１１Ｂに示すように、同じ移動ベクトルが、２つの座標系（移動ベクトルＡ１１０３と、移動ベクトルＢ１１０４）で表現されている。そこで、初期姿勢・座標系設定部５１１は、別の系の値に変換する変換行列（ローカル系→実座標系への変換行列Ｍ）を算出する。

図１１Ｃは、ＳＬＡＭローカル系から実座標系の値に変換するための変換行列Ｍの内容について示している。図１１Ｃにおいて、ＳＬＡＭローカル系から実座標系の値に変換するための変換行列Ｍは、座標系間のスケール相違を吸収するためのスケール変換行列Ｍ１、進行方向由来の系から緯度経度由来の系へ座標軸を変換した値にするための回転行列Ｍ２、ＸＹＺの座標軸定義を変えた値にするためのＭ３、右手系から左手系の値に変換するＭ４、原点を初期カメラ位置から平面直角座標系の原点に変更した値に変換するＭ５、の５つの行列の積算から成る。

スケール変換行列Ｍ１は、画像変化による任意スケールを実座標のスケールとする変換行列である。スケール変換行列Ｍ１によって、（１）ＳＬＡＭローカル系１１１１を、（２）ｍスケールのＳＬＡＭローカル系１１１２に変換することができる。

図１１Ｄは、画像変化由来の任意スケールを、緯度経度の座標系のスケール［ｍ］にするスケール変換行列Ｍ１の算出の一例を示す説明図である。

図１１Ｄにおいて、まず、ＳＬＡＭローカル系の２画像カメラ位置Ｑ１（前時刻画像Ｆ１に対応）、Ｑ２（同後時刻画像Ｆ２）から、その差分（各位置差、Ｑ２−Ｑ１）である移動ベクトルＡ１１０３を算出する。成分定義はＳＬＡＭローカル系そのもの（従来のＶ−ＳＬＡＭ出力値）であってもよい。

つぎに、２画像のＧＮＳＳ位置Ｓ１（前時刻画像Ｆ１に対応）、Ｓ２（同後時刻画像Ｆ２）から、実座標系（直角平面座標系）の値を使うが成分（軸）定義が異なる、特殊実座標系の移動ベクトルＢ１１０４（＝Ｓ２−Ｓ１）を算出する。特殊実座標系は、（Ｘ成分＝東が＋の経度座標値差、Ｙ成分＝−（標高値差）、Ｚ成分＝北が＋の緯度座標値差）とする。

そして、移動ベクトルＡ１１０３の大きさ＝ｌｅｎＡ、および、移動ベクトルＢ１１０４の大きさ＝ｌｅｎＢ、をそれぞれ求める。求めた大きさから、大きさ比 Ｒａｔｅ＝（ｌｅｎＢ÷ｌｅｎＡ）を求めて、Ｒａｔｅ倍するスケール変換行列をスケール変換行列Ｍ１として求める。Ｅｙｅ（ｍ，ｎ）をｍ行ｎ列の単位行列としてあらわすとき、スケール変換行列Ｍ１は、
Ｍ１＝Ｒａｔｅ×Ｅｙｅ（３，３）；となる。

図１１Ｃに戻って、回転行列Ｍ２は、進行方向由来の座標系を緯度経度由来に変更する変換行列である。回転行列Ｍ２によって、（２）ｍスケールのＳＬＡＭローカル系１１１２を、（３）特殊実座標系１１１３に変換することができる。

図１１Ｅは、回転変換行列Ｍ２の算出の一例を示す説明図である。図１１Ｅにおいて、まず、移動ベクトルをそれぞれの長さで割り、正規化したローカル系の移動ベクトルＡ’＝Ａ／ｌｅｎＡと、正規化した実座標系の移動ベクトルＢ’＝Ｂ／ｌｅｎＢを求める。

つぎに、符号１１０５に示すように、（ａ）ベクトルＡ’からベクトルＢ’、への成す角Θを、内積から求める。
Θ＝ａｃｏｓ（内積（Ａ’，Ｂ’））

そして、符号１１０６に示すように、（ｂ）ベクトルＡ’とベクトルＢ’の外積＝Ａ’×Ｂ’となる、上方向ベクトル（ＶｅｃｔｏｒＵＰ）を求めて向きを考慮した角度Θ’を算出する。上方向ベクトルのＹ値が正の場合は、角度Θ’＝−Θとし、負の場合は、角度Θ’＝Θ、とする。

軸定義の変換と、座標値の変換は逆になるため、Ｙ軸周りの（−Θ’）回転行列を、行列Ｍ２とする。

図１１Ｃに戻って、実座標系定義変換行列Ｍ３は、Ｘ軸周りの−９０度回転する変換行列である。実座標系定義変換行列Ｍ３によって、（３）特殊実座標系１１１３を、（４）特殊実座標系２、１１１４に変換することができる。

実座標系定義変換行列Ｍ４は、右手系から左手系に変換する変換行列である。ＸとＹを互いに変換する。実座標系定義変換行列Ｍ４によって、（４）特殊実座標系２、１１１４を、（５）実座標系、すなわち、平面直角座標系（左手系）１１１５に変換することができる。ここでは、本来の平面直角座標系と原点位置が異なっている。

位置移動変換行列Ｍ５は、原点位置を移動させる変換行列である。具体的には、カメラの初期実座標位置（初期カメラ位置）を平行移動し、初期カメラ位置＝平面直角座標系Ｘ［ｍ］，平面直角座標系Ｙ［ｍ］，標高［ｍ］とする。位置移動変換行列Ｍ５によって、（５）平面直角座標系（左手系）１１１５を、（６）本来の平面直角座標系（左手系）１１１６に変換することができる。

このようにして、ＳＬＡＭローカル系１１１１（図１１Ａに示した１１０１）を実座標（平面直角座標系（左手系））１１１６（図１１Ａに示した１１０２）とすることができる。

初期姿勢・座標系設定部５１１は、この座標系変換行列Ｍを保持することで、従来のＶ−ＳＬＡＭと同様に算出していたＳＬＡＭローカル座標系での初期環境マップを、平面直角座標系の実座標環境マップに変換することができる。この結果、画像由来で特に大きさには意味がなかったＳＬＡＭローカル系のスケールを、本システムにおいて使うことで、ｍ単位の実座標スケールに統一することが可能になる。

なお、初期姿勢・座標系設定部５１１は、必要があれば、変換行列を持つだけでなく、実際に算出済の特徴点群の３次元初期位置や、２画像の撮影位置・姿勢位置を、この変換行列を用いて実座標系の値に変換してもよい。特に、頻繁に参照する特徴点群の位置は、後述するトラッキング処理機能（フレーム姿勢推定）失敗時の再初期化実施前後で統一のとれた値として使うため、実座標系の値として保持することが望ましい。実座標系の値としてあらかじめ保持しておけば、各画像上への投影位置は、該変換抜きで計算することができる。

一方で、平面直角座標系のような実座標系の値は、数値が非常に大きくなることが多いため、環境マップの特徴点群３次元位置は従来と同じローカル座標系の値のまま保持し、加えて新たに変換行列も保持することで、必要な時だけ変換行列を用いて実座標系の値に変換してもよい。あるいは、実座標系の値であっても、適当な初期値からの差分値としてもよい。

本システム５００においては、従来と同じローカル座標系で初期姿勢・座標系設定をおこなってから、実座標系に変換するための情報を作成して、以後の３次元座標値はすべて実座標変換をおこなった実座標系の値で保持するものとして、説明する。

なお、既存の実座標環境マップを入力する場合には、初期姿勢・座標系設定部の処理を飛ばし、入力した実座標環境マップを初期実座標環境マップとして、以後の処理と同様の
処理を実施する。

本システム５００においては、従来のＶ−ＳＬＡＭと同様に、初期姿勢・座標系設定部５１１の処理を２画像（初期ＫＦ）に対して実施すると、初期化が完了されたとみなして、以後の処理を、まだ処理していない画像に対して順次実施していくことにする。したがって、以後の処理は、初期化に用いた２画像（初期ＫＦ）に対しては実施せず、それ以後の画像に実施するようにする。

以後の処理である、トラッキング処理機能、マッピング処理機能、ループクローズ処理機能は、説明を簡単にするため、シーケンシャルに処理する形とする。実際には複数スレッドを用いた同時処理であってもよい。その場合には、各処理機能がそれぞれ、内部保持するＫＦ位置・姿勢や、実座標環境マップを相互参照するので、適宜、既存の編集ロック機能などを用いて複数処理での同時編集を防ぐことができる。各処理機能は、処理対象の画像がなくなるまで、映像の各画像を順に処理していくこととする。

（フレーム姿勢推定部５２１の内容）
図１０において、位置姿勢推定（トラッキング）処理機能５２０を担当するフレーム姿勢推定部５２１は、通常の処理に失敗した時の対応処理（後述するリローカリゼーション失敗時の処理）以外は、従来のＶ−ＳＬＡＭと同様の処理をおこなう。すなわち、フレーム姿勢推定部５２１は、カメラ移動した入力新画像（歪み補正済）に対し画像特徴群を算出し、画像特徴量を比較することで同じ特徴点と思われる算出済の３Ｄ特徴点群の位置（実座標環境マップ５５０）を得る。

このとき、定速走行とみなし新画像カメラの初期位置・姿勢を推定し、１つ前の画像で用いた３Ｄ特徴点群を、推定した初期位置・姿勢を用いて新画像上に投影する。そして、その付近で対応する特徴点を検索することによって、同じ特徴点と思われる３Ｄ特徴点の候補を絞り込むようにしてもよい。

その後、同じ特徴点として見つかった３Ｄ特徴点群全体に対し、新画像上への再投影誤差が小さくなるように、新画像カメラの位置・姿勢を最適化する。すなわち、３Ｄ特徴点群の位置は変化させずに、カメラ位置・姿勢だけ最適化するＢＡをおこなう。続けて、新画像と３Ｄ特徴点群を共有する第１のＫＦ群を探して、ついで、当該第１のＫＦ群１と３Ｄ特徴点群を共有する第２のＫＦ群を探し、これらの第１のＫＦ群、第２のＫＦ群の３Ｄ特徴点群を得る。

このとき、得た３Ｄ特徴点群に対し、新画像のカメラ位置からの距離（規定距離の範囲内）や、カメラからの閲覧方向の違い（たとえば、新画像のカメラ位置から該３Ｄ特徴点へ向けた閲覧方向ベクトルと、今迄のＫＦ群のカメラ位置から該特徴点へ向けた閲覧方向ベクトルとの内積の大きさが規定値以上）などを用いて、任意の取捨選択をしてもよい。第１のＫＦ群、第２のＫＦ群から得た、より多くの３Ｄ特徴点群を使って、フレーム姿勢推定部５２１は、再度新画像上に投影し、再投影誤差が小さくなるような位置・姿勢最適化を実行する。

フレーム姿勢推定部５２１は、具体的には、たとえば、図６に示した、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。

なお、フレーム姿勢推定部５２１で、十分な３Ｄ特徴点群が得られないなどの理由で、位置・姿勢推定に失敗した場合には、従来のＶ−ＳＬＡＭと同様に、位置・姿勢復帰処理であるリローカリゼーション処理を実施するようにしてもよい。リローカリゼーション処理では、画像特徴的に似たＫＦがないか全ＫＦ群を探索し、似たＫＦ候補群が見つかったら、それらＫＦの３Ｄ特徴点群と新画像の特徴点群とのマッチングをおこない、マッチング数が多いＫＦを最終的なＫＦとして選定するようにしてもよい。

リローカリゼーション処理は、つぎに、当該ＫＦと新画像のマッチングした特徴点群どうしを使って、より少数の特徴点を用いた既知のＰｎＰ問題を解くことによって、初期位置・姿勢推定をおこなう。そして、得た新たな新画像の初期位置・姿勢から、より多くの特徴点群を使った非線形最小二乗法などの任意の最適化手法を用いて、位置・姿勢最適化を実施し、当該新画像の推定カメラ位置・姿勢とする。

ここまで、本システム５００のフレーム姿勢推定部５２１は、従来のＶ−ＳＬＡＭと同じ処理を実施する。一方で、本システム５００のフレーム姿勢推定部５２１は、上述したリローカリゼーション処理も失敗した場合の処理が、従来のＶ−ＳＬＡＭと異なる。リローカリゼーション処理も失敗した場合には、従来のＶ−ＳＬＡＭでは、処理続行が不能であるため、そのまま処理を終了する。しかし、本システム５００の場合は、処理終了をする代わりに初期姿勢・座標系設定部５１１に戻って、既存の実座標環境マップ５５０や全画像位置姿勢データ５６０などの内部算出データを残したまま、初期化処理を再実施することができる。

従来のＶ−ＳＬＡＭでは、リローカリゼーション失敗は、今迄の追跡してきた画像およびＫＦ群と対応関係がまったく取れなくなったことを意味する。上述したように、従来のＶ−ＳＬＡＭは、算出するＳＬＡＭローカル系が初期化で用いた初期画像に関係する座標系であるため、一度対応関係が取れなくなると、以後、再度初期化処理をおこなっても、今迄の算出した環境マップとは異なる新たな初期画像に関する座標系で計算を始めるため、実質的に対応が取れなくなったところまでと、再度初期化を始めて以降とでは、算出する環境マップおよび、カメラの位置・姿勢の値の対応が取れず、実質的に別物の細切れとなってしまう。

このため、従来のＶ−ＳＬＡＭは、リローカリゼーション失敗時には、初期化処理をおこなっても意味が無いため、初期化処理をおこなわずに処理終了としていた。しかし、本システム５００では、初期化処理として初期化後の座標系およびＶ−ＳＬＡＭの値は、すべて実座標系とすることができるので、今迄追跡してきた画像およびＫＦ群との対応関係が取れなくなっても、実座標系の値である以上、算出する環境マップやカメラ位置・姿勢の値は整合性の取れた値となっている。

これにより、初期化の前後で算出する環境マップおよびカメラ位置・姿勢の値は、そのまま両方を混ぜて保持しても問題がないため、本システム５００においては、従来のＶ−ＳＬＡＭと異なり、リローカリゼーション失敗時には、初期姿勢・座標系設定部５１１による処理を再度実施する。このとき、上述したように、特に多数かつ頻繁に参照をする特徴点群の３次元位置は、ＳＬＡＭローカル座標系の値と実座標系への変換行列として保持していると、初期化処理を実行するたびに、双方の値が変わってしまう（積算した実座標系の値は同じ）可能性があって煩雑なため、できるだけ実座標系の値にして保持しておくことが望ましい。

（ＫＦ更新部５２２の内容）
図１０において、位置姿勢推定（トラッキング）処理機能５２０を担当するＫＦ（キーフレーム）更新部５２２は、従来のＶ−ＳＬＡＭのように画像特徴的に新画像をＫＦにするか判定する。また、本システム５００のＫＦ更新部５２２は、さらにＧＮＳＳ位置を保持する画像か否かも用いて、新たにＫＦ画像を選定する。すなわち、すべての画像にＧＮＳＳ位置が無く、ＧＮＳＳ位置が無い画像が規定数以上続いた場合において、ＧＮＳＳ位置がある画像が新画像として入力された場合に、画像特徴的な従来の判定の結果に関わらず、新画像を新しいＫＦとして採用する。

なお、従来のＶ−ＳＬＡＭの画像特徴的なＫＦにするか否かの判定は、たとえば、最後のＫＦからの経過時間や経過フレーム数が規定値を超えた場合や、フレーム姿勢推定部で取得したＫＦ群１のうち、最も新画像と３Ｄ特徴点群を共有するＫＦとの共有３Ｄ特徴点群数が、規定数以下だった場合、などである。その後、新追加するＫＦに対し、ＫＦ更新部は、ＫＦとされた新画像を実座標環境マップのＫＦ群に追加する。上述したように、特徴点を共有するＫＦ群どうしで別途グラフ構造（ＫＦ群１）を保持している場合には、適宜、新追加するＫＦ（新画像）についても、該グラフ構造を更新する。

ＫＦ更新部５２２は、具体的には、たとえば、図６に示した、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。

図１２は、ＫＦ更新部の処理の手順の一例を示すフローチャートである。図１２のフローチャートにおいて、ＫＦ更新部５２２において、現在の画像が、現在利用中のＫＦから規定フレーム数以上離れているか否かについて判断する（ステップＳ１２０１）。ここで、規定フレーム数以上離れていない場合（ステップＳ１２０１：Ｎｏ）は、何もせずに、一連の処理を終了する。

一方、規定フレーム数以上離れている場合（ステップＳ１２０１：Ｙｅｓ）は、つぎに、現在の画像が、現在利用中のＫＦとの共通特徴点が規定数以下か否かを判断する（ステップＳ１２０２）。ここで、共通特徴点が規定数以下でない場合（ステップＳ１２０２：Ｎｏ）は、ステップＳ１２０４へ移行する。一方、共通特徴点が規定数以下である場合（ステップＳ１２０２：Ｙｅｓ）は、つぎに、現在の画像フレームが、現在利用中のＫＦと最も共通特徴点が多い他のＫＦに対し、共通特徴点が規定数以下か否かを判断する（ステップＳ１２０３）。

ステップＳ１２０３において、共通特徴点が規定数以下でない場合（ステップＳ１２０３：Ｎｏ）は、ステップＳ１２０４へ移行する。一方、共通特徴点が規定数以下である場合（ステップＳ１２０３：Ｙｅｓ）は、ステップＳ１２０５へ移行する。つぎに、ステップＳ１２０４において、現在の画像が、ＧＮＳＳの位置情報を保持しているか否かを判断する（ステップＳ１２０４）。ここで、ＧＮＳＳの位置情報を保持していない場合（ステップＳ１２０４：Ｎｏ）は、一連の処理を終了する。一方、ＧＮＳＳの位置情報を保持している場合（ステップＳ１２０４：Ｙｅｓ）は、ステップＳ１２０５へ移行する。

ステップＳ１２０５において、現在のＫＦは、ＧＮＳＳ位置情報を保持する最も新しいＫＦから規定ＫＦ数以上離れているか否かを判断する（ステップＳ１２０５）。ここで、最も新しいＫＦから規定ＫＦ数以上離れている場合（ステップＳ１２０５：ＹＥＳ）は、ステップＳ１２０６へ移行する。一方、離れていない場合（ステップＳ１２０５：ＮＯ）は、一連の処理を終了する。

ステップＳ１２０６において、現在の画像を新ＫＦとする（ステップＳ１２０６）。そして、新ＫＦを実座標環境マップのＫＦ群に追加する（ステップＳ１２０７）。さらに、ＫＦ群の特徴点共有関係のグラフ構造に新ＫＦを追加し、グラフを更新する（ステップＳ１２０８）。これにより、一連の処理を終了する。

なお、ＫＦの追加判断だけを、トラッキング処理機能５２０を担当する処理部いずれか（たとえばＫＦ更新部５２２）でおこない、実際のＫＦ追加処理を独立させ、ローカルマッピング処理機能５３０を担当する処理部５３１〜５３３のいずれかで実施するようにしてもよい。

本システム５００においては、ＫＦ追加処理を、ＫＦ更新部５２２で実施するものとして説明した。しかし、トラッキング処理機能５２０は、全画像フレームに対する処理であり、ローカルマッピング処理機能５３０は、ＫＦ追加タイミングで実施するＫＦに関する処理として考える方を優先させるなら、ＫＦ追加処理を実施するか否かの判断のみをトラッキング処理機能５２０を担当する処理部５２１〜５２２のいずれかで実施し、実際のＫＦ追加処理は、ローカルマッピング処理機能５３０を担当する処理部５３１〜５３３のいずれかで実施した方がよい。従来のＶ−ＳＬＡＭにおいても、ＫＦ追加処理自体は、ローカルマッピング処理機能５３０を担当する処理部５３１〜５３３のいずれかで実施することが多い。

（３Ｄマップ特徴点更新部５３１の内容）
環境マップ作成（ローカルマッピング）処理機能５３０を担当する３Ｄマップ特徴点更新部５３１は、従来のＶ−ＳＬＡＭと同様の、追加したＫＦを使って最近追加した３Ｄマップ点の除去判断を実施するととともに、新たな３Ｄマップ点の追加処理を実行する。

３Ｄマップ特徴点更新部５３１は、具体的には、たとえば、図６に示した、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。

３Ｄマップ点の除去判断処理として、３Ｄマップ特徴点更新部５３１は、最近追加した３Ｄマップ点群が、新追加ＫＦを含めたＫＦ群全体で規定数以上のＫＦから閲覧できているか否かなどから、３Ｄマップ点が利用されているか否かを判定する。そして、３Ｄマップ点が利用されていないと判断された場合には、３Ｄマップ点を除去する。

なお、３Ｄマップ特徴点更新部５３１では、除去判断だけおこない、実際の除去処理は、続くＫＦ姿勢・特徴点マップ最適化部５３３のＢＡなどの３Ｄマップ点の利用有無を別途詳細に調査利用する処理などと同時に実施してもよい。

新３Ｄマップ追加処理として、３Ｄマップ特徴点更新部５３１は、追加した新ＫＦで３Ｄ特徴点群と対応付いていない特徴点を探し、ＫＦ更新部５２２で更新した新ＫＦと特徴点を共有する第１のＫＦ群の同じく対応付いていない特徴点と、画像特徴量から同じ特徴点を探す。このとき、さらに当該ＫＦでのエピポーラ制約や再投影誤差などの任意の方法で、同じ特徴点か否かを絞り込んでもよい。同じ特徴点が見つかった場合には、２つのＫＦのカメラ位置と、当該ＫＦ上の画像に映る同じ特徴点の画像内位置を用いて、既知の三角測量の手法を使って、当該特徴点の３次元位置を求め、新たな３Ｄ特徴点として、実座標環境マップに追加する。

（グラフ制約生成部５３２の内容）
環境マップ作成（ローカルマッピング）処理機能５３０を担当するグラフ制約生成部５３２は、続くＫＦ姿勢・特徴点マップ最適化部５３３で、従来のように現キーフレームと周辺の特徴点群３次元位置をＢＡで求める処理（ローカルＢＡ）をおこなう前に、新たに、現キーフレームの位置と周辺の特徴点群の３次元位置を入力ＧＮＳＳ情報に合わせて姿勢グラフを用いて事前に補正する処理を実施するため、その準備をおこなう処理部である。

グラフ制約生成部５３２は、具体的には、たとえば、図６に示した、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。

姿勢グラフという概念、および、グラフ構造を用いた最適化計算自体は、一般的なグラフ理論（姿勢グラフ構造）に基づく最適化計算と同じでもよく、既存のｇ２ｏ（Ｇｅｎｅｒａｌ Ｇｒａｐｈ Ｏｐｔｉｍｉｚａｔｉｏｎ）などの最適化ライブラリを用いてもよい。

グラフ制約生成部５３２は、この一般的なグラフ構造を活用し、ＫＦ位置・姿勢のみの最適化（実座標環境マップのうちＫＦ情報群のおおまかな最適化）と、当該最適化後のＫＦ群と周辺の特徴点群の双方を使った位置・姿勢の最適化（実座標環境マップ全体の詳細最適化）、という２段階の最適化向けに、それぞれ最適化対象（ノード）と拘束条件（エッジ）の異なる２つの姿勢グラフを作成する。

なお、本システム５００において、ＫＦ位置・姿勢のみの最適化と、周辺特徴点群も用いた最適化の２段階でＧＮＳＳ情報に合わせた最適化を実施しているが、これは一例であり、ＫＦ位置と周辺特徴群を用いた最適化を一度だけ実施してもよいし、さらにもう一度ＫＦ群のみの最適化をして３段階の最適化をしてもよいし、順番を変更してもよい。このとき、この最適化を何段階で実施するかをＫＦごとに変更するようにしてもよい。また、ＫＦ群の位置・姿勢の最適化ではなく、ＫＦ群の位置またはＫＦ群の姿勢どちらか片方のみの最適化としてもよい。

最適化対象となるＫＦ群が周辺特徴点群と比べて数が少ないため、ＫＦの位置であるＧＮＳＳ位置による制約条件による補正効果が効きづらくなるのを避けるため、本システム５００では、まず、ＫＦ位置・姿勢を大まかにＧＮＳＳ位置によって十分補正してから、周辺特徴点群を含めた最適化をおこなう二段階としている。

グラフ制約生成部５３２は、第一段の新ＫＦとその近傍のＫＦ群の位置・姿勢の最適化、および、第二段のさらに周辺特徴点群位置も含めた最適化を実施するため、従来のＶ−ＳＬＡＭと異なり、推定済のＫＦ群の３次元位置と、ＧＮＳＳ情報で入力した該ＫＦに対応するＧＮＳＳ位置（３次元位置）とのずれを最小化する、新たな拘束条件を導入したグラフ構造を作成する。

図１３は、グラフ制約生成部の処理の手順の一例を示すフローチャートである。図１３のフローチャートにおいて、まず、新ＫＦにＧＮＳＳ位置があり、かつ、他にＫＦはあるか否かを判断する（ステップＳ１３０１）。この例では、新ＫＦにＧＮＳＳ位置があり、かつ、他にＫＦがある場合のみを対象にしており、それ以外は対象としていないため、新ＫＦにＧＮＳＳ位置がない場合、あるいは、他にＫＦはない場合（ステップＳ１３０１：Ｎｏ）は、何もせずに一連の処理を終了する。

なお、最適化を実施する効果はあまり無いが、新ＫＦにＧＮＳＳ位置がない場合も、ＧＮＳＳ情報でＫＦ群と周辺特徴点群を最適化したい場合には、ステップＳ１３０１では、他にＫＦがあるか否かの判断だけをおこなえばよい。

ステップＳ１３０１において、新ＫＦにＧＮＳＳ位置があり、かつ、他にＫＦがある場合（ステップＳ１３０１：Ｙｅｓ）は、当該新ＫＦを姿勢グラフにノードとして登録する（ステップＳ１３０２）。そして、当該新ＫＦと特徴点を共有するＫＦ群を、位置・姿勢を変更できるノードとして姿勢グラフに追加し、共有特徴点数が多い程、強く相対位置・姿勢を保つエッジを、特徴点群を共有するＫＦどうしに設定する（ステップＳ１３０３）。

つぎに、姿勢グラフにないＫＦ群のうち、新ＫＦとの時刻差が閾値以内のＫＦ群を、位置・姿勢変化をおこなわないノードとして姿勢グラフに追加し、他の登録済ＫＦのうち、最も共有特徴点数が多いＫＦと共有特徴点数が多い程、強く相対位置・姿勢を保つエッジを設定する（ステップＳ１３０４）。

そして、各ＫＦでＧＮＳＳ位置があれば、ＧＮＳＳ位置を当該ＫＦと繋がるノードとして追加し、ＧＮＳＳ位置と当該ＫＦの間に、ＧＮＳＳ位置と当該ＫＦの位置差がより小さくなるエッジを設定し（ステップＳ１３０５）、当該姿勢グラフを、ＫＦ群の位置・姿勢最適化用グラフ（第１の姿勢グラフ）とする（ステップＳ１３０６）。

つぎに、当該姿勢グラフの位置・姿勢変化をおこなうＫＦ群から閲覧できる３次元位置を持った特徴点群を当該姿勢グラフの位置変化をおこなうノードとして追加し、位置・姿勢変化をおこなわないＫＦ群からのみ閲覧できる３次元位置を持った特徴点群を当該姿勢グラフの位置変化をおこなわないノードとして追加し、それぞれ閲覧するＫＦとの間に、再投影誤差をより小さくするエッジを設定する（ステップＳ１３０７）。そして、当該姿勢グラフのＫＦ群どうしを結ぶエッジを削除する（ステップＳ１３０８）。さらに、特徴点ノード数とＧＮＳＳノード数を調べ、エッジ種類ごとの重みを調整する（ステップＳ１３０９）。この当該姿勢グラフを、ＫＦの位置・姿勢と周辺マップ特徴点位置の最適化用グラフ（第２の姿勢グラフ）とし（ステップＳ１３１０）、一連の処理を終了する。

このようにして、グラフ制約生成部５３２の処理によって、新しい２つの姿勢グラフ（第１の姿勢グラフおよび第２の姿勢グラフ）を生成することができる。なお、図１３のフローチャートにおいては、わかりやすくするために、毎回グラフを作成するようにしたが、既存グラフを更新して内容を作成するようにしてもよい。

（ＫＦ姿勢・特徴点マップ最適化部５３３の内容）
環境マップ作成（ローカルマッピング）処理機能５３０を担当するＫＦ姿勢・特徴点マップ最適化部５３３は、新たに、グラフ制約生成部５３２において生成した新しい２つの姿勢グラフを用いて、一般的なグラフ最適化計算をおこなう。そして、ＫＦ姿勢・特徴点マップ最適化部５３３は、入力されたＧＮＳＳ情報から得たＫＦのＧＮＳＳ位置に合わせたＫＦ位置・姿勢の最適化と、最適化したＫＦ群の周辺の特徴点群の３次元位置の最適化、という２つの最適化を実施する。

ＫＦ姿勢・特徴点マップ最適化部５３３は、具体的には、たとえば、図６に示した、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。

ＫＦ姿勢・特徴点マップ最適化部５３３は、その後さらに、ＧＮＳＳ位置に合わせて補正最適化済のＫＦ群の位置・姿勢と、特徴点群の３次元位置（実座標環境マップ）と、を用いて、従来のＶ−ＳＬＡＭと同じように、新ＫＦ近傍でのＢＡ処理（ローカルＢＡ）の実施を通して、画像上の被写体の映りをメインとした位置・姿勢補正をおこなう。

この結果、追加した新ＫＦの影響として、新ＫＦに関係の深いＫＦ群、多くは映像時系列に近いＫＦ群の位置・姿勢の調整と、新ＫＦの周辺の特徴点群の３次元位置の調整とを、入力ＧＮＳＳ情報にあらかじめ合わせておいてから、従来の画像の特徴点の再投影誤差を用いた最適化を実施して位置・姿勢を微調整することになる。したがって、従来のＶ−ＳＬＡＭの課題である、徐々にスケールドリフトが発生するという課題を解決した実座標値で出力することが可能となる。

なお、本システム５００のグラフ制約生成部５３２における姿勢グラフ生成と、ＫＦ姿勢・特徴点マップ最適化部５３３における、ローカルＢＡに先立って実施する、当該姿勢グラフを用いたグラフ最適化処理は、すべての新ＫＦに対して実行してもよいが、前述したように、新ＫＦにＧＮＳＳ位置がある場合だけ実行してもよい。たとえば、入力映像の一部画像にのみＧＮＳＳ位置がある場合に、ＧＮＳＳ位置が無い区間が終わり新たにＧＮＳＳ位置が得られた時だけ、上述したように、ＫＦ更新部５２２においてＧＮＳＳ位置のある画像を積極的にＫＦとして判定し（図１２のステップＳ１２０４、およびＳ１２０５）、得られた当該ＧＮＳＳ位置を必ず用いた実座標環境マップ（ＫＦ群の位置・姿勢、と特徴点群の３次元位置）５５０の補正処理をおこなう（図１３のステップＳ１３０１）ようにしてもよい。

一方で、新ＫＦにＧＮＳＳ位置がない時は、従来のＶ−ＳＬＡＭと同じように、画像特徴変化による位置・姿勢推定をおこなうことにより、ＧＮＳＳ位置が無い場合の無駄な姿勢グラフによる最適化処理を省き、必要なときだけ処理を実施することが可能となる。同様に、実座標環境マップ５５０を入力とする場合で、ＧＮＳＳ情報の入力がない場合も、本処理を省略してもよい。

また、必須ではないが、上述したように、入力姿勢情報があり、かつ、ＫＦ群に入力姿勢情報があるのであれば、ＧＮＳＳ位置と同様に姿勢情報を姿勢グラフに含めて最適化に用いてもよい。

（ループ検出・クロージング部５４１の内容）
ループクローズ処理機能５４０を担当するループ検出・クロージング部５４１は、従来のＶ−ＳＬＡＭと同様に、新ＫＦと保持するＫＦ画像群との画像全体の画像特徴量を比較して類似度を調べて、映像取得時の走行経路で同じ場所を複数回走行していないか（ループが発生していないか）を確認する。そして、類似度が高く同じ場所を走行していると思われた場合には、関係する実座標環境マップ５５０のＫＦ群情報５５１の「ループＫＦ ＩＤ」に、該当する過去に同場所走行した時のＫＦ群を設定して、互いの参照を可能としておく。

また、ループ検出・クロージング部５４１は、ループ発生時の新ＫＦに対し、新ＫＦ近傍のＫＦ群を用いたローカルＢＡ、または、全ＫＦ群を用いたグローバルＢＡのいずれかを実施して、同場所走行時のＫＦの位置関係を調整する。新ＫＦ近傍のＫＦ群は、マップ特徴点の共有状態などから選定してもよく、過去に同場所走行した時のＫＦ群との共有状態を利用してもよい。

ループ検出・クロージング部５４１は、具体的には、たとえば、図６に示した、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。

なお、本システム５００においては、上述した各処理を通して、入力ＧＮＳＳ情報１００２を用いてスケールドリフトが起きないように、実座標環境マップ５５０を構築済である。したがって、スケールドリフト対策が主体のループ検出・クロージング部５４１における処理は省略してもよい。

以上説明したように、本実施の形態によれば、撮影された時系列画像（たとえば映像１００１）のうちの任意の画像について、任意の画像のＧＮＳＳ情報（第１の撮影位置）１００２を測位により取得し、任意の画像の特徴から、当該任意の画像の第２の撮影位置を推定する。そして、第１の撮影位置と、第２の撮影位置とのずれを最小化する拘束条件を用いて、任意の画像の撮影位置、任意の画像の撮影姿勢、および、任意の画像の中で抽出された特徴点の推定された３次元位置、の少なくともいずれか一つを調整する。

このように、単カメラ映像とともに取得したＧＮＳＳ情報を使って、緯度経度の実座標で、該撮影カメラの位置・姿勢を推定し、同時に実座標で環境マップを作成するＶ−ＳＬＡＭ機能を持つ。この結果、Ｖ−ＳＬＡＭの算出処理の一部修正という低い処理コストで、従来のＶ−ＳＬＡＭの課題であった、スケールドリフトの無い実座標環境マップと、映像撮影位置・姿勢を推定することができる。これにより、高精度な位置推定をおこなうことができる。

特に、本実施の形態の初期化処理は、従来のＶ−ＳＬＡＭの初期化のように初期化に用いた画像依存のローカル座標系ではなく、常に同じ緯度経度由来の実座標系を用いることができるため、映像の途中でトラッキングが失敗した場合でも、必要であれば、再度初期化を実施して、処理を続行することができる。この結果、従来のＶ−ＳＬＡＭのように、トラッキングが失敗するごとに細切れになった、まったくスケールの異なる座標系で作成された複数の環境マップや、カメラ撮影位置・姿勢データ群を作成することがない。

すなわち、本実施の形態による処理をおこなうことにより、映像全体で同じ実座標スケールの１つの長い環境マップと、カメラ撮影位置・姿勢データを作成することができる（ただし、失敗箇所だけマップ特徴点がやや疎となったり、位置・姿勢推定データが抜ける可能性はある）。

この長い環境マップは、他の映像を用いたＶ−ＳＬＡＭ処理の入力として使用することができるので、ある映像では移動体で周辺建物が映らずトラッキング失敗していた場所があっても、その失敗箇所を含んだ環境マップを移動体による隠れがない他の映像とともに入力して、本実施の形態によるＶ−ＳＬＡＭ処理を実行することで、トラッキング失敗箇所の特徴点を新しい映像の特徴点で補うことができ、より精度の高い環境マップを構築することができる。

特に、走行量の多い道路など、移動体などによる隠れが頻繁に発生する場所であっても、撮影した映像中での隠れを気にすることなく、本実施の形態によって複数映像で欠落箇所を補い合うことができ、環境マップを高精度に作成することができる。

また、本実施の形態によれば、入力ＧＮＳＳ情報に合わせて、実座標環境マップを作成することができるため、一度だけ測量車のような精度の高いＧＮＳＳ情報と映像を同時取得できる車両で取得したデータを用いて、実座標環境マップを作成すれば、当該実座標環境マップを映像しか取得できない任意の一般車の映像に入力適用することで、任意の車の映像の撮影位置・姿勢を、マップ作成時の測量車と同じ精度かつ同じ実座標系で、マップ作成処理をおこなわない低処理コストで推定することができる。この結果、一般車映像を使ったサービスを、ＧＮＳＳ位置の測定記録装置の有無に関わらず実現できるため、より多くの車両データを簡単に収集活用することが可能となる。

さらに、本実施の形態によれば、撮影位置だけを後で線形的にスケール補正するのではなく、独自のＫＦ選定処理および最適化処理によって、ＧＮＳＳ情報（ＧＮＳＳ位置との位置誤差）と、画像解析による被写体の見えの正しさ（再投影誤差など）の双方を使って、補正範囲を自動調整しながら、撮影位置に加えて姿勢をも補正することができる。この結果、右左折などの従来のＶ−ＳＬＡＭでは大きくスケールが変わる場所でも、同じ実座標（同じスケール）による高精度な撮影位置・姿勢推定が可能となる。

また、この補正時のＧＮＳＳ情報の重みを適宜変えることもできるので、ＧＮＳＳ情報の信頼度によっては、画像解析による被写体の見えの正しさを優先するような、フレキシブルな最適化をおこなうことも可能である。たとえば、高精度ＧＮＳＳ情報を取得できる測量車のデータに対しては、よりＧＮＳＳ情報の重みを重視した最適化とし、一般車のＧＰＳ情報のような低精度ＧＮＳＳ情報を含むデータに対しては、ＧＮＳＳ情報の重みをやや落として画像解析結果を重視する最適化とすることができる。この結果、ＧＮＳＳ情報の信頼度を勘案した撮影位置・姿勢推定、および実座標環境マップ作成が可能となる。

なお、本実施の形態で説明した移動体位置推定方法は、あらかじめ用意されたプログラムをパーソナルコンピュータやワークステーションなどのコンピュータで実行することにより実現することができる。プログラム配信プログラムは、ハードディスク、フレキシブルディスク、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｃ）−ＲＯＭ、ＭＯ（Ｍａｇｎｅｔｏ−Ｏｐｔｉｃａｌ Ｄｉｓｋ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリなどのコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。また、移動体位置推定プログラムは、インターネットなどのネットワークを介して配布してもよい。

上述した実施の形態に関し、さらに以下の付記を開示する。

（付記１）撮影された時系列画像のうちの任意の画像について、当該任意の画像の第１の撮影位置を測位により取得し、
前記任意の画像の特徴から、当該任意の画像の第２の撮影位置を推定し、
前記第１の撮影位置と、前記第２の撮影位置とのずれを最小化する拘束条件を用いて、前記任意の画像の撮影位置、前記任意の画像の撮影姿勢、および、前記任意の画像の中で抽出された特徴点の推定された３次元位置、の少なくともいずれか一つを調整する、
情報処理装置を有することを特徴とする移動体位置推定システム。

（付記２）前記情報処理装置は、
前記時系列画像の各画像について、相互に区別可能な特徴量を持つ画像特徴点を複数抽出し、
前記時系列画像の系列中で近傍となる複数の画像相互間で、特徴量が類似する前記画像特徴点どうしを対応付け、
対応付けられた各特徴点の各前記画像上での２次元位置を用いて、前記第２の撮影位置を推定する、
ことを特徴とする付記１に記載の移動体位置推定システム。

（付記３）前記情報処理装置は、
最初に推定する少なくとも２つの画像の前記第２の撮影位置を、前記第１の撮影位置と一致させるように決定する、
ことを特徴とする付記１または２に記載の移動体位置推定システム。

（付記４）前記時系列画像は、移動体の撮像手段によって撮影されたものであることを特徴とする付記１〜３のいずれか一つに記載の移動体位置推定システム。

（付記５）前記第１の撮影位置は、ＧＮＳＳ情報にかかる前記移動体の位置であることを特徴とする付記４に記載の移動体位置推定システム。

（付記６）前記情報処理装置は、
前記任意の画像からの時間的な近さ、距離的な近さ、および、前記任意の画像と共通する特徴点の有無、の少なくともいずれかに基づいて、前記時系列画像の中から、前記任意の画像とともに、前記拘束条件を用いた調整の対象となる画像群を決定することを特徴とする付記１〜５のいずれか一つに記載の移動体位置推定システム。

（付記７）前記任意の画像から時間的または距離的に近く、かつ、当該任意の画像と共通する特徴点がある画像は、前記調整の対象となる画像群とすることを特徴とする付記６に記載の移動体位置推定システム。

（付記８）前記任意の画像から時間的または距離的に近くない画像は、前記調整の対象となる画像群としないことを特徴とする付記６に記載の移動体位置推定システム。

（付記９）前記任意の画像と共通する特徴点がない画像は、前記調整の対象となる画像群としないことを特徴とする付記６に記載の移動体位置推定システム。

（付記１０）前記任意の画像からの時間的または距離的に近く、かつ、当該任意の画像と共通する特徴点がない画像は、前記調整の対象となる画像群とせず、前記調整の際に参照する画像群とすることを特徴とする付記６に記載の移動体位置推定システム。

（付記１１）情報処理装置が、
撮影された時系列画像のうちの任意の画像について、当該任意の画像の第１の撮影位置を測位により取得し、
前記任意の画像の特徴から、当該任意の画像の第２の撮影位置を推定し、
前記第１の撮影位置と、前記第２の撮影位置とのずれを最小化する拘束条件を用いて、前記任意の画像の撮影位置、前記任意の画像の撮影姿勢、および、前記任意の画像の中で抽出された特徴点の推定された３次元位置、の少なくともいずれか一つを調整する、
ことを特徴とする移動体位置推定方法。

ＫＦ１〜ＫＦ１６ キーフレーム
Ｇ１〜Ｇ１６ ＧＮＳＳ値（ＧＮＳＳ位置）
Ｃ１〜Ｃ１６ 拘束条件
Ｎ１１〜Ｎ１６ 特徴点
５００ 移動体位置推定システム
５０１ サーバ（情報処理装置／移動体位置推定装置）
５０２ 車載機（情報収集装置）
５０３ 移動体
５０４ ネットワーク
５０５ 衛星
５１０ システムの初期化処理機能
５１１ 初期姿勢・座標系設定部
５２０ 位置姿勢推定（トラッキング）処理機能
５２１ フレーム姿勢推定部
５２２ ＫＦ（キーフレーム）更新部
５３０ 環境マップ作成（ローカルマッピング）処理機能
５３１ ３Ｄマップ特徴点更新部
５３２ グラフ制約生成部
５３３ ＫＦ（キーフレーム）姿勢・特徴点マップ最適化部
５４０ ループクローズ処理機能
５４１ ループ検出・クロージング部
５５０ 実座標環境マップ
５５１ ＫＦ（キーフレーム）群情報
５５２ 特徴点群情報
５６０ 全画像位置姿勢データ

Claims (7)

1. 撮影された時系列画像のうちの任意の画像について、当該任意の画像の第１の撮影位置を測位により取得し、
   前記任意の画像の特徴から、当該任意の画像の第２の撮影位置を推定し、
   前記第１の撮影位置と、前記第２の撮影位置とのずれを最小化する拘束条件を用いて、前記任意の画像の撮影位置、前記任意の画像の撮影姿勢、および、前記任意の画像の中で抽出された特徴点の推定された３次元位置、の少なくともいずれか一つを調整する、
   情報処理装置を有することを特徴とする移動体位置推定システム。
2. 前記情報処理装置は、
   前記時系列画像の各画像について、相互に区別可能な特徴量を持つ画像特徴点を複数抽出し、
   前記時系列画像の系列中で近傍となる複数の画像相互間で、特徴量が類似する前記画像特徴点どうしを対応付け、
   対応付けられた各特徴点の各前記画像上での２次元位置を用いて、前記第２の撮影位置を推定する、
   ことを特徴とする請求項１に記載の移動体位置推定システム。
3. 前記情報処理装置は、
   最初に推定する少なくとも２つの画像の前記第２の撮影位置を、前記第１の撮影位置と一致させるように決定する、
   ことを特徴とする請求項１または２に記載の移動体位置推定システム。
4. 前記時系列画像は、移動体の撮像手段によって撮影されたものであることを特徴とする請求項１〜３のいずれか一つに記載の移動体位置推定システム。
5. 前記第１の撮影位置は、ＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）情報にかかる前記移動体の位置であることを特徴とする請求項４に記載の移動体位置推定システム。
6. 前記情報処理装置は、
   前記任意の画像からの時間的な近さ、距離的な近さ、および、前記任意の画像と共通する特徴点の有無、の少なくともいずれかに基づいて、前記時系列画像の中から、前記任意の画像とともに、前記拘束条件を用いた調整の対象となる画像群を決定することを特徴とする請求項１〜５のいずれか一つに記載の移動体位置推定システム。
7. 情報処理装置が、
   撮影された時系列画像のうちの任意の画像について、当該任意の画像の第１の撮影位置を測位により取得し、
   前記任意の画像の特徴から、当該任意の画像の第２の撮影位置を推定し、
   前記第１の撮影位置と、前記第２の撮影位置とのずれを最小化する拘束条件を用いて、前記任意の画像の撮影位置、前記任意の画像の撮影姿勢、および、前記任意の画像の中で抽出された特徴点の推定された３次元位置、の少なくともいずれか一つを調整する、
   ことを特徴とする移動体位置推定方法。

Images