(57)【要約】【課題】  誤検出指標や検出精度の低い指標による影響を減少させ、位置姿勢計測を高精度かつ安定して行う為の技術を提供すること。【解決手段】  撮像装置によって時系列で撮像される撮像画像から、物体上に配置または設定された指標を検出する（Ｓ６０１０）。注目フレームにおける撮像画像において検出されている夫々の指標について、注目フレームよりも過去のフレームからの検出状況を取得する（Ｓ６０２０）。取得した指標の検出状況に応じて、この指標の信頼度を算出する（Ｓ６０３０）。検出された夫々の指標の画像座標に関する情報と、指標の信頼度とを少なくとも用いて、物体または撮像装置の位置及び姿勢を算出する（Ｓ６０５０）。【選択図】  図２
2008134161.tif 000002
【特許請求の範囲】
【請求項１】
撮像装置によって時系列で撮像される各フレームの撮像画像を入力手段が入力する入力工程と、
物体上に配置または設定された指標を前記撮像画像から検出手段が検出する検出工程と、
注目フレームにおける撮像画像において検出されている夫々の指標について、当該注目フレームよりも過去のフレームからの検出状況を取得手段が取得する取得工程と、
前記取得工程で取得した指標の検出状況に応じて、当該指標の信頼度を算出手段が算出する算出工程と、
前記検出工程で検出された夫々の指標の画像座標に関する情報と、前記算出工程で算出された当該指標の信頼度とを少なくとも用いて、前記物体または前記撮像装置の位置及び姿勢を位置姿勢算出手段が求める位置姿勢算出工程と
を備えることを特徴とする位置姿勢計測方法。
【請求項２】
前記取得工程では、前記検出状況として、前記各フレームの撮像画像における指標の検出の連続性を取得し、
前記算出工程では、前記連続性に基づいて夫々の指標の信頼度を算出することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項３】
前記取得工程では、前記検出状況として、前記各フレームの撮像画像における指標の検出回数を取得し、
前記算出工程では、前記検出回数に基づいて夫々の指標の信頼度を算出することを特徴とする請求項１又は２に記載の位置姿勢計測方法。
【請求項４】
撮像装置によって時系列で撮像される各フレームの撮像画像を入力する入力手段と、
物体上に配置または設定された指標を前記撮像画像から検出する検出手段と、
注目フレームにおける撮像画像において検出されている夫々の指標について、当該注目フレームよりも過去のフレームからの検出状況を取得する取得手段と、
前記取得手段が取得した指標の検出状況に応じて、当該指標の信頼度を算出する算出手段と、
前記検出手段によって検出された夫々の指標の画像座標に関する情報と、前記算出手段によって算出された当該指標の信頼度とを少なくとも用いて、前記物体または前記撮像装置の位置及び姿勢を求める位置姿勢算出手段と
を備えることを特徴とする位置姿勢計測装置。
【請求項５】
コンピュータに請求項１乃至３の何れか１項に記載の位置姿勢計測方法を実行させるためのプログラム。
【請求項６】
請求項５に記載のプログラムを格納したことを特徴とする、コンピュータ読み取り可能な記憶媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、視点位置姿勢計測の精度向上と安定性を向上させる技術に関する。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない融合を目的とした複合現実感に関する研究が盛んに行われている。複合現実画像は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間の画像を重畳描画することにより生成される。そして、複合現実感システムで利用される画像表示装置は、例えば、ビデオシースルー方式によって実現される。ここで、仮想空間の画像とは、例えば、コンピュータ・グラフィックスにより描画された仮想物体や文字情報等である。
【０００３】
複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置の位置姿勢と称する）を求める問題に帰結される。
【０００４】
この問題を解決する方法として次のような試みがなされている（非特許文献１を参照）。先ず、環境中や対象物体上に、対象の座標系における配置が既知であるような複数の指標を設置あるいは設定する。そして、既知の情報であるその指標の対象座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、対象座標系に対する撮像装置の位置姿勢を求める。
【０００５】
また、撮像装置に慣性センサを装着し、この慣性センサのセンサ計測値を利用することにより、画像情報のみを用いる場合と比べて安定した位置合わせを実現する試みがなされている。例えば、センサ計測値に基づいて推定した撮像装置の位置及び姿勢を指標の検出処理に利用する方法が提案されている。同推定結果を画像に基づく位置及び姿勢算出の初期値として用いる方法も提案されている。さらに、指標が見えない状況においても同推定結果を大まかな位置及び姿勢として用いる方法も提案されている（特許文献１、非特許文献２を参照）。
【０００６】
従来の画像情報を用いた位置合わせ手法は、指標の検出結果がすべて正しいことを前提としていた。さらに、指標検出結果はすべて均等な重みとして計算されていた。そのため、誤検出や検出精度の低い指標による影響を大きく受け、正しい位置姿勢計測が行えない場面が起こり得る。そこで近年、Ｍ推定のような統計的推定手法を導入し、検出した指標（特徴点）の画像上での観察座標と撮像装置の位置姿勢や指標の位置から推定した指標の画像座標（再投影座標）との間の誤差（再投影誤差）を求める。そして、この誤差をもとに検出指標の信頼度を算出し、誤検出指標を排除することや、その影響を下げる手法が提案されている（非特許文献３を参照）。
【非特許文献１】佐藤, 田村：“複合現実感における位置合わせ手法”, 画像の認識・理解シンポジウム(MIRU 2002)論文集I, 情報処理学会シンポジウムシリーズ, vol.2002, no.11, pp.I.61-I.68, July 2002
【非特許文献２】藤井博文，神原誠之，岩佐英彦，竹村治雄，横矢直和，拡張現実のためのジャイロセンサを併用したステレオカメラによる位置合わせ，電子情報通信学会技術研究報告PRMU99-192（信学技報vol.99, no.574, pp.1-8）
【非特許文献３】佐藤，神原，横矢，竹村：“マーカと自然特徴点の追跡による動画像からのカメラ移動パラメータの復元”，電子情報通信学会論文誌，Ｄ−II，ｖｏｌ．Ｊ８６−Ｄ−II，ｎｏ．１０，ｐｐ．１４３１−１４４０，２００３．
【特許文献１】特開２００５−３３３１９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかし、再投影誤差に基づいて信頼度を算出し、指標に重み付けすることで誤検出を排除する手法は、統計的に例外値を求めることで誤検出指標と判定する仕組みである。そのため、画像上で検出される指標の検出誤差はガウス分布等の誤差モデルにあてはまると仮定し、それを基に確からしく検出されている指標が多数ある場合にしか成立しない。したがって、指標が少ない場合などでは誤検出の影響を受けやすく、誤検出排除手法として十分とはいえない。また、例えば照明条件等により指標の検出結果が小刻みに変わる（あるフレームでは検出されていた指標が次のフレームでは検出されない）ような状況では、フレーム間で位置姿勢計測結果の不整合が生じてしまい、安定した位置姿勢計測を行うことができない。
【０００８】
本発明は以上の問題に鑑みてなされたものであり、誤検出指標や検出精度の低い指標による影響を減少させ、位置姿勢計測を高精度かつ安定して行う為の技術を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測方法は以下の構成を備える。
【００１０】
即ち、撮像装置によって時系列で撮像される各フレームの撮像画像を入力手段が入力する入力工程と、
物体上に配置または設定された指標を前記撮像画像から検出手段が検出する検出工程と、
注目フレームにおける撮像画像において検出されている夫々の指標について、当該注目フレームよりも過去のフレームからの検出状況を取得手段が取得する取得工程と、
前記取得工程で取得した指標の検出状況に応じて、当該指標の信頼度を算出手段が算出する算出工程と、
前記検出工程で検出された夫々の指標の画像座標に関する情報と、前記算出工程で算出された当該指標の信頼度とを少なくとも用いて、前記物体または前記撮像装置の位置及び姿勢を位置姿勢算出手段が求める位置姿勢算出工程と
を備えることを特徴とする。
【００１１】
本発明の目的を達成するために、例えば、本発明の位置姿勢計測装置は以下の構成を備える。
【００１２】
即ち、撮像装置によって時系列で撮像される各フレームの撮像画像を入力する入力手段と、
物体上に配置または設定された指標を前記撮像画像から検出する検出手段と、
注目フレームにおける撮像画像において検出されている夫々の指標について、当該注目フレームよりも過去のフレームからの検出状況を取得する取得手段と、
前記取得手段が取得した指標の検出状況に応じて、当該指標の信頼度を算出する算出手段と、
前記検出手段によって検出された夫々の指標の画像座標に関する情報と、前記算出手段によって算出された当該指標の信頼度とを少なくとも用いて、前記物体または前記撮像装置の位置及び姿勢を求める位置姿勢算出手段と
を備えることを特徴とする。
【発明の効果】
【００１３】
本発明の構成により、誤検出指標や検出精度の低い指標の影響を下げ、高精度かつより安定な撮像装置の位置姿勢計測を行うことができる。
【発明を実施するための最良の形態】
【００１４】
以下添付図面を参照して、本発明をその好適な実施形態に従って詳細に説明する。
【００１５】
［第１の実施形態］
本実施形態では、複数フレームの指標検出状況によって各指標の信頼度を変えて重み付けを行い、誤検出指標や精度の低い指標の排除または影響を下げ、位置合わせの精度を向上させる。以下、本実施形態に係る指標の検出状況に応じた重み付けによる撮像装置の位置姿勢測定方法について説明する。
【００１６】
図１は、本実施形態に係る撮像装置の位置姿勢計測を行う位置姿勢計測装置２０００の機能構成を示すブロック図である。
【００１７】
位置姿勢計測装置２０００は、画像入力部２０２０、指標検出部２０３０、指標検出状況取得部２０４０、信頼度算出部２０５０、撮像装置初期位置姿勢入力部２０６０、データ管理部２０７０、位置姿勢算出部２０８０によって構成されている。また、画像入力部２０２０には、３次元現実空間の動画像を撮像する撮像装置２０１０が接続されている。
【００１８】
本実施形態では、画像入力部２０２０、指標検出部２０３０、指標検出状況取得部２０４０、信頼度算出部２０５０、撮像装置初期位置姿勢入力部２０６０、データ管理部２０７０、位置姿勢算出部２０８０の各部は、ハードウェアで構成されているものとする。従ってこれら各部によっては、ＣＰＵなどの制御部やメモリなどが搭載されている。
【００１９】
また、３次元現実空間中の背景、若しくは同空間中に配置されている対象物体上には、指標が複数配置されている。ここで、物体に配置された指標をＰｋ（ｋ＝１，，，Ｋo）で表す。但し、Ｋoは配置された指標の数（図３（ａ）の例ではＫo＝３）である。
【００２０】
図３は、本実施形態で用いる指標を説明する図である。図３（ｂ）に示すように指標Ｐｋは頂点ｐｋｉ（ｉ＝１，，，Ｎｋ）により構成される。但し、Ｎｋは、指標Ｐｋを構成する頂点の総数（本実施形態の場合はＮｋ＝４）を表す。
【００２１】
以下では、計測の基準とする座標系（ここでは３次元現実空間中の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）を世界座標系と呼称する。また、世界座標系における各指標の位置姿勢は既知であるとする。また、３次元現実空間中に存在する夫々の指標を構成する各頂点は、世界座標系において位置が既知であるとする。
【００２２】
指標Ｐｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各頂点が識別可能であるような指標であれば、何れの形態であってもよい。例えば、図３（ｂ）に示すような四角形指標は内部に自身に固有の識別子を表すパターンを持ち、一意に同定可能である。
【００２３】
このような指標を検出する際には、撮影画像に２値化処理を施した後にラベリング処理を行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として抽出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって候補領域が指標領域であるかを判定し、内部のパターンを読み出すことによって指標の方向と識別子を取得する。
【００２４】
以下、位置姿勢計測装置２０００を構成する各部、及び撮像装置２０１０の動作について説明する。
【００２５】
上述の通り撮像装置２０１０は３次元現実空間の動画像を撮像する。撮像した各フレームの画像（現実空間画像）は、画像入力部２０２０を介して位置姿勢計測装置２０００に入力され、指標検出部２０３０に送信される。
【００２６】
指標検出部２０３０は、画像入力部２０２０から受けた画像中に撮影されている指標Ｐｋを構成する各頂点ｐｋｉの画像座標を検出する。さらに、指標検出部２０３０は、検出された各々の指標Ｐｋnの識別（同定）を行う。そして、指標の識別子ｋnと各頂点pｋnｉの画像座標ｕＰｋnｉとを、指標検出部２０３０による検出結果として、データ管理部２０７０及び指標検出状況取得部２０４０に出力する。
【００２７】
ここで、n（n＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。また、Ｎ個の指標から成る構成点（頂点）の総数をＮTotalとする。例えば図１の場合は、識別子が１、２、３である四角形形状の指標（Ｐ１，Ｐ２，Ｐ３）が撮影される場合を示している。従って、Ｎ＝３であり、識別子ｋ１＝１、ｋ２＝２、ｋ３＝３とこれらに対応する画像座標ｕPｋ１ｉ、ｕPｋ２ｉ、ｕPｋ３ｉ（ｉ＝１、２、３、４）が指標検出部２０３０から出力される。ＮTotalは、１２（＝３×４）になる。
【００２８】
指標検出状況取得部２０４０は、指標検出部２０３０から入力した各フレームにおける指標の検出結果を蓄積・管理する。そして、現在のフレーム（最新フレームであり、以下、第ｆフレームと呼称する）で検出されているそれぞれの指標の検出状況ＶＰｋｎを信頼度算出部２０５０に出力する。
【００２９】
例えば、第ｆフレームで検出されている夫々の指標について、過去のフレームから連続して検出されているフレーム数（連続検出フレーム数）aＰｋｎを、指標Ｐｋnの検出状況ＶＰｋｎとして出力する。本実施形態では、連続検出フレーム数に上限を設け、ある一定フレーム数（例えば１秒間であれば３０フレームであり、以下ではｈフレームとする）以上はカウントしないものとする。
【００３０】
信頼度算出部２０５０は、指標検出状況取得部２０４０が求めた夫々の指標の検出状況ＶＰｋｎに基づいて夫々の指標の信頼度ωＰｋｎを算出し、算出した信頼度ωＰｋｎをデータ管理部２０７０に出力する。
【００３１】
ここで、信頼度算出部２０５０における信頼度算出について説明する。指標の信頼度ωＰｋｎは、検出状況ＶＰｋｎを引数とする重み関数
【００３２】
【数１】
2008134161.tif 000003
【００３３】
によって算出する。重み関数ω（ＶＰｋｎ）は、正（＞０）であり、ロバスト推定手法のひとつであるＭ推定で用いられるような重み関数（確率論的モデルを作り、観測データとよくあてはまるものにより大きな重み付けを行う関数）であってもよい。また、実験的、経験的に求めた関数であってもよい。
【００３４】
本実施形態では、指標ごとの連続検出フレーム数aＰｋｎを、この指標の検出状況ＶＰｋｎと定義している。「より連続したフレームで検出されている指標ほど、信頼できる指標である」という基準を考え、より連続して検出された指標の信頼度を高くするような重み関数を定義する。例えば、それぞれの指標ごとの連続検出フレーム数aＰｋｎと、すべての指標の中で最も多く検出された指標の連続検出フレーム数amaxＰｋｎにより、信頼度ωＰｋｎを次式のように求める。
【００３５】
【数２】
2008134161.tif 000004
【００３６】
または、次式のように求めてもよい。
【００３７】
【数３】
2008134161.tif 000005
【００３８】
また、指標ごとの連続検出フレーム数を反映する信頼度であれば、上記以外の関数を用いてもよい。
【００３９】
撮像装置初期位置姿勢入力部２０６０は、世界座標系における撮像装置２０１０の位置姿勢の初期値ＭＷＣを求め、これをデータ管理部２０７０に出力する。本実施形態では、検出された夫々の指標の信頼度、各頂点の画像座標、各頂点の世界座標系における位置をデータ管理部２０７０から入力し、信頼度が閾値以上の指標の情報のみを用いて撮像装置２０１０の位置姿勢を算出する。
【００４０】
複数の点の画像座標と世界座標系における位置との対応から撮像装置の位置と姿勢とを求める方法（一般にＰｎＰ問題と呼ばれる）は、写真測量やコンピュータビジョンの分野で広く公知であるので、その詳細な説明は省略する。
【００４１】
なお、初期位置姿勢の取得方法はこれに限定するものではなく、例えば、１フレーム前の処理において位置姿勢算出部２０８０によって算出された位置姿勢を保持しておき、これを初期位置姿勢として出力しても良い。また、６自由度位置姿勢センサを撮像装置に装着させてその出力値として得られる概略の位置姿勢を初期位置姿勢として用いてもよい。また、３自由度姿勢センサを撮像装置に装着させることによって得られる撮像装置の姿勢と信頼度が閾値以上となる指標を用いて初期位置姿勢を求めてもよい。あるいは、その他の何れかの公知技術を用いて撮像装置の概略位置姿勢を初期位置姿勢として求めても良い。
【００４２】
ここで、ある三次元座標系Bに対する三次元の座標系Aの位置を表す三次元ベクトルをｔ、姿勢を表す3×3回転行列をＲとする。この場合、A上での位置がｘA（三次元ベクトル）で表される点のB上での座標ｘB（三次元ベクトル）は、以下に示す４×４行列ＭＢＡを用いて同次座標表現によって以下の式５のように表される。
【００４３】
【数４】
2008134161.tif 000006
【００４４】
【数５】
2008134161.tif 000007
【００４５】
本実施形態では、ＭＢＡを座標系Ｂに対する座標系Ａの位置姿勢を表す手段として用いる。
【００４６】
データ管理部２０７０は、予め求めておいた既知の情報である世界座標系における夫々の指標Ｐｋnの位置姿勢ＭＷＭや、各頂点の世界座標系における位置（世界座標）を保持している。また、指標検出部２０３０によって検出された各々の指標Ｐｋnの識別子ｋnと各頂点pｋnｉの画像座標ｕＰｋnｉとを指標検出部２０３０から入力すると共に、夫々の指標の信頼度を信頼度算出部２０５０から入力する。また、撮像装置初期位置姿勢入力部２０６０が求めた世界座標系における撮像装置２０１０の位置姿勢の初期値ＭＷＣをも入力する。そしてこれら入力されたデータを保持し、必要に応じて適宜出力する。
【００４７】
位置姿勢算出部２０８０は、データ管理部２０７０から、あるフレームにおける撮像装置２０１０の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標を取得する。そして取得したこれらを用いて撮像装置２０１０の位置姿勢算出処理を行い、その結果として得られた撮像装置２０１０の位置姿勢情報（即ち、世界座標系における撮像装置２０１０の位置と姿勢）を出力する。
【００４８】
図２は、位置姿勢計測装置２０００が行う撮像装置２０１０の位置姿勢算出処理のフローチャートである。
【００４９】
先ずステップＳ６０１０において指標検出部２０３０は、撮像装置２０１０から画像入力部２０２０を介して入力した最新の画像（第ｆフレーム）から指標の検出処理を行う。そして検出結果（検出された各指標の識別子ｋnと各頂点の画像座標群ｕＰｋnｉ）を、データ管理部２０７０および指標検出状況取得部２０４０に出力する。指標検出部２０３０は、時系列に入力される画像の夫々に対して、常にこの検出処理を実行する。
【００５０】
次にステップＳ６０２０において指標検出状況取得部２０４０は、指標検出部２０３０から検出結果を入力し、夫々の検出指標について第ｆフレームよりも過去のフレームからの検出状況を算出し、算出した検出状況を信頼度算出部２０５０に出力する。検出状況の算出は、例えば、夫々の指標についてカウンタを用意し、現在のフレームで検出されていれば（上限ｈを超えない範囲で）カウンタを１増加させ、検出されていなければカウンタを０にリセットするという処理によって実現できる。
【００５１】
次にステップＳ６０３０において信頼度算出部２０５０は、指標検出状況取得部２０４０から夫々の検出指標の検出状況を入力し、上記重み関数を用いてそれぞれの指標の信頼度を算出する。それぞれの指標について算出した信頼度をデータ管理部２０７０に出力する。
【００５２】
次にステップＳ６０４０において撮像装置初期位置姿勢入力部２０６０は、第ｆフレームにおける世界座標系における撮像装置２０１０の初期位置姿勢ＭＷＣを算出する。そして算出した初期位置姿勢ＭＷＣをデータ管理部２０７０に出力する。
【００５３】
次にステップＳ６０５０において、位置姿勢算出部２０８０は、データ管理部２０７０から第ｆフレームにおける撮像装置２０１０の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標を取得する。そして取得したこれらを用いて、撮像装置２０１０の位置姿勢算出処理を行い、その結果として得られた撮像装置２０１０の位置姿勢情報を出力する。本ステップにおける処理の詳細については図４を用いて後述する。
【００５４】
最後にステップＳ６０６０では、位置姿勢算出処理を終了するか否かの判定が行われる。オペレータが、位置姿勢計測装置２０００に対して、位置姿勢算出処理の終了を指示した場合には、本処理を終了させる。一方、位置姿勢算出処理の継続を指示した場合には、再びステップＳ６０１０へと戻り、次フレーム（第（ｆ＋１）フレーム）の画像に対する処理が実行される。
【００５５】
図４は、上記ステップＳ６０５０における処理の詳細を示すフローチャートである。
【００５６】
先ずステップＳ４０１０において位置姿勢算出部２０８０は、第ｆフレームにおける撮像装置２０１０の初期位置姿勢と、同フレームで検出された各指標の識別子、信頼度、各頂点の画像座標と世界座標を、データ管理部２０７０から取得する。
【００５７】
次にステップＳ４０２０において位置姿勢算出部２０８０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力された指標の頂点の総数ＮTotalの数が３点以上の場合はステップＳ４０３０に処理を進め、３点未満の場合はステップＳ４０９０に処理を進める。例えば、四角形の４頂点で構成される指標１つであればＮTotalは４、三角形の３頂点で構成される指標２つならばＮTotalは６となる。
【００５８】
ここで、位置姿勢算出部２０８０は、世界座標系（あるいは任意の座標系）における撮像装置２０１０の位置姿勢を、求めるべき未知パラメータとして扱う。ここで、姿勢の表現方法として、３値のベクトルα＝［ξ  φ  ζ］Ｔを用いる。αは回転軸・回転角による姿勢の表現方法であり、回転行列Ｒは次式に示すようなαの関数となる。
【００５９】
【数６】
2008134161.tif 000008
【００６０】
このとき、算出すべき撮像装置２０１０の位置姿勢は、位置ｔ=[x y z]Ｔ及び姿勢α＝［ξ  φ  ζ］Ｔによって表現され、求める未知パラメータは、６値の状態ベクトルｓ＝［ｘ  ｙ  ｚ  ξ  φ  ζ］Ｔと記述される。
【００６１】
次にステップＳ４０３０において位置姿勢算出部２０８０は、各々の指標の頂点ｐｋｎｉに対して、その画像座標の推定値（再投影座標）uＰｋｎｉ’を算出する。uＰｋｎｉ’の算出は、指標の頂点ｐｋｎｉの世界座標ｘｗＰｋｎｉと現在の状態ベクトルｓの関数（観測方程式）
【００６２】
【数７】
2008134161.tif 000009
【００６３】
に基づいて行われる。
【００６４】
具体的には、関数Ｆｃ（）は、ｘｗＰｋｎｉからカメラ座標における位置ベクトルｘＣＰｋnｉを求める次式
【００６５】
【数８】
2008134161.tif 000010
【００６６】
及び、ｘＣＰｋnｉから画像上の座標ｕＰｋnｉ’を求める次式
【００６７】
【数９】
2008134161.tif 000011
【００６８】
によって構成されている。ここでｆＢｘ及びｆＢｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置２０１０の焦点距離であり、既知の値として予めデータ管理部２０７０によって保持されているものとする。
【００６９】
次にステップＳ４０４０において位置姿勢算出部２０８０は、各々の指標の頂点ｐｋnｉの実測値ｕＰｋnｉと、それに対応する画像座標の計算値ｕＰｋnｉ’との誤差（再投影誤差）ΔｕＰｋnｉを次式によって算出する。
【００７０】
【数１０】
2008134161.tif 000012
【００７１】
次にステップＳ４０５０において位置姿勢算出部２０８０は、各々の指標の頂点ｐｋnｉに対して、状態ベクトルｓに関する画像ヤコビアンＪｕｓＰｋnｉ（＝∂ｕＰｋnｉ／∂ｓ）を求める。係る画像ヤコビアンとは即ち、式７の関数Ｆｃ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×６列のヤコビ行列のことである。
【００７２】
具体的には、式９の右辺をカメラ座標ｘＣＰｋnｉの各要素で変微分した解を各要素に持つ２行×３列のヤコビ行列ＪｕｘＰｋnｉ（＝∂ｕＰｋnｉ／∂ｘ）と、式８の右辺をベクトルｓの各要素で偏微分した解を各要素に持つ３行×６列のヤコビ行列ＪｘｓＰｋnｉ（＝∂ｘ／∂ｓ）を算出し、次式によってＪｕｓＰｋnｉを算出する。
【００７３】
【数１１】
2008134161.tif 000013
【００７４】
次にステップＳ４０６０において位置姿勢算出部２０８０は、各指標の頂点の誤差ΔｕＰｋnｉ、画像ヤコビアンＪｕｓＰｋnｉ、各指標の信頼度ωＰｋｎに基づいて、状態ベクトルｓの補正値Δｓを算出する。以下、補正値Δｓの算出処理の詳細について説明する。
【００７５】
はじめに、各指標の頂点における再投影誤差を垂直に並べた２ＮTotal次元の誤差ベクトルＵを作成する。
【００７６】
【数１２】
2008134161.tif 000014
【００７７】
また、各指標の頂点におけるヤコビ行列ＪｕｓＰｋnｉを垂直に並べた（２ＮTotal）行×６列の行列Φを作成する。
【００７８】
【数１３】
2008134161.tif 000015
【００７９】
次に、指標Ｐｋｎの各頂点ｐｋnｉに対応する要素（頂点毎にｘ座標とｙ座標の２要素を有する）に対して、この指標の信頼度ωＰｋｎを対角成分に持つような、２ＮTotal行×２ＮTotal列の対角行列Ｗを作成する。
【００８０】
【数１４】
2008134161.tif 000016
【００８１】
行列Ｗを重みとした最小二乗法によってΔｓを求めることを考えると、次の正規方程式
【００８２】
【数１５】
2008134161.tif 000017
【００８３】
が得られるので、Δｓを次式より算出する。
【００８４】
【数１６】
2008134161.tif 000018
【００８５】
このように、指標の検出の連続性を信頼度として表す行列Ｗを重みとしてΔｓの算出に利用することで、検出の連続性に応じて各指標のΔｓの算出への寄与度が変化するという効果を得ることができる。すなわち、過去のフレームから連続的に検出されている指標（より信頼できる指標である可能性が高い）をΔｓの算出に積極的に利用し、連続性の乏しい指標（誤検出指標である可能性が高い）から受ける悪影響を軽減するという効果を得ることができる。
【００８６】
なお、Ｍ推定のような統計的推定手法によって指標の再投影誤差ΔｕＰｋnｉに応じた重みを求め、ωＰｋｎとの積を得てこれを重みとして用いてもよい。
【００８７】
ここで、Δｓは６次元ベクトルであるから、２ＭTotalが６以上であれば、Δｓを求めることができる。なお、本実施形態では式１６に示すようにΔｓを求めているが、他の方法によって求めてもよい。例えば、式１５は連立一次方程式であるので、ΔｓをＧａｕｓｓの消去法で解いてもよいし、その他のいずれの手法で解いてもよい。
【００８８】
次にステップＳ４０７０において位置姿勢算出部２０８０は、ステップＳ４０６０で算出した補正値Δｓを用いて、以下の式１７に従ってｓを補正し、得られた値を新たなｓとする。
【００８９】
【数１７】
2008134161.tif 000019
【００９０】
なお、状態ベクトルｓの初期値としては、撮像装置初期位置姿勢入力部２０６０が求めた「世界座標系における撮像装置２０１０の位置姿勢の初期値ＭＷＣ」を用いる。
【００９１】
次にステップＳ４０８０において位置姿勢算出部２０８０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ４０３０以降の処理を行う。
【００９２】
一方、ステップＳ４０８０において計算が収束したと判定されると、ステップＳ４０９０において位置姿勢算出部２０８０は、世界座標系における撮像装置２０１０の位置及び姿勢ｓを出力する。この時の出力の形態は、ｓそのものであっても良いし、ｓの位置成分を3値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓから生成した座標変換行列Ｍであっても良い。
【００９３】
以上の説明により、世界座標系に対する（すなわち、世界座標系における）撮像装置２０１０の位置及び姿勢、若しくは位置のみを取得することができる。
【００９４】
また、本実施形態によれば、指標の検出の連続性を考慮して位置姿勢推定を行うので、誤検出指標（連続フレーム間で同一の指標として検出されない場合が多い）の影響を軽減し、撮像装置２０１０の位置姿勢計測を安定して行うことができる。
【００９５】
＜変形例１＞
上記説明では、信頼度ωＰｋｎは、過去のフレームから現フレームまでに連続して多く検出された指標に信頼度を高くするように求めていた。しかし、複数フレームにおける指標の検出状況ＶＰｋｎに応じて信頼度を求めるのであれば、検出情報の種類はこれに限定されるものではないし、信頼度の算出方法はその他の方法でもよい。
【００９６】
例えば、現フレームから遡って過去ｈフレームにおける指標の検出回数bＰｋｎを検出状況ＶＰｋｎfreqとし、それに基づいて信頼度ωＰｋｎを求めてもよい。この場合は、検出回数が多い指標に信頼度を高く、検出回数の少ない指標に小さな信頼度を与えるような関数ωfreq（ＶＰｋｎfreq）を重み関数として定義する。例えば、それぞれの指標における検出回数bＰｋｎと、検出された指標のうち最も多く検出された指標の最大検出回数bmaxＰｋｎによって、信頼度を
【００９７】
【数１８】
2008134161.tif 000020
【００９８】
として算出してもよい。または、
【００９９】
【数１９】
2008134161.tif 000021
【０１００】
として算出してもよい。指標の検出回数に基づく信頼度であれば上記以外の計算方法で求めてもよい。また、このようにして得られた過去ｈフレームにおける指標の検出回数に基づく信頼度をそのまま信頼度ωＰｋｎとして用いるだけでなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得るようにしても良い。
【０１０１】
＜変形例２＞
上記変形例１では、過去ｈフレームにおける指標の検出回数bＰｋｎに基づいて信頼度を算出していた。しかし、複数フレームにおける指標の検出状況ＶＰｋｎに応じて信頼度を求めるのであれば、検出回数bＰｋｎを過去ｈフレームにおける連続性と検出回数の両方に基づいて求め、信頼度を計算しても良い。
【０１０２】
例えば、現在のフレームからある過去フレームまでの間で一定回数以上連続して検出された検出回数の合計を検出回数bＰｋｎとする。ここで求めた検出回数bＰｋｎを検出状況ＶＰｋｎとし、それに基づいて信頼度ωＰｋｎを求めてもよい。この場合は、一定回数以上連続して検出された指標の検出回数の合計が多い指標に信頼度を高く、一定回数以上連続して検出された指標の検出回数の合計が少ない指標に小さな信頼度を与えるような関数ωsum（ＶsumＰｋｎ）を重み関数として定義する。例えば、それぞれの指標における検出回数bＰｋｎと、検出された指標のうち最も多く検出された指標の最大検出回数bmaxＰｋｎによって、信頼度を
【０１０３】
【数２０】
2008134161.tif 000022
【０１０４】
として算出してもよい。または、
【０１０５】
【数２１】
2008134161.tif 000023
【０１０６】
として算出してもよい。また、このようにして得られた現在のフレームからある過去フレームまでの間で一定回数以上連続して検出された検出回数の合計に基づく信頼度をそのまま信頼度ωＰｋｎとして用いるだけではなく、その他の方法で求めた信頼度との積や平均を求めることでこれらを組み合わせた信頼度を得てもよい。
【０１０７】
＜変形例３＞
また、上記説明では、図３（ｂ）に示すような複数の頂点を有する指標を用いた。しかし、それぞれが異なる色を有する円形状や球状のマーカを指標として用いる場合や、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点を指標として用いる場合であっても、第１の実施形態と同様の効果を奏することができる。即ち、撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、指標は何れの形態であってもよい。
【０１０８】
円形状や球状のマーカや自然特徴を用いる場合には、指標の情報は世界座標系（基準座標系）における位置ｘｗＰｋｎ＝［ｘｗ、ｙｗ、ｚｗ］で表され、第１の実施形態における指標の頂点と同様に扱うことができる。また、これらの指標（多角形形状の指標、円形状の指標、自然特徴等）を混在して用いる場合であっても、第１の実施形態と同様の効果を得ることが可能であることは、いうまでもない。
【０１０９】
＜変形例４＞
上記説明では、最適化演算において非線形方程式をテーラー展開し、１次近似で線形化することで補正値を求め、繰り返し補正していくことで最適解を求めるＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法を用いている。しかし、補正値の算出は必ずしもＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、最急降下法を用いても求めてもよい。即ち、他の何れの数値計算手法を適用しても良い。
【０１１０】
［第２の実施形態］
第１の実施形態では、位置姿勢計測装置２０００を構成する各部はハードウェアでもって構成されているものとして説明した。しかし、これらの各部のうち一部若しくは全部をソフトウェアでもって構成し、残りを例えば画像処理ボードなどのハードウェアでもって構成するようにしても良い。この場合、このソフトウェアは、一般のＰＣ等のコンピュータにインストールされる。そして、このコンピュータが有するＣＰＵがこのインストールされたプログラムを実行することで、このコンピュータは、位置姿勢計測装置２０００を構成する各部が有する機能を実現することになる。
【０１１１】
図５は、係るコンピュータのハードウェア構成を示すブロック図である。
【０１１２】
ＣＰＵ５０１は、ＲＡＭ５０２やＲＯＭ５０３に格納されているプログラムやデータを用いて本コンピュータ全体の制御を行う。更に、本コンピュータを位置姿勢計測装置２０００に適用した場合には、位置姿勢計測装置２０００が行うものとして上述した各処理（図２，４に示したフローチャートに従った処理）を実行する。
【０１１３】
ＲＡＭ５０２には、外部記憶装置５０６からロードされたプログラムやデータ、Ｉ／Ｆ（インターフェース）５０７を介して撮像装置２０１０から受信した各フレームの画像を一時的に記憶するためのエリアを有する。また、ＲＡＭ５０２は、ＣＰＵ５０１が各処理を実行する際に用いるワークエリアも有する。即ち、ＲＡＭ５０２は、各種のエリアを適宜提供することができる。
【０１１４】
ＲＯＭ５０３には、本コンピュータの設定データや、ブートプログラムなどが格納されている。
【０１１５】
操作部５０４は、キーボードやマウスなどにより構成されており、本コンピュータの操作者が操作することで、各種の指示をＣＰＵ５０１に対して入力することができる。
【０１１６】
表示部５０５は、ＣＲＴや液晶画面等により構成されており、ＣＰＵ５０１による処理結果を画像や文字などでもって表示することができる。
【０１１７】
外部記憶装置５０６は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。ここには、ＯＳ（オペレーティングシステム）や、位置姿勢計測装置２０００を構成する各部（データ管理部２０７０を除く）が有する機能をＣＰＵ５０１に実行させるためのプログラムやデータ等が保存されている。また、データ管理部２０７０が管理しているものとして説明した情報や、既知の情報として説明したものについてもこの外部記憶装置５０６に保存されている。外部記憶装置５０６に保存されているプログラムやデータは、ＣＰＵ５０１による制御に従って適宜ＲＡＭ５０２にロードされ、ＣＰＵ５０１による処理対象となる。
【０１１８】
Ｉ／Ｆ５０７は、上記撮像装置２０１０を本コンピュータに接続するためのもので、撮像装置２０１０が撮像した各フレームの画像は、このＩ／Ｆ５０７を介して外部記憶装置５０６やＲＡＭ５０２に送出される。
【０１１９】
５０８は上述の各部を繋ぐバスである。
【０１２０】
［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。
【０１２１】
また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１２２】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれたとする。その後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１２３】
本発明を上記記録媒体に適用する場合、その記録媒体には、先に説明したフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０１２４】
【図１】本発明の第１の実施形態に係る撮像装置の位置姿勢計測を行う位置姿勢計測装置２０００の機能構成を示すブロック図である。
【図２】位置姿勢計測装置２０００が行う撮像装置２０１０の位置姿勢算出処理のフローチャートである。
【図３】本発明の第１の実施形態で用いる指標を説明する図である。
【図４】ステップＳ６０５０における処理の詳細を示すフローチャートである。
【図５】コンピュータのハードウェア構成を示すブロック図である。
【図１】
2008134161.tif 000024
【図２】
2008134161.tif 000025
【図３】
2008134161.tif 000026
【図４】
2008134161.tif 000027
【図５】
2008134161.tif 000028