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

Abstract

【課題】 ノイズがある状況においても、ロバストな位置姿勢計測を行うこと。
【解決手段】 撮像装置により撮像された、計測対象物体を含む画像を取得し、前記計測対象物体の概略位置姿勢情報を取得し、前記撮像装置の位置に基づいて、前記画像における前記計測対象物体の影の方向を推定し、前記概略位置姿勢に基づき前記複数の幾何特徴を２次元平面に投影し、前記影の方向に基づいて前記幾何特徴ごとに前記計測対象物体の影に対応する可能性があるかを判定し、該可能性があると判定された幾何特徴に対して第１のフラグを設定し、前記画像から複数の画像特徴を検出し、前記概略位置姿勢に基づいて前記複数の幾何特徴を前記画像に投影し、前記設定手段で設定された第１のフラグに基づいて該投影された複数の幾何特徴と前記複数の画像特徴とを対応づける対応づけ手段と、前記対応付け手段で対応付けされた結果に基づいて、前記計測対象物体の位置姿勢を導出する。
【選択図】 図２

Description

本発明は、３次元形状が既知である物体の位置及び姿勢を計測する技術に関する。

生産現場では、生産効率向上のため、ロボットによる自動化が進められている。ロボットによる組み立て作業では、対象物体を認識し、正確な位置姿勢を取得する必要がある。これを実現する方法として、対象物体を撮影した濃淡画像や距離画像を用いた方法が開発されている。濃淡画像や距離画像には、様々な要因で発生するノイズが混在しているため、ノイズに対してロバスト性の高い方法が望まれる。

例えば、対象物体を撮影した濃淡画像や距離画像から得られる計測データを形状モデルに当て嵌めるモデルフィッティングの手法により、カメラの位置姿勢計測を行う方法が知られている。具体的には、対象物体の概略の位置姿勢である概略位置姿勢を与え、濃淡画像から抽出したエッジや距離画像から得られる距離値が形状モデルとより正しくフィッティングするように位置姿勢を最適化していく方法である。また、カメラの位置姿勢計測をロバスト化する手段としては、カメラの位置姿勢が取り得る範囲に所定のサンプリング間隔で複数の概略位置姿勢を生成して、それぞれの概略位置姿勢においてフィッティングの繰り返し計算を行い最も成績の良いフィッティング結果をフィッティングの最終結果とする方法が知られている。

対象物体を撮影した濃淡画像や距離画像に存在する影へのロバスト化の方法としては、取得された画像中で対象物体の影領域に関する情報を推定した上で、影領域内の偽の輪郭に誤って対応づかないような処理を行う方法が提案されている。特許文献１では、距離画像中の距離値が欠損を含む領域を影領域と判定した上で、フィッティングの最適化計算時に対応付ける点が影領域内に存在するか否かで異なる重みづけ係数を用いている。

特開２０１２−４２３９６号公報

特許文献１で開示されている方法は、影が発生している箇所が距離画像中に欠損部として明確に反映される場合を前提としており、二次的な反射光等の影響によって距離画像中で距離値をもってしまっているような影については対応できない。

本発明は、上記課題に鑑みてなされたものであり、物体を含む画像に影が含まれていても、前記物体の位置姿勢を精度よく計測することを目的とする。

上記課題を解決するために、本初発明の情報処理装置は、例えば、撮像装置により撮像された、計測対象物体を含む画像を取得する取得手段と、前記計測対象物体の概略位置姿勢情報を取得する概略位置姿勢取得手段と、前記計測対象物体の複数の幾何特徴を表す３次元モデル情報を保持するモデル情報保持手段と、前記撮像装置の位置に基づいて、前記画像における前記計測対象物体の影の方向を推定する推定手段と、前記概略位置姿勢に基づき前記複数の幾何特徴を２次元平面に投影し、前記影の方向に基づいて前記幾何特徴ごとに前記計測対象物体の影に対応する可能性があるかを判定し、該可能性があると判定された幾何特徴に対して第１のフラグを設定する第１の設定手段と、前記画像から複数の画像特徴を検出する検出手段と、前記概略位置姿勢に基づいて前記複数の幾何特徴を前記画像に投影し、前記設定手段で設定された第１のフラグに基づいて該投影された複数の幾何特徴と前記複数の画像特徴とを対応づける対応づけ手段と、前記対応付け手段で対応付けされた結果に基づいて、前記計測対象物体の位置姿勢を導出する導出手段とを有することを特徴とする。

本発明によれば、物体を含む画像に影が含まれていても、前記物体の位置姿勢を精度よく計測することが可能になる。

第１〜第３の実施形態におけるシステムの構成を示す図。 第１〜第３の実施形態におけるプログラムのモジュール構成を示す図。 第１、第２の実施形態における位置姿勢算出手順を示すフローチャート。 第１の実施形態における誤対応箇所検出の詳細な手順を示すフローチャート。 第１、第２の実施形態における位置姿勢算出処理の詳細な手順を示すフローチャート。 第１〜第３の実施形態における対応点の探索の原理を示す図。 第１〜第３の実施形態におけるエッジの投影像と検出されたエッジの関係を説明する図。 第２の実施形態における誤対応箇所検出の詳細な手順を示すフローチャート。 （ａ）第２の実施形態における計測対象物体１０１を撮影した濃淡画像と計測対象物体の関係を説明する概念図、（ｂ）第２の実施形態における濃淡画像中のエッジ点近傍の各画素における輝度値を示す図。 第３の実施形態における位置姿勢算出手順を示すフローチャート。 第３の実施形態における位置姿勢算出処理の詳細な手順を示すフローチャート。 第２の実施形態における影方向算出処理の詳細な手順を示すフローチャート。 第１の実施形態における３次元モデルの概略位置姿勢と撮像装置１０２の画像面との仮想空間上での関係を説明する図。 変形例２−１における位置姿勢算出処理の詳細な手順を示すフローチャート。 本発明にかかる情報処理装置のハードウェア構成の例を示す図である。

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

以下、本発明にかかる実施形態の情報処理装置を詳細に説明する。

本発明にかかる実施形態を説明するのに先立ち、各実施形態に示す情報処理装置が実装されるハードウェア構成について、図１５を用いて説明する。

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

［第１の実施形態］３次元モデルについて誤対応しやすい点を抽出する
図１は、第１の実施形態における情報処理装置１０４を用いたシステム構成の模式図である。

計測対象物体１０１は、位置及び姿勢（位置姿勢）を計測する対象となる物体である。本実施形態では、説明を簡単にするために、位置姿勢計測対象としての計測対象物体１０１は、図１に示すごとく、１つのみがある位置に載置されているものとする。しかし、以下に説明する位置姿勢計測処理は、計測対象物体１０１の形状や個数、載置形態に大きく依存するものではない。例えば、複数の異形状の計測対象物体が混合されて山積みされているような状態において、ある計測対象物体の位置姿勢を計測する場合にも、本実施形態は適用可能である。

図２は、第１の実施形態における情報処理装置１０４を用いたシステム構成の構成図である。

情報処理装置１０４は、撮像装置１０２および投影装置１０３と接続されており、投影装置１０３及び撮像装置１０２の動作制御を行うと共に、撮像装置１０３から得られた撮像画像を用いて、計測対象物体１０１の位置姿勢を求める。

撮像装置１０２は、現実空間の静止画若しくは動画像を撮像するものであり、本実施形態では、投影装置１０３によって光が照射された計測対象物体１０１を撮像するために使用する。そして撮像装置１０３は、撮像した画像（撮像画像）を、情報処理装置１０４に対して送出する。撮像装置１０２の内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は、例えば、後述のＺｈａｎｇの方法により事前に校正しておく。

投影装置１０３は、計測対象物体１０１に所定の光を照射するために使用する。投影装置１０３は、液晶プロジェクタである。投影装置の内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は、例えば、下記の文献に開示される方法により事前に校正しておく。なお、スリット光の投影ができれば投影装置１０３以外の装置を用いても良く、例えば、ＤＭＤ（デジタルミラーデバイス）やＬＣＯＳを用いたプロジェクタであってもよい。
Ｚ． Ｚｈａｎｇ，“Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．１１，ｐｐ．１３３０−１３３４，２０００
次に、情報処理装置１０４の機能構成例について説明する。

計測情報取得部２０１は、撮像装置１０２が取得した濃淡画像と距離画像を計測情報として情報処理装置１０４に取り込む（画像取得）。

装置情報取得部２０２は、投影装置１０３と撮像装置１０２との位置関係情報を事前情報として取得する。投影装置１０３と撮像装置１０２との位置関係情報は、投影装置１０２からパターン光を照射してカメラで撮影し、例えば以下の文献に記載された方法で求める。
Ｍ、Ｋｉｍｕｒａ、“Ｐｒｏｊｅｃｔｏｒ Ｃａｌｉｂｒａｔｉｏｎ ｕｓｉｎｇ Ａｒｂｉｔｒａｒｙ Ｐｌａｎｅｓ ａｎｄ Ｃａｌｉｂｒａｔｅｄ Ｃａｍｅｒａ”Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｐａｔｔｅｒｎ Ｒｅｃｏｇｎｉｔｉｏｎ、ＣＶＰＲ、２００７．
概略位置姿勢取得部２０３は、撮像装置１０２に対する計測対象物体１０１の位置及び姿勢の概略値を取得する（概略位置姿勢取得）。入力された概略位置姿勢は、計測対象物体１０１の位置姿勢を求める為の繰り返し演算の初期値として与えられるものである。本実施形態では、情報処理装置１０４は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。

しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢と推定された速度・加速度から現在の位置及び姿勢を予測したものでもよい。また、様々な姿勢で撮像した対象物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチング（パターンマッチング）を行うことによって、計測対象物体１０１の大まかな位置と姿勢を推定してもよい。あるいは、他のセンサによる計測対象物体１０１の位置及び姿勢の計測が可能である場合には、該センサによる出力値を位置及び姿勢の概略値として用いてもよい。

センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめ分かっている場合にはその値を概略値として用いる。

モデル情報取得部２０４は、計測対象物体１０１の３次元モデル情報を保持する。３次元モデル情報は、３次元ＣＡＤソフトで取り扱うことができるＣＡＤモデルそのもの、もしくは３次元ＣＡＤモデルをＣｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ分野で使われる複数のポリゴン要素に変換したものである。本実施形態では、計測対象物体１０１の形状を模した３次元幾何モデルであって、ポリゴン要素で構成される３次元幾何モデルを用いる。しかしながら、形状モデルはこれにかぎられるものではなく、エッジを含めた形状を表すモデルであればなんでもよい。

エッジ検出部２０５は、取得した計測情報、装置情報、概略位置姿勢情報、およびモデル情報から、後述の影方向算出部２０６、誤対応箇所検出部２０７、および位置姿勢算出部２０８で用いるエッジの情報を検出する。ここで、検出するエッジの情報は、３次元モデルについてのものと、濃淡画像についてのものがある。検出されるエッジの情報として、本実施形態では３次元モデルのエッジ上での点の３次元座標や、仮想的に設定した画像面上でのエッジ点の２次元座標や、濃淡画像上でのエッジ点の２次元座標を用いる。しかしながら、検出するものは点に限定されず、例えば各エッジ点が属するエッジの方向（直線の方向や曲線の接線方向）を表すベクトルであってもよい。

影方向算出部２０６は、エッジ検出部２０５で算出した３次元モデルにおける各エッジ、もしくはエッジ上に設定した各点について、影方向を算出して付与する。影方向とは、投影装置１０３と撮像装置１０２、および仮想的に位置姿勢を設定された３次元モデルの間の相対的な位置関係によって算出される、計測対象物体１０１のあるエッジ近傍に発生すると推測される影の方向を示すベクトルである。本実施形態では、算出される影方向ベクトルは２次元平面上のベクトルとする。このような影方向ベクトルの例としては、後述のステップＳ３０２のように、投影装置１０３から仮想的に設定した概略位置姿勢の３次元モデルに向かう方向ベクトルを、撮像装置１０２の画像面に投影したものを用いる。

しかしながら、算出される影方向ベクトルはこれに限られるものではなく、３次元ベクトルであっても構わない。３次元ベクトルの例としては、投影装置１０３から仮想的に設定した概略位置姿勢の３次元モデルの各エッジに向かう方向ベクトルそのものを用いるのが良い。付与する影方向ベクトルは、本実施形態では３次元モデル上の各エッジ点ごとに算出することで最も厳密な影発生方向の情報を得ているが、計算処理の簡略化のために、一つのエッジを形成する直線や曲線上に複数のエッジ点が存在する場合、エッジごとに一つのまとまりとしてグループ化し、代表的な影方向ベクトルを付与しても良い。また、３次元モデルで代表的な一つの影方向ベクトルを付与しても良い。

誤対応箇所検出部２０７は、影方向の情報に基づき、誤対応箇所を抽出する。誤対応箇所とは、位置姿勢算出の際に影由来のエッジに誤って対応づく可能性のある箇所を意味する。抽出する箇所は、本実施形態では３次元モデルのエッジ上に設定した点であるが、上述の、複数のエッジ点をグループ化したエッジを抽出しても良い。

位置姿勢導出部２０８は、取得した計測情報、装置情報、概略位置姿勢の情報に加え、検出した誤対応箇所の情報を用いて、計測対象物体１０１の位置姿勢を導出（算出）する。具体的には、誤対応箇所の情報を３次元モデルのエッジの情報に反映させた上で、不図示の対応付け部が、３次元モデルのエッジと濃淡画像のエッジとの対応付けと対応点間の距離の総和が最小となる位置姿勢を最小二乗法による最適化計算を繰り返すことで、計測対象物体１０１の位置姿勢を導出する。

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

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

次に、本実施形態における位置姿勢算出の処理手順について説明する。図３は、本実施形態における位置姿勢算出の処理手順を示すフローチャートである。

［ステップＳ３０１］
まず、ステップＳ３０１においては、後の位置姿勢の算出に必要な情報が取得される。具体的には、計測情報、装置情報、概略位置姿勢、モデル情報が取得される。これらの情報は、それぞれ計測情報取得部２０１、装置情報取得部２０２、概略位置姿勢取得部２０３、モデル情報取得部２０４によって取得される。取得された情報については、エッジ点算出部２０５へ送られる。ここで、計測情報である濃淡画像は、投影装置１０３を用いて照明を照射した計測対象物体１０１を、撮像装置１０２で撮影された画像である。しかしながら、予め外部のメモリやネットワーク上に保持しておいたものを計測情報入力部２０１が読み込んだものであってもよい。また、ステップＳ３０１において取得される情報について、取得されるタイミングは、情報処理装置１０４の処理速度性能によって、同時であっても、順番に取得されても良く、取得される順番についても任意である。

［ステップＳ３０２］
次に、ステップＳ３０２において、エッジ検出部２０５によって３次元モデルと濃淡画像についてエッジの情報が算出される。

本実施形態では、具体的にエッジを以下のように求める。

３次元モデルについては、まず、３次元モデルのエッジ上に点を生成させることでエッジ点（幾何特徴）の３次元情報を得る。ここで、３次元モデルのエッジとは、仮想的に設定した撮像装置１０２の画像面で概略位置姿勢の３次元モデルを見たときに、３次元モデルの輪郭を形成している箇所を指す。エッジ上への点の生成方法は、例えば仮想的な３次元空間において配置された３次元モデルのエッジ上で等間隔になるように複数の点を生成させる方法があるが、必ずしもその方法を用いる必要はない。例えば、仮想的に設定した撮像装置１０２の画像面で概略位置姿勢の３次元モデルを見た時に画像面上でエッジ点が等間隔になるように、逆算的に３次元モデルのエッジ上の３次元座標を生成しても良い。なお、同じエッジ上に生成したエッジ点間のベクトルを算出することで、エッジの方向を表す３次元ベクトルを算出することも可能である。

次に、概略位置姿勢における３次元モデル上でのエッジ点を、仮想的に設定した撮像装置１０２の画像面上に投影させることで、画像面上で見た時のエッジ点の２次元座標を得る。算出されたエッジ点の２次元座標については、モデル上の対応するエッジ点の情報と紐づけて保存する。

濃淡画像については、画像中の輝度値を参照し、任意の探索方向の周辺画素に対して輝度値が極値をとっているような画素の２次元座標をエッジ点（画像特徴）と判定して情報を保存する。エッジ点の判定基準については閾値を設けてもよく、輝度値や輝度勾配の値が閾値以上でかつ極値をとる濃淡画像中の画素をエッジ点と判定しても構わない。また、後述のステップＳ５０３で述べるような、画像面上に投影した３次元モデルのエッジ点を起点に探索を行い、近傍に位置する濃淡画像上のエッジ点を検出する処理をあらかじめ実施してもよい。

［ステップＳ３０３］
次に、ステップＳ３０３において、影方向算出部２０６によってモデル情報のエッジについて影が発生すると推測される方向が算出され、モデル情報に付与される。本実施形態では、算出される影方向ベクトルは、投影装置１０３から仮想的に設定した概略位置姿勢の３次元モデルに向かう方向ベクトルを、撮像装置１０２の画像面に投影したものとする。

本実施形態では、具体的に影方向を以下のように求める。

まず、仮想的に設定した３次元モデルの概略位置姿勢におけるエッジ点の３次元位置（Ｘｏ、Ｙｏ、Ｚｏ）及び投影装置１０３の３次元位置（Ｘｐ、Ｙｐ、Ｚｐ）を、撮像装置１０２の画像面に投影するためのプロジェクション行列Ｐを求める。ここで用いる３次元モデルの３次元位置は、カメラに対する計測対象物体の概略位置姿勢の位置成分とする。投影装置１０３の３次元位置は、撮像装置１０２に対する３次元モデルの概略位置姿勢及び事前情報である投影装置１０３と撮像装置１０２の位置関係情報に基づいて求める。プロジェクション行列Ｐは、カメラパラメータＫ（式１）と世界座標系からカメラ座標系への変換行列ＲＴ（式２）の積Ｋ・ＲＴで表される。

（式１）

（式２）
ここでは、カメラ座標系を世界座標系と設定する。これにより、プロジェクション行列Ｐは（式３）となる。なお、カメラパラメータＫの各要素ａ、ｂ、ｃ、ｄ、ｅは、カメラ固有のパラメータであり上記のように予め求めておく。

（式３）
次に、プロジェクション行列Ｐ、３次元モデルの３次元位置の斉次座標（Ｘｏ、Ｙｏ、Ｚｏ、１）と撮像装置１０２の３次元位置の斉次座標（Ｘｐ、Ｙｐ、Ｚｐ、１）、及びそれぞれを画像面に投影した点の斉次座標（ｕｏ、ｖｏ、１）、（ｕｐ、ｖｐ、１）の関係式（式４）、（式５）から、３次元モデルの画像面での位置（ｕｏ、ｖｏ）と投影装置１０３の画像面での位置（ｕｐ、ｖｐ）を求める。そして、３次元モデルの画像面での位置（ｕｏ、ｖｏ）から投影装置１０３の画像面での位置（ｕｐ、ｖｐ）へのベクトルｒ’＝（ｕｐ−ｕｏ、ｖｐ−ｖｏ）を求め、更に単位ベクトルｒ＝ｒ’／｜ｒ’｜に変換し、これを影方向とする。

（式４）

（式５）
次に、画像面上での３次元モデルのエッジ点を影方向と関連付ける。この関連付けにより、以降のステップ３０４における誤対応箇所の抽出が可能となる。

［ステップＳ３０４］
次に、ステップＳ３０４において、誤対応箇所検出部２０７は、ステップＳ３０３で３次元モデルのエッジ点ごとに算出した影方向の情報に基づき、誤対応箇所として影由来のエッジ（影に起因したエッジ）に誤って対応づく可能性のあるエッジ点を抽出する。

図４は、本実施形態における処理手順を示すフローチャートである。以下、ステップＳ３０４内の各ステップについて説明する。

（ステップＳ４０１）
まず、誤対応箇所検出部２０７は、概略位置姿勢に基づいて仮想的に設定された３次元モデルについて、モデルの面上の任意の点を撮像装置１０２の画像面上に投影する。３次元モデルの面上への点の設定方法としては、例えば面上で等間隔になるように設定する方法が良い。また、投影した画像面上で等間隔になるように、逆算的に３次元モデルの面上の点を求めても良い。

（ステップＳ４０２）
次に、誤対応箇所検出部２０７は、画像面上に設定した点から、対応する３次元モデル上の点までの距離を算出する。ここで、距離計算の対象となる点は、ステップＳ４０１において設定した３次元モデルの面に対応する点と、ステップＳ３０２において設定した３次元モデルのエッジに対応する点の両方である。つまり、ステップＳ４０２においては、撮像装置１０２が計測対象物体１０１についての距離画像を取得するように、仮想的に設定した空間上で、３次元モデルに関しての距離画像を取得するのと等価な処理を行っている。算出された距離値については、対応する画像面上の点に紐づけて保存する。

（ステップＳ４０３）
次に、誤対応箇所検出部２０７は、着目するエッジ点について画像面上での影方向に画像面上での近傍点を探索し、対応する距離値を取得する。ここで取得する距離値はステップＳ４０２において求めたものである。

図１３は、エッジ上のある点について、影方向に画像面上の近傍点を探索したときの点と距離値の関係を示す概念図である。図１３（ａ）と図１３（ｂ）では異なる３次元モデルについて示している。図１３（ａ）、図１３（ｂ）ではそれぞれ点Ｐａ、点Ｐｂを着目するエッジ点として示している。また、点Ｐａ、点Ｐｂに対応する距離値はｄ_Ｐａ、ｄ_Ｐｂとして示している。さらに、図１３（ｂ）において、影方向に探索したときに得られる近傍点Ｐｂ＋１に対応する距離値をｄ_Ｐｂ＋１として示している。

（ステップＳ４０４）
次に、誤対応箇所検出部２０７は、着目するエッジ点について、次の２つの条件のいずれかを満たすかどうかを判定する。この条件は、着目するエッジ点を境界として影方向に大きな段差が存在するかを判定している。言い換えると、仮想的に設定した空間上で、着目するエッジ点近傍に影が発生しうるかを判定することで、後述の位置姿勢算出処理において誤って対応付く可能性の有無を判定している。
条件（ａ）：影方向の近傍点が存在しない
条件（ｂ）：着目するエッジ点の距離値と近傍点の距離値の差分が閾値以上
条件（ａ）を満足する場合、（投影された点Ｐａ（着目幾何特徴）から影の方向に所定の範囲を探索した結果、Ｐａ以外（着目幾何特徴以外）のエッジ点が検出されない場合）、画像面上では点Ｐａと背景が接して見えており、点Ｐから見て影方向に影が発生する可能性がある。図１３（ａ）は条件（ａ）を満足する例である。一方、条件（ｂ）を満足する場合、３次元モデルが大きな段差をもつ形状であり、点Ｐが段差のエッジとなっていると考えられる。この場合についても、点Ｐから見て影方向に影が発生する可能性がある。図１３（ｂ）は条件（ｂ）を満足する例である。この場合、閾値と比較する値はｄ_Ｐｂ＋１とｄ_Ｐｂの差分である。条件（ａ）もしくは条件（ｂ）を満足する場合、ステップＳ４０５に進み、それ以外の場合はステップＳ４０６に進む。

（ステップＳ４０５）
ステップＳ４０４において条件（ａ）もしくは条件（ｂ）を満足する場合、着目するエッジ点（着目幾何特徴）の近傍には影が発生する可能性があるため、計測対象物体１０１の位置姿勢を算出する際には誤って対応づいてしまう可能性が生じる。よって、ステップＳ４０５では、誤対応箇所検出部２０７は、誤対応する可能性のある点について誤対応箇所として新たにフラグ情報を付与する。

（ステップＳ４０６）
ステップＳ４０６では、誤対応箇所検出部２０７は、全エッジ点について誤対応箇所であるかどうかの判定がされたかをチェックする。全エッジ点について判定がされた場合は、ステップＳ３０３の処理を終了してステップＳ３０４に移行する。一方、まだ判定がされていないエッジ点が存在する場合は、ステップＳ４０３に移行して、ステップＳ４０４からステップＳ４０６の処理を繰り返す。

以上、ステップＳ４０１からステップＳ４０６までの処理を実施することにより、誤対応箇所を抽出する。

［ステップＳ３０５］
次に、ステップＳ３０５において、誤対応箇所検出部２０７は、ステップ３０４で抽出したエッジ点の情報を用いて、以下で説明する処理により計測対象物体１０１の位置姿勢を算出する。

図５はステップＳ３０５で示した、概略位置姿勢を用いた最適化計算により位置姿勢を算出する位置姿勢算出処理の詳細な手順を示したフローチャートである。

本実施形態では、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法により計測対象物体の位置姿勢（以下、ｓで表す）の概略位置姿勢を反復演算により繰り返し補正することにより位置姿勢を算出する。ここでは、濃淡画像上で検出されたエッジと推定される位置姿勢に基づいて画像上に投影される形状モデルのエッジとの距離の総和が最小化されるように位置姿勢を最適化する。より具体的には、濃淡画像上での点と直線の符号付距離、１次のテイラー展開によってそれぞれ物体の位置姿勢の微小変化の１次関数として表現する。そして、符号付距離が０になるような位置姿勢の微小変化に関する線形の連立方程式を立式して解くことにより、物体の位置姿勢の微小変化を求めて位置姿勢を補正することを繰り返す。

（ステップＳ５０１）
ステップＳ５０１において、位置姿勢算出部２０８が、初期化を行う。ここでは、ステップＳ３０１で取得された概略位置姿勢を３次元モデルの位置姿勢として設定する。また、モデル上のエッジ点の情報や、濃淡画像上のエッジ点の情報、および誤対応箇所に関する情報も取得する。

（ステップＳ５０２）
ステップＳ５０２において、位置姿勢算出部２０８は、３次元モデル上のエッジ点からステップＳ３０４で抽出した誤対応箇所と判定された点を以降の計算に反映させる処理を行う。これらの点については、フラグ情報が付与されているために容易に抽出が可能である。反映方法としては、本実施形態では、誤対応箇所の点を不活性化して以降の計算に用いないようにする。そのため、フラグ情報が付与されている点については、ステップＳ５０３で対応付けを行わない。しかしながら必ずしも不活性化する必要はない。不活性化する以外の反映方法としては、例えば後述のステップＳ５０３において、位置姿勢計測のための重み係数の値へ反映させてもよい。具体的には、誤対応箇所の点に対応する重み係数の値を誤対応箇所以外の点に対応する重み係数よりも低く設定することにより、誤対応箇所の位置姿勢算出結果への寄与を低くすることができる。

（ステップＳ５０３）
ステップＳ５０３において、位置姿勢算出部２０８が、エッジの対応付けを行う。図６はエッジの対応付け方法を示す図である。図６（ａ）は濃淡画像上に３次元モデルのエッジを投影した図である。まず画像上（画像中）に投影されたエッジ６０１上の制御点６０２ごとに、投影されたエッジ６０１の法線方向に探索ライン６０３をとる。次に、制御点を原点とした探索ラインの所定の範囲内で１次元のエッジの検出を行い、検出されたエッジの中で制御点に最も近い点を対応点として保持する。図６（ｂ）は原点が制御点で、横軸に探索ライン、縦軸に輝度勾配の絶対値をとるグラフである。先の画像上にエッジは画素値の輝度勾配の絶対値の極値として検出される。ここでは輝度勾配の絶対値の極値が所定の閾値６０５よりも大きく、且つ最も制御点に近い点６０４を対応点とする。

上述したように、ステップＳ５０２において、誤対応箇所と判定されたモデル点にフラグが付いている。そのため、そのフラグがついたモデル点については、本ステップで、対応付けを行わないようにしてもよい。また、対応付けした場合にも、後述するステップＳ５０５の演算において、重みを小さくしてもよい。

なお、本実施形態では、濃淡画像だけでなく、取得した距離画像についても対応付けを行う。距離画像との対応付けについては、３次元モデルの面上から任意の数の点を抽出して、概略位置姿勢に基づいて距離画像上に投影する。そして、投影された点の近傍の計測点と対応づけるようにすればよい。

（ステップＳ５０４）
ステップＳ５０４において、位置姿勢算出部２０８は、線形連立方程式を解くための係数行列と誤差ベクトルの算出を行う。ここで係数行列の各要素は、位置姿勢の推定値の微小変化に対する一次の偏微分係数であり、具体的には画像座標の偏微分係数である。誤差ベクトルは、投影されたエッジと検出されたエッジの画像上での距離である。

図７は、エッジの投影像と検出されたエッジの関係を説明する図である。図７では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。ある制御点（投影された各エッジを画像上で等間隔に分割する点）の画像上での位置を（ｕ_０、ｖ_０）、該制御点が所属するエッジの画像上での傾きをｕ軸に対する傾きθと表す。傾きθは、エッジの両端の３次元座標をｓに基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該エッジの画像上での法線ベクトルは（ｓｉｎθ、−ｃｏｓθ）となる。また、該制御点の対応点の画像座標を（ｕ’、ｖ’）とする。ここで、点（ｕ’、ｖ’）を通り、傾きがθである直線上の点（ｕ、ｖ）は、

と表せる（θは定数）。ここで、

（定数）とした。制御点の画像上での位置は計測対象物体１０１の位置姿勢により変化する。また、計測対象物体１０１の位置姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体１０１の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す３次元ベクトルなどによって表現される。位置姿勢により変化する制御点の画像座標（ｕ、ｖ）は（ｕ_０、ｖ_０）の近傍で１次のテイラー展開によって（式８）のように近似できる。但しΔｓ_ｉ（ｉ＝１、２、・・・、６）はｓの各成分の微小変化を表す。

正しいｓによって得られる制御点の画像上での位置は数６が表す直線上にあると仮定できる。（式８）によって近似されるｕ、ｖを（式７）に代入することにより、（式９）が得られる。

ただし、

（定数）とした。

情報処理装置１の座標系における３次元座標は、計測対象物体１０１の位置姿勢ｓによってモデル座標系における３次元座標（ｘ、ｙ、ｚ）に変換することができる。位置姿勢の推定値により、ある点がモデル座標系の点（ｘ_０、ｙ_０、ｚ_０）に変換されるとする。（ｘ、ｙ、ｚ）は計測対象物体１０１の位置姿勢により変化するものであり、（ｘ_０、ｙ_０、ｚ_０）の近傍で１次のテイラー展開によって数９のように近似できる。

（式９）はステップＳ５０２において対応付けが行われたすべてのエッジについて成り立つため、（式１１）のようなΔｓ_ｉに関する線形連立方程式が成り立つ。

ここで（式１１）を（式１２）のように表す。

（式１２）の線形連立方程式の係数行列Ｊを算出するための偏微分係数の算出は、例えば、以下の文献に記載されている方法で行う。
Ｖ．Ｌｅｐｅｔｉｔ ａｎｄ Ｐ．Ｆｕａ、“Ｋｅｙｐｏｉｎｔ ｒｅｃｏｇｎｉｔｉｏｎ ｕｓｉｎｇ ｒａｎｄｏｍｉｚｅｄ ｔｒｅｅｓ、”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ、ｖｏｌ．２８、ｎｏ．９、２００６．
（ステップＳ５０５）
ステップＳ５０５において、位置姿勢算出部２０８が、（式１２）をもとに、行列Ｊの一般化逆行列（Ｊ^Ｔ・Ｊ）^−１・Ｊ^Ｔを用いてΔｓを求める。

しかしながら、エッジには誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは、誤差ｄ−ｒが大きくなる。そのため（式１１）、（式１２）の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。そこで、誤差ｄ−ｒが大きいデータには小さな重みを与え、誤差ｄ−ｒが小さいデータには大きな重みを与える。重みは例えば（式１３）に示すようなＴｕｋｅｙの関数により与える。

ｃ_１、ｃ_２は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であればなんでもよい。

各データ（エッジ）に対応する重みをｗ_ｉとする。ここで（式１４）のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗ_ｉが入る。この重み行列Ｗを用いて、（式１１）を（式１４）のように変形する。

（式１６）のように（式１５）を解くことにより補正値Δｓを求める。

上記演算を、ステップＳ５０４で得られた距離画像についての対応付けについても行うことにより、濃淡画像だけでなく、距離画像を用いて精度の良い補正値を得ることができる。具体的には、距離画像も併用することによって、面方向だけでなく、奥行き方向についての位置姿勢の算出の精度が向上するという効果を得ることができる。

（ステップＳ５０６）
ステップＳ５０６では、位置姿勢算出部２０８が、ステップＳ５０５において算出された位置姿勢の補正値Δｓにより、位置姿勢をｓ＋Δｓに更新する。

（ステップＳ５０７）
ステップＳ５０７では、位置姿勢算出部２０８が、位置姿勢が収束したか否かの判定を行い、収束したと判定された場合はその時の位置姿勢をフィッティング結果として処理を終了する。収束していないと判定された場合は収束するまでＳ５０２〜Ｓ５０６を繰り返し行う。収束判定では、Ｓ５０６の補正量が所定の値以下でほぼ変化がないと判定された場合に収束したと判定する。なお、収束判定の方法はこの方法に限定されるものではなく反復回数が所定回数に達した場合には、収束したものと判定してもよい。

以上説明したように、ステップＳ５０１〜Ｓ５０７の処理により計測対象物体１０１の位置姿勢が算出される。

なお、上記では３次元の位置姿勢を算出すると説明したが、２次元の位置姿勢を算出してもよい。この場合、ベクトルｓは、位置を表す２つの要素と姿勢を表す１つの要素からなる３次元ベクトルとなる。

また、本実施形態における計測対象物体１０１の位置姿勢の算出方法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に限るものではない。例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。

（変形例１−１）
第１の実施形態のその他の変形例としては、誤対応箇所の抽出方法として、概略位置姿勢の３次元モデルの各エッジ点について、あらかじめ影が発生しうる方向を算出しておき、影方向の情報と合わせて判定することで、誤対応箇所を抽出することができる。

具体的には、まず、影方向算出部２０６で影方向を算出する。影方向の算出方法は、上述のステップＳ３０３と同様である。次に、誤対応箇所検出部２０７で３次元モデルの各エッジ点について、エッジ方向に対する法線方向を算出する。エッジ方向とは、三次元モデルを構成する各線分の進行方向である。具体的には、まず、３次元モデルの各エッジ点を２次元平面に投影する。そして、投影された各モデル点のエッジ方向に対する法線方向を取得する。ここで、エッジ方向に対する法線方向は、モデルの内側に向かうものと外側に向かうものの２通りが考えられるが、本変形例では影が発生しうる方向として、外側に向かう法線方向を算出する。次に、各エッジ点について上記の法線方向と影方向との内積値を算出し、誤対応箇所であるかどうかを判定する。ここで、内積値が所定値以上であるエッジ点について、誤対応箇所と判定する。なぜなら、内積値が大きいほど影方向と法線方向が近い方向であることを示すため、これらの方向に紐づけられるエッジ点は、誤って対応づく可能性が高くなるためである。本変形例においては、以降の処理方法は第１の実施形態と同様である。

（変形例１−２）
第１の実施形態では、概略位置姿勢取得部２０３で取得する概略位置姿勢が、計測対象物体１０１の位置姿勢を求める演算を実施する度に任意の概略位置姿勢を取得することを想定しており、影方向算出部は演算の度に新たに影方向を算出する必要がある。一方で、計測対象物体の形状や配置の方法などによって、計測対象物体１０１の位置姿勢がおおよそ何通りかに決まってしまっている場合には、計算処理時間の短縮のために、あらかじめ想定される概略位置姿勢について誤って対応づく可能性のあるエッジ点を抽出しておくことが可能である。

具体的には、ステップＳ３０１からステップＳ３０４を、あらかじめ想定される概略位置姿勢について繰り返し、各概略位置姿勢に紐づけて誤対応箇所の情報を保存する。その後、計測情報取得部２０１で取得した濃淡画像と概略位置姿勢の情報を用いて、計測対象物体１０１の位置姿勢を算出する演算処理（ステップＳ５０１からステップＳ５０６まで）を実施する。このとき、ステップＳ５０１では、上述のあらかじめ想定される概略位置姿勢が設定され、ステップＳ５０２では、概略位置姿勢に紐づいて保存された誤対応箇所が呼び出される。この変形例では、ステップＳ３０１からステップＳ３０４までの処理が各概略位置姿勢について１回で済むため、毎回計算する場合と比較して計算処理時間を短縮させることができる。

［第２の実施形態］
第１の実施形態では、ステップＳ３０２において３次元モデルについて算出したエッジ点について誤対応箇所の抽出を行った。

これに対して本発明の第２の実施形態では、同じくステップＳ３０２において濃淡画像について検出したエッジについて誤対応箇所の抽出を行う点が、第１の実施形態と異なる。誤対応箇所の抽出方法としては、エッジ点と近傍の画素における輝度値の情報を用いて誤対応箇所であるかどうかの判別を行う。

図９は、ある位置姿勢の計測対象物体１０１について撮像を実施して取得した濃淡画像について、計測対象物体１０１の位置姿勢と取得される濃淡画像での見え方との関係、および濃淡画像と輝度値プロファイルとの関係を示す概念図である。図９（ａ）のような計測対象物体１０１に位置姿勢で撮像装置１０２が撮像を実施した場合、取得される濃淡画像においては、計測対象物体１０１のエッジに加えて影によるエッジも映り込む。影によるエッジ近傍では、輝度値が影方向に沿って一旦低下し、その後増大するように変化するという特徴がある。

例えば、図９（ｂ）に示すような濃淡画像が得られ、濃淡画像から抽出したエッジ点の中で、影由来のエッジ点Ｐに着目する場合、影方向についての近傍の画素Ｐ＋１、Ｐ−１における輝度値Ｌ^Ｐ＋１とＬ^Ｐ−１はエッジ点Ｐの輝度値Ｌ^Ｐを境界として影方向に増大する。このとき、エッジ点近傍に存在する計測対象物体１０１由来のエッジ点Ｐ´の影方向について近傍の画素Ｐ´＋１、Ｐ´−１における輝度値Ｌ^Ｐ´＋１とＬ^Ｐ´−１は、エッジ点Ｐの輝度値Ｌ^Ｐ´を境界として影方向に減少する。エッジ点Ｐ近傍の箇所を濃淡画像上で見た場合、影方向に「明、暗、明」と変化しているような見え方になる。具体的には、ステップＳ８０２からステップＳ８０５にかけて濃淡画像における着目するエッジ点近傍の影方向の輝度値の変化を参照することで、このエッジが影由来のものかどうかを判定することが可能となる。

以下、本実施形態における、位置姿勢算出の処理手順について説明する。

まず、計測情報、装置情報、概略位置姿勢、モデル情報を取得し、続いてエッジを検出するが、これら一連の方法は第１の実施形態と同様なので説明を省略する。

次に、影方向を算出する。図１２は、本実施形態における影方向算出の処理手順を示すフローチャートである。以下、図１２の各ステップについて説明する。

［ステップＳ１２０１］
まず、影方向算出部２０６は、３次元モデルのエッジ点について影方向を算出する。ステップＳ１２０１における影方向の算出方法は、第１の実施形態における、ステップＳ３０３と同様である。算出された影方向は、３次元モデルのエッジ点に紐づけられて保存される。

［ステップＳ１２０２］
次に、影方向算出部２０６は、影方向を算出した３次元モデルのエッジ点について、濃淡画像のエッジ点との対応付けを行う。ステップＳ１２０２における対応付けの方法は、第１の実施形態における、ステップＳ５０３と同様である。

［ステップＳ１２０３］
次に、影方向算出部２０６は、ステップＳ１２０２において対応づけられた濃淡画像のエッジ点について、ステップＳ１２０１で算出された影方向を紐づけて保存する。

［ステップＳ１２０４］
次に、影方向算出部２０６は、３次元モデルの全エッジ点について、ステップＳ１２０１からステップＳ１２０３までの処理が完了したかをチェックする。全ての３次元モデルのエッジ点について処理が完了している場合は、ステップＳ１２０５に進む。一方、まだ処理が完了されていない３次元モデルのエッジ点が存在する場合は、ステップＳ１２０１からステップＳ１２０４の処理を繰り返す。

［ステップＳ１２０５］
次に、影方向算出部２０６は、対応付けられていない濃淡画像のエッジ点が残っているかどうかを判定する。残っていない場合は、以降の処理を省略し、影算出の処理は終了する。残っている場合は、ステップＳ１２０６に進む。

［ステップＳ１２０６］
ステップＳ１２０６では、影方向算出部２０６は、対応付けられていない濃淡画像のエッジ点の影方向を、既に対応付けられていている近傍のエッジ点の影方向の情報を参照して決定する。影方向の決定方法としては、例えば既に対応付けられているエッジ点のうち、最も近傍にあるエッジ点に紐づいている影方向としてよい。また、近傍の所定の領域内に複数の対応づけられたエッジ点が存在する場合は、それらのエッジ点に紐づく影方向の平均値としてもよい。ステップＳ１２０６で影方向を決定した後は、ステップＳ１２０５に戻る。

以上の処理を実施することにより、影方向算出部２０６は、影方向を算出する。

次に、誤対応箇所を抽出する。図８は、本実施形態における誤対応箇所抽出の処理手順を示すフローチャートである。以下、図８の各ステップについて説明する。

［ステップＳ８０１］
まず、誤対応箇所検出部２０７は、濃淡画像中の着目するエッジ点について、影方向に近傍（周囲）の画素を探索し、それらの輝度値（画素値）を取得する。ここで用いる濃淡画像は第１の実施形態において、３次元モデルとの位置姿勢算出に用いたものと同じであって構わない。

［ステップＳ８０２］
次に、誤対応箇所検出部２０７は、着目するエッジ点を境界として、取得した輝度値が影方向に増大しているかどうかを判定する。具体的には、図９（ｂ）に示すような輝度プロファイルが得られた場合、着目するエッジ点Ｐに対する影方向の近傍に存在する画素Ｐ＋１、Ｐ−１に対応する輝度値Ｌ^Ｐ＋１、Ｌ^Ｐー１について比較し、Ｌ^Ｐ＋１＞Ｌ^Ｐー１の関係にあるかどうかを判定する。判定の結果、輝度値が増大していれば誤対応箇所の候補としてステップＳ８０３に進み、それ以外の場合にはステップＳ８０３からステップＳ８０６までの処理を省略してステップＳ８０７に進む。

［ステップＳ８０３］
ステップＳ８０３では、誤対応箇所検出部２０７は、着目するエッジ点を起点として、影方向とは逆の方向へ別のエッジ点が存在するかどうかを探索し、結果を出力する。具体的には、図９（ｂ）のエッジ点Ｐを起点に影方向とは逆の方向へ探索し、探索する画素がエッジ点Ｐ´のようなエッジ点である場合は、フラグ情報を新たに付与して出力する。

［ステップＳ８０４］
ステップＳ８０４では、誤対応箇所検出部２０７は、ステップＳ８０３探索の結果を参照し、別のエッジ点が発見されたと判定した場合は、エッジ点Ｐ´について影方向の近傍画素の輝度値を取得してステップＳ８０５に進む。近傍に別のエッジ点が発見されなかった場合は、着目するエッジ点は誤対応箇所の候補から外れたと判定し、ステップＳ８０５とステップＳ８０６の処理を省略してステップＳ８０７に進む。

［ステップＳ８０５］
ステップＳ８０５では、誤対応箇所検出部２０７は、ステップＳ８０３で発見した別のエッジ点を境界として、取得した輝度値が影方向に低下しているかどうかを判定する。具体的には、図９（ｂ）のエッジ点Ｐ´に対する影方向の近傍に存在する画素Ｐ´＋１、Ｐ´−１に対応する輝度値Ｌ^Ｐ´＋１、Ｌ^Ｐ´−１について比較し、Ｌ^Ｐ´＋１＜Ｌ^Ｐ´−１の関係にあるかどうかを判定する。判定の結果、輝度値が低下していれば誤対応箇所としてステップＳ８０６に進み、それ以外の場合には着目するエッジ点は誤対応箇所の候補から外れたと判断してステップＳ８０７に進む。

［ステップＳ８０６］
ステップＳ８０６では、誤対応箇所検出部２０７は、ステップＳ８０５において誤対応箇所として判定されたエッジ点について、新たにフラグ情報を付与する。なお、ステップＳ８０６での処理内容は、第１の実施形態におけるステップＳ４０５の処理内容と同様である。

［ステップＳ８０７］
ステップＳ８０７では、誤対応箇所検出部２０７は、全エッジ点について誤対応箇所であるかどうかの判定がされたかをチェックする。全エッジ点について判定がされた場合は、誤対応箇所抽出の処理を終了して位置姿勢の算出のための処理に移行する。一方、まだ判定がされていないエッジ点が存在する場合は、ステップＳ８０１に移行して、ステップＳ８０１からステップＳ８０７の処理を繰り返す。なお、ステップＳ８０７での処理内容は、第１の実施形態におけるステップＳ４０６の処理内容と同様である。

以上の処理を実施することにより、誤対応箇所を抽出する。

次に、位置姿勢を算出する。位置姿勢の算出方法は第１の実施形態における方法と同様であるため、説明を省略する。

以上の処理を実施することにより、計測対象物１０１の位置姿勢を算出することが可能である。

（変形例２−１）
本実施形態は、３次元モデル上のエッジ点と対応づく濃淡画像上のエッジ点について誤対応箇所であるかどうかの判定を行い、誤対応箇所と判定された場合には、第１の実施形態と同様に以降の位置姿勢算出に反映させる、という処理を行うものである。位置姿勢算出における変形例としては、誤対応箇所について以降の計算への反映を行わずに対応づけを実施し、３次元モデルのエッジ点と濃淡画像の誤対応箇所が対応づいている場合には、近傍の別のエッジ点と置き換えるという方法を用いても良い。

図１４は、本変形例における位置姿勢算出の処理手順を示すフローチャートである。図１４におけるステップＳ１４０１、ステップＳ１４０２の処理内容は、第１の実施形態および第２の実施形態におけるステップＳ５０１、ステップＳ５０３と同様であるが、ステップＳ１４０２で対応づけられる濃淡画像のエッジ点は、誤対応箇所を含んでいる。ステップＳ１４０３では、ステップＳ１４０２で３次元モデルのエッジ点と対応づけた濃淡画像のエッジ点について、誤対応箇所であるかどうかの判別を行う。ステップＳ１４０３において誤対応箇所であると判定された場合は、ステップＳ１４０４において、近傍点への置き換えを行う。

ステップＳ１４０４において置き換えられる近傍点は、誤対応箇所以外のエッジ点である必要がある。具体的な例としては、ステップＳ８０３において探索された、影方向とは逆の方向に存在するエッジ点が好ましい。なぜなら、そのようなエッジ点は、計測対象物１０１のエッジに由来する点である可能性が高いからである。

以降のステップＳ１４０５からステップＳ１４０８の処理内容は、第１の実施形態および第２の実施形態におけるステップＳ５０４からステップＳ５０７までの処理内容と同様である。本変形例では、対応づいた誤対応箇所を、計測対象物に由来している可能性が高い別のエッジ点に置き換え以降の計算に用いることで、誤対応を防ぐことができる。

［第３の実施形態］
第１の実施形態では、３次元モデル上のエッジ点について誤対応箇所の抽出を行った。一方、第２の実施形態では、計測情報取得部で取得した濃淡画像上のエッジ点について、誤対応箇所の抽出を行った。これらに対し、本発明における第３の実施形態では、それぞれについて誤対応箇所の抽出を行った上で、位置姿勢算出の計算を行う。本実施形態は、第１の実施形態と第２の実施形態とを組み合わせることで、それぞれの方法において発生しうる課題をお互いに補うことを可能としている。

第１の実施形態においては、３次元モデル上のエッジ点について、影由来のエッジに誤って対応する可能性のある箇所を抽出している。この方法では、仮想的な三次元空間内において計測対象物体１０１が３次元モデルと同じく理想的な形状であり、なおかつ撮像装置１０２、投影装置１０３との相対的な位置関係が理想的な配置となっていることを前提に計算を行っている。しかしながら、実際に撮像を行う際の状況は理想的であるとは限らない。誤対応箇所として抽出していても、計測対象物１０１の形状誤差や、撮像装置１０２と投影装置１０３の位置関係の誤差などによって、誤対応箇所と対応づいた濃淡画像上のエッジ点近傍には影由来のエッジが存在しない可能性がある。つまり、そのような場合、位置姿勢算出に用いても問題ないエッジ点についても余計に排除してしまっていることになる。よって、３次元モデル上のエッジ点について抽出した誤対応箇所について、実際に撮像を行って取得した濃淡画像の情報から抽出した誤対応箇所に含まれるかどうか判定する処理を新たに加えることで、そのような課題を解決することができる。

一方、第２の実施形態においては、濃淡画像上のエッジ点について輝度値を参照し、影由来のエッジ点を抽出している。この方法では、抽出したエッジ点全てについて近傍の画素の輝度が「明、暗、明」の条件を満足するかどうかの判定を行うため、計測対象物体１０１の材質・形状や位置姿勢によっては、影由来ではないエッジについても不活性化してしまう可能性がある。その場合、位置姿勢算出に用いるエッジ点の数が減少するため、位置姿勢算出の精度が悪化してしまう可能性がある。よって、濃淡画像上のエッジ点について抽出した誤対応箇所について、３次元モデルと概略位置姿勢の情報から抽出した誤対応箇所に含まれるかどうか判定する処理を新たに加えることで、そのような課題を解決することができる。

以下、第３の実施形態における、位置姿勢の算出方法について説明する。図１０は、本実施形態における誤対応箇所抽出の処理手順を示すフローチャートである。図１０においては、ステップＳ１００１、ステップＳ１００２はそれぞれ第１の実施形態におけるステップＳ３０１、ステップＳ３０２と同様の処理である。また、ステップＳ１００３とステップＳ１００４はともに誤対応箇所の抽出を行うステップであり、第１の実施形態、第２の実施形態およびそれらの変形例におけるステップＳ３０４に対応するが、ステップＳ１００３は３次元モデルについて誤対応箇所の抽出を行うのに対し、ステップＳ１００４は濃淡画像について誤対応箇所の抽出を行う。ステップＳ１００３とステップＳ１００３を実行する順番については、並列に処理を行っても良いし、どちらか一方を先に実行した後にもう一方を実行しても良い。ステップＳ１００５では、抽出した誤対応箇所の情報を反映させた上で位置姿勢算出が行われるが、本実施形態においては、３次元モデル上のエッジ点と濃淡画像上のエッジ点の両方から抽出した誤対応箇所を用いる点で第１の実施形態、第２の実施形態とは異なる。

図１１は、本実施形態におけるステップＳ１００５の具体的な処理手順を示すフローチャートである。以下、図１１におけるステップについて説明する。

（ステップＳ１１０１）
ステップＳ１１０１では、位置姿勢導出部２０５が初期化を行う。概略位置姿勢やエッジ点の情報を取得する点は第１の実施形態、第２の実施形態と同様である。ただし、本実施形態では他の実施形態と異なり、３次元モデル上のエッジ点において抽出した誤対応箇所と濃淡画像上のエッジ点において抽出した誤対応箇所の両方を取得する。

（ステップＳ１１０２）
ステップＳ１１０２では、位置姿勢導出部２０５は、３次元モデル上のエッジ点から抽出した誤対応箇所と濃淡画像上のエッジ点から抽出した誤対応箇所の点の位置を比較し、画像面上で一致する点があるかどうかを判定する。２種類の誤対応箇所の点の位置は完全に一致していなくても、着目する点間の距離がある許容範囲内であれば、一致するとみなしても良い。その場合、点間距離の許容範囲をあらかじめ設定しておく必要があり、また、点間距離を算出するステップが別途必要となる。ステップＳ１１０２において、２種対の誤対応箇所が一致すると判定された場合はステップＳ１１０３へ進み、それ以外の場合は、誤対応箇所ではないと判定してステップＳ１１０４へ進む。

（ステップＳ１１０３）
ステップＳ１１０３では、位置姿勢導出部２０５は、ステップＳ１１０２において一致した誤対応箇所について、以降の計算に反映させる処理を行う。ここで、反映させるのは３次元モデル上のエッジ点と濃淡画像上のエッジ点の両方についてであるが、片方を反映させることで位置姿勢算出の結果に十分に効果があるのであれば、どちらか一方だけを反映させる処理であっても良い。

（ステップＳ１１０４〜ステップＳ１１０８）
ステップＳ１１０４からステップＳ１１０８までにかけての処理は、第１の実施形態、第２の実施形態におけるステップＳ５０３からステップＳ５０７までにかけての処理と同様であるため、説明を省略する。

（変形例３−１）
本実施形態においては、３次元モデルのエッジ点についての誤対応箇所の検出と、濃淡画像のエッジ点についての誤対応箇所の検出を独立に行っていたが、処理時間短縮のためには、必ずしも独立に実施する必要はない。例えば、どちらか一方の検出処理を先に行ってから対応付けを行い、対応付けの結果、誤対応箇所と対応づいたエッジ点が存在する場合に限り、そのエッジ点が誤対応箇所であるかどうかの判定を行ってもよい。本変形例においては、２つの誤対応箇所が一致するかどうかの判定を行わずに済むため、処理時間の短縮効果が期待できる。

＜各実施形態の効果＞
第１の実施形態では、３次元モデル上のエッジ点について、影の発生する方向と概略位置姿勢の情報を用いて誤って影由来のエッジに対応づく可能性があるかどうかを判定し、位置姿勢算出において反映させることで位置姿勢計測の安定性を向上させ、ロバスト性の高い位置姿勢計測が可能となる。

第２の実施形態では、濃淡画像上のエッジ点について、影の発生する方向と輝度値の情報を用いて影由来のエッジであるかどうかを判定し、位置姿勢算出において誤対応箇所の情報を反映させることで位置姿勢計測の安定性を向上させ、ロバスト性の高い位置姿勢計測が可能となる。

第３の実施形態では、３次元モデル上のエッジ点と濃淡画像上のエッジ点の各々について抽出した誤対応箇所について、一致するものがあるかどうかを判定し、位置姿勢算出において反映させることで位置姿勢計測の安定性を向上させ、ロバスト性の高い位置姿勢計測が可能となる。

（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。

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

このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。

Claims (12)

1. 撮像装置により撮像された、計測対象物体を含む画像を取得する取得手段と、
   前記計測対象物体の概略位置姿勢情報を取得する概略位置姿勢取得手段と、
   前記計測対象物体の複数の幾何特徴を表す３次元モデル情報を保持する保持手段と、
   前記撮像装置の位置に基づいて、前記画像における前記計測対象物体の影の方向を推定する推定手段と、
   前記概略位置姿勢に基づき前記複数の幾何特徴を２次元平面に投影し、前記影の方向に基づいて前記幾何特徴ごとに前記計測対象物体の影に対応する可能性があるかを判定し、該可能性があると判定された幾何特徴に対して第１のフラグを設定する第１の設定手段と、
   前記画像から複数の画像特徴を検出する検出手段と、
   前記概略位置姿勢に基づいて前記複数の幾何特徴を前記画像に投影し、前記設定手段で設定された第１のフラグに基づいて該投影された複数の幾何特徴と前記複数の画像特徴とを対応づける対応づけ手段と、
   前記対応付け手段で対応付けされた結果に基づいて、前記計測対象物体の位置姿勢を導出する導出手段とを有することを特徴とする情報処理装置。
2. 前記導出手段は、前記対応づけられた前記画像特徴と前記幾何特徴とのずれを小さくするように前記概略位置姿勢を更新することにより、前記計測対象物体の位置姿勢を導出することを特徴とする請求項１に記載の情報処理装置。
3. 前記設定手段は、前記概略位置姿勢に基づき前記複数の幾何特徴を前記２次元平面に投影し、該投影された複数の幾何特徴のうちの着目幾何特徴から前記影の方向に所定の範囲内で探索を行った結果、前記着目幾何特徴以外の他の幾何特徴が検出されなかった場合に、前記計測対象物体の影に対応する可能性があると判定し前記第１のフラグを設定することを特徴とする請求項１に記載の情報処理装置。
4. 前記設定手段は、前記概略位置姿勢に基づき三次元空間に配置された前記複数の幾何特徴を前記２次元平面に投影し、前記２次元平面における前記投影された複数の幾何特徴のうちの着目幾何特徴の方向と前記影の方向とに基づいて、前記計測対象物体の影に対応する可能性があると判定し前記フラグを設定することを特徴とする請求項１または２に記載の情報処理装置。
5. 更に、前記検出手段によって検出された複数の画像特徴に対して、該画像特徴の近傍の画素値に基づいて、該画像特徴が前記計測対象物体の影に起因する画像特徴かを判定し、該影に起因する画像特徴と判定された場合に前記画像特徴に対して第２のフラグを設定する第２の設定手段を備え、
   前記対応付け手段は、更に前記第２のフラグに基づいて該投影された複数の幾何特徴と前記複数の画像特徴とを対応づけることを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記対応付け手段は、前記第１のフラグが設定されている幾何特徴については、前記画像特徴との対応付けを行わないことを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
7. 撮像装置により撮像された、計測対象物体を含む画像を取得する取得手段と、
   前記計測対象物体の概略位置姿勢情報を取得する概略位置姿勢取得手段と、
   前記計測対象物体の複数の幾何特徴を表す３次元モデル情報を保持する保持手段と、
   前記撮像装置の位置に基づいて、前記画像における前記計測対象物体の影の方向を推定する推定手段と、
   前記画像から複数の画像特徴を検出する検出手段と、
   前記検出手段によって検出された複数の画像特徴に対して、前記複数の画像特徴それぞれの位置から前記影の方向に存在する画素値に基づいて、前記複数の画像特徴それぞれが前記計測対象物体の影に起因する画像特徴かを判定し、影に起因する画像特徴と判定された場合に該画像特徴に対して第２のフラグを設定する第２の設定手段と、
   前記概略位置姿勢に基づいて前記複数の幾何特徴を前記画像に投影し、前記設定手段で設定された第２のフラグに基づいて該投影された複数の幾何特徴と前記複数の画像特徴とを対応づける対応づけ手段と、
   前記対応付け手段で対応付けされた結果に基づいて、前記計測対象物体の位置姿勢を導出する導出手段とを有することを特徴とする情報処理装置。
8. 前記対応付け手段は、前記第２のフラグが設定されている画像特徴については、前記幾何特徴との対応付けを行わないことを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
9. 前記対応付け手段は、前記幾何特徴と対応付けられた画像特徴に前記第２のフラグが設定されている場合、前記画像特徴とは異なる画像特徴を抽出し、該抽出した画像特徴と前記幾何特徴とを対応付けることを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
10. 計測対象物体を含む画像を取得する取得工程と、
    前記計測対象物体の概略位置姿勢情報を取得する概略位置姿勢取得工程と、
    前記撮像装置の位置に基づいて、前記画像における前記計測対象物体の影の方向を推定する推定手段と、
    前記概略位置姿勢に基づき前記計測対象物体を表す複数の幾何特徴を２次元平面に投影し、前記影の方向に基づいて前記幾何特徴ごとに前記計測対象物体の影に対応する可能性があるかを判定し、該可能性があると判定された幾何特徴に対して第１のフラグを設定する第１の設定工程と、
    前記画像から複数の画像特徴を検出する検出工程と、
    前記概略位置姿勢に基づいて前記複数の幾何特徴を前記画像に投影し、前記設定手段で設定された第１のフラグに基づいて該投影された複数の幾何特徴と前記複数の画像特徴とを対応づける対応づけ工程と、
    前記対応付け手段で対応付けされた結果に基づいて、前記計測対象物体の位置姿勢を導出する導出工程とを有することを特徴とする情報処理方法。
11. 撮像装置により撮像された、計測対象物体を含む画像を取得する取得工程と、
    前記計測対象物体の概略位置姿勢情報を取得する概略位置姿勢取得工程と、
    前記撮像装置の位置に基づいて、前記画像における前記計測対象物体の影の方向を推定する推定手段と、
    前記画像から複数の画像特徴を検出する検出手段と、
    前記検出手段によって検出された複数の画像特徴に対して、前記複数の画像特徴それぞれの位置から前記影の方向に存在する画素値に基づいて、前記複数の画像特徴それぞれが前記計測対象物体の影に起因する画像特徴かを判定し、影に起因する画像特徴と判定された場合に該画像特徴に対して第２のフラグを設定する設定工程と、
    前記概略位置姿勢に基づいて前記計測対象物体を表す複数の幾何特徴を前記画像に投影し、前記設定工程において設定された第２のフラグに基づいて該投影された複数の幾何特徴と前記複数の画像特徴とを対応づける対応づけ工程と、
    前記対応付け工程で対応付けされた結果に基づいて、前記計測対象物体の位置姿勢を導出する導出工程とを有することを特徴とする情報処理装置。
12. コンピュータを、請求項１乃至９のいずれか１項に記載の情報処理装置の各手段として機能させるためのプログラム。