(57)【要約】【課題】  少ない指標の観測によって、撮像装置の位置と姿勢の計測を安定して行うこと。【解決手段】  指標検出部１１０は、撮像装置１３０によって取得された画像から、情景中に配置された指標の画像座標を検出する。姿勢センサ１４０は撮像装置１３０に装着されており、姿勢計測値を出力する。姿勢予測部４５０は、姿勢センサ１４０による姿勢計測値に基づいて撮像装置１３０の姿勢を予測する。位置姿勢算出部４２０は、撮像装置１３０の姿勢の予測値と、各々の指標の画像座標と世界座標の組を入力し、これらの情報に基づいて撮像装置１３０の位置と姿勢センサ１４０の方位ドリフト誤差補正値の更新値を求め、撮像装置１３０の位置及び姿勢を算出する。【選択図】  図４
2005326282.tif 000002
【特許請求の範囲】
【請求項１】
対象物体の位置及び姿勢を算出する位置姿勢計測方法であって、
前記対象物体に装着した撮像装置から撮像画像を入力する画像入力工程と、
前記対象物体の姿勢に関する情報を計測する姿勢センサから、姿勢計測値を入力する姿勢入力工程と、
情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、
前記対象物体の位置及び姿勢を得るための未知パラメータを求める演算工程であって、前記姿勢計測値と前記パラメータの推定値に基づいて求められる前記指標の画像座標に関わる特徴量の理論値と、前記特徴検出工程で検出された当該特徴量の実測値との間の誤差を小さくするような前記パラメータを、前記パラメータに対する前記特徴量の画像ヤコビアンを利用して繰り返し演算によって求める演算工程と、前記求められたパラメータを利用して、前記対象物体の位置及び姿勢を求める位置姿勢演算工程とを有することを特徴とする位置姿勢計測方法。
【請求項２】
情景を撮像する撮像装置を計測対象物体として、当該対象物体の位置及び姿勢を算出する位置姿勢計測方法であって、
撮像装置から撮像画像を入力する画像入力工程と、
前記撮像装置の姿勢に関する情報を計測する姿勢センサから、姿勢計測値を入力する姿勢入力工程と、
前記情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、
前記撮像装置の位置及び姿勢を得るための未知パラメータを求める演算工程であって、前記姿勢計測値と前記パラメータの推定値に基づいて求められる前記指標の画像座標に関わる特徴量の理論値と、前記特徴検出工程で検出される当該特徴量の実測値との間の誤差を小さくするような前記パラメータを、前記パラメータに対する前記特徴量の画像ヤコビアンを利用して繰り返し演算によって求める演算工程とを有し、得られた前記パラメータを利用して、前記撮像装置の位置及び姿勢を得ることを特徴とする位置姿勢計測方法。
【請求項３】
前記パラメータは、前記姿勢センサの方位誤差を補正するための補正値に関するパラメータと、前記対象物体の位置を表すパラメータであることを特徴とする、請求項１または請求項２に記載の位置姿勢計測方法。
【請求項４】
前記パラメータは、前記対象物体の位置を表すパラメータであることを特徴とする、請求項１または請求項２に記載の位置姿勢計測方法。
【請求項５】
情景を撮像する撮像装置の位置及び姿勢を算出する位置姿勢計測方法であって、
撮像装置から撮像画像を入力する画像入力工程と、
前記撮像装置の傾斜角に関する情報を計測する傾斜角センサから、傾斜角計測値を入力する姿勢入力工程と、
前記情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、
前記特徴検出工程で検出される前記指標の画像座標に関わる特徴量の実測値と前記傾斜角計測値とを用いて、前記撮像装置の位置及び方位角を求める演算工程と
を有することを特徴とする位置姿勢計測方法。
【請求項６】
情景を撮像する撮像装置の位置及び姿勢を算出する位置姿勢計測方法であって、
撮像装置から撮像画像を入力する画像入力工程と、
前記撮像装置の姿勢に関する情報を計測する姿勢センサから、姿勢計測値を入力する姿勢入力工程と、
前記情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、
前記姿勢計測値の誤差補正値をオペレータが更新する補正値更新工程と、
前記姿勢計測値と前記誤差補正値から誤差補正後の姿勢を得る誤差補正工程と、
前記誤差補正後の姿勢と前記指標の画像座標に関わる特徴量の実測値とを用いて、前記撮像装置の少なくとも位置を求める演算工程と
を有することを特徴とする位置姿勢計測方法。
【請求項７】
請求項１乃至請求項６のいずれかに記載の位置姿勢計測方法をコンピュータに実行させるための制御プログラム。
【請求項８】
請求項７に記載の制御プログラムを格納するコンピュータ可読メモリ。
【請求項９】
対象物体の位置及び姿勢を算出する位置姿勢計測装置であって、
前記対象物体に装着した撮像部と、
前記対象物体の姿勢に関する情報を計測する姿勢センサと、
情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出手段と、
前記対象物体の位置及び姿勢を得るための未知パラメータを求める演算手段であって、前記姿勢計測値と前記パラメータの推定値に基づいて求められる前記指標の画像座標に関わる特徴量の理論値と、前記特徴検出工程で検出された当該特徴量の実測値との間の誤差を小さくするような前記パラメータを、前記パラメータに対する前記特徴量の画像ヤコビアンを利用して繰り返し演算によって求めるパラメータ演算手段と、
前記求められたパラメータを利用して、前記対象物体の位置及び姿勢を求める位置および姿勢を求める位置姿勢演算手段とを有することを特徴とする位置姿勢計測装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の位置及び姿勢を計測するものに関する。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない結合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感の提示を行う画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）の画像を重畳描画してそれを表示するビデオシースルー方式によって実現される。
【０００３】
またその他にも、観察者の頭部に装着された光学シースルー型ディスプレイに、観察者の視点の位置及び姿勢に応じて生成した仮想空間の画像を表示する光学シースルー方式によって実現される。
【０００４】
このような画像表示装置の応用としては、患者の体表面に体内の様子を重畳表示する手術支援や、現実空間に浮遊する仮想の敵と戦う複合現実感ゲームなど、今までのバーチャルリアリティとは異なった新たな分野が期待されている。
【０００５】
これらの応用に対して共通に要求されるのは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来から多くの試みがなされてきた。複合現実感における位置合わせの問題は、ビデオシースルー方式の場合は情景中における（すなわち、世界座標系における）撮像装置の位置及び姿勢を求める問題に帰結される。同様に、光学シースルー方式の場合は情景中における観察者の視点あるいはディスプレイの位置及び姿勢を求める問題に帰結される。
【０００６】
前者の問題を解決する方法として、情景中に複数の指標を配置あるいは設定し、撮像装置によって撮像された画像内における指標の投影像の座標を検出することで、情景中における撮像装置の位置及び姿勢を求めることが一般的に行われている。また、撮像装置に慣性センサを装着して、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用したり、同推定結果を画像に基づく位置及び姿勢算出の初期値として用いたり、さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いることで、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている（例えば、非特許文献１）。
【０００７】
また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置（及び慣性センサ）を装着し、前者と同様な方法によって撮像装置の位置及び姿勢を求め、それに基づいて計測対象物の位置及び姿勢を求めることが一般的に行われている。
【０００８】
図１を用いて、撮像装置の位置及び姿勢を計測する位置姿勢計測装置の従来例について説明する。図１は従来の位置姿勢計測装置の構成を示す図である。同図に示したように、本従来例における位置姿勢計測装置１００は、画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢センサ１４０、姿勢予測部１５０、及び位置姿勢算出部１２０によって構成されており、撮像装置１３０に接続されている。
【０００９】
また、現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標（以下、指標）として、世界座標系（現実空間の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における位置が既知である複数個の指標Ｑｋ（ｋ＝１，，，Ｋ１）が配置されている。指標Ｑｋは、位置及び姿勢を計測しようとする計測対象範囲内の各々の地点に撮像装置１３０を位置させた時に撮像装置１３０によって取得される画像上において、少なくとも３個以上の指標が常に観測されるように設置されていることが望ましい。図１の例は、４個の指標Ｑ１，Ｑ２，Ｑ３，Ｑ４が配置されており、そのうちの３個Ｑ１，Ｑ３，Ｑ４が撮像装置１３０の視野内に含まれている状況を示している。
【００１０】
指標Ｑｋは、例えば、それぞれが異なる色を有する円形状のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、何れの形態であってもよい。
【００１１】
撮像装置１３０が出力する画像（以下、これを撮影画像と呼ぶ）は、位置姿勢計測装置１００に入力される。
【００１２】
画像入力部１６０は、位置姿勢装置１００へ入力される撮影画像をデジタルデータに変換し、データ記憶部１７０に保存する。また、画像が入力されたタイミングを姿勢予測部１５０へと出力する。
【００１３】
姿勢センサ１４０は撮像装置１３０に装着されており、姿勢センサ１４０自身の現在の姿勢を計測して姿勢予測部１５０へと出力する。姿勢センサ１４０は、ジャイロセンサをベースとしたセンサユニットであり、例えば、株式会社トキメックのＴＩＳＳ−５−４０や、米国ＩｎｔｅｒＳｅｎｓｅ社のＩｎｅｒｔｉａＣｕｂｅ２等によって構成される。なお、これらの姿勢センサは時間の経過に伴って蓄積されるドリフト誤差を有しているため、出力される姿勢計測値は、真の姿勢とは異なった、誤差を有する姿勢となる。
【００１４】
姿勢予測部１５０は、１フレーム前の時刻（以下、時刻τ−１と称す。撮像装置１３０がＮＴＳＣ信号を出力する場合であって、撮像装置の位置及び姿勢をフレーム毎に求めるのであれば３３．３ｍｓｅｃ前、フィールド毎に求めるのであれば１６．７ｍｓｅｃ前を表している）における撮像装置１３０の姿勢の算出値（位置姿勢算出部１２０の出力値）をデータ記憶部１７０から入力し、さらに、姿勢センサ１４０から現フレームの時刻（以下、時刻τと称す）における姿勢計測値を入力し、これらのデータを用いて時刻τにおける撮像装置１３０の姿勢を予測し、データ記憶部１７０へと出力する。
【００１５】
指標検出部１１０は、データ記憶部１７０より撮影画像を入力し、入力した画像中に撮影されている指標Ｑｋの画像座標を検出する。例えば、指標Ｑｋの各々が異なる色を有するマーカによって構成されている場合には、撮影画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標Ｑｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを撮影画像上に施すことにより、指標の位置を検出する。なお、時刻τにおける撮像装置１３０の位置の算出値（位置姿勢算出部１２０の出力）及び姿勢の予測値（姿勢予測部１５０の出力）をデータ記憶部１７０からさらに入力し、これらの値に基づいて画像上における指標の位置を予測し探索範囲を限定することにより、指標検出処理の計算負荷を軽減したり、指標の誤検出や誤同定を低減させたりすることが可能である。
【００１６】
指標検出部１１０は、さらに、検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎとその識別子ｋｎをデータ記憶部１７０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。例えば図１の場合には、Ｎ＝３であり、識別子ｋ１＝１，ｋ２＝３，ｋ３＝４とこれらに対応する画像座標ｕＱｋ１，ｕＱｋ２，ｕＱｋ３が出力される。
【００１７】
位置姿勢算出部１２０は、時刻τにおける姿勢の予測値と、指標検出部１１０によって検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎと世界座標ｘＷＱｋｎの組をデータ記憶部１７０から入力し、入力した姿勢の予測値と時刻τ−１における位置の算出値を初期値として、指標の対応関係に基づいて撮像装置１３０の位置及び姿勢を算出する。算出された位置及び姿勢は、位置及び姿勢の算出値としてデータ記憶部１７０に出力されるとともに、不図示のＩ／Ｆを介して外部へ出力される。
【００１８】
データ記憶部１７０は、画像入力部１６０から入力される画像、姿勢予測部１５０から入力される姿勢の予測値、位置姿勢算出部１２０から入力される位置及び姿勢の算出値、指標検出部１１０から入力される指標の画像座標及び識別子、さらに、既知の値である指標の世界座標等のデータを保持し、必要に応じてこれらを入出力する。
【００１９】
図２のフローチャートを用いて、本従来例における姿勢予測部１５０を実現するための具体的な処理の手順を説明する。
【００２０】
なお、姿勢を表現する方法には様々なものが存在するが、ここでは、３×３の回転行列Ｒによって表現されているものとする。
【００２１】
ステップＳ２０００において、姿勢予測部１５０は、画像入力部１６０からのトリガ（新たな画像が入力されたときに発生する）があるか否かを判定する。トリガ入力があった場合は、ステップＳ２０１０へと処理を進める。一方、トリガ入力が無い場合は、ステップＳ２０００の処理を再度実行する。
ステップＳ２０１０において、姿勢予測部１５０は、姿勢センサ１４０から姿勢計測値Ｒ＃（＃はセンサによる計測値を表す記号）を入力し、これを時刻τにおける姿勢計測値Ｒ＃τとする。
【００２２】
ステップＳ２０２０において、姿勢予測部１５０は、時刻τ−１における姿勢の算出値Ｒτ−１をデータ記憶部１７０から入力する。
【００２３】
ステップＳ２０３０において、姿勢予測部１５０は、時刻τ−１と時刻τの間における撮像装置１３０の相対的な姿勢変化△Ｒ＃を、次式によって算出する。
【００２４】
△Ｒ＃＝（Ｒ＃τ−１・ＲＳＣ）−１・Ｒ＃τ・ＲＳＣ
ここで、ＲＳＣは、カメラ座標系（撮像装置１３０の位置及び姿勢を表す座標系）からセンサ座標系（姿勢センサ１４０の位置及び姿勢を表す座標系）へ姿勢を変換する３×３の行列であり、固定値である姿勢センサ１４０と撮像装置１３０の相対姿勢に基づいて、既知の値として予め設定されている。
【００２５】
ステップＳ２０４０において、姿勢予測部１５０は、時刻τ−１における姿勢の算出値Ｒτ−１に姿勢の変化分△Ｒ＃を追加した姿勢を次式によって求め、時刻τにおける姿勢の予測値Ｒ＊τとしてデータ記憶部１７０へと出力する。
【００２６】
Ｒ＊τ＝Ｒτ−１・△Ｒ＃
ステップＳ２０５０において、姿勢予測部１５０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、時刻τの姿勢計測値Ｒ＃τを一時刻前の姿勢計測値Ｒ＃τ−１として保存し、処理をステップＳ２０００へと進める。
【００２７】
次に、図３のフローチャートを用いて、本従来例における位置姿勢算出部１２０を実現するための具体的な処理の手順を説明する。本従来例において、撮像装置１３０の位置及び姿勢は、非線形方程式の反復解法によって算出される。
【００２８】
位置姿勢算出部１２０では、算出すべき撮像装置１３０の位置及び姿勢を、それぞれ３値ベクトルｔ＝［ｘｙｚ］Ｔ及びω＝［ξψζ］Ｔによって内部的に表現している。したがって、求めるべき未知パラメータは、６値の状態ベクトルｓ＝［ｘｙｚξψζ］Ｔと記述される。
【００２９】
姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。なお、姿勢ωは、次式
【００３０】
【数１】
2005326282.tif 000003
【００３１】
（ただし、
【００３２】
【数２】
2005326282.tif 000004
【００３３】
である。）によって、３×３の回転行列Ｒによっても表現可能であり、ωとＲとは、互いに一意に変換することができる。Ｒからωへの変換方法は公知であるので、その詳細な説明は省略する。
【００３４】
ステップＳ３０００において、位置姿勢算出部１２０は、時刻τにおける撮像装置１３０の姿勢の予測値Ｒ＊τを入力し、３値ベクトルω＊τ（＝［ξ＊τψ＊τζ＊τ］Ｔ）を得る。
【００３５】
ステップＳ３００５において、位置姿勢算出部１２０は、姿勢の予測値ω＊τと時刻τ−１における位置の算出値ｔτ−１と合わせて、状態ベクトルの初期値ｓ＝［ｘτ−１ｙτ−１ｚτ−１ξ＊τψ＊τζ＊τ］Ｔとする。
【００３６】
ステップＳ３０１０において、位置姿勢算出部１２０は、時刻τにおいて入力される撮影画像に対して指標検出部１１０によって検出された指標の画像座標とその世界座標を、データ記憶部１７０から入力する。
【００３７】
ステップＳ３０２０において、位置姿勢算出部１２０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力した指標の数が３点以上の場合はステップＳ３０３０に処理を進め、３点未満の場合はステップＳ３０９０に処理を進める。
【００３８】
ステップＳ３０３０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を算出する。ｕＱｋｎ＊の算出は、既知な情報として予め保持している指標Ｑｋｎ各々の世界座標ｘＷＱｋｎと現在の状態ベクトルｓの関数：
ｕＱｋｎ＊＝Ｆｃ（ｘＷＱｋｎ，Ｓ）
に基づいて行われる。具体的には、関数Ｆｃ（）は、ｘＷＱｋｎとｓから当該指標のカメラ座標（カメラ上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）ｘＣＱｋｎを求める次式、
【００３９】
【数３】
2005326282.tif 000005
【００４０】
及び、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める次式、
【００４１】
【数４】
2005326282.tif 000006
【００４２】
によって構成されている。ここでｆＣｘ及びｆＣｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。
【００４３】
ステップＳ３０４０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差△ｕＱｋｎを、次式によって算出する。
△ｕＱｋｎ＝ｕＱｋｎ−ｕＱｋｎ＊
ステップＳ３０５０において、位置姿勢算出部１２０は、各々の指標Ｑｋｎに対して、状態ベクトルｓに関する画像ヤコビアン（すなわち、式４の関数Ｆｃ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列）ＪｕｓＱｋｎ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）と、式５の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｘｓＱｋｎ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＱｋｎを算出する。
【００４４】
ＪｕｓＱｋｎ＝ＪｕｘＱｋｎ・ＪｘｓＱｋｎ
ステップＳ３０６０において、位置姿勢算出部１２０は、ステップＳ３０４０及びステップＳ３０５０において算出した誤差△ｕＱｋｎ及び画像ヤコビアンＪｕｓＱｋｎに基づいて、状態ベクトルｓの補正値△ｓを算出する。具体的には、誤差△ｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトル
【００４５】
【数５】
2005326282.tif 000007
【００４６】
及び、画像ヤコビアンＪｕｓＱｋｎを垂直に並べた２Ｎ行×６列の行列
【００４７】
【数６】
2005326282.tif 000008
【００４８】
を作成し、θの擬似逆行列θ’を用いて、
△ｓ＝θ’Ｕ
として算出する。図１の例ではＮ＝３であるので、は６次元ベクトル、θは６行×６列の行列となる。
【００４９】
ステップＳ３０７０において、位置姿勢算出部１２０は、ステップＳ３０６０において算出した補正値△ｓを用いて式１２に従って状態ベクトルｓを補正し、得られた値をｓの新たな推定値とする。
【００５０】
ｓ＋△ｓ→ｓ
ステップＳ３０８０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値△ｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ３０３０以降の処理を行う。
【００５１】
ステップＳ３０８０において計算が収束したと判定されると、ステップＳ３０９０において、位置姿勢算出部１２０は、得られた状態ベクトルｓを、時刻τにおける撮像装置１３０の位置及び姿勢の情報（すなわち、ｓτ）として出力する。位置及び姿勢の情報はｓそのものであっても良いが、例えば、姿勢を表す３×３行列Ｒと位置を表す３次元ベクトルｔの組みをｓから算出して、これらを出力してもよい。
【００５２】
ステップＳ３１００において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ３０００へと進み、次フレーム（時刻τ＋１）以降の入力データに対して同様の処理を実行する。
【００５３】
以上の方法によって撮像装置の位置及び姿勢を取得することが、従来からなされてきた。また、任意の計測対象物体（例えば、光学シースルーのＨＭＤ）の位置及び姿勢を計測するという目的において、撮像装置を当該計測対象物体に装着して、上記と同様の技術によって撮像装置の位置及び姿勢を計測し、撮像装置と計測対象物体との既知の相対的な位置と姿勢の関係から計測対象物体の位置及び姿勢を得ることが従来からなされてきた。
【非特許文献１】藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９，ｎｏ．５７４，ｐｐ．１−８）
【発明の開示】
【発明が解決しようとする課題】
【００５４】
しかし、上記の従来手法では、姿勢センサから得られる情報は、指標の座標予測や初期値の算出等、画像から検出した指標に基づく位置合わせの補助的な情報としてしか用いられておらず、最終的な位置及び姿勢の推定値を決定付けるのは、画像情報のみであった。それゆえに、安定した位置合わせを実現するのに十分な画像情報が入力画像中に含まれていない状況下、例えば、画像上の一部の領域に指標が偏在して観測されている場合や、指標が３点のみしか観測されていない場合、指標の検出に誤差が含まれている場合等に、得られる解の精度や安定性が不十分となることがあった。また、観測されている指標が２点以下の場合には、解を求めることができなかった。これらの問題を回避するためには情景中に多数の指標を満遍なく設定する必要があり、指標同士の識別が困難になるという問題や、現実空間の美観を損ねてしまうという問題が生じていた。
【００５５】
本発明は上述の問題に鑑みてなされたものであり、画像上で観測される指標の数が少ない場合であっても、撮像装置の位置及び姿勢を計測することが可能にすることを目的とする。また、従来に比べて安定した解を得ることができるようにすることを目的とする。
【課題を解決するための手段】
【００５６】
上述の目的を達成するために本発明は以下の構成を有することを特徴とする。
【００５７】
請求項１記載の発明は、対象物体の位置及び姿勢を算出する位置姿勢計測方法であって、前記対象物体に装着した撮像装置から撮像画像を入力する画像入力工程と、前記対象物体の姿勢に関する情報を計測する姿勢センサから、姿勢計測値を入力する姿勢入力工程と、情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、前記対象物体の位置及び姿勢を得るための未知パラメータを求める演算工程であって、前記姿勢計測値と前記パラメータの推定値に基づいて求められる前記指標の画像座標に関わる特徴量の理論値と、前記特徴検出工程で検出された当該特徴量の実測値との間の誤差を小さくするような前記パラメータを、前記パラメータに対する前記特徴量の画像ヤコビアンを利用して繰り返し演算によって求める演算工程と、前記求められたパラメータを利用して、前記対象物体の位置及び姿勢を求める位置姿勢演算工程とを有することを特徴とする。
請求項２記載の発明は、情景を撮像する撮像装置を計測対象物体として、当該対象物体の位置及び姿勢を算出する位置姿勢計測方法であって、撮像装置から撮像画像を入力する画像入力工程と、前記撮像装置の姿勢に関する情報を計測する姿勢センサから、姿勢計測値を入力する姿勢入力工程と、前記情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、前記撮像装置の位置及び姿勢を得るための未知パラメータを求める演算工程であって、前記姿勢計測値と前記パラメータの推定値に基づいて求められる前記指標の画像座標に関わる特徴量の理論値と、前記特徴検出工程で検出される当該特徴量の実測値との間の誤差を小さくするような前記パラメータを、前記パラメータに対する前記特徴量の画像ヤコビアンを利用して繰り返し演算によって求める演算工程とを有し、得られた前記パラメータを利用して、前記撮像装置の位置及び姿勢を得ることを特徴とする。
【００５８】
請求項５記載の発明は、情景を撮像する撮像装置の位置及び姿勢を算出する位置姿勢計測方法であって、撮像装置から撮像画像を入力する画像入力工程と、前記撮像装置の傾斜角に関する情報を計測する傾斜角センサから、傾斜角計測値を入力する姿勢入力工程と、前記情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、前記特徴検出工程で検出される前記指標の画像座標に関わる特徴量の実測値と前記傾斜角計測値とを用いて、前記撮像装置の位置及び方位角を求める演算工程とを有することを特徴とする。
【００５９】
請求項６記載の発明は、情景を撮像する撮像装置の位置及び姿勢を算出する位置姿勢計測方法であって、撮像装置から撮像画像を入力する画像入力工程と、前記撮像装置の姿勢に関する情報を計測する姿勢センサから、姿勢計測値を入力する姿勢入力工程と、前記情景中に配置されている指標の画像座標に関わる特徴量を前記撮像画像から検出し実測値を得る特徴検出工程と、前記姿勢計測値の誤差補正値をオペレータが更新する補正値更新工程と、前記姿勢計測値と前記誤差補正値から誤差補正後の姿勢を得る誤差補正工程と、前記誤差補正後の姿勢と前記指標の画像座標に関わる特徴量の実測値とを用いて、前記撮像装置の少なくとも位置を求める演算工程とを有することを特徴とする。
【発明の効果】
【００６０】
本発明によれば、画像上で観測される指標の数が２点の場合であっても、撮像装置の位置及び姿勢を計測することが可能となる。また、３点以上の指標が得られた場合においても、求めるべきパラメータ数が少ないため、情報の冗長性が相対的に増加し、従来手法と比して安定した解を得ることができる。この効果は、特に、指標の数が３点の場合に発揮される。したがって、解が得られない状況に陥る可能性が低下し、さらに、（同じ安定性を有する）解を得るために必要となる指標の数が減少する。その結果、シーン中に設定する指標の数を減らせられる（多数のマーカを設置しなくても良い）という利点が生じ、これが、誤認識率の低下につながり、また、美観を損ねるという問題の軽減にも貢献する。
【発明を実施するための最良の形態】
【００６１】
以下添付図面を参照して、本発明を好適な実施形態に従って詳細に説明する。
【００６２】
［第１の実施形態］
本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。
【００６３】
図４は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置４００は、画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢センサ１４０、姿勢予測部４５０、及び位置姿勢算出部４２０によって構成されており、撮像装置１３０に接続されている。また、現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標が、前記従来例と同様に設置されているものとする。なお、図１の構成図と同じ番号で表した各構成要素（指標検出部１１０、姿勢センサ１４０、画像入力部１６０、データ記憶部１７０）の機能は、先に述べた従来の位置姿勢計測装置におけるそれらの機能と同様であるので、その詳細な説明は省略する。
【００６４】
位置姿勢算出部４２０は、撮像装置１３０の姿勢の予測値と、指標検出部１１０によって検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎと世界座標ｘＷＱｋｎの組をデータ記憶部１７０から入力し、これらの情報に基づいて撮像装置１３０の位置及び姿勢を算出し、Ｉ／Ｆを介して外部へ出力する。また、算出した撮像装置１３０の位置をデータ記憶部１７０に出力し、さらに、位置及び姿勢の算出工程で導出される姿勢センサ１４０の方位ドリフト誤差補正値の更新値によって、データ記憶部１７０が保持する方位ドリフト誤差補正値を更新する。
【００６５】
本実施形態における位置姿勢算出部４２０と、従来例における位置姿勢算出部１２０との相違点は、位置姿勢算出部１２０が姿勢予測部１５０から入力した姿勢の推定値を繰り返し演算の初期値としてしか用いていなかったのに対して、予測値のうちの信頼性の高い要素に関しては、それを未知パラメータとして設定せずに、既知の値として取り扱うことにある。すなわち、位置姿勢算出部４２０における解の算出工程では、姿勢センサ１４０による姿勢の計測精度は傾斜角方向（ピッチ角及びロール角）に関しては十分許容範囲内であり、方位角方向（ヨー角方向）のみにドリフト誤差を有するものであるという仮定の上で処理が実行される。この仮定は、地球の重力方向を観測する傾斜角センサを構成要素として有することで傾斜角方向のドリフト誤差の蓄積をキャンセルする機能を有しているジャイロセンサベースのセンサユニットを姿勢センサ１４０として用いる場合に妥当なものとなる。この仮定を満たす姿勢センサとして、例えば、株式会社トキメックのＴＩＳＳ−５−４０を用いることができる。
姿勢予測部４５０は、方位ドリフト誤差補正値φをデータ記憶部１７０から入力し、姿勢センサ１４０から入力した姿勢計測値を補正して撮像装置１３０の姿勢を予測し、データ記憶部１７０へと出力する。
【００６６】
なお、図４に示した画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部４５０、及び位置姿勢算出部４２０の夫々は、独立した装置として扱っても良いし、夫々ソフトウェアとして１つもしくは複数のコンピュータにインストールし、夫々のコンピュータのＣＰＵにより実行することで、その機能を実現するようにしても良い。本実施形態では、各部（画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部４５０、及び位置姿勢算出部４２０）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。
【００６７】
図５は画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部４５０、及び位置姿勢算出部４２０の夫々をソフトウェアとして実行するコンピュータの基本構成を示す図である。
【００６８】
１００１はＣＰＵで、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１６０、指標検出部１１０、姿勢予測部４５０、及び位置姿勢算出部４２０の夫々のソフトウェアの実行を制御して、各部の機能を実現する。
【００６９】
１００２はＲＡＭで、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備えると共に、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。データ記憶部１７０の機能は、ＲＡＭ１００２によって実現される。
【００７０】
１００３はＲＯＭで、一般にコンピュータの記憶プログラムや設定データなどが格納されている。１００４、１００５は夫々キーボード、マウスで、操作者は夫々を用いて、各種の指示をＣＰＵ１００１に入力することができる。
【００７１】
１００６は表示部で、ＣＲＴや液晶画面などにより構成されており、例えば、撮像装置１３０の位置姿勢計測のために表示すべきメッセージ等を表示することができる。
【００７２】
１００７は外部記憶装置で、ハードディスクなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やソフトウェアのプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。
【００７３】
１００８は記憶媒体ドライブで、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出して、ＲＡＭ１００２や外部記憶装置１００７に出力する。
【００７４】
１００９はＩ／Ｆで、撮像装置１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢ等のシリアルポート、また、算出した撮像装置１３０の位置及び姿勢を外部へ出力するためのイーサネット（登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０の機能の一部は、Ｉ／Ｆ１００９によって実現される。
１０１０は上述の各部を繋ぐバスである。
【００７５】
図６は、姿勢予測部４５０の処理手順を示すフローチャートであり、ＣＰＵ１００１が姿勢予測部４５０のソフトウェアのプログラムを実行することで行われる。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、ＲＡＭ１００２に既にロードされているものとする。
【００７６】
ステップＳ６０００において、姿勢予測部４５０は、姿勢センサ１４０から姿勢計測値Ｒ＃を入力する。
ステップＳ６０１０において、姿勢予測部４５０は、データ記憶部１７０から方位ドリフト誤差補正値φ＊を入力する。
ステップＳ６０２０において、姿勢予測部４５０は、姿勢計測値Ｒ＃（姿勢センサ１４０の姿勢を表している）に、姿勢センサ１４０から撮像装置１３０への姿勢変換と、方位ドリフト誤差補正値φ＊によるドリフト誤差補正を反映させ、方位ドリフト誤差補正後の撮像装置１３０の姿勢を算出し、これを、姿勢の予測値Ｒ＊とする。
【００７７】
Ｒ＊＝△Ｒ（φ＊）・Ｒ＃・ＲＳＣ
ここで、△Ｒ（φ）は、方位方向にφだけの回転を加える回転行列であり、φの関数として次式によって定義される。
【００７８】
【数７】
2005326282.tif 000009
【００７９】
ここで、ｌ＝（ｌ１，ｌ２，ｌ３）は、世界座標系における鉛直上方向（地球の重力の反対方向）を表す既知のベクトルを表している。
【００８０】
ステップＳ６０３０において、姿勢予測部４５０は、姿勢の予測値Ｒ＊をデータ記憶部１７０へと出力する。
【００８１】
ステップＳ６０４０において、姿勢予測部４５０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ６０００へと進める。
【００８２】
図７は、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部４２０のソフトウェアのプログラムを実行することで行われる。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、ＲＡＭ１００２に既にロードされているものとする。
【００８３】
位置姿勢算出部４２０は、前記従来例における位置姿勢算出部１２０とは異なり、撮像装置１３０の位置ｔ＝［ｘｙｚ］Ｔと、姿勢センサ１４０の方位ドリフト誤差補正値の更新値φの合計４パラメータを、算出すべき未知パラメータとして取り扱う。すなわち、本実施形態では、姿勢についてはその全てを未知とするのではなく、姿勢の予測値Ｒ＊には方位方向のドリフト誤差のみが含まれており、その方位ドリフト誤差補正値の更新値φのみを求めることで、正確な姿勢が得られるというモデルを適用する。以下では、求めるべき未知パラメータを４値の状態ベクトルｓ’＝［ｘｙｚφ］Ｔによって記述する。
【００８４】
ステップＳ７０００において、位置姿勢算出部４２０は、撮像装置１３０の姿勢の予測値Ｒ＊（姿勢予測部４５０の出力）をデータ記憶部１７０から入力する。
【００８５】
ステップＳ７００５において、位置姿勢算出部４２０は、状態ベクトルｓ’の初期値を、ｓ’＝［ｘτ−１ｙτ−１ｚτ−１０］Ｔと設定する。ここで、ｘτ−１，ｙτ−１，ｚτ−１は、１ループ前の処理（時刻τ−１とおく）におけるステップＳ７０９０において算出された計測対象物体１３０の位置を表している。
【００８６】
ステップＳ７０１０において、位置姿勢算出部４２０は、指標検出部１１０によって検出された指標の画像座標とその世界座標を、データ記憶部１７０から入力する。例えば図４の場合には、Ｎ＝２であり、画像座標ｕＱｋ１，ｕＱｋ３と対応する世界座標ｘＷＱｋ１，ｘＷＱｋ３が入力される。
【００８７】
ステップＳ７０２０において、位置姿勢算出部４２０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力した指標の数が２点以上の場合はステップＳ７０３０に処理を進め、２点未満の場合はステップＳ７０９０に処理を進める。
【００８８】
ステップＳ７０３０において、位置姿勢算出部４２０は、各々の指標Ｑｋｎに対して、その画像座標の推定値ｕＱｋｎ＊を、状態ベクトルｓ’の関数：
ｕＱｋｎ＊＝Ｆｃ’（ｘＷＱｋｎ，ｓ’）
に基づいて算出する。具体的には、関数Ｆ’ｃ（）は、カメラ座標ｘＣＱｋｎから画像座標ｕＱｋｎ＊を求める式６及び、世界座標ｘＷＱｋｎとｓ’からカメラ座標ｘＣＱｋｎを求める次式、
【００８９】
【数８】
2005326282.tif 000010
【００９０】
によって構成されている。ここでＲ＊は、ステップＳ７０００で入力した姿勢の予測値を表している。また、△Ｒ（φ）は，方位方向にφだけの回転を加える回転行列を表しており、式１４によって定義される。
【００９１】
ステップＳ７０４０において、位置姿勢算出部４２０は、各々の指標Ｑｋｎに対して、画像座標の推定値ｕＱｋｎ＊と実測値ｕＱｋｎとの誤差△ｕＱｋｎを、式７に基づいて算出する。
【００９２】
ステップＳ７０５０において、位置姿勢算出部４２０は、各々の指標Ｑｋｎに対して、状態ベクトルｓ’に関する画像ヤコビアン（すなわち、式１５の関数Ｆｃ’（）を状態ベクトルｓ’の各要素で偏微分した解を各要素に持つ２行×４列のヤコビ行列）Ｊｕｓ’Ｑｋｎ（＝∂ｕ／∂ｓ’）を算出する。具体的には、式６の右辺をカメラ座標ｘＣＱｋｎの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＱｋｎ（＝∂ｕ／∂ｘ）と、式１６の右辺を状態ベクトルｓ’の各要素で偏微分した解を各要素に持つ３行×４列のヤコビ行列Ｊｘｓ’Ｑｋｎ（＝∂ｘ／∂ｓ’）を算出し、式８（ただし、ｓをｓ’と置き換える）によってＪｕｓ’Ｑｋｎを算出する。
【００９３】
ステップＳ７０６０において、位置姿勢算出部４２０は、ステップＳ７０４０及びステップＳ７０５０において算出した誤差△ｕＱｋｎ及び画像ヤコビアンＪｕｓ’Ｑｋｎに基づいて、状態ベクトルｓ’の補正値△ｓ’を算出する。具体的には、誤差△ｕＱｋｎを垂直に並べた２Ｎ次元の誤差ベクトルＵ及び、画像ヤコビアンＪｕｓ’Ｑｋｎを垂直に並べた２Ｎ行×４列の行列θを作成し、θの擬似逆行列θ’を用いて、式１１（ただし、ｓをｓ’と置き換える）によって算出する。図４の例ではＮ＝２であるので、Ｕは４次元ベクトル、θは４行×４列の行列となる。
【００９４】
ステップＳ７０７０において、位置姿勢算出部４２０は、ステップＳ７０６０において算出した補正値△ｓ’を用いて、式１２（ただし、ｓをｓ’と置き換える）に従って状態ベクトルｓ’を補正し、得られた値をｓ’の新たな推定値とする。
【００９５】
ステップＳ７０８０において、位置姿勢算出部４２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値△ｓ’が予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後の状態ベクトルｓ’を用いて、再度ステップＳ７０３０以降の処理を行う。
【００９６】
ステップＳ７０８０において計算が収束したと判定されると、ステップＳ７０８５において、位置姿勢算出部４２０は、得られた状態ベクトルｓ’から、撮像装置１３０の姿勢を算出する。具体的には、前ステップまでで得られた状態ベクトルｓ’の算出値からドリフト誤差補正値の更新値φを得て、次式：
Ｒ＝△Ｒ（φ）・Ｒ＊
によって、撮像装置１３０の姿勢Ｒを算出する。
【００９７】
ステップＳ７０９０において、位置姿勢算出部４２０は、得られた撮像装置１３０の位置及び姿勢の情報をＩ／Ｆ１００９を介して外部へ出力する。また、撮像装置１３０の位置ｔをデータ記憶部１７０へと出力する。なお、位置及び姿勢の出力の形態は例えば姿勢を表す３×３行列Ｒと位置を表す３次元ベクトルｔの組であってもよいし、姿勢成分をオイラー角に変換したものでもよいし、位置及び姿勢から算出した撮像装置１３０のビューイング変換行列でもよいし、他のいずれの位置姿勢記述方法によるものでもよい。
【００９８】
ステップＳ７０９５において、位置姿勢算出部４２０は、以上の算出工程で得られた方位ドリフト誤差補正値の更新値φを用いて、データ記憶部１７０が保持している方位ドリフト誤差補正値φ＊を次式によって更新する。
φ＊＋φ→φ＊
ステップＳ７１００において、位置姿勢算出部４２０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ７０００へと進み、次フレーム（時刻τ＋１）以降の入力データに対して同様の処理を実行する。
【００９９】
以上によって、撮像装置の位置及び姿勢が計測される。
従来の位置姿勢計測装置では、撮像装置の位置及び姿勢を計測するためには３点以上の指標が必要であり、また、指標が３点のみの場合には入力データに冗長性がないために解が不安定になる（指標の座標検出誤差の影響を大きく受ける）という問題があった。一方、本実施形態に係る位置姿勢計測装置によれば、画像上で観測される指標の数が２点の場合であっても、撮像装置の位置及び姿勢を計測することが可能となる。また、３点以上の指標が得られた場合においても、求めるべきパラメータ数が少ないため、情報の冗長性が相対的に増加し、従来手法と比して安定した解を得ることができる。この効果は、特に、指標の数が３点の場合に発揮される。したがって、（同じ安定性を有する）解を得るために必要となる指標の数が減少し、解が得られない状況を減らすことに貢献する。その結果、シーン中に貼るマーカ数を減らせられるので、誤認識率の低下による精度の向上を実現できる。
【０１００】
［第２の実施形態］
第１の実施形態は、空間中を移動する撮像装置自体の位置及び姿勢を計測することを目的としていた。本実施形態に係る位置姿勢計測装置は、任意の対象物体の位置及び姿勢を計測することを目的とした位置姿勢計測装置であって、第１の実施形態の位置姿勢計測装置に撮像画像を取得するカメラを追加した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。
【０１０１】
図８は、本実施形態に係る位置姿勢計測装置の構成を示す図である。図８に示したように、本実施形態に係る位置姿勢計測装置８００は、画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢センサ１４０、姿勢予測部４５０、位置姿勢算出部８２０、及び撮像装置８３０によって構成されている。
図１，４と同じ部分については同じ番号を付けており、その説明を省略する。ただし、撮像装置８３０によって取得された画像が撮影画像として画像入力部１６０に入力されるという点、姿勢予測部４５０による姿勢の予測対象が撮像装置８３０であるという点、姿勢センサ１４０が撮像装置８３０に装着されている点が、第１の実施形態とは異なっている。
【０１０２】
撮像装置８３０は、計測対象物体８８０に固定して装着されている。ここで、カメラ座標系における計測対象物体８８０の位置及び姿勢は既知であるとする。
【０１０３】
位置姿勢算出部８２０は、撮像装置８３０の姿勢の予測値Ｒ＊と、指標検出部１１０によって検出された各々の指標Ｑｋｎの画像座標ｕＱｋｎと世界座標ｘＷＱｋｎの組をデータ記憶部１７０から入力し、これらの情報に基づいて、第１の実施形態における位置姿勢算出部３２０と同様な処理によって、撮像装置８３０の位置及び姿勢を算出する。また、算出した位置ｔをデータ記憶部１７０へと出力し、さらに、位置及び姿勢の算出工程で導出される姿勢センサ１４０の方位ドリフト誤差補正値の更新値φによって、データ記憶部１７０が保持する方位ドリフト誤差補正値を更新する。
【０１０４】
位置姿勢算出部８２０はさらに、上記によって算出した（世界座標系における）撮像装置８３０の位置及び姿勢と、既知の値であるカメラ座標系における計測対象物体８８０の位置及び姿勢とに基づいて、計測対象物体８８０の位置及び姿勢を算出する。算出された位置及び姿勢は、Ｉ／Ｆ１００９を介して外部へ出力される。
【０１０５】
以上によって、任意の計測対象物体の位置及び姿勢の計測が実現される。
【０１０６】
なお、本実施形態における位置姿勢算出部８２０は撮像装置８３０の位置及び姿勢を一旦求めた後に計測対象物体８８０の位置及び姿勢を求めていたが、計測対象物体８８０の位置及び姿勢を直接求めてもよい。この場合、姿勢予測部４５０は計測対象物体８８０の姿勢を予測するようにし（式１３のＲＳＣを、計測物体座標系からセンサ座標系へ姿勢を変換する３×３の行列に変更する）、計測対象物体８８０の位置を状態ベクトルｓ’の構成要素として設定し、さらに、ｓ’から指標のカメラ座標ｘＣＱｋｎを求める式１６を、
【０１０７】
【数９】
2005326282.tif 000011
【０１０８】
に置き換えればよい。ここで、ＲＣＯは計測物体座標系（計測対象物体８８０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）からカメラ座標系へ姿勢を変換する行列、ｔＣＯは同座標系間の位置を変換するベクトルであり、既知の値として予め保持されているカメラ座標系における計測対象物体８８０の位置及び姿勢に基づいて、予め算出されているものとする。
【０１０９】
［第３の実施形態］
上記の実施形態の各々では、姿勢センサの方位ドリフト誤差補正値の更新値φを未知数として求めていたが、姿勢センサの精度が良い場合や、使用時間が短時間である場合や、あるいは、方位ドリフト誤差補正値の更新値の入力を手作業で行うことが可能な場合には、位置姿勢算出部で求めるべきパラメータを撮像装置の位置のみに限定してもよい。本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢を計測することを目的とした位置姿勢計測装置であって、第１の実施形態における位置姿勢計測装置の位置姿勢算出部４２０の機能を変更した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。
【０１１０】
本実施形態では、第１の実施形態におけるφをすべて０とおく。すなわち、本実施形態における位置姿勢算出部４２０では、求めるべき未知パラメータを３値の状態ベクトルｓ’’＝［ｘｙｚ］Ｔによって記述する。また、本実施形態における位置姿勢算出部４２０では、第１の実施形態における位置姿勢算出部４２０の各処理工程（各ヤコビ行列や式１６等）からφに関する項を取り除いたものを用いればよい。例えば式１６は、世界座標ｘＷＱｋｎとｓ’’からカメラ座標ｘＣＱｋｎを求める次式に変更すればよい。
【０１１１】
【数１０】
2005326282.tif 000012
【０１１２】
本実施形態に係る位置姿勢計測装置によれば、求めるべき未知パラメータ数が少なくなるため、得られる解（撮像装置１３０の位置及び姿勢）の安定性のさらなる向上が期待できる。
【０１１３】
なお、手作業で方位ドリフト誤差補正値の更新値を入力するためには、例えば、図４の構成に補正値更新手段を更に追加すればよい。補正値更新手段は、オペレータの入力に従って方位ドリフト誤差補正値の更新値φを得て、式１８にしたがってデータ記憶部１７０に保存された方位ドリフト誤差補正値φ＊を更新する。補正値更新手段は、例えば、キーボード１００４の特定のキーをインターフェイスとして用いることで実現される。例えば，［＋］キーにより＋０．１度を，［−］キーにより−０．１度を更新値として設定するように構成すればよい。なお、第１の実施形態のような、画像情報に基づいて方位ドリフト誤差補正値の更新値を導出する形態においても、手入力による補正値更新手段を併用することが可能なことはいうまでもない。
【０１１４】
［第４の実施形態］
上記の実施形態の各々では、未知の値として求めるパラメータを、位置及び方位ドリフト誤差補正値の更新値φか、位置のみかのいずれかに固定していた。しかし、何れのパラメータを未知とするかは必ずしも固定である必要はなく、夫々のパラメータの特性に応じて未知とするパラメータを適宜変更することにより、より好適な位置姿勢の推定を行うことができる。本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢を計測することを目的とした位置姿勢計測装置であり、第１の実施形態における位置姿勢計測装置の位置姿勢算出部４２０の機能を変更した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。
【０１１５】
本実施形態における位置姿勢算出部は、第１の実施形態における位置姿勢算出部と第３の実施形態における位置姿勢算出部の夫々の機能を併せ持っており、通常は位置のみを未知のパラメータとする第３の実施形態における位置姿勢算出部の処理を行い、一定時間間隔（例えば１０秒（３００フレーム）に１回）毎に、位置と方位ドリフト誤差補正値の更新値を未知のパラメータとする第１の実施形態における位置姿勢算出部の処理を実行する。なお、方位ドリフト誤差補正値を更新する時間間隔は、姿勢センサ１４０のドリフト特性に応じて設定するのが好ましく、オペレータによる対話的操作によって設定可能であることが好ましい。
【０１１６】
本実施形態に係る位置姿勢計測装置によれば、姿勢センサ１４０として、短時間であれば方位ドリフト誤差を無視できる程度の精度が得られる姿勢センサを用いる場合に、方位ドリフト誤差を補正しつつ、得られる解の安定性を向上させることが期待できる。
【０１１７】
［第５の実施形態］
上記の実施形態の各々では、ある１時刻の画像情報から方位ドリフト誤差補正値の更新値を求めていた。しかし、方位ドリフト誤差の値はフレーム間での相関性が高いので、複数フレームの情報を利用することで、より高精度に求めることができる。本実施形態に係る位置姿勢計測装置は、撮像装置の位置及び姿勢を計測することを目的とした位置姿勢計測装置であり、第１の実施形態における位置姿勢計測装置の位置姿勢算出部４２０の機能を変更した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。
【０１１８】
本実施形態における位置姿勢算出部９２０は、第１の実施形態における位置姿勢算出部と第３の実施形態における位置姿勢算出部の夫々の機能を併せ持っており、夫々のパラメータ推定処理を共に実行する。図９は、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部９２０のソフトウェアのプログラムを実行することで行われる。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、ＲＡＭ１００２に既にロードされているものとする。
【０１１９】
ステップＳ９０００において、位置姿勢算出部９２０は、第１の実施形態におけるステップＳ７０００と同様に、撮像装置１３０の姿勢の予測値Ｒ＊（姿勢予測部４５０の出力）をデータ記憶部１７０から入力する。
【０１２０】
ステップＳ９０１０において、位置姿勢算出部９２０は、第１の実施形態におけるステップＳ７０１０と同様に、指標検出部１１０によって検出された指標の画像座標とその世界座標を、データ記憶部１７０から入力する。
【０１２１】
ステップＳ９０２０において、位置姿勢算出部９２０は、撮像装置１３０の位置ｔ＝［ｘｙｚ］Ｔと姿勢センサ１４０の方位ドリフト誤差補正値の更新値φを未知のパラメータとして、第１の実施形態におけるステップＳ７００５及びステップＳ７０２０からステップＳ７０８０と同様な処理によってこれらを推定する。
【０１２２】
ステップＳ９０３０において、位置姿勢算出部９２０は、ステップＳ９０２０で算出された方位ドリフト誤差補正値の更新値φを積算し、積算値φＳＵＭを求める。
【０１２３】
ステップＳ９０４０において、位置姿勢算出部９２０は、予め定めたフレーム数（例えば３０フレーム）の積算処理を行ったか否かの判定を行い、行っている場合は処理をステップＳ９０５０に進め、行っていない場合は処理をステップＳ９０８０へと進める。
【０１２４】
ステップＳ９０５０において、位置姿勢算出部９２０は、ステップＳ９０３０で求めた積算値φＳＵＭを処理フレーム数で除算することで、各フレームで得られた方位ドリフト誤差補正値の更新値の平均値を算出し、これを新たに、方位ドリフト誤差補正値の更新値φとする。このあと、積算値φＳＵＭは０にクリアされる。
【０１２５】
ステップＳ９０６０において、位置姿勢算出部９２０は、第１の実施形態におけるステップＳ７０９５と同様に、ステップＳ９０５０で得られた方位ドリフト誤差補正値の更新値φを用いて、データ記憶部１７０が保持している方位ドリフト誤差補正値φ＊を式１８によって更新する。
【０１２６】
ステップＳ９０７０において、位置姿勢算出部９２０は、ステップＳ９０５０で得られた方位ドリフト誤差補正値の更新値φを用いて、式１７によって撮像装置１３０の姿勢を算出し、得られた姿勢を新たな姿勢の予測値とする。
【０１２７】
ステップＳ９０８０において、位置姿勢算出部９２０は、撮像装置１３０の位置ｔ＝［ｘｙｚ］Ｔを未知のパラメータとして、第３の実施形態と同様な処理によってこれを推定する。
【０１２８】
ステップＳ９０９０において、位置姿勢算出部９２０は、第１の実施形態におけるステップＳ７０９０と同様に、得られた撮像装置１３０の位置及び姿勢の情報を出力する。
【０１２９】
ステップＳ９１００において、位置姿勢算出部９２０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ９０００へと進み、次フレーム（時刻τ＋１）以降の入力データに対して同様の処理を実行する。
【０１３０】
以上によって、複数フレームの情報を用いることによる、方位ドリフト誤差の補正値の更新値の精度向上が実現される。なお、本実施形態では各フレームで得られる更新値の平均値を用いていたが、各フレームで得られる更新値の中央値を用いても良いし、他のいずれのローパスフィルタを用いても良い。また、第４の実施形態における位置とドリフト誤差補正値の更新値とを未知パラメータとする位置姿勢算出処理手段として、本実施形態における位置姿勢算出部を用いることも、もちろん可能である。
【０１３１】
（変形例１）
上記の実施形態の各々において、誤差ベクトルＵと行列θに基づく状態ベクトルの補正値△ｓの算出に式１１で表現される最急降下法を用いているが、補正値△ｓの算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。
【０１３２】
（変形例２）
上記の実施形態の各々において、指標Ｑとして一つの指標が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を用いていた。しかし、本発明の本質はセンサ計測値によって未知パラメータの自由度を限定し、残された未知パラメータを非線形方程式の反復解法によって算出することにあり、その適用範囲は、指標の種類に限定されるものではない。
【０１３３】
例えば、公知の位置姿勢計測装置（例えば、高橋，石井，牧野，中静，ＶＲインターフェースのための単眼による長方形マーカ位置・姿勢の高精度実時間推定法，３次元画像コンファレンス９６公演論文集，ｐｐ．１６７−１７２，１９９６．を参照）に用いられているような、特定の幾何形状をもつマーカを指標として用いることも出来る。四角形のマーカを指標として用いる場合、四角形の各頂点の世界座標を既知の値として保持しておき（あるいは、マーカの位置、向き、サイズからこれらの値を演算し）、画像から各頂点の画像座標を検出することで、上記実施形態における指標と同様の効果を得ることができる。
【０１３４】
また、別の公知の位置姿勢計測装置（例えば、Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ  ｐａｒａｍｅｔｅｒｉｚｅｄ  ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ  ｍｏｄｅｌｓ  ｔｏ  ｉｍａｇｅｓ，ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１．を参照）に用いられているような、線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。例えば、原点からの直線の距離を評価のための基準として、画像からの検出値ｄと状態ベクトルｓ’からの推定値ｄ＊から算出する誤差△ｄによって誤差ベクトルＵを構成し、ｄ＊の算出式を状態ベクトルｓ’の各要素で偏微分した解を各要素に持つ１行×６列のヤコビ行列Ｊｄｓ’（＝∂ｄ／∂ｓ’）によって行列Θを構成することで、上記実施形態と同様な枠組みによって位置及び姿勢の計測を行うことが出来る。また、線指標と点指標、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することが可能である。
【０１３５】
（変形例３）
第２の実施形態における撮像装置８３０は１台であったが、複数台の撮像装置を計測対象物体８８０に設置し、位置及び姿勢の計測に用いることも可能である。この場合、画像入力部１５０、指標検出部１１０はそれぞれのカメラからの入力画像を処理する。また、姿勢予測部４５０及び位置姿勢算出部８２０は、計測対象物体８８０の位置及び姿勢を基準とした演算を行う。位置姿勢算出部８２０では、計測対象物体８８０の位置及び方位ドリフト補正値によって状態ベクトルｓ’を構成し、各画像から得られる指標の情報から、式１９（カメラ毎にＲＣＯとｔＣＯが異なる）に基づいて各指標の誤差と画像ヤコビアンを求め、それらを積み重ねることで誤差ベクトルＵ及び行列θを構成することで、上記実施形態と同様な枠組みによって位置及び姿勢が推定できる。
【０１３６】
また、第１の実施形態における計測対象である撮像装置１３０は１台であったが、ステレオ・ビデオシースルー・ヘッドマウントディスプレイのような２台の撮像装置を計測する場合であっても、一方の撮像装置（例えば左眼用の撮像装置）を基準とすることで、同様な手法によって位置及び姿勢の計測を行うことができる。
【０１３７】
（変形例４）
上記の実施形態の夫々では、方位ドリフト誤差を有する姿勢センサを用いていたが、姿勢センサとして方位方向にのみ顕著な誤差を有する他の姿勢センサを用いることも可能である。例えば、加速度センサによって傾斜方向の角度を計測し、地磁気センサによって方位方向の角度を計測するタイプの姿勢センサにおいても、位置と方位誤差補正値の更新値を未知パラメータとして、上記実施形態と同様な処理によって計測対象の位置と姿勢の計測を行うことができる。ただし、この場合、方位ドリフト誤差とは誤差の性質が異なるため、第４の実施形態や、第５の実施形態のような形態に用いるには適していない。また、傾斜方向のみを計測する姿勢センサを用いる場合でも、方位方向の計測値が常に０となる３軸の姿勢センサと仮定すると、同様な処理によって計測対象の位置と姿勢の計測を行うことができる。
【０１３８】
［他の実施形態］
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても達成される。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１３９】
さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１４０】
本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０１４１】
【図１】撮像装置の位置及び姿勢の計測を行う従来の位置姿勢計測装置の構成を示す図である。
【図２】撮像装置の位置及び姿勢の計測を行う従来の位置姿勢計測装置における姿勢予測部の処理手順を示すフローチャートである。
【図３】撮像装置の位置及び姿勢の計測を行う従来の位置姿勢計測装置における位置姿勢算出部の処理手順を示すフローチャートである。
【図４】第１の実施形態における位置姿勢計測装置の構成を示す図である。
【図５】コンピュータの基本構成を示す図である。
【図６】ＣＰＵ１００１が姿勢予測部４５０のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。
【図７】ＣＰＵ１００１が位置姿勢算出部４２０のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。
【図８】第２の実施形態における位置姿勢計測装置の構成を示す図である。
【図９】ＣＰＵ１００１が位置姿勢算出部９２０のソフトウェアのプログラムを実行することで行われる、撮像装置１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。
【図１】
2005326282.tif 000013
【図２】
2005326282.tif 000014
【図３】
2005326282.tif 000015
【図４】
2005326282.tif 000016
【図５】
2005326282.tif 000017
【図６】
2005326282.tif 000018
【図７】
2005326282.tif 000019
【図８】
2005326282.tif 000020
【図９】
2005326282.tif 000021