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

データ処理装置、データ処理方法、及びデータ処理プログラム Download PDF

Info

Publication number
JP6281938B2
JP6281938B2 JP2013260076A JP2013260076A JP6281938B2 JP 6281938 B2 JP6281938 B2 JP 6281938B2 JP 2013260076 A JP2013260076 A JP 2013260076A JP 2013260076 A JP2013260076 A JP 2013260076A JP 6281938 B2 JP6281938 B2 JP 6281938B2
Authority
JP
Japan
Prior art keywords
normal
principal component
depth
density
wall
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2013260076A
Other languages
English (en)
Other versions
JP2015118441A (ja
Inventor
洋司 黒田
洋司 黒田
尚吾 清水
尚吾 清水
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
MEIJI UNIVERSITY LEGAL PERSON
Original Assignee
MEIJI UNIVERSITY LEGAL PERSON
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by MEIJI UNIVERSITY LEGAL PERSON filed Critical MEIJI UNIVERSITY LEGAL PERSON
Priority to JP2013260076A priority Critical patent/JP6281938B2/ja
Publication of JP2015118441A publication Critical patent/JP2015118441A/ja
Application granted granted Critical
Publication of JP6281938B2 publication Critical patent/JP6281938B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Image Analysis (AREA)
  • Image Processing (AREA)

Description

本発明は、データ処理装置、データ処理方法、及びデータ処理プログラムに関する。
動作環境において位置を推定する技術(例えば、対環境自己位置推定)や、周囲の環境に関する情報を把握する技術(例えば、探索環境地図構築)は、以前から提案され、様々な分野(例えば、移動ロボットの動作制御)への応用が試みられている。周囲の環境として周囲に位置している種々の物体の表面の形状を示す形状データを取得する3次元測域距離センサ(3DLRF:Three Dimensional Laser Range Finder)が用いられることがある。3DLRFは、物体の表面を所定の空間間隔(精度)でサンプリングしたサンプル点からなる点群(point cloud)データ(点群情報)を取得する。その空間間隔は、例えば、数mm以下の高精度なものもある。自己位置推定(localization)や、地図構築(mapping)において、互いに相対的な位置や姿勢(相対位置姿勢)の関係が不明な2つの点群が重なり合うように方向や位置を合わせる処理(matching:位置合わせ)が必要になることがある。
例えば、特許文献1に記載の自己位置推定装置は、2つの点群の位置合わせを行う際、ICP(Iterative Closest Point)法を用いる。ICP法は、反復計算により2つの点群の間の距離を最小化するアルゴリズムであり、複雑な形状を示す点群についても実行可能であるという汎用性を有する。
特開2011−8320号公報
しかしながら、ICP法は、反復解法であるため、その計算処理に多くの時間を要する。そのため、実時間での処理が求められる用途、例えば、移動ロボットの動作の制御に応用することは現実的ではない。たとえ平面度の高い面を多く含む物体から取得された点群同士の位置合わせに利用されたときでも処理時間が短くなるとは限らない。このような点群は、特に室内等の人工的な環境で現れることが多い。
また、ICP法による処理結果は、点群を構成する各サンプル点の初期位置に対する依存性が強く、局所解に陥る傾向がある。つまり、期待される位置合わせの結果が得られる初期位置の範囲が狭いことを意味する。これについて、RANSAC(Random Sample Consensus)法を採用して、各サンプル点の初期位置をランダムに設定することも考えられるが、計算時間がさらに増加してしまう。
本発明は上記の点に鑑みてなされたものであり、2つのデータがそれぞれ示す物体間の方向や位置を合わせるための処理量を軽減することができ、初期位置に対する依存性を低くすることができるデータ処理装置、データ処理方法、及びデータ処理プログラムを提供する。
(1)本発明は上記の課題を解決するためになされたものであり、本発明の一態様は、第1のデータが示す第1の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第1の法線主成分として算出する主成分算出部と、第2のデータが示す第2の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第2の法線主成分から、前記第1の法線主成分との近似の度合いに基づいて前記第1の法線主成分に対応する第2の法線主成分を定める対応部と、前記第1の法線主成分の方向と前記第2の法線主成分の方向との間の相対的な方向を算出する相対方向算出部と、を備え、前記主成分算出部は、前記第1の物体の表面までの原点からの深さを大きさとして有する法線である第1の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第1の壁面主成分として算出し、前記対応部は、前記第2の物体の表面までの原点からの深さを大きさとして有する法線である第2の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第2の壁面主成分から、前記第1の壁面主成分との近似の度合いに基づいて前記第1の壁面主成分に対応する第2の壁面主成分を定め、前記第1の壁面主成分と、当該第1の壁面主成分に対応する第2の壁面主成分との差分に基づいて前記第1の物体と前記第2の物体との間の相対的な位置を算出する相対並進位置算出部を、さらに備えるデータ処理装置である。
(2)本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第1の物体の形状を表す複数の点からなる第1の点群データに基づいて前記第1の物体の表面の法線のうち、予め定めた密度よりも高密度で分布している領域における法線の重心を前記第1の法線主成分として算出する。
(3)本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第1の物体の表面の法線を、当該第1の物体の表面の法線間の距離に基づいて複数の法線の群に分類し、各群に属する法線毎に前記第1の法線主成分を算出する。
(4)本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第1の物体の形状を表す複数の点からなる第1の点群データに基づいて前記第1の物体の表面の深さ付き法線のうち、予め定めた密度よりも高密度で分布している領域における深さ付き法線の重心を前記第1の壁面主成分として算出する。
(5)本発明のその他の態様は、上述のデータ処理装置であって、前記主成分算出部は、前記第1の物体の表面の深さ付き法線を、当該第1の物体の表面の深さ付き法線間の距離に基づいて複数の深さ付き法線の群に分類し、各群に属する深さ付き法線毎に前記第1の壁面主成分を算出する。
(6)本発明のその他の態様は、データ処理方法において、第1のデータが示す第1の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第1の法線主成分として算出する主成分算出過程と、第2のデータが示す第2の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第2の法線主成分から、前記第1の法線主成分との近似の度合いに基づいて前記第1の法線主成分に対応する第2の法線主成分を定める対応過程と、前記第1の法線主成分の方向と前記第2の法線主成分の方向との間の相対的な方向を算出する相対方向算出過程と、を有し、前記主成分算出過程は、前記第1の物体の表面までの原点からの深さを大きさとして有する法線である第1の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第1の壁面主成分として算出し、前記対応過程は、前記第2の物体の表面までの原点からの深さを大きさとして有する法線である第2の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第2の壁面主成分から、前記第1の壁面主成分との近似の度合いに基づいて前記第1の壁面主成分に対応する第2の壁面主成分を定め、前記第1の壁面主成分と、当該第1の壁面主成分に対応する第2の壁面主成分との差分に基づいて前記第1の物体と前記第2の物体との間の相対的な位置を算出する相対並進位置算出過程を、さらに有するデータ処理方法である。
(7)本発明のその他の態様は、データ処理装置のコンピュータに、第1のデータが示す第1の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第1の法線主成分として算出する主成分算出手順、第2のデータが示す第2の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第2の法線主成分から、前記第1の法線主成分との近似の度合いに基づいて前記第1の法線主成分に対応する第2の法線主成分を定める対応手順、前記第1の法線主成分の方向と前記第2の法線主成分の方向との間の相対的な方向を算出する相対方向算出手順を実行させるためのデータ処理プログラムであって、前記主成分算出手順は、前記第1の物体の表面までの原点からの深さを大きさとして有する法線である第1の深さ付き法線のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第1の壁面主成分として算出し、前記対応手順は、前記第2の物体の表面までの原点からの深さを大きさとして有する法線である第2の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第2の壁面主成分から、前記第1の壁面主成分との近似の度合いに基づいて前記第1の壁面主成分に対応する第2の壁面主成分を定め、前記第1の壁面主成分と、当該第1の壁面主成分に対応する第2の壁面主成分との差分に基づいて前記第1の物体と前記第2の物体との間の相対的な位置を算出する相対並進位置算出手順を、さらに実行させるためのデータ処理プログラムである。
本発明によれば、2つのデータがそれぞれ示す物体間の方向や位置を合わせるための処理量を軽減することができ、初期位置に対する依存性を低くすることができる。
本発明の第1の実施形態に係るデータ処理装置の構成を示すブロック図である。 点群データの一例を示す図である。 法線ベクトルの一例を示す。 本実施形態に係るデータ処理を示すフローチャートである。 本発明の第2の実施形態に係るデータ処理装置の構成を示すブロック図である。 深さ情報付き法線ベクトルの一例を示す。 本実施形態に係るデータ処理を示すフローチャートである。 モデル点群の一例を示す図である。 誤差の初期位置依存性を示す図である。 処理時間の初期位置依存性を示す図である。 点群データを取得した環境の一例を示す写真である。 処理前の点群データの一例を示す図である。 処理後の点群データの一例を示す図である。
(第1の実施形態)
以下、図面を参照しながら本発明の実施形態について説明する。
図1は、本実施形態に係るデータ処理装置1の構成を示すブロック図である。
データ処理装置1は、データ入力部101、主成分算出部102、対応部103、回転算出部(相対方向算出部)104、データ出力部105、及び記憶部11を含んで構成され、各部はバス12で接続されている。
データ入力部101には、データ処理装置1の外部、例えば、3DLRFからクエリ点群データ(第1のデータ)が入力される。点群データは、予め定めた撮像領域に含まれる被写体(物体)の形状を所定の空間間隔でサンプリングしたサンプル点からなる点群を示すデータである。入力されたクエリ点群データが示す点群をクエリ点群(query point cloud)と呼び、クエリ点群を構成するサンプル点をクエリ点と呼ぶことがある。各サンプル点は、被写体(第1の物体)の表面の1点の座標を示す。データ入力部101は、入力されたクエリ点群データを主成分算出部102に出力する。データ入力部101は、例えば、データ入力インタフェースである。
クエリ点群データは、その他の機器、例えば、サーバ装置からLAN(Local Area Network)、インターネット等の通信路を介して受信した点群データであってもよい。
主成分算出部102は、データ入力部101から入力されたクエリ点群データQを構成するサンプル点qpiのそれぞれについて、そのサンプル点における被写体の表面に垂直な方向を示す法線(normal)を示す法線ベクトル(normal vector)qniを算出する。主成分算出部102は、例えば、ある注目サンプル点を頂点の1つとして共有する面を形成し、その注目サンプル点に隣接する2つのサンプル点の組を抽出する。
主成分算出部102は、注目サンプル点と抽出した2つのサンプル点の組から形成される面毎に法線ベクトルを算出し、算出した面毎の法線ベクトルを平均することによって、その注目サンプル点に係る法線ベクトルqniを算出する。この処理で注目サンプル点について算出した法線ベクトルqniは、頂点法線ベクトルと呼ばれる。算出した法線ベクトルqniは、3次元のベクトルである。法線ベクトルqniを用いることで各サンプル点qpiでの位置情報が欠落するが、被写体とモデルとの間の相対的な姿勢の差異(相対姿勢)である回転と相対的な位置の差異である並進とを別個に分離して扱うことができる。
主成分算出部102は、各サンプル点qpiで算出した法線ベクトルqniの大きさを、例えば、1に規格化する。
主成分算出部102は、各サンプル点qpiで算出した法線ベクトルqniの分布が所定の密度よりも高い密度で分布している部分における法線ベクトルqniを抽出する。
例えば、主成分算出部102は、各サンプル点qpiで算出した法線ベクトルqniの分布についてクラスタリング(clustering)を行って複数のクラスタ(cluster:群)に分類し、分類されたクラスタ毎の代表ベクトルを法線主成分として抽出する。これにより、所定の密度よりも高い密度で分布している部分の法線ベクトルを抽出することができる。主成分算出部102は、クラスタリングを行う際、例えば、階層的(hierarchical)クラスタリングの一手法である群平均法(group average method)を用いてもよい。
階層的クラスタリングは、次の過程(i)−(iv)を有する手法である:(i)それぞれ1個の法線ベクトルをメンバとして有するクラスタを設定する、(ii)各クラスタ間の類似度のうち、最も類似度が高いクラスタ同士を統合して1個のクラスタを形成する、(iii)クラスタの数が1個になった時に処理を終了し、そうでなければ(iv)に進む、(iv)(ii)で形成されたクラスタと他のクラスタのそれぞれとの類似度を算出し、(ii)に戻る。階層的クラスタリングは、主成分の数、つまりクラスタの数が既知でない場合でも適用可能であり、各クラスタに属する法線ベクトルの数によらずにクラスタ間の類似度を算出することができる。
群平均法は、この階層的クラスタリングにおいて、2つのクラスタのそれぞれから選択した法線ベクトル間の類似度を全ての法線ベクトルの組のそれぞれについて算出し、算出した類似度の平均値を2つのクラスタ間の類似度として定める手法である。群平均法によれば、最終的に求めようとする各クラスタの代表(例えば、重心)を法線ベクトルの代表として取得することができる。また、建造物等、人工的に構成された被写体を示す点群データに基づく法線ベクトルの分布は、ベクトル空間内の特定の領域に集中している明確な集団を形成していることが多いため、その領域毎に定めた法線ベクトルの代表によって、法線ベクトルの分布をモデル化することができる。
主成分算出部102は、分類されたクラスタのうち、そのクラスタに属する法線ベクトルの数が予め定めた法線ベクトルの数の閾値よりも少ないクラスタを排除する。これにより、外れ値、つまり所定の平面度よりも平面度が低い形状を示す部分のサンプル点に係る法線ベクトルが除去され、平面度がより高い形状を示すサンプル点に係る法線ベクトルが主に抽出される。
主成分算出部102は、法線ベクトル空間全体をその範囲とする1つのクラスタを除き、排除されずに残った複数のクラスタのそれぞれについて、代表値を算出する。代表値は、例えば、それぞれのクラスタに属する法線ベクトルの重心(centroid)である。残ったクラスタの数、つまり代表値の数をNCQと表す。以下の説明では、この法線ベクトルの重心を法線主成分と呼び、点群データからクラスタ毎に法線主成分を算出することを法線主成分分析と呼ぶことがある。
主成分算出部102は、2個のクラスタa,b間の類似度sabを、例えば、式(1)を用いて算出することができる。
式(1)において、c,cは、それぞれクラスタa,bの重心を示す。それぞれの重心は、3次元のベクトルである。||…||は、ベクトル…のノルム(norm)を示す。従って、式(1)で算出される類似度sabは、重心c,c間における余弦(cosine)を示す。
主成分算出部102は、2個のクラスタa,bを統合して形成された1個のクラスタの重心cabを、例えば、式(2)を用いて算出することができる。
式(2)において、n,nは、それぞれクラスタa,bに属する法線ベクトルの数を示す。従って、式(2)で算出される重心cabは、クラスタa,bの重心c,cを、それぞれのクラスタa,bに属する法線ベクトルの数n,nで重みづけた加重平均である。
主成分算出部102は、算出したクラスタ毎の代表ベクトル(例えば、重心)を法線主成分として含む法線主成分点群データを生成し、生成した法線主成分点群データを対応部103に出力する。従って、法線主成分点群データは、NCQ個の3次元のベクトルから形成される。位置合わせに係る処理対象がNCQ個の法線主成分に限定されるので、処理量が著しく低減する。
対応部103は、主成分算出部102から入力された法線主成分点群データ(クエリ点群に基づく)が示す法線主成分のそれぞれと、モデル点群(model point cloud)に基づく法線主成分点群データが示す法線主成分のいずれかとの対応付け(ペアリング:pairing)を行う。
モデル点群とは、位置合わせの目標とする目標物(モデル、第2の物体)の形状を所定の空間間隔でサンプリングしたサンプル点からなる点群である。モデル点群を示すデータをモデル点群データ(第2の点群データ)と呼ぶことがある。モデル点群に基づく法線主成分点群データは、例えば、モデル点群データについて上述した法線主成分分析を行ってクラスタ毎に算出した重心から形成されるデータである。
モデル点群に基づく法線主成分点群データは、予め記憶部11に記憶され、記憶部11から読み出されたものであってもよいし、予め主成分算出部102から入力されたものでもよい。また、モデル点群に基づく法線主成分点群データは、CAD(Computer Aided Design)技術を用いて生成した目標物の形状を示すモデル点群データから生成されたものであってもよい。
対応部103は、法線主成分点群データが示す法線主成分cmjとの近似の度合いに基づいて、クエリ点群に基づく法線主成分点群データが示す法線主成分cqiのそれぞれに対応する法線主成分cmjを定める。
対応部103は、例えば、法線主成分cqiに対応する法線主成分cmjを、式(3)を用いて選択する。
式(3)において、f(i)は、法線主成分cqiを識別するインデックスiに対応するインデックスjを示す。jは、法線主成分cmjを識別するインデックスを示す。即ち、f(i)は、法線主成分cqiと法線主成分cmjとの対応関係を示す。個々の法線主成分cqiとこれに対応する法線主成分cmjとの組をペア(pair)と呼ぶ。NCMは、法線主成分点群データが示す法線主成分cmjの個数を示す。arg max … は、…を最大とするjを与えることを示す。また、各法線主成分は1に規格化されているところ、式(3)は、法線主成分cqiとの内積が最も大きくなる法線主成分cmj、即ち、法線主成分cqiとのなす角度が最も小さい法線主成分cmj、言い換えれば法線主成分cqiに最も近似した法線主成分cmjが選択される。対応部103は、この選択をNCQ個の法線主成分cqiのそれぞれについて行う。
対応部103は、法線主成分cqiと法線主成分cmjとの対応関係を示す法線主成分対応データを生成し、生成した法線主成分対応データ、モデル点群並びにクエリ点群それぞれに基づく法線主成分点群データを回転算出部104に出力する。
回転算出部104は、対応部103から法線主成分対応データ、クエリ点群ならびにモデル点群に基づく法線主成分データが入力される。回転算出部104は、法線主成分対応データを参照して、クエリ点群に基づく法線主成分データとモデル点群に基づく法線主成分データとを用いて回転行列Rを算出する。回転行列Rは、クエリ点群に基づく法線主成分cqiが示す方向を基準とした、その法線主成分cqiに対応するモデル点群に基づく法線主成分cmjの相対的な方向(相対姿勢、回転誤差ともいう)を、法線主成分のペア間で総合した方向への回転変換を示す行列である。従って、回転行列Rを算出することにより、法線主成分cqiの方向を基準とした法線主成分cmjの相対的な方向を算出することができる。回転行列Rは、クエリ点群が示す物体の方向を基準としたモデル点群が示す物体の方向との間の相対姿勢を近似し、その相対姿勢を補償する際に用いられる。
回転算出部104は、回転行列Rを算出する際、例えば、最小二乗法を用いる。最小二乗法により回転行列Rを算出する手法については、例えば、次の文献に記載されている: K.S.Arun,T.S.Huang,and S.D.Blostein,“Least−Squares Fitting of Two 3D Point Sets”,IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. PAMI−9,No.5,pp.698−700,SEPMTEMBER 1987。
最小二乗法では、回転算出部104は、モデル点群に基づく法線主成分cmjと、その法線主成分に対応するクエリ点群に基づく法線主成分cqiに回転行列Rを乗算した回転補正後の法線主成分Rcqiとの間の誤差関数を算出する。誤差関数は、例えば、法線主成分cmjから回転補正後の法線主成分Rcqiの差分ベクトルのフロベニウスノルム(Frobenius norm)である。誤差関数はこれに限られず、法線主成分cmjと回転補正後の法線主成分Rcqiとの間の誤差の大きさを示すものであれば、いかなる関数(例えば、差分ベクトルのユークリッドノルム(Eucledean norm))であってもよい。
そして、回転算出部104は、算出した誤差関数を小さくするように、例えば、誤差関数を最小にする回転行列Rを算出する。回転行列Rは、原点を中心とする回転変換を示す3行3列の行列である。回転行列Rは、例えば、極座標系における水平面内の方位角(azimuth)と水平面からの仰角(elevation)で表されてもよいし、直交座標系での各座標軸周りの回転角(例えば、ヨー(yaw)角、ピッチ(pitch)角、ロール(roll)角)で表されてもよい。これにより、クエリ点群とモデル点群との間の相対姿勢が回転行列Rによって定められる。
なお、回転算出部104は、クエリ点群に基づく法線主成分cqiとその法線主成分cqiに対応するモデル点群に基づく法線主成分cmjとの間の回転軸と回転量を、それぞれペア間で平均することで相対姿勢を算出してもよい。回転軸は、法線主成分cqiとその法線主成分cqiに対応する法線主成分cmjとの外積に平行な軸である。回転量は、その回転軸まわりの法線主成分cqiと法線主成分cmjとのなす角度である。この回転軸の方向と、回転量によっても回転行列Rを定めることができる。
回転算出部104は、算出した回転行列Rを示す回転データをデータ出力部105に出力する。
データ出力部105は、回転算出部104から入力された回転データをデータ処理装置1の外部に出力する。データ入力部101は、例えば、データ入力インタフェースである。
また、データ出力部105は、データ入力部101から入力されたクエリ点群データが示すサンプル点毎の座標に、回転算出部104で算出した回転行列Rを乗じて得られる回転後の座標を示す回転座標データをデータ処理装置1の外部に出力してもよい。これにより、クエリ点群とモデル点群との間の相対姿勢が補償されたサンプル毎の座標が得られる。
(点群データの一例)
次に、点群データの一例について説明する。
図2は、点群データの一例を示す図である。
図2に示す点群データは、建造物の廊下とロビーとが交差した部位とその周辺における内壁の形状をサンプリングしたサンプル点で形成される。この形状の基本的構成は、Z軸方向の長さ(高さ)がX軸方向に進むほど長くなる台形をX−Z平面の断面として有する 四角柱が、Y軸方向の長さ(幅)が他の方向よりも長い直方体のほぼ中央部で結合して形成される。X軸,Y軸,Z軸は、それぞれ3次元直交座標系を構成する互いに直交する座標軸の方向を示す。
(法線ベクトルの一例)
図3は、法線ベクトルの一例を示す。
図3(a)は、図2に示す点群データに基づいて算出されたサンプル毎の長さ1の法線ベクトルを、その起点を原点Oに配置して形成された終点の分布を示す。法線ベクトルの終点は、半径が1である単位球面上に分布しているため、この法線ベクトルの分布を法線球と呼ぶ。法線球の濃度は法線ベクトルの分布の密度を示す。濃度が高いほど法線ベクトルの密度が高く、濃度が低いほど法線ベクトルの密度が低い。
図3(b)、(c)は、それぞれ図3(a)に示す法線球のうち法線ベクトルの密度が所定の密度の閾値よりも高い部分を示す。図3(b)よりも図3(c)の方が、密度の閾値が高い。図3(c)は法線ベクトルの密度が高い部分が狭い領域、特に6個のそれぞれ孤立した領域c1−c6に集中していることを示す。これは、点群データが示すサンプル点が示す物体の表面が、主に特定の方向に向けられていることを示す。上述したように、主成分算出部102は、サンプル点毎の法線ベクトルに対してクラスタリングを行い、クラスタ毎の代表としてその重心を法線主成分と定めるが、定められた法線主成分は、法線ベクトルの密度が高い領域の重心に近似する。従って、6個の法線主成分が算出される。
本実施形態では、このような法線ベクトルに基づく法線主成分を用いることで、原点を中心とする回転変換を示す回転行列Rを算出する際にクエリ点群やモデル点群が示す物体の表面の形状や、それらの点群間での相対並進位置が捨象される。これに対し、各点群が示す物体間の姿勢の差異を示す情報が維持されるので、回転行列Rを精度よく求めることができる。また、上述したように回転行列Rを算出する際の基礎となる点群間での法線主成分の対応付けが、法線主成分間の近似の度合いの指標として、それらの間のなす角度に基づいて行われる。これらのことから、回転行列Rを算出する際の演算量を著しく低減させることができる。
法線ベクトルqniの分布の密度は、点群が示す形状の平面度に依存する。平面度とは、形状が平面に近似する度合い、つまり凹凸の少なさの度合いである。平面度が高いほど、複数の法線ベクトルの向きが、特定の1つの方向に整列し、平面度が低いほど、複数の法線ベクトルの向きが、乱雑に分布する。そのため、平面度が高いほど法線ベクトルqniの分布の密度が高くなる。法線ベクトルがある1点を中心に分布している場合、被写体の形状のうち、その法線ベクトルが示す方向に垂直な面が支配的であるためである。
なお、主成分算出部102は、法線主成分を算出する際、サンプル点毎の法線ベクトルに対してクラスタリングを行うことに代え、所定の密度の閾値よりも法線ベクトルの密度が高い領域の法線ベクトルを抽出してもよい。その場合、主成分算出部102は、抽出した領域毎に法線ベクトルの代表(例えば、重心)を算出し、算出した代表を法線主成分と定める。
法線ベクトルの密度を示す指標は、例えば、法線方向の分散値qσiである。法線方向の分散値qσiは、そのサンプル点iから予め定めた範囲内におけるサンプル点毎の法線ベクトルの分散である。従って、分散値qσiが小さいほど平面度が高いことを示し、分散値qσiが大きいほど平面度が低いことを示す。
(データ処理の流れ)
次に、本実施形態に係るデータ処理について説明する。
図4は、本実施形態に係るデータ処理を示すフローチャートである。
(ステップS101)データ入力部101は、被写体の形状を示すクエリ点群データを取得する。その後、ステップS102に進む。
(ステップS102)主成分算出部102は、クエリ点群データが示す被写体の表面の法線を示す方向を示す法線ベクトルをサンプル点毎に算出する。その後、ステップS103に進む。
(ステップS103)主成分算出部102は、各サンプル点qpiで算出した法線ベクトルqniの分布についてクラスタリングを行って複数のクラスタに分類し、分類されたクラスタ毎の代表ベクトルを法線主成分として定める。これにより、法線ベクトルの分布が所定の密度よりも高い密度で分布している部分の法線ベクトルの代表が法線主成分として抽出される。その後、ステップS104に進む。
(ステップS104)対応部103は、クエリ点群に基づく法線主成分のそれぞれと、モデル点群に基づく法線主成分のいずれかとの対応付けを、両者間の近似の度合いに基づいて行う。その後、ステップS105に進む。
(ステップS105)回転算出部104は、クエリ点群に基づく法線主成分と、その法線主成分に対応するモデル点群に基づく法線主成分との間の相対的な方向を示す回転行列を算出する。その後、図4に示す処理を終了する。
本実施形態において、生成される回転データは、例えば、クエリ点群が示す第1の物体の姿勢とモデル点群が示す第2の物体との間の姿勢の差異を示すところ、第2の物体の姿勢が既知であれば、第1の物体の姿勢を取得することができる。この回転データを用いて、第1の物体の姿勢の制御、第1の物体の姿勢を基準としたロボットの動作の制御、その他の操作対象物の姿勢の制御、等に用いることができる。
上述したように、本実施形態では、第1のデータ(例えば、クエリ点群データ)が示す第1の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表を第1の法線主成分(例えば、クエリ点群に基づく法線主成分)として算出する。また、本実施形態では、第2のデータ(例えば、モデル点群データ)が示す第2の物体の表面の法線のうち、所定の密度よりも高密度で分布している領域の法線の代表である第2の法線主成分(例えば、モデル点群に基づく主成分)から、第1の法線主成分との近似の度合いに基づいて第1の法線主成分に対応する第2の法線主成分を定める。また、本実施形態では、第1の法線主成分の方向と前記第2の法線主成分の方向との間の相対的な方向(例えば、回転行列)を算出する。
従って、第1の物体の表面の法線を代表する第1の法線主成分の方向と第2の物体の表面の法線を代表する第2の法線主成分の方向との間の相対的な方向を直接算出するため処理量が著しく低減する。
また、第1の法線主成分とこれに対応する第2の法線主成分を用いることで第1の物体の方向と第2の物体の方向とを合わせる際に、第1の物体の表面が疎らな領域や曲率が高い領域を一部に含んでいても安定した処理が実現する。
(第2の実施形態)
次に、本発明の第2の実施形態について説明する。上述した実施形態と同一の構成については、同一の符号を付して説明を援用する。
以下、図面を参照しながら本発明の実施形態について説明する。
図5は、本実施形態に係るデータ処理装置2の構成を示すブロック図である。
データ処理装置2は、データ処理装置1(図1)において、主成分算出部102及び対応部103に代えて主成分算出部202及び対応部203を備え、さらに相対並進位置算出部206及び座標変換部207を含んで構成される。
主成分算出部202は、主成分算出部102(図1)と同様に、データ入力部101から入力されたクエリ点群データに基づいてサンプル点毎に法線ベクトルを算出し、クラスタリングを行ってクラスタ毎に法線主成分を算出する。
主成分算出部202は、クエリ点群データが示すサンプル点毎の座標に回転算出部104で算出した回転行列Rを乗算して回転後の座標を算出して、クエリ点群が示す物体の方向についてモデル点群が示す物体の方向との間の相対姿勢を補償する。なお、主成分算出部202は、回転行列Rを乗算することに代え、回転算出部104で回転行列Rを算出する過程で算出された回転後の座標を採用してもよい。
そして、主成分算出部202は、サンプル点毎にその回転後の座標について法線ベクトルを算出し、クエリ点群データが示す物体の形状について相対姿勢が補償された法線主成分が算出される。これにより、クエリ点群とモデル点群との間では、相対並進位置が支配的となるため、本来6次元の処理である相対並進位置の算出を3次元の処理に縮退させることができ、処理量を格段に低減することができる。6次元とは、3次元空間内の相対並進位置の他、3次元の各座標軸周りの回転も含む。
主成分算出部202は、サンプル点毎に回転後の座標について深さ(depth)を算出する。深さとは、原点から注目するサンプル点が属する平面(つまり、壁面)までの距離である。そして、主成分算出部202は、サンプル点毎に算出した法線ベクトルに、そのサンプル点について算出した深さを乗算し、その深さを大きさとして有する深さ情報付き法線ベクトルを算出する。深さ情報付き法線ベクトルは、そのサンプル点の座標を示す座標ベクトルにおける、その法線ベクトルの方向への余弦成分のベクトルに相当する。よって、主成分算出部202は、サンプル点毎の座標ベクトルと法線ベクトルに基づいて深さ情報付き法線ベクトルを算出してもよい。
主成分算出部202は、サンプル点毎に算出した深さ情報付き法線ベクトルのうち、その分布が所定の密度よりも高い密度で分布している部分の代表となる深さ情報付き法線ベクトルを抽出する。以下の説明では、この代表となる深さ情報付き法線ベクトルを壁面主成分と呼ぶ。
ここで、主成分算出部202は、サンプル点毎に算出した深さ情報付き法線ベクトルについて、上述した法線主成分分析と同様な処理を行うことで壁面主成分を算出することができる。この処理を特に壁面主成分分析と呼ぶ。
壁面主成分分析において、主成分算出部202は、各サンプル点で算出した深さ情報付き法線ベクトルについてクラスタリングを行って複数のクラスタに分類し、分類されたクラスタ毎の代表ベクトルを壁面主成分として抽出してもよい。ここで、主成分算出部202は、例えば、群平均法を用いてもよい。
主成分算出部202は、2つのクラスタa,b間の類似度s(ハット)abを、例えば、式(4)を用いて算出してもよい。ここで、「(ハット)」という記載は、その直前の文字の真上にハット記号(circumflex)が付されていることを示す。ここでは、ハット記号は、壁面主成分に係ることを示す。
式(4)において、c(ハット),c(ハット)は、それぞれクラスタa,bの代表ベクトル、例えば、重心を示す。δは、類似度s(ハット)abが発散することを防止するための、予め定めた微小な実数を示す。また、c(ハット),c(ハット)は、それぞれクラスタa,bの重心を示す。ここで、分母にc(ハット)−c(ハット)のノルムを乗じるのは、クラスタ間の距離が小さくなるほど、類似度s(ハット)abを増加させるという効果を強調するためである。
また、主成分算出部202は、2個のクラスタa,bを統合して形成された1個のクラスタの重心c(ハット)abを、例えば、式(5)を用いて各クラスタにおける壁面主成分の重心cabを代表ベクトルとして算出することができる。
主成分算出部202は、算出したクラスタ毎の代表ベクトル(例えば、重心)からなる壁面主成分点群データを生成し、生成した壁面主成分点群データを対応部203に出力する。壁面主成分点群データは、N(ハット)CQ個の3次元のベクトルから形成される。従って、位置合わせに係る処理対象をN(ハット)CQ個の壁面主成分に限定することで、処理量を著しく低減することができる。
対応部203は、主成分算出部202から入力されたクエリ点群に基づく壁面主成分点群データが示す壁面主成分のそれぞれと、モデル点群に基づく壁面主成分点群データが示す壁面主成分のいずれかとの対応付けを、両者間の近似の度合いに基づいて行う。
対応部203は、例えば、クエリ点群に基づく壁面主成分点群データが示す壁面主成分c(ハット)qiに対応するモデル点群に基づく壁面主成分点群データが示す壁面主成分c(ハット)mjを、式(6)を用いて選択する。
式(6)において、g(i)は、壁面主成分c(ハット)qiを識別するインデックスiに対応するインデックスjを示す。jは、壁面主成分c(ハット)mjを識別するインデックスを示す。即ち、g(i)は、壁面主成分c(ハット)qiと壁面主成分c(ハット)mjとの対応関係、言い換えれば壁面主成分c(ハット)qiとこれに対応する壁面主成分c(ハット)mjとのペアを示す。N(ハット)CMは、壁面主成分点群データが示す壁面主成分c(ハット)mjの個数を示す。N(ハット)CM、N(ハット)CQは、それぞれ3又は3よりも大きい整数である。
従って、式(6)は、壁面主成分c(ハット)qiとの内積の、壁面主成分cqiとの間の距離に対する比が最も大きくなる壁面主成分c(ハット)mjを選択することを示す。これにより、深さ及び方向ともに壁面主成分c(ハット)qiに最も近似した法線主成分c(ハット)mjが選択される。対応部203は、この選択をN(ハット)CM個の壁面主成分c(ハット)qiのそれぞれについて行う。
対応部203は、式(7)に示すように、モデル点群に基づく壁面主成分c(ハット)mg(i)から、これに対応するクエリ点群に基づく壁面主成分c(ハット)qiの差分をペア間誤差ベクトルνとして、その壁面主成分c(ハット)qiに係るペア毎に算出する。
対応部203は、壁面主成分c(ハット)qiと壁面主成分c(ハット)mjとの対応関係と、壁面主成分c(ハット)qiに対応するペア間誤差ベクトルνを示すペア間誤差データを生成する。対応部203は、生成したペア間誤差データとモデル点群に基づく壁面主成分点群データとを対応付けて相対並進位置算出部206に出力する。
なお、対応部203は、対応部103と同様に、クエリ点群に基づく法線主成分点群データが示す法線主成分のそれぞれと、モデル点群に基づく法線主成分データが示す法線主成分との対応付けを行う。
モデル点群に基づく壁面主成分点群データは、予め記憶部11に記憶され、記憶部11から読み出されたものであってもよいし、予め主成分算出部202から入力されたものでもよい。また、モデル点群に基づく壁面主成分点群データは、CAD(Computer Aided Design)技術を用いて生成した目標物の形状を示すモデル点群データから生成されたものであってもよい。
相対並進位置算出部206には、対応部203からペア間誤差データとモデル点群に基づくモデル壁面主成分点群データが入力される。相対並進位置算出部206は、ペア間誤差データが示すペア間誤差ベクトルνと、これに対応するモデル点群に基づく壁面主成分点群データが示す壁面主成分c(ハット)qiに基づいて、並進誤差ベクトルεを算出する。並進誤差ベクトルεは、クエリ点群に基づく壁面主成分データが示す壁面主成分c(ハット)qiと、その壁面主成分c(ハット)qiに対応する壁面主成分c(ハット)mjであってモデル点群に基づく壁面主成分データが示す壁面主成分c(ハット)mjとの間の相対並進位置を示す3次元のベクトルである。並進誤差ベクトルεは、壁面主成分c(ハット)qiの位置を基準とした壁面主成分c(ハット)mjの相対的な位置(相対並進位置)を、ペア間で総合した相対並進位置を示す。
並進誤差ベクトルεは、クエリ点群とモデル点群との間の相対並進位置に近似する。そのため、並進誤差ベクトルεは、クエリ点群とモデル点群との間の相対並進位置を補償する際に用いられる。
相対並進位置算出部206は、少なくとも3個のペア間誤差ベクトルνを用いて並進誤差ベクトルεを算出する。これは、クエリ点群が示す物体とモデル点群が示す物体との間には姿勢の差異があるため、この姿勢の差異(つまり、相対姿勢)を補償するためである。相対並進位置算出部206は、例えば、式(8)を用いて並進誤差ベクトルεを算出する。
式(8)において、Wは、重み係数wの総和である。mは、N(ハット)CQ個の壁面主成分cqiのうちの互いに異なる3個からなる組のそれぞれを識別するインデックスである。このような壁面主成分の組はN(ハット)CQ個あるため、mは、1からN(ハット)CQまでの間のいずれかの整数を示す。各組を構成する3個の壁面主成分を、それぞれcqj,cqk,cqlと表す。ここで、j,k,lは、それぞれ、壁面主成分を識別するインデックスであり、互いに異なる値をとる(j≠k≠l)。M(ハット)CQは、N(ハット)CQである。
従って、壁面主成分εは、3個の壁面主成分からなる組m毎の並進誤差ベクトルε(ハット)を、それぞれ重み係数wで乗算して得られる乗算値を組m間で加算し、加算した値を総和Wで正規化して算出される。
重み係数wは、式(9)に示されるように、組mに係る3個の壁面主成分cqj,cqk,cqlに基づいて算出される。
即ち、式(9)は、3個の壁面主成分c(ハット)qj,c(ハット)qk,c(ハット)qlのうちの2個からなる壁面主成分の対毎にその2つの壁面主成分の外積を算出し、算出した外積のノルムを対間で乗算し、乗算した値を重み係数wとして算出することを示す。
壁面主成分の組m毎の並進誤差ベクトルε(ハット)は、式(10)に示されるように、その組mを構成する3個の壁面主成分c(ハット)qj,c(ハット)qk,c(ハット)qlと、それらにそれぞれ対応するペア間誤差ベクトルν,ν,νに基づいて算出される。
式(10)において、[…]−1は、行列…の逆行列を示す。
従って、重み係数wで乗算された並進誤差ベクトルε(ハット)は、ペア間誤差ベクトルν,ν,νを、それぞれ対応する壁面主成分c(ハット)qj,c(ハット)qk,c(ハット)qlに射影し、射影して得られる成分をさらにX軸方向、Y軸方向、Z軸方向に射影した成分を示す。このようにペア間誤差ベクトルν,ν,νを、互いに直交した成分に射影し、射影した成分を並進誤差ベクトルε(ハット)として取得することで、高い精度で位置決めをすることができる。また、全ての壁面主成分の組m毎の並進誤差ベクトルε(ハット)を、重み係数wで加重平均をとることで、ペア間の相対姿勢の差異を緩和することができる。
相対並進位置算出部206は、算出した並進誤差ベクトルεを示す並進データを座標変換部207に出力する。
座標変換部207には、データ入力部101からクエリ点群データと、回転算出部104から回転行列Rを示す回転データ、ならびに相対並進位置算出部206から並進誤差ベクトルεを示す並進データと、が入力される。
座標変換部207は、クエリ点群データが示すサンプル点毎の座標を、回転行列Rと並進誤差ベクトルを用いて変換する。ここで、座標変換部207は、サンプル点毎の座標(ベクトル)に回転行列Rを乗算して回転後の座標を算出し、算出した座標に並進誤差ベクトルεを加算して、変換後の座標を算出する。この演算を示す4行4列の同次変換行列Sを、式(11)に示す。
式(11)において、0は、各要素値が0である3行1列のベクトルである。1は、同次変換行列Sの第4行第4列の要素値である。このように変換後の座標を算出することによって、モデル点群を目標位置としたクエリ点群の方向合わせ及び位置合わせを行うことができる。
なお、座標変換部207は、回転行列Rを乗算することに代え、回転算出部104もしくは主成分算出部202で回転行列Rを算出する過程で算出された回転後の座標を採用してもよい。
座標変換部207は、サンプル毎の変換後の座標を示す変換座標データを、データ出力部105を介してデータ処理装置2の外部に出力する。出力される変換座標データは、第2の物体の位置及び姿勢に合うように、相対姿勢及び相対並進位置が補償されたクエリ点群データが示す第1の物体の形状を示す。ここで、データ出力部105は、変換座標データに代えて、もしくは変換座標データとともに並進データを出力してもよいし、更に回転算出部104で生成された回転データを出力してもよい。
(深さ情報付き法線ベクトルの一例)
図6は、深さ情報付き法線ベクトルの一例を示す。
図6(a)は、図2に示す点群データに基づいて算出されたサンプル毎の深さ情報付き法線ベクトルを、その起点を原点Oに配置して形成された終点の分布を示す。法線ベクトルの終点は、それぞれの深さを半径とする曲面上に分布する。この法線ベクトルの分布を深さ情報付き法線球と呼ぶ。深さ情報付き法線球の濃度は、深さ情報付き法線ベクトルの分布の密度を示す。濃度が高いほど深さ情報付き法線ベクトルの密度が高く、濃度が低いほど深さ情報付き法線ベクトルの密度が低い。
図6(b)は、図6(a)が示す深さ情報付き法線ベクトルの分布をZ軸の負方向に向けて観察して得られる平面図である。この平面図では、深さ情報付き法線ベクトルの分布に加え、図2に示す点群を薄く重ね合わせて表示されている。
図6(c)は、図6(b)のA領域を拡大した拡大図である。
図6(b)、(c)は、深さ情報付き法線ベクトルの分布が高い密度で集中している点Pが存在する。この点Pは、その座標を示す位置ベクトルと同一の方向に法線を有する壁面Lが、その位置ベクトルのノルムに相当する深さだけ離れた位置に存在することを示す。
従って、サンプル点毎の深さ情報付き法線ベクトルについてクラスタリングを行って、その分布の密度が高い領域を抽出し、壁面主成分として算出されたその領域の代表をもって、壁面の法線方向と位置に関する情報を維持することができる。
(処理の流れ)
次に、本実施形態に係るデータ処理について説明する。
図7は、本実施形態に係るデータ処理を示すフローチャートである。
図7に示すデータ処理は、ステップS101−S105(図4)とステップS206−S210を有する。ステップS101−S105の処理が終了した後、ステップS206に進む。
(ステップS206)主成分算出部202は、クエリ点群データが示すサンプル点毎の座標に回転行列Rを乗算して回転後の座標を算出する。これにより、クエリ点群についてモデル点群との間の相対姿勢を補償する。その後、ステップS207に進む。
(ステップS207)主成分算出部202は、サンプル点毎に回転後の座標について法線ベクトルを算出し、算出した法線ベクトルと座標に基づいて深さ情報付き法線ベクトルを算出する。その後、ステップS208に進む。
(ステップS208)主成分算出部202は、各サンプル点で算出した深さ情報付き法線ベクトルについてクラスタリングを行って複数のクラスタに分類し、分類されたクラスタ毎の代表ベクトルを壁面主成分として算出する。その後、ステップS209に進む。
(ステップS209)対応部203は、クエリ点群に基づく壁面主成分のそれぞれと、モデル点群に基づく壁面主成分のいずれかとの対応付けを行う。その後、ステップS210に進む。
(ステップS210)相対並進位置算出部206は、クエリ点群に基づく壁面主成分の組と、その壁面主成分のそれぞれに対応するペア間誤算ベクトルに基づいてクエリ点群とモデル点群との間の相対並進位置を示す並進誤差ベクトルを算出する。その後、図7に示す処理を終了する。
本実施形態において算出した相対並進位置を示す並進データは、モデル点が表す第1の物体の位置を基準としたクエリ点群が表す第2の物体の位置を示す。この並進データを用いて、第1の物体の位置の制御、第1の物体の位置を基準としたロボットの動作の制御、その他の操作対象物の位置の制御、等に用いることができる。
その他、並進データは、重力加速度等の加速度を検出する加速度センサを備えた電子機器において、その位置の補償に用いることができる。そのような電子機器には、検出した加速度を2階積分することによって自機の位置を算出する位置推定機能を備えた多機能携帯電話機(いわゆるスマートフォン)がある。その他、自機が備える駆動機構の動作状態(例えば、モータの回転速度)に基づいて自機の位置を算出する位置算出機能を備えたロボットがある。
この位置算出機能で算出される位置の精度は、計算誤差の累積やノイズにより、一般に低くなる傾向がある。そこで、データ処理装置2が、その電子機器を撮影して得られるクエリ点群データに基づいて所定の時間間隔(例えば、1分)で算出した相対並進位置を示す並進データを生成する。そして、この位置算出機能は、生成された並進データが示す相対並進位置を自機の位置の原点として採用して、算出した位置を逐次に補正することができる。
(評価実験)
次に、本実施形態による効果を示すために行った評価実験について説明する。ここでは、(1)初期位置依存性評価実験、(2)実時間性評価実験、(3)堅牢性評価実験、と3つのそれぞれ異なる観点で行われた評価実験について述べる。いずれの評価実験でも比較対象として、従来技術としてICP法による実験結果と比較する。
まず、初期位置依存性評価実験について説明する。初期位置依存性評価実験では、クエリ点群に対して本実施形態によって回転行列Rと並進誤差ベクトルεを算出し、目標とする回転行列Rと並進誤差ベクトルεとの誤差をそれぞれ補正量の誤差として測定する。これらの操作を、一定の位置に配置されたモデル点群を用いて複数の予め定めた初期位置のそれぞれについて実行する。初期位置は、回転行列Rを1°間隔、並進誤差ベクトルεを0.1m間間隔で設定する。但し、このモデル点群は、X−Y平面内の回転対称性を有するため、X座標、Z座標、Y軸回りの回転角(仰角)、Z軸回りの回転角(方位角)のそれぞれに初期位置を分布させた。
また、それぞれの初期位置について大局解が得られるか否かを調査する。大局解が得られない場合には、局所解に陥る場合がある。大局解が得られるか否かの判断は、誤差が予め期待される誤差の閾値を下回るか否かによって行う。これにより、大局解を導くことができる初期位置の範囲を示す収束限界も評価値として扱う。
図8は、モデル点群の一例を示す図である。
図8に示すモデル点群は、初期位置依存性評価実験及び実時間性評価実験で用いた点群である。この点群は、建造物の内壁で構成される屋内の形状を模擬する450万個のサンプル点で形成される。模擬している形状は、長手方向がそれぞれX方向、Y方向である2つの直方体がそれぞれ中心部で交差して形成される。但し、標準偏差が5cmであるガウシアンノイズが重畳されている。この点群が網羅する空間は、X,Y,Z方向、それぞれの方向の長さが、15m、15m、5mである。点群の最大密度、つまり解像度は、1m当たり1万サンプルである。
図9は、誤差の初期位置依存性を示す図である。
図9(a)は、方位角の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図9(a)において、縦軸、横軸は、それぞれ方位角の誤差、初期の方位角を示す。◆印、×印は、それぞれ本実施形態、従来技術を示す。収束限界を与える誤差の閾値を1°とする。
これによれば、本実施形態では、方位角の誤差は、概ね0.01°未満と極めて誤差が小さく、初期の方位角に依存しない。但し、初期の方位角が−89°、89°の場合、それぞれ誤差が90°となる。従って、収束限界は88°となる。この現象は、モデル点群が90°(4相)の回転対称性を有するために、90°回転した方向に位置合わせが行われてしまうことによる。
また、大局解が得られたか否かにより、誤差が大きく異なることから収束限界を越えたか否かの判定を安定して行うことができる。
これに対して、従来技術では方位角の誤差が−9°〜9°の範囲では、方位角の誤差は1°未満となる。また、初期の方位角が−10°以下又は10°以上の範囲では、方位角の誤差がいずれも10°以上と急激に増加し、初期の方位角の絶対値が大きくなるほど方位角の誤差が増加する。これより、大局解を得ることができる収束限界は9°となる。
一般に、n相対称を有する点群の位置合わせの収束限界の上限は360/n度(この例では、90度)となる。この上限でそれぞれの収束限界を正規化すると、正規化した収束限界は本実施形態では0.98、従来技術では0.10となる。この結果は、本実施形態の方が広範に高精度の位置合わせを実現できることを示す。
図9(b)は、仰角の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図9(b)において、縦軸、横軸は、それぞれ仰角の誤差、初期の仰角を示す。これによれば、本実施形態では、仰角の誤差が概ね0.01°未満と極めて小さく、初期の仰角に依存しない。但し、初期の仰角が−89°、89°の場合、それぞれ誤差が90°となる。これより、大局解を得ることができる収束限界は88°となる。
これに対して、従来技術では仰角の誤差が−6°〜6°の範囲では、仰角の誤差は1°未満となる。また、初期の仰角が−7°以下又は7°以上の範囲では、初期の仰角の絶対値が大きくなるほど仰角の誤差が増加する傾向が現れる。これより、大局解を得ることができる収束限界は6°となる。
図9(c)は、X座標の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図9(c)において、縦軸、横軸は、それぞれX座標の誤差、初期のX座標を示す。収束限界を与える誤差の閾値を0.1mとする。
これによれば、本実施形態では、X座標の誤差は概ね0.001m未満と極めて小さく、初期のX座標に依存しない。但し、初期のX座標が−5.0m、5.0mの場合、誤差が3.3mとなる。よって、収束限界は、4.9mとなる。
これに対して、従来技術ではX座標の誤差が−1.6m〜1.6mの範囲では、X座標の誤差は概ね0.1m未満となる。また、初期のX座標の誤差が−1.7m以下又は1.7m以上の範囲では、X座標の誤差は0.1mを越え初期のX座標の絶対値が大きくなるほどX座標の誤差が増加する。よって、大局解を得ることができる収束限界は1.6mとなる。
図9(d)は、Z座標の誤差の初期位置依存性を本実施形態、従来技術のそれぞれについて示す。図9(d)において、縦軸、横軸は、それぞれZ座標の誤差、初期のZ座標を示す。これによれば、本実施形態では、Z座標の誤差は概ね0.001m未満と極めて小さく、Z座標に依存しない。但し、初期のZ座標が−5.0m、5.0mの場合、誤差が3.3mとなる。よって、収束限界は、4.9mとなる。
これに対して、従来技術ではZ座標の誤差が−1.8m〜1.8mの範囲では、Z座標の誤差は0.2m未満となる。また、初期のZ座標の誤差が−1.9m以下又は1.9m以上の範囲では、Z座標の誤差は0.1mを越え初期のZ座標の絶対値が大きくなるほどZ座標の誤差が増加する。よって、大局解を得ることができる収束限界は1.8mとなる。
以上に述べたように、初期位置依存性評価実験により、本実施形態によれば、各初期位置について高精度でクエリ点群について位置合わせを行うことができること、大局解を得ることができる初期位置の範囲が広くなることが示される。
次に、実時間性評価実験について説明する。ここでは、実時間性とは、処理の高速性と初期位置の差異に応じた処理時間安定性を含みうる概念である。実時間性評価実験では、複数の予め定めた初期位置のそれぞれについてクエリ点群の位置合わせに要する処理時間を本実施形態、従来技術のそれぞれについて観測した。但し、初期位置の設定において、X方向にのみ0.1m間隔で分布する並進誤差ベクトルεを設定し、つまり回転を与えない。なお、実時間評価実験で用いたクエリ点群、モデル点群は、初期位置依存性評価実験で使用したものとそれぞれ同一のものである。
図10は、処理時間の初期位置依存性を示す図である。
◆印、×印は、それぞれ本実施形態、従来技術を示し、○印は収束限界を示す。縦軸、横軸は、それぞれ処理時間、初期のX座標を示す。
本実施形態では、処理時間は、いずれも0.524−0.537秒と従来技術の数十秒単位よりも格段に短く、初期のX座標にも依存しない。これは、本実施形態では、従来技術のように反復処理に頼らずに回転行列Rと並進誤差ベクトルεが直接算出されるためである。これに対し、従来技術では処理時間は9秒から111秒の間であり、初期のX座標が大きくなるほど処理時間が長くなる。
次に、堅牢性評価実験について説明する。堅牢性評価実験では、実環境で取得した点群を用いた場合でも有効に位置合わせを行うことができるか否かを検証する。
実環境で取得した点群は、一部に点が疎らな曲面や曲率が大きい曲面を含むため、精度が低下するおそれがあるからである。
ここで、取得した点群について回転及び並進移動を与えて処理前のクエリ点群を生成し、取得した点群はそのままモデル点群として用いる。そして、本実施形態による処理後のクエリ点群とモデル点群の位置とを比較する。
図11は、点群データを取得した環境の一例を示す写真である。
図11は、建造物の内部である廊下であり、その基本的構成態様は、一方向の長さが他の方向の長さよりも長い直方体である。但し、直方体の側面が随所で開放され、机、椅子、廃物入れ等の什器が備えられているために、具体的構成態様として、サンプル点が疎らである部分や、曲率が大きい部分が含まれる。図11(a)、(b)間では、互いに撮影した視点が異なる。
図12は、処理前の点群データの一例を示す図である。
図12(a)、(b)は、図11に示す環境で取得された点群データの平面図、斜視図を示す。薄く表示された部分が処理前のクエリ点群を示し、濃く表示された部分がモデル点群を示す。処理前のクエリ点群とモデル点群の間では、X−Y平面内で回転と相対並進移動が与えられている。
図12(c)は、図12(a)、(b)に示した、処理前のクエリ点群とモデル点群のそれぞれについて算出した深さ情報付き法線ベクトルの分布を示す。薄く表示された部分が処理前のクエリ点群に係る深さ情報付き法線ベクトルの分布を示し、濃く表示された部分がモデル点群に係る深さ情報付き法線ベクトルの分布を示す。これらの分布は、いずれも深さ情報付き法線ベクトルの分布が密な領域が、撮影領域に含まれる平面に応じて数点に集中している。このことは、深さ情報付き法線ベクトルの分布が算出された壁面主成分で代表できることを示す。
図13は、処理後の点群データの一例を示す図である。
図13(a)、(b)は、それぞれ点群データの平面図、斜視図を示す。薄く表示された部分が処理後のクエリ点群を示し、濃く表示された部分がモデル点群を示す。図13は、両点群が互いに重なり合っていることを示す。この結果は、本実施形態では、一部に点が疎らな部分や曲率が大きい部分を含む実環境で取得した点群であっても、一定の精度をもって位置合わせを行うことができることを示す。なお、建造物の内部に限らず、屋外で取得した点群データであっても、建造物、その他の設置物のように一部に平面を含む環境であれば、同様に位置合わせを行うことができる。
これに対し、従来技術のICP法では、全サンプル点について各最近傍点との誤差の総和の最小解を与えるため、これほど疎らな点群同士を重ね合わせることは原理的に困難である。
このように、本実施形態では、第1のデータ(例えば、クエリ点群データ)が示す第1の物体の表面までの原点からの深さを大きさとして有する法線である第1の深さ付き法線(例えば、クエリ点に基づく深さ付き法線ベクトル)のうち、予め定めた密度よりも高密度で分布している深さ付き法線の代表を第1の壁面主成分(例えば、クエリ点に基づく壁面主成分)として算出する。また、本実施形態では、第2のデータ(例えば、モデル点群データ)が示す第2の物体の表面までの原点からの深さを大きさとして有する法線である第2の深さ付き法線のうち、所定の密度よりも高密度で分布している領域の深さ付き法線の代表である第2の壁面主成分(例えば、モデル点に基づく壁面主成分)から、前記第1の壁面主成分との近似の度合いに基づいて前記第1の壁面主成分に対応する第2の壁面主成分を定める。また、本実施形態では、前記第1の壁面主成分と、当該第1の壁面主成分に対応する第2の壁面主成分との差分(例えば、ペア間誤差ベクトル)に基づいて前記第1の物体と前記第2の物体との間の相対的な位置(例えば、並進誤差ベクトル)を算出する。
これにより、第1の物体の表面の深さ付き法線を代表する第1の壁面主成分と第1の物体の表面の深さ付き法線を代表する第2の壁面主成分を用いることで第1の物体の位置と第2の物体の位置とを合わせる際に、第1の物体の表面が疎らな領域や曲率が高い領域を一部に含んでいても安定した処理を実現することができる。また、反復処理を行わずに第1の壁面主成分とこれに対応する第2の壁面主成分との間の相対的な位置を直接算出するため処理量を著しく低減することができる。
なお、上述した実施形態では、物体の形状を示すデータとして点群データを用いる場合を例にとって説明したが、これには限られない。物体の形状を示すデータであれば、いかなる形式のデータであってもよい。例えば、点群データに代えて、物体の2次元の形状を示す平面画像と、視点(原点)からその物体の表面の各点までの距離もしくは奥行を示す奥行画像(例えば、デプスマップ)との組を用いてもよい。
なお、上述した実施形態におけるデータ処理装置1、2の一部、例えば、主成分算出部102、202、対応部103、203、回転算出部104、相対並進位置算出部206、及び座標変換部207をコンピュータで実現するようにしてもよい。その場合、この制御機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することによって実現してもよい。なお、ここでいう「コンピュータシステム」とは、データ処理装置1、2に内蔵されたコンピュータシステムであって、OSや周辺機器等のハードウェアを含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ROM、CD−ROM等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間、動的にプログラムを保持するもの、その場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含んでもよい。また上記プログラムは、前述した機能の一部を実現するためのものであってもよく、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであってもよい。
また、上述した実施形態におけるデータ処理装置の一部、または全部を、LSI(Large Scale Integration)等の集積回路として実現してもよい。データ処理装置1、2の各機能ブロックは個別にプロセッサ化してもよいし、一部、または全部を集積してプロセッサ化してもよい。また、集積回路化の手法はLSIに限らず専用回路、または汎用プロセッサで実現してもよい。また、半導体技術の進歩によりLSIに代替する集積回路化の技術が出現した場合、当該技術による集積回路を用いてもよい。
以上、図面を参照してこの発明の一実施形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、この発明の要旨を逸脱しない範囲内において様々な設計変更等をすることが可能である。
1、2…データ処理装置、101…データ入力部、102、202…主成分算出部、
103、203…対応部、104…回転算出部、105…データ出力部、
11…記憶部、206…相対並進位置算出部、207…座標変換部

Claims (7)

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

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2013260076A JP6281938B2 (ja) 2013-12-17 2013-12-17 データ処理装置、データ処理方法、及びデータ処理プログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2013260076A JP6281938B2 (ja) 2013-12-17 2013-12-17 データ処理装置、データ処理方法、及びデータ処理プログラム

Publications (2)

Publication Number Publication Date
JP2015118441A JP2015118441A (ja) 2015-06-25
JP6281938B2 true JP6281938B2 (ja) 2018-02-21

Family

ID=53531132

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2013260076A Active JP6281938B2 (ja) 2013-12-17 2013-12-17 データ処理装置、データ処理方法、及びデータ処理プログラム

Country Status (1)

Country Link
JP (1) JP6281938B2 (ja)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018010515A (ja) * 2016-07-14 2018-01-18 エーティーラボ株式会社 メッシュ作成装置、メッシュ作成方法およびメッシュ作成プログラム
US11906294B2 (en) * 2020-07-28 2024-02-20 Ricoh Company, Ltd. Alignment apparatus, alignment system, alignment method, and recording medium

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04119475A (ja) * 1990-09-10 1992-04-20 Nippon Telegr & Teleph Corp <Ntt> 三次元形状識別装置
JP3243905B2 (ja) * 1993-09-27 2002-01-07 スズキ株式会社 物体認識方法及び装置

Also Published As

Publication number Publication date
JP2015118441A (ja) 2015-06-25

Similar Documents

Publication Publication Date Title
Kaess Simultaneous localization and mapping with infinite planes
JP5920352B2 (ja) 情報処理装置、情報処理方法及びプログラム
CN111508021B (zh) 一种位姿确定方法、装置、存储介质及电子设备
JP5627325B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
JP6025830B2 (ja) 3d走査用のセンサ位置決め
EP2808842B1 (en) An apparatus and method for tracking and reconstructing three-dimensional objects
Sweeney et al. Solving for relative pose with a partially known rotation is a quadratic eigenvalue problem
JP5924862B2 (ja) 情報処理装置、情報処理方法及びプログラム
Dong-Si et al. Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration
WO2015096508A1 (zh) 一种模型约束下的在轨三维空间目标姿态估计方法及系统
JP5555207B2 (ja) 3次元姿勢推定装置、3次元姿勢推定方法、及びプログラム
JP6349418B2 (ja) 高精度単眼移動によるオブジェクト位置特定
CN111179427A (zh) 自主移动设备及其控制方法、计算机可读存储介质
JP2012026974A (ja) 3次元物体認識装置及び3次元物体認識方法
JP2014085933A (ja) 3次元姿勢推定装置、3次元姿勢推定方法、及びプログラム
JP7114686B2 (ja) 拡張現実装置及び位置決め方法
CN113048980A (zh) 位姿优化方法、装置、电子设备及存储介质
Tamjidi et al. 6-DOF pose estimation of a portable navigation aid for the visually impaired
US20240078701A1 (en) Location determination and mapping with 3d line junctions
JP6281938B2 (ja) データ処理装置、データ処理方法、及びデータ処理プログラム
CN113793379A (zh) 相机姿态求解方法及系统、设备和计算机可读存储介质
TWM598411U (zh) 擴增實境裝置
KR102548013B1 (ko) 모델 정합 방법 및 상기 방법을 수행하는 컴퓨팅 장치
WO2022172831A1 (ja) 情報処理装置
Sargeant et al. A webcam photogrammetric method for robot calibration

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20160928

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20170718

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20170801

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20170913

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20171222

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20180118

R150 Certificate of patent or registration of utility model

Ref document number: 6281938

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250