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

Abstract

【課題】物体の位置及び姿勢の真値と初期値として入力される位置及び姿勢の概略値とのずれに関わらず、ロバストに物体の位置及び姿勢を算出する。
【解決手段】検出部１２０は、基準位置姿勢において視点から観測される三次元形状モデル上の幾何特徴を基準幾何特徴として検出する。類似度算出部１４０は、参照位置姿勢において視点から観測される三次元形状モデル上の基準幾何特徴に対応する幾何特徴である参照幾何特徴を探索し、基準幾何特徴と参照幾何特徴との類似度を算出する。評価値算出部１５０は、類似度に基づいて、基準幾何特徴と参照幾何特徴との対応付けに係る評価値を算出する。
【選択図】図１

Description

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

近年、ロボット技術の発展とともに、これまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。複雑なタスクの代表的な例として、工業製品の組立が挙げられる。ロボットが自律的に組立作業を行うためには、ハンド等のエンドエフェクタによって部品を把持することが必要となる。ロボットハンドによって部品を把持するためには、把持の対象となる部品とロボットとの相対的な位置及び姿勢を計測する必要がある。このような位置及び姿勢の計測は、ロボットによる工業製品の組立だけではなく、自律移動ロボットの自己位置同定や三次元空間再構成のための手段としての位置及び姿勢の推定等、様々な場面で必要とされる。

位置及び姿勢を計測する方法として、カメラで撮影した濃淡画像やカラー画像上から検出される幾何特徴や、距離センサで画像様に計測して得られた距離画像に対して物体の三次元形状モデルを当てはめるモデルフィッティングによる方法がある。以下では、上記濃淡画像やカラー画像を総称して二次元画像と称す。二次元画像に対するモデルフィッティングでは、三次元形状モデルの幾何特徴を画像上に投影して画像上で検出された幾何特徴との対応付けが行われる。そして、この対応間の距離を最小化するように位置及び姿勢の補正を繰り返されることでカメラと物体との位置及び姿勢の関係が算出される。距離画像に対するモデルフィッティングでは、距離画像の各計測点が三次元座標を持つ三次元点群に変換され、この三次元点群と三次元形状モデル上の三次元点群との対応が求められる。そして、対応間の距離を最小化するように位置及び姿勢の補正が繰り返されることでカメラと物体との位置及び姿勢の関係が算出される。

モデルフィッティングによる計測方法では、三次元形状モデルと撮像画像上とで幾何特徴の正確な対応を求めることが重要である。上述した手法では、位置及び姿勢の初期値として位置及び姿勢の概略値が入力されるものとし、この位置及び姿勢の概略値に基づいて三次元形状モデル上の幾何特徴と撮像画像上の幾何特徴との対応が求められる。このような手法では、位置及び姿勢の概略値と位置及び姿勢の真値とがずれている場合、誤対応の発生は避けられない。誤対応が多数含まれると、位置及び姿勢の推定処理が破綻したり、位置及び姿勢の推定精度が低下したりするという問題がある。

この問題の解決策として、三次元形状モデル上の幾何特徴と撮像画像上の幾何特徴との対応に対してＭ推定を適用することで、誤対応の影響を軽減する方法が知られている（例えば、非特許文献１参照）。この方法では、対応間の距離が平均値に近い場合は重みを高く、遠い場合には重みを低く設定することにより、平均値から遠い対応点は対応の信頼度が低いと想定して位置及び姿勢の算出に与える影響を小さくしている。

近藤友紀, 宮本敦, 金子俊一, 五十嵐悟, "M推定を導入したロバストICP位置決め法", Technical report of IEICE. PRMU 100(507) pp.21-26 20001207.

しかしながら、非特許文献１の手法では、三次元形状モデル上の幾何特徴と撮像画像上の幾何特徴との対応間の距離に基づいて、それぞれの対応が位置及び姿勢の算出の処理結果に与える寄与度を調整する。具体的には、対応間の距離の小さいもの程、寄与度が大きく設定される。但し、この手法はあくまでも数値計算的に外れ値を除外する方法であり、三次元形状モデル上の幾何特徴及び撮像画像上の幾何特徴が、物体のどの部位のものか、どんな形状を表すか等、物体形状に基づく幾何学的な類似性を考慮して対応が判定されていない。従って、上記の位置及び姿勢の概略値と位置及び姿勢の真値とのずれによって、三次元形状モデル上の幾何特徴と撮像画像上の幾何特徴とで、対応間の距離が偶発的に小さくなるような誤対応が存在する場合、Ｍ推定でその悪影響を排除することは不可能である。

そこで、本発明の目的は、物体の位置及び姿勢の真値と初期値として入力される位置及び姿勢の概略値とのずれに関わらず、三次元形状モデルの幾何特徴と撮像画像上の幾何特徴との正確な対応を求め、ロバストに物体の位置及び姿勢を算出することにある。

本発明の情報処理装置は、物体の三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を基準位置姿勢として設定する第１の設定手段と、前記基準位置姿勢において視点から観測される前記三次元形状モデル上の幾何特徴を基準幾何特徴として検出する検出手段と、前記基準位置姿勢とは異なる、前記三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を参照位置姿勢として設定する第２の設定手段と、前記参照位置姿勢において視点から観測される前記三次元形状モデル上の前記基準幾何特徴に対応する幾何特徴である参照幾何特徴を探索する探索手段と、前記基準幾何特徴と前記参照幾何特徴との類似度を算出する第１の算出手段と、前記第１の算出手段により算出される類似度に基づいて、前記基準幾何特徴と前記参照幾何特徴との対応付けに係る評価値を算出する第２の算出手段とを有することを特徴とする。

本発明によれば、物体の位置及び姿勢の真値と初期値として入力される位置及び姿勢の概略値とのずれに関わらず、三次元形状モデルの幾何特徴と撮像画像上の幾何特徴との正確な対応を求め、ロバストに物体の位置及び姿勢を算出することが可能となる。

本発明の実施形態においてオフラインでモデル作成を行うモデル作成装置と、オンラインで位置姿勢算出を行う三次元計測装置との構成を示す図である。 三次元形状モデルの一例を示す図である。 本発明の実施形態におけるモデル作成処理の手順を示すフローチャートである。 位置姿勢推定処理を示すフローチャートである。 基準位置姿勢を説明するための図である。 基準位置姿勢に配置された仮想カメラの観測画像平面上に三次元形状モデルが投影された状態を示す図である。 本発明の他の実施形態における、図３（ａ）のステップＳ１４００の詳細を示すフローチャートである。

以下、本発明を適用した好適な実施形態を、添付図面を参照しながら詳細に説明する。

先ず、本発明の第１の実施形態について説明する。以下の説明では、先ず、位置姿勢推定処理の事前準備段階としてオフラインで行うモデル作成処理について述べる。その後、モデル作成処理により作成された後述の位置姿勢算出用モデルを利用してオンラインで物体の位置及び姿勢（以下、位置姿勢と称す）を推定する処理について述べる。なお、以下の説明における「物体の位置姿勢」とは、物体と当該物体を撮影するカメラとの位置姿勢の関係のことを意味する。

モデル作成処理では、三次元形状モデル上の幾何特徴（以下、モデル幾何特徴と称す）の対応付けが成功する度合いを示す評価値を予め算出し、当該モデル幾何特徴と評価値とを組にして位置姿勢算出用モデルを作成する。また、位置姿勢推定処理では、各モデル幾何特徴に対応する評価値に基づき、位置姿勢推定に利用するモデル幾何特徴を選択して物体の位置姿勢を算出する。

本実施形態で対象とする位置姿勢推定方法では、初期値として与えられる概略的な位置姿勢（以下、概略位置姿勢と称す）に基づいて、位置姿勢算出用モデル上の三次元点と距離画像上の三次元点とを対応付ける。そして、各対応の距離の和を最小化するように位置姿勢の補正を繰り返すことで物体の位置姿勢を算出する。このとき、位置姿勢の真値と概略位置姿勢とにずれがあると、三次元点の対応に誤りが発生する。そこで本実施形態では、オフラインで、概略位置姿勢のずれを多数模擬して位置姿勢算出用モデル上の三次元点と距離画像上の三次元点との対応付けをシミュレートする。そして、このシミュレートの全試行回数に対する正しく対応付いた回数の割合を算出することで、位置姿勢算出用モデル上の各三次元点の評価値をそれぞれ算出する。位置姿勢推定時には、位置姿勢算出用モデルのうち、評価値の低い三次元点（モデル幾何特徴）は排除し、評価値の高い三次元点（モデル幾何特徴）のみを選択して物体の位置姿勢を算出する。これにより、ロバストに物体の位置姿勢を推定することができる。

なお、画像を撮影するカメラの視点が変われば、それに伴って画像上で観測される三次元点も変化する。また、各三次元点の正しい対応が得られる割合も変化する。従って、本実施形態では、代表的なカメラの視点を複数設定しておき、視点毎に、モデル幾何特徴と評価値との組みを生成する。ここで、視点毎の、モデル幾何特徴と評価値との組みを視点別位置姿勢算出用モデルとする。そして、視点別位置姿勢算出用モデルを視点の数だけまとめて保存したものを位置姿勢算出用モデルと定義する。

図１は、本実施形態においてオフラインでモデル作成を行うモデル作成装置１と、オンラインで位置姿勢算出を行う三次元計測装置２との構成を示している。図１に示すように、モデル作成装置１は、三次元形状モデル入力部１００、基準位置姿勢設定部１１０、検出部１２０、参照位置姿勢設定部１３０、類似度算出部１４０、評価値算出部１５０、位置姿勢算出用モデル作成部１６０から構成されている。さらに、三次元計測装置２は、モデル作成装置１、撮像部１７０及び位置姿勢算出部１８０から構成されている。また、図１に示す構成は、本発明の情報処理装置の適用例となる構成である。

三次元形状モデル入力部１００は、物体の形状を表す三次元形状モデルを入力する。図２は、三次元形状モデルの一例を示す図である。図２に示すように、三次元形状モデルは、面パッチの複数の組み合わせにより構成される。各面パッチは、面とその輪郭線を表すパラメータとにより定義されている。図２の例では、三次元形状モデルは、円形の面パッチＡ、Ｆ、半円筒形状の面パッチＢ〜Ｅの６つの面パッチにより構成されている。但し、三次元形状モデルはこれに限るものではない。例えば、各面パッチを三角形ポリゴンにより表現したポリゴンモデルであってもよい。その他、物体の形状を複数の面パッチにより表現するモデルであれば如何なるものであっても構わない。

基準位置姿勢設定部１１０は、三次元形状モデルと、三次元形状モデルを観測する仮想カメラとの相対的な位置姿勢関係を基準位置姿勢として入力する。基準位置姿勢は、実際に物体をカメラで撮影した場合における位置姿勢の真値に相当する。なお、基準位置姿勢設定部１１０は、第１の設定手段の適用例となる構成である。

検出部１２０は、基準位置姿勢において三次元形状モデル上で観測される幾何特徴（モデル幾何特徴）を検出する。本実施形態では、基準位置姿勢において三次元形状モデルを撮像する過程をシミュレートして擬似的な距離画像を生成し、距離画像の各画素の座標と奥行き値とから得られる三次元点群をモデル幾何特徴として検出する。ここで検出されるモデル幾何特徴を以下では基準幾何特徴と称す。

参照位置姿勢設定部１３０は、基準位置姿勢設定部１１０により設定された基準位置姿勢に対して、位置姿勢の変動を与えた位置姿勢を参照位置姿勢として複数設定する。参照位置姿勢は、位置姿勢推定時の概略位置姿勢に相当する。位置姿勢の変動の大きさは、オンラインでのモデルフィッティング時に入力される概略位置姿勢の誤差範囲に応じて決定するのが望ましい。位置姿勢の真値に対して、概略位置姿勢の誤差範囲が予め想定されているのであれば、その範囲内の大きさの変動をランダムに与える。或いは、位置姿勢の真値に対して、概略位置姿勢のずれの方向、大きさといった傾向が予めわかるのであれば、同様の傾向を持たせて変動を与えてもよい。その他、位置姿勢推定時に想定される概略位置姿勢と同様の変動を与えることができれば、いかなる方法であっても構わない。また、設定する参照位置姿勢は複数ではなく、単数であってもよい。例えば、位置姿勢のずれが一定方向に限られているのであればその方向に変動を与えた位置姿勢を代表として一つ入力してもよい。なお、参照位置姿勢設定部１３０は、第２の設定手段の適用例となる構成である。

類似度算出部１４０は、検出部１２０により検出された各基準幾何特徴について、複数の参照位置姿勢における三次元形状モデル上の対応点（参照幾何特徴）を探索し、各対応点について類似度を算出する。基準幾何特徴の対応点を探索する方法は、例えば、大石らの方法（大石岳史，中澤篤志，池内克史，"インデックス画像を用いた複数距離画像の高速同時位置あわせ,"電子情報通信学会論文誌,Vol.J89-D No.3,pp.513-521,Mar.2006）により行う。具体的には、三次元形状モデルを構成する各面パッチに固有のＩＤと色とを割り当て、参照位置姿勢に対応する観測画像平面に三次元形状モデルをレンダリングする。この観測画像平面上において、基準幾何特徴が検出された画素にレンダリングされている三次元形状モデル上の点をその基準幾何特徴の対応点とする。

類似度の算出は、基準幾何特徴がそれぞれ三次元形状モデル上で属している面パッチと、参照位置姿勢の観測画像平面上における対応点が三次元形状モデル上で属している面パッチとの同一性を比較することで行う。この類似度の算出は、基準幾何特徴毎に行われる。なお、類似度算出部１４０は、第１の算出手段の適用例となる構成である。

評価値算出部１５０は、類似度算出部１４０により算出された複数の類似度に基づき、基準幾何特徴毎に対応付けの成功度合いを示す評価値を算出する。評価値は、設定された参照位置姿勢の数に対する、類似した対応が得られた参照位置姿勢の数の割合として算出される。なお、評価値算出部１５０は、第２の算出手段の適用例となる構成である。

位置姿勢算出用モデル作成部１６０は、各基準幾何特徴と、評価値算出部１７０により算出された評価値とを組にし、さらに該当する基準位置姿勢をともに記録することで、後述の視点別位置姿勢算出用モデルを作成する。そして、位置姿勢算出用モデル作成部１６０は、視点別位置姿勢算出用モデルを全ての基準位置姿勢についてまとめることにより、位置姿勢算出用モデルを作成する。

撮像部１７０は、距離画像を撮像する距離センサである。距離画像は、各画素が奥行きの情報を持つ画像である。距離センサとしては、物体に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するTime-of-flight方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

位置姿勢算出部１８０は、位置姿勢算出用モデル作成部１６０により作成された位置姿勢算出用モデルを用いて、物体の位置姿勢を算出する。具体的には、位置姿勢算出部１８０は、撮像部１９０によって入力された距離画像の三次元点群に対して、位置姿勢算出用モデルの三次元点群を当てはめることにより、物体の位置姿勢を計測する。なお、位置姿勢算出部１８０は、第３の算出手段の適用例となる構成である。

図３は、第１の実施形態におけるモデル作成処理の手順を示すフローチャートである。以下、図３を参照しながら、モデル作成処理の各処理について説明する。なお、図３（ａ）は、モデル作成処理の全体的な手順を示しており、図３（ｂ）は、図３（ａ）のステップＳ１４００の詳細を示している。

ステップＳ１０００において、三次元形状モデル入力部１００は、三次元形状モデルを入力する。ステップＳ１１００において、基準位置姿勢設定部１１０は、三次元形状モデルを観測する仮想カメラの基準位置姿勢を複数設定する。繰り返しになるが、基準位置姿勢を複数設定するのは、物体を撮影するカメラの視点が変われば、それに伴って、撮影画像上の幾何特徴と三次元形状モデル上の幾何特徴とについて正しい対応付けが得られる割合が変化するためである。

複数の基準位置姿勢の用意には、例えば、図５に示すような、物体を覆うGeodesic domeを用いる。具体的には、Geodesic domeの各頂点を仮想カメラ５０１の位置、そこから三次元形状モデル５０２を眺める方向を仮想カメラ５０１の視点として基準位置姿勢を設定する。このとき、仮想カメラ５０１のロール方向については任意の値に設定してよい。なお、Geodesic domeにより近似される球の半径は、仮想カメラ５０１と三次元形状モデル５０２との距離に相当する。従って、位置姿勢推定に利用する画像に関して、撮像距離が予め想定されているのであれば、その距離をGeodesic Domeの半径と同一にするのが望ましい。あるいは、段階的に大きさの異なるGeodesic Domeを用意し、それぞれのGeodesic Domeの各頂点を基準位置姿勢として設定してもよい。但し、基準位置姿勢の設定方法はこれに限るものではない。例えば、物体を覆う球を仮定して地球儀のように緯度経度を割り当て、緯度方向、軽度方向に等間隔に視点を取ってもよい。あるいは、位置姿勢推定の初期値として発生しやすい確率に基づき、発生確率の高い基準位置姿勢については、密度を上げて設定してもよい。その他、実際の位置姿勢推定における、カメラと物体との相対位置姿勢を模擬できるものであれば、いかなる方法であっても構わない。

ステップＳ１２００において、基準位置姿勢設定部１１０は、ステップＳ１１００で設定された複数の基準位置姿勢の中から未処理の基準位置姿勢を１つ選択して設定する。ステップＳ１３００において、検出部１２０は、ステップＳ１２００で設定された基準位置姿勢において観測される三次元形状モデルのモデル幾何特徴を基準幾何特徴として検出する。即ち、検出部１２０は、ステップＳ１２００で設定された基準位置姿勢において観測される三次元形状モデルの距離画像を取得する。具体的には、検出部１２０は、当該基準位置姿勢に配置された仮想カメラの観測画像平面上に三次元形状モデルをレンダリングし、デプスバッファに格納されたデータから各画素に対する距離値を算出することで距離画像を得る。観測画像平面にレンダリングする際のカメラパラメータは、実際に位置姿勢算出で用いるカメラに合わせたものを取得して利用するのが望ましい。なお、距離画像は撮像画像面上の全画素について算出しなくてもよい。例えば、撮像画像上で垂直方向、水平方向に１０画素毎で等間隔に画素をサンプリングして、その画素の距離値を算出すればよい。このとき、三次元形状モデルを構成する各面パッチには、異なる固有の色をＩＤとして割り当てておき、割り当てられたＩＤの色で各面パッチを色分けしてレンダリングする。次に、検出部１２０は、得られた各画素の距離値に、画素位置に対応する視線ベクトルの成分を乗算することにより、基準座標系における三次元座標を持つ三次元点群データに変換する。この処理により三次元座標を算出した三次元点群を、基準幾何特徴とする。このとき、各基準幾何特徴が属する面パッチのＩＤを記録しておく。

図６（ａ）は、基準位置姿勢に配置された仮想カメラの観測画像平面上に三次元形状モデルが投影された状態を示す図である。例えば、図６（ａ）の例では、基準幾何特徴ａ〜ｆに対して面パッチＢを示すＩＤが記録される。なお、基準幾何特徴の取得方法はこれに限るものではない。例えば、三次元形状モデルの各面パッチ上から予め多数の三次元点をサンプリングしておき、これを利用してもよい。この方法では、サンプリングしておいた各三次元点を観測位置姿勢に基づいて観測画像平面上に投影し、各画素に投影された三次元点のうち、視点から見て最も手前に投影されたものを利用する。この場合、各三次元点には、三次元形状モデルを構成する各面パッチに割り当てたＩＤに基づき、その三次元点自身が属する面パッチのＩＤを記録しておく。なお、三次元形状モデルから基準位置姿勢において観測される三次元点を抽出できれば、この他のいかなる方法であってもよい。

ステップＳ１４００では、ステップＳ１３００で検出された各基準幾何特徴に対して対応付ける評価値が算出される。本ステップは、図３（ｂ）に示すステップＳ１４１０〜ステップＳ１４９０により構成される。以下、図３（ｂ）を参照しながら、ステップＳ１４００の詳細について説明する。

ステップＳ１４１０において、参照位置姿勢設定部１３０は、基準位置姿勢に対して、位置姿勢の変動を与えた位置姿勢を参照位置姿勢として複数設定する。参照位置姿勢は、位置姿勢推定時の初期値として与えられる概略位置姿勢に該当する。モデル作成装置１は、設定した参照位置姿勢の数をＮとして各基準幾何特徴に対してＮ回の対応付けを試行する。ここで、モデル作成装置１は、ステップＳ１３００で検出された基準幾何特徴の数をＭ、各基準幾何特徴ｉ（ｉ＝１〜Ｍ）に対して対応付けが成功した回数をカウントするカウンタをＣｉとし、Ｃｉ〜ＣＭを全て０にセットしておく。

ステップＳ１４２０において、参照位置姿勢設定部１３０は、ステップＳ１４１０で設定した複数の参照位置姿勢の中から未処理の参照位置姿勢を１つ選択して設定する。ステップＳ１４３０において、検出部１２０は、ステップＳ１３００で検出された複数の基準幾何特徴のうち、未処理の基準幾何特徴を１つ選択する。ここで選択された基準幾何特徴の識別番号をｉとする。

ステップＳ１４４０において、類似度算出部１４０は、参照位置姿勢における観測画像平面から、ステップＳ１４３０で選択された基準幾何特徴ｉに対応する画素（対応点）を探索する。オンラインの位置姿勢推定における対応点の探索では、三次元形状モデルの三次元点を撮像画像平面に投影し、投影された画像座標の三次元点を対応点とする。そこで、それをシミュレートするために、基準幾何特徴を参照位置姿勢における観測画像平面上に投影して基準幾何特徴に対応する画素（対応点）を求める。即ち、類似度算出部１４０は、ステップＳ１３００における基準幾何特徴の検出と同様の方法により、参照位置姿勢に配置された仮想カメラの観測画像平面上に三次元形状モデルをレンダリングする。このとき、類似度算出部１４０は、ステップＳ１３００で割り当てられたＩＤと同じＩＤを各面パッチに対して割り当ててレンダリングを行う。次に、類似度算出部１４０は、レンダリングを行った観測画像平面から、基準幾何特徴が検出された座標に対応する画素（対応点）を参照する。そして類似度位置算出部１４０は、この画素の属する面パッチの色のＩＤを、当該対応点のＩＤとして決定する。

ステップＳ１４５０において、類似度算出部１４０は、ステップＳ１３００で記録された基準幾何特徴のＩＤと、ステップＳ１４４０で決定された対応点のＩＤとを比較して、類似度を算出する。２つのＩＤが一致すれば、類似度算出部１４０は正対応であると判定して類似度１とし、ステップＳ１４６０へ進む。一方、２つのＩＤが一致しなければ、類似度算出部１４０は誤対応であると判定して類似度０とし、ステップＳ１４７０へ進む。図６（ｂ）は、参照位置姿勢に配置された仮想カメラの観測画像平面上に三次元形状モデルが投影された状態を示す図である。ステップＳ１３００において、図６（ａ）に示すように基準幾何特徴ａ、ｂ、ｃ、ｄ、ｅ、ｆには面パッチＢのＩＤが記録される。これに対して、図６（ｂ）に示すように、参照位置姿勢に配置された仮想カメラの観測画像平面上における各基準幾何特徴に対応する画素（対応点）のうち、対応点ａ、ｂ、ｃ、ｄには面パッチＢのＩＤが記録され、対応点ｅ、ｆには面パッチＣのＩＤが記録される。このような場合、基準幾何特徴ａ、ｂ、ｃ、ｄについては、正対応と判定されて類似度１と算出され、基準幾何特徴ｅ、ｆについては、誤対応と判定されて類似度０と算出される。

ステップＳ１４６０において、類似度算出部１４０は、基準幾何特徴ｉに対して対応付けが成功した回数をカウントするカウンタＣｉをＣｉ＝Ｃｉ＋１に更新する。ステップＳ１４７０において、類似度算出部１４０は、ステップＳ１４３０からステップＳ１４６０までの一連の処理が、Ｍ個の基準幾何特徴全てに対して行われたか否かを判定する。全基準幾何特徴に対して処理がなされていれば、処理はステップＳ１４８０へ進む。一方、未処理の基準幾何特徴がある場合、処理はステップＳ１４３０へ戻り、次の基準幾何特徴について処理が繰り返される。

ステップＳ１４８０において、類似度算出部１４０は、ステップＳ１４２０からステップＳ１４７０までの一連の処理を全参照位置姿勢について実行したか否かを判定する。全参照位置姿勢について実行された場合、処理はステップＳ１４９０に移行する。一方、未処理の参照位置姿勢がある場合、処理はステップＳ１４２０に戻り、次の参照位置姿勢について処理が繰り返される。つまり、ステップＳ１４２０からＳ１４８０までの処理は、参照位置姿勢の数Ｎだけ実行される。

ステップＳ１４９０において、評価値算出部１５０は、ステップＳ１３００で検出された全基準幾何特徴のそれぞれに対して対応付け試行をＮ回行ったうち、正対応と判定された試行が何回あったかにより評価値を算出する。具体的には、評価値算出部１５０は、各基準幾何特徴ｉに対する正対応のカウンタをＣｉとして、評価値Ｖｉを以下の数１により算出する。
Ｖｉ＝Ｃｉ／Ｎ・・・式１

ステップＳ１５００において、位置姿勢算出用モデル作成部１６０は、ステップＳ１３００で検出されたＭ個の基準幾何特徴とステップＳ１４００で算出されたＭ個の評価値とを組にする。そして、位置姿勢算出用モデル作成部１６０は、Ｍ個の基準幾何特徴と評価値との組みと該当する基準位置姿勢とを、当該基準位置姿勢における視点別位置姿勢算出用モデルとして保存する。

ステップＳ１６００において、モデル作成装置１は、ステップＳ１２００からＳ１５００までの処理を全基準位置姿勢について実行したか否かを判定する。全基準位置姿勢について処理が実行されていない場合、処理はステップＳ１２００に戻り、次の基準位置姿勢について処理が繰り返される。一方、全基準位置姿勢について処理が実行された場合、処理は終了する。この結果、設定された全ての基準位置姿勢について視点別位置姿勢算出用モデルが保存され、位置姿勢算出用モデルを得ることができる。

次に、図４を参照しながら、位置姿勢算出用モデルを利用した位置姿勢推定処理について説明する。図４は、位置姿勢推定処理を示すフローチャートである。尚、以下の位置姿勢推定処理は、図３による処理結果を外部装置に転送し、外部装置が行うようにしてもよい。

ステップＳ２０００において、位置姿勢算出部１８０は、物体の概略位置姿勢を入力する。位置姿勢算出部１８０には、位置姿勢計測の基準となる三次元の座標系（基準座標系）が規定されているものとする。位置姿勢算出部１８０に対する物体の位置姿勢とは、基準座標系における物体の位置姿勢を表す。本実施形態では、カメラ中心を原点とし、カメラの光軸をｚ軸とした座標系を基準座標系とする。また、本実施形態では、位置姿勢算出部１８０は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略位置姿勢として用いる。しかしながら、概略位置姿勢の入力方法はこれに限るものではない。例えば、過去の位置姿勢の計測結果を基に位置姿勢の変化量を推定し、過去の位置姿勢と推定された変化量から現在の位置姿勢を予測したものでもよい。また、物体が置かれているおおよその位置姿勢が予めわかっている場合にはその値を概略位置姿勢として用いてもよい。

ステップＳ２１００において、位置姿勢算出部１８０は、位置姿勢算出用モデルの中から、視点別位置姿勢算出用モデルを一つ選択して入力する。具体的には、位置姿勢算出部１８０は、位置姿勢算出用モデルとして保存されている複数の視点別位置姿勢算出用モデルのうち、ステップＳ２０００で入力された概略位置姿勢に最も類似した基準位置姿勢に対応する視点別位置姿勢算出用モデルを選択する。最も類似した基準位置姿勢の選択には、位置姿勢算出部１８０は、概略位置姿勢と基準位置姿勢との姿勢の差分をΔＲとして複数の基準位置姿勢に対してΔＲをそれぞれ算出し、この値が最も小さいものを選択する。或いは、位置姿勢算出部１８０は、概略位置姿勢と基準位置姿勢との位置の差分をΔＴ、姿勢の差分をΔＲとしてそれぞれ算出し、ΔＴ、ΔＲの値に関して全基準位置姿勢において順位付けを行い、各順位の和が最も小さいものを選択してもよい。

ステップＳ２２００において、撮像部１７０は、距離センサからの奥行き情報が格納された距離画像を撮影し、位置姿勢算出部２００に入力する。ステップＳ２３００において、位置姿勢算出部１８０は、ステップＳ２１００で入力された視点別位置姿勢算出用モデルと、ステップＳ２２００で入力された距離画像との対応を求める。ここで、位置姿勢算出部１８０は、視点別位置姿勢算出用モデルの中から評価値が低い基準幾何特徴を排除する。具体的には、位置姿勢算出部１８０は、評価値の閾値thresholdを予め設定しておき（例えば、threshold=0.3）、各基準幾何特徴の評価値Ｖｉを参照して、Ｖｉ＞thresholdとなる基準幾何特徴を選択する。このように、誤対応となりやすい基準幾何特徴を予め排除することで、位置姿勢推定への悪影響を軽減する。あるいは、閾値thresholdを高めに設定（例えば、threshold=0.8）することで、確実に正対応が得られるような基準幾何特徴のみ利用することができる。次に、位置姿勢算出部１８０は、選択された基準幾何特徴を距離画像平面に投影し、投影された位置に最も近い画素の距離値を距離画像から参照する。そして、位置姿勢算出部１８０は、画素の距離値に当該画素の位置に対応する視線ベクトルを乗算することにより、三次元点の座標を求める。ここで求められる三次元点が、距離画像上の幾何特徴に対応付けられる三次元形状モデル上のモデル幾何特徴（対応点）となる。

ステップＳ２４００において、位置姿勢算出部１８０は、ステップＳ２３００で求められた距離画像上の幾何特徴と、三次元形状モデル上の対応点との距離の和を非線型最適化手法により最小化することで物体の位置姿勢を算出する。即ち、位置姿勢算出部１８０は、距離画像上の幾何特徴と、三次元形状モデル上の対応点との対応を基に、物体の位置姿勢を算出するための観測方程式を立式する。基準座標系における三次元点群の三次元座標（ｘ´，ｙ´，ｚ´）は、物体の位置姿勢ｓによって三次元形状モデルの座標系における三次元座標（ｘ，ｙ，ｚ）に変換される。概略位置姿勢によって、三次元点群データ中のある点が物体座標に変換されるとする。（ｘ，ｙ，ｚ）は物体の位置姿勢により変化するものであり、（ｘ´，ｙ´，ｚ´）の近傍で１次のテイラー展開によって式２のように近似できる。

位置姿勢算出部１８０は、式２の観測方程式を対応付けが行われた全ての三次元点群について立式する。次に位置姿勢算出部１８０は、式２を基に位置姿勢の補正値を算出し、概略位置姿勢を補正する。式２はｓの各成分の微小変化Δｓ_i（ｉ＝１，２，・・・，６）についての方程式であるため、式３のようなΔｓ_iに関する線形連立方程式を立てることができる。

この連立方程式を解くことによって、位置姿勢の補正値Δｓ_iが得られる。位置姿勢算出部１８０は、補正値Δｓ_iを用いて概略位置姿勢を補正することによって最終的な物体の位置姿勢を算出する。ここで式３を式４のように表す。

位置姿勢算出部１８０は、式４の両辺に対して行列Ｊの一般化逆行列（Ｊ^T・Ｊ）^-1・Ｊ^Tを乗じることにより位置姿勢の補正値Δｓを求め、概略位置姿勢を補正する。なお、以上述べた実施形態では、位置姿勢の補正は一度しか行われないが、補正された概略位置姿勢を基に対応付け処理をやり直すことで、位置姿勢の補正を反復してもよい。

以上、第１の実施形態では、距離画像と三次元形状モデルとのモデルフィッティングにおいて、三次元形状モデルに対して予め対応付けの評価値を付加する方法について述べた。即ち、本実施形態では、モデルフィッティング時に想定される初期位置姿勢の誤差を与えて対応付けを多数シミュレートし、その成功率を各基準幾何特徴の評価値とした。この方法により、物体の位置姿勢算出時に、評価値の低い基準幾何特徴を使用しないことで誤対応の可能性を減らし、計測のロバスト化が可能となる。また、この方法では、位置姿勢推定に利用する基準幾何特徴の数が削減される。その結果、計算の高速化も同時に可能となる。

次に、本発明の第２の実施形態について説明する。第１の実施形態では、類似度判定部１４０は、基準幾何特徴が三次元形状モデル上で属している面パッチのＩＤと、参照位置姿勢の撮像画像平面上における対応点が三次元形状モデル上で属している面パッチのＩＤとが比較される。そして、両者のＩＤが一致するかしないかに応じて対応付けの正誤判定が行われる。これに対し、第２の実施形態では、正誤判定に面パッチのＩＤを利用せず、三次元形状モデルの三次元点と撮像画像の三次元点とを用いて面の類似度を算出して対応付けの正誤判定を行う。即ち、第２の実施形態では、基準幾何特徴と、その基準幾何特徴の三次元形状モデル面での法線ベクトルとを組にして記録しておく。そして、基準幾何特徴の座標及び法線ベクトルにより定義される面と、三次元形状モデル上で当該基準幾何特徴に対応付けられた面とにおける面間の距離及び法線方向の同一性が類似度として算出され、対応付けの正誤が決定される。

なお、第２の実施形態における基本的な構成は図１に示す構成と同様であるため説明を省略する。また、第２の実施形態における処理は、図３及び図４に示す処理とほぼ同様である。但し、第２の実施形態における図３（ａ）のステップＳ１４００に示す処理の詳細は、図７に示す処理となる。ここで、図７におけるステップＳ７４１０、Ｓ７４２０、Ｓ７４３０、Ｓ７４６０、Ｓ７４７０はそれぞれ、図３（ｂ）のステップＳ１４１０、Ｓ１４２０、Ｓ１４３０、Ｓ１４７０、Ｓ１４８０と同様であるため、説明を省略する。全体の処理を通して第１の実施形態と異なるのは、ステップＳ１３００、Ｓ７４４０、Ｓ７４５０、Ｓ７４８０である。以下では、これら第１の実施形態と異なるステップについて説明する。

先ず、ステップＳ１３００について説明する。第２の実施形態では、基準幾何特徴として検出された三次元点自身の属する面パッチのＩＤは付与しない。その代わりに、各基準幾何特徴における三次元形状モデルの法線ベクトルが算出され、基準幾何特徴の座標とともに組にして記録される。具体的には、観測画像平面のデプスバッファから第１の実施形態と同じ方法で基準幾何特徴が検出される。このとき、観測画像平面で隣接する基準幾何特徴をつなぐことで面パッチが構築され、基準幾何特徴を共有する面パッチの法線ベクトルの平均ベクトルが算出され、基準幾何特徴の座標と組にして記録される。或いは、三次元形状モデル面上から予め三次元点が多数サンプリングされ、この三次元点を観測画像平面に投影して基準幾何特徴が検出される場合、基準幾何特徴の座標とともに法線ベクトルも算出されて記録される。

ステップＳ７４４０においては、類似度算出部１４０は、参照位置姿勢における観測画像平面から、ステップＳ７４３０で選択された基準幾何特徴に対応する画素（対応点）を探索する。即ち、類似度算出部１４０は、ステップＳ１３００と同じ方法で観測画像平面に三次元形状モデルをレンダリングし、各画素のデプスバッファから算出される距離値を基にしてステップＳ１３００と同様の方法により三次元点の座標を算出する。ここで算出された三次元点を参照幾何特徴とする。さらに、類似度算出部１４０は、算出した参照幾何特徴から、隣接する参照幾何特徴を連結することで面パッチを構成し、参照幾何特徴を共有する面パッチの法線ベクトルの平均ベクトルを各参照幾何特徴に対応付けて記録しておく。なお、観測画像平面上で同一の二次元座標に相当する基準幾何特徴と参照幾何特徴とが対応付けられる。

ステップＳ７４５０においては、類似度算出部１４０は、ステップＳ７４４０で求めた基準幾何特徴と参照幾何特徴との対応の類似度を算出する。ここで、基準幾何特徴の座標をｐ１、ｐ１と組にしてステップＳ１３００で記録しておいた法線ベクトルをｎ１とする。また、参照幾何特徴の座標をｐ２、ｐ２と組にしてステップＳ７４４０で記録しておいた法線ベクトルｎ２とする。ここで、類似度算出部１４０は、点ｐ１を通り法線ベクトルｎ１に垂直な面から点ｐ２までの距離をｄｉｓｔ、法線ベクトルｎ１とｎ２とのなす角をθとして、この２つを類似度として算出しておく。

ステップＳ７４８０においては、評価値算出部１５０は、複数の参照位置姿勢に関して算出された各基準幾何特徴の類似度を統合して対応付けの評価値を算出する。即ち、評価値算出部１５０は、各基準幾何特徴に関して、それぞれの参照位置姿勢に対して算出されたｄｉｓｔの値の範囲が０〜１になるよう正規化し、同様にθについても正規化を行う。次に、評価値算出部１５０は、基準幾何特徴ｉに関して、以下の式５により評価値Ｖｉを算出する。
Ｖｉ＝（１−Σ（ｄｉｓｔ（ｊ）・θ（ｊ）））／Ｎ・・・・式５
ここで、ｄｉｓｔ（ｊ）・θ（ｊ）はそれぞれ、ｊ番目の参照位置姿勢において算出されたｄｉｓｔ、θであり、Σ（ｄｉｓｔ（ｊ）・θ（ｊ））は、各参照位置姿勢における類似度ｄｉｓｔ、θの積を全位置姿勢に渡って加算された和である。また、Ｎは、参照位置姿勢の数である。

但し、評価値算出の方法はこの限りではない。例えば、各基準幾何特徴に関して、各参照位置姿勢において算出されたｄｉｓｔ、θに対して、以下の式６、式７をともに満たす場合、その対応は正対応であると判定されて類似度を１、満たさない場合は類似度を０として評価値を算出してもよい。
ｄｉｓｔ＜ｔｈｒｅｓｈ１・・・式６
θ＜ｔｈｒｅｓｈ２・・・式７
ここで、ｔｈｒｅｓｈ１、ｔｈｒｅｓｈ２は閾値であり、予め適切な値を設定しておけばよい。なお、基準幾何特徴に対して、対応する参照幾何特徴との幾何的な類似度を算出する方法であれば、この他のいかなる方法であってもよい。例えば、式６及び式７の何れか一方を満たした場合、その対応は正対応であると判定してもよい。或いは、式６及び式７の何れかのみを利用して正対応を判定してもよい。

第１の実施形態では、基準幾何特徴が三次元形状モデル上で属している面パッチのＩＤと、参照位置姿勢の撮影画像平面上における対応点が三次元形状モデル上で属している面パッチのＩＤとが一致するかしないかにより対応の正誤を判定していた。従って、三次元形状モデルを構成する面パッチのパッチ分割方法に各基準幾何特徴の評価値が依存する。これに対し、第２の実施形態では、三次元形状モデル上における基準幾何特徴、参照幾何特徴の座標と法線ベクトルとに基づいて対応付けの正誤が判定されるため、三次元形状モデルを構成する面パッチの分割方法に依存せず、評価値を算出することが可能となる。

次に、本発明の第３の実施形態について説明する。第１の実施形態では、位置姿勢推定処理における対応付け方法として、三次元形状モデル上の基準幾何特徴を概略位置姿勢に基づいて距離画像平面に投影し、投影された基準幾何特徴に対応する画素の距離値から変換される三次元点と対応付ける方法をとった。そのため、基準幾何特徴の評価値の算出においても、位置姿勢推定処理時の対応付けを模擬して投影画像上での基準幾何特徴の対応点を探索した。これに対し、第３の実施形態では、位置姿勢推定処理時の対応付けにおいて、距離画像から得られた三次元点のうち、三次元形状モデル上の基準幾何特徴から最近傍にある三次元点を対応付ける。これに伴い、モデル作成処理においても、最近傍点の探索による対応点の探索を行って評価値を算出する。

なお、第３の実施形態においても基本的な構成は図１に示す構成と同様であるため説明を省略する。また、第３の実施形態における処理は、図３及び図４に示す処理とほぼ同様である。第３の実施形態における処理と図３及び図４に示す処理と異なるのは、ステップＳ１４４０、Ｓ１４５０、Ｓ２３００である。

ステップＳ１４４０において、類似度算出部１４０は、参照位置姿勢における観測画像平面から三次元点を検出する。この方法は、第１の実施形態におけるステップＳ１３００において、基準位置姿勢における観測画像平面から基準幾何特徴を検出したのと同じ方法により行う。本実施形態では、ステップＳ１４４０で検出される三次元点を参照幾何特徴とする。その後、参照幾何特徴の中から基準幾何特徴との空間的な距離の最も近い点を探索して対応点とする。

ステップＳ１４５０において、類似度算出部１４０は、対応付けられた基準幾何特徴と参照幾何特徴とを含む三次元形状モデル上の面パッチのＩＤが一致するかを比較し、面パッチのＩＤが一致すれば正対応と判定する。一方、面パッチのＩＤが一致しなければ、類似度算出部１４０は誤対応と判定する。或いは、第２の実施形態と同様に、基準幾何特徴と参照幾何特徴との座標と、それらの座標における面の法線ベクトルとに基づいて類似度を算出してもよい。

ステップＳ２３００において、位置姿勢算出部１８０は、距離画像から得た三次元点のうち、三次元形状モデル上の基準幾何特徴に最も近い点を対応点とする。なお、位置姿勢推定処理時の対応点の探索方法と、基準幾何特徴の評価値算出処理時の対応点の探索方法とが同様であれば、他の対応付け方法であってもよい。例えば、位置姿勢推定処理時の対応点の探索方法として、概略位置姿勢に配置した三次元形状モデル上の三次元点から、その三次元点の三次元形状モデル面での法線方向に探索を行うNormal Shootingと呼ばれる方法であってもよい。この場合、モデル作成においても、Normal Shootingによる対応探索を用いて評価値を算出する。

この対応付け方法に適したモデル作成は、基本的には第１の実施形態と同様の方法で行い、以下のステップを変更して行う。即ち、ステップＳ１４４０において、類似度算出部１４０は、参照位置姿勢における三次元形状モデルの観測画像平面から第２の実施形態のステップＳ７４４０と同様の方法により距離画像の三次元点群を得る。類似度算出部１４０は、この距離画像の三次元点群から、隣接画素の点を連結することで距離画像の平面パッチを生成する。次に類似度算出部１４０は、基準幾何特徴の各点から法線方向に、距離画像の平面パッチとの交点を探索する。この結果、基準幾何特徴の三次元座標と法線の組と、距離画像の平面パッチの座標と法線の組とが対応付く。ステップＳ１４５０において、類似度算出部１４０は、得られた座標と法線の対応を用いて、第２の実施形態と同様の方法で対応付けの正誤を判定する。

さらに、この三次元形状モデルを用いた位置姿勢推定も、基本的には第１の実施形態と同様の方法で行い、以下のステップを変更して行う。即ち、位置姿勢算出部１８０は、ステップＳ２２００で入力された距離画像に対して、隣接画素の計測点を結んで平面パッチを構築しておく。そして、ステップＳ２４００において、位置姿勢算出部１８０は、三次元形状モデル上の三次元点に対して法線方向に探索を行い、距離画像から構築したメッシュとの交点を対応付ける。これ以降の処理は、第１の実施形態と同様の方法で行えばよい。

なお、この他、位置姿勢推定時の対応探索方法に合わせて、モデル作成時の対応点の探索が同様の方法によりなされていれば、対応付け方法はこれらに限るものではない。

次に、第４の実施形態について説明する。第１の実施形態では、各基準幾何特徴の持つ評価値を参照し、この値が閾値よりも低い場合、その基準幾何特徴は対応が不正確である可能性が高いとして利用しなかった。これに対し、第４の実施形態では、評価値によって利用する基準幾何特徴を決めるのではなく、評価値に応じて、位置姿勢算出への寄与度を設定してもよい。この場合、ステップＳ２３００における閾値による基準幾何特徴の選択は行わない。その代わりに、ステップＳ２４００において、各基準幾何特徴の保持する評価値に基づき、評価値の大きい基準幾何特徴に対して寄与度が大きく設定され、評価値の小さい基準幾何特徴に対して寄与度が小さく設定される。これにより、正確な対応が得られる可能性が高い基準幾何特徴の寄与度を大きく設定する。具体的には、基準幾何特徴ｉに対応する重みをＶｉとし、式８のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外は全て０の正方行列であり、対角成分には選択された視点における各基準幾何特徴の評価値Ｖｉが入る。なお、重みを与える関数は、評価値の高い基準幾何特徴に対して大きな重み、低い基準幾何特徴に対して小さな重みを与える関数であればよい。この重み行列Ｗを用いて、式４を式９のように変形される。

式１０のように式９を解くことにより補正値Δｓが求められる。

なお、第１の実施形態と同様の方法により評価値に基づいて基準幾何特徴の選択が行われた後、選択された基準幾何特徴に対して評価値に基づく重み付けが行われてもよい。

次に、本発明の第５の実施形態について説明する。第１の実施形態では、距離画像とのフィッティングを行うための位置姿勢算出用モデルが作成され、位置姿勢算出用モデルを利用して位置姿勢推定処理が行われる方法について述べた。これに対し、第５の実施形態では、二次元画像とのフィッティングを行うための位置姿勢算出用モデルの作成、及び、作成した位置姿勢算出用モデルを利用した位置姿勢推定について述べる。

なお、第５の実施形態における、基準位置姿勢設定部１１０、参照位置姿勢設定部１３０、評価値算出部１５０については、第１の実施形態と同様であるため、これらについては説明を省略する。

本実施形態における三次元形状モデル入力部１００は、物体の三次元形状を表すモデルを入力する。本実施形態では、三次元形状モデルとして、各面パッチを三角形ポリゴンにより表現したポリゴンモデルを入力する。

検出部１２０は、基準位置姿勢において、三次元形状モデルを観察した画像から基準幾何特徴を検出する。本実施形態では、物体の輪郭として二次元画像から検出されるエッジ（境界線分）を基準幾何特徴として位置姿勢推定に利用する。具体的には、検出部１２０は、三次元形状モデルを構成する面パッチのエッジの中から、基準位置姿勢において三次元形状モデルを観測した場合に観測画像上で輪郭として観測されるものを検出する。ここで、検出部１２０は、観測画像平面上でエッジの線分が等間隔になるようエッジを三次元空間上で分割する。本実施形態では、この各分割点を制御点とし、制御点の三次元座標とエッジの方向を表す三次元ベクトルとを組にしたものを基準幾何特徴とする。

類似度算出部１４０は、検出部１２０で検出された基準幾何特徴に対応するエッジを、各参照位置姿勢における観測画像平面から探索して、基準幾何特徴との類似度を算出する。

評価値算出部１５０は、類似度算出部１４０により算出された複数の類似度に基づき、基準幾何特徴毎に対応付けの成功度合いを示す評価値を算出する。評価値は、設定された参照位置姿勢の数に対する、類似した対応が得られた参照位置姿勢の数の割合として算出する。

位置姿勢算出用モデル作成部１６０は、基準幾何特徴と、評価値算出部により算出された評価値とを組とし、さらに該当する基準位置姿勢とともに記録することで、視点別位置姿勢算出用モデルを作成する。

撮像部１７０は、物体の二次元画像を撮像する。位置姿勢算出部１８０は、位置姿勢算出用モデル作成部１６０により作成された位置姿勢算出用モデルを用いて、物体の位置姿勢を算出する。具体的には、撮像部１７０によって入力された二次元画像中のエッジに対して、位置姿勢算出用モデルのエッジを当てはめることによって、物体の位置姿勢を計測する。

続いて各処理について説明する。本実施形態において、基本的な処理は図３及び図４と同様である。即ち、ステップＳ１０００、Ｓ１１００、Ｓ１２００、Ｓ１４１０、Ｓ１４２０、Ｓ１４３０、Ｓ１４６０、Ｓ１４７０、Ｓ１４８０、Ｓ１４９０、Ｓ１５００、Ｓ２０００については、第１の実施形態と同様であるため、説明を省略する。

ステップＳ１３００において、検出部１２０は、三次元形状モデルを基準位置姿勢に配置して、基準幾何特徴を検出する。前述したように、本実施形態では、観測画像平面において輪郭線として観測される三次元形状モデルのエッジを検出する。そのために、検出部１２０は、三次元形状モデルを構成するポリゴンのエッジの中から、観測画像平面を境界面として、視線方向からの観測の可否が切り替わる線分を遮蔽輪郭として抽出する。例えば、線分Ｅが三次元形状モデル上のポリゴンＡ、ポリゴンＢが共有するエッジであるとする。このとき、仮想カメラの座標系を基準として、ポリゴンＡの法線ベクトルｎ１、ポリゴンＢの法線ベクトルｎ２、観測画像平面中心へ向かう視線ベクトルをｅとしたとき、検出部１２０は、以下の式１１の関係を満たす線分Ｅを遮蔽輪郭線分として検出する。
（ｎ１・ｅ）（ｎ２・ｅ）＜０・・・式１１

ここで、ｎ１・ｅ、ｎ２・ｅはそれぞれベクトルｎ１とベクトルｅの内積、ベクトルｎ２とベクトルｅの内積である。なお、三次元形状モデルの法線の向きはそれぞれ物体内領域から物体外領域へ向くよう統一されているものとする。

検出部１２０は、上述した関係を満たすエッジを三次元形状モデルから抽出した後、エッジを三次元空間で分割し、分割した点を制御点として設定する。このとき、検出部１２０は、基準位置姿勢において三次元形状モデルを観測した際に、エッジが観測画像平面上で等間隔（例えば、画像上での長さが４画素毎）になるように制御点を設定する。この制御点の三次元座標と、エッジの方向を示す三次元ベクトルとを組にしたものを基準幾何特徴とする。

ステップＳ１４４０において、類似度算出部１４０は、参照位置姿勢における観測画像平面から、基準幾何特徴に対応する幾何特徴（参照幾何特徴）を探索する。オンラインの位置姿勢推定における対応点の探索では、三次元形状モデルを撮像画像平面に投影し、投影された画像座標からエッジ方向に垂直に一定範囲で輝度勾配を検出することでエッジを探索する。そこで、類似度算出部１４０は、それをシミュレートするために、基準幾何特徴を観測画像平面上に投影して、対応する参照幾何特徴を求める。

先ず類似度算出部１４０は、三次元形状モデルを参照位置姿勢に配置して、ステップＳ１３００と同様の方法で参照幾何特徴を検出しておく。但し、ステップＳ１３００では、類似度算出部１４０は、投影されたエッジが観測画像平面上で４画素毎になるよう分割したのに対し、本ステップでは、観測画像平面上で１画素毎になるよう分割して参照幾何特徴を検出する。

次に、類似度算出部１４０は、参照幾何特徴を検出した観測画像平面に、基準幾何特徴（制御点の三次元座標、そのエッジ方向を示す三次元ベクトル）を投影して、基準幾何特徴の二次元座標とエッジ方向を示す二次元ベクトルを得る。観測画像平面にエッジを投影する際のカメラパラメータは、オンラインの位置姿勢算出で用いるカメラに合わせたものを取得して利用するのが望ましい。さらに、類似度算出部１４０は、観測画像平面に、参照幾何特徴の制御点の三次元座標を投影して、参照幾何特徴の二次元座標を得る。ここで、類似度算出部１４０は、基準幾何特徴の二次元座標から、二次元ベクトルに垂直な方向で一定範囲（例えば、５０画素）の探索を行い、その範囲内に投影された参照幾何特徴が存在するか否かを探索する。なお、一定範囲内に複数の参照幾何特徴が存在する場合があるが、本実施形態では、基準幾何特徴の制御点と最も距離の近い制御点を持つものを参照幾何特徴とする。

さらに、類似度算出部１４０は、基準幾何特徴と参照幾何特徴との対応に対し、２つの幾何特徴が同一輪郭線分上にあるか否か、２つの幾何特徴のなす角が類似しているか否か、２つの幾何特徴の類似度を算出する。基準幾何特徴の制御点の三次元座標をｐｏｓＡ、方向を示す三次元ベクトルをｄｉｒＡと表し、対応する参照幾何特徴の制御点の三次元座標をｐｏｓＢ、方向を示す三次元ベクトルをｄｉｒＢと表す。このとき、以下の式１２及び式１３の関係をともに満たす場合、類似度算出部１４０は、基準幾何特徴と参照幾何特徴との対応は正対応であるとして類似度１、満たさなければ誤対応であるとして類似度０とする。

ここで、ｎｏｒｍａｌ（ｘ）は、ベクトルｘの法線ベクトルを、ドット（・）は内積を、｜｜ｘ｜｜はｘの絶対値をそれぞれ示すものとする。また、（ｐｏｓＢ−ｐｏｓＡ）は、座標Ａから座標Ｂへ向かうベクトルを長さ１に正規化したものである。なお、ｔｈｒｅｓｈ３、ｔｈｒｅｓｈ４はともに閾値であり、例えば、それぞれ１に近い値を設定する。但し、エッジの組に関して幾何的な類似度を出すことができれば、類似度算出方法はこの他の方法であっても構わない。例えば、式１２又は式１３の類似度だけで判定を行ってもよい。

ステップＳ１５００において、位置姿勢算出用モデル作成部１６０は、ステップＳ１３００で検出された基準幾何特徴とステップＳ１４００で算出された評価値とを組にする。さらに、位置姿勢算出用モデル作成部１６０は、当該組を、ステップＳ１２００で選択された基準位置姿勢とともに保存して、その基準位置姿勢における視点別位置姿勢算出用モデルを得る。以上の処理が設定された全ての基準位置姿勢について繰り返されることにより、全基準位置視線に関する視点別位置姿勢算出用モデルが位置姿勢算出用モデルとして得られる。

以上、ステップＳ１０００〜ステップＳ１６００の処理をモデル作成処理とする。続いて、位置姿勢算出用モデルを利用した位置姿勢推定処理について説明する。

ステップＳ２１００において、位置姿勢算出部１８０は、モデル作成処理により作成された位置姿勢算出用モデルを入力する。ステップＳ２２００において、位置姿勢算出部１８０は、撮像部１７０によって撮像された二次元画像を入力する。

ステップＳ２３００において、位置姿勢算出部１８０は、ステップＳ２１００で入力された位置姿勢算出用モデルと、ステップＳ２２００で入力された二次元画像上で観測されるエッジとを対応付ける。そのために、位置姿勢算出部１８０は、位置姿勢算出用モデルから、評価値が閾値以上の基準幾何特徴に対応する視点別位置姿勢算出用モデルを選択する。その後、位置姿勢算出部１８０は、選択した視点別位置姿勢算出用モデルを観測画像平面上に投影して、二次元座標と二次元ベクトルとを得る。次に位置姿勢算出部１８０は、得られた二次元座標から、二次元ベクトルの法線方向に一定範囲内で濃度勾配の極値を検出することで、二次元画像上で対応するエッジの座標を検出する。なお、ここで検出されるエッジは画素値の濃度勾配の極値として検出されるため、近傍に複数のエッジが検出されることがある。本実施形態では、検出されるエッジのうち、投影された制御点に対して最も距離の近いものを対応するエッジとする。

ステップＳ２４００において、位置姿勢算出部１８０は、ステップＳ２３００で求めた位置姿勢算出用モデルと二次元画像との対応を基に、物体の位置姿勢を算出する観測方程式を立式し、物体の位置姿勢の補正を行う。これには、例えば、Vacchetti らの方法（L. Vacchetti, V. Lepetit, and P.Fua,“Combining edge and texture information for real-time accurate 3D damera tracking", Proc.3^rd IEEE / ACM International Symposium on Mixed and Augmented Reality (ISMAR'04), pp.48-57, 2004.）を利用して行う。なお、基準幾何特徴を選択して用いるのではなく、全て基準幾何特徴を用いて、評価値に基づいて位置姿勢算出の際の寄与度を重み付けしてもよい。或いは、基準幾何特徴の選択を行った後、評価値に基づいて重み付けを行ってもよい。

以上、第５の実施形態では、二次元画像とのモデルフィッティングにおいて、三次元形状モデルに対して予め対応付けの正確さをシミュレートした評価値を付加しておく。そして、評価値に基づき正確に対応付けできる可能性の高い基準幾何特徴を選択して、物体の位置姿勢を算出するようにしている。これにより、誤対応の影響を軽減し、計測のロバスト化が可能となる。

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

１：モデル作成装置、２：三次元計測装置、１００：三次元形状モデル入力部、１１０：基準位置姿勢設定部、１２０：検出部、１３０：参照位置姿勢設定部、１４０：類似度算出部、１５０：評価値算出部、１６０：位置姿勢算出用モデル作成部、１７０：撮像部、１８０：位置姿勢算出部

Claims (11)

1. 物体の三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を基準位置姿勢として設定する第１の設定手段と、
   前記基準位置姿勢において視点から観測される前記三次元形状モデル上の幾何特徴を基準幾何特徴として検出する検出手段と、
   前記基準位置姿勢とは異なる、前記三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を参照位置姿勢として設定する第２の設定手段と、
   前記参照位置姿勢において視点から観測される前記三次元形状モデル上の前記基準幾何特徴に対応する幾何特徴である参照幾何特徴を探索する探索手段と、
   前記基準幾何特徴と前記参照幾何特徴との類似度を算出する第１の算出手段と、
   前記第１の算出手段により算出される類似度に基づいて、前記基準幾何特徴と前記参照幾何特徴との対応付けに係る評価値を算出する第２の算出手段とを有することを特徴とする情報処理装置。
2. 前記評価値に基づく前記基準幾何特徴と、撮像手段により撮像された前記物体の画像データ上の幾何特徴とのフィッティングにより、前記物体の位置及び姿勢を算出する第３の算出手段を有することを特徴とする請求項１に記載の情報処理装置。
3. 前記第２の設定手段は、複数の参照位置姿勢を設定し、前記第１の算出手段は、前記複数の参照位置姿勢の夫々について、前記基準幾何特徴と前記参照幾何特徴との類似度を算出し、前記第２の算出手段は、前記第１の算出手段により算出された複数の類似度に基づいて、前記各基準幾何特徴について、前記基準幾何特徴と前記参照幾何特徴との対応付けに係る評価値を算出することを特徴とする請求項２に記載の情報処理装置。
4. 前記第２の算出手段は、前記第１の算出手段により算出された複数の類似度に基づいて、前記複数の参照位置姿勢の数に対する、前記基準幾何特徴と前記参照幾何特徴との対応付けが正しいと判定される数の割合を前記評価値として算出することを特徴とする請求項３に記載の情報処理装置。
5. 前記基準幾何特徴は、前記三次元形状モデル上の点、又は、前記三次元形状モデル上の方向を持つ点であることを特徴とする請求項１乃至４の何れか１項に記載の情報処理装置。
6. 前記第１の算出手段は、前記基準幾何特徴が属する前記三次元形状モデルの面パッチと、前記参照幾何特徴が属する前記三次元形状モデルの面パッチとの同一性に基づいて、前記基準幾何特徴と前記参照幾何特徴との類似度を算出することを特徴とする請求項１乃至５の何れか１項に記載の情報処理装置。
7. 前記第１の算出手段は、前記三次元形状モデル上における前記基準幾何特徴の位置及び方向と、前記三次元形状モデル上における前記参照幾何特徴の位置及び方向との同一性に基づいて、前記基準幾何特徴と前記参照幾何特徴との類似度を算出することを特徴とする請求項１乃至５の何れか１項に記載の情報処理装置。
8. 前記第３の算出手段は、所定の値以上の前記評価値に対応する前記基準幾何特徴を選択し、選択した前記基準幾何特徴と、前記物体の画像データ上の幾何特徴とのフィッティングにより、前記物体の位置及び姿勢を算出することを特徴とする請求項２乃至７の何れか１項に記載の情報処理装置。
9. 前記第３の算出手段は、前記評価値に対応する重み付けを前記基準幾何特徴に対して設定し、前記重み付けが設定された前記基準幾何特徴と、前記物体の画像データ上の幾何特徴とのフィッティングにより、前記物体の位置及び姿勢を算出することを特徴とする請求項２乃至６の何れか１項に記載の情報処理装置。
10. 情報処理装置によって実行される情報処理方法であって、
    物体の三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を基準位置姿勢として設定する第１の設定ステップと、
    前記基準位置姿勢において視点から観測される前記三次元形状モデル上の幾何特徴を基準幾何特徴として検出する検出ステップと、
    前記基準位置姿勢とは異なる、前記三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を参照位置姿勢として設定する第２の設定ステップと、
    前記参照位置姿勢において視点から観測される前記三次元形状モデル上の前記基準幾何特徴に対応する幾何特徴である参照幾何特徴を探索する探索ステップと、
    前記基準幾何特徴と前記参照幾何特徴との類似度を算出する第１の算出ステップと、
    前記第１の算出ステップにより算出される類似度に基づいて、前記基準幾何特徴と前記参照幾何特徴との対応付けに係る評価値を算出する第２の算出ステップとを含むことを特徴とする情報処理方法。
11. 物体の三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を基準位置姿勢として設定する第１の設定ステップと、
    前記基準位置姿勢において視点から観測される前記三次元形状モデル上の幾何特徴を基準幾何特徴として検出する検出ステップと、
    前記基準位置姿勢とは異なる、前記三次元形状モデルと前記三次元形状モデルを観測する視点との相対的な位置及び姿勢の関係を参照位置姿勢として設定する第２の設定ステップと、
    前記参照位置姿勢において視点から観測される前記三次元形状モデル上の前記基準幾何特徴に対応する幾何特徴である参照幾何特徴を探索する探索ステップと、
    前記基準幾何特徴と前記参照幾何特徴との類似度を算出する第１の算出ステップと、
    前記第１の算出ステップにより算出される類似度に基づいて、前記基準幾何特徴と前記参照幾何特徴との対応付けに係る評価値を算出する第２の算出ステップとをコンピュータに実行させるためのプログラム。

Images