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