位置姿勢計測方法及び装置
【課題】 誤検出指標や検出精度の低い指標による影響を減少させ、高精度かつより安定な撮像装置の位置姿勢計測を行う。
【解決手段】 指標検出部２０３０は、物体上に配置または設定された指標を撮像画像から検出する。評価量算出部２０６０は評価量を、この指標の画像上での２次元幾何特徴及び／又はこの指標の３次元空間中における撮像装置２０１０と指標との間の関係を表す３次元幾何特徴を利用して算出する。信頼度算出部２０７０は、算出した指標の評価量に応じてこの指標の信頼度を算出する。位置姿勢算出部２０８０は、算出された指標の信頼度と、検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、物体または撮像装置２０１０の位置姿勢を求める。
【解決手段】 指標検出部２０３０は、物体上に配置または設定された指標を撮像画像から検出する。評価量算出部２０６０は評価量を、この指標の画像上での２次元幾何特徴及び／又はこの指標の３次元空間中における撮像装置２０１０と指標との間の関係を表す３次元幾何特徴を利用して算出する。信頼度算出部２０７０は、算出した指標の評価量に応じてこの指標の信頼度を算出する。位置姿勢算出部２０８０は、算出された指標の信頼度と、検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、物体または撮像装置２０１０の位置姿勢を求める。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、視点位置姿勢計測の精度向上と安定性を向上させる技術に関する。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない融合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実画像は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像を重畳描画することにより生成される。そして、複合現実感システムで利用される画像表示装置は、例えば、ビデオシースルー方式によって実現される。ここで、仮想空間の画像とは、例えば、コンピュータ・グラフィックスにより描画された仮想物体や文字情報等である。
【０００３】
複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置の位置姿勢と称する）を求める問題に帰結される。
【０００４】
この問題を解決する方法としては次のような試みがなされている。即ち、環境中や対象物体上に、対象の座標系における配置が既知であるような複数の指標を設置あるいは設定する。そして、既知の情報であるその指標の対象座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、対象座標系に対する撮像装置の位置姿勢を求める（非特許文献１を参照）。
【０００５】
また、撮像装置に慣性センサを装着し、センサ計測値を利用することにより、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている。例えば、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用する方法が提案されている。同推定結果を画像に基づく位置及び姿勢算出の初期値として用いる方法も提案されている。さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いる方法も提案されている（特許文献１、非特許文献２を参照）。
【０００６】
従来の画像情報を用いた位置合わせ手法は、指標の検出結果がすべて正しいことを前提としていた。さらに、指標検出結果はすべて均等な重みとして計算されていた。そのため、誤検出や検出精度の低い指標による影響を大きく受け、正しい位置姿勢計測が行えない場面が起こり得る。
【０００７】
そこで、Ｍ推定のような統計的推定手法を導入し、検出した指標（特徴点）の画像上での観察座標と撮像装置の位置姿勢や指標の位置から推定した指標の画像座標（再投影座標）との間の誤差（再投影誤差）を求る。そして、この誤差をもとに検出指標の信頼度を算出し、誤検出指標を排除することや、その影響を下げる手法が近年提案されている（非特許文献３を参照）。
【特許文献１】特開２００５−３３３１９号公報
【非特許文献１】佐藤, 田村：“複合現実感における位置合わせ手法”, 画像の認識・理解シンポジウム（ＭＩＲＵ ２００２）論文集Ｉ, 情報処理学会シンポジウムシリーズ、 ｖｏｌ．２００２、 ｎｏ．１１、 ｐｐ．Ｉ．６１−Ｉ．６８， Ｊｕｌｙ ２００２
【非特許文献２】藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９、 ｎｏ．５７４、 ｐｐ．１−８）
【非特許文献３】佐藤，神原，横矢，竹村：“マーカと自然特徴点の追跡による動画像からのカメラ移動パラメータの復元”，電子情報通信学会論文誌，Ｄ−III，ｖｏｌ．Ｊ８６−Ｄ−II，ｎｏ．１０，ｐｐ．１４３１−１４４０，２００３．
【０００８】
近年、現実空間と仮想空間の繋ぎ目のない融合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感システムで利用される画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像を重畳描画してそれを表示するビデオシースルー方式によって主に実現される。ここで、仮想空間の画像とは、例えば、コンピュータ・グラフィックスにより描画された仮想物体や文字情報等である。
【０００９】
複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置の位置姿勢と称する）を求める問題に帰結される。
【００１０】
この問題を解決する方法としては次のような試みがなされている。即ち、環境中や対象物体上に、対象の座標系における配置が既知であるような複数の指標を設置あるいは設定する。そして、既知の情報であるその指標の対象座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、対象座標系に対する撮像装置の位置姿勢を求める（非特許文献１を参照）。
【００１１】
また、撮像装置に慣性センサを装着し、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用したり、同推定結果を画像に基づく位置及び姿勢算出の初期値として用いたり、さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いることで、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている（特許文献１、非特許文献２を参照）。
【００１２】
従来の画像情報を用いた位置合わせ手法は、指標の検出結果がすべて正しいことを前提としていた。さらに、指標検出結果はすべて均等な重みとして計算されていた。そのため、誤検出や検出精度の低い指標による影響を大きく受け、正しい位置姿勢計測が行えない場面が起こり得る。
【００１３】
そこで、Ｍ推定のような統計的推定手法を導入し、検出した指標（特徴点）の画像上での観察座標と撮像装置の位置姿勢や指標の位置から推定した指標の画像座標（再投影座標）との間の誤差（再投影誤差）を求る。そして、この誤差をもとに検出指標の信頼度を算出し、誤検出指標を排除することや、その影響を下げる手法が近年提案されている（非特許文献３を参照）。
【特許文献１】特開２００５−３３３１９（ジャイロ＆ＩＣＰ位置あわせ 内山，佐藤）
【非特許文献１】佐藤, 田村：“複合現実感における位置合わせ手法”, 画像の認識・理解シンポジウム（ＭＩＲＵ ２００２）論文集Ｉ, 情報処理学会シンポジウムシリーズ、 ｖｏｌ．２００２、 ｎｏ．１１、 ｐｐ．Ｉ．６１−Ｉ．６８， Ｊｕｌｙ ２００２
【非特許文献２】藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９、 ｎｏ．５７４、 ｐｐ．１−８）
【非特許文献３】佐藤，神原，横矢，竹村：“マーカと自然特徴点の追跡による動画像からのカメラ移動パラメータの復元”，電子情報通信学会論文誌，Ｄ−III，ｖｏｌ．Ｊ８６−Ｄ−II，ｎｏ．１０，ｐｐ．１４３１−１４４０，２００３．
【発明の開示】
【発明が解決しようとする課題】
【００１４】
しかし、再投影誤差の統計量のみに基づいて信頼度を算出し、指標に重み付けすることで誤検出を排除または検出精度（検出すべき画像座標の正確さ）の低い指標の影響を軽減する試みは、必ずしも有効であるとはいえない。なぜなら、上記の手法は多数の指標が正しく検出され、例外的に現れた誤検出指標を排除することに関しては有効であるが、正しい指標より誤検出した指標の方が多い場合では、検出精度の低い指標の影響を受ける可能性があるからである。また、同じ画像で撮影された指標であっても、マーカの配置や撮影時の状況に応じて検出精度の低い指標が含まれる場合があるため位置姿勢計測精度の低下につながるという課題がある
本発明は上記問題に鑑みてなされたものである。即ち、本発明は、二次元形状を持つ指標（円マーカや多角形マーカ等）特有の評価量に応じて重み付けを変え、検出精度の低い指標による影響を排除又は減少させ、高精度且つより安定な撮像装置の位置姿勢計測を行うことを目的とする。
【課題を解決するための手段】
【００１５】
上記目的を達成するために、本発明は以下の構成をを有する。
【００１６】
本願請求項１記載の発明は、撮像装置によって撮像された撮像画像を入力する画像入力工程と、物体上に配置または設定された指標を前記撮像画像から検出する指標検出工程と、前記撮像画像から検出されている夫々の指標の信頼度を算出する手がかりとして用いるための評価量を算出する指標評価量算出工程と、前記指標評価量算出工程で算出した指標の評価量に応じて当該指標の信頼度を算出する信頼度算出工程と、前記信頼度算出工程で算出された指標の信頼度と、前記指標検出工程で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める位置姿勢算出工程とを有し、前記評価量は、前記指標の画像上での２次元幾何特徴および／または前記指標の３次元空間中における前記撮像装置と前記指標との間の関係を表す３次元幾何特徴に基づくことを特徴とする。
【発明の効果】
【００１７】
本発明によれば、検出精度の低い指標の影響を下げることができるので、高精度かつより安定的に撮像装置の位置姿勢計測を行うことができる。
【発明を実施するための最良の形態】
【００１８】
以下添付図面を参照して、本発明をその好適な実施形態に従って詳細に説明する。
【００１９】
［第１の実施形態］
本実施形態に係る撮像装置の位置姿勢計測装置は、検出した２次元形状を持つ指標の評価量に応じて信頼度を変えて重み付けを行い、精度の低い指標の排除または影響を下げ、撮像装置の位置姿勢計測結果の精度を向上させる。以下、本実施形態に係る指標の評価量に応じた重み付けによる撮像装置の位置姿勢測定方法について説明する。
【００２０】
図１は、本実施形態に係る撮像装置の位置姿勢計測装置２０００の概略構成を示している。位置姿勢計測装置２０００は、画像入力部２０２０、指標検出部２０３０、撮像装置概略位置姿勢入力部２０４０、データ管理部２０５０、評価量算出部２０６０、信頼度算出部２０７０、位置姿勢算出部２０８０によって構成されている。また、画像入力部２０２０には、計測対象である撮像装置２０１０が接続されている。
【００２１】
環境中または対象物体上には、指標が複数配置されている。ここで、物体に配置された指標をＰｋ（ｋ＝１，，，Ｋo）で表す。Ｋoは配置された指標の数（図３（ａ）の例ではＫ０＝３）である。また、図３（ｂ）で示すような多角形の指標Ｐｋは頂点ｐｋｉ（ｉ＝１，，，Ｎｋ）を含む。Ｎｋは、指標Ｐｋを構成する頂点の総数（本実施形態の場合はＮｋ＝４）を表す。また、図３（ｃ）に示すように、それぞれの頂点の相対的な位置関係が既知の点ｐｋｉ（ｉ＝１，，，Ｎｋ）によって構成される指標であってもよい。
【００２２】
以下では、計測の基準とする座標系（ここでは、環境中の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）を世界座標系と呼び、世界座標系における各指標の位置姿勢は既知であるとする。また、環境中に配置された夫々の指標を構成する各頂点は、世界座標系において位置が既知であるとする。
【００２３】
指標Ｐｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各頂点が識別可能であるような指標であれば、何れの形態であってもよい。例えば、図３（ａ）、（ｂ）に示すような四角形指標は内部に識別子を表すパターンを持ち、一意に同定可能である。このような指標を検出する際には、撮影画像に２値化処理を施した後にラベリング処理を行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として抽出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって候補領域が指標領域であるかを判定する。そして、指標領域と判定された場合は、内部のパターンを読み出すことによって指標の方向と識別子を取得する。
【００２４】
撮像装置２０１０によって撮影された画像は、画像入力部２０２０を介して指標検出部２０３０に入力される。
【００２５】
指標検出部２０３０は、画像入力部２０２０から画像を入力し、入力された画像中に撮影されている指標Ｐｋｎを構成する各頂点ｐｋｎｉの画像座標を検出する。
【００２６】
さらに、指標検出部２０３０は、検出された各々の指標Ｐｋnの識別（同定）を行う。
【００２７】
そして、世界座標系における指標の位置姿勢ＭＷＭや各頂点の世界座標ｘｗｐｋｎｉ（指標に関する既知の情報として予め保持している）をデータ管理部２０５０から入力する。
【００２８】
そして、検出された夫々の指標の識別子ｋnと世界座標系における位置姿勢ＭＷＭ、各頂点pｋnｉの画像座標ｕｐｋnｉと世界座標ｘｗｐｋｎｉの組を要素として有するリスト（以下、データリストと称す）を作成し、データ管理部２０５０に出力する。ここで、n（n＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。また、Ｎ個の指標から成る構成点（頂点）の総数をＮTotalとする。例えば図１の場合は、識別子が１、２、３である四角形形状の指標が撮影される場合を示しているので、Ｎ＝３である。また、識別子ｋ１＝１、ｋ２＝２、ｋ３＝３と、これらの指標の位置姿勢、各頂点の画像座標ｕｐｋ１ｉ、ｕｐｋ２ｉ、ｕｐｋ３ｉ、（ｉ＝１、２、３、４）及び世界座標ｘｗｐｋ１ｉ、ｘｗｐｋ２ｉ、ｘｗｐｋ３ｉ、（ｉ＝１、２、３、４）が出力される。ＮTotalは、１２（＝３×４）になる。
【００２９】
ここで、ある三次元座標系Bに対する三次元の座標系Aの位置を表す三次元ベクトルをｔ、姿勢を表す3×3回転行列をＲとする。この場合、A上での位置がｘA（三次元ベクトル）で表される点のB上での座標ｘB（三次元ベクトル）は、下記の式１に示す４×４行列ＭＢＡを使って同次座標表現によって下記の式２のように表される。
【００３０】
【数１】
【００３１】
【数２】
【００３２】
本実施形態においては、ＭＢＡを座標系Ｂに対する座標系Ａの位置姿勢を表す手段として用いる。
【００３３】
撮像装置概略位置姿勢入力部２０４０は、世界座標系における撮像装置２０１０の位置姿勢の概略値ＭＷＣを求め、これをデータ管理部２０５０へと出力する。撮像装置の概略位置姿勢としては、例えば、６自由度位置姿勢センサを撮像装置２０１０に装着させてその出力値を用いてもよい。また、３自由度姿勢センサを撮像装置２０１０に装着することによって撮像装置２０１０の姿勢を計測し、指標検出部２０３０で得たデータリストをデータ管理部２０５０から入力し、これらの情報に基づいて求めてもよい。あるいは、指標検出部２０３０で得たデータリストをデータ管理部２０５０から入力し、画像座標と世界座標との対応関係から得られる線形連立方程式を解くことによって撮像装置の位置姿勢を推定する手法を用いて求めてもよい。その他の何れの手法を用いて撮像装置の概略位置姿勢を求め初期値としてもよい。
【００３４】
データ管理部２０５０は、データリストの保持・管理を行っており、必要に応じてこれを出力する。また、指標に関する既知の情報として、世界座標系における指標の位置姿勢ＭＷＭや各頂点の世界座標ｘｗｐｋｎｉを予め保持している。さらに、撮像装置概略位置姿勢入力部２０４０で求めた世界座標系における撮像装置２０１０の概略位置姿勢ＭＷＣを入力・保持し、必要に応じてこれを出力する。
【００３５】
評価量算出部２０６０は、データ管理部２０５０から、あるフレームにおける撮像装置２０１０の概略位置姿勢と、同フレームで検出された指標のデータリストを入力する。そして、データリストの要素である夫々の指標に対してその評価量ＶＰｋｎを算出し、得られた評価量を各指標のパラメータとしてデータリスト内に保存する。
【００３６】
ここで、本実施形態に係る評価量算出部２０６０における、評価量算出の詳細について説明する。まず、世界座標系における指標の重心位置と撮像装置２０１０の概略位置ｔＷＣとに基づいて、指標と撮像装置２０１０の視点とを結ぶ共線ベクトル
【００３７】
【数３】
【００３８】
を求める。ここで、世界座標系における指標の重心位置については、指標に関する既知の値として予め保持しておいても良いし、指標の各頂点の世界座標の平均値として求めても良い。
【００３９】
次に、指標の各頂点の世界座標に基づいて、世界座標系における指標の法線ベクトル
【００４０】
【数４】
【００４１】
を求める。この値は、指標に関する既知の値として予め保持しておいても良い。さらに、この指標の法線ベクトル
【００４２】
【数５】
【００４３】
と共線ベクトル
【００４４】
【数６】
【００４５】
とのなす角θＰｋｎ（図４（ａ）にその幾何的な関係を示す）を、式７の関係に基づいて算出する。
【００４６】
【数７】
【００４７】
最後に、評価量ＶＰｋｎ＝θＰｋｎとしてデータリストへと保存する。このようにして求めた評価量は、各指標がカメラに対してどれくらい正面から捕らえられているかを表す値となる。
【００４８】
信頼度算出部２０７０は、データ管理部２０５０からデータリストを入力し、データリスト中の夫々の指標に対して、評価量算出部２０６０で求めたそれぞれの指標の評価量ＶＰｋｎに基づいて指標の信頼度ωＰｋｎを算出する。そして、得られた評価量を各指標のパラメータとしてデータリスト内に保存する。
【００４９】
ここで、本実施形態に係る信頼度算出部２０７０における、信頼度算出の詳細について説明する。指標の信頼度ωＰｋｎは、指標の評価量ＶＰｋｎを引数とする重み関数
【００５０】
【数８】
【００５１】
によって算出する。重み関数ω（ＶＰｋｎ）は、正（＞０）である。係る関数は、ロバスト推定手法のひとつであるＭ推定で用いられるような重み関数（確率論的モデルを作り、観測データとよくあてはまるものに、より大きな重み付けを行う関数）であってもよいし、実験的、経験的に求めた関数であってもよい。
【００５２】
ここで、
【００５３】
【数９】
【００５４】
と
【００５５】
【数１０】
【００５６】
のなす角θＰｋｎは０°から９０°までの範囲で変化する。θＰｋｎ＝９０°であれば、平面形状の指標は検出できないので信頼度は０とする。また、θＰｋｎが９０°付近であれば、指標の各頂点の観察座標の検出精度が低下するので小さな信頼度を与える。θＰｋｎが０°付近では検出精度が高くなるので大きな信頼度を与える。このように、θＰｋｎ＝９０°で０、θＰｋｎ＝０°で最大となるような評価値θＰｋｎの関数ω（θＰｋｎ）によって信頼度ωＰｋｎを定義する。
【００５７】
位置姿勢算出部２０８０は、データ管理部２０５０からデータリスト及び撮像装置２０１０の概略位置姿勢を入力する。そして、あるフレームにおける撮像装置の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標とをもとに撮像装置２０１０の位置姿勢算出処理を行う。そして、その結果として得られた撮像装置２０１０の位置姿勢情報（すなわち、世界座標系における撮像装置の位置と姿勢）を出力する。
【００５８】
図２は、本実施形態の位置姿勢測定装置２０００が位置姿勢情報を求める際に行う処理のフローチャートである。
【００５９】
ステップＳ６０１０において、指標検出部２０３０は、入力画像に対して指標の検出処理を実行し、検出結果として生成したデータリストをデータ管理部２０５０に出力する。
【００６０】
ステップＳ６０２０において、撮像装置概略位置姿勢算出部２０４０は、入力画像の撮像時刻と同一時刻の世界座標系における撮像装置２０１０の概略位置姿勢ＭＷＣを算出し、データ管理部２０５０へと入力する。
【００６１】
ステップＳ６０３０において、評価量算出部２０６０は、データ管理部２０５０から撮像装置２０１０の概略位置姿勢ＭＷＣを入力し、データリスト中の夫々の指標の評価量ＶＰｋｎを算出する。
【００６２】
ステップＳ６０４０において、信頼度算出部２０７０は、データリスト中の夫々の指標に対して、その評価量ＶＰｋｎに基づいて信頼度ωＰｋｎを算出する。
【００６３】
ステップＳ６０５０において、位置姿勢算出部２０８０は、データ管理部２０５０が保持している撮像装置２０１０の概略位置姿勢情報ＭＷＣと検出指標のデータリストを入力する。そして、これをもとに撮像装置２０１０の位置姿勢算出処理を行い、その結果として得られた撮像装置２０１０の位置姿勢情報を出力する。
【００６４】
最後にステップＳ６０６０では、位置姿勢算出処理を終了するか否かの判定が行われる。オペレータが、位置姿勢算出装置２０００に対して、位置姿勢算出処理の終了を指示した場合には、処理を終了させ、位置姿勢算出処理の継続を指示した場合には、再びステップＳ６０１０へと戻り、次フレームの入力画像に対する処理が実行される。
【００６５】
次に、図５に示すフローチャートを参照しながら、位置姿勢算出部２０８０の処理（図２のステップＳ６０５０）の詳細を説明する。
【００６６】
ステップＳ４０１０において、位置姿勢算出部２０８０は、あるフレームにおける撮像装置の初期位置姿勢と、データリスト（同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標）を、データ管理部２０５０から入力する。
【００６７】
ステップＳ４０２０において、位置姿勢算出部２０８０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力された指標の頂点の総数ＮTotalが３以上の場合はステップＳ４０３０に処理を進め、３未満の場合はステップＳ４０９０に処理を進める。ＮTotalについてであるが、例えば、四角形の４頂点で構成される指標１つであればＮTotalは４、三角形の３頂点で構成される指標２つならばＮTotalは６となる。
【００６８】
位置姿勢算出部２０８０では、世界座標系（あるいは任意の座標系）における撮像装置２０１０の位置姿勢を求めるべき未知パラメータとして扱う。ここで、姿勢の表現方法として、３値のベクトルａ＝［ξ ψ ζ］Ｔを用いる。ａは回転軸・回転角による姿勢の表現方法であり、回転行列Ｒは次式に示すようなａの関数となる。
【００６９】
【数１１】
【００７０】
このとき、算出すべき撮像装置の位置姿勢は、位置ｔ＝［ｘ ｙ ｚ］Ｔ及び姿勢ａ＝［ξ ψ ζ］Ｔによって表現される。そして、求める未知パラメータは、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］と記述される。
【００７１】
ステップＳ４０３０において、位置姿勢算出部２０８０は、各々の指標の頂点ｐｋｎｉに対して、その画像座標の推定値（再投影座標）uＰｋｎｉ’を算出する。uＰｋｎｉ’の算出は、指標の頂点ｐｋｎｉの世界座標ｘｗＰｋｎｉと現在の状態ベクトルｓの関数（観測方程式）
【００７２】
【数１２】
【００７３】
に基づいて行われる。
【００７４】
具体的には、関数ＦC（）は、ｘｗＰｋｎｉからカメラ座標における位置ベクトルｘＣＰｋnｉを求める次式
【００７５】
【数１３】
【００７６】
及び、ｘＣＰｋnｉから画像上の座標ｕＰｋnｉ’を求める次式
【００７７】
【数１４】
【００７８】
によって構成されている。ここでｆＢｘ及びｆＢｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置２０１０の焦点距離であり、既知の値として予め保持されているものとする。
【００７９】
ステップＳ４０４０において、位置姿勢算出部２０８０は、各々の指標の頂点ｐｋnｉの実測値ｕＰｋnｉと、それに対応する画像座標の計算値ｕＰｋnｉ’との誤差（再投影誤差）ΔｕＰｋnｉを次式によって算出する。
【００８０】
【数１５】
【００８１】
ステップＳ４０５０において、位置姿勢算出部２０８０は、各々の指標の頂点ｐｋnｉに対して、状態ベクトルｓに関する画像ヤコビアンＪｕｓＰｋnｉ（＝∂ｕＰｋnｉ／∂ｓ）を算出する。ここで、係るヤコビアンは即ち、式１２の関数Ｆｃ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。
【００８２】
具体的には、式１４の右辺をカメラ座標ｘＣＰｋnｉの各要素で変微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＰｋnｉ（＝∂ｕＰｋnｉ／∂ｘ）と、式１３の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｘｓＰｋnｉ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＰｋnｉを算出する。
【００８３】
【数１６】
【００８４】
ステップＳ４０６０において位置姿勢算出部２０８０は、ステップＳ４０４０及びステップＳ４０５０において算出した各指標の頂点の誤差ΔｕＰｋnｉ、画像ヤコビアンＪｕｓＰｋnｉ、ステップＳ４０１０で入力した各指標の信頼度ωＰｋｎに基づいて、状態ベクトルｓの補正値Δｓを算出する。以下、補正値Δｓの算出処理の詳細を説明する。
【００８５】
はじめに、各指標の頂点における再投影誤差を垂直に並べた２ＮTotal次元の誤差ベクトルＵを作成する。
【００８６】
【数１７】
【００８７】
また、各指標の頂点におけるヤコビ行列ＪｕｓＰｋnｉを垂直に並べた（２ＮTotal）行×６列の行列Φを作成する。
【００８８】
【数１８】
【００８９】
次に、指標Ｐｋｎの各頂点ｐｋnｉに対応する要素（頂点毎にｘ座標とｙ座標の２要素を有する）に対して当該指標の信頼度ωＰｋｎを対角成分に持つような、２ＮTotal行×２ＮTotal列の対角行列Ｗを作成する。
【００９０】
【数１９】
【００９１】
行列Ｗを重みとした最小二乗法によってΔｓを求めることを考えると、次の正規方程式
【００９２】
【数２０】
【００９３】
が得られるので、Δｓを次式より算出する。
【００９４】
【数２１】
【００９５】
このように、２次元指標特有の検出時の評価量に基づく信頼度を表す行列Ｗを重みとしてΔｓの算出に利用することで、検出時の指標の評価量に応じて各指標のΔｓの算出への寄与度が変化するという効果を得ることができる。２次元指標特有の検出時の評価量とは、ここでは、撮像装置と指標の相対姿勢である。即ち、より信頼できる指標である可能性が高い指標をΔｓの算出に積極的に利用し、検出精度が低い可能性が高い指標から受ける悪影響を軽減するという効果を得ることができる。
【００９６】
なお、Ｍ推定のような統計的推定手法によって指標の再投影誤差ΔｕＰｋnｉに応じた重みを求め、ωＰｋｎとの積を得てこれを重みとして用いてもよい。
【００９７】
ここで、Δｓは６次元ベクトルであるから、２ＭTotalが６以上であれば、Δｓを求めることができる。式２１のようにΔｓを求めることができるが、他の方法によって求めてもよい。例えば、式２０は連立一次方程式であるので、ΔｓをＧａｕｓｓの消去法で解いてもよいし、その他のいずれの手法で解いてもよい。
【００９８】
ステップＳ４０７０において、位置姿勢算出部２０８０は、ステップＳ４０６０において算出した補正値Δｓを用いて、以下の式２２に従ってｓを補正し、得られた値を新たなｓとする。
【００９９】
【数２２】
【０１００】
ステップＳ４０８０において、位置姿勢算出部２０８０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ４０３０以降の処理を行う。
【０１０１】
ステップＳ４０８０において計算が収束したと判定されると、ステップＳ４０９０において、位置姿勢算出部２０８０は、世界座標系における撮像装置の位置及び姿勢ｓを出力する。この時の出力の形態は、ｓそのものであっても良いし、ｓの位置成分を3値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓから生成した座標変換行列Ｍであっても良い。
【０１０２】
以上の処理によって、世界座標系に対する（すなわち、世界座標系における）撮像装置の位置または位置と姿勢を取得することができる。以上のように、本実施形態に係る位置姿勢計測装置によれば、２次元形状を持つ指標の特有の評価量を考慮して位置姿勢推定を行うので、検出精度の低い指標の影響を軽減し、撮像装置の位置姿勢計測を高精度に行うことができる。
【０１０３】
［第２の実施形態］
第１の実施形態では、信頼度ωＰｋｎは、撮像装置と指標の相対姿勢に基づく評価量に基づいて決定されていた。しかし、二次元形状を持つ指標の幾何形状の特性を利用した評価量に基づいて信頼度を算出するのであれば、評価量とする情報の種類はこれに限定されるものではない。即ち、信頼度の算出方法はその他の方法、またはその他の方法と組み合わせた方法であってもよい。例えば、指標に関する２次元幾何情報（例えば、面積や縦横比など）を画像から得て、これを評価値として信頼度を算出することができる。
【０１０４】
例えば、検出された指標の画像上での面積ＳＰｋｎを評価値ＶＳＰｋｎとして信頼度ωＰｋｎを算出してもよい。この場合、撮像装置概略位置姿勢入力部２０４０は不要となる。
【０１０５】
評価量算出部２０６０は、データリストに保持している指標の各頂点の画像座標pｋnｉを用いて、画像上で検出した指標領域の画像中における面積ＳＰｋｎを算出し、これを夫々の指標の評価量とする。指標の面積は、指標検出部２０３０の指標検出工程で得られるラベリング画像を指標検出部２０３０から評価量算出部２０６０へと入力し、当該指標領域の画素数をカウントすることで算出してもよい。ここでラベリング画像とは、例えば指標領域以外の画素の画素値を０に、各指標領域に相当する画素の画素値をその画像上における夫々の指標領域の識別番号に設定した画像である。
【０１０６】
信頼度算出部２０７０は、画像上で検出した指標の面積ＳＰｋｎが大きいとき高い信頼度を、面積ＳＰｋｎが小さいとき低い信頼度を与えるような重み関数ωＳ（ＶＳＰｋｎ）を用いて、夫々の指標の信頼度を算出する。面積ＳＰｋｎの大小の判定は、分散や標準偏差を用いてもよいし、実験的、経験的に求めてもよい。
【０１０７】
また、このようにして得られた指標の面積に基づく信頼度ωＳＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得、これを用いてもよい。
【０１０８】
＜変形例１＞
第２の実施形態で求めた信頼度の算出方法において、指標が四角形指標の場合には、画像上で検出した辺の縦横比を評価値ＶratioＰｋｎとして信頼度ωＰｋｎを算出してもよい。
【０１０９】
評価量算出部２０６０は、データリストに保持している指標の各頂点の画像座標pｋnｉを用いて、例えば、夫々の頂点毎に存在する隣り合う２辺に対して、「見かけ上の短辺／見かけ上の長辺」によって長さの比を求める。そして、これらのうちの最小値を選択することによって評価量を得る。信頼度算出部２０７０は、例えば、指標が図３に示すような四角形指標であるとき評価値が１に近い指標に高い信頼度を、評価値が０に近い指標には低い信頼度を与えるような重み関数ωratio（ＶratioＰｋｎ）を用いて、夫々の指標の信頼度を算出する。
【０１１０】
また、このようにして得られた検出した辺の縦横比に基づく信頼度ωratioＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１１１】
＜変形例２＞
第２の実施形態で求めた信頼度の算出方法において、指標が四角形指標の場合には、画像上で検出した指標における隣り合う２辺の間の角度を評価値ＶangleＰｋｎとして信頼度ωＰｋｎを算出してもよい。
【０１１２】
隣り合う２辺の組み合わせは夫々の頂点毎に存在するので、例えば、夫々の２辺間のなす角を求め、これらのうちの最小値を評価値とする。例えば、四角形指標の隣り合う辺の間の角度が９０°付近であれば大きな信頼度を、０°付近であれば小さな信頼度を与えるような関数ωangle（ＶangleＰｋｎ）を重み関数として定義する。
【０１１３】
また、このようにして得られた指標における隣り合う２辺の間の角度に基づく信頼度ωangleＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１１４】
＜変形例３＞
第２の実施形態で求めた信頼度の算出方法において、円形の平面形状からなる円形指標を指標として用いる場合には、画像上で検出した指標の真円度を評価値ＶcircleＰｋｎとして信頼度ωＰｋｎを算出してもよい。この場合、指標検出部２０３０から評価量算出部２０６０へとラベリング画像が出力される。
【０１１５】
真円度とは，円形指標の投影像として検出された領域（この領域は、円形指標が正面から撮影された場合には真円に近い形状となり、斜め方向から撮影された場合には楕円に近い形状となる）の真円からのずれ量である。円真度は、例えば、検出領域の外接円と内接円の半径の差によって定義できる。
【０１１６】
信頼度算出部２０７０は、このｒを夫々の指標領域に対して求め、これを評価値ＶcircleＰｋｎとする。信頼度算出部２０７０は、ｒが小さな指標には高い信頼度を、逆にｒが大きな指標には低い信頼度を与えるような重み関数ωcircle（ＶcircleＰｋｎ）を用いて、夫々の指標の信頼度を算出する。
【０１１７】
また、このようにして得られた円形マーカの真円度に基づく信頼度ωcircleＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１１８】
＜変形例４＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、複数種の指標を混在して用いる場合に、夫々の指標の種類に応じて信頼度を定義してもよい。ここで複数の指標とは、例えば、内部にＩＤ情報を有するような多角形形状の指標、画像情報からは同定することができない単色領域からなる円形指標（以上の指標は人為的な指標）、エッジやテクスチャといった自然特徴によって定義される自然特徴指標である。
【０１１９】
例えば、円形指標や自然特徴指標などに比べて誤検出や同定間違いが起きにくいＩＤ情報を持つ多角形指標には高い信頼度を与える。自然特徴と比べると相対的に誤検出の可能性が低い円形指標には中位の信頼度を与える。最も誤検出の可能性の高い自然特徴指標には低い信頼度を与える。そして、これらの信頼度に応じて上記式２１に基づく位置姿勢算出を行っても良い。
【０１２０】
また、このようにして得られた指標の種類に基づく信頼度をそのまま用いるだけでなく、その他の方法で求めた例えば指標の幾何形状の特性を利用した信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２１】
＜変形例５＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、検出した指標が識別子を持ち、かつ認識の誤りを修正する誤り修正情報を持つとき、誤り修正の有無を評価値ＶIDmdfＰｋｎとして信頼度ωＰｋｎを算出してもよい。何らかの理由で情報が抜けた場合等に指標の誤り修正が行われるため、誤り修正が行われた指標には低い信頼度を、逆に誤り訂正が行われない指標に高い信頼度を与えるような関数ωIDmdf（ＶIDmdfＰｋｎ）を重み関数として定義する。
【０１２２】
また、このようにして得られた誤り修正の有無に基づく信頼度ωIDmdfＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２３】
＜変形例６＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、画像上で検出した指標のコントラストの強さを評価値ＶＣＰｋｎとして信頼度ωＰｋｎを算出してもよい。コントラストＣは、画像の明暗の差を表したもので、画像中の濃淡レベルの最小値をＩｍｉｎ、最大値をＩｍａｘとすると次式で定義される。
【０１２４】
【数２３】
【０１２５】
コントラストＣを指標の周辺領域において調べ、その値を評価値ＶＣＰｋｎとする。コントラストが高い指標には、検出精度が高くなるので高い信頼度を、コントラストが低い指標は、検出精度が低くなるので低い信頼度を与えるような関数ωＣ（ＶＣＰｋｎ）を重み関数として定義する。
【０１２６】
また、このようにして得られたコントラストに基づく信頼度ωＣＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２７】
＜変形例７＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、画像上で検出した指標の鮮鋭度を評価値ＶsharpＰｋｎとして信頼度ωＰｋｎを算出してもよい。鮮鋭度の計測方法は、エッジの斜度を計測する方法であってもよいし、像の空間周波数成分を計測する方法であってもよいし、その他の何れの方法でもよい。指標の周辺の鮮鋭度が高いとき、エッジがはっきり検出できるため高い信頼度を、逆に鮮鋭度が低いときは、低い信頼度を与えるような関数ωsharp（ＶsharpＰｋｎ）を重み関数として定義する。
【０１２８】
また、このように得られた鮮鋭度に基づく信頼度ωsharpＰｋｎをそのまま信頼度ωＰｋｎとして用いてもよいし、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２９】
＜変形例８＞
第１の実施形態では、撮像装置概略位置姿勢入力部２０４０は次のような処理を行っていた。即ち、世界座標系における撮像装置２０１０の位置姿勢の概略値ＭＷＣを求め、概略値ＭＷＣと世界座標系における位置または位置姿勢が既知の指標より撮像装置２０１０と指標の相対姿勢を求めていた。しかし、４点以上の特徴点を有する指標を用いる場合には次のようにしても良い。即ち、２次元ホモグラフィの計算に基づいて画像上で検出した指標の画像座標より指標と撮像装置２０１０の概略位置姿勢ＭＭＣを求め、これに基づいて、指標の法線ベクトルと撮像装置２０１０から指標への視軸とのなす角を算出してもよい。
【０１３０】
具体的には、求めた指標と撮像装置２０１０の概略相対位置に基づいて、撮像装置を基準とした座標系（以降、撮像装置座標系とする）における指標と撮像装置２０１０の視点とを結ぶ共線ベクトル
【０１３１】
【数２４】
【０１３２】
を求める。そして、撮像装置座標系における指標の各頂点の座標（ＭＭＣから算出可能）に基づいて、撮像装置座標系における指標の法線ベクトル
【０１３３】
【数２５】
【０１３４】
を求める。以上のようにして、指標の法線ベクトル
【０１３５】
【数２６】
【０１３６】
と共線ベクトル
【０１３７】
【数２７】
【０１３８】
が得られたら、以降は第１の実施形態と同様な処理を行えばよい。
【０１３９】
また、このように得られた撮像装置２０１０と指標との相対姿勢に基づく信頼度ωθＰｋｎをそのまま信頼度ωＰｋｎとして用いてもよい。また、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１４０】
＜変形例９＞
第２の実施形態では、指標の２次元幾何情報を画像から得て、これを評価量として信頼度を算出していた。しかし、撮像装置の概略位置姿勢ＭＷＣに基づいて指標の各頂点の推定座標（再投影座標）を求め、再投影座標の２次元幾何情報に基づいて信頼度を算出してもよい。
【０１４１】
撮像装置２０１０の概略位置姿勢ＭＷＣが実際の位置姿勢（真値）から大きくずれていなければ、画像上で検出した座標の代わりに再投影座標を用いて信頼度を算出しても本発明の本質が損なわれないことはいうまでもない。
【０１４２】
＜変形例１０＞
第１の実施形態及び第２の実施形態では、指標の位置は既知であるとして撮像装置の位置姿勢を求めていたが、指標の位置または位置姿勢も撮像装置の位置姿勢とともにを求めてもよい。
【０１４３】
このとき、求めるべき位置または位置姿勢情報は、すなわち世界座標系における撮像装置２０１０の位置姿勢と、世界座標系における指標の位置または位置姿勢である。世界系座標における指標の位置を３値ベクトル[ｘＰｋｎ ｙＰｋｎ ｚＰｋｎ]、または位置姿勢を６値ベクトル［ｘＰｋｎ ｙＰｋｎ ｚＰｋｎ ξＰｋｎ ψＰｋｎ ζＰｋｎ］Ｔとして扱い、この未知パラメータを状態ベクトルｓＰｋｎと記述する。
【０１４４】
状態ベクトルｓＰｋｎに適当な初期値を与える。初期値としては、オペレータが予めおおよその値を手入力しても良い。また、ある同一時刻に入力された（すなわち、撮影画像から検出された）複数の指標の検出座標をデータリストから抽出し、このデータを用いて、世界座標系系における指標の同時刻における位置または位置姿勢を公知の方法によって算出する。ここで、世界座標系における既知の点が３点以上必要となる。
【０１４５】
また、世界座標系における撮像装置の位置姿勢を６値のベクトル［ｘＷＣ ｙＷＣ ｚＷＣ ξＷＣ ψＷＣ ζＷＣ］Ｔとして扱う。そして、この未知パラメータを状態ベクトルｓＷＣと記述する。求めるべき位置または位置姿勢の状態ベクトルをｓ＝[ｓＷＣ ｓＰｋｎ]と記述する。
【０１４６】
なお、世界座標系における指標の位置又は位置姿勢を算出する公知の手法としては例えば、指標が同一平面上に配置されている場合には、４点以上の指標を用いて２次元ホモグラフィの計算に基づいて指標の位置又は位置姿勢を得る手法を用いることができる。また、同一平面上にない６点以上の指標を用いる手法や、これらの解を初期値にして、ニュートン法のような繰り返し計算によって最適解を得る手法を用いることもできる。
【０１４７】
ここで位置姿勢算出部２０８０は、各々の指標の頂点ｐｋｎｉに対して、その画像座標の推定値（再投影座標）uＰｋｎｉ’を算出する。uＰｋｎｉ’の算出は、指標の頂点ｐｋｎｉの世界座標ｘｗＰｋｎｉと現在の状態ベクトルｓの関数（観測方程式）
【０１４８】
【数２８】
【０１４９】
に基づいて行われる。
【０１５０】
以降の処理については、第１の実施形態と同様に算出する。即ち、第１の実施形態と同様に信頼度を求め、信頼度を対角成分にもつ行列Ｗを重みとし、重みを考慮した最小二乗法で算出することで、指標の位置または位置姿勢と撮像装置の位置姿勢を求めることができる。
【０１５１】
また、撮像装置の位置と姿勢（外部パラメータ）のみではなく、撮像装置の内部パラメータ（焦点距離やアスペクト比、歪み補正パラメータ）を求める問題であっても、同様の重み付けの枠組みを適用できることは言うまでもない。
【０１５２】
＜変形例１１＞
また、上記実施形態においては、図３（ｂ）に示すような複数の頂点を有する２次元形状を持つ指標（三角形や四角形等の外形から成る指標）を用いていた。しかし２次元形状を持つ指標と同様に、複数の座標の集合を構成要素として有するような指標（これらを総称して多角形形状と呼ぶ）を用いる場合であっても、画像上の２次元幾何特徴やカメラと指標の３次元幾何特徴に基づいた評価量を求めることは可能である。従って、本実施形態で述べた発明の効果を得ることが可能である。
【０１５３】
例えば、図３（ｃ）に示すようにそれぞれの相対的な位置関係が既知の点ｐｋｉ（ｉ＝１，，，Ｎｋ）によって構成される指標であってもよい。また、複数の座標の集合を構成要素として有するような指標であればいずれの指標を用いてもよい。
【０１５４】
＜変形例１２＞
なお、上記実施形態においては、最適化演算において非線形方程式をテーラー展開し、１次近似で線形化することで補正値を求め、繰り返し補正していくことで最適解を求めるＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法を用いている。しかし、補正値の算出は必ずしもＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよい。また、最急降下法を用いても求めてもよい。他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。
【０１５５】
［第３の実施形態］
上記実施形態では、図１に示した位置姿勢計測装置２０００を構成する各部はハードウェアでもって実装されているものとして説明した。しかし、図１に示した各部の一部若しくは全部をソフトウェアでもって実現し、残りをハードウェアでもって実現しても良い。この場合、例えば、このハードウェアをパーソナルコンピュータに挿入可能な機能拡張カードとして実現し、この機能拡張カードをパーソナルコンピュータに挿入する。また、このソフトウェアは、このパーソナルコンピュータが有するメモリ上に格納する。係る構成によれば、パーソナルコンピュータが有するＣＰＵがこのソフトウェアを実行すると共に、この機能拡張カードの動作制御をも行うことにより、第１の実施形態及び各種の変形例で説明した処理を行うことができる。
【０１５６】
図６は、位置姿勢計測装置２０００に適用可能なコンピュータのハードウェア構成を示すブロック図である。
【０１５７】
６０１はＣＰＵで、ＲＡＭ６０２やＲＯＭ６０３に格納されているプログラムやデータを用いて本コンピュータ全体の制御を行う。
【０１５８】
６０２はＲＡＭで、外部記憶装置６０６からロードされたプログラムやデータ、Ｉ／Ｆ（インターフェース）６０７を介して外部から受信したプログラムやデータを一時的に記憶するためのエリアを有する。更には、ＣＰＵ６０１が各種の処理を実行する際に用いるワークエリアも有する。即ち、ＲＡＭ６０２は各種のエリアを適宜提供することができる。
【０１５９】
６０３はＲＯＭで、本コンピュータの設定データやブートプログラムなどを格納する。
【０１６０】
６０４は操作部で、キーボードやマウスなどにより構成されており、本コンピュータの操作者が操作することで、各種の指示をＣＰＵ６０１に対して入力することができる。
【０１６１】
６０５は表示部で、ＣＲＴや液晶画面などにより構成されており、ＣＰＵ６０１による処理結果を画像や文字などでもって表示することができる。
【０１６２】
６０６は外部記憶装置で、ハードディスクなどに代表される大容量情報記憶装置であって、ここにはＯＳ（オペレーティングシステム）や、本コンピュータが行う各種の処理をＣＰＵ６０１に実行させるためのプログラムやデータ等が保存される。このプログラムやデータには、上記ソフトウェアや、機能拡張カード６０８の動作制御プログラムなども含まれる。また、外部記憶装置６０６には、上記説明において予め保持しているものとして説明した各種の情報や、Ｉ／Ｆ６０７を介して撮像装置２０１０から受信した撮像画像も保存される。
【０１６３】
外部記憶装置６０６に保存されている各種の情報はＣＰＵ６０１による制御に従って適宜ＲＡＭ６０２にロードされる。そしてＣＰＵ６０１がこのロードされたプログラムやデータを用いて処理を実行することにより、本コンピュータは第１の実施形態及び各種の変形例で説明した処理を実行することができる。
【０１６４】
６０７はＩ／Ｆで、本コンピュータを上記撮像装置２０１０に接続するためのものである。
【０１６５】
６０８は機能拡張カードである。
【０１６６】
６０９は上述の各部を繋ぐバスである。
【０１６７】
［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。
【０１６８】
また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１６９】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれたとする。その後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１７０】
本発明を上記記録媒体に適用する場合、その記録媒体には、先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０１７１】
【図１】第１の実施形態に係る位置姿勢算出装置の概略構成を示す図である。
【図２】第１の実施形態に係る位置姿勢算出方法の概略処理を示すフローチャートである。
【図３】第１の実施形態に係る指標の概略構成を示す図である。
【図４】第１の実施形態に係る指標と撮像装置の相対姿勢を示す図である。
【図５】第１の実施形態に係る位置姿勢算出部２０８０の概略処理を示すフローチャートである。
【図６】位置姿勢計測装置２０００に適用可能なコンピュータのハードウェア構成を示すブロック図である。
【技術分野】
【０００１】
本発明は、視点位置姿勢計測の精度向上と安定性を向上させる技術に関する。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない融合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実画像は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像を重畳描画することにより生成される。そして、複合現実感システムで利用される画像表示装置は、例えば、ビデオシースルー方式によって実現される。ここで、仮想空間の画像とは、例えば、コンピュータ・グラフィックスにより描画された仮想物体や文字情報等である。
【０００３】
複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置の位置姿勢と称する）を求める問題に帰結される。
【０００４】
この問題を解決する方法としては次のような試みがなされている。即ち、環境中や対象物体上に、対象の座標系における配置が既知であるような複数の指標を設置あるいは設定する。そして、既知の情報であるその指標の対象座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、対象座標系に対する撮像装置の位置姿勢を求める（非特許文献１を参照）。
【０００５】
また、撮像装置に慣性センサを装着し、センサ計測値を利用することにより、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている。例えば、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用する方法が提案されている。同推定結果を画像に基づく位置及び姿勢算出の初期値として用いる方法も提案されている。さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いる方法も提案されている（特許文献１、非特許文献２を参照）。
【０００６】
従来の画像情報を用いた位置合わせ手法は、指標の検出結果がすべて正しいことを前提としていた。さらに、指標検出結果はすべて均等な重みとして計算されていた。そのため、誤検出や検出精度の低い指標による影響を大きく受け、正しい位置姿勢計測が行えない場面が起こり得る。
【０００７】
そこで、Ｍ推定のような統計的推定手法を導入し、検出した指標（特徴点）の画像上での観察座標と撮像装置の位置姿勢や指標の位置から推定した指標の画像座標（再投影座標）との間の誤差（再投影誤差）を求る。そして、この誤差をもとに検出指標の信頼度を算出し、誤検出指標を排除することや、その影響を下げる手法が近年提案されている（非特許文献３を参照）。
【特許文献１】特開２００５−３３３１９号公報
【非特許文献１】佐藤, 田村：“複合現実感における位置合わせ手法”, 画像の認識・理解シンポジウム（ＭＩＲＵ ２００２）論文集Ｉ, 情報処理学会シンポジウムシリーズ、 ｖｏｌ．２００２、 ｎｏ．１１、 ｐｐ．Ｉ．６１−Ｉ．６８， Ｊｕｌｙ ２００２
【非特許文献２】藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９、 ｎｏ．５７４、 ｐｐ．１−８）
【非特許文献３】佐藤，神原，横矢，竹村：“マーカと自然特徴点の追跡による動画像からのカメラ移動パラメータの復元”，電子情報通信学会論文誌，Ｄ−III，ｖｏｌ．Ｊ８６−Ｄ−II，ｎｏ．１０，ｐｐ．１４３１−１４４０，２００３．
【０００８】
近年、現実空間と仮想空間の繋ぎ目のない融合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感システムで利用される画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像を重畳描画してそれを表示するビデオシースルー方式によって主に実現される。ここで、仮想空間の画像とは、例えば、コンピュータ・グラフィックスにより描画された仮想物体や文字情報等である。
【０００９】
複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置の位置姿勢と称する）を求める問題に帰結される。
【００１０】
この問題を解決する方法としては次のような試みがなされている。即ち、環境中や対象物体上に、対象の座標系における配置が既知であるような複数の指標を設置あるいは設定する。そして、既知の情報であるその指標の対象座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、対象座標系に対する撮像装置の位置姿勢を求める（非特許文献１を参照）。
【００１１】
また、撮像装置に慣性センサを装着し、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用したり、同推定結果を画像に基づく位置及び姿勢算出の初期値として用いたり、さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いることで、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている（特許文献１、非特許文献２を参照）。
【００１２】
従来の画像情報を用いた位置合わせ手法は、指標の検出結果がすべて正しいことを前提としていた。さらに、指標検出結果はすべて均等な重みとして計算されていた。そのため、誤検出や検出精度の低い指標による影響を大きく受け、正しい位置姿勢計測が行えない場面が起こり得る。
【００１３】
そこで、Ｍ推定のような統計的推定手法を導入し、検出した指標（特徴点）の画像上での観察座標と撮像装置の位置姿勢や指標の位置から推定した指標の画像座標（再投影座標）との間の誤差（再投影誤差）を求る。そして、この誤差をもとに検出指標の信頼度を算出し、誤検出指標を排除することや、その影響を下げる手法が近年提案されている（非特許文献３を参照）。
【特許文献１】特開２００５−３３３１９（ジャイロ＆ＩＣＰ位置あわせ 内山，佐藤）
【非特許文献１】佐藤, 田村：“複合現実感における位置合わせ手法”, 画像の認識・理解シンポジウム（ＭＩＲＵ ２００２）論文集Ｉ, 情報処理学会シンポジウムシリーズ、 ｖｏｌ．２００２、 ｎｏ．１１、 ｐｐ．Ｉ．６１−Ｉ．６８， Ｊｕｌｙ ２００２
【非特許文献２】藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告ＰＲＭＵ９９−１９２（信学技報ｖｏｌ．９９、 ｎｏ．５７４、 ｐｐ．１−８）
【非特許文献３】佐藤，神原，横矢，竹村：“マーカと自然特徴点の追跡による動画像からのカメラ移動パラメータの復元”，電子情報通信学会論文誌，Ｄ−III，ｖｏｌ．Ｊ８６−Ｄ−II，ｎｏ．１０，ｐｐ．１４３１−１４４０，２００３．
【発明の開示】
【発明が解決しようとする課題】
【００１４】
しかし、再投影誤差の統計量のみに基づいて信頼度を算出し、指標に重み付けすることで誤検出を排除または検出精度（検出すべき画像座標の正確さ）の低い指標の影響を軽減する試みは、必ずしも有効であるとはいえない。なぜなら、上記の手法は多数の指標が正しく検出され、例外的に現れた誤検出指標を排除することに関しては有効であるが、正しい指標より誤検出した指標の方が多い場合では、検出精度の低い指標の影響を受ける可能性があるからである。また、同じ画像で撮影された指標であっても、マーカの配置や撮影時の状況に応じて検出精度の低い指標が含まれる場合があるため位置姿勢計測精度の低下につながるという課題がある
本発明は上記問題に鑑みてなされたものである。即ち、本発明は、二次元形状を持つ指標（円マーカや多角形マーカ等）特有の評価量に応じて重み付けを変え、検出精度の低い指標による影響を排除又は減少させ、高精度且つより安定な撮像装置の位置姿勢計測を行うことを目的とする。
【課題を解決するための手段】
【００１５】
上記目的を達成するために、本発明は以下の構成をを有する。
【００１６】
本願請求項１記載の発明は、撮像装置によって撮像された撮像画像を入力する画像入力工程と、物体上に配置または設定された指標を前記撮像画像から検出する指標検出工程と、前記撮像画像から検出されている夫々の指標の信頼度を算出する手がかりとして用いるための評価量を算出する指標評価量算出工程と、前記指標評価量算出工程で算出した指標の評価量に応じて当該指標の信頼度を算出する信頼度算出工程と、前記信頼度算出工程で算出された指標の信頼度と、前記指標検出工程で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める位置姿勢算出工程とを有し、前記評価量は、前記指標の画像上での２次元幾何特徴および／または前記指標の３次元空間中における前記撮像装置と前記指標との間の関係を表す３次元幾何特徴に基づくことを特徴とする。
【発明の効果】
【００１７】
本発明によれば、検出精度の低い指標の影響を下げることができるので、高精度かつより安定的に撮像装置の位置姿勢計測を行うことができる。
【発明を実施するための最良の形態】
【００１８】
以下添付図面を参照して、本発明をその好適な実施形態に従って詳細に説明する。
【００１９】
［第１の実施形態］
本実施形態に係る撮像装置の位置姿勢計測装置は、検出した２次元形状を持つ指標の評価量に応じて信頼度を変えて重み付けを行い、精度の低い指標の排除または影響を下げ、撮像装置の位置姿勢計測結果の精度を向上させる。以下、本実施形態に係る指標の評価量に応じた重み付けによる撮像装置の位置姿勢測定方法について説明する。
【００２０】
図１は、本実施形態に係る撮像装置の位置姿勢計測装置２０００の概略構成を示している。位置姿勢計測装置２０００は、画像入力部２０２０、指標検出部２０３０、撮像装置概略位置姿勢入力部２０４０、データ管理部２０５０、評価量算出部２０６０、信頼度算出部２０７０、位置姿勢算出部２０８０によって構成されている。また、画像入力部２０２０には、計測対象である撮像装置２０１０が接続されている。
【００２１】
環境中または対象物体上には、指標が複数配置されている。ここで、物体に配置された指標をＰｋ（ｋ＝１，，，Ｋo）で表す。Ｋoは配置された指標の数（図３（ａ）の例ではＫ０＝３）である。また、図３（ｂ）で示すような多角形の指標Ｐｋは頂点ｐｋｉ（ｉ＝１，，，Ｎｋ）を含む。Ｎｋは、指標Ｐｋを構成する頂点の総数（本実施形態の場合はＮｋ＝４）を表す。また、図３（ｃ）に示すように、それぞれの頂点の相対的な位置関係が既知の点ｐｋｉ（ｉ＝１，，，Ｎｋ）によって構成される指標であってもよい。
【００２２】
以下では、計測の基準とする座標系（ここでは、環境中の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）を世界座標系と呼び、世界座標系における各指標の位置姿勢は既知であるとする。また、環境中に配置された夫々の指標を構成する各頂点は、世界座標系において位置が既知であるとする。
【００２３】
指標Ｐｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各頂点が識別可能であるような指標であれば、何れの形態であってもよい。例えば、図３（ａ）、（ｂ）に示すような四角形指標は内部に識別子を表すパターンを持ち、一意に同定可能である。このような指標を検出する際には、撮影画像に２値化処理を施した後にラベリング処理を行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として抽出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって候補領域が指標領域であるかを判定する。そして、指標領域と判定された場合は、内部のパターンを読み出すことによって指標の方向と識別子を取得する。
【００２４】
撮像装置２０１０によって撮影された画像は、画像入力部２０２０を介して指標検出部２０３０に入力される。
【００２５】
指標検出部２０３０は、画像入力部２０２０から画像を入力し、入力された画像中に撮影されている指標Ｐｋｎを構成する各頂点ｐｋｎｉの画像座標を検出する。
【００２６】
さらに、指標検出部２０３０は、検出された各々の指標Ｐｋnの識別（同定）を行う。
【００２７】
そして、世界座標系における指標の位置姿勢ＭＷＭや各頂点の世界座標ｘｗｐｋｎｉ（指標に関する既知の情報として予め保持している）をデータ管理部２０５０から入力する。
【００２８】
そして、検出された夫々の指標の識別子ｋnと世界座標系における位置姿勢ＭＷＭ、各頂点pｋnｉの画像座標ｕｐｋnｉと世界座標ｘｗｐｋｎｉの組を要素として有するリスト（以下、データリストと称す）を作成し、データ管理部２０５０に出力する。ここで、n（n＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。また、Ｎ個の指標から成る構成点（頂点）の総数をＮTotalとする。例えば図１の場合は、識別子が１、２、３である四角形形状の指標が撮影される場合を示しているので、Ｎ＝３である。また、識別子ｋ１＝１、ｋ２＝２、ｋ３＝３と、これらの指標の位置姿勢、各頂点の画像座標ｕｐｋ１ｉ、ｕｐｋ２ｉ、ｕｐｋ３ｉ、（ｉ＝１、２、３、４）及び世界座標ｘｗｐｋ１ｉ、ｘｗｐｋ２ｉ、ｘｗｐｋ３ｉ、（ｉ＝１、２、３、４）が出力される。ＮTotalは、１２（＝３×４）になる。
【００２９】
ここで、ある三次元座標系Bに対する三次元の座標系Aの位置を表す三次元ベクトルをｔ、姿勢を表す3×3回転行列をＲとする。この場合、A上での位置がｘA（三次元ベクトル）で表される点のB上での座標ｘB（三次元ベクトル）は、下記の式１に示す４×４行列ＭＢＡを使って同次座標表現によって下記の式２のように表される。
【００３０】
【数１】
【００３１】
【数２】
【００３２】
本実施形態においては、ＭＢＡを座標系Ｂに対する座標系Ａの位置姿勢を表す手段として用いる。
【００３３】
撮像装置概略位置姿勢入力部２０４０は、世界座標系における撮像装置２０１０の位置姿勢の概略値ＭＷＣを求め、これをデータ管理部２０５０へと出力する。撮像装置の概略位置姿勢としては、例えば、６自由度位置姿勢センサを撮像装置２０１０に装着させてその出力値を用いてもよい。また、３自由度姿勢センサを撮像装置２０１０に装着することによって撮像装置２０１０の姿勢を計測し、指標検出部２０３０で得たデータリストをデータ管理部２０５０から入力し、これらの情報に基づいて求めてもよい。あるいは、指標検出部２０３０で得たデータリストをデータ管理部２０５０から入力し、画像座標と世界座標との対応関係から得られる線形連立方程式を解くことによって撮像装置の位置姿勢を推定する手法を用いて求めてもよい。その他の何れの手法を用いて撮像装置の概略位置姿勢を求め初期値としてもよい。
【００３４】
データ管理部２０５０は、データリストの保持・管理を行っており、必要に応じてこれを出力する。また、指標に関する既知の情報として、世界座標系における指標の位置姿勢ＭＷＭや各頂点の世界座標ｘｗｐｋｎｉを予め保持している。さらに、撮像装置概略位置姿勢入力部２０４０で求めた世界座標系における撮像装置２０１０の概略位置姿勢ＭＷＣを入力・保持し、必要に応じてこれを出力する。
【００３５】
評価量算出部２０６０は、データ管理部２０５０から、あるフレームにおける撮像装置２０１０の概略位置姿勢と、同フレームで検出された指標のデータリストを入力する。そして、データリストの要素である夫々の指標に対してその評価量ＶＰｋｎを算出し、得られた評価量を各指標のパラメータとしてデータリスト内に保存する。
【００３６】
ここで、本実施形態に係る評価量算出部２０６０における、評価量算出の詳細について説明する。まず、世界座標系における指標の重心位置と撮像装置２０１０の概略位置ｔＷＣとに基づいて、指標と撮像装置２０１０の視点とを結ぶ共線ベクトル
【００３７】
【数３】
【００３８】
を求める。ここで、世界座標系における指標の重心位置については、指標に関する既知の値として予め保持しておいても良いし、指標の各頂点の世界座標の平均値として求めても良い。
【００３９】
次に、指標の各頂点の世界座標に基づいて、世界座標系における指標の法線ベクトル
【００４０】
【数４】
【００４１】
を求める。この値は、指標に関する既知の値として予め保持しておいても良い。さらに、この指標の法線ベクトル
【００４２】
【数５】
【００４３】
と共線ベクトル
【００４４】
【数６】
【００４５】
とのなす角θＰｋｎ（図４（ａ）にその幾何的な関係を示す）を、式７の関係に基づいて算出する。
【００４６】
【数７】
【００４７】
最後に、評価量ＶＰｋｎ＝θＰｋｎとしてデータリストへと保存する。このようにして求めた評価量は、各指標がカメラに対してどれくらい正面から捕らえられているかを表す値となる。
【００４８】
信頼度算出部２０７０は、データ管理部２０５０からデータリストを入力し、データリスト中の夫々の指標に対して、評価量算出部２０６０で求めたそれぞれの指標の評価量ＶＰｋｎに基づいて指標の信頼度ωＰｋｎを算出する。そして、得られた評価量を各指標のパラメータとしてデータリスト内に保存する。
【００４９】
ここで、本実施形態に係る信頼度算出部２０７０における、信頼度算出の詳細について説明する。指標の信頼度ωＰｋｎは、指標の評価量ＶＰｋｎを引数とする重み関数
【００５０】
【数８】
【００５１】
によって算出する。重み関数ω（ＶＰｋｎ）は、正（＞０）である。係る関数は、ロバスト推定手法のひとつであるＭ推定で用いられるような重み関数（確率論的モデルを作り、観測データとよくあてはまるものに、より大きな重み付けを行う関数）であってもよいし、実験的、経験的に求めた関数であってもよい。
【００５２】
ここで、
【００５３】
【数９】
【００５４】
と
【００５５】
【数１０】
【００５６】
のなす角θＰｋｎは０°から９０°までの範囲で変化する。θＰｋｎ＝９０°であれば、平面形状の指標は検出できないので信頼度は０とする。また、θＰｋｎが９０°付近であれば、指標の各頂点の観察座標の検出精度が低下するので小さな信頼度を与える。θＰｋｎが０°付近では検出精度が高くなるので大きな信頼度を与える。このように、θＰｋｎ＝９０°で０、θＰｋｎ＝０°で最大となるような評価値θＰｋｎの関数ω（θＰｋｎ）によって信頼度ωＰｋｎを定義する。
【００５７】
位置姿勢算出部２０８０は、データ管理部２０５０からデータリスト及び撮像装置２０１０の概略位置姿勢を入力する。そして、あるフレームにおける撮像装置の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標とをもとに撮像装置２０１０の位置姿勢算出処理を行う。そして、その結果として得られた撮像装置２０１０の位置姿勢情報（すなわち、世界座標系における撮像装置の位置と姿勢）を出力する。
【００５８】
図２は、本実施形態の位置姿勢測定装置２０００が位置姿勢情報を求める際に行う処理のフローチャートである。
【００５９】
ステップＳ６０１０において、指標検出部２０３０は、入力画像に対して指標の検出処理を実行し、検出結果として生成したデータリストをデータ管理部２０５０に出力する。
【００６０】
ステップＳ６０２０において、撮像装置概略位置姿勢算出部２０４０は、入力画像の撮像時刻と同一時刻の世界座標系における撮像装置２０１０の概略位置姿勢ＭＷＣを算出し、データ管理部２０５０へと入力する。
【００６１】
ステップＳ６０３０において、評価量算出部２０６０は、データ管理部２０５０から撮像装置２０１０の概略位置姿勢ＭＷＣを入力し、データリスト中の夫々の指標の評価量ＶＰｋｎを算出する。
【００６２】
ステップＳ６０４０において、信頼度算出部２０７０は、データリスト中の夫々の指標に対して、その評価量ＶＰｋｎに基づいて信頼度ωＰｋｎを算出する。
【００６３】
ステップＳ６０５０において、位置姿勢算出部２０８０は、データ管理部２０５０が保持している撮像装置２０１０の概略位置姿勢情報ＭＷＣと検出指標のデータリストを入力する。そして、これをもとに撮像装置２０１０の位置姿勢算出処理を行い、その結果として得られた撮像装置２０１０の位置姿勢情報を出力する。
【００６４】
最後にステップＳ６０６０では、位置姿勢算出処理を終了するか否かの判定が行われる。オペレータが、位置姿勢算出装置２０００に対して、位置姿勢算出処理の終了を指示した場合には、処理を終了させ、位置姿勢算出処理の継続を指示した場合には、再びステップＳ６０１０へと戻り、次フレームの入力画像に対する処理が実行される。
【００６５】
次に、図５に示すフローチャートを参照しながら、位置姿勢算出部２０８０の処理（図２のステップＳ６０５０）の詳細を説明する。
【００６６】
ステップＳ４０１０において、位置姿勢算出部２０８０は、あるフレームにおける撮像装置の初期位置姿勢と、データリスト（同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標）を、データ管理部２０５０から入力する。
【００６７】
ステップＳ４０２０において、位置姿勢算出部２０８０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力された指標の頂点の総数ＮTotalが３以上の場合はステップＳ４０３０に処理を進め、３未満の場合はステップＳ４０９０に処理を進める。ＮTotalについてであるが、例えば、四角形の４頂点で構成される指標１つであればＮTotalは４、三角形の３頂点で構成される指標２つならばＮTotalは６となる。
【００６８】
位置姿勢算出部２０８０では、世界座標系（あるいは任意の座標系）における撮像装置２０１０の位置姿勢を求めるべき未知パラメータとして扱う。ここで、姿勢の表現方法として、３値のベクトルａ＝［ξ ψ ζ］Ｔを用いる。ａは回転軸・回転角による姿勢の表現方法であり、回転行列Ｒは次式に示すようなａの関数となる。
【００６９】
【数１１】
【００７０】
このとき、算出すべき撮像装置の位置姿勢は、位置ｔ＝［ｘ ｙ ｚ］Ｔ及び姿勢ａ＝［ξ ψ ζ］Ｔによって表現される。そして、求める未知パラメータは、６値の状態ベクトルｓ＝［ｘ ｙ ｚ ξ ψ ζ］と記述される。
【００７１】
ステップＳ４０３０において、位置姿勢算出部２０８０は、各々の指標の頂点ｐｋｎｉに対して、その画像座標の推定値（再投影座標）uＰｋｎｉ’を算出する。uＰｋｎｉ’の算出は、指標の頂点ｐｋｎｉの世界座標ｘｗＰｋｎｉと現在の状態ベクトルｓの関数（観測方程式）
【００７２】
【数１２】
【００７３】
に基づいて行われる。
【００７４】
具体的には、関数ＦC（）は、ｘｗＰｋｎｉからカメラ座標における位置ベクトルｘＣＰｋnｉを求める次式
【００７５】
【数１３】
【００７６】
及び、ｘＣＰｋnｉから画像上の座標ｕＰｋnｉ’を求める次式
【００７７】
【数１４】
【００７８】
によって構成されている。ここでｆＢｘ及びｆＢｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置２０１０の焦点距離であり、既知の値として予め保持されているものとする。
【００７９】
ステップＳ４０４０において、位置姿勢算出部２０８０は、各々の指標の頂点ｐｋnｉの実測値ｕＰｋnｉと、それに対応する画像座標の計算値ｕＰｋnｉ’との誤差（再投影誤差）ΔｕＰｋnｉを次式によって算出する。
【００８０】
【数１５】
【００８１】
ステップＳ４０５０において、位置姿勢算出部２０８０は、各々の指標の頂点ｐｋnｉに対して、状態ベクトルｓに関する画像ヤコビアンＪｕｓＰｋnｉ（＝∂ｕＰｋnｉ／∂ｓ）を算出する。ここで、係るヤコビアンは即ち、式１２の関数Ｆｃ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列である。
【００８２】
具体的には、式１４の右辺をカメラ座標ｘＣＰｋnｉの各要素で変微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＰｋnｉ（＝∂ｕＰｋnｉ／∂ｘ）と、式１３の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｘｓＰｋnｉ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＰｋnｉを算出する。
【００８３】
【数１６】
【００８４】
ステップＳ４０６０において位置姿勢算出部２０８０は、ステップＳ４０４０及びステップＳ４０５０において算出した各指標の頂点の誤差ΔｕＰｋnｉ、画像ヤコビアンＪｕｓＰｋnｉ、ステップＳ４０１０で入力した各指標の信頼度ωＰｋｎに基づいて、状態ベクトルｓの補正値Δｓを算出する。以下、補正値Δｓの算出処理の詳細を説明する。
【００８５】
はじめに、各指標の頂点における再投影誤差を垂直に並べた２ＮTotal次元の誤差ベクトルＵを作成する。
【００８６】
【数１７】
【００８７】
また、各指標の頂点におけるヤコビ行列ＪｕｓＰｋnｉを垂直に並べた（２ＮTotal）行×６列の行列Φを作成する。
【００８８】
【数１８】
【００８９】
次に、指標Ｐｋｎの各頂点ｐｋnｉに対応する要素（頂点毎にｘ座標とｙ座標の２要素を有する）に対して当該指標の信頼度ωＰｋｎを対角成分に持つような、２ＮTotal行×２ＮTotal列の対角行列Ｗを作成する。
【００９０】
【数１９】
【００９１】
行列Ｗを重みとした最小二乗法によってΔｓを求めることを考えると、次の正規方程式
【００９２】
【数２０】
【００９３】
が得られるので、Δｓを次式より算出する。
【００９４】
【数２１】
【００９５】
このように、２次元指標特有の検出時の評価量に基づく信頼度を表す行列Ｗを重みとしてΔｓの算出に利用することで、検出時の指標の評価量に応じて各指標のΔｓの算出への寄与度が変化するという効果を得ることができる。２次元指標特有の検出時の評価量とは、ここでは、撮像装置と指標の相対姿勢である。即ち、より信頼できる指標である可能性が高い指標をΔｓの算出に積極的に利用し、検出精度が低い可能性が高い指標から受ける悪影響を軽減するという効果を得ることができる。
【００９６】
なお、Ｍ推定のような統計的推定手法によって指標の再投影誤差ΔｕＰｋnｉに応じた重みを求め、ωＰｋｎとの積を得てこれを重みとして用いてもよい。
【００９７】
ここで、Δｓは６次元ベクトルであるから、２ＭTotalが６以上であれば、Δｓを求めることができる。式２１のようにΔｓを求めることができるが、他の方法によって求めてもよい。例えば、式２０は連立一次方程式であるので、ΔｓをＧａｕｓｓの消去法で解いてもよいし、その他のいずれの手法で解いてもよい。
【００９８】
ステップＳ４０７０において、位置姿勢算出部２０８０は、ステップＳ４０６０において算出した補正値Δｓを用いて、以下の式２２に従ってｓを補正し、得られた値を新たなｓとする。
【００９９】
【数２２】
【０１００】
ステップＳ４０８０において、位置姿勢算出部２０８０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ４０３０以降の処理を行う。
【０１０１】
ステップＳ４０８０において計算が収束したと判定されると、ステップＳ４０９０において、位置姿勢算出部２０８０は、世界座標系における撮像装置の位置及び姿勢ｓを出力する。この時の出力の形態は、ｓそのものであっても良いし、ｓの位置成分を3値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓから生成した座標変換行列Ｍであっても良い。
【０１０２】
以上の処理によって、世界座標系に対する（すなわち、世界座標系における）撮像装置の位置または位置と姿勢を取得することができる。以上のように、本実施形態に係る位置姿勢計測装置によれば、２次元形状を持つ指標の特有の評価量を考慮して位置姿勢推定を行うので、検出精度の低い指標の影響を軽減し、撮像装置の位置姿勢計測を高精度に行うことができる。
【０１０３】
［第２の実施形態］
第１の実施形態では、信頼度ωＰｋｎは、撮像装置と指標の相対姿勢に基づく評価量に基づいて決定されていた。しかし、二次元形状を持つ指標の幾何形状の特性を利用した評価量に基づいて信頼度を算出するのであれば、評価量とする情報の種類はこれに限定されるものではない。即ち、信頼度の算出方法はその他の方法、またはその他の方法と組み合わせた方法であってもよい。例えば、指標に関する２次元幾何情報（例えば、面積や縦横比など）を画像から得て、これを評価値として信頼度を算出することができる。
【０１０４】
例えば、検出された指標の画像上での面積ＳＰｋｎを評価値ＶＳＰｋｎとして信頼度ωＰｋｎを算出してもよい。この場合、撮像装置概略位置姿勢入力部２０４０は不要となる。
【０１０５】
評価量算出部２０６０は、データリストに保持している指標の各頂点の画像座標pｋnｉを用いて、画像上で検出した指標領域の画像中における面積ＳＰｋｎを算出し、これを夫々の指標の評価量とする。指標の面積は、指標検出部２０３０の指標検出工程で得られるラベリング画像を指標検出部２０３０から評価量算出部２０６０へと入力し、当該指標領域の画素数をカウントすることで算出してもよい。ここでラベリング画像とは、例えば指標領域以外の画素の画素値を０に、各指標領域に相当する画素の画素値をその画像上における夫々の指標領域の識別番号に設定した画像である。
【０１０６】
信頼度算出部２０７０は、画像上で検出した指標の面積ＳＰｋｎが大きいとき高い信頼度を、面積ＳＰｋｎが小さいとき低い信頼度を与えるような重み関数ωＳ（ＶＳＰｋｎ）を用いて、夫々の指標の信頼度を算出する。面積ＳＰｋｎの大小の判定は、分散や標準偏差を用いてもよいし、実験的、経験的に求めてもよい。
【０１０７】
また、このようにして得られた指標の面積に基づく信頼度ωＳＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得、これを用いてもよい。
【０１０８】
＜変形例１＞
第２の実施形態で求めた信頼度の算出方法において、指標が四角形指標の場合には、画像上で検出した辺の縦横比を評価値ＶratioＰｋｎとして信頼度ωＰｋｎを算出してもよい。
【０１０９】
評価量算出部２０６０は、データリストに保持している指標の各頂点の画像座標pｋnｉを用いて、例えば、夫々の頂点毎に存在する隣り合う２辺に対して、「見かけ上の短辺／見かけ上の長辺」によって長さの比を求める。そして、これらのうちの最小値を選択することによって評価量を得る。信頼度算出部２０７０は、例えば、指標が図３に示すような四角形指標であるとき評価値が１に近い指標に高い信頼度を、評価値が０に近い指標には低い信頼度を与えるような重み関数ωratio（ＶratioＰｋｎ）を用いて、夫々の指標の信頼度を算出する。
【０１１０】
また、このようにして得られた検出した辺の縦横比に基づく信頼度ωratioＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１１１】
＜変形例２＞
第２の実施形態で求めた信頼度の算出方法において、指標が四角形指標の場合には、画像上で検出した指標における隣り合う２辺の間の角度を評価値ＶangleＰｋｎとして信頼度ωＰｋｎを算出してもよい。
【０１１２】
隣り合う２辺の組み合わせは夫々の頂点毎に存在するので、例えば、夫々の２辺間のなす角を求め、これらのうちの最小値を評価値とする。例えば、四角形指標の隣り合う辺の間の角度が９０°付近であれば大きな信頼度を、０°付近であれば小さな信頼度を与えるような関数ωangle（ＶangleＰｋｎ）を重み関数として定義する。
【０１１３】
また、このようにして得られた指標における隣り合う２辺の間の角度に基づく信頼度ωangleＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１１４】
＜変形例３＞
第２の実施形態で求めた信頼度の算出方法において、円形の平面形状からなる円形指標を指標として用いる場合には、画像上で検出した指標の真円度を評価値ＶcircleＰｋｎとして信頼度ωＰｋｎを算出してもよい。この場合、指標検出部２０３０から評価量算出部２０６０へとラベリング画像が出力される。
【０１１５】
真円度とは，円形指標の投影像として検出された領域（この領域は、円形指標が正面から撮影された場合には真円に近い形状となり、斜め方向から撮影された場合には楕円に近い形状となる）の真円からのずれ量である。円真度は、例えば、検出領域の外接円と内接円の半径の差によって定義できる。
【０１１６】
信頼度算出部２０７０は、このｒを夫々の指標領域に対して求め、これを評価値ＶcircleＰｋｎとする。信頼度算出部２０７０は、ｒが小さな指標には高い信頼度を、逆にｒが大きな指標には低い信頼度を与えるような重み関数ωcircle（ＶcircleＰｋｎ）を用いて、夫々の指標の信頼度を算出する。
【０１１７】
また、このようにして得られた円形マーカの真円度に基づく信頼度ωcircleＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１１８】
＜変形例４＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、複数種の指標を混在して用いる場合に、夫々の指標の種類に応じて信頼度を定義してもよい。ここで複数の指標とは、例えば、内部にＩＤ情報を有するような多角形形状の指標、画像情報からは同定することができない単色領域からなる円形指標（以上の指標は人為的な指標）、エッジやテクスチャといった自然特徴によって定義される自然特徴指標である。
【０１１９】
例えば、円形指標や自然特徴指標などに比べて誤検出や同定間違いが起きにくいＩＤ情報を持つ多角形指標には高い信頼度を与える。自然特徴と比べると相対的に誤検出の可能性が低い円形指標には中位の信頼度を与える。最も誤検出の可能性の高い自然特徴指標には低い信頼度を与える。そして、これらの信頼度に応じて上記式２１に基づく位置姿勢算出を行っても良い。
【０１２０】
また、このようにして得られた指標の種類に基づく信頼度をそのまま用いるだけでなく、その他の方法で求めた例えば指標の幾何形状の特性を利用した信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２１】
＜変形例５＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、検出した指標が識別子を持ち、かつ認識の誤りを修正する誤り修正情報を持つとき、誤り修正の有無を評価値ＶIDmdfＰｋｎとして信頼度ωＰｋｎを算出してもよい。何らかの理由で情報が抜けた場合等に指標の誤り修正が行われるため、誤り修正が行われた指標には低い信頼度を、逆に誤り訂正が行われない指標に高い信頼度を与えるような関数ωIDmdf（ＶIDmdfＰｋｎ）を重み関数として定義する。
【０１２２】
また、このようにして得られた誤り修正の有無に基づく信頼度ωIDmdfＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２３】
＜変形例６＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、画像上で検出した指標のコントラストの強さを評価値ＶＣＰｋｎとして信頼度ωＰｋｎを算出してもよい。コントラストＣは、画像の明暗の差を表したもので、画像中の濃淡レベルの最小値をＩｍｉｎ、最大値をＩｍａｘとすると次式で定義される。
【０１２４】
【数２３】
【０１２５】
コントラストＣを指標の周辺領域において調べ、その値を評価値ＶＣＰｋｎとする。コントラストが高い指標には、検出精度が高くなるので高い信頼度を、コントラストが低い指標は、検出精度が低くなるので低い信頼度を与えるような関数ωＣ（ＶＣＰｋｎ）を重み関数として定義する。
【０１２６】
また、このようにして得られたコントラストに基づく信頼度ωＣＰｋｎをそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２７】
＜変形例７＞
第１の実施形態及び第２の実施形態で求めた信頼度の算出方法において、画像上で検出した指標の鮮鋭度を評価値ＶsharpＰｋｎとして信頼度ωＰｋｎを算出してもよい。鮮鋭度の計測方法は、エッジの斜度を計測する方法であってもよいし、像の空間周波数成分を計測する方法であってもよいし、その他の何れの方法でもよい。指標の周辺の鮮鋭度が高いとき、エッジがはっきり検出できるため高い信頼度を、逆に鮮鋭度が低いときは、低い信頼度を与えるような関数ωsharp（ＶsharpＰｋｎ）を重み関数として定義する。
【０１２８】
また、このように得られた鮮鋭度に基づく信頼度ωsharpＰｋｎをそのまま信頼度ωＰｋｎとして用いてもよいし、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１２９】
＜変形例８＞
第１の実施形態では、撮像装置概略位置姿勢入力部２０４０は次のような処理を行っていた。即ち、世界座標系における撮像装置２０１０の位置姿勢の概略値ＭＷＣを求め、概略値ＭＷＣと世界座標系における位置または位置姿勢が既知の指標より撮像装置２０１０と指標の相対姿勢を求めていた。しかし、４点以上の特徴点を有する指標を用いる場合には次のようにしても良い。即ち、２次元ホモグラフィの計算に基づいて画像上で検出した指標の画像座標より指標と撮像装置２０１０の概略位置姿勢ＭＭＣを求め、これに基づいて、指標の法線ベクトルと撮像装置２０１０から指標への視軸とのなす角を算出してもよい。
【０１３０】
具体的には、求めた指標と撮像装置２０１０の概略相対位置に基づいて、撮像装置を基準とした座標系（以降、撮像装置座標系とする）における指標と撮像装置２０１０の視点とを結ぶ共線ベクトル
【０１３１】
【数２４】
【０１３２】
を求める。そして、撮像装置座標系における指標の各頂点の座標（ＭＭＣから算出可能）に基づいて、撮像装置座標系における指標の法線ベクトル
【０１３３】
【数２５】
【０１３４】
を求める。以上のようにして、指標の法線ベクトル
【０１３５】
【数２６】
【０１３６】
と共線ベクトル
【０１３７】
【数２７】
【０１３８】
が得られたら、以降は第１の実施形態と同様な処理を行えばよい。
【０１３９】
また、このように得られた撮像装置２０１０と指標との相対姿勢に基づく信頼度ωθＰｋｎをそのまま信頼度ωＰｋｎとして用いてもよい。また、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得て、これを用いてもよい。
【０１４０】
＜変形例９＞
第２の実施形態では、指標の２次元幾何情報を画像から得て、これを評価量として信頼度を算出していた。しかし、撮像装置の概略位置姿勢ＭＷＣに基づいて指標の各頂点の推定座標（再投影座標）を求め、再投影座標の２次元幾何情報に基づいて信頼度を算出してもよい。
【０１４１】
撮像装置２０１０の概略位置姿勢ＭＷＣが実際の位置姿勢（真値）から大きくずれていなければ、画像上で検出した座標の代わりに再投影座標を用いて信頼度を算出しても本発明の本質が損なわれないことはいうまでもない。
【０１４２】
＜変形例１０＞
第１の実施形態及び第２の実施形態では、指標の位置は既知であるとして撮像装置の位置姿勢を求めていたが、指標の位置または位置姿勢も撮像装置の位置姿勢とともにを求めてもよい。
【０１４３】
このとき、求めるべき位置または位置姿勢情報は、すなわち世界座標系における撮像装置２０１０の位置姿勢と、世界座標系における指標の位置または位置姿勢である。世界系座標における指標の位置を３値ベクトル[ｘＰｋｎ ｙＰｋｎ ｚＰｋｎ]、または位置姿勢を６値ベクトル［ｘＰｋｎ ｙＰｋｎ ｚＰｋｎ ξＰｋｎ ψＰｋｎ ζＰｋｎ］Ｔとして扱い、この未知パラメータを状態ベクトルｓＰｋｎと記述する。
【０１４４】
状態ベクトルｓＰｋｎに適当な初期値を与える。初期値としては、オペレータが予めおおよその値を手入力しても良い。また、ある同一時刻に入力された（すなわち、撮影画像から検出された）複数の指標の検出座標をデータリストから抽出し、このデータを用いて、世界座標系系における指標の同時刻における位置または位置姿勢を公知の方法によって算出する。ここで、世界座標系における既知の点が３点以上必要となる。
【０１４５】
また、世界座標系における撮像装置の位置姿勢を６値のベクトル［ｘＷＣ ｙＷＣ ｚＷＣ ξＷＣ ψＷＣ ζＷＣ］Ｔとして扱う。そして、この未知パラメータを状態ベクトルｓＷＣと記述する。求めるべき位置または位置姿勢の状態ベクトルをｓ＝[ｓＷＣ ｓＰｋｎ]と記述する。
【０１４６】
なお、世界座標系における指標の位置又は位置姿勢を算出する公知の手法としては例えば、指標が同一平面上に配置されている場合には、４点以上の指標を用いて２次元ホモグラフィの計算に基づいて指標の位置又は位置姿勢を得る手法を用いることができる。また、同一平面上にない６点以上の指標を用いる手法や、これらの解を初期値にして、ニュートン法のような繰り返し計算によって最適解を得る手法を用いることもできる。
【０１４７】
ここで位置姿勢算出部２０８０は、各々の指標の頂点ｐｋｎｉに対して、その画像座標の推定値（再投影座標）uＰｋｎｉ’を算出する。uＰｋｎｉ’の算出は、指標の頂点ｐｋｎｉの世界座標ｘｗＰｋｎｉと現在の状態ベクトルｓの関数（観測方程式）
【０１４８】
【数２８】
【０１４９】
に基づいて行われる。
【０１５０】
以降の処理については、第１の実施形態と同様に算出する。即ち、第１の実施形態と同様に信頼度を求め、信頼度を対角成分にもつ行列Ｗを重みとし、重みを考慮した最小二乗法で算出することで、指標の位置または位置姿勢と撮像装置の位置姿勢を求めることができる。
【０１５１】
また、撮像装置の位置と姿勢（外部パラメータ）のみではなく、撮像装置の内部パラメータ（焦点距離やアスペクト比、歪み補正パラメータ）を求める問題であっても、同様の重み付けの枠組みを適用できることは言うまでもない。
【０１５２】
＜変形例１１＞
また、上記実施形態においては、図３（ｂ）に示すような複数の頂点を有する２次元形状を持つ指標（三角形や四角形等の外形から成る指標）を用いていた。しかし２次元形状を持つ指標と同様に、複数の座標の集合を構成要素として有するような指標（これらを総称して多角形形状と呼ぶ）を用いる場合であっても、画像上の２次元幾何特徴やカメラと指標の３次元幾何特徴に基づいた評価量を求めることは可能である。従って、本実施形態で述べた発明の効果を得ることが可能である。
【０１５３】
例えば、図３（ｃ）に示すようにそれぞれの相対的な位置関係が既知の点ｐｋｉ（ｉ＝１，，，Ｎｋ）によって構成される指標であってもよい。また、複数の座標の集合を構成要素として有するような指標であればいずれの指標を用いてもよい。
【０１５４】
＜変形例１２＞
なお、上記実施形態においては、最適化演算において非線形方程式をテーラー展開し、１次近似で線形化することで補正値を求め、繰り返し補正していくことで最適解を求めるＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法を用いている。しかし、補正値の算出は必ずしもＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよい。また、最急降下法を用いても求めてもよい。他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。
【０１５５】
［第３の実施形態］
上記実施形態では、図１に示した位置姿勢計測装置２０００を構成する各部はハードウェアでもって実装されているものとして説明した。しかし、図１に示した各部の一部若しくは全部をソフトウェアでもって実現し、残りをハードウェアでもって実現しても良い。この場合、例えば、このハードウェアをパーソナルコンピュータに挿入可能な機能拡張カードとして実現し、この機能拡張カードをパーソナルコンピュータに挿入する。また、このソフトウェアは、このパーソナルコンピュータが有するメモリ上に格納する。係る構成によれば、パーソナルコンピュータが有するＣＰＵがこのソフトウェアを実行すると共に、この機能拡張カードの動作制御をも行うことにより、第１の実施形態及び各種の変形例で説明した処理を行うことができる。
【０１５６】
図６は、位置姿勢計測装置２０００に適用可能なコンピュータのハードウェア構成を示すブロック図である。
【０１５７】
６０１はＣＰＵで、ＲＡＭ６０２やＲＯＭ６０３に格納されているプログラムやデータを用いて本コンピュータ全体の制御を行う。
【０１５８】
６０２はＲＡＭで、外部記憶装置６０６からロードされたプログラムやデータ、Ｉ／Ｆ（インターフェース）６０７を介して外部から受信したプログラムやデータを一時的に記憶するためのエリアを有する。更には、ＣＰＵ６０１が各種の処理を実行する際に用いるワークエリアも有する。即ち、ＲＡＭ６０２は各種のエリアを適宜提供することができる。
【０１５９】
６０３はＲＯＭで、本コンピュータの設定データやブートプログラムなどを格納する。
【０１６０】
６０４は操作部で、キーボードやマウスなどにより構成されており、本コンピュータの操作者が操作することで、各種の指示をＣＰＵ６０１に対して入力することができる。
【０１６１】
６０５は表示部で、ＣＲＴや液晶画面などにより構成されており、ＣＰＵ６０１による処理結果を画像や文字などでもって表示することができる。
【０１６２】
６０６は外部記憶装置で、ハードディスクなどに代表される大容量情報記憶装置であって、ここにはＯＳ（オペレーティングシステム）や、本コンピュータが行う各種の処理をＣＰＵ６０１に実行させるためのプログラムやデータ等が保存される。このプログラムやデータには、上記ソフトウェアや、機能拡張カード６０８の動作制御プログラムなども含まれる。また、外部記憶装置６０６には、上記説明において予め保持しているものとして説明した各種の情報や、Ｉ／Ｆ６０７を介して撮像装置２０１０から受信した撮像画像も保存される。
【０１６３】
外部記憶装置６０６に保存されている各種の情報はＣＰＵ６０１による制御に従って適宜ＲＡＭ６０２にロードされる。そしてＣＰＵ６０１がこのロードされたプログラムやデータを用いて処理を実行することにより、本コンピュータは第１の実施形態及び各種の変形例で説明した処理を実行することができる。
【０１６４】
６０７はＩ／Ｆで、本コンピュータを上記撮像装置２０１０に接続するためのものである。
【０１６５】
６０８は機能拡張カードである。
【０１６６】
６０９は上述の各部を繋ぐバスである。
【０１６７】
［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。
【０１６８】
また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１６９】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれたとする。その後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１７０】
本発明を上記記録媒体に適用する場合、その記録媒体には、先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０１７１】
【図１】第１の実施形態に係る位置姿勢算出装置の概略構成を示す図である。
【図２】第１の実施形態に係る位置姿勢算出方法の概略処理を示すフローチャートである。
【図３】第１の実施形態に係る指標の概略構成を示す図である。
【図４】第１の実施形態に係る指標と撮像装置の相対姿勢を示す図である。
【図５】第１の実施形態に係る位置姿勢算出部２０８０の概略処理を示すフローチャートである。
【図６】位置姿勢計測装置２０００に適用可能なコンピュータのハードウェア構成を示すブロック図である。
【特許請求の範囲】
【請求項１】
撮像装置によって撮像された撮像画像を入力する画像入力工程と、
物体上に配置または設定された指標を前記撮像画像から検出する指標検出工程と、
前記撮像画像から検出されている夫々の指標の信頼度を算出する手がかりとして用いるための評価量を算出する指標評価量算出工程と、
前記指標評価量算出工程で算出した指標の評価量に応じて当該指標の信頼度を算出する信頼度算出工程と、
前記信頼度算出工程で算出された指標の信頼度と、前記指標検出工程で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める位置姿勢算出工程とを有し、
前記評価量は、前記指標の画像上での２次元幾何特徴および／または前記指標の３次元空間中における前記撮像装置と前記指標との間の関係を表す３次元幾何特徴に基づくことを特徴とする位置姿勢計測方法。
【請求項２】
前記指標評価量算出工程では、前記撮像画像を撮影した時点での撮像装置の位置姿勢と指標の配置情報を用いて、画像上における当該指標の再投影座標を求め、該再投影座標を用いて当該指標の２次元幾何情報を算出し、これを当該指標の評価量とし、
前記信頼度算出工程では、前記２次元幾何情報に基づいて当該指標の信頼度を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項３】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、当該指標の２次元幾何情報を前記撮像画像から取得し、
前記信頼度算出工程では、前記２次元幾何情報に基づいて当該指標の信頼度を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項４】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、前記撮像装置と当該指標の相対姿勢を取得し、
前記信頼度算出工程では、前記相対姿勢に基づいて当該指標の信頼度を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項５】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、前記指標検出工程で検出された当該指標の画像座標を用いて、指標を撮影した時点での前記撮像装置と当該指標の相対姿勢を取得することを特徴とする請求項４に記載の位置姿勢計測方法。
【請求項６】
前記指標評価量算出工程では、前記撮像画像を撮影した時点での撮像装置の位置姿勢と指標の配置情報を用いて、前記撮像装置と当該指標の相対姿勢を取得することを特徴とする請求項４に記載の位置姿勢計測方法。
【請求項７】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、画像上における当該指標の面積を取得し、
前記信頼度算出工程では、前記面積に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項８】
前記指標評価量算出工程では、前記指標が多角形形状を有する場合、前記撮像画像から検出されている夫々の指標の評価量として、画像上における当該指標の隣り合う辺の比を取得し、
前記信頼度算出工程では、前記隣り合う辺の比に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項９】
前記指標評価量算出工程では、前記指標が円形状を持つとき、前記撮像画像で検出されている夫々の指標の評価量として、画像上における当該指標の真円度を取得し、
前記信頼度算出工程では、前記真円度に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項１０】
前記指標評価量算出工程では、前記指標が多角形形状を有する場合、前記撮像画像から検出されている夫々の指標の評価量として、画像上における当該指標の隣り合う各辺で作られる角度を取得し、
前記信頼度算出工程では、前記隣り合う各辺で作られる角度に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項１１】
前記位置姿勢算出工程では、前記信頼度算出工程で算出された当該指標の信頼度と、前記指標検出工程で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める過程において、当該指標の位置または位置姿勢をも求めることを特徴とする請求項１乃至請求項１０の何れか１項に記載の位置姿勢計測方法。
【請求項１２】
撮像装置によって撮像された撮像画像を入力する画像入力手段と、
物体上に配置または設定された指標を前記撮像画像から検出する指標検出手段と、
前記撮像画像で検出されている夫々の指標の信頼度を算出する手がかりとして用いるための評価量を、当該指標の画像上での２次元幾何特徴および／または当該指標の３次元空間中における撮像装置と指標との間の関係を表す３次元幾何特徴を利用して算出する指標評価量算出手段と、
前記指標評価量算出手段で取得した指標の評価量に応じて当該指標の信頼度を算出する信頼度算出手段と、
前記信頼度算出手段で算出された当該指標の信頼度と、前記指標検出手段で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める位置姿勢算出手段と
を有することを特徴とする位置姿勢計測装置。
【請求項１３】
コンピュータに請求項１乃至請求項１１の何れか１項に記載の位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【請求項１】
撮像装置によって撮像された撮像画像を入力する画像入力工程と、
物体上に配置または設定された指標を前記撮像画像から検出する指標検出工程と、
前記撮像画像から検出されている夫々の指標の信頼度を算出する手がかりとして用いるための評価量を算出する指標評価量算出工程と、
前記指標評価量算出工程で算出した指標の評価量に応じて当該指標の信頼度を算出する信頼度算出工程と、
前記信頼度算出工程で算出された指標の信頼度と、前記指標検出工程で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める位置姿勢算出工程とを有し、
前記評価量は、前記指標の画像上での２次元幾何特徴および／または前記指標の３次元空間中における前記撮像装置と前記指標との間の関係を表す３次元幾何特徴に基づくことを特徴とする位置姿勢計測方法。
【請求項２】
前記指標評価量算出工程では、前記撮像画像を撮影した時点での撮像装置の位置姿勢と指標の配置情報を用いて、画像上における当該指標の再投影座標を求め、該再投影座標を用いて当該指標の２次元幾何情報を算出し、これを当該指標の評価量とし、
前記信頼度算出工程では、前記２次元幾何情報に基づいて当該指標の信頼度を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項３】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、当該指標の２次元幾何情報を前記撮像画像から取得し、
前記信頼度算出工程では、前記２次元幾何情報に基づいて当該指標の信頼度を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項４】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、前記撮像装置と当該指標の相対姿勢を取得し、
前記信頼度算出工程では、前記相対姿勢に基づいて当該指標の信頼度を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項５】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、前記指標検出工程で検出された当該指標の画像座標を用いて、指標を撮影した時点での前記撮像装置と当該指標の相対姿勢を取得することを特徴とする請求項４に記載の位置姿勢計測方法。
【請求項６】
前記指標評価量算出工程では、前記撮像画像を撮影した時点での撮像装置の位置姿勢と指標の配置情報を用いて、前記撮像装置と当該指標の相対姿勢を取得することを特徴とする請求項４に記載の位置姿勢計測方法。
【請求項７】
前記指標評価量算出工程では、前記撮像画像から検出されている夫々の指標の評価量として、画像上における当該指標の面積を取得し、
前記信頼度算出工程では、前記面積に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項８】
前記指標評価量算出工程では、前記指標が多角形形状を有する場合、前記撮像画像から検出されている夫々の指標の評価量として、画像上における当該指標の隣り合う辺の比を取得し、
前記信頼度算出工程では、前記隣り合う辺の比に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項９】
前記指標評価量算出工程では、前記指標が円形状を持つとき、前記撮像画像で検出されている夫々の指標の評価量として、画像上における当該指標の真円度を取得し、
前記信頼度算出工程では、前記真円度に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項１０】
前記指標評価量算出工程では、前記指標が多角形形状を有する場合、前記撮像画像から検出されている夫々の指標の評価量として、画像上における当該指標の隣り合う各辺で作られる角度を取得し、
前記信頼度算出工程では、前記隣り合う各辺で作られる角度に基づいて当該指標の信頼度を算出することを特徴とする請求項２または請求項３に記載の位置姿勢計測方法。
【請求項１１】
前記位置姿勢算出工程では、前記信頼度算出工程で算出された当該指標の信頼度と、前記指標検出工程で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める過程において、当該指標の位置または位置姿勢をも求めることを特徴とする請求項１乃至請求項１０の何れか１項に記載の位置姿勢計測方法。
【請求項１２】
撮像装置によって撮像された撮像画像を入力する画像入力手段と、
物体上に配置または設定された指標を前記撮像画像から検出する指標検出手段と、
前記撮像画像で検出されている夫々の指標の信頼度を算出する手がかりとして用いるための評価量を、当該指標の画像上での２次元幾何特徴および／または当該指標の３次元空間中における撮像装置と指標との間の関係を表す３次元幾何特徴を利用して算出する指標評価量算出手段と、
前記指標評価量算出手段で取得した指標の評価量に応じて当該指標の信頼度を算出する信頼度算出手段と、
前記信頼度算出手段で算出された当該指標の信頼度と、前記指標検出手段で検出された夫々の指標の画像座標に関する情報とを少なくとも用いて、前記物体または撮像装置の位置姿勢を求める位置姿勢算出手段と
を有することを特徴とする位置姿勢計測装置。
【請求項１３】
コンピュータに請求項１乃至請求項１１の何れか１項に記載の位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２００８−１１６３７３（Ｐ２００８−１１６３７３Ａ）
【公開日】平成２０年５月２２日（２００８．５．２２）
【国際特許分類】
【出願番号】特願２００６−３００９８１（Ｐ２００６−３００９８１）
【出願日】平成１８年１１月６日（２００６．１１．６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２０年５月２２日（２００８．５．２２）
【国際特許分類】
【出願日】平成１８年１１月６日（２００６．１１．６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]