JP4914039B2 - 情報処理方法および装置 - Google Patents

Description

本発明は、物体に装着した６位置姿勢センサの物体に対する配置情報を求める技術に関する。

近年、現実空間と仮想空間の繋ぎ目のない融合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感システムで利用される画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）の画像を重畳描画してそれを表示するビデオシースルー方式によって主に実現される。

複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置に対する対象物体の位置姿勢と称する）を求める問題に帰結される。

この問題を解決する方法として、対象物体上に、対象物体の座標系における配置が既知であるような複数の指標を設置あるいは設定し、既知の情報であるその指標の物体座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、撮像装置に対する対象物体の位置姿勢を求めることが、写真測量等の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。

しかし、前述の画像情報のみを用いた位置合わせ手法は、照明条件の変化などに影響されやすく、安定性に問題がある。そのため、磁気センサのような常に安定した計測値が得られる６自由度位置姿勢センサを物体と撮像装置に装着することによって撮像装置に対する対象物体の位置姿勢を求めることが行われる。

さらに、磁気センサのような６自由度位置姿勢センサの計測値を、撮像装置が撮像した画像内における指標の投影像の座標を利用して高精度化するハイブリッドな位置姿勢計測方法も提案されている（例えば、非特許文献３を参照）。

６自由度位置姿勢センサを物体と撮像装置に装着し、物体と撮像装置との相対位置姿勢を求めて物体上に位置ずれなく仮想物体を描画するためには、計測対象物体に対する６自由度位置姿勢センサの配置情報、つまり相対位置姿勢が正確に求まっている必要がある。従来は、物体に対する６自由度位置姿勢センサの配置情報（以下ではこれを較正情報と呼ぶ）を求める作業（以下ではこれを較正と呼ぶ）は、手作業で行っていた。
Ｒ．Ｍ．Ｈａｒａｌｉｃｋ，Ｃ．Ｌｅｅ，Ｋ．Ｏｔｔｅｎｂｅｒｇ，ａｎｄ Ｍ．Ｎｏｌｌｅ：Ｒｅｖｉｅｗ ａｎｄ ａｎａｌｙｓｉｓ ｏｆ ｓｏｌｕｔｉｏｎｓ ｏｆ ｔｈｅ ｔｈｒｅｅ ｐｏｉｎｔ ｐｅｒｓｐｅｃｔｉｖｅ ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｐｒｏｂｌｅｍ，Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｖｏｌ．１３，ｎｏ．３，ｐｐ．３３１−３５６，１９９４． Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ ｐａｒａｍｅｔｅｒｉｚｅｄ ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ ｍｏｄｅｌｓ ｔｏ ｉｍａｇｅｓ，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１． 内山，山本，田村："複合現実感のためのハイブリッド位置合わせ手法−６自由度センサとビジョン手法の併用−"，日本バーチャルリアリティ学会論文誌，ｖｏｌ．８，ｎｏ．１，ｐｐ．１１９−１２５，２００３．

しかしながら、この較正を手作業で行うことは、作業の手間がかかるという観点で改善の余地があった。

本発明は以上を鑑みてなされたものであり、物体に装着した位置姿勢センサの当該物体に対する配置情報を、簡便かつ正確に求めることができるようにすることを目的とする。

上記目的を達成するために、本発明は以下の構成を有する。

本願請求項１記載の情報処理装置は、物体に配された指標を撮像装置で撮像した画像を入力する画像入力手段と、前記画像から前記指標の画像座標を検出する指標検出手段と、前記撮像装置の世界座標系における位置姿勢を入力する撮像位置姿勢入力手段と、前記物体に配された位置姿勢センサによって計測された世界座標系における位置姿勢を入力する物体位置姿勢入力手段と、前記検出された画像座標と、前記入力された撮像装置の世界座標系における位置姿勢と、前記入力された物体に配された位置姿勢センサによって計測された世界座標系における位置姿勢に基づいて、前記物体に配された位置姿勢センサの物体座標系における位置姿勢を算出する位置姿勢算出手段とを有することを特徴とする。

本発明によれば、物体に装着した位置姿勢センサの当該物体に対する配置情報を、簡便かつ正確に求めることができる。

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

［第１の実施形態］
本実施形態に係るセンサ較正装置では、物体に装着した６自由度位置姿勢センサの較正を、６自由度位置姿勢センサを装着した移動可能なカメラで対象物体に装着した指標を撮影することにより行う。本実施形態では、物体に対する６自由度位置姿勢センサの位置姿勢を未知パラメータとして取り扱う。以下、本実施形態に係る６自由度位置姿勢センサ較正装置及び６自由度位置姿勢センサ較正方法について説明する。

図１は、本実施形態に係るセンサ較正装置１１００の概略構成を示している。図１に示すように、本実施形態に係るセンサ較正装置１１００は、撮像部１０１０、指標検出部１０２０、位置姿勢算出部１０３０、データ管理部１０４０、指示部１０５０、較正情報算出部１０６０によって構成されており、指標が装着された物体１０００に接続されている。

また、物体１０００には６自由度位置姿勢センサＡ２が装着されており、世界座標系に対する６自由度位置姿勢センサＡ２の位置姿勢を計測することが可能になっている。６自由度位置姿勢センサＡ２は位置姿勢算出部１０３０に接続されている。また、撮像部１０１０には６自由度位置姿勢センサＡ１が装着されている。撮像部を構成するカメラに対する６自由度位置姿勢センサＡ１の位置姿勢は既知であり、カメラの世界座標系に対する位置姿勢を計測することが可能であるとする。６自由度位置姿勢センサＡ１は位置姿勢算出部１０３０に接続されている。

なお、物体１０００上には、図３（Ａ）に示すように指標が複数配置されている。ここで、物体に配置された指標をＰ^ｋ（ｋ＝１，，，Ｋ_ｏ）で表す。但し、Ｋ_ｏは物体上に配置された指標の数である。また、図３（Ｂ）で示すように指標Ｐ^ｋは点ｐ^ｋｉ（（ｋ＝１，，，Ｋ_ｏ）、（ｉ＝１，，，Ｎ_ｋ））により構成される。但し、Ｎ_ｋは、指標Ｐ^ｋを構成する点の総数を表す。

ここで物体座標系（物体上の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における各指標の位置姿勢は既知である。また、物体上に配置された夫々の指標を構成する各点は、指標ごとに規定される指標座標系において位置が既知であるとする。

指標Ｐ^ｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各点が識別可能であるような指標であれば、何れの形態であってもよい。例えば、図３に示すような四角形指標は内部に識別子を表すパターンを持ち、一意に同定可能である。このような指標を検出する際には、撮影画像に２値化処理を施した後にラベリング処理を行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として抽出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって候補領域が指標領域であるかを判定し、内部のパターンを読み出すことによって指標の方向と識別子を取得する。

撮像部１０１０は、物体上に配置された指標を様々な位置や方向から撮影する。撮影された画像は、指標検出部１０２０に入力される。

指標検出部１０２０は、撮像部１０１０より画像を入力し、入力した画像中に撮影されている指標Ｐ^ｋを構成する各点の画像座標を検出する。

さらに、指標検出部１０２０は、検出された各々の指標Ｐ^ｋｎを構成する点ｐ^ｋｎｉの画像座標ｕ^Ｐｋｎｉとその識別子ｋ_ｎをデータ管理部１０４０へと出力する。ここで、ｎ（ｎ＝１，，，Ｍ）は検出された指標夫々に対するインデックスであり、Ｍは検出された指標の総数を表している。例えば図１の場合は、識別子が１、２，３である四角形形状の指標が撮影される場合を示しており、Ｍ＝３であり、識別子ｋ_１＝１、ｋ_２＝２、ｋ_３＝３とこれらに対応する画像座標ｕ^Ｐｋ１ｉ、ｕ^Ｐｋ２ｉ、ｕ^Ｐｋ３ｉ、（ｉ＝１、２、３、４）が出力される。

次に位置姿勢算出部１０３０について説明する。ここで、ある三次元座標系Ｂに対する三次元の座標系Ａの位置を表す三次元ベクトルをｔ、姿勢を表す３×３回転行列をＲとすると、Ａ上での位置がｘ_Ａ（三次元ベクトル）で表される点のＢ上での座標ｘ_Ｂ（三次元ベクトル）は、式（１）に示す４×４行列Ｍ_ＢＡを使って同次座標表現によって式（２）のように表される。

本実施形態においては、Ｍ_ＢＡを座標系Ｂに対する座標系Ａの位置姿勢を表す手段として用いる。

位置姿勢算出部１０３０は、カメラに装着された６自由度位置姿勢センサＡ１の計測値より得られる世界座標系におけるカメラの位置姿勢Ｍ_ＷＣと、物体に装着された６自由度位置姿勢センサＡ２の計測値より得られる世界座標系における物体に装着された６自由度位置姿勢センサＡ２の位置姿勢Ｍ_ＷＡ２から、物体装着６自由度位置姿勢センサＡ２の座標系におけるカメラの位置姿勢Ｍ_Ａ２Ｃを式（３）のように算出する。

また本実施形態では、位置及び姿勢を、次に述べるように６次元ベクトルによって表現する。位置と姿勢は、夫々３値ベクトルｘ＝［ｘ ｙ ｚ］^Ｔ及びω＝［ξ ψ ζ］^Ｔによって表現される。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。これらの、位置ｘ及び姿勢ωをまとめて、６次元ベクトルａ＝［ｘ ｙ ｚ ξ ψ ζ］^Ｔとして表現する。

センサ出力値より求めた物体装着６自由度位置姿勢センサ座標系におけるカメラの位置姿勢を表す４×４行列Ｍ_Ａ２Ｃは、６次元ベクトルａ_Ａ２Ｃ＝［ｘ_Ａ２Ｃ ｙ_Ａ２Ｃ ｚ_Ａ２Ｃ ξ_Ａ２Ｃ ψ_Ａ２Ｃ ζ_Ａ２Ｃ］^Ｔで表される。Ｍ_Ａ２Ｃからａ_Ａ２Ｃへの変換は公知の方法を使って行うことができる。

この物体装着６自由度センサ座標系におけるカメラの位置姿勢を、データ管理部１０４０の要求に従って、データ管理部１０４０へ出力する。

指示部１０５０は、不図示のオペレータからデータ取得コマンドが入力されたとき時には「データ取得」の指示をデータ管理部１０４０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部１０６０に送信する。

データ管理部１０４０は、指示部１０５０から「データ取得」の指示を受けると、位置姿勢算出部１０３０から物体装着６自由度位置姿勢センサの座標系におけるカメラの位置と姿勢を入力し、指標検出部１０２０から指標の画像座標とその識別子を入力し、［ある時刻における物体装着６自由度位置姿勢センサ座標系におけるカメラの位置と姿勢−同時刻に検出された指標の画像座標−同時刻に検出された指標の識別子］の組を一つのデータリストに追加しこれを保持する。また、データ管理部１０４０は、較正情報算出部１０６０からの要求に従って、生成したデータリストを較正情報算出部１０６０に出力する。

較正情報算出部１０６０は、指示部１０５０から「較正情報算出」の指示を受け取ると、データ管理部１０４０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、物体座標系における６自由度位置姿勢センサの位置と姿勢）を出力する。

図２は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお同フローチャートに従ったプログラムコードは、本実施形態の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。

ステップＳ６０１０において、指示部１０５０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、センサ較正用のデータ取得を行う位置に物体１０００またはカメラ１０１０を配置した時に、データ取得コマンドを入力する。指示部１０５０は、データ取得コマンドが入力されている場合には、ステップＳ６０２０へと処理を移行させる。

ステップＳ６０２０において、データ管理部１０４０は、位置姿勢算出部１０３０から、センサ出力値より求めた物体装着６自由度位置姿勢センサ座標系におけるカメラの位置及び姿勢ａ_Ａ２Ｃ＝［ｘ_Ａ２Ｃ ｙ_Ａ２Ｃ ｚ_Ａ２Ｃ ξ_Ａ２Ｃ ψ_Ａ２Ｃ ζ_Ａ２Ｃ］^Ｔを入力する。

ステップＳ６０３０において、データ管理部１０４０は、指標検出部１０２０から、指標検出部１０２０によって検出された指標Ｐ^ｋｎを構成する点の画像座標群ｕ^Ｐｋｎｉとその識別子ｋ_ｎを入力する。指標検出部１０２０は、入力された画像に対して常に指標の検出処理を行っており、本ステップの処理により、同一時刻における物体装着６自由度位置姿勢センサ座標系におけるカメラの位置姿勢と、指標の画像座標及び識別子を得ることができる。なお、指標検出部１０２０から入力される情報は、必ずしも全ての指標に関するものである必要はなく、その時点で画像上において検出されている指標に関する情報であればよい。

次に、ステップＳ６０４０において、データ管理部１０４０は、検出された指標Ｐ^ｋｎ全てについて、入力したデータの組をデータリストＤＬにデータＤ_Ｌとして追加する。具体的には、位置姿勢算出部１０３０から入力するａ_Ａ２Ｃをａ_Ａ２Ｃｊ＝［ｘ_Ａ２Ｃｊ ｙ_Ａ２Ｃｊ ｚ_Ａ２Ｃｊ ξ_Ａ２Ｃｊ ψ_Ａ２Ｃｊ ζ_Ａ２Ｃｊ］^Ｔとし、指標検出部１０２０から入力する識別子ｋ_ｎをｋ_ｎｊとし、同じく指標検出部１０２０から入力するｕ^Ｐｋｎｉをｕ_Ｌ ^{Ｐｋｎｊｉ}として、Ｄ_Ｌ＝［ａ_Ａ２Ｃｊ，ｕ_Ｌ ^{Ｐｋｎｊｉ}，ｋ_ｎｊ］の組を、Ｌ番目のデータとしてデータリストＤＬに登録する。ここでｊ（ｊ＝１，，，Ｎ_Ｊ）は、撮影した画像に対するインデックスであり、Ｌ（Ｌ＝１，，，Ｎ_Ｌ）は、データリストＤＬに登録したデータの組夫々に対するインデックスであり、Ｎ_Ｊは、撮影した画像の総数を表し、Ｎ_Ｌは、登録したデータの総組数を表している。

以上の処理によって、データの取得が行われる。

ステップＳ６０５０では、データ管理部１０４０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ６０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも全ての指標に関するデータが、少なくとも二個はデータリストＤＬに含まれていること、が挙げられる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。

次にステップＳ６０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ６０７０へと処理を移行し、入力されていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。

較正情報算出部１０６０は、求めるべき較正情報、すなわち物体座標系における６自由度位置姿勢センサの位置姿勢を未知パラメータとして６値ベクトルとして扱う。以下では、物体座標系における物体装着６自由度位置姿勢センサの位置姿勢をｓ_ＯＡ２＝［ｘ_ＯＡ２ ｙ_ＯＡ２ ｚ_ＯＡ２ ξ_ＯＡ２ ψ_ＯＡ２ ζ_ＯＡ２］^Ｔと記述する。

ステップＳ６０７０において、較正情報算出部１０６０は、状態ベクトルｓ_ＯＡ２に初期値を与える。ｓ_ＯＡ２の初期値としては、オペレータが指示部１０５０を介しておおよその値を手入力しても良い。物体装着６自由度位置姿勢センサ座標系におけるカメラの位置姿勢を表す変換行列をＭ_Ａ２Ｃ、物体座標系における６自由度位置姿勢センサの位置姿勢を表す変換行列Ｍ_ＯＡ２、また、物体座標系における物体装着指標の位置姿勢を表す変換行列をＭ_ＯＭとし、これらより指標座標からカメラ座標系への変換行列Ｍ_ＣＭは式（４）のようになる。

ステップＳ６０８０において、較正情報算出部１０６０は、データリストＤＬ中の各データＤ_Ｌ＝［ａ_Ａ２Ｃｊ，ｕ_Ｌ ^{Ｐｋｎｊｉ}，ｋ_ｊ］、（（ｊ＝１，２，，，，Ｎ_Ｊ）（Ｌ＝１，２，，，，Ｎ_Ｌ）および状態ベクトルｓ_ＯＡ２、から、全てのＬに対して、指標Ｐ^ｋｎｊの画像座標の計算値ｕ_Ｌ ^{Ｐｋｎｊｉ}’＝［ｕ_ｘＬ ^{Ｐｋｎｊｉ}’，ｕ_ｙＬ ^{Ｐｋｎｊｉ}’］を求める。ここで指標の画像座標の計算値とは、物体座標系における位置及び姿勢がＭ_ＯＭである指標の指標座標系における位置ベクトルｘ_Ｍ ^Ｐｋｎｊが既知な点Ｐ^ｋｎｊの、画像中に見えるべき位置（座標）を指す。ｕ_Ｌ ^{Ｐｋｎｊｉ}’の算出は、物体座標系における６自由度位置姿勢センサ座標系の位置を表す状態ベクトルｓ_ＯＡ２の関数

に基づいて行われる。

具体的には、関数Ｆ_Ｌ（）は、Ｌ番目のデータを取得した時の（すなわち、同データセット中のカメラ１０１０の位置と姿勢がａ_Ａ２Ｃｊである時の）指標Ｐ^ｋｎｊのカメラ座標における位置ベクトルｘ_ＣＬ ^{Ｐｋｎｊｉ}を、ｓ_ＯＡ２から求める次式、

及び、ｘ_ＣＬ ^{Ｐｋｎｊｉ}から指標Ｐ^ｋｎｊの画像上の座標ｕ_Ｌ ^{Ｐｋｎｊｉ}’を求める次式、

によって構成されている。ここでｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向におけるカメラ１０１０の焦点距離であり、既知の値として予め保持されているものとする。また、Ｍ_ＯＡ２（ｓ_ＯＡ２）はｓ_ＯＡ２によって定まる変換行列であり、次式によって定義される。

ただし、

である。

ステップＳ６０９０において、較正情報算出部１０６０は、全てのＬに対して、データリストＤＬ中の各データに含まれる指標Ｐ^ｋｎｊの実際の画像座標ｕ_Ｌ ^{Ｐｋｎｊｉ}と、それに対応する画像座標の計算値ｕ_Ｌ ^{Ｐｋｎｊｉ}’との誤差△ｕ_Ｌ ^{Ｐｋｎｊｉ}を次式によって算出する。

ステップＳ６１００において、較正情報算出部１０６０は、全てのＬに対して、状態ベクトルｓ_ＯＡ２に関する画像ヤコビアン（すなわち、数５の関数Ｆ_ｊ（）を状態ベクトルｓ_ＯＡ２の各要素で偏微分した解を各要素に持つ（２×Ｎ_Ｌ）行×６列のヤコビ行列）Ｊ_ｕＬ ^{Ｐｋｎｊｉ} _ｓ（＝∂ｕ_Ｌ ^{Ｐｋｎｊｉ}／∂ｓ_ＯＡ２）を算出する。ここで、Ｎ_Ｌは検出したすべての指標の総数である。

ステップＳ６１１０において、較正情報算出部１０６０は、以上のステップで算出した、全てのＬに対する誤差Δｕ_Ｌ ^{Ｐｋｎｊｉ}及びヤコビ行列Ｊ_ｕＬ ^{Ｐｋｎｊｉ} _ｓに基づいて、ｓ_ＯＡ２の補正値Δｓ_ＯＡ２を算出する。具体的には、全てのＬに対する誤差Δｕ_Ｌ ^{Ｐｋｎｊｉ}を垂直に並べた２Ｎ_Ｌ次元ベクトルの誤差ベクトル

及びヤコビ行列Ｊ_ｕＬ ^{Ｐｋｎｊｉ} _ｓを垂直に並べた（２Ｎ_Ｌ）行×６列の行列

を作成し、Φの擬似逆行列Φ^＋を用いて、△ｓ_ＯＡ２を次式より算出する。

△ｓ_ＯＡ２＝Φ^＋Ｕ ・・・式（１３）
ここで、Δｓ_ＯＡ２は６次元ベクトルであるから、２Ｎ_Ｌが６以上であれば、Δｓ_ＯＡ２を求めることができる。なおΦ^＋は、例えばΦ^＋＝（Φ^ＴΦ）^−１Φ^Ｔによって求めることができるが、他の方法によって求めてもよい。

ステップＳ６１２０において、較正情報算出部１０６０は、ステップＳ６１１０において算出した補正値Δｓ_ＯＡ２を用いて、式（１４）に従ってｓ_ＯＡ２を補正し、得られた値を新たなｓ_ＯＡ２とする。ここで状態ベクトルｓ_ＯＡ２は、物体座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢の状態ベクトルである。

ｓ_ＯＡ２＋△ｓ_ＯＡ２→△ｓ_ＯＡ２ ・・・式（１４）
ステップＳ６１３０において、較正情報算出部１０６０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓ_ＯＡ２が予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓ_ＯＡ２を用いて、再度ステップＳ６０８０以降の処理を行う。

ステップＳ６１３０において計算が収束したと判定されると、ステップＳ６１４０において、較正情報算出部１０６０は、較正情報として、物体座標系における６自由度位置姿勢センサの位置及び姿勢ｓ_ＯＡ２を出力する。この時の出力の形態は、ｓ_ＯＡ２そのものであっても良いし、ｓ_ＯＡ２の位置成分を３値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓ_ＯＡ２から生成した座標変換行列Ｍ_ＯＡ２であっても良い。

最後にステップＳ６１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、センサ較正装置１１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。

以上の処理によって、対象物体に対する（すなわち、対象物体座標系における）対象物体に装着した６自由度位置姿勢センサの位置または位置と姿勢を取得することができる。

＜変形例１−１＞
なお、本実施形態では、較正情報として物体に装着した６自由度位置姿勢センサであったが、カメラに装着した６自由度位置姿勢センサのカメラに対する位置姿勢も同時に較正情報として求めてもよい。

本実施形態では、カメラに装着した６自由度位置姿勢センサのカメラ座標系における位置姿勢Ｍ_ＣＡ１は、既知であるとしていたがここでは未知パラメータとする。また、カメラに装着された６自由度位置姿勢センサＡ１のセンサ計測値より得られる世界座標系におけるカメラ装着６自由度位置姿勢センサＡ１の位置姿勢Ｍ_ＷＡ１と、物体に装着された６自由度位置姿勢センサＡ２のセンサ計測値より得られる世界座標系における物体装着６自由度位置姿勢センサＡ２の位置姿勢Ｍ_ＷＡ２より、物体装着６自由度位置姿勢センサＡ２の座標系におけるカメラ装着６自由度位置姿勢センサＡ１の位置姿勢Ｍ_Ａ２Ａ１を式（１５）のように算出する。

カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢Ｍ_ＣＡ１、物体装着６自由度位置姿勢センサＡ２の座標系におけるカメラ装着６自由度位置姿勢センサＡ１の位置姿勢を表す変換行列をＭ_Ａ２Ａ１、物体座標系における６自由度位置姿勢センサの位置姿勢を表す変換行列をＭ_ＯＡ２、また、物体座標系における物体装着指標の位置姿勢を表す変換行列をＭ_ＯＭとすると、これらより指標座標系からカメラ座標系への変換行列Ｍ_ＣＭは式（１６）のように表せる。

求めるべき較正情報、すなわち物体座標系における６自由度位置姿勢センサの位置姿勢Ｍ_ＯＡ２、カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢Ｍ_ＣＡ１、を本実施形態と同様に６値ベクトルとして扱う。ここでは、物体座標系における６自由度位置姿勢センサの位置姿勢を状態ベクトルｓ_ＯＡ２＝［ｘ_ＯＡ２ ｙ_ＯＡ２ ｚ_ＯＡ２ ξ_ＯＡ２ ψ_ＯＡ２ ζ_ＯＡ２］^Ｔ、カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢を状態ベクトルｓ_ＣＡ１＝［ｘ_ＣＡ１ ｙ_ＣＡ１ ｚ_ＣＡ１ ξ_ＣＡ１ ψ_ＣＡ１ ζ_ＣＡ１］^Ｔ、と記述する。

夫々の状態ベクトルｓ_ＯＡ２、ｓ_ＣＡ１に適当な初期値を与える。初期値は、例えば手計測を行うなどしてあらかじめ求めておく。較正情報である未知パラメータをｓ＝［ｓ_ＯＡ２ ^Ｔ ｓ_ＣＡ１ ^Ｔ］^Ｔとし、例えば本実施形態と同様に画像ヤコビアンを用いた繰り返し計算によって、全入力データに対するｓの最適値を求める。これにより物体座標系における６自由度位置姿勢センサの位置姿勢、カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢を得て、較正情報として出力することができる。

［第２の実施形態］
本実施形態に係るセンサ較正装置では、物体に装着した６自由度位置姿勢センサの配置情報（物体座標系におけるセンサの位置と姿勢）を算出する。本実施形態では、較正済みの指標の物理座標と画像座標の対応関係から得られる物体に対するカメラの位置姿勢と、何らかの方法で求めた世界座標系におけるカメラの位置姿勢とを利用して、物体に装着した６自由度位置姿勢センサの較正を行う。以下、本実施形態に係る６自由度位置姿勢センサ較正装置及び６自由度位置姿勢センサ較正方法について説明する。

図４は、本実施形態に係るセンサ較正装置２１００の概略構成を示している。図４に示すように、本実施形態に係るセンサ較正装置は、カメラ２０１０、画像入力部２０１５、指標検出部２０２０、計測値入力部２０３０、データ管理部２０４０、指示部２０５０、較正情報算出部２０６０、位置姿勢算出部２０７０によって較正されており、較正済みの指標が装着された物体２０００に接続されている。

また、物体２０００には６自由度位置姿勢センサＡ２が装着されており、世界座標系に対する６自由度位置姿勢センサＡ２の位置姿勢を計測することが可能となっている。６自由度位置姿勢センサＡ２は計測値入力部２０３０に接続されている。また、カメラ２０１０の世界座標系に対する位置姿勢（以下ではこれを、第一の実施形態と同様にＭ_ＷＣと記す）は何らかの方法で取得可能となっている。本実施形態では、カメラ２０１０は固定カメラであって、世界座標系におけるその位置姿勢は予め既知な値として入力されており、不図示のメモリに記憶されているものとする。カメラ２０１０の位置姿勢の入力方法はこれに限定されるものではなく、例えば、カメラ２０１０に６自由度位置姿勢センサを設置し、その計測値を取得することによって入力しても良い。

物体２０００上には、第一の実施形態における図３（Ａ）と同様に、物体座標系上における配置情報が既知である指標Ｐ^ｋが複数配置されている。指標Ｐ^ｋは、第一の実施形態と同様に、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各点が識別可能であるような指標であれば、何れの形態であってもよい。

カメラ２０１０は、物体上に配置された指標を撮影する。撮影された画像は、画像入力部２０１５を経て指標検出部２０２０に入力される。

指標検出部２０２０は、画像入力部２０１５より画像を入力し、第一の実施形態における指標検出部１０２０と同様に、入力した画像中に撮影されている指標Ｐ^ｋを構成する各点の画像座標を検出し、検出された各々の指標Ｐ^ｋｎを構成する点ｐ^ｋｎｉの画像座標ｕ^Ｐｋｎｉとその識別子ｋ_ｎを位置姿勢算出部２０７０へと出力する。

位置姿勢算出部２０７０は、指標検出部２０２０から検出された指標の画像座標とその識別子を入力し、既知の値である物体座標系における夫々の指標の配置情報に基づいて、物体座標系におけるカメラ２０１０の位置姿勢（以下ではこれをＭ_ＯＣと記す）を算出する。３次元位置が既知な複数の指標の画像座標からカメラの位置姿勢を求める方法は公知の技術であるので、その詳細な説明は省略する。算出されたカメラ２０１０の位置姿勢は、データ管理部２０４０へと出力される。

計測値入力部２０３０は、６自由度位置姿勢センサＡ２の計測値より得られる世界座標系における６自由度位置姿勢センサＡ２の位置姿勢（以下ではこれを、第一の実施形態と同様にＭ_ＷＡ２と記す）を入力し、データ管理部２０４０に出力する。

指示部２０５０は、不図示のオペレータからデータ取得コマンドが入力されたとき時には「データ取得」の指示をデータ管理部２０４０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部２０６０に送信する。

データ管理部２０４０は、指示部２０５０から「データ取得」の指示を受けると、６自由度位置姿勢センサＡ２の世界座標系における位置姿勢Ｍ_ＷＡ２を計測値入力部２０３０から入力し、物体座標系におけるカメラ２０１０の位置姿勢Ｍ_ＯＣを位置姿勢算出部２０７０から入力し、さらに、世界座標系におけるカメラ２０１０の位置姿勢Ｍ_ＷＣを不図示のメモリから入力し、［ある時刻における６自由度位置姿勢センサＡ２の世界座標系における位置姿勢Ｍ_ＷＡ２−同時刻に算出された物体座標系におけるカメラ２０１０の位置姿勢Ｍ_ＯＣ−同時刻の世界座標系におけるカメラ２０１０の位置姿勢Ｍ_ＷＣ］の組を一つのデータリストに追加しこれを保持する。また、データ管理部２０４０は、較正情報算出部２０６０からの要求に従って、生成したデータリストを較正情報算出部２０６０に出力する。

較正情報算出部２０６０は、指示部２０５０から「較正情報算出」の指示を受け取ると、データ管理部２０４０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報、すなわち、物体座標系における６自由度位置姿勢センサＡ２の位置姿勢（以下ではこれを、第一の実施形態と同様にＭ_ＯＡ２と記す）を出力する。

図５は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお同フローチャートに従ったプログラムコードは、本実施形態の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。

ステップＳ７０１０において、指示部２０５０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、センサ較正用のデータ取得を行う位置に物体２０００またはカメラ２０１０を配置した時に、データ取得コマンドを入力する。指示部２０５０は、データ取得コマンドが入力されている場合には、ステップＳ７０２０へと処理を移行させる。

ステップＳ７０２０において、データ管理部２０４０は、世界座標系におけるカメラ２０１０の位置姿勢の情報を不図示のメモリから入力する。

ステップＳ７０３０において、データ管理部２０４０は、位置姿勢算出部２０７０から、物体座標系におけるカメラ２０１０の位置姿勢を入力する。

ステップＳ７０３５において、データ管理部２０４０は、計測値入力部２０３０から、世界座標系における物体装着６自由度位置姿勢センサＡ２の位置姿勢を入力する。

次に、ステップＳ７０４０において、データ管理部２０４０は、入力したデータの組をデータリストＤＬにデータＤ_Ｌとして追加する。

以上の処理によって、データの取得が行われる。

ステップＳ７０５０では、データ管理部２０４０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ７０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ７０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも二組以上のデータがデータリストＤＬに含まれていることが挙げられる。本実施形態においては、データが一組あれば解の算出が可能であるので、必ずしもこのような判定工程は必要ない。また、より多くのデータを要求するように条件を設定してもよい。

次にステップＳ７０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ７０７０へと処理を移行し、入力されていない場合には、再びステップＳ７０１０へと戻り、データ取得コマンドの入力を待つ。

ステップＳ７０７０において、データリストＤＬに含まれる夫々のデータの組に対して、世界座標系におけるカメラ２０１０の位置姿勢Ｍ_ＷＣと、世界座標系における６自由度位置姿勢センサＡ２の位置姿勢Ｍ_ＷＡ２と、物体座標系におけるカメラ２０１０の位置姿勢Ｍ_ＯＣとから、較正対象である物体座標系における６自由度位置姿勢センサＡ２の位置姿勢Ｍ_ＯＡ２を式（１７）のように算出する。

さらに、データリストＤＬに複数のデータの組が含まれている場合には、式（１７）によって夫々のデータの組毎に推定されたＭ_ＯＡ２ｉ（ｉ＝１，．．，Ｎ；ＮはデータリストＤＬに含まれるデータの組数）の平均を求め、これを最終的なＭ_ＯＡ２とする。複数の位置姿勢Ｍ_ｉの平均は、例えば、位置成分に関しては、夫々のＭ_ｉの位置を表す３次元ベクトルｔ_ｉの平均値ｔを求め、姿勢成分に関しては、夫々のＭ_ｉの姿勢を表す回転行列Ｒ_ｉを姿勢の四元数表現である４次元ベクトルω_ｉに一旦変換し、ω_ｉの平均値ωを求め、得られたωを姿勢の回転行列表現Ｒに変換し、さらに、得られた位置ｔと姿勢Ｒから、式（１）等に基づいて求めることができる。なお、姿勢の回転行列表現と四元数表現の相互変換は三次元幾何学の基本的事項であるので、その詳細な説明は省略する。

また、Ｎ_Ｊ枚の画像を撮影すると、撮影画像ごとに存在するＭ_ＯＣｉ，Ｍ_Ａ２Ｃｉを用いて式（１８）のように表せる。

ここで、

とすると、式（１８）は式（２１）のように表せる。

Ａ＝Ｂ・Ｍ_ＯＡ２ ・・・式（２１）
撮影した画像が複数ある場合には、Ｍ_ＯＡ２を求めるためにＢの一般化逆行列を用いて式（２２）のように計算してもよい。
Ｍ_ＯＡ２＝（Ｂ^ｔ・Ｂ）^−１・Ｂ^ｔ・Ａ ・・・式（２２）
ステップＳ７１００は、以上の処理によって求められたＭ_ＯＡ２を、較正情報、すなわち、物体座標系における６自由度センサＡ２の位置姿勢として出力する。この時の出力の形態は、Ｍ_ＯＡ２そのものであっても良いし、Ｍ_ＯＡ２の位置成分を３値のベクトルｔで表し、姿勢成分をオイラー角や３×３の回転行列Ｒで表したものであっても良いし、他のいずれの表現方法でもよい。

最後にステップＳ７１１０では、較正処理を終了するか否かの判定が行われる。オペレータが、センサ較正装置２１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ７０１０へと戻り、データ取得コマンドの入力を待つ。

以上の処理によって、対象物体に装着した６自由度位置姿勢センサの対象物体に対する（すなわち、対象物体座標系における）位置及び姿勢を、簡便かつ正確に取得することができる。

［他の実施形態］
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても、達成されることは言うまでもない。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

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

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

第１の実施形態に係るセンサ較正装置の概略構成を示す図である。 第１の実施形態に係るセンサ較正方法の概略処理を示すフローチャートである。 第１の実施形態に係る指標の概略構成を示す図である。 第２の実施形態に係るセンサ較正装置の概略構成を示す図である。 第２の実施形態に係るセンサ較正方法の概略処理を示すフローチャートである。

Claims (15)

1. 物体に配された指標を撮像装置で撮像した画像を入力する画像入力手段と、
   前記画像から前記指標の画像座標を検出する指標検出手段と、
   前記撮像装置の世界座標系における位置姿勢を入力する撮像位置姿勢入力手段と、
   前記物体に配された位置姿勢センサによって計測された世界座標系における位置姿勢を入力する物体位置姿勢入力手段と、
   前記検出された画像座標と、前記入力された撮像装置の世界座標系における位置姿勢と、前記入力された物体に配された位置姿勢センサによって計測された世界座標系における位置姿勢に基づいて、前記物体に配された位置姿勢センサの当該物体の物体座標系における位置姿勢を算出する位置姿勢算出手段と
   を有することを特徴とする情報処理装置。
2. 前記位置姿勢算出手段が、
   前記入力された撮像装置の世界座標系における位置姿勢と、前記入力された位置姿勢センサにより計測された世界座標系の位置姿勢とに基づいて、前記指標の前記画像の面における画像座標を算出する画像座標算出工程と
   を更に有し、
   前記位置姿勢算出手段が、前記算出された画像座標と、前記検出された画像座標とを比較することにより、前記物体に配された位置姿勢センサの物体座標系における位置姿勢を算出することを特徴とする請求項１に記載の情報処理装置。
3. 前記位置姿勢算出手段が、
   前記物体に配された位置姿勢センサの物体座標系における位置姿勢の初期値を入力する初期値入力手段と、
   前記前記算出された画像座標と、前記検出された画像座標との誤差に基づいて、補正値を算出する補正値算出手段と、
   前記算出した補正値に基づいて、前記初期値を補正する補正手段と
   を更に有し、
   前記位置姿勢算出手段が、前記初期値を補正した値を、前記物体に配された位置姿勢センサの物体座標系における位置姿勢として算出することを特徴とする請求項２に記載の情報処理装置。
4. 前記位置姿勢算出手段が、
   前記入力された撮像装置の世界座標系における位置姿勢と、前記入力された位置姿勢センサにより計測された世界座標系の位置姿勢とに基づいて、前記撮像装置と前記物体に配された位置姿勢センサとの相対位置姿勢を求める相対位置姿勢算出手段とを
   更に有し、
   前記画像座標算出手段が、前記相対位置姿勢に基づいて、前記指標の前記画像の面における画像座標を算出することを特徴とする請求項２又は３に記載の情報処理装置。
5. 前記撮像位置姿勢入力手段が、現実空間に固定された位置が既知である撮像装置の世界座標系における位置姿勢を入力することを特徴とする請求項１乃至４の何れか１項に記載の情報処理装置。
6. 前記撮像位置姿勢入力手段が、前記撮像装置に配された位置姿勢センサによって計測された世界座標系における位置姿勢を入力することを特徴とする請求項１乃至４の何れか１項に記載の情報処理装置。
7. 前記位置姿勢算出手段が、前記入力された撮像装置の世界座標系における位置姿勢と、前記入力された位置姿勢であって前記物体に配された位置姿勢センサにより計測された世界座標系の位置姿勢とに基づいて、前記物体に配された位置姿勢センサの物体座標系における位置姿勢と前記撮像装置に配された位置姿勢センサの当該撮像装置の撮像座標系における位置姿勢とを同時に算出することを特徴とする請求項６に記載の情報処理装置。
8. 前記位置姿勢算出手段が、
   前記入力された撮像装置の世界座標系における位置姿勢と、前記入力された位置姿勢センサにより計測された世界座標系の位置姿勢とに基づいて、前記指標の前記画像の面における画像座標を算出する画像座標算出手段と
   を更に有し、
   前記位置姿勢算出手段が、前記算出された画像座標と、前記検出された画像座標とを比較することにより、前記物体に配された位置姿勢センサの物体座標系における位置姿勢と前記撮像装置に配された位置姿勢センサの当該撮像装置の撮像座標系における位置姿勢とを同時に算出することを特徴とする請求項７に記載の情報処理装置。
9. 前記位置姿勢算出手段が、
   前記物体に配された位置姿勢センサの物体座標系における位置姿勢と前記撮像装置に配された位置姿勢センサの当該撮像装置の撮像座標系における位置姿勢の初期値を入力する初期値入力手段と、
   前記前記算出された画像座標と、前記検出された画像座標との誤差に基づいて、補正値を算出する補正値算出手段と、
   前記算出した補正値に基づいて、前記初期値を補正する補正手段と
   を更に有し、
   前記位置姿勢算出手段が、前記初期値を補正した値を、前記物体に配された位置姿勢センサの物体座標系における位置姿勢と前記撮像装置に配された位置姿勢センサの当該撮像装置の撮像座標系における位置姿勢として算出することを特徴とする請求項８に記載の情報処理装置。
10. 前記補正値算出手段が、画像ヤコビアンを用いて、補正値を算出することを特徴とする請求項３又は９に記載の情報処理装置。
11. 前記位置姿勢算出手段が、
    前記検出した画像座標の個数が、予め設定された閾値以上であるか否かを判定する検出判定手段と
    を更に有し、
    前記位置姿勢算出手段が、前記判定手段が予め設定された閾値以上であると判定した場合に、算出を実行することを特徴とする請求項１乃至１０の何れか１項に記載の情報処理装置。
12. 前記判定手段が、前記検出した画像座標の個数が、２つ以上であるか否かを判定し、
    前記位置姿勢算出手段が、前記判定手段が２つ以上であると判定した場合に、算出を実行することを特徴とする請求項１１に記載の情報処理装置。
13. 請求項１乃至１２の何れか１項に記載の情報処理装置の各手段をコンピュータに機能させるためのプログラム。
14. 請求項１３に記載のプログラムを記録する記録媒体。
15. 情報処理装置が行う情報処理方法であって、
    前記情報処理装置の画像入力手段が、物体に配された指標を撮像装置で撮像した画像を入力する画像入力工程と、
    前記情報処理装置の指標検出手段が、前記画像から前記指標の画像座標を検出する指標検出工程と、
    前記情報処理装置の撮像位置姿勢入力手段が、前記撮像装置の世界座標系における位置姿勢を入力する撮像位置姿勢入力工程と、
    前記情報処理装置のセンサ位置姿勢入力手段が、前記物体に配された位置姿勢センサによって計測された世界座標系における位置姿勢を入力するセンサ位置姿勢入力工程と、
    前記情報処理装置の位置姿勢算出手段が、前記検出された画像座標と、前記入力された撮像装置の世界座標系における位置姿勢と、前記入力された物体に配された位置姿勢センサによって計測された世界座標系における位置姿勢に基づいて、前記物体に配された位置姿勢センサの当該物体の物体座標系における位置姿勢を算出する位置姿勢算出工程と
    を有することを特徴とする情報処理方法。

Images