JP6281938B2 - データ処理装置、データ処理方法、及びデータ処理プログラム - Google Patents

Description

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

動作環境において位置を推定する技術（例えば、対環境自己位置推定）や、周囲の環境に関する情報を把握する技術（例えば、探索環境地図構築）は、以前から提案され、様々な分野（例えば、移動ロボットの動作制御）への応用が試みられている。周囲の環境として周囲に位置している種々の物体の表面の形状を示す形状データを取得する３次元測域距離センサ（３ＤＬＲＦ：Ｔｈｒｅｅ Ｄｉｍｅｎｓｉｏｎａｌ Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）が用いられることがある。３ＤＬＲＦは、物体の表面を所定の空間間隔（精度）でサンプリングしたサンプル点からなる点群（ｐｏｉｎｔ ｃｌｏｕｄ）データ（点群情報）を取得する。その空間間隔は、例えば、数ｍｍ以下の高精度なものもある。自己位置推定（ｌｏｃａｌｉｚａｔｉｏｎ）や、地図構築（ｍａｐｐｉｎｇ）において、互いに相対的な位置や姿勢（相対位置姿勢）の関係が不明な２つの点群が重なり合うように方向や位置を合わせる処理（ｍａｔｃｈｉｎｇ：位置合わせ）が必要になることがある。

例えば、特許文献１に記載の自己位置推定装置は、２つの点群の位置合わせを行う際、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法を用いる。ＩＣＰ法は、反復計算により２つの点群の間の距離を最小化するアルゴリズムであり、複雑な形状を示す点群についても実行可能であるという汎用性を有する。

特開２０１１−８３２０号公報

しかしながら、ＩＣＰ法は、反復解法であるため、その計算処理に多くの時間を要する。そのため、実時間での処理が求められる用途、例えば、移動ロボットの動作の制御に応用することは現実的ではない。たとえ平面度の高い面を多く含む物体から取得された点群同士の位置合わせに利用されたときでも処理時間が短くなるとは限らない。このような点群は、特に室内等の人工的な環境で現れることが多い。
また、ＩＣＰ法による処理結果は、点群を構成する各サンプル点の初期位置に対する依存性が強く、局所解に陥る傾向がある。つまり、期待される位置合わせの結果が得られる初期位置の範囲が狭いことを意味する。これについて、ＲＡＮＳＡＣ（Ｒａｎｄｏｍ Ｓａｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）法を採用して、各サンプル点の初期位置をランダムに設定することも考えられるが、計算時間がさらに増加してしまう。

本発明は上記の点に鑑みてなされたものであり、２つのデータがそれぞれ示す物体間の方向や位置を合わせるための処理量を軽減することができ、初期位置に対する依存性を低くすることができるデータ処理装置、データ処理方法、及びデータ処理プログラムを提供する。

（１）本発明は上記の課題を解決するためになされたものであり、本発明の一態様は、第１のデータが示す第１の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第１の法線主成分として算出する主成分算出部と、第２のデータが示す第２の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第２の法線主成分から、前記第１の法線主成分との近似の度合いに基づいて前記第１の法線主成分に対応する第２の法線主成分を定める対応部と、前記第１の法線主成分の方向と前記第２の法線主成分の方向との間の相対的な方向を算出する相対方向算出部と、を備え、前記主成分算出部は、前記第１の物体の表面までの原点からの深さを大きさとして有する法線である第１の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第１の壁面主成分として算出し、前記対応部は、前記第２の物体の表面までの原点からの深さを大きさとして有する法線である第２の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第２の壁面主成分から、前記第１の壁面主成分との近似の度合いに基づいて前記第１の壁面主成分に対応する第２の壁面主成分を定め、前記第１の壁面主成分と、当該第１の壁面主成分に対応する第２の壁面主成分との差分に基づいて前記第１の物体と前記第２の物体との間の相対的な位置を算出する相対並進位置算出部を、さらに備えるデータ処理装置である。

（２）本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第１の物体の形状を表す複数の点からなる第１の点群データに基づいて前記第１の物体の表面の法線のうち、予め定めた密度よりも高密度で分布している領域における法線の重心を前記第１の法線主成分として算出する。

（３）本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第１の物体の表面の法線を、当該第１の物体の表面の法線間の距離に基づいて複数の法線の群に分類し、各群に属する法線毎に前記第１の法線主成分を算出する。

（４）本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第１の物体の形状を表す複数の点からなる第１の点群データに基づいて前記第１の物体の表面の深さ付き法線のうち、予め定めた密度よりも高密度で分布している領域における深さ付き法線の重心を前記第１の壁面主成分として算出する。

（５）本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第１の物体の表面の深さ付き法線を、当該第１の物体の表面の深さ付き法線間の距離に基づいて複数の深さ付き法線の群に分類し、各群に属する深さ付き法線毎に前記第１の壁面主成分を算出する。

（６）本発明のその他の態様は、データ処理方法において、第１のデータが示す第１の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第１の法線主成分として算出する主成分算出過程と、第２のデータが示す第２の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第２の法線主成分から、前記第１の法線主成分との近似の度合いに基づいて前記第１の法線主成分に対応する第２の法線主成分を定める対応過程と、前記第１の法線主成分の方向と前記第２の法線主成分の方向との間の相対的な方向を算出する相対方向算出過程と、を有し、前記主成分算出過程は、前記第１の物体の表面までの原点からの深さを大きさとして有する法線である第１の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第１の壁面主成分として算出し、前記対応過程は、前記第２の物体の表面までの原点からの深さを大きさとして有する法線である第２の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第２の壁面主成分から、前記第１の壁面主成分との近似の度合いに基づいて前記第１の壁面主成分に対応する第２の壁面主成分を定め、前記第１の壁面主成分と、当該第１の壁面主成分に対応する第２の壁面主成分との差分に基づいて前記第１の物体と前記第２の物体との間の相対的な位置を算出する相対並進位置算出過程を、さらに有するデータ処理方法である。

（７）本発明のその他の態様は、データ処理装置のコンピュータに、第１のデータが示す第１の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第１の法線主成分として算出する主成分算出手順と、第２のデータが示す第２の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第２の法線主成分から、前記第１の法線主成分との近似の度合いに基づいて前記第１の法線主成分に対応する第２の法線主成分を定める対応手順と、前記第１の法線主成分の方向と前記第２の法線主成分の方向との間の相対的な方向を算出する相対方向算出手順と、を実行させるためのデータ処理プログラムであって、前記主成分算出手順は、前記第１の物体の表面までの原点からの深さを大きさとして有する法線である第１の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第１の壁面主成分として算出し、前記対応手順は、前記第２の物体の表面までの原点からの深さを大きさとして有する法線である第２の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第２の壁面主成分から、前記第１の壁面主成分との近似の度合いに基づいて前記第１の壁面主成分に対応する第２の壁面主成分を定め、前記第１の壁面主成分と、当該第１の壁面主成分に対応する第２の壁面主成分との差分に基づいて前記第１の物体と前記第２の物体との間の相対的な位置を算出する相対並進位置算出手順を、さらに実行させるためのデータ処理プログラムである。

本発明によれば、２つのデータがそれぞれ示す物体間の方向や位置を合わせるための処理量を軽減することができ、初期位置に対する依存性を低くすることができる。

本発明の第１の実施形態に係るデータ処理装置の構成を示すブロック図である。 点群データの一例を示す図である。 法線ベクトルの一例を示す。 本実施形態に係るデータ処理を示すフローチャートである。 本発明の第２の実施形態に係るデータ処理装置の構成を示すブロック図である。 深さ情報付き法線ベクトルの一例を示す。 本実施形態に係るデータ処理を示すフローチャートである。 モデル点群の一例を示す図である。 誤差の初期位置依存性を示す図である。 処理時間の初期位置依存性を示す図である。 点群データを取得した環境の一例を示す写真である。 処理前の点群データの一例を示す図である。 処理後の点群データの一例を示す図である。

（第１の実施形態）
以下、図面を参照しながら本発明の実施形態について説明する。
図１は、本実施形態に係るデータ処理装置１の構成を示すブロック図である。
データ処理装置１は、データ入力部１０１、主成分算出部１０２、対応部１０３、回転算出部（相対方向算出部）１０４、データ出力部１０５、及び記憶部１１を含んで構成され、各部はバス１２で接続されている。

データ入力部１０１には、データ処理装置１の外部、例えば、３ＤＬＲＦからクエリ点群データ（第１のデータ）が入力される。点群データは、予め定めた撮像領域に含まれる被写体（物体）の形状を所定の空間間隔でサンプリングしたサンプル点からなる点群を示すデータである。入力されたクエリ点群データが示す点群をクエリ点群（ｑｕｅｒｙ ｐｏｉｎｔ ｃｌｏｕｄ）と呼び、クエリ点群を構成するサンプル点をクエリ点と呼ぶことがある。各サンプル点は、被写体（第１の物体）の表面の１点の座標を示す。データ入力部１０１は、入力されたクエリ点群データを主成分算出部１０２に出力する。データ入力部１０１は、例えば、データ入力インタフェースである。
クエリ点群データは、その他の機器、例えば、サーバ装置からＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネット等の通信路を介して受信した点群データであってもよい。

主成分算出部１０２は、データ入力部１０１から入力されたクエリ点群データＱを構成するサンプル点ｑ_ｐｉのそれぞれについて、そのサンプル点における被写体の表面に垂直な方向を示す法線（ｎｏｒｍａｌ）を示す法線ベクトル（ｎｏｒｍａｌ ｖｅｃｔｏｒ）ｑ_ｎｉを算出する。主成分算出部１０２は、例えば、ある注目サンプル点を頂点の１つとして共有する面を形成し、その注目サンプル点に隣接する２つのサンプル点の組を抽出する。

主成分算出部１０２は、注目サンプル点と抽出した２つのサンプル点の組から形成される面毎に法線ベクトルを算出し、算出した面毎の法線ベクトルを平均することによって、その注目サンプル点に係る法線ベクトルｑ_ｎｉを算出する。この処理で注目サンプル点について算出した法線ベクトルｑ_ｎｉは、頂点法線ベクトルと呼ばれる。算出した法線ベクトルｑ_ｎｉは、３次元のベクトルである。法線ベクトルｑ_ｎｉを用いることで各サンプル点ｑ_ｐｉでの位置情報が欠落するが、被写体とモデルとの間の相対的な姿勢の差異（相対姿勢）である回転と相対的な位置の差異である並進とを別個に分離して扱うことができる。
主成分算出部１０２は、各サンプル点ｑ_ｐｉで算出した法線ベクトルｑ_ｎｉの大きさを、例えば、１に規格化する。

主成分算出部１０２は、各サンプル点ｑ_ｐｉで算出した法線ベクトルｑ_ｎｉの分布が所定の密度よりも高い密度で分布している部分における法線ベクトルｑ_ｎｉを抽出する。
例えば、主成分算出部１０２は、各サンプル点ｑ_ｐｉで算出した法線ベクトルｑ_ｎｉの分布についてクラスタリング（ｃｌｕｓｔｅｒｉｎｇ）を行って複数のクラスタ（ｃｌｕｓｔｅｒ：群）に分類し、分類されたクラスタ毎の代表ベクトルを法線主成分として抽出する。これにより、所定の密度よりも高い密度で分布している部分の法線ベクトルを抽出することができる。主成分算出部１０２は、クラスタリングを行う際、例えば、階層的（ｈｉｅｒａｒｃｈｉｃａｌ）クラスタリングの一手法である群平均法（ｇｒｏｕｐ ａｖｅｒａｇｅ ｍｅｔｈｏｄ）を用いてもよい。

階層的クラスタリングは、次の過程（ｉ）−（ｉｖ）を有する手法である：（ｉ）それぞれ１個の法線ベクトルをメンバとして有するクラスタを設定する、（ｉｉ）各クラスタ間の類似度のうち、最も類似度が高いクラスタ同士を統合して１個のクラスタを形成する、（ｉｉｉ）クラスタの数が１個になった時に処理を終了し、そうでなければ（ｉｖ）に進む、（ｉｖ）（ｉｉ）で形成されたクラスタと他のクラスタのそれぞれとの類似度を算出し、（ｉｉ）に戻る。階層的クラスタリングは、主成分の数、つまりクラスタの数が既知でない場合でも適用可能であり、各クラスタに属する法線ベクトルの数によらずにクラスタ間の類似度を算出することができる。

群平均法は、この階層的クラスタリングにおいて、２つのクラスタのそれぞれから選択した法線ベクトル間の類似度を全ての法線ベクトルの組のそれぞれについて算出し、算出した類似度の平均値を２つのクラスタ間の類似度として定める手法である。群平均法によれば、最終的に求めようとする各クラスタの代表（例えば、重心）を法線ベクトルの代表として取得することができる。また、建造物等、人工的に構成された被写体を示す点群データに基づく法線ベクトルの分布は、ベクトル空間内の特定の領域に集中している明確な集団を形成していることが多いため、その領域毎に定めた法線ベクトルの代表によって、法線ベクトルの分布をモデル化することができる。

主成分算出部１０２は、分類されたクラスタのうち、そのクラスタに属する法線ベクトルの数が予め定めた法線ベクトルの数の閾値よりも少ないクラスタを排除する。これにより、外れ値、つまり所定の平面度よりも平面度が低い形状を示す部分のサンプル点に係る法線ベクトルが除去され、平面度がより高い形状を示すサンプル点に係る法線ベクトルが主に抽出される。
主成分算出部１０２は、法線ベクトル空間全体をその範囲とする１つのクラスタを除き、排除されずに残った複数のクラスタのそれぞれについて、代表値を算出する。代表値は、例えば、それぞれのクラスタに属する法線ベクトルの重心（ｃｅｎｔｒｏｉｄ）である。残ったクラスタの数、つまり代表値の数をＮ_ＣＱと表す。以下の説明では、この法線ベクトルの重心を法線主成分と呼び、点群データからクラスタ毎に法線主成分を算出することを法線主成分分析と呼ぶことがある。

主成分算出部１０２は、２個のクラスタａ，ｂ間の類似度ｓ_ａｂを、例えば、式（１）を用いて算出することができる。

式（１）において、ｃ_ａ，ｃ_ｂは、それぞれクラスタａ，ｂの重心を示す。それぞれの重心は、３次元のベクトルである。｜｜…｜｜は、ベクトル…のノルム（ｎｏｒｍ）を示す。従って、式（１）で算出される類似度ｓ_ａｂは、重心ｃ_ａ，ｃ_ｂ間における余弦（ｃｏｓｉｎｅ）を示す。
主成分算出部１０２は、２個のクラスタａ，ｂを統合して形成された１個のクラスタの重心ｃ_ａｂを、例えば、式（２）を用いて算出することができる。

式（２）において、ｎ_ａ，ｎ_ｂは、それぞれクラスタａ，ｂに属する法線ベクトルの数を示す。従って、式（２）で算出される重心ｃ_ａｂは、クラスタａ，ｂの重心ｃ_ａ，ｃ_ｂを、それぞれのクラスタａ，ｂに属する法線ベクトルの数ｎ_ａ，ｎ_ｂで重みづけた加重平均である。
主成分算出部１０２は、算出したクラスタ毎の代表ベクトル（例えば、重心）を法線主成分として含む法線主成分点群データを生成し、生成した法線主成分点群データを対応部１０３に出力する。従って、法線主成分点群データは、Ｎ_ＣＱ個の３次元のベクトルから形成される。位置合わせに係る処理対象がＮ_ＣＱ個の法線主成分に限定されるので、処理量が著しく低減する。

対応部１０３は、主成分算出部１０２から入力された法線主成分点群データ（クエリ点群に基づく）が示す法線主成分のそれぞれと、モデル点群（ｍｏｄｅｌ ｐｏｉｎｔ ｃｌｏｕｄ）に基づく法線主成分点群データが示す法線主成分のいずれかとの対応付け（ペアリング：ｐａｉｒｉｎｇ）を行う。
モデル点群とは、位置合わせの目標とする目標物（モデル、第２の物体）の形状を所定の空間間隔でサンプリングしたサンプル点からなる点群である。モデル点群を示すデータをモデル点群データ（第２の点群データ）と呼ぶことがある。モデル点群に基づく法線主成分点群データは、例えば、モデル点群データについて上述した法線主成分分析を行ってクラスタ毎に算出した重心から形成されるデータである。

モデル点群に基づく法線主成分点群データは、予め記憶部１１に記憶され、記憶部１１から読み出されたものであってもよいし、予め主成分算出部１０２から入力されたものでもよい。また、モデル点群に基づく法線主成分点群データは、ＣＡＤ（Ｃｏｍｐｕｔｅｒ Ａｉｄｅｄ Ｄｅｓｉｇｎ）技術を用いて生成した目標物の形状を示すモデル点群データから生成されたものであってもよい。

対応部１０３は、法線主成分点群データが示す法線主成分ｃ_ｍｊとの近似の度合いに基づいて、クエリ点群に基づく法線主成分点群データが示す法線主成分ｃ_ｑｉのそれぞれに対応する法線主成分ｃ_ｍｊを定める。
対応部１０３は、例えば、法線主成分ｃ_ｑｉに対応する法線主成分ｃ_ｍｊを、式（３）を用いて選択する。

式（３）において、ｆ_（ｉ）は、法線主成分ｃ_ｑｉを識別するインデックスｉに対応するインデックスｊを示す。ｊは、法線主成分ｃ_ｍｊを識別するインデックスを示す。即ち、ｆ_（ｉ）は、法線主成分ｃ_ｑｉと法線主成分ｃ_ｍｊとの対応関係を示す。個々の法線主成分ｃ_ｑｉとこれに対応する法線主成分ｃ_ｍｊとの組をペア（ｐａｉｒ）と呼ぶ。Ｎ_ＣＭは、法線主成分点群データが示す法線主成分ｃ_ｍｊの個数を示す。ａｒｇ ｍａｘ … は、…を最大とするｊを与えることを示す。また、各法線主成分は１に規格化されているところ、式（３）は、法線主成分ｃ_ｑｉとの内積が最も大きくなる法線主成分ｃ_ｍｊ、即ち、法線主成分ｃ_ｑｉとのなす角度が最も小さい法線主成分ｃ_ｍｊ、言い換えれば法線主成分ｃ_ｑｉに最も近似した法線主成分ｃ_ｍｊが選択される。対応部１０３は、この選択をＮ_ＣＱ個の法線主成分ｃ_ｑｉのそれぞれについて行う。
対応部１０３は、法線主成分ｃ_ｑｉと法線主成分ｃ_ｍｊとの対応関係を示す法線主成分対応データを生成し、生成した法線主成分対応データ、モデル点群並びにクエリ点群それぞれに基づく法線主成分点群データを回転算出部１０４に出力する。

回転算出部１０４は、対応部１０３から法線主成分対応データ、クエリ点群ならびにモデル点群に基づく法線主成分データが入力される。回転算出部１０４は、法線主成分対応データを参照して、クエリ点群に基づく法線主成分データとモデル点群に基づく法線主成分データとを用いて回転行列Ｒを算出する。回転行列Ｒは、クエリ点群に基づく法線主成分ｃ_ｑｉが示す方向を基準とした、その法線主成分ｃ_ｑｉに対応するモデル点群に基づく法線主成分ｃ_ｍｊの相対的な方向（相対姿勢、回転誤差ともいう）を、法線主成分のペア間で総合した方向への回転変換を示す行列である。従って、回転行列Ｒを算出することにより、法線主成分ｃ_ｑｉの方向を基準とした法線主成分ｃ_ｍｊの相対的な方向を算出することができる。回転行列Ｒは、クエリ点群が示す物体の方向を基準としたモデル点群が示す物体の方向との間の相対姿勢を近似し、その相対姿勢を補償する際に用いられる。

回転算出部１０４は、回転行列Ｒを算出する際、例えば、最小二乗法を用いる。最小二乗法により回転行列Ｒを算出する手法については、例えば、次の文献に記載されている： Ｋ．Ｓ．Ａｒｕｎ，Ｔ．Ｓ．Ｈｕａｎｇ，ａｎｄ Ｓ．Ｄ．Ｂｌｏｓｔｅｉｎ，“Ｌｅａｓｔ−Ｓｑｕａｒｅｓ Ｆｉｔｔｉｎｇ ｏｆ Ｔｗｏ ３Ｄ Ｐｏｉｎｔ Ｓｅｔｓ”，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， Ｖｏｌ． ＰＡＭＩ−９，Ｎｏ．５，ｐｐ．６９８−７００，ＳＥＰＭＴＥＭＢＥＲ １９８７。

最小二乗法では、回転算出部１０４は、モデル点群に基づく法線主成分ｃ_ｍｊと、その法線主成分に対応するクエリ点群に基づく法線主成分ｃ_ｑｉに回転行列Ｒを乗算した回転補正後の法線主成分Ｒｃ_ｑｉとの間の誤差関数を算出する。誤差関数は、例えば、法線主成分ｃ_ｍｊから回転補正後の法線主成分Ｒｃ_ｑｉの差分ベクトルのフロベニウスノルム（Ｆｒｏｂｅｎｉｕｓ ｎｏｒｍ）である。誤差関数はこれに限られず、法線主成分ｃ_ｍｊと回転補正後の法線主成分Ｒｃ_ｑｉとの間の誤差の大きさを示すものであれば、いかなる関数（例えば、差分ベクトルのユークリッドノルム（Ｅｕｃｌｅｄｅａｎ ｎｏｒｍ））であってもよい。

そして、回転算出部１０４は、算出した誤差関数を小さくするように、例えば、誤差関数を最小にする回転行列Ｒを算出する。回転行列Ｒは、原点を中心とする回転変換を示す３行３列の行列である。回転行列Ｒは、例えば、極座標系における水平面内の方位角（ａｚｉｍｕｔｈ）と水平面からの仰角（ｅｌｅｖａｔｉｏｎ）で表されてもよいし、直交座標系での各座標軸周りの回転角（例えば、ヨー（ｙａｗ）角、ピッチ（ｐｉｔｃｈ）角、ロール（ｒｏｌｌ）角）で表されてもよい。これにより、クエリ点群とモデル点群との間の相対姿勢が回転行列Ｒによって定められる。

なお、回転算出部１０４は、クエリ点群に基づく法線主成分ｃ_ｑｉとその法線主成分ｃ_ｑｉに対応するモデル点群に基づく法線主成分ｃ_ｍｊとの間の回転軸と回転量を、それぞれペア間で平均することで相対姿勢を算出してもよい。回転軸は、法線主成分ｃ_ｑｉとその法線主成分ｃ_ｑｉに対応する法線主成分ｃ_ｍｊとの外積に平行な軸である。回転量は、その回転軸まわりの法線主成分ｃ_ｑｉと法線主成分ｃ_ｍｊとのなす角度である。この回転軸の方向と、回転量によっても回転行列Ｒを定めることができる。
回転算出部１０４は、算出した回転行列Ｒを示す回転データをデータ出力部１０５に出力する。

データ出力部１０５は、回転算出部１０４から入力された回転データをデータ処理装置１の外部に出力する。データ入力部１０１は、例えば、データ入力インタフェースである。
また、データ出力部１０５は、データ入力部１０１から入力されたクエリ点群データが示すサンプル点毎の座標に、回転算出部１０４で算出した回転行列Ｒを乗じて得られる回転後の座標を示す回転座標データをデータ処理装置１の外部に出力してもよい。これにより、クエリ点群とモデル点群との間の相対姿勢が補償されたサンプル毎の座標が得られる。

（点群データの一例）
次に、点群データの一例について説明する。
図２は、点群データの一例を示す図である。
図２に示す点群データは、建造物の廊下とロビーとが交差した部位とその周辺における内壁の形状をサンプリングしたサンプル点で形成される。この形状の基本的構成は、Ｚ軸方向の長さ（高さ）がＸ軸方向に進むほど長くなる台形をＸ−Ｚ平面の断面として有する 四角柱が、Ｙ軸方向の長さ（幅）が他の方向よりも長い直方体のほぼ中央部で結合して形成される。Ｘ軸，Ｙ軸，Ｚ軸は、それぞれ３次元直交座標系を構成する互いに直交する座標軸の方向を示す。

（法線ベクトルの一例）
図３は、法線ベクトルの一例を示す。
図３（ａ）は、図２に示す点群データに基づいて算出されたサンプル毎の長さ１の法線ベクトルを、その起点を原点Ｏ_Ｎに配置して形成された終点の分布を示す。法線ベクトルの終点は、半径が１である単位球面上に分布しているため、この法線ベクトルの分布を法線球と呼ぶ。法線球の濃度は法線ベクトルの分布の密度を示す。濃度が高いほど法線ベクトルの密度が高く、濃度が低いほど法線ベクトルの密度が低い。

図３（ｂ）、（ｃ）は、それぞれ図３（ａ）に示す法線球のうち法線ベクトルの密度が所定の密度の閾値よりも高い部分を示す。図３（ｂ）よりも図３（ｃ）の方が、密度の閾値が高い。図３（ｃ）は法線ベクトルの密度が高い部分が狭い領域、特に６個のそれぞれ孤立した領域ｃ１−ｃ６に集中していることを示す。これは、点群データが示すサンプル点が示す物体の表面が、主に特定の方向に向けられていることを示す。上述したように、主成分算出部１０２は、サンプル点毎の法線ベクトルに対してクラスタリングを行い、クラスタ毎の代表としてその重心を法線主成分と定めるが、定められた法線主成分は、法線ベクトルの密度が高い領域の重心に近似する。従って、６個の法線主成分が算出される。
本実施形態では、このような法線ベクトルに基づく法線主成分を用いることで、原点を中心とする回転変換を示す回転行列Ｒを算出する際にクエリ点群やモデル点群が示す物体の表面の形状や、それらの点群間での相対並進位置が捨象される。これに対し、各点群が示す物体間の姿勢の差異を示す情報が維持されるので、回転行列Ｒを精度よく求めることができる。また、上述したように回転行列Ｒを算出する際の基礎となる点群間での法線主成分の対応付けが、法線主成分間の近似の度合いの指標として、それらの間のなす角度に基づいて行われる。これらのことから、回転行列Ｒを算出する際の演算量を著しく低減させることができる。

法線ベクトルｑ_ｎｉの分布の密度は、点群が示す形状の平面度に依存する。平面度とは、形状が平面に近似する度合い、つまり凹凸の少なさの度合いである。平面度が高いほど、複数の法線ベクトルの向きが、特定の１つの方向に整列し、平面度が低いほど、複数の法線ベクトルの向きが、乱雑に分布する。そのため、平面度が高いほど法線ベクトルｑ_ｎｉの分布の密度が高くなる。法線ベクトルがある１点を中心に分布している場合、被写体の形状のうち、その法線ベクトルが示す方向に垂直な面が支配的であるためである。

なお、主成分算出部１０２は、法線主成分を算出する際、サンプル点毎の法線ベクトルに対してクラスタリングを行うことに代え、所定の密度の閾値よりも法線ベクトルの密度が高い領域の法線ベクトルを抽出してもよい。その場合、主成分算出部１０２は、抽出した領域毎に法線ベクトルの代表（例えば、重心）を算出し、算出した代表を法線主成分と定める。
法線ベクトルの密度を示す指標は、例えば、法線方向の分散値ｑ_σｉである。法線方向の分散値ｑ_σｉは、そのサンプル点ｉから予め定めた範囲内におけるサンプル点毎の法線ベクトルの分散である。従って、分散値ｑ_σｉが小さいほど平面度が高いことを示し、分散値ｑ_σｉが大きいほど平面度が低いことを示す。

（データ処理の流れ）
次に、本実施形態に係るデータ処理について説明する。
図４は、本実施形態に係るデータ処理を示すフローチャートである。
（ステップＳ１０１）データ入力部１０１は、被写体の形状を示すクエリ点群データを取得する。その後、ステップＳ１０２に進む。
（ステップＳ１０２）主成分算出部１０２は、クエリ点群データが示す被写体の表面の法線を示す方向を示す法線ベクトルをサンプル点毎に算出する。その後、ステップＳ１０３に進む。

（ステップＳ１０３）主成分算出部１０２は、各サンプル点ｑ_ｐｉで算出した法線ベクトルｑ_ｎｉの分布についてクラスタリングを行って複数のクラスタに分類し、分類されたクラスタ毎の代表ベクトルを法線主成分として定める。これにより、法線ベクトルの分布が所定の密度よりも高い密度で分布している部分の法線ベクトルの代表が法線主成分として抽出される。その後、ステップＳ１０４に進む。
（ステップＳ１０４）対応部１０３は、クエリ点群に基づく法線主成分のそれぞれと、モデル点群に基づく法線主成分のいずれかとの対応付けを、両者間の近似の度合いに基づいて行う。その後、ステップＳ１０５に進む。
（ステップＳ１０５）回転算出部１０４は、クエリ点群に基づく法線主成分と、その法線主成分に対応するモデル点群に基づく法線主成分との間の相対的な方向を示す回転行列を算出する。その後、図４に示す処理を終了する。

本実施形態において、生成される回転データは、例えば、クエリ点群が示す第１の物体の姿勢とモデル点群が示す第２の物体との間の姿勢の差異を示すところ、第２の物体の姿勢が既知であれば、第１の物体の姿勢を取得することができる。この回転データを用いて、第１の物体の姿勢の制御、第１の物体の姿勢を基準としたロボットの動作の制御、その他の操作対象物の姿勢の制御、等に用いることができる。

上述したように、本実施形態では、第１のデータ（例えば、クエリ点群データ）が示す第１の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第１の法線主成分（例えば、クエリ点群に基づく法線主成分）として算出する。また、本実施形態では、第２のデータ（例えば、モデル点群データ）が示す第２の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第２の法線主成分（例えば、モデル点群に基づく主成分）から、第１の法線主成分との近似の度合いに基づいて第１の法線主成分に対応する第２の法線主成分を定める。また、本実施形態では、第１の法線主成分の方向と前記第２の法線主成分の方向との間の相対的な方向（例えば、回転行列）を算出する。

従って、第１の物体の表面の法線を代表する第１の法線主成分の方向と第２の物体の表面の法線を代表する第２の法線主成分の方向との間の相対的な方向を直接算出するため処理量が著しく低減する。
また、第１の法線主成分とこれに対応する第２の法線主成分を用いることで第１の物体の方向と第２の物体の方向とを合わせる際に、第１の物体の表面が疎らな領域や曲率が高い領域を一部に含んでいても安定した処理が実現する。

（第２の実施形態）
次に、本発明の第２の実施形態について説明する。上述した実施形態と同一の構成については、同一の符号を付して説明を援用する。
以下、図面を参照しながら本発明の実施形態について説明する。
図５は、本実施形態に係るデータ処理装置２の構成を示すブロック図である。
データ処理装置２は、データ処理装置１（図１）において、主成分算出部１０２及び対応部１０３に代えて主成分算出部２０２及び対応部２０３を備え、さらに相対並進位置算出部２０６及び座標変換部２０７を含んで構成される。

主成分算出部２０２は、主成分算出部１０２（図１）と同様に、データ入力部１０１から入力されたクエリ点群データに基づいてサンプル点毎に法線ベクトルを算出し、クラスタリングを行ってクラスタ毎に法線主成分を算出する。
主成分算出部２０２は、クエリ点群データが示すサンプル点毎の座標に回転算出部１０４で算出した回転行列Ｒを乗算して回転後の座標を算出して、クエリ点群が示す物体の方向についてモデル点群が示す物体の方向との間の相対姿勢を補償する。なお、主成分算出部２０２は、回転行列Ｒを乗算することに代え、回転算出部１０４で回転行列Ｒを算出する過程で算出された回転後の座標を採用してもよい。

そして、主成分算出部２０２は、サンプル点毎にその回転後の座標について法線ベクトルを算出し、クエリ点群データが示す物体の形状について相対姿勢が補償された法線主成分が算出される。これにより、クエリ点群とモデル点群との間では、相対並進位置が支配的となるため、本来６次元の処理である相対並進位置の算出を３次元の処理に縮退させることができ、処理量を格段に低減することができる。６次元とは、３次元空間内の相対並進位置の他、３次元の各座標軸周りの回転も含む。

主成分算出部２０２は、サンプル点毎に回転後の座標について深さ（ｄｅｐｔｈ）を算出する。深さとは、原点から注目するサンプル点が属する平面（つまり、壁面）までの距離である。そして、主成分算出部２０２は、サンプル点毎に算出した法線ベクトルに、そのサンプル点について算出した深さを乗算し、その深さを大きさとして有する深さ情報付き法線ベクトルを算出する。深さ情報付き法線ベクトルは、そのサンプル点の座標を示す座標ベクトルにおける、その法線ベクトルの方向への余弦成分のベクトルに相当する。よって、主成分算出部２０２は、サンプル点毎の座標ベクトルと法線ベクトルに基づいて深さ情報付き法線ベクトルを算出してもよい。

主成分算出部２０２は、サンプル点毎に算出した深さ情報付き法線ベクトルのうち、その分布が所定の密度よりも高い密度で分布している部分の代表となる深さ情報付き法線ベクトルを抽出する。以下の説明では、この代表となる深さ情報付き法線ベクトルを壁面主成分と呼ぶ。
ここで、主成分算出部２０２は、サンプル点毎に算出した深さ情報付き法線ベクトルについて、上述した法線主成分分析と同様な処理を行うことで壁面主成分を算出することができる。この処理を特に壁面主成分分析と呼ぶ。

壁面主成分分析において、主成分算出部２０２は、各サンプル点で算出した深さ情報付き法線ベクトルについてクラスタリングを行って複数のクラスタに分類し、分類されたクラスタ毎の代表ベクトルを壁面主成分として抽出してもよい。ここで、主成分算出部２０２は、例えば、群平均法を用いてもよい。
主成分算出部２０２は、２つのクラスタａ，ｂ間の類似度ｓ（ハット）_ａｂを、例えば、式（４）を用いて算出してもよい。ここで、「（ハット）」という記載は、その直前の文字の真上にハット記号（ｃｉｒｃｕｍｆｌｅｘ）が付されていることを示す。ここでは、ハット記号は、壁面主成分に係ることを示す。

式（４）において、ｃ（ハット）_ａ，ｃ（ハット）_ｂは、それぞれクラスタａ，ｂの代表ベクトル、例えば、重心を示す。δは、類似度ｓ（ハット）_ａｂが発散することを防止するための、予め定めた微小な実数を示す。また、ｃ（ハット）_ａ，ｃ（ハット）_ｂは、それぞれクラスタａ，ｂの重心を示す。ここで、分母にｃ（ハット）_ａ−ｃ（ハット）_ｂのノルムを乗じるのは、クラスタ間の距離が小さくなるほど、類似度ｓ（ハット）_ａｂを増加させるという効果を強調するためである。
また、主成分算出部２０２は、２個のクラスタａ，ｂを統合して形成された１個のクラスタの重心ｃ（ハット）_ａｂを、例えば、式（５）を用いて各クラスタにおける壁面主成分の重心ｃ_ａｂを代表ベクトルとして算出することができる。

主成分算出部２０２は、算出したクラスタ毎の代表ベクトル（例えば、重心）からなる壁面主成分点群データを生成し、生成した壁面主成分点群データを対応部２０３に出力する。壁面主成分点群データは、Ｎ（ハット）_ＣＱ個の３次元のベクトルから形成される。従って、位置合わせに係る処理対象をＮ（ハット）_ＣＱ個の壁面主成分に限定することで、処理量を著しく低減することができる。

対応部２０３は、主成分算出部２０２から入力されたクエリ点群に基づく壁面主成分点群データが示す壁面主成分のそれぞれと、モデル点群に基づく壁面主成分点群データが示す壁面主成分のいずれかとの対応付けを、両者間の近似の度合いに基づいて行う。
対応部２０３は、例えば、クエリ点群に基づく壁面主成分点群データが示す壁面主成分ｃ（ハット）_ｑｉに対応するモデル点群に基づく壁面主成分点群データが示す壁面主成分ｃ（ハット）_ｍｊを、式（６）を用いて選択する。

式（６）において、ｇ_（ｉ）は、壁面主成分ｃ（ハット）_ｑｉを識別するインデックスｉに対応するインデックスｊを示す。ｊは、壁面主成分ｃ（ハット）_ｍｊを識別するインデックスを示す。即ち、ｇ_（ｉ）は、壁面主成分ｃ（ハット）_ｑｉと壁面主成分ｃ（ハット）_ｍｊとの対応関係、言い換えれば壁面主成分ｃ（ハット）_ｑｉとこれに対応する壁面主成分ｃ（ハット）_ｍｊとのペアを示す。Ｎ（ハット）_ＣＭは、壁面主成分点群データが示す壁面主成分ｃ（ハット）_ｍｊの個数を示す。Ｎ（ハット）_ＣＭ、Ｎ（ハット）_ＣＱは、それぞれ３又は３よりも大きい整数である。

従って、式（６）は、壁面主成分ｃ（ハット）_ｑｉとの内積の、壁面主成分ｃ_ｑｉとの間の距離に対する比が最も大きくなる壁面主成分ｃ（ハット）_ｍｊを選択することを示す。これにより、深さ及び方向ともに壁面主成分ｃ（ハット）_ｑｉに最も近似した法線主成分ｃ（ハット）_ｍｊが選択される。対応部２０３は、この選択をＮ（ハット）_ＣＭ個の壁面主成分ｃ（ハット）_ｑｉのそれぞれについて行う。

対応部２０３は、式（７）に示すように、モデル点群に基づく壁面主成分ｃ（ハット）_{ｍｇ（ｉ）}から、これに対応するクエリ点群に基づく壁面主成分ｃ（ハット）_ｑｉの差分をペア間誤差ベクトルν_ｉとして、その壁面主成分ｃ（ハット）_ｑｉに係るペア毎に算出する。

対応部２０３は、壁面主成分ｃ（ハット）_ｑｉと壁面主成分ｃ（ハット）_ｍｊとの対応関係と、壁面主成分ｃ（ハット）_ｑｉに対応するペア間誤差ベクトルν_ｉを示すペア間誤差データを生成する。対応部２０３は、生成したペア間誤差データとモデル点群に基づく壁面主成分点群データとを対応付けて相対並進位置算出部２０６に出力する。
なお、対応部２０３は、対応部１０３と同様に、クエリ点群に基づく法線主成分点群データが示す法線主成分のそれぞれと、モデル点群に基づく法線主成分データが示す法線主成分との対応付けを行う。

モデル点群に基づく壁面主成分点群データは、予め記憶部１１に記憶され、記憶部１１から読み出されたものであってもよいし、予め主成分算出部２０２から入力されたものでもよい。また、モデル点群に基づく壁面主成分点群データは、ＣＡＤ（Ｃｏｍｐｕｔｅｒ Ａｉｄｅｄ Ｄｅｓｉｇｎ）技術を用いて生成した目標物の形状を示すモデル点群データから生成されたものであってもよい。

相対並進位置算出部２０６には、対応部２０３からペア間誤差データとモデル点群に基づくモデル壁面主成分点群データが入力される。相対並進位置算出部２０６は、ペア間誤差データが示すペア間誤差ベクトルν_ｉと、これに対応するモデル点群に基づく壁面主成分点群データが示す壁面主成分ｃ（ハット）_ｑｉに基づいて、並進誤差ベクトルεを算出する。並進誤差ベクトルεは、クエリ点群に基づく壁面主成分データが示す壁面主成分ｃ（ハット）_ｑｉと、その壁面主成分ｃ（ハット）_ｑｉに対応する壁面主成分ｃ（ハット）_ｍｊであってモデル点群に基づく壁面主成分データが示す壁面主成分ｃ（ハット）_ｍｊとの間の相対並進位置を示す３次元のベクトルである。並進誤差ベクトルεは、壁面主成分ｃ（ハット）_ｑｉの位置を基準とした壁面主成分ｃ（ハット）_ｍｊの相対的な位置（相対並進位置）を、ペア間で総合した相対並進位置を示す。
並進誤差ベクトルεは、クエリ点群とモデル点群との間の相対並進位置に近似する。そのため、並進誤差ベクトルεは、クエリ点群とモデル点群との間の相対並進位置を補償する際に用いられる。

相対並進位置算出部２０６は、少なくとも３個のペア間誤差ベクトルν_ｉを用いて並進誤差ベクトルεを算出する。これは、クエリ点群が示す物体とモデル点群が示す物体との間には姿勢の差異があるため、この姿勢の差異（つまり、相対姿勢）を補償するためである。相対並進位置算出部２０６は、例えば、式（８）を用いて並進誤差ベクトルεを算出する。

式（８）において、Ｗは、重み係数ｗ_ｍの総和である。ｍは、Ｎ（ハット）_ＣＱ個の壁面主成分ｃ_ｑｉのうちの互いに異なる３個からなる組のそれぞれを識別するインデックスである。このような壁面主成分の組はＮ（ハット）_ＣＱＣ_３個あるため、ｍは、１からＮ（ハット）_ＣＱＣ_３までの間のいずれかの整数を示す。各組を構成する３個の壁面主成分を、それぞれｃ_ｑｊ，ｃ_ｑｋ，ｃ_ｑｌと表す。ここで、ｊ，ｋ，ｌは、それぞれ、壁面主成分を識別するインデックスであり、互いに異なる値をとる（ｊ≠ｋ≠ｌ）。Ｍ（ハット）_ＣＱは、Ｎ（ハット）_ＣＱＣ_３である。
従って、壁面主成分εは、３個の壁面主成分からなる組ｍ毎の並進誤差ベクトルε（ハット）_ｍを、それぞれ重み係数ｗ_ｍで乗算して得られる乗算値を組ｍ間で加算し、加算した値を総和Ｗで正規化して算出される。
重み係数ｗ_ｍは、式（９）に示されるように、組ｍに係る３個の壁面主成分ｃ_ｑｊ，ｃ_ｑｋ，ｃ_ｑｌに基づいて算出される。

即ち、式（９）は、３個の壁面主成分ｃ（ハット）_ｑｊ，ｃ（ハット）_ｑｋ，ｃ（ハット）_ｑｌのうちの２個からなる壁面主成分の対毎にその２つの壁面主成分の外積を算出し、算出した外積のノルムを対間で乗算し、乗算した値を重み係数ｗ_ｍとして算出することを示す。
壁面主成分の組ｍ毎の並進誤差ベクトルε（ハット）_ｍは、式（１０）に示されるように、その組ｍを構成する３個の壁面主成分ｃ（ハット）_ｑｊ，ｃ（ハット）_ｑｋ，ｃ（ハット）_ｑｌと、それらにそれぞれ対応するペア間誤差ベクトルν_ｊ，ν_ｋ，ν_ｌに基づいて算出される。

式（１０）において、［…］^−１は、行列…の逆行列を示す。
従って、重み係数ｗ_ｍで乗算された並進誤差ベクトルε（ハット）_ｍは、ペア間誤差ベクトルν_ｊ，ν_ｋ，ν_ｌを、それぞれ対応する壁面主成分ｃ（ハット）_ｑｊ，ｃ（ハット）_ｑｋ，ｃ（ハット）_ｑｌに射影し、射影して得られる成分をさらにＸ軸方向、Ｙ軸方向、Ｚ軸方向に射影した成分を示す。このようにペア間誤差ベクトルν_ｊ，ν_ｋ，ν_ｌを、互いに直交した成分に射影し、射影した成分を並進誤差ベクトルε（ハット）_ｍとして取得することで、高い精度で位置決めをすることができる。また、全ての壁面主成分の組ｍ毎の並進誤差ベクトルε（ハット）_ｍを、重み係数ｗ_ｍで加重平均をとることで、ペア間の相対姿勢の差異を緩和することができる。
相対並進位置算出部２０６は、算出した並進誤差ベクトルεを示す並進データを座標変換部２０７に出力する。

座標変換部２０７には、データ入力部１０１からクエリ点群データと、回転算出部１０４から回転行列Ｒを示す回転データ、ならびに相対並進位置算出部２０６から並進誤差ベクトルεを示す並進データと、が入力される。
座標変換部２０７は、クエリ点群データが示すサンプル点毎の座標を、回転行列Ｒと並進誤差ベクトルを用いて変換する。ここで、座標変換部２０７は、サンプル点毎の座標（ベクトル）に回転行列Ｒを乗算して回転後の座標を算出し、算出した座標に並進誤差ベクトルεを加算して、変換後の座標を算出する。この演算を示す４行４列の同次変換行列Ｓを、式（１１）に示す。

式（１１）において、０は、各要素値が０である３行１列のベクトルである。１は、同次変換行列Ｓの第４行第４列の要素値である。このように変換後の座標を算出することによって、モデル点群を目標位置としたクエリ点群の方向合わせ及び位置合わせを行うことができる。
なお、座標変換部２０７は、回転行列Ｒを乗算することに代え、回転算出部１０４もしくは主成分算出部２０２で回転行列Ｒを算出する過程で算出された回転後の座標を採用してもよい。
座標変換部２０７は、サンプル毎の変換後の座標を示す変換座標データを、データ出力部１０５を介してデータ処理装置２の外部に出力する。出力される変換座標データは、第２の物体の位置及び姿勢に合うように、相対姿勢及び相対並進位置が補償されたクエリ点群データが示す第１の物体の形状を示す。ここで、データ出力部１０５は、変換座標データに代えて、もしくは変換座標データとともに並進データを出力してもよいし、更に回転算出部１０４で生成された回転データを出力してもよい。

（深さ情報付き法線ベクトルの一例）
図６は、深さ情報付き法線ベクトルの一例を示す。
図６（ａ）は、図２に示す点群データに基づいて算出されたサンプル毎の深さ情報付き法線ベクトルを、その起点を原点Ｏ_Ｎに配置して形成された終点の分布を示す。法線ベクトルの終点は、それぞれの深さを半径とする曲面上に分布する。この法線ベクトルの分布を深さ情報付き法線球と呼ぶ。深さ情報付き法線球の濃度は、深さ情報付き法線ベクトルの分布の密度を示す。濃度が高いほど深さ情報付き法線ベクトルの密度が高く、濃度が低いほど深さ情報付き法線ベクトルの密度が低い。

図６（ｂ）は、図６（ａ）が示す深さ情報付き法線ベクトルの分布をＺ軸の負方向に向けて観察して得られる平面図である。この平面図では、深さ情報付き法線ベクトルの分布に加え、図２に示す点群を薄く重ね合わせて表示されている。
図６（ｃ）は、図６（ｂ）のＡ領域を拡大した拡大図である。
図６（ｂ）、（ｃ）は、深さ情報付き法線ベクトルの分布が高い密度で集中している点Ｐが存在する。この点Ｐは、その座標を示す位置ベクトルと同一の方向に法線を有する壁面Ｌが、その位置ベクトルのノルムに相当する深さだけ離れた位置に存在することを示す。
従って、サンプル点毎の深さ情報付き法線ベクトルについてクラスタリングを行って、その分布の密度が高い領域を抽出し、壁面主成分として算出されたその領域の代表をもって、壁面の法線方向と位置に関する情報を維持することができる。

（処理の流れ）
次に、本実施形態に係るデータ処理について説明する。
図７は、本実施形態に係るデータ処理を示すフローチャートである。
図７に示すデータ処理は、ステップＳ１０１−Ｓ１０５（図４）とステップＳ２０６−Ｓ２１０を有する。ステップＳ１０１−Ｓ１０５の処理が終了した後、ステップＳ２０６に進む。
（ステップＳ２０６）主成分算出部２０２は、クエリ点群データが示すサンプル点毎の座標に回転行列Ｒを乗算して回転後の座標を算出する。これにより、クエリ点群についてモデル点群との間の相対姿勢を補償する。その後、ステップＳ２０７に進む。
（ステップＳ２０７）主成分算出部２０２は、サンプル点毎に回転後の座標について法線ベクトルを算出し、算出した法線ベクトルと座標に基づいて深さ情報付き法線ベクトルを算出する。その後、ステップＳ２０８に進む。

（ステップＳ２０８）主成分算出部２０２は、各サンプル点で算出した深さ情報付き法線ベクトルについてクラスタリングを行って複数のクラスタに分類し、分類されたクラスタ毎の代表ベクトルを壁面主成分として算出する。その後、ステップＳ２０９に進む。
（ステップＳ２０９）対応部２０３は、クエリ点群に基づく壁面主成分のそれぞれと、モデル点群に基づく壁面主成分のいずれかとの対応付けを行う。その後、ステップＳ２１０に進む。
（ステップＳ２１０）相対並進位置算出部２０６は、クエリ点群に基づく壁面主成分の組と、その壁面主成分のそれぞれに対応するペア間誤算ベクトルに基づいてクエリ点群とモデル点群との間の相対並進位置を示す並進誤差ベクトルを算出する。その後、図７に示す処理を終了する。

本実施形態において算出した相対並進位置を示す並進データは、モデル点が表す第１の物体の位置を基準としたクエリ点群が表す第２の物体の位置を示す。この並進データを用いて、第１の物体の位置の制御、第１の物体の位置を基準としたロボットの動作の制御、その他の操作対象物の位置の制御、等に用いることができる。
その他、並進データは、重力加速度等の加速度を検出する加速度センサを備えた電子機器において、その位置の補償に用いることができる。そのような電子機器には、検出した加速度を２階積分することによって自機の位置を算出する位置推定機能を備えた多機能携帯電話機（いわゆるスマートフォン）がある。その他、自機が備える駆動機構の動作状態（例えば、モータの回転速度）に基づいて自機の位置を算出する位置算出機能を備えたロボットがある。

この位置算出機能で算出される位置の精度は、計算誤差の累積やノイズにより、一般に低くなる傾向がある。そこで、データ処理装置２が、その電子機器を撮影して得られるクエリ点群データに基づいて所定の時間間隔（例えば、１分）で算出した相対並進位置を示す並進データを生成する。そして、この位置算出機能は、生成された並進データが示す相対並進位置を自機の位置の原点として採用して、算出した位置を逐次に補正することができる。

（評価実験）
次に、本実施形態による効果を示すために行った評価実験について説明する。ここでは、（１）初期位置依存性評価実験、（２）実時間性評価実験、（３）堅牢性評価実験、と３つのそれぞれ異なる観点で行われた評価実験について述べる。いずれの評価実験でも比較対象として、従来技術としてＩＣＰ法による実験結果と比較する。

まず、初期位置依存性評価実験について説明する。初期位置依存性評価実験では、クエリ点群に対して本実施形態によって回転行列Ｒと並進誤差ベクトルεを算出し、目標とする回転行列Ｒと並進誤差ベクトルεとの誤差をそれぞれ補正量の誤差として測定する。これらの操作を、一定の位置に配置されたモデル点群を用いて複数の予め定めた初期位置のそれぞれについて実行する。初期位置は、回転行列Ｒを１°間隔、並進誤差ベクトルεを０．１ｍ間間隔で設定する。但し、このモデル点群は、Ｘ−Ｙ平面内の回転対称性を有するため、Ｘ座標、Ｚ座標、Ｙ軸回りの回転角（仰角）、Ｚ軸回りの回転角（方位角）のそれぞれに初期位置を分布させた。
また、それぞれの初期位置について大局解が得られるか否かを調査する。大局解が得られない場合には、局所解に陥る場合がある。大局解が得られるか否かの判断は、誤差が予め期待される誤差の閾値を下回るか否かによって行う。これにより、大局解を導くことができる初期位置の範囲を示す収束限界も評価値として扱う。

図８は、モデル点群の一例を示す図である。
図８に示すモデル点群は、初期位置依存性評価実験及び実時間性評価実験で用いた点群である。この点群は、建造物の内壁で構成される屋内の形状を模擬する４５０万個のサンプル点で形成される。模擬している形状は、長手方向がそれぞれＸ方向、Ｙ方向である２つの直方体がそれぞれ中心部で交差して形成される。但し、標準偏差が５ｃｍであるガウシアンノイズが重畳されている。この点群が網羅する空間は、Ｘ，Ｙ，Ｚ方向、それぞれの方向の長さが、１５ｍ、１５ｍ、５ｍである。点群の最大密度、つまり解像度は、１ｍ^２当たり１万サンプルである。

図９は、誤差の初期位置依存性を示す図である。
図９（ａ）は、方位角の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図９（ａ）において、縦軸、横軸は、それぞれ方位角の誤差、初期の方位角を示す。◆印、×印は、それぞれ本実施形態、従来技術を示す。収束限界を与える誤差の閾値を１°とする。
これによれば、本実施形態では、方位角の誤差は、概ね０．０１°未満と極めて誤差が小さく、初期の方位角に依存しない。但し、初期の方位角が−８９°、８９°の場合、それぞれ誤差が９０°となる。従って、収束限界は８８°となる。この現象は、モデル点群が９０°（４相）の回転対称性を有するために、９０°回転した方向に位置合わせが行われてしまうことによる。
また、大局解が得られたか否かにより、誤差が大きく異なることから収束限界を越えたか否かの判定を安定して行うことができる。

これに対して、従来技術では方位角の誤差が−９°〜９°の範囲では、方位角の誤差は１°未満となる。また、初期の方位角が−１０°以下又は１０°以上の範囲では、方位角の誤差がいずれも１０°以上と急激に増加し、初期の方位角の絶対値が大きくなるほど方位角の誤差が増加する。これより、大局解を得ることができる収束限界は９°となる。
一般に、ｎ相対称を有する点群の位置合わせの収束限界の上限は３６０／ｎ度（この例では、９０度）となる。この上限でそれぞれの収束限界を正規化すると、正規化した収束限界は本実施形態では０．９８、従来技術では０．１０となる。この結果は、本実施形態の方が広範に高精度の位置合わせを実現できることを示す。

図９（ｂ）は、仰角の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図９（ｂ）において、縦軸、横軸は、それぞれ仰角の誤差、初期の仰角を示す。これによれば、本実施形態では、仰角の誤差が概ね０．０１°未満と極めて小さく、初期の仰角に依存しない。但し、初期の仰角が−８９°、８９°の場合、それぞれ誤差が９０°となる。これより、大局解を得ることができる収束限界は８８°となる。
これに対して、従来技術では仰角の誤差が−６°〜６°の範囲では、仰角の誤差は１°未満となる。また、初期の仰角が−７°以下又は７°以上の範囲では、初期の仰角の絶対値が大きくなるほど仰角の誤差が増加する傾向が現れる。これより、大局解を得ることができる収束限界は６°となる。

図９（ｃ）は、Ｘ座標の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図９（ｃ）において、縦軸、横軸は、それぞれＸ座標の誤差、初期のＸ座標を示す。収束限界を与える誤差の閾値を０．１ｍとする。
これによれば、本実施形態では、Ｘ座標の誤差は概ね０．００１ｍ未満と極めて小さく、初期のＸ座標に依存しない。但し、初期のＸ座標が−５．０ｍ、５．０ｍの場合、誤差が３．３ｍとなる。よって、収束限界は、４．９ｍとなる。
これに対して、従来技術ではＸ座標の誤差が−１．６ｍ〜１．６ｍの範囲では、Ｘ座標の誤差は概ね０．１ｍ未満となる。また、初期のＸ座標の誤差が−１．７ｍ以下又は１．７ｍ以上の範囲では、Ｘ座標の誤差は０．１ｍを越え初期のＸ座標の絶対値が大きくなるほどＸ座標の誤差が増加する。よって、大局解を得ることができる収束限界は１．６ｍとなる。

図９（ｄ）は、Ｚ座標の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図９（ｄ）において、縦軸、横軸は、それぞれＺ座標の誤差、初期のＺ座標を示す。これによれば、本実施形態では、Ｚ座標の誤差は概ね０．００１ｍ未満と極めて小さく、Ｚ座標に依存しない。但し、初期のＺ座標が−５．０ｍ、５．０ｍの場合、誤差が３．３ｍとなる。よって、収束限界は、４．９ｍとなる。
これに対して、従来技術ではＺ座標の誤差が−１．８ｍ〜１．８ｍの範囲では、Ｚ座標の誤差は０．２ｍ未満となる。また、初期のＺ座標の誤差が−１．９ｍ以下又は１．９ｍ以上の範囲では、Ｚ座標の誤差は０．１ｍを越え初期のＺ座標の絶対値が大きくなるほどＺ座標の誤差が増加する。よって、大局解を得ることができる収束限界は１．８ｍとなる。
以上に述べたように、初期位置依存性評価実験により、本実施形態によれば、各初期位置について高精度でクエリ点群について位置合わせを行うことができること、大局解を得ることができる初期位置の範囲が広くなることが示される。

次に、実時間性評価実験について説明する。ここでは、実時間性とは、処理の高速性と初期位置の差異に応じた処理時間安定性を含みうる概念である。実時間性評価実験では、複数の予め定めた初期位置のそれぞれについてクエリ点群の位置合わせに要する処理時間を本実施形態、従来技術のそれぞれについて観測した。但し、初期位置の設定において、Ｘ方向にのみ０．１ｍ間隔で分布する並進誤差ベクトルεを設定し、つまり回転を与えない。なお、実時間評価実験で用いたクエリ点群、モデル点群は、初期位置依存性評価実験で使用したものとそれぞれ同一のものである。

図１０は、処理時間の初期位置依存性を示す図である。
◆印、×印は、それぞれ本実施形態、従来技術を示し、○印は収束限界を示す。縦軸、横軸は、それぞれ処理時間、初期のＸ座標を示す。
本実施形態では、処理時間は、いずれも０．５２４−０．５３７秒と従来技術の数十秒単位よりも格段に短く、初期のＸ座標にも依存しない。これは、本実施形態では、従来技術のように反復処理に頼らずに回転行列Ｒと並進誤差ベクトルεが直接算出されるためである。これに対し、従来技術では処理時間は９秒から１１１秒の間であり、初期のＸ座標が大きくなるほど処理時間が長くなる。

次に、堅牢性評価実験について説明する。堅牢性評価実験では、実環境で取得した点群を用いた場合でも有効に位置合わせを行うことができるか否かを検証する。
実環境で取得した点群は、一部に点が疎らな曲面や曲率が大きい曲面を含むため、精度が低下するおそれがあるからである。
ここで、取得した点群について回転及び並進移動を与えて処理前のクエリ点群を生成し、取得した点群はそのままモデル点群として用いる。そして、本実施形態による処理後のクエリ点群とモデル点群の位置とを比較する。

図１１は、点群データを取得した環境の一例を示す写真である。
図１１は、建造物の内部である廊下であり、その基本的構成態様は、一方向の長さが他の方向の長さよりも長い直方体である。但し、直方体の側面が随所で開放され、机、椅子、廃物入れ等の什器が備えられているために、具体的構成態様として、サンプル点が疎らである部分や、曲率が大きい部分が含まれる。図１１（ａ）、（ｂ）間では、互いに撮影した視点が異なる。

図１２は、処理前の点群データの一例を示す図である。
図１２（ａ）、（ｂ）は、図１１に示す環境で取得された点群データの平面図、斜視図を示す。薄く表示された部分が処理前のクエリ点群を示し、濃く表示された部分がモデル点群を示す。処理前のクエリ点群とモデル点群の間では、Ｘ−Ｙ平面内で回転と相対並進移動が与えられている。
図１２（ｃ）は、図１２（ａ）、（ｂ）に示した、処理前のクエリ点群とモデル点群のそれぞれについて算出した深さ情報付き法線ベクトルの分布を示す。薄く表示された部分が処理前のクエリ点群に係る深さ情報付き法線ベクトルの分布を示し、濃く表示された部分がモデル点群に係る深さ情報付き法線ベクトルの分布を示す。これらの分布は、いずれも深さ情報付き法線ベクトルの分布が密な領域が、撮影領域に含まれる平面に応じて数点に集中している。このことは、深さ情報付き法線ベクトルの分布が算出された壁面主成分で代表できることを示す。

図１３は、処理後の点群データの一例を示す図である。
図１３（ａ）、（ｂ）は、それぞれ点群データの平面図、斜視図を示す。薄く表示された部分が処理後のクエリ点群を示し、濃く表示された部分がモデル点群を示す。図１３は、両点群が互いに重なり合っていることを示す。この結果は、本実施形態では、一部に点が疎らな部分や曲率が大きい部分を含む実環境で取得した点群であっても、一定の精度をもって位置合わせを行うことができることを示す。なお、建造物の内部に限らず、屋外で取得した点群データであっても、建造物、その他の設置物のように一部に平面を含む環境であれば、同様に位置合わせを行うことができる。
これに対し、従来技術のＩＣＰ法では、全サンプル点について各最近傍点との誤差の総和の最小解を与えるため、これほど疎らな点群同士を重ね合わせることは原理的に困難である。

このように、本実施形態では、第１のデータ（例えば、クエリ点群データ）が示す第１の物体の表面までの原点からの深さを大きさとして有する法線である第１の深さ付き法線（例えば、クエリ点に基づく深さ付き法線ベクトル）のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第１の壁面主成分（例えば、クエリ点に基づく壁面主成分）として算出する。また、本実施形態では、第２のデータ（例えば、モデル点群データ）が示す第２の物体の表面までの原点からの深さを大きさとして有する法線である第２の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第２の壁面主成分（例えば、モデル点に基づく壁面主成分）から、前記第１の壁面主成分との近似の度合いに基づいて前記第１の壁面主成分に対応する第２の壁面主成分を定める。また、本実施形態では、前記第１の壁面主成分と、当該第１の壁面主成分に対応する第２の壁面主成分との差分（例えば、ペア間誤差ベクトル）に基づいて前記第１の物体と前記第２の物体との間の相対的な位置（例えば、並進誤差ベクトル）を算出する。

これにより、第１の物体の表面の深さ付き法線を代表する第１の壁面主成分と第１の物体の表面の深さ付き法線を代表する第２の壁面主成分を用いることで第１の物体の位置と第２の物体の位置とを合わせる際に、第１の物体の表面が疎らな領域や曲率が高い領域を一部に含んでいても安定した処理を実現することができる。また、反復処理を行わずに第１の壁面主成分とこれに対応する第２の壁面主成分との間の相対的な位置を直接算出するため処理量を著しく低減することができる。

なお、上述した実施形態では、物体の形状を示すデータとして点群データを用いる場合を例にとって説明したが、これには限られない。物体の形状を示すデータであれば、いかなる形式のデータであってもよい。例えば、点群データに代えて、物体の２次元の形状を示す平面画像と、視点（原点）からその物体の表面の各点までの距離もしくは奥行を示す奥行画像（例えば、デプスマップ）との組を用いてもよい。

なお、上述した実施形態におけるデータ処理装置１、２の一部、例えば、主成分算出部１０２、２０２、対応部１０３、２０３、回転算出部１０４、相対並進位置算出部２０６、及び座標変換部２０７をコンピュータで実現するようにしてもよい。その場合、この制御機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することによって実現してもよい。なお、ここでいう「コンピュータシステム」とは、データ処理装置１、２に内蔵されたコンピュータシステムであって、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間、動的にプログラムを保持するもの、その場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含んでもよい。また上記プログラムは、前述した機能の一部を実現するためのものであってもよく、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであってもよい。
また、上述した実施形態におけるデータ処理装置の一部、または全部を、ＬＳＩ（Ｌａｒｇｅ Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）等の集積回路として実現してもよい。データ処理装置１、２の各機能ブロックは個別にプロセッサ化してもよいし、一部、または全部を集積してプロセッサ化してもよい。また、集積回路化の手法はＬＳＩに限らず専用回路、または汎用プロセッサで実現してもよい。また、半導体技術の進歩によりＬＳＩに代替する集積回路化の技術が出現した場合、当該技術による集積回路を用いてもよい。

以上、図面を参照してこの発明の一実施形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、この発明の要旨を逸脱しない範囲内において様々な設計変更等をすることが可能である。

１、２…データ処理装置、１０１…データ入力部、１０２、２０２…主成分算出部、
１０３、２０３…対応部、１０４…回転算出部、１０５…データ出力部、
１１…記憶部、２０６…相対並進位置算出部、２０７…座標変換部

Claims (7)

1. 第１のデータが示す第１の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第１の法線主成分として算出する主成分算出部と、
   第２のデータが示す第２の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第２の法線主成分から、前記第１の法線主成分との近似の度合いに基づいて前記第１の法線主成分に対応する第２の法線主成分を定める対応部と、
   前記第１の法線主成分の方向と前記第２の法線主成分の方向との間の相対的な方向を算出する相対方向算出部と、
   を備え、
   前記主成分算出部は、前記第１の物体の表面までの原点からの深さを大きさとして有する法線である第１の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第１の壁面主成分として算出し、
   前記対応部は、前記第２の物体の表面までの原点からの深さを大きさとして有する法線である第２の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第２の壁面主成分から、前記第１の壁面主成分との近似の度合いに基づいて前記第１の壁面主成分に対応する第２の壁面主成分を定め、
   前記第１の壁面主成分と、当該第１の壁面主成分に対応する第２の壁面主成分との差分に基づいて前記第１の物体と前記第２の物体との間の相対的な位置を算出する相対並進位置算出部を、
   さらに備えるデータ処理装置。
2. 前記主成分算出部は、前記第１の物体の形状を表す複数の点からなる第１の点群データに基づいて前記第１の物体の表面の法線のうち、予め定めた密度よりも高密度で分布している領域における法線の重心を前記第１の法線主成分として算出する請求項１に記載のデータ処理装置。
3. 前記主成分算出部は、前記第１の物体の表面の法線を、当該第１の物体の表面の法線間の距離に基づいて複数の法線の群に分類し、各群に属する法線毎に前記第１の法線主成分を算出する請求項２に記載のデータ処理装置。
4. 前記主成分算出部は、前記第１の物体の形状を表す複数の点からなる第１の点群データに基づいて前記第１の物体の表面の深さ付き法線のうち、予め定めた密度よりも高密度で分布している領域における深さ付き法線の重心を前記第１の壁面主成分として算出する請求項１から請求項３のいずれか一項に記載のデータ処理装置。
5. 前記主成分算出部は、前記第１の物体の表面の深さ付き法線を、当該第１の物体の表面の深さ付き法線間の距離に基づいて複数の深さ付き法線の群に分類し、各群に属する深さ付き法線毎に前記第１の壁面主成分を算出する請求項４に記載のデータ処理装置。
6. 第１のデータが示す第１の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第１の法線主成分として算出する主成分算出過程と、
   第２のデータが示す第２の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第２の法線主成分から、前記第１の法線主成分との近似の度合いに基づいて前記第１の法線主成分に対応する第２の法線主成分を定める対応過程と、
   前記第１の法線主成分の方向と前記第２の法線主成分の方向との間の相対的な方向を算出する相対方向算出過程と、
   を有し、
   前記主成分算出過程は、前記第１の物体の表面までの原点からの深さを大きさとして有する法線である第１の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第１の壁面主成分として算出し、
   前記対応過程は、前記第２の物体の表面までの原点からの深さを大きさとして有する法線である第２の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第２の壁面主成分から、前記第１の壁面主成分との近似の度合いに基づいて前記第１の壁面主成分に対応する第２の壁面主成分を定め、
   前記第１の壁面主成分と、当該第１の壁面主成分に対応する第２の壁面主成分との差分に基づいて前記第１の物体と前記第２の物体との間の相対的な位置を算出する相対並進位置算出過程を、
   さらに有するデータ処理方法。
7. データ処理装置のコンピュータに、
   第１のデータが示す第１の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第１の法線主成分として算出する主成分算出手順と、
   第２のデータが示す第２の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第２の法線主成分から、前記第１の法線主成分との近似の度合いに基づいて前記第１の法線主成分に対応する第２の法線主成分を定める対応手順と、
   前記第１の法線主成分の方向と前記第２の法線主成分の方向との間の相対的な方向を算出する相対方向算出手順と、
   を実行させるためのデータ処理プログラムであって、
   前記主成分算出手順は、前記第１の物体の表面までの原点からの深さを大きさとして有する法線である第１の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第１の壁面主成分として算出し、
   前記対応手順は、前記第２の物体の表面までの原点からの深さを大きさとして有する法線である第２の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第２の壁面主成分から、前記第１の壁面主成分との近似の度合いに基づいて前記第１の壁面主成分に対応する第２の壁面主成分を定め、
   前記第１の壁面主成分と、当該第１の壁面主成分に対応する第２の壁面主成分との差分に基づいて前記第１の物体と前記第２の物体との間の相対的な位置を算出する相対並進位置算出手順を、
   さらに実行させるためのデータ処理プログラム。