JP6323993B2 - 情報処理装置、情報処理方法、及びコンピュータプログラム - Google Patents

Description

本発明は、３次元形状を有する対象物体の位置姿勢を取得するための技術に関するものである。

距離撮像等により対象物体の表面位置から取得される三次元点と、対象物体の３次元形状モデルから抽出される幾何特徴との対応を求め、その対応間距離を評価関数として対象物体の位置姿勢を推定するModel-Based Visionがある。この技術では、位置姿勢のパラメータを推定するために、あらかじめ対象物体の３次元形状モデルが必要となる。この３次元形状モデルとして、CADソフトによる設計データを用いる方法、対象物体の形状を模した（近似した）ポリゴンモデルを利用する方法、がある。

物体の形状を表すCADデータは、パラメータで表現される部分形状を複数組み合わせて形状を表現する。各部分形状は、面とその輪郭を定義する曲線で構成され、各面は、円錐、トーラス曲面などの解析曲面、あるいはB-Spline曲面やNURBS曲面などの自由曲面で表現される。これらのパラメータで表現される面は少ない情報量で複雑な曲面形状を高精度に表現できるという長所をもつ。したがって、物体の位置姿勢推定において、CADデータの持つ各面のパラメータを利用すれば、モデル上の幾何特徴を高精度に算出できるため、処理を精度よく行うことが可能である。しかし、その反面、面上の点の算出や３次元点との対応探索において、NURBS曲面のような自由曲面では複雑な幾何計算を必要とする。さらに、輪郭線との内外判定においても煩雑な計算を要するため、処理時間がかかるという問題がある。

一方、ポリゴンモデルは、多角形の面の組み合わせで物体形状を表現するものである。ポリゴンモデルでは、面上の点の算出や３次元点との対応探索を単純な幾何計算で行えるため、計算の負荷が低いという長所を持つ。その反面、物体形状を多角形近似するため、物体の形状とモデルとに誤差が生じる。この誤差をなくすためには、面を非常に小さなポリゴンの集合で近似する方法がある。しかし、必要となるポリゴンの総数は許容する誤差に対して指数的に増大するため現実的ではない。

これら、各モデルの持つ問題への対策として、CADデータのようなパラメータ表現のモデルとポリゴンモデルとを組み合わせることで、高速かつ正確に処理を行う方法がある。

たとえば、非特許文献１では、基準面の格子上の各頂点について、基準面から曲面形状までの距離値を算出する際に、曲面形状を表現するパラメータと、形状を近似したポリゴンモデルとをペアにしたモデルを保持しておき、利用する方法が開示されている。この方法では、まず、各格子点から基準面に垂直な方向に向かう直線と、ポリゴンとの交点座標を導出し、その後、交点から曲面形状までの距離を曲面パラメータから算出することで、頂点から曲面形状までの距離を補正している。

特開2009-020671号公報

上記の方法では、基準面の各格子点と曲面との距離算出において先ずポリゴンを介して高速に近似距離を算出した後、ポリゴンと関連付けられた曲面との高精度な距離を曲面パラメータから算出して近似距離を補正することで、高速化と高精度化を両立している。この方法では、基準面の各格子点と、モデルとの対応関係を求めているが、任意の３次元空間上の点との対応探索は行っていないため、対象物体の距離画像から得られる３次元点群とモデルとの位置合わせにはそのまま適用できない。

一方で、ポリゴンモデル、曲面モデルとを目的に応じてそれぞれ使い分けるような方法もあるが、この場合、高速化と高精度化とを両立して対応間距離を算出することは不可能である。

本発明は、このような問題に鑑みてなされたものであり、対象物体と、該対象物体の形状を模したモデルと、の間の対応間距離を高速かつ高精度に算出する為の技術を提供することを目的とする。

本発明の目的を達成するために、例えば、本発明の情報処理装置は、
対象物体を含む画像を取得する画像取得手段と、
前記対象物体の概略の位置姿勢を示す概略位置姿勢を取得する手段と、
前記対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報を保持する保持手段と、
繰り返し行われる推定の過程で計算される指標値と閾値との大小関係に応じて、前記複数のモデル情報のうちの何れか１つを選択モデル情報として選択する選択手段と、
前記選択モデル情報が示すモデルを、前記概略位置姿勢が示す位置姿勢で配置し、前記対象物体の表面上のそれぞれの幾何特徴と、該配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を求める対応付け手段と、
前記対象物体の表面上のそれぞれの幾何特徴について求めた距離に基づいて、前記対象物体の位置姿勢を推定する推定手段と
を備えることを特徴とする。

本発明の構成によれば、対象物体と、該対象物体の形状を模したモデルと、の間の対応間距離を高速かつ高精度に算出することができる。

情報処理装置１の機能構成例を示すブロック図。 ポリゴンモデル及び区分パラメトリック曲面モデルを説明する図。 情報処理装置１が行う処理のフローチャート。 情報処理装置１が行う処理のフローチャート。 情報処理装置２の機能構成例を示すブロック図。 情報処理装置２が行う処理のフローチャート。 モデル座標系における３次元点を示す図。 情報処理装置２が行う処理のフローチャート。 システムの構成を示す図。 システムの動作を示すフローチャート。

以下、添付図面を参照し、本発明の好適な実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の１つである。

いくつかの実施形態によれば、対象物体と、該対象物体の形状を模したモデルと、の間の対応間距離を高速かつ高精度に算出することができる。

いくつかの実施形態においては、対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報から、処理において使用されるモデル情報が選択される。処理に適したモデル情報を選択することにより、対象物体と、該対象物体の形状を模したモデルと、の間の対応間距離を高速かつ高精度に算出することができる。具体的には、実施形態１においては、例えば残差又は補正回数等に基づいた規定の条件に従って、使用するモデル情報が切り換えられる。実施形態１及びその変形例においては、位置姿勢情報を反復演算により補正する際に、それぞれの繰り返しにおいて異なったモデル情報を用いることができる。また、実施形態１及びその変形例においては、対応間距離を算出する際に、対象物体の各点について異なったモデル情報を用いることができる。また、実施形態２及びその変形例においては、対象物体とモデル表面との対応関係を判定するためのモデル情報と、対象物体とモデル表面との距離を算出するための別のモデル情報とを、それぞれ選択することができる。このように以下の実施形態においては、対象物体と、該対象物体の形状を模したモデルと、の間の位置合わせを行う際に、複数のモデル情報を同時に用いて処理を行うことができる。

［第１の実施形態］
本実施形態では、位置姿勢を求める対象となる対象物体の距離画像から得られる３次元点群（対象物体の表面上の点群）と、該対象物体の形状を近似したモデルと、を用いて対象物体の位置姿勢を求める処理に関するものである。その際、本実施形態では、対象物体の形状をポリゴン群を用いて近似したポリゴンモデルと、対象物体の形状をパラメトリック曲面を用いて近似した区分パラメトリック曲面モデルと、を３次元点ごとに切り替えて使用する。

ポリゴンモデルを用いると、対象物体の表面上のそれぞれの３次元点に対応する対応点の探索を、区分パラメトリック曲面モデルよりも高速に行うことができる。しかしポリゴンモデルの形状には、対象物体の形状に対する近似誤差が含まれてしまうため、対象物体とポリゴンモデルとの間では、対応する点間の距離を正確に算出することはできない。

一方、区分パラメトリック曲面モデルを用いれば、上記の距離をより高精度に算出することができる。しかし、各パラメトリック曲面上で物体形状を表す有効領域の判定が必要となるため、計算に時間がかかる。

そこで、本実施形態では、現時点で使用しているモデルと３次元点群とのずれが大きい場合には、次に使用するモデルとしてポリゴンモデルを選択することで、高速に位置姿勢算出を行う。逆に、現在使用しているモデルと３次元点群とのずれが小さい場合には、次に使用するモデルとして区分パラメトリック曲面モデルを選択することで、高精度に位置姿勢算出を行う。

なお、本実施形態では、物体表面上の三次元点とモデル表面との対応間距離を最小化するように位置姿勢の補正を繰り返すことで位置合わせを実施する。そこで、位置姿勢のフィッティング度合いを測る指標として、位置合わせにおける各三次元点の残差を利用する。ここで残差とは、補正された位置姿勢における、三次元点と、三次元形状モデル面上の点との距離である。残差が大きい時には、推定された位置姿勢のずれが大きいと判断し、残差が小さい時には位置姿勢のずれが小さいと判断する。これにより、各三次元点の残差に応じて、三次元点ごとにポリゴンモデルと区分パラメトリックモデルを切り替えることによって、処理速度と精度を両立して位置姿勢の算出が可能となる。

先ず、本実施形態に係る情報処理装置１の機能構成例について、図１のブロック図を用いて説明する。モデル保持部１１は、位置姿勢の計測対象となる対象物体の形状をポリゴン群で近似したモデルであるポリゴンモデルのモデル情報と、該対象物体の形状をパラメトリック曲面で近似したモデルである区分パラメトリック曲面モデルのモデル情報と、を保持している。ここで、ポリゴンモデルと区分パラメトリック曲面モデルについて、図２を用いて説明する。図２には同じ対象物体に対する区分パラメトリック曲面モデル（図２（ａ））、ポリゴンモデル（図２（ｂ））を示している。

区分パラメトリック曲面モデルとは、図２（ａ）に示す如く、対象物体を、境界線で区分されたパラメトリック表現の面（区分パラメトリック面）を複数個組み合わせて高精度に表現するモデルである。本実施形態では、区分パラメトリック面として、トリム曲面と呼ばれる面を扱う。トリム曲面は、基本形状を表すベース曲面と、ベース曲面における有効領域を定義する輪郭曲線とを表現するパラメータによって定義される面である。この表現方法では、３次元空間中の点Ｐの座標（ｘ，ｙ，ｚ）は、平面内の領域を動く２つのパラメータｕ，ｖにより、３つの連続関数ｘ＝ｆ（ｕ，ｖ）、ｙ＝ｇ（ｕ，ｖ）、ｚ＝ｈ（ｕ，ｖ）によって表現される。この表現で表わされるベース曲面に対して、ｕｖ平面上の曲線で有効な領域を定義することで、対象物体の部分形状を表現する。図２（ａ）における斜線部は、トリム曲面の一例を示している。なお、トリム曲面のベース曲面は、球体や円錐、トーラス曲面などの解析曲面、あるいはB-Spline曲面やNURBS曲面、ベジェ曲面などの自由曲面で表現される場合がある。一般的なCADデータは、上述した区分パラメトリック曲面モデルにより構成されている。したがって、IGES、STEPなどの規格化されたフォーマットのデータで、対象物体のCADデータが入手可能な場合には、そのデータを区分パラメトリック曲面モデルとしてそのまま利用することが可能である。ただし、区分パラメトリック曲面モデルとして、CADデータ以外のものを用いてもかまわない。たとえば、対象物体を区分された陰関数曲面の組み合わせにより表現するようなモデルでもかまわない。このほか、対象物体の形状をパラメータによる数値計算で高精度に表現でき、かつ、３次元座標値が与えられている点（３次元点）から最短距離に位置する面上の点およびその距離が算出可能なものであれば、ベース曲面はいかなるものであってもかまわない。

次に、ポリゴンモデルについて説明する。ポリゴンモデルとは、図２（ｂ）に示す如く、対象物体の形状を三角形のポリゴンの集合で近似表現したモデルである。ポリゴンモデルは、CADデータから曲面上の点を離散的にサンプリングし、これらの点を連結することで取得可能である。ポリゴンモデルは、各ポリゴンの頂点の３次元座標値と、各ポリゴンを構成する頂点の連結情報によって表わされる。なお、ポリゴンは三角形でなくても四角形でも五角形でもよい。その他、面の頂点の座標と、その面を構成する頂点の連結情報により表現されるものであればいかなるものであってもかまわない。

物体表面位置取得部１２は、対象物体の表面上のそれぞれの点（３次元点）の位置（３次元座標値を有する）を取得する。本実施形態では、距離センサを用いて対象物体を撮像することで得られる距離画像の各画素の座標位置と該画素の画素値（奥行き値）とから、該画素に対応する３次元点の位置を求める。距離センサとしては、対象物に対して照射したレーザ光やスリット光・ランダムドットパターン光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。アクティブ式の距離計測方法としては、空間符号化法を用いても良い。さらに、距離センサはこれに限るものではなく、光の飛行時間を利用するTime-of-flight方式のものであってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行き値を計算するパッシブ式であってもよい。ただし、対象物体の表面上の３次元点群の位置を取得する方法は上記の方法に限るものではなく、様々な方法を適用することができる。例えば、プローブで接触することにより対象物体の表面上の３次元点群の位置を取得するような接触式三次元測定装置を用いて該３次元点群を取得してもよい。

概略位置姿勢入力部１３は、対象物体の概略の位置姿勢を示す位置姿勢情報を対応間距離算出部１５及び位置姿勢算出部１６に対して供給する。そして、該位置姿勢情報は後述する処理により位置姿勢算出部１６によって補正されるので、概略位置姿勢入力部１３は、この補正された位置姿勢情報を取得すると、該取得した位置姿勢情報を対応間距離算出部１５及び位置姿勢算出部１６に対して再び供給する。なお、対応間距離算出部１５及び位置姿勢算出部１６に対して供給する位置姿勢情報は、前回位置姿勢算出部１６によって補正された位置姿勢情報に限るものではない。例えば、過去の位置及び姿勢の算出結果をもとに位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測したものでもよい。また、対象物体が置かれているおおよその位置や姿勢があらかじめわかっている場合にはその値を概略の位置姿勢として用いてもよい。また、計測対象の物体に磁気式あるいは光学式の位置姿勢センサを取りつけて、その位置姿勢を計測しても良いし、物体に所定形状のマーカを取り付けて、それをカメラで撮影して位置姿勢を算出することもできる。

なお、対象物体の位置姿勢や、３次元点群の位置は、特に触れない限りは、上記の距離センサの位置を原点とし、距離画像の水平方向をｘ軸、垂直方向をｙ軸、距離センサの光軸をｚ軸とした座標系（基準座標系）における位置姿勢、位置であるとする。また、モデルには、該モデルに対して与えられた１点を原点とし、該原点で互いに直交する３軸をそれぞれ、ｘ、ｙ、ｚ軸とする座標系（モデル座標系）が与えられている。

モデル選択部１４は、対象物体の表面上のそれぞれの３次元点に対し、ポリゴンモデル情報及び区分パラメトリック曲面モデル情報のうち、該３次元点について適合度算出部１７が求めた適合度に応じて決まる一方を、選択モデル情報としてモデル保持部１１から選択する。

対応間距離算出部１５は、対象物体の表面上のそれぞれの３次元点（幾何特徴）に対し、該３次元点と、該３次元点に対して選択された選択モデル情報が示すモデルを位置姿勢情報が示す位置姿勢で配置した場合の該モデルにおいて該３次元点に対応する対応点（幾何特徴）と、の間の距離を求める。モデルがポリゴンモデルであれば、３次元点とポリゴンとの対応を探索することで３次元点とポリゴン上の１点との間の距離を求める。一方、モデルが区分パラメトリック曲面モデルであれば、点と曲面との対応を探索することで、対応する曲面と３次元点との間の距離を求める。

位置姿勢算出部１６は、対応間距離算出部１５がそれぞれの３次元点について求めた距離（対応間距離）に基づく評価値を最適化するように、現在の位置姿勢情報を補正する。

適合度算出部１７は、対象物体の表面上のそれぞれの３次元点の残差を適合度として求める。各３次元点の残差は、該３次元点について選択した選択モデル情報に従うモデルを、位置姿勢算出部１６が補正した位置姿勢情報が示す位置姿勢で配置し、該３次元点と、該配置したモデルにおいて該３次元点に対応する対応点と、の間の距離である。

次に、情報処理装置１が、上記の反復演算により位置姿勢情報を繰り返し補正することで対象物体の位置姿勢を求める行う処理について、同処理のフローチャートを示す図３を用いて説明する。

＜ステップＳ３０２における処理について＞
物体表面位置取得部１２は、位置姿勢の計測対象となる対象物体の表面上のそれぞれの点（３次元点群）の位置（３次元座標値を有する）を、上記の如く、距離画像から取得する。以下の説明では、Ｎ（Ｎは２以上の整数）個の３次元点ｋｃ＿ｉ（ｉ＝１，２，…，Ｎ）の基準座標系における位置を取得したとする。また、距離画像中の画素と、該画素に対応する３次元点との対応関係は既知であるものとする。

＜ステップＳ３０３における処理について＞
以下に説明するように、本実施形態では、はじめに対象物体の概略の位置姿勢を示す位置姿勢情報を与え、これを繰り返し補正することで、対象物体の位置姿勢を求める。然るに、ステップＳ３０３において概略位置姿勢入力部１３は、この繰り返し処理の初期値として、対象物体の、基準座標系における概略の位置姿勢を示す位置姿勢情報を取得する。

＜ステップＳ３０４における処理について＞
適合度算出部１７は、対象物体の表面上のそれぞれの３次元点に対する適合度としての残差ｅｒ＿ｉ（ｉ＝１，２，…，Ｎ）を初期化する。各残差の初期値には十分大きな値（例えば１０００）を設定する。

＜ステップＳ３０５における処理について＞
モデル選択部１４は、Ｎ個の３次元点のうちまだ選択されていない３次元点を１つ選択する。

＜ステップＳ３０６における処理について＞
モデル選択部１４は、ステップＳ３０５で選択した３次元点ｋｃ＿ｉの残差ｅｒ＿ｉと、閾値θとの大小比較を行う。閾値θには、ポリゴンモデルと区分パラメトリック曲面モデルとの形状の誤差の値を設定しておく。たとえば、区分パラメトリック曲面モデルとしてCADデータを利用し、CADデータにテッセレーション処理を施してポリゴンモデルを作成する場合、生成されるポリゴンと曲面との距離の許容値を、ポリゴン近似誤差として設定する。このポリゴン近似誤差を閾値θに用いてもよい。

この大小比較の結果、ｅｒ＿ｉ≧θであれば、この時点での位置姿勢情報にはまだ大きな誤差が含まれていると考えられる。したがってこのような場合には、モデル選択部１４は、３次元点ｋｃ＿ｉについて対応間距離を求めるために用いるモデルとして、幾何計算が高速に行えるポリゴンモデルを、モデル保持部１１から選択する。

一方、ｅｒ＿ｉ＜θであれば、この時点での位置姿勢情報における３次元点の残差がモデル近似誤差と同程度（未満）であると考えられる。したがってこのような場合には、モデル選択部１４は、３次元点ｋｃ＿ｉについて対応間距離を求めるために用いるモデルとして、高精度な形状を表現する区分パラメトリック曲面モデルを、モデル保持部１１から選択する。

＜ステップＳ３０７における処理について＞
対応間距離算出部１５は、現在の位置姿勢情報が示す位置姿勢に、３次元点ｋｃ＿ｉについて選択したモデルを配置し、３次元点ｋｃ＿ｉと、該配置したモデルにおいて該３次元点に対応する対応点ｐｍ＿ｉと、の間の距離（対応間距離）ｄ＿ｉを求める。ここで、３次元点ｋｃ＿ｉについて選択されたモデルがポリゴンモデルと区分パラメトリック曲面モデルとで、対応間距離を求める方法が異なる。以下では、ポリゴンモデル、区分パラメトリック曲面モデルのそれぞれが選択された場合における対応間距離算出方法について説明する。

＜＜ポリゴンモデルが選択された場合について＞＞
まず、ポリゴンモデルから、３次元点ｋｃ＿ｉに対応するポリゴンを探索する。ここでは、以下の文献に開示されている方法と同様に、ポリゴンモデルを距離画像上に投影することによって対応探索を高速に行う。

[大石，中澤，池内“インデックス画像を用いた複数距離画像の高速同時位置合せ”，vol.J89-D，no.3，pp.513-521，2006．］
まず現在の位置姿勢情報が示す位置姿勢にポリゴンモデルを配置し、該配置したポリゴンモデルを、校正済みである距離センサの内部パラメータ（透視投影パラメータ）に基づいて、距離画像と同サイズの画像バッファ（情報処理装置１内のメモリ）に描画する。このとき、ポリゴンごとに固有の色を割り当てて描画を行う。これにより、距離画像の各画素（と同位置）に投影されたポリゴンを、色によって同定することができる。なお、ポリゴンの描画処理をGPU（Graphics Processing Unit）上で高速に行い、描画された結果の画像を読み出してもよい。次に、距離画像上の画素に対応する３次元点ｋｃ＿ｉと、該画素に投影されたポリゴンと、を対応付ける。

次に、３次元点ｋｃ＿ｉの位置をポリゴンモデルのモデル座標系における位置ｋｍ＿ｉに変換し、位置ｋｍ＿ｉから３次元点ｋｃ＿ｉと対応付けられたポリゴンにおろした垂線の足の位置を、対応点ｐｍ＿ｉの位置として探索する。そして、位置ｋｍ＿ｉと対応点ｐｍ＿ｉの位置との間の距離を距離ｄ＿ｉとして求める。

なお、３次元点ｋｃ＿ｉとポリゴン上の点との対応付け方法はこの方法に限らない。たとえば、normal shootingと呼ばれる、３次元点から法線方向へ交差するポリゴンを探索する方法を用いてもよい。この方法では、距離画像の隣接画素の３次元点を連結したメッシュから各３次元点の法線を算出しておく。そして、概略位置姿勢に基づいてポリゴンモデルと３次元点との座標系を合わせた上で、各３次元点から法線方向の直線上で最初に交差するポリゴンを探索し、交わったポリゴンをその３次元点と対応付ける。あるいは、ポリゴン上から点を均一にサンプリングしておき、概略位置姿勢に基づき、ポリゴンモデルと３次元点との座標系を合わせた上で、３次元点と最近傍にある点が属するポリゴンを対応付ける最近傍探索を用いてもよい。なお、normal shootingや最近傍探索による対応付けは、各３次元点や各ポリゴンを包含するBounding Boxを分類するkd-treeを事前に構築しておき、kd-treeを用いて探索を行うことで処理の高速化が可能である。あるいは、ポリゴンモデルのポリゴン上から点を均一にサンプリングしておき、これらの点を概略位置姿勢に基づいて距離画像面上に投影し、投影された画素にリンクされている３次元点と、サンプリング元のポリゴンとを対応付けてもよい。このほか、対応付け方法は高速に処理できるものが望ましいが、３次元点とポリゴンモデルのポリゴンとを一意に対応付ける方法であればいかなる方法であってもかまわない。

＜＜区分パラメトリック曲面モデルが選択された場合について＞＞
区分パラメトリック曲面モデルから、３次元点ｋｃ＿ｉと対応するトリム曲面を探索し、３次元点ｋｃ＿ｉから曲面までの距離を算出する。ここで、区分パラメトリック曲面モデルを構成するトリム曲面の総数をＭとする。まず、Ｍ個のトリム曲面に対し、各曲面のパラメータを用いて、モデル座標系に変換した位置ｋｍ＿ｉと、その曲面との最短距離および、最短距離に位置する曲面上の点をそれぞれ算出する。ここでは、このとき、算出された点は物体形状の有効領域内に存在するように算出する。なお、これらの算出には、３次元点ｋｃ＿ｉと曲面との対応点および対応間距離を曲面パラメータから算出する一般的な方法を用いればよい。ここで、ｊ番目のトリム曲面と３次元点ｋｃ＿ｉとの最短距離をｄｔ＿ｊ、その距離に位置する点をｐｔ＿ｊとする。次に、Ｍ個のトリム曲面の中から、ｄｔ＿ｊの値が最小のトリム曲面を選択する。そして、選択されたトリム曲面と３次元点ｋｃ＿ｉとの距離をｄ＿ｉ、その距離に位置する点の位置を対応点ｐｍ＿ｉの位置としてそれぞれ求める。

＜ステップＳ３０８における処理について＞
ステップＳ３０８では、モデル選択部１４は、Ｎ個全ての３次元点を選択したか否かを判断する。この判断の結果、Ｎ個全ての３次元点を選択したのであれば、処理はステップＳ３０９に進み、まだ選択していない３次元点が残っている場合には、処理はステップＳ３０５に戻る。

＜ステップＳ３０９における処理について＞
位置姿勢算出部１６は、距離画像から得られた３次元点群に対し、３次元点ごとに選択されたモデルが３次元空間中で当てはまるように、位置姿勢情報を補正する。本実施形態では、非線形最適化手法を用いて、位置姿勢情報を反復演算により補正することで対象物体の位置姿勢を示す位置姿勢情報を求める。

ステップＳ３０９では、ICP(Iterative Closest Point)法を用いて、距離画像から得られた三次元点群に対して、三次元形状モデルが三次元空間中で当てはまる対象物体の位置姿勢を算出する。

３次元幾何モデルの表面点群をPとする。

Ｐ＝｛ｐｍ＿１，ｐｍ＿２，…，ｐｍ＿Ｎ｝ （１）
距離画像点群をＫとする。

Ｋ＝｛ｋｍ＿１，ｋｍ＿２，…，ｋｍ＿Ｎ｝ （２）
３次元幾何モデルの表面点群Pを変換して距離点群Kに位置合わせする。点群Ｐの各点ｐiに距離が最も近い点群Kの点をｂi∈Kとしたとき、（３）の誤差関数を定義できる。Ｒとｔはそれぞれ姿勢パラメータと移動ベクトルである。

誤差関数Ｅを小さくするＲとｔを求め、（４）による補正を行う。
Ｐ←ＲＰ＋ｔ （４）

誤差関数Ｅを小さくするＲとｔを求める方法については、以下の文献に載っている。
Ｋ．Ｓ．Ａｒｕｎ，Ｔ．Ｓ．Ｈｕａｎｇ，ａｎｄ Ｓ．Ｄ．Ｂｌｏｓｔｅｉｎ，“Ｌｅａｓｔ−Ｓｑｕａｒｅｓ Ｆｉｔｔｉｎｇ ｏｆ Ｔｗｏ ３−Ｄ Ｐｏｉｎｔ Ｓｅｔｓ，” ＰＡＭＩ，ｖｏｌ．９，ｎｏ．５，１９８７

本ステップでは、ICP法ではなく、Gauss-Newton法などの非線形最適化手法を用いて対象物体の位置姿勢を算出しても良い。その場合、ステップＳ３０７で算出した、各三次元点kc_iの持つ対応間距離d_iおよび対応点pm_iに基づいて、位置姿勢の最適化を行う。具体的には、ステップＳ３０７で算出した距離d_iを、位置及び姿勢の微小変化の関数としてpm_iを用いた線形近似により表現し、この距離が0となるように各計測データについて線形方程式を立式する。

そして、線形方程式を連立方程式として解くことにより、対象物体の位置及び姿勢の微小変化を求め、位置及び姿勢を補正する。なお、非線形最適化手法はGauss-Newton法に限るものではない。例えば、より計算がロバストであるLevenberg-Marquardt法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やICCG法など、他の非線形最適化手法を用いてもよい。

なお、ここでは、距離値を評価関数として最小化するように位置姿勢の補正を行ったが、最適化の方法はこの限りでない。例えば、[立野，小竹，内山“ビンピッキングのための距離・濃淡画像を最ゆうに統合する高精度高安定なモデルフィッティング手法”，vol.J94-D，no.8，pp.1410-1422，2011．］に記載されているような方法を用いても良い。即ち、この方法により、距離に基づいて算出した尤度を評価値として最大化するように位置姿勢の補正を行ってもよい。このほか、非線形最適化手法の方法は、評価値を最適化するように位置姿勢を補正するものであれば、いかなる方法であってもかまわない。

また、上記説明では、位置姿勢情報の補正量を未知パラメータとして求め、求めた補正量に基づき、概略位置姿勢の補正を繰り返すことで最終的な位置姿勢を算出する反復解法について主に述べた。しかし、距離センサに対する対象物体の位置姿勢を表現する6自由度を未知パラメータとして、概略位置姿勢に基づいて探索した対応結果から、直接未知パラメータを算出してもよい。この方法では、概略位置姿勢に基づく多数の対応関係から、最小二乗法を用いて最終的な位置姿勢を直接算出する。

＜ステップＳ３１０における処理について＞
適合度算出部１７は、各３次元点ｋｃ＿ｉの残差ｅｒ＿ｉを求める。適合度算出部１７は、３次元点ｋｃ＿ｉについて選択した選択モデル情報が示すモデルを、ステップＳ３０９で補正した位置姿勢情報が示す位置姿勢で配置し、該３次元点ｋｃ＿ｉと、該配置したモデルにおいて該３次元点ｋｃ＿ｉに対応する対応点と、の間の距離を求める。この求めた距離が、３次元点ｋｃ＿ｉに対する残差ｅｒ＿ｉとなる。

即ち補正された位置姿勢情報が示す位置姿勢に３次元点ｋｃ＿ｉについて選択したモデルを配置し、該モデルのモデル座標系に変換した３次元点ｋｃ＿ｉの位置ｋｍ＿ｉ’とステップＳ３０７で求めた対応点ｐｍ＿ｉの位置との距離を残差ｅｒ＿ｉとして求める。

＜ステップＳ３１１における処理について＞
位置姿勢算出部１６は、上記の反復演算が収束しているか否かを判断する。この判断の結果、収束していると判断した場合には、この反復演算を終了し、この時点で求まっている位置姿勢情報を、対象物体の位置姿勢を示す位置姿勢情報として出力する。出力先については特定の出力先に限るものではなく、情報処理装置１内／外の適当なメモリに対して出力しても良いし、外部の適当な装置に対して出力しても良い。一方、まだ収束していないと判断した場合には、処理はステップＳ３０５に戻る。

収束判定には、たとえば、全３次元点の残差の和あるいは位置姿勢の補正量が所定値以下となった場合に収束したと判定してもよいし、位置姿勢の補正処理を所定回数以上実施したら収束したと判定してもよい。なお、収束判定方法は特定の方法に限るものではなく、如何なる方法を採用しても良い。

このように、本実施形態によれば、残差が大きいものにはポリゴンモデルを、小さいものには区分パラメトリック曲面モデルを用いることで、フィッティング度合いに応じて高速化と高精度化とを効率よく切り替えた位置姿勢算出が可能となる。

＜第１の実施形態の変形例１＞
第１の実施形態では、３次元点ごとにモデル選択を行っていた。これに対し変形例１では、３次元点ごとの残差の総和と閾値の大小比較に基づいてモデル選択を行う。すなわち、変形例１では、１つの対象物体に対して、ポリゴンモデル、区分パラメトリック曲面モデルの何れか１つが選択されることになる。

本変形例でも、図１に示した構成を有する情報処理装置１を用いるが、下記の点が第１の実施形態とは異なる。以下では、第１の実施形態と異なる点について重点的に説明し、以下で特に触れない限りは、第１の実施形態と同様であるとする。

本実施形態に係る情報処理装置１が、対象物体の位置姿勢を求めるために行う処理について、同処理のフローチャートを示す図４を用いて説明する。なお、図４において、ステップＳ４０２，Ｓ４０３，Ｓ４０７，Ｓ４０９はそれぞれ、図３のステップＳ３０２，Ｓ３０３，Ｓ３０９，Ｓ３１１と同様の処理ステップであるため、これらの処理ステップに係る説明は省略する。然るに以下では、ステップＳ４０４〜Ｓ４０６、Ｓ４０８のそれぞれの処理ステップについて説明する。

＜ステップＳ４０４における処理について＞
適合度算出部１７は、対象物体に対する適合度Ｅｒｒを初期化する。該適合度の初期値には、想定される位置姿勢残差よりも極端に大きい値（例えば１０００）を設定する。本実施形態では、対象物体に対する適合度は、該対象物体の表面上のそれぞれの３次元点の残差の総和であるとする。

＜ステップＳ４０５における処理について＞
モデル選択部１４は、適合度Ｅｒｒと、閾値Ψとの大小比較を行う。閾値Ψには、ポリゴンモデルと、区分パラメトリック曲面モデルと、の形状の誤差に基づいて値を設定しておくのがよい。たとえば、区分パラメトリック曲面モデルとしてCADデータを利用し、CADデータにテッセレーション処理を施してポリゴンモデルを作成する場合、第１の実施形態で用いたθの値を利用してΨ＝N×θとして設定する。

この大小比較の結果、Ｅｒｒ≧Ψであれば、３次元点群と、選択されているモデルと、にはまだまだ大きなずれがあると考えられる。したがってこのような場合には、モデル選択部１４は、対応間距離算出部１５が使用するモデルとして、幾何計算が高速に行えるポリゴンモデルを、モデル保持部１１から選択する。

一方、Ｅｒｒ＜Ψであれば、３次元点群と、選択されているモデルと、のずれは、モデル近似誤差と同程度であると考えられる。したがってこのような場合には、モデル選択部１４は、対応間距離算出部１５が使用するモデルとして、高精度な形状を表現する区分パラメトリック曲面モデルを、モデル保持部１１から選択する。

＜ステップＳ４０６における処理について＞
対応間距離算出部１５は、現在の位置姿勢情報が示す位置姿勢に、ステップＳ４０５で選択したモデルを配置し、各３次元点について、該３次元点と、該配置したモデルにおいて該３次元点に対応する対応点と、の間の距離（対応間距離）ｄ＿ｉを求める。対応点間距離を求める為の処理については、第１の実施形態で説明したとおりである。

＜ステップＳ４０８における処理について＞
適合度算出部１７は、第１の実施形態で説明したように各３次元点ｋｃ＿ｉの残差ｅｒ＿ｉを求め、その合計値を適合度Ｅｒｒとして求める。

＜第１の実施形態の変形例２＞
第１の実施形態の変形例１では、残差の総和を適合度として用いた。しかし、適合度に適用できる指標には、他にも様々なものが考え得る。本変形例では、位置姿勢情報の補正回数を適合度に用いた場合（ケース１）、位置姿勢情報の変化量（補正量）を適合度に用いた場合（ケース２）、について説明する。

本変形例でも、図１に示した構成を有する情報処理装置１を用いるが、下記の点が第１の実施形態とは異なる。以下では、第１の実施形態と異なる点について重点的に説明し、以下で特に触れない限りは、第１の実施形態と同様であるとする。本変形例に係る情報処理装置１が、対象物体の位置姿勢を求めるために行う処理は、図４のフローチャートに従った処理において、下記の点を変更したものである。

先ずケース１について説明する。ステップＳ４０４では、適合度算出部１７は、位置姿勢情報の補正回数（補正演算処理の実行回数）を示す変数ｋを０に初期化する。次に、ステップＳ４０５では、モデル選択部１４は、変数ｋの値が閾値未満であればポリゴンモデルを選択し、変数ｋの値が閾値以上であれば区分パラメトリック曲面モデルを選択する。このときに用いる閾値には、ポリゴンモデルを利用した位置姿勢情報の補正の収束が期待できる値（回数）を設定しておくのがよい。そしてステップＳ４０８では、適合度算出部１７は、変数ｋの値を１つインクリメントする。

次に、ケース２について説明する。ステップＳ４０４では、適合度算出部１７は、補正前の位置姿勢情報が示す位置と、補正後の位置姿勢情報が示す位置と、の間の距離を示す変数ΔＴを十分に大きな値（例えば1000）に初期化する。次に、ステップＳ４０５では、モデル選択部１４は、変数ΔＴの値が閾値以上であればポリゴンモデルを選択し、変数ｋの値が閾値未満であれば区分パラメトリック曲面モデルを選択する。ステップＳ４０８では、適合度算出部１７は、補正前の位置姿勢情報が示す位置と、補正後の位置姿勢情報が示す位置と、の間の距離を求め、求めた距離を変数ΔＴに設定する。

上記の通り、適合度に適用できる指標には、他にも様々なものが考えられ、たとえば、補正前の位置姿勢情報が示す姿勢と、補正後の位置姿勢情報が示す姿勢と、の差分を、ΔＴとして用いても良い。例えば、モデル表面上に設定した点群が位置姿勢の補正前後で移動した距離の和や平均によって三次元点群と三次元モデルとのずれを推定してもよい。

すなわち、第１の実施形態の変形例１，２では、繰り返し行われる補正の過程で計算される指標値と閾値との大小関係に応じてモデル選択を行っており、この構成に沿えば、他の変形例も考え得る。

＜第１の実施形態の変形例３＞
第１の実施形態では、算出される位置姿勢における、それぞれの３次元点の残差に基づいて、ポリゴンモデルと区分パラメトリック曲面モデルとを切り替えて位置姿勢算出を行う方法について説明した。しかし、モデル選択の際の判定にはこのほかの方法を用いてもかまわない。以下では、その例として、対象物体上で、物体表面上の各部位における位置姿勢推定の要求精度に基づき切り替える方法、対象物体の設計形状からの形状誤差を表す公差情報を用いた方法、について説明する。ここでいう「部位」とは、１つの対象物体全体であっても良いし、１つの対象物体の各部分であっても良い。

まず、前者の方法について説明する。この方法では、対象物体の形状モデルの部位ごとに、算出される位置姿勢における、３次元点とモデル表面とのずれの許容値を設定しておき、この情報に基づいて利用するモデルを選択する。これは、３次元点とモデル表面とを高精度にフィットさせたい部位には、高精度に対応探索が可能な区分パラメトリック曲面モデルを利用し、そうでない部位にはポリゴンモデルを利用するというものである。

具体的には、まず、各ポリゴンモデルの各ポリゴンに対して、該当する形状を表現する区分パラメトリック曲面モデルのトリム曲面との形状の近似誤差を算出しておく。ここで、この近似誤差をポリゴン近似誤差と呼ぶ。さらに、算出される位置姿勢におけるモデル面と３次元点とのずれの許容値を、ポリゴンモデルの各ポリゴンにそれぞれ設定しておく。そして、各ポリゴンに設定されたポリゴン近似誤差と、ずれの許容値とを比較して、位置姿勢算出時に利用するモデルを選択する。

具体的には、ある部位を形成するポリゴンのうち規定数以上のポリゴンのポリゴン近似誤差が該部位の許容値よりも大きければ、その部位に関しては、ポリゴンではずれの許容量に対してポリゴン近似誤差が大きい為、区分パラメトリック曲面モデルを選択する。なお、「ある部位を形成するポリゴンのうち規定数以上のポリゴンのポリゴン近似誤差が該部位の許容値よりも大きければ」、という条件以外にも様々な条件を適用することができる。例えば、「ある部位を形成するポリゴンのポリゴン近似誤差の平均値が該部位の許容値よりも大きければ」、という条件を代わりに用いても良い。

逆に、「ある部位を形成するポリゴンのうち規定数以上のポリゴンのポリゴン近似誤差が該部位の許容値よりも大きい」という条件が満たされなかった場合には、ずれの許容量に対してポリゴン近似誤差が小さいため、ポリゴンモデルを選択する。なお、この条件以外にも様々な条件を適用することができる。例えば、「ある部位を形成するポリゴンのポリゴン近似誤差の平均値が該部位の許容値よりも小さければ」、という条件を代わりに用いても良い。

なお、ずれの許容値を連続的な値として設定するのではなく、３次元点とモデル表面とを高精度にフィットさせたいか否かの２値の値を属性として設定し、この情報によってモデルを選択してもよい。また、この各モデルの属性は、ユーザにより選択させてもよい。

次に、対象物体の設計形状からの形状誤差を表す公差情報を用いた方法について説明する。この方法では、対象物体の形状の部位ごとの形状誤差を表す公差情報がCADデータに付与されているものとし、この情報に基づいて利用するモデルを選択する。なお、実際に物体の３次元形状を計測して、CADデータと形状を比較することによって公差情報を求めても良い。この方法は、モデル表面上のある部位の公差が、ポリゴン近似誤差よりも大きければ、そもそも高精度な対応間距離の算出は期待できないと判断してポリゴンモデルを利用するというものである。逆に公差が小さければ、高精度な対応間距離の算出が期待できると判断して、区分パラメトリック曲面モデルを利用する。具体的には、まず、各ポリゴンモデルの各ポリゴンに対して、該当する形状を表現する区分パラメトリック曲面モデルのトリム曲面との形状の近似誤差を算出しておく。さらに、ポリゴンモデルの各ポリゴンまたは区分パラメトリック曲面モデルを構成する各トリム曲面について、CADデータ上で該当する形状の公差をそれぞれ記録しておく。そして、各ポリゴンに設定されたポリゴン近似誤差と公差とを比較して、位置姿勢算出時に利用するモデルを選択する。

具体的には、ある部位を形成するポリゴンのうち規定数以上のポリゴンのポリゴン近似誤差が該部位の公差よりも大きければ、その部位に関しては、区分パラメトリック曲面モデルを選択する。なお、「ある部位を形成するポリゴンのうち規定数以上のポリゴンのポリゴン近似誤差が該部位の公差よりも大きければ」、という条件以外にも様々な条件を適用することができる。例えば、「ある部位を形成するポリゴンのポリゴン近似誤差の平均値が該部位の公差よりも大きければ」、という条件を代わりに用いても良い。

逆に、「ある部位を形成するポリゴンのうち規定数以上のポリゴンのポリゴン近似誤差が該部位の公差よりも大きい」という条件が満たされなかった場合には、ポリゴン近似誤差に対して物体表面のずれが大きいため、ポリゴンモデルを選択する。なお、この条件以外にも様々な条件を適用することができる。例えば、「ある部位を形成するポリゴンのポリゴン近似誤差の平均値が該部位の公差よりも小さければ」、という条件を代わりに用いても良い。

なお、上述した方法は、第１の実施形態の変形例１と組み合わせてもよい。つまり、設定されたずれの許容値または公差をポリゴン近似誤差と比較することで、モデル全体で統一して利用するモデルを選択してもよい。

＜第１の実施形態の変形例４＞
第１の実施形態及び変形例１，２では、３次元形状モデルとしてポリゴンモデルと区分パラメトリック曲面を用いているが、用いるモデルの種類はこれに限らず形状再現精度が異なる２種のモデルであれば良い。例えば、物体表面をより細かく区分したポリゴンモデルとより粗く区分したポリゴンモデルの組み合わせであっても良いし、曲面の次数が異なる複数の区分パラメトリック曲面の組み合わせであっても良い。さらに、全ての実施形態及びその変形例は、形状再現精度が２段階に異なる２種のモデルにのみ適用可能なのではなく、３段階以上の精度の異なるモデルの組み合わせに対しても適用できる。

＜第１の実施形態の変形例５＞
第１の実施形態１及びその変形例１〜４では、距離画像から得られる三次元点群とモデルとを位置合わせする際に、規定の条件に従ってモデルを切り替えながら、三次元点群とモデルとの距離（対応間距離）が計算された。そして、この距離に基づいて位置合わせが行われた。これに対して、対象物体を撮影した撮影画像上の幾何特徴（以下、単に特徴と呼ぶ）と、モデルの幾何特徴（以下、単に特徴と呼ぶ）との対応関係に基づいて位置合わせを行う方法がある。このような方法は、例えばT. Drummond and R. Cipolla, “Real-time visual tracking of complex structures,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002.に記載されている。このような位置合わせ方法において、撮影画像上で検出した特徴とモデルの特徴との対応間距離を算出する際に、第１の実施形態及びその変形例１〜４で述べた方法を用いてもよい。

具体的には、まず、所定位置の視点から対象物体を撮影することによって得られた撮影画像の画像取得が行われ、撮影画像上の特徴が抽出される。この撮影画像は、例えば濃淡画像でありうる。また、この特徴は、例えばエッジでありうる。撮影画像からのエッジの抽出は、例えばフィルタリング処理等の従来技術に従って行うことができる。

次に、モデルの特徴の位置が検出される。具体的には、撮影画像の視点と対象物体との概略的な位置姿勢関係を示す概略位置姿勢取得を行ってこれにしたがって視点とモデルとを配置した場合に、配置された視点から配置されたモデルを見た場合におけるモデルの像を含む投影画像が算出される。そして、投影画像中におけるモデルの特徴の位置が算出される。モデルの特徴は、例えばモデルの輪郭線でありうる。具体的には、ポリゴンモデルの特徴はポリゴンのエッジであってもよいし、投影画像中におけるモデルの外輪郭であってもよい。ここではモデルの像を含む投影画像から特徴を抽出する方法について述べた。一方で、事前にモデル上から輪郭線またはエッジの座標値を抽出しておき、これらの座標値を概略的な位置姿勢に基づいて画像面上に投影することで、投影画像中におけるモデル特徴の位置を算出してもよい。また、区分パラメトリック曲面モデルの特徴は、区分パラメトリック曲面モデルを構成するトリム曲面の輪郭線であってもよいし、投影画像中におけるモデルの外輪郭であってもよい。

ここで用いられるモデルは、第１の実施形態及びその変形例１〜４で述べた方法に従って選択される。例えば、位置姿勢のフィッティング度合いに応じて、対象物体とモデルとの位置ずれが大きい時には、ポリゴンモデルを用いて高速に位置姿勢算出を行うことができる。また、対象物体とモデルとの位置ずれが小さい時には、区分パラメトリック曲面モデルを用いて高精度に位置姿勢算出を行うことができる。

そして、撮影画像上の特徴について、対応するモデルの特徴が判定され、対応する特徴間の距離が検出される。例えば、モデルの特徴のうち、投影画像中における位置が撮影画像上の特徴の位置により近い特徴を、この撮影画像上の特徴に対応するモデルの特徴として判定することができる。こうして撮影画像上の特徴と選択されたモデルの特徴との対応付けが行われ、撮影画像上の幾何特徴に対応する選択されたモデルの対応幾何特徴が決定されて、対応間距離が算出される。このように撮影画像上のそれぞれの特徴について検出された対応間距離を用いて、位置合わせを行うことができる。なお、ここでは位置合わせに用いる特徴として、エッジの位置を表す座標値を用いたが、エッジの位置を表す座標および方向を表すベクトルとを用いて位置合わせを行ってもよい。

［第２の実施形態］
第１の実施形態で説明した位置姿勢算出方法では、区分パラメトリック曲面モデルを選択した場合に、３次元点と対応する区分パラメトリック曲面との探索において、複数のトリム曲面との対応間距離を算出する必要がある。また、各トリム曲面との対応間距離を算出する際に、算出された点が物体形状を表す有効領域上に存在するか否かを判定する必要がある。これらの理由から、区分パラメトリック曲面モデルを選択した場合には、対応間距離の算出処理に時間がかかる。

そこで本実施形態では、ポリゴンモデルの各ポリゴンと、区分パラメトリック曲面と、を関連付けておくことで、対応間距離の算出を高速化する方法について述べる。具体的には、まず、３次元点と対応するモデル表面（区分パラメトリック曲面モデル上の面）をポリゴンモデルを介して特定する。ポリゴンと３次元点との対応は、第１の実施形態で説明した方法と同様の方法を用いることで高速に探索が可能である。その後、３次元点とモデル表面までの距離を算出する際には、ポリゴンと関連付けられた区分パラメトリック曲面の曲面パラメータを用いることで、高精度な値を算出する。これらの処理により、区分パラメトリック曲面モデルを選択したとしても、３次元点とモデル表面との対応間距離を高速に算出することが可能となる。

本実施形態に係る情報処理装置２の機能構成例について、図５のブロック図を用いて説明する。なお、図５において、図１に示した機能部と同じ機能部については同じ参照番号を付しており、これに係る説明は省略する。

モデル保持部５１は、第１の実施形態と同様に、対象物体の形状を複数枚のポリゴンで近似したポリゴンモデルと、該対象物体の形状をパラメトリック曲面で近似した区分パラメトリック曲面モデルとを保持（管理）している。本実施形態では、これに加え、ポリゴンモデルの各ポリゴンには、該ポリゴンに対応する区分パラメトリック曲面モデルのトリム曲面のＩＤが付与されている。例えば、図２に示すポリゴンモデルと区分パラメトリック曲面モデルとをモデル保持部５１が保持している場合、図２（ｂ）の斜線部分のポリゴンには、図２（ａ）の斜線部分のトリム曲面のＩＤが付与されている。

対応探索部５４は、モデル保持部５１が保持しているポリゴンモデルから、物体表面位置取得部１２が取得したそれぞれの３次元点に対応するポリゴンを探索する。距離算出部５５は、物体表面位置取得部１２が取得したそれぞれの３次元点とモデル表面との間の対応間距離を算出する。具体的には、各３次元点について、該３次元点に対応するポリゴンに付与されているＩＤのトリム曲面を参照し、その曲面パラメータを用いて、該３次元点とモデル表面との間の対応点間距離を算出する。

次に、本実施形態に係る情報処理装置２が、対象物体の位置姿勢を求めるために行う処理について、同処理のフローチャートを示す図６を用いて説明する。なお、図６において、ステップＳ６０２〜Ｓ６０４，Ｓ６０７〜Ｓ６０９はそれぞれ、図３のステップＳ３０２，Ｓ３０３，Ｓ３０５，Ｓ３０８，Ｓ３０９，Ｓ３１１と同様の処理ステップであるため、これらの処理ステップに係る説明は省略する。

なお、以下では説明を簡単にするために、区分パラメトリック曲面モデルが選択されることを前提としているが、本実施形態は、第１の実施形態において区分パラメトリック曲面モデルが選択された場合に適用されるものである（特にＳ６０５，Ｓ６０６）。

＜ステップＳ６０５における処理について＞
対応探索部５４は、モデル保持部５１が保持しているポリゴンモデルから、ステップＳ６０４で選択した３次元点に対応するポリゴンを探索する。本ステップでは、上記のステップＳ３０７において、ポリゴンモデルが選択された場合における処理と同様の処理により高速に探索を行う。

＜ステップＳ６０６における処理について＞
距離算出部５５は、ステップＳ６０４で選択した３次元点に対応するモデル表面上の対応点を特定すると共に、該３次元点と該対応点との間の距離（対応間距離）を算出する。

具体的には、基準座標系で規定されている３次元点ｋｃ＿ｉの位置を、現在の位置姿勢情報が示す位置姿勢によって規定されるモデル座標系に変換した位置ｋｍ＿ｉ’を求める。そして、現在の位置姿勢情報が示す位置姿勢で配置した区分パラメトリック曲面モデルにおける該３次元点に対応する対応点ｐｍ＿ｉを特定すると共に、対応点ｐｍ＿ｉの位置と位置ｋｍ＿ｉ’との間の距離（対応間距離）ｄ＿ｉを求める。

ここで、図７は、各３次元点を、現在の位置姿勢情報に基づいてモデル座標系に変換したものである。白丸は３次元点、細破線は各３次元点とポリゴンとの対応をそれぞれ表している。したがって、距離算出においては、このポリゴンと関連付けられたトリム曲面との距離のみを算出する。図７（ｂ）の円柱は、図２（ａ）の斜線部のトリム曲面を表現するベース曲面である。円柱は、モデル座標系における、円柱の回転中心となる軸を規定する原点の座標Ｏ’と、回転中心となる軸の方向を示す３次元ベクトルaxis、円柱の半径ｒ、の３つのパラメータで表現される。このとき、モデル座標系における、ｋｍ＿ｉ’と円柱との距離ｄ＿ｉは以下の式により算出される。

ｄ＿ｉ＝ａｂｓ（ｋｍ＿ｉ’−ａｂｓ（ａｘｉｓ・ｋｍ＿ｉ’）×ａｘｉｓ））−ｒ
ａ・ｂはベクトルａとベクトルｂとの内積である。また、ｋｍ＿ｉ’はｋｍ＿ｉ’＝ｋｍ＿ｉ−Ｏ’で表されるベクトルである。図７（ａ）では、各３次元点について、トリム曲面までの距離を示す線分が示してある。さらに、３次元点からトリム曲面までの最短距離を持つ３次元点ｐｍ＿ｉの座標を算出する。ここで算出した距離ｄ＿ｉとモデル点ｐｍ＿ｉとを、３次元点とともに記録しておく。なお、ここでは、トリム曲面が円柱の場合について詳しく説明したが、その他のトリム曲面の場合においても、三次元点と曲面との対応点および対応間距離を曲面パラメータから算出する公知の方法を用いて実施すればよい。

このように、本実施形態によれば、ポリゴンでの対応探索を一度介することで、三次元点と対応するトリム面を一意に特定することができるため、複数のトリム曲面との対応探索処理を省略できる。そのため、区分パラメトリック曲面モデルを用いて位置姿勢情報を補正する場合であっても、３次元点とモデル表面との対応間距離を高速に算出することが可能となる。なお、本実施形態で説明した対応間距離の算出方法は、第１の実施形態で区分パラメトリック曲面モデルを選択した場合に対応点間距離を求める際に適用しても良い。

なお、上記の各実施形態や各変形例では、様々な例を挙げて説明したが、何れも以下に示す構成の一例に過ぎない。即ち、対象物体の概略の位置姿勢を示す位置姿勢情報を取得し、対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報のうち規定の条件に応じて決まる１つを選択モデル情報として選択する。そして、該選択モデル情報が示すモデルを上記位置姿勢情報が示す位置姿勢に配置し、対象物体上の点と、該配置したモデルにおいて該点に対応する対応点と、の間の距離の評価値を算出し、該評価値に基づいて、対象物体の位置姿勢を推定する。

＜第２の実施形態の変形例１＞
本変形例では、３次元点と該３次元点に対応するポリゴンとの距離を求め、該求めた距離が閾値未満であれば、区分パラメトリック曲面モデルを利用して高精度な距離を算出し直す。これにより、位置姿勢の補正を実施する前に、３次元点とモデル表面とのずれを評価して、高速化と高精度化とを効率よく切り替えて位置姿勢の算出が可能となる。

本変形例でも、図５に示した構成を有する情報処理装置２を用いるが、下記の点が第２の実施形態とは異なる。以下では、第２の実施形態と異なる点について重点的に説明し、以下で特に触れない限りは、第２の実施形態と同様であるとする。

本変形例に係る情報処理装置２が、対象物体の位置姿勢を求めるために行う処理について、同処理のフローチャートを示す図８を用いて説明する。なお、図８において、ステップＳ８０２〜Ｓ８０４，Ｓ８０８〜Ｓ８１０はそれぞれ、図３のステップＳ３０２，Ｓ３０３，Ｓ３０５，Ｓ３０８，Ｓ３０９，Ｓ３１１と同様の処理ステップであるため、これらの処理ステップに係る説明は省略する。

＜ステップＳ８０５における処理について＞
対応探索部５４は、モデル保持部５１が保持しているポリゴンモデルから、ステップＳ８０４で選択した３次元点ｋｃ＿ｉに対応するポリゴンを探索する。本ステップでは、上記のステップＳ３０７において、ポリゴンモデルが選択された場合における処理と同様の処理により高速に探索を行う。

＜ステップＳ８０６における処理について＞
距離算出部５５は、ステップＳ８０４で選択した３次元点ｋｃ＿ｉに対応するポリゴン上の対応点を特定すると共に、該３次元点ｋｃ＿ｉと該対応点との間の距離（対応間距離）ｄ＿ｉを算出する。そして距離算出部５５は、距離ｄ＿ｉと閾値φとの大小比較を行う。この閾値φには、たとえば、第１の実施形態で用いた閾値θと同様に、ポリゴンモデルと区分パラメトリック曲面モデルとの形状の誤差の値を設定しておく。

この大小比較の結果、ｄ＿ｉ≧φであれば、現在の位置姿勢情報が示す位置姿勢における３次元点とモデル表面とのずれが依然大きいため、距離算出部５５は、この距離ｄ＿ｉをそのまま位置姿勢算出部１６に対して送出する。そして処理はステップＳ８０８に進む。

一方、ｄ＿ｉ＜φであれば、算出された位置姿勢情報が示す位置姿勢における３次元点とモデル表面とのずれがモデル近似誤差と同程度であることを意味する。この場合、高精度な形状を表現する区分パラメトリック曲面モデルを用いて再度対応間距離を算出するために、処理はステップＳ８０７に進む。

＜ステップＳ８０７における処理について＞
距離算出部５５は、ステップＳ８０４で選択した３次元点に対応するポリゴンに付与されているＩＤのトリム曲面を参照し、その曲面パラメータを用いて、該３次元点と対応するモデル表面上の対応点と、該３次元点との間の対応点間距離を算出する。即ち、上記のステップＳ６０６における処理と同様の処理を行う。そしてこの対応点間距離が位置姿勢算出部１６に対して送出される。

このように、本変形例によれば、位置姿勢情報の補正を実施する前に、３次元点とモデル表面とのずれを評価し、高速化と高精度化とを効率よく切り替えて位置姿勢の算出が可能となる。

＜第２の実施形態の変形例２＞
第２の実施形態では、ポリゴンモデルと区分パラメトリック曲面モデルとを組にした三次元形状モデルを利用した。ただし、モデルの組み合わせ方法はこの限りではない。たとえば、対応探索の速度と、表現する物体形状の精度とのトレードオフを段階的に切り替え可能なモデルを複数組み合わせて、三次元形状モデルとして保持しておき、これらからモデルを選択して利用してもよい。すなわち、まず、より再現精度が低いモデル情報を用いて、対象物体上の点に対応する部分（例えばポリゴン面のような表面の一部）が特定される。そして、より再現精度が高いモデル情報が示すモデルから、特定された部分に対応する部分（例えばパラメトリック曲面のトリム曲面のような表面の一部）が特定される。最後に、対象物体上の点と、より再現精度が高いモデル情報が示すモデルから特定された部分上の対応点と、の距離の評価値が算出される。

＜第２の実施形態の変形例３＞
第２の実施形態では、距離画像から得られる三次元点群とモデルとの位置合わせにおいて、ポリゴンモデルの各ポリゴンと、同一形状を表す区分パラメトリック曲面と、の関連付けを用いて、高速に対応間距離を算出する方法について述べた。この方法を、第１の実施形態の変形例５で述べた、対象物体を撮影した撮影画像上の幾何特徴と、モデル面上から抽出した幾何特徴との対応関係に基づく位置合わせに適用してもよい。例えば、幾何特徴はエッジであってもよく、具体的には、対象物体を撮影した撮影画像上のエッジを表す線分（幾何特徴）と、モデル面上から抽出した線分（幾何特徴）との対応関係に基づく位置合わせに、この方法を適用してもよい。すなわち、まず、撮影画像上の特徴について、対応するポリゴンモデルの特徴が判定される。そして、ポリゴンモデルの特徴に対応する区分パラメトリック曲面の特徴を示す関連付け情報を参照して、撮影画像上の特徴に対応する区分パラメトリック曲面の特徴が選択される。こうして、ポリゴンモデルのような、より再現精度が低い第１のモデル情報が示す第１のモデルの幾何特徴と、区分パラメトリック曲面モデルのような、より再現精度が高い第２のモデル情報が示す第２のモデルの幾何情報と、の対応付けが行われる。そして、撮影画像上の特徴と、区分パラメトリック曲面の特徴との対応間距離が算出される。上述のように、ポリゴンモデルの特徴はポリゴンのエッジであってもよく、これに対応する区分パラメトリック曲面の特徴は区分パラメトリック曲面の輪郭線であってもよい。こうして算出できる輪郭線を表現する曲線パラメータに従う曲線と撮影画像上の特徴との距離は、高精度な対応間距離として使用できる。なお、この他の位置合わせ方法として、エッジを表現する線分を細かく分割した、座標と方向とにより表現されるエッジレットを用いた位置合わせに対しても、同様の方法を適用可能である。

［第３の実施形態］
本実施形態では、第１，２の実施形態及び変形例等で説明した方法で算出した位置姿勢情報の使用目的の一例として、ロボットに、該位置姿勢情報が示す位置姿勢を有する対象物体を操作させるシステムについて説明する。

本実施形態に係るシステムの構成例について、図９を用いて説明する。図９に示す如く、システムは、情報処理装置３、ロボットコントローラ部、ロボット９００、を有する。

ロボット９００は多関節ロボットであり、ロボットコントローラ部からの制御命令を受けて稼働する。ロボット９００のアームの先端にはエンドエフェクタであるハンドが装着されており、対象物体に対する作業を行うことが可能である。本実施形態ではエンドエフェクタとして対象物体を把持することが可能なチャック機構を持つハンドを用いる。エンドエフェクタにはモータ駆動可能なハンドを用いてもよいし、空気圧で対象物体を吸着する吸着パッドを用いてもよい。ロボット９００と物体表面位置取得部９０１との相対位置姿勢、ロボット９００のアームとエンドエフェクタとの相対位置姿勢のキャリブレーション作業は本実施形態の実施に先立って公知の技術によって予め行われているものとする。これによって、位置姿勢算出部９０６で算出する位置姿勢をパレットが置かれた空間に固定されたワークスペース座標系に変換することが可能になる。さらに、ワークスペース座標系で指定した位置姿勢にハンドが設定されるようにロボットを制御することが可能になる。ただし、ロボットは多関節ロボットではなくＮＣ制御可能な可動式の機械でもよい。

物体表面位置取得部９０１は対象物体９１０の距離画像を撮像し、撮像した距離画像を用いて、対象物体９１０の表面上の各３次元点の位置を取得する。即ち、物体表面位置取得部９０１の基本的な動作は、物体表面位置取得部１２と同様である。したがって、物体表面位置取得部９０１は、ロボット９００のハンド部分近くに固定的に装着してもよいし、ワークスペース座標系のどこかに固定してもよい。物体表面位置取得部９０１は、たとえば小型プロジェクタとカメラとにより構成されるものとし、空間符号化法により距離画像を撮影する。ただし、距離画像撮影は、ステレオカメラによるステレオ法で行ってもよいし、レーザ投光機とカメラとにより光切断法で行ってもよいし、その他のいかなる方法であってもよい。

対象物体９１０は、工業製品を構成する部品である。ロボット９００によりピックされて製品に組み付けられる。素材は、プラスチック、金属、ビニールなど、様々なものを用いることができる。複数の対象物体９１０が様々な位置姿勢でパレットに山積みされている。

対応探索部９０４、モデル保持部９０２、距離算出部９０５、位置姿勢算出部９０６、概略位置姿勢入力部９０３のそれぞれは、対応探索部５４、モデル保持部５１、距離算出部５５、位置姿勢算出部１６、概略位置姿勢入力部１３と同様の機能部である。位置姿勢算出部９０６は、最終的に求めた対象物体９１０の位置姿勢をロボット作業指示部９０７に対して送出する。

ロボット作業指示部９０７は、位置姿勢算出部９０６から受けた位置姿勢にロボット９００のアームを移動させて把持または吸着を行うよう指示するための制御命令を生成してロボット制御部９０８に対して送出する。ただし、ロボットの作業は移動、把持、吸着に限られるものではなく対象物体の外観検査など、他の作業も含むことは言うまでもない。

ロボット制御部９０８は、この制御命令に従ってロボット９００を制御し、位置姿勢算出部９０６が求めた位置姿勢情報が示す位置姿勢にロボット９００のアームを移動させ、所望の動作（対象物体９１０を把持させたり、移動させたりする動作）を行わせる。

上記のシステムの動作について、図１０のフローチャートを用いて説明する。先ず、ステップＳ１００１において情報処理装置３は、対象物体９１０の位置姿勢を求める。次にステップＳ１００２で情報処理装置３は、求めた位置姿勢をロボットコントローラ部に対して送出するので、ロボットコントローラ部は、この位置姿勢にある対象物体９１０に対して所望の操作を行わせるべく、ロボット９００に対して制御命令を送出する。

ステップＳ１００３では、ロボット９００は、この制御命令に従って動作する。本システムは、停止命令がユーザによって入力されない限りは上記のステップＳ１００１〜Ｓ１００３の処理を繰り返し行う（ステップＳ１００４）。

なお、情報処理装置３の構成や位置姿勢を計算する為の方法には、第１の実施形態、第２の実施形態、それらの変形例の何れを適用しても良いし、適用時にはその構成や処理を適宜変形例させても良い。

［第４の実施形態］
図１に示した情報処理装置１内の各部、図５に示した情報処理装置２内の各部、図９に示した情報処理装置３内の各部、はハードウェアで構成しても良い。しかし、モデル保持部（１１，５１，９０２）をメモリで構成し、その他の各部をコンピュータプログラムで構成しても良い。

その場合、一般のＰＣ（パーソナルコンピュータ）等を情報処理装置１〜３に適用することができる。例えば、モデル保持部（１１，５１，９０２）をハードディスク内に構成し、且つモデル保持部以外の各部の機能をＣＰＵに実行させるためのコンピュータプログラムをこのハードディスクに保存しておく。そしてＰＣのＣＰＵがハードディスクに保存されているコンピュータプログラムやデータをＲＡＭに読み出し、読み出したコンピュータプログラムやデータを用いて処理を実行することで、このＰＣは、情報処理装置１〜３として機能することになる。また、上記の各実施形態や変形例は、適宜組み合わせて用いても良い。

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

Claims (17)

1. 対象物体を含む画像を取得する画像取得手段と、
   前記対象物体の概略の位置姿勢を示す概略位置姿勢を取得する手段と、
   前記対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報を保持する保持手段と、
   繰り返し行われる推定の過程で計算される指標値と閾値との大小関係に応じて、前記複数のモデル情報のうちの何れか１つを選択モデル情報として選択する選択手段と、
   前記選択モデル情報が示すモデルを、前記概略位置姿勢が示す位置姿勢で配置し、前記対象物体の表面上のそれぞれの幾何特徴と、該配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を求める対応付け手段と、
   前記対象物体の表面上のそれぞれの幾何特徴について求めた距離に基づいて、前記対象物体の位置姿勢を推定する推定手段と
   を備えることを特徴とする情報処理装置。
2. 前記選択手段は、前記対象物体に対する適合度に基づき、前記複数のモデル情報のうちの何れか１つを選択モデル情報として選択し、
   前記推定手段は更に、
   前記対象物体の表面上のそれぞれの幾何特徴に対し、該幾何特徴と、前記推定手段が推定した概略位置姿勢が示す位置姿勢で配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を求め、該求めた距離を前記適合度として求める手段を備える
   ことを特徴とする請求項１に記載の情報処理装置。
3. 前記選択手段は、前記推定の実行回数に基づき、前記複数のモデル情報のうちの何れか１つを選択モデル情報として選択する
   ことを特徴とする請求項１に記載の情報処理装置。
4. 前記選択手段は、前記推定手段による推定の前の概略位置姿勢が示す位置と、前記推定手段による推定の後の概略位置姿勢が示す位置と、の差分に基づき、前記複数のモデル情報のうちの何れか１つを選択モデル情報として選択する
   ことを特徴とする請求項１に記載の情報処理装置。
5. 前記選択手段は、前記推定手段による推定の前の概略位置姿勢が示す姿勢と、前記推定手段による推定の後の概略位置姿勢が示す姿勢と、の差分に基づき、前記複数のモデル情報のうちの何れか１つを選択モデル情報として選択する
   ことを特徴とする請求項１に記載の情報処理装置。
6. 対象物体を含む画像を取得する画像取得手段と、
   前記対象物体の概略の位置姿勢を示す概略位置姿勢を取得する手段と、
   前記対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報を保持する保持手段と、
   前記対象物体の複数の部分における各部分である部位ごとに、前記複数のモデル情報のうち規定の条件に応じて決まる１つを選択モデル情報として選択し、該選択されたモデルの幾何特徴と前記画像中の対象物体の幾何特徴とを対応づける対応付け手段と、
   前記対応付けの結果に基づいて、前記対象物体の位置姿勢を推定する推定手段と
   を備えることを特徴とする情報処理装置。
7. 前記対応付け手段は、前記対象物体の部位ごとに、該部位の形状をパラメトリック曲面で近似したパラメトリック曲面モデル情報、該部位の形状をポリゴン群で近似したポリゴンモデル情報、の何れか１つを、前記パラメトリック曲面モデルに対する前記ポリゴンモデルの近似誤差に応じて選択し、選択したモデル情報が示すモデルを、前記概略位置姿勢で配置し、前記対象物体の表面上のそれぞれの幾何特徴と、該配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を求め、
   前記推定手段は、前記対象物体の表面上のそれぞれの幾何特徴について求めた距離に基づいて前記概略位置姿勢を推定することを特徴とする請求項６に記載の情報処理装置。
8. 前記対応付け手段は、前記対象物体の部位ごとに、該部位の形状をパラメトリック曲面で近似したパラメトリック曲面モデル情報、該部位の形状をポリゴン群で近似したポリゴンモデル情報、の何れか１つを、前記対象物体の形状の公差に基づいて選択し、選択したモデル情報が示すモデルを、前記概略位置姿勢で配置し、前記対象物体の表面上のそれぞれの幾何特徴と、該配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を求め、
   前記推定手段は、前記対象物体の表面上のそれぞれの幾何特徴について求めた距離に基づいて前記概略位置姿勢を推定することを特徴とする請求項６に記載の情報処理装置。
9. 前記対応付け手段は、
   前記対象物体の表面上の各幾何特徴に対し、該幾何特徴に対する残差が閾値以上であれば前記対象物体の形状をポリゴン群で近似したポリゴンモデル情報を選択モデル情報として選択し、前記残差が該閾値未満であれば前記対象物体の形状をパラメトリック曲面で近似したパラメトリック曲面モデル情報を選択モデル情報として選択する選択手段と、
   前記幾何特徴について選択された選択モデル情報に従うモデルを、前記概略位置姿勢で配置し、該幾何特徴と、該配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を求める演算手段と、
   前記対象物体の表面上の各幾何特徴について求めた距離に基づいて前記概略位置姿勢を補正する補正手段と、
   前記対象物体の表面上の各幾何特徴に対し、該幾何特徴について選択した選択モデル情報に従うモデルを前記補正手段が補正した概略位置姿勢で配置し、該幾何特徴と、該配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を、該幾何特徴に対する残差として求める手段と
   を備える
   ことを特徴とする請求項６に記載の情報処理装置。
10. 前記保持手段は、
    第１のモデル情報と該第１のモデル情報よりも再現精度の高い第２のモデル情報とを前記対象物体の部位ごとに関連づけて保持し、
    前記対応付け手段は、
    前記概略位置姿勢で配置した前記第１のモデルの幾何特徴と前記画像中の対象物体の幾何特徴とを対応づけることにより、前記第１のモデルの幾何特徴を特定する特定手段を更に備え、
    前記特定された第１のモデルの幾何特徴と関連付けられる第２のモデルの幾何特徴と、前記対象物体の幾何特徴とを対応付けることを特徴とする、請求項６に記載の情報処理装置。
11. 前記第１のモデル情報は、ポリゴンモデル情報であり、
    前記第２のモデル情報は、区分パラメトリック曲面モデル情報であることを特徴とする、請求項１０に記載の情報処理装置。
12. 前記特定手段は、前記対象物体上の前記幾何特徴と、前記第１のモデルにおいて該幾何特徴に対応する対応幾何特徴と、の距離を算出し、
    前記特定手段が算出した前記距離が閾値以上である幾何特徴については前記特定手段が算出した前記距離の評価値を用い、前記特定手段が算出した前記距離が閾値未満である幾何特徴については前記第２のモデルを用いて対応づけられた結果を用いて、前記対象物体の位置姿勢を推定する
    ことを特徴とする、請求項１０又は１１に記載の情報処理装置。
13. 所定位置の視点から対象物体を撮影した撮影画像を取得する手段と、
    前記撮影画像上の幾何特徴を抽出する手段と、
    をさらに備え、
    前記推定手段は、前記対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報から選択された選択モデル情報が示すモデルを前記概略位置姿勢が示す位置姿勢に基づいて前記画像上に投影し、前記抽出された幾何特徴と、該投影したモデルにおいて該幾何特徴に対応する対応幾何特徴とに基づいて、前記対象物体の位置姿勢を推定する
    ことを特徴とする、請求項６に記載の情報処理装置。
14. 情報処理装置が行う情報処理方法であって、
    前記情報処理装置の保持手段は、対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報を保持し、
    前記情報処理方法は、
    前記情報処理装置の画像取得手段が、前記対象物体を含む画像を取得する画像取得工程と、
    前記情報処理装置の位置姿勢取得手段が、前記対象物体の概略の位置姿勢を示す概略位置姿勢を取得する工程と、
    前記情報処理装置の選択手段が、繰り返し行われる推定の過程で計算される指標値と閾値との大小関係に応じて、前記複数のモデル情報のうちの何れか１つを選択モデル情報として選択する工程と、
    前記情報処理装置の対応付け手段が、前記選択モデル情報が示すモデルを、前記概略位置姿勢が示す位置姿勢で配置し、前記対象物体の表面上のそれぞれの幾何特徴と、該配置したモデルにおいて該幾何特徴に対応する対応幾何特徴と、の間の距離を求める工程と、
    前記情報処理装置の推定手段が、前記対象物体の表面上のそれぞれの幾何特徴について求めた距離に基づいて、前記対象物体の位置姿勢を推定する推定工程と
    を有することを特徴とする情報処理方法。
15. 情報処理装置が行う情報処理方法であって、
    前記情報処理装置の保持手段は、対象物体の形状の再現精度がそれぞれ異なる複数のモデル情報を保持し、
    前記情報処理方法は、
    前記情報処理装置の画像取得手段が、前記対象物体を含む画像を取得する画像取得工程と、
    前記情報処理装置の位置姿勢取得手段が、前記対象物体の概略の位置姿勢を示す概略位置姿勢を取得する工程と、
    前記情報処理装置の対応付け手段が、前記対象物体の複数の部分における各部分である部位ごとに、前記複数のモデル情報のうち規定の条件に応じて決まる１つを選択モデル情報として選択し、該選択されたモデルの幾何特徴と前記画像中の対象物体の幾何特徴とを対応づける工程と、
    前記情報処理装置の推定手段が、前記対応付けの結果に基づいて、前記対象物体の位置姿勢を推定する工程と
    を有することを特徴とする情報処理方法。
16. 更に、
    ロボットを制御する制御部に対し、前記推定手段が推定した位置姿勢に従って該ロボットを移動させるための指示を送出する手段を備えることを特徴とする請求項１乃至１３の何れか１項に記載の情報処理装置。
17. コンピュータを、請求項１乃至１３の何れか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。

Images