JP4532856B2 - 位置姿勢計測方法及び装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、現実空間を撮像する撮像部と姿勢変化量を計測する姿勢変化計測部を備える撮像装置の位置姿勢を計測する位置姿勢計測技術に関するものである。
【０００２】
【従来の技術】
現実空間を撮像するカメラなどの撮像部（以下適宜カメラと言い換える）の位置姿勢計測は、例えば現実空間と仮想空間とを融合表示する複合現実感システムにおいて必要となる。
【０００３】
特許文献１乃至４において従来の技術として開示されるように、このようなカメラの位置姿勢を求める方法として、当該カメラの撮像画像を用いずに、例えば磁気センサ等の位置姿勢センサによってカメラの位置姿勢を計測する方法がある（以下、方法１と呼ぶ）。
【０００４】
特許文献１乃至３記載の発明は、方法１の方法では十分な精度が得られないという課題に着目した発明である。そして、方法１に加えて、現実空間に配置した位置が既知のマーカ、または現実空間中の位置が既知の特徴点（以下、マーカと特徴点を合わせて指標という）を用いて、カメラの位置姿勢を測定する位置姿勢センサの計測誤差を補正する方法が開示されている。特許文献１乃至３で開示される方法は、個々に計算原理や手段、工程が異なるものの、カメラの位置姿勢を測定するための６自由度の位置姿勢センサから得られる情報と、現実空間に配置した位置が既知の点の情報及びこれらの点をカメラで捉えた情報とを基にして、カメラの位置姿勢を求めるという点で共通である。そこで、これらの方法をまとめて、以降では方法２と呼ぶ。
【０００５】
また、別の方法として、非特許文献１乃至３などに開示されるように、現実空間（シーン）に存在する指標をカメラで撮像して得られる情報のみから、このカメラの位置姿勢を求める方法が数多く実施されている。カメラ以外にセンサを用いないこの方法を、以降では方法３とよぶ。
【０００６】
方法３は、画像処理のみによってカメラの位置姿勢を測定する方法であるため、高速な頭部の動きによってフレーム間での指標の位置が大きく変化するような場合には追跡に失敗してしまう問題がある。
【０００７】
この問題に対処する方法として、非特許文献４では、カメラ以外のセンサを組み合わせて利用する方法が提案されている。この方法は、現在のフレームで検出した指標の座標とジャイロセンサから得た角速度から、次フレームにおける指標の座標を予測することにより、画像中の指標の検出精度を向上させる方法である。さらに、この方法を発展させて、非特許文献５では、指標の座標予測方法に、画像情報から得られる情報を追加する改良が加えられた方法が提案されている。カメラ以外のセンサを用いて指標の座標を予測するこれらの方法を、以降では方法４と呼ぶ。
【０００８】
これ以外の方法として、同時に、ジャイロセンサなどの慣性センサを利用する方法の別の方法として、拡張カルマンフィルタを用いる方法が提案されている。これは、方法４のように、次画像フレームにおける指標の予測計算のために慣性センサを単純に利用する方法ではない。これは、カメラ位置姿勢の推定計算（予測計算）を行う拡張カルマンフィルタに対して、慣性センサ計測情報を直接的に入力値として組み込む試みである。同時に、指標の画像座標値も拡張カルマンフィルタへの入力となる。
【０００９】
すなわち、慣性センサの計測値と画像から検出する指標の位置を情報の入力源とし、拡張カルマンフィルタにこれらの情報を直接的に適用し、カメラの現在の位置姿勢を予測的に推定する方法である（慣性センサや画像から得られる指標の位置は、現在のカメラの位置姿勢を基準とすると過去に得られた情報であるため、現在の位置姿勢の推定は予測的に求められることとなる）。この方法は、正確に一定間隔で連続した時間にて、途切れなく連続して得られるセンサの計測情報と画像から検出した指標の情報とを用いて、連続したカメラの動きを予測することによって、カメラの位置姿勢を推定する方法である。この方法として、非特許文献６では、６個の加速度センサを組み合わせることで３軸の並進加速度と３軸の回転角加速度を得て、それを拡張カルマンフィルタへの入力として用いることでカメラの位置と姿勢を推定する方法が提案されている。また、非特許文献７及び８においても、ジャイロセンサと拡張カルマンフィルタを用いる手法が提案されている。ジャイロセンサと拡張カルマンフィルタを用いるこれらの方法を方法５と呼ぶ。
以上のように、大きく分けて、方法１から方法５までの方法が従来の技術として存在する。
【００１０】
【特許文献１】
特開平１１−０８４３０７号公報
【特許文献２】
特開２０００−０４１１７３号公報
【特許文献３】
特願２０００−３６４２３０号（特開２００２−２２８４４２号公報）
【特許文献４】
特開２００２−２３０５８６号公報
【００１１】
【非特許文献１】
W. A. Hoff and K. Nguyen, "Computer vision-based registration techniques for augmented reality", Proc. SPIE, vol.2904, pp. 538-548, Nov. 1996
【非特許文献２】
U. Neumann and Y. Cho, " A self-tracking augmented reality system&quot, Proc. VRST '96, pp.109-115, July 1996.
【非特許文献３】
暦本 純一、「２次元マトリックスコードを利用した拡張現実感システムの構成手法」、インタラクティブシステムとソフトウェアIV、近代科学社、pp.199-208, Dec. 1996.
【非特許文献４】
S. You, U. Neumann, and R. Azuma,"Hybrid inertial and vision tracking for augmented reality registration", Proc. IEEE Virtual Reality '99, pp.260-267, Mar. 1999.
【非特許文献５】
藤井, 神原, 岩佐, 竹村、「拡張現実感のためのジャイロセンサを併用したステレオカメラによる位置合わせ」 信学技報, PRMU99-192, Jan. 2000.
【非特許文献６】
横小路, 菅原, 吉川、「頭部装着型ディスプレイのための視覚と加速度を用いた位置追跡法」,日本バーチャルリアリティ学会第2回大会論文集, pp.121-124, Sep. 1997.
【非特許文献７】
L. Chai, K. Nguyen, B. Hoff, and T. Vincent,"An adaptive estimator for registration in augmented reality", ibid, pp.23-32, Oct. 1999.
【非特許文献８】
S. You and U. Neumann, "Fusion of vision and gyro tracking for robust augmented reality registration", Proc. IEEE Virtual Reality 2001, pp.71-78, Mar. 2001.
【００１２】
【発明が解決しようとする課題】
方法１は、方法２ですでに問題視されているように、一般に６自由度センサのみを利用する方法では、得られる位置姿勢の精度が十分ではないという問題がある。また、６自由度センサは一般に、カメラに固定して用いるセンサ部分の他に、このセンサ部分で検出するための信号を出力するための、現実空間（カメラを除く）に固定された別の装置を必要とする。例えば、磁気式の６自由度センサの場合、測定対象に取り付けられたセンサで計測するための磁場を発生する装置が、光学式センサの場合には、受光装置あるいは、発光装置がこの別の装置に相当する。そして、この別の装置が必要であることは、センサの計測範囲がある限られた範囲に拘束されることを意味する。また、設置場所の関係上、別の装置の設置自体が困難であったり、設置可能であっても取り付けが面倒であるといった問題点もある。さらには、センサ自体が一般に高価格であるという問題点もある。
【００１３】
方法２は、方法１の問題点のうち、計測精度が十分でない点に鑑みて考案された方法であり、計測精度が低いという問題点に対しては解決している。しかし、６自由度センサを利用する以上、計測範囲の問題、設置の問題、価格の問題は解決されない。
【００１４】
方法３は、６自由度センサを用いない方法であるため、原理的に計測範囲の問題も存在せず、設置やコストの問題もない。また、現実空間内で位置が既知である指標が、十分な数カメラにて観察できているならば、位置姿勢の計測精度も十分である。しかし、一旦、指標がカメラから観察できない状態になると、カメラの位置姿勢を全く求めることができなくなってしまう。そのため、カメラが動きうるあらゆる位置とあらゆる姿勢にて、常に、必要十分な情報量の指標が見えていなければならないこととなる。すなわち、非常に多くの指標を現実空間内に配置しておく、あるいは、指標の３次元位置を何らかの方法で入力しておかなければならないという問題がある。
【００１５】
さらには、画像から指標を検出する際に、指標の検出安定性に問題がある場合がある。これを詳しく説明すると、指標の検出を安定化するためには、個々の指標について、単体でその指標を識別、特定することが可能となるよう、面的に広がりをもつ大きな指標を用い、かつ、カメラで得られる画像中にてその指標が十分に大きな面積を占めるような条件になるようにしなければならない。しかし、そのような大きな指標を、現実空間に常に設置できるとは限らない。そのため、大きな指標を用いる代わりに、単体ではどの指標か識別できないような例えば点状の小さな指標を用いざるを得ない状況が生じる。この場合、例えば急激なカメラの動きがあった場合、指標の検出に失敗する可能性が高くなる。
【００１６】
方法４は、方法３の指標の検出の安定性の問題に鑑みて考案された方法である。しかし、この方法では、画像からの指標検出に先立って、指標位置を予測するためだけにジャイロセンサを用いており、指標の検出の安定性を増す以外に関しては、方法３と同じ問題を持っている。なお、方法３においては、ジャイロセンサにより得られる情報は、カメラの位置姿勢の推定計算には利用されておらず、位置姿勢の推定計算に関しては方法３と同様である。ジャイロセンサのような慣性センサは、現実空間内に設置しなければならない装置はないため、方法１および方法２のもつ計測範囲の問題と設置の問題はない。また、一般に低価格であるため、方法１および方法２の価格の問題も解決する方法である。
【００１７】
方法５は、以上の方法１から方法４までの方法とは異なり、正確に一定間隔の連続した時間にて、途切れなく連続してカメラの位置姿勢を推定する方法である。方法５では、一定間隔で連続した情報を取得する必要があり、また、一定間隔で推定計算を連続的に行う必要がある。しかし、得られる位置姿勢を複合現実感システムに応用するような場合を考慮すると、仮想空間の描画という必要時間の不確かな処理が工程内に含まれるため、描画が終わる時点でのカメラの位置姿勢が、拡張カルマンフィルタによる計算を行う時点では予測できず、必ずしも有効に機能しない。その上、同じ理由で推定計算を正確な一定間隔で行うこと自体困難である場合がある。そのため、複合現実感システムにはそもそも方法５を適用することが困難であったり、適用しても計算結果が正しく求められなくなるなどの問題がある。
【００１８】
また、拡張カルマンフィルタの設計と利用局面に合わせたパラメータの調整が繊細であり、良好な動作を実現するための設定を行うことが難しいという問題がある。良好な設定が行えなかった場合、ある一時刻の情報のみを用いる方法１から方法３までの方法では起こりえない問題として、位置姿勢計測結果が時間軸方向でふらつく（ある一時刻で切り出してみると、その時点での画像中の指標位置が合っていない状態になりえるといった位置姿勢の精度が悪い結果を生む）現象が発生しうる。なお、方法４は、ジャイロセンサの計測結果を指標の検出領域の推定という限定した形態で利用するため、このような方法５の問題は発生せず、基本的に方法１から方法３と同様である。
本発明は上述の問題点に鑑みてなされたものであり、位置姿勢計測において、簡便な構成により精度の高い計測結果を得ることができるようにすることを目的とする。
【００１９】
【課題を解決するための手段】
本発明の目的を達成するために、例えば本発明の、現実空間画像を連続して撮像する撮像装置の位置姿勢を計測する位置姿勢計測方法は、撮像装置に設けられた角速度センサにより順次計測される角速度に基づいて、前回の位置姿勢計測時から現処理対象の現実空間画像を撮像した時までの撮像装置の姿勢の変化量を計測する姿勢変化計測工程と、撮像装置が撮像した現処理対象の現実空間画像中に含まれる指標の画像中の座標を求める第１の画像座標計算工程と、記憶手段に記憶された、前回の位置姿勢計測時に計測された撮像装置の位置姿勢と、姿勢変化計測工程で得られる姿勢の変化量とに基づいて、現処理対象の現実空間画像を撮像した時の撮像装置の位置姿勢を推定する位置姿勢推定工程と、当位置姿勢推定工程で得た位置姿勢における、撮像装置の撮像画面中における指標の座標を求める第２の画像座標計算工程と、第１の画像座標計算工程で求めた指標の座標と、第２の画像座標計算工程で求めた指標の座標とから得られる情報量に応じた修正方法により、位置姿勢推定工程で推定した位置姿勢を修正する位置姿勢修正工程と、修正された位置姿勢を計測結果として記憶手段に記憶する記憶制御工程とを有することを特徴とする。
【００２０】
また、本発明の、現実空間画像を連続して撮像する撮像装置の位置姿勢を計測する位置姿勢計測装置は、撮像装置に設けられた角速度センサにより順次計測される角速度に基づいて、前回の位置姿勢計測時から現処理対象の現実空間画像を撮像した時までの撮像装置の姿勢の変化量を計測する姿勢変化計測手段と、撮像装置が撮像した現処理対象の現実空間画像中に含まれる指標の画像中の座標を求める第１の画像座標計算手段と、記憶手段に記憶された、前回の位置姿勢計測時に計測された撮像装置の位置姿勢と、姿勢変化計測手段で得られる姿勢の変化量とに基づいて、現処理対象の現実空間画像を撮像した時の撮像装置の位置姿勢を推定する位置姿勢推定手段と、当位置姿勢推定手段で得た位置姿勢における、撮像装置の撮像画面中における指標の座標を求める第２の画像座標計算手段と、第１の画像座標計算手段で求めた指標の座標と、第２の画像座標計算手段で求めた指標の座標とから得られる情報量に応じた修正方法により、位置姿勢推定手段で推定した位置姿勢を修正する位置姿勢修正手段と、修正された位置姿勢を計測結果として記憶手段に記憶する記憶制御手段とを有することを特徴とする。
【００２２】
また、上述の目的は、本発明の位置姿勢計測方法をコンピュータ装置に実現させるためのコンピュータプログラム及び、このコンピュータプログラムを記録したコンピュータ装置読み取り可能な記録媒体によっても達成される。
【００２３】
【発明の実施の形態】
以下添付図面を参照して、本発明を好適な実施形態に従って詳細に説明する。
［第１の実施形態］
図１に本実施形態における位置姿勢測定装置１００を頭部装着型装置（ＨＭＤ＝Head Mounted Display）１０１に適用した場合の機能構成を示す。ＨＭＤ１０１は、カメラ１０２と角速度センサ１０３と表示器１０４を有し、現実空間内を移動可能である。カメラ１０２は現実空間を撮像し、撮像した画像を表す信号を撮像画像取得部１０５へ出力する。角速度センサ１０３は、振動ジャイロなどの原理に基づいて、ＨＭＤの動きの角速度を計測する。表示器１０４は、画像を表示することのできる表示器であり、通常は右目用、左目用の２つの表示器から構成される。
【００２４】
なお、本実施形態においては、カメラ１０２、角速度センサ１０３及び表示器１０４の３つがその位置を固定されてなるＨＭＤ１０１を用いているが、カメラ１０２と角速度センサ１０３とが固定された位置関係を有して現実空間内を移動可能でありさえすれば、必ずしも表示器１０４は他の２つに対して固定されていなくてもよいし、ＨＭＤ１０１内に内蔵されていなくてもよい。
【００２５】
１０５は、撮像画像取得部であり、カメラ１０２から送られてきた信号を受け、画像データを生成する。得られる画像データは、１０６の特徴点検出部に送られると共に、後述の撮像画像描画部１１１へと送られる。特徴点検出部１０６は、入力された画像データに含まれる、現実空間中の指標（予め配置したマーカでも良いし、建造物や特徴的な地形等の特徴点でも良い）を検出する機能を有する。このとき、画像における指標の位置（画像中の２次元座標）も検出される。
【００２６】
位置姿勢更新処理部１０７は、角速度センサ１０３からの角速度信号と、例えば装置内のクロック信号を利用した計時処理によって得られる経過時間から、姿勢の変化量を計算し、この変化量を位置姿勢バッファ１１０に保持されている過去の値または位置姿勢の最初期値１０８に単純に積算することによって、位置姿勢（を表す情報）を更新する。カメラ位置姿勢推定演算部１０９は、位置姿勢更新処理部１０７にて得られたカメラの位置姿勢に基づいて、現実空間内にある指標の３次元位置を画像面へと投影し、得られる画像上での指標の投影点と、特徴点検出部１０６にて得られている指標の画像面上での検出位置とに基づいて、カメラの位置姿勢を推定する。この推定処理に関しては詳しく後述する。
【００２７】
カメラ位置姿勢推定演算部１０９で得られたカメラの位置姿勢は、位置姿勢バッファ１１０に保持されると共に、仮想空間融合描画部１１２に渡される。位置姿勢バッファ１１０は、カメラ位置姿勢推定演算部１０９にて求めたカメラの位置姿勢を保持しておくバッファであり、保持された位置姿勢は上述のように位置姿勢更新部１０７で利用される。
【００２８】
撮像画像の描画部１１１と仮想空間融合描画部１１２は、本実施形態に係る位置姿勢測定装置１００の出力するカメラ位置姿勢を利用する装置の一例であり、位置姿勢の測定とは無関係な構成要素である。
【００２９】
撮像画像の描画部１１１は、特徴点検出部１０６と同様に、画像取得部１０５にて得られた画像データを受け取り、現実空間の撮像結果そのものであるこの画像データを、表示器１０４への表示用のバッファ（図示せず）に描画する。
【００３０】
仮想空間融合描画部１１２は、撮像画像描画部１１１にて描画したバッファと同じ表示バッファに対して仮想空間（仮想物体画像）を上書き描画することによって、仮想空間の融合描画を行う。より具体的には、カメラ１０２で撮影した現実空間（現実空間画像）と、予め定められた仮想空間モデルをカメラ位置姿勢推定演算部１０９で求めたカメラの位置姿勢に基づいて描画することによって生成した仮想空間（仮想物体画像）とを合成する。
【００３１】
仮想空間融合描画部１１２は、表示バッファの内容をＨＭＤ１０１に内蔵されている表示器１０４へ送信する。これにより表示器１０４では融合描画結果が表示される。
【００３２】
このような構成を有する位置姿勢計測装置１００は、ＨＭＤ１０１のカメラ１０２からの映像信号及び、角速度センサ１０３からの検出信号を受信可能であれば、ＨＭＤ１０１が移動する範囲とは無関係に任意の場所に設置可能であり、６自由度センサを用いる際に必要な別の装置とは大きく異なる。
【００３３】
図２は、図１で示した本実施形態におけるＨＭＤ１０１を利用する環境を示す模式図である。この図にあるように、ＨＭＤ１０１には、カメラ１０２が内蔵され、角速度センサ１０３が固定され、また表示器１０４が内蔵されている。このＨＭＤ１０１は現実空間内で自由に移動することが可能であり、観察対象となる現実空間には、指標としてのマーカ２０４が配置されている。なお、上述したように、指標は現実空間内での３次元位置が既知な点であり、かつ、指標を撮影した画像から特徴点検出部１０６にて検出可能な特徴量をもつ。
【００３４】
検出可能な特徴量を与えるには、例えばマーカ２０４の形状や色、その面積等を、撮影した画像から特徴点検出部１０６において検出可能であるように定めることによって実現することができる。
【００３５】
引き続いて、上記構成を備える位置姿勢計測装置１００が行う処理について、図３と図４に示したフローチャートを参照して説明する。図３と図４は、本実施形態におけるメインの処理のフローチャートである。なお、これらの二つの図は、紙面の都合で二つに分割しているだけであり、フローチャート結合子により接続している通り、ひとつのフローチャートである。
【００３６】
ステップＳ３０１にて、まず、カメラの初期位置姿勢を設定する。カメラの初期位置姿勢とは、以降のステップで利用するカメラの位置姿勢の初期値のことであり、本実施形態を実行する時点でのカメラのおおよその位置姿勢を表すものである。この初期位置姿勢は、処理開始時におけるカメラの正確な位置姿勢を表している必要はなく、おおよその位置と姿勢を表現していればよい。
【００３７】
ステップＳ３０１で用いることの可能な初期位置姿勢としては、例えば、起動時点でのカメラの位置姿勢が大きく変化しないという仮定の基に、予め定数値として保存してある値を用いることができる。ここでは、初期値１０８に格納してある定数値であるカメラの位置姿勢初期値を、カメラの位置姿勢として設定するものとする。
【００３８】
また、定数値を用いる代わりに、位置姿勢の測定対象であるカメラ（カメラ１０２）で撮像した画像を用いて、その画像特徴を利用して推定したカメラの位置姿勢を用いることが可能である。この場合、カメラの位置姿勢の６自由度を推定することが可能となるマーカ（初期位置姿勢設定用マーカ）を、図２のマーカ２０４とは別に用意し、それを近傍に配置しておく。そして、初期位置姿勢設定用マーカが撮像されるようにカメラを設定した状態にて処理を開始し、ステップＳ３０１にて、カメラで撮像された初期位置姿勢設定用マーカからカメラの位置姿勢を推定し、初期位置姿勢として利用する。
【００３９】
次に、ステップＳ３０２とステップＳ３０４を並列に実行する。ステップＳ３０２は、後述するステップＳ３０３と連なって連続的に実行されるステップであり、ステップＳ３０４はこのステップのみで連続的に実行されるステップである。ステップＳ３０２とＳ３０３の説明の前に、これらのステップで行う処理の説明のため、まずは、ステップＳ３０４を説明する。
【００４０】
ステップＳ３０４では、カメラ１０２からの映像を撮像画像取得部１０５にて取得する処理を実行する。カメラ１０２は、連続した時間にて連続したフレームの画像を撮像しつづけており、この取得工程では、この連続したフレームにおけるある一時刻の画像の１枚分の信号を取得する。ある一時刻の画像がすべて取得できたタイミングで、後述するステップＳ３０３で利用する画像の取得が終了したことを表すトリガーを発生する。また、取得された画像データは、ステップＳ３０５への処理にて利用される。このステップＳ３０４は、１画面分の画像を取得する処理が終了すると、すぐさま次のフレームの取得を行うために、再びステップＳ３０４に処理が渡される。すなわち、ステップＳ３０４は連続的にカメラからの画像データを取得しつづけ、１枚分の画像データが取得できたタイミングで、トリガーを発生するステップである。
【００４１】
ステップＳ３０４にて、１枚分の画像データの取得が終了すると、引き続きステップＳ３０５が実行され、ステップＳ３０５では、得られた画像を基にして特徴点検出部１０７により、その画像から２０４に示すマーカの検出を行う。後の説明のため、得られた画像上でのマーカの検出点の位置をＶ_i（ｉ＝１，２，...）と表現しておく。
【００４２】
ステップＳ３０２では、角速度センサ１０３を用いて角速度の計測を行う。次に、ステップＳ３０３にて、ステップＳ３０２で計測した角速度を、経過時間を基に積算する。ステップＳ３０３の処理終了後、直ちにステップＳ３０２の処理が行われ、角速度の計測が再び行われる。ここで、経過時間とはステップＳ３０３にて積算処理を行う間隔であり、ステップＳ３０２の計測とステップＳ３０３の計算時間との両方を加味した微小な時間のことである。この２つのステップは、微小な時間を均等に刻むクロックに同期して動作させることができ、そのクロックの間隔を経過時間として利用することができる。このようなクロックは、コンピュータ機器の動作用に通常用いられるクロック発生器が出力するクロックを用いて生成することが可能である。
【００４３】
ステップＳ３０２及びＳ３０３は繰り返し実行され、ステップＳ３０２で計測された角速度は、随時ステップＳ３０３にて積算される。ただし、ステップＳ３０３は、単純に積算しつづけるだけではなく、ステップＳ３０４が１枚の画像取得後に発生するトリガーを入力として、そのトリガーを受けた時点での積算結果をステップＳ３０６に渡し、積算値を０にリセットする。
【００４４】
ステップＳ３０６では、ステップＳ３０３からデータが受け渡されるタイミング、すなわち、ステップＳ３０３がステップＳ３０４のトリガーを入力として積算値を出力するタイミングにて、その時点で保存されているカメラの位置姿勢を基にして、ステップＳ３０３で得られた角速度の積算値を足し合わせることにより、新たなカメラ位置姿勢を計算する。ここで、本フローチャートが動作を開始してステップＳ３０６を初めて実行する際に用いられるカメラの位置姿勢は、ステップＳ３０１にて設定されたカメラの位置姿勢の初期値である。後述するように、以降のステップを経た後、再びこのステップＳ３０６が実行されるが、その段階では、後述するステップにより計算されたカメラの位置姿勢を用いることとなる。
【００４５】
ステップＳ３０７では、ステップＳ３０６にて得られたカメラの位置姿勢を利用して、その位置姿勢で撮影されるシーン（現実空間）中に配置されているマーカ２０４の３次元位置をカメラの撮像面上での２次元位置、すなわち、カメラにて得られる画像面上での２次元位置に投影計算する。この投影計算は、マーカ２０４の座標値、すなわち、シーン内に配置されている世界座標系でのマーカの座標値を、カメラ座標系、すなわち、カメラの位置を原点としカメラの姿勢に応じて軸の向きが変化する三次元座標系の座標値へと変換するビューイング変換を行う工程と、カメラ座標系の座標値を画像面へと透視投影変換する工程の２つの工程により計算することができる。以下にその方法の詳細を示す。
【００４６】
まず、カメラの位置を表す３次元ベクトルをｔ_ConW、姿勢を表す３ｘ３回転変換行列をＲ_ConWとすると、カメラ座標系上の点Ｘ_c＝［ｘ_cｙ_cｚ_c］を世界座標系（図１）上の点Ｘ_w＝［ｘ_wｙ_wｚ_w］に変換する変換式は、
【００４７】

【００４８】
と表せる。ここで、姿勢の計測値を、回転軸ベクトル［ｒ_x ｒ_y ｒ_z］（ｒ_x ²＋ｒ_y ²＋ｒ_z ²＝１）と回転角ｒ_aによって表現すると、
【００４９】

として計算することができる。
【００５０】
ビューイング変換とは、この式（１）で表された座標変換の逆変換に相当する変換である。すなわち、世界座標系上の点Ｘ_w＝［ｘ_wｙ_wｚ_w］から、カメラ座標系Ｘ_c＝［ｘ_cｙ_cｚ_c］への変換は、
【００５１】

【００５２】
と表される。
ここで、以降の工程の説明のため、新たに
【００５３】

【００５４】
とし、Ｍ_conW ^-1の回転成分をＲ、平行移動成分をｔとおく。これにより、
Ｘ_c＝ＲＸ_w＋ｔ‥‥ （４）
と表すことができる。補足として、このＲ，ｔは、カメラの位置姿勢の計測値そのものではない（逆変換の回転成分と平行移動成分である）。この式（４）のＸ_wとして、マーカの位置を用いることにより、マーカの世界座標系上での座標値からカメラ座標系の座標値を計算することができる。
【００５５】
次に、カメラ座標系上の点を画像面へと透視投影変換する。投影される画像面上の２次元点の座標をＵ＝［ｕ_xｕ_y］^tとすると、次式により計算することができる。
【００５６】

【００５７】
ここで、ｆ_x，ｆ_yはカメラのｘ軸方向とｙ軸方向それぞれの焦点距離である。カメラの焦点距離は物理的には１つの値を持つが、ここではｘ軸方向とｙ軸方向のアスペクト比を同時に表現するため、２つの値をもって焦点距離として表している。また、焦点距離はカメラ固有の値であり予め求められているものとする。
【００５８】
この透視投影変換式（５）を、
（ｕ_xｕ_y）＝Ｐ（ｘ_cｙ_cｚ_c）‥‥ （６）
Ｕ＝Ｐ（Ｘ_c）
と表現すると、世界座標系上にある点が画面上に変換される式は、式（４）と式（６）から、
Ｕ＝Ｐ（ＲＸ_w＋ｔ）‥‥ （７）
と表すことができる。
【００５９】
すなわち、ステップＳ３０７で行うマーカの３次元位置の画像面への投影計算とは、世界座標系上でのマーカの点の座標値をＸ_wとし、式（７）を用いて画像面の位置Ｕを計算する工程である。
【００６０】
次に、ステップＳ３０８にて、ステップＳ３０７で得られた画像面上でのマーカの投影位置と、ステップＳ３０５にて得られている、画像中のマーカ位置とを比較し、画像面上での距離が十分に近いマーカ対を同じマーカであると同定する。
【００６１】
ステップＳ３０９では、ステップＳ３０８にて同定できたマーカが、ステップＳ３０６で求めたカメラの位置姿勢を６自由度全てについて修正可能な情報量を有するかの判定の一例として、同定されたマーカが３点（３対）以上あるかどうかを判定する。３点以上ある場合にはステップＳ３１１へと進み、２点以下の場合にはステップＳ３１０へと進む処理を行う。すなわち、ステップＳ３１０ではさらに、同定されたマーカがあるかどうかを判定し、ある場合にはステップＳ３１３へと進み、ない場合にはステップＳ３１４へと進む処理を行う。
【００６２】
次に、ステップＳ３１１およびＳ３１２での処理を説明する。ステップＳ３１１とＳ３１２では、３点以上の同定されたマーカを利用したカメラ位置姿勢の推定（初期値の修正）を行う。式（４）の回転成分Ｒは、式（２）で示したように回転軸と回転角で表すことができ、さらに回転角を回転軸の長さで（（ω_x，ω_y，ω_z）＝（ｒ_aｒ_x，ｒ_aｒ_y，ｒ_aｒ_z））と表現することにより３値で表現できる。そこで、回転成分Ｒと平行移動成分ｔを合わせた６自由度のパラメータをベクトルＳ＝［ω_x ω_y ω_z ｔ_x ｔ_y ｔ_z］^tとおく。これにより、３次元空間内での回転と平行移動による座標変換であるビューイング変換式（４）をＸ_c＝Ｔ（Ｓ，Ｘ_w）と表現するとすると、投影変換まで含めた変換式（７）は、
Ｕi＝Ｎ^〜（Ｔ（Ｓ，Ｘ_i））＝Ｆ（（Ｓ，Ｘ_i））‥‥ （８）
と表現できる。
【００６３】
一方、画像上で検出した指標の位置に関しては、３次元位置姿勢センサの計測値を基にして得られたビューイング変換の６自由度パラメータＳと真のカメラの位置姿勢から得られるであろうビューイング変換の６自由度パラメータの差をΔＳと表現すると（すなわち、真のビューイング変換の６自由度パラメータはＳ＋ΔＳとなる）、
Ｖi＝Ｎ^〜（Ｔ（Ｓ＋ΔＳ，Ｘ_i））＝Ｆ（（Ｓ＋ΔＳ，Ｘ_i））‥‥ （９）
という式が成り立つ。
【００６４】
もしも、ステップＳ３０６で得られているカメラの位置姿勢が真値を表現していれば、ΔＳは０でありＵ_iとＶ_iは同じ点となるが、ステップＳ３０６で得られているカメラの位置姿勢は真値ではないため、Ｕ_iとＶ_iは同じ点にはならない。カメラの位置姿勢を求めるということは、Ｕ_iとＶ_iとの間の誤差を基にして、ΔＳもしくはＳ＋ΔＳを推定するということである。言い換えれば、この２点の距離、すなわち画像面上での誤差ベクトル、
ΔＵ_i＝Ｕ_i−Ｖ_i‥‥ （１０）
の長さの二乗誤差
【００６５】

が、最小になるΔＳもしくはＳ＋ΔＳを求めることができればよい。
【００６６】
これは、ニュートン法などの繰り返し演算を用いて、求めることができる。以下では、ニュートン法を用いた場合を例に説明する。まず、式（８）の右辺をｔ_x，ｔ_y，ｔ_z，ω_x，ω_y，ω_zで偏微分した関数を求める。式（８）をｔ_x，ｔ_y，ｔ_z，ω_x，ω_y，ω_zという６変数で偏微分した解を各要素に持つヤコビ行列は、
【００６７】

【００６８】
である。ここで、ｊ要素数のベクトルａからｋ要素数のベクトルｂへのヤコビ行列は、ｊ×ｋ要素数の行列となりこれをＪ_baと表示している。
ｕ_xとｕ_yとをなんらかの変数ｐで偏微分すると、式（５）及び式（６）から、
【００６９】

となることから、式（１２）の行列の右側３列の各要素は、
【００７０】

としてカメラ座標系上での指標の位置と焦点距離により求めることができる。
【００７１】
また同様に、式（１２）の行列の左側３列の各要素は、

である。ここで、

【００７２】
はその詳細は述べないが世界座標系上での指標の位置と回転軸回転角により求めることができるので、式（１４）すなわち式（１２）の行列の左側３列の要素は、世界座標系上での指標の位置と回転軸回転角により求めることができる。
【００７３】
さらに、式（１２）から、

【００７４】
という式が成り立つ。これは、Ｓの微小な変化量に対して、そのときのＳの値を基にして得られるＪ_usを用いて、Ｕの微小な変化量を計算することができることを意味している。このことから、実際の誤差量ΔＵとΔＳとは、ΔＳが小さいときには、
【００７５】

【００７６】
という関係となる。ここで、この補正計算に用いる点（指標）は、ｎ点（ｎ≧３）あることを考える。式（１６）は、それぞれの点にて成り立つことから、ｎ点のベクトルをさらに垂直に並べたベクトルを用いて、
【００７７】

【００７８】
という式が成り立つ。式（１７）中の［ΔＵ₁ ΔＵ₂ … ΔＵ_n］と［Ｊ_us1 Ｊ_us2 … Ｊ_usn］をそれぞれ
【００７９】

【００８０】
とおくと、すべての点にて同じ重みで誤差最小となるΔＳは、Φの一般化逆行列（疑似逆行列）を用いて、
【００８１】

として求めることができる。
【００８２】
これは、ニュートン法でＳを求める問題において、Ｓがある状態にあるときに、次のＳを求める計算過程に相当する。言い換えると、ニュートン法の繰り返し最適化過程のある一過程の計算として適用できる。すなわち、ある初期値Ｓ₀があるときに、不変量である指標の世界座標系上での位置ｘ₁,ｘ₂,…,ｘ_nと、誤差ベクトル［ΔＵ₁ ΔＵ₂ … ΔＵ_n］とを基にして式（１９）を用いてΔＳを求めることができる。
【００８３】
この結果を用いて、Ｓ₁＝Ｓ₀＋ΔＳとし、［ΔＵ₁ ΔＵ₂ … ΔＵ_n］を計算し直し、再び式（１９）を用いてΔＳを求める。さらにこの結果を用いて、Ｓ₂＝Ｓ₁＋ΔＳとし、再び同じ計算を行うという処理を［ΔＵ₁ ΔＵ₂ … ΔＵ_n］が０に近づくまで、あるいはｋ回繰り返したときにとＳ_k-1とＳ_kの差がほとんどなくなるまで繰り返す。この繰り返し処理により、画像面上での指標の投影位置と検出位置の誤差が最小になるカメラのビューイング変換の６自由度パラメータをＳ_kとして求めることができる。
【００８４】
ステップＳ３１１では、この繰り返し演算に必要となる初期値として、ステップＳ３０６にて得られているカメラの位置姿勢を設定する処理を行う。この値を基にして上述の繰り返し演算を行う処理が、ステップＳ３１２である。
【００８５】
ここで、式（１１）の誤差errが最小になるようにＳを求める方法は、なんらかの繰り返し演算を用いればよいだけであり、必ずしもニュートン法を用いる必要はない。例えば、Levenberg-Marquardt法（ＬＭ法）や、シンプレックス法などが利用できることが知られている。
【００８６】
以上でステップＳ３１１、およびＳ３１２によるカメラの位置姿勢推定方法について述べた。次に、ステップＳ３０９にてＮｏと判定され、ステップＳ３１０にてＹｅｓと判定された場合、すなわち、利用できるマーカが１点もしくは２点しかない場合に進むステップＳ３１３について説明する。この場合、利用できるマーカ点数が少ないため、カメラの位置姿勢という６自由度すべてが最適になるようにカメラの位置姿勢を推定することはできない。
【００８７】
そこで、カメラの姿勢の３自由度（座標軸周りの回転方向の自由度）、もしくは、カメラの位置の３自由度（座標軸に平行な並進方向の自由度）のいずれかのみを補正する処理を行う。利用できるマーカが１点しかない場合に関しては、その点が完全に一致するように、姿勢もしくは位置を補正する方法を用いる。もしも、利用できるマーカが２点ある場合には、２点ともが完全に一致する補正量の平均的な補正量を用いて、補正を行う。
【００８８】
次に、ステップＳ３１０にてＮｏと判定された場合に進むステップＳ３１４について説明する。ステップＳ３１４では、マーカを利用したカメラ位置姿勢の推定や補正を行うことができないため、ステップＳ３０６にて得られているカメラの位置姿勢をそのまま現在のカメラ位置姿勢であると見做す。
【００８９】
ステップＳ３１２、ステップＳ３１３、またはステップＳ３１４のいずれの工程を経る場合であっても、これらの工程からは、図４のステップＳ４０１へと処理が続く。ステップＳ４０１では、ステップＳ３０４にて撮像画像取得部１０５により得られている画像を、撮像画像の描画部１１１を用いて描画する。次に、ステップＳ４０２では、ステップＳ３１２、Ｓ３１３、Ｓ３１４のいずれかの方法により得られているカメラの位置姿勢を用いて、仮想空間の融合描画部１１２により仮想空間（仮想物体の画像）を撮像画像描画結果の上に上書き描画する。この結果、現実空間と仮想空間の融合描画結果が得られる。
【００９０】
ステップＳ４０３では、得られた融合描画結果を表示部１０４に表示する。これにより、ＨＭＤ１０１を通して、観察者は現実空間と仮想空間の融合画像を観察することができる。この後、ステップＳ４０４にて、全処理を終了するかどうかを判定する。
【００９１】
もしも、処理を終了する場合には、ステップＳ３０２とＳ３０３の処理と、ステップＳ３０４の処理を終了させ、全工程を終了する。終了しない場合、すなわち、継続してカメラの位置姿勢を求め続け、融合画像の表示を行う場合には、ステップＳ３０６へと進む。ステップＳ３０６では、上述した通り、その処理を行う時点で得られているカメラの位置姿勢を基にした処理を行うため、ステップＳ３１２、Ｓ３１３、Ｓ３１４のいずれかの方法により得られたカメラの位置姿勢が利用される。
【００９２】
【他の実施形態】
上述の実施形態においては、本発明による位置姿勢計測装置をＨＭＤに設けたカメラの位置姿勢検出に適用した場合のみを説明したが、本発明による位置姿勢計測装置は、他の任意の撮像装置の位置姿勢計測に適用可能である。
【００９３】
また、上述の実施形態においては、検出対象となる撮像装置の姿勢を検出する角速度センサを用いた場合のみを説明したが、前回求めた位置姿勢から現時点での位置姿勢を推定可能な情報が得られれば、角速度センサに限らず６自由度センサ以外の任意のセンサからの信号を利用しうる。
【００９４】
上述の実施形態においては、１つの機器から構成される位置姿勢計測装置についてのみ説明したが、同等の機能を複数の機器から構成されるシステムによって実現しても良い。
【００９５】
尚、前述した実施形態の機能を実現するソフトウェアのプログラム（実施形態では図３、図４に示すフローチャートに対応したプログラム）を、記録媒体から直接、或いは有線／無線通信を用いて当該プログラムを実行可能なコンピュータを有するシステム又は装置に供給し、そのシステム或いは装置のコンピュータが該供給されたプログラムを実行することによって同等の機能が達成される場合も本発明に含む。
【００９６】
従って、本発明の機能処理をコンピュータで実現するために、該コンピュータに供給、インストールされるプログラムコード自体も本発明を実現するものである。つまり、本発明の機能処理を実現するためのコンピュータプログラム自体も本発明に含まれる。
【００９７】
その場合、プログラムの機能を有していれば、オブジェクトコード、インタプリタにより実行されるプログラム、ＯＳに供給するスクリプトデータ等、プログラムの形態を問わない。
【００９８】
プログラムを供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、磁気テープ等の磁気記録媒体、ＭＯ、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−ＲＷ、ＤＶＤ−ＲＯＭ、ＤＶＤ−Ｒ、ＤＶＤ−ＲＷ等の光／光磁気記憶媒体、不揮発性の半導体メモリなどがある。
【００９９】
有線／無線通信を用いたプログラムの供給方法としては、コンピュータネットワーク上のサーバに本発明を形成するコンピュータプログラムそのもの、もしくは圧縮され自動インストール機能を含むファイル等、クライアントコンピュータ上で本発明を形成するコンピュータプログラムとなりうるデータファイル（プログラムデータファイル）を記憶し、接続のあったクライアントコンピュータにプログラムデータファイルをダウンロードする方法などが挙げられる。この場合、プログラムデータファイルを複数のセグメントファイルに分割し、セグメントファイルを異なるサーバに配置することも可能である。
【０１００】
つまり、本発明の機能処理をコンピュータで実現するためのプログラムデータファイルを複数のユーザに対してダウンロードさせるサーバ装置も本発明に含む。
【０１０１】
また、本発明のプログラムを暗号化してＣＤ−ＲＯＭ等の記憶媒体に格納してユーザに配布し、所定の条件を満たしたユーザに対して暗号化を解く鍵情報を、例えばインターネットを介してホームページからダウンロードさせることによって供給し、その鍵情報を使用することにより暗号化されたプログラムを実行してコンピュータにインストールさせて実現することも可能である。
【０１０２】
また、コンピュータが、読み出したプログラムを実行することによって、前述した実施形態の機能が実現される他、そのプログラムの指示に基づき、コンピュータ上で稼動しているＯＳなどが、実際の処理の一部または全部を行ない、その処理によっても前述した実施形態の機能が実現され得る。
【０１０３】
さらに、記録媒体から読み出されたプログラムが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれた後、そのプログラムの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行ない、その処理によっても前述した実施形態の機能が実現され得る。
【０１０４】
【発明の効果】
以上の説明により、本発明によれば、６自由度センサを用いる必要がないため、６自由度センサの計測範囲の問題、設置の問題、価格の問題を解決することができる。また、カメラの姿勢変化量を計測するため、カメラで得られる画像のみから位置姿勢を推定する方法のもつ指標の検出の不安定を引き起こすことがない。さらに、カメラの姿勢変化量の計測結果を、指標の検出位置の予測だけに留まらず、カメラの位置姿勢の計測にも直接利用することができる。加えて、拡張カルマンフィルタを用いる方法のように、正確な一定間隔での位置姿勢の予測計算を行う必要無く、カメラの位置姿勢を計測することが可能となる。
【図面の簡単な説明】
【図１】 本発明の一実施形態に係る位置姿勢計測装置とＨＭＤを有するシステムの機能構成を示すブロック図である。
【図２】 図１のシステムの使用状態を説明する模式図である。
【図３】 本発明の一実施形態に係る位置姿勢計測装置が行う位置計測処理を説明するフローチャートである。
【図４】 本発明の一実施形態に係る位置姿勢計測装置が行う位置計測処理を説明するフローチャートである。

Claims (10)

1. 現実空間画像を連続して撮像する撮像装置の位置姿勢を計測する位置姿勢計測方法であって、
   前記撮像装置に設けられた角速度センサにより順次計測される角速度に基づいて、前回の位置姿勢計測時から現処理対象の現実空間画像を撮像した時までの前記撮像装置の姿勢の変化量を計測する姿勢変化計測工程と、
   前記撮像装置が撮像した前記現処理対象の現実空間画像中に含まれる指標の画像中の座標を求める第１の画像座標計算工程と、
   記憶手段に記憶された、前記前回の位置姿勢計測時に計測された前記撮像装置の位置姿勢と、前記姿勢変化計測工程で得られる姿勢の変化量とに基づいて、現処理対象の現実空間画像を撮像した時の前記撮像装置の位置姿勢を推定する位置姿勢推定工程と、
   当該位置姿勢推定工程で得た位置姿勢における、前記撮像装置の撮像画面中における指標の座標を求める第２の画像座標計算工程と、
   前記第１の画像座標計算工程で求めた指標の座標と、前記第２の画像座標計算工程で求めた指標の座標とから得られる情報量に応じた修正方法により、前記位置姿勢推定工程で推定した位置姿勢を修正する位置姿勢修正工程と、
   前記修正された位置姿勢を計測結果として前記記憶手段に記憶する記憶制御工程と
   を有することを特徴とする位置姿勢計測方法。
2. 前記位置姿勢が座標軸周りの回転方向の３自由度と、前記座標軸に平行な並進方向の３自由度の計６自由度を有し、
   前記位置姿勢修正工程が、前記情報量が前記位置姿勢の６自由度のどの程度を修正するに足りるかに応じて、前記修正方法を決定することを特徴とする請求項１記載の位置姿勢計測方法。
3. 前記情報量が、前記第１の画像座標計算工程で求めた指標と、前記第２の画像座標計算工程で求めた指標とのうち、対応関係にある指標対を同定することによって得られることを特徴とする請求項１又は請求項２に記載の位置姿勢計測方法。
4. 前記情報量が、前記同定された指標対の数であることを特徴とする請求項３記載の位置姿勢計測方法。
5. 前記修正方法には、前記同定された各指標対間の距離が最小となるように前記位置姿勢推定工程で推定した位置姿勢を修正する方法が含まれることを特徴とする請求項３又は請求項４記載の位置姿勢計測方法。
6. 前記修正方法には、前記同定された指標対の少なくとも１つが一致するような補正に基づいて前記位置姿勢推定工程で推定した位置姿勢を修正する方法が含まれることを特徴とする請求項３乃至請求項５のいずれか１項に記載の位置姿勢計測方法。
7. 前記修正方法には、前記同定された指標対がない場合、前記位置姿勢推定工程で推定した位置姿勢をそのまま修正後の位置姿勢とする方法が含まれることを特徴とする請求項３乃至請求項６のいずれか１項に記載の位置姿勢計測方法。
8. 現実空間画像を連続して撮像する撮像装置の位置姿勢を計測する位置姿勢計測装置であって、
   前記撮像装置に設けられた角速度センサにより順次計測される角速度に基づいて、前回の位置姿勢計測時から現処理対象の現実空間画像を撮像した時までの前記撮像装置の姿勢の変化量を計測する姿勢変化計測手段と、
   前記撮像装置が撮像した前記現処理対象の現実空間画像中に含まれる指標の画像中の座標を求める第１の画像座標計算手段と、
   記憶手段に記憶された、前記前回の位置姿勢計測時に計測された前記撮像装置の位置姿勢と、前記姿勢変化計測手段で得られる姿勢の変化量とに基づいて、現処理対象の現実空間画像を撮像した時の前記撮像装置の位置姿勢を推定する位置姿勢推定手段と、
   当該位置姿勢推定手段で得た位置姿勢における、前記撮像装置の撮像画面中における指標の座標を求める第２の画像座標計算手段と、
   前記第１の画像座標計算手段で求めた指標の座標と、前記第２の画像座標計算手段で求めた指標の座標とから得られる情報量に応じた修正方法により、前記位置姿勢推定手段で推定した位置姿勢を修正する位置姿勢修正手段と、
   前記修正された位置姿勢を計測結果として前記記憶手段に記憶する記憶制御手段と
   を有することを特徴とする位置姿勢計測装置。
9. 請求項１乃至請求項７のいずれか１項に記載の位置姿勢計測方法における各工程をコンピュータ装置に実行させるためのプログラム。
10. 請求項９記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images