位置姿勢計測装置及び方法
【課題】観察対象物体の3次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別することにより、位置姿勢検出処理の安定化、効率化を図る
【解決手段】位置姿勢算出装置は、観察対象物体の表面形状を表す3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する。そして、観察対象物体に対する撮像装置の位置姿勢が算出済みとなっている参照画像において複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を、抽出された複数の幾何特徴より選別する。位置姿勢算出装置は、こうして選別された幾何特徴を入力画像における観察対象物体の画像に対応付けることにより、撮像装置の観察対象物体に対する位置姿勢を算出する。
【解決手段】位置姿勢算出装置は、観察対象物体の表面形状を表す3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する。そして、観察対象物体に対する撮像装置の位置姿勢が算出済みとなっている参照画像において複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を、抽出された複数の幾何特徴より選別する。位置姿勢算出装置は、こうして選別された幾何特徴を入力画像における観察対象物体の画像に対応付けることにより、撮像装置の観察対象物体に対する位置姿勢を算出する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、観察対象物体の表面形状を表す3次元モデルデータと撮像装置が撮像した観察対象物体の撮像画像を利用して、撮像装置と観察対象物体との相対的な位置姿勢を算出する位置姿勢計測装置及び方法に関するものである。
【背景技術】
【0002】
従来より、現実空間を撮像するカメラなどの撮像装置を用いて、観察対象物体と当該観察対象物体を撮像する撮像装置との相対的な位置姿勢を計測する技術が提案されている。この種の位置姿勢計測技術は、現実空間と仮想空間を融合表示する複合現実感システムや、ロボットの位置姿勢計測において、非常に有用な技術である。
【0003】
従来より、撮像装置の位置姿勢を計測する方法として、3次元位置が既知の指標を用いる方法が提案されている。この方法では、撮像装置のおおよその位置姿勢を用いて各指標の3次元位置を撮像面へ投影した投影位置と、撮像画像から検出される指標の撮像画像上の位置との距離を誤差として、この誤差を小さくする目的関数を最適化するように位置姿勢を推定する。指標としては、予め検出が容易な幾何学的特徴を有するもの、または色相が特異な特徴を有するものを利用することが多い。
【0004】
また、画像中でのエッジと観察対象の3次元モデルとの対応をもとに撮像装置の位置及び姿勢を求める方法も提案されている。ここで、撮影画像上で撮像される観察対象物体により観察される輝度が不連続に変化する領域をエッジと呼称する。エッジはスケールや観察方向に対して不変であるため、エッジを利用した位置合わせは精度が高いという特徴がある。非特許文献1で鄭なされているエッジを利用した位置合わせは、次の1から3の処理によって実現される。
(1)前フレームでのカメラの位置及び姿勢及び予め校正済みのカメラの固有パラメータに基づいて、前述の3次元線分モデルを画像上に投影する。
(2)投影された各線分を、画像上で一定間隔となるように分割し、分割点を設定する。そして各分割点について、該分割点を通過し向きが投影された線分の法線方向である線分(探索ライン)上でエッジ探索を行い、探索ライン上における輝度値の勾配が極大でありかつ分割点に最も近い点を対応エッジとして検出する。
(3)分割点毎に検出された対応エッジと、投影される線分との間の画像上での距離の総和が最小となるようなカメラの位置及び姿勢の補正値を算出し、カメラの位置及び姿勢を補正する。
【0005】
エッジを利用した位置合わせでは、3次元線分モデル中のエッジが撮影画像から検出できない場合がある。また、撮影画像から3次元線分モデルとしてモデル化されていないエッジが多量に検出される場合には、誤った点を対応点として検出する誤対応が発生する場合がある。誤対応がある場合には、位置姿勢の最適化計算において繰り返し計算が収束しない、局所解に陥る、得られる撮像装置の位置及び姿勢の精度が低いものになるなどの問題が発生する。この課題に対して、非特許文献2では、ロバスト推定手法の一つであるM推定を用いることが記載されている。即ち、非特許文献2は、前述の対応点と線分との距離が大きいデータについては重みを小さく、距離が小さいデータについては重みを大きくして、重み付き誤差の和を最小化することにより誤検出の影響を排除する。
【0006】
また、非特許文献4では、画像上での線分周辺の見えの情報を保持しておくことにより、3次元線分モデルと撮影画像中のエッジとの対応付けの精度をより向上させている。撮影画像から3次元線分モデルと対応付いたエッジの見え情報を随時更新することで、照明変化や視点変化に起因する誤検出の影響を排除している。
【0007】
また、非特許文献3では、3次元線分モデルの情報以外に、撮影画像中に撮像される観察対象物体から抽出した点特徴の情報を併用し、エッジを利用した位置合わせが破綻することを回避している。撮影画像中から抽出した点特徴のフレーム間対応に基づいて現フレームにおけるカメラの位置及び姿勢を反復演算により算出し、エッジによる位置合わせの結果と統合することで、位置姿勢推定の安定性を向上させている。
【0008】
また一方、エッジを利用した位置合わせでは、3次元の線分の集合により記述されている観察対象物体の3次元線分モデルを持つことが前提となっている。非特許文献1、2、3、4では、観察対象物体の3次元線分モデルは、現実の観察対象物体を利用し、手動で作成されている。しかし、現実の観察対象物体から観察されるであろう3次元エッジを選別し、手動で生成するのは、比較的複雑で、多くの時間を要する。また、曲面が形作る輪郭に基づくエッジは、オフラインの3次元線分モデルとして記述することは困難である。
【0009】
これに対し、CADモデルなどの観察対象物体の形状を表す3次元モデルデータから3次元線分モデルを生成して、エッジによる位置姿勢推定を行う方法が、特許文献1で提案されている。この手法では、あらかじめ3次元モデルデータを描画し、描画結果から3次元モデルデータの幾何的な情報に基づくエッジを検出することで、3次元線分モデルを生成する。また、非特許文献5では、対象物体の3次元モデルデータをあらかじめ描画し、描画した視点の奥行き値から幾何的情報に基づくエッジを抽出することで、3次元線分モデルを生成している。これらの手法では、あらかじめ観察対象物体の3次元モデルを描画し、描画画像からエッジを抽出することで、観察対象物体の3次元線分モデルを生成している。これにより、観察対象物体の3次元線分モデルを手動で作成する手間を低減することができる。
【0010】
しかし、上記の手法は、曲面を有する物体を様々な方向から見るような場面のように、あらかじめ描画した観察対象物体の見えと、オンラインでの見えが同じとは限らないものに対しては対応できないという問題がある。これに対して、テクスチャの設定された3次元モデルをオンラインで描画し、その描画画像から3次元線分モデルを随時生成して、位置姿勢推定を行う手法が非特許文献6で提案されている。この手法によれば、現実空間中の観察対象物体と同様の見えとなるように、テクスチャの設定された3次元モデルデータを随時描画することで、様々な方向から見る場面に対しても対応することができる。
【0011】
一方、3次元モデルデータを利用せず、撮影画像のみから特徴の抽出と3次元位置計算を行い、オンラインで3次元線分モデルを生成しながら位置姿勢推定を行う手法が非特許文献7で提案されている。この手法では、観察対象物体の3次元モデルを必要とせず、時系列的な撮影画像のみから3次元線分モデルを生成して位置姿勢推定することが可能である。
【非特許文献1】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.
【非特許文献2】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.
【非特許文献3】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.
【非特許文献4】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.
【非特許文献5】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.
【非特許文献6】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.
【非特許文献7】E. Eade and T. Drummond, “Edgelandmarks in monocular SLAM,” Proc. BMVC06, pp.7-16, 2006.
【非特許文献8】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.
【非特許文献9】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.
【特許文献1】特開2007-207251号公報
【発明の開示】
【発明が解決しようとする課題】
【0012】
特許文献1で提案されている従来技術では、観察対象物体の表面形状を表す3次元モデルデータの幾何的な情報に基づいて3次元線分モデルを生成している。しかし、オフラインであらかじめ3次元モデルデータを描画して3次元線分モデルを生成する手法では、生成した3次元線分モデルに対応付く特徴が撮影画像から常に検出されるという保証はない。3次元線分モデルに対応付く特徴を撮影画像から検出できるか否かが、対象物体の表面色や撮像時の照明環境に依存しているからである。生成された3次元線分モデルに対応付く特徴が撮影画像から検出できない場合、その線分が誤対応を起こす可能性が高くなり、位置姿勢推定精度が低下するという課題が発生する。
【0013】
また、非特許文献5で開示されている手法でも、あらかじめ3次元モデルを描画して、その描画画像の奥行き値から3次元線分モデルを生成しているため、生成した3次元線分モデルに対応付く特徴が実際の撮影画像から検出できるとは限らない。そのため、特許文献1と同様の課題が発生する。
【0014】
このような誤対応問題に対しては、非特許文献2に提案されるようなM推定を利用した誤対応に対するロバスト化を適用することが考えられる。しかし、この手法は、撮影画像から安定して検出されるエッジのみから構成された3次元線分モデルを前提としているため、3次元線分モデルに登録してある線分が撮影画像から観察されない場合には、アウトライヤとして排除する処理しか行わない。そのため、3次元線分モデル自体が不正確である場合など、誤対応として排除される特徴が多くある場合には、位置姿勢推定が不安定になるという課題がある。また、非特許文献4に提案されているような、線分周辺の見えを利用した対応付けロバスト化手法でも、3次元線分モデルに対応付く特徴が撮影画像から検出できない場合はアウトライヤとして排除するだけである。そのため、非特許文献4の手法を用いても、3次元線分モデルが不正確な場合には、位置姿勢推定が不安定になる。
【0015】
一方、物体表面の模様などの、幾何的には検出できないが色の不連続領域としては検出できる特徴が観察対象物体に存在する場合、前述の従来手法では、このような色に基づく特徴を利用することはできず、逆に特徴が誤対応する原因となる。これに対し、非特許文献6で提案されている手法では、観察時の見た目と同様の見た目を描画できるようなテクスチャデータが設定された3次元モデルデータを利用することで、色に基づく特徴を含めて3次元線分モデルを生成し位置姿勢推定を行っている。しかし、非特許文献6の手法は、観察時の見た目と同様の見た目を描画できるようなテクスチャデータが設定された3次元モデルデータを利用することを前提としている。したがって、3次元モデルデータの設定が不正確、あるいは、実環境に光源変化があるような場合などの3次元モデルデータと実際の観察対象物体の見た目が異なる場合には、生成される3次元線分モデルが不正確になる。そのため、観察対象物体の見た目に合わせて、同様な見た目になるように3次元モデルデータを作成する必要があり、また、実環境が変化した場合には対応できないという課題がある。
【0016】
また、非特許文献3で提案されている手法では、観察対象物体を撮像した撮影画像から色に基づく特徴を随時抽出し、3次元線分モデルに加えて、抽出した特徴も利用して位置姿勢推定を行っている。しかし、この手法は、3次元線分モデルとともに、撮影画像から抽出した特徴を併用することに焦点を当てた手法であり、3次元線分モデルは、十分に特徴として観察されるエッジのみから構成されていることを前提としている。そのため、3次元線分モデルが不正確な場合、上記手法と同様に、3次元線分データと撮影画像中のエッジとの間に誤対応が頻出し、位置姿勢推定が不安定になるという課題がある。
【0017】
また、非特許文献7で提案されているような、純粋に撮影画像のみから3次元線分モデルを生成し位置姿勢推定する手法では、位置姿勢推定の精度が十分ではないという課題がある。3次元モデルデータを利用して3次元線分モデルを生成する手法と比較して、3次元線分モデルの精度が低くなってしまうためである。
【0018】
本発明は、以上の問題を鑑みてなされたものであり、観察対象物体の3次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別することにより、位置姿勢検出処理の安定化、効率化を図ることを目的とする。また、観察対象物体の3次元モデルデータから抽出した特徴が、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定可能とすることを目的とする。
【課題を解決するための手段】
【0019】
上記の目的を達成するための本発明の一態様による位置姿勢計測装置は以下の構成を備える。即ち、
撮像装置が撮像した画像を入力する入力手段と、
観察対象物体の表面形状を表す3次元モデルデータを格納する格納手段と、
前記3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出手段と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別手段と、
前記幾何特徴選別手段で選別された幾何特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出手段とを備える。
【0020】
また、本発明の他の態様による位置姿勢計測装置は、更に、
前記参照画像から、明度または色に基づいて前記観察対象物体に関連する複数の画像特徴を抽出する画像特徴抽出手段と、
前記画像特徴抽出手段で抽出した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合する統合手段とを更に備え、
前記算出手段は、前記物体特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する。
【0021】
また、上記の目的を達成するための本発明の他の態様による位置姿勢計測方法は、
撮像装置が撮像した画像を入力する入力工程と、
観察対象物体の表面形状を表す3次元モデルデータを格納した格納手段より前記3次元モデルデータを読み出し、前記3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出工程と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別工程と、
前記幾何特徴選別工程で選別された幾何特徴を前記入力工程で入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出工程とを有する。
【発明の効果】
【0022】
本発明によれば、観察対象物体の3次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別するので、位置姿勢検出処理の安定化、効率化を図ることができる。また、観察対象物体の3次元モデルデータから抽出した特徴が、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定することができる。
【発明を実施するための最良の形態】
【0023】
以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
【0024】
[第1実施形態]
本実施形態では、本発明を、画像上のエッジを利用した位置合わせ手法に適用した場合について説明する。
【0025】
図1は、第1実施形態における位置姿勢計測装置1の機能構成を示している。なお、位置姿勢計測装置1は、例えば一般的なコンピュータ装置に以下で説明する機能を実現するためのソフトウエアを実行させることによっても実現され得る。この場合、以下に説明する各機能構成の一部或いは全てが、コンピュータ装置のCPUによって実行されることになる。なお、第2、第3実施形態についても同様である。
【0026】
図1に示したように、位置姿勢計測装置1は、画像取得部110、モデル保存部120、参照画像保存部130、幾何特徴抽出部140、色特徴抽出部150、画像特徴検出部160、位置姿勢算出部170を有する。画像取得部110は撮像装置100に接続されている。撮像装置100は、動画撮影が可能であり、観察対象物体10を撮影する。位置姿勢計測装置1は、観察対象物体の3次元モデルデータ20に記述された観察対象物体10と撮像装置100との相対的な位置姿勢を、撮像装置100により得られた撮像画像の情報を用いて求める。なお、本実施形態において位置姿勢計測装置1による位置姿勢計算処理が適用できる条件として、撮像装置100が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置1を構成する各部について説明する。
【0027】
画像取得部110は、撮像装置100によって撮像された画像を位置姿勢計測装置1に入力する。画像取得部110は、撮像装置の出力がNTSCなどのアナログ出力であればアナログビデオキャプチャボードによって実現される。また撮像装置の出力がIEEE1394などのデジタル出力であれば、例えばIEEE1394インタフェースボードによって実現される。また、予め記憶装置に記憶してある静止画像や動画像のデジタルデータを読み出してもよい。
【0028】
モデル保存部120は、位置姿勢計測の基準となる観察対象物体10の表面形状を表す3次元モデルデータ20を記憶する。3次元モデルデータ20は、観察対象物体の3次元の幾何的な情報を有するモデルであり、頂点情報及び各頂点を結んで構成される面の情報によって定義される。位置姿勢計測装置1では、観察対象物体の3次元形状を表す面の情報を含んでいれば良いので、3次元モデルデータとしては、面の情報がパラメトリックに記述されたソリッドモデルでも良いし、ポリゴンモデルでも良い。また、任意の3次元モデラーのフォーマットを変換する処理部を位置姿勢計測装置1に含むようにしても構わない。なお、本実施形態では、頂点情報及び各頂点を結んで構成される面の情報を含むポリゴンモデルを3次元モデルデータとして利用する。
【0029】
参照画像保存部130は、過去フレームにおいて観察対象物体が撮像された撮影画像と、撮像されたときの観察対象物体と撮像装置との相対的な位置姿勢とを保存する。参照画像保存部130で参照画像として保持する撮影画像としては、直前のフレームの撮影画像のみとしても良いし、過去フレームの複数の撮影画像としても良いし、また、過去フレームにおいて位置姿勢推定精度の高かったフレームの撮影画像のみとしても良い。即ち、撮影画像中に撮像される観察対象物体と撮像装置との相対位置姿勢が既知の撮影画像であれば、数を問わず、またいずれの撮影画像でもよい。本実施形態では直前フレームの撮影画像を参照画像として保持するものとする。
【0030】
幾何特徴抽出部140は、モデル保存部120に保存された観察対象物体10の3次元モデルデータ20を画像平面上に描画することにより観察対象物体10の幾何的な情報に基づく特徴(以後、幾何特徴)を抽出する。なお、3次元モデルデータ20の描画においては、撮像装置100の位置姿勢として、初期値として設定された位置姿勢、或いは撮像装置100に関して算出されている現在の位置姿勢が用いられる。そして、幾何特徴抽出部140は、抽出した幾何特徴の中で、撮像装置により撮像された撮影画像中から検出が期待できる幾何特徴のみを、参照画像保存部130に保持されている位置姿勢が既知の画像(参照画像)を参照して選別する。幾何特徴の抽出方法および選別方法については後述する。
【0031】
色特徴抽出部150は、参照画像保存部130に参照画像として保持されている過去フレームの撮影画像中の観察対象物体の画像から、観察対象物体の明度や色の少なくともいずれかに基づく画像特徴(以後、色特徴という)を抽出する。そして、色特徴抽出部150は、参照画像保存部130に保存された観察対象物体の位置姿勢の情報と、モデル保存部120に保存された3次元モデルデータ20に基づいて、参照画像が保存された位置姿勢から観察される観察対象物体の幾何特徴を抽出する。そして、色特徴抽出部150は、抽出した色特徴と幾何特徴とを比較し、対応する幾何特徴が検出された色特徴を除去することで、色に基づいて抽出される特徴のみを選別する。色に基づく特徴の抽出方法については後述する。
【0032】
画像特徴検出部160は、画像取得部110によって入力された画像上において、撮像装置と観察対象物体との位置姿勢を算出するのに用いられる画像特徴を検出する。本実施形態では、画像特徴検出部160は画像上のエッジの検出を行う。エッジの検出方法については後述する。
【0033】
概略位置姿勢を反復演算の初期値として、撮像装置と観察対象物体との相対的な位置姿勢を算出する。より具体的には、位置姿勢算出部170は、
・画像特徴検出部160が検出した画像特徴の情報と、
・幾何特徴抽出部140が抽出した幾何特徴と、
・色特徴抽出部150が抽出した色特徴とに基づいて、観察対象物体10を基準とした座標系(以下、基準座標系)における撮像装置100の位置及び姿勢を算出する。
【0034】
次に、第1実施形態における位置姿勢計測方法の処理手順について説明する。図2は、第1実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【0035】
(ステップS1010)
まずステップS1010では初期化を行う。ここでは、位置姿勢計測装置1のCPUが、基準座標系における撮像装置100と観察対象物体10との相対的な概略位置姿勢の設定を行う。第1実施形態における位置姿勢計測方法は、概略の撮像装置の位置姿勢を、撮影画像上に撮像される観察対象物体のエッジ情報を利用して逐次更新していく方法である。そのため、位置姿勢計測を開始する前に予め撮像装置の概略の位置及び姿勢を初期位置及び初期姿勢として与える必要がある。
【0036】
概略の位置及び姿勢を与える初期化の方法としては、例えば予め決まった位置及び姿勢を設定しておき、撮像装置をその位置及び姿勢になるように移動することで初期化を行うことが挙げられる。または、画像内で検出するだけで認識可能な人工的な指標を配置し、該指標の各頂点の画像座標と基準座標系における3次元位置との対応から撮像装置の位置姿勢を求めて概略の位置姿勢としてもよい。また、識別性の高い自然特徴点を予め検出してその3次元位置を求めておき、初期化時に画像上で該特徴点を検出し、その画像座標と3次元位置との対応から撮像装置の位置姿勢を求めてもよい。さらに、磁気式や光学式、超音波式などの6自由度位置姿勢センサによって撮像装置の位置姿勢を計測し、それを概略の位置姿勢としてもよい。人工的な指標や自然特徴点などの画像情報と、前述の6自由度位置姿勢センサや3自由度の姿勢センサ、3自由度の位置センサを併用して計測される撮像装置の位置姿勢を用いて初期化してもよい。
【0037】
(ステップS1020)
ステップS1020では、撮像装置100が撮像した画像を画像取得部110を介して位置姿勢計測装置1に取り込む。
【0038】
(ステップS1030)
次にステップS1030において、幾何特徴抽出部140は、観察対象物体の幾何的情報に基づく特徴を検出する。第1実施形態で検出する幾何的情報に基づく特徴はエッジである。図3は、本実施形態における観察対象物体の幾何特徴の検出方法について詳細な処理手順を示すフローチャートである。
【0039】
まず、ステップS1210において、幾何特徴抽出部140は、モデル描画を行う。モデル描画とは、撮像装置100に設定されている位置姿勢に基づいて、モデル保存部120に保存された観察対象物体の3次元モデルを画像平面上に投影することである。最初のモデル描画では、ステップS1010で求めた観察対象物体の概略位置姿勢(初期値)が用いられることになる。また、2回目以降の処理では、撮像装置100と観察対象物体10との間の直前に取得された相対位置姿勢が用いられる。
【0040】
モデル描画を行うためには、撮像装置100の位置姿勢とともに、射影行列の内部パラメータ(焦点距離や主点位置など)を設定する必要がある。本実施形態では、カメラの内部パラメータをあらかじめ計測しておき、実際に用いるカメラ(撮像装置100)と内部パラメータを一致させておく。また、視点からモデルまでの距離の最大値と最小値を設定しておき、その範囲外のモデルの描画は行わないことで、描画処理の計算コストを低減する。図4にステップ1210により描画された結果を示す。概略位置姿勢により3次元モデルを画像平面上に投影することにより、画像平面上での2次元座標とその輝度値、2次元座標に対応する画像平面から3次元モデルまでの奥行き値(デプス値)が格納されたデプスバッファが算出される。
【0041】
次にステップS1220において、幾何特徴抽出部140は、ステップS1210の描画過程において生成されたデプスバッファに対して、エッジ検出を行う。デプスバッファに対してエッジ検出することで、奥行きが不連続に変化する領域を求めることができる。エッジ検出では、例えば、Cannyアルゴリズムなどによってエッジが抽出され、2値画像形式として利用できるようになる。この過程において、抽出されたエッジは、画像中で白く表され、それ以外は黒く表される。
【0042】
次にステップS1230において、幾何特徴抽出部140は、ステップS1220で生成された2値化画像について、隣接エッジのラベリングを行い、エッジの連結成分を抽出する。ラベリングは、例えばある画素の周囲8画素の中にエッジが存在すれば、同一ラベルを割り当てることにより行う。
【0043】
図5は、ステップS1230において、連結成分を抽出したエッジを撮影画像上に重ねた結果を示す図である。図5(a)は撮像された画像そのものであり、図5(b)は撮影画像上に抽出したエッジを重ね合わせた図である。ステップS1010で算出した概略の位置姿勢と、実際の位置姿勢が異なる場合には、図5(b)に示すように実際に撮像される物体と抽出したエッジとの間にずれが生じる。
【0044】
次にステップS1240において、幾何特徴抽出部140は、ステップS1230で連結成分を抽出したエッジに対して、エッジ素の抽出を行う。エッジ素とは、画像上での端点間の距離が極短いエッジのことをいう。同一ラベルにラベリングされたエッジを画像上で等間隔に分割するように分割点を算出して、この分割点の周辺の極短い連結成分を求めることでエッジ素を抽出する。図6はラベリングされたエッジから等間隔に抽出したエッジ素を表す図である。本実施形態では、分割点から3pixel離れた連結成分を端点(始点と終点)に設定して、分割点を中心としたエッジ素を抽出している。デプスバッファから抽出したエッジ素の総数をNとして、各エッジ素をGfi(i=1,2…N)で表す。エッジ素の数Nが多いほど、処理時間が長くなる。そのため、画像上でのエッジ素を抽出する間隔を変えることで処理時間を制御できる。また、エッジ素の数を一定として処理時間が一定になるように、画像上でのエッジ素間の間隔を逐次変更してもよい。
【0045】
次にステップS1250において、幾何特徴抽出部140は、ステップS1240で算出されたエッジ素に対して、基準座標系における3次元座標を求める。これには、ステップS1210で生成したデプスバッファを利用する。前述の通り、デプスバッファには、画像座標に対応する奥行き値が格納されている。但し、デプスバッファに格納されている奥行き値は、ステップ1210のレンダリング過程で用いた、視点からモデルまでの距離の最大値と最小値により0から1の値に正規化されている。そのため、デプスバッファの奥行き値から直接、基準座標系における3次元座標を求めることはできない。そこで、上述の距離の最大値と最小値を利用して、まずはデプスバッファの値を、カメラ座標系における視点からエッジ素までの距離に変換する。同時に射影行列の内部パラメータを利用して、カメラ座標系におけるエッジ素の3次元座標を求める。そして、カメラ座標系における3次元座標に対して、レンダリング過程で用いた位置姿勢の逆変換をほどこすことで、基準座標系におけるエッジ素の3次元座標を求める。3次元座標が算出されたエッジ素GFiの集合を以後、幾何特徴モデルと呼ぶ。
【0046】
(ステップS1040)
次に、ステップS1040において、参照画像保存部130に、参照画像としての過去フレームの撮影画像が保存されているか否かが判定される。参照画像が保存されている場合は、ステップS1050に進み、保存されていない場合は、ステップS1050以降の過去フレームの撮影画像を利用するステップをスキップして、ステップS1080へ進む。特に最も初期のフレームについては、過去フレームの撮影画像(参照画像)が存在しないため、処理は自動的にステップS1080に進む。
【0047】
(ステップS1050)
ステップS1050において、幾何特徴抽出部140は、参照画像保存部130に保存された過去フレームの撮影画像を利用して、ステップS1030で抽出した幾何特徴の選別を行う。即ち、幾何特徴抽出部140は、観察対象物体10に対する撮像装置100の位置姿勢が算出済みとなっている(既知である)参照画像において、ステップS1030で抽出した複数の幾何特徴に対応する画像特徴を探索する。そして、対応する画像特徴が検出された幾何特徴を当該複数の幾何特徴より取り出すことにより選別が行われる。図7は、本実施形態における観察対象物体の幾何特徴の選別方法について詳細な処理手順を示すフローチャートである。
【0048】
まず、ステップS1310において、幾何特徴抽出部140は、変数iを1にセットする。処理は、ステップS1320に進む。
【0049】
ステップS1320において、幾何特徴抽出部140は、参照画像保存部130に参照画像とともに保存されている、観察対象物体と撮像装置との相対位置姿勢の情報を取得する。そして、幾何特徴抽出部140は、取得した相対位置姿勢に基づいて、ステップS1030で求めた幾何特徴モデルのエッジ素GFiの3次元座標を当該参照画像中に投影し、幾何特徴モデルのエッジ素GFiの参照画像中における2次元座標を求める。図8は、参照画像上にエッジ素を投影した図である。図8(a)は参照画像保存部130に保存された参照画像であり、図8(b)は参照画像上に投影したエッジ素を重ね合わせた図である。図8(b)に示すように、エッジ素は、参照画像保存部130に保存されている位置姿勢の情報に基づいて、参照画像上に撮像される観察対象物体と重なる位置に投影される。
【0050】
次に、ステップS1330において、幾何特徴抽出部140は、ステップS1320で求めた2次元座標の端点(始点と終点)を結んだ直線の法線方向に平行でかつエッジ素の中心点を通過する線分(以下、探索ライン)上における濃度勾配を計算する。
【0051】
次に、ステップ1340において、幾何特徴抽出部140は、濃度勾配の極値が存在するかを算出する。図9は、本実施形態における特徴判別を説明する図である。図9に示される下側の2つのエッジ素のように、影等によりエッジ素の近傍の探索ライン上に極値がない場合は、撮影画像中から検出できない幾何特徴であると見なして、ステップ1350において、エッジ素GFiを除去し、幾何特徴モデルの総数Nから1減分する。図9上に示される、上側3つのエッジ素のように、近傍に極値がある場合は、撮影画像から検出できる幾何特徴であると見なして、ステップ1360に進む。本実施形態では、エッジ素の近傍を、中心点から探索ライン両端方向に5[pixel]以内の範囲と定義する。
【0052】
次にステップS1360において、幾何特徴抽出部140は、変数iを1増分し、処理はステップS1370に進む。ステップS1370において、幾何特徴抽出部140は、全てのエッジ素GFiについて上記処理が終了したか否かを判断する。全てのエッジ素GFiについて上記処理が終了している場合には、当該判別処理を終了し、終了していない場合は処理はステップS1320に戻る。以上の処理により、幾何特徴モデルの中で、参照画像から検出されない可能性が高いエッジ素が除去される。除去されなかったエッジ素は、撮影画像から検出が期待できるエッジとして選別されたことになる。
【0053】
(ステップS1060)
次にステップS1060において、色特徴抽出部150は、参照画像保存部130において保存されている過去フレームの撮影画像(参照画像)から観察対象物体の色情報に基づく特徴(画像特徴)を検出する(画像特徴抽出処理)。図10は、本実施形態における前フレームの撮影画像(参照画像)からの、観察対象物体の色に基づく特徴の検出方法について詳細な処理手順を示すフローチャートである。
【0054】
まず、ステップS1410において、色特徴抽出部150は、参照画像保存部130に保存された参照画像からエッジを抽出する。参照画像に対してエッジ検出することで、輝度値が不連続に変化する領域を求めることができる。エッジの検出方法は、ステップS1220で記載した処理と同様である。ただし、ステップS1220の処理と異なり、参照画像を対象としてエッジ検出を行う。
【0055】
次に、ステップS1420において、色特徴抽出部150は、ステップS1410で抽出したエッジのラベリングを行い、エッジの連結成分を抽出する。ラベリング方法は、ステップS1230で記載した処理と同様である。
【0056】
次に、ステップS1430において、色特徴抽出部150は、ステップS1420で抽出したエッジの連結成分からエッジ素を抽出する。エッジ素の抽出方法は、ステップS1240で記載した処理と同様である。
【0057】
次に、ステップS1440において、色特徴抽出部150は、ステップS1430で抽出したエッジ素の基準座標上の3次元座標を計算する。まず、色特徴抽出部150は、参照画像保存部130に保存された観察対象物体と撮像装置との相対位置姿勢に基づいて、モデル保存部120に保存されている3次元モデルデータ20を描画し、参照画像のデプスバッファを取得する。そして、色特徴抽出部150は、取得したデプスバッファを利用して、エッジ素の3次元座標を計算する。デプスバッファからのエッジ素の3次元座標の算出方法は、ステップS1250で記載した処理と同様である。以上の処理により、基準座標系における3次元座標が算出された参照画像の色の境界に基づくエッジ素CFjの集合を以後、色特徴モデルと呼ぶ。
【0058】
(ステップS1070)
次に、ステップS1070において、色特徴抽出部150は、ステップS1440で生成したデプスバッファを利用して、ステップS1060で抽出した色特徴の選別(画像特徴選別処理)を行う。図11は、本実施形態における観察対象物体の色特徴モデルのエッジ素CFiの選別方法について詳細な処理手順を示すフローチャートである。
【0059】
まず、ステップS1510において、色特徴抽出部150は、変数jを1にセットし、処理はステップS1520に進む。
【0060】
次に、ステップS1520において、色特徴抽出部150は、ステップS1430で求めた、色の特徴に基づくエッジ素CFiの探索ライン上における、ステップS1440で求めたデプスバッファ上のデプス値の勾配を計算する。
【0061】
次に、ステップS1530において、色特徴抽出部150は、ステップS1520で求めたデプスバッファ上の濃度勾配から極値が検出されるかを判別する。エッジ素の近傍でデプスバッファ上から極値が検出された場合、そのエッジ素は幾何的にも抽出されるエッジ素であると見なして、ステップS1540において、エッジ素CFjを除去する。エッジ素の近傍でデプスバッファ上から極値が検出されない場合は、色特徴抽出部150は、当該エッジ素は物体表面の色に基づくエッジであると見なして、処理をステップS1550に進める。
【0062】
次にステップS1550において、色特徴抽出部150は、変数jを1増分し、処理をステップS1560に進める。ステップS1560において、全てのエッジ素CFjについて処理が終了しているどうかが判断される。全てのエッジ素について処理が終了している場合には、当該判別処理を終了し、終了していない場合は処理をステップ1320に戻す。以上の処理により、色特徴モデルの中で、幾何的にも検出できるエッジ素が除去され、物体表面の色の境界に基づくエッジのみが選別される。
【0063】
(ステップS1080)
次にステップS1080において、画像特徴検出部160は、画像特徴選別処理で選別した色特徴と、幾何特徴選別処理で選別した幾何特徴とを物体特徴として統合する。より具体的には、画像特徴検出部160は、ステップS1030で生成した幾何特徴モデルとステップS1060で生成した色特徴モデルを統合した物体特徴モデルの生成を行う。物体特徴モデルは、幾何特徴モデルのエッジ素と色特徴モデルのエッジ素の集合であり、総数をLとし、各エッジ素をOFk(k=1,2,…,L)で表す。エッジ素の総数Lは、幾何特徴モデルの総数Mと色特徴モデルの総数Nを足したものである。但し、本実施形態では、幾何特徴モデルの総数Mと色特徴モデルの総数Nは、それぞれステップS1050、S1070の選別処理によって選別された数となっている。図12は、本実施形態における観察対象物体の幾何特徴モデルと色特徴モデルの統合方法について詳細な処理手順を示すフローチャートである。
【0064】
まず、ステップS1610において、画像特徴検出部160は、ステップS1030で生成され、ステップS1050で選別されたエッジ素で構成された幾何特徴モデルを物体特徴モデルに登録する。物体特徴モデルの総数Lには幾何特徴モデルの総数Mが加えられる。
【0065】
次に、ステップS1620において、画像特徴検出部160は、ステップS1060において色特徴モデルが生成されている場合は、ステップS1630に進み、生成されていない場合は、処理を終了する。
【0066】
次に、ステップS1630において、画像特徴検出部160は、ステップS1010で求めた概略位置姿勢に基づいて、色特徴モデルの3次元座標を投影し、現フレームの撮影画像上での画像座標を算出する。幾何特徴モデルの3次元座標に対応する撮影画像上での画像座標は、ステップS1030中のモデル描画・エッジ素抽出処理により既に算出されている。
【0067】
次に、ステップS1640では、ステップS1630で、画像座標を求めた色特徴モデルを物体特徴モデルに登録し、物体特徴モデルの総数Lに色特徴モデルの総数Lを加える。
【0068】
(ステップS1090)
ステップ1090において、画像特徴検出部160は、ステップS1090で求めた物体特徴モデルのエッジ素OFk(k=1,2,…,L)に対応する、撮像装置100によって撮像された現フレーム撮影画像の画像特徴の検出を行う。本実施形態における画像特徴はエッジである。図13は、本実施形態におけるエッジ検出方法の詳細な処理手順を示すフローチャートである。図14は、物体特徴モデルのエッジ素OFkを撮影画像上に重ねた結果を示す図である。図14(a)は撮像された画像そのものであり、図14(b)は物体特徴モデルのエッジ素を重ね合わせた図である。ステップS1330やS1520と同様に、エッジ素の画像座標周辺のエッジを撮影画像から抽出する。
【0069】
まず、ステップS1710において、画像特徴検出部160は、変数kを1にセットし、処理素ステップS1720に進める。
【0070】
次に、ステップS1720において、画像特徴検出部160は、物体特徴モデルのエッジ素OFkに対応するエッジを画像から検出する。エッジ素OFkの探索ライン(エッジ素の法線方向の線分)上において、撮影画像上の濃度勾配から極値を算出する。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。本実施形態では、探索ライン上でエッジが複数存在する場合には、最もエッジ素の中心点に近いエッジを対応点とし、その画像座標とエッジ素OFkの3次元座標を保持する。なお、本実施形態では最もエッジ素OFkに近いエッジを対応点としているが、これに限るものではなく、濃度勾配の極値の絶対値が最も大きいエッジを対応点としてもよい。また、非特許文献2に示すように、1つではなく複数の点を対応点候補として保持しても本発明の本質が損なわれることはない。
【0071】
ステップS1730において、画像特徴検出部160は、変数kを1増分し、処理をステップS1740に進める。すべてのエッジ素OFkについて処理が終了している場合には当該処理を終了する。一方、すべてのエッジ素OFkについて処理が終了していない場合にはステップS1720に処理を戻し、上記処理が繰り返される。
【0072】
(ステップS1100)
ステップS1100において、位置姿勢算出部170は、撮像装置100の観察対象物体10に対する相対的な位置姿勢を算出する。相対的な位置姿勢は、幾何特徴選別処理で選別された幾何特徴と画像特徴選別処理で選別された色特徴を統合して得られた物体特徴を、現フレームにおける観察対象物体の画像に対応付けることにより算出される。より、具体的には、位置姿勢算出部170は、非線形最適化計算を用いて、撮像装置と観察対象物体との概略の相対的な位置姿勢を反復演算により補正することにより撮像装置の位置姿勢を算出する。ここで、ステップS1080において生成された物体特徴モデルのエッジ素OFkのうち、ステップS1090において対応点が求まったエッジ素の総数をLcとする。図15は、エッジの情報を利用して撮像装置の位置及び姿勢を算出する方法を説明する図である。図15では、画像の水平方向、垂直方向をそれぞれx軸、y軸としている。あるエッジ素の中心点の投影された画像座標を(u0,v0)、エッジ素の直線の画像上での傾きをx軸に対する傾きθと表す。傾きθは、エッジ素端点(始点と終点)の撮影画像上での2次元座標を結んだ直線の傾きとして算出する。エッジ素の直線の画像上での法線ベクトルは(sinθ, −cosθ)となる。また、該エッジ素の対応点の画像座標を(u’, v’)とする。
【0073】
ここで、点(u , v)を通り、傾きがθである直線の方程式は、
【数1】
と表せる。
【0074】
エッジ素の撮影画像上での画像座標は撮像装置の位置及び姿勢により変化する。また、撮像装置の位置及び姿勢の自由度は6自由度である。ここで撮像装置の位置及び姿勢を表すパラメータをsで表す。sは6次元ベクトルであり、撮像装置の位置を表す3つの要素と、姿勢を表す3つの要素からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す3次元ベクトルなどによって表現される。エッジ素中心点の画像座標(u , v)は(u0 , v0)の近傍で1次のテイラー展開によって数3のように近似できる。
【0075】
【数2】
【0076】
上記数3におけるu、vの偏微分の導出方法は例えば非特許文献8に開示されるように広く知られているのでここではその詳細は述べない。数2を数1に代入することにより、数3が得られる。
【0077】
【数3】
【0078】
ここで、数3に示す直線が該エッジ素の対応点の画像座標(u’, v’)を通過するように、撮像装置の位置及び姿勢sの補正値Δsを算出する。r0=u0sinθ-v0cosθ(定数)、d=u’sinθ-v’ cosθ(定数)とすると、
【数4】
が得られる。数4はLc個のエッジ素について成り立つため、数5のようなΔsに対する線形連立方程式が成り立つ。
【0079】
【数5】
【0080】
ここで数5を数6のように簡潔に表す。
【数6】
【0081】
数6をもとにGauss−Newton法などによって、行列Jの一般化逆行列(JT・J)−1を用いてΔsが求められる。しかしながら、エッジの検出には誤検出が多いので、次に述べるようなロバスト推定手法を用いる。一般に、誤検出されたエッジに対応するエッジ素では誤差d−rが大きくなる。そのため数5、数6の連立方程式に対する寄与度が大きくなり、その結果得られるΔsの精度が低下してしまう。そこで、誤差d−rが大きいエッジ素のデータには小さな重みを与え、誤差d−rが小さいエッジ素のデータには大きな重みを与える。重みは例えば数7Aに示すようなTukeyの関数により与える。
【0082】
【数7A】
【0083】
cは定数である。なお、重みを与える関数はTukeyの関数である必要はなく、例えば次式(数7B)で示されるようなHuberの関数など、誤差d−rが大きいエッジ素には小さな重みを与え、誤差d−rが小さいエッジ素には大きな重みを与える関数であればなんでもよい。
【0084】
【数7B】
【0085】
エッジ素OFkに対応する重みをwiとする。ここで数8のように重み行列Wを定義する。
【数8】
【0086】
重み行列Wは、対角成分以外はすべて0のLc×Lc正方行列であり、対角成分には重みwiが入る。この重み行列Wを用いて、数6を数9のように変形する。
【0087】
【数9】
【0088】
数10のように数9を解くことにより補正値Δsを求める。
【0089】
【数10】
【0090】
これにより得られたΔsを用いて、撮像装置の位置及び姿勢を更新する。次に、撮像装置の位置及び姿勢の反復演算が収束しているかどうかを判定する。補正値Δsが十分に小さかったり、誤差r−dの総和が十分小さい、誤差r−dの総和が変化しないといった場合には、撮像装置の位置及び姿勢の計算が収束したと判定する。収束していないと判定された場合には、更新された撮像装置の位置及び姿勢を用いて再度線分の傾きθ、r0、d及びu、vの偏微分を計算し直し、数10より再度補正値Δsを求め直す。なお、ここでは非線形最適化手法としてGauss−Newton法を用いた。しかしながら、非線形最適化手法はこれに限るものではなく、Newton−Raphson法、Levenberg−Marquardt法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。以上、ステップS1100における撮像装置の位置姿勢算出方法について説明した。
【0091】
(ステップS1110)
ステップS1110において、位置姿勢算出部170は、位置姿勢算出を終了する入力がなされたかどうかを判定し、入力された場合には当該処理を終了し、入力されなかった場合には処理をステップS1120に進める。
【0092】
(ステップS1120)
ステップS1120において、位置姿勢算出部170は、位置姿勢の算出結果と位置姿勢計測の対象にした撮影画像を参照画像保存部130に参照画像として保存する。その後、処理はステップS1020に戻り、新たな画像の取得と、再度の位置姿勢算出が行われる。
【0093】
以上述べたように、第1実施形態では、
・随時撮影画像上で撮像される観察対象物体から色に基づく画像特徴を抽出し、
・これと同時に、観察対象物体の3次元モデルから抽出した幾何情報のうち撮影画像から検出されるものを選別し、
・観察対象物体の色に基づく画像特徴と選別された幾何情報を統合して位置姿勢計測に利用する。このため、第1実施形態によれば、3次元モデルから得られる特徴と撮影画像から得られる特徴が大きく異なる場合や、撮像の途中に光源変化があった場合にも、観察対象物体と撮像装置との相対位置姿勢を安定かつ高精度に算出することができる。
【0094】
[変形例1−1]
上述の第1実施形態では、ステップS1060の色特徴抽出処理において、参照画像中に撮像された観察対象物体のデプスバッファを算出するために、参照画像保存部130に保存されている参照画像の位置姿勢を利用して3次元モデルデータを描画した。しかし、ステップS1030の幾何特徴抽出処理で利用する概略位置姿勢として、前フレームの位置姿勢を利用する場合には、参照画像部に保存された位置姿勢と、ステップS1030で利用する概略位置姿勢は同じとなる。
【0095】
したがって、ステップS1060の色特徴抽出処理では、ステップS1030で算出したデプスバッファをそのまま利用することができ、3次元モデルデータの描画を行わなくてすむ。これにより、色特徴抽出処理で3次元モデルデータの描画処理を行う必要が無くなり、計算コストを減少させることが可能である。
【0096】
[変形例1−2]
上述の第1実施形態において、ステップS1060とステップS1070の色特徴抽出処理及び色特徴選別処理を行わず、幾何特徴のみを利用して位置姿勢推定を行うようにしても良い。この場合、色特徴抽出処理を省略し、幾何情報のみを利用した位置姿勢推定が行われることになる。即ち、位置姿勢算出部170は、幾何特徴抽出部140により選別された幾何特徴を、入力された画像中の観察対象物体10の画像に対応付けることにより、撮像装置100の観察対象物体10に対する位置姿勢を算出する。この構成によれば、テクスチャのない一色の物体のような、色に基づく特徴が検出されない観察対象物体に関しては、位置姿勢推定に利用する情報を損失することなく、色特徴抽出処理を省略する分の計算コストを減少させることが可能である。また、色特徴選別処理(ステップS1070)を行わず、ステップS1060で抽出された色特徴は全て用いるようにしてもよい。但し、この場合、現フレーム中の画像における同一の画像特徴が、色特徴と幾何特徴の両方に関連して検出される可能性が大きくなるため、処理効率が下がる可能性がある。
【0097】
[第2実施形態]
第1実施形態では、位置姿勢推定後、参照画像保存部130に撮影画像を毎フレーム保存し、過去フレームの撮影画像として常に直前のフレームの撮影画像を利用していた。それに対して、第2実施形態では、参照画像保存部に保存した撮影画像を上書きせず、複数の撮影画像を保存しておき、複数の参照画像を利用して位置姿勢推定を行う方法について述べる。
【0098】
図16は、第2実施形態における位置姿勢計測装置2の構成を示している。同図に示したように、位置姿勢計測装置2は、画像取得部210、モデル保存部220、参照画像保存部230、参照画像選択部240、幾何特徴抽出部250、色特徴抽出部260、画像特徴検出部270、位置姿勢算出部280を有する。画像取得部210は撮像装置200に接続されている。撮像装置200は、観察対象物体30を撮影する。位置姿勢計測装置2は、3次元モデルデータ40に記述された観察対象物体30と撮像装置200との相対的な位置姿勢を、撮像装置200により得られた撮像画像の情報を用いて求める。なお、第2実施形態において位置姿勢計測装置2が適用できる条件として、撮像装置200が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置2を構成する各部について説明する。
【0099】
画像取得部210、モデル保存部220、画像特徴検出部270、位置姿勢算出部280は、それぞれ第1実施形態における画像取得部110、モデル保存部120、画像特徴検出部160、位置姿勢算出部170と同様の処理を行う。
【0100】
参照画像保存部230は、過去フレームにおいて撮像装置200によって観察対象物体30が撮像された撮影画像と、撮像されたときの観察対象物体と撮像装置との相対位置姿勢を保存する。同時に、参照画像保存部230は、保存する撮影画像から抽出した色特徴(本実施形態では色の境界に基づくエッジ)も基準座標系における3次元座標とともに保存しておく。第2実施形態では、あらかじめ定められた保存条件を満足する過去フレームの撮影画像が参照画像保存部230に保存される。保存条件の例としては、
・位置姿勢の推定精度が一定以上高いこと、
・保存してある参照画像から十分離れていること、即ち、保存してある参照画像に対応する位置姿勢と現フレームに関連する位置姿勢の差が所定値より大きいこと、
・以前に参照画像を保存したときから一定フレーム(所定の時間)を経過していること、
等が挙げられる。
【0101】
参照画像選択部240は、参照画像保存部230に保存された複数の参照画像から、以後のステップ(幾何特徴抽出処理)で利用する参照画像を選択する。第2実施形態では、撮像装置200に設定されている現在の位置姿勢に最も位置姿勢が近い参照画像が選択され、以後のステップの処理に利用される。
【0102】
幾何特徴抽出部250は、モデル保存部220に保存された観察対象物体30の3次元モデルデータ40と概略位置姿勢から、観察対象物体の幾何的な情報に基づく特徴を抽出する。また、幾何特徴抽出部250は、参照画像選択部240で選択された参照画像を利用して、3次元モデルデータ40から抽出した幾何特徴の中で、撮像装置200により撮像された撮影画像中から検出が期待できる特徴のみを選別する。
【0103】
色特徴抽出部260は、位置姿勢が推定された撮影画像を利用して、撮影画像中に撮像される観察対象物体の色に基づく特徴を抽出する。また、色特徴抽出部260は、推定された位置姿勢とモデル保存部220に保存された3次元モデルデータ40に基づいて、当該位置姿勢から観察される観察対象物体の幾何特徴を抽出して、比較することで、色に基づくエッジのみを抽出する。同時に、色特徴抽出部260は、色に基づくエッジの基準座標における3次元座標を求める。求めた色特徴は、参照画像保存部230にて保存される。
【0104】
次に、第2実施形態における位置姿勢計測方法の処理手順について説明する。図17は、第2実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【0105】
(ステップS2010)
ステップS2010では初期化を行う。ステップS2010の処理内容は基本的に第1実施形態におけるステップS1010と同様であるので。
【0106】
(ステップS2020)。
【0107】
ステップS2020の処理は第1実施形態におけるステップS1020の処理と同様である。
【0108】
(ステップS2030)
ステップS2030において、幾何特徴抽出部250は、観察対象物体30の3次元モデルデータ40に基づいて、幾何的情報に基づく特徴を検出する。ステップS2030の処理内容は、基本的に第1実施形態におけるステップS1030の処理と同様である。
【0109】
(ステップS2040)
参照画像保存部に過去フレームの撮影画像が保存されている場合は、処理はステップS2040からステップS2050に進む。一方、保存されていない場合は、処理は、ステップS2050以降の過去フレームの撮影画像を利用するステップをスキップして、ステップS2070へ進む。
【0110】
(ステップS2050)
ステップS2050において、参照画像選択部240は、参照画像保存部230に保存されている複数の参照画像から、適切な参照画像を選択する。本実施形態では、参照画像の選択に、現在の位置姿勢と参照画像の位置姿勢の差を利用する。なお、本実施形態では、現在の位置姿勢として、直前フレームの撮影画像から推定した位置姿勢を用いるものとする。参照画像選択部240は、現在の位置姿勢と参照画像保存部230に保存してある各参照画像の位置姿勢とを比較して、現在の位置姿勢に最も近い参照画像を選択する。ステップS2050において選択された参照画像は、以後のステップにおいて参照画像として利用される。
【0111】
(ステップS2060)
次に、ステップS2060において、幾何特徴抽出部250は、ステップS2030で抽出した幾何特徴の選別を行う。ステップS2060の処理内容は、基本的に第1実施形態におけるステップS1050の処理と同様である。但し、参照画像としてステップS2050で選択した参照画像を利用する点がステップS1050と異なる。
【0112】
(ステップS2070)
次にステップS2070において、画像特徴検出部270は、ステップS2050で生成した幾何特徴モデルと、ステップS2050で選択した参照画像に対応する色特徴モデルを利用して、幾何特徴モデルと色特徴モデルを統合した物体特徴モデルを生成する。ステップS2070の処理内容は、基本的に第1実施形態におけるステップS1080の処理と同様である。ステップS1080と異なる点は、ステップS2070で利用する色特徴モデルはステップS2050で選択した参照画像に対応する色特徴モデルを利用する点である。なお、選択した参照画像に対応する色特徴モデルは、参照画像が保存される過程のステップS2130で生成され、参照画像保存部230に保存されている(後述)。
【0113】
(ステップS2080)
ステップS2080において、画像特徴検出部270は、ステップS2080で求めた物体特徴モデルのエッジ素に対応する画像特長を、撮像装置200によって撮像された現フレームの撮影画像から検出する。ステップS2080の処理内容は、基本的に第1実施形態におけるステップS1090の処理と同様である。
【0114】
(ステップS2090)
ステップS2090において、位置姿勢算出部280は、ステップS2070で生成した物体特徴モデルと、ステップS2080で対応付けた画像特徴の位置情報から、撮像装置200の位置姿勢を算出する。ステップS2090の処理内容は、基本的に第1実施形態におけるステップS1100の処理と同様である。
【0115】
(ステップS2100)
ステップS2100において、位置姿勢計測装置2は、位置姿勢算出を終了する指示の入力がなされたかどうかを判定し、終了する指示が入力された場合には本処理を終了する。一方、終了する指示が入力されなかった場合には、処理をステップS2110に進める。
【0116】
(ステップS2110)
ステップS2110において、参照画像保存部230は、位置姿勢推定した撮影画像を参照画像として保存するかを判定する。保存する場合は、処理をステップS2120に進める。また、保存しない場合は、処理をステップS2020に戻し、新たな画像を取得して、上述の位置姿勢算出を行う。第2実施形態では、参照画像として保存する条件は、本例では、保存しようとする撮影画像が参照画像保存部230に保存されている参照画像の位置姿勢から一定以上離れており、かつ、最近に保存した参照フレームから一定フレーム経過していることとする。
【0117】
(ステップS2120)
ステップS2120において、色特徴抽出部260は、ステップS2090で推定された位置姿勢を利用して、ステップS2110で保存すると判定された撮影画像から観察対象物体の色情報に基づく特徴を検出し、色特徴モデルを生成する。なお、色特徴モデルの生成方法は、基本的に第1実施形態におけるステップS1060の処理と同様である。
【0118】
(ステップS2130)
次に、ステップS2130において、色特徴抽出部260は、ステップS2120で抽出した色特徴モデルの選別を行う。ステップS2130の処理内容は、基本的に第1実施形態におけるステップS1070の処理と同様である。
【0119】
(ステップS2140)
次に、ステップS2140において、参照画像保存部230は、ステップS2120で生成した色特徴モデルとステップS2090で算出した観察対象物体の位置姿勢と、位置姿勢算出の対象とした撮影画像とを保存する。その後、ステップS2020に戻り、新たな画像を取得して、再度位置姿勢算出を行う。
【0120】
以上述べたように、第2実施形態では、位置姿勢の異なる複数の参照画像を保持しつづけることで、毎フレーム撮影画像を参照画像として保存する必要を無くすことができる。また、該参照画像から抽出した色特徴モデルを参照画像保存部230に保持することにより、毎フレームについて色特徴を算出する必要がなくなる。さらに、複数の参照画像から、現在の観察対象物体の位置姿勢に最も近い位置姿勢の参照画像とその色特徴モデルを利用することにより、計算コストを抑えながら、幾何特徴と色特徴を統合利用した位置姿勢推定が可能である。
【0121】
[変形例2−1]
上述の第2実施形態では、参照画像として保存する撮影画像は、前回保存したときよりも一定フレーム過ぎており、かつ、保存してある撮影画像の位置姿勢と現在の位置姿勢が一定以上離れている場合に、無条件で保存していた。しかしながら、保存する位置姿勢の推定精度が低いような場合、位置姿勢の推定精度の低い参照画像から色特徴モデルを生成することになる。そのため、不正確な特徴モデルを利用することによる位置姿勢推定の精度低下が発生する。そこで、参照画像として保存するときに、位置姿勢の推定精度も算出して、推定精度が一定以上の場合のみ参照画像として保存するという制限を加える。これにより、位置姿勢の推定が不安定な場合でも、一定以上推定精度が高い位置姿勢とそのときの撮影画像のみが参照画像として保存されるため、安定した位置姿勢推定が可能となる。推定精度算出方法としては、例えば、非特許文献6で開示されているような位置姿勢推定の破綻検出手法を用いればよい。
【0122】
[変形例2−2]
上述の第2実施形態では、一旦、参照画像として保存した撮影画像は、本位置姿勢計測装置が動作している間ずっと保存されていた。しかし、撮像の途中で、光源環境が変化して、観察対象物体の色合いが変化することも考えられる。
【0123】
そこで、ステップS2140において、撮影画像を参照画像として保存するときに、時刻情報を付加しておき、一定時間が過ぎた参照画像は削除するようにしてもよい。即ち、時系列的に古い画像特徴を判別して、時系列的に画像特徴を随時更新していくようにしてもよい。これにより、時系列的に参照画像が更新されていき、撮影環境に変化が生じた場合にも柔軟に対応することが可能になる。また、単純に一定時間過ぎた参照画像を削除するのではなく、一定時間過ぎた参照画像は上書きフラグをONにしておき、撮影画像を参照画像として保存する際に、当該参照画像で上書きフラグがONの参照画像を上書きするようにしてもよい。更に、撮影画像を参照画像として保存する際に、保存する撮影画像の位置姿勢と上書きフラグがONになった参照画像の位置姿勢とを比較して、両者が十分に近い場合に、その参照画像に上書きするようにしてもよい。このようにすれば、一定時間位置姿勢が変化しなかったときに、有用な参照画像が削除されることを防ぐことができる。また、参照画像として保持する撮影画像の枚数を一定数に設定しておき、保存された時刻情報が一番古い参照画像から上書きすることにしてもよい。
【0124】
[変形例2−3]
上述の第2実施形態では、参照画像として保存する処理は、位置姿勢の算出が終わったあとステップS2110の参照画像保存判定後に行っていた。しかし、保存処理は、非同期的に行っても問題のない処理である。そこで、参照画像の保存・色特徴抽出処理、すなわち、ステップS2120〜S2140の処理を、別スレッドで行うようにしてもよい。これにより、位置姿勢の算出と参照画像の保存処理を独立して実行することが可能となり、参照画像の保存処理が実行された場合でも、位置姿勢推定のフレームレートへの影響を低減することができる。また、参照画像から特徴抽出する処理に計算コストがかかる処理を入れ込むことも可能である。例えば、参照画像からの色特徴抽出処理において、複数の参照画像から抽出した色特徴から画像間対応をとり、参照画像のみから色特徴の3次元再構成を行えば、物体の3次元モデルからは得られない特徴も利用して、位置姿勢推定することが可能となる。
【0125】
[変形例2−4]
上述の第2実施形態では、参照画像保存部230に保存された複数の参照画像の中で、一つだけを選択して、色特徴モデルや幾何特徴の判別に利用していた。しかしながら、観察対象物体の表面属性によっては、鏡面反射などの視点位置によって見た目が変わる情報も存在する。視点位置によって変わる特徴を色特徴モデルとして抽出してしまうと、色特徴モデルを利用した位置姿勢推定が不安定になる。また、観察物体に対して、オクルージョンが発生した場合にも同様の問題が発生する。
【0126】
そこで、ステップS2050において、現在の位置姿勢に近い参照画像を複数選択しておき、複数の色特徴モデルを比較して、これら複数の色特徴モデルの中で安定的に検出されている色特徴を利用するようにしてもよい。なお、「安定的に検出される色特徴」とは、例えば、複数の(2つ以上の)参照画像から検出され得る色特徴である。このようにすれば、鏡面反射やオクルージョンによる、偽の色特徴の影響を除去しながら位置姿勢推定することが可能となる。また、ステップS2060において、複数の参照画像から幾何特徴選別処理を行うことで、複数の参照画像から、幾何特徴の検出判定を行い、複数の画像において、安定して検出できる幾何特徴を利用するように構成することができる。なお、「安定して検出できる幾何特徴」とは、例えば、複数の(2つ以上の)参照画像から対応する色特徴が検出され得る幾何特徴である。これにより、鏡面反射やオクルージョンが生じた場合にも安定した位置姿勢推定を行うことが可能になる。
【0127】
[第3実施形態]
第1・第2実施形態では、位置姿勢推定後、参照画像保存部130,240に撮影画像のフレーム画像を参照画像として保存し、参照画像から対象物体の色に基づく特徴を抽出していた。それに対し、第3実施形態では、撮影画像から3次元モデルの表面色の情報を推定し、3次元モデルを描画した結果として得られる描画画像から対象物体の色に基づく特徴を抽出し、位置姿勢推定を行う方法について述べる。
【0128】
図18は、第3実施形態における位置姿勢計測装置3の構成を示している。同図に示したように、位置姿勢計測装置3は、画像取得部310、モデル保存部320、幾何特徴抽出部330、色特徴抽出部340、画像特徴検出部350、位置姿勢算出部360、物体表面色推定部370を有する。画像取得部310は撮像装置300に接続されている。撮像装置300は、観察対象物体30を撮影する。位置姿勢計測装置3は、3次元モデルデータ60に記述された観察対象物体と撮像装置300との相対的な位置姿勢を撮像画像の情報を用いて求める。なお、第3実施形態において位置姿勢計測装置3が適用できる条件として、撮像装置300が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置3を構成する各部について説明する。
【0129】
画像取得部310、画像特徴検出部350、位置姿勢算出部360は、第1実施形態における画像取得部110、画像特徴検出部160、位置姿勢算出部170と同様の処理を行う。
【0130】
モデル保存部320は、位置姿勢計測の基準となる観察対象物体50の3次元モデルデータ60を記憶する。3次元モデルデータ60は、観察対象物体50の表面形状を表す3次元の幾何的な情報、および物体表面の色情報を有するモデルであり、頂点情報及び各頂点を結んで構成される面の情報と表面の色情報を表すテクスチャ画像によって定義される。第3実施形態では、頂点情報及び各頂点を結んで構成される面の情報を含むポリゴンモデルを3次元モデルデータ60として利用する。また、同時に3次元モデルデータ60は、観察対象物体の表面の色情報をテクスチャ画像として保持する。テクスチャ画像上での2次元座標と3次元モデル中の頂点との相対位置関係は、テクスチャ変換行列としてあらかじめ求めておく。テクスチャ画像は、物体表面色推定部370において撮影画像からオンライン、すなわち実行時点で推定するため、最初期フレームでは、観察対象物体の表面色は未知である。そこで、テクスチャ画像は、あらかじめ一定色に初期化しておく。
【0131】
幾何特徴抽出部330は、モデル保存部320に保存された観察対象物体50の3次元モデルデータ60と概略位置姿勢から、3次元モデルデータを描画する。そして、描画結果として得られた画像平面上での3次元モデルデータの輝度値と、そのときの奥行き値を保存する。そして、幾何特徴抽出部330は、奥行き値から観察対象物体50の幾何的な情報に基づく特徴を抽出する。
【0132】
色特徴抽出部340は、上述の画像平面上での3次元モデルデータ60の輝度値から、観察対象物体の色に基づく特徴(エッジ)を抽出する。同時に、色に基づく特徴(エッジ)の基準座標における3次元座標を求める。
【0133】
物体表面色推定部370は、位置姿勢が推定された撮影画像を利用して観察対象物体50の表面色を推定する。本実施形態では、撮影画像上に撮像される観察対象物体50の輝度値を、テクスチャ画像に対してマッピングすることで、観察対象物体の表面色の推定を行う。
【0134】
次に、第3実施形態における位置姿勢計測方法の処理手順について説明する。図19は、第3実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【0135】
(ステップS3010)
ステップS3010では初期化を行う。ステップS3010の処理内容は基本的に第1実施形態におけるステップS1010と同様である。
【0136】
(ステップS3020)。
【0137】
ステップS3020の処理は第1実施形態におけるステップS1020の処理と同様である。
【0138】
(ステップS3030)
ステップS3030において、幾何特徴抽出部330は、3次元モデルデータ60を用いて観察対象物体50を描画し、幾何的情報に基づく特徴を検出する。ステップS3030の処理内容は基本的に第1実施形態におけるステップS1010と同様である。ステップS1010と異なる点は、3次元モデルを描画する際にモデル保存部320に保持しているテクスチャ画像によるテクスチャマッピングを行い、描画結果をカラーバッファとして保存する点である。表面色が推定された3次元モデルを描画することで、実際の撮影画像に撮像される観察対象物体の見た目に近い、3次元モデルの描画結果が得られる。この描画結果として得られる画像平面上における3次元モデルデータの輝度値をカラーバッファとして保存し、以後の処理に利用する。
【0139】
(ステップS3040)
ステップS3040では、モデル保存部320に保持しているテクスチャ画像が更新されている場合は、ステップS3050に処理を進める。一方、最初期フレームなどで、テクスチャ画像が更新されていない場合は、ステップS3050〜S3070をスキップして、ステップS3090へ処理を進める。
【0140】
(ステップS3050)
ステップS3050において、幾何特徴抽出部330は、ステップS3030で抽出した幾何特徴の選別を行う。ステップS3050の処理内容は、基本的に第1実施形態におけるステップS1050の処理と同様である。ステップS1050と異なる点は、参照画像の代わりに、ステップS3030で保存したカラーバッファを利用する点である。
【0141】
(ステップS3060)
ステップS3060において、色特徴抽出部340は、ステップS3030で保存したカラーバッファから、観察対象物体の色情報に基づく特徴を検出し、色特徴モデルを生成する。色特徴モデルの生成方法は、基本的に第1実施形態におけるステップS1060の処理と同様である。ステップS1060と異なる点は、参照画像の代わりに、ステップS3030で保存したカラーバッファを利用する点である。また、色特徴の逆投影に利用する位置姿勢には、ステップ3030において3次元モデルデータの描画に利用した位置姿勢と同一のものを利用する。
【0142】
(ステップS3070)
次に、ステップS3070において、色特徴抽出部340は、ステップS3060で抽出した色特徴モデルの選別を行う。ステップS3070の処理内容は、基本的に第1実施形態におけるステップS1070の処理と同様である。ステップS1060と異なる点は、参照画像の代わりに、ステップS3030で保存したカラーバッファを利用する点である。また、色特徴の逆投影に利用する位置姿勢には、ステップ3030において3次元モデルデータの描画に利用した位置姿勢と同一のものを利用する。
【0143】
(ステップS3080)
次にステップS3080において、画像特徴検出部350は、ステップS3030で生成した幾何特徴モデルと、ステップS3060で生成した色特徴モデルを統合した物体特徴モデルを生成する。ステップS3080の処理内容は、基本的に第1実施形態におけるステップS1080の処理と同様である。
【0144】
(ステップS3090)
ステップ3090において、画像特徴検出部350は、ステップS3080で求めた物体特徴モデルのエッジ素に対応する画像特長を、撮像装置300によって撮像された現フレーム撮影画像より検出する。ステップS3090の処理内容は、基本的に第1実施形態におけるステップS1090の処理と同様である。
【0145】
(ステップS3100)
ステップS3100において、位置姿勢算出部360は、ステップS3080で生成した物体特徴モデルと、ステップS3090で対応付けた画像特徴の位置情報から、撮像装置300の位置姿勢を算出する。ステップS3100の処理内容は、基本的に第1実施形態におけるステップS1100の処理と同様である。
【0146】
(ステップS3110)
ステップS3110において、位置姿勢計測装置3は、位置姿勢算出を終了する入力がなされたかどうかを判定する。位置姿勢算出を終了する入力がなされた場合には当該終了し、位置姿勢算出を終了する入力がなされなかった場合には処理をステップS3120に進める。
【0147】
(ステップS3120)
ステップS3120において、物体表面色推定部370は、位置姿勢推定した撮影画像から、観察対象物体の表面色を推定する。まず、物体表面色推定部370は、ステップS3030において基準座標系におけるエッジ素の3次元座標を求めた手順と同様の処理により、撮影画像上の2次元座標に対応する基準座標系における3次元座標を求める。次に、求めた3次元座標に対応するテクスチャ画像上の2次元座標を、モデル保存部320に保存されているテクスチャ変換行列を利用することで求める。以上の手順により、撮影画像上の2次元座標とテクスチャ画像上の2次元座標の関係を求め、撮影画像上に撮像される観察対象物体の輝度値をテクスチャ画像上にマッピングすることで、テクスチャ画像を更新し、観察対象物体の表面色を推定する。このとき、撮影画像上の輝度値をそのままテクスチャ画像上の輝度値として設定せずに、学習係数に応じて徐々にテクスチャ画像の輝度値を更新する。これにより、位置姿勢推定精度が低い撮影画像が混入した場合や、オクルージョンが発生した場合の影響を低減し、確からしい表面色のみを更新することができる。学習係数は、位置姿勢推定精度の高さと、表面色がどのくらい急激に変化しうるかにより適切な値を設定する。以上の処理により表面色を更新した後は、ステップS3020に戻り、新たな画像を取得して、再度位置姿勢算出を行う。
【0148】
以上述べたように、第3実施形態では、位置姿勢推定した撮影画像から毎フレームのテクスチャ画像を更新し、物体の表面色を直接推定する。これにより、参照画像として撮影画像を保存する必要をなくした。以上のように、第3実施形態では、物体の表面色が推定された3次元モデルデータを描画し、実際に撮影画像に撮像される観察対象物体の見た目に近い描画結果から観察対象物体の色に基づく特徴を抽出する。これにより、観察対象物体の幾何特徴と色特徴とを統合利用した位置姿勢推定が可能となる。
【0149】
[他の実施形態]
第1〜第3実施形態では、参照画像から抽出する色に基づく特徴として、エッジを用いたが、これに限るものでない。例えば、Harris検出器や、非特許文献9に示されるSIFT検出器などによって検出される点特徴を利用しても良い。このとき、点特徴の記述子としては、点特徴周辺の輝度分布を利用しても、非特許文献9に示されるSIFT記述子を利用しても良く、点特徴の検出子と記述子の選択に特に制限はない。
【0150】
また、色に基づく特徴として点特徴を利用する場合、色特徴モデルとしては、参照画像に撮像される観察対象物体から検出される点特徴の、基準座標系における3次元位置と点特徴が検出された画像上での画像座標、その画像へのポインタが保存される。そして、色特徴モデル中の点特徴と、現フレームの撮影画像から検出した点特徴とを対応付けることで、位置姿勢が推定される。点特徴は、細かい模様のある物体など、特徴としてエッジを利用した場合には誤対応が頻出するような物体に対して、特に検出されやすい。そのため、対象に細かい模様があるような場合には、エッジを特徴として利用するよりも、安定した位置姿勢推定が行うことが可能である。
【0151】
以上、実施形態を詳述したが、本発明は、例えば、システム、装置、方法、プログラムもしくは記憶媒体等としての実施態様をとることが可能である。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
【0152】
尚、本発明は、ソフトウェアのプログラムをシステム或いは装置に直接或いは遠隔から供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムコードを読み出して実行することによって前述した実施形態の機能が達成される場合を含む。この場合、供給されるプログラムは実施形態で図に示したフローチャートに対応したコンピュータプログラムである。
【0153】
従って、本発明の機能処理をコンピュータで実現するために、該コンピュータにインストールされるプログラムコード自体も本発明を実現するものである。つまり、本発明は、本発明の機能処理を実現するためのコンピュータプログラム自体も含まれる。
【0154】
その場合、プログラムの機能を有していれば、オブジェクトコード、インタプリタにより実行されるプログラム、OSに供給するスクリプトデータ等の形態であっても良い。
【0155】
コンピュータプログラムを供給するためのコンピュータ読み取り可能な記憶媒体としては以下が挙げられる。例えば、フロッピー(登録商標)ディスク、ハードディスク、光ディスク、光磁気ディスク、MO、CD−ROM、CD−R、CD−RW、磁気テープ、不揮発性のメモリカード、ROM、DVD(DVD−ROM,DVD−R)などである。
【0156】
その他、プログラムの供給方法としては、クライアントコンピュータのブラウザを用いてインターネットのホームページに接続し、該ホームページから本発明のコンピュータプログラムをハードディスク等の記録媒体にダウンロードすることが挙げられる。この場合、ダウンロードされるプログラムは、圧縮され自動インストール機能を含むファイルであってもよい。また、本発明のプログラムを構成するプログラムコードを複数のファイルに分割し、それぞれのファイルを異なるホームページからダウンロードすることによっても実現可能である。つまり、本発明の機能処理をコンピュータで実現するためのプログラムファイルを複数のユーザに対してダウンロードさせるWWWサーバも、本発明に含まれるものである。
【0157】
また、本発明のプログラムを暗号化してCD−ROM等の記憶媒体に格納してユーザに配布するという形態をとることもできる。この場合、所定の条件をクリアしたユーザに、インターネットを介してホームページから暗号を解く鍵情報をダウンロードさせ、その鍵情報を使用して暗号化されたプログラムを実行し、プログラムをコンピュータにインストールさせるようにもできる。
【0158】
また、コンピュータが、読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているOSなどとの協働で実施形態の機能が実現されてもよい。この場合、OSなどが、実際の処理の一部または全部を行ない、その処理によって前述した実施形態の機能が実現される。
【0159】
さらに、記録媒体から読み出されたプログラムが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれて前述の実施形態の機能の一部或いは全てが実現されてもよい。この場合、機能拡張ボードや機能拡張ユニットにプログラムが書き込まれた後、そのプログラムの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるCPUなどが実際の処理の一部または全部を行なう。
【0160】
以上のように、上記各実施形態によれば、位置姿勢が既知である参照画像(過去フレームにおいて観察対象物体が撮像された撮影画像やテクスチャが設定されたモデル画像)を利用して、使用すべき画像特徴が決定される。そして、このような画像特徴を用いて現在のフレームに関する撮像装置の位置姿勢を算出するので、位置姿勢推定の安定性が向上する。即ち、各実施形態によれば、観察対象物体の3次元モデルデータから抽出した3次元線分モデルが、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定することができる。また、3次元モデルから抽出される幾何的特徴に加えて、観察対象物体の表面の色に基づく特徴を利用することで、3次元モデルデータの中に含まれていない情報も利用して位置姿勢推定することが可能である。
【図面の簡単な説明】
【0161】
【図1】第1実施形態における位置姿勢計測装置1の構成を示す図である。
【図2】第1実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【図3】第1実施形態における対象幾何特徴抽出の詳細な処理手順を示すフローチャートである。
【図4】3次元モデルデータの描画を示す図である。
【図5】デプスバッファから抽出したエッジを撮影画像に重ね合わせた図である。
【図6】エッジ素の抽出を示す図である。
【図7】本発明の一実施形態における対象幾何特徴選別の詳細な処理手順を示すフローチャートである。
【図8】幾何特徴モデルのエッジ素の参照画像への投影を示す図である。
【図9】幾何特徴モデルのエッジ素近傍のエッジ探索を示す図である。
【図10】第1実施形態における対象色特徴抽出の詳細な処理手順を示すフローチャートである。
【図11】第1実施形態における対象色特徴選別の詳細な処理手順を示すフローチャートである。
【図12】第1実施形態における幾何特徴モデルと色特徴モデルとの統合の詳細な処理手順を示すフローチャートである。
【図13】第1実施形態における現フレームの撮影画像からの画像特徴抽出の詳細な処理手順を示すフローチャートである。
【図14】物体特徴モデルのエッジ素を現フレームの撮影画像に重ね合わせた図である
【図15】エッジの情報を利用して撮像装置の位置及び姿勢を算出する方法を説明する図である。
【図16】第2実施形態における位置姿勢計測装置2の構成を示す図である
【図17】第2実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【図18】第3実施形態における位置姿勢計測装置3の構成を示す図である
【図19】第3実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【技術分野】
【0001】
本発明は、観察対象物体の表面形状を表す3次元モデルデータと撮像装置が撮像した観察対象物体の撮像画像を利用して、撮像装置と観察対象物体との相対的な位置姿勢を算出する位置姿勢計測装置及び方法に関するものである。
【背景技術】
【0002】
従来より、現実空間を撮像するカメラなどの撮像装置を用いて、観察対象物体と当該観察対象物体を撮像する撮像装置との相対的な位置姿勢を計測する技術が提案されている。この種の位置姿勢計測技術は、現実空間と仮想空間を融合表示する複合現実感システムや、ロボットの位置姿勢計測において、非常に有用な技術である。
【0003】
従来より、撮像装置の位置姿勢を計測する方法として、3次元位置が既知の指標を用いる方法が提案されている。この方法では、撮像装置のおおよその位置姿勢を用いて各指標の3次元位置を撮像面へ投影した投影位置と、撮像画像から検出される指標の撮像画像上の位置との距離を誤差として、この誤差を小さくする目的関数を最適化するように位置姿勢を推定する。指標としては、予め検出が容易な幾何学的特徴を有するもの、または色相が特異な特徴を有するものを利用することが多い。
【0004】
また、画像中でのエッジと観察対象の3次元モデルとの対応をもとに撮像装置の位置及び姿勢を求める方法も提案されている。ここで、撮影画像上で撮像される観察対象物体により観察される輝度が不連続に変化する領域をエッジと呼称する。エッジはスケールや観察方向に対して不変であるため、エッジを利用した位置合わせは精度が高いという特徴がある。非特許文献1で鄭なされているエッジを利用した位置合わせは、次の1から3の処理によって実現される。
(1)前フレームでのカメラの位置及び姿勢及び予め校正済みのカメラの固有パラメータに基づいて、前述の3次元線分モデルを画像上に投影する。
(2)投影された各線分を、画像上で一定間隔となるように分割し、分割点を設定する。そして各分割点について、該分割点を通過し向きが投影された線分の法線方向である線分(探索ライン)上でエッジ探索を行い、探索ライン上における輝度値の勾配が極大でありかつ分割点に最も近い点を対応エッジとして検出する。
(3)分割点毎に検出された対応エッジと、投影される線分との間の画像上での距離の総和が最小となるようなカメラの位置及び姿勢の補正値を算出し、カメラの位置及び姿勢を補正する。
【0005】
エッジを利用した位置合わせでは、3次元線分モデル中のエッジが撮影画像から検出できない場合がある。また、撮影画像から3次元線分モデルとしてモデル化されていないエッジが多量に検出される場合には、誤った点を対応点として検出する誤対応が発生する場合がある。誤対応がある場合には、位置姿勢の最適化計算において繰り返し計算が収束しない、局所解に陥る、得られる撮像装置の位置及び姿勢の精度が低いものになるなどの問題が発生する。この課題に対して、非特許文献2では、ロバスト推定手法の一つであるM推定を用いることが記載されている。即ち、非特許文献2は、前述の対応点と線分との距離が大きいデータについては重みを小さく、距離が小さいデータについては重みを大きくして、重み付き誤差の和を最小化することにより誤検出の影響を排除する。
【0006】
また、非特許文献4では、画像上での線分周辺の見えの情報を保持しておくことにより、3次元線分モデルと撮影画像中のエッジとの対応付けの精度をより向上させている。撮影画像から3次元線分モデルと対応付いたエッジの見え情報を随時更新することで、照明変化や視点変化に起因する誤検出の影響を排除している。
【0007】
また、非特許文献3では、3次元線分モデルの情報以外に、撮影画像中に撮像される観察対象物体から抽出した点特徴の情報を併用し、エッジを利用した位置合わせが破綻することを回避している。撮影画像中から抽出した点特徴のフレーム間対応に基づいて現フレームにおけるカメラの位置及び姿勢を反復演算により算出し、エッジによる位置合わせの結果と統合することで、位置姿勢推定の安定性を向上させている。
【0008】
また一方、エッジを利用した位置合わせでは、3次元の線分の集合により記述されている観察対象物体の3次元線分モデルを持つことが前提となっている。非特許文献1、2、3、4では、観察対象物体の3次元線分モデルは、現実の観察対象物体を利用し、手動で作成されている。しかし、現実の観察対象物体から観察されるであろう3次元エッジを選別し、手動で生成するのは、比較的複雑で、多くの時間を要する。また、曲面が形作る輪郭に基づくエッジは、オフラインの3次元線分モデルとして記述することは困難である。
【0009】
これに対し、CADモデルなどの観察対象物体の形状を表す3次元モデルデータから3次元線分モデルを生成して、エッジによる位置姿勢推定を行う方法が、特許文献1で提案されている。この手法では、あらかじめ3次元モデルデータを描画し、描画結果から3次元モデルデータの幾何的な情報に基づくエッジを検出することで、3次元線分モデルを生成する。また、非特許文献5では、対象物体の3次元モデルデータをあらかじめ描画し、描画した視点の奥行き値から幾何的情報に基づくエッジを抽出することで、3次元線分モデルを生成している。これらの手法では、あらかじめ観察対象物体の3次元モデルを描画し、描画画像からエッジを抽出することで、観察対象物体の3次元線分モデルを生成している。これにより、観察対象物体の3次元線分モデルを手動で作成する手間を低減することができる。
【0010】
しかし、上記の手法は、曲面を有する物体を様々な方向から見るような場面のように、あらかじめ描画した観察対象物体の見えと、オンラインでの見えが同じとは限らないものに対しては対応できないという問題がある。これに対して、テクスチャの設定された3次元モデルをオンラインで描画し、その描画画像から3次元線分モデルを随時生成して、位置姿勢推定を行う手法が非特許文献6で提案されている。この手法によれば、現実空間中の観察対象物体と同様の見えとなるように、テクスチャの設定された3次元モデルデータを随時描画することで、様々な方向から見る場面に対しても対応することができる。
【0011】
一方、3次元モデルデータを利用せず、撮影画像のみから特徴の抽出と3次元位置計算を行い、オンラインで3次元線分モデルを生成しながら位置姿勢推定を行う手法が非特許文献7で提案されている。この手法では、観察対象物体の3次元モデルを必要とせず、時系列的な撮影画像のみから3次元線分モデルを生成して位置姿勢推定することが可能である。
【非特許文献1】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.
【非特許文献2】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.
【非特許文献3】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.
【非特許文献4】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.
【非特許文献5】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.
【非特許文献6】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.
【非特許文献7】E. Eade and T. Drummond, “Edgelandmarks in monocular SLAM,” Proc. BMVC06, pp.7-16, 2006.
【非特許文献8】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.
【非特許文献9】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.
【特許文献1】特開2007-207251号公報
【発明の開示】
【発明が解決しようとする課題】
【0012】
特許文献1で提案されている従来技術では、観察対象物体の表面形状を表す3次元モデルデータの幾何的な情報に基づいて3次元線分モデルを生成している。しかし、オフラインであらかじめ3次元モデルデータを描画して3次元線分モデルを生成する手法では、生成した3次元線分モデルに対応付く特徴が撮影画像から常に検出されるという保証はない。3次元線分モデルに対応付く特徴を撮影画像から検出できるか否かが、対象物体の表面色や撮像時の照明環境に依存しているからである。生成された3次元線分モデルに対応付く特徴が撮影画像から検出できない場合、その線分が誤対応を起こす可能性が高くなり、位置姿勢推定精度が低下するという課題が発生する。
【0013】
また、非特許文献5で開示されている手法でも、あらかじめ3次元モデルを描画して、その描画画像の奥行き値から3次元線分モデルを生成しているため、生成した3次元線分モデルに対応付く特徴が実際の撮影画像から検出できるとは限らない。そのため、特許文献1と同様の課題が発生する。
【0014】
このような誤対応問題に対しては、非特許文献2に提案されるようなM推定を利用した誤対応に対するロバスト化を適用することが考えられる。しかし、この手法は、撮影画像から安定して検出されるエッジのみから構成された3次元線分モデルを前提としているため、3次元線分モデルに登録してある線分が撮影画像から観察されない場合には、アウトライヤとして排除する処理しか行わない。そのため、3次元線分モデル自体が不正確である場合など、誤対応として排除される特徴が多くある場合には、位置姿勢推定が不安定になるという課題がある。また、非特許文献4に提案されているような、線分周辺の見えを利用した対応付けロバスト化手法でも、3次元線分モデルに対応付く特徴が撮影画像から検出できない場合はアウトライヤとして排除するだけである。そのため、非特許文献4の手法を用いても、3次元線分モデルが不正確な場合には、位置姿勢推定が不安定になる。
【0015】
一方、物体表面の模様などの、幾何的には検出できないが色の不連続領域としては検出できる特徴が観察対象物体に存在する場合、前述の従来手法では、このような色に基づく特徴を利用することはできず、逆に特徴が誤対応する原因となる。これに対し、非特許文献6で提案されている手法では、観察時の見た目と同様の見た目を描画できるようなテクスチャデータが設定された3次元モデルデータを利用することで、色に基づく特徴を含めて3次元線分モデルを生成し位置姿勢推定を行っている。しかし、非特許文献6の手法は、観察時の見た目と同様の見た目を描画できるようなテクスチャデータが設定された3次元モデルデータを利用することを前提としている。したがって、3次元モデルデータの設定が不正確、あるいは、実環境に光源変化があるような場合などの3次元モデルデータと実際の観察対象物体の見た目が異なる場合には、生成される3次元線分モデルが不正確になる。そのため、観察対象物体の見た目に合わせて、同様な見た目になるように3次元モデルデータを作成する必要があり、また、実環境が変化した場合には対応できないという課題がある。
【0016】
また、非特許文献3で提案されている手法では、観察対象物体を撮像した撮影画像から色に基づく特徴を随時抽出し、3次元線分モデルに加えて、抽出した特徴も利用して位置姿勢推定を行っている。しかし、この手法は、3次元線分モデルとともに、撮影画像から抽出した特徴を併用することに焦点を当てた手法であり、3次元線分モデルは、十分に特徴として観察されるエッジのみから構成されていることを前提としている。そのため、3次元線分モデルが不正確な場合、上記手法と同様に、3次元線分データと撮影画像中のエッジとの間に誤対応が頻出し、位置姿勢推定が不安定になるという課題がある。
【0017】
また、非特許文献7で提案されているような、純粋に撮影画像のみから3次元線分モデルを生成し位置姿勢推定する手法では、位置姿勢推定の精度が十分ではないという課題がある。3次元モデルデータを利用して3次元線分モデルを生成する手法と比較して、3次元線分モデルの精度が低くなってしまうためである。
【0018】
本発明は、以上の問題を鑑みてなされたものであり、観察対象物体の3次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別することにより、位置姿勢検出処理の安定化、効率化を図ることを目的とする。また、観察対象物体の3次元モデルデータから抽出した特徴が、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定可能とすることを目的とする。
【課題を解決するための手段】
【0019】
上記の目的を達成するための本発明の一態様による位置姿勢計測装置は以下の構成を備える。即ち、
撮像装置が撮像した画像を入力する入力手段と、
観察対象物体の表面形状を表す3次元モデルデータを格納する格納手段と、
前記3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出手段と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別手段と、
前記幾何特徴選別手段で選別された幾何特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出手段とを備える。
【0020】
また、本発明の他の態様による位置姿勢計測装置は、更に、
前記参照画像から、明度または色に基づいて前記観察対象物体に関連する複数の画像特徴を抽出する画像特徴抽出手段と、
前記画像特徴抽出手段で抽出した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合する統合手段とを更に備え、
前記算出手段は、前記物体特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する。
【0021】
また、上記の目的を達成するための本発明の他の態様による位置姿勢計測方法は、
撮像装置が撮像した画像を入力する入力工程と、
観察対象物体の表面形状を表す3次元モデルデータを格納した格納手段より前記3次元モデルデータを読み出し、前記3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出工程と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別工程と、
前記幾何特徴選別工程で選別された幾何特徴を前記入力工程で入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出工程とを有する。
【発明の効果】
【0022】
本発明によれば、観察対象物体の3次元モデルデータから抽出した特徴のうち、位置姿勢の算出に用いる特徴を適切に選別するので、位置姿勢検出処理の安定化、効率化を図ることができる。また、観察対象物体の3次元モデルデータから抽出した特徴が、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定することができる。
【発明を実施するための最良の形態】
【0023】
以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
【0024】
[第1実施形態]
本実施形態では、本発明を、画像上のエッジを利用した位置合わせ手法に適用した場合について説明する。
【0025】
図1は、第1実施形態における位置姿勢計測装置1の機能構成を示している。なお、位置姿勢計測装置1は、例えば一般的なコンピュータ装置に以下で説明する機能を実現するためのソフトウエアを実行させることによっても実現され得る。この場合、以下に説明する各機能構成の一部或いは全てが、コンピュータ装置のCPUによって実行されることになる。なお、第2、第3実施形態についても同様である。
【0026】
図1に示したように、位置姿勢計測装置1は、画像取得部110、モデル保存部120、参照画像保存部130、幾何特徴抽出部140、色特徴抽出部150、画像特徴検出部160、位置姿勢算出部170を有する。画像取得部110は撮像装置100に接続されている。撮像装置100は、動画撮影が可能であり、観察対象物体10を撮影する。位置姿勢計測装置1は、観察対象物体の3次元モデルデータ20に記述された観察対象物体10と撮像装置100との相対的な位置姿勢を、撮像装置100により得られた撮像画像の情報を用いて求める。なお、本実施形態において位置姿勢計測装置1による位置姿勢計算処理が適用できる条件として、撮像装置100が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置1を構成する各部について説明する。
【0027】
画像取得部110は、撮像装置100によって撮像された画像を位置姿勢計測装置1に入力する。画像取得部110は、撮像装置の出力がNTSCなどのアナログ出力であればアナログビデオキャプチャボードによって実現される。また撮像装置の出力がIEEE1394などのデジタル出力であれば、例えばIEEE1394インタフェースボードによって実現される。また、予め記憶装置に記憶してある静止画像や動画像のデジタルデータを読み出してもよい。
【0028】
モデル保存部120は、位置姿勢計測の基準となる観察対象物体10の表面形状を表す3次元モデルデータ20を記憶する。3次元モデルデータ20は、観察対象物体の3次元の幾何的な情報を有するモデルであり、頂点情報及び各頂点を結んで構成される面の情報によって定義される。位置姿勢計測装置1では、観察対象物体の3次元形状を表す面の情報を含んでいれば良いので、3次元モデルデータとしては、面の情報がパラメトリックに記述されたソリッドモデルでも良いし、ポリゴンモデルでも良い。また、任意の3次元モデラーのフォーマットを変換する処理部を位置姿勢計測装置1に含むようにしても構わない。なお、本実施形態では、頂点情報及び各頂点を結んで構成される面の情報を含むポリゴンモデルを3次元モデルデータとして利用する。
【0029】
参照画像保存部130は、過去フレームにおいて観察対象物体が撮像された撮影画像と、撮像されたときの観察対象物体と撮像装置との相対的な位置姿勢とを保存する。参照画像保存部130で参照画像として保持する撮影画像としては、直前のフレームの撮影画像のみとしても良いし、過去フレームの複数の撮影画像としても良いし、また、過去フレームにおいて位置姿勢推定精度の高かったフレームの撮影画像のみとしても良い。即ち、撮影画像中に撮像される観察対象物体と撮像装置との相対位置姿勢が既知の撮影画像であれば、数を問わず、またいずれの撮影画像でもよい。本実施形態では直前フレームの撮影画像を参照画像として保持するものとする。
【0030】
幾何特徴抽出部140は、モデル保存部120に保存された観察対象物体10の3次元モデルデータ20を画像平面上に描画することにより観察対象物体10の幾何的な情報に基づく特徴(以後、幾何特徴)を抽出する。なお、3次元モデルデータ20の描画においては、撮像装置100の位置姿勢として、初期値として設定された位置姿勢、或いは撮像装置100に関して算出されている現在の位置姿勢が用いられる。そして、幾何特徴抽出部140は、抽出した幾何特徴の中で、撮像装置により撮像された撮影画像中から検出が期待できる幾何特徴のみを、参照画像保存部130に保持されている位置姿勢が既知の画像(参照画像)を参照して選別する。幾何特徴の抽出方法および選別方法については後述する。
【0031】
色特徴抽出部150は、参照画像保存部130に参照画像として保持されている過去フレームの撮影画像中の観察対象物体の画像から、観察対象物体の明度や色の少なくともいずれかに基づく画像特徴(以後、色特徴という)を抽出する。そして、色特徴抽出部150は、参照画像保存部130に保存された観察対象物体の位置姿勢の情報と、モデル保存部120に保存された3次元モデルデータ20に基づいて、参照画像が保存された位置姿勢から観察される観察対象物体の幾何特徴を抽出する。そして、色特徴抽出部150は、抽出した色特徴と幾何特徴とを比較し、対応する幾何特徴が検出された色特徴を除去することで、色に基づいて抽出される特徴のみを選別する。色に基づく特徴の抽出方法については後述する。
【0032】
画像特徴検出部160は、画像取得部110によって入力された画像上において、撮像装置と観察対象物体との位置姿勢を算出するのに用いられる画像特徴を検出する。本実施形態では、画像特徴検出部160は画像上のエッジの検出を行う。エッジの検出方法については後述する。
【0033】
概略位置姿勢を反復演算の初期値として、撮像装置と観察対象物体との相対的な位置姿勢を算出する。より具体的には、位置姿勢算出部170は、
・画像特徴検出部160が検出した画像特徴の情報と、
・幾何特徴抽出部140が抽出した幾何特徴と、
・色特徴抽出部150が抽出した色特徴とに基づいて、観察対象物体10を基準とした座標系(以下、基準座標系)における撮像装置100の位置及び姿勢を算出する。
【0034】
次に、第1実施形態における位置姿勢計測方法の処理手順について説明する。図2は、第1実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【0035】
(ステップS1010)
まずステップS1010では初期化を行う。ここでは、位置姿勢計測装置1のCPUが、基準座標系における撮像装置100と観察対象物体10との相対的な概略位置姿勢の設定を行う。第1実施形態における位置姿勢計測方法は、概略の撮像装置の位置姿勢を、撮影画像上に撮像される観察対象物体のエッジ情報を利用して逐次更新していく方法である。そのため、位置姿勢計測を開始する前に予め撮像装置の概略の位置及び姿勢を初期位置及び初期姿勢として与える必要がある。
【0036】
概略の位置及び姿勢を与える初期化の方法としては、例えば予め決まった位置及び姿勢を設定しておき、撮像装置をその位置及び姿勢になるように移動することで初期化を行うことが挙げられる。または、画像内で検出するだけで認識可能な人工的な指標を配置し、該指標の各頂点の画像座標と基準座標系における3次元位置との対応から撮像装置の位置姿勢を求めて概略の位置姿勢としてもよい。また、識別性の高い自然特徴点を予め検出してその3次元位置を求めておき、初期化時に画像上で該特徴点を検出し、その画像座標と3次元位置との対応から撮像装置の位置姿勢を求めてもよい。さらに、磁気式や光学式、超音波式などの6自由度位置姿勢センサによって撮像装置の位置姿勢を計測し、それを概略の位置姿勢としてもよい。人工的な指標や自然特徴点などの画像情報と、前述の6自由度位置姿勢センサや3自由度の姿勢センサ、3自由度の位置センサを併用して計測される撮像装置の位置姿勢を用いて初期化してもよい。
【0037】
(ステップS1020)
ステップS1020では、撮像装置100が撮像した画像を画像取得部110を介して位置姿勢計測装置1に取り込む。
【0038】
(ステップS1030)
次にステップS1030において、幾何特徴抽出部140は、観察対象物体の幾何的情報に基づく特徴を検出する。第1実施形態で検出する幾何的情報に基づく特徴はエッジである。図3は、本実施形態における観察対象物体の幾何特徴の検出方法について詳細な処理手順を示すフローチャートである。
【0039】
まず、ステップS1210において、幾何特徴抽出部140は、モデル描画を行う。モデル描画とは、撮像装置100に設定されている位置姿勢に基づいて、モデル保存部120に保存された観察対象物体の3次元モデルを画像平面上に投影することである。最初のモデル描画では、ステップS1010で求めた観察対象物体の概略位置姿勢(初期値)が用いられることになる。また、2回目以降の処理では、撮像装置100と観察対象物体10との間の直前に取得された相対位置姿勢が用いられる。
【0040】
モデル描画を行うためには、撮像装置100の位置姿勢とともに、射影行列の内部パラメータ(焦点距離や主点位置など)を設定する必要がある。本実施形態では、カメラの内部パラメータをあらかじめ計測しておき、実際に用いるカメラ(撮像装置100)と内部パラメータを一致させておく。また、視点からモデルまでの距離の最大値と最小値を設定しておき、その範囲外のモデルの描画は行わないことで、描画処理の計算コストを低減する。図4にステップ1210により描画された結果を示す。概略位置姿勢により3次元モデルを画像平面上に投影することにより、画像平面上での2次元座標とその輝度値、2次元座標に対応する画像平面から3次元モデルまでの奥行き値(デプス値)が格納されたデプスバッファが算出される。
【0041】
次にステップS1220において、幾何特徴抽出部140は、ステップS1210の描画過程において生成されたデプスバッファに対して、エッジ検出を行う。デプスバッファに対してエッジ検出することで、奥行きが不連続に変化する領域を求めることができる。エッジ検出では、例えば、Cannyアルゴリズムなどによってエッジが抽出され、2値画像形式として利用できるようになる。この過程において、抽出されたエッジは、画像中で白く表され、それ以外は黒く表される。
【0042】
次にステップS1230において、幾何特徴抽出部140は、ステップS1220で生成された2値化画像について、隣接エッジのラベリングを行い、エッジの連結成分を抽出する。ラベリングは、例えばある画素の周囲8画素の中にエッジが存在すれば、同一ラベルを割り当てることにより行う。
【0043】
図5は、ステップS1230において、連結成分を抽出したエッジを撮影画像上に重ねた結果を示す図である。図5(a)は撮像された画像そのものであり、図5(b)は撮影画像上に抽出したエッジを重ね合わせた図である。ステップS1010で算出した概略の位置姿勢と、実際の位置姿勢が異なる場合には、図5(b)に示すように実際に撮像される物体と抽出したエッジとの間にずれが生じる。
【0044】
次にステップS1240において、幾何特徴抽出部140は、ステップS1230で連結成分を抽出したエッジに対して、エッジ素の抽出を行う。エッジ素とは、画像上での端点間の距離が極短いエッジのことをいう。同一ラベルにラベリングされたエッジを画像上で等間隔に分割するように分割点を算出して、この分割点の周辺の極短い連結成分を求めることでエッジ素を抽出する。図6はラベリングされたエッジから等間隔に抽出したエッジ素を表す図である。本実施形態では、分割点から3pixel離れた連結成分を端点(始点と終点)に設定して、分割点を中心としたエッジ素を抽出している。デプスバッファから抽出したエッジ素の総数をNとして、各エッジ素をGfi(i=1,2…N)で表す。エッジ素の数Nが多いほど、処理時間が長くなる。そのため、画像上でのエッジ素を抽出する間隔を変えることで処理時間を制御できる。また、エッジ素の数を一定として処理時間が一定になるように、画像上でのエッジ素間の間隔を逐次変更してもよい。
【0045】
次にステップS1250において、幾何特徴抽出部140は、ステップS1240で算出されたエッジ素に対して、基準座標系における3次元座標を求める。これには、ステップS1210で生成したデプスバッファを利用する。前述の通り、デプスバッファには、画像座標に対応する奥行き値が格納されている。但し、デプスバッファに格納されている奥行き値は、ステップ1210のレンダリング過程で用いた、視点からモデルまでの距離の最大値と最小値により0から1の値に正規化されている。そのため、デプスバッファの奥行き値から直接、基準座標系における3次元座標を求めることはできない。そこで、上述の距離の最大値と最小値を利用して、まずはデプスバッファの値を、カメラ座標系における視点からエッジ素までの距離に変換する。同時に射影行列の内部パラメータを利用して、カメラ座標系におけるエッジ素の3次元座標を求める。そして、カメラ座標系における3次元座標に対して、レンダリング過程で用いた位置姿勢の逆変換をほどこすことで、基準座標系におけるエッジ素の3次元座標を求める。3次元座標が算出されたエッジ素GFiの集合を以後、幾何特徴モデルと呼ぶ。
【0046】
(ステップS1040)
次に、ステップS1040において、参照画像保存部130に、参照画像としての過去フレームの撮影画像が保存されているか否かが判定される。参照画像が保存されている場合は、ステップS1050に進み、保存されていない場合は、ステップS1050以降の過去フレームの撮影画像を利用するステップをスキップして、ステップS1080へ進む。特に最も初期のフレームについては、過去フレームの撮影画像(参照画像)が存在しないため、処理は自動的にステップS1080に進む。
【0047】
(ステップS1050)
ステップS1050において、幾何特徴抽出部140は、参照画像保存部130に保存された過去フレームの撮影画像を利用して、ステップS1030で抽出した幾何特徴の選別を行う。即ち、幾何特徴抽出部140は、観察対象物体10に対する撮像装置100の位置姿勢が算出済みとなっている(既知である)参照画像において、ステップS1030で抽出した複数の幾何特徴に対応する画像特徴を探索する。そして、対応する画像特徴が検出された幾何特徴を当該複数の幾何特徴より取り出すことにより選別が行われる。図7は、本実施形態における観察対象物体の幾何特徴の選別方法について詳細な処理手順を示すフローチャートである。
【0048】
まず、ステップS1310において、幾何特徴抽出部140は、変数iを1にセットする。処理は、ステップS1320に進む。
【0049】
ステップS1320において、幾何特徴抽出部140は、参照画像保存部130に参照画像とともに保存されている、観察対象物体と撮像装置との相対位置姿勢の情報を取得する。そして、幾何特徴抽出部140は、取得した相対位置姿勢に基づいて、ステップS1030で求めた幾何特徴モデルのエッジ素GFiの3次元座標を当該参照画像中に投影し、幾何特徴モデルのエッジ素GFiの参照画像中における2次元座標を求める。図8は、参照画像上にエッジ素を投影した図である。図8(a)は参照画像保存部130に保存された参照画像であり、図8(b)は参照画像上に投影したエッジ素を重ね合わせた図である。図8(b)に示すように、エッジ素は、参照画像保存部130に保存されている位置姿勢の情報に基づいて、参照画像上に撮像される観察対象物体と重なる位置に投影される。
【0050】
次に、ステップS1330において、幾何特徴抽出部140は、ステップS1320で求めた2次元座標の端点(始点と終点)を結んだ直線の法線方向に平行でかつエッジ素の中心点を通過する線分(以下、探索ライン)上における濃度勾配を計算する。
【0051】
次に、ステップ1340において、幾何特徴抽出部140は、濃度勾配の極値が存在するかを算出する。図9は、本実施形態における特徴判別を説明する図である。図9に示される下側の2つのエッジ素のように、影等によりエッジ素の近傍の探索ライン上に極値がない場合は、撮影画像中から検出できない幾何特徴であると見なして、ステップ1350において、エッジ素GFiを除去し、幾何特徴モデルの総数Nから1減分する。図9上に示される、上側3つのエッジ素のように、近傍に極値がある場合は、撮影画像から検出できる幾何特徴であると見なして、ステップ1360に進む。本実施形態では、エッジ素の近傍を、中心点から探索ライン両端方向に5[pixel]以内の範囲と定義する。
【0052】
次にステップS1360において、幾何特徴抽出部140は、変数iを1増分し、処理はステップS1370に進む。ステップS1370において、幾何特徴抽出部140は、全てのエッジ素GFiについて上記処理が終了したか否かを判断する。全てのエッジ素GFiについて上記処理が終了している場合には、当該判別処理を終了し、終了していない場合は処理はステップS1320に戻る。以上の処理により、幾何特徴モデルの中で、参照画像から検出されない可能性が高いエッジ素が除去される。除去されなかったエッジ素は、撮影画像から検出が期待できるエッジとして選別されたことになる。
【0053】
(ステップS1060)
次にステップS1060において、色特徴抽出部150は、参照画像保存部130において保存されている過去フレームの撮影画像(参照画像)から観察対象物体の色情報に基づく特徴(画像特徴)を検出する(画像特徴抽出処理)。図10は、本実施形態における前フレームの撮影画像(参照画像)からの、観察対象物体の色に基づく特徴の検出方法について詳細な処理手順を示すフローチャートである。
【0054】
まず、ステップS1410において、色特徴抽出部150は、参照画像保存部130に保存された参照画像からエッジを抽出する。参照画像に対してエッジ検出することで、輝度値が不連続に変化する領域を求めることができる。エッジの検出方法は、ステップS1220で記載した処理と同様である。ただし、ステップS1220の処理と異なり、参照画像を対象としてエッジ検出を行う。
【0055】
次に、ステップS1420において、色特徴抽出部150は、ステップS1410で抽出したエッジのラベリングを行い、エッジの連結成分を抽出する。ラベリング方法は、ステップS1230で記載した処理と同様である。
【0056】
次に、ステップS1430において、色特徴抽出部150は、ステップS1420で抽出したエッジの連結成分からエッジ素を抽出する。エッジ素の抽出方法は、ステップS1240で記載した処理と同様である。
【0057】
次に、ステップS1440において、色特徴抽出部150は、ステップS1430で抽出したエッジ素の基準座標上の3次元座標を計算する。まず、色特徴抽出部150は、参照画像保存部130に保存された観察対象物体と撮像装置との相対位置姿勢に基づいて、モデル保存部120に保存されている3次元モデルデータ20を描画し、参照画像のデプスバッファを取得する。そして、色特徴抽出部150は、取得したデプスバッファを利用して、エッジ素の3次元座標を計算する。デプスバッファからのエッジ素の3次元座標の算出方法は、ステップS1250で記載した処理と同様である。以上の処理により、基準座標系における3次元座標が算出された参照画像の色の境界に基づくエッジ素CFjの集合を以後、色特徴モデルと呼ぶ。
【0058】
(ステップS1070)
次に、ステップS1070において、色特徴抽出部150は、ステップS1440で生成したデプスバッファを利用して、ステップS1060で抽出した色特徴の選別(画像特徴選別処理)を行う。図11は、本実施形態における観察対象物体の色特徴モデルのエッジ素CFiの選別方法について詳細な処理手順を示すフローチャートである。
【0059】
まず、ステップS1510において、色特徴抽出部150は、変数jを1にセットし、処理はステップS1520に進む。
【0060】
次に、ステップS1520において、色特徴抽出部150は、ステップS1430で求めた、色の特徴に基づくエッジ素CFiの探索ライン上における、ステップS1440で求めたデプスバッファ上のデプス値の勾配を計算する。
【0061】
次に、ステップS1530において、色特徴抽出部150は、ステップS1520で求めたデプスバッファ上の濃度勾配から極値が検出されるかを判別する。エッジ素の近傍でデプスバッファ上から極値が検出された場合、そのエッジ素は幾何的にも抽出されるエッジ素であると見なして、ステップS1540において、エッジ素CFjを除去する。エッジ素の近傍でデプスバッファ上から極値が検出されない場合は、色特徴抽出部150は、当該エッジ素は物体表面の色に基づくエッジであると見なして、処理をステップS1550に進める。
【0062】
次にステップS1550において、色特徴抽出部150は、変数jを1増分し、処理をステップS1560に進める。ステップS1560において、全てのエッジ素CFjについて処理が終了しているどうかが判断される。全てのエッジ素について処理が終了している場合には、当該判別処理を終了し、終了していない場合は処理をステップ1320に戻す。以上の処理により、色特徴モデルの中で、幾何的にも検出できるエッジ素が除去され、物体表面の色の境界に基づくエッジのみが選別される。
【0063】
(ステップS1080)
次にステップS1080において、画像特徴検出部160は、画像特徴選別処理で選別した色特徴と、幾何特徴選別処理で選別した幾何特徴とを物体特徴として統合する。より具体的には、画像特徴検出部160は、ステップS1030で生成した幾何特徴モデルとステップS1060で生成した色特徴モデルを統合した物体特徴モデルの生成を行う。物体特徴モデルは、幾何特徴モデルのエッジ素と色特徴モデルのエッジ素の集合であり、総数をLとし、各エッジ素をOFk(k=1,2,…,L)で表す。エッジ素の総数Lは、幾何特徴モデルの総数Mと色特徴モデルの総数Nを足したものである。但し、本実施形態では、幾何特徴モデルの総数Mと色特徴モデルの総数Nは、それぞれステップS1050、S1070の選別処理によって選別された数となっている。図12は、本実施形態における観察対象物体の幾何特徴モデルと色特徴モデルの統合方法について詳細な処理手順を示すフローチャートである。
【0064】
まず、ステップS1610において、画像特徴検出部160は、ステップS1030で生成され、ステップS1050で選別されたエッジ素で構成された幾何特徴モデルを物体特徴モデルに登録する。物体特徴モデルの総数Lには幾何特徴モデルの総数Mが加えられる。
【0065】
次に、ステップS1620において、画像特徴検出部160は、ステップS1060において色特徴モデルが生成されている場合は、ステップS1630に進み、生成されていない場合は、処理を終了する。
【0066】
次に、ステップS1630において、画像特徴検出部160は、ステップS1010で求めた概略位置姿勢に基づいて、色特徴モデルの3次元座標を投影し、現フレームの撮影画像上での画像座標を算出する。幾何特徴モデルの3次元座標に対応する撮影画像上での画像座標は、ステップS1030中のモデル描画・エッジ素抽出処理により既に算出されている。
【0067】
次に、ステップS1640では、ステップS1630で、画像座標を求めた色特徴モデルを物体特徴モデルに登録し、物体特徴モデルの総数Lに色特徴モデルの総数Lを加える。
【0068】
(ステップS1090)
ステップ1090において、画像特徴検出部160は、ステップS1090で求めた物体特徴モデルのエッジ素OFk(k=1,2,…,L)に対応する、撮像装置100によって撮像された現フレーム撮影画像の画像特徴の検出を行う。本実施形態における画像特徴はエッジである。図13は、本実施形態におけるエッジ検出方法の詳細な処理手順を示すフローチャートである。図14は、物体特徴モデルのエッジ素OFkを撮影画像上に重ねた結果を示す図である。図14(a)は撮像された画像そのものであり、図14(b)は物体特徴モデルのエッジ素を重ね合わせた図である。ステップS1330やS1520と同様に、エッジ素の画像座標周辺のエッジを撮影画像から抽出する。
【0069】
まず、ステップS1710において、画像特徴検出部160は、変数kを1にセットし、処理素ステップS1720に進める。
【0070】
次に、ステップS1720において、画像特徴検出部160は、物体特徴モデルのエッジ素OFkに対応するエッジを画像から検出する。エッジ素OFkの探索ライン(エッジ素の法線方向の線分)上において、撮影画像上の濃度勾配から極値を算出する。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。本実施形態では、探索ライン上でエッジが複数存在する場合には、最もエッジ素の中心点に近いエッジを対応点とし、その画像座標とエッジ素OFkの3次元座標を保持する。なお、本実施形態では最もエッジ素OFkに近いエッジを対応点としているが、これに限るものではなく、濃度勾配の極値の絶対値が最も大きいエッジを対応点としてもよい。また、非特許文献2に示すように、1つではなく複数の点を対応点候補として保持しても本発明の本質が損なわれることはない。
【0071】
ステップS1730において、画像特徴検出部160は、変数kを1増分し、処理をステップS1740に進める。すべてのエッジ素OFkについて処理が終了している場合には当該処理を終了する。一方、すべてのエッジ素OFkについて処理が終了していない場合にはステップS1720に処理を戻し、上記処理が繰り返される。
【0072】
(ステップS1100)
ステップS1100において、位置姿勢算出部170は、撮像装置100の観察対象物体10に対する相対的な位置姿勢を算出する。相対的な位置姿勢は、幾何特徴選別処理で選別された幾何特徴と画像特徴選別処理で選別された色特徴を統合して得られた物体特徴を、現フレームにおける観察対象物体の画像に対応付けることにより算出される。より、具体的には、位置姿勢算出部170は、非線形最適化計算を用いて、撮像装置と観察対象物体との概略の相対的な位置姿勢を反復演算により補正することにより撮像装置の位置姿勢を算出する。ここで、ステップS1080において生成された物体特徴モデルのエッジ素OFkのうち、ステップS1090において対応点が求まったエッジ素の総数をLcとする。図15は、エッジの情報を利用して撮像装置の位置及び姿勢を算出する方法を説明する図である。図15では、画像の水平方向、垂直方向をそれぞれx軸、y軸としている。あるエッジ素の中心点の投影された画像座標を(u0,v0)、エッジ素の直線の画像上での傾きをx軸に対する傾きθと表す。傾きθは、エッジ素端点(始点と終点)の撮影画像上での2次元座標を結んだ直線の傾きとして算出する。エッジ素の直線の画像上での法線ベクトルは(sinθ, −cosθ)となる。また、該エッジ素の対応点の画像座標を(u’, v’)とする。
【0073】
ここで、点(u , v)を通り、傾きがθである直線の方程式は、
【数1】
と表せる。
【0074】
エッジ素の撮影画像上での画像座標は撮像装置の位置及び姿勢により変化する。また、撮像装置の位置及び姿勢の自由度は6自由度である。ここで撮像装置の位置及び姿勢を表すパラメータをsで表す。sは6次元ベクトルであり、撮像装置の位置を表す3つの要素と、姿勢を表す3つの要素からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す3次元ベクトルなどによって表現される。エッジ素中心点の画像座標(u , v)は(u0 , v0)の近傍で1次のテイラー展開によって数3のように近似できる。
【0075】
【数2】
【0076】
上記数3におけるu、vの偏微分の導出方法は例えば非特許文献8に開示されるように広く知られているのでここではその詳細は述べない。数2を数1に代入することにより、数3が得られる。
【0077】
【数3】
【0078】
ここで、数3に示す直線が該エッジ素の対応点の画像座標(u’, v’)を通過するように、撮像装置の位置及び姿勢sの補正値Δsを算出する。r0=u0sinθ-v0cosθ(定数)、d=u’sinθ-v’ cosθ(定数)とすると、
【数4】
が得られる。数4はLc個のエッジ素について成り立つため、数5のようなΔsに対する線形連立方程式が成り立つ。
【0079】
【数5】
【0080】
ここで数5を数6のように簡潔に表す。
【数6】
【0081】
数6をもとにGauss−Newton法などによって、行列Jの一般化逆行列(JT・J)−1を用いてΔsが求められる。しかしながら、エッジの検出には誤検出が多いので、次に述べるようなロバスト推定手法を用いる。一般に、誤検出されたエッジに対応するエッジ素では誤差d−rが大きくなる。そのため数5、数6の連立方程式に対する寄与度が大きくなり、その結果得られるΔsの精度が低下してしまう。そこで、誤差d−rが大きいエッジ素のデータには小さな重みを与え、誤差d−rが小さいエッジ素のデータには大きな重みを与える。重みは例えば数7Aに示すようなTukeyの関数により与える。
【0082】
【数7A】
【0083】
cは定数である。なお、重みを与える関数はTukeyの関数である必要はなく、例えば次式(数7B)で示されるようなHuberの関数など、誤差d−rが大きいエッジ素には小さな重みを与え、誤差d−rが小さいエッジ素には大きな重みを与える関数であればなんでもよい。
【0084】
【数7B】
【0085】
エッジ素OFkに対応する重みをwiとする。ここで数8のように重み行列Wを定義する。
【数8】
【0086】
重み行列Wは、対角成分以外はすべて0のLc×Lc正方行列であり、対角成分には重みwiが入る。この重み行列Wを用いて、数6を数9のように変形する。
【0087】
【数9】
【0088】
数10のように数9を解くことにより補正値Δsを求める。
【0089】
【数10】
【0090】
これにより得られたΔsを用いて、撮像装置の位置及び姿勢を更新する。次に、撮像装置の位置及び姿勢の反復演算が収束しているかどうかを判定する。補正値Δsが十分に小さかったり、誤差r−dの総和が十分小さい、誤差r−dの総和が変化しないといった場合には、撮像装置の位置及び姿勢の計算が収束したと判定する。収束していないと判定された場合には、更新された撮像装置の位置及び姿勢を用いて再度線分の傾きθ、r0、d及びu、vの偏微分を計算し直し、数10より再度補正値Δsを求め直す。なお、ここでは非線形最適化手法としてGauss−Newton法を用いた。しかしながら、非線形最適化手法はこれに限るものではなく、Newton−Raphson法、Levenberg−Marquardt法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。以上、ステップS1100における撮像装置の位置姿勢算出方法について説明した。
【0091】
(ステップS1110)
ステップS1110において、位置姿勢算出部170は、位置姿勢算出を終了する入力がなされたかどうかを判定し、入力された場合には当該処理を終了し、入力されなかった場合には処理をステップS1120に進める。
【0092】
(ステップS1120)
ステップS1120において、位置姿勢算出部170は、位置姿勢の算出結果と位置姿勢計測の対象にした撮影画像を参照画像保存部130に参照画像として保存する。その後、処理はステップS1020に戻り、新たな画像の取得と、再度の位置姿勢算出が行われる。
【0093】
以上述べたように、第1実施形態では、
・随時撮影画像上で撮像される観察対象物体から色に基づく画像特徴を抽出し、
・これと同時に、観察対象物体の3次元モデルから抽出した幾何情報のうち撮影画像から検出されるものを選別し、
・観察対象物体の色に基づく画像特徴と選別された幾何情報を統合して位置姿勢計測に利用する。このため、第1実施形態によれば、3次元モデルから得られる特徴と撮影画像から得られる特徴が大きく異なる場合や、撮像の途中に光源変化があった場合にも、観察対象物体と撮像装置との相対位置姿勢を安定かつ高精度に算出することができる。
【0094】
[変形例1−1]
上述の第1実施形態では、ステップS1060の色特徴抽出処理において、参照画像中に撮像された観察対象物体のデプスバッファを算出するために、参照画像保存部130に保存されている参照画像の位置姿勢を利用して3次元モデルデータを描画した。しかし、ステップS1030の幾何特徴抽出処理で利用する概略位置姿勢として、前フレームの位置姿勢を利用する場合には、参照画像部に保存された位置姿勢と、ステップS1030で利用する概略位置姿勢は同じとなる。
【0095】
したがって、ステップS1060の色特徴抽出処理では、ステップS1030で算出したデプスバッファをそのまま利用することができ、3次元モデルデータの描画を行わなくてすむ。これにより、色特徴抽出処理で3次元モデルデータの描画処理を行う必要が無くなり、計算コストを減少させることが可能である。
【0096】
[変形例1−2]
上述の第1実施形態において、ステップS1060とステップS1070の色特徴抽出処理及び色特徴選別処理を行わず、幾何特徴のみを利用して位置姿勢推定を行うようにしても良い。この場合、色特徴抽出処理を省略し、幾何情報のみを利用した位置姿勢推定が行われることになる。即ち、位置姿勢算出部170は、幾何特徴抽出部140により選別された幾何特徴を、入力された画像中の観察対象物体10の画像に対応付けることにより、撮像装置100の観察対象物体10に対する位置姿勢を算出する。この構成によれば、テクスチャのない一色の物体のような、色に基づく特徴が検出されない観察対象物体に関しては、位置姿勢推定に利用する情報を損失することなく、色特徴抽出処理を省略する分の計算コストを減少させることが可能である。また、色特徴選別処理(ステップS1070)を行わず、ステップS1060で抽出された色特徴は全て用いるようにしてもよい。但し、この場合、現フレーム中の画像における同一の画像特徴が、色特徴と幾何特徴の両方に関連して検出される可能性が大きくなるため、処理効率が下がる可能性がある。
【0097】
[第2実施形態]
第1実施形態では、位置姿勢推定後、参照画像保存部130に撮影画像を毎フレーム保存し、過去フレームの撮影画像として常に直前のフレームの撮影画像を利用していた。それに対して、第2実施形態では、参照画像保存部に保存した撮影画像を上書きせず、複数の撮影画像を保存しておき、複数の参照画像を利用して位置姿勢推定を行う方法について述べる。
【0098】
図16は、第2実施形態における位置姿勢計測装置2の構成を示している。同図に示したように、位置姿勢計測装置2は、画像取得部210、モデル保存部220、参照画像保存部230、参照画像選択部240、幾何特徴抽出部250、色特徴抽出部260、画像特徴検出部270、位置姿勢算出部280を有する。画像取得部210は撮像装置200に接続されている。撮像装置200は、観察対象物体30を撮影する。位置姿勢計測装置2は、3次元モデルデータ40に記述された観察対象物体30と撮像装置200との相対的な位置姿勢を、撮像装置200により得られた撮像画像の情報を用いて求める。なお、第2実施形態において位置姿勢計測装置2が適用できる条件として、撮像装置200が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置2を構成する各部について説明する。
【0099】
画像取得部210、モデル保存部220、画像特徴検出部270、位置姿勢算出部280は、それぞれ第1実施形態における画像取得部110、モデル保存部120、画像特徴検出部160、位置姿勢算出部170と同様の処理を行う。
【0100】
参照画像保存部230は、過去フレームにおいて撮像装置200によって観察対象物体30が撮像された撮影画像と、撮像されたときの観察対象物体と撮像装置との相対位置姿勢を保存する。同時に、参照画像保存部230は、保存する撮影画像から抽出した色特徴(本実施形態では色の境界に基づくエッジ)も基準座標系における3次元座標とともに保存しておく。第2実施形態では、あらかじめ定められた保存条件を満足する過去フレームの撮影画像が参照画像保存部230に保存される。保存条件の例としては、
・位置姿勢の推定精度が一定以上高いこと、
・保存してある参照画像から十分離れていること、即ち、保存してある参照画像に対応する位置姿勢と現フレームに関連する位置姿勢の差が所定値より大きいこと、
・以前に参照画像を保存したときから一定フレーム(所定の時間)を経過していること、
等が挙げられる。
【0101】
参照画像選択部240は、参照画像保存部230に保存された複数の参照画像から、以後のステップ(幾何特徴抽出処理)で利用する参照画像を選択する。第2実施形態では、撮像装置200に設定されている現在の位置姿勢に最も位置姿勢が近い参照画像が選択され、以後のステップの処理に利用される。
【0102】
幾何特徴抽出部250は、モデル保存部220に保存された観察対象物体30の3次元モデルデータ40と概略位置姿勢から、観察対象物体の幾何的な情報に基づく特徴を抽出する。また、幾何特徴抽出部250は、参照画像選択部240で選択された参照画像を利用して、3次元モデルデータ40から抽出した幾何特徴の中で、撮像装置200により撮像された撮影画像中から検出が期待できる特徴のみを選別する。
【0103】
色特徴抽出部260は、位置姿勢が推定された撮影画像を利用して、撮影画像中に撮像される観察対象物体の色に基づく特徴を抽出する。また、色特徴抽出部260は、推定された位置姿勢とモデル保存部220に保存された3次元モデルデータ40に基づいて、当該位置姿勢から観察される観察対象物体の幾何特徴を抽出して、比較することで、色に基づくエッジのみを抽出する。同時に、色特徴抽出部260は、色に基づくエッジの基準座標における3次元座標を求める。求めた色特徴は、参照画像保存部230にて保存される。
【0104】
次に、第2実施形態における位置姿勢計測方法の処理手順について説明する。図17は、第2実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【0105】
(ステップS2010)
ステップS2010では初期化を行う。ステップS2010の処理内容は基本的に第1実施形態におけるステップS1010と同様であるので。
【0106】
(ステップS2020)。
【0107】
ステップS2020の処理は第1実施形態におけるステップS1020の処理と同様である。
【0108】
(ステップS2030)
ステップS2030において、幾何特徴抽出部250は、観察対象物体30の3次元モデルデータ40に基づいて、幾何的情報に基づく特徴を検出する。ステップS2030の処理内容は、基本的に第1実施形態におけるステップS1030の処理と同様である。
【0109】
(ステップS2040)
参照画像保存部に過去フレームの撮影画像が保存されている場合は、処理はステップS2040からステップS2050に進む。一方、保存されていない場合は、処理は、ステップS2050以降の過去フレームの撮影画像を利用するステップをスキップして、ステップS2070へ進む。
【0110】
(ステップS2050)
ステップS2050において、参照画像選択部240は、参照画像保存部230に保存されている複数の参照画像から、適切な参照画像を選択する。本実施形態では、参照画像の選択に、現在の位置姿勢と参照画像の位置姿勢の差を利用する。なお、本実施形態では、現在の位置姿勢として、直前フレームの撮影画像から推定した位置姿勢を用いるものとする。参照画像選択部240は、現在の位置姿勢と参照画像保存部230に保存してある各参照画像の位置姿勢とを比較して、現在の位置姿勢に最も近い参照画像を選択する。ステップS2050において選択された参照画像は、以後のステップにおいて参照画像として利用される。
【0111】
(ステップS2060)
次に、ステップS2060において、幾何特徴抽出部250は、ステップS2030で抽出した幾何特徴の選別を行う。ステップS2060の処理内容は、基本的に第1実施形態におけるステップS1050の処理と同様である。但し、参照画像としてステップS2050で選択した参照画像を利用する点がステップS1050と異なる。
【0112】
(ステップS2070)
次にステップS2070において、画像特徴検出部270は、ステップS2050で生成した幾何特徴モデルと、ステップS2050で選択した参照画像に対応する色特徴モデルを利用して、幾何特徴モデルと色特徴モデルを統合した物体特徴モデルを生成する。ステップS2070の処理内容は、基本的に第1実施形態におけるステップS1080の処理と同様である。ステップS1080と異なる点は、ステップS2070で利用する色特徴モデルはステップS2050で選択した参照画像に対応する色特徴モデルを利用する点である。なお、選択した参照画像に対応する色特徴モデルは、参照画像が保存される過程のステップS2130で生成され、参照画像保存部230に保存されている(後述)。
【0113】
(ステップS2080)
ステップS2080において、画像特徴検出部270は、ステップS2080で求めた物体特徴モデルのエッジ素に対応する画像特長を、撮像装置200によって撮像された現フレームの撮影画像から検出する。ステップS2080の処理内容は、基本的に第1実施形態におけるステップS1090の処理と同様である。
【0114】
(ステップS2090)
ステップS2090において、位置姿勢算出部280は、ステップS2070で生成した物体特徴モデルと、ステップS2080で対応付けた画像特徴の位置情報から、撮像装置200の位置姿勢を算出する。ステップS2090の処理内容は、基本的に第1実施形態におけるステップS1100の処理と同様である。
【0115】
(ステップS2100)
ステップS2100において、位置姿勢計測装置2は、位置姿勢算出を終了する指示の入力がなされたかどうかを判定し、終了する指示が入力された場合には本処理を終了する。一方、終了する指示が入力されなかった場合には、処理をステップS2110に進める。
【0116】
(ステップS2110)
ステップS2110において、参照画像保存部230は、位置姿勢推定した撮影画像を参照画像として保存するかを判定する。保存する場合は、処理をステップS2120に進める。また、保存しない場合は、処理をステップS2020に戻し、新たな画像を取得して、上述の位置姿勢算出を行う。第2実施形態では、参照画像として保存する条件は、本例では、保存しようとする撮影画像が参照画像保存部230に保存されている参照画像の位置姿勢から一定以上離れており、かつ、最近に保存した参照フレームから一定フレーム経過していることとする。
【0117】
(ステップS2120)
ステップS2120において、色特徴抽出部260は、ステップS2090で推定された位置姿勢を利用して、ステップS2110で保存すると判定された撮影画像から観察対象物体の色情報に基づく特徴を検出し、色特徴モデルを生成する。なお、色特徴モデルの生成方法は、基本的に第1実施形態におけるステップS1060の処理と同様である。
【0118】
(ステップS2130)
次に、ステップS2130において、色特徴抽出部260は、ステップS2120で抽出した色特徴モデルの選別を行う。ステップS2130の処理内容は、基本的に第1実施形態におけるステップS1070の処理と同様である。
【0119】
(ステップS2140)
次に、ステップS2140において、参照画像保存部230は、ステップS2120で生成した色特徴モデルとステップS2090で算出した観察対象物体の位置姿勢と、位置姿勢算出の対象とした撮影画像とを保存する。その後、ステップS2020に戻り、新たな画像を取得して、再度位置姿勢算出を行う。
【0120】
以上述べたように、第2実施形態では、位置姿勢の異なる複数の参照画像を保持しつづけることで、毎フレーム撮影画像を参照画像として保存する必要を無くすことができる。また、該参照画像から抽出した色特徴モデルを参照画像保存部230に保持することにより、毎フレームについて色特徴を算出する必要がなくなる。さらに、複数の参照画像から、現在の観察対象物体の位置姿勢に最も近い位置姿勢の参照画像とその色特徴モデルを利用することにより、計算コストを抑えながら、幾何特徴と色特徴を統合利用した位置姿勢推定が可能である。
【0121】
[変形例2−1]
上述の第2実施形態では、参照画像として保存する撮影画像は、前回保存したときよりも一定フレーム過ぎており、かつ、保存してある撮影画像の位置姿勢と現在の位置姿勢が一定以上離れている場合に、無条件で保存していた。しかしながら、保存する位置姿勢の推定精度が低いような場合、位置姿勢の推定精度の低い参照画像から色特徴モデルを生成することになる。そのため、不正確な特徴モデルを利用することによる位置姿勢推定の精度低下が発生する。そこで、参照画像として保存するときに、位置姿勢の推定精度も算出して、推定精度が一定以上の場合のみ参照画像として保存するという制限を加える。これにより、位置姿勢の推定が不安定な場合でも、一定以上推定精度が高い位置姿勢とそのときの撮影画像のみが参照画像として保存されるため、安定した位置姿勢推定が可能となる。推定精度算出方法としては、例えば、非特許文献6で開示されているような位置姿勢推定の破綻検出手法を用いればよい。
【0122】
[変形例2−2]
上述の第2実施形態では、一旦、参照画像として保存した撮影画像は、本位置姿勢計測装置が動作している間ずっと保存されていた。しかし、撮像の途中で、光源環境が変化して、観察対象物体の色合いが変化することも考えられる。
【0123】
そこで、ステップS2140において、撮影画像を参照画像として保存するときに、時刻情報を付加しておき、一定時間が過ぎた参照画像は削除するようにしてもよい。即ち、時系列的に古い画像特徴を判別して、時系列的に画像特徴を随時更新していくようにしてもよい。これにより、時系列的に参照画像が更新されていき、撮影環境に変化が生じた場合にも柔軟に対応することが可能になる。また、単純に一定時間過ぎた参照画像を削除するのではなく、一定時間過ぎた参照画像は上書きフラグをONにしておき、撮影画像を参照画像として保存する際に、当該参照画像で上書きフラグがONの参照画像を上書きするようにしてもよい。更に、撮影画像を参照画像として保存する際に、保存する撮影画像の位置姿勢と上書きフラグがONになった参照画像の位置姿勢とを比較して、両者が十分に近い場合に、その参照画像に上書きするようにしてもよい。このようにすれば、一定時間位置姿勢が変化しなかったときに、有用な参照画像が削除されることを防ぐことができる。また、参照画像として保持する撮影画像の枚数を一定数に設定しておき、保存された時刻情報が一番古い参照画像から上書きすることにしてもよい。
【0124】
[変形例2−3]
上述の第2実施形態では、参照画像として保存する処理は、位置姿勢の算出が終わったあとステップS2110の参照画像保存判定後に行っていた。しかし、保存処理は、非同期的に行っても問題のない処理である。そこで、参照画像の保存・色特徴抽出処理、すなわち、ステップS2120〜S2140の処理を、別スレッドで行うようにしてもよい。これにより、位置姿勢の算出と参照画像の保存処理を独立して実行することが可能となり、参照画像の保存処理が実行された場合でも、位置姿勢推定のフレームレートへの影響を低減することができる。また、参照画像から特徴抽出する処理に計算コストがかかる処理を入れ込むことも可能である。例えば、参照画像からの色特徴抽出処理において、複数の参照画像から抽出した色特徴から画像間対応をとり、参照画像のみから色特徴の3次元再構成を行えば、物体の3次元モデルからは得られない特徴も利用して、位置姿勢推定することが可能となる。
【0125】
[変形例2−4]
上述の第2実施形態では、参照画像保存部230に保存された複数の参照画像の中で、一つだけを選択して、色特徴モデルや幾何特徴の判別に利用していた。しかしながら、観察対象物体の表面属性によっては、鏡面反射などの視点位置によって見た目が変わる情報も存在する。視点位置によって変わる特徴を色特徴モデルとして抽出してしまうと、色特徴モデルを利用した位置姿勢推定が不安定になる。また、観察物体に対して、オクルージョンが発生した場合にも同様の問題が発生する。
【0126】
そこで、ステップS2050において、現在の位置姿勢に近い参照画像を複数選択しておき、複数の色特徴モデルを比較して、これら複数の色特徴モデルの中で安定的に検出されている色特徴を利用するようにしてもよい。なお、「安定的に検出される色特徴」とは、例えば、複数の(2つ以上の)参照画像から検出され得る色特徴である。このようにすれば、鏡面反射やオクルージョンによる、偽の色特徴の影響を除去しながら位置姿勢推定することが可能となる。また、ステップS2060において、複数の参照画像から幾何特徴選別処理を行うことで、複数の参照画像から、幾何特徴の検出判定を行い、複数の画像において、安定して検出できる幾何特徴を利用するように構成することができる。なお、「安定して検出できる幾何特徴」とは、例えば、複数の(2つ以上の)参照画像から対応する色特徴が検出され得る幾何特徴である。これにより、鏡面反射やオクルージョンが生じた場合にも安定した位置姿勢推定を行うことが可能になる。
【0127】
[第3実施形態]
第1・第2実施形態では、位置姿勢推定後、参照画像保存部130,240に撮影画像のフレーム画像を参照画像として保存し、参照画像から対象物体の色に基づく特徴を抽出していた。それに対し、第3実施形態では、撮影画像から3次元モデルの表面色の情報を推定し、3次元モデルを描画した結果として得られる描画画像から対象物体の色に基づく特徴を抽出し、位置姿勢推定を行う方法について述べる。
【0128】
図18は、第3実施形態における位置姿勢計測装置3の構成を示している。同図に示したように、位置姿勢計測装置3は、画像取得部310、モデル保存部320、幾何特徴抽出部330、色特徴抽出部340、画像特徴検出部350、位置姿勢算出部360、物体表面色推定部370を有する。画像取得部310は撮像装置300に接続されている。撮像装置300は、観察対象物体30を撮影する。位置姿勢計測装置3は、3次元モデルデータ60に記述された観察対象物体と撮像装置300との相対的な位置姿勢を撮像画像の情報を用いて求める。なお、第3実施形態において位置姿勢計測装置3が適用できる条件として、撮像装置300が撮像した情景の撮像画像中に該観察対象物体が映っていることを前提としている。以下、位置姿勢計測装置3を構成する各部について説明する。
【0129】
画像取得部310、画像特徴検出部350、位置姿勢算出部360は、第1実施形態における画像取得部110、画像特徴検出部160、位置姿勢算出部170と同様の処理を行う。
【0130】
モデル保存部320は、位置姿勢計測の基準となる観察対象物体50の3次元モデルデータ60を記憶する。3次元モデルデータ60は、観察対象物体50の表面形状を表す3次元の幾何的な情報、および物体表面の色情報を有するモデルであり、頂点情報及び各頂点を結んで構成される面の情報と表面の色情報を表すテクスチャ画像によって定義される。第3実施形態では、頂点情報及び各頂点を結んで構成される面の情報を含むポリゴンモデルを3次元モデルデータ60として利用する。また、同時に3次元モデルデータ60は、観察対象物体の表面の色情報をテクスチャ画像として保持する。テクスチャ画像上での2次元座標と3次元モデル中の頂点との相対位置関係は、テクスチャ変換行列としてあらかじめ求めておく。テクスチャ画像は、物体表面色推定部370において撮影画像からオンライン、すなわち実行時点で推定するため、最初期フレームでは、観察対象物体の表面色は未知である。そこで、テクスチャ画像は、あらかじめ一定色に初期化しておく。
【0131】
幾何特徴抽出部330は、モデル保存部320に保存された観察対象物体50の3次元モデルデータ60と概略位置姿勢から、3次元モデルデータを描画する。そして、描画結果として得られた画像平面上での3次元モデルデータの輝度値と、そのときの奥行き値を保存する。そして、幾何特徴抽出部330は、奥行き値から観察対象物体50の幾何的な情報に基づく特徴を抽出する。
【0132】
色特徴抽出部340は、上述の画像平面上での3次元モデルデータ60の輝度値から、観察対象物体の色に基づく特徴(エッジ)を抽出する。同時に、色に基づく特徴(エッジ)の基準座標における3次元座標を求める。
【0133】
物体表面色推定部370は、位置姿勢が推定された撮影画像を利用して観察対象物体50の表面色を推定する。本実施形態では、撮影画像上に撮像される観察対象物体50の輝度値を、テクスチャ画像に対してマッピングすることで、観察対象物体の表面色の推定を行う。
【0134】
次に、第3実施形態における位置姿勢計測方法の処理手順について説明する。図19は、第3実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【0135】
(ステップS3010)
ステップS3010では初期化を行う。ステップS3010の処理内容は基本的に第1実施形態におけるステップS1010と同様である。
【0136】
(ステップS3020)。
【0137】
ステップS3020の処理は第1実施形態におけるステップS1020の処理と同様である。
【0138】
(ステップS3030)
ステップS3030において、幾何特徴抽出部330は、3次元モデルデータ60を用いて観察対象物体50を描画し、幾何的情報に基づく特徴を検出する。ステップS3030の処理内容は基本的に第1実施形態におけるステップS1010と同様である。ステップS1010と異なる点は、3次元モデルを描画する際にモデル保存部320に保持しているテクスチャ画像によるテクスチャマッピングを行い、描画結果をカラーバッファとして保存する点である。表面色が推定された3次元モデルを描画することで、実際の撮影画像に撮像される観察対象物体の見た目に近い、3次元モデルの描画結果が得られる。この描画結果として得られる画像平面上における3次元モデルデータの輝度値をカラーバッファとして保存し、以後の処理に利用する。
【0139】
(ステップS3040)
ステップS3040では、モデル保存部320に保持しているテクスチャ画像が更新されている場合は、ステップS3050に処理を進める。一方、最初期フレームなどで、テクスチャ画像が更新されていない場合は、ステップS3050〜S3070をスキップして、ステップS3090へ処理を進める。
【0140】
(ステップS3050)
ステップS3050において、幾何特徴抽出部330は、ステップS3030で抽出した幾何特徴の選別を行う。ステップS3050の処理内容は、基本的に第1実施形態におけるステップS1050の処理と同様である。ステップS1050と異なる点は、参照画像の代わりに、ステップS3030で保存したカラーバッファを利用する点である。
【0141】
(ステップS3060)
ステップS3060において、色特徴抽出部340は、ステップS3030で保存したカラーバッファから、観察対象物体の色情報に基づく特徴を検出し、色特徴モデルを生成する。色特徴モデルの生成方法は、基本的に第1実施形態におけるステップS1060の処理と同様である。ステップS1060と異なる点は、参照画像の代わりに、ステップS3030で保存したカラーバッファを利用する点である。また、色特徴の逆投影に利用する位置姿勢には、ステップ3030において3次元モデルデータの描画に利用した位置姿勢と同一のものを利用する。
【0142】
(ステップS3070)
次に、ステップS3070において、色特徴抽出部340は、ステップS3060で抽出した色特徴モデルの選別を行う。ステップS3070の処理内容は、基本的に第1実施形態におけるステップS1070の処理と同様である。ステップS1060と異なる点は、参照画像の代わりに、ステップS3030で保存したカラーバッファを利用する点である。また、色特徴の逆投影に利用する位置姿勢には、ステップ3030において3次元モデルデータの描画に利用した位置姿勢と同一のものを利用する。
【0143】
(ステップS3080)
次にステップS3080において、画像特徴検出部350は、ステップS3030で生成した幾何特徴モデルと、ステップS3060で生成した色特徴モデルを統合した物体特徴モデルを生成する。ステップS3080の処理内容は、基本的に第1実施形態におけるステップS1080の処理と同様である。
【0144】
(ステップS3090)
ステップ3090において、画像特徴検出部350は、ステップS3080で求めた物体特徴モデルのエッジ素に対応する画像特長を、撮像装置300によって撮像された現フレーム撮影画像より検出する。ステップS3090の処理内容は、基本的に第1実施形態におけるステップS1090の処理と同様である。
【0145】
(ステップS3100)
ステップS3100において、位置姿勢算出部360は、ステップS3080で生成した物体特徴モデルと、ステップS3090で対応付けた画像特徴の位置情報から、撮像装置300の位置姿勢を算出する。ステップS3100の処理内容は、基本的に第1実施形態におけるステップS1100の処理と同様である。
【0146】
(ステップS3110)
ステップS3110において、位置姿勢計測装置3は、位置姿勢算出を終了する入力がなされたかどうかを判定する。位置姿勢算出を終了する入力がなされた場合には当該終了し、位置姿勢算出を終了する入力がなされなかった場合には処理をステップS3120に進める。
【0147】
(ステップS3120)
ステップS3120において、物体表面色推定部370は、位置姿勢推定した撮影画像から、観察対象物体の表面色を推定する。まず、物体表面色推定部370は、ステップS3030において基準座標系におけるエッジ素の3次元座標を求めた手順と同様の処理により、撮影画像上の2次元座標に対応する基準座標系における3次元座標を求める。次に、求めた3次元座標に対応するテクスチャ画像上の2次元座標を、モデル保存部320に保存されているテクスチャ変換行列を利用することで求める。以上の手順により、撮影画像上の2次元座標とテクスチャ画像上の2次元座標の関係を求め、撮影画像上に撮像される観察対象物体の輝度値をテクスチャ画像上にマッピングすることで、テクスチャ画像を更新し、観察対象物体の表面色を推定する。このとき、撮影画像上の輝度値をそのままテクスチャ画像上の輝度値として設定せずに、学習係数に応じて徐々にテクスチャ画像の輝度値を更新する。これにより、位置姿勢推定精度が低い撮影画像が混入した場合や、オクルージョンが発生した場合の影響を低減し、確からしい表面色のみを更新することができる。学習係数は、位置姿勢推定精度の高さと、表面色がどのくらい急激に変化しうるかにより適切な値を設定する。以上の処理により表面色を更新した後は、ステップS3020に戻り、新たな画像を取得して、再度位置姿勢算出を行う。
【0148】
以上述べたように、第3実施形態では、位置姿勢推定した撮影画像から毎フレームのテクスチャ画像を更新し、物体の表面色を直接推定する。これにより、参照画像として撮影画像を保存する必要をなくした。以上のように、第3実施形態では、物体の表面色が推定された3次元モデルデータを描画し、実際に撮影画像に撮像される観察対象物体の見た目に近い描画結果から観察対象物体の色に基づく特徴を抽出する。これにより、観察対象物体の幾何特徴と色特徴とを統合利用した位置姿勢推定が可能となる。
【0149】
[他の実施形態]
第1〜第3実施形態では、参照画像から抽出する色に基づく特徴として、エッジを用いたが、これに限るものでない。例えば、Harris検出器や、非特許文献9に示されるSIFT検出器などによって検出される点特徴を利用しても良い。このとき、点特徴の記述子としては、点特徴周辺の輝度分布を利用しても、非特許文献9に示されるSIFT記述子を利用しても良く、点特徴の検出子と記述子の選択に特に制限はない。
【0150】
また、色に基づく特徴として点特徴を利用する場合、色特徴モデルとしては、参照画像に撮像される観察対象物体から検出される点特徴の、基準座標系における3次元位置と点特徴が検出された画像上での画像座標、その画像へのポインタが保存される。そして、色特徴モデル中の点特徴と、現フレームの撮影画像から検出した点特徴とを対応付けることで、位置姿勢が推定される。点特徴は、細かい模様のある物体など、特徴としてエッジを利用した場合には誤対応が頻出するような物体に対して、特に検出されやすい。そのため、対象に細かい模様があるような場合には、エッジを特徴として利用するよりも、安定した位置姿勢推定が行うことが可能である。
【0151】
以上、実施形態を詳述したが、本発明は、例えば、システム、装置、方法、プログラムもしくは記憶媒体等としての実施態様をとることが可能である。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
【0152】
尚、本発明は、ソフトウェアのプログラムをシステム或いは装置に直接或いは遠隔から供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムコードを読み出して実行することによって前述した実施形態の機能が達成される場合を含む。この場合、供給されるプログラムは実施形態で図に示したフローチャートに対応したコンピュータプログラムである。
【0153】
従って、本発明の機能処理をコンピュータで実現するために、該コンピュータにインストールされるプログラムコード自体も本発明を実現するものである。つまり、本発明は、本発明の機能処理を実現するためのコンピュータプログラム自体も含まれる。
【0154】
その場合、プログラムの機能を有していれば、オブジェクトコード、インタプリタにより実行されるプログラム、OSに供給するスクリプトデータ等の形態であっても良い。
【0155】
コンピュータプログラムを供給するためのコンピュータ読み取り可能な記憶媒体としては以下が挙げられる。例えば、フロッピー(登録商標)ディスク、ハードディスク、光ディスク、光磁気ディスク、MO、CD−ROM、CD−R、CD−RW、磁気テープ、不揮発性のメモリカード、ROM、DVD(DVD−ROM,DVD−R)などである。
【0156】
その他、プログラムの供給方法としては、クライアントコンピュータのブラウザを用いてインターネットのホームページに接続し、該ホームページから本発明のコンピュータプログラムをハードディスク等の記録媒体にダウンロードすることが挙げられる。この場合、ダウンロードされるプログラムは、圧縮され自動インストール機能を含むファイルであってもよい。また、本発明のプログラムを構成するプログラムコードを複数のファイルに分割し、それぞれのファイルを異なるホームページからダウンロードすることによっても実現可能である。つまり、本発明の機能処理をコンピュータで実現するためのプログラムファイルを複数のユーザに対してダウンロードさせるWWWサーバも、本発明に含まれるものである。
【0157】
また、本発明のプログラムを暗号化してCD−ROM等の記憶媒体に格納してユーザに配布するという形態をとることもできる。この場合、所定の条件をクリアしたユーザに、インターネットを介してホームページから暗号を解く鍵情報をダウンロードさせ、その鍵情報を使用して暗号化されたプログラムを実行し、プログラムをコンピュータにインストールさせるようにもできる。
【0158】
また、コンピュータが、読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているOSなどとの協働で実施形態の機能が実現されてもよい。この場合、OSなどが、実際の処理の一部または全部を行ない、その処理によって前述した実施形態の機能が実現される。
【0159】
さらに、記録媒体から読み出されたプログラムが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれて前述の実施形態の機能の一部或いは全てが実現されてもよい。この場合、機能拡張ボードや機能拡張ユニットにプログラムが書き込まれた後、そのプログラムの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるCPUなどが実際の処理の一部または全部を行なう。
【0160】
以上のように、上記各実施形態によれば、位置姿勢が既知である参照画像(過去フレームにおいて観察対象物体が撮像された撮影画像やテクスチャが設定されたモデル画像)を利用して、使用すべき画像特徴が決定される。そして、このような画像特徴を用いて現在のフレームに関する撮像装置の位置姿勢を算出するので、位置姿勢推定の安定性が向上する。即ち、各実施形態によれば、観察対象物体の3次元モデルデータから抽出した3次元線分モデルが、撮影画像中に撮像される観察対象物体から抽出できる特徴と大きく異なる場合でも、安定して位置姿勢推定することができる。また、3次元モデルから抽出される幾何的特徴に加えて、観察対象物体の表面の色に基づく特徴を利用することで、3次元モデルデータの中に含まれていない情報も利用して位置姿勢推定することが可能である。
【図面の簡単な説明】
【0161】
【図1】第1実施形態における位置姿勢計測装置1の構成を示す図である。
【図2】第1実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【図3】第1実施形態における対象幾何特徴抽出の詳細な処理手順を示すフローチャートである。
【図4】3次元モデルデータの描画を示す図である。
【図5】デプスバッファから抽出したエッジを撮影画像に重ね合わせた図である。
【図6】エッジ素の抽出を示す図である。
【図7】本発明の一実施形態における対象幾何特徴選別の詳細な処理手順を示すフローチャートである。
【図8】幾何特徴モデルのエッジ素の参照画像への投影を示す図である。
【図9】幾何特徴モデルのエッジ素近傍のエッジ探索を示す図である。
【図10】第1実施形態における対象色特徴抽出の詳細な処理手順を示すフローチャートである。
【図11】第1実施形態における対象色特徴選別の詳細な処理手順を示すフローチャートである。
【図12】第1実施形態における幾何特徴モデルと色特徴モデルとの統合の詳細な処理手順を示すフローチャートである。
【図13】第1実施形態における現フレームの撮影画像からの画像特徴抽出の詳細な処理手順を示すフローチャートである。
【図14】物体特徴モデルのエッジ素を現フレームの撮影画像に重ね合わせた図である
【図15】エッジの情報を利用して撮像装置の位置及び姿勢を算出する方法を説明する図である。
【図16】第2実施形態における位置姿勢計測装置2の構成を示す図である
【図17】第2実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【図18】第3実施形態における位置姿勢計測装置3の構成を示す図である
【図19】第3実施形態における位置姿勢計測方法の処理手順を示すフローチャートである。
【特許請求の範囲】
【請求項1】
撮像装置が撮像した画像を入力する入力手段と、
観察対象物体の表面形状を表す3次元モデルデータを格納する格納手段と、
前記3次元モデルデータを画像平面に描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出手段と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別手段と、
前記幾何特徴選別手段で選別された幾何特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出手段とを備えることを特徴とする位置姿勢計測装置。
【請求項2】
前記参照画像から、明度または色に基づいて前記観察対象物体に関連する複数の画像特徴を抽出する画像特徴抽出手段と、
前記画像特徴抽出手段で抽出した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合する統合手段とを更に備え、
前記算出手段は、前記物体特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出することを特徴とする請求項1に記載の位置姿勢計測装置。
【請求項3】
前記参照画像の位置姿勢によって前記3次元モデルデータを画像平面に描画することにより前記複数の画像特徴に対応する幾何特徴を探索し、対応する幾何特徴が検出された画像特徴を除去することで選別された画像特徴を取得する画像特徴選別手段を更に備え、
前記統合手段は、前記画像特徴選別手段で選別した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合することを特徴とする請求項2に記載の位置姿勢計測装置。
【請求項4】
前記算出手段で算出された位置姿勢と共に、前記入力手段で入力した画像を、前記参照画像として保持する保持手段を更に備えることを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。
【請求項5】
前記幾何特徴抽出手段は、幾何特徴として、前記3次元モデルデータを画像平面に描画して得られたデプス値に基づいてエッジを抽出することを特徴とする請求項1乃至4のいずれか1項に記載の位置姿勢計測装置。
【請求項6】
前記画像特徴抽出手段は、画像特徴として、前記参照画像から色または明度の境界に基づくエッジを抽出することを特徴とする請求項2に記載の位置姿勢計測装置。
【請求項7】
前記算出手段において算出された位置姿勢の推定精度を算出する推定精度算出手段を更に備え、
前記保持手段は、前記推定精度が所定値を超える場合のみ、前記入力手段で入力した画像を前記参照画像として保持することを特徴とする請求項4に記載の位置姿勢計測装置。
【請求項8】
前記保持手段は、複数の参照画像を保持し、
前記幾何特徴選別手段は、前記複数の参照画像から、前記算出手段によって算出された現在の位置姿勢に最も近い位置姿勢を有する参照画像を選択して用いることを特徴とする請求項4に記載の位置姿勢計測装置。
【請求項9】
前記保持手段は、前記入力手段で入力した画像に更に時刻情報を付加して前記参照画像として保持し、時系列的に古い参照画像から更新していくことを特徴とする請求項8に記載の位置姿勢計測装置。
【請求項10】
前記保持手段は、複数の参照画像を保持し、
前記幾何特徴選別手段は、複数の参照画像で検出され得る画像特徴のみを選別することを特徴とする請求項4に記載の位置姿勢計測装置。
【請求項11】
前記3次元モデルデータはテクスチャの情報を有し、
前記3次元モデルデータを前記撮像装置について算出された直前の位置姿勢を用いて画像平面上にテクスチャと共に描画する描画手段を更に備え、
前記参照画像は前記描画手段により描画された画像であることを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。
【請求項12】
撮像装置が撮像した画像を入力する入力工程と、
観察対象物体の表面形状を表す3次元モデルデータを格納した格納手段より前記3次元モデルデータを読み出し、前記3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出工程と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別工程と、
前記幾何特徴選別工程で選別された幾何特徴を前記入力工程で入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出工程とを有することを特徴とする位置姿勢計測方法。
【請求項13】
請求項12に記載された位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【請求項14】
請求項12に記載された位置姿勢計測方法をコンピュータに実行させるためのプログラムを格納したコンピュータ読み取り可能な記憶媒体。
【請求項1】
撮像装置が撮像した画像を入力する入力手段と、
観察対象物体の表面形状を表す3次元モデルデータを格納する格納手段と、
前記3次元モデルデータを画像平面に描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出手段と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別手段と、
前記幾何特徴選別手段で選別された幾何特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出手段とを備えることを特徴とする位置姿勢計測装置。
【請求項2】
前記参照画像から、明度または色に基づいて前記観察対象物体に関連する複数の画像特徴を抽出する画像特徴抽出手段と、
前記画像特徴抽出手段で抽出した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合する統合手段とを更に備え、
前記算出手段は、前記物体特徴を前記入力手段により入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出することを特徴とする請求項1に記載の位置姿勢計測装置。
【請求項3】
前記参照画像の位置姿勢によって前記3次元モデルデータを画像平面に描画することにより前記複数の画像特徴に対応する幾何特徴を探索し、対応する幾何特徴が検出された画像特徴を除去することで選別された画像特徴を取得する画像特徴選別手段を更に備え、
前記統合手段は、前記画像特徴選別手段で選別した画像特徴と、前記幾何特徴選別手段で選別した幾何特徴とを物体特徴として統合することを特徴とする請求項2に記載の位置姿勢計測装置。
【請求項4】
前記算出手段で算出された位置姿勢と共に、前記入力手段で入力した画像を、前記参照画像として保持する保持手段を更に備えることを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。
【請求項5】
前記幾何特徴抽出手段は、幾何特徴として、前記3次元モデルデータを画像平面に描画して得られたデプス値に基づいてエッジを抽出することを特徴とする請求項1乃至4のいずれか1項に記載の位置姿勢計測装置。
【請求項6】
前記画像特徴抽出手段は、画像特徴として、前記参照画像から色または明度の境界に基づくエッジを抽出することを特徴とする請求項2に記載の位置姿勢計測装置。
【請求項7】
前記算出手段において算出された位置姿勢の推定精度を算出する推定精度算出手段を更に備え、
前記保持手段は、前記推定精度が所定値を超える場合のみ、前記入力手段で入力した画像を前記参照画像として保持することを特徴とする請求項4に記載の位置姿勢計測装置。
【請求項8】
前記保持手段は、複数の参照画像を保持し、
前記幾何特徴選別手段は、前記複数の参照画像から、前記算出手段によって算出された現在の位置姿勢に最も近い位置姿勢を有する参照画像を選択して用いることを特徴とする請求項4に記載の位置姿勢計測装置。
【請求項9】
前記保持手段は、前記入力手段で入力した画像に更に時刻情報を付加して前記参照画像として保持し、時系列的に古い参照画像から更新していくことを特徴とする請求項8に記載の位置姿勢計測装置。
【請求項10】
前記保持手段は、複数の参照画像を保持し、
前記幾何特徴選別手段は、複数の参照画像で検出され得る画像特徴のみを選別することを特徴とする請求項4に記載の位置姿勢計測装置。
【請求項11】
前記3次元モデルデータはテクスチャの情報を有し、
前記3次元モデルデータを前記撮像装置について算出された直前の位置姿勢を用いて画像平面上にテクスチャと共に描画する描画手段を更に備え、
前記参照画像は前記描画手段により描画された画像であることを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。
【請求項12】
撮像装置が撮像した画像を入力する入力工程と、
観察対象物体の表面形状を表す3次元モデルデータを格納した格納手段より前記3次元モデルデータを読み出し、前記3次元モデルデータを描画することにより前記観察対象物体の幾何的情報に基づく複数の幾何特徴を抽出する幾何特徴抽出工程と、
前記観察対象物体に対する前記撮像装置の位置姿勢が既知の参照画像において前記複数の幾何特徴に対応する画像特徴を探索し、対応する画像特徴が検出された幾何特徴を前記複数の幾何特徴より選別する幾何特徴選別工程と、
前記幾何特徴選別工程で選別された幾何特徴を前記入力工程で入力された画像における前記観察対象物体の画像に対応付けることにより、前記撮像装置の前記観察対象物体に対する位置姿勢を算出する算出工程とを有することを特徴とする位置姿勢計測方法。
【請求項13】
請求項12に記載された位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【請求項14】
請求項12に記載された位置姿勢計測方法をコンピュータに実行させるためのプログラムを格納したコンピュータ読み取り可能な記憶媒体。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【公開番号】特開2010−79452(P2010−79452A)
【公開日】平成22年4月8日(2010.4.8)
【国際特許分類】
【出願番号】特願2008−244945(P2008−244945)
【出願日】平成20年9月24日(2008.9.24)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
【公開日】平成22年4月8日(2010.4.8)
【国際特許分類】
【出願日】平成20年9月24日(2008.9.24)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
[ Back to top ]