(57)【要約】【課題】最近傍探索を高速化できる技術を提供する。【解決手段】画像処理装置が、対象物の第１モデル点群データと第１測定点群データとを取得する取得部と、第２モデル点群データと第２測定点群データを取得するソート部と、基準点群データに係る第１点群での基準点に対する最近傍点を参照点群データに係る第２点群から探索する探索部と、基準点設定部とを備え、探索部が、基準点に対する基準座標軸方向の距離が基準距離よりも短い注目点を第２点群から選択する選択部と、基準距離よりも短い基準点と注目点との点間距離により基準距離を更新する更新部と、注目点として新たに選択可能な点が無くなるまで更新された基準距離が反映されつつ選択処理と更新処理とが逐次実行されるように、選択部と更新部とを制御する探索制御部と、選択処理と更新処理との逐次実行処理が終了されたときの基準距離の設定に係る注目点を最近傍点として決定する決定部とを備える。【選択図】図２
2013045141.tif 000002
【特許請求の範囲】
【請求項１】
対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得部と、
前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート部と、
前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索部と、
前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定部と、
を備え、
前記探索部が、
前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択部と、
前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新部と、
前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御部と、
前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定部と、
を備えたことを特徴とする画像処理装置。
【請求項２】
請求項１に記載された画像処理装置であって、
前記基準点設定部が、
前記基準座標軸方向についての前記第１点群の並び順に前記基準点を逐次設定し、
前記探索部が、
逐次設定された前記基準点に関する前記探索処理のそれぞれにおいて、当該基準点と、当該基準点の直前に設定された基準点に対して探索された最近傍点との距離を前記基準距離の初期値として設定しつつ、前記探索処理を逐次行うことを特徴とする画像処理装置。
【請求項３】
請求項１または請求項２に記載された画像処理装置であって、
座標空間の各座標軸のうち前記参照点群データの派生元の点群データにおける座標値の分布幅が最も広い座標軸を前記基準座標軸として設定する座標軸設定部、
をさらに備えることを特徴とする画像処理装置。
【請求項４】
請求項１から請求項３の何れか１つの請求項に記載された画像処理装置であって、
前記対象物の三次元形状を測定可能な測定部、
をさらに備え、
前記取得部が、
前記測定部が前記対象物を測定して取得した測定情報に基づいて、前記第１測定点群データを取得することを特徴とする画像処理装置。
【請求項５】
画像処理装置に搭載されたコンピュータにおいて実行されることにより当該画像処理装置を請求項１から請求項４の何れか１つの請求項に記載の画像処理装置として機能させることを特徴とするプログラム。
【請求項６】
対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得工程と、
前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート工程と、
前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索工程と、
前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定工程と、
を有し、
前記探索工程が、
前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択工程と、
前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新工程と、
前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御工程と、
前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定工程と、
を有することを特徴とする画像処理方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、最近傍点の探索技術に関する。
【背景技術】
【０００２】
近年、対象物の三次元形状を測定した測定データを、対象物の三次元形状を設計数値的に表現したＣＡＤデータなどのモデルデータに配置合わせすることにより、対象物の位置および姿勢を認識する装置が用いられている。認識された位置および姿勢は、例えば、ロボット等による対象物のパレタイジング、あるいは対象物の実際の形状と、設計形状との差異の判定などに利用される。
【０００３】
該配置合わせは、例えば、ＩＣＰアルゴリズム（Iterative Closest Points Algorithm）などを用いて行なわれる。ＩＣＰアルゴリズムは、２つの形状データ間で、最近傍探索（ＮＮＳ：Nearest Neighbor Search）と、最近傍探索の結果に基づいて２つの形状データを相対的に近づける座標変換とを繰り返すことにより、２つの形状データを配置合わせする手法である。該座標変換においては、他方の三次元データの移動後における各点から各最近傍点までの距離の和が最小となる変換が求められる。また、ＩＣＰアルゴリズムの実行過程で繰り返し行なわれる各座標変換の情報に基づいて、配置合わせされる前の２つの形状データを配置合わせ終了後の形状データに変換する座標変換が求められる。従って、配置合わせ前の２つの形状データ間の相対的な位置および姿勢が求められる。
【０００４】
図３は、最近傍探索の概念を説明する図である。黒塗りの四角形で表現されたモデル点群ＡＧ１（図３）は、対象物の各点の三次元座標を設計数値的にそれぞれ表現したと点群である。黒塗りの円で表現された測定点群ＢＧ１（図３）は、当該対象物上の各点の実測に基づく３次元座標をそれぞれ表現した点群である。点群ＣＧ１（図３）は、モデル点群ＡＧ１と測定点群ＢＧ１とについて最近傍探索が行なわれた結果を示す点群であり、モデル点群ＡＧ１と測定点群ＢＧ１とにおいて、互いに距離が最短となるモデル点と測定点との対のそれぞれが破線で示されている。図３に示されるように、最近傍探索においては、基準となる三次元データ上の各点について、各点との距離が最短となる点（最近傍点）が他方の３次元データ上でそれぞれ探索される。
【０００５】
ここで、最近傍探索が、一方の形状データの各点のそれぞれについて、他方の形状データの各点との間での一般的な総当たり探索（線形探索）によって行なわれる場合には、ＩＣＰアルゴリズムが実行される際の処理時間の、例えば、９９％が最近傍探索に費やされる。このため、最近傍探索の処理時間の短縮が求められている。
【０００６】
例えば、特許文献１には、２つの３次元形状の一致度を判定する判定装置が開示されている。該判定装置は、それぞれの三次元形状における各特徴点を、表面からの深さに基づく特徴量によって木構造化された複数のサブ空間に分類し、互いに対応するサブ空間の間で、互いに対応する特徴点を探索することによって、探索処理の処理時間の短縮を図っている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２０１１−３１２７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、木構造を用いた最近傍探索によっても、処理時間の短縮が十分ではないといった問題がある。
【０００９】
本発明は、こうした問題を解決するためになされたもので、最近傍探索を高速化できる技術を提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記の課題を解決するために、第１の態様に係る画像処理装置は、対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得部と、前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート部と、前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索部と、前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定部とを備え、前記探索部が、前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択部と、前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新部と、前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御部と、前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定部とを備えたことを特徴とする。
【００１１】
第２の態様に係る画像処理装置は、第１の態様に係る画像処理装置であって、前記基準点設定部が、前記基準座標軸方向についての前記第１点群の並び順に前記基準点を逐次設定し、前記探索部が、逐次設定された前記基準点に関する前記探索処理のそれぞれにおいて、当該基準点と、当該基準点の直前に設定された基準点に対して探索された最近傍点との距離を前記基準距離の初期値として設定しつつ、前記探索処理を逐次行うことを特徴とする。
【００１２】
第３の態様に係る画像処理装置は、第１または第２の態様に係る画像処理装置であって、座標空間の各座標軸のうち前記参照点群データの派生元の点群データにおける座標値の分布幅が最も広い座標軸を前記基準座標軸として設定する座標軸設定部をさらに備えることを特徴とする。
【００１３】
第４の態様に係る画像処理装置は、第１から第３の何れか１つの態様に係る画像処理装置であって、前記対象物の三次元形状を測定可能な測定部をさらに備え、前記取得部が、前記測定部が前記対象物を測定して取得した測定情報に基づいて、前記第１測定点群データを取得することを特徴とする。
【００１４】
第５の態様にかかるプログラムは、画像処理装置に搭載されたコンピュータにおいて実行されることにより当該画像処理装置を第１から第４の何れか１つの態様に係る画像処理装置として機能させることを特徴とする。
【００１５】
第６の態様に係る画像処理方法は、対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得工程と、前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート工程と、前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索工程と、前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定工程とを有し、前記探索工程が、前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択工程と、前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新工程と、前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御工程と、前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定工程とを有することを特徴とする。
【発明の効果】
【００１６】
第１から第６の何れの形態に係る発明によっても、第１モデル点群データと第１測定点群データとが基準座標軸の座標値順にそれぞれソートされた各点群データに係る第１点群と第２点群とにおいて、第１点群の基準点に対する基準座標軸方向の距離が基準距離よりも短い注目点が第２点群から選択される。また、基準点と注目点との点間距離が基準距離よりも短い場合には、当該点間距離によって基準距離が設定され更新される。そして、更新された基準距離が、選択点の選択処理に反映されつつ、該選択処理と、基準距離の更新処理とが逐次実行され、これらの処理が終了されたときの基準距離の設定に用いられた注目点が最近傍点として決定される。従って、探索が進むにつれて、基準軸方向の探索範囲が狭められつつ最近傍点の探索が行なわれるので、最近傍探索が高速化され得る。
【図面の簡単な説明】
【００１７】
【図１】実施形態に係る画像処理装置を用いた画像処理システムの外観構成を例示する図である。
【図２】実施形態に係る画像処理装置の主な機能構成を例示するブロック図である。
【図３】最近傍探索の概念を示す図である。
【図４】実施形態に係る画像処理装置が行なう最近傍探索を説明するための図である。
【図５】実施形態に係る画像処理装置が行なう最近傍探索において注目点の選択が可能な領域を例示する図である。
【図６】基準座標軸の設定例について説明するための図である。
【図７】基準座標軸の設定例について説明するための図である。
【図８】各種手法による最近傍探索の処理時間を例示するグラフを示す図である。
【図９】各種手法による最近傍探索の処理時間を例示するグラフを示す図である。
【図１０】実施形態に係る最近傍探索における点間距離の計算回数を例示するグラフを示す図である。
【図１１】実施形態に係る最近傍探索における点間距離の計算回数を例示するグラフを示す図である。
【図１２】実施形態に係る画像処理装置の動作フローの一例を示す図である。
【図１３】実施形態に係る画像処理装置の動作フローの一例を示す図である。
【図１４】実施形態に係る画像処理装置の動作フローの一例を示す図である。
【図１５】実施形態に係る画像処理装置の動作フローの一例を示す図である。
【図１６】実施形態に係る画像処理装置の動作フローの一例を示す図である。
【発明を実施するための形態】
【００１８】
以下、本発明の一実施形態を図面に基づいて説明する。図面では同様な構成および機能を有する部分に同じ符号が付され、下記説明では重複説明が省略される。また、各図面は模式的に示されたものであり、例えば、各図面における画像上の表示物のサイズおよび位置関係等は正確に図示されたものではない。また、画像データと、該画像データに基づいて表示される画像とをまとめて「画像」と適宜総称する。また、ユークリッド距離は、単に「距離」とも称される。なお、図４〜図７には、直交するＸＹＺの３軸が付されている。
【００１９】
＜実施形態について＞
＜（1）画像処理システム１００Ａの構成について＞
図１は、実施形態に係る画像処理装置２００Ａを用いた画像処理システム１００Ａの外観構成を例示する図である。図１に示されるように、画像処理システム１００Ａは、画像処理装置２００Ａとロボット３００Ａとを主に備えて構成されている。
【００２０】
画像処理装置２００Ａは、画像処理部２００とステレオカメラ４００とを主に備えて構成され、ステレオカメラ４００は、左カメラ６１と右カメラ６２とを主に備えて構成されている。左カメラ６１と右カメラ６２とは所定の基線長を隔てて設けられており、それぞれのカメラの焦点距離などのカメラパラメータは既知である。ステレオカメラ４００は、着脱機構などにより柱状物などに着脱自在に構成されており、図１の例では、ロボット装置３００Ａの腕部６４の両端部のうち、手部６５側の端部に取付けられている。ステレオカメラ４００の制御は、画像処理部２００が通信回線ＤＬ１を介して供給する制御信号によって行なわれる。
【００２１】
画像処理装置２００Ａは、ステレオカメラ４００が対象物を撮像した左画像１（図２）および右画像２（図２）を画像処理部２００で処理することによって、原測定点群データ４（図２）を取得する。原測定点群データ４は、ステレオカメラ４００を基準とするカメラ座標系における対象物上の各点の三次元座標が、実測に基づいて表現された点群データである。
【００２２】
ここで、後述するロボット装置３００Ａは、ロボット装置３００Ａの本体部６３を基準とした所定のロボット座標系において、ロボット装置３００Ａの各部の動作を制御可能に構成されている。従って、画像処理システム１００Ａにおいては、腕部６４に取付けられたステレオカメラ４００の、ロボット座標系における位置・姿勢も取得可能である。従って、ステレオカメラ４００によって測定された原測定点群データ４は、カメラ座標系からロボット座標系に座標変換され得る。
【００２３】
ロボット装置３００Ａの近傍に設置された作業台６６には、既知の三次元形状を有する複数の対象物７１が載置されている。ロボット装置３００Ａに対する作業台６６の位置・姿勢は既知である。画像処理部２００の記憶部５２（図２）には、対象物７１が作業台６６上の所定の基準位置に、所定の基準姿勢で載置されたときの該対象物７１についての原モデル点群データ３（図２）が、予め記憶されている。原モデル点群データ３としては、例えば、対象物７１の各点の三次元座標がロボット座標系において表現されたＣＡＤデータ等が、採用される。なお、原モデル点群データ３が、例えば、ステレオカメラ４００に比べて、測定精度が十分に高い三次元測定機を用いて測定された対象物７１の三次元形状データに基づいて生成された点群データなどであったとしても良い。すなわち、原モデル点群データ３は、対象物７１上の各点の三次元座標が、設計数値的に表現された点群データである。
【００２４】
ロボット装置３００Ａは、本体部６３、所定の回転軸周りに回転可能に構成された回転部６８、回転部６８に取付けられ、関節部を有して屈伸可能な腕部６４、および腕部６４の先端に取り付けられた手部６５を主に備えて構成される。ロボット装置３００Ａの制御は、画像処理部２００が通信回線ＤＬ２を介して供給する制御信号によって行なわれる。なお、通信回線ＤＬ１、ＤＬ２は、有線の回線であっても、無線通信による回線であっても良い。本体部６３は、画像処理部２００からの制御に基づいて、回転部６８による所定の回転軸周りの回転動作と、関節部での腕部６４の屈伸動作とを制御するとともに、手部６５による対象物の把持解放動作を制御する。
【００２５】
なお、ロボット装置３００Ａは、作業台６６上の所定の基準位置に所定の基準姿勢で載置された対象物７１、および該対象物７１に対する位置・姿勢が既知の対象物７１を把持して、不図示の他の作業台等へ載置するパレタイジング動作などを実行可能に構成されている。従って、作業台６６に載置された対象物７１の実際の位置・姿勢が不明であったとしても、基準の位置・姿勢に対する該対象物７１の実際の位置・姿勢を求めることができれば、ロボット装置３００Ａは、該対象物７１のパレタイジング動作を行い得る。
【００２６】
そこで、画像処理システム１００Ａは、画像処理装置２００Ａが、作業台６６における位置・姿勢が不明な対象物７１についての原測定点群データ４を取得し、原測定点群データ４を用いて基準の位置・姿勢に対する該対象物７１の実際の位置・姿勢を求める。
【００２７】
図１２は、実施形態に係る画像処理装置２００Ａが、基準の位置・姿勢に対する該対象物７１の位置・姿勢を求める動作フローＳ１００の一例を示す図である。
【００２８】
動作フローＳ１００に示される動作において、原測定点群データ４（図２）は、先ず、記憶部５２から原モデル点群データ３を取得する（図１２のステップＳ１１０）。次に、画像処理装置２００Ａは、ステレオカメラ４００によって対象物７１を測定し、原測定点群データ４を取得する（図１２のステップＳ１２０）。原測定点群データ４は、具体的には、三次元化部３１（図２）が、入出力部５１を介して取得した左画像１、右画像２を処理することによって取得される。三次元化部３１は、左画像１と右画像２との間で対応点探索処理を行なって両画像間で互いに対応した対応点を取得し、取得した対応点に対応した対象物７１の点の三次元座標を三角測量の原理を用いて求めることなどによって原測定点群データ４を取得する。取得された原測定点群データ４は、カメラ座標系で表現されており、取得部３２（図２）に供給される。
【００２９】
取得部３２は、原測定点群データ４の原モデル点群データ３への相対的な粗位置合わせを行い、第１モデル点群データ５（図２）と第１測定点群データ６（図２）とを取得する（図１２のステップＳ１３０）。具体的には、取得部３２は、先ず、ロボット座標系におけるステレオカメラ４００の位置・姿勢に基づいて、原測定点群データ４をカメラ座標系からロボット座標系に座標変換する。次に、取得部３２は、互いにロボット座標系で表現された原モデル点群データ３と原測定点群データ４とのそれぞれの重心を算出し、算出された２つの重心が一致するように、原測定点群データ４を原モデル点群データ３に相対的に移動させることにより該粗位置合わせを行なう。該粗位置合わせが施された原モデル点群データ３が第１モデル点群データ５であり、該粗位置合わせが施された原測定点群データ４が第１測定点群データ６である。なお、三次元化部３１は、予め、生成された第１モデル点群データ５と第１測定点群データ６とを例えば、入出力部５１を介して取得することなどによっても第１モデル点群データ５と第１測定点群データ６とを取得することもできる。
【００３０】
第１モデル点群データ５と第１測定点群データ６とが取得されると、画像処理部２００のＣＰＵ４３Ａ（図２）は、ＩＣＰ法を用いた原測定点群データ４の原モデル点群データ３への配置合わせ（位置・姿勢合わせ）を行なう（図１２のステップＳ１４０）。ＩＣＰ法を用いた配置合わせの詳細については後述する。
【００３１】
該配置合わせが完了すると、ＣＰＵ４３Ａは、原測定点群データ４を原モデル点群データ３に配置合わせする座標変換情報を取得する（図１２のステップＳ１５０）。具体的には、ＣＰＵ４３Ａは、原測定点群データ４の原モデル点群データ３への相対的な粗位置合わせの変換と、ＩＣＰ法を用いた配置合わせの変換とを総合することにより該座標変換情報を取得する。
【００３２】
該座標変換情報が取得されると、ＣＰＵ４３Ａは、該座標変換情報を表現したアフィン変換の回転行列と平行移動行列とに基づいて、作業台６６における所定の基準位置および基準姿勢に対する対象物７１の実際の位置・姿勢情報２０（図２）を取得する（図１２のステップＳ１６０）。取得された位置・姿勢情報２０は、記憶部５２に記憶される。ロボット装置３００Ａは、位置・姿勢情報２０に基づいて対象物７１のパレタイジング動作を行なう。
【００３３】
なお、画像処理システム１００Ａのうち画像処理装置２００Ａのみを用いることによって、例えば、対象物７１の実際の形状と、設計された形状との差異を検出する形状検査が行なわれ得る。原測定点群データ４の原モデル点群データ３への相対的な配置合わせを行なうことができれば、配置合わせ後における原モデル点群データ３と原測定点群データ４との差に基づいて、対象物７１の形状検査が可能となる。従って、形状検査用の原モデル点群データ３として、作業台６６上の基準位置に基準姿勢で載置された対象物７１のロボット座標系で表現された設計数値的な点群データが使用される必要は無い。形状検査用の原モデル点群データ３としては、任意の三次元座標系の任意の位置に、任意の姿勢で配置された、対象物７１についての設計数値的な点群データが使用可能である。
【００３４】
また、パレタイジング動作用および形状検査用の何れにおいても、ステレオカメラ４００などの三次元測定機によって、先ず、対象物７１のポリゴンデータ等が取得され、該ポリゴンデータが原測定点群データ４に変換されて使用されたとしても本発明の有用性を損なうものではない。また、パレタイジング動作用および形状検査用の何れにおいても、対象物７１の三次元形状が、例えば、ベジェ曲線などによって表現された形状データが記憶部５２に予め記憶されていたとしても良い。この場合には、画像処理部２００において、該形状データから原モデル点群データ３が生成されて使用される。
【００３５】
＜（２）画像処理装置２００Ａの構成について＞
図２は、実施形態に係る画像処理装置２００Ａの主な機能構成を例示するブロック図である。既述したように画像処理装置２００Ａは、画像処理部２００とステレオカメラ４００とを主に備えて構成される。また、画像処理部２００は、例えば、入出力部５１、記憶部５２、およびＣＰＵ４３Ａを主に備えて構成される。
【００３６】
◎入出力部５１について：
入出力部５１は、例えばＵＳＢインタフェース、またはＢｌｕｅｔｏｏｔｈ（登録商標）インタフェースなどの入出力インタフェース、マルチメディアドライブ、およびネットワークアダプタなどのＬＡＮやインターネットに接続するためのインタフェースなどを備えて構成され、ＣＰＵ４３Ａとの間でデータの授受を行うものである。具体的には、入出力部５１は、例えば、ＣＰＵ４３Ａがステレオカメラ４００を制御するための各種の制御信号を、通信回線ＤＬ１を介してステレオカメラ４００へと供給する。また、入出力部５１は、ステレオカメラ４００が撮影した左画像１および右画像２を画像処理部２００に供給する。また、入出力部５１は、例えば、予め生成された原モデル点群データ３および原測定点群データ４が記憶された光ディスクなどの記憶媒体を受け付けることなどによって、原モデル点群データ３および原測定点群データ４を画像処理部２００に供給することもできる。
【００３７】
◎記憶部５２について：
記憶部５２は、例えば、それぞれ不図示のＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、およびフラッシュメモリなどの読み書き自在な不揮発性メモリによって構成される。不揮発性メモリに代えて、ハードディスク装置などが採用されても良い。
【００３８】
該ＲＯＭは、読出し専用メモリであり、ＣＰＵ４３Ａを動作させるプログラムＰＧ１などを格納している。該ＲＡＭは、読み書き自在の揮発性メモリであり、画像処理装置２００Ａが取得、または処理した各種画像などを一時的に記憶する画像格納部として機能する。また、該ＲＡＭは、ＣＰＵ４３Ａの処理情報を一時的に記憶するワークメモリなどとしても機能する。なお、後述するＣＰＵ４３Ａにおける各要素の間での情報の授受は、該ＲＡＭを介して行なわれる。該不揮発性メモリは、画像処理部２００の各種制御パラメータや各種動作モードなどの情報を恒久的に記録する。原モデル点群データ３も予め該不揮発性メモリに記憶されている。
【００３９】
◎ＣＰＵ４３Ａについて：
ＣＰＵ（Central Processing Unit）４３Ａは、画像処理装置２００Ａの各機能部を統轄制御する制御処理装置であり、記憶部５２に格納されたプログラムＰＧ１などに従った制御および処理を実行する。ＣＰＵ４３Ａは、三次元化部３１、取得部３２、座標軸設定部３３、ソート部３４、基準点設定部３５、選択部３６、初期化部３７、更新部３８、探索制御部３９、決定部４０、および配置合わせ部４１などの各要素としても動作する。
【００４０】
ＣＰＵ４３Ａは、これらの各要素などとして動作することによって、左画像１および右画像２をそれぞれ処理して対象物７１についての位置・姿勢情報２０（図２）を取得する。また、ＣＰＵ４３Ａ、入出力部５１、記憶部５２等のそれぞれは、電気的に接続されている。したがって、ＣＰＵ４３Ａは、例えば、入出力部５１を介したステレオカメラ４００、ロボット装置３００Ａの制御およびステレオカメラ４００からの画像情報の取得等を所定のタイミングで実行できる。なお、図２に示される構成例では、三次元化部３１〜配置合わせ部４１などの各要素は、ＣＰＵ４３Ａで所定のプログラムＰＧ１を実行することによって実現されているが、これらの各要素はそれぞれ、例えば、専用のハードウェア装置などによって実現されてもよい。
【００４１】
○三次元化部３１：
三次元化部３１は、左画像１と右画像２との間で対応点探索処理を行なって両画像間で互いに対応した対応点を取得し、取得した対応点に対応した対象物７１の点の三次元座標を三角測量の原理を用いて求めることなどによって原測定点群データ４を取得する。原測定点群データ４の取得においては、予め記憶部５２に記憶されているステレオカメラ４００についての各種のカメラパラメータおよび基線長情報などが用いられる。取得された原測定点群データ４は、所定のカメラ座標系で表現されており、取得部３２（図２）に供給される。
【００４２】
○取得部３２：
取得部３２は、原測定点群データ４を、記憶部５２から読出した原モデル点群データ３に相対的に粗位置合わせを行うことにより、第１モデル点群データ５（図２）と第１測定点群データ６（図２）とを取得する。第１モデル点群データ５は、対象物７１上の各点の三次元座標を設計数値的にそれぞれ表現した点群データであり、第１測定点群データ６は、対象物７１上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データである。
【００４３】
取得部３２は、第１モデル点群データ５を基準点群データ１０（図２）の派生元の点群データとしてソート部３４に供給するとともに、第１測定点群データ６を参照点群データ１１（図２）の派生元の点群データとして座標軸設定部３３およびソート部３４に供給する。なお、基準点群データ１０が表現する第１点群において最近傍探索の基準となる基準点が設定され、参照点群データ１１が表現する第２点群において最近傍点の候補点である注目点が設定される。
【００４４】
なお、第１測定点群データ６が基準点群データ１０の派生元の点群データとしてソート部３４に供給されるとともに、第１モデル点群データ５が参照点群データ１１の派生元の点群データとして座標軸設定部３３およびソート部３４に供給されたとしても本発明の有用性を損なうものではない。
【００４５】
すなわち、画像処理部２００においては、第１モデル点群データ５と第１測定点群データ６とのうち一方の点群データが基準点群データ１０の派生元の点群データとしてソート部３４に供給される。また、他方の点群データが参照点群データ１１の派生元の点群データとして座標軸設定部３３およびソート部３４に供給される。
【００４６】
なお、第１モデル点群データ５と第１測定点群データ６との何れの点群データが基準点群データ１０の派生元の点群データであるかを規定した情報は、例えば、予め記憶部５２に記憶されており、取得部３２その他の各部によって取得されて用いられる。
【００４７】
○座標軸設定部３３：
座標軸設定部３３は、座標空間の各座標軸のうち参照点群データ１１の派生元の点群データにおける座標値の分布幅が最も広い座標軸を基準座標軸として設定し、該基準座標軸についての基準座標軸情報９（図２）をソート部３４に供給する。なお、図２に例示された構成においては、座標軸設定部３３は、第１測定点群データ６に基づいて基準座標軸を設定している。
【００４８】
○ソート部３４：
ソート部３４は、第１モデル点群データ５を基準座標軸の座標値順にソートすることにより第２モデル点群データ７を取得するとともに、第１測定点群データ６を基準座標軸の座標値順にソートすることにより第２測定点群データ８を取得する。なお、該ソート処理は、例えば、メモリ空間における各データの記憶アドレスがソート順に従って変更されることにより、あるいは記憶アドレスは変更されずにソート順を示す新たな情報が各データに付加されることなどにより実現される。
【００４９】
ソート部３４は、予め記憶部５２に記憶された設定情報に基づいて、第２モデル点群データ７と第２測定点群データ８との何れか一方の点群データを基準点群データ１０として基準点設定部３５および配置合わせ部４１に供給する。また、ソート部３４は、他方の点群データを参照点群データ１１として選択部３６および配置合わせ部４１に供給する。図２に例示された構成においては、第２モデル点群データ７が基準点群データ１０として採用され、第２測定点群データ８が参照点群データ１１として採用されている。
【００５０】
○基準点設定部３５：
基準点設定部３５は、基準点群データ１０が表現する第１点群の各点を基準点としてそれぞれ設定する。画像処理部２００においては、該基準点に対する距離が最短となる最近傍点が、参照点群データ１１が表現する第２点群において探索される。なお、基準点設定部３５が、例えば、基準座標軸方向についての該第１点群の並び順に基準点を逐次設定したとしても本発明の有用性を損なうものではない。基準点設定部３５は、設定した基準点を規定した基準点情報１２を選択部３６、初期化部３７および配置合わせ部４１へと供給する。
【００５１】
○探索部４２：
探索部４２は、基準点群データ１０が表現する第１点群において設定された基準点に対する距離が最短となる最近傍点を、参照点群データ１１が表現する第２点群において探索する探索処理を行なう。探索部４２は、また、選択部３６、初期化部３７、更新部３８、探索制御部３９、および決定部４０としても動作する。
【００５２】
○選択部３６：
選択部３６は、それぞれ選択された基準点について、該基準点に対する基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を第２点群において選択する選択処理を行なう。選択部３６は、選択した注目点を規定する注目点情報１３を更新部３８に供給する。また、選択部３６は、選択した注目点が、最近傍探索の開始当初における最初の注目点である場合には、該最初の注目点を規定した注目点情報１３を初期化部３７に対しても供給する。
【００５３】
図５は、実施形態に係る画像処理装置２００Ａ（画像処理部２００）が行なう最近傍探索において注目点の選択が可能な領域Ｒ１を例示する図である。黒塗りの四角形で示された点は、基準点設定部３５によって設定された１つの基準点Ａ３である。黒塗りの円でそれぞれ示された各点は、参照点群データ１１によって表現された第２点群である。なお、図５においてはＸ軸が基準座標軸である。
【００５４】
枠Ｓ１は、ＸＹ空間における該第２点群の分布空間の外縁を示している。また、領域Ｒ２は、枠Ｓ１に内包された領域である。斜線が付された領域Ｒ１は、注目点の選択が可能な領域であり、基準点Ａ３を空間的な基準点として基準距離に基づいて設定されている。
【００５５】
いわゆる総当たり探索（線形探索）においては、領域Ｒ２が注目点の選択可能な範囲、すなわち基準点Ａ３に対する最近傍点の探索範囲となる。対して、画像処理装置２００Ａにおいては、領域Ｒ２よりも狭い探索範囲Ｒ１が注目点の選択が可能な範囲となる。従って、画像処理装置２００Ａによれば、線形探索に比べて最近傍点の探索処理に要する処理時間を短縮できる。
【００５６】
さらに、後述するように、画像処理装置２００Ａにおいては、最近傍点の探索（最近傍探索）が進むにつれて領域Ｒ１の基準座標軸方向に沿った幅が狭くなりつつ最近傍探索が行なわれる。従って、線形探索が行なわれる場合に比べて最近傍点の探索処理に要する処理時間がさらに短縮され得る。
【００５７】
○初期化部３７：
初期化部３７は、それぞれ供給された基準点情報１２と、最初の注目点を規定した注目点情報１３とに基づいて、基準点と最初の注目点との距離を、基準距離１６の初期値として設定する。設定された基準距離１６は、選択部３６および更新部３８に供給される。
【００５８】
○更新部３８：
更新部３８は、基準点と注目点との点間距離が基準距離１６よりも短い場合には、当該点間距離を基準距離１６として設定することにより基準距離１６を更新する更新処理を行なう。更新部３８は、更新された基準距離１６を選択部３６に供給するとともに、基準距離１６と、該基準距離１６の設定に用いられた注目点を規定した注目点情報１３とを対応づけて決定部４０に供給する。また、更新部３８は、基準距離１６の初期値についても、該基準距離１６と、該基準距離１６の設定に用いられた最初の注目点を規定した注目点情報１３とを対応づけて決定部４０へと供給する。
【００５９】
○探索制御部３９：
探索制御部３９は、第２点群において注目点として新たに選択可能な未選択点が無くなるまで、更新処理によって更新された基準距離１６が注目点の選択処理に反映されつつ、選択処理と更新処理とがそれぞれ逐次実行されるように、選択部３６と更新部３８とを制御する。
【００６０】
○決定部４０：
決定部４０は、第２点群において注目点として新たに選択可能な未選択点が無くなったことにより、注目点の選択処理と基準距離の更新処理との逐次実行処理が終了されたことを探索制御部３９からの信号に基づいて検知する。そして、決定部４０は、該選択処理と該更新処理との逐次実行処理が終了されたときの基準距離１６の設定に用いられた注目点を最近傍点として決定する。決定部４０は、最近傍点を規定した最近傍点情報１７を選択部３６および配置合わせ部４１に供給する。
【００６１】
なお、基準点設定部３５が基準座標軸方向についての第１点群の並び順に基準点を逐次設定する動作を行なう場合には、探索制御部３９は、逐次設定される基準点に対して最近傍探索を逐次行なう。また、この場合、最近傍点情報１７を供給された選択部３６は、探索された最近傍点を、次に設定される基準点に対する探索処理における最初の注目点として設定する。該最初の注目点に関する注目点情報１３は、初期化部３７へと供給され、初期化部３７において、基準距離１６の初期値の設定に用いられる。
【００６２】
○配置合わせ部４１：
配置合わせ部４１は、基準点設定部３５によってそれぞれ設定された基準点のそれぞれについて、最近傍点が探索されると、各基準点と各最近傍点との対応する組み合わせ間での距離の和が最小値となる座標変換を、例えば、最小二乗法などによって求める。該座標変換は、参照点群データ１１を基準点群データ１０に相対的に配置合わせする座標変換情報である。配置合わせ部４１は、該座標変換を用いて参照点群データ１１を座標変換することによって参照点群データ１１の基準点群データ１０への配置合わせを行なう。該配置合わせが完了すると、配置合わせ部４１は、各基準点と各最近傍点との対応する組み合わせ間での距離の和が所定の閾値以下である否かを判定する。
【００６３】
該判定の結果、該距離の和が閾値より大きければ、配置合わせ部４１は、基準点群データ１０および参照点群データ１１に基づいて、第１モデル点群データ５および第１測定点群データ６を更新し、取得部３２に供給する。そして、該第１モデル点群データ５および該第１測定点群データ６についての上述した配置合わせ処理が再び行なわれる。すなわちＩＣＰ法による配置合わせが行なわれる。
【００６４】
該判定の結果、該距離の和が閾値以下であれば、配置合わせ部４１は、原測定点群データ４を原モデル点群データ３に配置合わせする座標変換情報を取得する。該座標変換情報は、原測定点群データ４の原モデル点群データ３への相対的な粗位置合わせの変換と、ＩＣＰ法を用いた各配置合わせの変換とを総合することによって求められる。
【００６５】
該座標変換情報が取得されると、配置合わせ部４１は、該座標変換情報を表現したアフィン変換の回転行列と平行移動行列とに基づいて、作業台６６における所定の基準位置および基準姿勢に対する対象物７１の実際の位置・姿勢情報２０（図２）を取得する。取得された位置・姿勢情報２０は、記憶部５２に供給され、記憶部５２に記憶される。
【００６６】
＜（３）画像処理装置２００Ａが行なう配置合わせ動作について＞
図１３、図１４は、図１２のステップＳ１４０におけるＩＣＰ法を用いた配置合わせの処理に係る画像処理装置２００Ａ（より詳細には、画像処理部２００）の動作フローの一例を示す図である。
【００６７】
次に、画像処理装置２００Ａの画像処理部２００が行なうＩＣＰ法を用いた配置合わせの動作について、図１３、図１４を参照しつつ詳しく説明する。
【００６８】
図１２のステップＳ１４０の処理が開始されると、処理は、図１３のステップＳ２１０に移され、座標軸設定部３３が、既述した処理手法によってソート処理用の基準座標軸を設定する（ステップＳ２１０）。
【００６９】
図６および図７は、基準座標軸の設定例について説明するための図である。なお、ここでは、第１モデル点群データ５が基準点群データ１０の派生元の点群データであり、第１測定点群データ６が参照点群データ１１の派生元の点群データであるとして説明が行なわれる。
【００７０】
測定点群ＢＧ２（図６）と測定点群ＢＧ３（図７）とは、それぞれ、第１測定点群データ６が表現した測定点群の一例である。測定点群ＢＧ２においては、Ｘ、Ｙ、Ｚの各軸方向の座標値の分布幅のうちＸ軸方向に沿った分布幅が最大となっている。すなわち、各座標軸方向に沿った測定点群の分布密度のうちＸ軸方向に沿った分布密度が最小となっている。従って、測定点群ＢＧ２については、Ｘ軸が基準座標軸として選択された場合に、注目点の選択が可能な領域Ｒ１（図５）に内包された点の数が最小となる。
【００７１】
また、測定点群ＢＧ３においては、Ｙ軸方向に沿った座標値の分布幅が最大となっている。すなわち、各座標軸方向に沿った測定点群の分布密度のうちＹ軸方向に沿った分布密度が最小となっている。従って、測定点群ＢＧ３については、Ｙ軸が基準座標軸として選択された場合に、領域Ｒ１に内包された点の数が最小となる。
【００７２】
上述したように、座標値の最大値から最小値を減じた値が最大となる座標軸がソート対象の基準座標軸として選択されれば、点間距離の計算対象となる注目点の点数が最も少なくなり得る。従って、最近傍探索の処理時間の更なる短縮が可能となる。なお、何れの座標軸が基準座標軸として選択されたとしても、本発明の有用性を損なうものではない。
【００７３】
図１３のステップＳ２１０において基準座標軸が設定されると、ソート部３４は、第１モデル点群データ５を基準座標軸の座標値順にソートすることにより第２モデル点群データ７を取得するとともに、第１測定点群データ６を基準座標軸の座標値順にソートすることにより第２測定点群データ８を取得する（図１３のステップＳ２２０）。なお、第１モデル点群データ５と第１測定点群データ６との点数は、例えば、ほぼ同じ点数でも、異なった点数であってもよい。
【００７４】
次に、基準点設定部３５は、第２モデル点群データ７（基準点群データ１０）が表現する第１点群において基準点を設定する（図１３ステップＳ２３０）。基準点が設定されると、探索部４２によって基準点に対する最近傍点を探索する最近傍探索処理が行なわれる（図１３のステップＳ２４０）。
【００７５】
図１５、図１６は、図１３のステップＳ２４０における最近傍探索処理に係る画像処理装置２００Ａ（より詳細には、画像処理部２００）の動作フローの一例を示す図である。最近傍探索処理が開始されると、選択部３６は、第２測定点群データ８（参照点群データ１１）が表現する第２点群において昇順探索（前方探索）用の最初の注目点を選択する（図１５のステップＳ３１０）。
【００７６】
最初の注目点が選択されると、初期化部３７は、基準点と該注目点との点間距離を、基準距離１６の初期値として設定する（図１５のステップＳ３２０）。
【００７７】
次に、更新部３８は、該点間距離が基準距離１６未満であるか否かを判定する（図１５のステップＳ３３０）。
【００７８】
ステップＳ３３０での判定の結果、該点間距離が基準距離１６未満であれば、更新部３８は、該点間距離を基準距離１６として設定することにより基準距離１６を更新し（図１５のステップＳ３４０）、処理はステップＳ３５０に移される。また、ステップＳ３３０での判定の結果、該点間距離が基準距離１６以上であれば、処理は、ステップＳ３５０に移される。
【００７９】
なお、ステップＳ３３０での判定においては、基準点と注目点との各座標値の差についての二乗和に基づいて比較することにより、該二乗和の平方根を求める場合に比べて処理時間を短縮されている。また、座標の二乗和を求める前に、基準点と注目点とのＹ（Ｚ）座標の差の絶対値と、基準距離１６とを比較し、Ｙ（Ｚ）座標の差の絶対値が、基準距離１６よりも大きければ、座標の二乗和を計算することなく、処理はステップＳ３５０に移される。従って処理時間はさらに短縮され得る。また、後述するステップＳ３８０（図１６）においても、ステップＳ３３０と同様に処理が行なわれる。
【００８０】
図１５のステップＳ３５０において、選択部３６は、第２点群において基準座標軸の座標値の昇順で注目点の１つ隣の点と、基準点との基準軸方向の距離が基準距離１６未満であるか否かを判定する。
【００８１】
ステップＳ３５０での判定の結果、基準点との基準軸方向の距離が基準距離１６未満であれば、選択部３６は、該隣の点を新たな注目点として選択し（図１５のステップＳ３６０）、探索制御部３９は、処理をステップＳ３２０に戻す。
【００８２】
ステップＳ３５０での判定の結果、基準点との基準軸方向の距離が基準距離１６以上であれば、探索制御部３９は、昇順方向の注目点の選択（最近傍点の探索）において、注目点として選択可能な未選択点が無くなったと判定し、処理を図１６のステップＳ３７０に移す。
【００８３】
図４は、画像処理装置２００Ａ（画像処理部２００）が行なう、上述した最近傍探索（前方探索）を説明するための図である。
【００８４】
図４に示された基準点Ａ１は、最近傍点の探索と処理が行なわれている基準点であり、基準点Ａ０は、基準点Ａ１の直前に、最近傍点が探索された基準点である。基準点Ａ０と基準点Ａ１とは、基準座標軸であるＸ軸の座標値順に、ソートされたときに隣り合う点である。第２点群における点Ｂ１〜Ｂ６は、基準軸方向の座標値が、基準点Ａ１についての該座標値に近い注目点の例であり、それぞれ、基準点Ａ１に対する注目点として順次設定される。特に点Ｂ１は、基準点Ａ０に対して探索された最近傍点でもある。また、点Ｂ１〜Ｂ６のそれぞれのＸ座標は、昇順に増加している。
【００８５】
基準点Ａ１と点Ｂ１、Ｂ２、Ｂ３、およびＢ４のそれぞれとの距離（ユークリッド距離）は、それぞれ距離（点間距離）Ｌ１、Ｌ２、Ｌ３、およびＬ４である。座標Ｘ１は、基準点Ａ１のＸ座標である。座標Ｘ２は、座標Ｘ１から＋Ｘ方向（昇順方向、前方）に、距離Ｌ３離れた座標であり、座標Ｘ３は、座標Ｘ１から＋Ｘ方向に、距離Ｌ１離れた座標である。
【００８６】
基準点Ａ１に対する最初の注目点としては、点Ｂ１が選択される。該選択は、第１点群と第２点群とは、それぞれソート済みであるので、基準点Ａ０の最近傍点と、基準点Ａ１の最近傍点との位置は近いと推定されることに基づいている。そして、基準距離１６としては、距離Ｌ１が採用される。
【００８７】
距離Ｌ１の基準距離１６としての採用により、点Ｂ１の次の注目点としては、座標Ｘ１〜座標Ｘ３の範囲のＸ座標を有する点Ｂ２〜Ｂ４がＸ座標の昇順に選択可能となる。そして、点Ｂ２が該次の注目点として、先ず選択される。
【００８８】
ここで、距離Ｌ２の値は、距離Ｌ１の値よりも大きい。従って、距離Ｌ２は、基準距離１６として設定されることなく、点Ｂ３が点Ｂ２の次の注目点として選択される。また、距離Ｌ３の値は、距離Ｌ１の値よりも小さいため、距離Ｌ３が基準距離１６として設定されて、基準距離１６が更新される。
【００８９】
基準距離１６の該更新により、座標Ｘ１〜座標Ｘ２の範囲のＸ座標を有する点が、次の注目点として選択され得る。このように、探索が進むにつれて、注目点の選択範囲、すなわち最近傍点の探索範囲は狭められていく。なお、点Ｂ４および点Ｂ４よりもＸ座標が大きい点Ｂ５以降の各点は、基準点Ａ１に対するユークリッド距離を実際に計算されるまでもなく注目点としての選択対象から除外される。そして、昇順方向の近傍点探索は終了される。後方探索（降順探索）についても、昇順探索と同様にして行なわれる。
【００９０】
なお、注目点の逐次の選択処理として、例えば、基準点に対して、昇順方向と降順方向の両方向において、基準点に対する各測定点の基準座標軸方向の距離（絶対値）に基づいて、該距離の昇順で注目点が選択される手法が採用され得る。該手法が採用されたとしても探索が進むにつれて、注目点の選択範囲、すなわち最近傍点の探索範囲は狭められていく。従って、該手法が、上述した前方探索および後方探索による注目点の選択手法に代えて選択されたとしても、本発明の有用性を損なうものではない。
【００９１】
また、基準点Ａ１に対する最初の注目点として、前回の探索に係る最近傍点、すなわち、基準点Ａ０に対する最近傍点が採用されない場合には、選択部３６は、例えば、基準点Ａ１に対する基準座標軸方向の距離が最短となる測定点などを最初の注目点として選択する。前回の探索に係る最近傍点が最初の注目点として採用されない場合には、各基準点についての対応点探索処理の並列化が可能となるので、該並列化処理によって探索処理の更なる高速化がなされ得る。従って、最初の注目点として、前回の探索に係る最近傍点が採用されないとしても、本発明の有用性を損なうものではない。
【００９２】
ステップＳ３７０において、選択部３６は、第２測定点群データ８（参照点群データ１１）が表現する第２点群において降順探索（後方探索）用の最初の注目点を選択する。
【００９３】
該最初の注目点が選択されると、更新部３８は、基準点と該注目点との点間距離が基準距離１６未満であるか否かを判定する（図１６のステップＳ３８０）。
【００９４】
ステップＳ３８０での判定の結果、該点間距離が基準距離１６未満であれば、更新部３８は、該点間距離を基準距離１６として設定することにより基準距離１６を更新し（図１６のステップＳ３９０）、処理はステップＳ４００に移される。また、ステップＳ３８０での判定の結果、該点間距離が基準距離１６以上であれば、処理は、ステップＳ４００に移される。
【００９５】
図１６のステップＳ４００において、選択部３６は、第２点群において基準座標軸の座標値の昇順で注目点の１つ隣の点と、基準点との基準軸方向の距離が基準距離１６未満であるか否かを判定する。
【００９６】
ステップＳ４００での判定の結果、基準点との基準軸方向の距離が基準距離１６未満であれば、選択部３６は、該隣の点を新たな注目点として選択し（図１６のステップＳ４１０）、探索制御部３９は、処理をステップＳ３８０に戻す。
【００９７】
ステップＳ４００での判定の結果、基準点との基準軸方向の距離が基準距離１６以上であれば、探索制御部３９は、降順方向の注目点の選択（最近傍点の探索）において、注目点として選択可能な未選択点が無くなったと判定し、処理を図１６のステップＳ４２０に移す。
【００９８】
ステップＳ４２０において、決定部４０は、基準距離１６の設定に用いられた注目点を最近傍点として決定し、処理は、図１３のステップＳ２５０へと移される。
【００９９】
図１３のステップＳ２５０において、基準点設定部３５は、全ての基準点について最近傍探索処理が終了したか否か、すなわち、第１点群における全ての点が基準点として設定されたか否かを判定する。
【０１００】
ステップＳ２５０での判定の結果、全ての基準点について最近傍探索処理が終了していなければ、処理は、図１３のステップＳ２３０へと戻される。また、該判定の結果、全ての基準点について最近傍探索処理が終了していれば、処理は、図１４のステップＳ２６０へと移される。
【０１０１】
ステップＳ２６０において、配置合わせ部４１は、設定された各基準点と、各基準点にそれぞれ探索された各最近傍点とについて、各基準点と各最近傍点との対応する組み合わせ間での距離の和が最小値となる座標変換を、例えば、最小二乗法などによって求める。該座標変換は、参照点群データ１１を基準点群データ１０に相対的に配置合わせする座標変換情報である（ステップＳ２６０）。
【０１０２】
該座標変換情報が求められると、配置合わせ部４１は、該座標変換情報を用いて参照点群データ１１を座標変換することによって参照点群データ１１の基準点群データ１０への配置合わせを行なう（図１４のステップＳ２７０）。
【０１０３】
該配置合わせが完了すると、配置合わせ部４１は、各基準点と各最近傍点との対応する組み合わせ間での距離の和が所定の閾値以下である否かを判定する（図１４のステップＳ２８０）。
【０１０４】
ステップＳ２８０での判定の結果、該距離の和が閾値より大きければ、配置合わせ部４１は、基準点群データ１０および参照点群データ１１に基づいて、第１モデル点群データ５および第１測定点群データ６を更新する（図１４のステップＳ２９０）。そして、処理は、図１３のステップＳ２１０へと戻される。そして、該第１モデル点群データ５および該第１測定点群データ６についての上述した配置合わせ処理が再び行なわれる。
【０１０５】
なお、ステップＳ２２０（図１３）におけるソート処理の手法として、ステップＳ２２０の処理が初めて行なわれる場合には、例えば、一般的に使用される最も高速なソート法の一つであるクイックソート法などが使用される。
【０１０６】
一方、ステップＳ２２０の処理が１回以上行なわれている状態で、ステップＳ２２０が行なわれる場合には、ステップＳ２７０（図１４）での配置合わせ処理によって、先のステップＳ２２０でのソート結果にずれが生ずる。しかし、該ずれが、極端に大きなずれとなることは、通常、起こりえず、該配置合わせ処理によっても、先のステップＳ２２０でのソート結果は、ある程度維持されていると推定される。
【０１０７】
ステップＳ２２０の処理が１回以上行なわれている状態で、ステップＳ２２０が行なわれる場合には、ステップＳ２２０でのソート法としては、例えば、公知のマージソート法などが採用される。従って、２回目移行のソート処理では、クイックソート法が用いられる場合に比べて、ソート処理の高速化が可能となる。
【０１０８】
ステップＳ２８０での判定の結果、該距離の和が閾値以下であれば、ＩＣＰ法を用いた配置合わせ処理は終了される。そして、処理は、図１２のステップＳ１５０へと移され、既述したステップＳ１５０〜Ｓ１６０の処理が行なわれて対象物７１についての位置・姿勢情報２０が取得される。
【０１０９】
次に、画像処理装置２００Ａによる最近傍探索の処理と、従来手法による最近傍探索の処理との比較結果について説明する。
【０１１０】
図８は、ランダムに発生させた点群データを対象として、各種法により行なわれた最近傍探索の処理時間を例示するグラフを示す図である。図８のグラフの横軸は、点群データの個数であり、１００点から１０，０００点の点群データが使用されている。図８のグラフの縦軸は、最近傍点の探索に要した処理時間が表示されている。該処理時間は、図１３のステップＳ２３０〜ステップＳ２５０の反復動作部分の処理に関する処理時間であり、後述する図９においても同様である。
【０１１１】
図８の凡例に示された「Ｌｉｎｅａｒ」は、一般的な線形探索手法を示し、また、「Ｆｌａｎｎ」は、一般的なＮＮＳ（最近傍探索）手法の一種であるＫｄＴｒｅｅ法を実現するオープンソースによる探索処理を示す。また、「Ｓｗｅｅｐ」は、画像処理装置２００Ａ（画像処理部２００）が行なう対応点探索処理であり、該処理は、図１５、図１６に示された動作フローに従って実現されている。なお、後述する図９の凡例についても、図８の凡例と共通の凡例が用いられている。
【０１１２】
図８に示されるように、ランダムに発生させた点群データを対象とした場合には、線形探索手法では、点数の二乗に比例して探索時間が増加しているが、Ｓｗｅｅｐ法では、点数の二乗に比例して探索時間が増加する様なことはなく、他の手法に比べて、処理時間を最も短く抑えることができている。また、点数が１０，０００点の場合では、Ｓｗｅｅｐ法では、ＫｄＴｒｅｅ法の約３倍の高速化が達成できている。
【０１１３】
図９は、４種類の部品Ａ〜Ｄを対象として、各種法により行なわれた最近傍探索の処理時間を例示するグラフを示す図である。なお、図９のグラフにおいては、処理時間は棒グラフで示されている。また、図９のグラフにおいては、各部品についての測定点群数とモデル点群数とがそれぞれ折れ線で表示されている。
【０１１４】
また、部品Ａ、Ｂは、平面部が多い単純形状の部品であり、部品Ｂ、Ｃは、平面部の他に曲面部を有する部品である。なお、部品Ｂには、くぼみ部分が設けられている。
【０１１５】
図９に示されるように、ランダムに発生させた点群データを処理対象としない場合でも、本願手法（Ｓｗｅｅｐ法）は、他の手法に比べて、処理時間をより抑制できている。なお、Ｓｗｅｅｐ法の処理時間は、対象物の形状の差異にはあまり依存せず、主として、処理対象データの点数に依存している。
【０１１６】
図１０、図１１は、実施形態に係る最近傍探索（Ｓｗｅｅｐ法）における点間距離の計算回数を例示するグラフを示す図である。より詳細には、図１０、図１１には、図８に示された、ランダムに生成された１０，０００点の点群に対するＳｗｅｅｐ法における最近傍探索の処理内容が解析されて、基準点と注目点との点間距離の計算回数がヒストグラムとして示されている。図１０の横軸は、昇順探索（前方探索）における図１５のステップＳ３３０において、実際に該点間距離が計算された回数である。また、図１１の横軸は、降順探索（後方探索）における図１６のステップＳ３８０において、実際に該点間距離が計算された回数である。該点間距離の計算には、膨大な時間を要するため、該点間距離の計算回数が少なければ少ないほど、処理時間は短縮される。
【０１１７】
図１０の破線Ｅ１に内包された部分に示されるように、Ｓｗｅｅｐ法の前法探索においては、基準点と注目点との点間距離の計算回数のほとんどは、１０回以内に収まっている。また、図１１の破線Ｅ２に内包された部分に示されるように、Ｓｗｅｅｐ法の後方探索においては、該点間距離の計算回数のほとんどは、３回以内に収まっている。一方、同一データに対して線形探索法が適用された場合には、該点間距離の計算回数は、１０，０００回となる。従って、該点間距離の計算回数の比較によっても、Ｓｗｅｅｐ法が線形探索法に比べて、著しく高速化されていることが裏付けられている。
【０１１８】
上述したように、画像処理装置２００Ａ（画像処理部２００）によれば、第１モデル点群データ５と第１測定点群データ６とが基準座標軸の座標値順にそれぞれソートされた各点群データに係る第１点群と第２点群とにおいて、第１点群の基準点に対する基準座標軸方向の距離が基準距離１６よりも短い注目点が第２点群から選択される。また、基準点と注目点との点間距離が基準距離１６よりも短い場合には、当該点間距離によって基準距離１６が設定され更新される。そして、更新された基準距離１６が、選択点の選択処理に反映されつつ、該選択処理と、基準距離の更新処理とが逐次実行され、これらの処理が終了されたときの基準距離１６の設定に用いられた注目点が最近傍点として決定される。従って、最近傍点の探索が進むにつれて、基準軸方向の探索範囲が狭められつつ最近傍点の探索が行なわれるので、最近傍探索が高速化され得る。
【０１１９】
＜変形例について＞
以上、本発明の実施の形態について説明してきたが、本発明は上記実施の形態に限定されるものではなく様々な変形が可能である。
【０１２０】
例えば、画像処理装置２００Ａのステレオカメラ４００に代えて、検出光を投影して三角測量の原理で三次元測定する測定機、あるいはレーザートラッカーなどが採用されたとしても本発明の有用性を損なうものではない。また、画像処理システム１００Ａにおいて、ステレオカメラ４００は、腕部６４に取付けられているが、ロボット座標系に対するステレオカメラ４００の位置・姿勢が取得可能な他の場所に取付けられたとしても本発明の有用性を損なうものではない。
【符号の説明】
【０１２１】
１００Ａ  画像処理システム
２００Ａ  画像処理装置
２００  画像処理部
３００Ａ  ロボット装置
４００  ステレオカメラ
１  左画像
２  右画像
３  原モデル点群データ
４  原測定点群データ
５  第１モデル点群データ
６  第１測定点群データ
７  第２モデル点群データ
８  第２測定点群データ
９  基準座標軸情報
１０  基準点群データ
１１  参照点群データ
１２  基準点情報
１３  注目点情報
１６  基準距離
１７  最近傍点情報
２０  位置・姿勢情報
４２  探索部
４３Ａ  ＣＰＵ
６１  左カメラ
６２  右カメラ
６３  本体部
６４  腕部
６５  手部
６６  作業台
６８  回転部
７１  対象物
ＤＬ１，ＤＬ２  通信回線
【図１】
2013045141.tif 000003
【図２】
2013045141.tif 000004
【図３】
2013045141.tif 000005
【図４】
2013045141.tif 000006
【図５】
2013045141.tif 000007
【図６】
2013045141.tif 000008
【図７】
2013045141.tif 000009
【図８】
2013045141.tif 000010
【図９】
2013045141.tif 000011
【図１０】
2013045141.tif 000012
【図１１】
2013045141.tif 000013
【図１２】
2013045141.tif 000014
【図１３】
2013045141.tif 000015
【図１４】
2013045141.tif 000016
【図１５】
2013045141.tif 000017
【図１６】
2013045141.tif 000018
【手続補正書】
【提出日】平成23年10月6日(2011.10.6)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】００１４
【補正方法】変更
【補正の内容】
【００１４】
第５の態様にかかるプログラムは、コンピュータが読み取り可能なプログラムであって、前記コンピュータが前記プログラムを読み取り、前記コンピュータのＣＰＵがメモリを用いて前記プログラムを実行することにより、前記コンピュータを、対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得部と、前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート部と、前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索部と、前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定部とを備え、前記探索部が、前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択部と、前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新部と、前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御部と、前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定部とを備えた画像処理装置として機能させることを特徴とする。
【手続補正２】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得部と、
前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート部と、
前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索部と、
前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定部と、
を備え、
前記探索部が、
前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択部と、
前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新部と、
前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御部と、
前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定部と、
を備えたことを特徴とする画像処理装置。
【請求項２】
請求項１に記載された画像処理装置であって、
前記基準点設定部が、
前記基準座標軸方向についての前記第１点群の並び順に前記基準点を逐次設定し、
前記探索部が、
逐次設定された前記基準点に関する前記探索処理のそれぞれにおいて、当該基準点と、当該基準点の直前に設定された基準点に対して探索された最近傍点との距離を前記基準距離の初期値として設定しつつ、前記探索処理を逐次行うことを特徴とする画像処理装置。
【請求項３】
請求項１または請求項２に記載された画像処理装置であって、
座標空間の各座標軸のうち前記参照点群データの派生元の点群データにおける座標値の分布幅が最も広い座標軸を前記基準座標軸として設定する座標軸設定部、
をさらに備えることを特徴とする画像処理装置。
【請求項４】
請求項１から請求項３の何れか１つの請求項に記載された画像処理装置であって、
前記対象物の三次元形状を測定可能な測定部、
をさらに備え、
前記取得部が、
前記測定部が前記対象物を測定して取得した測定情報に基づいて、前記第１測定点群データを取得することを特徴とする画像処理装置。
【請求項５】
コンピュータが読み取り可能なプログラムであって、
前記コンピュータが前記プログラムを読み取り、前記コンピュータのＣＰＵがメモリを用いて前記プログラムを実行することにより、前記コンピュータを、
対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得部と、
前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート部と、
前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索部と、
前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定部と、
を備え、
前記探索部が、
前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択部と、
前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新部と、
前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御部と、
前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定部と、
を備えた画像処理装置として機能させることを特徴とするプログラム。
【請求項６】
対象物上の各点の三次元座標を設計数値的にそれぞれ表現した第１モデル点群データと、当該対象物上の各点の３次元座標を実測に基づいてそれぞれ表現した第１測定点群データとを取得する取得工程と、
前記第１モデル点群データを予め設定された基準座標軸の座標値順にソートすることにより第２モデル点群データを取得するとともに、前記第１測定点群データを当該基準座標軸の座標値順にソートすることにより第２測定点群データを取得するソート工程と、
前記第２モデル点群データと前記第２測定点群データとのうち一方の基準点群データと他方の参照点群データとについて、前記基準点群データが表現する第１点群において予め設定された基準点に対する距離が最短となる最近傍点を、前記参照点群データが表現する第２点群において探索する探索処理を行なう探索工程と、
前記第１点群の各点を前記基準点としてそれぞれ設定する基準点設定工程と、
を有し、
前記探索工程が、
前記基準点に対する前記基準座標軸方向の距離が予め設定された基準距離よりも短い注目点を前記第２点群において選択する選択処理を行なう選択工程と、
前記基準点と前記注目点との点間距離が前記基準距離よりも短い場合には、当該点間距離を前記基準距離として設定することにより前記基準距離を更新する更新処理を行なう更新工程と、
前記第２点群において前記注目点として新たに選択可能な未選択点が無くなるまで、前記更新処理によって更新された前記基準距離が前記選択処理に反映されつつ前記選択処理と前記更新処理とがそれぞれ逐次実行されるように、前記選択部と前記更新部とを制御する探索制御工程と、
前記選択処理と前記更新処理との逐次実行処理が終了されたときの前記基準距離の設定に用いられた前記注目点を前記最近傍点として決定する決定工程と、
を有することを特徴とする画像処理方法。