(57)【要約】【課題】  指標の配置に関する拘束条件がある場合に、拘束条件を満たすように指標の配置情報を求めることができないため、指標の配置情報を精度よく求めることができなかった。【解決手段】  指標の配置情報に関する幾何条件で定義可能である拘束条件を設定する拘束条件設定工程と、指標画像が含まれる撮影画像を取得する取得工程と、前記撮影画像から指標を検出する指標検出工程と、前記検出された画像上における指標の位置と前記拘束条件とを用いて、前記指標の配置情報を推定する推定工程とを有する。【選択図】  図９
2005327103.tif 000002
【特許請求の範囲】
【請求項１】
空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する方法であって、
指標を含む撮影画像から指標を検出する指標検出工程と、
指標の拘束条件を設定する拘束条件設定工程と、
前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算工程と、
前記指標検出工程が検出した指標の画像面上での位置と、前記指標投影位置計算工程により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定工程により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算工程と、
前記補正値計算工程によって求めた補正値によって指標の配置情報を補正する配置情報補正工程と、
前記配置情報補正工程により補正された指標の配置情報を前記指標投影位置計算工程の近似値のかわりに利用して、前期指標投影位置計算工程と前記補正値計算工程と前記配置情報補正工程とを繰返し行うことにより指標の配置情報を算出する繰り返し演算工程とを備えることを特徴とする指標配置情報推定方法。
【請求項２】
前記拘束条件は該拘束条件に固有の方程式によって表現され、該拘束条件を満たす指標の配置情報は、該拘束条件に固有の配置情報パラメータによって表現されることを特徴とする請求項１に記載の指標配置情報推定方法。
【請求項３】
前記拘束条件は、指標が未知の同一平面上に存在することであるという拘束条件であり、前記拘束条件に固有の方程式は平面の方程式であり、前記配置情報パラメータは指標の平面上の位置と平面の法線ベクトルを回転軸とした回転角であることを特徴とする請求項２に記載の指標配置情報推定方法。
【請求項４】
前記指標は平面図形であることを特徴とする請求項１乃至３のいずれかに記載の指標配置情報推定方法。
【請求項５】
空間中に存在する指標は，位置と姿勢をその配置情報とするような、法線方向の定義可能な指標であって，
前記拘束条件は、複数の指標の法線ベクトルの方向が同一であるという拘束条件であり、前記拘束条件に固有の方程式は法線ベクトルの方程式であり、前記配置情報パラメータは指標の位置と該法線ベクトルを回転軸とした回転角であることを特徴とする請求項２に記載の指標配置情報推定方法。
【請求項６】
前記拘束条件が複数存在し、該複数の拘束条件について、前記拘束条件に固有の方程式と、前記指標の配置情報パラメータを求めることによって指標の配置情報を推定することを特徴とする請求項２に記載の指標配置情報推定方法。
【請求項７】
撮影画像から指標の空間中での配置情報を推定する方法であって、
指標の配置情報に関する幾何条件で定義可能である拘束条件を設定する拘束条件設定工程と、
指標画像が含まれる撮影画像を取得する取得工程と、
前記撮影画像から指標を検出する指標検出工程と、
前記検出された画像上における指標の位置と前記拘束条件とを用いて、前記指標の配置情報を推定する推定工程とを有する指標配置情報推定方法。
【請求項８】
前記設定工程はユーザの指示に応じて拘束条件を設定し、
前記ユーザの指示には、指標のグループ化が含まれることを特徴とする請求項７記載の指標配置情報推定方法。
【請求項９】
前記ユーザの指示には、拘束条件の種類が含まれることを特徴とする請求項８記載の指標配置情報推定方法。
【請求項１０】
請求項１乃至９のいずれかに記載の指標配置情報推定方法をコンピュータにて実現するためのプログラム。
【請求項１１】
請求項１０記載のプログラムを格納するための記録媒体。
【請求項１２】
撮影画像から指標の空間中での配置情報を推定する情報処理装置であって、
指標の配置情報に関する幾何条件で定義可能である拘束条件を保持する保持手段と、
指標画像が含まれる撮影画像を取得する取得手段と、
前記撮影画像から指標を検出する指標検出手段と、
前記検出された画像上における指標の位置と前記拘束条件とを用いて、前記指標の配置情報を推定する推定手段とを有する情報処理装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元空間中に配置した指標の配置情報を推定するものに関する。
【背景技術】
【０００２】
近年、複合現実感（Ｍｉｘｅｄ  Ｒｅａｌｉｔｙ；ＭＲ）技術の研究が盛んである。ＭＲ技術は、現実空間とコンピュータによって作られる仮想空間を継ぎ目なく融合する技術である。ＭＲ技術の中でも、現実空間に仮想空間を重ね合わせて表示するＡｕｇｍｅｎｔｅｄ  Ｒｅａｌｉｔｙ（ＡＲ、拡張現実感、増強現実感とも呼ばれる）技術が特に注目を集めている。
【０００３】
ＡＲの画像表示装置は、ビデオカメラ等の撮像装置によって撮影された現実空間の画像に、該撮像装置の位置及び姿勢に応じて生成された仮想空間（コンピュータグラフィクスにより描画された仮想物体や文字情報等）の画像を重畳描画した合成画像を表示するビデオシースルー方式か、あるいは観察者の頭部に装着された光学シースルー型ディスプレイに、観察者の視点の位置及び姿勢に応じて生成された仮想空間の画像を表示する光学シースルー方式によって実現される。
【０００４】
ＡＲ技術は、患者の体表面に体内の様子を重畳表示する手術支援や、空き地に仮想のビルを重畳表示する建築シミュレーション、組立作業時に作業手順や配線の様子を重畳表示する組み立て支援等様々な分野への応用が期待される。
【０００５】
ＡＲ技術において最も重要な課題の一つは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来より多くの取り組みが行われてきた。ＡＲにおける位置合わせの問題は、ビデオシースルー方式の場合はシーン中における（すなわち基準座標系における）撮像装置の位置及び姿勢を求める問題に帰結する。同様に、光学シースルー方式の場合は、シーン中における観察者の視点あるいはディスプレイの位置及び姿勢を求める問題に帰結する。
【０００６】
前者の問題を解決する方法として、シーン中に複数の指標を配置あるいは設定し、撮像装置が撮影した画像内における指標の投影位置と指標の基準座標系における位置との対応関係から、基準座標系における該撮像装置の位置及び姿勢を求めることが一般的に行われている。また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置を装着し、前者と同様な方法によって該撮像装置の位置及び姿勢を求め、それに基づいて計測対象物の位置及び姿勢を求めることが一般的に行われている。
【０００７】
図１を用いて、撮像装置が撮影した画像から検出される指標の２次元座標と、基準座標系における指標の３次元位置とを対応づけることで該撮像装置の位置及び姿勢を計測する位置姿勢計測装置の従来例について説明する。図１に示すように、本従来例における位置姿勢計測装置１００は、指標検出部１１０及び位置姿勢算出部１２０によって構成されており、撮像装置１３０に接続されている。
【０００８】
また、現実空間中には、撮像装置１３０の位置姿勢を求めるための指標として、基準座標系における位置が既知であるＫ個の指標Ｑｋ（ｋ＝１，２，・・・，Ｋ）が配置されている。図１の例は、４個の指標Ｑ１、Ｑ２、Ｑ３、Ｑ４が配置されており、そのうちの３個Ｑ１、Ｑ３、Ｑ４が撮像装置１３０の視野内に含まれている状況を示している。
【０００９】
指標Ｑｋは、例えば各々が異なる色を有する円形状のマーカなど、撮影画像内における指標の投影位置が検出可能であって、かつ何れの指標であるかが同定可能であるような指標であれば何れの形態であってもよい。例えば３次元空間中の自然特徴点を指標として用い、撮影画像内からテンプレートマッチングによって検出してもよい。撮像装置１３０が出力する画像は、位置姿勢計測装置１００に入力される。指標検出部１１０は、撮像装置１３０により画像を入力し、該画像上に撮影されている指標Ｑｋの画像座標を検出する。例えば、指標Ｑｋの各々が異なる色を有する円形状のマーカによって構成されている場合には、入力画像上から各々のマーカ色に対応する領域を検出し、その重心位置を当該指標の検出座標とする。
【００１０】
さらに指標検出部１１０は、検出された各々の指標Ｑｋｎの画像座標ｕＭｋｎとその識別子ｋｎを位置姿勢算出部１２０へと出力する。ここで、ｎ（＝１，２，・・・，Ｎ）は検出された指標の通し番号を表す記号であり、Ｎは検出された指標の総数を表している。例えば図１の場合には、Ｎ＝３であり、識別子ｋ１＝１、ｋ２＝３、ｋ３＝４とこれらに対応する画像座標ｕＭｋ１、ｕＭｋ２、ｕＭｋ３、が出力される。
【００１１】
位置姿勢算出部１２０は、検出された各々の指標Ｑｋｎの画像座標ｕＭｋｎと、既知な情報として予め保持している当該指標Ｑｋｎの基準座標系における位置との対応関係に基づいて、撮像装置１３０の位置及び姿勢を算出する。指標の３次元座標と画像座標の組から撮像装置の位置及び姿勢を算出する方法は、写真測量の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。位置姿勢算出部１２０は、例えば非特許文献１に記載の手法によって、撮像装置１３０の位置及び姿勢を算出する。
【００１２】
なお、ここでは複数の３次元空間中の点としての指標（以下、点指標）を用いる場合について説明したが、例えば非特許文献３、４で開示されているように、大きさが既知の正方形形状の指標（以下、正方形指標）を用いた撮像装置の位置及び姿勢を算出が提案されている。また、例えば非特許文献７で開示されているように、正方形指標と点指標を組み合わせた撮像装置の位置及び姿勢算出方法も提案されている。点指標には狭いところにも設定できるというメリットがあり、正方形指標には同定が簡単である、一つの指標の情報量が多いため一つの指標だけから撮像装置の位置及び姿勢を求めることができるというメリットがあり、相補的に利用することができる。
【００１３】
以上の方法によって、撮像装置が撮影した画像に基づいて、該撮像装置の位置及び姿勢を取得することが従来からなされてきた。
【００１４】
一方、例えば特許文献１、２や非特許文献５で開示されているように、計測対象である撮像装置に磁気センサや超音波センサなどの６自由度位置姿勢センサを取り付け、前述のような画像処理による指標の検出との併用によって位置及び姿勢を計測することも行われている。センサの出力値は測定範囲によって精度は変わるが安定的に得ることができるため、センサと画像処理を併用する方法は、画像処理だけによる方法に比べてロバスト性を向上させることができる。
【００１５】
指標を利用した位置合わせ手法では、計測対象である撮像装置の基準座標系における位置及び姿勢を求めるために、点指標の場合は基準座標系における位置、正方形指標の場合は基準座標系における位置及び姿勢が既知である必要がある。正方形指標の場合には、基準座標系を別途設けることなく、正方形指標そのものを座標系の基準とする場合が多いが、複数の正方形指標を用いる場合には、互いの位置及び姿勢の関係が既知である必要があるため、基準座標系が必要であることに変わりはない。
【００１６】
指標の位置及び姿勢の計測は、巻尺や分度器を用いた手作業や、測量器によって行うことが可能であるが、精度や手間の問題から、画像を利用した計測が行われている。点指標の位置計測は、バンドル調整法と呼ばれる方法で求めることができる。バンドル調整法は、点指標を撮像装置によって多数撮影し、点指標、点指標の画像上での投影点、撮像装置の視点の３点が同一直線上に存在するという拘束条件をもとに、指標が実際に画像上で観察される投影位置と撮像装置の位置及び姿勢と指標の位置から計算される投影位置との誤差（投影誤差）が最小化されるように、繰返し演算によって画像を撮影した撮像装置の位置及び姿勢と、点指標の位置を求める方法である。
【００１７】
また、非特許文献６では３次元空間中に配置された多数の正方形マーカの位置及び姿勢を計測する方法が開示されている。非特許文献６では、３次元空間中に配置された多数の正方形マーカの画像を多数撮影し、投影誤差が最小になるように繰返し演算によって各画像を撮影した撮像装置の位置及び姿勢と、正方形マーカの位置及び姿勢を求めている。
【特許文献１】特開平１１−０８４３０７号公報
【特許文献２】特開２０００−０４１１７３号公報
【非特許文献１】Ｒ．Ｍ．Ｈａｒａｌｉｃｋ，Ｃ．Ｌｅｅ，Ｋ．Ｏｔｔｅｎｂｅｒｇ，ａｎｄ  Ｍ．Ｎｏｌｌｅ：“Ｒｅｖｉｅｗ  ａｎｄ  ａｎａｌｙｓｉｓ  ｏｆ  ｓｏｌｕｔｉｏｎｓ  ｏｆ  ｔｈｅ  ｔｈｒｅｅ  ｐｏｉｎｔ  ｐｅｒｓｐｅｃｔｉｖｅ  ｐｏｓｅ  ｅｓｔｉｍａｔｉｏｎ  ｐｒｏｂｌｅｍ”，Ｉｎｔ’ｌ．Ｊ．  Ｃｏｍｐｕｔｅｒ  Ｖｉｓｉｏｎ，ｖｏｌ．１３，ｎｏ．３，ｐｐ．３３１−３５６，１９９４．
【非特許文献２】Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ  ａｎｄ  Ｒ．Ｃ．Ｂｏｌｌｅｓ：“Ｒａｎｄｏｍ  ｓａｍｐｌｅ  ｃｏｎｓｅｎｓｕｓ：  ａ  ｐａｒａｄｉｇｍ  ｆｏｒ  ｍｏｄｅｌ  ｆｉｔｔｉｎｇ  ｗｉｔｈ  ａｐｐｌｉｃａｔｉｏｎｓ  ｔｏ  ｉｍａｇｅ  ａｎａｌｙｓｉｓ  ａｎｄ  ａｕｔｏｍａｔｅｄ  ｃａｒｔｏｇｒａｐｈｙ”，Ｃｏｍｍ．ＡＣＭ，ｖｏｌ．２４，ｎｏ．６，ｐｐ．３８１−３９５，１９８１．
【非特許文献３】暦本純一：“２次元マトリックスコードを利用した拡張現実感の構成手法”，インタラクティブシステムとソフトウェアＩＶ，近代科学社，１９９６．
【非特許文献４】加藤，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，浅野，橘：“マーカー追跡に基づく拡張現実感システムとそのキャリブレーション”，日本バーチャルリアリティ学会論文誌，ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１６，１９９９．
【非特許文献５】Ａ．Ｓｔａｔｅ，Ｇ．Ｈｉｒｏｔａ，Ｄ．Ｔ．Ｃｈｅｎ，Ｗ．Ｆ．Ｇａｒｒｅｔｔ  ａｎｄ  Ｍ．Ａ．Ｌｉｖｉｎｇｓｔｏｎ：“Ｓｕｐｅｒｉｏｒ  ａｕｇｍｅｎｔｅｄ  ｒｅａｌｉｔｙ  ｒｅｇｉｓｔｒａｔｉｏｎ  ｂｙ  ｉｎｔｅｇｒａｔｉｎｇ  ｌａｎｄｍａｒｋ  ｔｒａｃｋｉｎｇ  ａｎｄ  ｍａｇｎｅｔｉｃ  ｔｒａｃｋｉｎｇ”，Ｐｒｏｃ．ＳＩＧＧＲＡＰＨ’９６，ｐｐ．４２９−４３８，１９９６．
【非特許文献６】Ｇ．Ｂａｒａｔｏｆｆ，Ａ．Ｎｅｕｂｅｃｋ  ａｎｄ  Ｈ．Ｒｅｇｅｎｂｒｅｃｈｔ：“Ｉｎｔｅｒａｃｔｉｖｅ  ｍｕｌｔｉ−ｍａｒｋｅｒ  ｃａｌｉｂｒａｔｉｏｎ  ｆｏｒ  ａｕｇｍｅｎｔｅｄ  ｒｅａｌｉｔｙ  ａｐｐｌｉｃａｔｉｏｎｓ”，Ｐｒｏｃ．ＩＳＭＡＲ２００２，ｐｐ．１０７−１１６，２００２．
【非特許文献７】Ｈ．Ｋａｔｏ，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，Ｉ．Ｐｏｕｐｙｒｅｖ，Ｋ．Ｉｍａｍｏｔｏ  ａｎｄ  Ｋ．Ｔａｃｈｉｂａｎａ：“Ｖｉｒｔｕａｌ  ｏｂｊｅｃｔ  ｍａｎｉｐｕｌａｔｉｏｎ  ｏｎ  ａ  ｔａｂｌｅ−ｔｏｐ  ＡＲ  ｅｎｖｉｒｏｎｍｅｎｔ”，Ｐｒｏｃ．ＩＳＡＲ２０００，ｐｐ．１１１−１１９，２０００．
【発明の開示】
【発明が解決しようとする課題】
【００１８】
前述した従来の指標の位置及び姿勢の計測手法では、例えば複数の指標が同一平面上にあるといった指標の配置に関する拘束条件がある場合に、計測結果が拘束条件を満たさない場合がある。これは、前述した計測手法が、撮影した画像上での指標の投影誤差を最小化する手法であるため、指標の投影位置の計算に用いるパラメータ（カメラの焦点距離、主点位置、レンズ歪補正パラメータ）や、指標の画像上での観察位置に誤差がある場合には、計測値にも誤差を生じるからである。この結果、例えば同一平面上にあるはずの複数の指標が同一平面上に存在しないという計測結果が得られる。
【００１９】
本発明は、上記の課題に鑑みてなされたものであり、その目的は、指標の配置に関して拘束条件がある場合には、その拘束条件を満たすように指標の位置及び姿勢を求めることにある。
【課題を解決するための手段】
【００２０】
上記目的を達成するために本発明は以下の構成を有することを特徴とする。
【００２１】
請求項１記載の発明は、空間中に幾何拘束のもとに存在する指標の空間中での配置情報を推定する方法であって、指標を含む撮影画像から指標を検出する指標検出工程と、指標の拘束条件を設定する拘束条件設定工程と、前記撮影画像の撮像部の位置及び姿勢の近似値と、指標の配置情報の近似値とを基にして、指標が画像面に投影される位置を計算する指標投影位置計算工程と、前記指標検出工程が検出した指標の画像面上での位置と、前記指標投影位置計算工程により得られた指標の画像面上での投影位置との誤差が小さくなるように、かつ、前記拘束条件設定工程により設定された指標の拘束条件を満たすように、指標の配置情報を補正する補正値を求める補正値計算工程と、前記補正値計算工程によって求めた補正値によって指標の配置情報を補正する配置情報補正工程と、前記配置情報補正工程により補正された指標の配置情報を前記指標投影位置計算工程の近似値のかわりに利用して、前期指標投影位置計算工程と前記補正値計算工程と前記配置情報補正工程とを繰返し行うことにより指標の配置情報を算出する繰り返し演算工程とを備えることを特徴とする。
【００２２】
請求項７記載の発明は、撮影画像から指標の空間中での配置情報を推定する方法であって、指標の配置情報に関する幾何条件で定義可能である拘束条件を設定する拘束条件設定工程と、指標画像が含まれる撮影画像を取得する取得工程と、前記撮影画像から指標を検出する指標検出工程と、前記検出された画像上における指標の位置と前記拘束条件とを用いて、前記指標の配置情報を推定する推定工程とを有する。
【発明の効果】
【００２３】
以上説明したように、本発明によれば、指標の配置に関して拘束条件がある場合には、その拘束条件を満たすように指標の位置及び姿勢を求めることで指標の位置及び姿勢を精度よく求めることが可能になる。
【００２４】
また、請求項６記載の発明によれば、指標の配置に関して複数の拘束条件がある場合に、それらの複数の拘束条件を満たすように指標の位置及び姿勢を求めることで指標の位置及び姿勢を精度よく求めることが可能になる。
【発明を実施するための最良の形態】
【００２５】
以下、添付の図面を参照して本発明の好適な実施形態について説明する。
【００２６】
＜第一実施形態＞
図２は本実施形態における指標位置姿勢推定装置２の機能構成を示すブロック図である。撮像部２１０はカメラであり、指標が配置されたシーンを撮影する。画像取り込み部２２０は、撮像部２１０によって撮影された画像をコンピュータ内に取り込む。画像取り込み部２２０によってコンピュータ内に取り込まれた画像から、指標検出同定部２３０によって指標を検出し、検出されたそれぞれの指標の同定を行う。指標配置条件設定部２４０では、指標の配置に関する拘束条件を設定する。指標位置姿勢算出部２５０は、指標検出同定部２３０の指標検出結果を基に、指標配置条件設定部２４０で設定した指標の配置に関する拘束条件を満たすように指標の位置及び姿勢を算出する。
【００２７】
図３は、本実施形態における指標位置姿勢推定装置のハードウェア構成を示すブロック図である。図３に示したハードウェア構成は通常のパーソナルコンピュータの構成と同等であるが、さらに画像取り込み器３１０、カメラ３１１が接続されている。画像取り込み器３１０はカメラ３１１で撮影された画像をコンピュータ内に取り込むものであり、画像取り込み部２２０に対応する。画像取り込み器３１０は例えばビデオキャプチャボードであるが、カメラで撮影した画像をコンピュータ内に取り込むものであれば何でもよい。ＣＰＵ３０１は、ディスク３０５またはＲＯＭ３０６、不図示の光学メディア、外部記憶装置等に保存されているプログラムを実行することにより、指標検出同定部２３０、指標配置条件設定部２４０、指標位置姿勢算出部２５０として機能する。
【００２８】
次に、以上の構成を備えた本実施形態の指標位置姿勢推定装置の動作の概要について述べる。
【００２９】
まず、本実施形態において用いられる指標について説明する。図４は本実施形態で用いられる指標を説明する図である。図４（ａ）は配置情報が３次元空間における点の位置によって表される点指標である。画像内で検出できるように、点指標は同一色からなる円形形状をとり、その重心位置を点指標の３次元空間における位置であるとする。図４（ｂ）は、配置情報が３次元空間中における位置と姿勢によって表現される平面指標である。本実施形態では、平面指標として正方形指標を用いるが、平面指標は正方形指標に限るものではなく、平面図形様の指標であれば何れの形態であってもよい。正方形指標の３次元空間における位置は、正方形の中心、すなわち２本の対角線の交点であるとする。また、図４（ｂ）に示すように、指標の法線方向をｚ軸とし、ｘ軸、ｙ軸を指標の二つの辺に平行になるように正方形指標の座標系をとり、基準座標系から見た正方形指標の座標系の姿勢を正方形指標の姿勢とする。本実施形態における正方形指標は画像上で容易に検出可能なように黒枠で囲まれているので、黒枠内部の正方形を正方形指標として用いる。
【００３０】
（１．指標の撮影）
撮像部２１０において、指標が配置されたシーンの画像を多数撮影する。撮影された画像は画像取り込み部２２０によってコンピュータ内に取り込まれる。
【００３１】
（２．指標の検出同定）
指標検出同定部２２０において、コンピュータ内に取り込まれた撮影画像から指標を検出し同定を行う。ここで、指標の検出とは、２次元画像上での指標の画像座標を求めることであり、点指標の場合は画像上での重心の画像座標を、正方形指標では各頂点の画像座標を求める。指標の画像座標は、自動で求めてもよいし、ユーザが画像上をマウスでクリックするなどして手動で指定してもよい。点指標の位置を自動で求める場合には、例えば指標と同一の色を有する画素を該色に対応するＹＵＶ色空間画像の特定領域にあるかどうかで抽出し、同一色として連結している画素をラベリングすることで指標領域を求め、画像内での指標領域の重心を計算する。また、正方形指標の各頂点の位置を自動で求める場合には、例えば入力画像を二値化して、正方形指標の黒枠領域を検出し、黒枠領域に内接する四角形を求める。
【００３２】
指標の同定とは、画像内で検出される指標を一意に特定することである。指標の同定は、自動で行ってもよいし、ユーザが手動で行ってもよい。点指標の同定を自動で行う場合には、例えば複数の点指標の色をそれぞれ異なる色にして、指標検出の際の指標領域のラベリング色を基に同定を行う。また、正方形指標の同定を自動的に行う場合には、例えば指標毎に固有のパターンを持つようにすることで同定を行う。
【００３３】
（３．指標配置条件設定）
指標配置条件設定部２４０において、指標の配置に関する拘束条件を設定する。本実施形態では、指標の配置に関する拘束条件として、複数の指標が同一平面上に存在するという拘束条件を与える。平面は未知の平面であるとし、未知な平面は複数存在するとする。図５は同一平面上に存在する複数の指標を設定するＧＵＩを説明する図である。ＧＵＩ上には、撮影画像表示部分があり、撮像装置によって撮影された画像が表示される。撮影画像は、撮影画像切替部分のボタンによって表示を切り替えることが可能である。Ｐｒｅｖｉｏｕｓボタンをマウスでクリックすると、一つ前の撮影画像を表示し、Ｎｅｘｔボタンをクリックすると一つ後の撮影画像を表示する。ユーザは撮影画像を見ながら、同一平面上にある指標を設定する。撮影画像上では、同定された指標の周辺に、各指標に固有の名前が表示される。図５では、点指標が２つ（図中ＰＯＩＮＴ１、ＰＯＩＮＴ２）、正方形指標（図中ＳＱＵＡＲＥ１、ＳＱＵＡＲＥ２）が２つ撮影されている撮影画像を撮影画像表示部分に表示している場合である。このうち点指標ＰＯＩＮＴ１と正方形指標ＳＱＵＡＲＥ１が同一平面上にあるとする。ユーザは、撮影画像中のＰＯＩＮＴ１かＳＱＵＡＲＥ１のどちらかの指標の周辺をマウスでクリックし、選択する。選択された指標の名前表示は太字に変わる。次に、選択した指標と同一平面に存在する指標の周辺をシフトキーを押しながらマウスを左クリックすることで、同一平面上にある指標をグループ化する。すでに同一平面上に存在するとグループ化されている指標をクリックした場合には、同一グループ内のすべての指標の名前表示が太字に変わる。この状態で、他の指標をシフトキーを押しながら左クリックすると、グループに追加することができる。また、グループ化されている指標の周辺をシフトキーを押しながらマウスを右クリックすると、グループから外れ、シフトキーを押さないで右クリックするとグループ全体が解除される。
【００３４】
（４．指標の位置姿勢算出）
指標の検出同定結果、指標の配置に関する拘束条件をもとに、基準座標系における指標の位置及び姿勢の算出を行う。はじめに透視投影変換について説明する。図６はカメラ座標系及び画面座標系を説明する図である。画面座標系の原点ｏｉは視軸と画像面との交点にとり、画像の水平方向をｘｉ軸、垂直方向をｙｉ軸にとる。また、カメラ座標系の原点ｏｃと画像面との距離（焦点距離）をｆとし、カメラ座標系のｚｃ軸は視軸の反対方向、ｘｃ軸は画像の水平方向と平行に、ｙｃ軸は画像の垂直方向と平行にとる。
【００３５】
透視投影変換により、カメラ座標系上の点ｘｃ＝［ｘｃｙｃｚｃ］ｔは式（２）のように画面座標がｕ＝［ｕｘｕｙ］ｔである点に投影される。
【００３６】
【数１】
2005327103.tif 000003
【００３７】
本実施形態ではレンズ歪みは存在しないか、もしくは補正されていると仮定し、カメラはピンホールカメラであると想定している。式（２）は、図７に示すように、空間中の点と、該点の画像上の投影点とカメラ位置（視点）は同一直線上に存在することを示しており、共線条件式とも呼ばれる。
【００３８】
基準座標系におけるカメラの位置をｔ＝［ｔｘｔｙｔｚ］ｔ、カメラの姿勢（実際には、カメラ座標系に対する基準座標系の姿勢）をω＝［ωｘωｙωｚ］であるとする。ωは３自由度の姿勢表現方法であり、姿勢を回転軸ベクトル及び回転角で表現している。回転角をｒａとすると、ｒａはωによって式（３）のように表される。
【００３９】
【数２】
2005327103.tif 000004
【００４０】
また、回転軸ベクトルをｒａｘｉｓ＝［ｒｘｒｙｒｚ］ｔとすると、ｒａｘｉｓとωの関係は式（４）のように表される。
【００４１】
【数３】
2005327103.tif 000005
【００４２】
ω（回転角ｒａ、回転軸ベクトルｒａｘｉｓ）と３×３回転変換行列Ｒとの関係は式（５）のように表される。
【００４３】
【数４】
2005327103.tif 000006
【００４４】
基準座標系上の点ｘｗ＝［ｘｗｙｗｚｗ］ｔのカメラ座標ｘｃはｔ、Ｒによって式（６）のように表される。
【００４５】
【数５】
2005327103.tif 000007
【００４６】
式（２）、（６）から、透視投影変換によって、基準座標上の点ｘｗ＝［ｘｗｙｗｚｗ］ｔは式（７）のように画像上の点ｕ＝［ｕｘｕｙ］ｔに投影される。
【００４７】
【数６】
2005327103.tif 000008
【００４８】
理想的には、ｔ、ω、ｘｗをもとに式（７）から計算される投影位置（計算位置）と実際に観察される位置（観察位置）は一致する。そのため、計算位置と観察位置の画像の水平方向のずれをＦ、垂直方向のずれをＧ、観察位置をｕｏ＝［ｕｏｘｕｏｙ］ｔとすると、式（８）に示すようにＦ、Ｇは０となる。
【００４９】
【数７】
2005327103.tif 000009
【００５０】
Ｆ、Ｇはカメラ位置ｔ、カメラ姿勢ω、観察対象点の基準座標上での位置ｘｗに関する関数である。ここで、基準座標上での点の位置ｘｗは、指標の種類や、平面上に拘束するかどうかによって異なる変数の関数となるため、Ｆ、Ｇも指標の種類や、平面上に拘束するかどうかによって異なる変数の関数となる。以下では、指標を平面上に拘束する場合としない場合に分けて説明する。
（指標を平面上に拘束しない場合）
点指標の場合には、点指標の基準座標系における位置をｔｐｎ＝［ｔｐｎｘｔｐｎｙｔｐｎｚ］ｔとすると、式（９）に示すように、ｘｗとｔｐｎは同じになる。
【００５１】
【数８】
2005327103.tif 000010
【００５２】
そのため、式（１０）に示すように、Ｆ、Ｇはカメラ位置ｔ、カメラ姿勢ω、点指標の基準座標系における位置ｔｐに関する関数となる。
【００５３】
【数９】
2005327103.tif 000011
【００５４】
次に正方形指標の場合について説明する。正方形指標は、基準座標系における位置ｔｓ＝［ｔｓｘｔｓｙｔｓｚ］ｔ及び基準座標系に対する姿勢ωｓ＝［ωｓｘωｓｙωｓｚ］（ωｓに対応した３×３回転変換行列をＲｓとする）で表現される。正方形指標の頂点の正方形指標座標系における位置をｘｓ＝［ｘｓｙｓ０］ｔとする。正方形指標の頂点の基準座標系における位置ｘｗは式（１１）のようにｔｓとωｓ（Ｒｓ）に関する関数となる。
【００５５】
【数１０】
2005327103.tif 000012
【００５６】
そのため、式（１２）に示すように、Ｆ、Ｇは、カメラ位置ｔ、カメラ姿勢ω、正方形指標の位置ｔｓ及び正方形指標の姿勢ωｓの関数となる。
【００５７】
【数１１】
2005327103.tif 000013
【００５８】
（指標を平面上に拘束する場合）
まず平面の定義方法について説明する。図８に示すように、平面を極座標表現ｒ＝［ｒθφ］によって表現する。平面座標系のｚｐ軸は平面の法線方向に平行であり、基準座標系における原点を通過する。平面の姿勢は、ｙ軸まわりの回転θと、ｘ軸まわりの回転φによって定義する。また、ｒは平面と基準座標系の原点ｏｗとの符号付き距離である。平面の姿勢を表す回転行列Ｒｐは式（１３）のように表される。
【００５９】
【数１２】
2005327103.tif 000014
【００６０】
また、平面座標系の原点ｏｐの基準座標系における位置ｔｐｌ＝［ｔｐｌｘｔｐｌｙｔｐｌｚ］ｔは、ｚｐ軸と平面との交点とし、式（１４）のように求める。ただし
【００６１】
【数１３】
2005327103.tif 000015
は平面の法線ベクトルを表す。
【００６２】
【数１４】
2005327103.tif 000016
【００６３】
上述の極座標表現ｒ＝［ｒθφ］で表される平面座標系上の点ｘｐ＝［ｘｐｙｐ０］ｔの基準座標系における位置ｘｗは式（１５）のように表される。式（１５）から、ｘｗはｘｐ、ｒの関数であることがわかる。
【００６４】
【数１５】
2005327103.tif 000017
【００６５】
平面上に拘束される点指標は、平面座標系上の位置ｔｐｌｐ＝［ｔｐｌｐｘｔｐｌｐｙ０］ｔで表される。式（１６）のように、ｘｐはｔｐｌｐと同じになる。
【００６６】
【数１６】
2005327103.tif 000018
【００６７】
そのため、ｘｗはｔｐｌｐ、ｒの関数であり、Ｆ、Ｇは式（１７）に示すように、カメラ位置ｔ、カメラ姿勢ω、点指標の平面上の位置ｔｐｌｐ及び平面のパラメータｒの関数となる。
【００６８】
【数１７】
2005327103.tif 000019
【００６９】
一方正方形指標の場合、平面座標系上の位置ｔｐｌｓ＝［ｔｐｌｓｘｔｐｌｓｙ０］ｔに加えて、正方形指標の法線ベクトルまわりの（すなわち平面座標系のｚｐ軸まわりの）回転θｐｌで表されるため、正方形指標の頂点の正方形指標座標系における位置をｘｓ＝［ｘｓｙｓ０］ｔとすると、その平面座標ｘｐは式（１８）のように表される。
【００７０】
【数１８】
2005327103.tif 000020
【００７１】
よってｘｗはｔｐｌｓ、θｐｌ、ｒの関数であり、Ｆ、Ｇは式（１９）に示すように、カメラ位置ｔ、カメラ姿勢ω、正方形指標の平面上の位置ｔｐｌｓ、正方形指標の平面上での姿勢θｐｌ及び平面のパラメータｒの関数となる。
【００７２】
【数１９】
2005327103.tif 000021
【００７３】
式（１０）、（１２）、（１７）、（１９）はカメラの位置及び姿勢、指標の位置及び姿勢、平面のパラメータについて非線形な方程式となっている。そこで、１次の項までのテイラー展開を用いてカメラの位置及び姿勢、指標の位置及び姿勢、平面のパラメータの近似値の近傍で線形化し、繰返し計算によってカメラの位置及び姿勢、指標の位置及び姿勢、平面のパラメータを求める。
【００７４】
式（２０）、（２１）、（２２）、（２３）は、それぞれ式（１０）、（１２）、（１７）、（１９）を線形化したものである。Δｔｘ、Δｔｙ、Δｔｚはカメラの位置、Δωｘ、Δωｙ、Δωｚはカメラの姿勢、Δｔｐｘ、Δｔｐｙ、Δｔｐｚは平面に拘束されない点指標の位置、Δｔｓｘ、Δｔｓｙ、Δｔｓｚは平面に拘束されない正方形指標の位置、Δωｓｘ、Δωｓｙ、Δωｓｚは平面に拘束されない正方形指標の姿勢、Δｔｐｌｐｘ、Δｔｐｌｐｙは平面に拘束される点指標の平面上での位置、Δｔｐｌｓｘ、Δｔｐｌｓｙは平面に拘束される正方形指標の平面上での位置、Δθｐｌは平面に拘束される正方形指標の平面上での姿勢、Δｒ、Δθ、Δφは平面のパラメータの近似値に対する補正量を表す。
【００７５】
【数２０】
2005327103.tif 000022
【００７６】
【数２１】
2005327103.tif 000023
【００７７】
【数２２】
2005327103.tif 000024
【００７８】
【数２３】
2005327103.tif 000025
【００７９】
ここで式（２０）、（２１）、（２２）、（２３）のＦ０、Ｇ０は定数であり、カメラ位置及び姿勢、点指標の位置または正方形指標の位置及び姿勢、平面パラメータの近似値から求まる指標の投影位置（計算位置）と観察位置ｕｏとの差である。
【００８０】
式（２０）、（２１）、（２２）、（２３）は、ある画像上で観察される一個の点指標または正方形指標の一個の頂点についての観測方程式である。実際には複数の画像上で複数の点指標や正方形指標の頂点が観察されるため、式（２０）、（２１）、（２２）、（２３）は複数得られる。そこで複数の式（２０）、（２１）、（２２）、（２３）を、カメラの位置及び姿勢、平面上に拘束されない点指標の位置、平面上に拘束されない正方形指標の位置及び姿勢、平面上に拘束される点指標の位置、平面上に拘束される正方形指標の位置及び姿勢、平面のパラメータの近似値に対する補正値についての連立方程式として解き、近似値を補正することを繰り返すことによってカメラの位置及び姿勢、平面上に拘束されない点指標の位置、平面上に拘束されない正方形指標の位置及び姿勢、平面上に拘束される点指標の位置、平面上に拘束される正方形指標の位置及び姿勢、平面のパラメータを求める。
【００８１】
図９は、本実施形態における点指標の位置、正方形指標の位置及び姿勢を求める手順を示すフローチャートである。なお、指標を含むシーンの撮影及び撮影画像からの指標抽出及び同定は終了しているものとする。ステップＳ１００では、各画像を撮影したカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値の設定を行う。ステップＳ１１０以降において、ステップＳ１００で設定された近似値を補正するための補正値を求める。ステップＳ１１０では、カメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値から、投影誤差を最小化するような各近似値の補正値を同時に求めるための連立方程式の立式を行う。ステップＳ１２０では、ステップＳ１１０で立式された連立方程式から各近似値の補正値を求める。ステップＳ１３０では、ステップＳ１２０で求めた補正値によって近似値を補正し、新たな近似値とする。ステップＳ１４０では、ステップＳ１３０において補正されたカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータが最適値に収束しているかどうかの判定を行う。以下、各ステップについて詳細に説明する。
【００８２】
ステップＳ１００において、各画像を撮影したカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値を入力する。ここで、撮影された画像の枚数をＮ枚とし、それぞれの画像を撮影したカメラの位置をｔｉ＝［ｔｉｘｔｉｙｔｉｚ］ｔ（ｉ＝１，・・・，Ｎ）、姿勢をωｉ＝［ωｉｘωｉｙωｉｚ］（ｉ＝１，・・・，Ｎ）とする。また、位置を求める点指標のうち、平面上に拘束されない点指標の数をＫｐ１個、平面上に拘束される点指標の数をＫｐ２個とし、それぞれの点指標の位置をｔｐｉ＝［ｔｐｉｘｔｐｉｙｔｐｉｚ］ｔ（ｉ＝１，・・・，Ｋｐ１）、ｔｐｌｐｉ＝［ｔｐｌｐｉｘｔｐｌｐｉｙ］ｔ（ｉ＝１，・・・，Ｋｐ２）とする。また、位置及び姿勢を求める正方形指標のうち、平面上に拘束されない正方形指標の数をＫｓ１個、平面上に拘束される正方形指標の数をＫｓ２個とし、それぞれの正方形指標の位置をｔｓｉ＝［ｔｓｉｘｔｓｉｙｔｓｉｚ］ｔ（ｉ＝１，・・・，Ｋｓ１）、ｔｐｌｓｉ＝［ｔｐｌｓｉｘｔｐｌｓｉｙ］ｔ（ｉ＝１，・・・，Ｋｓ２）とし、姿勢をωｓｉ＝［ωｓｉｘωｓｉｙωｓｉｚ］（ｉ＝１，・・・，Ｋｓ１）、θｐｌｉ（ｉ＝１，・・・，Ｋｓ２）とする。
【００８３】
カメラの位置及び姿勢の近似値は、カメラに磁気センサのような６自由度位置姿勢センサを装着し、センサの出力値から得てもよいし、基準座標系における位置が既知の点と、該点の画像上での投影位置の対応関係から求めてもよい。基準座標系における位置が既知の点を用いる場合には、シーンには位置が既知の点と、求めようとしている配置情報が未知の指標とが混在していることになる。もしくは、後述する指標の位置および姿勢の近似値を利用して、その値を基にしてカメラの位置姿勢の近似値を求めても良い。
【００８４】
また、モーションコントロールカメラのように機械的にカメラの位置姿勢を得てもよいし、あらかじめ位置姿勢がキャリブレーション済みのカメラを用いて撮影し、そのキャリブレーション結果を用いてもよい。
【００８５】
指標の位置及び姿勢の近似値は、巻尺や分度器、測量器などを用いて手動で測定した概略の値を用いてもよいし、本実施形態の方法等で一度推定された値を用いてもよい。平面上に拘束される指標の位置及び姿勢の近似値は、平面座標系での位置及び姿勢を用いる。
【００８６】
平面のパラメータの近似値は、同一平面上にのる指標がすべて点指標であるならば、点指標の位置の近似値から回帰平面を求めることによって得る。また、同一平面上にのる指標に正方形指標が含まれている場合には、正方形指標を該平面であると仮定して正方形指標の位置及び姿勢から求める。さらに、指標の位置及び姿勢の近似値から求めないで、平面のパラメータをユーザが直接指定してもよい。
【００８７】
次にステップＳ１１０において、画像上で観察される指標の数だけ式（２０）、（２１）、（２２）、（２３）の観測方程式を立てる。式（２０）、（２１）、（２２）、（２３）は、一台のカメラの位置及び姿勢の補正値と、一個の点指標の位置の補正値または一個の正方形指標の位置及び姿勢の補正値と、一個の平面のパラメータの補正値についての観測方程式である。ここでは、式（２４）に示すように、Ｎ台のカメラの位置及び姿勢、Ｋｐ１個の平面上に拘束されない点指標の位置、Ｋｐ２個の平面上に拘束される点指標の位置、Ｋｓ１個の平面上に拘束されない正方形指標の位置及び姿勢、Ｋｓ２個の平面上に拘束される正方形指標の位置及び姿勢、Ｐ個の平面のパラメータについての観測方程式を立てる。この場合、未知数となる補正値の数は、（６×Ｎ＋３×Ｋｐ１＋２×Ｋｐ２＋６×Ｋｓ１＋３×Ｋｓ２＋３×Ｐ）個である。
【００８８】
【数２４】
2005327103.tif 000026
【００８９】
画像ｉ（ｉ＝１，・・・，Ｎ）から検出される点指標の数をｄｐｉ個、正方形指標の数をｄｓｉ個とすると、Ｎ枚の画像から検出される点指標の数Ｄｐ、正方形指標のＤｓは式（２５）のようになる。
【００９０】
【数２５】
2005327103.tif 000027
【００９１】
Ｎ枚の画像から検出される点指標の数がＤｐ、正方形指標の数がＤｓ個である場合には、観測方程式（２４）が（Ｄｐ＋４×Ｄｓ）組、つまり２×（Ｄｐ＋４×Ｄｓ）本の観測方程式が立つ。式（２４）の観測方程式の左辺の定数項Ｆ０、Ｇ０を右辺に移項し、連立方程式を立てると、この連立方程式は式（２６）のように行列形式で書くことができる。
【００９２】
【数２６】
2005327103.tif 000028
【００９３】
Ｊはヤコビ行列と呼ばれ、Ｆ、Ｇのカメラ位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータについての偏微分係数を並べたものである。ヤコビ行列Ｊの行数は、観測方程式の数２×（Ｄｐ＋４×Ｄｓ）個であり、列数は未知数の数（６×Ｎ＋３×Ｋｐ１＋２×Ｋｐ２＋６×Ｋｓ１＋３×Ｋｓ２＋３×Ｐ）個である。Δは補正値ベクトルである。補正値ベクトルの要素数は未知数の数（６×Ｎ＋３×Ｋｐ１＋２×Ｋｐ２＋６×Ｋｓ１＋３×Ｋｓ２＋３×Ｐ）個である。Ｅは誤差ベクトルであり、近似値による投影位置の計算位置と観察位置ｕｏの差−Ｆ０、−Ｇ０を要素として持つ。Ｅの要素数は、観測方程式の数２×（Ｄｐ＋４×Ｄｓ）個である。
【００９４】
なお、基準座標系における位置が既知である点指標または位置及び姿勢が既知の正方形指標を同時に撮影することによって、基準座標系の原点、スケール、姿勢を明示的に指定することができる。これらの指標についての式（２４）の方程式では、指標の位置及び指標についての偏微分係数の値はすべて０となる。基準座標系の原点、スケール、姿勢を明示的に指定するためには、点指標であれば位置が既知の点指標を３個用いればよく、正方形指標であれば位置及び姿勢が既知の正方形指標を１個用いればよい。
【００９５】
次に、ステップＳ１２０においてカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値に対する補正値を求める。補正値は式（２６）から求める。ヤコビ行列Ｊが正方行列である場合には，式（２６）の両辺にＪの逆行列をかけることで補正値ベクトルΔを求める。Ｊが正方行列でない場合には、式（２７）に示すように最小二乗法によって補正値ベクトルΔを求める。
【００９６】
【数２７】
2005327103.tif 000029
【００９７】
ここで、ヤコビ行列Ｊの各要素の求め方について説明する。まずＦ＝［ＦＧ］ｔとおく。式（７）、（８）よりＦ、Ｇは式（２８）のように書けるので、Ｆ、Ｇはｘｃ、ｙｃ、ｚｃの関数である。
【００９８】
【数２８】
2005327103.tif 000030
【００９９】
Ｆ、Ｇのｘｃ、ｙｃ、ｚｃによる偏微分係数を各要素に持つヤコビ行列
【０１００】
【数２９】
2005327103.tif 000031
は、式（２９）のように書ける。
【０１０１】
【数３０】
2005327103.tif 000032
【０１０２】
ここで、ｘｃ、ｙｃ、ｚｃが変数ｓ１、ｓ２、・・・、ｓｍの関数であるならば、Ｆ、Ｇもまたｓ１、ｓ２、・・・、ｓｍの関数である。Ｆ、Ｇのｓ１、ｓ２、・・・、ｓｍによる偏微分係数を各要素に持つヤコビ行列ＪＦｓは、式（３０）のように分解することができる。
【０１０３】
【数３１】
2005327103.tif 000033
【０１０４】
ｓをカメラ位置、カメラ姿勢、指標位置、指標姿勢、平面パラメータに置き換えて、
【０１０５】
【数３２】
2005327103.tif 000034
を求めることにより、Ｆ、Ｇのカメラ位置、カメラ姿勢、指標位置、指標姿勢、平面パラメータについての偏微分係数を求めることができる。
【０１０６】
Ｆ、Ｇのカメラ位置ｔｘ、ｔｙ、ｔｚによる偏微分係数を各要素に持つヤコビ行列ＪＦｔは、式（３１）のように書ける。
【０１０７】
【数３３】
2005327103.tif 000035
【０１０８】
ただし、姿勢ωと３×３回転変換行列Ｒの関係は式（５）で表される。
【０１０９】
Ｆ、Ｇのカメラ姿勢ωｘ、ωｙ、ωｚによる偏微分係数を各要素に持つヤコビ行列ＪＦωは、式（３２）のように分解して書ける。
【０１１０】
【数３４】
2005327103.tif 000036
【０１１１】
ここで、
【０１１２】
【数３５】
2005327103.tif 000037
は式（３３）に示すようなヤコビ行列である。
【０１１３】
【数３６】
2005327103.tif 000038
【０１１４】
また、
【０１１５】
【数３７】
2005327103.tif 000039
は式（３４）に示すようなヤコビ行列である。
【０１１６】
【数３８】
2005327103.tif 000040
【０１１７】
【数３９】
2005327103.tif 000041
【０１１８】
【数４０】
2005327103.tif 000042
【０１１９】
Ｆ、Ｇの平面上に拘束されない点指標の位置ｔｐｘ、ｔｐｙ、ｔｐｚによる偏微分係数を各要素に持つヤコビ行列
【０１２０】
【数４１】
2005327103.tif 000043
は、式（３７）のように書ける。
【０１２１】
【数４２】
2005327103.tif 000044
【０１２２】
Ｆ、Ｇの平面上に拘束されない正方形指標の位置ｔｓｘ、ｔｓｙ、ｔｓｚによる偏微分係数を各要素に持つヤコビ行列
【０１２３】
【数４３】
2005327103.tif 000045
は、式（３８）のように書ける。
【０１２４】
【数４４】
2005327103.tif 000046
【０１２５】
Ｆ、Ｇの平面上に拘束されない正方形指標の姿勢ωｓによる偏微分係数を各要素に持つヤコビ行列
【０１２６】
【数４５】
2005327103.tif 000047
は、式（３９）のように分解することができる。
【０１２７】
【数４６】
2005327103.tif 000048
【０１２８】
【数４７】
2005327103.tif 000049
は、式（３４）〜（３６）と同様にして求めることができる。
【０１２９】
次に、Ｆ、Ｇの平面上に拘束される点指標の位置ｔｐｌｐｘ、ｔｐｌｐｙによる偏微分係数を各要素に持つヤコビ行列
【０１３０】
【数４８】
2005327103.tif 000050
は、式（４０）のように書ける。
【０１３１】
【数４９】
2005327103.tif 000051
は式（３８）内と同様に求められる。
【０１３２】
【数５０】
2005327103.tif 000052
【０１３３】
Ｆ、Ｇの平面上に拘束される正方形指標の位置ｔｐｌｓｘ、ｔｐｌｓｙによる偏微分係数を各要素に持つヤコビ行列
【０１３４】
【数５１】
2005327103.tif 000053
は、式（４１）のように書ける。
【０１３５】
【数５２】
2005327103.tif 000054
【０１３６】
Ｆ、Ｇの平面上に拘束される正方形指標の姿勢θｐｌによる偏微分係数を各要素に持つヤコビ行列
【０１３７】
【数５３】
2005327103.tif 000055
は、式（４２）のように書ける。
【０１３８】
【数５４】
2005327103.tif 000056
【０１３９】
Ｆ、Ｇの平面のパラメータｒ、θ、φによる偏微分係数を各要素に持つヤコビ行列ＪＰｒは、式（４３）のように書ける。
【０１４０】
【数５５】
2005327103.tif 000057
【０１４１】
次にステップＳ１４０において、収束判定を行う。収束判定は、補正値の絶対値が指定した閾値を下回る、投影位置の計算位置と観察位置の差が指定した閾値を下回る、投影位置の計算位置と観察位置の差が極小となる、などの条件を判定することにより行う。本ステップで収束していると判定された場合、指標の位置及び姿勢推定を終了し、収束していない場合は、ステップＳ１１０に戻り、補正されたカメラの位置及び姿勢、点指標の位置、正方形指標の位置及び姿勢、平面のパラメータの近似値をもとに再度連立方程式を立式する。
【０１４２】
以上、説明したように、本実施形態では、指標が配置されたシーンの画像を多数撮影し、撮影された画像から指標を検出・同定し、同一平面上に存在する複数の指標を指定し、画像上の指標観察位置と、指標が同一平面上に存在するという拘束条件とから、撮影したカメラの位置及び姿勢、平面上に拘束されない指標の位置及び姿勢、平面上に拘束される指標の位置及び姿勢、平面のパラメータの近似値に対する補正値を求めて近似値を補正する操作を繰返し行うことによって、拘束条件を満たすように指標の位置及び姿勢を求める。
【０１４３】
なお、本実施形態では、平面パラメータはすべて未知であるとしたが、平面パラメータが既知であってもよい。例えば、基準座標系におけるｘｙ平面など、容易に平面パラメータを求めることができる場合は、平面パラメータを既知とすることができる。平面パラメータが既知の場合には、式（２２）の平面上に拘束される点指標の観測方程式は式（４４）のようになる。
【０１４４】
【数５６】
2005327103.tif 000058
【０１４５】
また、式（２３）の平面上に拘束される正方形指標の観測方程式は式（４５）のようになる。
【０１４６】
【数５７】
2005327103.tif 000059
【０１４７】
＜第二実施形態＞
第一実施形態では、指標の配置に関する拘束条件として、複数の指標が同一平面上に存在するという拘束条件を与えたときに、拘束条件を満たすように指標の位置姿勢を推定方法について説明した。第二実施形態では、複数の指標が同一平面上に存在し、かつそれらの平面のなす角度が既知である場合について説明する。
【０１４８】
なお、ここでは平面が二つ存在し、２平面間のなす角度が既知である場合について説明するが、平面の数は二つに限定されるものではない。
【０１４９】
図１０は、指標が拘束される平面が二つ存在する場合を示している。平面１は第一実施形態で説明したように、基準座標系において極座標表現ｒ１＝［ｒ１θ１φ１］によって表現される。
【０１５０】
一方、平面２も極座標表現ｒ２＝［ｒ２θ２φ２］によって表現されるが、θ２、φ２は、平面１に対する相対姿勢であるとする。つまり、平面１をｙｐ１軸まわりにθ２だけ回転した後、ｘｐ１軸まわりにφ２だけ回転したときの姿勢が、平面２の姿勢であるとする。ｒ２は、基準座標系の原点と、平面２との符号付き距離である。このとき、平面１、平面２の姿勢を表す回転行列Ｒｐ１、Ｒｐ２はそれぞれ式（４７）、（４８）のように書ける。
【０１５１】
【数５８】
2005327103.tif 000060
【０１５２】
【数５９】
2005327103.tif 000061
【０１５３】
ここで、平面１の法線ベクトル
【０１５４】
【数６０】
2005327103.tif 000062
と、平面２の法線ベクトル
【０１５５】
【数６１】
2005327103.tif 000063
のなす角度がαであることから、式（４９）が成り立つ。
【０１５６】
【数６２】
2005327103.tif 000064
【０１５７】
式（４９）から、θ２、αよりφ２を求めることができる。
【０１５８】
平面１上に拘束される点指標、正方形指標の観測方程式は、第一実施形態の式（２２）、（２３）と同じである。一方、平面２はｒ２、θ２の関数となるため、平面２上に拘束される点指標、正方形指標の観測方程式は、それぞれ式（５０）、式（５１）のようになる。
【０１５９】
【数６３】
2005327103.tif 000065
【０１６０】
【数６４】
2005327103.tif 000066
【０１６１】
本実施形態において指標の位置及び姿勢を求める際には、観測方程式（２２）、（２３）、（５０）、（５１）をもとに連立方程式を立て、カメラの位置及び姿勢、指標の位置及び姿勢、平面１の３パラメータ、平面２の２パラメータの近似値に対する補正値を求め、繰返し計算によって近似値を最適化する。なお、式（４９）からφ２を求める際には符号が異なる２つの解が得られるが、繰返し計算ごとに求まるθ２に対して、２つφ２の解のうち投影誤差が小さくなる方を解として採用する。
【０１６２】
以上説明したように、本実施形態では、指標の配置に関する拘束条件として、複数の指標が同一平面上に存在し、かつそれらの平面のなす角度が既知である場合に、拘束条件を満たすように指標の位置及び姿勢を求めることができる。
【０１６３】
＜第三実施形態＞
第三実施形態では、指標の配置に関する拘束条件として、複数の平面図形様の指標の法線ベクトルの方向が同じである場合の、指標の位置及び姿勢推定方法について説明する。
【０１６４】
図１１は本実施形態を適用可能な指標配置を示す図である。図では、正方形指標が４個配置されており、４個の正方形指標の法線ベクトル、すなわち各正方形指標のｚｓ軸の方向が同じであるとする。
【０１６５】
正方形指標の法線ベクトルの向きをθ、φで表す。法線ベクトルは、基準座標系のｚ軸の単位ベクトルをｘ軸まわりにθだけ回転し、次にｙ軸まわりにφだけ回転したベクトルであるとする。θ、φは、法線ベクトルが同じ方向である複数の正方形指標にとって共通の値を持つ。個々の正方形指標のｚ軸まわりの回転をψとすると、正方形指標の基準座標系に対する姿勢Ｒｓは式（５２）によって表される。
【０１６６】
【数６５】
2005327103.tif 000067
【０１６７】
個々の正方形指標の基準座標系における位置をｔｓ＝［ｔｓｘｔｓｙｔｓｚ］ｔとすると、正方形指標上の点ｘｓ＝［ｘｓｙｓ］ｔの基準座標系における位置ｘｗは式（５３）によって表される。
【０１６８】
【数６６】
2005327103.tif 000068
【０１６９】
本実施形態において、正方形指標の各頂点の基準座標系における位置は、法線ベクトルの向きθ、φ、正方形指標のｚ軸まわりの姿勢ψ、正方形指標の基準座標系における位置ｔｓの関数となる。よって、法線ベクトルが拘束される場合の正方形指標の観測方程式は式（５４）のようになる。
【０１７０】
【数６７】
2005327103.tif 000069
【０１７１】
本実施形態において指標の位置及び姿勢を求める際には、複数の観測方程式（５４）をもとに連立方程式を立て、カメラの位置及び姿勢、指標の位置及び姿勢、法線ベクトルの向きθ、φの近似値に対する補正値を求め、繰返し計算によって近似値を最適化する。
【０１７２】
以上説明したように、本実施形態では、指標の配置に関する拘束条件として、複数の指標の法線ベクトルの向きが共通である場合に、拘束条件を満たすように指標の位置及び姿勢を求めることができる。
【０１７３】
＜第四実施形態＞
第一、第二、第三実施形態では、指標の配置に関する拘束条件の種類を一つとして、指標の位置及び姿勢を求めていた、第四実施形態では、複数の拘束条件が存在する場合の、指標の位置及び姿勢を求める方法について説明する。
【０１７４】
図１２は、本実施形態を適用可能な指標配置を示す図である。図は、点指標１、正方形指標１が同一平面（平面１）上に存在し、正方形指標２、正方形指標３の法線ベクトルの向きが共通であり、点指標２、正方形指標４は配置に関して拘束条件がない場合を示している。
【０１７５】
図１２に示すシーンを撮像装置で撮影すると、点指標１については観測方程式（２２）が、正方形指標１については観測方程式（２３）が、正方形指標２、３については観測方程式（５４）が、点指標２については観測方程式（２０）が、正方形指標については観測方程式（２１）が成り立つ。式（２０）〜（２３）、（５４）は一台のカメラの位置及び姿勢の補正値と、一個の指標の位置及び姿勢、一個の平面のパラメータ、一個の法線ベクトルの向きの補正値についての観測方程式である。本実施形態では、式（５５）に示すように、Ｎ台のカメラの位置及び姿勢、Ｋｐ１個の平面上に拘束されない点指標の位置、Ｋｐ２個の平面上に拘束される点指標の位置、Ｋｓ１個の平面上に拘束されない正方形指標の位置及び姿勢、Ｋｓ２個の平面上に拘束される正方形指標の位置及び姿勢、Ｋｓ３個の法線ベクトルが拘束される正方形指標の位置及び姿勢、Ｐ個の平面のパラメータ、Ｖ個の法線ベクトルの向きの補正値についての観測方程式を立てる。
【０１７６】
【数６８】
2005327103.tif 000070
【０１７７】
撮影画像上で検出された点指標及び、正方形指標の各頂点について式（５５）の観測方程式を立て連立方程式として解くことにより、カメラ位置ｔｉ、カメラ姿勢ωｉ、配置に関する拘束のない点指標の位置ｔｐｉ、配置に関する拘束のない正方形指標の位置ｔｓｉ、姿勢ωｓｉ、平面上に拘束される点指標の位置ｔｐｌｐｉ、平面上に拘束される正方形指標の位置ｔｐｌｓｉ、姿勢θｐｌｉ、法線ベクトルが拘束される正方形指標の位置ｔｖｓｉ、姿勢ψｉ、平面のパラメータｒｉ、法線ベクトルの向きθｖｉ、φｖｉの近似値についての補正値を同時に求め、繰返し演算によって近似値の補正を繰返して最適化する。
【０１７８】
また、本実施形態の指標配置条件を設定するためのＧＵＩは、指標のグループ化と各グループの拘束条件の種類を設定することができる。すなわち、実施形態１において図５を用いて説明したＧＵＩにおいて、さらに拘束条件の種類を設定するためのＵＩを有している。
【０１７９】
以上説明したように、本実施形態では、指標の配置に関する拘束条件が複数存在する場合に、すべての拘束条件を満たすように指標の位置及び姿勢を求めることができる。
【０１８０】
＜変形例１＞
図１３に示すように、指標を円筒の側面部分に拘束してもよい。円筒は、円筒の中心に円筒座標系の原点をとり、図のように円筒座標系をとる。円筒の姿勢はｘ軸まわりの回転θとｙ軸まわりの回転φで表される。円筒の姿勢を表す回転行列Ｒｃｙｌは式（５６）のように表される。
【０１８１】
【数６９】
2005327103.tif 000071
【０１８２】
円筒の原点の基準座標系での位置がｔｃｙｌ＝［ｔｃｙｌｘｔｃｙｌｙｔｃｙｌｚ］ｔとすると、円筒座標系上の点ｘｃｙｌ＝［ｘｃｙｌｙｃｙｌｚｃｙｌ］ｔの基準座標系の位置ｘｗは式（５７）のようになる。
【０１８３】
【数７０】
2005327103.tif 000072
【０１８４】
また、円筒の側面上の点は、ｚ軸まわりの回転角βとｚ座標ｄ、既知の円筒半径ｒｃｙｌによって、円筒座標系において式（５８）のように表せる。
【０１８５】
【数７１】
2005327103.tif 000073
【０１８６】
円筒の側面に拘束される指標の投影位置は、カメラの位置及び姿勢、円筒の位置及び姿勢、円筒上での指標の位置β、ｄについての関数となるので、これらの補正値を求める観測方程式を解くことで、円筒の側面上に拘束された指標の位置を求めることができる。
【０１８７】
＜変形例２＞
図１４に示すように、指標を球面上に拘束してもよい。球の中心に球座標系をとる。球は方向性を持たないため、基準座標系に対する位置だけで表される。球座標系の原点の基準座標系での位置がｔｓｐｈ＝［ｔｓｐｈｘｔｓｐｈｙｔｓｐｈｚ］ｔとすると、球座標系上の点ｘｓｈｐ＝［ｘｓｈｐｙｓｈｐｚｓｈｐ］ｔの基準座標系の位置ｘｗは式（５９）のようになる。
【０１８８】
【数７２】
2005327103.tif 000074
【０１８９】
また、球面上の点は、既知の円筒半径ｒｓｐｈと角度θ、φによって、球座標系において式（６０）のように表せる。
【０１９０】
【数７３】
2005327103.tif 000075
【０１９１】
球面に拘束される指標の投影位置は、カメラの位置及び姿勢、球の位置、球面上での指標の位置θ、φについての関数となるので、これらの補正値を求める観測方程式を解くことで、球面上に拘束された指標の位置を求めることができる。
【０１９２】
＜変形例３＞
図１５に示すように、指標を直線上に拘束してもよい。直線の通る点ｔｌ＝［ｔｌｘｔｌｙｔｌｚ］ｔ、方向ベクトルをｄ＝［ｄｘｄｙｄｚ］ｔとすると、直線上にある指標の基準座標系の位置は式（６１）のように表せる。ｓは直線上での指標位置を表す変数である。
【０１９３】
【数７４】
2005327103.tif 000076
【０１９４】
直線上に拘束される指標の投影位置は、カメラの位置及び姿勢、直線の通る点ｔｌ、直線の方向ベクトルｄ、直線上での指標の位置ｓについての関数となるので、これらの補正値を求める観測方程式を解くことで、球面上に拘束された指標の位置を求めることができる。
【０１９５】
＜変形例３＞
上記実施形態では、指標の配置情報の補正方法として、観測方程式をテイラー展開しその１次の項までを利用して繰り返し補正していく方法を述べた。これは、ある繰り返しの一過程において、局所的に観測方程式を線形近似し、誤差０となると仮定した場合の未知パラメータの補正量を推定する方法であり、一般にニュートン法と呼ばれている方法と等価な方法である。ニュートン法は非線形方程式を数値演算により求解する代表的な方法であるが、上記実施形態に利用する繰り返し演算方法はこの方法に限ったものではない。例えば、ニュートン法と同様に周知の技術として知られているＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法のように、一過程において推定される未知パラメータの補正量のばらつきに基づいて、補正量を動的に変更してもよい。また、テイラー展開した結果をより高次の項まで考慮して、繰り返し演算を行うような方法であっても良い。本発明の本質は、配置情報量の異なる複数種類の指標が混在して存在している場合であっても、それぞれの種類の指標を有効に利用して最適な解を求めることにあり、数値解法としての繰り返し演算の具体的な方法によって本質が失われるものではない。
【図面の簡単な説明】
【０１９６】
【図１】従来の位置姿勢計測装置を説明する図である。
【図２】第一実施形態における指標位置姿勢推定装置２の機能構成を示すブロック図である。
【図３】第一実施形態における指標位置姿勢推定装置のハードウェア構成を示すブロック図である。
【図４】第一実施形態で用いられる指標を説明する図である。
【図５】同一平面上に存在する複数の指標を設定するＧＵＩを説明する図である。
【図６】カメラ座標系及び画面座標系を説明する図である。
【図７】共線条件式を説明する図である。
【図８】平面の定義方法を説明する図である。
【図９】第一実施形態における点指標の位置、正方形指標の位置及び姿勢を求める手順を示すフローチャートである。
【図１０】指標が拘束される平面が二つ存在する場合を示す図である。
【図１１】第三実施形態を適用可能な指標配置を示す図である。
【図１２】第四実施形態を適用可能な指標配置を示す図である。
【図１３】指標が円筒の側面上に拘束されることを説明する図である。
【図１４】指標が球面上に拘束されることを説明する図である。
【図１】
2005327103.tif 000077
【図２】
2005327103.tif 000078
【図３】
2005327103.tif 000079
【図４】
2005327103.tif 000080
【図５】
2005327103.tif 000081
【図６】
2005327103.tif 000082
【図７】
2005327103.tif 000083
【図８】
2005327103.tif 000084
【図９】
2005327103.tif 000085
【図１０】
2005327103.tif 000086
【図１１】
2005327103.tif 000087
【図１２】
2005327103.tif 000088
【図１３】
2005327103.tif 000089
【図１４】
2005327103.tif 000090