(57)【要約】【課題】  物体に装着した６自由度位置姿勢センサの当該物体に対する配置情報を、簡便かつ正確に求めることができるようにする。【解決手段】  物体に装着した第一の位置姿勢センサの該物体に対する配置情報を較正情報として求める情報処理方法であって、撮像装置によって前記物体を撮影した画像を入力する画像入力工程と、前記第一の位置姿勢センサの計測値及び前記撮像装置に装着した第二の位置姿勢センサの計測値を入力するセンサ計測値入力工程と、前記画像から前記物体に装着した指標の画像座標に関する情報を検出する指標検出工程と、前記センサ計測値入力工程で入力された前記第一および第二の位置姿勢センサの計測値と、前記指標検出工程で検出された前記指標の画像座標に関する情報とを用いて、前記較正情報を算出する較正情報算出工程とを有する。【選択図】  図１
2007033257.tif 000002
【特許請求の範囲】
【請求項１】
物体に装着した第一の位置姿勢センサの該物体に対する配置情報を較正情報として求める情報処理方法であって、
撮像装置によって前記物体を撮影した画像を入力する画像入力工程と、
前記第一の位置姿勢センサの計測値及び前記撮像装置に装着した第二の位置姿勢センサの計測値を入力するセンサ計測値入力工程と、
前記画像から前記物体に装着した指標の画像座標に関する情報を検出する指標検出工程と、
前記センサ計測値入力工程で入力された前記第一および第二の位置姿勢センサの計測値と、前記指標検出工程で検出された前記指標の画像座標に関する情報とを用いて、前記較正情報を算出する較正情報算出工程を有することを特徴とする情報処理方法。
【請求項２】
前記較正情報算出工程は、
前記第一および第二の位置姿勢センサの計測値と較正情報の推定値とに基づいて、前記指標検出工程で検出された前記指標の前記画像内における画像座標に関する情報を推定する推定工程と、
前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記推定工程で推定された当該指標の画像座標に関する情報との間の誤差を軽減させるように、前記較正情報の推定値を補正する補正工程とを有することを特徴とする請求項１に記載の情報処理方法。
【請求項３】
前記較正情報算出工程は、
前記センサ計測値入力工程で入力された前記第一および第二の位置姿勢センサの計測値に基づいて、前記第二の位置姿勢センサと前記第一の位置姿勢センサとの間の計測時における相対的な位置姿勢を求める相対位置姿勢算出工程を有し、
前記相対位置姿勢算出工程で算出されたセンサ間の相対的な位置姿勢と、前記指標検出工程で検出された前記指標の画像座標に関する情報を用いて、前記較正情報を算出することを特徴とする請求項１または請求項２に記載の情報処理方法。
【請求項４】
前記第一の位置姿勢センサの前記物体に対する配置情報に加えて、前記第二の位置姿勢センサの前記撮像装置に対する配置情報を較正情報として求めることを特徴とする請求項１乃至請求項３に記載のいずれかに記載の情報処理方法。
【請求項５】
物体に装着した第一の６自由度位置姿勢センサの当該物体に対する配置情報を較正情報として求める情報処理装置であって、
前記撮像装置によって前記物体を撮影した画像を入力する画像入力手段と、
前記第一の６自由度位置姿勢センサの計測値及び、前記撮像装置に装着した第二の６自由度位置姿勢センサの計測値を入力するセンサ計測値入力手段と、
前記画像から前記物体に装着した指標の画像座標に関する情報を検出する指標検出手段と、
前記センサ計測値入力手段が入力した前記夫々のセンサ計測値と、前記指標検出手段が検出した前記指標の画像座標に関する情報とを少なくとも用いて、前記較正情報を算出する較正情報算出手段を有することを特徴とする情報処理装置。
【請求項６】
物体に装着した位置姿勢センサの当該物体に対する配置情報を較正情報として求める情報処理方法であって、
撮像装置によって前記物体を撮影した画像を入力する画像入力工程と、
前記位置姿勢センサの計測値を入力するセンサ計測値入力工程と、
前記画像から前記物体に装着した指標の画像座標に関する情報を検出する指標検出工程と、
前記撮像装置の基準座標系における位置と姿勢を入力する位置姿勢入力工程と、
前記センサ計測値入力工程で入力された前記計測値と、前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記位置姿勢入力工程で入力された基準座標系における前記撮像装置の位置姿勢とを用いて、前記較正情報を算出する較正情報算出工程を有することを特徴とする情報処理方法。
【請求項７】
前記較正情報算出工程は、
前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記物体に対する前記指標の較正情報とに基づいて、前記物体と前記撮像装置との間の相対的な位置姿勢を算出する位置姿勢算出工程を有し、
前記センサ計測値入力工程で入力された前記センサ計測値と、前記位置姿勢算出工程で算出された前記物体と前期撮像装置との間の相対的な位置姿勢と、前記位置姿勢入力工程で入力された基準座標系における前記撮像装置の位置姿勢とに基づいて、前記較正情報を算出することを特徴とする請求項６に記載の情報処理方法。
【請求項８】
物体に装着した６自由度位置姿勢センサの当該物体に対する配置情報を較正情報として求める情報処理装置であって、
撮像装置によって前記物体を撮影した画像を入力する画像入力手段と、
前記６自由度位置姿勢センサの計測値を入力するセンサ計測値入力手段と、
前記画像から前記物体に装着した指標の画像座標に関する情報を検出する指標検出手段と、
前記撮像装置の基準座標系における位置と姿勢を入力する位置姿勢入力手段と、
前記センサ計測値入力手段が入力した前記センサ計測値と、前記指標検出手段が検出した前記指標の画像座標に関する情報と、前記位置姿勢入力手段が入力した前記撮像装置の位置姿勢とを用いて、前記較正情報を算出する較正情報算出手段を有することを特徴とする情報処理装置。
【請求項９】
前記請求項１乃至請求項４及び前記請求項６乃至請求項７のいずれかに記載の情報処理方法をコンピュータにおいて実現するためのプログラム。
【請求項１０】
前記請求項９に記載のプログラムを記録する記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体に装着した６位置姿勢センサの物体に対する配置情報を求める技術に関する。
【背景技術】
【０００２】
近年、現実空間と仮想空間の繋ぎ目のない融合を目的とした、複合現実感に関する研究が盛んに行われている。複合現実感システムで利用される画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、撮像装置の位置及び姿勢に応じて生成した仮想空間（たとえばコンピュータ・グラフィックスにより描画された仮想物体や文字情報など）の画像を重畳描画してそれを表示するビデオシースルー方式によって主に実現される。
【０００３】
複合現実感の実現のためには、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということが重要であり、従来から多くの取り組みが行われてきた。複合現実感における位置合わせの問題は、仮想の情報を重畳しようとする対象物体と撮像装置との間の、相対的な位置姿勢（以下ではこれを、撮像装置に対する対象物体の位置姿勢と称する）を求める問題に帰結される。
【０００４】
この問題を解決する方法として、対象物体上に、対象物体の座標系における配置が既知であるような複数の指標を設置あるいは設定し、既知の情報であるその指標の物体座標系における三次元座標と、撮像装置が撮像した画像内における指標の投影像の座標とを利用して、撮像装置に対する対象物体の位置姿勢を求めることが、写真測量等の分野において古くから提案されている（例えば、非特許文献１、非特許文献２を参照）。
【０００５】
しかし、前述の画像情報のみを用いた位置合わせ手法は、照明条件の変化などに影響されやすく、安定性に問題がある。そのため、磁気センサのような常に安定した計測値が得られる６自由度位置姿勢センサを物体と撮像装置に装着することによって撮像装置に対する対象物体の位置姿勢を求めることが行われる。
【０００６】
さらに、磁気センサのような６自由度位置姿勢センサの計測値を、撮像装置が撮像した画像内における指標の投影像の座標を利用して高精度化するハイブリッドな位置姿勢計測方法も提案されている（例えば、非特許文献３を参照）。
【０００７】
６自由度位置姿勢センサを物体と撮像装置に装着し、物体と撮像装置との相対位置姿勢を求めて物体上に位置ずれなく仮想物体を描画するためには、計測対象物体に対する６自由度位置姿勢センサの配置情報、つまり相対位置姿勢が正確に求まっている必要がある。従来は、物体に対する６自由度位置姿勢センサの配置情報（以下ではこれを較正情報と呼ぶ）を求める作業（以下ではこれを較正と呼ぶ）は、手作業で行っていた。
【非特許文献１】Ｒ．Ｍ．Ｈａｒａｌｉｃｋ，Ｃ．Ｌｅｅ，Ｋ．Ｏｔｔｅｎｂｅｒｇ，ａｎｄ  Ｍ．Ｎｏｌｌｅ：Ｒｅｖｉｅｗ  ａｎｄ  ａｎａｌｙｓｉｓ  ｏｆ  ｓｏｌｕｔｉｏｎｓ  ｏｆ  ｔｈｅ  ｔｈｒｅｅ  ｐｏｉｎｔ  ｐｅｒｓｐｅｃｔｉｖｅ  ｐｏｓｅ  ｅｓｔｉｍａｔｉｏｎ  ｐｒｏｂｌｅｍ，Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｊｏｕｒｎａｌ  ｏｆ  Ｃｏｍｐｕｔｅｒ  Ｖｉｓｉｏｎ，ｖｏｌ．１３，ｎｏ．３，ｐｐ．３３１−３５６，１９９４．
【非特許文献２】Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ  ｐａｒａｍｅｔｅｒｉｚｅｄ  ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ  ｍｏｄｅｌｓ  ｔｏ  ｉｍａｇｅｓ，ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１．
【非特許文献３】内山，山本，田村：“複合現実感のためのハイブリッド位置合わせ手法−６自由度センサとビジョン手法の併用−”，日本バーチャルリアリティ学会論文誌，ｖｏｌ．８，ｎｏ．１，ｐｐ．１１９−１２５，２００３．
【発明の開示】
【発明が解決しようとする課題】
【０００８】
しかしながら、この較正を手作業で行うことは、作業の手間がかかるという観点で改善の余地があった。
【０００９】
本発明は以上を鑑みてなされたものであり、物体に装着した位置姿勢センサの当該物体に対する配置情報を、簡便かつ正確に求めることができるようにすることを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、本発明は以下の構成を有する。
【００１１】
本願請求項１記載の情報処理方法は、物体に装着した第一の位置姿勢センサの該物体に対する配置情報を較正情報として求める情報処理方法であって、撮像装置によって前記物体を撮影した画像を入力する画像入力工程と、前記第一の位置姿勢センサの計測値及び前記撮像装置に装着した第二の位置姿勢センサの計測値を入力するセンサ計測値入力工程と、前記画像から前記物体に装着した指標の画像座標に関する情報を検出する指標検出工程と、前記センサ計測値入力工程で入力された前記第一および第二の位置姿勢センサの計測値と、前記指標検出工程で検出された前記指標の画像座標に関する情報とを用いて、前記較正情報を算出する較正情報算出工程を有することを特徴とする。
【００１２】
本願請求項６記載の情報処理方法は、物体に装着した位置姿勢センサの当該物体に対する配置情報を較正情報として求める情報処理方法であって、撮像装置によって前記物体を撮影した画像を入力する画像入力工程と、前記位置姿勢センサの計測値を入力するセンサ計測値入力工程と、前記画像から前記物体に装着した指標の画像座標に関する情報を検出する指標検出工程と、前記撮像装置の基準座標系における位置と姿勢を入力する位置姿勢入力工程と、前記センサ計測値入力工程で入力された前記計測値と、前記指標検出工程で検出された前記指標の画像座標に関する情報と、前記位置姿勢入力工程で入力された基準座標系における前記撮像装置の位置姿勢とを用いて、前記較正情報を算出する較正情報算出工程を有することを特徴とする。
【発明の効果】
【００１３】
本発明によれば、物体に装着した位置姿勢センサの当該物体に対する配置情報を、簡便かつ正確に求めることができる。
【発明を実施するための最良の形態】
【００１４】
以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
【００１５】
［第１の実施形態］
本実施形態に係るセンサ較正装置では、物体に装着した６自由度位置姿勢センサの較正を、６自由度位置姿勢センサを装着した移動可能なカメラで対象物体に装着した指標を撮影することにより行う。本実施形態では、物体に対する６自由度位置姿勢センサの位置姿勢を未知パラメータとして取り扱う。以下、本実施形態に係る６自由度位置姿勢センサ較正装置及び６自由度位置姿勢センサ較正方法について説明する。
【００１６】
図１は、本実施形態に係るセンサ較正装置１１００の概略構成を示している。図１に示すように、本実施形態に係るセンサ較正装置１１００は、撮像部１０１０、指標検出部１０２０、位置姿勢算出部１０３０、データ管理部１０４０、指示部１０５０、較正情報算出部１０６０によって構成されており、指標が装着された物体１０００に接続されている。
【００１７】
また、物体１０００には６自由度位置姿勢センサＡ２が装着されており、世界座標系に対する６自由度位置姿勢センサＡ２の位置姿勢を計測することが可能になっている。６自由度位置姿勢センサＡ２は位置姿勢算出部１０３０に接続されている。また、撮像部１０１０には６自由度位置姿勢センサＡ１が装着されている。撮像部を構成するカメラに対する６自由度位置姿勢センサＡ１の位置姿勢は既知であり、カメラの世界座標系に対する位置姿勢を計測することが可能であるとする。６自由度位置姿勢センサＡ１は位置姿勢算出部１０３０に接続されている。
【００１８】
なお、物体１０００上には、図３（Ａ）に示すように指標が複数配置されている。ここで、物体に配置された指標をＰｋ（ｋ＝１，，，Ｋｏ）で表す。但し、Ｋｏは物体上に配置された指標の数である。また、図３（Ｂ）で示すように指標Ｐｋは点ｐｋｉ（（ｋ＝１，，，Ｋｏ）、（ｉ＝１，，，Ｎｋ））により構成される。但し、Ｎｋは、指標Ｐｋを構成する点の総数を表す。
【００１９】
ここで物体座標系（物体上の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における各指標の位置姿勢は既知である。また、物体上に配置された夫々の指標を構成する各点は、指標ごとに規定される指標座標系において位置が既知であるとする。
【００２０】
指標Ｐｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各点が識別可能であるような指標であれば、何れの形態であってもよい。例えば、図３に示すような四角形指標は内部に識別子を表すパターンを持ち、一意に同定可能である。このような指標を検出する際には、撮影画像に２値化処理を施した後にラベリング処理を行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として抽出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって候補領域が指標領域であるかを判定し、内部のパターンを読み出すことによって指標の方向と識別子を取得する。
【００２１】
撮像部１０１０は、物体上に配置された指標を様々な位置や方向から撮影する。撮影された画像は、指標検出部１０２０に入力される。
【００２２】
指標検出部１０２０は、撮像部１０１０より画像を入力し、入力した画像中に撮影されている指標Ｐｋを構成する各点の画像座標を検出する。
【００２３】
さらに、指標検出部１０２０は、検出された各々の指標Ｐｋｎを構成する点ｐｋｎｉの画像座標ｕＰｋｎｉとその識別子ｋｎをデータ管理部１０４０へと出力する。ここで、ｎ（ｎ＝１，，，Ｍ）は検出された指標夫々に対するインデックスであり、Ｍは検出された指標の総数を表している。例えば図１の場合は、識別子が１、２，３である四角形形状の指標が撮影される場合を示しており、Ｍ＝３であり、識別子ｋ１＝１、ｋ２＝２、ｋ３＝３とこれらに対応する画像座標ｕＰｋ１ｉ、ｕＰｋ２ｉ、ｕＰｋ３ｉ、（ｉ＝１、２、３、４）が出力される。
【００２４】
次に位置姿勢算出部１０３０について説明する。ここで、ある三次元座標系Ｂに対する三次元の座標系Ａの位置を表す三次元ベクトルをｔ、姿勢を表す３×３回転行列をＲとすると、Ａ上での位置がｘＡ（三次元ベクトル）で表される点のＢ上での座標ｘＢ（三次元ベクトル）は、式（１）に示す４×４行列ＭＢＡを使って同次座標表現によって式（２）のように表される。
【００２５】
【数１】
2007033257.tif 000003
【００２６】
【数２】
2007033257.tif 000004
【００２７】
本実施形態においては、ＭＢＡを座標系Ｂに対する座標系Ａの位置姿勢を表す手段として用いる。
【００２８】
位置姿勢算出部１０３０は、カメラに装着された６自由度位置姿勢センサＡ１の計測値より得られる世界座標系におけるカメラの位置姿勢ＭＷＣと、物体に装着された６自由度位置姿勢センサＡ２の計測値より得られる世界座標系における物体に装着された６自由度位置姿勢センサＡ２の位置姿勢ＭＷＡ２から、物体装着６自由度位置姿勢センサＡ２の座標系におけるカメラの位置姿勢ＭＡ２Ｃを式（３）のように算出する。
【００２９】
【数３】
2007033257.tif 000005
【００３０】
また本実施形態では、位置及び姿勢を、次に述べるように６次元ベクトルによって表現する。位置と姿勢は、夫々３値ベクトルｘ＝［ｘ  ｙ  ｚ］Ｔ及びω＝［ξ  ψ  ζ］Ｔによって表現される。姿勢を３値によって表現する方法には様々なものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸方向を定義するような３値のベクトルによって表現されているものとする。これらの、位置ｘ及び姿勢ωをまとめて、６次元ベクトルａ＝［ｘ  ｙ  ｚ  ξ  ψ  ζ］Ｔとして表現する。
【００３１】
センサ出力値より求めた物体装着６自由度位置姿勢センサ座標系におけるカメラの位置姿勢を表す４×４行列ＭＡ２Ｃは、６次元ベクトルａＡ２Ｃ＝［ｘＡ２Ｃ  ｙＡ２Ｃ  ｚＡ２Ｃ  ξＡ２Ｃ  ψＡ２Ｃ  ζＡ２Ｃ］Ｔで表される。ＭＡ２ＣからａＡ２Ｃへの変換は公知の方法を使って行うことができる。
【００３２】
この物体装着６自由度センサ座標系におけるカメラの位置姿勢を、データ管理部１０４０の要求に従って、データ管理部１０４０へ出力する。
【００３３】
指示部１０５０は、不図示のオペレータからデータ取得コマンドが入力されたとき時には「データ取得」の指示をデータ管理部１０４０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部１０６０に送信する。
【００３４】
データ管理部１０４０は、指示部１０５０から「データ取得」の指示を受けると、位置姿勢算出部１０３０から物体装着６自由度位置姿勢センサの座標系におけるカメラの位置と姿勢を入力し、指標検出部１０２０から指標の画像座標とその識別子を入力し、［ある時刻における物体装着６自由度位置姿勢センサ座標系におけるカメラの位置と姿勢−同時刻に検出された指標の画像座標−同時刻に検出された指標の識別子］の組を一つのデータリストに追加しこれを保持する。また、データ管理部１０４０は、較正情報算出部１０６０からの要求に従って、生成したデータリストを較正情報算出部１０６０に出力する。
【００３５】
較正情報算出部１０６０は、指示部１０５０から「較正情報算出」の指示を受け取ると、データ管理部１０４０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、物体座標系における６自由度位置姿勢センサの位置と姿勢）を出力する。
【００３６】
図２は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお同フローチャートに従ったプログラムコードは、本実施形態の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。
【００３７】
ステップＳ６０１０において、指示部１０５０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、センサ較正用のデータ取得を行う位置に物体１０００またはカメラ１０１０を配置した時に、データ取得コマンドを入力する。指示部１０５０は、データ取得コマンドが入力されている場合には、ステップＳ６０２０へと処理を移行させる。
【００３８】
ステップＳ６０２０において、データ管理部１０４０は、位置姿勢算出部１０３０から、センサ出力値より求めた物体装着６自由度位置姿勢センサ座標系におけるカメラの位置及び姿勢ａＡ２Ｃ＝［ｘＡ２Ｃ  ｙＡ２Ｃ  ｚＡ２Ｃ  ξＡ２Ｃ  ψＡ２Ｃ  ζＡ２Ｃ］Ｔを入力する。
【００３９】
ステップＳ６０３０において、データ管理部１０４０は、指標検出部１０２０から、指標検出部１０２０によって検出された指標Ｐｋｎを構成する点の画像座標群ｕＰｋｎｉとその識別子ｋｎを入力する。指標検出部１０２０は、入力された画像に対して常に指標の検出処理を行っており、本ステップの処理により、同一時刻における物体装着６自由度位置姿勢センサ座標系におけるカメラの位置姿勢と、指標の画像座標及び識別子を得ることができる。なお、指標検出部１０２０から入力される情報は、必ずしも全ての指標に関するものである必要はなく、その時点で画像上において検出されている指標に関する情報であればよい。
【００４０】
次に、ステップＳ６０４０において、データ管理部１０４０は、検出された指標Ｐｋｎ全てについて、入力したデータの組をデータリストＤＬにデータＤＬとして追加する。具体的には、位置姿勢算出部１０３０から入力するａＡ２ＣをａＡ２Ｃｊ＝［ｘＡ２Ｃｊ  ｙＡ２Ｃｊ  ｚＡ２Ｃｊ  ξＡ２Ｃｊ  ψＡ２Ｃｊ  ζＡ２Ｃｊ］Ｔとし、指標検出部１０２０から入力する識別子ｋｎをｋｎｊとし、同じく指標検出部１０２０から入力するｕＰｋｎｉをｕＬＰｋｎｊｉとして、ＤＬ＝［ａＡ２Ｃｊ，ｕＬＰｋｎｊｉ，ｋｎｊ］の組を、Ｌ番目のデータとしてデータリストＤＬに登録する。ここでｊ（ｊ＝１，，，ＮＪ）は、撮影した画像に対するインデックスであり、Ｌ（Ｌ＝１，，，ＮＬ）は、データリストＤＬに登録したデータの組夫々に対するインデックスであり、ＮＪは、撮影した画像の総数を表し、ＮＬは、登録したデータの総組数を表している。
【００４１】
以上の処理によって、データの取得が行われる。
【００４２】
ステップＳ６０５０では、データ管理部１０４０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ６０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも全ての指標に関するデータが、少なくとも二個はデータリストＤＬに含まれていること、が挙げられる。ただし、入力データの多様性が増すほどに導出される較正情報の精度は向上するので、より多くのデータを要求するように条件を設定してもよい。
【００４３】
次にステップＳ６０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ６０７０へと処理を移行し、入力されていない場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。
【００４４】
較正情報算出部１０６０は、求めるべき較正情報、すなわち物体座標系における６自由度位置姿勢センサの位置姿勢を未知パラメータとして６値ベクトルとして扱う。以下では、物体座標系における物体装着６自由度位置姿勢センサの位置姿勢をｓＯＡ２＝［ｘＯＡ２  ｙＯＡ２  ｚＯＡ２  ξＯＡ２  ψＯＡ２  ζＯＡ２］Ｔと記述する。
【００４５】
ステップＳ６０７０において、較正情報算出部１０６０は、状態ベクトルｓＯＡ２に初期値を与える。ｓＯＡ２の初期値としては、オペレータが指示部１０５０を介しておおよその値を手入力しても良い。物体装着６自由度位置姿勢センサ座標系におけるカメラの位置姿勢を表す変換行列をＭＡ２Ｃ、物体座標系における６自由度位置姿勢センサの位置姿勢を表す変換行列ＭＯＡ２、また、物体座標系における物体装着指標の位置姿勢を表す変換行列をＭＯＭとし、これらより指標座標からカメラ座標系への変換行列ＭＣＭは式（４）のようになる。
【００４６】
【数４】
2007033257.tif 000006
【００４７】
ステップＳ６０８０において、較正情報算出部１０６０は、データリストＤＬ中の各データＤＬ＝［ａＡ２Ｃｊ，ｕＬＰｋｎｊｉ，ｋｊ］、（（ｊ＝１，２，，，，ＮＪ）（Ｌ＝１，２，，，，ＮＬ）および状態ベクトルｓＯＡ２、から、全てのＬに対して、指標Ｐｋｎｊの画像座標の計算値ｕＬＰｋｎｊｉ’＝［ｕｘＬＰｋｎｊｉ’，ｕｙＬＰｋｎｊｉ’］を求める。ここで指標の画像座標の計算値とは、物体座標系における位置及び姿勢がＭＯＭである指標の指標座標系における位置ベクトルｘＭＰｋｎｊが既知な点Ｐｋｎｊの、画像中に見えるべき位置（座標）を指す。ｕＬＰｋｎｊｉ’の算出は、物体座標系における６自由度位置姿勢センサ座標系の位置を表す状態ベクトルｓＯＡ２の関数
【００４８】
【数５】
2007033257.tif 000007
【００４９】
に基づいて行われる。
【００５０】
具体的には、関数ＦＬ（）は、Ｌ番目のデータを取得した時の（すなわち、同データセット中のカメラ１０１０の位置と姿勢がａＡ２Ｃｊである時の）指標Ｐｋｎｊのカメラ座標における位置ベクトルｘＣＬＰｋｎｊｉを、ｓＯＡ２から求める次式、
【００５１】
【数６】
2007033257.tif 000008
【００５２】
及び、ｘＣＬＰｋｎｊｉから指標Ｐｋｎｊの画像上の座標ｕＬＰｋｎｊｉ’を求める次式、
【００５３】
【数７】
2007033257.tif 000009
【００５４】
によって構成されている。ここでｆＢｘ及びｆＢｙは、それぞれｘ軸方向及びｙ軸方向におけるカメラ１０１０の焦点距離であり、既知の値として予め保持されているものとする。また、ＭＯＡ２（ｓＯＡ２）はｓＯＡ２によって定まる変換行列であり、次式によって定義される。
【００５５】
【数８】
2007033257.tif 000010
【００５６】
ただし、
【００５７】
【数９】
2007033257.tif 000011
【００５８】
である。
【００５９】
ステップＳ６０９０において、較正情報算出部１０６０は、全てのＬに対して、データリストＤＬ中の各データに含まれる指標Ｐｋｎｊの実際の画像座標ｕＬＰｋｎｊｉと、それに対応する画像座標の計算値ｕＬＰｋｎｊｉ’との誤差△ｕＬＰｋｎｊｉを次式によって算出する。
【００６０】
【数１０】
2007033257.tif 000012
【００６１】
ステップＳ６１００において、較正情報算出部１０６０は、全てのＬに対して、状態ベクトルｓＯＡ２に関する画像ヤコビアン（すなわち、数５の関数Ｆｊ（）を状態ベクトルｓＯＡ２の各要素で偏微分した解を各要素に持つ（２×ＮＬ）行×６列のヤコビ行列）ＪｕＬＰｋｎｊｉｓ（＝∂ｕＬＰｋｎｊｉ／∂ｓＯＡ２）を算出する。ここで、ＮＬは検出したすべての指標の総数である。
【００６２】
ステップＳ６１１０において、較正情報算出部１０６０は、以上のステップで算出した、全てのＬに対する誤差ΔｕＬＰｋｎｊｉ及びヤコビ行列ＪｕＬＰｋｎｊｉｓに基づいて、ｓＯＡ２の補正値ΔｓＯＡ２を算出する。具体的には、全てのＬに対する誤差ΔｕＬＰｋｎｊｉを垂直に並べた２ＮＬ次元ベクトルの誤差ベクトル
【００６３】
【数１１】
2007033257.tif 000013
【００６４】
及びヤコビ行列ＪｕＬＰｋｎｊｉｓを垂直に並べた（２ＮＬ）行×６列の行列
【００６５】
【数１２】
2007033257.tif 000014
【００６６】
を作成し、Φの擬似逆行列Φ＋を用いて、△ｓＯＡ２を次式より算出する。
【００６７】
△ｓＯＡ２＝Φ＋Ｕ        ・・・式（１３）
ここで、ΔｓＯＡ２は６次元ベクトルであるから、２ＮＬが６以上であれば、ΔｓＯＡ２を求めることができる。なおΦ＋は、例えばΦ＋＝（ΦＴΦ）−１ΦＴによって求めることができるが、他の方法によって求めてもよい。
【００６８】
ステップＳ６１２０において、較正情報算出部１０６０は、ステップＳ６１１０において算出した補正値ΔｓＯＡ２を用いて、式（１４）に従ってｓＯＡ２を補正し、得られた値を新たなｓＯＡ２とする。ここで状態ベクトルｓＯＡ２は、物体座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢の状態ベクトルである。
【００６９】
ｓＯＡ２＋△ｓＯＡ２→△ｓＯＡ２      ・・・式（１４）
ステップＳ６１３０において、較正情報算出部１０６０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値ΔｓＯＡ２が予め定めた閾値より小さいかどうかといった何らかの判断基準を用いて、計算が収束しているか否かの判定を行う。収束していない場合には、補正後の状態ベクトルｓＯＡ２を用いて、再度ステップＳ６０８０以降の処理を行う。
【００７０】
ステップＳ６１３０において計算が収束したと判定されると、ステップＳ６１４０において、較正情報算出部１０６０は、較正情報として、物体座標系における６自由度位置姿勢センサの位置及び姿勢ｓＯＡ２を出力する。この時の出力の形態は、ｓＯＡ２そのものであっても良いし、ｓＯＡ２の位置成分を３値のベクトルで表し、姿勢成分をオイラー角や３×３の回転行列で表したものであっても良いし、ｓＯＡ２から生成した座標変換行列ＭＯＡ２であっても良い。
【００７１】
最後にステップＳ６１５０では、較正処理を終了するか否かの判定が行われる。オペレータが、センサ較正装置１１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ６０１０へと戻り、データ取得コマンドの入力を待つ。
【００７２】
以上の処理によって、対象物体に対する（すなわち、対象物体座標系における）対象物体に装着した６自由度位置姿勢センサの位置または位置と姿勢を取得することができる。
【００７３】
＜変形例１−１＞
なお、本実施形態では、較正情報として物体に装着した６自由度位置姿勢センサであったが、カメラに装着した６自由度位置姿勢センサのカメラに対する位置姿勢も同時に較正情報として求めてもよい。
【００７４】
本実施形態では、カメラに装着した６自由度位置姿勢センサのカメラ座標系における位置姿勢ＭＣＡ１は、既知であるとしていたがここでは未知パラメータとする。また、カメラに装着された６自由度位置姿勢センサＡ１のセンサ計測値より得られる世界座標系におけるカメラ装着６自由度位置姿勢センサＡ１の位置姿勢ＭＷＡ１と、物体に装着された６自由度位置姿勢センサＡ２のセンサ計測値より得られる世界座標系における物体装着６自由度位置姿勢センサＡ２の位置姿勢ＭＷＡ２より、物体装着６自由度位置姿勢センサＡ２の座標系におけるカメラ装着６自由度位置姿勢センサＡ１の位置姿勢ＭＡ２Ａ１を式（１５）のように算出する。
【００７５】
【数１３】
2007033257.tif 000015
【００７６】
カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢ＭＣＡ１、物体装着６自由度位置姿勢センサＡ２の座標系におけるカメラ装着６自由度位置姿勢センサＡ１の位置姿勢を表す変換行列をＭＡ２Ａ１、物体座標系における６自由度位置姿勢センサの位置姿勢を表す変換行列をＭＯＡ２、また、物体座標系における物体装着指標の位置姿勢を表す変換行列をＭＯＭとすると、これらより指標座標系からカメラ座標系への変換行列ＭＣＭは式（１６）のように表せる。
【００７７】
【数１４】
2007033257.tif 000016
【００７８】
求めるべき較正情報、すなわち物体座標系における６自由度位置姿勢センサの位置姿勢ＭＯＡ２、カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢ＭＣＡ１、を本実施形態と同様に６値ベクトルとして扱う。ここでは、物体座標系における６自由度位置姿勢センサの位置姿勢を状態ベクトルｓＯＡ２＝［ｘＯＡ２  ｙＯＡ２  ｚＯＡ２  ξＯＡ２  ψＯＡ２  ζＯＡ２］Ｔ、カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢を状態ベクトルｓＣＡ１＝［ｘＣＡ１  ｙＣＡ１  ｚＣＡ１  ξＣＡ１  ψＣＡ１  ζＣＡ１］Ｔ、と記述する。
【００７９】
夫々の状態ベクトルｓＯＡ２、ｓＣＡ１に適当な初期値を与える。初期値は、例えば手計測を行うなどしてあらかじめ求めておく。較正情報である未知パラメータをｓ＝［ｓＯＡ２Ｔ  ｓＣＡ１Ｔ］Ｔとし、例えば本実施形態と同様に画像ヤコビアンを用いた繰り返し計算によって、全入力データに対するｓの最適値を求める。これにより物体座標系における６自由度位置姿勢センサの位置姿勢、カメラ座標系におけるカメラ装着６自由度位置姿勢センサの位置姿勢を得て、較正情報として出力することができる。
【００８０】
［第２の実施形態］
本実施形態に係るセンサ較正装置では、物体に装着した６自由度位置姿勢センサの配置情報（物体座標系におけるセンサの位置と姿勢）を算出する。本実施形態では、較正済みの指標の物理座標と画像座標の対応関係から得られる物体に対するカメラの位置姿勢と、何らかの方法で求めた世界座標系におけるカメラの位置姿勢とを利用して、物体に装着した６自由度位置姿勢センサの較正を行う。以下、本実施形態に係る６自由度位置姿勢センサ較正装置及び６自由度位置姿勢センサ較正方法について説明する。
【００８１】
図４は、本実施形態に係るセンサ較正装置２１００の概略構成を示している。図４に示すように、本実施形態に係るセンサ較正装置は、カメラ２０１０、画像入力部２０１５、指標検出部２０２０、計測値入力部２０３０、データ管理部２０４０、指示部２０５０、較正情報算出部２０６０、位置姿勢算出部２０７０によって較正されており、較正済みの指標が装着された物体２０００に接続されている。
【００８２】
また、物体２０００には６自由度位置姿勢センサＡ２が装着されており、世界座標系に対する６自由度位置姿勢センサＡ２の位置姿勢を計測することが可能となっている。６自由度位置姿勢センサＡ２は計測値入力部２０３０に接続されている。また、カメラ２０１０の世界座標系に対する位置姿勢（以下ではこれを、第一の実施形態と同様にＭＷＣと記す）は何らかの方法で取得可能となっている。本実施形態では、カメラ２０１０は固定カメラであって、世界座標系におけるその位置姿勢は予め既知な値として入力されており、不図示のメモリに記憶されているものとする。カメラ２０１０の位置姿勢の入力方法はこれに限定されるものではなく、例えば、カメラ２０１０に６自由度位置姿勢センサを設置し、その計測値を取得することによって入力しても良い。
【００８３】
物体２０００上には、第一の実施形態における図３（Ａ）と同様に、物体座標系上における配置情報が既知である指標Ｐｋが複数配置されている。指標Ｐｋは、第一の実施形態と同様に、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各点が識別可能であるような指標であれば、何れの形態であってもよい。
【００８４】
カメラ２０１０は、物体上に配置された指標を撮影する。撮影された画像は、画像入力部２０１５を経て指標検出部２０２０に入力される。
【００８５】
指標検出部２０２０は、画像入力部２０１５より画像を入力し、第一の実施形態における指標検出部１０２０と同様に、入力した画像中に撮影されている指標Ｐｋを構成する各点の画像座標を検出し、検出された各々の指標Ｐｋｎを構成する点ｐｋｎｉの画像座標ｕＰｋｎｉとその識別子ｋｎを位置姿勢算出部２０７０へと出力する。
【００８６】
位置姿勢算出部２０７０は、指標検出部２０２０から検出された指標の画像座標とその識別子を入力し、既知の値である物体座標系における夫々の指標の配置情報に基づいて、物体座標系におけるカメラ２０１０の位置姿勢（以下ではこれをＭＯＣと記す）を算出する。３次元位置が既知な複数の指標の画像座標からカメラの位置姿勢を求める方法は公知の技術であるので、その詳細な説明は省略する。算出されたカメラ２０１０の位置姿勢は、データ管理部２０４０へと出力される。
【００８７】
計測値入力部２０３０は、６自由度位置姿勢センサＡ２の計測値より得られる世界座標系における６自由度位置姿勢センサＡ２の位置姿勢（以下ではこれを、第一の実施形態と同様にＭＷＡ２と記す）を入力し、データ管理部２０４０に出力する。
【００８８】
指示部２０５０は、不図示のオペレータからデータ取得コマンドが入力されたとき時には「データ取得」の指示をデータ管理部２０４０に、較正情報算出コマンドが入力されたときには「較正情報算出」の指示を較正情報算出部２０６０に送信する。
【００８９】
データ管理部２０４０は、指示部２０５０から「データ取得」の指示を受けると、６自由度位置姿勢センサＡ２の世界座標系における位置姿勢ＭＷＡ２を計測値入力部２０３０から入力し、物体座標系におけるカメラ２０１０の位置姿勢ＭＯＣを位置姿勢算出部２０７０から入力し、さらに、世界座標系におけるカメラ２０１０の位置姿勢ＭＷＣを不図示のメモリから入力し、［ある時刻における６自由度位置姿勢センサＡ２の世界座標系における位置姿勢ＭＷＡ２−同時刻に算出された物体座標系におけるカメラ２０１０の位置姿勢ＭＯＣ−同時刻の世界座標系におけるカメラ２０１０の位置姿勢ＭＷＣ］の組を一つのデータリストに追加しこれを保持する。また、データ管理部２０４０は、較正情報算出部２０６０からの要求に従って、生成したデータリストを較正情報算出部２０６０に出力する。
【００９０】
較正情報算出部２０６０は、指示部２０５０から「較正情報算出」の指示を受け取ると、データ管理部２０４０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報、すなわち、物体座標系における６自由度位置姿勢センサＡ２の位置姿勢（以下ではこれを、第一の実施形態と同様にＭＯＡ２と記す）を出力する。
【００９１】
図５は、本実施形態の較正装置が較正情報を求める際に行う処理のフローチャートである。なお同フローチャートに従ったプログラムコードは、本実施形態の、不図示のＲＡＭやＲＯＭなどのメモリ内に格納され、不図示のＣＰＵにより読み出され、実行される。
【００９２】
ステップＳ７０１０において、指示部２０５０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、センサ較正用のデータ取得を行う位置に物体２０００またはカメラ２０１０を配置した時に、データ取得コマンドを入力する。指示部２０５０は、データ取得コマンドが入力されている場合には、ステップＳ７０２０へと処理を移行させる。
【００９３】
ステップＳ７０２０において、データ管理部２０４０は、世界座標系におけるカメラ２０１０の位置姿勢の情報を不図示のメモリから入力する。
【００９４】
ステップＳ７０３０において、データ管理部２０４０は、位置姿勢算出部２０７０から、物体座標系におけるカメラ２０１０の位置姿勢を入力する。
【００９５】
ステップＳ７０３５において、データ管理部２０４０は、計測値入力部２０３０から、世界座標系における物体装着６自由度位置姿勢センサＡ２の位置姿勢を入力する。
【００９６】
次に、ステップＳ７０４０において、データ管理部２０４０は、入力したデータの組をデータリストＤＬにデータＤＬとして追加する。
【００９７】
以上の処理によって、データの取得が行われる。
【００９８】
ステップＳ７０５０では、データ管理部２０４０によって、それまでに取得されたデータリストが、較正情報を算出するに足るだけの情報を有しているかどうかの判定が行われる。条件を満たしていない場合には、再びステップＳ７０１０へと戻り、データ取得コマンドの入力を待つ。一方、データリストが較正情報算出の条件を満たしている場合には、ステップＳ７０６０へと処理を移行させる。データリストが較正情報を算出するに足るだけの情報を有す条件としては、例えば、少なくとも二組以上のデータがデータリストＤＬに含まれていることが挙げられる。本実施形態においては、データが一組あれば解の算出が可能であるので、必ずしもこのような判定工程は必要ない。また、より多くのデータを要求するように条件を設定してもよい。
【００９９】
次にステップＳ７０６０において、較正情報算出コマンドがオペレータから入力されたか否かの判定を行う。較正情報算出コマンドが入力されている場合には、ステップＳ７０７０へと処理を移行し、入力されていない場合には、再びステップＳ７０１０へと戻り、データ取得コマンドの入力を待つ。
【０１００】
ステップＳ７０７０において、データリストＤＬに含まれる夫々のデータの組に対して、世界座標系におけるカメラ２０１０の位置姿勢ＭＷＣと、世界座標系における６自由度位置姿勢センサＡ２の位置姿勢ＭＷＡ２と、物体座標系におけるカメラ２０１０の位置姿勢ＭＯＣとから、較正対象である物体座標系における６自由度位置姿勢センサＡ２の位置姿勢ＭＯＡ２を式（１７）のように算出する。
【０１０１】
【数１５】
2007033257.tif 000017
【０１０２】
さらに、データリストＤＬに複数のデータの組が含まれている場合には、式（１７）によって夫々のデータの組毎に推定されたＭＯＡ２ｉ（ｉ＝１，．．，Ｎ；ＮはデータリストＤＬに含まれるデータの組数）の平均を求め、これを最終的なＭＯＡ２とする。複数の位置姿勢Ｍｉの平均は、例えば、位置成分に関しては、夫々のＭｉの位置を表す３次元ベクトルｔｉの平均値ｔを求め、姿勢成分に関しては、夫々のＭｉの姿勢を表す回転行列Ｒｉを姿勢の四元数表現である４次元ベクトルωｉに一旦変換し、ωｉの平均値ωを求め、得られたωを姿勢の回転行列表現Ｒに変換し、さらに、得られた位置ｔと姿勢Ｒから、式（１）等に基づいて求めることができる。なお、姿勢の回転行列表現と四元数表現の相互変換は三次元幾何学の基本的事項であるので、その詳細な説明は省略する。
【０１０３】
また、ＮＪ枚の画像を撮影すると、撮影画像ごとに存在するＭＯＣｉ，ＭＡ２Ｃｉを用いて式（１８）のように表せる。
【０１０４】
【数１６】
2007033257.tif 000018
【０１０５】
ここで、
【０１０６】
【数１７】
2007033257.tif 000019
【０１０７】
【数１８】
2007033257.tif 000020
【０１０８】
とすると、式（１８）は式（２１）のように表せる。
【０１０９】
Ａ＝Ｂ・ＭＯＡ２      ・・・式（２１）
撮影した画像が複数ある場合には、ＭＯＡ２を求めるためにＢの一般化逆行列を用いて式（２２）のように計算してもよい。
ＭＯＡ２＝（Ｂｔ・Ｂ）−１・Ｂｔ・Ａ      ・・・式（２２）
ステップＳ７１００は、以上の処理によって求められたＭＯＡ２を、較正情報、すなわち、物体座標系における６自由度センサＡ２の位置姿勢として出力する。この時の出力の形態は、ＭＯＡ２そのものであっても良いし、ＭＯＡ２の位置成分を３値のベクトルｔで表し、姿勢成分をオイラー角や３×３の回転行列Ｒで表したものであっても良いし、他のいずれの表現方法でもよい。
【０１１０】
最後にステップＳ７１１０では、較正処理を終了するか否かの判定が行われる。オペレータが、センサ較正装置２１００に対して、較正処理の終了を指示した場合には、処理を終了させ、較正処理の継続（再較正）を指示した場合には、再びステップＳ７０１０へと戻り、データ取得コマンドの入力を待つ。
【０１１１】
以上の処理によって、対象物体に装着した６自由度位置姿勢センサの対象物体に対する（すなわち、対象物体座標系における）位置及び姿勢を、簡便かつ正確に取得することができる。
【０１１２】
［他の実施形態］
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても、達成されることは言うまでもない。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１１３】
さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。
【０１１４】
本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明した図２に示すフローチャートに対応するプログラムコードが格納されることになる。
【図面の簡単な説明】
【０１１５】
【図１】第１の実施形態に係るセンサ較正装置の概略構成を示す図である。
【図２】第１の実施形態に係るセンサ較正方法の概略処理を示すフローチャートである。
【図３】第１の実施形態に係る指標の概略構成を示す図である。
【図４】第２の実施形態に係るセンサ較正装置の概略構成を示す図である。
【図５】第２の実施形態に係るセンサ較正方法の概略処理を示すフローチャートである。
【図１】
2007033257.tif 000021
【図２】
2007033257.tif 000022
【図３】
2007033257.tif 000023
【図４】
2007033257.tif 000024
【図５】
2007033257.tif 000025