JP4898464B2 - 情報処理装置および方法 - Google Patents

Description

本発明は、撮像装置の位置、姿勢を求めるための技術に関するものである。

近年、現実空間に仮想空間の情報を重ね合わせてユーザに提示する拡張現実感（Augmented Reality、以下AR）技術の研究が盛んである。AR技術における情報提示装置としては、ビデオシースルー型のヘッドマウントディスプレイ（Head Mounted Display、以下HMD）が代表的である。ビデオシースルー型HMDには、現実空間を撮影するカメラが内蔵されている。一方で、このカメラの現実空間における位置及び姿勢に応じて、コンピュータグラフィクス（以下、CG）によって仮想物体が描画される。そして、描画された仮想物体を現実空間の画像上に重畳することにより合成画像を生成し、この合成画像を液晶パネルなどのHMDの表示デバイス上に表示させる。このような情報提示装置により、利用者は、あたかも仮想物体が現実空間中に実在するかのように感じることができる。

AR技術を実現する上で解決しなければならない大きな問題の一つとして、位置合わせの問題がある。仮想物体が現実空間中に実在するように利用者に感じさせるためには、仮想物体と現実空間との間の幾何学的な整合性が取れている必要がある。即ち、仮想物体は常に現実空間中に存在すべき位置に存在しているように利用者から観察されなければならない。

ビデオシースルー型HMDを利用するAR技術では、HMDに内蔵されているカメラから現実空間の画像を取得する毎に、現実空間中におけるカメラの画像撮影時における位置及び姿勢を計測する。そしてこのカメラの位置及び姿勢と、焦点距離などのカメラの固有パラメータに基づいてCGを描画し、現実空間の画像上に重畳する。このような一連の処理が、AR技術ではでは一般的に行われる。そのため、ビデオシースルー型HMDを利用するAR技術の場合、位置合わせの問題は、HMDに内蔵したカメラの現実空間における位置及び姿勢を計測する問題となる。カメラの位置及び姿勢の計測は、例えば磁気センサや超音波センサ、光学式センサなどの6自由度のカメラの位置及び姿勢を計測する物理センサによって行うことが可能である。

ところで、ビデオシースルー型HMDを利用する場合には、HMDに内蔵されているカメラからの画像情報を位置合わせのために利用することが可能である。画像情報を利用する位置合わせ方法は、物理センサを利用する方法に比べて手軽でかつ低コストであるため広く利用されている。画像情報を利用する位置合わせ手法は、一般に、現実空間中における3次元位置が既知の指標をカメラで撮影し、指標の撮影画像上での位置と3次元位置との対応をもとにカメラの位置及び姿勢を算出する。

指標として、現実空間中に人為的に配置したマーカを用いる場合と、現実空間中に元来存在するコーナー点やエッジなどの自然特徴を利用する場合がある。実用上は、画像からの検出や識別がしやすい人工的なマーカが、安定性や計算負荷の点から広く利用されている。

非特許文献１には、固有の2次元パターンが内部に描かれた正方形形状のマーカを指標として用いた位置合わせ方法が開示されている。この方法では、画像中から正方形指標の領域を抽出し、内部の2次元パターンを用いてマーカの識別を行う。さらに、カメラによって撮影された画像上における正方形マーカの各頂点の位置と、正方形マーカの各頂点のマーカ座標系における位置との対応をもとに、カメラの位置及び姿勢を算出している。このような正方形マーカなどの人工的なマーカは手軽に利用できるため広く利用されている。しかしながら、マーカを配置することが物理的に不可能な場合や、美観を損ねるなどの理由のためにマーカを配置したくない場合にはマーカを利用することができない。

一方、近年の計算機の能力向上に伴い、現実空間中に元来存在する自然特徴を利用した位置合わせ手法の研究が盛んに行われている。位置合わせに用いられる自然特徴としては、コーナー点などの点形状の特徴（以下、点特徴）と、エッジなどの線特徴が主に用いられる。

非特許文献２、３、４、５、６、７には、エッジを利用した位置合わせの方法が開示されている。エッジはスケールや観察方向に対して不変であるため、エッジを利用した位置合わせは精度が高いという特徴がある。エッジを利用した位置合わせでは、線分の集合により記述されている現実空間や現実物体の3次元モデルデータを持つことが前提となっている。非特許文献２、３、４で開示されているエッジを利用した位置合わせは、次の１から３の処理によって実現される。

１． 前フレームでのカメラの位置及び姿勢、及び予め校正済みのカメラの固有パラメータに基づいて、前述の3次元モデルデータ（線分モデル）を画像上に投影する。

２． 投影された線分モデルを構成する各線分を、画像上で一定間隔となるように分割し、分割点を設定する。そして各分割点について、該分割点を通過し向きが投影された線分の法線方向である線分（探索ライン）上でエッジ探索を行い、探索ライン上における輝度値の勾配が極大でありかつ分割点に最も近い点を対応エッジとして検出する。

３． 各分割点毎に検出された対応エッジと、投影される線分との間の画像上での距離の総和が最小となるようなカメラの位置及び姿勢の補正値を算出し、カメラの位置及び姿勢を補正する。

前述のエッジを利用した位置合わせ方法では、前フレームで算出されたカメラの位置及び姿勢に基づいてエッジ検出を行っている。そして、画像上で検出されるエッジの情報をもとに前フレームの位置及び姿勢に対する補正値を算出し、前フレームにおけるカメラの位置及び姿勢を該補正値により補正すること現フレームにおけるカメラの位置及び姿勢を算出している。そのため、例えば前フレームで位置姿勢算出が失敗した場合、以降のフレームにおいても正しくカメラの位置及び姿勢を算出することはできなくなり、位置合わせが破綻する。このような状況は、例えばカメラが高速に移動する場合や、移動物体がカメラの前を横切る場合などにしばしば起こり得る。このような位置合わせの破綻を回避するために、各フレームにおいて一つのカメラの位置及び姿勢を出力するのではなく、複数のカメラの位置及び姿勢を出力し、それらを複数の仮説として次フレームで利用する手法が提案されている。

非特許文献５では、点特徴の情報を併用し、１フレームにおいて複数のカメラの位置及び姿勢を算出することによって、エッジを利用した位置合わせが破綻することを回避している。非特許文献５では、エッジを利用した位置合わせと共に、フレーム間での点特徴の対応に基づいて、現フレームにおけるカメラの位置及び姿勢を反復演算により算出する。この際に、前フレームにおいてエッジ情報から得られるカメラの位置及び姿勢と、点特徴の情報から得られるカメラの位置及び姿勢を夫々初期値とすることにより、2種類のカメラの位置及び姿勢を算出する。このようにして得られた2種類のカメラの位置及び姿勢のうち、尤度の大きい方を現フレームにおける点特徴の情報から得られるカメラの位置及び姿勢とする。そしてこのカメラの位置及び姿勢を一つの仮説として次フレームに出力すると共に、エッジを用いた位置合わせにおけるカメラの位置及び姿勢の初期値とする。さらに前述のエッジを用いた位置合わせを行って、その結果得られるカメラの位置及び姿勢をもう1つの仮説として次フレームに出力する。このようにして、非特許文献５では、常に２つのカメラの位置及び姿勢を次フレームに出力し、常に妥当性の高い位置及び姿勢を選択することにより、位置合わせが破綻することを回避している。

また、非特許文献６、７では、パーティクルフィルタによってカメラの複数の位置及び姿勢を保持することで、エッジを用いた位置合わせが破綻することを回避している。パーティクルフィルタでは、複数のカメラの位置及び姿勢を6次元空間における離散的なパーティクルの集合として保持する。各パーティクルは、カメラの位置及び姿勢とともに、該位置及び姿勢の信頼度を表す重みをデータとして持つ。各フレームにおいては、前フレームから得られるパーティクルの集合から、各パーティクルの重みに基づいて新たにパーティクルの集合を発生させる。次に、新たに発生させた各パーティクルの位置及び姿勢を動きモデルに基づいて変化させる。さらに、各パーティクルについて尤度を求め、尤度に応じた重みを与えたパーティクルの集合を複数のカメラの位置及び姿勢として次フレームに出力する。現フレームにおけるカメラの位置及び姿勢としては、各パーティクルの位置及び姿勢の重み付け平均が一般に用いられる。このように、非特許文献６、７では、複数のカメラの位置及び姿勢をパーティクルとして保持することにより、位置合わせが破綻することを回避している。
加藤, M. Billinghurst, 浅野, 橘, "マーカー追跡に基づく拡張現実感システムとそのキャリブレーション", 日本バーチャルリアリティ学会論文誌, vol.4, no.4, pp.607-617, 1999. T. Drummond and R. Cipolla, "Real-time visual tracking of complex structures," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002. A. I. Comport, E. Marchand, and F. Chaumette, "A real-time tracker for markerless augmented reality," Proc. The 2nd IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR03), pp.36-45, 2003. L. Vacchetti, V. Lepetit, and P. Fua, "Combining edge and texture information for real-time accurate 3D camera tracking," Proc. The 3rd IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR04), pp.48-57, 2004. E. Rosten and T. Drummond, "Fusing points and lines for high performance tracking," Proc. The 10th IEEE International Conference on Computer Vision (ICCV’05), pp.1508-1515, 2005. M. Pupilli and A. Calway, "Real-time camera tracking using known 3D models and a particle filter," Proc. The 18th International Conference on Pattern Recognition (ICPR’06), pp.199-203, 2006. G. Klein and D. Murray, "Full-3D edge tracking with a particle filter," Proc. British Machine Vision Conference 2006, 2006. I. Skrypnyk and D. G. Lowe, "Scene modelling, recognition and tracking with invariant image features," Proc. The 3rd IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR04), pp.110-119, 2004. H. Wuest, F. Vial, and D. Stricker, "Adaptive line tracking with multiple hypotheses for augmented reality," Proc. The Fourth Int’l Symp. on Mixed and Augmented Reality (ISMAR05), pp.62-69, 2005. K. Satoh, S. Uchiyama, H. Yamamoto, and H. Tamura, "Robust vision-based registration utilizing bird’s-eye view with user’s view," Proc. The Second Int’l Symp. on Mixed and Augmented Reality (ISMAR03), pp.46-55, 2003.

（従来の課題）
非特許文献５で開示されている手法は、位置合わせの破綻を回避するために、点特徴の情報を利用することで複数のカメラの位置及び姿勢を生成していた。しかしながら、コーナー点などの点特徴が現実空間中に存在しない又は少ない場合や、見た目が類似している点特徴が多い場合には、非特許文献５で開示されている手法は利用することができない。また、カメラの２つの位置及び姿勢のどちらが用いられるかは次フレームにならないとわからないため、AR技術において仮想物体を描画する際に、必ずしも正しいカメラの位置及び姿勢を用いることができないという問題がある。

また、非特許文献６、７で開示されている手法は、数百個単位のパーティクルについて尤度の計算をしなければならないため、計算負荷が高い。そのため実時間性が求められるARアプリケーションに向いていない。また、パーティクルフィルタを用いる手法は、カメラの位置及び姿勢をパーティクルの集合として表現するため、得られるカメラの位置及び姿勢が不正確であったり、フレーム間でジッタを生じてしまうという問題がある。

本発明は以上の問題に鑑みてなされたものであり、現実空間中における撮像装置の位置及び姿勢を求める際に生ずる「位置合わせの破綻」を回避しつつ、より簡便且つより高精度に撮像装置の位置及び姿勢を求めるための技術を提供することを目的とする。

本発明の目的を達成するために、例えば、本発明の情報処理装置は以下の構成を備える。

即ち、現実空間の動画像を撮像する撮像装置に接続されている情報処理装置であって、
前記動画像を構成する各フレームの画像を前記撮像装置から取得する毎に、取得したフレームの画像の撮像時における前記撮像装置の撮像位置、撮像姿勢を求める計算手段と、
前記計算手段が求めた撮像位置及び撮像姿勢を示す撮像位置姿勢情報を、メモリに記録する記録手段とを備え、
前記計算手段は、
前記メモリに記録されている複数の撮像位置姿勢情報に基づいて、予め設定された個数の候補位置姿勢情報を求める候補位置姿勢算出手段と、
前記取得したフレームの画像中における観察対象物体の画像特徴を検出する検出手段と、
前記候補位置姿勢算出手段が求めたそれぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報からそれぞれ異なる候補位置情報及び候補姿勢情報の組み合わせを複数個生成し、該生成した複数個の組み合わせと前記画像特徴に対応する既知の３次元情報とから推定される該画像特徴の前記取得したフレームの画像中における推定位置と、前記検出手段による前記画像特徴の検出位置と、の誤差に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報の補正値を算出し、該補正値に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報を補正する補正手段と、
前記補正手段が補正した前記それぞれの候補位置姿勢情報について評価値を求め、求めたそれぞれの評価値に基づいて、該それぞれの候補位置姿勢情報のうち１つを選択する選択手段とを備え、
前記記録手段は、
前記選択手段が選択した候補位置姿勢情報を、前記撮像位置姿勢情報として前記メモリに記録する
ことを特徴とする。

本発明の構成によれば、現実空間中における撮像装置の位置及び姿勢を求める際に生ずる「位置合わせの破綻」を回避しつつ、より高精度に撮像装置の位置及び姿勢を求めることができる。

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

［第１の実施形態］
図１は、本実施形態に係る位置姿勢計測装置１の機能構成を示すブロック図である。図１に示す如く、位置姿勢計測装置１は、画像入力部１１０、画像特徴検出部１２０、モデルデータ保存部１３０、位置姿勢算出部１４０、位置姿勢選択部１５０、位置姿勢生成部１６０、位置姿勢保存部１７０により構成されている。また、画像入力部１１０には、撮像装置１００が接続されている。

図２は、撮像装置１００を装着したユーザと、このユーザが観察する空間とを模式的に示した図である。

観察者４０は、ビデオシースルー型のＨＭＤ３０を装着している。ビデオシースルー型ＨＭＤ３０には、左眼、右眼のそれぞれ対応した撮像装置１００Ｌ、１００Ｒが内蔵されている。撮像装置１００Ｒ、１００Ｌは、現実空間の動画像をそれぞれ撮像するものであり、図２では、現実物体としての観察対象物体１０を含む現実空間の動画像を撮像している。撮像装置１００Ｌ（１００Ｒ）が撮像した各フレームの画像は順次、位置姿勢計測装置１に入力される。位置姿勢計測装置１は、撮像装置１００Ｌ（１００Ｒ）から入力された画像と、モデルデータ保存部１３０に保存されている観察対象物体１０の３次元モデルデータとに基づいて、撮像装置１００Ｌ（１００Ｒ）の位置及び姿勢を算出する。

そして位置姿勢計測装置１は、算出した撮像装置１００Ｌ（１００Ｒ）の位置及び姿勢に基づいて仮想物体２０の画像を、この算出のために用いたフレームの画像上に重ねて描画することで、合成画像を生成する。そして位置姿勢計測装置１は、この生成した合成画像を、ビデオシースルー型ＨＭＤ３０が有する表示デバイスに対して出力するので、この表示デバイスは、この合成画像を表示する。ここで、ビデオシースルー型ＨＭＤ３０が有する表示デバイスは、ビデオシースルー型ＨＭＤ３０を装着した観察者４０の眼前に位置するようにビデオシースルー型ＨＭＤ３０に取り付けられているものである。従って観察者４０の眼前には、この合成画像が表示されることになる。

なお、撮像装置１００Ｌによって撮像された画像に加えて、撮像装置１００Ｒによって撮像された画像をも用いることで以上のような位置姿勢算出処理を行っても良い。本実施形態では撮像装置１００Ｌによって撮像された画像のみを用いて、観察者の視点（観察者の位置及び姿勢）に相当する撮像装置１００Ｌの位置及び姿勢を算出するものとする。

次に、位置姿勢計測装置１を構成する各部について説明する。

画像入力部１１０は、撮像装置１００Ｌから順次送出される各フレームの画像（現実空間画像）を受け、後段の画像特徴検出部１２０に対して転送する。画像入力部１１０は、撮像装置１００Ｌの出力がＮＴＳＣなどのアナログ出力であればアナログビデオキャプチャボードによって実現される。また撮像装置１００Ｌの出力がＩＥＥＥ１３９４などのデジタル出力であれば、例えばＩＥＥＥ１３９４インタフェースボードによって実現される。なお、現実空間の動画像のファイルが、例えば、ハードディスク装置などに予め保存されており、位置姿勢計測装置１がこれを用いる場合には、画像入力部１１０は、このファイルを読み出し、後段の画像特徴検出部１２０に対して転送する処理を行う。

画像特徴検出部１２０は、画像入力部１１０から画像を受けると、この画像から、撮像装置１００Ｌの位置及び姿勢を算出する為に用いる画像特徴を検出する。本実施形態では、画像特徴検出部１２０は画像上のエッジの検出を行うものとする。エッジの検出方法については後述する。

モデルデータ保存部１３０は、撮像装置１００Ｌの位置及び姿勢を算出する際の基準となる、観察対象物体１０の３次元モデルデータを記憶する。

図３は、本実施形態における３次元モデルの定義方法を説明する図である。３次元モデルは、頂点の情報、各頂点を結んで構成される面の情報、面を構成する線分の情報、によって定義される。図３（ａ）の左側に示すように、本実施形態における３次元モデルは点P1〜点P8の8点から構成される直方体である。そして、直方体の座標系のX軸を点P1から点P4に向かう方向に、Y軸を点P5から点P1に向かう方向に、Z軸を点P1から点P2に向かう方向に取る。また原点をP5に取る。また、図３（ａ）の中央、及び右側に示すように、直方体は面F1〜F6により構成されており、直方体は線分L1〜L12により構成されている。

図３（ｂ）に示すように、点P1〜点P8は3次元座標値によって表される。また図３（ｃ）に示すように、面F1〜F6は、面を構成する点のIDと、各点を接続する順番によって表される。また図３（ｄ）に示すように、線分L1〜L14は、両端の点のIDによって表される。

従って、本実施形態では、モデルデータ保存部１３０には、観察対象物体１０の３次元モデルを再現可能なデータ、即ち、図３（ｂ）、（ｃ）、（ｄ）に示すようなテーブルのデータが、３次元モデルデータとして保存されているものとする。しかし、３次元モデルデータの表現方法については様々なものがあり、以下の説明は、特定の表現方法に従った３次元モデルデータを用いることに限定されるものではない。

位置姿勢保存部１７０は、過去数フレーム分の画像（例えば現フレームから過去３フレーム分の画像）のそれぞれについて位置姿勢算出部１４０が算出した撮像装置１００Ｌの位置及び姿勢を保存する。換言すれば、位置姿勢保存部１７０は、過去数フレーム分の画像（例えば現フレームから過去３フレーム分の画像）のそれぞれの撮像時における撮像装置１００Ｌの位置及び姿勢を保存する。なお、保存するものは厳密には「位置を示す位置情報、姿勢を示す姿勢情報」である。このように以下の説明では、「位置を保存する」とは「位置を示す位置情報を保存する」ことを意図しており、「姿勢を保存する」とは「姿勢を示す姿勢情報を保存する」ことを意図している。

本実施形態では、位置姿勢保存部１７０には、１つ前のフレームにおいて位置姿勢算出部１４０が求めた撮像装置１００Ｌの位置及び姿勢、２つ前のフレームにおいて位置姿勢算出部１４０が求めた撮像装置１００Ｌの位置及び姿勢、３つ前のフレームにおいて位置姿勢算出部１４０が求めた撮像装置１００Ｌの位置及び姿勢、が保存されるものとする。

なお、最初のフレームの画像（例えば１フレーム目の画像）を用いて算出された撮像装置１００Ｌの位置及び姿勢を位置姿勢保存部１７０に保存する前には、後述する初期化処理から得られる撮像装置１００Ｌの初期位置及び初期姿勢を保存しているものとする。

位置姿勢生成部１６０は、位置姿勢保存部１７０に保存されている位置及び姿勢から、複数の位置及び姿勢を生成する。

位置姿勢算出部１４０は、位置姿勢生成部１６０が生成した各位置及び姿勢を、反復演算を行うために用いる初期値とし、それぞれの初期値から求められる複数の位置及び姿勢を求める。より具体的には、画像特徴検出部１２０が検出した画像特徴の情報と、モデルデータ保存部１３０に保存されているモデルデータに基づいて、観察対象物体１０を基準とした座標系（以下、基準座標系）における撮像装置１００Ｌの位置及び姿勢を算出する。

位置姿勢選択部１５０は、位置姿勢算出部１４０が算出した複数の位置及び姿勢から、最も妥当な位置及び姿勢を選択し、位置姿勢保存部１７０に保存する。なお、位置姿勢選択部１５０が選択した位置及び姿勢を用いて、仮想物体２０の画像が描画される。

なお、図１には、撮像装置１００Ｌの位置及び姿勢を求めるための構成のみを記しており、係る位置及び姿勢を用いて仮想物体の画像を生成し、現実空間画像と合成してＨＭＤ等に出力するための構成については記していない。しかし、求めた撮像装置１００Ｌの位置及び姿勢は如何なる目的で使用しても良い。

次に、位置姿勢計測装置１が行う、撮像装置１００Ｌの位置及び姿勢を求める処理について、同処理のフローチャートを示す図４を用いて説明する。

（ステップＳ１０１０）
ステップＳ１０１０では、位置姿勢保存部１７０に、撮像装置１００Ｌの初期位置を示す初期位置情報、初期姿勢を示す初期姿勢情報を格納する。

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

このような初期化を行えば、撮像装置１００Ｌの初期位置、初期姿勢が得られるので、得た初期位置を示す初期位置情報、初期姿勢を示す初期姿勢情報をそれぞれ位置姿勢保存部１７０に格納する。そして処理をステップＳ１０２０に進める。

（ステップＳ１０２０）
ステップＳ１０２０では、撮像装置１００Ｌが撮像した画像（現実空間画像）のデータを画像入力部１１０が取得し、後段の画像特徴検出部１２０に転送する。

（ステップＳ１０３０）
ステップＳ１０３０では、画像特徴検出部１２０は、画像入力部１１０から受けた現実空間画像から、画像特徴の検出処理を行う。本実施形態では画像特徴はエッジであるとする。図５は、ステップＳ１０３０における処理の詳細を示すフローチャートである。

先ずステップＳ１１１０では、モデル投影を行う。ここでのモデル投影とは先ず、モデルデータ保存部１３０に保存されている３次元モデルデータに基づいて観察対象物体１０の３次元モデルを生成する。そして、生成した３次元モデルを、位置姿勢保存部１７０に保存されている最新の位置及び姿勢に基づいて、現実空間画像上に投影する。

具体的には、３次元モデルを表す線分を現実空間画像上に投影したときの、該線分の現実空間画像上での直線の方程式を求めることである。直線の方程式は、線分の両端を現実空間画像上に投影し、現実空間画像上での両端の座標を結んだ直線の方程式として算出する。モデル投影を行うためには、撮像装置１００Ｌの位置及び姿勢とともに、焦点距離や主点位置等、撮像装置１００Ｌ固有のパラメータが既知である必要がある。本実施形態では、焦点距離や主点位置等、撮像装置１００Ｌに固有のパラメータは予め計測され、データとして画像特徴検出部１２０が保持しているものとして説明する。

図６は、ステップＳ１１１０において行われる、現実空間画像上への３次元モデルの投影を示す図である。図６（ａ）は現実空間画像そのものを示しており、６００は観察対象物体１０の像である。図６（ｂ）は３次元モデルを投影した現実空間画像を示しており、６０１は、投影した３次元モデルの像である。

位置姿勢保存部１７０に保存された最新の位置及び姿勢と、現在の実際の撮像装置１００Ｌの位置及び姿勢とが異なる場合には、図６（ｂ）に示すように実際に撮像された像６００と、投影した３次元モデルの像６０１との間にはずれが生じる。図６（ｂ）において、破線で示されている線分は、３次元モデルを構成する線分のうち、実際には隠れて見えない線分を表している。

次にステップＳ１１２０では、分割点の設定を行う。ステップＳ１１１０において算出された各線分の方程式を利用して、投影された線分を現実空間画像上で等間隔に分割するように分割点を設定する。図７は、３次元モデルの現実空間画像上での分割点を示す図である。分割点の総数をNとし、各分割点をDPj（j=1,2,...,N）で表す。分割点の数Nが多いほど、処理時間が長くなる。そのため、現実空間画像上での分割点の間隔を変えることで処理時間を制御できる。また、分割点の数を一定として処理時間が一定になるように、現実空間画像上での分割点間の間隔を逐次変更してもよい。

次にステップＳ１１３０では変数ｊを１に初期化する。

ステップＳ１１４０では、分割点DPjが隠蔽されているかどうかの判定を行う。具体的には、分割点DPjが３次元モデルの他の面に隠蔽されている場合、すなわち図７の破線上の分割点である場合には分割点DPjは隠蔽されている状態にある。分割点DPjが可視かどうかの判定は、例えば非特許文献９に示されるように、グラフィクスハードウェアを利用して上述の３次元モデルを描画した後、該分割点を描画してグラフィクスハードウェアにデプスバッファが更新されたかどうかを確認することで行える。分割点DPjが隠蔽されている場合には処理をステップＳ１１６０に進め、隠蔽されていない場合には処理をステップＳ１１５０に進める。

ステップＳ１１５０では、分割点DPjに対応するエッジの検出を行う。図８は、本実施形態におけるエッジ検出方法を説明する図である。図８（ａ）に示すように、各分割点において、投影された線分の法線方向に平行でかつ該分割点を通過する線分（以下、探索ライン）上においてエッジを１次元探索する。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する（図８（ｂ）参照）。本実施形態では、探索ライン上でエッジが複数検出される場合には、現実空間画像上で最も分割点に近いエッジを対応点とする。しかしながら対応点の決定方法はこれに限るものではなく、探索ライン上で濃度勾配の極値の絶対値が最大のエッジを対応点としてもよい。

ステップＳ１１６０では、変数ｊが保持する値を１つインクリメントする。

ステップＳ１１７０では、すべての分割点DPjについて処理が終了している場合（ｊ＞Ｎの場合）には本処理を終了し、ステップＳ１０３１にリターンする。一方、すべての分割点DPjについて処理が終了していない場合（ｊ≦Ｎの場合）には処理をステップＳ１１４０に進め、以降の処理を行う。

このようにして、現実空間画像から画像特徴を検出する。

（ステップＳ１０３１）
ステップＳ１０３１では、位置姿勢算出部１４０は、３フレーム分の位置情報、姿勢情報が位置姿勢保存部１７０に格納されているか否かをチェックする。即ち、１つ前のフレームにおける撮像装置１００Ｌの位置及び姿勢、２つ前のフレームにおける撮像装置１００Ｌの位置及び姿勢、３つ前のフレームにおける撮像装置１００Ｌの位置及び姿勢、が位置姿勢保存部１７０に格納されているか否かをチェックする。係るチェックの結果、格納されている場合には処理をステップＳ１０４０に進める。一方、格納されていない場合には、処理をステップＳ１０３２に進める。

（ステップＳ１０４０）
次にステップＳ１０４０では先ず、位置姿勢生成部１６０は、位置姿勢保存部１７０に保存されている位置情報（Ｐ_org ^t-1、Ｐ_org ^t-2、Ｐ_org ^t-3）を用いて、複数の位置情報（候補位置情報）ｐ_ｉ（ｉ＝１，２，…，Ｉ）を求める。また、位置姿勢生成部１６０は、位置姿勢保存部１７０に保存されている姿勢情報（ｑ_org ^t-1、ｑ_org ^t-2、ｑ_org ^t-3）を用いて、複数の姿勢情報（候補姿勢情報）ｑ_ｉ（ｉ＝１，２，…，Ｉ）を求める。

ここで、ｐ_org、ｐ_iは３次元空間における撮像装置１００Ｌの位置を表す３次元ベクトルであり、ｑ_org、ｑ_iは撮像装置１００Ｌの姿勢を表すクォータニオンである。

また、ｐ_org ^t-1、ｐ_org ^t-2、ｐ_org ^t-3はそれぞれ、１フレーム前、２フレーム前、３フレーム前において位置姿勢保存部１７０に保存された撮像装置１００Ｌの位置情報である。また、ｑ_org ^t-1、ｑ_org ^t-2、ｑ_org ^t-3はそれぞれ、１フレーム前、２フレーム前、３フレーム前において位置姿勢保存部１７０に保存された撮像装置１００Ｌの姿勢情報である。

ここで、位置姿勢保存部１７０に保存されている位置情報、姿勢情報に基づいて生成する位置情報、姿勢情報の数（Ｉ）は、撮像装置１００Ｌの動きモデルとして予め決めたモデルの個数に一致する。本実施形態では、撮像装置１００Ｌの位置の動きモデルとして位置静止、等速度運動、等加速度運動の３つを仮定する。また、撮像装置１００Ｌの姿勢の動きモデルとして、姿勢静止、等角速度運動、等角加速度運動の３つを仮定する。従ってＩ＝３となる。

即ち、生成する３つの位置情報はそれぞれ、位置静止モデル、等速度運動モデル、等加速度運動モデルに対応するものである。従って、位置情報ｐ_１は位置姿勢モデルに対応するものであり、位置情報ｐ_２は等速度運動モデルに対応するものであり、位置情報ｐ_３は等加速度運動モデルに対応するものである。

ｐ_１，ｐ_２、ｐ_３はそれぞれ、以下のようにして求められる。

また、姿勢情報についても同様である。即ち、生成する３つの姿勢情報はそれぞれ、姿勢静止モデル、等速度運動モデル、等加速度運動モデルに対応するものである。従って、姿勢情報ｑ_１は姿勢姿勢モデルに対応するものであり、姿勢情報ｑ_２は等速度運動モデルに対応するものであり、姿勢情報ｑ_３は等加速度運動モデルに対応するものである。

ｑ_１，ｑ_２、ｑ_３はそれぞれ、以下のようにして求められる。

ここで

はクォータニオンｑの共役クォータニオンを表す。

図１３は、ステップＳ１０４０における位置及び姿勢の生成方法について説明する図である。図１３では、位置Ｐ_１、ｐ_２、ｐ_３の生成方法を示している。図１３に示すように、位置姿勢保存部１７０に保存されている位置情報Ｐ_org ^t-1, p_org ^t-2, p_org ^t-3（図中白丸にて示す）を用いて、位置情報ｐ_１、ｐ_２、ｐ_３（図中三角形にて示す）が生成される。ｐ_１、ｐ_２、ｐ_３はそれぞれ、静止モデル、等速度モデル、等加速度モデルに応じて生成される。この生成されたｐ_１、ｐ_２，ｐ_３を反復演算における初期値とし、ステップＳ１０６０において非線形最適化計算を行う。最適化の結果、異なる位置及び姿勢（図１３では位置のみ黒丸で示している）が得られる。そして、これらの中から、最も適する位置及び姿勢がステップＳ１０９０で選択され、フレームｔにおける位置及び姿勢として位置姿勢保存部１７０に保存される。

本ステップでは更に、位置姿勢生成部１６０は、ｐ_１〜ｐ_３から１つを選択したものと、ｑ_１〜ｑ_３から１つを選択したものとを組み合わせてセットにした場合に、全ての組み合わせについてセットを生成する。本実施形態の場合、Ｓｔ（１）＝（ｐ_１，ｑ_１）、Ｓｔ（２）＝（ｐ_１，ｑ_２）、Ｓｔ（３）＝（ｐ_１，ｑ_３）、Ｓｔ（４）＝（ｐ_２，ｑ_１）、Ｓｔ（５）＝（ｐ_２，ｑ_２）、Ｓｔ（６）＝（ｐ_２，ｑ_３）、Ｓｔ（７）＝（ｐ_３，ｑ_１）、Ｓｔ（８）＝（ｐ_３，ｑ_２）、Ｓｔ（９）＝（ｐ_３，ｑ_３）の９（＝Ｍ）セットが生成されることになる。

（ステップＳ１０５０）
次にステップＳ１０５０では、変数ｉ（例えば、位置姿勢算出部１４０が有するレジスタ）の値を１に初期化する。

（ステップＳ１０６０）
ステップＳ１０６０では、非線形最適化計算を用いて、Ｓｔ（ｉ）を構成する位置情報、姿勢情報を反復演算により補正することで、撮像装置１００Ｌの位置及び姿勢を算出する。ここで、分割点DPjのうち、隠蔽されておらず、ステップＳ１０３０において対応点が求まった分割点の総数をNcとする。

図９は、エッジの情報を利用して撮像装置１００Ｌの位置及び姿勢を算出する方法を説明する図である。図９では、現実空間画像の水平方向、垂直方向をそれぞれｘ軸、ｙ軸としている。ある分割点の投影された画像座標を（ｕ₀，ｖ₀）、該分割点が所属する線分Ｌの現実空間画像上での傾きをｘ軸に対する傾きθと表す。傾きθは、線分の両端の３次元座標を、Ｓｔ（ｉ）を構成する位置情報、姿勢情報に基づいて現実空間画像上に投影し、現実空間画像上での両端の座標を結んだ直線の傾きとして算出する。線分Ｌの現実空間画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、該分割点の対応点の画像座標を（ｕ’，ｖ’）とする。

ここで、点（ｕ，ｖ）を通り、傾きがθである直線の方程式は、

と表せる。分割点が現実空間画像上に投影される座標は、撮像装置１００Ｌの位置及び姿勢により変化する。また、撮像装置１００Ｌの位置及び姿勢の自由度は6自由度である。ここで撮像装置１００Ｌの位置及び姿勢を表すパラメータをsで表す。sは6次元ベクトルであり、撮像装置１００Ｌの位置を表す3つの要素と、姿勢を表す3つの要素からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す3次元ベクトルなどによって表現される。分割点の画像座標（ｕ，ｖ）は（ｕ_０，ｖ_０）の近傍で1次のテイラー展開によって次の式９のように近似できる。

ｕ，ｖの偏微分∂ｕ／∂ｓ_ｉ、∂ｖ／∂ｓ_ｉの導出方法は例えば非特許文献１０に開示されているように、広く知られているのでここではその詳細な説明は省略する。

式９を式８に代入することにより、以下の式１０を得る。

ここで、式１０に示す直線が該分割点の対応点の画像座標（ｕ’，ｖ’）を通過するように、撮像装置１００Ｌの位置及び姿勢sの補正値Δｓを算出する。ｒ_０＝ｕ_０ｓｉｎθ−ｖ_０ｃｏｓθ（定数）、ｄ＝ｕ’ｓｉｎθ−ｖ’ｃｏｓθ（定数）とすると、

が得られる。式１１はNc個の分割点について成り立つため、式１２のようなΔｓに対する線形連立方程式が成り立つ。

ここで、式１２を以下の式１３のように簡潔に表す。

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

cは定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はない。例えば、次式で示されるようなＨｕｂｅｒの関数など、誤差（d−r）が大きい分割点には小さな重みを与え、誤差（d−r）が小さい分割点には大きな重みを与える関数であれば良い。

分割点DPiに対応する重みをw_iとする。ここで、式１６のように重み行列Wを定義する。

重み行列Wは、対角成分以外はすべて0のNc×Nc正方行列であり、対角成分には重みw_iが入る。この重み行列Wを用いて、式１３を式１７のように変形する。

そして式１８のように式１７を解くことにより、補正値Δｓを求める。

これにより得られたΔｓを用いて、ｓ＋Δｓ→ｓというように、撮像装置１００Ｌの位置及び姿勢を更新する。

次に、撮像装置１００Ｌの位置及び姿勢の反復演算が収束しているかどうかを判定する。補正値Δｓが十分に小さかったり、誤差（r−d）の総和が十分小さい、誤差（r−d）の総和が変化しないといった場合には、撮像装置１００Ｌの位置及び姿勢の計算が収束したと判定する。収束していないと判定された場合には、更新された撮像装置１００Ｌの位置及び姿勢を用いて再度、線分の傾きθ、r₀、d、及びｕ、ｖの偏微分を計算し直し、式１８に従って再度補正値Δｓを求め直す。

なお、ここでは非線形最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いた。しかしながら、非線形最適化手法はこれに限るものではなく、Ｎｅｗｔｏｎ−Ｒａｐｈｓｏｎ法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。

（ステップＳ１０７０）
ステップＳ１０７０では、全てのセットＳｔ（１）〜Ｓｔ（９）について、ステップＳ１０６０における処理を行ったか否かをチェックする。即ち、ｉ＝Ｍであるか否かをチェックする。係るチェックの結果、全てのセットＳｔ（１）〜Ｓｔ（９）についてステップＳ１０６０における処理を行った場合（ｉ＝Ｍの場合）には処理をステップＳ１０９０に進める。一方、全てのセットＳｔ（１）〜Ｓｔ（９）についてステップＳ１０６０における処理を行っていない場合（ｉ＜Ｍの場合）には処理をステップＳ１０８０に進める。

（ステップＳ１０８０）
ステップＳ１０８０では変数ｉの値を１つインクリメントする。

（ステップＳ１０９０）
ステップＳ１０９０では、位置姿勢選択部１５０は、ステップＳ１０６０において算出されたＳｔ（１）〜Ｓｔ（９）のうち、最も妥当なものを一つ選択し、選択したセットを構成する位置情報、姿勢情報を位置姿勢保存部１７０に保存する。本実施形態では、位置及び姿勢の妥当性の評価尺度としてとして、上述したロバスト推定で用いられる重みの平均値を用いる。妥当性Ｃは次式により算出される。

即ち、各セットについて係る妥当性Ｃ（評価値）を計算し、妥当性Ｃが最も大きいセットを構成する位置情報、姿勢情報を位置姿勢保存部１７０に保存する。

（ステップＳ１０３２）
ステップＳ１０３２では、位置姿勢保存部１７０には、ｋ（１≦ｋ＜３）フレーム分の位置情報、姿勢情報しか格納されていないので、ｐ_１〜ｐ_ｋ、ｑ_１〜ｑ_ｋを用いてステップＳ１０４０〜ステップＳ１０９０の処理を行う。即ち、位置姿勢保存部１７０に格納されている位置情報、姿勢情報が何フレーム分であるのかが異なる以外、ステップＳ１０３２において行う処理と、ステップＳ１０４０〜ステップＳ１０９０で行う処理とは実質的には同じである。

（ステップＳ１０９５）
ステップＳ１０９５では、図４のフローチャートに従った処理の終了指示が外部から入力された、若しくは終了する条件が満たされた場合には、本処理の終了させる。一方、終了指示は入力されていないし、終了する条件も満たされていない場合には、処理をステップＳ１０２０に戻し、以降の処理を繰り返す。

以上の説明により、本実施形態によれば、撮像装置１００Ｌの高速な動きなどによって位置合わせが破綻することを回避することができ、自然特徴を利用した位置合わせの安定性を向上させることができる。

＜変形例＞
以上の説明では、各フレームにつきエッジ検出は一回しか行わなかった。すなわち、位置姿勢保存部１７０に保存された最新の位置情報、姿勢情報を用いて、エッジ検出を一回行っていた。しかしながら、エッジ検出は１フレームに一回だけ行うことに限るものではない。

エッジ検出処理にかける時間に余裕があれば、上記各セット毎にモデル投影を行って、エッジ検出を行ってもよい。上記各セット毎にエッジ検出を行うことによって、反復演算の収束性が向上することが期待されるため、位置姿勢計測の精度が向上する。

［第２の実施形態］
第１の実施形態では、フレーム毎に、１組の位置情報、姿勢情報を位置姿勢保存部１７０に格納していた。本実施形態では、フレーム毎に、複数組の位置情報、姿勢情報を位置姿勢保存部２７０に格納する。

なお、以下の説明において、特に触れないものについては、第１の実施形態と同じであるとする。

図１０は、本実施形態に係る位置姿勢計測装置２の機能構成を示すブロック図である。同図において図１に示したものと同じものについては同じ番号を付けており、その説明は省略する。

位置姿勢保存部２７０は、過去数フレーム分の画像（例えば現フレームから過去３フレーム分の画像）のそれぞれについて、位置姿勢算出部１４０が算出した撮像装置１００Ｌの複数の位置及び姿勢を保存する。なお、位置姿勢保存部２７０は、過去数フレーム分の画像（例えば現フレームから過去３フレーム分の画像）のそれぞれについて、位置姿勢算出部１４０が算出した撮像装置１００Ｌの複数の位置及び姿勢のうち、最も妥当性の高いもの（主要位置、主要姿勢）を、その他のものとは識別可能なように保存している。

本実施形態では、位置姿勢保存部２７０には、１つ前のフレームにおいて位置姿勢算出部１４０が求めた撮像装置１００Ｌの複数の位置及び姿勢、２つ前のフレームにおいて位置姿勢算出部１４０が求めた撮像装置１００Ｌの複数の位置及び姿勢、３つ前のフレームにおいて位置姿勢算出部１４０が求めた撮像装置１００Ｌの複数の位置及び姿勢、が保存されるものとする。もちろん、各フレームにおける主要位置、主要姿勢については、その他のものとは識別可能なように保存される。

なお、最初のフレームの画像（例えば１フレーム目の画像）を用いて算出された撮像装置１００Ｌの位置及び姿勢を位置姿勢保存部２７０に保存する前には、第１の実施形態で説明したような初期化処理から得られる撮像装置１００Ｌの初期位置及び初期姿勢をそれぞれ、主要位置、主要姿勢として保存しているものとする。

画像特徴検出部２２０は、位置姿勢保存部２７０に保存された主要位置、主要姿勢を用いて、第１の実施形態と同様の処理でもって、画像からエッジの検出を行う。

位置姿勢生成部２６０は、位置姿勢保存部２７０に保存されている複数の位置及び姿勢から、複数の位置及び姿勢を生成する。

位置姿勢選択部２５０は、位置姿勢算出部１４０が算出した複数の位置及び姿勢の中から、同一の位置及び姿勢と見なせるものを統合するとともに、最も妥当な位置及び姿勢を主要位置、主要姿勢として選択し、位置姿勢保存部２７０に保存する。また、妥当性が上位の複数の位置及び姿勢を位置姿勢保存部２７０に保存する。なお、位置姿勢選択部２５０が選択した主要位置及び主要姿勢を用いて、仮想物体２０の画像が描画される。

次に、位置姿勢計測装置２が行う、撮像装置１００Ｌの位置及び姿勢を求める処理について、同処理のフローチャートを示す図１１を用いて説明する。

（ステップＳ２０１０）
ステップＳ２０１０では、位置姿勢保存部２７０に、撮像装置１００Ｌの初期位置を示す初期位置情報、初期姿勢を示す初期姿勢情報をそれぞれ、主要位置情報、主要姿勢情報として格納する。本ステップにおいて行われる処理は上記ステップＳ１０１０と同じである。

（ステップＳ２０２０）
ステップＳ２０２０では、上記ステップＳ１０２０と同様に、撮像装置１００Ｌが撮像した画像（現実空間画像）のデータを画像入力部１１０が取得し、後段の画像特徴検出部２２０に転送する。

（ステップＳ２０３０）
ステップＳ２０３０では、画像特徴検出部２２０は、位置姿勢保存部２７０に保存された主要位置、主要姿勢を用いて、上記ステップＳ１０３０と同様の処理でもって、画像からエッジの検出を行う。

（ステップＳ２０３１）
ステップＳ２０３１では、位置姿勢算出部１４０は、３フレーム分の位置情報、姿勢情報が位置姿勢保存部２７０に格納されているか否かをチェックする。係るチェックの結果、格納されている場合には処理をステップＳ２０４０に進める。一方、格納されていない場合には、処理をステップＳ２０３２に進める。

（ステップＳ２０４０）
次にステップＳ２０４０では先ず、位置姿勢生成部２６０は、位置姿勢保存部２７０に保存されている位置情報（p_org ^t-1（ｊ） （ｊ＝１，２，…，Ｎ）、p_org ^t-2、p_org ^t-3を用いて、複数の位置情報ｐ_ｉ（ｊ） （ｉ＝１，２，…，Ｉ）を求める。なお、位置情報は第１の実施形態と同様、３次元空間における撮像装置１００Ｌの位置を表す３次元ベクトルである。

ここで、p_org ^t-1（ｊ）は、１フレーム前において位置姿勢保存部２７０に保存されたＮ個の位置情報のうちｊ番目の位置情報である。位置情報p_org ^t-1（ｊ）には、１フレーム前における主要位置情報も含まれている。また、p_org ^t-2は、２フレーム前において位置姿勢保存部２７０に保存された主要位置情報である。また、p_org ^t-3は、３フレーム前において位置姿勢保存部２７０に保存された主要位置情報である。

また、位置姿勢生成部２６０は、位置姿勢保存部２７０に保存されている姿勢情報（ｑ_org ^t-1（ｊ） （ｊ＝１，２，…，Ｎ）、ｑ_org ^t-2、ｑ_org ^t-3を用いて、複数の姿勢情報ｑ_ｉ（ｊ） （ｉ＝１，２，…，Ｉ）を求める。なお、姿勢情報は第１の実施形態と同様、撮像装置１００Ｌの姿勢を表すクォータニオンである。

ここで、ｑ_org ^t-1（ｊ）は、１フレーム前において位置姿勢保存部２７０に保存されたＮ個の姿勢情報のうちｊ番目の姿勢情報である。姿勢情報ｑ_org ^t-1（ｊ）には、１フレーム前における主要姿勢情報も含まれている。また、ｑ_org ^t-2は、２フレーム前において位置姿勢保存部２７０に保存された主要姿勢情報である。また、ｑ_org ^t-3は、３フレーム前において位置姿勢保存部２７０に保存された主要姿勢情報である。

ここで、生成する位置情報、姿勢情報の数（Ｉ）は、第１の実施形態と同様、撮像装置１００Ｌの動きモデルの数に対応させるので、Ｉ＝３となる。

そして、各ｊについて、ｐ_１（ｊ）、ｐ_２（ｊ）、ｐ_３（ｊ）、ｑ_１（ｊ）、ｑ_２（ｊ）、ｑ_３（ｊ）を、第１の実施形態と同様にして求める。

即ち、ｐ_１（ｊ）は、p_org ^t-1（ｊ）のみを用いて求める。ｐ_２（ｊ）は、p_org ^t-1（ｊ）、p_org ^t-2を用いて求める。ｐ_３（ｊ）は、p_org ^t-1（ｊ）、p_org ^t-2、p_org ^t-3を用いて求める。

また、姿勢情報についても同様に、ｑ_１（ｊ）は、ｑ_org ^t-1（ｊ）のみを用いて求める。ｑ_２（ｊ）は、ｑ_org ^t-1（ｊ）、ｑ_org ^t-2を用いて求める。ｑ_３（ｊ）は、ｑ_org ^t-1（ｊ）、ｑ_org ^t-2、ｑ_org ^t-3を用いて求める。

本ステップでは更に、位置姿勢生成部１６０は、ｐ_１（ｊ）〜ｐ_３（ｊ）から１つを選択したものと、ｑ_１（ｊ）〜ｑ_３（ｊ）から１つを選択したものとを組み合わせてセットにした場合に、全ての組み合わせについてセットを生成する。本実施形態の場合、Ｓｔ_ｊ（１）＝（ｐ_１（ｊ），ｑ_１（ｊ））、Ｓｔ_ｊ（２）＝（ｐ_１（ｊ），ｑ_２（ｊ））、Ｓｔ_ｊ（３）＝（ｐ_１（ｊ），ｑ_３（ｊ））、Ｓｔ_ｊ（４）＝（ｐ_２（ｊ），ｑ_１（ｊ））、Ｓｔ_ｊ（５）＝（ｐ_２（ｊ），ｑ_２（ｊ））、Ｓｔ_ｊ（６）＝（ｐ_２（ｊ），ｑ_３（ｊ））、Ｓｔ_ｊ（７）＝（ｐ_３（ｊ），ｑ_１（ｊ））、Ｓｔ_ｊ（８）＝（ｐ_３（ｊ），ｑ_２（ｊ））、Ｓｔ_ｊ（９）＝（ｐ_３（ｊ），ｑ_３（ｊ））の９セットが生成されることになる。そして係るセットは、ｊ＝１〜Ｎの全てについて求めるので、結果としてセットは９×Ｎ（＝Ｍ）個生成されることになる。

（ステップＳ２０５０）
ステップＳ２０５０では、変数ｉの値、変数ｊの値を共に１に初期化する。

（ステップＳ２０６０）
ステップＳ２０６０では、非線形最適化計算を用いて、Ｓｔ_ｊ（ｉ）を構成する位置情報、姿勢情報を反復演算により補正することで、撮像装置１００Ｌの位置及び姿勢を算出する。係る処理については、上記ステップＳ１０６０と同様にして行う。

（ステップＳ２０７０）
ステップＳ２０７０では、ｉ＝９であるか否かをチェックする。係るチェックの結果、ｉ＝９である場合には処理をステップＳ２０７１に進め、ｉ＜９である場合には処理をステップＳ２０８０に進める。

（ステップＳ２０８０）
ステップＳ２０８０では、変数ｉの値を１つインクリメントし、処理をステップＳ２０６０に戻す。

（ステップＳ２０７１）
ステップＳ２０７１では、ｊ＝Ｎであるか否かをチェックする。係るチェックの結果、ｊ＝Ｎである場合には処理をステップＳ２０９０に進め、ｊ＜Ｎである場合には、処理をステップＳ２０７２に進める。

（ステップＳ２０７２）
ステップＳ２０７２では、変数ｊの値を１つインクリメントする。

（ステップＳ２０７３）
ステップＳ２０７３では、変数ｉの値を１に初期化し、処理をステップＳ２０６０に戻す。

（ステップＳ２０９０）
ステップＳ２０９０では、位置姿勢選択部２５０は、ステップＳ２０６０において算出されたＭ個の位置及び姿勢のうち、同一の位置及び姿勢と見なせるものについては統合する。そして、最も妥当な位置及び姿勢を選択し、主要位置情報、主要姿勢情報として位置姿勢保存部２７０に保存する。また、妥当性が上位の複数の位置及び姿勢を位置姿勢保存部２７０に保存する。

図１２は、ステップＳ２０９０におけるの処理の詳細を示すフローチャートである。

先ずステップＳ２１１０では、位置姿勢選択部２５０は、ステップＳ２０６０において算出されたＭ個のセット（位置及び姿勢）のうち、位置姿勢保存部２７０に格納する数（最大値）Ｔ（上記Ｎに相当）を設定する。係る最大数Ｔは予め定められたものでも良いし、ユーザが適宜選択入力するようにしても良い。

次にステップＳ２１２０では、位置姿勢選択部２５０は、ステップＳ２０６０において算出されたＭ個のセットのそれぞれについて、上記式１９に基づいて妥当性Ｃを求める。そして、このＭ個のセットを、妥当性Ｃの大きい順に並び替える。そして各セットの処理済みフラグFk（k=1,2,...,M）をFALSEにセットする。

次にステップＳ２１３０では、変数ｉ、ｊのそれぞれの値を１に初期化する。

ステップＳ２１４０では、ソートされた各セットにおいて、ｊ番目のセット中の位置及び姿勢と同一と見なせる位置及び姿勢を含むセットk（k=j+1,j+2,...,M）を探索し、検索したセットの処理済みフラグFkをTRUEにセットする。位置及び姿勢が同一かどうかは次のように判定する。

ここで、ｊ番目のセットを構成する位置情報をｐ_ｊ、姿勢情報をｑ_ｊと表す。更に、ｊ番目のセットと比較する対象のセットを構成する位置情報をp_comp、姿勢情報をq_compと表す。

このような場合、２つの位置情報の差は次式のように2点間の距離として計算される。

また、２つの姿勢情報の差はを次式のように算出する。まず次のようにクォータニオンｑ_ｄｉｆを算出する。

ｑ_ｄｉｆの回転角をθとした場合、クォータニオンｑ_ｄｉｆの実数部分ａ_ｄｉｆはｃｏｓ（θ／２）を表す。2つの姿勢の差を次式のように回転角θの絶対値として算出する。

同一位置及び同一姿勢であるかどうかは、式２０，２２によって計算される値の両方が、予め設定した閾値より小さいか否かによって決定される。どちらも予め設定した閾値より小さければ、2つの位置及び姿勢は同一であるとみなす。

次にステップＳ２１５０では、セットk(k=j+1,j+2,...,M)の処理済みフラグがＴＲＵＥであるか否かをチェックする。係るチェックの結果、全てのセットの処理済みフラグがＴＲＵＥである場合には本処理を終了し、処理をステップＳ２０９５にリターンする。一方、処理済みフラグがＴＲＵＥではないセットが１つでも存在する場合には処理をステップＳ２１６０に進める。

次にステップＳ２１６０では、処理済みフラグがＦＡＬＳＥであり、且つｊ番目のセットに対する妥当性Ｃの次に高い妥当性Ｃを有するセットの、上記ソートによる最上位（ｊ＝１）からの順位を変数ｊにセットする。

ステップＳ２１７０では、変数ｉの値を１つインクリメントする。

次にステップＳ２１８０では、ｉ＞Ｔであるか否かをチェックする。係るチェックの結果、ｉ＞Ｔである場合には本処理を終了し、処理をステップＳ２０９５にリターンする。一方、ｉ≦Ｔである場合には、処理をステップＳ２１４０に戻し、以降の処理を行う。

図１２のフローチャートに従った処理の終了後、処理済みフラグがＦＡＬＳＥであるセットのうち、妥当性Ｃの大きいものから上位Ｔ個が、位置姿勢保存部２７０に保存される。なお、この保存されたＴ個のセットのうち、最も妥当性Ｃの大きいセットを構成する位置情報、姿勢情報がそれぞれ、主要位置情報、主要姿勢情報となる。

（ステップＳ２０３２）
ステップＳ２０３２では、位置姿勢保存部２７０には、ｋ（１≦ｋ＜３）フレーム分の位置情報、姿勢情報しか格納されていないので、ｐ_１（ｊ）〜ｐ_ｋ（ｊ）、ｑ_１（ｊ）〜ｑ_ｋ（ｊ）を用いてステップＳ２０４０〜ステップＳ２０９０の処理を行う。即ち、位置姿勢保存部２７０に格納されている位置情報、姿勢情報が何フレーム分であるのかが異なる以外、ステップＳ２０３２において行う処理と、ステップＳ２０４０〜ステップＳ２０９０で行う処理とは実質的には同じである。

（ステップＳ２０９５）
図１１に戻って、次にステップＳ２０９５では、図１１のフローチャートに従った処理の終了指示が外部から入力された、若しくは終了する条件が満たされた場合には、本処理の終了させる。一方、終了指示は入力されていないし、終了する条件も満たされていない場合には、処理をステップＳ２０２０に戻し、以降の処理を繰り返す。

以上の説明により、本実施形態によれば、前フレームから得られる複数の位置及び姿勢をもとに複数の位置及び姿勢を発生させ、それぞれについて非線形最適化により撮像装置の位置及び姿勢を求め、その中から最も妥当な位置及び姿勢を選択することにより、位置姿勢算出が破綻することを防止することが可能になる。

＜変形例＞
第２の実施形態では、各フレームにつきエッジ検出は一回しか行わなかった。すなわち、位置姿勢保存部２７０に保存された主要位置姿勢に基づきエッジ検出を一回行っていた。しかしながら、エッジ検出は１フレームに一回に限るものではない。エッジ検出処理にかける時間に余裕があれば、位置姿勢生成部２６０にて生成された各位置及び姿勢に応じてモデル投影を行って、エッジ検出を行ってもよい。また、位置姿勢保存部２７０に保存されているN個の位置及び姿勢に基づいてモデル投影を行ってエッジ検出を行ってもよい。なるべく近い値の位置及び姿勢を用いてエッジ検出を行うことによって、反復演算の収束性が向上することが期待されるため、位置姿勢計測の精度が向上する。

［第３の実施形態］
以下に、各種の変形例を示す。

［変形例１］
第１，２の実施形態において、エッジ検出を行う際に、各分割点について一つのエッジを対応点として検出していた。しかしながら、検出される対応点の数は一つに限るものではなく、非特許文献４に示すように、複数のエッジを対応点候補として検出してもよい。

位置姿勢算出部１４０において、位置姿勢算出の際に、ある位置及び姿勢に基づいて画像上に投影された線分に最も近い対応点候補を用いる。複数の対応点候補を持ち反復演算の繰り返しステップごとに対応点を変化させることにより、初期値の精度が悪い場合に誤対応の影響を受けにくくなる。

［変形例２］
第１，２の実施形態において、位置姿勢選択部１５０、２５０は、位置及び姿勢の妥当性に基づいて、位置及び姿勢を選択するだけであった。しかしながら、最も妥当性の高い位置及び姿勢の妥当性が十分でない場合、即ち式１９により算出される妥当性がある閾値以下の場合には、位置姿勢算出が破綻していると考えられるため、位置姿勢計測装置に再初期化処理を促してもよい。

再初期化処理は、例えば図４のステップＳ１０１０のような処理により自動的に行ってもよいし、ユーザに再初期化するように通知してもよい。これによって、位置姿勢算出が破綻しても、再初期化を行って再び位置姿勢算出を継続することが可能になる。

［変形例３］
第１，２の実施形態では、位置姿勢生成部１６０、２６０において、静止モデル、等速度／等角速度モデル、等加速度／等角加速度モデルといった運動モデルに基づいて位置及び姿勢を生成していた。しかしながら、位置及び姿勢の生成方法はこれに限るものではなく、例えば、位置姿勢保存部１７０、または２７０に保存されている位置及び姿勢を起点としたランダムな動きによって生成してもよい。また、上述の運動モデルと、ランダムな動きの両方から新たな位置及び姿勢を生成してもよい。その他、物体の動きを規定するものであればいかなる動きモデルであってもよい。

［変形例４］
第１，２の実施形態では、位置姿勢選択部１５０、２５０において、ロバスト推定の重みの平均値に基づいて位置及び姿勢の妥当性を決定していた。しかしながら、妥当性の決定方法はこれに限るものではない。

例えば、算出された位置及び姿勢を用いて再度3次元モデルの線分を描画し、該描画された線分と対応点との距離の総和を用いてもよい。また、前述の距離をロバスト推定の重みで重み付けした距離の総和を用いてもよい。さらには、前述の距離に閾値に対して閾値を設定し、該閾値より距離が小さい対応点の数を用いてもよい。また、重みが0でない分割点の総数、分割点の総数に対する重みが0でない分割点の割合などでもよい。このように計算された位置姿勢の妥当性を評価するものであればいかなる方法であってもよい。

［変形例５］
第１，２の実施形態では、同一位置であるかどうかの判定は二点間の距離、同一姿勢であるかどうかの判定は二つの姿勢の間の回転角の絶対値に基づいて行っていた。しかしながら、同一位置、同一姿勢の判定方法はこれに限るものではない。

例えば、姿勢を表すクォータニオンを4次元ベクトルと考えて、4次元ベクトルの距離に基づいて判定してもよい。また、姿勢をクォータニオンではなく3×3の回転行列で表して、2つの姿勢の間の相対姿勢を表す回転行列が、単位行列に近いかどうかにより判定してもよい。さらには、位置と姿勢を両方含む4×4の変換行列を用いて、2つの4×4変換行列の各要素の差分を用いて判定してもよい。このように、二つの位置と姿勢の差を測るものであれば、同一位置、同一姿勢の判定方法はいかなるものであってもよい。

［変形例６］
第１、２の実施形態では、自然特徴としてエッジを用いたが、これに限るものではない。例えば、Harris検出器や非特許文献８に示されるSIFT検出器などによって検出される点特徴を利用してもよい。この場合、3次元モデルとしては、点特徴の基準座標系における3次元位置と、特徴周辺における画像情報がモデルデータ保存部に保存される。また、自然特徴に限らず、人工的なマーカであっても良い。

［第４の実施形態］
図１，１０にそれぞれ示した位置姿勢計測装置１，２を構成する各部は、第１２の実施形態ではそれぞれハードウェアで構成されているものとして説明した。しかし、モデルデータ保存部１３０、位置姿勢保存部１７０、２７０を除く各部についてはソフトウェアでもって構成するようにしても良い。この場合、このソフトウェアをＰＣ（パーソナルコンピュータ）等のコンピュータに読み込ませ、このコンピュータが有するＣＰＵが係るソフトウェアを実行することで、このコンピュータは、第１，２の実施形態で説明したような動作を行うことになる。

図１４は、位置姿勢計測装置１、２に適用可能なコンピュータのハードウェア構成例を示すブロック図である。

１４０１はＣＰＵで、ＲＡＭ１４０２やＲＯＭ１４０３に格納されているプログラムやデータを用いて本コンピュータ全体の制御を行うと共に、本コンピュータを適用する位置姿勢計測装置１、２が行う上記各処理を実行する。

１４０２はＲＡＭで、外部記憶装置１４０６からロードされたプログラムやデータ、Ｉ／Ｆ（インターフェース）１４０７を介して撮像装置１００Ｌから送出される各フレームの画像データを一時的に記憶するためのエリアを有する。また、ＣＰＵ１４０１が各種の処理を実行する際に用いるワークエリアも有する。即ち、ＲＡＭ１４０２は各種のエリアを適宜提供することができる。

１４０３はＲＯＭで、本コンピュータの設定データや、ブートプログラムなどを格納する。

１４０４は操作部で、キーボードやマウスなどにより構成されており、本コンピュータの操作者が操作することで、各種の指示をＣＰＵ１４０１に対して入力することができる。

１４０５は表示部で、ＣＲＴや液晶画面等により構成されており、ＣＰＵ１４０１による処理結果を画像や文字などでもって表示することができる。

１４０６は、ハードディスクドライブ装置に代表される大容量情報記憶装置として機能する外部記憶装置である。ここには、ＯＳ（オペレーティングシステム）や、図１，１１に示したモデルデータ保存部１３０、位置姿勢保存部１７０、２７０を除く各部の機能をＣＰＵ１４０１に実行させるためのプログラムやデータなどが保存されている。また、予め設定されているものとして説明した情報、既知の情報として説明したものについても、この外部記憶装置１４０６に保存されてる。外部記憶装置１４０６に保存されているプログラムやデータはＣＰＵ１４０１による制御に従って適宜ＲＡＭ１４０２にロードされる。ＣＰＵ１４０１はこのロードされたプログラムやデータを用いて処理を実行することで、本コンピュータは位置姿勢計測装置１、２が行う上述の各処理を実行することになる。

１４０７はＩ／Ｆで、上記撮像装置１００Ｌを本コンピュータに接続するためのものであり、撮像装置１００Ｌが撮像した動画像を構成する各フレームの画像データは、このＩ／Ｆ１４０７を介してＲＡＭ１４０２や外部記憶装置１４０６に送出される。

１４０８は上述の各部を繋ぐバスである。

なお、図１４に示した構成は一例であり、係る構成に限定するものではない。

［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。

また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれたとする。その後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

本発明を上記記録媒体に適用する場合、その記録媒体には、先に説明したフローチャートに対応するプログラムコードが格納されることになる。

本発明の第１の実施形態に係る位置姿勢計測装置１の機能構成を示すブロック図である。 撮像装置１００Ｌ、１００Ｒを装着したユーザと、このユーザが観察する空間とを模式的に示した図である。 本発明の第１の実施形態における３次元モデルの定義方法を説明する図である。 位置姿勢計測装置１が行う、撮像装置１００Ｌの位置及び姿勢を求める処理のフローチャートである。 ステップＳ１０３０における処理の詳細を示すフローチャートである。 ステップＳ１１１０において行われる、現実空間画像上への３次元モデルの投影を示す図である。 ３次元モデルの現実空間画像上での分割点を示す図である。 本発明の第１の実施形態におけるエッジ検出方法を説明する図である。 エッジの情報を利用して撮像装置１００Ｌの位置及び姿勢を算出する方法を説明する図である。 本発明の第２の実施形態に係る位置姿勢計測装置２の機能構成を示すブロック図である。 位置姿勢計測装置２が行う、撮像装置１００Ｌの位置及び姿勢を求める処理のフローチャートである。 ステップＳ２０９０におけるの処理の詳細を示すフローチャートである。 ステップＳ１０４０における位置及び姿勢の生成方法について説明する図である。 位置姿勢計測装置１、２に適用可能なコンピュータのハードウェア構成例を示すブロック図である。

Claims (16)

1. 現実空間の動画像を撮像する撮像装置に接続されている情報処理装置であって、
   前記動画像を構成する各フレームの画像を前記撮像装置から取得する毎に、取得したフレームの画像の撮像時における前記撮像装置の撮像位置、撮像姿勢を求める計算手段と、
   前記計算手段が求めた撮像位置及び撮像姿勢を示す撮像位置姿勢情報を、メモリに記録する記録手段とを備え、
   前記計算手段は、
   前記メモリに記録されている複数の撮像位置姿勢情報に基づいて、予め設定された個数の候補位置姿勢情報を求める候補位置姿勢算出手段と、
   前記取得したフレームの画像中における観察対象物体の画像特徴を検出する検出手段と、
   前記候補位置姿勢算出手段が求めたそれぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報からそれぞれ異なる候補位置情報及び候補姿勢情報の組み合わせを複数個生成し、該生成した複数個の組み合わせと前記画像特徴に対応する既知の３次元情報とから推定される該画像特徴の前記取得したフレームの画像中における推定位置と、前記検出手段による前記画像特徴の検出位置と、の誤差に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報の補正値を算出し、該補正値に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報を補正する補正手段と、
   前記補正手段が補正した前記それぞれの候補位置姿勢情報について評価値を求め、求めたそれぞれの評価値に基づいて、該それぞれの候補位置姿勢情報のうち１つを選択する選択手段とを備え、
   前記記録手段は、
   前記選択手段が選択した候補位置姿勢情報を、前記撮像位置姿勢情報として前記メモリに記録する
   ことを特徴とする情報処理装置。
2. 前記候補位置姿勢算出手段は、複数の動きモデルに基づいて候補位置姿勢情報を求めることを特徴とする請求項１に記載の情報処理装置。
3. 前記候補位置姿勢算出手段は、位置と姿勢について同一または異なる動きモデルを用いて独立に候補位置情報及び候補姿勢情報を求めて候補位置姿勢情報とすることを特徴とする請求項２に記載の情報処理装置。
4. 前記動きモデルには、静止、等速度、等加速度、ランダムが含まれていることを特徴とする請求項２乃至３の何れか１項に記載の情報処理装置。
5. 前記補正手段は、前記それぞれの候補位置姿勢情報について、
   前記補正値の算出の際に、画像特徴毎に前記誤差の大小に応じた重みを求めることを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記選択手段は、前記それぞれの候補位置姿勢情報について、前記重みの平均値を前記評価値として算出し、当該平均値の最も大きい候補位置姿勢情報を選択することを特徴とする請求項５に記載の情報処理装置。
7. 前記選択手段は、前記補正手段によって補正されたそれぞれの候補位置姿勢情報を用いて３次モデルの線分を描画して得られる画像上の該線分の画像特徴の位置と、前記取得したフレームの画像中における画像特徴の位置と、の距離の総和を求め、該総和を用いて、候補位置姿勢情報を選択することを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
8. 前記選択手段は、前記補正手段によって補正されたそれぞれの候補位置姿勢情報を用いて３次モデルの線分を描画して得られる画像上の該線分の画像特徴の位置と、前記取得したフレームの画像中における画像特徴の位置と、の距離の総和を求め、該総和に前記重みを乗じた値を算出し、該算出した値を用いて候補位置姿勢情報を選択することを特徴とする請求項５に記載の情報処理装置。
9. 前記補正手段は、前記評価値が所定の値以下の場合に、再初期化処理を行うことを特徴とする請求項１乃至８のいずれか１項に記載の情報処理装置。
10. 前記画像特徴は、画像中におけるエッジであり、前記３次元情報は当該エッジの集合によって表現されるものであることを特徴とする請求項１乃至９のいずれか１項に記載の情報処理装置。
11. 現実空間の動画像を撮像する撮像装置に接続されている情報処理装置であって、
    前記動画像を構成する各フレームの画像を前記撮像装置から取得する毎に、取得したフレームの画像の撮像時における前記撮像装置の撮像位置、撮像姿勢の候補として予め設定された個数の撮像位置、撮像姿勢を求める計算手段と、
    前記予め設定された個数の撮像位置及び撮像姿勢を示す撮像位置姿勢情報を、メモリに記録する記録手段とを備え、
    前記計算手段は、
    前記メモリに記録されている複数の撮像位置姿勢情報に基づいて、予め設定された個数の候補位置姿勢情報を求める候補位置姿勢算出手段と、
    前記取得したフレームの画像中における観察対象物体の画像特徴を検出する検出手段と、
    前記候補位置姿勢算出手段が求めたそれぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報からそれぞれ異なる候補位置情報及び候補姿勢情報の組み合わせを複数個生成し、該生成した複数個の組み合わせと前記画像特徴に対応する既知の３次元情報とから推定される該画像特徴の前記取得したフレームの画像中における推定位置と、前記検出手段による前記画像特徴の検出位置と、の誤差に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報の補正値を算出し、該補正値に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報を補正する補正手段と、
    前記補正手段が補正した前記それぞれの候補位置姿勢情報について評価値を求め、求めたそれぞれの評価値に基づいて、該それぞれの候補位置姿勢情報のうち予め設定された個数の候補位置姿勢情報を選択する選択手段とを備え、
    前記記録手段は、
    前記選択手段が選択した予め設定された個数の候補位置姿勢情報を、最も評価値の大きい候補位置姿勢情報とそれ以外の候補位置姿勢情報とを区別して、前記撮像位置姿勢情報として前記メモリに記録する
    ことを特徴とする情報処理装置。
12. 前記選択手段は、参照した候補位置姿勢情報が示す位置及び姿勢に近い位置、姿勢を有するものとして判断された候補位置姿勢情報を、被統合候補位置姿勢情報として前記参照した候補位置姿勢情報に統合し、
    前記被統合候補位置姿勢情報以外の候補位置姿勢情報のうち、評価値の大きいものから上位予め設定された個数分の候補位置姿勢情報を選択することを特徴とする請求項１１に記載の情報処理装置。
13. 現実空間の動画像を撮像する撮像装置に接続されている情報処理装置が行う情報処理方法であって、
    前記動画像を構成する各フレームの画像を前記撮像装置から取得する毎に、取得したフレームの画像の撮像時における前記撮像装置の撮像位置、撮像姿勢を求める計算工程と、
    前記計算工程で求めた撮像位置及び撮像姿勢を示す撮像位置姿勢情報を、メモリに記録する記録工程とを備え、
    前記計算工程は、
    前記メモリに記録されている複数の撮像位置姿勢情報に基づいて、予め設定された個数の候補位置姿勢情報を求める候補位置姿勢算出工程と、
    前記取得したフレームの画像中における観察対象物体の画像特徴を検出する検出工程と、
    前記候補位置姿勢算出工程で求めたそれぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報からそれぞれ異なる候補位置情報及び候補姿勢情報の組み合わせを複数個生成し、該生成した複数個の組み合わせと前記画像特徴に対応する既知の３次元情報とから推定される該画像特徴の前記取得したフレームの画像中における推定位置と、前記検出工程による前記画像特徴の検出位置と、の誤差に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報の補正値を算出し、該補正値に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報を補正する補正工程と、
    前記補正工程で補正した前記それぞれの候補位置姿勢情報について評価値を求め、求めたそれぞれの評価値に基づいて、該それぞれの候補位置姿勢情報のうち１つを選択する選択工程とを備え、
    前記記録工程では、
    前記選択工程で選択した候補位置姿勢情報を、前記撮像位置姿勢情報として前記メモリに記録する
    ことを特徴とする情報処理方法。
14. 現実空間の動画像を撮像する撮像装置に接続されている情報処理装置が行う情報処理方法であって、
    前記動画像を構成する各フレームの画像を前記撮像装置から取得する毎に、取得したフレームの画像の撮像時における前記撮像装置の撮像位置、撮像姿勢の候補として予め設定された個数の撮像位置、撮像姿勢を求める計算工程と、
    前記予め設定された個数の撮像位置及び撮像姿勢を示す撮像位置姿勢情報を、メモリに記録する記録工程とを備え、
    前記計算工程は、
    前記メモリに記録されている複数の撮像位置姿勢情報に基づいて、予め設定された個数の候補位置姿勢情報を求める候補位置姿勢算出工程と、
    前記取得したフレームの画像中における観察対象物体の画像特徴を検出する検出工程と、
    前記候補位置姿勢算出工程で求めたそれぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報からそれぞれ異なる候補位置情報及び候補姿勢情報の組み合わせを複数個生成し、該生成した複数個の組み合わせと前記画像特徴に対応する既知の３次元情報とから推定される該画像特徴の前記取得したフレームの画像中における推定位置と、前記検出工程による前記画像特徴の検出位置と、の誤差に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報の補正値を算出し、該補正値に基づいて、前記それぞれの候補位置姿勢情報を構成する候補位置情報及び候補姿勢情報を補正する補正工程と、
    前記補正工程で補正した前記それぞれの候補位置姿勢情報について評価値を求め、求めたそれぞれの評価値に基づいて、該それぞれの候補位置姿勢情報のうち予め設定された個数の候補位置姿勢情報を選択する選択工程とを備え、
    前記記録工程では、
    前記選択工程で選択した予め設定された個数の候補位置姿勢情報を、最も評価値の大きい候補位置姿勢情報とそれ以外の候補位置姿勢情報とを区別して、前記撮像位置姿勢情報として前記メモリに記録する
    ことを特徴とする情報処理方法。
15. コンピュータに請求項１３又は１４に記載の情報処理方法を実行させるためのコンピュータプログラム。
16. 請求項１５に記載のコンピュータプログラムを格納したことを特徴とする、コンピュータ読み取り可能な記憶媒体。

Images