JP4522140B2 - 指標配置情報推定方法および情報処理装置 - Google Patents

Description

本発明は、３次元空間中に配置した指標の配置情報を推定するものに関する。

近年、複合現実感（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ；ＭＲ）技術の研究が盛んである。ＭＲ技術は、現実空間とコンピュータによって作られる仮想空間を継ぎ目なく融合する技術である。ＭＲ技術の中でも、現実空間に仮想空間を重ね合わせて表示するＡｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＡＲ、拡張現実感、増強現実感とも呼ばれる）技術が特に注目を集めている。

ＡＲの画像表示装置は、ビデオカメラ等の撮像装置によって撮影された現実空間の画像に、該撮像装置の位置及び姿勢に応じて生成された仮想空間（コンピュータグラフィクスにより描画された仮想物体や文字情報等）の画像を重畳描画した合成画像を表示するビデオシースルー方式か、あるいは観察者の頭部に装着された光学シースルー型ディスプレイに、観察者の視点の位置及び姿勢に応じて生成された仮想空間の画像を表示する光学シースルー方式によって実現される。

ＡＲ技術は、患者の体表面に体内の様子を重畳表示する手術支援や、空き地に仮想のビルを重畳表示する建築シミュレーション、組立作業時に作業手順や配線の様子を重畳表示する組み立て支援等様々な分野への応用が期待される。

ＡＲ技術において最も重要な課題の一つは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来より多くの取り組みが行われてきた。ＡＲにおける位置合わせの問題は、ビデオシースルー方式の場合はシーン中における（すなわち基準座標系における）撮像装置の位置及び姿勢を求める問題に帰結する。同様に、光学シースルー方式の場合は、シーン中における観察者の視点あるいはディスプレイの位置及び姿勢を求める問題に帰結する。

前者の問題を解決する方法として、シーン中に複数の指標を配置あるいは設定し、撮像装置が撮影した画像内における指標の投影位置と指標の基準座標系における位置との対応関係から、基準座標系における該撮像装置の位置及び姿勢を求めることが一般的に行われている。また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置を装着し、前者と同様な方法によって該撮像装置の位置及び姿勢を求め、それに基づいて計測対象物の位置及び姿勢を求めることが一般的に行われている。

図１を用いて、撮像装置が撮影した画像から検出される指標の２次元座標と、基準座標系における指標の３次元位置とを対応づけることで該撮像装置の位置及び姿勢を計測する位置姿勢計測装置の従来例について説明する。図１に示すように、本従来例における位置姿勢計測装置１００は、指標検出部１１０及び位置姿勢算出部１２０によって構成されており、撮像装置１３０に接続されている。

また、現実空間中には、撮像装置１３０の位置姿勢を求めるための指標として、基準座標系における位置が既知であるＫ個の指標Ｑ_ｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。図１の例は、４個の指標Ｑ_１、Ｑ_２、Ｑ_３、Ｑ_４が配置されており、そのうちの３個Ｑ_１、Ｑ_３、Ｑ_４が撮像装置１３０の視野内に含まれている状況を示している。

指標Ｑ_ｋは、例えば各々が異なる色を有する円形状のマーカなど、撮影画像内における指標の投影位置が検出可能であって、かつ何れの指標であるかが同定可能であるような指標であれば何れの形態であってもよい。例えば３次元空間中の自然特徴点を指標として用い、撮影画像内からテンプレートマッチングによって検出してもよい。撮像装置１３０が出力する画像は、位置姿勢計測装置１００に入力される。指標検出部１１０は、撮像装置１３０により画像を入力し、該画像上に撮影されている指標Ｑ_ｋの画像座標を検出する。例えば、指標Ｑ_ｋの各々が異なる色を有する円形状のマーカによって構成されている場合には、入力画像上から各々のマーカ色に対応する領域を検出し、その重心位置を当該指標の検出座標とする。

さらに指標検出部１１０は、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｍｋｎとその識別子ｋ_ｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（＝１，２，・・・，Ｎ）は検出された指標の通し番号を表す記号であり、Ｎは検出された指標の総数を表している。例えば図１の場合には、Ｎ＝３であり、識別子ｋ_１＝１、ｋ_２＝３、ｋ_３＝４とこれらに対応する画像座標ｕ^Ｍｋ１、ｕ^Ｍｋ２、ｕ^Ｍｋ３、が出力される。

位置姿勢算出部１２０は、検出された各々の指標Ｑ_ｋｎの画像座標ｕ^Ｍｋｎと、既知な情報として予め保持している当該指標Ｑ_ｋｎの基準座標系における位置との対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。指標の３次元座標と画像座標の組から撮像装置の位置及び姿勢を算出する方法は、写真測量の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。位置姿勢算出部１２０は、例えば非特許文献１に記載の手法によって、撮像装置１３０の位置及び姿勢を算出する。

なお、ここでは複数の３次元空間中の点としての指標（以下、点指標）を用いる場合について説明したが、例えば非特許文献３、４で開示されているように、大きさが既知の正方形形状の指標（以下、正方形指標）を用いた撮像装置の位置及び姿勢を算出が提案されている。また、例えば非特許文献７で開示されているように、正方形指標と点指標を組み合わせた撮像装置の位置及び姿勢算出方法も提案されている。点指標には狭いところにも設定できるというメリットがあり、正方形指標には同定が簡単である、一つの指標の情報量が多いため一つの指標だけから撮像装置の位置及び姿勢を求めることができるというメリットがあり、相補的に利用することができる。

以上の方法によって、撮像装置が撮影した画像に基づいて、該撮像装置の位置及び姿勢を取得することが従来からなされてきた。

一方、例えば特許文献１、２や非特許文献５で開示されているように、計測対象である撮像装置に磁気センサや超音波センサなどの６自由度位置姿勢センサを取り付け、前述のような画像処理による指標の検出との併用によって位置及び姿勢を計測することも行われている。センサの出力値は測定範囲によって精度は変わるが安定的に得ることができるため、センサと画像処理を併用する方法は、画像処理だけによる方法に比べてロバスト性を向上させることができる。

指標を利用した位置合わせ手法では、計測対象である撮像装置の基準座標系における位置及び姿勢を求めるために、点指標の場合は基準座標系における位置、正方形指標の場合は基準座標系における位置及び姿勢が既知である必要がある。正方形指標の場合には、基準座標系を別途設けることなく、正方形指標そのものを座標系の基準とする場合が多いが、複数の正方形指標を用いる場合には、互いの位置及び姿勢の関係が既知である必要があるため、基準座標系が必要であることに変わりはない。

指標の位置及び姿勢の計測は、巻尺や分度器を用いた手作業や、測量器によって行うことが可能であるが、精度や手間の問題から、画像を利用した計測が行われている。点指標の位置計測は、バンドル調整法と呼ばれる方法で求めることができる。バンドル調整法は、点指標を撮像装置によって多数撮影し、点指標、点指標の画像上での投影点、撮像装置の視点の３点が同一直線上に存在するという拘束条件をもとに、指標が実際に画像上で観察される投影位置と撮像装置の位置及び姿勢と指標の位置から計算される投影位置との誤差（投影誤差）が最小化されるように、繰返し演算によって画像を撮影した撮像装置の位置及び姿勢と、点指標の位置を求める方法である。

また、非特許文献６では３次元空間中に配置された多数の正方形マーカの位置及び姿勢を計測する方法が開示されている。非特許文献６では、３次元空間中に配置された多数の正方形マーカの画像を多数撮影し、投影誤差が最小になるように繰返し演算によって各画像を撮影した撮像装置の位置及び姿勢と、正方形マーカの位置及び姿勢を求めている。
特開平１１−０８４３０７号公報 特開２０００−０４１１７３号公報 Ｒ．Ｍ．Ｈａｒａｌｉｃｋ，Ｃ．Ｌｅｅ，Ｋ．Ｏｔｔｅｎｂｅｒｇ，ａｎｄ Ｍ．Ｎｏｌｌｅ："Ｒｅｖｉｅｗ ａｎｄ ａｎａｌｙｓｉｓ ｏｆ ｓｏｌｕｔｉｏｎｓ ｏｆ ｔｈｅ ｔｈｒｅｅ ｐｏｉｎｔ ｐｅｒｓｐｅｃｔｉｖｅ ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｐｒｏｂｌｅｍ"，Ｉｎｔ’ｌ．Ｊ． Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｖｏｌ．１３，ｎｏ．３，ｐｐ．３３１−３５６，１９９４． Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ ａｎｄ Ｒ．Ｃ．Ｂｏｌｌｅｓ："Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓｕｓ： ａ ｐａｒａｄｉｇｍ ｆｏｒ ｍｏｄｅｌ ｆｉｔｔｉｎｇ ｗｉｔｈ ａｐｐｌｉｃａｔｉｏｎｓ ｔｏ ｉｍａｇｅ ａｎａｌｙｓｉｓ ａｎｄ ａｕｔｏｍａｔｅｄ ｃａｒｔｏｇｒａｐｈｙ"，Ｃｏｍｍ．ＡＣＭ，ｖｏｌ．２４，ｎｏ．６，ｐｐ．３８１−３９５，１９８１． 暦本純一："２次元マトリックスコードを利用した拡張現実感の構成手法"，インタラクティブシステムとソフトウェアＩＶ，近代科学社，１９９６． 加藤，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，浅野，橘："マーカー追跡に基づく拡張現実感システムとそのキャリブレーション"，日本バーチャルリアリティ学会論文誌，ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１６，１９９９． Ａ．Ｓｔａｔｅ，Ｇ．Ｈｉｒｏｔａ，Ｄ．Ｔ．Ｃｈｅｎ，Ｗ．Ｆ．Ｇａｒｒｅｔｔ ａｎｄ Ｍ．Ａ．Ｌｉｖｉｎｇｓｔｏｎ："Ｓｕｐｅｒｉｏｒ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ ｒｅｇｉｓｔｒａｔｉｏｎ ｂｙ ｉｎｔｅｇｒａｔｉｎｇ ｌａｎｄｍａｒｋ ｔｒａｃｋｉｎｇ ａｎｄ ｍａｇｎｅｔｉｃ ｔｒａｃｋｉｎｇ"，Ｐｒｏｃ．ＳＩＧＧＲＡＰＨ’９６，ｐｐ．４２９−４３８，１９９６． Ｇ．Ｂａｒａｔｏｆｆ，Ａ．Ｎｅｕｂｅｃｋ ａｎｄ Ｈ．Ｒｅｇｅｎｂｒｅｃｈｔ："Ｉｎｔｅｒａｃｔｉｖｅ ｍｕｌｔｉ−ｍａｒｋｅｒ ｃａｌｉｂｒａｔｉｏｎ ｆｏｒ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ ａｐｐｌｉｃａｔｉｏｎｓ"，Ｐｒｏｃ．ＩＳＭＡＲ２００２，ｐｐ．１０７−１１６，２００２． Ｈ．Ｋａｔｏ，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，Ｉ．Ｐｏｕｐｙｒｅｖ，Ｋ．Ｉｍａｍｏｔｏ ａｎｄ Ｋ．Ｔａｃｈｉｂａｎａ："Ｖｉｒｔｕａｌ ｏｂｊｅｃｔ ｍａｎｉｐｕｌａｔｉｏｎ ｏｎ ａ ｔａｂｌｅ−ｔｏｐ ＡＲ ｅｎｖｉｒｏｎｍｅｎｔ"，Ｐｒｏｃ．ＩＳＡＲ２０００，ｐｐ．１１１−１１９，２０００．

前述した従来の指標の位置及び姿勢の計測手法では、例えば複数の指標が同一平面上にあるといった指標の配置に関する拘束条件がある場合に、計測結果が拘束条件を満たさない場合がある。これは、前述した計測手法が、撮影した画像上での指標の投影誤差を最小化する手法であるため、指標の投影位置の計算に用いるパラメータ（カメラの焦点距離、主点位置、レンズ歪補正パラメータ）や、指標の画像上での観察位置に誤差がある場合には、計測値にも誤差を生じるからである。この結果、例えば同一平面上にあるはずの複数の指標が同一平面上に存在しないという計測結果が得られる。

本発明は、上記の課題に鑑みてなされたものであり、その目的は、指標の配置に関して拘束条件がある場合には、その拘束条件を満たすように指標の位置及び姿勢を求めることにある。

上記目的を達成するために本発明は以下の構成を有することを特徴とする。

請求項１に記載の発明は、空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する情報処理装置が行う指標配置情報推定方法であって、前記情報処理装置が有する指標検出手段が、指標を含む撮影画像から指標を検出する指標検出工程と、前記情報処理装置が有する拘束条件設定手段が、指標の拘束条件を設定する拘束条件設定工程と、前記情報処理装置が有する指標投影位置計算手段が、前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算工程と、前記情報処理装置が有する補正値計算手段が、前記指標検出工程が検出した指標の画像面上での位置と、前記指標投影位置計算工程により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定工程により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算工程と、前記情報処理装置が有する配置情報補正手段が、前記補正値計算工程によって求めた補正値によって指標の配置情報を補正する配置情報補正工程と、前記情報処理装置が有する繰り返し演算手段が、前記配置情報補正工程により補正された指標の配置情報を前記指標投影位置計算工程の近似値のかわりに利用して、前記指標投影位置計算工程と前記補正値計算工程と前記配置情報補正工程とを繰返し行うことにより指標の配置情報を算出する繰り返し演算工程とを備え、前記拘束条件は、指標が未知の同一平面上に存在することであるという拘束条件であり、該拘束条件に固有な平面の方程式によって表現され、該拘束条件を満たす指標の配置情報は、該拘束条件に固有の指標の平面上の位置と平面の法線ベクトルを回転軸とした回転角によって表現される。

請求項３に記載の発明は、空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する情報処理装置が行う指標配置情報推定方法であって、前記情報処理装置が有する指標検出手段が、指標を含む撮影画像から指標を検出する指標検出工程と、前記情報処理装置が有する拘束条件設定手段が、指標の拘束条件を設定する拘束条件設定工程と、前記情報処理装置が有する指標投影位置計算手段が、前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算工程と、前記情報処理装置が有する補正値計算手段が、前記指標検出工程が検出した指標の画像面上での位置と、前記指標投影位置計算工程により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定工程により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算工程と、前記情報処理装置が有する配置情報補正手段が、前記補正値計算工程によって求めた補正値によって指標の配置情報を補正する配置情報補正工程と、前記情報処理装置が有する繰り返し演算手段が、前記配置情報補正工程により補正された指標の配置情報を前記指標投影位置計算工程の近似値のかわりに利用して、前記指標投影位置計算工程と前記補正値計算工程と前記配置情報補正工程とを繰返し行うことにより指標の配置情報を算出する繰り返し演算工程とを備え、前記空間中に存在する指標は，位置と姿勢をその配置情報とするような、法線方向の定義可能な指標であって、前記拘束条件は、複数の指標の法線ベクトルの方向が同一であるという拘束条件であり、前記拘束条件に固有な法線ベクトルの方程式で表現され、該拘束条件を満たす指標の配置情報は、指標の位置と該法線ベクトルを回転軸とした回転角によって表現されることを特徴とする指標配置情報推定方法。

以上説明したように、本発明によれば、指標の配置に関して拘束条件がある場合には、その拘束条件を満たすように指標の位置及び姿勢を求めることで指標の位置及び姿勢を精度よく求めることが可能になる。

また、請求項６記載の発明によれば、指標の配置に関して複数の拘束条件がある場合に、それらの複数の拘束条件を満たすように指標の位置及び姿勢を求めることで指標の位置及び姿勢を精度よく求めることが可能になる。

以下、添付の図面を参照して本発明の好適な実施形態について説明する。

＜第一実施形態＞
図２は本実施形態における指標位置姿勢推定装置２の機能構成を示すブロック図である。撮像部２１０はカメラであり、指標が配置されたシーンを撮影する。画像取り込み部２２０は、撮像部２１０によって撮影された画像をコンピュータ内に取り込む。画像取り込み部２２０によってコンピュータ内に取り込まれた画像から、指標検出同定部２３０によって指標を検出し、検出されたそれぞれの指標の同定を行う。指標配置条件設定部２４０では、指標の配置に関する拘束条件を設定する。指標位置姿勢算出部２５０は、指標検出同定部２３０の指標検出結果を基に、指標配置条件設定部２４０で設定した指標の配置に関する拘束条件を満たすように指標の位置及び姿勢を算出する。

図３は、本実施形態における指標位置姿勢推定装置のハードウェア構成を示すブロック図である。図３に示したハードウェア構成は通常のパーソナルコンピュータの構成と同等であるが、さらに画像取り込み器３１０、カメラ３１１が接続されている。画像取り込み器３１０はカメラ３１１で撮影された画像をコンピュータ内に取り込むものであり、画像取り込み部２２０に対応する。画像取り込み器３１０は例えばビデオキャプチャボードであるが、カメラで撮影した画像をコンピュータ内に取り込むものであれば何でもよい。ＣＰＵ３０１は、ディスク３０５またはＲＯＭ３０６、不図示の光学メディア、外部記憶装置等に保存されているプログラムを実行することにより、指標検出同定部２３０、指標配置条件設定部２４０、指標位置姿勢算出部２５０として機能する。

次に、以上の構成を備えた本実施形態の指標位置姿勢推定装置の動作の概要について述べる。

まず、本実施形態において用いられる指標について説明する。図４は本実施形態で用いられる指標を説明する図である。図４（ａ）は配置情報が３次元空間における点の位置によって表される点指標である。画像内で検出できるように、点指標は同一色からなる円形形状をとり、その重心位置を点指標の３次元空間における位置であるとする。図４（ｂ）は、配置情報が３次元空間中における位置と姿勢によって表現される平面指標である。本実施形態では、平面指標として正方形指標を用いるが、平面指標は正方形指標に限るものではなく、平面図形様の指標であれば何れの形態であってもよい。正方形指標の３次元空間における位置は、正方形の中心、すなわち２本の対角線の交点であるとする。また、図４（ｂ）に示すように、指標の法線方向をｚ軸とし、ｘ軸、ｙ軸を指標の二つの辺に平行になるように正方形指標の座標系をとり、基準座標系から見た正方形指標の座標系の姿勢を正方形指標の姿勢とする。本実施形態における正方形指標は画像上で容易に検出可能なように黒枠で囲まれているので、黒枠内部の正方形を正方形指標として用いる。

（１．指標の撮影）
撮像部２１０において、指標が配置されたシーンの画像を多数撮影する。撮影された画像は画像取り込み部２２０によってコンピュータ内に取り込まれる。

（２．指標の検出同定）
指標検出同定部２３０において、コンピュータ内に取り込まれた撮影画像から指標を検出し同定を行う。ここで、指標の検出とは、２次元画像上での指標の画像座標を求めることであり、点指標の場合は画像上での重心の画像座標を、正方形指標では各頂点の画像座標を求める。指標の画像座標は、自動で求めてもよいし、ユーザが画像上をマウスでクリックするなどして手動で指定してもよい。点指標の位置を自動で求める場合には、例えば指標と同一の色を有する画素を該色に対応するＹＵＶ色空間画像の特定領域にあるかどうかで抽出し、同一色として連結している画素をラベリングすることで指標領域を求め、画像内での指標領域の重心を計算する。また、正方形指標の各頂点の位置を自動で求める場合には、例えば入力画像を二値化して、正方形指標の黒枠領域を検出し、黒枠領域に内接する四角形を求める。

指標の同定とは、画像内で検出される指標を一意に特定することである。指標の同定は、自動で行ってもよいし、ユーザが手動で行ってもよい。点指標の同定を自動で行う場合には、例えば複数の点指標の色をそれぞれ異なる色にして、指標検出の際の指標領域のラベリング色を基に同定を行う。また、正方形指標の同定を自動的に行う場合には、例えば指標毎に固有のパターンを持つようにすることで同定を行う。

（３．指標配置条件設定）
指標配置条件設定部２４０において、指標の配置に関する拘束条件を設定する。本実施形態では、指標の配置に関する拘束条件として、複数の指標が同一平面上に存在するという拘束条件を与える。平面は未知の平面であるとし、未知な平面は複数存在するとする。図５は同一平面上に存在する複数の指標を設定するＧＵＩを説明する図である。ＧＵＩ上には、撮影画像表示部分があり、撮像装置によって撮影された画像が表示される。撮影画像は、撮影画像切替部分のボタンによって表示を切り替えることが可能である。Ｐｒｅｖｉｏｕｓボタンをマウスでクリックすると、一つ前の撮影画像を表示し、Ｎｅｘｔボタンをクリックすると一つ後の撮影画像を表示する。ユーザは撮影画像を見ながら、同一平面上にある指標を設定する。撮影画像上では、同定された指標の周辺に、各指標に固有の名前が表示される。図５では、点指標が２つ（図中ＰＯＩＮＴ１、ＰＯＩＮＴ２）、正方形指標（図中ＳＱＵＡＲＥ１、ＳＱＵＡＲＥ２）が２つ撮影されている撮影画像を撮影画像表示部分に表示している場合である。このうち点指標ＰＯＩＮＴ１と正方形指標ＳＱＵＡＲＥ１が同一平面上にあるとする。ユーザは、撮影画像中のＰＯＩＮＴ１かＳＱＵＡＲＥ１のどちらかの指標の周辺をマウスでクリックし、選択する。選択された指標の名前表示は太字に変わる。次に、選択した指標と同一平面に存在する指標の周辺をシフトキーを押しながらマウスを左クリックすることで、同一平面上にある指標をグループ化する。すでに同一平面上に存在するとグループ化されている指標をクリックした場合には、同一グループ内のすべての指標の名前表示が太字に変わる。この状態で、他の指標をシフトキーを押しながら左クリックすると、グループに追加することができる。また、グループ化されている指標の周辺をシフトキーを押しながらマウスを右クリックすると、グループから外れ、シフトキーを押さないで右クリックするとグループ全体が解除される。

（４．指標の位置姿勢算出）
指標の検出同定結果、指標の配置に関する拘束条件をもとに、基準座標系における指標の位置及び姿勢の算出を行う。はじめに透視投影変換について説明する。図６はカメラ座標系及び画面座標系を説明する図である。画面座標系の原点ｏ_ｉは視軸と画像面との交点にとり、画像の水平方向をｘ_ｉ軸、垂直方向をｙ_ｉ軸にとる。また、カメラ座標系の原点ｏ_ｃと画像面との距離（焦点距離）をｆとし、カメラ座標系のｚ_ｃ軸は視軸の反対方向、ｘ_ｃ軸は画像の水平方向と平行に、ｙ_ｃ軸は画像の垂直方向と平行にとる。

透視投影変換により、カメラ座標系上の点ｘ_ｃ＝［ｘ_ｃｙ_ｃｚ_ｃ］^ｔは式（２）のように画面座標がｕ＝［ｕ_ｘｕ_ｙ］^ｔである点に投影される。

本実施形態ではレンズ歪みは存在しないか、もしくは補正されていると仮定し、カメラはピンホールカメラであると想定している。式（２）は、図７に示すように、空間中の点と、該点の画像上の投影点とカメラ位置（視点）は同一直線上に存在することを示しており、共線条件式とも呼ばれる。

基準座標系におけるカメラの位置をｔ＝［ｔ_ｘｔ_ｙｔ_ｚ］^ｔ、カメラの姿勢（実際には、カメラ座標系に対する基準座標系の姿勢）をω＝［ω_ｘω_ｙω_ｚ］であるとする。ωは３自由度の姿勢表現方法であり、姿勢を回転軸ベクトル及び回転角で表現している。回転角をｒ_ａとすると、ｒ_ａはωによって式（３）のように表される。

また、回転軸ベクトルをｒ_ａｘｉｓ＝［ｒ_ｘｒ_ｙｒ_ｚ］^ｔとすると、ｒ_ａｘｉｓとωの関係は式（４）のように表される。

ω（回転角ｒ_ａ、回転軸ベクトルｒ_ａｘｉｓ）と３×３回転変換行列Ｒとの関係は式（５）のように表される。

基準座標系上の点ｘ_ｗ＝［ｘ_ｗｙ_ｗｚ_ｗ］^ｔのカメラ座標ｘ_ｃはｔ、Ｒによって式（６）のように表される。

式（２）、（６）から、透視投影変換によって、基準座標上の点ｘ_ｗ＝［ｘ_ｗｙ_ｗｚ_ｗ］^ｔは式（７）のように画像上の点ｕ＝［ｕ_ｘｕ_ｙ］^ｔに投影される。

理想的には、ｔ、ω、ｘ_ｗをもとに式（７）から計算される投影位置（計算位置）と実際に観察される位置（観察位置）は一致する。そのため、計算位置と観察位置の画像の水平方向のずれをＦ、垂直方向のずれをＧ、観察位置をｕ_ｏ＝［ｕ_ｏｘｕ_ｏｙ］^ｔとすると、式（８）に示すようにＦ、Ｇは０となる。

Ｆ、Ｇはカメラ位置ｔ、カメラ姿勢ω、観察対象点の基準座標上での位置ｘ_ｗに関する関数である。ここで、基準座標上での点の位置ｘ_ｗは、指標の種類や、平面上に拘束するかどうかによって異なる変数の関数となるため、Ｆ、Ｇも指標の種類や、平面上に拘束するかどうかによって異なる変数の関数となる。以下では、指標を平面上に拘束する場合としない場合に分けて説明する。
（指標を平面上に拘束しない場合）
点指標の場合には、点指標の基準座標系における位置をｔ_ｐｎ＝［ｔ_ｐｎｘｔ_ｐｎｙｔ_ｐｎｚ］^ｔとすると、式（９）に示すように、ｘ_ｗとｔ_ｐｎは同じになる。

そのため、式（１０）に示すように、Ｆ、Ｇはカメラ位置ｔ、カメラ姿勢ω、点指標の基準座標系における位置ｔ_ｐに関する関数となる。

次に正方形指標の場合について説明する。正方形指標は、基準座標系における位置ｔ_ｓ＝［ｔ_ｓｘｔ_ｓｙｔ_ｓｚ］^ｔ及び基準座標系に対する姿勢ω_ｓ＝［ω_ｓｘω_ｓｙω_ｓｚ］（ω_ｓに対応した３×３回転変換行列をＲ_ｓとする）で表現される。正方形指標の頂点の正方形指標座標系における位置をｘ_ｓ＝［ｘ_ｓｙ_ｓ０］^ｔとする。正方形指標の頂点の基準座標系における位置ｘ_ｗは式（１１）のようにｔ_ｓとω_ｓ（Ｒ_ｓ）に関する関数となる。

そのため、式（１２）に示すように、Ｆ、Ｇは、カメラ位置ｔ、カメラ姿勢ω、正方形指標の位置ｔ_ｓ及び正方形指標の姿勢ω_ｓの関数となる。

（指標を平面上に拘束する場合）
まず平面の定義方法について説明する。図８に示すように、平面を極座標表現ｒ＝［ｒθφ］によって表現する。平面座標系のｚ_ｐ軸は平面の法線方向に平行であり、基準座標系における原点を通過する。平面の姿勢は、ｙ軸まわりの回転θと、ｘ軸まわりの回転φによって定義する。また、ｒは平面と基準座標系の原点ｏ_ｗとの符号付き距離である。平面の姿勢を表す回転行列Ｒ_ｐは式（１３）のように表される。

また、平面座標系の原点ｏ_ｐの基準座標系における位置ｔ_ｐｌ＝［ｔ_ｐｌｘｔ_ｐｌｙｔ_ｐｌｚ］^ｔは、ｚ_ｐ軸と平面との交点とし、式（１４）のように求める。ただし

は平面の法線ベクトルを表す。

上述の極座標表現ｒ＝［ｒθφ］で表される平面座標系上の点ｘ_ｐ＝［ｘ_ｐｙ_ｐ０］^ｔの基準座標系における位置ｘ_ｗは式（１５）のように表される。式（１５）から、ｘ_ｗはｘ_ｐ、ｒの関数であることがわかる。

平面上に拘束される点指標は、平面座標系上の位置ｔ^ｐｌ _ｐ＝［ｔ^ｐｌ _ｐｘｔ^ｐｌ _ｐｙ０］^ｔで表される。式（１６）のように、ｘ_ｐはｔ^ｐｌ _ｐと同じになる。

そのため、ｘ_ｗはｔ^ｐｌ _ｐ、ｒの関数であり、Ｆ、Ｇは式（１７）に示すように、カメラ位置ｔ、カメラ姿勢ω、点指標の平面上の位置ｔ^ｐｌ _ｐ及び平面のパラメータｒの関数となる。

一方正方形指標の場合、平面座標系上の位置ｔ^ｐｌ _ｓ＝［ｔ^ｐｌ _ｓｘｔ^ｐｌ _ｓｙ０］^ｔに加えて、正方形指標の法線ベクトルまわりの（すなわち平面座標系のｚ_ｐ軸まわりの）回転θ^ｐｌで表されるため、正方形指標の頂点の正方形指標座標系における位置をｘ_ｓ＝［ｘ_ｓｙ_ｓ０］^ｔとすると、その平面座標ｘ_ｐは式（１８）のように表される。

よってｘ_ｗはｔ^ｐｌ _ｓ、θ^ｐｌ、ｒの関数であり、Ｆ、Ｇは式（１９）に示すように、カメラ位置ｔ、カメラ姿勢ω、正方形指標の平面上の位置ｔ^ｐｌ _ｓ、正方形指標の平面上での姿勢θ^ｐｌ及び平面のパラメータｒの関数となる。

式（１０）、（１２）、（１７）、（１９）はカメラの位置及び姿勢、指標の位置及び姿勢、平面のパラメータについて非線形な方程式となっている。そこで、１次の項までのテイラー展開を用いてカメラの位置及び姿勢、指標の位置及び姿勢、平面のパラメータの近似値の近傍で線形化し、繰返し計算によってカメラの位置及び姿勢、指標の位置及び姿勢、平面のパラメータを求める。

式（２０）、（２１）、（２２）、（２３）は、それぞれ式（１０）、（１２）、（１７）、（１９）を線形化したものである。Δｔ_ｘ、Δｔ_ｙ、Δｔ_ｚはカメラの位置、Δω_ｘ、Δω_ｙ、Δω_ｚはカメラの姿勢、Δｔ_ｐｘ、Δｔ_ｐｙ、Δｔ_ｐｚは平面に拘束されない点指標の位置、Δｔ_ｓｘ、Δｔ_ｓｙ、Δｔ_ｓｚは平面に拘束されない正方形指標の位置、Δω_ｓｘ、Δω_ｓｙ、Δω_ｓｚは平面に拘束されない正方形指標の姿勢、Δｔ^ｐｌ _ｐｘ、Δｔ^ｐｌ _ｐｙは平面に拘束される点指標の平面上での位置、Δｔ^ｐｌ _ｓｘ、Δｔ^ｐｌ _ｓｙは平面に拘束される正方形指標の平面上での位置、Δθ^ｐｌは平面に拘束される正方形指標の平面上での姿勢、Δｒ、Δθ、Δφは平面のパラメータの近似値に対する補正量を表す。

ここで式（２０）、（２１）、（２２）、（２３）のＦ^０、Ｇ^０は定数であり、カメラ位置及び姿勢、点指標の位置または正方形指標の位置及び姿勢、平面パラメータの近似値から求まる指標の投影位置（計算位置）と観察位置ｕ_ｏとの差である。

式（２０）、（２１）、（２２）、（２３）は、ある画像上で観察される一個の点指標または正方形指標の一個の頂点についての観測方程式である。実際には複数の画像上で複数の点指標や正方形指標の頂点が観察されるため、式（２０）、（２１）、（２２）、（２３）は複数得られる。そこで複数の式（２０）、（２１）、（２２）、（２３）を、カメラの位置及び姿勢、平面上に拘束されない点指標の位置、平面上に拘束されない正方形指標の位置及び姿勢、平面上に拘束される点指標の位置、平面上に拘束される正方形指標の位置及び姿勢、平面のパラメータの近似値に対する補正値についての連立方程式として解き、近似値を補正することを繰り返すことによってカメラの位置及び姿勢、平面上に拘束されない点指標の位置、平面上に拘束されない正方形指標の位置及び姿勢、平面上に拘束される点指標の位置、平面上に拘束される正方形指標の位置及び姿勢、平面のパラメータを求める。

図９は、本実施形態における点指標の位置、正方形指標の位置及び姿勢を求める手順を示すフローチャートである。なお、指標を含むシーンの撮影及び撮影画像からの指標抽出及び同定は終了しているものとする。ステップＳ１００では、各画像を撮影したカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値の設定を行う。ステップＳ１１０以降において、ステップＳ１００で設定された近似値を補正するための補正値を求める。ステップＳ１１０では、カメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値から、投影誤差を最小化するような各近似値の補正値を同時に求めるための連立方程式の立式を行う。ステップＳ１２０では、ステップＳ１１０で立式された連立方程式から各近似値の補正値を求める。ステップＳ１３０では、ステップＳ１２０で求めた補正値によって近似値を補正し、新たな近似値とする。ステップＳ１４０では、ステップＳ１３０において補正されたカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータが最適値に収束しているかどうかの判定を行う。以下、各ステップについて詳細に説明する。

ステップＳ１００において、各画像を撮影したカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値を入力する。ここで、撮影された画像の枚数をＮ枚とし、それぞれの画像を撮影したカメラの位置をｔ_ｉ＝［ｔ_ｉｘｔ_ｉｙｔ_ｉｚ］^ｔ（ｉ＝１，・・・，Ｎ）、姿勢をω_ｉ＝［ω_ｉｘω_ｉｙω_ｉｚ］（ｉ＝１，・・・，Ｎ）とする。また、位置を求める点指標のうち、平面上に拘束されない点指標の数をＫ_ｐ１個、平面上に拘束される点指標の数をＫ_ｐ２個とし、それぞれの点指標の位置をｔ_ｐｉ＝［ｔ_ｐｉｘｔ_ｐｉｙｔ_ｐｉｚ］^ｔ（ｉ＝１，・・・，Ｋ_ｐ１）、ｔ^ｐｌ _ｐｉ＝［ｔ^ｐｌ _ｐｉｘｔ^ｐｌ _ｐｉｙ］^ｔ（ｉ＝１，・・・，Ｋ_ｐ２）とする。また、位置及び姿勢を求める正方形指標のうち、平面上に拘束されない正方形指標の数をＫ_ｓ１個、平面上に拘束される正方形指標の数をＫ_ｓ２個とし、それぞれの正方形指標の位置をｔ_ｓｉ＝［ｔ_ｓｉｘｔ_ｓｉｙｔ_ｓｉｚ］^ｔ（ｉ＝１，・・・，Ｋ_ｓ１）、ｔ^ｐｌ _ｓｉ＝［ｔ^ｐｌ _ｓｉｘｔ^ｐｌ _ｓｉｙ］^ｔ（ｉ＝１，・・・，Ｋ_ｓ２）とし、姿勢をω_ｓｉ＝［ω_ｓｉｘω_ｓｉｙω_ｓｉｚ］（ｉ＝１，・・・，Ｋ_ｓ１）、θ^ｐｌ _ｉ（ｉ＝１，・・・，Ｋ_ｓ２）とする。

カメラの位置及び姿勢の近似値は、カメラに磁気センサのような６自由度位置姿勢センサを装着し、センサの出力値から得てもよいし、基準座標系における位置が既知の点と、該点の画像上での投影位置の対応関係から求めてもよい。基準座標系における位置が既知の点を用いる場合には、シーンには位置が既知の点と、求めようとしている配置情報が未知の指標とが混在していることになる。もしくは、後述する指標の位置および姿勢の近似値を利用して、その値を基にしてカメラの位置姿勢の近似値を求めても良い。

また、モーションコントロールカメラのように機械的にカメラの位置姿勢を得てもよいし、あらかじめ位置姿勢がキャリブレーション済みのカメラを用いて撮影し、そのキャリブレーション結果を用いてもよい。

指標の位置及び姿勢の近似値は、巻尺や分度器、測量器などを用いて手動で測定した概略の値を用いてもよいし、本実施形態の方法等で一度推定された値を用いてもよい。平面上に拘束される指標の位置及び姿勢の近似値は、平面座標系での位置及び姿勢を用いる。

平面のパラメータの近似値は、同一平面上にのる指標がすべて点指標であるならば、点指標の位置の近似値から回帰平面を求めることによって得る。また、同一平面上にのる指標に正方形指標が含まれている場合には、正方形指標を該平面であると仮定して正方形指標の位置及び姿勢から求める。さらに、指標の位置及び姿勢の近似値から求めないで、平面のパラメータをユーザが直接指定してもよい。

次にステップＳ１１０において、画像上で観察される指標の数だけ式（２０）、（２１）、（２２）、（２３）の観測方程式を立てる。式（２０）、（２１）、（２２）、（２３）は、一台のカメラの位置及び姿勢の補正値と、一個の点指標の位置の補正値または一個の正方形指標の位置及び姿勢の補正値と、一個の平面のパラメータの補正値についての観測方程式である。ここでは、式（２４）に示すように、Ｎ台のカメラの位置及び姿勢、Ｋ_ｐ１個の平面上に拘束されない点指標の位置、Ｋ_ｐ２個の平面上に拘束される点指標の位置、Ｋ_ｓ１個の平面上に拘束されない正方形指標の位置及び姿勢、Ｋ_ｓ２個の平面上に拘束される正方形指標の位置及び姿勢、Ｐ個の平面のパラメータについての観測方程式を立てる。この場合、未知数となる補正値の数は、（６×Ｎ＋３×Ｋ_ｐ１＋２×Ｋ_ｐ２＋６×Ｋ_ｓ１＋３×Ｋ_ｓ２＋３×Ｐ）個である。

画像ｉ（ｉ＝１，・・・，Ｎ）から検出される点指標の数をｄ_ｐｉ個、正方形指標の数をｄ_ｓｉ個とすると、Ｎ枚の画像から検出される点指標の数Ｄ_ｐ、正方形指標のＤｓは式（２５）のようになる。

Ｎ枚の画像から検出される点指標の数がＤ_ｐ、正方形指標の数がＤ_ｓ個である場合には、観測方程式（２４）が（Ｄ_ｐ＋４×Ｄ_ｓ）組、つまり２×（Ｄ_ｐ＋４×Ｄ_ｓ）本の観測方程式が立つ。式（２４）の観測方程式の左辺の定数項Ｆ^０、Ｇ^０を右辺に移項し、連立方程式を立てると、この連立方程式は式（２６）のように行列形式で書くことができる。

Ｊはヤコビ行列と呼ばれ、Ｆ、Ｇのカメラ位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータについての偏微分係数を並べたものである。ヤコビ行列Ｊの行数は、観測方程式の数２×（Ｄ_ｐ＋４×Ｄ_ｓ）個であり、列数は未知数の数（６×Ｎ＋３×Ｋ_ｐ１＋２×Ｋ_ｐ２＋６×Ｋ_ｓ１＋３×Ｋ_ｓ２＋３×Ｐ）個である。Δは補正値ベクトルである。補正値ベクトルの要素数は未知数の数（６×Ｎ＋３×Ｋ_ｐ１＋２×Ｋ_ｐ２＋６×Ｋ_ｓ１＋３×Ｋ_ｓ２＋３×Ｐ）個である。Ｅは誤差ベクトルであり、近似値による投影位置の計算位置と観察位置ｕ_ｏの差−Ｆ_０、−Ｇ_０を要素として持つ。Ｅの要素数は、観測方程式の数２×（Ｄ_ｐ＋４×Ｄ_ｓ）個である。

なお、基準座標系における位置が既知である点指標または位置及び姿勢が既知の正方形指標を同時に撮影することによって、基準座標系の原点、スケール、姿勢を明示的に指定することができる。これらの指標についての式（２４）の方程式では、指標の位置及び指標についての偏微分係数の値はすべて０となる。基準座標系の原点、スケール、姿勢を明示的に指定するためには、点指標であれば位置が既知の点指標を３個用いればよく、正方形指標であれば位置及び姿勢が既知の正方形指標を１個用いればよい。

次に、ステップＳ１２０においてカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値に対する補正値を求める。補正値は式（２６）から求める。ヤコビ行列Ｊが正方行列である場合には，式（２６）の両辺にＪの逆行列をかけることで補正値ベクトルΔを求める。Ｊが正方行列でない場合には、式（２７）に示すように最小二乗法によって補正値ベクトルΔを求める。

ここで、ヤコビ行列Ｊの各要素の求め方について説明する。まずＦ＝［ＦＧ］^ｔとおく。式（７）、（８）よりＦ、Ｇは式（２８）のように書けるので、Ｆ、Ｇはｘ_ｃ、ｙ_ｃ、ｚ_ｃの関数である。

Ｆ、Ｇのｘ_ｃ、ｙ_ｃ、ｚ_ｃによる偏微分係数を各要素に持つヤコビ行列

は、式（２９）のように書ける。

ここで、ｘ_ｃ、ｙ_ｃ、ｚ_ｃが変数ｓ_１、ｓ_２、・・・、ｓ_ｍの関数であるならば、Ｆ、Ｇもまたｓ_１、ｓ_２、・・・、ｓ_ｍの関数である。Ｆ、Ｇのｓ_１、ｓ_２、・・・、ｓ_ｍによる偏微分係数を各要素に持つヤコビ行列Ｊ_Ｆｓは、式（３０）のように分解することができる。

ｓをカメラ位置、カメラ姿勢、指標位置、指標姿勢、平面パラメータに置き換えて、

を求めることにより、Ｆ、Ｇのカメラ位置、カメラ姿勢、指標位置、指標姿勢、平面パラメータについての偏微分係数を求めることができる。

Ｆ、Ｇのカメラ位置ｔ_ｘ、ｔ_ｙ、ｔ_ｚによる偏微分係数を各要素に持つヤコビ行列Ｊ_Ｆｔは、式（３１）のように書ける。

ただし、姿勢ωと３×３回転変換行列Ｒの関係は式（５）で表される。

Ｆ、Ｇのカメラ姿勢ω_ｘ、ω_ｙ、ω_ｚによる偏微分係数を各要素に持つヤコビ行列Ｊ_Ｆωは、式（３２）のように分解して書ける。

ここで、

は式（３３）に示すようなヤコビ行列である。

また、

は式（３４）に示すようなヤコビ行列である。

Ｆ、Ｇの平面上に拘束されない点指標の位置ｔ_ｐｘ、ｔ_ｐｙ、ｔ_ｐｚによる偏微分係数を各要素に持つヤコビ行列

は、式（３７）のように書ける。

Ｆ、Ｇの平面上に拘束されない正方形指標の位置ｔ_ｓｘ、ｔ_ｓｙ、ｔ_ｓｚによる偏微分係数を各要素に持つヤコビ行列

は、式（３８）のように書ける。

Ｆ、Ｇの平面上に拘束されない正方形指標の姿勢ω_ｓによる偏微分係数を各要素に持つヤコビ行列

は、式（３９）のように分解することができる。

は、式（３４）〜（３６）と同様にして求めることができる。

次に、Ｆ、Ｇの平面上に拘束される点指標の位置ｔ^ｐｌ _ｐｘ、ｔ^ｐｌ _ｐｙによる偏微分係数を各要素に持つヤコビ行列

は、式（４０）のように書ける。

は式（３８）内と同様に求められる。

Ｆ、Ｇの平面上に拘束される正方形指標の位置ｔ^ｐｌ _ｓｘ、ｔ^ｐｌ _ｓｙによる偏微分係数を各要素に持つヤコビ行列

は、式（４１）のように書ける。

Ｆ、Ｇの平面上に拘束される正方形指標の姿勢θ^ｐｌによる偏微分係数を各要素に持つヤコビ行列

は、式（４２）のように書ける。

Ｆ、Ｇの平面のパラメータｒ、θ、φによる偏微分係数を各要素に持つヤコビ行列Ｊ_Ｐｒは、式（４３）のように書ける。

次にステップＳ１４０において、収束判定を行う。収束判定は、補正値の絶対値が指定した閾値を下回る、投影位置の計算位置と観察位置の差が指定した閾値を下回る、投影位置の計算位置と観察位置の差が極小となる、などの条件を判定することにより行う。本ステップで収束していると判定された場合、指標の位置及び姿勢推定を終了し、収束していない場合は、ステップＳ１１０に戻り、補正されたカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値をもとに再度連立方程式を立式する。

以上、説明したように、本実施形態では、指標が配置されたシーンの画像を多数撮影し、撮影された画像から指標を検出・同定し、同一平面上に存在する複数の指標を指定し、画像上の指標観察位置と、指標が同一平面上に存在するという拘束条件とから、撮影したカメラの位置及び姿勢、平面上に拘束されない指標の位置及び姿勢、平面上に拘束される指標の位置及び姿勢、平面のパラメータの近似値に対する補正値を求めて近似値を補正する操作を繰返し行うことによって、拘束条件を満たすように指標の位置及び姿勢を求める。

なお、本実施形態では、平面パラメータはすべて未知であるとしたが、平面パラメータが既知であってもよい。例えば、基準座標系におけるｘｙ平面など、容易に平面パラメータを求めることができる場合は、平面パラメータを既知とすることができる。平面パラメータが既知の場合には、式（２２）の平面上に拘束される点指標の観測方程式は式（４４）のようになる。

また、式（２３）の平面上に拘束される正方形指標の観測方程式は式（４５）のようになる。

＜第二実施形態＞
第一実施形態では、指標の配置に関する拘束条件として、複数の指標が同一平面上に存在するという拘束条件を与えたときに、拘束条件を満たすように指標の位置姿勢を推定方法について説明した。第二実施形態では、複数の指標が同一平面上に存在し、かつそれらの平面のなす角度が既知である場合について説明する。

なお、ここでは平面が二つ存在し、２平面間のなす角度が既知である場合について説明するが、平面の数は二つに限定されるものではない。

図１０は、指標が拘束される平面が二つ存在する場合を示している。平面１は第一実施形態で説明したように、基準座標系において極座標表現ｒ_１＝［ｒ_１θ_１φ_１］によって表現される。

一方、平面２も極座標表現ｒ_２＝［ｒ_２θ_２φ_２］によって表現されるが、θ_２、φ_２は、平面１に対する相対姿勢であるとする。つまり、平面１をｙ_ｐ１軸まわりにθ_２だけ回転した後、ｘ_ｐ１軸まわりにφ_２だけ回転したときの姿勢が、平面２の姿勢であるとする。ｒ_２は、基準座標系の原点と、平面２との符号付き距離である。このとき、平面１、平面２の姿勢を表す回転行列Ｒ^ｐ１、Ｒ^ｐ２はそれぞれ式（４７）、（４８）のように書ける。

ここで、平面１の法線ベクトル

と、平面２の法線ベクトル

のなす角度がαであることから、式（４９）が成り立つ。

式（４９）から、θ_２、αよりφ_２を求めることができる。

平面１上に拘束される点指標、正方形指標の観測方程式は、第一実施形態の式（２２）、（２３）と同じである。一方、平面２はｒ_２、θ_２の関数となるため、平面２上に拘束される点指標、正方形指標の観測方程式は、それぞれ式（５０）、式（５１）のようになる。

本実施形態において指標の位置及び姿勢を求める際には、観測方程式（２２）、（２３）、（５０）、（５１）をもとに連立方程式を立て、カメラの位置及び姿勢、指標の位置及び姿勢、平面１の３パラメータ、平面２の２パラメータの近似値に対する補正値を求め、繰返し計算によって近似値を最適化する。なお、式（４９）からφ_２を求める際には符号が異なる２つの解が得られるが、繰返し計算ごとに求まるθ_２に対して、２つφ_２の解のうち投影誤差が小さくなる方を解として採用する。

以上説明したように、本実施形態では、指標の配置に関する拘束条件として、複数の指標が同一平面上に存在し、かつそれらの平面のなす角度が既知である場合に、拘束条件を満たすように指標の位置及び姿勢を求めることができる。

＜第三実施形態＞
第三実施形態では、指標の配置に関する拘束条件として、複数の平面図形様の指標の法線ベクトルの方向が同じである場合の、指標の位置及び姿勢推定方法について説明する。

図１１は本実施形態を適用可能な指標配置を示す図である。図では、正方形指標が４個配置されており、４個の正方形指標の法線ベクトル、すなわち各正方形指標のｚ_ｓ軸の方向が同じであるとする。

正方形指標の法線ベクトルの向きをθ、φで表す。法線ベクトルは、基準座標系のｚ軸の単位ベクトルをｘ軸まわりにθだけ回転し、次にｙ軸まわりにφだけ回転したベクトルであるとする。θ、φは、法線ベクトルが同じ方向である複数の正方形指標にとって共通の値を持つ。個々の正方形指標のｚ軸まわりの回転をψとすると、正方形指標の基準座標系に対する姿勢Ｒ_ｓは式（５２）によって表される。

個々の正方形指標の基準座標系における位置をｔ_ｓ＝［ｔ_ｓｘｔ_ｓｙｔ_ｓｚ］^ｔとすると、正方形指標上の点ｘ_ｓ＝［ｘ_ｓｙ_ｓ］^ｔの基準座標系における位置ｘ_ｗは式（５３）によって表される。

本実施形態において、正方形指標の各頂点の基準座標系における位置は、法線ベクトルの向きθ、φ、正方形指標のｚ軸まわりの姿勢ψ、正方形指標の基準座標系における位置ｔ_ｓの関数となる。よって、法線ベクトルが拘束される場合の正方形指標の観測方程式は式（５４）のようになる。

本実施形態において指標の位置及び姿勢を求める際には、複数の観測方程式（５４）をもとに連立方程式を立て、カメラの位置及び姿勢、指標の位置及び姿勢、法線ベクトルの向きθ、φの近似値に対する補正値を求め、繰返し計算によって近似値を最適化する。

以上説明したように、本実施形態では、指標の配置に関する拘束条件として、複数の指標の法線ベクトルの向きが共通である場合に、拘束条件を満たすように指標の位置及び姿勢を求めることができる。

＜第四実施形態＞
第一、第二、第三実施形態では、指標の配置に関する拘束条件の種類を一つとして、指標の位置及び姿勢を求めていた、第四実施形態では、複数の拘束条件が存在する場合の、指標の位置及び姿勢を求める方法について説明する。

図１２は、本実施形態を適用可能な指標配置を示す図である。図は、点指標１、正方形指標１が同一平面（平面１）上に存在し、正方形指標２、正方形指標３の法線ベクトルの向きが共通であり、点指標２、正方形指標４は配置に関して拘束条件がない場合を示している。

図１２に示すシーンを撮像装置で撮影すると、点指標１については観測方程式（２２）が、正方形指標１については観測方程式（２３）が、正方形指標２、３については観測方程式（５４）が、点指標２については観測方程式（２０）が、正方形指標については観測方程式（２１）が成り立つ。式（２０）〜（２３）、（５４）は一台のカメラの位置及び姿勢の補正値と、一個の指標の位置及び姿勢、一個の平面のパラメータ、一個の法線ベクトルの向きの補正値についての観測方程式である。本実施形態では、式（５５）に示すように、Ｎ台のカメラの位置及び姿勢、Ｋ_ｐ１個の平面上に拘束されない点指標の位置、Ｋ_ｐ２個の平面上に拘束される点指標の位置、Ｋ_ｓ１個の平面上に拘束されない正方形指標の位置及び姿勢、Ｋ_ｓ２個の平面上に拘束される正方形指標の位置及び姿勢、Ｋ_ｓ３個の法線ベクトルが拘束される正方形指標の位置及び姿勢、Ｐ個の平面のパラメータ、Ｖ個の法線ベクトルの向きの補正値についての観測方程式を立てる。

撮影画像上で検出された点指標及び、正方形指標の各頂点について式（５５）の観測方程式を立て連立方程式として解くことにより、カメラ位置ｔ_ｉ、カメラ姿勢ω_ｉ、配置に関する拘束のない点指標の位置ｔ_ｐｉ、配置に関する拘束のない正方形指標の位置ｔ_ｓｉ、姿勢ω_ｓｉ、平面上に拘束される点指標の位置ｔ^ｐｌ _ｐｉ、平面上に拘束される正方形指標の位置ｔ^ｐｌ _ｓｉ、姿勢θ^ｐｌ _ｉ、法線ベクトルが拘束される正方形指標の位置ｔ^ｖ _ｓｉ、姿勢ψ_ｉ、平面のパラメータｒ_ｉ、法線ベクトルの向きθ^ｖ _ｉ、φ^ｖ _ｉの近似値についての補正値を同時に求め、繰返し演算によって近似値の補正を繰返して最適化する。

また、本実施形態の指標配置条件を設定するためのＧＵＩは、指標のグループ化と各グループの拘束条件の種類を設定することができる。すなわち、実施形態１において図５を用いて説明したＧＵＩにおいて、さらに拘束条件の種類を設定するためのＵＩを有している。

以上説明したように、本実施形態では、指標の配置に関する拘束条件が複数存在する場合に、すべての拘束条件を満たすように指標の位置及び姿勢を求めることができる。

＜変形例１＞
図１３に示すように、指標を円筒の側面部分に拘束してもよい。円筒は、円筒の中心に円筒座標系の原点をとり、図のように円筒座標系をとる。円筒の姿勢はｘ軸まわりの回転θとｙ軸まわりの回転φで表される。円筒の姿勢を表す回転行列Ｒ_ｃｙｌは式（５６）のように表される。

円筒の原点の基準座標系での位置がｔ^ｃｙｌ＝［ｔ^ｃｙｌ _ｘｔ^ｃｙｌ _ｙｔ^ｃｙｌ _ｚ］ｔとすると、円筒座標系上の点ｘ_ｃｙｌ＝［ｘ_ｃｙｌｙ_ｃｙｌｚ_ｃｙｌ］^ｔの基準座標系の位置ｘ_ｗは式（５７）のようになる。

また、円筒の側面上の点は、ｚ軸まわりの回転角βとｚ座標ｄ、既知の円筒半径ｒ_ｃｙｌによって、円筒座標系において式（５８）のように表せる。

円筒の側面に拘束される指標の投影位置は、カメラの位置及び姿勢、円筒の位置及び姿勢、円筒上での指標の位置β、ｄについての関数となるので、これらの補正値を求める観測方程式を解くことで、円筒の側面上に拘束された指標の位置を求めることができる。

＜変形例２＞
図１４に示すように、指標を球面上に拘束してもよい。球の中心に球座標系をとる。球は方向性を持たないため、基準座標系に対する位置だけで表される。球座標系の原点の基準座標系での位置がｔ^ｓｐｈ＝［ｔ^ｓｐｈ _ｘｔ^ｓｐｈ _ｙｔ^ｓｐｈ _ｚ］^ｔとすると、球座標系上の点ｘ_ｓｈｐ＝［ｘ_ｓｈｐｙ_ｓｈｐｚ_ｓｈｐ］^ｔの基準座標系の位置ｘ_ｗは式（５９）のようになる。

また、球面上の点は、既知の円筒半径ｒ_ｓｐｈと角度θ、φによって、球座標系において式（６０）のように表せる。

球面に拘束される指標の投影位置は、カメラの位置及び姿勢、球の位置、球面上での指標の位置θ、φについての関数となるので、これらの補正値を求める観測方程式を解くことで、球面上に拘束された指標の位置を求めることができる。

＜変形例３＞
図１５に示すように、指標を直線上に拘束してもよい。直線の通る点ｔ_ｌ＝［ｔ_ｌｘｔ_ｌｙｔ_ｌｚ］^ｔ、方向ベクトルをｄ＝［ｄ_ｘｄ_ｙｄ_ｚ］^ｔとすると、直線上にある指標の基準座標系の位置は式（６１）のように表せる。ｓは直線上での指標位置を表す変数である。

直線上に拘束される指標の投影位置は、カメラの位置及び姿勢、直線の通る点ｔ_ｌ、直線の方向ベクトルｄ、直線上での指標の位置ｓについての関数となるので、これらの補正値を求める観測方程式を解くことで、球面上に拘束された指標の位置を求めることができる。

＜変形例３＞
上記実施形態では、指標の配置情報の補正方法として、観測方程式をテイラー展開しその１次の項までを利用して繰り返し補正していく方法を述べた。これは、ある繰り返しの一過程において、局所的に観測方程式を線形近似し、誤差０となると仮定した場合の未知パラメータの補正量を推定する方法であり、一般にニュートン法と呼ばれている方法と等価な方法である。ニュートン法は非線形方程式を数値演算により求解する代表的な方法であるが、上記実施形態に利用する繰り返し演算方法はこの方法に限ったものではない。例えば、ニュートン法と同様に周知の技術として知られているＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法のように、一過程において推定される未知パラメータの補正量のばらつきに基づいて、補正量を動的に変更してもよい。また、テイラー展開した結果をより高次の項まで考慮して、繰り返し演算を行うような方法であっても良い。本発明の本質は、配置情報量の異なる複数種類の指標が混在して存在している場合であっても、それぞれの種類の指標を有効に利用して最適な解を求めることにあり、数値解法としての繰り返し演算の具体的な方法によって本質が失われるものではない。

従来の位置姿勢計測装置を説明する図である。 第一実施形態における指標位置姿勢推定装置２の機能構成を示すブロック図である。 第一実施形態における指標位置姿勢推定装置のハードウェア構成を示すブロック図である。 第一実施形態で用いられる指標を説明する図である。 同一平面上に存在する複数の指標を設定するＧＵＩを説明する図である。 カメラ座標系及び画面座標系を説明する図である。 共線条件式を説明する図である。 平面の定義方法を説明する図である。 第一実施形態における点指標の位置、正方形指標の位置及び姿勢を求める手順を示すフローチャートである。 指標が拘束される平面が二つ存在する場合を示す図である。 第三実施形態を適用可能な指標配置を示す図である。 第四実施形態を適用可能な指標配置を示す図である。 指標が円筒の側面上に拘束されることを説明する図である。 指標が球面上に拘束されることを説明する図である。

Claims (7)

1. 空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する情報処理装置が行う指標配置情報推定方法であって、
   前記情報処理装置が有する指標検出手段が、指標を含む撮影画像から指標を検出する指標検出工程と、
   前記情報処理装置が有する拘束条件設定手段が、指標の拘束条件を設定する拘束条件設定工程と、
   前記情報処理装置が有する指標投影位置計算手段が、前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算工程と、
   前記情報処理装置が有する補正値計算手段が、前記指標検出工程が検出した指標の画像面上での位置と、前記指標投影位置計算工程により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定工程により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算工程と、
   前記情報処理装置が有する配置情報補正手段が、前記補正値計算工程によって求めた補正値によって指標の配置情報を補正する配置情報補正工程と、
   前記情報処理装置が有する繰り返し演算手段が、前記配置情報補正工程により補正された指標の配置情報を前記指標投影位置計算工程の近似値のかわりに利用して、前記指標投影位置計算工程と前記補正値計算工程と前記配置情報補正工程とを繰返し行うことにより指標の配置情報を算出する繰り返し演算工程とを備え、
   前記拘束条件は、指標が未知の同一平面上に存在することであるという拘束条件であり、該拘束条件に固有な平面の方程式によって表現され、該拘束条件を満たす指標の配置情報は、該拘束条件に固有の指標の平面上の位置と平面の法線ベクトルを回転軸とした回転角によって表現されることを特徴とする指標配置情報推定方法。
2. 前記指標は平面図形であることを特徴とする請求項１に記載の指標配置情報推定方法。
3. 空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する情報処理装置が行う指標配置情報推定方法であって、
   前記情報処理装置が有する指標検出手段が、指標を含む撮影画像から指標を検出する指標検出工程と、
   前記情報処理装置が有する拘束条件設定手段が、指標の拘束条件を設定する拘束条件設定工程と、
   前記情報処理装置が有する指標投影位置計算手段が、前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算工程と、
   前記情報処理装置が有する補正値計算手段が、前記指標検出工程が検出した指標の画像面上での位置と、前記指標投影位置計算工程により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定工程により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算工程と、
   前記情報処理装置が有する配置情報補正手段が、前記補正値計算工程によって求めた補正値によって指標の配置情報を補正する配置情報補正工程と、
   前記情報処理装置が有する繰り返し演算手段が、前記配置情報補正工程により補正された指標の配置情報を前記指標投影位置計算工程の近似値のかわりに利用して、前記指標投影位置計算工程と前記補正値計算工程と前記配置情報補正工程とを繰返し行うことにより指標の配置情報を算出する繰り返し演算工程とを備え、
   前記空間中に存在する指標は，位置と姿勢をその配置情報とするような、法線方向の定義可能な指標であって、
   前記拘束条件は、複数の指標の法線ベクトルの方向が同一であるという拘束条件であり、前記拘束条件に固有な法線ベクトルの方程式で表現され、該拘束条件を満たす指標の配置情報は、指標の位置と該法線ベクトルを回転軸とした回転角によって表現されることを特徴とする指標配置情報推定方法。
4. 請求項１乃至３のいずれか１項に記載の指標配置情報推定方法をコンピュータに実現させるためのプログラム。
5. 請求項４に記載のプログラムを格納するためのコンピュータ読み取り可能な記憶媒体。
6. 空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する情報処理装置であって、
   指標を含む撮影画像から指標を検出する指標検出手段と、
   指標の拘束条件を設定する拘束条件設定手段と、
   前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算手段と、
   前記指標検出手段が検出した指標の画像面上での位置と、前記指標投影位置計算手段により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定手段により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算手段と、
   前記補正値計算手段によって求めた補正値によって指標の配置情報を補正する配置情報補正手段と、
   前記配置情報補正手段により補正された指標の配置情報を前記指標投影位置計算手段の近似値のかわりに利用して、前記指標投影位置計算手段での計算と前記補正値計算手段での計算と前記配置情報補正手段での補正とを繰返し行うことにより指標の配置情報を算出する繰り返し演算手段とを備え、
   前記拘束条件は、指標が未知の同一平面上に存在することであるという拘束条件であり、該拘束条件に固有な平面の方程式によって表現され、該拘束条件を満たす指標の配置情報は、該拘束条件に固有の指標の平面上の位置と平面の法線ベクトルを回転軸とした回転角によって表現されることを特徴とする情報処理装置。
7. 空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する情報処理装置であって、
   指標を含む撮影画像から指標を検出する指標検出手段と、
   指標の拘束条件を設定する拘束条件設定手段と、
   前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算手段と、
   前記指標検出手段が検出した指標の画像面上での位置と、前記指標投影位置計算手段により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定手段により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算手段と、
   前記補正値計算手段によって求めた補正値によって指標の配置情報を補正する配置情報補正手段と、
   前記配置情報補正手段により補正された指標の配置情報を前記指標投影位置計算手段の近似値のかわりに利用して、前記指標投影位置計算手段と前記補正値計算手段と前記配置情報補正手段とを繰返し行うことにより指標の配置情報を算出する繰り返し演算手段とを備え、
   前記空間中に存在する指標は，位置と姿勢をその配置情報とするような、法線方向の定義可能な指標であって、
   前記拘束条件は、複数の指標の法線ベクトルの方向が同一であるという拘束条件であり、前記拘束条件に固有な法線ベクトルの方程式で表現され、該拘束条件を満たす指標の配置情報は、指標の位置と該法線ベクトルを回転軸とした回転角によって表現されることを特徴とする情報処理装置。

Images