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