(57)【要約】【課題】指標が２点しか検出されない場合であっても、撮像装置の位置及び姿勢のうちの少なくとも何れか一方を算出する。【解決手段】画像入力部１０１は、撮像装置１００により撮像された画像データを入力する。指標検出部１０２０は、上記画像データ上における指標の位置を検出する。３次元位置算出部１０５０は、第１の座標系における撮像装置１００の傾斜角、上記画像データ上における指標の位置、及び、第１の座標系における指標の位置に基づいて、第２の座標系における指標の位置を算出する。位置姿勢算出部１０６０は、第１の座標系における撮像装置１００の傾斜角、及び、第２の座標系における指標の位置に基づいて、第１の座標系における撮像装置１００の位置及び姿勢のうちの少なくとも何れか一方を算出する。【選択図】図１
2014215821.tif 000002
【特許請求の範囲】
【請求項１】
撮像手段により撮像された画像データを入力する入力手段と、
前記画像データ上における指標の位置を検出する検出手段と、
第１の座標系における前記撮像手段の傾斜角、前記画像データ上における前記指標の位置、及び、前記第１の座標系における前記指標の位置に基づいて、第２の座標系における前記指標の位置を算出する第１の算出手段と、
前記第１の座標系における前記撮像手段の傾斜角、及び、前記第２の座標系における前記指標の位置に基づいて、前記第１の座標系における前記撮像手段の位置及び姿勢のうちの少なくとも何れか一方を算出する第２の算出手段とを有することを特徴とする情報処理装置。
【請求項２】
前記第１の算出手段は、前記第１の座標系における前記撮像手段の傾斜角、前記画像データ上における前記指標の位置、及び、前記第１の座標系における前記指標の位置に基づいて、前記撮像手段と前記指標との距離を算出し、前記距離に基づいて、前記第２の座標系における前記指標の位置を算出することを特徴とする請求項１に記載の情報処理装置。
【請求項３】
前記検出手段は、前記画像データ上における２点の前記指標の位置を検出し、
前記第１の算出手段は、前記第１の座標系における前記撮像手段の傾斜角、前記画像データ上における２点の前記指標の位置、及び、前記第１の座標系における２点の前記指標の位置に基づいて、前記第２の座標系における２点の前記指標の位置を算出し、
前記第２の算出手段は、前記第１の座標系における前記撮像手段の傾斜角、及び、前記第２の座標系における２点の前記指標の位置に基づいて、前記第１の座標系における前記撮像手段の位置及び姿勢のうちの少なくとも何れか一方を算出することを特徴とする請求項１又は２に記載の情報処理装置。
【請求項４】
前記検出手段による検出結果に基づいて、前記検出手段における前記指標の位置の検出方法を切り替える切り替え手段を更に有することを特徴とする請求項１乃至３の何れか１項に記載の情報処理装置。
【請求項５】
前記切り替え手段は、前記検出手段により人為的な指標が検出できなかった場合に、特徴点を指標として検出するように、前記検出手段における前記指標の位置の検出方法を切り替えることを特徴とする請求項４に記載の情報処理装置。
【請求項６】
情報処理装置によって実行される情報処理方法であって、
撮像手段により撮像された画像データを入力する入力ステップと、
前記画像データ上における指標の位置を検出する検出ステップと、
第１の座標系における前記撮像手段の傾斜角、前記画像データ上における前記指標の位置、及び、前記第１の座標系における前記指標の位置に基づいて、第２の座標系における前記指標の位置を算出する第１の算出ステップと、
前記第１の座標系における前記撮像手段の傾斜角、及び、前記第２の座標系における前記指標の位置に基づいて、前記第１の座標系における前記撮像手段の位置及び姿勢のうちの少なくとも何れか一方を算出する第２の算出ステップとを有することを特徴とする情報処理方法。
【請求項７】
撮像手段により撮像された画像データを入力する入力ステップと、
前記画像データ上における指標の位置を検出する検出ステップと、
第１の座標系における前記撮像手段の傾斜角、前記画像データ上における前記指標の位置、及び、前記第１の座標系における前記指標の位置に基づいて、第２の座標系における前記指標の位置を算出する第１の算出ステップと、
前記第１の座標系における前記撮像手段の傾斜角、及び、前記第２の座標系における前記指標の位置に基づいて、前記第１の座標系における前記撮像手段の位置及び姿勢のうちの少なくとも何れか一方を算出する第２の算出ステップとをコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像装置の位置及び姿勢のうちの少なくとも一方を算出する技術に関するものである。
【背景技術】
【０００２】
近年、複合現実感（Mixed Reality；ＭＲ）技術の研究が盛んである。ＭＲ技術は、現実空間とコンピュータによって生成される仮想空間とを繋ぎ目なく融合する技術であり、現実空間に仮想空間を重ね合わせて表示する技術が注目を集めている。
【０００３】
ＭＲ技術は、空き地に仮想のビルを重畳表示する建築シミュレーション、組み立て作業時に作業手順や配線の様子を重畳表示する組み立て支援、患者の体表面に体内の様子を重畳表示する手術支援等、様々な分野への応用が期待される。
【０００４】
仮想物体が現実空間に実在するように利用者が感じるためには、仮想物体と現実空間との間に幾何学的な整合性がとれている必要がある。ビデオシースルー型のＨＭＤ（Head Mounted Display）を利用するようなＭＲを実現するためには、カメラから現実空間を撮影した画像データが入力される毎に、画像データ撮影時のカメラの現実空間における位置及び姿勢が計測される。そして、このカメラの位置及び姿勢と焦点距離等のカメラの固有パラメータとに基づいてＣＧが描画され、係るＣＧを現実空間の画像データ上に重畳するという処理が一般的に行われる。そのため、ＭＲにおける現実空間と仮想空間との位置合わせの問題は、カメラの現実空間における位置及び姿勢を計測する問題となる。
【０００５】
ＭＲにおける位置合わせ手法として、様々な方法がこれまでに提案されている（非特許文献１参照）。例えば、画像データ上の指標の投影像とその指標の３次元位置との対応に基づいて撮像装置の位置及び姿勢を求める方法は、写真測量の分野において古くから提案されている（非特許文献２参照）。また、固有の２次元パターンを内部に有する正方形形状のマーカ（正方形マーカ）を指標として、その投影像に基づいて撮像装置の位置及び姿勢を求める方法が提案されている（非特許文献３参照）。ここでは、指標の画像データ上の２次元座標と３次元位置との対応（２Ｄ−３Ｄ対応）が利用される。即ち、指標の３次元位置と撮像装置の位置及び姿勢とから計算された指標の投影像の位置と、画像データ上の指標の投影像の位置との誤差が最小になるように反復演算によって撮像装置の位置及び姿勢を最適化することが行われている。
【０００６】
また、美的観点や物理的な制約から、正方形マーカのような人為的な指標を配置できない場合は、ポスターや窓の角のようなシーンに配置されている物体の特徴的な点を画像特徴として検出する検出器を利用して、撮像装置の位置及び姿勢を求めることができる。これらは、画像データ上で検出される特徴的な点は、特徴点（feature point）、又は、関心点（interest point）等と呼ばれる。
【０００７】
検出器としては、例えば、Ｈａｒｒｉｓ検出器が知られている（非特許文献４参照）。Ｈａｒｒｉｓ検出器は、ある点の近傍で画素値の濃淡変化の激しい点に高い値を与えることにより、角点を検出する。また、ＦＡＳＴ（Features from Accelerated Segment Test）もよく知られている（非特許文献５参照）。ＦＡＳＴは、注目画素を中心としたある固定半径を持つ円周上の画素の輝度値を、注目画素の輝度値と比較することにより、特徴点を検出する。例えば、円周上の画素を１６個とした場合、注目画素と比較して、１２個連続して注目画素よりも明るい又は暗い場合に角点として検出される。そして、円周上の１６個の画素の輝度値をベクトルとして保持し、ベクトル同士のＳＳＤ（Sum of Squared Differences）等を計算することにより相違度が求められ、特徴点の対応付けが行われる。検出器によって多数の特徴点が検出される際に、検出された特徴点の多くに検出誤差等によるノイズが含まれる場合がある。それらのノイズの影響を受けると、撮像装置の位置及び姿勢を正しく算出することができない。そこで、ロバスト推定手法を適用することによりノイズの影響が排除され、正しく検出された特徴点の２Ｄ−３Ｄ対応のみを用いて計算される。
【０００８】
例えば、ロバスト推定手法の一つとしてＲＡＮＳＡＣ（Random Sample Consensus）が利用されている（非特許文献６参照）。ＲＡＮＳＡＣでは、与えられた指標の画像データ上の２次元座標と３次元位置との対応（２Ｄ−３Ｄ対応）の中から、推定したいパラメータ（ここでは、位置及び姿勢）を算出するのに最小限の指標の２Ｄ−３Ｄ対応がランダムに選択される。そして、ランダムに選択された指標の２Ｄ−３Ｄ対応に基づいてパラメータを算出することが多数回行われ、算出されたパラメータの中で最も妥当なパラメータを解として選択することにより、アウトライヤの影響が排除される。このように、ＲＡＮＳＡＣは、指標の２Ｄ−３Ｄ対応をランダムにサンプリングして多数回計算を行うため、一般的に処理負荷が高く、計算に時間がかかる。
【０００９】
一方、撮像装置によって撮影された画像データだけを用いるのではなく、ジャイロセンサを撮像装置に装着し、画像データとジャイロセンサの出力情報とを併用して撮像装置の位置及び姿勢を求めることも行われている。画像データとジャイロセンサの出力情報とを併用することにより、画像データのみを用いるよりも、ロバスト性を向上させることができる。ここでジャイロセンサとは、３軸方向の角速度を計測するための複数の角速度センサと、３軸方向の加速度を計測するための複数の加速度センサとの組合せによって構成される３軸姿勢センサのことを指す。ジャイロセンサには、一般的にドリフト誤差があるため、時間経過に伴って徐々に方位方向の計測値に誤差が生じてしまう。そのため、ジャイロセンサから出力される姿勢計測値をそのまま撮像装置の姿勢とした場合には、間違った撮像装置の位置が得られることがあった。
【００１０】
そこで、非特許文献７では、ジャイロセンサから出力される姿勢の計測値のうち、傾斜角成分をそのまま撮像装置の姿勢の傾斜角成分とする技術が開示されている。さらに、上述した指標の３次元位置と撮像装置の位置及び姿勢とから計算された指標の投影像の位置と、画像データ上の指標の投影像の位置との誤差が最小化されるように、撮像装置の位置及び姿勢の方位角成分を反復演算によって最適化することが行われている。このような反復演算により、撮像装置の位置及び姿勢の方位角成分を算出する場合には、初期値が必要となる。前フレームの結果を初期値として用いる場合、前フレームで撮像装置の位置及び姿勢の方位角成分が算出されなければ（例えば、前フレームで指標が撮影されなかった場合）、初期値を得ることができない。また、撮影画像データだけから算出された撮像装置の位置及び姿勢の方位角成分を初期値として用いる場合には、初期値そのものが誤ったものになる可能性があるため、その後の反復演算でも最適化できないことがあった。
【００１１】
一方、非特許文献８に開示される技術では、ジャイロセンサから出力される傾斜角計測値を撮像装置の傾斜角として用い、当該撮像装置の傾斜角、及び、指標の画像データ上の２次元座標と３次元位置との対応に基づいて、撮像装置の位置及び姿勢の方位角を、反復計算を解くことなく直接的に算出している。
【先行技術文献】
【非特許文献】
【００１２】
【非特許文献１】佐藤, 内山, 田村, “複合現実感における位置合わせ手法”, 日本ＶＲ学会論文誌，Ｖｏｌ．８，ｎｏ．２，ｐｐ．１７１−１８０，２００３．
【非特許文献２】社団法人 日本写真測量学会，“解析写真測量 改訂版”，１９８９．
【非特許文献３】加藤，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，浅野，橘，“マーカ追跡に基づく拡張現実感システムとそのキャリブレーション”，日本バーチャルリアリティ学会論文誌，Ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１７，１９９９．
【非特許文献４】ｃ．Ｈａｒｒｉｓ ａｎｄ ｍ．Ｓｔｅｐｈｅｎｓ、“Ａ ｃｏｍｂｉｎｅｄ ｃｏｒｎｅｒ ａｎｄ ｅｄｇｅ ｄｅｔｅｃｔｏｒ”、Ｐｒｏｃ．４ｔｈ Ａｌｖｅｙ Ｖｉｓｉｏｎ Ｃｏｎｆ、ｐｐ．１４７−１５１，１９８８．
【非特許文献５】Ｅ．Ｒｏｓｔｅｎ，Ｔ．Ｄｒｕｍｍｏｎｄ，“Ｆｕｓｉｎｇ ｐｏｉｎｔｓ ａｎｄ ｌｉｎｅｓ ｆｏｒ ｈｉｇｈ ｐｅｒｆｏｒｍａｎｃｅ ｔｒａｃｋｉｎｇ” ，Ｉｎ：１０ｔｈ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ．Ｖｏｌ．２，ｐｐ．１５０８−１５１５，２００５．
【非特許文献６】Ｄ．Ｎｉｓｔｅｒ，“Ｐｒｅｅｍｐｔｉｖｅ ＲＡＮＳＡＣ ｆｏｒ ｌｉｖｅ ｓｔｒｕｃｔｕｒｅ ａｎｄ Ｍｏｔｉｏｎ ｅｓｔｉｍａｔｉｏｎ，”Ｐｒｏｃ． ９ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ（ＩＣＣＶ‘０３），ｐｐ．１９９−２０６，２００３．
【非特許文献７】Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，ａｎｄ Ｈ．Ｙａｍａｍｏｔｏ：“Ａ ｈｅａｄ ｔｒａｃｋｉｎｇ ｍｅｔｈｏｄ ｕｓｉｎｇ ｂｉｒｄ’ｓ−ｅｙｅ ｖｉｅｗ ｃａｍｅｒａ ａｎｄ ｇｙｒｏｓｃｏｐｅ，”Ｐｒｏｃ．３ｒｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔ’ｌ Ｓｙｍｐ．ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ２００４），ｐｐ．８９−９８，２００４．
【非特許文献８】Ｄ．Ｋｏｔａｋｅ，Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，ａｎｄ Ｈ．Ｙａｍａｍｏｔｏ，“Ａ ｈｙｂｒｉｄ ａｎｄ ｌｉｎｅａｒ ｒｅｇｉｓｔｒａｔｉｏｎ ｍｅｔｈｏｄ ｕｔｉｌｉｚｉｎｇ ｉｎｃｌｉｎａｔｉｏｎ ｃｏｎｓｔｒａｉｎｔ，”Ｐｒｏｃ．Ｆｏｕｒｔｈ ＩＥＥＥ ａｎｄ ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ２００５），ｐｐ．１４０−１４９，Ｏｃｔ．２００５．
【発明の概要】
【発明が解決しようとする課題】
【００１３】
しかしながら、従来のように、ジャイロセンサの傾斜角成分の計測値に基づき、撮像装置の位置及び姿勢を直接的に算出する手法では、指標が２点（同一水平面上にある２点を除く）しか検出されない場合、解が求まらず、撮像装置の位置及び姿勢を求めることができなかった。また、多数の指標を利用するＲＡＮＳＡＣのような手法では、撮像装置の位置及び姿勢を求めるために、指標が３点以上必要であり、処理に時間がかかるという問題があった。
【００１４】
そこで、本発明の目的は、指標が２点しか検出されない場合であっても、撮像手段の位置及び姿勢のうちの少なくとも何れか一方を算出することにある。
【課題を解決するための手段】
【００１５】
本発明の情報処理装置は、撮像手段により撮像された画像データを入力する入力手段と、前記画像データ上における指標の位置を検出する検出手段と、第１の座標系における前記撮像手段の傾斜角、前記画像データ上における前記指標の位置、及び、前記第１の座標系における前記指標の位置に基づいて、第２の座標系における前記指標の位置を算出する第１の算出手段と、前記第１の座標系における前記撮像手段の傾斜角、及び、前記第２の座標系における前記指標の位置に基づいて、前記第１の座標系における前記撮像手段の位置及び姿勢のうちの少なくとも何れか一方を算出する第２の算出手段とを有することを特徴とする。
【発明の効果】
【００１６】
本発明によれば、指標が２点しか検出されない場合であっても、撮像装置の位置及び姿勢のうちの少なくとも何れか一方を算出することが可能となる。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態に係る位置姿勢計測装置の構成を示す図である。
【図２】本発明の実施形態に係る位置姿勢計測装置の処理を示すフローチャートである。
【図３】本発明の実施形態に係る位置姿勢計測装置のハードウェア構成を示す図である。
【図４】正方形指標の例を示す図である。
【図５】円形状指標の例を示す図である。
【発明を実施するための形態】
【００１８】
以下、本発明を適用した好適な実施形態を、添付図面を参照しながら詳細に説明する。
【００１９】
先ず、本発明の第１の実施形態について説明する。第１の実施形態では、傾斜角センサを装着した撮像装置の位置及び姿勢を計測する方法について説明する。なお、本実施形態において、傾斜角センサの計測値は、３軸姿勢センサであるジャイロセンサの傾斜角成分の計測値である。一般的に、ジャイロセンサは、３軸姿勢センサであるが、方位角成分はドリフト誤差を含んでいる。そのため、信頼できる傾斜角成分のみの計測値を利用する。本実施形態では、傾斜角センサとしてジャイロセンサを利用しているが、これに限定されず、姿勢の傾斜角成分を計測可能な任意のセンサを利用することができる。
【００２０】
図１は、本発明の第１の実施形態に係る位置姿勢計測装置１０００の構成を示す図である。なお、位置姿勢計測装置１０００は、情報処理装置の例となる構成である。図１に示すように、第１の実施形態に係る位置姿勢計測装置１０００には、撮像装置１００及び傾斜角センサ１１０が接続されている。撮像装置１００は、例えばビデオカメラであり、指標１５０が配置されたシーンを撮影する。本実施形態では、撮影装置１００の焦点距離やレンズ歪み係数等のカメラ内部パラメータは、予め公知の方法で求めておき、既知であるとする。
【００２１】
画像入力部１０１０は、撮像装置１００によって撮影された２次元画像データを位置姿勢計測装置１０００内に入力する。画像入力部１０１０は、例えば、ＰＣに設置されたビデオキャプチャカードである。また、本実施形態では、画像入力部１０１０から入力された２次元画像データには、レンズ歪み補正が施されており、歪みはないものとして扱う。指標検出部１０２０は、画像入力部１０１０から入力された２次元画像データ上における指標１５０の座標を検出する。
【００２２】
傾斜角センサ１１０は、撮像装置１００上に固定されており、傾斜角入力部１０３０に接続されている。傾斜角入力部１０３０は、傾斜角センサ１１０により計測された基準座標系における撮像装置１００の傾斜角を入力する。データ管理部１０４０は、指標検出部１０２０により検出された２次元画像データ上における指標１５０の座標と、傾斜角入力部１０３０により取得された基準座標系における撮像装置１００の傾斜角とをセットにして、３次元位置算出部１０５０に出力する。なお、上記基準座標系は、第１の座標系の例である。
【００２３】
３次元位置算出部１０５０は、撮像装置１００で規定される座標系（以下、撮像装置座標系と称す）における指標１５０の３次元位置を、２次元画像データ上における指標１５０の座標と基準座標系における撮像装置１００の傾斜角とに基づいて算出し、データ管理部１０４０に対して出力する。位置姿勢算出部１０６０は、撮像装置座標系における指標１５０の３次元位置、及び、基準座標系における撮像装置１００の傾斜角をデータ管理部１０４０から入力し、基準座標系における撮像装置１００の位置及び姿勢を算出して出力する。なお、上記撮像装置座標系は、第２の座標系の例である。また、３次元位置算出部１０５０は、第１の算出手段の例であり、位置姿勢算出部１０６０は、第２の算出手段の例である。
【００２４】
ここで、本実施形態で用いられる指標１５０について説明する。環境中又は物体上には、図１に示すような指標１５０が複数存在しているものとする。図１に示すように、指標１５０は、環境中に存在する角点等、２次元画像データから抽出可能な特徴点である。本実施形態では、公知の検出器（例えば、Ｈａｒｒｉｓ検出器）を用いて特徴点を検出する。検出器で検出された特徴点の座標は、指標１５０の座標として利用される。ここで、指標１５０が存在している環境は、動的に変化せず、指標間の相対的な位置関係は変化しないものとする。基準座標系における指標１５０の３次元位置は、予め公知の方法で求めておく。本実施形態では、特徴点を指標１５０として扱うが、これに限るものではなく、公知の検出器により検出可能であれば、何れの形態であってもよい。
【００２５】
次に、図２を参照しながら、本実施形態に係る位置姿勢計測装置１０００の処理について説明する。なお、図２に示す処理は、位置姿勢計測装置１０００における後述するＣＰＵ３０１がＲＯＭ３０７や記憶媒体３０６から必要なデータ及びプログラムを読み込んで実行することにより実現する処理である。
【００２６】
ステップＳ２０１０において、画像入力部１０１０は、撮像装置１００によって撮影された２次元画像データを入力する。ステップＳ２０２０において、指標検出部１０２０は、画像入力部１０１０にて入力された２次元画像データ上における指標１５０の座標を検出する。そして、指標検出部１０２０は、検出した各々の２次元画像データ上における指標１５０の座標ｕjを、指標１５０の識別子ｊ（ｊ＝１，２，・・・，Ｊ）とともに、データ管理部１０４０に対して登録する。ここで、Ｊは検出された指標の総数を表す。
【００２７】
指標１５０の識別は、検出器により特徴点が検出され、その特徴点周辺の画像データに対するテンプレートマッチング法により行われる。このテンプレートマッチング法は、画像入力部１０１０から入力される２次元画像データとテンプレート画像データとを重ね合わせることにより比較照合し、両者が一致しているか否かを判定することにより行われる。即ち、上記比較照合時に、両者の類似度が算出され、その値が大きい程、指標１５０として確からしいとするものである。類似度としては、入力された２次元画像データにおける輝度情報とテンプレート画像データの輝度情報との相関が用いられる。例えば、正規化相互相関は、２次元画像データの明るさが正規化された量になっているために、明るさが変化した場合にも照合することができる。しかし、指標１５０の識別方法はこれに限定されず、公知の何れの方法を用いてもよい。
【００２８】
ステップＳ２０３０において、傾斜角入力部１０３０は、撮像装置１００に装着された傾斜角センサ１１０の出力値である、基準座標系における撮像装置１００の傾斜角を入力する。厳密には、傾斜角センサ１１０は、センサ自身で定義されるセンサ座標系の傾斜角を出力するため、この傾斜角センサ１１０の傾斜角は、基準座標系における撮像装置１００の傾斜角と異なる。しかし、本実施形態では、傾斜角センサ１１０は撮像装置１００に固定されており、撮像装置１００に対する傾斜角センサ１１０の相対姿勢は、予め何らかの公知の方法で求めておく。そのため、傾斜角入力部１０３０は、傾斜角センサ１１０によって計測された撮像装置１００の傾斜角と、撮像装置１００に対する傾斜角センサ１１０の相対姿勢とに基づいて得られた、基準座標系における撮像装置１００の傾斜角を、データ管理部１０４０に対して出力する。
【００２９】
ステップＳ２０４０において、３次元位置算出部１０５０は、２次元画像データ上における２点の指標１５０の座標をデータ管理部１０４０から取得する。そして、３次元位置算出部１０５０は、２次元画像データ上における２点の指標１５０の座標と、予め計測しておいた基準座標系における指標１５０の３次元位置と、基準座標系における撮像装置１００の傾斜角とに基づいて、撮像装置座標系における指標１５０の３次元位置を算出する。そして、３次元位置算出部１０５０は、撮像装置座標系における指標１５０の３次元位置をデータ管理部１０４０に対して出力する。なお、撮像装置座標系における指標１５０の３次元位置の算出処理の詳細については後述する。
【００３０】
ステップＳ２０５０において、位置姿勢算出部１０６０は、撮像装置座標系における指標１５０の３次元位置と基準座標系における撮像装置１００の傾斜角とに基づいて、基準座標系における撮像装置１００の位置及び姿勢を算出する。ステップＳ２０６０において、位置姿勢算出部１０６０は、ステップＳ２０５０で算出した基準座標系における撮像装置１００の位置及び姿勢の妥当性を判定する。ステップＳ２０７０において、位置姿勢算出部１０６０は、基準座標系における撮像装置１００の位置及び姿勢を出力する。
【００３１】
以下、基準座標系における撮像装置１００の位置及び姿勢の算出処理について詳細に説明する。基準座標系には、位置が既知の指標１５０が配置又は設定されているものとする。本実施形態では、基準座標系のｙw軸を鉛直上方向にとる。基準座標系における撮像装置１００の姿勢を表す３×３回転変換行列をＲwcと表し、基準座標系における撮像装置１００の位置を表す３次元ベクトルをｔwcと表す。Ｒwcは、式１に示すように、基準座標系のｚw軸まわりの回転変換Ｒzwwc、基準座標系のｘw軸まわりの回転変換Ｒxwwc、基準座標系のｙw軸まわりの回転変換Ｒywwc、を順に組み合わせることで表現可能である。
【００３２】
【数１】
2014215821.tif 000003
【００３３】
ここで、Ｒxwwc・Ｒzwwcは、Ｒwcの傾斜角成分であり、Ｒywwcは、Ｒwcの方位角成分であるため、Ｒiwc＝Ｒxwwc・Ｒzwwc、Ｒawc＝Ｒywwcとおくと、式１は、次の式２のように表すことができる。ここでｉは、ｉｎｃｌｉｎａｔｉｏｎ（傾斜角）を表し、ａは、ａｚｉｍｕｔｈ（方位角）を表す。
【００３４】
【数２】
2014215821.tif 000004
【００３５】
従って、撮像装置座標系から基準座標系への座標変換は、基準座標系における撮像装置座標系の原点の位置ｔwcを用いて、次の式３のように表すことができる。
【００３６】
【数３】
2014215821.tif 000005
【００３７】
但し、ｘw及びｘcはそれぞれ、基準座標系及び撮像装置座標系における点の座標を表している。また、傾斜角センサ１１０は、基準座標系における撮像装置１００の傾斜角を出力するため、Ｒiwcは、傾斜角センサ１１０の出力値となる。ここで、Ｒiwcは、次の式４のように表すことができる。
【００３８】
【数４】
2014215821.tif 000006
【００３９】
また、方位角をθと表すと、Ｒawcは、次の式５のように表すことができる。
【００４０】
【数５】
2014215821.tif 000007
【００４１】
ここで、基準座標系における位置をｘw＝［ｘw ｙw ｚw］tとし、撮像装置座標系における位置をｘc＝［ｘc ｙc ｚc］tとし、基準座標系における撮像装置座標系の原点の位置ｔwc＝［ｔx ｔy ｔz］tとする。また、α＝ｃｏｓθ、β＝ｓｉｎθとする。式４及び式５を式３に代入すると、次の式６が得られる。
【００４２】
【数６】
2014215821.tif 000008
【００４３】
ある指標が２次元画像データ上で検出される位置ｕ＝［ｕ ｖ １］tは、実際に２次元画像データ上で検出される座標ｕ´＝［ｕ’ ｖ’ １］tを焦点距離ｆで除算した正規化座標系で、次の式７のように表される。
【００４４】
【数７】
2014215821.tif 000009
【００４５】
また、撮像装置座標系における２点の指標の位置ｘc1及びｘc2は、次の式８のように表される。
【００４６】
【数８】
2014215821.tif 000010
【００４７】
ここで、λ1及びλ2は、スケールファクタである。
【００４８】
次に、ステップＳ２０４０における、撮像装置座標系における指標の３次元位置算出処理について詳細に説明する。本実施形態では、検出されたＪ個の指標のうち、２点の指標を利用する。以下では、任意に選択された２点の指標の座標をｕ1及びｕ2と表す。３次元位置算出部１０５０は、撮像装置１００からそれぞれの指標までの距離λ1及びλ2を算出することにより、式８により撮像装置座標系における指標の３次元位置ｘc1及びｘc2を求める。基準座標系における２点の指標の３次元位置は既知であることから、２点間の３次元距離もまた既知である。従って、撮像装置座標系における２点の指標の３次元位置ｘc1とｘc2との距離をＣとすると、次の式９のように表すことができる。
【００４９】
【数９】
2014215821.tif 000011
【００５０】
式８及び式９から、次の式１０に表すようなλ1及びλ2に関する拘束条件が得られる。
【００５１】
【数１０】
2014215821.tif 000012
【００５２】
また、式６に式８を代入すると、次の式１１が得られる。
【００５３】
【数１１】
2014215821.tif 000013
【００５４】
式１１から撮像装置１００の位置であるｔを消去すると、次の式１２が得られる。
【００５５】
【数１２】
2014215821.tif 000014
【００５６】
式１２からｙ座標のみ抜き出すと、次の式１３のような２点の指標までの距離λ1及びλ2に関する拘束条件が得られる。
【００５７】
【数１３】
2014215821.tif 000015
【００５８】
従って、式１３を式１０に代入して、２次方程式を解けば、撮像装置１００から２点の指標までの距離λ1及びλ2が算出される。ここで、２次方程式を解くと、２つの解が得られる。そこで、３次元位置算出部１０５０は、２つの解のそれぞれを式８に代入することにより、撮像装置座標系における２点の指標の３次元位置ｘc1及びｘc2を算出する。
【００５９】
次に、ステップＳ２０５０における、基準座標系における撮像装置１００の位置及び姿勢算出処理について詳細に説明する。基準座標系における撮像装置１００の傾斜角は、傾斜角センサ１１０から得られるため、基準座標系における撮像装置１００の方位角θ及び位置ｔが未知パラメータとなる。
【００６０】
ステップＳ２０４０で算出された撮像装置座標系における２点の指標の位置ｘc1及びｘc2を、式１２に代入すると、次の式１４のように表せる。
【００６１】
【数１４】
2014215821.tif 000016
【００６２】
ここで、撮像装置座標系における２点の指標の座標ｘc1とｘc2との差、及び、基準座標系における２点の指標の座標ｘw1とｘw2との差を、次の式１５のように表す。
【００６３】
【数１５】
2014215821.tif 000017
【００６４】
式１５を式１４に代入すると、次の式１６が得られる。
【００６５】
【数１６】
2014215821.tif 000018
【００６６】
また、ｘ座標及びｙ座標を取り出すと、次の式１７が得られる。
【００６７】
【数１７】
2014215821.tif 000019
【００６８】
式１７の連立方程式を解いて、α及びθを求める。α＝ｃｏｓθ、β＝ｓｉｎθであるため、次の式１８により、基準座標系における撮像装置１００の方位角θを一意に求めることができる。
【００６９】
【数１８】
2014215821.tif 000020
【００７０】
ここで、本来、α及びβは、α2＋β2＝１となることを満たさなければならないが、実際には、検出誤差や校正誤差等により、必ずしもこれを満たさない。しかし、実験的に、極めて１に近くなるので、実用上は合理的な仮定であると考える。
【００７１】
次に、位置姿勢算出部１０６０は、基準座標系における撮像装置１００の位置ｔを算出する。α及びβ並びに式８を式１１に代入することにより、未知パラメータは、基準座標系における撮像装置１００の位置ｔのみとなり、次の式１９によって求められる。
【００７２】
【数１９】
2014215821.tif 000021
【００７３】
次に、ステップＳ２０６０における、撮像装置１００の位置及び姿勢の妥当性の判定処理について詳細に説明する。ステップＳ２０４０における、撮像装置座標系における指標の３次元位置算出過程で２つの解が得られるため、ステップＳ２０５０においても、２つの解が得られる。そこで、解の妥当性を判定する必要がある。前フレームの撮像装置１００の位置及び姿勢が算出されている場合には、前フレームの位置及び姿勢からの差分が近い方が、妥当な解であると判定される。
【００７４】
以上のように、本実施形態では、先ず、傾斜角センサ１１０を装着した撮像装置１００は、指標が配置されたシーンを撮影する。次に、位置姿勢計測装置１０００は、撮影された２次元画像データから指標の座標を検出し、傾斜角センサ１１０により計測された基準座標系における撮像装置１００の傾斜角と、２次元画像データ上における指標の座標とに基づいて、撮像装置座標系における指標の３次元位置を推定する。そして、位置姿勢計測装置１０００は、基準座標系における撮像装置１００の傾斜角と、撮像装置座標系における指標の座標とに基づいて、基準座標系における撮像装置１００の位置及び姿勢を算出する。このように、傾斜角センサ１１０により計測される基準座標系における撮像装置１００の傾斜角を利用することにより、２点の指標に基づき、基準座標系における撮像装置１００の位置及び姿勢を算出することができる。
【００７５】
本実施形態においては、２点しか指標を検出できない場面であっても、基準座標系における撮像装置１００の位置及び姿勢を計測することができる。また、多数の指標を検出することができる場合であっても、検出した指標の多くにノイズや検出誤差が含まれる際に有効である。例えば、ロバスト推定手法の一つであるＲＡＮＳＡＣを利用する際に、ランダムにサンプリングする最小限の指標の対応は、２組のみでよいため、３組以上を必要としていた従来の手法と比較して、アウトライヤの含まれる確率が低い。そのため、正しい解を得るまでに計算する回数が少なくて済むため、ＲＡＮＳＡＣの処理を高速化することができる。
【００７６】
図３は、本実施形態に係る位置姿勢計測装置１０００のハードウェア構成を示す図である。図３に示した構成は、位置姿勢計測装置１０００であるパーソナルコンピュータにカメラ３０３及び傾斜角センサ３０５を接続することにより実現する。なお、カメラ３０３は、図１の撮像装置１００に対応する構成であり、傾斜角センサ３０５は、図１の傾斜角センサ１１０に対応する構成である。
【００７７】
画像取り込み器３０２は、カメラ３０３で撮影された２次元画像データをパーソナルコンピュータ内に取り込むものであり、画像入力部１０１０に対応する。画像取り込み器３０２は、例えばビデオキャプチャボードであり、カメラ３０３で撮影された２次元画像データをパーソナルコンピュータ内に取り込むものであれば何でもよい。傾斜角入力器３０４は、傾斜角センサ３０５で計測されたカメラ３０３の傾斜角をパーソナルコンピュータ内に取り込む。ＣＰＵ３０１は、マウス３０９やキーボード３１０からの指示や入力などに基づいて、記憶媒体３０６、ＲＯＭ３０７、又は、不図示の外部記憶装置等に保存されているプログラムをＲＡＭ３０８に読み出すなどして実行する。これにより、ＣＰＵ３０１は、図１における指標検出部１０２０、３次元位置算出部１０５０、位置姿勢算出部１０６０として機能する。また、データ管理部１０４０は、記憶媒体３０６に情報を保存したり、記憶媒体３０６から情報の読み出しを行う。
【００７８】
次に、本発明の第２の実施形態について説明する。第１の実施形態では、指標として環境中で検出できる角点のような特徴点を用いて説明しているが、指標は特徴点に限るものではなく、識別することができれば、何れの形態であってもよい。例えば、図４（ａ）に示すような、正方形指標４００を用いてもよい。正方形指標４００は、内部に識別子を表すパターンを持ち、一意に同定可能であるとする。ここで、正方形指標４００の識別子をｊ（ｊ＝１，２，・・・，Ｊ）とおく。また、図４（ｂ）に示すように、正方形指標４００の３次元空間における位置は、正方形の４隅の角点４０１であるとする。正方形指標４００は２次元画像データから容易に検出可能なように黒枠で囲まれているため、黒枠内部の正方形を正方形指標であるとする。
【００７９】
正方形指標を検出する場合には、２次元画像データ上での正方形指標の各頂点の座標が算出される。正方形指標の各頂点の位置を自動で求めるために、例えば、２次元画像データを２値化して、正方形指標の黒枠領域を検出し、黒枠領域に内接する四角形を求めればよい。
【００８０】
また、それぞれが異なるテクスチャ特徴を有するような平面物体を指標とする場合には、既知の情報として予め保持している各々の指標のテンプレート画像データによるテンプレートマッチングを２次元画像データに対して施すことにより、２次元画像データから指標を検出すればよい。
【００８１】
また、図５に示すように、それぞれが異なる色を有する円形状指標５００であってもよい。この場合には、色領域毎に識別子を持たせておき、２次元画像データ上から各々の指標の色に対応する領域を検出し、その重心位置に基づいて指標の検出座標とすればよい。なお、指標は、２次元画像データ上で検出及び識別可能であれば、図１、図４及び図５に示した態様に限られない。
【００８２】
次に、本発明の第３の実施形態について説明する。第２の実施形態の説明では、図４に示すような、正方形指標４００の４隅の角点４０１を指標としてもよいと述べた。正方形指標は、四角形の一部や、内部の識別子のパターンが隠れると検出及び識別できないものが一般的である。そこで、正方形指標のような人為的な指標において、指標の一部が遮蔽され、検出できない場合には、正方形の４頂点等の特徴点を、Ｈａｒｒｉｓ検出器等を利用して検出すればよい。そして、その特徴点周辺の画像データに対するテンプレートマッチング法により、特徴点を識別することができる。正方形指標に限るものではなく、三角形や多角形等の人為的な指標においても同様である。従って、正方形指標のような人為的な指標の一部が遮蔽されて、指標を検出及び識別できない場合には、検出器によって正方形の頂点等の特徴点を検出するように指標の検出方法を切り替えることが好ましい。
【００８３】
そこで、本発明の第３の実施形態では、図１に示した構成に指標検出方法切り替え部（不図示）を追加する。指標検出方法切り替え部は、指標検出部１０２０により検出された指標の座標に基づいて、指標検出方法を切り替え、指標検出部１０２０に対して切り替え後の指標検出方法を設定する。このように、指標検出方法が切り替えられた場合、指標検出部１０２０は、切り替え後の指標検出方法で指標の座標を再度検出する。
【００８４】
以下、指標検出方法の切り替え方法について具体的に説明する。指標検出方法切り替え部は、例えば、前フレームのカメラの位置姿勢と、正方形指標のような人為的な指標の座標とに基づいて、２次元画像データ上で検出され得る位置に指標を投影する。指標検出方法切り替え部は、投影した２次元画像データ上の位置付近で、人為的な指標が検出できない場合、正方形指標のような人為的な指標の一部が遮蔽されているために指標が検出できないと判定し、特徴点を検出するように指標検出方法を切り替える。
【００８５】
このように指標検出方法を切り替えることにより、正方形指標のような人為的な指標の一部が遮蔽されても、２つの特徴点が検出及び識別できれば、基準座標系における撮像装置１００の位置及び姿勢を算出することができる。
【００８６】
次に、本発明の第４の実施形態について説明する。第１の実施形態では、撮像装置１００の位置及び姿勢を一意に決定するために、前フレームの撮像装置１００の位置及び姿勢に基づいて、撮像装置１００の位置及び姿勢の妥当性を判定した。しかし、妥当性の判定は、前フレームの撮像装置１００の位置及び姿勢に基づいた方法に限るものではない。
【００８７】
例えば、多数の指標が検出された場合には、次のように判定することができる。先ず、算出された撮像装置１００の位置及び姿勢と指標の３次元位置とに基づいて、２次元画像データ上における指標の座標が算出される。次に、このように算出された２次元画像データ上における指標の座標と、２次元画像データから検出された指標の座標との誤差に基づいて、それぞれの指標が、アウトライヤ又はインライヤなのかが算出される。そこで、インライヤとなる指標の数が多い撮像装置１００の位置及び姿勢を、妥当であると判定すればよい。
【００８８】
また、撮像装置１００の移動領域が予め分かっている場合には、撮像装置１００の位置の取り得る領域が設定され、算出された撮像装置１００の位置が、設定された領域内か否かをチェックすることにより、算出された撮像装置１００の位置及び姿勢の妥当性を判定するようにしてもよい。
【００８９】
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【符号の説明】
【００９０】
１００：撮像装置、１１０：傾斜角センサ、１０００：位置姿勢計測装置、１０１０：画像入力部、１０２０：指標検出部、１０３０：傾斜角入力部、１０４０：データ管理部、１０５０：３次元位置算出部、１０６０：位置姿勢算出部
【図１】
2014215821.tif 000022
【図２】
2014215821.tif 000023
【図３】
2014215821.tif 000024
【図４】
2014215821.tif 000025
【図５】
2014215821.tif 000026