JP4859205B2 - 情報処理装置、情報処理方法、及びプログラム - Google Patents

Description

本発明は、物体の位置及び姿勢、特に撮像装置の位置及び姿勢を計測する情報処理装置、情報処理方法、及びプログラムに関するものである。

近年、現実空間と仮想空間の繋ぎ目のない結合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）を重畳描画した画像を表示する装置として実現することができる。

このような画像表示装置を実現するには、現実空間中に定義した基準座標系（重畳しようとする仮想物体の位置及び姿勢を決定する基準となる現実空間中の座標系）と、撮像装置の座標系（カメラ座標系）との間の、相対的な位置及び姿勢を計測することが不可欠である。これは、仮想物体（仮想空間画像）を現実空間上の位置に合わせて描画するには、基準座標系に対する撮像装置の現実のカメラパラメータと同一のカメラパラメータを用いて仮想物体の画像を生成しなければならないからである。例えば、現実の室内のある位置に仮想物体を重畳表示する場合には、部屋に基準座標系を定義し、その基準座標系における撮像装置の位置及び姿勢を求めればよい。また、観察者が手に保持する現実の箱に何らかの仮想の模様やラベルを重畳表示する場合には、箱自身の物体座標系を基準座標系と考え、撮像装置に対する箱（基準座標系）の位置及び姿勢を求めればよい。

撮像装置の位置及び姿勢を計測する方法として、現実空間中に複数の指標（人為的なマーカや自然特徴など）を配置あるいは設定し、撮像装置によって撮像された画像内における指標の投影像の座標を検出して、指標の座標情報との関係に基づいて撮像装置の位置及び姿勢を求めることが一般的に行われている（例えば、非特許文献１）。しかし、このアプローチを用いる場合、常に指標が撮像されなくてはならないという制約がある。

一方、撮像装置に磁気センサや超音波センサなどを用いた６自由度位置姿勢センサを装着し、これによって計測した撮像装置の位置及び姿勢の誤差を、指標を撮像した画像を用いて得られる情報（画像情報）によって補正する試みがなされている（例えば、特許文献１，特許文献２）。特許文献２で開示されている方法では、撮像画像中に指標が検出された場合にはその情報に基づいてセンサ計測値の誤差が補正され、指標が検出されない場合には６自由位置姿勢センサの計測値がそのまま撮像装置の位置及び姿勢として使用される。指標の検出の有無に関わらずに撮像装置の位置及び姿勢が得られるため、複合現実感の提示を安定して行うことができる。

また、特許文献２の方法では、検出された指標の数が３点以上の場合には、撮像装置の位置及び姿勢の６自由度が画像情報に基づいて算出され、検出された指標の数が２点及び１点の場合には、センサによって計測された撮像装置の位置あるいは姿勢のいずれか（２または３自由度）のみを補正する処理が適用される。すなわち、検出された指標の数を判定基準として、撮像装置の位置及び姿勢の算出に利用するアルゴリズムを切り替えることが行われている。これにより、画像情報のみからでは撮像装置の位置及び姿勢を求められない場合（撮像された指標の数が３点に満たない場合）であっても、センサ計測値を基準としながら、その誤差を極力打ち消すような補正のなされた位置及び姿勢を取得することができる。

一方、特許文献１の方法では、検出された指標の数に関わらず、センサによって計測された撮像装置の位置あるいは姿勢のいずれかのみを画像情報に基づいて補正する処理が適用される。この補正方法では、姿勢を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような回転補正値を個別に求め、それらを平均化することで姿勢計測値に対する補正値が算出される。また、位置を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような平行移動補正値を個別に求め、それらを平均化することで位置計測値に対する補正値が算出される。指標の数に関わらず補正の自由度を２または３に限定するため、情報量が不十分な場合であっても安定した解を得ることができる。

加藤ら："マーカー追跡に基づく拡張現実感システムとそのキャリブレーション",日本バーチャルリアリティ学会論文誌, vol.4, no.4, pp.607-616, 1999. J. Park, B. Jiang, and U. Neumann: "Vision-based pose computation: robust and accurate augmented reality tracking," Proc. 2nd International Workshop on Augmented Reality (IWAR'99), pp.3-12, 1999. D. G. Lowe: "Fitting parameterized three−dimensional models to images," IEEE Transactions on PAMI, vol.13, no.5, pp.441−450, 1991. 佐藤，内山，山本：ＵＧ＋Ｂ法：主観及び客観視点カメラと姿勢センサを用いた位置合わせ手法，日本バーチャルリアリティ学会論文誌，vol.10, no.3, pp.391-400, 2005. I. Skrypnyk and D. Lowe: "Scene modeling, recognition and tracking with invariant image features," Proc. 3rd International Symposium on Mixed and Augmented Reality (ISMAR'04), pp.110-119, 2004. D. Kotake, K. Satoh, S. Uchiyama, and H. Yamamoto: "A hybrid and linear registration method utilizing inclination constraint," Proc. 4th International Symposium on Mixed and Augmented Reality (ISMAR'05), pp.140-149, 2005. D.G. Lowe, "Fitting Parameterized three-dimensional models to images", IEEE Transactions on PAMI, vol. 13, no.5, pp. 441-450, 1991. 特開2003-222509号公報 特開2003-279310号公報 特開2003-344018号公報 特開2004-233334号公報

上記特許文献２の手法では、指標が３点以上観測されている際には必ず、位置及び姿勢の６自由度を画像情報から求めるアルゴリズムが選択される。しかし実際には、画像上の一部の領域に指標が偏在して観測されている場合のように、指標の数が３点以上であっても、位置及び姿勢の６自由度を安定して求めるのに十分な画像情報が入力画像中に含まれていない状況が存在する。特許文献２の手法は、このような状況下において、得られる解の精度や安定性が不十分となることがあり改善の余地があった。

一方、上記特許文献１の手法は、精度よりも安定性を重視した手法であり、画像上の一部の領域に指標が偏在して観測されている場合のような画像情報が不十分な状況下であっても、特許文献２の手法と比較して一般に安定した計測を行うことができる。これは、実際の実施系においては磁気センサのような精度は悪いものの安定性の高い６自由度センサを利用するためである。ただし、十分な画像情報が得られている場合であっても一部のパラメータの補正のみしか行わない方法であるため、画像情報を最大限に活かした精度を得る方法ではない。特に、複合現実感のための撮像装置の位置姿勢推定方法としては、撮像画像に捕らえた指標を最大限に利用して撮像装置の位置姿勢を推定する方が望ましい。これは、撮像画像の上に仮想物体を重畳表示することが目的であるため、画像面内でのずれを極力小さくすることが望まれるためである。言い換えると、特許文献２の手法は、画像情報を活かした精度を得るという目的において改善の余地があった。

以上のように、特許文献１の安定性の高さを実現するという効果と、特許文献２の画像情報を活かした高精度を得るという効果を同時に実現することは困難であった。

さらに、上記特許文献１の手法では、個々の指標に対する２次元的な補正値の平均を求めているだけであるので、全指標上での誤差の和を最小とするような最適な補正を行うことができないという点も改善の余地があった。

本発明はこのような従来技術の問題点に鑑みてなされたものであり、計測対象物体の位置及び姿勢の計測を、安定性と精度とを両立しながら行うことが可能な情報処理装置、情報処理方法、及びプログラムを提供することを目的とする。

上述の目的は、以下の装置によって達成される。

すなわち、現実空間に存在する指標を撮像装置によって撮像した画像を入力する画像入力手段と、前記画像から指標を検出する指標検出手段と、前記検出された指標が３つ以上ある場合に、該検出された指標に基づいた凸包の面積を算出する面積算出手段と、前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記算出された面積が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択手段と、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出手段とを有することを特徴とする情報処理装置。
または、現実空間に存在する指標を撮像装置によって撮像した画像を入力する画像入力手段と、前記画像から指標を検出する指標検出手段と、前記検出された指標が２つである場合に、該検出された２つの指標の間の距離を算出する距離算出手段と、前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記算出された距離が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択手段と、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出手段とを有することを特徴とする情報処理装置。
または、現実空間に存在する物体に配された指標を撮像装置によって撮像した画像を入力する画像入力手段と、前記画像から指標を検出する指標検出手段と、前記検出された指標が３つ以上ある場合に、該検出された指標に基づいた凸包の面積を算出する面積算出手段と、前記物体の位置姿勢を構成する６自由度のパラメータのうち、前記算出された面積が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択手段と、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出手段とを有することを特徴とする情報処理装置。
または、現実空間に存在する物体に配された指標を撮像装置によって撮像した画像を入力する画像入力手段と、前記画像から指標を検出する指標検出手段と、前記検出された指標が２つである場合に、該検出された２つの指標の間の距離を算出する距離算出手段と、前記物体の位置姿勢を構成する６自由度のパラメータのうち、前記算出された距離が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択手段と、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出手段とを有することを特徴とする情報処理装置。

本発明に係る位置姿勢計測装置によれば、検出された指標の数だけでなく、その分布範囲に関する評価値を考慮することにより、より適切な方法で計測対象物体の位置姿勢を計測できるそのため、従来の方法と比べて安定性と精度に優れた計測が実現できる。

以下添付図面を参照して、本発明を好適な実施形態に従って詳細に説明する。
［第１の実施形態］
本実施形態に係る位置姿勢計測装置は、撮像装置と計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０によって構成される。そして、撮像装置１３０及び、撮像装置１３０と計測対象物体１７０に取り付けられた位置姿勢センサ１４０（１４０ａ，１４０ｂ）に接続されている。

計測対象物体１７０上の複数の位置には、物体座標系（計測対象物体１７０上に定義される座標系）における位置が既知である複数個の指標Ｑ_ｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。図１の例は、Ｋ＝９、すなわち９個の指標Ｑ_１〜Ｑ_９が配置されている状況を示している。指標Ｑ_ｋは、例えば、それぞれが異なる色を有する同一形状（図では円形）のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する四角形の単色領域によって形成されるような、四角形指標を用いることも可能である。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。また指標は故意に設定されたものであっても良いし、故意に設定されたものではない、自然形状のものを用いても良い。

例えばビデオカメラである撮像装置１３０が出力する画像（以下、これを撮影画像と呼ぶ）は、位置姿勢計測装置１００に入力される。

６自由度センサである位置姿勢センサ１４０ａ及び１４０ｂは、それぞれ撮像装置１３０及び計測対象物体１７０に装着されており、基準座標系における撮像装置１３０及び計測対象物体１７０の位置及び姿勢を計測する。位置姿勢センサ１４０が出力する計測値は、位置姿勢計測装置１００に入力される。位置姿勢センサ１４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。なお、位置姿勢センサ１４０によって計測される位置及び姿勢は、磁場の歪み等の影響によって誤差を含んだものとなっている。ただし、計測範囲内にあっては連続的に安定して位置及び姿勢を計測することが可能である。

画像入力部１６０は、位置姿勢装置１００へ入力される撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。

センサ計測値入力部１５０は、位置姿勢センサ１４０（１４０ａ，１４０ｂ）から夫々の計測値を入力し、位置姿勢算出部１２０へと出力する。

指標検出部１１０は、画像入力部１６０より撮影画像を入力し、入力した画像中に撮影されている指標Ｑ_ｋの画像座標を検出する。例えば、指標Ｑ_ｋの各々が異なる色を有するマーカによって構成されている場合には、撮影画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標Ｑ_ｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを撮影画像上に施すことにより、指標の位置を検出する。また、四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、４つの直線によって形成されている領域を指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の識別子を取得する。なお、このようにして検出される四角形指標は、本明細書では、４つの頂点の個々によって形成される４つの指標であると考える。

指標検出部１１０は、さらに、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。

位置姿勢算出部１２０は、センサ計測値入力部１５０の出力である撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、計測対象物体１７０あるいは撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

なお、図１に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。

図２は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々の機能を、ソフトウェアを実行することで実現するためのコンピュータの基本構成を示す図である。

ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々のソフトウェアの実行を制御して、各部の機能を実現する。

ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備えると共に、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。

ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データなどが格納されている。キーボード１００４、マウス１００５は入力デバイスであり、操作者はこれらを用いて、各種の指示をＣＰＵ１００１に入力することができる。

表示部１００６は、ＣＲＴや液晶ディスプレイなどにより構成されており、例えば、位置姿勢計測のために表示すべきメッセージ等を表示することができる。

外部記憶装置１００７は、ハードディスクドライブなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やＣＰＵ１００１が実行するプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。

記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出して、ＲＡＭ１００２や外部記憶装置１００７に出力する。

Ｉ／Ｆ１００９は、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、位置姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポート、また、算出した位置及び姿勢を外部へ出力するためのイーサネット(登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０及びセンサ計測値入力部１５０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
上述した各構成要素は、バス１０１０によって相互に接続される。

図３は、位置姿勢算出部１２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部１２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。

ステップＳ３０００において、位置姿勢算出部１２０は、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを指標抽出部１１０から入力する。なお、各指標の物体座標系における３次元座標ｘ_Ｏ ^Ｑｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。

ステップＳ３００５において、位置姿勢算出部１２０は、撮像装置１３０及び計測対象物体１７０の位置及び姿勢のセンサによる計測値をセンサ計測値入力部１５０から入力する。

ステップＳ３０１０において、位置姿勢算出部１２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ３０８０に処理を進め、それ以外の場合はステップＳ３０２０に処理を進める。

ステップＳ３０２０において、位置姿勢算出部１２０は、検出された指標の総数が一つか否かを判定する。指標の総数が一つの場合はステップＳ３０２５に処理を進め、それ以外の場合はステップＳ３０３０に処理を進める。

ステップＳ３０２５において、位置姿勢算出部１２０は、ステップＳ３００５で得た撮像装置１３０の姿勢の計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（物体座標系における指標の３次元座標ｘ_Ｏ ^Ｑｋｎと撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値から導出する投影座標の理論値ｕ^Ｑｋｎ＊と、実際の検出座標ｕ^Ｑｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。ステップＳ３０２５における補正処理としては、周知の処理を適用することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。

ステップＳ３０３０において、位置姿勢算出部１２０は、検出された指標の総数が二つか否かを判定する。指標の総数が二つの場合はステップＳ３０３３に処理を進め、それ以外の場合はステップＳ３０４０に処理を進める。

ステップＳ３０３３において、位置姿勢算出部１２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ_１（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ_１以上の場合にはステップＳ３０３５に処理を進め、それ以外の場合はステップＳ３０２５に処理を進め、上述した処理を行う。

ステップＳ３０３５において、位置姿勢算出部１２０は、ステップＳ３００５で得た計測対象物体１７０の位置及び姿勢の計測値と、撮像装置１３０の位置の計測値を固定値として、撮像装置１３０の姿勢の計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この処理工程の詳細は後述する。

ステップＳ３０４０は、検出された指標の総数が三つ以上の場合のみ実行される。ここで、位置姿勢算出部１２０は、検出された全指標の分布範囲に関する評価値の一例として、検出された全指標の画像座標を包含する凸包を算出する。画像上の点群に対する凸包の算出方法は画像処理の基本的事項として広く公知のものであるので、その詳細な説明は省略する。

図６，図７，及び図８は、撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示している。図６に示す撮影画像６００上では指標Ｑ１，Ｑ５，Ｑ６，Ｑ９が、図７の撮影画像７００上では指標Ｑ１，Ｑ２，Ｑ６，Ｑ７が，図８の撮影画像８００上では指標Ｑ２，Ｑ６，Ｑ７が観測されており、それ以外の指標は、他の物体（本実施形態においてはユーザの手６１０）によって隠蔽されているものとする。

これらの撮影画像に対して指標の検出を行い、図３のフローチャートで示した位置姿勢算出部１２０の処理ステップを実行すると、各画像において検出される指標の数は３以上であるので、ステップＳ３０４０以降の処理が実行されることになる。図９，図１０及び図１１は、それぞれ撮影画像６００，７００，８００に対してステップＳ３０４０の処理を実行した際に得られる、検出指標の成す凸包（９００，１０００，１１００）を示している。なお、以下では、各凸包の面積をそれぞれＡ_９００，Ａ_１０００，及びＡ_１１００と表記したときに、以下に述べる閾値Ｔ_２及び閾値Ｔ_３に対して、Ａ_１１００＜Ｔ_２＜Ａ_１０００＜Ｔ_３＜Ａ_９００という関係が満たされているものとする。

ステップＳ３０５０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ_２（例えば、撮影画像全体の面積の１／１６）との比較を行う。面積が閾値Ｔ_２以上の場合はステップＳ３０６０に処理を進め、それ以外の場合はステップＳ３０２５へと処理を進める。図６〜図１１の例では、撮影画像８００についてはステップＳ３０２５へ、撮影画像７００及び６００についてはステップＳ３０６０へと処理が進行する。

ステップＳ３０６０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ_３（例えば、撮影画像全体の面積の１／９）との比較を行う。面積が閾値Ｔ_３以上の場合はステップＳ３０７０に処理を進め、それ以外の場合はステップＳ３０３５へと処理を進める。図６〜図１１の例では、撮影画像７００についてはステップＳ３０３５へ、撮影画像６００についてはステップＳ３０７０へと処理が進行する。

ステップＳ３０７０において、位置姿勢算出部１２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程の詳細は後述する。

ステップＳ３０８０において、位置姿勢算出部１２０は、ステップＳ３０２５、Ｓ３０３５及びＳ３０７０で得られた撮像装置１３０及び計測対象物体１７０の位置及び姿勢を表すデータを、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

なお、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の各工程では、撮像装置１３０に関する計測値のみに補正を加えているので、ステップＳ３０８０で出力される位置及び姿勢のデータは、計測対象物体１７０に関しては、ステップＳ３００５で入力したセンサによる計測値そのものとなる。しかし、位置及び姿勢の出力形態はこれに限られるものではなく、逆に、撮像装置１３０の位置及び姿勢の計測値には補正を加えずにセンサによる計測値をそのまま出力し、計測対象物体１７０の位置及び姿勢の計測値に補正を加えて出力しても良い。この場合は、ステップＳ３０８０において、以下に述べる座標変換処理を行った後に、変換後の位置及び姿勢を出力する。

同次座標系を用いた４×４の座標変換行列によって位置及び姿勢を表記すると、計測対象物体１７０の補正後の位置及び姿勢M_WOは、センサ計測値として得られた撮像装置１３０の位置及び姿勢M^# _WC（以下、^# はセンサによる計測値を表す記号）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢M^# _WOと、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢M_WCに基づいて、次式によって算出される。

このとき、撮像装置１３０によって定義されるカメラ座標系における計測対象物体１７０の位置及び姿勢は、

となる。すなわち、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢（M_WC）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢（M^# _WO）とを出力する場合と、センサ計測値として得られた撮像装置１３０の位置及び姿勢（M^# _WC）と、式１によって求められる計測対象物体１７０の補正後の位置及び姿勢（M_WO）とを出力する場合では、計測対象物体１７０と撮像装置１３０の相対的な位置及び姿勢の関係は等価となる。したがって、本装置の出力を利用するアプリケーションが要求する形態に合わせて、出力形態を選択できるようになっていることが好ましい。

また、カメラ座標系における計測対象物体１７０の位置及び姿勢（M_CO）を式２から算出してこれを出力しても良いし、物体座標系における撮像装置１３０の位置及び姿勢（M_CO ^-1）を求めて、これを出力してもよい。

ステップＳ３０９０において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、ステップＳ３０００へと処理を進める。
以上によって、撮像装置の位置及び姿勢が計測される。

次に、図４のフローチャートを用いて、上記ステップＳ３０３５における撮像装置１３０の姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の姿勢を、３値ベクトル
s = ω_WC = [ξ_WC ψ_WC ζ_WC]^T
によって内部的に表現している。

姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。なお、姿勢ωは、次式、

によって、３×３の回転行列Rによっても表現可能であり、ωとRとは、互いに一意に変換することができる。Rからωへの変換方法は公知であるので、その詳細な説明は省略する。

ステップＳ４０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の姿勢を、sの初期値とする。

ステップＳ４０１０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を算出する。ｕ^Ｑｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、指標Ｑ_ｋｎ各々の物体座標系における座標ｘ_Ｏ ^Ｑｋｎ（既知な情報として予め保持している）から画像座標を算出する関数：

に基づいて行われる。具体的には、観測方程式Ｆｃ（）は、ｘ_Ｏ ^Ｑｋｎとsから当該指標のカメラ座標（撮像装置１３０の視点位置を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘ_Ｃ ^Ｑｋｎを求める次式、

及び、カメラ座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める次式、

によって構成されている。ここでt_WC及びt_WOは、撮像装置１３０と計測対象物体１７０の夫々の位置計測値（平行移動成分）を表す３次元ベクトルであり、ここでは固定値として取り扱われる。また、R_WOは計測対象物体１７０の姿勢計測値（回転成分）を表す３×３の回転行列であり、同様に固定値として取り扱われる。また、ｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。

ステップＳ４０２０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、画像座標の推定値ｕ^Ｑｋｎ＊と実測値ｕ^Ｑｋｎとの誤差Δｕ^Ｑｋｎを、次式によって算出する。

ステップＳ４０３０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、sに関する画像ヤコビアン（すなわち、式４の観測方程式Ｆｃ（）をsの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列）Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘ_Ｃ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｑｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列Ｊ_ｘｓ ^Ｑｋｎ（＝∂ｘ／∂ｓ）を算出し、次式によってＪ_ｕｓ ^Ｑｋｎを算出する。

ステップＳ４０４０において、位置姿勢算出部１２０は、ステップＳ４０２０及びステップＳ４０３０において算出した誤差Δｕ^Ｑｋｎ及び画像ヤコビアンＪ_ｕｓ ^Ｑｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差Δｕ^Ｑｋｎを垂直に並べた２Ｎ次元の誤差ベクトル

及び、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎを垂直に並べた２Ｎ行×３列の行列

を作成し、Θの擬似逆行列Θ⁺を用いて、

として算出する。このように、本実施形態では補正値Δsを、局所的な線形近似によって算出している。

ステップＳ４０５０において、位置姿勢算出部１２０は、ステップＳ４０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。

ステップＳ４０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のsを用いて、再度ステップＳ４０１０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ４０７０へと処理を進める。
以上のステップＳ４０００からＳ４０６０までの工程は、複数の観測方程式からなるsに関する非線形連立方程式をニュートン法によって求解する工程である。この一連の工程は、後述する変形例５に示すとおり、他のいかなる非線形解法を用いても良い。

ステップＳ４０７０において、位置姿勢算出部１２０は、ステップＳ４０６０までの処理によって得られたsを、撮像装置１３０の姿勢の（補正後の）推定値 ω_WCとする。

以上によって、センサ計測値として得られた位置パラメータを固定して、全指標上での誤差の和を最小化するような姿勢パラメータを求めることができる。

次に、図５のフローチャートを用いて、上記ステップＳ３０７０における撮像装置１３０の位置及び姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の位置及び姿勢を、６値ベクトルs = [t_WC ^T ω_WC ^T]^T = [x_WC y_WC z_WC ξ_WC ψ_WC ζ_WC]^Tによって内部的に表現している。

ステップＳ５０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の位置及び姿勢を、sの初期値とする。

ステップＳ５０１０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値ｕ^Ｑｋｎ＊を算出する。ｕ^Ｑｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、各指標Ｑ_ｋｎの物体座標系における座標ｘ_Ｏ ^Ｑｋｎから画像座標を算出する関数：

に基づいて行われる。具体的には、観測方程式Ｆ’ｃ（）は、ｘ_Ｏ ^Ｑｋｎとsから当該指標のカメラ座標ｘ_Ｃ ^Ｑｋｎを求める式５、及び、カメラ座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める式６によって構成されている。ただしここでは、式５におけるt_WC（撮像装置１３０の位置を表す）を、固定値としてではなく、sを構成するパラメータの一部として取り扱う。一方、計測対象物体１７０の位置t_WO及び姿勢R_WOに関しては、センサによる計測値をそのまま固定値として使用する。

ステップＳ５０２０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、画像座標の推定値ｕ^Ｑｋｎ＊と実測値ｕ^Ｑｋｎとの誤差Δｕ^Ｑｋｎを、式７によって算出する。

ステップＳ５０３０において、位置姿勢算出部１２０は、各々の指標Ｑ_ｋｎに対して、sに関する画像ヤコビアン（すなわち、式１３の観測方程式Ｆ’ｃ（）をsの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）Ｊ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘ_Ｃ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘ ^Ｑｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列Ｊ_ｘｓ ^Ｑｋｎ（＝∂ｘ／∂ｓ）を算出し、式８によってＪ_ｕｓ ^Ｑｋｎを算出する。

ステップＳ５０４０において、位置姿勢算出部１２０は、ステップＳ５０２０及びステップＳ５０３０において算出した誤差Δｕ^Ｑｋｎ及び画像ヤコビアンＪ_ｕｓ ^Ｑｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差Δｕ^Ｑｋｎを垂直に並べた２Ｎ次元の誤差ベクトルＵ及び、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎを垂直に並べた２Ｎ行×６列の行列Θを作成し、Θの擬似逆行列Θ⁺を用いて、式１１によって算出する。

ステップＳ５０５０において、位置姿勢算出部１２０は、ステップＳ５０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。

ステップＳ５０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後のsを用いて、再度ステップＳ５０１０以降の処理を行う。一方、収束していると判定された場合には、ステップＳ５０７０へと処理を進める。

ステップＳ５０７０において、位置姿勢算出部１２０は、ステップＳ５０６０までの処理によって得られたsを、撮像装置１３０の補正後の位置及び姿勢の推定値とする。

なお、本実施形態では、ステップＳ３０７０の処理において、撮像装置１３０の位置及び姿勢を補正の対象としていたが、計測対象物体１７０の位置及び姿勢を補正の対象として処理を行っても良い。この場合は、算出すべき未知パラメータを、計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [t_WO ^T ω_WO ^T]^T = [x_WO y_WO z_WO ξ_WO ψ_WO ζ_WO]^Tによって内部的に表現し、センサ計測値として得られた計測対象物体１７０の位置及び姿勢をsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。

このとき、撮像装置１３０の位置t_WC及び姿勢R_WCに関しては、センサによる計測値をそのまま固定値として使用する。

また、カメラ座標系における計測対象物体１７０の位置及び姿勢や、物体座標系における撮像装置１３０の位置及び姿勢を未知のパラメータとして求めても良い。

前者の場合、算出すべき未知パラメータを、カメラ座標系における計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [t_CO ^T ω_CO ^T]^T = [x_CO y_CO z_CO ξ_CO ψ_CO ζ_CO]^Tによって内部的に表現する。また、カメラ座標系における計測対象物体１７０の位置及び姿勢（M^# _CO）を、次式、

によってセンサ計測値（M^# _WC及びM^# _WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。

一方、後者の場合は、算出すべき未知パラメータを、物体座標系における撮像装置１３０の位置及び姿勢を表す６値ベクトルs = [t_OC ^T ω_OC ^T]^T = [x_OC y_OC z_OC ξ_OC ψ_OC ζ_OC]^Tによって内部的に表現する。また、物体座標系における撮像装置１３０の位置及び姿勢（M^# _CO）を、次式、

によってセンサ計測値（M^# _WC及びM^# _WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。

以上のようにして、計測対象物体１７０と撮像装置１３０との間の相対的な位置及び姿勢の関係を未知のパラメータとして、センサによる位置及び姿勢計測値の誤差を補正することができる。

従来の位置姿勢計測装置では、指標の分布を考慮していないため、画像上の一部の領域に指標が偏在して観測されている場合に解が不安定になる。一方、本実施形態に係る位置姿勢計測装置によれば、ステップＳ３０３３や、ステップＳ３０４０，Ｓ３０５０，Ｓ３０６０によって指標の分布範囲の大きさを考慮した手法の選択が行われるため、例えば検出された指標の数が多くても、それらが画像上の小領域に偏在しているような場合に、位置及び姿勢の６自由度を安定して求めるのに不十分であるという判定を行うことができる。そのため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、不安定な解が得られる状況に陥る可能性が低下し、従来手法と比して安定した解を得ることができる。

また、本実施形態に係る位置姿勢計測装置によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行う場合に、その制約の中で指標上での誤差の和を最小化するような補正を行うことができる。したがって、未知とするパラメータを減らすことによって撮像装置の位置及び姿勢の計測を安定化させようとする場合であっても、従来と比べて高精度な計測を行うことができる。

［第２の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等に設定された基準座標系における任意の計測対象物体の位置及び姿勢の計測を行う。

本実施形態に係る位置姿勢計測装置は、撮像装置に位置姿勢センサが取り付けられておらず、三脚等によって既知の位置及び姿勢に固定して設置されていることが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。

図１２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１２００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。

撮像装置１３０は、三脚１２８０によって、空間中の既知の位置、姿勢に固定して設置されている。撮像装置１３０が出力する撮影画像は、位置姿勢計測装置１２００の画像入力部１６０に入力される。基準座標系における撮像装置１３０の位置及び姿勢は、既知の値として予め保持されているものとする。

センサ計測値入力部１５０は、計測対象物体１７０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１２２０へと出力する。

位置姿勢算出部１２２０は、センサ計測値入力部１５０の出力である計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、計測対象物体１７０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

なお、図１２に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。

本実施形態における位置姿勢算出部１２２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）とほぼ同一である。唯一の違いは、撮像装置１３０の位置及び姿勢が、計測値として与えられるのではなく、既知の値として予め保持されていることである。位置姿勢算出部１２２０は、撮像装置１３０の位置及び姿勢の既知の値を、撮像装置１３０の位置及び姿勢の仮の計測値とおき、第１の実施形態におけるステップＳ３０００からステップＳ３０７０までと同一の工程によって撮像装置１３０の位置及び姿勢の計測値の補正を行い、この補正結果に基づいて、ステップＳ３０８０において、第１の実施形態と同様に計測対象物体１７０の位置及び姿勢の計測値を補正する（式１）。本実施形態では、位置姿勢算出部１２２０がステップＳ３０８０において出力するのは、計測対象物体１７０の位置及び姿勢のみである。

以上によって、計測対象物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。

なお、既知の位置に固定された撮像装置によって任意の計測対象物体の位置及び姿勢を計測する方法は、計測対象物体に取り付けるセンサを姿勢センサに変更しても実現できる。例えば、検出された指標の分布が閾値未満の場合は、姿勢センサの姿勢計測値を固定値として用いて、位置のみを指標の情報から推定する。また、指標の分布が閾値以上の場合には、位置及び姿勢の６パラメータを指標の情報から推定する。この場合の撮像装置は１台に限定されるものではなく、複数の撮像装置を用いることが可能である。

［第３の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。

本実施形態においては撮像装置以外の計測対象物体は存在せず、本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢の計測のみを行うことが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。

図１３は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１３００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。

また、現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標として、基準座標系における位置が既知である複数個の指標Ｑ_ｋが配置されている。指標Ｑ_ｋは、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、どのような形態であってもよい。

センサ計測値入力部１５０は、撮像装置１３０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１３２０へと出力する。

位置姿勢算出部１３２０は、センサ計測値入力部１５０の出力である撮像装置１３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

なお、図１３に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。

本実施形態における位置姿勢算出部１３２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）と類似しているため、図３を参照しながら、第１の実施形態との相違点のみについて説明する。

ステップＳ３００５において、位置姿勢算出部１３２０は、センサによって計測された撮像装置１３０の位置及び姿勢を、センサ計測値入力部１５０から入力する。

ステップＳ３０２５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の姿勢計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（基準座標系における指標の３次元座標ｘ_Ｗ ^Ｑｋｎと撮像装置１３０の位置及び姿勢の計測値から導出する投影座標の理論値ｕ^Ｑｋｎ＊と、実際の検出座標ｕ^Ｑｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。

このような補正処理としては、周知の方法を用いることができるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。

ステップＳ３０３５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の位置計測値を固定値として、撮像装置１３０の姿勢計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この補正処理工程は、第１の実施形態におけるステップＳ３０３５の補正処理工程（図４のフローチャートに対応）と類似したものとなる。ただし、本実施形態の場合、指標の観測方程式Ｆ’’ｃ（）が、基準座標系における指標Ｑ_ｋｎの座標ｘ_Ｗ ^Ｑｋｎ（既知な情報として予め保持している）から画像座標を算出する関数となる。すなわち、観測方程式Ｆ’’ｃ（）は次式によって表される。

具体的には、観測方程式Ｆ’’ｃ（）は、ｘ_Ｗ ^Ｑｋｎとsから当該指標のカメラ座標ｘ_Ｃ ^Ｑｋｎを求める次式、

及び、カメラ座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める式６によって構成されており、この点のみが異なっている。ここでt_WCは、撮像装置１３０の位置計測値を表す３次元ベクトルであり、ここでは固定値として取り扱われる。

ステップＳ３０７０において、位置姿勢算出部１３２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程での具体的な処理は、例えば特許文献２に開示されているような周知技術を用いて実現することが可能であるので、ここでの説明は省略する。

ステップＳ３０８０において、位置姿勢算出部１３２０は、ステップＳ３０７０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

以上によって、撮像装置の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。

［第４の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における任意の計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。本実施形態においては、撮像装置ではなく、任意の計測対象物体の位置及び姿勢を計測することが、第３の実施形態と異なっている。以下では、第３の実施形態との相違点のみを説明する。

図１４は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１４００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、撮像部１４３０、位置姿勢計測部１４４０、及び位置姿勢算出部１４２０によって構成されており、計測対象物体１４７０に接続されている。

撮像部１４３０は、位置及び姿勢を計測したい対象である計測対象物体１４７０に固定して取り付けられており、指標を含む現実空間の映像を撮影する。

位置姿勢計測部１４４０は、撮像部１４３０に固定して装着されており、基準座標系における撮像部１４３０の位置及び姿勢を計測してセンサ計測値入力部１５０へと出力する。位置姿勢計測部１４４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。

画像入力部１６０は、撮像部１４３０が撮影した撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。

センサ計測値入力部１５０は、撮像部１４３０の位置及び姿勢の計測値を位置姿勢計測部１４４０から入力し、位置姿勢算出部１４２０へと出力する。

位置姿勢算出部１４２０は、センサ計測値入力部１５０の出力である撮像部１４３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力し、入力した情報に基づいて、撮像部１４３０の位置及び姿勢の計測値の誤差を補正する。
位置姿勢算出部１４２０はさらに、得られた撮像部１４３０の位置及び姿勢と、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係についての情報（具体的には、計測対象物体１４７０上のどの位置に撮像部１４３０が設置されているかという情報。計測対象物体１４７０によって定義される物体座標系における、撮像部１４３０の位置及び姿勢によって表現される）から、基準座標系における計測対象物体１４７０の位置及び姿勢を算出し、Ｉ／Ｆ１００９を介してこのデータを外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。なお、この変換工程は必ずしも必要ではなく、撮像部１４３０の位置及び姿勢をそのまま出力することも可能である。

なお、本実施形態では、位置姿勢計測部１４４０は撮像部１４３０の位置及び姿勢を計測していたが、位置姿勢計測部１４４０は計測対象物体１４７０の位置及び姿勢を計測するように構成されていても良い。この場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた計測対象物体１４７０の位置及び姿勢から撮像部１４３０の位置及び姿勢を算出し、これを初期値として上記と同様な処理を実行する。

また、位置姿勢算出部１４２０は、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の何れの工程においても、撮像部１４３０の姿勢あるいは位置及び姿勢を補正し、その後計測対象物体１４７０の位置及び姿勢を求めていた。しかし、ステップＳ３０３５及びＳ３０７０の工程においては、計測対象物体１４７０の位置及び姿勢を直接求めるような構成としても良い。

位置姿勢計測部１４４０が撮像部１４３０の位置及び姿勢を計測している場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた撮像部１４３０の位置及び姿勢から計測対象物体１４７０の位置及び姿勢を算出し、これを初期値とする。

ステップＳ３０３５においては、計測対象物体１４７０の姿勢を表す３値ベクトルs = ω_WO = [ξ_WO ψ_WO ζ_WO]^Tを未知パラメータとする。また、ステップＳ３０７０においては、計測対象物体１４７０の位置及び姿勢を表す６値ベクトルs = [t_WO ^T ω_WO ^T]^T = [x_WO y_WO z_WO ξ_WO ψ_WO ζ_WO]^Tを未知パラメータとする。そして、観測方程式Ｆ’’ｃ（）を定義するのに必要な式２０を、

に変更する。ここで、t_OC 及びR_OCは、計測対象物体１７０によって定義される物体座標系における撮像部１４３０の姿勢を表す３×３の回転行列及び位置を表す３次元ベクトルであり、撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を表す既知の値として、予め保持されているものとする。なお、ステップＳ３０３５では、t_WOは固定値として設定する。

以上によって、任意の物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。

［第５の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。

図１５は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１５００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５５０、及び位置姿勢算出部１５２０によって構成されており、撮像装置１３０及び姿勢センサ１５４０に接続されている。

本実施形態においては、撮像装置１３０に、位置姿勢センサではなく姿勢センサが装着されている点が、第３の実施形態と異なっている。姿勢センサを用いることで、位置姿勢センサを用いる場合に存在する、センサの計測範囲以外の領域を計測できないという制約を回避することができる。以下では、第３の実施形態との相違点のみを説明する。

姿勢センサ１５４０は、撮像装置１３０に装着されており、姿勢センサ１５４０自身の現在の姿勢を計測してセンサ計測値入力部１５５０へと出力する。姿勢センサ１５４０は、例えばジャイロセンサと加速度センサをベースとしたセンサユニットであり、具体的には、株式会社トキメックのTISS-5-40や、米国InterSense社のInertiaCube2等によって構成される。これらのセンサによって計測される姿勢計測値は、慣性計測値を時間積分した値として計測されたものであるため、真の姿勢とは異なった誤差を有する。ただし、これらの姿勢センサは、内蔵する加速度センサによって地球の重力方向を観測することによって、傾斜角方向（ピッチ角及びロール角）のドリフト誤差の蓄積をキャンセルする機能を有している。そのため、傾斜角方向に関してはドリフト誤差が発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては時間の経過に伴って蓄積されるドリフト誤差を有している。

センサ計測値入力部１５５０は、方位ドリフト誤差補正値の更新値を位置姿勢算出部１５２０から入力し、姿勢センサ１５４０の現在の方位ドリフト誤差補正値を更新し保持する。また、姿勢センサ１５４０から姿勢計測値を入力し、現在の方位ドリフト誤差補正値によってこれを補正して、撮像装置１３０の姿勢の予測値として位置姿勢算出部１５２０へと出力する。

位置姿勢算出部１５２０は、センサ計測値入力部１５５０の出力である撮像装置１３０の姿勢の予測値と、指標検出部１１０の出力である各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎを入力データとして、撮像装置１３０の位置及び姿勢を算出して出力する。また、位置及び姿勢の算出工程で導出される姿勢センサ１５４０の方位ドリフト誤差補正値の更新値を、センサ計測値入力部１５５０へと出力する。

なお、図１５に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。

図１６は、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部１５２０のソフトウェアのプログラムを実行することで行われる。

ステップＳ１５０００において、位置姿勢算出部１５２０は、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを指標抽出部１１０から入力する。なお、各指標の基準座標系における３次元座標ｘ^Ｑｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。

ステップＳ１５００５において、位置姿勢算出部１５２０は、撮像装置１３０の姿勢の予測値R^*をセンサ計測値入力部１５５０から入力し、これを、撮像装置１３０の姿勢の初期値として設定する。また、２回目以降の処理では、１ループ前の処理において算出された撮像装置１３０の位置を、撮像装置１３０の姿勢の初期値として設定する。

ステップＳ１５０１０において、位置姿勢算出部１５２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ１５０９０に処理を進め、それ以外の場合はステップＳ１５０２０に処理を進める。

ステップＳ１５０２０において、位置姿勢算出部１５２０は、検出された指標の総数が一つか否かを判定する。指標の総数が一つの場合はステップＳ１５０２５に処理を進め、それ以外の場合はステップＳ１５０３０に処理を進める。

ステップＳ１５０２５において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値の平行移動成分のうちの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加え、ステップＳ１５０９０に処理を進める。ステップＳ１５０２５での処理工程としては、周知の処理を提供することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの平行移動による補正方法（段落「００３０」〜「００３６」）およびランドマークを複数点利用したカメラの平行移動による補正方法（段落「００５１」〜「００５６」）に開示されている方法を用いることができる。ただし、特許文献１では６自由度位置姿勢センサの計測値を誤差補正の対象とした場合を説明しているが、本実施形態では、前フレームで求めた位置と姿勢センサの計測値とを組み合わせたものが補正対象となる点が異なっている。

ステップＳ１５０３０において、位置姿勢算出部１５２０は、検出された指標の総数が二つか否かを判定する。指標の総数が二つの場合はステップＳ１５０３２に処理を進め、それ以外の場合はステップＳ１５０４０に処理を進める。

ステップＳ１５０３２において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ_１（例えば、画像の対角線長の１／８）との比較を行う。距離が閾値Ｔ_１以上の場合にはステップＳ１５０３４に処理を進め、それ以外の場合はステップＳ１５０２５に処理を進める。

ステップＳ１５０３４において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離と所定の値として定められている閾値Ｔ_２（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ_２以上の場合にはステップＳ１５０３８に処理を進め、それ以外の場合はステップＳ１５０３６に処理を進める。

ステップＳ１５０３６において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうち、位置を表す３パラメータ（平行移動成分）を更新対象として（すなわち、姿勢計測値には誤差は含まれていないものと仮定して）、検出されている指標上での誤差を最小とするような補正を位置のパラメータのみに加え、ステップＳ１５０９０に処理を進める。この処理工程の詳細は、例えば特許文献３（第６の実施形態）に開示されているので、ここでの説明は省略する。また、位置を表す３パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置の補正を行うことができる。

ステップＳ１５０３８において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうちの位置を表す３パラメータ（平行移動成分）と、姿勢センサ１３４０の方位ドリフト誤差補正値の更新値φの合計４パラメータとを、算出すべき未知パラメータとして取り扱い、検出されている指標の情報を用いてこれらを算出する。この処理工程の詳細は、例えば特許文献３（第７の実施形態）に開示されているので、ここでの説明は省略する。また、これら４パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置と方位ドリフト誤差補正値の更新値の算出を行うことができる。さらに、ステップＳ１５０８０へと処理を進める。

ステップＳ１５０４０は、検出された指標の総数が三つ以上の場合のみ実行される。ここで、位置姿勢算出部１５２０は、検出された全指標の分布範囲に関する評価値の一例として、検出された全指標の画像座標を包含する凸包を算出する。

ステップＳ１５０５０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ_３（例えば、画像の面積の１／１６）との比較を行う。面積が閾値Ｔ_３以上の場合はステップＳ１５０６０に処理を進め、それ以外の場合はステップＳ１５０３６へと処理を進め、上述した処理を行う。

ステップＳ１５０６０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ_４との比較を行う。面積が閾値Ｔ_４（例えば、画像の面積の１／９）以上の場合はステップＳ１５０７０に処理を進め、それ以外の場合はステップＳ１５０３８へと処理を進め、上述した処理を行う。

ステップＳ１５０７０において、位置姿勢算出部１５２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この算出は、例えば、撮像装置１３０の位置と姿勢を表す６パラメータを変数として、ステップＳ１５００５で設定した初期値を用いて、非線形方程式の反復解法によって誤差を最小化するパラメータを求めることによって実現される。すなわち、位置と姿勢を表す６パラメータを未知変数とした観測方程式を構成し、これに基づいて求められる指標投影位置の誤差と各パラメータに関する画像ヤコビアンとを用いて当該パラメータの補正値を求め、補正処理を反復的に行うことで画像上での誤差を最小とするような位置と姿勢を算出する。
位置姿勢算出部１５２０はさらに、ステップＳ１５００５で設定した姿勢の初期値と求められた姿勢との変化分を算出し、その方位成分を方位ドリフト誤差補正値の更新値φとする。

ステップＳ１５０８０において、位置姿勢算出部１５２０は、ステップＳ１５０３８あるいはステップＳ１５０７０において得られた方位ドリフト誤差補正値の更新値φを、センサ計測値入力部処理１５５０へと出力する。

ステップＳ１５０９０において、位置姿勢算出部１５２０は、ステップＳ１５０８０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

ステップＳ１５１００において、位置姿勢算出部１５２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ１５０００へと進める。

以上によって、撮像装置の位置及び姿勢が計測される。
本実施形態に係る位置姿勢計測装置によれば、ステップＳ１５０３４やステップＳ１５０５０によって指標の分布範囲の大きさを考慮した手法の選択（方位ドリフト誤差補正値の更新を行うか否かの判断）が行われるため、画像上の一部の領域に指標が偏在して観測されている場合に、方位ドリフト誤差補正値の更新を不正確に行ってしまうという状況を回避することができる。

また、本実施形態に係る位置姿勢計測装置によれば、第３の実施形態の効果に加え、ステップＳ１５０６０による手法の選択によって、画像情報が不足する場合であっても姿勢センサによる傾斜角情報を信頼することで位置及び姿勢を安定して導出できるほか、画像情報が十分得られる場合であれば位置及び姿勢の推定を高精度に行えるという、通常の画像情報に基づく位置姿勢推定手法が有する利点を両立することができる。

［第１乃至第５の実施形態の変形例］
（変形例１）
上記の実施形態の各々では、画像上における指標の分布に基づいて、適用する補正アルゴリズムが選択される。これらの補正アルゴリズム（第１の実施形態におけるステップＳ３０２５，Ｓ３０３５，Ｓ３０７０、第２乃至第４の実施形態におけるこれらの変形、及び、第５の実施形態におけるステップＳ１５０２５，Ｓ１５０３６，Ｓ１５０３８，Ｓ１５０７０）は、必ずしも上記の実施形態で示したものである必要はなく、指標の分布に従って見込まれる画像情報の情報量に応じて適切に設定されたものであれば、他の補正アルゴリズムに変更することも可能である。

例えば、第１乃至第４の実施形態におけるステップＳ３０２５やＳ３０３５において、姿勢の計測値を補正するのではなく、位置の計測値を補正する構成としてもよい。この場合、ステップＳ３０２５では、例えば、特許文献１の段落「００３０」〜「００３６」および「００５１」〜「００５６」に開示されているような、１つ又は複数の指標を利用したカメラの平行移動による補正方法に基づいて、位置計測値のうちのカメラの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加えることができる。

また、ステップＳ３０３５では、算出すべき未知パラメータを、撮像装置１３０の位置を表す３値ベクトルs = t_WC = [x_WC y_WC z_WC ]^Tとして、図５と同様な処理手順によって位置の補正を行う。第１の実施形態との相違点は、観測方程式を構成する式５を、次式のように変更することである。

ここで、R_WCは、固定値として用いる撮像装置１３０のセンサによる姿勢計測値を表している。また、R_WO 及びt_WOは、第１の実施形態における式５と同様に、計測対象物体１７０の位置及び姿勢のセンサによる計測値を表しており、固定値として取り扱われる。

なお、式２２は線形方程式であるので、式６と組み合わせた観測方程式から連立方程式を作成しこれを解くことで、繰り返し演算を用いなくても補正値を得ることができる。

なお、不図示のＵＩを介して、姿勢の補正と位置の補正の何れを優先させるかをユーザが自由に選択できるようにしてもよい。この場合、ステップＳ３０２５及びＳ３０３５の実行時に、ユーザの選択に応じて、姿勢あるいは位置の補正アルゴリズムが実行される。これにより、特性の異なる位置姿勢センサを用いた場合でも、より好ましい計測を行うことができる。

また、ステップＳ３０２５においては、計測値として得られる６パラメータのうちの任意の２パラメータを補正対象として選択し、これを未知数として上述した方法と記と同様にして求めてもよい。同様に、ステップＳ３０３５においては、計測値として得られる６パラメータのうちの任意の３パラメータを補正対象として選択し、これを未知数として上述した方法と同様な枠組みによって求めてもよい。

（変形例２）
上記第１乃至第４の実施形態では、画像上における指標の分布の尺度に基づいて、３種類のアルゴリズムへと処理を分岐させていた（ステップＳ３０３３、ステップＳ３０５０及びステップＳ３０６０）。しかし、指標の分布の尺度に基づいて行う分岐数は３通りに限られるものではない。例えば、ステップＳ３０５０の処理を省いて、閾値Ｔ_３と凸包の面積の大小関係によってステップＳ３０３５あるいはステップＳ３０７０へと処理を分岐させるだけでもよい。また、より多数の閾値を設けて、４通り以上の分岐を行ってもよい。例えば、補正対象とするパラメータの数（２〜６）を凸包の面積に応じて変更し、これを未知数として同様な方法により位置姿勢を算出してもよい。

（変形例３）
上記の実施形態の各々では、位置姿勢センサや姿勢センサによる計測値と、撮像装置によって得られる画像情報を併用することにより、計測対象（計測対象物体や撮像装置）の位置及び姿勢を求めていた。しかし、画像上における指標の分布に基づいて、位置及び姿勢を求めるために用いるアルゴリズムを選択するという基本的な技術思想は、これらのセンサによる計測値を用いずに、画像情報のみから計測対象の位置及び姿勢を得る場合であっても適用できる。

例えば、図１３に示した第３の実施形態の構成において、位置姿勢計測装置１３００がセンサ計測値入力部１５０を有さず、また、位置姿勢センサ１４０が撮像装置１３０に設置されていない場合を考える。この場合、位置姿勢算出部１３２０は、初期値として位置姿勢センサ１４０による撮像装置１３０の位置及び姿勢の計測値を用いるのではなく、前フレームで求めた撮像装置１３０の位置及び姿勢の算出値（あるいは、過去の算出値から予測した現フレームにおける位置及び姿勢）を用いることにすると、これを仮の計測値とおいて図３と同様な処理を行うことで、指標の分布に応じて適切な位置及び姿勢の算出アルゴリズムを選択することが可能となる。

（変形例４）
上記の実施形態の各々では、指標の分布を計る評価値として、指標が３点以上検出されている場合には、指標が成す凸包を求めてその面積を利用していた。しかし、指標の分布を評価する尺度はこれに限られるものではなく、他の尺度を用いてもよい。例えば、画像中心から最も遠い位置にある指標を第１の指標として選択し、第１の指標から最も遠い位置にある指標を第２の指標として選択し、さらに、第１，第２の指標を結ぶ直線から最も遠い距離にある指標を第３の指標として選択し、これら第１，第２，第３の指標が成す三角形の面積を分布の尺度として、凸包の面積の変わりに用いても良い。また、簡易的には、このような第１，第２の指標間の距離を分布の尺度としても良いし、指標の画像座標の分散や標準偏差といった、分布を数値で表しうる他の任意の情報を分布の尺度として用いてもよい。

他の指標の画像座標の分散を分布の尺度として用いる具体的な方法として、指標の画像座標の共分散を利用する方法が考えられる。検出されている指標の画像座標の分散共分散行列を求め、その固有値（この値は主成分分析における各主成分の分散に相当する）を用いる方法である。この場合、第２固有値を尺度とすることで、画像上における指標の２次元的広がりを評価することが可能である。また、第１固有値を尺度とすることで、画像の１次元的な広がりを評価することができる。

また、それ以外にも、凸包の周囲長等の、凸包の面積以外の特徴量を用いてもよい。また、検出された全ての指標に対する外接矩形や外接円などから得られる特徴量を用いてもよい。また、指標の画像座標のモーメントの解析値（例えば、慣性等価楕円の長軸長と短軸長）等を用いてもよい。また、単一の多角形指標を用いる場合には、その指標を検出する際に得られる指標領域の面積をそのまま用いてもよい。

また、検出された指標の分布を直接計測しなくても、指標の分布の大小を何らかの情報に基づいて推定してもよい。例えば、過去のフレームやセンサの情報から得たカメラや物体の位置及び姿勢の推定値（初期値）に基づいて、画像上で観測されるであろう指標の座標を算出し、計算上の指標の分布を求めてもよい。また、検出された指標の見かけの大きさ等から指標までの距離を近似的に推定して、それに応じて分布を推定してもよい。

（変形例５）
第１の実施形態におけるステップＳ３０３５では、図４のステップＳ４０４０にて詳細に説明したように、誤差ベクトルＵと行列Θに基づく補正値Δsの算出に、式１１で表現される方法を用いている。また、その結果のΔs を用いてsをs+Δsに置き換えるというニュートン法によってsの算出を行っている。しかしながら、そのsを算出する方法は、このニュートン法に限ったものではない。例えば公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を用いて求めてもよいし、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても発明の本質が損なわれないことは言うまでもない。また、上記の実施形態におけるsを状態ベクトル、式４を観測方程式とすることで、画像ヤコビアンを利用して位置姿勢センサによる計測値のうちの一部のパラメータのみを補正するという効果を有した拡張カルマンフィルタ（Extended Kalman Filter）や、反復拡張カルマンフィルタ（iterative Extended Kalman Filter）を構成することができる。なお、拡張カルマンフィルタ及び反復拡張カルマンフィルタについては非特許文献２に記載されるように周知であるので、その詳細に関するこれ以上の説明は省略する。

（変形例６）
上記の実施形態の各々においては、各々が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を用いていた。しかし、点指標以外の指標を用いることも可能である。例えば、公知の位置姿勢計測装置（例えば、非特許文献３を参照）に用いられているような、線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。例えば、原点から線指標までの距離を評価のための基準として画像からの検出値とsによる推定値から算出する誤差によって誤差ベクトルＵを構成し、観測方程式をsの各要素で偏微分した解を各要素に持つ１行×６列のヤコビ行列によって行列Θを構成することで、上記実施形態と同様にして位置及び姿勢の計測（補正）を行うことができる。また、線指標と点指標、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することも可能である。

（変形例７）
上記の実施形態の各々では、画像面積に対する指標の分布に応じて以降の処理を選択していた。しかし、計測対象物体に対する相対的な比率を尺度としてもよい。例えば、第１の実施形態において、ステップＳ３０４０で、全検出指標の成す凸包の面積Ｓ_Ｈに加えて、画像上への計測対象物体１７０（その大まかな形状を表す３次元データが記憶されているものとする）の投影像の面積Ｓ_Ｏを求め、これらの比率（Ｓ_Ｈ／Ｓ_Ｏ）を得る。そして、上記比率に対して適宜設定した閾値を用いてステップＳ３０５０とステップＳ３０６０での条件分岐を行えばよい。

また、画像中ではなく、物理空間における指標の分布を尺度とすることも可能である。例えば、第１の実施形態において、ステップＳ３０４０で、物体座標系における全検出指標の３次元座標によって構成される３次元凸包を求め、その体積Ｖ_Ｈを算出し、計測対象物体１７０の体積Ｖ_Ｏとの比率（Ｖ_Ｈ／Ｖ_Ｏ）を得ることで、計測対象物体１７０上における検出指標の分布の割合を導出する。そして、上記比率に対して適宜設定した閾値を用いてステップＳ３０５０とステップＳ３０６０での条件分岐を行えばよい。また、物体座標系における互いが最も遠い３点の検出指標によって構成される三角形の面積を求め、計測対象物体１７０のサイズ（例えば、外接直方体の最も長い２辺が成す矩形の面積）との比率を求め、これを尺度として用いることも可能である。

［第６の実施形態］
上記第１乃至第５の実施形態では、位置及び姿勢を算出するいくつかの手法の中から、指標の分布に応じて一つの手法を選択していた。本実施形態では、一つの手法を選択するのとは異なる方法によって、指標の分布を加味した位置及び姿勢の安定計測を実現する。

本実施形態に係る位置姿勢計測装置は、第１の実施形態と同様に、撮像装置と計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

本実施形態に係る位置姿勢計測装置の構成は第１の実施形態とほぼ同じであり、位置姿勢算出部１２０の内部動作のみが異なっている。そのため、以下では、第１の実施形態との相違点である、本実施形態の位置姿勢算出部の動作についてのみ説明する。

図１７は、本実施形態の位置姿勢算出部１７２０の処理手順を示すフローチャートである。本実施形態では、ＣＰＵ１００１が位置姿勢算出部１７２０の機能を実現するプログラムを実行することにより以下の手順を実現する。

ステップＳ１７０００において、位置姿勢算出部１７２０は、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを指標抽出部１１０から入力する。なお、各指標の物体座標系における３次元座標ｘ_Ｏ ^Ｑｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。

ステップＳ１７００５において、位置姿勢算出部１７２０は、撮像装置１３０及び計測対象物体１７０の位置及び姿勢の、センサによる計測値を、センサ計測値入力部１５０から入力する。以下では、撮像装置１３０の位置計測値を３値ベクトルｔ^# _WC 、姿勢計測値を３×３の回転行列Ｒ^# _WCで表現する。また、計測対象物体１７０の位置計測値を３値ベクトルｔ^# _WＯ 、姿勢計測値を３×３の回転行列Ｒ^# _WＯで表現する。

ステップＳ１７０１０において、位置姿勢算出部１７２０は、指標が検出されているか否かを判定する。指標が検出されていない場合（Ｎ＝０）はステップＳ１７１２０に処理を進め、それ以外の場合（Ｎ≧１）はステップＳ１７０２０に処理を進める。

ステップＳ１７０２０において、位置姿勢算出部１７２０は、検出された指標の総数Ｎが１か否かを判定する。検出された指標の総数Ｎが１の場合はステップＳ１７０２５に処理を進め、それ以外の場合はステップＳ１７０３０に処理を進める。

ステップＳ１７０２５において、位置姿勢算出部１７２０は、検出されている指標上での誤差を打ち消すように、撮像装置１３０の姿勢計測値の２パラメータ（パン及びチルト角）に対して補正を加える。そして、ステップＳ１７１２０に処理を進める。ステップＳ１７０２５における補正処理は、第１の実施形態におけるステップＳ３０２５と同様であるので、詳細な説明は省略する。

ステップＳ１７０３０において、位置姿勢算出部１７２０は、検出されている指標上での誤差を打ち消すように、撮像装置１３０の姿勢計測値の３パラメータ（パン、チルト及びロール角）に対して補正を加える補正行列ΔＲ_{WC_1}を算出する。はじめに、第１の実施形態におけるステップＳ３０３５と同様な方法によって姿勢の３パラメータを補正し、補正後の撮像装置の姿勢Ｒ_{WC_1}を得る。そして、得られたＲ_{WC_1}と、センサによる姿勢計測値Ｒ^# _WCとに基づいて、ΔＲ_{WC_1}を次式２３のように算出する。

また、ステップＳ１７０３０では位置の補正は行っていないが、以下の説明における便宜上、位置の補正値を、３値のベクトルΔｔ_{WC_1}と表記する。そして、Δｔ_{WC_1}= [0 0 0]^Tとおく。

ステップＳ１７０４０において、位置姿勢算出部１７２０は、検出された指標の総数Ｎが２か否かを判定する。指標の総数Ｎが２の場合はステップＳ１７０４３に処理を進め、それ以外の場合はステップＳ１７０５０に処理を進める。

ステップＳ１７０４３において、位置姿勢算出部１７２０は、検出されている指標上での誤差を打ち消すように、撮像装置１３０の姿勢計測値の２パラメータ（パン及びチルト角）に対して補正を加える補正行列ΔＲ_{WC_２}を算出する。はじめに、ステップＳ１７０２５と同様な方法によって姿勢を補正し、補正後の撮像装置の姿勢Ｒ_{WC_2}を得る。そして、得られたＲ_{WC_2}と、センサによる姿勢計測値Ｒ^# _WCとに基づいて、式２３と同様にしてΔＲ_{WC_2}を算出する。

ステップＳ１７０４７において、位置姿勢算出部１７２０は、二つの姿勢補正値（補正行列ΔＲ_{WC_1}及びΔＲ_{WC_2}）を合成し、実際に適用する姿勢補正値（補正行列ΔＲ_WC）を算出する。また、位置の補正は行っていないので、位置の補正ベクトルΔｔ_WCを[0 0 0]^Tとおく。そして、ステップＳ１７１１０へに処理を進める。

以下、ステップＳ１７０４７における、姿勢補正値の合成処理の詳細を説明する。まず、検出された二つの指標間の距離distに応じて、次式２４によって両補正値の重みｗ_１を決定する。

ここで、Ｔ_１ ^minとＴ_１ ^maxは指標間の距離を正規化するためのパラメータであり、適切な値が予め設定されているものとする。図１８は、式２４によって得られる重みｗ_１とdistとの関係を表している。

次に、夫々の補正行列ΔＲ_{WC_1}及びΔＲ_{WC_2}の姿勢成分を、四元数表現（４値のベクトルΔＨ_{WC_1}，ΔＨ_{WC_2}と表す）で得て、次式２５によってその加重和ΔＨ_WCを算出する。

そして、得られた加重和ΔＨ_WCを、回転行列表現であるΔＲ_WCに変換する。なお、回転行列と四元数である加重和との相互変換方法は、コンピュータ・グラフィックス等の分野における基本的事項であるので、その詳細な説明は省略する。

式２５によって、重みｗ_１が０の時は２パラメータの補正が選択され、重みｗ_１が１の時は３パラメータの補正が選択される。また、重みｗ_１が０から１の間である時は、２パラメータの補正と３パラメータの補正の中間的な変換行列が、重みｗ_１の大きさに応じて生成される。

次に、ステップＳ１７０４０で、Ｎ＝２でなかった場合、即ち検出された指標が３つ以上の場合について説明する。ステップＳ１７０５０において、位置姿勢算出部１７２０は、検出された全指標の画像座標を包含する凸包を算出する。

ステップＳ１７０６０において、位置姿勢算出部１７２０は、ステップＳ１７０５０で算出した凸包の面積Ａと、所定の値として定められている閾値Ｔ_４（例えば、撮影画像全体の面積の１／１２）との比較を行う。面積Ａが閾値Ｔ_４以上の場合はステップＳ１７０９０に処理を進め、それ以外の場合はステップＳ１７０７０へと処理を進める。なお、後述する二つのパラメータＴ_２ ^max及びＴ_３ ^minに対して、Ｔ_２ ^max≦Ｔ_４≦Ｔ_３ ^minの関係が成り立つ値であれば、閾値Ｔ_４はいずれの値であってもよい。

ステップＳ１７０７０において、位置姿勢算出部１７２０は、ステップＳ１７０４３と同様に、撮像装置１３０の姿勢計測値のパン及びチルト角に補正を加える補正行列ΔＲ_{WC_2}を算出する。

ステップＳ１７０８０において、位置姿勢算出部１７２０は、ステップＳ１７０４７と同様に、二つの補正行列ΔＲ_{WC_1}及びΔＲ_{WC_2}を合成し、補正行列ΔＲ_WCを算出する。そして、ステップＳ１７１１０へと処理を進める。ただし、ステップＳ１７０８０では、指標間の距離distではなく、ステップＳ１７０５０で求めた凸包の面積Ａに応じて、次式２６によって重みｗ_１を決定する。

ここで、Ｔ_２ ^minとＴ_２ ^maxは凸包の面積を正規化するためのパラメータであり、Ｔ_２ ^max≦Ｔ_４を満たす適切な値が予め設定されているものとする。

ステップＳ１７０９０において、位置姿勢算出部１７２０は、検出されている全指標上での誤差の和を最小とするような、撮像装置１３０の位置及び姿勢の補正値を算出する。ここで、位置の補正値は３値のベクトルΔｔ_{WC_３}で、姿勢の補正値は３×３の回転行列ΔＲ_{WC_３}で表現する。はじめに、第１の実施形態におけるステップＳ３０７０と同様な方法によって撮像装置の位置及び姿勢を補正し、補正後の撮像装置の位置ｔ_{WC_３}及び姿勢Ｒ_{WC_３}を得る。そして、得られたＲ_{WC_３}と、センサによる姿勢計測値Ｒ^# _WCとに基づいて、式２３と同様にしてΔＲ_{WC_３}を算出する。また、得られたｔ_{WC_３}とセンサによる位置計測値ｔ^# _WCとに基づいて、Δｔ_{WC_３}を次式２７のように算出する。

ステップＳ１７１００において、位置姿勢算出部１７２０は、二つの補正値（ΔＲ_{WC_1}とΔｔ_{WC_1}及びΔＲ_{WC_３}とΔｔ_{WC_３}）を合成し、センサ計測値に実際に適用する補正値を算出する。そして、ステップＳ１７１１０へに処理を進める。

以下、ステップＳ１７１００の処理の詳細を説明する。まず、ステップＳ１７０５０で求めた凸包の面積Ａに応じて、次式２８によって重みｗ_２を決定する。

ここで、Ｔ_２ ^minとＴ_２ ^maxは凸包の面積を正規化するためのパラメータであり、Ｔ_４≦Ｔ_３ ^minを満たす適切な値が予め設定されているものとする。図1９は、式２６及び式２８の夫々によって得られる、凸包の面積Ａと重みｗ_１及びｗ_２との関係を表している。

次に、位置と姿勢のそれぞれについて、二つの補正値の加重和を算出する。姿勢に関しては、夫々の補正行列ΔＲ_{WC_1}及びΔＲ_{WC_３}の姿勢成分を四元数表現（４値のベクトルΔＨ_{WC_1}，ΔＨ_{WC_３}と表す）で得て、次式２９によってその加重和ΔＨ_WCを算出する。

そして、得られた四元数ΔＨ_WCを、回転行列表現であるΔＲ_WCに変換する。一方、位置の補正値に関しては、次式３０によって二つのベクトルの加重和ΔＴ_WCを直接算出する。

上述のように、Δｔ_{WC_1}= [0 0 0]^Tである。従って、式２９及び式３０によって、重みｗ_２が０の時は姿勢３値のみの補正が選択され、重みｗ_２が１の時は位置３値を加えた６自由度の補正が選択される。また、重みｗ_２が０から１の間である時は、姿勢のみの補正と６自由度の補正との中間的な補正値が、重みｗ_２の大きさに応じて生成される。

ステップＳ１７１１０において、位置姿勢算出部１７２０は、次式３１及び３２によって撮像装置１３０の位置及び姿勢のセンサ計測値に補正を加え、補正後の位置ｔ_ＷＣ及び姿勢Ｒ_ＷＣを得る。

ステップＳ１７１２０において、位置姿勢算出部１７２０は、得られた撮像装置１３０及び計測対象物体１７０の位置及び姿勢を表すデータを、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。

なお、第１の実施形態と同様に、計測対象物体１７０の位置及び姿勢の計測値に補正を加えて出力しても良い。また、カメラ座標系における計測対象物体１７０の位置及び姿勢を算出してこれを出力しても良いし、物体座標系における撮像装置１３０の位置及び姿勢を求めて、これを出力してもよい。

ステップＳ１７１３０において、位置姿勢算出部１７２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、ステップＳ１７０００へと処理を進める。

以上によって、撮像装置の位置及び姿勢の計測が行われる。

本実施形態では、検出された指標の分布状況に応じて、複数の補正方法の使用割合を変化させている。そのため、時系列画像上で検出指標の分布が徐々に変化する場合に複数の手法が推定値の算出もしくは補正値に与える影響度が徐々に変化し、手法の切り替えがシームレスに行われる。そのため、閾値処理で手法を完全に切り替える第１の実施形態の場合と比べ、時間軸上でのスムースさにおいて優位性がある。

また、本実施形態では、画像を重視する方法とセンサによる計測値を重視する方法を合成している。その結果、推定される位置及び姿勢に対する画像情報の影響度（寄与度）が、検出された指標の分布に応じて変化する。すなわち、画像情報が十分に得られている場合には画像情報を優先的に利用し、画像情報が不十分な場合には画像情報の影響度が低くなるという効果を得ることができる。

なお、上記では、重みｗ（ｗ_１及びｗ_２）の値によらず、常に二つの方法による補正値の算出を行っていた。しかし、重みｗを先に算出し、その値が０または１の場合には、加重が０となる補正値の算出処理を実行しないという構成をとることも可能である。これにより、重みが０または１の場合の計算量が削減できる。

また、上記では、二つの方法によって得られた補正値（補正ベクトルや補正行列）の加重和を求め、これをセンサによる計測値に付加している。しかし、二つの方法によって得られた位置及び姿勢自体の加重和を求めることでも、同様な効果を得ることができる。この場合、ステップＳ１７０４７では、ステップＳ１７０３０で求めた姿勢Ｒ_{WC_1}とステップＳ１７０４３で求めた姿勢Ｒ_{WC_２}との加重和を、式２５と同様にして算出する。また、ステップＳ１７０７０では、ステップＳ１７０３０で求めた姿勢Ｒ_{WC_1}とステップＳ１７０７０で求めた姿勢Ｒ_{WC_２}との加重和を、式２５と同様にして算出する。また、ステップＳ１７１００では、ステップＳ１７０３０で求めた姿勢Ｒ_{WC_1}及び位置ｔ^# _WCとステップＳ１７０９０で得た姿勢Ｒ_{WC_３}及び位置ｔ_{WC_３}の加重和を、式２９及び式３０と同様にして算出する。そして、ステップＳ１７１１０を実行せずに、加重和の結果として得られた位置及び姿勢を出力する。

本実施形態で示した技術的思想の本質は、検出された指標の分布に応じて、複数の方法によって得られた解を合成することにある。したがって、第２乃至第５の実施形態においても、指標の分布に応じた手法の選択工程を両者による結果の加重和に変更することで、同様な効果を有する位置姿勢計測装置を構成することができる。

例として、第５の実施形態に本実施形態の考え方を適用した場合を考える。例えば、ステップＳ１５０７０とステップＳ１５０３８の処理の双方を実行し、夫々による位置と姿勢を得る。そして、凸包の面積Ａに対する重みｗ_２を式２８と同様にして算出し、式２９や式３０と同様にして位置及び姿勢の加重和を算出する。このとき、姿勢に関しては、夫々の方法で得た補正値の加重和を算出して、センサ計測値に付加してもよい。他の手法の選択工程を加重和に変更する方法も同様であるので、これ以上の説明は省略する。

なお、本実施形態においても、指標の分布の尺度として、検出された指標の成す凸包の面積を用いることは必須ではなく、上記実施形態の変形例４と同様な様々な尺度を用いても良い。

また、式２４、式２６、式２８及び、図１８、図１９で示したように、本実施形態では、指標の分布の尺度が下限値から上限値まで推移した際に、０から１の間を線形に推移するように重み（ｗ_１またはｗ_２）を設定している。。しかし、重みの算出方法は必ずしもこれに従う必要はない。すなわち、指標の分布の尺度が下限値の場合に重みの値が０、上限値の場合に１、二つの区間の間で単調増加する値となっていれば、同様の効果を得ることができる。例えば式２６を次式３３のように変形してもよい。

もちろん、同様な他の単調増加関数に置き換えてもよい。

また、二つの姿勢の加重平均を求める方法として、本実施形態では、式２５や式２９で表されるような四元数の加重和を用いている。しかし、姿勢の加重平均方法はこれに限られるものではなく、他の算出方法を用いてもよい。例えば、式２５を用いる代わりに以下のような方法を用いてもよい。まず、姿補正行列ΔＲ_{WC_2}とΔＲ_{WC_1}の差分を表す姿勢を回転軸a及び回転角θによる表現で求める。次に、回転角θに重みｗ_１を乗算した角度をθ’とおく。最後に、回転軸a及び回転角θ’で表現される姿勢を求めてこれをΔＲ_{WC_2}に乗算する。この結果は、ｗ_１が０の時にはΔＲ_{WC_2}、ｗ_１が１の時にはΔＲ_{WC_１}となる。そして、ｗ_１が０と１の中間値の場合には、二つの補正行列を重み付けして内挿した補正行列を得ることができる。もちろん、二つの姿勢を重み付けして内挿するような他のいずれの算出方法を用いてもよいことは、いうまでもない。

［第７の実施形態］
上記の実施形態では、検出された指標の分布状況を明示的に数値化し、その値に基づいて、注目フレームにおける検出指標以外の情報の影響度を制御していた。本実施形態は、指標の分布状況に応じて指標以外の情報の影響度を制御する方法である点では共通するが、指標の分布の明示的な数値化は行わない点で異なる。

本実施形態に係る位置姿勢計測装置は、計測対象である撮像装置が撮影した時系列画像を利用して、撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図２０は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置２０００は、画像入力部２０５０、指標検出部２０１０、位置姿勢算出部２０２０、重み設定部２０６０、及び姿勢センサ２０４０によって構成される。そして、計測対象である撮像装置２０３０に接続されている。撮像装置２０３０の位置及び姿勢が、本実施形態に係る位置姿勢計測装置２０００によって計測される。

シーン（撮像装置２０３０が撮影する空間）には、基準座標系（記号Ｗによって表現する）が定義されている。シーン中の複数の位置には、基準座標系における位置（基準座標）が既知である複数個の指標Ｑ_ｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。ここで、ｋは指標の識別子を表している。また、Ｋは配置されている指標の総数を表している。指標Ｑ_ｋの配置は、計測範囲内を撮像装置２０３０が移動した際に、撮影画像中に常に観測されるように調整されていることが好ましい。

図２０の例は、Ｋ＝６、すなわち６個の指標Ｑ_１〜Ｑ_６がシーン中に配置されている状況を示している。指標Ｑ_ｋは、例えば、それぞれが異なる色を有する同一形状（図では円形）のマーカによって構成される。あるいは、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって指標を構成してもよい。また、ある程度の面積を有する四角形の単色領域によって形成されるような、四角形指標を用いることも可能である。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。また指標は故意に設定されたものであっても良いし、故意に設定されたものではない、自然形状のものを用いても良い。

撮像装置２０３０は、例えばビデオカメラである。撮像装置２０３０が撮像する画像は、位置姿勢計測装置２０００に入力される。撮像装置２０３０にはカメラ座標系が定義されている。以下では必要に応じて、カメラ座標系を記号Ｃによって表現する。位置姿勢計測装置２０００は、基準座標系に対するこのカメラ座標系の位置と姿勢を、撮像装置２０３０の位置と姿勢として計測する。カメラ座標系は、撮像装置２０３０の視点位置を原点、視線方向を−Ｚ軸と定義し、更に、Ｚ軸に直交してかつ互いに直交する２軸をＸ軸、Ｙ軸として定義した座標系である。撮像装置２０３０のカメラ内部パラメータは既知であるものとする。

姿勢センサ２０４０は、撮像装置２０３０に装着されており、基準座標系における撮像装置２０３０の姿勢を計測する。姿勢センサ２０４０が出力する計測値は、位置姿勢算出部２０２０に入力される。姿勢センサ２０４０は、例えば、米国InterSense社のInertiaCube3等によって構成される。

画像入力部２０５０は、位置姿勢計測装置２０００へ入力される撮影画像をデジタルデータに変換し、指標検出部２０１０へと出力する。

指標検出部２０１０は、画像入力部２０５０より撮影画像を入力する。指標検出部２０１０は、入力画像を解析し、画像中に撮影されている指標を検出（並びに同定）する。なお、本明細書において、特に説明がない場合、「検出された指標」とは、「同定された指標」を意味する。指標検出部２０１０は、さらに、検出された夫々の指標（以下、必要に応じて検出指標と呼ぶ）の画像座標と対応する基準座標を、位置姿勢算出部２０２０へと出力する。

なお、以下では、検出指標の夫々に通し番号ｎ（ｎ＝１，２，・・・，Ｎ）をつけ、画像上で検出された夫々の指標をＱ_ｋｎと表記する。ここで、Ｎは画像上で検出された指標の総数を表している。また、検出指標Ｑ_ｋｎの画像座標（実測値）をｕ_ｎと表記し、必要に応じて指標Ｑ_ｋの画像座標をｕ_Ｃ ^Ｑｋと表記する（すなわち、ｕ_ｎ＝ｕ_Ｃ ^Ｑｋｎ）。また、既知の値である指標Ｑ_ｋの基準座標をｘ_ｗ ^Ｑｋと表記する。

指標の検出は、使用する指標の種類に応じた方法で行われる。例えば、図２０の例のように、指標の各々が異なる色を有するマーカによって構成されている場合には、撮影画像上から各々のマーカ色に対応する領域を検出する。そして、その重心位置を指標の検出座標とする。また、指標の各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、テンプレート画像によるテンプレートマッチングによって指標の位置を検出する。この場合、各々の指標のテンプレート画像は、既知の情報として予め保持しておく。

また、四角形指標を用いる場合は、画像に２値化処理及びラベリング処理を施し、４つの直線によって形成されているラベル領域を指標候補として検出する。さらに、各指標候補の矩形領域の中に特定のパターンがあるか否かを判定することによって誤検出の排除を行う。また、矩形領域中のパターンによって、指標の識別子を取得する。最後に、矩形領域の４頂点の座標が指標の位置として出力される。なお、このようにして検出される四角形指標は、本明細書では、４つの頂点の個々によって形成される４つの指標であると考える。

重み設定部２０６０は、「姿勢計測値を重視する度合い」を表すパラメータ（以下、必要に応じて「重みパラメータ」と表す）の設定を行うユーザインタフェース（ＵＩ）を提供する。重み設定部２０６０は、キーボード１００４やマウス１００５（後述）等の入力デバイスを介して重みパラメータを入力し、位置姿勢算出部２０２０へと出力する。

ＵＩ上における操作者に対する重みパラメータの表現方法としては、後述する位置姿勢算出部２０２０の内部で使用される重み係数wの値をそのまま用いても良い。また、重み係数wの指数部の数値（10の何乗であるか）を用いてもよい。また、正規化された値（例えば、0から1の間の値）を入力し、内部で重み係数wにマッピングしても良い。これらの数値の入力は、キーボードから値を直接打ち込むことで行っても良いし、スライダー等のＧＵＩで選択しても良い。

また、スライダー等のＧＵＩを用いる場合には、数値以外の概念的な表現でパラメータを入力し、その入力値を内部で重み係数wにマッピングしても良い。概念的な表現としては、例えば、「最小」 「最大」とその間の何段階かの中から選択するようなＵＩが利用できる。また、「指標重視」「センサ重視」とその間の何段階かの中から選択しても良い。

位置姿勢算出部２０２０は、撮像装置２０３０の姿勢の計測値を姿勢センサ２０４０から入力し、検出指標Ｑ_ｋｎの画像座標ｕ^Ｑｋｎと基準座標ｘ_w ^Ｑｋｎの組を指標検出部２０１０から入力する。そして、入力した情報に基づいて、撮像装置２０３０の位置及び姿勢を算出して出力する。位置姿勢算出部２０２０の処理の詳細については、フローチャートを用いて後に詳しく説明する。

なお、画像入力部２０５０、指標検出部２０１０、重み設定部２０６０、及び位置姿勢算出部２０２０の少なくとも一部は、独立した装置として実現しても良い。あるいは、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部２０５０、指標検出部２０１０、重み設定部２０６０、及び位置姿勢算出部２０２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので、説明を省略する。

図２１は、位置姿勢算出部２０２０の処理手順を示すフローチャートである。この処理手順は、本実施形態では、ＣＰＵ１００１が位置姿勢算出部２０２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従った手順を実現するためのプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。

以下では、撮像装置２０３０の位置を、３値のベクトルt = [x y z]^Tによって内部的に表現する。また、撮像装置２０３０の姿勢を、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルω= [ξψ ζ]^Tによって表現する。また、撮像装置２０３０の位置と姿勢を、６値のベクトルｓ= [t^T ω^T]^T = [x y z ξ ψ ζ]^Tによって表現する。

なお、姿勢の情報は必要に応じて、カメラ座標系から基準座標系への回転変換を行う３×３の回転行列Ｒに変換して使用されるものとする。ωとＲは互いに一意に変換することができる。ωからＲへの変換は式３によって可能である。Ｒからωへの変換方法は公知であるので、その詳細な説明は省略する。

ステップＳ２１０００において、位置姿勢算出部２０２０は、時刻τにおける撮像装置２０３０の姿勢計測値ω^* _τ( = [ξ^* _τ ψ^* _τ ζ^* _τ]^T)を、姿勢センサ２０４０から入力する。

ステップＳ２１０１０において、位置姿勢算出部２０２０は、時刻τにおける撮影画像から検出された指標Ｑ_ｋｎ（ｎ＝１，２，・・・，Ｎ）の画像座標ｕ_ｎ（＝ｕ_Ｃ ^Ｑｋｎ）と基準座標ｘ_w ^Ｑｋｎの組を、指標検出部２０１０から入力する。

ステップＳ２１０２０において、位置姿勢算出部２０２０は、入力された検出指標の情報が位置及び姿勢の推定に十分か否かを判定し、それに応じて処理の分岐を行う。具体的には、入力した指標の数が３点以上の場合はステップＳ２１０３０に処理を進め、３点未満の場合は処理を終了する。

ステップＳ２１０３０において、位置姿勢算出部２０２０は、姿勢計測値のうちの傾斜角に関する２パラメータを規定値として扱い、夫々の検出指標の情報を用いて残された４パラメータを推定する。ここで、残された４パラメータとは、撮像装置２０３０の方位角と位置である。この処理は、例えば、非特許文献６に開示されている手法によって実現できるので、詳細な説明は省略する。以下では、ステップＳ２１０３０で得られる位置及び姿勢を、以降の処理における位置及び姿勢の初期値ｓ₀= [t₀ ^T ω₀ ^T]^T = [x₀ y₀ z₀ ξ₀ ψ₀ ζ₀]^Tと表記する。なお、得られた姿勢ω₀と姿勢計測値ω^* _τとの差異は、方位角成分のみとなる。すなわち、ω₀はω^* _τの方位角ドリフト誤差を補正した姿勢と考えることができる。

ステップＳ２１０４０において、位置姿勢算出部２０２０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値（理論値）ｕ_ｎ ^＊（＝ｕ_Ｃ ^Ｑｋｎ＊）を算出する。ｕ_ｎ ^＊は、ｓによって定義される指標の観測方程式、すなわち、指標の基準座標ｘ_w ^Ｑｋｎから画像座標を算出する式１９に基づいて算出される。ただし式１９では、画像座標の推定値ｕ_ｎ ^＊を、ｕ^Ｑｋｎ＊と表記している。

ステップＳ２１０５０において、位置姿勢算出部２０２０は、各々の指標Ｑ_ｋｎに対して、画像座標の推定値ｕ_ｎ ^＊と実測値ｕ_ｎとの差異（再投影誤差）Δｕ_ｎを、次式３４によって算出する。

ステップＳ２１０６０において、位置姿勢算出部２０２０は、各々の指標Ｑ_ｋｎに対して、ｓに関する画像ヤコビアンＪ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。ここで、画像ヤコビアンとは、式１９の観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。具体的には、ヤコビ行列Ｊ_ｕｘ ^Ｑｋｎ（＝∂ｕ／∂ｘ）とヤコビ行列Ｊ_ｘｓ ^Ｑｋｎ（＝∂ｘ／∂ｓ）を算出し、次式３５によってＪ_ｕｓ ^Ｑｋｎを算出する。

ここで、Ｊ_ｕｘ ^Ｑｋｎは、式６の右辺をカメラ座標ｘ_Ｃ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列である。また、Ｊ_ｘｓ ^Ｑｋｎは、式２０の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列である。

ステップＳ２１０７０において、位置姿勢算出部２０２０は、ステップＳ２１０３０で得た姿勢の初期値ω₀に対する現在の姿勢推定値ωの差異Δω( = [Δξ Δψ Δζ]^T)を、次式３６によって算出する。

ステップＳ２１０８０において、位置姿勢算出部２０２０は、上記のステップで算出した再投影誤差Δｕ_ｎ、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎ、及び姿勢の差異Δωに基づいて、ｓの補正値Δｓを算出する。具体的には、ヤコビアンを並べた行列Θと誤差ベクトルＵを作成し、次式３７によってΔｓを算出する。

ここで、行列Θは、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎを垂直に並べた行列に、姿勢のパラメータξ，ψ，ζ自身のｓに関するヤコビアンを追加した行列である。Θは次式３８によって定義される。

また、Θ'は、Θの転置行列に重み係数wを付加した行列であり、次式３９によって定義される。

ここでwは、画像情報と姿勢計測値とのバランスをとるための重み係数であり、「姿勢計測値を重視する度合い」を表している。wの値によって、最終的に得られる位置及び姿勢に対する姿勢計測値の影響力の基準値が決定される。重み係数wの値は、重み設定部２０６０によって与えられる。なお、姿勢計測値の実際の影響力は、指標から得られる情報量の大小に応じて受動的に変更される。

一方、誤差ベクトルＵは、Δｕ_ｎとΔωの各項を垂直に並べた２Ｎ＋３次元のベクトルである。Ｕは次式４０によって定義される。

ここで、式３７乃至式４０によって求められるΔｓの意味について説明する。式３６の目的は、直感的には、誤差ベクトルＵを小さくするようなΔｓを求めることである。例えば第５の実施形態のステップＳ１５０７０における処理のような、指標の情報のみを用いてカメラの位置と姿勢を求める手法では、誤差ベクトルＵは次式４１によって定義される。

これは、指標の再投影誤差を評価基準として、これを最小化することを意図している。

一方、式４０の誤差ベクトルＵは、式４１の誤差ベクトルＵに姿勢の差異Δωを付加したものとなっている。これは、本実施形態に係る位置姿勢算出部２０２０が、指標の再投影誤差と姿勢計測値の差異を評価基準としていることを示している。直感的には、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするようなΔｓが算出される。

ステップＳ２１０９０において、位置姿勢算出部２０２０は、ステップＳ２１０８０において算出した補正値Δｓを用い、式１２に従ってｓを補正し、得られた値をｓの新たな推定値とする。

ステップＳ２１１００において、位置姿勢算出部２０２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ２１０４０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ２１１１０へと処理を進める。

ステップＳ２１１１０において、位置姿勢算出部２０２０は、ステップＳ２１１００までの処理によって得られたｓを、撮像装置２０３０の位置と姿勢の推定値として出力する。なお、位置及び姿勢の出力形態はｓそのものに限定されるものではない。例えば姿勢を、回転行列、回転軸ベクトルと回転角、オイラー角、４元数等に変換して出力してもよい。また、同次座標表現による４×４の位置姿勢変換行列やその逆行列によって、位置及び姿勢を表してもよい。また、撮像装置２０３０に対する基準座標系の位置及び姿勢を求めてこれを出力してもよい。もちろん、これらのいずれかの組み合わせを同時に出力してもよい。

以上のようにして、撮像装置２０３０の位置及び姿勢が算出される。なお、連続的に撮影される時系列画像に対して撮像装置２０３０の位置及び姿勢を求める場合には、入力されたフレーム毎に上述の処理を実行すればよい。

本実施形態に係る位置姿勢計測装置によれば、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような位置と姿勢が算出される。

本実施形態に係る位置姿勢計測装置によれば、画像から得られる情報が不十分な場合には、検出指標の項の影響力が小さくなるので、姿勢計測値を重視した位置及び姿勢を得ることができる。その結果、姿勢計測値を信用して未知パラメータ数を限定する手法と同様な、解を安定化させる効果を得ることができる。一方、画像情報が十分に得られている場合には、検出指標の項の影響力が大きくなる。その結果、姿勢計測値の誤差に影響されることなく、高精度な解を得ることができる。

すなわち、本実施形態に係る位置姿勢計測装置によれば、画像と姿勢センサから得られる情報を無駄なく利用することが可能となり、従来の方法と比べて安定性と精度の双方に優れた計測が実現できる。

［第８の実施形態］
本実施形態に係る位置計測装置は、環境側に配置した複数台の撮像装置及び指標、計測対象物体に設置した複数台の撮像装置、指標、及び姿勢センサを用いて、任意の計測対象物体の位置と姿勢の計測を行う。以下では、本実施形態に係る位置計測装置及び位置計測方法について説明する。

図２２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置２２００は、画像入力部２２５０、指標検出部２２１０、位置姿勢算出部２２２０、モード選択部２２８０、１台以上の主観視点カメラ２２３０，１台以上の客観視点カメラ２２６０、及び姿勢センサ２０４０によって構成されている。位置姿勢計測装置２２００は、計測対象物体２２７０に接続されている。基準座標系における計測対象物体２２７０の位置と姿勢が、本実施形態に係る位置計測装置２２００によって計測される。

シーンには、第１の実施形態と同様に、基準座標系における位置が既知である１つ以上の指標Ｑ_ｋ（ｋ＝１，２，・・・，Ｋ_Ｗ）が配置されている。以下では、環境側に配置されているこれらの指標を、必要に応じて主観視点指標と呼ぶ。主観視点指標の形態は、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かつ、いずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れのものであってもよい。図２２では、Ｋ_Ｗ＝６、すなわち６個の主観視点指標が配置されている。

また、計測対象物体２２７０には、物体座標系上における位置（物体座標）が既知である１つ以上の指標Ｑ_ＫＷ＋ｋ（ｋ＝１，・・・，Ｋ_Ｏ）が配置されている。以下では、物体側に配置されているこれらの指標を、必要に応じて客観視点指標と呼ぶ。客観視点指標の形態は、主観視点指標と同様に、撮影画像上における投影像の画像座標が検出可能であって、かつ、いずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れのものであってもよい。図２２では、Ｋ_Ｏ＝４、すなわち４個の客観視点指標が配置されている。なお，客観視点指標は、計測対象物体２２７０上に直接配置されていなくてもよい。客観視点指標は、例えば、主観視点カメラ２２３０上や姿勢センサ２０４０上に配置されていてもよい。

主観視点カメラ２２３０ａ及び２２３０ｂ（まとめて、主観視点カメラ２２３０ともいう）は、例えばビデオカメラである。主観視点カメラ２２３０は、計測対象物体２２７０上に配置されている。主観視点カメラ２２３０の夫々が撮像する画像（主観視点画像）は、画像入力部２２５０に入力される。主観視点カメラ２２３０のカメラ内部パラメータや、物体座標系における位置及び姿勢は既知であるものとする。主観視点指標及び主観視点カメラ２２３０の配置は、計測範囲内を計測対象物体２２７０が移動した際に、その撮影画像中に常に主観視点指標を観測可能なように調整されていることが好ましい。図２２では、２台の主観視点カメラ２２３０ａ，２２３０ｂが配置されている。以下では必要に応じて、主観視点カメラ２２３０の夫々によって定義される主観視点カメラ座標系を、記号Ｃ_１，Ｃ_２で表現する

客観視点カメラ２２６０ａ〜２２６０ｄ（まとめて、客観視点カメラ２２６０ともいう）は、例えばビデオカメラである。客観視点カメラ２２６０は、計測対象物体２２７０を撮影可能なように、計測範囲を包囲する位置に設置されている。客観視点カメラ２２６０の夫々が撮像する画像（客観視点画像）は、画像入力部２２５０に入力される。客観視点カメラ２２６０のカメラ内部パラメータや、基準座標系における位置及び姿勢は既知であるものとする。客観視点指標及び客観視点カメラ２２６０の配置は、計測範囲内を計測対象物体２２７０が移動した際に、その撮影画像中に常に客観視点指標を観測可能なように調整されていることが好ましい。図２２では、４台の客観視点カメラ２２６０ａ，２２６０ｂ，２２６０ｃ，２２６０ｄが配置されている。以下では必要に応じて、客観視点カメラ２２６０ａ〜２２６０ｄの夫々によって定義される客観視点カメラ座標系を、それぞれ記号Ｂ_１，Ｂ_２，Ｂ_３，Ｂ_４で表現する。

姿勢センサ２０４０は、計測対象物体２２７０上に配置されている。姿勢センサ２０４０は、計測対象物体２２７０の姿勢を計測して位置姿勢算出部２２２０へと出力する。

画像入力部２２５０は、夫々のカメラから入力される撮影画像をデジタルデータに変換し、指標検出部２２１０へと出力する。

指標検出部２２１０は、画像入力部２２５０より夫々の撮影画像を入力する。指標検出部２２１０は、夫々の入力画像を解析し、夫々の画像中に撮影されている指標を検出（並びに同定）する。指標検出部２２１０は、夫々の検出指標について、検出したカメラの識別子と、画像座標の実測値と、対応する３次元座標の組を、位置姿勢算出部２２２０へと出力する。ここで、３次元座標とは、指標が主観視点指標の場合は基準座標を、客観視点指標の場合は物体座標を意味している。なお、指標の検出方法は第１の実施形態と同様にいずれの方法であっても良いので、詳細な説明は省略する。

なお、以下では、検出された指標の夫々に通し番号ｎ（ｎ＝１，，，Ｎ）をつけ、画像上で検出された夫々の指標をＱ_ｋｎと表記する。ここで、Ｎは各画像上で検出された指標の総数である。また、Ｎ＝ΣＮ_ｄであり、Ｎ_ｄは夫々のカメラの撮影画像上で検出された指標の数を表している。また、ｄはカメラの識別子（ｄ＝Ｃ_１，Ｃ_２，Ｂ_１，Ｂ_２，Ｂ_３，Ｂ_４）を表している。また、指標Ｑ_ｋｎの画像座標をｕ_ｎ、その指標を撮影したカメラの識別子をｄ_ｎと表記し、識別子ｄのカメラで検出された指標Ｑ_ｋの画像座標をｕ_ｄ ^Ｑｋと表記する（すなわち、ｕ_ｎ＝ｕ_ｄｎ ^Ｑｋｎ）。また、指標Ｑ_ｋが主観視点指標である場合に、既知の値であるその基準座標をｘ_ｗ ^Ｑｋと表記する。同様に、指標Ｑ_ｋが客観視点指標である場合に、既知の値であるその物体座標をｘ_Ｏ ^Ｑｋと表記する。

モード選択部２２８０は、キーボード１００４やマウス１００５等の入力デバイスを介して、位置姿勢算出部２２２０において使用するアルゴリズムを選択する。例えばラジオボタン等のＧＵＩによって、「指標の情報を重視」モード、「姿勢センサの計測値を重視」モード、それらの「中間」モード、のいずれかを選択可能とする。モード選択部２２８０は、選択されたモードがいずれであるかを、位置姿勢算出部２２２０に出力する。

位置姿勢算出部２２２０は、計測対象物体２２７０の姿勢計測値を姿勢センサ２０４０から入力する。また、夫々の検出指標について、検出したカメラの識別子ｄ_ｎと、画像座標の実測値ｕ_ｎと、対応する３次元座標（ｘ_ｗ ^Ｑｋｎまたはｘ_Ｏ ^Ｑｋｎ）の組を、指標検出部２２１０から入力する。そして、入力した情報に基づいて、計測対象物体２２７０の位置及び姿勢を算出して出力する。位置姿勢算出部２２２０の処理の詳細については、フローチャートを用いて後に詳しく説明する。

なお、画像入力部２２５０、指標検出部２２１０、モード選択部２２８０、及び位置姿勢算出部２２２０の少なくとも一部は、独立した装置として実現しても良い。あるいは、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部２２５０、指標検出部２２１０、モード選択部２２８０、及び位置姿勢算出部２２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。なお、ソフトウェアを実行することで各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので、詳細な説明は省略する。

図２３は、位置姿勢算出部２２２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部２２２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。

以下では、計測対象物体２２７０の位置及び姿勢を、第１の実施形態と同様、３値のベクトルt = [x y z]^T及びω= [ξψ ζ]^Tによって表現する。また、計測対象物体２２７０の位置と姿勢を、６値のベクトルｓ= [t^T ω^T]^T = [x y z ξ ψ ζ]^Tによって表現する。

ステップＳ２３０００において、位置姿勢算出部２２２０は、時刻τにおける計測対象物体２２７０の姿勢計測値ω^* _τ( = [ξ^* _τ ψ^* _τ ζ^* _τ]^T)を、姿勢センサ２０４０から入力する。

ステップＳ２３０１０において、位置姿勢算出部２２２０は、時刻τにおける夫々の撮影画像から検出された検出指標Ｑ_ｋｎの情報を、指標検出部２２１０から入力する。具体的には、検出したカメラの識別子ｄ_ｎと、画像座標の実測値ｕ_ｎと、対応する３次元座標（ｘ_ｗ ^Ｑｋｎまたはｘ_Ｏ ^Ｑｋｎ）の組を入力する。

ステップＳ２３０２０において、位置姿勢算出部２２２０は、入力された検出指標の情報が位置及び姿勢の推定に十分か否かを判定し、それに応じて処理の分岐を行う。例えば、入力した指標の実体の総数が３点以上（Ｎ≧３）の場合はステップＳ２３０３０に処理を進め、３点未満の場合は処理を終了する。ここで、実態の総数とは、同一指標が複数の画像中で検出された場合には、１つの指標が検出されたものとして計数した総数を意味する。

ステップＳ２３０３０において、位置姿勢算出部２２２０は、モード選択部２２８０で選択されているモードが、「指標の情報を重視」モードであるか否かの判定を行う。「指標の情報を重視」モードの場合はステップＳ２３０３５へと処理を進め、その他の場合はステップＳ２３０４０へと処理を進める。

ステップＳ２３０３５において、位置姿勢算出部２２２０は、夫々の検出指標の再投影誤差を評価基準として用いる手法によって、計測対象物体２２７０の位置及び姿勢を推定する。主観及び客観視点カメラから得られる検出指標の再投影誤差を最小化することで物体の位置及び姿勢を求める手法は、非線形最適化計算に基づく手法が非特許文献１等に開示されているので、詳細な説明は省略する。さらに、ステップＳ２３０７０へと処理を進める。

ステップＳ２３０４０において、位置姿勢算出部２２２０は、姿勢計測値のうちの傾斜角に関する２パラメータを規定値として扱い、夫々の検出指標の情報を用いて残された４パラメータを推定する。ここで、残された４パラメータとは、計測対象物体２２７０の方位角と位置である。この処理は、例えば、非特許文献１に開示されている手法によって実現できるので、詳細な説明は省略する。以下では、ステップＳ２３０４０で得られる位置及び姿勢を、以降の処理における位置及び姿勢の初期値ｓ₀= [t₀ ^T ω₀ ^T]^T = [x₀ y₀ z₀ ξ₀ ψ₀ ζ₀]^Tと表記する。

ステップＳ２３０５０において、位置姿勢算出部２２２０は、モード選択部２２８０で選択されているモードが、「姿勢センサの計測値を重視」モードであるか否かの判定を行う。「姿勢センサの計測値を重視」モードの場合は、ステップＳ２３０４０で得られる位置及び姿勢をそのまま出力することとして、ステップＳ２３０７０へと処理を進める。また、その他の場合はステップＳ２３０６０へと処理を進める。

ステップＳ２３０６０において、位置姿勢算出部２２２０は、夫々の検出指標の再投影誤差と、推定された姿勢パラメータと姿勢計測値との間の差異とを評価基準として用いる手法によって、計測対象物体２２７０の位置及び姿勢を推定する。ステップＳ２３０６０の処理の詳細については、フローチャートを用いて後に詳しく説明する。

ステップＳ２３０７０において、位置姿勢算出部２２２０は、以上の処理によって得られた計測対象物体２２７０の位置及び姿勢を出力する。

次に、図２４のフローチャートを用いて、ステップＳ２３０６０における処理の詳細を説明する。

ステップＳ２４０１０において、位置姿勢算出部２２２０は、各々の指標Ｑ_ｋｎに対して、その画像座標の推定値（理論値）ｕ_ｎ ^＊を算出する。ｕ_ｎ ^＊は、指標Ｑ_ｋｎが主観視点指標の場合には、次式４２で表される主観視点カメラ指標Ｃ_ｉによる観測方程式に基づいて算出される。

観測方程式Ｆｃ_ｉ（）は、ｘ_w ^Ｑｋｎとｓから当該指標のカメラ座標ｘ_Ｃｉ ^Ｑｋｎを求める式４３と、ｘ_Ｃｉ ^Ｑｋｎから画像座標ｕ_ｎ ^＊を求める式４４によって構成される。

ここでｆ^Ｃｉ _ｘ及びｆ^Ｃｉ _ｙは、それぞれｘ軸方向及びｙ軸方向における主観視点カメラ指標Ｃ_ｉの焦点距離である。また、R_CiOは物体座標系Oから主観視点カメラ座標系Ｃ_ｉへ姿勢を変換する行列、t_CiOは同座標系間の位置を変換するベクトルである。いずれの値も、主観視点カメラ２２３０ａ，２２３０ｂ毎に予め位置の値として保持されているものとする。

一方、指標Ｑ_ｋｎが客観視点指標の場合には、ｕ_ｎ ^＊は、次式４５で表される客観視点カメラＢ_ｉによる観測方程式に基づいて算出される。

観測方程式Ｆ_Ｂｉ（）は、ｘ_Ｏ ^Ｑｋｎとｓから当該指標の世界座標ｘ_Ｗ ^Ｑｋｎを求める式４６と、ｘ_Ｗ ^Ｑｋｎから夫々の客観視点カメラ座標系における座標ｘ_Ｂｉ ^Ｑｋｎを求める式４７と、ｘ_Ｂｉ ^Ｑｋｎから画像座標ｕ_ｎ ^＊を求める式４８によって構成される。

ステップＳ２４０２０において、位置姿勢算出部２２２０は、各々の指標Ｑ_ｋｎに対して、画像座標の推定値ｕ_ｎ ^＊と実測値ｕ_ｎとの差異（再投影誤差）Δｕ_ｎを、式３４によって算出する。

ステップＳ２４０３０において、位置姿勢算出部２２２０は、各々の指標Ｑ_ｋｎに対して、ｓに関する画像ヤコビアンＪ_ｕｓ ^Ｑｋｎ（＝∂ｕ／∂ｓ）を算出する。ここで、画像ヤコビアンとは、式４２もしくは式４５の観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。観測方程式Ｆｃ_ｉ（）の画像ヤコビアンは式３５と同様にして得ることができる。一方、観測方程式Ｆ_Ｂｉ（）の画像ヤコビアンは、ヤコビ行列Ｊ_ｕｘＢ ^Ｑｋｎ（＝∂ｕ／∂ｘ_Ｂ）とヤコビ行列Ｊ_ｘＢｘＷ ^Ｑｋｎ（＝∂ｘ_Ｂ／∂ｘ_Ｗ）とヤコビ行列Ｊ_ｘＷｓ ^Ｑｋｎ（＝∂ｘ_Ｗ／∂ｓ）を算出し、次式４９によって算出する。

ここで、Ｊ_ｕｘＢ ^Ｑｋｎは、式４８の右辺を客観視点カメラ座標ｘ_Ｂｉ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列である。また、Ｊ_ｘＢｘＷ ^Ｑｋｎは、式４７の右辺を世界座標ｘ_Ｗ ^Ｑｋｎの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列である。また、Ｊ_ｘＷｓ ^Ｑｋｎは、式４６の右辺を状態ベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列である。

ステップＳ２４０４０において、位置姿勢算出部２２２０は、ステップＳ２３０４０で得た姿勢の初期値ω₀に対する現在の姿勢推定値ωの差異Δω( = [Δξ Δψ Δζ]^T)を、式３５によって算出する。

ステップＳ２４０５０において、位置姿勢算出部２２２０は、上記のステップで算出した再投影誤差Δｕ^Ｑｋｎ、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎ、及び姿勢の差異Δωに基づいて、ｓの補正値Δｓを算出する。具体的には、ヤコビアンを並べた行列Θと誤差ベクトルＵを作成し、式３７によってΔｓを算出する。なお、行列Θ、行列Θ'、及び行列Ｕの定義は、それぞれ式３８，式３９，式４０と同様である。ただし、第７の実施形態とは異なり、夫々の行列には、異なるカメラ（客観視点カメラ及び主観視点カメラ）による検出指標Ｑ^ｋｎに関する情報が全て含まれている。また、本実施形態では、重み係数ｗは、予め求めた適切な値が固定値として設定されているものとする。

ステップＳ２４０６０において、位置姿勢算出部２２２０は、ステップＳ２４０５０において算出した補正値Δｓを用いて式１２に従ってｓを補正し、得られた値をｓの新たな推定値とする。

ステップＳ２４０７０において、位置姿勢算出部２２２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ２４０１０以降の処理を行う。一方、収束していると判断した場合には処理を終了する。

以上のようにして、計測対象物体２２７０の位置及び姿勢が算出される。本実施形態に係る位置姿勢計測装置によれば、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、夫々のカメラによる検出指標の再投影誤差を極力小さくするような位置と姿勢が算出される。

本実施形態では、主観視点カメラ２２３０と客観視点カメラ２２６０の双方を用いた。しかし、計測対象物体２２７０の位置及び姿勢の計測に使用するカメラの構成はこれに限定されるものではない。例えば、主観視点カメラ２２３０のみを用いても良いし、客観視点カメラ２２６０のみを用いても良い。

なお、主観視点カメラ２２３０の位置及び姿勢を求めることが目的の場合にも、本実施形態と同様な方法を用いることができる。この場合、主観視点カメラ２２３０（のうちのひとつ）自体が計測対象物体２２７０であると考えればよい。

本実施形態では、予め定めた重み係数を固定値として用いた。しかし、第１の実施形態と同様に重み設定部２０６０を設けて、操作者が重み係数wを設定することも可能である。また、重み設定部２０６０とモード選択部２２８０を同一のＵＩで設定できるような工夫をしても良い。例えば、重み設定部２０６０の入力手段として、正規化されたパラメータ（例えば、0から1の間の値）を数値入力するＵＩを考える。この場合、入力されたパラメータが0場合は「指標の情報を重視」モード、1の場合は「姿勢センサの計測値を重視」モード、0と1の中間値の場合は「中間」モードとしても良い。そして、「中間」モードの場合にのみ、入力されたパラメータを重み係数wに変換して用いても良い。

［第７及び第８の実施形態の変形例］
（変形例７）
上記第７及び第８の実施形態では、姿勢センサ２０４０による姿勢計測値から離れすぎてはいけないという制約条件を考慮しながら、計測対象物体（撮像装置２０３０または計測対象物体２２７０）の位置及び姿勢を推定していた。しかし、上記実施形態で述べた技術的思想の適用範囲は、姿勢計測値から離れすぎてはいけないという制約条件だけを考慮することに限定されるものではない。

例えば、連続的に撮影される時系列画像に対して計測対象物体の位置及び姿勢を求める場合には、前のフレームとの連続性を考慮してもよい。前のフレームで求めた位置及び姿勢から離れすぎてはいけないという制約条件を考慮することで、フレーム間の連続性に優れた位置及び姿勢を得ることができる。

本変形例の処理ステップは、上記第７及び第８の実施形態とほぼ同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、ステップＳ２１１１０において位置及び姿勢の推定結果（ｓ_τ-1）が保持される点と、補正値の算出工程（ステップＳ２１０８０）において前フレームに対する拘束条件が考慮される点のみが異なっている。なお、第８の実施形態も同様に変更することができる。

本変形例では、ステップＳ２１０８０における式３８の行列Θ、式３９のΘ'、式４０の誤差ベクトルＵの夫々を、次式５０〜５２のように変更する。

すなわち、前フレームの結果に対する拘束条件を表す６つの評価値が、新たに追加される。ここで、w_１，w_２，w_３は、それぞれ姿勢計測値の影響度、前フレームの位置の影響度、前フレームの姿勢の影響度を定める重みパラメータである。本変形例では、夫々の重みパラメータには適当な値が予め設定されているものとする。これらの値は、姿勢センサの信頼度やフレーム間の連続性を重視する度合いに応じて個別に設定しても良いし、簡易的には同一の値を設定してもよい。また、パラメータごとに異なる値を設定してもよい。また、上記の実施形態と同様に、重み設定部２０６０を介してユーザが値を調整できるようにしてもよい。

以上によって、姿勢が姿勢計測値から離れすぎてはいけないという制約条件と、位置及び姿勢が前フレームの推定結果から離れすぎてはいけないという制約条件を同時に考慮することができる。そして、これらの拘束条件の範囲内で、指標の再投影誤差を極力小さくするような位置と姿勢を算出することができる。

なお、前フレームにおける位置及び姿勢の推定結果であるｓ_τ-1は、必ずしもステップＳ２１１１０で推定された位置及び姿勢そのものでなくてもよい。例えば、計測対象物体の移動の履歴から、現フレームにおける計測対象物体の位置及び姿勢を予測した値を用いてもよい。この場合、予測は計測対象物体の速度や角速度から単純な線形近似によって行ってもよいし、他のいずれの方法によって行ってもよい。

なお、本変形例では前フレームにおける位置及び姿勢パラメータの全て（６自由度）を制約条件として用いていたが、必ずしも全パラメータを制約条件としなくてもよい。例えば、姿勢の制約は姿勢センサの計測値に対してのみ行ってもよい。この場合、式５０の行列Θ、式５１のΘ'、式５２の誤差ベクトルＵの夫々から、前フレームの姿勢に関する項を除外すればよい。同様に、前フレームの位置に関する項を省略しても良い。また、前フレームにおける位置及び姿勢パラメータのうちの任意の組み合わせに対してのみ拘束を行ってもよい。

（変形例８）
上記第７及び第８の実施形態に追加する形で、さらに、計測対象物体（撮像装置２０３０または計測対象物体２２７０）上に位置センサが設置されている場合を考える。この場合、姿勢センサから得られる姿勢計測値と位置センサから得られる位置計測値の双方を制約条件として用いることができる。

本変形例の処理ステップは、上記第７及び第８の実施形態とほぼ同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、位置センサによる位置計測値を入力する工程をステップＳ２１０１０の直後にステップＳ２１０１５として有する点と、補正値の算出工程（ステップＳ２１０８０）において位置計測値に対する拘束が考慮される点のみが異なっている。なお、第８の実施形態も同様に変更することができる（図２３参照）。

新規に追加されるステップとして、ステップＳ２１０１５において、位置姿勢算出部２０２０は、時刻τにおける撮像装置２０３０の位置計測値ｔ^* _τを、位置センサから入力する。

ステップＳ２１０３０において、位置姿勢算出部２０２０は、位置及び姿勢の初期値ｓ₀を算出する。本変形例では、ステップＳ２１０１０で入力した姿勢計測値ω^* _τに前フレームの処理で推定された姿勢センサの誤差補正値Δω_τ-1を乗算した姿勢を、姿勢の初期値ω₀とおく。また、ステップＳ２１０１５で入力した位置計測値ｔ^* _τと次式５３を用いて、位置の初期値ｔ_０を算出する。

ここで、ｘ_ｏは、既知の値である計測対象物体の座標系における位置センサの計測点の座標を示している。すなわち、式５３は、位置センサの計測値（計測点の位置を表している）から計測対象の位置への変換式を表している。

本変形例では、ステップＳ２１０７０で算出する姿勢の初期値と現在の推定姿勢との差異を求める式（式３６）を、次式５４のように、位置及び姿勢の双方を含むように変形する。

そして、ステップＳ２１０８０で算出する、式３８の行列Θ、式３９のΘ'、式４０の誤差ベクトルＵの夫々を、次式５５〜５７のように変更する。

すなわち、位置の計測値に対する拘束条件が新たに追加される。ここで、w_１およびw_２は、それぞれ位置計測値の影響度と姿勢計測値の影響度を定める重みパラメータである。本変形例では、w_１とw_２には適当な値が予め設定されているものとする。これらの値は、夫々のセンサの信頼度に応じて個別に設定しても良いし、簡易的には同一の値を設定しても良い。また、パラメータごとに異なる値を設定してもよい。また、上記の実施形態と同様に、重み設定部２０６０を介してユーザが値を調整できるようにしても良い。

ステップＳ２１１１０では、推定された位置及び姿勢を出力する。このとき、姿勢計測値ω^＊ _τと最終的に得られた姿勢ωとの差異を求め、姿勢の誤差補正値Δω_τ-1として保持しておく。

以上によって、位置及び姿勢が位置計測値と姿勢計測値の夫々から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような位置と姿勢を算出することができる。

本変形例に係る位置姿勢計測装置によれば、画像から得られる情報が不十分な場合には、指標の項の影響力が小さくなるので、夫々のセンサの計測値を重視した位置及び姿勢を得ることができる。一方、画像情報が十分に得られている場合には、指標の項の影響力が大きくなる。その結果、夫々のセンサ計測値の誤差に影響されることなく、高精度な解を得ることができる。

なお、本変形例においても、フレーム間の連続性を考慮することができる。この場合、変形例７と同様に、前フレームで求めた位置及び姿勢に対する制約を補正値の算出工程に追加すればよい。具体的には、式５５の行列Θ、式５６のΘ'、式５７の誤差ベクトルＵの夫々に、前フレームの位置及び姿勢のパラメータを評価する項を追加すればよい。なお、全１２の制約条件（前フレームに関して６、センサ計測値に関して６）の任意の組み合わせを選択して用いてもよいことは言うまでもない。

（変形例９）
上記第７及び第８の実施形態と変形例７及び８の夫々では、計測対象物体（撮像装置２０３０または計測対象物体２２７０）の位置及び姿勢の６自由度のパラメータを推定していた。しかし、上記実施形態で述べた技術的思想の適用範囲は、推定パラメータが位置及び姿勢の６値の場合のみに限定されるものではない。

例えば、計測対象物体上にさらに高精度な位置センサが設置されている場合を考える。この場合、公知の方法として、位置センサから得られる位置を既知の値として、計測対象物体の姿勢パラメータのみを画像情報から求めるというアプローチを取ることができる。このアプローチは、例えば、特許文献４の第６の実施形態に記載されているように、計測対象物体の姿勢パラメータのみを未知とした非線形最適化処理によって実現できる。そして、このようなアプローチをとる場合においても、上記実施形態で述べた技術的思想をそのまま適用することができる。すなわち、姿勢センサ２０４０による姿勢計測値に対する拘束条件を考慮しながら、計測対象物体の姿勢パラメータを推定することができる。

本変形例の処理ステップは、上記第７及び第８の実施形態と同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、位置センサによる位置計測値を入力する工程（ステップＳ２１０１５）を有する点と、計測対象物体の姿勢ωのみが未知パラメータとなる点のみが異なっている。なお、第８の実施形態も同様に変更することができる。

ステップＳ２１０１５、ステップＳ２１０３０、ステップＳ２１１１０の処理は、変形例７と同様である。

本変形例では、計測対象物体の位置を既知の値として扱い、未知パラメータは姿勢を表すωのみとなる。すなわち、ステップＳ２１０４０では、計測対象物体の位置を固定値とおく。また、ステップＳ２１０６０では、ωの各パラメータについてのみ画像ヤコビアンを算出する。さらに、ステップＳ２１０８０における式３８の行列Θ及び式３９のΘ'を、未知パラメータの変化に応じて次式５８及び式５９のように変更する。

以上によって、６自由度の位置及び姿勢を未知パラメータとしない場合であっても、姿勢センサの計測値から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような姿勢を求めることができる。

なお、本変形例においても、フレーム間の連続性を考慮することができる。この場合、変形例７と同様に、前フレームで求めた姿勢に対する制約を補正値の算出工程に追加すればよい。具体的には、式５８の行列Θ、式５９のΘ'、式４０の誤差ベクトルＵの夫々に、前フレームの姿勢のパラメータを評価する項を追加すればよい。

（変形例１０）
上記第７及び第８の実施形態と変形例７乃至９の夫々においては、重みパラメータｗの値は予め定めた固定値であるか、あるいは、重み設定部２０６０を介してユーザが設定した値であった。しかし、重みパラメータの値はこれらの方法で設定することに限定されるものではない。本変形例では、検出された指標の分布に応じて重みパラメータを能動的に調整することで、指標から得られる情報量に応じて指標の影響度を調整する効果を、より積極的に実現する。

本変形例の処理ステップは、上記第７及び第８の実施形態と同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、検出された指標の分布状況を算出する工程（ステップＳ２１０２５）と、算出された指標の分布状況に応じて重みパラメータｗを決定する工程（ステップＳ２１０２７）を有する点のみが異なっている。なお、第８の実施形態も同様に変更することができる。

新規に追加されるステップとして、ステップＳ２１０２５において、位置姿勢算出部２０２０は、検出された全指標の画像座標を包含する凸包を算出する。この処理は、第６の実施形態におけるステップＳ１７０５０の処理と同様である。

次に、新規に追加されるステップとして、ステップＳ２１０２７において、位置姿勢算出部２０２０は、ステップＳ２１０２５で求めた凸包の面積Ａに応じて、次式６０によって重み調整パラメータｗ_０を決定する。

ここで、Ｔ^minとＴ^maxは凸包の面積を正規化するためのパラメータであり、適切な値が予め設定されているものとする。ｗ_０の範囲は０から１に正規化された値であり、指標の分布が十分であれば０に、不十分であれば１に近い値となる。得られたｗ_０に基づいて、補正値Δｓの算出工程（ステップＳ２１０８０）で用いる重み係数ｗを、次式６１によって算出する。

ここでｗ’は、重み設定部２０６０で設定された重み係数を表している。すなわち、重み設定部２０６０で設定された重み係数を、指標の分布に応じて増減する。なお、ｗ’の設定は重み設定部２０６０で行わなくてもよく、予め適切な値を設定しておいてもよい。

以上によると、指標の分布が十分であれば重み係数ｗの値は小さくなるので、姿勢が姿勢計測値から離れすぎてはいけないという制約条件の影響度が小さくなる。そのため、センサの誤差に制約を受けることなく、検出された画像情報を十分に生かした位置及び姿勢の計測を行うことができる。

一方、指標の分布が不十分な場合は重み係数ｗの値が大きくなるので、姿勢計測値から離れすぎてはいけないという制約条件の影響度が大きくなる。そのため、検出された画像情報の情報量が不足していても、センサから得られる姿勢を逸脱しない推定結果を得ることができる。

以上のように、本変形例によっても、画像から得られる情報の影響度を指標の分布に応じて制御するという効果を得ることができる。

なお、本変形例においても、指標の分布の尺度として指標の成す凸包の面積を用いることは必須ではなく、上記実施形態の変形例４と同様な様々な尺度を用いてもよい。また、重み係数ｗの変位は指標の分布の尺度に対して線形である必要はなく、第６の実施形態と同様に二つの区間を単調増加する任意の値を用いることができる。

なお、変形例７と同様に、前のフレームで求めた位置及び姿勢から離れすぎてはいけないという制約条件を同時に考慮する場合でも、指標の分布に応じて重み係数を調整することができる。この場合、ベースとなる重み係数としてｗ_１’，ｗ_２’，ｗ_３’を保持しておいて、夫々にｗ_０を乗算することで、実際に適用する重み係数（ｗ_１，ｗ_２，ｗ_３）を求めればよい。また、変形例８のように位置センサによる拘束を利用する場合も同様に、指標の分布に応じて重み係数を調整することができる。

なお、検出された画像情報の影響度を指標の分布に応じて調整するという効果は、上記で述べた１２の制約条件（前フレームに関して６、センサ計測値に関して６）の任意の組み合わせを用いる場合であっても得ることができる。例えば、センサを使用せずに、位置及び姿勢が前フレームの推定結果（の少なくともいずれか一つのパラメータ）から離れすぎてはいけないという制約条件のみを用いるという構成をとることもできる。

この場合、指標の分布が十分であれば重み係数ｗの値は小さくなるので、位置や姿勢が前フレームにおける推定値から離れすぎてはいけないという制約条件の影響度が小さくなる。そのため、フレーム間の平滑化を行おうとする拘束の影響を受けることなく、検出された画像情報を十分に生かした位置及び姿勢の計測を行うことができる。

一方、指標の分布が不十分な場合は重み係数ｗの値が大きくなるので、位置や姿勢が前フレームにおける推定値から離れすぎてはいけないという制約条件の影響度が大きくなる。そのため、検出された画像情報の情報量が不足していても、フレーム間での不自然なジッタ−を回避し、安定した位置及び姿勢を得ることができる。

（変形例１１）
上記第７及び第８の実施形態と変形例７乃至１０の夫々は、式３７で表現される方法を用いて補正値Δsを算出している。また、sをs+Δsに置き換えることで推定値sの更新を行っている。しかしながら、行列Θ、行列Θ'、及び誤差ベクトルＵに基づいてsを算出する方法はこの方法に限ったものではない。例えば公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を組み合わせて求めてもよい。また、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよい。他の何れの数値計算手法を併用しても発明の本質が損なわれないことは言うまでもない。

（変形例１２）
上記の夫々の実施形態や変形例では、式３で表されるような３値によって姿勢を表現し、これを最適化計算の工程で用いていた。しかし、最適化計算の工程で用いる姿勢の表現はこれに限定されるものではない。例えば、オイラー角によって姿勢を３値で表現し、このオイラー角を未知パラメータとして最適化計算を行ってもよい。この場合、式３の代わりに、オイラー角から３×３の回転行列Ｒを求める式を得て、これを夫々の観測方程式に組み込めばよい。また、姿勢計測値や前フレームの姿勢推定値による拘束を行う場合には、夫々のパラメータを同様にオイラー角に変更すればよい。もちろん、その他の表現方法を未知パラメータとした場合であっても同様に解くことが可能であり、パラメータの表現方法は本質的問題ではない。なお、オイラー角を用いた非線形最適化計算によるカメラの位置及び姿勢の推定方法は例えば非特許文献７に開示されているので、詳細な説明は省略する。

本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても達成される。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

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

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

第１の実施形態における位置姿勢計測装置の構成を示す図である。 位置姿勢計測装置の各部をソフトウェアにより実現することのできるコンピュータの基本構成を示す図である。 第１の実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。 図３のステップＳ３０３５における位置及び姿勢算出処理の詳細を説明するフローチャートである。 図３のステップＳ３０７０における位置及び姿勢算出処理の詳細を説明するフローチャートである。 撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。 撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。 撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。 図６の撮影画像（撮影画像６００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。 図７の撮影画像（撮影画像７００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。 図８の撮影画像（撮影画像８００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。 第２の実施形態における位置姿勢計測装置の構成を示す図である。 第３の実施形態における位置姿勢計測装置の構成を示す図である。 第４の実施形態における位置姿勢計測装置の構成を示す図である。 第５の実施形態における位置姿勢計測装置の構成を示す図である。 第５の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。 第６の実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。 式２８によって得られる、検出された指標間の距離と重みｗ_１との関係を示す図である。 式２６及び式２８の夫々によって得られる、凸包の面積と重みｗ_１及びｗ_２との関係を示す図である。 第７の実施形態における位置姿勢計測装置の構成を示す図である。 第７の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。 第８の実施形態における位置姿勢計測装置の構成を示す図である。 第８の実施形態における、計測対象物体２２７０の位置及び姿勢を算出する処理を説明するフローチャートである。 第８の実施形態における、指標の再投影誤差と姿勢センサの誤差を評価基準とする位置と姿勢の推定処理を説明するフローチャートである。

Claims (28)

1. 現実空間に存在する指標を撮像装置によって撮像した画像を入力する画像入力手段と、
   前記画像から指標を検出する指標検出手段と、
   前記検出された指標が３つ以上ある場合に、該検出された指標に基づいた凸包の面積を算出する面積算出手段と、
   前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記算出された面積の大きさに応じた個数のパラメータを、算出対象として選択する選択手段と、
   前記算出対象として選択されたパラメータを、前記検出された指標を用いて算出する算出手段と
   を有することを特徴とする情報処理装置。
2. 前記選択手段が、前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記算出された面積が大きくなるに応じて多くなる個数のパラメータを、前記算出対象として選択することを特徴とする請求項１に記載の情報処理装置。
3. 前記検出された指標が２つである場合に、該検出された２つの指標の間の距離を算出する距離算出手段をさらに有し、
   前記選択手段が、前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記前記算出された距離の大きさに応じた個数のパラメータを、前記算出対象として選択することを特徴とする請求項１又は２に記載の情報処理装置。
4. 前記選択手段が、前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記前記算出された距離が大きくなるに応じて多くなる個数のパラメータを、前記算出対象として選択することを特徴とする請求項３に記載の情報処理装置。
5. 前記画像入力手段が、現実空間に存在する物体センサ及び前記指標が配された物体を、センサが配された撮像装置によって撮像した画像を入力し、
   前記算出手段が、前記物体に配されたセンサの計測値と前記撮像装置に配されたセンサの計測値と前記画像から検出された指標とを用いて、前記物体に配されたセンサあるいは前記撮像装置に配されたセンサの計測値のうち、前記算出手段として選択されたパラメータに対応する計測値を補正することを特徴とする請求項１乃至４の何れか１項に記載の情報処理装置。
6. 前記画像入力手段が、現実空間に固定して配され位置が既知である指標を、位置及び姿勢のうち少なくとも姿勢を計測するセンサが配された撮像装置によって撮像した画像を入力し、
   前記算出手段が、前記撮像装置に配されたセンサの計測値と前記画像から検出された指標を用いて、前記撮像装置に配されたセンサの計測値のうち、前記算出手段として選択されたパラメータに対応する計測値を補正することを特徴とする請求項１乃至４の何れか１項に記載の情報処理装置。
7. 前記選択手段が、前記算出された面積が予め設定された第１の面積の閾値以上の場合に、前記撮像装置の位置姿勢を構成する６自由度のパラメータの全てを前記算出対象として選択することを特徴とする請求項１乃至６の何れか１項に記載の情報処理装置。
8. 前記選択手段が、前記算出された面積が予め設定された第１の面積の閾値よりも小さく、予め設定された第２の面積の閾値以上の場合に、前記撮像装置の位置姿勢の姿勢成分を構成する３自由度のパラメータの全てを算出対象として選択することを特徴とする請求項１乃至７の何れか１項に記載の情報処理装置。
9. 前記選択手段が、前記算出された面積が予め設定された第２の面積の閾値よりも小さい場合に、前記撮像装置の位置姿勢の姿勢成分を構成するパン角とチルト角とを前記算出対象として選択することを特徴とする請求項１乃至６の何れか１項に記載の情報処理装置。
10. 前記選択手段が、前記算出された面積が予め設定された第２の面積の閾値よりも大きい場合に、前記撮像装置の位置姿勢の姿勢成分を構成する３自由度のパラメータの全てを前記算出対象として選択することを特徴とする請求項１乃至６、及び９の何れか１項に記載の情報処理装置。
11. 前記選択手段が、前記算出された面積が予め設定された第１の面積の閾値よりも小さく、予め設定された第２の面積の閾値以上の場合に、前記撮像装置の位置姿勢の位置成分を構成する３自由度のパラメータの全てを前記算出対象として選択することを特徴とする請求項１乃至７の何れか１項に記載の情報処理装置。
12. 前記選択手段が、前記算出された面積が予め設定された第２の面積の閾値以上の場合に、前記撮像装置の位置姿勢の位置成分を構成する３自由度のパラメータの全てを前記算出対象として選択することを特徴とする請求項１乃至７、及び１１の何れか１項に記載の情報処理装置。
13. 前記選択手段が、前記算出された距離が予め設定された距離の閾値以上の場合に、前記撮像装置の位置姿勢の姿勢成分を構成する３自由度のパラメータの全てを前記算出対象として選択することを特徴とする請求項３又は４に記載の情報処理装置。
14. 前記選択手段が、前記算出された距離が予め設定された距離の閾値よりも小さい場合に、前記撮像装置の位置姿勢の姿勢成分を構成するパン角とチルト角とを前記算出対象として選択することを特徴とする請求項３、４及び１３の何れか１項に記載の情報処理装置。
15. 前記選択手段が、前記算出された距離が予め設定された距離の閾値以上の場合に、前記撮像装置の位置姿勢の位置成分を構成する３自由度のパラメータの全てを前記算出対象として選択することを特徴とする請求項３又は４に記載の情報処理装置。
16. 前記選択手段が、前記検出された指標が１つである場合に、前記撮像装置の位置姿勢の姿勢成分を構成するパン角とチルト角とを算出対象として選択することを特徴とする請求項２、３、１３、及び１４の何れか１項に記載の情報処理装置。
17. 前記算出手段が、
    前記撮像装置の位置姿勢を構成するパラメータを用いて、前記指標の前記画像の面における位置を算出する画像位置算出手段と、
    前記算出された指標の位置と前記検出された指標の位置との誤差に基づいて補正値を算出し、前記算出された面積に基づいて該補正値に重みづけを与え、前記算出対象として選択され、算出されたパラメータを補正する補正手段と
    を有することを特徴とする請求項１乃至１６の何れか１項に記載の情報処理装置。
18. 前記算出手段が、
    前記撮像装置の位置姿勢を構成するパラメータを用いて、前記指標の前記画像の面における位置を算出する画像位置算出手段と、
    前記算出された指標の位置と前記検出された指標の位置との誤差に基づいて補正値を算出し、前記算出された距離に基づいて該補正値に重みづけを与え、前記算出手段として選択され、算出されたパラメータを補正する補正手段と
    を有することを特徴とする請求項２、３、１３乃至１５の何れか１項に記載の情報処理装置。
19. 現実空間に存在する指標を撮像装置によって撮像した画像を入力する画像入力手段と、
    前記画像から指標を検出する指標検出手段と、
    前記検出された指標が２つである場合に、該検出された２つの指標の間の距離を算出する距離算出手段と、
    前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記算出された距離の大きさに応じた個数のパラメータを、算出対象として選択する選択手段と、
    前記算出手段として選択されたパラメータを前記検出された指標を用いて算出する算出手段と
    を有することを特徴とする情報処理装置。
20. 現実空間に存在する物体に配された指標を撮像装置によって撮像した画像を入力する画像入力手段と、
    前記画像から指標を検出する指標検出手段と、
    前記検出された指標が３つ以上ある場合に、該検出された指標に基づいた凸包の面積を算出する面積算出手段と、
    前記物体の位置姿勢を構成する６自由度のパラメータのうち、前記算出された面積が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択手段と、
    前記算出手段として選択されたパラメータを前記検出された指標を用いて算出する算出手段と
    を有することを特徴とする情報処理装置。
21. 現実空間に存在する物体に配された指標を撮像装置によって撮像した画像を入力する画像入力手段と、
    前記画像から指標を検出する指標検出手段と、
    前記検出された指標が２つである場合に、該検出された２つの指標の間の距離を算出する距離算出手段と、
    前記物体の位置姿勢を構成する６自由度のパラメータのうち、前記算出された距離が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択手段と、
    前記算出手段として選択されたパラメータを前記検出された指標を用いて算出する算出手段と
    を有することを特徴とする情報処理装置。
22. 前記撮像装置は前記現実空間に固定され、前記現実空間における位置姿勢が既知であることを特徴とする請求項２０又は２１に記載の情報処理装置。
23. コンピュータを、請求項１乃至２２の何れか１項に記載の情報処理装置の各手段として機能させるためのプログラム。
24. コンピュータを、請求項１乃至２２の何れか１項に記載の情報処理装置の各手段として機能させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体。
25. 画像入力手段が、現実空間に存在する指標を撮像装置によって撮像した画像を入力する画像入力工程と、
    指標検出手段が、前記画像から指標を検出する指標検出工程と、
    面積算出手段が、前記検出された指標が３つ以上ある場合に、該検出された指標に基づいた凸包の面積を算出する面積算出工程と、
    選択手段が、前記算出された面積に基づいて、前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記算出された面積が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択工程と、
    算出手段が、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出工程と
    を有することを特徴とする情報処理方法。
26. 画像入力手段が、現実空間に存在する物体に配された指標を撮像装置によって撮像した画像を入力する画像入力工程と、
    指標検出手段が、前記画像から指標を検出する指標検出工程と、
    面積算出手段が、前記検出された指標が３つ以上ある場合に、該検出された指標に基づいた凸包の面積を算出する面積算出工程と、
    選択手段が、前記算出された面積に基づいて、前記物体の位置姿勢を構成する６自由度のパラメータのうち、前記算出された面積が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択工程と、
    算出手段が、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出工程と
    を有することを特徴とする情報処理方法。
27. 画像入力手段が、現実空間に存在する指標を撮像装置によって撮像した画像を入力する画像入力工程と、
    指標検出手段が、前記画像から指標を検出する指標検出工程と、
    距離算出手段が、前記検出された指標が２つである場合に、該検出された２つの指標の間の距離を算出する距離算出工程と、
    選択手段が、前記算出された距離に基づいて、前記撮像装置の位置姿勢を構成する６自由度のパラメータのうち、前記算出された距離が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択工程と、
    算出手段が、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出工程と
    を有することを特徴とする情報処理方法。
28. 画像入力手段が、現実空間に存在する物体に配された指標を撮像装置によって撮像した画像を入力する画像入力工程と、
    指標検出手段が、前記画像から指標を検出する指標検出工程と、
    距離算出手段が、前記検出された指標が２つである場合に、該検出された２つの指標の間の距離を算出する距離算出工程と、
    選択手段が、前記算出された距離に基づいて、前記物体の位置姿勢を構成する６自由度のパラメータのうち、前記算出された距離が大きくなるに応じて多くなる個数のパラメータを、算出対象として選択する選択工程と、
    算出手段が、前記算出対象として選択されたパラメータを前記検出された指標を用いて算出する算出工程と
    を有することを特徴とする情報処理方法。

Images