JP2009168626A - 位置姿勢計測装置、位置姿勢計測方法 - Google Patents

Abstract

【課題】 計測対象物体の位置及び姿勢の計測を、安定性と精度とを両立して行う為の技術を提供すること。
【解決手段】 センサ計測値入力部１５０は位置姿勢センサ１４０から撮像装置１３０の位置及び姿勢の計測値を取得する。画像入力部１６０は撮像装置１３０による撮像画像を取得する。指標検出部１１０は撮像画像に含まれている指標の画像座標を検出する。位置姿勢補正部１２０は、撮像画像に含まれている指標の現実空間中の位置を計測値に基づいて撮像画像上に投影した位置を推定画像座標として求める。そして、画像座標と推定画像座標との誤差から成る行列と、投影の為に用いた関数を用いて計算される画像ヤコビアンから成る行列と、計測値の補正量と、からなる連立方程式の逆問題を解くことで補正量を求める際には、正則化処理を伴う行列演算を利用する。
【選択図】 図１

Description

本発明は、計測対象物の位置姿勢を計測するための技術に関するものである。

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

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

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

一方、撮像装置に６自由度位置姿勢センサを装着し、これによって計測した撮像装置の位置及び姿勢の誤差を、指標を含む撮像画像を用いて得られる情報（画像情報）によって補正する試みがなされている（特許文献１、特許文献２、特許文献３）。６自由度位置姿勢センサとしては、磁気センサや超音波センサなどが用いられる。

特許文献２で開示されている方法では、撮像画像から指標が検出された場合にはその情報に基づいて６自由位置姿勢センサによる計測値の誤差が補正される。一方、指標が検出されない場合には、６自由位置姿勢センサによる計測値がそのまま撮像装置の位置及び姿勢として使用される。係る方法によれば、指標の検出の有無に関わらずに撮像装置の位置及び姿勢が得られるため、複合現実感の提示を安定して行うことができる。

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

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

また、特許文献３では、画像上における指標の分布を考慮し、センサ計測値の補正方法を適応的に切り替える手法を提案している。この手法では、画像上の一部の領域に指標が偏在して観測されている場合には、センサによって計測された撮像装置の位置あるいは姿勢のいずれかのみを、画像情報に基づいて補正する。これにより、画像から得られる情報量が不十分な場合であっても、安定した解を得ることができる。一方、十分に広い範囲で指標が観測されている場合には、補正するパラメータを限定しない。これにより、画像から得られる情報量が十分な場合には、より正確な位置姿勢を得ることができる。
加藤ら："マーカー追跡に基づく拡張現実感システムとそのキャリブレーション",日本バーチャルリアリティ学会論文誌, vol.4, no.4, pp.607-616, 1999. J. Park, B. Jiang, and U. Neumann: "Vision-based pose computation: robust and accurate augmented reality tracking," Proc. 2nd International Workshop on Augmented Reality (IWAR'99), pp.3-12, 1999. D. G. Lowe: "Fitting parameterized three−dimensional models to images," IEEE Transactions on PAMI, vol.13, no.5, pp.441−450, 1991. 特開2003-222509号公報 特開2003-279310号公報 特開2006-215924号公報

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

一方、上記特許文献１に開示の手法は、精度よりも安定性を重視した手法であり、画像上の一部の領域に指標が偏在して観測されている場合のように、画像情報が不十分な状況下であっても、特許文献２に開示の手法と比較して安定した計測を行うことができる。しかし、十分な画像情報が得られている場合であっても、一部のパラメータの補正のみしか行わないので、画像情報を最大限に生かした精度を得ることができなかった。

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

そして、上記特許文献３に開示の手法では、指標の分布が不十分な場合に未知数とするパラメータを予め定義しておく必要があった。また、分布の良否を判別する閾値等を予め設定しておく必要があった。そのため、画像から得られる情報を生かしきれているとはいえなかった。

本発明は以上の問題に鑑みてなされたものであり、計測対象物体の位置及び姿勢の計測を、安定性と精度とを両立して行う為の技術を提供することを目的とする。

本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は以下の構成を備える。

即ち、複数の指標が配されている現実空間を撮像する撮像装置の位置及び姿勢を計測する位置姿勢計測装置であって、
前記撮像装置に取り付けられたセンサから、前記撮像装置の位置及び姿勢の計測値を取得する手段と、
前記撮像装置による撮像画像を取得する手段と、
前記撮像画像に含まれている指標の画像座標を検出する手段と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める手段と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正手段と、
前記補正手段により補正された計測値を、前記撮像装置の位置及び姿勢として出力する手段と、
を備え、
前記補正手段は、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする。

本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は以下の構成を備える。

即ち、複数の指標が配されている計測対象物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記計測対象物体に取り付けられたセンサから、前記計測対象物体の位置及び姿勢の計測値を取得する手段と、
前記計測対象物体を撮像する撮像装置による撮像画像を取得する手段と、
前記撮像画像に含まれている指標の画像座標を検出する手段と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める手段と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正手段と、
前記補正手段により補正された計測値を、前記計測対象物体の位置及び姿勢として出力する手段と、
を備え、
前記補正手段は、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする。

本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。

即ち、複数の指標が配されている現実空間を撮像する撮像装置の位置及び姿勢を計測する位置姿勢計測装置が行う位置姿勢計測方法であって、
前記撮像装置に取り付けられたセンサから、前記撮像装置の位置及び姿勢の計測値を取得する工程と、
前記撮像装置による撮像画像を取得する工程と、
前記撮像画像に含まれている指標の画像座標を検出する工程と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める工程と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正工程と、
前記補正工程で補正された計測値を、前記撮像装置の位置及び姿勢として出力する工程と、
を備え、
前記補正工程では、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする。

本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。

即ち、複数の指標が配されている計測対象物体の位置及び姿勢を計測する位置姿勢計測装置が行う位置姿勢計測方法であって、
前記計測対象物体に取り付けられたセンサから、前記計測対象物体の位置及び姿勢の計測値を取得する工程と、
前記計測対象物体を撮像する撮像装置による撮像画像を取得する工程と、
前記撮像画像に含まれている指標の画像座標を検出する工程と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める工程と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正工程と、
前記補正工程で補正された計測値を、前記計測対象物体の位置及び姿勢として出力する工程と、
を備え、
前記補正工程では、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする。

本発明の構成によれば、計測対象物体の位置及び姿勢の計測を、安定性と精度とを両立して行うことができる。

以下、添付図面を参照し、本発明の好適な実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の１つである。

［第１の実施形態］
図１は、本実施形態に係るシステムの機能構成例を示すブロック図である。図１に示す如く、本実施形態に係るシステムは、位置姿勢計測装置１００、撮像装置１３０、位置姿勢センサ１４０により構成されている。また、位置姿勢計測装置１００は、指標検出部１１０、位置姿勢補正部１２０、センサ計測値入力部１５０、画像入力部１６０、により構成されている。

また、係るシステムが設けられている空間（室内等の空間）には、複数個の指標（図１ではＱ１〜Ｑ６）が配されている。

先ず、指標Ｑ１〜Ｑ６について説明する。指標Ｑ１〜Ｑ６は、撮像装置１３０による撮像対象として現実空間中に配されるものであって、指標Ｑ１〜Ｑ６のそれぞれについて、基準座標系における位置は既知である。ここで、基準座標系については上述の通りであるが、現実空間中の１点を原点として定義し、係る原点で互いに直交する３軸をそれぞれｘ軸、ｙ軸、ｚ軸とする座標系のことである。

なお、図１では、指標はＱ１〜Ｑ６の６個を示しているが、係る個数は６個に限定するものではなく、一般には１個以上の指標Ｑｋ（ｋ＝１，２，・・・，Ｋ）が現実空間中に配されていれば良い。図１の例では、Ｋ＝６、すなわち６個の指標Ｑ１〜Ｑ６が配されている状況を示している。

指標Ｑｋは、例えば、それぞれが異なる色を有する同一形状（図１では円形）のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する四角形の単色領域によって形成されるような、四角形指標を用いることも可能である。

しかし、指標の形態は、以上例示したものに限定しない。即ち、撮像装置１３０による撮像画像上における投影像の画像座標が検出可能であって、撮像画像中のそれぞれの指標がいずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。また、指標は故意に設定されたものであっても良いし、故意に設定されたものではない、自然形状のものを用いても良い。

次に、撮像装置１３０について説明する。

撮像装置１３０は、現実空間の動画像を撮像するものであり、撮像した各フレームの画像（現実空間画像）は順次、位置姿勢計測装置１００が有する画像入力部１６０に送出する。また、撮像装置１３０には、６自由度センサである位置姿勢センサ１４０が取り付けてある。

次に、係る位置姿勢センサ１４０について説明する。位置姿勢センサ１４０は、基準座標系における撮像装置１３０の位置及び姿勢を計測する。位置姿勢センサ１４０は、計測した計測値（撮像装置１３０の位置及び姿勢の計測値）は、位置姿勢計測装置１００が有するセンサ計測値入力部１５０に送出する。以下では、位置姿勢センサ１４０が計測した撮像装置１３０の位置及び姿勢の計測値を「位置姿勢情報」と呼称する場合がある。

位置姿勢センサ１４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。なお、位置姿勢センサ１４０は、位置及び姿勢が計測できるのであれば、どのように構成されていてもよい。なお、位置姿勢センサ１４０によって計測される位置及び姿勢は、磁場の歪み等の影響によって誤差を含んだものとなっている点に注意されたい。

次に、位置姿勢計測装置１００について説明する。

画像入力部１６０は、撮像装置１３０から順次送出される各フレームの画像を受けると、これをディジタルデータに変換してから後段の指標検出部１１０に送出する。

センサ計測値入力部１５０は、位置姿勢センサ１４０から位置姿勢情報を受けると、これを後段の位置姿勢補正部１２０に送出する。

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

なお、撮像画像中に映っているそれぞれの指標の識別子と画像座標を検出するための技術は、周知の技術であるので、係る技術についてのこれ以上の説明は省略する。

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

位置姿勢補正部１２０は、センサ計測値入力部１５０から送出された位置姿勢情報（撮像装置１３０の位置及び姿勢の計測値）と、指標検出部１１０から送出された各々の指標Ｑｋｎの識別子ｋｎと画像座標ｕ^Ｑｋｎを受ける。そして、これらの情報に基づいて、撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

図３は、位置姿勢計測装置１００が有する位置姿勢補正部１２０が行う処理のフローチャートである。

先ず、ステップＳ３０００において、位置姿勢補正部１２０は、指標検出部１１０から送出された指標Ｑｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋｎを取得する。なお、指標Ｑｋｎの基準座標系における配置位置情報（３次元座標）ｘ_Ｗ ^Ｑｋｎは、既知の値として位置姿勢補正部１２０が保持しているものとする。

次に、ステップＳ３０１０において、位置姿勢補正部１２０は、センサ計測値入力部１５０から送出された計測値（撮像装置１３０の位置及び姿勢の計測値）を位置姿勢情報として取得する。

次に、ステップＳ３０２０において、位置姿勢補正部１２０は、指標検出部１１０が検出した全ての指標についての誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。係る算出の過程では、補正値の算出に用いる行列とその転置行列の積を正則化する。なお、ステップＳ３０２０における処理の詳細については後述する。

次に、ステップＳ３０３０において、位置姿勢補正部１２０は、ステップＳ３０２０で算出した撮像装置１３０の位置及び姿勢のデータを本装置の外部に対して送出する。なお、係るデータの取り扱いについては、本装置の外部に送出することに限定するものではない。例えば、係るデータを、他のアプリケーションから利用可能な状態として位置姿勢計測装置１００が保持しておいても良い。

次に、ステップＳ３０４０において、位置姿勢補正部１２０は、処理を終了するか否かの判定を行う。係る判定は、例えば、位置姿勢計測装置１００の操作者が本処理の終了指示を入力したか否かを位置姿勢補正部１２０が判定することで行っても良いし、本処理の終了条件が満たされたか否かを位置姿勢補正部１２０が判定することで行っても良い。係る判定の結果、終了する場合には、本処理を終了させるし、終了しない場合には、処理をステップＳ３０００に戻し、以降の処理を繰り返す。

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

先ず、ステップＳ４０００において、位置姿勢補正部１２０は、上記ステップＳ３０１０で取得した位置姿勢情報を、ベクトルsの初期値に設定する。

次に、ステップＳ４０１０において、位置姿勢補正部１２０は、指標Ｑｋｎの画像座標の推定値（推定画像座標）ｕ^Ｑｋｎ＊を算出する。ｕ^Ｑｋｎ＊の算出は、ベクトルsによって定義される指標の観測方程式、即ち、指標Ｑｋｎの基準座標系における座標ｘ_Ｗ ^Ｑｋｎから画像座標を算出する為の関数（式１）を用いて行う。

具体的には、観測方程式Ｆｃ（）は、以下の式２と式３とで構成されている。

式２は、ｘ_Ｗ ^Ｑｋｎとsから指標Ｑｋｎのカメラ座標系における座標ｘ_Ｃ ^Ｑｋｎを求める式である。カメラ座標系とは、撮像装置１３０の視点位置を原点として定義し、係る原点で互いに直交する３軸をそれぞれＸ軸、Ｙ軸、Ｚ軸として定義した座標系である。

また、式３は、座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める式である。

上記式２，３において、t_WCは、撮像装置１３０の位置計測値（平行移動成分）を表す３次元ベクトルであり、ベクトルsを構成するパラメータの一部として取り扱う。また、ｆ^Ｃ _ｘ及びｆ^Ｃ _ｙはそれぞれ、ｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として位置姿勢補正部１２０が保持している。

即ち、撮像画像に含まれている指標の現実空間中における位置を、位置姿勢センサ１４０による計測値に基づいて撮像画像上に投影した位置を、推定画像座標として求める。

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

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

次に、ステップＳ４０４０において、位置姿勢補正部１２０は、ステップＳ４０２０で求めた誤差Δｕ^Ｑｋｎと、ステップＳ４０３０で求めた画像ヤコビアンＪ_ｕｓ ^Ｑｋｎを用いて、ベクトルsの補正量Δsを算出する。具体的には先ず、誤差Δｕ^Ｑｋｎを垂直に並べた２Ｎ次元の誤差ベクトル（式６）と、画像ヤコビアンＪ_ｕｓ ^Ｑｋｎを垂直に並べた２Ｎ行×３列の行列（ヤコビ行列）（式７）を作成する。

そして、Θの正則化一般化逆行列Θ+を用いて、以下の式８に基づいて補正量Δｓを算出する（演算処理）。

このように、本実施形態では補正量Δsを、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法を用いて算出している。

ここで、連立方程式の逆問題（式８）を解く際に、正則化処理を伴う行列演算を利用することで、Θの一般化逆行列を安定して得る。具体的には、Θの一般化逆行列（式９）の代わりに、Θの正則化一般化逆行列（式１０）を用いる。

すなわち、正則化項μ²Ｐを付加することで、ヤコビ行列Θとその転置行列Θ^Ｔとの積行列であるΘ^ＴΘを正則化する（したがって、Θ^ＴΘ＋μ²Ｐは正則行列となる）。これにより、一般化逆行列を安定して得ることができる。ここでＰは、ベクトル（推定値）ｓの長さ制限や滑らかさ制限などの何らかのコスト関数を表現する正方行列である。要素毎に重みを付ける必要がなく、単純に初期値からのずれを評価する場合には、Ｐとして単位行列を用いればよい。またμは、行列Θ^ＴΘの条件数に基づいて算出する微小な定数である。

次に、ステップＳ４０５０において、位置姿勢補正部１２０は、式１１に示す如く、ステップＳ４０４０で求めた補正量Δsを用いてベクトルsを補正し、補正したベクトルｓを新たな推定値とする。

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

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

以上説明した処理によれば、撮像装置１３０の位置及び姿勢を未知のパラメータとして、位置姿勢センサ１４０による計測値の誤差を補正することができ、その際に正則化によって過剰な補正を抑制することができる。

そして、本実施形態によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行うのではなく、全てのパラメータを補正の対象として、画像情報に応じた補正を行う。そのため、従来と比べて高精度な計測を行うことができる。

なお、本実施形態では、撮像装置の位置及び姿勢の計測を行っていたが、撮像装置をある物体に装着した場合には、上述の方法で撮像装置の位置及び姿勢を計測することで、この物体の位置及び姿勢を求めることもできる。

［第２の実施形態］
第１の実施形態では、室内等の空間中における撮像装置の位置及び姿勢の計測を行っていた。しかし、正則化の考え方は、撮像装置以外の対象物体の位置及び姿勢を計測する場合であっても適用できる。

図５は、本実施形態に係るシステムの機能構成例を示すブロック図である。図５において、図１に示したものと同じものについては同じ参照番号を割り当てている。本実施形態に係る位置姿勢計測装置５００は、撮像装置１３０と計測対象物体５７０との間の相対位置姿勢を計測する。以下、第１の実施形態と異なる点についてのみ説明する。

計測対象物体５７０上の複数の位置には、物体座標系（計測対象物体５７０上に定義される座標系）における位置が既知である複数個の指標Ｑｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。図５では、Ｋ＝９、すなわち９個の指標Ｑ１〜Ｑ９が配置されている状況を示している。

指標Ｑｋは、第１の実施形態と同様に、撮像装置１３０による撮像画像上における投影像の画像座標が検出可能であって、撮像画像中のそれぞれの指標がいずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。

位置姿勢センサ１４０ａは、図１の位置姿勢センサ１４０と同じものであり、撮像装置１３０の位置及び姿勢を計測すべく、撮像装置１３０に取り付けてある。位置姿勢センサ１４０ａは、撮像装置１３０の位置及び姿勢を計測すると、その計測値（撮像装置１３０の位置姿勢情報）を、位置姿勢計測装置５００が有するセンサ計測値入力部１５０に対して送出する。

位置姿勢センサ１４０ｂは、計測対象物体５７０の位置及び姿勢を計測すべく、計測対象物体５７０に取り付けてある。位置姿勢センサ１４０ｂは、計測対象物体５７０の位置及び姿勢を計測すると、その計測値を、位置姿勢計測装置５００が有するセンサ計測値入力部１５０に対して送出する。

センサ計測値入力部１５０は、位置姿勢センサ１４０ａ、１４０ｂのそれぞれから送出された計測値を、後段の位置姿勢補正部５２０に対して送出する。

位置姿勢補正部５２０は、センサ計測値入力部１５０から送出された計測値と、指標検出部１１０から送出された各々の指標Ｑｋｎの識別子ｋｎと画像座標ｕ^Ｑｋｎを受ける。そして、これらの情報に基づいて、撮像装置１３０若しくは計測対象物体５７０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。

図６は、位置姿勢計測装置５００が有する位置姿勢補正部５２０が行う処理のフローチャートである。

先ず、ステップＳ６０００において、位置姿勢補正部５２０は、指標検出部１１０から送出された指標Ｑｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋｎを取得する。なお、指標Ｑｋの基準座標系における配置位置情報（３次元座標）ｘ_Ｗ ^Ｑｋｎは、既知の値として位置姿勢補正部５２０が保持しているものとする。

次に、ステップＳ６０１０において、位置姿勢補正部５２０は、センサ計測値入力部１５０から送出された計測値（撮像装置１３０の位置及び姿勢の計測値と、計測対象物体５７０の位置及び姿勢の計測値）を取得する。

次に、ステップＳ６０２０において、位置姿勢補正部５２０は、指標検出部１１０が検出した全ての指標についての誤差の和を最小とするような撮像装置１３０の位置と姿勢、計測対象物体５７０の位置と姿勢、を算出する。係る算出の過程では、第１の実施形態と同様にして、補正値の算出に用いる行列とその転置行列の積を正則化する。

ステップＳ６０２０における処理のうち、撮像装置１３０の位置及び姿勢を補正するための処理は、第１の実施形態で説明したステップＳ３０２０における処理、即ち、図４に示したフローチャートに従った処理と基本的には同じであるが、下記の点が異なる。

即ち、本実施形態の場合、指標の観測方程式Ｆ'ｃ（）が、物体座標系における指標Ｑｋｎの座標ｘ_Ｏ ^Ｑｋｎ（既知な情報として位置姿勢補正部５２０が保持している）から画像座標を算出する関数となる。すなわち、観測方程式Ｆ'ｃ（）は下記の式１２によって表される。

具体的には、観測方程式Ｆ'ｃ（）は、ｘ_Ｏ ^Ｑｋｎとベクトルｓから指標Ｑｋｎの座標ｘ_Ｃ ^Ｑｋｎを求める式（式１３）と、座標ｘ_Ｃ ^Ｑｋｎから画像座標ｕ^Ｑｋｎ＊を求める上記（式３）によって構成されている。そして、本実施形態は、係る点のみが第１の実施形態と異なっている。

ここで、t_WC、t_WOはそれぞれ、撮像装置１３０と計測対象物体５７０の夫々の位置計測値（平行移動成分）を表す３次元ベクトルであり、ここでは固定値として取り扱われる。また、R_WOは計測対象物体５７０の姿勢計測値（回転成分）を表す３×３の回転行列であり、同様に固定値として取り扱われる。

なお、ステップＳ６０２０では更に、計測対象物体５７０の位置及び姿勢を補正する処理を行うのであるが、係る処理は次の通りである。

先ず、以下に述べる座標変換処理を行った後に、変換後の位置及び姿勢を出力する。

同次座標系を用いた４×４の座標変換行列によって位置及び姿勢を表記すると、計測対象物体５７０の補正後の位置及び姿勢M_WOは、下記の式１４によって算出される。

ここで、M^# _WCは、位置姿勢センサ１４０ａから得られた計測値（撮像装置１３０の位置及び姿勢）である。以下、^#は、位置姿勢センサ１４０ａ、位置姿勢センサ１４０ｂの何れかのセンサによる計測値を表す記号とする。また、M^# _WOは、位置姿勢センサ１４０ｂから得られた計測値（計測対象物体５７０の位置及び姿勢）であり、M_WCは、ステップＳ６０２０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢である。

このとき、撮像装置１３０によって定義されるカメラ座標系における計測対象物体５７０の位置及び姿勢は、下記の式１５に示す式に従ったものとなる。

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

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

なお、以上の説明では、ステップＳ６０２０では、撮像装置１３０の位置及び姿勢、計測対象物体５７０の位置及び姿勢、の両方に対して補正したものを出力するものとしているが、何れか一方のみを補正し、他方については補正しなくても良い。

次に、ステップＳ６０３０において、位置姿勢補正部５２０は、ステップＳ６０２０で算出した撮像装置１３０の位置及び姿勢のデータ、計測対象物体５７０の位置及び姿勢のデータ、を本装置の外部に対して送出する。なお、係るデータの取り扱いについては、第１の実施形態と同様に、本装置の外部に送出することに限定するものではない。例えば、係るデータを、他のアプリケーションから利用可能な状態として位置姿勢計測装置５００が保持しておいても良い。

次に、ステップＳ６０４０において、位置姿勢補正部５２０は、処理を終了するか否かの判定を行う。係る判定は、例えば、位置姿勢計測装置５００の操作者が本処理の終了指示を入力したか否かを位置姿勢補正部５２０が判定することで行っても良いし、本処理の終了条件が満たされたか否かを位置姿勢補正部５２０が判定することで行っても良い。係る判定の結果、終了する場合には、本処理を終了させるし、終了しない場合には、処理をステップＳ６０００に戻し、以降の処理を繰り返す。

以上の説明により、本実施形態によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行うのではなく、全てのパラメータを補正の対象として画像情報に応じた補正を行う。そのため、従来と比べて高精度な計測を行うことができる。

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

本実施形態に係るシステムでは、撮像装置には位置姿勢センサは装着されておらず、代わりに、撮像装置は、位置及び姿勢が既知の場所に三脚等によって固定して設置されていることが、第１の実施形態、第２の実施形態とは異なっている。

以下では、第２の実施形態との相違点についてのみ説明する。

図７は、本実施形態に係るシステムの機能構成例を示すブロック図である。図７において図５に示したものと同じものについては同じ参照番号を割り当てている。

撮像装置１３０は、三脚７８０によって、現実空間中の既知の位置に、既知の姿勢でもって固定して設置されている。撮像装置１３０の基準座標系における既知の位置及び姿勢は、データとして位置姿勢計測装置７００が保持している。また、撮像装置１３０の動作については第１の実施形態と同様であり、撮像画像を後段の位置姿勢計測装置７００が有する画像入力部１６０に送出する。

位置姿勢補正部５２０は、第２の実施形態で説明した処理と同じ処理を行うのであるが、唯一の違いは、撮像装置１３０の位置及び姿勢が、計測値として与えられるのではなく、既知の値として得られている点にある。従って、本実施形態と第２の実施形態とで、実質的な処理の違いはない。

本実施形態では先ず、位置姿勢補正部５２０は、撮像装置１３０の位置及び姿勢の既知の値を、撮像装置１３０の位置及び姿勢の仮の計測値とする。そして次に、第２の実施形態で説明したステップＳ６０００〜ステップＳ６０２０の処理を行うことで、撮像装置１３０の位置及び姿勢の計測値の補正を行う。そして、この補正結果に基づいて、ステップＳ６０３０において、第２の実施形態と同様に計測対象物体５７０の位置及び姿勢の計測値を補正する（式１４）。本実施形態では、位置姿勢補正部５２０がステップＳ６０３０において出力するのは、計測対象物体５７０の位置及び姿勢のみである。

以上の説明により、本実施形態によれば、計測対象物体の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行うのではなく、全てのパラメータを補正の対象として画像情報に応じた補正を行う。そのため、従来と比べて高精度な計測を行うことができる。

＜変形例１＞
第１の実施形態では、式１０で表される一般化逆行列を用いて正則化を行ったが、正則化の方法はこれに限られるものではない。例えば、式９における画像ヤコビアンΘとその転置行列Θ^Tとの積Θ^TΘを固有値分解し、その微小固有値を０としてランクを低減させる方法を用いても良い。

具体的には、まずΘ^TΘを固有値分解し、式１６に示した結果を得る。

ここで、Λは対角要素にΘ^TΘの固有値が配置された対角行列、Ｖは対応する固有ベクトルからなる行列を表す。

次に、Λの微小な固有値を０としてランクを低減させたものを行列Λ’とする。ここで、微小な固有値とは、丸め誤差に埋もれて正確な値が分からないような、数値的に非常に小さな固有値のことを表す。

そして、再びΘ^TΘを、以下の式１７によって算出する。

そして、算出結果を元のΘ^TΘの代わりに式９に代入して用いる。

＜変形例２＞
変形例１では、式９における画像ヤコビアンΘとその転置行列Θ^Tとの積Θ^TΘの微小固有値を０としたが、Θ^TΘの固有値が小さい順に、予め定めた数であるｎ個（ｎ＝１〜５）を０としてもよい。例えば、Θ^TΘの固有値が小さい順に３個または４個を０とする。そうすることにより、センサによって計測された撮像装置の位置あるいは姿勢のいずれか（２または３自由度）のみを補正する特許文献２の方法と同様に、撮像装置の位置及び姿勢を安定化させることができる。

＜変形例３＞
第１の実施形態におけるステップＳ３０２０では、図４のステップＳ４０４０にて詳細に説明したように、誤差ベクトルＵと行列Θに基づく補正量Δsの算出に、式８で表現されるＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いている。しかし、補正量Δsの算出は必ずしもＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いて行わなくてもよい。例えば、公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を用いて求めてもよいし、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよい。さらに、他の何れの数値計算手法を適用しても良い。

何れの数値計算においても、正則化を伴う行列演算を利用することにより、補正量Δsを安定して得ることができる。また、上記の実施形態におけるsを状態ベクトル、式１を観測方程式とすることで、画像ヤコビアンを利用して位置姿勢センサによる計測値を補正することができる。すなわち、そのような効果を有した拡張カルマンフィルタ（Extended Kalman Filter）や、反復拡張カルマンフィルタ（iterative Extended Kalman Filter）を構成することができる。なお、拡張カルマンフィルタ及び反復拡張カルマンフィルタについては非特許文献２に記載されるように周知であるので、その詳細に関するこれ以上の説明は省略する。

＜変形例４＞
第２の実施形態、又は第３の実施形態で用いる計測対象物体には、情景を撮像するカメラ（主観視点カメラ）を用いることも可能である。その場合、上記の実施形態の夫々で述べた方法で指標の３次元位置を算出した上で、特開２００７−４０９８０号公報に開示されているような方法で、主観視点カメラの位置及び姿勢を算出することが可能である。この場合、主観視点カメラで撮影した画像に、主観視点カメラの位置及び姿勢に応じて生成したＣＧ画像を重畳描画し、これを画像表示装置に表示することで、複合現実感の提示を行うことができる。

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

＜変形例６＞
上記各実施形態では、位置姿勢計測装置１００，５００，７００を構成する各部は全て、暗にハードウェアで構成されているものとして説明したが、これら各部のうち一部若しくは全部をソフトウェアでもって構成しても良い。

例えば、位置姿勢計測装置１００，５００，７００を構成する各部をソフトウェアで実装した場合、位置姿勢計測装置１００，５００，７００には、一般のＰＣ（パーソナルコンピュータ）を適用することができる。この場合、係るＰＣのメモリには係るソフトウェアをインストールし、係るＰＣのＣＰＵは、このインストールされたソフトウェアを実行する。これにより、係るＰＣは、位置姿勢計測装置１００や位置姿勢計測装置５００、位置姿勢計測装置７００が行うものとして上述した各処理を実行することになる。

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

ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行う。また、係るコンピュータを位置姿勢計測装置１００に適用した場合には、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０のそれぞれが行うものとして上述した各処理を実行する。また、係るコンピュータを位置姿勢計測装置５００や位置姿勢計測装置７００に適用した場合には、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０のそれぞれが行うものとして上述した各処理を実行する。

ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶する為のエリアを有する。まら、ＲＡＭ１００２は、Ｉ／Ｆ（インターフェース）１００９を介して撮像装置１３０から受信した撮像画像のデータや、位置姿勢センサ１４０（１４０ａ、１４０ｂ）から受信した計測値のデータ等を一時的に記憶するためのエリアを有する。更に、ＲＡＭ１００２は、ＣＰＵ１００１が各種の処理を実行する際に用いるワークエリアを有する。即ち、ＲＡＭ１００２は、各種のエリアを適宜提供することができる。

ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データ等を格納する。

キーボード１００４、マウス１００５は入力デバイスであり、操作者はこれらを用いて、各種の指示をＣＰＵ１００１に対して入力することができる。

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

外部記憶装置１００７は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置１００７には、ＯＳ（オペレーティングシステム）が保存されている。更に、外部記憶装置１００７には、コンピュータの適用対象（位置姿勢計測装置１００、５００、７００の何れか）が行うものとして上述した各処理をＣＰＵ１００１に実行させるためのプログラム（コンピュータプログラム）やデータ等が保存されている。

コンピュータを位置姿勢計測装置１００に適用した場合、係るプログラムには、ＣＰＵ１００１を、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０のそれぞれとして機能させるためのプログラムが含まれている。また、コンピュータを位置姿勢計測装置５００や位置姿勢計測装置７００に適用した場合、係るプログラムには、次のようなプログラムが含まれている。即ち、ＣＰＵ１００１を、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部５２０のそれぞれとして機能させるためのプログラムが含まれている。

また、外部記憶装置１００７に保存されているデータには、上述の説明において、既知の情報として説明したものが保存されている。

外部記憶装置１００７に保存されているプログラムやデータは、ＣＰＵ１００１による制御に従って、適宜ＲＡＭ１００２にロードされ、ＣＰＵ１００１による処理対象となる。

記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出し、ＲＡＭ１００２や外部記憶装置１００７に出力する。なお、外部記憶装置１００７に保存されているものとして説明した情報の一部を係る記憶媒体に記録させるようにしても良い。

Ｉ／Ｆ１００９は、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポートや、情報を外部へ出力するためのイーサネットポート等によって構成されている。また、位置姿勢センサ１４０（１４０ａ、１４０ｂ）を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポートもＩ／Ｆ１００９に含まれる。画像入力部１６０及びセンサ計測値入力部１５０の機能の一部は、Ｉ／Ｆ１００９によって実現される。

上述した各構成要素は、バス１０１０によって相互に接続される。

なお、コンピュータのハードウェア構成についてはこれに限定するものではなく、様々な構成が考え得る。

また、図１に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０の少なくとも一部を、独立した装置として実現し、位置姿勢計測装置１００と係る装置との間でデータ通信を行うようにしても良い。

また、図５や図７に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部５２０の少なくとも一部を、独立した装置として実現し、位置姿勢計測装置５００（７００）と係る装置との間でデータ通信を行うようにしても良い。

また、以上説明した様々な実施形態や変形例は、適宜組み合わせて実施しても良い。

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

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

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

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

本発明の第１の実施形態に係るシステムの機能構成例を示すブロック図である。 位置姿勢計測装置１００、５００、７００に適用可能なコンピュータのハードウェア構成例を示すブロック図である。 位置姿勢計測装置１００が有する位置姿勢補正部１２０が行う処理のフローチャートである。 ステップＳ３０２０における処理の詳細を示すフローチャートである。 本発明の第２の実施形態に係るシステムの機能構成例を示すブロック図である。 位置姿勢計測装置５００が有する位置姿勢補正部５２０が行う処理のフローチャートである。 本発明の第３の実施形態に係るシステムの機能構成例を示すブロック図である。

Claims (10)

1. 複数の指標が配されている現実空間を撮像する撮像装置の位置及び姿勢を計測する位置姿勢計測装置であって、
   前記撮像装置に取り付けられたセンサから、前記撮像装置の位置及び姿勢の計測値を取得する手段と、
   前記撮像装置による撮像画像を取得する手段と、
   前記撮像画像に含まれている指標の画像座標を検出する手段と、
   前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める手段と、
   前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正手段と、
   前記補正手段により補正された計測値を、前記撮像装置の位置及び姿勢として出力する手段と、
   を備え、
   前記補正手段は、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測装置。
2. 複数の指標が配されている計測対象物体の位置及び姿勢を計測する位置姿勢計測装置であって、
   前記計測対象物体に取り付けられたセンサから、前記計測対象物体の位置及び姿勢の計測値を取得する手段と、
   前記計測対象物体を撮像する撮像装置による撮像画像を取得する手段と、
   前記撮像画像に含まれている指標の画像座標を検出する手段と、
   前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める手段と、
   前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正手段と、
   前記補正手段により補正された計測値を、前記計測対象物体の位置及び姿勢として出力する手段と、
   を備え、
   前記補正手段は、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測装置。
3. 前記補正手段は、前記画像ヤコビアンからなる行列とその転置行列との積行列に正則化処理を加えることで正則行列を得、前記補正量の算出に当該正則行列を利用することを特徴とする請求項１又は２に記載の位置姿勢計測装置。
4. 前記補正手段は、前記積行列に正方行列を付加することで当該積行列の正則化を行うことを特徴とする請求項３に記載の位置姿勢計測装置。
5. 前記補正手段は、前記積行列の固有値のうち予め定めた値よりも小さな固有値を０とすることで、前記積行列の正則化を行うことを特徴とする請求項４に記載の位置姿勢計測装置。
6. 前記補正手段は、前記積行列の固有値のうち値の小さい順から、予め定めた数の固有値を０とすることで、前記積行列の正則化を行うことを特徴とする請求項４に記載の位置姿勢計測装置。
7. 複数の指標が配されている現実空間を撮像する撮像装置の位置及び姿勢を計測する位置姿勢計測装置が行う位置姿勢計測方法であって、
   前記撮像装置に取り付けられたセンサから、前記撮像装置の位置及び姿勢の計測値を取得する工程と、
   前記撮像装置による撮像画像を取得する工程と、
   前記撮像画像に含まれている指標の画像座標を検出する工程と、
   前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める工程と、
   前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正工程と、
   前記補正工程で補正された計測値を、前記撮像装置の位置及び姿勢として出力する工程と、
   を備え、
   前記補正工程では、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測方法。
8. 複数の指標が配されている計測対象物体の位置及び姿勢を計測する位置姿勢計測装置が行う位置姿勢計測方法であって、
   前記計測対象物体に取り付けられたセンサから、前記計測対象物体の位置及び姿勢の計測値を取得する工程と、
   前記計測対象物体を撮像する撮像装置による撮像画像を取得する工程と、
   前記撮像画像に含まれている指標の画像座標を検出する工程と、
   前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める工程と、
   前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正工程と、
   前記補正工程で補正された計測値を、前記計測対象物体の位置及び姿勢として出力する工程と、
   を備え、
   前記補正工程では、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測方法。
9. コンピュータを請求項１乃至６の何れか１項に記載の位置姿勢計測装置が有する各手段として機能させるためのコンピュータプログラム。
10. 請求項９に記載のコンピュータプログラムを格納した、コンピュータ読み取り可能な記憶媒体。

Images