JP5620972B2 - 情報処理装置、情報処理方法 - Google Patents

Description

本発明は、撮像装置や現実物体上に装着された姿勢センサの校正技術に関するものである。

現実物体に文字やＣＧ映像を重畳して提示する複合現実感に関する研究が盛んに行われている。複合現実感を提示する提示装置は、ビデオカメラ等の撮像装置で撮像した現実物体の撮像画像に、撮像装置と現実物体との間の位置と姿勢の関係に応じて生成した画像を重畳描画し、これを表示する装置として実現できる。このような提示装置を実現する為には、撮像装置に対する現実物体の位置と姿勢を、リアルタイムに計測する必要がある。

撮像装置に対する現実物体の位置と姿勢を、撮像装置による撮像画像を用いて計測することが一般的に行われている。この方法では、現実物体上に配置したマーカ等の特徴を撮像画像上で検出し、これに基づいて現実物体の位置と姿勢を算出する。

また、撮像装置と現実物体の夫々に姿勢センサを装着し、姿勢センサによる姿勢計測値と、撮像装置による撮像画像とを併用する方法が提案されている（非特許文献１を参照）。この方法によると、撮像画像のみを用いる場合に比べ、現実物体の位置と姿勢を安定して計測することができる。

ところで、非特許文献１に開示されている方法では、撮像装置と現実物体の夫々の姿勢が姿勢センサによって計測可能であることを前提としている。しかし、姿勢センサが実際に出力する姿勢計測値は、姿勢センサ自身の姿勢を表しており、計測対象物体（撮像装置及び現実物体）の姿勢そのものではない。すなわち、姿勢センサによる姿勢計測値をそのまま計測対象物体の姿勢として用いることはできず、何らかの座標変換を行う必要がある。具体的には、姿勢センサ自身の姿勢を、計測対象物体の姿勢に変換する座標変換が必要となる。この座標変換は、計測対象物体を基準とした座標系（以下、計測対象物体座標系と呼称する）における姿勢センサの姿勢によって定義できる。以下では、必要に応じて、計測対象物体座標系における姿勢センサの姿勢を姿勢センサの「配置情報」と呼ぶ。また係る配置情報を姿勢センサの「Ｌｏｃａｌ Ｔｒａｎｓｆｏｒｍ」と呼ぶこともある。また、姿勢センサの配置情報を取得する作業及び処理を姿勢センサの「校正」と呼ぶ。

従来までに行われていた校正の為の方法の一つは、Ｌｏｃａｌ Ｔｒａｎｓｆｏｒｍを表すパラメータをオペレータが対話的に増減する試行錯誤的な方法である。

また、特許文献１には、現実空間中に配置したマーカを複数の視点から撮像した画像を利用して、撮像装置に装着した姿勢センサの配置情報を自動計算する方法が開示されている。この方法によると、非常に簡便な作業によって、撮像装置に装着した姿勢センサの校正を行うことができる。

特開２００５−３２６２７５号公報

小竹，佐藤，内山，山本："傾斜角拘束を利用したハイブリッド位置合わせ手法"，画像の認識・理解シンポジウム(MIRU2006)論文集，pp.241-248，July 2006． K. Satoh, S. Uchiyama, H. Yamamoto, and H. Tamura: "Robust vision-based registration utilizing bird’s-eye view with user’s view," Proc. 2nd IEEE／ACM Int’l Symp. on Mixed and Augmented Reality (ISMAR 2003) , pp.46-55, October 2003. A. I. Comport, E. Marchand, and F. Chaumette, "A real-time tracker for markerless augmented reality," Proc. 2nd IEEE／ACM Int’l Symp. on Mixed and Augmented Reality (ISMAR 2003) , pp.36-45, October 2003.

特許文献１に開示されている手法では、撮像装置以外の任意の現実物体（言い換えると、撮像装置を有さない現実物体）に装着した姿勢センサの校正を行うことはできない。従って従来では、撮像装置以外の任意の現実物体に姿勢センサを装着した場合には、オペレータがこの姿勢センサのＬｏｃａｌ Ｔｒａｎｓｆｏｒｍを試行錯誤的に調整するしかなかった。しかし、この作業は容易ではなく、オペレータに高度なスキルを要求するという課題があった。

また、特許文献１に開示されている手法では、初期値として用いるパラメータを、何らかの方法によって予め推定し、入力しておかなくてはならないという課題があった。

また、特許文献１に開示されている手法では、現実空間中に配置したマーカの３次元位置を定義する世界座標系において、重力軸が何れの方向を向いているかを予め入力しておかなくてはならないという課題があった。

本発明は以上の課題に鑑みてなされたものであり、撮像装置や現実物体に装着された姿勢センサの校正を、簡便かつ正確に行う為の技術を提供することを目的とする。

本発明の目的を達成するために、例えば、本発明の情報処理装置は以下の構成を備える。

即ち、複数の時点において、撮像装置に装着された撮像装置用姿勢センサによる姿勢計測値と、当該撮像装置により撮像される現実物体に装着された現実物体用姿勢センサによる姿勢計測値と、を取得する姿勢計測値の取得手段と、
前記複数の時点で、前記撮像装置が前記現実物体を撮像することで得られる画像を取得する画像の取得手段と、
前記現実物体上に配置され且つ該現実物体上の位置を表す物体座標が既知である複数の指標の、前記複数の時点で撮像されたそれぞれの画像上における画像座標を取得する画像座標の取得手段と、
前記複数の指標についての前記物体座標と前記複数の時点のそれぞれにおける前記画像座標とに基づいて、当該複数の時点のそれぞれにおける前記撮像装置に対する前記現実物体の位置姿勢の推定値を算出する位置姿勢算出手段と、
前記撮像装置に対する前記撮像装置用姿勢センサの配置情報と、前記現実物体に対する前記現実物体用姿勢センサの配置情報と、前記複数の時点のそれぞれにおける前記現実物体の位置の推定値とともに、前記撮像装置用姿勢センサによる姿勢計測値に含まれる方位誤差と、前記現実物体用姿勢センサによる姿勢計測値に含まれる方位誤差との相対値である相対方位誤差の補正値を、前記物体座標と、当該複数の時点における、前記撮像装置用姿勢センサによる姿勢計測値と、前記現実物体用姿勢センサによる姿勢計測値と、前記画像座標と、前記撮像装置に対する前記現実物体の位置姿勢の推定値とを用いて求める配置情報算出手段と
を備えることを特徴とする。

本発明の構成により、撮像装置又は現実物体に装着された姿勢センサの撮像装置又は現実物体に対する姿勢を、簡便かつ正確に取得することができる。また、初期値や重力軸の方向を予め入力しておくという手間を省略することができる。

本発明の第１の実施形態に係るセンサ校正装置を有するシステムの機能構成を示すブロック図である。 センサ校正装置１００、５００に適用可能なコンピュータのハードウェア構成を示すブロック図である。 センサ校正装置１００が行うメインの処理のフローチャートである。 ステップＳ３０６０における処理の詳細を示すフローチャートである。 本発明の第２の実施形態に係るセンサ校正装置を有するシステムの機能構成を示すブロック図である。

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

［第１の実施形態］
本実施形態に係る情報処理装置は、ビデオカメラなどの撮像装置に装着した姿勢センサ（撮像装置用姿勢センサ）と、現実物体に装着した姿勢センサ（現実物体用姿勢センサ）の校正を同時に行う。従って、以下では本実施形態に係る情報処理装置を、センサ校正装置と呼称する。以下では、このセンサ校正装置、及びこのセンサ校正装置が行うセンサ校正方法について説明する。

図１は、本実施形態に係るセンサ校正装置を有するシステムの機能構成を示すブロック図である。図１に示す如く、センサ校正装置１００には、校正対象である姿勢センサ１１０及び姿勢センサ１１５と、ビデオカメラに代表される動画像が撮像可能な撮像装置１２０と、が接続されている。

センサ校正装置１００は、画像取得部１３０、画像処理部１３５、３次元座標保持部１４０、姿勢計測値取得部１４５、指示部１５０、データ管理部１５５、校正部１６０、位置姿勢算出部１７０、画像生成部１８０、表示部１９０によって構成されている。

姿勢センサ１１０は撮像装置１２０に装着されており、撮像装置１２０は姿勢センサ１１０にとっての姿勢計測対象物である。また、姿勢センサ１１５は現実物体１２５に装着されており、現実物体１２５は姿勢センサ１１５にとっての姿勢計測対象物である。本実施形態に係るセンサ校正装置１００の目的は、撮像装置１２０に対する姿勢センサ１１０の配置情報と、現実物体１２５に対する姿勢センサ１１５の配置情報とを求めることである。

夫々の姿勢センサ１１０，１１５は、センサ座標系における自身の姿勢を計測する。姿勢センサは、例えばジャイロセンサと加速度センサをベースとしたセンサユニットである。具体的には、株式会社トキメックのTISS-5-40や、米国InterSense社のInertiaCube3等によって構成される。これらのセンサによって計測される姿勢計測値は、慣性計測値を時間積分した値として算出されるため、蓄積誤差を含んでいる。ただし、これらの姿勢センサは、内蔵する加速度センサによって地球の重力方向を観測することによって、傾斜角方向（ピッチ角及びロール角）の誤差の蓄積をキャンセルする機能を有している。そのため、傾斜角方向に関しては蓄積誤差が発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては、時間の経過に伴って蓄積される誤差（方位誤差）を有している。また、姿勢センサのセンサ座標系は重力軸を基準として設定されるため、複数の姿勢センサのセンサ座標系は、重力軸まわりの１自由度である方位成分を残して共通に定義される。

以下では、姿勢センサ１１０が計測した３自由度の姿勢計測値を、３×３の回転行列R_WSaτと表記する。同様に、姿勢センサ１１５が計測した３自由度の姿勢計測値を３×３の回転行列R_WSbτと表記する。夫々の姿勢センサ１１０，１１５が計測した姿勢計測値は、姿勢計測値取得部１４５にデータとして入力される。

姿勢計測値取得部１４５は、データ管理部１５５から要求を受けると、上記姿勢センサ１１０，１１５のそれぞれから受け取った姿勢計測値のデータをデータ管理部１５５に出力する。また、姿勢計測値取得部１４５は、後述する校正部１６０によって算出された夫々の姿勢センサ１１０，１１５の配置情報（Ｌｏｃａｌ Ｔｒａｎｓｆｏｒｍ）を校正部１６０から受けると、これを保持する。さらに、姿勢計測値取得部１４５は、姿勢センサ１１０，１１５の姿勢計測値をそれぞれ撮像装置１２０、現実物体１２５の姿勢を示す情報へと変換し、変換した情報を位置姿勢算出部１７０へと出力する。係る変換は、姿勢センサ１１０、１１５のそれぞれの姿勢計測値に、対応するＬｏｃａｌ Ｔｒａｎｓｆｏｒｍを乗算することで行われる。

なお、撮像装置１２０の位置と姿勢は、撮像装置１２０上に設定したカメラ座標系の位置と姿勢によって定義される。カメラ座標系とは、撮像装置１２０の撮像系の投影中心を原点とし、光軸方向を−Ｚ軸とおいた座標系であり、Ｘ軸とＹ軸は、Ｚ軸に直交する互いに独立な軸として定義される。また、現実物体１２５の位置と姿勢は、現実物体１２５上に設定した物体座標系の位置と姿勢によって定義される。物体座標系とは、現実物体１２５内の１点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系である。

撮像装置１２０は、現実物体１２５を含む現実空間の動画像を撮像するものであり、撮像した各フレームの画像（撮像画像）は、画像取得部１３０にデータとして入力される。画像取得部１３０は、撮像装置１２０から撮像画像のデータを受けると、これを必要に応じて記憶した後、画像処理部１３５、画像生成部１８０に出力する。

ここで、現実物体１２５上には、撮像装置１２０によって撮像するための指標として、物体座標系における位置（物体座標）が既知である複数個の指標Qk（k = 1, , , K）が配置されている。ここで、kは指標の識別番号を表し、Kは指標の総数を表す。指標Qkは、センサ校正用のデータを取得する時の撮像装置１２０によって、少なくとも４個以上の指標が同時に観測されるように配置されていることが望ましい。図１の例では、４個の指標Q1，Q2，Q3，Q4が現実物体１２５の同一平面上に配置されている。

指標Qkは、例えば、それぞれが異なる色を有する小領域からなるカラーマーカによって構成される。あるいは、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成される。このように、本実施形態で用いることのできる指標としては、撮像画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが何らかの方法で識別可能であるような指標であれば、どのようなものであっても良い。また、指標は故意に（人為的に）設定されたものであってもよい。また、例えば現実物体の角や模様のような、現実物体上に元来存在する特徴を指標として用いてもよい。また、ある程度の面積を有する単色の多角形領域によって形成される多角形マーカを用いてもよい。この場合、多角形の各頂点を指標として扱う。

３次元座標保持部１４０は、現実空間中に配した各指標（図１の場合、Ｑ１〜Ｑ４）について、識別番号と物体座標のセットを保持している。即ち図１の場合には、指標Ｑ１の識別番号と物体座標とのセット、指標Ｑ２の識別番号と物体座標とのセット、指標Ｑ３の識別番号と物体座標とのセット、指標Ｑ４の識別番号と物体座標とのセット、がデータとして３次元座標保持部１４０に保持されている。そして３次元座標保持部１４０は、画像処理部１３５から識別番号を受けると、この識別番号とセットにして保持している物体座標を画像処理部１３５に返信する。以下では、識別番号ｋの指標Qkの物体座標をxO^Qkと表記する。

画像処理部１３５は、画像取得部１３０から撮像画像を受けると、係る撮像画像中に含まれている指標を検出し、その画像座標を求める。更に、画像処理部１３５は、検出した指標を同定し、係る指標の識別番号を特定する。

ここで、指標の検出は、使用する指標の種類に応じた方法で行われる。例えば、指標の夫々が異なる色を有するカラーマーカによって構成されている場合には、撮像画像上から各々のマーカ色に対応する領域を検出する。そして、その重心位置を指標の検出座標とする。また、指標の夫々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、テンプレート画像によるテンプレートマッチングによって指標の位置を検出する。この場合、各々の指標のテンプレート画像は、既知の情報として予め保持しておく。また、四角形マーカを用いる場合は、撮像画像に２値化処理、及びラベリング処理を施し、４つの直線によって形成されているラベル領域をマーカ候補として検出する。さらに、各マーカ候補の矩形領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除する。また、矩形領域中のパターンによって、四角形マーカの識別番号を取得する。最後に、矩形領域の４頂点の座標が、４点の指標の位置として出力される。

このように、撮像画像中の指標の識別番号、画像座標を抽出するための技術については係る分野では周知の技術であるので、画像座標、識別番号を取得するための処理についてより詳細な説明は省略する。

画像処理部１３５は、検出した指標の識別番号を特定すると、検出した指標の物体座標を得るべく、この特定した識別番号を３次元座標保持部１４０に送出する。これにより、３次元座標保持部１４０からは、この特定した識別番号とセットになっている物体座標が返信されるので、これを取得する。即ち、画像処理部１３５は、検出した指標の識別番号を３次元座標保持部１４０に送出することで、この検出した指標の物体座標を３次元座標保持部１４０から取得することができる。このようにして画像処理部１３５は、１枚の撮像画像から検出した全ての指標について、識別番号、画像座標、物体座標を得る。

そして画像処理部１３５は、検出した指標毎の画像座標u^Qkjτと物体座標xO^Qkjのセット（j =１, , , Jτ）を、位置姿勢算出部１７０に出力する。以下では、検出した各セットを対応情報と呼称し、検出した全ての指標のセット（画像座標と物体座標のセット）をまとめて指標情報Ｍτと表記する。従って、指標情報Ｍτには、各指標についての対応情報が含まれていることになる。

ここで、記号τは、画像取得部１３０が撮像装置１２０から撮像画像を取得した時刻（タイミング）を表す識別子である。従って、Ｍτは、画像取得部１３０が識別子τが表す時刻（以下、単に時刻τ）に取得した撮像画像から得られた指標情報であることを示している。なお、係る時刻τについては、不図示のタイマでもって計時しており、センサ校正装置１００を構成する各部は適宜、係るタイマが計時する時刻を取得できるものとする。もちろん、時刻以外にも、「タイミング」を表すことのできるものであれば、如何なるものを用いても良い。

また、記号jは、１枚の撮像画像内に含まれている各指標に対して付けている通し番号を示している。係る記号ｊは説明上登場するものである。また、記号Jτは、画像取得部１３０が時刻τに取得した撮像画像から画像処理部１３５が検出した指標の総数を表す。また、記号kjは、j番目の指標の識別番号を表す。

画像処理部１３５は更に上記指標情報Ｍτを用いて、撮像装置１２０に対する現実物体１２５の位置の推定値t’_COτと姿勢の推定値R’_COτを算出する。ここで、t’_COτは位置を表す３次元のベクトルを表している。また、R’_COτは姿勢を表す３×３の行列を表している。

現実物体上の複数の点の３次元座標と画像座標の対応から、現実物体の位置と姿勢を求める方法は写真測量やコンピュータビジョンの分野において広く知られているので、係る方法についての詳細な説明は省略する。なお、撮像装置１２０に対する現実物体１２５の位置と姿勢の算出は、後述する位置姿勢算出部１７０における処理とは異なり、姿勢センサ１１０，１１５による姿勢計測値は利用していない。

画像処理部１３５は、データ管理部１５５から要求を受けると、指標情報Mτ、現実物体１２５の位置の推定値t’_COτ、姿勢の推定値R’_COτを、データ管理部１５５に出力する。

指示部１５０は、オペレータにより「データ取得」コマンドが入力されたときには、「データ取得」の指示をデータ管理部１５５に送信する。また、「配置情報算出」コマンドが入力されたときには、「配置情報算出」の指示を校正部１６０に送信する。指示部１５０へのコマンド入力は、例えばキーボードを用いて、特定のコマンドを割り当てたキーを押すことによって行う。また、コマンドの入力は、ディスプレイ上に表示されたＧＵＩで行う等、何れの方法で行ってもよい。

データ管理部１５５は、指示部１５０から「データ取得」の指示を受けると、係る指示を受けた時刻に最も近い時刻（時刻τ）に画像取得部１３０が取得した撮像画像に基づいて画像処理部１３５が取得した情報群を取得する。即ち、指標情報Mτ、現実物体１２５の位置の推定値t’_COτ、姿勢の推定値R’_COτを画像処理部１３５から取得する。また、データ管理部１５５は、指示部１５０から「データ取得」の指示を受けると、時刻τに上記姿勢センサ１１０，１１５による姿勢計測値のデータを取得する。

そして、データ管理部１５５は、このようにして取得した各情報をセットにして、自身が管理するデータリストに追加登録する。

・ 時刻τに姿勢計測値取得部１４５が取得した姿勢センサ１１０の姿勢計測値R_WSaτ
・ 時刻τに姿勢計測値取得部１４５が取得した姿勢センサ１１５の姿勢計測値R_WSbτ
・ 時刻τに画像取得部１３０が取得した撮像画像に基づいて求めた現実物体１２５の位置の推定値t’_COτ、姿勢の推定値R’_COτ
・ 時刻τに画像取得部１３０が取得した撮像画像に基づいて求めた指標情報Ｍτ
・ １つ目の指標の画像座標u^Qk1τと物体座標xO^Qk1のセット
・ ２つ目の指標の画像座標u^Qk2τと物体座標xO^Qk2のセット
・ …
そして、データ管理部１５５は、校正部１６０から要求を受けると、係るデータリストを校正部１６０に送出する。

校正部１６０は、指示部１５０から「配置情報算出」の指示を受けると、データ管理部１５５に対して上記データリストの取得要求を行う。そしてデータ管理部１５５がこの取得要求に応じて送出したデータリストを取得する。そして校正部１６０は、この取得したデータリストを用いて、後述する各数値演算処理を行うことで、姿勢センサ１１０の配置情報（即ち、Ｌｏｃａｌ Ｔｒａｎｓｆｏｒｍ）、姿勢センサ１１５の配置情報を求める。そして求めたそれぞれの配置情報は、データとして姿勢計測値取得部１４５に送出され、そこで保持されると共に、必要に応じて外部へと出力される。

以上が、撮像装置１２０上に配置された姿勢センサ１１０と、現実物体１２５上に配置された姿勢センサ１１５の校正を同時に実行する為の構成である。次に説明する構成は、係る構成によって得られた結果を確認するための、複合現実感の提示を行う為の部分である。

位置姿勢算出部１７０は、非特許文献１に開示されている手法によって、撮像装置１２０に対する現実物体１２５の位置と姿勢を算出する。

即ち、画像処理部１３５は上述の通り、撮像装置１２０から１フレーム分の撮像画像が入力される毎に、この撮像画像に基づいて指標情報Ｍτを取得し、取得した指標情報Ｍτを位置姿勢算出部１７０に送出する。従って、位置姿勢算出部１７０はこの指標情報Ｍτを受ける。

また、位置姿勢算出部１７０は、時刻τに最も近い時刻（ほぼ時刻τと見なしても良い）における撮像装置１２０の姿勢、現実物体１２５の姿勢として姿勢計測値取得部１４５が求めたものを取得する。

ここで、「時刻τに最も近い時刻における撮像装置１２０の姿勢」とは、次のようにして求まるものである。即ち、時刻τに最も近い時刻に姿勢計測値取得部１４５が取得した姿勢センサ１１０の姿勢計測値を、姿勢計測値取得部１４５がこの時点で保持している姿勢センサ１１０の配置情報を用いて上述の通り求めたものである。また、「時刻τに最も近い時刻における現実物体１２５の姿勢」とは、次のようにして求まるものである。即ち、時刻τに最も近い時刻に姿勢計測値取得部１４５が取得した姿勢センサ１１５の姿勢計測値を、姿勢計測値取得部１４５がこの時点で保持している姿勢センサ１１５の配置情報を用いて上述の通り求めたものである。

そして位置姿勢算出部１７０は、指標情報Ｍτ、撮像装置１２０の姿勢、現実物体１２５の姿勢、を用いて、非特許文献１に開示されている技術を用いて、撮像装置１２０に対する現実物体１２５の位置と姿勢を算出する。この算出は、与えられたＬｏｃａｌ Ｔｒａｎｓｆｏｒｍが正しいという仮定に基づいて行われるので、得られた位置と姿勢は、Ｌｏｃａｌ Ｔｒａｎｓｆｏｒｍの正確さを反映したものとなる。

画像生成部１８０は、撮像画像を画像取得部１３０から取得する。また、撮像装置１２０に対する現実物体１２５の位置と姿勢を位置姿勢算出部１７０から取得し、取得した位置と姿勢に基づいて描画した仮想物体の画像を撮像画像上に重畳することで、複合現実感を提示する画像（重畳画像）を生成する。そして画像生成部１８０は、生成した画像を、表示部１９０に出力する。

表示部１９０は、複合現実感を提示する画像を画像生成部１８０から取得し、これを表示することでオペレータに複合現実感を提示する。オペレータは係る画像を目視し、提示された複合現実感における現実空間と仮想空間との間の、位置合わせの正確さをもって、校正結果の正確さを判断することができる。なお、表示部１９０は、撮像装置１２０と一体化した構成になっていても良い。その場合の、撮像装置１２０および表示部１９０は、いわゆる頭部装着型ディスプレイ（Ｈｅａｄ Ｍｏｕｎｔｅｄ Ｄｉｓｐｌａｙ）を構成するものとなりうる。

次に、センサ校正装置１００が行うメインの処理について、同処理のフローチャートを示す図３を用いて、以下説明する。なお、図３のフローチャートに従った処理を行う際には、オペレータは、撮像装置１２０と現実物体１２５の夫々の姿勢を様々に変化させながら、「データ取得」コマンドを数回入力する。そしてその後に、「配置情報算出」コマンドを入力するという操作を行う。以下では、「データ取得」コマンドが入力された時刻として記号τを用いる。そして、時刻τが異なるときには、撮像装置１２０及び現実物体１２５の夫々の姿勢は異なっているものとする。

ステップＳ３０１０において、指示部１５０は、「データ取得」コマンドがオペレータから入力されたか否かの判定を行う。指示部１５０は、「データ取得」コマンドが入力されている場合には、「データ取得」の指示をデータ管理部１５５に送信し、ステップＳ３０２０へと処理を移行させる。一方、「データ取得」コマンドが入力されていない場合には、ステップＳ３０５０へと処理を移行させる。

ステップＳ３０２０において、データ管理部１５５は、時刻τに画像取得部１３０が取得した撮像画像に基づいて画像処理部１３５が取得した画像処理結果を画像処理部１３５から取得する。即ち、係る画像処理結果とは上述の通り、指標情報Ｍτ、現実物体１２５の位置の推定値t’_COτ、姿勢の推定値R’_COτである。

ステップＳ３０３０において、データ管理部１５５は、時刻τにおいて姿勢センサ１１０、姿勢センサ１１５が計測した姿勢計測値R_WSaτ、R_WSbτを、姿勢計測値取得部１４５から取得する。

ステップＳ３０４０において、データ管理部１５５は、ステップＳ３０２０，Ｓ３０３０で取得した情報群を組み合わせたセット[R_WSaτ，R_WSbτ，t’_COτ，R’_COτ，M_τ]を、データリストＬに追加登録する。

以上で示したステップＳ３０１０からステップＳ３０４０までの処理によって、１回の「データ取得」指示で取得されるセットがデータリストＬに登録される。そして、ステップＳ３０１０からステップＳ３０４０の処理を複数回実行することにより、複数セットをデータリストＬに登録することができる。このとき、時刻τ（τ= 1, , , T）は、データリストＬに登録されている各セットの識別子と解釈できる。ここでＴは、データリストＬ中に含まれる異なるセット（異なる時刻、異なる姿勢）の数を表している。

ステップＳ３０５０において、指示部１５０は、「配置情報算出」コマンドがオペレータにより入力されたか否かの判定を行う。指示部１５０は、「配置情報算出」コマンドが入力されている場合には、「配置情報算出」の指示を校正部１６０に送信し、ステップＳ３０６０へと処理を移行させる。一方、「配置情報算出」コマンドが入力されていない場合には、ステップＳ３０８０へと処理を移行させる。

ステップＳ３０６０において、校正部１６０は、データ管理部１５５からデータリストＬを取得し、この取得したデータリストＬを用いて、姿勢センサ１１０、１１５のそれぞれの配置情報を算出する処理を行う。係る処理の詳細については、図４のフローチャートを用いて後述する。

次に、ステップＳ３０７０において、校正部１６０は、ステップＳ３０６０で算出された姿勢センサ１１０、１１５のそれぞれの配置情報を、姿勢計測値取得部１４５へと出力する。また、必要に応じて、配置情報をセンサ校正装置１００の外部へと出力する。

ステップＳ３０８０において、位置姿勢算出部１７０は、時刻τにおいて画像取得部１３０が取得した撮像画像に基づいて画像処理部１３５が取得した指標情報Ｍτを、画像処理部１３５から取得する。

ステップＳ３０９０において、位置姿勢算出部１７０は、時刻τにおける撮像装置１２０の姿勢、現実物体１２５の姿勢として姿勢計測値取得部１４５が求めたものを取得する。この取得したそれぞれの姿勢は、姿勢計測値取得部１４５が保持している姿勢センサのＬｏｃａｌ Ｔｒａｎｓｆｏｒｍに基づいて、姿勢計測値に座標変換を施した後の姿勢を表している。従って、ステップＳ３０６０における配置情報算出処理が正しく行われていれば、本ステップで得られる姿勢は、撮像装置１２０の姿勢と現実物体１２５の姿勢を正しく（もちろん、センサの計測誤差は含んでいる）表すものとなる。

ステップＳ３１００において位置姿勢算出部１７０は、ステップＳ３０８０で取得した指標情報Ｍτ、ステップＳ３０９０で取得した撮像装置１２０の姿勢、現実物体１２５の姿勢、に基づいて、撮像装置１２０に対する現実物体１２５の位置と姿勢を算出する。係る算出処理は、例えば非特許文献１に開示されている公知の手法によって行うことができるので、詳細な説明は省略する。そして位置姿勢算出部１７０は、算出した位置と姿勢のデータを、画像生成部１８０へと出力する。また、必要に応じて、算出した位置と姿勢のデータを、外部へと出力する。

ステップＳ３１１０において、画像生成部１８０は、複合現実感を提示する画像を生成して表示部１９０へと出力する。複合現実感を提示する画像の生成は、以下の処理によって行う。まず、画像生成部１８０は、撮像装置１２０に対する現実物体１２５の位置と姿勢を位置姿勢算出部１７０から取得する。さらに、時刻τにおいて画像取得部１３０が取得した撮像画像を、この画像取得部１３０から取得する。そして、撮像装置１２０に対する現実物体１２５の位置と姿勢に基づいて描画した仮想物体の画像を、この撮像画像上に重畳する。仮想物体としては、例えば、現実物体１２５の種類に依存しない情報として、物体座標系の３つの軸を表す矢印を表示してもよい。また、X=0平面，Y=0平面，Z=0平面等をワイヤーフレームで表示してもよい。あるいは、現実物体１２５のワイヤーフレームモデルを予め保持しておいて、これを現実物体１２５上に重畳表示してもよい。現実物体１２５の位置と姿勢が正しく求められていることが確認できるような内容であれば、仮想物体は何れのものであってもよい。

次に、ステップＳ３１２０において、表示部１９０は、複合現実感を提示する画像を画像生成部１８０から取得し、これを表示することでオペレータに複合現実感を提示する。

最後にステップＳ３１３０において、処理を終了するか否かの判定を行う。指示部１５０を介してオペレータにより本処理の終了指示が入力された場合には、本処理を終了させる。一方、本処理の終了指示がない場合には、再びステップＳ３０１０へと処理を戻す。

次に、図４に示すフローチャートを用いて、ステップＳ３０６０における配置情報算出処理の詳細について説明する。図４は、上記ステップＳ３０６０における処理の詳細を示すフローチャートである。

校正部１６０は、求めるべき姿勢センサの配置情報、即ち、Ｌｏｃａｌ Ｔｒａｎｓｆｏｒｍを、３値ベクトルω = [ξ ψ ζ]^Tとして扱う。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。また以下では、必要に応じて、夫々のＬｏｃａｌ Ｔｒａｎｓｆｏｒｍを３×３の回転行列R(ω)によって表記する。回転行列と上記３値ベクトルとの間の変換は、公知の変換式によって容易に行うことができる。以下では、姿勢センサ１１０の配置情報をω_SC = [ξ_SC ψ_SC ζ_SC]^T（未知）、姿勢センサ１１５の配置情報をω_SO = [ξ_SO ψ_SO ζ_SO]^T（未知）と表記する。

校正部１６０は、Ｌｏｃａｌ Ｔｒａｎｓｆｏｒｍを算出する過程において、データリストＬに登録されている各時刻τにおける現実物体１２５の位置t_COτ= [x_COτ y_COτ z_COτ]^T（未知）を推定する。また、各時刻τにおける相対方位誤差の補正値φτ（未知）を推定する。ここで、相対方位誤差とは、姿勢センサ１１０の姿勢計測値に含まれる方位誤差と、姿勢センサ１１５の姿勢計測値に含まれる方位誤差の相対値を表している。なお、相対方位誤差には、夫々の姿勢センサのセンサ座標系の方位成分の差異も含まれている。

以下では、これらの算出対象の未知パラメータを（６＋４Ｔ）次元の状態ベクトルs = [ω_SC ω_SO t_CO１ φ_１ ・・・ t_COτ φ_τ ・・・t_COＴ φ_Ｔ]^Tで記述する。以下の計算では、データリストＬが保持している複数時刻における指標の情報と姿勢計測値とを用いて、状態ベクトルsの値を繰り返し計算によって最適化する。具体的には、夫々の検出指標の画像座標の実測値と、ある状態ベクトルsを仮定した際のこの指標の画像座標の理論値と間の誤差を全体として最小化するように、sの値を更新する。

ステップＳ４０１０において、校正部１６０は、データ管理部１５５からデータリストＬを取得する。

ステップＳ４０２０において、校正部１６０は、状態ベクトルｓに適当な初期値を与える。まず、各τにおけるt_COτの初期値として、データリストＬ中の対応する時刻τにおける現実物体１２５の位置推定値t’_COτを設定する。また、全てのφ_τに共通の初期値として、全周方向を等分割した値からまだ選択されていない値を選択して設定する。例えば、全周方向を６分割した場合には、０度，６０度，１２０度，１８０度，２４０度，３００度の何れかを設定する。また、ω_SCの初期値として、予め用意した幾つかの候補の中から、φ_τの初期値との組み合わせとしてまだ選択されていない姿勢を選択して設定する。さらに、ω_SOの初期値として、上記で設定されたφ_τとω_SCの初期値を利用して、次式によって算出した姿勢を設定する。

ここで、R_WSaτ，R_WSbτ，R’_COτの夫々は、データリストＬから選択したある一つの（ある時刻τの）セットから得た値を表している。また、ΔR(φ_τ)は、方位角方向にφ_τだけの回転（相対方位誤差の補正）を加える３×３の回転行列を表しており、次式によって定義される。

一方、R(ω)は、ωによって決定される回転行列であり、次式によって定義される。

ただし、θは次式によって定義される。

ステップＳ４０３０において、校正部１６０は、データリストＬに登録されている各対応情報に関して、指標の画像座標の理論値を算出する。指標の画像座標の理論値とは、撮像装置１２０に対する現実物体１２５の位置と姿勢に仮説が与えられた時に、この指標が撮像画像中に見えるべき座標を指す。以下では、指標の物体座標xO_iと、画像座標（実測値）u_iのセットに関する画像座標の理論値を、u_i’と表記する。ここで、記号i（i = 1, , , I）は、データリストＬ中の指標の対応情報の夫々に割り振った通し番号であり、Iは指標の対応情報の総数を表す。すなわち、I = Σ_τＪ_τの関係にある。

理論値u_i’は、状態ベクトルsを変数とした観測方程式F_τi()に基づいて、次式によって算出される。

ここで、τ_ｉは、ｉ番目の対応情報が属するセットの時刻を表す。観測方程式F_τi()は、物体座標xO_iからカメラ座標x_Ciへの変換と、カメラ座標x_Ciから画像座標u_i’への変換によって構成される。前者はモデルビュー変換であり、状態ベクトルsの要素であるω_SC，ω_SO，t_COτi，φ_τiを用いて、次式によって定義される。

一方、後者は透視変換であり、次式によって定義される。

ここでfx及びfyは、それぞれx軸方向、y軸方向における撮像装置１２０の焦点距離であり、既知の値として校正部１６０内に予め保持されているものとする。

ステップＳ４０４０において、校正部１６０は、指標の対応情報の夫々に関して（即ち全てのiについて）、実測値として得られた画像座標u_iと、それに対応する画像座標の理論値u_i’との誤差Δu_iを次式によって算出する。

ステップＳ４０５０において、校正部１６０は、全てのiに対して、状態ベクトルsに関する画像ヤコビアンJ_i（＝∂u_i／∂s）を算出する。画像ヤコビアンとは、（式５）の観測方程式F_τi（）を、状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×（６＋４Ｔ）列の行列（ヤコビ行列）である。具体的には先ず、（式７）の右辺をカメラ座標系上の位置ベクトルx_Ciの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列J_{u_xCi}（＝∂u_i／∂x_Ci）を算出する。また、（式６）の右辺を状態ベクトルsの各要素で偏微分した解を各要素に持つ３行×（６＋４Ｔ）列のヤコビ行列J_{xCi_s}（＝∂x_Ci／∂s）を算出する。そして、次式によって画像ヤコビアンJ_iを算出する。

ヤコビ行列J_{u_xCi}の導出方法は公知であるので、その説明は省略する。例えば、非特許文献２等にその導出方法が開示されている。一方、J_{xCi_s}の算出は以下のようにして行う。以下の計算を説明するために、モデルビュー変換を表す（式６）を、次の４つの式に分解して考える。

これらの式に従うと、J_{xCi_s}は、ω_SCとその他の要素（これをs’と置く）に関する二つの行列に、以下のように分解できる。

ここで、J_{xCi_ωSC}はω_SCによる（式１０）の偏微分であり、（式１０）、（式１１）から以下のように導出できる。

ここで、J_{xCi_rWCτ}はR_WCτiの各要素による（式１０）の偏微分を表している。J_{rWCτ_rSC}はR(ω_SC)の各要素による（式１１）の偏微分を表している。J_{rSC_ωSC}はω_SCによるR(ω_SC)の偏微分を表している。また、J_{xCi_xWi}はx_Wiによる（式１０）の偏微分を表している。また、J_{xWi_s}’はs’による（式１２）の偏微分を表しており、次式のように、s’の各要素に分解して求めることができる。

ここで、J_{xWi_ωSO}はω_SOによる（式１２）の偏微分であり、（式１２）、（式１３）から以下のように導出できる。

ここで、J_{xWi_rWOτ}はR_WOτiの各要素による（式１２）の偏微分を表している。J_{rWOτ_rSO}はR(ω_SO)の各要素による（式１３）の偏微分を表している。J_{rSO_ωSO}はω_SOによるR(ω_SO)の偏微分を表している。また、J_{xWi_tτ}は、t_COτiによる（式１２）の偏微分を表しており、（式１２）から以下のように導出できる。

また、J_{xWi_φτ}はφ_τiによる（式１２）の偏微分を表しており、（式１２）及び（式１３）から以下のように導出できる。

ここで、J_{rWOτ_Δrτ}はR(φ_τi)の各要素による（式１３）の偏微分を表している。J_{Δrτ_φτ}はφ_τiによるR(φ_τi)の偏微分を表している。

これらのヤコビ行列を順次算出することで、J_{xCi_s}を求めることができる。なお、これ以上の式の導出は一般的な偏微分の計算によって可能であるので省略する。

ステップＳ４０６０において、校正部１６０は、以上のステップで算出した、全てのｉに対する誤差Δu_i及びヤコビ行列J_iに基づいて、sの補正値Δsを算出する。具体的には、全てのiに対する誤差Δu_iを垂直に並べた誤差ベクトルU及び全てのiに対するヤコビ行列J_iを垂直に並べた行列Φを作成し、次式によってΔsを算出する。

ここで、Φ^＋はΦの擬似逆行列Φ^＋であり、例えばΦ^＋＝（Φ^ＴΦ）^−１Φ^Ｔによって求めることができる。Φ^＋の導出は、他の何れの手法によってもよい。

なお、以上で述べたステップＳ４０６０の処理によって補正値を算出することは、非線形最適化手法の一種であるガウス・ニュートン法を適用していることに相当する。誤差ベクトルUとヤコビ行列Φを利用した補正値の算出方法はガウス・ニュートン法以外にも提案されており、例えば、レーベンバーグ・マーカート法や、最急降下法等を用いてもよい。また、外れ値を除外するＭ推定等のロバスト推定手法を組み合わせてもよいし、その他の何れの解法を用いてもよい。

ステップＳ４０７０において、校正部１６０は、ステップＳ４０６０において算出した補正値Δsを用いて、次式に従って状態ベクトルsを補正し、得られた値を新たなsとする。

ステップＳ４０８０において、校正部１６０は、更新された状態ベクトルsを用いて、ステップＳ４０３０と同一の処理により、指標の対応情報の夫々に関してその画像座標の理論値を算出する。

ステップＳ４０９０において、校正部１６０は、更新された状態ベクトルsを用いて、ステップＳ４０４０と同一の処理により、指標の対応情報の夫々に関してその画像座標の実測値と理論値との誤差Δu_iを算出する。

ステップＳ４１００において、校正部１６０は、何らかの判断基準を用いて、反復処理を継続するか否かの判断を行う。これ以上の反復処理は不要と判断した場合には、ステップＳ４１１０へと処理を進める。一方、反復処理を行うと判断した場合には、更新されたsを用いて、ステップＳ４０５０からステップＳ４０９０までの処理を再度実行する。

反復処理を行うか否かの判断は、例えば、計算が収束または発散しているか否かの判定によって行う。計算が収束も発散もしていないと判定した場合には、反復処理を継続する。一方、計算が収束あるいは発散していると判定した場合には、反復処理を終了する。また、収束や発散の判定を行わずに、一定回数の反復処理を必ず行うようにしてもよい。収束や発散の判定は、例えば、ステップＳ４０９０で更新された誤差ベクトルUの大きさによって行うことができる。例えば、収束の判定を、誤差ベクトルUの大きさが予め定めた閾値より小さいか否か、あるいは、誤差ベクトルUの大きさの変化量が予め定めた閾値より小さいか否かといった基準によって行う。何れも閾値よりも小さければ、処理を終了すると判断する。

また、発散の判定を、誤差ベクトルUの大きさが増加傾向にあるか否か、あるいは、誤差ベクトルUの大きさが予め定めた閾値より大きいか否かといった基準によって行う。また、補正値Δsの大きさによって収束判定を行うこともできる。

このような反復計算の終了条件を用いて反復計算を有限回数行うことについては従来からなされていることであるので、これについてのこれ以上の説明は省略する。

ステップＳ４１１０において、校正部１６０は、ステップＳ４０２０で設定した初期値に対して行った最適化計算（ステップＳ４０３０〜ステップＳ４１００）が、成功したか否かの判定を行う。具体的には、ステップＳ４０９０で更新された誤差ベクトルUの大きさが予め定めた閾値以下であれば、最適化計算が成功したものと判断する。最適化計算が失敗したと判断した場合には、処理をステップＳ４０２０へと移行する。そして、状態ベクトルの初期値として他の候補を再設定し、以降の最適化計算（ステップＳ４０３０〜ステップＳ４１００）を再度実行する。一方、最適化計算が成功したと判断した場合には、処理を終了する。

以上の処理によって、撮像装置１２０や現実物体１２５に装着した姿勢センサの校正を、簡便かつ正確に行うことができる。また、初期値や重力軸の方向を予め入力しておくという手間を必要としない。

［第２の実施形態］
第１の実施形態では、撮像装置１２０に姿勢センサ１１０が装着されていた。しかし、現実物体１２５に装着した姿勢センサ１１５の配置情報を計測するという目的のみを考えると、撮像装置１２０に姿勢センサ１１０が装着されていることは必須ではない。本実施形態では、撮像装置１２０に姿勢センサを設けない場合のシステムについて説明する。

図５は、本実施形態に係るセンサ校正装置を有するシステムの機能構成を示すブロック図である。図５において、図１に示したものと同じものについては同じ番号を付けており、その説明は省略する。図５に示した構成が図１に示した構成と異なる点は、以下の３点である。

・ 撮像装置１２０には姿勢センサが設けられていない
・ 撮像装置１２０は三脚５００でその姿勢が固定されている
・ センサ校正装置１００には新たに姿勢取得部１６５が付加されている
以下では、この３点を中心に説明する。もちろん、以下説明する点以外については第１の実施形態と同様である。

撮像装置１２０は、三脚５００上に設置されている。撮像装置１２０の姿勢R_WCは予め計測されており、データとして外部のメモリ５９９内に保持されている。撮像装置１２０の姿勢R_WCを得る為には、世界座標系における３次元位置が既知である指標を現実空間中に幾つか配置し、その幾つかの指標を撮像装置１２０によって撮像する。そして、指標の画像座標を検出し、指標の世界座標と画像座標との対応関係から世界座標系における撮像装置１２０の姿勢を算出すればよい。なお、撮像装置１２０の姿勢R_WCとしては、重力軸（の逆方向を示す軸）に対するカメラ座標系の傾斜が得られていればよい。即ち、世界座標系における方位は任意の方向でよく、Ｙ軸が重力軸の逆方向と一致していればよい。

姿勢取得部１６５は、外部のメモリ５９９内に保持されている撮像装置１２０の姿勢R_WCのデータを読み出す。以下の処理では、第１の実施形態における姿勢センサ１１０の姿勢計測値R_WSaτを、姿勢R_WCに置き換えて考えればよい。また、第１の実施形態においては導出対象の一つであった姿勢センサ１１０の配置情報ω_SCを既知の単位行列と考えればよい。

そして以降の処理は、第１の実施形態の場合と同様である。第１の実施形態からの変更点として、状態ベクトルsやヤコビ行列J_iから、ω_SCに関する成分を除外する。そして、この新たなsに対して、第１の実施形態の最適化計算と同様な処理を実行する。なお、ω_SCが既知であるので、ステップＳ４０２０で設定される初期値の組み合わせは、φのみによって決定される。すなわち、φの初期値としてある角度を選択すると、（式１）によりω_SOの初期値が一意に決定される。

なお、撮像装置１２０は、その姿勢R_WCが何らかの手段によって計測可能であれば、必ずしも固定されている必要はない。例えば世界座標系の指標の観測によって計測し続けてもよい。また、磁気式センサや光学式センサなどのセンサを撮像装置１２０に装着することで、このセンサにより撮像装置１２０の位置姿勢を計測してもよい。

＜変形例１＞
上記の実施形態では、夫々が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を指標として用いていた。しかし、点指標以外の指標を用いることも可能である。例えば、エッジのような線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。

線指標を用いる場合の処理の概要を以下に述べる。線特徴を用いる場合であっても、複数の時刻における指標の情報と夫々の姿勢計測値を入力として、指標の投影誤差を評価基準として状態ベクトルsを最適化するという枠組みは同じである。線指標を用いる場合、画像処理部１３５における現実物体１２５の位置と姿勢の推定は、例えば、非特許文献３に開示されている手法によって行う。

校正部１６０では先ず、状態ベクトルがsであるときに線指標上のいくつかの点（分割点と呼ぶ）が観察されるであろう画像座標の理論値を算出する。そして、その画像座標の近辺で実際に検出されるエッジ（実測値）までの法線方向の距離を、分割点の誤差と定義する。そして、全ての分割点に関する誤差によって誤差ベクトルＵを構成する。一方、状態ベクトルsの関数として定義される上記の誤差の導出式をsの各要素で偏微分して、その解を各要素に持つ１行×（６＋４Ｔ）列のヤコビ行列Φを構成する。

そして、以上で得られる誤差ベクトルＵ及びヤコビ行列Φを用いて上記実施形態と同様な最適化計算を行うことで、配置情報の算出を行うことができる。もちろん、線指標と点指標、その他の指標から得られる誤差、及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することも可能である。

＜変形例２＞
上記の実施形態では、校正部１６０における最適化計算において、時刻τにおける現実物体１２５の位置t_COτを未知としていた。しかし、画像処理の結果としてデータリストＬ中に保持されている各時刻τにおける現実物体１２５の位置の推定値t’_COτをt_COτとおいて、最適化計算工程においてはt_COτを既知の値として扱ってもよい。この場合、上記の実施形態からの変更点として、状態ベクトルsやヤコビ行列J_iから、t_COτに関する成分を除外すればよい。例えば、第１の実施形態の場合であれば、状態ベクトルsを、s = [ω_SC ω_SO φ_１ … φ_τ … φ_Ｔ]^Tと定義する。そして、このsに対して上記の実施形態における最適化計算と同様な処理を実行する。

また、上記の実施形態では、校正部１６０の最適化計算において、姿勢センサの相対方位誤差は時刻毎に異なるものと仮定していた。しかし、校正のためのデータを取得する短時間においては、相対方位誤差は一定と仮定してもよい。例えば、第１の実施形態の場合であれば、状態ベクトルsを、s = [ω_SC ω_SO φ t_CO１ … t_COτ … t_COＴ]^Tと定義できる。そして、このsに対して上記の実施形態の最適化計算と同様な処理を実行する。ここで、φは、時刻τに依存しない相対方位誤差の補正値を表している。

もちろん、本変形例で述べた双方の仮定をおいて、s = [ω_SC ω_SO φ]^Tを未知パラメータとした最適化計算を行うことも可能である。

＜変形例３＞
第１の実施形態では、姿勢センサ１１０の配置情報と姿勢センサ１１５の配置情報を同時に求めていた。しかし、何れか一方の姿勢センサが既に校正済みの場合には、校正済みの姿勢センサの配置情報は既知としてもよい。この場合、校正部１６０の処理（ステップＳ３０６０）のみを変更すればよい。

まず、姿勢センサ１１５が校正済みであり、姿勢センサ１１０のみが未校正である場合を考える。この場合、姿勢センサ１１５の配置情報ω_SOは既知となるので、第１の実施形態からの変更点として、状態ベクトルsやヤコビ行列J_iから、ω_SOに関する成分を除外すればよい。そして、このsに対して、第１の実施形態の最適化計算と同様な処理を実行する。なお、ω_SOが既知の場合、ステップＳ４０２０におけるω_SCの算出は、次式によって行えばよい。

このとき、φの初期値としてある角度を選択すると、（式２４）によりω_SCの初期値が一意に決定される。

次に、姿勢センサ１１０が校正済みであり、姿勢センサ１１５のみが未校正である場合を考える。この場合、姿勢センサ１１０の配置情報ω_SCは既知となるので、第２の実施形態と同様に、第１の実施形態の状態ベクトルsやヤコビ行列J_iから、ω_SCに関する成分を除外すればよい。

＜変形例４＞
上記の実施形態において、姿勢センサ１１０、姿勢センサ１１５は３軸の姿勢を計測するセンサであった。しかし、これらの姿勢センサが重力軸に対する２軸の傾斜角のみを計測するセンサ（一般的に傾斜センサと呼ばれる）の場合であっても、同様の方法によって姿勢センサの配置情報を算出することはできる。この場合、最適化処理におけるパラメータφ_τは、相対方位誤差の補正値ではなく、撮像装置１２０と現実物体１２５との間の相対的な方位角の差を表すことになる。

＜変形例５＞
上記の実施形態では、指標の検出及び同定を画像処理によって行っていた。しかし、現実物体１２５の形状や状況によっては、物体座標系上での位置が既知でかつ画像処理によって検出及び同定可能な指標を用意することが困難な場合がある。このような場合には、指標の情報を手入力で与えてもよい。すなわち、オペレータがモニタに表示された撮像画像上においてマウス等のポインティングデバイスを用いて指標の画像座標を入力し、さらに、指標の識別番号を入力するような構成としてもよい。これは、物体座標系上での位置が既知でかつ画像上において目視で位置を特定可能な点がある場合に有効である。

また、指標の検出のみが画像処理によって可能な場合には、同定処理のみをオペレータが手入力で行ってもよい。また、画像処理によって検出及び／または同定された指標の誤りを手入力によって修正できるようにしてもよい。また、画像処理によって検出及び／または同定する指標と、手入力によって検出及び／または同定する指標とを組み合わせて用いてもよい。

［第３の実施形態］
図１，５に示したセンサ校正装置１００、５００を構成する各部は全てハードウェアでもって構成されているものとして説明した。しかし、データを保持するための構成を除くその他の各部のうち一部若しくは全部をソフトウェア（コンピュータプログラム）でもって構成しても良い。この場合、係るソフトウェアは、ＰＣ（パーソナルコンピュータ）等のコンピュータが有するメモリ上にロードされ、このコンピュータが有するＣＰＵが係るソフトウェアを実行することになる。これにより、係るコンピュータは、図１，５に示したセンサ校正装置１００、５００と同様の処理を行うことができる。

図２は、センサ校正装置１００、５００に適用可能なコンピュータのハードウェア構成を示すブロック図である。

ＣＰＵ１００１は、ＲＡＭ１００２やＲＯＭ１００３に格納されているプログラムやデータを用いてコンピュータ全体の制御を行うと共に、本コンピュータを適用するセンサ校正装置１００、５００が行う上述の各処理を実行する。

ＲＡＭ１００２は、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータ、Ｉ／Ｆ（インターフェース）１００９を介して外部から受信したプログラムやデータ、を一時的に記憶するためのエリアを有する。また、ＲＡＭ１００２は、ＣＰＵ１００１が各種の処理を実行する際に用いるワークエリアも有する。即ち、ＲＡＭ１００２は、各種のエリアを適宜提供することができる。

ＲＯＭ１００３は、本コンピュータの設定データや、ブートプログラムなどを格納する。

キーボード１００４，マウス１００５は、本コンピュータの操作者がＣＰＵ１００１に対して各種の指示を入力するための入力デバイスとしての一例であり、その他の入力デバイスを用いても良い。例えば、上記指示部１５０に対して入力するコマンドは、キーボード１００４やマウス１００５を用いて行う。

表示部１００６は、上記表示部１９０に相当するものであり、ＣＲＴや液晶画面などにより構成されている。表示部１００６は、ＣＰＵ１００１による処理結果を画像や文字などでもって表示することができる。例えば上述のように、撮像画像と仮想物体の画像とが合成された画像を表示することができる。

外部記憶装置１００７は、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置１００７内には、ＯＳ（オペレーティングシステム）や、センサ校正装置１００、５００が行う上述の各処理をＣＰＵ１００１に実行させるためのプログラムやデータが保存されている。また、外部記憶装置１００７には、以上の説明において、既知の情報として説明したものについても保存されている。

外部記憶装置１００７に保存されているプログラムやデータは、ＣＰＵ１００１による制御に従って適宜ＲＡＭ１００２にロードされる。そしてＣＰＵ１００１はこのロードされたプログラムやデータを用いて処理を実行することで、本コンピュータは、センサ校正装置１００、５００が行う上述の各処理を実行することになる。

記憶媒体ドライブ１００８は、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記録されているプログラムやデータを読み出し、読み出したプログラムやデータをＲＡＭ１００２や外部記憶装置１００７に送出するものである。係る記憶媒体には、外部記憶装置１００７に保存されているものとして説明した情報のうち一部を記録しておくようにしても良い。また、係る記憶媒体は、上記外部のメモリ５９９として機能するようにしても良い。

Ｉ／Ｆ１００９には、上記姿勢センサ１１０、１１５、撮像装置１２０が接続されている。Ｉ／Ｆ１００９は、以下の複数種のポートによって構成されている。撮像画像を取得するという画像取得部１３０の機能の一部は、アナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポートによって実現される。また、姿勢計測値を取得するという姿勢計測値取得部１４５の機能の一部は、ＵＳＢポートやＲＳ−２３２Ｃポートによって実現される。入力データは、Ｉ／Ｆ１００９を介してＲＡＭ１００２や外部記憶装置１００７に取り込まれる。また、校正部１６０が算出した夫々の姿勢センサの配置情報や、位置姿勢算出部１７０が算出した物体の位置及び姿勢は、必要に応じて、イーサネット（登録商標）ポート等（Ｉ／Ｆ１００９の機能の一部）によって外部へ出力される。

１０１０は上述の各部を繋ぐバスである。

なお、センサ校正装置１００、５００に適用可能なコンピュータのハードウェア構成については図２に示した構成に限定するものではなく、当業者であれば適宜変形しうるものである。

［その他の実施形態］
また、本発明の目的は、以下のようにすることによって達成されることはいうまでもない。即ち、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記録媒体（または記憶媒体）を、システムあるいは装置に供給する。係る記憶媒体は言うまでもなく、コンピュータ読み取り可能な記憶媒体である。そして、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムコードを読み出し実行する。この場合、記録媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記録した記録媒体は本発明を構成することになる。

また、コンピュータが読み出したプログラムコードを実行することにより、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行う。その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれたとする。その後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

本発明を上記記録媒体に適用する場合、その記録媒体には、先に説明したフローチャートに対応するプログラムコードが格納されることになる。

Claims (6)

1. 複数の時点において、撮像装置に装着された撮像装置用姿勢センサによる姿勢計測値と、当該撮像装置により撮像される現実物体に装着された現実物体用姿勢センサによる姿勢計測値と、を取得する姿勢計測値の取得手段と、
   前記複数の時点で、前記撮像装置が前記現実物体を撮像することで得られる画像を取得する画像の取得手段と、
   前記現実物体上に配置され且つ該現実物体上の位置を表す物体座標が既知である複数の指標の、前記複数の時点で撮像されたそれぞれの画像上における画像座標を取得する画像座標の取得手段と、
   前記複数の指標についての前記物体座標と前記複数の時点のそれぞれにおける前記画像座標とに基づいて、当該複数の時点のそれぞれにおける前記撮像装置に対する前記現実物体の位置姿勢の推定値を算出する位置姿勢算出手段と、
   前記撮像装置に対する前記撮像装置用姿勢センサの配置情報と、前記現実物体に対する前記現実物体用姿勢センサの配置情報と、前記複数の時点のそれぞれにおける前記現実物体の位置の推定値とともに、前記撮像装置用姿勢センサによる姿勢計測値に含まれる方位誤差と、前記現実物体用姿勢センサによる姿勢計測値に含まれる方位誤差との相対値である相対方位誤差の補正値を、前記物体座標と、当該複数の時点における、前記撮像装置用姿勢センサによる姿勢計測値と、前記現実物体用姿勢センサによる姿勢計測値と、前記画像座標と、前記撮像装置に対する前記現実物体の位置姿勢の推定値とを用いて求める配置情報算出手段と
   を備えることを特徴とする情報処理装置。
2. 前記配置情報算出手段は、
   前記撮像装置に対する前記撮像装置用姿勢センサの配置情報と、前記現実物体に対する前記現実物体用姿勢センサの配置情報と、を未知パラメータとし、当該未知パラメータに基づいて決まる前記指標の画像座標の理論値と、当該指標について前記画像座標の取得手段が取得した画像座標と、の誤差を小さくするように前記未知パラメータを求めることを特徴とする請求項１に記載の情報処理装置。
3. 前記配置情報算出手段が算出した配置情報に基づいて、仮想物体の画像を生成する手段と、
   前記撮像装置により撮像された画像上に、前記仮想物体の画像を重畳した重畳画像を生成する手段と、
   前記重畳画像を表示する手段と
   を備えることを特徴とする請求項１又は２に記載の情報処理装置。
4. 情報処理装置が行う情報処理方法であって、
   前記情報処理装置が有する姿勢計測値の取得手段が、複数の時点において、撮像装置に装着された撮像装置用姿勢センサによる姿勢計測値と、当該撮像装置により撮像される現実物体に装着された現実物体用姿勢センサによる姿勢計測値と、を取得する姿勢計測値の取得工程と、
   前記情報処理装置が有する画像の取得手段が、前記複数の時点で、前記撮像装置が前記現実物体を撮像することで得られる画像を取得する画像の取得工程と、
   前記情報処理装置が有する画像座標の取得手段が、前記現実物体上に配置され且つ該現実物体上の位置を表す物体座標が既知である複数の指標の、前記複数の時点で撮像されたそれぞれの画像上における画像座標を取得する画像座標の取得工程と、
   前記情報処理装置が有する位置姿勢算出手段が、前記複数の指標についての前記物体座標と前記複数の時点のそれぞれにおける前記画像座標とに基づいて、当該複数の時点のそれぞれにおける前記撮像装置に対する前記現実物体の位置姿勢の推定値を算出する位置姿勢算出工程と、
   前記情報処理装置が有する配置情報算出手段が、前記撮像装置に対する前記撮像装置用姿勢センサの配置情報と、前記現実物体に対する前記現実物体用姿勢センサの配置情報と、前記複数の時点のそれぞれにおける前記現実物体の位置の推定値とともに、前記撮像装置用姿勢センサによる姿勢計測値に含まれる方位誤差と、前記現実物体用姿勢センサによる姿勢計測値に含まれる方位誤差との相対値である相対方位誤差の補正値を、前記物体座標と、当該複数の時点における、前記撮像装置用姿勢センサによる姿勢計測値と、前記現実物体用姿勢センサによる姿勢計測値と、前記画像座標と、前記撮像装置に対する前記現実物体の位置姿勢の推定値とを用いて求める配置情報算出工程と
   を備えることを特徴とする情報処理方法。
5. コンピュータを、請求項１乃至３の何れか１項に記載の情報処理装置の各手段として機能させるためのコンピュータプログラム。
6. 請求項５に記載のコンピュータプログラムを格納した、コンピュータ読み取り可能な記憶媒体。

Images