"現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する手段と、前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する手段と、前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う手段と、前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定手段と、前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新手段と、前記更新手段により繰り返し更新された位置姿勢情報を出力する手段とを備えることを特徴とする位置姿勢計測装置。前記設定手段は、前記投影された図形の領域と、前記特徴部分に対応する前記距離画像内の位置同士を繋いだ領域と、の重複領域を、前記対応探索領域として設定することを特徴とする請求項1に記載の位置姿勢計測装置。前記設定手段は、前記投影された図形の領域と、前記仮想物体において該図形に接する図形に対応付いた画像特徴を結んだ領域と、の重複領域を底面とし、前記視点を頂点とする3次元領域を、前記対応探索領域として設定することを特徴とする請求項1に記載の位置姿勢計測装置。位置姿勢計測装置が行う位置姿勢計測方法であって、前記位置姿勢計測装置の取得手段が、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する工程と、前記位置姿勢計測装置の投影手段が、前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する工程と、前記位置姿勢計測装置の対応付け手段が、前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う工程と、前記位置姿勢計測装置の設定手段が、前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定工程と、前記位置姿勢計測装置の更新手段が、前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新工程と、前記位置姿勢計測装置の出力手段が、前記更新工程で繰り返し更新された位置姿勢情報を出力する工程とを備えることを特徴とする位置姿勢計測方法。コンピュータを、請求項1乃至3の何れか1項に記載の位置姿勢計測装置の各手段として機能させるためのコンピュータプログラム。対象物体以外の物体が背景に散在する環境では、距離画像から対象物体に対応する計測点を選択するだけであっても多くの処理が必要となる。例えば非特許文献1で開示されている手法では、対称物体の位置及び姿勢の概略値に基づいて、距離画像上の計測点に最も近い形状モデルの点を探索する必要がある。そのため概略位置姿勢が真の位置姿勢から離れている際には、背景などに誤対応してしまい、正しい位置姿勢を求めることができない場合や反復計算の収束が遅くなる場合がある。そこで誤対応の影響を軽減するために、対応点間の距離に関する統計的な値に基づいて重み付けを行うロバスト推定がよく用いられる。しかしながら、多くの誤対応が発生する場合や概略位置姿勢が真値から大きく離れている場合では、統計的な値を用いて誤対応と正しい対応を見分けることが難しいため、失敗しやすいという問題がある。これに対し、距離画像と同時に撮影可能な2次元画像を利用する手法が提案されている。例えば特許文献2では、2次元画像から抽出した対象物体の領域に基づき、距離画像から物体の検出を行う方法が開示されている。この手法では、2次元画像からパターンやテクスチャを利用して対象物体の存在領域を限定し、その領域内の距離画像に対して処理を行う。これにより高速かつロバストに対象物体の検出が可能となる。しかしながら、対象物体と同じ物体や似た物体が複数存在するような複雑な背景下では、対象物体に対応する領域が多数発見されるため適用が難しいという問題がある。本発明はこのような問題に鑑みてなされたものであり、従来の2次元画像と距離画像を併用した場合の位置姿勢計測手法に比べて、複雑背景下であっても高速かつロバストに対象物体の位置姿勢を計測する為の技術を提供することを目的とする。本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する手段と、前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する手段と、前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う手段と、前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定手段と、前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新手段と、前記更新手段により繰り返し更新された位置姿勢情報を出力する手段とを備えることを特徴とする。本発明の構成によれば、従来の2次元画像と距離画像を併用した場合の位置姿勢計測手法に比べて、複雑背景下であっても高速かつロバストに位置姿勢を計測することが可能になる。以下、添付図面を参照し、本発明の好適な実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の1つである。[第1の実施形態]本実施形態では、2次元濃淡撮像画像と距離画像とに対して、複数の多角形で構成され且つ現実物体の形状を模した仮想物体をフィッティングし、この現実物体の位置及び姿勢を計測するのであるが、特に、その際に行う、対応付け方法について説明する。先ず、本実施形態に係る位置姿勢計測装置である3次元計測装置1の構成例について、図1のブロック図を用いて説明する。画像取得部110は、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する。距離画像は、距離センサで撮像されるものである。距離センサとしては、撮像対象に照射したレーザ光やスリット光、パターン光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するTimeofflight方式であっても良い。また、撮像画像は、濃淡画像であってもカラー画像であっても良く、距離センサで利用したカメラによって撮影される。本実施形態では、撮像画像として濃淡画像を用いる。なお、レーザ光やパターン光などの反射光をカメラで撮影し距離を計測する距離センサのように通常の撮像画像も同時に撮影可能であるものであれば、撮像画像及び距離画像は同一視点から撮影した画像となる。ただし、撮像画像と距離画像の撮影位置姿勢が近く、かつ両者の撮影装置の幾何関係が既知であるならば、撮像画像及び距離画像のどちらか一方を幾何変換することで、同一視点から撮影した画像として扱っても良い。例えば、撮像画像と距離画像との間の対応する点を用いて基礎行列を求めておくことで、両者の画像座標系の相互変換が可能である。また、撮像画像及び距離画像は、予め記憶装置に保存しておいたものを取得しても良い。距離センサと撮像画像を撮像したカメラのカメラパラメータ画角や解像度、焦点距離などは既知とする。カメラの焦点距離や主点位置、レンズ歪みパラメータなどのカメラパラメータは、例えばRYTsaiAversatilecameracalibrationtechniqueforhighaccuracy3DmachinevisionmetrologyusingofftheshelfTVcamerasandlensesProceedingsofComputerVisionandPatternRecognitionpp6716771998で開示されている方法によって事前にキャリブレーションしておく。概略位置姿勢取得部120は、撮像画像の視点に対する現実物体の位置及び姿勢の概略値概略位置姿勢を取得する。撮像画像の視点に対する現実物体の位置及び姿勢とは、撮像画像の視点の位置及び姿勢を基準とする座標系における現実物体の位置及び姿勢を指す。本実施形態では、画像取得部110撮像画像の視点を画像取得部110とした場合の位置及び姿勢を基準とする座標系における現実物体の位置及び姿勢とする。しかしながら、画像取得部110を基準とする座標系に対する相対的な位置及び姿勢が既知であり、かつ変化しなければ、3次元計測装置1のいずれの部分を基準としても良い。本実施形態ではこの概略位置姿勢を後述する繰り返し計算により繰り返し更新して、最終的に出力する位置及び姿勢を求める。然るに概略位置姿勢取得部120は、繰り返し計算における現在のステップにて求まった概略位置姿勢を取得して記憶し、記憶した概略位置姿勢を次のステップに供給する。概略位置姿勢の初期値については特に限定するものではなく、予め定められたものであっても良いし、予め磁気式センサなどの他のセンサにより計測した現実物体の位置及び姿勢を概略位置姿勢の初期値としても良い。また、現実物体を含む情景を撮影した画像や距離画像から物体認識技術を利用して現実物体の位置及び姿勢の概略位置姿勢を推定しても良い。形状モデル保持部130には、仮想物体を構成する各多角形面に係る情報、例えば、各多角形図形の頂点の座標値、頂点間を結ぶ線分の両端の頂点を示す情報等、仮想物体を定義するために必要な情報を保持している。また、各面と各面に接する線分の対応関係も保持している。2次元画像対応探索部140は、概略位置姿勢取得部120から概略位置姿勢を取得し、取得した概略位置姿勢を用いて仮想物体を撮像画像上に投影する。厳密には、この投影には、上記のカメラパラメータも用いる。この投影は、概略位置姿勢取得部120から取得した概略位置姿勢を有する仮想物体を仮想空間中に配置し、配置した仮想物体を上記視点から場合に見える仮想物体画像を生成することに等価である。これは、後述する距離画像上への投影についても同様である。そして2次元画像対応探索部140は、撮像画像上に投影した仮想物体の幾何特徴例えば多角形の一辺と、撮像画像中に映っている現実物体においてこの幾何特徴に対応する特徴部分画像特徴と、の対応付けを行う。概略位置姿勢に基づいて投影した仮想物体の幾何特徴の撮像画像上における位置は、ほとんどの場合、該幾何特徴に対応する画像特徴の位置からはずれている。そこで、撮像画像上に投影された幾何特徴の周辺領域を探索領域Ω2とし、この探索領域Ω2内で、この幾何特徴に対応する画像特徴を探索する。このように、周辺領域のみを探索することにより、任意の幾何特徴に対応する画像特徴を高速に探索することが可能となる。対応探索領域設定部150は、2次元画像対応探索部140により対応付けが行われた撮像画像上に投影した仮想物体の幾何特徴と撮像画像中に映っている現実物体においてこの幾何特徴に対応する画像特徴の組を用いて、対応探索領域Ω3を設定する。撮像画像において画像特徴が存在する領域に対応する距離画像上の領域は不安定になりやすく、距離画像上で安定して奥行き値が測定できる領域に対応する撮像画像上の領域では画像特徴は検出されにくい。そのため、仮想物体を構成するそれぞれの多角形を、概略位置姿勢を用いて距離画像上に投影し、該投影された多角形の領域から、撮像画像上の画像特徴に対応する距離画像内の領域の周辺を省いた残りの領域を、対応探索領域Ω3として設定する。なお投影された多角形の領域は、この多角形についての幾何特徴を撮像画像上に投影した位置同士を結んだ多角形領域である。これにより、距離計測が不安定な画像特徴周辺の距離画像領域を位置合わせに利用しないため、位置合わせが安定化する。距離画像対応探索部160は、上記の設定された対応探索領域Ω3内の各画素について、該画素に対応する仮想物体上の点3次元点を探索する。距離計測が不安定であると考えられる領域を除いた対応探索領域内のみから、仮想物体上の対応する3次元点を探索するため、高速かつロバストに対応探索が可能となる。ただし距離画像上の特徴は3次元点に限るものではない。例えば、距離画像から得られる3次元点群に対し、面当てはめを行って生成した平面や曲面であっても良い。位置姿勢算出部170は、以下に示す評価関数を最小化するように、概略位置姿勢を繰り返し更新する。評価関数=仮想物体を構成するそれぞれの多角形について求めた対応探索領域Ω3内の画素の奥行き値が示す位置と、該画素に対応する仮想物体上の上記3次元点の位置と、の間の3次元空間における距離、幾何特徴と特徴部分との間の3次元空間における距離、を表す評価関数なお、幾何特徴と撮像画像上の特徴との間の距離と、幾何特徴と距離画像上の特徴との間の距離と、に基づく評価関数を用いて現実物体の位置及び姿勢を推定するのであれば、如何なる方法を用いても良い。次に、3次元計測装置1が現実物体の位置及び姿勢を求めるために行う処理について、同処理のフローチャートを示す図2を用いて説明する。この処理では、上記の概略位置姿勢を示す位置姿勢情報を、上記評価関数を最小化するように、繰り返し計算反復演算により繰り返し補正することにより、最終的な位置及び姿勢を求める。<ステップS1010>ステップS1010では、画像取得部110は、現実物体の撮像画像、該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する。<ステップS1020>ステップS1020では、概略位置姿勢取得部120は、上記の概略位置姿勢の初期値を取得する。なお、ステップS1010における処理とステップS1020における処理の順序は逆でも良い。<ステップS1030>ステップS1030では、2次元画像対応探索部140は、概略位置姿勢取得部120から概略位置姿勢を取得し、取得した概略位置姿勢を用いて仮想物体を撮像画像上に投影する。厳密には、この投影には、上記のカメラパラメータも用いる。そして2次元画像対応探索部140は、撮像画像上に投影した仮想物体の幾何特徴例えば多角形の一辺と、撮像画像中に映っている現実物体においてこの幾何特徴に対応する特徴部分画像特徴と、の対応付けを行う。以下の式は、焦点距離がfである場合に、仮想物体における幾何特徴の3次元座標x,y,zを2次元平面投影面=撮像画像上の座標u,vに透視投影変換する式である。ただし投影方法は視点と現実物体との間の距離が十分に遠ければ弱透視投影であっても良いし、平行投影であっても良い。u,vT=f×x/z,f×y/zTここで、本ステップにおいて2次元画像対応探索部140が行う、幾何特徴と画像特徴との対応付けについて、図3を用いて説明する。図3に示す如く、撮像画像230の視点260を基準とする現実物体210の概略位置姿勢に基づいて、現実物体210の幾何特徴ここでは点線で示す線分220を撮像画像230上に投影したものが投影幾何特徴290である。この投影幾何特徴290に対応する画像特徴250を探索すべく、この投影幾何特徴290の周辺領域240を探索領域Ω2として設定する。図3では、投影幾何特徴290の方向ベクトル方向とは直交する両方向に数画素分広げた領域を探索領域Ω2としている。なお、幾何特徴220としての線分の長さは、現実物体210を撮像画像230上に投影したときの物体全長の10とする。しかしながら探索領域の長さはこれに限るものではなく、固定長であっても良いし、位置姿勢推定の残差に応じて大きくしても良い。また、前フレームの情報や磁気センサなどの情報から分かる現実物体210の撮像画像230上での動きベクトルと上記線分の内積をとり、動き方向に対して探索範囲を広くしても良い。そして次に、探索領域Ω2内の各画素に対してエッジ検出を行うことで、画像特徴250を探索し、探索した画像特徴250を投影幾何特徴290と対応付ける。なお、探索領域Ω2の設定方法はこれに限るものではない。例えば、幾何特徴は角などの画像特徴として検出しやすいものであっても良く、その場合、探索領域Ω2を矩形や楕円領域など任意の領域とし、投影幾何特徴周辺から2次元探索を行って画像特徴を探索するようにしても良い。<ステップS1040>ステップS1040では対応探索領域設定部150は、2次元画像対応探索部140により対応付けが行われた投影幾何特徴と投影幾何特徴に対応する特徴部分の組を用いて、距離画像上に対応探索領域Ω3を設定する。ここで、本ステップにおいて、対応探索領域設定部150が行う、対応探索領域Ω3の設定処理について、図4を用いて説明する。画像特徴と対応付けられた幾何特徴線分と投影した各面との対応関係は既知である。図4aにおいて点線で示す枠320は、投影幾何特徴同士を結ぶことで形成される枠であり、1つの多角形を、概略位置姿勢を用いて距離画像上に投影したものである。実線で示す枠310は、撮像画像上の画像特徴に対応する距離画像上の位置同士を結ぶことで形成される枠である。上記の通り、画像特徴周辺は距離画像としては計測が不安定となる領域である。また、投影幾何特徴から画像特徴に向かうベクトルを太らせた領域330は対応する領域でない可能性がある。そこで、枠320内の領域を初期の対応探索領域Ω3-0とする。そして、図4bに示す如く、画像特徴250から投影幾何特徴290に向かう線分390を求め、求めた線分390を囲む領域330を求める。そして、対応探索領域Ω3-0から領域330を除外した残りの領域340を、対応探索領域Ω3とする。なお、領域330の形状は矩形に限るものではなく、求めた線分を囲むことができるのであれば、如何なる形状であっても良い。ただし、画像特徴上の奥行き値はノイズが多いため、画像特徴は除外領域に含まれるようにする。また、対応探索領域Ω3の求め方は上記の方法に限定するものではなく、多角形を距離画像上に投影した領域を、対応付けられた画像特徴に基づいて縮小した領域であればよい。例えば図5に示す如く対応探索領域Ω3を求めても良い。即ち、対応探索領域を設定する多角形を撮像画像上に投影した領域枠320内の領域と、その多角形に接する多角形に対応付いた撮像画像上の画像特徴エッジや角等を結んだ領域410と、の重複領域440を対応探索領域Ω3としても良い。また、画像特徴上は距離画像の奥行き値が不安定であるため、領域410を収縮した領域と、枠320内の領域との重複領域を対応探索領域Ω3として出力しても良い。さらに、対応探索領域Ω3は2次元平面に限るものではなく、3次元領域であっても良い。例えば図6に示す如く、対応探索領域Ω3を設定しても良い。即ち多角形を撮像画像上に投影した領域530とその多角形に接する多角形に対応付いた撮像画像上の画像特徴を結んだ枠310内の領域との重複領域Ω3p540を底面とし、視点260を頂点とする3次元領域を対応探索領域Ω3550としても良い。以上の様々な方法により、距離画像対応探索部160は後述の処理において、奥行き値も考慮して対応付けを行うことが可能となる。<ステップS1050>ステップS1050では、距離画像対応探索部160は、上記の設定された対応探索領域Ω3内の各画素について、該画素に対応する仮想物体上の点3次元点を探索する。そして距離画像対応探索部160は、対応探索領域Ω3内の各画素と、該画素に対応する仮想物体上の点3次元点と、を対応付ける。<ステップS1060>ステップS1060では位置姿勢算出部170は、上記の評価関数を最小化するように、非線型最適化手法により概略位置姿勢を更新する。この処理では、仮想物体の幾何特徴と対応付けた画像特徴および3次元点の3次元空間中での距離をGaussNewton法により最小化する。なお、本ステップにて更新した概略位置姿勢は、概略位置姿勢取得部120にて記憶される。なお、位置姿勢の算出方法はこれに限るものではない。例えば、LevenbergMarquardt法や最急降下法などの最適化手法を採用しても良い。また、共役勾配法など、他の非線型最適化計算手法を用いても良い。<ステップS1070>ステップS1070では、位置姿勢算出部170は、ステップS1060の計算の終了条件が満たされたか否かを判断する。終了条件には様々なものが考え得る。例えば、概略位置姿勢の更新前後の誤差ベクトルの二乗和の差がほぼ0となった場合には、終了条件が満たされたと判断する。また、概略位置姿勢の更新量がほぼ0である場合に、終了条件が満たされたと判断しても良い。そしてこの判断の結果、終了条件が満たされたと判断した場合、位置姿勢算出部170は、この時点で求まっている概略位置姿勢を最終的な位置及び姿勢として出力する。出力先については特に限定するものではなく、適当なメモリに出力しても良いし、外部の装置に対して出力しても良い。また、何らかの処理に対して供給するようにしても良い。一方、この判断の結果、終了条件は満たされていないと判断した場合、処理はステップS1030に戻り、ステップS1060にて更新された概略位置姿勢を用いて以降の処理を行う。なお、本実施形態は、ロボットの自己位置推定や、ロボットと現実物体との間の相対的な位置及び姿勢を推定することにも利用できるものであり、その応用については様々なものが考え得る。[第2の実施形態]図1に示した3次元計測装置1を構成する各部はハードウェアで実装しても良いが、形状モデル保持部130をRAMなどのメモリとして実装し、その他の各部をソフトウェアコンピュータプログラムとして実装しても良い。この場合、このメモリを有すると共に、このソフトウェアを格納する為のメモリと、このソフトウェアを実行するためのCPUと、を有するコンピュータは、3次元計測装置1として機能することができる。この場合、このCPUは、このソフトウェアを実行することで、第1の実施形態で説明した各処理を実行することができる。また、画像取得部110を上記の撮像画像を撮像すると共に、距離画像をも生成するハードウェアとして実装しても良い。その他の実施例また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェアプログラムを、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータまたはCPUやMPU等がプログラムを読み出して実行する処理である。"