JP2015090560A - 画像処理装置、画像処理方法 - Google Patents

Abstract

【課題】 ノイズがある状況においても、ロバストな位置姿勢計測を行うこと。
【解決手段】 対象物体が含まれる画像を取得し、取得した画像に含まれる対象物体の概略位置姿勢を取得し、取得した画像で前記対象物体の影領域に関する情報を推定し、推定された影領域に関する情報に基づいて、概略位置姿勢を補正し、補正された概略位置姿勢と保持するモデル情報とに基づいて、画像中の対象物体の位置姿勢を導出する。
【選択図】 図２

Description

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

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

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

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

しかしながら、特許文献１に記載の方法では、対象物体を撮影した輝度画像や距離画像に影や鏡面反射などのノイズが存在する場合、必ずしも最適な位置姿勢が得られるとは限らず、精度やロバスト性が低下する。例えば、対象物体の周囲に生じた影に起因するエッジと形状モデルのエッジとを誤対応させてしまうことにより、局所解に陥り、正しいフィッティング結果が得られないことがある。

本発明は、以上の課題を鑑みてなされたものであり、ロバスト性の高い位置姿勢計測を行うことを目的とする。

上記の課題を解決するために、例えば、本発明の画像処理装置は、対象物体が含まれる画像を取得する画像取得手段と、前記対象物体のモデル情報を保持する保持手段と、前記画像に含まれる対象物体の概略位置姿勢を取得する取得手段と、前記取得された画像中で前記対象物体の影領域に関する情報を推定する推定手段と、前記推定された影領域に関する情報に基づいて、前記概略位置姿勢を補正する補正手段と、前記補正手段によって補正された概略位置姿勢と前記モデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出手段とを備える。

また、例えば、本発明の画像処理装置は、対象物体が含まれる画像を取得する画像取得手段と、前記対象物体のモデル情報を保持する保持手段と、前記画像に含まれる対象物体の概略位置姿勢を取得する取得手段と、前記取得された画像中で前記対象物体の反射領域を推定する推定手段と、前記推定された反射領域に基づいて、前記概略位置姿勢を補正する補正手段と、前記補正手段によって補正された概略位置姿勢と前記モデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出手段とを備える。

また、例えば、本発明の画像処理装置は、対象物体が含まれる画像を取得する画像取得手段と、前記対象物体のモデル情報と前記対象物体の取り得る位置姿勢の確率情報と保持する保持手段と、前記画像に含まれる対象物体の概略位置姿勢を取得する取得手段と、前記補正手段は、前記保持する確率情報に基づいて、前記概略位置姿勢を補正手段と、前記補正手段で補正された概略位置姿勢と前記保持手段で保持するモデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出手段とを備える。

本発明によれば、ロバスト性の高い位置姿勢計測を行うことができる。

第１〜第３の実施形態におけるシステムの構成を示した図。 第１〜第３の実施形態におけるプログラムのモジュール構成を示す図。 第１〜第３の実施形態における位置姿勢算出処理手順を示したフローチャート。 第１の実施形態における影の幅の算出方法の原理を示す図。 第１〜第３の実施形態における位置姿勢算出処理の詳細な手順を示したフローチャート。 第１〜第３の実施形態における対応点の探索の原理を示す図。 第１〜第３の実施形態におけるエッジの投影像と検出されたエッジの関係を説明する図。 （ａ）第１の実施形態における計測対象物体１０１を撮影した輝度画像の模式図 （ｂ）第１の実施形態における計測対象物体１０１を撮影した輝度画像８０１の探索ライン８０５における輝度値の変化を示した図。 第２の実施形態における計測対象物体９０５上で鏡面反射が発生した状況を示した模式図。 第２の実施形態における事前情報算出のための具体的な処理手順を示したフローチャート。 第３の実施形態における計測対象物体１１０１の安定姿勢の例を示す図。 第４の実施形態におけるロボットシステムの構成例を示す図。 第３の実施形態における計測対象物体１１０１とカメラ１０２の姿勢関係を示す図。 画像処理装置１４０に適用可能なハードウェア構成例を示すブロック図。

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

（第１の実施形態）
図１は、第１の実施形態における画像処理装置のシステム構成の模式図である。

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

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

プロジェクタ１０３は、計測対象物体１０１に所定の光を照射するために使用する。所定の光としては、一様な光（全点灯光）や距離画像を取得するためのスリット光などでいずれであってもかまわない。プロジェクタ１０３は、液晶プロジェクタである。プロジェクタの内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は、例えば、下記の文献に開示される方法により事前に校正しておく。なお、スリット光の投影ができればプロジェクタ１０３以外の装置を用いても良く、例えば、ＤＭＤ（デジタルミラーデバイス）やＬＣＯＳを用いたプロジェクタであってもよい。

Ｚ． Ｚｈａｎｇ， “Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，” ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｖｏｌ．２２， ｎｏ．１１， ｐｐ．１３３０−１３３４， ２０００
画像処理装置１０４は、撮像装置１０２およびプロジェクタ１０３と接続されており、プロジェクタ１０３及び撮像装置１０２の動作制御を行うと共に、撮像装置１０３から得られた撮像画像を用いて、計測対象物体１０１の位置姿勢を求める。本発明の具体的な処理が記述されたプログラムを内部に保持し、内蔵するＣＰＵがそのプログラムを実行することで、計測対象物体１０１の位置姿勢計算を実行する装置である。また、画像処理装置１０４は、キーボード、マウスまたはモニタなどのユーザインターフェースを備える。

次に、画像処理装置１０４の機能構成例について、図２のブロック図を用いて説明する。計測情報入力部２０１は、撮像装置１０２が取得した輝度画像を計測情報として画像処理装置に取り込む（画像取得）。

事前情報保持部２０２は、プロジェクタ１０３と撮像装置１０２との位置関係情報を事前情報として保持する。プロジェクタ１０３と撮像装置１０２との位置関係情報は、プロジェクタからパターン光を照射してカメラで撮影し、例えば以下の文献に記載された方法で求める。

Ｍ． Ｋｉｍｕｒａ, “Ｐｒｏｊｅｃｔｏｒ Ｃａｌｉｂｒａｔｉｏｎ ｕｓｉｎｇ Ａｒｂｉｔｒａｒｙ Ｐｌａｎｅｓ ａｎｄ Ｃａｌｉｂｒａｔｅｄ Ｃａｍｅｒａ”Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｐａｔｔｅｒｎ Ｒｅｃｏｇｎｉｔｉｏｎ、 ＣＶＰＲ、 ２００７．
また、事前情報保持部２０２は、計測対象物体１０１の３次元モデル情報を保持する。３次元モデル情報は、３次元ＣＡＤソフトで取り扱うことができるＣＡＤモデルそのもの、もしくは３次元ＣＡＤモデルをＣｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ分野で使われる複数のポリゴン要素に変換したものである。本実施形態では、計測対象物体１０１の形状を模した３次元幾何モデルであって、ポリゴン要素で構成される３次元幾何モデルを用いる。しかしながら、モデル情報はこれにかぎられるものではなく、例えば２次元的なモデルであってもかまわない。

概略位置姿勢取得部２０３は、撮像装置２０３０に対する計測対象物体の位置及び姿勢の概略値を取得する（概略位置姿勢取得）。入力された概略位置姿勢は、計測対象物体１０１の位置姿勢を求める為の繰り返し演算の初期値として与えられるものである。本実施形態では、画像処理装置１４０は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。

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

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

概略位置姿勢補正部２０４は、概略位置姿勢取得部２０３が取得した概略位置姿勢を、事前情報保持部２０２が保持している事前情報に基づいて補正する。具体的な処理は後述するが、本実施形態では、概略位置姿勢を計測対象物体１０１の影が存在しない方向（影と反対方向）にシフトさせることによって、位置姿勢算出処理におけるエッジの誤対応を防ぐことができる。

位置姿勢導出部２０５は、概略位置姿勢補正部２０４によって補正された概略位置姿勢と３次元モデル情報とに基づいて、計測対象物体の位置姿勢を導出（算出）する。具体的には、撮像装置１０２によって撮像された撮像画像に基づいて、不図示のエッジ検出部が、計測情報である輝度画像上からエッジを検出する。そして、不図示の対応付け部が、保持しているモデルエッジとの対応付けと対応点間の距離の総和が最小となる位置姿勢を最小二乗法による最適化計算を繰り返すことで、計測対象物体１０１の位置姿勢を導出する。

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

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

（ステップＳ３０１）
まず、ステップＳ３０１において、計測情報入力部２０１が、撮像装置１０２から計測対象物体１０１を撮影した輝度画像を取得し、位置姿勢導出部２０５に送る。輝度画像は、プロジェクタ１０３を用いて照明を照射した計測対象物体１０１を、撮像装置１０２で撮影された画像である。しかしながら、予め外部のメモリやネットワーク上に保持しておいたものを計測情報入力部２０１が読み込んだものであってもよい。

（ステップＳ３０２）
次に、ステップＳ３０２において、概略位置姿勢補正部２０４が、事前情報保持部２０２が保持する事前情報に基づいて、計測対象物体１０１の概略位置姿勢を補正する。概略位置姿勢の補正方向は、計測対象物体１０１からプロジェクタ１０３までの方向ベクトルを、撮像装置１０２の画像面に投影したときの方向ベクトルとする。概略位置姿勢の補正量は、プロジェクタ１０３からの照明によって生じた計測対象物体１０１の影の領域の幅とする。

本実施形態では、補正方向を以下のように求める。

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

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

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

本実施形態では、補正量は、事前情報保持部２０２が保持する３次元モデル情報から取得される計測対象物体１０１の高さ、プロジェクタ１０３と撮像装置１０２との位置関係情報及び計測対象物体１０１の概略位置姿勢を用いて求める。

具体的には、図４に示したように、計測対象物体１０１と水平面とのなす角度をθとする。そして、事前情報保持部２０２が保持する３次元モデルから計測対象物体１０１の高さｈを取得する。

計測対象物体１０１の高さは、計測対象物体１０１が配置される向きが予め決まっている場合には、決まっている向きで配置したときの配置面からの高さとする。また、予め決まっていない場合には、計測対象物体１０１が安定する向きで配置したときの高さとする。例えば、安定する向きは、計測対象物体１０１の重心位置が低くなる向きに設定すればよい。

また、計測対象物体１０１を囲む最小のバウンディングボックスを用いてもよい。この場合、バウンディングボックスの各面が配置面に接する配置方法（６つ）のうち、計測対象物体１０１の重心が最も低くなる向きに配置したときのバウンディングボックスの高さとする。ただし、これに限らず起こり得る確率が高い計測対象物体１０１の姿勢における高さが設定できれば方法は何でもよい。

そして、取得したｈとｃｏｓθ及び撮像装置１０２から計測対象物体までの距離Ｈと撮像装置１０２とプロジェクタ１０３間の距離Ｂとの比Ｂ／Ｈとを乗じたもの（ｈ×ｃｏｓθ×（Ｂ／Ｈ））を影の幅として求める。そして、求めた影の幅を補正量ｗとする。

上記で述べた方法で求め補正方向及び補正量に基づいて、計測対象物体１０１の概略位置姿勢を補正する。具体的には、概略位置姿勢（位置Ｘ、姿勢Ｒ）の位置Ｘにｗ・ｒを加算する。即ち、補正後の概略位置姿勢は、位置（Ｘ＋ｗ・ｒ）、姿勢Ｒとなる。

これにより、概略位置姿勢が影の存在しない方向にシフトするので、以降の位置姿勢算出処理において、モデルのエッジが影領域に起因するエッジに誤対応してしまい、局所解に陥ることを防止することができる。

なお、補正方向または補正量を画像処理装置１０４に備えられたユーザインターフェースを介してユーザが手動で値を指定してもよい。

（ステップＳ３０３）
ステップＳ３０３において、ステップ３０２で補正した概略位置姿勢を用いて、以下で説明する処理により計測対象物体１０１の位置姿勢を算出する。

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

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

（ステップＳ５０１）
ステップＳ５０１において、位置姿勢導出部２０５が、初期化を行う。ここでは、ステップＳ３０２で補正された概略位置姿勢を計測対象物体１０１の位置姿勢として設定する。

（ステップＳ５０２）
ステップＳ５０２において、位置姿勢導出部２０５が、エッジの対応付けを行う。まず、設定された計測対象物体１０１の概略位置姿勢に基づいて、３次元形状モデルの各エッジを画像面に投影する。その後エッジの対応付けを行う。

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

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

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

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

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

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

ただし、

（定数）とした。

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

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

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

（式１２）の線形連立方程式の係数行列Ｊを算出するための偏微分係数の算出は、例えば、以下の文献に記載されている方法で行う。

Ｖ．Ｌｅｐｅｔｉｔ ａｎｄ Ｐ．Ｆｕａ、 “Ｋｅｙｐｏｉｎｔ ｒｅｃｏｇｎｉｔｉｏｎ ｕｓｉｎｇ ｒａｎｄｏｍｉｚｅｄ ｔｒｅｅｓ、” ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ、 ｖｏｌ．２８、 ｎｏ．９、 ２００６．
（ステップＳ５０４）
ステップＳ５０４において、位置姿勢導出部２０５が、（式１２）をもとに、行列Ｊの一般化逆行列（Ｊ^Ｔ・Ｊ）^−１・Ｊ^Ｔを用いてΔｓを求める。

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

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

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

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

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

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

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

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

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

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

また、計測対象物体１０１の位置姿勢の算出方法は、様々な姿勢で撮像した対象物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチング（パターンマッチング）を行うことによって、計測対象物体１０１の位置姿勢を推定してもよい。例えば、以下の文献の方法に記載の方法で行っても良い。即ち、予め複数の姿勢で撮影した対象物体の画像をリファレンス画像として保持しておく。そして、リファレンス画像と入力画像のそれぞれから抽出されるエッジ同士の対応間の距離の誤差の総和が最小となるリファレンス画像に基づいて、対象物体１０１の位置姿勢を推定してもよい。
Marcel German、“Automatic Pose Estimation For Range Images on the GPU”、3DMI、 2007
（変形例１−１）
第１の実施形態では、事前情報保持部２０２が保持する事前情報に基づいて概略位置姿勢を補正したが、計測情報入力部２０１を介して得られる計測情報に基づいて概略位置姿勢を補正してもよい。

具体的には、例えば、以下の文献に示すような方法で、輝度画像に基づいてプロジェクタの位置を推定してもよい。そして、第１の実施形態で述べた方法で、計測対象物体からプロジェクタまでの方向ベクトルを画像面に投影した方向ベクトルを求め、これを補正方向とする。なお、光源はプロジェクタに限らず、他の光源であってもよい。例えば、蛍光灯照明装置であってもよい。

山下 幸宏、坂上 文彦、佐藤 淳、“非平面に投影された影に基づく形状と光源の同時推定”、画像の認識・理解シンポジウム、２０１１。

また、補正量は、輝度画像中の輝度値が連続して低い部分を影領域として抽出し、抽出した影領域における補正方向での幅とする。図８（ａ）は、計測対象物体１０１を撮影した輝度画像の模式図である。

領域８０２は、輝度画像８０１における計測対象物体領域、８０３は影領域である。探索ライン８０５を、補正方向８０４でかつ影領域の中心付近を通るように設定する。探索ライン８０５において各画素の輝度値を取得し、予め設定した閾値よりも小さくなる連続領域を検出する。図８（ｂ）は、横軸に探索ライン８０５、縦軸に輝度値を示した図である。輝度値が予め設定した閾値８０６よりも小さくなる連続幅を検出し、これを補正量とする。

なお、補正方向や補正量は、影の方向や影の領域を求めることができれば他の方法で求めてもよい。例えば、計測対象物体の輝度画像と同時に距離画像を取得し、距離画像の距離値が取得できなかった領域を影の領域としてもよい。また、影の領域の輪郭を抽出し、光源に最も近い距離値をもつ輪郭上の点から最も遠い距離値をもつ輪郭上の点までの方向を影の方向としても良い。

また、補正方向または補正量を画像処理装置１０４に備えられたユーザインターフェースを介してユーザが手動で値を指定してもよい。例えば、取得した輝度画像を画像処理装置１０４に備えられたモニタ上に表示し、ユーザが影の領域や落ちる方向を目視で確認し、マウスやキーボードなどを使用して補正方向または補正量を指定する。

（第２の実施形態）
第２の実施形態では、計測対象物体９０５上で鏡面反射が観測されることが原因で計測対象物体９０５の位置姿勢計測の安定性が低下するのを防止するための方法について説明する。

第１の実施形態で述べたように、計測対象物体を撮影した輝度画像上から検出したエッジ（計測エッジ）に計測対象物体の輪郭や凹凸部以外のエッジが含まれていた場合、エッジ同士の対応付けに誤りが発生し、位置姿勢計測精度が低下する危険性がある。計測対象物体上での鏡面反射領域の輝度値は周辺領域の輝度値に比べ非常に高く、鏡面反射領域の境界でエッジが検出される。第１の実施形態では、計測エッジに影のエッジが含まれている場合について説明したが、本実施形態では、鏡面反射領域の境界のエッジが計測エッジに含まれている場合について説明する。

第２の実施形態は、第１の実施形態と基本的な構成がほぼ共通であるが、第１の実施形態で説明した一連の処理のうち、事前情報保持部２０２が保持する事前情報及び補正方法（ステップＳ３０２）が異なる。以下では、本実施形態における事前情報保持部２０２が保持する事前情報及び補正方法についてのみ説明する。

図９は、計測対象物体９０５上で鏡面反射が発生した状況を示した模式図である。撮像装置１０２の観測方向ベクトル９０１と計測対象物体１０１の接点が、物体の輪郭エッジである。図９では輪郭エッジのみを図示したが、凹凸形状のある物体の場合には、凹凸部でもエッジが生じる。

以下では、輪郭エッジと凹凸部に生じるエッジをまとめて構造的エッジと呼ぶこととする。鏡面反射が発生することで、計測対象物体を撮影した輝度画像上から、鏡面反射領域の境界のエッジ９０２が検出される。このため、概略位置姿勢によっては、対応付けにおいて、モデルエッジが構造的エッジ９０３ではなく、鏡面反射領域の境界のエッジ９０２と誤って対応付く危険性がある。

そこで、モデルエッジが鏡面反射領域の境界のエッジ９０２と誤って対応付けされることを抑制するために、概略位置姿勢を補正する方法について説明する。

具体的には、上記のように予め求めておいた概略位置姿勢を、鏡面反射領域の情報と、構造的エッジと構造的エッジから最も近傍にある鏡面反射領域の境界のエッジとの位置関係の情報に基づいて補正する。

鏡面反射領域１００４の情報は、鏡面反射領域の重心位置とする。また、構造的エッジと鏡面反射領域の境界のエッジとの位置関係の情報は、鏡面反射領域の重心と、重心から構造的エッジの法線ベクトル方向に伸ばした線分と構造的エッジの交点、との相対位置とする。

事前情報保持部２０２は、事前情報としてプロジェクタ１０３と計測対象物体９０５との位置関係、撮像装置１０２と計測対象物体９０５との位置関係、計測対象物体９０５の形状モデル及び表面特性（反射特性）を保持する。

これらの事前情報に基づいて、鏡面反射領域の情報（反射領域情報）及び構造的エッジと鏡面反射領域の境界のエッジとの位置関係の情報を求める方法について、図１０に示したフローチャートを用いて説明する。本実施形態では、図１０の処理を概略位置姿勢補正部２０４が行うものとして説明するが、本処理は、その他、事前情報保持部２０２などによって行われてもよい。

（ステップＳ１００１）
ステップＳ１００１では、概略位置姿勢補正部２０４が、鏡面反射なしの属性を設定した計測対象物体９０５の形状モデルのＣＧ画像をカメラの位置姿勢に基づいて、所定の光源環境で描画し（レンダリング）、輝度画像を生成する。

（ステップＳ１００２）
ステップＳ１００２では、概略位置姿勢補正部２０４が、ステップＳ１００１で生成した輝度画像から、輝度値の勾配が予め設定した閾値以上となる部分をエッジとして検出する。

ここでは、計測対象物体９０５の構造的エッジが検出される。

（ステップＳ１００３）
ステップＳ１００３では、概略位置姿勢補正部２０４が、計測対象物体９０５の形状モデルに鏡面反射の属性（反射特性）を設定し、撮像装置１０２の位置姿勢に基づいて所定の光源環境で描画し（レンダリング）、輝度画像を生成する。鏡面反射の属性は、計測対象物体９０５の材質や表面形状による反射率の変化に基づいて決定する。

（ステップＳ１００４）
ステップＳ１００４では、概略位置姿勢補正部２０４が、ステップＳ１００３で生成した輝度画像から、輝度値が飽和している領域を鏡面反射領域として抽出する。そして、抽出した鏡面反射領域から領域の重心または輪郭を抽出する。輪郭の抽出は、画像処理の一般的な手法として知られる輪郭追跡を用いて行う。これらを鏡面反射領域の情報とする。

（ステップＳ１１０５）
ステップＳ１１０５では、概略位置姿勢補正部２０４が、ステップＳ１００２で検出した構造的エッジから最も近傍（周囲）にある鏡面反射領域を決定する。例えば、構造的エッジと鏡面反射領域の重心までの距離が最小となる鏡面反射領域を最も近傍にあるものとして決定する。そして、鏡面反射領域の重心と、鏡面反射領域の重心から構造的エッジの法線ベクトル方向に伸ばした線分と構造的エッジの交点、との相対位置を求める。これを構造的エッジと鏡面反射領域の境界のエッジとの位置関係の情報とする。

そして、事前情報保持部２０２に記憶させる。

なお、ステップＳ１００１及びステップＳ１００３では、対象物体１０１の輝度画像を、概略位置姿勢補正部２０４が形状モデルを描画（レンダリング）することで生成したが、これに限らず別の方法であっても良い。例えば、ステップＳ１００１の処理に代わって、鏡面反射が発生しない位置に光源を配置して対象物体を撮像した画像を事前情報保持部２０２が保持するようにしてもよい。また、ステップＳ１００３の処理に代わって、所定の光源条件で対象物体を撮像した画像を事前情報保持部２０２が保持するようにしてもよい。

次に、事前情報保持部２０２が保持する事前情報に基づいて、概略位置姿勢補正部２０４が、概略位置姿勢を補正する方法について説明する。具体的には、事前情報を用いて、鏡面反射領域の重心Ａから、重心から構造的エッジの法線ベクトル方向に伸ばした線分と構造的エッジの交点ＢまでのベクトルＡＢを求める。このベクトルの方向を補正方向、ベクトルの大きさを補正量とし、概概略位置姿勢（位置Ｘ、姿勢Ｒ）の位置Ｘにこれを加算する。これより、補正後の概略位置姿勢は、位置（Ｘ＋ＡＢ）、姿勢Ｒとなる。

なお、鏡面反射領域の情報は、鏡面反射領域の重心に限らず、鏡面反射の領域の位置が示せれば何でも良い。例えば、鏡面反射領域の輪郭としても良い。また、構造的エッジと鏡面反射領域の境界のエッジとの位置関係の情報は、鏡面反射領域の重心と、鏡面反射領域の重心から構造的エッジの法線ベクトル方向に伸ばした線分と構造的エッジの交点との相対関係に限らない。構造的エッジと鏡面反射領域の境界のエッジとの位置関係を示すものであれば何でも良い。例えば、構造的エッジからの距離が最小となる鏡面反射領域の輪郭上の点と、構造的エッジからの距離が最小となる鏡面反射領域の輪郭上の点から構造的エッジの法線ベクトル方向に伸ばした線分と構造的エッジの交点との相対位置であってもよい。

以上のように算出した補正量及び補正方向を用いて、ステップＳ３０２以降の処理を第１の実施形態と同様に実行する。

（変形例２−１）
第２の実施形態では、事前情報保持部２０２が保持する事前情報に基づいて概略位置姿勢を補正したが、計測情報入力部２０１を介して得られる計測情報に基づいて、概略位置姿勢を補正してもよい。

具体的には、計測対象物体を撮影した輝度画像上から輝度値が飽和している領域を鏡面反射領域として抽出し、鏡面反射の重心または輪郭位置などの情報を鏡面反射領域の情報として算出する。また、輝度画像上から構造的エッジを抽出し、鏡面反射領域と鏡面反射領域近傍に存在する構造的エッジとの位置関係を算出する。そして、ここで求めた鏡面反射領域の情報及び鏡面反射領域と鏡面反射領域近傍に存在する構造的エッジとの位置関係を用いて、概略位置姿勢の補正量及び補正方向を決定する。

なお、光源はプロジェクタに限らず他の光源であってもよい。例えば、蛍光灯照明装置であってもよい。

また、補正方向または補正量を画像処理装置１０４に備えられたユーザインターフェースを介してユーザが手動で値を指定してもよい。例えば、取得した輝度画像を画像処理装置１０４に備えられたモニタ上に表示し、ユーザが影の領域や落ちる方向を目視で確認し、マウスやキーボードなどを使用して補正方向または補正量を指定する。

（第３の実施形態）
第３の実施形態では、計測対象物体１１０１の概略位置姿勢を、計測対象物体１１０１が配置される確率が高い姿勢に補正する方法について説明する。計測対象物体１１０１からエッジが密集して検出される場合など、対応付けの誤りが発生しやすい状況では、概略位置姿勢を実際の位置姿勢にできるだけ近いものに設定することで対応付けの誤りが抑制される。

第３の実施形態は、第１、２の実施形態と基本的な構成がほぼ共通であるが、第１及び第２の実施形態で説明した一連の処理のうち、事前情報保持部２０２が保持する事前情報及び補正方法（ステップＳ３０２）が異なる。以下では、本実施形態における事前情報保持部２０２が保持する事前情報及び補正方法についてのみ説明する。

事前情報として、計測対象物体１１０１の姿勢の存在確率（確率情報）を保持する。計測対象物体１１０１の姿勢の取り得る確率は、計測対象物体１１０１の姿勢の安定度とする。安定度は、例えば、物体を配置したときの倒れにくさから求める。本実施形態では、以下の方法で安定度を求める。

まず、物体の安定姿勢を決定する。安定姿勢は、物体の形状から抽出される面、エッジまたは点が、水平面と接する時の物体の姿勢とする。エッジのみの場合には２つ以上、点のみの場合には３点以上、エッジと点が両方接する場合には合計で２点以上接し、面が１つでも接する場合にはエッジや点が接する数はいくつでも構わない。

物体の凸形状から面を抽出し、この抽出した面と接する平面を求め、求めた平面が計測対象物体と交差しないどうか判定する。交差すると判定した場合は、求めた平面を設置面としたときの物体の姿勢は存在しないため、安定姿勢ではないと判断する。交差しないと判定した場合には、求めた平面を配置面としたときの物体の姿勢を求め、これを安定姿勢とする。

次に、物体の凸形状からエッジを抽出し、抽出したエッジから２つを任意に選択し、これらに接する平面を求め、求めた平面が物体と交差しないどうか判定する。交差すると判定した場合は、求めた平面を設置面としたときの物体の姿勢は存在しないため、安定姿勢ではないと判断する。交差しないと判定した場合には、求めた平面を配置面としたときの物体の姿勢を求め、これを安定姿勢とする。

次に、計測対象物体の凸形状から点を抽出し、抽出した点から３つを任意に選択し、これらに接する平面を求め、求めた平面が計測対象物体と交差しないどうか判定する。交差すると判定した場合は、求めた平面を設置面としたときの物体の姿勢は存在しないため、安定姿勢ではないと判断する。交差しないと判定した場合には、求めた平面を配置面としたときの物体の姿勢を求め、これを安定姿勢とする。

次に、計測対象物体の凸形状から抽出したエッジから１つ及び抽出した点から１つをそれぞれ任意に選択し、選択したエッジと点に接する平面を求め、求めた平面が計測対象物体と交差しないどうか判定する。交差すると判定した場合は、求めた平面を設置面としたときの物体の姿勢は存在しないため、安定姿勢ではないと判断する。交差しないと判定した場合には、求めた平面を設置面としたときの物体の姿勢を求め、これを安定姿勢とする。

求められた安定姿勢において、安定度（倒れにくさ）を求める。物体の設置点が物体の重心位置から遠いものほど設置点での反力が大きくなるため、倒れにくいと考えられる。また、重心位置が水平面に近いほど、物体の微小な姿勢変化によるトルクの変化が小さくなり、倒れにくいと考えられる。この両者を組み合わせた指標として、重心から設置点までのベクトルと重心から水平面に対して垂直な方向に下ろしたベクトルのなす角度のうち、最大となるものを安定度とする。なす角度が大きいほど安定度が高くなる。

図１１は、計測対象物体１１０１を安定姿勢で配置したときの側面図の１例である。この図を用いて安定度の算出方法について説明する。まず、計測対象物体１１０１の重心からエッジまでのベクトルｅ１、ｅ２、及び重心から水平面に垂直な方向に下ろしたベクトルｅ３を算出する。

そして、計測対象物体１１０１の重心からエッジまでのベクトルとｅ１及びｅ２と重心から水平面に垂直な方向に下ろしたベクトルｅ３のなす角θ_１及びθ_２を算出する。θ_１とθ_２の大小を比較し、大きい方を安定度とする。この例では、θ_１＞θ_２となるため、安定度はθ_１となる。なお、上述した方法では、水平面に垂直な軸周りで物体を回転させて配置した時の安定度は、一様となる。

姿勢の存在確率は上述した方法に限らず他の方法で求めてもよい。例えば、計測対象物体１１０１の位置姿勢計測を複数回繰り返し、各計測で得られた姿勢の頻度を存在確率として求めてもよい。

上述した方法で求めた姿勢の存在確率は、事前情報保持部２０２で保持する。

次に補正方法について説明する。ここでは、図１３に示すような計測対象物体１０１を原点に配置した球を設定し、計測対象物体１１０１と撮像装置１０２の相対姿勢を球面上での撮像装置の位置ベクトルとして表現する。

まず、概略位置姿勢に最も近い安定姿勢を選択する。概略位置姿勢から最も近い安定姿勢は、概略位置姿勢におけるカメラの位置ベクトルｐと、各安定姿勢におけるカメラの位置ベクトルｑとの内積ｐ・ｑ最大となるときの姿勢とする。

次に、概略位置姿勢における計測対象物体カメラの位置ベクトルｐの終点と、概略位置姿勢に最も近い安定姿勢におけるカメラの位置ベクトルｑ_ｍの終点とを結ぶ線分ｌ＝ｐ＋α・ｐ・ｑ_ｍ（０≦α≦１）を算出する。

そして、線分ｌ上の１点に撮像装置を設置し、このときの計測対象物体の姿勢を補正後の概略位置姿勢の姿勢とする。線分ｌ上の１点は、最大確率姿勢の存在確率に基づいて決定する。例えば、最大確率姿勢の存在確率が７０％のときは、α＝０。７と設定した時の線分ｌ上の点とする。なお、αを１と設定した場合には、補正後の概略位置姿勢の姿勢は最大確率姿勢と同じになる。

以上のようにして補正した概略位置姿勢を用いて、ステップＳ３０２以降の処理を第１の実施形態と同様に実行する。

（第４の実施形態）
本実施形態に係る画像処理装置の好適な適用事例としては、産業用ロボットアームで対象物体の把持、移動、解放といった動作を行うシステムにおいて対象物体の位置姿勢計測に利用することが挙げられる。以下、図１２を用いて、本発明に係る画像処理装置の適用例を説明する。

図１２は、本発明の第４の実施形態に係る画像処理装置を含む情報処理システムの概略
構成の一例を示す模式図である。１２０１はカメラ及びプロジェクタである。画像処理装置１０４とカメラ及びプロジェクタ１２０１は第１〜３実施形態で説明したシステムと同様のものであり、先に説明した処理手順が記述されたプログラムを格納し、対象物体１２０４の位置姿勢の計測が可能である。

ロボットコントローラ１２０２はロボットコントローラであり、ロボットアーム１２０３を制御する。ロボットアーム１２０３はロボットアームであり、回転軸及び／または並進移動軸からなる可動軸を有する。ロボットコントローラ１２０２は、画像処理装置１０４に接続されており、ロボットアーム１２０３への命令は画像処理装置１０４から受け取った計測対象物体１２０４の位置姿勢を具体的な制御命令に変換してロボットアーム１２０３を動かす。

本実施形態における画像処理装置１０４は上記に加えてさらに次の処理手順を包含する。まず第１〜第３の実施形態で説明したシステム座標系すなわちカメラ座標系とロボットアームで定義されるロボット座標系の相対関係を保持する。画像処理装置１０４は、計測対象物体１２０４の位置姿勢を計測し、その結果をカメラの座標系からロボット座標系に変換する。次に、変換された計測対象物体１２０４のロボット座標系における位置姿勢を元にロボットコントローラ１２０２を介して、ロボットアーム１２０３を対象物体の把持できる位置姿勢に移動する。また、計測対象物体１２０４にロボットアーム１２０３が把持する物体を組み付ける場合には、把持物体及び対象物体のロボット座標系における位置姿勢を元にロボットコントローラ１２０２を介して、ロボットアーム１２０３を組み付け可能な位置姿勢に移動する。

以上によって本実施形態におけるロボットシステムは、計測対象物体の位置姿勢が不定でも位置姿勢の計測を行うことで、計測対象物体の把持や組み付けなどが可能となる。

［第５の実施形態］
画像処理装置１０４を構成する各機能部は何れもハードウェアで構成しても良いが、ソフトウェア（コンピュータプログラム）で構成しても良い。このような場合、このコンピュータプログラムをインストールすると該コンピュータプログラムを実行する装置は、画像処理装置１０４に適用可能である。画像処理装置１０４に適用可能な装置のハードウェア構成例について、図１４のブロック図を用いて説明する。

ＣＰＵ１４０１は、ＲＡＭ１４０２やＲＯＭ１４０３に格納されているコンピュータプログラムやデータを用いて処理を実行することで、本装置全体の動作制御を行うと共に、画像処理装置１０４が行うものとして上述した各処理を実行する。

ＲＡＭ１４０２は、外部記憶装置１４０６からロードされたコンピュータプログラムやデータ、Ｉ／Ｆ（インターフェース）１４０７を介して外部から受信したデータなどを一時的に記憶するためのエリアを有する。更にＲＡＭ１４０２は、ＣＰＵ１４０１が各種の処理を実行する際に用いるワークエリアを有する。即ち、ＲＡＭ１４０２は、各種のエリアを適宜提供することができる。ＲＯＭ１４０３には、本装置の設定データやブートプログラムなどが格納されている。

操作部１４０４は、キーボードやマウスなどにより構成されており、本装置のユーザが操作することで、各種の指示をＣＰＵ１４０１に対して入力することができる。例えば、上記の調整領域の指定は、この操作部１４０４を操作することで指定することができる。

表示部１４０５は、ＣＲＴや液晶画面等により構成されており、ＣＰＵ１４０１による処理結果を画像や文字などでもって表示することができる。

外部記憶装置１４０６は、ハードディスクドライブ装置に代表される、大容量情報記憶装置である。この外部記憶装置１４０６には、ＯＳ（オペレーティングシステム）や、図１、２に示した画像処理装置１０４内の各機能部が行うものとして上述した各処理をＣＰＵ１４０１に実行させるためのコンピュータプログラムやデータが保存されている。このデータには、上記の説明において既知の情報として説明した情報も含まれる。外部記憶装置１４０６に保存されているコンピュータプログラムやデータは、ＣＰＵ１４０１による制御に従って適宜ＲＡＭ１４０２にロードされ、ＣＰＵ１４０１による処理対象となる。

Ｉ／Ｆ１４０７は、本装置が外部機器と通信を行うためのものであり、例えば、このＩ／Ｆ１４０７には、上記の撮像装置１０２、プロジェクタ１０３等を接続することができる。上記の各部は何れも、バス１４０８に接続されている。

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

＜各実施形態の効果＞
第１の実施形態では、概略位置姿勢を補正することで、計測対象物体の影のエッジが検出される場合にも、対応付けの誤りの発生を抑制することで位置姿勢計測の安定性を向上させ、ロバスト性の高い位置姿勢計測が可能となる。

第２の実施形態では、概略位置姿勢を補正することで、計測対象物体上に発生する鏡面反射領域の境界でエッジが検出される場合にも、対応付けの誤りの発生を抑制することで位置姿勢計測の安定性を向上させ、ロバスト性の高い位置姿勢計測が可能となる。

第３の実施形態では、計測対象物体の姿勢として存在する確率の高い姿勢に概略位置姿勢を補正することが可能となる。これにより、対応付けの誤りの発生を抑制することで位置姿勢計測の安定性を向上させ、ロバスト性の高い位置姿勢計測が可能となる。

第４の実施形態ではカメラとプロジェクトをアーム上に設置したロボットシステムにおいて、本発明よる対象物体の位置姿勢の計測結果に基づいてロボットアームを処理することで、位置姿勢が不定の対象物体の把持や組み付けなどの操作を可能にした。

＜定義＞
各実施形態における事前情報は、第１の実施形態で説明したように、光源とカメラとの位置関係情報及び計測対象物体の高さとしてもよい。また、第２の実施形態で説明したように、光源と対象物体との位置関係情報、カメラと計測対象物体との位置関係情報、計測対象物体の形状モデル及び表面特性としてもよい。また、第３の実施形態で説明したように、計測対象物体の姿勢の存在確率としてもよい。概略位置姿勢の補正方向及び補正量を算出可能であれば、いずれの情報であっても構わない。

各実施形態における計測情報は、計測対象物体を撮影した輝度画像としたが、事前情報を用いて補正量または補正方向を決定する場合には、モデルフィッティングに適応可能な情報であればどのようなものであってもよい。例えば、計測対象物体を撮影した距離画像としてもよい。また、計測情報を用いて補正量または補正方向を決定する場合には、モデルフィッティングに適応可能な情報と補正量または補正方向が算出可能な情報があればどのようなものであってもよい。例えば、レーザーレンジセンサなどで計測した三次元点群情報などでも構わない。

各実施形態における補正手段は、第１の実施形態から第３の実施形態で説明したように、事前情報のみ、または計測情報のみ、または事前情報と計測情報の両方に基づいて概略位置姿勢を補正してもよい。また、概略位置姿勢の補正方向または補正量のどちらか一方の値が事前に分かる場合には、どちらか一方については、既知の情報として与えてもよい。

本実施形態における補正手段は、第１の実施形態で説明したように、影の方向及び影の領域情報に基づいて補正方向及び補正量を算出してもよい。また、第２の実施形態で説明したように、鏡面反射の領域情報及び鏡面反射領域と鏡面反射領域近傍に存在する構造的エッジとの位置関係に基づいて補正方向及び補正量を算出してもよい。また、第３の実施形態で説明したように、存在確率の高い位置姿勢に基づいて補正後の概略位置姿勢を算出してもよい。

本実施形態における位置姿勢推定手段は、第１の実施形態では、計測対象物体の３次元の位置姿勢を推定するとして説明したが、２次元の位置姿勢推定であっても構わない。

Claims (19)

1. 対象物体が含まれる画像を取得する画像取得手段と、
   前記対象物体のモデル情報を保持する保持手段と、
   前記画像に含まれる対象物体の概略位置姿勢を取得する取得手段と、
   前記取得された画像中で前記対象物体の影領域に関する情報を推定する推定手段と、
   前記推定された影領域に関する情報に基づいて、前記概略位置姿勢を補正する補正手段と、
   前記補正手段によって補正された概略位置姿勢と前記モデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出手段とを備えることを特徴とする画像処理装置。
2. 前記保持手段は、更に前記対象物体に光を投影する投影手段と前記画像を撮像する撮像手段との位置関係を保持し、
   前記推定手段は、前記保持される位置関係と前記概略位置姿勢とに基づいて前記対象物体の位置に対する影の存在する方向を推定することを特徴とする請求項１に記載の画像処理装置。
3. 前記推定手段は、前記保持するモデル情報から前記対象物体の高さを取得し、該取得した高さと前記位置関係と前記概略位置姿勢とに基づいて前記計測対象物体の影の幅を推定することを特徴とする請求項２に記載の画像処理装置。
4. 前記画像取得手段は、前記計測対象物体を撮影した輝度画像と距離画像のうち、少なくともいずれか一方を取得し、
   前記推定手段は、前記輝度画像または距離画像に基づいて影領域に関する情報を推定することを特徴とする請求項１に記載の画像処理装置。
5. 前記画像取得手段は、前記輝度画像を取得し、該取得した輝度画像中で輝度値が所定の値よりも小さい領域を影領域とすることを特徴とする請求項４に記載の画像処理装置。
6. 前記影領域に関する情報は、前記影領域の大きさを含むことを特徴とする請求項１に記載の画像処理装置。
7. 前記補正手段は、前記取得した影が存在する方向と反対の方向を補正方向とし、前記概略位置姿勢を該補正方向に補正することを特徴とする請求項１乃至６のいずれか１項に記載の画像処理装置。
8. 対象物体が含まれる画像を取得する画像取得手段と、
   前記対象物体のモデル情報を保持する保持手段と、
   前記画像に含まれる対象物体の概略位置姿勢を取得する取得手段と、
   前記取得された画像中で前記対象物体の反射領域を推定する推定手段と、
   前記推定された反射領域に基づいて、前記概略位置姿勢を補正する補正手段と、
   前記補正手段によって補正された概略位置姿勢と前記モデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出手段とを備えることを特徴とする画像処理装置。
9. 前記推定手段は、前記モデル情報に前記対象物体の反射特性を設定し、該設定されたモデルを所定の光源環境でレンダリングすることによって、前記対象物体の反射領域を推定することを特徴とする請求項８に記載の画像処理装置。
10. 前記保持手段は、前記対象物体の形状に起因するエッジの周囲で、前記反射領域を探索し、該探索された反射領域と前記エッジとの位置関係を保持し、
    前記補正手段は、前記反射領域と前記エッジとの位置関係に基づいて、前記概略位置姿勢を補正することを特徴とする請求項８または９に記載の画像処理装置
11. 前記補正手段は、前記保持された反射領域と前記エッジとの位置関係から、前記エッジに対して前記反射領域が存在する方向と反対の方向を補正方向とし、前記概略位置姿勢を前記補正方向に補正することを特徴とする請求項８乃至１０のいずれか１項に記載の画像処理装置。
12. 前記画像取得手段は、前記対象物体を撮影した輝度画像または距離画像のうち、少なくともいずれか一方を取得し、
    前記推定手段は、前記輝度画像または距離画像に基づいて反射領域を推定することを特徴とする請求項８に記載の画像処理装置
13. 対象物体が含まれる画像を取得する画像取得手段と、
    前記対象物体のモデル情報と前記対象物体の取り得る位置姿勢の確率情報と保持する保持手段と、
    前記画像に含まれる対象物体の概略位置姿勢を取得する取得手段と、
    前記補正手段は、前記保持する確率情報に基づいて、前記概略位置姿勢を補正手段と、
    前記補正手段で補正された概略位置姿勢と前記保持手段で保持するモデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出手段とを備えることを特徴とする画像処理装置。
14. 前記モデル情報は、前記対象物体の形状を表すモデルであることを特徴とする請求項1乃至１３のいずれか１項に記載の画像処理装置。
15. 請求項１乃至１４のいずれか１項に記載の画像処理装置と、
    前記位置姿勢推定手段によって位置姿勢が推定された対象物体を把持する把持手段とを備えることを特徴とするロボットシステム。
16. 対象物体が含まれる画像を取得する画像取得工程と、
    前記画像に含まれる対象物体の概略位置姿勢を取得する取得工程と、
    前記取得された画像中で前記対象物体の影領域に関する情報を推定する推定工程と、
    前記推定された影領域に関する情報に基づいて、前記概略位置姿勢を補正する補正工程と、
    前記補正手段によって補正された概略位置姿勢と保持手段で保持する前記対象物体のモデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出工程とを有することを特徴とする画像処理方法。
17. 対象物体が含まれる画像を取得する画像取得工程と、
    前記画像に含まれる対象物体の概略位置姿勢を取得する取得工程と、
    前記取得された画像中で前記対象物体の反射領域を推定する推定工程と、
    前記推定された反射領域に基づいて、前記概略位置姿勢を補正する補正工程と、
    前記補正手段によって補正された概略位置姿勢と保持手段で保持される前記対象物体のモデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出工程とを有することを特徴とする画像処理方法。
18. 対象物体が含まれる画像を取得する画像取得工程と、
    前記画像に含まれる対象物体の概略位置姿勢を取得する取得工程と、
    保持手段で保持する前記対象物体の確率情報に基づいて、前記概略位置姿勢を補正する補正工程と、
    前記補正工程で補正された概略位置姿勢と前記保持手段で保持する前記対象物体のモデル情報とに基づいて、前記画像中の対象物体の位置姿勢を導出する導出工程とを備えることを特徴とする画像処理方法。
19. コンピュータを、請求項１乃至１４の何れか１項に記載の画像処理装置の各手段として機能させるためのコンピュータプログラム。

Images