位置姿勢計測装置、位置姿勢計測方法
【課題】 従来の２次元画像と距離画像を併用した場合の位置姿勢計測手法に比べて、複雑背景下であっても高速かつロバストに位置姿勢を計測すること。
【解決手段】 ２次元画像対応探索部１４０は、撮像画像中における仮想物体の幾何特徴と、撮像画像中に映っている現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う。対応探索領域設定部１５０は、仮想物体を構成するそれぞれの図形を距離画像上に投影し、該投影された図形の領域から、特徴部分に対応する距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する。位置姿勢算出部１７０は対応探索領域内の画素の奥行き値が示す位置と該画素に対応する仮想物体上の位置との３次元空間における距離、幾何特徴と特徴部分との３次元空間における距離、を表す評価関数を最小化するように位置姿勢情報を繰り返し更新する。
【解決手段】 ２次元画像対応探索部１４０は、撮像画像中における仮想物体の幾何特徴と、撮像画像中に映っている現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う。対応探索領域設定部１５０は、仮想物体を構成するそれぞれの図形を距離画像上に投影し、該投影された図形の領域から、特徴部分に対応する距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する。位置姿勢算出部１７０は対応探索領域内の画素の奥行き値が示す位置と該画素に対応する仮想物体上の位置との３次元空間における距離、幾何特徴と特徴部分との３次元空間における距離、を表す評価関数を最小化するように位置姿勢情報を繰り返し更新する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置及び姿勢の計測技術に関するものである。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、工業製品の組立てのようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。
【０００３】
対象物体の位置及び姿勢を計測する方法として、照射した光の反射光を解析することにより求めた画素毎の距離値が格納された距離画像から、物体の３次元モデルを利用して計測する方法がある。これは部品の形状情報であるCADモデルを利用することからモデルフィッティングと呼ばれている。モデルフィッティングによる位置姿勢計測手法では、距離画像から得られる計測点とモデル表面との対応を最近傍探索し、その対応間の距離を最小化することにより物体の位置姿勢を推定する（非特許文献１）。
【０００４】
距離画像を用いたモデルフィッティングでは、物体形状の稜線部や鏡面部のような物体表面の反射が乱れる箇所でアーティファクトと呼ばれるノイズが距離計測時に観測される。そこで距離画像と同時に取得される２次元画像を相補的に組み合わせて情報量を増やすことによって高精度に物体の位置姿勢を計測する手法が提案されている（特許文献１）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特願2009-175387号公報
【特許文献２】特開平07-287756号公報
【非特許文献】
【０００６】
【非特許文献１】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.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
対象物体以外の物体が背景に散在する環境では、距離画像から対象物体に対応する計測点を選択するだけであっても多くの処理が必要となる。例えば非特許文献１で開示されている手法では、対称物体の位置及び姿勢の概略値に基づいて、距離画像上の計測点に最も近い形状モデルの点を探索する必要がある。そのため概略位置姿勢が真の位置姿勢から離れている際には、背景などに誤対応してしまい、正しい位置姿勢を求めることができない場合や反復計算の収束が遅くなる場合がある。そこで誤対応の影響を軽減するために、対応点間の距離に関する統計的な値に基づいて重み付けを行うロバスト推定がよく用いられる。しかしながら、多くの誤対応が発生する場合や概略位置姿勢が真値から大きく離れている場合では、統計的な値を用いて誤対応と正しい対応を見分けることが難しいため、失敗しやすいという問題がある。
【０００８】
これに対し、距離画像と同時に撮影可能な２次元画像を利用する手法が提案されている。例えば特許文献２では、２次元画像から抽出した対象物体の領域に基づき、距離画像から物体の検出を行う方法が開示されている。この手法では、２次元画像からパターンやテクスチャを利用して対象物体の存在領域を限定し、その領域内の距離画像に対して処理を行う。これにより高速かつロバストに対象物体の検出が可能となる。しかしながら、対象物体と同じ物体や似た物体が複数存在するような複雑な背景下では、対象物体に対応する領域が多数発見されるため適用が難しいという問題がある。
【０００９】
本発明はこのような問題に鑑みてなされたものであり、従来の２次元画像と距離画像を併用した場合の位置姿勢計測手法に比べて、複雑背景下であっても高速かつロバストに対象物体の位置姿勢を計測する為の技術を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する手段と、前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する手段と、前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う手段と、前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定手段と、前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新手段と、前記更新手段により繰り返し更新された位置姿勢情報を出力する手段とを備えることを特徴とする。
【発明の効果】
【００１１】
本発明の構成によれば、従来の２次元画像と距離画像を併用した場合の位置姿勢計測手法に比べて、複雑背景下であっても高速かつロバストに位置姿勢を計測することが可能になる。
【図面の簡単な説明】
【００１２】
【図１】３次元計測装置１の構成例を示すブロック図。
【図２】３次元計測装置１が行う処理のフローチャート。
【図３】幾何特徴と画像特徴との対応付けについて説明する図。
【図４】対応探索領域Ω３の設定処理について説明する図。
【図５】対応探索領域Ω３の設定処理について説明する図。
【図６】対応探索領域Ω３の設定処理について説明する図。
【発明を実施するための形態】
【００１３】
以下、添付図面を参照し、本発明の好適な実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の１つである。
【００１４】
［第１の実施形態］
本実施形態では、２次元濃淡撮像画像と距離画像とに対して、複数の多角形で構成され且つ現実物体の形状を模した仮想物体をフィッティングし、この現実物体の位置及び姿勢を計測するのであるが、特に、その際に行う、対応付け方法について説明する。
【００１５】
先ず、本実施形態に係る位置姿勢計測装置である３次元計測装置１の構成例について、図１のブロック図を用いて説明する。
【００１６】
画像取得部１１０は、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する。距離画像は、距離センサで撮像されるものである。距離センサとしては、撮像対象に照射したレーザ光やスリット光、パターン光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するTime-of-flight方式であっても良い。また、撮像画像は、濃淡画像であってもカラー画像であっても良く、距離センサで利用したカメラによって撮影される。本実施形態では、撮像画像として濃淡画像を用いる。
【００１７】
なお、レーザ光やパターン光などの反射光をカメラで撮影し距離を計測する距離センサのように通常の撮像画像も同時に撮影可能であるものであれば、撮像画像及び距離画像は同一視点から撮影した画像となる。ただし、撮像画像と距離画像の撮影位置姿勢が近く、かつ両者の撮影装置の幾何関係が既知であるならば、撮像画像及び距離画像のどちらか一方を幾何変換することで、同一視点から撮影した画像として扱っても良い。例えば、撮像画像と距離画像との間の対応する点を用いて基礎行列を求めておくことで、両者の画像座標系の相互変換が可能である。また、撮像画像及び距離画像は、予め記憶装置に保存しておいたものを取得しても良い。
【００１８】
距離センサと撮像画像を撮像したカメラのカメラパラメータ（画角や解像度、焦点距離など）は既知とする。カメラの焦点距離や主点位置、レンズ歪みパラメータなどのカメラパラメータは、例えば[R. Y. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses, ” Proceedings of Computer Vision and Pattern Recognition, pp. 671-677, 1998.]で開示されている方法によって事前にキャリブレーションしておく。
【００１９】
概略位置姿勢取得部１２０は、撮像画像の視点に対する現実物体の位置及び姿勢の概略値（概略位置姿勢）を取得する。「撮像画像の視点に対する現実物体の位置及び姿勢」とは、撮像画像の視点の位置及び姿勢を基準とする座標系における現実物体の位置及び姿勢を指す。本実施形態では、画像取得部１１０（撮像画像の視点を画像取得部１１０とした場合）の位置及び姿勢を基準とする座標系における現実物体の位置及び姿勢とする。しかしながら、画像取得部１１０を基準とする座標系に対する相対的な位置及び姿勢が既知であり、かつ変化しなければ、３次元計測装置１のいずれの部分を基準としても良い。
【００２０】
本実施形態ではこの概略位置姿勢を後述する繰り返し計算により繰り返し更新して、最終的に出力する位置及び姿勢を求める。然るに概略位置姿勢取得部１２０は、繰り返し計算における現在のステップにて求まった概略位置姿勢を取得して記憶し、記憶した概略位置姿勢を次のステップに供給する。
【００２１】
概略位置姿勢の初期値については特に限定するものではなく、予め定められたものであっても良いし、予め磁気式センサなどの他のセンサにより計測した現実物体の位置及び姿勢を概略位置姿勢の初期値としても良い。また、現実物体を含む情景を撮影した画像や距離画像から物体認識技術を利用して現実物体の位置及び姿勢の概略位置姿勢を推定しても良い。
【００２２】
形状モデル保持部１３０には、仮想物体を構成する各多角形（面）に係る情報、例えば、各多角形（図形）の頂点の座標値、頂点間を結ぶ線分の両端の頂点を示す情報等、仮想物体を定義するために必要な情報を保持している。また、各面と各面に接する線分の対応関係も保持している。
【００２３】
２次元画像対応探索部１４０は、概略位置姿勢取得部１２０から概略位置姿勢を取得し、取得した概略位置姿勢を用いて仮想物体を撮像画像上に投影する。厳密には、この投影には、上記のカメラパラメータも用いる。この投影は、概略位置姿勢取得部１２０から取得した概略位置姿勢を有する仮想物体を仮想空間中に配置し、配置した仮想物体を上記視点から場合に見える仮想物体画像を生成することに等価である。これは、後述する距離画像上への投影についても同様である。そして２次元画像対応探索部１４０は、撮像画像上に投影した仮想物体の幾何特徴（例えば多角形の一辺）と、撮像画像中に映っている現実物体においてこの幾何特徴に対応する特徴部分（画像特徴）と、の対応付けを行う。
【００２４】
概略位置姿勢に基づいて投影した仮想物体の幾何特徴の撮像画像上における位置は、ほとんどの場合、該幾何特徴に対応する画像特徴の位置からはずれている。そこで、撮像画像上に投影された幾何特徴の周辺領域を探索領域Ω２とし、この探索領域Ω２内で、この幾何特徴に対応する画像特徴を探索する。このように、周辺領域のみを探索することにより、任意の幾何特徴に対応する画像特徴を高速に探索することが可能となる。
【００２５】
対応探索領域設定部１５０は、２次元画像対応探索部１４０により対応付けが行われた「撮像画像上に投影した仮想物体の幾何特徴」と「撮像画像中に映っている現実物体においてこの幾何特徴に対応する画像特徴」の組を用いて、対応探索領域Ω３を設定する。
【００２６】
撮像画像において画像特徴が存在する領域に対応する距離画像上の領域は不安定になりやすく、距離画像上で安定して奥行き値が測定できる領域に対応する撮像画像上の領域では画像特徴は検出されにくい。そのため、仮想物体を構成するそれぞれの多角形を、概略位置姿勢を用いて距離画像上に投影し、該投影された多角形の領域から、撮像画像上の画像特徴に対応する距離画像内の領域の周辺を省いた残りの領域を、対応探索領域Ω３として設定する。なお「投影された多角形の領域」は、この多角形についての幾何特徴を撮像画像上に投影した位置同士を結んだ多角形領域である。これにより、距離計測が不安定な画像特徴周辺の距離画像領域を位置合わせに利用しないため、位置合わせが安定化する。
【００２７】
距離画像対応探索部１６０は、上記の設定された対応探索領域Ω３内の各画素について、該画素に対応する仮想物体上の点（３次元点）を探索する。距離計測が不安定であると考えられる領域を除いた対応探索領域内のみから、仮想物体上の対応する３次元点を探索するため、高速かつロバストに対応探索が可能となる。ただし距離画像上の特徴は３次元点に限るものではない。例えば、距離画像から得られる３次元点群に対し、面当てはめを行って生成した平面や曲面であっても良い。
【００２８】
位置姿勢算出部１７０は、以下に示す評価関数を最小化するように、概略位置姿勢を繰り返し更新する。
【００２９】
評価関数＝仮想物体を構成するそれぞれの多角形について求めた対応探索領域Ω３内の画素の奥行き値が示す位置と、該画素に対応する仮想物体上の（上記３次元点の）位置と、の間の３次元空間における距離、幾何特徴と特徴部分との間の３次元空間における距離、を表す評価関数
なお、幾何特徴と撮像画像上の特徴との間の距離と、幾何特徴と距離画像上の特徴との間の距離と、に基づく評価関数を用いて現実物体の位置及び姿勢を推定するのであれば、如何なる方法を用いても良い。
【００３０】
次に、３次元計測装置１が現実物体の位置及び姿勢を求めるために行う処理について、同処理のフローチャートを示す図２を用いて説明する。この処理では、上記の概略位置姿勢を示す位置姿勢情報を、上記評価関数を最小化するように、繰り返し計算（反復演算）により繰り返し補正することにより、最終的な位置及び姿勢を求める。
【００３１】
＜ステップＳ１０１０＞
ステップＳ１０１０では、画像取得部１１０は、現実物体の撮像画像、該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する。
【００３２】
＜ステップＳ１０２０＞
ステップＳ１０２０では、概略位置姿勢取得部１２０は、上記の概略位置姿勢の初期値を取得する。なお、ステップＳ１０１０における処理とステップＳ１０２０における処理の順序は逆でも良い。
【００３３】
＜ステップＳ１０３０＞
ステップＳ１０３０では、２次元画像対応探索部１４０は、概略位置姿勢取得部１２０から概略位置姿勢を取得し、取得した概略位置姿勢を用いて仮想物体を撮像画像上に投影する。厳密には、この投影には、上記のカメラパラメータも用いる。そして２次元画像対応探索部１４０は、撮像画像上に投影した仮想物体の幾何特徴（例えば多角形の一辺）と、撮像画像中に映っている現実物体においてこの幾何特徴に対応する特徴部分（画像特徴）と、の対応付けを行う。
【００３４】
以下の式は、焦点距離がｆである場合に、仮想物体における幾何特徴の３次元座標（ｘ，ｙ，ｚ）を２次元平面（投影面＝撮像画像）上の座標（ｕ，ｖ）に透視投影変換する式である。ただし投影方法は視点と現実物体との間の距離が十分に遠ければ弱透視投影であっても良いし、平行投影であっても良い。
【００３５】
（ｕ，ｖ）Ｔ＝（ｆ×ｘ／ｚ，ｆ×ｙ／ｚ）Ｔ
ここで、本ステップにおいて２次元画像対応探索部１４０が行う、幾何特徴と画像特徴との対応付けについて、図３を用いて説明する。図３に示す如く、撮像画像２３０の視点２６０を基準とする現実物体２１０の概略位置姿勢に基づいて、現実物体２１０の幾何特徴（ここでは点線で示す線分）２２０を撮像画像２３０上に投影したものが投影幾何特徴２９０である。この投影幾何特徴２９０に対応する画像特徴２５０を探索すべく、この投影幾何特徴２９０の周辺領域２４０を探索領域Ω２として設定する。図３では、投影幾何特徴２９０の方向ベクトル方向とは直交する両方向に数画素分広げた領域を探索領域Ω２としている。
【００３６】
なお、幾何特徴２２０としての線分の長さは、現実物体２１０を撮像画像２３０上に投影したときの物体全長の10％とする。しかしながら探索領域の長さはこれに限るものではなく、固定長であっても良いし、位置姿勢推定の残差に応じて大きくしても良い。また、前フレームの情報や磁気センサなどの情報から分かる現実物体２１０の撮像画像２３０上での動きベクトルと上記線分の内積をとり、動き方向に対して探索範囲を広くしても良い。
【００３７】
そして次に、探索領域Ω２内の各画素に対してエッジ検出を行うことで、画像特徴２５０を探索し、探索した画像特徴２５０を投影幾何特徴２９０と対応付ける。なお、探索領域Ω２の設定方法はこれに限るものではない。例えば、幾何特徴は角などの画像特徴として検出しやすいものであっても良く、その場合、探索領域Ω２を矩形や楕円領域など任意の領域とし、投影幾何特徴周辺から２次元探索を行って画像特徴を探索するようにしても良い。
【００３８】
＜ステップＳ１０４０＞
ステップＳ１０４０では対応探索領域設定部１５０は、２次元画像対応探索部１４０により対応付けが行われた「投影幾何特徴」と「投影幾何特徴に対応する特徴部分」の組を用いて、距離画像上に対応探索領域Ω３を設定する。
【００３９】
ここで、本ステップにおいて、対応探索領域設定部１５０が行う、対応探索領域Ω３の設定処理について、図４を用いて説明する。画像特徴と対応付けられた幾何特徴（線分）と投影した各面との対応関係は既知である。
【００４０】
図４（ａ）において点線で示す枠３２０は、投影幾何特徴同士を結ぶことで形成される枠であり、１つの多角形を、概略位置姿勢を用いて距離画像上に投影したものである。実線で示す枠３１０は、撮像画像上の画像特徴に対応する距離画像上の位置同士を結ぶことで形成される枠である。
【００４１】
上記の通り、画像特徴周辺は距離画像としては計測が不安定となる領域である。また、投影幾何特徴から画像特徴に向かうベクトルを太らせた領域３３０は対応する領域でない可能性がある。そこで、枠３２０内の領域を初期の対応探索領域Ω３−０とする。そして、図４（ｂ）に示す如く、画像特徴２５０から投影幾何特徴２９０に向かう線分３９０を求め、求めた線分３９０を囲む領域３３０を求める。そして、対応探索領域Ω３−０から領域３３０を除外した残りの領域３４０を、対応探索領域Ω３とする。
【００４２】
なお、領域３３０の形状は矩形に限るものではなく、求めた線分を囲むことができるのであれば、如何なる形状であっても良い。ただし、画像特徴上の奥行き値はノイズが多いため、画像特徴は除外領域に含まれるようにする。
【００４３】
また、対応探索領域Ω３の求め方は上記の方法に限定するものではなく、多角形を距離画像上に投影した領域を、対応付けられた画像特徴に基づいて縮小した領域であればよい。例えば図５に示す如く対応探索領域Ω３を求めても良い。即ち、対応探索領域を設定する多角形を撮像画像上に投影した領域（枠３２０内の領域）と、その多角形に接する多角形に対応付いた撮像画像上の画像特徴（エッジや角等）を結んだ領域４１０と、の重複領域４４０を対応探索領域Ω３としても良い。
【００４４】
また、画像特徴上は距離画像の奥行き値が不安定であるため、領域４１０を収縮した領域と、枠３２０内の領域との重複領域を対応探索領域Ω３として出力しても良い。さらに、対応探索領域Ω３は２次元平面に限るものではなく、３次元領域であっても良い。例えば図６に示す如く、対応探索領域Ω３を設定しても良い。即ち多角形を撮像画像上に投影した領域５３０とその多角形に接する多角形に対応付いた撮像画像上の画像特徴を結んだ枠３１０内の領域との重複領域Ω３ｐ（５４０）を底面とし、視点２６０を頂点とする３次元領域を対応探索領域Ω３（５５０）としても良い。以上の様々な方法により、距離画像対応探索部１６０は後述の処理において、奥行き値も考慮して対応付けを行うことが可能となる。
【００４５】
＜ステップＳ１０５０＞
ステップＳ１０５０では、距離画像対応探索部１６０は、上記の設定された対応探索領域Ω３内の各画素について、該画素に対応する仮想物体上の点（３次元点）を探索する。そして距離画像対応探索部１６０は、対応探索領域Ω３内の各画素と、該画素に対応する仮想物体上の点（３次元点）と、を対応付ける。
【００４６】
＜ステップＳ１０６０＞
ステップＳ１０６０では位置姿勢算出部１７０は、上記の評価関数を最小化するように、非線型最適化手法により概略位置姿勢を更新する。この処理では、仮想物体の幾何特徴と対応付けた画像特徴および３次元点の３次元空間中での距離をGauss-Newton法により最小化する。なお、本ステップにて更新した概略位置姿勢は、概略位置姿勢取得部１２０にて記憶される。
【００４７】
なお、位置姿勢の算出方法はこれに限るものではない。例えば、Levenberg-Marquardt法や最急降下法などの最適化手法を採用しても良い。また、共役勾配法など、他の非線型最適化計算手法を用いても良い。
【００４８】
＜ステップＳ１０７０＞
ステップＳ１０７０では、位置姿勢算出部１７０は、ステップＳ１０６０の計算の終了条件が満たされたか否かを判断する。終了条件には様々なものが考え得る。例えば、概略位置姿勢の更新前後の誤差ベクトルの二乗和の差がほぼ0となった場合には、終了条件が満たされたと判断する。また、概略位置姿勢の更新量がほぼ0である場合に、終了条件が満たされたと判断しても良い。
【００４９】
そしてこの判断の結果、終了条件が満たされたと判断した場合、位置姿勢算出部１７０は、この時点で求まっている概略位置姿勢を最終的な位置及び姿勢として出力する。出力先については特に限定するものではなく、適当なメモリに出力しても良いし、外部の装置に対して出力しても良い。また、何らかの処理に対して供給するようにしても良い。
【００５０】
一方、この判断の結果、終了条件は満たされていないと判断した場合、処理はステップＳ１０３０に戻り、ステップＳ１０６０にて更新された概略位置姿勢を用いて以降の処理を行う。
【００５１】
なお、本実施形態は、ロボットの自己位置推定や、ロボットと現実物体との間の相対的な位置及び姿勢を推定することにも利用できるものであり、その応用については様々なものが考え得る。
【００５２】
［第２の実施形態］
図１に示した３次元計測装置１を構成する各部はハードウェアで実装しても良いが、形状モデル保持部１３０をＲＡＭなどのメモリとして実装し、その他の各部をソフトウェア（コンピュータプログラム）として実装しても良い。この場合、このメモリを有すると共に、このソフトウェアを格納する為のメモリと、このソフトウェアを実行するためのＣＰＵと、を有するコンピュータは、３次元計測装置１として機能することができる。この場合、このＣＰＵは、このソフトウェアを実行することで、第１の実施形態で説明した各処理を実行することができる。また、画像取得部１１０を上記の撮像画像を撮像すると共に、距離画像をも生成するハードウェアとして実装しても良い。
【００５３】
（その他の実施例）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置及び姿勢の計測技術に関するものである。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、工業製品の組立てのようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。
【０００３】
対象物体の位置及び姿勢を計測する方法として、照射した光の反射光を解析することにより求めた画素毎の距離値が格納された距離画像から、物体の３次元モデルを利用して計測する方法がある。これは部品の形状情報であるCADモデルを利用することからモデルフィッティングと呼ばれている。モデルフィッティングによる位置姿勢計測手法では、距離画像から得られる計測点とモデル表面との対応を最近傍探索し、その対応間の距離を最小化することにより物体の位置姿勢を推定する（非特許文献１）。
【０００４】
距離画像を用いたモデルフィッティングでは、物体形状の稜線部や鏡面部のような物体表面の反射が乱れる箇所でアーティファクトと呼ばれるノイズが距離計測時に観測される。そこで距離画像と同時に取得される２次元画像を相補的に組み合わせて情報量を増やすことによって高精度に物体の位置姿勢を計測する手法が提案されている（特許文献１）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特願2009-175387号公報
【特許文献２】特開平07-287756号公報
【非特許文献】
【０００６】
【非特許文献１】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.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
対象物体以外の物体が背景に散在する環境では、距離画像から対象物体に対応する計測点を選択するだけであっても多くの処理が必要となる。例えば非特許文献１で開示されている手法では、対称物体の位置及び姿勢の概略値に基づいて、距離画像上の計測点に最も近い形状モデルの点を探索する必要がある。そのため概略位置姿勢が真の位置姿勢から離れている際には、背景などに誤対応してしまい、正しい位置姿勢を求めることができない場合や反復計算の収束が遅くなる場合がある。そこで誤対応の影響を軽減するために、対応点間の距離に関する統計的な値に基づいて重み付けを行うロバスト推定がよく用いられる。しかしながら、多くの誤対応が発生する場合や概略位置姿勢が真値から大きく離れている場合では、統計的な値を用いて誤対応と正しい対応を見分けることが難しいため、失敗しやすいという問題がある。
【０００８】
これに対し、距離画像と同時に撮影可能な２次元画像を利用する手法が提案されている。例えば特許文献２では、２次元画像から抽出した対象物体の領域に基づき、距離画像から物体の検出を行う方法が開示されている。この手法では、２次元画像からパターンやテクスチャを利用して対象物体の存在領域を限定し、その領域内の距離画像に対して処理を行う。これにより高速かつロバストに対象物体の検出が可能となる。しかしながら、対象物体と同じ物体や似た物体が複数存在するような複雑な背景下では、対象物体に対応する領域が多数発見されるため適用が難しいという問題がある。
【０００９】
本発明はこのような問題に鑑みてなされたものであり、従来の２次元画像と距離画像を併用した場合の位置姿勢計測手法に比べて、複雑背景下であっても高速かつロバストに対象物体の位置姿勢を計測する為の技術を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する手段と、前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する手段と、前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う手段と、前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定手段と、前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新手段と、前記更新手段により繰り返し更新された位置姿勢情報を出力する手段とを備えることを特徴とする。
【発明の効果】
【００１１】
本発明の構成によれば、従来の２次元画像と距離画像を併用した場合の位置姿勢計測手法に比べて、複雑背景下であっても高速かつロバストに位置姿勢を計測することが可能になる。
【図面の簡単な説明】
【００１２】
【図１】３次元計測装置１の構成例を示すブロック図。
【図２】３次元計測装置１が行う処理のフローチャート。
【図３】幾何特徴と画像特徴との対応付けについて説明する図。
【図４】対応探索領域Ω３の設定処理について説明する図。
【図５】対応探索領域Ω３の設定処理について説明する図。
【図６】対応探索領域Ω３の設定処理について説明する図。
【発明を実施するための形態】
【００１３】
以下、添付図面を参照し、本発明の好適な実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の１つである。
【００１４】
［第１の実施形態］
本実施形態では、２次元濃淡撮像画像と距離画像とに対して、複数の多角形で構成され且つ現実物体の形状を模した仮想物体をフィッティングし、この現実物体の位置及び姿勢を計測するのであるが、特に、その際に行う、対応付け方法について説明する。
【００１５】
先ず、本実施形態に係る位置姿勢計測装置である３次元計測装置１の構成例について、図１のブロック図を用いて説明する。
【００１６】
画像取得部１１０は、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する。距離画像は、距離センサで撮像されるものである。距離センサとしては、撮像対象に照射したレーザ光やスリット光、パターン光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するTime-of-flight方式であっても良い。また、撮像画像は、濃淡画像であってもカラー画像であっても良く、距離センサで利用したカメラによって撮影される。本実施形態では、撮像画像として濃淡画像を用いる。
【００１７】
なお、レーザ光やパターン光などの反射光をカメラで撮影し距離を計測する距離センサのように通常の撮像画像も同時に撮影可能であるものであれば、撮像画像及び距離画像は同一視点から撮影した画像となる。ただし、撮像画像と距離画像の撮影位置姿勢が近く、かつ両者の撮影装置の幾何関係が既知であるならば、撮像画像及び距離画像のどちらか一方を幾何変換することで、同一視点から撮影した画像として扱っても良い。例えば、撮像画像と距離画像との間の対応する点を用いて基礎行列を求めておくことで、両者の画像座標系の相互変換が可能である。また、撮像画像及び距離画像は、予め記憶装置に保存しておいたものを取得しても良い。
【００１８】
距離センサと撮像画像を撮像したカメラのカメラパラメータ（画角や解像度、焦点距離など）は既知とする。カメラの焦点距離や主点位置、レンズ歪みパラメータなどのカメラパラメータは、例えば[R. Y. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses, ” Proceedings of Computer Vision and Pattern Recognition, pp. 671-677, 1998.]で開示されている方法によって事前にキャリブレーションしておく。
【００１９】
概略位置姿勢取得部１２０は、撮像画像の視点に対する現実物体の位置及び姿勢の概略値（概略位置姿勢）を取得する。「撮像画像の視点に対する現実物体の位置及び姿勢」とは、撮像画像の視点の位置及び姿勢を基準とする座標系における現実物体の位置及び姿勢を指す。本実施形態では、画像取得部１１０（撮像画像の視点を画像取得部１１０とした場合）の位置及び姿勢を基準とする座標系における現実物体の位置及び姿勢とする。しかしながら、画像取得部１１０を基準とする座標系に対する相対的な位置及び姿勢が既知であり、かつ変化しなければ、３次元計測装置１のいずれの部分を基準としても良い。
【００２０】
本実施形態ではこの概略位置姿勢を後述する繰り返し計算により繰り返し更新して、最終的に出力する位置及び姿勢を求める。然るに概略位置姿勢取得部１２０は、繰り返し計算における現在のステップにて求まった概略位置姿勢を取得して記憶し、記憶した概略位置姿勢を次のステップに供給する。
【００２１】
概略位置姿勢の初期値については特に限定するものではなく、予め定められたものであっても良いし、予め磁気式センサなどの他のセンサにより計測した現実物体の位置及び姿勢を概略位置姿勢の初期値としても良い。また、現実物体を含む情景を撮影した画像や距離画像から物体認識技術を利用して現実物体の位置及び姿勢の概略位置姿勢を推定しても良い。
【００２２】
形状モデル保持部１３０には、仮想物体を構成する各多角形（面）に係る情報、例えば、各多角形（図形）の頂点の座標値、頂点間を結ぶ線分の両端の頂点を示す情報等、仮想物体を定義するために必要な情報を保持している。また、各面と各面に接する線分の対応関係も保持している。
【００２３】
２次元画像対応探索部１４０は、概略位置姿勢取得部１２０から概略位置姿勢を取得し、取得した概略位置姿勢を用いて仮想物体を撮像画像上に投影する。厳密には、この投影には、上記のカメラパラメータも用いる。この投影は、概略位置姿勢取得部１２０から取得した概略位置姿勢を有する仮想物体を仮想空間中に配置し、配置した仮想物体を上記視点から場合に見える仮想物体画像を生成することに等価である。これは、後述する距離画像上への投影についても同様である。そして２次元画像対応探索部１４０は、撮像画像上に投影した仮想物体の幾何特徴（例えば多角形の一辺）と、撮像画像中に映っている現実物体においてこの幾何特徴に対応する特徴部分（画像特徴）と、の対応付けを行う。
【００２４】
概略位置姿勢に基づいて投影した仮想物体の幾何特徴の撮像画像上における位置は、ほとんどの場合、該幾何特徴に対応する画像特徴の位置からはずれている。そこで、撮像画像上に投影された幾何特徴の周辺領域を探索領域Ω２とし、この探索領域Ω２内で、この幾何特徴に対応する画像特徴を探索する。このように、周辺領域のみを探索することにより、任意の幾何特徴に対応する画像特徴を高速に探索することが可能となる。
【００２５】
対応探索領域設定部１５０は、２次元画像対応探索部１４０により対応付けが行われた「撮像画像上に投影した仮想物体の幾何特徴」と「撮像画像中に映っている現実物体においてこの幾何特徴に対応する画像特徴」の組を用いて、対応探索領域Ω３を設定する。
【００２６】
撮像画像において画像特徴が存在する領域に対応する距離画像上の領域は不安定になりやすく、距離画像上で安定して奥行き値が測定できる領域に対応する撮像画像上の領域では画像特徴は検出されにくい。そのため、仮想物体を構成するそれぞれの多角形を、概略位置姿勢を用いて距離画像上に投影し、該投影された多角形の領域から、撮像画像上の画像特徴に対応する距離画像内の領域の周辺を省いた残りの領域を、対応探索領域Ω３として設定する。なお「投影された多角形の領域」は、この多角形についての幾何特徴を撮像画像上に投影した位置同士を結んだ多角形領域である。これにより、距離計測が不安定な画像特徴周辺の距離画像領域を位置合わせに利用しないため、位置合わせが安定化する。
【００２７】
距離画像対応探索部１６０は、上記の設定された対応探索領域Ω３内の各画素について、該画素に対応する仮想物体上の点（３次元点）を探索する。距離計測が不安定であると考えられる領域を除いた対応探索領域内のみから、仮想物体上の対応する３次元点を探索するため、高速かつロバストに対応探索が可能となる。ただし距離画像上の特徴は３次元点に限るものではない。例えば、距離画像から得られる３次元点群に対し、面当てはめを行って生成した平面や曲面であっても良い。
【００２８】
位置姿勢算出部１７０は、以下に示す評価関数を最小化するように、概略位置姿勢を繰り返し更新する。
【００２９】
評価関数＝仮想物体を構成するそれぞれの多角形について求めた対応探索領域Ω３内の画素の奥行き値が示す位置と、該画素に対応する仮想物体上の（上記３次元点の）位置と、の間の３次元空間における距離、幾何特徴と特徴部分との間の３次元空間における距離、を表す評価関数
なお、幾何特徴と撮像画像上の特徴との間の距離と、幾何特徴と距離画像上の特徴との間の距離と、に基づく評価関数を用いて現実物体の位置及び姿勢を推定するのであれば、如何なる方法を用いても良い。
【００３０】
次に、３次元計測装置１が現実物体の位置及び姿勢を求めるために行う処理について、同処理のフローチャートを示す図２を用いて説明する。この処理では、上記の概略位置姿勢を示す位置姿勢情報を、上記評価関数を最小化するように、繰り返し計算（反復演算）により繰り返し補正することにより、最終的な位置及び姿勢を求める。
【００３１】
＜ステップＳ１０１０＞
ステップＳ１０１０では、画像取得部１１０は、現実物体の撮像画像、該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する。
【００３２】
＜ステップＳ１０２０＞
ステップＳ１０２０では、概略位置姿勢取得部１２０は、上記の概略位置姿勢の初期値を取得する。なお、ステップＳ１０１０における処理とステップＳ１０２０における処理の順序は逆でも良い。
【００３３】
＜ステップＳ１０３０＞
ステップＳ１０３０では、２次元画像対応探索部１４０は、概略位置姿勢取得部１２０から概略位置姿勢を取得し、取得した概略位置姿勢を用いて仮想物体を撮像画像上に投影する。厳密には、この投影には、上記のカメラパラメータも用いる。そして２次元画像対応探索部１４０は、撮像画像上に投影した仮想物体の幾何特徴（例えば多角形の一辺）と、撮像画像中に映っている現実物体においてこの幾何特徴に対応する特徴部分（画像特徴）と、の対応付けを行う。
【００３４】
以下の式は、焦点距離がｆである場合に、仮想物体における幾何特徴の３次元座標（ｘ，ｙ，ｚ）を２次元平面（投影面＝撮像画像）上の座標（ｕ，ｖ）に透視投影変換する式である。ただし投影方法は視点と現実物体との間の距離が十分に遠ければ弱透視投影であっても良いし、平行投影であっても良い。
【００３５】
（ｕ，ｖ）Ｔ＝（ｆ×ｘ／ｚ，ｆ×ｙ／ｚ）Ｔ
ここで、本ステップにおいて２次元画像対応探索部１４０が行う、幾何特徴と画像特徴との対応付けについて、図３を用いて説明する。図３に示す如く、撮像画像２３０の視点２６０を基準とする現実物体２１０の概略位置姿勢に基づいて、現実物体２１０の幾何特徴（ここでは点線で示す線分）２２０を撮像画像２３０上に投影したものが投影幾何特徴２９０である。この投影幾何特徴２９０に対応する画像特徴２５０を探索すべく、この投影幾何特徴２９０の周辺領域２４０を探索領域Ω２として設定する。図３では、投影幾何特徴２９０の方向ベクトル方向とは直交する両方向に数画素分広げた領域を探索領域Ω２としている。
【００３６】
なお、幾何特徴２２０としての線分の長さは、現実物体２１０を撮像画像２３０上に投影したときの物体全長の10％とする。しかしながら探索領域の長さはこれに限るものではなく、固定長であっても良いし、位置姿勢推定の残差に応じて大きくしても良い。また、前フレームの情報や磁気センサなどの情報から分かる現実物体２１０の撮像画像２３０上での動きベクトルと上記線分の内積をとり、動き方向に対して探索範囲を広くしても良い。
【００３７】
そして次に、探索領域Ω２内の各画素に対してエッジ検出を行うことで、画像特徴２５０を探索し、探索した画像特徴２５０を投影幾何特徴２９０と対応付ける。なお、探索領域Ω２の設定方法はこれに限るものではない。例えば、幾何特徴は角などの画像特徴として検出しやすいものであっても良く、その場合、探索領域Ω２を矩形や楕円領域など任意の領域とし、投影幾何特徴周辺から２次元探索を行って画像特徴を探索するようにしても良い。
【００３８】
＜ステップＳ１０４０＞
ステップＳ１０４０では対応探索領域設定部１５０は、２次元画像対応探索部１４０により対応付けが行われた「投影幾何特徴」と「投影幾何特徴に対応する特徴部分」の組を用いて、距離画像上に対応探索領域Ω３を設定する。
【００３９】
ここで、本ステップにおいて、対応探索領域設定部１５０が行う、対応探索領域Ω３の設定処理について、図４を用いて説明する。画像特徴と対応付けられた幾何特徴（線分）と投影した各面との対応関係は既知である。
【００４０】
図４（ａ）において点線で示す枠３２０は、投影幾何特徴同士を結ぶことで形成される枠であり、１つの多角形を、概略位置姿勢を用いて距離画像上に投影したものである。実線で示す枠３１０は、撮像画像上の画像特徴に対応する距離画像上の位置同士を結ぶことで形成される枠である。
【００４１】
上記の通り、画像特徴周辺は距離画像としては計測が不安定となる領域である。また、投影幾何特徴から画像特徴に向かうベクトルを太らせた領域３３０は対応する領域でない可能性がある。そこで、枠３２０内の領域を初期の対応探索領域Ω３−０とする。そして、図４（ｂ）に示す如く、画像特徴２５０から投影幾何特徴２９０に向かう線分３９０を求め、求めた線分３９０を囲む領域３３０を求める。そして、対応探索領域Ω３−０から領域３３０を除外した残りの領域３４０を、対応探索領域Ω３とする。
【００４２】
なお、領域３３０の形状は矩形に限るものではなく、求めた線分を囲むことができるのであれば、如何なる形状であっても良い。ただし、画像特徴上の奥行き値はノイズが多いため、画像特徴は除外領域に含まれるようにする。
【００４３】
また、対応探索領域Ω３の求め方は上記の方法に限定するものではなく、多角形を距離画像上に投影した領域を、対応付けられた画像特徴に基づいて縮小した領域であればよい。例えば図５に示す如く対応探索領域Ω３を求めても良い。即ち、対応探索領域を設定する多角形を撮像画像上に投影した領域（枠３２０内の領域）と、その多角形に接する多角形に対応付いた撮像画像上の画像特徴（エッジや角等）を結んだ領域４１０と、の重複領域４４０を対応探索領域Ω３としても良い。
【００４４】
また、画像特徴上は距離画像の奥行き値が不安定であるため、領域４１０を収縮した領域と、枠３２０内の領域との重複領域を対応探索領域Ω３として出力しても良い。さらに、対応探索領域Ω３は２次元平面に限るものではなく、３次元領域であっても良い。例えば図６に示す如く、対応探索領域Ω３を設定しても良い。即ち多角形を撮像画像上に投影した領域５３０とその多角形に接する多角形に対応付いた撮像画像上の画像特徴を結んだ枠３１０内の領域との重複領域Ω３ｐ（５４０）を底面とし、視点２６０を頂点とする３次元領域を対応探索領域Ω３（５５０）としても良い。以上の様々な方法により、距離画像対応探索部１６０は後述の処理において、奥行き値も考慮して対応付けを行うことが可能となる。
【００４５】
＜ステップＳ１０５０＞
ステップＳ１０５０では、距離画像対応探索部１６０は、上記の設定された対応探索領域Ω３内の各画素について、該画素に対応する仮想物体上の点（３次元点）を探索する。そして距離画像対応探索部１６０は、対応探索領域Ω３内の各画素と、該画素に対応する仮想物体上の点（３次元点）と、を対応付ける。
【００４６】
＜ステップＳ１０６０＞
ステップＳ１０６０では位置姿勢算出部１７０は、上記の評価関数を最小化するように、非線型最適化手法により概略位置姿勢を更新する。この処理では、仮想物体の幾何特徴と対応付けた画像特徴および３次元点の３次元空間中での距離をGauss-Newton法により最小化する。なお、本ステップにて更新した概略位置姿勢は、概略位置姿勢取得部１２０にて記憶される。
【００４７】
なお、位置姿勢の算出方法はこれに限るものではない。例えば、Levenberg-Marquardt法や最急降下法などの最適化手法を採用しても良い。また、共役勾配法など、他の非線型最適化計算手法を用いても良い。
【００４８】
＜ステップＳ１０７０＞
ステップＳ１０７０では、位置姿勢算出部１７０は、ステップＳ１０６０の計算の終了条件が満たされたか否かを判断する。終了条件には様々なものが考え得る。例えば、概略位置姿勢の更新前後の誤差ベクトルの二乗和の差がほぼ0となった場合には、終了条件が満たされたと判断する。また、概略位置姿勢の更新量がほぼ0である場合に、終了条件が満たされたと判断しても良い。
【００４９】
そしてこの判断の結果、終了条件が満たされたと判断した場合、位置姿勢算出部１７０は、この時点で求まっている概略位置姿勢を最終的な位置及び姿勢として出力する。出力先については特に限定するものではなく、適当なメモリに出力しても良いし、外部の装置に対して出力しても良い。また、何らかの処理に対して供給するようにしても良い。
【００５０】
一方、この判断の結果、終了条件は満たされていないと判断した場合、処理はステップＳ１０３０に戻り、ステップＳ１０６０にて更新された概略位置姿勢を用いて以降の処理を行う。
【００５１】
なお、本実施形態は、ロボットの自己位置推定や、ロボットと現実物体との間の相対的な位置及び姿勢を推定することにも利用できるものであり、その応用については様々なものが考え得る。
【００５２】
［第２の実施形態］
図１に示した３次元計測装置１を構成する各部はハードウェアで実装しても良いが、形状モデル保持部１３０をＲＡＭなどのメモリとして実装し、その他の各部をソフトウェア（コンピュータプログラム）として実装しても良い。この場合、このメモリを有すると共に、このソフトウェアを格納する為のメモリと、このソフトウェアを実行するためのＣＰＵと、を有するコンピュータは、３次元計測装置１として機能することができる。この場合、このＣＰＵは、このソフトウェアを実行することで、第１の実施形態で説明した各処理を実行することができる。また、画像取得部１１０を上記の撮像画像を撮像すると共に、距離画像をも生成するハードウェアとして実装しても良い。
【００５３】
（その他の実施例）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【特許請求の範囲】
【請求項１】
現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する手段と、
前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する手段と、
前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う手段と、
前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定手段と、
前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新手段と、
前記更新手段により繰り返し更新された位置姿勢情報を出力する手段と
を備えることを特徴とする位置姿勢計測装置。
【請求項２】
前記設定手段は、前記投影された図形の領域と、前記特徴部分に対応する前記距離画像内の位置同士を繋いだ領域と、の重複領域を、前記対応探索領域として設定することを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項３】
前記設定手段は、前記投影された図形の領域と、前記仮想物体において該図形に接する図形に対応付いた画像特徴を結んだ領域と、の重複領域を底面とし、前記視点を頂点とする３次元領域を、前記対応探索領域として設定することを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項４】
位置姿勢計測装置が行う位置姿勢計測方法であって、
前記位置姿勢計測装置の取得手段が、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する工程と、
前記位置姿勢計測装置の投影手段が、前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する工程と、
前記位置姿勢計測装置の対応付け手段が、前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う工程と、
前記位置姿勢計測装置の設定手段が、前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定工程と、
前記位置姿勢計測装置の更新手段が、前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新工程と、
前記位置姿勢計測装置の出力手段が、前記更新工程で繰り返し更新された位置姿勢情報を出力する工程と
を備えることを特徴とする位置姿勢計測方法。
【請求項５】
コンピュータを、請求項１乃至３の何れか１項に記載の位置姿勢計測装置の各手段として機能させるためのコンピュータプログラム。
【請求項１】
現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する手段と、
前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する手段と、
前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う手段と、
前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定手段と、
前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新手段と、
前記更新手段により繰り返し更新された位置姿勢情報を出力する手段と
を備えることを特徴とする位置姿勢計測装置。
【請求項２】
前記設定手段は、前記投影された図形の領域と、前記特徴部分に対応する前記距離画像内の位置同士を繋いだ領域と、の重複領域を、前記対応探索領域として設定することを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項３】
前記設定手段は、前記投影された図形の領域と、前記仮想物体において該図形に接する図形に対応付いた画像特徴を結んだ領域と、の重複領域を底面とし、前記視点を頂点とする３次元領域を、前記対応探索領域として設定することを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項４】
位置姿勢計測装置が行う位置姿勢計測方法であって、
前記位置姿勢計測装置の取得手段が、現実物体の撮像画像、及び該撮像画像と同じ視点から撮像され且つ各画素が奥行き値を有する距離画像と、を取得する工程と、
前記位置姿勢計測装置の投影手段が、前記視点に対する前記現実物体の位置及び姿勢を示す位置姿勢情報を取得し、該取得した位置姿勢情報を用いて、複数の図形で構成され且つ前記現実物体の形状を模した仮想物体を前記撮像画像上に投影する工程と、
前記位置姿勢計測装置の対応付け手段が、前記撮像画像中における前記仮想物体の幾何特徴と、前記撮像画像中に映っている前記現実物体において該幾何特徴に対応する特徴部分と、の対応付けを行う工程と、
前記位置姿勢計測装置の設定手段が、前記仮想物体を構成するそれぞれの図形を、前記位置姿勢情報を用いて前記距離画像上に投影し、該投影された図形の領域から、前記特徴部分に対応する前記距離画像内の部分の周辺を省いた残りの領域を、対応探索領域として設定する設定工程と、
前記位置姿勢計測装置の更新手段が、前記仮想物体を構成するそれぞれの図形について求めた対応探索領域内の画素の奥行き値が示す位置と、該画素に対応する前記仮想物体上の位置と、の距離、前記幾何特徴と前記特徴部分との距離、を表す評価関数の値を小さくするように前記位置姿勢情報を繰り返し更新する更新工程と、
前記位置姿勢計測装置の出力手段が、前記更新工程で繰り返し更新された位置姿勢情報を出力する工程と
を備えることを特徴とする位置姿勢計測方法。
【請求項５】
コンピュータを、請求項１乃至３の何れか１項に記載の位置姿勢計測装置の各手段として機能させるためのコンピュータプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１２−２２５８８８（Ｐ２０１２−２２５８８８Ａ）
【公開日】平成２４年１１月１５日（２０１２．１１．１５）
【国際特許分類】
【出願番号】特願２０１１−９６３７０（Ｐ２０１１−９６３７０）
【出願日】平成２３年４月２２日（２０１１．４．２２）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２４年１１月１５日（２０１２．１１．１５）
【国際特許分類】
【出願日】平成２３年４月２２日（２０１１．４．２２）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]