位置姿勢計測方法及び装置
【課題】 安定性と精度とが両立した位置及び姿勢の計測を行うことの可能な位置姿勢測定装置を提供すること。
【解決手段】 空間中に配置された指標を含む画像を撮像し、撮像画像から指標を検出する。複数の指標が検出された場合には、その分布範囲を求め、その大きさに応じて位置姿勢算出に適用するアルゴリズムを選択する（Ｓ３０３３、Ｓ３０５０、Ｓ３０６０）。例えば、指標が十分広い範囲に分布している場合は、撮像装置の位置及び姿勢の６パラメータを未知数として算出する（Ｓ３０７０）。指標の分布範囲が小さくなるほど、求めるべき未知パラメータの自由度を小さくする（Ｓ３０３５，Ｓ３０２５）。
【解決手段】 空間中に配置された指標を含む画像を撮像し、撮像画像から指標を検出する。複数の指標が検出された場合には、その分布範囲を求め、その大きさに応じて位置姿勢算出に適用するアルゴリズムを選択する（Ｓ３０３３、Ｓ３０５０、Ｓ３０６０）。例えば、指標が十分広い範囲に分布している場合は、撮像装置の位置及び姿勢の６パラメータを未知数として算出する（Ｓ３０７０）。指標の分布範囲が小さくなるほど、求めるべき未知パラメータの自由度を小さくする（Ｓ３０３５，Ｓ３０２５）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の位置及び姿勢、特に撮像装置の位置及び姿勢を計測する方法及び装置に関するものである。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない結合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）を重畳描画した画像を表示する装置として実現することができる。
【０００３】
このような画像表示装置を実現するには、現実空間中に定義した基準座標系（重畳しようとする仮想物体の位置及び姿勢を決定する基準となる現実空間中の座標系）と、撮像装置の座標系（カメラ座標系）との間の、相対的な位置及び姿勢を計測することが不可欠である。これは、仮想物体（仮想空間画像）を現実空間上の位置に合わせて描画するには、基準座標系に対する撮像装置の現実のカメラパラメータと同一のカメラパラメータを用いて仮想物体の画像を生成しなければならないからである。例えば、現実の室内のある位置に仮想物体を重畳表示する場合には、部屋に基準座標系を定義し、その基準座標系における撮像装置の位置及び姿勢を求めればよい。また、観察者が手に保持する現実の箱に何らかの仮想の模様やラベルを重畳表示する場合には、箱自身の物体座標系を基準座標系と考え、撮像装置に対する箱（基準座標系）の位置及び姿勢を求めればよい。
【０００４】
撮像装置の位置及び姿勢を計測する方法として、現実空間中に複数の指標（人為的なマーカや自然特徴など）を配置あるいは設定し、撮像装置によって撮像された画像内における指標の投影像の座標を検出して、指標の座標情報との関係に基づいて撮像装置の位置及び姿勢を求めることが一般的に行われている（例えば、非特許文献１）。しかし、このアプローチを用いる場合、常に指標が撮像されなくてはならないという制約がある。
【０００５】
一方、撮像装置に磁気センサや超音波センサなどを用いた６自由度位置姿勢センサを装着し、これによって計測した撮像装置の位置及び姿勢の誤差を、指標を撮像した画像を用いて得られる情報（画像情報）によって補正する試みがなされている（例えば、特許文献１，特許文献２）。特許文献２で開示されている方法では、撮像画像中に指標が検出された場合にはその情報に基づいてセンサ計測値の誤差が補正され、指標が検出されない場合には６自由位置姿勢センサの計測値がそのまま撮像装置の位置及び姿勢として使用される。指標の検出の有無に関わらずに撮像装置の位置及び姿勢が得られるため、複合現実感の提示を安定して行うことができる。
【０００６】
また、特許文献２の方法では、検出された指標の数が３点以上の場合には、撮像装置の位置及び姿勢の６自由度が画像情報に基づいて算出され、検出された指標の数が２点及び１点の場合には、センサによって計測された撮像装置の位置あるいは姿勢のいずれか（２または３自由度）のみを補正する処理が適用される。すなわち、検出された指標の数を判定基準として、撮像装置の位置及び姿勢の算出に利用するアルゴリズムを切り替えることが行われている。これにより、画像情報のみからでは撮像装置の位置及び姿勢を求められない場合（撮像された指標の数が３点に満たない場合）であっても、センサ計測値を基準としながら、その誤差を極力打ち消すような補正のなされた位置及び姿勢を取得することができる。
【０００７】
一方、特許文献１の方法では、検出された指標の数に関わらず、センサによって計測された撮像装置の位置あるいは姿勢のいずれかのみを画像情報に基づいて補正する処理が適用される。この補正方法では、姿勢を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような回転補正値を個別に求め、それらを平均化することで姿勢計測値に対する補正値が算出される。また、位置を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような平行移動補正値を個別に求め、それらを平均化することで位置計測値に対する補正値が算出される。指標の数に関わらず補正の自由度を２または３に限定するため、情報量が不十分な場合であっても安定した解を得ることができる。
【０００８】
【非特許文献１】加藤ら：”マーカー追跡に基づく拡張現実感システムとそのキャリブレーション”,日本バーチャルリアリティ学会論文誌, 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等によって構成される。なお、位置姿勢センサ１４０によって計測される位置及び姿勢は、磁場の歪み等の影響によって誤差を含んだものとなっている。ただし、計測範囲内にあっては連続的に安定して位置及び姿勢を計測することが可能である。
【００２３】
画像入力部１６０は、位置姿勢装置１００へ入力される撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。
【００２４】
センサ計測値入力部１５０は、位置姿勢センサ１４０（１４０ａ，１４０ｂ）から夫々の計測値を入力し、位置姿勢算出部１２０へと出力する。
【００２５】
指標検出部１１０は、画像入力部１６０より撮影画像を入力し、入力した画像中に撮影されている指標Ｑｋの画像座標を検出する。例えば、指標Ｑｋの各々が異なる色を有するマーカによって構成されている場合には、撮影画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標Ｑｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを撮影画像上に施すことにより、指標の位置を検出する。また、四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、４つの直線によって形成されている領域を指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の識別子を取得する。なお、このようにして検出される四角形指標は、本明細書では、４つの頂点の個々によって形成される４つの指標であると考える。
【００２６】
指標検出部１１０は、さらに、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。
【００２７】
位置姿勢算出部１２０は、センサ計測値入力部１５０の出力である撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、計測対象物体１７０あるいは撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【００２８】
なお、図１に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。
【００２９】
図２は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々の機能を、ソフトウェアを実行することで実現するためのコンピュータの基本構成を示す図である。
【００３０】
ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々のソフトウェアの実行を制御して、各部の機能を実現する。
【００３１】
ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備えると共に、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。
【００３２】
ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データなどが格納されている。キーボード１００４、マウス１００５は入力デバイスであり、操作者はこれらを用いて、各種の指示をＣＰＵ１００１に入力することができる。
【００３３】
表示部１００６は、ＣＲＴや液晶ディスプレイなどにより構成されており、例えば、位置姿勢計測のために表示すべきメッセージ等を表示することができる。
【００３４】
外部記憶装置１００７は、ハードディスクドライブなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やＣＰＵ１００１が実行するプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。
【００３５】
記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出して、ＲＡＭ１００２や外部記憶装置１００７に出力する。
【００３６】
Ｉ／Ｆ１００９は、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、位置姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポート、また、算出した位置及び姿勢を外部へ出力するためのイーサネット(登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０及びセンサ計測値入力部１５０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
上述した各構成要素は、バス１０１０によって相互に接続される。
【００３７】
図３は、位置姿勢算出部１２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部１２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。
【００３８】
ステップＳ３０００において、位置姿勢算出部１２０は、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを指標抽出部１１０から入力する。なお、各指標の物体座標系における３次元座標ｘＯＱｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。
【００３９】
ステップＳ３００５において、位置姿勢算出部１２０は、撮像装置１３０及び計測対象物体１７０の位置及び姿勢のセンサによる計測値をセンサ計測値入力部１５０から入力する。
【００４０】
ステップＳ３０１０において、位置姿勢算出部１２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ３０８０に処理を進め、それ以外の場合はステップＳ３０２０に処理を進める。
【００４１】
ステップＳ３０２０において、位置姿勢算出部１２０は、検出された指標の総数が一つか否かを判定する。指標の総数が一つの場合はステップＳ３０２５に処理を進め、それ以外の場合はステップＳ３０３０に処理を進める。
【００４２】
ステップＳ３０２５において、位置姿勢算出部１２０は、ステップＳ３００５で得た撮像装置１３０の姿勢の計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（物体座標系における指標の３次元座標ｘＯＱｋｎと撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値から導出する投影座標の理論値ｕＱｋｎ＊と、実際の検出座標ｕＱｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。ステップＳ３０２５における補正処理としては、周知の処理を適用することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。
【００４３】
ステップＳ３０３０において、位置姿勢算出部１２０は、検出された指標の総数が二つか否かを判定する。指標の総数が二つの場合はステップＳ３０３３に処理を進め、それ以外の場合はステップＳ３０４０に処理を進める。
【００４４】
ステップＳ３０３３において、位置姿勢算出部１２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ１（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ１以上の場合にはステップＳ３０３５に処理を進め、それ以外の場合はステップＳ３０２５に処理を進め、上述した処理を行う。
【００４５】
ステップＳ３０３５において、位置姿勢算出部１２０は、ステップＳ３００５で得た計測対象物体１７０の位置及び姿勢の計測値と、撮像装置１３０の位置の計測値を固定値として、撮像装置１３０の姿勢の計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この処理工程の詳細は後述する。
【００４６】
ステップＳ３０４０は、検出された指標の総数が三つ以上の場合のみ実行される。ここで、位置姿勢算出部１２０は、検出された全指標の分布範囲に関する評価値の一例として、検出された全指標の画像座標を包含する凸包を算出する。画像上の点群に対する凸包の算出方法は画像処理の基本的事項として広く公知のものであるので、その詳細な説明は省略する。
【００４７】
図６，図７，及び図８は、撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示している。図６に示す撮影画像６００上では指標Ｑ１，Ｑ５，Ｑ６，Ｑ９が、図７の撮影画像７００上では指標Ｑ１，Ｑ２，Ｑ６，Ｑ７が，図８の撮影画像８００上では指標Ｑ２，Ｑ６，Ｑ７が観測されており、それ以外の指標は、他の物体（本実施形態においてはユーザの手６１０）によって隠蔽されているものとする。
【００４８】
これらの撮影画像に対して指標の検出を行い、図３のフローチャートで示した位置姿勢算出部１２０の処理ステップを実行すると、各画像において検出される指標の数は３以上であるので、ステップＳ３０４０以降の処理が実行されることになる。図９，図１０及び図１１は、それぞれ撮影画像６００，７００，８００に対してステップＳ３０４０の処理を実行した際に得られる、検出指標の成す凸包（９００，１０００，１１００）を示している。なお、以下では、各凸包の面積をそれぞれＡ９００，Ａ１０００，及びＡ１１００と表記したときに、以下に述べる閾値Ｔ２及び閾値Ｔ３に対して、Ａ１１００＜Ｔ２＜Ａ１０００＜Ｔ３＜Ａ９００という関係が満たされているものとする。
【００４９】
ステップＳ３０５０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ２（例えば、撮影画像全体の面積の１／１６）との比較を行う。面積が閾値Ｔ２以上の場合はステップＳ３０６０に処理を進め、それ以外の場合はステップＳ３０２５へと処理を進める。図６〜図１１の例では、撮影画像８００についてはステップＳ３０２５へ、撮影画像７００及び６００についてはステップＳ３０６０へと処理が進行する。
【００５０】
ステップＳ３０６０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ３（例えば、撮影画像全体の面積の１／９）との比較を行う。面積が閾値Ｔ３以上の場合はステップＳ３０７０に処理を進め、それ以外の場合はステップＳ３０３５へと処理を進める。図６〜図１１の例では、撮影画像７００についてはステップＳ３０３５へ、撮影画像６００についてはステップＳ３０７０へと処理が進行する。
【００５１】
ステップＳ３０７０において、位置姿勢算出部１２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程の詳細は後述する。
【００５２】
ステップＳ３０８０において、位置姿勢算出部１２０は、ステップＳ３０２５、Ｓ３０３５及びＳ３０７０で得られた撮像装置１３０及び計測対象物体１７０の位置及び姿勢を表すデータを、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【００５３】
なお、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の各工程では、撮像装置１３０に関する計測値のみに補正を加えているので、ステップＳ３０８０で出力される位置及び姿勢のデータは、計測対象物体１７０に関しては、ステップＳ３００５で入力したセンサによる計測値そのものとなる。しかし、位置及び姿勢の出力形態はこれに限られるものではなく、逆に、撮像装置１３０の位置及び姿勢の計測値には補正を加えずにセンサによる計測値をそのまま出力し、計測対象物体１７０の位置及び姿勢の計測値に補正を加えて出力しても良い。この場合は、ステップＳ３０８０において、以下に述べる座標変換処理を行った後に、変換後の位置及び姿勢を出力する。
【００５４】
同次座標系を用いた４×４の座標変換行列によって位置及び姿勢を表記すると、計測対象物体１７０の補正後の位置及び姿勢MWOは、センサ計測値として得られた撮像装置１３０の位置及び姿勢M#WC（以下、# はセンサによる計測値を表す記号）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢M#WOと、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢MWCに基づいて、次式によって算出される。
【００５５】
【数１】
【００５６】
このとき、撮像装置１３０によって定義されるカメラ座標系における計測対象物体１７０の位置及び姿勢は、
【００５７】
【数２】
【００５８】
となる。すなわち、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢（MWC）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢（M#WO）とを出力する場合と、センサ計測値として得られた撮像装置１３０の位置及び姿勢（M#WC）と、式１によって求められる計測対象物体１７０の補正後の位置及び姿勢（MWO）とを出力する場合では、計測対象物体１７０と撮像装置１３０の相対的な位置及び姿勢の関係は等価となる。したがって、本装置の出力を利用するアプリケーションが要求する形態に合わせて、出力形態を選択できるようになっていることが好ましい。
【００５９】
また、カメラ座標系における計測対象物体１７０の位置及び姿勢（MCO）を式２から算出してこれを出力しても良いし、物体座標系における撮像装置１３０の位置及び姿勢（MCO-1）を求めて、これを出力してもよい。
【００６０】
ステップＳ３０９０において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、ステップＳ３０００へと処理を進める。
以上によって、撮像装置の位置及び姿勢が計測される。
【００６１】
次に、図４のフローチャートを用いて、上記ステップＳ３０３５における撮像装置１３０の姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の姿勢を、３値ベクトル
s = ωWC = [ξWC ψWC ζWC]T
によって内部的に表現している。
【００６２】
姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。なお、姿勢ωは、次式、
【００６３】
【数３】
【００６４】
によって、３×３の回転行列Rによっても表現可能であり、ωとRとは、互いに一意に変換することができる。Rからωへの変換方法は公知であるので、その詳細な説明は省略する。
【００６５】
ステップＳ４０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の姿勢を、sの初期値とする。
【００６６】
ステップＳ４０１０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、指標Ｑｋｎ各々の物体座標系における座標ｘＯＱｋｎ（既知な情報として予め保持している）から画像座標を算出する関数：
【００６７】
【数４】
【００６８】
に基づいて行われる。具体的には、観測方程式Ｆｃ（）は、ｘＯＱｋｎとsから当該指標のカメラ座標（撮像装置１３０の視点位置を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘＣＱｋｎを求める次式、
【００６９】
【数５】
【００７０】
及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める次式、
【００７１】
【数６】
【００７２】
によって構成されている。ここでtWC及びtWOは、撮像装置１３０と計測対象物体１７０の夫々の位置計測値（平行移動成分）を表す３次元ベクトルであり、ここでは固定値として取り扱われる。また、RWOは計測対象物体１７０の姿勢計測値（回転成分）を表す３×３の回転行列であり、同様に固定値として取り扱われる。また、ｆＣｘ及びｆＣｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。
【００７３】
ステップＳ４０２０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差ΔｕＱｋｎを、次式によって算出する。
【００７４】
【数７】
【００７５】
ステップＳ４０３０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、sに関する画像ヤコビアン（すなわち、式４の観測方程式Ｆｃ（）をsの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列）ＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＱｋｎを算出する。
【００７６】
【数８】
【００７７】
ステップＳ４０４０において、位置姿勢算出部１２０は、ステップＳ４０２０及びステップＳ４０３０において算出した誤差ΔｕＱｋｎ及び画像ヤコビアンＪｕｓＱｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差ΔｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトル
【００７８】
【数９】
【００７９】
及び、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた２Ｎ行×３列の行列
【００８０】
【数１０】
【００８１】
を作成し、Θの擬似逆行列Θ+を用いて、
【００８２】
【数１１】
【００８３】
として算出する。このように、本実施形態では補正値Δsを、局所的な線形近似によって算出している。
【００８４】
ステップＳ４０５０において、位置姿勢算出部１２０は、ステップＳ４０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。
【００８５】
【数１２】
【００８６】
ステップＳ４０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のsを用いて、再度ステップＳ４０１０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ４０７０へと処理を進める。
以上のステップＳ４０００からＳ４０６０までの工程は、複数の観測方程式からなるsに関する非線形連立方程式をニュートン法によって求解する工程である。この一連の工程は、後述する変形例５に示すとおり、他のいかなる非線形解法を用いても良い。
【００８７】
ステップＳ４０７０において、位置姿勢算出部１２０は、ステップＳ４０６０までの処理によって得られたsを、撮像装置１３０の姿勢の（補正後の）推定値 ωWCとする。
【００８８】
以上によって、センサ計測値として得られた位置パラメータを固定して、全指標上での誤差の和を最小化するような姿勢パラメータを求めることができる。
【００８９】
次に、図５のフローチャートを用いて、上記ステップＳ３０７０における撮像装置１３０の位置及び姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の位置及び姿勢を、６値ベクトルs = [tWC T ωWC T]T = [xWC yWC zWC ξWC ψWC ζWC]Tによって内部的に表現している。
【００９０】
ステップＳ５０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の位置及び姿勢を、sの初期値とする。
【００９１】
ステップＳ５０１０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、各指標Ｑｋｎの物体座標系における座標ｘＯＱｋｎから画像座標を算出する関数：
【００９２】
【数１３】
【００９３】
に基づいて行われる。具体的には、観測方程式Ｆ’ｃ（）は、ｘＯＱｋｎとsから当該指標のカメラ座標ｘＣＱｋｎを求める式５、及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める式６によって構成されている。ただしここでは、式５におけるtWC（撮像装置１３０の位置を表す）を、固定値としてではなく、sを構成するパラメータの一部として取り扱う。一方、計測対象物体１７０の位置tWO及び姿勢RWOに関しては、センサによる計測値をそのまま固定値として使用する。
【００９４】
ステップＳ５０２０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差ΔｕＱｋｎを、式７によって算出する。
【００９５】
ステップＳ５０３０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、sに関する画像ヤコビアン（すなわち、式１３の観測方程式Ｆ’ｃ（）をsの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）ＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、式８によってＪｕｓＱｋｎを算出する。
【００９６】
ステップＳ５０４０において、位置姿勢算出部１２０は、ステップＳ５０２０及びステップＳ５０３０において算出した誤差ΔｕＱｋｎ及び画像ヤコビアンＪｕｓＱｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差ΔｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトルＵ及び、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた２Ｎ行×６列の行列Θを作成し、Θの擬似逆行列Θ+を用いて、式１１によって算出する。
【００９７】
ステップＳ５０５０において、位置姿勢算出部１２０は、ステップＳ５０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。
【００９８】
ステップＳ５０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後のsを用いて、再度ステップＳ５０１０以降の処理を行う。一方、収束していると判定された場合には、ステップＳ５０７０へと処理を進める。
【００９９】
ステップＳ５０７０において、位置姿勢算出部１２０は、ステップＳ５０６０までの処理によって得られたsを、撮像装置１３０の補正後の位置及び姿勢の推定値とする。
【０１００】
なお、本実施形態では、ステップＳ３０７０の処理において、撮像装置１３０の位置及び姿勢を補正の対象としていたが、計測対象物体１７０の位置及び姿勢を補正の対象として処理を行っても良い。この場合は、算出すべき未知パラメータを、計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [tWO T ωWO T]T = [xWO yWO zWO ξWO ψWO ζWO]Tによって内部的に表現し、センサ計測値として得られた計測対象物体１７０の位置及び姿勢をsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１０１】
【数１４】
【０１０２】
このとき、撮像装置１３０の位置tWC及び姿勢RWCに関しては、センサによる計測値をそのまま固定値として使用する。
【０１０３】
また、カメラ座標系における計測対象物体１７０の位置及び姿勢や、物体座標系における撮像装置１３０の位置及び姿勢を未知のパラメータとして求めても良い。
【０１０４】
前者の場合、算出すべき未知パラメータを、カメラ座標系における計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [tCO T ωCO T]T = [xCO yCO zCO ξCO ψCO ζCO]Tによって内部的に表現する。また、カメラ座標系における計測対象物体１７０の位置及び姿勢（M#CO）を、次式、
【０１０５】
【数１５】
【０１０６】
によってセンサ計測値（M#WC及びM#WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１０７】
【数１６】
【０１０８】
一方、後者の場合は、算出すべき未知パラメータを、物体座標系における撮像装置１３０の位置及び姿勢を表す６値ベクトルs = [tOCT ωOC T]T = [xOC yOC zOC ξOC ψOC ζOC]Tによって内部的に表現する。また、物体座標系における撮像装置１３０の位置及び姿勢（M#CO）を、次式、
【０１０９】
【数１７】
【０１１０】
によってセンサ計測値（M#WC及びM#WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１１１】
【数１８】
【０１１２】
以上のようにして、計測対象物体１７０と撮像装置１３０との間の相対的な位置及び姿勢の関係を未知のパラメータとして、センサによる位置及び姿勢計測値の誤差を補正することができる。
【０１１３】
従来の位置姿勢計測装置では、指標の分布を考慮していないため、画像上の一部の領域に指標が偏在して観測されている場合に解が不安定になる。一方、本実施形態に係る位置姿勢計測装置によれば、ステップＳ３０３３や、ステップＳ３０４０，Ｓ３０５０，Ｓ３０６０によって指標の分布範囲の大きさを考慮した手法の選択が行われるため、例えば検出された指標の数が多くても、それらが画像上の小領域に偏在しているような場合に、位置及び姿勢の６自由度を安定して求めるのに不十分であるという判定を行うことができる。そのため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、不安定な解が得られる状況に陥る可能性が低下し、従来手法と比して安定した解を得ることができる。
【０１１４】
また、本実施形態に係る位置姿勢計測装置によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行う場合に、その制約の中で指標上での誤差の和を最小化するような補正を行うことができる。したがって、未知とするパラメータを減らすことによって撮像装置の位置及び姿勢の計測を安定化させようとする場合であっても、従来と比べて高精度な計測を行うことができる。
【０１１５】
［第２の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等に設定された基準座標系における任意の計測対象物体の位置及び姿勢の計測を行う。
【０１１６】
本実施形態に係る位置姿勢計測装置は、撮像装置に位置姿勢センサが取り付けられておらず、三脚等によって既知の位置及び姿勢に固定して設置されていることが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。
【０１１７】
図１２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１２００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。
【０１１８】
撮像装置１３０は、三脚１２８０によって、空間中の既知の位置、姿勢に固定して設置されている。撮像装置１３０が出力する撮影画像は、位置姿勢計測装置１２００の画像入力部１６０に入力される。基準座標系における撮像装置１３０の位置及び姿勢は、既知の値として予め保持されているものとする。
【０１１９】
センサ計測値入力部１５０は、計測対象物体１７０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１２２０へと出力する。
【０１２０】
位置姿勢算出部１２２０は、センサ計測値入力部１５０の出力である計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、計測対象物体１７０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【０１２１】
なお、図１２に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１２２】
本実施形態における位置姿勢算出部１２２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）とほぼ同一である。唯一の違いは、撮像装置１３０の位置及び姿勢が、計測値として与えられるのではなく、既知の値として予め保持されていることである。位置姿勢算出部１２２０は、撮像装置１３０の位置及び姿勢の既知の値を、撮像装置１３０の位置及び姿勢の仮の計測値とおき、第１の実施形態におけるステップＳ３０００からステップＳ３０７０までと同一の工程によって撮像装置１３０の位置及び姿勢の計測値の補正を行い、この補正結果に基づいて、ステップＳ３０８０において、第１の実施形態と同様に計測対象物体１７０の位置及び姿勢の計測値を補正する（式１）。本実施形態では、位置姿勢算出部１２２０がステップＳ３０８０において出力するのは、計測対象物体１７０の位置及び姿勢のみである。
【０１２３】
以上によって、計測対象物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１２４】
なお、既知の位置に固定された撮像装置によって任意の計測対象物体の位置及び姿勢を計測する方法は、計測対象物体に取り付けるセンサを姿勢センサに変更しても実現できる。例えば、検出された指標の分布が閾値未満の場合は、姿勢センサの姿勢計測値を固定値として用いて、位置のみを指標の情報から推定する。また、指標の分布が閾値以上の場合には、位置及び姿勢の６パラメータを指標の情報から推定する。この場合の撮像装置は１台に限定されるものではなく、複数の撮像装置を用いることが可能である。
【０１２５】
［第３の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。
【０１２６】
本実施形態においては撮像装置以外の計測対象物体は存在せず、本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢の計測のみを行うことが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。
【０１２７】
図１３は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１３００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。
【０１２８】
また、現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標として、基準座標系における位置が既知である複数個の指標Ｑｋが配置されている。指標Ｑｋは、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、どのような形態であってもよい。
【０１２９】
センサ計測値入力部１５０は、撮像装置１３０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１３２０へと出力する。
【０１３０】
位置姿勢算出部１３２０は、センサ計測値入力部１５０の出力である撮像装置１３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【０１３１】
なお、図１３に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１３２】
本実施形態における位置姿勢算出部１３２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）と類似しているため、図３を参照しながら、第１の実施形態との相違点のみについて説明する。
【０１３３】
ステップＳ３００５において、位置姿勢算出部１３２０は、センサによって計測された撮像装置１３０の位置及び姿勢を、センサ計測値入力部１５０から入力する。
【０１３４】
ステップＳ３０２５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の姿勢計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（基準座標系における指標の３次元座標ｘＷＱｋｎと撮像装置１３０の位置及び姿勢の計測値から導出する投影座標の理論値ｕＱｋｎ＊と、実際の検出座標ｕＱｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。
【０１３５】
このような補正処理としては、周知の方法を用いることができるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。
【０１３６】
ステップＳ３０３５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の位置計測値を固定値として、撮像装置１３０の姿勢計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この補正処理工程は、第１の実施形態におけるステップＳ３０３５の補正処理工程（図４のフローチャートに対応）と類似したものとなる。ただし、本実施形態の場合、指標の観測方程式Ｆ’’ｃ（）が、基準座標系における指標Ｑｋｎの座標ｘＷＱｋｎ（既知な情報として予め保持している）から画像座標を算出する関数となる。すなわち、観測方程式Ｆ’’ｃ（）は次式によって表される。
【０１３７】
【数１９】
【０１３８】
具体的には、観測方程式Ｆ’’ｃ（）は、ｘＷＱｋｎとsから当該指標のカメラ座標ｘＣＱｋｎを求める次式、
【０１３９】
【数２０】
【０１４０】
及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める式６によって構成されており、この点のみが異なっている。ここでtWCは、撮像装置１３０の位置計測値を表す３次元ベクトルであり、ここでは固定値として取り扱われる。
【０１４１】
ステップＳ３０７０において、位置姿勢算出部１３２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程での具体的な処理は、例えば特許文献２に開示されているような周知技術を用いて実現することが可能であるので、ここでの説明は省略する。
【０１４２】
ステップＳ３０８０において、位置姿勢算出部１３２０は、ステップＳ３０７０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【０１４３】
以上によって、撮像装置の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１４４】
［第４の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における任意の計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。本実施形態においては、撮像装置ではなく、任意の計測対象物体の位置及び姿勢を計測することが、第３の実施形態と異なっている。以下では、第３の実施形態との相違点のみを説明する。
【０１４５】
図１４は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１４００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、撮像部１４３０、位置姿勢計測部１４４０、及び位置姿勢算出部１４２０によって構成されており、計測対象物体１４７０に接続されている。
【０１４６】
撮像部１４３０は、位置及び姿勢を計測したい対象である計測対象物体１４７０に固定して取り付けられており、指標を含む現実空間の映像を撮影する。
【０１４７】
位置姿勢計測部１４４０は、撮像部１４３０に固定して装着されており、基準座標系における撮像部１４３０の位置及び姿勢を計測してセンサ計測値入力部１５０へと出力する。位置姿勢計測部１４４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。
【０１４８】
画像入力部１６０は、撮像部１４３０が撮影した撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。
【０１４９】
センサ計測値入力部１５０は、撮像部１４３０の位置及び姿勢の計測値を位置姿勢計測部１４４０から入力し、位置姿勢算出部１４２０へと出力する。
【０１５０】
位置姿勢算出部１４２０は、センサ計測値入力部１５０の出力である撮像部１４３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、撮像部１４３０の位置及び姿勢の計測値の誤差を補正する。
位置姿勢算出部１４２０はさらに、得られた撮像部１４３０の位置及び姿勢と、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係についての情報（具体的には、計測対象物体１４７０上のどの位置に撮像部１４３０が設置されているかという情報。計測対象物体１４７０によって定義される物体座標系における、撮像部１４３０の位置及び姿勢によって表現される）から、基準座標系における計測対象物体１４７０の位置及び姿勢を算出し、Ｉ／Ｆ１００９を介してこのデータを外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。なお、この変換工程は必ずしも必要ではなく、撮像部１４３０の位置及び姿勢をそのまま出力することも可能である。
【０１５１】
なお、本実施形態では、位置姿勢計測部１４４０は撮像部１４３０の位置及び姿勢を計測していたが、位置姿勢計測部１４４０は計測対象物体１４７０の位置及び姿勢を計測するように構成されていても良い。この場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた計測対象物体１４７０の位置及び姿勢から撮像部１４３０の位置及び姿勢を算出し、これを初期値として上記と同様な処理を実行する。
【０１５２】
また、位置姿勢算出部１４２０は、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の何れの工程においても、撮像部１４３０の姿勢あるいは位置及び姿勢を補正し、その後計測対象物体１４７０の位置及び姿勢を求めていた。しかし、ステップＳ３０３５及びＳ３０７０の工程においては、計測対象物体１４７０の位置及び姿勢を直接求めるような構成としても良い。
【０１５３】
位置姿勢計測部１４４０が撮像部１４３０の位置及び姿勢を計測している場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた撮像部１４３０の位置及び姿勢から計測対象物体１４７０の位置及び姿勢を算出し、これを初期値とする。
【０１５４】
ステップＳ３０３５においては、計測対象物体１４７０の姿勢を表す３値ベクトルs = ωWO = [ξWO ψWO ζWO]Tを未知パラメータとする。また、ステップＳ３０７０においては、計測対象物体１４７０の位置及び姿勢を表す６値ベクトルs = [tWOT ωWOT]T = [xWO yWO zWO ξWO ψWO ζWO]Tを未知パラメータとする。そして、観測方程式Ｆ’’ｃ（）を定義するのに必要な式２０を、
【０１５５】
【数２１】
【０１５６】
に変更する。ここで、tOC 及びROCは、計測対象物体１７０によって定義される物体座標系における撮像部１４３０の姿勢を表す３×３の回転行列及び位置を表す３次元ベクトルであり、撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を表す既知の値として、予め保持されているものとする。なお、ステップＳ３０３５では、tWOは固定値として設定する。
【０１５７】
以上によって、任意の物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１５８】
［第５の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。
【０１５９】
図１５は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１５００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５５０、及び位置姿勢算出部１５２０によって構成されており、撮像装置１３０及び姿勢センサ１５４０に接続されている。
【０１６０】
本実施形態においては、撮像装置１３０に、位置姿勢センサではなく姿勢センサが装着されている点が、第３の実施形態と異なっている。姿勢センサを用いることで、位置姿勢センサを用いる場合に存在する、センサの計測範囲以外の領域を計測できないという制約を回避することができる。以下では、第３の実施形態との相違点のみを説明する。
【０１６１】
姿勢センサ１５４０は、撮像装置１３０に装着されており、姿勢センサ１５４０自身の現在の姿勢を計測してセンサ計測値入力部１５５０へと出力する。姿勢センサ１５４０は、例えばジャイロセンサと加速度センサをベースとしたセンサユニットであり、具体的には、株式会社トキメックのTISS-5-40や、米国InterSense社のInertiaCube2等によって構成される。これらのセンサによって計測される姿勢計測値は、慣性計測値を時間積分した値として計測されたものであるため、真の姿勢とは異なった誤差を有する。ただし、これらの姿勢センサは、内蔵する加速度センサによって地球の重力方向を観測することによって、傾斜角方向（ピッチ角及びロール角）のドリフト誤差の蓄積をキャンセルする機能を有している。そのため、傾斜角方向に関してはドリフト誤差が発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては時間の経過に伴って蓄積されるドリフト誤差を有している。
【０１６２】
センサ計測値入力部１５５０は、方位ドリフト誤差補正値の更新値を位置姿勢算出部１５２０から入力し、姿勢センサ１５４０の現在の方位ドリフト誤差補正値を更新し保持する。また、姿勢センサ１５４０から姿勢計測値を入力し、現在の方位ドリフト誤差補正値によってこれを補正して、撮像装置１３０の姿勢の予測値として位置姿勢算出部１５２０へと出力する。
【０１６３】
位置姿勢算出部１５２０は、センサ計測値入力部１５５０の出力である撮像装置１３０の姿勢の予測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力データとして、撮像装置１３０の位置及び姿勢を算出して出力する。また、位置及び姿勢の算出工程で導出される姿勢センサ１５４０の方位ドリフト誤差補正値の更新値を、センサ計測値入力部１５５０へと出力する。
【０１６４】
なお、図１５に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１６５】
図１６は、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部１５２０のソフトウェアのプログラムを実行することで行われる。
【０１６６】
ステップＳ１５０００において、位置姿勢算出部１５２０は、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを指標抽出部１１０から入力する。なお、各指標の基準座標系における３次元座標ｘＱｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。
【０１６７】
ステップＳ１５００５において、位置姿勢算出部１５２０は、撮像装置１３０の姿勢の予測値R*をセンサ計測値入力部１５５０から入力し、これを、撮像装置１３０の姿勢の初期値として設定する。また、２回目以降の処理では、１ループ前の処理において算出された撮像装置１３０の位置を、撮像装置１３０の姿勢の初期値として設定する。
【０１６８】
ステップＳ１５０１０において、位置姿勢算出部１５２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ１５０９０に処理を進め、それ以外の場合はステップＳ１５０２０に処理を進める。
【０１６９】
ステップＳ１５０２０において、位置姿勢算出部１５２０は、検出された指標の総数が一つか否かを判定する。指標の総数が一つの場合はステップＳ１５０２５に処理を進め、それ以外の場合はステップＳ１５０３０に処理を進める。
【０１７０】
ステップＳ１５０２５において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値の平行移動成分のうちの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加え、ステップＳ１５０９０に処理を進める。ステップＳ１５０２５での処理工程としては、周知の処理を提供することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの平行移動による補正方法（段落「００３０」〜「００３６」）およびランドマークを複数点利用したカメラの平行移動による補正方法（段落「００５１」〜「００５６」）に開示されている方法を用いることができる。ただし、特許文献１では６自由度位置姿勢センサの計測値を誤差補正の対象とした場合を説明しているが、本実施形態では、前フレームで求めた位置と姿勢センサの計測値とを組み合わせたものが補正対象となる点が異なっている。
【０１７１】
ステップＳ１５０３０において、位置姿勢算出部１５２０は、検出された指標の総数が二つか否かを判定する。指標の総数が二つの場合はステップＳ１５０３２に処理を進め、それ以外の場合はステップＳ１５０４０に処理を進める。
【０１７２】
ステップＳ１５０３２において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ１（例えば、画像の対角線長の１／８）との比較を行う。距離が閾値Ｔ１以上の場合にはステップＳ１５０３４に処理を進め、それ以外の場合はステップＳ１５０２５に処理を進める。
【０１７３】
ステップＳ１５０３４において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離と所定の値として定められている閾値Ｔ２（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ２以上の場合にはステップＳ１５０３８に処理を進め、それ以外の場合はステップＳ１５０３６に処理を進める。
【０１７４】
ステップＳ１５０３６において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうち、位置を表す３パラメータ（平行移動成分）を更新対象として（すなわち、姿勢計測値には誤差は含まれていないものと仮定して）、検出されている指標上での誤差を最小とするような補正を位置のパラメータのみに加え、ステップＳ１５０９０に処理を進める。この処理工程の詳細は、例えば特許文献３（第６の実施形態）に開示されているので、ここでの説明は省略する。また、位置を表す３パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置の補正を行うことができる。
【０１７５】
ステップＳ１５０３８において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうちの位置を表す３パラメータ（平行移動成分）と、姿勢センサ１３４０の方位ドリフト誤差補正値の更新値φの合計４パラメータとを、算出すべき未知パラメータとして取り扱い、検出されている指標の情報を用いてこれらを算出する。この処理工程の詳細は、例えば特許文献３（第７の実施形態）に開示されているので、ここでの説明は省略する。また、これら４パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置と方位ドリフト誤差補正値の更新値の算出を行うことができる。さらに、ステップＳ１５０８０へと処理を進める。
【０１７６】
ステップＳ１５０４０は、検出された指標の総数が三つ以上の場合のみ実行される。ここで、位置姿勢算出部１５２０は、検出された全指標の分布範囲に関する評価値の一例として、検出された全指標の画像座標を包含する凸包を算出する。
【０１７７】
ステップＳ１５０５０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ３（例えば、画像の面積の１／１６）との比較を行う。面積が閾値Ｔ３以上の場合はステップＳ１５０６０に処理を進め、それ以外の場合はステップＳ１５０３６へと処理を進め、上述した処理を行う。
【０１７８】
ステップＳ１５０６０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ４との比較を行う。面積が閾値Ｔ４（例えば、画像の面積の１／９）以上の場合はステップＳ１５０７０に処理を進め、それ以外の場合はステップＳ１５０３８へと処理を進め、上述した処理を行う。
【０１７９】
ステップＳ１５０７０において、位置姿勢算出部１５２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この算出は、例えば、撮像装置１３０の位置と姿勢を表す６パラメータを変数として、ステップＳ１５００５で設定した初期値を用いて、非線形方程式の反復解法によって誤差を最小化するパラメータを求めることによって実現される。すなわち、位置と姿勢を表す６パラメータを未知変数とした観測方程式を構成し、これに基づいて求められる指標投影位置の誤差と各パラメータに関する画像ヤコビアンとを用いて当該パラメータの補正値を求め、補正処理を反復的に行うことで画像上での誤差を最小とするような位置と姿勢を算出する。
位置姿勢算出部１５２０はさらに、ステップＳ１５００５で設定した姿勢の初期値と求められた姿勢との変化分を算出し、その方位成分を方位ドリフト誤差補正値の更新値φとする。
【０１８０】
ステップＳ１５０８０において、位置姿勢算出部１５２０は、ステップＳ１５０３８あるいはステップＳ１５０７０において得られた方位ドリフト誤差補正値の更新値φを、センサ計測値入力部処理１５５０へと出力する。
【０１８１】
ステップＳ１５０９０において、位置姿勢算出部１５２０は、ステップＳ１５０８０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【０１８２】
ステップＳ１５１００において、位置姿勢算出部１５２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ１５０００へと進める。
【０１８３】
以上によって、撮像装置の位置及び姿勢が計測される。
本実施形態に係る位置姿勢計測装置によれば、ステップＳ１５０３４やステップＳ１５０５０によって指標の分布範囲の大きさを考慮した手法の選択（方位ドリフト誤差補正値の更新を行うか否かの判断）が行われるため、画像上の一部の領域に指標が偏在して観測されている場合に、方位ドリフト誤差補正値の更新を不正確に行ってしまうという状況を回避することができる。
【０１８４】
また、本実施形態に係る位置姿勢計測装置によれば、第３の実施形態の効果に加え、ステップＳ１５０６０による手法の選択によって、画像情報が不足する場合であっても姿勢センサによる傾斜角情報を信頼することで位置及び姿勢を安定して導出できるほか、画像情報が十分得られる場合であれば位置及び姿勢の推定を高精度に行えるという、通常の画像情報に基づく位置姿勢推定手法が有する利点を両立することができる。
【０１８５】
［第１乃至第５の実施形態の変形例］
（変形例１）
上記の実施形態の各々では、画像上における指標の分布に基づいて、適用する補正アルゴリズムが選択される。これらの補正アルゴリズム（第１の実施形態におけるステップＳ３０２５，Ｓ３０３５，Ｓ３０７０、第２乃至第４の実施形態におけるこれらの変形、及び、第５の実施形態におけるステップＳ１５０２５，Ｓ１５０３６，Ｓ１５０３８，Ｓ１５０７０）は、必ずしも上記の実施形態で示したものである必要はなく、指標の分布に従って見込まれる画像情報の情報量に応じて適切に設定されたものであれば、他の補正アルゴリズムに変更することも可能である。
【０１８６】
例えば、第１乃至第４の実施形態におけるステップＳ３０２５やＳ３０３５において、姿勢の計測値を補正するのではなく、位置の計測値を補正する構成としてもよい。この場合、ステップＳ３０２５では、例えば、特許文献１の段落「００３０」〜「００３６」および「００５１」〜「００５６」に開示されているような、１つ又は複数の指標を利用したカメラの平行移動による補正方法に基づいて、位置計測値のうちのカメラの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加えることができる。
【０１８７】
また、ステップＳ３０３５では、算出すべき未知パラメータを、撮像装置１３０の位置を表す３値ベクトルs = tWC = [xWC yWC zWC ]Tとして、図５と同様な処理手順によって位置の補正を行う。第１の実施形態との相違点は、観測方程式を構成する式５を、次式のように変更することである。
【０１８８】
【数２２】
【０１８９】
ここで、RWCは、固定値として用いる撮像装置１３０のセンサによる姿勢計測値を表している。また、RWO 及びtWOは、第１の実施形態における式５と同様に、計測対象物体１７０の位置及び姿勢のセンサによる計測値を表しており、固定値として取り扱われる。
【０１９０】
なお、式２２は線形方程式であるので、式６と組み合わせた観測方程式から連立方程式を作成しこれを解くことで、繰り返し演算を用いなくても補正値を得ることができる。
【０１９１】
なお、不図示のＵＩを介して、姿勢の補正と位置の補正の何れを優先させるかをユーザが自由に選択できるようにしてもよい。この場合、ステップＳ３０２５及びＳ３０３５の実行時に、ユーザの選択に応じて、姿勢あるいは位置の補正アルゴリズムが実行される。これにより、特性の異なる位置姿勢センサを用いた場合でも、より好ましい計測を行うことができる。
【０１９２】
また、ステップＳ３０２５においては、計測値として得られる６パラメータのうちの任意の２パラメータを補正対象として選択し、これを未知数として上述した方法と記と同様にして求めてもよい。同様に、ステップＳ３０３５においては、計測値として得られる６パラメータのうちの任意の３パラメータを補正対象として選択し、これを未知数として上述した方法と同様な枠組みによって求めてもよい。
【０１９３】
（変形例２）
上記第１乃至第４の実施形態では、画像上における指標の分布の尺度に基づいて、３種類のアルゴリズムへと処理を分岐させていた（ステップＳ３０３３、ステップＳ３０５０及びステップＳ３０６０）。しかし、指標の分布の尺度に基づいて行う分岐数は３通りに限られるものではない。例えば、ステップＳ３０５０の処理を省いて、閾値Ｔ３と凸包の面積の大小関係によってステップＳ３０３５あるいはステップＳ３０７０へと処理を分岐させるだけでもよい。また、より多数の閾値を設けて、４通り以上の分岐を行ってもよい。例えば、補正対象とするパラメータの数（２〜６）を凸包の面積に応じて変更し、これを未知数として同様な方法により位置姿勢を算出してもよい。
【０１９４】
（変形例３）
上記の実施形態の各々では、位置姿勢センサや姿勢センサによる計測値と、撮像装置によって得られる画像情報を併用することにより、計測対象（計測対象物体や撮像装置）の位置及び姿勢を求めていた。しかし、画像上における指標の分布に基づいて、位置及び姿勢を求めるために用いるアルゴリズムを選択するという基本的な技術思想は、これらのセンサによる計測値を用いずに、画像情報のみから計測対象の位置及び姿勢を得る場合であっても適用できる。
【０１９５】
例えば、図１３に示した第３の実施形態の構成において、位置姿勢計測装置１３００がセンサ計測値入力部１５０を有さず、また、位置姿勢センサ１４０が撮像装置１３０に設置されていない場合を考える。この場合、位置姿勢算出部１３２０は、初期値として位置姿勢センサ１４０による撮像装置１３０の位置及び姿勢の計測値を用いるのではなく、前フレームで求めた撮像装置１３０の位置及び姿勢の算出値（あるいは、過去の算出値から予測した現フレームにおける位置及び姿勢）を用いることにすると、これを仮の計測値とおいて図３と同様な処理を行うことで、指標の分布に応じて適切な位置及び姿勢の算出アルゴリズムを選択することが可能となる。
【０１９６】
（変形例４）
上記の実施形態の各々では、指標の分布を計る評価値として、指標が３点以上検出されている場合には、指標が成す凸包を求めてその面積を利用していた。しかし、指標の分布を評価する尺度はこれに限られるものではなく、他の尺度を用いてもよい。例えば、画像中心から最も遠い位置にある指標を第１の指標として選択し、第１の指標から最も遠い位置にある指標を第２の指標として選択し、さらに、第１，第２の指標を結ぶ直線から最も遠い距離にある指標を第３の指標として選択し、これら第１，第２，第３の指標が成す三角形の面積を分布の尺度として、凸包の面積の変わりに用いても良い。また、簡易的には、このような第１，第２の指標間の距離を分布の尺度としても良いし、指標の画像座標の分散や標準偏差といった、分布を数値で表しうる他の任意の情報を分布の尺度として用いてもよい。
【０１９７】
他の指標の画像座標の分散を分布の尺度として用いる具体的な方法として、指標の画像座標の共分散を利用する方法が考えられる。検出されている指標の画像座標の分散共分散行列を求め、その固有値（この値は主成分分析における各主成分の分散に相当する）を用いる方法である。この場合、第２固有値を尺度とすることで、画像上における指標の２次元的広がりを評価することが可能である。また、第１固有値を尺度とすることで、画像の１次元的な広がりを評価することができる。
【０１９８】
また、それ以外にも、凸包の周囲長等の、凸包の面積以外の特徴量を用いてもよい。また、検出された全ての指標に対する外接矩形や外接円などから得られる特徴量を用いてもよい。また、指標の画像座標のモーメントの解析値（例えば、慣性等価楕円の長軸長と短軸長）等を用いてもよい。また、単一の多角形指標を用いる場合には、その指標を検出する際に得られる指標領域の面積をそのまま用いてもよい。
【０１９９】
また、検出された指標の分布を直接計測しなくても、指標の分布の大小を何らかの情報に基づいて推定してもよい。例えば、過去のフレームやセンサの情報から得たカメラや物体の位置及び姿勢の推定値（初期値）に基づいて、画像上で観測されるであろう指標の座標を算出し、計算上の指標の分布を求めてもよい。また、検出された指標の見かけの大きさ等から指標までの距離を近似的に推定して、それに応じて分布を推定してもよい。
【０２００】
（変形例５）
第１の実施形態におけるステップＳ３０３５では、図４のステップＳ４０４０にて詳細に説明したように、誤差ベクトルＵと行列Θに基づく補正値Δ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によって表現する。また、撮像装置２０３０の位置と姿勢を、６値のベクトルｓ= [tT ωT]T = [x y z ξ ψ ζ]Tによって表現する。
【０２７９】
なお、姿勢の情報は必要に応じて、カメラ座標系から基準座標系への回転変換を行う３×３の回転行列Ｒに変換して使用されるものとする。ωとＲは互いに一意に変換することができる。ωからＲへの変換は式３によって可能である。Ｒからωへの変換方法は公知であるので、その詳細な説明は省略する。
【０２８０】
ステップＳ２１０００において、位置姿勢算出部２０２０は、時刻τにおける撮像装置２０３０の姿勢計測値ω*τ( = [ξ*τ ψ*τ ζ*τ]T)を、姿勢センサ２０４０から入力する。
【０２８１】
ステップＳ２１０１０において、位置姿勢算出部２０２０は、時刻τにおける撮影画像から検出された指標Ｑｋｎ（ｎ＝１，２，・・・，Ｎ）の画像座標ｕｎ（＝ｕＣＱｋｎ）と基準座標ｘwＱｋｎの組を、指標検出部２０１０から入力する。
【０２８２】
ステップＳ２１０２０において、位置姿勢算出部２０２０は、入力された検出指標の情報が位置及び姿勢の推定に十分か否かを判定し、それに応じて処理の分岐を行う。具体的には、入力した指標の数が３点以上の場合はステップＳ２１０３０に処理を進め、３点未満の場合は処理を終了する。
【０２８３】
ステップＳ２１０３０において、位置姿勢算出部２０２０は、姿勢計測値のうちの傾斜角に関する２パラメータを規定値として扱い、夫々の検出指標の情報を用いて残された４パラメータを推定する。ここで、残された４パラメータとは、撮像装置２０３０の方位角と位置である。この処理は、例えば、非特許文献６に開示されている手法によって実現できるので、詳細な説明は省略する。以下では、ステップＳ２１０３０で得られる位置及び姿勢を、以降の処理における位置及び姿勢の初期値ｓ0= [t0T ω0T]T = [x0 y0 z0 ξ0 ψ0 ζ0]Tと表記する。なお、得られた姿勢ω0と姿勢計測値ω*τとの差異は、方位角成分のみとなる。すなわち、ω0はω*τの方位角ドリフト誤差を補正した姿勢と考えることができる。
【０２８４】
ステップＳ２１０４０において、位置姿勢算出部２０２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値（理論値）ｕｎ＊（＝ｕＣＱｋｎ＊）を算出する。ｕｎ＊は、ｓによって定義される指標の観測方程式、すなわち、指標の基準座標ｘwＱｋｎから画像座標を算出する式１９に基づいて算出される。ただし式１９では、画像座標の推定値ｕｎ＊を、ｕＱｋｎ＊と表記している。
【０２８５】
ステップＳ２１０５０において、位置姿勢算出部２０２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕｎ＊と実測値ｕｎとの差異（再投影誤差）Δｕｎを、次式３４によって算出する。
【０２８６】
【数３４】
【０２８７】
ステップＳ２１０６０において、位置姿勢算出部２０２０は、各々の指標Ｑｋｎに対して、ｓに関する画像ヤコビアンＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。ここで、画像ヤコビアンとは、式１９の観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。具体的には、ヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）とヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、次式３５によってＪｕｓＱｋｎを算出する。
【０２８８】
【数３５】
【０２８９】
ここで、ＪｕｘＱｋｎは、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列である。また、ＪｘｓＱｋｎは、式２０の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列である。
【０２９０】
ステップＳ２１０７０において、位置姿勢算出部２０２０は、ステップＳ２１０３０で得た姿勢の初期値ω0に対する現在の姿勢推定値ωの差異Δω( = [Δξ Δψ Δζ]T)を、次式３６によって算出する。
【０２９１】
【数３６】
【０２９２】
ステップＳ２１０８０において、位置姿勢算出部２０２０は、上記のステップで算出した再投影誤差Δｕｎ、画像ヤコビアンＪｕｓＱｋｎ、及び姿勢の差異Δωに基づいて、ｓの補正値Δｓを算出する。具体的には、ヤコビアンを並べた行列Θと誤差ベクトルＵを作成し、次式３７によってΔｓを算出する。
【０２９３】
【数３７】
【０２９４】
ここで、行列Θは、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた行列に、姿勢のパラメータξ，ψ，ζ自身のｓに関するヤコビアンを追加した行列である。Θは次式３８によって定義される。
【０２９５】
【数３８】
【０２９６】
また、Θ'は、Θの転置行列に重み係数wを付加した行列であり、次式３９によって定義される。
【０２９７】
【数３９】
【０２９８】
ここでwは、画像情報と姿勢計測値とのバランスをとるための重み係数であり、「姿勢計測値を重視する度合い」を表している。wの値によって、最終的に得られる位置及び姿勢に対する姿勢計測値の影響力の基準値が決定される。重み係数wの値は、重み設定部２０６０によって与えられる。なお、姿勢計測値の実際の影響力は、指標から得られる情報量の大小に応じて受動的に変更される。
【０２９９】
一方、誤差ベクトルＵは、ΔｕｎとΔωの各項を垂直に並べた２Ｎ＋３次元のベクトルである。Ｕは次式４０によって定義される。
【０３００】
【数４０】
【０３０１】
ここで、式３７乃至式４０によって求められるΔｓの意味について説明する。式３６の目的は、直感的には、誤差ベクトルＵを小さくするようなΔｓを求めることである。例えば第５の実施形態のステップＳ１５０７０における処理のような、指標の情報のみを用いてカメラの位置と姿勢を求める手法では、誤差ベクトルＵは次式４１によって定義される。
【０３０２】
【数４１】
【０３０３】
これは、指標の再投影誤差を評価基準として、これを最小化することを意図している。
【０３０４】
一方、式４０の誤差ベクトルＵは、式４１の誤差ベクトルＵに姿勢の差異Δωを付加したものとなっている。これは、本実施形態に係る位置姿勢算出部２０２０が、指標の再投影誤差と姿勢計測値の差異を評価基準としていることを示している。直感的には、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするようなΔｓが算出される。
【０３０５】
ステップＳ２１０９０において、位置姿勢算出部２０２０は、ステップＳ２１０８０において算出した補正値Δｓを用い、式１２に従ってｓを補正し、得られた値をｓの新たな推定値とする。
【０３０６】
ステップＳ２１１００において、位置姿勢算出部２０２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ２１０４０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ２１１１０へと処理を進める。
【０３０７】
ステップＳ２１１１０において、位置姿勢算出部２０２０は、ステップＳ２１１００までの処理によって得られたｓを、撮像装置２０３０の位置と姿勢の推定値として出力する。なお、位置及び姿勢の出力形態はｓそのものに限定されるものではない。例えば姿勢を、回転行列、回転軸ベクトルと回転角、オイラー角、４元数等に変換して出力してもよい。また、同次座標表現による４×４の位置姿勢変換行列やその逆行列によって、位置及び姿勢を表してもよい。また、撮像装置２０３０に対する基準座標系の位置及び姿勢を求めてこれを出力してもよい。もちろん、これらのいずれかの組み合わせを同時に出力してもよい。
【０３０８】
以上のようにして、撮像装置２０３０の位置及び姿勢が算出される。なお、連続的に撮影される時系列画像に対して撮像装置２０３０の位置及び姿勢を求める場合には、入力されたフレーム毎に上述の処理を実行すればよい。
【０３０９】
本実施形態に係る位置姿勢計測装置によれば、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような位置と姿勢が算出される。
【０３１０】
本実施形態に係る位置姿勢計測装置によれば、画像から得られる情報が不十分な場合には、検出指標の項の影響力が小さくなるので、姿勢計測値を重視した位置及び姿勢を得ることができる。その結果、姿勢計測値を信用して未知パラメータ数を限定する手法と同様な、解を安定化させる効果を得ることができる。一方、画像情報が十分に得られている場合には、検出指標の項の影響力が大きくなる。その結果、姿勢計測値の誤差に影響されることなく、高精度な解を得ることができる。
【０３１１】
すなわち、本実施形態に係る位置姿勢計測装置によれば、画像と姿勢センサから得られる情報を無駄なく利用することが可能となり、従来の方法と比べて安定性と精度の双方に優れた計測が実現できる。
【０３１２】
［第８の実施形態］
本実施形態に係る位置計測装置は、環境側に配置した複数台の撮像装置及び指標、計測対象物体に設置した複数台の撮像装置、指標、及び姿勢センサを用いて、任意の計測対象物体の位置と姿勢の計測を行う。以下では、本実施形態に係る位置計測装置及び位置計測方法について説明する。
【０３１３】
図２２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置２２００は、画像入力部２２５０、指標検出部２２１０、位置姿勢算出部２２２０、モード選択部２２８０、１台以上の主観視点カメラ２２３０，１台以上の客観視点カメラ２２６０、及び姿勢センサ２０４０によって構成されている。位置姿勢計測装置２２００は、計測対象物体２２７０に接続されている。基準座標系における計測対象物体２２７０の位置と姿勢が、本実施形態に係る位置計測装置２２００によって計測される。
【０３１４】
シーンには、第１の実施形態と同様に、基準座標系における位置が既知である１つ以上の指標Ｑｋ（ｋ＝１，２，・・・，ＫＷ）が配置されている。以下では、環境側に配置されているこれらの指標を、必要に応じて主観視点指標と呼ぶ。主観視点指標の形態は、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かつ、いずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れのものであってもよい。図２２では、ＫＷ＝６、すなわち６個の主観視点指標が配置されている。
【０３１５】
また、計測対象物体２２７０には、物体座標系上における位置（物体座標）が既知である１つ以上の指標ＱＫＷ＋ｋ（ｋ＝１，・・・，ＫＯ）が配置されている。以下では、物体側に配置されているこれらの指標を、必要に応じて客観視点指標と呼ぶ。客観視点指標の形態は、主観視点指標と同様に、撮影画像上における投影像の画像座標が検出可能であって、かつ、いずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れのものであってもよい。図２２では、ＫＯ＝４、すなわち４個の客観視点指標が配置されている。なお，客観視点指標は、計測対象物体２２７０上に直接配置されていなくてもよい。客観視点指標は、例えば、主観視点カメラ２２３０上や姿勢センサ２０４０上に配置されていてもよい。
【０３１６】
主観視点カメラ２２３０ａ及び２２３０ｂ（まとめて、主観視点カメラ２２３０ともいう）は、例えばビデオカメラである。主観視点カメラ２２３０は、計測対象物体２２７０上に配置されている。主観視点カメラ２２３０の夫々が撮像する画像（主観視点画像）は、画像入力部２２５０に入力される。主観視点カメラ２２３０のカメラ内部パラメータや、物体座標系における位置及び姿勢は既知であるものとする。主観視点指標及び主観視点カメラ２２３０の配置は、計測範囲内を計測対象物体２２７０が移動した際に、その撮影画像中に常に主観視点指標を観測可能なように調整されていることが好ましい。図２２では、２台の主観視点カメラ２２３０ａ，２２３０ｂが配置されている。以下では必要に応じて、主観視点カメラ２２３０の夫々によって定義される主観視点カメラ座標系を、記号Ｃ１，Ｃ２で表現する
【０３１７】
客観視点カメラ２２６０ａ〜２２６０ｄ（まとめて、客観視点カメラ２２６０ともいう）は、例えばビデオカメラである。客観視点カメラ２２６０は、計測対象物体２２７０を撮影可能なように、計測範囲を包囲する位置に設置されている。客観視点カメラ２２６０の夫々が撮像する画像（客観視点画像）は、画像入力部２２５０に入力される。客観視点カメラ２２６０のカメラ内部パラメータや、基準座標系における位置及び姿勢は既知であるものとする。客観視点指標及び客観視点カメラ２２６０の配置は、計測範囲内を計測対象物体２２７０が移動した際に、その撮影画像中に常に客観視点指標を観測可能なように調整されていることが好ましい。図２２では、４台の客観視点カメラ２２６０ａ，２２６０ｂ，２２６０ｃ，２２６０ｄが配置されている。以下では必要に応じて、客観視点カメラ２２６０ａ〜２２６０ｄの夫々によって定義される客観視点カメラ座標系を、それぞれ記号Ｂ１，Ｂ２，Ｂ３，Ｂ４で表現する。
【０３１８】
姿勢センサ２０４０は、計測対象物体２２７０上に配置されている。姿勢センサ２０４０は、計測対象物体２２７０の姿勢を計測して位置姿勢算出部２２２０へと出力する。
【０３１９】
画像入力部２２５０は、夫々のカメラから入力される撮影画像をデジタルデータに変換し、指標検出部２２１０へと出力する。
【０３２０】
指標検出部２２１０は、画像入力部２２５０より夫々の撮影画像を入力する。指標検出部２２１０は、夫々の入力画像を解析し、夫々の画像中に撮影されている指標を検出（並びに同定）する。指標検出部２２１０は、夫々の検出指標について、検出したカメラの識別子と、画像座標の実測値と、対応する３次元座標の組を、位置姿勢算出部２２２０へと出力する。ここで、３次元座標とは、指標が主観視点指標の場合は基準座標を、客観視点指標の場合は物体座標を意味している。なお、指標の検出方法は第１の実施形態と同様にいずれの方法であっても良いので、詳細な説明は省略する。
【０３２１】
なお、以下では、検出された指標の夫々に通し番号ｎ（ｎ＝１，，，Ｎ）をつけ、画像上で検出された夫々の指標をＱｋｎと表記する。ここで、Ｎは各画像上で検出された指標の総数である。また、Ｎ＝ΣＮｄであり、Ｎｄは夫々のカメラの撮影画像上で検出された指標の数を表している。また、ｄはカメラの識別子（ｄ＝Ｃ１，Ｃ２，Ｂ１，Ｂ２，Ｂ３，Ｂ４）を表している。また、指標Ｑｋｎの画像座標をｕｎ、その指標を撮影したカメラの識別子をｄｎと表記し、識別子ｄのカメラで検出された指標Ｑｋの画像座標をｕｄＱｋと表記する（すなわち、ｕｎ＝ｕｄｎＱｋｎ）。また、指標Ｑｋが主観視点指標である場合に、既知の値であるその基準座標をｘｗＱｋと表記する。同様に、指標Ｑｋが客観視点指標である場合に、既知の値であるその物体座標をｘＯＱｋと表記する。
【０３２２】
モード選択部２２８０は、キーボード１００４やマウス１００５等の入力デバイスを介して、位置姿勢算出部２２２０において使用するアルゴリズムを選択する。例えばラジオボタン等のＧＵＩによって、「指標の情報を重視」モード、「姿勢センサの計測値を重視」モード、それらの「中間」モード、のいずれかを選択可能とする。モード選択部２２８０は、選択されたモードがいずれであるかを、位置姿勢算出部２２２０に出力する。
【０３２３】
位置姿勢算出部２２２０は、計測対象物体２２７０の姿勢計測値を姿勢センサ２０４０から入力する。また、夫々の検出指標について、検出したカメラの識別子ｄｎと、画像座標の実測値ｕｎと、対応する３次元座標（ｘｗＱｋｎまたはｘＯＱｋｎ）の組を、指標検出部２２１０から入力する。そして、入力した情報に基づいて、計測対象物体２２７０の位置及び姿勢を算出して出力する。位置姿勢算出部２２２０の処理の詳細については、フローチャートを用いて後に詳しく説明する。
【０３２４】
なお、画像入力部２２５０、指標検出部２２１０、モード選択部２２８０、及び位置姿勢算出部２２２０の少なくとも一部は、独立した装置として実現しても良い。あるいは、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部２２５０、指標検出部２２１０、モード選択部２２８０、及び位置姿勢算出部２２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。なお、ソフトウェアを実行することで各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので、詳細な説明は省略する。
【０３２５】
図２３は、位置姿勢算出部２２２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部２２２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。
【０３２６】
以下では、計測対象物体２２７０の位置及び姿勢を、第１の実施形態と同様、３値のベクトルt = [x y z]T及びω= [ξψ ζ]Tによって表現する。また、計測対象物体２２７０の位置と姿勢を、６値のベクトルｓ= [tT ωT]T = [x y z ξ ψ ζ]Tによって表現する。
【０３２７】
ステップＳ２３０００において、位置姿勢算出部２２２０は、時刻τにおける計測対象物体２２７０の姿勢計測値ω*τ( = [ξ*τ ψ*τ ζ*τ]T)を、姿勢センサ２０４０から入力する。
【０３２８】
ステップＳ２３０１０において、位置姿勢算出部２２２０は、時刻τにおける夫々の撮影画像から検出された検出指標Ｑｋｎの情報を、指標検出部２２１０から入力する。具体的には、検出したカメラの識別子ｄｎと、画像座標の実測値ｕｎと、対応する３次元座標（ｘｗＱｋｎまたはｘＯＱｋｎ）の組を入力する。
【０３２９】
ステップＳ２３０２０において、位置姿勢算出部２２２０は、入力された検出指標の情報が位置及び姿勢の推定に十分か否かを判定し、それに応じて処理の分岐を行う。例えば、入力した指標の実体の総数が３点以上（Ｎ≧３）の場合はステップＳ２３０３０に処理を進め、３点未満の場合は処理を終了する。ここで、実態の総数とは、同一指標が複数の画像中で検出された場合には、１つの指標が検出されたものとして計数した総数を意味する。
【０３３０】
ステップＳ２３０３０において、位置姿勢算出部２２２０は、モード選択部２２８０で選択されているモードが、「指標の情報を重視」モードであるか否かの判定を行う。「指標の情報を重視」モードの場合はステップＳ２３０３５へと処理を進め、その他の場合はステップＳ２３０４０へと処理を進める。
【０３３１】
ステップＳ２３０３５において、位置姿勢算出部２２２０は、夫々の検出指標の再投影誤差を評価基準として用いる手法によって、計測対象物体２２７０の位置及び姿勢を推定する。主観及び客観視点カメラから得られる検出指標の再投影誤差を最小化することで物体の位置及び姿勢を求める手法は、非線形最適化計算に基づく手法が非特許文献１等に開示されているので、詳細な説明は省略する。さらに、ステップＳ２３０７０へと処理を進める。
【０３３２】
ステップＳ２３０４０において、位置姿勢算出部２２２０は、姿勢計測値のうちの傾斜角に関する２パラメータを規定値として扱い、夫々の検出指標の情報を用いて残された４パラメータを推定する。ここで、残された４パラメータとは、計測対象物体２２７０の方位角と位置である。この処理は、例えば、非特許文献１に開示されている手法によって実現できるので、詳細な説明は省略する。以下では、ステップＳ２３０４０で得られる位置及び姿勢を、以降の処理における位置及び姿勢の初期値ｓ0= [t0T ω0T]T = [x0 y0 z0 ξ0 ψ0 ζ0]Tと表記する。
【０３３３】
ステップＳ２３０５０において、位置姿勢算出部２２２０は、モード選択部２２８０で選択されているモードが、「姿勢センサの計測値を重視」モードであるか否かの判定を行う。「姿勢センサの計測値を重視」モードの場合は、ステップＳ２３０４０で得られる位置及び姿勢をそのまま出力することとして、ステップＳ２３０７０へと処理を進める。また、その他の場合はステップＳ２３０６０へと処理を進める。
【０３３４】
ステップＳ２３０６０において、位置姿勢算出部２２２０は、夫々の検出指標の再投影誤差と、推定された姿勢パラメータと姿勢計測値との間の差異とを評価基準として用いる手法によって、計測対象物体２２７０の位置及び姿勢を推定する。ステップＳ２３０６０の処理の詳細については、フローチャートを用いて後に詳しく説明する。
【０３３５】
ステップＳ２３０７０において、位置姿勢算出部２２２０は、以上の処理によって得られた計測対象物体２２７０の位置及び姿勢を出力する。
【０３３６】
次に、図２４のフローチャートを用いて、ステップＳ２３０６０における処理の詳細を説明する。
【０３３７】
ステップＳ２４０１０において、位置姿勢算出部２２２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値（理論値）ｕｎ＊を算出する。ｕｎ＊は、指標Ｑｋｎが主観視点指標の場合には、次式４２で表される主観視点カメラ指標Ｃｉによる観測方程式に基づいて算出される。
【０３３８】
【数４２】
【０３３９】
観測方程式Ｆｃｉ（）は、ｘwＱｋｎとｓから当該指標のカメラ座標ｘＣｉＱｋｎを求める式４３と、ｘＣｉＱｋｎから画像座標ｕｎ＊を求める式４４によって構成される。
【０３４０】
【数４３】
【数４４】
【０３４１】
ここでｆＣｉｘ及びｆＣｉｙは、それぞれｘ軸方向及びｙ軸方向における主観視点カメラ指標Ｃｉの焦点距離である。また、RCiOは物体座標系Oから主観視点カメラ座標系Ｃｉへ姿勢を変換する行列、tCiOは同座標系間の位置を変換するベクトルである。いずれの値も、主観視点カメラ２２３０ａ，２２３０ｂ毎に予め位置の値として保持されているものとする。
【０３４２】
一方、指標Ｑｋｎが客観視点指標の場合には、ｕｎ＊は、次式４５で表される客観視点カメラＢｉによる観測方程式に基づいて算出される。
【０３４３】
【数４５】
【０３４４】
観測方程式ＦＢｉ（）は、ｘＯＱｋｎとｓから当該指標の世界座標ｘＷＱｋｎを求める式４６と、ｘＷＱｋｎから夫々の客観視点カメラ座標系における座標ｘＢｉＱｋｎを求める式４７と、ｘＢｉＱｋｎから画像座標ｕｎ＊を求める式４８によって構成される。
【０３４５】
【数４６】
【数４７】
【数４８】
【０３４６】
ステップＳ２４０２０において、位置姿勢算出部２２２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕｎ＊と実測値ｕｎとの差異（再投影誤差）Δｕｎを、式３４によって算出する。
【０３４７】
ステップＳ２４０３０において、位置姿勢算出部２２２０は、各々の指標Ｑｋｎに対して、ｓに関する画像ヤコビアンＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。ここで、画像ヤコビアンとは、式４２もしくは式４５の観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。観測方程式Ｆｃｉ（）の画像ヤコビアンは式３５と同様にして得ることができる。一方、観測方程式ＦＢｉ（）の画像ヤコビアンは、ヤコビ行列ＪｕｘＢＱｋｎ（＝∂ｕ／∂ｘＢ）とヤコビ行列ＪｘＢｘＷＱｋｎ（＝∂ｘＢ／∂ｘＷ）とヤコビ行列ＪｘＷｓＱｋｎ（＝∂ｘＷ／∂ｓ）を算出し、次式４９によって算出する。
【０３４８】
【数４９】
【０３４９】
ここで、ＪｕｘＢＱｋｎは、式４８の右辺を客観視点カメラ座標ｘＢｉＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列である。また、ＪｘＢｘＷＱｋｎは、式４７の右辺を世界座標ｘＷＱｋｎの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列である。また、ＪｘＷｓＱｋｎは、式４６の右辺を状態ベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列である。
【０３５０】
ステップＳ２４０４０において、位置姿勢算出部２２２０は、ステップＳ２３０４０で得た姿勢の初期値ω0に対する現在の姿勢推定値ωの差異Δω( = [Δξ Δψ Δζ]T)を、式３５によって算出する。
【０３５１】
ステップＳ２４０５０において、位置姿勢算出部２２２０は、上記のステップで算出した再投影誤差ΔｕＱｋｎ、画像ヤコビアンＪｕｓＱｋｎ、及び姿勢の差異Δωに基づいて、ｓの補正値Δｓを算出する。具体的には、ヤコビアンを並べた行列Θと誤差ベクトルＵを作成し、式３７によってΔｓを算出する。なお、行列Θ、行列Θ'、及び行列Ｕの定義は、それぞれ式３８，式３９，式４０と同様である。ただし、第７の実施形態とは異なり、夫々の行列には、異なるカメラ（客観視点カメラ及び主観視点カメラ）による検出指標Ｑｋｎに関する情報が全て含まれている。また、本実施形態では、重み係数ｗは、予め求めた適切な値が固定値として設定されているものとする。
【０３５２】
ステップＳ２４０６０において、位置姿勢算出部２２２０は、ステップＳ２４０５０において算出した補正値Δｓを用いて式１２に従ってｓを補正し、得られた値をｓの新たな推定値とする。
【０３５３】
ステップＳ２４０７０において、位置姿勢算出部２２２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ２４０１０以降の処理を行う。一方、収束していると判断した場合には処理を終了する。
【０３５４】
以上のようにして、計測対象物体２２７０の位置及び姿勢が算出される。本実施形態に係る位置姿勢計測装置によれば、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、夫々のカメラによる検出指標の再投影誤差を極力小さくするような位置と姿勢が算出される。
【０３５５】
本実施形態では、主観視点カメラ２２３０と客観視点カメラ２２６０の双方を用いた。しかし、計測対象物体２２７０の位置及び姿勢の計測に使用するカメラの構成はこれに限定されるものではない。例えば、主観視点カメラ２２３０のみを用いても良いし、客観視点カメラ２２６０のみを用いても良い。
【０３５６】
なお、主観視点カメラ２２３０の位置及び姿勢を求めることが目的の場合にも、本実施形態と同様な方法を用いることができる。この場合、主観視点カメラ２２３０（のうちのひとつ）自体が計測対象物体２２７０であると考えればよい。
【０３５７】
本実施形態では、予め定めた重み係数を固定値として用いた。しかし、第１の実施形態と同様に重み設定部２０６０を設けて、操作者が重み係数wを設定することも可能である。また、重み設定部２０６０とモード選択部２２８０を同一のＵＩで設定できるような工夫をしても良い。例えば、重み設定部２０６０の入力手段として、正規化されたパラメータ（例えば、0から1の間の値）を数値入力するＵＩを考える。この場合、入力されたパラメータが0場合は「指標の情報を重視」モード、1の場合は「姿勢センサの計測値を重視」モード、0と1の中間値の場合は「中間」モードとしても良い。そして、「中間」モードの場合にのみ、入力されたパラメータを重み係数wに変換して用いても良い。
【０３５８】
［第７及び第８の実施形態の変形例］
（変形例７）
上記第７及び第８の実施形態では、姿勢センサ２０４０による姿勢計測値から離れすぎてはいけないという制約条件を考慮しながら、計測対象物体（撮像装置２０３０または計測対象物体２２７０）の位置及び姿勢を推定していた。しかし、上記実施形態で述べた技術的思想の適用範囲は、姿勢計測値から離れすぎてはいけないという制約条件だけを考慮することに限定されるものではない。
【０３５９】
例えば、連続的に撮影される時系列画像に対して計測対象物体の位置及び姿勢を求める場合には、前のフレームとの連続性を考慮してもよい。前のフレームで求めた位置及び姿勢から離れすぎてはいけないという制約条件を考慮することで、フレーム間の連続性に優れた位置及び姿勢を得ることができる。
【０３６０】
本変形例の処理ステップは、上記第７及び第８の実施形態とほぼ同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、ステップＳ２１１１０において位置及び姿勢の推定結果（ｓτ-1）が保持される点と、補正値の算出工程（ステップＳ２１０８０）において前フレームに対する拘束条件が考慮される点のみが異なっている。なお、第８の実施形態も同様に変更することができる。
【０３６１】
本変形例では、ステップＳ２１０８０における式３８の行列Θ、式３９のΘ'、式４０の誤差ベクトルＵの夫々を、次式５０〜５２のように変更する。
【０３６２】
【数５０】
【数５１】
【数５２】
【０３６３】
すなわち、前フレームの結果に対する拘束条件を表す６つの評価値が、新たに追加される。ここで、w１，w２，w３は、それぞれ姿勢計測値の影響度、前フレームの位置の影響度、前フレームの姿勢の影響度を定める重みパラメータである。本変形例では、夫々の重みパラメータには適当な値が予め設定されているものとする。これらの値は、姿勢センサの信頼度やフレーム間の連続性を重視する度合いに応じて個別に設定しても良いし、簡易的には同一の値を設定してもよい。また、パラメータごとに異なる値を設定してもよい。また、上記の実施形態と同様に、重み設定部２０６０を介してユーザが値を調整できるようにしてもよい。
【０３６４】
以上によって、姿勢が姿勢計測値から離れすぎてはいけないという制約条件と、位置及び姿勢が前フレームの推定結果から離れすぎてはいけないという制約条件を同時に考慮することができる。そして、これらの拘束条件の範囲内で、指標の再投影誤差を極力小さくするような位置と姿勢を算出することができる。
【０３６５】
なお、前フレームにおける位置及び姿勢の推定結果であるｓτ-1は、必ずしもステップＳ２１１１０で推定された位置及び姿勢そのものでなくてもよい。例えば、計測対象物体の移動の履歴から、現フレームにおける計測対象物体の位置及び姿勢を予測した値を用いてもよい。この場合、予測は計測対象物体の速度や角速度から単純な線形近似によって行ってもよいし、他のいずれの方法によって行ってもよい。
【０３６６】
なお、本変形例では前フレームにおける位置及び姿勢パラメータの全て（６自由度）を制約条件として用いていたが、必ずしも全パラメータを制約条件としなくてもよい。例えば、姿勢の制約は姿勢センサの計測値に対してのみ行ってもよい。この場合、式５０の行列Θ、式５１のΘ'、式５２の誤差ベクトルＵの夫々から、前フレームの姿勢に関する項を除外すればよい。同様に、前フレームの位置に関する項を省略しても良い。また、前フレームにおける位置及び姿勢パラメータのうちの任意の組み合わせに対してのみ拘束を行ってもよい。
【０３６７】
（変形例８）
上記第７及び第８の実施形態に追加する形で、さらに、計測対象物体（撮像装置２０３０または計測対象物体２２７０）上に位置センサが設置されている場合を考える。この場合、姿勢センサから得られる姿勢計測値と位置センサから得られる位置計測値の双方を制約条件として用いることができる。
【０３６８】
本変形例の処理ステップは、上記第７及び第８の実施形態とほぼ同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、位置センサによる位置計測値を入力する工程をステップＳ２１０１０の直後にステップＳ２１０１５として有する点と、補正値の算出工程（ステップＳ２１０８０）において位置計測値に対する拘束が考慮される点のみが異なっている。なお、第８の実施形態も同様に変更することができる（図２３参照）。
【０３６９】
新規に追加されるステップとして、ステップＳ２１０１５において、位置姿勢算出部２０２０は、時刻τにおける撮像装置２０３０の位置計測値ｔ*τを、位置センサから入力する。
【０３７０】
ステップＳ２１０３０において、位置姿勢算出部２０２０は、位置及び姿勢の初期値ｓ0を算出する。本変形例では、ステップＳ２１０１０で入力した姿勢計測値ω*τに前フレームの処理で推定された姿勢センサの誤差補正値Δωτ-1を乗算した姿勢を、姿勢の初期値ω0とおく。また、ステップＳ２１０１５で入力した位置計測値ｔ*τと次式５３を用いて、位置の初期値ｔ０を算出する。
【０３７１】
【数５３】
【０３７２】
ここで、ｘｏは、既知の値である計測対象物体の座標系における位置センサの計測点の座標を示している。すなわち、式５３は、位置センサの計測値（計測点の位置を表している）から計測対象の位置への変換式を表している。
【０３７３】
本変形例では、ステップＳ２１０７０で算出する姿勢の初期値と現在の推定姿勢との差異を求める式（式３６）を、次式５４のように、位置及び姿勢の双方を含むように変形する。
【０３７４】
【数５４】
【０３７５】
そして、ステップＳ２１０８０で算出する、式３８の行列Θ、式３９のΘ'、式４０の誤差ベクトルＵの夫々を、次式５５〜５７のように変更する。
【０３７６】
【数５５】
【数５６】
【数５７】
【０３７７】
すなわち、位置の計測値に対する拘束条件が新たに追加される。ここで、w１およびw２は、それぞれ位置計測値の影響度と姿勢計測値の影響度を定める重みパラメータである。本変形例では、w１とw２には適当な値が予め設定されているものとする。これらの値は、夫々のセンサの信頼度に応じて個別に設定しても良いし、簡易的には同一の値を設定しても良い。また、パラメータごとに異なる値を設定してもよい。また、上記の実施形態と同様に、重み設定部２０６０を介してユーザが値を調整できるようにしても良い。
【０３７８】
ステップＳ２１１１０では、推定された位置及び姿勢を出力する。このとき、姿勢計測値ω＊τと最終的に得られた姿勢ωとの差異を求め、姿勢の誤差補正値Δωτ-1として保持しておく。
【０３７９】
以上によって、位置及び姿勢が位置計測値と姿勢計測値の夫々から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような位置と姿勢を算出することができる。
【０３８０】
本変形例に係る位置姿勢計測装置によれば、画像から得られる情報が不十分な場合には、指標の項の影響力が小さくなるので、夫々のセンサの計測値を重視した位置及び姿勢を得ることができる。一方、画像情報が十分に得られている場合には、指標の項の影響力が大きくなる。その結果、夫々のセンサ計測値の誤差に影響されることなく、高精度な解を得ることができる。
【０３８１】
なお、本変形例においても、フレーム間の連続性を考慮することができる。この場合、変形例７と同様に、前フレームで求めた位置及び姿勢に対する制約を補正値の算出工程に追加すればよい。具体的には、式５５の行列Θ、式５６のΘ'、式５７の誤差ベクトルＵの夫々に、前フレームの位置及び姿勢のパラメータを評価する項を追加すればよい。なお、全１２の制約条件（前フレームに関して６、センサ計測値に関して６）の任意の組み合わせを選択して用いてもよいことは言うまでもない。
【０３８２】
（変形例９）
上記第７及び第８の実施形態と変形例７及び８の夫々では、計測対象物体（撮像装置２０３０または計測対象物体２２７０）の位置及び姿勢の６自由度のパラメータを推定していた。しかし、上記実施形態で述べた技術的思想の適用範囲は、推定パラメータが位置及び姿勢の６値の場合のみに限定されるものではない。
【０３８３】
例えば、計測対象物体上にさらに高精度な位置センサが設置されている場合を考える。この場合、公知の方法として、位置センサから得られる位置を既知の値として、計測対象物体の姿勢パラメータのみを画像情報から求めるというアプローチを取ることができる。このアプローチは、例えば、特許文献４の第６の実施形態に記載されているように、計測対象物体の姿勢パラメータのみを未知とした非線形最適化処理によって実現できる。そして、このようなアプローチをとる場合においても、上記実施形態で述べた技術的思想をそのまま適用することができる。すなわち、姿勢センサ２０４０による姿勢計測値に対する拘束条件を考慮しながら、計測対象物体の姿勢パラメータを推定することができる。
【０３８４】
本変形例の処理ステップは、上記第７及び第８の実施形態と同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、位置センサによる位置計測値を入力する工程（ステップＳ２１０１５）を有する点と、計測対象物体の姿勢ωのみが未知パラメータとなる点のみが異なっている。なお、第８の実施形態も同様に変更することができる。
【０３８５】
ステップＳ２１０１５、ステップＳ２１０３０、ステップＳ２１１１０の処理は、変形例７と同様である。
【０３８６】
本変形例では、計測対象物体の位置を既知の値として扱い、未知パラメータは姿勢を表すωのみとなる。すなわち、ステップＳ２１０４０では、計測対象物体の位置を固定値とおく。また、ステップＳ２１０６０では、ωの各パラメータについてのみ画像ヤコビアンを算出する。さらに、ステップＳ２１０８０における式３８の行列Θ及び式３９のΘ'を、未知パラメータの変化に応じて次式５８及び式５９のように変更する。
【０３８７】
【数５８】
【数５９】
【０３８８】
以上によって、６自由度の位置及び姿勢を未知パラメータとしない場合であっても、姿勢センサの計測値から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような姿勢を求めることができる。
【０３８９】
なお、本変形例においても、フレーム間の連続性を考慮することができる。この場合、変形例７と同様に、前フレームで求めた姿勢に対する制約を補正値の算出工程に追加すればよい。具体的には、式５８の行列Θ、式５９のΘ'、式４０の誤差ベクトルＵの夫々に、前フレームの姿勢のパラメータを評価する項を追加すればよい。
【０３９０】
（変形例１０）
上記第７及び第８の実施形態と変形例７乃至９の夫々においては、重みパラメータｗの値は予め定めた固定値であるか、あるいは、重み設定部２０６０を介してユーザが設定した値であった。しかし、重みパラメータの値はこれらの方法で設定することに限定されるものではない。本変形例では、検出された指標の分布に応じて重みパラメータを能動的に調整することで、指標から得られる情報量に応じて指標の影響度を調整する効果を、より積極的に実現する。
【０３９１】
本変形例の処理ステップは、上記第７及び第８の実施形態と同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、検出された指標の分布状況を算出する工程（ステップＳ２１０２５）と、算出された指標の分布状況に応じて重みパラメータｗを決定する工程（ステップＳ２１０２７）を有する点のみが異なっている。なお、第８の実施形態も同様に変更することができる。
【０３９２】
新規に追加されるステップとして、ステップＳ２１０２５において、位置姿勢算出部２０２０は、検出された全指標の画像座標を包含する凸包を算出する。この処理は、第６の実施形態におけるステップＳ１７０５０の処理と同様である。
【０３９３】
次に、新規に追加されるステップとして、ステップＳ２１０２７において、位置姿勢算出部２０２０は、ステップＳ２１０２５で求めた凸包の面積Ａに応じて、次式６０によって重み調整パラメータｗ０を決定する。
【０３９４】
【数６０】
【０３９５】
ここで、ＴminとＴmaxは凸包の面積を正規化するためのパラメータであり、適切な値が予め設定されているものとする。ｗ０の範囲は０から１に正規化された値であり、指標の分布が十分であれば０に、不十分であれば１に近い値となる。得られたｗ０に基づいて、補正値Δｓの算出工程（ステップＳ２１０８０）で用いる重み係数ｗを、次式６１によって算出する。
【０３９６】
【数６１】
【０３９７】
ここでｗ’は、重み設定部２０６０で設定された重み係数を表している。すなわち、重み設定部２０６０で設定された重み係数を、指標の分布に応じて増減する。なお、ｗ’の設定は重み設定部２０６０で行わなくてもよく、予め適切な値を設定しておいてもよい。
【０３９８】
以上によると、指標の分布が十分であれば重み係数ｗの値は小さくなるので、姿勢が姿勢計測値から離れすぎてはいけないという制約条件の影響度が小さくなる。そのため、センサの誤差に制約を受けることなく、検出された画像情報を十分に生かした位置及び姿勢の計測を行うことができる。
【０３９９】
一方、指標の分布が不十分な場合は重み係数ｗの値が大きくなるので、姿勢計測値から離れすぎてはいけないという制約条件の影響度が大きくなる。そのため、検出された画像情報の情報量が不足していても、センサから得られる姿勢を逸脱しない推定結果を得ることができる。
【０４００】
以上のように、本変形例によっても、画像から得られる情報の影響度を指標の分布に応じて制御するという効果を得ることができる。
【０４０１】
なお、本変形例においても、指標の分布の尺度として指標の成す凸包の面積を用いることは必須ではなく、上記実施形態の変形例４と同様な様々な尺度を用いてもよい。また、重み係数ｗの変位は指標の分布の尺度に対して線形である必要はなく、第６の実施形態と同様に二つの区間を単調増加する任意の値を用いることができる。
【０４０２】
なお、変形例７と同様に、前のフレームで求めた位置及び姿勢から離れすぎてはいけないという制約条件を同時に考慮する場合でも、指標の分布に応じて重み係数を調整することができる。この場合、ベースとなる重み係数としてｗ１’，ｗ２’，ｗ３’を保持しておいて、夫々にｗ０を乗算することで、実際に適用する重み係数（ｗ１，ｗ２，ｗ３）を求めればよい。また、変形例８のように位置センサによる拘束を利用する場合も同様に、指標の分布に応じて重み係数を調整することができる。
【０４０３】
なお、検出された画像情報の影響度を指標の分布に応じて調整するという効果は、上記で述べた１２の制約条件（前フレームに関して６、センサ計測値に関して６）の任意の組み合わせを用いる場合であっても得ることができる。例えば、センサを使用せずに、位置及び姿勢が前フレームの推定結果（の少なくともいずれか一つのパラメータ）から離れすぎてはいけないという制約条件のみを用いるという構成をとることもできる。
【０４０４】
この場合、指標の分布が十分であれば重み係数ｗの値は小さくなるので、位置や姿勢が前フレームにおける推定値から離れすぎてはいけないという制約条件の影響度が小さくなる。そのため、フレーム間の平滑化を行おうとする拘束の影響を受けることなく、検出された画像情報を十分に生かした位置及び姿勢の計測を行うことができる。
【０４０５】
一方、指標の分布が不十分な場合は重み係数ｗの値が大きくなるので、位置や姿勢が前フレームにおける推定値から離れすぎてはいけないという制約条件の影響度が大きくなる。そのため、検出された画像情報の情報量が不足していても、フレーム間での不自然なジッタ−を回避し、安定した位置及び姿勢を得ることができる。
【０４０６】
（変形例１１）
上記第７及び第８の実施形態と変形例７乃至１０の夫々は、式３７で表現される方法を用いて補正値Δsを算出している。また、sをs+Δsに置き換えることで推定値sの更新を行っている。しかしながら、行列Θ、行列Θ'、及び誤差ベクトルＵに基づいてsを算出する方法はこの方法に限ったものではない。例えば公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を組み合わせて求めてもよい。また、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよい。他の何れの数値計算手法を併用しても発明の本質が損なわれないことは言うまでもない。
【０４０７】
（変形例１２）
上記の夫々の実施形態や変形例では、式３で表されるような３値によって姿勢を表現し、これを最適化計算の工程で用いていた。しかし、最適化計算の工程で用いる姿勢の表現はこれに限定されるものではない。例えば、オイラー角によって姿勢を３値で表現し、このオイラー角を未知パラメータとして最適化計算を行ってもよい。この場合、式３の代わりに、オイラー角から３×３の回転行列Ｒを求める式を得て、これを夫々の観測方程式に組み込めばよい。また、姿勢計測値や前フレームの姿勢推定値による拘束を行う場合には、夫々のパラメータを同様にオイラー角に変更すればよい。もちろん、その他の表現方法を未知パラメータとした場合であっても同様に解くことが可能であり、パラメータの表現方法は本質的問題ではない。なお、オイラー角を用いた非線形最適化計算によるカメラの位置及び姿勢の推定方法は例えば非特許文献７に開示されているので、詳細な説明は省略する。
【０４０８】
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても達成される。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０４０９】
さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０４１０】
本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０４１１】
【図１】第１の実施形態における位置姿勢計測装置の構成を示す図である。
【図２】位置姿勢計測装置の各部をソフトウェアにより実現することのできるコンピュータの基本構成を示す図である。
【図３】第１の実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図４】図３のステップＳ３０３５における位置及び姿勢算出処理の詳細を説明するフローチャートである。
【図５】図３のステップＳ３０７０における位置及び姿勢算出処理の詳細を説明するフローチャートである。
【図６】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図７】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図８】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図９】図６の撮影画像（撮影画像６００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１０】図７の撮影画像（撮影画像７００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１１】図８の撮影画像（撮影画像８００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１２】第２の実施形態における位置姿勢計測装置の構成を示す図である。
【図１３】第３の実施形態における位置姿勢計測装置の構成を示す図である。
【図１４】第４の実施形態における位置姿勢計測装置の構成を示す図である。
【図１５】第５の実施形態における位置姿勢計測装置の構成を示す図である。
【図１６】第５の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図１７】第６の実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図１８】式２８によって得られる、検出された指標間の距離と重みｗ１との関係を示す図である。
【図１９】式２６及び式２８の夫々によって得られる、凸包の面積と重みｗ１及びｗ２との関係を示す図である。
【図２０】第７の実施形態における位置姿勢計測装置の構成を示す図である。
【図２１】第７の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図２２】第８の実施形態における位置姿勢計測装置の構成を示す図である。
【図２３】第８の実施形態における、計測対象物体２２７０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図２４】第８の実施形態における、指標の再投影誤差と姿勢センサの誤差を評価基準とする位置と姿勢の推定処理を説明するフローチャートである。
【技術分野】
【０００１】
本発明は、物体の位置及び姿勢、特に撮像装置の位置及び姿勢を計測する方法及び装置に関するものである。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない結合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）を重畳描画した画像を表示する装置として実現することができる。
【０００３】
このような画像表示装置を実現するには、現実空間中に定義した基準座標系（重畳しようとする仮想物体の位置及び姿勢を決定する基準となる現実空間中の座標系）と、撮像装置の座標系（カメラ座標系）との間の、相対的な位置及び姿勢を計測することが不可欠である。これは、仮想物体（仮想空間画像）を現実空間上の位置に合わせて描画するには、基準座標系に対する撮像装置の現実のカメラパラメータと同一のカメラパラメータを用いて仮想物体の画像を生成しなければならないからである。例えば、現実の室内のある位置に仮想物体を重畳表示する場合には、部屋に基準座標系を定義し、その基準座標系における撮像装置の位置及び姿勢を求めればよい。また、観察者が手に保持する現実の箱に何らかの仮想の模様やラベルを重畳表示する場合には、箱自身の物体座標系を基準座標系と考え、撮像装置に対する箱（基準座標系）の位置及び姿勢を求めればよい。
【０００４】
撮像装置の位置及び姿勢を計測する方法として、現実空間中に複数の指標（人為的なマーカや自然特徴など）を配置あるいは設定し、撮像装置によって撮像された画像内における指標の投影像の座標を検出して、指標の座標情報との関係に基づいて撮像装置の位置及び姿勢を求めることが一般的に行われている（例えば、非特許文献１）。しかし、このアプローチを用いる場合、常に指標が撮像されなくてはならないという制約がある。
【０００５】
一方、撮像装置に磁気センサや超音波センサなどを用いた６自由度位置姿勢センサを装着し、これによって計測した撮像装置の位置及び姿勢の誤差を、指標を撮像した画像を用いて得られる情報（画像情報）によって補正する試みがなされている（例えば、特許文献１，特許文献２）。特許文献２で開示されている方法では、撮像画像中に指標が検出された場合にはその情報に基づいてセンサ計測値の誤差が補正され、指標が検出されない場合には６自由位置姿勢センサの計測値がそのまま撮像装置の位置及び姿勢として使用される。指標の検出の有無に関わらずに撮像装置の位置及び姿勢が得られるため、複合現実感の提示を安定して行うことができる。
【０００６】
また、特許文献２の方法では、検出された指標の数が３点以上の場合には、撮像装置の位置及び姿勢の６自由度が画像情報に基づいて算出され、検出された指標の数が２点及び１点の場合には、センサによって計測された撮像装置の位置あるいは姿勢のいずれか（２または３自由度）のみを補正する処理が適用される。すなわち、検出された指標の数を判定基準として、撮像装置の位置及び姿勢の算出に利用するアルゴリズムを切り替えることが行われている。これにより、画像情報のみからでは撮像装置の位置及び姿勢を求められない場合（撮像された指標の数が３点に満たない場合）であっても、センサ計測値を基準としながら、その誤差を極力打ち消すような補正のなされた位置及び姿勢を取得することができる。
【０００７】
一方、特許文献１の方法では、検出された指標の数に関わらず、センサによって計測された撮像装置の位置あるいは姿勢のいずれかのみを画像情報に基づいて補正する処理が適用される。この補正方法では、姿勢を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような回転補正値を個別に求め、それらを平均化することで姿勢計測値に対する補正値が算出される。また、位置を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような平行移動補正値を個別に求め、それらを平均化することで位置計測値に対する補正値が算出される。指標の数に関わらず補正の自由度を２または３に限定するため、情報量が不十分な場合であっても安定した解を得ることができる。
【０００８】
【非特許文献１】加藤ら：”マーカー追跡に基づく拡張現実感システムとそのキャリブレーション”,日本バーチャルリアリティ学会論文誌, 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等によって構成される。なお、位置姿勢センサ１４０によって計測される位置及び姿勢は、磁場の歪み等の影響によって誤差を含んだものとなっている。ただし、計測範囲内にあっては連続的に安定して位置及び姿勢を計測することが可能である。
【００２３】
画像入力部１６０は、位置姿勢装置１００へ入力される撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。
【００２４】
センサ計測値入力部１５０は、位置姿勢センサ１４０（１４０ａ，１４０ｂ）から夫々の計測値を入力し、位置姿勢算出部１２０へと出力する。
【００２５】
指標検出部１１０は、画像入力部１６０より撮影画像を入力し、入力した画像中に撮影されている指標Ｑｋの画像座標を検出する。例えば、指標Ｑｋの各々が異なる色を有するマーカによって構成されている場合には、撮影画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標Ｑｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを撮影画像上に施すことにより、指標の位置を検出する。また、四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、４つの直線によって形成されている領域を指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の識別子を取得する。なお、このようにして検出される四角形指標は、本明細書では、４つの頂点の個々によって形成される４つの指標であると考える。
【００２６】
指標検出部１１０は、さらに、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。
【００２７】
位置姿勢算出部１２０は、センサ計測値入力部１５０の出力である撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、計測対象物体１７０あるいは撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【００２８】
なお、図１に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。
【００２９】
図２は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々の機能を、ソフトウェアを実行することで実現するためのコンピュータの基本構成を示す図である。
【００３０】
ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々のソフトウェアの実行を制御して、各部の機能を実現する。
【００３１】
ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備えると共に、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。
【００３２】
ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データなどが格納されている。キーボード１００４、マウス１００５は入力デバイスであり、操作者はこれらを用いて、各種の指示をＣＰＵ１００１に入力することができる。
【００３３】
表示部１００６は、ＣＲＴや液晶ディスプレイなどにより構成されており、例えば、位置姿勢計測のために表示すべきメッセージ等を表示することができる。
【００３４】
外部記憶装置１００７は、ハードディスクドライブなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やＣＰＵ１００１が実行するプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。
【００３５】
記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出して、ＲＡＭ１００２や外部記憶装置１００７に出力する。
【００３６】
Ｉ／Ｆ１００９は、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、位置姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポート、また、算出した位置及び姿勢を外部へ出力するためのイーサネット(登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０及びセンサ計測値入力部１５０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
上述した各構成要素は、バス１０１０によって相互に接続される。
【００３７】
図３は、位置姿勢算出部１２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部１２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。
【００３８】
ステップＳ３０００において、位置姿勢算出部１２０は、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを指標抽出部１１０から入力する。なお、各指標の物体座標系における３次元座標ｘＯＱｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。
【００３９】
ステップＳ３００５において、位置姿勢算出部１２０は、撮像装置１３０及び計測対象物体１７０の位置及び姿勢のセンサによる計測値をセンサ計測値入力部１５０から入力する。
【００４０】
ステップＳ３０１０において、位置姿勢算出部１２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ３０８０に処理を進め、それ以外の場合はステップＳ３０２０に処理を進める。
【００４１】
ステップＳ３０２０において、位置姿勢算出部１２０は、検出された指標の総数が一つか否かを判定する。指標の総数が一つの場合はステップＳ３０２５に処理を進め、それ以外の場合はステップＳ３０３０に処理を進める。
【００４２】
ステップＳ３０２５において、位置姿勢算出部１２０は、ステップＳ３００５で得た撮像装置１３０の姿勢の計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（物体座標系における指標の３次元座標ｘＯＱｋｎと撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値から導出する投影座標の理論値ｕＱｋｎ＊と、実際の検出座標ｕＱｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。ステップＳ３０２５における補正処理としては、周知の処理を適用することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。
【００４３】
ステップＳ３０３０において、位置姿勢算出部１２０は、検出された指標の総数が二つか否かを判定する。指標の総数が二つの場合はステップＳ３０３３に処理を進め、それ以外の場合はステップＳ３０４０に処理を進める。
【００４４】
ステップＳ３０３３において、位置姿勢算出部１２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ１（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ１以上の場合にはステップＳ３０３５に処理を進め、それ以外の場合はステップＳ３０２５に処理を進め、上述した処理を行う。
【００４５】
ステップＳ３０３５において、位置姿勢算出部１２０は、ステップＳ３００５で得た計測対象物体１７０の位置及び姿勢の計測値と、撮像装置１３０の位置の計測値を固定値として、撮像装置１３０の姿勢の計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この処理工程の詳細は後述する。
【００４６】
ステップＳ３０４０は、検出された指標の総数が三つ以上の場合のみ実行される。ここで、位置姿勢算出部１２０は、検出された全指標の分布範囲に関する評価値の一例として、検出された全指標の画像座標を包含する凸包を算出する。画像上の点群に対する凸包の算出方法は画像処理の基本的事項として広く公知のものであるので、その詳細な説明は省略する。
【００４７】
図６，図７，及び図８は、撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示している。図６に示す撮影画像６００上では指標Ｑ１，Ｑ５，Ｑ６，Ｑ９が、図７の撮影画像７００上では指標Ｑ１，Ｑ２，Ｑ６，Ｑ７が，図８の撮影画像８００上では指標Ｑ２，Ｑ６，Ｑ７が観測されており、それ以外の指標は、他の物体（本実施形態においてはユーザの手６１０）によって隠蔽されているものとする。
【００４８】
これらの撮影画像に対して指標の検出を行い、図３のフローチャートで示した位置姿勢算出部１２０の処理ステップを実行すると、各画像において検出される指標の数は３以上であるので、ステップＳ３０４０以降の処理が実行されることになる。図９，図１０及び図１１は、それぞれ撮影画像６００，７００，８００に対してステップＳ３０４０の処理を実行した際に得られる、検出指標の成す凸包（９００，１０００，１１００）を示している。なお、以下では、各凸包の面積をそれぞれＡ９００，Ａ１０００，及びＡ１１００と表記したときに、以下に述べる閾値Ｔ２及び閾値Ｔ３に対して、Ａ１１００＜Ｔ２＜Ａ１０００＜Ｔ３＜Ａ９００という関係が満たされているものとする。
【００４９】
ステップＳ３０５０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ２（例えば、撮影画像全体の面積の１／１６）との比較を行う。面積が閾値Ｔ２以上の場合はステップＳ３０６０に処理を進め、それ以外の場合はステップＳ３０２５へと処理を進める。図６〜図１１の例では、撮影画像８００についてはステップＳ３０２５へ、撮影画像７００及び６００についてはステップＳ３０６０へと処理が進行する。
【００５０】
ステップＳ３０６０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ３（例えば、撮影画像全体の面積の１／９）との比較を行う。面積が閾値Ｔ３以上の場合はステップＳ３０７０に処理を進め、それ以外の場合はステップＳ３０３５へと処理を進める。図６〜図１１の例では、撮影画像７００についてはステップＳ３０３５へ、撮影画像６００についてはステップＳ３０７０へと処理が進行する。
【００５１】
ステップＳ３０７０において、位置姿勢算出部１２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程の詳細は後述する。
【００５２】
ステップＳ３０８０において、位置姿勢算出部１２０は、ステップＳ３０２５、Ｓ３０３５及びＳ３０７０で得られた撮像装置１３０及び計測対象物体１７０の位置及び姿勢を表すデータを、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【００５３】
なお、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の各工程では、撮像装置１３０に関する計測値のみに補正を加えているので、ステップＳ３０８０で出力される位置及び姿勢のデータは、計測対象物体１７０に関しては、ステップＳ３００５で入力したセンサによる計測値そのものとなる。しかし、位置及び姿勢の出力形態はこれに限られるものではなく、逆に、撮像装置１３０の位置及び姿勢の計測値には補正を加えずにセンサによる計測値をそのまま出力し、計測対象物体１７０の位置及び姿勢の計測値に補正を加えて出力しても良い。この場合は、ステップＳ３０８０において、以下に述べる座標変換処理を行った後に、変換後の位置及び姿勢を出力する。
【００５４】
同次座標系を用いた４×４の座標変換行列によって位置及び姿勢を表記すると、計測対象物体１７０の補正後の位置及び姿勢MWOは、センサ計測値として得られた撮像装置１３０の位置及び姿勢M#WC（以下、# はセンサによる計測値を表す記号）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢M#WOと、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢MWCに基づいて、次式によって算出される。
【００５５】
【数１】
【００５６】
このとき、撮像装置１３０によって定義されるカメラ座標系における計測対象物体１７０の位置及び姿勢は、
【００５７】
【数２】
【００５８】
となる。すなわち、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢（MWC）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢（M#WO）とを出力する場合と、センサ計測値として得られた撮像装置１３０の位置及び姿勢（M#WC）と、式１によって求められる計測対象物体１７０の補正後の位置及び姿勢（MWO）とを出力する場合では、計測対象物体１７０と撮像装置１３０の相対的な位置及び姿勢の関係は等価となる。したがって、本装置の出力を利用するアプリケーションが要求する形態に合わせて、出力形態を選択できるようになっていることが好ましい。
【００５９】
また、カメラ座標系における計測対象物体１７０の位置及び姿勢（MCO）を式２から算出してこれを出力しても良いし、物体座標系における撮像装置１３０の位置及び姿勢（MCO-1）を求めて、これを出力してもよい。
【００６０】
ステップＳ３０９０において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、ステップＳ３０００へと処理を進める。
以上によって、撮像装置の位置及び姿勢が計測される。
【００６１】
次に、図４のフローチャートを用いて、上記ステップＳ３０３５における撮像装置１３０の姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の姿勢を、３値ベクトル
s = ωWC = [ξWC ψWC ζWC]T
によって内部的に表現している。
【００６２】
姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。なお、姿勢ωは、次式、
【００６３】
【数３】
【００６４】
によって、３×３の回転行列Rによっても表現可能であり、ωとRとは、互いに一意に変換することができる。Rからωへの変換方法は公知であるので、その詳細な説明は省略する。
【００６５】
ステップＳ４０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の姿勢を、sの初期値とする。
【００６６】
ステップＳ４０１０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、指標Ｑｋｎ各々の物体座標系における座標ｘＯＱｋｎ（既知な情報として予め保持している）から画像座標を算出する関数：
【００６７】
【数４】
【００６８】
に基づいて行われる。具体的には、観測方程式Ｆｃ（）は、ｘＯＱｋｎとsから当該指標のカメラ座標（撮像装置１３０の視点位置を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘＣＱｋｎを求める次式、
【００６９】
【数５】
【００７０】
及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める次式、
【００７１】
【数６】
【００７２】
によって構成されている。ここでtWC及びtWOは、撮像装置１３０と計測対象物体１７０の夫々の位置計測値（平行移動成分）を表す３次元ベクトルであり、ここでは固定値として取り扱われる。また、RWOは計測対象物体１７０の姿勢計測値（回転成分）を表す３×３の回転行列であり、同様に固定値として取り扱われる。また、ｆＣｘ及びｆＣｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。
【００７３】
ステップＳ４０２０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差ΔｕＱｋｎを、次式によって算出する。
【００７４】
【数７】
【００７５】
ステップＳ４０３０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、sに関する画像ヤコビアン（すなわち、式４の観測方程式Ｆｃ（）をsの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列）ＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＱｋｎを算出する。
【００７６】
【数８】
【００７７】
ステップＳ４０４０において、位置姿勢算出部１２０は、ステップＳ４０２０及びステップＳ４０３０において算出した誤差ΔｕＱｋｎ及び画像ヤコビアンＪｕｓＱｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差ΔｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトル
【００７８】
【数９】
【００７９】
及び、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた２Ｎ行×３列の行列
【００８０】
【数１０】
【００８１】
を作成し、Θの擬似逆行列Θ+を用いて、
【００８２】
【数１１】
【００８３】
として算出する。このように、本実施形態では補正値Δsを、局所的な線形近似によって算出している。
【００８４】
ステップＳ４０５０において、位置姿勢算出部１２０は、ステップＳ４０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。
【００８５】
【数１２】
【００８６】
ステップＳ４０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のsを用いて、再度ステップＳ４０１０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ４０７０へと処理を進める。
以上のステップＳ４０００からＳ４０６０までの工程は、複数の観測方程式からなるsに関する非線形連立方程式をニュートン法によって求解する工程である。この一連の工程は、後述する変形例５に示すとおり、他のいかなる非線形解法を用いても良い。
【００８７】
ステップＳ４０７０において、位置姿勢算出部１２０は、ステップＳ４０６０までの処理によって得られたsを、撮像装置１３０の姿勢の（補正後の）推定値 ωWCとする。
【００８８】
以上によって、センサ計測値として得られた位置パラメータを固定して、全指標上での誤差の和を最小化するような姿勢パラメータを求めることができる。
【００８９】
次に、図５のフローチャートを用いて、上記ステップＳ３０７０における撮像装置１３０の位置及び姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の位置及び姿勢を、６値ベクトルs = [tWC T ωWC T]T = [xWC yWC zWC ξWC ψWC ζWC]Tによって内部的に表現している。
【００９０】
ステップＳ５０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の位置及び姿勢を、sの初期値とする。
【００９１】
ステップＳ５０１０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、各指標Ｑｋｎの物体座標系における座標ｘＯＱｋｎから画像座標を算出する関数：
【００９２】
【数１３】
【００９３】
に基づいて行われる。具体的には、観測方程式Ｆ’ｃ（）は、ｘＯＱｋｎとsから当該指標のカメラ座標ｘＣＱｋｎを求める式５、及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める式６によって構成されている。ただしここでは、式５におけるtWC（撮像装置１３０の位置を表す）を、固定値としてではなく、sを構成するパラメータの一部として取り扱う。一方、計測対象物体１７０の位置tWO及び姿勢RWOに関しては、センサによる計測値をそのまま固定値として使用する。
【００９４】
ステップＳ５０２０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差ΔｕＱｋｎを、式７によって算出する。
【００９５】
ステップＳ５０３０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、sに関する画像ヤコビアン（すなわち、式１３の観測方程式Ｆ’ｃ（）をsの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）ＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、式８によってＪｕｓＱｋｎを算出する。
【００９６】
ステップＳ５０４０において、位置姿勢算出部１２０は、ステップＳ５０２０及びステップＳ５０３０において算出した誤差ΔｕＱｋｎ及び画像ヤコビアンＪｕｓＱｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差ΔｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトルＵ及び、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた２Ｎ行×６列の行列Θを作成し、Θの擬似逆行列Θ+を用いて、式１１によって算出する。
【００９７】
ステップＳ５０５０において、位置姿勢算出部１２０は、ステップＳ５０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。
【００９８】
ステップＳ５０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後のsを用いて、再度ステップＳ５０１０以降の処理を行う。一方、収束していると判定された場合には、ステップＳ５０７０へと処理を進める。
【００９９】
ステップＳ５０７０において、位置姿勢算出部１２０は、ステップＳ５０６０までの処理によって得られたsを、撮像装置１３０の補正後の位置及び姿勢の推定値とする。
【０１００】
なお、本実施形態では、ステップＳ３０７０の処理において、撮像装置１３０の位置及び姿勢を補正の対象としていたが、計測対象物体１７０の位置及び姿勢を補正の対象として処理を行っても良い。この場合は、算出すべき未知パラメータを、計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [tWO T ωWO T]T = [xWO yWO zWO ξWO ψWO ζWO]Tによって内部的に表現し、センサ計測値として得られた計測対象物体１７０の位置及び姿勢をsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１０１】
【数１４】
【０１０２】
このとき、撮像装置１３０の位置tWC及び姿勢RWCに関しては、センサによる計測値をそのまま固定値として使用する。
【０１０３】
また、カメラ座標系における計測対象物体１７０の位置及び姿勢や、物体座標系における撮像装置１３０の位置及び姿勢を未知のパラメータとして求めても良い。
【０１０４】
前者の場合、算出すべき未知パラメータを、カメラ座標系における計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [tCO T ωCO T]T = [xCO yCO zCO ξCO ψCO ζCO]Tによって内部的に表現する。また、カメラ座標系における計測対象物体１７０の位置及び姿勢（M#CO）を、次式、
【０１０５】
【数１５】
【０１０６】
によってセンサ計測値（M#WC及びM#WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１０７】
【数１６】
【０１０８】
一方、後者の場合は、算出すべき未知パラメータを、物体座標系における撮像装置１３０の位置及び姿勢を表す６値ベクトルs = [tOCT ωOC T]T = [xOC yOC zOC ξOC ψOC ζOC]Tによって内部的に表現する。また、物体座標系における撮像装置１３０の位置及び姿勢（M#CO）を、次式、
【０１０９】
【数１７】
【０１１０】
によってセンサ計測値（M#WC及びM#WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１１１】
【数１８】
【０１１２】
以上のようにして、計測対象物体１７０と撮像装置１３０との間の相対的な位置及び姿勢の関係を未知のパラメータとして、センサによる位置及び姿勢計測値の誤差を補正することができる。
【０１１３】
従来の位置姿勢計測装置では、指標の分布を考慮していないため、画像上の一部の領域に指標が偏在して観測されている場合に解が不安定になる。一方、本実施形態に係る位置姿勢計測装置によれば、ステップＳ３０３３や、ステップＳ３０４０，Ｓ３０５０，Ｓ３０６０によって指標の分布範囲の大きさを考慮した手法の選択が行われるため、例えば検出された指標の数が多くても、それらが画像上の小領域に偏在しているような場合に、位置及び姿勢の６自由度を安定して求めるのに不十分であるという判定を行うことができる。そのため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、不安定な解が得られる状況に陥る可能性が低下し、従来手法と比して安定した解を得ることができる。
【０１１４】
また、本実施形態に係る位置姿勢計測装置によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行う場合に、その制約の中で指標上での誤差の和を最小化するような補正を行うことができる。したがって、未知とするパラメータを減らすことによって撮像装置の位置及び姿勢の計測を安定化させようとする場合であっても、従来と比べて高精度な計測を行うことができる。
【０１１５】
［第２の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等に設定された基準座標系における任意の計測対象物体の位置及び姿勢の計測を行う。
【０１１６】
本実施形態に係る位置姿勢計測装置は、撮像装置に位置姿勢センサが取り付けられておらず、三脚等によって既知の位置及び姿勢に固定して設置されていることが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。
【０１１７】
図１２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１２００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。
【０１１８】
撮像装置１３０は、三脚１２８０によって、空間中の既知の位置、姿勢に固定して設置されている。撮像装置１３０が出力する撮影画像は、位置姿勢計測装置１２００の画像入力部１６０に入力される。基準座標系における撮像装置１３０の位置及び姿勢は、既知の値として予め保持されているものとする。
【０１１９】
センサ計測値入力部１５０は、計測対象物体１７０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１２２０へと出力する。
【０１２０】
位置姿勢算出部１２２０は、センサ計測値入力部１５０の出力である計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、計測対象物体１７０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【０１２１】
なお、図１２に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１２２】
本実施形態における位置姿勢算出部１２２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）とほぼ同一である。唯一の違いは、撮像装置１３０の位置及び姿勢が、計測値として与えられるのではなく、既知の値として予め保持されていることである。位置姿勢算出部１２２０は、撮像装置１３０の位置及び姿勢の既知の値を、撮像装置１３０の位置及び姿勢の仮の計測値とおき、第１の実施形態におけるステップＳ３０００からステップＳ３０７０までと同一の工程によって撮像装置１３０の位置及び姿勢の計測値の補正を行い、この補正結果に基づいて、ステップＳ３０８０において、第１の実施形態と同様に計測対象物体１７０の位置及び姿勢の計測値を補正する（式１）。本実施形態では、位置姿勢算出部１２２０がステップＳ３０８０において出力するのは、計測対象物体１７０の位置及び姿勢のみである。
【０１２３】
以上によって、計測対象物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１２４】
なお、既知の位置に固定された撮像装置によって任意の計測対象物体の位置及び姿勢を計測する方法は、計測対象物体に取り付けるセンサを姿勢センサに変更しても実現できる。例えば、検出された指標の分布が閾値未満の場合は、姿勢センサの姿勢計測値を固定値として用いて、位置のみを指標の情報から推定する。また、指標の分布が閾値以上の場合には、位置及び姿勢の６パラメータを指標の情報から推定する。この場合の撮像装置は１台に限定されるものではなく、複数の撮像装置を用いることが可能である。
【０１２５】
［第３の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。
【０１２６】
本実施形態においては撮像装置以外の計測対象物体は存在せず、本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢の計測のみを行うことが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。
【０１２７】
図１３は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１３００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。
【０１２８】
また、現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標として、基準座標系における位置が既知である複数個の指標Ｑｋが配置されている。指標Ｑｋは、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、どのような形態であってもよい。
【０１２９】
センサ計測値入力部１５０は、撮像装置１３０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１３２０へと出力する。
【０１３０】
位置姿勢算出部１３２０は、センサ計測値入力部１５０の出力である撮像装置１３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【０１３１】
なお、図１３に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１３２】
本実施形態における位置姿勢算出部１３２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）と類似しているため、図３を参照しながら、第１の実施形態との相違点のみについて説明する。
【０１３３】
ステップＳ３００５において、位置姿勢算出部１３２０は、センサによって計測された撮像装置１３０の位置及び姿勢を、センサ計測値入力部１５０から入力する。
【０１３４】
ステップＳ３０２５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の姿勢計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（基準座標系における指標の３次元座標ｘＷＱｋｎと撮像装置１３０の位置及び姿勢の計測値から導出する投影座標の理論値ｕＱｋｎ＊と、実際の検出座標ｕＱｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。
【０１３５】
このような補正処理としては、周知の方法を用いることができるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。
【０１３６】
ステップＳ３０３５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の位置計測値を固定値として、撮像装置１３０の姿勢計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この補正処理工程は、第１の実施形態におけるステップＳ３０３５の補正処理工程（図４のフローチャートに対応）と類似したものとなる。ただし、本実施形態の場合、指標の観測方程式Ｆ’’ｃ（）が、基準座標系における指標Ｑｋｎの座標ｘＷＱｋｎ（既知な情報として予め保持している）から画像座標を算出する関数となる。すなわち、観測方程式Ｆ’’ｃ（）は次式によって表される。
【０１３７】
【数１９】
【０１３８】
具体的には、観測方程式Ｆ’’ｃ（）は、ｘＷＱｋｎとsから当該指標のカメラ座標ｘＣＱｋｎを求める次式、
【０１３９】
【数２０】
【０１４０】
及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める式６によって構成されており、この点のみが異なっている。ここでtWCは、撮像装置１３０の位置計測値を表す３次元ベクトルであり、ここでは固定値として取り扱われる。
【０１４１】
ステップＳ３０７０において、位置姿勢算出部１３２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程での具体的な処理は、例えば特許文献２に開示されているような周知技術を用いて実現することが可能であるので、ここでの説明は省略する。
【０１４２】
ステップＳ３０８０において、位置姿勢算出部１３２０は、ステップＳ３０７０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【０１４３】
以上によって、撮像装置の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１４４】
［第４の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における任意の計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。本実施形態においては、撮像装置ではなく、任意の計測対象物体の位置及び姿勢を計測することが、第３の実施形態と異なっている。以下では、第３の実施形態との相違点のみを説明する。
【０１４５】
図１４は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１４００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、撮像部１４３０、位置姿勢計測部１４４０、及び位置姿勢算出部１４２０によって構成されており、計測対象物体１４７０に接続されている。
【０１４６】
撮像部１４３０は、位置及び姿勢を計測したい対象である計測対象物体１４７０に固定して取り付けられており、指標を含む現実空間の映像を撮影する。
【０１４７】
位置姿勢計測部１４４０は、撮像部１４３０に固定して装着されており、基準座標系における撮像部１４３０の位置及び姿勢を計測してセンサ計測値入力部１５０へと出力する。位置姿勢計測部１４４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。
【０１４８】
画像入力部１６０は、撮像部１４３０が撮影した撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。
【０１４９】
センサ計測値入力部１５０は、撮像部１４３０の位置及び姿勢の計測値を位置姿勢計測部１４４０から入力し、位置姿勢算出部１４２０へと出力する。
【０１５０】
位置姿勢算出部１４２０は、センサ計測値入力部１５０の出力である撮像部１４３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、撮像部１４３０の位置及び姿勢の計測値の誤差を補正する。
位置姿勢算出部１４２０はさらに、得られた撮像部１４３０の位置及び姿勢と、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係についての情報（具体的には、計測対象物体１４７０上のどの位置に撮像部１４３０が設置されているかという情報。計測対象物体１４７０によって定義される物体座標系における、撮像部１４３０の位置及び姿勢によって表現される）から、基準座標系における計測対象物体１４７０の位置及び姿勢を算出し、Ｉ／Ｆ１００９を介してこのデータを外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。なお、この変換工程は必ずしも必要ではなく、撮像部１４３０の位置及び姿勢をそのまま出力することも可能である。
【０１５１】
なお、本実施形態では、位置姿勢計測部１４４０は撮像部１４３０の位置及び姿勢を計測していたが、位置姿勢計測部１４４０は計測対象物体１４７０の位置及び姿勢を計測するように構成されていても良い。この場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた計測対象物体１４７０の位置及び姿勢から撮像部１４３０の位置及び姿勢を算出し、これを初期値として上記と同様な処理を実行する。
【０１５２】
また、位置姿勢算出部１４２０は、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の何れの工程においても、撮像部１４３０の姿勢あるいは位置及び姿勢を補正し、その後計測対象物体１４７０の位置及び姿勢を求めていた。しかし、ステップＳ３０３５及びＳ３０７０の工程においては、計測対象物体１４７０の位置及び姿勢を直接求めるような構成としても良い。
【０１５３】
位置姿勢計測部１４４０が撮像部１４３０の位置及び姿勢を計測している場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた撮像部１４３０の位置及び姿勢から計測対象物体１４７０の位置及び姿勢を算出し、これを初期値とする。
【０１５４】
ステップＳ３０３５においては、計測対象物体１４７０の姿勢を表す３値ベクトルs = ωWO = [ξWO ψWO ζWO]Tを未知パラメータとする。また、ステップＳ３０７０においては、計測対象物体１４７０の位置及び姿勢を表す６値ベクトルs = [tWOT ωWOT]T = [xWO yWO zWO ξWO ψWO ζWO]Tを未知パラメータとする。そして、観測方程式Ｆ’’ｃ（）を定義するのに必要な式２０を、
【０１５５】
【数２１】
【０１５６】
に変更する。ここで、tOC 及びROCは、計測対象物体１７０によって定義される物体座標系における撮像部１４３０の姿勢を表す３×３の回転行列及び位置を表す３次元ベクトルであり、撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を表す既知の値として、予め保持されているものとする。なお、ステップＳ３０３５では、tWOは固定値として設定する。
【０１５７】
以上によって、任意の物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１５８】
［第５の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。
【０１５９】
図１５は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１５００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５５０、及び位置姿勢算出部１５２０によって構成されており、撮像装置１３０及び姿勢センサ１５４０に接続されている。
【０１６０】
本実施形態においては、撮像装置１３０に、位置姿勢センサではなく姿勢センサが装着されている点が、第３の実施形態と異なっている。姿勢センサを用いることで、位置姿勢センサを用いる場合に存在する、センサの計測範囲以外の領域を計測できないという制約を回避することができる。以下では、第３の実施形態との相違点のみを説明する。
【０１６１】
姿勢センサ１５４０は、撮像装置１３０に装着されており、姿勢センサ１５４０自身の現在の姿勢を計測してセンサ計測値入力部１５５０へと出力する。姿勢センサ１５４０は、例えばジャイロセンサと加速度センサをベースとしたセンサユニットであり、具体的には、株式会社トキメックのTISS-5-40や、米国InterSense社のInertiaCube2等によって構成される。これらのセンサによって計測される姿勢計測値は、慣性計測値を時間積分した値として計測されたものであるため、真の姿勢とは異なった誤差を有する。ただし、これらの姿勢センサは、内蔵する加速度センサによって地球の重力方向を観測することによって、傾斜角方向（ピッチ角及びロール角）のドリフト誤差の蓄積をキャンセルする機能を有している。そのため、傾斜角方向に関してはドリフト誤差が発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては時間の経過に伴って蓄積されるドリフト誤差を有している。
【０１６２】
センサ計測値入力部１５５０は、方位ドリフト誤差補正値の更新値を位置姿勢算出部１５２０から入力し、姿勢センサ１５４０の現在の方位ドリフト誤差補正値を更新し保持する。また、姿勢センサ１５４０から姿勢計測値を入力し、現在の方位ドリフト誤差補正値によってこれを補正して、撮像装置１３０の姿勢の予測値として位置姿勢算出部１５２０へと出力する。
【０１６３】
位置姿勢算出部１５２０は、センサ計測値入力部１５５０の出力である撮像装置１３０の姿勢の予測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力データとして、撮像装置１３０の位置及び姿勢を算出して出力する。また、位置及び姿勢の算出工程で導出される姿勢センサ１５４０の方位ドリフト誤差補正値の更新値を、センサ計測値入力部１５５０へと出力する。
【０１６４】
なお、図１５に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１６５】
図１６は、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部１５２０のソフトウェアのプログラムを実行することで行われる。
【０１６６】
ステップＳ１５０００において、位置姿勢算出部１５２０は、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを指標抽出部１１０から入力する。なお、各指標の基準座標系における３次元座標ｘＱｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。
【０１６７】
ステップＳ１５００５において、位置姿勢算出部１５２０は、撮像装置１３０の姿勢の予測値R*をセンサ計測値入力部１５５０から入力し、これを、撮像装置１３０の姿勢の初期値として設定する。また、２回目以降の処理では、１ループ前の処理において算出された撮像装置１３０の位置を、撮像装置１３０の姿勢の初期値として設定する。
【０１６８】
ステップＳ１５０１０において、位置姿勢算出部１５２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ１５０９０に処理を進め、それ以外の場合はステップＳ１５０２０に処理を進める。
【０１６９】
ステップＳ１５０２０において、位置姿勢算出部１５２０は、検出された指標の総数が一つか否かを判定する。指標の総数が一つの場合はステップＳ１５０２５に処理を進め、それ以外の場合はステップＳ１５０３０に処理を進める。
【０１７０】
ステップＳ１５０２５において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値の平行移動成分のうちの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加え、ステップＳ１５０９０に処理を進める。ステップＳ１５０２５での処理工程としては、周知の処理を提供することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの平行移動による補正方法（段落「００３０」〜「００３６」）およびランドマークを複数点利用したカメラの平行移動による補正方法（段落「００５１」〜「００５６」）に開示されている方法を用いることができる。ただし、特許文献１では６自由度位置姿勢センサの計測値を誤差補正の対象とした場合を説明しているが、本実施形態では、前フレームで求めた位置と姿勢センサの計測値とを組み合わせたものが補正対象となる点が異なっている。
【０１７１】
ステップＳ１５０３０において、位置姿勢算出部１５２０は、検出された指標の総数が二つか否かを判定する。指標の総数が二つの場合はステップＳ１５０３２に処理を進め、それ以外の場合はステップＳ１５０４０に処理を進める。
【０１７２】
ステップＳ１５０３２において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ１（例えば、画像の対角線長の１／８）との比較を行う。距離が閾値Ｔ１以上の場合にはステップＳ１５０３４に処理を進め、それ以外の場合はステップＳ１５０２５に処理を進める。
【０１７３】
ステップＳ１５０３４において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離と所定の値として定められている閾値Ｔ２（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ２以上の場合にはステップＳ１５０３８に処理を進め、それ以外の場合はステップＳ１５０３６に処理を進める。
【０１７４】
ステップＳ１５０３６において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうち、位置を表す３パラメータ（平行移動成分）を更新対象として（すなわち、姿勢計測値には誤差は含まれていないものと仮定して）、検出されている指標上での誤差を最小とするような補正を位置のパラメータのみに加え、ステップＳ１５０９０に処理を進める。この処理工程の詳細は、例えば特許文献３（第６の実施形態）に開示されているので、ここでの説明は省略する。また、位置を表す３パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置の補正を行うことができる。
【０１７５】
ステップＳ１５０３８において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうちの位置を表す３パラメータ（平行移動成分）と、姿勢センサ１３４０の方位ドリフト誤差補正値の更新値φの合計４パラメータとを、算出すべき未知パラメータとして取り扱い、検出されている指標の情報を用いてこれらを算出する。この処理工程の詳細は、例えば特許文献３（第７の実施形態）に開示されているので、ここでの説明は省略する。また、これら４パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置と方位ドリフト誤差補正値の更新値の算出を行うことができる。さらに、ステップＳ１５０８０へと処理を進める。
【０１７６】
ステップＳ１５０４０は、検出された指標の総数が三つ以上の場合のみ実行される。ここで、位置姿勢算出部１５２０は、検出された全指標の分布範囲に関する評価値の一例として、検出された全指標の画像座標を包含する凸包を算出する。
【０１７７】
ステップＳ１５０５０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ３（例えば、画像の面積の１／１６）との比較を行う。面積が閾値Ｔ３以上の場合はステップＳ１５０６０に処理を進め、それ以外の場合はステップＳ１５０３６へと処理を進め、上述した処理を行う。
【０１７８】
ステップＳ１５０６０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ４との比較を行う。面積が閾値Ｔ４（例えば、画像の面積の１／９）以上の場合はステップＳ１５０７０に処理を進め、それ以外の場合はステップＳ１５０３８へと処理を進め、上述した処理を行う。
【０１７９】
ステップＳ１５０７０において、位置姿勢算出部１５２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この算出は、例えば、撮像装置１３０の位置と姿勢を表す６パラメータを変数として、ステップＳ１５００５で設定した初期値を用いて、非線形方程式の反復解法によって誤差を最小化するパラメータを求めることによって実現される。すなわち、位置と姿勢を表す６パラメータを未知変数とした観測方程式を構成し、これに基づいて求められる指標投影位置の誤差と各パラメータに関する画像ヤコビアンとを用いて当該パラメータの補正値を求め、補正処理を反復的に行うことで画像上での誤差を最小とするような位置と姿勢を算出する。
位置姿勢算出部１５２０はさらに、ステップＳ１５００５で設定した姿勢の初期値と求められた姿勢との変化分を算出し、その方位成分を方位ドリフト誤差補正値の更新値φとする。
【０１８０】
ステップＳ１５０８０において、位置姿勢算出部１５２０は、ステップＳ１５０３８あるいはステップＳ１５０７０において得られた方位ドリフト誤差補正値の更新値φを、センサ計測値入力部処理１５５０へと出力する。
【０１８１】
ステップＳ１５０９０において、位置姿勢算出部１５２０は、ステップＳ１５０８０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【０１８２】
ステップＳ１５１００において、位置姿勢算出部１５２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ１５０００へと進める。
【０１８３】
以上によって、撮像装置の位置及び姿勢が計測される。
本実施形態に係る位置姿勢計測装置によれば、ステップＳ１５０３４やステップＳ１５０５０によって指標の分布範囲の大きさを考慮した手法の選択（方位ドリフト誤差補正値の更新を行うか否かの判断）が行われるため、画像上の一部の領域に指標が偏在して観測されている場合に、方位ドリフト誤差補正値の更新を不正確に行ってしまうという状況を回避することができる。
【０１８４】
また、本実施形態に係る位置姿勢計測装置によれば、第３の実施形態の効果に加え、ステップＳ１５０６０による手法の選択によって、画像情報が不足する場合であっても姿勢センサによる傾斜角情報を信頼することで位置及び姿勢を安定して導出できるほか、画像情報が十分得られる場合であれば位置及び姿勢の推定を高精度に行えるという、通常の画像情報に基づく位置姿勢推定手法が有する利点を両立することができる。
【０１８５】
［第１乃至第５の実施形態の変形例］
（変形例１）
上記の実施形態の各々では、画像上における指標の分布に基づいて、適用する補正アルゴリズムが選択される。これらの補正アルゴリズム（第１の実施形態におけるステップＳ３０２５，Ｓ３０３５，Ｓ３０７０、第２乃至第４の実施形態におけるこれらの変形、及び、第５の実施形態におけるステップＳ１５０２５，Ｓ１５０３６，Ｓ１５０３８，Ｓ１５０７０）は、必ずしも上記の実施形態で示したものである必要はなく、指標の分布に従って見込まれる画像情報の情報量に応じて適切に設定されたものであれば、他の補正アルゴリズムに変更することも可能である。
【０１８６】
例えば、第１乃至第４の実施形態におけるステップＳ３０２５やＳ３０３５において、姿勢の計測値を補正するのではなく、位置の計測値を補正する構成としてもよい。この場合、ステップＳ３０２５では、例えば、特許文献１の段落「００３０」〜「００３６」および「００５１」〜「００５６」に開示されているような、１つ又は複数の指標を利用したカメラの平行移動による補正方法に基づいて、位置計測値のうちのカメラの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加えることができる。
【０１８７】
また、ステップＳ３０３５では、算出すべき未知パラメータを、撮像装置１３０の位置を表す３値ベクトルs = tWC = [xWC yWC zWC ]Tとして、図５と同様な処理手順によって位置の補正を行う。第１の実施形態との相違点は、観測方程式を構成する式５を、次式のように変更することである。
【０１８８】
【数２２】
【０１８９】
ここで、RWCは、固定値として用いる撮像装置１３０のセンサによる姿勢計測値を表している。また、RWO 及びtWOは、第１の実施形態における式５と同様に、計測対象物体１７０の位置及び姿勢のセンサによる計測値を表しており、固定値として取り扱われる。
【０１９０】
なお、式２２は線形方程式であるので、式６と組み合わせた観測方程式から連立方程式を作成しこれを解くことで、繰り返し演算を用いなくても補正値を得ることができる。
【０１９１】
なお、不図示のＵＩを介して、姿勢の補正と位置の補正の何れを優先させるかをユーザが自由に選択できるようにしてもよい。この場合、ステップＳ３０２５及びＳ３０３５の実行時に、ユーザの選択に応じて、姿勢あるいは位置の補正アルゴリズムが実行される。これにより、特性の異なる位置姿勢センサを用いた場合でも、より好ましい計測を行うことができる。
【０１９２】
また、ステップＳ３０２５においては、計測値として得られる６パラメータのうちの任意の２パラメータを補正対象として選択し、これを未知数として上述した方法と記と同様にして求めてもよい。同様に、ステップＳ３０３５においては、計測値として得られる６パラメータのうちの任意の３パラメータを補正対象として選択し、これを未知数として上述した方法と同様な枠組みによって求めてもよい。
【０１９３】
（変形例２）
上記第１乃至第４の実施形態では、画像上における指標の分布の尺度に基づいて、３種類のアルゴリズムへと処理を分岐させていた（ステップＳ３０３３、ステップＳ３０５０及びステップＳ３０６０）。しかし、指標の分布の尺度に基づいて行う分岐数は３通りに限られるものではない。例えば、ステップＳ３０５０の処理を省いて、閾値Ｔ３と凸包の面積の大小関係によってステップＳ３０３５あるいはステップＳ３０７０へと処理を分岐させるだけでもよい。また、より多数の閾値を設けて、４通り以上の分岐を行ってもよい。例えば、補正対象とするパラメータの数（２〜６）を凸包の面積に応じて変更し、これを未知数として同様な方法により位置姿勢を算出してもよい。
【０１９４】
（変形例３）
上記の実施形態の各々では、位置姿勢センサや姿勢センサによる計測値と、撮像装置によって得られる画像情報を併用することにより、計測対象（計測対象物体や撮像装置）の位置及び姿勢を求めていた。しかし、画像上における指標の分布に基づいて、位置及び姿勢を求めるために用いるアルゴリズムを選択するという基本的な技術思想は、これらのセンサによる計測値を用いずに、画像情報のみから計測対象の位置及び姿勢を得る場合であっても適用できる。
【０１９５】
例えば、図１３に示した第３の実施形態の構成において、位置姿勢計測装置１３００がセンサ計測値入力部１５０を有さず、また、位置姿勢センサ１４０が撮像装置１３０に設置されていない場合を考える。この場合、位置姿勢算出部１３２０は、初期値として位置姿勢センサ１４０による撮像装置１３０の位置及び姿勢の計測値を用いるのではなく、前フレームで求めた撮像装置１３０の位置及び姿勢の算出値（あるいは、過去の算出値から予測した現フレームにおける位置及び姿勢）を用いることにすると、これを仮の計測値とおいて図３と同様な処理を行うことで、指標の分布に応じて適切な位置及び姿勢の算出アルゴリズムを選択することが可能となる。
【０１９６】
（変形例４）
上記の実施形態の各々では、指標の分布を計る評価値として、指標が３点以上検出されている場合には、指標が成す凸包を求めてその面積を利用していた。しかし、指標の分布を評価する尺度はこれに限られるものではなく、他の尺度を用いてもよい。例えば、画像中心から最も遠い位置にある指標を第１の指標として選択し、第１の指標から最も遠い位置にある指標を第２の指標として選択し、さらに、第１，第２の指標を結ぶ直線から最も遠い距離にある指標を第３の指標として選択し、これら第１，第２，第３の指標が成す三角形の面積を分布の尺度として、凸包の面積の変わりに用いても良い。また、簡易的には、このような第１，第２の指標間の距離を分布の尺度としても良いし、指標の画像座標の分散や標準偏差といった、分布を数値で表しうる他の任意の情報を分布の尺度として用いてもよい。
【０１９７】
他の指標の画像座標の分散を分布の尺度として用いる具体的な方法として、指標の画像座標の共分散を利用する方法が考えられる。検出されている指標の画像座標の分散共分散行列を求め、その固有値（この値は主成分分析における各主成分の分散に相当する）を用いる方法である。この場合、第２固有値を尺度とすることで、画像上における指標の２次元的広がりを評価することが可能である。また、第１固有値を尺度とすることで、画像の１次元的な広がりを評価することができる。
【０１９８】
また、それ以外にも、凸包の周囲長等の、凸包の面積以外の特徴量を用いてもよい。また、検出された全ての指標に対する外接矩形や外接円などから得られる特徴量を用いてもよい。また、指標の画像座標のモーメントの解析値（例えば、慣性等価楕円の長軸長と短軸長）等を用いてもよい。また、単一の多角形指標を用いる場合には、その指標を検出する際に得られる指標領域の面積をそのまま用いてもよい。
【０１９９】
また、検出された指標の分布を直接計測しなくても、指標の分布の大小を何らかの情報に基づいて推定してもよい。例えば、過去のフレームやセンサの情報から得たカメラや物体の位置及び姿勢の推定値（初期値）に基づいて、画像上で観測されるであろう指標の座標を算出し、計算上の指標の分布を求めてもよい。また、検出された指標の見かけの大きさ等から指標までの距離を近似的に推定して、それに応じて分布を推定してもよい。
【０２００】
（変形例５）
第１の実施形態におけるステップＳ３０３５では、図４のステップＳ４０４０にて詳細に説明したように、誤差ベクトルＵと行列Θに基づく補正値Δ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によって表現する。また、撮像装置２０３０の位置と姿勢を、６値のベクトルｓ= [tT ωT]T = [x y z ξ ψ ζ]Tによって表現する。
【０２７９】
なお、姿勢の情報は必要に応じて、カメラ座標系から基準座標系への回転変換を行う３×３の回転行列Ｒに変換して使用されるものとする。ωとＲは互いに一意に変換することができる。ωからＲへの変換は式３によって可能である。Ｒからωへの変換方法は公知であるので、その詳細な説明は省略する。
【０２８０】
ステップＳ２１０００において、位置姿勢算出部２０２０は、時刻τにおける撮像装置２０３０の姿勢計測値ω*τ( = [ξ*τ ψ*τ ζ*τ]T)を、姿勢センサ２０４０から入力する。
【０２８１】
ステップＳ２１０１０において、位置姿勢算出部２０２０は、時刻τにおける撮影画像から検出された指標Ｑｋｎ（ｎ＝１，２，・・・，Ｎ）の画像座標ｕｎ（＝ｕＣＱｋｎ）と基準座標ｘwＱｋｎの組を、指標検出部２０１０から入力する。
【０２８２】
ステップＳ２１０２０において、位置姿勢算出部２０２０は、入力された検出指標の情報が位置及び姿勢の推定に十分か否かを判定し、それに応じて処理の分岐を行う。具体的には、入力した指標の数が３点以上の場合はステップＳ２１０３０に処理を進め、３点未満の場合は処理を終了する。
【０２８３】
ステップＳ２１０３０において、位置姿勢算出部２０２０は、姿勢計測値のうちの傾斜角に関する２パラメータを規定値として扱い、夫々の検出指標の情報を用いて残された４パラメータを推定する。ここで、残された４パラメータとは、撮像装置２０３０の方位角と位置である。この処理は、例えば、非特許文献６に開示されている手法によって実現できるので、詳細な説明は省略する。以下では、ステップＳ２１０３０で得られる位置及び姿勢を、以降の処理における位置及び姿勢の初期値ｓ0= [t0T ω0T]T = [x0 y0 z0 ξ0 ψ0 ζ0]Tと表記する。なお、得られた姿勢ω0と姿勢計測値ω*τとの差異は、方位角成分のみとなる。すなわち、ω0はω*τの方位角ドリフト誤差を補正した姿勢と考えることができる。
【０２８４】
ステップＳ２１０４０において、位置姿勢算出部２０２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値（理論値）ｕｎ＊（＝ｕＣＱｋｎ＊）を算出する。ｕｎ＊は、ｓによって定義される指標の観測方程式、すなわち、指標の基準座標ｘwＱｋｎから画像座標を算出する式１９に基づいて算出される。ただし式１９では、画像座標の推定値ｕｎ＊を、ｕＱｋｎ＊と表記している。
【０２８５】
ステップＳ２１０５０において、位置姿勢算出部２０２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕｎ＊と実測値ｕｎとの差異（再投影誤差）Δｕｎを、次式３４によって算出する。
【０２８６】
【数３４】
【０２８７】
ステップＳ２１０６０において、位置姿勢算出部２０２０は、各々の指標Ｑｋｎに対して、ｓに関する画像ヤコビアンＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。ここで、画像ヤコビアンとは、式１９の観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。具体的には、ヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）とヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、次式３５によってＪｕｓＱｋｎを算出する。
【０２８８】
【数３５】
【０２８９】
ここで、ＪｕｘＱｋｎは、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列である。また、ＪｘｓＱｋｎは、式２０の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列である。
【０２９０】
ステップＳ２１０７０において、位置姿勢算出部２０２０は、ステップＳ２１０３０で得た姿勢の初期値ω0に対する現在の姿勢推定値ωの差異Δω( = [Δξ Δψ Δζ]T)を、次式３６によって算出する。
【０２９１】
【数３６】
【０２９２】
ステップＳ２１０８０において、位置姿勢算出部２０２０は、上記のステップで算出した再投影誤差Δｕｎ、画像ヤコビアンＪｕｓＱｋｎ、及び姿勢の差異Δωに基づいて、ｓの補正値Δｓを算出する。具体的には、ヤコビアンを並べた行列Θと誤差ベクトルＵを作成し、次式３７によってΔｓを算出する。
【０２９３】
【数３７】
【０２９４】
ここで、行列Θは、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた行列に、姿勢のパラメータξ，ψ，ζ自身のｓに関するヤコビアンを追加した行列である。Θは次式３８によって定義される。
【０２９５】
【数３８】
【０２９６】
また、Θ'は、Θの転置行列に重み係数wを付加した行列であり、次式３９によって定義される。
【０２９７】
【数３９】
【０２９８】
ここでwは、画像情報と姿勢計測値とのバランスをとるための重み係数であり、「姿勢計測値を重視する度合い」を表している。wの値によって、最終的に得られる位置及び姿勢に対する姿勢計測値の影響力の基準値が決定される。重み係数wの値は、重み設定部２０６０によって与えられる。なお、姿勢計測値の実際の影響力は、指標から得られる情報量の大小に応じて受動的に変更される。
【０２９９】
一方、誤差ベクトルＵは、ΔｕｎとΔωの各項を垂直に並べた２Ｎ＋３次元のベクトルである。Ｕは次式４０によって定義される。
【０３００】
【数４０】
【０３０１】
ここで、式３７乃至式４０によって求められるΔｓの意味について説明する。式３６の目的は、直感的には、誤差ベクトルＵを小さくするようなΔｓを求めることである。例えば第５の実施形態のステップＳ１５０７０における処理のような、指標の情報のみを用いてカメラの位置と姿勢を求める手法では、誤差ベクトルＵは次式４１によって定義される。
【０３０２】
【数４１】
【０３０３】
これは、指標の再投影誤差を評価基準として、これを最小化することを意図している。
【０３０４】
一方、式４０の誤差ベクトルＵは、式４１の誤差ベクトルＵに姿勢の差異Δωを付加したものとなっている。これは、本実施形態に係る位置姿勢算出部２０２０が、指標の再投影誤差と姿勢計測値の差異を評価基準としていることを示している。直感的には、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするようなΔｓが算出される。
【０３０５】
ステップＳ２１０９０において、位置姿勢算出部２０２０は、ステップＳ２１０８０において算出した補正値Δｓを用い、式１２に従ってｓを補正し、得られた値をｓの新たな推定値とする。
【０３０６】
ステップＳ２１１００において、位置姿勢算出部２０２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ２１０４０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ２１１１０へと処理を進める。
【０３０７】
ステップＳ２１１１０において、位置姿勢算出部２０２０は、ステップＳ２１１００までの処理によって得られたｓを、撮像装置２０３０の位置と姿勢の推定値として出力する。なお、位置及び姿勢の出力形態はｓそのものに限定されるものではない。例えば姿勢を、回転行列、回転軸ベクトルと回転角、オイラー角、４元数等に変換して出力してもよい。また、同次座標表現による４×４の位置姿勢変換行列やその逆行列によって、位置及び姿勢を表してもよい。また、撮像装置２０３０に対する基準座標系の位置及び姿勢を求めてこれを出力してもよい。もちろん、これらのいずれかの組み合わせを同時に出力してもよい。
【０３０８】
以上のようにして、撮像装置２０３０の位置及び姿勢が算出される。なお、連続的に撮影される時系列画像に対して撮像装置２０３０の位置及び姿勢を求める場合には、入力されたフレーム毎に上述の処理を実行すればよい。
【０３０９】
本実施形態に係る位置姿勢計測装置によれば、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような位置と姿勢が算出される。
【０３１０】
本実施形態に係る位置姿勢計測装置によれば、画像から得られる情報が不十分な場合には、検出指標の項の影響力が小さくなるので、姿勢計測値を重視した位置及び姿勢を得ることができる。その結果、姿勢計測値を信用して未知パラメータ数を限定する手法と同様な、解を安定化させる効果を得ることができる。一方、画像情報が十分に得られている場合には、検出指標の項の影響力が大きくなる。その結果、姿勢計測値の誤差に影響されることなく、高精度な解を得ることができる。
【０３１１】
すなわち、本実施形態に係る位置姿勢計測装置によれば、画像と姿勢センサから得られる情報を無駄なく利用することが可能となり、従来の方法と比べて安定性と精度の双方に優れた計測が実現できる。
【０３１２】
［第８の実施形態］
本実施形態に係る位置計測装置は、環境側に配置した複数台の撮像装置及び指標、計測対象物体に設置した複数台の撮像装置、指標、及び姿勢センサを用いて、任意の計測対象物体の位置と姿勢の計測を行う。以下では、本実施形態に係る位置計測装置及び位置計測方法について説明する。
【０３１３】
図２２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置２２００は、画像入力部２２５０、指標検出部２２１０、位置姿勢算出部２２２０、モード選択部２２８０、１台以上の主観視点カメラ２２３０，１台以上の客観視点カメラ２２６０、及び姿勢センサ２０４０によって構成されている。位置姿勢計測装置２２００は、計測対象物体２２７０に接続されている。基準座標系における計測対象物体２２７０の位置と姿勢が、本実施形態に係る位置計測装置２２００によって計測される。
【０３１４】
シーンには、第１の実施形態と同様に、基準座標系における位置が既知である１つ以上の指標Ｑｋ（ｋ＝１，２，・・・，ＫＷ）が配置されている。以下では、環境側に配置されているこれらの指標を、必要に応じて主観視点指標と呼ぶ。主観視点指標の形態は、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かつ、いずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れのものであってもよい。図２２では、ＫＷ＝６、すなわち６個の主観視点指標が配置されている。
【０３１５】
また、計測対象物体２２７０には、物体座標系上における位置（物体座標）が既知である１つ以上の指標ＱＫＷ＋ｋ（ｋ＝１，・・・，ＫＯ）が配置されている。以下では、物体側に配置されているこれらの指標を、必要に応じて客観視点指標と呼ぶ。客観視点指標の形態は、主観視点指標と同様に、撮影画像上における投影像の画像座標が検出可能であって、かつ、いずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れのものであってもよい。図２２では、ＫＯ＝４、すなわち４個の客観視点指標が配置されている。なお，客観視点指標は、計測対象物体２２７０上に直接配置されていなくてもよい。客観視点指標は、例えば、主観視点カメラ２２３０上や姿勢センサ２０４０上に配置されていてもよい。
【０３１６】
主観視点カメラ２２３０ａ及び２２３０ｂ（まとめて、主観視点カメラ２２３０ともいう）は、例えばビデオカメラである。主観視点カメラ２２３０は、計測対象物体２２７０上に配置されている。主観視点カメラ２２３０の夫々が撮像する画像（主観視点画像）は、画像入力部２２５０に入力される。主観視点カメラ２２３０のカメラ内部パラメータや、物体座標系における位置及び姿勢は既知であるものとする。主観視点指標及び主観視点カメラ２２３０の配置は、計測範囲内を計測対象物体２２７０が移動した際に、その撮影画像中に常に主観視点指標を観測可能なように調整されていることが好ましい。図２２では、２台の主観視点カメラ２２３０ａ，２２３０ｂが配置されている。以下では必要に応じて、主観視点カメラ２２３０の夫々によって定義される主観視点カメラ座標系を、記号Ｃ１，Ｃ２で表現する
【０３１７】
客観視点カメラ２２６０ａ〜２２６０ｄ（まとめて、客観視点カメラ２２６０ともいう）は、例えばビデオカメラである。客観視点カメラ２２６０は、計測対象物体２２７０を撮影可能なように、計測範囲を包囲する位置に設置されている。客観視点カメラ２２６０の夫々が撮像する画像（客観視点画像）は、画像入力部２２５０に入力される。客観視点カメラ２２６０のカメラ内部パラメータや、基準座標系における位置及び姿勢は既知であるものとする。客観視点指標及び客観視点カメラ２２６０の配置は、計測範囲内を計測対象物体２２７０が移動した際に、その撮影画像中に常に客観視点指標を観測可能なように調整されていることが好ましい。図２２では、４台の客観視点カメラ２２６０ａ，２２６０ｂ，２２６０ｃ，２２６０ｄが配置されている。以下では必要に応じて、客観視点カメラ２２６０ａ〜２２６０ｄの夫々によって定義される客観視点カメラ座標系を、それぞれ記号Ｂ１，Ｂ２，Ｂ３，Ｂ４で表現する。
【０３１８】
姿勢センサ２０４０は、計測対象物体２２７０上に配置されている。姿勢センサ２０４０は、計測対象物体２２７０の姿勢を計測して位置姿勢算出部２２２０へと出力する。
【０３１９】
画像入力部２２５０は、夫々のカメラから入力される撮影画像をデジタルデータに変換し、指標検出部２２１０へと出力する。
【０３２０】
指標検出部２２１０は、画像入力部２２５０より夫々の撮影画像を入力する。指標検出部２２１０は、夫々の入力画像を解析し、夫々の画像中に撮影されている指標を検出（並びに同定）する。指標検出部２２１０は、夫々の検出指標について、検出したカメラの識別子と、画像座標の実測値と、対応する３次元座標の組を、位置姿勢算出部２２２０へと出力する。ここで、３次元座標とは、指標が主観視点指標の場合は基準座標を、客観視点指標の場合は物体座標を意味している。なお、指標の検出方法は第１の実施形態と同様にいずれの方法であっても良いので、詳細な説明は省略する。
【０３２１】
なお、以下では、検出された指標の夫々に通し番号ｎ（ｎ＝１，，，Ｎ）をつけ、画像上で検出された夫々の指標をＱｋｎと表記する。ここで、Ｎは各画像上で検出された指標の総数である。また、Ｎ＝ΣＮｄであり、Ｎｄは夫々のカメラの撮影画像上で検出された指標の数を表している。また、ｄはカメラの識別子（ｄ＝Ｃ１，Ｃ２，Ｂ１，Ｂ２，Ｂ３，Ｂ４）を表している。また、指標Ｑｋｎの画像座標をｕｎ、その指標を撮影したカメラの識別子をｄｎと表記し、識別子ｄのカメラで検出された指標Ｑｋの画像座標をｕｄＱｋと表記する（すなわち、ｕｎ＝ｕｄｎＱｋｎ）。また、指標Ｑｋが主観視点指標である場合に、既知の値であるその基準座標をｘｗＱｋと表記する。同様に、指標Ｑｋが客観視点指標である場合に、既知の値であるその物体座標をｘＯＱｋと表記する。
【０３２２】
モード選択部２２８０は、キーボード１００４やマウス１００５等の入力デバイスを介して、位置姿勢算出部２２２０において使用するアルゴリズムを選択する。例えばラジオボタン等のＧＵＩによって、「指標の情報を重視」モード、「姿勢センサの計測値を重視」モード、それらの「中間」モード、のいずれかを選択可能とする。モード選択部２２８０は、選択されたモードがいずれであるかを、位置姿勢算出部２２２０に出力する。
【０３２３】
位置姿勢算出部２２２０は、計測対象物体２２７０の姿勢計測値を姿勢センサ２０４０から入力する。また、夫々の検出指標について、検出したカメラの識別子ｄｎと、画像座標の実測値ｕｎと、対応する３次元座標（ｘｗＱｋｎまたはｘＯＱｋｎ）の組を、指標検出部２２１０から入力する。そして、入力した情報に基づいて、計測対象物体２２７０の位置及び姿勢を算出して出力する。位置姿勢算出部２２２０の処理の詳細については、フローチャートを用いて後に詳しく説明する。
【０３２４】
なお、画像入力部２２５０、指標検出部２２１０、モード選択部２２８０、及び位置姿勢算出部２２２０の少なくとも一部は、独立した装置として実現しても良い。あるいは、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部２２５０、指標検出部２２１０、モード選択部２２８０、及び位置姿勢算出部２２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。なお、ソフトウェアを実行することで各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので、詳細な説明は省略する。
【０３２５】
図２３は、位置姿勢算出部２２２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部２２２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。
【０３２６】
以下では、計測対象物体２２７０の位置及び姿勢を、第１の実施形態と同様、３値のベクトルt = [x y z]T及びω= [ξψ ζ]Tによって表現する。また、計測対象物体２２７０の位置と姿勢を、６値のベクトルｓ= [tT ωT]T = [x y z ξ ψ ζ]Tによって表現する。
【０３２７】
ステップＳ２３０００において、位置姿勢算出部２２２０は、時刻τにおける計測対象物体２２７０の姿勢計測値ω*τ( = [ξ*τ ψ*τ ζ*τ]T)を、姿勢センサ２０４０から入力する。
【０３２８】
ステップＳ２３０１０において、位置姿勢算出部２２２０は、時刻τにおける夫々の撮影画像から検出された検出指標Ｑｋｎの情報を、指標検出部２２１０から入力する。具体的には、検出したカメラの識別子ｄｎと、画像座標の実測値ｕｎと、対応する３次元座標（ｘｗＱｋｎまたはｘＯＱｋｎ）の組を入力する。
【０３２９】
ステップＳ２３０２０において、位置姿勢算出部２２２０は、入力された検出指標の情報が位置及び姿勢の推定に十分か否かを判定し、それに応じて処理の分岐を行う。例えば、入力した指標の実体の総数が３点以上（Ｎ≧３）の場合はステップＳ２３０３０に処理を進め、３点未満の場合は処理を終了する。ここで、実態の総数とは、同一指標が複数の画像中で検出された場合には、１つの指標が検出されたものとして計数した総数を意味する。
【０３３０】
ステップＳ２３０３０において、位置姿勢算出部２２２０は、モード選択部２２８０で選択されているモードが、「指標の情報を重視」モードであるか否かの判定を行う。「指標の情報を重視」モードの場合はステップＳ２３０３５へと処理を進め、その他の場合はステップＳ２３０４０へと処理を進める。
【０３３１】
ステップＳ２３０３５において、位置姿勢算出部２２２０は、夫々の検出指標の再投影誤差を評価基準として用いる手法によって、計測対象物体２２７０の位置及び姿勢を推定する。主観及び客観視点カメラから得られる検出指標の再投影誤差を最小化することで物体の位置及び姿勢を求める手法は、非線形最適化計算に基づく手法が非特許文献１等に開示されているので、詳細な説明は省略する。さらに、ステップＳ２３０７０へと処理を進める。
【０３３２】
ステップＳ２３０４０において、位置姿勢算出部２２２０は、姿勢計測値のうちの傾斜角に関する２パラメータを規定値として扱い、夫々の検出指標の情報を用いて残された４パラメータを推定する。ここで、残された４パラメータとは、計測対象物体２２７０の方位角と位置である。この処理は、例えば、非特許文献１に開示されている手法によって実現できるので、詳細な説明は省略する。以下では、ステップＳ２３０４０で得られる位置及び姿勢を、以降の処理における位置及び姿勢の初期値ｓ0= [t0T ω0T]T = [x0 y0 z0 ξ0 ψ0 ζ0]Tと表記する。
【０３３３】
ステップＳ２３０５０において、位置姿勢算出部２２２０は、モード選択部２２８０で選択されているモードが、「姿勢センサの計測値を重視」モードであるか否かの判定を行う。「姿勢センサの計測値を重視」モードの場合は、ステップＳ２３０４０で得られる位置及び姿勢をそのまま出力することとして、ステップＳ２３０７０へと処理を進める。また、その他の場合はステップＳ２３０６０へと処理を進める。
【０３３４】
ステップＳ２３０６０において、位置姿勢算出部２２２０は、夫々の検出指標の再投影誤差と、推定された姿勢パラメータと姿勢計測値との間の差異とを評価基準として用いる手法によって、計測対象物体２２７０の位置及び姿勢を推定する。ステップＳ２３０６０の処理の詳細については、フローチャートを用いて後に詳しく説明する。
【０３３５】
ステップＳ２３０７０において、位置姿勢算出部２２２０は、以上の処理によって得られた計測対象物体２２７０の位置及び姿勢を出力する。
【０３３６】
次に、図２４のフローチャートを用いて、ステップＳ２３０６０における処理の詳細を説明する。
【０３３７】
ステップＳ２４０１０において、位置姿勢算出部２２２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値（理論値）ｕｎ＊を算出する。ｕｎ＊は、指標Ｑｋｎが主観視点指標の場合には、次式４２で表される主観視点カメラ指標Ｃｉによる観測方程式に基づいて算出される。
【０３３８】
【数４２】
【０３３９】
観測方程式Ｆｃｉ（）は、ｘwＱｋｎとｓから当該指標のカメラ座標ｘＣｉＱｋｎを求める式４３と、ｘＣｉＱｋｎから画像座標ｕｎ＊を求める式４４によって構成される。
【０３４０】
【数４３】
【数４４】
【０３４１】
ここでｆＣｉｘ及びｆＣｉｙは、それぞれｘ軸方向及びｙ軸方向における主観視点カメラ指標Ｃｉの焦点距離である。また、RCiOは物体座標系Oから主観視点カメラ座標系Ｃｉへ姿勢を変換する行列、tCiOは同座標系間の位置を変換するベクトルである。いずれの値も、主観視点カメラ２２３０ａ，２２３０ｂ毎に予め位置の値として保持されているものとする。
【０３４２】
一方、指標Ｑｋｎが客観視点指標の場合には、ｕｎ＊は、次式４５で表される客観視点カメラＢｉによる観測方程式に基づいて算出される。
【０３４３】
【数４５】
【０３４４】
観測方程式ＦＢｉ（）は、ｘＯＱｋｎとｓから当該指標の世界座標ｘＷＱｋｎを求める式４６と、ｘＷＱｋｎから夫々の客観視点カメラ座標系における座標ｘＢｉＱｋｎを求める式４７と、ｘＢｉＱｋｎから画像座標ｕｎ＊を求める式４８によって構成される。
【０３４５】
【数４６】
【数４７】
【数４８】
【０３４６】
ステップＳ２４０２０において、位置姿勢算出部２２２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕｎ＊と実測値ｕｎとの差異（再投影誤差）Δｕｎを、式３４によって算出する。
【０３４７】
ステップＳ２４０３０において、位置姿勢算出部２２２０は、各々の指標Ｑｋｎに対して、ｓに関する画像ヤコビアンＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。ここで、画像ヤコビアンとは、式４２もしくは式４５の観測方程式をｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。観測方程式Ｆｃｉ（）の画像ヤコビアンは式３５と同様にして得ることができる。一方、観測方程式ＦＢｉ（）の画像ヤコビアンは、ヤコビ行列ＪｕｘＢＱｋｎ（＝∂ｕ／∂ｘＢ）とヤコビ行列ＪｘＢｘＷＱｋｎ（＝∂ｘＢ／∂ｘＷ）とヤコビ行列ＪｘＷｓＱｋｎ（＝∂ｘＷ／∂ｓ）を算出し、次式４９によって算出する。
【０３４８】
【数４９】
【０３４９】
ここで、ＪｕｘＢＱｋｎは、式４８の右辺を客観視点カメラ座標ｘＢｉＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列である。また、ＪｘＢｘＷＱｋｎは、式４７の右辺を世界座標ｘＷＱｋｎの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列である。また、ＪｘＷｓＱｋｎは、式４６の右辺を状態ベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列である。
【０３５０】
ステップＳ２４０４０において、位置姿勢算出部２２２０は、ステップＳ２３０４０で得た姿勢の初期値ω0に対する現在の姿勢推定値ωの差異Δω( = [Δξ Δψ Δζ]T)を、式３５によって算出する。
【０３５１】
ステップＳ２４０５０において、位置姿勢算出部２２２０は、上記のステップで算出した再投影誤差ΔｕＱｋｎ、画像ヤコビアンＪｕｓＱｋｎ、及び姿勢の差異Δωに基づいて、ｓの補正値Δｓを算出する。具体的には、ヤコビアンを並べた行列Θと誤差ベクトルＵを作成し、式３７によってΔｓを算出する。なお、行列Θ、行列Θ'、及び行列Ｕの定義は、それぞれ式３８，式３９，式４０と同様である。ただし、第７の実施形態とは異なり、夫々の行列には、異なるカメラ（客観視点カメラ及び主観視点カメラ）による検出指標Ｑｋｎに関する情報が全て含まれている。また、本実施形態では、重み係数ｗは、予め求めた適切な値が固定値として設定されているものとする。
【０３５２】
ステップＳ２４０６０において、位置姿勢算出部２２２０は、ステップＳ２４０５０において算出した補正値Δｓを用いて式１２に従ってｓを補正し、得られた値をｓの新たな推定値とする。
【０３５３】
ステップＳ２４０７０において、位置姿勢算出部２２２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のｓを用いて、再度ステップＳ２４０１０以降の処理を行う。一方、収束していると判断した場合には処理を終了する。
【０３５４】
以上のようにして、計測対象物体２２７０の位置及び姿勢が算出される。本実施形態に係る位置姿勢計測装置によれば、姿勢が姿勢計測値（正確には、姿勢計測値の方位誤差を補正した値）から離れすぎてはいけないという制約条件を加味しながら、夫々のカメラによる検出指標の再投影誤差を極力小さくするような位置と姿勢が算出される。
【０３５５】
本実施形態では、主観視点カメラ２２３０と客観視点カメラ２２６０の双方を用いた。しかし、計測対象物体２２７０の位置及び姿勢の計測に使用するカメラの構成はこれに限定されるものではない。例えば、主観視点カメラ２２３０のみを用いても良いし、客観視点カメラ２２６０のみを用いても良い。
【０３５６】
なお、主観視点カメラ２２３０の位置及び姿勢を求めることが目的の場合にも、本実施形態と同様な方法を用いることができる。この場合、主観視点カメラ２２３０（のうちのひとつ）自体が計測対象物体２２７０であると考えればよい。
【０３５７】
本実施形態では、予め定めた重み係数を固定値として用いた。しかし、第１の実施形態と同様に重み設定部２０６０を設けて、操作者が重み係数wを設定することも可能である。また、重み設定部２０６０とモード選択部２２８０を同一のＵＩで設定できるような工夫をしても良い。例えば、重み設定部２０６０の入力手段として、正規化されたパラメータ（例えば、0から1の間の値）を数値入力するＵＩを考える。この場合、入力されたパラメータが0場合は「指標の情報を重視」モード、1の場合は「姿勢センサの計測値を重視」モード、0と1の中間値の場合は「中間」モードとしても良い。そして、「中間」モードの場合にのみ、入力されたパラメータを重み係数wに変換して用いても良い。
【０３５８】
［第７及び第８の実施形態の変形例］
（変形例７）
上記第７及び第８の実施形態では、姿勢センサ２０４０による姿勢計測値から離れすぎてはいけないという制約条件を考慮しながら、計測対象物体（撮像装置２０３０または計測対象物体２２７０）の位置及び姿勢を推定していた。しかし、上記実施形態で述べた技術的思想の適用範囲は、姿勢計測値から離れすぎてはいけないという制約条件だけを考慮することに限定されるものではない。
【０３５９】
例えば、連続的に撮影される時系列画像に対して計測対象物体の位置及び姿勢を求める場合には、前のフレームとの連続性を考慮してもよい。前のフレームで求めた位置及び姿勢から離れすぎてはいけないという制約条件を考慮することで、フレーム間の連続性に優れた位置及び姿勢を得ることができる。
【０３６０】
本変形例の処理ステップは、上記第７及び第８の実施形態とほぼ同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、ステップＳ２１１１０において位置及び姿勢の推定結果（ｓτ-1）が保持される点と、補正値の算出工程（ステップＳ２１０８０）において前フレームに対する拘束条件が考慮される点のみが異なっている。なお、第８の実施形態も同様に変更することができる。
【０３６１】
本変形例では、ステップＳ２１０８０における式３８の行列Θ、式３９のΘ'、式４０の誤差ベクトルＵの夫々を、次式５０〜５２のように変更する。
【０３６２】
【数５０】
【数５１】
【数５２】
【０３６３】
すなわち、前フレームの結果に対する拘束条件を表す６つの評価値が、新たに追加される。ここで、w１，w２，w３は、それぞれ姿勢計測値の影響度、前フレームの位置の影響度、前フレームの姿勢の影響度を定める重みパラメータである。本変形例では、夫々の重みパラメータには適当な値が予め設定されているものとする。これらの値は、姿勢センサの信頼度やフレーム間の連続性を重視する度合いに応じて個別に設定しても良いし、簡易的には同一の値を設定してもよい。また、パラメータごとに異なる値を設定してもよい。また、上記の実施形態と同様に、重み設定部２０６０を介してユーザが値を調整できるようにしてもよい。
【０３６４】
以上によって、姿勢が姿勢計測値から離れすぎてはいけないという制約条件と、位置及び姿勢が前フレームの推定結果から離れすぎてはいけないという制約条件を同時に考慮することができる。そして、これらの拘束条件の範囲内で、指標の再投影誤差を極力小さくするような位置と姿勢を算出することができる。
【０３６５】
なお、前フレームにおける位置及び姿勢の推定結果であるｓτ-1は、必ずしもステップＳ２１１１０で推定された位置及び姿勢そのものでなくてもよい。例えば、計測対象物体の移動の履歴から、現フレームにおける計測対象物体の位置及び姿勢を予測した値を用いてもよい。この場合、予測は計測対象物体の速度や角速度から単純な線形近似によって行ってもよいし、他のいずれの方法によって行ってもよい。
【０３６６】
なお、本変形例では前フレームにおける位置及び姿勢パラメータの全て（６自由度）を制約条件として用いていたが、必ずしも全パラメータを制約条件としなくてもよい。例えば、姿勢の制約は姿勢センサの計測値に対してのみ行ってもよい。この場合、式５０の行列Θ、式５１のΘ'、式５２の誤差ベクトルＵの夫々から、前フレームの姿勢に関する項を除外すればよい。同様に、前フレームの位置に関する項を省略しても良い。また、前フレームにおける位置及び姿勢パラメータのうちの任意の組み合わせに対してのみ拘束を行ってもよい。
【０３６７】
（変形例８）
上記第７及び第８の実施形態に追加する形で、さらに、計測対象物体（撮像装置２０３０または計測対象物体２２７０）上に位置センサが設置されている場合を考える。この場合、姿勢センサから得られる姿勢計測値と位置センサから得られる位置計測値の双方を制約条件として用いることができる。
【０３６８】
本変形例の処理ステップは、上記第７及び第８の実施形態とほぼ同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、位置センサによる位置計測値を入力する工程をステップＳ２１０１０の直後にステップＳ２１０１５として有する点と、補正値の算出工程（ステップＳ２１０８０）において位置計測値に対する拘束が考慮される点のみが異なっている。なお、第８の実施形態も同様に変更することができる（図２３参照）。
【０３６９】
新規に追加されるステップとして、ステップＳ２１０１５において、位置姿勢算出部２０２０は、時刻τにおける撮像装置２０３０の位置計測値ｔ*τを、位置センサから入力する。
【０３７０】
ステップＳ２１０３０において、位置姿勢算出部２０２０は、位置及び姿勢の初期値ｓ0を算出する。本変形例では、ステップＳ２１０１０で入力した姿勢計測値ω*τに前フレームの処理で推定された姿勢センサの誤差補正値Δωτ-1を乗算した姿勢を、姿勢の初期値ω0とおく。また、ステップＳ２１０１５で入力した位置計測値ｔ*τと次式５３を用いて、位置の初期値ｔ０を算出する。
【０３７１】
【数５３】
【０３７２】
ここで、ｘｏは、既知の値である計測対象物体の座標系における位置センサの計測点の座標を示している。すなわち、式５３は、位置センサの計測値（計測点の位置を表している）から計測対象の位置への変換式を表している。
【０３７３】
本変形例では、ステップＳ２１０７０で算出する姿勢の初期値と現在の推定姿勢との差異を求める式（式３６）を、次式５４のように、位置及び姿勢の双方を含むように変形する。
【０３７４】
【数５４】
【０３７５】
そして、ステップＳ２１０８０で算出する、式３８の行列Θ、式３９のΘ'、式４０の誤差ベクトルＵの夫々を、次式５５〜５７のように変更する。
【０３７６】
【数５５】
【数５６】
【数５７】
【０３７７】
すなわち、位置の計測値に対する拘束条件が新たに追加される。ここで、w１およびw２は、それぞれ位置計測値の影響度と姿勢計測値の影響度を定める重みパラメータである。本変形例では、w１とw２には適当な値が予め設定されているものとする。これらの値は、夫々のセンサの信頼度に応じて個別に設定しても良いし、簡易的には同一の値を設定しても良い。また、パラメータごとに異なる値を設定してもよい。また、上記の実施形態と同様に、重み設定部２０６０を介してユーザが値を調整できるようにしても良い。
【０３７８】
ステップＳ２１１１０では、推定された位置及び姿勢を出力する。このとき、姿勢計測値ω＊τと最終的に得られた姿勢ωとの差異を求め、姿勢の誤差補正値Δωτ-1として保持しておく。
【０３７９】
以上によって、位置及び姿勢が位置計測値と姿勢計測値の夫々から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような位置と姿勢を算出することができる。
【０３８０】
本変形例に係る位置姿勢計測装置によれば、画像から得られる情報が不十分な場合には、指標の項の影響力が小さくなるので、夫々のセンサの計測値を重視した位置及び姿勢を得ることができる。一方、画像情報が十分に得られている場合には、指標の項の影響力が大きくなる。その結果、夫々のセンサ計測値の誤差に影響されることなく、高精度な解を得ることができる。
【０３８１】
なお、本変形例においても、フレーム間の連続性を考慮することができる。この場合、変形例７と同様に、前フレームで求めた位置及び姿勢に対する制約を補正値の算出工程に追加すればよい。具体的には、式５５の行列Θ、式５６のΘ'、式５７の誤差ベクトルＵの夫々に、前フレームの位置及び姿勢のパラメータを評価する項を追加すればよい。なお、全１２の制約条件（前フレームに関して６、センサ計測値に関して６）の任意の組み合わせを選択して用いてもよいことは言うまでもない。
【０３８２】
（変形例９）
上記第７及び第８の実施形態と変形例７及び８の夫々では、計測対象物体（撮像装置２０３０または計測対象物体２２７０）の位置及び姿勢の６自由度のパラメータを推定していた。しかし、上記実施形態で述べた技術的思想の適用範囲は、推定パラメータが位置及び姿勢の６値の場合のみに限定されるものではない。
【０３８３】
例えば、計測対象物体上にさらに高精度な位置センサが設置されている場合を考える。この場合、公知の方法として、位置センサから得られる位置を既知の値として、計測対象物体の姿勢パラメータのみを画像情報から求めるというアプローチを取ることができる。このアプローチは、例えば、特許文献４の第６の実施形態に記載されているように、計測対象物体の姿勢パラメータのみを未知とした非線形最適化処理によって実現できる。そして、このようなアプローチをとる場合においても、上記実施形態で述べた技術的思想をそのまま適用することができる。すなわち、姿勢センサ２０４０による姿勢計測値に対する拘束条件を考慮しながら、計測対象物体の姿勢パラメータを推定することができる。
【０３８４】
本変形例の処理ステップは、上記第７及び第８の実施形態と同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、位置センサによる位置計測値を入力する工程（ステップＳ２１０１５）を有する点と、計測対象物体の姿勢ωのみが未知パラメータとなる点のみが異なっている。なお、第８の実施形態も同様に変更することができる。
【０３８５】
ステップＳ２１０１５、ステップＳ２１０３０、ステップＳ２１１１０の処理は、変形例７と同様である。
【０３８６】
本変形例では、計測対象物体の位置を既知の値として扱い、未知パラメータは姿勢を表すωのみとなる。すなわち、ステップＳ２１０４０では、計測対象物体の位置を固定値とおく。また、ステップＳ２１０６０では、ωの各パラメータについてのみ画像ヤコビアンを算出する。さらに、ステップＳ２１０８０における式３８の行列Θ及び式３９のΘ'を、未知パラメータの変化に応じて次式５８及び式５９のように変更する。
【０３８７】
【数５８】
【数５９】
【０３８８】
以上によって、６自由度の位置及び姿勢を未知パラメータとしない場合であっても、姿勢センサの計測値から離れすぎてはいけないという制約条件を加味しながら、指標の再投影誤差を極力小さくするような姿勢を求めることができる。
【０３８９】
なお、本変形例においても、フレーム間の連続性を考慮することができる。この場合、変形例７と同様に、前フレームで求めた姿勢に対する制約を補正値の算出工程に追加すればよい。具体的には、式５８の行列Θ、式５９のΘ'、式４０の誤差ベクトルＵの夫々に、前フレームの姿勢のパラメータを評価する項を追加すればよい。
【０３９０】
（変形例１０）
上記第７及び第８の実施形態と変形例７乃至９の夫々においては、重みパラメータｗの値は予め定めた固定値であるか、あるいは、重み設定部２０６０を介してユーザが設定した値であった。しかし、重みパラメータの値はこれらの方法で設定することに限定されるものではない。本変形例では、検出された指標の分布に応じて重みパラメータを能動的に調整することで、指標から得られる情報量に応じて指標の影響度を調整する効果を、より積極的に実現する。
【０３９１】
本変形例の処理ステップは、上記第７及び第８の実施形態と同様である。以下、第７の実施形態との差異について、図２１を参照しながら説明する。基本的には、検出された指標の分布状況を算出する工程（ステップＳ２１０２５）と、算出された指標の分布状況に応じて重みパラメータｗを決定する工程（ステップＳ２１０２７）を有する点のみが異なっている。なお、第８の実施形態も同様に変更することができる。
【０３９２】
新規に追加されるステップとして、ステップＳ２１０２５において、位置姿勢算出部２０２０は、検出された全指標の画像座標を包含する凸包を算出する。この処理は、第６の実施形態におけるステップＳ１７０５０の処理と同様である。
【０３９３】
次に、新規に追加されるステップとして、ステップＳ２１０２７において、位置姿勢算出部２０２０は、ステップＳ２１０２５で求めた凸包の面積Ａに応じて、次式６０によって重み調整パラメータｗ０を決定する。
【０３９４】
【数６０】
【０３９５】
ここで、ＴminとＴmaxは凸包の面積を正規化するためのパラメータであり、適切な値が予め設定されているものとする。ｗ０の範囲は０から１に正規化された値であり、指標の分布が十分であれば０に、不十分であれば１に近い値となる。得られたｗ０に基づいて、補正値Δｓの算出工程（ステップＳ２１０８０）で用いる重み係数ｗを、次式６１によって算出する。
【０３９６】
【数６１】
【０３９７】
ここでｗ’は、重み設定部２０６０で設定された重み係数を表している。すなわち、重み設定部２０６０で設定された重み係数を、指標の分布に応じて増減する。なお、ｗ’の設定は重み設定部２０６０で行わなくてもよく、予め適切な値を設定しておいてもよい。
【０３９８】
以上によると、指標の分布が十分であれば重み係数ｗの値は小さくなるので、姿勢が姿勢計測値から離れすぎてはいけないという制約条件の影響度が小さくなる。そのため、センサの誤差に制約を受けることなく、検出された画像情報を十分に生かした位置及び姿勢の計測を行うことができる。
【０３９９】
一方、指標の分布が不十分な場合は重み係数ｗの値が大きくなるので、姿勢計測値から離れすぎてはいけないという制約条件の影響度が大きくなる。そのため、検出された画像情報の情報量が不足していても、センサから得られる姿勢を逸脱しない推定結果を得ることができる。
【０４００】
以上のように、本変形例によっても、画像から得られる情報の影響度を指標の分布に応じて制御するという効果を得ることができる。
【０４０１】
なお、本変形例においても、指標の分布の尺度として指標の成す凸包の面積を用いることは必須ではなく、上記実施形態の変形例４と同様な様々な尺度を用いてもよい。また、重み係数ｗの変位は指標の分布の尺度に対して線形である必要はなく、第６の実施形態と同様に二つの区間を単調増加する任意の値を用いることができる。
【０４０２】
なお、変形例７と同様に、前のフレームで求めた位置及び姿勢から離れすぎてはいけないという制約条件を同時に考慮する場合でも、指標の分布に応じて重み係数を調整することができる。この場合、ベースとなる重み係数としてｗ１’，ｗ２’，ｗ３’を保持しておいて、夫々にｗ０を乗算することで、実際に適用する重み係数（ｗ１，ｗ２，ｗ３）を求めればよい。また、変形例８のように位置センサによる拘束を利用する場合も同様に、指標の分布に応じて重み係数を調整することができる。
【０４０３】
なお、検出された画像情報の影響度を指標の分布に応じて調整するという効果は、上記で述べた１２の制約条件（前フレームに関して６、センサ計測値に関して６）の任意の組み合わせを用いる場合であっても得ることができる。例えば、センサを使用せずに、位置及び姿勢が前フレームの推定結果（の少なくともいずれか一つのパラメータ）から離れすぎてはいけないという制約条件のみを用いるという構成をとることもできる。
【０４０４】
この場合、指標の分布が十分であれば重み係数ｗの値は小さくなるので、位置や姿勢が前フレームにおける推定値から離れすぎてはいけないという制約条件の影響度が小さくなる。そのため、フレーム間の平滑化を行おうとする拘束の影響を受けることなく、検出された画像情報を十分に生かした位置及び姿勢の計測を行うことができる。
【０４０５】
一方、指標の分布が不十分な場合は重み係数ｗの値が大きくなるので、位置や姿勢が前フレームにおける推定値から離れすぎてはいけないという制約条件の影響度が大きくなる。そのため、検出された画像情報の情報量が不足していても、フレーム間での不自然なジッタ−を回避し、安定した位置及び姿勢を得ることができる。
【０４０６】
（変形例１１）
上記第７及び第８の実施形態と変形例７乃至１０の夫々は、式３７で表現される方法を用いて補正値Δsを算出している。また、sをs+Δsに置き換えることで推定値sの更新を行っている。しかしながら、行列Θ、行列Θ'、及び誤差ベクトルＵに基づいてsを算出する方法はこの方法に限ったものではない。例えば公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を組み合わせて求めてもよい。また、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよい。他の何れの数値計算手法を併用しても発明の本質が損なわれないことは言うまでもない。
【０４０７】
（変形例１２）
上記の夫々の実施形態や変形例では、式３で表されるような３値によって姿勢を表現し、これを最適化計算の工程で用いていた。しかし、最適化計算の工程で用いる姿勢の表現はこれに限定されるものではない。例えば、オイラー角によって姿勢を３値で表現し、このオイラー角を未知パラメータとして最適化計算を行ってもよい。この場合、式３の代わりに、オイラー角から３×３の回転行列Ｒを求める式を得て、これを夫々の観測方程式に組み込めばよい。また、姿勢計測値や前フレームの姿勢推定値による拘束を行う場合には、夫々のパラメータを同様にオイラー角に変更すればよい。もちろん、その他の表現方法を未知パラメータとした場合であっても同様に解くことが可能であり、パラメータの表現方法は本質的問題ではない。なお、オイラー角を用いた非線形最適化計算によるカメラの位置及び姿勢の推定方法は例えば非特許文献７に開示されているので、詳細な説明は省略する。
【０４０８】
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても達成される。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０４０９】
さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０４１０】
本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０４１１】
【図１】第１の実施形態における位置姿勢計測装置の構成を示す図である。
【図２】位置姿勢計測装置の各部をソフトウェアにより実現することのできるコンピュータの基本構成を示す図である。
【図３】第１の実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図４】図３のステップＳ３０３５における位置及び姿勢算出処理の詳細を説明するフローチャートである。
【図５】図３のステップＳ３０７０における位置及び姿勢算出処理の詳細を説明するフローチャートである。
【図６】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図７】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図８】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図９】図６の撮影画像（撮影画像６００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１０】図７の撮影画像（撮影画像７００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１１】図８の撮影画像（撮影画像８００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１２】第２の実施形態における位置姿勢計測装置の構成を示す図である。
【図１３】第３の実施形態における位置姿勢計測装置の構成を示す図である。
【図１４】第４の実施形態における位置姿勢計測装置の構成を示す図である。
【図１５】第５の実施形態における位置姿勢計測装置の構成を示す図である。
【図１６】第５の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図１７】第６の実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図１８】式２８によって得られる、検出された指標間の距離と重みｗ１との関係を示す図である。
【図１９】式２６及び式２８の夫々によって得られる、凸包の面積と重みｗ１及びｗ２との関係を示す図である。
【図２０】第７の実施形態における位置姿勢計測装置の構成を示す図である。
【図２１】第７の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図２２】第８の実施形態における位置姿勢計測装置の構成を示す図である。
【図２３】第８の実施形態における、計測対象物体２２７０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図２４】第８の実施形態における、指標の再投影誤差と姿勢センサの誤差を評価基準とする位置と姿勢の推定処理を説明するフローチャートである。
【特許請求の範囲】
【請求項１】
計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、
撮像画像を取得する撮影画像取得工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
前記指標検出工程により検出された前記指標の分布範囲に関する評価値を算出する分布範囲算出工程と、
前記検出された指標の画像座標に関する情報および前記分布範囲に関する評価値を用いて、前記計測対象物体の位置及び姿勢を算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
【請求項２】
前記計測対象物体は撮像装置であり、
前記撮影画像は、前記撮像装置によって撮影された画像であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項３】
前記撮影画像は、前記計測対象物体との相対位置関係が既知である撮像装置によって撮影された画像であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項４】
前記撮影画像は、前記計測対象物体を撮影する撮像装置によって撮影された画像であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項５】
前記位置姿勢算出工程は、前記分布範囲の評価値に応じて、予め定められた複数の算出方法から１つの算出方法を選択することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項６】
前記分布範囲に関する評価値は、前記検出された指標の３次元的な分布範囲に関する評価値であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項７】
前記分布範囲に関する評価値は、前記検出された指標の前記撮像画像上における分布範囲に関する評価値であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項８】
前記位置姿勢算出工程は、前記指標の分布範囲に関する評価値に応じて、前記検出された指標の画像座標に関する情報が、前記計測対象物体の位置及び姿勢の算出に与える影響度を制御することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項９】
前記位置姿勢算出工程は、
前記指標検出工程で検出された前記指標の画像座標に関する情報を利用して、異なる方法を用いて、前記計測対象物体の第１位置姿勢と第２位置姿勢を算出し、
前記分布範囲算出工程で算出された前記指標の分布範囲に関する評価値に基づいて、前記第１位置姿勢と第２位置姿勢から、前記計測対象物体の位置及び姿勢を算出することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項１０】
さらに、前記計測対象物体の位置及び姿勢を表すパラメータのうちの少なくとも一つを計測部から、計測結果を取得する計測結果取得工程を有し、
前記位置姿勢算出工程は、前記指標の分布範囲に関する評価値に基づいて、前記指標の画像座標に関する情報の影響度と、前記計測結果の影響度とを制御することにより、前記計測対象物体の位置及び姿勢を算出することを特徴とする、請求項１に記載の位置姿勢計測方法。
【請求項１１】
前記計測部は、前記計測対象物体に装着された位置及び／または姿勢センサであることを特徴とする請求項１０に記載の位置姿勢計測方法。
【請求項１２】
さらに、過去のフレームの撮影画像から算出された前記計測対象物体の位置及び姿勢を表すパラメータのうちの少なくとも一つを保持する保持工程を有し、
前記位置姿勢算出工程は、
前記指標の分布範囲に関する評価値に基づいて、前記指標の画像座標に関する情報の影響度と、前記過去のフレームの撮影画像から算出された位置及び姿勢を表すパラメータの影響度とを制御することにより、前記計測対象物体の位置及び姿勢を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項１３】
さらに、前記計測対象物体の姿勢を計測する姿勢センサから姿勢計測値を取得する姿勢計測値取得工程を有し、
前記位置姿勢算出工程は、前記指標の画像座標に関わる情報の情報量の大小に応じて、前記指標の画像座標に関する情報の影響度と前記姿勢計測値の影響度を設定し、前記撮像装置の位置及び姿勢を算出することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項１４】
さらに、前記計測対象物体の姿勢を計測する姿勢センサから姿勢計測値を取得する姿勢計測値取得工程を有し、
前記位置姿勢算出工程は、前記指標検出工程で検出された前記指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される当該画像座標に関する情報の理論値との間の差異と、前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異との双方を評価基準として用いて前記撮像装置の位置及び姿勢を算出することを特徴とする請求項１３に記載の位置姿勢計測方法。
【請求項１５】
計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、
前記計測対象物体に装着された姿勢センサから姿勢計測値を入力する姿勢計測値入力工程と、
撮像画像を取得する撮影画像取得工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
前記検出された指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程とを有し、
前記位置姿勢算出工程は、
前記検出された指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される前記検出された指標の画像座標に関する情報の理論値との間の差異、および、
前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異
を評価基準として用いて、前記撮像装置の位置及び姿勢を算出することを特徴とする位置姿勢計測方法。
【請求項１６】
前記計測対象物体は撮像装置であり、
前記撮影画像は、前記撮像装置によって撮影された画像であることを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項１７】
前記撮影画像は、前記計測対象物体との相対位置関係が既知である撮像装置によって撮影された画像であることを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項１８】
前記撮影画像は、前記計測対象物体を撮影する撮像装置によって撮影された画像であることを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項１９】
さらに、過去のフレームの撮影画像から算出された前記計測対象物体の位置及び姿勢を表すパラメータのうちの少なくとも一つを保持する保持工程を有し、
前記位置姿勢算出工程は、
前記指標の画像座標に関する情報の実測値と、前記検出された指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される前記検出された指標の画像座標に関する情報の理論値との間の差異、
前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異を前記推定値と前記保持手段が保持する過去の位置姿勢を表すパラメータのうちの少なくとも一つとの間の差異、および、
前記推定値と前記過去のフレームの撮影画像から算出された位置姿勢を表すパラメータのうちの少なくとも一つとの間の差異
をと評価機銃として用いて、前記計測対象物体の位置及び姿勢を算出することを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項２０】
計測対象物体としての撮像装置の位置及び姿勢を計測する位置姿勢計測方法であって、
前記撮像装置に装着した姿勢センサから姿勢計測値を入力する姿勢計測値入力工程と、
前記撮像装置が撮影した撮像画像を入力する画像入力工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
オペレータが入力した重みを設定する重み設定工程と、
前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程とを有し、
前記位置姿勢算出工程は、前記重み設定工程で設定された重みに応じて、前記指標の画像座標に関する情報と前記姿勢計測値の影響度を変更し、前記撮像装置の位置及び姿勢を算出することを特徴とする位置姿勢計測方法。
【請求項２１】
前記位置姿勢算出工程は、前記指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される当該画像座標に関する情報の理論値との間の差異と、前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異、の少なくともいずれかを、前記重み設定工程で設定された重みに応じて加重し、当該差異を評価基準として用いて前記撮像装置の位置と姿勢を算出することを特徴とする、請求項２０に記載の位置姿勢計測方法。
【請求項２２】
計測対象物体としての撮像装置の位置及び姿勢を計測する位置姿勢計測方法であって、
前記撮像装置に装着した姿勢センサから姿勢計測値を入力する姿勢計測値入力工程と、
前記撮像装置が撮影した撮像画像を入力する画像入力工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
前記指標の画像座標に関する情報を重視する第１のモード、前記姿勢計測値を重視する第２のモード、前記第１のモード及び第２のモードの中間的なモードである第３のモードのいずれかを選択するモード選択工程と、
前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記モード選択工程で選択されたモードに応じた手法を適用することで、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
【請求項２３】
前記位置姿勢算出工程は、
前記モード選択工程によって前記第１のモードが選択されている場合に、前記姿勢計測値を規定値や評価基準として使用せずに、前記指標検出工程で検出される前記指標の画像座標に関する情報に基づいて前記撮像装置の位置及び姿勢を算出し、
前記モード選択工程によって前記第３のモードが選択されている場合に、前記指標検出工程で検出される前記指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される当該画像座標に関する情報の理論値との間の差異と、前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異との双方を評価基準として、前記撮像装置の位置及び姿勢を算出し、
前記モード選択工程によって前記第２のモードが選択されている場合に、前記姿勢計測値の少なくとも一部のパラメータを規定値として、前記指標検出工程で検出される前記指標の画像座標に関する情報に基づいて、前記撮像装置の位置及び姿勢の内の未知パラメータを算出することを特徴とする請求項２２に記載の位置姿勢計測方法。
【請求項２４】
請求項１乃至請求項２３のいずれか１項に記載の位置姿勢計測方法をコンピュータに実行させるための制御プログラム。
【請求項２５】
請求項２４に記載の制御プログラムを格納するコンピュータ読み取り可能な記録媒体。
【請求項２６】
計測対象物体の位置及び姿勢を計測する位置姿勢計測装置であって、
撮像画像を取得する撮影画像取得手段と、
前記撮像画像に含まれる指標を検出する指標検出手段と、
前記指標検出手段により検出された前記指標の分布範囲に関する評価値を算出する分布範囲算出手段と、
前記検出された指標の画像座標に関する情報および前記分布範囲に関する評価値を用いて、前記計測対象物体の位置及び姿勢を算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。
【請求項２７】
計測対象物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記計測対象物体に装着された姿勢センサから姿勢計測値を入力する姿勢計測値入力手段と、
撮像画像を取得する撮影画像取得手段と、
前記撮像画像に含まれる指標を検出する指標検出手段と、
前記検出された指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出手段とを有し、
前記位置姿勢算出手段は、
前記検出された指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される前記検出された指標の画像座標に関する情報の理論値との間の差異、および、
前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異
を評価基準として用いて、前記撮像装置の位置及び姿勢を算出することを特徴とする位置姿勢計測装置。
【請求項１】
計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、
撮像画像を取得する撮影画像取得工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
前記指標検出工程により検出された前記指標の分布範囲に関する評価値を算出する分布範囲算出工程と、
前記検出された指標の画像座標に関する情報および前記分布範囲に関する評価値を用いて、前記計測対象物体の位置及び姿勢を算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
【請求項２】
前記計測対象物体は撮像装置であり、
前記撮影画像は、前記撮像装置によって撮影された画像であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項３】
前記撮影画像は、前記計測対象物体との相対位置関係が既知である撮像装置によって撮影された画像であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項４】
前記撮影画像は、前記計測対象物体を撮影する撮像装置によって撮影された画像であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項５】
前記位置姿勢算出工程は、前記分布範囲の評価値に応じて、予め定められた複数の算出方法から１つの算出方法を選択することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項６】
前記分布範囲に関する評価値は、前記検出された指標の３次元的な分布範囲に関する評価値であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項７】
前記分布範囲に関する評価値は、前記検出された指標の前記撮像画像上における分布範囲に関する評価値であることを特徴とする請求項１記載の位置姿勢計測方法。
【請求項８】
前記位置姿勢算出工程は、前記指標の分布範囲に関する評価値に応じて、前記検出された指標の画像座標に関する情報が、前記計測対象物体の位置及び姿勢の算出に与える影響度を制御することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項９】
前記位置姿勢算出工程は、
前記指標検出工程で検出された前記指標の画像座標に関する情報を利用して、異なる方法を用いて、前記計測対象物体の第１位置姿勢と第２位置姿勢を算出し、
前記分布範囲算出工程で算出された前記指標の分布範囲に関する評価値に基づいて、前記第１位置姿勢と第２位置姿勢から、前記計測対象物体の位置及び姿勢を算出することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項１０】
さらに、前記計測対象物体の位置及び姿勢を表すパラメータのうちの少なくとも一つを計測部から、計測結果を取得する計測結果取得工程を有し、
前記位置姿勢算出工程は、前記指標の分布範囲に関する評価値に基づいて、前記指標の画像座標に関する情報の影響度と、前記計測結果の影響度とを制御することにより、前記計測対象物体の位置及び姿勢を算出することを特徴とする、請求項１に記載の位置姿勢計測方法。
【請求項１１】
前記計測部は、前記計測対象物体に装着された位置及び／または姿勢センサであることを特徴とする請求項１０に記載の位置姿勢計測方法。
【請求項１２】
さらに、過去のフレームの撮影画像から算出された前記計測対象物体の位置及び姿勢を表すパラメータのうちの少なくとも一つを保持する保持工程を有し、
前記位置姿勢算出工程は、
前記指標の分布範囲に関する評価値に基づいて、前記指標の画像座標に関する情報の影響度と、前記過去のフレームの撮影画像から算出された位置及び姿勢を表すパラメータの影響度とを制御することにより、前記計測対象物体の位置及び姿勢を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項１３】
さらに、前記計測対象物体の姿勢を計測する姿勢センサから姿勢計測値を取得する姿勢計測値取得工程を有し、
前記位置姿勢算出工程は、前記指標の画像座標に関わる情報の情報量の大小に応じて、前記指標の画像座標に関する情報の影響度と前記姿勢計測値の影響度を設定し、前記撮像装置の位置及び姿勢を算出することを特徴とする請求項１記載の位置姿勢計測方法。
【請求項１４】
さらに、前記計測対象物体の姿勢を計測する姿勢センサから姿勢計測値を取得する姿勢計測値取得工程を有し、
前記位置姿勢算出工程は、前記指標検出工程で検出された前記指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される当該画像座標に関する情報の理論値との間の差異と、前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異との双方を評価基準として用いて前記撮像装置の位置及び姿勢を算出することを特徴とする請求項１３に記載の位置姿勢計測方法。
【請求項１５】
計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、
前記計測対象物体に装着された姿勢センサから姿勢計測値を入力する姿勢計測値入力工程と、
撮像画像を取得する撮影画像取得工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
前記検出された指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程とを有し、
前記位置姿勢算出工程は、
前記検出された指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される前記検出された指標の画像座標に関する情報の理論値との間の差異、および、
前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異
を評価基準として用いて、前記撮像装置の位置及び姿勢を算出することを特徴とする位置姿勢計測方法。
【請求項１６】
前記計測対象物体は撮像装置であり、
前記撮影画像は、前記撮像装置によって撮影された画像であることを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項１７】
前記撮影画像は、前記計測対象物体との相対位置関係が既知である撮像装置によって撮影された画像であることを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項１８】
前記撮影画像は、前記計測対象物体を撮影する撮像装置によって撮影された画像であることを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項１９】
さらに、過去のフレームの撮影画像から算出された前記計測対象物体の位置及び姿勢を表すパラメータのうちの少なくとも一つを保持する保持工程を有し、
前記位置姿勢算出工程は、
前記指標の画像座標に関する情報の実測値と、前記検出された指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される前記検出された指標の画像座標に関する情報の理論値との間の差異、
前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異を前記推定値と前記保持手段が保持する過去の位置姿勢を表すパラメータのうちの少なくとも一つとの間の差異、および、
前記推定値と前記過去のフレームの撮影画像から算出された位置姿勢を表すパラメータのうちの少なくとも一つとの間の差異
をと評価機銃として用いて、前記計測対象物体の位置及び姿勢を算出することを特徴とする請求項１５記載の位置姿勢計測方法。
【請求項２０】
計測対象物体としての撮像装置の位置及び姿勢を計測する位置姿勢計測方法であって、
前記撮像装置に装着した姿勢センサから姿勢計測値を入力する姿勢計測値入力工程と、
前記撮像装置が撮影した撮像画像を入力する画像入力工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
オペレータが入力した重みを設定する重み設定工程と、
前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程とを有し、
前記位置姿勢算出工程は、前記重み設定工程で設定された重みに応じて、前記指標の画像座標に関する情報と前記姿勢計測値の影響度を変更し、前記撮像装置の位置及び姿勢を算出することを特徴とする位置姿勢計測方法。
【請求項２１】
前記位置姿勢算出工程は、前記指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される当該画像座標に関する情報の理論値との間の差異と、前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異、の少なくともいずれかを、前記重み設定工程で設定された重みに応じて加重し、当該差異を評価基準として用いて前記撮像装置の位置と姿勢を算出することを特徴とする、請求項２０に記載の位置姿勢計測方法。
【請求項２２】
計測対象物体としての撮像装置の位置及び姿勢を計測する位置姿勢計測方法であって、
前記撮像装置に装着した姿勢センサから姿勢計測値を入力する姿勢計測値入力工程と、
前記撮像装置が撮影した撮像画像を入力する画像入力工程と、
前記撮像画像に含まれる指標を検出する指標検出工程と、
前記指標の画像座標に関する情報を重視する第１のモード、前記姿勢計測値を重視する第２のモード、前記第１のモード及び第２のモードの中間的なモードである第３のモードのいずれかを選択するモード選択工程と、
前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記モード選択工程で選択されたモードに応じた手法を適用することで、前記撮像装置の位置及び姿勢を算出する位置姿勢算出工程とを有することを特徴とする位置姿勢計測方法。
【請求項２３】
前記位置姿勢算出工程は、
前記モード選択工程によって前記第１のモードが選択されている場合に、前記姿勢計測値を規定値や評価基準として使用せずに、前記指標検出工程で検出される前記指標の画像座標に関する情報に基づいて前記撮像装置の位置及び姿勢を算出し、
前記モード選択工程によって前記第３のモードが選択されている場合に、前記指標検出工程で検出される前記指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される当該画像座標に関する情報の理論値との間の差異と、前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異との双方を評価基準として、前記撮像装置の位置及び姿勢を算出し、
前記モード選択工程によって前記第２のモードが選択されている場合に、前記姿勢計測値の少なくとも一部のパラメータを規定値として、前記指標検出工程で検出される前記指標の画像座標に関する情報に基づいて、前記撮像装置の位置及び姿勢の内の未知パラメータを算出することを特徴とする請求項２２に記載の位置姿勢計測方法。
【請求項２４】
請求項１乃至請求項２３のいずれか１項に記載の位置姿勢計測方法をコンピュータに実行させるための制御プログラム。
【請求項２５】
請求項２４に記載の制御プログラムを格納するコンピュータ読み取り可能な記録媒体。
【請求項２６】
計測対象物体の位置及び姿勢を計測する位置姿勢計測装置であって、
撮像画像を取得する撮影画像取得手段と、
前記撮像画像に含まれる指標を検出する指標検出手段と、
前記指標検出手段により検出された前記指標の分布範囲に関する評価値を算出する分布範囲算出手段と、
前記検出された指標の画像座標に関する情報および前記分布範囲に関する評価値を用いて、前記計測対象物体の位置及び姿勢を算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。
【請求項２７】
計測対象物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記計測対象物体に装着された姿勢センサから姿勢計測値を入力する姿勢計測値入力手段と、
撮像画像を取得する撮影画像取得手段と、
前記撮像画像に含まれる指標を検出する指標検出手段と、
前記検出された指標の画像座標に関する情報と、前記姿勢計測値とを用いて、前記撮像装置の位置及び姿勢を算出する位置姿勢算出手段とを有し、
前記位置姿勢算出手段は、
前記検出された指標の画像座標に関する情報の実測値と、前記撮像装置の位置及び姿勢の推定値に基づいて計算される前記検出された指標の画像座標に関する情報の理論値との間の差異、および、
前記推定値と前記姿勢計測値の少なくとも一部のパラメータとの間の差異
を評価基準として用いて、前記撮像装置の位置及び姿勢を算出することを特徴とする位置姿勢計測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【公開番号】特開２００６−２４２９４３（Ｐ２００６−２４２９４３Ａ）
【公開日】平成１８年９月１４日（２００６．９．１４）
【国際特許分類】
【出願番号】特願２００６−２６１７７（Ｐ２００６−２６１７７）
【出願日】平成１８年２月２日（２００６．２．２）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成１８年９月１４日（２００６．９．１４）
【国際特許分類】
【出願日】平成１８年２月２日（２００６．２．２）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]