(57)【要約】【課題】  安定性と精度とが両立した位置及び姿勢の計測を行うことの可能な位置姿勢測定装置を提供すること。【解決手段】  センサ計測値として得られた位置姿勢計測値を、指標の既知座標を用いて算出される画像座標の推定値（理論値）と、当該指標を撮像した画像中で検出された画像座標の実測値との誤差が最小となるような補正値を求め（Ｓ４０４０）、この補正値によって補正する（Ｓ４０５０）。【選択図】  図４
2006215924.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号公報
【特許文献３】特開2003-344018号公報
【発明の開示】
【発明が解決しようとする課題】
【０００９】
上記特許文献２の手法では、指標が３点以上観測されている際には必ず、位置及び姿勢の６自由度を画像情報から求めるアルゴリズムが選択される。しかし実際には、画像上の一部の領域に指標が偏在して観測されている場合のように、指標の数が３点以上であっても、位置及び姿勢の６自由度を安定して求めるのに十分な画像情報が入力画像中に含まれていない状況が存在する。特許文献２の手法は、このような状況下において、得られる解の精度や安定性が不十分となることがあった。
【００１０】
一方、上記特許文献１の手法は、精度よりも安定性を重視した手法であり、画像上の一部の領域に指標が偏在して観測されている場合のような画像情報が不十分な状況下であっても、特許文献２の手法と比較して安定した計測を行うことができる。しかし、十分な画像情報が得られている場合でも一部のパラメータの補正のみしか行わないので、画像情報を最大限に生かした精度を得ることができなかった。
【００１１】
また、上記特許文献１の手法では、個々の指標に対する２次元的な補正値の平均を求めているだけであるので、全指標上での誤差の和を最小とするような最適な補正を行うことができなかった。
【００１２】
本発明はこのような従来技術の問題点に鑑みてなされたものであり、計測対象物体の位置及び姿勢の計測を、安定性と精度とを両立しながら行うことが可能な位置姿勢測定方法及び装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
上述の目的は、計測対象物体としての撮像装置の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサの位置姿勢計測値を入力する位置姿勢計測値入力工程と、撮像装置が撮像した撮像画像を入力する画像入力工程と、撮像画像から、指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値から求められる計測対象物体の位置及び姿勢を表す複数のパラメータの所定の一部について、指標検出工程で検出される指標の画像座標に関わる特徴量と、複数のパラメータを用いて算出される当特徴量の推定値との誤差に応じた補正を行う補正工程とを有し、補正されたパラメータを含む複数のパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によって達成される。
【００１４】
また、上述の目的は、計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサの位置姿勢計測値を入力する位置姿勢計測値入力工程と、計測対象物体との相対位置及び姿勢が既知である撮像装置が撮像した撮像画像を入力する画像入力工程と、撮像画像から、指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値から求められる計測対象物体の位置及び姿勢を表す複数のパラメータの所定の一部について、指標検出工程で検出される指標の画像座標に関わる特徴量と、複数のパラメータを用いて算出される当特徴量の推定値との誤差に応じた補正を行う補正工程とを有し、補正されたパラメータを含む複数のパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によっても達成される。
【００１５】
また、上述の目的は、計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサの位置姿勢計測値を入力する位置姿勢計測値入力工程と、計測対象物体を撮像する撮像装置から撮像画像を入力する画像入力工程と、撮像画像から、計測対象物体上に配置された指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値から求められる計測対象物体の位置及び姿勢を表す複数のパラメータの所定の一部について、指標検出工程で検出される指標の画像座標に関わる特徴量と、複数のパラメータを用いて算出される当特徴量の推定値との誤差に応じた補正を行う補正工程とを有し、補正されたパラメータを含む複数のパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によっても達成される。
【００１６】
また、上述の目的は、計測対象物体としての撮像装置の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサの位置姿勢計測値を入力する位置姿勢計測値入力工程と、撮像装置が撮像した撮像画像を入力する画像入力工程と、撮像画像から、指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値から求められる計測対象物体の位置及び姿勢を表すパラメータの所定の一部を、指標検出工程で検出される指標の画像座標に関わる特徴量と、パラメータを用いて算出される当特徴量の推定値と、一部のパラメータに関する当推定値の画像ヤコビアンとを用いて補正する補正工程とを有し、補正されたパラメータを含む複数のパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によっても達成される。
【００１７】
また、上述の目的は、計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサの位置姿勢計測値を入力する位置姿勢計測値入力工程と、計測対象物体との相対位置及び姿勢が既知である撮像装置が撮像した撮像画像を入力する画像入力工程と、撮像画像から、指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値から求められる計測対象物体の位置及び姿勢を表すパラメータの所定の一部を、指標検出工程で検出される指標の画像座標に関わる特徴量と、パラメータを用いて算出される当特徴量の推定値と、一部のパラメータに関する当推定値の画像ヤコビアンとを用いて補正する補正工程とを有し、補正されたパラメータを含む複数のパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によっても達成される。
【００１８】
また、上述の目的は、計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサの位置姿勢計測値を入力する位置姿勢計測値入力工程と、計測対象物体を撮像する撮像装置から撮像画像を入力する画像入力工程と、撮像画像から、計測対象物体上に配置された指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値から求められる計測対象物体の位置及び姿勢を表すパラメータの所定の一部を、指標検出工程で検出される指標の画像座標に関わる特徴量と、パラメータを用いて算出される当特徴量の推定値と、一部のパラメータに関する当推定値の画像ヤコビアンとを用いて補正する補正工程とを有し、補正されたパラメータを含む複数のパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によっても達成される。
【００１９】
また、上述の目的は、計測対象物体としての撮像装置の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサから位置姿勢計測値を入力する位置姿勢計測値入力工程と、撮像装置から撮像画像を入力する画像入力工程と、撮像画像から、指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値のうち、姿勢を表すパラメータを固定値とし、位置を表すパラメータを、指標検出工程で検出される指標の画像座標に関わる特徴量と、指標の既知の座標とを利用して補正する補正工程とを有し、補正されたパラメータと、姿勢を表すパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によっても達成される。
【００２０】
また、上述の目的は、計測対象物体の位置及び姿勢を計測する位置姿勢計測方法であって、計測対象物体の位置及び姿勢を計測する位置姿勢センサから位置姿勢計測値を入力する位置姿勢計測値入力工程と、計測対象物体を撮像する撮像装置から撮像画像を入力する画像入力工程と、撮像画像から、計測対象物体上に配置された指標の画像座標に関わる特徴量を検出する指標検出工程と、位置姿勢計測値から求められる計測対象物体の位置及び姿勢を表すパラメータの夫々を、指標検出工程で検出される指標の画像座標に関わる特徴量と、パラメータを用いて算出される当特徴量の推定値と、夫々のパラメータに関する当推定値の画像ヤコビアンとを利用して補正する補正工程とを有し、補正されたパラメータを、計測対象物体の位置及び姿勢計測結果とすることを特徴とする位置姿勢計測方法によっても達成される。
【発明の効果】
【００２１】
本発明に係る位置姿勢計測装置によれば、指標の画像座標の理論値と実測値との誤差を最小とする補正値を求め、この補正値によってセンサの計測値を補正することで、従来の方法と比べて安定性と精度に優れた計測が実現できる。
【発明を実施するための最良の形態】
【００２２】
以下添付図面を参照して、本発明を好適な実施形態に従って詳細に説明する。
［第１の実施形態］
本実施形態に係る位置姿勢計測装置は、撮像装置と計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。
【００２３】
図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０によって構成される。そして、撮像装置１３０及び、撮像装置１３０と計測対象物体１７０に取り付けられた位置姿勢センサ１４０（１４０ａ，１４０ｂ）に接続されている。
【００２４】
計測対象物体１７０上の複数の位置には、物体座標系（計測対象物体１７０上に定義される座標系）における位置が既知である複数個の指標Ｑｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。図１の例は、Ｋ＝９、すなわち９個の指標Ｑ１〜Ｑ９が配置されている状況を示している。指標Ｑｋは、例えば、それぞれが異なる色を有する同一形状（図では円形）のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する四角形の単色領域によって形成されるような、四角形指標を用いることも可能である。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。また指標は故意に設定されたものであっても良いし、故意に設定されたものではない、自然形状のものを用いても良い。
【００２５】
例えばビデオカメラである撮像装置１３０が出力する画像（以下、これを撮影画像と呼ぶ）は、位置姿勢計測装置１００に入力される。
【００２６】
６自由度センサである位置姿勢センサ１４０ａ及び１４０ｂは、それぞれ撮像装置１３０及び計測対象物体１７０に装着されており、基準座標系における撮像装置１３０及び計測対象物体１７０の位置及び姿勢を計測する。位置姿勢センサ１４０が出力する計測値は、位置姿勢計測装置１００に入力される。位置姿勢センサ１４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。なお、位置姿勢センサ１４０によって計測される位置及び姿勢は、磁場の歪み等の影響によって誤差を含んだものとなっている。
【００２７】
画像入力部１６０は、位置姿勢装置１００へ入力される撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。
【００２８】
センサ計測値入力部１５０は、位置姿勢センサ１４０（１４０ａ，１４０ｂ）から夫々の計測値を入力し、位置姿勢算出部１２０へと出力する。
【００２９】
指標検出部１１０は、画像入力部１６０より撮影画像を入力し、入力した画像中に撮影されている指標Ｑｋの画像座標を検出する。例えば、指標Ｑｋの各々が異なる色を有するマーカによって構成されている場合には、撮影画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標Ｑｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを撮影画像上に施すことにより、指標の位置を検出する。また、四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、４つの直線によって形成されている領域を指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の識別子を取得する。なお、このようにして検出される四角形指標は、本明細書では、４つの頂点の個々によって形成される４つの指標であると考える。
【００３０】
指標検出部１１０は、さらに、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。
【００３１】
位置姿勢算出部１２０は、センサ計測値入力部１５０の出力である撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、計測対象物体１７０あるいは撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【００３２】
なお、図１に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。
【００３３】
図２は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々の機能を、ソフトウェアを実行することで実現するためのコンピュータの基本構成を示す図である。
【００３４】
ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２０の夫々のソフトウェアの実行を制御して、各部の機能を実現する。
【００３５】
ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備えると共に、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。
【００３６】
ＲＯＭ１００３は、一般にコンピュータのプログラムや設定データなどが格納されている。キーボード１００４、マウス１００５は入力デバイスであり、操作者はこれらを用いて、各種の指示をＣＰＵ１００１に入力することができる。
【００３７】
表示部１００６は、ＣＲＴや液晶ディスプレイなどにより構成されており、例えば、位置姿勢計測のために表示すべきメッセージ等を表示することができる。
【００３８】
外部記憶装置１００７は、ハードディスクドライブなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やＣＰＵ１００１が実行するプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。
【００３９】
記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出して、ＲＡＭ１００２や外部記憶装置１００７に出力する。
【００４０】
Ｉ／Ｆ１００９は、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、位置姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポート、また、算出した位置及び姿勢を外部へ出力するためのイーサネット（登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０及びセンサ計測値入力部１５０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
上述した各構成要素は、バス１０１０によって相互に接続される。
【００４１】
図３は、位置姿勢算出部１２０の処理手順を示すフローチャートであり、本実施形態ではＣＰＵ１００１が位置姿勢算出部１２０の機能を実現するプログラムを実行することにより実現される。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、例えば外部記憶装置１００７からＲＡＭ１００２に既にロードされているものとする。
【００４２】
ステップＳ３０００において、位置姿勢算出部１２０は、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを指標抽出部１１０から入力する。なお、各指標の物体座標系における３次元座標ｘＯＱｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。
【００４３】
ステップＳ３００５において、位置姿勢算出部１２０は、撮像装置１３０及び計測対象物体１７０の位置及び姿勢のセンサによる計測値をセンサ計測値入力部１５０から入力する。
【００４４】
ステップＳ３０１０において、位置姿勢算出部１２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ３０８０に処理を進め、それ以外の場合はステップＳ３０２０に処理を進める。
【００４５】
ステップＳ３０２０において、位置姿勢算出部１２０は、検出された指標の総数が一つか否かを判定する．指標の総数が一つの場合はステップＳ３０２５に処理を進め、それ以外の場合はステップＳ３０３０に処理を進める。
【００４６】
ステップＳ３０２５において、位置姿勢算出部１２０は、ステップＳ３００５で得た撮像装置１３０の姿勢の計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（物体座標系における指標の３次元座標ｘＯＱｋｎと撮像装置１３０及び計測対象物体１７０の位置及び姿勢の計測値から導出する投影座標の理論値ｕＱｋｎ＊と、実際の検出座標ｕＱｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。ステップＳ３０２５における補正処理としては、周知の処理を適用することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。
【００４７】
ステップＳ３０３０において、位置姿勢算出部１２０は、検出された指標の総数が二つか否かを判定する．指標の総数が二つの場合はステップＳ３０３３に処理を進め、それ以外の場合はステップＳ３０４０に処理を進める。
【００４８】
ステップＳ３０３３において、位置姿勢算出部１２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ１（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ１以上の場合にはステップＳ３０３５に処理を進め、それ以外の場合はステップＳ３０２５に処理を進め、上述した処理を行う。
【００４９】
ステップＳ３０３５において、位置姿勢算出部１２０は、ステップＳ３００５で得た計測対象物体１７０の位置及び姿勢の計測値と、撮像装置１３０の位置の計測値を固定値として、撮像装置１３０の姿勢の計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この処理工程の詳細は後述する。
【００５０】
ステップＳ３０４０において、位置姿勢算出部１２０は、検出された全指標（このステップが実行されるのは、検出された指標の総数が三つ以上の場合のみである）の画像座標を包含する凸包を算出する。画像上の点群に対する凸包の算出方法は画像処理の基本的事項として広く公知のものであるので、その詳細な説明は省略する。
【００５１】
図６，図７，及び図８は、撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示している。図６に示す撮影画像６００上では指標Ｑ１，Ｑ５，Ｑ６，Ｑ９が、図７の撮影画像７００上では指標Ｑ１，Ｑ２，Ｑ６，Ｑ７が，図８の撮影画像８００上では指標Ｑ２，Ｑ６，Ｑ７が観測されており、それ以外の指標は、他の物体（本実施形態においてはユーザの手６１０）によって隠蔽されているものとする。
【００５２】
これらの撮影画像に対して指標の検出を行い、図３のフローチャートで示した位置姿勢算出部１２０の処理ステップを実行すると、各画像において検出される指標の数は３以上であるので、ステップＳ３０４０以降の処理が実行されることになる。図９，図１０及び図１１は、それぞれ撮影画像６００，７００，８００に対してステップＳ３０４０の処理を実行した際に得られる、検出指標の成す凸包（９００，１０００，１１００）を示している。なお、以下では、各凸包の面積をそれぞれＡ９００，Ａ１０００，及びＡ１１００と表記したときに、以下に述べる閾値Ｔ２及び閾値Ｔ３に対して、Ａ１１００＜Ｔ２＜Ａ１０００＜Ｔ３＜Ａ９００という関係が満たされているものとする。
【００５３】
ステップＳ３０５０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ２（例えば、撮影画像全体の面積の１／１６）との比較を行う。面積が閾値Ｔ２以上の場合はステップＳ３０６０に処理を進め、それ以外の場合はステップＳ３０２５へと処理を進める。図６〜図１１の例では、撮影画像８００についてはステップＳ３０２５へ、撮影画像７００及び６００についてはステップＳ３０６０へと処理が進行する。
【００５４】
ステップＳ３０６０において、位置姿勢算出部１２０は、ステップＳ３０４０で算出した凸包の面積と、所定の値として定められている閾値Ｔ３（例えば、撮影画像全体の面積の１／９）との比較を行う。面積が閾値Ｔ３以上の場合はステップＳ３０７０に処理を進め、それ以外の場合はステップＳ３０３５へと処理を進める。図６〜図１１の例では、撮影画像７００についてはステップＳ３０３５へ、撮影画像６００についてはステップＳ３０７０へと処理が進行する。
【００５５】
ステップＳ３０７０において、位置姿勢算出部１２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程の詳細は後述する。
【００５６】
ステップＳ３０８０において、位置姿勢算出部１２０は、ステップＳ３０２５、Ｓ３０３５及びＳ３０７０で得られた撮像装置１３０及び計測対象物体１７０の位置及び姿勢を表すデータを、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【００５７】
なお、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の各工程では、撮像装置１３０に関する計測値のみに補正を加えているので、ステップＳ３０８０で出力される位置及び姿勢のデータは、計測対象物体１７０に関しては、ステップＳ３００５で入力したセンサによる計測値そのものとなる。しかし、位置及び姿勢の出力形態はこれに限られるものではなく、逆に、撮像装置１３０の位置及び姿勢の計測値には補正を加えずにセンサによる計測値をそのまま出力し、計測対象物体１７０の位置及び姿勢の計測値に補正を加えて出力しても良い。この場合は、ステップＳ３０８０において、以下に述べる座標変換処理を行った後に、変換後の位置及び姿勢を出力する。
【００５８】
同次座標系を用いた４×４の座標変換行列によって位置及び姿勢を表記すると、計測対象物体１７０の補正後の位置及び姿勢MWOは、センサ計測値として得られた撮像装置１３０の位置及び姿勢M#WC（以下、# はセンサによる計測値を表す記号）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢M#WOと、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢MWCに基づいて、次式によって算出される。
【００５９】
【数１】
2006215924.tif 000003
【００６０】
このとき、撮像装置１３０によって定義されるカメラ座標系における計測対象物体１７０の位置及び姿勢は、
【００６１】
【数２】
2006215924.tif 000004
【００６２】
となる。すなわち、ステップＳ３０７０までの処理結果として得られた撮像装置１３０の補正後の位置及び姿勢（MWC）と、センサ計測値として得られた計測対象物体１７０の位置及び姿勢（M#WO）とを出力する場合と、センサ計測値として得られた撮像装置１３０の位置及び姿勢（M#WC）と、式１によって求められる計測対象物体１７０の補正後の位置及び姿勢（MWO）とを出力する場合では、計測対象物体１７０と撮像装置１３０の相対的な位置及び姿勢の関係は等価となる。したがって、本装置の出力を利用するアプリケーションが要求する形態に合わせて、出力形態を選択できるようになっていることが好ましい。
【００６３】
また、カメラ座標系における計測対象物体１７０の位置及び姿勢（MCO）を式２から算出してこれを出力しても良いし、物体座標系における撮像装置１３０の位置及び姿勢（MCO-1）を求めて、これを出力してもよい。
【００６４】
ステップＳ３０９０において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、ステップＳ３０００へと処理を進める。
以上によって、撮像装置の位置及び姿勢が計測される。
【００６５】
次に、図４のフローチャートを用いて、上記ステップＳ３０３５における撮像装置１３０の姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の姿勢を、３値ベクトル
s = ωWC = [ξWC  ψWC  ζWC]T
によって内部的に表現している。
【００６６】
姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。なお、姿勢ωは、次式、
【００６７】
【数３】
2006215924.tif 000005
【００６８】
によって、３×３の回転行列Rによっても表現可能であり、ωとRとは、互いに一意に変換することができる。Rからωへの変換方法は公知であるので、その詳細な説明は省略する。
【００６９】
ステップＳ４０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の姿勢を、sの初期値とする。
【００７０】
ステップＳ４０１０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、指標Ｑｋｎ各々の物体座標系における座標ｘＯＱｋｎ（既知な情報として予め保持している）から画像座標を算出する関数：
【００７１】
【数４】
2006215924.tif 000006
【００７２】
に基づいて行われる。具体的には、観測方程式Ｆｃ（）は、ｘＯＱｋｎとsから当該指標のカメラ座標（撮像装置１３０の視点位置を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘＣＱｋｎを求める次式、
【００７３】
【数５】
2006215924.tif 000007
【００７４】
及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める次式、
【００７５】
【数６】
2006215924.tif 000008
【００７６】
によって構成されている。ここでtWC及びtWOは、撮像装置１３０と計測対象物体１７０の夫々の位置計測値（平行移動成分）を表す３次元ベクトルであり、ここでは固定値として取り扱われる。また、RWOは計測対象物体１７０の姿勢計測値（回転成分）を表す３×３の回転行列であり、同様に固定値として取り扱われる。また、ｆＣｘ及びｆＣｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。
【００７７】
ステップＳ４０２０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差ΔｕＱｋｎを、次式によって算出する。
【００７８】
【数７】
2006215924.tif 000009
【００７９】
ステップＳ４０３０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、sに関する画像ヤコビアン（すなわち、式４の観測方程式Ｆｃ（）をsの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列）ＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルsの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＱｋｎを算出する。
【００８０】
【数８】
2006215924.tif 000010
【００８１】
ステップＳ４０４０において、位置姿勢算出部１２０は、ステップＳ４０２０及びステップＳ４０３０において算出した誤差ΔｕＱｋｎ及び画像ヤコビアンＪｕｓＱｋｎに基づいて、sの補正値Δsを算出する。具体的には、誤差ΔｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトル
【００８２】
【数９】
2006215924.tif 000011
【００８３】
及び、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた２Ｎ行×３列の行列
【００８４】
【数１０】
2006215924.tif 000012
【００８５】
を作成し、Θの擬似逆行列Θ+を用いて、
【００８６】
【数１１】
2006215924.tif 000013
【００８７】
として算出する。このように、本実施形態では補正値Δsを、最急降下法を用いて算出している。
【００８８】
ステップＳ４０５０において、位置姿勢算出部１２０は、ステップＳ４０４０において算出した補正値Δsを用いて式１２に従ってsを補正し、得られた値をsの新たな推定値とする。
【００８９】
【数１２】
2006215924.tif 000014
【００９０】
ステップＳ４０６０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δsが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後のsを用いて、再度ステップＳ４０１０以降の処理を行う。一方、収束していると判断した場合には、ステップＳ４０７０へと処理を進める。
【００９１】
ステップＳ４０７０において、位置姿勢算出部１２０は、ステップＳ４０６０までの処理によって得られたsを、撮像装置１３０の姿勢の（補正後の）推定値 ωWCとする。
【００９２】
以上によって、センサ計測値として得られた位置パラメータを固定して、全指標上での誤差の和を最小化するような姿勢パラメータを求めることができる。
【００９３】
次に、図５のフローチャートを用いて、上記ステップＳ３０７０における撮像装置１３０の位置及び姿勢算出の処理工程を説明する。以下では、算出すべき未知パラメータである撮像装置１３０の位置及び姿勢を、６値ベクトルs = [tWC T  ωWC T]T = [xWC  yWC  zWC  ξWC  ψWC  ζWC]Tによって内部的に表現している。
【００９４】
ステップＳ５０００において、位置姿勢算出部１２０は、センサ計測値として得られた撮像装置１３０の位置及び姿勢を、sの初期値とする。
【００９５】
ステップＳ５０１０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、sによって定義される指標の観測方程式、すなわち、各指標Ｑｋｎの物体座標系における座標ｘＯＱｋｎから画像座標を算出する関数：
【００９６】
【数１３】
2006215924.tif 000015
【００９７】
に基づいて行われる。具体的には、観測方程式Ｆ’ｃ（）は、ｘＯＱｋｎと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の初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１０５】
【数１４】
2006215924.tif 000016
【０１０６】
このとき、撮像装置１３０の位置tWC及び姿勢RWCに関しては、センサによる計測値をそのまま固定値として使用する。
【０１０７】
また、カメラ座標系における計測対象物体１７０の位置及び姿勢や、物体座標系における撮像装置１３０の位置及び姿勢を未知のパラメータとして求めても良い。
【０１０８】
前者の場合、算出すべき未知パラメータを、カメラ座標系における計測対象物体１７０の位置及び姿勢を表す６値ベクトルs = [tCO T ωCO T]T = [xCO  yCO  zCO  ξCO  ψCO  ζCO]Tによって内部的に表現する。また、カメラ座標系における計測対象物体１７０の位置及び姿勢（M#CO）を、次式、
【０１０９】
【数１５】
2006215924.tif 000017
【０１１０】
によってセンサ計測値（M#WC及びM#WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１１１】
【数１６】
2006215924.tif 000018
【０１１２】
一方、後者の場合は、算出すべき未知パラメータを、物体座標系における撮像装置１３０の位置及び姿勢を表す６値ベクトルs = [tOCT ωOC T]T = [xOC  yOC  zOC  ξOC  ψOC  ζOC]Tによって内部的に表現する。また、物体座標系における撮像装置１３０の位置及び姿勢（M#CO）を、次式、
【０１１３】
【数１７】
2006215924.tif 000019
【０１１４】
によってセンサ計測値（M#WC及びM#WO）に基づいて求め、これをsの初期値とする。また、観測方程式Ｆ’ｃ（）を構成する式５を、次式のように変形する。
【０１１５】
【数１８】
2006215924.tif 000020
【０１１６】
以上のようにして、計測対象物体１７０と撮像装置１３０との間の相対的な位置及び姿勢の関係を未知のパラメータとして、センサによる位置及び姿勢計測値の誤差を補正することができる。
【０１１７】
従来の位置姿勢計測装置では、指標の分布を考慮していないため、画像上の一部の領域に指標が偏在して観測されている場合に解が不安定になる。一方、本実施形態に係る位置姿勢計測装置によれば、ステップＳ３０３３や、ステップＳ３０４０，Ｓ３０５０，Ｓ３０６０によって指標の分布範囲の大きさを考慮した手法の選択が行われるため、例えば検出された指標の数が多くても、それらが画像上の小領域に偏在しているような場合に、位置及び姿勢の６自由度を安定して求めるのに不十分であるという判定を行うことができる。そのため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、不安定な解が得られる状況に陥る可能性が低下し、従来手法と比して安定した解を得ることができる。
【０１１８】
また、本実施形態に係る位置姿勢計測装置によれば、撮像装置の位置及び姿勢を構成する一部のパラメータのみを選択してセンサ計測値の補正を行う場合に、その制約の中で指標上での誤差の和を最小化するような補正を行うことができる。したがって、未知とするパラメータを減らすことによって撮像装置の位置及び姿勢の計測を安定化させようとする場合であっても、従来と比べて高精度な計測を行うことができる。
【０１１９】
［第２の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等に設定された基準座標系における任意の計測対象物体の位置及び姿勢の計測を行う。
【０１２０】
本実施形態に係る位置姿勢計測装置は、撮像装置に位置姿勢センサが取り付けられておらず、三脚等によって既知の位置及び姿勢に固定して設置されていることが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。
【０１２１】
図１２は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１２００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。
【０１２２】
撮像装置１３０は、三脚１２８０によって、空間中の既知の位置、姿勢に固定して設置されている。撮像装置１３０が出力する撮影画像は、位置姿勢計測装置１２００の画像入力部１６０に入力される。基準座標系における撮像装置１３０の位置及び姿勢は、既知の値として予め保持されているものとする。
【０１２３】
センサ計測値入力部１５０は、計測対象物体１７０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１２２０へと出力する。
【０１２４】
位置姿勢算出部１２２０は、センサ計測値入力部１５０の出力である計測対象物体１７０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、計測対象物体１７０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【０１２５】
なお、図１２に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１２２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１２６】
本実施形態における位置姿勢算出部１２２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）とほぼ同一である。唯一の違いは、撮像装置１３０の位置及び姿勢が、計測値として与えられるのではなく、既知の値として予め保持されていることである。位置姿勢算出部１２２０は、撮像装置１３０の位置及び姿勢の既知の値を、撮像装置１３０の位置及び姿勢の仮の計測値とおき、第１の実施形態におけるステップＳ３０００からステップＳ３０７０までと同一の工程によって撮像装置１３０の位置及び姿勢の計測値の補正を行い、この補正結果に基づいて、ステップＳ３０８０において、第１の実施形態と同様に計測対象物体１７０の位置及び姿勢の計測値を補正する（式１）。本実施形態では、位置姿勢算出部１２２０がステップＳ３０８０において出力するのは、計測対象物体１７０の位置及び姿勢のみである。
【０１２７】
以上によって、計測対象物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１２８】
［第３の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。
【０１２９】
本実施形態においては撮像装置以外の計測対象物体は存在せず、本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢の計測のみを行うことが、第１の実施形態とは異なっている。以下では、第１の実施形態との相違点のみを説明する。
【０１３０】
図１３は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１３００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０によって構成されており、撮像装置１３０及び位置姿勢センサ１４０に接続されている。
【０１３１】
また、現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標として、基準座標系における位置が既知である複数個の指標Ｑｋが配置されている。指標Ｑｋは、第１の実施形態と同様に、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、どのような形態であってもよい。
【０１３２】
センサ計測値入力部１５０は、撮像装置１３０の位置及び姿勢の計測値を位置姿勢センサ１４０から入力し、位置姿勢算出部１３２０へと出力する。
【０１３３】
位置姿勢算出部１３２０は、センサ計測値入力部１５０の出力である撮像装置１３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、撮像装置１３０の位置及び姿勢の計測値の誤差を補正し、補正後の位置及び姿勢のデータを出力する。
【０１３４】
なお、図１３に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１３２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１３５】
本実施形態における位置姿勢算出部１３２０の処理手順は、第１の実施形態における位置姿勢算出部１２０の処理手順（図３のフローチャートに対応）と類似しているため、図３を参照しながら、第１の実施形態との相違点のみについて説明する。
【０１３６】
ステップＳ３００５において、位置姿勢算出部１３２０は、センサによって計測された撮像装置１３０の位置及び姿勢を、センサ計測値入力部１５０から入力する。
【０１３７】
ステップＳ３０２５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の姿勢計測値のうちのパン及びチルト角に対して、検出されている指標上での誤差（基準座標系における指標の３次元座標ｘＷＱｋｎと撮像装置１３０の位置及び姿勢の計測値から導出する投影座標の理論値ｕＱｋｎ＊と、実際の検出座標ｕＱｋｎとの誤差）を打ち消すような補正を加え、ステップＳ３０８０に処理を進める。
【０１３８】
このような補正処理としては、周知の方法を用いることができるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの回転による補正方法（段落「００１９」〜「００２９」）およびランドマークを複数点利用したカメラの回転による補正方法（段落「００３９」〜「００５０」））に開示されている方法を用いることができる。
【０１３９】
ステップＳ３０３５において、位置姿勢算出部１３２０は、ステップＳ３００５で得た撮像装置１３０の位置計測値を固定値として、撮像装置１３０の姿勢計測値を表す３パラメータ（パン，チルト及びロール角）に対して、検出されている全指標上での誤差の和を最小とするような補正を加え、ステップＳ３０８０に処理を進める。この補正処理工程は、第１の実施形態におけるステップＳ３０３５の補正処理工程（図４のフローチャートに対応）と類似したものとなる。ただし、本実施形態の場合、指標の観測方程式Ｆ’’ｃ（）が、基準座標系における指標Ｑｋｎの座標ｘＷＱｋｎ（既知な情報として予め保持している）から画像座標を算出する関数となる。すなわち、観測方程式Ｆ’’ｃ（）は次式によって表される。
【０１４０】
【数１９】
2006215924.tif 000021
【０１４１】
具体的には、観測方程式Ｆ’’ｃ（）は、ｘＷＱｋｎとsから当該指標のカメラ座標ｘＣＱｋｎを求める次式、
【０１４２】
【数２０】
2006215924.tif 000022
【０１４３】
及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める式６によって構成されており、この点のみが異なっている。ここでtWCは、撮像装置１３０の位置計測値を表す３次元ベクトルであり、ここでは固定値として取り扱われる。
【０１４４】
ステップＳ３０７０において、位置姿勢算出部１３２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この処理工程での具体的な処理は、例えば特許文献２に開示されているような周知技術を用いて実現することが可能であるので、ここでの説明は省略する。
【０１４５】
ステップＳ３０８０において、位置姿勢算出部１３２０は、ステップＳ３０７０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【０１４６】
以上によって、撮像装置の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１４７】
［第４の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における任意の計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。本実施形態においては、撮像装置ではなく、任意の計測対象物体の位置及び姿勢を計測することが、第３の実施形態と異なっている。以下では、第３の実施形態との相違点のみを説明する。
【０１４８】
図１４は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１４００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、撮像部１４３０、位置姿勢計測部１４４０、及び位置姿勢算出部１４２０によって構成されており、計測対象物体１４７０に接続されている。
【０１４９】
撮像部１４３０は、位置及び姿勢を計測したい対象である計測対象物体１４７０に固定して取り付けられており、指標を含む現実空間の映像を撮影する。
【０１５０】
位置姿勢計測部１４４０は、撮像部１４３０に固定して装着されており、基準座標系における撮像部１４３０の位置及び姿勢を計測してセンサ計測値入力部１５０へと出力する。位置姿勢計測部１４４０は、例えば、米国Polhemus社のFASTRAK等によって構成される。
【０１５１】
画像入力部１６０は、撮像部１４３０が撮影した撮影画像をデジタルデータに変換し、指標検出部１１０へと出力する。
【０１５２】
センサ計測値入力部１５０は、撮像部１４３０の位置及び姿勢の計測値を位置姿勢計測部１４４０から入力し、位置姿勢算出部１４２０へと出力する。
【０１５３】
位置姿勢算出部１４２０は、センサ計測値入力部１５０の出力である撮像部１４３０の位置及び姿勢の計測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力し、入力した情報に基づいて、撮像部１４３０の位置及び姿勢の計測値の誤差を補正する。
位置姿勢算出部１４２０はさらに、得られた撮像部１４３０の位置及び姿勢と、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係についての情報（具体的には、計測対象物体１４７０上のどの位置に撮像部１４３０が設置されているかという情報。計測対象物体１４７０によって定義される物体座標系における、撮像部１４３０の位置及び姿勢によって表現される）から、基準座標系における計測対象物体１４７０の位置及び姿勢を算出し、Ｉ／Ｆ１００９を介してこのデータを外部へと出力する。あるいは、このデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。なお、この変換工程は必ずしも必要ではなく、撮像部１４３０の位置及び姿勢をそのまま出力することも可能である。
【０１５４】
なお、本実施形態では、位置姿勢計測部１４４０は撮像部１４３０の位置及び姿勢を計測していたが、位置姿勢計測部１４４０は計測対象物体１４７０の位置及び姿勢を計測するように構成されていても良い。この場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた計測対象物体１４７０の位置及び姿勢から撮像部１４３０の位置及び姿勢を算出し、これを初期値として上記と同様な処理を実行する。
【０１５５】
また、位置姿勢算出部１４２０は、ステップＳ３０２５，Ｓ３０３５，Ｓ３０７０の何れの工程においても、撮像部１４３０の姿勢あるいは位置及び姿勢を補正し、その後計測対象物体１４７０の位置及び姿勢を求めていた。しかし、ステップＳ３０３５及びＳ３０７０の工程においては、計測対象物体１４７０の位置及び姿勢を直接求めるような構成としても良い。
【０１５６】
位置姿勢計測部１４４０が撮像部１４３０の位置及び姿勢を計測している場合には、位置姿勢算出部１４２０は、既知の値として予め保持している撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を用いて、計測値として得られた撮像部１４３０の位置及び姿勢から計測対象物体１４７０の位置及び姿勢を算出し、これを初期値とする。
【０１５７】
ステップＳ３０３５においては、計測対象物体１４７０の姿勢を表す３値ベクトルs = ωWO = [ξWO  ψWO  ζWO]Tと未知パラメータとする。また、ステップＳ３０７０においては、計測対象物体１４７０の位置及び姿勢を表す６値ベクトルs = [tWOT ωWOT]T = [xWO  yWO  zWO  ξWO  ψWO  ζWO]Tを未知パラメータとする。そして、観測方程式Ｆ’’ｃ（）を定義するのに必要な式２０を、
【０１５８】
【数２１】
2006215924.tif 000023
【０１５９】
に変更する。ここで、tOC 及びROCは、計測対象物体１７０によって定義される物体座標系における撮像部１４３０の姿勢を表す３×３の回転行列及び位置を表す３次元ベクトルであり、撮像部１４３０と計測対象物体１４７０との相対的な位置姿勢の関係を表す既知の値として、予め保持されているものとする。なお、ステップＳ３０３５では、tWOは固定値として設定する。
【０１６０】
以上によって、任意の物体の位置及び姿勢が計測される。このように、本実施形態に係る位置姿勢計測装置によっても、指標の分布範囲の大きさを考慮した手法の選択が行われるため、画像上の一部の領域に指標が偏在して観測されている場合であっても適切な位置姿勢推定手法が選択され、従来手法と比して安定した解を得ることができる。
【０１６１】
［第５の実施形態］
本実施形態に係る位置姿勢計測装置は、室内等の空間中における撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置について説明する。
【０１６２】
図１５は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１５００は、画像入力部１６０、指標検出部１１０、センサ計測値入力部１５５０、及び位置姿勢算出部１５２０によって構成されており、撮像装置１３０及び姿勢センサ１５４０に接続されている。
【０１６３】
本実施形態においては、撮像装置１３０に、位置姿勢センサではなく姿勢センサが装着されている点が、第３の実施形態と異なっている。姿勢センサを用いることで、位置姿勢センサを用いる場合に存在する、センサの計測範囲以外の領域を計測できないという制約を回避することができる。以下では、第３の実施形態との相違点のみを説明する。
【０１６４】
姿勢センサ１５４０は、撮像装置１３０に装着されており、姿勢センサ１５４０自身の現在の姿勢を計測してセンサ計測値入力部１５５０へと出力する。姿勢センサ１５４０は、例えばジャイロセンサをベースとしたセンサユニットであり、具体的には、株式会社トキメックのTISS-5-40や、米国InterSense社のInertiaCube2等によって構成される。これらのセンサによって計測される姿勢計測値は、真の姿勢とは異なった、誤差を有する姿勢である。ただし、これらの姿勢センサは、地球の重力方向を観測する傾斜角センサを構成要素として有しており、傾斜角方向（ピッチ角及びロール角）のドリフト誤差の蓄積をキャンセルする機能を有しているので、傾斜角方向に関してはドリフト誤差を発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては時間の経過に伴って蓄積されるドリフト誤差を有している。
【０１６５】
センサ計測値入力部１５５０は、方位ドリフト誤差補正値の更新値を位置姿勢算出部１５２０から入力し、姿勢センサ１５４０の現在の方位ドリフト誤差補正値を更新し保持する。また、姿勢センサ１５４０から姿勢計測値を入力し、現在の方位ドリフト誤差補正値によってこれを補正して、撮像装置１３０の姿勢の予測値として位置姿勢算出部１５２０へと出力する。
【０１６６】
位置姿勢算出部１５２０は、センサ計測値入力部１５５０の出力である撮像装置１３０の姿勢の予測値と、指標検出部１１０の出力である各々の指標Ｑｋｎの画像座標ｕＱｋｎを入力データとして、撮像装置１３０の位置及び姿勢を算出して出力する。また、位置及び姿勢の算出工程で導出される姿勢センサ１５４０の方位ドリフト誤差補正値の更新値を、センサ計測値入力部１５５０へと出力する。
【０１６７】
なお、図１５に示した画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０の少なくとも一部は、独立した装置として実現しても良いし、夫々１つもしくは複数のコンピュータにインストールし、コンピュータのＣＰＵにより実行することで、その機能を実現するソフトウェアとして実現しても良い。本実施形態では、各部（画像入力部１６０、指標検出部１１０、センサ計測値入力部１５０、及び位置姿勢算出部１５２０）は、それぞれソフトウェアにより実現され、同一のコンピュータにインストールされているものとする。ソフトウェアを実行することで、各部の機能を実現するためのコンピュータの基本構成は、第１の実施形態と同様であるので説明を省略する。
【０１６８】
図１６は、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部１５２０のソフトウェアのプログラムを実行することで行われる。
【０１６９】
ステップＳ１５０００において、位置姿勢算出部１５２０は、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎを指標抽出部１１０から入力する。なお、各指標の基準座標系における３次元座標ｘＱｋｎは、既知の値としてＲＡＭ１００２に予めロードされているものとする。
【０１７０】
ステップＳ１５００５において、位置姿勢算出部１５２０は、撮像装置１３０の姿勢の予測値R*をセンサ計測値入力部１５５０から入力し、これを、撮像装置１３０の姿勢の初期値として設定する。また、２回目以降の処理では、１ループ前の処理において算出された撮像装置１３０の位置を、撮像装置１３０の姿勢の初期値として設定する。
【０１７１】
ステップＳ１５０１０において、位置姿勢算出部１５２０は、指標が検出されているか否かを判定する。指標が検出されていない場合はステップＳ１５０９０に処理を進め、それ以外の場合はステップＳ１５０２０に処理を進める。
【０１７２】
ステップＳ１５０２０において、位置姿勢算出部１５２０は、検出された指標の総数が一つか否かを判定する．指標の総数が一つの場合はステップＳ１５０２５に処理を進め、それ以外の場合はステップＳ１５０３０に処理を進める。
【０１７３】
ステップＳ１５０２５において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値の平行移動成分のうちの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加え、ステップＳ１５０９０に処理を進める。ステップＳ１５０２５での処理工程のとしては、周知の処理を提供することが可能であるため、これ以上の説明は行わない。具体的には、例えば特許文献１（ランドマーク１点を利用したカメラの平行移動による補正方法（段落「００３０」〜「００３６」）およびランドマークを複数点利用したカメラの平行移動による補正方法（段落「００５１」〜「００５６」）に開示されている方法を用いることができる。ただし、特許文献１では６自由度位置姿勢センサの計測値を誤差補正の対象とした場合を説明しているが、本実施形態では、前フレームで求めた位置と姿勢センサの計測値とを組み合わせたものが補正対象となる点が異なっている。
【０１７４】
ステップＳ１５０３０において、位置姿勢算出部１５２０は、検出された指標の総数が二つか否かを判定する．指標の総数が二つの場合はステップＳ１５０３２に処理を進め、それ以外の場合はステップＳ１５０４０に処理を進める。
【０１７５】
ステップＳ１５０３２において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離を算出し、その値と所定の値として定められている閾値Ｔ１（例えば、画像の対角線長の１／８）との比較を行う。距離が閾値Ｔ１以上の場合にはステップＳ１５０３４に処理を進め、それ以外の場合はステップＳ１５０２５に処理を進める。
【０１７６】
ステップＳ１５０３４において、位置姿勢算出部１５２０は、検出された二つの指標の画像上での距離と所定の値として定められている閾値Ｔ２（例えば、画像の対角線長の１／４）との比較を行う。距離が閾値Ｔ２以上の場合にはステップＳ１５０３８に処理を進め、それ以外の場合はステップＳ１５０３６に処理を進める。
【０１７７】
ステップＳ１５０３６において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうち、位置を表す３パラメータ（平行移動成分）を更新対象として（すなわち、姿勢計測値には誤差は含まれていないものと仮定して）、検出されている指標上での誤差を最小とするような補正を位置のパラメータのみに加え、ステップＳ１５０９０に処理を進める。この処理工程の詳細は、例えば特許文献３（第６の実施形態）に開示されているので、ここでの説明は省略する。また、位置を表す３パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置の補正を行うことができる。
【０１７８】
ステップＳ１５０３８において、位置姿勢算出部１５２０は、ステップＳ１５００５で設定した初期値のうちの位置を表す３パラメータ（平行移動成分）と、姿勢センサ１３４０の方位ドリフト誤差補正値の更新値φの合計４パラメータとを、算出すべき未知パラメータとして取り扱い、検出されている指標の情報を用いてこれらを算出する。この処理工程の詳細は、例えば特許文献３（第７の実施形態）に開示されているので、ここでの説明は省略する。また、これら４パラメータを未知変数とした観測方程式を構成し、画像ヤコビアンを用いた誤差最小化計算を行うことによっても、画像上での誤差を最小とするような位置と方位ドリフト誤差補正値の更新値の算出を行うことができる。さらに、ステップＳ１５０８０へと処理を進める。
【０１７９】
ステップＳ１５０４０において、位置姿勢算出部１５２０は、検出された全指標（このステップが実行されるのは、指標の総数が三つ以上の場合のみである）の画像座標を包含する凸包を算出する。
【０１８０】
ステップＳ１５０５０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ３（例えば、画像の面積の１／１６）との比較を行う。面積が閾値Ｔ３以上の場合はステップＳ１５０６０に処理を進め、それ以外の場合はステップＳ１５０３６へと処理を進め、上述した処理を行う。
【０１８１】
ステップＳ１５０６０において、位置姿勢算出部１５２０は、Ｓ１５０４０で算出した凸包の面積と所定の値として定められている閾値Ｔ４との比較を行う。面積が閾値Ｔ４（例えば、画像の面積の１／９）以上の場合はステップＳ１５０７０に処理を進め、それ以外の場合はステップＳ１５０３８へと処理を進め、上述した処理を行う。
【０１８２】
ステップＳ１５０７０において、位置姿勢算出部１５２０は、検出されている全指標上での誤差の和を最小とするような撮像装置１３０の位置と姿勢を算出する。この算出は、例えば、撮像装置１３０の位置と姿勢を表す６パラメータを変数として、ステップＳ１５００５で設定した初期値を用いて、非線形方程式の反復解法によって誤差を最小化するパラメータを求めることによって実現される。すなわち、位置と姿勢を表す６パラメータを未知変数とした観測方程式を構成し、これに基づいて求められる指標投影位置の誤差と各パラメータに関する画像ヤコビアンとを用いて当該パラメータの補正値を求め、補正処理を反復的に行うことで画像上での誤差を最小とするような位置と姿勢を算出する。
位置姿勢算出部１５２０はさらに、ステップＳ１５００５で設定した姿勢の初期値と求められた姿勢との変化分を算出し、その方位成分を方位ドリフト誤差補正値の更新値φとする。
【０１８３】
ステップＳ１５０８０において、位置姿勢算出部１５２０は、ステップＳ１５０３８あるいはステップＳ１５０７０において得られた方位ドリフト誤差補正値の更新値φを、センサ計測値入力部処理１５５０へと出力する。
【０１８４】
ステップＳ１５０９０において、位置姿勢算出部１５２０は、ステップＳ１５０８０までの結果として得られた撮像装置１３０の位置及び姿勢を、Ｉ／Ｆ１００９を介して外部へと出力する。あるいは、これらのデータを、他のアプリケーションから利用可能な状態としてＲＡＭ１００２上に格納する。
【０１８５】
ステップＳ１５１００において、位置姿勢算出部１５２０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ１５０００へと進める。
【０１８６】
以上によって、撮像装置の位置及び姿勢が計測される。
本実施形態に係る位置姿勢計測装置によれば、ステップＳ１５０３４やステップＳ１５０５０によって指標の分布範囲の大きさを考慮した手法の選択（方位ドリフト誤差補正値の更新を行うか否かの判断）が行われるため、画像上の一部の領域に指標が偏在して観測されている場合に、方位ドリフト誤差補正値の更新を不正確に行ってしまうという状況を回避することができる。
【０１８７】
また、本実施形態に係る位置姿勢計測装置によれば、第３の実施形態の効果に加え、ステップＳ１５０６０による手法の選択によって、画像情報が不足する場合であっても姿勢センサによる傾斜角情報を信頼することで位置及び姿勢を安定して導出できるほか、画像情報が十分得られる場合であれば位置及び姿勢の推定を高精度に行えるという、通常の画像情報に基づく位置姿勢推定手法が有する利点を両立することができる。
【０１８８】
［他の実施形態］
（変形例１）
上記の実施形態の各々では、画像上における指標の分布に基づいて、適用する補正アルゴリズムが選択される。これらの補正アルゴリズム（第１の実施形態におけるステップＳ３０２５，Ｓ３０３５，Ｓ３０７０、第２乃至第４の実施形態におけるこれらの変形、及び、第５の実施形態におけるステップＳ１５０２５，Ｓ１５０３６，Ｓ１５０３８，Ｓ１５０７０）は、必ずしも上記の実施形態で示したものである必要はなく、指標の分布に従って見込まれる画像情報の情報量に応じて適切に設定されたものであれば、他の補正アルゴリズムに変更することも可能である。
【０１８９】
例えば、第１乃至第４の実施形態におけるステップＳ３０２５やＳ３０３５において、姿勢の計測値を補正するのではなく、位置の計測値を補正する構成としてもよい。この場合、ステップＳ３０２５では、例えば、特許文献１の段落「００３０」〜「００３６」および「００５１」〜「００５６」に開示されているような、１つ又は複数の指標を利用したカメラの平行移動による補正方法に基づいて、位置計測値のうちのカメラの視軸と直交する２軸方向の値を補正対象として、検出されている指標上での誤差を打ち消すような補正を加えることができる。
【０１９０】
また、ステップＳ３０３５では、算出すべき未知パラメータを、撮像装置１３０の位置を表す３値ベクトルs = tWC = [xWC  yWC  zWC ]Tとして、図５と同様な処理手順によって位置の補正を行う。第１の実施形態との相違点は、観測方程式を構成する式５を、次式のように変更することである。
【０１９１】
【数２２】
2006215924.tif 000024
【０１９２】
ここで、RWCは、固定値として用いる撮像装置１３０のセンサによる姿勢計測値を表している。また、RWO 及びtWOは、第１の実施形態における式５と同様に、計測対象物体１７０の位置及び姿勢のセンサによる計測値を表しており、固定値として取り扱われる。
【０１９３】
なお、式２２は線形方程式であるので、式６と組み合わせた観測方程式から連立方程式を作成しこれを解くことで、繰り返し演算を用いなくても補正値を得ることができる。
【０１９４】
なお、不図示のＵＩを介して、姿勢の補正と位置の補正の何れを優先させるかをユーザが自由に選択できるようにしてもよい。この場合、ステップＳ３０２５及びＳ３０３５の実行時に、ユーザの選択に応じて、姿勢あるいは位置の補正アルゴリズムが実行される。これにより、特性の異なる位置姿勢センサを用いた場合でも、より好ましい計測を行うことができる。
【０１９５】
また、ステップＳ３０２５においては、計測値として得られる６パラメータのうちの任意の２パラメータを補正対象として選択し、これを未知数として上述した方法と記と同様にして求めてもよい。同様に、ステップＳ３０３５においては、計測値として得られる６パラメータのうちの任意の３パラメータを補正対象として選択し、これを未知数として上述した方法と同様な枠組みによって求めてもよい。
【０１９６】
（変形例２）
上記第１乃至第４の実施形態では、画像上における指標の分布に基づいて、３種類のアルゴリズムへと処理を分岐させていた（ステップＳ３０３３、ステップＳ３０５０及びステップＳ３０６０）。しかし、指標の分布に基づいて行う分岐数は３通りに限られるものではない。例えば、ステップＳ３０５０の処理を省いて、閾値Ｔ３と凸包の面積の大小関係によってステップＳ３０３５あるいはステップＳ３０７０へと処理を分岐させるだけでもよい。また、より多数の閾値を設けて、４通り以上の分岐を行ってもよい。例えば、補正対象とするパラメータの数（２〜６）を凸包の面積に応じて変更し、これを未知数として同様な方法により位置姿勢を算出してもよい。
【０１９７】
（変形例３）
上記の実施形態の各々では、位置姿勢センサや姿勢センサによる計測値と、撮像装置によって得られる画像情報を併用することにより、計測対象（計測対象物体や撮像装置）の位置及び姿勢を求めていた。しかし、画像上における指標の分布に基づいて、位置及び姿勢を求めるために用いるアルゴリズムを選択するという基本的な技術思想は、これらのセンサによる計測値を用いずに、画像情報のみから計測対象の位置及び姿勢を得る場合であっても適用できる。
例えば、図１３に示した第３の実施形態の構成において、位置姿勢計測装置１３００がセンサ計測値入力部１５０を有さず、また、位置姿勢センサ１４０が撮像装置１３０に設置されていない場合を考える。この場合、位置姿勢算出部１３２０は、初期値として位置姿勢センサ１４０による撮像装置１３０の位置及び姿勢の計測値を用いるのではなく、前フレームで求めた撮像装置１３０の位置及び姿勢の算出値（あるいは、過去の算出値から予測した現フレームにおける位置及び姿勢）を用いることにすると、これを仮の計測値とおいて図３と同様な処理を行うことで、指標の分布に応じて適切な位置及び姿勢の算出アルゴリズムを選択することが可能となる。
【０１９８】
（変形例４）
上記の実施形態の各々では、指標の分布を計る尺度として、指標が３点以上検出されている場合には、指標が成す凸包を求めてその面積を利用していた。しかし、指標の分布を測る尺度はこれに限られるものではなく、他の尺度を用いてもよい。例えば、画像中心から最も遠い位置にある指標を第１の指標として選択し、第１の指標から最も遠い位置にある指標を第２の指標として選択し、さらに、第１，第２の指標を結ぶ直線から最も遠い距離にある指標を第３の指標として選択し、これら第１，第２，第３の指標が成す三角形の面積を分布の尺度として、凸包の面積の変わりに用いても良い。また、簡易的には、このような第１，第２の指標間の距離を分布の尺度としても良いし、指標の画像座標の分散や標準偏差といった、分布を数値で表しうる他の任意の情報を分布の尺度として用いてもよい。
【０１９９】
（変形例５）
第１の実施形態におけるステップＳ３０３５では、図４のステップＳ４０４０にて詳細に説明したように、誤差ベクトルＵと行列Θに基づく補正値Δsの算出に、式１１で表現される最急降下法を用いている。しかし、補正値Δsの算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるLM法（Levenberg-Marquardt法）を用いて求めてもよいし、公知のロバスト推定手法であるM推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても発明の本質が損なわれないことは言うまでもない。また、上記の実施形態におけるsを状態ベクトル、式４を観測方程式とすることで、画像ヤコビアンを利用して位置姿勢センサによる計測値のうちの一部のパラメータのみを補正するという効果を有した拡張カルマンフィルタ（Extended Kalman Filter）や、反復拡張カルマンフィルタ（iterative Extended Kalman Filter）を構成することができる。なお、拡張カルマンフィルタ及び反復拡張カルマンフィルタについては非特許文献２に記載されるように周知であるので、その詳細に関するこれ以上の説明は省略する。
【０２００】
（変形例６）
上記の実施形態の各々においては、各々が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を用いていた。しかし、点指標以外の指標を用いることも可能である。例えば、公知の位置姿勢計測装置（例えば、非特許文献３を参照）に用いられているような、線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。例えば、原点から線指標までの距離を評価のための基準として画像からの検出値とsによる推定値から算出する誤差によって誤差ベクトルＵを構成し、観測方程式をsの各要素で偏微分した解を各要素に持つ１行×６列のヤコビ行列によって行列Θを構成することで、上記実施形態と同様にして位置及び姿勢の計測（補正）を行うことができる。また、線指標と点指標、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することも可能である。
【０２０１】
（変形例７）
上記の実施形態の各々では、画像面積に対する指標の分布に応じて以降の処理を選択していた。しかし、計測対象物体に対する相対的な比率を尺度としてもよい。例えば、第１の実施形態において、ステップＳ３０４０で、全検出指標の成す凸包の面積ＳＨに加えて、画像上への計測対象物体１７０（その大まかな形状を表す３次元データが記憶されているものとする）の投影像の面積ＳＯを求め、これらの比率（ＳＨ／ＳＯ）を得る。そして、上記比率に対して適宜設定した閾値を用いてステップＳ３０５０とステップＳ３０６０での条件分岐を行えばよい。
【０２０２】
また、画像中ではなく、物理空間における指標の分布を尺度とすることも可能である。例えば、第１の実施形態において、ステップＳ３０４０で、物体座標系における全検出指標の３次元座標によって構成される３次元凸包を求め、その体積ＶＨを算出し、計測対象物体１７０の体積ＶＯとの比率（ＶＨ／ＶＯ）を得ることで、計測対象物体１７０上における検出指標の分布の割合を導出する。そして、上記比率に対して適宜設定した閾値を用いてステップＳ３０５０とステップＳ３０６０での条件分岐を行えばよい。また、物体座標系における互いが最も遠い３点の検出指標によって構成される三角形の面積を求め、計測対象物体１７０のサイズ（例えば、外接直方体の最も長い２辺が成す矩形の面積）との比率を求め、これを尺度として用いることも可能である。
【０２０３】
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても達成される。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０２０４】
さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０２０５】
本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０２０６】
【図１】第１の実施形態における位置姿勢計測装置の構成を示す図である。
【図２】位置姿勢計測装置の各部をソフトウェアにより実現することのできるコンピュータの基本構成を示す図である。
【図３】実施形態における、撮像装置１３０及び計測対象物体１７０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図４】図３のステップＳ３０３５における位置及び姿勢算出処理の詳細を説明するフローチャートである。
【図５】図３のステップＳ３０７０における位置及び姿勢算出処理の詳細を説明するフローチャートである。
【図６】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図７】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図８】撮像装置１３０によって計測対象物体１７０を撮影することで取得される撮影画像の一例を示す図である。
【図９】図６の撮影画像（撮影画像６００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１０】図７の撮影画像（撮影画像７００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１１】図８の撮影画像（撮影画像８００）に対してステップＳ３０４０の処理を実行した際に得られる凸包を示す図である。
【図１２】第２の実施形態における位置姿勢計測装置の構成を示す図である。
【図１３】第３の実施形態における位置姿勢計測装置の構成を示す図である。
【図１４】第４の実施形態における位置姿勢計測装置の構成を示す図である。
【図１５】第５の実施形態における位置姿勢計測装置の構成を示す図である。
【図１６】第５の実施形態における、撮像装置１３０の位置及び姿勢を算出する処理を説明するフローチャートである。
【図１】
2006215924.tif 000025
【図２】
2006215924.tif 000026
【図３】
2006215924.tif 000027
【図４】
2006215924.tif 000028
【図５】
2006215924.tif 000029
【図６】
2006215924.tif 000030
【図７】
2006215924.tif 000031
【図８】
2006215924.tif 000032
【図９】
2006215924.tif 000033
【図１０】
2006215924.tif 000034
【図１１】
2006215924.tif 000035
【図１２】
2006215924.tif 000036
【図１３】
2006215924.tif 000037
【図１４】
2006215924.tif 000038
【図１５】
2006215924.tif 000039
【図１６】
2006215924.tif 000040