JP6719925B2 - 情報処理装置、情報処理方法、プログラム - Google Patents

Description

本発明は、被計測物体の位置姿勢を計測するための、形状モデルと画像特徴とを対応付けを行う方法に関する。

物体の位置姿勢を非接触で推定する技術の一つとして、被計測物体の形状モデルと被計測物体を観測した画像における物体像との差が小さくするように形状モデルの位置姿勢を推定する技術が知られている。この技術は、モデルフィッティングと呼ばれ、ロボットによる部品のビンピッキング等、不特定の位置姿勢に置かれた部品（被計測物体）をビジョンセンサにより位置姿勢を計測する場合等において利用される。

モデルフィッティングでは、形状モデルを構成する各幾何特徴毎に、観測された画像中に含まれる画像特徴との対応付けを行い、形状モデルの幾何特徴と画像特徴との差を計算する。そして、その差が小さくなるように被計測物体の位置及び姿勢を表す非線形パラメータを繰り返し推定する。ここで、モデルフィッティングにおける対応付け処理は、処理する幾何特徴のデータ量によっては探索処理量が膨大となるため、高速かつ効率的に対応付け処理を行う技術が要求されている。

特許文献１には、画像特徴としてエッジを利用した場合の形状モデルと画像特徴の対応付けを高速に行う方法が開示されている。この方法では、注目画素から距離が最短となるエッジまでの符号付き距離値を各画素に格納した符号付き距離場をあらかじめ生成し、形状モデルと画像特徴の対応探索時に、その符号付き距離場を参照することにより、高速に最近傍探索を行っている。

特開２０１０―２０５０９５号公報

しかしながら、特許文献１の方法では、モデルのエッジ点（幾何特徴）の位置に対して、画像上で単純に最近傍となるエッジ特徴（画像特徴）を対応付ける手法であり、モデルエッジ点の法線方向を考慮していない。そのため、対応付けの結果に誤りを多く含み、対応付け精度が低くなるという課題がある。

本発明は、以上の課題を鑑みてなされたものであり、モデル特徴と画像特徴とを高速かつ安定的に対応付け、精度よく物体の位置姿勢を推定することを目的とする。

上記課題を解決するために、本願発明の情報処理装置は、例えば、物体のモデル情報を保持するモデル保持手段と、前記物体を含む画像を取得する画像取得手段と、前記画像から前記物体を表す画像特徴を検出する特徴検出手段と、前記モデル情報を構成するモデル特徴の位置及び方向を、画像座標系における位置及び方向に変換する座標変換手段と、前記画像座標系における前記モデル特徴の方向に基づいて、予め設定された複数の方向のうち選択された方向に関して前記画像における注目画素から前記検出された画像特徴までの距離を示す参照マップを特定する選択手段と、前記選択手段により選択された参照マップと、前記座標変換手段により変換したモデル特徴の位置とに基づいて、前記モデル特徴に対応する画像特徴を対応付ける対応付け手段と、前記対応付け手段により対応付けられた結果に基づいて、前記物体の位置姿勢を導出する導出手段とを備える。

本発明により、モデル特徴と画像特徴とを高速かつ安定的に対応付けることができ、精度よく物体の位置姿勢を推定することができる。

本発明の第１の実施形態における情報処理装置の構成を示す図である。 本発明の第１の実施形態における３次元形状モデルの構成要素を説明する図である。 本発明の第１の実施形態における、方向別符号付き距離場を説明する図である。 本発明の第１の実施形態における情報処理装置の行う処理手順を示すフローチャートである。 本発明の第１の実施形態における、方向別符号付き距離場の生成を説明する図である。 本発明の第２の実施形態における情報処理装置の構成を示す図である。 本発明の第２の実施形態における情報処理装置の行う処理手順を示すフローチャートである。 本発明の第３の実施形態における情報処理装置の構成を示す図である。 本発明のハードウェア構成図を示す図である。

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

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

［実施形態１］
本実施形態では、本発明の情報処理装置及びその方法を、被計測物体の３次元形状モデルと画像中のエッジ特徴とに基づくモデルフィッティングにおける、３次元形状モデル中のモデルエッジ点と入力画像から検出したエッジ特徴とを対応付ける方法に適用した場合について説明する。

本実施形態では、あらかじめ設定した探索方向の数だけ、探索方向を限定した符号付き距離場（以後、方向別符号付き距離場と称する）を複数生成する。そして、方向別符号付き距離場の中から、モデルエッジ点の画像上における法線方向に近い方向別符号付き距離場を選択して参照することで、モデルエッジ点の法線方向に対して最近傍となるエッジ特徴を参照する。これにより、対応付け精度の高い最近傍探索処理を高速に行うことが可能となる。モデルフィッティングを行うことで、被測定物体の位置姿勢の推定を行う利用例が挙げられる。

次に、図１を参照して、本実施形態における情報処理装置１の機能構成を説明する。図１に示すように、情報処理装置１は、画像取得部１１０、形状モデル保持部１２０、特徴検出部１３０、方向別参照マップ生成部１４０、座標変換部１５０、方向別参照マップ選択部１６０、対応付け部１７０、残差算出部１８０、位置姿勢導出部１９０を備えている。

これらの各機能部は、上記説明した図９におけるＣＰＵ９１０が、ＲＯＭ９２０に格納されたプログラムをＲＡＭ９３０に展開し、後述する各フローチャートに従った処理を実行することで実現されている。また例えば、ＣＰＵ９１０を用いたソフトウェア処理の代替としてハードウェアを構成する場合には、ここで説明する各機能部の処理に対応させた演算部や回路を構成すればよい。

また、情報処理装置１は、外部の撮像装置１０に接続されている。撮像装置１０は、通常の画像を撮影するカメラである。撮影される画像は濃淡画像であってもよいしカラー画像であってもよい。また、各画素が奥行きの情報を持つ距離画像であってもよい。被計測物体の像が得られる限り、撮影される画像に制限はなく、いかなる画像を用いてもよい。本実施形態では撮像装置１０は各画素に輝度値が格納された濃淡画像を撮像するカメラであり、濃淡画像を出力するものとする。また、本実施形態における撮像装置１０の、焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照するか、または、以下の文献で開示される方法によって事前にキャリブレーションしておく。

Ｒ． Ｙ． Ｔｓａｉ、 “Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ ”、 ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ、 ｖｏｌ．ＲＡ−３、 ｎｏ．４、 １９８７．
次に、情報処理装置１を構成する各部について説明する。

画像取得部１１０は、撮像装置１０が撮影した画像を取得する。上述の通り、本実施形態では撮像装置１０は濃淡画像を撮像するカメラであり、画像取得部１１０は、インターフェースを介して濃淡画像を取得する。

形状モデル保持部１２０は、被計測物体の形状を表す３次元形状モデル２０を保持する。本実施形態では、３次元形状モデル（例えば、ＣＡＤモデルやポリゴンモデル）は、図２（ｂ）に示すような、モデル座標系における３次元位置と３次元方向から構成される局所的な３次元線分情報（以後、モデル特徴またはモデルエッジ点と称する）によって構成されるものとする。ただし、３次元形状モデルとして保持する形状情報は、被計測物体の形状または特徴を表す３次元座標情報を含む限り、表現形式に特に制限はない。例えば、単純な３次元点の集合や、稜線を表す３次元ラインの集合、３次元点３点で構成される面および線の集合で表されるポリゴン形式の形状情報など、他の表現で表しても良い。３点と３辺および１面により構成されるポリゴンの集合として、３次元形状モデルを表現してもよいし、単純な３次元点の集合として３次元形状を表現してもよい。また陰関数の組み合わせによりパラメトリックに３次元形状を表現する方法を用いてもよい。被計測物体の形状に即する限り、３次元形状モデルの表現方法に特に制限はない。形状モデル保持部１２０において保持される３次元形状モデルは、座標変換部１５０に入力される。

特徴検出部１３０では、画像取得部１１０から入力された二次元画像から画像特徴を検出する。本実施形態では画像特徴としてエッジ特徴を検出する。エッジ特徴とは、画像中の輝度が不連続に変化する点を表し、画像中の輝度勾配が極値となる位置をエッジ位置、勾配方向の法線をエッジ方向として定義する。ただし、本実施形態の画像特徴は、エッジ特徴に限られるものではなく、例えば、画像特徴としてＨａｒｒｉｓ特徴点やＦＡＳＴ特徴点などの、特徴点を検出してもよい。この場合、被計測物体の三次元形状モデルを三次元点群データによって表し、特徴点と三次元の点の対応に基づいて位置姿勢を算出する。また、特定の特徴のみ用いるのではなく、複数の特徴（特徴点とエッジ）を位置姿勢の算出に利用してもよい。以上に示すように、画像特徴としては、３次元形状モデルを構成するモデル点に対応する特徴である限り、いかなる特徴であっても特に問題ない。特徴検出処理の詳細については後述する。

方向別参照マップ生成部１４０では、特徴検出部１３０で検出したエッジ特徴に基づいて、モデルエッジ点に対応するエッジ特徴を高速に探索するための方向別参照マップ（参照距離情報）を生成する。方向別参照マップとは、画像を構成する各画素（各要素）、所定の探索方向において近傍となるエッジ特徴の位置を参照するための情報を格納したマップのことである。ここで、本実施形態における所定の探索方向とは、エッジ特徴を探索する画像上での方向を表す。本実施形態では、方向別参照マップとして方向別符号付き距離場を生成する。方向別符号付き距離場とは、着目する探索方向にそって、画素から最も近いエッジ特徴までの符号付き距離を画素値として保持した画像である。

図３に、方向別符号付き距離場の例を示す。図４（ａ）は横方向、図４（ｂ）は縦方向、図４（ｃ）は斜め右上方向、図４（ｄ）は斜め右下方向に関する方向別符号付き距離場を表している。モデル点の画像上における法線方向に応じて、モデル点毎に適切な探索方向の方向別符号付き距離場を参照することで、モデル点の法線方向において、もっとも近接するエッジ特徴の位置を高速に探索することが可能になる。本実施形態では、方向別符号付き距離場として、画像上で横方向・縦方向・斜め右上方向・斜め右下方向の計４方向分の方向別符号付き距離場を生成する。ただし、方向別符号付き距離場として生成する方向や数は、以上に限るものでなく、例えば、画像上で縦・横の２方向のみからなる方向別符号付き距離場を生成しても良いし、斜め方向の分割数を増やして計１０方向の方向別符号付き距離場を生成しても良い。複数の探索方向に関する方向別符号付き距離場を生成する限り、生成する方法数と方向に制限はない。また、方向別符号付き距離場を保持する画像は、方向毎に画像を生成しても良いし、方向毎の符号付き距離場の画素値をＲＧＢ情報などのマルチプレーンの情報として保持し、複数の方向の方向別符号付き距離場の情報を１枚の画像で保持しても良い。複数の方向に関する方向別符号付き距離場の情報を保持する限り、方向別符号付き距離場を保持するデータ形式に制限はなく、いかなるデータ形式でも良い。方向別符号付き距離場の生成処理の詳細については後述する。

座標変換部１５０では、計測物体の位置及び姿勢の概略値、および、撮像装置１０の内部パラメータに基づき、モデル座標系で表されるモデル点の位置と方向を、画像座標系における位置と方向に変換を行う。座標変換処理の具体的な処理に関しては後述する。
方向別参照マップ選択部１６０では、座標変換部１５０により求められた、画像座標系におけるモデル点の方向に基づいて、モデル点の法線方向に最も探索方向が近い方向別参照マップを選択する。方向別参照マップの選択処理の詳細に関しては後述する。

対応付け部１７０は、座標変換部１５０により求められた画像座標系におけるモデル点の位置に基づいて、マップ選択部１６０により選択された方向別参照マップから最近傍となるエッジ特徴を参照することで、モデル点とエッジ特徴との対応付けを行う。対応付け処理の詳細に関しては後述する。

残差算出部１８０は、情報処理装置１の対応付け部１７０から、モデルエッジ点とそれに対応付けられたエッジ特徴の組を取得し、モデルエッジ点とエッジ特徴との間の画像上での距離（以後、残差と称する）を算出する。処理の詳細は後述する。

位置姿勢導出部１９０では、残差算出部１８０において算出された残差情報に基づいて、残差が最小になるように、撮像装置１０と被測定物体との間の位置及び姿勢を導出（位置姿勢推定）する。処理の詳細については後述する。

以上が、情報処理装置１の構成の一例についての説明である。

次に、本実施形態に処理手順について図４を参照しながら説明する。図４は、本実施形態における特徴検出の処理手順を示すフローチャートである。

（ステップＳ１１００）
まず、ステップＳ１１００において、座標変換部１５０は、モデル座標系で表現されているモデル点の位置と方向を画像特徴と同じ画像座標系へ幾何変換するための座標変換パラメータを取得する。具体的には、撮像装置１０に対する被測定物体の位置及び姿勢の概略値を座標変換パラメータとして入力する。本実施形態では、物体の置かれているおおよその位置や姿勢があらかじめわかっているものとして、その値を概略値として用いる。ただし、位置姿勢の概略値の設定方法は、以上の方法に限るものではなく、例えば、時間軸方向に連続して物体の位置姿勢を計測する場合には、前回（前時刻）の位置姿勢の計測値を概略の位置及び姿勢として用いてもよい。また、様々な姿勢で撮像した被計測物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチングを行うことによって、被計測物体の大まかな位置と姿勢を推定してもよい。被計測物体の６自由度の位置及び姿勢を得られる限り、入力する座標変換パラメータに制限はなく、いかなる形式であってもよい。ステップＳ１１００の処理を終了したら、ステップＳ１２００に進む。

（ステップＳ１２００）
次に、ステップＳ１２００において、画像取得部１１０は、撮像装置１０から濃淡画像を取得する。撮像装置１０から直接取得しなくても、予め撮像した画像を記憶媒体に記憶させておき、その記憶媒体を読み込むことによって、濃淡画像を取得してもかまわない。濃淡画像の取得が終了したら、画像取得部は、特徴検出部に取得した濃淡画像を送出する。ステップＳ１２００の処理を終了し、ステップＳ１３００に進む。

（ステップＳ１３００）
次に、ステップＳ１３００では、特徴検出部１３０は、ステップＳ１２００において取得した濃淡画像から画像特徴の検出を行う。本実施形態では、上述の通り、画像特徴としてエッジを検出する。なお、エッジとは濃度勾配の極値となる点である。本実施形態ではエッジ検出方法として、以下の文献で開示されるＣａｎｎｙエッジアルゴリズムを用いる。

Ｃａｎｎｙ、 Ｊ、 “Ａ Ｃｏｍｐｕｔａｔｉｏｎａｌ Ａｐｐｒｏａｃｈ Ｔｏ Ｅｄｇｅ Ｄｅｔｅｃｔｉｏｎ”、 ＩＥＥＥ Ｔｒａｎｓ。 Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ、 ８（６）：６７９-６９８、 １９８６。

エッジ検出処理の結果、濃淡画像中でのエッジとなる画素の位置が得られる。なお、エッジ検出方法はこれに限るものではなく、例えば、Ｓｏｂｅｌフィルタやｐｒｅｗｉｔｔフィルタなどのエッジ検出フィルタを用いて、その応答値からエッジ位置を算出してもよい。また、上述したような画像全体からエッジを検出する方式に限らず、以下の文献で開示されているような、着目領域近傍で局所的にエッジ検出を行うような方法を用いても良い。

Ｔ。 Ｄｒｕｍｍｏｎｄ ａｎｄ Ｒ。 Ｃｉｐｏｌｌａ、 “Ｒｅａｌ−ｔｉｍｅ ｖｉｓｕａｌ ｔｒａｃｋｉｎｇ ｏｆ ｃｏｍｐｌｅｘ ｓｔｒｕｃｔｕｒｅｓ
”、 ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ、 ｖｏｌ。２４、 ｎｏ。７、 ｐｐ。９３２−９４６、 ２００２。

濃淡画像上で検出され、モデルエッジ点と対応づけられるエッジ特徴であれば、いかなる方法で検出しても問題はなく、その検出方法の選択に特に制限はない。濃淡画像から全ての画像エッジを検出する処理を終了すればステップＳ１３００の処理を終了し、ステップＳ１４００に進む。

（ステップＳ１４００）
次に、ステップＳ１４００では、方向別参照マップ生成部１４０は、ステップＳ１３００において算出した画像エッジに基づいて、複数の方向別符号付き距離マップを生成する。本実施形態では、上述の通り、画像上で横方向・縦方向・斜め右上方向・斜め右下方向の計４方向分の方向別符号付き距離場を生成する。

まず、方向別符号付き距離場を格納する画像として、ステップＳ１２００において入力された画像と同サイズの画像を４つ生成する。そして、全ての画像に対し、ステップＳ１３００において検出した全てのエッジ特徴の位置の画素に０を代入し、それ以外の画素には未処理であるというフラグを立てる。そして、方向別符号付き距離場毎に、膨張する方向を限定した符号付き距離場の膨張処理を行う。具体的には、未処理である全ての画素に対して、限定する方向にそって隣接する画素をチェックし、隣接画素に符号付き距離値が格納されている場合、隣接画素の符号付き距離値に１だけ増減させた符号付き距離値を着目画素に格納する。例えば、横方向の方向別符号付き距離場の場合は、着目画素の左右の隣接画素をチェックし、左右の隣接画素に格納された符号付き距離値を参照して、着目画素の符号付き距離値を格納する。以上の処理を、１回分の膨張処理として、画像上での探索したい画素範囲の分だけ膨張処理を繰り返す事で、方向別符号付き距離場の生成を行う。膨張処理を行う回数は、手動で設定した固定値を用いても良いし、入力画像サイズの割合に応じて設定しても良い。また、被計測物体の概略位置姿勢のあいまい性が既知の場合は、概略位置姿勢を画像上に射影した範囲の値に応じて設定しても良い。または、各方向の方向付き距離場において未処理の画素が無くなるまで膨張処理を行っても良い。本実施形態では、膨張処理を行う回数は、画像サイズ長辺の４分の１の画素数より設定するものとする。

図５に、横方向の方向別符号付き距離場の膨張処理の例を示す。まず、初期状態（図５（ａ））では、エッジ位置の画素にのみ符号付き距離値が格納されている。そして、１回の膨張処理の結果（図５（ｂ））、エッジ位置の画素に横方向で隣接する画素に方向に応じた＋１または−１の符号付き距離値が格納される。そして、２回（図５（ｃ））３回（図５（ｄ））と膨張処理を繰り返す分だけ、横方向に拡張された符号付き距離場が生成される。なお、隣接画素の符号付き距離値が異なり、代入する符号付き距離値に複数の候補がある場合は、絶対値が小さい符号付き距離値を代入する。ただし、この場合の代入方法は以上に限るものではなく、例えば、絶対値が大きい符号付き距離値を選択しても良いし、単純に先に代入した方を選択しても良く、選択方法に特に制限はない。

以上、全ての方向に関する方向別符号付き距離場に対して、設定した回数だけ方向別符号付き距離場の膨張処理を繰り返し、方向別符号付き距離場の生成が完了すれば、ステップＳ１４００の処理を終了し、ステップＳ１５００に進む。

（ステップＳ１５００）
次に、ステップＳ１５００では、座標変換部１５０は、ステップＳ１１００において入力された位置姿勢パラメータおよび撮像装置１０の内部パラメータを用いて、モデルエッジ点の座標変換処理を行う。

具体的には、まず、形状モデル保持部１２０から三次元形状モデル２０を取得する。そして、３次元形状モデル２０を構成する全てのモデルエッジ点に対して、モデル座標系において表現されるモデルエッジ点の位置と方向と、ステップＳ１１００において入力された位置姿勢パラメータおよび撮像装置１０の内部パラメータとを用いて、画像座標系におけるモデルエッジ点の位置と方向を算出する。

全てのモデルエッジ点に対する座標変換処理が完了すれば、ステップＳ１５００の処理を終了し、ステップＳ１６００に進む。

（ステップＳ１６００）
次に、ステップＳ１６００では、方向別参照マップ選択部１６０は、ステップＳ１５００において算出された画像座標系におけるモデルエッジ点の方向に基づいて、ステップＳ１４００において生成された複数の方向別符号付き距離場の中から、ステップＳ１６００の対応付け処理において参照する方向別符号付き距離場を選択する。

まず、画像座標系におけるモデルエッジ点の方向から、モデルエッジ点の法線方向を算出する。ここで、モデルエッジ点の法線方向を（ｎ_ｘ、ｎ_ｙ）、方向別符号付き距離場の方向を（ｓ_ｘ、ｓ_ｙ）とすると、モデルエッジ点の法線方向と方向別符号付き距離場の方向との方向の差θは、以下の式より計算される。

ステップＳ１４００において生成された複数の方向別符号付き距離場の探索方向に対して以上の計算を行い、方向の差θがもっとも小さい方向の方向別符号付き距離場を、対応探索に用いる方向別符号付き距離場として選択する。

なお、方向別符号付き距離場の選択方法は以上に限るものではなく、例えば、複数の方向別符号付き距離場に関して方向の差θが所定の範囲内となる場合に、複数の方向別符号付き距離場を対応探索に用いる方向別符号付き距離場として選択しても良いし、方向の差θが一定以上に大きい場合は、モデルエッジの法線方向に近い方向別符号付き距離場がないものとして取り扱っても良く、方向別符号付き距離場を選択する方法や数に特に制約はない。

以上の処理を、全てのモデルエッジ点に対して行い、モデルエッジ点毎に参照する方向別符号付き距離場の選択が完了すれば、ステップＳ１６００の処理を終了し、ステップＳ１７００に進む。

（ステップＳ１７００）
次に、ステップＳ１７００では、対応付け部１７０は、ステップＳ１５００において算出した画像座標系におけるモデルエッジ点の位置と、ステップＳ１６００において選択した方向別符号付き距離場とに基づいて、モデルエッジ点に対応するエッジ特徴を探索して、モデルエッジ点とエッジ特徴との対応付けを行う。

まず、ステップＳ１６００において選択した方向別符号付き距離場において、ステップＳ１５００において算出した画像座標系におけるモデルエッジ点の位置にあたる画素を参照することで、モデルエッジ点の法線方向に最も近接するエッジ特徴までの距離を表す、符号付き距離値を取得する。ここで、画像座標系におけるモデルエッジ点の位置を（ｍ_ｘ、ｍ_ｙ）、取得した符号付き距離値をｄ、参照する方向別符号付き距離場の方向を（ｓ_ｘ、ｓ_ｙ）とすると、モデルエッジ点に対して探索方向において最近傍となるエッジ特徴の位置（ｅ_ｘ、ｅ_ｙ）は以下の式より算出できる。

以上により算出したエッジ特徴の位置（ｅ_ｘ、ｅ_ｙ）を、モデルエッジ点に対応するエッジ特徴の位置として決定する。なお、ステップＳ１６００において、モデルエッジ点毎に複数の方向別符号付き距離場を選択している場合は、其々の方向別符号付き距離場によるエッジ特徴の位置を算出し、対応付け結果として複数のエッジ特徴の位置を保存する。また、ステップＳ１６００において、方向別符号付き距離場を選択しなかったモデルエッジ点に関しては、本ステップにおける処理をスキップする。

以上の処理を、全てのモデルエッジ点に対して行い、全てのモデルエッジ点に関するエッジ特徴の対応付け処理が完了したら、この処理を終了し、モデルエッジ点群とエッジ特徴群との対応付けの結果が決定される。

（ステップＳ１８００）
次に、ステップＳ１８００において、残差算出部１８０は、ステップＳ２１００において対応が算出されたモデルエッジ点とエッジ特徴との組に基づいて、モデルエッジ点とエッジ特徴との画像上での残差を算出する。一つのモデルエッジ点に対して複数のエッジ特徴が対応づけられている場合は、それぞれにおける残差を算出し、より残差が小さい方の残差をモデルエッジ点の残差として算出する。全てのモデルエッジ点に関する残差の算出が終了すれば、ステップＳ１８００の処理を終了し、ステップＳ１９００に進む。

（ステップＳ１９００）
次に、ステップＳ１９００では、位置姿勢導出部１９０は、ステップＳ１８００において算出したモデルエッジ点とエッジ特徴間の残差に基づいて、三次元形状モデル２０と被測定物体間の位置及び姿勢を算出する。本ステップでは、モデルエッジ点とエッジ特徴間の残差が最小になるように、線形連立方程式を解くことで、位置姿勢の更新を行う。本処理に関しては本発明の本質に関わる話ではないため、記述は省略する。詳細は以下の文献に記述されている。

Ｔ Ｄｒｕｍｍｏｎｄ、 Ｒ Ｃｉｐｏｌｌａ、 “Ｒｅａｌ−ｔｉｍｅ ｖｉｓｕａｌ ｔｒａｃｋｉｎｇ ｏｆ ｃｏｍｐｌｅｘ ｓｔｒｕｃｔｕｒｅｓ”、Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ、 ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ２４ （７）、 ９３２−９４６。

位置及び姿勢の更新処理が終了すればばステップＳ２３００の処理を終了し、ステップＳ２０００に進む。

（ステップＳ２０００）
次に、ステップＳ２０００では、位置姿勢導出部１９０は、ステップＳ１９００で更新した位置姿勢が、収束しているか否か、すなわち、さらに反復計算を必要とするか否かの判定を行う。具体的には、補正値がほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。収束していなければ、ステップＳ１８００に戻り、更新した位置姿勢を用いて、再度残差算出処理を行う。収束していると判断した場合は、この処理は終了する。

以上の処理により、撮像装置と被測定物体間の相対的な位置姿勢の最終的な推定値が決定される。

本発明の情報処理装置１により、撮像装置１０と被測定物体間の位置及び姿勢の推定を高速に行うことが可能となる。

以上に述べたように、本実施形態では、あらかじめ複数の探索方向に関して、方向別符号付き距離場を生成しておき、モデルエッジ点の画像上における法線方向に応じて、参照する方向別符号付き距離場を変えることで、モデルエッジ点の法線方向に対して最近傍となるエッジ特徴を高速に参照することができる。これにより、モデルエッジ点とエッジ特徴との対応付けを精度高くかつ高速に処理することが可能となる。そして、その結果を用いて位置姿勢推定を行うことにより、被測定物体の位置姿勢を安定的かつ高速に推定することが可能となる。

［変形例１］
上述の実施形態では、モデルエッジ点の法線方向に最近傍探索するために、モデルエッジ点の法線方向に基づいて方向別参照マップを選択する処理について述べた。しかし、方向別参照マップを選択する基準は、以上に述べた方法に限るものではない。

例えば、被計測物体または撮像装置が移動する状況において、画像上の被計測物体を時系列的にトラッキングするシチュエーションなど、次のフレームにおける被計測物体の画像上の位置がおおよそ予測できる場面では、物体の予測位置に向かって最近傍探索するように方向別参照マップを選択しても良い。

具体的には、上述の実施形態におけるステップＳ１６００の方向別参照マップ選択処理において、モデルエッジ点の法線方向の代わりに、着目するモデルエッジ点の現時点の位置から予測位置へ向かう方向を、方向別参照マップを選択する基準として用いる。また、モデルエッジ点の画像上での速度ベクトルが既知である場合には、速度ベクトルに基づく方向を用いて、方向別参照マップを選択しても良い。モデル点の情報に基づいて選択する限り、方向別参照マップを選択する基準に制限はなく、いかなる基準で方向別参照マップを選択しても良い。また、この場合、入力画像から検出する画像特徴は、エッジ特徴でなくても良く、例えば、特徴点を検出して、特徴点位置を示す方向別参照マップを生成・選択しても良い。また、画像特徴として特徴点を検出する場合には、３次元形状モデルとしては、モデルエッジ点のような位置と方向を備える形式でなくても良く、例えば、３次元位置のみを保持するモデル点の集合を、３次元形状モデルとして利用しても良い。

以上、被計測物体の予測情報を用いて、方向別参照マップを選択することで、予測情報に基づくより確度の高い最近傍探索処理を行うことが可能になる。

［変形例２］
上述の実施形態では、符号付き距離値を画素として格納した方向別符号付き距離場を方向別参照マップとして利用する方法について述べた。しかし、方向別参照マップとして利用するデータは、上述の方向別符号付き距離場に限るものではない。例えば、ステップＳ１３００において検出したエッジ特徴からエッジ位置とインデックス番号（インデックス値）を紐づけたエッジ特徴データベースを生成しておき、ステップＳ１４００における、方向別参照マップ生成において、各方向別参照マップの各画素に、所定の探索方向において最近傍となるエッジ特徴のインデックス番号を格納しても良い。この場合、ステップＳ１６００における対応付け処理は、画像座標系におけるモデルエッジ位置に基づいてインデックス番号を参照し、エッジ特徴データベースを介して、最近傍となるエッジ特徴の位置を参照する。以上のように、方向別参照マップとして保持する情報は、所定の探索方向において近傍となるエッジ位置が高速に参照可能である限り、いかなる情報を保持しても良く、方向別参照マップとして保持するデータ形式に特に制限はない。

以上、方向別参照マップとして、エッジインデックスを保持することにより、モデルエッジ点の法線方向に向かって最近傍となるエッジ特徴を高速に参照することが可能になる。

［第２の実施形態］
第１の実施形態では、本発明において物体の位置・姿勢の推定に適用する方法について説明した。第２の実施形態では、本発明を物体の照合に適用する方法について説明する。

図６は、第２の実施形態にかかる情報処理装置２の機能ブロック図を示す。以下、第１の実施形態と同じ機能のものには同じ番号が付してあり、その説明を省略する。

本実施形態では、照合する対象の数だけ三次元形状モデルを保持しており、それぞれのモデルごとに、方向別エッジマップ選択部１６０、対応付け部１７０、残差算出部１８０の処理を行い、照合部６９０で残差の少ないモデルを照合されたモデルとして求める。

形状モデル保持部６２０は、照合する対象である物体の形状を表す３次元形状モデルを保持する。３次元形状モデルの保持方法は、第１の実施形態における形状モデル保持部１２０と同一である。第１の実施形態と異なる点は、本実施形態では、照合する対象の数だけ、３次元形状モデルを複数保持する。

モデル照合部６９０は、画像内の特徴と３次元形状モデルとを照合する。この際、モデル照合部６９０は、処理の詳細については、後述する。

次に、第２の実施形態の処理フローチャートについて説明する。ステップ７９００以外は第１の実施形態と同じであるので、省略する。ただし、本実施形態では、ステップＳ１６００〜１８００の処理については、形状モデル保存部６２０が保存するモデルの数だけ行われる。

（ステップＳ７９００）
続いて、ステップＳ７９００において、モデル照合部６９０は、３次元モデル（３次元形状モデル）ごとにステップＳ１８００で求められた残差に基づいて、計測データにもっとも類似する３次元モデル（３次元形状モデル）を判断する。本ステップでは、個々の３次元形状モデル中のモデルエッジの残差から、統計量として中央値を算出し、これを３次元形状モデルの照合度として保持する。全ての３次元形状モデルに対して、この残差の統計量の算出を行い、残差の統計量が最も小さくなる３次元形状モデルを判定することで、３次元形状モデルを照合する。なお、残差の統計量としては、残差の中央値以外でも、例えば平均値や最頻値を用いてもよい。残差の少なさを判定できる指標であれば、いずれの方法でよく、特に制限はない。

本実施形態によれば、高速かつ安定的にモデルの照合を行うことが可能になる。

［第３の実施形態］
本発明による情報処理装置１、２の好適な適用事例としては、以下のような形態も考えられる。すなわち、撮像装置１０により得られる濃淡画像を基に被測定物体３０の位置姿勢を推定し、推定された位置姿勢に基づいて産業用ロボットアーム５０を制御する。そして、ロボットアーム先端のロボットハンドにより被測定物体の把持（物体保持）などを行う利用例があげられる。以下、図７を用いて本発明の一実施形態である情報処理装置１の適用例を説明する。図７では、情報処理装置１とロボット５０を用いて被測定物体３０を把持するロボットシステムの構成例を示す。

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

撮像装置１０は、通常の濃淡画像を撮影するカメラであり、産業用ロボットアームの手先等の被測定物体３０を撮像できる位置に設置する。

情報処理装置１は、撮像装置１０から得られる濃淡画像を基に被測定物体３０の位置姿勢を推定する。情報処理装置１で推定された被測定物体３０の位置姿勢は、ロボット５０に入力され、被測定物体３０の把持などを行うようにロボットアームを制御する。本実施形態の情報処理装置により、ロボットシステムは被測定物体３０の位置が不定でも位置姿勢推定を行うことで、被測定物体３０を把持（保持）することが可能となる。

＜実施形態の効果＞
第１の実施形態では、あらかじめ複数の探索方向に関して、方向別符号付き距離場を生成しておき、モデルエッジ点の画像上における法線方向に応じて、参照する方向別符号付き距離場を変えることで、モデルエッジ点の法線方向に対して最近傍となるエッジ特徴を高速に参照することができる。そして、濃淡画像から検出した特徴に、被測定物体の形状を表す三次元形状モデルが当てはまるようにモデルフィッティングを行うことで、被測定物体の位置姿勢の推定を行う方法について述べた。方向別に最近傍となるエッジ特徴を高速に参照できる方向別参照マップを用いて、モデルエッジ点とエッジ特徴との対応付け処理を高速に行い、その結果を用いて位置姿勢推定を行うことにより、被測定物体の位置姿勢を高速に推定することが可能となる。

第２の実施形態では、方向別に最近傍となるエッジ特徴を高速に参照できる方向別参照マップを用いて、モデルエッジ点とエッジ特徴との対応付け処理を高速に行い、その結果を用いてモデル照合を行うことにより、高速かつ安定的にモデルの照合を行うことが可能になる。

第３の実施形態では、本発明の情報処理装置により、被測定物体の位置姿勢を推定し、推定結果に基づいてロボットシステムが被測定物体の把持および移動を行う方法を示した。これにより、被測定物体の位置が不定でもロボットシステムが被測定物体を把持することが可能となる。

＜定義＞
本願明細書の説明を通して、被計測物体の３次元モデル情報として、３次元位置と３次元方向の情報を備えるモデルエッジ点により構成されたモデルを例に説明した。しかし、３次元形状モデルとしては他の表現方式を用いても良い。例えば、単純な３次元点の集合や、稜線を表す３次元ラインの集合、３次元点３点で構成される面および線の集合で表されるポリゴン形式の形状情報など、他の表現で表しても良い。３点と３辺および１面により構成されるポリゴンの集合として、３次元形状モデルを表現してもよいし、単純な３次元点の集合として３次元形状を表現してもよい。また陰関数の組み合わせによりパラメトリックに３次元形状を表現する方法を用いてもよい。被計測物体の形状または特徴に即する限り、３次元形状モデルの表現方法に特に制限はない。

本願明細書における画像取得は、濃淡画像を取得するカメラによる入力に限るものでなく、対象像が得られる限り他の方式でもよい。例えば、濃淡画像であってもよいしカラー画像であってもよい。また、また、カメラにより撮影される画像に限るものではなく、記憶媒体等に保存されたファイルまたは外部ネットワークから読み込んだ画像を入力しても特に問題はない。また、各画素が奥行きの情報を持つ距離画像であってもよい。

本願明細書の説明を通して、座標変換パラメータは、被計測物体の置かれているおおよその位置や姿勢があらかじめわかっているものとして、その値を座標変換パラメータとして取得することを例に説明した。しかし、本願明細書における座標変換パラメータの取得は、これに限るものではない。例えば、時間軸方向に連続して物体の位置姿勢を計測する場合には、前回（前時刻）の位置姿勢の計測値を概略の位置及び姿勢として用いてもよいし、様々な姿勢で撮像した被計測物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチングを行うことによって、被計測物体の大まかな位置と姿勢を推定してもよい。被計測物体の６自由度の位置及び姿勢を得られる限り、入力する座標変換パラメータに制限はなく、いかなる形式であってもよい。

本願明細書における画像から検出する画像特徴としては、３次元形状モデルとの対応付けを行うことが可能である特徴であれば、いかなる特徴であっても特に問題ない。例えば、エッジ特徴の他に、Ｈａｒｒｉｓ特徴点やＦＡＳＴ特徴点などの、特徴点を検出しても特に問題はない。

本願明細書における方向別参照マップとしては、方向別参照マップを構成する各画素に、所定の方向において近傍となるエッジ特徴の位置を参照できる参照情報が格納されている限り、方向別参照マップとして生成する情報に特に制限はない。例えば、画素値毎に、着目する方向にそって最も近いエッジ画素までの符号付き距離を保持した方向別符号付き距離場を方向別参照マップとして生成しても良いし、画像から検出したエッジ特徴にインデックス番号を付与しておき、着目する方向にそって最も近いエッジ画素を示すインデックス番号を画素値として保持した方向別インデックスマップを、方向別参照マップとして生成してもよい。また、方向別参照マップを生成する方向の種類および数としては、画像上で横方向・縦方向・斜め右上方向・斜め右下方向の計４方向に限るものではなく、例えば、画像上で縦・横の２方向のみからなる方向別符号付き距離場を生成しても良いし、斜め方向の分割数を増やして計１０方向の方向別符号付き距離場を生成しても良い。複数の方向に関する方向別符号付き距離場を生成する限り、生成する方法数と方向に制限はない。また、方向別符号付き距離場を保持する画像は、方向毎に画像を生成しても良いし、方向毎の符号付き距離場の画素値をＲＧＢ情報などのマルチプレーンの情報として保持し、複数の方向の方向別符号付き距離場の情報を１枚の画像で保持しても良い。複数の方向に関する方向別符号付き距離場の情報を保持する限り、方向別符号付き距離場を保持するデータ形式に制限はなく、いかなるデータ形式でも良い。

本願明細書における方向別参照マップを選択する方法としては、画像座標系におけるモデルエッジ点の法線方向に基づく方法に限るものはない。例えば、例えば、被計測物体を時系列的にトラッキングするシチュエーションなど、次のフレームにおける被計測物体の画像上の位置がおおよそ予測できる場面では、物体の予測位置に向かって最近傍探索するように方向別参照マップを選択しても良い。モデル点の情報に基づいて選択する限り、方向別参照マップを選択する基準に制限はなく、いかなる基準で方向別参照マップを選択しても良い。

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

Claims (12)

1. 物体のモデル情報を保持するモデル保持手段と、
   前記物体を含む画像を取得する画像取得手段と、
   前記画像から前記物体を表す画像特徴を検出する特徴検出手段と、
   前記モデル情報を構成するモデル特徴の位置及び方向を、画像座標系における位置及び方向に変換する座標変換手段と、
   前記画像座標系における前記モデル特徴の方向に基づいて、予め設定された複数の方向のうち選択された方向に関して前記画像における注目画素から前記検出された画像特徴までの距離を示す参照マップを特定する選択手段と、
   前記選択手段により特定された参照マップと、前記座標変換手段により変換したモデル特徴の位置とに基づいて、前記モデル特徴に対応する画像特徴を対応付ける対応付け手段と、
   前記対応付け手段により対応付けられた結果に基づいて、前記物体の位置姿勢を導出する導出手段とを備えることを特徴とする情報処理装置。
2. 更に、前記特徴検出手段で検出された画像特徴に基づいて、前記参照マップを生成する生成手段を備えることを特徴とする請求項１に記載の情報処理装置。
3. 前記参照マップは、前記画像の画素に対応した要素をもつマップであり、該参照マップの要素は、該要素に対応する注目画素から前記複数の方向のうち所定の方向において最も近い前記画像特徴までの距離を格納することを特徴とする請求項１または２に記載の情報処理装置。
4. 前記参照マップは、該参照マップを構成する各画素に、所定の方向を基準に近接する前記画像特徴のインデックス値を格納していることを特徴とする請求項１乃至３のいずれか１項に記載の情報処理装置。
5. 前記複数の方向は、前記画像の横方向および縦方向を含むことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記距離は符号付き距離であることを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
7. 更に前記物体の概略位置姿勢を取得する手段と、
   前記概略位置姿勢に基づいて、前記画像におけるモデル特徴の方向を決定する手段とを備え、
   前記選択手段は、前記決定された前記画像におけるモデル特徴の方向に基づいて選択された方向に関する前記参照マップを特定することを特徴とする請求項１乃至６のいずれか１項に記載の情報処理装置。
8. 前記選択手段は、予め設定された複数の方向のうち、前記画像におけるモデル特徴の方向の法線の方向に最も近い方向における参照マップを特定することを特徴とする請求項７に記載の情報処理装置。
9. 前記選択手段は、前記画像の横方向の第１の参照マップと、前記画像の縦方向の第２の参照マップと、前記画像の斜め右上方向の第３の参照マップと、前記画像の斜め右下方向の第４の参照マップのうち、いずれかの参照マップを特定することを特徴とする請求項１乃至８のいずれか１項に記載の情報処理装置。
10. 前記特徴検出手段は、前記画像からエッジ特徴を検出することを特徴とする請求項１乃至９のいずれか１項に記載の情報処理装置。
11. 物体を含む画像を取得する画像取得工程と、
    前記画像から前記物体を表す画像特徴を検出する特徴検出工程と、
    モデル情報を構成するモデル特徴の位置及び方向を、画像座標系における位置及び方向に変換する座標変換工程と、
    前記画像座標系における前記モデル特徴の方向に基づいて、予め設定された複数の方向のうち選択された方向に関して前記画像における注目画素から前記検出された画像特徴までの距離を示す参照マップを特定する選択工程と、
    前記選択工程において特定された参照マップと、前記座標変換工程により変換したモデル特徴の位置とに基づいて、前記モデル特徴に対応する画像特徴を対応付ける対応付け工程と、
    前記対応付け工程において応付けられた結果に基づいて、前記物体の位置姿勢を導出する導出工程を備えることを特徴とする情報処理方法。
12. コンピュータを、請求項１乃至１０のいずれか１項に記載の情報処理装置の各手段として機能させるためのプログラム。