(57)【特許請求の範囲】
【請求項１】
移動体に搭載された観測部によって観測された複数の３次元点データを取得し、
前記移動体の位置と、前記移動体の位置から前記観測部によって観測された複数の前記３次元点データとに基づいて、前記移動体の位置と複数の前記３次元点データが示す３次元点の各々との間の距離に応じた特徴量を計算し、
計算された前記特徴量と、前記移動体の移動空間上の参照点、及び前記参照点の位置と前記参照点の位置から観測された複数の３次元点データが示す３次元点の各々との間の距離に応じた前記特徴量の組み合わせが複数登録されたデータベースとに基づいて、前記データベースに登録された複数の前記参照点から、前記移動体の位置に対応する前記参照点を特定する、
処理をコンピュータに実行させる情報処理プログラム。
【請求項２】
前記特徴量は、前記移動体の位置からの各観測方向についての、前記移動体の位置と前記３次元点データが示す３次元点との間の距離の各々に対応する、前記観測方向に存在する前記３次元点の頻度を表すヒストグラムである、
請求項１に記載の情報処理プログラム。
【請求項３】
前記データベースには、前記参照点、及び前記参照点の位置から観測された複数の３次元点データに基づき計算された各観測方向の前記ヒストグラムの組み合わせが複数登録され、
前記移動体の位置に対応する前記参照点を特定する際に、
前記移動体の位置から前記観測部によって観測された複数の前記３次元点データに基づき計算された各観測方向の前記ヒストグラムと、前記データベースに格納された各観測方向の前記ヒストグラムとの間の類似度に基づいて、前記移動体の位置に対応する前記参照点を特定する、
請求項２に記載の情報処理プログラム。
【請求項４】
前記データベースには、前記参照点、及び前記参照点の位置から観測された複数の３次元点データに基づき計算された、前記参照点の位置を中心とする正ｎ面体の各面に対応する各観測方向の前記ヒストグラムの組み合わせが複数登録され、
前記移動体の位置に対応する前記参照点を特定する際に、
各参照点について、
前記移動体の位置を中心とする正ｎ面体を回転させたときに得られる、前記移動体の位置を中心とする正ｎ面体と前記参照点の位置を中心とする正ｎ面体との間の各重なり方に対し、
前記移動体の位置から前記観測部によって観測された複数の前記３次元点データに基づき計算された、前記移動体の位置を中心とする正ｎ面体の各面に対応する各観測方向の前記ヒストグラムと、前記データベースに格納された前記参照点の位置を中心とする正ｎ面体の各面に対応する各観測方向の前記ヒストグラムとの間の類似度を各面について算出し、各面について算出された前記類似度の総和を算出し、
算出された前記類似度の総和のうち、最大となる前記類似度の総和に基づいて、前記移動体の位置に対応する前記参照点を特定する、
請求項２又は請求項３に記載の情報処理プログラム。
【請求項５】
複数の３次元点データに基づいて、移動体が移動可能な空間上の各点について、前記点と複数の前記３次元点データが示す３次元点の各々との間の距離に応じて、前記移動体が移動可能な空間上の複数の点から、前記移動体の移動空間上の参照点を抽出し、抽出された前記参照点と複数の前記３次元点データが示す３次元点の各々との間の距離に応じた特徴量を計算し、前記参照点及び前記参照点について計算された前記特徴量の組み合わせをデータベースに登録する、
処理をコンピュータに実行させるデータベース生成プログラム。
【請求項６】
移動体に搭載された観測部によって観測された複数の３次元点データを取得する取得部と、
前記移動体の位置と、前記取得部によって取得された、前記移動体の位置から前記観測部によって観測された複数の前記３次元点データとに基づいて、前記移動体の位置と複数の前記３次元点データが示す３次元点の各々との間の距離に応じた特徴量を計算する特徴量計算部と、
前記特徴量計算部によって計算された前記特徴量と、前記移動体の移動空間上の参照点、及び前記参照点の位置と前記参照点の位置から観測された複数の３次元点データが示す３次元点の各々との間の距離に応じた前記特徴量の組み合わせが複数登録されたデータベースとに基づいて、前記データベースに登録された複数の前記参照点から、前記移動体の位置に対応する前記参照点を特定する特定部と、
を含む情報処理装置。
【請求項７】
移動体に搭載された観測部によって観測された複数の３次元点データを取得し、
前記移動体の位置と、前記移動体の位置から前記観測部によって観測された複数の前記３次元点データとに基づいて、前記移動体の位置と複数の前記３次元点データが示す３次元点の各々との間の距離に応じた特徴量を計算し、
計算された前記特徴量と、前記移動体の移動空間上の参照点、及び前記参照点の位置と前記参照点の位置から観測された複数の３次元点データが示す３次元点の各々との間の距離に応じた前記特徴量の組み合わせが複数登録されたデータベースとに基づいて、前記データベースに登録された複数の前記参照点から、前記移動体の位置に対応する前記参照点を特定する、
処理をコンピュータに実行させる情報処理方法。
【発明の詳細な説明】
【技術分野】
【０００１】
開示の技術は、情報処理装置、データベース生成装置、方法、プログラム、及び記憶媒体に関する。
【背景技術】
【０００２】
３次元センサによって得られたデータ中から物体位置姿勢推定を行う３次元物体照合装置が知られている。この３次元物体照合装置は、粗い位置姿勢推定を行う第１のステップと、その結果を受けて詳細な位置姿勢推定を行う第２のステップによる２段階の位置姿勢推定を行う。【０００３】
また、被計測物の背景情報の変化に影響を受けることなく被計測物の位置及び姿勢を認識する認識物体認識装置が知られている。この認識物体認識装置は、新たな計測位置から計測して得られた三次元形状上の座標値をコンピュータに入力し、各ボクセル位置を記憶する環境モデルを構築し、座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶する。そして、認識物体認識装置は、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせを行い、回転量及び並進量から自己位置を同定する。そして、認識物体認識装置は、統合して得られた計測データおよび誤差分布に対しモデルデータによって表される被計測物を回転および並進させ、計測データとモデルデータを構成する要素との距離に関する評価値が最小になるようにする位置合わせをする。【０００４】
また、認識対象物を認識するための処理速度を向上させることができる３次元物体認識装置が知られている。この３次元物体認識装置は、認識対象の３次元モデルを中心に置いた空間を複数のボクセルに分割し、夫々のボクセルに３次元モデル表面の最近点までの距離及び最近点を示すＩＤを記録させたディスタンスフィールドを記憶させる。３次元物体認識装置は、３次元センサの位置姿勢を仮定し、３次元センサにより計測した点群を３次元モデルに設定されるモデル座標系に変換した各計測点を対応するディスタンスフィールドの夫々のボクセルに入れることにより最近点との距離を取得する。【０００５】
また、計測された点群データから、対象物体に相当する点群を自動で抽出する物体認識装置が知られている。この物体認識装置は、計測点群及びモデル点群の各点について、各点に対する近傍点の相対的な座標情報より決定される形状属性を推定する。そして、物体認識装置は、同一の形状属性を持つ計測点群とモデル点群の各形状属性のマッチング処理を行い、形状属性の一致度に基づき、計測点群中におけるモデル点群の存在情報を取得する。【０００６】
また、距離センサから得られる距離データを用いて、対象物体の三次元的な位置姿勢を認識する物体認識装置が知られている。この物体認識装置は、頂点の三次元位置と頂点間を結び物体の表面を表す情報からなる三次元形状データを読み込み、モデルの頂点に対して物体の局所的な表面形状を表す特徴量を作成する。そして、物体認識装置は、実際の環境をセンサで計測して距離データを取得し、物体の表面情報を作成する。そして、物体認識装置は、シーン頂点に対して物体の局所的な表面形状を表す特徴量を作成し、シーン頂点の特徴量とモデル頂点の特徴量の類似度を算出する。そして、物体認識装置は、類似度がしきい値よりも高いペアが存在したら、シーン頂点とモデル頂点を対応点ペアとする。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００７−３２２３５１号公報
【特許文献２】特開２００８−３０９６７１号公報
【特許文献３】特開２０１２−２６９７４号公報
【特許文献４】国際公開第２０１４／１５５７１５号
【特許文献５】特開２００９−１２８１９２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかし、密度の異なる３次元点群データを用いて移動体の位置を推定する場合、移動体の位置を精度よく推定することができないという課題がある。例えば、移動体が備えるセンサから得られる３次元点群データの密度が、地図データの３次元点群データの密度よりも低い場合、疎な３次元点群データと密な３次元点群データとを対応付けることは難しい。【０００９】
一つの側面では、開示の技術は、密度の異なる３次元点群データ同士であっても３次元点群データを対応付けることが目的である。
【課題を解決するための手段】
【００１０】
一つの実施態様では、移動体に搭載された観測部によって観測された複数の３次元点データを取得する。そして、前記移動体の位置と、前記移動体の位置から前記観測部によって観測された複数の前記３次元点データとに基づいて、前記移動体の位置と複数の前記３次元点データが示す３次元点の各々との間の距離に応じた特徴量を計算する。そして、計算された前記特徴量と、前記移動体の移動空間上の参照点、及び前記参照点の位置と前記参照点の位置から観測された複数の３次元点データが示す３次元点の各々との間の距離に応じた前記特徴量の組み合わせが複数登録されたデータベースを参照する。そして、前記データベースに登録された複数の前記参照点から、前記移動体の位置に対応する前記参照点を特定する。
【発明の効果】
【００１１】
一つの側面として、密度の異なる３次元点群データ同士であっても３次元点群データを対応付けることができる、という効果を有する。
【図面の簡単な説明】
【００１２】
【図１】本実施形態に係るデータベース生成装置の概略ブロック図である。
【図２】３次元点データが格納されたテーブルの一例を示す図である。
【図３】モデル点群とデータ点群の一例を示す図である。
【図４】参照点の抽出を説明するための説明図である。
【図５】参照点から観測される可視点群を説明するための説明図である。
【図６】参照点を中心とする正ｎ面体の各面について算出されるヒストグラムを説明するための説明図である。
【図７】正ｎ面体の各面について計算されたヒストグラムの一例を示す図である。
【図８】参照点と参照点の各面について計算されたヒストグラムとが対応付けられたテーブルの一例を示す図である。
【図９】本実施形態に係る情報処理装置の概略ブロック図である。
【図１０】移動体の位置から観測される可視点群の一例を示す図である。
【図１１】参照点を中心とする正ｎ面体と移動体の位置を中心とする正ｎ面体との間の各重なり方について算出されるヒストグラム間の類似度を説明するための説明図である。
【図１２】本実施形態に係るデータベース生成装置として機能するコンピュータの概略構成を示すブロック図である。
【図１３】本実施形態に係る情報処理装置の制御部として機能するコンピュータの概略構成を示すブロック図である。
【図１４】本実施形態におけるデータベース生成処理の一例を示すフローチャートである。
【図１５】本実施形態における情報処理の一例を示すフローチャートである。
【図１６】本実施形態における特定処理の一例を示すフローチャートである。
【発明を実施するための形態】
【００１３】
以下、図面を参照して開示の技術の実施形態の一例を詳細に説明する。
【００１４】
＜データベース生成装置＞
【００１５】
図１に示すデータベース生成装置１０は、予め取得された複数の３次元点データに基づいて、移動体の移動空間上の参照点及び参照点の位置に応じた特徴量の組み合わせが複数登録されたデータベースを生成する。データベース生成装置１０は、図１に示すように、点群記憶部１２と、参照点抽出部１４と、観測点群抽出部１６と、特徴量計算部１８と、特徴量データベース２０とを備える。
【００１６】
点群記憶部１２には、予め取得された複数の３次元点データが格納される。３次元点データは、例えば、図２に示すように、３次元点群データの識別情報を表すＩＤと、３次元点群データに含まれる各点を示す３次元点データとが対応付けられてテーブルの形式で格納される。例えば、図２に示すID=000001の３次元点群データ「(q1,r1,s1),(q2,r2,s2),…,(qx,rx,sx)」には、「(q1,r1,s1)」、「(q2,r2,s2)」、・・・、「(qx,rx,sx)」の各点が含まれることを表している。また、「(q1,r1,s1)」、「(q2,r2,s2)」、・・・、「(qx,rx,sx)」の各々は、各点の３次元位置座標を示している。
【００１７】
点群記憶部１２には、例えば、Light Detection and Ranging（LiDAR）などのセンサによって取得された、高精細な複数の３次元点データを表すHigh-Definition Map（HD-MAP）であるモデル点群が格納される。点群記憶部１２に格納されるモデル点群は、例えば、移動体に搭載されたセンサによって取得される。例えば、移動体が走路を走行しているときに、移動体に搭載されたセンサによって、モデル点群が取得される。
【００１８】
図３に、点群記憶部１２に格納される複数の３次元点データの一例を表すモデル点群Ｘと、後述する情報処理装置のセンサによって取得される複数の３次元点データの一例を表すデータ点群Ｙとを示す。
【００１９】
モデル点群Ｘは、例えば、LiDARなどのセンサによって取得される高精細な点群データであり、地図データとして利用される。一方、データ点群Ｙは、例えばLaser Range Finder（LRF）などのセンサによって取得される。LRFなどのセンサによって取得されるデータ点群Ｙは、LiDARなどのセンサによって取得されるモデル点群Ｘよりも疎な点群データである。モデル点群Ｘが地図データとして格納され、移動体のセンサによってデータ点群Ｙが取得される場合、移動体の位置を推定する際にはモデル点群Ｘとデータ点群Ｙとの間の対応付けを行う必要がある。しかし、地図データのモデル点群Ｘとセンサによって取得されたデータ点群Ｙとで３次元点データの密度が異なる場合には、対応付けが難しい。
【００２０】
例えば、３次元点群データ同士の位置合わせでは、まず、複数の３次元点データのうち、特徴的な３次元形状を表す代表的な３次元点データが特徴点として複数検出される。次に、各特徴点について、特徴点近傍に存在する３次元点データの３次元座標及び法線情報が用いられ、特徴点近傍の３次元形状（例えば曲率など）に関する特徴量が記述される。しかし、特徴点の近傍に３次元形状を表現するだけの十分な密度の３次元点データが必要となるため、３次元点群データの密度が疎である場合には、正確な特徴量を記述できず、３次元点群データ同士の位置合わせの精度が低下してしまう。
【００２１】
そこで、本実施形態では、３次元点群データの密度の異なりによる位置合せ精度の低下を抑制するために、モデル点群において、移動体が移動可能な空間上の各点から、移動体の位置との対応付けに利用される参照点を抽出する。以下、参照点を抽出する参照点抽出部１４について詳述する。
【００２２】
参照点抽出部１４は、点群記憶部１２に格納されたモデル点群に基づいて、移動体が移動可能な空間上の各点について、点と点から観測される複数の３次元点データが示す３次元点の各々との間の距離を計算する。そして、参照点抽出部１４は、点の各々について計算された、複数の３次元点データが示す３次元点の各々との間の距離に応じて、移動体が移動可能な空間上の複数の点から、移動体の移動空間上の参照点を抽出する。
【００２３】
図４には、参照点の抽出を説明するための説明図が示されている。なお、説明の簡単のため、図４では、３次元空間を２次元空間で表して説明する。
【００２４】
例えば、参照点抽出部１４は、図４に示されるように、点群記憶部１２に格納されたモデル点群が存在する３次元空間２１を、所定個のボクセルに分割して、ボクセルグリッドを生成する。なお、モデル点群には、３次元空間２１に存在する物体２２の表面上の複数の点の各々の３次元位置座標を示す複数の３次元点データが含まれる。そして、参照点抽出部１４は、図４に示されるように、ボクセルグリッドの各ボクセルに対し、ボクセルから最近傍の３次元点データが示す３次元点までの距離を割り当てることにより、Distance Field(DF)２３を生成する。なお、参照点抽出部１４は、ＤＦ２３を生成する際、ボクセルから最近傍の３次元点までの距離の計算には、Fast Voxel Traversalアルゴリズムなどを用いることができる。なお、図４に示すＤＦ２３では、ボクセルから最近傍の３次元点までの距離が近いほど、色が濃いボクセルとして表される。
【００２５】
次に、参照点抽出部１４は、ＤＦ２３の各ボクセルについて、ボクセルに割り当てられた距離の値と当該ボクセルの周囲のボクセルに割り当てられた距離の値とを比較して、当該ボクセルに割り当てられた距離の値が、極大値であるか否かを判定する。なお、判定に用いられる周囲のボクセルの範囲は予め設定される。
【００２６】
また、参照点抽出部１４は、極大値の判定結果に基づいて、極大値と判定された距離の値が割り当てられた各ボクセルのうち、所定の閾値以上の距離の値を有するボクセルを参照ボクセルとして設定する。図４では、ボクセル内に丸印を付したボクセルが参照ボクセルである。そして、参照点抽出部１４は、図４に示されるように、設定された参照ボクセルの中心座標を参照点２４として抽出する。
【００２７】
観測点群抽出部１６は、参照点抽出部１４によって抽出された各参照点について、点群記憶部１２に格納されたモデル点群に基づいて、参照点から観測されるボクセルの各々を抽出する。例えば、参照ボクセルから隣接するボクセルを一定方向に順次辿って、物体２２を示す３次元点を含むボクセルに到達した場合に、そのボクセルを参照点から観測されるボクセルとすることができる。そして、観測点群抽出部１６は、各参照点に対し、抽出された各ボクセルについて、ボクセルの内部に存在する３次元点を、可視点群として設定する。例えば、観測点群抽出部１６は、図５に示されるように、参照点２５から観測可能な複数の３次元点データが示す３次元点を、可視点群２６として設定する。
【００２８】
特徴量計算部１８は、参照点抽出部１４によって抽出された各参照点について、参照点と、可視点群に含まれる複数の３次元点データが示す３次元点の各々との間の距離に応じて、参照点の位置から観測される可視空間の形状に関する特徴量を計算する。そして、特徴量計算部１８は、各参照点について、参照点及び参照点について計算された特徴量の組み合わせを特徴量データベース２０に登録する。
【００２９】
具体的には、特徴量計算部１８は、各参照点について、参照点からの各観測方向についてのヒストグラムを、特徴量として計算する。観測方向についてのヒストグラムは、参照点の位置と３次元点データが示す３次元点との間の距離の各々に対応する、当該観測方向に存在する３次元点の頻度を表す。
【００３０】
本実施形態の特徴量について、以下詳細に説明する。
【００３１】
図６に示されるような、特定の参照点Ｐｒの特徴量について説明する。まず、特徴量計算部１８は、図６に示されるように、参照点Ｐｒを中心とする正ｎ面体を設定する。そして、特徴量計算部１８は、可視点群のｘｙｚ座標系と、参照点Ｐｒ及び正ｎ面体の座標系とについて、対応する軸が平行になるように、参照点Ｐｒ及び正ｎ面体の座標系を設定する。
【００３２】
次に、特徴量計算部１８は、図６に示されるように、正ｎ面体の各面に、決められた順番でインデックスｉを付与し、正ｎ面体の各面Ｓｉ（ｉ＝１，…，ｎ）に対応する観測方向を定義する。
【００３３】
そして、特徴量計算部１８は、参照点Ｐｒからの正ｎ面体の各面Ｓｉに対応する観測方向に存在する可視点群の３次元点データのヒストグラムを特徴量として計算する。例えば、特徴量計算部１８は、図６に示されるように、参照点Ｐｒから、３次元点データを表す可視点Ｐｖまでの視線ｒと交差する面Ｓｉについてのヒストグラムにおいて、可視点Ｐｖまでの距離を表す可視深度ｄに対応する距離のビンに投票する。可視深度ｄは、以下の式（１）によって算出される。
【００３４】
ｄ＝｜Ｐｒ−Ｐｖ｜２                （１）
【００３５】
特徴量計算部１８によって、例えば、図７に示されるような、正ｎ面体の各面のヒストグラムが特徴量として生成される。例えば、図７に示されるヒストグラム２７は、正ｎ面体のＳ１の面について計算されたヒストグラムを表し、ヒストグラム２８は、正ｎ面体のＳ２の面について計算されたヒストグラムを表す。ヒストグラムの横軸は可視深度ｄを表し、縦軸は頻度を表す。また、特徴量計算部１８は、例えば、各面のヒストグラムについて、ヒストグラムの各ビンの総和が１になるように正規化する。
【００３６】
そして、特徴量計算部１８は、各参照点について、参照点及び参照点の位置を中心とする正ｎ面体の各面について計算されたヒストグラムの組み合わせを、特徴量データベース２０に登録する。
【００３７】
特徴量データベース２０には、特徴量計算部１８によって得られた、各参照点についての、参照点及び参照点について計算された正ｎ面体の各面のヒストグラムの組み合わせが登録される。参照点及び参照点の位置を中心とする正ｎ面体の各面について計算されたヒストグラムの組み合わせは、例えば、図８に示すようにテーブルの形式で格納される。図８に示す例では、参照点を識別するための識別情報を表す参照点ＩＤと、参照点の３次元位置と、参照点において設定された正ｎ面体の面の識別情報を表すインデックスと、ヒストグラムとが対応付けられて格納される。例えば、参照点ＩＤ「000001」に対し、参照点の３次元位置（q1,r1,s1）が対応付けられ、正ｎ面体の各面を表すインデックス1,…,nが対応付けられる。そして、参照点ＩＤ「000001」の正ｎ面体の面を表すインデックス「1」に対し、ヒストグラムを表すベクトル（0.1,0.3,…）が対応付けられる。
【００３８】
＜情報処理装置＞
【００３９】
情報処理装置３０は、図９に示すように、センサ３２と、制御部３４とを備える。センサ３２は、開示の技術の観測部の一例である。情報処理装置３０は、移動体に搭載される。
【００４０】
センサ３２は、移動体に搭載され、移動体周辺の物体の表面上の各点の３次元位置を示す複数の３次元点データを逐次観測する。センサ３２は、例えば、３次元点データを検出可能なLRF等によって実現される。移動体としては、例えば自動車又はロボット等が挙げられる。
【００４１】
制御部３４は、センサ３２によって観測された複数の３次元点データと、データベース生成装置１０によって生成された特徴量データベースとに基づいて、特徴量データベースに格納された参照点と移動体の位置との対応付けを行う。制御部３４は、図９に示すように、情報取得部３６と、位置姿勢推定部３８と、特徴量計算部４０と、特徴量データベース４２と、特定部４４と、点群記憶部４５と、位置合わせ部４６と、出力部４８とを備える。情報取得部３６は、開示の技術の取得部の一例である。
【００４２】
情報取得部３６は、センサ３２によって観測された複数の３次元点データをデータ点群として逐次取得する。
【００４３】
位置姿勢推定部３８は、情報取得部３６によって取得されたデータ点群に基づいて、移動体の位置及び移動体の姿勢を推定する。例えば、位置姿勢推定部３８は、情報取得部３６によって取得されたデータ点群に基づいて、Simultaneous Localization and Mapping（SLAM）を用いて、移動体の位置及び移動体の姿勢を推定する。
【００４４】
特徴量計算部４０は、位置姿勢推定部３８により推定された移動体の位置と、情報取得部３６により取得されたデータ点群とに基づいて、移動体の位置から観測される可視空間の形状に関する特徴量を計算する。特徴量計算部４０は、データベース生成装置１０の特徴量計算部１８と同様の処理によって、特徴量を計算する。具体的には、特徴量計算部４０は、移動体の位置と、移動体の位置からセンサ３２により観測された複数の３次元点データが示す３次元点の各々との間の距離に応じた可視空間の形状に関する特徴量の一例であるヒストグラムを計算する。
【００４５】
より詳細には、まず、特徴量計算部４０は、データベース生成装置１０の特徴量計算部１８と同様に、位置姿勢推定部３８により推定された移動体の位置を中心とする正ｎ面体を設定する。次に、特徴量計算部４０は、正ｎ面体の各面に、決められた順番でインデックスｉを付与し、移動体の位置からの正ｎ面体の各面Ｓｉについて、面Ｓｉに対応する観測方向に存在する可視点群を設定する。また、特徴量計算部４０は、正ｎ面体の各面Ｓｉについて、面Ｓｉに対応する観測方向に存在する可視点群の３次元点データのヒストグラムを特徴量として計算する。そして、特徴量計算部１８は、正ｎ面体の各面について計算されたヒストグラムの各々について、ヒストグラムの各ビンの総和が１になるように正規化する。なお、特徴量計算部４０によって、例えば、図１０に示されるような可視点群２６Ｄが設定される。図１０に示されるように、移動体の位置２５Ｄから観測可能な可視点群２６Ｄは、上記図５に示される可視点群２６よりも疎な点群となる。
【００４６】
本実施形態において、正ｎ面体の各面について計算されるヒストグラムは、参照点の位置又は移動体の位置から観測される可視空間の形状を表している。例えば、上記図５の参照点２５から観測される可視点群２６に基づき計算されるヒストグラムと、上記図１０の移動体の位置２５Ｄから観測される可視点群２６Ｄに基づき計算されるヒストグラムとは類似する。そのため、３次元点群同士の密度が異なる場合であっても、移動体の位置と参照点とを対応付けることができる。
【００４７】
特徴量データベース４２には、データベース生成装置１０によって予め生成された特徴量データベース２０と同じ情報が格納される。具体的には、データベース生成装置１０によって予め生成された、各参照点についての、参照点及び参照点の位置を中心とする正ｎ面体の各面について計算されたヒストグラムの組み合わせが登録される。なお、参照点の位置を中心とする正ｎ面体の各面のヒストグラムにおいて、ヒストグラムの全てのビンの要素が０の面については、未観測の面とみなして類似度の算出には用いないように、マスクを設定する。
【００４８】
特定部４４は、特徴量計算部４０により計算された、移動体の位置を中心とした正ｎ面体の各面のヒストグラムに基づき、特徴量データベース４２に登録された複数の参照点から、移動体の位置に対応する参照点を特定する。
【００４９】
以下、特徴量計算部４０によって計算された、移動体の位置を中心とした正ｎ面体の各面のヒストグラムと、特徴量データベース４２に登録された各参照点を中心とした正ｎ面体の各面のヒストグラムとの間の対応付けについて説明する。
【００５０】
具体的には、特定部４４は、各参照点について、移動体の位置を中心とする正ｎ面体を回転させたときに得られる、移動体の位置を中心とする正ｎ面体と参照点の位置を中心とする正ｎ面体との間の各重なり方に対し、各面のヒストグラム間の類似度を算出する。そして、特定部４４は、参照点及び重なり方の組み合わせ毎に算出された、各面のヒストグラム間の類似度の総和のうち、最大となる類似度の総和に基づいて、移動体の位置に対応する参照点を特定する。なお、特定部４４は、移動体の位置に対応する各面のヒストグラムにおいて、ヒストグラムの全てのビンの要素が０の面については、未観測の面とみなして類似度の算出には用いないように、マスクを設定する。
【００５１】
図１１に、ヒストグラム間の類似度の算出方法を説明するための説明図を示す。なお、図１１に示す特徴量データベース４２には、各参照点について、参照点を中心とする正ｎ面体の各面について計算されたヒストグラムが格納されている。
【００５２】
例えば、特定部４４は、図１１に示すように、特徴量データベース４２に登録された各参照点から、１つの参照点Ｍを選択する。そして、特定部４４は、参照点Ｍを中心とする正ｎ面体ＰＭと、移動体の位置Ｄを中心とする正ｎ面体ＰＤとの間において、正ｎ面体ＰＤを回転させたときの全ての重なり方について、各面のヒストグラム間の類似度を算出する。なお、正ｎ面体同士の重なりとは、各面のインデックスに係わらず、正ｎ面体同士の各面を一致させた状態を表す。
【００５３】
例えば、特定部４４は、図１１に示されるように、参照点Ｍを中心とする正ｎ面体ＰＭの面Ｓ１のヒストグラムと、移動体の位置Ｄを中心とする正ｎ面体ＰＤの面Ｓ１との間の類似度を算出する。ヒストグラム間の類似度としては、例えば、ベクトル間の距離を用いることができる。また、特定部４４は、図１１に示されるように、参照点Ｍを中心とする正ｎ面体ＰＭの面Ｓ２のヒストグラムと、移動体の位置Ｄを中心とする正ｎ面体ＰＤの面Ｓ２との間の類似度を算出する。また、特定部４４は、図１１に示されるように、参照点Ｍを中心とする正ｎ面体ＰＭの面Ｓ３のヒストグラムと、移動体の位置Ｄを中心とする正ｎ面体ＰＤの面Ｓ３との間の類似度を算出する。そして、特定部４４は、各面のヒストグラム間の類似度の総和を算出する。なお、特定部４４は、移動体の位置に対応する各面のヒストグラムにおいて、ヒストグラムの全てのビンの要素が０の面については、マスクＭｋを設定する。
【００５４】
次に、特定部４４は、図１１に示されるように、移動体の位置Ｄを中心とする正ｎ面体ＰＤを回転させ、回転させた正ｎ面体ＰＤ’の各面のヒストグラムと選択された参照点Ｍを中心とする正ｎ面体ＰＭの各面のヒストグラムとの類似度を算出する。例えば、特定部４４は、図１１に示されるように、参照点Ｍを中心とする正ｎ面体ＰＭの面Ｓ１のヒストグラムと、回転された正ｎ面体ＰＤ’の面Ｓ１との間の類似度を算出する。また、特定部４４は、図１１に示されるように、参照点Ｍを中心とする正ｎ面体ＰＭの面Ｓ２のヒストグラムと、回転された正ｎ面体ＰＤ’の面Ｓｒとの間の類似度を算出する。また、特定部４４は、図１１に示されるように、参照点Ｍを中心とする正ｎ面体ＰＭの面Ｓ３のヒストグラムと、回転された正ｎ面体ＰＤ’の面Ｓ２との間の類似度を算出する。そして、特定部４４は、各面のヒストグラム間の類似度の総和を算出する。
【００５５】
特定部４４は、特徴量データベース４２に登録された各参照点について、全ての重なり方に対して、各面について算出されたヒストグラム間の類似度の総和を算出する。
【００５６】
特定部４４は、各参照点と各重なり方との全ての組み合わせについて算出された類似度の総和の各々から、最大となる類似度の総和を選択する。そして、特定部４４は、最大となる類似度の総和が予め設定された閾値以上である場合、最大となる類似度の総和に対応する参照点と位置姿勢推定部３８によって推定された移動体の位置とが対応していると判定する。また、特定部４４は、最大となる類似度の総和に対応する正ｎ面体ＰＤの回転を出力する。
【００５７】
点群記憶部４５には、データベース生成装置１０の点群記憶部１２と同じ情報であるモデル点群が格納される。
【００５８】
位置合わせ部４６は、特定部４４により得られた参照点及び正ｎ面体ＰＤの回転に応じて、位置姿勢推定部３８により推定された移動体の位置の座標系と点群記憶部４５のモデル点群の座標系との間の座標変換を推定する。移動体の位置で得られるデータ点群と点群記憶部４５に記憶されたモデル点群の間で３次元点の位置合わせ（移動体位置及び姿勢の同定）を行うことで、相対的な座標変換が推定される。
【００５９】
そして、位置合わせ部４６は、推定された座標変換に基づき、例えばIterative Closest Point（ICP）を用いて、情報取得部３６により取得されたデータ点群と点群記憶部４５のモデル点群とを一致させるように、詳細な座標変換を推定する。
【００６０】
出力部４８は、位置合わせ部４６によって推定された座標変換に関する情報を結果として出力する。
【００６１】
データベース生成装置１０は、例えば、図１２に示すコンピュータ５０で実現することができる。コンピュータ５０はＣＰＵ５１、一時記憶領域としてのメモリ５２、及び不揮発性の記憶部５３を備える。また、コンピュータ５０は、センサや表示装置及び入力装置等の入出力装置（図示省略）が接続される入出力interface（Ｉ／Ｆ）５４、及び記録媒体５９に対するデータの読み込み及び書き込みを制御するread/write（Ｒ／Ｗ）部５５を備える。また、コンピュータ５０は、インターネット等のネットワークに接続されるネットワークＩ／Ｆ５６を備える。ＣＰＵ５１、メモリ５２、記憶部５３、入出力Ｉ／Ｆ５４、Ｒ／Ｗ部５５、及びネットワークＩ／Ｆ５６は、バス５７を介して互いに接続される。
【００６２】
記憶部５３は、Hard Disk Drive（ＨＤＤ）、solid state drive（ＳＳＤ）、フラッシュメモリ等によって実現できる。記憶媒体としての記憶部５３には、コンピュータ５０をデータベース生成装置１０として機能させるためのデータベース生成プログラム６０が記憶されている。データベース生成プログラム６０は、参照点抽出プロセス６２と、観測点群抽出プロセス６３と、特徴量計算プロセス６４とを有する。また、記憶部５３は、点群記憶部１２を構成する情報が記憶される点群記憶領域６６を有する。また、記憶部５３は、特徴量データベース２０を構成する情報が記憶される特徴量記憶領域６７を有する。
【００６３】
ＣＰＵ５１は、データベース生成プログラム６０を記憶部５３から読み出してメモリ５２に展開し、データベース生成プログラム６０が有するプロセスを順次実行する。ＣＰＵ５１は、参照点抽出プロセス６２を実行することで、図１に示す参照点抽出部１４として動作する。また、ＣＰＵ５１は、観測点群抽出プロセス６３を実行することで、図１に示す観測点群抽出部１６として動作する。また、ＣＰＵ５１は、特徴量計算プロセス６４を実行することで、図１に示す特徴量計算部１８として動作する。また、ＣＰＵ５１は、点群記憶領域６６から情報を読み出して、点群記憶部１２をメモリ５２に展開する。また、ＣＰＵ５１は、特徴量記憶領域６７から情報を読み出して、特徴量データベース２０をメモリ５２に展開する。これにより、データベース生成プログラム６０を実行したコンピュータ５０が、データベース生成装置１０として機能することになる。そのため、ソフトウェアであるデータベース生成プログラム６０を実行するプロセッサはハードウェアである。
【００６４】
なお、データベース生成プログラム６０により実現される機能は、例えば半導体集積回路、より詳しくはApplication Specific Integrated Circuit（ＡＳＩＣ）等で実現することも可能である。
【００６５】
情報処理装置３０は、例えば、図１３に示すコンピュータ８０で実現することができる。コンピュータ８０はＣＰＵ８１、一時記憶領域としてのメモリ８２、及び不揮発性の記憶部８３を備える。また、コンピュータ８０は、センサ３２や表示装置及び入力装置等の入出力装置（図示省略）が接続される入出力Ｉ／Ｆ８４、及び記録媒体８９に対するデータの読み込み及び書き込みを制御するＲ／Ｗ部８５を備える。また、コンピュータ８０は、インターネット等のネットワークに接続されるネットワークＩ／Ｆ８６を備える。ＣＰＵ８１、メモリ８２、記憶部８３、入出力Ｉ／Ｆ８４、Ｒ／Ｗ部８５、及びネットワークＩ／Ｆ８６は、バス８７を介して互いに接続される。
【００６６】
記憶部８３は、ＨＤＤ、ＳＳＤ、フラッシュメモリ等によって実現できる。記憶媒体としての記憶部８３には、コンピュータ８０を情報処理装置３０として機能させるための情報処理プログラム９０が記憶されている。情報処理プログラム９０は、情報取得プロセス９２と、位置姿勢推定プロセス９３と、特徴量計算プロセス９４と、特定プロセス９５と、位置合わせプロセス９６と、出力プロセス９７とを有する。また、記憶部８３は、特徴量データベース４２を構成する情報が記憶される特徴量記憶領域９８を有する。また、記憶部８３は、点群記憶部４５を構成する情報が記憶される点群記憶領域９９を有する。
【００６７】
ＣＰＵ８１は、情報処理プログラム９０を記憶部８３から読み出してメモリ８２に展開し、情報処理プログラム９０が有するプロセスを順次実行する。ＣＰＵ８１は、情報取得プロセス９２を実行することで、図９に示す情報取得部３６として動作する。また、ＣＰＵ８１は、位置姿勢推定プロセス９３を実行することで、図９に示す位置姿勢推定部３８として動作する。また、ＣＰＵ８１は、特徴量計算プロセス９４を実行することで、図９に示す特徴量計算部４０として動作する。また、ＣＰＵ８１は、特定プロセス９５を実行することで、図９に示す特定部４４として動作する。また、ＣＰＵ８１は、位置合わせプロセス９６を実行することで、図９に示す位置合わせ部４６として動作する。また、ＣＰＵ８１は、出力プロセス９７を実行することで、図９に示す出力部４８として動作する。また、ＣＰＵ５１は、特徴量記憶領域９８から情報を読み出して、特徴量データベース４２をメモリ８２に展開する。また、ＣＰＵ８１は、点群記憶領域９９から情報を読み出して、点群記憶部４５をメモリ８２に展開する。これにより、情報処理プログラム９０を実行したコンピュータ８０が、情報処理装置３０として機能することになる。そのため、ソフトウェアである情報処理プログラム９０を実行するプロセッサはハードウェアである。
【００６８】
なお、情報処理プログラム９０により実現される機能は、例えば半導体集積回路、より詳しくはＡＳＩＣ等で実現することも可能である。
【００６９】
次に、本実施形態に係るデータベース生成装置１０の作用について説明する。データベース生成装置１０において、例えばLiDARなどのセンサによって取得されたモデル点群が点群記憶部１２に格納される。そして、データベース生成装置１０は、図１４に示すデータベース生成処理を実行する。以下、各処理について詳述する。
【００７０】
ステップＳ１００において、参照点抽出部１４は、点群記憶部１２に格納されたモデル点群を取得する。
【００７１】
ステップＳ１０２において、参照点抽出部１４は、上記ステップＳ１００で取得されたモデル点群が存在する３次元空間を、所定個のボクセルに分割して、ボクセルグリッドを生成する。
【００７２】
ステップＳ１０４において、参照点抽出部１４は、上記ステップＳ１０２で生成されたボクセルグリッドの各ボクセル対し、ボクセルから最近傍の３次元点データが示す３次元点までの距離を割り当てることにより、ＤＦを生成する。
【００７３】
ステップＳ１０６において、参照点抽出部１４は、上記ステップＳ１０４で生成されたＤＦの各ボクセルに割り当てられた距離の値が、周囲のボクセルと比較して極大値であるか否かを判定する。そして、参照点抽出部１４は、極大値の判定結果に基づいて、極大値と判定された距離の値が割り当てられた各ボクセルのうち、所定の閾値以上の距離の値を有するボクセルを参照ボクセルとして設定する。そして、参照点抽出部１４は、参照ボクセルの中心座標を参照点として設定する。
【００７４】
ステップＳ１０８において、観測点群抽出部１６は、上記ステップＳ１０６で抽出された各参照点について、参照点から観測されるボクセルの各々を抽出する。そして、観測点群抽出部１６は、抽出された各ボクセルについて、ボクセルの内部に存在する３次元点データが示す３次元点を、可視点群として設定する。
【００７５】
ステップＳ１１０において、特徴量計算部１８は、上記ステップＳ１０６で抽出された各参照点について、参照点から上記ステップＳ１０８で設定された可視点群に含まれる可視点までの視線と交差する正ｎ面体の各面に対し、ヒストグラムを生成する。
【００７６】
ステップＳ１１２において、特徴量計算部１８は、ステップＳ１０６で抽出された各参照点について、参照点及び参照点を中心とする正ｎ面体の各面について計算されたヒストグラムの組み合わせを、特徴量データベース２０に登録する。そして、データベース生成処理が終了される。
【００７７】
次に、本実施形態に係る情報処理装置３０の作用について説明する。データベース生成装置１０によって生成された、各参照点についての、参照点及び参照点の位置を中心とする正ｎ面体の各面のヒストグラムの組み合わせが情報処理装置３０へ入力されると、情報処理装置３０の特徴量データベース４２に登録される。そして、情報処理装置３０において、センサ３２が複数の３次元点データであるデータ点群を逐次検出し、情報取得部３６はセンサ３２によって検出されたデータ点群を逐次取得する。情報取得部３６がデータ点群を取得しているときに、情報処理装置３０の制御部３４によって、図１５に示す情報処理が実行される。情報処理ルーチンは、情報取得部３６がデータ点群を取得する毎に実行される。以下、各処理について詳述する。
【００７８】
ステップＳ２００において、位置姿勢推定部３８は、情報取得部３６によって取得されたデータ点群に基づいて、移動体の位置及び移動体の姿勢を推定する。
【００７９】
ステップＳ２０２において、特徴量計算部４０は、上記ステップＳ２００で推定された移動体の位置と、情報取得部３６により取得されたデータ点群とに基づいて、移動体の位置を中心とする正ｎ面体の各面に対するヒストグラムを計算する。
【００８０】
ステップＳ２０４において、特定部４４は、上記ステップＳ２０２で計算された、移動体の位置を中心とした正ｎ面体の各面のヒストグラムに基づき、特徴量データベース４２に登録された複数の参照点から、移動体の位置に対応する参照点を特定する。ステップＳ２０４の処理は、図１６に示す特定処理ルーチンによって実現される。
【００８１】
＜特定処理ルーチン＞
ステップＳ３００において、特定部４４は、特徴量データベース４２に登録された各参照点から、１つの参照点を設定する。
【００８２】
ステップＳ３０２において、特定部４４は、上記ステップＳ３００で設定された参照点を中心とする正ｎ面体と、上記ステップＳ２００で推定された移動体の位置を中心とする正ｎ面体との間における、重なり方を設定する。
【００８３】
ステップＳ３０４において、特定部４４は、ステップＳ３０２で設定された重なり方において、特徴量データベース４２の参照点の正ｎ面体の各面のヒストグラムとステップＳ２０２で得られた移動体の正ｎ面体の各面のヒストグラムとの間の類似度を算出する。
【００８４】
ステップＳ３０６において、特定部４４は、上記ステップＳ３０４で算出された、各面のヒストグラム間の類似度の総和を算出する。そして、特定部４４は、上記ステップＳ３００で設定された参照点及び上記ステップＳ３０２で設定された重なり方の組み合わせにおける、類似度の総和を記憶領域に格納する。
【００８５】
ステップＳ３０８で、特定部４４は、ステップＳ３００で設定された参照点を中心とする正ｎ面体と、移動体の位置を中心とする正ｎ面体との間における、全ての重なり方について上記ステップＳ３０２〜ステップＳ３０６の処理を実行したか否かを判定する。全ての重なり方について上記ステップＳ３０２〜ステップＳ３０６の処理を実行した場合には、ステップＳ３１０へ進む。一方、上記ステップＳ３０２〜ステップＳ３０６の処理を実行していない重なり方が存在する場合には、ステップＳ３０２へ戻る。
【００８６】
ステップＳ３１０において、特定部４４は、特徴量データベース４２の全ての参照点について、上記ステップＳ３００〜ステップＳ３０８の処理を実行したか否かを判定する。全ての参照点について、上記ステップＳ３００〜ステップＳ３０８の処理を実行した場合には、ステップＳ３１２へ進む。一方、上記ステップＳ３００〜ステップＳ３０８の処理を実行していない参照点が特徴量データベース４２に存在する場合には、ステップＳ３００へ進む。
【００８７】
ステップＳ３１２において、特定部４４は、上記ステップＳ３０６で記憶領域に格納された類似度の総和の各々のうち、最大となる類似度の総和を特定する。
【００８８】
ステップＳ３１４において、特定部４４は、上記ステップＳ３１２で特定された、最大となる類似度の総和に基づいて、最大となる類似度の総和に対応する、参照点及び重なり方を特定する。
【００８９】
ステップＳ３１６において、特定部４４は、上記ステップＳ３１４で特定された参照点及び重なり方を、結果として出力する。
【００９０】
次に、図１５の情報処理ルーチンのステップＳ２０６で、位置合わせ部４６は、ステップＳ２０４で得られた参照点及び重なり方に基づいて、ステップＳ２００で推定された移動体の位置の座標系と点群記憶部４５のモデル点群の座標系との間の座標変換を推定する。
【００９１】
ステップＳ２０８において、位置合わせ部４６は、上記ステップＳ２０６で推定された座標変換に基づき、例えばＩＣＰを用いて、情報取得部３６で取得されたデータ点群と点群記憶部４５のモデル点群とを一致させるようにして、詳細な座標変換を推定する。
【００９２】
ステップＳ２１０において、出力部４８は、上記ステップＳ２０８で推定された座標変換に関する情報を結果として出力して、情報処理ルーチンを終了する。
【００９３】
以上説明したように、本実施形態に係るデータベース生成装置は、移動体が移動可能な空間上の各点について、点と点から観測される複数の３次元点データが示す３次元点の各々との間の距離に応じて、移動体が移動可能な空間上の各点から参照点を抽出する。そして、データベース生成装置は、抽出された参照点と複数の３次元点データが示す３次元点の各々との間の距離に応じた特徴量を計算し、参照点及び参照点について計算された特徴量の組み合わせをデータベースに登録する。これにより、密度の異なる３次元点群データ同士であっても、３次元点群データ同士を対応付けるための参照点を取得することができる。
【００９４】
また、本実施形態に係る情報処理装置は、移動体の位置と移動体に搭載されたセンサによって観測された複数の３次元点データとに基づいて、移動体の位置と複数の３次元点データが示す３次元点の各々との間の距離に応じた特徴量を計算する。そして、情報処理装置は、特徴量と、参照点及び参照点の位置と参照点の位置から観測された複数の３次元点データが示す３次元点の各々との間の距離に応じた特徴量の組み合わせが複数登録されたデータベースに基づき、移動体の位置に対応する参照点を特定する。これにより、密度の異なる３次元点群データ同士であっても、３次元点群データ同士を対応付けることができる。例えば、密な３次元点群データである地図データと、移動体が備えるセンサから得られる疎な３次元点群データとを対応付けて、移動体の位置を推定することができる。
【００９５】
上述したように、３次元点群の密度が異なる場合において、３次元点のいずれかを特徴点とする場合、特徴点についての特徴量を計算するための情報量に差が生じてしまい、計算された特徴量に基づく特徴点同士の対応付けが困難になる。本実施形態では、３次元点群に含まれる特徴点の特徴量を利用するのではなく、移動体が移動可能な空間上の点から、観測された３次元点までの距離に応じた特徴量、すなわち、移動体が移動可能な空間上の点からの可視性に基づく特徴量を利用する。これにより、３次元点群の密度に異なりがある場合でも、参照点と移動体の位置の各々について、対応付けが可能な同様の特徴量を計算することができ、密度の異なる３次元点群の対応付けが可能になる。
【００９６】
なお、上記では、データベース生成プログラム及び情報処理プログラムが記憶部に予め記憶（インストール）されている態様を説明したが、これに限定されない。開示の技術に係るプログラムは、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、ＵＳＢメモリ等の記録媒体に記録された形態で提供することも可能である。
【００９７】
本明細書に記載された全ての文献、特許出願及び技術規格は、個々の文献、特許出願及び技術規格が参照により取り込まれることが具体的かつ個々に記された場合と同程度に、本明細書中に参照により取り込まれる。
【００９８】
次に、上記実施形態の変形例を説明する。
【００９９】
上記実施形態では、データベース生成装置と情報処理装置とを別々の装置として構成する場合を例に説明したが、データベース生成装置と情報処理装置とを１つの装置として構成してもよい。
【符号の説明】
【０１００】
１０   データベース生成装置
１２   点群記憶部
１４   参照点抽出部
１６   観測点群抽出部
１８   特徴量計算部
２０   特徴量データベース
２４，２５    参照点
２６   可視点群
３０   情報処理装置
３２   センサ
３４   制御部
３６   情報取得部
３８   位置姿勢推定部
４０   特徴量計算部
４２   特徴量データベース
４４   特定部
４５   点群記憶部
４６   位置合わせ部
４８   出力部
５０，８０    コンピュータ
５１，８１    ＣＰＵ
５３，８３    記憶部
５９，８９    記録媒体
６０   データベース生成プログラム
９０   情報処理プログラム
【図１】
0006673504.tif 000002
【図２】
0006673504.tif 000003
【図３】
0006673504.tif 000004
【図４】
0006673504.tif 000005
【図５】
0006673504.tif 000006
【図６】
0006673504.tif 000007
【図７】
0006673504.tif 000008
【図８】
0006673504.tif 000009
【図９】
0006673504.tif 000010
【図１０】
0006673504.tif 000011
【図１１】
0006673504.tif 000012
【図１２】
0006673504.tif 000013
【図１３】
0006673504.tif 000014
【図１４】
0006673504.tif 000015
【図１５】
0006673504.tif 000016
【図１６】
0006673504.tif 000017