(57)【特許請求の範囲】
【請求項１】
計測対象物体の３次元形状モデルを入力する第１入力手段と、
前記３次元形状モデルを構成する曲面パッチに対応する２次元のパラメータ平面を作成する作成手段と、
前記曲面パッチ上の点の法線ベクトルを算出する第１算出手段と、
前記パラメータ平面と前記法線ベクトルとを対応づけて保持する保持手段と、
前記計測対象物体を観測位置から観測する観測方向ベクトルを入力する第２入力手段と、
前記法線ベクトルと前記観測方向ベクトルとが予め定められた角度条件となる領域を、  前記パラメータ平面から選択する選択手段と、
前記選択手段により選択された領域に対応する３次元形状モデル上の座標を、前記観測位置から観測可能な３次元形状モデル上の幾何特徴領域を構成する座標として算出する第２算出手段と、
を備えることを特徴とする幾何特徴抽出装置。
【請求項２】
前記角度条件は、前記法線ベクトルと前記観測方向ベクトルとの内積が閾値より小さいことを条件とすることを特徴とする請求項１に記載の幾何特徴抽出装置。
【請求項３】
前記保持手段は、前記３次元形状モデルの点の法線とともに、前記点の座標も保持することを特徴とする請求項１又は２に記載の幾何特徴抽出装置。
【請求項４】
前記第２算出手段は、前記選択手段により選択された領域における表面の曲率を算出することを特徴とする請求項１乃至３の何れか１項に記載の幾何特徴抽出装置。
【請求項５】
請求項１乃至４の何れか１項に記載の幾何特徴抽出装置を有することを特徴とする三次元計測装置。
【請求項６】
請求項１乃至４の何れか１項に記載の幾何特徴抽出装置を有することを特徴とする物体認識装置。
【請求項７】
第１入力手段が、計測対象物体の３次元形状モデルを入力する第１入力工程と、
作成手段が、前記３次元形状モデルを構成する曲面パッチに対応する２次元のパラメータ平面を作成する作成工程と、
第１算出手段が、前記曲面パッチ上の点の法線ベクトルを算出する第１算出工程と、
保持手段が、前記パラメータ平面と前記法線ベクトルとを対応づけて保持する保持工程と、
第２入力手段が、前記計測対象物体を観測位置から観測する観測方向ベクトルを入力する第２入力工程と、
選択手段が、前記法線ベクトルと前記観測方向ベクトルとが予め定められた角度条件となる領域を、前記パラメータ平面から選択する選択工程と、
第２算出手段が、前記選択工程により選択された領域に対応する３次元形状モデル上の座標を、前記観測位置から観測可能な３次元形状モデル上の幾何特徴領域を構成する座標として算出する第２算出工程と、
を備えることを特徴とする幾何特徴抽出方法。
【請求項８】
請求項７に記載の幾何特徴抽出方法をコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、観測方向に依存したパラメトリック形状モデル等からの幾何特徴抽出装置、幾何特徴抽出方法、及びプログラム、三次元計測装置、物体認識装置に関する。
【背景技術】
【０００２】
濃淡画像や距離画像に含まれる計測対象物体の像から認識や位置姿勢計測を行う方法としてＭｏｄｅｌ−Ｂａｓｅｄ Ｖｉｓｉｏｎがある。距離画像とは、各画素が奥行きの情報を持つ画像である。この方法は、撮像画像から検出される幾何特徴と、計測対象物体の３次元形状モデルから抽出される幾何特徴との対応を求め、その対応間の距離を評価関数として、認識や位置姿勢計測のパラメータを推定するものである。この技術ではあらかじめ計測対象物体の３次元形状モデルが必要となる。この３次元形状モデルとして、ＣＡＤソフトによる設計データを流用する方法はＣＡＤ−Ｂａｓｅｄ Ｖｉｓｉｏｎと呼ばれる。
【０００３】
一般的に、物体の形状を表すＣＡＤデータは、パラメトリック形状モデルにより表現されている場合が多い。パラメトリック形状モデルとは、Ｂ−Ｓｐｌｉｎｅ曲面、ＮＵＲＢＳ（Ｎｏｎ−Ｕｎｉｆｏｒｍ Ｒａｔｉｏｎａｌ Ｂ−Ｓｐｌｉｎｅ）曲面などのパラメトリック曲面を複数組み合わせて形状を表現するモデルである。パラメトリック曲面は、基底関数とパラメータとの組み合わせによって曲面を定義するため、少ない情報量で複雑な曲面形状を表現できるという長所を持つ。従って、物体の認識や位置姿勢計測に、設計時の形状情報をコンパクトに保持するパラメトリック形状モデルを用いれば、モデル上の幾何特徴を高精度に算出できるため、処理を精度よく行うことが可能となる。
【０００４】
例えば、非特許文献１では、パラメトリック形状モデルを用いて撮像した距離画像から対象物体の位置および姿勢を推定する方法として、Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ（ＩＣＰ）アルゴリズムが開示されている。具体的には、距離画像から変換された３次元点群データに物体の３次元形状モデルを当てはめることにより物体の位置および姿勢を推定する。位置および姿勢の概略値に基づいて各３次元点に最も近い形状モデルの幾何特徴の探索を行い、点と形状モデルの幾何特徴との距離の総和が最小となるような位置および姿勢の更新を繰り返し行うという方法である。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】米国特許第6504957号公報
【特許文献２】特開2002-178576号公報
【非特許文献】
【０００６】
【非特許文献１】P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.14, no.2, pp.239-256, 1992.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
一般的に、パラメトリック形状モデルは、複数のパラメータによる複雑な幾何演算や積和演算を必要とするため、曲面上の幾何特徴を逐次探索する場合の計算に時間を要する。そのため、パラメトリック形状モデルと撮像画像上との対応を求める際に処理時間が大きくなることが課題となる。例えば、前述したパラメトリック形状モデルを利用した位置あわせ手法（非特許文献１参照）では、距離画像から変換された３次元点群とパラメトリック形状モデルとで幾何特徴の対応を求める。その際に、パラメトリック曲面上の幾何特徴を逐次探索する必要がある。この幾何特徴を逐次探索する際に、パラメトリック曲面上の点の座標や法線を計算するための処理時間が長くなる。従って、特に撮像画像との対応を利用して３次元計測を行うようなリアルタイムでの処理における応答時間に影響を及ぼすことになる。
【０００８】
また特許文献１には、撮像画像中の幾何特徴が検出されやすい領域を形状モデル上からあらかじめ抽出する方法が開示されている。この方法では、撮像画像で計測が安定しやすい領域として、形状モデル上で曲率の小さい領域を幾何特徴として抽出する。しかし、この方法では、計測対象物体に対する撮像装置の位置および姿勢を考慮していない。形状モデルと撮像画像とで対応付けられるエッジや面などの幾何特徴は、計測対象物体の観測方向、すなわち計測対象物体に対する撮像装置の位置および姿勢によって大きく変化する。そのため、特許文献１の方法では、撮像装置が計測対象物体に対して傾いた位置にある場合、抽出した幾何特徴の精度が悪くなってしまうという課題がある。
【０００９】
また、特許文献２では、パラメトリック形状モデルを単純な等価曲面にパラメータ変換して、モデル上の幾何特徴の算出を高速化する手法が開示されている。しかし、この方法は、パラメトリック形状モデルを高速に描画することを目的としており、計測点との対応を求めるためには、結局逐次探索処理が必要となる。
【００１０】
上記の課題に鑑み、本発明は、パラメトリック形状モデルを用いた物体の認識や位置および姿勢計測において、撮像画像との対応関係を利用するための幾何特徴を物体の観測方向に基づいて効率的に求めることを目的とする。
【課題を解決するための手段】
【００１１】
上記の目的を達成する本発明に係る幾何特徴抽出装置は、
計測対象物体の３次元形状モデルを入力する第１入力手段と、
前記３次元形状モデルを構成する曲面パッチに対応する２次元のパラメータ平面を作成する作成手段と、
前記曲面パッチ上の点の法線ベクトルを算出する第１算出手段と、
前記パラメータ平面と前記法線ベクトルとを対応づけて保持する保持手段と、
前記計測対象物体を観測位置から観測する観測方向ベクトルを入力する第２入力手段と、
前記法線ベクトルと前記観測方向ベクトルとが予め定められた角度条件となる領域を、  前記パラメータ平面から選択する選択手段と、
前記選択手段により選択された領域に対応する３次元形状モデル上の座標を、前記観測位置から観測可能な３次元形状モデル上の幾何特徴領域を構成する座標として算出する第２算出手段と、
を備えることを特徴とする。
【発明の効果】
【００１２】
本発明によれば、パラメトリック形状モデルに対し、観測方向に応じて幾何特徴を高速に抽出することが可能になる。
【図面の簡単な説明】
【００１３】
【図１】第１実施形態に係る幾何特徴抽出処理部１００および三次元計測装置１０９の構成図。
【図２】第１実施形態に係るパラメトリック形状モデルからの幾何特徴抽出の処理手順を示すフローチャート。
【図３】（ａ）第１実施形態に係る法線マップ作成の処理手順を示すフローチャート、（ｂ）第１実施形態に係る幾何特徴抽出の処理手順を示すフローチャート。
【図４】第１実施形態に係るパラメトリック形状モデルを構成する曲面パッチの図。
【図５】第１実施形態に係るパラメトリック形状モデルを構成する曲面パッチに対応するｕｖパラメータ空間および法線マップの図。
【図６】第１実施形態に係る幾何特徴の判定に関する図。
【図７】第２実施形態に係る幾何特徴抽出処理部７００および物体認識装置７０９の構成図。
【図８】第２実施形態に係る幾何特徴抽出の処理手順を示すフローチャート。
【発明を実施するための形態】
【００１４】
（第１実施形態）
第１実施形態では、まず３次元形状モデル、特にパラメトリック形状モデルの各曲面パッチに対して、対応する法線マップを作成しておく。そして、観察位置からの計測対象物体の観測方向と法線マップとに基づいて、モデル上の幾何特徴を高速に抽出し、抽出した幾何特徴と撮像した距離画像とを対応付けることで物体の位置姿勢を計測する場合について説明する。
【００１５】
図１を参照して、第１実施形態に係る幾何特徴抽出処理部１００および三次元計測装置１０９の構成について説明する。図１に示すように、幾何特徴抽出処理部１００は、形状モデル入力部１０１と、法線情報保持部１０２と、観測方向入力部１０３と、特徴領域選択部１０４と、幾何特徴算出部１０５とを備える。また、三次元計測装置１０９は、幾何特徴抽出処理部１００と、画像入力部１０６と、対応探索部１０７と、位置姿勢算出部１０８とを備える。各処理部は不図示のＣＰＵ等により演算が実行される。以下、幾何特徴抽出処理部１００および三次元計測装置１０９を構成する各処理部について説明する。
【００１６】
第１入力手段として機能する形状モデル入力部１０１では、計測対象物体の形状を表すパラメトリック表現のモデルを入力する。ここで、パラメトリック表現とは、ｕｖ平面内の領域を動く２つのパラメータをｕ,ｖとして、３次元空間の点Ｐの座標(ｘ,ｙ,ｚ)を３つの連続関数ｘ＝ｆ（ｕ,ｖ）、ｙ＝ｇ（ｕ,ｖ）、ｚ＝ｈ（ｕ,ｖ）によって表現する方法である。図４（ａ）に示すように、パラメトリック形状モデルを構成する曲面パッチ４００は、ベースとなるパラメトリック曲面４０１とｕｖ曲線４０２とによって定義される。ここでｕｖ曲線４０２とは、パラメトリック曲面４０１において有効な領域をｕｖ平面上の輪郭で表現するものである。本実施形態では、ベースとなるパラメトリック曲面４０１としてＢ−Ｓｐｌｉｎｅ曲面を用いる。
【００１７】
各Ｂ−Ｓｐｌｉｎｅ曲面はｕｖ平面に対応する碁盤の目状に配置されたｎ×ｍ個の制御点列と、ｕ方向（ｎ＋ｐ個）及びｖ方向（ｍ＋ｑ個）のノット列とによって定義される。例えば図４（ｂ）に示すように、ｐ１、ｐ２、…、ｐ８は、図４（ａ）の曲面パッチ４００のベース曲面であるパラメトリック曲面４０１（Ｂ−ｓｐｌｉｎｅ曲面）を定義する制御点を示している。しかし、ベースとなるパラメトリック曲面４０１は、２次元パラメータ（ｕ,ｖ）から３次元座標（ｘ,ｙ,ｚ）への写像を行う曲面であればよい。すなわちパラメトリック曲面４０１は、ベジェ曲面であっても、ＮＵＲＢＳ（Ｎｏｎ−Ｕｎｉｆｏｒｍ Ｒａｔｉｏｎａｌ Ｂ−Ｓｐｌｉｎｅ）曲面であっても、解析曲面であってもよい。また、ポリゴンモデルを上述したパラメトリック曲面４０１へ変換したものであってもよい。さらに、このベース曲面であるパラメトリック曲面４０１に対して、曲面上で有効となる領域をｕｖ平面により定義するｕｖ曲線４０２を用いて一つの曲面パッチ４００を表現する。ｕｖ曲線４０２は方向を持つ一本の閉曲線により構成され、ｕｖ平面上を閉曲線に沿って進んだときに左側に存在する領域を曲面上で有効な領域とする。
【００１８】
作成手段、第１算出手段、及び保持手段として機能する法線情報保持部１０２は、パラメトリック形状モデルを構成する各曲面パッチについて、対応する法線マップ画像を作成する。より具体的には、各画素に対して３値を保持するような２次元画像を作成し、この画像を各曲面パッチに対応するｕ,ｖのパラメータ空間（パラメータ平面）とみなす。そして、画像上の各画素（ｕ,ｖ）に対して３次元パラメトリック曲面上の点の法線（ｎｘ, ｎｙ, ｎｚ）Ｔを算出する。さらに、求めた法線（ｎｘ, ｎｙ, ｎｚ）Ｔの３値をｕ,ｖの２次元画像上の対応する画素（ｕ,ｖ）に対して保持することで法線マップ画像を作成する。ここで作成した各曲面の法線マップ画像は、特徴領域選択部１０４において、観測画像上で有効な幾何特徴となる領域を選択する際に用いられる。あらかじめ曲面パッチの３次元法線ベクトルを、２次元の法線マップに集約して作成しておくことでランタイムでの法線の算出を省くことが可能となる。
【００１９】
第２入力手段として機能する観測方向入力部１０３は、計測対象物体を撮像する撮像装置の計測対象物体に対する方向を入力する。ここで、撮像装置の計測対象物体に対する方向とは、撮像装置の座標系を基準とした場合の計測対象物体の概略位置姿勢を表す。本実施形態では、ここで入力される概略位置姿勢を手がかりとして、抽出したモデル上の幾何特徴を利用して、撮像装置を基準とした場合の計測対象物体の位置姿勢を高精度に計測する。従って、概略位置姿勢は撮像装置に対する計測対象物体のおおよその位置姿勢であればよい。なお、位置姿勢の計測は時間軸方向に連続して行うものとし、前回（前時刻）の概略位置姿勢の計測結果を用いて現在時刻の計測物体の観測方向として用いる。しかしながら、計測対象物体の観測方向の入力方法はこれに限るものではない。例えば、過去の計測値から物体の運動の速度や角速度を線形フィルタやカルマンフィルタなど時系列フィルタリング処理により推定し、推定された速度や角速度によって現在の時刻における物体の計測物体の観測方向を予測したものを用いてもよい。また、他のセンサによる計測対象物体の観測方向の計測が可能である場合には、他のセンサによる出力値を計測対象物体の観測方向として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより姿勢を計測する磁気式センサであってもよい。その他、物体の概略位置姿勢を計測するセンサであればいかなるセンサであってもよい。また、計測対象物体の置かれているおおよその位置姿勢があらかじめわかっている場合には、その値を用いてもよい。あるいは、計測対象物体を含む情景を撮影した画像や、距離画像から物体認識技術を利用して計測対象物体の観測方向を推定してもよい。観測方向入力部１０３により入力された計測対象物体の観測方向は、特徴領域選択部１０４で用いられる。
【００２０】
選択手段として機能する特徴領域選択部１０４は、観測方向入力部１０３により入力された観測位置からの観測方向より求まる視線ベクトル（観測方向ベクトル）と、法線情報保持部１０２の保持する各曲面パッチの法線とを比較する。そして、観測画像上で可観測となるモデル上の特徴領域をｕｖ平面において判定する。
【００２１】
第２算出手段として機能する幾何特徴算出部１０５は、特徴領域選択部１０４により選択されたｕｖ平面の領域に対して、対応するパラメトリック曲面の幾何特徴を算出する。具体的には、２次元パラメータ（ｕ,ｖ）から対応する曲面上の点の３次元座標（ｘ,ｙ,ｚ）を３次元形状モデル上の幾何特徴領域を構成する座標として算出する。あるいは、２次元パラメータ（ｕ,ｖ）から３次元座標（ｘ,ｙ,ｚ）をあらかじめ算出したものをテーブルとして保持し、テーブルを参照してもよい。
【００２２】
上述における各処理部が、幾何特徴抽出に関する処理を行う。本実施形態における各処理部を用いて実際の距離画像から物体の位置姿勢を求めるために、画像入力部１０６、対応探索部１０７、位置姿勢算出部１０８が用いられる。
【００２３】
画像入力部１０６は、距離画像を撮像する距離センサである。距離画像とは、各画素が奥行きの情報を持つ画像である。距離センサとしては、対象に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。
【００２４】
対応探索部１０７は、画像入力部１０６により入力された画像上の幾何特徴と、幾何特徴算出部１０５で算出した幾何特徴との対応付けを行う。具体的な幾何特徴の対応付け方法としては、例えば、[Chen, Y. and Medioni, G. “Object Modeling by Registration of Multiple Range Images,” Proc. IEEE Conf. on Robotics and Automation,1991.]に紹介されているような手法がある。この方法により、算出した点から法線方向に探索を行い、距離画像の計測点を連結したメッシュとの交点を求め、交点を対応付けてもよい。あるいは、概略位置姿勢に基づいて算出した幾何特徴を投影し、距離画像上で、計測点と同じ画素に投影されたものを対応付けてもよい。あるいは、算出した点に対し、法線方向に３次元空間上を探索し、距離画像のメッシュとの交差判定を行って、交差したメッシュとの対応を利用してもよい。
【００２５】
位置姿勢算出部１０８は、対応探索部１０７において対応付けられた形状モデルの幾何特徴と距離画像上の幾何特徴との組を用いて、計測対象物体の位置姿勢を推定する。計測対称物体の位置姿勢の算出方法としては、繰り返し最適化により、対応付けられた形状モデルの幾何特徴と距離画像上の幾何特徴との組の３次元空間中での位置の差異を最小化することによって行う。なお、ここで述べた画像入力部１０６、対応探索部１０７、位置姿勢算出部１０８は一例であって、距離画像とモデルを利用する位置あわせ処理との組み合わせであっても構わない。
【００２６】
本発明に係る幾何特徴抽出装置により、観測方向に則した幾何特徴を高速に抽出できるため、位置姿勢計測も高速に行うことが可能となる。
次に、図２を参照して、第１実施形態において、計測対象物体のパラメトリック形状モデルから幾何特徴を抽出する処理手順を示すフローチャートについて説明する。
【００２７】
ステップＳ２０１において、形状モデル入力部１０１により計測対象物体のパラメトリック形状モデルを入力する。本実施形態において、モデルは複数のＢ−Ｓｐｌｉｎｅ曲面パッチにより表されるものとする。
【００２８】
ステップＳ２０２において、パラメトリック形状モデルの各曲面パッチに対して法線マップ画像を作成する。ここで、図３（ａ）はステップＳ２０２における法線マップ作成の処理手順を説明するフローチャートである。以下、各処理について説明する。
【００２９】
ステップＳ３０１において、各曲面パッチに対応したｕｖ平面を表現する画像をそれぞれ作成する。このとき、各曲面パッチのモデル上でのスケールに応じて、モデル上で面積が大きいものは高解像度で、小さいものは低解像度でｕｖ平面を作成することにより、モデル上での幾何特徴の密度が均一になるようにする。より具体的には、各曲面パッチを表すｕｖのパラメータ空間が０≦ｕ≦１、０≦ｖ≦１に正規化されているものとする。例えば、図４（ａ）のベース曲面（パラメトリック曲面４０１）を定義する図４（ｂ）の制御点に対し、ｕｖ平面の四隅の点が対応するとする。すなわち、（ｕ,ｖ）＝（０,０）、（０,１）、（１,１）、（１,０）に対応する制御点がそれぞれｐ１＝（ｘ１,ｙ１,ｚ１）、ｐ４＝（ｘ４,ｙ４,ｚ４）、ｐ８＝（ｘ８,ｙ８,ｚ８）、ｐ５＝（ｘ５,ｙ５,ｚ５）であるとする。このとき、各曲面パッチを表すｕｖ平面の画像解像度比Ｕ：Ｖを、以下の式（１）のように決定する。
【００３０】
【数１】
0005430456.tif 000002
【００３１】
ただし、画像解像度の決定方法には、例えば、制御点の代わりにｕｖ平面の四隅に対応した曲面上の点を使用してもよいし、ｕｖ平面の解像度をすべて同一にしてもよい。その他、画像解像度の決定方法はいかなる方法であってもよい。
【００３２】
ステップＳ３０２において、各曲面パッチのｕｖ平面を表す画像に対して当該曲面パッチの輪郭を描画して、図５（ａ）に示されるように、パラメトリック曲面４０１の有効領域を設定する。具体的には、まず、画像座標を０≦ｕ≦１、０≦ｖ≦１となるよう正規化した上で、ｕｖ平面上に曲面パッチ４００の輪郭であるｕｖ曲線４０２を描画する。このとき、方向を持つｕｖ曲線４０２がｕｖ平面上を進行するとして、左側に存在する領域を有効領域、右側に存在する領域を無効領域とする。そして、無効領域に属する各画素に対して無効領域であることを示すため、法線では３値のノルムが１であるのに対し、それと区別が出来るよう各画素に対して格納する３値をそれぞれ０として記録しておく。
【００３３】
ステップＳ３０３において、ステップＳ３０２で有効領域であると判定されたｕｖ平面の各画素（ｕ,ｖ）に対して、対応する曲面パッチ上の点ｐのモデル座標系における法線を算出し、画素（ｕ,ｖ）に対応付けて登録しておく。具体的には、各画素（ｕ,ｖ）に対し、対応する各曲面上の点の法線をｎ（ｕ,ｖ）＝（ｎｘ,ｎｙ,ｎｚ）Ｔと表す場合、ｎ（ｕ,ｖ）は以下の式（２）により求まる。
【００３４】
【数２】
0005430456.tif 000003
【００３５】
ここで、Ψ＝Ψ（ｕ,ｖ）は、パラメータ（ｕ,ｖ）に対応する曲面上の点ｐの座標（ｘ,ｙ,ｚ）を表す。なお、求めた法線ベクトルｎ（ｕ,ｖ）は、ノルムが１となるよう正規化して単位法線ベクトルとする。次に、式（２）により求めたｎ（ｕ,ｖ） ＝ （ｎｘ,ｎｙ,ｎｚ）Ｔの３値を画素（ｕ,ｖ）に対応させて保持しておく。より具体的には、例えば、当該画素（ｕ,ｖ）の画素値（ｒ,ｇ,ｂ）を、算出した単位法線ベクトル（ｎｘ,ｎｙ,ｎｚ）Ｔによりｒ＝ｎｘ、ｇ＝ｎｙ、ｂ＝ｎｚと置く。そして図５（ｂ）に示されるようなＵＶ法線マップとして保持しておく。ただし、各画素に対する法線の保持の仕方はこの限りではない。例えば、曲面上で有効領域と判定されたｕｖ平面の画素のみを抽出して、各画素に対する法線ベクトルｎ（ｕ,ｖ）を対応付けてテーブルに保持してもよい。
【００３６】
本実施形態では、曲面の有効領域をあらかじめｕｖ平面で表現し、さらに各画素（ｕ,ｖ）に対応付けて法線ベクトルをあらかじめ求めておく。これにより、輪郭線による曲面の有効領域の判定や法線の算出をリアルタイムで行う必要がないため、処理の高速化が可能となる。
【００３７】
法線マップの作成後、図２のフローチャートに戻りステップＳ２０３において、計測対象物体の観測方向を観測方向入力部１０３により幾何特徴抽出処理部１００に入力する。前述したように、物体の位置及び姿勢の概略値としては、前の時刻において計測された位置及び姿勢を用いることができる。また、速度や角速度などの運動の推定結果を加味した位置及び姿勢の予測値、他のセンサによる計測値、そして２次元画像や距離画像からの物体認識結果などを用いることもできる。本実施形態では前の時刻において計測された位置及び姿勢を用いる。
【００３８】
ステップＳ２０４において、ステップＳ２０３で入力された計測対象物体の観測方向およびステップＳ２０２で作成された法線マップに基づいて、パラメトリック形状モデル上から幾何特徴となる領域を選択する。ここで図３（ｂ）を参照して、ステップＳ２０４における幾何特徴領域選択の処理手順を示すフローチャートについて説明する。
【００３９】
ステップＳ３５１において、観測方向および撮像装置の内部パラメータ（焦点距離、主点位置）に基づき、撮像装置の視線ベクトル（観測方向ベクトル）ｅ ＝（ｅｘ,ｅｙ,ｅｚ）Ｔを算出する。ここで、視線ベクトルとは、撮像装置の座標系を基準とした場合の、撮像画像の画像中心に対する方向を示すベクトルである。この視線ベクトルｅ ＝（ｅｘ,ｅｙ,ｅｚ）Ｔを、回転成分ｒ１１〜ｒ３３と並進成分tx、ty、tzとから成る変換行列Ｒを用いて変換する。具体的には、撮像装置の座標系表現からパラメトリック形状モデルの座標系表現へと変換した視線ベクトルｅ’＝（ｅｘ’,ｅｙ’,ｅｚ’）Ｔを式（３）により求める。
【００４０】
【数３】
0005430456.tif 000004
【００４１】
なお、モデル座標系表現に変換した視線ベクトルe’は、ノルムが１となるよう正規化する。ただし、視線ベクトルをパラメトリック形状モデル座標系へ変換せず、あるいは、法線マップの法線を撮像装置の座標系へ変換してもよい。
【００４２】
ステップＳ３５２において、観測可能な曲面パッチを選択する。すなわち、パラメトリック形状モデルを構成する全曲面パッチの中から、入力された観測方向からはセルフオクルージョンにより観測されない曲面パッチをカリングし、撮像画像上で観測される曲面パッチのみを選択する。具体的には、オフライン処理として、予め多数の方向からパラメトリック曲面モデルを描画し、各方向において観測される曲面パッチのみを方向ごとに登録しておく。そして、オンライン処理では、ステップＳ３５１において算出された視線ベクトルに対して、登録しておいた方向の中から最も近いものを選択し、当該方向に登録されている曲面パッチを選択する。ただし、撮像画像上で観測される曲面パッチの選択方法はこの限りではない。例えば、パラメトリック曲面モデルを描画した画像に対してデプスマップを参照し、セルフオクルージョンとなる画素領域を各曲面パッチの法線マップ上に登録したものを方向別に保持してもよい。
【００４３】
ステップＳ３５３において、ステップＳ３５２で選択された曲面パッチの各法線マップに対して、法線マップの中からステップＳ３０２で有効領域と設定された画素（ｕ,ｖ）の法線ベクトルｎとステップＳ３５１で求めた視線ベクトルｅ'との内積を算出する。そして、入力された観測方向においてモデル上から観測される幾何特徴領域を判定する。
【００４４】
図６を参照して、計測対象物体の観測方向に基づいて各曲面パッチ上の幾何特徴を抽出する仕組みについて説明する。図６（ａ）は、撮像装置６００により、ある観測方向からひとつの曲面パッチを撮像する様子を示している。また、図６（ｂ）は、図６（ａ）の様子を断面線６０４により分割した断面図を表している。そして、図６（ｃ）はこのときの撮像画像６２０を示している。撮像画像６２０において物体曲面の表側が可観測となる領域６２２は、曲面の法線ベクトルと視線ベクトルとのなす角度をθ（０°≦θ≦１８０°）とすると、θ＞９０°となる領域６１３に対応する。これに対して、θ≦９０°となる輪郭エッジ領域６１４及び非観測領域６１５については、曲面パッチのエッジおよび裏側が観測され、曲面の表側は観測されない。そこで、視線ベクトルｅ’と、各画素に対応づけられた曲面の法線ベクトルｎとの内積ｎ・ｅ’を利用して、観測方向において可観測となる角度条件を曲面パッチ上の領域を法線マップ上で判定する。具体的には、内積ｎ・ｅ’を以下の式（４）により求める。
【００４５】
【数４】
0005430456.tif 000005
【００４６】
この式（４）で求めた内積ｎ・ｅ’に対して、方向ベクトルに対して表面が観測される領域を判定するため、ある閾値（threshold）を予め定め、以下の式（５）を満たす画素（ｕ,ｖ）を撮像画像上で可観測な局所領域と判定する。
【００４７】
【数５】
0005430456.tif 000006
【００４８】
この閾値には、十分に小さい値を利用する。撮像装置の視線ベクトルに基づき、上述の式（５）により可観測となる特徴領域を判定することで、撮像装置と計測対象物体との位置関係により変化する幾何特徴を、パラメトリック形状モデルから高速かつ精度よく抽出することが可能となる。
【００４９】
上記のように特徴領域を選択した後、図２のステップＳ２０５において、幾何特徴算出部１０５により、可観測な局所領域であると判定された法線マップ中の画素（ｕ,ｖ）に対して対応する曲面上の点の座標（ｘ,ｙ,ｚ）を求める。ここで座標が求められた点を、撮像画像上の幾何特徴と対応付けて位置姿勢推定に利用する。
【００５０】
ステップＳ２０６において、画像入力部１０６により距離センサからの奥行き情報が格納された距離画像を撮影し、三次元計測装置１０９に入力する。
【００５１】
ステップＳ２０７において、対応探索部１０７を用いて、ステップＳ２０５で算出した形状モデル上の幾何特徴と、ステップＳ２０６で撮像した距離画像上の幾何特徴とを対応付け、対応付けた形状モデルの幾何特徴と距離画像上の幾何特徴の組を出力する。
【００５２】
ステップＳ２０８において、位置姿勢算出部１０８を用いて、非線型最適化手法により計測対象物体の位置及び姿勢を更新する。
【００５３】
ステップＳ２０９において、収束判定を行い、収束していれば終了し、収束していなければステップＳ２０３に戻る。収束判定では、例えば、位置及び姿勢の更新前後の誤差ベクトルの二乗和の差を用いて判定する。
【００５４】
以上述べたように、第１実施形態では、各曲面パッチに対して法線マップを作成しておき、計測対象物体の観測方向と法線方向との内積に基づいて、位置姿勢推定において距離画像上の幾何特徴と対応付けるための幾何特徴を高速に抽出できる。
【００５５】
（第２実施形態）
第１実施形態においては、撮像装置の視線ベクトルと各曲面パッチの法線との内積ｎ・ｅ’を求めてｎ・ｅ’の値がある閾値以下となる画素は画像上で可観測な面領域と判定し、距離画像での位置あわせに有効な幾何特徴として抽出した。しかし、その一方で、法線ベクトルと視線ベクトルとが直交する領域は、図６（ｂ）でみられるように、輪郭エッジ領域６１４となる。従って、輪郭エッジとなる領域を２次元画像との位置あわせに有効なエッジ幾何特徴として利用してもよい。具体的には、ある小さな値である閾値(threshold)を定めておき、以下の式（６）を満たす画素については輪郭エッジ領域と判定する。
【００５６】
【数６】
0005430456.tif 000007
【００５７】
その後、輪郭エッジ領域であると判定された画素に対して、ステップＳ２０５と同様の処理により、曲面上の点の座標（ｘ,ｙ,ｚ）を求める。ここで座標が求められた点を、撮像画像上の幾何特徴と対応付けて位置姿勢推定に利用する。具体的には、形状モデルの幾何特徴上の点を２次元画像上に投影し、投影された点の周辺領域に対応する幾何特徴を２次元画像上で探索することにより、形状モデルの幾何特徴と２次元画像上での幾何特徴とを対応付ける。さらに、具体的な位置姿勢の算出方法としては、繰り返し最適化により、対応付けられた形状モデルの幾何特徴と撮像した２次元画像上の幾何特徴との組の２次元画像上での距離の総和を評価関数とすることで最適化演算を行うことが可能である。
【００５８】
あるいは、法線マップ上の近傍４画素（ｕ−１,ｖ−１）、（ｕ,ｖ−１）、（ｕ−１,ｖ）、（ｕ,ｖ）に対して ｎ・ｅ’の符号に着目してもよい。すなわち、この内積の符号の切り替わる領域に輪郭エッジ領域が存在すると判定して、４画素の内積の補間により輪郭エッジとなる画素（ｕ’,ｖ’）を求めてもよい。具体的には、例えば、同一の行に存在する画素の内積の符号がそれぞれ同一であり、かつ、２つの行で符号が異なる場合、行間に輪郭エッジが存在すると判定する。このとき、各画素の内積値から符号が０となる画素を補間して求めることで輪郭エッジとなる画素（ｕ’,ｖ’）と、エッジの方向とを算出することが可能である。この場合、表面パッチ上における輪郭エッジを、点の座標とエッジの方向により、エッジ線分として検出可能となる。従って、形状モデル上の輪郭エッジである点の座標とエッジの方向とを２次元画像上に投影し、投影されたエッジ線分の垂直方向に対し、対応する幾何特徴を１次元的に探索することにより、形状モデルの幾何特徴と２次元画像上での幾何特徴とを対応付ける。その後の位置姿勢推定は第１実施形態で述べた方法により同様に行うことが可能である。
【００５９】
（第３実施形態）
第１実施形態においては、計測対象物体の観測方向に基づき、撮像画像の視線ベクトルを求め、各曲面パッチの法線マップと視線ベクトルとの内積により幾何特徴を判定した。しかしながら、撮像装置の観測方向に基づき、撮像画像の各画素に対する方向ベクトルを求め、これを視線ベクトルの代わりに用いてもよい。具体的には、法線マップの各画素（ｕ,ｖ）に対し、法線ｎ＝ （ｎｘ, ｎｙ, ｎｚ）Ｔとともに、対応する曲面上の点の座標（ｘ,ｙ,ｚ）を算出しておき、法線と対応付けて保持する。特徴領域選択部１０４では、算出しておいた点を撮像装置の概略位置姿勢およびカメラ内部パラメータに基づいて撮像画像上に投影し、投影された点の画像座標を求める。次に、投影された点の画像座標に対する方向ベクトルを求め、モデル座標系に変換する。さらに、曲面上の点が投影された画像座標に対する方向ベクトルと、投影した曲面上の点の法線ベクトルとの内積を求め、第１実施形態と同様の方法により幾何特徴領域を判定する。この例では、視線ベクトルではなく、各曲面上の点を画像上に投影した画素に対する方向ベクトルを用いることで、撮像画像上で可観測となる幾何特徴をより高精度に判定することが可能である。さらに、曲面上の点の法線だけでなく曲面上の点の座標もあらかじめ算出しておくことで、幾何特徴抽出時に点の座標算出処理を行う必要がなく、さらに高速に幾何特徴を抽出することが可能となる。ただし、方向ベクトルと法線ベクトルとの対応の求め方はこの限りではない。例えば、各曲面パッチに対してパッチを覆うＢｏｕｎｄｉｎｇ Ｂｏｘを設定しておき、各画素への方向ベクトルとＢｏｕｎｄｉｎｇ Ｂｏｘとの交差判定を行う。その後、交差したＢｏｕｎｄｉｎｇ Ｂｏｘに対応する曲面パッチの各法線ベクトルと、交差した方向ベクトルとの内積により幾何特徴領域を判定してもよい。あるいは、各曲面パッチに対してパッチの重心座標を求めておき、曲面上の点の座標を投影する代わりに重心座標を代表として用いてもよい。その他、各曲面パッチ上の点の法線ベクトルと観測方向を表すベクトルとの対応関係を求める方法であればいかなる方法であってもよい。
【００６０】
（第４実施形態）
第１実施形態においては、各曲面パッチの法線マップの画像解像度は、モデル中での曲面パッチの相対的な面積比に応じてサイズを決定していた。しかし、これに加えて、撮像画像の視点を考慮して使用する法線マップの画像解像度を切り替えてもよい。具体的には、まず曲面パッチの法線マップの解像度を階層的に設定して複数作成しておく。そして、観測方向入力部１０３により入力される撮像装置の概略位置姿勢に基づき、計測対象物体と撮像装置との距離が離れている場合には、解像度の低い法線マップを使用して幾何特徴の判定を行う。これにより、視点が遠い場合には粗くサンプリングして作成した低解像度の法線マップを利用することで処理を減らし、高速に幾何特徴を求めることが可能となる。また、視点が近い場合は細かくサンプリングして作成した高解像度の法線マップに基づいて高精度に幾何特徴を求めることが可能となる。
【００６１】
（第５実施形態）
第１実施形態では、パラメトリック形状モデルに対して、位置姿勢推定時に使用する幾何特徴を観測方向に基づいて高速に抽出する場合について説明した。第２実施形態では、異なる多数の視点の観測画像においてパラメトリック形状モデルから抽出される曲面上の点の曲率を事前に計算して、この曲率を用いて、距離画像から計測対象物体の「認識」を行う場合について説明する。ここで、「認識」とは、撮像画像上にどのような物体が存在していて、さらにその物体がどのような位置姿勢であるか、すなわちおおよその位置姿勢を求めることを意味する。
【００６２】
図７を参照して、第５実施形態における幾何特徴抽出処理部７００および物体認識装置７０９の構成について説明する。幾何特徴抽出処理部７００の構成は、第１実施形態における幾何特徴抽出処理部１００と同様であり、形状モデル入力部７０１と、法線情報保持部７０２と、観測方向入力部７０３と、特徴領域選択部７０４と、幾何特徴算出部７０５とを備える。また、物体認識装置７０９は、幾何特徴抽出処理部７００と、画像入力部７０６と、類似度算出部７０７と、認識判定部７０８とを備える。以下、幾何特徴抽出処理部７００および物体認識装置７０９を構成する各処理部について説明する。なお、形状モデル入力部７０１、法線情報保持部７０２、特徴領域選択部７０４、画像入力部７０６は、第１実施形態の各対応する処理部と同様であるため、説明を省略する。
【００６３】
観測方向入力部７０３は、計測対象物体を撮像する観測方向を入力する。ここで、観測方向とは、ある視点から計測対象物体への方向を意味する。物体認識では、物体がどのような位置姿勢で存在するかを求めるために、あらかじめ複数の視点から対象物体を観測した画像をデータベースとして複数保持しておく。そしてデータベースの複数の画像と入力画像との幾何特徴の類似度を判定して、最も類似度の高い画像を選択することにより、物体のおおまかな位置姿勢を認識する。観測方向入力部７０３では、データベースとして保持する複数の観測画像を生成するため、計測対象物体を観測する視点として、観測時の概略位置姿勢を入力する。一般的に、計測対象物体を観測する視点には計測対象物体を覆う、ある大きさのＧｅｏｄｅｓｉｃ ｄｏｍｅのような多面体を仮定し、多面体の各頂点を設定する。ただし、視点の設定方法はこれに限るものではない。例えば、計測対象物体を覆う、ある大きさの球を仮定して地球儀のような緯度経度を割り当て、緯度方向、経度方向に等間隔に視点を取る方法なども用いることができる。その他、物体の認識に有効な観測視点を設定できる方法であればいかなる方法でもよい。
【００６４】
幾何特徴算出部７０５は、特徴領域選択部７０４の選択した可観測領域となる曲面上の点に対して、物体認識に使用する幾何特徴を導出する。具体的には、[Paul J. Besl, “Surfaces in Range Image Understanding” Springer-Verlag, pp.63-115, 1988.]に記載されているような方法により、法線マップ上の近傍画素に対応する曲面上の各点の座標（ｘ,ｙ,ｚ）および法線（ｎｘ,ｎｙ,ｎｚ）Ｔを用いて、ガウス曲率、主曲率、平均曲率を導出する。ただし、導出する幾何特徴はガウス曲率に限るものではない。例えば、第１実施形態で求めたような輪郭エッジであってもよい。その後、求めた幾何特徴を対応する画素（ｕ,ｖ）に対して登録しておく。
【００６５】
類似度算出部７０７は、幾何特徴算出部７０５により算出された幾何特徴と、画像入力部７０６により入力される画像上とで幾何特徴の類似度を算出する。具体的には、まず、幾何特徴算出部７０５により算出された各画素の幾何特徴を、画像を部分領域に分割したブロック単位で求める。そして、ブロック領域の各画素の幾何特徴と、入力画像上の同一領域に存在するブロックの各画素とで、幾何特徴の差異を算出する。  
認識判定部７０８は、類似度算出部７０７により算出された多数のデータベースと入力画像との類似度から、最も類似度の高いものを出力することにより、撮像画像上に含まれる物体と、その概略位置姿勢を出力する。
【００６６】
次に図８を参照して、第５実施形態において、計測対象物体のパラメトリック形状モデルから幾何特徴を抽出する処理手順を示すフローチャートについて説明する。なお、ステップＳ８０１は第１実施形態と同様であるため、説明を省略する。
【００６７】
ステップＳ８０２において、ステップＳ８０１で入力されたパラメトリック形状モデルの各曲面パッチに対して法線マップ画像を作成する。基本的な処理は第１実施形態で説明したステップＳ２０２と同様であるため、説明を省略する。なお、本実施形態では、第３実施形態と同様に、法線ｎ＝（ｎｘ,ｎｙ,ｎｚ）Ｔに加えて、対応する曲面上の点の座標（ｘ,ｙ,ｚ）を導出しておき、各画素（ｕ,ｖ）と対応付けて保持しておく。
【００６８】
ステップＳ８０３において、観測方向入力部１０３により計測対象物体の観測方向を幾何特徴抽出処理部７００に入力する。
【００６９】
ステップＳ８０４において、入力する観測方向におけるパラメトリック形状モデルの観測画像上から、認識に有効となる特徴領域を選択する。具体的には、第３実施形態と同様の方法で、ステップＳ８０２で算出された曲面上の点の座標（ｘ,ｙ,ｚ）を観測方向およびカメラ内部パラメータに基づいて撮像画像上に投影して画像座標を求め、投影された各画素に対する方向ベクトルを求める。ここで、曲面上の点の法線と点を投影した画素に対する方向ベクトルとの内積を計算し、第１実施形態と同様の方法により画像上で可観測となる点を求める。
【００７０】
ステップＳ８０５において、幾何特徴算出部７０５により、各視点における観測画像に対し、画素ごとのガウス曲率あるいは平均曲率を導出して保持する。具体的には、可観測であると判定された領域の画素（ｕ,ｖ）に対して、ｕｖ平面上の近傍画素にそれぞれ対応する曲面上の点の座標（ｘ,ｙ,ｚ）および法線（ｎｘ,ｎｙ,ｎｚ）Ｔを用いる。そして、各視点における観測画像に対し、画素ごとのガウス曲率あるいは平均曲率を導出して保持する。
【００７１】
ステップＳ８０６において、設定している全視点に対して処理を終えたか否かを判定する。全視点に対して処理を終えていなければ（ステップＳ８０６；ＮＯ）ステップＳ８０３に戻り、終えていれば（ステップＳ８０６；ＹＥＳ）ステップＳ８０７へ進む。
【００７２】
ステップＳ８０７において、認識の対象としている全物体に対して処理を終えたか否かを判定する。終えていなければ（ステップＳ８０６；ＮＯ）ステップＳ８０１に戻り、終えていれば（ステップＳ８０６；ＹＥＳ）ステップＳ８０８へ進む。
【００７３】
ステップＳ８０８において、画像入力部７０６により撮像画像を入力する。
【００７４】
ステップＳ８０９において、類似度算出部７０７は、ステップＳ８０５で作成された、多数の物体の、多数の視点における観測画像中の幾何特徴のデータベースと、入力される画像との幾何特徴の類似度を算出する。
【００７５】
ステップＳ８１０において、全データベースに対して類似度を算出しているか否かの判定を行う。全データベースに対して類似度を算出していなければ（ステップＳ８１０；ＮＯ）ステップＳ８０９に戻り、算出していれば（ステップＳ８０９；ＹＥＳ）ステップＳ８１１に進む。
【００７６】
ステップＳ８１１において、ステップＳ８１０で算出された各データベースの画像に対する類似度のうち、最も類似度の高いものを出力することで、撮像画像中に含まれる物体と、その概略位置姿勢を出力する。
【００７７】
以上のように、本実施形態においては、異なる多数の観測方向における観測画像において、観測される曲面パッチの点の曲率を抽出して、物体の認識に使用することができる。
【００７８】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【図１】
0005430456.tif 000008
【図２】
0005430456.tif 000009
【図３】
0005430456.tif 000010
【図７】
0005430456.tif 000011
【図８】
0005430456.tif 000012
【図４】
0005430456.tif 000013
【図５】
0005430456.tif 000014
【図６】
0005430456.tif 000015