JPWO2017208394A1 - 位置合わせ装置、位置合わせ方法及び位置合わせプログラム - Google Patents

Abstract

ペア探索部（１２）により探索された全ての特徴点ペアＰａ−ｂの中から、複数の特徴点ペアＰａ−ｂを選択して、その選択した特徴点ペアＰａ−ｂから、３次元点群データＢの剛体変換に用いる行列Ｇを算出し、その行列Ｇを用いて、３次元点群データＢを剛体変換する剛体変換部（１３）を備え、剛体変換部（１３）が、複数の特徴点ペアＰａ−ｂを選択する選択処理を繰り返し実施して、行列Ｇを算出する算出処理と３次元点群データＢを剛体変換する剛体変換処理とを繰り返し実施するように構成する。

Description

この発明は、計測対象における複数の計測点の３次元座標を示す点群データ間の位置合わせを行う位置合わせ装置、位置合わせ方法及び位置合わせプログラムに関するものである。

例えば、ステレオカメラなどを用いて、異なる視点から計測対象の物体を撮影することで、複数の３次元点群データが得られる。３次元点群データは、計測対象における複数の計測点の３次元座標を示す点群データのことである。
このとき、複数の３次元点群データを統合することで、計測対象の全体の３次元点群データが得られるが、同じ計測点についての３次元座標がずれていると、統合後の３次元点群データから得られる計測対象の形状が、実際の計測対象の形状と異なるものとなる。
このため、複数の３次元点群データを統合するには、複数の３次元点群データの位置合わせを行う必要がある。

以下の特許文献１には、２つの３次元点群データの位置合わせを行う位置合わせ装置が開示されている。
以下、この位置合わせ装置の位置合わせ手順を簡単に説明する。
ここでは、説明の便宜上、２つの３次元点群データを３次元点群データＡ，３次元点群データＢとする。

（１）３次元点群データＡから複数の特徴点ａをそれぞれ抽出し、３次元点群データＢから複数の特徴点ｂをそれぞれ抽出する。
特徴点は、計測対象の物体における形状の特徴を表す計測点であり、例えば、物体の角の点や境界に属する点などが該当する。
（２）複数の特徴点ａの中から、任意の３つの特徴点ａを抽出して、３つの特徴点ａを頂点とする三角形Δ_ａを生成する。
また、複数の特徴点ｂの中から、任意の３つの特徴点ｂを抽出して、３つの特徴点ｂを頂点とする三角形Δ_ｂを生成する。
抽出する３つの特徴点を変えることで、複数の三角形Δ_ａ，Δ_ｂを生成する。
（３）複数の三角形Δ_ａと複数の三角形Δ_ｂとの間で、形状が近似している三角形を探索する。
（４）形状が近似している三角形Δ_ａと三角形Δ_ｂにおける各頂点同士を対応付けて、対応関係がある頂点同士である特徴点ａと特徴点ｂを特徴点ペアとする。
（５）特徴点ペアから３次元点群データＢの剛体変換に用いる行列を算出し、その行列を用いて、３次元点群データＢを剛体変換することで、３次元点群データＡと３次元点群データＢの位置合わせを行う。
剛体変換に用いる行列は、３次元点群データＢを回転させる行列と３次元点群データＢを平行移動させるベクトルからなる行列である。

特開２０１２−１４２５９号公報

従来の位置合わせ装置は以上のように構成されているので、複数の三角形Δ_ａと複数の三角形Δ_ｂとの間に、形状が類似している三角形が複数存在している場合、対応関係がない三角形Δ_ａと三角形Δ_ｂが探索される可能性がある。対応関係がない三角形Δ_ａと三角形Δ_ｂが探索された場合、特徴点ペアの組み合わせに誤りが生じるため、剛体変換に用いる行列の算出精度が低下して、複数の３次元点群データの位置合わせ精度が劣化してしまうことがあるという課題があった。

この発明は上記のような課題を解決するためになされたもので、複数の３次元点群データの位置合わせ精度を高めることができる位置合わせ装置、位置合わせ方法及び位置合わせプログラムを得ることを目的とする。

この発明に係る位置合わせ装置は、計測対象における複数の計測点の３次元座標を示す第１の点群データから複数の特徴点を抽出するとともに、計測対象における複数の計測点の３次元座標を示す第２の点群データから複数の特徴点を抽出し、第１の点群データから抽出した複数の特徴点と、第２の点群データから抽出した複数の特徴点との間で対応関係がある特徴点のペアをそれぞれ探索するペア探索部と、ペア探索部により探索された全ての特徴点のペアの中から、複数の特徴点のペアを選択して、その選択した特徴点のペアから、第２の点群データの剛体変換に用いる行列を算出し、その行列を用いて、第２の点群データを剛体変換する剛体変換部とを備え、剛体変換部が、複数の特徴点のペアを選択する選択処理を繰り返し実施して、行列を算出する算出処理と第２の点群データを剛体変換する剛体変換処理とを繰り返し実施するようにしたものである。

この発明によれば、剛体変換部が、複数の特徴点のペアを選択する選択処理を繰り返し実施して、行列を算出する算出処理と第２の点群データを剛体変換する剛体変換処理とを繰り返し実施するように構成したので、複数の３次元点群データの位置合わせ精度を高めることができる効果がある。

この発明の実施の形態１による位置合わせ装置を示す構成図である。 この発明の実施の形態１による位置合わせ装置のハードウェア構成図である。 位置合わせ装置がソフトウェアやファームウェアなどで実現される場合のコンピュータのハードウェア構成図である。 位置合わせ装置がソフトウェアやファームウェアなどで実現される場合のペア探索部１２のペア探索処理手順を示すフローチャートである。 位置合わせ装置がソフトウェアやファームウェアなどで実現される場合の剛体変換部１３の剛体変換処理手順を示すフローチャートである。 この発明の実施の形態２による位置合わせ装置を示す構成図である。 この発明の実施の形態２による位置合わせ装置のハードウェア構成図である。 位置合わせ装置がソフトウェアやファームウェアなどで実現される場合の剛体変換部１５の剛体変換処理手順を示すフローチャートである。

以下、この発明をより詳細に説明するために、この発明を実施するための形態について、添付の図面にしたがって説明する。

実施の形態１．
図１はこの発明の実施の形態１による位置合わせ装置を示す構成図であり、図２はこの発明の実施の形態１による位置合わせ装置のハードウェア構成図である。
図１及び図２において、３次元センサ１は計測対象における複数の計測点の３次元座標を示す３次元点群データＡ（第１の点群データ）を観測するとともに、計測対象における複数の計測点の３次元座標を示す３次元点群データＢ（第２の点群データ）を観測するセンサである。
例えば、３次元点群データＡと３次元点群データＢは、３次元センサ１によって異なる視点から観測されたデータである。あるいは、３次元センサ１によって異なる時刻で観測されたデータである。

３次元点群データＡ，Ｂは、複数の計測点の３次元座標（ｘ，ｙ，ｚ）のほかに、色の情報を含んでいてもよいし、ポリゴンの情報を含んでいてもよい。ポリゴンの情報は、各々のポリゴンの頂点となる３次元点のインデックスを示す情報である。
この実施の形態１では、３次元点群データＢを剛体変換することで、３次元点群データＡと３次元点群データＢの位置合わせを行うことを想定しており、３次元点群データＡをターゲットの３次元点群データ、３次元点群データＢをソースの３次元点群データと称することがある。
この実施の形態１では、位置合わせ装置が、３次元センサ１により観測された３次元点群データＡ，Ｂを取得することを想定しているが、外部記憶装置２に記憶されている３次元点群データＡ，Ｂを取得するものであってもよい。
外部記憶装置２は位置合わせが行われる３次元点群データＡ，Ｂを記憶している例えばハードディスクなどの記憶装置である。

点群データ読込部１１は例えば図２の点群データ読込回路２１で実現されるものであり、３次元センサ１により観測された３次元点群データＡ，Ｂを読み込む処理を実施する。
ペア探索部１２は例えば図２のペア探索回路２２で実現されるものであり、点群データ読込部１１により読み込まれた３次元点群データＡから複数の特徴点ａを抽出するとともに、点群データ読込部１１により読み込まれた３次元点群データＢから複数の特徴点ｂを抽出し、複数の特徴点ａと複数の特徴点ｂとの間で対応関係がある特徴点のペアをそれぞれ探索する処理を実施する。以下、特徴点のペアを特徴点ペアＰ_ａ−ｂのように表記する。

剛体変換部１３は例えば図２の剛体変換回路２３で実現されるものであり、内部にメモリ１３ａを備えている。
剛体変換部１３はペア探索部１２により探索された全ての特徴点ペアＰ_ａ−ｂの中から、複数の特徴点ペアＰ_ａ−ｂとして、例えば、３つの特徴点ペアＰ_ａ−ｂを選択する処理を実施する。
剛体変換部１３は選択した３つの特徴点ペアＰ_ａ−ｂから、３次元点群データＢの剛体変換に用いる行列Ｇを算出し、その行列Ｇを用いて、３次元点群データＢを剛体変換する処理を実施する。
剛体変換部１３は３次元点群データＢの最終的な剛体変換結果が得られるまで、３つの特徴点ペアＰ_ａ−ｂを選択する選択処理を繰り返し実施して、行列Ｇを算出する算出処理と３次元点群データＢを剛体変換する剛体変換処理とを繰り返し実施する。

点群データ出力部１４は例えば図２の点群データ出力回路２４で実現されるものであり、剛体変換部１３により剛体変換された３次元点群データＢを外部記憶装置２に格納する処理を実施する。または、剛体変換部１３により剛体変換された３次元点群データＢを表示装置３に表示する処理を実施する。
表示装置３は例えば液晶ディスプレイなどの表示器であり、点群データ出力部１４から出力された３次元点群データＢを表示する。

図１では、位置合わせ装置の構成要素である点群データ読込部１１、ペア探索部１２、剛体変換部１３及び点群データ出力部１４のそれぞれが、図２に示すような専用のハードウェア、即ち、点群データ読込回路２１、ペア探索回路２２、剛体変換回路２３及び点群データ出力回路２４で実現されるものを想定している。
ここで、点群データ読込回路２１、ペア探索回路２２、剛体変換回路２３及び点群データ出力回路２４は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、または、これらを組み合わせたものが該当する。

ただし、位置合わせ装置の構成要素が専用のハードウェアで実現されるものに限るものではなく、位置合わせ装置がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
ソフトウェアやファームウェアはプログラムとして、コンピュータのメモリに格納される。コンピュータは、プログラムを実行するハードウェアを意味し、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）などが該当する。
コンピュータのメモリは、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒy）などの不揮発性又は揮発性の半導体メモリや、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）などが該当する。

図３は位置合わせ装置がソフトウェアやファームウェアなどで実現される場合のコンピュータのハードウェア構成図である。
位置合わせ装置がソフトウェアやファームウェアなどで実現される場合、点群データ読込部１１、ペア探索部１２、剛体変換部１３及び点群データ出力部１４の処理手順をコンピュータに実行させるための位置合わせプログラムをメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されている位置合わせプログラムを実行するようにすればよい。
図４は位置合わせ装置がソフトウェアやファームウェアなどで実現される場合のペア探索部１２のペア探索処理手順を示すフローチャートである。
図５は位置合わせ装置がソフトウェアやファームウェアなどで実現される場合の剛体変換部１３の剛体変換処理手順を示すフローチャートである。

また、図２では位置合わせ装置の構成要素のそれぞれが専用のハードウェアで実現される例を示し、図３では、位置合わせ装置がソフトウェアやファームウェアなどで実現される例を示しているが、位置合わせ装置における一部の構成要素が専用のハードウェアで実現され、残りの構成要素がソフトウェアやファームウェアなどで実現されるものであってもよい。

次に動作について説明する。
点群データ読込部１１は、３次元センサ１により観測された３次元点群データＡ，Ｂを読み込み、その３次元点群データＡ，Ｂをペア探索部１２に出力する。
ペア探索部１２は、点群データ読込部１１から３次元点群データＡ，Ｂを受けると、その３次元点群データＡから複数の特徴点ａを抽出するとともに、その３次元点群データＢから複数の特徴点ｂを抽出する（図４のステップＳＴ１）。
特徴点は、計測対象の物体における形状の特徴を表す計測点であり、例えば、物体の角の点や境界に属する点などが該当する。

３次元点群データＡ，Ｂから特徴点ａ，ｂを抽出する処理自体は公知の技術であるため詳細な説明を省略するが、例えば、以下の非特許文献１に開示されている特徴点の抽出方法を利用して、３次元点群データＡ，Ｂから特徴点ａ，ｂを抽出するものとする。
［非特許文献１］
Yong Zhong著、「Intrinsic shape signatures: A shape descriptor for 3D object recognition」、IEEE、Proceedings of International Conference on Computer Vision Workshops、2009年9月27日発行、ｐ．689〜696

ペア探索部１２は、３次元点群データＡ，Ｂから複数の特徴点ａ，ｂを抽出すると、各々の特徴点ａに対して、周辺形状を表す特徴ベクトルＶ_ａを算出する（図４のステップＳＴ２）。
また、ペア探索部１２は、各々の特徴点ｂに対して、周辺形状を表す特徴ベクトルＶ_ｂを算出する（図４のステップＳＴ２）。
特徴ベクトルは、一般的に、特徴点の周辺に存在している計測点に対する特徴点の位置関係や法線の向きの違いを示す多次元のベクトルである。
特徴ベクトルＶ_ａ，Ｖ_ｂを算出する処理自体は公知の技術であり、また、特徴ベクトルＶ_ａ，Ｖ_ｂの記述方法も公知の技術であるため詳細な説明を省略するが、この実施の形態１では、例えば、以下の非特許文献２に開示されているＳＨＯＴ（Ｓｉｇｎａｔｕｒｅ ｏｆ Ｈｉｓｔｏｇｒａｍｓ ｏｆ ＯｒｉｅｎＴａｔｉｏｎｓ）特徴量を特徴ベクトルとして利用するものとする。
［非特許文献２］
Federico Tombariら著、「Unique signatures of Histograms for local surface description」、Springer、Proceedings of 11th European Conference on Computer Vision、2010年9月5日発行、ｐ．356〜369

ペア探索部１２は、複数の特徴点ａに対して特徴ベクトルＶ_ａを算出し、複数の特徴点ｂに対して特徴ベクトルＶ_ｂを算出すると、複数の特徴点ａにおける特徴ベクトルＶ_ａと、複数の特徴点ｂにおける特徴ベクトルＶ_ｂとの組み合わせ毎に、特徴ベクトルＶ_ａと特徴ベクトルＶ_ｂの類似度を算出する。２つの特徴ベクトル間の類似度を算出する処理自体は公知の技術であるため詳細な説明を省略する。
そして、ペア探索部１２は、３次元点群データＡから抽出した特徴点ａ毎に、当該特徴点ａにおける特徴ベクトルＶ_ａと、複数の特徴点ｂにおける特徴ベクトルＶ_ｂとの類似度を比較し、複数の特徴点ｂにおける特徴ベクトルＶ_ｂの中で、最も類似度が高い特徴ベクトルＶ_ｂに係る特徴点ｂを特定する。
ペア探索部１２は、最も類似度が高い特徴ベクトルＶ_ｂに係る特徴点ｂを特定すると、当該特徴点ａと、その特定した特徴点ｂとを特徴点ペアＰ_ａ−ｂに決定する（図４のステップＳＴ３）。
具体的には、例えば、特徴点ａの個数がＮ個で、特徴点ｂの個数がＭ個である場合、Ｎ個の特徴点ａのそれぞれについて、Ｍ個の特徴点ｂの中から、最も類似度が高い特徴ベクトルＶ_ｂに係る特徴点ｂを特定し、当該特徴点ａと、その特定した特徴点ｂとを特徴点ペアＰ_ａ−ｂに決定する。
この場合、Ｎ個の特徴点ペアＰ_ａ−ｂが決定される。Ｎは３以上の整数である。

剛体変換部１３は、ペア探索部１２により決定されたＮ個の特徴点ペアＰ_ａ−ｂを内部のメモリ１３ａに格納する。
剛体変換部１３は、メモリ１３ａに格納しているＮ個の特徴点ペアＰ_ａ−ｂの中から、例えば、３つの特徴点ペアＰ_ａ−ｂを選択する（図５のステップＳＴ１１）。
３つの特徴点ペアＰ_ａ−ｂは、Ｎ個の特徴点ペアＰ_ａ−ｂの中から無作為に選択されるが、過去に選択されていない組み合わせの特徴点ペアＰ_ａ−ｂが選択されるものとする。
ただし、３つの特徴点ペアＰ_ａ−ｂは、Ｎ個の特徴点ペアＰ_ａ−ｂの中から、無作為に選択されるものに限るものではなく、特定のルールにしたがって選択されるものであってもよい。例えば、ペア探索部１２により算出された類似度が高い特徴点ペアＰ_ａ−ｂが優先的に選択される態様などが考えられる。

剛体変換部１３は、３つの特徴点ペアＰ_ａ−ｂを選択すると、下記の式（１）に示すように、３つの特徴点ペアＰ_ａ−ｂに含まれている特徴点ａの３次元座標ｐ_ａ，ｉ（ｉ＝１，２，３）を定義する。

また、剛体変換部１３は、下記の式（２）に示すように、３つの特徴点ペアＰ_ａ−ｂに含まれている特徴点ｂの３次元座標ｐ_ｂ，ｉ（ｉ＝１，２，３）を定義する。

剛体変換部１３は、３つの特徴点ペアＰ_ａ−ｂに含まれている特徴点ａの３次元座標ｐ_ａ，ｉと特徴点ｂの３次元座標ｐ_ｂ，ｉを定義すると、特徴点ａの３次元座標ｐ_ａ，ｉと特徴点ｂの３次元座標ｐ_ｂ，ｉから、３次元点群データＢの剛体変換に用いる行列Ｇを算出する（図５のステップＳＴ１２）。
剛体変換に用いる行列Ｇは、３次元点群データＢを回転させる行列である回転行列Ｒと、３次元点群データＢを平行移動させるベクトルである並進ベクトルｔとからなる行列である。
したがって、剛体変換部１３は、剛体変換に用いる行列Ｇとして、回転行列Ｒと並進ベクトルｔを算出するが、３つの特徴点ペアＰ_ａ−ｂに含まれている特徴点ｂを剛体変換することで、特徴点ａと特徴点ｂが出来る限り近づくようにするには、下記に示す式（３）が最小になる回転行列Ｒと並進ベクトルｔを求める必要がある。

式（３）において、||ｋ||はベクトルｋのノルムを表す記号である。

式（３）が最小になる回転行列Ｒと並進ベクトルｔを算出する方法は複数存在するが、この実施の形態１では、以下の非特許文献３に記載されている方法を用いて、回転行列Ｒと並進ベクトルｔを算出する例を説明する。
［非特許文献３］
八木康史ら編、「コンピュータビジョン最先端ガイド３」、アドコム・メディア株式会社、2010年12月8日発行、ｐ．36〜37

剛体変換部１３は、３つの特徴点ペアＰ_ａ−ｂに対して、下記の式（４）に示すように、共分散行列Σを算出する。

式（４）において、ｋ^ｔはベクトルｋの転置である。
、μ_ａは３つの特徴点ａにおける３次元座標ｐ_ａ，ｉの重心座標、μ_ｂは３つの特徴点ｂにおける３次元座標ｐ_ｂ，ｉの重心座標である。

剛体変換部１３は、共分散行列Σを算出すると、その共分散行列Σを特異値分解することで、下記の式（８）に示すように回転行列Ｒを算出し、また、下記の式（９）に示すように並進ベクトルｔを算出する。
即ち、剛体変換部１３は、共分散行列Σを特異値分解することで、下記の式（７）における行列Ｕ，Ｖ^ｔが求まるので、行列Ｕ，Ｖ^ｔを下記の式（８）に代入することで回転行列Ｒを算出する。また、算出した回転行列Ｒを下記の式（９）に代入することで並進ベクトルｔを算出する。

式（８）において、ｄｅｔ（）は行列式を示す記号である。

剛体変換部１３は、剛体変換に用いる行列Ｇとして、回転行列Ｒと並進ベクトルｔを算出すると、その回転行列Ｒを用いて、ソースの３次元点群データＢを回転させ、その並進ベクトルｔを用いて、ソースの３次元点群データＢを平行移動させることで、ソースの３次元点群データＢを剛体変換する（図５のステップＳＴ１３）。

剛体変換部１３は、ソースの３次元点群データＢを剛体変換すると、剛体変換後の３次元点群データＢと、ターゲットの３次元点群データＡとの一致度Ｓを算出する（図５のステップＳＴ１４）。
即ち、剛体変換部１３は、剛体変換後の３次元点群データＢに含まれている各々の特徴点ｂから、ターゲットの３次元点群データＡに含まれている最近傍の特徴点ａまでの距離をそれぞれ求め、それらの距離の平均値の逆数を一致度Ｓとして算出する。

式（１０）において、Ｈは剛体変換後の３次元点群データＢに含まれている特徴点ｂの数である。
ｄ（ｐ_ｂ，ｉ，Ａ）は剛体変換後の３次元点群データＢに含まれている特徴点ｂから、ターゲットの３次元点群データＡに含まれている最近傍の特徴点ａまでの距離であり、下記の式（１１）のように表される。

式（１１）において、ｐ_ａ，ｊは３次元点群データＡに含まれている複数の特徴点ａの３次元座標である。

剛体変換部１３は、剛体変換後の３次元点群データＢと、ターゲットの３次元点群データＡとの一致度Ｓを算出すると、その一致度Ｓの算出処理が１回目の算出処理であれば、その一致度Ｓをメモリ１３ａに格納するとともに、剛体変換後の３次元点群データＢをメモリ１３ａに格納する。
剛体変換部１３は、その一致度Ｓの算出処理が２回目以降の算出処理であれば、今回算出した一致度Ｓと、メモリ１３ａに格納されている一致度Ｓとを比較し、今回算出した一致度Ｓが、メモリ１３ａに格納されている一致度Ｓより高ければ（図５のステップＳＴ１５：Ｙｅｓの場合）、今回算出した一致度Ｓをメモリ１３ａに上書き保存するとともに、今回の剛体変換処理による剛体変換後の３次元点群データＢをメモリ１３ａに上書き保存する（図５のステップＳＴ１６）。今回算出した一致度Ｓが、メモリ１３ａに格納されている一致度Ｓ以下である場合、今回の剛体変換処理による剛体変換後の３次元点群データＢは破棄される。
これにより、メモリ１３ａに格納される一致度Ｓは、これまでの算出処理で算出された一致度Ｓの中で、最も高い一致度Ｓに更新され、メモリ１３ａに格納される剛体変換後の３次元点群データＢは、最も高い一致度Ｓに対応する３次元点群データＢに更新される。

剛体変換部１３は、これまでに剛体変換処理を実施した回数である試行回数Ｃと、事前に設定された回数である設定試行回数Ｃ_ＥＳ（第１の閾値）とを比較するとともに、メモリ１３ａに格納されている一致度Ｓと、事前に設定された一致度である設定一致度Ｓ_ＥＳ（第２の閾値）とを比較する。設定試行回数Ｃ_ＥＳや設定一致度Ｓ_ＥＳは、３次元点群データのデータ数などによって変わるが、例えば、設定試行回数Ｃ_ＥＳとしては１０回が考えられ、設定一致度Ｓ_ＥＳとしては１／１０ｃｍが考えられる。
剛体変換部１３は、Ｃ＜Ｃ_ＥＳ、かつ、Ｓ＜Ｓ_ＥＳの場合、即ち、試行回数Ｃが設定試行回数Ｃ_ＥＳに到達しておらず、かつ、メモリ１３ａに格納されている一致度Ｓが設定一致度Ｓ_ＥＳより低い場合（図５のステップＳＴ１７：Ｎｏの場合）、ステップＳＴ１１の処理に戻り、特徴点ペアＰ_ａ−ｂの選択処理、剛体変換に用いる行列Ｇの算出処理及び剛体変換処理を繰り返し実施する（図５のステップＳＴＳＴ１１〜ＳＴ１６）。

剛体変換部１３は、Ｃ＝Ｃ_ＥＳ、または、Ｓ≧Ｓ_ＥＳの場合、即ち、試行回数Ｃが設定試行回数Ｃ_ＥＳに到達している場合、または、メモリ１３ａに格納されている一致度Ｓが設定一致度Ｓ_ＥＳ以上である場合（図５のステップＳＴ１７：Ｙｅｓの場合）、メモリ１３ａに格納されている剛体変換後の３次元点群データＢを点群データ出力部１４に出力する（図５のステップＳＴ１８）。

点群データ出力部１４は、剛体変換部１３から剛体変換後の３次元点群データＢを受けると、位置合わせ後の３次元点群データＢとして、剛体変換後の３次元点群データＢを外部記憶装置２に格納する。あるいは、剛体変換後の３次元点群データＢを表示装置３に表示する。

以上で明らかなように、この実施の形態１によれば、剛体変換部１３が、複数の特徴点のペアを選択する選択処理を繰り返し実施して、剛体変換に用いる行列Ｇを算出する算出処理と３次元点群データＢを剛体変換する剛体変換処理とを繰り返し実施するように構成したので、３次元点群データＡと３次元点群データＢの位置合わせ精度を高めることができる効果を奏する。
即ち、この実施の形態１では、ペア探索部１２により決定された複数の特徴点ペアＰ_ａ−ｂの中に、組み合わせに誤りがある特徴点ペアＰ_ａ−ｂが含まれている場合でも、剛体変換部１３が、誤りがある特徴点ペアＰ_ａ−ｂから算出した行列Ｇを用いた剛体変換後の３次元点群データＢを出力する可能性を低減できるため、３次元点群データＡと３次元点群データＢの位置合わせ精度を高めることができる。

この実施の形態１では、剛体変換部１３が、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを選択する例を示したが、これに限るものではなく、Ｎ個の特徴点ペアＰ_ａ−ｂの中から、４つ以上の特徴点ペアＰ_ａ−ｂを選択するようにしてもよい。

実施の形態２．
上記実施の形態１では、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを選択すると、剛体変換部１３が、３つの特徴点ペアＰ_ａ−ｂの良否を判定することなく、３つの特徴点ペアＰ_ａ−ｂから剛体変換に用いる行列Ｇを算出するものを示したが、この実施の形態２では、剛体変換部１５が、３つの特徴点ペアＰ_ａ−ｂの良否を判定し、その判定の結果が否であれば、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択するものについて説明する。

図６はこの発明の実施の形態２による位置合わせ装置を示す構成図であり、図７はこの発明の実施の形態２による位置合わせ装置のハードウェア構成図である。
図６及び図７において、図１及び図２と同一符号は同一または相当部分を示すので説明を省略する。
剛体変換部１５は例えば図７の剛体変換回路２５で実現されるものであり、内部にメモリ１５ａを備えている。
剛体変換部１５は図１の剛体変換部１３と同様に、ペア探索部１２により探索された全ての特徴点ペアＰ_ａ−ｂの中から、複数の特徴点ペアＰ_ａ−ｂとして、例えば、３つの特徴点ペアＰ_ａ−ｂを選択する処理を実施する。
剛体変換部１５は図１の剛体変換部１３と同様に、選択した３つの特徴点ペアＰ_ａ−ｂから、３次元点群データＢの剛体変換に用いる行列Ｇを算出し、その行列Ｇを用いて、３次元点群データＢを剛体変換する処理を実施する。
剛体変換部１５は図１の剛体変換部１３と同様に、３次元点群データＢの最終的な剛体変換結果が得られるまで、３つの特徴点ペアＰ_ａ−ｂを選択する選択処理を繰り返し実施して、行列の算出処理と３次元点群データＢの剛体変換処理を繰り返し実施する。
剛体変換部１５は図１の剛体変換部１３と異なり、選択した３つの特徴点ペアＰ_ａ−ｂの良否を判定し、その判定の結果が否であれば、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択する処理を実施する。

図６では、位置合わせ装置の構成要素である点群データ読込部１１、ペア探索部１２、剛体変換部１５及び点群データ出力部１４のそれぞれが、図７に示すような専用のハードウェア、即ち、点群データ読込回路２１、ペア探索回路２２、剛体変換回路２５及び点群データ出力回路２４で実現されるものを想定している。
ここで、点群データ読込回路２１、ペア探索回路２２、剛体変換回路２５及び点群データ出力回路２４は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ、ＦＰＧＡ、または、これらを組み合わせたものが該当する。

ただし、位置合わせ装置の構成要素が専用のハードウェアで実現されるものに限るものではなく、位置合わせ装置がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
位置合わせ装置がソフトウェアやファームウェアなどで実現される場合、点群データ読込部１１、ペア探索部１２、剛体変換部１５及び点群データ出力部１４の処理手順をコンピュータに実行させるための位置合わせプログラムを図３に示すメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されている位置合わせプログラムを実行するようにすればよい。
図８は位置合わせ装置がソフトウェアやファームウェアなどで実現される場合の剛体変換部１５の剛体変換処理手順を示すフローチャートである。図８において、図５と同一符号は同一または相当部分を示している。

次に動作について説明する。
ただし、剛体変換部１５以外は、上記実施の形態１と同様であるため、ここでは剛体変換部１５の処理内容だけを説明する。
剛体変換部１５は、図１の剛体変換部１３と同様に、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂを内部のメモリ１５ａに格納する。
剛体変換部１５は、メモリ１５ａに格納しているＮ個の特徴点ペアＰ_ａ−ｂの中から、例えば、３つの特徴点ペアＰ_ａ−ｂを選択する（図８のステップＳＴ１１）。

剛体変換部１５は、３つの特徴点ペアＰ_ａ−ｂを選択すると、３つの特徴点ペアＰ_ａ−ｂの良否を判定する。
３つの特徴点ペアＰ_ａ−ｂにおける良否の判定は、３つの特徴点ペアＰ_ａ−ｂの間の位置関係が、剛体変換に用いる行列Ｇを高精度に算出することが可能な関係にあるか否かを検証するものである。
剛体変換部１５は、３つの特徴点ペアＰ_ａ−ｂにおける良否の具体的な判定として、例えば、下記に示すような判定を行う。

剛体変換部１５は、３つの特徴点ペアＰ_ａ−ｂに含まれている３つの特徴点ａを頂点とする多角形である三角形と、３つの特徴点ペアＰ_ａ−ｂに含まれている３つの特徴点ｂを頂点とする多角形である三角形とが類似しているか否かを判定する。
剛体変換部１５は、２つの三角形が類似していると判定すれば、３つの特徴点ペアＰ_ａ−ｂが良であると判定し、２つの三角形が類似していないと判定すれば、３つの特徴点ペアＰ_ａ−ｂが否であると判定する。
以下、剛体変換部１５による２つの三角形の類似判定方法を具体的に説明する。
まず、剛体変換部１５は、３つの特徴点ａを頂点とする三角形と、３つの特徴点ｂを頂点とする三角形との対応する辺毎に、対応する辺の長さの差分を求める。
次に、剛体変換部１５は、対応する辺のうち、長い方の辺の長さに対する当該差分の比率が１割以内であるか否かを判定する。
剛体変換部１５は、３つの対応する辺の全てにおいて、差分の比率が１割以内であれば、２つの三角形が類似していると判定し、３つの対応する辺の中に、差分の比率が１割を超える対応する辺が１つでもあれば、２つの三角形が類似していないと判定する。

剛体変換部１５は、３つの特徴点ペアＰ_ａ−ｂが否であると判定すれば（図８のステップＳＴ２１：Ｎｏの場合）、ステップＳＴ１１の処理に戻り、メモリ１５ａに格納しているＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択する（図８のステップＳＴ１１）。
このとき、再選択する３つの特徴点ペアＰ_ａ−ｂの組み合わせは、過去に選択されていない組み合わせである。
剛体変換部１５は、３つの特徴点ペアＰ_ａ−ｂが良であると判定すれば（図８のステップＳＴ２１：Ｙｅｓの場合）、ステップＳＴ１２に移行する。以降の処理内容は、上記実施の形態１における図１の剛体変換部１３と同様であるため説明を省略する。

以上で明らかなように、この実施の形態２によれば、剛体変換部１５が、３つの特徴点ペアＰ_ａ−ｂの良否を判定し、その判定の結果が否であれば、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択するように構成したので、上記実施の形態１よりも、３次元点群データＢの剛体変換に用いる行列Ｇの算出精度が向上し、さらに、３次元点群データＡと３次元点群データＢの位置合わせ精度を高めることができる効果を奏する。
また、実行する必要がない剛体変換処理や、一致度の算出処理を省略することができるため、上記実施の形態１よりも、計算量を削減して、処理時間を短縮することができる。

この実施の形態２では、剛体変換部１５が、３つの特徴点ペアＰ_ａ−ｂの良否を判定し、その判定の結果が否であれば、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択するものを示したが、剛体変換部１５が、３次元点群データＢの剛体変換に用いる行列Ｇの良否を判定し、その判定の結果が否であれば、ペア探索部１２により探索されたＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択するようにしてもよい。この場合も、３次元点群データＡと３次元点群データＢの位置合わせ精度を高めることができる。

３次元点群データＢの剛体変換に用いる行列Ｇの良否は、例えば、以下のように判定する。
剛体変換部１５は、図１の剛体変換部１３と同様に、ソースの３次元点群データＢを剛体変換すると、下記の式（１２）に示すように、３つの特徴点ペアＰ_ａ−ｂに含まれている３つの特徴点ａと、３つの特徴点ペアＰ_ａ−ｂに含まれている剛体変換後の３つの特徴点ｂとの距離Ｄを算出する。

剛体変換部１５は、算出した距離Ｄが予め設定された距離閾値より短ければ、３次元点群データＢの剛体変換に用いる行列Ｇが良であると判定し、その距離閾値以上であれば、３次元点群データＢの剛体変換に用いる行列Ｇが否であると判定する。距離閾値としては、例えば１０ｃｍなどが考えられる。
剛体変換部１５は、３次元点群データＢの剛体変換に用いる行列Ｇが良であると判定すれば、以降の処理内容は上記実施の形態１における剛体変換部１３と同様である。
剛体変換部１５は、３次元点群データＢの剛体変換に用いる行列Ｇが否であると判定すれば、ステップＳＴ１１の処理に戻り、メモリ１５ａに格納しているＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択する。

３次元点群データＢの剛体変換に用いる行列Ｇの良否は、次のように判定してもよい。
剛体変換部１５は、図１の剛体変換部１３と同様に、ソースの３次元点群データＢを剛体変換するが、３つの特徴点ａを頂点とする三角形の面積と、剛体変換後の３つの特徴点ｂを頂点とする三角形の面積とが異なる場合、３次元点群データＢの剛体変換だけでは、３次元点群データＡと３次元点群データＢの位置合わせが不十分であるため、剛体変換後の３次元点群データＢの拡大縮小を実施する場合がある。
そこで、剛体変換部１５は、３つの特徴点ペアＰ_ａ−ｂに含まれている３つの特徴点ａを頂点とする三角形と、３つの特徴点ペアＰ_ａ−ｂに含まれている剛体変換後の３つの特徴点ｂを頂点とする三角形との大きさの比率、即ち、３次元点群データＢの拡大縮小率ｒを算出する。

３次元点群データＢの拡大縮小率ｒは、例えば、以下の非特許文献４に記載されている方法を用いて、算出することができる。

［非特許文献４］
Timo Zinserら著、「Point Set Registration with Integrated Scale Estimation」、Proceedings of the Eighth International Conference on Pattern Recognition and Image Processing、2005年5月18日発行、ｐ．116〜119

剛体変換部１５は、算出した拡大縮小率ｒが１に近ければ、即ち、算出した拡大縮小率ｒが、予め設定された閾値の範囲内であれば、３次元点群データＢの剛体変換に用いる行列Ｇが良であると判定する。この閾値としては、例えば、０．９〜１．１などが考えられる。
剛体変換部１５は、算出した拡大縮小率ｒが１から遠ければ、即ち、算出した拡大縮小率ｒが、予め設定された閾値の範囲外であれば、３次元点群データＢの剛体変換に用いる行列Ｇが否であると判定する。
剛体変換部１５は、３次元点群データＢの剛体変換に用いる行列Ｇが良であると判定すれば、以降の処理内容は上記実施の形態１における剛体変換部１３と同様である。
剛体変換部１５は、３次元点群データＢの剛体変換に用いる行列Ｇが否であると判定すれば、ステップＳＴ１１の処理に戻り、メモリ１５ａに格納しているＮ個の特徴点ペアＰ_ａ−ｂの中から、３つの特徴点ペアＰ_ａ−ｂを再選択する。

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

この発明は、計測対象における複数の計測点の３次元座標を示す点群データ間の位置合わせを行う位置合わせ装置、位置合わせ方法及び位置合わせプログラムに適している。

１ ３次元センサ、２ 外部記憶装置、３ 表示装置、１１ 点群データ読込部、１２ ペア探索部、１３ 剛体変換部、１３ａ メモリ、１４ 点群データ出力部、１５ 剛体変換部、１５ａ メモリ、２１ 点群データ読込回路、２２ ペア探索回路、２３ 剛体変換回路、２４ 点群データ出力回路、２５ 剛体変換回路、３１ メモリ、３２ プロセッサ。

Claims (13)

1. 計測対象における複数の計測点の３次元座標を示す第１の点群データから複数の特徴点を抽出するとともに、前記計測対象における複数の計測点の３次元座標を示す第２の点群データから複数の特徴点を抽出し、前記第１の点群データから抽出した複数の特徴点と、前記第２の点群データから抽出した複数の特徴点との間で対応関係がある特徴点のペアをそれぞれ探索するペア探索部と、
   前記ペア探索部により探索された全ての特徴点のペアの中から、複数の特徴点のペアを選択して、その選択した特徴点のペアから、前記第２の点群データの剛体変換に用いる行列を算出し、前記行列を用いて、前記第２の点群データを剛体変換する剛体変換部とを備え、
   前記剛体変換部は、前記複数の特徴点のペアを選択する選択処理を繰り返し実施して、前記行列を算出する算出処理と前記第２の点群データを剛体変換する剛体変換処理とを繰り返し実施することを特徴とする位置合わせ装置。
2. 前記剛体変換部は、前記第２の点群データを剛体変換する毎に、前記第１の点群データと剛体変換後の第２の点群データとの間の一致度を算出し、今回算出した一致度が過去に算出した全ての一致度より高ければ、位置合わせ後の第２の点群データとして、今回の剛体変換処理による剛体変換後の第２の点群データを上書き保存することを特徴とする請求項１記載の位置合わせ装置。
3. 前記剛体変換部は、前記剛体変換処理の実施回数が第１の閾値に到達するまでの間、前記ペアの選択処理、前記行列の算出処理及び前記剛体変換処理を繰り返し実施することを特徴とする請求項２記載の位置合わせ装置。
4. 前記剛体変換部は、前記第１の点群データと剛体変換後の第２の点群データとの間の一致度が第２の閾値より高くなるまでの間、前記ペアの選択処理、前記行列の算出処理及び前記剛体変換処理を繰り返し実施することを特徴とする請求項２記載の位置合わせ装置。
5. 前記剛体変換部は、前記ペア探索部により探索された全ての特徴点のペアの中から、複数の特徴点のペアを選択する選択処理を繰り返し実施する際、組み合わせが異なる複数の特徴点のペアを選択することを特徴とする請求項２記載の位置合わせ装置。
6. 前記剛体変換部は、前記ペア探索部により探索された全ての特徴点のペアの中から、複数の特徴点のペアを選択すると、前記選択した複数の特徴点のペアの良否を判定し、前記判定の結果が否であれば、前記ペア探索部により探索された全ての特徴点のペアの中から、複数の特徴点のペアを再選択することを特徴とする請求項５記載の位置合わせ装置。
7. 前記剛体変換部は、前記選択したペアに含まれている前記第１の点群データから抽出された複数の特徴点を頂点とする多角形と、前記選択したペアに含まれている前記第２の点群データから抽出された複数の特徴点を頂点とする多角形とが類似しているか否かを判定し、前記類似の判定結果から前記選択した複数の特徴点のペアの良否を判定することを特徴とする請求項６記載の位置合わせ装置。
8. 前記剛体変換部は、前記第２の点群データの剛体変換に用いる行列の良否を判定し、前記判定の結果が否であれば、前記ペア探索部により探索された全ての特徴点のペアの中から、複数の特徴点のペアを再選択することを特徴とする請求項５記載の位置合わせ装置。
9. 前記剛体変換部は、前記第２の点群データの剛体変換に用いる行列を用いて、前記選択したペアに含まれている前記第２の点群データから抽出された複数の特徴点を剛体変換し、前記選択したペアに含まれている前記第１の点群データから抽出された複数の特徴点と、前記剛体変換後の複数の特徴点との距離から、前記行列の良否を判定することを特徴とする請求項８記載の位置合わせ装置。
10. 前記剛体変換部は、前記第２の点群データの剛体変換に用いる行列を用いて、前記選択したペアに含まれている前記第２の点群データから抽出された複数の特徴点を剛体変換し、前記選択したペアに含まれている前記第１の点群データから抽出された複数の特徴点を頂点とする多角形と、前記剛体変換後の複数の特徴点を頂点とする多角形との大きさの比率から、前記行列の良否を判定することを特徴とする請求項８記載の位置合わせ装置。
11. 前記ペア探索部は、前記第１及び第２の点群データから抽出した特徴点毎に、当該特徴点の周辺形状を表す特徴ベクトルを求め、前記第１の点群データから抽出した複数の特徴点に対応する特徴ベクトルと、前記第２の点群データから抽出した複数の特徴点に対応する特徴ベクトルとを比較することで、対応関係がある複数の特徴点のペアを探索することを特徴とする請求項１記載の位置合わせ装置。
12. ペア探索部が、計測対象における複数の計測点の３次元座標を示す第１の点群データから複数の特徴点を抽出するとともに、前記計測対象における複数の計測点の３次元座標を示す第２の点群データから複数の特徴点を抽出し、前記第１の点群データから抽出した複数の特徴点と、前記第２の点群データから抽出した複数の特徴点との間で対応関係がある特徴点のペアをそれぞれ探索し、
    剛体変換部が、前記ペア探索部により探索された全ての特徴点のペアの中から、複数の特徴点のペアを選択して、その選択した特徴点のペアから、前記第２の点群データの剛体変換に用いる行列を算出し、前記行列を用いて、前記第２の点群データを剛体変換し、
    前記剛体変換部では、前記複数の特徴点のペアを選択する選択処理を繰り返し実施して、前記行列を算出する算出処理と前記第２の点群データを剛体変換する剛体変換処理とを繰り返し実施することを特徴とする位置合わせ方法。
13. 計測対象における複数の計測点の３次元座標を示す第１の点群データから複数の特徴点を抽出するとともに、前記計測対象における複数の計測点の３次元座標を示す第２の点群データから複数の特徴点を抽出し、前記第１の点群データから抽出した複数の特徴点と、前記第２の点群データから抽出した複数の特徴点との間で対応関係がある特徴点のペアをそれぞれ探索するペア探索処理手順と、
    前記ペア探索処理手順により探索された全ての特徴点のペアの中から、複数の特徴点のペアを選択して、その選択した特徴点のペアから、前記第２の点群データの剛体変換に用いる行列を算出し、前記行列を用いて、前記第２の点群データを剛体変換する剛体変換処理手順とをコンピュータに実行させるための位置合わせプログラムであり、
    前記剛体変換処理手順では、前記複数の特徴点のペアを選択する選択処理を繰り返し実施して、前記行列を算出する算出処理と前記第２の点群データを剛体変換する剛体変換処理とを繰り返し実施することを特徴とする位置合わせプログラム。

Images