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

Abstract

【課題】 物体に装着した較正対象指標の該物体に対する配置情報と、該物体に装着したセンサの該物体に対する配置情報とを、同時に簡便かつ正確に求めることができるようにする。
【解決手段】 物体を客観視点位置から撮影した客観視点画像を取得し、センサの計測値を入力し、客観視点画像から較正対象指標の画像座標に関する情報を検出し、計測値と較正対象指標の画像座標に関する情報とを用いて、物体に装着した較正対象指標の物体に対する配置情報と、物体に装着したセンサの物体に対する配置情報とを求める。
【選択図】 図１

Description

本発明は、物体に装着した指標の物体に対する配置情報と、物体に装着したセンサの物体に対する配置情報を求めるものに関する。

計測対象物体に複数の指標を設定し、カメラによって計測対象物体を撮影し、撮像した画像内における指標の投影像の画像座標を検出することで、計測対象物体の（カメラに対する、あるいは、カメラの設置されている空間における）位置及び姿勢を求めることが行われている（例えば、非特許文献１を参照）。

本件の発明者らも、非特許文献３において、計測対象物体上に指標と姿勢センサを装着し、客観視点位置から計測対象物体を撮像した客観視点画像から検出した指標の像と、姿勢センサによる姿勢計測値を利用して、物体の位置及び姿勢を計測する方法を提案している。さらに、撮像装置が計測対象物体である場合に、該撮像装置に指標と姿勢センサを装着し、客観視点位置から該撮像装置を撮像した客観視点画像から検出した指標の像と、該撮像装置自身の撮像画像から検出した情景中に別途配置されている指標の像と、姿勢センサによる姿勢計測値を利用して、物体の位置及び姿勢を計測する方法を提案している。

物体に設定した複数の指標の像を検出することで物体の位置及び姿勢を算出する方法においては、指標と計測対象物体との相対的な位置関係が既知でなければならない。また、非特許文献３のように物体の位置及び姿勢の計測に姿勢センサを用いる場合には、計測対象物体に対する姿勢センサの相対姿勢が既知でなければならない。

従来は、これらの関係、すなわち物体に対する指標の配置情報および物体に対するセンサの配置情報（以下ではこれを一般的に較正情報と呼ぶ）を求める作業（以下ではこれを一般的に較正と呼ぶ）を、別々に行っていた。すなわち、従来は、指標の較正は指標の較正で、姿勢センサの較正は姿勢センサの較正で、別々の工程として行っていた。
Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ ｐａｒａｍｅｔｅｒｉｚｅｄ ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ ｍｏｄｅｌｓ ｔｏ ｉｍａｇｅｓ，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１． Ａ．Ｉ．Ｃｏｍｐｏｒｔ，Ｅ．Ｍａｒｃｈａｎｄ，Ｆ．Ｃｈａｕｍｅｔｔｅ，Ａ ｒｅａｌ−ｔｉｍｅ ｔｒａｃｋｅｒ ｆｏｒ ｍａｒｋｅｒｌｅｓｓ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ，Ｐｒｏｃ．Ｉｎｔ'ｌ Ｓｙｍｐ．ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ ２００４，ｐｐ．３６−４５，２００４． Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，ａｎｄ Ｈ．Ｙａｍａｍｏｔｏ："Ａ ｈｅａｄ ｔｒａｃｋｉｎｇ ｍｅｔｈｏｄ ｕｓｉｎｇ ｂｉｒｄ'ｓ−ｅｙｅ ｖｉｅｗ ｃａｍｅｒａ ａｎｄ ｇｙｒｏｓｃｏｐｅ，"Ｐｒｏｃ． ３ｒｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔ'ｌ Ｓｙｍｐ．ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ ２００４），ｐｐ．２０２−２１１，２００４．

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

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

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

本願請求項１記載の発明は、物体に装着した較正対象指標の該物体に対する配置情報と、該物体に装着したセンサの該物体に対する配置情報とを較正情報として求める情報処理方法であって、前記物体を撮影した画像を取得し、前記センサの計測値を入力し、前記画像から前記較正対象指標の画像座標に関する情報を検出し、前記計測値と前記較正対象指標の画像座標に関する情報とを用いて、前記較正情報を求めることを有することを特徴とする。

本願請求項８記載の発明は、撮像装置に装着した較正対象指標の撮像装置に対する位置と、該撮像装置に装着した姿勢センサの撮像装置に対する姿勢を較正情報として算出する情報処理方法であって、前記撮像装置を撮影した第一の画像を取得し、前記撮像装置によって撮影された第二の画像を取得し、前記姿勢センサによる計測値を入力し、前記較正対象指標の画像座標に関する情報を前記第一の画像から検出し、情景に配置された基準指標の画像座標に関する情報を前記第二の画像から検出し、前記計測値と、前記較正対象指標の画像座標に関する情報と、前記基準指標の画像座標に関する情報とを用いて、前記較正情報を算出することを特徴とする。

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

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

［第１の実施形態］
本実施形態に係る指標較正装置は、撮像装置に装着した指標の撮像装置に対する配置情報と、撮像装置に装着された姿勢センサの配置情報とを取得する。以下、本実施形態に係る指標較正装置及び指標較正方法について説明する。

図１は、本実施形態に係る指標較正装置１００の概略構成を示す図である。図１に示したように、本実施形態に係る指標較正装置１００は、主観視点指標検出部１１０、センサ計測値入力部１２０、客観視点カメラ１４０、客観視点指標検出部１５０、指示部１６０、データ管理部１７０、較正情報算出部１８０によって構成されており、較正対象である撮像装置１３０と、後述する姿勢センサ１９０に接続されている。

なお、撮像装置１３０上には、較正される対象である姿勢センサ１９０が装着されており、さらに、較正される対象であるふたつの指標Ｐ^１，Ｐ^２（以下、これを客観視点指標と呼ぶ）が設定されているものとする。主観視点カメラ座標系（撮像装置１３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における、姿勢センサ１９０の姿勢と夫々の客観視点指標の位置は未知であり、この未知である主観視点カメラ座標系における姿勢センサの姿勢及び夫々の客観視点指標の位置が、本実施形態に係る指標較正装置が較正する情報、すなわち、本実施形態に係る指標較正装置の出力となる。

本実施形態では、計測対象物体である撮像装置上に指標を配置するともに、姿勢サンサを装着しているので、較正情報は指標の配置情報および姿勢センサの配置情報となる。

現実空間中の複数の位置には、撮像装置１３０によって撮影するための指標（以下、主観視点指標（基準指標））として、世界座標系（現実空間の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における位置が既知である複数個の主観視点指標Ｑ^ｋ（ｋ＝１，，，Ｋ_１）が配置されている。主観視点指標Ｑ^ｋは、指標較正用のデータを取得する時の撮像装置１３０によって、少なくとも３個以上の指標が観測されるように設置されていることが望ましい。図１の例は、４個の主観視点指標Ｑ^１，Ｑ^２，Ｑ^３，Ｑ^４が配置されており、そのうちの３個Ｑ^１，Ｑ^３，Ｑ^４が撮像装置１３０の視野内に含まれている状況を示している。

主観視点指標Ｑ^ｋは、例えば、それぞれが異なる色を有する円形状のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。また、ある程度の面積を有する四角形領域によって形成されるような四角形指標を用いることも可能である。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが識別可能であるような指標であれば、何れの形態であってもよい。

主観視点指標検出部１１０は、撮像装置１３０が出力する画像（以下、これを主観視点画像と呼ぶ）を撮像装置１３０から入力し、入力した画像中に撮影されている主観視点指標Ｑ^ｋの画像座標を検出する。例えば、主観視点指標Ｑ^ｋの各々が異なる色を有するマーカによって構成されている場合には、主観視点画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、主観視点指標Ｑ^ｋの各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを主観視点画像上に施すことにより、指標の位置を検出する。基準指標として四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の方向と識別子を取得する。なお、このようにして検出される四角形指標は、本明細では、４つの頂点の個々によって形成される４つの指標であると考える。

さらに、主観視点指標検出部１１０は、検出された各々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを、データ管理部１７０の要求に従ってデータ管理部１７０へと出力する。ここで、ｎ（ｎ＝１，，，Ｎ）は検出された指標夫々に対するインデックスであり、Ｎは検出された指標の総数を表している。例えば図１の場合には、Ｎ＝３であり、識別子ｋ_１＝１，ｋ_２＝３，ｋ_３＝４とこれらに対応する画像座標ｕ^Ｑｋ１，ｕ^Ｑｋ２，ｕ^Ｑｋ３が出力される。本実施形態における主観視点指標検出部１１０は、撮像装置１３０からの画像の入力をトリガとして上記の指標検出処理を逐次実行しているが、データ管理部１７０からの要求に応じて（その時点で入力している主観視点画像を利用して）処理を実行する構成としても良い。

センサ計測値入力部１２０は、姿勢センサ１９０から姿勢計測値を入力し、データ管理部１７０の要求に従ってデータ管理部１７０へと出力する。姿勢センサ１９０が出力する値は一般的にはセンサが独自に定義するセンサ座標系におけるセンサ自身の姿勢であるが、センサ計測値入力部１２０は、既知の値であるセンサ座標系から世界座標系への変換行列を乗算することでこれを世界座標系におけるセンサ自身の姿勢に変換し、変換後の回転行列（以下ではこれをＲ_ＷＳと記する）をデータ管理部１７０へと出力する。

客観視点カメラ１４０は、撮像装置１３０を撮像可能な位置に固定して配置されている。世界座標系における客観視点カメラ１４０の位置及び姿勢は、既知の値として較正情報算出部１８０に予め保持されているものとする。

客観視点指標検出部１５０は、客観視点カメラ１４０が撮影した画像（以下、これを客観視点画像と呼ぶ）を入力し、主観視点指標検出部１１０と同様な処理によって（較正対象となっている客観視点指標Ｐ^１，Ｐ^２の種類に応じて）画像中に撮影されている各客観視点指標の画像座標を検出し、検出された画像座標ｕ^Ｐ１，ｕ^Ｐ２を、データ管理部１７０の要求に従ってデータ管理部１７０へと出力する。本実施形態における客観視点指標検出部１５０は、客観視点画像の入力をトリガとして上記の指標検出処理を逐次実行しているが、データ管理部１７０からの要求に応じて（その時点で入力している客観視点画像を利用して）処理を実行する構成としても良い。

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

データ管理部１７０は、指示部１６０から「データ取得」の指示を受けると、主観視点指標検出部１１０から主観視点指標の画像座標とその識別子を入力し、センサ計測値入力部１２０からセンサ計測値Ｒ_ＷＳを入力し、客観視点指標検出部１５０から客観視点指標の画像座標ｕ^Ｐ１，ｕ^Ｐ２を入力し、［ある時刻におけるセンサ計測値−同時刻に検出された全客観視点指標の画像座標−同時刻に検出されたある主観視点指標の検出情報（画像座標と識別子）］の組を、入力された主観視点指標の数だけ作成し、データリストに追加してこれを保持する。また、データ管理部１７０は、較正情報算出部１８０からの要求にしたがって、保持しているデータリストを較正情報算出部１８０に出力する。

較正情報算出部１８０は、指示部１６０から「較正情報算出」の指示を受けると、データ管理部１７０からデータリストを入力し、これをもとに較正処理を行い、その結果として得られた較正情報（すなわち、各客観視点指標の主観視点カメラ座標系における位置と姿勢センサ１９０の主観視点カメラ座標系における姿勢）を出力する。

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

ステップＳ２００５において、指示部１６０は、データ取得コマンドがオペレータから入力されたか否かの判定を行う。オペレータは、データ取得を行う位置に撮像装置１３０を配置して、データ取得コマンドを入力する。指示部１６０は、データ取得コマンドが入力されている場合には、ステップＳ２０１０へと処理を移行させる。

ステップＳ２０１０において、データ管理部１７０は、主観視点指標検出部１１０から、主観視点指標検出部１１０によって検出された夫々の主観視点指標Ｑ^ｋｎの画像座標ｕ^Ｑｋｎとその識別子ｋ_ｎを入力する。

ステップＳ２０２０において、データ管理部１７０は、客観視点指標検出部１５０から、客観視点指標検出部１５０によって検出された客観視点指標の画像座標ｕ^Ｐ１，ｕ^Ｐ２を入力する。

ステップＳ２０３０において、データ管理部１７０は、センサ計測値入力部１２０から姿勢計測値Ｒ_ＷＳを入力する。以上のステップＳ２０１０，Ｓ２０２０，Ｓ２０３０の処理により、同一時刻におけるセンサ計測値と各客観視点指標及び主観視点指標の画像座標を得ることができる。

ステップＳ２０４０において、データ管理部１７０は、入力した夫々の主観視点指標Ｑ^ｋｎの情報を、データリストＬにデータＤ_ｉとして追加する。具体的には、主観視点指標検出部１１０から入力する識別子ｋ_ｎをｋ_ｉとし、同じく主観視点指標検出部１１０から入力するｕ^Ｑｋｎをｕ^Ｑｋｉとし、客観視点指標検出部１５０から入力する客観視点指標の画像座標ｕ^Ｐ１，ｕ^Ｐ２をｕ^Ｐ１ｉ，ｕ^Ｐ２ｉとし、センサ計測値入力部１２０から入力する姿勢計測値Ｒ_ＷＳをＲ_ＷＳｉとして、Ｄ_ｉ＝［ｋ_ｉ，ｕ_ｉ ^Ｑｋｉ，ｕ^Ｐ１ｉ，ｕ^Ｐ２ｉ，Ｒ_ＷＳｉ］の組を、ｉ番目のデータとしてデータリストＬに登録する。ここでｉ（ｉ＝１，，，Ｉ）は、データリストＬに登録したデータの組夫々に対するインデックスであり、Ｉは登録したデータの総組数を表している。

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

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

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

較正情報算出部１８０は、求めるべき較正情報を９値の状態ベクトル、すなわち、主観視点カメラ座標系における客観視点指標Ｐ^１の位置ｘ_Ｃ ^Ｐ１＝（ｘ_Ｃ ^Ｐ１，ｙ_Ｃ ^Ｐ１，ｚ_Ｃ ^Ｐ１）、主観視点カメラ座標系における客観視点指標Ｐ^２の位置ｘ_Ｃ ^Ｐ２＝（ｘ_Ｃ ^Ｐ２，ｙ_Ｃ ^Ｐ２，ｚ_Ｃ ^Ｐ２）、主観視点カメラ座標系における姿勢センサ１９０の姿勢ω_ＣＳ＝（ξ_ＣＳ，φ_ＣＳ，ζ_ＣＳ）によって定められるｓ＝［ｘ_Ｃ ^Ｐ１ ｙ_Ｃ ^Ｐ１ ｚ_Ｃ ^Ｐ１ ｘ_Ｃ ^Ｐ２ ｙ_Ｃ ^Ｐ２ ｚ_Ｃ ^Ｐ２ ξ_ＣＳ φ_ＣＳ ζ_ＣＳ］^Ｔとして扱う。なお、姿勢を３値で表現する方法にはさまざまなものが存在するが、ここでは、ベクトルの大きさによって回転角を、ベクトルの向きによって回転軸を定義するような３値のベクトルによって表現されているものとする。

ステップＳ２０７０において、較正情報算出部１８０は、状態ベクトルｓに適当な初期値を与える。この値は、例えば、オペレータによる対話的な設定操作等によって予め与えておく。

ステップＳ２０８０において、較正情報算出部１８０は、データリストＬ中の各データＤ_ｉおよび状態ベクトルｓから、全てのｉに対して、主観視点指標Ｑ^ｋｉの主観視点画像座標の理論値ｕ'_ｉ ^Ｑｋｉ＝［ｕ'_ｘｉ ^Ｑｋｉ，ｕ'_ｙｉ ^Ｑｋｉ］を算出する。ここで主観視点指標の主観視点画像座標の理論値とは、状態ベクトルの値がｓである時に、世界座標系における位置ｘ_Ｗ ^Ｑｋｉが既知な主観視点指標Ｑ^ｋｉの、主観視点画像中に見えるべき座標を指す。ｕ'_ｉ ^Ｑｋｉの算出は、状態ベクトルｓの関数

に基づいて行われる。

具体的には、関数Ｆ_ｉ（ ）は、ｉ番目のデータを取得した時の主観視点指標Ｑ^ｋｉの主観視点カメラ座標における位置ｘ_Ｃｉ ^Ｑｋｉを、状態ベクトルｓ及びデータＤ_ｉから求める次式、

及び、主観視点指標Ｑ^ｋｉの主観視点画像上の座標ｕ'_ｉ ^Ｑｋｉをｘ_Ｃｉ ^Ｑｋｉから求める次式、

によって構成されている。ここでｆ^Ｃ _ｘ及びｆ^Ｃ _ｙは、それぞれｘ軸方向及びｙ軸方向における撮像装置１３０の焦点距離であり、既知の値として予め保持されているものとする。

式２におけるＲ_ＷＣｉは、ｉ番目のデータを取得した時の世界座標系における主観視点カメラの姿勢を表しており、状態ベクトルｓの要素であるω_ＣＳによって定まる行列Ｒ_ＣＳと、データＤ_ｉの要素である姿勢計測値Ｒ_ＷＳｉによって、次式によって定義される。

ここで、Ｒ_ＣＳは、センサ座標系から主観視点カメラ座標系への姿勢の変換を施す回転行列であり、次式によって定義される。

ただし、

である。

また、ｔ_ＷＣｉは、ｉ番目のデータを取得した時の世界座標系における主観視点カメラの位置であり、状態ベクトルｓ及びＤ_ｉから、次式によって得ることができる。

ここで、

を表している。なお、Ｒ_ＷＢは世界座標系における客観視点カメラ１４０の姿勢を表す回転行列、ｔ_ＷＢは世界座標系における客観視点カメラ１４０の位置を表す平行移動ベクトルであり、既知の値として予め保持されている世界座標系における客観視点カメラ１４０の位置及び姿勢に基づいて、予め算出されているものとする。また、ｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは、それぞれｘ軸方向及びｙ軸方向における客観視点カメラ１４０の焦点距離であり、既知の値として予め保持されているものとする。

なお、式７から式９の関係は、客観視点指標Ｐ^ｊ（ｊ＝１，２）を客観視点カメラで観測する際の幾何変換を式１０及び式１１のように定式化し、

これを展開することで次式

を得て、式１２及び式１３を解くことで得ることができる。なお、式１２及び式１３における記号ｘ'_ＷＣｉ，ｙ'_ＷＣｉ，ｚ'_ＷＣｉは、次式の関係を表している。

ステップＳ２０９０において、較正情報算出部１８０は、全てのｉに対して、データリストＬ中の各データに含まれる主観視点指標Ｑ^ｋｉの実際の画像座標ｕ_ｉ ^Ｑｋｉと、それに対応する画像座標の理論値ｕ'_ｉ ^Ｑｋｉとの誤差△ｕ_ｉ ^Ｑｋｉを次式によって算出する。

ステップＳ２１００において、較正情報算出部１８０は、全てのｉに対して、状態ベクトルｓに関する画像ヤコビアン（すなわち、式１の関数Ｆ_ｉ（）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×９列のヤコビ行列）Ｊ_ｕｉｓ ^Ｑｋｉ（＝∂ｕ_ｉ ^Ｑｋｉ／∂ｓ）を算出する。具体的には、式３の右辺を主観視点カメラ座標系上の位置ベクトルｘ_Ｃｉ ^Ｑｋｉの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_{ｕｉｘＣｉ} ^Ｑｋｉ（＝∂ｕ_ｉ ^Ｑｋｉ／∂ｘ_Ｃｉ ^Ｑｋｉ）と、式２の右辺を状態ベクトルｓの各要素で偏微分した解を各要素に持つ３行×９列のヤコビ行列Ｊ_ｘＣｉｓ ^Ｑｋｉ（＝∂ｘ_Ｃｉ ^Ｑｋｉ／∂ｓ）を算出し、次式によってＪ_ｕｉｓ ^Ｑｋｉを算出する。

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

及びヤコビ行列Ｊ_ｕｉｓ ^Ｑｋｉを垂直に並べた２Ｉ行×９列の行列

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

△ｓ^Ｐｋ＝Φ^＋Ｕ ・・・式（１９）
なおΦ^＋は、例えばΦ^＋＝（Φ^ＴΦ）^−１Φ^Ｔによって求めることができるが、他の方法によって求めてもよい。

ステップＳ２１２０において、較正情報算出部１８０は、ステップＳ２１１０において算出した補正値△ｓを用いて、式２０に従ってｓを補正し、得られた値を新たなｓとする。

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

ステップＳ２１３０において計算が収束したと判定されると、ステップＳ２１４０において、較正情報算出部１８０は、得られた状態ベクトルｓを出力する。

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

以上の処理によって、撮像装置に装着した指標の撮像装置に対する配置情報および撮像装置に装着したセンサの撮像装置に対する配置情報を、簡便かつ正確に取得することができる。

＜変形例＞
なお、上記実施形態においては、状態ベクトルの補正値の算出に式１９で表現されるＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法を用いているが、補正値の算出は必ずしもＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても本発明の本質が損なわれないことは言うまでもない。

また、上記実施形態においては、撮像装置に姿勢センサを用いたが、位置姿勢センサなどの他のセンサを用いても構わない。

また、本実施形態においては、画像上で２次元座標を与えるような指標を主観視点指標として用いているが、非特許文献１や非特許文献２で開示されているような線特徴や幾何特徴を、評価のための基準指標として用いてもよい。例えば、線特徴を用いる場合には、直線の原点からの距離を誤差評価基準として、画像からの検出値ｄと状態ベクトルｓからの推定値ｄ'から算出する誤差△ｄによって誤差ベクトルＵを構成し、ｄ'の算出式を状態ベクトルｓの各要素で偏微分した解を各要素に持つ１行×４列のヤコビ行列Ｊ_ｄｓ（＝∂ｄ／∂ｓ）によって行列Φを構成することで、上記実施形態と同様な枠組みによって補正値の算出を行うことが出来る。なお、ｄ'の算出式は、非特許文献１や非特許文献２等で撮像装置の位置姿勢の関数として開示されており、かつ、撮像装置の位置姿勢はｓの関数として得られる（式４及び式７）ので、これらから容易に得ることができる。また、線特徴と点特徴、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することも可能である。

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

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

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

なお、物体１０００上には、図５（Ａ）に示すように指標が複数配置されている。ここで、物体に配置された指標をＰ^ｋ（ｋ＝１，，，Ｋ_ｏ）で表す。但し、Ｋ_ｏは物体上に配置された較正対象となる指標の数である。また、図５（Ｂ）で示すように指標Ｐ^ｋは点ｐ^ｋｉ（（ｋ＝１，，，Ｋ_ｏ）、（ｉ＝１，，，Ｎ_ｋ））により構成される。但し、Ｎ_ｋは、指標Ｐ^ｋを構成する点の総数を表す。ここで物体上に配置された夫々の指標を構成する各点の指標座標系における位置は既知であるとする。物体座標系（物体上の一点を原点として定義し、互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）における各指標の位置姿勢は未知である。また，物体上に配置された６自由度位置姿勢センサの物体座標系における位置姿勢は未知である。なお、物体上には、物体座標系を規定するための基準指標として、物体座標系における位置が既知でかつ同一直線上にない指標が３点以上配置されている。すべての基準指標は、撮像部１０１０によって他の較正対象のマーカと少なくとも１回は同一画像内で観測される必要がある。また、物体に装着した指標Ｐ^ｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの指標であるか、さらには指標を構成する各点が識別可能であるような指標であれば、何れの形態であってもよい。

例えば、基準指標として四角形指標を用いる場合は、画像に２値化処理を施した後にラベリングを行い、一定面積以上の領域の中から４つの直線によって形成されているものを指標候補として検出する。さらに、候補領域の中に特定のパターンがあるか否かを判定することによって誤検出を排除し、また、指標の方向と識別子を取得する。なお、このようにして検出される四角形指標は、本明細では、４つの頂点の個々によって形成される４つの指標により１つの指標座標系を構成すると考える。

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

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

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

次に位置姿勢算出部１０３０について説明する。ここで、ある座標系Ａの別の座標系Ｂに対する位置をｔ、姿勢を表す３×３回転行列をＲとすると、Ａ上の点ｘ_Ａの、Ｂ上の点ｘ_Ｂへの座標変換は式２１に示す４×４行列Ｍ_ＢＡを使って式２２のように表される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

較正情報算出部１０６０は、求めるべき較正情報、すなわち物体座標系における指標の位置姿勢と、物体座標系における６自由度位置姿勢センサの位置姿勢を未知パラメータとして夫々６値ベクトルとして扱う。以下では、物体座標系における指標の位置姿勢を状態ベクトルｓ_ｍｋ＝［ｘ_ｍｋ ｙ_ｍｋ ｚ_ｍｋ ξ_ｍｋ ψ_ｍｋ ζ_ｍｋ］^Ｔ（ｋ＝１，，，Ｋ_０）、物体座標系における物体装着６自由度位置姿勢センサの位置姿勢をｓ_ＯＡ２＝［ｘ_ＯＡ２ ｙ_ＯＡ２ ｚ_ＯＡ２ ξ_ＯＡ２ ψ_ＯＡ２ ζ_ＯＡ２］^Ｔと記述する。ただしＫ_０は物体上の配置された較正対象となる指標の数である。

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

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

に基づいて行われる。

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

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

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

Ｍ_ＯＭ（ｓ_ｍｋ）は次式によって定義される。

ただし、（ｋ＝１，，，Ｋ_０）でＫ_０は物体上に配置された較正対象となる指標の数である。また、

である。また、

ただし、

である。

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

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

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

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

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

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

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

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

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

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

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

＜変形例２−１＞
なお、上記第２の実施形態では、較正情報として物体に装着した複数の指標の物体座標系における指標の位置姿勢と、物体に装着した６自由度位置姿勢センサの物体座標系における物体装着６自由度位置姿勢センサの位置姿勢であったが、それらに加えてカメラに装着した６自由度位置姿勢センサのカメラ座標系における位置姿勢を較正情報として求めてもよい。

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

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

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

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

＜変形例２−２＞
第２の実施形態に係る指標較正装置は、対象物体に対する（すなわち、対象物体座標系における）指標の「位置姿勢」と、対象物体に装着した６自由度センサの位置姿勢の較正を行うためのものであり、物体上に配置された夫々の指標を構成する各点が、指標ごとに規定される指標座標系において位置が既知であった。しかし、指標の形状はこれに限るものではなく、一点から構成される点形状の指標（以下、点指標）であってもよい。この場合には、指標較正装置は、物体上に配置された点指標の物体に対する「位置」と、対象物体に対する６自由度センサの位置姿勢を較正情報として算出する。点指標は例えば、それぞれが異なる色を有する円形状のマーカであってもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点であってもよい。

図６で示すように物体に配置された点指標をＰ^ｋ（ｋ＝１，，，Ｋ_ｏ）で表す。但し、Ｋ_ｏは物体上に配置された較正対象となる点指標の数である。また、物体に装着した点指標Ｐ^ｋは、撮影画像上における投影像の画面座標が検出可能であって、かついずれの点指標であるか識別可能であれば何れの形態であってもよい。なお、物体上には、物体座標系を規定するための基準指標として、物体座標系における位置が既知でかつ同一直線上にない指標が３点以上配置されている。対象物体上に配置された点指標をカメラで観測する際の幾何変換は、カメラ座標系における物体装着６自由度位置姿勢センサＡ２の位置姿勢を表す変換行列Ｍ_ＣＡ２ｊ、物体座標系における６自由度位置姿勢センサの位置姿勢を表す変換行列Ｍ_ＯＡ２、また、物体座標系における物体装着点指標の位置を表す変換行列ｘ_ＯＭｋとすると、これらより式３９のように表せる。

ただし、ｘ_ＣＬ ^Ｐｋｎｊは点指標Ｐ^ｋのカメラ座標を表す。またｊ（ｊ＝１，，，Ｎ_Ｊ）は、撮影した画像に対するインデックスであり、Ｌ（Ｌ＝１，，，Ｎ_Ｌ）は、データリストＤＬに登録したデータの組夫々に対するインデックスであり、Ｎ_Ｊは、撮影した画像の総数を表し、Ｎ_Ｌは、登録したデータの総組数を表している。

第２の実施例と同様に、求めるべき較正情報、すなわち物体座標系における点指標の位置ｘ_ＯＭｋを状態ベクトルｓ_ＯＭｋ＝［ｘ_ＯＭｋ ^Ｔ ｙ_ＯＭｋ ^Ｔ ｚ_ＯＭｋ ^Ｔ］^Ｔ、とし、物体座標系における６自由度位置姿勢センサの位置姿勢Ｍ_ＯＡ２を状態ベクトルｓ_ＯＡ２＝［ｘ_ＯＡ２ ｙ_ＯＡ２ ｚ_ＯＡ２ ξ_ＯＡ２ ψ_ＯＡ２ ζ_ＯＡ２］^Ｔ、と記述する。

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

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

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

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

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

Claims (13)

1. 物体に装着した較正対象指標の該物体に対する配置情報と、該物体に装着したセンサの該物体に対する配置情報とを較正情報として求める情報処理方法であって、
   前記物体を撮影した画像を取得し、
   前記センサの計測値を入力し、
   前記画像から前記較正対象指標の画像座標に関する情報を検出し、
   前記計測値と前記較正対象指標の画像座標に関する情報とを用いて、前記較正情報を求めることを特徴とする情報処理方法。
2. 前記センサは姿勢センサであり、
   前記センサの配置情報は、該物体に対する該姿勢センサの姿勢に関する情報であることを特徴とする請求項１に記載の情報処理方法。
3. 前記センサは６自由度位置姿勢センサであり、
   前記センサの配置情報は、該物体に対する該６自由度位置姿勢センサの位置及び姿勢に関する情報であることを特徴とする請求項１に記載の情報処理方法。
4. 前記物体は撮像部を有し、
   前記撮像部によって撮影された画像を取得し、
   前記画像から基準指標の画像座標に関する情報を検出し、
   前記較正情報を、前記計測値、前記較正対象指標の画像座標に関する情報および前記基準指標の画像座標に関する情報を用いて求めることを特徴とする請求項１乃至請求項３のいずれかに記載の情報処理方法。
5. 前記較正対象指標は、該物体に対する位置によって配置情報が記述される指標と、該物体に対する位置及び姿勢によって配置情報が記述される指標のいずれかまたはそれらの組み合わせであることを特徴とする、請求項１乃至請求項４のいずれかに記載の情報処理方法。
6. 前記較正情報を求める工程は、
   前記計測値と、較正情報の推定値とに基づいて、前記検出されている前記較正対象指標の前記画像内における画像座標に関する情報を推定する推定工程と、
   前記検出された前記較正対象指標の画像座標に関する情報と、前記推定工程が推定した該指標の画像座標に関する情報との間の誤差を軽減させるように、前記較正情報の推定値を補正する補正工程とを有することを特徴とする請求項１乃至請求項５のいずれかに記載の情報処理方法。
7. 前記物体は撮像部を有し、
   前記物体が有する撮像部によって撮影された画像を入力し、
   前記前記物体が有する撮像部によって撮影された画像から、情景に配置された基準指標の画像座標に関する情報を検出するとともに、
   前記較正情報を求める工程は、
   前記較正情報算出手段は、前記計測値と、前記検出された較正対象指標の画像座標に関する情報と、前記基準指標の画像座標に関する情報とを用いて、前記較正情報を算出することを特徴とする請求項１乃至請求項６のいずれかに記載の情報処理方法。
8. 撮像装置に装着した較正対象指標の撮像装置に対する位置と、該撮像装置に装着した姿勢センサの撮像装置に対する姿勢を較正情報として算出する情報処理方法であって、
   前記撮像装置を撮影した第一の画像を取得し、
   前記撮像装置によって撮影された第二の画像を取得し、
   前記姿勢センサによる計測値を入力し、
   前記較正対象指標の画像座標に関する情報を前記第一の画像から検出し、
   情景に配置された基準指標の画像座標に関する情報を前記第二の画像から検出し、
   前記計測値と、前記較正対象指標の画像座標に関する情報と、前記基準指標の画像座標に関する情報とを用いて、前記較正情報を算出することを特徴とする情報処理方法。
9. 前記較正情報を算出する工程は、
   前記計測値と、前記較正対象指標の画像座標に関する情報と、較正情報の推定値とに基づいて、前記基準指標の前記第二の画像内における画像座標に関する情報を推定し、
   前記検出された基準指標の画像座標に関する情報と、前記推定された基準指標の画像座標に関する情報との間の誤差を軽減させるように、前記較正情報の推定値を補正することを特徴とする請求項８に記載の情報処理方法。
10. 前記撮像装置の位置・姿勢に対する前記第１の画像を撮影する撮像部の位置・姿勢が算出可能であることを特徴とする請求項８または請求項９に記載の情報処理方法。
11. 前記請求項１乃至請求項１０のいずれかの情報処理方法をコンピュータにおいて実現するためのプログラム。
12. 物体に装着した較正対象指標の該物体に対する配置情報と、該物体に装着したセンサの該物体に対する配置情報とを較正情報として算出する情報処理装置であって、
    前記物体を撮影する撮像手段と、
    前記センサによる計測値を入力する計測値入力手段と、
    前記撮像手段によって撮影された第一の画像を入力する第一の画像入力手段と、
    前記較正対象指標の画像座標に関する情報を前記第一の画像から検出する第一の検出手段と、
    前記計測値入力手段が入力した前記計測値と、前記第一の検出手段が検出した夫々の較正対象指標の画像座標に関する情報とを用いて、前記較正情報を算出する較正情報算出手段とを有することを特徴とする情報処理装置。
13. 撮像装置に装着した較正対象指標の撮像装置に対する位置と、該撮像装置に装着した姿勢センサの撮像装置に対する姿勢を較正情報として算出する情報処理装置であって、
    前記撮像装置を撮影した第一の画像を取得する第１の取得手段と、
    前記撮像装置によって撮影された第二の画像を第２の取得手段と、
    前記姿勢センサによる計測値を入力する入力手段と、
    前記較正対象指標の画像座標に関する情報を前記第一の画像から検出する第一の検出手段と、
    情景に配置された基準指標の画像座標に関する情報を前記第二の画像から検出する第二の検出手段と、
    前記計測値と、前記較正対象指標の画像座標に関する情報と、前記基準指標の画像座標に関する情報とを用いて、前記較正情報を算出する較正情報算出手段とを有することを特徴とする情報処理装置。
    

Images