特許 6853150 選別装置、方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6853150

(24)【登録日】2021年3月15日

(45)【発行日】2021年3月31日

(54)【発明の名称】選別装置、方法及びプログラム

(51)【国際特許分類】

   G06T 17/00 20060101AFI20210322BHJP

【ＦＩ】

   G06T17/00

【請求項の数】7

【全頁数】19

(21)【出願番号】特願2017-178016(P2017-178016)

(22)【出願日】2017年9月15日

(65)【公開番号】特開2019-53597(P2019-53597A)

(43)【公開日】2019年4月4日

【審査請求日】2019年8月23日

(73)【特許権者】

【識別番号】000208891

【氏名又は名称】ＫＤＤＩ株式会社

(74)【代理人】

【識別番号】100092772

【弁理士】

【氏名又は名称】阪本 清孝

(74)【代理人】

【識別番号】100119688

【弁理士】

【氏名又は名称】田邉 壽二

(72)【発明者】

【氏名】松▲崎▼ 康平

【審査官】 村松 貴士

(56)【参考文献】

【文献】 特開２０１６−２１７９４１（ＪＰ，Ａ）

【文献】 特開２０１６−２２４６７４（ＪＰ，Ａ）

【文献】 特開２００３−１２３０５７（ＪＰ，Ａ）

【文献】 国際公開第２００７／０６９７２１（ＷＯ，Ａ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０６Ｔ １／００ − １９／２０

Ｇ０１Ｂ １１／００ − １１／３０

(57)【特許請求の範囲】

【請求項1】

複数の位置姿勢においてそれぞれ取得されたフレーム点群データ同士を共通座標系において統合した参照点群データをボクセル分割する第一分割部と、
前記ボクセル分割された参照点群データをモデル化してボクセルごとの参照点群モデル化データを得る第一計算部と、
前記共通座標系に基づいて前記複数の位置姿勢において取得されたフレーム点群データをそれぞれボクセル分割する第二分割部と、
前記取得された位置姿勢ごとに前記ボクセル分割されたフレーム点群データをモデル化して、位置姿勢ごと及びボクセルごとのフレーム点群モデル化データを得る第二計算部と、
前記第一計算部で得たボクセル毎の参照点群モデル化データと、前記第二計算部で得た位置姿勢ごと及びボクセルごとのフレーム点群モデル化データと、を比較する比較部と、
前記比較部での比較結果に基づいて前記第一計算部で得たボクセル毎の参照点群モデル化データから選別を行う選別部と、を備えることを特徴とする選別装置。

【請求項2】

前記第二分割部では前記共通座標系において前記第一分割部において分割したのと共通のボクセルグリッドによってボクセルに分割することを含めてボクセル分割し、
前記選別部では、前記比較部において比較されることにより、前記第一計算部で得たボクセル毎の参照点群モデル化データと、前記第二計算部で得た位置姿勢ごと及び前記共通のボクセルグリッドにおけるボクセルごとのフレーム点群モデル化データと、の間で類似していると判定されたものを優先して、前記選別を行うことを特徴とする請求項１に記載の選別装置。

【請求項3】

前記第二分割部では前記共通座標系において前記第一分割部において分割したのと共通のボクセルグリッドをサブボクセル単位で並進移動した一連のボクセルグリッドによってボクセルに分割することを含めてボクセル分割し、
前記選別部では、前記比較部において比較されることにより、前記第一計算部で得たボクセル毎の参照点群モデル化データと、前記第二計算部で得た位置姿勢ごと及び前記一連のボクセルグリッドにおけるボクセルごとのフレーム点群モデル化データと、の間で類似していると判定されたものを優先して、前記選別を行うことを特徴とする請求項１または２に記載の選別装置。

【請求項4】

前記第二分割部では前記共通座標系において前記第一分割部において分割したのと共通のボクセルグリッドをサブボクセル単位で並進移動した一連のボクセルグリッドによってボクセルに分割することを含めてボクセル分割し、
前記選別部では、前記比較部において比較されることにより、前記第一計算部で得たボクセル毎の参照点群モデル化データと、前記第二計算部で得た位置姿勢ごと及び前記一連のボクセルグリッドにおけるボクセルごとのフレーム点群モデル化データと、の間で類似していないと判定されたものを優先して、前記選別を行うことを特徴とする請求項１または２に記載の選別装置。

【請求項5】

前記第二分割部では、前記共通座標系において前記第一分割部において分割したのと共通のボクセルグリッド及び当該共通のボクセルグリッドをサブボクセル単位で並進移動した一連のボクセルグリッドをさらにボクセル毎に回転して得られる複数の回転ボクセルグリッドによってボクセルに分割することを含めてボクセル分割し、前記比較部において比較される前記一連のボクセルグリッドに当該複数の回転ボクセルグリッドを含めることを特徴とする請求項３または４に記載の選別装置。

【請求項6】

複数の位置姿勢においてそれぞれ取得されたフレーム点群データ同士を共通座標系において統合した参照点群データをボクセル分割する第一分割段階と、
前記ボクセル分割された参照点群データをモデル化してボクセルごとの参照点群モデル化データを得る第一計算段階と、
前記共通座標系に基づいて前記複数の位置姿勢において取得されたフレーム点群データをそれぞれボクセル分割する第二分割段階と、
前記取得された位置姿勢ごとに前記ボクセル分割されたフレーム点群データをモデル化して、位置姿勢ごと及びボクセルごとのフレーム点群モデル化データを得る第二計算段階と、
前記第一計算段階で得たボクセル毎の参照点群モデル化データと、前記第二計算段階で得た位置姿勢ごと及びボクセルごとのフレーム点群モデル化データと、を比較する比較段階と、
前記比較段階での比較結果に基づいて前記第一計算段階で得たボクセル毎の参照点群モデル化データから選別を行う選別段階と、を備えることを特徴とする、コンピュータによって実行される方法。

【請求項7】

コンピュータを請求項１ないし５のいずれかに記載の選別装置として機能させることを特徴とするプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、点群データに関してボクセルベースのモデル化を行ったうえで自己位置推定等の後段側処理に供される際のデータ量を、後段側処理の精度等を確保したうえで効果的に削減することのできる選別装置、方法及びプログラムに関する。

【背景技術】

【0002】

近年、様々な種類の距離センサ（例えば、 LiDAR, Time-of-flight camera, structured lightなど）の商用化に伴い、3D点群(point cloud)が注目を集めている。3D点群は、物体認識や領域のセグメンテーション、自己位置推定、変化検知等のコンピュータビジョンにおける様々な技術領域で利用される。

【0003】

3D点群を用いた自己位置推定技術は，予め実世界の特定の範囲をセンサで計測して得られた点群（以後、地図点群と呼ぶ。）と、ユーザがその場でセンサを用いて取得した点群（以後、センサ点群と呼ぶ。）との位置合わせによって実現される。例えば，代表的な自己位置推定方法として、非特許文献1におけるIterative Closest Point (ICP)がある。
地図点群は一般的に、センサによる単位時間当たりの計測を1フレームとし、複数フレームを例えば非特許文献2に記載のSLAM（Simultaneous Localization and Mapping）技術を用いて統合することによって作成される。SLAM技術は環境地図の作成、すなわち複数フレームの統合と、自己位置推定、すなわち各フレームを計測した際のセンサの姿勢推定を同時に行う技術の総称である。従って、地図点群を作成する際には，結果としてフレーム毎の姿勢も得られる。

【0004】

近年の距離センサは1秒間の間に数十万点以上の3D点を計測するため、実世界の広範囲の計測を介して生成される地図点群のデータ量は膨大となる。そのため、要求されるストレージ量の削減及び／又は点群を用いた処理を効率化するために、点群のデータ量の削減が行われることがある。

【0005】

その方法の一つに、点群のダウンサンプリングがある。この方法は，例えば所与の点群からランダムに指定個数の点を選択することにより、データ量を削減する。しかしながらこの方法は、単純にデータをスパースにするため、データ量を削減するほど、例えば自己位置推定の様な後段の処理の結果に対して悪影響を与える可能性が高くなる。

【0006】

データ量削減のためのもう一つの方法として、点群のモデル化がある。この方法は、例えば多変量正規分布の様な確率密度モデルを用いて点群をモデル化し、元々のデータをモデルパラメータの集合として表現することによって、データ量を削減する。

【0007】

3D点群データのモデル化手法の多くは，ボクセルグリッドに依存している。例えば非特許文献3におけるNormal Distribution Transform (NDT)は空間をボクセルで区切った後に、各ボクセル内に位置する点群を3次元多変量正規分布のパラメータ、すなわち平均と分散共分散行列で表現する。そして、モデル化された地図点群とセンサ点群を用いて自己位置推定を実現する。また、非特許文献4に記載のNDT-D2D（Distribution to Distribution）は，地図点群だけでなくセンサ点群もモデル化し、モデル化されたデータ同士を用いて自己位置推定を実現する。

【0008】

しかしながら、ダウンサンプリングされた点群およびモデル化された点群データであっても、広範囲の計測から得られる点群に対するデータ量は膨大となる。一般的にデータのダウンサンプル率と後段の処理（当該データを用いての自己位置推定等の処理）の精度はトレードオフの関係になるため、ダウンサンプリングされた後であっても、多数の3D点で構成される地図点群のデータ量は依然として大きい。非特許文献3および4の手法において高い精度を得るためにはボクセルを比較的細かくする必要があり、また、ボクセル1つあたりが持つパラメータ数が多い。結果として，モデル化された地図点群のデータ量は大きくなる。

【0009】

このような問題に関連して非特許文献5に記載のOctoMapは、ボクセルの8分木表現を用いることで、均一の解像度のボクセルを用いる場合に比べてコンパクトなデータ量で空間をモデル化している。すなわち、例えば空の様な点の分布が均一の空間は粗いボクセルで表現し、複雑な点の分布を持つ空間は細かい解像度で表現する、というものである。

【先行技術文献】

【非特許文献】

【0010】

【非特許文献1】P. Besl and N. McKay, "A Method for Registration of 3-D Shapes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, pp. 239-256, 1992.

【非特許文献2】Zhang, Ji, and Sanjiv Singh. "LOAM: Lidar Odometry and Mapping in Real-time." Robotics: Science and Systems. Vol. 2. 2014.

【非特許文献3】Magnusson, Martin, Achim Lilienthal, and Tom Duckett. "Scanregistration for autonomous mining vehicles using 3D‐NDT." Journal of FieldRobotics 24.10 (2007): 803-827.

【非特許文献4】Stoyanov, Todor, Martin Magnusson, and Achim J. Lilienthal."Point set registration through minimization of the l 2 distance between 3d-ndtmodels." Robotics and Automation (ICRA), 2012 IEEE International Conferenceon. IEEE, 2012.

【非特許文献5】Hornung, Armin, et al. "OctoMap: An efficient probabilistic 3Dmapping framework based on octrees." Autonomous Robots 34.3 (2013): 189-206.

【発明の概要】

【発明が解決しようとする課題】

【0011】

しかしながら、以上のような従来手法は全て、後段側の処理精度等とのトレードオフ関係等のもとで効果的にデータ量を削減するということに関して、改善の余地を有しているものであった。

【0012】

例えば、非特許文献１〜４における一連の課題に対して対処しうるものである非特許文献５の手法においても依然として、以下のような課題があった。

【0013】

すなわち、この手法は空間内の全てを"occupied"（占有状態），"free"（空状態），"unknown"（不明）にモデル化した完全な3Dモデル表現が必要な場合には有用であるものの、例えば"occupied"のボクセルのみが必要な場合においては，それらのみを含む不完全な3Dモデル表現の方がよりコンパクトになる可能性がある。実際、例えばNDTでは"occupied"に相当するボクセルのデータのみが必要となり，"free"，"unknown"に相当するボクセルのデータは不要である。

【0014】

ここで、ボクセルに基づくデータの量を更に削減するための単純な方法として、ボクセルのダウンサンプリングが考えられる。すなわち、点群のダウンサンプリングと同様の考えを、ボクセル群に対して適用する。例えば、所与のボクセル群からランダムに指定個数のボクセルを選択することにより、総データ量を削減することができる。

【0015】

しかしながら、ボクセルのダウンサンプリングも前述の点群のダウンサンプリングと同様の課題を有している。すなわち、ボクセルのダウンサンプリングを行うことは、単純にデータをスパースにするため、データ量を削減するほど後段の処理の結果に対して悪影響を与える可能性が高くなる。特に、点と比べてボクセルは空間内で大きな範囲を占めるため，その欠損が与える影響はより大きくなる。

【0016】

上記のような従来技術の課題に鑑み、本発明は、点群データに関してボクセルベースのモデル化を行ったうえで自己位置推定等の後段側処理に供される際のデータ量を、後段側処理の精度等を確保したうえで効果的に削減することのできる選別装置、方法及びプログラムを提供することを目的とする。

【課題を解決するための手段】

【0017】

上記目的を達成するため、本発明は選別装置であって、複数の位置姿勢においてそれぞれ取得されたフレーム点群データ同士を共通座標系において統合した参照点群データをボクセル分割する第一分割部と、前記ボクセル分割された参照点群データをモデル化してボクセルごとの参照点群モデル化データを得る第一計算部と、前記共通座標系に基づいて前記複数の位置姿勢において取得されたフレーム点群データをそれぞれボクセル分割する第二分割部と、前記取得された位置姿勢ごとに前記ボクセル分割されたフレーム点群データをモデル化して、位置姿勢ごと及びボクセルごとのフレーム点群モデル化データを得る第二計算部と、前記第一計算部で得たボクセル毎の参照点群モデル化データと、前記第二計算部で得た位置姿勢ごと及びボクセルごとのフレーム点群モデル化データと、を比較する比較部と、前記比較部での比較結果に基づいて前記第一計算部で得たボクセル毎の参照点群モデル化データから選別を行う選別部と、を備えることを特徴とする。またこれに対応する方法及びプログラムであることを特徴とする。

【発明の効果】

【0018】

本発明によれば、効果的にデータ量を削減することができる。

【図面の簡単な説明】

【0019】

【図1】一実施形態に係る選別装置の機能ブロック図である。

【図2】一実施形態に係る選別装置の動作のフローチャートである。

【図3】一実施形態に係る本発明の手法を説明するための模式図である。

【図4】第二分割部の機能ブロック図である。

【図5】非対称性を説明するための模式図である。

【図6】ボクセル量子化誤りの模式例として、ボクセル分割位置のわずかな相違でモデル化パラメータが顕著に異なるようになるボクセルの模式例を示す図である。

【図7】ボクセル分割位置のわずかな相違ではモデル化パラメータはほとんど変化しないボクセルの模式例を示す図である。

【図8】回転分割部での回転の模式例を示す図である。

【発明を実施するための形態】

【0020】

図１は、一実施形態に係る選別装置の機能ブロック図である。図示する通り、選別装置10は第一分割部11、第一計算部12、比較部13、選別部14、整列部20、第二分割部21及び第二計算部22を備える。また図示する通り、選別装置10の各部間でのデータ授受としての各部の機能概略は以下の通りである。なお、以降では、参照点群データの一例として、地図点群データを用いて説明する。

【0021】

第一分割部11は、入力される地図点群データをボクセル分割して得られるボクセル分割された地図点群データを第一計算部12へと出力する。第一計算部12は当該ボクセルごとの地図点群データにおいてボクセルごとに点群をモデル化する計算を行い、得られたボクセルごとの地図点群モデル化データを比較部13へと出力する。

【0022】

整列部20は入力される位置姿勢ごとのフレーム点群データを所定方位へと整列させ、得られた位置姿勢ごとの整列されたフレーム点群データを第二分割部21へと出力する。なお、図３等も参照して後述するように、当該整列させることはすなわち第一分割部11に入力される地図点群データと同じ座標系にフレーム点群データを変換することに相当する。第二分割部21は整列部20で得られた位置姿勢ごとのフレーム点群データをボクセル分割して、得られる位置姿勢ごと及びボクセルごとのフレーム点群データを第二計算部22へと出力する。第二計算部22は当該位置姿勢ごと及びボクセルごとのフレーム点群データに対して点群をモデル化する計算を行い、得られた位置姿勢ごと及びボクセルごとのフレーム点群モデル化データを比較部13へと出力する。なお、詳細は後述するように、第二計算部22及び第一計算部12ではモデル化する対象となるデータは互いに異なるものの、モデル化の仕方（すなわち、モデル化のための計算処理）は互いに共通である。

【0023】

比較部13は、第一計算部12から得られるボクセルごとの地図点群モデル化データと、第二計算部22から得られる位置姿勢ごと及びボクセルごとのフレーム点群モデル化データと、を比較し、比較結果を選別部14へと出力する。選別部14では当該得られた比較結果に基づいて第一計算部12から得られるボクセルごとの地図点群モデル化データの中から選別を行う。ここで、第一計算部12において得られているボクセルごとの地図点群モデル化データは従来手法と同様の特に選別されておらずデータ量が多いものに相当するのに対し、本発明の一実施形態においては比較部13及び選別部14（並びに整列部20、第二分割部21及び第二計算部22）の処理を経たうえで、第一計算部12の当該得た全データの中から後段側処理に適していると判定されるものを選別することにより、選別されなかったものを除外することによりデータ量を削減すると共に、当該選別されたデータを用いてなされる後段側処理の精度を確保するようにすることができるという効果がある。

【0024】

なお、「後段側処理」とは上記の通り、選別装置10によって選別された地図点群データをリファレンスデータ等として用いて行われる任意の処理を指すものであり、その典型例としては自己位置推定処理を挙げることができる。以下でもこれを単に「後段側処理」と称することとする。

【0025】

図２は一実施形態に係る選別装置10の動作のフローチャートである。ステップS1では、管理者等によるマニュアル作業等によって、選別装置10への入力データとしての地図点群データ及びこれを構成しているフレーム点群等のデータを用意したうえで、ステップS2へと進む。なお、当該用意される地図点群データはステップS2において第一分割部11へと入力されることとなり、また当該用意されるフレーム点群等のデータはステップS3において整列部20へと入力されることとなる。

【0026】

図３は、ステップS1で当該用意するデータの説明を含む、本発明の一実施形態に係る手法の説明のための模式図である。ステップS1で当該用意するデータの模式的な例として、図３では地図点群データMD及びこれを構成するフレーム点群データ（の一部分の例）D1,D2,D3が示されている。なお、点群データD1,D2,D3は図３では模式的に丸状の範囲として表現することにより、当該点群データが分布する範囲の形で示されている。

【0027】

ステップS1で用意する地図点群データMDとはすなわち、例えば前掲の非特許文献２に開示のSLAM技術によって用意することが可能なものである。図３の例であれば、時刻T1に点群取得センサが位置姿勢PA1にあることによって取得されるフレーム点群データD1と、時刻T2に点群取得センサが位置姿勢PA2にあることによって取得されるフレーム点群データD2と、時刻T3に点群取得センサが位置姿勢PA3にあることによって取得されるフレーム点群データD3と、といったように、各時刻Ti(i=1,2,3, ..., N)において位置姿勢PAi(i=1,2,3, ..., N)にある点群取得センサの取得したフレーム点群データDi(i=1,2,3, ..., N)を、図３に示すような何らかの共通座標系CSにおいて表現されるデータとして統合することにより、地図点群データMDが得られる。これは式で形式的に表記すれば以下のような関係となっている。

【0028】

【数1】

【0029】

(数式2)において、(x_ij, y_ij, z_ij)_[CS]はDiの点群データの各点の空間座標を共通座標系CSにおいて与えたものであり、J(i)はDiの点群データの総数である。

【0030】

そして、ステップS1で用意するフレーム点群データDiとは上記の(数式2)において与えられるのと同じものを、共通座標系CSにおいてではなく当該時刻Tiのフレームにおいて点群データを取得したセンサのセンサ座標系SS(Ti)において与えたもの（すなわち、センサが自身の空間座標系において取得した点群データそのもの）であり、以下の式のように表現することができる。

【0031】

【数2】

【0032】

すなわち、(数式2)及び(数式3)で表現されるフレーム点群データDiは内容としては同じものであるが、その各点の空間座標を表現するための座標系が互いに異なっているものである。

【0033】

ステップS2では、ステップS1で用意した地図点群データMDを第一分割部11及び第一計算部12において順次処理することで、地図点群データMDを所定のボクセルグリッドVG0で空間分割したうえで当該各ボクセルに属する点群をモデル化してから、ステップS3へと進む。なお、図３には地図点群データMDを当該ボクセル分割するための所定のボクセルグリッドVG0の模式例が示されている。

【0034】

ここで、以降の説明の便宜上、地図点群データMDのうち、所定のボクセルグリッドVG0で分割することによる位置(n,m,l)（整数n,m,lで空間上のx,y,z方向における位置を格子点のように指定した位置）のボクセル内にある点群データをMD(n, m, l)_[VG0]と表記する。すなわち、データ集合間の関係として「MD={MD(n, m, l)_[VG0]}」の関係がある。また、そのモデル化データをmodel(MD(n, m, l)_[VG0])と表記する。

【0035】

ステップS3では、ステップS1で用意した各フレーム点群データDiを、整列部20、第二分割部21及び第二計算部22において順次処理することにより、地図点群データNDの姿勢に一致させた後に、各データDiを1通り以上のボクセルグリッドVGk(k=0,1,2, ...)で分割したうえでそれぞれのボクセルグリッドVGk（当該ボクセルグリッドVGkはすなわち、分割の仕方・区切り方に対応している）に応じたボクセル内においてモデル化してから、ステップS4へと進む。

【0036】

ここで、ステップS3で分割するためのボクセルグリッドVGk(k=0,1,2,...)のうち少なくとも１つの場合としてk=0のボクセルグリッドVG0はステップS2におけるものと共通である。また、以降の説明の便宜上、ステップS2における表記と共通の表記により、ボクセルグリッドVGkで分割することによるフレーム点群データDiの位置(u,v,w)（すなわち、ステップS2の(n,m,l)の場合と同様に、整数u,v,wによって格子点のように指定される位置）のボクセル内にある点群データをDi(u,v,w)_[VGk]と表記する。すなわち、データ集合間の関係として「Di={Di(u,v,w)_[VGk]}」の関係がある。また、そのモデル化データをmodel(Di(u,v,w)_[VGk])と表記する。

【0037】

なお、以上のステップS2及びS3は、逆順に実行されてもよいし、並行して実施されてもよい。

【0038】

ステップS4では、ステップS2で得たモデル化データmodel(MD(n, m, l)_[VG0])とステップS3で得たモデル化データmodel(Di(u,v,w)_[VGk])とを比較部13が比較してからステップS5へと進む。ステップS5ではステップS4における当該比較結果に基づいて選別部14がステップS2で得たモデル化データmodel(MD(n, m, l)_[VG0])の中から選別を行い、当該図２のフローチャートは終了する。

【0039】

ここで、図１の各機能部の概略説明の際にも述べたように、当該比較選別によって、地図点群データMDから得られるボクセル単位での各モデル化データmodel(MD(n, m, l)_[VG0])はこれが実際に後段側処理に供された場合の性能を模擬的に評価したうえで選別されることとなり、従来手法ではなし得なかったボクセル単位での効率的なダウンサンプリング（後段側処理に悪影響を与えない、あるいは精度とのトレードオフ関係のもとであっても悪影響を最小限に抑制するダウンサンプリング）が可能となる。

【0040】

すなわち、当該比較選別の詳細は後述するが、当該ステップS4及びS5による比較選別の「考え方」は、地図点群データMDの全体をステップS1において用意した際の各フレームの点群データDiを、後段側処理においてクエリとなる点群として入力されるデータを模擬的に表しているものとして利用することに相当する。

【0041】

すなわち、図３の例であれば、位置姿勢PA1にある際に取得されたフレーム点群データD1は、そもそもは(数式1)に表現される通り地図点群データMDの全体を構成するための部分データであるが、本発明においてはこのような関係を利用することで、後段側処理で位置姿勢PA1にある際にクエリとして入力されるデータを点群データD1によっていわばシミュレートしている。そして、当該シミュレートされた点群データD1がよくマッチングするようなボクセルを地図点群データMDの中から選別することにより、従来手法では不可能であった効果的なボクセル単位でのダウンサンプリングを実現する。

【0042】

ここではさらに次のような効果もある。すなわち、地図点群データMDに該当するフィールド内を再度移動しながら点群データを取集して実際に後段側処理（ここでは自己位置推定処理を想定）を行なって実績調査などをする必要なく、（従って、そのような追加調査などのコストを要することなく、）地図点群データMD（及びこれを構成するフレーム点群データDi）の情報のみを利用することで、当該地図点群データMDに対して効果的なダウンサンプリングが実現されるという効果も奏することが可能である。

【0043】

以下、図２のように動作する図１の各機能部の詳細を説明する。なお、説明の便宜上、処理されるデータの表記に関して、図２の各ステップの説明の際に用いた表記（例えば、モデル化データmodel(MD(n, m, l)_[VG0])など）とは別の表記で説明する場合があるが、その対応関係は適宜示すこととする。

【0044】

[整列部20]
整列部20は、所与の姿勢変換行列を用いて各フレーム点群データDiを姿勢変換することにより、各フレーム点群データDiの姿勢を地図点群データMDにおける姿勢と一致させる。当該姿勢変換はすなわち、フレーム点群データDiに関して、(数式3)のように取得した際のセンサの座標系SS(Ti)で表現されていたものを、(数式2)のように地図点群データMDの共通座標系CSで表現されるものに変換することに相当し、姿勢変換行列はフレーム点群データDiごとに存在する。

【0045】

なお、各フレーム点群データDiに関して予め(数式2)の共通座標系CSで表現されるものとして入力データ（図２のステップS1で用意する入力データ）を用意しておくことにより、整列部20を省略してもよい。

【0046】

[第一分割部11]
第一分割部11は、地図点群データMDについて、任意の所定の基準点rp^map（例えばローカル座標系の原点）を元に、空間を所定のサイズのボクセルで分割する。ただし、当該処理におけるボクセルのサイズは、後段側処理で使用されるボクセルのサイズと同サイズであることが望ましい。ボクセル分割する際の方向は共通座標系CSのx,y,z軸とすればよい。ここで得られたM個のボクセルを、V_p^map (p=1,2, …,M)とする。（各ボクセルV_p^mapはすなわち、図２の説明におけるMD(n, m, l)_[VG0]に該当し、ボクセルグリッドVG0で分割されたものに該当する。）

【0047】

[第二分割部21]
図４は、第二分割部21による分割の各実施形態を担うものとしての第二分割部21の機能ブロック図である。第二分割部21は、共通分割部1、平行移動分割部2及び回転分割部3を備え、各部1〜3による各実施形態においてボクセルグリッドVGk(k=0,1,2, ...)で分割された各フレーム点群データDiが第二計算部22へと出力される。各部1〜3で分割するボクセルのサイズは第一分割部11において分割したボクセルサイズと共通である。

【0048】

[共通分割部1]
共通分割部1では、第一分割部11でボクセル分割したのと同一のボクセルグリッドVG0によって各フレーム点群データDiをボクセル分割する。（従って、分割する際の基準点は同一の基準点rp^mapであり、ボクセル分割する際の方向も同一で例えば共通座標系CSのx,y,z軸とすればよい。）

【0049】

共通分割部1で各フレーム点群データDiに関して得られたL(i)個のボクセルをV_l^i,a(l=1,2, ..., L(i))と表記する。なお、添字aは次に説明する「非対称性」(asymmetry)を表すものであり、平行移動分割部2及び回転分割部3でのボクセルと区別するためのものである。（なお、当該ボクセルV_l^i,aにおけるDiの点群データが、ボクセルを指定するlと(u,v,w)との対応付けのもとで、図２の説明におけるDi(u,v,w)_[VG0]に該当する。）

【0050】

共通分割部1の役割は、後段側の比較部13及び選別部14において、地図点群データMDのボクセルと各フレーム点群データDiのボクセルとの間で、点群分布の非対称性に基づく性能評価による比較選別を可能とするようなフレーム点群データDiのモデル化データを、第二計算部22において算出可能とさせるためのものである。

【0051】

図５は共通分割部1による当該役割における点群分布の非対称性を説明するための模式例を示す図である。図５では、同一のオブジェクトOBに関して、[1]では後段側処理においてクエリとして取得されることが想定される点群を、[2]では地図点群データMDにおいて取得していることが想定される点群を、それぞれ模式例として示している。すなわち、[1]に示す後段側処理での取得点群（及びこれをシミュレートする各フレーム点群データDi）は、単一のセンサ位置姿勢AP11のみで取得されるものであるため、球状のオブジェクトOBに関してその前面部分（図中の実線部分）しか点群が取得されず、背面部分（図中の点線部分）に関しては点群が取得されないこととなる。一方、[2]に示すように同一のオブジェクトOBに関して地図点群データMDにおいては単一のセンサ位置姿勢AP11のみならずその他のセンサ位置姿勢AP12,AP13等からも網羅的にその点群が取得され、図中に実線で示すような球面の全面に渡って点群が取得されることとなる。このように、同一のオブジェクトOBであっても取得される点群の様子が異なることが「非対称性」である。

【0052】

当該非対称性は、より一般的に述べれば次の通りである。前述の(数式1)の通り、地図点群データMDはオブジェクトを複数の計測点から計測した各結果Diを統合して得られるものであるため、オブジェクトの形状を完全に近い状態で計測しており、得られる点群の密度も高くなる。それに対し、後段側処理の取得点群は物体を単一の計測点から計測した結果Diのみであるため、物体の一面しか計測することができず、不完全かつ疎な計測となる。

【0053】

このような非対称性は、地図点群データと後段側処理の取得点群のそれぞれを取得した際の位置姿勢が完全に一致している場合であっても、ボクセル内に位置する点群をモデル化したパラメータに差異をもたらすこととなる。さらに、ノイズの影響でモデルパラメータに差異が生じる可能性もある。その様なボクセルは、正しい位置姿勢であっても自己位置推定処理に悪影響を及ぼすため、除去すべき有害なボクセルであると考えられる。

【0054】

そして、共通分割部1によるボクセル分割を経た比較選別の処理によれば、このような非対称性に基づく有害なボクセルの自動排除、すなわち選別部14で選別されないようにすることが可能となる。図５の例であれば、[1]に示すオブジェクトOBの裏側（点線部分）に該当するようなボクセルは、自動排除の対象となる。

【0055】

以上、共通分割部1を説明した。なお、各部1〜3による各実施形態においてボクセルグリッドVGk(k=0,1,2, ...)で分割することとなるが、共通分割部1はk=0の場合のボクセルグリッドVG0（第一分割部11と同じボクセルグリッドVG0）の分割を行うものであり、以降に説明する平行移動分割部2及び回転分割部3はこれとは異なるk≧1で指定されるボクセルグリッドVGk(k≧1)の分割を行うものである。

【0056】

[平行移動分割部2]
平行移動分割部2に関して、その処理内容（すなわちボクセル分割の仕方の詳細）を説明してから、その後段側の比較選別処理における意義を説明することとする。

【0057】

平行移動分割部2では、共通分割部1で分割したボクセルグリッドVG0からサブボクセル単位の長さ（ボクセル解像度よりも細かいステップ幅）でボクセル分割基準位置が異なっているような一連のボクセルグリッドVGk(k≧1)の分割を行う。すなわち、共通分割部1で分割した際の基準点rp^mapからボクセルの解像度よりも細かいサイズのステップ幅による平行移動された基準点を用いて、各フレーム点群データDiをボクセルで分割する。具体的には、x軸方向，y軸方向，z軸方向のボクセルサイズをvlx，vly，vlzとし、ステップ幅をそれぞれrsx，rsy，rszとおく場合、それぞれでvlx，vly，vlzを除算した際の商の大きさのパターン数rsnx，rsny，rsnzで、各地図フレームをボクセルで分割する；
rsnx = vlx / rsx （数式4）
rsny = vly / rsy （数式5）
rsnz = vlz / rsz （数式6）
この時、各フレーム点群データDiは、Num = rsnx * rsny * rsnz 通りの方法で、ボクセルで分割されることとなる。あるいは、例えばx方向のみにNum = rsnx通り、という様に、ステップ幅を施す方向を限定してもよい。

【0058】

平行移動分割部2でNum通りの各分割において各フレーム点群データDiに関して得られたL2(i)個のボクセルを｛V_l^i,q(l=1,2, ..., L2(i))｝_n(n=1,2, ..., Num)と表記する。（なお、個数L2(i)に関しては、Num通りのそれぞれへの依存性があるが、表記簡略化の観点から当該依存性の表記はL2(i)においてではなく、分割手法を表現する最後の添え字nにおいて表現している。）添え字qは次に説明する量子化誤差（quantization error）を意味するものである。

【0059】

平行移動分割部2の役割は、後段側の比較部13及び選別部14において、地図点群データMDのボクセルと各フレーム点群データDiのボクセルとの間で、点群分布の量子化誤差に基づく性能評価による比較選別を可能とするようなフレーム点群データDiのモデル化データを、第二計算部22において算出可能とさせるためのものである。

【0060】

すなわち、モデル化の際に連続的な幾何的形状を計測した3D点群をボクセルで区切ることは、データのボクセル量子化誤りに起因する問題をもたらす。データのボクセル量子化誤りを説明するために、模式図を図６に示す。図６において、楕円体はある点群PG1のデータ分布を模式的に表し、直線で形成された立方体V11,V12,V21,V22は同じ大きさのボクセルを表している。ここで、図６の[1]及び[2]の間では、点群PG1の座標は変化せず、（すなわち[1]及び[2]では点群PG1データは同一のものが示されており、）ボクセルを形成する基準となる基準点の座標のみが基準点R1及び基準点R2のように変化している。すなわち図６は、同一の点群PG1データを[1]及び[2]において互いに異なるボクセルで量子化する場合の例を示しており、互いに共通の座標系CS1で同一の点群PG1データを与え、[1]ではその原点Oに一致する位置に基準点R1を設けてボクセル量子化し、[2]では当該原点Oから+x方向に一定距離だけずれた位置に基準点R2を設けてボクセル量子化している。

【0061】

そして、図６の[1]にてボクセル量子化された点群PG1データは2つのボクセルV11,V12の境界にまたがって概ね２等分される形で割り当てられているのに対して、図６の[2]のボクセル量子化では同じ点群PG1データの全てが２つのボクセルV21,V22のうち左側のボクセルV21のみに割り当てられている。結果として図６の[1]及び[2]の間で、同一の点群PG1データをモデル化しているのにも関わらず、そのモデル化の結果（例えば平均，分散共分散行列）は大きく変化してしまい、得られるモデルのパラメータも類似しないものとなってしまう。

【0062】

図６はボクセル分割位置の少しの違いでモデルのパラメータが顕著に変化してしまう場合の模式例であったが、逆に図７に模式図を示すように、ボクセル分割位置の少しの違いではモデルのパラメータがほとんど変化しない場合もある。

【0063】

すなわち、図７でも図６と同様に、同一の点群データPG2に関して[1]及び[2]で互いにボクセル分割位置がわずかに異なる場合が示されている。図７にて[1]では基準点R3を基準としてボクセル分割されたボクセルV31,V32が示され、[2]では基準点R3からわずかにずれた基準点R4を基準としてボクセル分割されたV41,V42が示されている。ここで、図６の場合とは逆に、図７の点群データPG2は[1],[2]のボクセルV31,V32とボクセルV41,V42との間でモデル化データにほとんど相違を発生させないような分布をなしている。

【0064】

後段側処理においては、その具体的な用途・目的等によって図６のような少しの分割位置の相違で顕著に変化するような位置にあるボクセルが精度上重要になる場合と、逆に図７のような少しの分割位置の相違ではほとんど変化しないような位置にあるボクセルが精度上重要になる場合との両方が考えられる。平行移動分割部2の処理を前提とした後段側の比較選別処理の各実施形態によって、この各場合に適した選別が可能となる。

【0065】

[回転分割部3]
回転分割部3は、上記の平行移動分割部2がボクセル分割の基準位置の「並進成分」の相違がボクセル分割に基づくモデル化データに与える影響を考慮するためのものであったのに対し、ボクセル分割の「方向」の相違がモデル化データに与える影響を考慮するためのものである。従って、回転分割部3に関して図６，７のような模式図を示すことは省略するが、その考え方としては全く同様であり、ボクセル分割の方向のわずかな相違（及び／又はある程度大きな相違）によって顕著に変化する又はほとんど変化しないようなボクセルが後段側処理の精度において重要になる場合へそれぞれ対処可能とするのが回転分割部3の役割である。

【0066】

回転分割部3では具体的に、共通分割部1で分割した1通りのボクセルグリッドVG0と、平行移動分割部2で分割したNum通りのボクセルグリッドVGk(k≧1)と、のそれぞれを、回転角度が0°の基準姿勢にあるものとし、これらにそれぞれ所定のNum2通りの回転変換を施した一連のボクセルグリッドにおいて、各フレーム点群データDiをボクセル分割する。すなわち、回転分割部3でのボクセル分割のパターン数は(1+Num)*Num2通りとなる。

【0067】

回転分割部3でNum2通りの各回転変換において各フレーム点群データDiに関して得られたL3(i)個のボクセルを｛V_l^i,r(l=1,2, ..., L3(i))｝_m(m=1,2, ..., Num2)と表記する。（なお、個数L3(i)に関しては、Num2通りのそれぞれへの依存性があるが、表記簡略化の観点から当該依存性の表記はL3(i)においてではなく、分割手法を表現する最後の添え字nにおいて表現している。）添え字rは回転（rotation）を意味するものである。

【0068】

なお、回転分割部3での回転であるが、図８に模式例を示すように、回転角度が0°の基準姿勢の各ボクセル分割（1+Num通り）に関して、そのボクセルの中心位置を基準として回転させてボクセルを得るものとする。すなわち、図８では[1]に回転角度が0°の基準姿勢の場合の4個のボクセルが模式的に示され、これに対して共通座標系CSにおいてz軸回りに30°の回転を行ったものの模式例が[2]に、z軸回りに45°の回転を行ったものの模式例が[3]に示されている。なお、回転分割部3での回転は所定の回転行列をNum2通り予め定義しておいたものを利用すればよい。ここで、後段側処理での精度向上のために個別に必要となる処理（例えば自己位置推定における初期位置姿勢の姿勢補正として必要な処理）に応じて、何らかの１つ以上の回転軸の周りにおいて、0°〜360°のような大きな範囲内で概ね均一に回転角を割り振ったものとして複数の回転行列を用意しておいてもよいし、何らかの１つ以上の回転軸の周りにおける0°近傍の微小範囲（例えば±5°の範囲など）内で概ね均一に回転角を割り振ったものとして複数の回転行列を用意しておいてもよい。

【0069】

図８のようにボクセル中心位置で回転する場合、回転されたボクセル同士に重複箇所が生じたり、回転されたボクセル間に隙間が発生したりする場合があるが、当該回転された各ボクセルにおいて独立に、各フレーム点群データDiから点群を取得するものとする。すなわち、ボクセルの重複位置においては重複している各ボクセルに点群がそれぞれ属することとなり、そのうちのいずれか１つに属することはない。また、ボクセルの隙間位置にある点群に関してはいずれのボクセルにも属さないものとして扱う。

【0070】

以上、図４の第二分割部21の各部を説明した。当該各部を組み合わせることによる一連のボクセルグリッドVGk(k=0,1,2, ...)による分割に関しては、以下の３つの実施形態が可能であり、後述する比較選別の詳細説明（比較部13及び選別部14の詳細説明）も、当該３つの実施形態に応じたそれぞれを説明する。
（１）第一実施形態…共通分割部1での1通りのボクセルグリッドVG0のみの分割を適用する。
（２）第二実施形態…上記第一実施形態の共通分割部1での1通りのボクセルグリッドVG0に加えてさらに、平行移動分割部2による一連のボクセルグリッドVGk(k≧1)の分割も適用する。
（３）第三実施形態…上記第二実施形態の一連のボクセルグリッドによる分割に加えてさらに、回転分割部3による一連のボクセルグリッドの分割も適用する。

【0071】

[第一計算部12及び第二計算部22]
概略説明において既に述べたように、第一計算部12は、地図点群データMDを第一分割部11で1通りのボクセルグリッドVG0によって分割された各ボクセル単位でモデル化する。また、第二計算部22は各フレーム点群データDiを第二分割部21で各実施形態に応じて分割された各ボクセル単位でモデル化する。

【0072】

ここで、第一計算部12で得られるボクセル単位での地図点群モデル化データは、ボクセルの各位置に対応して1個のみ得られるものであるのに対し、第二計算部22で得られるボクセル単位での各フレーム点群データDiのモデル化データは、ボクセルとしては同じ位置あるいは重複する位置を占めていても1個以上の異なるものが得られるものである、という相違が存在することを特筆する。

【0073】

例えば、図３の模式例ではフレーム点群データD1とフレーム点群データD2とで重複するボクセルがボクセルグリッドVG0において存在している。図２のフローチャートの説明の際の表記により、当該同じ位置にあるボクセル位置の１つを(u0,v0,w0)とすると、当該同じ位置(u0,v0,w0)にあるボクセルであっても、フレーム点群データD1のデータをモデル化したモデル化データmodel(D1(u0,v0,w0)_[VG0])と、フレーム点群データD2のデータをモデル化したモデル化データmodel(D2(u0,v0,w0)_[VG0])と、の２つの異なるモデル化データが得られることとなる。

【0074】

上記の例は第二分割部21で第一実施形態を適用したものとして、同じ位置となるボクセルで異なるモデル化データが得られる場合があることを説明した。第二実施形態又は第三実施形態が適用される場合はさらに同様に、（完全に同じ位置ではないが）重複する位置となるボクセルで異なるモデル化データが得られる場合があることとなる。

【0075】

第一計算部12及び第二計算部22では以上のようなそれぞれが対象とするボクセル（分割手法に応じたボクセル）及びその内部の点群データをモデル化する。具体的な当該モデル化の手法に関しては、任意の既存のモデル化手法が利用できる。例えば前掲の非特許文献３のように、各ボクセル内に位置する点群を3次元多変量正規分布のパラメータ、すなわち平均と分散共分散行列としてモデル化してもよい。データ量削減及び後段側の比較選別の高速化のために当該モデル化はデータの量子化を含むものであってもよい。

【0076】

なお、第一計算部12及び第二計算部22では、ボクセル内の対象データの点群の数が所定閾値以下となる場合には、（モデル化しても信頼性を欠くものとして、）モデル化を行うことを省略してもよい。当該省略されたボクセルに関しては、最初から点群の数がゼロであるボクセルと同様に、モデル化データが存在しないものとして扱えばよい。

【0077】

[比較部13]
比較部13は、第一処理として、地図点群MDのモデル化データの各ボクセルと各フレーム点群データDiのモデル化データの各ボクセルとの間において、同一空間に対応するボクセル同士又は大よそ同一の空間を含んでいるボクセル同士の持つモデル同士の距離を特定の尺度で計算したうえで、当該距離計算結果に基づいて第二処理として、地図点群MDのモデル化データの各ボクセルに関してデータ選別のための指標を求め、当該指標を比較結果として選別部14へと出力する。

【0078】

第一処理におけるモデル同士の距離の計算に関しては、第一計算部12及び第二計算部22で具体的に計算したモデルの種類に応じた所定の距離（モデル同士が類似しているほどその値（非負の値）が小さくなるような距離）として計算すればよい。例えば前掲の非特許文献３のモデルを計算した場合であれば、多変量正規分布間のL2距離として計算すればよい。

【0079】

また、第一処理におけるモデル同士の距離の計算に関しては、第二分割部21で各フレーム点群データDiに関して適用したボクセル分割の種別ごとに種類が区別される距離として計算を行うものとする。（すなわち、距離の計算の仕方そのものは同じであるが、ボクセル分割のされ方の違いに応じた種類の区別を設けるものとする。）すなわち、第一実施形態の場合、共通分割部1による分割のもとでの「非対称性」を考慮する距離d^a_p(p=1, 2, ...., M)のみの１種類の距離を計算し、第二実施形態ではさらに平行移動分割部2による分割のもとでの「並進成分の違いによる量子化誤差」を考慮する距離d^q_p(p=1, 2, ...., M)を追加した２種類の距離を計算し、第三実施形態ではさらに回転分割部3による分割のもとでの「回転成分の違いによる量子化誤差」を考慮する距離d^r_p(p=1, 2, ...., M)を追加した３種類の距離を計算するものとする。

【0080】

なお、上記の距離d^q_p(p=1, 2, ....,M)等におけるpは、第一分割部11の説明の際に用いた地図点群MDのモデル化データの各ボクセルV_p^map (p=1,2, …,M)を識別するものである。

【0081】

さらに、第一処理における距離計算の対象となるボクセル同士の対応付けに関しては、共通分割部1による分割の場合は、地図点群MDのモデル化データの各ボクセルと各フレーム点群データDiのモデル化データの各ボクセルとは同じボクセルグリッドVG0で分割されたものであるため、同位置のボクセル同士を距離計算対象として対応付ければよい。平行移動分割部2又は回転分割部3による分割の場合、地図点群MDのモデル化データの各ボクセルと各フレーム点群データDiのモデル化データの各ボクセルとは一般に位置がずれたものとなっているので、最も重複が多いボクセル又は最も中心間の距離が小さいボクセルを距離計算対象として対応付ければよい。

【0082】

さらに、上記対応付けた際に、地図点群MDのモデル化データの各ボクセルに関して、対応付けられたボクセル数をカウントしておくものとする。「非対称性」を考慮する距離d^a_p(p=1, 2, ...., M)の計算対象として対応付けたボクセル数をN_aとし、「並進成分の違いによる量子化誤差」を考慮する距離d^q_p(p=1, 2, ...., M)の計算対象として対応付けたボクセル数をN_qとし、「回転成分の違いによる量子化誤差」を考慮する距離d^r_p(p=1, 2, ...., M)の計算対象として対応付けたボクセル数をN_rとする。

【0083】

ここで、「p」で識別される地図点群MDのモデル化データのある一つのボクセルV_p^map (p=1,2, …,Mのいずれか)に関して、距離d^a_pは対応付けられたフレーム点群データDiのモデル化データのボクセルN_a個に関してそれぞれ計算され、距離d^q_pは対応付けられたフレーム点群データDiのモデル化データのボクセルN_q個に関してそれぞれ計算され、距離d^r_pは対応付けられたフレーム点群データDiのモデル化データのボクセルN_r個に関してそれぞれ計算されるものであり、当該個数N_a個、N_q個及びN_r個は2以上となる場合も（一般に頻繁に）あることを特筆する。後述する(数式7)、(数式8)及び(数式9)での和は、このように対応付けられたそれぞれN_a個、N_q個及びN_r個に関する和である。

【0084】

第二処理では、上記対応付けられたN_a個、N_q個及びN_r個の各距離をそれぞれ累積加算し、且つ、当該各個数で以下の(数式7)、(数式8)及び(数式9)のように当該累積加算和を正規化することにより、「p」で指定される地図点群MDのモデル化データの各ボクセルV_p^mapに関して、「非対称性」を考慮する正規化された距離d^norm,a_p、「並進成分の違いによる量子化誤差」を考慮する正規化された距離d^norm,q_p及び「回転成分の違いによる量子化誤差」を考慮する正規化された距離d^norm,r_pをそれぞれ指標として求める。
d^norm,a_p=Σd^a_p/N_a （数式7）
d^norm,q_p=Σd^q_p/N_q （数式8）
d^norm,r_p=Σd^r_p/N_r （数式9）

【0085】

[選別部14]
選別部14では、上記(数式7)、(数式8)及び(数式9)の指標を用いて、「p」で指定される地図点群MDのモデル化データの各ボクセル各ボクセルV_p^mapの中から選別を行う。

【0086】

第一実施形態においては、前述の通り共通分割部1による分割VG0のみが適用されることから、(数式7)の「非対称性」を考慮する正規化された距離d^norm,a_pのみが求まっている。そこで、選別部4では当該正規化距離が閾値TH11以下となるような一連のボクセルV_p^map又は当該正規化距離が小さい順からの順位が閾値TH12以内に収まるような一連のボクセルV_p^mapを選別対象とすればよい。

【0087】

第二実施形態においては、上記第一実施形態で選別対象となった一連のボクセルV_p^mapの中から、さらに（数式8）の「並進成分の違いによる量子化誤差」を考慮する正規化された距離d^norm,q_pに基づく選別を行ったものを最終的な選別結果とすればよい。なお、第二実施形態においては第一実施形態の閾値TH11,TH12を第一実施形態におけるものとは別の値として設定してもよい。

【0088】

ここで、「並進成分の違いによる量子化誤差」を考慮する正規化された距離d^norm,q_pに基づく選別に関しては、後段側処理で何を重視するかの考え方の違いにより以下の2通りの実施形態（A）、（B）が可能である。

【0089】

（A）前述の図６の例のように量子化するボクセルグリッドの位置がわずかに異なることで顕著に変化するようなボクセルのデータを後段側処理において重要視する場合、正規化された距離d^norm,q_pが閾値判定で小さいと判定されるもの、又は正規化された距離d^norm,q_pが小さい側の上位所定数を選別対象とすればよい。

【0090】

（B）逆に、前述の図７の例のように量子化するボクセルグリッドの位置がわずかに異なったとしてもほとんど変化しないようなボクセルのデータを後段側処理において重要視する場合、正規化された距離d^norm,q_pが閾値判定で大きいと判定されるもの、又は正規化された距離d^norm,q_pが大きい側の上位所定数を選別対象とすればよい。

【0091】

第三実施形態においては、上記第二実施形態で選別対象となった一連のボクセルV_p^mapの中から、さらに（数式9）の「回転成分の違いによる量子化誤差」を考慮する正規化された距離d^norm,r_pに基づく選別を行ったものを最終的な選別結果とすればよい。なお、第三実施形態においては第二実施形態の各閾値を第二実施形態で設定したものとは別の値として設定してもよい。

【0092】

ここで、同じく量子化誤差を考慮している第二実施形態と全く同様に、「回転成分の違いによる量子化誤差」を考慮する正規化された距離d^norm,r_pに基づく選別に関しては、後段側処理で何を重視するかの考え方の違いにより以下の2通りの実施形態（A）、（B）が可能である。

【0093】

（A）前述の位置に関する図６の例における量子化するボクセルグリッドの位置に代えて、これと同様に回転成分がわずかに異なることで顕著に変化するようなボクセルのデータを後段側処理において重要視する場合、正規化された距離d^norm,r_pが閾値判定で小さいと判定されるもの、又は正規化された距離d^norm,r_pが小さい側の上位所定数を選別対象とすればよい。

【0094】

（B）逆に、前述の位置に関する図７の例における量子化するボクセルグリッドの位置に代えて、これと同様に回転成分がわずかに異なったとしてもほとんど変化しないようなボクセルのデータを後段側処理において重要視する場合、正規化された距離d^norm,r_pが閾値判定で大きいと判定されるもの、又は正規化された距離d^norm,r_pが大きい側の上位所定数を選別対象とすればよい。

【0095】

以上、本発明の選別装置10によれば、後段側処理の精度等を確保したうえで効果的に当該後段側処理に供するデータ量を削減するようにすることが可能である。以下、本発明の説明上の補足を何点か述べる。

【0096】

（１）以上の説明より明らかなように、第二分割部21に関して、第四実施形態として、共通分割部1及び回転分割部3の分割を適用し、平行移動分割部2の分割適用は省略するようにすることも可能である。この場合、回転分割部3では具体的に、共通分割部1で分割した1通りのボクセルグリッドVG0のみを回転角度が0°の基準姿勢にあるものとし、これらにそれぞれ所定のNum2通りの回転変換を施した一連のボクセルグリッドにおいて、各フレーム点群データDiをボクセル分割する。すなわち、回転分割部3でのボクセル分割のパターン数はNum2通りとなる。（前述のNum=0とすればよい。）比較選別処理も同様に当該省略された場合に対して実施すればよい。

【0097】

（２）本発明は後段側処理の典型例として自己位置推定を想定し、地図点群データMDをボクセル化してのモデル化データのデータ量削減を実現することを説明してきた。しかし、本発明は必ずしも現実の都市空間等のフィールドにおいて取得された地図点群データのみならず、任意の対象における参照点群データを対象としても全く同様に実施可能である。ただしこの場合も、フレーム点群データDi及びその位置姿勢PAiに相当する情報は、当該任意の参照点群データに付随する情報として用意しておく必要がある。

【0098】

（３）選別装置10は一般的な構成のコンピュータとして実現可能である。すなわち、CPU（中央演算装置）、当該CPUにワークエリアを提供する主記憶装置、ハードディスクやSSDその他で構成可能な補助記憶装置、キーボード、マウス、タッチパネルその他といったユーザからの入力を受け取る入力インタフェース、ネットワークに接続して通信を行うための通信インタフェース、表示を行うディスプレイ、カメラ及びこれらを接続するバスを備えるような、一般的なコンピュータによって選別装置10を構成することができる。さらに、図１に示す選別装置10の各部の処理はそれぞれ、当該処理を実行させるプログラムを読み込んで実行するCPUによって実現することができるが、任意の一部の処理を別途の専用回路等（GPUを含む）において実現するようにしてもよい。

【符号の説明】

【0099】

10…選別装置、11…第一分割部、12…第一計算部、13…比較部、14…選別部、20…整列部、21…第二分割部、22…第二計算部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】