JP4715539B2 - 画像処理装置、その方法、および画像処理用プログラム - Google Patents

Description

本発明は、画像処理装置、画像処理方法、および画像処理用プログラムに関し、詳細には、三次元物体を認識する画像処理装置、画像処理方法、および画像処理用プログラムに関する。

三次元物体を様々な方向から撮影し、これにより得られた二次元画像に基づいて未知物体の認識を行う三次元物体認識システムが知られている。この三次元物体認識システムは、ロボットの視覚情報の取得や物体認識、車両の自動運転装置、パノラマ画像の作成、および三次元形状の計測等に広く利用されている。この種の三次元物体認識システムとしては、例えば、非特許文献１が公知である。同文献では、画像の特徴量を使用して、画像中から物体を検出する技術が開示されている。

Ｄｉｓｔｉｎｃｔｉｖｅ ｉｍａｇｅ ｆｅａｔｕｒｅｓ ｆｒｏｍ ｓｃａｌｅ ｉｎｖａｒｉａｎｔ Ｋｅｙｐｏｉｎｔｓ，Ｄａｖｉｄ Ｇ，Ｌｏｗｅ，Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，６０，２（２００４），ｐｐ．９１−１１０．

しかしながら、従来技術においては、二次元画像に基づいて、三次元物体を認識する場合に、二次元画像中の未知物体の三次元位置・姿勢を検出していないため、二次元画像中の未知物体を検出して、検出した未知物体を正確な三次元位置・姿勢で復元することができないという問題がある。

本発明は、上記課題に鑑みてなされたものであり、二次元画像に基づいて、三次元物体の三次元位置・姿勢を精度良く推定することが可能な画像処理装置、画像処理方法、および画像処理用プログラムを提供することを目的とする。

上述した課題を解決し、目的を達成するために、本発明は、参照用の物体を異なる複数の視点で各々撮像した登録画像の特徴量と、各登録画像の物体座標上での位置関係とを関連づけて格納したデータベース手段と、認識対象物体を１視点で撮像した１枚の入力画像の特徴量と、前記データベースに格納されている各登録画像の特徴量との相関を各々算出し、相関の高い登録画像をキーフレームとして検出する物体検出手段と、前記データベースを参照して、前記物体検出手段で検出されたキーフレームに対応する位置関係に基づいて、前記入力画像中の物体の三次元位置・姿勢を推定する物体位置・姿勢推定手段と、を備えたことを特徴とする。

また、本発明の好ましい態様によれば、前記特徴量は、テクスチャ情報であり、前記データベースには、前記各登録画像のテクスチャ情報の特徴点として、距離・向きが近い登録画像に対して、拘束条件を満たす特徴点を選択して格納することが望ましい。

また、本発明の好ましい態様によれば、前記参照用の物体のプリミティブモデルを定義して、前記データベースに格納するプリミティブモデル生成手段と、前記物体位置・姿勢推定手段で推定した物体の三次元位置・姿勢に基づいて、前記データベースに格納されている対応する参照用の物体のプリミティブモデルを復元して物体の把持部を推定する把持部推定手段と、を備えたことが望ましい。

また、本発明の好ましい態様によれば、前記プリミティブモデル生成手段は、前記参照用の物体のプリミティブモデルが対応する登録画像の領域を包含するように、当該プリミティブモデルを修正することが望ましい。

また、本発明の好ましい態様によれば、物体を撮像して画像を取得する撮像手段と、前記参照用の物体の位置・姿勢を変更させる物体位置・姿勢変更手段と、を備え、物体位置・姿勢変更手段で前記参照用の物体の位置・姿勢を変更しながら、前記撮像手段で、当該参照用の物体を撮像して、前記登録画像を入力することが望ましい。

また、本発明の好ましい態様によれば、前記物体検出手段は、未知物体を複数の視点で撮像した複数の入力画像毎に、前記データベースに格納されている各登録画像との特徴点の合致度を算出して、それぞれ合致度が最も高いキーフレームを検出し、各視点毎に、当該視点の入力画像と他の視点の入力画像との位置関係をそれぞれ取得し、前記キーフレームと前記位置関係に近い登録画像を前記データベースから選択し、各視点毎に、当該視点の入力画像と前記キーフレーム、および他の視点の入力画像と選択した登録画像の組み合わせを作成し、当該他の視点の入力画像と選択した登録画像の特徴点の合致度を算出し、
各視点毎の前記組み合わせにおいて、前記合致度の平均値が高い組み合わせのキーフレームを最終的なキーフレームとして選択することが望ましい。

また、本発明の好ましい態様によれば、物体位置・姿勢推定手段は、前記合致度の平均値が高い組み合わせにおいて、ロバスト推定を利用して、位置・姿勢推定時に登録画像を使用する頻度を変えることが望ましい。

また、本発明の好ましい態様によれば、前記特徴量は、テクスチャ情報および輪郭情報であり、前記物体位置・姿勢推定手段は、前記データベースに格納されている登録画像のテクスチャ情報および輪郭情報の両者を使用して、前記入力画像の物体の三次元位置・姿勢推定を行うことが望ましい。

また、本発明の好ましい態様によれば、前記物体位置・姿勢推定手段は、前記入力画像に対して特徴点の合致度が高い順に、その登録画像の前記位置関係に基づいて前記入力画像の物体の三次元位置・姿勢推定を行い、物体の三次元形状を入力画像に投影して輪郭線を抽出し、前記輪郭線と前記入力画像のエッジ線とに基づいて、前記物体の三次元位置・姿勢推定が正しか否かを判断することが望ましい。

また、本発明の好ましい態様によれば、前記物体位置・姿勢推定手段は、前記キーフレームの特徴点の三次元的なバラツキの平面度合いが閾値を超える場合に、前記物体の三次元位置・姿勢推定に使用することが望ましい。

また、本発明の好ましい態様によれば、検出した物体の周囲の三次元の凹凸構造を認識する三次元復元手段を備えたことが望ましい。

また、本発明の好ましい態様によれば、前記三次元復元手段は、位置・姿勢推定手段で推定した三次元位置・姿勢に基づいた物体の三次元形状を入力画像に投影し、入力画像上での物体の領域を算出し、前記入力画像の前記算出した領域内の画像を第１の画像として抽出し、前記登録画像内の前記物体の領域に含まれる画像を、前記入力画像の視点からみた第２の画像に変換し、前記第１の画像と前記第２の画像との差分が閾値以下の領域を前記物体の領域として抽出することが望ましい。

また、本発明の好ましい態様によれば、物体を撮像して画像を取得する複数の撮像手段と、前記複数の撮像手段の位置・姿勢を調整する位置・姿勢変更手段と、前記参照用の物体の位置・姿勢を変更させる物体位置・姿勢変更手段と、前記物体位置・姿勢変更手段で前記参照用の物体の位置・姿勢を変更しながら、前記複数の撮像手段で当該参照用の物体を撮像した画像と、前記データベースに格納されている対応する登録画像とを比較して、前記位置・姿勢変更手段のキャリブレーションを行うキャリブレーション手段を備えたことが望ましい。

上記した課題を解決して、本発明の目的を達成するために、本発明は、参照用の物体を異なる複数の視点で各々撮像した登録画像の特徴量と、各登録画像の物体座標上での位置関係とを関連づけてデータベースに格納する工程と、認識対象物体を１視点で撮像した１枚の入力画像の特徴量と、前記データベースに格納されている各登録画像の特徴量との相関を各々算出し、相関の高い登録画像をキーフレームとして検出する物体検出工程と、前記データベースを参照して、前記検出されたキーフレームに対応する位置関係に基づいて、前記入力画像中の物体の三次元位置・姿勢を推定する物体位置・姿勢推定工程と、を含むことを特徴とする。

また、本発明の好ましい態様によれば、前記特徴量は、テクスチャ情報であり、前記データベースには、前記各登録画像のテクスチャ情報の特徴点として、距離・向きが近い登録画像に対して、拘束条件を満たす特徴点を選択して格納することが望ましい。

また、本発明の好ましい態様によれば、前記参照用の物体のプリミティブモデルを定義して、前記データベースに格納するプリミティブモデル生成工程と、前記物体位置・姿勢推定手工程で推定した物体の三次元位置・姿勢に基づいて、前記データベースに格納されている対応する参照用の物体のプリミティブモデルを復元して物体の把持部を推定する把持部推定工程と、を含むことが望ましい。

また、本発明の好ましい態様によれば、前記プリミティブモデル生成工程では、前記参照用の物体のプリミティブモデルが対応する登録画像の領域を包含するように、当該プリミティブモデルを修正することが望ましい。

また、本発明の好ましい態様によれば、物体位置・姿勢変更手段で前記参照用の物体の位置・姿勢を変更しながら、撮像手段で、当該参照用の物体を撮像して、前記登録画像を入力することが望ましい。

また、本発明の好ましい態様によれば、前記物体検出工程では、未知物体を複数の視点で撮像した複数の入力画像毎に、前記データベースに格納されている各登録画像との特徴点の合致度を算出して、それぞれ合致度が最も高いキーフレームを検出し、各視点毎に、当該視点の入力画像と他の視点の入力画像との位置関係をそれぞれ取得し、前記キーフレームと前記位置関係に近い登録画像を前記データベースから選択し、各視点毎に、当該視点の入力画像と前記キーフレーム、および他の視点の入力画像と選択した登録画像の組み合わせを作成し、当該他の視点の入力画像と選択した登録画像の特徴点の合致度を算出し、各視点毎の前記組み合わせにおいて、前記合致度の平均値が高い組み合わせのキーフレームを最終的なキーフレームとして選択することが望ましい。

また、本発明の好ましい態様によれば、物体位置・姿勢推定工程では、前記合致度の平均値が高い組み合わせにおいて、ロバスト推定を利用して、位置・姿勢推定時に登録画像を使用する頻度を変えることが望ましい。

また、本発明の好ましい態様によれば、前記特徴量は、テクスチャ情報および輪郭情報であり、前記物体位置・姿勢推定工程では、前記データベースに格納されている登録画像のテクスチャ情報および輪郭情報の両者を使用して、前記入力画像の物体の三次元位置・姿勢推定を行うことが望ましい。

また、本発明の好ましい態様によれば、前記物体位置・姿勢推定工程では、前記入力画像に対して特徴点の合致度が高い順に、その登録画像の前記位置関係に基づいて前記入力画像の物体の三次元位置・姿勢推定を行い、物体の三次元形状を入力画像に投影して輪郭線を抽出し、前記輪郭線と前記入力画像のエッジ線とに基づいて、前記物体の三次元位置・姿勢推定が正しか否かを判断することが望ましい。

また、本発明の好ましい態様によれば、前記物体位置・姿勢推定工程は、前記キーフレームの特徴点の三次元的なバラツキの平面度合いが閾値を超える場合に、前記物体の三次元位置・姿勢推定に使用することが望ましい。

また、本発明の好ましい態様によれば、検出した物体の周囲の三次元の凹凸構造を認識する三次元復元工程を含むことが望ましい。

また、本発明の好ましい態様によれば、前記三次元復元工程では、位置・姿勢推定工程で推定した三次元位置・姿勢に基づいた物体の三次元形状を入力画像に投影し、入力画像上での物体の領域を算出し、前記入力画像の前記算出した領域内の画像を第１の画像として抽出し、前記登録画像内の前記物体の領域に含まれる画像を、前記入力画像の視点からみた第２の画像に変換し、前記第１の画像と前記第２の画像との差分が閾値以下の領域を前記物体の領域として抽出することが望ましい。

また、本発明の好ましい態様によれば、物体位置・姿勢変更手段で参照用の物体の位置・姿勢を変更しながら、複数の撮像手段で当該参照用の物体を撮像した画像と、前記データベースに格納されている対応する登録画像とを比較して、前記位置・姿勢変更手段のキャリブレーションを行うキャリブレーション工程を含むことが望ましい。

また、本発明の好ましい態様によれば、本発明の画像処理方法の各工程をコンピュータに実行させることが望ましい。

本発明によれば、参照用の物体を異なる複数の視点で各々撮像した登録画像の特徴量と、各登録画像の物体座標上での位置関係とを関連づけてデータベースに登録し、物体検出手段は、認識対象物体を撮像した入力画像の特徴量と、前記データベースに格納されている各登録画像の特徴量との相関を各々算出し、相関の高い登録画像をキーフレームとして検出し、物体位置・姿勢推定手段は、前記データベースを参照して、前記物体検出手段で検出されたキーフレームに対応する位置関係に基づいて、前記入力画像中の物体の三次元位置・姿勢を推定することとしたので、二次元画像に基づいて、三次元物体の三次元位置・姿勢を精度良く推定することが可能な画像処理装置、画像処理方法、および画像処理用プログラムを提供することが可能になるという効果を奏する。

以下に、この発明に係る画像処理方法、画像処理装置、および画像処理用プログラムについて、図面を参照しつつ詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。また、下記実施の形態における構成要素には、当業者が容易に想定できるものまたは実質的に同一のものが含まれる。本発明に係る画像処理装置は、各種システムに利用可能であるが、以下では、主として、ロボットの視覚情報の取得や物体認識に適用した場合を例示して説明する。

［画像処理装置］
図１−１は、本発明に係る画像処理装置の基本構成を示す図である。画像処理装置１は、図１−１に示すように、物体を撮像して画像を取得する画像取得部１０と、画像取得部１０で撮像された画像に基づいて、物体の三次元形状を検出する画像処理部２０と、レーザやカメラ等を使用して参照用の物体の三次元形状を取得する三次元形状取得部３０と、参照用の物体の登録画像の特徴量およびプリミティブモデル等を格納するデータベース登録部４０とを備えている。

画像取得部１０は、１または複数のデジタルカメラ（以下「カメラ」と称する）を備えており、物体を撮像して電気信号に変換して画像を取得する。画像取得部１０は、データベースに参照用の物体の情報を登録する場合には、Ｎ（Ｎは２以上の整数）視点で、参照用の物体の周囲から全周に亘り、異なる高さ位置から撮像し、撮像したＮ個の画像をデータベース登録部４０に出力する。

画像取得部１０は、Ｎ（Ｎは２以上の整数）視点毎にカメラ（ステレオカメラ）を設けて、同一物体を撮像する構成、１つのカメラの位置・姿勢を変更してＮ視点で同一物体を撮像する構成、および１つのカメラを固定し、物体の位置・姿勢を変更しながらＮ視点で撮像する構成のいずれでもよい。また、画像取得部１０は、物体の位置・姿勢を変更する位置・姿勢変更手段としてロボット・アームを備えることにしてもよく、また、カメラの位置・姿勢を調整する位置・姿勢調整手段としてロボット・アームを備えることにしてもよい。

画像データベース登録部４０は、特徴点・特徴量抽出部４１と、特徴点の三次元位置推定部４２と、プリミティブモデル生成部４３とを備えている。データベース登録部４０には、画像取得部１０から参照用の物体をその周囲から全周に亘り、異なる高さ位置からＮ視点で撮像されたＮ個の画像が入力される。

特徴点・特徴量抽出部４１は、入力されるＮ個の画像毎に特徴量・特徴点を抽出して、データベース格納部４４に出力する。具体的には、特徴点・特徴量抽出部４１は、テクスチャ情報として、特徴量（ＳＩＦＴなど）および各特徴量を持つ点（特徴点）の物体座標での座標、および輪郭情報として、特徴量（Ｐｅｄａｌ Ｃｕｒｖｅ）や物体の表面を構成する点またはパッチの三次元座標を検出する。

特徴点の三次元位置推定部４２は、物体座標とＮ個の画像の画像座標間の運動パラメータ（回転パラメータＲ、並進パラメータｔ）を算出して、データベース格納部４４に出力する。

プリミティブモデル生成部４３は、三次元形状取得部３０から入力される参照用の物体の三次元形状に基づいて、参照用の物体のプリミティブモデルを作成してデータベース格納部４４に出力する。プリミティブモデルの作成は、手作業入力または自動実行のいずれでもよい。

データベース格納部４４は、データベースを格納する。図１−２はデータベースの概念図、図１−３はデータベースの構成例を示す図である。図１−２に示すように、データベースには、参照用の物体の周囲から全周に亘り、異なる高さ位置から撮像した各登録画像の特徴量（テクスチャ情報、輪郭情報）および運動パラメータ、並びに参照用の物体のプリミティブモデルが格納される。データベース格納部４４は、参照用の物体毎に物体番号を付加し、また、各参照用の物体を撮像したＮ個の画像に登録番号を付加して、登録画像１〜登録画像Ｎとして管理する。データベース格納部４４は、データベースに、参照用の物体１〜Ｍ毎に登録画像１〜登録画像Ｎおよびプリミティブモデルを格納し、また、各登録画像１〜Ｎに、テクスチャ情報（特徴量（ＳＩＦＴなど）および各特徴量を持つ点（特徴点）の物体座標での座標）、輪郭情報（特徴量（Ｐｅｄａｌ Ｃｕｒｖｅ）および物体の表面を構成する点またはパッチの三次元座標）、並びに、物体座標と登録画像座標間の運動パラメータ（回転パラメータＲ、並進パラメータｔ）を関連づけて格納する。

画像処理部２０は、上記データベースを使用して、未知物体を撮像した認識対象画像から物体の検出を行うものであり、特徴点・特徴量抽出部２１と、特徴量マッチング部２２と、位置・姿勢推定部２３と、三次元復元部２４と、把持部推定部２５とを備えている。

画像処理部２０には、画像取得部１０で未知物体を撮像した認識対象画像が入力される。特徴点・特徴量抽出部２１は、認識対象画像から特徴量（テクスチャ情報および／または輪郭情報）とその特徴点を検出する。特徴量マッチング部２２は、認識対象画像の特徴点と、データベースに格納されている各物体１〜Ｍの登録画像１〜Ｎの特徴点との合致度をそれぞれ算出した後、合致度の高い順にソーティングし、合致度の高い登録画像をキーフレームとして検出する。これにより、このキーフレームに対応する参照用の物体が認識対象画像中に含まれていることを検出することができる。なお、データベースで検索対象の物体を指定可能な構成としてもよい。これにより、高速にキーフレームを検出することが可能となる。

位置・姿勢推定部２３は、特徴点・特徴量抽出部２１で検出したキーフレームの運動パラメータ（回転パラメータＲ、並進パラメータｔ）に基づいて、検出した物体の三次元位置・姿勢を推定する。把持部推定部２５は、検出した物体の三次元位置・姿勢に基づいて、データベースに格納されている対応する参照用の物体のプリミティブモデルを復元して物体の把持部を推定する。三次元復元部２４は、検出した物体の周囲の三次元の凹凸構造を認識する。

［画像処理装置の全体動作の概略］
図１−４は、画像処理装置１で未知物体を検出する場合の全体動作の概略を説明するためのフローチャートである。図１−４において、まず、画像処理部２０では、画像取得部１０から認識対象画像（以下「入力画像」と称する）が入力される（ステップＳ１）。

つぎに、入力画像中の物体の検出処理が行われる（ステップＳ２）。具体的には、特徴点・特徴量抽出部２１は、入力画像から特徴量（テクスチャ情報および／または輪郭情報）・その特徴点を検出する。特徴量マッチング部２２は、入力画像の特徴点と、データ画像データベース３の各物体の登録画像１〜Ｎの特徴点との合致度をそれぞれ算出した後、合致度の高い順にソーティングし、合致度の高い登録画像をキーフレームとして検出する。

位置・姿勢推定部２３は、キーフレーム（登録画像）の運動パラメータ（回転パラメータＲ、並進パラメータｔ）に基づいて、入力画像中の物体の三次元位置・姿勢を推定する（ステップＳ３）。把持部推定部２５は、検出した物体の三次元位置・姿勢に基づいて、データベースに格納されている対応する参照用の物体のプリミティブモデルを復元して物体の把持部を推定する（ステップＳ４）。なお、把持部がない物体を検出した場合には、把持部の決定は行わない。この物体の把持部の認識は、ロボットで物体を把持する制御を行う場合に有効である。三次元復元部２４は、検出した物体の周囲の三次元の凹凸構造を認識する（ステップＳ５）。

画像処理装置１をロボットシステムに適用した場合は、この後、ハンドの把持姿勢を決定し、アームの経路を計画し、ハンド・アームの把持制御を開始する。

なお、ステップＳ５とステップＳ６を実行する順序は、この順に限られるものではなく、ステップＳ６とステップＳ５の実行順序を逆にしてもよい。

上記構成の画像処理装置１では、参照用の物体を異なる複数の視点で各々撮像した登録画像の特徴量と、各登録画像の物体座標上での位置関係とを関連づけてデータベースに格納しておき、特徴点・特徴量抽出部２１および特徴量マッチング部２２は、認識対象物体を撮像した入力画像の特徴量と、前記データベースに格納されている各登録画像の特徴量との相関を各々算出し、相関の高い登録画像をキーフレームとして検出し、位置・姿勢推定部２３は、データベースを参照して、検出されたキーフレームに対応する位置関係に基づいて、入力画像中の物体の三次元位置・姿勢を推定することとしたので、簡単かつ低コストな構成で入力画像中の物体の三次元位置・姿勢を高精度に推定することが可能となる。

また、データベースでは、登録画像の特徴量として、テクスチャ情報および輪郭情報を登録することとしたので、後述するように、物体の模様の有無にかかわらず、物体の検出が可能となる。また、これらの２つの特徴量を組み合わせることで精度の高い物体の検出が可能となる。

また、データベースの登録画像と、物体の三次元形状の位置関係（運動パラメータ（回転パラメータ、並進パラメータ））を定義しておくことで、物体の検出と三次元位置・姿勢推定とを同時に行うことが可能となる。

また、参照用の物体のプリミティブモデルを定義して、データベースに格納しておき、
把持部推定部２５は、位置・姿勢推定部２３で推定した物体の三次元位置・姿勢に基づいて、データベースに格納されている対応する参照用の物体のプリミティブモデルを復元して物体の把持部を推定することとしたので、物体の詳細な三次元形状を使わずに、物体のプリミティブモデルを使うことで、物体の把持部の推定を高速に行うことが可能となる。また、物体の詳細な三次元形状を使わずに、プリミティブモデルを使うことで、後処理での周辺の三次元の凹凸構造との組み合わせを簡単に行うことが可能となる。

なお、上記画像処理装置１では、データベース作成用と物体検出用で同じカメラを使用する構成であるが、異なるカメラを使用することにしてもよい。また、検出対象画像の入力は、画像取得部１０で撮像して入力する場合に限られるものではなく、外部から送信される検出対象画像を入力することにしてもよい。

上記構成の画像処理装置１の実施例１〜１１を詳細に説明する。

［データベース］
（実施例１）
実施例１は、特徴点・特徴量抽出部４１が、登録画像の特徴点（テクスチャ情報）を検出する場合に、三次元物体の高精度な三次元位置・姿勢推定のために、有効な特徴点だけを抽出するものである。

図２−１は、特徴量検出部１１の特徴点（テクスチャ情報）を検出する場合の処理を説明するためのフローチャートである。図２−２は、登録画像ｉと登録画像ｊの距離・向きｄ＊（１−ＣＯＳθ）を説明するための図である。登録画像間の距離・向きｄ＊（１−ＣＯＳθ）は、各登録画像を撮像するときのカメラの撮像条件（位置、姿勢、撮影距離等）で定まるため、予めデータベースに各登録画像間の距離・向きｄ＊（１−ＣＯＳθ）の情報を格納しておくものとする。図２−３は、登録画像ｉと距離・向きｄ＊（１−ＣＯＳθ）が近い登録画像を示す概念図である。図２−４は、特徴点の選択を説明するための説明図である。

図２−１において、まず、特徴点・特徴量抽出部２１は、ｉ＝１を設定し（ステップＳ１０１）、データベースから登録画像ｉを選択する（ステップＳ１０２）。次に、特徴点・特徴量抽出部２１は、データベースを参照して、登録画像ｉに距離・向きｄ＊（１−ＣＯＳθ）が近い順に、登録画像ｊを選択する（ステップＳ１０３）。

特徴点・特徴量抽出部２１は、登録画像ｉ上の各特徴点に対応する特徴点を登録画像ｊ上で探索する（ステップＳ１０４）。この後、特徴点・特徴量抽出部２１は、登録画像ｉ上で、各対応点のうち、エピポーラ拘束条件を満たす点だけを選択する（ステップＳ１０５）。図２−４に示す例では、登録画像ｉ内の特徴点Ａ、Ｂに対応する登録画像ｊ内の特徴点Ａ’、Ｂ’のうち、Ａ’は、拘束条件式上にあるので特徴点Ａを選択し、Ｂ’は、拘束条件式上にないので特徴点Ｂは選択しない。

特徴点・特徴量抽出部２１は、選択された特徴点の累積数が閾値を超えたか否かを判断し（ステップＳ１０６）、閾値を超えていない場合には（ステップＳ１０６の「Ｎｏ」）、ステップＳ１０３に戻り、登録画像ｉに距離・向きｄ＊（１−ＣＯＳθ）が次に近い登録画像ｊを選択して同様な処理を行う。他方、閾値を超えた場合には（ステップＳ１０６の「Ｙｅｓ」）、ｉ＝Ｎ（画像総枚数）であるか否かを判断し（ステップＳ１０７）、ｉ＝Ｎではない場合には（ステップＳ１０７の「Ｎｏ」）、ｉ＝ｉ＋１として（ステップＳ１０８）、ステップＳ１０２に戻り、次の登録画像ｉについて特徴点を抽出する処理を行い、ｉ＝Ｎの場合には（ステップＳ１０７の「Ｙｅｓ」）、当該処理を終了する。この特徴点・特徴量抽出部２１で抽出された各登録画像の特徴点はデータベースに格納される。

実施例１によれば、データベースには、各登録画像のテクスチャ情報の特徴点として、距離・向きが近い登録画像に対して、エピポーラ拘束条件を満たす特徴点を選択して格納することとしたので、再現性の高い特徴点（＝物体をみる視点が多少変わっても抽出される特徴点）だけをデータベースに登録でき、データベースの登録画像と見え方の異なる物体を検出する際に、特徴点の対応の検出精度を向上させることが可能となる。また、特徴点の数を低減できるので、特徴点の対応の探索速度を速くすることが可能となる。

（実施例２）
実施例２は、プリミティブモデル生成部４３が、物体の座標と、データベースに登録される各登録画像との位置関係の誤差を吸収できるように、プリミティブモデルを修正するものである。

例えば、ロボットハンドで物体の把持部を把持する場合に、物体を小さく認識してしまうと物体の把持部を把持できない場合がある。実際にロボットハンドで物体を把持する際には、少し大きめの物体を把持するような制御（ハンドの接触センサー等の情報を利用）することが多いので、物体を少し大きめに認識できるように、プリミティブモデルを修正する。

図３は、プリミティブモデル生成部４３が、プリミティブモデルを修正する処理を説明するための説明図である。参照用の物体の座標と、データベースの登録画像の位置関係に誤差があると、プリミティブモデルによる投影像と物体の領域（輪郭）との間にずれが生じる。

図３では、プリミティブモデルの修正前と修正後を示している。図３に示す例では、３つの登録画像を選択した場合を示している。同図において、データベースの登録画像の複数または全てを選択し、選択した登録画像に対応する回転パラメータに従った三次元位置・姿勢のプリミティブモデルの投映像Ｐを形成する。そして、プリミティブモデルの投映像Ｐｒに、登録画像の領域（輪郭）Ｏを投影する。修正前の例では、登録画像の領域（輪郭）Ｏが、プリミティブモデルの投映像Ｐをはみ出している。そこで、修正後の例に示すように、プリミティブモデルの投影像Ｐが登録画像の領域（輪郭）Ｏを包含するように、プリミティブモデルを修正する。

なお、演算量が増えるが、三次元位置・姿勢推定した後、その際に選ばれたデータベースの登録画像（キーフレーム）分だけに同様の処理を行い、プリミティブモデルを調整してもよい。また、プリミティブモデルの大きさの調整だけでなく、傾きの調整を同時に行うことにしてもよい。

実施例２によれば、プリミティブモデル生成部４３は、参照用の物体のプリミティブモデルが対応する登録画像の領域を包含するように、プリミティブモデルを修正することとしたので、ロボットハンドで物体の把持部を把持する場合に、物体を小さく認識して把持部を把持できないという状態を防止することが可能となる。実際にロボットハンドで把持する際には、少し大きめの物体を把持するような制御（ハンドの接触センサー等の情報を利用）することが多いので、三次元位置・姿勢推定精度に多少誤差を含んでいても把持が可能になる。

（実施例３）
実施例３は、ロボットのハンド・アームで、参照用の物体を把持して、参照用の物体の位置・姿勢を変更し、様々な角度から画像を取得して三次元形状と特徴点（輪郭）を取得して、データベースを自動で取得するものである。

実施例３では、画像取得部１０は、ハンド・アーム（物体位置・姿勢変更手段）を備えており、ハンド・アームは、参照用の物体を把持してその位置姿勢を変更可能な構成となっているものとする。

図４−１は、ロボットのハンド・アームで物体の位置姿勢を変更しながら画像を取得する場合を説明するためのフローチャートである。図４−２は、ロボットのハンド・アームで参照用の物体の位置姿勢を変更しながら画像を取得する場合を説明するための説明図である。同図において、Ａｒはハンド・アームを示しており、Ｈｉはハンド・アームＡｒの輪郭、Ｏｂは参照用の物体を示しており、Ｏｉは物体Ｏｂの輪郭を示している。

図４−１において、まず、視点ｉで画像ｉを取得する（ステップＳ１２１）。制御信号とハンド・アームの３Ｄデータから、ハンド・アームを画像ｉに投影する（ステップＳ１２２）。この後、ハンド・アームの輪郭Ｈｉを抽出する（ステップＳ１２３）。Ｇｒａｐｈ Ｃｕｔ法などにより物体Ａの輪郭Ｏｉ抽出する（ステップＳ１２４）。物体の輪郭のうち、ハンド・アームの輪郭と接している輪郭を除去し、Ｏｉ’を得る（ステップＳ１２５）。これらの処理（ステップＳ１２１〜Ｓ１２５）を視点１〜Ｎについて実行する。

Ｏｉ’（ｉ＝１，．．．，Ｎ、Ｎ：視点数）を使って、Ｓｈａｐｅ ｆｒｏｍ ｓｉｌｈｏｕｅｔｔｅ法で三次元形状を復元する（ステップＳ１２６）。この後、画像ｉ（ｉ＝１，．．．，Ｎ）から特徴点を抽出する。そして、ステレオ視等により三次元座標を計算する（ステップＳ１２７）。

実施例３によれば、ハンド・アームで参照用の物体の位置・姿勢を変更しながら、カメラで、参照用の物体を撮像して、データベースを作成することとしたので、ロボットが未知の物体のデータベースを自動で構築でき、自ら学習していくことが可能となる。また、三次元形状が欠落している部分をロボットが自分で認識することができ、物体を持ち直して視点を変更して観察するなど、より高精度なデータベースの構築が可能となる。

［物体検出］
（実施例４）
実施例４は、１枚の入力画像から未知物体を検出するものである。

図５は、１枚の入力画像から未知物体を検出する処理を説明するためもフローチャートである。実施例１では、特徴点・特徴量抽出部２１には、画像取得部１０から１枚の入力画像が入力される。

図５において、特徴点・特徴量抽出部２１は、画像取得部１０から認識対象の入力画像が入力されると、この入力画像の特徴点を抽出する（ステップＳ１３１）。特徴量マッチング部２２は、ｌ＝１を設定し（ステップＳ１３２）、データベース４４から物体ｌを選択する（ステップＳ１３３）。また、特徴量マッチング部２２は、ｉ＝１を設定し（ステップＳ１３４）、データベース４４の物体ｌの登録画像ｉを選択する（ステップＳ１３５）。特徴量マッチング部２２は、入力画像と登録画像ｉの特徴点の合致度Ｅｉを算出する（ステップＳ１３６）。この後、ｉ＝Ｎ（登録画像総数）であるか否かを判断し（ステップＳ１３７）、ｉ＝Ｎではない場合には（ステップＳ１３７の「Ｎｏ」）、ｉ＝ｉ＋１として（ステップＳ１４０）、ステップＳ１３３に戻り、次の登録画像との特徴点の合致度Ｅｉを算出し、物体ｌの登録画像の全てが終了するまで処理を実行する。

ｉ＝Ｎの場合には（ステップＳ１３７の「Ｙｅｓ」）、ｌ＝Ｍ（登録物体総数）であるか否かを判断し（ステップＳ１３８）、ｌ＝Ｍではない場合には（ステップＳ１３８の「Ｎｏ」）、ｌ＝ｌ＋１として（ステップＳ１４１）、ステップＳ１３３に戻り、データベースの次の物体ｌについての登録画像との特徴点の合致度Ｅｉを算出し、全ての物体ｌが終了するまで同様の処理を行う。

ｉ＝Ｍ（登録物体総数）の場合には（ステップＳ１３８の「Ｙｅｓ」）、合致度Ｅｉが高い順に登録画像をソーティングして、合致度Ｅｉが最も高い登録画像をキーフレームとする（ステップＳ１３９）。これにより、入力画像中の物体を検出することができる。

なお、入力画像中に複数の物体が含まれている場合には、各物体毎にキーフレームの検出を行う。この場合、入力画像中で物体領域を検出した後に、各物体領域毎に物体検出を行う。

実施例４によれば、１枚の入力画像から未知物体を検出することとしたので、高速かつ簡易に未知物体を検出することが可能となる。

（実施例５）
実施例５は、同一未知物体を異なる角度で撮像した複数枚の入力画像を使用して、キーフレームの検出精度を向上させるものである。実施例５では、画像取得部１０は、異なる位置に配置された複数のカメラを備えており、各カメラで未知物体を撮像した入力画像を特徴点・特徴量抽出部２１に出力する。

図６−１は、同一未知物体を異なる角度で撮像した複数枚の入力画像に基づいて物体を検出する処理を説明するためのフローチャートである。図６−２は、入力画像と対応する登録画像の関係を説明するための図である。図６−３は、同一未知物体を異なる角度で撮像した複数枚の入力画像に基づいて物体を検出する処理を説明するための説明図である。

図６−１において、特徴点・特徴量抽出部２１には、画像取得部１０のカメラ１〜Ｎで未知物体を撮像した入力画像が入力される。特徴点・特徴量抽出部２１は、カメラｉで撮像した入力画像の特徴点を抽出し、特徴量マッチング部２２は、入力画像とデータベースの全登録画像１〜Ｎとの特徴点の合致度を算出し、そのうち最も合致度が高い登録画像ｉをカメラｉのキーフレームとして選択し、そのときの合致度をＥｉｉとする（ステップＳ１５１）。

カメラｉの入力画像ｉと他のカメラｊの入力画像ｊとの位置関係Ｒ、ｔを取得し、カメラｉのキーフレーム（登録画像）との位置関係Ｒ、ｔに近い登録画像ｉｊをデータベースから選択し、他のカメラｊの入力画像ｊと登録画像ｉｊの特徴点の合致度Ｅｉｊをそれぞれ算出する（ステップＳ１５２）。例えば、カメラが５つの場合は、他の４つのカメラの入力画像ｊについての合致度Ｅｉｊがそれぞれ算出される。入力画像間の位置関係Ｒ、ｔは、各カメラの位置・姿勢で定まり、各カメラの位置・姿勢は固定条件であるので、予めカメラｉの入力画像ｉと他のカメラｊの入力画像ｊとの位置関係Ｒ、ｔを保持しておくことができる。

カメラｉのキーフレーム（登録画像）との位置関係が、カメラｉの入力画像ｉと他のカメラｊの入力画像ｊとの位置関係Ｒ、ｔに近い登録画像iｊは、他のカメラｊの入力画像ｊとの特徴点の合致度が最も高くなると推測できる。図６−３は、カメラが５個の場合に、入力画像ｉと最も対応する（合致度が高い）登録画像ｉ（キーフレーム）と、他の４つの入力画像ｊにそれぞれ最も対応する登録画像iｊを概念的に示している。

このようにして、カメラｉについて、入力画像−登録画像の組み合わせ（Ｎ個の合致度Ｅの組み合わせ）ができる。カメラｉについてのＮ個の合致度Ｅの平均値Ｅｉ＝ΣｊＥｉｊを算出する（ステップＳ１５３）。この処理を、全てのカメラ１〜Ｎについて実行する（ステップＳ１５５、Ｓ１５１，１５２，Ｓ１５３）。

そして、各カメラについてのＮ個の合致度の平均値Ｅｉが最大となる組み合わせのキーフレームを最終的なキーフレームとして選択とする（ステップＳ１５４）。図６−２は、各カメラ１〜Ｎについての入力画像−登録画像の組み合わせとその合致度の平均値Ｅｉを示す図である。同図において、例えば、カメラ１の入力画像−登録画像の組み合わせの合致度の平均値Ｅ１が最大となる場合には、カメラ１の入力画像１に対するキーフレームが最終的なキーフレームとして選択される。

なお、実施例５では、複数個のカメラを備える構成としたが、１つのカメラを移動させて複数の視点で撮像することにしてもよい。

以上説明したように、実施例５によれば、未知物体を複数のカメラで撮像した複数の入力画像毎に、データベースに格納されている各登録画像との特徴点の合致度を算出して、それぞれ合致度が最も高いキーフレームを検出し、各カメラに、当該カメラの入力画像と他のカメラの入力画像との位置関係をそれぞれ取得し、キーフレームと位置関係に近い登録画像をデータベースから選択し、各カメラ毎に、当該カメラの入力画像とキーフレーム（登録画像）、および他のカメラの入力画像と選択した登録画像の組み合わせを作成し、当該他のカメラの入力画像と選択した登録画像の特徴点の合致度を算出し、各カメラ毎の組み合わせにおいて、合致度の平均値が高い組み合わせのキーフレーム（登録画像）を最終的なキーフレーム（登録画像）として選択することとしたので、物体の向きに近いデータベースの登録画像を複数のカメラの位置関係を利用して高精度に選択することができ、仮に、一部の入力画像のキーフレームを誤って選択しても、他の入力画像のキーフレームから正しいキーフレームを選択し直すことが可能となる。これにより、入力画像を撮影したカメラ間の位置関係と整合のとれたキーフレームがそれぞれ選択でき、三次元位置・姿勢推定精度を向上させることが可能となる。

［物体の三次元位置・姿勢推定］
（実施例６）
実施例６は、実施例５で決定した最適な入力画像−登録画像の組み合わせにおいて、ＲＡＮＳＡＣ（ランダムサンプルコンセンサス）法を使用してロバスト推定し、三次元位置・姿勢推定時に登録画像を使用する頻度を変えることにより、三次元位置・姿勢推定精度を向上させるものである。

図７−１は、実施例６を説明するためのフローチャートである。図７−２は、実施例６を説明するための説明図である。図７−２は、実施例５において、未知物体を撮像するカメラ数Ｎ＝３とした場合の最適な入力画像−登録画像の組み合わせの一例と、この各入力画像−登録画像のＥｌｉ（各入力画像−登録画像の特徴点の合致度）、Ｒｉ、Ｍｉ、Ｓｉの一例を示す図である。

図７−１において、まず、位置・姿勢推定部２３は、ｉ＝Ｅ１ｉ／ＲΣｉＥ１ｉを算出した後（ステップＳ１６１）、Ｍｉ＝Ｍ＿ｏ＊Ｒｉ（ただし、Ｍ＿ｏは定数、例えば、「１００」）を算出する（ステップＳ１６２）。次に、位置・姿勢推定部２３は、Ｓｉ＝（Σｉ＝１．．．ｉ Ｍｉ＋１）から、（Σｉ＝１．．．ｉ Ｍｉ＋１）までの整数の組み合わせを算出する（ステップＳ１６３）。

この後、位置・姿勢推定部２３は、１〜Ｍ＿ｏの整数から１つの整数をランダムに選択し、その整数が含まれるＳｋ（ｋ＝１．．．Ｎ）を選択し、入力画像ｋと対応する登録画像から三次元位置・姿勢推定を行い、インライア検出を行う（ステップＳ１６４）。図７−３に示す例では、ｉ＝３の組み合わせのＳｉが最も大きいので、ｉ＝３の入力画像−登録画像の組み合わせが選択される可能性が最も高くなる。

得られた三次元位置・姿勢推定結果を、他の入力画像−登録画像にも適用し、インライア検出を行う（ステップＳ１６５）。そして、ＲＡＮＳＡＣ処理が終了したか否かを判断し（ステップＳ１６６）、ＲＡＮＳＡＣ処理が終了してない場合には（ステップＳ１６６の「Ｙｅｓ」）、ステップＳ１６４に戻り、ＲＡＮＳＡＣ処理が終了するまで同一の処理を繰り返す。

実施例６によれば、入力画像と、各カメラで選択された登録画像との一致度を考慮して、ＲＡＮＳＡＣ法を使用して、三次元位置・姿勢推定時に、登録画像を使用する頻度を変えることとしたので、最も対応している入力画像と登録画像の組み合わせを多く使用して、三次元位置・姿勢推定ができ、インライアを検出し易くＲＡＮＳＡＣの収束を速くすることが可能となる（処理速度が速くなる）。また、視点によって、物体の一部が隠れている場合もあるが、その場合に対応する入力画像と登録画像の組み合わせは選ばれる確率が低くなるので、推定精度を向上させることが可能となる。

（実施例７）
実施例７は、テクスチャ情報と輪郭情報の両特徴量を使用して、三次元位置・姿勢推定の精度を向上させるものである。

図８−１は、実施例７を説明するためのフローチャート、図８−２〜図８−４は、実施例７を説明するための図である。

図８−１において、特徴量マッチング部２２は、入力画像のテクスチャ情報を使用して物体検出し、合致度Ｗｔを計算する（ステップＳ１７１）。具体的には、特徴点・特徴量抽出部２１は、入力画像のテクスチャ情報の特徴点を抽出し、特徴量マッチングモ部２２は、入力画像とデータベースの全登録画像１〜Ｎとのテクスチャ情報の特徴点の合致度を算出し、そのうち最も合致度が高い登録画像をキーフレームとして選択し、そのときの合致度をＷｔとする。

また、特徴量マッチング部２２は、入力画像の輪郭情報を使用して物体検出し、合致度Ｗｂを計算する（ステップＳ１７２）。具体的には、特徴点・特徴量抽出部２１は、入力画像の輪郭情報の特徴点を抽出し、特徴量マッチング部２２は、入力画像とデータベースの全登録画像１〜Ｎとの輪郭情報の特徴点の合致度を算出し、そのうち最も合致度が高い登録画像をキーフレームとして選択し、そのときの合致度をＷｔとする。

ここで、ＷｂとＷｔの値は、データベース作成時に予め決定しておいてもよい。例えば、模様が全くない物体は、Ｗｔ＝０、Ｗｂ＝１、模様が非常に多き物体はＷｔ＝１，Ｗｂ＝０などとすることができる。

位置・姿勢推定部２３は、Ｗｔ＞Ｗｂであるか否かを判断する（ステップＳ１７３）。位置・姿勢推定部２３は、Ｗｔ＞Ｗｂである場合は（ステップＳ１７３の「Ｙｅｓ」）、テクスチャ情報を使用して、物体の三次元位置・姿勢推定（＝回転パラメータＲ０、並進パラメータｔ０を推定）する（ステップＳ１７４）。他方、位置・姿勢推定部２３は、Ｗｔ＞Ｗｂでない場合には（ステップＳ１７３の「Ｎｏ」）、輪郭情報を使用して、物体の三次元位置・姿勢推定（＝回転パラメータＲ０、並進パラメータｔ０を推定）する（ステップＳ１７５）。

位置・姿勢推定部２３は、物体の特徴点（テクスチャ情報）を、回転パラメータＲ０、並進パラメータｔ０を使って入力画像に投影し、特徴点の座標Ｃｉ（１．．．Ｎ，Ｎ：投影された特徴点数）を計算する（ステップＳ１７６）。投影点の座標Ｃｉと、対応する入力画像中の特徴点までの距離Ｄｉを計算する（ステップＳ１７７）。図８−２は、テクスチャ情報で算出した回転パラメータＲｏ、並進パラメータｔ０を使って、入力画像に投影した点と、入力画像の対応点とのずれの一例を示す図である。

位置・姿勢推定部２３は、物体の三次元形状（輪郭）を、回転パラメータＲ０、並進パラメータｔ０を使って入力画像に投影し、投影像の輪郭Ｂ０を算出する（ステップＳ１７８）。輪郭Ｂ０上に計算点Ｂｊを設定（ｊ＝１．．．Ｍ，Ｍ＝Ｎ×Ｗｂ／Ｗｔ）する（ステップＳ１７９）。ここで、Ｗｂ／Ｗｔの値は、データベース作成時に予め決定しておいてもよい。位置・姿勢推定部２３は、計算点Ｂｊと、点Ｂｊでの輪郭Ｂ０の法線方向に最も近い、入力画像のエッジ点までの距離Ｄｊを計算する（ステップＳ１８０）。図８−３は、物体の三次元形状（輪郭）を、回転パラメータＲ０、並進パラメータｔ０を使って入力画像に投影した例を示す図、図８−４は、図８−３を反転表示した図である。

そして、位置・姿勢推定部２３は、回転パラメータＲ０、並進パラメータｔ０を初期値として、ΣｉＤｉ＋ΣｊＤｊが最小となるような回転パラメータＲ、並進パラメータｔを計算する（ステップＳ１８１）。

実施例７によれば、データベースに格納されている登録画像のテクスチャ情報および輪郭情報の両者を使用して、入力画像の物体の三次元位置・姿勢推定を行うこととしたので、物体の模様の有無にかかわらず、物体の三次元位置・姿勢推定が可能となり、また、これらの２つの特徴量を組み合わせることで精度の高い物体の検出が可能となる。

また、テクスチャの多い物体は、物体内部の模様のエッジと、輪郭のエッジを区別することが難しく、三次元位置・姿勢推定に輪郭情報を利用することは一般に困難である。これに対して、実施例７によれば、最初にテクスチャ情報を使用して、概略の三次元位置・姿勢を推定できると、投映像の周辺のエッジを輪郭のエッジとして抽出でき、この後、輪郭情報も三次元位置・姿勢推定に使用できる。この結果、テクスチャの多い物体の三次元位置・姿勢精度を向上させることができる。さらに、テクスチャの少ない物体は、輪郭情報だけでなく、例えば、ワンポイントの模様の情報も三次元位置・姿勢推定に使えることになるので、三次元位置・姿勢の推定精度を向上させることができる。

（実施例８）
実施例８は、テクスチャ情報の特徴点を使用して、物体の三次元位置・姿勢推定が原理的に困難な表裏対称物体を、輪郭情報も併用することで、物体の三次元位置・姿勢推定を高精度に行うものである。

図９−１は、実施例８を説明するためのフローチャートである。図９−２は、実施例８を説明するための図である。

図９−１において、位置・姿勢推定部２３は、テクスチャ情報の特徴点を使用して、入力画像との合致度Ｅｉが高い順に登録画像を選択する（ステップＳ１９１）。そして、位置・姿勢推定部２３は、選択した登録画像の回転パラメータＲ、並進パラメータｔを使用して、物体の三次元位置・姿勢推定を行い、物体の三次元形状を入力画像に投影し、輪郭を抽出する（ステップＳ１９２）。

位置・姿勢推定部２３は、輪郭上の点Ｐｉ（ｉ＝１．．．Ｎ、Ｎは輪郭線上に設定した点の数）から、Ｐｉにおける輪郭線の法線方向で、最も近い入力画像のエッジ点Ｑｉを選択する（ステップＳ１９３）。そして、Ｐｉでの輪郭線の法線方向と、Ｑｉでのエッジ線の接線方向のなす角θｉを計算した後（ステップＳ１９４）、Ｅ＝Σｉ｛θｉ｝を計算する（ステップＳ１９５）。位置・姿勢推定部２３は、Ｅ＜閾値であるか否かを判断し（ステップＳ１９６）、Ｅ＜閾値である場合には（ステップＳ１９６の「Ｙｅｓ」）、ステップＳ１９２で算出した三次元位置・姿勢推定が正しいと判断して当該処理を終了し、Ｅ＜閾値でない場合には（ステップＳ１９６の「Ｎｏ」）、ステップＳ１９１に戻り、次の登録画像について処理を行う。

実施例８によれば、位置・姿勢推定部２３は、入力画像に対して特徴点の合致度が高い順に、その登録画像の位置関係に基づいて入力画像の物体の三次元位置・姿勢推定を行い、物体の三次元形状を入力画像に投影して輪郭線を抽出し、輪郭線と入力画像のエッジ線とに基づいて、物体の三次元位置・姿勢推定が正しか否かを判断することとしたので、表裏対称物体において、１８０°反転した登録画像をキーフレームとして選択することなく、正しいキーフレームを選択することが可能となる。

（実施例９）
実施例９は、各登録画像上の特徴点の三次元的なバラツキが平面的に近いか否かを指標として、より精度良く物体の三次元位置・姿勢を推定するためのカメラの移動方向を決定するものである。

図１０−１は、実施例９を説明するためのフローチャート、図１０−２は、特徴点の三次元座標のバラツキを説明するための図である。図１０−３は、登録画像の選択方法を説明するための図である。

図１０−２において、Ｓは特徴点の三次元分布にフィッテイングした平面、ｄｎは特徴点とフィッテイングした平面Ｓとの距離を示している。特徴点の三次元座標のバラツキが平面上に近い場合は、物体の検出はできるが三次元位置・姿勢推定の精度が悪くなる。例えば、立方体形状の物体を正面から撮像した場合が該当する。そこで、実施例９では、特徴点の三次元的なバラツキの平面度合いが閾値を超える登録画像を三次元位置・姿勢推定に使用する。特徴点のバラツキの平面度合いＥｉは、Ｅｉ＝（１／Ｎ）Σ（ｄｎ）で表すことができる。ここで、Ｎはある登録画像上の特徴点数である。特徴点のバラツキの平面度合いＥｉは、登録画像毎に予め算出しておいてデータベースに格納しておく。

図１０−１において、位置・姿勢推定部２３は、キーフレームとして、登録画像ｉを選択する（ステップＳ２０１）。登録画像ｉの特徴点のバラツキの平面度合いＥｉを画像データベースから読み出し、Ｅｉが閾値以下であるか否かを判定する（ステップＳ２０２）。位置・姿勢推定部２３は、Ｅｉが閾値以下でない場合には（ステップＳ２０２の「Ｎｏ」）、登録画像ｉの特徴点のバラツキの平面度合いＥｉが閾値を越えており、高精度な三次元位置・姿勢推定を行うことができるので、当該処理を終了する。

位置・姿勢推定部２３は、Ｅｉが閾値以下である場合には（ステップＳ２０２の「Ｙｅｓ」）、ｄｉｊが小さく、かつＥｊが大きい登録画像ｊを選択する（ステップＳ２０３）。ここで、ｄｉｊ＝ｄ＊（１−ｃｏｓθ）は、登録画像ｉと登録画像ｊとの距離・向きである。図１０−３は、ｄｉｊが小さく、かつＥｊが大きい登録画像ｊを選択する場合を説明するための図である。登録画像において、ｄｉｊの小さい方からの序列と、Ｅｊの大きい方からの序列との和が小さいものを登録画像ｊとして選択する。

位置・姿勢推定部２３は、登録画像ｉと登録画像ｊの運動パラメータＲ（回転）、ｔ（並進）をデータベースからそれぞれ取得する（ステップＳ２０４）。そして、位置・姿勢推定部２３は、画像取得部１０に、両登録画像ｉ、ｊの運動パラメータＲ（回転）、ｔ（並進）の差だけカメラを回転・並進させて撮像させる（ステップＳ２０５）。これにより、登録画像ｊに対応する入力画像を取得することができる。そして、ｉ＝ｊに設定した後（ステップＳ２０６）、ステップＳ２０１に戻り、同じ処理をＥｉが閾値より大きくなるまで繰り返し行う。

実施例９によれば、位置・姿勢推定部２３は、キーフレームの特徴点の三次元的なバラツキの平面度合いが閾値を超える場合に、当該キーフレームを物体の三次元位置・姿勢推定に使用することとしたので、物体の三次元位置・姿勢推定を高精度に行えない登録画像（入力画像）を使用することを防止でき、物体の三次元位置・姿勢推定を高精度に行うことが可能となる。また、キーフレームの特徴点の三次元的なバラツキの平面度合いが閾値を超えるまで、カメラを移動させるので、必ず、精度の高いカメラの撮像位置まで移動させることが可能となる。

［物体の周辺の三次元の凹凸構造の認識］
（実施例１０）
実施例１０は、データベースを使用して推定した物体の三次元位置・姿勢に基づいて、画像内での見え方を推定し、実際の画像との差分から物体が他の物体に隠されている領域を検出するものである。

図１１−１は、実施例１０を説明するためのフローチャート、図１１−２は、実施例１０を説明するための図である。実施例１０を図１１−１のフローチャートに従って、図１１−２を参照しつつ説明する。

図１１−１において、三次元復元部２４は、位置・姿勢推定部２３で推定した三次元位置・姿勢に基づいた物体の三次元形状Ｏを入力画像に投影し、入力画像上での物体の領域を算出する（ステップＳ２１２）。

三次元復元部２４は、入力画像の領域内の画像１を抽出し（ステップＳ２１３）、登録画像内の物体の領域に含まれる画像を、入力画像の視点からみた画像２に変換する（ステップＳ２１４）。そして、三次元復元部２４は、画像１（第１の画像）と画像２（第２の画像）の差分をとり、差分が閾値以下の領域を物体の領域として抽出する（ステップＳ２１５）。

実施例１０によれば、三次元復元部２４は、位置・姿勢推定部２３で推定した三次元位置・姿勢に基づいた物体の三次元形状を入力画像に投影し、入力画像上での物体の領域を算出し、入力画像の算出した領域内の画像を第１の画像として抽出し、登録画像内の物体の領域に含まれる画像を、入力画像の視点からみた第２の画像に変換し、第１の画像と第２の画像との差分が閾値以下の領域を物体の領域として抽出することとしたので、入力画像内での見え方を推定し、実際の画像との差分から物体が他の物体に隠されている領域を検出することができ、物体の領域だけを抽出することが可能となり、物体以外の領域の三次元復元精度を向上させることができる。

例えば、図１１−２に示す例では、マグカップの取手の前方にある障害物（ジュースパック）を取り除くことなく、三次元復元できるので、ハンド・アームで把持する際に、ハンドの軌跡を、物体を避けて経路を計算することが可能となる。

［キャリブレーション］
（実施例１１）
実施例１１は、データベースに登録されている物体を１または複数使用して、画像取得部１０のステレオカメラの各カメラの位置関係のキャリブレーションや、カメラ台、ロボット・アーム、ハンド等のモータのキャリブレーションをロボットが自動で行うものである。画像取得部１０は、キャリブレーション手段を備えており、以下に示すフローチャートの処理を実行する。

図１２−１は、実施例１１を説明するためのフローチャートである。図１２−２は、実施例１１を説明するための説明図である。同図では、Ａｒはロボット・アーム、Ｏｂは参照用の物体を示している。

図１２−１において、画像取得部１０のロボット・アームでデータベースに登録されている物体を把持する（ステップＳ２２１）。ロボット・アームで物体を動かしながら画像を取得する（ステップＳ２２２）。画像を取得した時のモータの角度（カメラ台・アーム・ハンド・導体など）を読み取る（ステップＳ２２３）。

各カメラで撮像した入力画像から、物体上の特徴点Ｐｉを抽出し、Ｐｉの画像上での２次元座標（ｕｉ、ｖｉ）を読み取り、ステレオ視で三次元座標（Ｘｉ、Ｙｉ、Ｚｉ）を計算する（ステップＳ２２４）。

そして、特徴点Ｐｉと対応する登録画像中の特徴点Ｐｉ’を探索し、Ｐｉ’の三次元座標（Ｘｉ’、Ｙｉ’、Ｚｉ’）を計算する（ステップＳ２２５）。（ｕｉ、ｖｉ）と、（Ｘｉ、Ｙｉ、Ｚｉ）に基づき、各カメラ間の位置関係をキャリブレーションし、（Ｘｉ、Ｙｉ、Ｚｉ）と、（Ｘｉ’、Ｙｉ’、Ｚｉ’）に基づき、モータの角度をキャリブレーションする（ステップＳ２２６）。

実施例１１によれば、データベースに登録されている物体を１または複数使用して、画像取得部１０の各カメラの位置関係のキャリブレーションや、カメラ台、ロボット・アーム、ハンド等のモータのキャリブレーションをロボットが自動で行うこととしたので、高精度なキャリブレーションを行うことが可能となる。

特に、誤対応しにくい特徴点を多く有している場合や、特徴点の配置が三次元的に広くばらついているデータベースの登録画像を使用すると、高精度なキャリブレーションを行うことができる。さらに、このような条件を満たす登録画像の番号を知識として与えておき、そのデータベース画像に近い見え方をするように、実際に物体の向きを制御すると、高精度なキャリブレーションを行うことができる。

また、ステレオカメラやアーム・ハンドのキャリブレーションをロボットが定期的に自動で行うことができ、キャリブレーションの手間を省くことができる。また、データベースとして、移動ロボットの移動空間を考えると、移動台車のキャリブレーションも同様の手法で行うことができる。

（プログラム）
なお、本発明の目的は、上述した画像処理装置の機能を実現するソフトウエアのプログラムコードを記録した記録媒体を、システム（ロボットシステム等）または装置（例えば、ホストコンピュータ等）に供給し、そのシステムまたは装置のコンピュータ（または、ＣＰＵ、ＭＰＵ、ＤＳＰ）が記録媒体に格納されたプログラムコードを実行することによっても達成することが可能である。この場合、記録媒体から読み出されたプログラムコード自体が前述した画像処理装置の機能を実現することになり、そのプログラムコードまたはそのプログラムを記憶した記録媒体は本発明を構成することになる。プログラムコードを供給するための記録媒体としては、ＦＤ、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＤＶＤ、磁気テープ、不揮発性のメモリ、ＲＯＭなどの光記録媒体、磁気記録媒体、光磁気記録媒体、半導体記録媒体を使用することができる。

また、コンピュータが読み出したプログラムコードを実行することにより、前述した画像処理装置の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）などが実際の処理の一部または全部を行い、その処理によって前述した画像処理装置の機能が実現される場合も含まれること言うまでもない。

また、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれた後、そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって前述した画像処理装置の機能が実現される場合も含まれることは言うまでもない。

以上のように、本発明に係る画像処理方法、画像処理装置、および画像処理用プログラムは、ロボットの視覚情報の取得や物体認識、車両の自動運転装置、パノラマ画像の作成、および三次元形状の計測等に広く利用可能である。

本発明に係る画像処理装置の構成例を示す図である。 データベースの概念図である。 データベースの構成例を示す図である。 画像処理装置で未知物体を検出する場合の全体動作の概略を説明するためのフローチャートである。 実施例１を説明するためのフローチャートである。 実施例１を説明するための説明図である。 実施例１を説明するための説明図である。 実施例１を説明するための説明図である。 実施例２を説明するための説明図である。 実施例２を説明するための説明図である。 実施例３を説明するためのフローチャートである。 実施例３を説明するための説明図である。 実施例４を説明するためのフローチャートである。 実施例５を説明するためのフローチャートである。 実施例５を説明するための説明図である。 実施例５を説明するための説明図である。 実施例６を説明するためのフローチャートである。 実施例６を説明するための説明図である。 実施例６を説明するための説明図である。 実施例７を説明するためのフローチャートである。 実施例７を説明するための説明図である。 実施例７を説明するための説明図である。 実施例７を説明するための説明図である。 実施例８を説明するためのフローチャートである。 実施例８を説明するための説明図である。 実施例９を説明するためのフローチャートである。 実施例９を説明するための説明図である。 実施例９を説明するための説明図である。 実施例１０を説明するためのフローチャートである。 実施例１０を説明するための説明図である。 実施例１１を説明するためのフローチャートである。 実施例１１を説明するための説明図である。

符号の説明

１ 画像処理装置
１０ 画像取得部
２０ 画像処理部
２１ 特徴点・特徴量抽出部
２２ 特徴量マッチング部
２３ 位置・姿勢推定部
２４ 三次元復元部
２５ 把持部推定部
３０ 三次元形状取得部
４０ データベース登録部
４１ 特徴点・特徴量抽出部
４２ 特徴点の三次元位置推定部
４３ プリミティブモデル生成部
４４ データベース格納部

Claims (25)

1. 参照用の物体を異なる複数の視点で各々撮像した登録画像の特徴量と、各登録画像の物体座標上での位置関係とを関連づけて格納したデータベースと、
   認識対象物体を撮像した入力画像の特徴量と、前記データベースに格納されている各登録画像の特徴量との相関を各々算出し、相関の高い登録画像をキーフレームとして検出する物体検出手段と、
   前記データベースを参照して、前記物体検出手段で検出されたキーフレームに対応する前記位置関係に基づいて、前記入力画像中の物体の三次元位置・姿勢を推定する物体位置・姿勢推定手段と、
   を備え、
   前記物体検出手段は、未知物体を複数の視点で撮像した複数の入力画像毎に、前記データベースに格納されている各登録画像との特徴点の合致度を算出して、それぞれ合致度が最も高いキーフレームを検出し、各視点毎に、当該視点の入力画像と他の視点の入力画像との位置関係をそれぞれ取得し、前記キーフレームと前記位置関係に近い登録画像を前記データベースから選択し、各視点毎に、当該視点の入力画像と前記キーフレーム、および他の視点の入力画像と選択した登録画像の組み合わせを作成し、当該他の視点の入力画像と選択した登録画像の特徴点の合致度を算出し、各視点毎の前記組み合わせにおいて、前記合致度の平均値が高い組み合わせのキーフレームを最終的なキーフレームとして選択することを特徴とする画像処理装置。
2. 前記特徴量は、テクスチャ情報であり、
   前記データベースから登録画像を選択し、選択された登録画像上の各特徴点に対応する特徴点を、前記選択された登録画像と距離・向きが近い登録画像から抽出し、前記選択された登録画像の特徴点と前記距離・向きが近い登録画像の特徴点との位置を比較し、所定の位置条件にある前記距離・向きが近い登録画像の特徴点を、前記データベースに前記各登録画像のテクスチャ情報の特徴点として格納することを特徴とする請求項１に記載の画像処理装置。
3. 前記参照用の物体のプリミティブモデルを定義して、前記データベースに格納するプリミティブモデル生成手段と、
   前記物体位置・姿勢推定手段で推定した物体の三次元位置・姿勢に基づいて、前記データベースに格納されている対応する参照用の物体のプリミティブモデルを復元して物体の把持部を推定する把持部推定手段と、
   を備えたことを特徴とする請求項１に記載の画像処理装置。
4. 前記プリミティブモデル生成手段は、前記参照用の物体のプリミティブモデルが対応する登録画像の領域を包含するように、当該プリミティブモデルを修正することを特徴とする請求項３に記載の画像処理装置。
5. 物体を撮像して画像を取得する撮像手段と、
   前記参照用の物体の位置・姿勢を変更させる物体位置・姿勢変更手段と、
   を備え、
   物体位置・姿勢変更手段で前記参照用の物体の位置・姿勢を変更しながら、前記撮像手段で、当該参照用の物体を撮像して、当該参照用の物体の三次元形状が欠落している部分の特徴点を抽出して、前記データベースに格納することを特徴とする請求項１に記載の画像処理装置。
6. 物体位置・姿勢推定手段は、前記合致度の平均値が高い組み合わせにおいて、ロバスト推定を利用して、位置・姿勢推定時に登録画像を使用する頻度を変えることを特徴とする請求項１に記載の画像処理装置。
7. 前記特徴量は、テクスチャ情報および輪郭情報であり、
   前記物体位置・姿勢推定手段は、前記データベースに格納されている登録画像のテクスチャ情報および輪郭情報の両者を使用して、前記入力画像の物体の位置・姿勢推定を行うことを特徴とする請求項１に記載の画像処理装置。
8. 前記物体位置・姿勢推定手段は、
   前記入力画像に対して特徴点の合致度が高い順に、その登録画像の前記位置関係に基づいて前記入力画像の物体の三次元位置・姿勢推定を行い、物体の三次元形状を入力画像に投影して輪郭線を抽出し、
   前記輪郭線と前記入力画像のエッジ線とに基づいて、前記物体の三次元位置・姿勢推定が正しか否かを判断することを特徴とする請求項１に記載の画像処理装置。
9. 前記物体位置・姿勢推定手段は、前記キーフレームの特徴点の三次元的なバラツキの平面度合いが閾値を超える場合に、前記物体の三次元位置・姿勢推定に使用することを特徴とする請求項１に記載の画像処理装置。
10. 検出した物体の周囲の三次元の凹凸構造を認識する三次元復元手段を備えたことを特徴とする請求項１に記載の画像処理装置。
11. 前記三次元復元手段は、
    位置・姿勢推定手段で推定した三次元位置・姿勢に基づいた物体の三次元形状を入力画像に投影し、入力画像上での物体の領域を算出し、
    前記入力画像の前記算出した領域内の画像を第１の画像として抽出し、
    前記登録画像内の前記物体の領域に含まれる画像を、前記入力画像の視点からみた第２の画像に変換し、
    前記第１の画像と前記第２の画像との差分が閾値以下の領域を前記物体の領域として抽出することを特徴とする請求項１０に記載の画像処理装置。
12. 物体を撮像して画像を取得する複数の撮像手段と、
    前記複数の撮像手段の位置・姿勢を調整する位置・姿勢変更手段と、
    前記参照用の物体の位置・姿勢を変更させる物体位置・姿勢変更手段と、
    前記物体位置・姿勢変更手段で前記参照用の物体の位置・姿勢を変更しながら、前記複数の撮像手段で当該参照用の物体を撮像した画像と、前記データベースに格納されている対応する登録画像とを比較して、前記位置・姿勢変更手段のキャリブレーションを行うキャリブレーション手段と、
    を備えたことを特徴とする請求項１に記載の画像処理装置。
13. 参照用の物体を異なる複数の視点で各々撮像した登録画像の特徴量と、各登録画像の物体座標上での位置関係とを関連づけてデータベースに格納する工程と、
    認識対象物体を撮像した入力画像の特徴量と、前記データベースに格納されている各登録画像の特徴量との相関を各々算出し、相関の高い登録画像をキーフレームとして検出する物体検出工程と、
    前記データベースを参照して、前記検出されたキーフレームに対応する位置関係に基づいて、前記入力画像中の物体の三次元位置・姿勢を推定する物体位置・姿勢推定工程と、
    を含み、
    前記物体検出工程では、
    未知物体を複数の視点で撮像した複数の入力画像毎に、前記データベースに格納されている各登録画像との特徴点の合致度を算出して、それぞれ合致度が最も高いキーフレームを検出し、
    各視点毎に、当該視点の入力画像と他の視点の入力画像との位置関係をそれぞれ取得し、前記キーフレームと前記位置関係に近い登録画像を前記データベースから選択し、
    各視点毎に、当該視点の入力画像と前記キーフレーム、および他の視点の入力画像と選択した登録画像の組み合わせを作成し、当該他の視点の入力画像と選択した登録画像の特徴点の合致度を算出し、
    各視点毎の前記組み合わせにおいて、前記合致度の平均値が高い組み合わせのキーフレームを最終的なキーフレームとして選択することを特徴とする画像処理方法。
14. 前記特徴量は、テクスチャ情報であり、
    前記データベースから登録画像を選択し、選択された登録画像上の各特徴点に対応する特徴点を、前記選択された登録画像と距離・向きが近い登録画像から抽出し、前記選択された登録画像の特徴点と前記距離・向きが近い登録画像の特徴点との位置を比較し、所定の位置条件にある前記距離・向きが近い登録画像の特徴点を、前記データベースに前記各登録画像のテクスチャ情報の特徴点として格納することを特徴とする請求項１３に記載の画像処理方法。
15. 前記参照用の物体のプリミティブモデルを定義して、前記データベースに格納するプリミティブモデル生成工程と、
    前記物体位置・姿勢推定手工程で推定した物体の三次元位置・姿勢に基づいて、前記データベースに格納されている対応する参照用の物体のプリミティブモデルを復元して物体の把持部を推定する把持部推定工程と、
    を含むことを特徴とする請求項１３に記載の画像処理方法。
16. 前記プリミティブモデル生成工程では、前記参照用の物体のプリミティブモデルが対応する登録画像の領域を包含するように、当該プリミティブモデルを修正することを特徴とする請求項１５に記載の画像処理方法。
17. 物体位置・姿勢変更手段で前記参照用の物体の位置・姿勢を変更しながら、撮像手段で、当該参照用の物体を撮像して、当該参照用の物体の三次元形状が欠落している部分の特徴点を抽出して、前記データベースに格納することを特徴とする請求項１３に記載の画像処理方法。
18. 物体位置・姿勢推定工程では、前記合致度の平均値が高い組み合わせにおいて、ロバスト推定を利用して、位置・姿勢推定時に登録画像を使用する頻度を変えることを特徴とする請求項１７に記載の画像処理方法。
19. 前記特徴量は、テクスチャ情報および輪郭情報であり、
    前記物体位置・姿勢推定工程では、前記データベースに格納されている登録画像のテクスチャ情報および輪郭情報の両者を使用して、前記入力画像の物体の三次元位置・姿勢推定を行うことを特徴とする請求項１３に記載の画像処理方法。
20. 前記物体位置・姿勢推定工程では、
    前記入力画像に対して特徴点の合致度が高い順に、その登録画像の前記位置関係に基づいて前記入力画像の物体の三次元位置・姿勢推定を行い、物体の三次元形状を入力画像に投影して輪郭線を抽出し、
    前記輪郭線と前記入力画像のエッジ線とに基づいて、前記物体の三次元位置・姿勢推定が正しか否かを判断することを特徴とする請求項１３に記載の画像処理方法。
21. 前記物体位置・姿勢推定工程は、前記キーフレームの特徴点の三次元的なバラツキの平面度合いが閾値を超える場合に、前記物体の三次元位置・姿勢推定に使用することを特徴とする請求項１３に記載の画像処理方法。
22. 検出した物体の周囲の三次元の凹凸構造を認識する三次元復元工程を含むことを特徴とする請求項１３に記載の画像処理方法。
23. 前記三次元復元工程では、
    位置・姿勢推定工程で推定した三次元位置・姿勢に基づいた物体の三次元形状を入力画像に投影し、入力画像上での物体の領域を算出し、
    前記入力画像の前記算出した領域内の画像を第１の画像として抽出し、
    前記登録画像内の前記物体の領域に含まれる画像を、前記入力画像の視点からみた第２の画像に変換し、
    前記第１の画像と前記第２の画像との差分が閾値以下の領域を前記物体の領域として抽出することを特徴とする請求項２２に記載の画像処理方法。
24. 物体位置・姿勢変更手段で参照用の物体の位置・姿勢を変更しながら、複数の撮像手段で当該参照用の物体を撮像した画像と、前記データベースに格納されている対応する登録画像とを比較して、前記位置・姿勢変更手段のキャリブレーションを行うキャリブレーション工程を含むことを特徴とする請求項１３に記載の画像処理方法。
25. 請求項１３〜請求項２４にいずれか１つに記載の画像処理方法の各工程をコンピュータに実行させることを特徴とする画像処理用プログラム。

Images