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

Abstract

【課題】高精度な位置推定および環境地図生成をおこなうこと。【解決手段】Ｖ−ＳＬＡＭを用いた移動体位置推定をするにあたり、撮影された時系列画像の任意の画像につき、被写体（特徴点Ａ）１００に対して、移動体の各撮影位置において撮影された画像１０１〜１１０を示している。矢印は進行方向を示している。したがって、各三角形１０１〜１１０は、それぞれの画像が撮影された位置を示しており、また、画像１０１が一番最初に撮影されたものであり、時系列に沿って、１０２→１０３→・・・→１０９→１１０の順で撮影されたことを示している。より被写体（特徴点Ａ）１００が最後に見え始めた、すなわち最も近いときの画像１０８と画像１０９を三角測量を実施する画像ペアとする。見えの変化（交会角）θａが大きい画像のペアで三角測量を実施するため、変化量から位置を推定する三角測量の精度がよくなる。【選択図】図１Ａ

Description

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

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

関連する先行技術としては、時系列的に変化する撮影画像から特徴点の抽出と追跡をおこない、この特徴点を用いて、移動体の走行時におけるカメラ位置・姿勢を推定する技術がある。

特開２０１７−５３７９５号公報 特開２００７−３２２４０３号公報

しかしながら、従来技術では、生成する環境マップ（特徴点群の３次元位置）の精度が悪いという問題点がある。このため、その環境マップを利用した撮影位置姿勢推定も推定精度が悪化したり、環境マップと実際の撮影映像の齟齬が大きすぎて、最適化補正をすることができず、撮影位置姿勢推定自体が失敗してしまい、推定できない区間が発生するという問題点がある。

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

一つの実施態様では、撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなう移動体位置推定システムであって、前記時系列画像を時刻に対して逆行させた逆行画像を取得し、当該逆行画像を用いて前記情報処理をおこなう、情報処理装置を有することを特徴とする移動体位置推定システムが提供される。

また、一つの実施態様では、撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなう移動体位置推定システムであって、 相互に区別可能な特徴量を持つ画像特徴点が映る画像群のうち、最も遅い時刻近傍で撮影した画像を用いて前記情報処理をおこなうことを特徴とする移動体位置推定システムが提供される。

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

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

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

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

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

ＳＬＡＭは、移動中に取得した周辺状況に関する車載データ、たとえばＬＩＤＡＲ（Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）データなどを入力とし、自車走行経路（自車の位置と姿勢）と周辺環境地図（周辺物の３次元位置マップなど）を同時に作成する技術の総称である。

なかでも、Ｖ−ＳＬＡＭは、車載カメラで撮影した映像を入力とし、撮影した映像に映る被写体の変化を用いて、自車走行経路（自車位置・姿勢）と周辺環境地図（周辺の被写体の画像特徴点群の３次元位置マップ、以下、環境地図という）を推定作成することができる技術であり、一般車の単眼映像から自車位置と姿勢を推定することができる。

また、Ｖ−ＳＬＡＭでは、新たに複数画像上で観測された画像特徴点に対し、三角測量で初期３次元位置を推定し、環境マップに追加する。その上で、随時、撮影画像での該環境マップの画像特徴点の想定映り込み状態と実際の撮影画像との違いを考慮しながら、画像群の撮影位置姿勢と、環境マップの画像特徴点群の３次元位置を最適化補正し、最終的な撮影画像群の撮影位置姿勢を得ることができる。

後述する図５に示す、実施の形態にかかる移動体位置推定システム５００は、逆行判定部５１１、逆行画像取得部５１２、初期姿勢・座標系設定部５１３、フレーム姿勢推定部５２１、キーフレーム（ＫＦ）更新部５２２、３Ｄマップ特徴点更新部５３１、グラフ制約生成部５３２、ＫＦ姿勢・特徴点マップ最適化部５３３、ループ検出・クロージング部５４１、の９つのＶ−ＳＬＡＭをベースとする処理部を持ち、かつ、実座標環境マップ５５０（ＫＦ群情報５５１、特徴点群情報５５２）、全画像位置姿勢データ５６０、の２つの内部保持データを持っている。

また、ＫＦ（キーフレーム）は、特に映像に含まれる全画像群の中で特に主要な画像である。Ｖ−ＳＬＡＭでは、ＫＦ画像に対して撮影位置・姿勢を大域／局所の双方で矛盾が無いように、環境地図を用いて詳細な解析技法を用いて推定しておき、残る画像群は、ＫＦからの相対関係を使って簡易に推定する、という方法が取られることが多い。そのため、実施の形態にかかる移動体位置推定方法においても、同様の方法を採用するが、全画像群をＫＦと見なしてもかまわない。

また、環境地図（実座標環境マップ５５０）は、各画像特徴点の３次元位置のほかに、どの画像特徴点はどの画像で用いるかに関する情報を持っている。これは、後述する図５に示す、ＫＦ群情報５５１と、特徴点群情報５５２である。ＫＦ群情報５５１は、主要な映像内の画像（ＫＦ）群の情報と、該ＫＦ画像上に各画像特徴が映っている２次元位置に関する情報の２つであり、環境地図を任意の画像の位置姿勢推定に用いるために必須な情報である。実座標環境マップ５５０の内容については、後述する図８において詳細に説明する。

一般的に特徴点ベースのＶ−ＳＬＡＭの環境マップは、画像特徴点群の画像特徴と３次元位置、該特徴点群を閲覧している画像ＫＦの情報、また、画像ＫＦと似た画像を検索できるようにするための画像ＫＦ内の画像特徴群（多くは、３次元位置を保持している特徴点群よりも多くの画像特徴を持つ）、を含んでいる。

また、後述する図５に示す全画像位置姿勢データ５６０に示すように、画像位置姿勢は映像の全画像フレームの撮影位置・姿勢である。実座標環境マップ５５０のＫＦ群情報５５１に含まれるＫＦの撮影位置・姿勢データは、全画像位置・姿勢の一部でもあり、重複するが、分かり易さのために、ＫＦ位置・姿勢として他のＫＦ情報と共に記載している。なお、全画像位置姿勢データ５６０の内容については、後述する図９において詳細に説明する。

また、後述する図１０に示す映像１００１は、移動体（車両）に搭載したカメラ映像である。車載機などの車両の通信手段を用いたり、記録メディアを介して人手を使ったり、任意の方法で入手し、本発明の撮影位置推定システムの入力とする。また、特に記載していないが、映像の歪み補正等で用いるため、映像を撮影したカメラの内部パラメータは既知とし、適宜歪み補正を実施してよい。

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

本実施の形態にかかる移動体位置推定方法においては、Ｖ−ＳＬＡＭ処理をするにあたり、２つの処理を実行する。１つ目の処理は、映像を時間経過とは逆方向に処理するよう、映像を逆行するように取得する処理（逆行画像取得処理）である。この逆行画像取得処理は、たとえば、後述する逆行画像取得部５１２によっておこなわれる。

この逆行画像取得処理においては、たとえば、入力した映像ファイルに対し、撮影時刻が遡るように各画像を抽出して、後続のＶ−ＳＬＡＭ処理部に渡す。入力映像がライブ入力であった場合には、任意時間分の画像群をバッファリングして貯めておいて、貯めた時間分の映像に対して、随時、取得時刻と逆にＶ−ＳＬＡＭ処理部へと渡すようにすることができる。

２つ目の処理は、逆行画像取得処理をおこなう際に、Ｖ−ＳＬＡＭの処理対象の映像の状態や、逆行なしで試行したＶ−ＳＬＡＭの結果などから、入力した映像を時間反転（時間逆行）するか否かの判定をおこなう処理（逆行判定処理）である。したがって、この逆行判定処理によって、判定された結果に基づいて、逆行画像取得処理を実行するか否かを決定する。この逆行判定処理は、たとえば、後述する逆行判定部５１１によっておこなわれる。

逆行判定処理は、具体的には、たとえば、対象映像が移動体前方に向けて設置したカメラの映像であるか否かを判定して、移動体の前方を撮影した映像であれば、逆行画像（逆行映像）取得処理をおこなうと判定し、それ以外（たとえば、移動体の後方を撮影した映像）であれば、逆行画像取得処理をおこなわないと判定する。

ここで、当該カメラが前向き撮影カメラか否かは、任意の既知の方法、たとえば、移動体上のカメラ設置位置情報から得てもよいし、あるいは、撮影映像の被写体変化として、オプティカルフローによって周辺背景が近づく／遠ざかることを推定して、その推定結果に基づいて、当該カメラが前向き撮影であるか否かを判断するようにしてもよい。

後述するように、従来方式では、Ｖ−ＳＬＡＭ処理する画像群内で被写体の画像特徴点を最初に検知した２画像を、後述する、初期姿勢・座標系設定部５１３、３Ｄマップ特徴点更新部５３１の該画像特徴の算出、画像特徴の対応付けおよび三角測量で用いる。

このため、前向き撮影カメラの映像を処理する場合に、従来方式では、被写体が遠くに映り始めたときの２画像を該処理部５１３、５３１において用いることがほとんどであるのに対して、本実施の形態にかかる移動体位置推定方法では、逆行画像取得をおこなうため、被写体が最もカメラの近くに映ったときの２画像を使うことになる。この２画像は、被写体が大きく映っているため、画像特徴の算出および対応付けミスが生じにくく、従来のように被写体が遠くに映った時のように誤った対応づけを実施してしまうことが少ない。

このため、前向き撮影カメラの映像を処理する場合に、従来方式では、被写体が近くに映った時にも本来は対応付け済であるはずの画像特徴が対応付けミスで残ってしまっていて、それと路面微小凹凸などの本来は対応付けされないはずの間違った特徴と対応付けしてしまい、結果的に多数の間違ったマップ特徴が生じてしまうのに対して、本実施の形態にかかる移動体位置推定方法では、そのようなことが生じにくい。

また、本実施の形態にかかる移動体位置推定方法では、画像特徴の３Ｄ初期位置を求める三角測量でも、従来より見えの変化（交会角）が非常に大きな複数画像を使って実施することができるので、変化量から３Ｄ位置を推定する三角測量の精度が向上するため、環境マップにおける当該画像特徴の３Ｄ位置精度が向上する。

さらに、当該特徴点の３Ｄ位置は、最適化前の初期位置精度が向上したことに加えて、当該特徴点が大きく映る画像上では、３Ｄ位置の小さな変動は映り込む投影位置の大きな変化として現れることにより、３Ｄ特徴点が画像に映り込む投影位置と実際に該特徴が画像上に現れた位置との違い（再投影誤差）を使った該特徴点の３Ｄ位置の微調整最適化の精度も向上しやすい。この結果、環境マップの当該画像特徴の３Ｄ位置精度が向上する（後述する図４Ａを参照）。

図１Ａは、実施の形態にかかる移動体位置推定方法における姿勢グラフと最適化の一例を模式的に示す説明図である。また、図１Ｂは、従来技術にかかる移動体位置推定方法における姿勢グラフと最適化の一例を模式的に示す説明図である。

図１Ａ、図１Ｂにおいて、被写体（特徴点Ａ）１００に対して、移動体の各撮影位置において撮影された画像１０１〜１１０を示している。矢印は移動体の進行方向を示している。したがって、各三角形１０１〜１１０は、それぞれの画像が撮影された位置を示しており、また、画像１０１が一番最初に撮影されたものであり、時系列に沿って、１０２→１０３→１０４→・・・→１０９→１１０の順で撮影されたことを示している。

そして、特徴点Ａ１００は、移動体が三角形１０１、１０２では撮影されておらず、三角形１０３の画像において、特徴点Ａ１００が初めて映っている。その後、三角形１０３の画像まで、特徴点Ａ１００が映っている。したがって、特徴点Ａ１００が映っている画像群は、画像１０３〜１０９である。

図１Ｂに示すように、従来方式では、被写体（特徴点Ａ）１００が遠くに見え始めたときの画像を使うのが一般的であった。すなわち、画像１０３と画像１０４が三角測量を実施する画像ペアであった。この場合、見えの変化（交会角）θｂが小さい画像のペアで三角測量を実施するため、変化量から位置を推定する三角測量の精度が悪かった。

これに対して、実施の形態にかかる移動体位置推定方法は、図１Ａに示すように、より被写体（特徴点Ａ）１００が最後に見え始めた、すなわち最も近いときの画像を使うものである。すなわち、画像１０８と画像１０９を三角測量を実施する画像ペアとする。見えの変化（交会角）θａが大きい画像のペアで三角測量を実施するため、変化量から位置を推定する三角測量の精度がよくなるものである。

なお、本実施の形態では、後ろ向き撮影カメラによる映像は、そもそも、「被写体がカメラに最も大きく映った画像」から順次遠くに映った画像へと撮影されているため、逆行画像取得を実施する必要がないと判断している。しかし、前後向き撮影カメラ以外のカメラ、たとえばやや斜め向きのカメラなどは、前後どちらの撮影カメラにより近いかを、任意の方法で判断して決定してもよい。

逆行判定処理は、この他に、対象映像のフレームレートが規定値よりも小さい場合は、逆行しないものと判定し、規定値よりも大きい場合は、逆行するものと判定するようにしてもよい。

フレームレートが低い映像は、２画像間で被写体が大きく変化し、特に、カメラ近くに被写体が映っている場合は、遠方に被写体が映っている時よりも映りの変化が大きい。このため、フレームレートが非常に低い映像では、カメラ近くに被写体が映っている２画像によって画像特徴点の対応付けやそれを用いた三角測量などを行おうとすると、被写体の特徴点の出現位置や見えが大きく変化しているために、同じ特徴点と判定することができない虞れがある。

この結果、逆行画像取得でカメラ近くに被写体が映った画像から処理をおこなっても、結局、被写体が適度に遠くに映るまで同じ特徴点として判定できないため、該被写体の画像特徴を環境マップに反映できず、環境マップの精度があまり向上しない上、環境マップに反映されてから該被写体が遠ざかって特徴が見えなくなるまでしか、該環境マップの特徴点をＶ−ＳＬＡＭで利用できないので、利用できる画像数が減ってしまい、また、最も被写体の映りの良いカメラ至近の画像が全く使えない。

このため、初期の３Ｄ特徴点位置精度は若干向上するものの環境マップへの該特徴点登録が遅れるため、カメラ撮影位置姿勢算出に該被写体特徴点をあまり使えないので、従来方式に比べて逆行画像取得をおこなう利点が非常に少なくなる。

すなわち、図１Ｂにおいて、被写体（特徴点Ａ）１００が遠くに見え始めたときの画像を使うと、精度は低いが、低フレームレートでも、被写体が最もカメラに近い時の画像を含め、画像１０３〜１０９の７つの画像をフルでＶ−ＳＬＡＭに利用できる。したがって、至近画像は、再投影誤差による最適化に使うことができる。

これに対して、図１Ａにおいて、より被写体（特徴点Ａ）１００が最後に見えた、すなわち、最も近いときの画像を使うと、至近に映る画像群は、映り変化が激しすぎて特徴対応付けに失敗して、三角測量利用できないため、初期の３Ｄ位置（三角測量精度）はやや高いが、画像１０３〜１０７の５つの画像しかＶ−ＳＬＡＭに利用できず、特に最も映りのよい最至近画像は、最適計算にさえも使うことができないため、精度が悪化する。

このように、低フレームレートだと、時間逆行で三角測量が遅れ、特徴点Ａ１００を利用できる画像数が減り、結果的に誤差が大きくなって、逆行利点が無い。したがって、本実施の形態にかかる移動体位置推定方法では、フレームレートが低い映像は、前向き撮影カメラの場合は、逆行画像を取得しない、という判断をおこなうことができる。

また、後ろ向き撮影カメラでは、フレームレートが低い映像の場合、前向き撮影カメラを逆行画像取得した時と同じ問題が生じ、環境マップ反映が遅れてしまうので、前向き撮影カメラとは逆に、逆行画像取得をおこなうと判断してもよい。後ろ向き撮影カメラに逆行画像取得をおこなうことで、被写体が最も遠方に映った画像から順にＶ−ＳＬＡＭ処理をおこなうことになり、三角測量による初期の３Ｄ特徴点の位置精度は若干悪化するものの、環境マップへの登録が比較的早く実施できるため、該特徴点を使ったカメラ撮影位置姿勢算出に該特徴点を十分活用できるという利点がある。

なお、フレームレートが低い映像であっても、特徴点の対応付けのパラメータ、たとえば、対応付け対象の特徴点を画像内のどの範囲までを探索するかの探索範囲のしきい値などを、フレームレートが高い映像とは異なる適切な値に変更することで、カメラに近い画像同士を使った特徴点対応付けが実施できる場合もあり、その場合はフレームレートによる逆行判定を省略してもよい。

一般的には、探索範囲が大きい程、対応付け処理にかかる処理負荷が高くなるため、特に低フレームレートの映像に対しては、対応付けパラメータの変更をおこなうのではなく、低フレームレートの逆行判定をおこなうことが望ましい。

逆行判定処理においては、この他にさらに、逆行画像取得なしのＶ−ＳＬＡＭ、すなわち従来技術と同じＶ−ＳＬＡＭをあらかじめ一度実施しておき、その時に推定した撮影位置・姿勢の推定精度、または、推定失敗状況、または作成した環境マップの推定精度のいずれかを調べて、それが特に悪化している場合だけ、時間逆行によるＶ−ＳＬＡＭを実施すると判断してもよい。

このように判定することで、渋滞シーンなど他の被写体（他の並走車等）で隠れてしまって、道路周辺地物が最もカメラ近くでは映り込まないことが多いシーン映像など、逆行画像を取得することに効果があるのか判断に迷う場合にも、必要な場合だけ、逆行画像の取得をおこなうことができる。

たとえば、撮影位置・姿勢の推定精度は、位置や姿勢変化のばらつきが大きい場合に精度が悪いと判断する。また、推定失敗状況はＶ−ＳＬＡＭで入力した処理対象映像の画像数に対して、撮影位置・姿勢を算出できた画像数の占める割合を求め、割合は小さいときに精度が悪いと判断する。環境マップの推定精度は、たとえば環境マップの特徴点３Ｄ位置群の位置のばらつきの大きさや、明らかに高さや位置が異常値として推定されている特徴点群の含まれる割合などから、ばらつきや割合が大きいときに精度が悪いと判断する。

逆行判定処理では、これらの例で示した判断の１つ以上を、任意に組み合わせて判定してよい。

（逆行判定処理の手順）
図２Ａおよび図２Ｂは、実施の形態にかかる移動体位置推定方法の処理の手順の一例を示すフローチャートである。図２Ａは、逆行判定処理の手順の一例を示すフローチャートであり、図２Ｂは、逆行画像取得処理以降の処理の手順の一例を示すフローチャートである。

図２Ａのフローチャートにおいて、逆行判定部５１１は、まず、カメラが前向きか否かを判断する（ステップＳ２０１）。ここで、カメラが前向きでない場合（ステップＳ２０１：Ｎｏ）は、ステップＳ２０５へ移行する。一方、カメラが前向きの場合（ステップＳ２０１：Ｙｅｓ）は、カメラの撮影フレームレートがしきい値以上か否かを判断する（ステップＳ２０２）。

ステップＳ２０２において、カメラの撮影フレームレートがしきい値以上の場合（ステップＳ２０２：Ｙｅｓ）は、「映像を逆行取得する」と判定する（ステップＳ２０３）。一方、ステップＳ２０２において、カメラの撮影フレームレートがしきい値以上でない場合（ステップＳ２０２：Ｎｏ）は、「映像を逆行取得しない」と判定する（ステップＳ２０６）。

ステップＳ２０３において、「映像を逆行取得する」と判定した後、従来の時間逆行なしのＶ−ＳＬＡＭ結果があり、その撮影位置・姿勢算出が良好だったか否かを判断する（ステップＳ２０４）。ここで、Ｖ−ＳＬＡＭ結果があり、その撮影位置・姿勢算出が良好だった場合（ステップＳ２０４：Ｙｅｓ）は、逆行画像を取得する必要がないので、Ｖ−ＳＬＡＭの一連の処理を終了する。

一方、ステップＳ２０４において、Ｖ−ＳＬＡＭ結果がない場合、あるいは、Ｖ−ＳＬＡＭ結果があっても、その撮影位置・姿勢算出が良好でない場合（ステップＳ２０４：Ｎｏ）は、逆行画像を取得する必要があるので、図２ＢのステップＳ２１１へ移行する。

カメラが前向きでない場合（ステップＳ２０１：Ｎｏ）に移行したステップＳ２０５においても、カメラの撮影フレームレートがしきい値以上か否かを判断する（ステップＳ２０５）。ここでは、カメラの撮影フレームレートがしきい値以上でない場合（ステップＳ２０５：Ｎｏ）は、「映像を逆行取得する」と判定する（ステップＳ２０３）。一方、ステップＳ２０５において、カメラの撮影フレームレートがしきい値以上の場合（ステップＳ２０５：Ｙｅｓ）は、「映像を逆行取得しない」と判定する（ステップＳ２０６）。

ステップＳ２０６において、「映像を逆行取得しない」と判定した後、従来の時間逆行なしのＶ−ＳＬＡＭ結果があるか否かを判断する（ステップＳ２０７）。ここで、Ｖ−ＳＬＡＭ結果がある場合（ステップＳ２０７：Ｙｅｓ）は、Ｖ−ＳＬＡＭの一連の処理を終了する。一方、ステップＳ２０７において、Ｖ−ＳＬＡＭ結果がない場合（ステップＳ２０７：Ｎｏ）は、図２ＢのステップＳ２１１へ移行する。

図２Ｂのフローチャートにおいて、判定の結果、映像を逆行取得する必要があるか否かを判定する（ステップＳ２１１）。判定の結果は、「映像を逆行取得する」（図２ＡのステップＳ２０３）または「映像を逆行取得しない」（図２ＡのステップＳ２０６）との判定の結果である。

ステップＳ２１１において、映像を逆行取得する必要がある場合（ステップＳ２１１：Ｙｅｓ）は、映像を逆時刻順に取得する『逆行取得フラグ』をＯＮにして（ステップＳ２１２）、ステップＳ２１４へ移行する。一方、映像を逆行取得する必要がない場合（ステップＳ２１１：Ｎｏ）は、『逆行取得フラグ』をＯＦＦにして（ステップＳ２１３）、ステップＳ２１４へ移行する。

ステップＳ２１４において、すべての映像を処理したか否かを判断する（ステップＳ２１４）。ここで、未だ、すべての映像を処理していない場合（ステップＳ２１４：Ｎｏ）は、映像内の未処理画像群から、『逆行取得フラグ』を用いて、最も後で撮影した画像を１つ取得する（ステップＳ２１５）。これは、『逆行取得フラグ』がＯＮになっている場合であって、『逆行取得フラグ』がＯＦＦになっている場合は、最も前で撮影した画像を１つ取得するようにするとよい。ここまでが、逆行画像取得部５１２によって実行される処理である。

そして、取得した画像内で特徴点群を取得し（ステップＳ２１６）、画像の撮影位置姿勢を、最新ＫＦの撮影位置姿勢・特徴点群マップから推定する（ステップＳ２１７）。このように、特徴点群マップの投影位置から撮影位置姿勢を推定するが、マップ精度が向上しているので、推定精度も向上する。ただし、フレームレートが低いと、近くに映っている画像が２枚ないとマップ登録されないので、近くの特徴がマップにない可能性がある。ここまでが、後述するフレーム姿勢推定部５２１によって実行される処理である。

つぎに、取得した画像は、新ＫＦ画像であるか否かを判断する（ステップＳ２１８）。新ＫＦ画像であるか否かは、たとえば、最新ＫＦとの相違が大きい、または、指定時間が離れているか否かによって判断することができる。ここで、取得した画像が、新ＫＦ画像でない場合（ステップＳ２１８：Ｎｏ）は、何もせずに、ステップＳ２１４へ戻る。

一方、取得した画像が、新ＫＦ画像である場合（ステップＳ２１８：Ｙｅｓ）は、ステップＳ２１９へ移行する。ここまでが、後述するＫＦ（キーフレーム）更新部５２２によって実行される処理である。

つぎに、既存のＫＦ画像内で特徴点と同じ特徴で、対応付けしていない特徴があるか否かを判断する（ステップＳ２１９）。ここで、対応付けしていない特徴がない場合（ステップＳ２１９：Ｎｏ）は、ステップＳ２１４へ戻る。

一方、対応付けしていない特徴がある場合（ステップＳ２１９：Ｙｅｓ）は、見つけた既存ＫＦ画像と現（ＫＦ）画像の、画像内特徴位置と撮影位置姿勢を使って、三角測量を実施する（ステップＳ２２０）。そして、特徴点の特徴と、三角測量による３次元位置を、環境マップに追加更新する（ステップＳ２２１）。ここまでが、後述する３Ｄマップ特徴点更新部５３１によって実行される処理である。

その後、追加更新した環境マップとＫＦ群を使って、現（ＫＦ）画像の位置姿勢と環境マップを最適化する（ステップＳ２２２）。ここまでが、後述するグラフ制約生成部５３２、ＫＦ姿勢・特徴点マップ最適化部５３３によって実行される処理である。

その後、ステップＳ２１４へ戻る。そして、ステップＳ２１４において、すべての映像を処理した場合（ステップＳ２１４：Ｙｅｓ）は、図２Ａに戻って、一連の処理を終了する。

このように、本実施の形態にかかる移動体位置推定方法は、カメラ映像の設置位置（撮影方向）や、フレームレート、従来Ｖ−ＳＬＡＭの実施結果などから、映像を時間逆行するかを判定し、適宜、時間逆行させた画像群でＶ−ＳＬＡＭ処理することで、被写体およびその特徴点が画像内でより大きく映った画像を用いて三角測量をおこなうことができ、該特徴点の３Ｄ位置で構成された環境マップの精度を向上させると共に、該マップを使って推定算出するカメラの撮影位置・姿勢の推定精度を向上させるものである。

（Ｖ−ＳＬＡＭの環境マップの比較）
図３Ａは、実施の形態にかかる移動体位置推定方法における逆時刻によるＶ−ＳＬＡＭの環境マップの一例を示す説明図である。また、図３Ｂは、従来技術にかかる移動体位置推定方法における順時刻によるＶ−ＳＬＡＭの環境マップの一例を示す説明図である。図３Ａと図３Ｂとは、その比較を示している。

図３Ａ、図３Ｂは、環境マップの画像特徴の３Ｄ位置を、上空俯瞰した画像である。図３Ａ、図３Ｂは、後述する図４Ａ、図４Ｂのように、画像のほぼ中央を上から下へ伸びる道路を走行する移動体の映像を使って、同じ処理区間に対してＶ−ＳＬＡＭで作成された環境マップを示しており、各ドットはマップ特徴点群を示している。

図３Ａにおいて、逆画像を取得した場合に、逆時刻マップ特徴点群は、大きく被写体が映る画像で特徴対応付けと三角測量をおこなうため、本来の特徴のみのマップとなり、正しく道路周辺に３Ｄ位置が集中していることがわかる。一方、図３Ｂにおいて、逆画像を取得していない場合に、環境マップの特徴点が画像中央に縦に存在する道路より大きく離れた場所まで散らばっており、また、誤った特徴対応付けの結果、間違った特徴対応付けも増えてしまい、路面微小凹凸の特徴に対応するマップ特徴点も増えている。

このように、逆画像を取得することにより、生成する特徴点の３次元位置群である周辺環境マップの位置精度が向上して、位置誤差による特徴点位置の散らばりがなくなっていることがわかる。

（撮影位置・姿勢の推定結果の比較）
図４Ａは、実施の形態にかかる移動体位置推定方法における周辺環境マップの一例を示す説明図である。また、図４Ｂは、従来技術にかかる移動体位置推定方法における周辺環境マップの一例を示す説明図である。図４Ａと図４Ｂとは、撮影位置・姿勢の推定結果の比較を示している。

図４Ａと図４Ｂは、上空俯瞰した画像で、処理映像の同じ走行区間を示している。図４Ａにおいて、逆画像を取得した場合には、ほぼ走行区間全体で撮影位置・姿勢を推定できている。一方、図４Ｂにおいて、逆画像を取得していない場合には、環境マップ精度が低いために、撮影位置・姿勢の推定ができた画像が非常に少ない。

このように、初期の特徴点３Ｄ位置算出をおこなうタイミングが、従来方式おける「最も遠方に被写体が映った時（遠方に見え始めた時）」だったものが、本発明の実施にかかる移動体位置推定方法により、「最も近傍に被写体が映った時（最後に見えた時）」に変更することが可能になるため、入力映像状況に応じて、必要なら、被写体に対してより大きな映像内変化が得られる画像を使って実施することができる。このため、映像内変化から推定する特徴点の３次元位置推定の精度が向上し、高精度な周辺環境マップ（周辺特徴点群の３次元位置群）が得られる。

この結果、課題だった生成する周辺環境マップ（画像特徴点の３次元位置群）の位置精度が向上し、環境マップから推定した画像内容と実際の映像の映りの齟齬が拡大することなくＶ−ＳＬＡＭ処理を実施できるので、撮影位置姿勢推定処理の失敗（以後推定処理できず、撮影位置姿勢が推定できない区間が生じる）が起こりにくくなり、撮影位置姿勢の推定区間が伸びると共に、その精度も向上することができる。

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

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

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

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

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

そして、上記構成部は、大きく分けて４つの機能部に分けることができる。逆行判定部５１１と、逆行画像取得部５１２と、初期姿勢・座標系設定部５１３と、によって、システムの初期化処理機能５１０を実現することができる。また、フレーム姿勢推定部５２１と、ＫＦ更新部５２２と、によって、位置姿勢推定（トラッキング）処理機能５２０を実現することができる。

また、３Ｄマップ特徴点更新部５３１と、グラフ制約生成部５３２と、ＫＦ姿勢・特徴点マップ最適化部５３３と、によって、環境マップ作成（ローカルマッピング）処理機能５３０を実現することができる。また、ループ検出・クロージング部５４１によって、ループクローズ処理機能５４０を実現することができる。

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

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

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

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

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

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

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

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

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

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

このように、移動体位置推定システム５００は、逆行判定部５１１、逆行画像取得部５１２、初期姿勢・座標系設定部５１３、フレーム姿勢推定部５２１、キーフレーム（ＫＦ）更新部５２２、３Ｄマップ特徴点更新部５３１、グラフ制約生成部５３２、ＫＦ姿勢・特徴点マップ最適化部５３３、ループ検出・クロージング部５４１、の９つのＶ−ＳＬＡＭをベースとする処理部を持ち、かつ、実座標環境マップ５５０（ＫＦ群情報５５１、特徴点群情報５５２）、全画像位置姿勢データ５６０、の２つの内部保持データを持っている。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ここで、「ＩＤ」は、当該ＫＦの情報を識別する一意の識別情報であり、「親ＫＦ ＩＤ」および「子ＫＦ ＩＤ」は、ＫＦどうしをつなぐ情報であり、「ループＫＦ ＩＤ」は、ループクローズ処理などで使う情報である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

また、後述するローカルマッピング機能により、入力されたＧＮＳＳ情報１００２による位置と、映像１００１を解析した結果による位置の双方を適切にマージ反映させた推定ができるようにしてもよい。これにより、手修正した出力結果を入力とする再実施を通して、手修正結果に合わせて特徴点群を含めた実座標環境マップを滑らかに作成することが可能となる。ただし、このような、ＧＮＳＳ情報１００２による位置と、映像１００１を解析した結果による位置の双方を適切にマージ反映させた推定については、用いなくてもよい。

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

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

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

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

（逆行判定部５１１の内容）
システムの初期化処理機能５１０を担当する逆行判定部５１１は、逆行画像取得部５１２の動作を決定する処理部であり、Ｖ−ＳＬＡＭの処理対象の映像の状態や、逆行なしで試行したＶ−ＳＬＡＭの結果などから、入力した映像を時間反転（時間逆行）するかの判定をおこなう。

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

（逆行画像取得部５１２の内容）
システムの初期化処理機能５１０を担当する逆行画像取得部５１２は、逆行判定部５１１において、「時間逆行させる」と判断した場合に、以後のＶ−ＳＬＡＭ処理部で映像を時間経過とは逆方向に処理するよう、映像を逆行するように取得する。たとえば、入力した映像ファイルに対し、撮影時刻が遡るように各画像を抽出して、後続のＶ−ＳＬＡＭ処理部に渡す。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

なお、初期姿勢・座標系設定部５１３は、計算する座標系の決定とともに、初期化処理として以後のトラッキングなどの処理機能で必要とする内部データの作成をおこなうにあたり、上記の方法には限定されない、上記の方法以外の方法も用いておこなうようにしてもよい。

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

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

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

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

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

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

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

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

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

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

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

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

（ＫＦ更新部５２２の内容）
図１０において、位置姿勢推定（トラッキング）処理機能５２０を担当するＫＦ（キーフレーム）更新部５２２は、従来のＶ−ＳＬＡＭのように画像特徴的に新画像をＫＦにするか否かを判定する。

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

また、本システム５００のＫＦ更新部５２２は、さらにＧＮＳＳ位置を保持する画像か否かも用いて、新たにＫＦ画像を選定するようにしてもよい。すなわち、すべての画像にＧＮＳＳ位置が無く、ＧＮＳＳ位置が無い画像が規定数以上続いた場合において、ＧＮＳＳ位置がある画像が新画像として入力された場合に、画像特徴的な従来の判定の結果に関わらず、新画像を新しいＫＦとして採用する。

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

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

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

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

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

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

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

また、本実施の形態においては、ＧＮＳＳ位置情報を取得していなくてもよく、その場合は、ステップＳ１２０４およびＳ１２０５の各処理は実行せずに、省略するようにしてもよい。

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

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

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

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

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

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

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

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

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

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

（ＫＦ姿勢・特徴点マップ最適化部５３３の内容）
環境マップ作成（ローカルマッピング）処理機能５３０を担当するＫＦ姿勢・特徴点マップ最適化部５３３は、新たに、グラフ制約生成部５３２において生成した新しい２つの姿勢グラフを用いて、一般的なグラフ最適化計算をおこなう。

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

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

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

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

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

以上説明したように、本実施の形態によれば、サーバ（情報処理装置／移動体位置推定装置）５０１が、撮影された時系列画像（映像）１００１のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理（Ｓ２１６〜Ｓ２２２）をおこなうにあたり、逆行画像取得部５１２が、撮影画像を時刻に対して逆行させた逆行画像を取得し（Ｓ２１２）、当該逆行画像を用いて前記情報処理をおこなう。

また、サーバ（情報処理装置／移動体位置推定装置）５０１が、撮影された時系列画像（１００１）のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理（Ｓ２１６〜Ｓ２２２）をおこなうにあたり、逆行画像取得部５１２が、相互に区別可能な特徴量を持つ画像特徴点が映る画像群のうち、最も遅い時刻近傍で撮影した画像を用いて（Ｓ２１５）前記情報処理をおこなう。

これにより、用いる画像が、従来における「最も遠方に被写体の画像特徴点が映った時（遠方に見え始めた時）」だったものから、「最も近傍に被写体の画像特徴点が映った時（最後に見えた時）」に変更することができ、それによって、映像内変化から推定する特徴点の３次元位置推定の精度が向上し、高精度な周辺環境マップ（周辺特徴点群の３次元位置群）の作成が可能となる。

また、本実施の形態によれば、サーバ（情報処理装置／移動体位置推定装置）５０１の逆行判定部５１１が、逆行画像を取得するか否かを判定する。具体的には、たとえば、時系列画像の撮影方向に基づいて（Ｓ２０１）、逆行画像を取得するか否かを判定し、移動体の前方を撮影している場合に、逆行画像を取得すると判定する。また、時系列画像のフレームレートに基づいて（Ｓ２０２、Ｓ２０５）、逆行画像を取得するか否かを判定し、フレームレートが所定のしきい値以上の場合は、逆行画像を取得すると判定する。また、逆行未実施時の撮影位置姿勢推定結果の状態または精度に基づいて（Ｓ２０４、Ｓ２０７）、あるいは、環境地図の精度に基づいて（Ｓ２０４、Ｓ２０７）、逆行画像を取得するか否かを判定する。したがって、いずれの場合においても、逆行画像を用いることが有効な場合のみ、当該逆行画像を利用することができる。

本実施の形態にかかる移動体位置推定システムは、具体的には、たとえば、自動運転向け高精度地図更新システム、屋外撮影画像の地図貼り付けアプリケーション、屋外画像・映像内被写体の実座標位置の自動推定アプリケーション、任意道路シーンのＣＧ再現システムなどの幅広いシステムやアプリケーションに応用することができる。

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

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

（付記１）撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなう移動体位置推定システムであって、
前記時系列画像を時刻に対して逆行させた逆行画像を取得し、当該逆行画像を用いて前記情報処理をおこなう、
情報処理装置を有することを特徴とする移動体位置推定システム。

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

（付記３）前記情報処理装置は、前記逆行画像を取得するか否かを判定することを特徴とする付記１または２に記載の移動体位置推定システム。

（付記４）前記情報処理装置は、前記時系列画像の撮影方向に基づいて、前記逆行画像を取得するか否かを判定することを特徴とする付記３に記載の移動体位置推定システム。

（付記５）前記情報処理装置は、前記時系列画像の撮影方向に基づいて、移動体の前方を撮影している場合は、前記逆行画像を取得すると判定することを特徴とする付記４に記載の移動体位置推定システム。
（付記６）前記情報処理装置は、前記時系列画像のフレームレートに基づいて、前記逆行画像を取得するか否かを判定することを特徴とする付記３〜５のいずれか一つに記載の移動体位置推定システム。

（付記７）前記情報処理装置は、前記時系列画像のフレームレートに基づいて、前記フレームレートが所定のしきい値以上の場合は、前記逆行画像を取得すると判定することを特徴とする付記６に記載の移動体位置推定システム。

（付記８）前記情報処理装置は、逆行未実施時の撮影位置姿勢推定結果の状態または精度に基づいて、前記逆行画像を取得するか否かを判定することを特徴とする付記３〜７のいずれか一つに記載の移動体位置推定システム。

（付記９）前記情報処理装置は、環境地図の精度に基づいて、前記逆行画像を取得するか否かを判定することを特徴とする付記３〜８のいずれか一つに記載の移動体位置推定システム。

（付記１０）撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなう移動体位置推定システムであって、
相互に区別可能な特徴量を持つ画像特徴点が映る画像群のうち、最も遅い時刻近傍で撮影した画像を用いて前記情報処理をおこなうことを特徴とする移動体位置推定システム。

（付記１１）情報処理装置が、
撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなうにあたり、
前記時系列画像を時刻に対して逆行させた逆行画像を取得し、当該逆行画像を用いて前記情報処理をおこなう、
ことを特徴とする移動体位置推定方法。

（付記１２）情報処理装置が、
撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなうにあたり、
相互に区別可能な特徴量を持つ画像特徴点が映る画像群のうち、最も遅い時刻近傍で撮影した画像を用いて前記情報処理をおこなう、
ことを特徴とする移動体位置推定方法。

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

Claims (10)

1. 撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなう移動体位置推定システムであって、
   前記時系列画像を時刻に対して逆行させた逆行画像を取得し、当該逆行画像を用いて前記情報処理をおこなう、
   情報処理装置を有することを特徴とする移動体位置推定システム。
2. 前記情報処理装置は、
   前記逆行画像を用いて、
   相互に区別可能な特徴量を持つ画像特徴点を複数抽出する情報処理、
   前記時系列画像の系列中の複数の画像相互間で、前記特徴量が類似する前記画像特徴点どうしを対応付ける情報処理、および、
   対応付けられた各特徴点の各前記画像上での２次元位置を用いて、前記撮影位置を推定する情報処理、
   の少なくともいずれか一つをおこなうことを特徴とする請求項１に記載の移動体位置推定システム。
3. 前記情報処理装置は、前記逆行画像を取得するか否かを判定することを特徴とする請求項１または２に記載の移動体位置推定システム。
4. 前記情報処理装置は、前記時系列画像の撮影方向に基づいて、前記逆行画像を取得するか否かを判定することを特徴とする請求項３に記載の移動体位置推定システム。
5. 前記情報処理装置は、前記時系列画像のフレームレートに基づいて、前記逆行画像を取得するか否かを判定することを特徴とする請求項３または４に記載の移動体位置推定システム。
6. 前記情報処理装置は、逆行未実施時の撮影位置姿勢推定結果の状態または精度に基づいて、前記逆行画像を取得するか否かを判定することを特徴とする請求項３〜５のいずれか一つに記載の移動体位置推定システム。
7. 前記情報処理装置は、環境地図の精度に基づいて、前記逆行画像を取得するか否かを判定することを特徴とする請求項３〜６のいずれか一つに記載の移動体位置推定システム。
8. 撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなう移動体位置推定システムであって、
   相互に区別可能な特徴量を持つ画像特徴点が映る画像群のうち、最も遅い時刻近傍で撮影した画像を用いて前記情報処理をおこなうことを特徴とする移動体位置推定システム。
9. 情報処理装置が、
   撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなうにあたり、
   前記時系列画像を時刻に対して逆行させた逆行画像を取得し、当該逆行画像を用いて前記情報処理をおこなう、
   ことを特徴とする移動体位置推定方法。
10. 情報処理装置が、
    撮影された時系列画像のうちの任意の画像について、当該任意の画像の特徴から、当該任意の画像の撮影位置・姿勢推定および環境地図生成の情報処理をおこなうにあたり、
    相互に区別可能な特徴量を持つ画像特徴点が映る画像群のうち、最も遅い時刻近傍で撮影した画像を用いて前記情報処理をおこなう、
    ことを特徴とする移動体位置推定方法。

Images