JP4282216B2

JP4282216B2 - ３次元位置姿勢センシング装置

Info

Publication number: JP4282216B2
Application number: JP2000284318A
Authority: JP
Inventors: 祐一郎赤塚; 明人斉藤; 隆男柴▲崎▼
Original assignee: Olympus Corp
Current assignee: Olympus Corp
Priority date: 2000-09-19
Filing date: 2000-09-19
Publication date: 2009-06-17
Anticipated expiration: 2020-09-19
Also published as: JP2002090118A

Description

【０００１】
【発明の属する技術分野】
本発明は、画像撮影装置を利用して対象物を撮影し、当該対象物の相対的な３次元位置姿勢を計測する３次元位置姿勢センシング装置に関する。
【０００２】
【従来の技術】
従来、所定形状の指標たるマーカを撮影した画像を解析することで、該マーカに対する画像入力手段の位置や姿勢を求める技術が知られている。
【０００３】
例えば、「ＶＲインターフェースのための単眼による長方形マーカ位置・姿勢の高密度実時間推定方法」（３Ｄ Image Conference 96 予稿集 pp167-172高橋章、石井郁夫、牧野秀夫、中野真１９９６）には、位置座標が予め判っている長方形マーカを撮影して、当該長方形マーカの四隅の撮影画像上の位置から当該長方形マーカと撮影カメラの相対的位置姿勢関係を求め、該撮影カメラの位置及び姿勢を計算する技術が開示されている。
【０００４】
【発明が解決しようとする課題】
しかしながら、上述した従来技術では、カメラとマーカとが遠く離れている場合には、当該マーカの四隅の画像上での離間距離が小さくなり、検出精度が悪くなるという問題があり、ゆえに検出可能な範囲が限られていた。
【０００５】
これを回避するため、複数のマーカをカメラが撮影する可能性のある空間領域にある程度の密度で配置しておき、カメラの撮影範囲にあるマーカのうち、ある程度近い距離のマーカを使って、このカメラとマーカの相対的位置姿勢を求めるようにすることもできる。しかしながら、このように複数のマーカを用いた場合には、上述した従来技術のように単純な図形を用いた場合において、どのマーカに対しての相対的位置なのかを知ることが困難であった。
【０００６】
本発明は、上記問題に鑑みてなされたもので、その目的とするところは、対象物との位置姿勢関係が既知のマーカを撮影した画像を解析して、当該マーカと撮影手段との相対的位置姿勢関係を求め、これをもって当該対象物の位置及び姿勢を求める場合に、比較的広い領域で測定可能な３次元位置姿勢センシング装置を提供することにある。
【０００７】
【課題を解決するための手段】
上記目的を達成するために、本発明の第１の態様では、撮影装置で撮影した画像より、対象物と撮影装置の相対的な３次元位置姿勢関係を計測する装置であって、上記対象物と３次元位置姿勢関係が既知のマーカとを上記撮影装置で撮影した画像を入力する画像入力手段と、上記画像入力手段より入力された画像において、上記マーカを当該マーカの幾何学的特徴を用いて同定する同定手段と、上記同定手段により同定された複数のマーカのうちの１つのマーカの画像を解析して、該マーカと上記撮影装置の相対的位置姿勢関係を求め、上記対象物と上記撮影装置との３次元位置姿勢関係を求める、単一マーカによる位置姿勢検出手段と、上記同定手段で同定された複数のマーカの画像上の位置に基づいて、該マーカと上記撮影装置の相対的な位置姿勢関係を求めて、上記対象物と上記撮影装置の３次元位置姿勢関係を求める、複数マーカによる位置姿勢検出手段と、上記単一マーカによる位置姿勢検出手段と上記複数マーカによる位置姿勢検出手段を択一的に切換える切換手段と、を有し、上記切換手段は、画像上のマーカの視野角に基づいて、上記単一マーカによる位置姿勢検出手段と上記複数のマーカによる位置姿勢検出手段を切換えることを特徴とする３次元位置姿勢センシング装置が提供される。
【００１０】
第２の態様では、上記第１の態様において、上記単一マーカによる位置姿勢検出手段は、上記１つのマーカの画像より複数の特徴部位を検出し、上記特徴部位の画像上での位置関係と上記特徴部位の実際のマーカ上での位置関係とを比較して、上記マーカと上記撮影装置の相対的位置姿勢関係を求めるものであることを特徴とする３次元位置姿勢センシング装置が提供される。
【００１１】
第３の態様では、上記第２の態様において、上記マーカの形状は４角形以上の多角形であり、上記単一マーカによる位置姿勢検出手段は、この多角形の頂点を上記特徴部位として検出することを特徴とする３次元位置姿勢センシング装置が提供される。第４の態様では、上記第１の態様において、上記マーカは、当該マーカの大きさを表わす情報を含むことを特徴とする３次元位置姿勢センシング装置が提供される。
【００１２】
第５の態様では、上記第１の態様において、上記マーカは、当該マーカの場所の属性を表わす情報を含むことを特徴とする３次元位置姿勢センシング装置が提供される。第６の態様では、上記第１の態様において、上記マーカは、自発光型材料で形成されていることを特徴とする３次元位置姿勢センシング装置が提供される。第７の態様では、上記第６の態様において、上記自発光型材料が用いられたマーカは、非常災害時の緊急灯と同期して点灯することを特徴とする３次元位置姿勢センシング装置が提供される。
【００１３】
第８の態様では、上記第１の態様において、上記マーカは、入射光を反射して入射光路を逆進させる再帰性材料により形成されていることを特徴とする３次元位置姿勢センシング装置が提供される。第９の態様では、上記第１の態様において、上記マーカは、蛍光材料で形成されていることを特徴とする３次元位置姿勢センシング装置が提供される。
【００１４】
上記第１０の態様では、上記第１の態様において、上記マーカは、人間の目には不可視であることを特徴とする３次元位置姿勢センシング装置が提供される。上記第１１の態様では、上記第１の態様において、上記マーカは、投影機で投影されたものであることを特徴とする３次元位置姿勢センシング装置が提供される。
【００１５】
上記第１乃至第１１の態様によれば以下の作用が奏される。
【００１６】
即ち、本発明の第１の態様では、画像入力手段により上記対象物と３次元位置姿勢関係が既知のマーカとを撮影装置で撮影した画像が入力され、同定手段により上記画像入力手段より入力された画像において、上記マーカが当該マーカの幾何学的特徴を用いて同定され、単一マーカによる位置姿勢検出手段により上記同定手段により同定された複数のマーカのうちの１つのマーカの画像が解析されて、該マーカと上記撮影装置の相対的位置姿勢関係が求められ、上記対象物と上記撮影装置との３次元位置姿勢関係が求められ、複数マーカによる位置姿勢検出手段により、上記同定手段で同定された複数のマーカの画像上の位置に基づいて、該マーカと上記撮影装置の相対的な位置姿勢関係が求められて、上記対象物と上記撮影装置の３次元位置姿勢関係が求められ、切換手段により上記単一マーカによる位置姿勢検出手段と上記複数マーカによる位置姿勢検出手段とが択一的に切換えられる。ここで、上記切換手段により、画像上のマーカの視野角に基づいて、上記単一マーカによる位置姿勢検出手段と上記複数のマーカによる位置姿勢検出手段が切換えられる。
【００１９】
第２の態様では、上記第１の態様において、上記単一マーカによる位置姿勢検出手段により、上記１つのマーカの画像より複数の特徴部位が検出され、上記特徴部位の画像上での位置関係と上記特徴部位の実際のマーカ上での位置関係とが比較されて、上記マーカと上記撮影装置の相対的位置姿勢関係を求められる。
【００２０】
第３の態様では、上記第２の態様において、上記マーカは４角形以上の多角形で構成されており、上記単一マーカによる位置姿勢検出手段により、この多角形の頂点が上記特徴部位として検出される。
【００２１】
そして、第４の態様では、上記第１の態様において、上記マーカには、当該マーカの大きさを表わす情報を含められている。第５の態様では、上記第１の態様において、上記マーカには、当該マーカの場所の属性を表わす情報を含められている。第６の態様では、上記第１の態様において、上記マーカは、自発光型材料で形成されている。第７の態様では、上記第６の態様において、上記自発光型材料が用いられたマーカは、非常災害時の緊急灯と同期して点灯する。第８の態様では、上記第１の態様において、上記マーカは、入射光を反射して入射光路を逆進させる再帰性材料により形成されている。第９の態様では、上記第１の態様において、上記マーカは、蛍光材料で形成されている。
【００２２】
上記第１０の態様では、上記第１の態様において、上記マーカは、人間の目には不可視である。上記第１１の態様では、上記第１の態様において、上記マーカは、投影機で投影されたものである。
【００２３】
【発明の実施の形態】
以下、図面を参照して、本発明の実施の形態について説明する。
【００２４】
先ず、本発明の第１の実施の形態について説明する。
【００２５】
図１は本発明の第１の実施の形態に係る３次元位置姿勢センシング装置の構成を示す機能ブロック図である。以下、同図を参照して詳述する。
【００２６】
この図１に示されるように、３次元位置姿勢を推定すべき対象物４或いはその付近には、幾何学的に固有の特徴を持った複数の固有マーカ５（以下、コードマーカと称する）が配置されている。
【００２７】
そして、画像撮影部１は当該コードマーカ５を撮影し、その画像をコンピュータ２に転送する。上記画像撮影部１としては、一般的なＴＶカメラやディジタルビデオカメラ等を採用することができる。また、上記画像撮影部１から転送された画像を受けるコンピュータ２としては、通常のパーソナルコンピュータや特殊な画像処理演算装置等を採用することができる。
【００２８】
上記画像撮影部１がＴＶカメラでありアナログ信号を出力する場合には、上記コンピュータ２内には、当該アナログ信号をディジタル化する装置やユニットが含まれる。また、上記画像撮影部１がディジタルカメラやディジタルビデオカメラ等である場合には、ディジタル信号をコンピュータ２に直接的に転送し、当該コンピュータ２にてディジタル信号を処理することになる。
【００２９】
このように、コンピュータ２が、画像撮影部１で撮影されたコードマーカ５に係る画像を受け、ディジタル信号に変換し、該ディジタル信号を処理することで画像内のコードマーカ５を認識し、該コードマーカ５の画像内位置と予め登録されているコードマーカ５の３次元位置とを利用することで、対象物４の画像撮影部１に対する３次元位置姿勢を推定する。このコードマーカ５の３次元位置は、コンピュータ２内のメモリに予め格納されている。
【００３０】
さらに、図２に示されるように、カメラパラメータ検出部３を構成に取込めば、画像撮影部１たるカメラのレンズ焦点距離や歪値などの情報を当該カメラパラメータ検出部３よりコンピュータ２に転送し、当該コンピュータ２において、これらの情報を加味した上で３次元位置姿勢の推定がされる。
【００３１】
先ず、少なくとも３個以上のコードマーカ５が同定できた場合において、対象物４の位置姿勢を推定する方法に関して解説する。
【００３２】
以下、画像と座標変換に関する基本的な扱いに関して説明する。
【００３３】
基本的に対象物４と画像撮影部１は、それぞれ固有の座標系を有しており、画像撮影部１が撮影する画像は、カメラ画像面として定義される。
【００３４】
図３は、上記カメラ画像面と画像撮影部１のカメラ座標系、及びオブジェクト座標系の関係を示した図である。
【００３５】
対象物４が規定するオブジェクト座標系はその原点をＯm 、その３次元座標を（ｘm ，ｙm ，ｚm ）とする。一方、画像撮影部１が規定するカメラ座標系は、その原点をＯc 、その３次元座標を（ｘc ，ｙc ，ｚc ）とする。
【００３６】
カメラ画像面は、その軸がｕ軸とｖ軸により構成され、ｕ軸はカメラ座標系のｘc軸と平行に、ｖ軸はｙc軸に平行に取られ、カメラ座標系を規定するｚc軸が画像撮影部１の光学系の光軸と一致し、その光軸とカメラ画像面が交わる点（カメラ画像面の中心）が、（ｕ0 ，ｖ0 ）で定義される。
【００３７】
画像撮影部１に相対する対象物の３次元位置姿勢を推定する問題は、カメラ座標系に対するオブジェクト座標系の位置姿勢を推定する問題、換言すれば、オブジェクト座標系からカメラ座標系への座標変換パラメータ、又はカメラ座標系からオブジェクト座標系への座標変換パラメータを算出する問題に帰着される。
【００３８】
これを数学的に記述すると、斉次変換行列 cＨm 又は mＨc を利用して、
【００３９】
【数１】

【００４０】
と定義することができる。ここに、Ｒ＝（ｒij），Ｒ′＝（ｒ′ij）は３×３の回転行列を示し、ｔ＝（ｔx ，ｔy ，ｔz ），ｔ′＝（ｔ′x ，ｔ′y ，ｔ′z ）は３次元並進べクトルを示している。
【００４１】
以下に詳述するマーカ群｛Ｍi ；ｉ＝１，２，…，ｍ｝は、予めオブジェクト座標系での３次元位置が計測されており、それらを（ｘi^m，ｙi^m，ｚi^m）で表現する。また、その画像内位置を（ｕi ，ｖi ）で記述する。
【００４２】
すると、画像撮影部１をピンホールカメラモデルで近似したとき、それらの座標間には、以下の関係が成立する。
【００４３】
【数２】

【００４４】
ここに、（ｕ0 ，ｖ0 ）は画像内の中心、（αu ，αv ）はｕ方向とｖ方向の拡大率を表し、画像撮影部１に関するカメラ内部パラメータであり、カメラキャリブレーションにより推定できる値である。
【００４５】
以下、図７のフローチャートを参照して、コンピュータ２が画像を入力した後に、対象物４の３次元位置姿勢を推定する動作の流れを説明する。
【００４６】
先ず、画像を受け取ったコンピュータ２は、その画像の中からコードマーカ５に対応する領域と推定される候補領域を抽出する（ステップＳ１）。
【００４７】
続いて、このステップＳ１にて抽出された候補領域内を詳細に解析し、その中からコードマーカ５のコードに対応する幾何学的特徴を抽出し、そのコードが認識された場合には、その領域をマーカ領域として、その画像内位置とコードとを登録する（ステップＳ２）。そして、この登録された画像から抽出されたコードマーカ５の画像内２次元位置と対象物に相対する３次元位置とを利用することで画像撮影部１に対応する対象物の位置を算出する（ステップＳ３）。
【００４８】
以下、図７の上記ステップＳ１乃至Ｓ３について更に詳述する。
【００４９】
（ステップＳ１）
第１の実施の形態では、画像撮影部１がカラー画像を生成するものとして、コードマーカ５としては、図４乃至図６に示したものを想定する。
【００５０】
コードマーカ５の外枠は単色から成立しているので、その単色に敏感な色フィルタをアルゴリズム内に導入する。
【００５１】
具体的には、画像面（ｕ，ｖ）で定義される画像点に関して、カラー画像を構成するフィルタの計測値Ｒ（赤），Ｇ（緑），Ｂ（青）から、
ｉ＝（Ｒ＋Ｇ＋Ｂ）／３
ｒ＝Ｒ／（Ｒ＋Ｇ＋Ｂ）
ｇ＝Ｇ／（Ｒ＋Ｇ＋Ｂ）
に対応する３個のベクトルを算出する。
【００５２】
そして、コードマーカ５が取り得る画像内での色パターンの許容値が、
ｉmin ＜ｉ＜ｉmax
ｒmin ＜ｒ＜ｒmax
ｇmin ＜ｇ＜ｇmax
を満たす画像領域を抽出する。ここで、ｉmin ，ｉmax ，ｒmin ，ｒmax ，ｇmin ，ｇmax 等の値は、予め設定しておく。
【００５３】
次いで、画像領域内の穴埋めを行うことにより、コードマーカ５に対応する領域を決定する。
【００５４】
（ステップＳ２）
次に抽出された領域がマーカの像であるかを判定する。
【００５５】
例えば、パターンマッチングを用い、予め登録しておいたマーカ像と比較を行うことでマーカの判定ができる。
【００５６】
（ステップＳ３）
上記ステップ２で同定されたコードマーカ５の画像内位置（ｕi ，ｖi ）（ｉ＝１，２，３，…）とオブジェクト座標系における３次元マーカ位置（ｘi^m，ｙi^m，ｚi^m）が与えられたとき、いかにして先に示した式（３）で定義される斉次変換行列 cＨm を算出するが、本ステップ４の課題である。
【００５７】
これは基本的に文献（M.A.Fischler and R.C.Bolles, “Random sample consensus : A paradigm for model fitting with applications to image analysis and automated cartography,”Communications of the ACM, Vol.24, No.6, June 1981, pp.381-395）で示される方法を以下の如く変更しながら行う。
【００５８】
この文献で紹介されている方法では、同定されたマーカ群の中から一直線上にない任意の３個のマーカを選択し、その３個のコードマーカを利用してカメラ座標系とオブジェクト座標系間の座標変換パラメータ解の候補を算出する。
【００５９】
しかしながら、その座標変換パラメータとしては、最大４種類の解が存在することが判っているので、その４種類の各解に対して、選択されなかったマーカ群を利用して解の検証を行うことにより、解の絞り込みを行うと共に、その解を初期値として、全てのコードマーカを利用して解の更新を行うものである。
【００６０】
以下、その方法に関して簡単に説明する。
【００６１】
同定されたマーカ群の中から、ある選択基準に従って画像内で一直線上にない３個のコードマーカを選択する。この選択基準としては、カメラ画像面で３個のコードマーカを頂点とする３角形の面積が最大となる３個のコードマーカを選択する方法やカメラ画像面で３個のコードマーカを頂点とする３角形の面積が最小となる３個のマーカを選択する方法等が考えられる。
【００６２】
このようにして得られたコードマーカをＭi （ｉ＝１，２，３）とする。
【００６３】
次に、これら３個のコードマーカＭi （そのモデル座標系における３次元位置をＰi（ｘim，ｙim，ｚim）、画像内位置をＱi （ｕi ，ｖi ）とする）に関して、図８に示されるような３個の３角形ΔＯc Ｍi Ｍj （ｉ，ｊ＝１，２，３；ｉとｊは等しくない）を考える。
【００６４】
これらの３個の３角形に関して、カメラ画像径の原点Ｏc から各マーカＭi までの距離をｄi とし、マーカＭi ，Ｍj とカメラ座標系原点Ｏc がなす角度θijとする。また、マーカＭi ，Ｍj 間の距離をＲijとする。
【００６５】
このとき、距離Ｒ12，Ｒ23，Ｒ31と角度θ12，θ23，θ31は既知の値となるが、ｄ1 ，ｄ2 ，ｄ3 は未知の値となる。逆に言えば、距離ｄ1 ，ｄ2 ，ｄ3 を算出することができれば、オブジェクト座標系からカメラ座標系への座標変換パラメータを算出することができる。
【００６６】
以下、この点に関して解説する。
【００６７】
（１）距離Ｒ12，Ｒ23，Ｒ31の算出方法
Ｒ12は点Ｐ1 と点Ｐ2 間のユークリッド距離として算出される。同様に、Ｒ23，Ｒ31はそれぞれ、点Ｐ2 とＰ3 、点Ｐ3 とＰ1 間のユークリッド距離として算出される。
【００６８】
（２）角度θ12，θ23，θ31の算出方法
マーカＭ1 ，Ｍ2 とカメラ座標系の原点Ｏcとがなす角度θijは以下のように算出することができる。
【００６９】
先ず、
【００７０】
【数３】

【００７１】
を（ｕi ，ｖi ）の正規化された座標値とすると、
【００７２】
【数４】

【００７３】
が成立する。この正規化された画像点
【００７４】
【数５】

【００７５】
は、カメラ座標系でｚc ＝１に対応する（ｘc ，ｙc ）に対応することから、
【００７６】
【数６】

【００７７】
が成立する。
【００７８】
このようにして、３個の角度をその余弦より算出することができる。
【００７９】
（３）距離ｄi （ｉ＝１，２，３）の算出方法
３角形のＯc Ｍ1 Ｍ2 ，Ｏc Ｍ2 Ｍ3 ，Ｏc Ｍ3 Ｍ1 に対して第２余弦定理を適用すれば、
Ｒ12＝ｄ1²＋ｄ2²−２ｄ1 ｄ2 cosθ12
Ｒ23＝ｄ2²＋ｄ3²−２ｄ2 ｄ3 cosθ23
Ｒ31＝ｄ3²＋ｄ1²−２ｄ3 ｄ1 cosθ31
が導かれる。
【００８０】
これら３式において、未知数はｄ1 ，ｄ2 ，ｄ3 の３個であり、制約式も３個であるので、理論的には上式を満たす解｛（ｄ1(k)，ｄ2(k)，ｄ3(k)）｝：ｋ＝１，２，３，４｝が存在する。
【００８１】
その解法に関しては、文献（M.A.Fischler and R.C.Bolles, “Random sample consensus : A paradigm for model fitting with applications to image analysis and automated cartography,”Communications of the ACM, Vol.24, No.6, June 1981, pp.381-395）で詳しく述べられているように、この方程式には最大４個の解が存在することが分かっており、その解が４次方程式の解として数値解析的に解くことが可能である。
【００８２】
（４）解（ｄ1 ，ｄ2 ，ｄ3 ）の検証と最適解の選択
基本的には、最大４個の解の中で１個だけが正しい解を与えるものである。
【００８３】
上記の解の中でどの解が正しいかを検証するのが、本段階である。
【００８４】
各解（ｄ1 ，ｄ2 ，ｄ3 ）について、カメラ座標系Ｃでのマーカ位置（ｘi^c，ｙi^c，ｚi^c）を算出する方法について解説する。
【００８５】
カメラ座標系原点Ｃからマーカまでの距離がｄi であり、その画像内位置（ｕi ，ｖi ）であり、
【００８６】
【数７】

【００８７】
と書くことができる。
【００８８】
いま、オブジェクト座標系でのマーカ位置を（ｘi^m，ｙi^m，ｚi^m）とすれば、オブジェクト座標系Ｏm からカメラ座標系Ｏc への変換は、
【００８９】
【数８】

【００９０】
と書かれる。
【００９１】
ここにＲは回転行列を表し、ｔは並進ベクトルを表す。
【００９２】
いま、両座標系でのマーカ群の重心ベクトルを［ｘmean^c，ｙmean^c，ｚmean^c］^T ，［ｘmean^m，ｙmean^m，ｚmean^m］^Tとすると、
【００９３】
【数９】

【００９４】
が成立し、並進ベクトルと回転行列を別々の式で算出することができる。
【００９５】
ｉ＝１，２，３に対して、上記方程式を解く方法としては、quaternion法（四元数法）がある。この方法の詳細は文献（B.K.P.Horn, “Closed-form solution of absolute orientation using unitquaternions,”Journal of Optical Society of America A, Vol.4, No.4, 1987, pp.629-642.）に述べられているので、その詳細はここでは省略する。
【００９６】
このようにＲ，ｔが算出されると、斉次変換行列 cＨm は式（１，２）により計算することができる。以上のことを４個の解に対して繰り返し、 cＨm(1), cＨm(2), cＨm(3), cＨm(4)の４個の解を算出することができる。
【００９７】
さて、同定されたコードマーカのうち最初の選択されなかったコードマーカをＭ4 ，Ｍ5 ，…，Ｍm とする。
【００９８】
各斉次変換行列 cＨm(k)（ｋ＝１，２，３，４）に対して、解として最もふさわしい解を、これらＭ4 ，Ｍ5 ，…，Ｍm を利用して決定する方法を説明する。
【００９９】
▲１▼各解 cＨm(k)に対する評価関数dist(k)を最小にするｋを、以下のステップで算出する。
【０１００】
▲２▼各解 cＨm(k)（ｋ＝１，２，３，４）に関して以下の方法により、dist(k)を算出する。
【０１０１】
ａ）dist(k)：＝０と評価関数を初期化する。
【０１０２】
ｂ）同定されたが最初の３個に選択されなかったマーカＭj （ｊ＝４，５，…，ｍ）について、そのオブジェクト座標系における３次元位置（ｘj^m，ｙj^m，ｚj^m）を cＨm(k)を利用してカメラ画像面に変換する。その投影画像点を(ｕj′，ｖj′)とする。これは、
【０１０３】
【数１０】

【０１０４】
により算出することができる。
【０１０５】
続いて、マーカＭj の実際に画像内で測定された２次元位置(ｕj ，ｖj )と投影画像点(ｕj′，ｖj′)との２乗誤差ｅj を算出する。
【０１０６】
ｅj は以下のように算出できる。
【０１０７】
【数１１】

【０１０８】
▲３▼dist(k)が最小となる斉次変換行列の解 cＨm(k)を選択する。
【０１０９】
要約すると、上記のステップで求められる最適解 cＨm(k)は、コードマーカＭ1 ，Ｍ2 ，Ｍ3 から生成される解のうち、他のマーカＭ4 ，Ｍ5 ，…，Ｍm が最も支持する解を選択するものである。
【０１１０】
（５）解の更新
上記（４）で選択された解 cＨm(k)は、コードマーカＭ1 ，Ｍ2 ，Ｍ3 から推定されたものであり、他のマーカＭ4 ，Ｍ5 ，…，Ｍm に対する測定値を利用したものではない。本ステップでは、上記（４）で算出された解 cＨm(k)を初期推定値 cＨm(0)として、全てのコードマーカＭi （ｉ＝１，２，…，ｍ）によりこの解の更新を行う。すなわち、 cＨm を角度成分（roll（φz ）−pitch（φy ）−yaw（φx ）角）と並進成分（ｔx ，ｔy ，ｔz ）に展開して、６次元未知変数ｐ＝（φx ，φy ，φz ；ｔx ，ｔy ，ｔz ）とし、その初期推定値をｐ⁽⁰⁾＝（φx⁽⁰⁾，φy⁽⁰⁾，φz⁽⁰⁾；ｔx⁽⁰⁾，ｔy⁽⁰⁾，ｔz⁽⁰⁾）と定義する。
【０１１１】
具体的には、
【０１１２】
【数１２】

【０１１３】
により定義される。
【０１１４】
これをオブジェクト座標系でのマーカ３次元位置（ｘi^m，ｙi^m，ｚi^m）と、そのカメラ画像面での位置(ｕi ，ｖi )の関係を利用しながら、６次元位置姿勢パラメータｐ＝（φx ，φy ，φz ；ｔx ，ｔy ，ｔz ）を更新することを考える。
【０１１５】
オブジェクト座標系でのマーカ３次元位置（ｘi^m，ｙi^m，ｚi^m）と、そのカメラ画像面での位置(ｕi ，ｖi )の関係は、
【０１１６】
【数１３】

【０１１７】
により与えられる。この式を整理すると、各マーカＭi （ｉ＝１，２，…，ｍ）に関して、
【０１１８】
【数１４】

【０１１９】
なる２次制約式によって表現され、６次元パラメータの初期推定値ｐ⁽⁰⁾＝（φx⁽⁰⁾，φy⁽⁰⁾，φz⁽⁰⁾；ｔx⁽⁰⁾，ｔy⁽⁰⁾，ｔz⁽⁰⁾）を用いて、６次元パラメータｐ＝（φx ，φy ，φz ；ｔx ，ｔy ，ｔz ）を推定する問題となる。
【０１２０】
この問題はよく知られた非線形方程式問題であり、多くの著書がその解法を紹介しているので、ここではその詳細を述べない。
【０１２１】
このようにして６次元パラメータは、全てのマーカの測定値を利用して更新され、オブジェクト座標系からカメラ座標系への座標変換パラメータが算出される。すなわち、対象物４と画像撮影部１の間の位置関係を算出することができる。
【０１２２】
以上説明したように、第１の実施の形態によれば、マーカ群の一部が遮蔽されて検出できなかった場合でも、検出されたマーカだけから対象物４と画像撮影部１の間の３次元位置関係を算出することができる。
【０１２３】
また、マーカの検出に際しては、マーカ固有のコードを利用することにより、マーカ同定の信頼性が従来例に比べて極度に向上させることができるので、より安定な位置姿勢計測を実現することができる。
【０１２４】
前述のように、マーカの検出に際しては、最低３個を検出する必要があるが、４個以上検出できれば、更に信頼性が高くなる。
【０１２５】
ここで、先にも述べたように、図４乃至図６は、幾何学的特徴を持ったコードマーカ５の一例を表したものである。
【０１２６】
各コードマーカ５は幾何学的特徴を有しており、それがコードマーカ５にラベルを付加するに足るコードを生成できる点は重要である。
【０１２７】
図４は１つのマーカ画像を解析して４点の特徴点を認識する場合に好適なコードマーカの形状を示しており、図４（ａ），（ｂ）は正方形、図４（ｃ）は台形、図４（ｄ）はコの字形、図４（ｅ），（ｆ）は円形、図４（ｇ）は星型のコードマーカの構成例を示している。
【０１２８】
すなわち、１つのマーカを解析することで、前出のコードマーカ４つの代替となる４つの位置の既知な特徴点の情報を得ることができるわけである。
【０１２９】
図４（ａ）のコードマーカでは、マーカの検出は外枠内外の色を変え、その違いを検出することで行い、外枠を微分、細線化などすることでその４隅を位置の既知な特徴点として認識する。
【０１３０】
さらに、向きは下側の横線（外枠内側）を検出することで認識し、情報は外枠内部の８点（中央の１点は位置認識用）を検出することで取得する。チェックデジットについては、外枠内側左右の２点（２点→２ｂｉｔ）を用いる。
【０１３１】
図４（ｈ）はコードマーカ（図４（ａ））の特徴点が検出された様子を示す。外枠の左下端のオブジェクト座標における座標値を（u1,v1）、更に右下端、右上端、左上端の順にそれぞれ（u2,v2）、（u3,v3）、（u4,v4）とする。これら４つの座標値を前出の説明におけるM1からM4に当てはめ、対象物の位置姿勢の認識が可能になる。
【０１３２】
図４（ｂ）のコードマーカでは、特徴点検出を外枠内側四隅にある４点の検出により実現される以外は図４（ａ）と同様である。この他、図４（ｃ），（ｄ）のコードマーカについても、基本的には図４（ａ），（ｂ）と同様である。
【０１３３】
図４（ｅ）のコードマーカでは、マーク検出は外枠内外の色を変え、その違いを検出することで行い、４点検出については、外枠の内側四隅にある４点の検出により実現する。さらに、向きは下側の横線（外枠内側）を検出することで認識し、情報は外枠内部の８点（中央の１点は位置認識用）を検出することで取得する。また、チェックデジットについては、外枠内側左右の２点（２点→２ｂｉｔ）を用いる。図４（ｆ）のコードマーカでは、４点検出を外枠外側四隅にある４点の検出により実現する以外は図４（ｅ）と同様である。この他、図４（ｇ）のコードマーカについても、図４（ａ）乃至（ｆ）と同様である。
【０１３４】
図５は８ｂｉｔの情報を格納する外枠内部のコードの形状のバリエーションを示したものである。すなわち、図５（ａ）は円形のコード、図５（ｂ）は三角形のコード、図５（ｃ），（ｄ）は四角形のコード、図５（ｅ）は文字コード、図５（ｆ）は線形コード、図５（ｇ）は渦巻き状のコード、により８ｂｉｔの情報が格納される。但し、これらの形状に限定されないことは勿論である。
【０１３５】
図６は斜め方向から撮影された場合にも、認識率が高い例としてシンメトリー／立体マーク（半球、ピラミット形状）の例を示している。図６（ａ）は斜視図であり、図６（ｂ）は上面図、図６（ｃ）は側面図である。
【０１３６】
以上によりマーカが複数あり、その中の単一のマーカを解析することにより位置姿勢が検出できる。
【０１３７】
マーカの種類を、複数用意しておくことにより、例えばマーカの大きさなどのマーカの種類、建物の部屋番号や部屋、廊下、階段などの場所の属性を表わす情報、として用いることができる。また、マーカをＬＥＤやＬＣＤなど自発光型材料で構成することにより、その自発光型材料を制御することや、投影機によりマーカを必要に応じて投影することによりすれば、静的ではなく場合に応じて変更ができる動的なマーカを構築することができる。更に、これら自発光型材料が非常災害時の緊急灯と同期し、災害発生時に点灯するようにしておけば、災害時の自動誘導灯として用いることが可能になる。
【０１３８】
道路などでコーナーキューブとして使われているような再帰性材料で構築すれば、撮影装置に取り付けた僅かな光源を利用して動作ができる。
【０１３９】
蛍光材料によりマーカを構成すれば、照明などが無い状態でも動作することができる。更に、投影装置には感度のある赤外や紫外の波長でマーカを構築すれば、人間の目には不可視な状態で景観を損なわずに動作が可能になる。
【０１４０】
次に第２の実施の形態について説明する。
【０１４１】
前述した第１の実施の形態に係る３次元位置姿勢センシング装置では、複数のマーカの内の１つのマーカを解析したが、第２の実施の形態では、複数または単数のマーカによる位置姿勢センシングを切り換える手段を備える。
【０１４２】
図９は第２の実施の形態を説明する図で、解析するマーカ数の切り替え手段２１を有している。切り替え手段２１が１つのマーカによる位置姿勢センシング指示する場合には、例えば複数見つかっているマーカ５の内の、画面の中で最も左上に位置するマーカ５を解析し、その外枠から検出された特徴点の座標を位置姿勢推定手段２２に渡す。また、複数のマーカによる位置姿勢センシングを指示する場合には、同定されたマーカ５の座標を位置姿勢推定手段２２に渡す。位置姿勢推定手段２２の動作については、第１の実施の形態の中で詳述しているので、ここでは省略する。
【０１４３】
次に第３の実施の形態について説明する。
【０１４４】
第３の実施の形態においては、第２の実施の形態における解析するマーカ数の切り替え手段２１が、マーカの画面内における視野角により動作をする。ここで、画面の水平画角に占めるマーカの角度を視野角と呼ぶ。
【０１４５】
図１０にマーカが画像入力された画面を示す。
【０１４６】
水平方向の画面画角が４０度、その中に占めるマーカ３０から３３のそれぞれの水平方向の視野角が１５度、３度、５度、５度の場合を示している。マーカの中で最大の視野角を持つマーカ３０の視野角により単一のマーカでの位置姿勢センシングか、画面内の４つのマーカによるセンシングかを切り替える。いま、１０度をセンシング切り替えの閾値とする。図１０の場合は最大の視野角のマーカ３０が１０度を超えているため単一のマーカによるセンシングを行う。全てのマーカが１０度以下の視野角の場合には複数マーカによるセンシングを行う。また、全てのマーカが１０度以下で、しかもマーカの個数が３個以下の場合にはセンシングエラーとする。ここでは視野角が水平方向の場合を示したが、垂直方向、またその両方を掛け合わせた画像内に占める面積で判定を行うことも可能である。
【０１４７】
次に、本発明の第４の実施の形態について説明する。
【０１４８】
これは、画像撮影部１以外と対象物４間の３次元位置姿勢を推定する方法に関するものである。今まで説明してきた実施例では、対象物４と画像撮影部１間の位置関係を計測する例を説明したきた。
より実際的な例の場合には図１に示される画像撮影部１とコンピュータ２は、対象物４の位置姿勢を推定する位置センサとして利用される場合がある。この際には、該位置センサを含むシステム内に別の装置があり、その別の装置が規定する座標系をシステムが基準とするシステム座標系と考えることが多い。
【０１４９】
この場合、コンピュータ２は画像撮影部１が規定するカメラ座標系から別の装置が規定する基準座標系への座標変換パラメータをあらかじめ格納しており、その座標変換パラメータを利用して、コンピュータ２は、オブジェクト座標系から該基準座標系への座標変換パラメータを算出する。そして、基準座標系のおける対象物４の３次元位置姿勢を推定する。
【０１５０】
第４の実施の形態によれば、本発明の手段を別の装置のための位置姿勢センサとして利用することも可能である。
【０１５１】
以上、本発明の実施の形態について説明したが、本発明はこれに限定されることなく、その趣旨を逸脱しない範囲で種々の改良・変更が可能である。
【０１５２】
最後に、請求項記載の画像入力手段は画像撮影部１に包含される概念であり、同定手段、単一マーカによる位置姿勢検出手段、複数マーカによる位置姿勢検出手段、切換手段は、コンピュータ２に包含される概念である。
【０１５３】
【発明の効果】
以上詳述したように、本発明によれば、対象物との位置姿勢関係が既知のマーカを撮影した画像を解析して、当該マーカと撮影手段との相対的位置姿勢関係を求め、これをもって当該対象物の位置及び姿勢を求める場合に、比較的広い領域で測定可能な３次元位置姿勢センシング装置を提供することができる。
【図面の簡単な説明】
【図１】第１の実施の形態に係る３次元位置姿勢センシング装置の構成を示す機能ブロック図である。
【図２】第１の実施の形態に係る３次元位置姿勢センシング装置の改良例の構成を示す機能ブロック図である。
【図３】カメラ画像面と画像撮影部のカメラ座標系、及びオブジェクト座標系の関係を示した図である。
【図４】幾何学的特徴を持ったコードマーカの一例を表したものである。
【図５】幾何学的特徴を持ったコードマーカの一例を表したものである。
【図６】幾何学的特徴を持ったコードマーカの一例を表したものである。
【図７】第１の実施の形態において、対象物の３次元位置姿勢を推定する動作の流れを説明するフローチャートである。
【図８】３個のコードマーカＭiに関して３個の３角形ΔＯc Ｍi Ｍj （ｉ，ｊ＝１，２，３；ｉとｊは不等）を示した図である。
【図９】第２の実施の形態に係る３次元位置姿勢センシング装置の構成図である。
【図１０】マーカが画像入力された画面の一例を示す図である。
【符号の説明】
１画像撮影部
２コンピュータ
３カメラパラメータ検出部
４対象物
５コードマーカ

Claims

撮影装置で撮影した画像より、対象物と撮影装置の相対的な３次元位置姿勢関係を計測する装置であって、
上記対象物と３次元位置姿勢関係が既知のマーカとを上記撮影装置で撮影した画像を入力する画像入力手段と、
上記画像入力手段より入力された画像において、上記マーカを当該マーカの幾何学的特徴を用いて同定する同定手段と、
上記同定手段により同定された複数のマーカのうちの１つのマーカの画像を解析して、該マーカと上記撮影装置の相対的位置姿勢関係を求め、上記対象物と上記撮影装置との３次元位置姿勢関係を求める、単一マーカによる位置姿勢検出手段と、
上記同定手段で同定された複数のマーカの画像上の位置に基づいて、該マーカと上記撮影装置の相対的な位置姿勢関係を求めて、上記対象物と上記撮影装置の３次元位置姿勢関係を求める、複数マーカによる位置姿勢検出手段と、
上記単一マーカによる位置姿勢検出手段と上記複数マーカによる位置姿勢検出手段を択一的に切換える切換手段と、
を有し、
上記切換手段は、画像上のマーカの視野角に基づいて、上記単一マーカによる位置姿勢検出手段と上記複数のマーカによる位置姿勢検出手段を切換えることを特徴とする３次元位置姿勢センシング装置。
上記単一マーカによる位置姿勢検出手段は、上記１つのマーカの画像より複数の特徴部位を検出し、上記特徴部位の画像上での位置関係と上記特徴部位の実際のマーカ上での位置関係とを比較して、上記マーカと上記撮影装置の相対的位置姿勢関係を求めるものであることを特徴とする請求項１に記載の３次元位置姿勢センシング装置。
上記マーカの形状は４角形以上の多角形であり、上記単一マーカによる位置姿勢検出手段は、この多角形の頂点を上記特徴部位として検出することを特徴とする請求項２に記載の３次元位置姿勢センシング装置。
上記マーカは、当該マーカの大きさを表わす情報を含むことを特徴とする請求項１に記載の３次元位置姿勢センシング装置。
上記マーカは、当該マーカの場所の属性を表わす情報を含むことを特徴とする請求項１に記載の３次元位置姿勢センシング装置。
上記マーカは、自発光型材料で形成されていることを特徴とする請求項１に記載の３次元位置姿勢センシング装置。
上記自発光型材料が用いられたマーカは、非常災害時の緊急灯と同期して点灯することを特徴とする請求項６に記載の３次元位置姿勢センシング装置。
上記マーカは、入射光を反射して入射光路を逆進させる再帰性材料により形成されていることを特徴とする請求項１に記載の３次元位置姿勢センシング装置。
上記マーカは、蛍光材料で形成されていることを特徴とする請求項１に記載の３次元位置姿勢センシング装置。
上記マーカは、人間の目には不可視であることを特徴とする請求項１に記載の３次元位置姿勢センシング装置。
上記マーカは、投影機で投影されたものであることを特徴とする請求項１に記載の３次元位置姿勢センシング装置。