(57)【要約】【課題】  計測対象物体の位置及び姿勢の計測を、安定性と精度とを両立して行う為の技術を提供すること。【解決手段】  センサ計測値入力部１５０は位置姿勢センサ１４０から撮像装置１３０の位置及び姿勢の計測値を取得する。画像入力部１６０は撮像装置１３０による撮像画像を取得する。指標検出部１１０は撮像画像に含まれている指標の画像座標を検出する。位置姿勢補正部１２０は、撮像画像に含まれている指標の現実空間中の位置を計測値に基づいて撮像画像上に投影した位置を推定画像座標として求める。そして、画像座標と推定画像座標との誤差から成る行列と、投影の為に用いた関数を用いて計算される画像ヤコビアンから成る行列と、計測値の補正量と、からなる連立方程式の逆問題を解くことで補正量を求める際には、正則化処理を伴う行列演算を利用する。【選択図】  図１
2009168626.tif 000002
【特許請求の範囲】
【請求項１】
複数の指標が配されている現実空間を撮像する撮像装置の位置及び姿勢を計測する位置姿勢計測装置であって、
前記撮像装置に取り付けられたセンサから、前記撮像装置の位置及び姿勢の計測値を取得する手段と、
前記撮像装置による撮像画像を取得する手段と、
前記撮像画像に含まれている指標の画像座標を検出する手段と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める手段と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正手段と、
前記補正手段により補正された計測値を、前記撮像装置の位置及び姿勢として出力する手段と、
を備え、
前記補正手段は、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測装置。
【請求項２】
複数の指標が配されている計測対象物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記計測対象物体に取り付けられたセンサから、前記計測対象物体の位置及び姿勢の計測値を取得する手段と、
前記計測対象物体を撮像する撮像装置による撮像画像を取得する手段と、
前記撮像画像に含まれている指標の画像座標を検出する手段と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める手段と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正手段と、
前記補正手段により補正された計測値を、前記計測対象物体の位置及び姿勢として出力する手段と、
を備え、
前記補正手段は、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測装置。
【請求項３】
前記補正手段は、前記画像ヤコビアンからなる行列とその転置行列との積行列に正則化処理を加えることで正則行列を得、前記補正量の算出に当該正則行列を利用することを特徴とする請求項１又は２に記載の位置姿勢計測装置。
【請求項４】
前記補正手段は、前記積行列に正方行列を付加することで当該積行列の正則化を行うことを特徴とする請求項３に記載の位置姿勢計測装置。
【請求項５】
前記補正手段は、前記積行列の固有値のうち予め定めた値よりも小さな固有値を０とすることで、前記積行列の正則化を行うことを特徴とする請求項４に記載の位置姿勢計測装置。
【請求項６】
前記補正手段は、前記積行列の固有値のうち値の小さい順から、予め定めた数の固有値を０とすることで、前記積行列の正則化を行うことを特徴とする請求項４に記載の位置姿勢計測装置。
【請求項７】
複数の指標が配されている現実空間を撮像する撮像装置の位置及び姿勢を計測する位置姿勢計測装置が行う位置姿勢計測方法であって、
前記撮像装置に取り付けられたセンサから、前記撮像装置の位置及び姿勢の計測値を取得する工程と、
前記撮像装置による撮像画像を取得する工程と、
前記撮像画像に含まれている指標の画像座標を検出する工程と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める工程と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正工程と、
前記補正工程で補正された計測値を、前記撮像装置の位置及び姿勢として出力する工程と、
を備え、
前記補正工程では、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測方法。
【請求項８】
複数の指標が配されている計測対象物体の位置及び姿勢を計測する位置姿勢計測装置が行う位置姿勢計測方法であって、
前記計測対象物体に取り付けられたセンサから、前記計測対象物体の位置及び姿勢の計測値を取得する工程と、
前記計測対象物体を撮像する撮像装置による撮像画像を取得する工程と、
前記撮像画像に含まれている指標の画像座標を検出する工程と、
前記撮像画像に含まれている指標の現実空間中における位置を前記計測値に基づいて前記撮像画像上に投影した位置を、推定画像座標として求める工程と、
前記画像座標と前記推定画像座標との誤差と、前記投影の為に用いた関数を用いて計算される画像ヤコビアンと、に基づいて、前記計測値の補正量を求める演算処理を行い、当該演算処理によって得られた補正量を用いて前記計測値を補正する補正工程と、
前記補正工程で補正された計測値を、前記計測対象物体の位置及び姿勢として出力する工程と、
を備え、
前記補正工程では、前記誤差から成るベクトルと、前記画像ヤコビアンから成る行列と、前記補正量と、に関する連立方程式の逆問題を、正則化処理を伴う行列演算を利用して解くことで前記補正量を求めることを特徴とする位置姿勢計測方法。
【請求項９】
コンピュータを請求項１乃至６の何れか１項に記載の位置姿勢計測装置が有する各手段として機能させるためのコンピュータプログラム。
【請求項１０】
請求項９に記載のコンピュータプログラムを格納した、コンピュータ読み取り可能な記憶媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、計測対象物の位置姿勢を計測するための技術に関するものである。
【背景技術】
【０００２】
近年、現実空間と仮想空間との繋ぎ目のない結合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像を重畳描画した画像を表示する装置として実現することができる。仮想空間の画像としては、たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報などが用いられる。
【０００３】
このような画像表示装置を実現する為には、現実空間中に定義した基準座標系と、撮像装置の座標系（カメラ座標系）との間の、相対的な位置及び姿勢を計測することが不可欠である。ここで、基準座標系とは、重畳しようとする仮想物体の位置及び姿勢を決定する基準となる現実空間中の座標系のことを表す。これは、仮想物体（仮想空間画像）を現実空間上の位置に合わせて描画する為には、基準座標系に対する撮像装置のカメラパラメータと同一のカメラパラメータを用いて仮想物体の画像を生成しなければならないからである。例えば、現実の室内のある位置に仮想物体を重畳表示する場合には、この室内に基準座標系を定義し、その基準座標系における撮像装置の位置及び姿勢を求めればよい。また、観察者が自身の手に把持している現実の箱に何らかの仮想の模様やラベルを重畳表示する場合には、箱自身の物体座標系を基準座標系と考え、撮像装置に対する箱（基準座標系）の位置及び姿勢を求めればよい。
【０００４】
撮像装置の位置及び姿勢を計測する為の一般的な方法には、次のようなものがある。まず、現実空間中に複数の指標（人為的なマーカや自然特徴など）を配置あるいは設定する。そして、撮像装置によって、係る現実空間を撮像する。そして、この撮像された画像内における指標の投影像の座標を検出し、検出した座標と指標の配置情報との関係に基づいて撮像装置の位置及び姿勢を求める（非特許文献１）。しかし、このアプローチを用いる場合、常に指標が撮像されなくてはならないという制約がある。
【０００５】
一方、撮像装置に６自由度位置姿勢センサを装着し、これによって計測した撮像装置の位置及び姿勢の誤差を、指標を含む撮像画像を用いて得られる情報（画像情報）によって補正する試みがなされている（特許文献１、特許文献２、特許文献３）。６自由度位置姿勢センサとしては、磁気センサや超音波センサなどが用いられる。
【０００６】
特許文献２で開示されている方法では、撮像画像から指標が検出された場合にはその情報に基づいて６自由位置姿勢センサによる計測値の誤差が補正される。一方、指標が検出されない場合には、６自由位置姿勢センサによる計測値がそのまま撮像装置の位置及び姿勢として使用される。係る方法によれば、指標の検出の有無に関わらずに撮像装置の位置及び姿勢が得られるため、複合現実感の提示を安定して行うことができる。
【０００７】
また、特許文献２で開示されている方法では、検出された指標の数が３点以上の場合には、撮像装置の位置及び姿勢の６自由度が画像情報に基づいて算出される。一方、検出された指標の数が２点及び１点の場合には、６自由位置姿勢センサによって計測された撮像装置の位置あるいは姿勢のいずれか（２または３自由度）のみを補正する処理が適用される。すなわち、検出された指標の数を判定基準として、撮像装置の位置及び姿勢の算出に利用するアルゴリズムを切り替えることが行われている。これにより、画像情報のみからでは撮像装置の位置及び姿勢を求めることができなくても（撮像画像中の指標の数が３未満）、６自由位置姿勢センサによる計測値を基準とし、その誤差を極力打ち消すような補正がなされた位置及び姿勢を取得することができる。
【０００８】
一方、特許文献１に開示の方法では、検出された指標の数に関わらず、センサによって計測された撮像装置の位置あるいは姿勢のいずれかのみを画像情報に基づいて補正する処理が適用される。この補正方法では、姿勢を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような回転補正値を個別に求め、それらを平均化することで姿勢計測値に対する補正値が算出される。また、位置を補正する場合には、検出された指標毎に指標上における誤差を打ち消すような平行移動補正値を個別に求め、それらを平均化することで位置計測値に対する補正値が算出される。指標の数に関わらず補正の自由度を２または３に限定するため、情報量が不十分な場合であっても安定した解を得ることができる。
【０００９】
また、特許文献３では、画像上における指標の分布を考慮し、センサ計測値の補正方法を適応的に切り替える手法を提案している。この手法では、画像上の一部の領域に指標が偏在して観測されている場合には、センサによって計測された撮像装置の位置あるいは姿勢のいずれかのみを、画像情報に基づいて補正する。これにより、画像から得られる情報量が不十分な場合であっても、安定した解を得ることができる。一方、十分に広い範囲で指標が観測されている場合には、補正するパラメータを限定しない。これにより、画像から得られる情報量が十分な場合には、より正確な位置姿勢を得ることができる。
【非特許文献１】加藤ら："マーカー追跡に基づく拡張現実感システムとそのキャリブレーション",日本バーチャルリアリティ学会論文誌, 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 = [tWCT  ωWCT]T = [xWC  yWC  zWC  ξWC  ψWC  ζWC]Tによって内部的に表現している。
【００４９】
先ず、ステップＳ４０００において、位置姿勢補正部１２０は、上記ステップＳ３０１０で取得した位置姿勢情報を、ベクトルsの初期値に設定する。
【００５０】
次に、ステップＳ４０１０において、位置姿勢補正部１２０は、指標Ｑｋｎの画像座標の推定値（推定画像座標）ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、ベクトルsによって定義される指標の観測方程式、即ち、指標Ｑｋｎの基準座標系における座標ｘＷＱｋｎから画像座標を算出する為の関数（式１）を用いて行う。
【００５１】
【数１】
2009168626.tif 000003
【００５２】
具体的には、観測方程式Ｆｃ（）は、以下の式２と式３とで構成されている。
【００５３】
【数２】
2009168626.tif 000004
【００５４】
【数３】
2009168626.tif 000005
【００５５】
式２は、ｘＷＱｋｎとsから指標Ｑｋｎのカメラ座標系における座標ｘＣＱｋｎを求める式である。カメラ座標系とは、撮像装置１３０の視点位置を原点として定義し、係る原点で互いに直交する３軸をそれぞれＸ軸、Ｙ軸、Ｚ軸として定義した座標系である。
【００５６】
また、式３は、座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める式である。
【００５７】
上記式２，３において、tWCは、撮像装置１３０の位置計測値（平行移動成分）を表す３次元ベクトルであり、ベクトルsを構成するパラメータの一部として取り扱う。また、ｆＣｘ及びｆＣｙはそれぞれ、ｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として位置姿勢補正部１２０が保持している。
【００５８】
即ち、撮像画像に含まれている指標の現実空間中における位置を、位置姿勢センサ１４０による計測値に基づいて撮像画像上に投影した位置を、推定画像座標として求める。
【００５９】
次に、ステップＳ４０２０において、位置姿勢補正部１２０は、指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差ΔｕＱｋｎを、式４を用いて算出する。
【００６０】
【数４】
2009168626.tif 000006
【００６１】
次に、ステップＳ４０３０において、位置姿勢補正部１２０は、指標Ｑｋｎに対して、ベクトルsに関する画像ヤコビアンを算出する。ここで、画像ヤコビアンとは、式１の観測方程式Ｆｃ（）をベクトルsの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列ＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）のことを表す。具体的には、式２の右辺を座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）を算出する。また、式３の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出する。そして、式５を用いてＪｕｓＱｋｎを算出する。
【００６２】
【数５】
2009168626.tif 000007
【００６３】
次に、ステップＳ４０４０において、位置姿勢補正部１２０は、ステップＳ４０２０で求めた誤差ΔｕＱｋｎと、ステップＳ４０３０で求めた画像ヤコビアンＪｕｓＱｋｎを用いて、ベクトルsの補正量Δsを算出する。具体的には先ず、誤差ΔｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトル（式６）と、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた２Ｎ行×３列の行列（ヤコビ行列）（式７）を作成する。
【００６４】
【数６】
2009168626.tif 000008
【００６５】
【数７】
2009168626.tif 000009
【００６６】
そして、Θの正則化一般化逆行列Θ+を用いて、以下の式８に基づいて補正量Δｓを算出する（演算処理）。
【００６７】
【数８】
2009168626.tif 000010
【００６８】
このように、本実施形態では補正量Δsを、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法を用いて算出している。
【００６９】
ここで、連立方程式の逆問題（式８）を解く際に、正則化処理を伴う行列演算を利用することで、Θの一般化逆行列を安定して得る。具体的には、Θの一般化逆行列（式９）の代わりに、Θの正則化一般化逆行列（式１０）を用いる。
【００７０】
【数９】
2009168626.tif 000011
【００７１】
【数１０】
2009168626.tif 000012
【００７２】
すなわち、正則化項μ2Ｐを付加することで、ヤコビ行列Θとその転置行列ΘＴとの積行列であるΘＴΘを正則化する（したがって、ΘＴΘ＋μ2Ｐは正則行列となる）。これにより、一般化逆行列を安定して得ることができる。ここでＰは、ベクトル（推定値）ｓの長さ制限や滑らかさ制限などの何らかのコスト関数を表現する正方行列である。要素毎に重みを付ける必要がなく、単純に初期値からのずれを評価する場合には、Ｐとして単位行列を用いればよい。またμは、行列ΘＴΘの条件数に基づいて算出する微小な定数である。
【００７３】
次に、ステップＳ４０５０において、位置姿勢補正部１２０は、式１１に示す如く、ステップＳ４０４０で求めた補正量Δsを用いてベクトルsを補正し、補正したベクトルｓを新たな推定値とする。
【００７４】
【数１１】
2009168626.tif 000013
【００７５】
次に、ステップＳ４０６０において、位置姿勢補正部１２０は、誤差ベクトルＵが予め定めた閾値より小さいか否か、或いは、補正量Δsが予め定めた閾値より小さいか否かといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。係る判定の結果、収束してないと判定した場合には、ステップＳ４０５０で補正したベクトルｓを用いて再度、ステップＳ４０１０以降の処理を行う。一方、収束していると判定した場合には、処理をステップＳ４０７０に進める。
【００７６】
次に、ステップＳ４０７０において、位置姿勢補正部１２０は、ステップＳ４０６０までの処理によって得られたベクトルsを、撮像装置１３０の補正後の位置及び姿勢の推定値として設定する。
【００７７】
以上説明した処理によれば、撮像装置１３０の位置及び姿勢を未知のパラメータとして、位置姿勢センサ１４０による計測値の誤差を補正することができ、その際に正則化によって過剰な補正を抑制することができる。
【００７８】
そして、本実施形態によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行うのではなく、全てのパラメータを補正の対象として、画像情報に応じた補正を行う。そのため、従来と比べて高精度な計測を行うことができる。
【００７９】
なお、本実施形態では、撮像装置の位置及び姿勢の計測を行っていたが、撮像装置をある物体に装着した場合には、上述の方法で撮像装置の位置及び姿勢を計測することで、この物体の位置及び姿勢を求めることもできる。
【００８０】
［第２の実施形態］
第１の実施形態では、室内等の空間中における撮像装置の位置及び姿勢の計測を行っていた。しかし、正則化の考え方は、撮像装置以外の対象物体の位置及び姿勢を計測する場合であっても適用できる。
【００８１】
図５は、本実施形態に係るシステムの機能構成例を示すブロック図である。図５において、図１に示したものと同じものについては同じ参照番号を割り当てている。本実施形態に係る位置姿勢計測装置５００は、撮像装置１３０と計測対象物体５７０との間の相対位置姿勢を計測する。以下、第１の実施形態と異なる点についてのみ説明する。
【００８２】
計測対象物体５７０上の複数の位置には、物体座標系（計測対象物体５７０上に定義される座標系）における位置が既知である複数個の指標Ｑｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。図５では、Ｋ＝９、すなわち９個の指標Ｑ１〜Ｑ９が配置されている状況を示している。
【００８３】
指標Ｑｋは、第１の実施形態と同様に、撮像装置１３０による撮像画像上における投影像の画像座標が検出可能であって、撮像画像中のそれぞれの指標がいずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。
【００８４】
位置姿勢センサ１４０ａは、図１の位置姿勢センサ１４０と同じものであり、撮像装置１３０の位置及び姿勢を計測すべく、撮像装置１３０に取り付けてある。位置姿勢センサ１４０ａは、撮像装置１３０の位置及び姿勢を計測すると、その計測値（撮像装置１３０の位置姿勢情報）を、位置姿勢計測装置５００が有するセンサ計測値入力部１５０に対して送出する。
【００８５】
位置姿勢センサ１４０ｂは、計測対象物体５７０の位置及び姿勢を計測すべく、計測対象物体５７０に取り付けてある。位置姿勢センサ１４０ｂは、計測対象物体５７０の位置及び姿勢を計測すると、その計測値を、位置姿勢計測装置５００が有するセンサ計測値入力部１５０に対して送出する。
【００８６】
センサ計測値入力部１５０は、位置姿勢センサ１４０ａ、１４０ｂのそれぞれから送出された計測値を、後段の位置姿勢補正部５２０に対して送出する。
【００８７】
位置姿勢補正部５２０は、センサ計測値入力部１５０から送出された計測値と、指標検出部１１０から送出された各々の指標Ｑｋｎの識別子ｋｎと画像座標ｕＱｋｎを受ける。そして、これらの情報に基づいて、撮像装置１３０若しくは計測対象物体５７０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【００８８】
図６は、位置姿勢計測装置５００が有する位置姿勢補正部５２０が行う処理のフローチャートである。
【００８９】
先ず、ステップＳ６０００において、位置姿勢補正部５２０は、指標検出部１１０から送出された指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを取得する。なお、指標Ｑｋの基準座標系における配置位置情報（３次元座標）ｘＷＱｋｎは、既知の値として位置姿勢補正部５２０が保持しているものとする。
【００９０】
次に、ステップＳ６０１０において、位置姿勢補正部５２０は、センサ計測値入力部１５０から送出された計測値（撮像装置１３０の位置及び姿勢の計測値と、計測対象物体５７０の位置及び姿勢の計測値）を取得する。
【００９１】
次に、ステップＳ６０２０において、位置姿勢補正部５２０は、指標検出部１１０が検出した全ての指標についての誤差の和を最小とするような撮像装置１３０の位置と姿勢、計測対象物体５７０の位置と姿勢、を算出する。係る算出の過程では、第１の実施形態と同様にして、補正値の算出に用いる行列とその転置行列の積を正則化する。
【００９２】
ステップＳ６０２０における処理のうち、撮像装置１３０の位置及び姿勢を補正するための処理は、第１の実施形態で説明したステップＳ３０２０における処理、即ち、図４に示したフローチャートに従った処理と基本的には同じであるが、下記の点が異なる。
【００９３】
即ち、本実施形態の場合、指標の観測方程式Ｆ'ｃ（）が、物体座標系における指標Ｑｋｎの座標ｘＯＱｋｎ（既知な情報として位置姿勢補正部５２０が保持している）から画像座標を算出する関数となる。すなわち、観測方程式Ｆ'ｃ（）は下記の式１２によって表される。
【００９４】
【数１２】
2009168626.tif 000014
【００９５】
具体的には、観測方程式Ｆ'ｃ（）は、ｘＯＱｋｎとベクトルｓから指標Ｑｋｎの座標ｘＣＱｋｎを求める式（式１３）と、座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める上記（式３）によって構成されている。そして、本実施形態は、係る点のみが第１の実施形態と異なっている。
【００９６】
【数１３】
2009168626.tif 000015
【００９７】
ここで、tWC、tWOはそれぞれ、撮像装置１３０と計測対象物体５７０の夫々の位置計測値（平行移動成分）を表す３次元ベクトルであり、ここでは固定値として取り扱われる。また、RWOは計測対象物体５７０の姿勢計測値（回転成分）を表す３×３の回転行列であり、同様に固定値として取り扱われる。
【００９８】
なお、ステップＳ６０２０では更に、計測対象物体５７０の位置及び姿勢を補正する処理を行うのであるが、係る処理は次の通りである。
【００９９】
先ず、以下に述べる座標変換処理を行った後に、変換後の位置及び姿勢を出力する。
【０１００】
同次座標系を用いた４×４の座標変換行列によって位置及び姿勢を表記すると、計測対象物体５７０の補正後の位置及び姿勢MWOは、下記の式１４によって算出される。
【０１０１】
【数１４】
2009168626.tif 000016
【０１０２】
ここで、M#WCは、位置姿勢センサ１４０ａから得られた計測値（撮像装置１３０の位置及び姿勢）である。以下、#は、位置姿勢センサ１４０ａ、位置姿勢センサ１４０ｂの何れかのセンサによる計測値を表す記号とする。また、M#WOは、位置姿勢センサ１４０ｂから得られた計測値（計測対象物体５７０の位置及び姿勢）であり、MWCは、ステップＳ６０２０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢である。
【０１０３】
このとき、撮像装置１３０によって定義されるカメラ座標系における計測対象物体５７０の位置及び姿勢は、下記の式１５に示す式に従ったものとなる。
【０１０４】
【数１５】
2009168626.tif 000017
【０１０５】
即ち、計測対象物体５７０と撮像装置１３０との相対的な位置及び姿勢の関係は次の二つの場合で等価となる。
（ａ）  ステップＳ６０２０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢（MWC）と、位置姿勢センサ１４０ｂから得られた計測対象物体５７０の位置及び姿勢（M#WO）とを出力する場合
（ｂ）  位置姿勢センサ１４０ａから得られた撮像装置１３０の位置及び姿勢（M#WC）と、式１４によって求めた計測対象物体５７０の補正後の位置及び姿勢（MWO）とを出力する場合
したがって、本装置の出力を利用するアプリケーションが要求する形態に合わせて、出力形態を選択できるようになっていることが好ましい。
【０１０６】
また、カメラ座標系における計測対象物体５７０の位置及び姿勢（MCO）を式２から算出してこれを出力しても良いし、物体座標系における撮像装置１３０の位置及び姿勢（MCO-1）を求めて、これを出力してもよい。
【０１０７】
なお、以上の説明では、ステップＳ６０２０では、撮像装置１３０の位置及び姿勢、計測対象物体５７０の位置及び姿勢、の両方に対して補正したものを出力するものとしているが、何れか一方のみを補正し、他方については補正しなくても良い。
【０１０８】
次に、ステップＳ６０３０において、位置姿勢補正部５２０は、ステップＳ６０２０で算出した撮像装置１３０の位置及び姿勢のデータ、計測対象物体５７０の位置及び姿勢のデータ、を本装置の外部に対して送出する。なお、係るデータの取り扱いについては、第１の実施形態と同様に、本装置の外部に送出することに限定するものではない。例えば、係るデータを、他のアプリケーションから利用可能な状態として位置姿勢計測装置５００が保持しておいても良い。
【０１０９】
次に、ステップＳ６０４０において、位置姿勢補正部５２０は、処理を終了するか否かの判定を行う。係る判定は、例えば、位置姿勢計測装置５００の操作者が本処理の終了指示を入力したか否かを位置姿勢補正部５２０が判定することで行っても良いし、本処理の終了条件が満たされたか否かを位置姿勢補正部５２０が判定することで行っても良い。係る判定の結果、終了する場合には、本処理を終了させるし、終了しない場合には、処理をステップＳ６０００に戻し、以降の処理を繰り返す。
【０１１０】
以上の説明により、本実施形態によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行うのではなく、全てのパラメータを補正の対象として画像情報に応じた補正を行う。そのため、従来と比べて高精度な計測を行うことができる。
【０１１１】
［第３の実施形態］
本実施形態では、室内等に設定された基準座標系における任意の計測対象物体の位置及び姿勢の計測を行う。
【０１１２】
本実施形態に係るシステムでは、撮像装置には位置姿勢センサは装着されておらず、代わりに、撮像装置は、位置及び姿勢が既知の場所に三脚等によって固定して設置されていることが、第１の実施形態、第２の実施形態とは異なっている。
【０１１３】
以下では、第２の実施形態との相違点についてのみ説明する。
【０１１４】
図７は、本実施形態に係るシステムの機能構成例を示すブロック図である。図７において図５に示したものと同じものについては同じ参照番号を割り当てている。
【０１１５】
撮像装置１３０は、三脚７８０によって、現実空間中の既知の位置に、既知の姿勢でもって固定して設置されている。撮像装置１３０の基準座標系における既知の位置及び姿勢は、データとして位置姿勢計測装置７００が保持している。また、撮像装置１３０の動作については第１の実施形態と同様であり、撮像画像を後段の位置姿勢計測装置７００が有する画像入力部１６０に送出する。
【０１１６】
位置姿勢補正部５２０は、第２の実施形態で説明した処理と同じ処理を行うのであるが、唯一の違いは、撮像装置１３０の位置及び姿勢が、計測値として与えられるのではなく、既知の値として得られている点にある。従って、本実施形態と第２の実施形態とで、実質的な処理の違いはない。
【０１１７】
本実施形態では先ず、位置姿勢補正部５２０は、撮像装置１３０の位置及び姿勢の既知の値を、撮像装置１３０の位置及び姿勢の仮の計測値とする。そして次に、第２の実施形態で説明したステップＳ６０００〜ステップＳ６０２０の処理を行うことで、撮像装置１３０の位置及び姿勢の計測値の補正を行う。そして、この補正結果に基づいて、ステップＳ６０３０において、第２の実施形態と同様に計測対象物体５７０の位置及び姿勢の計測値を補正する（式１４）。本実施形態では、位置姿勢補正部５２０がステップＳ６０３０において出力するのは、計測対象物体５７０の位置及び姿勢のみである。
【０１１８】
以上の説明により、本実施形態によれば、計測対象物体の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行うのではなく、全てのパラメータを補正の対象として画像情報に応じた補正を行う。そのため、従来と比べて高精度な計測を行うことができる。
【０１１９】
＜変形例１＞
第１の実施形態では、式１０で表される一般化逆行列を用いて正則化を行ったが、正則化の方法はこれに限られるものではない。例えば、式９における画像ヤコビアンΘとその転置行列ΘTとの積ΘTΘを固有値分解し、その微小固有値を０としてランクを低減させる方法を用いても良い。
【０１２０】
具体的には、まずΘTΘを固有値分解し、式１６に示した結果を得る。
【０１２１】
【数１６】
2009168626.tif 000018
【０１２２】
ここで、Λは対角要素にΘTΘの固有値が配置された対角行列、Ｖは対応する固有ベクトルからなる行列を表す。
【０１２３】
次に、Λの微小な固有値を０としてランクを低減させたものを行列Λ’とする。ここで、微小な固有値とは、丸め誤差に埋もれて正確な値が分からないような、数値的に非常に小さな固有値のことを表す。
【０１２４】
そして、再びΘTΘを、以下の式１７によって算出する。
【０１２５】
【数１７】
2009168626.tif 000019
【０１２６】
そして、算出結果を元のΘTΘの代わりに式９に代入して用いる。
【０１２７】
＜変形例２＞
変形例１では、式９における画像ヤコビアンΘとその転置行列ΘTとの積ΘTΘの微小固有値を０としたが、ΘTΘの固有値が小さい順に、予め定めた数であるｎ個（ｎ＝１〜５）を０としてもよい。例えば、ΘTΘの固有値が小さい順に３個または４個を０とする。そうすることにより、センサによって計測された撮像装置の位置あるいは姿勢のいずれか（２または３自由度）のみを補正する特許文献２の方法と同様に、撮像装置の位置及び姿勢を安定化させることができる。
【０１２８】
＜変形例３＞
第１の実施形態におけるステップＳ３０２０では、図４のステップＳ４０４０にて詳細に説明したように、誤差ベクトルＵと行列Θに基づく補正量Δsの算出に、式８で表現されるＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いている。しかし、補正量Δsの算出は必ずしもＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いて行わなくてもよい。例えば、公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を用いて求めてもよいし、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよい。さらに、他の何れの数値計算手法を適用しても良い。
【０１２９】
何れの数値計算においても、正則化を伴う行列演算を利用することにより、補正量Δsを安定して得ることができる。また、上記の実施形態におけるsを状態ベクトル、式１を観測方程式とすることで、画像ヤコビアンを利用して位置姿勢センサによる計測値を補正することができる。すなわち、そのような効果を有した拡張カルマンフィルタ（Extended Kalman Filter）や、反復拡張カルマンフィルタ（iterative Extended Kalman Filter）を構成することができる。なお、拡張カルマンフィルタ及び反復拡張カルマンフィルタについては非特許文献２に記載されるように周知であるので、その詳細に関するこれ以上の説明は省略する。
【０１３０】
＜変形例４＞
第２の実施形態、又は第３の実施形態で用いる計測対象物体には、情景を撮像するカメラ（主観視点カメラ）を用いることも可能である。その場合、上記の実施形態の夫々で述べた方法で指標の３次元位置を算出した上で、特開２００７−４０９８０号公報に開示されているような方法で、主観視点カメラの位置及び姿勢を算出することが可能である。この場合、主観視点カメラで撮影した画像に、主観視点カメラの位置及び姿勢に応じて生成したＣＧ画像を重畳描画し、これを画像表示装置に表示することで、複合現実感の提示を行うことができる。
【０１３１】
＜変形例５＞
上記の実施形態の各々においては、画像座標に係る特徴量として各々が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を用いていた。しかし、点指標以外の指標を用いることも可能である。例えば、公知の位置姿勢計測装置（例えば、非特許文献３を参照）で用いられているような、線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。しかし、線指標はその画像座標に係る特徴量として画像上での位置を求めることはできない。例えば、原点から線指標までの距離を評価の基準とし、画像からの検出値とsによる推定値から算出する誤差によって誤差ベクトルＵを構成し、観測方程式をsの各要素で偏微分した解を各要素に持つ１行×６列のヤコビ行列により行列Θを構成すればよい。そうすることで、上記実施形態と同様にして位置及び姿勢の計測（補正）を行うことができる。また、線指標と点指標、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することも可能である。
【０１３２】
＜変形例６＞
上記各実施形態では、位置姿勢計測装置１００，５００，７００を構成する各部は全て、暗にハードウェアで構成されているものとして説明したが、これら各部のうち一部若しくは全部をソフトウェアでもって構成しても良い。
【０１３３】
例えば、位置姿勢計測装置１００，５００，７００を構成する各部をソフトウェアで実装した場合、位置姿勢計測装置１００，５００，７００には、一般のＰＣ（パーソナルコンピュータ）を適用することができる。この場合、係るＰＣのメモリには係るソフトウェアをインストールし、係るＰＣのＣＰＵは、このインストールされたソフトウェアを実行する。これにより、係るＰＣは、位置姿勢計測装置１００や位置姿勢計測装置５００、位置姿勢計測装置７００が行うものとして上述した各処理を実行することになる。
【０１３４】
図２は、位置姿勢計測装置１００、５００、７００に適用可能なコンピュータのハードウェア構成例を示すブロック図である。
【０１３５】
ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行う。また、係るコンピュータを位置姿勢計測装置１００に適用した場合には、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０のそれぞれが行うものとして上述した各処理を実行する。また、係るコンピュータを位置姿勢計測装置５００や位置姿勢計測装置７００に適用した場合には、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０のそれぞれが行うものとして上述した各処理を実行する。
【０１３６】
ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶する為のエリアを有する。まら、ＲＡＭ１００２は、Ｉ／Ｆ（インターフェース）１００９を介して撮像装置１３０から受信した撮像画像のデータや、位置姿勢センサ１４０（１４０ａ、１４０ｂ）から受信した計測値のデータ等を一時的に記憶するためのエリアを有する。更に、ＲＡＭ１００２は、ＣＰＵ１００１が各種の処理を実行する際に用いるワークエリアを有する。即ち、ＲＡＭ１００２は、各種のエリアを適宜提供することができる。
【０１３７】
ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データ等を格納する。
【０１３８】
キーボード１００４、マウス１００５は入力デバイスであり、操作者はこれらを用いて、各種の指示をＣＰＵ１００１に対して入力することができる。
【０１３９】
表示部１００６は、ＣＲＴや液晶ディスプレイなどにより構成されており、ＣＰＵ１００１による処理結果を画像や文字などでもって表示することができる。表示部１００６は、例えば、位置姿勢計測のために表示すべきメッセージ等を表示する。
【０１４０】
外部記憶装置１００７は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置１００７には、ＯＳ（オペレーティングシステム）が保存されている。更に、外部記憶装置１００７には、コンピュータの適用対象（位置姿勢計測装置１００、５００、７００の何れか）が行うものとして上述した各処理をＣＰＵ１００１に実行させるためのプログラム（コンピュータプログラム）やデータ等が保存されている。
【０１４１】
コンピュータを位置姿勢計測装置１００に適用した場合、係るプログラムには、ＣＰＵ１００１を、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０のそれぞれとして機能させるためのプログラムが含まれている。また、コンピュータを位置姿勢計測装置５００や位置姿勢計測装置７００に適用した場合、係るプログラムには、次のようなプログラムが含まれている。即ち、ＣＰＵ１００１を、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部５２０のそれぞれとして機能させるためのプログラムが含まれている。
【０１４２】
また、外部記憶装置１００７に保存されているデータには、上述の説明において、既知の情報として説明したものが保存されている。
【０１４３】
外部記憶装置１００７に保存されているプログラムやデータは、ＣＰＵ１００１による制御に従って、適宜ＲＡＭ１００２にロードされ、ＣＰＵ１００１による処理対象となる。
【０１４４】
記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出し、ＲＡＭ１００２や外部記憶装置１００７に出力する。なお、外部記憶装置１００７に保存されているものとして説明した情報の一部を係る記憶媒体に記録させるようにしても良い。
【０１４５】
Ｉ／Ｆ１００９は、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポートや、情報を外部へ出力するためのイーサネットポート等によって構成されている。また、位置姿勢センサ１４０（１４０ａ、１４０ｂ）を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポートもＩ／Ｆ１００９に含まれる。画像入力部１６０及びセンサ計測値入力部１５０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
【０１４６】
上述した各構成要素は、バス１０１０によって相互に接続される。
【０１４７】
なお、コンピュータのハードウェア構成についてはこれに限定するものではなく、様々な構成が考え得る。
【０１４８】
また、図１に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部１２０の少なくとも一部を、独立した装置として実現し、位置姿勢計測装置１００と係る装置との間でデータ通信を行うようにしても良い。
【０１４９】
また、図５や図７に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、位置姿勢補正部５２０の少なくとも一部を、独立した装置として実現し、位置姿勢計測装置５００（７００）と係る装置との間でデータ通信を行うようにしても良い。
【０１５０】
また、以上説明した様々な実施形態や変形例は、適宜組み合わせて実施しても良い。
【０１５１】
［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。係る記憶媒体は言うまでもなく、コンピュータ読み取り可能な記憶媒体である。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。
【０１５２】
また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１５３】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれたとする。その後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１５４】
本発明を上記記録媒体に適用する場合、その記録媒体には、先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０１５５】
【図１】本発明の第１の実施形態に係るシステムの機能構成例を示すブロック図である。
【図２】位置姿勢計測装置１００、５００、７００に適用可能なコンピュータのハードウェア構成例を示すブロック図である。
【図３】位置姿勢計測装置１００が有する位置姿勢補正部１２０が行う処理のフローチャートである。
【図４】ステップＳ３０２０における処理の詳細を示すフローチャートである。
【図５】本発明の第２の実施形態に係るシステムの機能構成例を示すブロック図である。
【図６】位置姿勢計測装置５００が有する位置姿勢補正部５２０が行う処理のフローチャートである。
【図７】本発明の第３の実施形態に係るシステムの機能構成例を示すブロック図である。
【図１】
2009168626.tif 000020
【図２】
2009168626.tif 000021
【図３】
2009168626.tif 000022
【図４】
2009168626.tif 000023
【図５】
2009168626.tif 000024
【図６】
2009168626.tif 000025
【図７】
2009168626.tif 000026