以下に図面を参照して、本発明にかかる移動***置推定システムおよび移動***置推定方法の実施の形態を詳細に説明する。
(実施の形態)
まず、図1~図4を用いて、本実施の形態にかかる移動***置推定システムおよび移動***置推定方法の概要について説明する。
移動する一般車の車載機のデータ(たとえば映像データなど)は、大量に収集(プローブ)され、車載データ解析に用いられているが、一般的に搭載されているGPS(Global Positioning System)装置では誤差の大きな自車位置測定しかできないため、詳細な自車位置を必要とするサービスには適用することができない。
この一般車のデータに対し、たとえば走行中の撮影時の位置・姿勢を高精度に推定してその車載画像に付加するならば、画像から走路周辺の地物を抽出して自動運転向け地図などの地図を作成更新したり、自動運転向けに撮影時の周辺状況を解析したりする新しいサービス分野に応用することができる。このため、これら一般車映像を使った新しいサービス利用の前提として、車載画像を撮影したカメラ位置・姿勢(映像の撮影位置・姿勢)を正確に推定する技術が必要とされている。
SLAMは、移動中に取得した周辺状況に関する車載データ、たとえばLIDAR(Laser Imaging Detection and Ranging)データなどを入力とし、自車走行経路(自車の位置と姿勢)と周辺環境地図(周辺物の3次元位置マップなど)を同時に作成する技術の総称である。なかでも、V-SLAMは、車載カメラで撮影した映像を入力とし、撮影した映像に映る被写体の変化を用いて、自車走行経路(自車位置・姿勢)と周辺環境地図(周辺の被写体の画像特徴点群の3次元位置マップ、以下、環境地図という)を推定作成することができる技術であり、一般車の映像から自車位置と姿勢を推定することができる。
ところで、一般的にSLAMは、環境地図を同時作成しながら姿勢推定を逐次的におこなう技術であるため、長距離での誤差蓄積や、大きく見えが変化する場所の調整失敗でスケール(縮尺)変動が発生する。それにより、上述のような、マップが歪み、正しい環境地図を作成できず、環境地図を元にした位置・姿勢推定に誤りが生じる、いわゆる「スケールドリフト」と呼ばれる問題が発生する。
図1は、スケールドリフトの一例を示す説明図である。図1において、左右の図はいずれも経路図を示している。左図は、(a)GPSで実際に測定した、地図上の実座標系101を示す経路図であり、右図は、(b)SLAM実行結果102に基づく経路図である。
地図上の実座標系101を示す経路図において、走行経路111は実線で示されているのに対して、SLAM実行結果102に基づく経路図においては、走行経路112は、当該走行経路112上で撮影した各画像の推定撮影位置を結んだ棒線で示されている。また、走行経路112の周辺に散らばる細かい点群は、周辺の被写体の画像特徴群の3次元位置マップ、すなわち、周辺環境地図である。このSLAM実行結果102は、特に実座標系とは無関係なSLAMローカル座標系での推定走行軌跡を、走行経路111に合わせて全体拡大および回転させて並べて示したものである。
図1からもわかるように、地図上の実座標系101を示す走行経路111に比べて、SLAM実行結果102に基づく走行経路112、すなわち、V-SLAMで推定した走行経路(走行画像の撮影位置を繋いだもの)が大きく異なっている。特に、経路で囲まれたエリアAとBの大きさが両者で異なっており、SLAM実行結果102において、V-SLAM実行時には、エリアAとエリアBの近傍で、局所的にスケールが大きく異なっている(スケールドリフトが発生している)ことがわかる。
SLAMには、このスケールドリフトの課題に対し、走行中に同じ場所(ループ)を通過することを検知し、該場所で複数回の通過時の推定位置・姿勢や環境地図がすべて整合性がとれるようSLAM推定結果を補正してスケールドリフトを改善する「ループクローズ」という技術がある。
V-SLAMでは、すべての画像の撮影位置・姿勢推定を実施するが、特に主要な画像をキーフレーム(「KF」)と呼ぶ。そして、このKFに対してのみ、撮影位置・姿勢を大域/局所の双方で矛盾が無いように、環境地図を用いた詳細な解析技法で、環境地図自体も特徴点群の追加や位置変更などの更新調整をおこないながら、その撮影位置・姿勢を推定する。残る画像群の撮影位置・姿勢は、環境地図を更新せずに、KFからの相対関係を使って簡易に推定する、という方法が取られることが多い。
図2は、ループクローズの一例を模式的に示す説明図であり、KFの位置・姿勢推定時のループクローズ技術の適用例を示している。図2において、各KFを三角形で示している。符号201に示すように、(1)SLAMでKFどうしの類似性、たとえばV-SLAMならKF画像どうしの類似性などから同一場所であるループを検出する。具体的には、たとえば、新たに作成されたKFと、過去のKFとの間で画像類似などからループを検出する。符号201では、ループを検出した状態を示している。
つぎに、符号202に示すように、(2)連続するKFどうしの関係を適度の近さで隣り合うよう拘束する通常のSLAMの位置・姿勢推定の最適化計算時の拘束条件に加えて、発見したループ箇所のKFどうしが近くなるような拘束条件を追加する。符号202において、点線で示した部分が拘束条件を示している。そして、符号203に示すように、(3)追加した拘束条件を含めた新たな拘束条件群で、ループ近傍のKF群および環境地図の補正をおこなう。
この結果、ループ近傍のKFの位置・姿勢が補正されるため、ループ近傍のスケール誤差は解消するが、ループから離れたKFのスケール誤差は解消されないし、必ずしも走行時にループが発生するとは限らないため、ループクローズによるスケールドリフト改善効果は限定的であるという課題(第1の課題)がある。すなわち、ループの辻褄が合うように全体が補正されるが、局所的な縮尺変動は補正されないという問題点がある。また、そもそも、ループ走行しないと適用できないという問題点がある。
また、SLAMは、初期画像からの差分で環境地図を作成するため、実座標位置(緯度経度、実世界スケール)とは無関係な、その映像独自のローカル系(SLAMローカル系)での値となる。そのため、実世界の地図と対応付けた活用が困難であり、上述した一般車映像を使ったサービス利用に活用するのには難しいという課題(第2の課題)がある。
従来、SLAMは、ロボットなどの自律移動のために開発され、使い捨ての環境地図を作成しながら周辺物体を避けた移動ができることが重要で、移動経路自体の正確さ(絶対値としての大きさの把握や、スケール固定など)はあまり重要でなかった、という経緯があるため、一般車映像を使ったサービス利用向けの撮影位置・姿勢推定に適用するには、新たに上記第1の課題および第2の課題の2つを解決することが必要である。
これに対し、先行技術におけるV-SLAMによる撮影位置・姿勢推定では、V-SLAM特有の第1の課題であるスケールドリフトを防ぐため、V-SLAMを実行後に、V-SLAMで算出した環境マップや推定撮影位置を、V-SLAMの入力映像と同時に別途取得したGNSS高精度位置を用いて、局所的な線形変換を用いて逐次的に補正する。
しかしながら、この方法では、補正に用いる位置座標変換は、規定距離離れた2地点の位置対による局所的な線形変換(アフィン変換)になるため、大まかな補正しかできず、右左折のような右左折後に特に大きくスケールが変わるようなシーンであっても、右左折前後の2地点を選定してしまう可能性があり、右左折前の比較的スケールが正しい画像に対しても、大きくスケールを間違えた右左折後の画像向けの大きな補正を含んだ位置座標変換がかかるため、補正精度が低くなる。
また、本来は撮影位置が変われば、見え方も変化するため、撮影位置の変更は姿勢にも影響するが、V-SLAM後の補正は、実際には位置補正しかおこなわないため、推定した姿勢はまったく補正できないという問題がある。
さらに、V-SLAMの算出後に補正処理をおこなう必要があるため、V-SLAM処理以外の処理コストがかかる。
このように、V-SLAMを用いた従来技術では、実座標系の撮影位置・姿勢を補正算出する処理コストが高く、補正した推定撮影位置・姿勢の精度も低いという問題があった。本発明は、これらの処理コストと推定精度悪化という課題を解決することを目的とする。
本発明は、図3と図4に示す実施の形態にかかる移動***置推定方法により、3D特徴点と走行軌跡の最適化(誤差最小化)計算にGNSS情報を含めて、このGNSS情報を用いて、(さらに、シーン変化に基づいた適切なKFを使って、)逐次補正をおこなうものであり、これにより、走行距離が伸びても縮尺変動しない補正処理をおこなうことができるため、広域でのスケールドリフトを精度良く回避することができる。
本実施では、画像解析による見えの変化と、GNSS位置の変化の双方を、無理なくV-SLAMのキーフレーム(KF)の位置・姿勢および周辺の環境マップの特徴点群位置の最適化計算に反映させるために、まずはGNSS位置を用いたKFの位置・姿勢最適化(図3)で大まかにGNSS位置に合わせこみをおこない、続けてGNSS位置を用いて再度のKFの位置・姿勢と環境マップの特徴点群の位置の双方に対する最適化(図4)で詳細にGNSS位置に合わせる、二段階の処理をおこなっている。しかし、後述するようにこれは一例であって、逐次的に最適化を実施するときに、前段のKFの位置・姿勢のみの最適化を常に、または時折省略して、そのときは後段のマップ特徴点群も含めた最適化だけを実施するようにしても構わない。
また、とくに記載がなければ、以後のKFの最適化はKFの位置・姿勢の最適化を示すが、位置だけを最適化しても構わない。たとえば、前段は位置だけを最適化し、後段では位置・姿勢を最適化してもよい。
以下では、この二段階の処理を、図3、図4を用いて順に説明する。
図3は、実施の形態にかかる移動***置推定方法における姿勢グラフと最適化の一例を模式的に示す説明図である。図3では、後述するグラフ制約生成部532において新たに作成する2つの最適化に用いる姿勢グラフと、後述するKF姿勢・特徴点マップ最適化部533における新たな追加処理である、該姿勢グラフを用いた最適化の一例を示している。また図3は、前述した二段階処理の、GNSS位置を用いて大まかにKFの位置・姿勢を最適化する前段の処理の例を示す。
図3において、三角形で示すキーフレーム(KF)は、KF1、KF2、KF3、・・・と、点線の矢印で示す進行方向に追加される。また、四角形は、GNSS位置に関するGNSS値を示している。符号301で示す状況では、KF1→KF2→KF3→KF4→KF5の順で、時系列に追加されたことを示しており、KF5は、姿勢推定対象最新KFである。また、G1、G3、G5は、それぞれKF1、KF3、KF5におけるGNSS値を示している。
GNSS位置に関するGNSS値を用いたキーフレーム(KF)位置・姿勢の逐次補正の処理手順を、符号301~304の順で説明する。
まず、符号301に示すように、移動経路の始まりのころは、(1)新たに求めたキーフレーム(KF)であるKF5を含め、今までに求めた過去のKF群(KF1~KF4)すべてに対し、隣り合うKFどうしを結ぶ実線で示す、KF画像間の見えの変化に合わせたKFの相対的な位置・姿勢変化量からのずれを最小化する、従来のV-SLAMで使われているものと同じ拘束条件を設定する。符号301では、KF1~KF5の隣どうしを互いに結ぶ拘束条件を設定する。
本実施では、この従来のKFどうしを結ぶ拘束条件に加えて、KFにGNSS位置があるならば、さらにKFとGNSS値とのずれを最小化する新たな拘束条件を設定する。符号301において、KF1とG1を結ぶ実線は、この拘束条件C1を示している。拘束条件C3、C5も同様に、それぞれKF3とG3、KF5とG5のこの拘束条件である。
符号301に示すように、これらKFどうしの見えに関する拘束条件と、KFとGNSS値との拘束条件との2種類の拘束条件を設定するが、さらにそのどちらをより優先するかを、各拘束条件の重みパラメータを設定して全体最適化への影響力を変えるなどで、調整してもよい。たとえば、用いるGNSS位置の精度が非常に良いことがあらかじめ分かっている場合は、GNSS値との拘束条件の影響力をより強めるよう、重みを重く調整する。
そして、符号302に示すように、(2)過去のキーフレーム(KF)群を含めて、符号301で設定した拘束条件に基づく最適化により、KFの位置・姿勢を補正する。符号302からもわかるように、GNSS値との拘束条件により、KF1をG1に、KF3をG3に、KF5をG5に近づくように補正されている。このように拘束条件に基づいて、KFどうしの見えの変化にも合わせながら、拘束条件全体の誤差を最小化するよう、KFをGNSS値に近づけるように補正する。このため、この補正による結果として、必ずしもKFの位置がGNSS値と完全に一致する結果になるとは限らない。
つぎに、移動体がさらに経路を進み、最初のキーフレーム(KF)であるKF1から十分離れたKF7を追加する位置まで進んだときの処理手順を符号303に示す。符号303において、移動体の進行方向への移動(時間の経過)にともない、KF6およびKF7(姿勢推定対象最新KF)があらたに追加されている。
符号303に示すように、(3)新たに求めたキーフレーム(KF)に対し、符号301と同様に、隣り合うKFとの見えの変化に関する従来の拘束条件と、KFにGNSS位置がある場合にはKFとGNSS値とのずれを最小化する拘束条件を、それぞれ追加する。
ここで、符号301と異なり、十分経路が伸びたので、最適化補正計算に使うKF、すなわち拘束条件を設定するKFの選定をおこなう。最適化補正計算に使うKFの選定は、以下の2つの選定方法のうちのいずれかを使うことができる。第1の選定方法は、時間的、または距離的な近さに基づく方法である。たとえば、撮影時間差やGNSS位置などから概算できる距離差が大きすぎるKFを使わないようにすることができる。
また、第2の選定方法は、共通する特徴点の有無に基づく方法である。たとえば、共通する特徴点がある場合は、位置・姿勢調整対象のKF、すなわち、補正対象KF群とする。
一方、共通する特徴点が無い場合は、位置・姿勢は調整しない(固定)が、調整には使う参考KFとする。
あるいは、最新KFとの共通特徴数、時間的な近さなどから、補正対象KFを選定したり、調整に用いる参考KFを選定する代わりに、調整時の重みを変えて同様の処理をおこなってもよい。たとえば、共通特徴数が少なかったり、時間、または距離的に遠いKFは、調整影響が少なくなるよう、重みづけをする。
符号303において、KF1とKF2は、時間的に遠く共通特徴点もないので、最適化補正計算の対象外とし、拘束条件は設定しない。また、KF3は、時間的に近いが、共通特徴点がないKFなので、位置固定で補正計算に入れる参考KFとし、そのKFに関する拘束条件は設定するが、調整で位置・姿勢が変化することがないようにその位置・姿勢を固定する。また、KF4~KF7は、位置が近く、共通特徴点がある補正対象KF群であるため、そのKFに関する拘束条件を設定するとともに調整で位置・姿勢を変化するよう設定する。その際、より新しいKFほど、より位置・姿勢を変化しやすいように、拘束条件に重みづけしてもよい。このように、KFの選定をおこないながら、姿勢推定対象最新KF7の位置・姿勢を調整するための各拘束条件を設定する。
そして、符号304に示すように、(4)過去N(N=4)フレーム含めて、拘束条件に基づく最適化により、キーフレーム(KF)の位置・姿勢を補正する。符号304からもわかるように、特にKF7がG7に基づいて大きく補正し、それと共にKF4~KF6も補正されている。以上に示した手順で、KFが追加されるごとに、GNSS値を用いた逐次補正の処理をおこなうことができる。
図3に示すような模式的なKF位置・姿勢の最適化は、たとえば、既存の任意のグラフ構造を用いた最適化処理を用いて実施することができる。具体的な姿勢グラフとして、KF群をノードとして互いに拘束条件で結ぶとともに、入力GNSS情報に該当するKFに関するGNSS位置があれば、そのGNSS位置も新たなノードとして該当KFに接続し、GNSS位置に該当KF位置との差がより小さくなるような拘束条件を設定する。このように作成したグラフ構造を用いて、最適化を実施する。
なお、このGNSS位置のノードの初期値として、GNSS位置(実座標位置)のみを設定してもよいが、GNSS位置に対応するKFの現在の推定姿勢情報を加えて位置・姿勢の値を持つノードとして設定してもよい。また、これらの値(GNSS位置に関するGNSS値およびKFから得た姿勢情報)は、適宜補正して用いてもよい。たとえば、明らかにKF姿勢の値が間違っていると思われる場合などに、KFから得た姿勢を補正して使うようにしてもよい。
また、GNSS位置を保持していないKFに対しては、GNSS値の補間計算をおこなうことで、GNSS位置のノードを、すべてのKFで保持するようにしてもよい。すべてのKFノードで接続するノードや保持するデータが同じ(位置・姿勢値)となるようにすることで、より簡単にグラフ最適化を計算することができる。
また、映像に対する入力姿勢情報が取得できる場合には、推定済のKF姿勢の代わりに、入力したカメラ姿勢情報を用いるようにしてもよい。V-SLAMによって映像変化から撮影姿勢は推定できるが、直路走行などの場合に、進行方向軸に対する回転のような姿勢変化(ロール角)に誤差が生じた場合は、該誤差からの復帰(値改善)が難しいため、別途センサで測定したカメラ姿勢情報を入力することで、より精度良い撮影位置・姿勢推定が可能となる。このとき、入力姿勢情報を使うタイミングも任意でよく、たとえば、KFに対応する入力姿勢情報が有れば常に使うようにしてもよいし、新KFに対してだけ入力姿勢情報を使い、他のKF群は推定済の姿勢を使ってもよい。または、推定したKF姿勢が明らかに間違っていると思われる場合だけ、入力姿勢情報を使うようにしてもよい。
図3では、たとえばグラフ構造に使うKF群として、新KFと映像時系列的、または空間的に近いKF群を使う。たとえば、KF画像の映像時刻が新KFと近かったり(時刻差が規定時間以内だったり)、保持するGNSS位置が新KFと近かったり(GNSS位置差が規定距離以内だったり)する任意のKF群を、規定閾値を使って選定する。加えて、共有するマップ特徴点の最低数の閾値を決めて、後述するCovisivilityグラフとして保持しているKFどうしの特徴点の共有関係を有効に用いて、新KFと3次元位置を持つマップ特徴点群を規定数以上共有しているKF群も選定する。
これら2つの方法で選定したKF群双方とも、グラフ構造のノードとし、ノード(KF)間に共有するマップ特徴点の共有数が多い程、相手の位置・姿勢変化の影響を受けるような拘束条件(たとえば、より強く相対位置・姿勢を保つようなエッジ)を設定する。
さらに、図3では、共通特徴点のないKFは、グラフ最適化による位置・姿勢変化の影響を受けないノード、すなわち、位置・姿勢固定で変更を受けないノードとして設定するようにしてもよい。
このように、図3では、逐次的に新KFに合わせて、位置・姿勢の滑らかな補正のために、映像時刻的に近いKFを選定する。さらに、図3では、映像時刻による時間制限の閾値設定が難しい場合を考慮して、映像時刻と切り離した共有するマップ特徴点の多さそのものを使って、KF群を選定している。映像時刻的な近さは、往々にして共有するマップ特徴点も多く、空間的にも近いことが多いが、これらのいずれかを用いてKF群を選定することで、より新KFの位置・姿勢との関連の深いKF群を補正対象として選定することができる。
そして、後述するKF姿勢・特徴点マップ最適化部で、ノードとするKF群の自動選定を通して補正最適化の影響範囲を調整した姿勢グラフを用いて、GNSS位置を使って新KFとKF群の位置・姿勢を最適化することで、従来のV-SLAMの課題であった、画像特徴変化のみの位置・姿勢推定で生じるスケールドリフトを、シーン変化(画像上に映り込む被写体の変化)を考慮しながらも精度良く回避して、KF位置・姿勢を推定することが可能となる。また、この移動***置推定方法を用いた補正処理は、ループ走行しなくても適用することができる。
図4は、実施の形態にかかる移動***置推定方法における姿勢グラフと最適化の別の一例を模式的に示す説明図である。図4では、図3のGNSS位置による最適化で変化したKF群の位置・姿勢に合わせて、周辺のマップ特徴点の3次元位置も変化させながら、全体整合性が良くなるようにKF群の位置・姿勢も再度同時に最適化して微調整することを目的としておこなう、前述した二段階処理の後段の詳細なGNSS位置を用いた最適化の例を示す。
図4も図3と同様に、姿勢グラフを使った最適化として実施する例を示す。
図4において、符号401は、(a)GNSSによる調整前の状態を示しており、符号402は、(b)GNSSによる調整後の状態を示している。符号402からわかるように、(b)GNSSによる調整後は、KFとともに特徴点も動いていることがわかる。
図4において、三角形で示すキーフレーム(KF)は、KF11、KF12、KF13、・・・KF16と、点線の矢印で示す進行方向に追加される。KF16は、最新KFである。また、G13、G15、G16は、それぞれKF13、KF15、KF16におけるGNSS位置から得たGNSS値を示している。
図4の姿勢グラフは、図3の姿勢グラフのような、KF群とGNSS位置のノードに加えて、新KF周辺のマップ特徴点群をグラフのノードとして追加する。具体的には、図4の符号401に示すように、姿勢グラフのKF群(KF11~KF16)から閲覧されるマップ特徴点群を白丸で示すノード(N11~N16)として追加する。
また、図4でも、図3と同じように、GNSS値のノードとKFのノードとの間に任意の重みをもった拘束条件(C15、C16)のエッジを追加する。さらに、マップ特徴点群のノード群と閲覧しているKFのノードとの間にも、任意の重みをもった拘束条件のエッジを追加する。このとき、マップ特徴点群のノードは、KF群から閲覧されるマップ特徴点群のすべてを使ってもよいし、一部を間引いて使ってもよい。このように、シーン変化に合わせて、最適化(誤差最小化)計算に使うキーフレーム(KF)や特徴点群を自動制御することで、精度のよい位置・姿勢調整を実現する。
調整に使う特徴点群の選定は、KF選定から、自動的に以下のように決定することができる。たとえば、位置・姿勢調整対象KF群に存在する特徴点を、位置調整対象の特徴点とすることができる。また、位置固定KF(調整に参考利用するか否かに関わらず)のみに存在する特徴点を、位置調整しない(固定の)特徴点とすることができる。
具体的には、図4において、KF11およびKF12は、最適化計算の対象としないKFであるため、KF11およびKF12の位置・姿勢は変化しない。KF13は、位置を固定して最適化調整に用いる参考KFであり、KF14、KF15、KF16は、最適化計算で位置・姿勢調整の対象となるKFである。また、N11およびN12は、最適化計算の対象としない、位置変化しない特徴点である。一方、N13~N16は、位置調整対象の特徴点である。そのうち、N14~N16は、最新の位置・姿勢推定対象である最新KF16から閲覧できたKF16に関連する特徴点である。
KFとマップ特徴点を結ぶエッジには、たとえば該KF画像上に該マップ特徴点を投影した画像位置と、画像特徴量から該特徴点と同じと思われるKF画像内で抽出した画像特徴の画像位置との差(再投影誤差)が大きい程、位置または位置・姿勢変化をおこなうような拘束条件を設定する。
さらに、図4の姿勢グラフは、新たにマップ特徴点をノード追加することで、多くのマップ特徴点を共有するKFどうしは同じ特徴点ノードとのエッジを多数保持して似た補正影響を受けるようになるため、図3で設定していたKF間に設定したエッジ(共有する特徴点数の多さを示す拘束条件のエッジ)はすべて削除してよい。
なお、姿勢グラフ全体として、エッジの種類別に全体最適化への影響力を変えるため、図3と同様に、拘束条件に重みを設定してもよい。GNSS位置による調整を示すGNSS位置とKFとのエッジ拘束条件と、画像ベースの画像特徴変化による調整を示すマップ特徴点とKFのエッジ拘束条件とに対し、任意の割合で最適化に影響させるよう、重みを設定することで、簡単にシステム調整をおこなうことができる。たとえば、マップ特徴点ノード数が非常に多い場合は、該ノードを使った画像ベースの影響力が非常に強くなるので、適宜影響を減らすよう、もう片方の重みよりも重みをより小さく調整してもよい。
このように、本実施の形態にかかる移動***置推定方法にあっては、図4で示す姿勢グラフを用いたグラフ最適化により、図3で示すようにGNSS位置で最適化済のKF群に合わせて、必要ならKF群の位置を微調整しながら、無理なく周辺のマップ特徴点群の位置も補正することができる。このため、従来のV-SLAMの出力結果の画像撮影位置を修正しただけでは得られない、V-SLAMとしての画像的なシーン変化にも、GNSS位置にも適度に補正した画像撮影位置と姿勢を得ることができ、さらに、同様に補正した特徴点群の3次元位置(実座標マップ)を得ることもできる。
(システム構成例)
図5は、実施の形態にかかる移動***置推定システムのシステム構成の一例を示す説明図である。
図5において、実施の形態にかかる移動***置推定システム500は、移動***置推定装置の一例であるサーバ501と、移動体503に搭載された、映像および衛星505からのGNSS情報を収集する情報収集装置の一例である車載機502とを備える。そして、サーバ501と車載機502とが、ネットワーク504によって接続されることにより、移動***置推定システム500を構成する。また、移動***置推定システム500は、図示は省略するが、クラウドコンピューティングシステムによって、その機能を実現するようにしてもよい。
サーバ501は、初期姿勢・座標系設定部511と、フレーム姿勢推定部521と、KF(キーフレーム)更新部522と、3Dマップ特徴点更新部531と、グラフ制約生成部532と、KF姿勢・特徴点マップ最適化部533と、ループ検出・クロージング部541の各機能部を有する。各構成部511、521、522、531~533、541によって、サーバ501の制御部を構成することができる。これらの構成部の詳細については、後述する。
また、サーバ501は、KF群情報551および特徴点群情報552などを記憶する実座標環境マップ550を備えている。あるいは、サーバ501は、実座標環境マップ550とアクセス可能に接続されている。
すなわち、実座標環境マップ550は、サーバ501内に設けられて(記憶されて)いてもよく、また、実座標環境マップ550は、図示を省略する別のサーバ内に設けられ、ネットワーク504などのネットワークによってサーバ501と接続されていてもよい。実座標環境マップ550の詳細については、後述する。
そして、上記構成部は、大きく分けて4つの機能部に分けることができる。初期姿勢・座標系設定部511によって、システムの初期化処理機能510を実現することができる。また、フレーム姿勢推定部521と、KF更新部522と、によって、位置姿勢推定(トラッキング)処理機能520を実現することができる。また、3Dマップ特徴点更新部531と、グラフ制約生成部532と、KF姿勢・特徴点マップ最適化部533と、によって、環境マップ作成(ローカルマッピング)処理機能530を実現することができる。また、ループ検出・クロージング部541によって、ループクローズ処理機能540を実現することができる。
移動体503は、具体的には、たとえば、情報を収集するコネクテッドカーであるが、これには限定されない。一般乗用車やタクシーなどの営業車、二輪車(自動二輪や自転車)、大型車(バスやトラック)などであってもよい。また、移動体503には、水上を移動する船舶や上空を移動する航空機、無人航空機(ドローン)、自動走行ロボットなどであってもよい。
車載機502は、撮影映像に関する情報およびGNSS情報を含む移動体503の情報を収集する。移動体503の情報には、移動体503から収集する、移動体503の姿勢情報なども含まれる。
移動体503には、車載機502が備えられている。車載機502は、移動体503に搭載された専用の装置であってもよく、取り外し可能な機器であってもよい。また、スマートフォンやタブレットなどの通信機能を備えた携帯端末装置を移動体503において利用するものであってもよい。また、車載機502の機能を、移動体503が備えている機能を用いて実現するようにしてもよい。
したがって、車載機502の『車載』という表現は、移動体に搭載された専用装置という意味には限定されない。車載機502は、移動体503における情報を収集し、収集した情報をサーバ501に対して送信できる機能を持った装置であれば、どのような形態の装置であってもよい。
車載機502は、撮影映像に関する情報およびGNSS情報を含む移動体503の情報(車載データ)を取得し、取得した車載データを保存する。そして、保存した車載データを、無線通信によって、ネットワーク504を介して、サーバ501へ送信する。また、サーバ501から配信されたプログラムを含む各種データを、ネットワーク504を介して、無線通信により受信する。
また、車載機502は、近距離通信機能により、近くを走行中の別の移動体503の情報を取得し、サーバ501へ送信するようにしてもよい。また、車載機502どうしが、近距離通信機能により、通信をおこない、他の車載機502を介して、サーバ501との通信をおこなうようにしてもよい。
このようにして、移動***置推定システム500において、サーバ501は、移動体503に搭載された車載機502から車載データを取得するとともに、各車載機502へ各種データを配信することができる。
また、車載機502は、通信手段を備えていなくてよい。すなわち、車載機502は、サーバ501とは、ネットワーク504を介して接続されていなくてもよい。その場合は、車載機502に蓄積されたデータは、オフラインで(たとえば、記録メディアを介して人手などにより)、サーバ501に入力することができる。
図5において、サーバ501が、初期姿勢・座標系設定部511と、フレーム姿勢推定部521と、KF更新部522と、3Dマップ特徴点更新部531と、グラフ制約生成部532と、KF姿勢・特徴点マップ最適化部533と、ループ検出・クロージング部541の各機能部を有する構成とした。図示は省略するが、これらの各機能部の少なくとも一つを、サーバ501に加えて、あるいは、サーバに代えて、車載機502が有するようにしてもよい。
車載機502が、各機能部511、521、522、531、532、533、541の少なくとも一つを有する場合は、サーバ501が実施する処理の内容と同じであってもよい。ただし、3D地図マップ情報は、任意の媒体(DVD/BLディスク、HDDなど)に保持していて利用する以外にも、適宜、図示を省略する外部サーバから無線ネットなどを経由して取得するようにしてもよい。
(移動***置推定装置のハードウェア構成例)
図6は、移動***置推定装置のハードウェア構成の一例を示すブロック図である。移動***置推定装置の一例であるサーバ501は、CPU(Central Processing Unit)601と、メモリ602と、ネットワークI/F(Interface)603と、記録媒体I/F604と、記録媒体605と、を有する。また、各構成部は、バス600によってそれぞれ接続される。
ここで、CPU601は、サーバ(移動***置推定装置)501の全体の制御を司る。メモリ602は、たとえば、ROM(Read Only Memory)、RAM(Random Access Memory)およびフラッシュROMなどを有する。具体的には、たとえば、フラッシュROMやROMが各種プログラムを記憶し、RAMがCPU601のワークエリアとして使用される。メモリ602に記憶されるプログラムは、CPU601にロードされることで、コーディングされている処理をCPU601に実行させる。
ネットワークI/F603は、通信回線を通じてネットワーク504に接続され、ネットワーク504を介して他の装置(たとえば、車載機502、実座標環境マップ550や全画像位置姿勢データ560が格納される装置、あるいは、他のサーバやシステム)に接続される。そして、ネットワークI/F603は、ネットワーク504と自装置内部とのインターフェースを司り、他の装置からのデータの入出力を制御する。ネットワークI/F603には、たとえば、モデムやLANアダプタなどを採用することができる。
記録媒体I/F604は、CPU601の制御にしたがって記録媒体605に対するデータのリード/ライトを制御する。記録媒体605は、記録媒体I/F604の制御で書き込まれたデータを記憶する。記録媒体605としては、たとえば、磁気ディスク、光ディスクなどが挙げられる。
なお、サーバ501は、上述した構成部のほかに、たとえば、SSD(Solid State Drive)、キーボード、ポインティングデバイス、ディスプレイなどを有していてもよい。
(車載機のハードウェア構成例)
図7は、車載機のハードウェア構成の一例を示すブロック図である。情報収集装置の一例である車載機502は、CPU701と、メモリ702と、無線通信装置703と、移動体I/F704と、受信装置705と、撮像装置706と、を有する。また、各構成部は、バス700によってそれぞれ接続される。
CPU701は、車載機502の全体の制御を司る。メモリ702は、たとえば、ROM、RAMおよびフラッシュROMなどを有する。具体的には、たとえば、フラッシュROMやROMが各種プログラムを記憶し、RAMがCPU701のワークエリアとして使用される。メモリ702に記憶されるプログラムは、CPU701にロードされることで、コーディングされている処理をCPU701に実行させる。
無線通信装置703は、発信された電波を受信したり、電波を発信したりする。アンテナと受信装置とを含む構成であり、各種通信規格による移動通信(具体的には、たとえば、3G、4G、5G、PHS通信など)、Wi-Fi(登録商標)などの通信を送受信する機能を備えている。
移動体I/F704は、移動体503と車載機502の自装置内部とのインターフェースを司り、移動体503からのデータの入出力を制御する、したがって、車載機502は、移動体I/F704を介して移動体503が備えるECU(各種センサなどを含む)707から情報を収集する。移動体I/F704は、具体的には、たとえば、有線により接続する際に用いるコネクタや近距離無線通信(具体的には、たとえば、Bluetooth(登録商標))装置などであってもよい。
受信装置(たとえばGPS(Global Positioning System)受信装置などのGNSS受信装置)705は、複数の衛星505からの電波を受信し、受信した電波に含まれる情報から、地球上の現在位置を算出する。
撮像装置(たとえばカメラなど)706は、静止画や動画を撮像する機器である。具体的には、たとえば、レンズと撮像素子とを備える構成である。撮像装置706による撮像画像は、メモリ702に保存される。また、カメラなどの撮像装置706は、画像認識機能や、バーコードやQRコード(登録商標)を読み取る機能や、OMR(Optical Mark Reader)、OCR(Optical Character Reader)機能などを備えていてもよい。
図7に示したように、GNSS受信装置705および撮像装置706は、車載機502が備えていてもよく、また、移動体503が備えていたり、別途、外付けされたものを用いるようにしてもよい。その際、GNSS受信装置705あるいは撮像装置706と、車載機502とのデータのやりとりは、有線または無線通信によりおこなうようにしてもよい。
GNSS受信装置705や撮像装置706を、車載機502が備えていない場合は、移動体I/F704などを介して、それらの情報を取得するようにしてもよい。また、車載機502は、図示は省略するが、各種入力装置、ディスプレイ、メモリカードなどの記録媒体の読み書き用のインターフェース、各種入力端子などを備えていてもよい。
(実座標環境マップの内容)
図8は、実座標環境マップのデータ構成の一例を示す説明図である。図8において、実座標環境マップ550は、KF群情報551と、特徴点群情報(3次元位置情報)552aと、特徴点群情報(KF画像内位置情報)552bと、を有する。
ここで、既存技術の環境地図に相当するものが、実座標環境マップ550であり、各画像特徴点の3次元位置(特徴点群情報(3次元位置情報)552a)の他に、どの画像特徴点はどの画像で閲覧されたか(用いるか)に関する情報を持っている。これを、特徴点群情報(KF画像内位置情報)552bと、KF群情報551の2つで表している。主要な映像内の画像(KF)群の情報であるKF群情報551と、該KF画像上に各画像特徴が映っている2次元位置に関する情報である特徴点群情報(KF画像内位置情報)552bは、環境地図を任意の画像の位置・姿勢推定に用いるために必須な情報である。
図8に示すように、KF群情報551は、「ID」、「親KF ID」、「子KF ID」、「ループKF ID」、「姿勢情報」、「位置情報」、「特徴量」、「GNSS位置」および「映像フレーム番号」を含む各種情報を持っている。
ここで、「ID」は、当該KFの情報を識別する一意の識別情報であり、「親KF ID」および「子KF ID」は、KFどうしをつなぐ情報であり、「ループKF ID」は、ループクローズ処理などで使う、再度同じ場所(ループ)を通過した時に通過時其々のKF群を互いに参照保持する情報である。
また、「姿勢情報」・「位置情報」は、KFの推定撮影位置・姿勢情報であり、「特徴量」は、任意の画像に対して似た画像か否かを判断するのに用いる画像全体としての特徴量であり、「GNSS位置」は、新たに入力GNSS情報に相当する該KFの撮影時のGNSS位置であり、「映像フレーム番号」は、対応する映像のフレーム番号である。
図8に示すように、特徴点群情報(3次元位置情報)552aは、「ID」、「位置座標」、「特徴量」および「観測KFのID群」を含む各種情報を持っている。
ここで、「ID」は、当該特徴点情報を識別する一意の識別情報であり、「位置座標」は、推定した特徴点の実座標位置座標であり、「特徴量」は、画像特徴であり、「観測KFのID群」は、当該特徴点が映っているKFの情報であり、KF群情報551の中の該当するKF情報の「ID」が関連付けされる。なお、実座標位置座標は、初期姿勢・座標系設定部で作成した任意の実座標変換を利用して実座標化するものとして、ローカル値で保持していてもよい。
特徴点群情報(KF画像内位置情報)552bは、KF画像から抽出された画像特徴点群の情報であり、複数のKF画像から同時閲覧されて選定されて3次元位置を持つ特徴点群と、3次元位置を持たない特徴点群の二種類が存在する。3次元位置を持たないKF特徴点群は、任意の画像が該KF画像と似ているかを詳細評価するのに使ったり、将来新たなKF画像が得られた時に新しく選定されて3次元位置を持つ特徴点群になるのに備えて、保持しておく。
図8に示すように、特徴点群情報(KF画像内位置情報)552bは、「ID」、「KF ID」、「マップ点ID」、「特徴点位置」、「特徴点角度」および「縮小階層番号」を含む各種情報を持っている。
ここで、「ID」は、当該特徴点情報を識別する一意の識別情報である。「KF ID」は、当該KF特徴点を抽出したKFを特定するための情報であり、KF群情報551の中の該当するKF情報の「ID」が関連付けされる。「マップ点ID」は、特徴点群情報(3次元位置情報)552aへの参照情報であり、特徴点群情報(3次元位置情報)552aの中の該当する特徴点情報の「ID」が関連付けされる。この「マップ点ID」は、複数のKF画像から同時閲覧されて選定されて3次元位置を持つ特徴点群だけが持っており、3次元位置を持たない特徴点群は持っていない。
また、「特徴点位置」・「特徴点角度」は、たとえば、ORB(Oriented FAST and Rotated BRIEF)特徴の重心位置および方向ベクトルに関する情報である。また、「縮小階層番号」は、当該KF画像内での抽出状況に関する情報である。この「縮小階層番号」は、たとえば、画像特徴として縮小率を変えてピラミッド階層的に求めた縮小画像群を用いて算出したORB特徴点を想定している場合に、縮小画像群のどれで抽出したのかに関する情報である。これら「特徴点位置」「特徴点角度」「縮小階層番号」などは、他の画像特徴を使う場合は、その特徴に合わせた情報にしてよい。
このようにして、実座標環境マップ550が形成され、KF群情報と特徴点群情報とが関連付けされて、記憶される。なお、一般的に特徴点ベースのV-SLAMの環境マップは、画像特徴点群の画像特徴と3次元位置、該特徴点群を閲覧している画像KFの情報、また、画像KFと似た画像を検索できるようにするための画像KF内の画像特徴群を含むが、実座標環境マップ550は、KF群情報551に「GNSS位置」情報を新たに保持する以外は、従来のV-SLAMの環境マップと同じデータであってもよい。
(全画像位置姿勢データの内容)
図9は、全画像位置姿勢データのデータ構成の一例を示す説明図である。全画像位置姿勢データ560は、主要な画像で構成するKFとは異なり、すべての映像中の画像に対し、推定した撮影位置と姿勢を保持する。ここで、既存技術の映像内の全画像の撮影位置・姿勢に相当するものが、全画像位置姿勢データ560である。
図9に示すように、全画像位置姿勢データ560は、「ID」、「親KF ID」、「姿勢情報」、「位置情報」、「映像フレーム番号」を含む各種情報を持っている。ここで、「ID」は、当該位置姿勢データを識別する一意の識別情報である。「親KF ID」は、映像的に近く位置・姿勢を参照するKFの情報である。「姿勢情報」・「位置情報」は、親KFからの相対位置および姿勢であり、「映像フレーム番号」は、対応する映像のフレーム番号である。
位置姿勢情報は、たとえば、映像的に近いKFに対する相対位置・姿勢として保持しておき、最終的にV-SLAM結果を出力する際に、KFの位置・姿勢を反映させながら、実座標値にする。このようにすることで、逐次的にV-SLAMを処理する際に、KFの位置・姿勢が最適化処理で途中で変化することを気にせずに、全画像の位置・姿勢を最終的なKFの位置・姿勢に合わせて簡単に算出することができる。また、位置姿勢情報は、KFと同様に、実座標値でもローカル値で保持するようにしてもよい。
なお、図8および図9からもわかるように、この例では、KFの位置・姿勢に関する情報は、KFの他の情報とともに全画像位置姿勢とは別に保持するものとしている。全画像位置姿勢は映像の全画像フレームの撮影位置・姿勢であり、実座標環境マップ550のKF群情報551に含まれるKFの位置姿勢情報は、映像中の一部画像であるKF画像の撮影位置・姿勢であるため、全画像位置姿勢データ560に含めるようにしてもよい。
また、全画像位置姿勢データ560は、従来のV-SLAMと同じデータであってもよい。
また、図5の移動***置推定システム500は、図示を省略するが、図8、図9で示した実座標環境マップ550、全画像位置姿勢データ560の各種情報の他に、従来と同様に、実座標環境マップを用いたV-SLAM計算を高速化するための様々な情報を追加で保持するようにしてもよい。たとえば、画像KF群内で3次元位置を持つマップ特徴点群を共有しているKFどうし、さらにその中でも最も特徴点群の共有数の多いKF群、などの関係を保持して、各KFどうしで互いに参照できてもよい。
より具体的には、たとえば、ORB-SLAMでは、Covisivilityグラフであり、各KFをノードとしてエッジにマップ特徴点を共有するKF群、エッジの重みを共有するマップ特徴点数とする、グラフ構造のデータとして保持してよい。これらは、後述するローカルマッピング処理などで、KF位置・姿勢や環境マップの最適化計算対象を求めたり、ループクローズ処理などで現在の画像フレームに似た画像を探索したりするのを高速化するのに利用することができる。
(移動***置推定システムの内容)
図10は、実施の形態にかかる移動***置推定システム、移動***置推定方法の内容の一例を示す説明図である。
図10において、カメラなどによる映像1001、映像1001と同時に取得したGNSS情報1002、の各入力データと、初期姿勢・座標系設定部511、フレーム姿勢推定部521、キーフレーム(KF)更新部522、3Dマップ特徴点更新部531、グラフ制約生成部532、KF姿勢・特徴点マップ最適化部533、ループ検出・クロージング部541、の7つのV-SLAMをベースとする処理部と、実座標環境マップ550(KF群情報551、特徴点群情報552)、全画像位置姿勢データ560、の2つの内部保持データ、さらに、初期環境マップ1010のデータを持っていてもよい。また、このうち内部保持データの少なくともどちらかを出力データ(実座標環境マップ550’、全画像位置姿勢データ560’)として出力することができる。
なお、本実施の形態にかかる移動***置推定システム500は、従来のV-SLAM技術をベースとしているため、各処理部の処理の一部で、従来のV-SLAMの処理と同じ処理をおこなうようにしてもよい。本実施の形態では、従来のV-SLAMとして特徴点ベースのV-SLAM、特にORB特徴を用いたORB-SLAMの基本的な処理例をあげ、従来のV-SLAM処理との差を示すようにして、以下に説明する。
(入力される情報の内容)
移動***置推定システム500には、映像1001、GNSS情報1002、姿勢情報1003の各情報が入力される。映像1001およびGNSS情報1002は、初期姿勢・座標系設定部511に入力され、姿勢情報1003は、グラフ制約生成部532に入力される。ただし、グラフ制約生成部532に入力される姿勢情報1003については、必須の入力情報でなくてもよい。
映像1001は、車両などの移動体503に搭載した車載機502が有する撮像装置706によって撮影された映像である。車載機502などの車両の通信手段を用いたり、記録メディアを介して人手を使ったり、任意の方法で入手し、本システム500の入力とすることができる。また、映像の歪み補正などで用いるため、映像を撮影した撮像装置706の内部パラメータは既知とし、適宜歪み補正を実施するものとする。
GNSS情報1002は、映像撮影時の移動体503の位置であり、GPSなどの任意の既存の測位手段によるデータであり、映像と同等の任意の方法で入手して、本システム500の入力とする。
なお、GNSS情報1002は、映像によるV-SLAMのスケールドリフトを補正するために新たに利用するものであり、できるだけ映像の全フレームで保持することが望ましいが、必ずしも全フレームで保持していなくてもよい。保持するフレームが多い程、本システムで出力する全画像位置姿勢、および、実座標環境マップの位置および姿勢精度を改善することができる。
また、後述するように、本システムの初期化で利用する少なくとも映像解析開始地点付近の2つの画像フレームは、GNSS情報を保持している必要があり、なるべく密にGNSS情報を保持しているほど、映像開始から早い段階で初期化処理が終了でき、撮影位置・姿勢推定処理を実施することができる。
同様に、GNSS情報1002は、なるべく正確な位置であることが望ましく、精度が高い程、本システムの出力結果の位置および姿勢精度を改善することができる。また、GNSS情報は、GPS受信機などの位置になることが多いが、GPS受信機とカメラの相対位置関係を用いて、できるだけカメラの位置情報に変換してあることが望ましい。
また、姿勢情報1003は、任意のIMU(inertial measurement unit)などから取得する、映像を撮影した時のカメラ姿勢情報である。IMUは、具体的には、加速度センサ、ジャイロセンサなどである。たとえば、カメラを中心とし、自車前方、右方、鉛直上方、などの座標軸に対する回転角、ロール、ピッチ、ヨー角などである。GNSS情報と同様に、映像の画像すべてに対して保持してもよく、任意画像にだけ保持していてもよい。
なお、GNSS情報1002および姿勢情報1003は、上述したように別途センサ群から入手するのではなく、一度、V-SLAMで推定した各カメラ撮影位置・姿勢を、手作業などの任意の手法で補正し、補正した各カメラ撮影位置・姿勢を、再度、同じ映像のGNSS情報1002および姿勢情報1003として読み込ませるようにしてもよい。
従来のV-SLAMの出力結果であるカメラ撮影位置を手修正しても、周辺の環境マップ(特徴点群)をそれに合わせて補正することはできない。しかしながら、本システムでは、後述する独自のローカルマッピング機能により、入力されたGNSS情報1002による位置と、映像1001を解析した結果による位置の双方を適切にマージ反映させた推定ができる。したがって、本システムによれば、手修正した出力結果を入力とする再実施を通して、手修正結果に合わせて特徴点群を含めた実座標環境マップを滑らかに作成することが可能となる。
なお、最初の実施時は姿勢情報を入力しなくても位置情報とともに姿勢情報も推定出力するので、再実行では、該推定姿勢情報も位置情報とともに入力して使えるが、姿勢情報は使わずに位置情報だけを入力として使ってもよい。たとえば、手修正した位置情報と異なり、まったく姿勢情報を手修正できなかった場合などでは、2つの情報は確からしさが異なっているため、精度の低い推定姿勢情報は使わずに位置情報だけを再実施時に入力利用して、実座標環境マップを作成することができる。
また、一度出力した実座標環境マップ550’を再び入力として利用してもよい。たとえば、ある走路の最初の走行映像の撮影位置・姿勢を推定する際には、実座標環境マップが存在しないので、当該実座標環境マップの入力無しで本システムにおける処理を実行し、つぎに同じ走路を走行した二回目以降の走行映像の撮影位置・姿勢を推定する場合には、最初の走行映像の処理結果として出力した実座標環境マップ550’を入力して、あたかも当該映像の処理で作成した内部データかのように利用することができる。このとき、一回目と二回目以降の走行映像では、撮影する車両やカメラ、走行レーン内の位置などが異なっていてもよい。なお、実座標環境マップ550’を入力する場合には、GNSS情報1002の入力を省略してもよい。
なお、車載機502が、各機能部511、521、522、531、532、533、541の少なくとも一つを有する場合は、映像1001やGNSS情報1002は、車載機502の内部に保持してV-SLAMを処理するようにしてもよい。
以後の本システムの説明では、特に記載がない場合には、実座標環境マップ入力がなく、一から実座標環境マップを作成する場合(GNSS情報1002の入力を必須とする場合)について説明をおこなう。また、GNSS情報1002として、平面直角座標系の値を例として説明をおこなう。
(初期姿勢・座標系設定部511の内容)
システムの初期化処理機能510を担当する初期姿勢・座標系設定部511は、計算する座標系の決定とともに、初期化処理として以後のトラッキングなどの処理機能で必要とする内部データの作成をおこなう。具体的には、映像開始時の場所近傍にある特徴点群の3次元位置を推定するとともに、初期KF位置・姿勢の推定をおこない、以後の処理で最低限必要となる、映像開始時の場所近傍の実座標環境マップを作成する。この初期姿勢・座標系設定部の処理のうち、計算に用いる座標系の決定処理以外は、従来のV-SLAMの初期処理と同じ処理でもよい。この初期化処理が完了しないと、以後のフレーム姿勢推定を含めた処理は実行されないことも、従来のV-SLAMと同じである。
初期姿勢・座標系設定部511は、具体的には、たとえば、図6に示した、メモリ602に記憶されたプログラムをCPU601が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図7に示した、メモリ702に記憶されたプログラムをCPU701が実行することによって、その機能を実現するようにしてもよい。
初期姿勢・座標系設定部511は、まず、歪み補正した映像の各画像に対し、任意の画像特徴群を取得する。つぎに、最初の2フレームで同時に映っている特徴点(各画像の特徴点のペア)を各画像特徴群から探索する。ペアの探索方法は、利用する画像特徴に依存し、既存の2画像の同特徴対の探索方法を利用してもよい。算出したペア数が十分多い場合には、特徴点群の画像変化を用いて、カメラの位置・姿勢変化と2画像に共通で映る各特徴点群の3次元位置を推定する。
すなわち、2画像に映る各特徴点ペアの位置・姿勢変化から既存の方法、たとえば、平面を想定したHomographyや、非平面を仮定した基礎行列などの幾何モデルを使った手法を用いて、2画像のカメラの位置・姿勢の変化を表す変換を推定するとともに、推定した2画像のカメラの位置・姿勢と各特徴ペアの各画像上の位置から、既存の三角測量などの手法を用いて、各特徴の3次元位置を推定する。
なお、ペア数が不足する場合には、2画像のどちらかの画像(たとえば、後時刻の画像)を他の画像(たとえば、さらにその後の時刻の画像)に変えてこの処理をおこなう。また、利用する最初の2フレームは、厳密に映像開始時点の画像でなくてもよく、同じ被写体が映っている可能性のある任意の2フレームであってもよい。たとえば、停車中映像であることがわかっているのであれば、カメラ撮影位置が変わったと思われる画像を、後時刻の画像として選ぶようにしてもよい。
また、このとき、すべての特徴点群ペアの3次元位置を算出するのではなく、他の特徴点群と比べて誤差の大きな特徴点を省いたり、画像全体でまんべんなく規定数の特徴点が得られるように、特徴点群が集中する画像部分では特徴点群を間引いたり、2カメラ位置と当該特徴点の成す角(交会角)が小さな特徴点を省いたり、というように、任意の特徴点選別をおこなってもよい。
また、初期姿勢・座標系設定部511は、従来のV-SLAMと同様にさらに最適化計算をおこなって、算出した初期値をより正確な値へと更新する処理を付加してもよい。具体的には、2画像の各画像に対し、カメラ位置と特徴点群の3次元位置とがわかっており、各画像に特徴点群がどう映り込むのかを計算することができるため、各画像への特徴点群の映り込み位置と、実際のカメラ画像での該特徴点の位置との差(再投影誤差と呼ぶ)を調べ、特徴点群の再投影誤差がなるべく少なくなるよう、特徴点やカメラ位置および姿勢を微調整する最適化補正(BA(Bundle Adjustment))をするようにしてもよい。
続いて、初期姿勢・座標系設定部511は、算出結果から初期環境マップ1010を作成する。すなわち、用いた2画像をKFとして推定した該画像の撮影位置・姿勢とともに初期環境マップ1010に登録して、同じく推定した特徴点群の情報(2画像上の位置や、3次元位置)も、初期環境マップ1010に登録する。この初期環境マップ1010は、初期化処理ということで、以後のトラッキングやローカルマッピングなどの機能部でおこなう方法とはやや異なる方法でKF位置・姿勢や特徴点群の3次元位置を推定しており、若干精度が低い。
なお、これらの2画像のカメラ位置・姿勢、および車両周辺の特徴点群の3次元位置の初期値の算出処理では、従来のV-SLAMと同様に2画像のうち片方の画像(多くは、より時間の早い画像。以後、「初期カメラの画像」と呼ぶ)のカメラ位置・姿勢(以後、「初期カメラ位置・姿勢」と呼ぶ)を原点および基準座標系としたローカル系で算出してよい。
たとえば、一般的に画像処理で用いる画像上の画素位置を示すための画素座標系は、撮影画像の画像横方向をX、画像下方向をYとすることが多い。このため、従来のV-SLAMもこれと似た基準座標系定義とするために、初期フレームのカメラ位置を原点(0,0,0)とし、自車右手方向X、自車鉛直下方向Y、自車前方方向Z、という右手系(SLAMローカル系)定義とすることが多い。本システムにおいても、このSLAMローカル系で2画像のカメラ位置・姿勢と、2画像に共通で映る特徴点群の3次元位置を算出する。
このように、初期姿勢・座標系設定部511は、従来のV-SLAMと同様に、初期環境マップ1010の作成(KF位置・姿勢の推定、および特徴点群の3次元位置の推定)処理をおこなう。
つぎに、初期姿勢・座標系設定部511は、SLAMローカル座標系で算出した環境マップのKF撮影位置・姿勢、および特徴点群の3次元位置を実座標系対応にするために、入力GNSS情報から2画像に対応するGNSS位置座標値を得て、SLAMローカル(座標)系と実座標系の変換行列を算出する。
図11A~11Cは、初期姿勢・座標系設定部511における変換行列算出の一例を示す説明図である。図11Aに示すように、本システム500では、実座標系として平面直角座標系を用いる。具体的には、符号1101は、(a)SLAMローカル系(右手系)を示している。具体的には、原点(初期カメラ)に対して、X方向が右手方向を示しており、Y方向が下方向を示しており、Z方向が進行方向を示している。
これに対して、符号1102は、(b)実座標系、すなわち、平面直角座標系(左手系)を示している。具体的には、平面直角座標系原点(0,0,0)に対して、X方向が「北」、すなわち、平面直角座標系X値[m]を示しており、Y方向が「東」、すなわち、平面直角座標系Y値[m]を示しており、Z方向が「上」、すなわち、標高値[m]を示している。
ただし、これは一例であって、従来のV-SLAMの右手系のSLAMローカル系とは異なる左手系の平面直角座標系を用いるのではなく、右手系の任意の座標系を用いるようにしてもよい。
図11Bは、移動ベクトルを示している。符号1103は、ローカル系の移動ベクトルAであり、符号1104は、実座標系の移動ベクトルBである。移動ベクトルとは、「任意2時刻の画像フレームの遅い時刻の方のフレーム(F2)の位置」と、「任意2時刻の画像フレームの早い時刻の方のフレーム(F1)の位置」の差分(F2-F1)を示す進行方向ベクトルである。図11Bに示すように、同じ移動ベクトルが、2つの座標系(移動ベクトルA1103と、移動ベクトルB1104)で表現されている。そこで、初期姿勢・座標系設定部511は、別の系の値に変換する変換行列(ローカル系→実座標系への変換行列M)を算出する。
図11Cは、SLAMローカル系から実座標系の値に変換するための変換行列Mの内容について示している。図11Cにおいて、SLAMローカル系から実座標系の値に変換するための変換行列Mは、座標系間のスケール相違を吸収するためのスケール変換行列M1、進行方向由来の系から緯度経度由来の系へ座標軸を変換した値にするための回転行列M2、XYZの座標軸定義を変えた値にするためのM3、右手系から左手系の値に変換するM4、原点を初期カメラ位置から平面直角座標系の原点に変更した値に変換するM5、の5つの行列の積算から成る。
スケール変換行列M1は、画像変化による任意スケールを実座標のスケールとする変換行列である。スケール変換行列M1によって、(1)SLAMローカル系1111を、(2)mスケールのSLAMローカル系1112に変換することができる。
図11Dは、画像変化由来の任意スケールを、緯度経度の座標系のスケール[m]にするスケール変換行列M1の算出の一例を示す説明図である。
図11Dにおいて、まず、SLAMローカル系の2画像カメラ位置Q1(前時刻画像F1に対応)、Q2(同後時刻画像F2)から、その差分(各位置差、Q2-Q1)である移動ベクトルA1103を算出する。成分定義はSLAMローカル系そのもの(従来のV-SLAM出力値)であってもよい。
つぎに、2画像のGNSS位置S1(前時刻画像F1に対応)、S2(同後時刻画像F2)から、実座標系(直角平面座標系)の値を使うが成分(軸)定義が異なる、特殊実座標系の移動ベクトルB1104(=S2-S1)を算出する。特殊実座標系は、(X成分=東が+の経度座標値差、Y成分=-(標高値差)、Z成分=北が+の緯度座標値差)とする。
そして、移動ベクトルA1103の大きさ=lenA、および、移動ベクトルB1104の大きさ=lenB、をそれぞれ求める。求めた大きさから、大きさ比 Rate=(lenB÷lenA)を求めて、Rate倍するスケール変換行列をスケール変換行列M1として求める。Eye(m,n)をm行n列の単位行列としてあらわすとき、スケール変換行列M1は、
M1=Rate×Eye(3,3);となる。
図11Cに戻って、回転行列M2は、進行方向由来の座標系を緯度経度由来に変更する変換行列である。回転行列M2によって、(2)mスケールのSLAMローカル系1112を、(3)特殊実座標系1113に変換することができる。
図11Eは、回転変換行列M2の算出の一例を示す説明図である。図11Eにおいて、まず、移動ベクトルをそれぞれの長さで割り、正規化したローカル系の移動ベクトルA’=A/lenAと、正規化した実座標系の移動ベクトルB’=B/lenBを求める。
つぎに、符号1105に示すように、(a)ベクトルA’からベクトルB’、への成す角Θを、内積から求める。
Θ=acos(内積(A’,B’))
そして、符号1106に示すように、(b)ベクトルA’とベクトルB’の外積=A’×B’となる、上方向ベクトル(VectorUP)を求めて向きを考慮した角度Θ’を算出する。上方向ベクトルのY値が正の場合は、角度Θ’=-Θとし、負の場合は、角度Θ’=Θ、とする。
軸定義の変換と、座標値の変換は逆になるため、Y軸周りの(-Θ’)回転行列を、行列M2とする。
図11Cに戻って、実座標系定義変換行列M3は、X軸周りの-90度回転する変換行列である。実座標系定義変換行列M3によって、(3)特殊実座標系1113を、(4)特殊実座標系2、1114に変換することができる。
実座標系定義変換行列M4は、右手系から左手系に変換する変換行列である。XとYを互いに変換する。実座標系定義変換行列M4によって、(4)特殊実座標系2、1114を、(5)実座標系、すなわち、平面直角座標系(左手系)1115に変換することができる。ここでは、本来の平面直角座標系と原点位置が異なっている。
位置移動変換行列M5は、原点位置を移動させる変換行列である。具体的には、カメラの初期実座標位置(初期カメラ位置)を平行移動し、初期カメラ位置=平面直角座標系X[m],平面直角座標系Y[m],標高[m]とする。位置移動変換行列M5によって、(5)平面直角座標系(左手系)1115を、(6)本来の平面直角座標系(左手系)1116に変換することができる。
このようにして、SLAMローカル系1111(図11Aに示した1101)を実座標(平面直角座標系(左手系))1116(図11Aに示した1102)とすることができる。
初期姿勢・座標系設定部511は、この座標系変換行列Mを保持することで、従来のV-SLAMと同様に算出していたSLAMローカル座標系での初期環境マップを、平面直角座標系の実座標環境マップに変換することができる。この結果、画像由来で特に大きさには意味がなかったSLAMローカル系のスケールを、本システムにおいて使うことで、m単位の実座標スケールに統一することが可能になる。
なお、初期姿勢・座標系設定部511は、必要があれば、変換行列を持つだけでなく、実際に算出済の特徴点群の3次元初期位置や、2画像の撮影位置・姿勢位置を、この変換行列を用いて実座標系の値に変換してもよい。特に、頻繁に参照する特徴点群の位置は、後述するトラッキング処理機能(フレーム姿勢推定)失敗時の再初期化実施前後で統一のとれた値として使うため、実座標系の値として保持することが望ましい。実座標系の値としてあらかじめ保持しておけば、各画像上への投影位置は、該変換抜きで計算することができる。
一方で、平面直角座標系のような実座標系の値は、数値が非常に大きくなることが多いため、環境マップの特徴点群3次元位置は従来と同じローカル座標系の値のまま保持し、加えて新たに変換行列も保持することで、必要な時だけ変換行列を用いて実座標系の値に変換してもよい。あるいは、実座標系の値であっても、適当な初期値からの差分値としてもよい。
本システム500においては、従来と同じローカル座標系で初期姿勢・座標系設定をおこなってから、実座標系に変換するための情報を作成して、以後の3次元座標値はすべて実座標変換をおこなった実座標系の値で保持するものとして、説明する。
なお、既存の実座標環境マップを入力する場合には、初期姿勢・座標系設定部の処理を飛ばし、入力した実座標環境マップを初期実座標環境マップとして、以後の処理と同様の
処理を実施する。
本システム500においては、従来のV-SLAMと同様に、初期姿勢・座標系設定部511の処理を2画像(初期KF)に対して実施すると、初期化が完了されたとみなして、以後の処理を、まだ処理していない画像に対して順次実施していくことにする。したがって、以後の処理は、初期化に用いた2画像(初期KF)に対しては実施せず、それ以後の画像に実施するようにする。
以後の処理である、トラッキング処理機能、マッピング処理機能、ループクローズ処理機能は、説明を簡単にするため、シーケンシャルに処理する形とする。実際には複数スレッドを用いた同時処理であってもよい。その場合には、各処理機能がそれぞれ、内部保持するKF位置・姿勢や、実座標環境マップを相互参照するので、適宜、既存の編集ロック機能などを用いて複数処理での同時編集を防ぐことができる。各処理機能は、処理対象の画像がなくなるまで、映像の各画像を順に処理していくこととする。
(フレーム姿勢推定部521の内容)
図10において、位置姿勢推定(トラッキング)処理機能520を担当するフレーム姿勢推定部521は、通常の処理に失敗した時の対応処理(後述するリローカリゼーション失敗時の処理)以外は、従来のV-SLAMと同様の処理をおこなう。すなわち、フレーム姿勢推定部521は、カメラ移動した入力新画像(歪み補正済)に対し画像特徴群を算出し、画像特徴量を比較することで同じ特徴点と思われる算出済の3D特徴点群の位置(実座標環境マップ550)を得る。
このとき、定速走行とみなし新画像カメラの初期位置・姿勢を推定し、1つ前の画像で用いた3D特徴点群を、推定した初期位置・姿勢を用いて新画像上に投影する。そして、その付近で対応する特徴点を検索することによって、同じ特徴点と思われる3D特徴点の候補を絞り込むようにしてもよい。
その後、同じ特徴点として見つかった3D特徴点群全体に対し、新画像上への再投影誤差が小さくなるように、新画像カメラの位置・姿勢を最適化する。すなわち、3D特徴点群の位置は変化させずに、カメラ位置・姿勢だけ最適化するBAをおこなう。続けて、新画像と3D特徴点群を共有する第1のKF群を探して、ついで、当該第1のKF群1と3D特徴点群を共有する第2のKF群を探し、これらの第1のKF群、第2のKF群の3D特徴点群を得る。
このとき、得た3D特徴点群に対し、新画像のカメラ位置からの距離(規定距離の範囲内)や、カメラからの閲覧方向の違い(たとえば、新画像のカメラ位置から該3D特徴点へ向けた閲覧方向ベクトルと、今迄のKF群のカメラ位置から該特徴点へ向けた閲覧方向ベクトルとの内積の大きさが規定値以上)などを用いて、任意の取捨選択をしてもよい。第1のKF群、第2のKF群から得た、より多くの3D特徴点群を使って、フレーム姿勢推定部521は、再度新画像上に投影し、再投影誤差が小さくなるような位置・姿勢最適化を実行する。
フレーム姿勢推定部521は、具体的には、たとえば、図6に示した、メモリ602に記憶されたプログラムをCPU601が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図7に示した、メモリ702に記憶されたプログラムをCPU701が実行することによって、その機能を実現するようにしてもよい。
なお、フレーム姿勢推定部521で、十分な3D特徴点群が得られないなどの理由で、位置・姿勢推定に失敗した場合には、従来のV-SLAMと同様に、位置・姿勢復帰処理であるリローカリゼーション処理を実施するようにしてもよい。リローカリゼーション処理では、画像特徴的に似たKFがないか全KF群を探索し、似たKF候補群が見つかったら、それらKFの3D特徴点群と新画像の特徴点群とのマッチングをおこない、マッチング数が多いKFを最終的なKFとして選定するようにしてもよい。
リローカリゼーション処理は、つぎに、当該KFと新画像のマッチングした特徴点群どうしを使って、より少数の特徴点を用いた既知のPnP問題を解くことによって、初期位置・姿勢推定をおこなう。そして、得た新たな新画像の初期位置・姿勢から、より多くの特徴点群を使った非線形最小二乗法などの任意の最適化手法を用いて、位置・姿勢最適化を実施し、当該新画像の推定カメラ位置・姿勢とする。
ここまで、本システム500のフレーム姿勢推定部521は、従来のV-SLAMと同じ処理を実施する。一方で、本システム500のフレーム姿勢推定部521は、上述したリローカリゼーション処理も失敗した場合の処理が、従来のV-SLAMと異なる。リローカリゼーション処理も失敗した場合には、従来のV-SLAMでは、処理続行が不能であるため、そのまま処理を終了する。しかし、本システム500の場合は、処理終了をする代わりに初期姿勢・座標系設定部511に戻って、既存の実座標環境マップ550や全画像位置姿勢データ560などの内部算出データを残したまま、初期化処理を再実施することができる。
従来のV-SLAMでは、リローカリゼーション失敗は、今迄の追跡してきた画像およびKF群と対応関係がまったく取れなくなったことを意味する。上述したように、従来のV-SLAMは、算出するSLAMローカル系が初期化で用いた初期画像に関係する座標系であるため、一度対応関係が取れなくなると、以後、再度初期化処理をおこなっても、今迄の算出した環境マップとは異なる新たな初期画像に関する座標系で計算を始めるため、実質的に対応が取れなくなったところまでと、再度初期化を始めて以降とでは、算出する環境マップおよび、カメラの位置・姿勢の値の対応が取れず、実質的に別物の細切れとなってしまう。
このため、従来のV-SLAMは、リローカリゼーション失敗時には、初期化処理をおこなっても意味が無いため、初期化処理をおこなわずに処理終了としていた。しかし、本システム500では、初期化処理として初期化後の座標系およびV-SLAMの値は、すべて実座標系とすることができるので、今迄追跡してきた画像およびKF群との対応関係が取れなくなっても、実座標系の値である以上、算出する環境マップやカメラ位置・姿勢の値は整合性の取れた値となっている。
これにより、初期化の前後で算出する環境マップおよびカメラ位置・姿勢の値は、そのまま両方を混ぜて保持しても問題がないため、本システム500においては、従来のV-SLAMと異なり、リローカリゼーション失敗時には、初期姿勢・座標系設定部511による処理を再度実施する。このとき、上述したように、特に多数かつ頻繁に参照をする特徴点群の3次元位置は、SLAMローカル座標系の値と実座標系への変換行列として保持していると、初期化処理を実行するたびに、双方の値が変わってしまう(積算した実座標系の値は同じ)可能性があって煩雑なため、できるだけ実座標系の値にして保持しておくことが望ましい。
(KF更新部522の内容)
図10において、位置姿勢推定(トラッキング)処理機能520を担当するKF(キーフレーム)更新部522は、従来のV-SLAMのように画像特徴的に新画像をKFにするか判定する。また、本システム500のKF更新部522は、さらにGNSS位置を保持する画像か否かも用いて、新たにKF画像を選定する。すなわち、すべての画像にGNSS位置が無く、GNSS位置が無い画像が規定数以上続いた場合において、GNSS位置がある画像が新画像として入力された場合に、画像特徴的な従来の判定の結果に関わらず、新画像を新しいKFとして採用する。
なお、従来のV-SLAMの画像特徴的なKFにするか否かの判定は、たとえば、最後のKFからの経過時間や経過フレーム数が規定値を超えた場合や、フレーム姿勢推定部で取得したKF群1のうち、最も新画像と3D特徴点群を共有するKFとの共有3D特徴点群数が、規定数以下だった場合、などである。その後、新追加するKFに対し、KF更新部は、KFとされた新画像を実座標環境マップのKF群に追加する。上述したように、特徴点を共有するKF群どうしで別途グラフ構造(KF群1)を保持している場合には、適宜、新追加するKF(新画像)についても、該グラフ構造を更新する。
KF更新部522は、具体的には、たとえば、図6に示した、メモリ602に記憶されたプログラムをCPU601が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図7に示した、メモリ702に記憶されたプログラムをCPU701が実行することによって、その機能を実現するようにしてもよい。
図12は、KF更新部の処理の手順の一例を示すフローチャートである。図12のフローチャートにおいて、KF更新部522において、現在の画像が、現在利用中のKFから規定フレーム数以上離れているか否かについて判断する(ステップS1201)。ここで、規定フレーム数以上離れていない場合(ステップS1201:No)は、何もせずに、一連の処理を終了する。
一方、規定フレーム数以上離れている場合(ステップS1201:Yes)は、つぎに、現在の画像が、現在利用中のKFとの共通特徴点が規定数以下か否かを判断する(ステップS1202)。ここで、共通特徴点が規定数以下でない場合(ステップS1202:No)は、ステップS1204へ移行する。一方、共通特徴点が規定数以下である場合(ステップS1202:Yes)は、つぎに、現在の画像フレームが、現在利用中のKFと最も共通特徴点が多い他のKFに対し、共通特徴点が規定数以下か否かを判断する(ステップS1203)。
ステップS1203において、共通特徴点が規定数以下でない場合(ステップS1203:No)は、ステップS1204へ移行する。一方、共通特徴点が規定数以下である場合(ステップS1203:Yes)は、ステップS1205へ移行する。つぎに、ステップS1204において、現在の画像が、GNSSの位置情報を保持しているか否かを判断する(ステップS1204)。ここで、GNSSの位置情報を保持していない場合(ステップS1204:No)は、一連の処理を終了する。一方、GNSSの位置情報を保持している場合(ステップS1204:Yes)は、ステップS1205へ移行する。
ステップS1205において、現在のKFは、GNSS位置情報を保持する最も新しいKFから規定KF数以上離れているか否かを判断する(ステップS1205)。ここで、最も新しいKFから規定KF数以上離れている場合(ステップS1205:YES)は、ステップS1206へ移行する。一方、離れていない場合(ステップS1205:NO)は、一連の処理を終了する。
ステップS1206において、現在の画像を新KFとする(ステップS1206)。そして、新KFを実座標環境マップのKF群に追加する(ステップS1207)。さらに、KF群の特徴点共有関係のグラフ構造に新KFを追加し、グラフを更新する(ステップS1208)。これにより、一連の処理を終了する。
なお、KFの追加判断だけを、トラッキング処理機能520を担当する処理部いずれか(たとえばKF更新部522)でおこない、実際のKF追加処理を独立させ、ローカルマッピング処理機能530を担当する処理部531~533のいずれかで実施するようにしてもよい。
本システム500においては、KF追加処理を、KF更新部522で実施するものとして説明した。しかし、トラッキング処理機能520は、全画像フレームに対する処理であり、ローカルマッピング処理機能530は、KF追加タイミングで実施するKFに関する処理として考える方を優先させるなら、KF追加処理を実施するか否かの判断のみをトラッキング処理機能520を担当する処理部521~522のいずれかで実施し、実際のKF追加処理は、ローカルマッピング処理機能530を担当する処理部531~533のいずれかで実施した方がよい。従来のV-SLAMにおいても、KF追加処理自体は、ローカルマッピング処理機能530を担当する処理部531~533のいずれかで実施することが多い。
(3Dマップ特徴点更新部531の内容)
環境マップ作成(ローカルマッピング)処理機能530を担当する3Dマップ特徴点更新部531は、従来のV-SLAMと同様の、追加したKFを使って最近追加した3Dマップ点の除去判断を実施するととともに、新たな3Dマップ点の追加処理を実行する。
3Dマップ特徴点更新部531は、具体的には、たとえば、図6に示した、メモリ602に記憶されたプログラムをCPU601が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図7に示した、メモリ702に記憶されたプログラムをCPU701が実行することによって、その機能を実現するようにしてもよい。
3Dマップ点の除去判断処理として、3Dマップ特徴点更新部531は、最近追加した3Dマップ点群が、新追加KFを含めたKF群全体で規定数以上のKFから閲覧できているか否かなどから、3Dマップ点が利用されているか否かを判定する。そして、3Dマップ点が利用されていないと判断された場合には、3Dマップ点を除去する。
なお、3Dマップ特徴点更新部531では、除去判断だけおこない、実際の除去処理は、続くKF姿勢・特徴点マップ最適化部533のBAなどの3Dマップ点の利用有無を別途詳細に調査利用する処理などと同時に実施してもよい。
新3Dマップ追加処理として、3Dマップ特徴点更新部531は、追加した新KFで3D特徴点群と対応付いていない特徴点を探し、KF更新部522で更新した新KFと特徴点を共有する第1のKF群の同じく対応付いていない特徴点と、画像特徴量から同じ特徴点を探す。このとき、さらに当該KFでのエピポーラ制約や再投影誤差などの任意の方法で、同じ特徴点か否かを絞り込んでもよい。同じ特徴点が見つかった場合には、2つのKFのカメラ位置と、当該KF上の画像に映る同じ特徴点の画像内位置を用いて、既知の三角測量の手法を使って、当該特徴点の3次元位置を求め、新たな3D特徴点として、実座標環境マップに追加する。
(グラフ制約生成部532の内容)
環境マップ作成(ローカルマッピング)処理機能530を担当するグラフ制約生成部532は、続くKF姿勢・特徴点マップ最適化部533で、従来のように現キーフレームと周辺の特徴点群3次元位置をBAで求める処理(ローカルBA)をおこなう前に、新たに、現キーフレームの位置と周辺の特徴点群の3次元位置を入力GNSS情報に合わせて姿勢グラフを用いて事前に補正する処理を実施するため、その準備をおこなう処理部である。
グラフ制約生成部532は、具体的には、たとえば、図6に示した、メモリ602に記憶されたプログラムをCPU601が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図7に示した、メモリ702に記憶されたプログラムをCPU701が実行することによって、その機能を実現するようにしてもよい。
姿勢グラフという概念、および、グラフ構造を用いた最適化計算自体は、一般的なグラフ理論(姿勢グラフ構造)に基づく最適化計算と同じでもよく、既存のg2o(General Graph Optimization)などの最適化ライブラリを用いてもよい。
グラフ制約生成部532は、この一般的なグラフ構造を活用し、KF位置・姿勢のみの最適化(実座標環境マップのうちKF情報群のおおまかな最適化)と、当該最適化後のKF群と周辺の特徴点群の双方を使った位置・姿勢の最適化(実座標環境マップ全体の詳細最適化)、という2段階の最適化向けに、それぞれ最適化対象(ノード)と拘束条件(エッジ)の異なる2つの姿勢グラフを作成する。
なお、本システム500において、KF位置・姿勢のみの最適化と、周辺特徴点群も用いた最適化の2段階でGNSS情報に合わせた最適化を実施しているが、これは一例であり、KF位置と周辺特徴群を用いた最適化を一度だけ実施してもよいし、さらにもう一度KF群のみの最適化をして3段階の最適化をしてもよいし、順番を変更してもよい。このとき、この最適化を何段階で実施するかをKFごとに変更するようにしてもよい。また、KF群の位置・姿勢の最適化ではなく、KF群の位置またはKF群の姿勢どちらか片方のみの最適化としてもよい。
最適化対象となるKF群が周辺特徴点群と比べて数が少ないため、KFの位置であるGNSS位置による制約条件による補正効果が効きづらくなるのを避けるため、本システム500では、まず、KF位置・姿勢を大まかにGNSS位置によって十分補正してから、周辺特徴点群を含めた最適化をおこなう二段階としている。
グラフ制約生成部532は、第一段の新KFとその近傍のKF群の位置・姿勢の最適化、および、第二段のさらに周辺特徴点群位置も含めた最適化を実施するため、従来のV-SLAMと異なり、推定済のKF群の3次元位置と、GNSS情報で入力した該KFに対応するGNSS位置(3次元位置)とのずれを最小化する、新たな拘束条件を導入したグラフ構造を作成する。
図13は、グラフ制約生成部の処理の手順の一例を示すフローチャートである。図13のフローチャートにおいて、まず、新KFにGNSS位置があり、かつ、他にKFはあるか否かを判断する(ステップS1301)。この例では、新KFにGNSS位置があり、かつ、他にKFがある場合のみを対象にしており、それ以外は対象としていないため、新KFにGNSS位置がない場合、あるいは、他にKFはない場合(ステップS1301:No)は、何もせずに一連の処理を終了する。
なお、最適化を実施する効果はあまり無いが、新KFにGNSS位置がない場合も、GNSS情報でKF群と周辺特徴点群を最適化したい場合には、ステップS1301では、他にKFがあるか否かの判断だけをおこなえばよい。
ステップS1301において、新KFにGNSS位置があり、かつ、他にKFがある場合(ステップS1301:Yes)は、当該新KFを姿勢グラフにノードとして登録する(ステップS1302)。そして、当該新KFと特徴点を共有するKF群を、位置・姿勢を変更できるノードとして姿勢グラフに追加し、共有特徴点数が多い程、強く相対位置・姿勢を保つエッジを、特徴点群を共有するKFどうしに設定する(ステップS1303)。
つぎに、姿勢グラフにないKF群のうち、新KFとの時刻差が閾値以内のKF群を、位置・姿勢変化をおこなわないノードとして姿勢グラフに追加し、他の登録済KFのうち、最も共有特徴点数が多いKFと共有特徴点数が多い程、強く相対位置・姿勢を保つエッジを設定する(ステップS1304)。
そして、各KFでGNSS位置があれば、GNSS位置を当該KFと繋がるノードとして追加し、GNSS位置と当該KFの間に、GNSS位置と当該KFの位置差がより小さくなるエッジを設定し(ステップS1305)、当該姿勢グラフを、KF群の位置・姿勢最適化用グラフ(第1の姿勢グラフ)とする(ステップS1306)。
つぎに、当該姿勢グラフの位置・姿勢変化をおこなうKF群から閲覧できる3次元位置を持った特徴点群を当該姿勢グラフの位置変化をおこなうノードとして追加し、位置・姿勢変化をおこなわないKF群からのみ閲覧できる3次元位置を持った特徴点群を当該姿勢グラフの位置変化をおこなわないノードとして追加し、それぞれ閲覧するKFとの間に、再投影誤差をより小さくするエッジを設定する(ステップS1307)。そして、当該姿勢グラフのKF群どうしを結ぶエッジを削除する(ステップS1308)。さらに、特徴点ノード数とGNSSノード数を調べ、エッジ種類ごとの重みを調整する(ステップS1309)。この当該姿勢グラフを、KFの位置・姿勢と周辺マップ特徴点位置の最適化用グラフ(第2の姿勢グラフ)とし(ステップS1310)、一連の処理を終了する。
このようにして、グラフ制約生成部532の処理によって、新しい2つの姿勢グラフ(第1の姿勢グラフおよび第2の姿勢グラフ)を生成することができる。なお、図13のフローチャートにおいては、わかりやすくするために、毎回グラフを作成するようにしたが、既存グラフを更新して内容を作成するようにしてもよい。
(KF姿勢・特徴点マップ最適化部533の内容)
環境マップ作成(ローカルマッピング)処理機能530を担当するKF姿勢・特徴点マップ最適化部533は、新たに、グラフ制約生成部532において生成した新しい2つの姿勢グラフを用いて、一般的なグラフ最適化計算をおこなう。そして、KF姿勢・特徴点マップ最適化部533は、入力されたGNSS情報から得たKFのGNSS位置に合わせたKF位置・姿勢の最適化と、最適化したKF群の周辺の特徴点群の3次元位置の最適化、という2つの最適化を実施する。
KF姿勢・特徴点マップ最適化部533は、具体的には、たとえば、図6に示した、メモリ602に記憶されたプログラムをCPU601が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図7に示した、メモリ702に記憶されたプログラムをCPU701が実行することによって、その機能を実現するようにしてもよい。
KF姿勢・特徴点マップ最適化部533は、その後さらに、GNSS位置に合わせて補正最適化済のKF群の位置・姿勢と、特徴点群の3次元位置(実座標環境マップ)と、を用いて、従来のV-SLAMと同じように、新KF近傍でのBA処理(ローカルBA)の実施を通して、画像上の被写体の映りをメインとした位置・姿勢補正をおこなう。
この結果、追加した新KFの影響として、新KFに関係の深いKF群、多くは映像時系列に近いKF群の位置・姿勢の調整と、新KFの周辺の特徴点群の3次元位置の調整とを、入力GNSS情報にあらかじめ合わせておいてから、従来の画像の特徴点の再投影誤差を用いた最適化を実施して位置・姿勢を微調整することになる。したがって、従来のV-SLAMの課題である、徐々にスケールドリフトが発生するという課題を解決した実座標値で出力することが可能となる。
なお、本システム500のグラフ制約生成部532における姿勢グラフ生成と、KF姿勢・特徴点マップ最適化部533における、ローカルBAに先立って実施する、当該姿勢グラフを用いたグラフ最適化処理は、すべての新KFに対して実行してもよいが、前述したように、新KFにGNSS位置がある場合だけ実行してもよい。たとえば、入力映像の一部画像にのみGNSS位置がある場合に、GNSS位置が無い区間が終わり新たにGNSS位置が得られた時だけ、上述したように、KF更新部522においてGNSS位置のある画像を積極的にKFとして判定し(図12のステップS1204、およびS1205)、得られた当該GNSS位置を必ず用いた実座標環境マップ(KF群の位置・姿勢、と特徴点群の3次元位置)550の補正処理をおこなう(図13のステップS1301)ようにしてもよい。
一方で、新KFにGNSS位置がない時は、従来のV-SLAMと同じように、画像特徴変化による位置・姿勢推定をおこなうことにより、GNSS位置が無い場合の無駄な姿勢グラフによる最適化処理を省き、必要なときだけ処理を実施することが可能となる。同様に、実座標環境マップ550を入力とする場合で、GNSS情報の入力がない場合も、本処理を省略してもよい。
また、必須ではないが、上述したように、入力姿勢情報があり、かつ、KF群に入力姿勢情報があるのであれば、GNSS位置と同様に姿勢情報を姿勢グラフに含めて最適化に用いてもよい。
(ループ検出・クロージング部541の内容)
ループクローズ処理機能540を担当するループ検出・クロージング部541は、従来のV-SLAMと同様に、新KFと保持するKF画像群との画像全体の画像特徴量を比較して類似度を調べて、映像取得時の走行経路で同じ場所を複数回走行していないか(ループが発生していないか)を確認する。そして、類似度が高く同じ場所を走行していると思われた場合には、関係する実座標環境マップ550のKF群情報551の「ループKF ID」に、該当する過去に同場所走行した時のKF群を設定して、互いの参照を可能としておく。
また、ループ検出・クロージング部541は、ループ発生時の新KFに対し、新KF近傍のKF群を用いたローカルBA、または、全KF群を用いたグローバルBAのいずれかを実施して、同場所走行時のKFの位置関係を調整する。新KF近傍のKF群は、マップ特徴点の共有状態などから選定してもよく、過去に同場所走行した時のKF群との共有状態を利用してもよい。
ループ検出・クロージング部541は、具体的には、たとえば、図6に示した、メモリ602に記憶されたプログラムをCPU601が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図7に示した、メモリ702に記憶されたプログラムをCPU701が実行することによって、その機能を実現するようにしてもよい。
なお、本システム500においては、上述した各処理を通して、入力GNSS情報1002を用いてスケールドリフトが起きないように、実座標環境マップ550を構築済である。したがって、スケールドリフト対策が主体のループ検出・クロージング部541における処理は省略してもよい。
以上説明したように、本実施の形態によれば、撮影された時系列画像(たとえば映像1001)のうちの任意の画像について、任意の画像のGNSS情報(第1の撮影位置)1002を測位により取得し、任意の画像の特徴から、当該任意の画像の第2の撮影位置を推定する。そして、第1の撮影位置と、第2の撮影位置とのずれを最小化する拘束条件を用いて、任意の画像の撮影位置、任意の画像の撮影姿勢、および、任意の画像の中で抽出された特徴点の推定された3次元位置、の少なくともいずれか一つを調整する。
このように、単カメラ映像とともに取得したGNSS情報を使って、緯度経度の実座標で、該撮影カメラの位置・姿勢を推定し、同時に実座標で環境マップを作成するV-SLAM機能を持つ。この結果、V-SLAMの算出処理の一部修正という低い処理コストで、従来のV-SLAMの課題であった、スケールドリフトの無い実座標環境マップと、映像撮影位置・姿勢を推定することができる。これにより、高精度な位置推定をおこなうことができる。
特に、本実施の形態の初期化処理は、従来のV-SLAMの初期化のように初期化に用いた画像依存のローカル座標系ではなく、常に同じ緯度経度由来の実座標系を用いることができるため、映像の途中でトラッキングが失敗した場合でも、必要であれば、再度初期化を実施して、処理を続行することができる。この結果、従来のV-SLAMのように、トラッキングが失敗するごとに細切れになった、まったくスケールの異なる座標系で作成された複数の環境マップや、カメラ撮影位置・姿勢データ群を作成することがない。
すなわち、本実施の形態による処理をおこなうことにより、映像全体で同じ実座標スケールの1つの長い環境マップと、カメラ撮影位置・姿勢データを作成することができる(ただし、失敗箇所だけマップ特徴点がやや疎となったり、位置・姿勢推定データが抜ける可能性はある)。
この長い環境マップは、他の映像を用いたV-SLAM処理の入力として使用することができるので、ある映像では移動体で周辺建物が映らずトラッキング失敗していた場所があっても、その失敗箇所を含んだ環境マップを移動体による隠れがない他の映像とともに入力して、本実施の形態によるV-SLAM処理を実行することで、トラッキング失敗箇所の特徴点を新しい映像の特徴点で補うことができ、より精度の高い環境マップを構築することができる。
特に、走行量の多い道路など、移動体などによる隠れが頻繁に発生する場所であっても、撮影した映像中での隠れを気にすることなく、本実施の形態によって複数映像で欠落箇所を補い合うことができ、環境マップを高精度に作成することができる。
また、本実施の形態によれば、入力GNSS情報に合わせて、実座標環境マップを作成することができるため、一度だけ測量車のような精度の高いGNSS情報と映像を同時取得できる車両で取得したデータを用いて、実座標環境マップを作成すれば、当該実座標環境マップを映像しか取得できない任意の一般車の映像に入力適用することで、任意の車の映像の撮影位置・姿勢を、マップ作成時の測量車と同じ精度かつ同じ実座標系で、マップ作成処理をおこなわない低処理コストで推定することができる。この結果、一般車映像を使ったサービスを、GNSS位置の測定記録装置の有無に関わらず実現できるため、より多くの車両データを簡単に収集活用することが可能となる。
さらに、本実施の形態によれば、撮影位置だけを後で線形的にスケール補正するのではなく、独自のKF選定処理および最適化処理によって、GNSS情報(GNSS位置との位置誤差)と、画像解析による被写体の見えの正しさ(再投影誤差など)の双方を使って、補正範囲を自動調整しながら、撮影位置に加えて姿勢をも補正することができる。この結果、右左折などの従来のV-SLAMでは大きくスケールが変わる場所でも、同じ実座標(同じスケール)による高精度な撮影位置・姿勢推定が可能となる。
また、この補正時のGNSS情報の重みを適宜変えることもできるので、GNSS情報の信頼度によっては、画像解析による被写体の見えの正しさを優先するような、フレキシブルな最適化をおこなうことも可能である。たとえば、高精度GNSS情報を取得できる測量車のデータに対しては、よりGNSS情報の重みを重視した最適化とし、一般車のGPS情報のような低精度GNSS情報を含むデータに対しては、GNSS情報の重みをやや落として画像解析結果を重視する最適化とすることができる。この結果、GNSS情報の信頼度を勘案した撮影位置・姿勢推定、および実座標環境マップ作成が可能となる。
なお、本実施の形態で説明した移動***置推定方法は、あらかじめ用意されたプログラムをパーソナルコンピュータやワークステーションなどのコンピュータで実行することにより実現することができる。プログラム配信プログラムは、ハードディスク、フレキシブルディスク、CD(Compact Disc)-ROM、MO(Magneto-Optical Disk)、DVD(Digital Versatile Disk)、USB(Universal Serial Bus)メモリなどのコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。また、移動***置推定プログラムは、インターネットなどのネットワークを介して配布してもよい。
上述した実施の形態に関し、さらに以下の付記を開示する。
(付記1)撮影された時系列画像のうちの任意の画像について、当該任意の画像の第1の撮影位置を測位により取得し、
前記任意の画像の特徴から、当該任意の画像の第2の撮影位置を推定し、
前記第1の撮影位置と、前記第2の撮影位置とのずれを最小化する拘束条件を用いて、前記任意の画像の撮影位置、前記任意の画像の撮影姿勢、および、前記任意の画像の中で抽出された特徴点の推定された3次元位置、の少なくともいずれか一つを調整する、
情報処理装置を有することを特徴とする移動***置推定システム。
(付記2)前記情報処理装置は、
前記時系列画像の各画像について、相互に区別可能な特徴量を持つ画像特徴点を複数抽出し、
前記時系列画像の系列中で近傍となる複数の画像相互間で、特徴量が類似する前記画像特徴点どうしを対応付け、
対応付けられた各特徴点の各前記画像上での2次元位置を用いて、前記第2の撮影位置を推定する、
ことを特徴とする付記1に記載の移動***置推定システム。
(付記3)前記情報処理装置は、
最初に推定する少なくとも2つの画像の前記第2の撮影位置を、前記第1の撮影位置と一致させるように決定する、
ことを特徴とする付記1または2に記載の移動***置推定システム。
(付記4)前記時系列画像は、移動体の撮像手段によって撮影されたものであることを特徴とする付記1~3のいずれか一つに記載の移動***置推定システム。
(付記5)前記第1の撮影位置は、GNSS情報にかかる前記移動体の位置であることを特徴とする付記4に記載の移動***置推定システム。
(付記6)前記情報処理装置は、
前記任意の画像からの時間的な近さ、距離的な近さ、および、前記任意の画像と共通する特徴点の有無、の少なくともいずれかに基づいて、前記時系列画像の中から、前記任意の画像とともに、前記拘束条件を用いた調整の対象となる画像群を決定することを特徴とする付記1~5のいずれか一つに記載の移動***置推定システム。
(付記7)前記任意の画像から時間的または距離的に近く、かつ、当該任意の画像と共通する特徴点がある画像は、前記調整の対象となる画像群とすることを特徴とする付記6に記載の移動***置推定システム。
(付記8)前記任意の画像から時間的または距離的に近くない画像は、前記調整の対象となる画像群としないことを特徴とする付記6に記載の移動***置推定システム。
(付記9)前記任意の画像と共通する特徴点がない画像は、前記調整の対象となる画像群としないことを特徴とする付記6に記載の移動***置推定システム。
(付記10)前記任意の画像からの時間的または距離的に近く、かつ、当該任意の画像と共通する特徴点がない画像は、前記調整の対象となる画像群とせず、前記調整の際に参照する画像群とすることを特徴とする付記6に記載の移動***置推定システム。
(付記11)情報処理装置が、
撮影された時系列画像のうちの任意の画像について、当該任意の画像の第1の撮影位置を測位により取得し、
前記任意の画像の特徴から、当該任意の画像の第2の撮影位置を推定し、
前記第1の撮影位置と、前記第2の撮影位置とのずれを最小化する拘束条件を用いて、前記任意の画像の撮影位置、前記任意の画像の撮影姿勢、および、前記任意の画像の中で抽出された特徴点の推定された3次元位置、の少なくともいずれか一つを調整する、
ことを特徴とする移動***置推定方法。