(57)【要約】【課題】観察対象物体の３次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別することにより、位置姿勢検出処理の安定化、効率化を図る【解決手段】位置姿勢算出装置は、観察対象物体の表面形状を表す３次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する。そして、観察対象物体に対する撮像装置の位置姿勢が算出済みとなっている参照画像において複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を、抽出された複数の幾何特徴より選別する。位置姿勢算出装置は、こうして選別された幾何特徴を入力画像における観察対象物体の画像に対応付けることにより、撮像装置の観察対象物体に対する位置姿勢を算出する。【選択図】  図１
2010079452.tif 000002
【特許請求の範囲】
【請求項１】
撮像装置が撮像した画像を入力する入力手段と、
観察対象物体の表面形状を表す３次元モデルデータを格納する格納手段と、
前記３次元モデルデータを画像平面に描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出手段と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別手段と、
前記幾何特徴選別手段で選別された幾何特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出手段とを備えることを特徴とする位置姿勢計測装置。
【請求項２】
前記参照画像から、明度または色に基づいて前記観察対象物体に関連する複数の画像特徴を抽出する画像特徴抽出手段と、
前記画像特徴抽出手段で抽出した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合する統合手段とを更に備え、
前記算出手段は、前記物体特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出することを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項３】
前記参照画像の位置姿勢によって前記３次元モデルデータを画像平面に描画することにより前記複数の画像特徴に対応する幾何特徴を探索し、対応する幾何特徴が検出された画像特徴を除去することで選別された画像特徴を取得する画像特徴選別手段を更に備え、
前記統合手段は、前記画像特徴選別手段で選別した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項４】
前記算出手段で算出された位置姿勢と共に、前記入力手段で入力した画像を、前記参照画像として保持する保持手段を更に備えることを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測装置。
【請求項５】
前記幾何特徴抽出手段は、幾何特徴として、前記３次元モデルデータを画像平面に描画して得られたデプス値に基づいてエッジを抽出することを特徴とする請求項１乃至４のいずれか１項に記載の位置姿勢計測装置。
【請求項６】
前記画像特徴抽出手段は、画像特徴として、前記参照画像から色または明度の境界に基づくエッジを抽出することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項７】
前記算出手段において算出された位置姿勢の推定精度を算出する推定精度算出手段を更に備え、
前記保持手段は、前記推定精度が所定値を超える場合のみ、前記入力手段で入力した画像を前記参照画像として保持することを特徴とする請求項４に記載の位置姿勢計測装置。
【請求項８】
前記保持手段は、複数の参照画像を保持し、
前記幾何特徴選別手段は、前記複数の参照画像から、前記算出手段によって算出された現在の位置姿勢に最も近い位置姿勢を有する参照画像を選択して用いることを特徴とする請求項４に記載の位置姿勢計測装置。
【請求項９】
前記保持手段は、前記入力手段で入力した画像に更に時刻情報を付加して前記参照画像として保持し、時系列的に古い参照画像から更新していくことを特徴とする請求項８に記載の位置姿勢計測装置。
【請求項１０】
前記保持手段は、複数の参照画像を保持し、
前記幾何特徴選別手段は、複数の参照画像で検出され得る画像特徴のみを選別することを特徴とする請求項４に記載の位置姿勢計測装置。
【請求項１１】
前記３次元モデルデータはテクスチャの情報を有し、
前記３次元モデルデータを前記撮像装置について算出された直前の位置姿勢を用いて画像平面上にテクスチャと共に描画する描画手段を更に備え、
前記参照画像は前記描画手段により描画された画像であることを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測装置。
【請求項１２】
撮像装置が撮像した画像を入力する入力工程と、
観察対象物体の表面形状を表す３次元モデルデータを格納した格納手段より前記３次元モデルデータを読み出し、前記３次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出工程と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別工程と、
前記幾何特徴選別工程で選別された幾何特徴を前記入力工程で入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出工程とを有することを特徴とする位置姿勢計測方法。
【請求項１３】
請求項１２に記載された位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【請求項１４】
請求項１２に記載された位置姿勢計測方法をコンピュータに実行させるためのプログラムを格納したコンピュータ読み取り可能な記憶媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、観察対象物体の表面形状を表す３次元モデルデータと撮像装置が撮像した観察対象物体の撮像画像を利用して、撮像装置と観察対象物体との相対的な位置姿勢を算出する位置姿勢計測装置及び方法に関するものである。
【背景技術】
【０００２】
従来より、現実空間を撮像するカメラなどの撮像装置を用いて、観察対象物体と当該観察対象物体を撮像する撮像装置との相対的な位置姿勢を計測する技術が提案されている。この種の位置姿勢計測技術は、現実空間と仮想空間を融合表示する複合現実感システムや、ロボットの位置姿勢計測において、非常に有用な技術である。
【０００３】
従来より、撮像装置の位置姿勢を計測する方法として、３次元位置が既知の指標を用いる方法が提案されている。この方法では、撮像装置のおおよその位置姿勢を用いて各指標の３次元位置を撮像面へ投影した投影位置と、撮像画像から検出される指標の撮像画像上の位置との距離を誤差として、この誤差を小さくする目的関数を最適化するように位置姿勢を推定する。指標としては、予め検出が容易な幾何学的特徴を有するもの、または色相が特異な特徴を有するものを利用することが多い。
【０００４】
また、画像中でのエッジと観察対象の３次元モデルとの対応をもとに撮像装置の位置及び姿勢を求める方法も提案されている。ここで、撮影画像上で撮像される観察対象物体により観察される輝度が不連続に変化する領域をエッジと呼称する。エッジはスケールや観察方向に対して不変であるため、エッジを利用した位置合わせは精度が高いという特徴がある。非特許文献１で鄭なされているエッジを利用した位置合わせは、次の１から３の処理によって実現される。
（１）前フレームでのカメラの位置及び姿勢及び予め校正済みのカメラの固有パラメータに基づいて、前述の３次元線分モデルを画像上に投影する。
（２）投影された各線分を、画像上で一定間隔となるように分割し、分割点を設定する。そして各分割点について、該分割点を通過し向きが投影された線分の法線方向である線分（探索ライン）上でエッジ探索を行い、探索ライン上における輝度値の勾配が極大でありかつ分割点に最も近い点を対応エッジとして検出する。
（３）分割点毎に検出された対応エッジと、投影される線分との間の画像上での距離の総和が最小となるようなカメラの位置及び姿勢の補正値を算出し、カメラの位置及び姿勢を補正する。
【０００５】
エッジを利用した位置合わせでは、３次元線分モデル中のエッジが撮影画像から検出できない場合がある。また、撮影画像から３次元線分モデルとしてモデル化されていないエッジが多量に検出される場合には、誤った点を対応点として検出する誤対応が発生する場合がある。誤対応がある場合には、位置姿勢の最適化計算において繰り返し計算が収束しない、局所解に陥る、得られる撮像装置の位置及び姿勢の精度が低いものになるなどの問題が発生する。この課題に対して、非特許文献２では、ロバスト推定手法の一つであるＭ推定を用いることが記載されている。即ち、非特許文献２は、前述の対応点と線分との距離が大きいデータについては重みを小さく、距離が小さいデータについては重みを大きくして、重み付き誤差の和を最小化することにより誤検出の影響を排除する。
【０００６】
また、非特許文献４では、画像上での線分周辺の見えの情報を保持しておくことにより、３次元線分モデルと撮影画像中のエッジとの対応付けの精度をより向上させている。撮影画像から３次元線分モデルと対応付いたエッジの見え情報を随時更新することで、照明変化や視点変化に起因する誤検出の影響を排除している。
【０００７】
また、非特許文献３では、３次元線分モデルの情報以外に、撮影画像中に撮像される観察対象物体から抽出した点特徴の情報を併用し、エッジを利用した位置合わせが破綻することを回避している。撮影画像中から抽出した点特徴のフレーム間対応に基づいて現フレームにおけるカメラの位置及び姿勢を反復演算により算出し、エッジによる位置合わせの結果と統合することで、位置姿勢推定の安定性を向上させている。
【０００８】
また一方、エッジを利用した位置合わせでは、３次元の線分の集合により記述されている観察対象物体の３次元線分モデルを持つことが前提となっている。非特許文献１、２、３、４では、観察対象物体の３次元線分モデルは、現実の観察対象物体を利用し、手動で作成されている。しかし、現実の観察対象物体から観察されるであろう３次元エッジを選別し、手動で生成するのは、比較的複雑で、多くの時間を要する。また、曲面が形作る輪郭に基づくエッジは、オフラインの３次元線分モデルとして記述することは困難である。
【０００９】
これに対し、ＣＡＤモデルなどの観察対象物体の形状を表す３次元モデルデータから３次元線分モデルを生成して、エッジによる位置姿勢推定を行う方法が、特許文献１で提案されている。この手法では、あらかじめ３次元モデルデータを描画し、描画結果から３次元モデルデータの幾何的な情報に基づくエッジを検出することで、３次元線分モデルを生成する。また、非特許文献５では、対象物体の３次元モデルデータをあらかじめ描画し、描画した視点の奥行き値から幾何的情報に基づくエッジを抽出することで、３次元線分モデルを生成している。これらの手法では、あらかじめ観察対象物体の３次元モデルを描画し、描画画像からエッジを抽出することで、観察対象物体の３次元線分モデルを生成している。これにより、観察対象物体の３次元線分モデルを手動で作成する手間を低減することができる。
【００１０】
しかし、上記の手法は、曲面を有する物体を様々な方向から見るような場面のように、あらかじめ描画した観察対象物体の見えと、オンラインでの見えが同じとは限らないものに対しては対応できないという問題がある。これに対して、テクスチャの設定された３次元モデルをオンラインで描画し、その描画画像から３次元線分モデルを随時生成して、位置姿勢推定を行う手法が非特許文献６で提案されている。この手法によれば、現実空間中の観察対象物体と同様の見えとなるように、テクスチャの設定された３次元モデルデータを随時描画することで、様々な方向から見る場面に対しても対応することができる。
【００１１】
一方、３次元モデルデータを利用せず、撮影画像のみから特徴の抽出と３次元位置計算を行い、オンラインで３次元線分モデルを生成しながら位置姿勢推定を行う手法が非特許文献７で提案されている。この手法では、観察対象物体の３次元モデルを必要とせず、時系列的な撮影画像のみから３次元線分モデルを生成して位置姿勢推定することが可能である。
【非特許文献１】T. Drummond and R. Cipolla,“Real-time visual tracking of complex structures,” IEEE Transactions on PatternAnalysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002.
【非特許文献２】L. Vacchetti, V. Lepetit, and P.Fua, “Combining edge and texture information for real-time accurate 3D cameratracking,” Proc. The 3rd IEEE/ACM International Symposium on Mixed andAugmented Reality (ISMAR04), pp.48-57, 2004.
【非特許文献３】E. Rosten and T. Drummond, “Fusingpoints and lines for high performance tracking,” Proc. The 10th IEEEInternational Conference on Computer Vision (ICCV’05), pp.1508-1515, 2005.
【非特許文献４】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.
【非特許文献５】G . Bleser, H . Wuest, D.Stricker, “Online camera pose estimation in partially known and dynamicscenes,” Proc. The 5th IEEE/ACM International Symposium on Mixed and AugmentedReality (ISMAR06), pp.56-65, 2006.
【非特許文献６】G. Reitmayr and T. W. Drummond,“Going out: robust model-based tracking for outdoor augmented reality,” Proc.The 5th IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR06),pp.109-118 2006.
【非特許文献７】E. Eade and T. Drummond, “Edgelandmarks in monocular SLAM,” Proc. BMVC06, pp.7-16, 2006.
【非特許文献８】K. Satoh, S. Uchiyama, H.Yamamoto, and H. Tamura, “Robust vision-based registration utilizing bird’s-eyeview with user’s view,” Proc. The 2nd IEEE/ACM International Symposium on Mixedand Augmented Reality (ISMAR03), pp.46-55, 2003.
【非特許文献９】I. Skrypnyk and D. G. Lowe, “Scenemodelling, recognition and tracking with invariant image features,” Proc. The3rd IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR04),pp.110-119, 2004.
【特許文献１】特開２００７-２０７２５１号公報
【発明の開示】
【発明が解決しようとする課題】
【００１２】
特許文献１で提案されている従来技術では、観察対象物体の表面形状を表す３次元モデルデータの幾何的な情報に基づいて３次元線分モデルを生成している。しかし、オフラインであらかじめ３次元モデルデータを描画して３次元線分モデルを生成する手法では、生成した３次元線分モデルに対応付く特徴が撮影画像から常に検出されるという保証はない。３次元線分モデルに対応付く特徴を撮影画像から検出できるか否かが、対象物体の表面色や撮像時の照明環境に依存しているからである。生成された３次元線分モデルに対応付く特徴が撮影画像から検出できない場合、その線分が誤対応を起こす可能性が高くなり、位置姿勢推定精度が低下するという課題が発生する。
【００１３】
また、非特許文献５で開示されている手法でも、あらかじめ３次元モデルを描画して、その描画画像の奥行き値から３次元線分モデルを生成しているため、生成した３次元線分モデルに対応付く特徴が実際の撮影画像から検出できるとは限らない。そのため、特許文献１と同様の課題が発生する。
【００１４】
このような誤対応問題に対しては、非特許文献２に提案されるようなＭ推定を利用した誤対応に対するロバスト化を適用することが考えられる。しかし、この手法は、撮影画像から安定して検出されるエッジのみから構成された３次元線分モデルを前提としているため、３次元線分モデルに登録してある線分が撮影画像から観察されない場合には、アウトライヤとして排除する処理しか行わない。そのため、３次元線分モデル自体が不正確である場合など、誤対応として排除される特徴が多くある場合には、位置姿勢推定が不安定になるという課題がある。また、非特許文献４に提案されているような、線分周辺の見えを利用した対応付けロバスト化手法でも、３次元線分モデルに対応付く特徴が撮影画像から検出できない場合はアウトライヤとして排除するだけである。そのため、非特許文献４の手法を用いても、３次元線分モデルが不正確な場合には、位置姿勢推定が不安定になる。
【００１５】
一方、物体表面の模様などの、幾何的には検出できないが色の不連続領域としては検出できる特徴が観察対象物体に存在する場合、前述の従来手法では、このような色に基づく特徴を利用することはできず、逆に特徴が誤対応する原因となる。これに対し、非特許文献６で提案されている手法では、観察時の見た目と同様の見た目を描画できるようなテクスチャデータが設定された３次元モデルデータを利用することで、色に基づく特徴を含めて３次元線分モデルを生成し位置姿勢推定を行っている。しかし、非特許文献６の手法は、観察時の見た目と同様の見た目を描画できるようなテクスチャデータが設定された３次元モデルデータを利用することを前提としている。したがって、３次元モデルデータの設定が不正確、あるいは、実環境に光源変化があるような場合などの３次元モデルデータと実際の観察対象物体の見た目が異なる場合には、生成される３次元線分モデルが不正確になる。そのため、観察対象物体の見た目に合わせて、同様な見た目になるように３次元モデルデータを作成する必要があり、また、実環境が変化した場合には対応できないという課題がある。
【００１６】
また、非特許文献３で提案されている手法では、観察対象物体を撮像した撮影画像から色に基づく特徴を随時抽出し、３次元線分モデルに加えて、抽出した特徴も利用して位置姿勢推定を行っている。しかし、この手法は、３次元線分モデルとともに、撮影画像から抽出した特徴を併用することに焦点を当てた手法であり、３次元線分モデルは、十分に特徴として観察されるエッジのみから構成されていることを前提としている。そのため、３次元線分モデルが不正確な場合、上記手法と同様に、３次元線分データと撮影画像中のエッジとの間に誤対応が頻出し、位置姿勢推定が不安定になるという課題がある。
【００１７】
また、非特許文献７で提案されているような、純粋に撮影画像のみから３次元線分モデルを生成し位置姿勢推定する手法では、位置姿勢推定の精度が十分ではないという課題がある。３次元モデルデータを利用して３次元線分モデルを生成する手法と比較して、３次元線分モデルの精度が低くなってしまうためである。
【００１８】
本発明は、以上の問題を鑑みてなされたものであり、観察対象物体の３次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別することにより、位置姿勢検出処理の安定化、効率化を図ることを目的とする。また、観察対象物体の３次元モデルデータから抽出した特徴が、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定可能とすることを目的とする。
【課題を解決するための手段】
【００１９】
上記の目的を達成するための本発明の一態様による位置姿勢計測装置は以下の構成を備える。即ち、
撮像装置が撮像した画像を入力する入力手段と、
観察対象物体の表面形状を表す３次元モデルデータを格納する格納手段と、
前記３次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出手段と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別手段と、
前記幾何特徴選別手段で選別された幾何特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出手段とを備える。
【００２０】
また、本発明の他の態様による位置姿勢計測装置は、更に、
前記参照画像から、明度または色に基づいて前記観察対象物体に関連する複数の画像特徴を抽出する画像特徴抽出手段と、
前記画像特徴抽出手段で抽出した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合する統合手段とを更に備え、
前記算出手段は、前記物体特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する。
【００２１】
また、上記の目的を達成するための本発明の他の態様による位置姿勢計測方法は、
撮像装置が撮像した画像を入力する入力工程と、
観察対象物体の表面形状を表す３次元モデルデータを格納した格納手段より前記３次元モデルデータを読み出し、前記３次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出工程と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別工程と、
前記幾何特徴選別工程で選別された幾何特徴を前記入力工程で入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出工程とを有する。
【発明の効果】
【００２２】
本発明によれば、観察対象物体の３次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別するので、位置姿勢検出処理の安定化、効率化を図ることができる。また、観察対象物体の３次元モデルデータから抽出した特徴が、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定することができる。
【発明を実施するための最良の形態】
【００２３】
以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
【００２４】
［第１実施形態］
本実施形態では、本発明を、画像上のエッジを利用した位置合わせ手法に適用した場合について説明する。
【００２５】
図１は、第１実施形態における位置姿勢計測装置１の機能構成を示している。なお、位置姿勢計測装置１は、例えば一般的なコンピュータ装置に以下で説明する機能を実現するためのソフトウエアを実行させることによっても実現され得る。この場合、以下に説明する各機能構成の一部或いは全てが、コンピュータ装置のＣＰＵによって実行されることになる。なお、第２、第３実施形態についても同様である。
【００２６】
図１に示したように、位置姿勢計測装置１は、画像取得部１１０、モデル保存部１２０、参照画像保存部１３０、幾何特徴抽出部１４０、色特徴抽出部１５０、画像特徴検出部１６０、位置姿勢算出部１７０を有する。画像取得部１１０は撮像装置１００に接続されている。撮像装置１００は、動画撮影が可能であり、観察対象物体１０を撮影する。位置姿勢計測装置１は、観察対象物体の３次元モデルデータ２０に記述された観察対象物体１０と撮像装置１００との相対的な位置姿勢を、撮像装置１００により得られた撮像画像の情報を用いて求める。なお、本実施形態において位置姿勢計測装置１による位置姿勢計算処理が適用できる条件として、撮像装置１００が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置１を構成する各部について説明する。
【００２７】
画像取得部１１０は、撮像装置１００によって撮像された画像を位置姿勢計測装置１に入力する。画像取得部１１０は、撮像装置の出力がＮＴＳＣなどのアナログ出力であればアナログビデオキャプチャボードによって実現される。また撮像装置の出力がＩＥＥＥ１３９４などのデジタル出力であれば、例えばＩＥＥＥ１３９４インタフェースボードによって実現される。また、予め記憶装置に記憶してある静止画像や動画像のデジタルデータを読み出してもよい。
【００２８】
モデル保存部１２０は、位置姿勢計測の基準となる観察対象物体１０の表面形状を表す３次元モデルデータ２０を記憶する。３次元モデルデータ２０は、観察対象物体の３次元の幾何的な情報を有するモデルであり、頂点情報及び各頂点を結んで構成される面の情報によって定義される。位置姿勢計測装置１では、観察対象物体の３次元形状を表す面の情報を含んでいれば良いので、３次元モデルデータとしては、面の情報がパラメトリックに記述されたソリッドモデルでも良いし、ポリゴンモデルでも良い。また、任意の３次元モデラーのフォーマットを変換する処理部を位置姿勢計測装置１に含むようにしても構わない。なお、本実施形態では、頂点情報及び各頂点を結んで構成される面の情報を含むポリゴンモデルを３次元モデルデータとして利用する。
【００２９】
参照画像保存部１３０は、過去フレームにおいて観察対象物体が撮像された撮影画像と、撮像されたときの観察対象物体と撮像装置との相対的な位置姿勢とを保存する。参照画像保存部１３０で参照画像として保持する撮影画像としては、直前のフレームの撮影画像のみとしても良いし、過去フレームの複数の撮影画像としても良いし、また、過去フレームにおいて位置姿勢推定精度の高かったフレームの撮影画像のみとしても良い。即ち、撮影画像中に撮像される観察対象物体と撮像装置との相対位置姿勢が既知の撮影画像であれば、数を問わず、またいずれの撮影画像でもよい。本実施形態では直前フレームの撮影画像を参照画像として保持するものとする。
【００３０】
幾何特徴抽出部１４０は、モデル保存部１２０に保存された観察対象物体１０の３次元モデルデータ２０を画像平面上に描画することにより観察対象物体１０の幾何的な情報に基づく特徴（以後、幾何特徴）を抽出する。なお、３次元モデルデータ２０の描画においては、撮像装置１００の位置姿勢として、初期値として設定された位置姿勢、或いは撮像装置１００に関して算出されている現在の位置姿勢が用いられる。そして、幾何特徴抽出部１４０は、抽出した幾何特徴の中で、撮像装置により撮像された撮影画像中から検出が期待できる幾何特徴のみを、参照画像保存部１３０に保持されている位置姿勢が既知の画像（参照画像）を参照して選別する。幾何特徴の抽出方法および選別方法については後述する。
【００３１】
色特徴抽出部１５０は、参照画像保存部１３０に参照画像として保持されている過去フレームの撮影画像中の観察対象物体の画像から、観察対象物体の明度や色の少なくともいずれかに基づく画像特徴（以後、色特徴という）を抽出する。そして、色特徴抽出部１５０は、参照画像保存部１３０に保存された観察対象物体の位置姿勢の情報と、モデル保存部１２０に保存された３次元モデルデータ２０に基づいて、参照画像が保存された位置姿勢から観察される観察対象物体の幾何特徴を抽出する。そして、色特徴抽出部１５０は、抽出した色特徴と幾何特徴とを比較し、対応する幾何特徴が検出された色特徴を除去することで、色に基づいて抽出される特徴のみを選別する。色に基づく特徴の抽出方法については後述する。
【００３２】
画像特徴検出部１６０は、画像取得部１１０によって入力された画像上において、撮像装置と観察対象物体との位置姿勢を算出するのに用いられる画像特徴を検出する。本実施形態では、画像特徴検出部１６０は画像上のエッジの検出を行う。エッジの検出方法については後述する。
【００３３】
概略位置姿勢を反復演算の初期値として、撮像装置と観察対象物体との相対的な位置姿勢を算出する。より具体的には、位置姿勢算出部１７０は、
・画像特徴検出部１６０が検出した画像特徴の情報と、
・幾何特徴抽出部１４０が抽出した幾何特徴と、
・色特徴抽出部１５０が抽出した色特徴とに基づいて、観察対象物体１０を基準とした座標系（以下、基準座標系）における撮像装置１００の位置及び姿勢を算出する。
【００３４】
次に、第１実施形態における位置姿勢計測方法の処理手順について説明する。図２は、第１実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【００３５】
（ステップＳ１０１０）
まずステップＳ１０１０では初期化を行う。ここでは、位置姿勢計測装置１のＣＰＵが、基準座標系における撮像装置１００と観察対象物体１０との相対的な概略位置姿勢の設定を行う。第１実施形態における位置姿勢計測方法は、概略の撮像装置の位置姿勢を、撮影画像上に撮像される観察対象物体のエッジ情報を利用して逐次更新していく方法である。そのため、位置姿勢計測を開始する前に予め撮像装置の概略の位置及び姿勢を初期位置及び初期姿勢として与える必要がある。
【００３６】
概略の位置及び姿勢を与える初期化の方法としては、例えば予め決まった位置及び姿勢を設定しておき、撮像装置をその位置及び姿勢になるように移動することで初期化を行うことが挙げられる。または、画像内で検出するだけで認識可能な人工的な指標を配置し、該指標の各頂点の画像座標と基準座標系における３次元位置との対応から撮像装置の位置姿勢を求めて概略の位置姿勢としてもよい。また、識別性の高い自然特徴点を予め検出してその３次元位置を求めておき、初期化時に画像上で該特徴点を検出し、その画像座標と３次元位置との対応から撮像装置の位置姿勢を求めてもよい。さらに、磁気式や光学式、超音波式などの６自由度位置姿勢センサによって撮像装置の位置姿勢を計測し、それを概略の位置姿勢としてもよい。人工的な指標や自然特徴点などの画像情報と、前述の６自由度位置姿勢センサや３自由度の姿勢センサ、３自由度の位置センサを併用して計測される撮像装置の位置姿勢を用いて初期化してもよい。
【００３７】
（ステップＳ１０２０）
ステップＳ１０２０では、撮像装置１００が撮像した画像を画像取得部１１０を介して位置姿勢計測装置１に取り込む。
【００３８】
（ステップＳ１０３０）
次にステップＳ１０３０において、幾何特徴抽出部１４０は、観察対象物体の幾何的情報に基づく特徴を検出する。第１実施形態で検出する幾何的情報に基づく特徴はエッジである。図３は、本実施形態における観察対象物体の幾何特徴の検出方法について詳細な処理手順を示すフローチャートである。
【００３９】
まず、ステップＳ１２１０において、幾何特徴抽出部１４０は、モデル描画を行う。モデル描画とは、撮像装置１００に設定されている位置姿勢に基づいて、モデル保存部１２０に保存された観察対象物体の３次元モデルを画像平面上に投影することである。最初のモデル描画では、ステップＳ１０１０で求めた観察対象物体の概略位置姿勢（初期値）が用いられることになる。また、２回目以降の処理では、撮像装置１００と観察対象物体１０との間の直前に取得された相対位置姿勢が用いられる。
【００４０】
モデル描画を行うためには、撮像装置１００の位置姿勢とともに、射影行列の内部パラメータ（焦点距離や主点位置など）を設定する必要がある。本実施形態では、カメラの内部パラメータをあらかじめ計測しておき、実際に用いるカメラ（撮像装置１００）と内部パラメータを一致させておく。また、視点からモデルまでの距離の最大値と最小値を設定しておき、その範囲外のモデルの描画は行わないことで、描画処理の計算コストを低減する。図４にステップ１２１０により描画された結果を示す。概略位置姿勢により３次元モデルを画像平面上に投影することにより、画像平面上での２次元座標とその輝度値、２次元座標に対応する画像平面から３次元モデルまでの奥行き値（デプス値）が格納されたデプスバッファが算出される。
【００４１】
次にステップＳ１２２０において、幾何特徴抽出部１４０は、ステップＳ１２１０の描画過程において生成されたデプスバッファに対して、エッジ検出を行う。デプスバッファに対してエッジ検出することで、奥行きが不連続に変化する領域を求めることができる。エッジ検出では、例えば、Cannyアルゴリズムなどによってエッジが抽出され、２値画像形式として利用できるようになる。この過程において、抽出されたエッジは、画像中で白く表され、それ以外は黒く表される。
【００４２】
次にステップＳ１２３０において、幾何特徴抽出部１４０は、ステップＳ１２２０で生成された２値化画像について、隣接エッジのラベリングを行い、エッジの連結成分を抽出する。ラベリングは、例えばある画素の周囲8画素の中にエッジが存在すれば、同一ラベルを割り当てることにより行う。
【００４３】
図５は、ステップＳ１２３０において、連結成分を抽出したエッジを撮影画像上に重ねた結果を示す図である。図５（ａ）は撮像された画像そのものであり、図５（ｂ）は撮影画像上に抽出したエッジを重ね合わせた図である。ステップＳ１０１０で算出した概略の位置姿勢と、実際の位置姿勢が異なる場合には、図５（ｂ）に示すように実際に撮像される物体と抽出したエッジとの間にずれが生じる。
【００４４】
次にステップＳ１２４０において、幾何特徴抽出部１４０は、ステップＳ１２３０で連結成分を抽出したエッジに対して、エッジ素の抽出を行う。エッジ素とは、画像上での端点間の距離が極短いエッジのことをいう。同一ラベルにラベリングされたエッジを画像上で等間隔に分割するように分割点を算出して、この分割点の周辺の極短い連結成分を求めることでエッジ素を抽出する。図６はラベリングされたエッジから等間隔に抽出したエッジ素を表す図である。本実施形態では、分割点から３pixel離れた連結成分を端点（始点と終点）に設定して、分割点を中心としたエッジ素を抽出している。デプスバッファから抽出したエッジ素の総数をＮとして、各エッジ素をGfi（i=1,2…N）で表す。エッジ素の数Ｎが多いほど、処理時間が長くなる。そのため、画像上でのエッジ素を抽出する間隔を変えることで処理時間を制御できる。また、エッジ素の数を一定として処理時間が一定になるように、画像上でのエッジ素間の間隔を逐次変更してもよい。
【００４５】
次にステップＳ１２５０において、幾何特徴抽出部１４０は、ステップＳ１２４０で算出されたエッジ素に対して、基準座標系における３次元座標を求める。これには、ステップＳ１２１０で生成したデプスバッファを利用する。前述の通り、デプスバッファには、画像座標に対応する奥行き値が格納されている。但し、デプスバッファに格納されている奥行き値は、ステップ１２１０のレンダリング過程で用いた、視点からモデルまでの距離の最大値と最小値により０から１の値に正規化されている。そのため、デプスバッファの奥行き値から直接、基準座標系における３次元座標を求めることはできない。そこで、上述の距離の最大値と最小値を利用して、まずはデプスバッファの値を、カメラ座標系における視点からエッジ素までの距離に変換する。同時に射影行列の内部パラメータを利用して、カメラ座標系におけるエッジ素の３次元座標を求める。そして、カメラ座標系における３次元座標に対して、レンダリング過程で用いた位置姿勢の逆変換をほどこすことで、基準座標系におけるエッジ素の３次元座標を求める。３次元座標が算出されたエッジ素GFiの集合を以後、幾何特徴モデルと呼ぶ。
【００４６】
（ステップＳ１０４０）
次に、ステップＳ１０４０において、参照画像保存部１３０に、参照画像としての過去フレームの撮影画像が保存されているか否かが判定される。参照画像が保存されている場合は、ステップＳ１０５０に進み、保存されていない場合は、ステップＳ１０５０以降の過去フレームの撮影画像を利用するステップをスキップして、ステップＳ１０８０へ進む。特に最も初期のフレームについては、過去フレームの撮影画像（参照画像）が存在しないため、処理は自動的にステップＳ１０８０に進む。
【００４７】
（ステップＳ１０５０）
ステップＳ１０５０において、幾何特徴抽出部１４０は、参照画像保存部１３０に保存された過去フレームの撮影画像を利用して、ステップＳ１０３０で抽出した幾何特徴の選別を行う。即ち、幾何特徴抽出部１４０は、観察対象物体１０に対する撮像装置１００の位置姿勢が算出済みとなっている（既知である）参照画像において、ステップＳ１０３０で抽出した複数の幾何特徴に対応する画像特徴を探索する。そして、対応する画像特徴が検出された幾何特徴を当該複数の幾何特徴より取り出すことにより選別が行われる。図７は、本実施形態における観察対象物体の幾何特徴の選別方法について詳細な処理手順を示すフローチャートである。
【００４８】
まず、ステップＳ１３１０において、幾何特徴抽出部１４０は、変数ｉを１にセットする。処理は、ステップＳ１３２０に進む。
【００４９】
ステップＳ１３２０において、幾何特徴抽出部１４０は、参照画像保存部１３０に参照画像とともに保存されている、観察対象物体と撮像装置との相対位置姿勢の情報を取得する。そして、幾何特徴抽出部１４０は、取得した相対位置姿勢に基づいて、ステップＳ１０３０で求めた幾何特徴モデルのエッジ素GFiの３次元座標を当該参照画像中に投影し、幾何特徴モデルのエッジ素GFiの参照画像中における２次元座標を求める。図８は、参照画像上にエッジ素を投影した図である。図８（ａ）は参照画像保存部１３０に保存された参照画像であり、図８（ｂ）は参照画像上に投影したエッジ素を重ね合わせた図である。図８（ｂ）に示すように、エッジ素は、参照画像保存部１３０に保存されている位置姿勢の情報に基づいて、参照画像上に撮像される観察対象物体と重なる位置に投影される。
【００５０】
次に、ステップＳ１３３０において、幾何特徴抽出部１４０は、ステップＳ１３２０で求めた２次元座標の端点（始点と終点）を結んだ直線の法線方向に平行でかつエッジ素の中心点を通過する線分（以下、探索ライン）上における濃度勾配を計算する。
【００５１】
次に、ステップ１３４０において、幾何特徴抽出部１４０は、濃度勾配の極値が存在するかを算出する。図９は、本実施形態における特徴判別を説明する図である。図９に示される下側の２つのエッジ素のように、影等によりエッジ素の近傍の探索ライン上に極値がない場合は、撮影画像中から検出できない幾何特徴であると見なして、ステップ１３５０において、エッジ素GFiを除去し、幾何特徴モデルの総数Ｎから１減分する。図９上に示される、上側３つのエッジ素のように、近傍に極値がある場合は、撮影画像から検出できる幾何特徴であると見なして、ステップ１３６０に進む。本実施形態では、エッジ素の近傍を、中心点から探索ライン両端方向に５[pixel]以内の範囲と定義する。
【００５２】
次にステップＳ１３６０において、幾何特徴抽出部１４０は、変数ｉを１増分し、処理はステップＳ１３７０に進む。ステップＳ１３７０において、幾何特徴抽出部１４０は、全てのエッジ素GFiについて上記処理が終了したか否かを判断する。全てのエッジ素GFiについて上記処理が終了している場合には、当該判別処理を終了し、終了していない場合は処理はステップＳ１３２０に戻る。以上の処理により、幾何特徴モデルの中で、参照画像から検出されない可能性が高いエッジ素が除去される。除去されなかったエッジ素は、撮影画像から検出が期待できるエッジとして選別されたことになる。
【００５３】
（ステップＳ１０６０）
次にステップＳ１０６０において、色特徴抽出部１５０は、参照画像保存部１３０において保存されている過去フレームの撮影画像（参照画像）から観察対象物体の色情報に基づく特徴（画像特徴）を検出する（画像特徴抽出処理）。図１０は、本実施形態における前フレームの撮影画像（参照画像）からの、観察対象物体の色に基づく特徴の検出方法について詳細な処理手順を示すフローチャートである。
【００５４】
まず、ステップＳ１４１０において、色特徴抽出部１５０は、参照画像保存部１３０に保存された参照画像からエッジを抽出する。参照画像に対してエッジ検出することで、輝度値が不連続に変化する領域を求めることができる。エッジの検出方法は、ステップＳ１２２０で記載した処理と同様である。ただし、ステップＳ１２２０の処理と異なり、参照画像を対象としてエッジ検出を行う。
【００５５】
次に、ステップＳ１４２０において、色特徴抽出部１５０は、ステップＳ１４１０で抽出したエッジのラベリングを行い、エッジの連結成分を抽出する。ラベリング方法は、ステップＳ１２３０で記載した処理と同様である。
【００５６】
次に、ステップＳ１４３０において、色特徴抽出部１５０は、ステップＳ１４２０で抽出したエッジの連結成分からエッジ素を抽出する。エッジ素の抽出方法は、ステップＳ１２４０で記載した処理と同様である。
【００５７】
次に、ステップＳ１４４０において、色特徴抽出部１５０は、ステップＳ１４３０で抽出したエッジ素の基準座標上の３次元座標を計算する。まず、色特徴抽出部１５０は、参照画像保存部１３０に保存された観察対象物体と撮像装置との相対位置姿勢に基づいて、モデル保存部１２０に保存されている３次元モデルデータ２０を描画し、参照画像のデプスバッファを取得する。そして、色特徴抽出部１５０は、取得したデプスバッファを利用して、エッジ素の３次元座標を計算する。デプスバッファからのエッジ素の３次元座標の算出方法は、ステップＳ１２５０で記載した処理と同様である。以上の処理により、基準座標系における３次元座標が算出された参照画像の色の境界に基づくエッジ素CFjの集合を以後、色特徴モデルと呼ぶ。
【００５８】
（ステップＳ１０７０）
次に、ステップＳ１０７０において、色特徴抽出部１５０は、ステップＳ１４４０で生成したデプスバッファを利用して、ステップＳ１０６０で抽出した色特徴の選別（画像特徴選別処理）を行う。図１１は、本実施形態における観察対象物体の色特徴モデルのエッジ素CFiの選別方法について詳細な処理手順を示すフローチャートである。
【００５９】
まず、ステップＳ１５１０において、色特徴抽出部１５０は、変数ｊを１にセットし、処理はステップＳ１５２０に進む。
【００６０】
次に、ステップＳ１５２０において、色特徴抽出部１５０は、ステップＳ１４３０で求めた、色の特徴に基づくエッジ素CFiの探索ライン上における、ステップＳ１４４０で求めたデプスバッファ上のデプス値の勾配を計算する。
【００６１】
次に、ステップＳ１５３０において、色特徴抽出部１５０は、ステップＳ１５２０で求めたデプスバッファ上の濃度勾配から極値が検出されるかを判別する。エッジ素の近傍でデプスバッファ上から極値が検出された場合、そのエッジ素は幾何的にも抽出されるエッジ素であると見なして、ステップＳ１５４０において、エッジ素CFjを除去する。エッジ素の近傍でデプスバッファ上から極値が検出されない場合は、色特徴抽出部１５０は、当該エッジ素は物体表面の色に基づくエッジであると見なして、処理をステップＳ１５５０に進める。
【００６２】
次にステップＳ１５５０において、色特徴抽出部１５０は、変数ｊを１増分し、処理をステップＳ１５６０に進める。ステップＳ１５６０において、全てのエッジ素CFjについて処理が終了しているどうかが判断される。全てのエッジ素について処理が終了している場合には、当該判別処理を終了し、終了していない場合は処理をステップ１３２０に戻す。以上の処理により、色特徴モデルの中で、幾何的にも検出できるエッジ素が除去され、物体表面の色の境界に基づくエッジのみが選別される。
【００６３】
（ステップＳ１０８０）
次にステップＳ１０８０において、画像特徴検出部１６０は、画像特徴選別処理で選別した色特徴と、幾何特徴選別処理で選別した幾何特徴とを物体特徴として統合する。より具体的には、画像特徴検出部１６０は、ステップＳ１０３０で生成した幾何特徴モデルとステップＳ１０６０で生成した色特徴モデルを統合した物体特徴モデルの生成を行う。物体特徴モデルは、幾何特徴モデルのエッジ素と色特徴モデルのエッジ素の集合であり、総数をＬとし、各エッジ素をOFk（k=1,2,…,L）で表す。エッジ素の総数Ｌは、幾何特徴モデルの総数Ｍと色特徴モデルの総数Ｎを足したものである。但し、本実施形態では、幾何特徴モデルの総数Ｍと色特徴モデルの総数Ｎは、それぞれステップＳ１０５０、Ｓ１０７０の選別処理によって選別された数となっている。図１２は、本実施形態における観察対象物体の幾何特徴モデルと色特徴モデルの統合方法について詳細な処理手順を示すフローチャートである。
【００６４】
まず、ステップＳ１６１０において、画像特徴検出部１６０は、ステップＳ１０３０で生成され、ステップＳ１０５０で選別されたエッジ素で構成された幾何特徴モデルを物体特徴モデルに登録する。物体特徴モデルの総数Ｌには幾何特徴モデルの総数Ｍが加えられる。
【００６５】
次に、ステップＳ１６２０において、画像特徴検出部１６０は、ステップＳ１０６０において色特徴モデルが生成されている場合は、ステップＳ１６３０に進み、生成されていない場合は、処理を終了する。
【００６６】
次に、ステップＳ１６３０において、画像特徴検出部１６０は、ステップＳ１０１０で求めた概略位置姿勢に基づいて、色特徴モデルの３次元座標を投影し、現フレームの撮影画像上での画像座標を算出する。幾何特徴モデルの３次元座標に対応する撮影画像上での画像座標は、ステップＳ１０３０中のモデル描画・エッジ素抽出処理により既に算出されている。
【００６７】
次に、ステップＳ１６４０では、ステップＳ１６３０で、画像座標を求めた色特徴モデルを物体特徴モデルに登録し、物体特徴モデルの総数Ｌに色特徴モデルの総数Ｌを加える。
【００６８】
（ステップＳ１０９０）
ステップ１０９０において、画像特徴検出部１６０は、ステップＳ１０９０で求めた物体特徴モデルのエッジ素OFk（k=1,2,…,L）に対応する、撮像装置１００によって撮像された現フレーム撮影画像の画像特徴の検出を行う。本実施形態における画像特徴はエッジである。図１３は、本実施形態におけるエッジ検出方法の詳細な処理手順を示すフローチャートである。図１４は、物体特徴モデルのエッジ素OFkを撮影画像上に重ねた結果を示す図である。図１４（ａ）は撮像された画像そのものであり、図１４（ｂ）は物体特徴モデルのエッジ素を重ね合わせた図である。ステップＳ１３３０やＳ１５２０と同様に、エッジ素の画像座標周辺のエッジを撮影画像から抽出する。
【００６９】
まず、ステップＳ１７１０において、画像特徴検出部１６０は、変数ｋを１にセットし、処理素ステップＳ１７２０に進める。
【００７０】
次に、ステップＳ１７２０において、画像特徴検出部１６０は、物体特徴モデルのエッジ素OFkに対応するエッジを画像から検出する。エッジ素OFkの探索ライン（エッジ素の法線方向の線分）上において、撮影画像上の濃度勾配から極値を算出する。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。本実施形態では、探索ライン上でエッジが複数存在する場合には、最もエッジ素の中心点に近いエッジを対応点とし、その画像座標とエッジ素OFkの３次元座標を保持する。なお、本実施形態では最もエッジ素OFkに近いエッジを対応点としているが、これに限るものではなく、濃度勾配の極値の絶対値が最も大きいエッジを対応点としてもよい。また、非特許文献２に示すように、１つではなく複数の点を対応点候補として保持しても本発明の本質が損なわれることはない。
【００７１】
ステップＳ１７３０において、画像特徴検出部１６０は、変数ｋを１増分し、処理をステップＳ１７４０に進める。すべてのエッジ素OFkについて処理が終了している場合には当該処理を終了する。一方、すべてのエッジ素OFkについて処理が終了していない場合にはステップＳ１７２０に処理を戻し、上記処理が繰り返される。
【００７２】
（ステップＳ１１００）
ステップＳ１１００において、位置姿勢算出部１７０は、撮像装置１００の観察対象物体１０に対する相対的な位置姿勢を算出する。相対的な位置姿勢は、幾何特徴選別処理で選別された幾何特徴と画像特徴選別処理で選別された色特徴を統合して得られた物体特徴を、現フレームにおける観察対象物体の画像に対応付けることにより算出される。より、具体的には、位置姿勢算出部１７０は、非線形最適化計算を用いて、撮像装置と観察対象物体との概略の相対的な位置姿勢を反復演算により補正することにより撮像装置の位置姿勢を算出する。ここで、ステップＳ１０８０において生成された物体特徴モデルのエッジ素OFkのうち、ステップＳ１０９０において対応点が求まったエッジ素の総数をLcとする。図１５は、エッジの情報を利用して撮像装置の位置及び姿勢を算出する方法を説明する図である。図１５では、画像の水平方向、垂直方向をそれぞれｘ軸、ｙ軸としている。あるエッジ素の中心点の投影された画像座標を（ｕ0,ｖ0）、エッジ素の直線の画像上での傾きをｘ軸に対する傾きθと表す。傾きθは、エッジ素端点(始点と終点)の撮影画像上での２次元座標を結んだ直線の傾きとして算出する。エッジ素の直線の画像上での法線ベクトルは（ｓｉｎθ, −ｃｏｓθ）となる。また、該エッジ素の対応点の画像座標を（ｕ’, ｖ’）とする。
【００７３】
ここで、点（ｕ , ｖ）を通り、傾きがθである直線の方程式は、
【数１】
2010079452.tif 000003
と表せる。
【００７４】
エッジ素の撮影画像上での画像座標は撮像装置の位置及び姿勢により変化する。また、撮像装置の位置及び姿勢の自由度は６自由度である。ここで撮像装置の位置及び姿勢を表すパラメータをｓで表す。ｓは６次元ベクトルであり、撮像装置の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す３次元ベクトルなどによって表現される。エッジ素中心点の画像座標（ｕ , ｖ）は（ｕ０ , ｖ０）の近傍で１次のテイラー展開によって数３のように近似できる。
【００７５】
【数２】
2010079452.tif 000004
【００７６】
上記数３におけるｕ、ｖの偏微分の導出方法は例えば非特許文献８に開示されるように広く知られているのでここではその詳細は述べない。数２を数１に代入することにより、数３が得られる。
【００７７】
【数３】
2010079452.tif 000005
【００７８】
ここで、数３に示す直線が該エッジ素の対応点の画像座標（ｕ’, ｖ’）を通過するように、撮像装置の位置及び姿勢ｓの補正値Δｓを算出する。r0=u0sinθ-v0cosθ（定数）、d=u’sinθ-v’ cosθ（定数）とすると、
【数４】
2010079452.tif 000006
が得られる。数４はLc個のエッジ素について成り立つため、数５のようなΔｓに対する線形連立方程式が成り立つ。
【００７９】
【数５】
2010079452.tif 000007
【００８０】
ここで数５を数６のように簡潔に表す。
【数６】
2010079452.tif 000008
【００８１】
数６をもとにＧａｕｓｓ−Ｎｅｗｔｏｎ法などによって、行列Ｊの一般化逆行列（ＪT・Ｊ）−1を用いてΔｓが求められる。しかしながら、エッジの検出には誤検出が多いので、次に述べるようなロバスト推定手法を用いる。一般に、誤検出されたエッジに対応するエッジ素では誤差d−rが大きくなる。そのため数５、数６の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。そこで、誤差d−rが大きいエッジ素のデータには小さな重みを与え、誤差d−rが小さいエッジ素のデータには大きな重みを与える。重みは例えば数７Ａに示すようなＴｕｋｅｙの関数により与える。
【００８２】
【数７Ａ】
2010079452.tif 000009
【００８３】
cは定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えば次式（数７Ｂ）で示されるようなＨｕｂｅｒの関数など、誤差d−rが大きいエッジ素には小さな重みを与え、誤差d−rが小さいエッジ素には大きな重みを与える関数であればなんでもよい。
【００８４】
【数７Ｂ】
2010079452.tif 000010
【００８５】
エッジ素OFkに対応する重みをwiとする。ここで数８のように重み行列Ｗを定義する。
【数８】
2010079452.tif 000011
【００８６】
重み行列Ｗは、対角成分以外はすべて0のLc×Lc正方行列であり、対角成分には重みwiが入る。この重み行列Ｗを用いて、数６を数９のように変形する。
【００８７】
【数９】
2010079452.tif 000012
【００８８】
数１０のように数９を解くことにより補正値Δｓを求める。
【００８９】
【数１０】
2010079452.tif 000013
【００９０】
これにより得られたΔｓを用いて、撮像装置の位置及び姿勢を更新する。次に、撮像装置の位置及び姿勢の反復演算が収束しているかどうかを判定する。補正値Δｓが十分に小さかったり、誤差r−dの総和が十分小さい、誤差r−dの総和が変化しないといった場合には、撮像装置の位置及び姿勢の計算が収束したと判定する。収束していないと判定された場合には、更新された撮像装置の位置及び姿勢を用いて再度線分の傾きθ、r0、d及びｕ、ｖの偏微分を計算し直し、数１０より再度補正値Δｓを求め直す。なお、ここでは非線形最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いた。しかしながら、非線形最適化手法はこれに限るものではなく、Ｎｅｗｔｏｎ−Ｒａｐｈｓｏｎ法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。以上、ステップＳ１１００における撮像装置の位置姿勢算出方法について説明した。
【００９１】
（ステップＳ１１１０）
ステップＳ１１１０において、位置姿勢算出部１７０は、位置姿勢算出を終了する入力がなされたかどうかを判定し、入力された場合には当該処理を終了し、入力されなかった場合には処理をステップＳ１１２０に進める。
【００９２】
（ステップＳ１１２０）
ステップＳ１１２０において、位置姿勢算出部１７０は、位置姿勢の算出結果と位置姿勢計測の対象にした撮影画像を参照画像保存部１３０に参照画像として保存する。その後、処理はステップＳ１０２０に戻り、新たな画像の取得と、再度の位置姿勢算出が行われる。
【００９３】
以上述べたように、第１実施形態では、
・随時撮影画像上で撮像される観察対象物体から色に基づく画像特徴を抽出し、
・これと同時に、観察対象物体の３次元モデルから抽出した幾何情報のうち撮影画像から検出されるものを選別し、
・観察対象物体の色に基づく画像特徴と選別された幾何情報を統合して位置姿勢計測に利用する。このため、第１実施形態によれば、３次元モデルから得られる特徴と撮影画像から得られる特徴が大きく異なる場合や、撮像の途中に光源変化があった場合にも、観察対象物体と撮像装置との相対位置姿勢を安定かつ高精度に算出することができる。
【００９４】
［変形例１−１］
上述の第１実施形態では、ステップＳ１０６０の色特徴抽出処理において、参照画像中に撮像された観察対象物体のデプスバッファを算出するために、参照画像保存部１３０に保存されている参照画像の位置姿勢を利用して３次元モデルデータを描画した。しかし、ステップＳ１０３０の幾何特徴抽出処理で利用する概略位置姿勢として、前フレームの位置姿勢を利用する場合には、参照画像部に保存された位置姿勢と、ステップＳ１０３０で利用する概略位置姿勢は同じとなる。
【００９５】
したがって、ステップＳ１０６０の色特徴抽出処理では、ステップＳ１０３０で算出したデプスバッファをそのまま利用することができ、３次元モデルデータの描画を行わなくてすむ。これにより、色特徴抽出処理で３次元モデルデータの描画処理を行う必要が無くなり、計算コストを減少させることが可能である。
【００９６】
［変形例１−２］
上述の第１実施形態において、ステップＳ１０６０とステップＳ１０７０の色特徴抽出処理及び色特徴選別処理を行わず、幾何特徴のみを利用して位置姿勢推定を行うようにしても良い。この場合、色特徴抽出処理を省略し、幾何情報のみを利用した位置姿勢推定が行われることになる。即ち、位置姿勢算出部１７０は、幾何特徴抽出部１４０により選別された幾何特徴を、入力された画像中の観察対象物体１０の画像に対応付けることにより、撮像装置１００の観察対象物体１０に対する位置姿勢を算出する。この構成によれば、テクスチャのない一色の物体のような、色に基づく特徴が検出されない観察対象物体に関しては、位置姿勢推定に利用する情報を損失することなく、色特徴抽出処理を省略する分の計算コストを減少させることが可能である。また、色特徴選別処理（ステップＳ１０７０）を行わず、ステップＳ１０６０で抽出された色特徴は全て用いるようにしてもよい。但し、この場合、現フレーム中の画像における同一の画像特徴が、色特徴と幾何特徴の両方に関連して検出される可能性が大きくなるため、処理効率が下がる可能性がある。
【００９７】
［第２実施形態］
第１実施形態では、位置姿勢推定後、参照画像保存部１３０に撮影画像を毎フレーム保存し、過去フレームの撮影画像として常に直前のフレームの撮影画像を利用していた。それに対して、第２実施形態では、参照画像保存部に保存した撮影画像を上書きせず、複数の撮影画像を保存しておき、複数の参照画像を利用して位置姿勢推定を行う方法について述べる。
【００９８】
図１６は、第２実施形態における位置姿勢計測装置２の構成を示している。同図に示したように、位置姿勢計測装置２は、画像取得部２１０、モデル保存部２２０、参照画像保存部２３０、参照画像選択部２４０、幾何特徴抽出部２５０、色特徴抽出部２６０、画像特徴検出部２７０、位置姿勢算出部２８０を有する。画像取得部２１０は撮像装置２００に接続されている。撮像装置２００は、観察対象物体３０を撮影する。位置姿勢計測装置２は、３次元モデルデータ４０に記述された観察対象物体３０と撮像装置２００との相対的な位置姿勢を、撮像装置２００により得られた撮像画像の情報を用いて求める。なお、第２実施形態において位置姿勢計測装置２が適用できる条件として、撮像装置２００が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置２を構成する各部について説明する。
【００９９】
画像取得部２１０、モデル保存部２２０、画像特徴検出部２７０、位置姿勢算出部２８０は、それぞれ第１実施形態における画像取得部１１０、モデル保存部１２０、画像特徴検出部１６０、位置姿勢算出部１７０と同様の処理を行う。
【０１００】
参照画像保存部２３０は、過去フレームにおいて撮像装置２００によって観察対象物体３０が撮像された撮影画像と、撮像されたときの観察対象物体と撮像装置との相対位置姿勢を保存する。同時に、参照画像保存部２３０は、保存する撮影画像から抽出した色特徴（本実施形態では色の境界に基づくエッジ）も基準座標系における３次元座標とともに保存しておく。第２実施形態では、あらかじめ定められた保存条件を満足する過去フレームの撮影画像が参照画像保存部２３０に保存される。保存条件の例としては、
・位置姿勢の推定精度が一定以上高いこと、
・保存してある参照画像から十分離れていること、即ち、保存してある参照画像に対応する位置姿勢と現フレームに関連する位置姿勢の差が所定値より大きいこと、
・以前に参照画像を保存したときから一定フレーム（所定の時間）を経過していること、
等が挙げられる。
【０１０１】
参照画像選択部２４０は、参照画像保存部２３０に保存された複数の参照画像から、以後のステップ（幾何特徴抽出処理）で利用する参照画像を選択する。第２実施形態では、撮像装置２００に設定されている現在の位置姿勢に最も位置姿勢が近い参照画像が選択され、以後のステップの処理に利用される。
【０１０２】
幾何特徴抽出部２５０は、モデル保存部２２０に保存された観察対象物体３０の３次元モデルデータ４０と概略位置姿勢から、観察対象物体の幾何的な情報に基づく特徴を抽出する。また、幾何特徴抽出部２５０は、参照画像選択部２４０で選択された参照画像を利用して、３次元モデルデータ４０から抽出した幾何特徴の中で、撮像装置２００により撮像された撮影画像中から検出が期待できる特徴のみを選別する。
【０１０３】
色特徴抽出部２６０は、位置姿勢が推定された撮影画像を利用して、撮影画像中に撮像される観察対象物体の色に基づく特徴を抽出する。また、色特徴抽出部２６０は、推定された位置姿勢とモデル保存部２２０に保存された３次元モデルデータ４０に基づいて、当該位置姿勢から観察される観察対象物体の幾何特徴を抽出して、比較することで、色に基づくエッジのみを抽出する。同時に、色特徴抽出部２６０は、色に基づくエッジの基準座標における３次元座標を求める。求めた色特徴は、参照画像保存部２３０にて保存される。
【０１０４】
次に、第２実施形態における位置姿勢計測方法の処理手順について説明する。図１７は、第２実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【０１０５】
（ステップＳ２０１０）
ステップＳ２０１０では初期化を行う。ステップＳ２０１０の処理内容は基本的に第１実施形態におけるステップＳ１０１０と同様であるので。
【０１０６】
（ステップＳ２０２０）。
【０１０７】
ステップＳ２０２０の処理は第１実施形態におけるステップＳ１０２０の処理と同様である。
【０１０８】
（ステップＳ２０３０）
ステップＳ２０３０において、幾何特徴抽出部２５０は、観察対象物体３０の３次元モデルデータ４０に基づいて、幾何的情報に基づく特徴を検出する。ステップＳ２０３０の処理内容は、基本的に第１実施形態におけるステップＳ１０３０の処理と同様である。
【０１０９】
（ステップＳ２０４０）
参照画像保存部に過去フレームの撮影画像が保存されている場合は、処理はステップＳ２０４０からステップＳ２０５０に進む。一方、保存されていない場合は、処理は、ステップＳ２０５０以降の過去フレームの撮影画像を利用するステップをスキップして、ステップＳ２０７０へ進む。
【０１１０】
（ステップＳ２０５０）
ステップＳ２０５０において、参照画像選択部２４０は、参照画像保存部２３０に保存されている複数の参照画像から、適切な参照画像を選択する。本実施形態では、参照画像の選択に、現在の位置姿勢と参照画像の位置姿勢の差を利用する。なお、本実施形態では、現在の位置姿勢として、直前フレームの撮影画像から推定した位置姿勢を用いるものとする。参照画像選択部２４０は、現在の位置姿勢と参照画像保存部２３０に保存してある各参照画像の位置姿勢とを比較して、現在の位置姿勢に最も近い参照画像を選択する。ステップＳ２０５０において選択された参照画像は、以後のステップにおいて参照画像として利用される。
【０１１１】
（ステップＳ２０６０）
次に、ステップＳ２０６０において、幾何特徴抽出部２５０は、ステップＳ２０３０で抽出した幾何特徴の選別を行う。ステップＳ２０６０の処理内容は、基本的に第１実施形態におけるステップＳ１０５０の処理と同様である。但し、参照画像としてステップＳ２０５０で選択した参照画像を利用する点がステップＳ１０５０と異なる。
【０１１２】
（ステップＳ２０７０）
次にステップＳ２０７０において、画像特徴検出部２７０は、ステップＳ２０５０で生成した幾何特徴モデルと、ステップＳ２０５０で選択した参照画像に対応する色特徴モデルを利用して、幾何特徴モデルと色特徴モデルを統合した物体特徴モデルを生成する。ステップＳ２０７０の処理内容は、基本的に第１実施形態におけるステップＳ１０８０の処理と同様である。ステップＳ１０８０と異なる点は、ステップＳ２０７０で利用する色特徴モデルはステップＳ２０５０で選択した参照画像に対応する色特徴モデルを利用する点である。なお、選択した参照画像に対応する色特徴モデルは、参照画像が保存される過程のステップＳ２１３０で生成され、参照画像保存部２３０に保存されている（後述）。
【０１１３】
（ステップＳ２０８０）
ステップＳ２０８０において、画像特徴検出部２７０は、ステップＳ２０８０で求めた物体特徴モデルのエッジ素に対応する画像特長を、撮像装置２００によって撮像された現フレームの撮影画像から検出する。ステップＳ２０８０の処理内容は、基本的に第１実施形態におけるステップＳ１０９０の処理と同様である。
【０１１４】
（ステップＳ２０９０）
ステップＳ２０９０において、位置姿勢算出部２８０は、ステップＳ２０７０で生成した物体特徴モデルと、ステップＳ２０８０で対応付けた画像特徴の位置情報から、撮像装置２００の位置姿勢を算出する。ステップＳ２０９０の処理内容は、基本的に第１実施形態におけるステップＳ１１００の処理と同様である。
【０１１５】
（ステップＳ２１００）
ステップＳ２１００において、位置姿勢計測装置２は、位置姿勢算出を終了する指示の入力がなされたかどうかを判定し、終了する指示が入力された場合には本処理を終了する。一方、終了する指示が入力されなかった場合には、処理をステップＳ２１１０に進める。
【０１１６】
（ステップＳ２１１０）
ステップＳ２１１０において、参照画像保存部２３０は、位置姿勢推定した撮影画像を参照画像として保存するかを判定する。保存する場合は、処理をステップＳ２１２０に進める。また、保存しない場合は、処理をステップＳ２０２０に戻し、新たな画像を取得して、上述の位置姿勢算出を行う。第２実施形態では、参照画像として保存する条件は、本例では、保存しようとする撮影画像が参照画像保存部２３０に保存されている参照画像の位置姿勢から一定以上離れており、かつ、最近に保存した参照フレームから一定フレーム経過していることとする。
【０１１７】
（ステップＳ２１２０）
ステップＳ２１２０において、色特徴抽出部２６０は、ステップＳ２０９０で推定された位置姿勢を利用して、ステップＳ２１１０で保存すると判定された撮影画像から観察対象物体の色情報に基づく特徴を検出し、色特徴モデルを生成する。なお、色特徴モデルの生成方法は、基本的に第１実施形態におけるステップＳ１０６０の処理と同様である。
【０１１８】
（ステップＳ２１３０）
次に、ステップＳ２１３０において、色特徴抽出部２６０は、ステップＳ２１２０で抽出した色特徴モデルの選別を行う。ステップＳ２１３０の処理内容は、基本的に第１実施形態におけるステップＳ１０７０の処理と同様である。
【０１１９】
（ステップＳ２１４０）
次に、ステップＳ２１４０において、参照画像保存部２３０は、ステップＳ２１２０で生成した色特徴モデルとステップＳ２０９０で算出した観察対象物体の位置姿勢と、位置姿勢算出の対象とした撮影画像とを保存する。その後、ステップＳ２０２０に戻り、新たな画像を取得して、再度位置姿勢算出を行う。
【０１２０】
以上述べたように、第２実施形態では、位置姿勢の異なる複数の参照画像を保持しつづけることで、毎フレーム撮影画像を参照画像として保存する必要を無くすことができる。また、該参照画像から抽出した色特徴モデルを参照画像保存部２３０に保持することにより、毎フレームについて色特徴を算出する必要がなくなる。さらに、複数の参照画像から、現在の観察対象物体の位置姿勢に最も近い位置姿勢の参照画像とその色特徴モデルを利用することにより、計算コストを抑えながら、幾何特徴と色特徴を統合利用した位置姿勢推定が可能である。
【０１２１】
［変形例２−１］
上述の第２実施形態では、参照画像として保存する撮影画像は、前回保存したときよりも一定フレーム過ぎており、かつ、保存してある撮影画像の位置姿勢と現在の位置姿勢が一定以上離れている場合に、無条件で保存していた。しかしながら、保存する位置姿勢の推定精度が低いような場合、位置姿勢の推定精度の低い参照画像から色特徴モデルを生成することになる。そのため、不正確な特徴モデルを利用することによる位置姿勢推定の精度低下が発生する。そこで、参照画像として保存するときに、位置姿勢の推定精度も算出して、推定精度が一定以上の場合のみ参照画像として保存するという制限を加える。これにより、位置姿勢の推定が不安定な場合でも、一定以上推定精度が高い位置姿勢とそのときの撮影画像のみが参照画像として保存されるため、安定した位置姿勢推定が可能となる。推定精度算出方法としては、例えば、非特許文献６で開示されているような位置姿勢推定の破綻検出手法を用いればよい。
【０１２２】
［変形例２−２］
上述の第２実施形態では、一旦、参照画像として保存した撮影画像は、本位置姿勢計測装置が動作している間ずっと保存されていた。しかし、撮像の途中で、光源環境が変化して、観察対象物体の色合いが変化することも考えられる。
【０１２３】
そこで、ステップＳ２１４０において、撮影画像を参照画像として保存するときに、時刻情報を付加しておき、一定時間が過ぎた参照画像は削除するようにしてもよい。即ち、時系列的に古い画像特徴を判別して、時系列的に画像特徴を随時更新していくようにしてもよい。これにより、時系列的に参照画像が更新されていき、撮影環境に変化が生じた場合にも柔軟に対応することが可能になる。また、単純に一定時間過ぎた参照画像を削除するのではなく、一定時間過ぎた参照画像は上書きフラグをＯＮにしておき、撮影画像を参照画像として保存する際に、当該参照画像で上書きフラグがＯＮの参照画像を上書きするようにしてもよい。更に、撮影画像を参照画像として保存する際に、保存する撮影画像の位置姿勢と上書きフラグがＯＮになった参照画像の位置姿勢とを比較して、両者が十分に近い場合に、その参照画像に上書きするようにしてもよい。このようにすれば、一定時間位置姿勢が変化しなかったときに、有用な参照画像が削除されることを防ぐことができる。また、参照画像として保持する撮影画像の枚数を一定数に設定しておき、保存された時刻情報が一番古い参照画像から上書きすることにしてもよい。
【０１２４】
［変形例２−３］
上述の第２実施形態では、参照画像として保存する処理は、位置姿勢の算出が終わったあとステップＳ２１１０の参照画像保存判定後に行っていた。しかし、保存処理は、非同期的に行っても問題のない処理である。そこで、参照画像の保存・色特徴抽出処理、すなわち、ステップＳ２１２０〜Ｓ２１４０の処理を、別スレッドで行うようにしてもよい。これにより、位置姿勢の算出と参照画像の保存処理を独立して実行することが可能となり、参照画像の保存処理が実行された場合でも、位置姿勢推定のフレームレートへの影響を低減することができる。また、参照画像から特徴抽出する処理に計算コストがかかる処理を入れ込むことも可能である。例えば、参照画像からの色特徴抽出処理において、複数の参照画像から抽出した色特徴から画像間対応をとり、参照画像のみから色特徴の３次元再構成を行えば、物体の３次元モデルからは得られない特徴も利用して、位置姿勢推定することが可能となる。
【０１２５】
［変形例２−４］
上述の第２実施形態では、参照画像保存部２３０に保存された複数の参照画像の中で、一つだけを選択して、色特徴モデルや幾何特徴の判別に利用していた。しかしながら、観察対象物体の表面属性によっては、鏡面反射などの視点位置によって見た目が変わる情報も存在する。視点位置によって変わる特徴を色特徴モデルとして抽出してしまうと、色特徴モデルを利用した位置姿勢推定が不安定になる。また、観察物体に対して、オクルージョンが発生した場合にも同様の問題が発生する。
【０１２６】
そこで、ステップＳ２０５０において、現在の位置姿勢に近い参照画像を複数選択しておき、複数の色特徴モデルを比較して、これら複数の色特徴モデルの中で安定的に検出されている色特徴を利用するようにしてもよい。なお、「安定的に検出される色特徴」とは、例えば、複数の（２つ以上の）参照画像から検出され得る色特徴である。このようにすれば、鏡面反射やオクルージョンによる、偽の色特徴の影響を除去しながら位置姿勢推定することが可能となる。また、ステップＳ２０６０において、複数の参照画像から幾何特徴選別処理を行うことで、複数の参照画像から、幾何特徴の検出判定を行い、複数の画像において、安定して検出できる幾何特徴を利用するように構成することができる。なお、「安定して検出できる幾何特徴」とは、例えば、複数の（２つ以上の）参照画像から対応する色特徴が検出され得る幾何特徴である。これにより、鏡面反射やオクルージョンが生じた場合にも安定した位置姿勢推定を行うことが可能になる。
【０１２７】
［第３実施形態］
第１・第２実施形態では、位置姿勢推定後、参照画像保存部１３０，２４０に撮影画像のフレーム画像を参照画像として保存し、参照画像から対象物体の色に基づく特徴を抽出していた。それに対し、第３実施形態では、撮影画像から３次元モデルの表面色の情報を推定し、３次元モデルを描画した結果として得られる描画画像から対象物体の色に基づく特徴を抽出し、位置姿勢推定を行う方法について述べる。
【０１２８】
図１８は、第３実施形態における位置姿勢計測装置３の構成を示している。同図に示したように、位置姿勢計測装置３は、画像取得部３１０、モデル保存部３２０、幾何特徴抽出部３３０、色特徴抽出部３４０、画像特徴検出部３５０、位置姿勢算出部３６０、物体表面色推定部３７０を有する。画像取得部３１０は撮像装置３００に接続されている。撮像装置３００は、観察対象物体３０を撮影する。位置姿勢計測装置３は、３次元モデルデータ６０に記述された観察対象物体と撮像装置３００との相対的な位置姿勢を撮像画像の情報を用いて求める。なお、第３実施形態において位置姿勢計測装置３が適用できる条件として、撮像装置３００が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置３を構成する各部について説明する。
【０１２９】
画像取得部３１０、画像特徴検出部３５０、位置姿勢算出部３６０は、第１実施形態における画像取得部１１０、画像特徴検出部１６０、位置姿勢算出部１７０と同様の処理を行う。
【０１３０】
モデル保存部３２０は、位置姿勢計測の基準となる観察対象物体５０の３次元モデルデータ６０を記憶する。３次元モデルデータ６０は、観察対象物体５０の表面形状を表す３次元の幾何的な情報、および物体表面の色情報を有するモデルであり、頂点情報及び各頂点を結んで構成される面の情報と表面の色情報を表すテクスチャ画像によって定義される。第３実施形態では、頂点情報及び各頂点を結んで構成される面の情報を含むポリゴンモデルを３次元モデルデータ６０として利用する。また、同時に３次元モデルデータ６０は、観察対象物体の表面の色情報をテクスチャ画像として保持する。テクスチャ画像上での２次元座標と３次元モデル中の頂点との相対位置関係は、テクスチャ変換行列としてあらかじめ求めておく。テクスチャ画像は、物体表面色推定部３７０において撮影画像からオンライン、すなわち実行時点で推定するため、最初期フレームでは、観察対象物体の表面色は未知である。そこで、テクスチャ画像は、あらかじめ一定色に初期化しておく。
【０１３１】
幾何特徴抽出部３３０は、モデル保存部３２０に保存された観察対象物体５０の３次元モデルデータ６０と概略位置姿勢から、３次元モデルデータを描画する。そして、描画結果として得られた画像平面上での３次元モデルデータの輝度値と、そのときの奥行き値を保存する。そして、幾何特徴抽出部３３０は、奥行き値から観察対象物体５０の幾何的な情報に基づく特徴を抽出する。
【０１３２】
色特徴抽出部３４０は、上述の画像平面上での３次元モデルデータ６０の輝度値から、観察対象物体の色に基づく特徴（エッジ）を抽出する。同時に、色に基づく特徴（エッジ）の基準座標における３次元座標を求める。
【０１３３】
物体表面色推定部３７０は、位置姿勢が推定された撮影画像を利用して観察対象物体５０の表面色を推定する。本実施形態では、撮影画像上に撮像される観察対象物体５０の輝度値を、テクスチャ画像に対してマッピングすることで、観察対象物体の表面色の推定を行う。
【０１３４】
次に、第３実施形態における位置姿勢計測方法の処理手順について説明する。図１９は、第３実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【０１３５】
（ステップＳ３０１０）
ステップＳ３０１０では初期化を行う。ステップＳ３０１０の処理内容は基本的に第１実施形態におけるステップＳ１０１０と同様である。
【０１３６】
（ステップＳ３０２０）。
【０１３７】
ステップＳ３０２０の処理は第１実施形態におけるステップＳ１０２０の処理と同様である。
【０１３８】
（ステップＳ３０３０）
ステップＳ３０３０において、幾何特徴抽出部３３０は、３次元モデルデータ６０を用いて観察対象物体５０を描画し、幾何的情報に基づく特徴を検出する。ステップＳ３０３０の処理内容は基本的に第１実施形態におけるステップＳ１０１０と同様である。ステップＳ１０１０と異なる点は、３次元モデルを描画する際にモデル保存部３２０に保持しているテクスチャ画像によるテクスチャマッピングを行い、描画結果をカラーバッファとして保存する点である。表面色が推定された３次元モデルを描画することで、実際の撮影画像に撮像される観察対象物体の見た目に近い、３次元モデルの描画結果が得られる。この描画結果として得られる画像平面上における３次元モデルデータの輝度値をカラーバッファとして保存し、以後の処理に利用する。
【０１３９】
（ステップＳ３０４０）
ステップＳ３０４０では、モデル保存部３２０に保持しているテクスチャ画像が更新されている場合は、ステップＳ３０５０に処理を進める。一方、最初期フレームなどで、テクスチャ画像が更新されていない場合は、ステップＳ３０５０〜Ｓ３０７０をスキップして、ステップＳ３０９０へ処理を進める。
【０１４０】
（ステップＳ３０５０）
ステップＳ３０５０において、幾何特徴抽出部３３０は、ステップＳ３０３０で抽出した幾何特徴の選別を行う。ステップＳ３０５０の処理内容は、基本的に第１実施形態におけるステップＳ１０５０の処理と同様である。ステップＳ１０５０と異なる点は、参照画像の代わりに、ステップS３０３０で保存したカラーバッファを利用する点である。
【０１４１】
（ステップＳ３０６０）
ステップＳ３０６０において、色特徴抽出部３４０は、ステップＳ３０３０で保存したカラーバッファから、観察対象物体の色情報に基づく特徴を検出し、色特徴モデルを生成する。色特徴モデルの生成方法は、基本的に第１実施形態におけるステップＳ１０６０の処理と同様である。ステップＳ１０６０と異なる点は、参照画像の代わりに、ステップＳ３０３０で保存したカラーバッファを利用する点である。また、色特徴の逆投影に利用する位置姿勢には、ステップ３０３０において３次元モデルデータの描画に利用した位置姿勢と同一のものを利用する。
【０１４２】
（ステップＳ３０７０）
次に、ステップＳ３０７０において、色特徴抽出部３４０は、ステップＳ３０６０で抽出した色特徴モデルの選別を行う。ステップＳ３０７０の処理内容は、基本的に第１実施形態におけるステップＳ１０７０の処理と同様である。ステップＳ１０６０と異なる点は、参照画像の代わりに、ステップＳ３０３０で保存したカラーバッファを利用する点である。また、色特徴の逆投影に利用する位置姿勢には、ステップ３０３０において３次元モデルデータの描画に利用した位置姿勢と同一のものを利用する。
【０１４３】
（ステップＳ３０８０）
次にステップＳ３０８０において、画像特徴検出部３５０は、ステップＳ３０３０で生成した幾何特徴モデルと、ステップS３０６０で生成した色特徴モデルを統合した物体特徴モデルを生成する。ステップＳ３０８０の処理内容は、基本的に第１実施形態におけるステップＳ１０８０の処理と同様である。
【０１４４】
（ステップＳ３０９０）
ステップ３０９０において、画像特徴検出部３５０は、ステップＳ３０８０で求めた物体特徴モデルのエッジ素に対応する画像特長を、撮像装置３００によって撮像された現フレーム撮影画像より検出する。ステップＳ３０９０の処理内容は、基本的に第１実施形態におけるステップＳ１０９０の処理と同様である。
【０１４５】
（ステップＳ３１００）
ステップＳ３１００において、位置姿勢算出部３６０は、ステップＳ３０８０で生成した物体特徴モデルと、ステップＳ３０９０で対応付けた画像特徴の位置情報から、撮像装置３００の位置姿勢を算出する。ステップＳ３１００の処理内容は、基本的に第１実施形態におけるステップＳ１１００の処理と同様である。
【０１４６】
（ステップＳ３１１０）
ステップＳ３１１０において、位置姿勢計測装置３は、位置姿勢算出を終了する入力がなされたかどうかを判定する。位置姿勢算出を終了する入力がなされた場合には当該終了し、位置姿勢算出を終了する入力がなされなかった場合には処理をステップＳ３１２０に進める。
【０１４７】
（ステップＳ３１２０）
ステップＳ３１２０において、物体表面色推定部３７０は、位置姿勢推定した撮影画像から、観察対象物体の表面色を推定する。まず、物体表面色推定部３７０は、ステップＳ３０３０において基準座標系におけるエッジ素の３次元座標を求めた手順と同様の処理により、撮影画像上の２次元座標に対応する基準座標系における３次元座標を求める。次に、求めた３次元座標に対応するテクスチャ画像上の２次元座標を、モデル保存部３２０に保存されているテクスチャ変換行列を利用することで求める。以上の手順により、撮影画像上の２次元座標とテクスチャ画像上の２次元座標の関係を求め、撮影画像上に撮像される観察対象物体の輝度値をテクスチャ画像上にマッピングすることで、テクスチャ画像を更新し、観察対象物体の表面色を推定する。このとき、撮影画像上の輝度値をそのままテクスチャ画像上の輝度値として設定せずに、学習係数に応じて徐々にテクスチャ画像の輝度値を更新する。これにより、位置姿勢推定精度が低い撮影画像が混入した場合や、オクルージョンが発生した場合の影響を低減し、確からしい表面色のみを更新することができる。学習係数は、位置姿勢推定精度の高さと、表面色がどのくらい急激に変化しうるかにより適切な値を設定する。以上の処理により表面色を更新した後は、ステップＳ３０２０に戻り、新たな画像を取得して、再度位置姿勢算出を行う。
【０１４８】
以上述べたように、第３実施形態では、位置姿勢推定した撮影画像から毎フレームのテクスチャ画像を更新し、物体の表面色を直接推定する。これにより、参照画像として撮影画像を保存する必要をなくした。以上のように、第３実施形態では、物体の表面色が推定された３次元モデルデータを描画し、実際に撮影画像に撮像される観察対象物体の見た目に近い描画結果から観察対象物体の色に基づく特徴を抽出する。これにより、観察対象物体の幾何特徴と色特徴とを統合利用した位置姿勢推定が可能となる。
【０１４９】
［他の実施形態］
第１〜第３実施形態では、参照画像から抽出する色に基づく特徴として、エッジを用いたが、これに限るものでない。例えば、Harris検出器や、非特許文献９に示されるSIFT検出器などによって検出される点特徴を利用しても良い。このとき、点特徴の記述子としては、点特徴周辺の輝度分布を利用しても、非特許文献９に示されるSIFT記述子を利用しても良く、点特徴の検出子と記述子の選択に特に制限はない。
【０１５０】
また、色に基づく特徴として点特徴を利用する場合、色特徴モデルとしては、参照画像に撮像される観察対象物体から検出される点特徴の、基準座標系における３次元位置と点特徴が検出された画像上での画像座標、その画像へのポインタが保存される。そして、色特徴モデル中の点特徴と、現フレームの撮影画像から検出した点特徴とを対応付けることで、位置姿勢が推定される。点特徴は、細かい模様のある物体など、特徴としてエッジを利用した場合には誤対応が頻出するような物体に対して、特に検出されやすい。そのため、対象に細かい模様があるような場合には、エッジを特徴として利用するよりも、安定した位置姿勢推定が行うことが可能である。
【０１５１】
以上、実施形態を詳述したが、本発明は、例えば、システム、装置、方法、プログラムもしくは記憶媒体等としての実施態様をとることが可能である。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
【０１５２】
尚、本発明は、ソフトウェアのプログラムをシステム或いは装置に直接或いは遠隔から供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムコードを読み出して実行することによって前述した実施形態の機能が達成される場合を含む。この場合、供給されるプログラムは実施形態で図に示したフローチャートに対応したコンピュータプログラムである。
【０１５３】
従って、本発明の機能処理をコンピュータで実現するために、該コンピュータにインストールされるプログラムコード自体も本発明を実現するものである。つまり、本発明は、本発明の機能処理を実現するためのコンピュータプログラム自体も含まれる。
【０１５４】
その場合、プログラムの機能を有していれば、オブジェクトコード、インタプリタにより実行されるプログラム、ＯＳに供給するスクリプトデータ等の形態であっても良い。
【０１５５】
コンピュータプログラムを供給するためのコンピュータ読み取り可能な記憶媒体としては以下が挙げられる。例えば、フロッピー（登録商標）ディスク、ハードディスク、光ディスク、光磁気ディスク、ＭＯ、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−ＲＷ、磁気テープ、不揮発性のメモリカード、ＲＯＭ、ＤＶＤ（ＤＶＤ−ＲＯＭ，ＤＶＤ−Ｒ）などである。
【０１５６】
その他、プログラムの供給方法としては、クライアントコンピュータのブラウザを用いてインターネットのホームページに接続し、該ホームページから本発明のコンピュータプログラムをハードディスク等の記録媒体にダウンロードすることが挙げられる。この場合、ダウンロードされるプログラムは、圧縮され自動インストール機能を含むファイルであってもよい。また、本発明のプログラムを構成するプログラムコードを複数のファイルに分割し、それぞれのファイルを異なるホームページからダウンロードすることによっても実現可能である。つまり、本発明の機能処理をコンピュータで実現するためのプログラムファイルを複数のユーザに対してダウンロードさせるＷＷＷサーバも、本発明に含まれるものである。
【０１５７】
また、本発明のプログラムを暗号化してＣＤ−ＲＯＭ等の記憶媒体に格納してユーザに配布するという形態をとることもできる。この場合、所定の条件をクリアしたユーザに、インターネットを介してホームページから暗号を解く鍵情報をダウンロードさせ、その鍵情報を使用して暗号化されたプログラムを実行し、プログラムをコンピュータにインストールさせるようにもできる。
【０１５８】
また、コンピュータが、読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているＯＳなどとの協働で実施形態の機能が実現されてもよい。この場合、ＯＳなどが、実際の処理の一部または全部を行ない、その処理によって前述した実施形態の機能が実現される。
【０１５９】
さらに、記録媒体から読み出されたプログラムが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれて前述の実施形態の機能の一部或いは全てが実現されてもよい。この場合、機能拡張ボードや機能拡張ユニットにプログラムが書き込まれた後、そのプログラムの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行なう。
【０１６０】
以上のように、上記各実施形態によれば、位置姿勢が既知である参照画像（過去フレームにおいて観察対象物体が撮像された撮影画像やテクスチャが設定されたモデル画像）を利用して、使用すべき画像特徴が決定される。そして、このような画像特徴を用いて現在のフレームに関する撮像装置の位置姿勢を算出するので、位置姿勢推定の安定性が向上する。即ち、各実施形態によれば、観察対象物体の３次元モデルデータから抽出した３次元線分モデルが、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定することができる。また、３次元モデルから抽出される幾何的特徴に加えて、観察対象物体の表面の色に基づく特徴を利用することで、３次元モデルデータの中に含まれていない情報も利用して位置姿勢推定することが可能である。
【図面の簡単な説明】
【０１６１】
【図１】第１実施形態における位置姿勢計測装置１の構成を示す図である。
【図２】第１実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【図３】第１実施形態における対象幾何特徴抽出の詳細な処理手順を示すフローチャートである。
【図４】３次元モデルデータの描画を示す図である。
【図５】デプスバッファから抽出したエッジを撮影画像に重ね合わせた図である。
【図６】エッジ素の抽出を示す図である。
【図７】本発明の一実施形態における対象幾何特徴選別の詳細な処理手順を示すフローチャートである。
【図８】幾何特徴モデルのエッジ素の参照画像への投影を示す図である。
【図９】幾何特徴モデルのエッジ素近傍のエッジ探索を示す図である。
【図１０】第１実施形態における対象色特徴抽出の詳細な処理手順を示すフローチャートである。
【図１１】第１実施形態における対象色特徴選別の詳細な処理手順を示すフローチャートである。
【図１２】第１実施形態における幾何特徴モデルと色特徴モデルとの統合の詳細な処理手順を示すフローチャートである。
【図１３】第１実施形態における現フレームの撮影画像からの画像特徴抽出の詳細な処理手順を示すフローチャートである。
【図１４】物体特徴モデルのエッジ素を現フレームの撮影画像に重ね合わせた図である
【図１５】エッジの情報を利用して撮像装置の位置及び姿勢を算出する方法を説明する図である。
【図１６】第２実施形態における位置姿勢計測装置２の構成を示す図である
【図１７】第２実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【図１８】第３実施形態における位置姿勢計測装置３の構成を示す図である
【図１９】第３実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【図１】
2010079452.tif 000014
【図２】
2010079452.tif 000015
【図３】
2010079452.tif 000016
【図４】
2010079452.tif 000017
【図５】
2010079452.tif 000018
【図６】
2010079452.tif 000019
【図７】
2010079452.tif 000020
【図８】
2010079452.tif 000021
【図９】
2010079452.tif 000022
【図１０】
2010079452.tif 000023
【図１１】
2010079452.tif 000024
【図１２】
2010079452.tif 000025
【図１３】
2010079452.tif 000026
【図１４】
2010079452.tif 000027
【図１５】
2010079452.tif 000028
【図１６】
2010079452.tif 000029
【図１７】
2010079452.tif 000030
【図１８】
2010079452.tif 000031
【図１９】
2010079452.tif 000032