JP5615055B2 - 情報処理装置及びその処理方法 - Google Patents

Description

本発明は、物体の３次元計測技術のオクルージョン判定に関する。

近年のロボット技術の発展とともに、工業製品の組み立てのようにこれまで人間が行っていた複雑なタスクをロボットが代わりに組み立てを行うようになってきている。ロボットが製品の組み立てを行うためには、対象となる部品の位置姿勢や３次元形状を計測する必要がある。

そこで対象物体に照射した光の反射光を解析することにより求めた画素毎の距離値の格納された距離画像から、物体の３次元形状を復元する方法や、物体の３次元形状モデルを利用して対象物体の位置姿勢を計測する方法が提案されている。これら方法では、複数の距離画像間の対応や距離画像から得られる計測点と形状モデルの表面との対応を探索し、その対応間の距離を最小化することにより、計測点群間の位置合わせや対象物体の位置姿勢の推定を行う。

対象物体と対象物体以外の物体とが混在する環境では、計測点と形状モデルの対応及び計測点群間の対応を探索する際に誤対応が発生しやすい。誤対応した対応点間の距離を最小化しても正しい幾何関係は得られないため、位置合わせに失敗する場合や計算が不安定になる場合が生じる。

非特許文献２のように、誤対応の影響を軽減するために、対応点間の距離に関する統計的な値に基づいて重み付けを行うＭ推定がよく用いられる。この手法では、対応間の距離が平均値に近い場合は重みを高く、遠い場合には重みを低く設定することにより、平均値から遠い対応点は対応の信頼度が低いと想定して位置合わせに与える影響を少なくしている。そのため、この手法は、外れ値のようなノイズによる影響を軽減するのには非常に有効である。

P. J. Besl and N. D. McKay, "A method for registration of 3-D shapes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.14, no.2, pp.239-256, 1992. 近藤, 宮本, 金子, 五十嵐, "Ｍ推定を導入したロバストＩＣＰ位置決め法", 電子情報通信学会技術研究報告. PRMU, パターン認識・メディア理解, vol.100, no.507, pp.21-26, 2001.

しかしながら、オクルージョンにより誤対応した対応点間の距離と他の正しい対応点間の距離との差が少ない場合、この手法では誤対応が発生し正しい対応を見分けることができないという課題がある。例えば非特許文献１で開示されている手法では、対象物体の位置及び姿勢の概略値に基づいて、計測点に最も近い形状モデルの点を探索する必要がある。このとき対象物体が遮蔽物体によりオクルージョンされている場合、遮蔽物体上の計測点をオクルージョンされて観測できない対象物体上の計測点だと判断し、形状モデルの点と誤って対応付けてしまう場合がある。特に遮蔽物体が薄い物体である場合やノイズや概略位置姿勢のズレが大きい場合、オクルージョンにより誤対応した対応点間の距離と他の正しい対応点間の距離との差が少なくなってしまう。このような場合にＭ推定ではオクルージョンによる誤対応の影響を軽減することが難しい。

本発明は以上の課題に鑑み、従来見分けることが難しかった、オクルージョンの発生による対象物に関する計測点の誤対応を減らすことで、計測点の対応付けを向上させる情報処理装置およびその処理方法を提供することを目的とする。

本発明の情報処理装置は、第１の位置に配置された対象物体から、複数の幾何特徴部分と該複数の幾何特徴部分における法線を取得する第１取得手段と、前記第１の位置とは異なる第２の位置に配置された前記対象物体の形状モデルから、前記対象物体の前記複数の幾何特徴部分に対応する部分の複数の法線を取得する第２取得手段と、前記対象物体と前記形状モデルとの対応する幾何特徴部分の組について、前記第１取得手段で取得された法線と、前記第２取得手段で取得された法線とに基づいて幾何特徴部分におけるオクルージョンの発生の有無を判定する判定手段とを備えることを特徴とする。

本発明の情報処理装置により、従来見分けることが難しかった、オクルージョンの発生による対象物に関する計測点の誤対応を減らすことで、計測点の対応付けを向上させることが可能となる。

法線の差によるオクルージョン判定の原理を説明する図。 情報処理装置1の構成を示す図。 オクルージョン判定の処理手順を示すフローチャート。 位置の差によるオクルージョン判定の原理を説明する図。 対象物体の位置姿勢推定の処理手順を示すフローチャート。 情報処理装置の構成を示す図。 対象物体の計測データ同士の位置合わせの処理手順を示すフローチャート。

＜実施形態１＞
実施形態１では、対象物体を計測して得た３次元点群に対し、対象物体の３次元形状モデルをフィッティングして位置及び姿勢を推定する際に、オクルージョン情報を取得する方法を適用する場合について説明する。まず本発明による対応点の組に対するオクルージョン判定の原理について図１を用いて説明する。図１（ａ）には、第１の位置で位置及び姿勢を計測した対象物体Ｏ１と（第１取得）、第２の位置での位置及び姿勢が予め設定された対象物体の形状モデルＭ１が示されている（第２取得）。オクルージョンが発生していない場合、図１(ａ)のように、対応点の組Ｋ１の法線の差d₁と対応点の組Ｋ２の法線の差d₂は、いずれも形状モデルＭ１と計測データＯ１間の姿勢差を表すので、同じ値になる。

それに対してオクルージョンが発生する場合は、図１(ｂ)のように、対応点の組Ｋ１'の法線の差d₁'と、オクルージョンにより誤対応した対応点の組Ｋ２'の法線の差d₂'は異なる値となる。図１(ｂ)では、形状モデルＭ１'の幾何特徴部分ｑ２'と遮蔽物体の計測データｐ２'がオクルージョンにより誤対応しており、ｐ２'の面の向きが本来対応付くべき対象物体Ｏ１'のものと異なるため法線の差ｄ１'とｄ２'が異なる値となる。

そこでオクルージョンしている領域は部分的であると仮定し、複数の対応点、すなわち対応する幾何特徴部分の組の法線の差を相互に比較し最も頻度の高い差の値を算出する。誤対応が部分的であれば、最も頻度の高い差の値（最頻値）は、誤対応が無い場合の計測データ間の姿勢差による法線の差に近似している。したがって、各対応する幾何特徴部分間の法線の差が最頻値に近ければオクルージョンしていないと判定する。

図２は、本実施例における情報処理装置１の構成を示している。同図に示すように、情報処理装置１は、幾何特徴計測手段１１０、幾何特徴取得手段１２０、オクルージョン情報取得手段１３０、位置姿勢算出手段１４０とを備える。以下、情報処理装置１を構成する各部について説明する。

幾何特徴計測手段１１０は、対象物体の複数の幾何特徴部分に対して３次元点の位置及びその位置における法線方向を計測する。カメラは、アクティブ式でレーザ光やスリット光、パターン光を対象に照射しその反射光反射光により３次元点を撮影し、三角測量により距離を計測する。しかしながら、距離センサはこの方法に限るものではなく、光の飛行時間を利用するＴｉｍｅ-ｏｆ-ｆｌｉｇｈｔ方式であっても良いし、またステレオカメラなどのパッシブ式のものであっても良い。

距離センサで計測できる情報が位置のみの場合、周囲の幾何特徴部分の位置を利用して計測データの法線方向を算出する。法線方向は、注目幾何特徴部分と近傍の幾何特徴部分の位置に対して主成分分析を行い、第３主成分を法線方向とすることで算出できる。あるいは、注目する幾何特徴部分及び近傍の幾何特徴部分の位置に対して面当てはめを行うことで法線方向を算出しても良い。ただし面の向きを表していれば、その表現方法は法線ベクトルに限る必要は無く、法線に直交する２つのベクトルであっても良い。

また、対象物体の幾何特徴部分は、３次元点に限るものではなく、幾何特徴部分の属性として位置及び面の向きを持っていれば良い。例えば、動画像からＳｔｒｕｃｔｕｒｅ-ｆｒｏｍ-Ｍｏｔｉｏｎで求めた特徴点群であっても良いし、計測点群に面当てはめを行って得た面であっても良い。さらに、対象物体の幾何特徴部分は記憶装置に保存しておいたものを取得しても良い。

幾何特徴取得手段１２０では、対象物体の形状モデルから、幾何特徴計測手段１１０で取得した複数の幾何特徴部分に対応する幾何特徴部分として面の位置及び法線方向を取得し、対応する幾何特徴部分の組を複数組出力する。本実施形態では形状モデルの幾何特徴部分として面を利用する。しかしながら形状モデルの幾何特徴部分は面に限るものではない。法線方向を持った３次元点であっても良く、位置及び面の向きの情報を持った幾何特徴部分であれば良い。

幾何特徴計測手段１１０で計測した幾何特徴部分に対応する形状モデルの幾何特徴部分の探索のために最近傍探索が用いられる。対象物体の概略位置姿勢に形状モデルを配置し、幾何特徴計測手段１１０で計測した幾何特徴部分と形状モデルの幾何特徴部分間との３次元空間での距離を算出し、両者の距離が最も近い幾何特徴部分同士を対応付ける。ただし対応付けの方向は逆になっても良く、形状モデルの幾何特徴部分に最も近い前記幾何特徴計測手段１１０で計測した幾何特徴部分を探索しても良い。また対応付け方法は、最近傍探索に限るものではない。例えば、幾何特徴計測手段１１０で用いたセンサのカメラモデルのパラメタである焦点距離や画角を用いてセンサの視点から見た２次元平面を作成し、投影画像上での対応付けを行っても良い。また、幾何特徴計測手段１１０で取得した幾何特徴部分と対象物体の概略位置姿勢に配置した形状モデルの幾何特徴部分を２次元平面に投影し、投影面上で最も近い幾何特徴部分同士を対応付けても良い。

オクルージョン情報取得手段１３０では、前記幾何特徴取得手段１２０で取得した計測点と形状モデルの幾何特徴部分との対応の組について、オクルージョンによる誤対応が発生しているかどうかを判定し、その情報を取得する。以下では、図３に示すオクルージョン判定のフローチャートを用いて、オクルージョン情報の取得方法について説明する。

Ｓ００１０およびＳ００２０において、オクルージョンが発生しておらず、対応が正しい場合について説明する。図１で示したように、対応している幾何特徴部分間の法線の差は計測データと形状モデル間の相対姿勢に相当し、理想的には全て同じ値になる。そこでオクルージョン領域は部分的であり少なくとも対応組の半分以上は正しい対応であると仮定し、対応点間の法線の差の最頻値が正しい対応の場合の法線の差に近似していると仮定する。よって、オクルージョン情報取得手段１３０は、全ての対応点の組を用いて対応点間の法線の差の最頻値を算出し、その最頻値を法線基準値として、各対応点の組の法線の差が法線基準値から第１閾値の範囲内である対応点をオクルージョンしていないと判定する。

法線の差は例えば以下の方法で求めることができる。ここでは法線の差を回転軸回転角表現のベクトルで表す。式（１）に示すように、対応している幾何特徴部分の法線同士の外積をとり回転軸ｖとする。また式（２）のように、法線の逆余弦を回転角ａとする。ここでn_p、n_qはそれぞれ形状モデルの幾何特徴部分の法線ベクトル、計測点の法線ベクトルを表す。

また、法線の差の最頻値は、回転軸回転角表現されたベクトルをオイラー角表現に変換し、各成分を独立に整列し、その最頻値を成分にもつオイラー角を再度回転軸に関する回転角表現に変換するものとする。

ただし、法線の差及びその最頻値の求め方はこれに限るものではなく、法線ベクトルの差とその最頻値であっても良い。さらに法線基準値は全ての対応点間の法線の差の最頻値に限るものではなく、計測データと形状モデル間の相対姿勢に相当する値であれば良い。例えば、全ての対応の組からではなく、ランダムに抽出した対応の組から法線の差の最頻値を算出しても良い。また、対応点間の法線の差が所定の範囲内にあるものだけを用いて平均値を算出し法線基準値として用いても良いし、法線の差のヒストグラムを作成し、そのピークを法線基準値として用いても良い。

Ｓ００３０のステップを説明する。ここでは、遮蔽物体の面の向きと対象物体の面の向きが偶然似た向きであった場合、法線の差だけではオクルージョンが判定できない。また計測データと形状モデルの姿勢がわずかにずれているため、単に対応点間の距離を比較してもオクルージョンしているかどうか見分けることは難しい。そこで法線の法線基準値を用いて姿勢のズレを補正し、形状モデルのわずかな位置姿勢の位置ズレと姿勢ズレを分離してオクルージョン情報を取得する。

図４は、位置の差によるオクルージョン判定を説明する図である。同図(ａ)に示すように、オクルージョンしていない場合でも、姿勢によって位置の差は変わるため、単に対応点間の位置の差ｌ_１及びｌ_２を比較しただけではオクルージョンの有無は判定できない。そこで図の（ｂ）のように、法線基準値を相対姿勢ｄｓとみなして形状モデルか計測データの幾何特徴部分のどちらか一方の位置（ｑ１、ｑ２）を回転させた位置（ｑ１'、ｑ２'）と他方の位置（ｐ１、ｐ２）との差を位置の差（ｌ_１'、ｌ_２'）とする。ただし法線の法線基準値から生成した相対姿勢の回転中心が分からないため、補正しなかった幾何特徴部分を基準とした奥行き値の差を位置の差として用いる。

具体的には、法線の差を回転軸に関する回転角表現に変換したものを用いて、形状モデルの幾何特徴部分の位置を補正する。法線の差の最頻値（法線基準値）の回転軸ｖ及び回転角ａを用いて計測点の位置または形状モデルの幾何特徴部分の位置のどちらか一方を回転させ、その３次元空間中の距離または奥行き値を他方と比較し、その差を位置の差とする。

任意の点ｕ を回転軸ｖ及び回転角ａで回転した点ｕ' は、式（３）で求めることができる。ただし、形状モデルと計測データ間の姿勢差を、法線基準値を用いてキャンセルできれば、その計算方法は上記の方法に限るものではない。例えば法線基準値を四元数や回転行列に変換して幾何特徴部分の位置を変換しても良い。

ここで、図３のステップのＳ００４０およびＳ００５０の動作について説明する。ここでは、Ｓ００１０及びＳ００２０の法線の差のオクルージョン判定と同様に、位置の差の最頻値を基準値２とし、位置の差が基準値２から所定の閾値２の範囲内である対応点をオクルージョンしていないと判定する。しかしながら、位置の差の基準値２は位置の差の最頻値に限るものではなく、位置の差の平均値であっても良いし、位置の差のヒストグラムのピークを基準値２として用いても良い。

またオクルージョン情報はオクルージョンの有無に限るものではない。法線の差によるオクルージョン情報として、対応点間の法線の差と法線基準値との差に応じた連続値としてオクルージョンの確からしさを出力しても良い。同様に位置の差によるオクルージョン情報として、位置の差と基準値２との差に応じた連続値としてオクルージョンの確からしさを出力してもよい。

この場合、オクルージョンの確からしさを示す値を位置合わせの際に重みとして利用しても良い。例えば式（４）から式（６）に示すような式で確からしさの指標を算出しても良い。ここで i は法線または位置を一意に識別する記号、r_i は確からしさの指標、ｇ_ｉ は法線または位置の差のベクトル、ｃ_ｉ は法線または位置の差の基準値、ｓ_ｉ は法線の差または位置の差の標準偏差、ｆ は重み関数を表す。重み関数ｆf は例えば式（６）に示すＴｕｋｅｙ関数やＨｕｂｅｒ関数など、誤差ｘが大きいデータには小さな重みを与え、誤差ｘが小さいデータには大きな重みを与える関数であれば、関数はどれでもかまわない。ここで、ｔは、定数を表す。

本実施形態では、Ｓ００２０での法線の差によるオクルージョン情報とＳ００５０による位置の差によるオクルージョン情報の積をオクルージョン情報取得手段１３０の出力とする。しかしながら、オクルージョン情報は前記法線の差によるオクルージョン情報及び前記位置の差によるオクルージョン情報のどちらか一方でも良いし、その両方を組み合わせた、例えば両方の和であっても良い。

図２の位置姿勢算出手段１４０では、オクルージョン情報取得手段１３０にてオクルージョンしていないと判定された形状モデルの幾何特徴部分と前記幾何特徴計測手段１１０で計測した幾何特徴部分の組を複数用いて、対象物体の位置及び姿勢を推定する。位置姿勢の算出方法としては、形状モデルの幾何特徴部分と幾何特徴計測手段１１０で計測した幾何特徴部分の３次元空間中での距離に基づく評価関数を最小化することによって行う。つまり、形状モデルの幾何特徴部分と前記幾何特徴計測手段１１０で計測した幾何特徴部分との間の差異に基づく評価関数に基づいて対象物体の位置及び姿勢を推定する手法であればいかなるものであっても本発明の本質を損なうものではない。また、オクルージョン情報取得手段１３０にて幾何特徴部分の組毎にオクルージョンの確からしさを示す数値を算出する場合、その数値を重みとして評価関数に掛け合わせたものを最小化しても良い。

図５は、本実施形態における位置及び姿勢の計測の処理手順を示すフローチャートである。この処理では、計測対象物体の位置及び姿勢の概略値を反復演算により繰り返し補正することにより位置及び姿勢を算出する。

Ｓ１０１０において、幾何特徴計測手段１１０は、対象物体の幾何特徴部分の計測データを計測する。Ｓ１０２０において、幾何特徴取得手段１２０は、Ｓ１０１０で計測した幾何特徴部分に対応する形状モデルの幾何特徴部分を取得し、幾何特徴部分の対応の組を出力する。

Ｓ１０３０において、オクルージョン情報取得手段１３０は、Ｓ１０２０で取得した計測データと形状モデルの幾何特徴部分の組のオクルージョン情報を取得する。本実施形態ではオクルージョン情報として各対応する幾何特徴部分の組毎にオクルージョンの有無を判定する。しかしながらＳ１０４０において、オクルージョン情報を重みとして利用する場合には、オクルージョンの確からしさを表す数値を算出しても良い。

Ｓ１０４０において、位置姿勢算出手段１４０は、Ｓ１０３０でオクルージョンしていないと判定された幾何特徴部分の組を用いて非線型最適化手法により対象物体の位置及び姿勢を更新する。この処理では、形状モデルの幾何特徴部分と対応付けた計測幾何特徴部分である３次元点の３次元空間中での距離をＧａｕｓｓ-Ｎｅｗｔｏｎ法により最小化する。そしてＳ１０５０で収束と判定されるまで、対象物体の位置及び姿勢の概略値を反復演算により繰り返し補正することにより位置及び姿勢を算出する。なお、計測対象物体の位置姿勢の算出方法はこれに限るものではない。Ｌｅｖｅｎｂｅｒｇ-Ｍａｒｑｕａｒｄｔ法や最急降下法などの最適化手法によって行っても良い。また、他の共役勾配法など非線型最適化計算手法を用いても良い。

Ｓ１０５０において、位置姿勢算出手段１４０は収束判定を行い、収束していれば終了し、そうでなければ対象物体の位置姿勢を概略位置姿勢として、Ｓ１０２０ に戻る。位置及び姿勢の更新前後の誤差ベクトルの二乗和の差がほぼゼロである場合に、収束したと判定される。ただし判定条件はこれに限るものではない。例えば位置及び姿勢の更新量がほぼゼロである場合に収束と判定しても良い。

＜実施形態２＞
実施形態１では、オクルージョン情報取得方法を利用して対象物体の計測データと形状モデルの位置合わせを行い、対象物体の位置姿勢を推定した。実施形態２では、第１の位置における第１の対象物体を計測して得た複数の３次元点群を、第２の位置にある同形状の第２の対象物体に移動し、対応する複数の３次元群に位置合わせする。そしてこの位置あわせを行うときに、本発明のオクージョンの判定に基づいた対応付けを適用する方法について説明する。

図６は、本実施形態における情報処理装置２の構成を示している。同図に示すように、情報処理装置２は、幾何特徴計測手段２１０、幾何特徴取得手段２２０、オクルージョン情報取得手段２３０および位置合わせ手段２４０を備える。以下、情報処理装置２を構成する各部について説明する。

幾何特徴計測手段２１０は、実施形態１の幾何特徴計測手段１１０と同様に対象物体の幾何特徴部分の位置及び法線方向（面の向き）を複数計測する。本実施形態では、３次元点の位置及び法線方向を計測することとするが、属性として位置及び面の向きを持っていればどんな幾何特徴部分であっても良い。また対象物体の幾何特徴部分の位置及び法線方向は記憶装置に保存しておいたものを取得しても良い。以降では、幾何特徴計測手段２１０で計測された幾何特徴部分群を計測データ１とする。

幾何特徴取得手段２２０では、実施形態１の幾何特徴計測手段１１０と同様に対象物体の幾何特徴部分の位置及び法線方向（面の向き）を複数の幾何特徴部分について計測し、計測データ１と対応付けた幾何特徴部分の組を出力する。本実施形態では３次元点の位置及び法線方向を計測することとするが、属性として位置及び面の向きを持っていればどんな幾何特徴部分であっても良い。対象物体の幾何特徴部分の位置及び法線方向は記憶装置に保存しておいたものを取得しても良いが、計測データ１とは異なる幾何特徴部分群を取得する。以降では、幾何特徴取得手段２２０で取得した幾何特徴部分群を計測データ２とする。幾何特徴部分群取得後、実施形態１の幾何特徴取得手段１２０で行った対応探索と同様に、計測データ１と計測データ２の間の対応探索を行い、対応点の組を出力する。

オクルージョン情報取得手段２３０では、実施形態１のオクルージョン情報取得手段１３０と同様に対応付けられている幾何特徴部分間のオクルージョン情報を取得する。オクルージョン情報取得手段１３０との違いは、判定対象が計測データと形状モデルの幾何特徴部分の組から、計測データ１及び計測データ２の計測データ同士の幾何特徴部分の組になることであるが、その処理の仕方は同様である。

位置合わせ手段２４０は、オクルージョン情報取得手段２３０にてオクルージョンしていないと判定された幾何特徴部分の組を用いて、計測データ１及び計測データ２の位置合わせを行う。位置合わせは、幾何特徴部分の組の３次元空間中での距離に基づく評価関数を最小化することによって行う。その他、計測データ１及び計測データ２との間の差異に基づく評価関数に基づいて対象物体の位置及び姿勢を推定する手法であればいかなるものであっても本発明の本質を損なうものではない。また、オクルージョン情報取得手段２３０にて幾何特徴部分の組毎にオクルージョンの確からしさを示す数値を算出する場合、その数値を重みとして評価関数に掛け合わせたものを最小化しても良い。

図７は本実施形態における複数の３次元点群同士を位置合わせの処理手順を示すフローチャートである。この処理では、計測データ１及び計測データ２の一方を基準データとし、他方の計測データの基準データに対する相対位置姿勢を反復演算により繰り返し補正することにより算出し、計測データ同士の位置合わせを行う。位置合わせされた計測データを１つの座標系に統合した３次元点群が対象物体の３次元形状を表す点群となる。

Ｓ２０００およびＳ２０１０において、幾何特徴計測手段２１０及び幾何特徴取得手段２２０は、それぞれ対象物体の幾何特徴部分の計測データを計測し取得する。計測データには計測した対象物体の幾何特徴部分（３次元点）の位置及びその位置における法線方向及び計測視点の概略位置姿勢が含まれる。本実施形態では、計測視点の概略位置姿勢としてＧＰＳ及び慣性センサを用いて計測した値が与えられる。しかしながら概略位置姿勢は計測データ間の概略の相対位置姿勢が分かれば良く、これを実現できるセンサならどのセンサを用いて計測しても良い。また概略位置姿勢を手動で与えても良いし、計測データ間の対応を手動で与えて算出しても良い。

Ｓ２０２０において、幾何特徴取得手段２２０は、Ｓ２０００およびＳ２０１０で計測した計測データ間の幾何特徴部分群の対応付けを行う。本実施形態では最近傍探索を用い、概略位置姿勢に基づき一方の計測データの幾何特徴部分に最も近い他方の計測データの幾何特徴部分を対応付ける。ただし対応探索方法はこれに限るものではなく、従来技術による画像上に投影することにより幾何特徴部分を対応付ける方法を用いても良い。

Ｓ２０３０において、オクルージョン情報取得手段２３０は、Ｓ２０２０で対応付けた計測データ間の幾何特徴部分の組のオクルージョン情報を取得する。本実施形態ではオクルージョン情報として各対応する組毎にオクルージョンの有無を判定する。しかしながらＳ２０４０でオクルージョン情報を重みとして利用する場合には、オクルージョンの確からしさを表す数値を算出しても良い。

Ｓ２０４０において、位置合わせ手段２４０は、幾何特徴部分の組を用いて非線型最適化手法により計測データを計測した際の計測位置及び姿勢を更新する。この処理では、対応付けた計測点群の組の３次元空間中での距離をＧａｕｓｓ -Ｎｅｗｔｏｎ法により最小化する。そしてＳ２０５０で収束と判定されるまで、各計測データの計測位置姿勢を反復演算により繰り返し補正する。なお、計測位置姿勢の算出方法はこれに限るものではない。従来のＬｅｖｅｎｂｅｒｇ -Ｍａｒｑｕａｒｄｔ法や最急降下法などの最適化手法や共役勾配法など、他の非線型最適化計算手法を用いてもかまわない。

Ｓ２０５０において 位置合わせ手段２４０は、収束判定を行い、収束していれば終了し、そうでなければ計測位置姿勢を更新してＳ１０２０ に戻る。ここでは、計測位置姿勢の更新前後の誤差ベクトルの二乗和の差がほぼゼロである場合に収束したと判定される。ただし判定条件はこれに限るものではない。例えば計測位置姿勢の更新量がほぼゼロである場合に収束と判定しても良い。収束後、推定した計測位置姿勢に基づいて計測データを１つの座標系に統合し、３次元形状を表す幾何特徴部分の集合として出力する。

本実施形態では２つの３次元点群同士を位置合わせする場合について説明した。しかしながら計測データ群は２つに限るものではない。計測データ群は３つ以上であっても良く、複数の計測データ群を扱う場合は任意の２つの計測データ群に対して本実施形態を適用すれば良い。

以上述べたように、実施形態１および２に記載された発明の構成をとることで、本発明は、物体の３次元形状計測や、物体認識、ロボットの自己位置推定、ロボットと物体との間の相対的な位置及び姿勢の推定に利用することができる。

（その他の実施例）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。

Claims (12)

1. 第１の位置に配置された対象物体から、複数の幾何特徴部分と該複数の幾何特徴部分における法線を取得する第１取得手段と、
   前記第１の位置とは異なる第２の位置に配置された前記対象物体の形状モデルから、前記対象物体の前記複数の幾何特徴部分に対応する部分の複数の法線を取得する第２取得手段と、
   前記対象物体と前記形状モデルとの対応する幾何特徴部分の組について、前記第１取得手段で取得された法線と、前記第２取得手段で取得された法線とに基づいて幾何特徴部分におけるオクルージョンの発生の有無を判定する判定手段と
   を備えることを特徴とする情報処理装置。
2. 前記第１取得手段で取得された法線と、前記第２取得手段で取得された法線との方向の差を算出する算出手段を更に備え、
   前記判定手段は、前記算出手段で算出した方向の差に基づいて、幾何特徴部分におけるオクルージョンの発生の有無を判定することを特徴とする請求項１に記載の情報処理装置。
3. 前記判定手段は、前記算出した方向の差のヒストグラムを作成し、該ヒストグラムのピークを法線基準値とし、算出した方向の差と該法線基準値との差が第１閾値より小さい前記幾何特徴部分には、オクルージョンの発生がないと判定することを特徴とする請求項２に記載の情報処理装置。
4. 前記判定手段でオクルージョンの発生がないと判定された幾何特徴部分の組の幾何特徴部分の間の距離が最小となるように前記形状モデルをフィッティングすることで前記対象物体の位置、姿勢を算出する位置姿勢算出手段、を更に備えることを特徴とする請求項１乃至３のいずれか１項に記載の情報処理装置。
5. 前記判定手段は、更に、前記幾何特徴部分の組における幾何特徴部分間の距離に基づいて、それぞれの幾何特徴部分におけるオクルージョンの発生の有無を判定することを特徴とする請求項４に記載の情報処理装置。
6. 前記幾何特徴部分は、予め定められた位置又は面の向きを有する３次元形状であることを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
7. 第１の対象物体が第１の位置にあるとき、該第１の対象物体から複数の幾何特徴部分と、該複数の幾何特徴部分における法線を取得する第１取得手段と、
   前記第１の位置とは異なる第２の位置にある、前記第１の対象物体の同形状の第２の対象物体から、前記第１の対象物体の前記複数の幾何特徴部分に対応する部分の複数の法線を取得する第２取得手段と、
   前記第１の位置における前記第１の対象物体と前記第２の位置における前記第２の対象物体との対応する幾何特徴部分の組について、前記第１取得手段で取得された法線と、前記第２取得手段で取得された法線とに基づいて、幾何特徴部分におけるオクルージョンの発生の有無を判定する判定手段と
   を備えることを特徴とする情報処理装置。
8. 前記第１取得手段で取得された法線と、前記第２取得手段で取得された法線との方向の差を算出する算出手段、を更に備え、
   前記判定手段は、前記算出手段で算出した方向の差に基づいて、幾何特徴部分におけるオクルージョンの発生の有無を判定することを特徴とする請求項７に記載の情報処理装置。
9. 前記判定手段でオクルージョンの発生がないと判定された幾何特徴部分の各組の幾何特徴部分間の距離が最小となるように、前記第２の対象物体を移動することにより、前記第２の対象物体を前記第１の位置に合わせる位置合わせ手段、を更に備えることを特徴とする請求項７または８に記載の情報処理装置。
10. 第１取得手段が、第１の位置に配置された対象物体から、複数の幾何特徴部分と該複数の幾何特徴部分における法線を取得する第１取得工程と、
    第２取得手段が、前記第１の位置とは異なる第２の位置に配置された前記対象物体の形状モデルから、前記対象物体の前記複数の幾何特徴部分に対応する部分の複数の法線を取得する第２取得工程と、
    判定手段が、前記対象物体と前記形状モデルとの対応する幾何特徴部分の組について、前記第１取得工程で取得された法線と、前記第２取得手段で取得された法線とに基づいて幾何特徴部分におけるオクルージョンの発生の有無を判定する判定工程と
    を備えることを特徴とする情報処理装置の処理方法。
11. 第１取得手段が、第１の対象物体が第１の位置にあるとき、該第１の対象物体から複数の幾何特徴部分と、該複数の幾何特徴部分における法線を取得する第１取得工程と、
    第２取得手段が、前記第１の位置とは異なる第２の位置にある、前記第１の対象物体の同形状の第２の対象物体から、前記第１の対象物体の前記複数の幾何特徴部分に対応する部分の複数の法線を取得する第２取得工程と、
    判定手段が、前記第１の位置における前記第１の対象物体と前記第２の位置における前記第２の対象物体との対応する幾何特徴部分の組について、前記第１取得工程で取得された法線と、前記第２取得工程で取得された法線とに基づいて、幾何特徴部分におけるオクルージョンの発生の有無を判定する判定工程と
    を備えることを特徴とする情報処理装置の処理方法。
12. 請求項１０または１１に記載の情報処理装置の処理方法の各工程をコンピュータに実行させるためのプログラム。

Images