JP2018091656A - 情報処理装置、計測装置、システム、算出方法、プログラムおよび物品の製造方法 - Google Patents

Abstract

【課題】高速かつ高精度に対象物体の位置姿勢を判別すること。
【解決手段】対象物体の位置および姿勢を算出する情報処理装置１００は、対象物体の形状の計測データおよび対象物体の形状モデルを取得する３次元形状モデル保持部１２と、３次元形状モデル保持部１２が取得した形状モデルにおいて、対象物体の姿勢を特定するための特定部位のサンプリングに関する情報を決定する特定部位内サンプリング部１６と、特定部位内サンプリング部１６が決定した特定部位のサンプリングに関する情報および対象物体の形状の計測データに基づいて対象物体の位置および姿勢を算出する位置姿勢算出部１７と、を有する。
【選択図】図２

Description

本発明は、情報処理装置、計測装置、システム、算出方法、プログラムおよび物品の製造方法に関する。

工場などの生産ラインにおいて、ビジョンシステムを用いてバラ積みされた物体の中から一個体を特定し、特定した把持対象の物体の位置姿勢を計測して、ロボットハンドにより把持を行う技術が近年開発されている。
物体の３次元位置姿勢を計測する方法として、対象物体の撮影画像から一個体のおおよその位置姿勢を検出し、その位置姿勢を初期値として、画像データに対して物体の３次元形状モデルをあてはめるモデルフィッティング法がある。モデルフィッティングの手法としては、特許文献１のように対象物体の３次元形状モデル上の幾何特徴からサンプリングしたモデル点を対象物体の距離画像上もしくは濃淡画像上に投影し、画像上の幾何特徴と対応付ける方法が知られている。また、誤認識しやすい形状をもつ対象物体の姿勢を判別する方法として、特許文献２の方法が知られている。特許文献２の方法では、事前に互いに誤りやすい複数の姿勢間の関係を登録しておき、概略位置姿勢からモデルフィッティングした姿勢と、事前の登録に基づいて変換パラメータを用いて算出した姿勢とを比較し、より一致度の高い姿勢を出力している。
特許文献１や特許文献２の方法では、サンプリングする密度を疎にしてモデル点の数を少なくするほど処理時間が短縮されるが、１点あたりの位置姿勢推定の計算に寄与する度合いが相対的に高まるために位置姿勢推定の精度が低下する。局所的な形状を手掛かりとして姿勢の違いやものの違いを判別する必要のある物体を計測する場合、手掛かりとなる部位についての３次元モデル上での幾何的な情報の不足により誤認識する可能性がある。
一方、位置姿勢推定の精度を上げるために、サンプリングする密度を細かくしてモデル点の数を多くすると、モデル点の数に比例して画像上の幾何特徴との対応探索時間が増大してしまう。
特許文献３は、モデルフィッティングに用いるモデル点をサンプリングする方法として、３次元モデルの面を平面や曲面の集合として表現し、各面を単位面積で分割したときの小領域ごとに生成する方法を開示している。特許文献３の方法では、距離計測の誤差が小さいと推測される領域ではサンプリングの密度を低く設定する一方で、距離計測の誤差が大きいと推測される小領域ほどサンプリングの密度を高く設定している。

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

しかしながら、特許文献３では、小領域ごとのモデル点の密度を距離計測の誤差に応じて制御しているが、密度を高く設定した小領域が姿勢の違いやものの違いを判別する手掛かりとなる局所的な形状を有する箇所と一致するとは限らない。

本発明は、例えば、高速かつ高精度に局所的な形状を有する物体の位置姿勢を判別することが可能な計測装置を提供することを目的とする。

上記課題を解決するために、本発明の一側面としての情報処理装置は、対象物体の位置および姿勢を算出する情報処理装置であって、前記対象物体の形状の計測データおよび前記対象物体の形状モデルを取得する取得部と、前記取得部が取得した前記形状モデルにおいて、前記対象物体の姿勢を特定するための特定部位のサンプリングに関する情報を決定するサンプリング部と、前記サンプリング部が決定した前記特定部位の前記サンプリングに関する情報および前記対象物体の形状の計測データに基づいて前記対象物体の位置および姿勢を算出する算出部と、を有する。

本発明によれば、例えば、高速かつ高精度に局所的な形状を有する物体の位置姿勢を判別することが可能な計測装置を提供することができる。

情報処理装置のハードウェア構成を示す図である。 情報処理装置の構成を示す図である。 対象物体の３次元形状モデルを示す図である。 対象物体の位置姿勢算出の処理手順を示すフローチャートである。 類似姿勢と特定部位の登録方法の説明図である。 対となる特定部位の登録についての説明図である。 モデル点を示す概念図である。 特定部位における幾何特徴上の計測点の数の上限値を推測する方法の説明図である。 位置姿勢算出方法の説明図である。 対象物体の位置姿勢算出の処理手順を示すフローチャートである。 対象物体の位置姿勢算出の処理手順を示すフローチャートである。 概略位置姿勢候補ごとのモデル点のサンプリング方法についての説明図である。 対象物体と類似物体を示す図である。 対象物体と類似物体の判別処理の手順を示すフローチャートである。 ロボットシステムの構成を示す図である。

（第１実施形態）
本実施形態では、類似姿勢の判別の手掛かりとなる局所的な特徴を有する部位（特定部位）に含まれる幾何特徴について、判別が十分可能な密度でモデル点を事前にサンプリングした上で、類似した姿勢を高精度に判別する方法について説明する。

図１は、本実施形態における情報処理装置１００のハードウェア構成を示す図である。
ＣＰＵ１０１は、バス１０６を介して接続する各デバイスを統括的に制御する。ＣＰＵ１０１は、読み出し専用メモリであるＲＯＭ１０２に記憶された処理ステップやプログラムを読み出して実行する。オペレーティングシステム（ＯＳ）をはじめ、本実施形態に係る各処理プログラム、デバイスドライバ等はＲＯＭ１０２に記憶されており、ＲＡＭ（ランダムアクセスメモリ）１０３に一時記憶され、ＣＰＵ１０１によって適宜実行される。入力Ｉ／Ｆ１０４は、外部の装置（撮像装置や操作装置など）から情報処理装置１００で処理可能な形式で入力信号として入力する。出力Ｉ／Ｆ１０５は、外部の装置（表示装置）へ表示装置が処理可能な形式で出力信号として出力する。

図２は、本実施形態に係る情報処理装置１００の構成を示している。
情報処理装置１００は、各処理部と記憶部２２を有している。各処理部は、計測データ保持部１０、概略位置姿勢算出部１１、３次元形状モデル保持部１２、モデル点サンプリング部１３、類似姿勢指定部１４、特定部位指定部１５、特定部位内サンプリング部１６、位置姿勢算出部１７、出力部２１を含む。また、情報処理装置１００は撮像装置１８、表示装置１９、操作装置２０、ロボットなどの外部装置の制御部２３に接続されている。なお、本実施形態では、情報処理装置１００の外部に撮像装置１８、表示装置１９、操作装置２０、制御部２３を接続しているが、撮像装置１８、表示装置１９、操作装置２０、制御部２３を含めて一体として情報処理装置として構成してもよい。

以下、情報処理装置１００が有する各部について説明する。
計測データ保持部１０は、撮像装置１８によって撮像された対象物体の濃淡画像（２次元画像）や距離画像（３次元情報）などの計測データ（計測情報）を取得して保持する。本実施形態において計測データ保持部１０は、撮像装置１８によって撮像した計測データを取得するが、これに限らず記憶媒体等から事前に取得した計測データを取得しても構わない。

概略位置姿勢算出部１１は、撮像装置１８に対する物体の位置及び姿勢の概略値（概略位置姿勢）を算出する概略算出部である。具体的には、まず、計測データ保持部１０から計測データを取得するとともに、３次元形状モデル保持部１２から対象物体の３次元モデルを取得する。そして、計測データ中のバラ積みされた物体の中から一個体を検出し、撮像装置１８に対する物体の位置及び姿勢の概略値を算出する。

撮像装置１８には、位置及び姿勢の計測の基準となる３次元の座標系（基準座標系）が規定されているものとする。本実施形態においては、撮像装置１８に使用するセンサの中心を原点とし、取得される画像の水平方向をｘ軸、垂直方向をｙ軸、センサの光軸をｚ軸とした座標系を基準座標系とする。撮像装置１８に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。

本実施形態では、センサで取得した距離画像および濃淡画像に対して複数の視点から観測した画像をテンプレートとしてパターンマッチングを行うことによりその一個体の基準座標系における概略位置姿勢を算出する。ただし、概略位置姿勢の認識方法は他の方法であっても構わない。例えば、基準座標系に対する対象物体の相対的な位置姿勢が既知であり、かつその位置姿勢が変化しないのであれば、対象物体が存在する空間における任意の位置を原点とした座標系を基準としてもよい。その他、バラ積みの中から１つ以上の個体を検出し、その３次元位置姿勢を算出できる方法であれば、ここで述べた以外のいかなる方法であっても構わない。本実施形態では、対象物体が所定の軸に対して回転させた場合に誤認識しやすい物体を対象にしているため、ここで取得した位置姿勢は誤認識されている可能性がある。概略位置姿勢算出部１１で算出した概略位置姿勢の情報は、位置姿勢算出部１７に入力される。

３次元形状モデル保持部１２は、バラ積みピッキングの対象とする対象物体の３次元形状モデルを取得し、保持する。したがって、３次元形状モデル保持部１２は、３次元形状モデルの取得部および保持部として機能する。３次元形状モデルには、たとえば、対象物体の３次元形状を複数ポリゴンの組み合わせにより近似表現したポリゴンモデルを用いることができる。各ポリゴンは、対象物体表面上の３次元座標における位置と、面を近似するポリゴンを構成するための各点の連結情報によって構成される。なお、ポリゴンは三角形で構成されるのが一般的であるが、四角形や五角形であってもよい。その他、表面点の３次元座標とその連結情報により物体形状を近似表現できるポリゴンモデルであればいかなるものであってもよい。あるいは、３次元形状モデルとして、ＣＡＤデータのように、Ｂ−Ｒｅｐ（Ｂｏｕｎｄａｒｙ−Ｒｅｐｒｅｓｅｎｔａｔｉｏｎ）表現と呼ばれる、区分されたパラメータ曲面の集合で形状を表現したモデルであってもよい。その他、物体の３次元形状を表現可能なモデルであればいかなるものであってもかまわない。なお、３次元形状モデルにはあらかじめ対象物体表面上の点の座標などを表す基準となるモデル座標系が設定されているものとする。

図３に、本実施形態で取り扱う対象物体とそのモデル座標系を示す。モデル座標系は、対象物体表面上の点を原点としたＸ^’軸、Ｙ^’軸、Ｚ^’軸で示されている。３次元形状モデル保持部１２は、ＲＯＭ１０２やＲＡＭ１０３などのメモリなどで構成されるが、記憶媒体等から３次元形状モデルを取得してもかまわない。保持される３次元形状モデルは、概略位置姿勢算出部１１、モデル点サンプリング部１３、類似姿勢指定部１４、特定部位指定部１５、位置姿勢算出部１７に入力される。

モデル点サンプリング部１３は、３次元形状モデル保持部１２から取得した３次元形状モデルの情報に基づいて、モデル点をサンプリングする。モデル点のサンプリングは、３次元形状モデルの情報をもとに、対象物体の位置及び姿勢の算出が可能な程度の密度で実施する。モデル点サンプリング部１３では、サンプリングしたモデル点について、さらに概略位置姿勢算出部１１から入力される概略位置姿勢の情報に基づいて、用いる点を選択する処理を実施してもよい。

モデル点サンプリング部１３は、用いるモデル点を選択する処理として、具体的には以下の処理を実施する。まず、３次元形状モデルを全方向から描画し、各方向から見える３次元形状モデルの幾何特徴を各方向に対応づけて登録する。次に、物体の概略位置姿勢と撮影パラメータから算出される視線ベクトルに最も近い方向に登録されている幾何特徴を選択することで、選択された幾何特徴に対応するモデル点を選択する。撮影パラメータと概略位置姿勢からモデル点を選択する方法としては、３次元形状モデル上の各点について３次元座標に加えて法線の情報も算出しておき、各方向の視線ベクトルと法線ベクトルとの内積を比較する方法を用いてもよい。この場合、内積値が負、すなわち視線ベクトルと法線ベクトルが対抗している点のみを登録する。

なお、モデル点サンプリング部１３によるモデル点のサンプリングはユーザ指示に基づいて行われてもよい。すなわち、ユーザが対象物体の３次元形状モデルが表示されたＧＵＩを参照しながら手動で点をサンプリングしてもよい。また、３次元形状モデルを構成する面について、各面の中心をモデル点としてサンプリングしてもよい。さらに、物体の概略位置姿勢に基づいて、計測データである距離画像上で均一になるような点をサンプリングしてもよい。具体的には、物体の概略位置姿勢と撮像装置１８の撮影パラメータに基づき３次元形状モデルを２次元画像上に投影し、２次元画像上で均一にサンプリングした３次元形状モデル上の点を３次元空間に逆投影する。その他、３次元形状モデルの表面上から点を算出するのであれば、その方法は特に問わない。モデル点サンプリング部１３でサンプリングされたモデル点の情報は、特定部位内サンプリング部１６と位置姿勢算出部１７に入力される。

類似姿勢指定部１４は、対象物体の３次元形状モデルを仮想的な３次元空間上に表示し、ユーザによる操作を介して、互いに誤りやすい異なる２つの姿勢（類似姿勢）の関係（変換パラメータ）を指定し、記憶部２２に登録する。類似姿勢の例については、図５や図７を用いて後述する。本実施形態では、類似姿勢指定部１４は、表示装置１９に３次元形状モデル保持部１２が保持する３次元形状モデルを送出し、表示装置１９のＧＵＩに対象物体の３次元形状モデル２つをレンダリングする。

そして、表示装置１９のＧＵＩに対して、操作装置２０によりユーザによる操作を取得して、互いに誤りやすいような姿勢に配置する。そして、このときの２つのモデルの仮想的な３次元空間における姿勢を取得し、類似姿勢間の変換パラメータを算出して類似姿勢として指定し、記録することで登録を行う。類似姿勢指定部１４で登録された変換パラメータは、特定部位指定部１５と位置姿勢算出部１７に入力される。なお、指定される類似姿勢は２つに限られるものではなく、３つ以上の互いに誤りやすい複数の類似姿勢を指定してもよい。

特定部位指定部１５は、類似姿勢指定部１４において登録した類似姿勢について、判別の手掛かりとなる特定部位を指定し、記憶部２２に登録する。特定部位は、類似姿勢の関係にある２つの姿勢において見た目が顕著に異なる部位である。具体的には、３次元形状モデルを形成する幾何特徴のうち、相手の３次元形状モデルを形成する幾何特徴と重ならない幾何特徴を登録する。特定部位の登録は、表示装置１９のＧＵＩに３次元形状モデル保持部１２から入力された対象物体の３次元形状モデルをレンダリングし、操作装置２０でユーザ操作によって選択された部位を取得して登録することが可能である。特定部位指定部１５で登録された特定部位の情報は、特定部位内でモデルデータをサンプリングするために特定部位内サンプリング部１６に入力される。

特定部位指定部１５では、類似姿勢の情報に加え、モデル点サンプリング部１３でモデルサンプリングを実施したときの３次元形状モデルとモデル点の相対位置関係の情報を用いて、特定部位を自動で登録する処理を実施してもよい。具体的には以下の処理を実施する。まず、仮想的な３次元空間上に類似姿勢の関係にある３次元形状モデルを２つレンダリングする。次に、３次元形状モデルとモデル点の相対位置関係の情報を用いて、レンダリングした各３次元形状モデルから特定部位算出用のモデル点をサンプリングする。サンプリングされた特定部位算出用のモデル点については、仮想的な３次元空間における３次元座標の情報とともに、どちらの３次元形状モデル由来の点であるかを属性情報として保持する。次に、各モデル点について、自分とは異なる属性情報をもつ近傍のモデル点との距離（モデル点間距離）を算出する。ここで、モデル点間距離の最小値が一定以上の長さである場合、その点が存在する部位について特定部位の可能性があると判断する。特定部位を指定する最後の処理として、モデル点間距離の最小値（最小モデル点間距離）が一定以上の長さであるモデル点を含む幾何特徴を特定部位として登録する。

特定部位内サンプリング部１６は、特定部位指定部１５から取得した特定部位の情報およびモデル点サンプリング部１３から取得したモデル点の情報に基づいて、類似姿勢との判別が十分可能な密度で特定部内でのモデル点のサンプリングを実施する。サンプリングの方法としては、どちらも３次元モデルの面上及びエッジ稜線上で均一になるようにサンプリングしてよいし、ランダムにサンプリングしても構わない。サンプリングしたモデル点については、その位置情報とともに、サンプリング元の幾何特徴（面もしくはエッジ稜線）との対応関係が記憶部２２に保持される。特定部位指定部１５でサンプリングされた特定部位のモデル点と対応関係の情報は、位置姿勢算出部１７に入力される。

位置姿勢算出部１７は、取得した情報に基づいて対象物体の位置及び姿勢（位置姿勢）を算出する。本実施形態において位置姿勢算出部１７が取得する情報は、計測データ（例えば、距離画像、濃淡画像）、３次元形状モデル、概略位置姿勢、２種類の方法でサンプリングされたモデル点、類似姿勢の変換パラメータ等である。
具体的にはまず、概略位置姿勢に基づき、３次元形状モデルと画像中の対象物体とが最もフィットするような位置姿勢を算出する。その後、類似姿勢の変換パラメータに基づき、算出された位置姿勢と誤りやすい関係にある位置姿勢を求め、その位置姿勢を初期値として別途モデルフィッティングを行う。その後、モデルフィッティング結果の評価値を、特定部位に含まれるモデル点を用いて算出・比較し、評価値のよい方の位置姿勢を最終結果として出力部２１に入力する。
モデルフィッティングは、モデル点を距離画像上もしくは濃淡画像上に投影し、画像上の幾何特徴とフィットするように位置姿勢を補正することで行う。モデル点と対応づける計測データについては、距離画像のみあるいは濃淡画像のみを用いてフィッティングを行ってもよいし、その両方を用いてもよい。

出力部２１は、位置姿勢算出部１７によって算出された対象物体の位置姿勢の情報を外部に出力する。出力先としては、対象物体を把持するロボットハンドの動作を制御する制御部２３等が挙げられる。

撮像装置１８は、対象物体の位置姿勢を認識するために必要な計測情報を取得するセンサであることが好ましい。たとえば、２次元画像を撮影するカメラでもよいし、各画素が奥行きの情報を持つ距離画像を撮影する距離センサでもよいし、これらを併用してもよい。距離センサとしては、対象物体に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測する方式のほか、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式等がある。また、ステレオカメラで撮影する画像から三角測量により距離を計算する方式も利用できる。その他、物体の３次元位置姿勢を認識するのに必要な情報を取得できればいかなるセンサであってもよい。

撮像装置１８は、対象物体に対して上方または横方などに固定されていてもよいし、またロボットハンドなどに備えつけられていてもよい。本実施形態では、距離画像および濃淡画像をともに取得可能なセンサを利用するものとする。上述のとおり、撮像装置１８が取得した２次元画像としての濃淡画像や距離画像などの計測データまたは計測情報は、計測データ保持部１０に入力される。なお、撮像装置１８に設定された座標系を以後、センサ座標系と呼ぶことにする。

表示装置１９は、３次元形状モデル保持部１２から類似姿勢指定部１４を介して３次元形状モデルを取得し、表示する。また、撮像装置１８から取得した画像や、位置姿勢算出部１７によって算出される位置姿勢を表示し、ユーザに確認させるようにしてもよい。表示装置１９は例えば液晶ディスプレイやＣＲＴディスプレイ等が用いられる。
操作装置２０は、例えば、キーボードとマウスであり、ユーザからの指示を入力するのに用いられ、特にマウスはＧＵＩを操作するのに用いられる。
なお、情報処理装置１００が備える各処理部の機能は、図１中のＣＰＵ１０１が、ＲＯＭ１０２に記録されているプログラムをＲＡＭ１０３にロードして実行することによって実現される。また、各処理部におけるデータの保持や記憶部２２におけるデータの記憶は、図１中のＲＯＭ１０２やＲＡＭ１０３などのメモリで行われる。

図４（Ａ）および図４（Ｂ）は、本実施形態における対象物体の位置姿勢を算出する処理手順を示すフローチャートである。本フローにおいては、まず位置姿勢算出の準備として、誤認識しやすい２つの位置姿勢（類似姿勢）に関する変換パラメータを登録するとともに、判別の手掛かりとなる特定部位を登録する。その後、登録された特定部位について、類似姿勢との判別が可能な数になるような密度でモデル点をサンプリングする。そして、位置姿勢算出時には、概略位置姿勢から一度フィッティングを行った後、登録された変換パラメータに基づき、類似姿勢を作成する。そして、作成された位置姿勢を初期値としてフィッティングを行った後、特定部位におけるフィッティング結果を比較し、評価値のより高い位置姿勢を採用する。

以下、各処理の詳細について説明する。
（ステップＳ４０１）
ステップＳ４０１において、情報処理装置１００に対象物体の３次元形状モデルを入力する。入力された３次元形状モデルは、３次元形状モデル保持部１２に保持される。モデル点サンプリング部１３、類似姿勢指定部１４、特定部位指定部１５、および位置姿勢算出部１７は、３次元形状モデル保持部１２から対象物体の３次元形状モデルを取得する。

（ステップＳ４０２）
ステップＳ４０２において、モデル点サンプリング部１３は、入力される３次元形状モデルの情報からモデル点をサンプリングする。ここでサンプリングされる点は、後述のステップＳ４０８とステップＳ４１０においてモデルフィッティングを行うために用いられる。モデルフィッティング用のモデル点をサンプリングする際には、サンプリングを実施する３次元形状モデル上の部位や、サンプリングするモデル点の数（即ち、サンプリング点数）について事前に設定を行う必要がある。なお、モデル点のサンプリングに関する情報については、本実施形態においてはサンプリング点数を設定しているが、３次元形状モデル上の面やエッジ稜線上にサンプリングを行うサンプリング密度を設定してもよい。

サンプリングを実施する３次元形状モデル上の部位については、例えば３次元形状モデル全体について実施するよう設定するのがよい。モデル全体についてサンプリングを実施することで、モデルフィッティングによって誤った位置姿勢を出力する可能性を低減することが期待できる。また、対象物体がモデルフィッティングに大きく寄与する幾何特徴を有している場合には、サンプリングを実施する部位を３次元形状モデルのうち幾何特徴の存在する部位のみに限定して設定してもよい。サンプリング点数については、モデルフィッティングの精度や処理時間についての所望の条件を満たす範囲内で、自由に設定して構わない。

（ステップＳ４０３）
ステップＳ４０３において、類似姿勢指定部１４は、互いに誤りやすい異なる２つの姿勢（第１の姿勢、第２の姿勢）の関係を表す、類似姿勢の変換パラメータを登録する。本ステップでの姿勢の登録方法は、例えば特許文献２に示されているような、ＧＵＩを用いた方法を用いることとする。このとき、ユーザは類似姿勢指定部１４を介して操作装置２０を用いてＧＵＩを操作する。

図５は、３次元形状モデルにおける類似姿勢および特定部位を説明する図である。３次元形状モデル保持部１２は表示装置１９に３次元形状モデルを送出し、表示装置１９は取得した３次元形状モデルを図５（Ａ）に示すように２つ、仮想空間内に配置した状態を表示する。ここで、表示された２つのモデルのうち一方を基準モデル５０１、もう一方のモデルを操作モデル５０２とする。表示装置１９の表示画面に表示された操作モデル５０２を情報処理装置１００のユーザに操作させ、基準モデル５０１に対して見た目が類似するような異なる姿勢に重ね合わせた状態を作成する。図５（Ｂ）はその例であり、２つのモデルが完全に重なった状態からモデル座標系のＺ^’軸まわりに操作モデル５０２を１８０度回転させることで、類似姿勢を作成することができる。類似姿勢を作成した後、２つの３次元形状モデル間の変換パラメータを算出する。

ここで、基準モデルおよび操作モデルのそれぞれの３次元形状モデルに対して設定されているモデル座標系をそれぞれ基準モデル座標系、操作モデル座標系と呼ぶ。さらに、仮想カメラに設定された座標系を仮想カメラ座標系と呼ぶ。なお、仮想カメラ座標系は、撮像装置１８の基準座標系と同様に設定するものとする。このとき、基準モデル座標系から仮想カメラ座標系への姿勢変換を行う３×３の回転行列、位置の変換を行う３列の並進ベクトルをそれぞれ、Ｒ_ＶＢ、ｔ_ＶＢとする。このとき、基準モデル座標系Ｘ_Ｂ＝［Ｘ_Ｂ，Ｙ_Ｂ，Ｚ_Ｂ］^Ｔから仮想カメラ座標系Ｘ_Ｖ＝［Ｘ_Ｖ，Ｙ_Ｖ，Ｚ_Ｖ］^Ｔへの変換は、４×４行列Ｔ_ＶＢを用いて以下のように表すことができる。
Ｘ_Ｖ’＝Ｔ_ＶＢＸ_Ｂ’
ここで、
Ｘ_Ｖ’＝［Ｘ_Ｖ，Ｙ_Ｖ，Ｚ_Ｖ，１］^Ｔ
Ｘ_Ｂ’＝［Ｘ_Ｂ，Ｙ_Ｂ，Ｚ_Ｂ，１］^Ｔ

である。これ以後、Ｔ_ＶＢを基準モデルの位置姿勢（第１の位置姿勢）と呼ぶ。

一方、操作モデル座標系から仮想カメラ座標系への姿勢変換を行う３×３の回転行列、位置の変換を行う３列の並進ベクトルをそれぞれ、Ｒ_ＶＯ、ｔ_ＶＯとする。このとき、操作モデル座標系Ｘ_Ｏ＝［Ｘ_Ｏ，Ｙ_Ｏ，Ｚ_Ｏ］^Ｔから仮想カメラ座標系Ｘ_Ｖ＝［Ｘ_Ｖ，Ｙ_Ｖ，Ｚ_Ｖ］^Ｔへの変換は、４×４行列Ｔ_ＶＯを用いて以下のように表すことができる。
Ｘ_Ｖ’＝Ｔ_ＶＯＸ_Ｏ’
ここで、
Ｘ_Ｏ’＝［Ｘ_Ｏ，Ｙ_Ｏ，Ｚ_Ｏ，１］^Ｔ
Ｘ_Ｖ’＝［Ｘ_Ｖ，Ｙ_Ｖ，Ｚ_Ｖ，１］^Ｔ

である。これ以後、Ｔ_ＶＯを操作モデルの位置姿勢（第２の位置姿勢）と呼ぶことにする。

基準モデルの位置姿勢Ｔ_ＶＢと、操作モデルの位置姿勢Ｔ_ＶＯより、２つの３次元形状モデル間の相対位置姿勢を求める。求める相対位置姿勢をＴ_ｒとすると、Ｔ_ｒは以下により求まる。
Ｔ_ｒ＝（Ｔ_ＶＢ）^−１Ｔ_ＶＯ
算出したＴ_ｒは位置を表す３パラメータ、姿勢を表す３パラメータの合計６パラメータで表現可能である。そこで、Ｔ_ｒから位置姿勢を表す６パラメータの値を取得し、これらを一組にしたものを変換パラメータとしてリストに追加する。なお、６パラメータの値の代わりに、４×４行列を構成する１６個のパラメータの値を一組にしたものを変換パラメータとすることもできる。あるいは、基準モデルの位置姿勢を表す６パラメータ、操作モデルの位置姿勢を表す６パラメータを一組にして変換パラメータとしてももちろんよい。その他、基準モデルと操作モデルとの相対位置姿勢Ｔ_ｒを復元可能、すなわち、２つのモデル間の位置姿勢を互いに変換可能であればいかなるパラメータを変換パラメータとしてもかまわない。また、姿勢を表す３パラメータのみを変換パラメータとしてもかまわない。

なお、本実施形態では変換パラメータを１つだけ登録したが、見た目を誤りやすい姿勢が複数存在する場合には、ここまで述べてきた操作を複数回実行することで、各変換パラメータの算出、リストへの追加を行えばよい。以上、ＧＵＩを用いた変換パラメータの登録方法について述べたが、ここで述べたＧＵＩは一例であり、これ以外のものを利用して類似姿勢の変換パラメータを登録しても構わない。以上、モデル点のサンプリング方法と類似姿勢の登録方法を述べたが、この処理は対象物体の３次元形状モデルの情報があれば実施可能であるため、本実施形態においてはステップＳ４０２とＳ４０３の順番を入れ替えて実施しても構わない。

（ステップＳ４０４）
ステップＳ４０４において、特定部位指定部１５は、ステップＳ４０３において登録された類似姿勢の判別に用いる特定部位を登録する。本実施形態における特定部位の登録は、ステップＳ４０３と同様に、ユーザがＧＵＩを用いて行う方法を用いることとする。ユーザは特定部位指定部１５を介して操作装置２０を用いてＧＵＩを操作する。表示装置１９に、類似姿勢の関係にある２つの３次元形状モデルと、特定部位を登録するための直方体が表示されている状態で、ユーザは操作装置２０を用いて直方体を移動もしくは拡大／縮小させて直方体内に囲まれる部位を選択し、特定部位として登録する。このとき、直方体を選択した時の仮想カメラから観測可能な操作モデル表面について、画面上で指定した直方体内における奥行き方向の存在範囲を算出し、算出した存在範囲と画面上の直方体により定義される３次元空間を算出する。そして、仮想カメラに対する操作モデルの位置姿勢に基づきモデル座標系に変換し直して記録する。

例えば、図５（Ｂ）のようにモデル座標系のＺ^’軸まわりに１８０度操作モデルを回転させ類似姿勢が登録されている場合、図５（Ｃ）のように画面と対象物体のＸＺ平面を略一致させた上で、特定部位指定用の矩形で特定部位５０３を指定する。類似姿勢を判別する手掛かりとなる形状は対象物体表面上の円筒形状であるため、円筒形状を含むように矩形を設定するのが有効である。その結果、図５（Ｄ）に示すように、直方体の領域が特定部位５０３として登録される。特定部位の指定にあたり、本実施形態では直方体を用いた方法について述べているが、直方体に限らず円、楕円、多角形など、他の幾何形状を用いた指定であっても構わない。

また、特定部位を登録するときに、ステップＳ４０２において登録した類似姿勢に基づき、対になる特定部位を新たに算出して記録してもよい。例えば、図６（Ａ）のような形状をした対象物体３の場合、図６（Ｂ）のようにモデル座標系のＺ^’軸まわりに１８０度回転させた類似姿勢を登録するのが姿勢判別に有効である。このような類似姿勢については、図６（Ｃ）のように特定部位として領域６０１、領域６０２を登録することが有効であると考えられるが、これらの２つの領域は対になる部位である。つまり、図６（Ｃ）のように二つの特定部位を登録した場合、位置姿勢算出時に類似姿勢の評価値を比較するためには、二つの姿勢の双方に関して特定部位を記録しておく必要がある。そこで、操作モデルを用いて記録した特定部位の３次元空間を変換パラメータに基づき類似姿勢に配置した基準モデルのモデル座標系に変換し、双方の３次元空間を記録する。これにより、類似姿勢の関係にある二つの姿勢についてそれぞれ比較すべき特定部位を記録することが可能である。なお、変換パラメータあるいは特定部位が複数登録されている場合は、それぞれの組み合わせについて本処理を実施する。以上、ＧＵＩを用いた特定部位の登録方法について述べたが、ここで述べたＧＵＩは一例であり、これ以外の方法を利用しても構わない。

（ステップＳ４０５）
ステップＳ４０５において、特定部位内サンプリング部１６は、ステップＳ４０４において登録された特定部位の情報からモデル点をサンプリングする。ここでサンプリングされる点は、後述のステップＳ４１２における評価値の算出に用いられる。ステップＳ４０５においてサンプリング類似姿勢判別用のモデル点をサンプリングする際には、サンプリングを実施する部位が特定部位に限定される。

サンプリング点数については事前に設定を行うが、ここでの設定値は、類似姿勢を判別するために必要なサンプリング点数以上の値としなければならない。ここで、設定されるサンプリング点数は大きいほど、後述のステップＳ４１１において算出される評価値の差が大きくなることが期待されるため、類似姿勢の判別が容易となる。図７は、ステップＳ４０２において対象物体全体についてサンプリングされたモデル点と、ステップＳ４０５において特定部位内についてサンプリングされたモデル点を示す図である。対象物体３の全体についてサンプリングされたモデル点と、特定部位についてサンプリングされたモデル点は、モデル点の密度が異なっており、特定部位内でサンプリングするモデル点の密度の方が高く設定される。特定部位内でサンプリングされたモデル点は、対象物体全体についてサンプリングされたモデル点よりも高い密度で生成されるため、類似姿勢を容易に判別することができる。

サンプリング点数の設定値は、例えば撮像装置１８によって取得される計測データのうち、特定部位として登録されている部位において存在しうる計測点の数の上限値とするのがよい。計測点の数の上限値は、撮像装置１８の解像度や、対象物体の計測データを取得可能な撮像装置１８の撮像可能範囲によって決まる値である。図８は、特定部位における幾何特徴上の計測点の数の上限値を推測する方法の説明図である。図８のように、仮想的な３次元空間において撮像装置１８の撮像可能範囲内に対象物体を配置することで、取得される画像上での対象物体３の幾何特徴のサイズ（エッジ稜線の長さや面の面積に対応する画素数）を推測することができる。一方、取得された画像から抽出される計測データの密度は、画像の解像度以上にはなり得ない。よって、幾何特徴の画像上でのサイズの情報と撮像装置１８の解像度の情報から、特定部位における幾何特徴上の計測点の数の上限値を推測することができる。このようにサンプリング点数を設定することで、類似姿勢を判別するモデル点を十分な数だけ確保できると期待される。

以上、特定部位におけるモデル点のサンプリングについて用途ごとに２つの方法を述べたが、モデル点のサンプリングに関する設定パラメータについてはＳ４０３と同様にサンプリング点数に限定されない。つまり、特定部位内のサンプリング密度を設定してもよい。また、ステップＳ４０３において複数の部位を特定部位として登録した場合については、登録した特定部位ごとに上述の方法で計測点の数の上限値を算出し、サンプリング点数の設定値としてもよい。

（ステップＳ４０６）
ステップＳ４０６において、計測データ保持部１０は、撮像装置１８により撮像された対象物体の距離画像および濃淡画像を取得する。図９（Ａ）に撮像した画像９００を示す。なお、本実施形態においては、モデル点のサンプリングの後に計測データを取得しているが、撮像装置１８によって撮像が実施された後であれば、ステップＳ４０１からステップＳ４０５までの一連の処理の中のどこで実施しても構わない。

（ステップＳ４０７）
ステップＳ４０７において、概略位置姿勢算出部１１は、撮影画像中に存在するバラ積みされた多数の対象物体の中から一個体の検出を行い、センサ座標系における対象物体の概略位置姿勢を表す６つのパラメータを算出して記録する。ここで算出した６つのパラメータに基づくモデル座標系からセンサ座標系への座標変換において、姿勢を表す３つのパラメータで表現される３×３の回転行列をＲ_ＳＭ、位置を表す３つのパラメータで表現される３列の並進ベクトルをｔ_ＳＭとする。このとき、モデル座標系Ｘ_Ｍ＝［Ｘ_Ｍ，Ｙ_Ｍ，Ｚ_Ｍ］^Ｔからセンサ座標系Ｘ_Ｓ＝［Ｘ_Ｓ，Ｙ_Ｓ，Ｚ_Ｓ］^Ｔへの変換は、４×４行列Ｔ_０’を用いて以下のように表すことができる。
Ｘ_Ｓ’＝Ｔ_０’Ｘ_Ｍ’
ここで、
Ｘ_Ｓ’＝［Ｘ_Ｓ，Ｙ_Ｓ，Ｚ_Ｓ，１］^Ｔ
Ｘ_Ｍ’＝［Ｘ_Ｍ，Ｙ_Ｍ，Ｚ_Ｍ，１］^Ｔ

である。これ以後、Ｔ_０’を概略位置姿勢と呼ぶことにする。図９（Ｂ）に、概略位置姿勢Ｔ_０’に基づき、撮影画像上に３次元形状モデルを投影した結果を示す。投影した３次元形状モデルが点線９０１である。

（ステップＳ４０８）
ステップＳ４０８において、位置姿勢算出部１７は、概略位置姿勢Ｔ_０’を初期値として３次元モデルと画像中の対象物体とのモデルフィッティングを行うことで、対象物体の位置姿勢を算出する。具体的には、３次元形状モデルを撮像装置のパラメータと概略位置姿勢とに基づいて撮影画像に投影する。そして、投影された３次元形状モデルの特徴と画像中の対象物体の特徴とを対応付けてその残差を小さくすることにより、対象物体の位置姿勢を算出する。対象物体の高精度な位置姿勢を算出する。ここで算出した位置姿勢の６パラメータにより表現可能な、モデル座標系からセンサ座標系への座標変換を行う４×４の回転行列をＴ_０とする。ここで、図９（Ｃ）に、フィッティング後の位置姿勢Ｔ_０に基づき、撮影画像上に３次元形状モデルを投影した結果を示す。投影した３次元形状モデルが点線９０２である。この例では、概略位置姿勢算出部１１が誤った一個体を検出したために、本ステップにおいて高精度な位置姿勢算出を行っても、正しい位置姿勢が算出されていない。

（ステップＳ４０９）
ステップＳ４０９において、位置姿勢算出部１７は、ステップＳ４０８で算出した位置姿勢に対して評価値を算出し、所定の閾値と比較を行うことで位置姿勢の正誤を判定し、後段の処理を実施するか否かの判定を行う。たとえばフィッティング後の位置姿勢におけるモデル表面上の幾何特徴と、画像中の幾何特徴との３次元的な距離を残差とする（ずれ量取得）。そして、全幾何特徴の残差の平均値Ｅをスコアとして用いることができる。

算出した残差の平均値Ｅが所定の閾値（たとえば０．１ｍｍ）よりも小さければ、正しい位置姿勢が導出できたと判定して本処理を終了する。一方、残差の平均値が閾値よりも大きければ、誤った位置姿勢が求まったものと判定して、ステップＳ４１０へ進む。閾値は、例えば、予めユーザによって設定しておけばよい。また、位置姿勢の正誤判定方法はこれに限るものではない。たとえば、算出されたＴ_０に基づき、モデルを投影してレンダリングした画像と、撮影画像とでの、物体部位内の輝度の正規化相互相関係数Ｒを求め、この値を利用してもよい。この場合、Ｒが所定値（たとえば０．９）よりも大きければステップＳ４１１へ進む。一方、小さければステップＳ４０８へ進む。なお、この方法においてモデルを投影してレンダリングを実施する際には、対象物体の表面特性を考慮して輝度の計算に反映させてもよい。このほか、ステップＳ４０８で算出された位置姿勢の正誤を峻別可能な方法であればいかなる方法であっても構わない。なお、この処理を省略し、必ずステップＳ４１０へ進むようにしてもよい。

（ステップＳ４１０）
ステップＳ４１０において、位置姿勢算出部１７は、位置姿勢Ｔ_０と、類似姿勢指定部１４から取得したＮ組の各変換パラメータとを用いて、新たな位置姿勢の候補を生成する。まず、変換パラメータから復元可能な相対位置姿勢をＴ_ｒ＿ｉ（ｉ＝１〜Ｎ）と表現し、それぞれを用いて作成した新たな候補の位置姿勢をＴ_ｉ’とする。Ｔ_ｉ’は以下により算出する。
Ｔ_ｉ’＝Ｔ_０Ｔ_ｒ＿ｉ ^−１
図９（Ｄ）は、新たな候補の位置姿勢Ｔ_ｉ’に基づき、撮影画像上に３次元形状モデルを投影したものである。投影した３次元形状モデルが点線９０３である。次に位置姿勢算出部１７は、生成された新たな候補の位置姿勢Ｔ_ｉ’を初期値として、ステップＳ４０８と同様に撮影画像と３次元形状モデルとがフィットするように、対象物体の位置姿勢を算出（導出）する。ここで算出された位置姿勢をＴ_ｉとする。

（ステップＳ４１１）
ステップＳ４１１において、位置姿勢算出部１７は、ステップＳ４０９で生成されたＮ個の位置姿勢Ｔ_ｉの算出が完了しているかを判定する。Ｎ個のＴ_ｉの算出が完了していればステップＳ４１２へ処理を進め、完了していなければステップＳ４１０へ処理を戻す。なお、Ｎ個の新たな候補位置姿勢に対してステップＳ４１０の処理を並列化して実施してもよい。

（ステップＳ４１２）
ステップＳ４１２において、位置姿勢算出部１７は、ステップＳ４１０で算出した（Ｎ＋１）個の位置姿勢Ｔ_ｉ（ｉ＝０〜Ｎ）に対して、Ｓ３０５で決定した特定部位のサンプリングに関する情報に基づいて、評価値を算出する。具体的には、特定部位のモデル点と計測点の一致度から評価値を算出する。そして、算出した評価値のうち、最良の評価値に対応する位置姿勢を最終的な対象物体の位置姿勢として出力する。

ここで用いる評価値には、ステップＳ４０９と同様に残差を用いてもよいし、算出された位置姿勢に基づいて対象物体を投影した画像と、撮影画像との正規化相互相関を用いてもよい。その他、複数の位置姿勢の正誤を評価値に基づいて峻別可能な方法であればいかなる方法であっても構わない。図９（Ｅ）は、Ｔ_０、Ｔ_ｉの２つの位置姿勢を評価し、Ｔ_ｉを最終位置姿勢として出力した様子を示す。このとき出力される最終位置姿勢が点線９０４で示される位置姿勢である。

以上、第１実施形態では、特定部位に応じた設定値で事前にモデル点のサンプリングを行った後で、サンプリングしたモデル点を用いてモデルフィッティングと類似姿勢の判別を実施する方法について述べた。この方法を用いることにより、局所的な部位の形状が異なるような対象物体について、高速かつ高精度に姿勢の違いを判別することが可能となる。

（第２実施形態）
第１実施形態においては、特定部位についてなるべく多くのモデル点をサンプリングするようにパラメータを設定した。モデル点の数が多いほど類似姿勢の判別が容易となる一方、評価値を算出するための処理時間が増大する。サンプリング点数もしくはサンプリング密度の設定値が過剰である場合、類似姿勢の峻別性と処理時間との両立ができなくなる場合が考えられる。そこで、本実施形態では、サンプリングするモデル点の数が判定の精度を維持しつつも処理時間の増加を抑制することができる所定の基準値以下になるよう、サンプリング点数もしくはサンプリング密度を決定する。具体的には、特定部位のサンプリングを行った後に、サンプリングした点の数が過剰であるかどうかを判別し、過剰分については間引くという処理を追加で行う。本実施形態にかかる情報処理装置１００の構成は第１実施形態と同様であるため説明を省略する。

次に、本実施形態の処理手順について説明する。図１０は、本実施形態における対象物体の位置姿勢を算出する処理手順を示す図である。図１０（Ａ）において、ステップＳ１００１からステップＳ１００５までは、第１実施形態におけるステップＳ４０１からステップＳ４０５までと同じ処理であるため説明を省略する。また、図１０（Ｂ）におけるステップＳ１００８からステップＳ１０１４までは、第１実施形態におけるステップＳ４０６からステップＳ４１２までと同じ処理であるため、これらについても説明を省略する。

（ステップＳ１００６）
ステップＳ１００６において、特定部位内サンプリング部１６は、ステップＳ１００５においてサンプリングしたモデル点の数についてサンプリングするモデル点数の所定の基準値との比較を行い、モデル点数が過剰であるか否か判定する。モデル点数の所定の基準値（以下、「サンプリング基準点数」ともいう）は面積に関わらず設定してよいが、観測される面積ごとに段階的に設定してもよい。ここで、サンプリング基準点数は事前に設定されるパラメータである。サンプリング基準点数は、類似姿勢を判別でき、かつ所望の処理時間以内で一連の処理を実施可能な範囲内で設定される。

特定部位内のモデル点の数が過剰であるかどうかを判別する方法としては、例えばサンプリング処理を実施する際に、生成したモデル点の数を数える方法がある。ステップＳ１００６において、特定部内におけるモデル点の数が基準モデル点数より多い場合、モデル点の数が過剰であると判断され、ステップＳ１００７へ進む。一方、特定部位内におけるモデル点の数が基準モデル点数以下である場合はステップＳ１００８に進む。

（ステップＳ１００７）
ステップＳ１００７において、特定部位内サンプリング部は、過剰であると判断されたモデル点を間引く処理を実施し、モデル点の数が所定の基準値以下になるようにする。モデル点を間引く方法としては、例えば特定部位におけるモデル点がなるべく等間隔に分布するように間引く方法がある。具体的には、まずサンプリング基準値の情報と、特定部位に含まれる幾何特徴の面積や稜線の長さの情報から、間引き後のモデル点について、モデル点が一様に分布している場合を想定したモデル点間の距離の理想値を算出する。次に、特定部位内で実際にサンプリングされている各モデル点について再近傍のモデル点との距離を算出する。距離が理想値より短い場合には、距離を算出するのに用いたモデル点のどちらかを間引く。この処理を全モデル点について順次実施することにより、モデル点がおおよそ等間隔に分布するようにモデル点を間引くことができる。また、モデル点を間引くその他の方法としては、ランダムに間引く方法を用いてもよい。

なお、モデル点の間引きに関する設定パラメータについては、サンプリング基準点数を設定しているが、間引き処理を実施した後の３次元形状モデル上の面やエッジ稜線上のモデル点の密度（以下、「サンプリング基準密度」ともいう）を設定してもよい。その場合に過剰なモデル点を間引く方法としてはまず、特定部位を所定の表面積ごとに分割し、領域内に存在するモデル点数の理想値（以下、「領域内基準モデル点数」ともいう）をサンプリング基準密度の情報から算出する。その後、分割した領域ごとに領域内に実際に存在するモデル点の数を数え、モデル点の数が領域内基準モデル点数より多い場合には、各領域において過剰な分だけのモデル点を間引く。この処理を実施することで、モデル点がおよそ一様に分布するようにモデル点を間引くことができる。

以上、本実施形態における処理手順について説明したが、本実施形態における特徴的な処理であるステップＳ１００６とステップＳ１００７について、これらの処理を必ずしもステップＳ１００５の直後に実施する必要はない。つまり、ステップＳ１００５において特定部位内のモデル点を作成してから、ステップＳ１０１４において対象物体の位置姿勢についての評価値を算出するのにモデル点が用いられるまでの間であれば、任意のタイミングで実施してもよい。また、特定部位を複数箇所指定した場合は、指定した特定部位ごとにモデル点数が過剰であるかどうかの判別とモデル点の間引き処理を実施するようにしてもよい。
以上説明したように、特定部位のサンプリングを行った後に、サンプリングした点の数が過剰であるかどうかを判別し、過剰分については間引くという処理を追加で行うことにより、判定の精度を維持しつつも処理時間の増加を抑制することができる。

（第３実施形態）

第１実施形態、第２実施形態においては、登録した特定部位について最適と思われる数だけモデル点を一意にサンプリングし、サンプリングされたモデル点を類似姿勢の判別に使用する。これら実施形態は、撮影画像上で対象物体がある程度限定された姿勢しかとらず、なおかつある程度同じ大きさで観測される場合を想定している。しかしながら、計測範囲がある程度広くて、かつ対象物体がバラ積み状態で配置されている場合、対象物体は様々な姿勢を取り得、なおかつ撮像装置１８の計測範囲内で、画像上では様々な大きさで観測されうる。その場合、第１実施形態、第２実施形態においてサンプリングしたモデル点の数は、必ずしも最適なものになるとは限らず、対象物体の配置によっては過剰な数のモデル点をサンプリングしてしまう可能性がある。本実施形態は、計測範囲内で対象物体がどこに配置されるかによって、最適なモデル点を選択して類似姿勢の判別に用いる。具体的には、対象物体の概略位置姿勢がどのような値となるかによって、どのモデル点をサンプリングするかをあらかじめ設定しておき、算出された概略位置姿勢の情報をもとに対応するモデル点を選択して類似姿勢の判別に使用する。この方法を用いると、計測範囲内で対象物体が様々な位置姿勢を取っていても最適な条件で類似姿勢を判別することができる。なお、本実施形態にかかる情報処理装置１００の構成は第１実施形態と同様であるため説明を省略する。

本実施形態の処理手順について説明する。図１１は、本実施形態における対象物体の位置姿勢を算出する処理手順を示す図である。図１１（Ａ）において、ステップＳ１１０１からステップＳ１１０４までは、第１実施形態におけるステップＳ４０１からステップＳ４０４までと同じ処理であるため説明を省略する。また、図１１（Ｂ）におけるステップＳ１１０７とステップＳ１１０８は、第１実施形態におけるステップＳ４０６とステップＳ４０７と同じ処理であるため、これらについても説明を省略する。さらに、図１１（Ｂ）におけるステップＳ１１１０からステップＳ１１１４については、第１実施形態におけるステップＳ４０８からステップＳ４１２までと同じ処理であるため、これらについても説明を省略する。

（ステップＳ１１０５）
ステップＳ１１０５において、特定部位内サンプリング部１６は、モデル点を設定する概略位置姿勢（以下、「概略位置姿勢候補」ともいう）と、概略位置姿勢候補ごとのサンプリング条件を設定する。サンプリング条件とは、例えば特定部位内のサンプリング密度やサンプリングするモデル点の数が挙げられる。サンプリング条件を設定する方法としては、例えば計測範囲内における対象物の位置ごとに設定する方法等がある。

図１２は、概略位置姿勢ごとのモデル点のサンプリング方法についての説明図である。
図１２（Ａ）では、撮像装置１８から見て、撮像範囲８００の中で手前側の位置に対象物体１２００が、奥側の位置に対象物体１２１０が配置されている。この場合、手前側の対象物体１２００の方が奥側の対象物体１２１０よりも大きく撮像される。そのため、手前側の対象物体１２００の方が、奥側の対象物体１２１０よりもモデル点の密度が低くても類似姿勢の判別が可能である。よって、図１２（Ａ）のような場合には、撮像装置の手前側におけるモデル点のサンプリング密度を、奥側におけるモデル点のサンプリング密度よりも低く設定するのが有効である。概略位置姿勢候補の数については、２つに限らず、いくつ設定してもよい。その場合、配置される対象物体が類似姿勢の判別に有利な位置（例えば、幾何特徴が大きく観測される位置）になる場合はサンプリング密度を低く設定するのが有効である。一方、配置される対象物体が類似姿勢の判別に不利な位置（例えば、幾何特徴が小さく観測される位置）になる場合は、サンプリング密度を高く設定するのが有効である。

サンプリング条件は、計測範囲内の位置だけではなく、対象物体の姿勢ごとに設定してもよい。例えば、図１２（Ｂ）のように観測される姿勢からＸＹＺ各軸まわりに９０度刻みで回転させた４×４×４通りの６４姿勢を概略位置姿勢候補とし、各姿勢における特定部位内の幾何特徴の見え方からそれぞれサンプリング密度を設定することができる。その一例として、図１２（Ｂ）の姿勢からＹ軸周りに９０度回転させた図１２（Ｃ）のように観測される姿勢を示す。図１２（Ｂ）の方が特定部位４内の幾何特徴が大きく観測される。そのような場合、図１２（Ｂ）のような姿勢におけるサンプリング密度を図１２（Ｃ）におけるサンプリング密度よりも低く設定するのが有効である。同様に、残りの姿勢についても撮影画像上での特定部位内の幾何特徴の見え方が異なる。そのため、類似姿勢の判別に有利な姿勢（例えば、幾何特徴が多く観測される姿勢）ではサンプリング密度を低く設定し、類似姿勢の判別に不利な姿勢（例えば、幾何特徴があまり観測されない姿勢）ではサンプリング密度を高く設定するのが有効である。なお、本実施形態では例としてＸＹＺ各軸まわりに９０度刻みで回転させた場合について説明したが、それに限らず任意の姿勢についてサンプリング密度を設定して構わない。

以上、概略位置姿勢候補ごとにサンプリング条件を設定する２つの方法を説明したが、これらの方法を組み合わせてもよい。例えば、位置については図１２（Ａ）のような２つの位置を想定し、かつ姿勢については対象物体の姿勢をＸＹＺ各軸まわりに９０度刻みで回転させた６４姿勢を想定した場合、全部で１２８通りの概略位置姿勢候補を想定することができる。この１２８通りの概略位置姿勢候補のそれぞれについてサンプリング点数を設定することで、より多くの位置姿勢について最適な条件で類似姿勢を判別することができる。

サンプリング点数の設定方法としては、位置ごと、姿勢ごと、もしくはそれらを組み合わせた位置姿勢候補ごとにサンプリング密度の値そのものを設定する方法を用いてもよいが、それ以外の方法を用いてもよい。例えば、あらかじめ基準となる位置姿勢候補（以下、「位置姿勢基準」ともいう）とその位置姿勢におけるサンプリング密度だけ設定しておく。そして、それ以外の位置姿勢候補においては、位置姿勢基準からの差分もしくは基準位置姿勢におけるサンプリング点数との割合を設定してもよい。また、サンプリング条件を設定するのは、１つの概略位置姿勢候補に限らず、複数の概略位置姿勢候補について共通のサンプリング条件を設定しても構わない。また、概略位置姿勢候補に紐づけてサンプリング条件を設定する方法に限らず、計測範囲内の領域や、姿勢の範囲に紐づけてサンプリング条件を設定してもよい。

（ステップＳ１１０６）
図１１のステップＳ１１０６において、特定部位内サンプリング部１６は、ステップＳ１１０５において設定されたサンプリング条件に従って概略位置姿勢候補ごとにモデル点のサンプリングを実施する。サンプリングされたモデル点の情報は概略位置姿勢候補に紐づけられる。概略位置姿勢候補の情報とそれに紐づくモデル点の情報は位置姿勢算出部１７に送られ、後述の処理であるステップＳ１１０９においてどのモデル点を使用するかが決定される。

（ステップＳ１１０９）
ステップＳ１１０９において、位置姿勢算出部１７はステップＳ１１０８において算出された概略位置姿勢の情報をもとに、どのモデル点を類似姿勢の判別に用いるかを決定する。使用するモデル点の決定方法としてはまず、ステップＳ１１０８において算出された概略位置姿勢と、ステップＳ１１０５で設定された概略位置姿勢候補の照合を行う。概略位置姿勢候補の中で、ステップＳ１１０８において算出された概略位置姿勢と一致するものがあった場合は、一致した概略位置姿勢候補に紐づけられたモデル点が類似姿勢の判別に用いられる。概略位置姿勢候補の中で、ステップＳ１１０８において算出された概略位置姿勢と一致するものがなかった場合は、最も近い概略位置姿勢候補が選択され、それに紐づくモデル点が類似姿勢の判別に用いられる。

以上説明したように、本実施形態によると、概略位置姿勢の情報に基づいて対応するモデル点を選択して類似姿勢の判別を行うことができるので、様々な位置姿勢の対象物体の計測において、判定の精度と処理時間の抑制を両立することができる。

（第４実施形態）
第１実施形態、第２実施形態、第３実施形態では、姿勢を誤りやすい形状の特徴をもつ同じ形状の対象物体を判別する方法について述べた。本実施形態では、対象物体と形状の一部が異なる類似物体とを判別する方法について述べる。
図１３は、対象物体と類似物体を示す図である。本実施形態で想定するのは、図１３（Ａ）で示すような形状をした対象物体３と、図１３（Ｂ）で示すような形状をした類似物体１３００との判別である。両者は形状の一部（対象物体３の特定部位４と類似物体１３００の特定部位１３０１）が異なっており、例えば対象物体のバラ積みに類似物体が混入していた場合、誤認識する可能性がある。そのような誤認識を防ぐために、まず対象物体とともに類似物体の３次元形状モデルを入力し、対象物体と類似物体で誤認識する可能性のある相対姿勢を登録した上で判別の手掛かりとなる特定部位を登録する。図１３（Ｃ）及び図１３（Ｄ）は、登録する特定部位５０３の一例を示す図である。その後、対象物体と類似物体の３次元形状モデルにおいてそれぞれ特定部位５０３内のモデル点をサンプリングし、撮影画像上で観測される物体についてモデルフィッティングを実施して特定部位内での評価値を算出する。そして、対象物体のモデル点を用いた特定部位内での評価値と、類似物体のモデル点を用いた特定部位内での評価値とを比較することで、対象物体と類似物体を判別する。なお、本実施形態にかかる情報処理装置１００の構成は第１実施形態と同様であるため説明を省略する。

次に、本実施形態の処理手順について説明する。図１４は、本実施形態における対象物体と類似物体を判別する処理手順を示す図である。図１４（Ｂ）において、ステップＳ１４０６からステップＳ１４０８までの処理は、第１実施形態におけるステップＳ４０６からステップＳ４０８までと同じ処理であるため、説明を省略する。

（ステップＳ１４０１）
ステップＳ１４０１において、モデル点サンプリング部１３、類似姿勢指定部１４、特定部位指定部１５、および位置姿勢算出部１７は、３次元形状モデル保持部１２から対象物体の３次元形状モデルと類似物体の３次元形状モデルを取得する。これらの処理を実施する順番は任意であり、もしくは同時に取得しても構わない。

（ステップＳ１４０２）
ステップＳ１４０２において、モデル点サンプリング部１３は、入力される対象物体の３次元形状モデルの情報と類似物体の３次元形状モデルの情報から、それぞれモデル点をサンプリングする。ここでサンプリングされる点は、後述のステップＳ１４０８とステップＳ１４１０においてモデルフィッティングを行うために用いられる。ここで、サンプリングを実施する条件については第１実施形態のステップＳ４０２における条件と同様であるため、説明を省略する。

（ステップＳ１４０３）
ステップＳ１４０３において、類似姿勢指定部１４は、対象物体と類似物体について誤認識しやすい相対位置姿勢の変換パラメータを登録する。図１３（Ａ）に示すような対象物体と類似物体については、図１３（Ｂ）に示すような相対位置姿勢に対応する変換パラメータが登録される。相対位置姿勢の変換パラメータの登録方法については第１実施形態のステップＳ４０３における方法と同様であるため、説明を省略する。

（ステップＳ１４０４）
ステップＳ１４０４において、特定部位指定部１５は、ステップＳ１４０３において登録された相対位置姿勢で対象物体と類似物体を判別する特定部位を登録する。ステップＳ１４０３において図１３（Ｂ）に示すような相対位置姿勢が登録された場合、図１３（Ｃ）に示すように特定部位を登録することが有効である。本実施形態における特定部位の登録方法は、ステップＳ１４０３、および第１実施形態のステップＳ４０４における方法と同様であるため、説明を省略する。

（ステップＳ１４０５）
ステップＳ１４０５において、特定部位内サンプリング部１６は、ステップＳ１４０４において登録された特定部位の情報から対象物体の３次元形状モデルと類似物体の３次元形状モデルのそれぞれについて、特定部位内でモデル点をサンプリングする。ここで、サンプリングされる点は、後述のステップＳ１４１１における評価値の算出に用いられる。ここで、サンプリングを実施する際の条件については、第１実施形態のステップＳ４０５における条件と同様であるため、説明を省略する。

（ステップＳ１４０９）
ステップＳ１４０９において、位置姿勢算出部１７は、ステップＳ１４０８で算出した対象物体の位置姿勢に対して評価値を算出し、所定の閾値と比較を行う。評価値の例としては、第１実施形態と同様に、フィッティング後の位置姿勢におけるモデル表面上の幾何特徴と画像中の幾何特徴との３次元的な距離の残差について、全幾何特徴の残差の平均値を用いることができる。算出した残差の平均値Ｅが所定の閾値よりも小さければ、対象物体であると判定し、以降の処理を省略することができる。一方、Ｅが所定の閾値よりも大きければ、類似物体である可能性があると判定してステップＳ１４１０に進む。このほか、ステップＳ１４０８で算出された位置姿勢について、対象物体と類似物体を峻別可能な方法であればいかなる方法であっても構わない。
なお、この処理を省略し、必ずステップＳ１４１０へ進むようにしてもよい。

（ステップＳ１４１０）
ステップＳ１４１０において、位置姿勢算出部１７は、ステップＳ１４０８において算出された位置姿勢Ｔ_０と、対象物体の姿勢と類似物体の姿勢との変換パラメータを用いて、新たな位置姿勢の候補を生成する。まず、変換パラメータから復元可能な相対位置姿勢をＴと表現し、それぞれを用いて作成した新たな候補の位置姿勢をＴ’とする。Ｔ’は以下により算出する。
Ｔ’＝Ｔ_０Ｔ^−１
次に位置姿勢算出部１７は、生成された新たな候補の位置姿勢Ｔ’を初期値として、撮影画像と３次元形状モデルとがフィットするように位置姿勢を算出する。ここで、３次元形状モデルは、対象物体のモデルと類似物体のモデルを両方用い、それぞれについて姿勢を算出する。対象物体の３次元形状モデルを用いて算出された位置姿勢をＴ_Ａ、類似物体の３次元形状モデルを用いて算出された位置姿勢をＴ_Ｂとする。

（ステップＳ１４１１）
ステップＳ１４１１において、位置姿勢算出部１７は、ステップＳ１４１０で算出した位置姿勢Ｔ_Ａ、Ｔ_Ｂに対し、特定部位として登録された部位のモデル点と計測点の一致度からそれぞれ評価値を算出する。ここで用いる評価値には、ステップＳ１４０９と同様に残差を用いてもよいし、算出された位置姿勢に基づいて対象物体を投影した画像と、撮影画像との正規化相互相関を用いてもよい。その他、複数の位置姿勢の正誤を評価値に基づいて峻別可能な方法であればいかなる方法であっても構わない。

（ステップＳ１４１２）
ステップＳ１４１２において、位置姿勢算出部１７は位置姿勢Ｔ_Ａについて算出した評価値（評価値Ａ）と、位置姿勢Ｔ_Ｂについて算出した評価値（評価値Ｂ）を比較する。具体的には、評価値Ａが評価値Ｂよりも高い値であった場合、撮影画像中の物体は対象物体であると判定される。一方、評価値Ｂが評価値Ａよりも高い値であった場合、撮影画像中の物体は類似物体であると判定される。

以上、本実施形態における処理手順について説明したが、本実施形態では必ずしも図１４の処理フローに厳密に従う必要はない。例えば、ステップＳ１４０５において特定部位内でモデル点のサンプリングを実施する際には、第２実施形態のように過剰にサンプリングされているかを判別し、過剰であった場合には間引く処理を追加してもよい。また、第３実施形態のように、特定部位内サンプリングを実施する概略位置姿勢候補点ごとにサンプリング条件を設定する方法を用いてもよい。

（第５実施形態）
上述の撮像装置１８は、ある支持部材に支持された状態で使用されうる。本実施形態では、一例として、図１５のように把持装置としてのロボットアーム１５００に備え付けられて使用される制御システムについて説明する。撮像装置１８を有する計測装置は、支持台１５５０にバラ積みされた対象物体３にパターン光を投影して撮像し、情報処理装置１００はその画像を取得する。そして、情報処理装置１００の位置姿勢算出部１７が、対象物体３の位置および姿勢を求め、求められた位置および姿勢の情報を制御部２３が取得する。制御部２３は、計測結果である、対象物体３の位置および姿勢の情報に基づいて、ロボットアーム１５００に駆動指令を送ってロボットアーム１５００を制御する。ロボットアーム１５００は保持部、把持部としての先端のロボットハンドなどで対象物体３を保持して、並進や回転などの移動をさせる。さらに、ロボットアーム１５００によって対象物体３を他の部品に組み付けることにより、複数の部品で構成された物品、例えば電子回路基板や機械などを製造することができる。また、移動された対象物体３を加工することにより、物品を製造することができる。また、情報処理装置１００が取得した計測データや画像や算出結果をディスプレイなどの表示装置１９に表示してもよい。

（物品の製造方法に係る実施形態）
以上に説明した実施形態に係る計測装置は、物品製造方法に使用しうる。当該物品製造方法は、当該計測装置を用いて物体の計測を行う工程と、計測の結果に基づいて当該工程で計測を行われた物体の処理を行う工程と、を含みうる。当該処理は、例えば、加工、切断、搬送、組立（組付）、検査、および選別のうちの少なくともいずれか一つを含みうる。本実施形態の物品製造方法は、従来の方法に比べて、物品の性能・品質・生産性・生産コストのうちの少なくとも１つにおいて有利である。

（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

以上、本発明の好ましい実施形態について説明したが、本発明は、これら実施形態に限定されず、その要旨の範囲内で種々の変形および変更が可能である。

１００ 情報処理装置
１２ ３次元形状モデル保持部
１５ 特定部位指定部
１６ 特定部位内サンプリング部
１７ 位置姿勢算出部

Claims (15)

1. 対象物体の位置および姿勢を算出する情報処理装置であって、
   前記対象物体の形状の計測データおよび前記対象物体の形状モデルを取得する取得部と、
   前記取得部が取得した前記形状モデルにおいて、前記対象物体の姿勢を特定するための特定部位のサンプリングに関する情報を決定するサンプリング部と、
   前記サンプリング部が決定した前記特定部位の前記サンプリングに関する情報および前記対象物体の形状の計測データに基づいて前記対象物体の位置および姿勢を算出する算出部と、を有することを特徴とする情報処理装置。
2. 前記サンプリング部は、前記サンプリングに関する情報として、サンプリングするモデル点の密度を決定し、当該決定された前記特定部位のモデル点の密度は、前記特定部位とは異なる他の領域をサンプリングするモデル点の密度より高いことを特徴とする請求項１に記載の情報処理装置。
3. 前記サンプリング部は、前記対象物体を撮像する撮像装置の解像度または撮像可能範囲に基づいて、前記サンプリングに関する情報を決定することを特徴とする請求項１または請求項２に記載の情報処理装置。
4. 前記サンプリング部は、前記サンプリングに関する情報として、サンプリングするモデル点の数を所定の基準値以下の範囲で決定することを特徴とする請求項１乃至３のうちいずれか１項に記載の情報処理装置。
5. 前記対象物体の概略の位置および姿勢を算出する概略算出部をさらに有し、
   前記サンプリング部は、前記対象物体の概略位置姿勢の候補ごとに前記サンプリングに関する情報を設定し、
   前記算出部は、前記概略算出部が算出した前記対象物体の概略の位置および姿勢に基づいて前記対象物体の位置および姿勢を算出するのに用いる前記サンプリングに関する情報を決定することを特徴とする請求項１乃至４のうちいずれか１項に記載の情報処理装置。
6. 前記取得部は、さらに、前記対象物体と形状の類似する類似物体の３次元形状モデルを取得し、
   前記サンプリング部は、さらに、前記対象物体と前記類似物体との判別に用いる特定部位のサンプリングに関する情報を決定し、
   前記算出部は、前記サンプリング部が決定した、前記対象物体の前記サンプリングに関する情報と前記類似物体の前記サンプリングに関する情報に基づいて、計測された物体が前記対象物体であるか否か判断することを特徴とする請求項１乃至５のうちいずれか１項に記載の情報処理装置。
7. 前記算出部により算出された前記対象物体の位置および姿勢の情報を出力する出力部をさらに有することを特徴とする請求項１乃至６のうちいずれか１項に記載の情報処理装置。
8. 前記特定部位は、前記対象物体の類似姿勢を判別するための部位であることを特徴とする請求項１乃至７のうちいずれか１項に記載の情報処理装置。
9. 対象物体の位置および姿勢を計測する計測装置であって、
   前記対象物体の形状を計測する計測部と、
   請求項１乃至８のうちいずれか１項に記載の情報処理装置と、を有し、
   前記情報処理装置は、前記計測部によって計測された前記対象物体の形状の計測データを取得し、前記対象物体の位置および姿勢を算出することを特徴とする計測装置。
10. 請求項１乃至８のうちいずれか１項に記載の情報処理装置と、
    前記対象物体を保持して移動させるロボットと、を有し、
    前記情報処理装置から出力された前記対象物体の位置および姿勢に基づいて、前記ロボットが前記対象物体を保持する、ことを特徴とするシステム。
11. 対象物体の位置および姿勢を計測する計測装置であって、
    前記対象物体の形状を計測する計測部と、
    前記計測部による前記形状の計測データと前記対象物体の形状モデルとを比較することによって、前記対象物体の位置および姿勢を算出する処理部と、を有し、
    前記処理部は、前記対象物体の形状モデルにおいて前記対象物体の姿勢を特定するための特定部位のサンプリングに関する情報を決定し、前記サンプリングに関する情報に基づいて前記対象物体の位置および姿勢を算出することを特徴とする計測装置。
12. 請求項１１に記載の計測装置と、
    前記対象物体を保持して移動させるロボットと、を有し、
    前記計測装置から出力された前記対象物体の位置および姿勢に基づいて、前記ロボットが前記対象物体を保持する、ことを特徴とするシステム。
13. 対象物体の位置および姿勢を算出する算出方法であって、
    前記対象物体の形状の計測データおよび前記対象物体の形状モデルを取得する取得工程と、
    前記取得工程で取得した前記形状モデルにおいて、前記対象物体の類似姿勢を判別し、姿勢を特定するための特定部位を指定する指定工程と、
    前記指定工程で指定した前記特定部位のサンプリングに関する情報を決定するサンプリング工程と、
    前記サンプリング工程で決定した前記特定部位の前記サンプリングに関する情報および前記対象物体の形状の計測データに基づいて前記対象物体の位置および姿勢を算出する算出工程と、を有することを特徴とする算出方法。
14. 請求項１３に記載の算出方法をコンピュータに実行させるためのプログラム。
15. 請求項１乃至８のうちいずれか１項に記載の情報処理装置を用いて対象物体の計測を行う工程と、該計測の結果に基づいて前記対象物体を処理する工程と、を含むことを特徴とする物品の製造方法。
    

Images