JP5290865B2 - 位置姿勢推定方法および装置 - Google Patents

Description

本発明は、観察対象物体の形状を表す３次元モデルデータと撮像装置が撮像した観察対象物体の撮像画像を利用して、撮像装置と観察対象物体との相対的な位置姿勢を計測する位置姿勢計測技術に関するものである。

現実空間を撮像するカメラなどの撮像装置を用いて、観察対象物体と上記観察対象物体を撮像する撮像装置との相対的位置姿勢を計測する技術が提案されている。このような位置姿勢の計測技術は、現実空間と仮想空間を融合表示する複合現実感システムや、ロボットの位置姿勢計測において、非常に有用な技術である。この位置姿勢計測技術は、観察する対象物体が事前にわかっている場合には、その物体の情報と実際の映像との比較・照合によって位置姿勢を推定する問題となる。

その方策として、物体の形状および表面情報(色・模様など)を表す３次元モデルをＣＧ描画することで、観察するカメラとの相対的な位置姿勢を推定する手法が非特許文献１に記載されている。その基本的なアプローチは、３次元モデルをＣＧ描画して得られる描画画像と実物体を撮影した実写画像の二つの画像が整合するようにカメラの位置姿勢を修正・最適化する方法である。

具体的には、まず、（１）あらかじめ入力されるカメラの位置及び姿勢（位置姿勢ともいう）の予測値を元に３次元モデルデータをＣＧ描画する。これにより、３次元モデルデータの表面情報（表面上の輝度値）が画像平面上に投影された画像が得られる。この画像を描画画像と呼称する。（２）結果として得られた描画画像からエッジを検出する。ここで、画像上で輝度が不連続に変化する領域をエッジと呼称する。（３）描画画像でエッジが検出された位置の近傍において、撮像画像等の実写画像からエッジ検出を行う。この処理により、描画画像中のエッジが実写画像中のどのエッジに対応するかを探索する。（４）前ステップの対応探索において、描画画像のエッジに対応する実写画像のエッジが複数検出された場合、エッジの類似度を利用して、対応するエッジを選別する。エッジの類似度は、両画像中でのエッジ周辺の輝度分布を正規化相互相関により比較することで求める。この処理により、対応候補として検出された実写画像のエッジの中で、もっともエッジのアピアランス(ここでは、エッジ周辺の輝度分布)が近いエッジを対応エッジとして選択する。（５）描画画像から検出されたエッジと、それに対応する実写画像のエッジとが、画像上での距離が最小になるように撮像装置の位置及び姿勢の補正値を求め、撮像装置の位置及び姿勢を更新する。この処理を前述の距離の和が収束するまで繰り返すことにより、最終的な撮像装置の位置及び姿勢を得る。

前述の３次元モデルに基づく位置姿勢推定方法では、描画画像中のエッジと実写画像中のエッジとの対応付けに基づいて位置姿勢を推定しているため、エッジの対応付けの正確さが位置姿勢推定の精度に大きな影響を与える。前述の手法では、両画像から抽出されるエッジ周辺の輝度分布を比較し、両画像間で最も類似するエッジを選択することでエッジの対応付けを行っている。しかし、例えば、実環境中に光源変化が発生するなどの原因により、位置姿勢推定に利用する３次元モデルデータの表面情報が、実写画像中に撮像される対象物体と大きく異なる場合がある。そのような場合には、その描画画像と実写画像から抽出した輝度分布を比較したとしても、正しくエッジの対応付けを行うことは困難となる課題がある。

このような課題に対し、非特許文献２では、実写画像中から、エッジ周辺の輝度分布を取得・更新して位置姿勢推定に利用する手法が開示されている。この手法では、３次元モデルデータの描画は行わず、画像平面に投影した３次元モデル中のエッジと実写画像中のエッジとを直接対応づけることで位置姿勢計算を行う。３次元モデル中のエッジに対応する実写画像中のエッジ周辺の輝度分布を、３次元モデルとの対応が既知である前フレームの実写画像から取得し、１次元ベクトルとして輝度値を保存する。この輝度分布を実写画像に基づいて毎フレーム更新することで、対象物体に光源変化が発生した場合でも、実写画像中に撮像され対象物体の画像情報に即した輝度分布を用いることで、精度の高い対応付けが可能となる。

G. Reitmayr and T. W. Drummond,"Going out: robust model-based tracking for outdoor augmented reality," Proc.The 5thIEEE/ACM International Symposium on Mixed and Augmented Reality(ISMAR06),pp.109-118 2006. H. Wuest, F.Vial, and D.Stricker,"Adaptive line tracking with multiple hypotheses for augmentedreality," Proc. The Fourth Int’l Symp. on Mixed and Augmented Reality(ISMAR05),pp.62-69, 2005. H. Wuest, F. Wientapper,D.Stricker, W.G. Kropatsch, "Adaptable model-based tracking using analysis-by-synthesistechniques," Computer analysis of images and patterns. 12th internationalconference, CAIP 2007, pp.20-27, 2007. K. Satoh, S. Uchiyama, H. Yamamoto,and H. Tamura, "Robust vision-based registration utilizing bird’s-eye view withuser’s view," Proc. The 2ndIEEE/ACMInternational Symposium on Mixed andAugmented Reality (ISMAR03), pp.46-55, 2003.

非特許文献２で開示されている従来手法では、３次元モデル中の幾何情報に対応する観察対象物体の表面情報が、実写画像中に撮像される対象物体の位置姿勢に基づいて、実写画像から毎フレーム逐次的に取得・更新される。そして、更新された表面情報が実写画像と３次元モデルとの整合に利用される。

しかし、上述の従来手法では、毎フレームの実写画像から表面情報を更新するため、実写画像の画像品質が悪い場合や対象物体の位置姿勢が不正確な場合は、対象物体の表面情報として不正確な画像情報が参照されることになる。すなわち、保持する３次元モデルの表面情報が不正確に更新されてしまい、表面情報を利用した位置姿勢推定が不正確になるという課題が発生する。

例えば、実写画像から対象物体の画像情報を参照するときに、図２に示すように実写画像中に撮像される対象物体の位置姿勢が正しく推定された位置姿勢情報に基づいて更新を行う場合には、正しい表面情報を取得・更新することが可能である。しかし、図３に示すように実写画像中に撮像される対象物体の位置姿勢が不正確に推定された位置姿勢情報に基づいて更新を行うと、不正確な表面情報が取得され、これによって３次元モデルの表面情報が不正確に更新されてしまう。また、例えば、図４に示すように、対象物体または撮像装置の素早い移動によって実写画像中にモーションブラーが発生した場合、ブラーにより画質が劣化した実写画像に基づいて更新を行うと、表面情報が不正確に更新されてしまう。また、図５に示すように、撮像装置と対象物体の間に手が入る等により、対象物体にオクルージョンが発生した場合、オクルージョン領域の画像情報を参照することによって表面情報が不正確に更新されてしまう。また、対象物体と撮像装置とがほとんど移動せず、かつ光源変化が発生しないなど、実写画像中に撮像される対象物体の画像情報に変化が少ない場面では、実写画像に基づいて表面情報を更新しても、新たな情報が得られることは少ない。すなわち、このような場合の更新処理は無駄な処理となることが多い。

以上のように、従来提案されている手法では、毎フレームの実写画像に対して無条件に更新が行される。そのため、実写画像の画像品質が劣化した場合や、位置姿勢の推定に失敗した場合に、不正確な表面情報を取得してしまい、位置姿勢推定が破綻するというという課題があった。また、実写画像にほとんど変化がない場合でも更新処理を行うため、無駄な処理が発生するという課題があった。

本発明は、上記の課題を鑑みてなされたものであり、３次元モデルデータの表面情報を実写画像を用いて更新するにおいて、不適格な実写画像を用いた更新を防止し、安定した位置姿勢の推定を可能にすることを目的とする。

上記目的を達成するための本発明の一態様による位置姿勢推定装置は以下の構成を備える。すなわち、
撮像装置によって観察対象物体を撮像して得られた実写画像を取得する取得手段と、
前記観察対象物体の幾何情報及び表面情報を表す３次元モデルデータを保持する保持手段と、
前記３次元モデルデータを前記撮像装置の位置姿勢情報に基づいて画像平面上に投影することにより描画画像を生成する描画手段と、
前記描画画像の表す幾何情報及び表面情報と、前記実写画像における観察対象物体の画像の幾何情報及び表面情報との対応に基づいて、前記撮像装置と前記観察対象物体の相対的な位置姿勢を算出する算出手段と、
前記算出手段で算出された位置姿勢または前記実写画像に基づいて前記３次元モデルデータの表面情報を更新すべきか否かを判断する判断手段と、
前記判断手段により更新すべきと判断された場合、算出された前記位置姿勢に基づいて前記実写画像における前記観察対象物体の画像情報を前記保持手段が保持する前記３次元モデルデータの前記表面情報に対応付けて、当該対応付けられた画像情報を用いて前記保持手段に保持された前記３次元モデルデータの表面情報を更新する更新手段とを備え、
前記取得手段による新たな実写画像の取得と、更新された前記表面情報を含む３次元モデルデータに基づく前記描画手段による描画画像の生成と、該生成された描画画像と前記新たな実写画像とを用いた前記算出手段による位置姿勢の算出と、該算出された位置姿勢または前記実写画像に基づく前記判断手段による判断と、該判断手段により更新すべきと判断された場合に、前記算出された位置姿勢に基づく前記更新手段による前記表面情報の更新とを繰り返す。

また、上記目的を達成するための本発明の他の態様による位置姿勢推定方法は、
取得手段が、撮像装置によって観察対象物体を撮像して得られた実写画像を取得する取得工程と、
描画手段が、保持手段に保持された前記観察対象物体の幾何情報及び表面情報を表す３次元モデルデータを前記撮像装置の位置姿勢情報に基づいて画像平面上に描画することにより描画画像を生成する描画工程と、
算出手段が、前記描画工程により生成された描画画像と、前記実写画像における観察対象物体の画像との対応を検出することにより、前記撮像装置と前記観察対象物体の相対的な位置姿勢を算出する算出工程と、
判断手段が、前記算出工程で算出された位置姿勢または前記実写画像に基づいて前記３次元モデルデータの表面情報を更新すべきか否かを判断する判断工程と、
更新手段が、前記判断工程で更新すべきと判断された場合、算出された前記位置姿勢に基づいて前記実写画像における前記観察対象物体の画像情報を前記保持手段が保持する前記３次元モデルデータの前記表面情報に対応付けて、当該対応付けられた画像情報を用いて前記保持手段に保持された前記３次元モデルデータの表面情報を更新する更新工程と、を有し、
前記取得工程による新たな実写画像の取得と、更新された前記表面情報を含む３次元モデルデータに基づく前記描画工程による描画画像の生成と、該生成された描画画像と前記新たな実写画像とを用いた前記算出工程による位置姿勢の算出と、該算出された位置姿勢または前記実写画像に基づく前記判断工程による判断と、該判断工程により更新すべきと判断された場合に、前記算出された位置姿勢に基づく前記更新工程による前記表面情報の更新とを繰り返す。

本発明によれば、３次元モデルデータの表面情報を実写画像を用いて更新するにおいて、不適格な実写画像を用いた更新が防止され、安定した位置姿勢の推定が可能となる。これにより、実写画像の画像品質が劣化する場合や、突発的に位置姿勢の推定に失敗する場合でも、正しく推定された位置姿勢、適切な実写画像を判別し、選択的にそれらを利用して表面情報の更新を行うことが可能となる。

第一の実施形態における位置姿勢推定装置１の構成を示す図。 正しく推定された位置姿勢に基づいた表面情報の更新を示す図。 不正確に推定された位置姿勢に基づいた表面情報の更新を示す図。 モーションブラーが発生した実写画像に基づいた表面情報の更新を示す。 オクルージョンが発生した実写画像に基づいた表面情報の更新を示す図。 第一の実施形態における３次元モデルデータを利用した位置姿勢の推定方法の処理手順を示すフローチャート。 第一の実施形態における、位置姿勢推定のためのモデル特徴抽出の詳細な処理手順を示すフローチャート。 第一の実施形態における、描画画像特徴と実写画像特徴との対応付けにおける詳細な処理手順を示すフローチャート。 第一の実施形態における、３次元モデルデータの表面情報の実写画像に基づく更新における詳細な処理手順を示すフローチャート。 第一の実施形態における位置姿勢推定装置１の構成を示す図。 第二の実施形態における表面情報を利用した位置姿勢の推定方法の処理手順を示すフローチャート。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。

［第一の実施形態］（３次元モデルの描画結果を利用した位置姿勢推定）
本実施形態では、本発明の画像処理装置及びその方法を、位置姿勢推定の正確さに基づいて表面情報の更新の有無を判断し、３次元モデルの描画結果および実写画像から抽出したエッジの対応に基づいて位置姿勢推定する場合について、説明する。

図１は、観察対象物体の形状を表す３次元モデルデータ１０を利用して位置姿勢推定を行う位置姿勢計測装置１の構成を示している。位置姿勢推定装置１において、３次元モデル保存部１１０は、３次元モデルデータ１０を保存する。画像取得部１２０は撮像装置１００からの撮像画像を実写画像として取得する。３次元モデル描画部１３０は、３次元モデル保存部１１０に保存されている３次元モデルデータ１０を画像平面上に投影／描画して描画画像を生成する。モデル特徴抽出部１４０は３次元モデル描画部１３０が描画した描画画像から、その輝度および／または色などに基づいて特徴（例えばエッジ特徴、点特徴等）を抽出する。画像特徴抽出部１５０は、画像取得部１２０が取得した実写画像における観察対象物体の画像からその輝度および／または色などに基づいて特徴（例えばエッジ特徴、点特徴等）を抽出する。特徴対応付け部１６０は、モデル特徴抽出部１４０が抽出した特徴と、画像特徴抽出部１５０が抽出した特徴とを対応付ける。位置姿勢算出部１７０は、特徴対応付け部１６０により対応付けられた特徴領域に基づいて撮像装置１００の位置姿勢を算出する。モデル更新部１８０は、位置姿勢算出部１７０で算出された位置姿勢に基づいて３次元モデルデータと実写画像とを対応付け、３次元モデルデータ１０が有する表面情報（テクスチャ等）を更新する。更新判断部１９０は、モデル更新部１８０による３次元モデルデータ１０の表面情報の更新を実行するか否かを判断する。撮像装置１００は、画像取得部１２０に接続されている。

次に、位置姿勢推定装置１を構成する各部について詳細に説明する。３次元モデル保存部１１０は、３次元モデルデータ１０を記憶する。３次元モデルデータ１０は、位置姿勢算出の基準となる、観察対象物体の３次元的な幾何情報(頂点座標や面情報)および表面情報（色や模様）を表すモデルである。３次元モデルデータ１０の形式は、対象物体の形状を表す幾何情報を保持し、かつ、対象物体の幾何情報に対応する表面情報が保持することが出来る限り、いずれの形式でもよい。例えば、頂点と面とで構成されたメッシュモデルにより幾何形状を表し、テクスチャ画像をＵＶマッピングでメッシュモデルに貼り付けることで表面情報を表してもよい。或いは、ＮＵＲＢＳ曲面により幾何形状を表し、テクスチャ画像を球面マッピングでＮＵＲＢＳ曲面に貼り付けることで表面情報を表してもよい。本実施形態では、頂点情報および各頂点を結んで構成される面の情報と、テクスチャ画像および頂点情報に対応するテクスチャ画像座標の情報とを含むＣＡＤモデルを３次元モデルデータ１０として利用する。

画像取得部１２０は、撮像装置１００によって撮像された撮像画像を実写画像として位置姿勢推定装置１に入力する。画像取得部１２０は、撮像装置の出力がＮＴＳＣなどのアナログ出力であればアナログビデオキャプチャボードによって実現される。また撮像装置の出力がＩＥＥＥ１３９４などのデジタル出力であれば、例えばＩＥＥＥ１３９４インタフェースボードによって実現される。また、予め記憶装置（不図示）に記憶してある静止画像や動画像のデジタルデータを読み出してもよい。したがって、以下では画像取得部１２０が取得する画像を実写画像とも記載する。

３次元モデル描画部１３０では、３次元モデル保存部１１０において、保存された３次元モデルデータ１０を描画する。３次元モデル描画部１３０において描画に利用するグラフィックライブラリは、例えば、OpenGLやDirectXなどの広く利用されているグラフィックライブラリでも、独自に開発したグラフィックライブラリでもよい。すなわち、３次元モデル保存部１１０に保存されたモデル形式を画像平面上に投影することができる限り、いずれの方式を用いても良い。本実施形態ではグラフィックライブラリとしてOpenGLを利用する。

モデル特徴抽出部１４０は、３次元モデル描画部１３０が生成した描画画像から、撮影画像（実写画像）に３次元モデルを当てはめるための特徴を抽出する。本実施形態では、モデル特徴抽出部１４０においては、３次元モデル描画部１３０が３次元モデルと撮像装置１００の位置姿勢に基づいて描画した描画画像から、エッジ情報を抽出する。モデル（描画画像）からの特徴抽出手法については、後述する。

画像特徴抽出部１５０は、画像取得部１２０によって取得された実写画像上において、撮像装置１００の位置及び姿勢を算出するのに用いられる画像特徴を検出する。本実施形態では、画像特徴抽出部１５０は撮像画像上のエッジの検出を行う。エッジの検出方法については後述する。

特徴対応付け部１６０では、モデル特徴抽出部１４０が抽出した特徴と画像特徴抽出部１５０が抽出した特徴とを、描画画像と実写画像中から抽出される輝度分布を利用して対応付ける。特徴の対応付け方法については後述する。

位置姿勢算出部１７０では、特徴対応付け部１６０によって対応付けられた特徴対応付け情報に基づき、３次元モデルデータ１０を基準とした座標系における撮像装置１００の位置及び姿勢を算出する。

モデル更新部１８０では、位置姿勢算出部１７０により算出された位置姿勢情報と画像取得部１２０により取得された実写画像とに基づき、３次元モデルデータ１０の表面情報を取得・更新する。３次元モデルデータの更新方法について後述する。

更新判断部１９０では、位置姿勢算出部１７０により算出された位置姿勢情報の推定信頼度を算出し、位置姿勢推定の正確さに基づいて、モデル更新部１８０による３次元モデルデータ１０の表面情報を更新する／しないを判断する。位置姿勢の推定信頼度の算出方法としては、例えば、
・位置姿勢算出部１７０の位置姿勢推定結果のアウトライア率（誤対応とみなしたエッジの割合）を用いる、
・位置姿勢推定結果における誤差の総和や誤差の分散を求める、
・推定した位置姿勢に基づいてもう一度３次元モデルを描画し、その描画画像と実写画像との差分の二乗和や、描画画像と実写画像の正規化相互相関、または相互情報量などで表される類似度を利用する、
等が挙げられる。すなわち、位置姿勢の正確さを推定できる尺度であれば、いずれの手法を用いてもよく、位置姿勢の推定信頼度の算出方法に特に制限はない。本実施形態では、位置姿勢の推定信頼度の算出に、位置姿勢推定結果のアウトライア率)を用いる。アウトライア率を用いた位置姿勢の推定信頼度の算出方法については後述する。

モデル更新部１８０では、更新判断部１９０の判断に従い、位置姿勢算出部１７０により算出された位置姿勢情報と、画像取得部１２０により入力された実写画像とに基づき、３次元モデルデータ１０の表面情報を取得・更新する。３次元モデルデータの更新方法については後述する。

次に、本実施形態における位置姿勢推定方法の処理手順について説明する。図６は、本実施形態における位置姿勢推定方法の処理手順を示すフローチャートである。なお、以下では描画画像と実写画像から抽出する特徴としてエッジを用いるが、本発明はこれに限られるものでない。例えば、Harris検出器などによって検出される点特徴を利用しても良い。

まずステップＳ１０１０で初期化を行う。ここでは、基準座標系における撮像装置１００と観察対象物体との相対的な概略位置姿勢の設定、および、３次元モデルデータの表面情報の初期化を行う。

本実施形態における位置姿勢計測方法は、撮像装置１００の概略の位置姿勢を、撮影画像上に撮像される観察対象物体のエッジ情報を利用して逐次更新していく方法である。そのため、位置姿勢計測を開始する前に予め撮像装置１００の概略の位置及び姿勢を初期位置及び初期姿勢として与える必要がある。そこで、例えば予め決まった位置及び姿勢を設定しておき、撮像装置１００をその位置及び姿勢になるように移動することで初期化を行う。または、画像内で検出するだけで認識可能な人工的な指標を配置し、該指標の各頂点の画像座標と基準座標系における３次元位置との対応から撮像装置の位置姿勢を求めて概略の位置姿勢としてもよい。また、識別性の高い自然特徴点を予め検出してその３次元位置を求めておき、初期化時に画像上で該特徴点を検出し、その画像座標と３次元位置との対応から撮像装置の位置姿勢を求めてもよい。また、非特許文献３のように、３次元モデルの幾何情報から抽出したエッジと画像中のエッジとの比較から撮像装置の位置姿勢を求めてもよい。さらに、磁気式や光学式、超音波式などの６自由度位置姿勢センサによって撮像装置の位置姿勢を計測し、それを概略の位置姿勢としてもよい。人工的な指標や自然特徴点などの画像情報と、前述の６自由度位置姿勢センサや３自由度の姿勢センサ、３自由度の位置センサとを併用して計測される撮像装置１００の位置姿勢を用いて初期化してもよい。

また、本実施形態における位置姿勢計測方法は、３次元モデルデータの形状および表面情報に基づいてＣＧ描画した描画結果を利用して位置姿勢を推定する。そのため、３次元モデルデータ１０に表面情報が設定されていることが前提である。しかし、表面情報が設定されていない３次元モデルデータ１０を利用する場合や、３次元モデルデータ１０の表面情報に不適切な情報が設定されている場合もある。そこで、そのような場合は、上述の位置姿勢の初期化処理により位置姿勢が求まった実写画像を利用して、３次元モデルの表面情報を初期化する。具体的には、位置姿勢の初期化処理により求めた位置姿勢を利用し、実写画像に撮像される観察物体の画像情報と、３次元モデルの表面情報との対応関係を算出する。そして、求めた対応関係に基づいて、実写画像の画像情報を３次元モデルの表面情報に反映させることで、３次元モデルの表面情報の初期化を行う。即ち、動的に３次元モデルの表面情報を取得するため、３次元モデルの表面情報にあらかじめ誤った情報が格納されていた場合にも、実環境中の対象物体に即した表面情報を反映させることが可能となる。また、もともと３次元モデルに表面情報が含まれていない場合でも、実写画像から対象物体の画像情報を取得することで、３次元モデルの表面情報に基づく位置姿勢推定を行うことが可能となる。

ステップＳ１０２０では、画像取得部１２０が、撮像装置１００が撮像した画像を位置姿勢推定装置１に取り込む。

次にステップＳ１０３０において、３次元モデル描画部１３０は、３次元モデルデータ１０をＣＧ描画し、実写画像と比較するための描画画像を得る。最初は、ステップＳ１０１０で求めた観察対象物体の概略位置姿勢に基づいて、３次元モデル保存部１１０に保存された３次元モデルデータ１０のＣＧ描画を行う。本実施形態で、描画に利用する射影行列の内部パラメータは、撮像装置１００の内部パラメータをあらかじめ計測しておき、実際に用いるカメラと内部パラメータを一致させておく。ＣＧ描画とは、ステップＳ１０１０で設定した視点の位置姿勢に基づいて、３次元モデル保存部１１０に保存された３次元モデルデータ１０を画像平面上に投影することである。ＣＧ描画を行うためには、位置姿勢の設定とともに、射影行列の内部パラメータ（焦点距離や主点位置など）を設定する必要がある。本実施形態では、撮像装置１００（カメラ）の内部パラメータをあらかじめ計測しておき、実際に用いるカメラと内部パラメータを一致させておく。また、視点からモデルまでの距離の最大値と最小値を設定しておき、その範囲外のモデルの描画は行わないことで、描画処理の計算コストを低減する。この処理はクリッピングと呼ばれ、一般的に行われている処理である。３次元モデルデータ１０のＣＧ描画により、カラーバッファおよびデプスバッファが算出される。ここで、カラーバッファには、画像平面上に投影された３次元モデルデータ１０の表面情報（テクスチャ画像）に応じた輝度値が格納される。また、デプスバッファには、画像平面から３次元モデルデータまでの奥行き値が格納される。以後、カラーバッファを３次元モデルデータ１０の描画画像と呼称する。３次元モデルデータの描画が終わったら、ステップＳ１０４０に進む。

次にステップＳ１０４０において、モデル特徴抽出部１４０は、ステップＳ１０３０で生成した描画画像から、実写画像と対応付けるためのエッジ特徴を抽出する。図７は、本実施形態における描画画像のエッジ特徴の検出方法について詳細な処理手順を示すフローチャートである。

まず、ステップＳ１１１０では、ステップＳ１０３０のＣＧ描画により生成された描画画像に対して、エッジ検出を行う。描画画像に対してエッジ検出することで、輝度が不連続に変化する領域を求めることができる。エッジを検出する手法としては、ここではCannyアルゴリズムを利用するが、画像の画素値が不連続に変化する領域を検出できれば、他の手法でも良く、例えば、sobelフィルタなどのエッジ検出フィルタを利用しても良い。カラーバッファに対してCannyアルゴリズムによるエッジ検出を行うことで、エッジ領域と非エッジ領域に分けられた２値画像が得られる。

次にステップＳ１１２０において、ステップＳ１１１０で生成された2値化画像について、隣接エッジのラベリングを行い、エッジの連結成分を抽出する。ラベリングは、例えば、ある画素の周囲8画素の中にエッジが存在すれば、同一ラベルを割り当てることにより行う。

次にステップＳ１１３０において、ステップＳ１１２０で連結成分を抽出したエッジに対して、エッジ素の抽出を行う。ここで、エッジ素とは、３次元座標および方向により表現される、３次元的なエッジを構成する要素をいう。同一ラベルにラベリングされたエッジを画像上で等間隔に分割するように分割点を算出して、この分割点の周辺の極短い連結成分を求めエッジ素を抽出する。本実施形態では、分割点から３pixel離れた連結成分を端点（始点と終点）に設定して、分割点を中心としたエッジ素を抽出する。デプスバッファから抽出したエッジ素は、総数をＮとして、各エッジ素をEFi(i=1,2…N)で表す。エッジ素の数Nが多いほど、処理時間が長くなる。そのため、エッジ素の総数が一定になるように、画像上でエッジ素間の間隔を逐次変更しても良い。

次にステップＳ１１４０において、ステップＳ１１３０で算出されたエッジ素に対して、基準座標系における３次元座標を求める。これには、ステップＳ１０３０で生成したデプスバッファを利用する。まず、デプスバッファに格納された奥行き値をカメラ座標系における値に変換する。デプスバッファに格納された値は、ステップＳ１０３０のクリッピング処理において設定したクリッピング範囲に応じて0から1の値に正規化されて格納されている。そのため、デプスバッファの奥行き値から、直接基準座標系における３次元座標を求めることはできない。そこで、クリッピング範囲の最小値と最大値を利用して、デプスバッファの値をカメラ座標系における視点からモデルまでの距離の値に変換する。次に、射影行列の内部パラメータを利用して、デプスバッファの画像平面上の２次元座標と、カメラ座標系における奥行き値から、カメラ座標系における３次元座標を求める。そして、カメラ座標系における３次元座標に対して、ステップＳ１０３０における３次元モデルデータの描画で用いた位置姿勢変換の逆変換をほどこすことで、基準座標系における３次元座標を求める。以上の処理を各エッジ素EFiに対して行うことで、各エッジ素に対して、基準座標系における３次元座標を求める。また、各エッジ素に対し、ステップＳ１１２０で求めたエッジ上で前後に隣接する画素の３次元座標を算出し、それらの差分をとることで、基準座標系における３次元的な方向を求める。

エッジ素EFiの３次元座標と方向の算出が終わったら、ステップＳ１０５０に進む。

ステップＳ１０５０において、画像特徴抽出部１５０は、ステップＳ１０４０で求めた描画画像中のエッジ素EFi（i=1,2,…,N）に対応する、撮像装置１００によって撮像された現フレームの実写画像中のエッジを検出する。エッジの検出は、エッジ素EFiの探索ライン(エッジ素の法線方向の線分)上において、撮影画像上の濃度勾配から極値を算出することにより行う。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。探索ライン上で検出されたエッジが一つのみである場合は、そのエッジを対応点とし、その画像座標とエッジ素EFiの３次元座標を保持する。また、探索ライン上で検出されたエッジが複数存在する場合には、複数の点を対応候補として保持する。以上の処理を全てのエッジ素EFiに対して繰り返し、処理が終了すればステップＳ１０５０の処理を終了し、ステップＳ１０６０に進む。

ステップＳ１０６０では、特徴対応付け部１６０は、対応点が複数存在するエッジ素に関して、最も確からしい対応点を決定する。ステップＳ１０４０で求めた描画画像中のエッジ素EFi（i=1,2,…,N）のうちのステップＳ１０５０で求めた対応点が複数存在するエッジ素EFj（j=1,2,…,M）に対し、エッジ周辺の輝度分布の比較によりもっとも確からしい対応を求める。ここで、Ｍは、対応点が複数存在するエッジ素の数である。図８は、本実施形態における対応エッジ選択手法について詳細な処理手順を示すフローチャートである。

まず、ステップＳ１２１０において、特徴対応付け部１６０は、ステップＳ１０３０で求めた３次元モデルデータ１０の描画画像から、エッジ素EFjのエッジ周辺の輝度分布を取得する。エッジ周辺の輝度分布としては、エッジの法線方向数ピクセルの輝度値を取得しても、エッジ位置から数ピクセル離れた円上の輝度値を取得しても、エッジ位置から数ピクセル離れたエッジ方向と平行方向の輝度値を取得してもよい。また、輝度分布を、輝度値の１次元ベクトル、輝度値のヒストグラム、または、勾配ヒストグラムとして表現してもよい。輝度分布として利用する情報は、描画画像と実写画像の輝度分布の類似度を計算できる限り、特に制限はなく、いずれの方式でもよい。本実施形態では、エッジ法線方向２１ピクセル上の輝度値の１次元ベクトルを、エッジ周辺の輝度分布として取得する。

次に、ステップＳ１２２０において、特徴対応付け部１６０は、エッジ素EFjの対応候補エッジの輝度分布を実写画像から取得する。ステップＳ１０５０で求めたエッジ素EFjの対応候補エッジに対して、ステップＳ１２１０と同様の処理により、実写画像中のエッジ周辺の輝度分布を取得する。

次に、ステップＳ１２３０では、ステップＳ１２１０およびステップＳ１２２０で求めた両画像の輝度分布を比較し、各エッジ素EFjの対応候補エッジとの類似度を算出する。エッジの類似度には、輝度分布間の差の二乗和（SSD：Sum of square distance）を利用しても、正規化相互相関（NCC：Normalizecross correction）を利用してもよい。輝度分布間の距離が計算できる手法であれば、いずれの手法でもよい。本実施例では、輝度分布間のSSDを要素数で正規化した値を評価値に利用する。

次に、ステップＳ１２４０では、ステップＳ１２３０で求めた評価値に基づき、各エッジ素EFjに対応するエッジを対応候補エッジの中から選択する。対応候補エッジの中で、ステップＳ１２３０で求めた評価値がもっとも高い（＝画像上でエッジの見た目が最も近い）エッジを対応エッジとして選択する。以上の処理を、対応点が複数存在する全てのエッジ素EFjに対して繰り返し、全てのエッジ素EFiについて対応点が求まればステップＳ１０６０の処理を終了し、ステップＳ１０７０に進む。

ステップＳ１０７０において、位置姿勢算出部１７０は、非線形最適化計算を用いて、撮像装置１００と観察対象物体との概略の相対的案位置姿勢を反復演算により補正することにより撮像装置１００の位置姿勢を算出する。ここで、ステップＳ１０４０において検出された描画画像のエッジ素EFiのうち、ステップＳ１０６０において対応エッジが求まったエッジ素の総数をLcとする。また、画像の水平方向、垂直方向をそれぞれｘ軸、ｙ軸とする。また、あるエッジ素の中心点の投影された画像座標を（ｕ₀、ｖ₀）、エッジ素の直線の画像上での傾きをｘ軸に対する傾きθと表す。傾きθは、エッジ素端点(始点と終点)の撮影画像上での２次元座標を結んだ直線の傾きとして算出する。エッジ素の直線の画像上での法線ベクトルは（sinθ、−cosθ）となる。また、該エッジ素の対応点の画像座標を（ｕ’、ｖ’）とする。

ここで、点（ｕ、ｖ）を通り、傾きがθである直線の方程式は、以下の数１のように表せる。

エッジ素の撮影画像上での画像座標は撮像装置１００の位置及び姿勢により変化する。また、撮像装置１００の位置及び姿勢の自由度は６自由度である。ここで撮像装置の位置及び姿勢を表すパラメータをｓで表す。ｓは６次元ベクトルであり、撮像装置の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す３次元ベクトルなどによって表現される。エッジ素中心点の画像座標（ｕ、ｖ）は（ｕ_０、ｖ_０）の近傍で１次のテイラー展開によって数２のように近似できる。

ｕ、ｖの偏微分∂ｕ／∂ｓ_i、∂ｖ／∂ｓ_iの導出方法は例えば非特許文献４に開示されるように広く知られているのでここではその詳細は述べない。数２を数１に代入することにより、数３が得られる。

ここで、数３に示す直線が該エッジ素の対応点の画像座標（ｕ’、ｖ’）を通過するように、撮像装置の位置及び姿勢ｓの補正値Δｓを算出する。ｒ₀＝ｕ₀sinθ−ｖ₀cosθ（定数）、ｄ＝ｕ'sinθ−ｖ'cosθ（定数）とすると、以下の数４が得られる。

数４はLc個のエッジ素について成り立つため、数５のようなΔｓに対する線形連立方程式が成り立つ。

ここで数５を数６のように簡潔に表す。

数６をもとにＧａｕｓｓ−Ｎｅｗｔｏｎ法などによって、行列Ｊの一般化逆行列（Ｊ^T・Ｊ）^-1を用いて補正値Δｓが求められる。しかしながら、エッジの検出には誤検出が多いので、次に述べるようなロバスト推定手法を用いる。一般に、誤検出されたエッジに対応するエッジ素では誤差d−rが大きくなる。そのため数５、数６の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。そこで、誤差d−rが大きいエッジ素のデータには小さな重みを与え、誤差d−rが小さいエッジ素のデータには大きな重みを与える。重みは例えば数７Ａに示すようなＴｕｋｅｙの関数により与える。

数７においてｃは定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えば次式の数７Ｂ、

で示されるようなＨｕｂｅｒの関数など、誤差d−rが大きいエッジ素には小さな重みを与え、誤差d−rが小さいエッジ素には大きな重みを与える関数であればなんでもよい。

エッジ素EFiに対応する重みをｗ_iとする。ここで数８のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外はすべて０のLc×Lc正方行列であり、対角成分には重みｗ_iが入る。この重み行列Ｗを用いて、数６を数９のように変形する。

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

これにより得られた補正値Δｓを用いて、撮像装置１００の位置及び姿勢を更新する。次に、撮像装置の位置及び姿勢の反復演算が収束しているかどうかを判定する。補正値Δｓが十分に小さかったり、誤差r−dの総和が十分小さい、誤差r−dの総和が変化しないといった場合には、撮像装置の位置及び姿勢の計算が収束したと判定する。収束していないと判定された場合には、撮像装置１００の更新された位置及び姿勢を用いて再度線分の傾きθ、r₀、d及びｕ、ｖの偏微分を計算し直し、数１０より再度補正値Δｓを求め直す。なお、ここでは非線形最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いた。しかしながら、Ｎｅｗｔｏｎ−Ｒａｐｈｓｏｎ法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。以上、ステップＳ１０７０における撮像装置の位置姿勢算出方法について説明した。

次にステップＳ１０８０において、更新判断部１９０は、ステップＳ１０７０において算出した位置姿勢の推定信頼度に基づき、モデル更新部１８０による表面情報の更新を実行するか、しないかを判断する。位置姿勢の推定信頼度としては、ステップＳ１０７０において算出した位置姿勢推定結果におけるアウトライア率を利用する。ここで、アウトライアとは、位置姿勢推定処理に用いたエッジ素の中で、誤対応とみなしたエッジ素のことを指し、アウトライア率は位置姿勢推定に用いたエッジ素中の誤対応の割合を表す。アウトライア率は、基本的に、位置姿勢推定が正しい場合は低く、位置姿勢推定が不正確な場合は高くなる傾向にある。そこで、アウトライア率が一定以下であれば位置姿勢推定結果が正確であるとみなし、位置姿勢が正しく推定されているときのみ、表面情報を更新する。位置姿勢推定の正確さを判断するアウトライア率の閾値は、３次元モデルの形状・表面情報や実写画像の撮影条件などの要因により変動するため、対象物体や条件に応じて設定する必要がある。本実施形態では、あらかじめ、位置姿勢推定が正確なシーンにおけるアウトライア率と、位置姿勢推定に失敗しているシーンのアウトライア率を算出し、それらに基づき、アウトライア率の閾値を設定しておく。

アウトライア率の算出では、まず、位置姿勢計算の過程でＴｕｋｅｙの関数より算出した重み行列Ｗ（数８）を参照し、重みが０であるエッジ素の数を算出する。重み行列Ｗには各エッジに対応する重み（位置姿勢計算における寄与度）が格納されており、重みが０であるエッジ素は信頼できないエッジ素であることを表す。重みが０であるエッジ素の数をアウトライアの数Loとし、ステップＳ１０７０において位置姿勢算出に用いたエッジ素の数Lcに対するアウトライアの数Loの比をとることで、アウトライア率を算出する。アウトライア率が閾値以下の場合は、位置姿勢推定が正確であると判断し、表面情報の更新処理を行うため、ステップＳ１０９０に進み、アウトライア率が閾値以下の場合は、表面情報の更新処理を行わず、ステップＳ１１００に進む。

次にステップＳ１０９０のアピアランス更新の処理を説明する。モデル更新部１８０は、ステップＳ１０２０で入力した実写画像から取得した観察対象物体の画像情報を、ステップＳ１０７０で算出した位置姿勢情報に基づいて３次元モデルデータ１０の表面情報（テクスチャ画像）に反映させることで表面情報を更新する。図９は、本実施形態における物体アピアランスの更新手法について、詳細な処理手順を示すフローチャートである。

まず、ステップＳ１３１０において、モデル更新部１８０は、ステップＳ１０７０で求めた観察対象物体の位置姿勢に基づいて、３次元モデルデータ１０の頂点情報を画像平面上に投影する。この処理により３次元モデルデータ１０の頂点座標に対応する実写画像上での２次元座標が求まる。

次に、ステップＳ１３２０において、モデル更新部１８０は、３次元モデルデータ１０のテクスチャ画像と実写画像との対応関係を算出する。本実施形態においては、３次元モデルデータ１０の頂点座標に対応するテクスチャ画像上での２次元座標が既に与えられている。そこで、この対応情報と、ステップＳ１３１０で求めた３次元モデルデータ１０と実写画像間の対応情報から、実写画像とテクスチャ画像間の対応を算出する。

次に、ステップＳ１３３０において、モデル更新部１８０は、ステップＳ１３２０で求めた、実写画像とテクスチャ画像間の対応に基づき、実写画像の輝度情報をテクスチャ画像にマッピングし、３次元モデルデータ１０の表面情報を更新する。

以上の処理により、実写画像上に撮像される対象物体の画像情報に基づいて、３次元モデルデータの表面情報を更新する。全ての更新処理が終わったら、ステップＳ１１００に進む。

ステップＳ１１００では、位置姿勢算出を終了する入力がなされたかどうかを判定し、入力された場合には終了し、入力されなかった場合にはステップＳ１０２０に戻り、新たな画像を取得して再度位置姿勢算出を行う。

以上述べたように、本実施形態では、推定した位置姿勢の正確さに基づいて、実写画像による更新を行うか否かを判断することで、不正確な位置姿勢に基づいて更新した場合に、不正確な表面情報が取得・更新されることを防ぐことが可能となる。これにより、シーンによっては、位置姿勢が不正確に推定されるような場合でも、その位置姿勢推定の正確さを判断することで、正確な位置姿勢が推定された場合にのみ選択的に更新することが可能となる。

［第二の実施形態］（線分モデルとエッジ周辺の輝度分布を利用した位置姿勢推定）
第一の実施形態では、実写画像中に撮像される対象物体の画像情報を、３次元モデルの表面情報として保持・更新し、位置姿勢推定に利用した。第二の実施形態では、３次元モデルとして、主要な稜線を表す線分情報から構成されたモデル（エッジモデルデータ）を利用する。そして、実写画像中に撮像される対象物体の画像情報を、３次元モデル中の線分に対応する１次元情報の集合として、保持・更新し、位置姿勢推定に利用する手法に、本発明を適用した例について説明する。

図１０は、本実施形態における位置姿勢推定装置２の構成を示している。同図に示したように、位置姿勢推定装置２は、３次元モデル保存部２１０と、表面情報保存部２２０と、画像取得部２３０とを備える。位置姿勢推定装置２は、更に、３次元モデル投影部２４０と、画像特徴抽出部２５０と、特徴対応付け部２６０と、位置姿勢算出部２７０と、更新判断部２８０と、モデル更新部２９０を備える。３次元エッジモデルデータ２０は、３次元モデル保存部２１０に保存され、画像特徴抽出部２５０およびモデル更新部２９０に接続されている。本実施形態では、３次元モデルデータとして、主要な稜線を表す線分から構成されたモデルを表す３次元エッジモデルデータ２０が用いられる。撮像装置１００は、画像取得部２３０に接続されている。位置姿勢推定装置２は、３次元モデル保存部２１０に保存されている観察対象物体の形状を表す３次元エッジモデルデータ２０をもとに、実写画像中に撮像される観察対象物体の位置および姿勢を計測する。なお、本実施形態において、位置姿勢推定装置２が適用できる条件としては、３次元モデル保存部２１０に保存された３次元エッジモデルデータ２０が、現実に撮像される観察対象物体の形状に即していることを前提としている。

次に、位置姿勢推定装置２を構成する各部について説明する。

３次元モデル保存部２１０は、３次元エッジモデルデータ２０を記憶する。３次元エッジモデルデータ２０は、対象物体の主要な稜線を表す線分、および線分から構成される面の情報により、対象物体の形状を表すモデルデータである。

３次元モデル投影部２４０は、３次元エッジモデルデータ２０を画像平面上に投影し、投影画像上において一定の間隔になるよう、３次元モデルデータ中のエッジ情報からエッジ素を算出する。モデルの投影手法については、後述する。

画像特徴抽出部２５０は、画像取得部２３０によって入力された画像上において、３次元モデル投影部２４０で算出したエッジ素に対応するエッジ特徴を、実写画像中から検出する。エッジの検出方法については後述する。

特徴対応付け部２６０は、３次元モデル投影部２４０で算出したエッジ素と画像特徴抽出部２５０で検出した特徴を、表面情報保存部２２０が保存する輝度分布情報と実写画像中から抽出される輝度分布情報とを利用して対応付ける。本実施形態では、表面情報保存部２２０が、実写画像中に撮像される対象物体の輝度値の１次元ベクトル、および輝度値の分散の１次元ベクトルを、表面情報として保持する。表面情報を利用した特徴の対応付け方法については後述する。

モデル更新部２９０は、更新判断部２８０の判断に従い、位置姿勢算出部２７０により算出された位置姿勢情報と、画像取得部２３０により取得された実写画像とに基づき、表面情報保存部２２０が保存する対象物体の表面情報を取得・更新する。３次元モデルデータの更新方法については後述する。

画像取得部２３０、位置姿勢算出部２７０、更新判断部２８０、モデル更新部２９０は、第一の実施形態における画像取得部１２０、位置姿勢算出部１７０、更新判断部１９０、モデル更新部１８０と同様であるので説明を省略する。

次に、本実施形態における位置姿勢推定方法の処理手順について説明する。図１１は、本実施形態における位置姿勢推定方法の処理手順を示すフローチャートである。

ステップＳ２０１０では初期化を行う。ステップＳ２０１０の処理内容は基本的に第一の実施形態におけるステップＳ１０１０とほとんど同様であり、重複部分は説明を省略する。ステップ１０１０と異なる点は、対象物体の表面情報の初期化として、３次元エッジモデルデータ２０中のエッジに対応する画像情報を、表面情報保存部２２０に輝度値の１次元ベクトルとして保存する点である。

ステップＳ２０２０の処理は第一の実施形態におけるステップＳ１０２０の処理と同様であり、画像取得部２３０が、撮像装置１００が撮像した画像を位置姿勢推定装置２に取り込む。

次に、３次元モデル投影部２４０は、ステップＳ２０１０で求めた観察対象物体の概略位置姿勢に基づいて、３次元モデル保存部２１０に保存された３次元エッジモデルデータを画像平面上に投影する。本実施形態では、第一の実施形態と同様に、モデルの投影に利用する射影行列の内部パラメータは、撮像装置１００の内部パラメータをあらかじめ計測しておき、実際に用いるカメラと内部パラメータを一致させておく。まず、３次元エッジモデルデータ中の３次元エッジ情報を、内部パラメータとステップＳ２０１０で設定した視点の位置姿勢に基づいて、画像平面上に投影する。次に、３次元モデル投影部２４０は、投影したエッジを、画像平面上で一定間隔になるように分割し、エッジ素を抽出する。本実施形態では、分割点同士が２０pixel離れる様にエッジの分割数を求め、エッジ素を抽出する。抽出したエッジ素は、総数をＮとして、各エッジ素をEFi(i=1,2…N)で表す。エッジ素の数Nが多いほど、処理時間が長くなる。そのため、エッジ素の総数が一定になるように、画像上でエッジ素間の間隔を逐次変更しても良い。投影したエッジから抽出したエッジ素に対して、各エッジ素に対応する３次元エッジモデル上のエッジを参照することで、エッジ素の基準座標系における３次元座標および３次元方向を求める。全てのエッジ素EFiの３次元座標と方向の算出が終わったら、ステップＳ２０４０に進む。

ステップＳ２０４０において、画像特徴抽出部２５０は、ステップＳ２０３０で求めた３次元エッジモデルのエッジ素EFi（i=1,2,…,N）に対応する、撮像装置１００によって撮像された現フレームの実写画像中のエッジを検出する。エッジの検出は、エッジ素EFiの探索ライン(エッジ素の法線方向の線分)上において、撮影画像上の濃度勾配から極値を算出することにより行う。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。探索ライン上で検出されたエッジが一つのみである場合は、そのエッジを対応点とし、その画像座標とエッジ素EFiの３次元座標を保持する。また、探索ライン上で検出されたエッジが複数存在する場合には、複数の点を対応候補として保持する。以上の処理を全てのエッジ素EFiに対して繰り返し、処理が終了すればステップＳ２０４０の処理を終了し、ステップＳ２０５０に進む。

ステップＳ２０５０において、特徴対応付け部２６０は、対応点が複数存在するエッジ素に関して、最も確からしい対応点を決定する。ステップＳ２０３０で求めた描画画像中のエッジ素EFi（i=1,2,…,N）のうちのステップＳ２０４０で求めた対応点が複数存在するエッジ素EFj（j=1,2,…,M）に対し、エッジ周辺の輝度分布の比較によりもっとも確からしい対応を求める。これは、エッジ周辺の輝度分布を比較することにより行われる。ここで、Ｍは、対応点が複数存在するエッジ素の数である。

まず、特徴対応付け部２６０は、各エッジ素EFjの対応候補エッジの輝度分布を実写画像から取得する。本実施形態では、エッジの輝度分布として、エッジ法線方向２１ピクセル上の輝度値の１次元ベクトルを取得する。

次に、各エッジ素EFjに対応する、表面情報保存部２２０に保存している輝度分布と、対応候補エッジの輝度分布を比較し、各エッジ素EFjの対応候補エッジとの類似度を算出する。類似度の算出は、以下の式より計算する。

ここで、ｍは輝度分布のサイズ、ｉは輝度分布のインデックスで、ｘは対応候補エッジEFjの輝度分布を表す輝度値、μは対応候補エッジEFjの輝度分布が保持する輝度値の平均、σは輝度分布が保持する輝度値の分散、ωは対応候補EFjの重みを表す。この式に値を代入することで、対応候補エッジの類似度が計算される。

特徴対応付け部２６０は、対応候補エッジの中で、求めた類似度がもっとも高い（すなわち、画像上でエッジの見た目が最も近い）エッジを対応エッジとして選択する。以上の処理を、対応点が複数存在する全てのエッジ素EFjに対して繰り返し、全てのエッジ素EFiについて対応点が求まればステップＳ２０５０の処理を終了し、ステップＳ２０６０に進む。

ステップＳ２０６０において、位置姿勢算出部２７０は、ステップＳ２０３０において抽出した３次元エッジモデルデータ２０のエッジ素EFiのうち、ステップＳ２０５０において対応エッジが求まったエッジ素に基づいて、撮像装置の位置姿勢を算出する。本実施形態では、非線形最適化計算を用いて、撮像装置と観察対象物体との概略の相対的な位置姿勢を反復演算により補正することにより、撮像装置の位置姿勢が算出される。ステップＳ２０６０の処理内容は基本的に第一の実施形態におけるステップＳ１０７０と同一であるので重複部分は説明を省略する。

次にステップＳ２０７０において、更新判断部２８０は、ステップＳ２０６０において算出した位置姿勢の推定信頼度に基づき、表面情報を更新するか、しないかを判断する。ステップＳ２０７０の処理内容は基本的に第一の実施形態におけるステップＳ１０８０と同一であるので重複部分は説明を省略する。

次にステップＳ２０８０において、モデル更新部２９０は、３次元エッジモデルデータ２０のエッジ素に対応する対象物体の表面情報を、表面情報保存部２２０が保存する表面情報に反映・更新する。この更新は、ステップＳ２０２０において入力された実写画像から取得した観察対象物体の画像情報と、ステップＳ２０６０において算出した位置姿勢情報とに基づいて行なわれる。図９は、本実施形態における物体アピアランスの更新手法について、詳細な処理手順を示すフローチャートである。

まず、３次元モデルのエッジ素EFiに対応する、表面情報保存部２２０が保存する輝度分布の輝度値μと、それに対応するステップＳ２０５０で求めた輝度分布の輝度値ｘとの誤差を算出する。

算出した誤差Ｄとエッジ素EFiに対応する輝度分布の分散σと比較し、誤差Ｄが分散σの３倍以下の場合は、以下の式より、エッジ素EFiに対応する輝度分布を更新する。

ここで、αとβは、実写画像から検出した輝度分布の情報をどの程度反映させるかを決定する学習係数である。学習係数には、事前に固定値を設定しておいても良いし、ステップＳ２０７０で算出した位置姿勢の推定信頼度を利用しても良い。本実施形態では、事前に固定値を設定していた学習係数を利用する。

また、式（数１１）より算出した誤差Ｄが分散σの３倍以上である場合、誤対応であると見なし、以下の式より、エッジ素EFiに対応する輝度分布を更新する。

これは、誤対応したエッジの輝度分布の重みを下げる処理に相当する。

以上の処理により、実写画像上に撮像される対象物体の画像情報に基づいて、表面情報保存部２２０が保存する、エッジ素EFiに対応する表面情報を更新する。全てのエッジ素EFiに対応する表面情報の更新処理が終わったら、ステップＳ２０９０に進む
ステップＳ２０９０では、位置姿勢算出を終了する入力がなされたかどうかを判定し、入力された場合には終了し、入力されなかった場合にはステップＳ２０２０に戻り、新たな画像を取得して再度位置姿勢算出を行う。

以上述べたように、本実施形態では、実写画像中に撮像される対象物体の画像情報を、３次元モデルのエッジに対応する１次元ベクトルとして、保持・更新することにより、実写画像に即した表面情報に基づく位置姿勢推定が可能となる。これにより、実環境中で光源が変化した場合でも、動的に対象物体の画像情報を３次元モデルに反映することが可能となり、光源変化に頑健な物体位置姿勢推定が可能となる。

［変形例１］ モーションブラーを判別して更新判断
第一および第二の実施形態では、更新を判断するための基準として、位置姿勢推定の正確さを利用していた。しかし、本発明の本質は、実写画像中から対象物体の表面情報を表す画像情報を正確に取得できるかどうかを判断することであり、その判断基準としては、上述の方法に限るものではない。例えば、実写画像のモーションブラーを判別し、モーションブラー発生の有無を利用して、表面情報を更新するか、しないかを判断してもよい。モーションブラーの発生が判別できれば、モーションブラーが発生した実写画像を参照することによって、３次元モデルの表面情報が不正確に更新されることを防ぐことが可能となる。モーションブラーの判別方法としては、例えば、
・外部センサとして、例えば角速度センサを用意し、角速度センサからの信号（角速度の大きさ）に基づいてモーションブラーの発生を予想する、
・前フレームの位置姿勢推定値と現フレームの位置姿勢推定値の差分から、現フレームの位置姿勢の移動量を算出し、その移動量に基づいて、モーションブラーを検知する、
等が挙げられる。但し、これら手法は一例であり、モーションブラーを判別することが出来ればいずれの手法でも良く、手法の選択に特に制限はない。

［変形例２］ オクルージョンを判別して更新判断
更新を判断するための基準としては、以上の実施形態において説明した判断基準に限るものではない。例えば、実写画像中に撮像される対象物体にオクルージョンが発生しているかどうかの判別し、それに基づいて、表面情報を更新するか、しないかを判断しても良い。オクルージョンの発生を判別することが可能であれば、他の物体によって対象物体にオクルージョンが発生している実写画像の画像情報を参照することで、３次元モデルの表面情報が不正確に更新されることを防ぐことが可能となる。

実写画像からのオクルージョンの判別方法としては、例えば、
・人間の手によるオクルージョンの場合は、実写画像中に撮像される対象物体の領域内において肌色検出を行い、肌色として検出された領域が一定以上に大きい場合は、更新しないと判断する、
・既知の物体によるオクルージョンの場合は、実写画像中の対象物体の物体領域と、既知の物体の物体領域とのオーバーラップ領域を求め、領域が重なっている場合には、更新しないと判断する、
等してもよい。但し、これら手法は一例であり、実写画像中に撮像される対象物体にオクルージョンが発生しているか否かを判別し、その結果に基づき、表面情報を更新するか、しないかを判別することが出来る手法であればよく、その選択に特に制限はない。

［変形例３］ 実写画像のフレーム間の変化量から更新判断
更新を判断するための基準としては、以上の実施形態において説明した判断基準に限るものではない。例えば、実写画像のフレーム間における画像情報の変化の大きさを算出し、変化の大きさに基づいて、更新するか、しないかを更新を判断してよい。実写画像中に撮像される対象物体の画像情報がフレーム間であまり変化しないような場面では、実写画像に基づいて表面情報の更新を行っても、新しい情報を得ることは少ない。そのため、フレーム間で変化の少ない実写画像に基づいた更新は、無駄な処理になることが多い。これに対し、実写画像中の画像情報の変化量を算出し、フレーム間で実写画像中の物体領域の画像情報があまり変化しない場合は、表面情報の更新処理を行わないようにすることで、処理の無駄を省くことが可能となる。実写画像中の物体の変化量としては、例えば、
・実写画像中に撮像される対象物体領域の輝度のヒストグラムを算出し、フレーム間の輝度ヒストグラムの類似性を評価することで、表面情報の更新を判断する、
・画像全体または対象物体領域から輝度勾配のヒストグラムを算出し、フレーム間の輝度勾配ヒストグラムの類似性から、表面情報を更新するか、しないかを判断する、
等があげられる。但し、これら手法は一例であり、実写画像中に撮像される対象物体が、フレーム間でどの程度画像情報が変化したかを判別することが出来れば、いずれの方法でよく、手法の選択に特に制限はない。

［変形例４］ 照度変化を感知して更新判断
更新を判断するための基準としては、以上の実施形態において説明した判断基準に限るものではない。例えば、実写画像中の照度変化を感知して、更新を判断してよい。対象物体または環境中の照度の変化を感知し、照明の明るさに変化がある場合に応じて、対象物体の表面情報を取得・更新することによって、照明変化に応じた対象物体の見えの変化に対応することが可能となる。対象物体・環境中の照度変化を感知する方法としては、例えば、
・画像全体の輝度ヒストグラムを算出し、輝度ヒストグラムのピークの変化から照度変化を判断する、
・撮像装置に環境中の照度を測るための照度センサを取り付け、照度センサから出力される信号に基づいて照度変化を判断する、
等があげられる。但し、これら手法は一例であり、対象物体または環境中の照度変化を感知することが出来る手法であれば、いずれの手法でよく、手法の選択に特に制限はない。

［変形例５］ 姿勢変化を感知して、更新判断
更新を判断するための基準としては、以上の実施形態において説明した判断基準に限るものではない。例えば、対象物体の姿勢変化を検知し、姿勢変化に応じて更新を判断しても良い。対象物体の位置姿勢が環境に対して変化するような場面では、物体と光源環境との相対的な姿勢が変化によって、物体の陰影が変化し、物体の見えが大きく変化する。物体の姿勢変化を感知し、物体が姿勢変化に応じて表面情報の更新を行うことが出来れば、物体の姿勢変化による見えの変化に対応することが可能になる。物体の姿勢変化を感知する方法としては、例えば、
・対象物体に加速度センサまたは角速度センサなどの対象の動きを感知するセンサを取り付け、センサの信号（出力値）から対象物体の姿勢変化を算出することで、更新判断を行う、
・撮像装置に動きセンサを取り付け、動きセンサからの信号により物体の動きを感知し、更新判断を行う、
等があげられる。但し、これら手法は一例であり、対象物体の姿勢変化を感知することが出来る方法であれば、いずれの手法でよく、手法の選択に特に制限はない。

［変形例６］ 上述の判断基準を組み合わせて、更新判断
第一および第二の実施形態では、単一の判断基準から、表面情報の更新を行うか否かを判断していたが、これに限るものではない。例えば、上述した判断基準を組み合わせて、複数の判断基準に基づいて、更新を判断しても良い。例えば、表面情報の更新判断基準として、それぞれ、位置姿勢の正確さやオクルージョンの有無、モーションブラーの有無や実写画像情報のフレーム間変化量等を算出・評価して、それら全ての論理積あるいは論理和に基づいて更新判断しても良い。または、それら一部組み合わせを抜き出して更新判断するようにしても良い。複数の判断基準から複合的に表面情報の更新の可否を判断することが出来れば、組み合わせ方法や、評価基準の内容に制限はなく、いずれの方法でもよい。

以上説明してきたように、上記各実施形態、各変形例によれば、無駄な更新処理の実行が防止され、処理の効率が向上する。例えば、実写画像の変化が少ない場面など、更新を行っても表面情報に変化が無いような場合は更新処理の実行を行わないことで、無駄な処理を省くことを可能としている。

［変形例７］ 判断基準から更新評価値を算出し、更新評価値を重みにして更新
以上で説明した各実施形態では、判断基準の評価を行い、評価が一定以上であれば更新し、一定以下であれば更新しないように、更新判断処理を行っていたが、これに限るものではない。例えば、実写画像の画像情報による３次元モデルの表面情報の更新処理において、それらをブレンディングする割合として、更新判断の基準に基づいて算出した更新評価値（詳細は後述する）を利用しても良い。具体的には、第一の実施形態を例にあげると、ステップＳ１０８０においては、更新するか、しないかの判断は行わず、判断基準に基づいた更新評価値を算出することのみを行う。そして、ステップＳ１０９０における更新処理において、３次元モデルの表面情報とそれに対応する実写画像中の画像情報とを、算出した更新評価値にしたがってブレンディングすることで、表面情報の更新を行う。すなわち、更新評価値が高い場合は実写画像の寄与度が高く、更新評価値が低い場合は実写画像の寄与度が小さくなるように、表面情報を更新することで、更新評価値の連続値に応じた表面情報の更新が可能となる。

更新評価値としては、例えば、
・位置姿勢推定の正確さを更新基準とした場合には、位置姿勢算出におけるアウトライア率を利用する、
・オクルージョンの有無を更新基準とした場合には、実写画像中におけるオクルージョン領域の割合を利用する、
ことなどがあげられる。また、そのほかの上述した更新基準に基づいて算出しても良いし、さらに、あらかじめ位置姿勢推定の失敗しやすさに応じて事前に適当な値を設定しておいても良いし、その値を上述した他のパラメータを利用して変化させてもよい。実写画像の画像情報が適切に取得できる場合は高く、実写画像の画像情報が不適切である場合は低くなるように設定出来る限り、更新評価値の設定方法に制限はなく、いずれの方法でも良い。また、ステップＳ１０８０で更新するかしないかを判断し、更新すると判断された場合に、ステップＳ１０９０で更新評価値に基づいたブレンディングを行うようにしてもよい。

以上、実施形態を詳述したが、本発明は、例えば、システム、装置、方法、プログラムもしくは記憶媒体等としての実施態様をとることが可能である。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。

尚、本発明は、ソフトウェアのプログラムをシステム或いは装置に直接或いは遠隔から供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムコードを読み出して実行することによって前述した実施形態の機能が達成される場合を含む。この場合、供給されるプログラムは実施形態で図に示したフローチャートに対応したコンピュータプログラムである。

また、コンピュータが、コンピュータ読み取り可能な記憶媒体から読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているＯＳなどとの協働で実施形態の機能が実現されてもよい。この場合、ＯＳなどが、実際の処理の一部または全部を行ない、その処理によって前述した実施形態の機能が実現される。

Claims (15)

1. 撮像装置によって観察対象物体を撮像して得られた実写画像を取得する取得手段と、
   前記観察対象物体の幾何情報及び表面情報を表す３次元モデルデータを保持する保持手段と、
   前記３次元モデルデータを前記撮像装置の位置姿勢情報に基づいて画像平面上に投影することにより描画画像を生成する描画手段と、
   前記描画画像の表す幾何情報及び表面情報と、前記実写画像における観察対象物体の画像の幾何情報及び表面情報との対応に基づいて、前記撮像装置と前記観察対象物体の相対的な位置姿勢を算出する算出手段と、
   前記算出手段で算出された位置姿勢または前記実写画像に基づいて前記３次元モデルデータの表面情報を更新すべきか否かを判断する判断手段と、
   前記判断手段により更新すべきと判断された場合、算出された前記位置姿勢に基づいて前記実写画像における前記観察対象物体の画像情報を前記保持手段が保持する前記３次元モデルデータの前記表面情報に対応付けて、当該対応付けられた画像情報を用いて前記保持手段に保持された前記３次元モデルデータの表面情報を更新する更新手段とを備え、
   前記取得手段による新たな実写画像の取得と、更新された前記表面情報を含む３次元モデルデータに基づく前記描画手段による描画画像の生成と、該生成された描画画像と前記新たな実写画像とを用いた前記算出手段による位置姿勢の算出と、該算出された位置姿勢または前記実写画像に基づく前記判断手段による判断と、該判断手段により更新すべきと判断された場合に、前記算出された位置姿勢に基づく前記更新手段による前記表面情報の更新とを繰り返すことを特徴とする位置姿勢推定装置。
2. 前記判断手段は、前記算出手段において算出された前記位置姿勢の正確さを推定し、推定された正確さに基づいて前記３次元モデルデータの表面情報を更新すべきか否かを決定することを特徴とする請求項１に記載の位置姿勢推定装置。
3. 前記判断手段は、前記実写画像中の前記観察対象物体におけるモーションブラーの大きさを判別することにより、前記３次元モデルデータの表面情報を更新すべきか否かを決定することを特徴とする請求項１に記載の位置姿勢推定装置。
4. 前記判断手段は、前記実写画像に撮像されている前記観察対象物体におけるオクルージョンの有無を判別することにより、前記３次元モデルデータの表面情報を更新すべきか否かを決定することを特徴とする請求項１に記載の位置姿勢推定装置。
5. 前記判断手段は、フレーム間における、前記実写画像中に撮像されている前記観察対象物体の画像情報の変化の大きさに基づいて、前記３次元モデルデータの表面情報を更新すべきか否かを決定することを特徴とする請求項１に記載の位置姿勢推定装置。
6. 前記判断手段は、前記観察対象物体における照度変化を検知することにより、前記３次元モデルデータの表面情報を更新すべきか否かを決定することを特徴とする請求項１に記載の位置姿勢推定装置。
7. 前記判断手段は、前記観察対象物体の前記撮像装置に対する位置姿勢の変化に基づいて、前記３次元モデルデータの表面情報を更新すべきか否かを決定することを特徴とする請求項１に記載の位置姿勢推定装置。
8. 前記更新手段は、前記３次元モデルデータの表面情報を更新すべきか否かを判断するための判断基準に関わる評価値を、前記実写画像の画像情報を前記表面情報に反映させるときの割合として用いることを特徴とする請求項１乃至７のいずれか１項に記載の位置姿勢推定装置。
9. 前記更新手段は、前記観察対象物体の画像情報に基づいて前記３次元モデルデータのテクスチャ画像を更新することを特徴とする請求項１乃至８のいずれか１項に記載の位置姿勢推定装置。
10. 前記表面情報は、３次元モデルデータを構成する線分情報に対応する１次元の情報の集合であり、
    前記更新手段は、前記観察対象物体の画像情報に基づいて、前記１次元の情報を更新することを特徴とする請求項１乃至９のいずれか１項に記載の位置姿勢推定装置。
11. 前記描画手段は、前記３次元モデルデータを画像平面上に投影することにより前記描画画像を生成し、
    前記算出手段は、前記描画手段により得られた前記描画画像と前記実写画像とが対応するように、前記撮像装置と前記観察対象物体の相対的な位置姿勢を算出することを特徴とする請求項１乃至９のいずれか１項に記載の位置姿勢推定装置。
12. 前記描画手段は、前記３次元モデルデータ中の線分情報を画像平面上に投影して、線分による描画画像を生成し、
    前記算出手段は、前記観察対象物体の表面情報に基づいて、前記描画手段により投影した線分と実写画像とが対応するように、前記撮像装置と前記観察対象物体の相対的な位置姿勢を算出することを特徴とする請求項１乃至８および１０のいずれか１項に記載の位置姿勢推定装置。
13. 取得手段が、撮像装置によって観察対象物体を撮像して得られた実写画像を取得する取得工程と、
    描画手段が、保持手段に保持された前記観察対象物体の幾何情報及び表面情報を表す３次元モデルデータを前記撮像装置の位置姿勢情報に基づいて画像平面上に描画することにより描画画像を生成する描画工程と、
    算出手段が、前記描画工程により生成された描画画像と、前記実写画像における観察対象物体の画像との対応を検出することにより、前記撮像装置と前記観察対象物体の相対的な位置姿勢を算出する算出工程と、
    判断手段が、前記算出工程で算出された位置姿勢または前記実写画像に基づいて前記３次元モデルデータの表面情報を更新すべきか否かを判断する判断工程と、
    更新手段が、前記判断工程で更新すべきと判断された場合、算出された前記位置姿勢に基づいて前記実写画像における前記観察対象物体の画像情報を前記保持手段が保持する前記３次元モデルデータの前記表面情報に対応付けて、当該対応付けられた画像情報を用いて前記保持手段に保持された前記３次元モデルデータの表面情報を更新する更新工程と、を有し、
    前記取得工程による新たな実写画像の取得と、更新された前記表面情報を含む３次元モデルデータに基づく前記描画工程による描画画像の生成と、該生成された描画画像と前記新たな実写画像とを用いた前記算出工程による位置姿勢の算出と、該算出された位置姿勢または前記実写画像に基づく前記判断工程による判断と、該判断工程により更新すべきと判断された場合に、前記算出された位置姿勢に基づく前記更新工程による前記表面情報の更新とを繰り返すことを特徴とする位置姿勢推定方法。
14. 請求項１３に記載の位置姿勢推定方法をコンピュータに実行させるためのコンピュータプログラム。
15. 請求項１３に記載の位置姿勢推定方法をコンピュータに実行させるためのコンピュータプログラムを格納したコンピュータ読み取り可能な記憶媒体。

Images