(57)【特許請求の範囲】
【請求項１】
撮像装置により撮像された、測定対象物に対する３次元位置情報が既知であり幾何学的に固有の特徴を有する少なくとも３個のマーカが写っている画像を入力する画像入力手段と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出手段と、
上記抽出した領域において、上記各マーカの上記幾何学的に固有の特徴を利用して個々のマーカを特定するマーカ特定手段と、
上記特定されたマーカの画像上での大きさの情報を用いて推定された上記撮像装置から上記マーカまでの距離と、上記特定された各マーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算する位置姿勢演算手段と、
を有することを特徴とする３次元位置姿勢センシング装置。
【請求項２】
撮像装置により撮像された、測定対象物に対する３次元位置情報が既知であり幾何学的に固有の特徴を有する少なくとも４個のマーカが写っている画像を入力する画像入力手段と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出手段と、
上記抽出した領域において、上記各マーカの上記幾何学的に固有の特徴を利用して個々のマーカを特定するマーカ特定手段と、
上記マーカ特定手段によって特定されたマーカより３つのマーカを選択するマーカ選択手段と、
上記マーカ選択手段によって選択された３つのマーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算するためのパラメータ組を複数算出するパラメータ組算出手段と、
上記マーカ選択手段によって選択されなかったマーカに対して、上記パラメータ組算出手段によって算出されたパラメータ組により算出される投影画像点と、上記選択されなかったマーカの画像内で測定された２次元位置との誤差の和を最小にする１つのパラメータ組を選択するパラメータ組選択手段と、
を有することを特徴とする３次元位置姿勢センシング装置。
【請求項３】
前記パラメータ組選択手段によって選択された１つのパラメータ組に関して、各マーカの測定対象物に対する３次元位置情報を適用して評価することにより、該パラメータ組を更新するパラメータ更新手段をさらに含むことを特徴とする請求項２記載の３次元位置姿勢センシング装置。
【請求項４】
前記マーカは円形のマーカであり、
前記位置姿勢演算手段は、前記円形のマーカの画像内での投影像を楕円として近似し、楕円の長軸の長さを利用して、前記撮影装置から前記マーカまでの距離を推定し、その推定された距離を含めて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算することを特徴とする請求項１記載の３次元位置姿勢センシング装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、３次元位置姿勢センシング装置に係り、特に、画像撮影装置を利用して対象物の３次元位置姿勢を推定することにより、対象物の３次元位置姿勢をセンシングするようにした３次元位置姿勢センシング装置に関する。
【０００２】
【従来の技術】
一般に、相対的な位置が既知な少なくとも３点のランドマークやマーカを画像撮影により認識し、対象物と撮影装置との相対的位置姿勢関係を推定する問題は、ｎ点問題の一部として考えられる（文献１：Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ  ａｎｄ  Ｒ．Ｃ．Ｂｏｌｌｅｓ，“Ｒａｎｄｏｍ  ｓａｍｐｌｅ  ｃｏｎｓｅｎｓｕｓ：Ａ  ｐａｒａｄｉｇｍ  ｆｏｒ  ｍｏｄｅｌ  ｆｉｔｔｉｎｇ  ｗｉｔｈ  ａｐｐｌｉｃａｔｉｏｎｓ  ｔｏ  ｉｍａｇｅ  ａｎａｌｙｓｉｓ  ａｎｄ  ａｕｔｏｍａｔｅｄ  ｃａｒｔｏｇｒａｐｈｙ，”Ｃｏｍｍｕｎｉｃａｔｉｏｎｓｏｆ  ｔｈｅ  ＡＣＭ，Ｖｏｌ．２４，Ｎｏ．６，Ｊｕｎｅ  １９８１，ｐｐ．３８１−３９５参照）。
【０００３】
この場合、ランドマークあるいはマーカの個数が３点のみのときには、解が複数個存在することが知られている。
【０００４】
このような問題を解決するために、特殊なマーカを利用する方法としては、特開平７−９８２０８号公報に開示されている方法などを挙げることができる。
【０００５】
すなわち、この特開平７−９８２０８号公報に開示されている方法は、大円１個と小円１個の位置関係を利用するものである。
【０００６】
また、文献２（Ｗ．Ａ．Ｈｏｆｆ，Ｔ．Ｌｙｏｎ，ａｎｄ  Ｋ．Ｎｇｕｙｅｎ，“Ｃｏｍｐｕｔｅｒ  Ｖｉｓｉｏｎ−Ｂａｓｅｄ  ＲｅｇｉｓｔｒａｔｉｏｎＴｅｃｈｎｉｑｕｅｓ  ｆｏｒ  Ａｕｇｍｅｎｔｅｄ  Ｒｅａ１１ｔｙ，”Ｐｒｏｃ．ｏｆ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｒｏｂｏｔｓ  ａｎｄ  Ｃｏｍｐｕｔｅｒ  ＶｉｓｉｏｎＸＶ，Ｖｏｌ．２９０４，ｉｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｓｙｓｔｅｍｓ  ａｎｄ  Ａｄｖａｎｃｅｄ  Ｍａｎｕｆａｃｔｕｒｉｎｇ，ＳＰＩＥ，Ｂｏｓｔｏｎ，Ｍａｓｓａｃｈｕｓｅｔｔｓ，Ｎｏｖ．１９−２１，ｐｐ．５３８−５４８，１９９６．）では、形状が同一の複数のマーカを利用して、カメラ画像から３次元位置姿勢を推定するシステムが開示されている。
【０００７】
【発明が解決しようとする課題】
しかるに、上述した特開平７−９８２０８号公報に開示されているテクニックでは、基本的に、大円１個とその付近に定義された小円１個のみからマーカが定義されるため、次のような欠点があった。
【０００８】
（１）大円１個と小円１個の画像内での大きさが小さいときには、測定誤差が大きくなる。
【０００９】
（２）大円１個と小円１個が遮蔽や画像処理上の限界から認識できなかったときには、位置姿勢が認識できなくなってしまう。
【００１０】
また、上述した文献２では、複数のマーカが同一のパターン構造で構成されているときには、マーカ自体を同定することが困難なことが多い。
【００１１】
これは、遮蔽などの影響によりマーカ群の一部が認識できないときには、特に問題となる。
【００１２】
また、環境が複雑な場合には、単一カラーなどや白色または黒色だけで構成されるような単純なマ一カの場合には、マーカと同様なパターンが、環境内に存在する場合が多く、これらのマーカをマーカでないものから差異を明らかにしながら認識することが困難であった。
【００１３】
そこで、本発明では、以上のような点に鑑みてなされたもので、
（１）遮蔽などの影響により、マーカ群の一部が観察されないときでも、対象物の３次元位置姿勢を推定することができるようにするとともに、
（２）従来のｎ点問題では、解を確定することができなかった３個のマーカからだけでも位置姿勢を推定することができるようにした、
３次元位置姿勢センシング装置を提供することを目的としている。
【００１４】
【課題を解決するための手段】
本発明によると、上記課題を解決するために、
（１）  撮像装置により撮像された、測定対象物に対する３次元位置情報が既知であり幾何学的に固有の特徴を有する少なくとも３個のマーカが写っている画像を入力する画像入力手段と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出手段と、
上記抽出した領域において、上記各マーカの上記幾何学的に固有の特徴を利用して個々のマーカを特定するマーカ特定手段と、
上記特定されたマーカの画像上での大きさの情報を用いて推定された上記撮像装置から上記マーカまでの距離と、上記特定された各マーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算する位置姿勢演算手段と、
を有することを特徴とする３次元位置姿勢センシング装置が提供される。
【００１５】
また、本発明によると、上記課題を解決するために、
（２）  撮像装置により撮像された、測定対象物に対する３次元位置情報が既知であり幾何学的に固有の特徴を有する少なくとも４個のマーカが写っている画像を入力する画像入力手段と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出手段と、
上記抽出した領域において、上記各マーカの上記幾何学的に固有の特徴を利用して個々のマーカを特定するマーカ特定手段と、
上記マーカ特定手段によって特定されたマーカより３つのマーカを選択するマーカ選択手段と、
上記マーカ選択手段によって選択された３つのマーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算するためのパラメータ組を複数算出するパラメータ組算出手段と、
上記マーカ選択手段によって選択されなかったマーカに対して、上記パラメータ組算出手段によって算出されたパラメータ組により算出される投影画像点と、上記選択されなかったマーカの画像内で測定された２次元位置との誤差の和を最小にする１つのパラメータ組を選択するパラメータ組選択手段と、
を有することを特徴とする３次元位置姿勢センシング装置が提供される。
【００１６】
また、本発明によると、上記課題を解決するために、
（３）  前記パラメータ組選択手段によって選択された１つのパラメータ組に関して、各マーカの測定対象物に対する３次元位置情報を適用して評価することにより、該パラメータ組を更新するパラメータ更新手段をさらに含むことを特徴とする（２）記載の３次元位置姿勢センシング装置が提供される。
また、本発明によると、上記課題を解決するために、
（４）  前記マーカは円形のマーカであり、
前記位置姿勢演算手段は、前記円形のマーカの画像内での投影像を楕円として近似し、楕円の長軸の長さを利用して、前記撮影装置から前記マーカまでの距離を推定し、その推定された距離を含めて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算することを特徴とする（１）記載の３次元位置姿勢センシング装置が提供される。
【００１７】
すなわち、本発明は、具体的には、位置姿勢を推定すべき対象物あるいは対象物付近に、対象物に対する相対的位置関係があらかじめ決定している『幾何学的に固有の特徴を持った固有マーカ（コードマーカ）』を複数配置し、その複数のマーカを画像撮影装置により撮影し、画像内から固有マーカ群を抽出するとともに、そのマーカの幾何学的特徴を抽出かつそのマーカの同定を行い、そのマーカの対象物に対する３次元位置と画像撮影装置により撮影された画像内での２次元位置を利用して、画像撮影装置または他の装置を規定する座標系における対象物の３次元位置姿勢を推定する方式を与えるものである。
【００１８】
【発明の実施の形態】
以下図面を参照して本発明の実施の形態について説明する。
【００１９】
（第１の実施の形態）
図１乃至図８を参照して第１の実施の形態について説明する。
【００２０】
図１は、第１の実施の形態による３次元位置姿勢センシング装置の構成を示すブロック図である。
【００２１】
図１に示されているように、３次元位置姿勢を推定すべき対象物１あるいはその付近には、幾何学的に固有の特徴を持った複数の固有マーカ２（以下コードマーカと略す）が配置されている。
【００２２】
そして、このコードマーカ２を画像撮影装置３により撮影し、その撮影した画像５がコンピュータ４内に転送される。
【００２３】
ここで、画像撮影装置３は、一般的なＴＶカメラなどあるいはディジタルビデオカメラなどでよく、画像撮影装置３から画像５を受け取るコンピュータ４は、通常のコンピュータであっても特殊な画像処理演算装置であっても構わない。
【００２４】
また、画像撮影装置３としてのＴＶカメラがアナログ信号を出力する場合には、コンピュータ４内には、画像５をディジタル化する装置やユニットが含まれていてもよい。
【００２５】
また、画像撮影装置３がディジタルカメラやディジタルビデオカメラなどの場合には、直接コンピュータ４に画像５を転送することにより、コンピュータ４が画像５をディジタル信号として入力することができればよい。
【００２６】
このようにして、第１の実施の形態による３次元位置姿勢センシング装置では、コンピュータ４が画像撮影装置３から受け取ったコードマーカ２が撮影された画像５を受け取り、それがディジタル画像の形に変換されるとともに、そのディジタル画像をコンピュータ４で処理することにより、画像５内からコードマーカ２を認識し、そのコードマーカの画像内位置とあらかじめ登録されているコードマーカの３次元位置とを利用することにより、対象物１の画像撮影装置３に対する３次元位置姿勢を推定するものである。
【００２７】
なお、本実施の形態では、少なくとも４個のコードマーカが同定できた場合に関して、対象物の位置姿勢を推定する方法に関して解説するものとする。
【００２８】
そして、少なくとも３個のコードマーカが同定できた場合に関しては、別の実施の形態で解説するものとする。
【００２９】
まず、本実施の形態における画像と座標変換に関する基本的な扱いに関して説明しておく。
【００３０】
基本的に、対象物１と画像撮影装置３は固有の座標系を有しており、画像撮影装置３が撮影した画像５は、カメラ画像面として定義される。
【００３１】
図２は、この画像撮影装置３と、カメラ画像面と、対象物１が規定するオブジェクト座標系との関係を表したものである。
【００３２】
ここで、対象物１が規定するオブジェクト座標系はその原点を０m 、その３次元座標を（ｘm ，ｙm ，ｚm ）とする。
【００３３】
一方、画像撮影装置３が規定するカメラ座標系は、その原点を０c 、その３次元座標を（ｘc ，ｙc ，ｚc ）とする。
【００３４】
カメラ画像面はその軸がｕ軸とｖ軸により構成され、ｕ軸はカメラ座標系のｘc と平行に、ｖ軸はｙc 軸に平行に取られ、カメラ座標系を規定するｚc 軸が画像撮影装置３の光学系の光軸と一致し、その光軸とカメラ画像面が交わる点（カメラ画像面の中心）が、（ｕ0 ，ｖ0 ）で定義される。
【００３５】
画像撮影装置３に相対する対象物１の３次元位置姿勢を推定する問題は、カメラ座標系に対するオブジェクト座標系の位置姿勢を推定する問題、言い換えれば、オブジェクト座標系からカメラ座標系への座標変換パラメータ、またはカメラ座標系からオブジェクト座標系への座標変換パラメータを算出する問題に帰着される。
【００３６】
これを数学的に記述すると、斉次変換行列ｃＨｍまたはｍＨｃを利用して、
【数１】
0004794708.tif 000002
と書くことができる。
【００３７】
ここに、Ｒ＝（ｒｉｊ），Ｒ′＝（ｒ′ｉｊ）は３×３の回転行列を表し、ｔ＝（ｔｘ，ｔｙ，ｔｚ），ｔ′＝（ｔ′ｘ，ｔ′ｙ，ｔ′ｚ）は３次元並進べクトルを表す。
【００３８】
以下で詳細に説明するマーカ群｛Ｍｉ；ｉ＝１，２，．．．，ｍ｝は、あらかじめオブジェクト座標系での３次元位置が計測されており、それらを（ｘi m ，ｙi m ，ｚi m ）で表現する。
【００３９】
また、その画像内位置を（ｕｉ，ｖｉ）で記述することとする。
【００４０】
すると、画像撮影装置３をピンホールカメラモデルで近似したとき、それらの座標間には、以下の関係が成立する。
【００４１】
【数２】
0004794708.tif 000003
ここに（ｕｏ，ｖｏ）は画像内の中心、（αu ，αv ）はｕ方向とｖ方向の拡大率を表し、画像撮影装置３に関するカメラ内部パラメータであり、カメラキャリブレーションにより推定できる値である。
【００４２】
図３は、本実施の形態における幾何学的特徴を持ったコードマーカ２群の一例を表したものである。
【００４３】
このコードマーカ２は、円形の形状をしており、その大円内には、小円のパターンがコードとして表されている。
【００４４】
この例では、大円の中央部に小円１個があり、そのまわりに４個の小円が配置されている。
【００４５】
この５個の小円の白黒（またはカラー）コードにより、各マーカに固有のラベルを付与することができる。
【００４６】
例えば、図３の場合には、コード０からコード１１までの１２個の異なったコードを生成することができる。
【００４７】
図４は、本実施の形態における別のコードパターンを表したものである。
【００４８】
このパタ一ンの場合には、大円の中に７個の小円を配置することで、各種のコードを生成した場合を表わしている。
【００４９】
なお、コードを生成するパターンとしては、これだけには限らす、例えば、図５に示されるような、同心円上にコードを付加したものでも構わない。
【００５０】
ここで、基本的に重要なことは、各マーカが幾何学的特徴を有しており、それがマーカにラベルを付加するに足るコードを生成できることである。
【００５１】
また、マーカ自体は円形でなくてもよく、例えば、正方形でも正多角形でもよい。
【００５２】
図６は、本発明において、コンピュータ４に画像５が入力された後に、対象物１の３次元位置姿勢を推定するまでの処理手順を示すフローチャートである。
【００５３】
以下、各ステップについて簡単に説明する。
【００５４】
（１）ステップ１：
画像５を受け取ったコンピュータ４は、その画像５の中からコードマーカ２に対応する領域と推定される候補領域を抽出する。
【００５５】
（２）ステップ２：
コンピュータ４は、上記ステップ１により抽出された候補領域内を詳細に解祈し、その中からコードマーカ２のコードに対応する幾何学的特徴を抽出し、そのコードが認識された場合には、その領域をマーカ領域として、その画像内位置とコードを登録する。
【００５６】
（３）ステップ３：
コンピュータ４は、上記ステップ２で登録された画像から抽出されたコードマーカ２の画像内２次元位置と対象物１に相対する３次元位置とを利用することにより、画像撮影装置３に対応する対象物１の３次元位置姿勢を算出する。
【００５７】
以下、本実施の形態の中心となるステップ１，２、３について、より詳細に説明する。
【００５８】
ステップ１：
本実施の形態では、画像撮影装置３がカラー画像を生成するものとするとともに、コードマーカ２としては、図３で示されるようなコードマーカ（大円と小円の組み合わせ）からなると仮定する。
【００５９】
この際、大円の背景色は、ある規定された色から成り立っており、対象物１内で固有な色であると仮定する。
【００６０】
また、小円内のパターンは、白または黒のみのパターンから成り立っていると仮定する。
【００６１】
マーカの領域は単色から成立しているので、その単色に敏感な色フィルタをアルゴリズム内に導入するものとする。
【００６２】
具体的には、画像面（ｕ，ｖ）で定義される画像点に関して、カラー画像を構成する３フィルタの計測値Ｒ（赤）、Ｇ（緑）、Ｂ（青）から、次の式
【数３】
0004794708.tif 000004
に対応する３個のべクトルを算出し、マーカが取りうる画像内での色パターンの許容値を、次の式
【数４】
0004794708.tif 000005
を満たす画像領域を抽出する。
【００６３】
ここで、ｉｍｉｎ，ｉｍａｘ，ｒｍｉｎ，ｒｍａｘ，ｇｍｉｎ，ｇｍａｘなどの値は、あらかじめ設定しておくものとする。
【００６４】
次に、領域内の穴埋めを行うことによりマーカに対応する領域を決定する。
【００６５】
ステップ２：
次に、抽出された領域がマーカの像であるか否かを判定する。
【００６６】
基本的に、マーカの形状は円形であるので、その投影像である画像内での領域は、楕円形で近似することが可能である。
【００６７】
従つて、本ステップ２では、マーカの領域が楕円で近似することができるか否かを判定する。
【００６８】
この方法としては、文献３（Ｋ．Ｒａｈａｒｄｊａ  ａｎｄ  Ａ．Ｋｏｓａｋａ“Ｖｉｓｉｏｎ−ｂａｓｅｄ  ｂｉｎ−ｐｉｃｋｉｎｇ：Ｒｅｃｏｇｎｉｔｉｏｎ  ａｎｄ  ｌｏｃａｌｉｚａｔｉｏｎ  ｏｆ  ｍｕｌｔｉｐｌｅ  ｃｏｍｐｌｅｘ  ｏｂｊｅｃｔｓ  ｕｓｉｎｇ  ｓｉｍｐｌｅ  ｖｉｓｕａｌ  ｃｕｅｓ，”Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  １９９６  ＩＥＥＥ／ＲＳＪ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｒｏｂｏｔｓ  ａｎｄ  Ｓｙｓｔｅｍｓ，Ｏｓａｋａ，Ｊａｐａｎ，Ｎｏｖｅｍｂｅｒ  １９９６）で示されている方法により行う。
【００６９】
具体的には、
（１）マーカの候補領域と考えられる各領域を含む直方領域を切り出し、マーカ候補領域内を１、領域外を０とラベル化する。
【００７０】
この１でラベル化される領域に対して、穴埋め処理を行い、内部に存在するラベル０で表される小領域を除去する。
【００７１】
（２）ラベル１で表されるマーカ候補領域の１次モーメントｑ0 と２次モーメントＭを算出する。
【００７２】
（３）ラベル１で表されるマーカ候補領域の境界点列の集合をＡ＝｛ｑ｝として、Ａの各点に関して、以下の式で表される正規化距離ｄを算出する。
【００７３】
【数５】
0004794708.tif 000006
（４）ｄの集合Ａに対する平均値μと標準偏差σd を算出する。
【００７４】
そして、σd がある閾値よりも小さければ、マー力領域として登録するが、そうでなければマーカ領域として登録しない。
【００７５】
このようにして、楕円であると判定された場合には、楕円領域内での３値処理により、楕円領域内で考えられるパターンの抽出を行う。
【００７６】
具体的には、図７の（ａ）に示されているようなマーカ領域に対して、
（１）上記ステップ１で得られた穴埋めされたマーカ領域に対して、メディアンフィルタなどを施してノイズ成分を除去する（図７の（ｂ））。
【００７７】
（２）その領域内の濃淡値（明度）の平均値μg と標準偏差σg を算出する。
【００７８】
（３）領域内の各画素の濃度値ｇに対して、ある定められた実数ｔを利用し、
１）ｇ−μg ＞ｔσg ならば、その画素をラベル１
２）ｇ−μg ＜−ｔσg ならば、その画素をラベル−１
３）１）でも２）でもなければ、その画素をラベル０
でラベル化する。
【００７９】
こうして得られた領域内で、１，−１，０で表される小領域を抽出する（図７の（ｃ））。
【００８０】
（４）１または−１でラベル化された小領域で、マーカ領域の中央にもつとも近い小領域を抽出する。
【００８１】
この小領域をセンターパターンと命名し、そのセンターパターンと領域の１次モーメントｑ0 と２次モーメントＭを利用し、センターパターンからその他のパターンまでの正規化距離と正規化角度（楕円領域を円領域ヘ変換したときのパターン間角度）を算出する。
【００８２】
図７の（ｄ）は、このセンターパターンからその他のパターンまでの正規化距離と正規化角度を算出する様子を表している。
【００８３】
これらの正規化距離とパタ−ン間の正規化角度がある一定の幾何学的性質を保っていたときには、このマーカ候補領域は、マーカ領域として認識され、その小領域が作るパターンのコードを読みとることにより、マーカの同定を行うことができる。
【００８４】
図７の（ｄ）では、図３のコードパターンのコード２として認識されることになる。
【００８５】
このようにして同定されたマーカ領域は、その画像内でのセンターパターンの重心位置が、コードマーカの位置（ｕｉ，ｖｉ）（ｉ＝１，２，３，．．．）として登録される。
【００８６】
ステップ３：
ステップ２で同定されたマーカの画像内位置（ｕｉ，ｖｉ）（ｉ＝１，２，３，．．．）とオブジェクト座標系における３次元マーカ位置（ｘi m ，ｙi m ，ｚi m ）が与えられたとき、如何にして式（３）で表される斉次変換行列cHm 上を算出するが、このステップ３の課題である。
【００８７】
これは、基本的には、上述した文献１（Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ  ａｎｄ  Ｒ．Ｃ．Ｂｏｌｌｅｓ，“Ｒａｎｄｏｍ  ｓａｍｐｌｅ  ｃｏｎｓｅｎｓｕｓ：Ａ  ｐａｒａｄｉｇｍ  ｆｏｒ  ｍｏｄｅｌ  ｆｉｔｔｉｎｇ  ｗｉｔｈ  ａｐｐｌｉｃａｔｉｏｎｓ  ｔｏ  ｉｍａｇｅ  ａｎａｌｙｓｉｓ  ａｎｄ  ａｕｔｏｍａｔｅｄｃａｒｔｏｇｒａｐｈｙ，”Ｃｏｍｍｕｎｉｃａｔｉｏｎｓ  ｏｆｔｈｅ  ＡＣＭ，Ｖｏｌ．２４，Ｎｏ．６，Ｊｕｎｅ  １９８１，ｐｐ．３８１−３９５）で示される方法を以下に示すように変更しながら行う。
【００８８】
すなわち、この文献１で紹介されている方法では、同定されたマーカ群の中から一直線上にない任意の３個のマーカを選択し、その３個のマーカを利用してカメラ座標系とオブジェクト座標系間の座標変換パラメータ解の候補を算出するようにしている。
【００８９】
しかるに、その座標変換パラメータとしては、最大４種類の解が存在することがわかっているので、本発明では、その４種類の各解に対して、選択されなかつたマーカ群を利用して解の検証を行うことにより、解の絞り込みを行うとともに、その解を初期値として、すべてのマーカを利用して解の更新を行うようにしたものである。
【００９０】
以下、その方法に関して簡単に説明する。
【００９１】
同定されたマーカ群の中から、ある選択基準に従って画像内で一直線上にない３個のマーカを選択する。
【００９２】
この選択基準としては、
（１）カメラ画像面で３個のマーカを頂点とする３角形の面積が最大となる３個のマーカを選択する方法
（２）カメラ画像面で３個のマーカを頂点とする３角形の最大内角が最小である３個のマーカを選択する方法
などが考えられる。
【００９３】
このようにして得られたマーカをＭｉ（ｉ＝１，２，３）とする。
【００９４】
次に、これら３個のマーカＭｉ（そのモデル座標系における３次元位置をＰi 、（ｘi m ，ｙi m ，ｚi m ）、画像内位置をＱi （ｕi ，ｖi ）とする）に関して、図８に示されるような３個の３角形Δ０c ＭｉＭｊ（ｉ，ｊ＝１，２，３；ｉとｊは等しくない）を考える。
【００９５】
これらの３個の３角形に関して、カメラ画像系の原点０c から各マーカＭｉまでの距離をｄｉとし、マーカＭｉ，Ｍｊとカメラ座標系原点０c がなす角度θijとする。
【００９６】
また、マーカＭｉ，Ｍｊ間の距離をＲijとする。
【００９７】
このとき距離Ｒ12，Ｒ23，Ｒ31と角度θ12，θ23，θ31は既知の値となるが、ｄ１，ｄ２，ｄ３は未知の値となる。
【００９８】
逆に言えば、距離ｄ１，ｄ２，ｄ３を算出することができれば、オブジェクト座標系からカメラ座標系への座標変換パラメータを算出することができる。
【００９９】
以下この点に関して解説する。
【０１００】
（１）距離Ｒ12，Ｒ23，Ｒ31の算出方法
Ｒ12は点Ｐ1 と点Ｐ2 間のユークリッド距離として算出される。
【０１０１】
同様に、Ｒ23、Ｒ31はそれぞれ、点Ｐ2 とＰ3 、点Ｐ3 とＰ1 間のユークリッド距離として算出される。
【０１０２】
（２）角度θ12，θ23，θ31の算出方法
マーカＭｉ，Ｍｊとカメラ座標系の原点０ｃとがなす角度θijは以下のように算出することができる。まず、
【数６】
0004794708.tif 000007
で与えられる。
【０１０３】
このようにして、３個の角度をその余弦より算出することができる。
【０１０４】
（３）距離ｄｉ（ｉ＝１，２，３）の算出
３角形０ｃＭ１Ｍ２，０ｃＭ２Ｍ３，０ｃＭ３Ｍ１に対して第２余弦定理を適用すれば、
【数７】
0004794708.tif 000008
が導かれる。
【０１０５】
これらの３つの式において、未知数はｄ１，ｄ２，ｄ３の３個であり、制約式も３つであるので、理論的には上式を満たす解｛（ｄ１（ｋ），ｄ２（ｋ），ｄ３（ｋ））：ｋ＝１，２，３，４｝が存在する。
【０１０６】
その解法に関しては、上述した文献１（Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ  ａｎｄ  Ｒ．Ｃ．Ｂｏｌｌｅｓ，“Ｒａｎｄｏｍ  ｓａｍｐｌｅ  ｃｏｎｓｅｎｓｕｓ：Ａ  ｐａｒａｄｉｇｍ  ｆｏｒ  ｍｏｄｅｌ  ｆｉｔｔｉｎｇ  ｗｉｔｈ  ａｐｐｌｉｃａｔｉｏｎｓ  ｔｏ  ｉｍａｇｅ  ａｎａｌｙｓｉｓ  ａｎｄ  ａｕｔｏｍａｔｅｄ  ｃａｒｔｏｇｒａｐｈｙ，”Ｃｏｍｍｕｎｉｃａｔｉｏｎｓ  ｏｆ  ｔｈｅ  ＡＣＭ，Ｖｏｌ．２４，Ｎｏ．６，Ｊｕｎｅ  １９８１，ｐｐ．３８１−３９５）で詳しく述べられているように、この方程式には最大４個の解が存在することがわかっており、その解が４次方程式の解として数値解祈的に解くことが可能である。
【０１０７】
（４）解（ｄ１，ｄ２，ｄ３）の検証と最適解の選択
基本的には、最大４個の解の中で１個だけが正しい解を与えるものである。
【０１０８】
上記の解の中で、どの解が正しいかを検証するのが、このステップである。
【０１０９】
各解（ｄ１，ｄ２，ｄ３）について、カメラ座標系Ｃでのマーカ位置（ｘi c ，ｙi c ，ｚi c ）を算出する方法について解説する。
【０１１０】
カメラ座標系原点Ｃからマーカまでの距離がｄｉであり、その画像内位置が  （ｕi ，ｖi ）であり、
【数８】
0004794708.tif 000009
が成立し、
【数９】
0004794708.tif 000010
と書くことができる。
【０１１１】
いま、オブジェクト座標系でのマーカ位置を（ｘi m ，ｙi m ，ｚi m ）とすれば、オブジェクト座標系０ｍからカメラ座標系０ｃへの変換は、
【数１０】
0004794708.tif 000011
と書かれる。
【０１１２】
ここに、Ｒは回転行列を表し、ｔは並進ベクトルを表す。
【０１１３】
いま、両座標系でのマーカ群の重心ベクトルを
【数１１】
0004794708.tif 000012
が成立し、並進べクトルと回転行列を別々の式で算出することができる。
【０１１４】
ｉ＝１，２，３に対して、上記方程式を解く方法としては、ｑｕａｔｅｒｎｉｏｎ法（四元数法）がある。
【０１１５】
この方法の詳細は文献３（Ｂ．Ｋ．Ｐ．Ｈｏｒｎ，“Ｃｌｏｓｅｄ−ｆｏｒｍｓｏｌｕｔｉｏｎ  ｏｆ  ａｂｓｏ１ｕｔｅ  ｏｒｉｅｎｔａｔｉｏｎ  ｕｓｉｎｇ  ｕｎｉｔ  ｑｕａｔｅｒｎｉｏｎｓ，”Ｊｏｕｒｎａｌ  ｏｆ  Ｏｐｔｉｃａｌ  Ｓｏｃｉｅｔｙ  ｏｆ  Ａｍｅｒｉｃａ  Ａ，Ｖ０１．４，Ｎｏ．４，１９８７，ｐｐ．６２９−６４２．）に述べられているので、その詳細はここでは省略する。
【０１１６】
このようにＲ，ｔが算出されると、斉次変換行列ｃＨｍは式（１），（２）により計算することができる。
【０１１７】
以上のことを４個の解に対して繰り返すことにより、ｃＨｍ（１），ｃＨｍ  （２），ｃＨｍ（３），ｃＨｍ（４）の４個の解を算出することができる。
【０１１８】
さて、同定されたコードマーカのうち最初の選択されなかつたコードマーカをＭ４，Ｍ５，．．．，Ｍｍとする。
【０１１９】
次に、各斉次変換行列解ｃＨｍ（ｋ）（ｋ＝１，２，３，４）に対して、解として最もふさわしい解を、これらＭ４，Ｍ５，．．．，Ｍｍを利用して決定する方法を説明する。
【０１２０】
（１）各解ｃＨｍ（ｋ）に対する評画関数ｄｉｓｔ（ｋ）を最小にするｋを、以下のステップで算出する。
【０１２１】
（２）各解ｃＨｍ（ｋ）（ｋ＝１，２，３，４）に関して以下の方法により、ｄｉｓｔ（ｋ）を算出する。
【０１２２】
ａ）ｄｉｓｔ（ｋ）：＝０と評価関数を初期化する。
【０１２３】
ｂ）同定されたが最初の３個に選択されなかったマーカＭｊ（ｊ＝４，５，．．．，ｍ）について、そのオブジェクト座標系における３次元位置（ｘj m ，ｙj m ，ｚj m ）を、ｃＨｍ（ｋ）を利用してカメラ画像面に変換する。
【０１２４】
その投影画像点を（ｕj ´，ｖj ´）とする。これは、
【数１２】
0004794708.tif 000013
により算出することができる。
【０１２５】
続いて、マーカＭｊの実際に画像内で測定された２次元位置（ｕj ，ｖj ）と投影画像点（ｕj ´，ｖj ´）との２乗誤差ｅj を算出する。
【０１２６】
この２乗誤差ｅj は、以下のようにして算出することができる。
【０１２７】
ｅj ＝（ｕj ´−ｕj  )2 ＋（ｖj ´−ｖj ）2 
そして、ｄｉｓｔ（ｋ）は、
【数１３】
0004794708.tif 000014
で求められる。
【０１２８】
（３）以上のようにして求められたｄｉｓｔ（ｋ）が最小となる斉次変換行列の解ｃＨｍ（ｋ）を選択する。
【０１２９】
要約すると、上記のステップで求められる最適解ｃＨｍ（ｋ）は、コードマーカＭ１，Ｍ２，Ｍ３から生成される解のうち、他のマーカＭ４，Ｍ５，．．．，Ｍｍが最も支持する解を選択するものである。
【０１３０】
（５）解の更新
上記ステップ（４）で選択された解ｃＨｍ（ｋ）は、コードマーカＭ１，Ｍ２，Ｍ３から推定されたものであり、他のマーカＭ４，Ｍ５，．．．，Ｍｍに対する測定値を利用したものではない。
【０１３１】
そこで、本ステップ（５）では、上記ステップ（４）で算出された解ｃＨｍ  （ｋ）を初期推定値ｃＨｍ(0) として、すべてのコードマーカＭｉ（ｉ＝１，２，．．．，ｍ）により、この解の更新を行う。
【０１３２】
すなわち、ｃＨｍを角度成分（ｒｏｌｌ（φｚ）−ｐｉｔｃｈ（φｙ）−ｙａｗ（φｘ））と並進成分（ｔｘ，ｔｙ，ｔｚ）に展開して、６次元未知変数ｐ＝（φｘ，φｙ，φｚ；ｔｘ，ｔｙ，ｔｚ）とし、その初期推定値をｐ(0) ＝（φｘ(0) ，φｙ(0) ，φｚ(0) ；ｔｘ(0) ．ｔｙ(0) ，ｔｚ(0) ）と定義する。
【０１３３】
具体的には、
【数１４】
0004794708.tif 000015
により定義される。
【０１３４】
これをオブジェクト座標系でのマーカ３次元位置（ｘi m ，ｙi m ，ｚi m ）と、そのカメラ画像面での位置（ｕi ，ｖi ）の関係を利用しながら、６次元位置姿勢パラメータｐ＝（φｘ，φｙ，φｚ；ｔｘ，ｔｙ，ｔｚ）を更新することを考える。
【０１３５】
オブジェクト座標系でのマーカ３次元位置（ｘi m ，ｙi m ，ｚi m ）と、そのカメラ画像面での位置（ｕi ，ｖi ）の関係は
【数１５】
0004794708.tif 000016
により与えられる。
【０１３６】
この式を整理すると、各マーカＭｉ（ｉ＝１，２，．．．，ｍ）に関して、
【数１６】
0004794708.tif 000017
なる２次制約式によって表現され、６次元パラメータの初期推定値ｐ(0) ＝（φｘ(0) ，φｙ(0) ，φｚ(0) ；ｔｘ(0) ．ｔｙ(0) ，ｔｚ(0) ）を用いて、６次元パラメータｐ＝（φｘ，φｙ，φｚ；ｔｘ，ｔｙ，ｔｚ）を推定する問題となる。
【０１３７】
この問題はよく知られた非線形方程式問題であり、多くの文献がその解法を紹介しているので、ここではその詳細を述べない。
【０１３８】
このようにして６次元パラメータは、すべてのマーカの測定値を利用して更新され、オブジェクト座標系からカメラ座標系への座標変換パラメータが算出される。
【０１３９】
すなわち、対象物１と画像撮影装置３との間の位置関係を算出することができる。
【０１４０】
以上のような第１の実施の形態によれば、マーカ群の一部が遮蔽されて検出できなかった場合でも、検出されたマーカだけから対象物１と画像撮影装置３との間の３次元位置関係を算出することができる。
【０１４１】
また、マーカの検出に際しては、マーカ固有のコードを利用することにより、マーカ同定の信頼性が従来技術に比べて格段に向上させることができるので、より安定な位置姿勢計測を実現することができる。
【０１４２】
（第２の実施の形態）
次に、第２の実施の形態について説明する。
【０１４３】
上述した第１の実施の形態では、画像撮影装置３としてカラー画像を生成できるものを仮定し、その色情報を利用することにより、マーカ領域を抽出するようにしている。
【０１４４】
これに対し、本第２の実施の形態では、カラー画像を用いずにモノクロ画像を撮影する画像撮影装置３を利用し、マーカが独自に持つ幾何学的構造を画像内から抽出することにより、マーカ領域を抽出するようにしている。
【０１４５】
また、本第２の実施の形態では、抽出されたマーカ領域の領域の大きさの情報を利用してカメラ座標系からマーカまでの距離の初期推定値を算出することにより、マーカの数が３個のときでも、安定的に対象物１と画像撮影装置３間の３次元位置姿勢パラメータを算出することができるようにしている。
【０１４６】
上述した第１の実施の形態のようにカラー画像を利用する方式では、複雑な環境下で照明光が変化する場合には、単一色領域を抽出したり、単純な閾値処理によってマーカに対応する領域を正確に抽出することが困難な場合が多い。
【０１４７】
本実施の形態で説明する方式では、このような複雑な環境下でもロバストに３次元位置姿勢推定が行えるように、マーカが独自に持つ幾何学的関係を画像内から抽出することを行う。
【０１４８】
本実施の形態の基本的な構成は図１で示された構成と同様であり、その処理のステップは、図６で示された方法と同様であるが、ステップ１では、画像撮影装置３がカラー画像の代わりにモノクロ画像をコンピュータ４に送出する。
【０１４９】
また、ステップ２では、カラー画像の代わりにモノクロ画像からマーカ領域が抽出される点が異なる。
【０１５０】
また、ステップ３においては、マーカ領域からマーカを同定するのに必要となるコードが検出されるとともに、マーカ領域自体の大きさの情報が抽出される点が異なる。
【０１５１】
さらに、ステップ４においては、マーカ領域の大きさの情報も利用しながら、対象物１と画像撮影装置３との間の３次元位置姿勢パラメータが算出される。
【０１５２】
以下、その詳細を、ステップ２、３、４について解説する。
【０１５３】
なお、本実施の形態でも、コードマーカとしては、図３で示されるような円形マーカを想定して解説する。
【０１５４】
ステップ２：
図９は、第２の実施の形態におけるステップ２での処理の手順を示すフローチャートである。
【０１５５】
ステップ１１では、画像撮影装置３により送出されたモノクロ画像が、コンピュータ４内のメモリ領域に記憶された後、その画像配列Ｉ（ｕ，ｖ）に対して、メディアンフィルタなどの平滑化フィルタを適用し、マーカ領域内に存在する微細なテクスチャや画像に含まれるノイズ成分を除去する。
【０１５６】
続いて、ステップ１２では、平滑化された画像について領域分割アルゴリズムを適用し、画像を領域分割する。
【０１５７】
この領域分割アルゴリズムとしては、上述した文献３（Ｋ．Ｒａｈａｒｄｊａａｎｄ  Ａ．Ｋｏｓａｋａ“Ｖｉｓｉｏｎ−ｂａｓｅｄ  ｂｉｎ−ｐｉｃｋｉｎｇ：Ｒｅｃｏｇｎｉｔｉｏｎ  ａｎｄ  ｌｏｃａｌｉｚａｔｉｏｎ  ｏｆ  ｍｕｌｔｉｐｌｅ  ｃｏｍｐｌｅｘ  ｏｂｊｅｃｔｓ  ｕｓｉｎｇ  ｓｉｍｐｌｅｖｉｓｕａｌ  ｃｕｅｓ，”Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  １９９６  ＩＥＥＥ／ＲＳＪ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｒｏｂｏｔｓ  ａｎｄ  Ｓｙｓｔｅｍｓ，Ｏｓａｋａ，Ｊａｐａｎ，Ｎｏｖｅｍｂｅｒ  １９９６）に示されているＳｐｅｄｇｅ−ａｎｄ−Ｍｅｄｇｅ方法や、文献４（Ｔ．Ｐａｖｌｉｄｉｓ  ａｎｄ  Ｙ．Ｌｉｏｗ，“Ｉｎｔｅｇｒａｔｉｎｇ  ｒｅｇｉｏｎ  ｇｒｏｗｉｎｇ  ａｎｄ  ｅｄｇｅ  ｄｅｔｅｃｔｉｏｎ，”ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  Ｐａｔｔｅｒｎ  Ａｎａｌｙｓｉｓ  ａｎｄ  Ｍａｃｈｉｎｅ  Ｉｎｔｅｌｌｉｇｅｎｃｅ，Ｖｏｌ．１２，Ｎｏ．３，ｐｐ．２２５−２３３，１９９０）に示されるＳｐｌｉｔ一ａｎｄ−Ｍｅｒｇｅ方法や、Ｃａｎｎｙエッジ抽出方法で抽出したエッジ成分を連結して領域分割する方法など、どのような方法でもよい。
【０１５８】
次に、ステップ１３では、このようにして分割される各領域に関して、その領域の幾何学的特徴量を計算する。
【０１５９】
例えば、その領域ｋに対して『面槓Ａ（ｋ）』、『大きさＬ（ｋ）』、『濃度の平均値ｍ（ｋ）』、『濃度の標準偏差ｓ（ｋ）』などを算出する。
【０１６０】
これらの特徴量が、マーカとして取りうる値として妥当であるか否かを閾値処理により判定する。
【０１６１】
具体的には、上記各特徴量が、あらかじめ定められた範囲内の値であるか否かを見ればよい。
【０１６２】
例えば、上記各特徴量が、面積Ａ（ｋ）、大きさＬ（ｋ）、濃度の平均値ｍ  （ｋ）、濃度の標準偏差ｓ（ｋ）の場合には、
【数１７】
0004794708.tif 000018
をすべて満たしているか否かを判定条件とすればよい。
【０１６３】
ここで、Ａｍｉｎ，Ａｍａｘなどの値は、対象物１の大きさ、コ一ドマーカ２の大きさ・色、画像撮影装置３と対象物１の距離の上限と下限などを考慮することにより、あらかじめ設定できるものである。
【０１６４】
このようにして分割された領域の中からマーカに対応すると思われる領域の１次候補が選択される。
【０１６５】
続いて、ステップ１４では、第１次選択で候補とされた領域に関して、その領域がコードマーカ領域として妥当であるか否かを詳細に判定する。
【０１６６】
この場合、基本的にコードマーカの外形は、図３に示されるように円形であるので、その画像内での投影像は楕円で近似することができる。
【０１６７】
そこで、第１次選択で候補とされた領域が、楕円であるか否かを詳細に判定する。
【０１６８】
この判定方法については、第１の実施の形態の場合と同様であるので、ここでは省略する。
【０１６９】
これでステップ２は終了する。
【０１７０】
ステップ３：
このステップ３において、第１の実施の形態の場合と原画像がカラー画像ではなく、モノクロ画像であることが異なるが、その処理の方法は第１の実施形態の場合と同様であるので、ここではその解説を省略する。
【０１７１】
ステップ４：
ステップ３で、コードマーカが同定できた後、ステップ４へ引き継がれる情報は、各コードマーカに対して、オブジェクト座標系におけるコードマーカの３次元位置（ｘi m ，ｙi m ，ｚi m ）、カメラ画像面における位置（ｕi ，ｖi ）、コードマーカの画像内における楕円近似に基づく長軸の長さｒi である。
【０１７２】
本ステップでは、コードマーカの画像内における楕円近似の長軸の長さｒi を利用して、カメラ座標系から各コードマーカまでの距離ｄｉの初期推定値を算出し、その初期推定値を有効に利用することで対象物１と画像撮影装置３との間の３次元位置関係を計測する方法を解説する。
【０１７３】
（１）画像撮影装置３からマーカ２までの距離の初期推定値の算出
以上のようにしてマーカが同定できた場合には、そのマーカ２と画像撮影装置３との間の距離の初期推定値を算出する。
【０１７４】
以下この方法を説明する。
【０１７５】
図１０に示すように、マーカの中心をＰi 、カメラの焦点を０c とするとともに、カメラ像面と０c Ｐi の交点をＱi とすると、Ｑi はマーカ像の中心であると判断することができる。
【０１７６】
また、マーカの３次元モデルを半径Ｒi の球で近似し、マーカの像を楕円で近似するとき、その画像内の楕円の長軸の長さをｒi とする。
【０１７７】
【数１８】
0004794708.tif 000019
ここに、θi はカメラの光軸と０Ｐi がなす角度であり、ｚi はＰi のカメラ座標系におけるｚ値である。
【０１７８】
そして、ｚi とｄi の間には、以下の関係が成立するから、
【数１９】
0004794708.tif 000020
と表現することができる。
【０１７９】
（２）対象物１と画像撮影装置３間の３次元位置姿勢推定
カメラ座標系０c でのマーカ位置（ｘi c ，ｙi c ，ｚi c ）を算出する方法についてまず解説し、続いてオブジェクト座標系とカメラ座標系間の座標変換パラメータの算出について解説する。
【０１８０】
各マーカｉとマーカｊに対して、その３次元的距離をＲｉｊ、カメラ視点０c （焦点位置）とＱｉ，Ｑｊがなす角度をθｉｊとすると、
【数２０】
0004794708.tif 000021
で与えられる。
【０１８１】
各マーカの基準座標系での３次元位置は確定しているので、マーカ間の距離もあらかじめ確定している。
【０１８２】
いま、この距離をａijとすれば、三角形の余弦定埋から
【数２１】
0004794708.tif 000022
が成立しなければならない。
【０１８３】
従って、前ステップで求められたｄｉの推定値とｄｉの誤差分散ならびにｃｏｓ（θｉｊ）の誤差分散を利用することにより、ｄｉの初期値を更新することができる。
【０１８４】
このようにｄｉの初期値を更新する方法としては、
（１）ニュートン法を利用する方法
（２）準ニュートン法を利用する方法
（３）カルマンフィルタを利用する方法
など多くの方法が挙げられる。
【０１８５】
本実施の形態では（３）のカルマンフィルタを利用して解を算出する方法について述べる。
【０１８６】
上記で説明されている距離ｄｉ（ｉ＝１，２，．．．，ｎ）を変数とするべクトルｐを定義し、その初期分散行列をＳ、すなわち
【数２２】
0004794708.tif 000023
で示される繰り返し演算を行うことにより、求めるべきべクトルｐを更新することができる。
【０１８７】
このｐはカメラ座標系の原点からマーカ位置までの距離に対応する。
【０１８８】
この距離が推定できると、第１の実施の形態で説明した方法と同様にして、オブジェクト座標系からカメラ座標系への座標変換パラメータを算出することができる。
【０１８９】
以上、第２の実施の形態によれば、原画像としてカラー画像を利用する必要がないため、より安価なシステムを構成することができるとともに、最低３個のコードマーカを使用する場合でも、対象物と画像撮影装置間の３次元位置関係を計測することができる。
【０１９０】
（第３の実施の形態）
次に、第３の実施の形態について説明する。
【０１９１】
上記第１及び第２の実施の形態においては、原画像からマーカ領域と思われる領域を抽出するのに、原画像と同じサイズの画像を利用するようにしている。
【０１９２】
しかるに、本発明では、基本的にマーカに対応する領域は、円形や多角形などの単純な形状をしており、その内部背景は均一な色などで構成されていることから、原画像と同じサイズの画像を処理しなくてもマーカ領域と想定される領域を抽出することが可能である。
【０１９３】
そこで、本実施の形態では、原画像をいったん縮小し、縮小された画像に対して、第１または第２の実施の形態において、マーカ領域を抽出するとともに、縮小画像内で抽出されたマーカの位置を利用して、原画像でのマーカ領域の位置を推定するとともに、原画像サイズでのマーカ領域内で、コードマーカの同定を行うことにより、原画像サイズでのマーカ領域抽出に関わる処理時間を短縮することができる手法を説明するものである。
【０１９４】
説明の関係上、本実施の形態では、第２の実施の形態で説明した方法に関して、縮小を生成して処理を行う場合について説明する。
【０１９５】
しかしながら、第１の実施の形態で説明した方法への適用も容易であることは言うまでもない。
【０１９６】
本実施の形態では、画像の大きさを原画像から１／１６に縮小した場合について、説明する。
【０１９７】
基本的には、図６のステップ１におけるマーカ領域の抽出を縮小画像で行うものである。
【０１９８】
サブステップ１：
原画像を１／１６に縮小する（行方向に１／４、列方向に１／４とする）。
【０１９９】
いま、原画像の画像配列を（ｉ，ｊ）とし、縮小画像の画像配列を（ｉｓ，ｊｓ）とすると、ｉ＝４＊ｉｓ＋０，ｉ＝４＊ｉｓ＋１，ｉ＝４＊ｉｓ＋２，ｉ＝４＊ｉｓ＋３の各Ｉに対して、かつｊ＝４＊ｊｓ＋０，ｊ＝４＊ｊｓ＋１，ｊ＝４＊ｊｓ＋２，ｊ＝４＊ｊｓ＋３に対するＪにおける１６画素の平均値を（ｉｓ，ｊｓ）の画素値とすることで、縮小画像を作成する。
【０２００】
サブステップ２：
サブステップ１で作成された縮小画像に対して、マーカに対応すると予想される領域を抽出する。
【０２０１】
この抽出方法は、第１の実施の形態または第２の実施の形態で述べられた方法と同様である。
【０２０２】
サブステップ３：
サブステップ２で抽出された領域の位置座標（ｉｓ，ｊｓ）を４倍ずつすることにより、原画像におけるマーカ領域を推定する。
【０２０３】
以上のような第３の実施の形態で説明した手法をとることにより、原画像の大きさに比べて１／１６の大きさの縮小画像を処理することで、マーカと想定される領域を抽出することができるので、全体の処理を高速化することができる。
【０２０４】
（第４の実施の形態）
次に、第４の実施の形態について説明する。
【０２０５】
この第４の実施の形態では、コードマーカ以外のランドマーク点を利用して、位置姿勢推定の精度を向上する方法について説明する。
【０２０６】
これまでに解説した第１乃至第３の実施の形態による方法では、コードマーカと呼ばれるマーカの３次元位置モデルとその画像への投影像の２次元位置との関係のみを利用して、コードマーカが規定する対象物と画像撮影装置間の３次元位置姿勢を推定するようにしている。
【０２０７】
このような方法は、コードマーカを多数利用することができる場合、または計測精度があまり必要ないときにはよい。
【０２０８】
しかしながら、高精度の位置姿勢推定が要求される場合には、より多くのマーカ３次元モデル特徴と画像内での観測点の対応関係が必要となる。
【０２０９】
以下に述べる第４の実施の形態による手法では、コードマーカ以外のモデル特徴を利用し、該モデル特徴と観測された画像特徴の対応関係を増加させることにより、より高精度の位置姿勢推定を実現するものである。
【０２１０】
以下、このようなモデル特徴のことをランドマークと呼ぶことにし、そのランドマークをどのように画像内から抽出し、かつ抽出されたランドマークを利用して、どのように対象物１の位置姿勢を推定するかについて、幾つかの実施例に基づいて解説する。
【０２１１】
（実施例１）
まず、実施例１として、コードマーカ位置以外のモデル特徴として、すでに認識された近接するコードマーカの位置を利用してモデル特徴の画像内位置を利用し、コードマーカ以外のモデル特徴の位置を推定するとともに、その推定された位置付近の画像特徴を抽出し、モデル特徴と画像特徴間の対応関係を利用して、対象物の位置姿勢を推定する手法を説明する。
【０２１２】
いま、オブジェクト座標系で、コードマーカ０，１，．．．，ｍ−１が位置べクトルｐ０，ｐ１，．．．，ｐｍ−１で表され、ランドマークｋの位置べクトルｐk が、ｐ０，ｐ１，．．，ｐｍ−１で線形結合で表されるとすると（ｂ０，ｂ１，．．．，ｂｍ−１は定数）、
【数２３】
0004794708.tif 000024
その画像内での位置ｑk ＝（ｕk ，ｖk ）は、コードマーカの画像内計測値ｑ0 ＝（ｕ0 ，ｖ0 “），ｑ1 ＝（ｕ1 ，ｖ1 ），ｑm-1 ＝（ｕm-1 ，ｖm-1 ）を利用して
【数２４】
0004794708.tif 000025
で予測することができる。
【０２１３】
この予測値を利用し、ｑk の近傍で最も確からしいランドマーク画像内から抽出することになる。
【０２１４】
図１１は、画像内からランドマークを抽出する一例を示しており、ここでは、この図１１にあるような例についてを考える。
【０２１５】
この例では、コードマーカ０，１，２があらかじめ抽出されていると仮定し、それら３個のコードマーカのちようど重心の位置に円形のランドマークが配置されている場合を考える。
【０２１６】
すると、画像内で抽出されたコ一ドマーカ０，１，２の画像内位置の重心を取り、その重心付近である適当な大きさのウィンドウを設定し、そのウィンドウ領域内にある円形の領域を閾値法で抽出することにより、ランドマークｋの抽出を行うことができる。
【０２１７】
このようにしてランドマークｋに対応する領域が抽出されると、その領域の重心位置が、ランドマークｋの位置として登録される。
【０２１８】
このようにして、コードマーカ群とランドマーク群が抽出され、画像内での位置が算出されると、第１の実施の形態で述べたような位置推定方法で対象物１の位置姿勢を推定することができる。
【０２１９】
また、別の方法としては、第２の実施の形態で説明したように、まず、最初にコードマーカ群を利用して対象物位置姿勢を推定し、対象物１の初期推定値を算出したのち、第１の実施の形態の解の更新の方法で述べたような方法により、ランドマーク群の画像内位置とオブジェクト座標系における位置を利用することで、対象物１の位置姿勢の推定値を更新することができる。
【０２２０】
（実施例２）
ここで述べる実施例２の手法では、あらかじめ抽出されたコードマーカを利用して、対象物１の３次元位置姿勢の初期推定値を算出するとともに、その初期推定値を利用して、ランドマークの画像内でのあるべき予測位置の推定値を算出し、その推定位置付近で、ランドマークを探索する。
【０２２１】
そして、ランドマークが同定できた場合には、そのランドマークのオブジェクト座標系での３次元位置と画像内での２次元位置を利用して、対象物１の３次元位置姿勢の推定値を更新するものである。
【０２２２】
以下、その方法について説明する。
【０２２３】
いま、画像内からコードマーカ０，１，．．．，ｍ−１が同定されたと仮定すると、これらのオブジェク卜座標系での３次元位置座標を画像内で計測された２次元座標を利用して、対象物１の３次元位置姿勢を推定することができる。
【０２２４】
この方法は、第１あるいは第２の実施の形態で示した方法と同様である。
【０２２５】
いま、この３次元位置姿勢の初期推定値をｃＨｍとする。
【０２２６】
また、ランドマークｋのオブジェクト座標系における位置を（ｘｋ，ｙｋ，ｚｋ）とすると、ランドマークｋの画像内における予測値（ｕｋ，ｖｋ）は式（３），（４）により算出することができる。
【０２２７】
この予測値（ｕｋ，ｖｋ）の周辺で、画像内からランドマークを抽出・同定することができる。
【０２２８】
このようにして、各ランドマークを抽出・同定できた場合には、実施例２で説明したのと同様にして、対象物１の３次元位置姿勢の推定値を更新することができる。
【０２２９】
以上、述べたような手法を利用すれば、コードマーカ以外のランドマークを利用して、対象物の３次元位置姿勢を推定することが可能となり、よりロバストで正確な位置姿勢推定を行うことがてきる。
【０２３０】
また、この手法により、あらかじめ登録しておくべきコードマーカの数を減少させることもできる。
【０２３１】
（第５の実施の形態）
この第５の実施の形態では、画像撮影装置以外の装置と対象物間の３次元位置姿勢を推定する手法を説明する。
【０２３２】
これまでに説明してきた第１乃至４の実施の形態は、いずれも、対象物１と画像撮影装置３間の位置関係を計測する例である。
【０２３３】
しかるに、より実際的な例の場合として、図１に示される画像撮影装置３とコンピュータ４は、あるシステムにおいて、対象物１の位置姿勢を推定する位置センサとして利用される場合がある。
【０２３４】
この際には、画像撮影装置３とコンピュータ４からなる位置センサを含むシステム内に別の装置があり、その別の装置が規定する座標系をシステムが基準とするシステム座標系と考えることが多い。
【０２３５】
図１２は、第５の実施の形態で想定される構成を表したブロック図である。
【０２３６】
すなわち、図１２に示すように、コードマーカ１２２が装着された対象物１２１を撮影する画像撮影装置１２３が画像データをデータ処理装置であるコンピュータ１２４に送出し、コンピュータ１２４はその画像データを解析することにより、対象物１２１が規定するオブジェクト座標系から画像撮影装置１２３が規定するカメラ座標系への座標変換パラメータを算出する。
【０２３７】
一方、コンピュータ１２４は、画像撮影装置１２３が規定するカメラ座標系から基準座標系を規定する装置１２５が規定する基準座標系への座標変換パラメータをあらかじめ格納している。
【０２３８】
その座標変換パラメータを利用して、コンピュータ１２４は、オブジェクト座標系から該基準座標系への座標変換パラメータを算出する。
【０２３９】
いま、基準座標系を規定する装置１２５に対して、画像撮影装置１２３がキャリブレーションできていると仮定する。
【０２４０】
より具体的には、画像撮影装置１２３が規定するカメラ座標系０ｃと基準座標系０ｒの間の関係が斉次変換行列ｒＨｃ（カメラ座標系から基準座標系への斉次変換行列）で決定されていると仮定する。
【０２４１】
コンピュータ１２４が画像からマーカ群を同定し、カメラ座標系での対象物１２１の３次元位置姿勢を推定したとすると、オブジェクト座標系からカメラ座標系への座標変換パラメータを算出することができ、それはｃＨｍで表すことができる。
【０２４２】
これら２個の斉次変換行列から、オブジェクト座標系から基準座標系への斉次変換行列を、以下のように算出することができる。
【０２４３】
【数２５】
0004794708.tif 000026
このことにより、基準座標系における対象物１２１の３次元位置姿勢を推定することができる。
【０２４４】
本第５の実施の形態によりわかるように、本発明を別の装置のための位置姿勢センサとして利用することも可能である。
【０２４５】
（第６の実施の形態）
この第６の実施の形態では、本発明をセンサブローブ（ワイヤレスセンサプローブ）について適用する場合について説明する。
【０２４６】
近年、３次元オブジェクトの３次元点を計測するために、センサプローブが多く開発されるようになってきている。
【０２４７】
その手法としては、Ｆｌａｓｈ  Ｐｏｉｎｔのような光学的センサを利用する方法、磁気センサを利用する方法などがある。
【０２４８】
Ｆｌａｓｈ  Ｐｏｉｎｔのような発光ダイオードを利用する光学的センサの場合には、高精度で計測できることはあるが、センサプローブと装置間をワイヤで接続する必要があるので、操作性の問題がある。
【０２４９】
一方、磁気センサの場合には、ワイヤレスでの計測が可能であるが、金属の器具などが周辺にあるとノイズの影響が大であるという問題点がある。
【０２５０】
本実施の形態は、これらの問題点を解決すべく発明されたワイヤレスでかつ電磁波などの影響を受けないセンサプローブに適用した場合である。
【０２５１】
具体的には、センサプローブ自体に第１の実施の形態または第２の実施の形態で説明したようなコードマーカを装着することにより、画像撮像装置からセンサプローブまでの位置姿勢を推定し、センサプローブが探針する位置を推定するものである。
【０２５２】
以下、具体的に、このセンサプローブに適用した場合について解説する。
【０２５３】
図１３は、この第６の実施の形態による構成を表すブロック図である。
【０２５４】
すなわち、図１３に示すように、探針されるべき物体Ｘ１３９をセンサプローブ１３８で探針する。
【０２５５】
そのセンサプローブ１３８には、これまでの実施の形態で説明してきたようなコードマーカ（２，１２２）が装着されており、そのコードマーカを画像撮影装置１３３が撮影する。
【０２５６】
そして、画像撮影装置１３３で撮影された画像データは、コンピュータ１３４に送信される。
【０２５７】
コンピュータ１３４は、画像撮影装置１３３から送信される画像データに基づいて画像解析することにより、まず、画像撮影装置１３３に対するセンサプローブ１３８の位置姿勢パラメータを計測する。
【０２５８】
この場合、センサプロープ１３８が基準とすべき座標系は、コンピュータ１３４に接続された基準座標系を規定する装置１３６により規定されており、コンピュータ１３４はその基準座標系に合致した探針点の３次元位置データ１３７を該基準座標系を規定する装置１３６に送出する。
【０２５９】
図１４は、センサプローブ１３８の例を表したものである。
【０２６０】
この図１４において、センサプローブには、その先端に探針すべき針がついており、その針の先端が、第１または第２の実施の形態で述べられたようなオブジェク座標系の原点として定義される。
【０２６１】
そして、それを原点としてオブジェクト座標系を規定するＸm 軸、Ｙm 軸、Ｚm 軸の３軸が定義される。
【０２６２】
また、このセンサプローブには、第１の実施の形態や第２の実施の形態で解説したような複数のコードマーカが装着されている。
【０２６３】
そのコードマーカの位置はオブジェクト座標系上であらかじめ確定されていると仮定し、その３次元座標を（ｘi m ，ｙi m ，ｚi m ）とする。
【０２６４】
このとき画像撮影装置１３３で、このコードマーカが撮影され、その画像がコンピュータ１３４で解析されることにより、オブジェクト座標系から画像撮影装置１３３が規定するカメラ座標系への座標変換パラメータを算出することができる。
【０２６５】
すなわち、オブジェクト座標系から画像撮影装置１３３が規定するカメラ座標系への斉次変換行列を算出することができる。すなわち、
【数２６】
0004794708.tif 000027
で表現されるｃＨｍを算出することができる。
【０２６６】
さて、センサプローブ１３８の針の先端がオブジェクト座標系の原点と一致することから、（ｘi m ，ｙi m ，ｚi m ）＝（０，０，０）を代入すると、針先端のカメラ座標系における３次元位置を算出することができる。
【０２６７】
その値は並進ベクトルｔそのものの値となる。
【０２６８】
画像撮影装置１３３がある基準座標系Ｒにおいて、キャリブレーションされている場合を考える。
【０２６９】
すなわち、図１３で示されたように、他の装置１３６の座標系で画像撮影装置１３３が規定されている場合を考える。
【０２７０】
この場合には、画像撮影装置１３３から基準座標系への斉次変換行列ｒＨｃがあらかじめ較正されていると考えることができる。
【０２７１】
従って、基準座標系におけるセンサプローブ１３８の先端の３次元座標（ｘi r ，ｙi r ，ｚi r ）は、
【数２７】
0004794708.tif 000028
で表されることになる。
【０２７２】
これにより、本センサプローブは基準座標系における探針点の３次元位置を提供することができる。
【０２７３】
そして、本実施の形態によれば、従来技術では実現できなかったワイヤレスのセンサプローブを実現することができることになる。
【０２７４】
（第７の実施の形態）
この第７の実施の形態では、ステレオカメラを利用するようにした場合について説明する。
【０２７５】
これまで説明してきた実施の形態は、画像撮影装置が撮影する画像１枚を使用して対象物と画像撮影装置間の位置関係を計測する場合についてである。
【０２７６】
本実施の形態では、画像撮影装置が複数あり、それらと対象物間の位置関係を計測する方法について解説する。
【０２７７】
本方式によれば、検出されるべきコードマーカの数が３個の場合でも、安定的に位置姿勢推定が可能となる。
【０２７８】
基本的に、本実施の形態では、画像撮影装置間のキャリブレーション（画像撮影装置間の相対的位置が確定していること）があらかじめなされている場合に関してのみ、解説するものとする。
【０２７９】
また、画像撮影装置としては２台の場合のみを説明するが、２台を越える複数台の画像撮影装置への拡張は容易である。
【０２８０】
図１５は、本実施の形態による方式の概念図を表したブロック図である。
【０２８１】
すなわち、図１５に示すように、あらかじめ相対的位置が確定した複数（但し、この例ではステレオカメラを利用する例として左右の２台）の画像撮影装置２０３，２０４からの撮影画像データがデータ処理装置であるコンピュータ２０５ヘ送出される。
【０２８２】
コンピュータ２０５は、あらかじめオブジェクト座標系における位置が確定しているコードマーカ（２０２）位置を利用することにより、各画像撮影装置２０３，２０４と対象物２０１あるいは一部の画像撮影装置２０３または２０４と対象物２０１との間の３次元位置関係を計測する。
【０２８３】
本実施の形態においては、複数の画像撮影装置を有するため、複数の画像撮影装置は、全撮影装置は基準とするセンサ基準座標系に基づいてキャリブレーションされていると仮定する。
【０２８４】
すわなち、各画像撮影装置ｊに関して、センサ基準座標系で規定される３次元点（ｘi s ，ｙi s ，ｚi s ）が画像撮影装置ｊにおいて、画像位置（ｕi j ，ｖi j ）で観測されたとすると、すでにキャリブレーションにより確定している斉次変換行列ｊＨｓを利用して、
【数２８】
0004794708.tif 000029
と記述することができる。
【０２８５】
ここに、αM j ，αM j ，ｕ0 j ，ｖ0 j は、画像撮影装置ｊに関するカメラ内部パラメータであり、カメラキャリブレーションにより確定した値である。
【０２８６】
もし、オブジェクト座標系で規定される３次元点（ｘi m ，ｙi m ，ｚi m ）を考えると、その画像撮影装置ｊでの位置（ｕi j ，ｖi j ）は、以下の式により記述できる。
【０２８７】
【数２９】
0004794708.tif 000030
そこで、本実施の形態で解決すべき問題は、上記の式で表現されているオブジェクト座標系からセンサ基準座標系への座標変換パラメータｓＨｍを如何にして推定するかと言う問題である。
【０２８８】
図１６は、本実施の形態の処理手順を表したフローチャートである。
【０２８９】
本方式においては、左右画像を左右の画像撮影装置から受け取り、各画像内からコードマーカを同定し、その画像内位置を算出する（ステップＳ１１，Ｓ１２，Ｓ１３）。
【０２９０】
左右画像の両方から同定できたコードマーカに関して、そのコードマーカのセンサ基準座標系での３次元位置を左右画像内位置から算出する。
【０２９１】
こうしてセンサ基準座標系で算出された３次元位置座標とオブジェクト座標系で規定された３次元位置座標とを利用することにより、オブジェクト座標系からセンサ基準座標系への座標変換パラメータを算出する（ステップＳ１４）。
【０２９２】
各画像からコードマーカを同定するステップ（Ｓ１１，Ｓ１２，Ｓ１３）までは、第１の実施の形態や第２の実施の形態の場合と同様であるので、ここではステップＳ１４において、左右の画像の両者から同定されたコードマーカの画像内位置座標から、どのようにしてオブジェクト座標系からセンサ基準座標系への座標変換パラメータを算出するかを説明する。
【０２９３】
いま、左画像から得られたコードマーカｉの画像内位置座標を（ｕi 1 ，ｖi 1 ）、右画像から得られたコードマーカｉの画像内位置座標（ｕi 2 ，ｖi 2 ）とする。
【０２９４】
このとき、センサ基準座標系での３次元推定値は、以下のようにして算出することができる。
【０２９５】
【数３０】
0004794708.tif 000031
を定義したのち、（ｕi 1 ，ｖi 1 ）と（ｕi 2 ，ｖi 2 ）を正規化し、
【数３１】
0004794708.tif 000032
を算出する。すると、式（１００）から、
【数３２】
0004794708.tif 000033
が得られる。ここで
【数３３】
0004794708.tif 000034
とおけば、
【数３４】
0004794708.tif 000035
として得られる。
【０２９６】
この式により得られた３次元位置姿勢推定値（ｘi s ，ｙi s ，ｚi s ）とオブジェクト座標系内での３次元位置（ｘi m ，ｙi m ，ｚi m ）は、以下の回転行列Ｒと並進べクトルｔにより関連づけられ、第１の実施の形態または第２の実施の形態で説明したような、ｑｕａｔｅｒｎｉｏｎ法によりその回転行列Ｒと並進べクトルｔを求めることができる。
【０２９７】
また、このステレオ法では、検出されるべきコードマーカの数は最低で３個あればよい。
【０２９８】
これにより、オブジェクト座標系からセンサ基準座標系への座標変換パラメータｓＨｍを算出することが可能となる。ここに
【数３５】
0004794708.tif 000036
が成立する。
【０２９９】
本実施の形態によれば、互いにキャリブレーションされた複数の画像撮影装置を利用することにより、コードマーカの数が少ない場合でも、安定した位置姿勢センシング機能を実現することができる。
【０３００】
本実施の形態は、単一の画像撮影装置を利用したに比べて２倍のコードマーカを利用して位置姿勢を推定することと等価となり、遮蔽などの影響で数少ないマーカしか検出できない場合や、複雑な環境下で、画像撮影装置が撮影する画像内にノイズ成分が多く含まれる場合には、特に有効な方法となる。
【０３０１】
そして、上述したような第１乃至第７の実施の形態で示した本発明の明細書には、特許請求の範囲に示した請求項１乃至３以外に、以下のような付記１．乃至３６．として示すような発明が含まれている。
【０３０２】
付記１．撮像装置により撮像された、測定対象物に対する３次元位置情報が既知の少なくとも３個のマーカが写っている画像を入力する画像入力手段と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出手段と、
上記抽出した領域において、マーカの外観上の特徴から個々のマーカを特定するマーカ特定手段と、
上記推定されたマーカの画像上での大きさから、マーカまでの距離を推定する距離推定手段と、
上記推定された各マーカまでの距離と、このマーカの上記画像上での位置と、このマーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算する位置姿勢演算手段と、
を有することを特徴とする３次元位置姿勢センシング装置。
【０３０３】
付記２．撮像装置により撮像された、測定対象物に対する３次元位置情報が既知のマーカが写っている画像を入力する画像入力手段と、
上記入力された画像を縮小する画像縮小手段と、
上記縮小された画像上で上記各マーカに対応する領域を抽出する領域抽出手段と、
上記抽出された各マーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算する位置姿勢演算手段と、
を有することを特徴とする３次元位置姿勢センシング装置。
【０３０４】
付記３．上記測定対象物の特徴部分を画像より抽出する特徴部分抽出手段と、上記抽出した特徴部分の画像上の位置より、上記演算された測定対象物の位置姿勢を補正する手段をさらに含むことを特徴とする付記１または付記２記載の３次元位置センシング装置。
【０３０５】
付記４．画像を解析して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定する方法であり、
撮像装置により撮像された、測定対象物に対する３次元位置情報が既知の少なくとも３個のマーカが写っている画像を入力する画像入力工程と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出工程と、
上記抽出した領域において、マーカの外観上の特徴から個々のマーカを特定するマーカ特定工程と、
上記特定された各マーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算する位置姿勢演算工程と、
を有することを特徴とする３次元位置姿勢センシング方法。
【０３０６】
付記５．画像を解祈して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定する方法であり、
撮像装置により撮像された、測定対象物に対する３次元位置情報が既知の少なくとも４個のマーカが写つている画像を入力する画像入力工程と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出工程と、
上記抽出した領域において、マーカの外観上の特徴から個々のマーカを特定するマーカ特定工程と、
上記マーカ特定工程によつて特定されたマーカより３つのマーカを選択するマーカ選択工程と、
上記マーカ選択工程によつて選択された３つのマーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算するためのパラメータ組を複数算出するパラメータ組算出工程と、
上記マーカ選択工程によって選択されなかったマーカに対して、上記パラメータ組算出工程によつて算出されたパラメータ組を適用評価することにより、１つのパラメータ組を選択するパラメータ組選択工程と、
を有することを特徴とする３次元位置姿勢センシング方法。
【０３０７】
付記６．前記パラメータ組選択工程によつて選択された１つのパラメータ組に関して、各マーカの測定対象物に対する３次元位置情報を適用して評価することにより、該パラメータ組を改善するパラメータ改善工程をさらに含むことを特徴とする付記５記載の３次元位置姿勢センシング方法。
【０３０８】
付記７．画像を解析して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定する方法であり、
撮像装置により撮像された、測定対象物に対する３次元位置情報が既知の少なくとも３個のマーカが写っている画像を入力する画像入力工程と、
上記画像上で上記各マーカに対応する領域を抽出する領域抽出工程と、
上記抽出した領域において、マーカの外観上の特徴から個々のマーカを特定するマーカ特定工程と、
上記特定されたマーカの両像上での大きさから、マーカまでの距離を推定する距離推定工程と、
上記推定された各マーカまでの距離と、このマーカの上記画像上での位置と、このマーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算する位置姿勢演算工程と、
を有することを特徴とする３次元位置姿勢センシング方法。
【０３０９】
付記８．画像を解析して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定する方法であり、
撮像装置により撮像された、測定対象物に対する３次元位置情報が既知のマーカが写っている画像を入力する画像入力工程と、
上記入力された画像を縮小する画像縮小工程と、
上記縮小された画像上で上記各マーカに対応する領域を抽出する領域抽出工程と、
上記抽出された各マーカの上記両像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算する位置姿勢演算工程と、
を有することを特徴とする３次元位置姿勢センシング方法。
【０３１０】
付記９．上記測定対象物の特徴部分を画像より抽出する特徴部分抽出工程と、上記抽出した特徴部分の画像上の位置より、上記演算された測定対象物の位置姿勢を補正する工程をさらに含むことを特徴とする付記７または付記８記載の３次元位置姿勢センシング方法。
【０３１１】
付記１０．コンピュータによって画像を解析して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定するための処理プログラムを記録した記録媒体であり、上記処理プログラムはコンピュータに、撮像装置により撮像された、測定対象物に対する３次元位置情報が既知の少なくとも３個のマーカが写っている画像を人力させ、
上記画像上で上記各マーカに対応する領域を抽出させ、
上記抽出した領域において、マーカの外観上の特徴から個々のマーカを特定させ、
上記特定された各マーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算させる、
ことを特徴とする３次元位置姿勢センシング処理プログラムを記録した記録媒体。
【０３１２】
付記１１．コンピュータによって画像を解析して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定するための処理プログラムを記録した記録媒体であり、
上記処理プログラムはコンピュータに、撮像装置により撮像された、測定対象物に対する３次元位置情報が既知の少なくとも４個のマーカが写っている両像を入力させ、
上記画像上で上記各マーカに対応する領域を抽出させ、上記抽出した領域において、マーカの外観上の特徴から個々のマーカを特定させ、上記特定されたマーカより３つのマーカを選択させ、
上記選択された３つのマーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算するためのパラメータ組を複数算出させ、
上記選択されなかったマーカに対して、上記算出されたパラメータ組を適用評価することにより、１つのパラメータ組を選択させる、
ことを特徴とする３次元位置姿勢センシングブログラムを記録した記録媒体。
付記１２．上記処理プログラムは、コンピュータにさらに、前記選択された１つのパラメータ組に関して、各マーカの測定対象物に対する３次元位置情報を適用して評価することにより、該パラメータ組を改善させることを特徴とする付記１１記載の３次元位置姿勢センシング処理プログラムを記録した記録媒体。
【０３１３】
付記１３．コンピュータによって画像を解析して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定するための処理プログラムを記録した記録媒体であり、
上記処理プログラムはコンピュータに、撮像装置により撮像された、測定対象物に対する３次元位置情報が既知の少なくとも３個のマーカが写っている画像を入力させ、
上記画像上で上記各マーカに対応する領域を抽出させ、
上記抽出した領域において、マーカの外観上の特徴から個々のマーカを特定させ、
上記特定されたマーカの画像上での大きさから、マーカまでの距離を推定させ、
上記推定された各マーカまでの距離と、このマーカの上記画像上での位置と、このマーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算させる、
ことを特徴とする３次元位置姿勢センシング処理プログラムを記録した記録媒体。
【０３１４】
付記１４．コンピュータによつて画像を解析して、この画像を撮影した撮影装置に対する測定対象物の位置姿勢を測定するための処理プログラムを記録した記録媒体であり、
上記処理プログラムはコンピュータに、撮像装置により撮像された、測定対象物に対する３次元位置情報が既知のマーカが写っている画像を入力させ、
上記入力された画像を縮小させ、
上記縮小された画像上で上記各マーカに対応する領域を抽出させ、
上記抽出された各マーカの上記画像上での位置と、各マーカの測定対象物に対する３次元位置情報を用いて、上記撮像装置に対する測定対象物の３次元位置姿勢を演算させる、
ことを特徴とする３次元位置姿勢センシング処理プログラムを記録した記録媒体。
【０３１５】
付記１５．上記処理プログラムは、コンピュータにさらに、上記測定対象物の特徴部分を画像より抽出させ、上記抽出した特徴部分の画像上の位置より、上記演算された測定対象物の位置姿勢を補正させることを特徴とする付記１３または付記１４記載の３次元位置センシング処理プログラムを記録レた記録媒体。
【０３１６】
付記１６．表面に識別マークが配されたマーカであり、
上記識別マークの外形が円形であることを特徴とするマーカ。
【０３１７】
（効果）
この付記１６．のマーカによると、画像から識別マークを抽出する場合に、識別マークの外形が円形であるので、その抽出処理が容易である。
【０３１８】
すなわち、識別マークの外形が四角とか三角ではマーク部分の向きによっては画像上の図形の形状が大幅に異なり認識が困難であるのに対し、識別マークの外形が円形であるならマーク部分の向きによっても楕円になるだけであり、画像上の認識処理が容易となる。
【０３１９】
付記１７．表面に識別マークが配されたマーカであり、
上記識別マークが、
外形が円形である背景部と、
この背景部の内部に配設された複数の所定パターンを有し、
上記複数の所定パターンの着色の組み合わせで、マーカを特定可能であることを特徴とするマーカ。
【０３２０】
（効果）
この付記１７．のマーカによると、付記１６．のマーカによる上述の効果に加えて、領域内の輝度や色度を分析してマーカを特定可能であるという効果を有している。
【０３２１】
付記１８．表面に識別マークが配されたマーカであり、
上記識別マークが
外形が円形である背景部と、
この背景部の中心に配された中心印と、
上記中心印を囲む同心円上に等間隔で配された複数の周辺印とを有し、
上記中心印と周辺印の着色の組み合わせで、マーカを特定可能であることを特徴とするマーカ。
【０３２２】
（効果）
この付記１８．のマーカによると、付記１６．のマーカによる上述の効果に加えて、領域内の輝度や色度を分析してマーカを特定可能であるという効果を有している。
【０３２３】
付記１９．表面に識別マークが配されたマーカであり、
上記識別マークが、
外形が円形である背景部と、
この背景部の内部に配された半径の異なる複数の同心円とを有し、
上記同心円の間の領域の着色の組み合わせで、マーカを特定可能であることを特徴とするマーカ。
【０３２４】
付記２０．位置測定に供されるプローブであり、
測定対象に当接する部位である当接部と、
該プローブを識別する識別マークが表面に配されたマーク部と、
を具備することを特徴とするプローブ。
【０３２５】
付記２１．対象物あるいは対象物付近に装着された３次元位置が既知な複数のマーカを、画像撮影装置を利用して撮影しマーカの画像内位置を計測することにより、対象物との相対的３次元位置姿勢を計測する装置において、
複数のマーカに対応する領域を前記マーカの画像内から抽出する手段と、
該マーカに対応する領域の画像内での幾何学的特徴を算出することにより、個々の該マーカを同定する手段と、
該マーカの３次元位置に基づいて、対象物と画像撮影装置間の相対的３次元位置姿勢を推定する手段と、
を具備したことを特徴とする３次元位置姿勢センシング装置。
【０３２６】
付記２２．個々のマーカ内にはコード化されたパターンが配されることを特徴とする付記２１記載の３次元位置姿勢センシング装置。
【０３２７】
付記２３．前記マーカ領域の画像内での幾何学的特徴を算出し該マーカを同定する手段は、画像内のマーカ領域の大きさを計測し、
前記対象物と画像撮影装置間の相対的３次元位置姿勢を推定する手段は、該マーカ領域の大きさの計測値に基づいて画像撮影装置とマーカ間の距離の初期値を算出し、該初期値に基づいて対象物と画像撮影装置間の相対的３次元位置姿勢を算出することを特徴とする付記２１記載の３次元位置姿勢センシング装置。
【０３２８】
付記２４．前記画像撮影装置は他の装置に装着されており、画像撮影装置と該他の装置間の３次元位置姿勢関係が既知あるいは別途計測可能てあるとき、上記対象物と画像撮影装置間の相対的な３次元位置姿勢を利用することにより、対象物と該他の装置間の３次元位置姿勢を計測することを特徴とする付記２１記載の３次元位置姿勢センシング装置。
【０３２９】
付記２５．前記相対的距離推定値が実現可能である範囲をとるマーカのみを利用して推定値を算出することを特徴とする付記２３載の３次元位置姿勢センシング装置。
【０３３０】
付記２６．前記推定された相対的距離に推定誤差分散を算出し、その推定値と誤差分散の両方を考慮しながら、対象物と画像撮影装置の３次元位置姿勢を推定することを特徴とする付記２３記載の３次元位置姿勢センシング装置。
【０３３１】
付記２７．前記マーカとしては、円形のマーカであり、そのマーカの画像内での投影像を楕円として近似し、楕円の長軸の長さを利用して、画像撮影装置からマーカまでの距離を推定することを特徴とする付記２３記載の３次元位置姿勢センシング装置。
【０３３２】
付記２８．前記マーカとしては、円形のカラーマーカを利用することを特徴とする付記２７．記載の３次元位置姿勢センシング装置。
【０３３３】
付記２９．前記マーカの形状として円形を利用し、マーカ内のコードとして、小円の色の異なるパターンを利用することを特徴とする付記２２記載の３次元位置姿勢センシング装置。
【０３３４】
付記３０．前記マーカとしては、同心円上に色の異なるパターンを生成することを特徴とする付記２２記載の３次元位置姿勢センシング装置。
【０３３５】
付記３１．前記円形マーカの方向情報を付加して３次元位置センシングを行うことを特徴とする付記２７記載の３次元位置姿勢センシング装置。
【０３３６】
付記３２．原画像からマーカ領域を抽出する際、原画像の縮小画像を作成し、該縮小画像内からマーカに対応すると想定される領域の候補を抽出し、該候補領域が原画像内で対応する領域を算出したのち、原画像内の領域でマーカに対応する領域を抽出・認識することを特徴とする付記２１記載の３次元位置姿勢センシング装置。
【０３３７】
付記３３．固有の幾何学的特徴を有するマーカが認識されたのち、そのマーカの２次元または３次元位置関係を利用して、固有の幾何学的特徴を持たない別マーカを画像内から抽出し、該別マーカの画像内位置と３次元位置を利用して、対象物と画像撮影装置間の３次元位置姿勢パラメータを更新することを特徴とする付記２１記載の３次元位置姿勢センシング装置。
【０３３８】
付記３４．前記画像撮影装置は、複数の画像を撮影し、該複数の画像を利用することを特徴とする付記２１記載の３次元位置姿勢センシング装置。
【０３３９】
付記３５．マーカ群がセンサプローブの表面に装着され、該センサプローブを対象物として取り扱うことで、センサプローブの相対的位置姿勢を推定するとともに、センサプローブのプローブ先端の位置を計測することを特徴とする付記２１記載の３次元位置姿勢センシング装置。
【０３４０】
付記３６．マーカ形状が正多角形であることを特徴とする付記２３記載の３次元位置姿勢センシング装置。
【０３４１】
【発明の効果】
本発明は、以上のような方式を採用することにより、従来の方法では困難であった遮蔽などの影響を受けずに、ロバストに（安定に）対象物の３次元位置姿勢を推定することができる。
【０３４２】
それゆえ、本発明の３次元位置姿勢センシング装置は、画像撮影装置または他の装置を規定する座標系における対象物の３次元位置を推定することにより、ロボットによる対象物の把持や、対象物の検査などに有効的に利用することができる。
【０３４３】
従って、以上説明したように、本発明によれば、
（１）遮蔽などの影響により、マーカ群の一部が観察されないときでも、対象物の３次元位置姿勢を推定することができるようにするとともに、
（２）従来のｎ点問題では、解を確定することができなかった３個のマーカからだけでも位置姿勢を推定することができるようにした、
３次元位置姿勢センシング装置を提供することが可能となる。
【図面の簡単な説明】
【図１】図１は、第１の実施の形態による３次元位置姿勢センシング装置の構成を示すブロック図である。
【図２】図２は、図１の画像撮影装置３と、カメラ画像面と、対象物１が規定するオブジェクト座標系との関係を表した図である。
【図３】図３は、第１の実施の形態における幾何学的特徴を持ったコードマーカ２群の一例を表した図である。
【図４】図４は、第１の実施の形態における別のコードパターンを表した図である。
【図５】図５は、第１の実施の形態における別のコードパターンを表した図である。
【図６】図６は、第１の実施の形態において、対象物１の３次元位置姿勢を推定するまでの処理手順を表したフローチャートである。
【図７】図７は、第１の実施の形態において、コードパターンの抽出処理過程を表した図である。
【図８】図８は、第１の実施の形態において、得られた３個のマーカＭｉに関して想定される３個の３角形Δ０c ＭｉＭｊを示す図である。
【図９】図９は、第２の実施の形態におけるステップ２での処理の手順を示すフローチャートである。
【図１０】図１０は、第２実施の形態において、マーカの中心をＰi 、カメラの焦点を０c とするとともに、カメラ像面と０c Ｐi の交点をＱi とすると、Ｑi はマーカ像の中心であると判断することを示した図である。
【図１１】図１１は、第４の実施の形態において、画像内からランドマークを抽出する一例を示した図である。
【図１２】図１２は、第５の実施の形態で想定される構成を表したブロック図である。
【図１３】図１３は、第６の実施の形態による構成を表すブロック図である。
【図１４】図１４は、第６の実施の形態によるセンサプローブ１３８の例を表した図である。
【図１５】図１５は、第７の実施の形態の概念図を表したブロック図である。
【図１６】図１６は、第７の実施の形態の処理手順を表したフローチャートである。
【符号の説明】
１，１２１，２０１，…対象物、
２，１２２，２０２…マーカ
３，１２３，１３３，２０３，２０４…画像撮影装置、
４，１２４，１３４，２０５…コンピュータ、
１２５，１３６…基準座標系を規定する装置、
１３８…センサプローブ、
１３９…探針される物体Ｘ。
【図１】
0004794708.tif 000037
【図２】
0004794708.tif 000038
【図３】
0004794708.tif 000039
【図４】
0004794708.tif 000040
【図５】
0004794708.tif 000041
【図６】
0004794708.tif 000042
【図７】
0004794708.tif 000043
【図８】
0004794708.tif 000044
【図９】
0004794708.tif 000045
【図１０】
0004794708.tif 000046
【図１１】
0004794708.tif 000047
【図１２】
0004794708.tif 000048
【図１３】
0004794708.tif 000049
【図１４】
0004794708.tif 000050
【図１５】
0004794708.tif 000051
【図１６】
0004794708.tif 000052