位置姿勢計測方法及び装置
【課題】 傾斜角センサが装着された撮像装置または物体の位置と姿勢を、反復演算を行うことなく直接的に求める。
【解決手段】 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備える。
【解決手段】 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、傾斜角センサの計測値に基づき、撮像装置または物体の位置と姿勢を直接的に算出するものに関する。
【背景技術】
【０００２】
近年、複合現実感（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ；ＭＲ）技術の研究が盛んである。ＭＲ技術は、現実空間とコンピュータによって作られる仮想空間を継ぎ目なく融合する技術である。ＭＲ技術の中でも、現実空間に仮想空間を重ね合わせて表示するＡｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＡＲ、拡張現実感、増強現実感とも呼ばれる）技術が特に注目を集めている。
【０００３】
ＡＲの画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、該撮像装置の位置及び姿勢に応じて生成された仮想空間（コンピュータグラフィクスにより描画された仮想物体や文字情報）の画像を重畳描画した合成画像を表示するビデオシースルー方式か、あるいは観察者の頭部に装着された光学シースルー型ディスプレイに、観察者の視点の位置及び姿勢に応じて生成された仮想空間の画像を表示する光学シースルー方式によって実現される。
【０００４】
ＡＲ技術は、患者の体表面に体内の様子を重畳表示する手術支援や、空き地に仮想のビルを重畳表示する建築シミュレーション、組立作業時に作業手順を重畳表示する組み立て支援など様々な分野への応用が期待される。
【０００５】
ＡＲ技術において最も重要な課題の一つは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来より多くの取り組みが行われてきた。ＡＲにおける位置合わせの問題は、ビデオシースルー方式の場合はシーン中における（すなわちシーン中で規定された基準座標系における）撮像装置の位置及び姿勢を求める問題に帰結する。同様に、光学シースルー方式の場合は、シーン中における観察者の視点あるいはディスプレイの位置及び姿勢を求める問題となる。
【０００６】
前者の問題を解決する方法として、シーン中に複数の指標を配置あるいは設定し、撮像装置が撮影した画像上での指標の投影像の位置を検出することで、シーン中における該撮像装置の位置及び姿勢を求めることが一般的に行われている。また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置を装着し、前者と同様な方法によって該撮像装置の位置及び姿勢を求め、それに基づいて計測対象物の位置及び姿勢を求めることが一般的に行われている。
【０００７】
画像上の指標の投影像とその指標の三次元位置との対応をもとに撮像装置の位置及び姿勢を求める方法は、写真測量の分野において古くから提案されている（例えば、非特許文献１を参照）。また、前述の画像上の指標の投影像をもとに求めた撮像装置の位置及び姿勢を初期値とし、画像上での指標の投影像の実際の観察位置と、投影像の計算位置、すなわち指標の三次元位置と撮像装置の位置及び姿勢から計算される投影像の位置との誤差が最小になるように反復演算によって撮像装置の位置及び姿勢を最適化することが行われている（例えば、非特許文献２を参照）。
【０００８】
一方、撮像装置によって撮影された画像だけを用いるのではなく、ジャイロセンサ（厳密には、３軸方向の角速度を計測するための複数のジャイロセンサと、３軸方向の加速度を計測するための複数の加速度センサの組合せによって構成される３軸姿勢センサであるが、本明細では便宜上これをジャイロセンサと呼ぶことにする）を撮像装置に装着し、画像情報とジャイロセンサの情報を併用して撮像装置の位置及び姿勢を求めることも行われている。
【０００９】
特許文献１では、ジャイロセンサの出力する姿勢の計測値をそのまま撮像装置の姿勢として用い、該撮像装置の姿勢と画像上の指標の投影像及びその指標の三次元位置との対応をもとに撮像装置の位置を線形連立方程式を解くことによって求めている。また、非特許文献３では、ジャイロセンサの出力する姿勢の計測値のうち傾斜角成分をそのまま撮像装置の姿勢の傾斜角成分とし、前述した画像上での指標の投影像の観察位置と計算位置との誤差が最小化されるように、撮像装置の位置と姿勢の方位角成分を反復演算によって最適化することが行われている。
【非特許文献１】解析写真測量 改訂版，社団法人日本写真測量学会，１９８９．
【非特許文献２】加藤，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，浅野，橘：“マーカー追跡に基づく拡張現実感システムとそのキャリブレーション”，日本バーチャルリアリティ学会論文誌，ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１６，１９９９．
【非特許文献３】Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，ａｎｄ Ｈ．Ｙａｍａｍｏｔｏ：“Ａ ｈｅａｄ ｔｒａｃｋｉｎｇ ｍｅｔｈｏｄ ｕｓｉｎｇ ｂｉｒｄ’ｓ−ｅｙｅ ｖｉｅｗ ｃａｍｅｒａ ａｎｄ ｇｙｒｏｓｃｏｐｅ，”Ｐｒｏｃ．３ｒｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔ’ｌ Ｓｙｍｐ．ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ２００４），ｐｐ．８９−９８，２００４．
【特許文献１】特開２００２−２５９９９２号公報
【発明の開示】
【発明が解決しようとする課題】
【００１０】
前述した、撮像装置によって撮影された画像上での指標の投影像のみを利用した手法では、得られる撮像装置の位置及び姿勢が、指標の画像上での検出誤差や、画像の解像度が有限であることが原因となって、間違った位置及び姿勢が得られたり、ジッタを生じることがあった。
【００１１】
また、ジャイロセンサにはドリフト誤差があるため、時間経過に伴って徐々に方位方向の計測値に誤差が生じてしまう。そのため、ジャイロセンサが出力する姿勢計測値をそのまま撮像装置の姿勢とした場合には、間違った撮像装置の位置が得られることがあった。
【００１２】
さらに、反復演算によって撮像装置の位置及び姿勢、または撮像装置の位置及び姿勢の方位角成分を求める場合には初期値が必要である。前フレームの結果を初期値として用いる場合に、前フレームで撮像装置の位置及び姿勢が求められなければ（例えば、前フレームで指標が撮影されなかった場合）初期値を得ることができなかった。また、撮影画像だけから算出した撮像装置の位置及び姿勢を初期値として用いる場合には、前述のように初期値そのものが誤ったものになる可能性があるため、その後の反復演算でも最適化できないことがあった。
【００１３】
本発明は以上の問題に鑑みてなされたものであり、本願請求項１記載の発明は、傾斜角センサが装着された撮像装置または物体の位置と姿勢を、反復演算を行うことなく直接的に求めることを目的とする。
【００１４】
また、本願請求項２記載の発明は、傾斜角センサが装着された撮像装置と傾斜角が装着された物体との相対的な位置と姿勢を反復演算を行うことなく直接的に求めることを目的とする。
【課題を解決するための手段】
【００１５】
本発明の目的を達成するために、本発明における位置姿勢計測装置は次の構成を備える。
【００１６】
本願請求項１記載の位置姿勢計測方法は、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする。
【００１７】
本願請求項２記載の位置姿勢計測方法は、物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力工程と、撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする。
【発明の効果】
【００１８】
本願請求項１記載の発明によれば、傾斜角センサが装着された撮像装置または物体の位置と姿勢を、反復演算を行うことなく直接的に求めることができる。
【００１９】
また、本願請求項２記載の発明は、傾斜角センサが装着された撮像装置と傾斜角が装着された物体との相対的な位置と姿勢を反復演算を行うことなく直接的に求めることができる。
【発明を実施するための最良の形態】
【００２０】
以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
【００２１】
［第１の実施形態］
本実施形態では、本発明の位置姿勢計測装置及びその方法を、ジャイロセンサを装着した撮像装置の位置及び姿勢の計測に適用した場合について説明する。なお、本実施形態におけるジャイロセンサとは、前述したように３軸方向の角速度を計測するための複数のジャイロセンサと、３軸方向の加速度を計測するための複数の加速度センサの組合せによって構成される３軸姿勢センサであり、傾斜角成分はある程度信頼できる計測値を出力するが、方位角成分はドリフト誤差を含んでいる。そのため、本実施形態はジャイロセンサにのみ適用されるものではなく、姿勢の傾斜角成分を計測可能な任意の傾斜角センサについて適用できるものである。
【００２２】
図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、指標検出部１１０、姿勢入力部１２０、位置姿勢算出部１３０によって構成されており、撮像装置１４０に接続されている。また、ジャイロセンサ１５０は撮像装置１４０上に固定され、姿勢入力部１２０に接続されている。
【００２３】
図２は、本実施形態における座標系の関係を示している。基準座標系には、位置が既知の指標が配置または設定されているものとする。本実施形態に係る位置姿勢計測装置は、基準座標系に対する撮像装置の位置及び姿勢を計測する。本実施形態では、基準座標系のｙｗ軸を鉛直上方向にとる。
【００２４】
ジャイロセンサ１５０は、撮像装置の基準座標系に対する姿勢を出力する。基準座標系に対する撮像装置の姿勢を表す３×３回転変換行列をＲｗｃ、基準座標系に対する撮像装置の位置を表す３次元ベクトルをｔｗｃと表す。Ｒｗｃは、基準座標系のｚｗ軸まわりの回転変換
【００２５】
【数１】
【００２６】
軸まわりの回転変換
【００２７】
【数２】
【００２８】
軸まわりの回転変換
【００２９】
【数３】
【００３０】
を順に組み合わせることで表現可能であり、（式１）のように表すことができる
【００３１】
【数４】
【００３２】
ここで、
【００３３】
【数５】
【００３４】
はＲｗｃの傾斜角成分、
【００３５】
【数６】
【００３６】
はＲｗｃの方位角成分であるため、
【００３７】
【数７】
【００３８】
とおくと、
（式１）は（式２）のように記述できる（ここでｉはｉｎｃｌｉｎａｔｉｏｎ（傾斜角）、ａはａｚｉｍｕｔｈ（方位角）を表す）。
【００３９】
【数８】
【００４０】
よって、基準座標系から撮像装置の座標系（以下、撮像装置座標系）への座標変換は、
【００４１】
【数９】
【００４２】
及び撮像装置座標系における基準座標系の原点の位置ｔｃｗを用いて（式３）のように表せる。
【００４３】
【数１０】
【００４４】
但し、ｘｗ、ｘｃはそれぞれ基準座標系と撮像装置座標系における点の座標を表している。
【００４５】
ここで、
【００４６】
【数１１】
【００４７】
を（式４）のように表す。
【００４８】
【数１２】
【００４９】
また、方位角をθとすると、
【００５０】
【数１３】
【００５１】
は（式５）のように表される。
【００５２】
【数１４】
【００５３】
基準座標系における位置がｘｗ＝［ｘｗｙｗｚｗ］ｔである指標が画像上に投影される位置（ｕｘ，ｕｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘｃ＝［ｘｃｙｃｚｃ］ｔ、ｔｃｗ＝［ｔｘｔｙｔｚ］ｔとして（式６）のように表される。
【００５４】
【数１５】
【００５５】
次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて撮像装置の位置及び姿勢を算出する方法について説明する。図３は、本実施形態の処理手順を示すフローチャートである。
【００５６】
まずステップＳ１０１０において、指標検出部１１０は撮像装置１４０から画像を入力し、姿勢入力部１２０は撮影時のジャイロセンサの計測値を入力する。姿勢入力部１２０はジャイロセンサの計測値Ｒｗｃを傾斜角成分
【００５７】
【数１６】
【００５８】
と方位角成分
【００５９】
【数１７】
【００６０】
に分解し、（式４）からｒ１１，ｒ１２，ｒ１３，ｒ２１，ｒ２２，ｒ２３，ｒ３１，ｒ３２，ｒ３３を得て、位置姿勢算出部１３０へと出力する。次に、ステップＳ１０２０において、指標検出部１１０は撮影画像中から指標を検出して、位置姿勢算出部１３０へと出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を
【００６１】
【数１８】
【００６２】
とする。
【００６３】
次に、ステップＳ１０３０において、位置姿勢算出部１３０は、すべての検出した指標が同一水平面上（本実施形態における水平面はｙｗ＝定数で表される）にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の基準座標系における位置を
【００６４】
【数１９】
【００６５】
として、（式７）より検出指標のｙｗ座標の分散σ２を求める。
【００６６】
【数２０】
【００６７】
但し、
【００６８】
【数２１】
【００６９】
は指標のｙｗ座標の平均値であり、（式８）から求められる。
【００７０】
【数２２】
【００７１】
この分散σ２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ１０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ１０５０に進み、同一水平面上にないと判定された場合にはステップＳ１０４０に進む。
【００７２】
ステップＳ１０４０においては、位置姿勢算出部１３０は検出指標の数が３個以上であるか否かの判定を行う。ステップＳ１０４０において検出指標の数が３個以上であればステップＳ１０６０に進み、３個未満であれば終了する。また、ステップＳ１０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ１０５０において検出指標の数が２個以上であればステップＳ１０７０に進み、２個未満であれば終了する。
【００７３】
ステップＳ１０６０では、位置姿勢算出部１３０は手法（１Ａ）を用いて撮像装置の位置姿勢を算出し、これを出力する。ここで手法（１Ａ）について説明する。
【００７４】
（式６）の分母を払い、
【００７５】
【数２３】
【００７６】
とおくことによって、（式９）に示すα、β、ｔｘ、ｔｙ、ｔｚについての線形方程式が得られる。
【００７７】
【数２４】
【００７８】
入力した夫々の（ｎ点の）指標について、
【００７９】
【数２５】
【００８０】
とおいて、各指標について（式９）の線形方程式を立てると、（式１０）に示す連立方程式が得られる。
【００８１】
【数２６】
【００８２】
（式１０）を解くことによって、α、β、ｔｘ、ｔｙ、ｔｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。（式１０）を（式１１）に示すように簡略化して表す。
Ａ・ｘ＝ｂ （式１１）
ここでｘ＝［αβｔｘｔｙｔｚ］ｔである。また、Ａは行数（２×ｎ）、列数５の行列となり、ｂは（２×ｎ）次元ベクトルとなる。最小二乗法によってｘを（式１２）のように求める。
ｘ＝（Ａｔ・Ａ）−１・Ａｔ・ｂ （式１２）
なお、（式１２）では、Ａの一般化逆行列を（Ａｔ・Ａ）−１としているが，他の一般化逆行列を用いても本発明の本質が失われることがないのは言うまでもない。さらに、（式１２）より得られたα、βより、（式１３）によって方位角θを算出する。
【００８３】
【数２７】
【００８４】
得られたθと、センサ計測値の傾斜角成分
【００８５】
【数２８】
【００８６】
をもとに、基準座標系に対する撮像装置の姿勢Ｒｗｃを（式１４）によって算出する。
【００８７】
【数２９】
【００８８】
また、基準座標系における撮像装置の位置ｔｗｃを、（式１５）によって算出する。
ｔｗｃ＝−Ｒｗｃ・ｔｃｗ （式１５）
以上述べた方法で、ステップＳ１０６０において撮像装置の位置姿勢を算出し、これを出力する。
【００８９】
ステップＳ１０７０では、位置姿勢算出部１３０は手法（１Ｂ）を用いて撮像装置の位置姿勢を算出し、これを出力する。ここで手法（１Ｂ）について説明する。
【００９０】
指標の基準座標系におけるｙｗ座標をｙａｖｇとする。（式６）においてｙｗ＝０として、
【００９１】
【数３０】
【００９２】
とおいて分母を払い、さらに
【００９３】
【数３１】
【００９４】
とおくと（式１６）が得られる。
【００９５】
【数３２】
【００９６】
入力した夫々の（ｎ点の）指標について同様に
【００９７】
【数３３】
【００９８】
とおいて、各指標について（式１６）の線形方程式を立てると、（式１７）に示す連立方程式が得られる。
【００９９】
【数３４】
【０１００】
（式１７）を（式１２）に示した方法と同じように解くことによって、α’、β’、ｔ’ｘ、ｔ’ｙを得る。この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。
【０１０１】
（式１７）を解くことによって得られたα’、β’より、方位角θを（式１８）を用いて算出する。
【０１０２】
【数３５】
【０１０３】
得られたθと、センサ計測値の傾斜角成分
【０１０４】
【数３６】
【０１０５】
をもとに、基準座標系に対する撮像装置の姿勢Ｒｗｃを（式１４）によって算出する。また、ｔｃｗ＝［ｔｘｔｙｔｚ］ｔを（式１９）から求める。
【０１０６】
【数３７】
【０１０７】
また、基準座標系における撮像装置の位置ｔｗｃは、（式２０）によって算出する。
【０１０８】
【数３８】
【０１０９】
以上述べた方法で、ステップＳ１０７０において撮像装置の位置姿勢を算出し、これを出力する。
【０１１０】
以上述べたように、本実施形態では、撮像装置にジャイロセンサを装着した場合に、撮像装置の位置姿勢をセンサ計測値の傾斜角成分と画像情報から反復演算を必要としない方法で直接的に算出することができる。
【０１１１】
撮像装置にジャイロセンサを装着し、撮像装置によって撮影された画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に撮像装置の位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。実施形態では、１台のカメラの画像情報を利用する場合の位置姿勢計測方法について述べた。ある種のＡＲアプリケーションでは、ステレオ表示が有効であるため、ステレオカメラをＨＭＤ（Ｈｅａｄ Ｍｏｕｎｔｅｄ Ｄｉｓｐｌａｙ）に搭載している場合がある。このようなＨＭＤに姿勢センサが装着されている場合、ステレオペアのカメラに対して１つの傾斜角センサが装着されていると考えると、１台の場合と同様にそれぞれのカメラの傾斜角が一意に定まる。そのため、本実施形態で説明した位置姿勢計測方法において、ステレオカメラのそれぞれのカメラから得られる指標を利用するようにしてもよい。また、カメラ間の相対位置姿勢がわかっていれば、カメラは２台に限られることはなく、任意の台数のカメラを利用することもできる。
【０１１２】
［第２の実施形態］
第１の実施形態では、撮像装置にジャイロセンサを装着して、撮像装置の位置姿勢を求めた。第２の実施形態では、位置姿勢計測装置及びその方法を、基準座標系に固定された撮像装置によって撮影される移動物体の位置及び姿勢の計測に適用した場合について説明する。
【０１１３】
図４は、第２の実施形態における位置姿勢計測装置の構成を示している。同図に示したように、第２の実施形態における位置姿勢計測装置２００は、指標検出部２１０、姿勢入力部２２０、位置姿勢算出部２３０によって構成されている。基準座標系に固定された撮像装置２４０は指標検出部２１０に接続されている。また移動物体２６０上に装着されたジャイロセンサ２５０は姿勢入力部２２０に接続されている。第２の実施形態では、撮像装置２４０によって撮影された画像中で検出される移動物体上の指標と、移動物体上に装着されたジャイロセンサの計測値の傾斜角成分を使って、基準座標系に対する移動物体の位置姿勢を計測する。
【０１１４】
図５は、第２の実施形態における座標系の関係を示す図である。移動物体上には、移動物体座標系での位置が既知の指標が配置又は設置されているものとする。撮像装置は基準座標系に固定されており、撮像装置の基準座標系における位置を表す３次元ベクトルｔｗｃ、姿勢を表す３×３回転行列Ｒｗｃは既知とする。第２の実施形態では、基準座標系に対する移動物体の姿勢Ｒｗｏ及び位置ｔｗｏを求める。
【０１１５】
基準座標系に対する移動物体の姿勢を表す３×３回転行列Ｒｗｏは、（式２１）に示すように傾斜角成分
【０１１６】
【数３９】
【０１１７】
と方位角成分
【０１１８】
【数４０】
【０１１９】
に分解される。
【０１２０】
【数４１】
【０１２１】
移動物体座標系ｘｏから撮像装置座標系ｘｃへの座標変換は、
【０１２２】
【数４２】
【０１２３】
及び撮像装置座標系における移動物体の位置ｔｃｏを用いて（式２２）のように表せる。
【０１２４】
【数４３】
【０１２５】
ここで、
【０１２６】
【数４４】
【０１２７】
を（式２３）のように表す。
【０１２８】
【数４５】
【０１２９】
また、方位角をθとすると、
【０１３０】
【数４６】
【０１３１】
は（式２４）のように表される。
【０１３２】
【数４７】
【０１３３】
【数４８】
【０１３４】
とおいた場合に、移動物体座標がｘｏ＝［ｘｏｙｏｚｏ］ｔの指標が画像上に投影される位置（ｕｘ，ｕｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘｃ＝［ｘｃｙｃｚｃ］ｔ、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔとして（式２５）のように表される。
【０１３５】
【数４９】
【０１３６】
次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて移動物体の位置及び姿勢を算出する方法について説明する。図６は、第２の実施形態の処理手順を示すフローチャートである。
【０１３７】
まずステップＳ２０１０において、指標検出部２１０は撮像装置２４０から画像を入力する。姿勢入力部２２０は、撮影時の移動物体上のジャイロセンサの計測値Ｒｗｏを入力し、傾斜角成分
【０１３８】
【数５０】
【０１３９】
と方位角成分
【０１４０】
【数５１】
【０１４１】
に分解し、（式２３）から既知の値であるＲ１１，Ｒ１２，Ｒ１３，Ｒ２１，Ｒ２２，Ｒ２３，Ｒ３１，Ｒ３２，Ｒ３３を得て、位置姿勢算出部２３０へと出力する。次に、ステップＳ２０２０において、指標検出部２１０は撮影画像中から移動物体上の指標を検出して、位置姿勢算出部２３０へ出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を
【０１４２】
【数５２】
【０１４３】
とする。次に、ステップＳ２０３０において、位置姿勢算出部２３０は、すべての検出した指標が同一水平面上にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の移動物体座標系での位置を
【０１４４】
【数５３】
【０１４５】
として、（式２６）より検出指標のｙｗ座標の分散σ２を求める。
【０１４６】
【数５４】
【０１４７】
但し、
【０１４８】
【数５５】
【０１４９】
は指標のｙｗ座標の平均値であり、（式２７）から求められる。
【０１５０】
【数５６】
【０１５１】
この分散σ２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ２０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ２０５０に進み、同一水平面上にないと判定された場合にはステップＳ２０４０に進む。ステップＳ２０４０においては、位置姿勢算出部２３０は検出指標の数が３個以上であるか否かの判定を行う。ステップＳ２０４０において検出指標の数が３個以上であればステップＳ２０６０に進み、３個未満であれば終了する。また、ステップＳ２０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ２０５０において検出指標の数が２個以上であればステップＳ２０７０に進み、２個未満であれば終了する。
【０１５２】
ステップＳ２０６０では、位置姿勢算出部２３０は手法（２Ａ）を用いて移動物体の位置姿勢を算出し、これを出力する。ここで手法（２Ａ）について説明する。（式２５）の分母を払い、
【０１５３】
【数５７】
【０１５４】
とおいて得られるα、β、ｔｏｘ、ｔｏｙ、ｔｏｚについての線形方程式を検出された各指標について立てることにより、（式２８）に示す連立方程式が得られる。
【０１５５】
【数５８】
【０１５６】
（式２８）を解くことにより、α、β、ｔｏｘ、ｔｏｙ、ｔｏｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。α、βより、（式１３）を用いて方位角θを算出する。得られたθと、センサ計測値の傾斜角成分
【０１５７】
【数５９】
【０１５８】
をもとに、基準座標系に対する移動物体の姿勢Ｒｗｏを（式２９）によって算出する。
【０１５９】
【数６０】
【０１６０】
また移動物体の基準座標系における位置ｔｗｏを（式３０）によって算出する。
ｔｗｏ＝Ｒｗｃ・ｔｃｏ＋ｔｗｃ （式３０）
以上述べた方法で、ステップＳ２０６０において移動物体の位置姿勢を算出し、これを出力する。
【０１６１】
ステップＳ２０７０では、位置姿勢算出部２３０は手法（２Ｂ）を用いて移動物体の位置姿勢を算出し、これを出力する。ここで手法（２Ｂ）について説明する。
【０１６２】
指標の基準座標系におけるｙｗ座標をｙａｖｇとする。（式２５）において
【０１６３】
【数６１】
【０１６４】
として、
【０１６５】
【数６２】
【０１６６】
とおいて分母を払い、
【０１６７】
【数６３】
【０１６８】
とおいて得られるα’、β’、ｔ’ｏｘ、ｔ’ｏｙについての線形方程式を検出された各指標について立てることにより、（式３１）に示す連立方程式が得られる。
【０１６９】
【数６４】
【０１７０】
この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。（式３１）を解くことによって得られたα’、β’から、方位角θを（式１８）によって算出する。得られたθと、センサ計測値の傾斜角成分
【０１７１】
【数６５】
【０１７２】
をもとに、基準座標系に対する移動物体の姿勢Ｒｗｏを（式２９）によって算出する。また、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔを（式３２）によって算出する。
【０１７３】
【数６６】
【０１７４】
さらに移動物体の基準座標系における位置ｔｗｏを（式３３）によって算出する。
【０１７５】
【数６７】
【０１７６】
以上述べた方法で、ステップＳ２０７０において移動物体の位置姿勢を算出し、これを出力する。
【０１７７】
以上述べたように、本実施形態によれば、移動物体にジャイロセンサを装着した場合に、移動物体の位置姿勢をセンサ計測値の傾斜角成分と基準座標系に固定された撮像装置の画像情報から反復演算を必要としない方法で直接的に算出することができる。
【０１７８】
本実施形態によれば、移動物体にジャイロセンサを装着し、シーンに固定した撮像装置によって撮影された移動物体の画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に移動物体の位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。
【０１７９】
本実施形態では、１台のカメラの画像情報を利用する場合の位置姿勢計測方法について述べた。本実施形態でのカメラは基準座標系での位置姿勢が既知な客観視点カメラである。客観視点カメラの基準座標系における位置をｔｗｃ（＝［ｘｗｃｙｗｃｚｗｃ］ｔ）、姿勢を表す３×３回転行列をＲｗｃとすると、移動物体座標系ｘｏから撮像装置座標系ｘｃへの座標変換は、移動物体の基準座標系における位置ｔｗｏ（＝［ｘｗｏｙｗｏｚｗｏ］ｔ）を用いて、（式３４）で表せる。
【０１８０】
【数６８】
【０１８１】
【数６９】
【０１８２】
、（式２３）、（式２４）、（式３４）より点
【０１８３】
【数７０】
【０１８４】
が画像上に投影される位置（ｕｘ，ｕｙ）は（式３５）のように表される。
【０１８５】
【数７１】
【０１８６】
式（３５）は、ｔｗｏとα、βを未知パラメータとして、画像上で検出される各指標について成り立つ観測方程式である。この観測方程式を複数のカメラが撮影する画像上の指標について立てることにより、本実施形態で述べたのと同様な方法で、ｔｗｏとα、βを求めることができる。すなわち、本実施形態は１台のカメラに限定されるものではなく、基準座標系における位置姿勢が既知な複数のカメラの画像情報を利用することができる。
【０１８７】
［第３の実施形態］
第２の実施形態では、撮像装置を基準座標系に固定し、移動物体にジャイロセンサを装着したが、第３の実施形態では、位置姿勢計測装置及びその方法を、撮像装置、移動物体の両方にジャイロセンサを装着した場合の撮像装置に対する移動物体の位置及び姿勢の計測に用いた場合について説明する。
【０１８８】
図７は、第３の実施形態における位置姿勢計測装置の構成を示している。同図に示したように、第３の実施形態における位置姿勢計測装置３００は、指標検出部３１０、姿勢入力部３２０、位置姿勢算出部３３０によって構成されている。撮像装置３４０は指標検出部３１０に接続されている。また、移動物体３６０にはジャイロセンサ３５５が、撮像装置３４０にはジャイロセンサ３５０が装着されており、ジャイロセンサ３５０、３５５は姿勢入力部３２０に接続されている。第３の実施形態では、撮像装置３４０によって撮影された画像中で検出される移動物体の指標と、移動物体及び撮像装置に装着された夫々のジャイロセンサの計測値の傾斜角成分を使って、撮像装置に対する移動物体の位置姿勢を求める。図８は第３の実施形態における座標系の関係を表す図である。第３の実施形態では、撮像装置座標系に対する移動物体の姿勢Ｒｃｏ及び位置ｔｃｏを求める。基準座標系における移動物体の姿勢を表す３×３回転行列をＲｗｏ、撮像装置の姿勢を表す３×３回転行列をＲｗｃとする。Ｒｗｏは（式３６）に示すように、傾斜角成分
【０１８９】
【数７２】
【０１９０】
と方位角成分
【０１９１】
【数７３】
【０１９２】
に分解される。
【０１９３】
【数７４】
【０１９４】
またＲｗｃは（式３７）に示すように、傾斜角成分
【０１９５】
【数７５】
【０１９６】
と方位角成分
【０１９７】
【数７６】
【０１９８】
に分解される。
【０１９９】
【数７７】
【０２００】
移動物体座標系ｘｏから撮像装置座標系ｘｃへの変換は、
【０２０１】
【数７８】
【０２０２】
及び撮像装置座標系における移動物体の位置ｔｃｏを用いて（式３８）のように表せる。
【０２０３】
【数７９】
【０２０４】
【数８０】
【０２０５】
は方位角を表しているので、
【０２０６】
【数８１】
【０２０７】
としてまとめる。
【０２０８】
ここで、
【０２０９】
【数８２】
【０２１０】
、Ｒａをそれぞれ（式３９）、（式４０）のように表す。θは方位角である。
【０２１１】
【数８３】
【０２１２】
【数８４】
【０２１３】
【数８５】
【０２１４】
とおいた場合に、移動物体座標がｘｏ＝［ｘｏｙｏｚｏ］ｔの指標が画像上に投影される位置（ｕｘ，ｕｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘｃ＝［ｘｃｙｃｚｃ］ｔ、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔとして（式４１）のように表される。
【０２１５】
【数８６】
【０２１６】
次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて、撮像装置に対する移動物体の位置姿勢を算出する方法について説明する。図９は、第３の実施形態の処理手順を示すフローチャートである。
【０２１７】
まずステップＳ３０１０において、指標検出部３１０は、撮像装置から画像を入力する。また、姿勢入力部３２０は、撮影と同時に移動物体に装着されたジャイロセンサの計測値Ｒｗｏ及び撮像装置に装着されたジャイロセンサの計測値Ｒｗｃを入力し、それぞれ公知の手法を使って傾斜角成分
【０２１８】
【数８７】
【０２１９】
と方位角成分
【０２２０】
【数８８】
【０２２１】
に分解する。また、（式３９）から既知であるｒ１１，ｒ１２，ｒ１３，ｒ２１，ｒ２２，ｒ２３，ｒ３１，ｒ３２，ｒ３３を得て、位置姿勢算出部３３０に出力する。次に、ステップＳ３０２０において、指標検出部３１０は、撮影画像中から移動物体上の指標を検出して位置姿勢算出部３３０に出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を
【０２２２】
【数８９】
【０２２３】
とする。次に、ステップＳ３０３０において、位置姿勢算出部３３０は、すべての検出した指標が同一水平面上にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の移動物体座標系での位置を
【０２２４】
【数９０】
【０２２５】
として、（式４２）より検出指標のｙｗ座標の分散σ２を求める。
【０２２６】
【数９１】
【０２２７】
但し、
【０２２８】
【数９２】
【０２２９】
は指標のｙｗ座標の平均値であり、（式４３）から求められる。
【０２３０】
【数９３】
【０２３１】
この分散σ２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ３０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ３０５０に進み、同一水平面上にないと判定された場合にはステップＳ３０４０に進む。
【０２３２】
ステップＳ３０４０においては、位置姿勢算出部３３０は、検出指標の数が３個以上であるか否かの判定を行う。ステップＳ３０４０において検出指標の数が３個以上であればステップＳ３０６０に進み、３個未満であれば終了する。また、ステップＳ３０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ３０５０において検出指標の数が２個以上であればステップＳ３０７０に進み、２個未満であれば終了する。
【０２３３】
ステップＳ３０６０では、位置姿勢算出部３３０は、手法（３Ａ）を用いて撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。ここで手法（３Ａ）について説明する。（式４１）の分母を払い、
【０２３４】
【数９４】
【０２３５】
とおいて得られるα、β、ｔｏｘ、ｔｏｙ、ｔｏｚについての線形方程式を検出された各指標について立てることにより、（式４４）に示す連立方程式が得られる。
【０２３６】
【数９５】
【０２３７】
（式４４）を解くことにより、α、β、ｔｏｘ、ｔｏｙ、ｔｏｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。α、βより、（式１３）を用いて方位角θを算出する。移動物体の撮像装置に対する姿勢Ｒｃｏは（式４５）によって算出する。
【０２３８】
【数９６】
【０２３９】
以上述べた方法で、ステップＳ３０６０において撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。
【０２４０】
ステップＳ３０７０では、位置姿勢算出部３３０は、手法（３Ｂ）を用いて撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。ここで手法（３Ｂ）について説明する。
【０２４１】
指標の基準座標系におけるｙｗ座標をｙａｖｇとする。（式４１）において
【０２４２】
【数９７】
【０２４３】
として、
【０２４４】
【数９８】
【０２４５】
とおいて分母を払い、
【０２４６】
【数９９】
【０２４７】
とおいて得られるα’、β’、ｔ’ｏｘ、ｔ’ｏｙについての線形方程式を検出された各指標について立てることにより、（式４６）に示す連立方程式が得られる。
【０２４８】
【数１００】
【０２４９】
この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。（式４６）を解くことによって得られたα’、β’より、方位角θは（式１８）によって算出する。移動物体の撮像装置に対する姿勢Ｒｃｏは（式４５）によって算出する。また、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔは（式３２）から算出する。
【０２５０】
以上述べた方法で、ステップＳ３０７０において撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。
【０２５１】
以上述べたように、本実施形態では、移動物体と撮像装置にジャイロセンサを装着した場合に、撮像装置に対する移動物体の位置姿勢（すなわち、撮像装置と移動物体の相対的な位置と姿勢）をセンサ計測値の傾斜角成分と撮像装置の画像情報から反復演算を必要としないで直接的に算出することができる。
【０２５２】
本実施形態によれば、撮像装置と移動物体の両方にジャイロセンサを装着し、撮像装置によって撮影された移動物体の画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に撮像物体と移動物体の相対的な位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。
【０２５３】
［変形例１］
以上述べた実施形態では、ジャイロセンサを姿勢センサとして用いていたが、これに限るものではなく、姿勢の傾斜角成分を計測可能なセンサであればどのような傾斜角センサを用いてもよい。また、傾斜角は鉛直軸に対する傾斜角に限定されるものではなく、任意の軸に対する傾斜角であって、傾斜角センサは該軸に対する傾斜角を計測するものであればどのような傾斜角センサを用いてもよい。
【０２５４】
［変形例２］
以上述べた実施形態では、画像上での指標の画像座標とその三次元位置及び姿勢センサから得られる姿勢の傾斜角成分をもとに、反復演算をしないで撮像装置の位置姿勢（第１の実施形態）、移動物体の位置姿勢（第２の実施形態）、撮像装置と物体との相対位置姿勢（第３の実施形態）を求めた。このようにして得られる位置姿勢を初期値として、別の手段によって位置姿勢を最適化してもよい。例えば、非特許文献２の手法を用いて、指標の画像座標とその三次元位置に基づき、６自由度の位置姿勢すべてを最適化してもよいし、非特許文献３の手法を用いて、指標の画像座標とその三次元位置および姿勢センサの計測値に基づき位置と姿勢の方位角成分を最適化してもよい。その際に反復演算を行うことがあっても、構わない。上記実施形態１〜３の方法により、例えば反復演算を用いる最適化の初期値にすることにより、初期値が不適切である（例えば前フレームで位置姿勢を求めることができない時）ことによる位置姿勢算出精度の低下を抑制することができる。
【０２５５】
［変形例３］
以上述べた実施形態では、行列演算によって撮像装置と物体との相対位置姿勢を求めていたが、上記実施形態の本質は連立方程式を反復演算なしに直接的に解くことにあり、必ずしも行列演算によって求める必要がないのは言うまでもない。
【０２５６】
［変形例４］
また、以上述べた実施形態では、（式１０）に表されるように、すべての検出指標の信頼度を同じ信頼度とみなして連立方程式を解いているが、検出指標ごとに信頼度を変えて、重みを付与して連立方程式を解いてもよい。
【０２５７】
［変形例５］
さらに、以上述べた実施形態では、指標が同一水平面上にあるかどうかを判定して計算手法を切り替えていたが、指標が同一水平面上にあるかどうかが予め分かっている場合には、その判定ステップを省略して直接（式１０）または（式１７）（もしくは（式２８）または（式３１）、（式４４）または（式４６））を解いてもよい。
【０２５８】
（他の実施形態）
前述した実施の形態の機能を実現する様に各種のデバイスを動作させる様に該各種デバイスと接続された装置あるいはシステム内のコンピュータに、前記実施の形態の機能を実現するためのソフトウエアのプログラムコードを供給し、そのシステムあるいは装置のコンピュータ（ＣＰＵあるいはＭＰＵ）を格納されたプログラムに従って前記各種デバイスを動作させることによって実施したものも本発明の範疇に含まれる。
【０２５９】
この場合、前記ソフトウエアのプログラムコード自体が前述した実施の形態の機能を実現することになり、そのプログラムコード自体、及びそのプログラムコードをコンピュータに供給するための手段、例えばかかるプログラムコードを格納した記憶媒体は本発明を構成する。
【０２６０】
かかるプログラムコードを格納する記憶媒体としては例えばフロッピー（登録商標）ディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることが出来る。
【０２６１】
またコンピュータが供給されたプログラムコードを実行することにより、前述の実施形態の機能が実現されるだけではなく、そのプログラムコードがコンピュータにおいて稼働しているＯＳ（オペレーティングシステム）、あるいは他のアプリケーションソフト等と共同して前述の実施形態の機能が実現される場合にもかかるプログラムコードは本発明の実施形態に含まれることは言うまでもない。
【０２６２】
更に供給されたプログラムコードが、コンピュータの機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに格納された後そのプログラムコードの指示に基づいてその機能拡張ボードや機能格納ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も本発明に含まれることは言うまでもない。
【図面の簡単な説明】
【０２６３】
【図１】第一の実施形態の構成を示す図である。
【図２】第一の実施形態における座標系の関係を示す図である。
【図３】第一の実施形態の処理手順を示すフローチャートである。
【図４】第二の実施形態の構成を示す図である。
【図５】第二の実施形態における座標系の関係を示す図である。
【図６】第二の実施形態の処理手順を示すフローチャートである。
【図７】第三の実施形態の構成を示す図である。
【図８】第三の実施形態における座標系の関係を示す図である。
【図９】第三の実施形態の処理手順を示すフローチャートである。
【技術分野】
【０００１】
本発明は、傾斜角センサの計測値に基づき、撮像装置または物体の位置と姿勢を直接的に算出するものに関する。
【背景技術】
【０００２】
近年、複合現実感（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ；ＭＲ）技術の研究が盛んである。ＭＲ技術は、現実空間とコンピュータによって作られる仮想空間を継ぎ目なく融合する技術である。ＭＲ技術の中でも、現実空間に仮想空間を重ね合わせて表示するＡｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＡＲ、拡張現実感、増強現実感とも呼ばれる）技術が特に注目を集めている。
【０００３】
ＡＲの画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、該撮像装置の位置及び姿勢に応じて生成された仮想空間（コンピュータグラフィクスにより描画された仮想物体や文字情報）の画像を重畳描画した合成画像を表示するビデオシースルー方式か、あるいは観察者の頭部に装着された光学シースルー型ディスプレイに、観察者の視点の位置及び姿勢に応じて生成された仮想空間の画像を表示する光学シースルー方式によって実現される。
【０００４】
ＡＲ技術は、患者の体表面に体内の様子を重畳表示する手術支援や、空き地に仮想のビルを重畳表示する建築シミュレーション、組立作業時に作業手順を重畳表示する組み立て支援など様々な分野への応用が期待される。
【０００５】
ＡＲ技術において最も重要な課題の一つは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来より多くの取り組みが行われてきた。ＡＲにおける位置合わせの問題は、ビデオシースルー方式の場合はシーン中における（すなわちシーン中で規定された基準座標系における）撮像装置の位置及び姿勢を求める問題に帰結する。同様に、光学シースルー方式の場合は、シーン中における観察者の視点あるいはディスプレイの位置及び姿勢を求める問題となる。
【０００６】
前者の問題を解決する方法として、シーン中に複数の指標を配置あるいは設定し、撮像装置が撮影した画像上での指標の投影像の位置を検出することで、シーン中における該撮像装置の位置及び姿勢を求めることが一般的に行われている。また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置を装着し、前者と同様な方法によって該撮像装置の位置及び姿勢を求め、それに基づいて計測対象物の位置及び姿勢を求めることが一般的に行われている。
【０００７】
画像上の指標の投影像とその指標の三次元位置との対応をもとに撮像装置の位置及び姿勢を求める方法は、写真測量の分野において古くから提案されている（例えば、非特許文献１を参照）。また、前述の画像上の指標の投影像をもとに求めた撮像装置の位置及び姿勢を初期値とし、画像上での指標の投影像の実際の観察位置と、投影像の計算位置、すなわち指標の三次元位置と撮像装置の位置及び姿勢から計算される投影像の位置との誤差が最小になるように反復演算によって撮像装置の位置及び姿勢を最適化することが行われている（例えば、非特許文献２を参照）。
【０００８】
一方、撮像装置によって撮影された画像だけを用いるのではなく、ジャイロセンサ（厳密には、３軸方向の角速度を計測するための複数のジャイロセンサと、３軸方向の加速度を計測するための複数の加速度センサの組合せによって構成される３軸姿勢センサであるが、本明細では便宜上これをジャイロセンサと呼ぶことにする）を撮像装置に装着し、画像情報とジャイロセンサの情報を併用して撮像装置の位置及び姿勢を求めることも行われている。
【０００９】
特許文献１では、ジャイロセンサの出力する姿勢の計測値をそのまま撮像装置の姿勢として用い、該撮像装置の姿勢と画像上の指標の投影像及びその指標の三次元位置との対応をもとに撮像装置の位置を線形連立方程式を解くことによって求めている。また、非特許文献３では、ジャイロセンサの出力する姿勢の計測値のうち傾斜角成分をそのまま撮像装置の姿勢の傾斜角成分とし、前述した画像上での指標の投影像の観察位置と計算位置との誤差が最小化されるように、撮像装置の位置と姿勢の方位角成分を反復演算によって最適化することが行われている。
【非特許文献１】解析写真測量 改訂版，社団法人日本写真測量学会，１９８９．
【非特許文献２】加藤，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，浅野，橘：“マーカー追跡に基づく拡張現実感システムとそのキャリブレーション”，日本バーチャルリアリティ学会論文誌，ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１６，１９９９．
【非特許文献３】Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，ａｎｄ Ｈ．Ｙａｍａｍｏｔｏ：“Ａ ｈｅａｄ ｔｒａｃｋｉｎｇ ｍｅｔｈｏｄ ｕｓｉｎｇ ｂｉｒｄ’ｓ−ｅｙｅ ｖｉｅｗ ｃａｍｅｒａ ａｎｄ ｇｙｒｏｓｃｏｐｅ，”Ｐｒｏｃ．３ｒｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔ’ｌ Ｓｙｍｐ．ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ２００４），ｐｐ．８９−９８，２００４．
【特許文献１】特開２００２−２５９９９２号公報
【発明の開示】
【発明が解決しようとする課題】
【００１０】
前述した、撮像装置によって撮影された画像上での指標の投影像のみを利用した手法では、得られる撮像装置の位置及び姿勢が、指標の画像上での検出誤差や、画像の解像度が有限であることが原因となって、間違った位置及び姿勢が得られたり、ジッタを生じることがあった。
【００１１】
また、ジャイロセンサにはドリフト誤差があるため、時間経過に伴って徐々に方位方向の計測値に誤差が生じてしまう。そのため、ジャイロセンサが出力する姿勢計測値をそのまま撮像装置の姿勢とした場合には、間違った撮像装置の位置が得られることがあった。
【００１２】
さらに、反復演算によって撮像装置の位置及び姿勢、または撮像装置の位置及び姿勢の方位角成分を求める場合には初期値が必要である。前フレームの結果を初期値として用いる場合に、前フレームで撮像装置の位置及び姿勢が求められなければ（例えば、前フレームで指標が撮影されなかった場合）初期値を得ることができなかった。また、撮影画像だけから算出した撮像装置の位置及び姿勢を初期値として用いる場合には、前述のように初期値そのものが誤ったものになる可能性があるため、その後の反復演算でも最適化できないことがあった。
【００１３】
本発明は以上の問題に鑑みてなされたものであり、本願請求項１記載の発明は、傾斜角センサが装着された撮像装置または物体の位置と姿勢を、反復演算を行うことなく直接的に求めることを目的とする。
【００１４】
また、本願請求項２記載の発明は、傾斜角センサが装着された撮像装置と傾斜角が装着された物体との相対的な位置と姿勢を反復演算を行うことなく直接的に求めることを目的とする。
【課題を解決するための手段】
【００１５】
本発明の目的を達成するために、本発明における位置姿勢計測装置は次の構成を備える。
【００１６】
本願請求項１記載の位置姿勢計測方法は、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする。
【００１７】
本願請求項２記載の位置姿勢計測方法は、物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力工程と、撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする。
【発明の効果】
【００１８】
本願請求項１記載の発明によれば、傾斜角センサが装着された撮像装置または物体の位置と姿勢を、反復演算を行うことなく直接的に求めることができる。
【００１９】
また、本願請求項２記載の発明は、傾斜角センサが装着された撮像装置と傾斜角が装着された物体との相対的な位置と姿勢を反復演算を行うことなく直接的に求めることができる。
【発明を実施するための最良の形態】
【００２０】
以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
【００２１】
［第１の実施形態］
本実施形態では、本発明の位置姿勢計測装置及びその方法を、ジャイロセンサを装着した撮像装置の位置及び姿勢の計測に適用した場合について説明する。なお、本実施形態におけるジャイロセンサとは、前述したように３軸方向の角速度を計測するための複数のジャイロセンサと、３軸方向の加速度を計測するための複数の加速度センサの組合せによって構成される３軸姿勢センサであり、傾斜角成分はある程度信頼できる計測値を出力するが、方位角成分はドリフト誤差を含んでいる。そのため、本実施形態はジャイロセンサにのみ適用されるものではなく、姿勢の傾斜角成分を計測可能な任意の傾斜角センサについて適用できるものである。
【００２２】
図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、指標検出部１１０、姿勢入力部１２０、位置姿勢算出部１３０によって構成されており、撮像装置１４０に接続されている。また、ジャイロセンサ１５０は撮像装置１４０上に固定され、姿勢入力部１２０に接続されている。
【００２３】
図２は、本実施形態における座標系の関係を示している。基準座標系には、位置が既知の指標が配置または設定されているものとする。本実施形態に係る位置姿勢計測装置は、基準座標系に対する撮像装置の位置及び姿勢を計測する。本実施形態では、基準座標系のｙｗ軸を鉛直上方向にとる。
【００２４】
ジャイロセンサ１５０は、撮像装置の基準座標系に対する姿勢を出力する。基準座標系に対する撮像装置の姿勢を表す３×３回転変換行列をＲｗｃ、基準座標系に対する撮像装置の位置を表す３次元ベクトルをｔｗｃと表す。Ｒｗｃは、基準座標系のｚｗ軸まわりの回転変換
【００２５】
【数１】
【００２６】
軸まわりの回転変換
【００２７】
【数２】
【００２８】
軸まわりの回転変換
【００２９】
【数３】
【００３０】
を順に組み合わせることで表現可能であり、（式１）のように表すことができる
【００３１】
【数４】
【００３２】
ここで、
【００３３】
【数５】
【００３４】
はＲｗｃの傾斜角成分、
【００３５】
【数６】
【００３６】
はＲｗｃの方位角成分であるため、
【００３７】
【数７】
【００３８】
とおくと、
（式１）は（式２）のように記述できる（ここでｉはｉｎｃｌｉｎａｔｉｏｎ（傾斜角）、ａはａｚｉｍｕｔｈ（方位角）を表す）。
【００３９】
【数８】
【００４０】
よって、基準座標系から撮像装置の座標系（以下、撮像装置座標系）への座標変換は、
【００４１】
【数９】
【００４２】
及び撮像装置座標系における基準座標系の原点の位置ｔｃｗを用いて（式３）のように表せる。
【００４３】
【数１０】
【００４４】
但し、ｘｗ、ｘｃはそれぞれ基準座標系と撮像装置座標系における点の座標を表している。
【００４５】
ここで、
【００４６】
【数１１】
【００４７】
を（式４）のように表す。
【００４８】
【数１２】
【００４９】
また、方位角をθとすると、
【００５０】
【数１３】
【００５１】
は（式５）のように表される。
【００５２】
【数１４】
【００５３】
基準座標系における位置がｘｗ＝［ｘｗｙｗｚｗ］ｔである指標が画像上に投影される位置（ｕｘ，ｕｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘｃ＝［ｘｃｙｃｚｃ］ｔ、ｔｃｗ＝［ｔｘｔｙｔｚ］ｔとして（式６）のように表される。
【００５４】
【数１５】
【００５５】
次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて撮像装置の位置及び姿勢を算出する方法について説明する。図３は、本実施形態の処理手順を示すフローチャートである。
【００５６】
まずステップＳ１０１０において、指標検出部１１０は撮像装置１４０から画像を入力し、姿勢入力部１２０は撮影時のジャイロセンサの計測値を入力する。姿勢入力部１２０はジャイロセンサの計測値Ｒｗｃを傾斜角成分
【００５７】
【数１６】
【００５８】
と方位角成分
【００５９】
【数１７】
【００６０】
に分解し、（式４）からｒ１１，ｒ１２，ｒ１３，ｒ２１，ｒ２２，ｒ２３，ｒ３１，ｒ３２，ｒ３３を得て、位置姿勢算出部１３０へと出力する。次に、ステップＳ１０２０において、指標検出部１１０は撮影画像中から指標を検出して、位置姿勢算出部１３０へと出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を
【００６１】
【数１８】
【００６２】
とする。
【００６３】
次に、ステップＳ１０３０において、位置姿勢算出部１３０は、すべての検出した指標が同一水平面上（本実施形態における水平面はｙｗ＝定数で表される）にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の基準座標系における位置を
【００６４】
【数１９】
【００６５】
として、（式７）より検出指標のｙｗ座標の分散σ２を求める。
【００６６】
【数２０】
【００６７】
但し、
【００６８】
【数２１】
【００６９】
は指標のｙｗ座標の平均値であり、（式８）から求められる。
【００７０】
【数２２】
【００７１】
この分散σ２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ１０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ１０５０に進み、同一水平面上にないと判定された場合にはステップＳ１０４０に進む。
【００７２】
ステップＳ１０４０においては、位置姿勢算出部１３０は検出指標の数が３個以上であるか否かの判定を行う。ステップＳ１０４０において検出指標の数が３個以上であればステップＳ１０６０に進み、３個未満であれば終了する。また、ステップＳ１０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ１０５０において検出指標の数が２個以上であればステップＳ１０７０に進み、２個未満であれば終了する。
【００７３】
ステップＳ１０６０では、位置姿勢算出部１３０は手法（１Ａ）を用いて撮像装置の位置姿勢を算出し、これを出力する。ここで手法（１Ａ）について説明する。
【００７４】
（式６）の分母を払い、
【００７５】
【数２３】
【００７６】
とおくことによって、（式９）に示すα、β、ｔｘ、ｔｙ、ｔｚについての線形方程式が得られる。
【００７７】
【数２４】
【００７８】
入力した夫々の（ｎ点の）指標について、
【００７９】
【数２５】
【００８０】
とおいて、各指標について（式９）の線形方程式を立てると、（式１０）に示す連立方程式が得られる。
【００８１】
【数２６】
【００８２】
（式１０）を解くことによって、α、β、ｔｘ、ｔｙ、ｔｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。（式１０）を（式１１）に示すように簡略化して表す。
Ａ・ｘ＝ｂ （式１１）
ここでｘ＝［αβｔｘｔｙｔｚ］ｔである。また、Ａは行数（２×ｎ）、列数５の行列となり、ｂは（２×ｎ）次元ベクトルとなる。最小二乗法によってｘを（式１２）のように求める。
ｘ＝（Ａｔ・Ａ）−１・Ａｔ・ｂ （式１２）
なお、（式１２）では、Ａの一般化逆行列を（Ａｔ・Ａ）−１としているが，他の一般化逆行列を用いても本発明の本質が失われることがないのは言うまでもない。さらに、（式１２）より得られたα、βより、（式１３）によって方位角θを算出する。
【００８３】
【数２７】
【００８４】
得られたθと、センサ計測値の傾斜角成分
【００８５】
【数２８】
【００８６】
をもとに、基準座標系に対する撮像装置の姿勢Ｒｗｃを（式１４）によって算出する。
【００８７】
【数２９】
【００８８】
また、基準座標系における撮像装置の位置ｔｗｃを、（式１５）によって算出する。
ｔｗｃ＝−Ｒｗｃ・ｔｃｗ （式１５）
以上述べた方法で、ステップＳ１０６０において撮像装置の位置姿勢を算出し、これを出力する。
【００８９】
ステップＳ１０７０では、位置姿勢算出部１３０は手法（１Ｂ）を用いて撮像装置の位置姿勢を算出し、これを出力する。ここで手法（１Ｂ）について説明する。
【００９０】
指標の基準座標系におけるｙｗ座標をｙａｖｇとする。（式６）においてｙｗ＝０として、
【００９１】
【数３０】
【００９２】
とおいて分母を払い、さらに
【００９３】
【数３１】
【００９４】
とおくと（式１６）が得られる。
【００９５】
【数３２】
【００９６】
入力した夫々の（ｎ点の）指標について同様に
【００９７】
【数３３】
【００９８】
とおいて、各指標について（式１６）の線形方程式を立てると、（式１７）に示す連立方程式が得られる。
【００９９】
【数３４】
【０１００】
（式１７）を（式１２）に示した方法と同じように解くことによって、α’、β’、ｔ’ｘ、ｔ’ｙを得る。この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。
【０１０１】
（式１７）を解くことによって得られたα’、β’より、方位角θを（式１８）を用いて算出する。
【０１０２】
【数３５】
【０１０３】
得られたθと、センサ計測値の傾斜角成分
【０１０４】
【数３６】
【０１０５】
をもとに、基準座標系に対する撮像装置の姿勢Ｒｗｃを（式１４）によって算出する。また、ｔｃｗ＝［ｔｘｔｙｔｚ］ｔを（式１９）から求める。
【０１０６】
【数３７】
【０１０７】
また、基準座標系における撮像装置の位置ｔｗｃは、（式２０）によって算出する。
【０１０８】
【数３８】
【０１０９】
以上述べた方法で、ステップＳ１０７０において撮像装置の位置姿勢を算出し、これを出力する。
【０１１０】
以上述べたように、本実施形態では、撮像装置にジャイロセンサを装着した場合に、撮像装置の位置姿勢をセンサ計測値の傾斜角成分と画像情報から反復演算を必要としない方法で直接的に算出することができる。
【０１１１】
撮像装置にジャイロセンサを装着し、撮像装置によって撮影された画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に撮像装置の位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。実施形態では、１台のカメラの画像情報を利用する場合の位置姿勢計測方法について述べた。ある種のＡＲアプリケーションでは、ステレオ表示が有効であるため、ステレオカメラをＨＭＤ（Ｈｅａｄ Ｍｏｕｎｔｅｄ Ｄｉｓｐｌａｙ）に搭載している場合がある。このようなＨＭＤに姿勢センサが装着されている場合、ステレオペアのカメラに対して１つの傾斜角センサが装着されていると考えると、１台の場合と同様にそれぞれのカメラの傾斜角が一意に定まる。そのため、本実施形態で説明した位置姿勢計測方法において、ステレオカメラのそれぞれのカメラから得られる指標を利用するようにしてもよい。また、カメラ間の相対位置姿勢がわかっていれば、カメラは２台に限られることはなく、任意の台数のカメラを利用することもできる。
【０１１２】
［第２の実施形態］
第１の実施形態では、撮像装置にジャイロセンサを装着して、撮像装置の位置姿勢を求めた。第２の実施形態では、位置姿勢計測装置及びその方法を、基準座標系に固定された撮像装置によって撮影される移動物体の位置及び姿勢の計測に適用した場合について説明する。
【０１１３】
図４は、第２の実施形態における位置姿勢計測装置の構成を示している。同図に示したように、第２の実施形態における位置姿勢計測装置２００は、指標検出部２１０、姿勢入力部２２０、位置姿勢算出部２３０によって構成されている。基準座標系に固定された撮像装置２４０は指標検出部２１０に接続されている。また移動物体２６０上に装着されたジャイロセンサ２５０は姿勢入力部２２０に接続されている。第２の実施形態では、撮像装置２４０によって撮影された画像中で検出される移動物体上の指標と、移動物体上に装着されたジャイロセンサの計測値の傾斜角成分を使って、基準座標系に対する移動物体の位置姿勢を計測する。
【０１１４】
図５は、第２の実施形態における座標系の関係を示す図である。移動物体上には、移動物体座標系での位置が既知の指標が配置又は設置されているものとする。撮像装置は基準座標系に固定されており、撮像装置の基準座標系における位置を表す３次元ベクトルｔｗｃ、姿勢を表す３×３回転行列Ｒｗｃは既知とする。第２の実施形態では、基準座標系に対する移動物体の姿勢Ｒｗｏ及び位置ｔｗｏを求める。
【０１１５】
基準座標系に対する移動物体の姿勢を表す３×３回転行列Ｒｗｏは、（式２１）に示すように傾斜角成分
【０１１６】
【数３９】
【０１１７】
と方位角成分
【０１１８】
【数４０】
【０１１９】
に分解される。
【０１２０】
【数４１】
【０１２１】
移動物体座標系ｘｏから撮像装置座標系ｘｃへの座標変換は、
【０１２２】
【数４２】
【０１２３】
及び撮像装置座標系における移動物体の位置ｔｃｏを用いて（式２２）のように表せる。
【０１２４】
【数４３】
【０１２５】
ここで、
【０１２６】
【数４４】
【０１２７】
を（式２３）のように表す。
【０１２８】
【数４５】
【０１２９】
また、方位角をθとすると、
【０１３０】
【数４６】
【０１３１】
は（式２４）のように表される。
【０１３２】
【数４７】
【０１３３】
【数４８】
【０１３４】
とおいた場合に、移動物体座標がｘｏ＝［ｘｏｙｏｚｏ］ｔの指標が画像上に投影される位置（ｕｘ，ｕｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘｃ＝［ｘｃｙｃｚｃ］ｔ、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔとして（式２５）のように表される。
【０１３５】
【数４９】
【０１３６】
次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて移動物体の位置及び姿勢を算出する方法について説明する。図６は、第２の実施形態の処理手順を示すフローチャートである。
【０１３７】
まずステップＳ２０１０において、指標検出部２１０は撮像装置２４０から画像を入力する。姿勢入力部２２０は、撮影時の移動物体上のジャイロセンサの計測値Ｒｗｏを入力し、傾斜角成分
【０１３８】
【数５０】
【０１３９】
と方位角成分
【０１４０】
【数５１】
【０１４１】
に分解し、（式２３）から既知の値であるＲ１１，Ｒ１２，Ｒ１３，Ｒ２１，Ｒ２２，Ｒ２３，Ｒ３１，Ｒ３２，Ｒ３３を得て、位置姿勢算出部２３０へと出力する。次に、ステップＳ２０２０において、指標検出部２１０は撮影画像中から移動物体上の指標を検出して、位置姿勢算出部２３０へ出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を
【０１４２】
【数５２】
【０１４３】
とする。次に、ステップＳ２０３０において、位置姿勢算出部２３０は、すべての検出した指標が同一水平面上にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の移動物体座標系での位置を
【０１４４】
【数５３】
【０１４５】
として、（式２６）より検出指標のｙｗ座標の分散σ２を求める。
【０１４６】
【数５４】
【０１４７】
但し、
【０１４８】
【数５５】
【０１４９】
は指標のｙｗ座標の平均値であり、（式２７）から求められる。
【０１５０】
【数５６】
【０１５１】
この分散σ２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ２０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ２０５０に進み、同一水平面上にないと判定された場合にはステップＳ２０４０に進む。ステップＳ２０４０においては、位置姿勢算出部２３０は検出指標の数が３個以上であるか否かの判定を行う。ステップＳ２０４０において検出指標の数が３個以上であればステップＳ２０６０に進み、３個未満であれば終了する。また、ステップＳ２０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ２０５０において検出指標の数が２個以上であればステップＳ２０７０に進み、２個未満であれば終了する。
【０１５２】
ステップＳ２０６０では、位置姿勢算出部２３０は手法（２Ａ）を用いて移動物体の位置姿勢を算出し、これを出力する。ここで手法（２Ａ）について説明する。（式２５）の分母を払い、
【０１５３】
【数５７】
【０１５４】
とおいて得られるα、β、ｔｏｘ、ｔｏｙ、ｔｏｚについての線形方程式を検出された各指標について立てることにより、（式２８）に示す連立方程式が得られる。
【０１５５】
【数５８】
【０１５６】
（式２８）を解くことにより、α、β、ｔｏｘ、ｔｏｙ、ｔｏｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。α、βより、（式１３）を用いて方位角θを算出する。得られたθと、センサ計測値の傾斜角成分
【０１５７】
【数５９】
【０１５８】
をもとに、基準座標系に対する移動物体の姿勢Ｒｗｏを（式２９）によって算出する。
【０１５９】
【数６０】
【０１６０】
また移動物体の基準座標系における位置ｔｗｏを（式３０）によって算出する。
ｔｗｏ＝Ｒｗｃ・ｔｃｏ＋ｔｗｃ （式３０）
以上述べた方法で、ステップＳ２０６０において移動物体の位置姿勢を算出し、これを出力する。
【０１６１】
ステップＳ２０７０では、位置姿勢算出部２３０は手法（２Ｂ）を用いて移動物体の位置姿勢を算出し、これを出力する。ここで手法（２Ｂ）について説明する。
【０１６２】
指標の基準座標系におけるｙｗ座標をｙａｖｇとする。（式２５）において
【０１６３】
【数６１】
【０１６４】
として、
【０１６５】
【数６２】
【０１６６】
とおいて分母を払い、
【０１６７】
【数６３】
【０１６８】
とおいて得られるα’、β’、ｔ’ｏｘ、ｔ’ｏｙについての線形方程式を検出された各指標について立てることにより、（式３１）に示す連立方程式が得られる。
【０１６９】
【数６４】
【０１７０】
この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。（式３１）を解くことによって得られたα’、β’から、方位角θを（式１８）によって算出する。得られたθと、センサ計測値の傾斜角成分
【０１７１】
【数６５】
【０１７２】
をもとに、基準座標系に対する移動物体の姿勢Ｒｗｏを（式２９）によって算出する。また、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔを（式３２）によって算出する。
【０１７３】
【数６６】
【０１７４】
さらに移動物体の基準座標系における位置ｔｗｏを（式３３）によって算出する。
【０１７５】
【数６７】
【０１７６】
以上述べた方法で、ステップＳ２０７０において移動物体の位置姿勢を算出し、これを出力する。
【０１７７】
以上述べたように、本実施形態によれば、移動物体にジャイロセンサを装着した場合に、移動物体の位置姿勢をセンサ計測値の傾斜角成分と基準座標系に固定された撮像装置の画像情報から反復演算を必要としない方法で直接的に算出することができる。
【０１７８】
本実施形態によれば、移動物体にジャイロセンサを装着し、シーンに固定した撮像装置によって撮影された移動物体の画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に移動物体の位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。
【０１７９】
本実施形態では、１台のカメラの画像情報を利用する場合の位置姿勢計測方法について述べた。本実施形態でのカメラは基準座標系での位置姿勢が既知な客観視点カメラである。客観視点カメラの基準座標系における位置をｔｗｃ（＝［ｘｗｃｙｗｃｚｗｃ］ｔ）、姿勢を表す３×３回転行列をＲｗｃとすると、移動物体座標系ｘｏから撮像装置座標系ｘｃへの座標変換は、移動物体の基準座標系における位置ｔｗｏ（＝［ｘｗｏｙｗｏｚｗｏ］ｔ）を用いて、（式３４）で表せる。
【０１８０】
【数６８】
【０１８１】
【数６９】
【０１８２】
、（式２３）、（式２４）、（式３４）より点
【０１８３】
【数７０】
【０１８４】
が画像上に投影される位置（ｕｘ，ｕｙ）は（式３５）のように表される。
【０１８５】
【数７１】
【０１８６】
式（３５）は、ｔｗｏとα、βを未知パラメータとして、画像上で検出される各指標について成り立つ観測方程式である。この観測方程式を複数のカメラが撮影する画像上の指標について立てることにより、本実施形態で述べたのと同様な方法で、ｔｗｏとα、βを求めることができる。すなわち、本実施形態は１台のカメラに限定されるものではなく、基準座標系における位置姿勢が既知な複数のカメラの画像情報を利用することができる。
【０１８７】
［第３の実施形態］
第２の実施形態では、撮像装置を基準座標系に固定し、移動物体にジャイロセンサを装着したが、第３の実施形態では、位置姿勢計測装置及びその方法を、撮像装置、移動物体の両方にジャイロセンサを装着した場合の撮像装置に対する移動物体の位置及び姿勢の計測に用いた場合について説明する。
【０１８８】
図７は、第３の実施形態における位置姿勢計測装置の構成を示している。同図に示したように、第３の実施形態における位置姿勢計測装置３００は、指標検出部３１０、姿勢入力部３２０、位置姿勢算出部３３０によって構成されている。撮像装置３４０は指標検出部３１０に接続されている。また、移動物体３６０にはジャイロセンサ３５５が、撮像装置３４０にはジャイロセンサ３５０が装着されており、ジャイロセンサ３５０、３５５は姿勢入力部３２０に接続されている。第３の実施形態では、撮像装置３４０によって撮影された画像中で検出される移動物体の指標と、移動物体及び撮像装置に装着された夫々のジャイロセンサの計測値の傾斜角成分を使って、撮像装置に対する移動物体の位置姿勢を求める。図８は第３の実施形態における座標系の関係を表す図である。第３の実施形態では、撮像装置座標系に対する移動物体の姿勢Ｒｃｏ及び位置ｔｃｏを求める。基準座標系における移動物体の姿勢を表す３×３回転行列をＲｗｏ、撮像装置の姿勢を表す３×３回転行列をＲｗｃとする。Ｒｗｏは（式３６）に示すように、傾斜角成分
【０１８９】
【数７２】
【０１９０】
と方位角成分
【０１９１】
【数７３】
【０１９２】
に分解される。
【０１９３】
【数７４】
【０１９４】
またＲｗｃは（式３７）に示すように、傾斜角成分
【０１９５】
【数７５】
【０１９６】
と方位角成分
【０１９７】
【数７６】
【０１９８】
に分解される。
【０１９９】
【数７７】
【０２００】
移動物体座標系ｘｏから撮像装置座標系ｘｃへの変換は、
【０２０１】
【数７８】
【０２０２】
及び撮像装置座標系における移動物体の位置ｔｃｏを用いて（式３８）のように表せる。
【０２０３】
【数７９】
【０２０４】
【数８０】
【０２０５】
は方位角を表しているので、
【０２０６】
【数８１】
【０２０７】
としてまとめる。
【０２０８】
ここで、
【０２０９】
【数８２】
【０２１０】
、Ｒａをそれぞれ（式３９）、（式４０）のように表す。θは方位角である。
【０２１１】
【数８３】
【０２１２】
【数８４】
【０２１３】
【数８５】
【０２１４】
とおいた場合に、移動物体座標がｘｏ＝［ｘｏｙｏｚｏ］ｔの指標が画像上に投影される位置（ｕｘ，ｕｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘｃ＝［ｘｃｙｃｚｃ］ｔ、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔとして（式４１）のように表される。
【０２１５】
【数８６】
【０２１６】
次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて、撮像装置に対する移動物体の位置姿勢を算出する方法について説明する。図９は、第３の実施形態の処理手順を示すフローチャートである。
【０２１７】
まずステップＳ３０１０において、指標検出部３１０は、撮像装置から画像を入力する。また、姿勢入力部３２０は、撮影と同時に移動物体に装着されたジャイロセンサの計測値Ｒｗｏ及び撮像装置に装着されたジャイロセンサの計測値Ｒｗｃを入力し、それぞれ公知の手法を使って傾斜角成分
【０２１８】
【数８７】
【０２１９】
と方位角成分
【０２２０】
【数８８】
【０２２１】
に分解する。また、（式３９）から既知であるｒ１１，ｒ１２，ｒ１３，ｒ２１，ｒ２２，ｒ２３，ｒ３１，ｒ３２，ｒ３３を得て、位置姿勢算出部３３０に出力する。次に、ステップＳ３０２０において、指標検出部３１０は、撮影画像中から移動物体上の指標を検出して位置姿勢算出部３３０に出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を
【０２２２】
【数８９】
【０２２３】
とする。次に、ステップＳ３０３０において、位置姿勢算出部３３０は、すべての検出した指標が同一水平面上にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の移動物体座標系での位置を
【０２２４】
【数９０】
【０２２５】
として、（式４２）より検出指標のｙｗ座標の分散σ２を求める。
【０２２６】
【数９１】
【０２２７】
但し、
【０２２８】
【数９２】
【０２２９】
は指標のｙｗ座標の平均値であり、（式４３）から求められる。
【０２３０】
【数９３】
【０２３１】
この分散σ２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ３０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ３０５０に進み、同一水平面上にないと判定された場合にはステップＳ３０４０に進む。
【０２３２】
ステップＳ３０４０においては、位置姿勢算出部３３０は、検出指標の数が３個以上であるか否かの判定を行う。ステップＳ３０４０において検出指標の数が３個以上であればステップＳ３０６０に進み、３個未満であれば終了する。また、ステップＳ３０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ３０５０において検出指標の数が２個以上であればステップＳ３０７０に進み、２個未満であれば終了する。
【０２３３】
ステップＳ３０６０では、位置姿勢算出部３３０は、手法（３Ａ）を用いて撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。ここで手法（３Ａ）について説明する。（式４１）の分母を払い、
【０２３４】
【数９４】
【０２３５】
とおいて得られるα、β、ｔｏｘ、ｔｏｙ、ｔｏｚについての線形方程式を検出された各指標について立てることにより、（式４４）に示す連立方程式が得られる。
【０２３６】
【数９５】
【０２３７】
（式４４）を解くことにより、α、β、ｔｏｘ、ｔｏｙ、ｔｏｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。α、βより、（式１３）を用いて方位角θを算出する。移動物体の撮像装置に対する姿勢Ｒｃｏは（式４５）によって算出する。
【０２３８】
【数９６】
【０２３９】
以上述べた方法で、ステップＳ３０６０において撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。
【０２４０】
ステップＳ３０７０では、位置姿勢算出部３３０は、手法（３Ｂ）を用いて撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。ここで手法（３Ｂ）について説明する。
【０２４１】
指標の基準座標系におけるｙｗ座標をｙａｖｇとする。（式４１）において
【０２４２】
【数９７】
【０２４３】
として、
【０２４４】
【数９８】
【０２４５】
とおいて分母を払い、
【０２４６】
【数９９】
【０２４７】
とおいて得られるα’、β’、ｔ’ｏｘ、ｔ’ｏｙについての線形方程式を検出された各指標について立てることにより、（式４６）に示す連立方程式が得られる。
【０２４８】
【数１００】
【０２４９】
この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。（式４６）を解くことによって得られたα’、β’より、方位角θは（式１８）によって算出する。移動物体の撮像装置に対する姿勢Ｒｃｏは（式４５）によって算出する。また、ｔｃｏ＝［ｔｏｘｔｏｙｔｏｚ］ｔは（式３２）から算出する。
【０２５０】
以上述べた方法で、ステップＳ３０７０において撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。
【０２５１】
以上述べたように、本実施形態では、移動物体と撮像装置にジャイロセンサを装着した場合に、撮像装置に対する移動物体の位置姿勢（すなわち、撮像装置と移動物体の相対的な位置と姿勢）をセンサ計測値の傾斜角成分と撮像装置の画像情報から反復演算を必要としないで直接的に算出することができる。
【０２５２】
本実施形態によれば、撮像装置と移動物体の両方にジャイロセンサを装着し、撮像装置によって撮影された移動物体の画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に撮像物体と移動物体の相対的な位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。
【０２５３】
［変形例１］
以上述べた実施形態では、ジャイロセンサを姿勢センサとして用いていたが、これに限るものではなく、姿勢の傾斜角成分を計測可能なセンサであればどのような傾斜角センサを用いてもよい。また、傾斜角は鉛直軸に対する傾斜角に限定されるものではなく、任意の軸に対する傾斜角であって、傾斜角センサは該軸に対する傾斜角を計測するものであればどのような傾斜角センサを用いてもよい。
【０２５４】
［変形例２］
以上述べた実施形態では、画像上での指標の画像座標とその三次元位置及び姿勢センサから得られる姿勢の傾斜角成分をもとに、反復演算をしないで撮像装置の位置姿勢（第１の実施形態）、移動物体の位置姿勢（第２の実施形態）、撮像装置と物体との相対位置姿勢（第３の実施形態）を求めた。このようにして得られる位置姿勢を初期値として、別の手段によって位置姿勢を最適化してもよい。例えば、非特許文献２の手法を用いて、指標の画像座標とその三次元位置に基づき、６自由度の位置姿勢すべてを最適化してもよいし、非特許文献３の手法を用いて、指標の画像座標とその三次元位置および姿勢センサの計測値に基づき位置と姿勢の方位角成分を最適化してもよい。その際に反復演算を行うことがあっても、構わない。上記実施形態１〜３の方法により、例えば反復演算を用いる最適化の初期値にすることにより、初期値が不適切である（例えば前フレームで位置姿勢を求めることができない時）ことによる位置姿勢算出精度の低下を抑制することができる。
【０２５５】
［変形例３］
以上述べた実施形態では、行列演算によって撮像装置と物体との相対位置姿勢を求めていたが、上記実施形態の本質は連立方程式を反復演算なしに直接的に解くことにあり、必ずしも行列演算によって求める必要がないのは言うまでもない。
【０２５６】
［変形例４］
また、以上述べた実施形態では、（式１０）に表されるように、すべての検出指標の信頼度を同じ信頼度とみなして連立方程式を解いているが、検出指標ごとに信頼度を変えて、重みを付与して連立方程式を解いてもよい。
【０２５７】
［変形例５］
さらに、以上述べた実施形態では、指標が同一水平面上にあるかどうかを判定して計算手法を切り替えていたが、指標が同一水平面上にあるかどうかが予め分かっている場合には、その判定ステップを省略して直接（式１０）または（式１７）（もしくは（式２８）または（式３１）、（式４４）または（式４６））を解いてもよい。
【０２５８】
（他の実施形態）
前述した実施の形態の機能を実現する様に各種のデバイスを動作させる様に該各種デバイスと接続された装置あるいはシステム内のコンピュータに、前記実施の形態の機能を実現するためのソフトウエアのプログラムコードを供給し、そのシステムあるいは装置のコンピュータ（ＣＰＵあるいはＭＰＵ）を格納されたプログラムに従って前記各種デバイスを動作させることによって実施したものも本発明の範疇に含まれる。
【０２５９】
この場合、前記ソフトウエアのプログラムコード自体が前述した実施の形態の機能を実現することになり、そのプログラムコード自体、及びそのプログラムコードをコンピュータに供給するための手段、例えばかかるプログラムコードを格納した記憶媒体は本発明を構成する。
【０２６０】
かかるプログラムコードを格納する記憶媒体としては例えばフロッピー（登録商標）ディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることが出来る。
【０２６１】
またコンピュータが供給されたプログラムコードを実行することにより、前述の実施形態の機能が実現されるだけではなく、そのプログラムコードがコンピュータにおいて稼働しているＯＳ（オペレーティングシステム）、あるいは他のアプリケーションソフト等と共同して前述の実施形態の機能が実現される場合にもかかるプログラムコードは本発明の実施形態に含まれることは言うまでもない。
【０２６２】
更に供給されたプログラムコードが、コンピュータの機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに格納された後そのプログラムコードの指示に基づいてその機能拡張ボードや機能格納ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も本発明に含まれることは言うまでもない。
【図面の簡単な説明】
【０２６３】
【図１】第一の実施形態の構成を示す図である。
【図２】第一の実施形態における座標系の関係を示す図である。
【図３】第一の実施形態の処理手順を示すフローチャートである。
【図４】第二の実施形態の構成を示す図である。
【図５】第二の実施形態における座標系の関係を示す図である。
【図６】第二の実施形態の処理手順を示すフローチャートである。
【図７】第三の実施形態の構成を示す図である。
【図８】第三の実施形態における座標系の関係を示す図である。
【図９】第三の実施形態の処理手順を示すフローチャートである。
【特許請求の範囲】
【請求項１】
物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、
前記撮像装置から撮影画像を入力する画像入力工程と、
前記撮影画像から前記物体上の指標を検出する指標検出工程と、
前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする位置姿勢計測方法。
【請求項２】
物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力工程と、
撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力工程と、
前記撮像装置から撮影画像を入力する画像入力工程と、
前記撮影画像から前記物体上の指標を検出する指標検出工程と、
前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする位置姿勢計測方法。
【請求項３】
前記位置姿勢算出工程は、複数の位置姿勢算出手法を有し、前記複数の位置姿勢算出方法から前記検出された指標に応じた位置姿勢算出方法を選択することを特徴とする請求項１または２記載の位置姿勢計測方法。
【請求項４】
３個以上の検出された指標に基づき、前記位置と姿勢を算出することを特徴とする請求項１乃至３のいずれかに記載の位置姿勢計測方法。
【請求項５】
前記指標検出工程によって検出された指標が所定の軸に垂直な平面上にあることを制約条件として用いて、前記位置と姿勢を算出することを特徴とする請求項１乃至４のいずれかに記載の位置姿勢計測方法。
【請求項６】
前記検出された各指標に対して線形方程式を立てることにより得られる連立方程式を解くことにより、前記位置と姿勢を算出することを特徴とする請求項１乃至５のいずれかに記載の位置姿勢計測方法。
【請求項７】
前記算出された位置と姿勢を初期値として、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、前記位置と姿勢を最適化することを特徴とする請求項１乃至６のいずれかに記載の位置姿勢計測方法。
【請求項８】
請求項１乃至７のいずれかに記載の位置姿勢計測方法をコンピュータにて実現するためのプログラム。
【請求項９】
物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力手段と、
前記撮像装置から撮影画像を入力する画像入力手段と、
前記撮影画像から前記物体上の指標を検出する指標検出手段と、
前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出手段とを備えることを特徴とする位置姿勢計測装置。
【請求項１０】
物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力手段と、
撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力手段と、
前記撮像装置から撮影画像を入力する画像入力手段と、
前記撮影画像から前記物体上の指標を検出する指標検出手段と、
前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出手段とを備えることを特徴とする位置姿勢計測装置。
【請求項１】
物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、
前記撮像装置から撮影画像を入力する画像入力工程と、
前記撮影画像から前記物体上の指標を検出する指標検出工程と、
前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする位置姿勢計測方法。
【請求項２】
物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力工程と、
撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力工程と、
前記撮像装置から撮影画像を入力する画像入力工程と、
前記撮影画像から前記物体上の指標を検出する指標検出工程と、
前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする位置姿勢計測方法。
【請求項３】
前記位置姿勢算出工程は、複数の位置姿勢算出手法を有し、前記複数の位置姿勢算出方法から前記検出された指標に応じた位置姿勢算出方法を選択することを特徴とする請求項１または２記載の位置姿勢計測方法。
【請求項４】
３個以上の検出された指標に基づき、前記位置と姿勢を算出することを特徴とする請求項１乃至３のいずれかに記載の位置姿勢計測方法。
【請求項５】
前記指標検出工程によって検出された指標が所定の軸に垂直な平面上にあることを制約条件として用いて、前記位置と姿勢を算出することを特徴とする請求項１乃至４のいずれかに記載の位置姿勢計測方法。
【請求項６】
前記検出された各指標に対して線形方程式を立てることにより得られる連立方程式を解くことにより、前記位置と姿勢を算出することを特徴とする請求項１乃至５のいずれかに記載の位置姿勢計測方法。
【請求項７】
前記算出された位置と姿勢を初期値として、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、前記位置と姿勢を最適化することを特徴とする請求項１乃至６のいずれかに記載の位置姿勢計測方法。
【請求項８】
請求項１乃至７のいずれかに記載の位置姿勢計測方法をコンピュータにて実現するためのプログラム。
【請求項９】
物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力手段と、
前記撮像装置から撮影画像を入力する画像入力手段と、
前記撮影画像から前記物体上の指標を検出する指標検出手段と、
前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出手段とを備えることを特徴とする位置姿勢計測装置。
【請求項１０】
物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力手段と、
撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力手段と、
前記撮像装置から撮影画像を入力する画像入力手段と、
前記撮影画像から前記物体上の指標を検出する指標検出手段と、
前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出手段とを備えることを特徴とする位置姿勢計測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００６−２９２４１７（Ｐ２００６−２９２４１７Ａ）
【公開日】平成１８年１０月２６日（２００６．１０．２６）
【国際特許分類】
【出願番号】特願２００５−１０９８３４（Ｐ２００５−１０９８３４）
【出願日】平成１７年４月６日（２００５．４．６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成１８年１０月２６日（２００６．１０．２６）
【国際特許分類】
【出願日】平成１７年４月６日（２００５．４．６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]