JP2013217893A - モデル生成装置、位置姿勢推定装置、情報処理装置、モデル生成方法、位置姿勢推定方法、情報処理方法 - Google Patents

Abstract

【課題】 位置姿勢の推定処理と同時に幾何特徴のグループ分け処理を行う場合、幾何特徴の数やグループ分け方法によっては、幾何特徴のグループ分け処理の処理コストが高くなり、位置姿勢推定処理の高速性が損なわれる。
【解決手段】 対象物体の３次元形状モデルを入力する。前記対象物体の３次元形状モデルに対して、前記対象物体と該対象物体を撮像する撮像装置との相対的な位置姿勢を仮想的に設定できるように、少なくとも１つの前記撮像装置を配置するための位置姿勢を設定する。設定された前記撮像装置の位置姿勢を選択する。選択された前記撮像装置の位置姿勢において得られるべき画像と前記３次元形状モデルの幾何特徴の関係に基づいて、幾何特徴のグループ分けを行う。
【選択図】 図１

Description

本発明は、物体の位置及び姿勢を計測する技術に関する。

近年、これまで人間が行なっていた工業製品の組立のような複雑なタスクをロボットが代わりに行うようになりつつある。ロボットによる工業製品の組立では、ハンドなどのエンドエフェクタによって部品を把持するために、部品間の相対的な位置及び姿勢を高精度に計測する必要がある。ここで、位置及び姿勢を計測する方法としては、カメラが撮影する濃淡画像上から検出される特徴や距離センサから得られる距離画像に３次元形状モデルをあてはめるモデルフィッティングによる計測が一般的である。その中でも、３次元形状モデル中の特徴（３次元点、線および面など、以下、幾何特徴）を効率的に選択することで、高速に位置及び姿勢の推定を行う手法が以下に示す非特許文献１で開示されている。この非特許文献１で開示される手法は、まず３次元形状モデル中の幾何特徴を、推定する位置及び姿勢を表すパラメータへの影響が似通った幾何特徴群（例えば画像上平行方向にだけ影響のある幾何特徴群など）にグループ分けする。そして、それぞれのグループから選択した少数の幾何特徴に基づいて位置及び姿勢の推定を行う。この方法では、幾何特徴それぞれに関して、位置姿勢の６自由度各パラメータが微小変化したときの幾何特徴の変化量（以後、ヤコビアン）を計算することで、幾何特徴が位置及び姿勢を表す６自由度のどのパラメータに影響を与え易いかを算出している。

Christopher Kemp、 Tom Drummond、 "Dynamic Measurement Clustering to Aid Real Time Tracking、" iccv、 vol。 2、 pp。1500-1507、 Tenth IEEE International Conference on Computer Vision (ICCV'05) Volume 2、 2005。

しかしながら、非特許文献１にかかる手法では、以下の問題がある。すなわち、モデルフィッティング処理の中で幾何特徴のグループ分け処理を行っているため、幾何特徴の数やグループ分け方法によっては、幾何特徴のグループ分け処理の処理コストが高くなり、モデルフィッティング処理の高速性が損なわれる場合がある。

本発明は、上記課題に鑑みてなされたものであり、物体の位置及び姿勢を計測する際に、ヤコビアンに基づく幾何特徴のグループ分け処理の高速性が損なわれる可能性を低減し、効率的なモデルフィッティングを行えるようにすることを目的とする。

本発明の目的を達成するために、例えば、本発明のモデル作成装置は、対象物体の３次元形状モデルを入力するモデル入力手段と、前記対象物体の３次元形状モデルに対して、前記対象物体と該対象物体を撮像する撮像装置との相対的な位置姿勢を仮想的に設定できるように、少なくとも１つの前記撮像装置を配置するための位置姿勢を設定する設定手段と、前記設定手段により設定された前記撮像装置の位置姿勢を選択する選択手段と、前記選択手段により選択された前記撮像装置の位置姿勢において得られるべき画像と前記３次元形状モデルの幾何特徴の関係に基づいて、幾何特徴のグループ分けを行うグループ分け手段とを備えることを特徴とする。

本発明によれば、物体の位置及び姿勢を計測する際に、ヤコビアンに基づく幾何特徴のグループ分け処理の高速性が損なわれる可能性を低減し、効率的なモデルフィッティングを行えるようにすることが可能になる。

本発明の実施形態における情報処理装置の構成を示す図。 本発明の第１の実施形態における3次元形状モデルの構成要素を説明する図。 本発明の第１の実施形態におけるモデル生成方法の処理手順を示すフローチャート。 本発明の第１の実施形態における離散的観察条件の生成を説明する図。 本発明の第１の実施形態における幾何特徴と画像との対応付け方法を説明する図。 本発明の第１の実施形態における幾何特徴のグループ分け方法の処理手順を示すフローチャート。 本発明の第１の実施形態における位置姿勢推定方法の処理手順を示すフローチャート。 本発明の第２の実施形態における情報処理装置の構成を示す図。 対応付けを説明する図。 グループ分類を説明する図。 情報処理装置３００の機能構成例を説明する図。 モデル情報の構成例を説明する図。 情報処理装置３００が行う処理のフローチャート。 ロボットシステムの構成例を示す図。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。

［第１の実施形態］
本実施形態では、本発明の情報処理装置を用いた物体の位置及び姿勢（以下、位置姿勢）の推定のための位置姿勢推定用モデルの生成方法、および、生成した位置姿勢推定用モデルを用いた物体位置姿勢の推定手法に適用した場合について説明する。

なお、以下の説明における「物体の位置姿勢」とは、物体と当該物体を撮影する撮像装置との位置および姿勢の関係のことを意味する。

本実施形態で対象とする位置姿勢推定方法では、まず初期値として与えられる概略的な位置姿勢（以下、概略位置姿勢）に基づいて、３次元形状モデル中の３次元幾何特徴と２次元画像中の画像特徴および距離画像中の距離点とを対応付ける。そして、各対応の距離の和を最小化するように位置姿勢の補正を繰り返すことで、物体の位置姿勢を算出する。

ここで、位置姿勢の推定処理に用いる幾何特徴は、位置姿勢を表す６自由度パラメータ（位置３自由度、姿勢３自由度）中のどのパラメータに寄与し易いかがそれぞれ異なる。そのため、位置姿勢の推定処理に用いた幾何処理が位置姿勢を表す６自由度パラメータの一部にしか寄与しなければ、位置姿勢６自由度の推定に失敗（一部のパラメータの推定しかできない）する。若しくは、正しく推定するために多数の幾何特徴が必要となるなどの問題が発生する。

すなわち、位置姿勢の６自由度を効率的に推定するためには、位置姿勢の６自由度パラメータへの影響が似通らない幾何特徴をバランスよく選択して利用することが重要になる。

そこで本実施形態では、あらかじめ位置姿勢の推定パラメータへの影響が類似する幾何特徴を幾何特徴群としてグループ化しておき、位置姿勢パラメータへの影響が異なる幾何特徴群を選択的に利用して位置姿勢の推定処理を行うことで効率的な推定処理を行う。

しかしながら、幾何特徴の位置姿勢パラメータへの影響は、物体を観察する観察条件（物体と物体を観察する撮像装置との相対的な位置及び姿勢や、撮像装置の内部パラメータ等）によって異なる。そのため、観察条件が未定である事前処理の段階で計算することが出来ない。

そこで、本実施形態では、物体の位置姿勢を推定するための事前処理として、３次元形状モデルに対し、あらかじめ離散的に設定した様々な観察条件において、幾何特徴のグループ分け処理を行い、観察条件ごとに幾何特徴のグループ分け結果を保存する。

そして、位置姿勢推定時には、事前処理した中で、そのときの観察条件にもっとも近い観察条件で処理された幾何特徴のグループ分け結果を利用して位置姿勢推定を行う。

以下の説明では、先ず、位置姿勢推定処理の事前処理として、モデル作成処理について述べる。その後、モデル作成処理により作成された後述の位置姿勢算出用モデルを利用して物体の位置姿勢を推定する処理について述べる。

図１は、本実施形態において事前処理として視点別位置姿勢推定用モデル作成を行うモデル作成装置２と、位置姿勢推定用モデルを用いて位置姿勢算出を行う位置姿勢推定装置１の構成を示している。

図１に示すように、モデル作成装置２は、３次元形状モデル入力部１１０、位置姿勢設定部１２０、位置姿勢選択部１３０、幾何特徴グループ分け部１４０と、視点別モデル保存部１５０から構成されている。さらに、位置姿勢推定装置１は、モデル作成装置２、画像入力部１６０、幾何特徴選択部１７０および、位置姿勢推定部１８０から構成されている。また、図１に示す構成は、本発明の情報処理装置の適用例となる構成である。

次に、モデル作成装置２を構成する各部について説明する。

３次元形状モデル入力部１１０は、物体の形状を表す３次元形状モデル１０（以下、３次元形状モデル）を入力する。図２(a)、(b)は、本実施形態における、３次元形状モデルの一例を示す図であり、図２(c)は物体の形状を表すポリゴンモデルである。

図２に示すように、３次元形状モデルは、３次元平面情報（以後、面幾何特徴）と３次元線分情報（以後、線幾何特徴）によって構成される。ここで、面幾何特徴は、３次元位置と３次元法線方向からなる物体表面上の局所的な３次元平面情報であり、線幾何特徴は、３次元位置と３次元線分方向からなる物体輪郭上の局所的な３次元線分情報である。

３次元形状モデルの例としては、ＣＡＤモデルの他にも、対象としている物体を撮像したデータでもよい。なお、本実施形態において、単に幾何特徴と称した場合は、面幾何特徴と線幾何特徴の両方を指す。

面幾何特徴は、面幾何特徴の３次元座標と距離画像上の距離点群との対応する距離を計量するための幾何特徴であり、線幾何特徴は、線幾何特徴の投影座標と濃淡画像上のエッジとの対応する距離を計量するための幾何特徴である。３次元形状モデルは、３次元形状モデル入力部１１０に保存され、幾何特徴グループ分け部１４０に入力される。

位置姿勢設定部１２０は、３次元形状モデルに対し、複数の異なる観察条件を設定する。ここで、本実施形態では、３次元形状モデルを観察する仮想的な撮像装置の位置及び姿勢を観察条件として、離散的に様々な方向から観察するように網羅的に生成する。撮像装置の位置姿勢設定の具体的な処理方法は後述する。なお、以下では、仮想的に配置された撮像装置を仮想撮像装置という。

位置姿勢選択部１３０は、位置姿勢設定部１２０で設定された３次元形状モデルに対しての複数の観察条件の中から１つを選択する。ここで、本実施形態では、３次元形状モデルに対しての複数の仮想撮像装置の位置姿勢の中から、１つを選択する。具体的な処理に関しては後述する。

幾何特徴グループ分け部１４０では、位置姿勢設定部１２０において設定された複数の観察条件のうち、位置姿勢選択部１３０によって選択された観察条件ごとに、幾何特徴のヤコビアンの類似性に基づいて、幾何特徴のグループ分けを行う。ここで、本実施形態では、グループ分けとして、幾何特徴の位置姿勢パラメータへの影響の類似度に基づくクラスタリングを行う。具体的な処理の詳細については後述する。

視点別モデル保存部１５０は、幾何特徴グループ分け部１４０によりグループ分けされた幾何特徴情報をそのときの観察条件（グループ分けに用いた３次元形状モデルと仮想撮像装置との相対位置姿勢情報）と共に視点別に位置姿勢推定用モデルとして保存する。位置姿勢設定部１２０が設定する全ての観察条件の視点別位置姿勢推定用モデルを１つにまとめることで、位置姿勢推定用モデルを作成する。

次に、位置姿勢推定装置１を構成する各部について説明する。

２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラである。撮影される２次元画像は濃淡画像であってもよいしカラー画像であってもよい。本実施形態では２次元画像撮像装置２０は濃淡画像を撮影して出力する。

２次元画像撮像装置２０が撮影する画像は、画像入力部１６０を介して位置姿勢推定装置１に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照しても良い。または、[R. Y. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf cameras and lenses,” IEEE Journal of Robotics and Automation, vol. RA-3, no. 4, 1987.]において開示される方法によって事前にキャリブレーションしておいても良い。

距離画像撮像装置３０は、計測対象である物体表面上の点の３次元情報を計測する。距離画像撮像装置３０として距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。

本実施形態では、距離センサとしては、波長の異なる色IDを付与したマルチスリットラインを対象に照射し、その反射光をカメラで撮像して三角測量によって距離計測を行うワンショットアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するTime-of-flight方式であってもよい。

また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

距離画像撮像装置３０が計測した距離画像は、画像入力部１６０を介して位置姿勢推定装置１に入力される。また、距離画像撮像装置３０と２次元画像撮像装置２０の光軸は一致しており、２次元画像撮像装置２０が出力する濃淡画像の各画素と、距離画像撮像装置３０が出力する距離画像の各画素の対応は既知であるとする。

しかしながら、本発明の適用は、濃淡画像と距離画像が同一の視点である場合に限るものではない。例えば、濃淡画像を撮像する撮像装置と距離画像を撮像する撮像装置が別の位置姿勢にあり、濃淡画像と距離画像をそれぞれ別の視点から撮像しても良い。この場合は、撮像装置間の相対的な位置姿勢は既知であるとし、距離画像中の３次元点群を濃淡画像に投影することにより、濃淡画像と距離画像の対応を取る。同一の物体を撮像する撮像装置間の相対的な位置姿勢が既知であり、その画像間の対応が計算できる限り、撮像装置の位置関係に特に制限はない。

画像入力部１６０は、２次元画像撮像装置２０によって撮像された２次元画像および、距離画像撮像装置３０によって撮像された距離画像を位置姿勢推定装置１に入力する。画像入力部１６０は、撮像装置の出力がNTSCなどのアナログ出力であれば、アナログビデオキャプチャーボードによって実現される。また、撮像装置の出力がIEEE1394などのデジタル出力であれば、例えばIEEE1394インタフェースボードによって実現される。また、画像入力部１６０は、予め不図示の記憶装置に記憶してある静止画像や動画像のデジタルデータを読み出すことにより画像を取得するものであってもよい。

幾何特徴選択部１７０は、幾何特徴グループ分け部１４０によってグループ分けされ、視点別モデル保存部１５０に保存された幾何特徴の中から、位置姿勢算出に用いる幾何特徴を選出する。具体的な処理は後述する。

位置姿勢推定部１８０では、２次元画像撮像装置２０及び距離画像撮像装置３０により撮像された濃淡画像及び距離画像に基づき、幾何特徴選択部１７０によって選択された幾何特徴との対応を計算する。それにより、対象物体の位置及び姿勢を算出する。具体的な処理の詳細については後述する。

以上が、位置姿勢推定装置１の構成の一例についての説明である。なお、位置姿勢推定装置１には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御部、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶部が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力部、ネットワークカード等の通信部等も具備されていても良い。なお、これら各構成部は、バス等により接続され、主制御部が記憶部に記憶されたプログラムを実行することで制御される。

次に、本実施形態における位置姿勢推定用モデル生成および位置姿勢推定の処理手順について説明する。図３は、本実施形態における位置姿勢推定用モデル生成方法の処理手順を示すフローチャートである。
（ステップＳ１０００）
ステップＳ１０００において、３次元形状モデル入力部１１０は、対象物体の３次元形状モデルを入力する。
（ステップＳ１１００）
ステップＳ１１００において、位置姿勢設定部１２０では、対象物体の３次元形状モデルの観察条件を設定する。本実施形態では、観察条件として、３次元形状モデルを観察する仮想的な撮像装置の位置姿勢設定を行う。

網羅的な観察条件の生成には、例えば、図４に示すような、物体を覆うGeodesic Sphereを用いる。具体的には、Geodesic Sphereの各頂点を仮想撮像装置５０１の位置、そこから３次元形状モデル５０２を眺める方向を仮想撮像装置５０１の視点として基準位置姿勢を設定する。設定された位置姿勢から3次元形状モデルを撮像したときに、得られるべき画像は、仮想撮像装置の撮像面に相当する面へ3次元形状モデルを写像した像のことをいう。このとき、仮想撮像装置５０１のロール方向については任意の値に設定してよい。なお、Geodesic domeにより近似される球の半径は、仮想撮像装置５０１と対象物体の３次元形状モデル５０２との距離に相当する。

従って、位置姿勢推定に利用する画像に関して、撮像距離が予め想定されているのであれば、その距離をGeodesic Sphereの半径と同一にするのが望ましい。あるいは、段階的に大きさの異なるGeodesic Sphereを用意し、それぞれのGeodesic Sphereの各頂点を基準位置姿勢として設定してもよい。但し、基準位置姿勢の設定方法はこれに限るものではない。

例えば、対象物体の３次元形状モデルを覆う球を仮定して地球儀のように緯度経度を割り当て、緯度方向、経度方向に等間隔に視点を取ってもよい。その他、実際の位置姿勢推定における、仮想撮像装置と物体との相対位置姿勢を模擬できるものであれば、いかなる方法であっても構わない。対象物体の３次元形状モデルの観測条件として、Geodesic Sphereの各頂点に基づいた仮想撮像装置の位置および姿勢の生成が終了すれば、ステップＳ１１００の処理を終了し、ステップＳ１２００に進む。
（ステップＳ１２００）
次に、ステップＳ１２００において、位置姿勢選択部１３０は、ステップＳ１１００で設定された複数の位置姿勢条件の中から位置姿勢選択を行う。具体的には、未処理の位置姿勢を１つ選択して、３次元形状モデルを観察する仮想撮像装置の視点として設定する。
（ステップＳ１３００）
次に、ステップＳ１３００では、幾何特徴グループ分け部１４０は、幾何特徴間の位置姿勢パラメータへの影響の類似性を計算するために、ステップＳ１２００で設定された仮想撮像装置の位置姿勢を用いて、幾何特徴のヤコビアンを求める。ここで、幾何特徴のヤコビアンとは、撮像装置の位置姿勢６自由度のパラメータが微小変化したときに、着目する幾何特徴と画像特徴との対応間距離が変化する度合いを表す値である。

具体的には、線幾何特徴については画像上での点と直線の距離、面幾何特徴については３次元空間中での点と平面の距離を位置及び姿勢の関数としたときの位置及び姿勢の各要素に関する一次の偏微分係数を要素とする行列である。

幾何特徴のヤコビアンをみることで、その幾何特徴が位置姿勢６自由度のどのパラメータに影響を与え易いかがわかる。なお、本実施形態では、ヤコビアンを用いているが、これにかぎられるものではなく、幾何特徴の位置姿勢パラメータへの影響を表す指標であればよい。以下、面幾何特徴および線幾何特徴のヤコビアンの導出方法について、詳細を説明する。

まず、線幾何特徴のヤコビアンの導出について説明する。図５(a)は、線幾何特徴と濃淡画像上の画像特徴との直線−点間距離の計算を説明する図である。線幾何特徴を仮想撮像装置の位置及び姿勢sに基づいて、画像上に投影した位置を(u,v)、その法線方向を(n_u,n_v)（単位ベクトル）、線幾何特徴に対応する画像特徴の座標を(u’,v’)と表す。そのときの対応間の符号付距離err_2Dは以下の式より算出できる。

ここで、仮想撮像装置の位置及び姿勢sの自由度は6自由度である。すなわちsは6次元ベクトルであり、計測対象物体の位置を表す3つの要素（s1, s2, s3）と、姿勢を表す3つの要素（s4, s5, s6）からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す3次元ベクトルなどによって表現される。この仮想撮像装置の位置及び姿勢ｓの各パラメータで、対応間距離err_2Dを偏微分することによって、以下の線特徴のヤコビアン行列を算出する。

次に、面幾何特徴のヤコビアンの導出を説明する。図５(b)は、面幾何特徴と対応点(距離画像中の３次元点)との面−点間距離の計算を説明する図である。面幾何特徴のカメラ座標系における３次元座標を(x, y, z)、法線方向を(n_x, n_y, n_z)（単位ベクトル）、面幾何特徴点に対応する距離画像の3次元点の座標を(x’,y’,z’)とする。そのときの、３次元空間での符号付距離err_3Dは以下の式より算出できる。

そして、線幾何特徴のヤコビアンと同様に、仮想撮像装置の位置及び姿勢ｓの各パラメータで、対応間距離err_3Dを偏微分することによって、面特徴のヤコビアン行列は以下のように算出できる。

全ての線特徴・面特徴に対して、以上のヤコビアンの計算を終えたら、ステップＳ１３００の処理を終了し、ステップＳ１４００に進む
（ステップＳ１４００）
ステップＳ１４００では、幾何特徴グループ分け部１４０は、ステップＳ１３００で算出した、線幾何特徴および面幾何特徴のヤコビアンに基づき、位置姿勢６自由度の推定への影響が類似する特徴同士をグループ分けする。

本実施形態では、ヤコビアンの類似度に基づいて、例えばウォード法による階層型クラスタリングを行うことで、幾何特徴のグループ分けを行う。なお、線幾何特徴のヤコビアンと、面幾何特徴のヤコビアンは、計量する対応間距離の尺度が異なるため、両者をそれぞれ区別し、独立にグループ分けを行う。

以下、図６のフローチャートを用いて、ステップＳ１４００における線幾何特徴と面幾何特徴のグループ分けについて、具体的な処理を詳述する。

（ステップＳ１４１０）
まず、ステップ１４１０では、初期のクラスタとして、幾何特徴を１つのみ含むクラスタ群を生成する。すなわち全幾何特徴数がN個の場合、Ｎ個のクラスタが生成される。そして、生成したクラスタを、最下位階層のクラスタとして保存する。
（ステップＳ１４２０）
次に、各クラスタ間の全組み合わせに対して、それぞれクラスタ間のヤコビアンの類似度を算出する。ウォード法により算出される値は、類似するほど小さくなるため、ここでは、正確には非類似度が計算される。n_A個の構成要素を持つあるクラスタ{A}のi番目(i=1…n_A)の構成要素となる幾何特徴のヤコビアンをJ_ki ^A（k=1…6）とする。また、n_B個の構成要素を持つあるクラスタ{B}のj番目(j=1…n_B)の構成要素となる幾何特徴のヤコビアンJ_kj ^B（k=1…6）とする。そのときのクラスタ{A}とクラスタ{B}との間のクラスタ間類似度は、以下の式より算出される。

各クラスタ間の全ての組み合わせに対して、上記クラスタ間の非類似度ΔＳ_ＡＢを算出したら、ステップＳ１４２０の処理を終了し、ステップＳ１４３０に進む。
（ステップＳ１４３０）
次に、ステップＳ１４２０で計算された類似度を参照して、もっとも非類似度ΔＳ_ＡＢが低い組み合わせのクラスタを融合して、１つのクラスタをつくる。そして、融合したクラスタを、融合前のクラスタよりも一つ上の階層のクラスタとして保存する。
（ステップＳ１４４０）
ステップＳ１４３０の結果、全クラスタ数が1になっていれば終了し、そうでなければ、ステップＳ１４２０に戻り、全クラスタ数が１になるまで、処理を繰り返す。

以上に示す処理により、デンドログラムと呼ばれる樹状の分類構造に幾何特徴をグループ分けすることができる。デンドログラムは、枝の先端に近いところ(下位階層)で切断すれば、少数の構成単位からなる多数のクラスタを得られ、枝の根元に近いところ(上位階層)で切断すれば、多数の構成単位からなる少数のクラスタが得られる。

すなわち、デンドログラムにより構成されたクラスタは、小分類−中分類−大分類という階層的構造を備えているため、任意の階層で切断することによって、任意数のクラスタを得ることが可能になる。

以上の処理を、面幾何特徴および線幾何特徴に対してそれぞれ行い、全ての面幾何特徴のグループ分け、および、全ての線幾何特徴のグループ分けが終了したら、ステップＳ１４００の処理を終了し、ステップＳ１５００に進む。
（ステップＳ１５００）
次に、ステップＳ１５００において視点別モデル保存部１５０は、ステップＳ１４００において得られた幾何特徴のグループ分け結果と、ステップＳ１２００で選択した仮想撮像装置の位置及び姿勢と、を組にして、視点別位置姿勢推定用モデルとして保存する。
（ステップＳ１６００）
次に、ステップＳ１６００において、モデル作成装置２は、ステップＳ１２００からＳ１５００までの処理を、ステップＳ１１００で生成した全ての仮想撮像装置の位置姿勢において実行したか否かを判定する。全位置姿勢について処理が実行されていない場合、処理はステップＳ１２００に戻り、次の位置姿勢について処理が繰り返される。

一方、全位置姿勢について処理が実行された場合、処理は終了する。この結果、設定した全ての仮想撮像装置の位置姿勢について視点別位置姿勢推定用モデルが保存され、位置姿勢推定用モデルを得ることができる。

次に、図７を参照しながら、モデル作成装置２によって生成された位置姿勢推定用モデルを利用した位置姿勢推定方法について説明する。図７は、位置姿勢推定処理を示すフローチャートである。なお、以下の位置姿勢推定処理は、図３による処理結果を外部装置に転送し、外部装置が行うようにしてもよい。
（ステップＳ２０００）
まず、ステップＳ２０００では、位置姿勢推定装置１は、２次元画像撮像装置２０と距離画像撮像装置３０からなる撮像装置に対する物体の位置及び姿勢の概略値を入力する。本実施形態では、位置姿勢推定装置１は時間軸方向に連続して位置姿勢計測を行うものとして、前回（前時刻）の計測値を概略の位置姿勢として用いる。しかしながら、位置姿勢の概略値の入力方法はこれに限るものではない。

例えば、過去の位置姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置姿勢と推定された速度・加速度から現在の位置姿勢を予測したものでもよい。

また、様々な姿勢で撮像した対象物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチングを行うことによって、対象物体の大まかな位置姿勢を推定してもよい。

あるいは、センサによる物体の位置姿勢の計測が可能である場合には、該センサによる出力値を位置姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置姿勢を計測する光学式センサであってもよい。その他、６自由度の位置姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置姿勢があらかじめわかっている場合にはその値を概略値として用いる。
（ステップＳ２１００）
次に、ステップＳ２１００では、画像入力部１６０は、濃淡画像および距離画像の入力を行う。まず、２次元画像撮像装置２０から濃淡画像を取得する。同様に、距離画像撮像装置３０からの距離画像の取得を行う。

本実施形態では、距離画像は撮像装置から計測対象物体の表面までの距離が格納されているものとする。前述のように、２次元画像撮像装置２０と距離画像撮像装置３０の光軸は一致しているため、濃淡画像の各画素と距離画像の各画素の対応は既知である。濃淡画像および距離画像の取得が終了したら、ステップＳ２１００の処理を終了し、ステップＳ２２００に進む。
（ステップＳ２２００）
次に、ステップＳ２２００では、位置姿勢推定部１８０は、位置姿勢推定用モデルの中から、一つの視点別位置姿勢推定用モデルを選択することで、ステップＳ１４００で階層型構造としてグループ分けされた幾何特徴を得る。

具体的には、位置姿勢推定部１８０は、位置姿勢推定用モデルとして保存されている複数の視点別位置姿勢推定用モデルのうち、ステップＳ２０００で入力された概略位置姿勢に最も類似した位置姿勢に対応する視点別位置姿勢推定用モデルを選択する。

最も類似した視点別位置姿勢推定用モデルの選択には、概略位置姿勢と仮想撮像装置の位置姿勢との姿勢の差分をΔＲとして複数の基準位置姿勢に対してΔＲをそれぞれ算出し、この値が最も小さいものを選択する。或いは、概略位置姿勢と基準位置姿勢との位置の差分をΔＴ、姿勢の差分をΔＲとしてそれぞれ算出し、ΔＴ、ΔＲの値に関して全仮想撮像装置の位置姿勢において順位付けを行い、各順位の和が最も小さいものを選択してもよい。

位置姿勢推定用モデルを選択する方法は、以上に示す方法に限るものではなく、概略位置姿勢と仮想撮像装置の位置姿勢との類似性に基づいて計算する限り如何なる方式を選択しても良く、選択方法に特に制限はない。概略位置姿勢により、位置姿勢推定用モデルから、１つの位置姿勢推定用モデルを選択されたら、ステップＳ２２００の処理を終了し、ステップＳ２３００に進む。
（ステップＳ２３００）
次に、ステップＳ２３００において、幾何特徴選択部１７０は、視点別モデル保存部１５０中の階層型構造としてグループ分けされた幾何特徴の中から、位置姿勢算出に用いる幾何特徴を選出する。

本実施形態では、まず、階層型構造としてグループ分けされた幾何特徴から６つのクラスタとなるように切り出す。切り出された各クラスタ中の構成要素からランダムに１つの幾何特徴を選出することで、位置姿勢推定用の幾何特徴として計６つ選出する。異なるクラスタから選択的に幾何特徴を選出することで、位置姿勢パラメータへの影響が類似しない幾何特徴を選出することが可能になる。

なお、階層型構造から切り出すクラスタ数・利用する幾何特徴の数に関しては、位置姿勢を推定するために最低６つの幾何特徴が必要（位置姿勢の未知パラメータが６自由度あるため）であること以外に特に制限はない。

例えば、切り出すクラスタ数を３つにして、各クラスタから２つずつ幾何特徴をランダムに選出して処理を行っても、特に問題はない。また、切り出すクラスタ数および幾何特徴の数を設定する設定部を新たに設けて、クラスタ数および幾何特徴の数を任意に設定して推定処理を行っても本発明の本質を損なうことはない。

処理効率化という観点では、必要最低限数という意味で、選出する幾何特徴の数は６つであることが好適である。また、切り出すクラスタ数は、互いに類似しない幾何特徴を選ぶことが望ましいという観点で、選出する幾何特徴の数と同じ数にしておくことが好適である。

なお、切り出すクラスタの数が少なくなりすぎる場合（例えば１など）は、グループ分けせずにランダムに幾何特徴を選ぶことと同義となる。また、切り出すクラスタ数を最大数（全幾何特徴の数）にした場合には、全幾何特徴を用いた位置姿勢推定と変わらなくなる。そのため、処理を効率化するという観点では、切り出すクラスタ数が極端に少なくなり過ぎず、また、多くなり過ぎないほうが良い。
（ステップＳ２４００）
次に、ステップＳ２４００では、ステップＳ２１００において入力された濃淡画像及び距離画像と、ステップＳ２３００において選出した線幾何特徴および面幾何特徴との対応を算出し、対応付けた結果に基づいて計測対象物体の位置及び姿勢を推定する。以下、位置姿勢推定部１８０によって行われる位置姿勢推定処理について詳細な処理を詳述する。

まず、ステップＳ２０００において入力された対象物体の概略位置姿勢に基づいて、ステップＳ２１００において入力された距離画像中の３次元点群と、面幾何特徴との対応付けを行う。概略位置姿勢と校正済みの距離画像撮像装置３０の内部パラメータを用いて、ステップＳ２３００において選出された全ての面幾何特徴を、距離画像上に投影する。そして、投影した各面に対応する距離画像上の距離点群を、各面幾何特徴に対応する３次元点として保持する。

次に、ステップＳ２１００において入力された濃淡画像上のエッジと線幾何特徴との対応付けを行う。概略位置姿勢と校正済みの距離画像撮像装置３０の内部パラメータを用いて、ステップＳ２３００において選出された全ての線幾何特徴を画像へ投影し、画像上で検出されたエッジと、線幾何特徴とを対応付けする。線幾何特徴に対して、画像上から複数の対応エッジが検出された場合は、投影された線幾何特徴に対して画像上で最も近いエッジを対応付ける。

次に、面幾何特徴および線幾何特徴と距離・濃淡画像との対応データに基づいて、計測データと幾何特徴との間の誤差が最小になるように線形連立方程式を解くことで、位置姿勢の更新を行う。

ここで、画像上の距離と３次元空間中での距離は尺度が異なるため、単純に連立方程式を解くだけでは計測データのどちらか一方に寄与率が偏ってしまう。そのため、本実施形態では、以下の文献に示されるような、最尤推定に基づく最適化を行うことで、尺度を合わせた位置姿勢推定を行う。

立野、小竹、内山、“実部品のビンピッキングのための距離・濃淡画像を最尤に統合する高精度高安定なモデルフィッティング手法、”第１３回画像の認識・理解シンポジウム（ＭＩＲＵ２０１０）、ＯＳ５−１、２０１０。

最尤推定に基づく位置姿勢推定方法に関しては、本発明の本質に関わる話ではないため、詳細な処理の記述は省略する。詳細は上述の文献を参照されたい。なお、計測対象物体の位置及び姿勢の算出方法は、上述の手法に限るものでなく、例えば、Levenberg-Marquardt法による繰り返し演算を行っても良いし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やICCG法など、他の非線形最適化計算手法を用いてもよい。
（ステップＳ２５００）
次に、ステップＳ２４００で更新した位置姿勢が、収束しているか否か（すなわち、さらに反復計算を必要とするか否か）の判定を行う。収束しているか否かは、補正値がほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。

収束していなければ、再度、ステップＳ２２００に戻り、更新した位置姿勢を用いて、視点別位置姿勢推定用モデルの選出から処理を行う。なお、ステップＳ２４００で更新した位置姿勢の更新量が小さい場合などは、ステップＳ２４００へ戻り、同じ幾何特徴を用いて位置姿勢更新処理を行ってもよい。収束していると判断した場合は、この処理は終了し、撮像装置と対象物体間の相対的な位置姿勢の最終的な推定値が決定される。

以上述べたように、本実施形態では、位置姿勢６自由度の推定パラメータへの影響が類似するような幾何特徴群をあらかじめグループ化しておくことで、位置姿勢推定処理において、推定に効果的な幾何特徴を高速に選出することが可能になる。これにより、位置姿勢パラメータへの影響が似通った幾何特徴を偏って利用することなく、少ない幾何特徴から効率的に位置姿勢推定を行うことが可能になる。

（変形例１）
上述の実施形態では、幾何特徴のグループ分けとして、ウォード法による階層型クラスタリングによる方法を説明していた。しかし、幾何特徴のグループ分け手法は、これに限るものではない。

例えば、階層型クラスタリングを用いてグループ分けする場合においても、類似度算出に、最短距離法、最長距離法、群平均法、重心法、メジアン法、可変法などの、ウォード法以外の類似度算出方式を用いてグループ分けを行っても良い。

また、階層型クラスタリング以外にも、k-meansに代表されるような、非階層型クラスタリング方式を利用しても良い。この場合、グループ分けの処理は以下のようにすれば良い。

まずグループ分けの総数をＭとして決め、全幾何特徴のヤコビアンJ_i（i=1…n）の中から、Ｍ個の各グループの基準C_j（j=1…M）を選出する。次に、各サンプル点iを、距離|J_i-C_j|が最小となるC_jのグループへ分類する。続いて、各グループに属するサンプル点のヤコビアンの平均値を計算し、算出した平均値でグループの基準C_jを更新する。以上の分類と更新の２つの処理を、C_jの更新がなくなるまで繰り返すことで、幾何特徴のグループ分けを行う。

グループ分けの総数Ｍの設定としては、上述の実施形態におけるステップＳ２３００において切り出すクラスタの数と同様に、例えば、選出する幾何特徴の数と同様にすればよい。

すなわち、選出する幾何特徴の数を６つにする場合は、グループ分けの総数Ｍは６に設定すればよい。なお、グループ分けの総数Ｍの設定に関する好適な決め方および条件は、ステップＳ２３００における切り出すクラスタの数の設定と同様である。
（変形例２）
上述の実施形態では、幾何特徴のグループ分けを行う基準として、幾何特徴のヤコビアンが類似する幾何特徴をグループ分けしていた。しかしながら、幾何特徴のグループ分けを行う判定基準はこれに限るものではない。

例えば、ヤコビアンが類似しない幾何特徴を同一のグループにグループ分けしても良い。具体的には、ステップＳ１４２０において計算していたクラスタ間の非類似度ΔＳ_ＡＢがもっとも大きいクラスタ同士を選択してグループ分けしてもよい。

この場合、ヤコビアンが類似しない幾何特徴が１つのグループとしてグループ分けされるので、ステップＳ２３００では、１つのクラスタ内から幾何特徴をランダムに選出することで、ヤコビアンが類似しない幾何特徴を選出する。

以上に示すように、グループ分けする基準としては、ヤコビアンが類似する幾何特徴をグループ分けする方式に限るものでなく、類似しない幾何特徴同士をクラスタリングしてもよい。グループ分けの基準と位置姿勢推定時におけるグループ分け結果からの幾何特徴選出の基準が一致している限り、判別基準には特に制限はなく、いかなる基準でグループ分けを行っても特に問題はない。
（変形例３）
上述の実施形態では、グループ分けされた幾何特徴から位置姿勢推定に利用する幾何特徴を１セットのみ選出して、位置姿勢推定処理を行っていた。しかしながら、幾何特徴の選出および位置姿勢推定処理の方法はこれに限るものではない。

例えば、ＲＡＮＳＡＣ的に複数の幾何特徴セットを用意して、幾何特徴セットによる位置姿勢算出とその評価を繰り返すことで、最も妥当な幾何特徴セットを判別して位置姿勢を算出する処理を行ってもよい。

具体的には、まずステップＳ２３００において、階層型構造から切り出した各クラスタ中の構成要素から、ランダムに１つの幾何特徴を選出することを複数回繰り返すことで、６つの幾何特徴で１セットとなる位置姿勢算出用幾何特徴セットを複数用意する。

そして、ステップＳ２４００では、ステップＳ２３００において選出した幾何特徴セットごとにそれぞれ位置姿勢を推定する。そして、推定した位置姿勢に基づいて改めて幾何特徴を画像上に投影し、幾何特徴と画像中の特徴との対応間距離の総和が最も小さくなる位置姿勢を、もっとも妥当な位置姿勢として選択する。

以上に示すように、位置姿勢推定に用いる幾何特徴の選出方法および選出した幾何特徴による位置姿勢推定手法は、上述の実施形態に限るものでなく、選出した幾何特徴により位置姿勢を推定できる限り、いかなる方式を用いてもよい。
（変形例４）
上述の実施形態では、幾何特徴と画像との対応に基づいて計測対象物体の位置及び姿勢の６自由度の算出を行っていた。しかしながら、位置姿勢の推定方法として、位置及び姿勢の６自由度全てを推定する方法に限るものではない。例えば、位置姿勢６自由度パラメータへの影響が類似する幾何特徴群を選出し、それらから位置姿勢６自由度の中で一部分のパラメータのみを推定してもよい。

具体的には、まずステップＳ２３００において、階層型構造から切り出した同一のクラスタ中の構成要素から、ランダムに複数（例えば６つ）の幾何特徴を選出することで、位置姿勢６自由度パラメータへの影響が似通った幾何特徴群を選出する。

そして、ステップＳ２４００では、選出した幾何特徴群と画像との対応結果に基づいて、選出した幾何特徴群が寄与し易い位置姿勢パラメータのみを推定する。例えば、画像平面Ｘ方向に寄与し易い幾何特徴群を選出した場合には、画像平面Ｘ方向のみに推定するパラメータを限定して推定する。位置姿勢パラメータの一部のみを部分的に推定する方法自体に関しては、本発明の本質に関わる話ではないため、詳細な処理の記述は省略する。詳細は非特許文献１を参照されたい。

以上に示すように、本発明における位置姿勢の推定手法は、位置及び姿勢の６自由度パラメータ全てを推定する方式に限るものではない。選出した幾何特徴に基づいて、位置及び／又は姿勢の情報が推定できる限り、推定する情報および位置姿勢の推定方式に特に制限は無く、いかなる方式を用いても良い。
（変形例５）
上述の実施形態では、画像および幾何特徴の対応付けに基づく位置姿勢推定手法として、非線形最小二乗法による方式を述べた。しかしながら、本発明の情報処理装置に適用する位置姿勢推定手法は、これに限るものではない。

例えば、幾何特徴と画像との対応付けに基づいて、拡張カルマンフィルタのような時系列フィルタにより位置姿勢を推定する場合においても、本発明を適用可能である。時系列フィルタに基づいて位置姿勢を推定する場合には、１つの幾何特徴から位置姿勢を推定することが可能である。

１つの幾何特徴に基づく位置姿勢推定では、位置姿勢６自由度パラメータの中で、その幾何特徴が推定に寄与し易いパラメータのみが推定される。そのため、推定に寄与しやすい自由度が似通った幾何特徴ばかりが連続して選択された場合、位置姿勢６自由度全体を推定することが出来ない。

従って、１つの幾何特徴の情報に基づいて拡張カルマンフィルタによる位置姿勢推定を行う場合、３次元形状モデルから選出する幾何特徴の順番が、効率的な推定に重要である。以下、拡張カルマンフィルタによる位置姿勢推定に、本発明の位置姿勢推定装置を適用した場合について、処理の詳細を述べる。

まず、ステップＳ２３００において、位置姿勢更新に利用する幾何特徴を選出する。上述の実施形態においては、位置姿勢算出に利用する幾何特徴を６つ選出していたが、拡張カルマンフィルタに基づく位置姿勢推定では、最低１点の情報があれば推定が可能である。そのため、本変形例では、ステップＳ２３００の処理において、１点の幾何特徴をランダムに選出する。

次に、ステップＳ２４００では、選出した幾何特徴と画像との対応に基づいて、拡張カルマンフィルタに基づくフィルタリング処理により位置姿勢更新を行う。選出した１点の幾何特徴とその対応データに基づく拡張カルマンフィルタの計算方法自体に関しては、本発明の本質に関わるものではないため、詳細な説明は省略する。処理の詳細については、[Y. Hel-Or, M. Werman, “Pose estimation by fusing noisy data of different dimensions, ” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.17, no.2, pp. 195-201, 1995.]を参照されたい。１点の幾何特徴による位置姿勢更新が終了したら、ステップＳ２５００において、これ以上更新するか否かを判定する。

判定としては、例えば、あらかじめ位置姿勢処理にかける処理時間のリミットを決めておいて、リミットになるまで、更新を繰り返す。更新を終了する場合は、現状の位置姿勢を最終的な位置姿勢として位置姿勢推定装置１の処理を終了する。再度位置姿勢更新を行う場合は、ステップＳ２３００に戻り、改めて幾何特徴を選出する。このとき、前回選出した幾何特徴が所属するクラスタからは選択しないことによって、位置姿勢推定への影響が類似しない幾何特徴を選択することが可能である。

以上に示すように、グループ分けした幾何特徴を利用して位置姿勢を推定する手法は、上述の実施形態に限るものではない。幾何特徴クラスタから選出した幾何特徴を用いて位置姿勢を推定できる限り、位置姿勢推定方法に特に制限はなく、いかなる方式を用いてもよい。
（変形例６）
上述の実施形態・変形例では、画像および幾何特徴の対応付けに基づく位置姿勢推定手法として、非線形最小二乗法および時系列フィルタによる方式を述べた。しかしながら、本発明の位置姿勢推定装置に適用する位置姿勢推定手法は、これらに限るものではない。例えば、幾何特徴と画像との対応付けに基づくマッチングにより位置姿勢を算出する場合においても、本発明は適用可能である。

この場合、例えば、まず、ステップＳ２４００において、対象物体の位置及び姿勢として、概略の位置及び姿勢を中心に一定範囲において６自由度の値が網羅的にカバーできるように多数の位置及び姿勢を生成しておく。

次に、生成した位置及び姿勢における幾何特徴と、濃淡画像及び距離画像とのマッチング具合を評価することで、対象物体の位置及び姿勢を推定する。具体的には、まず、探索する位置及び姿勢の設定として、概略として入力された位置及び姿勢の値を中心に、位置及び姿勢６自由度全組み合わせの値を設定する。このとき、網羅的に設定する位置の最大幅・ステップ幅、および、姿勢の最大幅・ステップ幅の設定に関しては特に制限はない。本変形例では、位置の最大幅は、対象物体のサイズと同等に、姿勢の最大幅は９０度にして、位置のステップ幅は1[mm]、姿勢のステップ幅は1[度]として、探索する位置及び姿勢を設定する。次に、設定した位置及び姿勢を１つ選択し、選択した位置及び姿勢に基づいて、距離画像中の３次元点群と濃淡画像上のエッジと、選出した幾何特徴との対応付けを行う。

そして、幾何特徴と濃淡・距離画像との対応データに基づいて、計測データと幾何特徴との間の誤差を算出することで、マッチング具合を表す評価値を算出する。以上の処理を、設定した全ての位置及び姿勢に対して行うことで最も評価値が高い位置姿勢の値を探索する。

そして、もっとも評価値が高い位置姿勢を、対象物体の最終的な位置姿勢として出力する。以上に示すように、選出した幾何特徴により位置姿勢を推定する方式は、最小二乗法や時系列フィルタに基づく手法に限るものでない。選出した幾何特徴が濃淡・距離画像に当てはまるような位置姿勢を推定できる限り、位置姿勢算出方法に特に制限は無く、いかなる方式を用いてもよい。
（変形例７）
上述の実施形態では、濃淡画像と距離画像を撮像する場合について説明していた。しかし、本発明の位置姿勢推定装置は、濃淡画像と距離画像を両方撮影する場合に限るものではない。

例えば、距離画像のみが撮像される場合でも、本発明の位置姿勢推定装置を適用できる。本変形例における位置姿勢推定装置は、図１の構成から2次元画像撮像装置２０を抜いた構成となる。

また、位置姿勢推定処理に関しては、ステップＳ２１００における濃淡画像の入力処理が無くなり、ステップＳ２４００における濃淡画像と幾何特徴との対応付け処理がなくなる以外、上述の実施形態と変更はない。

また、同様に、濃淡画像のみが撮像される場合においても、本発明の位置姿勢推定装置を適用できる。この場合、図１の構成から距離画像撮像装置３０を抜いた構成となり、ステップＳ２１００における距離画像の入力処理が無くなり、ステップＳ２４００における距離画像と幾何特徴との対応付け処理がなくなる以外、上述の実施形態と変更はない。
（変形例８）
上述の実施形態では、幾何特徴グループ分け部１４０において、位置姿勢パラメータへの影響が類似する幾何特徴をグループ分けする方法として、幾何特徴のヤコビアンに基づいてグループ分けを行う場合を説明した。しかし、幾何特徴のグループ分け方法は、ヤコビアンの類似度に従う方法に限ったものではない。例えば、３次元形状モデルのラインあるいは面上に設定されている幾何特徴群のうち、ラインあるいは面の角度や位置が類似する幾何特徴群は、位置姿勢パラメータへの影響が類似すると判定する。そして、ラインあるいは面の角度や位置が類似する幾何特徴が同一グループになるようにグループ分けしても良い。

以上に示すように、本発明による位置姿勢推定装置１は、濃淡画像と距離画像が両方得られる場合に限るものでなく、どちらか片方の画像が得られる限り、問題なく適用することが可能である。

［第２の実施形態］
本発明による位置姿勢推定装置１の好適な適用事例としては、以下のような形態も考えられる。すなわち、２次元画像撮像装置２０および距離画像撮像装置３０により得られる２次元画像と距離画像を基に被計測物体６０の位置姿勢を推定し、産業用ロボットアーム（５０）によりその把持などを行う利用例があげられる。以下、図８を用いて本発明の一実施例である位置姿勢推定装置１の適用例を説明する。図８では、位置姿勢推定装置１とロボット５０を用いて被計測物体６０を把持するロボットシステムの構成例を示す。

ロボット５０はロボットコントローラ４０により制御され、指令された位置に手先を移動させ物体の把持などを行うロボットである。被計測物体６０は、作業台に置かれる位置が変わるため、現在の被計測物体６０の位置姿勢を推定し、ロボットの把持制御を行う必要がある。

２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラ、距離画像撮像装置３０は物体表面の距離を計測する距離センサであり、産業用ロボットアームの手先等の被計測物体６０を撮像できる位置に設置する。

位置姿勢推定装置１は、２次元画像撮像装置２０および距離画像撮像装置３０から得られる２次元画像と距離画像を基に被計測物体６０の位置姿勢を推定する。位置姿勢推定装置１で推定された被計測物体６０の位置姿勢は、ロボットコントローラ４０に入力され、被計測物体６０の把持などを行うようにロボットを制御する。本発明の情報処理装置により、ロボットシステムは被計測物体６０の位置が不定でも位置姿勢推定を行うことで、被計測物体６０を把持することが可能となる。

以上、実施形態を詳述したが、本発明は、例えば、システム、装置、方法、プログラム、もしくは記憶媒体等としての実施態様をとることが可能である。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。

尚、本発明は、ソフトウェアのプログラムをシステム或いは装置に直接或いは遠隔から供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムコードを読み出して実行することによって前述した実施形態の機能が達成される場合を含む。この場合、供給されるプログラムは実施形態で図に示したフローチャートに対応したコンピュータプログラムである。

従って、本発明の機能処理をコンピュータで実現するために、該コンピュータにインストールされるプログラムコード自体も本発明を実現するものである。つまり、本発明は、本発明の機能処理を実現するためのコンピュータプログラム自体も含まれる。

その場合、プログラムの機能を有していれば、オブジェクトコード、インタプリタにより実行されるプログラム、ＯＳに供給するスクリプトデータ等の形態であっても良い。

コンピュータプログラムを供給するためのコンピュータ読み取り可能な記憶媒体としては以下が挙げられる。例えば、フロッピー（登録商標）ディスク、ハードディスク、光ディスク、光磁気ディスク、ＭＯ、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−ＲＷ、磁気テープ、不揮発性のメモリカード、ＲＯＭ、ＤＶＤ（ＤＶＤ−ＲＯＭ，ＤＶＤ−Ｒ）などである。

その他、プログラムの供給方法としては、クライアントコンピュータのブラウザを用いてインターネットのホームページに接続し、該ホームページから本発明のコンピュータプログラムをハードディスク等の記録媒体にダウンロードすることが挙げられる。この場合、ダウンロードされるプログラムは、圧縮され自動インストール機能を含むファイルであってもよい。また、本発明のプログラムを構成するプログラムコードを複数のファイルに分割し、それぞれのファイルを異なるホームページからダウンロードすることによっても実現可能である。つまり、本発明の機能処理をコンピュータで実現するためのプログラムファイルを複数のユーザに対してダウンロードさせるＷＷＷサーバも、本発明に含まれるものである。

また、本発明のプログラムを暗号化してＣＤ−ＲＯＭ等の記憶媒体に格納してユーザに配布するという形態をとることもできる。この場合、所定の条件をクリアしたユーザに、インターネットを介してホームページから暗号を解く鍵情報をダウンロードさせ、その鍵情報を使用して暗号化されたプログラムを実行し、プログラムをコンピュータにインストールさせるようにもできる。

また、コンピュータが、読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているＯＳなどとの協働で実施形態の機能が実現されてもよい。この場合、ＯＳなどが、実際の処理の一部または全部を行ない、その処理によって前述した実施形態の機能が実現される。

さらに、記録媒体から読み出されたプログラムが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれて前述の実施形態の機能の一部或いは全てが実現されてもよい。この場合、機能拡張ボードや機能拡張ユニットにプログラムが書き込まれた後、そのプログラムの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行なう。

以上のように、上記各実施形態によれば、位置姿勢が既知である参照画像（過去フレームにおいて観察対象物体が撮像された撮影画像やテクスチャが設定されたモデル画像）を利用して、使用すべき画像特徴が決定される。そして、このような画像特徴を用いて現在のフレームに関する撮像装置の位置姿勢を算出するので、位置姿勢推定の安定性が向上する。即ち、各実施形態によれば、観察対象物体の３次元モデルデータから抽出した３次元線分モデルが、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定することができる。また、３次元モデルから抽出される幾何的特徴に加えて、観察対象物体の表面の色に基づく特徴を利用することで、３次元モデルデータの中に含まれていない情報も利用して位置姿勢推定することが可能である。

［第３の実施形態］
本実施形態では、位置姿勢の推定対象となる被計測物体の位置姿勢を、該被計測物体と同形状の３次元形状モデルを用いたモデルフィッティングにより推定する。本実施形態の対応付けでは先ず、次のような処理を行う。即ち、被計測物体を撮像した画像撮像装置に対する該被計測物体の位置姿勢の概略値（位置姿勢概略値）に基づいて、３次元形状モデルの幾何特徴（本実施形態では３次元形状モデルの線分及び面上に設定したサンプル点）の撮像画像上での位置を計算する。そして次に、サンプル点に対応する点（対応点）を該撮像画像から探索し、サンプル点と対応点との間の距離（対応間距離）を求める。この撮像画像は、本実施形態では、濃淡画像及び距離画像とする。

本実施形態では、対応付けの結果に混入する外れ値を精度良く取り除く。そのために本実施形態では、サンプル点に対応する対応点を探索し、対応間距離を求めるのに先立ち、「類似する対応間距離が予測されるサンプル点が同一グループになるように分類」する。そして、サンプル点に対応する対応点を探索した後、グループごとに、この探索の結果（対応付けの結果）を判定する。

同一グループでは対応付けが正しいサンプル点群については対応間距離が類似することが期待できる。対応付け結果の中で外れ値が少数派であると仮定すると、グループ内でサンプル点を対応付けた後、グループ内で類似する対応間距離を持つ対応の集まりは外れ値の可能性は低く、逆にそこから外れる対応間距離を持つ対応は外れ値の可能性が高いと考えられる。そこで、各グループの対応間距離の分布を基に外れ値を判定し、外れ値以外の対応間距離を利用して精度とロバスト性の高い位置姿勢推定を行う。

ここで、被計測物体としての部品の位置姿勢を推定するために、該部品と同形状の３次元形状モデルを、該部品を含む画像上に投影し、該画像上で部品と３次元形状モデルとの間で対応付けを行うとする。このときに、従来手法で対応付けを行うケースについて、図９を用いて説明する。

図９に示す如く、画像には部品５１０２が写っている。また、部品５１０２の位置姿勢概略値を基に画像上に投影した３次元形状モデルを重ねて表示させている。ただし、図９の形状モデルは対応付けの説明の都合上で表示させているだけであり、実際に形状モデルを画像上に投影することなはく、サンプル点の画像上での位置を計算するだけである。対応付けでは先ず、画像から、３次元形状モデル５１０１上のサンプル点５１０３に対応する点を探索して対応点を検出する。図９では対応点として、部品５１０２の辺上の点５１０４が探索されている。そして次に、サンプル点５１０３と点５１０４とをつなぐ線分の長さを対応間距離５１０５として求める。

ここで、図９では、対応点として誤対応となってしまった点５１０６，５１０７が外れ値として示されている。全てのサンプル点の対応間距離を統計的に利用し、外れ値を判定することを考えると、点５１０６のように他のサンプル点とは大きく異なる対応間距離を持つ対応については判定が容易である。しかし、点５１０７のように統計的に標準的な対応間距離に近い対応間距離を持つ対応は判定が難しい。

これに対し、図１０では、サンプル点のグループ分けを行い、グループ毎に外れ値の判定を行っている。グループ分けは、類似する対応間距離が予測されるサンプル点群が同一グループになるように行う。例えば、グループ５２０１のように、同一線分上で近接したサンプル点などが同一グループになる。図１０から分かるように、同一グループ内では対応付けが正しいサンプル点群については対応間距離が類似することが期待できる。したがって、グループ毎に外れ値判定を行えば、点５１０７のような偏差の少ない外れ値であっても高い信頼度で判定できる。

本実施形態に係る情報処理装置の機能構成例について、図１１のブロック図を用いて説明する。図１１に示す如く、情報処理装置５３００には画像撮像装置５３１０が接続されている。先ず、画像撮像装置５３１０について説明する。

画像撮像装置５３１０は、シーンの２次元画像を撮像するカメラ機能と、距離画像を取得するための距離センサと、を有している。本実施形態では、画像撮像装置５３１０は、被計測物体を撮像しているものとする。

２次元画像は、濃淡画像であっても良いし、カラー画像であっても良い。以下の説明では、２次元画像は濃淡画像であるものとして説明する。撮像するカメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば、下記の文献に示す方法によって事前にキャリブレーションしておく。

R. Y. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses, ” IEEE Journal of Robotics and Automation, vol.RA-3, no.4, 1987.
距離センサとしては、対象（ここでは被計測物体）に照射したレーザ光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、距離画像を計測するものであればいかなるものであっても良い。カメラと距離センサの両者の相対的な位置姿勢は変化しないと仮定し、事前にキャリブレーションしておく。キャリブレーションの方法には、例えば、次のようなものがある。即ち、３次元形状が既知であるキャリブレーション物体を様々な方向から観察し、２次元画像に基づいたキャリブレーション物体の位置及び姿勢と、距離画像に基づいたキャリブレーション物体の位置及び姿勢の差から、相対的な位置及び姿勢を求める方法である。

画像撮像装置５３１０により得られる２次元画像及び距離画像は、情報処理装置５３００に入力される。なお、この２次元画像及び距離画像は、予め撮像しておいて、何らかの記憶装置に格納しておいても良い。その場合、この２次元画像及び距離画像は、この記憶装置から情報処理装置５３００に入力されることになる。即ち、情報処理装置５３００が２次元画像及び距離画像を取得するための方法は特定の方法に限るものではない。しかし、いずれにせよ、２次元画像及び距離画像を撮像した装置に対する被計測物体の位置姿勢概略値は、後述する位置姿勢概略値入力部５３０３により入力されるものとする。また、この位置姿勢概略値は、被計測物体を基準とした場合には、被計測物体に対する視点の位置姿勢概略値でもある。

次に、情報処理装置５３００について説明する。画像撮像装置５３１０から送出された２次元画像及び距離画像は、画像入力部５３０１を介して対応付け部５３０５に入力される。

形状モデル保持部５３０２は、被計測物体と同形状の３次元形状モデルの情報（モデル情報）と、該３次元形状モデル上に予め設定された各サンプル点の位置情報と、が保持されている。

モデル情報が表現する３次元形状モデルとしては、例えば、３次元ＣＡＤに基づく３次元形状モデルが挙げられる。即ち、３次元ＣＡＤソフトウェアで取り扱うことができるＣＡＤモデルそのもの、もしくは３次元ＣＡＤモデルをＣｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ分野で使われる複数のポリゴン要素に変換したものである。以下の説明では、３次元形状モデルは、複数のポリゴンから成るものであるとして説明する。本実施形態で用いるモデル情報の構成例について図１２を用いて説明する。

３次元形状モデルは図１２に示すような点、線（辺）、面といった構成要素からなる。図１２（ａ）〜（ｃ）はいずれも同じ３次元形状モデルを示している。図１２では、ある視点から見た頂点、辺、面が示されており、この視点からでは見えない頂点、辺、面については示されていない。

このような３次元形状モデルのモデル情報は、図１２（ｄ）〜（ｆ）に示す如く、各頂点のインデックス及び座標値、各辺のインデックス及び該辺の両端の頂点のインデックス、各面のインデックス及び該面の辺のインデックス及び法線ベクトル成分、を有する。頂点の座標値は、３次元形状モデルが有するローカルな原点を基準としたものであっても良いし、どの点を基準にするのかについては別段限定はしない。これは、以下に説明するサンプル点の位置情報についても同様である。

サンプル点は、濃淡画像のエッジ特徴と対応付けを行うために３次元形状モデルの辺上に設定した点（ラインサンプル点）と、距離画像の持つ３次元点と対応付けを行うために３次元形状モデルの面上に設定した点（面サンプル点）と、から成る。

ただし、対応付けに利用できるサンプル点は３次元形状モデルの画像上での線及び面の見え方によって異なる。例えば、３次元形状モデルが図１２（ａ）〜（ｃ）に示したように画像上に投影された場合、頂点Ｐ１〜Ｐ４，Ｐ６〜Ｐ８は見えているが、頂点Ｐ５は見えていない。また、辺Ｌ１〜Ｌ７、Ｌ１０，Ｌ１１は見えているが、辺Ｌ８，Ｌ９、Ｌ１２は見えていない。また、面Ｓ１，Ｓ３，Ｓ４は見えているが、面Ｓ２，Ｓ５，Ｓ６は見えていない。

然るに、あらかじめ３次元形状モデルを囲むジオデシックドームの各頂点上に配置した複数の視点から、３次元形状モデル上で観測可能な辺及び面を抽出し、その上にサンプル点を設定する。各視点で観測可能なサンプル点群の位置を、視点に応じたデータセットとして保持する。これにより、視点と３次元形状モデルとの様々な位置姿勢関係に対応するサンプル点群を取得することが出来る。

位置姿勢概略値入力部５３０３は、画像撮像装置５３１０に対する被計測物体の位置姿勢概略値を取得する。位置姿勢概略値は、濃淡画像に対するテンプレートマッチング等の前処理により、予め分かっている場合を仮定する。また、逐次的に位置姿勢推定処理を繰り返す場合、位置姿勢概略値入力部５３０３を備えずに、１回前の位置姿勢推定処理で推定した結果を位置姿勢概略値として利用しても良い。

幾何特徴グループ分け部５３０４は、形状モデル保持部５３０２が保持している視点ごとのサンプル点群（位置情報群）のうち、位置姿勢概略値入力部５３０３から入力された位置姿勢概略値に対応するサンプル点群を、形状モデル保持部５３０２から取得する。そして、幾何特徴グループ分け部５３０４は、形状モデル保持部５３０２から取得したサンプル点群を、「類似する対応間距離が予測されるサンプル点群が同一グループになるように」複数のグループに分類する。ただし、対応間距離は対応付ける画像によってスケールが異なるため、濃淡画像のエッジ特徴と対応付けを行うラインサンプル点と、距離画像の３次元点群と対応付けを行う面サンプル点は別々にグループ分けを行う。

対応付け部５３０５は、画像入力部５３０１から受けた濃淡画像及び距離画像に対し、幾何特徴グループ分け部５３０４により分類されたそれぞれのサンプル点について対応付けを行ことで対応点を探索し、サンプル点と対応点との間の対応間距離を求める。

対応付け評価部５３０６は、対応付け部５３０５が求めたそれぞれのサンプル点の対応間距離をグループごとに評価し、外れ値としての対応点以外の対応点について求めた対応間距離をモデルフィッティングで用いる対応間距離として特定する。

位置姿勢推定部５３０７は、位置姿勢概略値入力部５３０３から入力された位置姿勢概略値を、対応付け評価部５３０６で特定した対応間距離を用いて補正することで、被計測物体の最終的な、より精度の高い位置姿勢を算出する。

次に、被計測物体の位置姿勢をモデルフィッティングを用いて推定するために情報処理装置５３００が行う処理について、同処理のフローチャートを示す図１３を用いて説明する。

＜ステップＳ５５０１＞
画像入力部５３０１は、画像撮像装置５３１０から出力された被計測物体の濃淡画像及び距離画像を取得する。濃淡画像及び距離画像のセットは１回だけ入力するようにしても良いし、連続的に入力するようにしても良い。後者の場合、以下の処理は、それぞれのセットについて行うことになる。

＜ステップＳ５５０２＞
幾何特徴グループ分け部５３０４は、位置姿勢概略値入力部５３０３から位置姿勢概略値を取得する。

＜ステップＳ５５０３＞
幾何特徴グループ分け部５３０４は、形状モデル保持部５３０２が保持している視点ごとのサンプル点群（位置情報群）のうち、位置姿勢概略値入力部５３０３から取得した位置姿勢概略値に対応するサンプル点群を、形状モデル保持部５３０２から取得する。即ち、オデシックドームの各頂点のうち、位置姿勢概略値入力部５３０３から取得した位置姿勢概略値に最も近い視点から取得したサンプル点群を取得する。

そして、幾何特徴グループ分け部５３０４は、形状モデル保持部５３０２から取得したサンプル点群を複数のグループのそれぞれに分類する。ここで、サンプル点群を複数のグループのそれぞれに分類する方法について説明する。

グループ分けは類似する対応間距離が予測されるサンプル点を同一グループにするために行う。対応間距離の類似度は、例えばサンプル点のヤコビアンと関係があるため、ヤコビアンの類似度を基にサンプル点をグループ分けできる。ここで、ヤコビアンとは位置姿勢６自由度の微小変化に対して、対応間距離がどのように増減するかを表す１次偏微分の行列を意味する。

ここで、サンプル点のヤコビアンから対応間距離の類似度が分かることを説明する。ある位置姿勢概略値をｘとして３次元形状モデルを画像上に投影し、該投影した３次元形状モデル上のサンプル点と、位置姿勢真値がｘＣである被計測物体とを対応付けることを考える。対応付けは各サンプル点を基点にして対応点を探索し、対応付けを行う空間でその距離を求める。サンプル点ｉ（ｉ＝１…ｎ）について求める対応間距離Δziを以下の式（６）のように定義する。

この式においてｈは、３次元形状モデル上のサンプル点の位置を対応付け空間に変換する式を表す。ここで、対応付けを行う空間で、位置姿勢概略値を基にしたサンプル点位置ｈ（ｘ，ｉ）に対し、位置姿勢真値を基にしたサンプル点位置ｈ（ｘＣ，ｉ）を理想的な対応点位置として表している。対応間距離Δｚｉは、位置姿勢概略値ｘのずれ量Δｘ＝（ｘ−ｘＣ）に応じて非線形に増減する。しかし、Δｘが微小であると考えると、ｈ（ｘ、ｉ）の１次偏微分（ヤコビアン）を用いて線形近似することで、上記の式（６）を以下の式（７）のように表すことができる。

ここで、Ｈｉは位置姿勢概略値ｘの６自由度の微小変化に対して、計量される対応間距離がどのように増減するかを表している。Δｘはサンプル点に依らず一定値であるため、各サンプル点から求める対応間距離Δｚｉは、そのヤコビアンＨｉに従うことが分かる。つまり、サンプル点をヤコビアンの類似度を基にグループ分けすることで、類似する対応間距離が予測されるサンプル点群に分類することができる。

次に、ヤコビアンの類似度でサンプル点を分類する手法の詳細について説明する。本実施形態では、k-means法に基づくグループ分けの方法を説明する。ただし、グループ分けは、k-means法に基づく方法に限らず、類似するヤコビアンのサンプル点を同一グループにできる方法であればどのような方法でも良い。

グループ分けを行うサンプル点ｉのヤコビアンをＨｉ、グループ総数をＭ（Ｍは２以上の自然数）として設定する。まず、ｎ個のヤコビアンからＭ個のヤコビアン（代表ヤコビアン）を選択する。Ｍ個のヤコビアンの選択はランダムに行っても良いし、規定の順序で行っても良い。そして選択したＭ個のヤコビアンＨＳ（１）〜ＨＳ（Ｍ）をそれぞれ、Ｍ個のグループＧ１〜ＧＭの基準Ｃ（１）〜Ｃ（Ｍ）として設定する（即ち、Ｃ（ｊ）＝ＨＳ（ｊ）、ｊ＝１…Ｍ）。

次に、処理１として、各サンプル点ｉを、距離｜Ｈｉ−Ｃｊ｜が最小となるグループＣｊに分類する。ここで、この距離｜Ｈｉ−Ｃ（ｊ）｜はヤコビアンＨｉの共分散行列で正規化を行ったマハラノビス距離を意味する。

次に、処理２として、グループｊに分類されている全てのサンプル点のヤコビアンの平均値を計算し、Ｃ（ｊ）を該計算した平均値に更新する。そして処理１及び処理２を、Ｃ（ｊ）の更新が無くなるまで繰り返す。このような処理により、サンプル点群を予想される対応間距離の類似度によってグループ分けすることができる。

＜ステップＳ５５０４＞
対応付け部５３０５は、ステップＳ５５０３で形状モデル保持部５３０２から取得したそれぞれのサンプル点について対応付けを行うことで、該サンプル点の対応点を探索し、探索した対応点との距離（対応間距離）を求める。この対応付けは、濃淡画像及び距離画像のそれぞれについて行う。

即ち、濃淡画像についてこのような対応付けを行う場合、ラインサンプル点から濃淡画像上のエッジ特徴を対応点として探索し、探索した対応点とラインサンプル点との間の対応間距離を求める。

また、距離画像についてこのような対応付けを行う場合、面サンプル点から距離画像の奥行き方向の対応点を探索し、探索した対応点と面サンプル点との間の対応間距離を求める。対応点の探索方法の詳細については、例えば、下記の文献１を参照されたい。

＜文献１＞ 立野、小竹、内山、“実部品のビンピッキングのための距離・濃淡画像を最尤に統合する高精度高安定なモデルフィッティング手法、”第１３回画像の認識・理解シンポジウム（ＭＩＲＵ２０１０）、ＯＳ５−１、２０１０。

＜ステップＳ５５０５＞
対応付け評価部５３０６は、対応付け部５３０５がサンプル点ごとに求めた対応間距離を評価することで、モデルフィッティングで用いる対応間距離を特定する。対応間距離の評価は、グループごとに行う。

同一グループ内では対応付けが正しいサンプル点群については対応間距離が類似することが期待できるため、対応付けで得られた対応間距離の分布の標準的な値(標準値)からそれぞれの対応間距離がどの程度離れているか（度合）を基準に外れ値を判定する。以下、判定方法の処理の詳細を説明する。

同一グループに属するサンプル点群から求めた対応間距離をＤｋ（ｋ＝１…ｌ：ｌは該グループに属するサンプル点（対応間距離）の個数）とする。そして、対応間距離の分布の標準的な値（基準距離）として、Ｄｋの中央値Ｄｍを設定する。そして、ｋ＝１…ｌを満たす全てのｋについて、｜Ｄｋ−Ｄｍ｜を計算し、この計算結果が閾値Ｔ以上（規定値以上）であれば外れ値として判定し、Ｄｋはモデルフィッティングで用いないと判定する。一方、｜Ｄｋ−Ｄｍ｜が閾値Ｔ未満（規定値未満）であれば非外れ値として判定し、Ｄｋはモデルフィッティングで用いると判定する。

なお、｜Ｄｋ−Ｄｍ｜はＤｋの分散値を基にしたマラハノビス距離として算出する。また、同一グループの対応間距離の分布の標準的な値の計量方法は、前述の中央値を設定する方法に限るものではなく、例えば平均値を用いても良い。

＜ステップＳ５５０６＞
位置姿勢推定部５３０７は、対応付け評価部５３０６によりモデルフィッティングで用いると判定された対応間距離を用いて、位置姿勢概略値入力部５３０３から取得した位置姿勢概略値を補正して、より精度の高い位置姿勢を推定する。補正量は対応間距離の最小二乗法の解を求めることで計算する。最小二乗法による位置姿勢推定アルゴリズムに関しては、ここでの本質に関わるものではないため、詳細な説明は省略する。処理の詳細については、上記の文献１を参照されたい。なお、位置姿勢の推定方法は最小二乗法に限るものではなく、対応付けを行った対応間距離を縮めるように位置姿勢を補正する方法であればどのような方法でも良い。

このように、本実施形態では、対応付けの結果をグループ毎に評価することで混入する外れ値を取り除き、その後、外れ値の取り除かれた対応間距離を用いて位置姿勢推定を行うので、精度とロバスト性の高い位置姿勢推定を行うことができる。

［第４の実施形態］
本実施形態では、同一グループ内で標準とするサンプル点の対応間距離Ｄｍからの差に応じて対応間距離に重み付けを行う。重みに応じて対応間距離を利用し位置姿勢推定を行うことで、精度とロバスト性の高い位置姿勢推定を行う。以下、重み付け処理の詳細を説明する。

同一グループに属するサンプル点群から求めた対応間距離をＤｋ（ｋ＝１…ｌ：ｌは該グループに属するサンプル点（対応間距離）の個数）とする。そして、対応間距離の分布の標準的な値として、Ｄｋの中央値Ｄｍを設定する。そして、ｋ＝１…ｌを満たす全てのｋについて、差｜Ｄｋ−Ｄｍ｜を計算し、該差から、例えば以下の式（８）に示すようなTukeyの関数によって決まる重み値を計算し、計算した重み値でＤｋを重み付けする。

ここで、ｗ（Ｄｋ）は、Ｄｋに対する重み値、Ｗは定数である。なお、重み値を与える関数はTukeyの関数である必要はなく、例えばHuberの関数など、中央値Ｄｍからの差が小さいＤｋには大きな重み値を与え、差が大きいＤｋには小さい重み値を与える関数であれば、如何なる関数でも良い。

そして、得られた重みに応じて各サンプル点の対応間距離を最小化するように、位置姿勢を補正する。重みに応じた位置姿勢の推定方法は、本発明の本質にかかわるものではないので、具体的な補正方法は、上記の文献１を参照されたい。

このように、本実施形態では、対応付けの結果をグループ毎に評価することで重み付けを行い、その後、重みに応じた対応間距離を用いて位置姿勢推定を行うので外れ値の影響を緩和し、精度とロバスト性の高い位置姿勢推定を行うことができる。

＜第３及び第４の実施形態の変形例１＞
第３及び第４の実施形態では、濃淡画像及び距離画像の両方を入力することを前提としていた。しかし、濃淡画像及び距離画像の一方だけを入力するようにしても良く、その場合、第３及び第４の実施形態で説明した処理は、入力した一方の画像について行えばよい。

＜第３及び第４の実施形態の変形例２＞
第３及び第４の実施形態では、処理の流れの中で、幾何特徴グループ分け部５３０４でサンプル点のグループ分類を行うようにしたが、このグループ分類は、視点ごとに予め行っておくようにしても良い。この場合、幾何特徴グループ分け部５３０４は、位置姿勢概略値入力部５３０３から取得した位置姿勢概略値に最も近い視点について予め行ったグループ分類の結果を取得することになる。視点ごとに予め行ったグループ分類結果は、形状モデル保持部５３０２等、適当なメモリに格納しておくことになる。

＜第３及び第４の実施形態の変形例３＞
第３及び第４の実施形態では、幾何特徴グループ分け部５３０４で、類似する対応間距離が予測されるサンプル点のグループ分け方法として、サンプル点のヤコビアンの類似度に従ってグループ分けを行う場合を説明した。ただし、サンプル点のグループ分けは、ヤコビアンの類似度に従う方法に限ったものではない。例えば、３次元形状モデルのラインあるいは面上に設定されているサンプル点群のうち、ラインあるいは面の角度や位置が類似するサンプル点群は、対応間距離の類似が予測されると仮定する。そして、ラインあるいは面の角度や位置が類似するサンプル点群が同一グループになるようにグループ分けしても良い。

＜第３及び第４の実施形態の変形例４＞
第３及び第４の実施形態では、対応付けを行う基準となる３次元形状モデルの幾何特徴として、３次元形状モデル上に設定したサンプル点を用いる。ただし、幾何特徴はサンプル点に限ったものではなく、画像上の特徴と３次元形状モデル上の特徴の対応関係が探索できるものであればどのような幾何特徴を利用しても良い。例えば、サンプル点ではなく３次元形状モデルのラインあるいは面そのものを幾何特徴として、画像上に存在する類似特徴との対応付けを行い、対応間距離を求めても良い。この場合、幾何特徴のグループ分けは第３の実施形態及びその変形例３で説明した方法と同様に、ラインあるいは面のヤコビアンや角度や位置が類似する幾何特徴が同一グループになるように分類する。

［第５の実施形態］
本実施形態では、第３及び第４の実施形態（変形例を含む）で説明した情報処理装置５３００の適用例について説明する。情報処理装置５３００の適用例として好適なものの１つに、図１４に示す如く、画像撮像装置５３１０により得られる２次元画像と距離画像を基に被計測物体の位置姿勢を推定し、産業用ロボットアームによりその把持などを行う例があげられる。以下では、図１４を用いて、この例について説明する。

図１４では、情報処理装置５３００とロボット５６２０とを用いて被計測物体５６１０を把持するロボットシステムの構成例を示している。ロボット５６２０はロボットコントローラ５６３０により制御され、ロボットコントローラ５６３０により指定された位置にロボットアームの手先を移動させ、ロボットコントローラ５６３０により指定された姿勢で物体の把持などを行うロボットである。

被計測物体５６１０は、作業台に置かれる位置や姿勢が変わるため、現在の被計測物体５６１０の位置姿勢を推定し、推定した位置にある物体を推定した姿勢で把持するための把持制御を行う必要がある。画像撮像装置５３１０は、産業用ロボットアームの手先等の被計測物体５６１０を撮像できる位置に設置する。

情報処理装置５３００は、画像撮像装置５３１０から得られる２次元画像と距離画像を基に被計測物体５６１０の位置姿勢を推定する。情報処理装置５３００で推定された被計測物体５６１０の位置姿勢は、ロボットコントローラ５６３０に入力（出力）される。ロボットコントローラ５６３０は、この入力された位置姿勢にある被計測物体５６１０を把持すべく、ロボット５６２０（ロボットアーム）を制御する。これにより、ロボットシステムは被計測物体の位置が不定でも位置姿勢推定を行うことで、被計測物体を把持することが可能となる。

［第６の実施形態］
図１１に示した、情報処理装置５３００を構成する各部は何れもハードウェアで構成しても良い。しかし、画像入力部５３０１、幾何特徴グループ分け部５３０４、位置姿勢概略値入力部５３０３、対応付け部５３０５、対応付け評価部５３０６、位置姿勢推定部５３０７の各部をソフトウェアで構成し、形状モデル保持部５３０２をメモリで構成しても良い。この場合、このメモリを有するＰＣ等のコンピュータに、このソフトウェア（コンピュータプログラム）をインストールし、このコンピュータのＣＰＵがこのソフトウェアを実行することで、これら各部の動作を実現することができる。

また、第３乃至６の実施形態（変形例を含む）では様々な情報処理装置やその応用例について説明したが、該情報処理装置は何れも、次のような構成の一例に過ぎない。即ち
被計測物体を含む画像に対し、該被計測物体の形状モデル上に設定されている複数の幾何特徴を対応付けることで、該被計測物体の位置姿勢を推定する情報処理装置である。

そしてこの情報処理装置は、形状モデル上の幾何特徴を複数のグループに分類（幾何特徴分類）し、画像から、形状モデル上の幾何特徴の対応を探索し、幾何特徴と該対応との対応間距離を求める（対応付け）。そして、幾何特徴のグループ毎に、対応の評価を行い（対応評価）、対応間距離と対応の評価とを基に、被計測物体の位置姿勢を推定する（位置姿勢推定）。なお、算出した位置姿勢に基づいて、回転及び／または並進移動軸からなる可動軸を有するロボットを操作する。また、推定するものは位置、姿勢の両方に限るものではなく、何れか一方だけも良い。

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

Claims (32)

1. 対象物体の３次元形状モデルを入力するモデル入力手段と、
   前記対象物体の３次元形状モデルに対して、前記対象物体と該対象物体を撮像する撮像装置との相対的な位置姿勢を仮想的に設定できるように、少なくとも１つの前記撮像装置を配置するための位置姿勢を設定する設定手段と、
   前記設定手段により設定された前記撮像装置の位置姿勢を選択する選択手段と、
   前記選択手段により選択された前記撮像装置の位置姿勢において得られるべき画像と前記３次元形状モデルの幾何特徴の関係に基づいて、幾何特徴のグループ分けを行うグループ分け手段と
   を備えることを特徴とするモデル生成装置。
2. 前記グループ分け手段は、前記選択手段により選択された前記撮像装置の位置姿勢または前記対象物体の位置姿勢が変化したときの、前記撮像装置において得られるべき画像の画像特徴と、前記３次元形状モデルの幾何特徴との関係が変化する度合に基づいて、幾何特徴のグループ分けを行うことを特徴とする請求項１に記載のモデル生成装置。
3. 前記グループ分け手段は、前記設定手段により設定された前記撮像装置の位置姿勢ごとに、前記撮像装置において得られるべき画像へ前記対象物体の３次元形状モデルを写像する関係において、前記幾何特徴のグループ分けを行う前記対象物体の３次元形状モデルの幾何特徴のヤコビアンを算出し、算出されたヤコビアンの類似度に基づいて、幾何特徴のグループ分けを行うことを特徴とする請求項１に記載のモデル生成装置。
4. 前記グループ分け手段は、前記設定手段で設定された位置姿勢ごとに、幾何特徴のグループ分けを行うことを特徴とする請求項１乃至３のいずれか１項に記載のモデル生成装置。
5. 前記設定手段は、前記対象物体の３次元形状モデルを球で覆い、該球に緯度、経度を割り当て、割り当てられた緯度、経度を利用して、仮想的に前記撮像装置の位置姿勢を設定することを特徴とする請求項１乃至４のいずれか１項に記載のモデル生成装置。
6. 前記得られるべき画像は、２次元画像及び／または距離画像であることを特徴とする請求項１乃至５のいずれか１項に記載のモデル生成装置。
7. 前記グループ分け手段の結果と前記設定手段によって設定された前記撮像装置の位置姿勢とを組にして、視点別モデルとして保存する視点別モデル保存手段を更に備えることを特徴とする請求項１乃至６のいずれか１項に記載のモデル生成装置。
8. 請求項１乃至７のいずれか１項に記載されたモデル生成装置と、
   撮像装置が撮像した画像を入力する画像入力手段と、
   前記グループ分け手段でグループ分けされたグループから、幾何特徴を選出する幾何特徴選出手段と、
   前記画像入力手段で入力された画像の特徴と前記幾何特徴選出手段で選出された幾何特徴とが当てはまるように、前記対象物体または前記撮像装置の位置及び姿勢を算出する算出手段とを備えることを特徴とする位置姿勢推定装置。
9. 前記画像入力手段で入力された画像の特徴とは、エッジ及び／または３次元点群であることを特徴とする請求項８に記載の位置姿勢推定装置。
10. 前記グループ分け手段は、前記幾何特徴選出手段で選出される幾何特徴の数と同じ数にグループ分けをすることを特徴とする請求項８または９に記載の位置姿勢推定装置。
11. 前記グループ分け手段は、前記撮像装置または前記対象物体の位置姿勢の変化に対する、前記画像の特徴と前記３次元形状モデルの幾何特徴との関係の変化の度合いが類似する幾何特徴をグループ分けし、
    前記幾何特徴選出手段は、前記グループ分けされた各グループのうち少なくとも２つ以上のグループからそれぞれ少なくとも１つ以上の幾何特徴を選出する
    ことを特徴とする請求項８乃至１０のいずれか１項に記載の位置姿勢推定装置。
12. 前記グループ分け手段は、前記撮像装置または前記対象物体の位置姿勢の変化に対する、前記画像の特徴と前記３次元形状モデルの幾何特徴との関係の変化の度合いが類似しない幾何特徴をグループ分けし、
    前記幾何特徴選出手段は、前記グループにグループ分けされた各グループのうち、１つのグループから幾何特徴を選出することを特徴とする請求項８乃至１０のいずれか１項に記載の位置姿勢推定装置。
13. 前記画像入力手段で入力された画像は、２次元画像及び／または距離画像であることを特徴とする請求項８乃至１２のいずれか１項に記載の位置姿勢推定装置。
14. 請求項８乃至１２のいずれか１項に記載された位置姿勢推定装置と、
    被計測物体を把持するための把持手段と、
    前記位置姿勢推定装置によって推定される前記被計測物体の位置姿勢に基づいて、前記把持手段を制御する制御手段と
    を備えることを特徴とする情報処理装置。
15. モデル生成装置が行うモデル生成方法であって、
    前記モデル生成装置のモデル入力手段が、対象物体の３次元形状モデルを入力するモデル入力工程と、
    前記モデル生成装置の設定手段が、前記対象物体の３次元形状モデルに対して、前記対象物体と該対象物体を撮像する撮像装置との相対的な位置姿勢を仮想的に設定できるように、少なくとも１つの前記撮像装置を配置するための位置姿勢を設定する設定工程と、
    前記モデル生成装置の選択手段が、前記設定工程で設定された前記撮像装置の位置姿勢を選択する選択工程と、
    前記モデル生成装置のグループ分け手段が、前記選択工程で選択された前記撮像装置の位置姿勢において得られるべき画像と前記３次元形状モデルの幾何特徴の関係に基づいて、幾何特徴のグループ分けを行うグループ分け工程と
    を備えることを特徴とするモデル生成方法。
16. 請求項１乃至７のいずれか１項に記載されたモデル生成装置を有する位置姿勢推定装置が行う位置姿勢推定方法であって、
    前記位置姿勢推定装置の画像入力手段が、撮像装置が撮像した画像を入力する画像入力工程と、
    前記位置姿勢推定装置の幾何特徴選出手段が、前記グループ分け工程でグループ分けされたグループから、幾何特徴を選出する幾何特徴選出工程と、
    前記位置姿勢推定装置の算出手段が、前記画像入力工程で入力された画像の特徴と前記幾何特徴選出工程で選出された幾何特徴とが当てはまるように、前記対象物体または前記撮像装置の位置及び姿勢を算出する算出工程とを備えることを特徴とする位置姿勢推定方法。
17. 請求項８乃至１２のいずれか１項に記載された位置姿勢推定装置を有する情報処理装置が行う情報処理方法であって、
    前記情報処理装置の制御手段が、前記位置姿勢推定装置によって推定される被計測物体の位置姿勢に基づいて、該被計測物体を把持するための把持手段を制御する制御工程を備えることを特徴とする情報処理方法。
18. 被計測物体を含む画像に対し、該被計測物体の形状モデル上に設定されている複数の幾何特徴を対応付けることで、該被計測物体の位置姿勢を推定する情報処理装置であって、
    前記形状モデル上の前記幾何特徴を複数のグループに分類する幾何特徴分類手段と、
    前記画像から、前記形状モデル上の前記幾何特徴の対応を探索し、前記幾何特徴と該対応との対応間距離を求める対応付け手段と、
    前記幾何特徴の前記グループ毎に、前記対応の評価を行う対応評価手段と、
    前記対応間距離と前記対応の評価とを基に、前記被計測物体の位置姿勢を推定する位置姿勢推定手段と
    を備えることを特徴とする情報処理装置。
19. 前記幾何特徴分類手段は、前記対応付け手段で得られる対応間距離が類似することが予想される前記幾何特徴が同一のグループになるように、前記幾何特徴の分類を行うことを特徴とする請求項１８に記載の情報処理装置。
20. 前記幾何特徴分類手段は、前記対応付け手段で得られる対応間距離が類似することが予想される前記幾何特徴が同一のグループになるように、前記被計測物体の位置姿勢の微小変化に対する幾何特徴の位置の変化の類似度に基づいて、前記幾何特徴の分類を行うことを特徴とする請求項１８又は１９に記載の情報処理装置。
21. 前記幾何特徴分類手段は、前記対応付け手段で得られる対応間距離が類似することが予想される前記幾何特徴が同一のグループになるように、前記形状モデル上の幾何特徴の位置及び／または幾何特徴を構成する線または面の方向の類似度に基づいて、前記幾何特徴の分類を行うことを特徴とする請求項１８又は１９に記載の情報処理装置。
22. 前記対応評価手段における前記幾何特徴の対応の評価方法として、前記幾何特徴分類手段で分類したグループ毎に前記幾何特徴の対応を誤対応かそうでないかで評価することを特徴とする請求項１８乃至２１の何れか１項に記載の情報処理装置。
23. 前記対応評価手段における前記幾何特徴の対応の評価方法として、前記幾何特徴分類手段で分類したグループ毎に前記幾何特徴の対応を誤対応の度合で評価することを特徴とする請求項１８乃至２１の何れか１項に記載の情報処理装置。
24. 前記対応評価手段における前記幾何特徴の対応の評価方法として、前記幾何特徴分類手段で分類したグループ毎に、同一グループに含まれる前記幾何特徴の対応間距離の標準値を計算し、前記幾何特徴それぞれの対応間距離と標準値との差に基づいて、各幾何特徴の対応を誤対応かそうでないかで評価することを特徴とする請求項１８乃至２２の何れか１項に記載の情報処理装置。
25. 前記対応評価手段における前記幾何特徴の対応の評価方法として、前記幾何特徴分類手段で分類したグループ毎に、同一グループに含まれる前記幾何特徴の対応間距離の標準値を計算し、前記幾何特徴それぞれの対応間距離と標準値との差に基づいて各対応を誤対応の度合いで評価することを特徴とする請求項１８乃至２１、請求項２３、の何れか１項に記載の情報処理装置。
26. 前記位置姿勢推定手段において、前記対応評価手段にて誤対応と評価した前記幾何特徴の対応を利用せず、前記位置姿勢推定手段で前記被計測物体の位置姿勢を推定することを特徴とする請求項１８乃至２１、請求項２２、請求項２４、の何れか１項に記載の情報処理装置。
27. 前記位置姿勢推定手段において、前記対応評価手段にて評価した誤対応の度合いに応じた前記幾何特徴の対応を重みに基づいて、前記位置姿勢推定手段で前記被計測物体の位置姿勢を推定することを特徴とする請求項１８乃至２１、請求項２３、請求項２５の何れか１項に記載の情報処理装置。
28. 更に、前記位置姿勢推定手段で算出した位置姿勢に基づいて、回転及び／または並進移動軸からなる可動軸を有するロボットを操作する操作手段を備えることを特徴とする請求項１８乃至２７の何れか１項に記載の情報処理装置。
29. 被計測物体を含む画像に対し、該被計測物体の形状モデル上に設定されている複数の幾何特徴を対応付けることで、該被計測物体の位置姿勢を推定する情報処理装置が行う情報処理方法であって、
    前記情報処理装置の幾何特徴分類手段が、前記形状モデル上の前記幾何特徴を複数のグループに分類する幾何特徴分類工程と、
    前記情報処理装置の対応付け手段が、前記画像から、前記形状モデル上の前記幾何特徴の対応を探索し、前記幾何特徴と該対応との対応間距離を求める対応付け工程と、
    前記情報処理装置の対応評価手段が、前記幾何特徴の前記グループ毎に、前記対応の評価を行う対応評価工程と、
    前記情報処理装置の位置姿勢推定手段が、前記対応間距離と前記対応の評価とを基に、前記被計測物体の位置姿勢を推定する位置姿勢推定工程と
    を備えることを特徴とする情報処理方法。
30. コンピュータを、請求項１乃至７の何れか１項に記載のモデル生成装置の各手段として機能させるためのコンピュータプログラム。
31. コンピュータを、請求項８乃至１３の何れか１項に記載の位置姿勢推定装置の各手段として機能させるためのコンピュータプログラム。
32. コンピュータを、請求項１４，１８乃至２８の何れか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。

Images