JP7540909B2 - 情報処理装置、情報処理方法及びプログラム - Google Patents

Description

本発明は、情報処理装置、情報処理方法、プログラム及びデータ構造に関する。

ＳＬＡＭ（Simultaneous Localization and Mapping）と呼ばれる、画像やセンサー情報等から自己位置の推定と環境地図の作成を同時に行う技術が知られている。例えば特許文献１には、ＳＬＡＭによる自己位置推定と環境地図（三次元点群データ）に、慣性計測装置（IMU : Inertial Measurement Unit）を用いて実寸法を付与する技術が開示されている。

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

ＶＩＳＵＡＬ、ＤＩＲＥＣＴ、ＬＩＤＡＲ、ＧＲＡＰＨなど様々なＳＬＡＭにより得られた環境地図には、点群等のデータが大量に含まれている。また、環境地図が広範囲に渡ると、環境地図のデータ量も更に膨大になることから、環境地図を利用して処理を行う装置の処理負荷も増加し、環境地図に含まれる位置情報精度も低下することになる。

そこで、本発明は、環境地図を利用して処理を行う装置の位置情報処理負荷の軽減を可能とする技術を提供することを目的とする。

本発明の一態様に係る情報処理装置は、２次元平面を複数に区切ることで定められる区画を識別する区画識別子と各区画の範囲とを対応づける第１情報と、区画識別子と環境地図とを対応づける第２情報であって、前記環境地図はキーフレームにおける特徴量とキーフレームに対応づけられる３次元空間内の点の座標とキーフレームにおけるセンサー姿勢とを含む、第２情報とを記憶する記憶部と、情報処理装置から、３次元空間をセンシングした位置を受信する受信部と、前記第１情報と前記第２情報とに基づいて、前記３次元空間をセンシングした位置を含む区画に対応づけられる環境地図を前記記憶部から抽出して前記情報処理装置に送信する。

本発明によれば、環境地図を利用して処理を行う装置の位置情報処理負荷の軽減を可能とする技術を提供することができる。

本実施形態に係る情報処理システムの一例を示す図である。 地図生成装置及び方向特定装置のハードウェア構成例を示す図である。 地図サーバのハードウェア構成例を示す図である。 地図生成装置の機能ブロック構成例を示す図である。 地図サーバの機能ブロック構成例を示す図である。 方向特定装置の機能ブロック構成例を示す図である。 地図生成装置で生成された環境地図をメッシュに対応づけて格納する際の処理手順の一例を示す図である。 環境地図を作成する範囲の一例を示す図である。 ３次元特徴点及びキーフレームの一例を示す図である。 メッシュ環境地図ＤＢのデータ構造の一例を示す図である。 メッシュ環境地図ＤＢのデータ構造の他の例を示す図である。 方向特定装置がセンシング方向を推定する際の処理手順の一例を示す図である。

添付図面を参照して、本発明の実施形態について説明する。なお、各図において、同一の符号を付したものは、同一又は同様の構成を有する。

＜システム構成＞
図１は、本実施形態に係る情報処理システムの一例を示す図である。情報処理システムは、地図生成装置１０と、地図サーバ２０と、方向特定装置３０とを含む。地図生成装置１０と、地図サーバ２０と、方向特定装置３０とは、無線又は有線の通信ネットワークＮを介して接続され、相互に通信を行うことができる。地図生成装置１０及び方向特定装置３０は複数存在していてもよい。

地図生成装置１０（環境地図生成装置）は、車両に搭載されており、カメラやＬＩＤＡＲ（Light Detection and Ranging）で地図生成装置１０の周囲をセンシングしながら、自己位置推定と環境地図の作成を行う。また、地図生成装置１０は、作成した環境地図を地図サーバ２０に送信する。なお、本実施形態では、地図生成装置１０は、特徴点を用いて環境地図を生成するＳＬＡＭ技術を利用することで、自己位置推定と環境地図の作成を行う例を示している。特徴点を用いて環境地図を生成するＳＬＡＭ技術には、例えば、ＰＴＡＭ（Parallel Tracking and Mapping）、ＯＲＢ（Oriented FAST and Rotated BRIEF）－ＳＬＡＭ、ＯＲＢ－ＳＬＡＭ２等の技術が存在する。環境地図は、地図生成装置１０が所定範囲の３次元空間をセンシングすることで得られるセンシング結果データ（以下、「センシング結果」と言う。）の間で対応づけが可能な特徴点（対応点）について、ＳｆＭ（Structure from Motion）技術を用いることで推定された、当該対応点を３次元空間にマッピングした点の座標を、ｘ、ｙ及びｚ方向の３次元座標で表現したデータである。例えば、カメラで３次元空間をセンシング（撮影）する場合、センシング結果は画像データである。ＬＩＤＡＲで３次元空間をセンシングする場合、センシング結果は、所定範囲をスキャンすることで得られるデータである。

また、環境地図には、３次元特徴点の座標を示す情報に加えて、キーフレームと呼ばれる情報も格納されている。キーフレームは、所定間隔で連続してセンシングされたセンシング結果の中から、所定のアルゴリズムにより選択されたセンシング結果である。キーフレームには、自己位置推定（位置追跡（トラッキング））により推定されたセンサー姿勢（位置及びセンシング方向）、センシング結果上の特徴点に関する情報（特徴量等）等が対応づけられて環境地図に格納されている。キーフレームは、主に、自己位置推定に失敗した場合に自己位置やセンサー姿勢を再認識する処理（Relocalization）や、ループクローズ（Loop Closing）補正をする際等に用いられる。以下、本実施形態では、センシング結果上の特徴点のうちセンシング結果の間で対応づけが可能な特徴点（対応点）を３次元空間にマッピングした点を「３次元特徴点」と称する。

地図サーバ２０は、地図生成装置１０から受信した環境地図を記憶するサーバである。地図サーバ２０は、環境地図をそのまま記憶するのではなく、緯度及び経度で位置を特定可能な２次元平面を複数に区切ることで定められる区画（以下、「メッシュ」と言う。）に環境地図を対応付けて格納する。緯度及び経度で位置を特定可能な２次元平面とは、例えば、道路地図等で表される２次元平面であってもよい。各々のメッシュの範囲は、緯度及び経度で位置を特定可能であれば、緯度及び経度をそのまま用いて表現されていてもよいし、緯度及び経度と相互に変換可能な他の座標系で表現されていてもよい。

本実施形態では、環境地図をメッシュに対応づけることを可能とするため、地図生成装置１０は、各キーフレームに、センシングした際の現在位置を示す情報（緯度及び経度）を対応づけておく。地図サーバ２０は、キーフレームに対応づけられている位置情報（緯度及び経度）に基づいて、環境地図をメッシュに対応づけて記憶する。

方向特定装置３０は、センサーと、自身の位置（緯度及び経度）を検出する位置検出装置とを備えている。また、方向特定装置３０は、３次元空間をセンシングしたセンシング結果に基づいて、当該をセンサーが３次元空間をセンシングした方向（センシング結果がセンシングされた方向）を特定する。方向特定装置３０は、例えば自動走行車両に搭載され、センシング結果に基づいて車両の向きを特定するといった用途に用いられる。しかしながら、方向特定装置３０はこれに限定されず、ドローン等の飛行体に搭載され、飛行中に自身の方向を特定する等、他の用途に用いられてもよい。

ここで、方向特定装置３０は、地図生成装置１０が環境地図を作成する際に用いたＳＬＡＭ技術を利用することで、３次元空間をセンシングした方向を検出する。具体的には、方向特定装置３０は、３次元空間をセンシングした位置（緯度及び経度）を地図サーバ２０に送信することで、当該位置から特定されるメッシュに対応づけられている環境地図をダウンロードする。続いて、方向特定装置３０は、センシングしたセンシング結果と、ダウンロードした環境地図とを用いて、前述したＳＬＡＭ技術において自己位置を再認識する処理（Relocalization）を利用することで、センサー姿勢を推定する。推定されたセンサー姿勢は、３次元空間をセンシングした方向に相当する。

地図生成装置１０及び方向特定装置３０は、ＧＮＳＳ（Global Navigation Satellite System）等を用いて現在位置を取得可能な位置検出装置を備える情報処理装置であれば、どのような情報処理装置で構成されていてもよい。また、地図生成装置１０と方向特定装置３０は、地図生成装置１０と方向特定装置３０の両方の機能を備えた同一の情報処理装置で実現されていてもよいし、異なる情報処理装置であってもよい。

地図サーバ２０は、１又は複数の物理的なサーバ、ハイパーバイザー（hypervisor）上で動作する仮想的なサーバ又はクラウドサーバ等の情報処理装置を用いて構成されていてもよい。

＜ハードウェア構成＞
（地図生成装置、方向特定装置）
図２は、地図生成装置１０及び方向特定装置３０のハードウェア構成例を示す図である。地図生成装置１０及び方向特定装置３０は、ＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphical processing unit）等のプロセッサ１１、メモリ、ＨＤＤ（Hard Disk Drive）及び／又はＳＳＤ（Solid State Drive）等の記憶装置１２、有線又は無線通信を行う通信ＩＦ（Interface）１３、入力操作を受け付ける入力装置１４、ディスプレイ等の出力装置１５、現在位置を検出する位置検出装置１６、３次元空間のセンシングを行うセンサー装置１７（カメラやＬＩＤＡＲ）を有する。地図生成装置１０が備える位置検出装置１６は、ＲＴＫ－ＧＮＳＳ（Global Navigation Satellite System）等に対応した、例えば数センチメートルの単位で高精度に現在位置（緯度、経度及び標高）を検出可能な装置であってもよい。一方、方向特定装置３０が備える位置検出装置１６は、ＲＴＫ－ＧＮＳＳのような高精度に現在位置を検出可能な装置であってもよいし、通常のＧＮＳＳのように、数メートル程度の誤差は許容されるような装置であってもよい。

図３は、地図サーバ２０のハードウェア構成例を示す図である。地図サーバ２０は、ＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphical processing unit）等のプロセッサ１１、メモリ、ＨＤＤ（Hard Disk Drive）及び／又はＳＳＤ（Solid State Drive）等の記憶装置１２、有線又は無線通信を行う通信ＩＦ（Interface）１３、入力操作を受け付ける入力装置１４、及び情報の出力を行う出力装置１５を有する。入力装置１４は、例えば、キーボード、タッチパネル、マウス及び／又はマイク等である。出力装置１５は、例えば、ディスプレイ、タッチパネル及び／又はスピーカ等である。

＜機能ブロック構成＞
（地図生成装置）
図４は、地図生成装置１０の機能ブロック構成例を示す図である。地図生成装置１０は、記憶部１００と、センサー部１０１と、検出部１０２と、ＳＬＡＭ処理部１０３と、送信部１０４とを含む。記憶部１００は、地図サーバ２０が備える記憶装置１２を用いて実現することができる。また、センサー部１０１と、検出部１０２と、ＳＬＡＭ処理部１０３と、送信部１０４とは、地図サーバ２０のプロセッサ１１が、記憶装置１２に記憶されたプログラムを実行することにより実現することができる。また、当該プログラムは、記憶媒体に格納することができる。当該プログラムを格納した記憶媒体は、コンピュータ読み取り可能な非一時的な記憶媒体（Non-transitory computer readable medium）であってもよい。非一時的な記憶媒体は特に限定されないが、例えば、ＵＳＢメモリ又はＣＤ－ＲＯＭ等の記憶媒体であってもよい。

記憶部１００は、ＳＬＡＭ処理部１０３で生成される環境地図を格納するＤＢ（データベース）を記憶する。

センサー部１０１は、センサー装置１７を用いて、地図生成装置１０の周辺の３次元空間をセンシングすることで、センシング結果を生成する。センシングする方向は１方向に限定されない。例えば、複数のセンサー装置１７を用いて、前方向、左前方向及び右前方向など複数の方向を同時にセンシングすることとしてもよい。

検出部１０２は、位置検出装置１６を用いて、センサー部１０１で３次元空間をセンシングした際の現在位置（緯度及び経度）を検出する。

ＳＬＡＭ処理部１０３は、センサー部１０１でセンシングされたセンシング結果を用いて、ＳＬＡＭ技術を用いて自己位置推定と環境地図の作成を行う。例えば、ＳＬＡＭ処理部１０３は、センサー部１０１でセンシングされたセンシング結果を分析することで、１つ前のセンシング結果と現在のセンシング結果との間で、センサー姿勢がどの程度変化したのかを推定する処理（位置追跡）と、１つ前のセンシング結果と現在のセンシング結果に共通する特徴点（対応点）から、３次元特徴点の座標を推定する処理（環境地図作成）とを、繰り返し行う。また、ＳＬＡＭ処理部１０３は、適宜キーフレームの選択を行い、キーフレームに関する情報を環境地図に格納していく。

また、ＳＬＡＭ処理部１０３は、推定した自己位置及びセンサー姿勢の誤差が蓄積することを抑制するために、バンドル調整を行うことで、誤差を最小化させる。また、移動経路が閉じたことを検出すると、それまでに推定した自己位置及びセンサー姿勢の誤差を調整する（ループクローズ処理）。なお、自己位置推定、環境地図作成及び誤差調整等の方法は、ＳＬＡＭ技術により様々であるため、上記の方法に限定されるものではない。

ＳＬＡＭ処理部１０３により作成された環境地図には、３次元特徴点に関する情報や、キーフレームに対応づけられる（キーフレームから観測される）各３次元特徴点の座標等を含む各キーフレームに関する情報等など、様々な情報が含まれる。

例えば、ＯＲＢ－ＳＬＡＭは、ＯＲＢにより特徴点の検出と特徴量の記述を行い、自己位置とセンサー姿勢の推定を行う。特徴点は、ＦＡＳＴ（Features from Accelerated Segment Test）技術を用いることで、注目画素とその周囲の画素の輝度差を用いて検出される。ＯＲＢ－ＳＬＡＭにより作成される環境地図には、３次元特徴点ごとの情報として、３次元特徴点の座標と、３次元特徴点の世界座標と視線方向（キーフレームの中心から３次元特徴点の世界座標に向かう方向）を示すベクトルと、３次元特徴点がどのキーフレームから観測されるのかを示す情報等が含まれる。また、キーフレームごとの情報として、センサー姿勢（座標及びセンシング方向）と、センサーとなるカメラ内部パラメータと、キーフレーム上の特徴点群の特徴量とが含まれる。

ここで、ＳＬＡＭ処理部１０３は、各キーフレームに関する情報に、検出部１０２で検出された、各キーフレームのセンシング位置を示す情報を含めることで、環境地図を生成する。各キーフレームのセンシング位置を示す情報は、例えば、ＲＴＫ－ＧＮＳＳ等を用いることで高精度に検出された、緯度、経度及び標高で表される位置情報である。

送信部１０４は、ＳＬＡＭ処理部１０３で作成された環境地図を、地図サーバ２０に送信する。

（地図サーバ）
図５は、地図サーバ２０の機能ブロック構成例を示す図である。地図サーバ２０は、記憶部２００と、取得部２０１と、格納部２０２と、受信部２０３と、送信部２０４とを含む。記憶部２００は、地図サーバ２０が備える記憶装置１２を用いて実現することができる。また、取得部２０１と、格納部２０２と、受信部２０３と、送信部２０４とは、地図サーバ２０のプロセッサ１１が、記憶装置１２に記憶されたプログラムを実行することにより実現することができる。また、当該プログラムは、記憶媒体に格納することができる。当該プログラムを格納した記憶媒体は、コンピュータ読み取り可能な非一時的な記憶媒体であってもよい。非一時的な記憶媒体は特に限定されないが、例えば、ＵＳＢメモリ又はＣＤ－ＲＯＭ等の記憶媒体であってもよい。

記憶部２００は、メッシュ環境地図ＤＢを記憶する。メッシュ環境地図ＤＢには、２次元平面を複数に区切ることで定められるメッシュを識別するメッシュ識別子（区画識別子）と各メッシュの範囲を示す情報とが対応づけられて格納されるメッシュ情報（第１情報）２００ａと、メッシュ識別子と環境地図とを対応づけるメッシュ単位環境地図（第２情報）２００ｂとが含まれる。また、メッシュ単位環境地図２００ｂは、各キーフレーム上の特徴点群の特徴量（キーフレームにおける特徴量）を示す情報と、各キーフレームに対応づけられる３次元特徴点の座標（３次元空間内の点の座標）を示す情報と各キーフレームにおけるセンサー姿勢を示す情報とを含む。

取得部２０１は、地図生成装置１０から、地図生成装置１０で生成された、各キーフレームに関する情報を含む環境地図を取得する。各キーフレームに関する情報には、各キーフレーム上の特徴点群の特徴量（キーフレームにおける特徴量）を示す情報と、各キーフレームに対応づけられる３次元特徴点の座標（３次元空間内の点の座標）を示す情報と、各キーフレームにおけるセンサー姿勢を示す情報と、各キーフレームのセンシング位置を示す情報（緯度、経度及び標高で表される位置情報）とが含まれている。

格納部２０２は、各キーフレームのセンシング位置を示す情報に基づいて、取得部２０１で取得した環境地図を、複数のメッシュのうちいずれかのメッシュに対応づけることで、メッシュ単位環境地図２００ｂを生成して記憶部２００に格納する。

受信部２０３は、方向特定装置３０から、３次元空間をセンシングした位置（緯度及び経度）を受信する。

送信部２０４は、受信部２０３で受信した、３次元空間をセンシングした位置を含むメッシュに対応づけられる環境地図を、メッシュ単位環境地図２００ｂから抽出して方向特定装置３０に送信する。

（方向特定装置）
図６は、方向特定装置３０の機能ブロック構成例を示す図である。方向特定装置３０は、記憶部３００と、センサー部３０１と、検出部３０２と、取得部３０３と、方向推定部３０４とを含む。記憶部３００は、方向特定装置３０が備える記憶装置１２を用いて実現することができる。また、センサー部３０１と、検出部３０２と、取得部３０３と、方向推定部３０４とは、地図サーバ２０のプロセッサ１１が、記憶装置１２に記憶されたプログラムを実行することにより実現することができる。また、当該プログラムは、記憶媒体に格納することができる。当該プログラムを格納した記憶媒体は、コンピュータ読み取り可能な非一時的な記憶媒体であってもよい。非一時的な記憶媒体は特に限定されないが、例えば、ＵＳＢメモリ又はＣＤ－ＲＯＭ等の記憶媒体であってもよい。

記憶部３００は、取得部３０３が地図サーバ２０から取得した、キーフレームに関する情報を記憶する。

センサー部３０１は、センサー装置１７を用いて、方向特定装置３０の周辺の３次元空間をセンシングする。

検出部３０２は、位置検出装置１６を用いて、センサー部３０１が３次元空間をセンシングした位置（緯度及び経度の座標）を検出する。

取得部３０３は、地図サーバ２０にアクセスすることで、検出部３０２で検出された位置を含むメッシュに対応する環境地図を地図サーバ２０から取得し、記憶部３００に格納する。

方向推定部３０４は、センサー部３０１でセンシングしたセンシング結果と、地図サーバ２０から取得した環境地図とを用いて、ＳＬＡＭ技術において自己位置を再認識する処理（Relocalization）を利用することで、センシング結果をセンシングした方向（センサー部３０１が３次元空間をセンシングした方向）を推定する。

＜処理手順＞
続いて、本実施形態に係る情報システムが行う処理手順について具体的に説明する。本実施形態において、環境地図を作成する範囲やメッシュの大きさは特に限定されないが、以下の説明では、地図生成装置１０が道路上を走行することで、ある程度広い範囲（例えば１つのメッシュが１～数ｋｍ四方といった単位）の環境地図を生成することを想定している。

（環境地図の格納）
図７は、地図生成装置で生成された環境地図をメッシュに対応づけて格納する際の処理手順の一例を示す図である。

ステップＳ１００で、地図生成装置１０のセンサー部１０１は、道路を走行しながら、地図生成装置１０の周囲の３次元空間をセンシングする。また、検出部１０２は、３次元空間をセンシングした位置を記録していく。

ステップＳ１０１で、地図生成装置１０のＳＬＡＭ処理部１０３は、センサー部１０１でセンシングされた各センシング結果を分析することで自己位置推定を行う。また、ＳＬＡＭ処理部１０３は、特定された各３次元特徴点に関する情報（３次元特徴点の座標等）を環境地図に格納すると共に、各フレームのうちキーフレームとして記録するフレームを選択し、選択したフレーム上の特徴点群の特徴量、フレームに対応づけられる３次元特徴点の座標及びフレームにおけるセンサー姿勢等を、キーフレームに関する情報として環境地図に格納する。また、ＳＬＡＭ処理部１０３は、各キーフレームに関する情報に、各キーフレームのセンシング位置を示す情報（検出部１０２で検出した緯度及び経度）を含めて環境地図に格納する。

図８は、環境地図を作成する範囲の一例を示す図である。図８のうち、Ｎ１０～Ｎ１４は、ノード（道路が交わる箇所）を示しており、Ｌ１０１～Ｌ１０８は、道路リンクを示している。例えば、地図生成装置１０は、ノードＮ１１、ノードＮ１５、ノードＮ１２及びノードＮ１１の順に反時計回りに走行したとする。この場合、地図生成装置１０は、ノードＮ１１からノードＮ１５及びノードＮ１２を通ってノードＮ１１に戻るまでの道路の周囲をセンシングしたセンシング結果を用いて、ノードＮ１１からノードＮ１５及びノードＮ１２を通ってノードＮ１１に戻るまでの道路の周囲についての環境地図を生成する。

また、地図生成装置１０は、更に、ノードＮ１１からノードＮ１０及びノードＮ１２を通ってノードＮ１１に戻る順に走行したとする。この場合、地図生成装置１０が生成する環境地図には、ノードＮ１１からノードＮ１０及びノードＮ１２を通ってノードＮ１１に戻るまでの道路の周囲についての環境地図が追加される。

図９は、３次元特徴点及びキーフレームの一例を示す図である。図９は、図８のエリアＡ１０を拡大した図である。図９の例では、地図生成装置１０は、道路上を上から下に向かって走行した場合の例を示している。キーフレームＫＦ１～ＫＦ７は、地図生成装置１０の正面に取り付けられたセンサー装置１７でセンシングされたセンシング結果に基づいて生成されたキーフレームを示している。キーフレームＫＦ１１～ＫＦ１６は、地図生成装置１０の右側面に取り付けられたセンサー装置１７でセンシングされたセンシング結果に基づいて生成されたキーフレームを示している。キーフレームＫ２１～ＫＦ２７は、地図生成装置１０の左側面に取り付けられたセンサー装置１７でセンシングされたセンシング結果に基づいて生成されたキーフレームを示している。図９の道路の周囲に存在する黒点は、検出された３次元特徴点を示している。

なお、図９はあくまで一例であり、地図生成装置１０には、更に多くのセンサー装置１７（前方からみて右４５度や左４５度など）が取り付けられていてもよい。その場合、更に多くのキーフレームが生成されることになる。若しくは、地図生成装置１０には、右側面及び左側面にセンサー装置１７が取り付けられていなくてもよい。その場合、キーフレームＫＦ１１～ＫＦ１６及びキーフレームＫ２１～ＫＦ２７は生成されないことになる。また、地図生成装置１０は、同一の道路を、センサー装置１７の取り付け方向を変えて走行するようにしてもよい。例えば、１週目はセンサー装置１７を正面に取り付けてセンシングし、２週目はセンサー装置１７を右方向に取り付けてセンシングし、３週目はセンサー装置１７を左方向に取り付けてセンシングするようにしてもよい。センサー装置１７が１つのみであっても、図９に示すような環境地図を作成することができる。図７に戻り説明を続ける。

ステップＳ１０２で、地図生成装置１０の送信部１０４は、生成した環境地図を地図サーバ２０に送信する。

ステップＳ１０３で、地図サーバ２０の格納部２０２は、地図生成装置１０から受信した環境地図に含まれている各キーフレームのセンシング位置を示す情報と、メッシュ情報２００ａとを突合することで、各キーフレームが含まれるメッシュのメッシュ識別子を判定する。続いて、格納部２０２は、各メッシュ識別子に、当該メッシュに含まれると判定されたキーフレームに関する情報を対応づけることで、メッシュ単位環境地図２００ｂを生成して記憶部２００に格納する。前述したように、キーフレームに関する情報には、キーフレーム上の特徴点群の特徴量を示す情報と、キーフレームに対応づけられる３次元特徴点の座標を示す情報と、キーフレームにおけるセンサー姿勢を示す情報とが含まれる。

このとき、格納部２０２は、取得部２０１で取得した環境地図に含まれる各キーフレームに関する情報のうち、各キーフレームのセンシング位置を示す情報（緯度、経度及び標高で表される位置情報）を含めずにメッシュ単位環境地図２００ｂを生成するようにしてもよいし、各キーフレームのセンシング位置を示す情報を含めてメッシュ単位環境地図２００ｂを生成するようにしてもよい。

図１０は、メッシュ環境地図ＤＢのデータ構造の一例を示す図である。図１０（ａ）は、メッシュ情報２００ａの一例を示し、図１０（ｂ）は、メッシュ単位環境地図２００ｂの一例を示す。メッシュ情報２００ａは、メッシュ識別子ごとに、メッシュの範囲を示すデータ（メッシュ範囲データ）が対応づけられて格納される。メッシュ範囲データは、例えば、メッシュの四隅の位置を緯度及び経度で表現したデータであってもよい。

メッシュ単位環境地図２００ｂは、メッシュごとに、各キーフレームに関する情報を含む環境地図が対応づけられているデータ構造を有している。ヘッダー部には、メッシュを一意に特定するメッシュ識別子が格納される。データ部には環境地図のデータが格納される。フッター部にはどのような情報が格納されていてもよいし、フッター部は省略されていてもよい。

まず、格納部２０２は、地図生成装置１０から受信した環境地図に含まれている、各キーフレームのセンシング位置を示す緯度及び経度に基づいて、各キーフレームがどのメッシュ内に存在するのかを判定する処理を、全てのキーフレームについて行う。続いて、格納部２０２は、キーフレームが存在するメッシュを１つ選択し、選択した１つのメッシュ内に存在する複数のキーフレームについてのキーフレームに関する情報を、当該選択した１つのメッシュのデータ部に格納する。格納部２０２は、当該処理を、キーフレームが存在する全てのメッシュについて繰り返し行うことで、メッシュ単位環境地図２００ｂを生成して記憶部２００に格納する。なお、あるメッシュのデータ部に格納されるキーフレームに関する情報に含まれる、キーフレームに対応づけられる３次元特徴点の座標を示す情報について、当該３次元特徴点の座標は、キーフレームと同じメッシュ内に存在するか否かを問わない。本実施形態では、キーフレームがメッシュ内に存在していれば、当該キーフレームに対応づけられる３次元特徴点の座標がメッシュ内に存在するのか否かを問わず（つまりメッシュからはみ出ている３次元特徴点も含めて）、全て、キーフレームに関する情報としてデータ部に格納される。

ここで、メッシュ環境地図を作成するエリアが広い場合、当該エリア内における環境地図の作成を１つの地図生成装置１０が行うと、多くの時間を要すると共に、処理負荷やメモリ消費量も膨大になる可能性がある。従って、当該エリアを複数に区分し、各々の区分について複数の地図生成装置１０が並行して環境地図を作成し、地図サーバ２０で複数の環境地図を統合した方が効率がよいと考えられる。

この場合、地図サーバ２０の取得部２０１は、複数の地図生成装置１０から複数の環境地図を取得する。続いて、格納部２０２は、複数の環境地図の各々に同一の（重複する）キーフレームに関する情報が存在する場合、同一のキーフレームに関する情報を統合する（２つの情報のうち１方を削除する）ことで、複数の環境地図を統合する。続いて、格納部２０２は、統合された複数の環境地図について、統合した後の各キーフレームのセンシング位置を示す情報に基づいて、統合した後の各キーフレームが含まれる区画を判定し、判定された各キーフレームが含まれる区画に、統合された環境地図に含まれる各キーフレームに関する情報を対応づけて記憶部２００に格納する。なお、複数の環境地図の各々に重複するキーフレームが存在するか否かについて、重複するキーフレームとは、キーフレーム上の特徴点の特徴量と、キーフレームに対応づけられる３次元特徴点の座標と、キーフレームにおけるセンサー姿勢とが完全に同一値であるキーフレームに限定されない。例えば、格納部２０２は、キーフレーム上の特徴点の特徴量、キーフレームに対応づけられる複数の３次元特徴点の各々の座標のうち所定数の３次元特徴点の座標、及び、センサー姿勢が所定の範囲内である場合、格納部２０２は、２つのキーフレームは重複すると判断してもよい。

なお、各地図生成装置１０が並行して環境地図を作成すると、世界座標が異なる環境地図が生成される可能性がある。そこで、本実施形態では、地図サーバ２０が環境地図の統合を容易に行えるようにするために、各地図生成装置１０が生成する環境地図の世界座標について、基準点（ｘ、ｙ及びｚが全てゼロになる地点）と、ｘ軸、ｙ軸及びｚ軸が示す方向（例えばｘ軸は北、ｙ軸は東、ｚ軸は標高等）とを揃えるようにしてもよい。基準点と、ｘ軸、ｙ軸及びｚ軸が示す方向を揃える方法はどのような方法でもよいが、例えば、地図生成装置１０が環境地図を生成する前に、ユーザが地図生成装置１０のキャリブレーション等を行うこととしてもよい。

若しくは、地図サーバ２０は、複数の環境地図を統合する前に、各キーフレームのセンシング位置を示す情報に基づいて、各地図生成装置１０から取得した環境地図の世界座標を揃えるようにしてもよい。この場合、地図サーバ２０が、ｚ軸方向についても世界座標を揃えることができるようにするため、各キーフレームのセンシング位置を示す情報には、緯度及び経度に加えて標高も含まれることとしてもよい。

図１１は、メッシュ環境地図ＤＢのデータ構造の他の例を示す図である。メッシュ単位環境地図２００ｂは、更に、２つの情報に分割されていてもよい。例えば、メッシュ識別子と、各メッシュに対応づけられる１以上のキーフレームを識別する識別子（キーフレーム識別子）とが対応づけられて格納される「対応関係情報」（図１１（ａ））と、キーフレーム上の特徴点の特徴量（キーフレームにおける特徴量）を示す情報とキーフレームに対応づけられる３次元特徴点の座標（３次元空間内の点の座標）を示す情報とキーフレームにおけるセンサー姿勢を示す情報とがキーフレームごとに格納される「キーフレーム単位の環境地図」（図１１（ｂ））と、に分割されていてもよい。この場合、「対応関係情報」を第２情報と称し、「キーフレーム単位の環境地図」を第３情報と称してもよい。

また、メッシュは、更に、大きさが異なる複数のメッシュに分けて定義されていてもよい。例えば、メッシュの大きさが小さくなる順に、第１次メッシュ、第２次メッシュ及び第３次メッシュが定義されていてもよい。この場合、メッシュ情報２００ａは、第１次メッシュのメッシュ範囲データ、第２次メッシュのメッシュ範囲データ及び第３次メッシュのメッシュ範囲データに分けて定義されていてもよい。

また、メッシュ単位環境地図２００ｂも、大きさが異なる複数のメッシュに分けて定義されていてもよい。また、データ部には、メッシュの大きさによって、キーフレームの密度が異なる環境地図が格納されるようにしてもよい。例えば、格納部２０２は、大きさが大きいメッシュ（例えば第１次メッシュ）のデータ部については、地図生成装置１０から受信した環境地図に含まれている各キーフレームに関する情報を例えば１／３等に間引いた環境地図を格納するようにしてもよい。また、大きさが中程度メッシュ（例えば第２次メッシュ）のデータ部については、地図生成装置１０から受信した環境地図に含まれている各キーフレームに関する情報を例えば１／２等に間引いた環境地図を格納するようにしてもよい。また、大きさが小さいメッシュ（例えば第３次メッシュ）のデータ部については、地図生成装置１０から受信した環境地図に含まれている各キーフレームに関する情報をそのまま含む環境地図を格納するようにしてもよい。

（センシング方向の特定）
図１２は、方向特定装置３０がセンシング方向を推定する際の処理手順の一例を示す図である。

ステップＳ２００で、方向特定装置３０のセンサー部３０１は、３次元空間をセンシングする。また、検出部３０２は、センサー部３０１が３次元空間をセンシングした際の現在位置（緯度及び経度）を取得する。

ステップＳ２０１で、取得部３０３は、３次元空間をセンシングした際の現在位置（緯度及び経度）を地図サーバ２０に送信する。

ステップＳ２０２で、地図サーバ２０の受信部２０３又は送信部２０４は、記憶部２００に記憶されているメッシュ環境地図ＤＢの中から、ステップＳ２０１で受信した現在位置に対応するメッシュの環境地図（メッシュ単位環境地図２００ｂ）を取得する。

ステップＳ２０３で、地図サーバ２０の送信部２０４は、取得した環境地図を方向特定装置３０に送信する。

ここで、記憶部２００に記憶されているメッシュ単位環境地図２００ｂが、図１１に示すように、「キーフレーム単位の環境地図」と「対応関係情報」とに分割されている場合、受信部２０３又は送信部２０４は、メッシュ情報２００ａにアクセスすることで、ステップＳ２０１で受信した現在位置に対応するメッシュ識別子を取得する。続いて、受信部２０３又は送信部２０４は、対応関係情報にアクセスすることで、メッシュ識別子に対応する複数のキーフレーム識別子を取得する。続いて、受信部２０３又は送信部２０４は、キーフレーム単位の環境地図にアクセスすることで、複数のキーフレーム識別子の各々に対応するキーフレームに関する情報を取得する（Ｓ２０２）。続いて、送信部２０４は、複数のキーフレーム識別子の各々に対応するキーフレームに関する情報をマージした情報を、環境地図として方向特定装置３０に送信する（Ｓ２０３）。

ステップＳ２０４で、方向特定装置３０の取得部３０３は、地図サーバ２０から取得した環境地図を記憶部３００に格納する。続いて、方向推定部３０４は、センサー部３０１でセンシングしたセンシング結果と、地図サーバ２０から受信した環境地図とを用いて、ＳＬＡＭ技術において自己位置を再認識する処理（Relocalization）を利用することで、センサー姿勢を推定する。

まず、方向推定部３０４は、取得した環境地図を参照し、センシング結果上の特徴点の特徴量（センシング結果の特徴量）と、各キーフレーム上の特徴点の特徴量（各キーフレームにおける特徴量）とを比較することで、センシング結果に対応するキーフレームを選択する。

ここで、取得部３０３が取得する環境地図に含まれるキーフレームに関する情報には、各キーフレームのセンシング位置を示す情報が含まれていてもよい。この場合、方向推定部３０４は、環境地図に含まれる各キーフレームがセンシングされた位置に基づいて、センシング結果をセンシングした際の現在位置（センシング結果をセンシングした位置）に最も近いキーフレームを選択するようにしてもよい。

続いて、方向推定部３０４は、選択したキーフレームに対応づけられる３次元特徴点（３次元空間内の点）を該キーフレーム上に投影した点のキーフレーム上の位置と、当該投影した点に対応する、センシング結果上の特徴点のセンシング結果上の位置と、該キーフレームにおけるセンサー姿勢とに基づいて、センシング結果がセンシングされた方向を推定する。例えば、方向推定部３０４は、選択したキーフレームに対応づけられる３次元特徴点を該キーフレーム上に投影した点のキーフレーム上の位置と、当該投影した点に対応する、センシング結果上の特徴点（すなわち対応点）の当該点群上の位置とに基づいて、ＳｆＭ技術等を利用することで、キーフレームからセンシング結果に至るセンサー姿勢の変化量を推定する。方向推定部３０４は、センシング結果上の特徴点（対応点）をどのような方法で検出するようにしてもよいが、例えば、キーフレーム上に投影した点の周囲の領域（キーフレーム上の領域）の特徴量と、センシング結果上の各特徴点の特徴量とのマッチングを行うことで検出するようにしてもよい。キーフレーム上に投影した点の周囲の領域の特徴量は、キーフレーム上の特徴点の特徴量としてキーフレームに関する情報に含まれていてもよい。

続いて、方向推定部３０４は、キーフレームにおけるセンサー姿勢に当該センサー姿勢の変化量を加えることで、センシング結果におけるセンサー姿勢を推定する。

例えば、ＯＲＢ－ＳＬＡＭでは、センシング結果上の特徴点の特徴量と類似する特徴量を有するキーフレームの候補を、環境地図の中から検索し、検索されたキーフレームの候補とセンシング結果とについてＰｎＰアルゴリズムを用いることでセンサー姿勢を推定する。

＜まとめ＞
以上説明した実施形態によれば、方向特定装置３０が環境地図を用いてセンシング方向を特定する際、膨大なデータ量の環境地図を全てダウンロードするのではなく、センシングした位置におけるメッシュに対応する環境地図をダウンロードするようにした。これにより、環境地図を利用して処理を行う方向特定装置３０の位置情報処理負荷を軽減することが可能になる。また、本実施形態によれば、広大な範囲の環境地図を一度に作成する必要がないことから、環境地図における取得位置精度維持向上が可能になる。

以上説明した実施形態は、本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。実施形態で説明したフローチャート、シーケンス、実施形態が備える各要素並びにその配置、材料、条件、形状及びサイズ等は、例示したものに限定されるわけではなく適宜変更することができる。また、異なる実施形態で示した構成同士を部分的に置換し又は組み合わせることが可能である。

例えば、メッシュは２次元平面に限定されず、高さ方向を含む３次元空間で表現されるようにしてもよい。この場合、メッシュ単位環境地図２００ｂにも、３次元空間で表現されるメッシュごとに、環境地図が格納される。例えば、方向特定装置３０がドローンであるなど、高さ方向にも移動する場合に、方向特定装置３０がダウンロードする環境地図のデータ量を削減することが可能になる。

また、以上説明した実施形態において、特徴点や特徴量は、ＶＩＳＵＡＬ－ＳＬＡＭのように輝度差に基づいて求められるものであってもよいし、ＤＩＲＥＣＴ－ＳＬＡＭのように画素値に基づいて求められるものであってもよい。また、自己位置推定は、ＧＲＡＰＨ－ＳＬＡＭのように、最適化モデルを探索することで行われることとしてもよい。

１０…地図生成装置、１１…プロセッサ、１２…記憶装置、１３…通信ＩＦ、１４…入力装置、１５…出力装置、１６…位置検出装置、１７…センサー装置、２０…地図サーバ、３０…方向特定装置、１００…記憶部、１０１…センサー部、１０２…検出部、１０３…ＳＬＡＭ処理部、１０４…送信部、２００…記憶部、２０１…取得部、２０２…格納部、２０３…受信部、２０４…送信部、３００…記憶部、３０１…センサー部、３０２…検出部、３０３…取得部、３０４…方向推定部

Claims (9)

1. ２次元平面を複数に区切ることで定められる区画を識別する区画識別子と各区画の範囲とを対応づける第１情報と、区画識別子と環境地図とを対応づける第２情報であって、該環境地図はキーフレーム上の点群に関する情報とキーフレームに対応づけられる３次元空間内の点の座標とキーフレームにおけるセンサー姿勢とを含む、第２情報とを記憶する記憶部と、
   他の情報処理装置から、３次元空間をセンシングした位置を受信する受信部と、
   前記第１情報と前記第２情報とに基づいて、前記３次元空間をセンシングした位置を含む区画に対応づけられる環境地図を前記記憶部から抽出して前記他の情報処理装置に送信する送信部と、
   を有する情報処理装置。
2. 各キーフレームに関する情報を含む環境地図であって、前記各キーフレームに関する情報には、各キーフレーム上の点群に関する情報と、各キーフレームに対応づけられる３次元空間内の点の座標と、各キーフレームにおけるセンサー姿勢と、各キーフレームのセンシング位置を示す情報とが含まれている、環境地図を取得する取得部と、
   前記各キーフレームのセンシング位置を示す情報に基づいて、前記取得部で取得した環境地図をいずれかの区画に対応づけて前記記憶部に格納する格納部と、
   を有する、
   請求項１に記載の情報処理装置。
3. 前記取得部は、複数の環境地図を取得し、
   前記格納部は、
   前記複数の環境地図の各々に含まれる各キーフレームに関する情報のうち、重複するキーフレームに関する情報を統合することで、前記複数の環境地図を統合し、
   統合された前記複数の環境地図について、前記各キーフレームのセンシング位置を示す情報に基づいて、統合された前記複数の環境地図をいずれかの区画に対応づけて前記記憶部に格納する、
   請求項２に記載の情報処理装置。
4. 前記記憶部に記憶される、いずれかの区画に対応づけられる環境地図について、該環境地図に含まれる前記キーフレームに対応づけられる３次元空間内の点の座標は、該いずれかの区画内に存在する場合と、該いずれかの区画内に存在しない場合との両方を含む、
   請求項１～３のいずれか一項に記載の情報処理装置。
5. 情報処理装置が行う情報処理方法であって、
   ２次元平面を複数に区切ることで定められる区画を識別する区画識別子と各区画の範囲とを対応づける第１情報と、区画識別子と環境地図とを対応づける第２情報であって、前記環境地図はキーフレーム上の点群に関する情報とキーフレームに対応づけられる３次元空間内の点の座標とキーフレームにおけるセンサー姿勢とを含む、第２情報とを記憶部に記憶するステップと、
   他の情報処理装置から、３次元空間をセンシングした位置を受信するステップと、
   前記第１情報と前記第２情報とに基づいて、前記３次元空間をセンシングした位置を含む区画に対応づけられる環境地図を前記記憶部から抽出して前記他の情報処理装置に送信するステップと、
   を含む情報処理方法。
6. コンピュータに、
   ２次元平面を複数に区切ることで定められる区画を識別する区画識別子と各区画の範囲とを対応づける第１情報と、区画識別子と環境地図とを対応づける第２情報であって、前記環境地図はキーフレーム上の点群に関する情報とキーフレームに対応づけられる３次元空間内の点の座標とキーフレームにおけるセンサー姿勢とを含む、第２情報とを記憶部に記憶するステップと、
   他の情報処理装置から、３次元空間をセンシングした位置を受信するステップと、
   前記第１情報と前記第２情報とに基づいて、前記３次元空間をセンシングした位置を含む区画に対応づけられる環境地図を前記記憶部から抽出して前記他の情報処理装置に送信するステップと、
   を実行させるためのプログラム。
7. ２次元平面を複数に区切ることで定められる区画を識別する区画識別子と各区画の範囲とを対応づける第１情報と、区画識別子と環境地図とを対応づける第２情報であって、前記環境地図はキーフレーム上の点群に関する情報とキーフレームに対応づけられる３次元空間内の点の座標とキーフレームにおけるセンサー姿勢とを含む、第２情報とを記憶するサーバと通信する情報処理装置であって、
   ３次元空間をセンシングするセンサー部と、
   前記３次元空間をセンシングした位置を検出する検出部と、
   前記３次元空間をセンシングした位置を含む区画に対応する環境地図を前記サーバから取得する取得部と、
   推定部であって、前記環境地図を参照することで、
   前記３次元空間をセンシングすることで得られるセンシング結果における点群に関する情報と、各キーフレーム上の点群に関する情報とを比較することで、前記センシング結果に対応するキーフレームを選択するか、又は、前記環境地図に含まれる各キーフレームがセンシングされた位置に基づいて特定される前記３次元空間をセンシングした位置に最も近いキーフレームを選択し、
   前記選択したキーフレームに対応づけられる３次元空間内の点を該キーフレーム上に投影した点と、該投影した点に対応する前記センシング結果における点と該キーフレームにおけるセンサー姿勢とに基づいて、前記センシング結果がセンシングされた方向を推定する推定部と、
   を有する情報処理装置。
8. ２次元平面を複数に区切ることで定められる区画を識別する区画識別子と各区画の範囲とを対応づける第１情報と、区画識別子と環境地図とを対応づける第２情報であって、前記環境地図はキーフレーム上の点群に関する情報とキーフレームに対応づけられる３次元空間内の点の座標とキーフレームにおけるセンサー姿勢とを含む、第２情報とを記憶するサーバと通信する情報処理装置が行う情報処理方法であって、
   ３次元空間をセンシングするステップと、
   前記３次元空間をセンシングした位置を検出するステップと、
   前記３次元空間をセンシングした位置を含む区画に対応する環境地図を前記サーバから取得するステップと、
   推定するステップであって、前記環境地図を参照することで、
   前記３次元空間をセンシングすることで得られるセンシング結果における点群に関する情報と、各キーフレーム上の点群に関する情報とを比較することで、前記センシング結果に対応するキーフレームを選択するか、又は、前記環境地図に含まれる各キーフレームがセンシングされた位置に基づいて特定される前記３次元空間をセンシングした位置に最も近いキーフレームを選択し、
   前記選択したキーフレームに対応づけられる３次元空間内の点を該キーフレーム上に投影した点と、該投影した点に対応する前記センシング結果における点と該キーフレームにおけるセンサー姿勢とに基づいて、前記センシング結果がセンシングされた方向を推定するステップと、
   を含む情報処理方法。
9. ２次元平面を複数に区切ることで定められる区画を識別する区画識別子と各区画の範囲とを対応づける第１情報と、区画識別子と環境地図とを対応づける第２情報であって、前記環境地図はキーフレーム上の点群に関する情報とキーフレームに対応づけられる３次元空間内の点の座標とキーフレームにおけるセンサー姿勢とを含む、第２情報とを記憶するサーバと通信する情報処理装置であって、
   ３次元空間をセンシングするセンサー部と、
   前記３次元空間をセンシングした位置を検出する検出部と、
   前記３次元空間をセンシングした位置を含む区画に対応する環境地図を前記サーバから取得する取得部と、
   推定部であって、前記環境地図を参照することで、
   前記３次元空間をセンシングすることで得られるセンシング結果における点群に関する情報と、各キーフレーム上の点群に関する情報とを比較することで、前記センシング結果に対応するキーフレームを選択するか、又は、前記環境地図に含まれる各キーフレームがセンシングされた位置に基づいて特定される前記３次元空間をセンシングした位置に最も近いキーフレームを選択し、
   前記選択したキーフレームに対応づけられる３次元空間内の点を該キーフレーム上に投影した点と、該投影した点に対応する前記センシング結果における点と該キーフレームにおけるセンサー姿勢とに基づいて、前記センシング結果がセンシングされた方向を推定する推定部と、
   を有する情報処理装置。