位置姿勢計測装置、その処理方法及びプログラム
【課題】２次元画像上における画像特徴と３次元形状モデルを構成する幾何特徴とを対応付け、その結果に基づいて物体の位置及び姿勢の計測を行なう。
【解決手段】位置姿勢計測装置は、３次元形状モデルを保持するモデル保持手段と、物体の位置及び姿勢を示す概略値を取得する手段と、撮像装置により撮像された物体の２次元画像を取得する手段と、概略値に基づいて３次元形状モデルにおける幾何特徴を２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する手段と、２次元画像から画像特徴の方向を算出する手段と、算出された画像特徴の方向と、３次元形状モデルから算出された幾何特徴の方向とを比較することにより、画像特徴と幾何特徴とを対応付ける手段と、当該対応付けられた画像特徴と幾何特徴とにおける距離に基づいて概略値を補正することにより物体の位置及び姿勢を算出する位置姿勢算出手段とを具備する。
【解決手段】位置姿勢計測装置は、３次元形状モデルを保持するモデル保持手段と、物体の位置及び姿勢を示す概略値を取得する手段と、撮像装置により撮像された物体の２次元画像を取得する手段と、概略値に基づいて３次元形状モデルにおける幾何特徴を２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する手段と、２次元画像から画像特徴の方向を算出する手段と、算出された画像特徴の方向と、３次元形状モデルから算出された幾何特徴の方向とを比較することにより、画像特徴と幾何特徴とを対応付ける手段と、当該対応付けられた画像特徴と幾何特徴とにおける距離に基づいて概略値を補正することにより物体の位置及び姿勢を算出する位置姿勢算出手段とを具備する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置姿勢計測装置、その処理方法及びプログラムに関する。
【背景技術】
【０００２】
近年、これまで人間が行なっていた複雑なタスクをロボットが代わりに行なうようになりつつある。複雑なタスクの代表例として、工業製品の組み立てが挙げられる。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組み立てを行なう。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。
【０００３】
一般に、このような位置及び姿勢の計測方法としては、カメラが撮像する２次元画像上から検出される画像特徴や距離センサから得られる距離画像に対して、物体の３次元形状モデルをモデルフィッティングさせる（当てはめる）技術が知られている。
【０００４】
非特許文献１には、２次元画像上から検出される画像特徴としてエッジを利用する技術が開示されている。この技術では、物体の形状を３次元線分の集合によって表し、物体の概略の位置及び姿勢の情報を既知として、当該２次元画像上で検出されるエッジに３次元線分の投影像をモデルフィッティングさせる。テクスチャのない直線を基調とした人工的な物体が多い環境下等で物体の位置及び姿勢を計測する場合、エッジを利用した計測が適している。
【０００５】
ここで、２次元画像から検出される画像特徴に基づいて、物体の位置及び姿勢の計測を正確に行なうためには、検出される画像特徴と３次元形状モデルにおける幾何特徴とが正確に対応付けられる必要がある。
【０００６】
上述した非特許文献１では、３次元線分を２次元画像上に投影した位置の近傍で検出されたエッジと３次元線分とを対応付けている。すなわち、この方法では、３次元線分の投影像の最近傍に検出されたエッジが正しく対応するエッジとみなしている。そのため、最近傍に検出されたエッジが、本来対応するはずのないエッジであれば、物体の位置及び姿勢を正確に計測できないか、また、計測の精度が低下するかしてしまう。特に、物体の概略の位置及び姿勢が不正確な場合や、また、２次元画像が複雑で対応候補となるエッジが多量に検出される場合には、３次元形状モデルの線分とエッジとの対応付けに誤対応が発生してしまう。
【０００７】
このような問題を解決するため、非特許文献２では、エッジ周辺の輝度値を利用することで３次元形状モデルの線分と濃淡画像中のエッジとの対応付けの精度を向上させる手法が開示されている。具体的には、学習画像を用いて、３次元形状モデル中の線分が画像上でどのような輝度分布として検出されるかを予め学習させておく。そして、濃淡画像中から実際に検出されたエッジ周辺の輝度分布との類似度を算出し、その結果に基づいてエッジの対応付けを行なう。この技術は、対象物体の表面情報として、ユニークに識別できるような輝度分布が含まれているような場合に有用である。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】T. Drummond and R. Cipolla, “Real-time visual tracking of complex structures," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002.
【非特許文献２】H. Wuest. F. Vial, and D. Stricker, "Adaptive line tracking with multiple hypotheses for augmented reality," Proc. The Fourth Int'l Symp. on Mixed and Augmented Reality (ISMAR05), pp.62-69, 2005.
【非特許文献３】L. Vacchetti, V. Lepetit, and P. Fua, “Combining edge and texture information for real-time accurate 3D camera tracking, " Proc. 3rd IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR ‘04) , pp.48-57, 2004.
【非特許文献４】R. Y. Tsai, "A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses", IEEE Journal of Robotics and Automation, vol.RA-3, no.4, 1987.
【発明の概要】
【発明が解決しようとする課題】
【０００９】
上述した通り、濃淡画像上の輝度を利用してエッジを対応付ける手法では、対象物体にユニークに識別可能な表面色情報がある場合など、見かけ上の輝度の分布に再現性が高い場合には有効に働く。
【００１０】
しかし、濃淡画像上の輝度は、物体の表面情報、光源状態、物体を観察する視点位置によって多様に変化するため、対象物体の表面色や環境状況によっては、３次元形状モデル中の幾何特徴と画像特徴とに誤対応が生じる可能性が高くなる。
【００１１】
本発明は、上記課題に鑑みてなされたものであり、２次元画像上における画像特徴の方向を用いて、当該画像特徴と３次元形状モデルを構成する幾何特徴と対応付け、その対応付け結果に基づいて物体の位置及び姿勢の計測を行なうようにした技術を提供する。
【課題を解決するための手段】
【００１２】
上記課題を解決するため、本発明の一態様による位置姿勢計測装置は、物体の幾何特徴を表す３次元形状モデルを保持するモデル保持手段と、前記物体の位置及び姿勢を示す概略値を取得する概略値取得手段と、撮像装置により撮像された前記物体の２次元画像を取得する画像取得手段と、前記概略値に基づいて前記３次元形状モデルにおける幾何特徴を前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する投影手段と、前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する検出手段と、前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける対応付け手段と、前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する位置姿勢算出手段とを具備する。
【発明の効果】
【００１３】
本発明によれば、３次元形状モデルを構成する幾何特徴と２次元画像上から検出した画像特徴とを従来よりも正確に対応付けられる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施の形態に係わる位置姿勢計測装置１の構成の一例を示す図。
【図２】３次元形状モデルの定義の一例を示す図。
【図３】物体の位置及び姿勢を計測する際の全体的な処理の流れの一例を示すフローチャート。
【図４】２次元画像からエッジを検出する際の処理の流れの一例を示すフローチャート。
【図５】エッジ検出の概要を示す図。
【図６】対応付け処理の概要を示す図。
【発明を実施するための形態】
【００１５】
以下、本発明の一実施の形態について添付図面を参照して詳細に説明する。
【００１６】
図１は、本発明の一実施の形態に係わる位置姿勢計測装置１の構成の一例を示す図である。なお、本実施形態においては、計測対象となる物体（以下、計測対象物体、又は単に物体と呼ぶ場合もある）の概略の位置及び姿勢が既知であると仮定する。そして、その概略値に基づいて３次元形状モデルデータ（以下、単に、３次元形状モデルと呼ぶ）と２次元画像（濃淡画像）とを対応させることにより物体の位置及び姿勢の計測（推定）を行なう。
【００１７】
ここで、計測対象とする物体がほぼ剛体であるとみなすと、対象物体が一体として変位及び回転していたとしても、物体面上の各所の相互関係は変化しない。物体の概略の位置及び姿勢が、実際の物体の位置及び姿勢とある程度近い場合は、面と面の境界といった物体面上の３次元線分の２次元投影像の方向に関しても、この相互関係は概ね維持される傾向となる。
【００１８】
そこで、本実施形態においては、この特性に着目し、３次元形状モデルの３次元線分を投影した投影像の方向と、２次元画像上のエッジの方向との差を算出して、その差が全体の最頻値に近いエッジを対応するエッジとして判定する。これにより、物体の位置及び姿勢の計測を行なう。
【００１９】
ここで、位置姿勢計測装置１には、撮像装置３０が接続されている。なお、撮像装置３０は、位置姿勢計測装置１内に内蔵されていても構わない。
【００２０】
撮像装置３０は、例えば、カメラであり、２次元画像を撮像する。本実施形態においては、撮像装置３０により撮像される２次元画像が、計測対象となる物体の輝度を表現する濃淡画像である場合を例に挙げて説明するが、これに限られず、例えば、カラー画像が撮像されても良い。
【００２１】
撮像装置３０により撮像された２次元画像は、２次元画像取得部２２を介して位置姿勢計測装置１内に入力される。なお、カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータ（２次元画像撮像時のパラメータ）は、例えば、非特許文献４に記載される方法によって予めキャリブレーションしておく。
【００２２】
位置姿勢計測装置１は、モデル保持部２１に保持された計測対象となる物体の形状を表す３次元形状モデルに基づいて、２次元画像内に含まれる物体の位置及び姿勢の計測を行なう。なお、本実施形態においては、位置姿勢計測装置１（モデル保持部２１）に保持された３次元形状モデルが、現実に撮像される物体の形状に即していることを前提とする。
【００２３】
ここで、位置姿勢計測装置１は、その機能的な構成として、モデル保持部２１と、２次元画像取得部２２と、概略値取得部２３と、モデル投影部２４と、画像特徴検出部２５と、対応付け部２６と、位置姿勢算出部２７とを具備して構成される。
【００２４】
モデル保持部２１は、計測対象となる物体の３次元形状モデルを保持する。３次元形状モデルは、例えば、物体の幾何特徴（形状）を複数の線分（エッジ）を用いて定義する。
【００２５】
より具体的には、３次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。物体の３次元形状モデル１０は、図２（ａ）に示すように、点Ｐ１〜点Ｐ１４の１４点から構成される。点Ｐ１〜点Ｐ１４は、図２（ｃ）に示すように、３次元座標値により表される。なお、３次元形状モデル１０に規定される基準座標系の原点をＰ１２とする。そして、点Ｐ１２から点Ｐ１３に向かう方向をｘ軸とし、点Ｐ１２から点Ｐ８に向かう方向をｙ軸とし、点Ｐ１２から点Ｐ１１に向かう方向をｚ軸とする。なお、ｙ軸は、鉛直上方向（重力軸の反対方向）に一致する。
【００２６】
３次元形状モデル１０は、図２（ｂ）に示すように、線分Ｌ１〜Ｌ１６により構成されている。線分Ｌ１〜Ｌ１６は、図２（ｄ）に示すように、線分を構成する点のＩＤ（識別子）によって表される。なお、ここでは図示を省略するが、３次元形状モデル１０は、面の情報も保持する。この場合、各面は、各面を構成する頂点のＩＤにより表される。
【００２７】
概略値取得部２３は、位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を取得する。本実施形態において、位置姿勢計測装置１に対する物体の位置及び姿勢とは、撮像装置３０（カメラ座標系）を基準とした物体の位置及び姿勢を示すが、必ずしも、撮像装置３０を基準にする必要はない。例えば、撮像装置３０の座標系に対する物体の相対的な位置及び姿勢が既知であり、且つその位置及び姿勢が変化しないのであれば、位置姿勢計測装置１におけるその他の部分を基準としても良い。また、本実施形態においては、物体の位置及び姿勢の概略値として、位置姿勢計測装置１が当該物体から過去（例えば、直前）に計測した計測値を用いる。但し、物体の位置及び姿勢の概略値は、必ずしも、このような値である必要はない。例えば、過去に計測した物体の位置及び姿勢の計測値に対して時系列フィルタリング処理（例えば、線形フィルタ、カルマンフィルタ）を実施し、物体の運動速度や角速度を推定する。そして、その推定結果に基づいて物体の位置及び姿勢を予測した値を概略値としても良い。また、外部のセンサから得られる物体の位置及び姿勢を概略値としても良い。ここで、センサは、物体の位置及び姿勢を６自由度で計測できれば良く、その方式（例えば、磁気式、光学式、超音波式）は、特に問わない。なお、物体の置かれているおおよその位置や姿勢が予め分かっているのであれば、その値を概略値として用いても良い。
【００２８】
モデル投影部２４は、撮像装置３０の内部パラメータや概略値取得部２３により取得された概略値を用いて、モデル保持部２１に保持された３次元形状モデルを２次元画像上（濃淡画像上）に投影する。そして、この投影の結果として、３次元形状モデルを構成する線分の２次元座標及び２次元方向を算出する。なお、３次元形状モデルの投影方法については後述する。
【００２９】
画像特徴検出部２５は、２次元画像取得部２２により取得された２次元画像から画像特徴としてエッジを検出する。そして、そのエッジの方向として輝度勾配の方向の法線を算出する。エッジ検出方法については後述する。
【００３０】
対応付け部２６は、画像特徴検出部２５により検出された各画像特徴（各エッジ）と、３次元形状モデルの各幾何特徴（３次元形状モデルを構成する各線分）とを対応付ける。この対応付け処理では、モデル投影部２４により算出された３次元形状モデルの線分の２次元方向と、２次元画像上のエッジの方向とにおける差に基づいて、２次元画像上のエッジに対して３次元形状モデルの線分を対応付ける。なお、対応付け処理の詳細については後述する。
【００３１】
位置姿勢算出部２７は、対応付け部２６により対応付けられた３次元形状モデルの線分と２次元画像上のエッジとの対応関係（組）を用いて、物体の位置及び姿勢を算出する。なお、物体の位置及び姿勢の算出処理の詳細については後述する。
【００３２】
以上が、位置姿勢計測装置１の構成の一例についての説明である。なお、上記説明した、位置姿勢計測装置１には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御手段、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。
【００３３】
次に、図３を用いて、図１に示す位置姿勢計測装置１により計測対象物体の位置及び姿勢を計測する際の処理の流れの一例について説明する。
【００３４】
[Ｓ１０１]
位置姿勢計測装置１は、まず、初期化処理を行なう。この初期化処理では、例えば、概略値取得部２３において、位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を取得する。本実施形態においては、上述した通り、物体の位置及び姿勢の概略値として、位置姿勢計測装置１が当該物体から過去（例えば、直前）に計測した計測値を用いる。また、初期化処理においては、撮像装置３０の内部パラメータ等の取得も行なわれる。
【００３５】
[Ｓ１０２]
位置姿勢計測装置１は、２次元画像取得部２２において、撮像装置３０を介して物体の２次元画像（濃淡画像）を取得する。
【００３６】
[Ｓ１０３]
位置姿勢計測装置１は、画像特徴検出部２５において、Ｓ１０２の処理で取得した２次元画像上から物体の画像特徴としてエッジを検出する。エッジは、濃度勾配の極値となる点である。本実施形態においては、非特許文献３に開示される方法を用いてエッジの検出を行なう。
【００３７】
ここで、Ｓ１０３におけるエッジ検出について説明する。図４には、２次元画像（濃淡画像）からエッジを検出する際の処理の流れの一例が示される。
【００３８】
[Ｓ２０１]
位置姿勢計測装置１は、まず、モデル投影部２４において、Ｓ１０１の処理で取得した概略値と、撮像装置３０の内部パラメータとを用いて、３次元形状モデルを２次元画像上へ投影する。なお、３次元形状モデルの投影は計算により行なわれる。そのため、この投影により、３次元形状モデルを構成する線分各々（幾何特徴各々）の２次元画像上における座標と方向が計算される。３次元形状モデル（線分）の投影像は、２次元画像上でも線分となる。
【００３９】
[Ｓ２０２]
続いて、位置姿勢計測装置１は、モデル投影部２４において、Ｓ２０１の処理で算出された投影像上（投影線分上）に制御点を設定する。制御点は、投影線分を等間隔に分割するように設定される。以降、この制御点をＥｄｇｅｌｅｔと呼称する。
【００４０】
位置姿勢計測装置１は、Ｅｄｇｅｌｅｔ毎（制御点毎）に、３次元形状モデルを構成する線分の３次元座標及び３次元方向と、投影結果として得られる２次元座標及び２次元方向とを保持する。なお、Ｅｄｇｅｌｅｔの数は、物体の形状等に合わせて適宜変更する。また、Ｅｄｇｅｌｅｔの数が増える程、処理時間が長くなるため、Ｅｄｇｅｌｅｔの総数が一定になるように、Ｅｄｇｅｌｅｔ間の間隔を適宜変更しても良い。
【００４１】
[Ｓ２０３]
Ｅｄｇｅｌｅｔ（制御点）の設定が済むと、位置姿勢計測装置１は、画像特徴検出部２５において、Ｓ２０２の処理で設定したＥｄｇｅｌｅｔ各々（制御点各々）に対応する２次元画像上のエッジを検出する。図５を用いて、本実施形態に係わるエッジ検出の概要について説明する。
【００４２】
エッジの検出は、図５（ａ）に示すように、投影像（投影線分）の法線方向に平行で且つＥｄｇｅｌｅｔを通過する線分（以下、探索ライン）上からエッジを１次元探索することにより行なう。ここで、画像特徴検出部２５は、図５（ｂ）に示すように、探索ライン上における２次元画像上の濃度勾配から極値を算出する（この場合、複数の極値（エッジ）が存在する）。すなわち、エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。そして、探索ライン上で検出された全てのエッジの２次元座標を、各Ｅｄｇｅｌｅｔの対応候補のエッジ（対応候補エッジ）の位置として保持する。このような処理により、３次元形状モデルの幾何特徴毎（より具体的には、Ｅｄｇｅｌｅｔ毎）に、対応候補となるエッジが２次元画像上から検出される。
【００４３】
対応候補エッジの検出が済むと、位置姿勢計測装置１は、画像特徴検出部２５において、Ｓ２０３の処理で検出された各対応候補エッジの方向を算出する。エッジの方向は、対応候補エッジ周辺の画素に対してエッジ検出フィルタを用いてフィルタリング処理し、それにより得られた画素の勾配方向に基づいて求めれば良い。本実施形態においては、対応候補エッジから所定範囲内の画素（例えば、９画素）に対して、水平方向のｓｏｂｅｌフィルタと垂直方向のｓｏｂｅｌフィルタとをかけ、その応答を正規化する。これにより、画素の勾配方向を算出する。そして、勾配方向の法線方向を求め、それをエッジの方向として取得する。
【００４４】
なお、画素の勾配方向の算出には、必ずしも、ｓｏｂｅｌフィルタを用いた手法を採用する必要はない。すなわち、注目したエッジの勾配方向を算出できれば良く、その手法については特に問わない。例えば、ｐｒｅｗｉｔｔフィルタなどの別のエッジ検出フィルタを用いても良い。また、ｃａｎｎｙアルゴリズムにより画像全体からエッジを検出し、それにより、勾配方向を算出しても良い。
【００４５】
[Ｓ２０４]
以上の処理をＥｄｇｅｌｅｔ全てに対して実施し、それにより、全てのＥｄｇｅｌｅｔにおける全ての対応候補エッジの位置及び方向を算出する。
【００４６】
[Ｓ１０４]
図３の説明に戻り、位置姿勢計測装置１は、対応付け２６において、３次元形状モデルを構成する線分における各Ｅｄｇｅｌｅｔに対して、対応候補エッジのいずれかをそれぞれ対応付ける。
【００４７】
ここで、図６を用いて、対応付け部２６による対応付け処理について説明する。
【００４８】
対応付け部２６は、まず、全てのＥｄｇｅｌｅｔにおいて、その各々の対応候補エッジと３次元形状モデルの線分との方向の差を算出する（図６（ａ））。そして、その方向の差に基づいて、ヒストグラムを作成する（図６（ｂ））。
【００４９】
このヒストグラムにおけるピークは、全てのＥｄｇｅｌｅｔにおける各対応候補エッジと３次元形状モデルの線分との方向の差の最頻値を示している。このとき、ヒストグラムの横軸（ｂｉｎ）の大きさが調整パラメータとなる。ｂｉｎの単位が細かいほど、エッジの方向の分解能が高くなる。その一方で、エッジの方向は、このｂｉｎを単位としたノイズの影響により変動するため、ｂｉｎの単位が細かすぎると、ノイズの影響を受け易くなる。そこで、本実施形態においては、ヒストグラムのｂｉｎの単位は、１［ｄｅｇｒｅｅ（角度）］とする。
【００５０】
次に、対応付け部２６は、各Ｅｄｇｅｌｅｔ毎に、３次元形状モデルを構成する線分と各対応候補エッジとにおける方向を比較し、その差が、最頻値から所定範囲内にあるか否かをチェックする。そして、両方向の差が、最頻値から所定範囲内にある対応候補エッジを対応エッジとして残し、それ以外の対応候補エッジを誤対応するエッジとみなして除外する。このとき、複数の対応候補エッジが残った場合には、その残った対応候補エッジの中からＥｄｇｅｌｅｔから最も近傍に検出された対応候補エッジを対応するエッジとして選択する。
【００５１】
ここで、最頻値から所定範囲内にあるか否かをチェックする際に用いる閾値が調整パラメータとなる。このパラメータは、エッジ方向の検出誤差がどの程度発生するかによって調整することが望ましい。本実施形態においては、エッジ方向の検出誤差が５［ｄｅｇｒｅｅ］ほど発生するとみなし、閾値を５［ｄｅｇｒｅｅ］に設定する。このような処理を、全Ｅｄｇｅｌｅｔの全対応候補エッジに対して繰り返し実施し、全てのＥｄｇｅｌｅｔに対して対応エッジを判定する。全てのＥｄｇｅｌｅｔに対して対応エッジの判定が済むと、Ｓ１０４の処理を終了し、Ｓ１０５の処理に進む。
【００５２】
[Ｓ１０５]
次に、位置姿勢計測装置１は、位置姿勢算出部２７において、物体に係わる３次元情報（位置姿勢計測装置１に対する計測対象物体の位置及び姿勢）を算出（計測）する。本実施形態においては、非線形最適化計算を用いて、物体の概略の位置及び姿勢を示す概略値を反復演算により補正することにより、物体の位置及び姿勢を算出する。
【００５３】
より具体的には、非線形最適化手法の一つであるガウス−ニュートン（Gauss-Newton）法により反復演算し、物体の位置及び姿勢の概略値を補正する。これにより、物体の位置及び姿勢を算出する。なお、位置姿勢算出処理は、ガウス−ニュートン法に限られず、例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いても良いし、よりシンプルな方法である最急降下法を用いても良い。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。
【００５４】
ここでは、対応付け部２６により対応付けられた２次元画像のエッジ（の３次元座標）と、（概略値に基づいてカメラ座標系に変換した）３次元形状モデルにおける線分との組を用いて、物体の位置及び姿勢を算出する。すなわち、当該対応するエッジと線分との３次元空間における距離の差に基づいて、物体の位置及び姿勢の概略値の最適化する。
【００５５】
より具体的には、３次元空間中での点と直線との符号付距離を、物体の位置及び姿勢の関数として線形近似する。そして、符号付距離が０になるような位置及び姿勢の微小変化に係わる線形の連立方程式を立式して解く。これにより、物体の位置及び姿勢の微小変化を求め、この値を補正する処理を繰り返す。なお、物体の位置及び姿勢の算出については、従来の技術（例えば、非特許文献１に記載された技術）を用いれば良いため、ここでは、その詳細な処理についての説明は省略する。
【００５６】
[Ｓ１０６]
最後に、位置姿勢計測装置１は、この処理を終了するか否かの判定を行なう。この処理を終了するか否かは、例えば、ユーザに処理の終了が指示されたか否かに基づき判定される。ここで、ユーザにより処理の終了が指示された場合には、そのままこの処理を終了し、そうでなければ、新たな２次元画像を取得し、再度、上述した処理を行なう。
【００５７】
以上説明したように本実施形態によれば、２次元画像（濃淡画像）上における輝度の変化から直接的な影響が少ない情報（エッジの方向）を用いて、３次元形状モデルを構成する線分に対して対応付けるエッジを判定する。そのため、物体の表面情報、光源状態、物体を観察する視点位置によらず、３次元形状モデルにおける線分と２次元画像上のエッジとの対応付けを安定した精度で行なえる。
【００５８】
これにより、輝度分布による識別が困難な物体や、光源の状態変化、物体と光源との相対的な位置姿勢変化があるようなシーンにおいても、物体の位置及び姿勢の計測をロバストに行なうことができる。
【００５９】
以上が本発明の代表的な実施形態の一例であるが、本発明は、上記及び図面に示す実施形態に限定することなく、その要旨を変更しない範囲内で適宜変形して実施できるものである。ここでいくつか変形例について列挙する。
【００６０】
（変形例１）
上述した実施形態においては、対応候補エッジの中から対応エッジを選択する際に、全てのＥｄｇｅｌｅｔにおける全対応候補エッジと３次元形状モデルの線分との方向の差に基づいて算出した最頻値を基準値として用いたが、これに限られない。例えば、例えば、最頻値の代わりに、Ｅｄｇｅｌｅｔと対応候補エッジとの２次元方向の差の中央値を用いても良い。具体的には、全てのＥｄｇｅｌｅｔの全対応候補エッジに対して、３次元形状モデルの線分と対応候補エッジとの２次元方向の差の中央値（平均値）を算出し、当該中央値から所定範囲内の方向の差を示すエッジを対応するエッジとして選択（判定）しても良い。
【００６１】
また、このような基準値（最頻値、中央値）に限られず、基準値は、３次元形状モデルにおける線分の２次元方向と対応候補エッジの方向とが、全体として類似するか否かを判定することができる指標であれば良い。
【００６２】
（変形例２）
上述した実施形態においては、２次元画像上から検出する画像特徴がエッジである場合について説明したが、画像特徴は、エッジに限られない。例えば、３次元形状モデルとして、３次元面の変曲点とその方向を保持しておく。そして、２次元画像上の特徴として、濃淡が緩やかに変化する領域を検出し、その特徴の方向として濃度が変化する方向を算出する方法を採っても良い。具体的には、２次元画像上での濃淡変化方向と３次元面の変曲点の方向とに基づいて、３次元面の編曲点と、２次元画像上の濃淡変化部分とを対応付けることで物体の位置及び姿勢を計測（推定）するように構成しても良い。２次元画像上で位置と方向を表現できる特徴である限り、特徴の表現に制限はない。
【００６３】
（その他の実施形態）
本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（又はＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【技術分野】
【０００１】
本発明は、位置姿勢計測装置、その処理方法及びプログラムに関する。
【背景技術】
【０００２】
近年、これまで人間が行なっていた複雑なタスクをロボットが代わりに行なうようになりつつある。複雑なタスクの代表例として、工業製品の組み立てが挙げられる。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組み立てを行なう。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。
【０００３】
一般に、このような位置及び姿勢の計測方法としては、カメラが撮像する２次元画像上から検出される画像特徴や距離センサから得られる距離画像に対して、物体の３次元形状モデルをモデルフィッティングさせる（当てはめる）技術が知られている。
【０００４】
非特許文献１には、２次元画像上から検出される画像特徴としてエッジを利用する技術が開示されている。この技術では、物体の形状を３次元線分の集合によって表し、物体の概略の位置及び姿勢の情報を既知として、当該２次元画像上で検出されるエッジに３次元線分の投影像をモデルフィッティングさせる。テクスチャのない直線を基調とした人工的な物体が多い環境下等で物体の位置及び姿勢を計測する場合、エッジを利用した計測が適している。
【０００５】
ここで、２次元画像から検出される画像特徴に基づいて、物体の位置及び姿勢の計測を正確に行なうためには、検出される画像特徴と３次元形状モデルにおける幾何特徴とが正確に対応付けられる必要がある。
【０００６】
上述した非特許文献１では、３次元線分を２次元画像上に投影した位置の近傍で検出されたエッジと３次元線分とを対応付けている。すなわち、この方法では、３次元線分の投影像の最近傍に検出されたエッジが正しく対応するエッジとみなしている。そのため、最近傍に検出されたエッジが、本来対応するはずのないエッジであれば、物体の位置及び姿勢を正確に計測できないか、また、計測の精度が低下するかしてしまう。特に、物体の概略の位置及び姿勢が不正確な場合や、また、２次元画像が複雑で対応候補となるエッジが多量に検出される場合には、３次元形状モデルの線分とエッジとの対応付けに誤対応が発生してしまう。
【０００７】
このような問題を解決するため、非特許文献２では、エッジ周辺の輝度値を利用することで３次元形状モデルの線分と濃淡画像中のエッジとの対応付けの精度を向上させる手法が開示されている。具体的には、学習画像を用いて、３次元形状モデル中の線分が画像上でどのような輝度分布として検出されるかを予め学習させておく。そして、濃淡画像中から実際に検出されたエッジ周辺の輝度分布との類似度を算出し、その結果に基づいてエッジの対応付けを行なう。この技術は、対象物体の表面情報として、ユニークに識別できるような輝度分布が含まれているような場合に有用である。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】T. Drummond and R. Cipolla, “Real-time visual tracking of complex structures," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002.
【非特許文献２】H. Wuest. F. Vial, and D. Stricker, "Adaptive line tracking with multiple hypotheses for augmented reality," Proc. The Fourth Int'l Symp. on Mixed and Augmented Reality (ISMAR05), pp.62-69, 2005.
【非特許文献３】L. Vacchetti, V. Lepetit, and P. Fua, “Combining edge and texture information for real-time accurate 3D camera tracking, " Proc. 3rd IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR ‘04) , pp.48-57, 2004.
【非特許文献４】R. Y. Tsai, "A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses", IEEE Journal of Robotics and Automation, vol.RA-3, no.4, 1987.
【発明の概要】
【発明が解決しようとする課題】
【０００９】
上述した通り、濃淡画像上の輝度を利用してエッジを対応付ける手法では、対象物体にユニークに識別可能な表面色情報がある場合など、見かけ上の輝度の分布に再現性が高い場合には有効に働く。
【００１０】
しかし、濃淡画像上の輝度は、物体の表面情報、光源状態、物体を観察する視点位置によって多様に変化するため、対象物体の表面色や環境状況によっては、３次元形状モデル中の幾何特徴と画像特徴とに誤対応が生じる可能性が高くなる。
【００１１】
本発明は、上記課題に鑑みてなされたものであり、２次元画像上における画像特徴の方向を用いて、当該画像特徴と３次元形状モデルを構成する幾何特徴と対応付け、その対応付け結果に基づいて物体の位置及び姿勢の計測を行なうようにした技術を提供する。
【課題を解決するための手段】
【００１２】
上記課題を解決するため、本発明の一態様による位置姿勢計測装置は、物体の幾何特徴を表す３次元形状モデルを保持するモデル保持手段と、前記物体の位置及び姿勢を示す概略値を取得する概略値取得手段と、撮像装置により撮像された前記物体の２次元画像を取得する画像取得手段と、前記概略値に基づいて前記３次元形状モデルにおける幾何特徴を前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する投影手段と、前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する検出手段と、前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける対応付け手段と、前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する位置姿勢算出手段とを具備する。
【発明の効果】
【００１３】
本発明によれば、３次元形状モデルを構成する幾何特徴と２次元画像上から検出した画像特徴とを従来よりも正確に対応付けられる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施の形態に係わる位置姿勢計測装置１の構成の一例を示す図。
【図２】３次元形状モデルの定義の一例を示す図。
【図３】物体の位置及び姿勢を計測する際の全体的な処理の流れの一例を示すフローチャート。
【図４】２次元画像からエッジを検出する際の処理の流れの一例を示すフローチャート。
【図５】エッジ検出の概要を示す図。
【図６】対応付け処理の概要を示す図。
【発明を実施するための形態】
【００１５】
以下、本発明の一実施の形態について添付図面を参照して詳細に説明する。
【００１６】
図１は、本発明の一実施の形態に係わる位置姿勢計測装置１の構成の一例を示す図である。なお、本実施形態においては、計測対象となる物体（以下、計測対象物体、又は単に物体と呼ぶ場合もある）の概略の位置及び姿勢が既知であると仮定する。そして、その概略値に基づいて３次元形状モデルデータ（以下、単に、３次元形状モデルと呼ぶ）と２次元画像（濃淡画像）とを対応させることにより物体の位置及び姿勢の計測（推定）を行なう。
【００１７】
ここで、計測対象とする物体がほぼ剛体であるとみなすと、対象物体が一体として変位及び回転していたとしても、物体面上の各所の相互関係は変化しない。物体の概略の位置及び姿勢が、実際の物体の位置及び姿勢とある程度近い場合は、面と面の境界といった物体面上の３次元線分の２次元投影像の方向に関しても、この相互関係は概ね維持される傾向となる。
【００１８】
そこで、本実施形態においては、この特性に着目し、３次元形状モデルの３次元線分を投影した投影像の方向と、２次元画像上のエッジの方向との差を算出して、その差が全体の最頻値に近いエッジを対応するエッジとして判定する。これにより、物体の位置及び姿勢の計測を行なう。
【００１９】
ここで、位置姿勢計測装置１には、撮像装置３０が接続されている。なお、撮像装置３０は、位置姿勢計測装置１内に内蔵されていても構わない。
【００２０】
撮像装置３０は、例えば、カメラであり、２次元画像を撮像する。本実施形態においては、撮像装置３０により撮像される２次元画像が、計測対象となる物体の輝度を表現する濃淡画像である場合を例に挙げて説明するが、これに限られず、例えば、カラー画像が撮像されても良い。
【００２１】
撮像装置３０により撮像された２次元画像は、２次元画像取得部２２を介して位置姿勢計測装置１内に入力される。なお、カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータ（２次元画像撮像時のパラメータ）は、例えば、非特許文献４に記載される方法によって予めキャリブレーションしておく。
【００２２】
位置姿勢計測装置１は、モデル保持部２１に保持された計測対象となる物体の形状を表す３次元形状モデルに基づいて、２次元画像内に含まれる物体の位置及び姿勢の計測を行なう。なお、本実施形態においては、位置姿勢計測装置１（モデル保持部２１）に保持された３次元形状モデルが、現実に撮像される物体の形状に即していることを前提とする。
【００２３】
ここで、位置姿勢計測装置１は、その機能的な構成として、モデル保持部２１と、２次元画像取得部２２と、概略値取得部２３と、モデル投影部２４と、画像特徴検出部２５と、対応付け部２６と、位置姿勢算出部２７とを具備して構成される。
【００２４】
モデル保持部２１は、計測対象となる物体の３次元形状モデルを保持する。３次元形状モデルは、例えば、物体の幾何特徴（形状）を複数の線分（エッジ）を用いて定義する。
【００２５】
より具体的には、３次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。物体の３次元形状モデル１０は、図２（ａ）に示すように、点Ｐ１〜点Ｐ１４の１４点から構成される。点Ｐ１〜点Ｐ１４は、図２（ｃ）に示すように、３次元座標値により表される。なお、３次元形状モデル１０に規定される基準座標系の原点をＰ１２とする。そして、点Ｐ１２から点Ｐ１３に向かう方向をｘ軸とし、点Ｐ１２から点Ｐ８に向かう方向をｙ軸とし、点Ｐ１２から点Ｐ１１に向かう方向をｚ軸とする。なお、ｙ軸は、鉛直上方向（重力軸の反対方向）に一致する。
【００２６】
３次元形状モデル１０は、図２（ｂ）に示すように、線分Ｌ１〜Ｌ１６により構成されている。線分Ｌ１〜Ｌ１６は、図２（ｄ）に示すように、線分を構成する点のＩＤ（識別子）によって表される。なお、ここでは図示を省略するが、３次元形状モデル１０は、面の情報も保持する。この場合、各面は、各面を構成する頂点のＩＤにより表される。
【００２７】
概略値取得部２３は、位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を取得する。本実施形態において、位置姿勢計測装置１に対する物体の位置及び姿勢とは、撮像装置３０（カメラ座標系）を基準とした物体の位置及び姿勢を示すが、必ずしも、撮像装置３０を基準にする必要はない。例えば、撮像装置３０の座標系に対する物体の相対的な位置及び姿勢が既知であり、且つその位置及び姿勢が変化しないのであれば、位置姿勢計測装置１におけるその他の部分を基準としても良い。また、本実施形態においては、物体の位置及び姿勢の概略値として、位置姿勢計測装置１が当該物体から過去（例えば、直前）に計測した計測値を用いる。但し、物体の位置及び姿勢の概略値は、必ずしも、このような値である必要はない。例えば、過去に計測した物体の位置及び姿勢の計測値に対して時系列フィルタリング処理（例えば、線形フィルタ、カルマンフィルタ）を実施し、物体の運動速度や角速度を推定する。そして、その推定結果に基づいて物体の位置及び姿勢を予測した値を概略値としても良い。また、外部のセンサから得られる物体の位置及び姿勢を概略値としても良い。ここで、センサは、物体の位置及び姿勢を６自由度で計測できれば良く、その方式（例えば、磁気式、光学式、超音波式）は、特に問わない。なお、物体の置かれているおおよその位置や姿勢が予め分かっているのであれば、その値を概略値として用いても良い。
【００２８】
モデル投影部２４は、撮像装置３０の内部パラメータや概略値取得部２３により取得された概略値を用いて、モデル保持部２１に保持された３次元形状モデルを２次元画像上（濃淡画像上）に投影する。そして、この投影の結果として、３次元形状モデルを構成する線分の２次元座標及び２次元方向を算出する。なお、３次元形状モデルの投影方法については後述する。
【００２９】
画像特徴検出部２５は、２次元画像取得部２２により取得された２次元画像から画像特徴としてエッジを検出する。そして、そのエッジの方向として輝度勾配の方向の法線を算出する。エッジ検出方法については後述する。
【００３０】
対応付け部２６は、画像特徴検出部２５により検出された各画像特徴（各エッジ）と、３次元形状モデルの各幾何特徴（３次元形状モデルを構成する各線分）とを対応付ける。この対応付け処理では、モデル投影部２４により算出された３次元形状モデルの線分の２次元方向と、２次元画像上のエッジの方向とにおける差に基づいて、２次元画像上のエッジに対して３次元形状モデルの線分を対応付ける。なお、対応付け処理の詳細については後述する。
【００３１】
位置姿勢算出部２７は、対応付け部２６により対応付けられた３次元形状モデルの線分と２次元画像上のエッジとの対応関係（組）を用いて、物体の位置及び姿勢を算出する。なお、物体の位置及び姿勢の算出処理の詳細については後述する。
【００３２】
以上が、位置姿勢計測装置１の構成の一例についての説明である。なお、上記説明した、位置姿勢計測装置１には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御手段、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。
【００３３】
次に、図３を用いて、図１に示す位置姿勢計測装置１により計測対象物体の位置及び姿勢を計測する際の処理の流れの一例について説明する。
【００３４】
[Ｓ１０１]
位置姿勢計測装置１は、まず、初期化処理を行なう。この初期化処理では、例えば、概略値取得部２３において、位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を取得する。本実施形態においては、上述した通り、物体の位置及び姿勢の概略値として、位置姿勢計測装置１が当該物体から過去（例えば、直前）に計測した計測値を用いる。また、初期化処理においては、撮像装置３０の内部パラメータ等の取得も行なわれる。
【００３５】
[Ｓ１０２]
位置姿勢計測装置１は、２次元画像取得部２２において、撮像装置３０を介して物体の２次元画像（濃淡画像）を取得する。
【００３６】
[Ｓ１０３]
位置姿勢計測装置１は、画像特徴検出部２５において、Ｓ１０２の処理で取得した２次元画像上から物体の画像特徴としてエッジを検出する。エッジは、濃度勾配の極値となる点である。本実施形態においては、非特許文献３に開示される方法を用いてエッジの検出を行なう。
【００３７】
ここで、Ｓ１０３におけるエッジ検出について説明する。図４には、２次元画像（濃淡画像）からエッジを検出する際の処理の流れの一例が示される。
【００３８】
[Ｓ２０１]
位置姿勢計測装置１は、まず、モデル投影部２４において、Ｓ１０１の処理で取得した概略値と、撮像装置３０の内部パラメータとを用いて、３次元形状モデルを２次元画像上へ投影する。なお、３次元形状モデルの投影は計算により行なわれる。そのため、この投影により、３次元形状モデルを構成する線分各々（幾何特徴各々）の２次元画像上における座標と方向が計算される。３次元形状モデル（線分）の投影像は、２次元画像上でも線分となる。
【００３９】
[Ｓ２０２]
続いて、位置姿勢計測装置１は、モデル投影部２４において、Ｓ２０１の処理で算出された投影像上（投影線分上）に制御点を設定する。制御点は、投影線分を等間隔に分割するように設定される。以降、この制御点をＥｄｇｅｌｅｔと呼称する。
【００４０】
位置姿勢計測装置１は、Ｅｄｇｅｌｅｔ毎（制御点毎）に、３次元形状モデルを構成する線分の３次元座標及び３次元方向と、投影結果として得られる２次元座標及び２次元方向とを保持する。なお、Ｅｄｇｅｌｅｔの数は、物体の形状等に合わせて適宜変更する。また、Ｅｄｇｅｌｅｔの数が増える程、処理時間が長くなるため、Ｅｄｇｅｌｅｔの総数が一定になるように、Ｅｄｇｅｌｅｔ間の間隔を適宜変更しても良い。
【００４１】
[Ｓ２０３]
Ｅｄｇｅｌｅｔ（制御点）の設定が済むと、位置姿勢計測装置１は、画像特徴検出部２５において、Ｓ２０２の処理で設定したＥｄｇｅｌｅｔ各々（制御点各々）に対応する２次元画像上のエッジを検出する。図５を用いて、本実施形態に係わるエッジ検出の概要について説明する。
【００４２】
エッジの検出は、図５（ａ）に示すように、投影像（投影線分）の法線方向に平行で且つＥｄｇｅｌｅｔを通過する線分（以下、探索ライン）上からエッジを１次元探索することにより行なう。ここで、画像特徴検出部２５は、図５（ｂ）に示すように、探索ライン上における２次元画像上の濃度勾配から極値を算出する（この場合、複数の極値（エッジ）が存在する）。すなわち、エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。そして、探索ライン上で検出された全てのエッジの２次元座標を、各Ｅｄｇｅｌｅｔの対応候補のエッジ（対応候補エッジ）の位置として保持する。このような処理により、３次元形状モデルの幾何特徴毎（より具体的には、Ｅｄｇｅｌｅｔ毎）に、対応候補となるエッジが２次元画像上から検出される。
【００４３】
対応候補エッジの検出が済むと、位置姿勢計測装置１は、画像特徴検出部２５において、Ｓ２０３の処理で検出された各対応候補エッジの方向を算出する。エッジの方向は、対応候補エッジ周辺の画素に対してエッジ検出フィルタを用いてフィルタリング処理し、それにより得られた画素の勾配方向に基づいて求めれば良い。本実施形態においては、対応候補エッジから所定範囲内の画素（例えば、９画素）に対して、水平方向のｓｏｂｅｌフィルタと垂直方向のｓｏｂｅｌフィルタとをかけ、その応答を正規化する。これにより、画素の勾配方向を算出する。そして、勾配方向の法線方向を求め、それをエッジの方向として取得する。
【００４４】
なお、画素の勾配方向の算出には、必ずしも、ｓｏｂｅｌフィルタを用いた手法を採用する必要はない。すなわち、注目したエッジの勾配方向を算出できれば良く、その手法については特に問わない。例えば、ｐｒｅｗｉｔｔフィルタなどの別のエッジ検出フィルタを用いても良い。また、ｃａｎｎｙアルゴリズムにより画像全体からエッジを検出し、それにより、勾配方向を算出しても良い。
【００４５】
[Ｓ２０４]
以上の処理をＥｄｇｅｌｅｔ全てに対して実施し、それにより、全てのＥｄｇｅｌｅｔにおける全ての対応候補エッジの位置及び方向を算出する。
【００４６】
[Ｓ１０４]
図３の説明に戻り、位置姿勢計測装置１は、対応付け２６において、３次元形状モデルを構成する線分における各Ｅｄｇｅｌｅｔに対して、対応候補エッジのいずれかをそれぞれ対応付ける。
【００４７】
ここで、図６を用いて、対応付け部２６による対応付け処理について説明する。
【００４８】
対応付け部２６は、まず、全てのＥｄｇｅｌｅｔにおいて、その各々の対応候補エッジと３次元形状モデルの線分との方向の差を算出する（図６（ａ））。そして、その方向の差に基づいて、ヒストグラムを作成する（図６（ｂ））。
【００４９】
このヒストグラムにおけるピークは、全てのＥｄｇｅｌｅｔにおける各対応候補エッジと３次元形状モデルの線分との方向の差の最頻値を示している。このとき、ヒストグラムの横軸（ｂｉｎ）の大きさが調整パラメータとなる。ｂｉｎの単位が細かいほど、エッジの方向の分解能が高くなる。その一方で、エッジの方向は、このｂｉｎを単位としたノイズの影響により変動するため、ｂｉｎの単位が細かすぎると、ノイズの影響を受け易くなる。そこで、本実施形態においては、ヒストグラムのｂｉｎの単位は、１［ｄｅｇｒｅｅ（角度）］とする。
【００５０】
次に、対応付け部２６は、各Ｅｄｇｅｌｅｔ毎に、３次元形状モデルを構成する線分と各対応候補エッジとにおける方向を比較し、その差が、最頻値から所定範囲内にあるか否かをチェックする。そして、両方向の差が、最頻値から所定範囲内にある対応候補エッジを対応エッジとして残し、それ以外の対応候補エッジを誤対応するエッジとみなして除外する。このとき、複数の対応候補エッジが残った場合には、その残った対応候補エッジの中からＥｄｇｅｌｅｔから最も近傍に検出された対応候補エッジを対応するエッジとして選択する。
【００５１】
ここで、最頻値から所定範囲内にあるか否かをチェックする際に用いる閾値が調整パラメータとなる。このパラメータは、エッジ方向の検出誤差がどの程度発生するかによって調整することが望ましい。本実施形態においては、エッジ方向の検出誤差が５［ｄｅｇｒｅｅ］ほど発生するとみなし、閾値を５［ｄｅｇｒｅｅ］に設定する。このような処理を、全Ｅｄｇｅｌｅｔの全対応候補エッジに対して繰り返し実施し、全てのＥｄｇｅｌｅｔに対して対応エッジを判定する。全てのＥｄｇｅｌｅｔに対して対応エッジの判定が済むと、Ｓ１０４の処理を終了し、Ｓ１０５の処理に進む。
【００５２】
[Ｓ１０５]
次に、位置姿勢計測装置１は、位置姿勢算出部２７において、物体に係わる３次元情報（位置姿勢計測装置１に対する計測対象物体の位置及び姿勢）を算出（計測）する。本実施形態においては、非線形最適化計算を用いて、物体の概略の位置及び姿勢を示す概略値を反復演算により補正することにより、物体の位置及び姿勢を算出する。
【００５３】
より具体的には、非線形最適化手法の一つであるガウス−ニュートン（Gauss-Newton）法により反復演算し、物体の位置及び姿勢の概略値を補正する。これにより、物体の位置及び姿勢を算出する。なお、位置姿勢算出処理は、ガウス−ニュートン法に限られず、例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いても良いし、よりシンプルな方法である最急降下法を用いても良い。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。
【００５４】
ここでは、対応付け部２６により対応付けられた２次元画像のエッジ（の３次元座標）と、（概略値に基づいてカメラ座標系に変換した）３次元形状モデルにおける線分との組を用いて、物体の位置及び姿勢を算出する。すなわち、当該対応するエッジと線分との３次元空間における距離の差に基づいて、物体の位置及び姿勢の概略値の最適化する。
【００５５】
より具体的には、３次元空間中での点と直線との符号付距離を、物体の位置及び姿勢の関数として線形近似する。そして、符号付距離が０になるような位置及び姿勢の微小変化に係わる線形の連立方程式を立式して解く。これにより、物体の位置及び姿勢の微小変化を求め、この値を補正する処理を繰り返す。なお、物体の位置及び姿勢の算出については、従来の技術（例えば、非特許文献１に記載された技術）を用いれば良いため、ここでは、その詳細な処理についての説明は省略する。
【００５６】
[Ｓ１０６]
最後に、位置姿勢計測装置１は、この処理を終了するか否かの判定を行なう。この処理を終了するか否かは、例えば、ユーザに処理の終了が指示されたか否かに基づき判定される。ここで、ユーザにより処理の終了が指示された場合には、そのままこの処理を終了し、そうでなければ、新たな２次元画像を取得し、再度、上述した処理を行なう。
【００５７】
以上説明したように本実施形態によれば、２次元画像（濃淡画像）上における輝度の変化から直接的な影響が少ない情報（エッジの方向）を用いて、３次元形状モデルを構成する線分に対して対応付けるエッジを判定する。そのため、物体の表面情報、光源状態、物体を観察する視点位置によらず、３次元形状モデルにおける線分と２次元画像上のエッジとの対応付けを安定した精度で行なえる。
【００５８】
これにより、輝度分布による識別が困難な物体や、光源の状態変化、物体と光源との相対的な位置姿勢変化があるようなシーンにおいても、物体の位置及び姿勢の計測をロバストに行なうことができる。
【００５９】
以上が本発明の代表的な実施形態の一例であるが、本発明は、上記及び図面に示す実施形態に限定することなく、その要旨を変更しない範囲内で適宜変形して実施できるものである。ここでいくつか変形例について列挙する。
【００６０】
（変形例１）
上述した実施形態においては、対応候補エッジの中から対応エッジを選択する際に、全てのＥｄｇｅｌｅｔにおける全対応候補エッジと３次元形状モデルの線分との方向の差に基づいて算出した最頻値を基準値として用いたが、これに限られない。例えば、例えば、最頻値の代わりに、Ｅｄｇｅｌｅｔと対応候補エッジとの２次元方向の差の中央値を用いても良い。具体的には、全てのＥｄｇｅｌｅｔの全対応候補エッジに対して、３次元形状モデルの線分と対応候補エッジとの２次元方向の差の中央値（平均値）を算出し、当該中央値から所定範囲内の方向の差を示すエッジを対応するエッジとして選択（判定）しても良い。
【００６１】
また、このような基準値（最頻値、中央値）に限られず、基準値は、３次元形状モデルにおける線分の２次元方向と対応候補エッジの方向とが、全体として類似するか否かを判定することができる指標であれば良い。
【００６２】
（変形例２）
上述した実施形態においては、２次元画像上から検出する画像特徴がエッジである場合について説明したが、画像特徴は、エッジに限られない。例えば、３次元形状モデルとして、３次元面の変曲点とその方向を保持しておく。そして、２次元画像上の特徴として、濃淡が緩やかに変化する領域を検出し、その特徴の方向として濃度が変化する方向を算出する方法を採っても良い。具体的には、２次元画像上での濃淡変化方向と３次元面の変曲点の方向とに基づいて、３次元面の編曲点と、２次元画像上の濃淡変化部分とを対応付けることで物体の位置及び姿勢を計測（推定）するように構成しても良い。２次元画像上で位置と方向を表現できる特徴である限り、特徴の表現に制限はない。
【００６３】
（その他の実施形態）
本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（又はＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【特許請求の範囲】
【請求項１】
物体の幾何特徴を表す３次元形状モデルを保持するモデル保持手段と、
前記物体の位置及び姿勢を示す概略値を取得する概略値取得手段と、
撮像装置により撮像された前記物体の２次元画像を取得する画像取得手段と、
前記概略値に基づいて前記３次元形状モデルにおける幾何特徴を前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する投影手段と、
前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する検出手段と、
前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける対応付け手段と、
前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する位置姿勢算出手段と
を具備することを特徴とする位置姿勢計測装置。
【請求項２】
前記検出手段は、
前記投影手段により前記２次元画像上に投影された３次元形状モデルの幾何特徴各々に対応して当該２次元画像上から複数の画像特徴を検出するとともに当該画像特徴の方向を算出し、
前記対応付け手段は、
全ての前記幾何特徴に対応して各幾何特徴毎に当該幾何特徴に対する各画像特徴の方向の差を算出し、当該算出により得られた全ての幾何特徴における全ての画像特徴の方向の差に基づいて基準値を算出する算出手段と、
各幾何特徴に対応して検出された複数の画像特徴の中から、前記基準値から所定範囲内の方向の差を示すいずれかの画像特徴を選択する選択手段と
を具備し、
各幾何特徴に対して前記選択された画像特徴を対応付ける
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項３】
前記３次元形状モデルにおける幾何特徴は、前記物体の形状を示す線分であり、
前記検出手段は、
前記投影手段により前記２次元画像上に投影された３次元形状モデルの線分に対して等間隔に設定された複数の制御点各々に対応して当該２次元画像上から画像特徴として複数のエッジを検出するとともに当該エッジの方向を算出し、
前記対応付け手段は、
全ての前記制御点に対応して各制御点毎に前記線分に対する各エッジの方向の差を算出し、当該算出により得られた全ての制御点における全てのエッジの方向の差に基づいて基準値を算出する算出手段と、
各制御点に対応して検出された複数のエッジの中から、前記基準値から所定範囲内の方向の差を示すいずれかのエッジを選択する選択手段と
を具備し、
前記３次元形状モデルの線分における各制御点に対して前記選択されたエッジを対応付ける
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項４】
前記基準値は、
前記全ての制御点における全てのエッジの方向の差の最頻値である
ことを特徴とする請求項３記載の位置姿勢計測装置。
【請求項５】
前記基準値は、
前記全ての制御点における全てのエッジの方向の差の平均値である
ことを特徴とする請求項３記載の位置姿勢計測装置。
【請求項６】
位置姿勢計測装置の処理方法であって、
概略値取得手段が、物体の位置及び姿勢を示す概略値を取得する工程と、
画像取得手段が、撮像装置により撮像された前記物体の２次元画像を取得する工程と、
投影手段が、前記物体の幾何特徴を表す３次元形状モデルにおける幾何特徴を前記概略値に基づいて前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する工程と、
検出手段が、前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する工程と、
対応付け手段が、前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける工程と、
位置姿勢算出手段が、前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する工程と
を含むことを特徴とする位置姿勢計測装置の処理方法。
【請求項７】
コンピュータを、
物体の幾何特徴を表す３次元形状モデルを保持するモデル保持手段、
前記物体の位置及び姿勢を示す概略値を取得する概略値取得手段、
撮像装置により撮像された前記物体の２次元画像を取得する画像取得手段、
前記概略値に基づいて前記３次元形状モデルにおける幾何特徴を前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する投影手段、
前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する検出手段、
前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける対応付け手段、
前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する位置姿勢算出手段
として機能させるためのプログラム。
【請求項１】
物体の幾何特徴を表す３次元形状モデルを保持するモデル保持手段と、
前記物体の位置及び姿勢を示す概略値を取得する概略値取得手段と、
撮像装置により撮像された前記物体の２次元画像を取得する画像取得手段と、
前記概略値に基づいて前記３次元形状モデルにおける幾何特徴を前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する投影手段と、
前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する検出手段と、
前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける対応付け手段と、
前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する位置姿勢算出手段と
を具備することを特徴とする位置姿勢計測装置。
【請求項２】
前記検出手段は、
前記投影手段により前記２次元画像上に投影された３次元形状モデルの幾何特徴各々に対応して当該２次元画像上から複数の画像特徴を検出するとともに当該画像特徴の方向を算出し、
前記対応付け手段は、
全ての前記幾何特徴に対応して各幾何特徴毎に当該幾何特徴に対する各画像特徴の方向の差を算出し、当該算出により得られた全ての幾何特徴における全ての画像特徴の方向の差に基づいて基準値を算出する算出手段と、
各幾何特徴に対応して検出された複数の画像特徴の中から、前記基準値から所定範囲内の方向の差を示すいずれかの画像特徴を選択する選択手段と
を具備し、
各幾何特徴に対して前記選択された画像特徴を対応付ける
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項３】
前記３次元形状モデルにおける幾何特徴は、前記物体の形状を示す線分であり、
前記検出手段は、
前記投影手段により前記２次元画像上に投影された３次元形状モデルの線分に対して等間隔に設定された複数の制御点各々に対応して当該２次元画像上から画像特徴として複数のエッジを検出するとともに当該エッジの方向を算出し、
前記対応付け手段は、
全ての前記制御点に対応して各制御点毎に前記線分に対する各エッジの方向の差を算出し、当該算出により得られた全ての制御点における全てのエッジの方向の差に基づいて基準値を算出する算出手段と、
各制御点に対応して検出された複数のエッジの中から、前記基準値から所定範囲内の方向の差を示すいずれかのエッジを選択する選択手段と
を具備し、
前記３次元形状モデルの線分における各制御点に対して前記選択されたエッジを対応付ける
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項４】
前記基準値は、
前記全ての制御点における全てのエッジの方向の差の最頻値である
ことを特徴とする請求項３記載の位置姿勢計測装置。
【請求項５】
前記基準値は、
前記全ての制御点における全てのエッジの方向の差の平均値である
ことを特徴とする請求項３記載の位置姿勢計測装置。
【請求項６】
位置姿勢計測装置の処理方法であって、
概略値取得手段が、物体の位置及び姿勢を示す概略値を取得する工程と、
画像取得手段が、撮像装置により撮像された前記物体の２次元画像を取得する工程と、
投影手段が、前記物体の幾何特徴を表す３次元形状モデルにおける幾何特徴を前記概略値に基づいて前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する工程と、
検出手段が、前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する工程と、
対応付け手段が、前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける工程と、
位置姿勢算出手段が、前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する工程と
を含むことを特徴とする位置姿勢計測装置の処理方法。
【請求項７】
コンピュータを、
物体の幾何特徴を表す３次元形状モデルを保持するモデル保持手段、
前記物体の位置及び姿勢を示す概略値を取得する概略値取得手段、
撮像装置により撮像された前記物体の２次元画像を取得する画像取得手段、
前記概略値に基づいて前記３次元形状モデルにおける幾何特徴を前記２次元画像上に投影し、当該２次元画像上に投影された３次元形状モデルの幾何特徴の方向を算出する投影手段、
前記２次元画像から画像特徴を検出するとともに当該画像特徴の方向を算出する検出手段、
前記２次元画像から算出された画像特徴の方向と、前記３次元形状モデルから算出された幾何特徴の方向とを比較することにより、前記画像特徴と前記幾何特徴とを対応付ける対応付け手段、
前記対応付け手段により対応付けられた前記画像特徴と前記幾何特徴とにおける距離に基づいて前記概略値を補正することにより前記物体の位置及び姿勢を算出する位置姿勢算出手段
として機能させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１２−２７６１（Ｐ２０１２−２７６１Ａ）
【公開日】平成２４年１月５日（２０１２．１．５）
【国際特許分類】
【出願番号】特願２０１０−１３９９４８（Ｐ２０１０−１３９９４８）
【出願日】平成２２年６月１８日（２０１０．６．１８）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２４年１月５日（２０１２．１．５）
【国際特許分類】
【出願日】平成２２年６月１８日（２０１０．６．１８）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]