情報処理装置および方法
【課題】 モデル座標系を基準とした位置姿勢の自由度のうち、特定自由度のパラメータを一意に定める特徴が不足するような物体においても、ロバストかつ高精度な位置姿勢計測を実現する。
【解決手段】 計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、前記計測対象物体の画像を取得する画像取得手段と、前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする。
【解決手段】 計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、前記計測対象物体の画像を取得する画像取得手段と、前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、三次元形状を有する計測対象物体の位置姿勢を取得する技術に関する。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、工業製品の組立のような人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。ロボットが、ハンドなどのエンドエフェクタによって部品を把持して組立を行うためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置姿勢を計測する必要がある。
物体の位置姿勢を計測する方法として、カメラで撮影した濃淡画像やカラー画像（以下、総称して二次元画像と呼ぶ）上から検出される特徴を用いる方法がある。
また、距離センサで画像様に計測して得られた距離データに対して、物体の三次元形状モデルをあてはめる方法がある。このような手法は、モデルフィッティングと呼ばれる。
モデルフィッティングによる位置姿勢推定を行う場合、物体の形状によっては位置姿勢を高精度に計測できないことがある。
例えば、距離データから算出される物体の重心位置と重心まわりのモーメントをモデルと比較する方法では、対象物体のアスペクト比が１に近いような場合に、撮像装置の座標系において、位置姿勢の６パラメータを精度よく推定するのが困難になる。
特許文献１では、距離画像に対するモデルフィッティングにおいて、撮像装置の座標系を基準とした位置姿勢パラメータの推定を複数の段階に分けることで、推定の安定化と効率化を図る方法が開示されている。
特許文献１に開示されている方法では、撮像装置の座標系を基準とした位置姿勢の６自由度成分を、高精度かつロバストに推定できるパラメータから順に推定する。まず、撮像画像の画像座標軸回転の２パラメータと視軸方向位置の１パラメータを推定した後、視軸に垂直な位置の２パラメータを推定し、最後に視軸回転の１パラメータを推定する。パラメータの推定手順を段階的に分けることで、ロバストかつ高精度に位置姿勢の計測を行うことが可能になる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特登録０３２５３４３０
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１でも指摘されているように、物体（モデル）座標系を基準とした位置姿勢の６自由度において、推定が困難な自由度成分を持つような物体が存在する。例えば、工業製品に使われる部品の中には、回転対称に近い形状であって、その回転方向の姿勢成分を一意に決定するための微小な切り欠きを有する部品が存在する。
このような部品では、画像とモデルとのフィッティングにより、位置姿勢の６パラメータを同時に推定しようとすると、ほかの自由度成分に比べて、特定自由度（この場合モデルの回転対称性を定義する軸まわりの姿勢）成分の推定に寄与する情報が不足する。そのため、特定自由度方向のパラメータを推定するのが困難となり、その結果、位置姿勢のロバスト性や精度が低くなるという問題がある。
特許文献１に開示されている方法では、撮像装置の座標系を基準とした位置姿勢の６自由度成分を、高精度かつロバストに推定できるパラメータから順に推定する。つまり、モデル座標系を基準とした特定自由度成分のみを分離して推定するわけではなく、物体の回転中心軸が撮像装置の視軸に一致する場合を除いて、物体の回転中心軸回りの姿勢成分の高い推定精度を得ることが出来ない。
本発明は、以上の課題を鑑みてなされたものであり、モデル座標系を基準とした位置姿勢の自由度の内に、一意に定めるのが困難な特定自由度のパラメータを有する物体においても、ロバストかつ高精度な位置姿勢の計測を実現することを目的とする。
【課題を解決するための手段】
【０００５】
上記課題を鑑み、本発明は、計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、前記計測対象物体の画像を取得する画像取得手段と、前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする。
【発明の効果】
【０００６】
本発明によれば、モデル座標系を基準とした位置姿勢の自由度の内に、一意に定めるのが困難な特定自由度のパラメータを有する物体においても、ロバストかつ高精度な位置姿勢の計測を実現することが出来る。
【図面の簡単な説明】
【０００７】
【図１】第一の実施形態における位置姿勢計測装置の構成を示す図である。
【図２】第一の実施形態における三次元形状モデルを説明する図である。
【図３】第一の実施形態における位置姿勢計測処理のフローチャートである。
【図４】第一の実施形態における第一の位置姿勢パラメータおよび第二の位置姿勢パラメータの設定を説明する図である。
【図５】ステップＳ１４００における第一の位置姿勢算出処理のフローチャートである。
【図６】画像からのエッジ検出を説明する図である。
【図７】線分の投影像と検出されたエッジの関係を説明する図である。
【図８】ステップＳ１６００における第二の位置姿勢算出処理のフローチャートである。
【図９】モデル特徴点の投影像と、対応する画像特徴を説明する図である。
【図１０】ステップＳ１２２０における距離画像とモデルとの対応付け処理を説明する図である。
【図１１】第二の実施形態における第一の位置姿勢パラメータおよび第二の位置姿勢パラメータの設定を説明する図である。
【図１２】変形例２−１における第一の位置姿勢パラメータおよび第二の位置姿勢パラメータの設定を説明する図である。
【図１３】第八の実施形態におけるモデル中心幾何情報決定部の構成を示す図である。
【図１４】第八の実施形態におけるモデル中心幾何情報決定処理のフローチャートである。
【発明を実施するための形態】
【０００８】
［第一の実施形態］
第一の実施形態では、物体における特定の軸に対してほぼ回転対称であり、かつその軸回りの回転を決めるための特徴が少ない物体の位置姿勢推定を行う。このような物体では、位置姿勢の６自由度成分を公平に扱って位置姿勢を算出すると、物体の回転中心軸まわりの姿勢パラメータが正しく求まらない。そこで、本実施形態では、まず６自由度の位置姿勢を算出した後で、物体の特定軸方向の姿勢パラメータのみ別途推定することで、高度な位置姿勢推定を行う。具体的には、まず、二次元画像上のエッジに対してモデルをあてはめることにより６自由度の位置姿勢を算出する。次に、モデル座標系における特定軸方向の姿勢を決める特徴の位置をもとにして、特定軸まわりの姿勢を算出する。
【０００９】
図１は、本実施形態における位置姿勢計測装置１として機能する情報処理装置の構成を示している。同図に示すように、位置姿勢計測装置１は、撮像部１１０、概略位置姿勢入力部１２０、三次元形状モデル入力部１３０、画像特徴検出部１４０、特定自由度設定部１５０、第一の姿勢算出部１６０、対応取得部１７０、第二の位置姿勢算出部１８０から構成されている。以下、位置姿勢計測装置１を構成する各部について説明する。
【００１０】
撮像部１１０は、カメラであり、計測対象である計測対象物体を撮像する。撮像により得られた画像は画像取得手段により取得される。カメラの内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は、公知の手法により、事前に校正しておく。
【００１１】
概略位置姿勢入力部１２０は、位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を入力する。位置姿勢計測装置１には、位置及び姿勢計測の基準となる３次元の座標系（基準座標系）が規定されているものとする。本実施形態では、撮像部１１０であるカメラのカメラ中心を原点とし、画像の水平方向をＸ軸、垂直方向をＹ軸、カメラの光軸をＺ軸とした撮像座標系を基準座標系（第一の座標系）とする。同様に、物体の３次元形状モデルにも、位置および姿勢計測の基準となる３次元の座標系（モデル座標系もしくは第二の座標系）が規定されているものとする。
【００１２】
位置姿勢計測装置１に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、位置姿勢計測装置１は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果をもとに、位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測してもよい。
【００１３】
また、物体が置かれているおおよその位置や姿勢があらかじめわかっている場合には、その値を概略値として用いる。概略位置及び姿勢は、第一の位置姿勢取得手段として機能する第一の位置姿勢算出部１６０において第一の位置姿勢を算出する際に用いられる。
【００１４】
３次元形状モデル入力部１３０は、計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段から、位置姿勢計測装置１によって位置及び姿勢を計測する対象物体の３次元形状モデルを入力する。図２は、本実施形態における３次元形状モデルを説明する図である。３次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示すように、観察対象物体の３次元形状モデルは点Ｐ１〜点Ｐ１５の１５点から構成される。また図２（ｂ）に示すように、観察対象物体１０の３次元形状モデルは線分Ｌ１〜Ｌ２１により構成されている。点Ｐ１〜点Ｐ１５は３次元座標値によって表される。線分Ｌ１〜Ｌ２１は、線分を構成する点のＩＤによって表される。さらに３次元形状モデルは、面の情報も保持する。各面は、各面を構成する点のＩＤによって表される。図２に示す３次元形状モデルでは、物体を構成する８つの面の情報が記憶されている。３次元形状モデルは、第一の位置姿勢算出部１６０において物体の第一の位置姿勢を算出する際と、第二の位置姿勢取得手段として機能する第二の位置姿勢算出部１８０において物体の第二の位置姿勢を算出する際に用いられる。
【００１５】
画像特徴検出部１４０は、撮像部１１０によって撮像された画像から画像内の画像特徴（幾何特徴）を検出する。本実施形態では画像特徴としてエッジを検出する。具体的には、概略位置姿勢入力部１２０から入力される物体の位置及び姿勢の概略値に基づいて、３次元形状モデルの各線分を画像上に投影し、投影された線分の近傍で対応するエッジ探索を行う。画像特徴は、第一の位置姿勢算出部１６０において、物体の第一の位置及び姿勢を算出する際に用いられる。
【００１６】
特定自由度設定部１５０は、第一の位置姿勢算出部１６０で推定する位置姿勢パラメータの成分、第二の位置姿勢算出部で推定する位置姿勢パラメータの成分をそれぞれ設定する。
【００１７】
尚、これ以後、第一の位置姿勢算出部１６０で推定する位置姿勢パラメータの成分を第一の位置姿勢パラメータとし、第二の位置姿勢算出部１８０で推定する位置姿勢パラメータの成分を第二の位置姿勢パラメータとする。
【００１８】
具体的には、基準座標系を基準とした物体の位置姿勢の６パラメータを第一の位置姿勢パラメータとして記録する。さらに、物体形状がほぼ回転体称であり、かつその軸回りの回転を決めるための特徴が少ないことに起因して、６パラメータのうち、算出が困難なパラメータをあらかじめ第二の位置姿勢パラメータとして記録しておく。
【００１９】
第一の位置姿勢算出部１６０は、画像特徴検出部１４０が検出した画像特徴を用いて、計測対象物体の第一の位置姿勢を算出する。
【００２０】
対応取得部１７０は、第一の位置姿勢に基づいて、第二の位置姿勢パラメータを算出する手がかりとなる、３次元形状モデルと撮像画像との幾何特徴の対応を取得する。取得した対応は、第二の位置姿勢算出部１８０において、第二の位置姿勢パラメータを推定する際に用いられる。
【００２１】
第二の位置姿勢算出部１８０は、第一の位置姿勢に基づき、対応取得部１７０によって得られた対応を利用して第二の位置姿勢を算出する。
【００２２】
図３は、第一の実施形態における物体の位置及び姿勢の計測の処理手順を示すフローチャートである。尚、位置姿勢計測装置１は、図３で示したステップＳ１１００〜ステップＳ１６００の一連の処理を繰り返すことで、時間軸方向に連続して位置姿勢の計測を行う。
【００２３】
（ステップＳ１１００）
特定自由度設定部１５０は、第一の位置姿勢パラメータと、第二の位置姿勢パラメータとを設定する。
【００２４】
図４（ａ）に、本実施形形態で対象とする物体の具体例とそのモデルの座標系を示す。
本実施形態では、図４（ａ）のように、物体がある軸に対してほぼ回転対称な形状を有することを前提とする。そこで、物体の回転対称性を定義する軸（以後、モデル中心軸と呼ぶ）を決定する。モデル中心軸の設定は、例えば、ユーザからの入力により行う。
【００２５】
具体的には、図４（ａ）のようなモデルの場合、モデルの回転対称な面上から対面する２点ａ、ｂおよびｃ、ｄの２組をユーザからの入力として取得する。このとき、線分ａｂと、線分ｃｄとの交点を通り、かつ四角形ａｃｂｄに垂直な方向をもつベクトルをモデル中心軸として算出する（図４（ｂ））。ただし、モデル中心軸の設定方法はこれに限るものではない。たとえば、もし三角柱のような回転対称な形状の場合には、ユーザからの入力として回転対称な三角形上の３点を取得する。そして、その３点が成す三角形の重心を通り、かつ三角形に垂直な方向をもつベクトルをモデル中心軸として算出する。
【００２６】
あるいは、モデル面上で回転対称性のある面（図４（ａ）中では八角形）をユーザからの入力として取得し、その重心を通り、かつ入力された面に直交する直線をモデル中心軸としてもよい。その他、モデル形状の回転対称性を定義する回転中心軸を算出できる他の手法を用いても良い。
【００２７】
次に、モデル中心軸に対して互いに直交する２つの軸を設定する。ここで、モデル中心軸と、設定した２つの軸とが座標軸となるようにモデル座標系を変換して新しいモデル座標系とする（図４（ｃ））。
【００２８】
ここでは、新しいモデル座標系において、Ｙ軸（実線）がモデル中心軸と一致するようにする。ただし、モデル座標系の変換方法はこれに限るものではない。例えば、モデル座標系のＸ軸がモデル中心軸と一致するように変換を行ってもよい。そのほか、モデル中心軸がモデル原点を通るようにモデル座標系が変換されていれば、いかなる変換であっても構わない。なお、以後の処理は、新しいモデル座標系を用いて行うものとする。基準座標系に対する物体の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。ここで、ｔｘ、ｔｙ、ｔｚはそれぞれＸ、Ｙ、Ｚ軸方向の位置（並進）成分を、θ、φ、ψはそれぞれＸ、Ｙ、Ｚ軸に対する姿勢（回転）成分を表す。この各回転成分の変換順序を考慮することで、座標変換において、物体軸回りの回転を分離する。この６パラメータを第一の位置姿勢パラメータとして記録する。さらに、モデル中心軸と一致する座標軸に対する回転成分を第二の位置姿勢パラメータとして記録する。本実施形態では、Ｙ軸がモデル中心軸と一致するため、Ｙ軸回りの回転成分であるφを第二の位置姿勢パラメータとして記録する。
【００２９】
（ステップＳ１２００）
撮像部１１０により、計測対象物体の二次元画像を撮像し、画像特徴検出部１４０に入力する。
【００３０】
（ステップＳ１３００）
位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を概略位置姿勢入力部１２０により位置姿勢計測装置１に入力する。本実施形態では、前の時刻において計測された位置及び姿勢を用いる。
【００３１】
（ステップＳ１４００）
二次元画像の情報に基づいて物体の第一の位置姿勢パラメータの算出を行う。本実施形態では、二次元画像上の特徴であるエッジに３次元形状モデル中の線分の投影像が当てはまるように計測対象物体の位置及び姿勢を算出する。図５は、本ステップにおける第一の位置姿勢算出の処理手順を示すフローチャートである。ここでは、画像上で検出されたエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和が最小になるように位置及び姿勢のパラメータｓの最適化を行う。このとき、位置姿勢に基づく、モデル座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換を数１のように表すものとする。
【００３２】
Ｘｃ＝Ｒ Ｘｍ ＋ Ｔ ・・・［数１］
ただしＲは物体の姿勢を表す３つの成分θ、φ、ψで表現される３×３行列、Ｔはモデルの位置を表す３つの成分から成る３次元ベクトルＴ＝［ｔｘ， ｔｙ， ｔｚ］Ｔである。さらに、概略位置姿勢に対する物体の位置及び姿勢の微小変化量を６次元ベクトルΔｓ ＝［Δｔｘ，Δｔｙ，Δｔｚ，Δθ，Δφ，Δψ］Ｔとし、これらの成分により表される姿勢の微小変化量をΔＲ、位置の微小変化量をΔＴとする。このとき、位置及び姿勢が微小変化したときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換を、数２のように表す。
【００３３】
Ｘｃ＝ＲΔＲ Ｘｍ ＋ Ｔ＋ΔＴ ・・・［数２］
本ステップでは、微小変化量Δｓを求めて、各成分の補正を行う処理を繰り返すことで、位置及び姿勢の算出を行う。以下、位置及び姿勢算出の処理手順について順に説明する。
【００３４】
（ステップＳ１４１０）
初期化処理を行う。ここでは、ステップＳ１３００で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【００３５】
（ステップＳ１４２０）
ステップＳ１２００において入力された二次元画像上において画像特徴の検出・画像特徴とモデルとの対応付けを行う。本ステップは画像特徴検出部１４０において行われる。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。エッジの検出方法としては、種々の方法が提案されているため、詳細な説明は省く。
【００３６】
図６は、本実施形態におけるエッジ検出を説明する図である。まず、ステップＳ１３００において入力された計測対象物体の位置及び姿勢の概略値と、校正済みのカメラの内部パラメータを用いて、３次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は画像上でも線分となる。次に、画像上で等間隔になるように投影線分上に制御点を設定し、制御点を通りかつ投影された線分に直交する線分上で１次元のエッジ検出を行う（図６（ａ））。エッジは画素値の濃度勾配の極値として検出されるため、図６（ｂ）に示すように、近傍にエッジが存在する場合には複数のエッジが検出されることがある。本実施形態では、検出されるエッジのうち最も濃度勾配の大きいものを対応するエッジとする。
【００３７】
（ステップＳ１４３０）
モデルの線分上の制御点と画像上のエッジとの対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。図７は、線分の投影像と検出されたエッジの関係を説明する図である。図７では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。位置及び姿勢の概略値ｓ０に基づいて投影されたモデルの線分上のある制御点の画像上での位置を（ｕ０，ｖ０）、該制御点が所属する線分の画像上での傾きをｕ軸に対する傾きθとして表す。
【００３８】
傾きθは、モデルの線分の両端の３次元座標をｓの概略値（ｓ０）に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該線分の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、該制御点の対応点（エッジ）の画像座標を（ｕ’，ｖ’）とする。ここで、点（ｕ’，ｖ’）を通り、傾きがθである直線（図７の破線）上の点（ｕ，ｖ）は、
【００３９】
【数１】
【００４０】
と表せる。なお、θは定数とする。ここで、
【００４１】
【数２】
【００４２】
（定数）である。制御点の画像上での位置は計測対象物体の位置及び姿勢ｓにより変化する。したがって、制御点の画像座標（ｕ，ｖ）は、（ｕ０，ｖ０）の近傍での１次のテイラー展開によって数４のように近似できる。但しΔｓｉ（ｉ＝１，２，・・・，６）はｓの各成分の微小変化を表す。つまり、Δｓ１＝Δｔｘ、Δｓ２＝Δｔｙ、Δｓ３＝Δｔｚ、Δｓ４＝Δθ、Δｓ５＝Δφ、Δｓ６＝Δψである。
【００４３】
【数３】
【００４４】
正しいｓによって得られる制御点の画像上での位置が数１によって表される直線上にあると仮定すると、数４を数３に代入することにより、観測方程式（数５）が得られる。
【００４５】
【数４】
【００４６】
ただし、
【００４７】
【数５】
【００４８】
（定数）である。数５の観測方程式は対応付けが行われたすべての制御点について立式することができる。ここで、数３はｓの各成分の微小変化Δｓｉ（ｉ＝１，２，・・・，６）についての方程式になっているため、数６に示すΔｓｉに関する線形連立方程式を立てることができる。
【００４９】
【数６】
【００５０】
ここで、正規化画像座標系における画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃで表される。したがって、これらの数２に基づいて算出したｕ、ｖから、左辺の行列の各要素を算出する。
【００５１】
この線形連立方程式を数７のように表す。
【００５２】
Ｊ１Δｓ＝Ｅ ・・・［数７］
左辺のＪ１が求める係数行列であり、右辺のＥが誤差ベクトルである。
【００５３】
（ステップＳ１４４０）
数５をもとに、行列Ｊ１の一般化逆行列（Ｊ１Ｔ・Ｊ１）−１・Ｊ１Ｔを用いて位置及び姿勢の補正値Δｓを最小二乗基準で求める。しかしながら、エッジの対応付けでは誤対応（外れ値）が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは数４右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制する。重みは例えば数８に示すようなＴｕｋｅｙの関数により与える。
【００５４】
【数７】
【００５５】
ｃ１は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であればなんでもよい。各計測データに対応する重みをｗｉとする。ここで数９のように重み行列Ｗを定義する。
【００５６】
【数８】
【００５７】
重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗｉが入る。この重み行列Ｗを用いて、数７を数１０のように変形する。
【００５８】
【数９】
【００５９】
数１０を数１１のように解くことにより補正値Δｓを求める。
【００６０】
【数１０】
【００６１】
（ステップＳ１４５０）
ステップＳ１４４０において算出された位置及び姿勢の補正値Δｓにより、位置及び姿勢の概略値を補正する。
【００６２】
ｓ←ｓ＋Δｓ
（ステップＳ１４６０）
収束判定を行い、収束していれば終了し、そうでなければステップＳ１４２０に戻る。収束判定では、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。
【００６３】
以上の処理によって、ステップＳ１３００で入力された位置及び姿勢の概略値が二次元画像の情報を用いて更新される。本ステップで最終的に算出された位置姿勢を第一の位置姿勢とする。
【００６４】
（ステップＳ１５００）
第一の位置姿勢に基づき、３次元形状モデル面上の点（モデル特徴点）に対応する二次元画像上の座標（画像特徴点）を取得する。本ステップにおいて、モデル座標系における特定軸方向の姿勢を決める特徴対応を別途取得することで、特定軸まわりの姿勢を高精度に算出可能となる。
【００６５】
まず、３次元形状モデルの面上から、特定自由度方向のパラメータの算出に寄与する幾何特徴を選択し、その３次元座標を記録する。図２に示すモデルにおいて、ｐ１３〜ｐ１５が特定自由度方向のパラメータを一意に決める特徴となる。このモデル特徴点の選択は事前準備としてステップＳ１１００の前にオフラインで行っておく。その際には、３次元形状モデルを自由な姿勢で観測できるツールを用いて、特定自由度方向のパラメータを一意に決める手がかりとなる幾何特徴を人手で指定したものを記録しておき、本ステップで読み出せばよい。あるいは、オンラインで人手による入力を介して指定してもよい。この場合は、第一の位置姿勢に基づいて撮像画像上にレンダリングした３次元形状モデルの中から、モデルの特定自由度方向のパラメータを一意に決める手がかりとなる幾何特徴が描画されている画素を人手により指定する。そして指定した画素に描画されているモデル面の３次元座標を取得することによりモデル特徴点を得る。
【００６６】
次に、モデル特徴点に対応する画像特徴点を撮像画像から決定する。具体的には、第一の位置姿勢に基づき、３次元形状モデルおよび選択されたモデル特徴点の描画を行う。この画像と撮像画像とを比較して、モデル特徴点に対応する画素を撮像画像上で人手により指定することで画像特徴点を得る。ただし、モデル特徴点と画像特徴点との対応取得方法はこれに限るものではない。例えば、モデル特徴点に該当する部位の画像パターンをテンプレートとして用意しておき、撮像画像とのパターンマッチングにより対応する画像特徴点を取得してもよい。具体的には、まず３次元形状モデルと合わせて、モデルの姿勢を一意に決める手がかりとなる特徴点の三次元座標も保持しておき、それを読み込んでモデル特徴点とする。さらに、撮像装置に対する対象物体の姿勢を変化させてあらかじめ複数の視点から画像を撮影し、モデル特徴点の周辺部位のパターン画像を複数保持しておく。そして、撮像画像と各パターン画像とのパターンマッチングにより、もっとも類似する画素の位置を取得することで、対応する画像特徴点を得る。このとき、第一の位置姿勢に基づき、物体を特定自由度方向へ回転させた姿勢で撮影したパターン画像を候補としてパターンマッチングを行うことで、効率よく画像特徴点を探索する。
【００６７】
以上の処理により、モデル特徴点に対応する画像特徴点を取得する。なお、モデル特徴点と画像特徴点との対応は最低一組必要だが、上述の処理を複数のモデル特徴点に対して行うことで、対応を複数取得してもよい。
【００６８】
（ステップＳ１６００）
ステップＳ１４００で算出された第一の位置姿勢は、特定自由度に関するパラメータφを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータφのみに関する最適化を行い、位置姿勢を算出する処理を行う。
【００６９】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。ここで、モデル座標系を基準としたＹ軸方向に対するΔφの微小回転を表す３×３行列をΔＲｙとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向にΔφだけ微小回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数１２のように表される。
【００７０】
Ｘｃ＝ＲΔＲｙ Ｘｍ ＋ Ｔ ・・・［数１２］
そこで、本ステップでは、微小変化量Δφを求めて、モデルのＹ軸回転方向の補正を行うことで位置及び姿勢の算出を行う。
【００７１】
図８は、本ステップにおける第二の位置姿勢算出の処理手順を示すフローチャートである。ここでは、画像上で検出された特徴点の座標と、第一の位置姿勢に基づいて画像上に投影されるモデル特徴点の座標との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。
【００７２】
（ステップＳ１６１０）
初期化処理を行う。ここでは、ステップＳ１４００で得られた第一の位置姿勢を、位置及び姿勢算出における概略値として入力する。
【００７３】
（ステップＳ１６２０）
モデル特徴点と画像特徴との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。図９は、モデル特徴点の投影像と、対応づいた画像特徴との関係を説明する図である。図９では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。位置及び姿勢の概略値φ０に基づいて投影されたモデル特徴点の画像上での位置を（ｕ０，ｖ０）として表す。また、該モデル特徴点に対応する画像特徴の画像座標を（ｕ’，ｖ’）とする。ここで、モデル点の画像上での位置は計測対象物体の微小変化φにより変化する。したがって、モデル特徴点を画面上に投影した座標（ｕ’，ｖ’）は、（ｕ０，ｖ０）の近傍での１次のテイラー展開によって数１０のように近似できる。ただし、Δφはφの指定した軸方向の微小変化を表す。
【００７４】
【数１１】
【００７５】
数１３の観測方程式を対応付けが行われたすべての点群データについて立式して整理すると数１４のように表される。
【００７６】
【数１２】
【００７７】
ただし、ｕｉ’、ｖｉ’はモデル特徴点を投影した二次元座標、ｕｉ０ ｖｉ０はモデル特徴点に対応する画像特徴の二次元座標を意味する（ｉ＝１・・・ｎ、ｎ：特徴点数））。ここで、左辺の行列の値は、以下のように求める。まず、回転中心軸ベクトル［ｕｘ，ｕｙ，ｕｚ］Ｔに対する回転αを表す回転行列ｒは以下の数１５により表される。
【００７８】
【数１３】
【００７９】
ただし、ｃ ＝ ｃｏｓα、ｓ ＝ ｓｉｎαである。ここで、αが微小なとき、ｃｏｓα≒１、ｓｉｎα≒αであることを利用すると、Ｙ軸に対する微小回転を表す行列ΔＲｙは以下の数１６で近似される。
【００８０】
【数１４】
【００８１】
ただし［ｕｘ，ｕｙ，ｕｚ］Ｔ＝［０，１，０］Ｔである。以上より、数１２は以下のように表される。
【００８２】
【数１５】
【００８３】
【数１６】
【００８４】
正規化画像座標系における画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃで表される。したがって、これらをφで偏微分することで各値を算出する。
各行列成分を算出した数１４の線形連立方程式を数１９のように表す。
【００８５】
Ｊ２Δφ＝Ｅ２ ・・・［数１９］
左辺のＪ２が求める係数行列であり、右辺のＥ２が誤差ベクトルである。
【００８６】
（ステップＳ１６３０）
行列Ｊ２の一般化逆行列（Ｊ２Ｔ・Ｊ２）−１・Ｊ２Ｔを用いて、数１９を数２０のように解くことで、位置及び姿勢の補正値Δφを最小二乗基準で求める。
【００８７】
Δφ＝（Ｊ２Ｔ・Ｊ２）−１・Ｊ２ＴＥ２ ・・・［数２０］
（ステップＳ１６４０）
ステップＳ１６３０において算出された位置及び姿勢の補正値Δφにより、位置及び姿勢の概略値を補正する。
【００８８】
φ←φ＋Δφ
（ステップＳ１６５０）
収束判定を行い、収束していれば終了し、そうでなければステップＳ１６２０に戻る。収束判定では、補正値Δφがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。
【００８９】
以上述べたように、第一の実施形態では、特定の軸に対する回転体形状あるいは回転対称な形状を有する物体の位置姿勢推定を行う場合について述べた。まず、モデルと画像とのエッジ対応を利用して一度物体の位置及び姿勢を推定し、その後、モデルと画像との特徴点対応を利用して画像上の二次元距離を最小化することでモデルの特定軸回転方向に限定した位置合わせを行う。モデルの特定軸回転方向のみを後段で推定することで、特定自由度方向の類似形状によりパラメータが求まりにくい物体においても、ロバストかつ高精度に位置姿勢の計測が可能となる。
【００９０】
（第二の実施形態）
第一の実施形態では、第一の位置姿勢算出において、まず物体の６自由度パラメータを最適化し、その後第二の位置姿勢算出で、特定自由度であるモデル座標系のＹ軸回転パラメータのみ最適化を行った。これに対して、本実施形態では、第一の位置姿勢算出においては、モデル座標系のＹ軸回転成分以外の５自由度のパラメータのみ最適化する。第一の位置姿勢算出において、推定が困難な自由度成分を削減することで、計算量を減らして効率よく位置姿勢を算出する。なお、本実施形態において、基本的な構成は第一の実施例と同様であるため説明を省略する。以下、本実施形態における各処理部について説明する。
【００９１】
尚、撮像部１１０、概略位置姿勢入力部１２０、三次元形状モデル入力部１３０、画像特徴検出部１４０、第一の姿勢算出部１６０、対応取得部１７０、第二の位置姿勢算出部１８０については、実施例１と同様のため、説明を省略する。
【００９２】
特定自由度設定部１５０は、第一の位置姿勢パラメータ、第二の位置姿勢パラメータをそれぞれ設定する。本実施形態においても、第一の実施形態と同様に、基準座標系を基準とした物体の位置姿勢の６パラメータのうち、モデルの形状に起因して算出が困難なパラメータを第二の位置姿勢パラメータとして記録しておく。ただし、第一の位置姿勢パラメータには、モデル座標系を基準とした位置姿勢の６パラメータのうち、第二の位置姿勢パラメータとして記録したパラメータ以外のパラメータを記録する。
【００９３】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００とステップＳ１４００である。したがって、ステップＳ１１００とステップＳ１４００の説明を行い、その他の処理の説明は省略する。
【００９４】
（ステップＳ１１００）
第一の実施形態と同様の方法でモデル中心軸に対して互いに直交する２つの軸を設定し、モデル中心軸と、設定した２つの軸とが座標軸となるようモデル座標系を変換して新しいモデル座標系とする。その後、変換後のモデル座標系を基準とした位置姿勢パラメータを表す６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔのうち、モデル中心軸まわりの回転成分であるφを第二の位置姿勢パラメータ、それ以外のパラメータを第一の位置姿勢パラメータとして記録する。
【００９５】
（ステップＳ１４００）
二次元画像の情報に基づいて物体の位置及び姿勢の計測を行う。本実施形態においても、画像上で検出されたエッジと、推定される位置及び姿勢に基づいて画像上に投影されるモデルの線分との距離の二乗和が最小になるように位置及び姿勢のパラメータの最適化を行う。ただし、位置姿勢パラメータであるｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔのうち、第二の位置姿勢算出部で推定するφを除いた５つのパラメータｔｘ，ｔｙ，ｔｚ，θ，ψを推定する。ここで、数１により表される、モデル座標系から基準座標系への変換において、姿勢変換を表す行列Ｒを各軸回りの回転に分解することで数２１のように表すことができる。
【００９６】
Ｘｃ＝ＲｘＲｚＲｙ Ｘｍ ＋ Ｔ ・・・［数２１］
ここでＲｘ、Ｒｚ、Ｒｙはそれぞれモデル座標系のＸ軸回転θ、Ｙ軸回転φ、Ｚ軸回転ψを表す行列である。
【００９７】
ここで、概略位置姿勢に対する物体の位置及び姿勢の微小変化量を５次元ベクトルΔｓ ＝［Δｔｘ，Δｔｙ，Δｔｚ，Δθ，Δψ］Ｔとする。Ｘ軸微小回転Δθを表す回転行列をΔＲｘ、Ｚ軸微小回転Δψを表す回転行列をΔＲｚ、位置の微小変化量をΔＴ＝［Δｔｘ，Δｔｙ，Δｔｚ］Ｔとすると、モデル座標系において、５つのパラメータｔｘ，ｔｙ，ｔｚ，θ，ψを微小変化させたときの物体座標系Ｘｍから基準座標系Ｘｃへの座標変換は、数２２のように表される。
【００９８】
Ｘｃ＝ＲｘΔＲｘＲｚΔＲｚＲｙ Ｘｍ ＋ Ｔ＋ΔＴ ・・・［数２２］
実施例１と同様の方法で、この変換式に基づいて微小変化量Δｓを求め、各位置姿勢成分の補正を行うことで、第一の位置及び姿勢の算出を行う。
【００９９】
以上述べたように、本実施形態では、第一の位置姿勢算出においては、第二の位置姿勢算出で推定する自由度のパラメータは算出しない。第一の位置姿勢算出において推定する自由度成分を削減することで、より効率よく位置姿勢の算出が可能となる。
【０１００】
（第三の実施形態）
第一の実施形態では、二次元画像を利用した位置姿勢推定について述べた。本実施形態では、第一の実施形態と同様の物体に対して距離画像を利用して位置姿勢推定を行う例について述べる。なお、本実施形態において、基本的な構成は第一の実施例と同様であるため説明を省略する。以下、本実施形態における各処理部について説明する。なお、概略位置姿勢入力部１２０、三次元形状モデル入力部１３０、特定自由度設定部１５０、対応取得部１７０については、第一の実施形態と同様のため、説明を省略する。
【０１０１】
撮像部１１０は、距離画像を撮像する距離センサである。距離画像は、各画素が奥行きの情報を持つ画像である。距離センサとしては、対象に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。
【０１０２】
画像特徴検出部１４０は、撮像部１１０によって撮像された画像から対応する幾何特徴を検出する。本実施形態では幾何特徴として三次元点を検出してモデル頂点と対応付ける。具体的には、概略位置姿勢入力部１２０から入力される物体の位置及び姿勢の概略値に基づいて、３次元形状モデルの各モデル頂点を画像上に投影し、モデル頂点が投影された画素に対応する距離値からカメラ座標系の三次元座標を算出する。三次元座標が算出された三次元点を投影されたモデル頂点に対応する画像特徴とする。画像特徴は、第一の位置姿勢算出部１６０において、第一の位置姿勢算出部１６０において物体の第一の位置及び姿勢を算出する際に用いられる。
【０１０３】
第一の位置姿勢算出部１６０は、画像特徴検出部１４０が検出した画像特徴を用いて、計測対象物体の第一の位置姿勢を算出する。本実施形態では、モデルの三次元面と、撮像画像から得られる点との距離を最小化することで第一の位置姿勢を算出する。
【０１０４】
対応取得部１７０は、第一の位置姿勢に基づいて、第二の位置姿勢パラメータを算出する手がかりとなる、３次元形状モデルと撮像画像との幾何特徴の対応を取得する。取得した対応は、第二の位置姿勢算出部１８０において、第二の位置姿勢パラメータを推定する際に用いられる。
【０１０５】
第二の位置姿勢算出部１８０は、第一の位置姿勢に基づき、対応取得部１７０によって得られた対応を利用して第二の位置姿勢を算出する。本実施形態では、モデル上の三次元点と、撮像画像から得られる点との距離を最小化することで第二の位置姿勢を算出する。
【０１０６】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。なお、ステップＳ１１００、ステップＳ１３００、ステップＳ１４１０、ステップＳ１４４０、ステップＳ１４５０、ステップＳ１４６０、ステップＳ１６１０、ステップＳ１６３０、ステップＳ１６４０、ステップＳ１６５０については第一の実施形態と同様であるため、説明を省略する。
【０１０７】
（ステップＳ１２００）
撮像部１１０により、計測対象物体の距離画像を撮像し、画像特徴検出部１４０に入力する。
【０１０８】
（ステップＳ１４００）
距離画像の情報に基づいて物体の位置及び姿勢の計測を行う。本実施形態では、距離画像上の特徴である三次元点に３次元形状モデルの投影像が当てはまるように計測対象物体の位置及び姿勢を算出する。本ステップにおける第一の位置姿勢算出の処理手順は、図５と同様であるため省略する。ここでは、距離画像から変換して得られる三次元点と、推定される位置及び姿勢に基づいて画像上に投影される３次元形状モデルの面との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。以下の説明では、物体の位置及び姿勢を、実施例１と同様に６次元ベクトルｓで表す。以下、位置及び姿勢算出の処理手順について順に説明する。なお、実施例１と処理が同一のものについては、説明を省略する。
【０１０９】
（ステップＳ１４２０）
ステップＳ１２００において入力された距離画像とモデルとの対応付けを行う。以下、距離画像とモデルとの対応付け処理について説明する。図１０は、本ステップにおける距離画像とモデルとの対応付け処理を説明する図である。本ステップでは、公知の手法により物体の３次元形状モデル（ポリゴンモデル）を距離画像上に投影することによって対応付けを行う。モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデルのどのポリゴンに対応するかを決定するためである。距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと位置及び姿勢の概略値を用いてポリゴンモデルを描画する。以下、ポリゴンモデルを描画した画像をポリゴン描画画像と呼ぶ。描画する際には、各ポリゴンに固有の色を割り当てて描画を行う。ポリゴンの色はＲＧＢの各色で表す。背景部分と区別するために、黒（Ｒ＝０、Ｇ＝０、Ｂ＝０）以外の色を用いてポリゴンを描画する。これによって、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデルを描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている３次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。
【０１１０】
（ステップＳ１４３０）
３次元形状モデルのポリゴンと距離画像から変換した３次元点との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。以下、距離画像の画素（３次元の点）と面との対応に基づく係数行列・誤差ベクトルの算出方法について説明する。
【０１１１】
３次元空間における平面の方程式は一般にａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１）の形で表される。ここでは、３次元形状モデル中のある面の計測物体の座標系における平面の方程式をａｘ＋ｂｙ＋ｃｚ＝ｅであるとする。この平面の方程式は、位置及び姿勢の概略値に基づいて、基準座標系における平面の方程式ａ’ｘ＋ｂ’ｙ＋ｃ’ｚ＝ｅ’に変換できる。位置及び姿勢の概略値に基づく、モデル座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換を数２３のように表す。
【０１１２】
Ｘｃ＝Ｒ Ｘｍ ＋ Ｔ ・・・［数２３］
ただしＲは姿勢を表す３×３回転行列、Ｔは位置を表す３次元ベクトルである。このとき、基準座標系における平面の方程式の係数は次式のようになる。
【０１１３】
【数１７】
【０１１４】
基準座標系における３次元座標の計測値が（ｘ’，ｙ’，ｚ’）である点Ｐが３次元形状モデル中のある面Ｆと対応付けられているとする。面Ｆに点Ｐから下ろした垂線の足をＱとする。点Ｑの基準座標系における３次元座標（ｘ，ｙ，ｚ）は、計測対象物体の位置及び姿勢ｓによって変換するものであり、ある位置及び姿勢ｓのときの３次元座標（ｘ０，ｙ０，ｚ０）の近傍で１次のテイラー展開によって数２５のように近似できる。
【０１１５】
【数１８】
【０１１６】
正しいｓによって変換される（ｘ，ｙ，ｚ）は、法線ベクトルが面Ｆと同じで点Ｐを通る平面（ａ’ｘ＋ｂ’ｙ＋ｃ’ｚ＝ｅ’， ｅ’＝ａ’ｘ’＋ｂ’ｙ’＋ｃ’ｚ’） 上にあると仮定すると、数２１を平面の方程式に代入することにより数２６が得られる。
【０１１７】
【数１９】
【０１１８】
ただし、
【０１１９】
【数２０】
【０１２０】
（定数）とした。数２６の観測方程式は対応付けが行われたすべての点群データについて立式することができる。
【０１２１】
数２５及び数２６の観測方程式はｓの各成分の微小変化Δｓｉ（ｉ＝１，２，・・・，６）についての方程式になっており、数１２に示すΔｓｉに関する線形連立方程式を立てることができる。
【０１２２】
【数２１】
【０１２３】
この線形連立方程式を数２８のように表す。
【０１２４】
ＪΔｓ ＝ Ｅ ・・・［数２８］
左辺のＪが求める係数行列であり、右辺のＥが誤差ベクトルである。
【０１２５】
（ステップＳ１５００）
第一の位置姿勢に基づき、３次元形状モデル面上の点（モデル特徴点）に対応する距離画像上の三次元座標（画像特徴点）を取得する。なお、モデル特徴点は、実施例１と同様の方法で指定して記録しておく。次に、モデル特徴点に対応する画像特徴点を撮像した距離画像から決定する。具体的には、第一の位置姿勢に基づき、距離画像面上に３次元形状モデルおよび記録されているモデル特徴点の描画を行う。この画像と撮像画像とを比較して、モデル特徴点に対応する画素を撮像画像上で人手により指定する。その画素の距離値を三次元点に変換することで画像特徴点を得る。
【０１２６】
ただし、モデル特徴点と画像特徴点との対応取得方法はこれに限るものではない。たとえば、実施例１と同様に、モデル特徴点に該当する部位の距離画像パターンを保持して置き、撮像画像とのパターンマッチングにより対応する画像特徴点を取得してもよい。以上の処理により、モデル特徴点に対応する画像特徴点を取得する。なお、モデル特徴点と画像特徴点との対応は最低一組必要だが、上述の処理を複数のモデル特徴点に対して行うことで、対応を複数取得してもよい。
【０１２７】
（ステップＳ１６２０）
モデル特徴点と画像特徴点との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。本実施例では、第一の位置および姿勢を位置及び姿勢の概略値として，概略位置姿勢に基づいて基準座標系へ変換したモデル特徴点の三次元座標を（ｘｃ０，ｙｃ０，ｚｃ０）として表す。また、該モデル特徴点に対応する画像特徴の三次元座標を（ｘｃ，ｙｃ，ｚｃ）とする。ここで、モデル点の画像上での位置は計測対象物体の微小変化φにより変化する。したがって、モデル特徴点を画面上に投影した座標（ｘｃ，ｙｃ，ｚｃ）は、（ｘｃ０，ｙｃ０，ｚｃ０）の近傍での１次のテイラー展開によって数２９のように近似できる。ただし、ΔφはＹ軸方向の回転φの微小変化を表す。
【０１２８】
【数２２】
【０１２９】
数２９の観測方程式を対応付けが行われたすべての点群データについて立式する。
【０１３０】
【数２３】
【０１３１】
ただし、ｘｉ’、ｙｉ’ 、ｚｉ’はモデル特徴点の三次元座標、ｘｉ０ 、ｙｉ０ 、ｚｉ０はモデル特徴点に対応する画像特徴の三次元元座標を意味する（ｉ＝１・・・ｎ、ｎ：特徴点数））。この線形連立方程式を数３１のように表す。
【０１３２】
Ｊ２Δφ＝Ｅ２ ・・・［数３１］
左辺のＪ２が求める係数行列であり、右辺のＥ２が誤差ベクトルである。ここで求めた方程式を第一の実施形態のステップＳ１６３０と同様の方法で解くことで、第二の位置姿勢を算出する。
【０１３３】
以上述べたように、本実施形態では、第一の実施形態と同様の物体に対して距離画像を利用して位置姿勢推定を行う例について述べた。まず、モデルと画像との３次元点の対応を利用して一度物体の位置及び姿勢を推定し、その後、モデルと画像との特徴点対応を利用して三次元距離を最小化することでモデルの特定軸回転方向に限定した位置合わせを行う。
【０１３４】
なお、二次元画像と距離画像を同時に取得することが可能な場合には、第一の位置姿勢を算出する前段の位置合わせ、第二の位置姿勢を算出する後段の位置合わせにおいて、二次元画像を利用する手法と三次元画像を利用する手法をそれぞれ切り替えても良いし、同時に利用しても良い。
【０１３５】
（第四の実施形態）
第一の実施形態では、第二の位置姿勢算出において、非線形最適化により、パラメータの補正を繰り返すことで第二の位置姿勢算出を行った。これに対して、ステップＳ１６１０で得られる３次元形状モデルと撮像特徴との幾何特徴の対応から方程式を立て、この方程式を解くことで直接パラメータを算出してもよい。本実施形態では、第二の位置姿勢算出において、特徴対応から直接パラメータを算出して第二の位置姿勢を算出する方法について述べる。
【０１３６】
なお、本実施形態における位置姿勢計測装置１の構成は、第一の実施形態と同様のため、説明を省略する。
【０１３７】
続いて、各処理について説明する。本実施例においては、基本的な処理フローは図３、図５と同様である。実施例１と異なる処理は、ステップＳ１６００のみであるため、その他の処理は説明を省略し、ステップＳ１６００について説明を行う。
【０１３８】
（ステップＳ１６００）
ステップＳ１４００で算出された第一の位置姿勢は、特定自由度に関するパラメータφを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータφに関する方程式を立て、これを解くことで第二の位置姿勢を算出する。
【０１３９】
第一の実施形態で示したように、第一の位置姿勢を概略位置姿勢としてモデル座標系のＹ軸方向にφだけ回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３２のように表される。
【０１４０】
【数２４】
【０１４１】
ただし、ｃ＝ｃｏｓφ、ｓ＝ｓｉｎφである。ここで概略位置姿勢からφだけ回転させたモデル特徴点を画面上に投影したときの正規化画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃにより算出される。このｕ、ｖが対応する画像特徴ｕｏ、ｖｏと等しい、という方程式を立てると、ｃ＝ｃｏｓφ、ｓ＝ｓｉｎφの２変数に関する以下のような方程式が立てられる。
【０１４２】
【数２５】
【０１４３】
ここで、Ａは２×２の行列、ｂは２次元ベクトルである。この方程式を解いて算出したｓｉｎφとｃｏｓφからｔａｎφを計算し、ａｔａｎφによりφを求める。
【０１４４】
以上述べたように、本実施形態では、第二の位置姿勢算出において、特徴対応から直接パラメータを算出して第二の位置姿勢を算出する方法について述べた。この方法では、第二の位置姿勢の算出で繰り返し処理を必要としないため、高速に位置姿勢を算出可能である。
【０１４５】
（第五の実施形態）
第一の実施形態では、特定の軸に対してほぼ回転対称な形状を有する物体の位置姿勢推定を行った。本実施形態では、モデル座標系の特定軸の並進方向を決める特徴が少ない物体の位置姿勢推定について述べる。このような物体では、位置姿勢の６自由度成分を公平に扱って位置姿勢を算出すると、モデルの特定軸並進方向のパラメータが正しく求まらない。そこで、位置姿勢を算出した後で、モデルの特定軸方向の並進パラメータのみ後段で推定する。本実施形態では、まず、エッジの位置合わせにより物体の位置姿勢を算出し、その後、モデルの特定軸の並進パラメータを推定することで位置および姿勢を計測する。
【０１４６】
なお、本実施形態における位置姿勢計測装置１の構成は、第一の実施形態と同様のため、説明を省略する。
【０１４７】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００、ステップＳ１６００ステップＳ１６２０である。したがって、その他の処理は説明を省略し、ステップＳ１１００とステップＳ１６００、ステップＳ１６２０の説明を行う。
【０１４８】
（ステップＳ１１００）
第一の位置姿勢パラメータと、第二の位置姿勢パラメータを設定する。図１１（ａ）に、本実施形態で対象とするモデルの具体例とその座標系を示す。本実施形態では、図１１（ａ）のように、モデルがある軸方向に対して並進方向を決める特徴が少ない形状であることを前提とする。このモデルに対して、モデルの並進方向を定義する軸（以後、モデル中心軸と呼ぶ）を決定する。モデル中心軸の設定は、たとえばユーザからの入力により行う。具体的には、図１１（ａ）のような四角柱形状のモデルの場合、四角柱の相対する２つの面を結ぶモデルのエッジ線分上から２点ａ、ｂをモデル中心軸として記録する（図１１（ｂ））。ただし、モデル中心軸の設定方法はこれに限るものではない。たとえば、モデル面上の頂点から主軸位置と方向を求め、これらにより表されるベクトルをモデル中心軸としてもよい。そのほか、モデル形状の回転対称性を定義する回転中心軸を算出できればいかなる方法であっても構わない。
【０１４９】
次に、モデル中心軸と互いに直交する２つの軸を設定する。ここで、モデル中心軸と、設定した２つの軸とが座標軸となるようにモデル座標系を変換して定義し直す（図１１（ｃ））。変換後のモデル座標系において、Ｙ軸（実線）がモデル中心軸と一致している。ただし、モデル座標系の変換方法はこれに限るものではない。たとえばモデル座標系のＸ軸がモデル中心軸と一致するように変換を行ってもよい。そのほか、モデル中心軸の方向が、モデルの類似形状の方向と同一になるようモデル座標系が変換されていれば、いかなる変換であっても構わない。なお、以後の処理は、変換後のモデル座標系を用いて行うものとする。変換後のモデル座標系を基準として、第一の実施形態と同様に、位置姿勢計測装置１の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。この６パラメータを第一の位置姿勢パラメータとして記録する。さらに、モデル中心軸と一致する座標軸に対する並進成分を第二の位置姿勢パラメータとして記録する。本実施例では、Ｙ軸がモデル中心軸と一致するため、Ｙ軸方向の並進成分であるｔｙを第二の位置姿勢パラメータとして記録する。
【０１５０】
（ステップＳ１６００）
ステップＳ１４００により算出された第一の位置姿勢は、特定自由度に関するパラメータｔｙを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータｔｙのみに関する最適化を行い、位置姿勢を算出する処理を行う。
【０１５１】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。ここで、モデル座標系を基準としたＹ軸方向に対するΔｔｙの微小並進を表す３次元ベクトルをΔＴ ＝ ［０， ｔｙ， ０］Ｔとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向に微小並進させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３４のように表される。
【０１５２】
Ｘｃ＝Ｒ （Ｘｍ＋ΔＴ）＋ Ｔ ・・・［数３４］
そこで、本ステップでは、微小変化量Δｔｙを求めて、モデル座標系Ｙ軸方向の並進パラメータの補正を行うことで位置及び姿勢の算出を行う。
【０１５３】
ここでは、第一の実施形態と同様に、画像上で検出された特徴点の座標と、第一の位置姿勢に基づいて画像上に投影されるモデル特徴点の座標との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。
【０１５４】
（ステップＳ１６２０）
モデル特徴点と画像特徴との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。本実施例においても、数１４のφをｔｙに置き換えた形で、同様の関係が成立する。
ここで、数３４は以下のように表される。
【０１５５】
【数２６】
【０１５６】
【数２７】
【０１５７】
正規化画像座標系における画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃで表される。したがって、これらをｔｙで偏微分することで、φをｔｘで置き換えた、数１４と同様の線形連立方程式を立てることができる。この方程式を実施例１と同様の方法で解き、パラメータを更新することで第二の位置姿勢を算出する。
【０１５８】
以上、本実施形態では、モデル座標系の特定軸の並進方向に対して類似形状を有する物体の位置姿勢推定について述べた。一度物体の位置姿勢を算出し、その後、モデルの特定軸の並進パラメータを推定することで、特定軸並進方向のパラメータも正しく位置姿勢を計測する。なお、変形例１−１と同様に、第一の位置姿勢算出では、ｔｙ以外の５パラメータのみの推定を行っても良い。
【０１５９】
（第六の実施形態）
前述の実施形態では、モデル座標系の特定１自由度のパラメータの推定が困難な形状を持つ物体の位置姿勢推定方法について述べた。本実施形態では、モデル座標系を基準とした特定軸に対する回転、並進の２自由度に関するパラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べる。
【０１６０】
なお、本実施形態における位置姿勢計測装置１の構成は、実施例１と同様のため、説明を省略する。
【０１６１】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００、ステップＳ１６００ステップＳ１６２０である。したがって、その他の処理は説明を省略し、ステップＳ１１００とステップＳ１６００、ステップＳ１６２０の説明を行う。
【０１６２】
（ステップＳ１１００）
第一の位置姿勢パラメータと、第二の位置姿勢パラメータを設定する。本実施例では、図１２のように、モデルが特定軸方向に対してほぼ回転対称かつその特定軸の並進方向成分を決める特徴が少ない形状であることを前提とする。
【０１６３】
このモデルに対して、モデル中心軸を設定する。モデル中心軸の設定は、たとえばユーザからの入力により行う。回転対称性を定義する軸に対して垂直なモデル面（図１２中では円）の輪郭線上の４点ａ、ｂ、ｃ、ｄをユーザからの入力として取得する。このとき、線分ａｂの垂直二等分線と、線分ｃｄの垂直二等分線との交点を通り、四角形ａｂｃｄに垂直な方向をもつベクトルをモデル中心軸として算出する（図４（ｂ））。ただし、モデル中心軸の設定方法はこれに限るものではない。たとえば、第一の実施形態と同様に、回転対称な面をユーザからの入力として取得し、その面の中心を通り、かつモデル面に直交する直線をモデル中心軸としてもよい。そのほか、モデル形状の回転対称性を定義する回転中心軸を算出できればいかなる方法であっても構わない。
【０１６４】
さらに、モデル中心軸と、モデル中心軸に対して互いに直交する２つの軸とが座標軸となるようにモデル座標系を変換して定義し直す。図１２は、上述の処理による変換後のモデルを示している。変換後のモデル座標系において、Ｙ軸（実線）がモデル中心軸と一致している。ここで、変換後のモデル座標系を基準として、実施例１と同様に、位置姿勢計測装置１の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。この６パラメータを第一の位置姿勢パラメータとして記録する。さらに、モデル中心軸と一致する座標軸に対する回転成分、並進成分を第二の位置姿勢パラメータとして記録する。本実施例では、Ｙ軸がモデル中心軸と一致するため、Ｙ軸方向回転成分であるφ、Ｙ軸方向並進成分であるｔｙを第二の位置姿勢パラメータとして記録する。
【０１６５】
（ステップＳ１６００）
ステップＳ１４００により算出された第一の位置姿勢は、特定自由度に関するパラメータφ、ｔｙを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータφおよびｔｙに関する最適化を行い、位置姿勢を算出する処理を行う。
【０１６６】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。
【０１６７】
ここで、モデル座標系を基準としたＹ軸方向に対するΔφの微小回転を表す３×３行列をΔＲｙ、Ｙ軸方向に対するΔｔｙの微小並進を表す３次元ベクトルをΔＴ ＝ ［０， ｔｙ， ０］Ｔとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向に微小回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３０のように表される。
【０１６８】
Ｘｃ＝Ｒ （ΔＲｙＸｍ＋ΔＴ）＋ Ｔ ・・・［数３７］
微小変化量Δφ、Δｔｙを求めて、モデル座標系Ｙ軸方向の回転、並進パラメータの補正を行うことで位置及び姿勢の算出を行う。
【０１６９】
（ステップＳ１６２０）
数３７の変換に基づき、第一の実施形態と同様の方法で、線形連立方程式の係数行列を算出する。この方程式は第一の実施形態と同様の方法で解き、パラメータを更新することで第二の位置姿勢を算出する。
【０１７０】
以上、本実施形態では、モデル座標系を基準とした特定軸に対する回転、並進の２自由度に関するパラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べた。なお、第二の実施形態と同様に、第一の位置姿勢算出では、φ、ｔｙ以外の４パラメータのみの推定を行っても良い。
【０１７１】
（第七の実施形態）
第六の実施形態では、モデル座標系を基準とした特定軸に対する回転、並進の２自由度に関するパラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べた。本変形例では、球あるいは正多面体のような形状で、かつモデル座標系における回転３自由度のパラメータに寄与する特徴が少ない物体の位置姿勢推定方法について述べる。
【０１７２】
なお、本実施例における位置姿勢計測装置１の構成は、第一の実施形態と同様のため、説明を省略する。
【０１７３】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００、ステップＳ１５００、ステップＳ１６００ステップＳ１６２０である。したがって、その他の処理は説明を省略し、ステップＳ１１００、ステップＳ１５００、ステップＳ１６００、ステップＳ１６２０の説明を行う。
【０１７４】
（ステップＳ１１００）
第一の位置姿勢パラメータと、第二の位置姿勢パラメータを設定する。本実施形態では、物体が、球のような、３自由度の回転に対する対称性を有することを前提とする。このモデルに対して、モデルの３自由度の回転を定義する原点がモデル座標系の原点となるようモデル座標系を変換して定義し直す。モデルの回転を定義する原点の設定は、たとえばモデルを包含するＢｏｕｎｄｉｎｇ Ｂｏｘの中心座標を利用する。そして、この原点を通り、互いに直交する３つの軸を設定し、この３つの軸がモデル座標系となるよう変換を行う。
【０１７５】
ここで、変換後のモデル座標系を基準として、第一の実施形態と同様に、位置姿勢計測装置１の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。この６パラメータを第一の位置姿勢パラメータとして記録する。そして、モデルのＸ軸回転θ、Ｙ軸回転φ、Ｚ軸回転ψを第二の位置姿勢パラメータとして記録する。
【０１７６】
（ステップＳ１５００）
第一の位置姿勢に基づき、３次元形状モデル面上の点（モデル特徴点）に対応する二次元画像上の座標（画像特徴点）を取得する。本実施形態でも第一の実施形態と同様の方法でモデルと画像との対応を取得する。本実施形態では、第二の位置姿勢算出において、３つのパラメータを推定する。二次元画像上の対応一組につき方程式が２つ得られるため、少なくとも２組の対応を取得する。
【０１７７】
（ステップＳ１６００）
ステップＳ１４００により算出された第一の位置姿勢は、θ、φ、ψを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、第二の位置姿勢パラメータθ、φ、ψに関する最適化を行い、位置姿勢を算出する処理を行う。
【０１７８】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。
【０１７９】
ここで、モデル座標系を基準としたＸ軸方向に対するΔθの微小回転を表す３×３行列をΔＲｘ、Ｙ軸方向に対するΔφの微小回転を表す３×３行列をΔＲｙ、Ｚ軸方向に対するΔψの微小回転を表す３×３行列をΔＲｚとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向に微小回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３８のように表される。
【０１８０】
Ｘｃ＝ＲｘΔＲｘ ＲｙΔＲｙ ＲｚΔＲｚＸｍ＋ Ｔ ・・・［数３８］
ここで、Ｒｘ、Ｒｙ、Ｒｚはそれぞれ数３１の行列Ｒを各軸回りの回転に分解したものであり、Ｒ＝ＲｘＲｙＲｚの関係が成り立つ。
【０１８１】
本ステップでは、微小変化量Δθ、Δφ、Δψを求めて、モデル座標系の回転パラメータの補正を行うことで位置及び姿勢の算出を行う。
【０１８２】
（ステップＳ１６２０）
数３８の変換に基づき、第一の実施形態と同様の方法で、線形連立方程式の係数行列を算出する。この方程式を第一の実施形態と同様の方法で解き、パラメータを更新することで第二の位置姿勢を算出する。
【０１８３】
以上、本実施形態では、モデル座標系を基準とした３自由度もしくは３自由度以下の回転パラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べた。なお、第二の実施形態と同様に、第一の位置姿勢算出では、θ、φ、ψ以外の３パラメータのみの推定を行っても良い。
【０１８４】
なお、球面上に特徴点が一点存在するような場合、その特徴点と球の中心とを結ぶ軸に対する回転パラメータは不定となる。このような形状に対しては、回転パラメータを２つとして、本実施形態と同様の方法で推定を行っても良い。
【０１８５】
（他の実施形態）
第二の位置姿勢パラメータの組み合わせは、これまで述べた方法に限るものではない。たとえば、薄い板のような平らな面１枚に、微小な特徴を有するような物体においては、物体面に垂直な軸に対する回転パラメータおよび物体面内の並進２パラメータの推定が困難である。そのため、このような物体に対して、これらの３パラメータを第二の位置姿勢パラメータとして推定を行っても良い。その他、推定が困難なパラメータをモデル座標系の特定自由度として分離できれば、どんな組み合わせであっても構わない。また、上記実施形態で説明された位置姿勢計測装置は、その機能のすべてもしくは一部を、ＣＰＵ（セントラルプロセッシングユニット）、コンピュータプログラムが格納されたＲＡＭ（ランダムアクセスメモリ）などから構成されるコンピュータで代用することが可能である。
【０１８６】
（第八の実施形態）
第一の実施形態および第六の実施形態では、ステップＳ１１００において、ユーザからの入力によりモデル中心軸の決定を行った。本実施形態では、物体の３次元設計ＣＡＤデータから、モデル中心軸を自動で決定する方法について述べる。一般的に、モデル全体の形状がほぼ回転対称な場合、回転対称な解析曲面の回転対称軸の多くが、モデル全体形状の回転対称軸と一致する。そこで、物体の３次元設計ＣＡＤデータを構成する解析曲面群の中から、回転対称な面形状の回転中心軸を抽出し、多数の回転対称面について、位置と方向とが一致する回転対称軸をモデル中心軸として設定する。
【０１８７】
図１３は、本実施形態におけるモデル中心軸決定部２の構成を示している。同図に示すように、モデル軸中心軸決定部２は、３次元ＣＡＤデータ保持部２１０、基準面選択および基準幾何情報抽出部２２０、参照面選択および参照幾何情報抽出部２３０、類似度算出部２４０、評価値算出部２５０、モデル幾何情報決定部２６０から構成されている。なお、モデル中心軸決定部２は、図１における特定自由度設定部１５０に、モデル中心軸を入力する。以下、モデル中心軸決定部２を構成する各部について説明する。
【０１８８】
３次元ＣＡＤデータ保持部２１０は、計測対象物体の３次元ＣＡＤデータを保持する。３次元ＣＡＤデータには、たとえば対象物体の設計時のデータを利用する。ここで、３次元ＣＡＤデータは、平面、Ｂ−Ｓｐｌｉｎｅ−曲面、トーラス曲面、円柱、円錐、円、といったいずれかの属性を持つ解析曲面を、輪郭線で切り取った、トリム曲面の集合により構成されるものとする。各解析曲面はその面形状を表現する解析パラメータを保持するものとする。なお、３次元ＣＡＤデータと、位置合わせに利用する三次元形状モデルの座標系は、同一のものであるとする。ただし、３次元ＣＡＤデータの座標系と位置合わせに利用する三次元形状モデルの座標系との相対位置関係が分かっていれば同一でなくてもかまわない。また、解析曲面の集合により形状を表現する３次元ＣＡＤデータであれば、リバースエンジニアリングにより生成したものを利用してもよい。
【０１８９】
基準面選択および基準幾何情報抽出部２２０は、３次元ＣＡＤデータを構成する複数の解析曲面の中から、回転対称軸の類似性を比較する際の基準となる解析曲面を１つ選択する。なお、回転対称な面のみを選択し、これ以外の面は選択しない。抽出した解析曲面を以後、基準面と呼ぶ。さらに、基準面選択および基準幾何情報抽出部２２０は、基準面の解析曲面パラメータから、面に固有の軸を抽出する。各解析曲面は、その面をＵＶパラメータ空間で表現するために、ＵＶ座標系を定義する２つの軸の、モデル座標系における位置と方向とをパラメータとして保持する。２つの軸とは、たとえば、解析曲面が回転対称であれば回転中心軸とそれに直交する軸、平面であれば面に平行な互いに直交する２軸がこれに相当する。そこで、この２軸のうち、回転中心軸を表すものを抽出する。抽出した軸を以後、基準軸と呼ぶことにする。
【０１９０】
参照面選択および参照幾何情報抽出部２３０は、３次元ＣＡＤデータを構成する複数の解析曲面の中から、基準面選択および基準幾何情報抽出部で抽出した解析曲面とは異なる解析曲面を１つ選択し、選択した解析曲面に固有の軸を抽出する。ここで抽出した面および軸を、以後参照面および参照軸と呼ぶことにする。
【０１９１】
類似度算出部２４０は、基準軸と参照軸のパラメータから、基準軸と、参照軸との類似度を算出する。
【０１９２】
評価値算出部２５０は、基準軸に対する各参照軸の類似度に基づいて、基準軸について、類似度の高い回転対称軸の多さを評価する評価値をそれぞれ算出する。
【０１９３】
モデル幾何情報決定部２６０は、各基準軸に対して算出した評価値に基づいて、モデル中心軸を決定する。
【０１９４】
図１４は、第八の実施形態におけるモデル軸決定の手順を示すフローチャートである。
【０１９５】
（ステップＳ２１００）
３次元ＣＡＤデータ保持部２１０により対象物体の３次元ＣＡＤデータを入力する。
【０１９６】
（ステップＳ２２００）
３次元ＣＡＤデータを構成する複数の解析曲面の中から、モデル軸決定に利用するための解析曲面を抽出する。本実施例では、円、円柱、円錐のいずれかの属性を持つ解析曲面を抽出する。なお、回転対称な形状であれば、いかなる解析曲面を抽出してもかまわない。ここで、抽出した解析曲面の数をＭとする。
【０１９７】
（ステップＳ２３００）
基準面選択および基準幾何情報抽出部２２０により、ステップＳ２２００で抽出したＭ個の解析曲面の中から基準面を選択し、基準軸の抽出を行う。まず、Ｍ個の解析曲面の中から１つ、基準とする解析曲面をランダムに選択する。選択した解析曲面を基準面Ｆｉ（ｉ＝１〜Ｍ）とする。次に、基準面Ｆｉを表現する解析曲面パラメータの中から、面の回転中心軸を、基準軸Ａｉとして抽出する。ここで、Ａｉはモデル座標系における始点位置を表す３次元座標ｐｉ、方向を表す３次元ベクトルｎｉにより表されるものとする。
【０１９８】
（ステップＳ２４００）
参照面選択および参照幾何情報抽出部２３０により、ステップＳ２２００で抽出したＭ個の解析曲面の中から、参照面および参照軸の抽出を行う。まず、Ｍ個の解析曲面のうち、基準面とは異なるものから１つ、参照する解析曲面を選択する。選択した解析曲面を参照面Ｆｊ（ｊ＝１〜Ｍ、ただしｊ≠ｉ）とする。さらに、ステップＳ２３００と同様に、参照面Ｆｊを表現する解析曲面パラメータから、回転中心軸をＡｊとして抽出する。なお、参照軸の始点位置を表す３次元座標をｐｊ、方向をあらわす３次元ベクトルをｎｊとする。
【０１９９】
（ステップＳ２５００）
類似度算出部２４０により、基準軸Ａｉと参照軸Ａｊのパラメータから、基準軸Ａｉに対する参照軸Ａｊとの類似度を算出する。具体的には、２軸間の距離類似度と、方向類似度とを算出する。まず、基準軸と参照軸との始点間距離をｄ１、参照軸の始点を基準軸へ投影した際の基準軸の始点との距離をｄ２として、距離類似度αｉｊを以下のように定義し、算出する。
【０２００】
【数２８】
【０２０１】
ここで、
【０２０２】
【数２９】
【０２０３】
【数３０】
【０２０４】
である。
【０２０５】
次に、基準軸と参照軸との方向の類似度βｉｊを以下のように定義し、算出する。
【０２０６】
【数３１】
【０２０７】
なお、類似度αｉｊ、類似度βｉｊは値が大きいほど２つの軸が類似していることを意味する。ここで、算出したｄ１、αｉｊ、βｉｊをそれぞれｉ、ｊとともに記録しておく。
【０２０８】
（ステップＳ２６００）
ステップＳ２２００で抽出したＭ個の解析曲面のうち、基準面以外に、参照面として未選択のものが存在すれば、ステップＳ２４００へ戻り、存在しなければステップＳ２６００へ進む。
【０２０９】
（ステップＳ２７００）
評価値算出部２５０により、基準軸と各参照軸との類似度に基づき、類似度が閾値以上であった参照軸を持つ参照面の面積の和を評価値として算出する。まず、基準軸Ａｉと各参照軸Ａｊとの類似度αｉｊ、βｉｊに基づいて、以下の条件をともに満たすｊを類似する軸を保有する面のＩＤとして記録する。
【０２１０】
θ＜αｉｊ、 φ＜βｉｊ ・・・［数４１］
ここで、θ、φはともに閾値である。θにはたとえばθ＝ｄ１×１０、φにはたとえばφ＝０．９を設定する。次に、記録しておいたＩＤに基づき、各参照面Ｆｊの表面積Ｓｊを求め、その総和を基準軸Ａｉの評価値Ｖｉとして算出する。
ただし、評価値の算出方法はこれに限るものではない。たとえば、αｉｊあるいはβｉｊのどちらか一方の条件のみ満たすもののＩＤを類似する軸とみなしてもよい。あるいは、閾値を利用せずに評価値Ｖｉ＝Σ（Ｓｊ×βｉｊ）としてもよい。あるいは、類似度が閾値以上であった面の総数を評価値としてもよい。その他、２つの軸の類似性を評価できる尺度であれば、これ以外のものであってもかまわない。
【０２１１】
（ステップＳ２８００）
ステップＳ２２００で抽出したＭ個の解析曲面のうち、基準面として未選択のものが存在すれば、ステップＳ２３００へ戻り、存在しなければステップＳ２９００へ進む。この結果、各基準面Ｆｉの軸に対する評価値Ｖｉを得る。
【０２１２】
（ステップＳ２９００）
モデル幾何情報決定部２６０により、各評価値Ｖｉの中から、もっとも大きい評価値をもつ基準軸Ａｉをモデル中心軸として決定する。あるいは、評価値の高い基準軸の中から上位数個を候補として抽出し、三次元ＣＡＤデータの形状と基準軸とを重畳させて画面上に表示し、ＧＵＩによりユーザに選択させてもよい。具体的には、モデル全体の表面積Ｓに対する評価値Ｖｉの割合を閾値ｔｈｒｅｓｈｏｌｄとして設定しておき（たとえばｔｈｒｅｓｈｏｌｄ＝０．１）、以下の関係を満たすものだけを抽出して提示する。
【０２１３】
Ｖｉ ／ Ｓ ＞ ｔｈｒｅｓｈｏｌｄ ・・・［数４０］
このとき、評価値の高い基準軸から順に抽出し、逐次、抽出された基準軸との類似度が閾値以上の参照軸に関しては、次の抽出候補から除外することで、類似した軸のみが候補として複数提示されるのを防いでもよい。
【０２１４】
以上、本実施形態では、物体の３次元設計ＣＡＤデータを構成する解析曲面群の中から、回転対称な面形状の回転対称軸を抽出し、多数の回転対称面について、位置と方向とが一致する回転対称軸をモデル中心軸として設定する方法について述べた。
【０２１５】
なお、回転対称軸を表現する始点位置の３パラメータおよび方向の３パラメータにより表される６自由度空間を適当な大きさに分割してｂｉｎを設定しておき、各解析曲面の回転対称軸の６パラメータに基づいてｂｉｎに投票を行って、類似するものを選別してもよい、この場合、もっとも投票の多かったｂｉｎに投票した全回転対称軸の平均値をとることで、モデル中心軸を決定する。このほか、回転対称な各解析曲面の回転対称軸に対して、方向と位置とが多数類似するものを決定できれば、上に述べた以外のいかなる方法であってもかまわない。
【０２１６】
また、本実施例では、ＣＡＤデータの解析曲面から回転対称な面を抽出したが、解析曲面でなくても正多角形あるいは正多角柱など、回転対称なプリミティブを抽出して同様の方法をとってもよい。
【０２１７】
（第九の実施形態）
第五の実施形態では、ステップＳ１１００において、ユーザからの入力あるいはモデル全体形状の主軸方向に基づいて、特定の１軸並進成分を規定するモデル中心軸の決定を行った。本実施形態では、第八の実施例と同様に、対象物体の３次元設計ＣＡＤデータから、モデルの特定の並進成分を規定するモデル中心軸を設定する方法について述べる。
【０２１８】
特定の１軸方向に関する並進パラメータの推定が困難な物体の場合、モデルを構成する個々の解析曲面のうち、相対的に細長い平面がモデル全体の形状において支配的に分布していることが期待される。そこで、物体の３次元設計ＣＡＤデータを構成する解析曲面群の中から平面を抽出し、その平面の重心を始点としてその平面の長軸方向へ向かうベクトルをモデル中心軸候補となる軸として抽出する。この軸との位置と方向とが一致する軸をモデル中心軸として設定する。
【０２１９】
なお、本実施形態におけるモデル幾何情報決定部２の構成は、第八の実施形態と同様のため、説明を省略する。ただし、本実施例では、基準面選択および基準幾何情報抽出部２２０、参照面選択および参照幾何情報抽出部２３０では、３次元ＣＡＤデータを構成する複数の解析曲面の中から、モデル中心軸の決定に利用する際の解析曲面として、平面のみをそれぞれ抽出する。なお、平面以外の解析曲面については選択しない。さらに、面に固有の軸として、その面の重心を始点として、面の長軸方向へと向かうベクトルを抽出する。
【０２２０】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図１４と同様である。第八の実施形態と異なる処理は、ステップＳ２２００、ステップＳ２３００、ステップＳ２４００である。したがって、その他の処理は説明を省略し、ステップＳ２２００、ステップＳ２３００、ステップＳ２４００の説明を行う。
【０２２１】
（ステップＳ２２００）
３次元ＣＡＤデータを構成する複数の解析曲面の中から、モデル軸決定に利用するための解析曲面を抽出する。本実施例では、平面のいずれかの属性を持つ解析曲面を抽出する。なお、曲面を持たず、平面のみで構成される面形状であれば、いかなる解析曲面を抽出してもかまわない。ここで、抽出した解析曲面の数をＭとする。
【０２２２】
（ステップＳ２３００）
抽出したＭ個の解析曲面の中から基準面を選択し、基準軸の抽出を行う。まず、Ｍ個の解析曲面の中から１つ、基準とする解析曲面をランダムに選択する。選択した解析曲面を基準面Ｆｉ（ｉ＝１〜Ｍ）とする。次に、基準面Ｆｉを表現する解析曲面パラメータから、面の重心および面に直交するベクトルを求め、重心位置を表す３次元座標をｐｉ、重心位置を始点として面の長軸方向に向かう３次元ベクトルをｎｉとする。ただし、軸の抽出方法はこの限りではない。
【０２２３】
（ステップＳ２４００）
抽出したＭ個の解析曲面の中から、参照面および参照軸の抽出を行う。まず、Ｍ個の解析曲面のうち、基準面とは異なるものから１つ、参照する解析曲面を選択する。選択した解析曲面を参照面Ｆｊ（ｊ＝１〜Ｍ、ただしｊ≠ｉ）とする。さらに、ステップＳ２３００と同様に、参照面Ｆｊを表現する解析曲面パラメータから、参照軸の始点位置を表す３次元座標をｐｊ、方向をあらわす３次元ベクトルをｎｊとして抽出する。
【０２２４】
以上、本実施形態では、３次元設計ＣＡＤデータを構成する解析曲面のうち、平面に関して、その平面の重心を始点として平面の長軸方向に向かうベクトルを軸としてそれぞれ抽出し、この軸の方向と位置とが多数類似するものを、モデル中心軸として設定した。
【０２２５】
なお、ステップＳ２３００において、ｎｉには、面の重心位置から長軸方向へと向かうベクトルの代わりに、面の重心位置を始点として、面に直交するベクトルを用いてもよい。このとき、ステップＳ２４００では、ステップＳ２３００と軸の抽出方法を統一し、面の重心位置を表す３次元座標をｐｉ、重心位置を始点として面に直交する３次元ベクトルをｎｉとして抽出する。さらに、ステップＳ２９００では、各評価値Ｖｉの中から、もっとも大きい評価値をもつ基準軸Ａｉの基準面Ｆｉの重心を始点としてその面の長軸方向に向かうベクトルをモデル中心軸として決定する。
【０２２６】
なお、第七の実施形態で述べたような、球形状の物体における回転３自由度を規定する中心点に関しても、同様の方法で決定可能である。この場合には、ＣＡＤデータを構成する解析曲面群から、球形状の解析曲面を複数抽出し、各面の中心点との距離が多数類似度する点をモデル中心点として決定する。あるいはＣＡＤデータから正多面体の形状を複数抽出して同様に中心点を求めることで、多数の中心点と類似する点をモデル中心点として決定してもよい。
【産業上の利用可能性】
【０２２７】
本発明は、ロボットの自己位置推定や、ロボットと物体との間の相対的な位置及び姿勢を推定するのに利用することができる。
【符号の説明】
【０２２８】
１ 位置姿勢計測装置
１１０ 撮像部
１２０ 概略位置姿勢入力部
１３０ 三次元形状モデル入力部
１４０ 画像特徴検出部
１５０ 特定自由度設定部
１６０ 第一の位置姿勢算出部
１７０ 対応取得部
１８０ 第二の位置姿勢算出部
２ モデル中心幾何情報決定部
２１０ ＣＡＤデータ保持部
２２０ 基準面選択および基準幾何情報抽出部
２３０ 参照面選択および参照幾何情報抽出部
２４０ 類似度算出部
２５０ 評価値算出部
２６０ モデル幾何情報決定部
【技術分野】
【０００１】
本発明は、三次元形状を有する計測対象物体の位置姿勢を取得する技術に関する。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、工業製品の組立のような人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。ロボットが、ハンドなどのエンドエフェクタによって部品を把持して組立を行うためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置姿勢を計測する必要がある。
物体の位置姿勢を計測する方法として、カメラで撮影した濃淡画像やカラー画像（以下、総称して二次元画像と呼ぶ）上から検出される特徴を用いる方法がある。
また、距離センサで画像様に計測して得られた距離データに対して、物体の三次元形状モデルをあてはめる方法がある。このような手法は、モデルフィッティングと呼ばれる。
モデルフィッティングによる位置姿勢推定を行う場合、物体の形状によっては位置姿勢を高精度に計測できないことがある。
例えば、距離データから算出される物体の重心位置と重心まわりのモーメントをモデルと比較する方法では、対象物体のアスペクト比が１に近いような場合に、撮像装置の座標系において、位置姿勢の６パラメータを精度よく推定するのが困難になる。
特許文献１では、距離画像に対するモデルフィッティングにおいて、撮像装置の座標系を基準とした位置姿勢パラメータの推定を複数の段階に分けることで、推定の安定化と効率化を図る方法が開示されている。
特許文献１に開示されている方法では、撮像装置の座標系を基準とした位置姿勢の６自由度成分を、高精度かつロバストに推定できるパラメータから順に推定する。まず、撮像画像の画像座標軸回転の２パラメータと視軸方向位置の１パラメータを推定した後、視軸に垂直な位置の２パラメータを推定し、最後に視軸回転の１パラメータを推定する。パラメータの推定手順を段階的に分けることで、ロバストかつ高精度に位置姿勢の計測を行うことが可能になる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特登録０３２５３４３０
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１でも指摘されているように、物体（モデル）座標系を基準とした位置姿勢の６自由度において、推定が困難な自由度成分を持つような物体が存在する。例えば、工業製品に使われる部品の中には、回転対称に近い形状であって、その回転方向の姿勢成分を一意に決定するための微小な切り欠きを有する部品が存在する。
このような部品では、画像とモデルとのフィッティングにより、位置姿勢の６パラメータを同時に推定しようとすると、ほかの自由度成分に比べて、特定自由度（この場合モデルの回転対称性を定義する軸まわりの姿勢）成分の推定に寄与する情報が不足する。そのため、特定自由度方向のパラメータを推定するのが困難となり、その結果、位置姿勢のロバスト性や精度が低くなるという問題がある。
特許文献１に開示されている方法では、撮像装置の座標系を基準とした位置姿勢の６自由度成分を、高精度かつロバストに推定できるパラメータから順に推定する。つまり、モデル座標系を基準とした特定自由度成分のみを分離して推定するわけではなく、物体の回転中心軸が撮像装置の視軸に一致する場合を除いて、物体の回転中心軸回りの姿勢成分の高い推定精度を得ることが出来ない。
本発明は、以上の課題を鑑みてなされたものであり、モデル座標系を基準とした位置姿勢の自由度の内に、一意に定めるのが困難な特定自由度のパラメータを有する物体においても、ロバストかつ高精度な位置姿勢の計測を実現することを目的とする。
【課題を解決するための手段】
【０００５】
上記課題を鑑み、本発明は、計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、前記計測対象物体の画像を取得する画像取得手段と、前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする。
【発明の効果】
【０００６】
本発明によれば、モデル座標系を基準とした位置姿勢の自由度の内に、一意に定めるのが困難な特定自由度のパラメータを有する物体においても、ロバストかつ高精度な位置姿勢の計測を実現することが出来る。
【図面の簡単な説明】
【０００７】
【図１】第一の実施形態における位置姿勢計測装置の構成を示す図である。
【図２】第一の実施形態における三次元形状モデルを説明する図である。
【図３】第一の実施形態における位置姿勢計測処理のフローチャートである。
【図４】第一の実施形態における第一の位置姿勢パラメータおよび第二の位置姿勢パラメータの設定を説明する図である。
【図５】ステップＳ１４００における第一の位置姿勢算出処理のフローチャートである。
【図６】画像からのエッジ検出を説明する図である。
【図７】線分の投影像と検出されたエッジの関係を説明する図である。
【図８】ステップＳ１６００における第二の位置姿勢算出処理のフローチャートである。
【図９】モデル特徴点の投影像と、対応する画像特徴を説明する図である。
【図１０】ステップＳ１２２０における距離画像とモデルとの対応付け処理を説明する図である。
【図１１】第二の実施形態における第一の位置姿勢パラメータおよび第二の位置姿勢パラメータの設定を説明する図である。
【図１２】変形例２−１における第一の位置姿勢パラメータおよび第二の位置姿勢パラメータの設定を説明する図である。
【図１３】第八の実施形態におけるモデル中心幾何情報決定部の構成を示す図である。
【図１４】第八の実施形態におけるモデル中心幾何情報決定処理のフローチャートである。
【発明を実施するための形態】
【０００８】
［第一の実施形態］
第一の実施形態では、物体における特定の軸に対してほぼ回転対称であり、かつその軸回りの回転を決めるための特徴が少ない物体の位置姿勢推定を行う。このような物体では、位置姿勢の６自由度成分を公平に扱って位置姿勢を算出すると、物体の回転中心軸まわりの姿勢パラメータが正しく求まらない。そこで、本実施形態では、まず６自由度の位置姿勢を算出した後で、物体の特定軸方向の姿勢パラメータのみ別途推定することで、高度な位置姿勢推定を行う。具体的には、まず、二次元画像上のエッジに対してモデルをあてはめることにより６自由度の位置姿勢を算出する。次に、モデル座標系における特定軸方向の姿勢を決める特徴の位置をもとにして、特定軸まわりの姿勢を算出する。
【０００９】
図１は、本実施形態における位置姿勢計測装置１として機能する情報処理装置の構成を示している。同図に示すように、位置姿勢計測装置１は、撮像部１１０、概略位置姿勢入力部１２０、三次元形状モデル入力部１３０、画像特徴検出部１４０、特定自由度設定部１５０、第一の姿勢算出部１６０、対応取得部１７０、第二の位置姿勢算出部１８０から構成されている。以下、位置姿勢計測装置１を構成する各部について説明する。
【００１０】
撮像部１１０は、カメラであり、計測対象である計測対象物体を撮像する。撮像により得られた画像は画像取得手段により取得される。カメラの内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は、公知の手法により、事前に校正しておく。
【００１１】
概略位置姿勢入力部１２０は、位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を入力する。位置姿勢計測装置１には、位置及び姿勢計測の基準となる３次元の座標系（基準座標系）が規定されているものとする。本実施形態では、撮像部１１０であるカメラのカメラ中心を原点とし、画像の水平方向をＸ軸、垂直方向をＹ軸、カメラの光軸をＺ軸とした撮像座標系を基準座標系（第一の座標系）とする。同様に、物体の３次元形状モデルにも、位置および姿勢計測の基準となる３次元の座標系（モデル座標系もしくは第二の座標系）が規定されているものとする。
【００１２】
位置姿勢計測装置１に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、位置姿勢計測装置１は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果をもとに、位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測してもよい。
【００１３】
また、物体が置かれているおおよその位置や姿勢があらかじめわかっている場合には、その値を概略値として用いる。概略位置及び姿勢は、第一の位置姿勢取得手段として機能する第一の位置姿勢算出部１６０において第一の位置姿勢を算出する際に用いられる。
【００１４】
３次元形状モデル入力部１３０は、計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段から、位置姿勢計測装置１によって位置及び姿勢を計測する対象物体の３次元形状モデルを入力する。図２は、本実施形態における３次元形状モデルを説明する図である。３次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図２（ａ）に示すように、観察対象物体の３次元形状モデルは点Ｐ１〜点Ｐ１５の１５点から構成される。また図２（ｂ）に示すように、観察対象物体１０の３次元形状モデルは線分Ｌ１〜Ｌ２１により構成されている。点Ｐ１〜点Ｐ１５は３次元座標値によって表される。線分Ｌ１〜Ｌ２１は、線分を構成する点のＩＤによって表される。さらに３次元形状モデルは、面の情報も保持する。各面は、各面を構成する点のＩＤによって表される。図２に示す３次元形状モデルでは、物体を構成する８つの面の情報が記憶されている。３次元形状モデルは、第一の位置姿勢算出部１６０において物体の第一の位置姿勢を算出する際と、第二の位置姿勢取得手段として機能する第二の位置姿勢算出部１８０において物体の第二の位置姿勢を算出する際に用いられる。
【００１５】
画像特徴検出部１４０は、撮像部１１０によって撮像された画像から画像内の画像特徴（幾何特徴）を検出する。本実施形態では画像特徴としてエッジを検出する。具体的には、概略位置姿勢入力部１２０から入力される物体の位置及び姿勢の概略値に基づいて、３次元形状モデルの各線分を画像上に投影し、投影された線分の近傍で対応するエッジ探索を行う。画像特徴は、第一の位置姿勢算出部１６０において、物体の第一の位置及び姿勢を算出する際に用いられる。
【００１６】
特定自由度設定部１５０は、第一の位置姿勢算出部１６０で推定する位置姿勢パラメータの成分、第二の位置姿勢算出部で推定する位置姿勢パラメータの成分をそれぞれ設定する。
【００１７】
尚、これ以後、第一の位置姿勢算出部１６０で推定する位置姿勢パラメータの成分を第一の位置姿勢パラメータとし、第二の位置姿勢算出部１８０で推定する位置姿勢パラメータの成分を第二の位置姿勢パラメータとする。
【００１８】
具体的には、基準座標系を基準とした物体の位置姿勢の６パラメータを第一の位置姿勢パラメータとして記録する。さらに、物体形状がほぼ回転体称であり、かつその軸回りの回転を決めるための特徴が少ないことに起因して、６パラメータのうち、算出が困難なパラメータをあらかじめ第二の位置姿勢パラメータとして記録しておく。
【００１９】
第一の位置姿勢算出部１６０は、画像特徴検出部１４０が検出した画像特徴を用いて、計測対象物体の第一の位置姿勢を算出する。
【００２０】
対応取得部１７０は、第一の位置姿勢に基づいて、第二の位置姿勢パラメータを算出する手がかりとなる、３次元形状モデルと撮像画像との幾何特徴の対応を取得する。取得した対応は、第二の位置姿勢算出部１８０において、第二の位置姿勢パラメータを推定する際に用いられる。
【００２１】
第二の位置姿勢算出部１８０は、第一の位置姿勢に基づき、対応取得部１７０によって得られた対応を利用して第二の位置姿勢を算出する。
【００２２】
図３は、第一の実施形態における物体の位置及び姿勢の計測の処理手順を示すフローチャートである。尚、位置姿勢計測装置１は、図３で示したステップＳ１１００〜ステップＳ１６００の一連の処理を繰り返すことで、時間軸方向に連続して位置姿勢の計測を行う。
【００２３】
（ステップＳ１１００）
特定自由度設定部１５０は、第一の位置姿勢パラメータと、第二の位置姿勢パラメータとを設定する。
【００２４】
図４（ａ）に、本実施形形態で対象とする物体の具体例とそのモデルの座標系を示す。
本実施形態では、図４（ａ）のように、物体がある軸に対してほぼ回転対称な形状を有することを前提とする。そこで、物体の回転対称性を定義する軸（以後、モデル中心軸と呼ぶ）を決定する。モデル中心軸の設定は、例えば、ユーザからの入力により行う。
【００２５】
具体的には、図４（ａ）のようなモデルの場合、モデルの回転対称な面上から対面する２点ａ、ｂおよびｃ、ｄの２組をユーザからの入力として取得する。このとき、線分ａｂと、線分ｃｄとの交点を通り、かつ四角形ａｃｂｄに垂直な方向をもつベクトルをモデル中心軸として算出する（図４（ｂ））。ただし、モデル中心軸の設定方法はこれに限るものではない。たとえば、もし三角柱のような回転対称な形状の場合には、ユーザからの入力として回転対称な三角形上の３点を取得する。そして、その３点が成す三角形の重心を通り、かつ三角形に垂直な方向をもつベクトルをモデル中心軸として算出する。
【００２６】
あるいは、モデル面上で回転対称性のある面（図４（ａ）中では八角形）をユーザからの入力として取得し、その重心を通り、かつ入力された面に直交する直線をモデル中心軸としてもよい。その他、モデル形状の回転対称性を定義する回転中心軸を算出できる他の手法を用いても良い。
【００２７】
次に、モデル中心軸に対して互いに直交する２つの軸を設定する。ここで、モデル中心軸と、設定した２つの軸とが座標軸となるようにモデル座標系を変換して新しいモデル座標系とする（図４（ｃ））。
【００２８】
ここでは、新しいモデル座標系において、Ｙ軸（実線）がモデル中心軸と一致するようにする。ただし、モデル座標系の変換方法はこれに限るものではない。例えば、モデル座標系のＸ軸がモデル中心軸と一致するように変換を行ってもよい。そのほか、モデル中心軸がモデル原点を通るようにモデル座標系が変換されていれば、いかなる変換であっても構わない。なお、以後の処理は、新しいモデル座標系を用いて行うものとする。基準座標系に対する物体の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。ここで、ｔｘ、ｔｙ、ｔｚはそれぞれＸ、Ｙ、Ｚ軸方向の位置（並進）成分を、θ、φ、ψはそれぞれＸ、Ｙ、Ｚ軸に対する姿勢（回転）成分を表す。この各回転成分の変換順序を考慮することで、座標変換において、物体軸回りの回転を分離する。この６パラメータを第一の位置姿勢パラメータとして記録する。さらに、モデル中心軸と一致する座標軸に対する回転成分を第二の位置姿勢パラメータとして記録する。本実施形態では、Ｙ軸がモデル中心軸と一致するため、Ｙ軸回りの回転成分であるφを第二の位置姿勢パラメータとして記録する。
【００２９】
（ステップＳ１２００）
撮像部１１０により、計測対象物体の二次元画像を撮像し、画像特徴検出部１４０に入力する。
【００３０】
（ステップＳ１３００）
位置姿勢計測装置１に対する物体の位置及び姿勢の概略値を概略位置姿勢入力部１２０により位置姿勢計測装置１に入力する。本実施形態では、前の時刻において計測された位置及び姿勢を用いる。
【００３１】
（ステップＳ１４００）
二次元画像の情報に基づいて物体の第一の位置姿勢パラメータの算出を行う。本実施形態では、二次元画像上の特徴であるエッジに３次元形状モデル中の線分の投影像が当てはまるように計測対象物体の位置及び姿勢を算出する。図５は、本ステップにおける第一の位置姿勢算出の処理手順を示すフローチャートである。ここでは、画像上で検出されたエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和が最小になるように位置及び姿勢のパラメータｓの最適化を行う。このとき、位置姿勢に基づく、モデル座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換を数１のように表すものとする。
【００３２】
Ｘｃ＝Ｒ Ｘｍ ＋ Ｔ ・・・［数１］
ただしＲは物体の姿勢を表す３つの成分θ、φ、ψで表現される３×３行列、Ｔはモデルの位置を表す３つの成分から成る３次元ベクトルＴ＝［ｔｘ， ｔｙ， ｔｚ］Ｔである。さらに、概略位置姿勢に対する物体の位置及び姿勢の微小変化量を６次元ベクトルΔｓ ＝［Δｔｘ，Δｔｙ，Δｔｚ，Δθ，Δφ，Δψ］Ｔとし、これらの成分により表される姿勢の微小変化量をΔＲ、位置の微小変化量をΔＴとする。このとき、位置及び姿勢が微小変化したときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換を、数２のように表す。
【００３３】
Ｘｃ＝ＲΔＲ Ｘｍ ＋ Ｔ＋ΔＴ ・・・［数２］
本ステップでは、微小変化量Δｓを求めて、各成分の補正を行う処理を繰り返すことで、位置及び姿勢の算出を行う。以下、位置及び姿勢算出の処理手順について順に説明する。
【００３４】
（ステップＳ１４１０）
初期化処理を行う。ここでは、ステップＳ１３００で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【００３５】
（ステップＳ１４２０）
ステップＳ１２００において入力された二次元画像上において画像特徴の検出・画像特徴とモデルとの対応付けを行う。本ステップは画像特徴検出部１４０において行われる。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。エッジの検出方法としては、種々の方法が提案されているため、詳細な説明は省く。
【００３６】
図６は、本実施形態におけるエッジ検出を説明する図である。まず、ステップＳ１３００において入力された計測対象物体の位置及び姿勢の概略値と、校正済みのカメラの内部パラメータを用いて、３次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は画像上でも線分となる。次に、画像上で等間隔になるように投影線分上に制御点を設定し、制御点を通りかつ投影された線分に直交する線分上で１次元のエッジ検出を行う（図６（ａ））。エッジは画素値の濃度勾配の極値として検出されるため、図６（ｂ）に示すように、近傍にエッジが存在する場合には複数のエッジが検出されることがある。本実施形態では、検出されるエッジのうち最も濃度勾配の大きいものを対応するエッジとする。
【００３７】
（ステップＳ１４３０）
モデルの線分上の制御点と画像上のエッジとの対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。図７は、線分の投影像と検出されたエッジの関係を説明する図である。図７では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。位置及び姿勢の概略値ｓ０に基づいて投影されたモデルの線分上のある制御点の画像上での位置を（ｕ０，ｖ０）、該制御点が所属する線分の画像上での傾きをｕ軸に対する傾きθとして表す。
【００３８】
傾きθは、モデルの線分の両端の３次元座標をｓの概略値（ｓ０）に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該線分の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、該制御点の対応点（エッジ）の画像座標を（ｕ’，ｖ’）とする。ここで、点（ｕ’，ｖ’）を通り、傾きがθである直線（図７の破線）上の点（ｕ，ｖ）は、
【００３９】
【数１】
【００４０】
と表せる。なお、θは定数とする。ここで、
【００４１】
【数２】
【００４２】
（定数）である。制御点の画像上での位置は計測対象物体の位置及び姿勢ｓにより変化する。したがって、制御点の画像座標（ｕ，ｖ）は、（ｕ０，ｖ０）の近傍での１次のテイラー展開によって数４のように近似できる。但しΔｓｉ（ｉ＝１，２，・・・，６）はｓの各成分の微小変化を表す。つまり、Δｓ１＝Δｔｘ、Δｓ２＝Δｔｙ、Δｓ３＝Δｔｚ、Δｓ４＝Δθ、Δｓ５＝Δφ、Δｓ６＝Δψである。
【００４３】
【数３】
【００４４】
正しいｓによって得られる制御点の画像上での位置が数１によって表される直線上にあると仮定すると、数４を数３に代入することにより、観測方程式（数５）が得られる。
【００４５】
【数４】
【００４６】
ただし、
【００４７】
【数５】
【００４８】
（定数）である。数５の観測方程式は対応付けが行われたすべての制御点について立式することができる。ここで、数３はｓの各成分の微小変化Δｓｉ（ｉ＝１，２，・・・，６）についての方程式になっているため、数６に示すΔｓｉに関する線形連立方程式を立てることができる。
【００４９】
【数６】
【００５０】
ここで、正規化画像座標系における画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃで表される。したがって、これらの数２に基づいて算出したｕ、ｖから、左辺の行列の各要素を算出する。
【００５１】
この線形連立方程式を数７のように表す。
【００５２】
Ｊ１Δｓ＝Ｅ ・・・［数７］
左辺のＪ１が求める係数行列であり、右辺のＥが誤差ベクトルである。
【００５３】
（ステップＳ１４４０）
数５をもとに、行列Ｊ１の一般化逆行列（Ｊ１Ｔ・Ｊ１）−１・Ｊ１Ｔを用いて位置及び姿勢の補正値Δｓを最小二乗基準で求める。しかしながら、エッジの対応付けでは誤対応（外れ値）が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは数４右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制する。重みは例えば数８に示すようなＴｕｋｅｙの関数により与える。
【００５４】
【数７】
【００５５】
ｃ１は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であればなんでもよい。各計測データに対応する重みをｗｉとする。ここで数９のように重み行列Ｗを定義する。
【００５６】
【数８】
【００５７】
重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗｉが入る。この重み行列Ｗを用いて、数７を数１０のように変形する。
【００５８】
【数９】
【００５９】
数１０を数１１のように解くことにより補正値Δｓを求める。
【００６０】
【数１０】
【００６１】
（ステップＳ１４５０）
ステップＳ１４４０において算出された位置及び姿勢の補正値Δｓにより、位置及び姿勢の概略値を補正する。
【００６２】
ｓ←ｓ＋Δｓ
（ステップＳ１４６０）
収束判定を行い、収束していれば終了し、そうでなければステップＳ１４２０に戻る。収束判定では、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。
【００６３】
以上の処理によって、ステップＳ１３００で入力された位置及び姿勢の概略値が二次元画像の情報を用いて更新される。本ステップで最終的に算出された位置姿勢を第一の位置姿勢とする。
【００６４】
（ステップＳ１５００）
第一の位置姿勢に基づき、３次元形状モデル面上の点（モデル特徴点）に対応する二次元画像上の座標（画像特徴点）を取得する。本ステップにおいて、モデル座標系における特定軸方向の姿勢を決める特徴対応を別途取得することで、特定軸まわりの姿勢を高精度に算出可能となる。
【００６５】
まず、３次元形状モデルの面上から、特定自由度方向のパラメータの算出に寄与する幾何特徴を選択し、その３次元座標を記録する。図２に示すモデルにおいて、ｐ１３〜ｐ１５が特定自由度方向のパラメータを一意に決める特徴となる。このモデル特徴点の選択は事前準備としてステップＳ１１００の前にオフラインで行っておく。その際には、３次元形状モデルを自由な姿勢で観測できるツールを用いて、特定自由度方向のパラメータを一意に決める手がかりとなる幾何特徴を人手で指定したものを記録しておき、本ステップで読み出せばよい。あるいは、オンラインで人手による入力を介して指定してもよい。この場合は、第一の位置姿勢に基づいて撮像画像上にレンダリングした３次元形状モデルの中から、モデルの特定自由度方向のパラメータを一意に決める手がかりとなる幾何特徴が描画されている画素を人手により指定する。そして指定した画素に描画されているモデル面の３次元座標を取得することによりモデル特徴点を得る。
【００６６】
次に、モデル特徴点に対応する画像特徴点を撮像画像から決定する。具体的には、第一の位置姿勢に基づき、３次元形状モデルおよび選択されたモデル特徴点の描画を行う。この画像と撮像画像とを比較して、モデル特徴点に対応する画素を撮像画像上で人手により指定することで画像特徴点を得る。ただし、モデル特徴点と画像特徴点との対応取得方法はこれに限るものではない。例えば、モデル特徴点に該当する部位の画像パターンをテンプレートとして用意しておき、撮像画像とのパターンマッチングにより対応する画像特徴点を取得してもよい。具体的には、まず３次元形状モデルと合わせて、モデルの姿勢を一意に決める手がかりとなる特徴点の三次元座標も保持しておき、それを読み込んでモデル特徴点とする。さらに、撮像装置に対する対象物体の姿勢を変化させてあらかじめ複数の視点から画像を撮影し、モデル特徴点の周辺部位のパターン画像を複数保持しておく。そして、撮像画像と各パターン画像とのパターンマッチングにより、もっとも類似する画素の位置を取得することで、対応する画像特徴点を得る。このとき、第一の位置姿勢に基づき、物体を特定自由度方向へ回転させた姿勢で撮影したパターン画像を候補としてパターンマッチングを行うことで、効率よく画像特徴点を探索する。
【００６７】
以上の処理により、モデル特徴点に対応する画像特徴点を取得する。なお、モデル特徴点と画像特徴点との対応は最低一組必要だが、上述の処理を複数のモデル特徴点に対して行うことで、対応を複数取得してもよい。
【００６８】
（ステップＳ１６００）
ステップＳ１４００で算出された第一の位置姿勢は、特定自由度に関するパラメータφを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータφのみに関する最適化を行い、位置姿勢を算出する処理を行う。
【００６９】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。ここで、モデル座標系を基準としたＹ軸方向に対するΔφの微小回転を表す３×３行列をΔＲｙとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向にΔφだけ微小回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数１２のように表される。
【００７０】
Ｘｃ＝ＲΔＲｙ Ｘｍ ＋ Ｔ ・・・［数１２］
そこで、本ステップでは、微小変化量Δφを求めて、モデルのＹ軸回転方向の補正を行うことで位置及び姿勢の算出を行う。
【００７１】
図８は、本ステップにおける第二の位置姿勢算出の処理手順を示すフローチャートである。ここでは、画像上で検出された特徴点の座標と、第一の位置姿勢に基づいて画像上に投影されるモデル特徴点の座標との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。
【００７２】
（ステップＳ１６１０）
初期化処理を行う。ここでは、ステップＳ１４００で得られた第一の位置姿勢を、位置及び姿勢算出における概略値として入力する。
【００７３】
（ステップＳ１６２０）
モデル特徴点と画像特徴との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。図９は、モデル特徴点の投影像と、対応づいた画像特徴との関係を説明する図である。図９では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。位置及び姿勢の概略値φ０に基づいて投影されたモデル特徴点の画像上での位置を（ｕ０，ｖ０）として表す。また、該モデル特徴点に対応する画像特徴の画像座標を（ｕ’，ｖ’）とする。ここで、モデル点の画像上での位置は計測対象物体の微小変化φにより変化する。したがって、モデル特徴点を画面上に投影した座標（ｕ’，ｖ’）は、（ｕ０，ｖ０）の近傍での１次のテイラー展開によって数１０のように近似できる。ただし、Δφはφの指定した軸方向の微小変化を表す。
【００７４】
【数１１】
【００７５】
数１３の観測方程式を対応付けが行われたすべての点群データについて立式して整理すると数１４のように表される。
【００７６】
【数１２】
【００７７】
ただし、ｕｉ’、ｖｉ’はモデル特徴点を投影した二次元座標、ｕｉ０ ｖｉ０はモデル特徴点に対応する画像特徴の二次元座標を意味する（ｉ＝１・・・ｎ、ｎ：特徴点数））。ここで、左辺の行列の値は、以下のように求める。まず、回転中心軸ベクトル［ｕｘ，ｕｙ，ｕｚ］Ｔに対する回転αを表す回転行列ｒは以下の数１５により表される。
【００７８】
【数１３】
【００７９】
ただし、ｃ ＝ ｃｏｓα、ｓ ＝ ｓｉｎαである。ここで、αが微小なとき、ｃｏｓα≒１、ｓｉｎα≒αであることを利用すると、Ｙ軸に対する微小回転を表す行列ΔＲｙは以下の数１６で近似される。
【００８０】
【数１４】
【００８１】
ただし［ｕｘ，ｕｙ，ｕｚ］Ｔ＝［０，１，０］Ｔである。以上より、数１２は以下のように表される。
【００８２】
【数１５】
【００８３】
【数１６】
【００８４】
正規化画像座標系における画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃで表される。したがって、これらをφで偏微分することで各値を算出する。
各行列成分を算出した数１４の線形連立方程式を数１９のように表す。
【００８５】
Ｊ２Δφ＝Ｅ２ ・・・［数１９］
左辺のＪ２が求める係数行列であり、右辺のＥ２が誤差ベクトルである。
【００８６】
（ステップＳ１６３０）
行列Ｊ２の一般化逆行列（Ｊ２Ｔ・Ｊ２）−１・Ｊ２Ｔを用いて、数１９を数２０のように解くことで、位置及び姿勢の補正値Δφを最小二乗基準で求める。
【００８７】
Δφ＝（Ｊ２Ｔ・Ｊ２）−１・Ｊ２ＴＥ２ ・・・［数２０］
（ステップＳ１６４０）
ステップＳ１６３０において算出された位置及び姿勢の補正値Δφにより、位置及び姿勢の概略値を補正する。
【００８８】
φ←φ＋Δφ
（ステップＳ１６５０）
収束判定を行い、収束していれば終了し、そうでなければステップＳ１６２０に戻る。収束判定では、補正値Δφがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。
【００８９】
以上述べたように、第一の実施形態では、特定の軸に対する回転体形状あるいは回転対称な形状を有する物体の位置姿勢推定を行う場合について述べた。まず、モデルと画像とのエッジ対応を利用して一度物体の位置及び姿勢を推定し、その後、モデルと画像との特徴点対応を利用して画像上の二次元距離を最小化することでモデルの特定軸回転方向に限定した位置合わせを行う。モデルの特定軸回転方向のみを後段で推定することで、特定自由度方向の類似形状によりパラメータが求まりにくい物体においても、ロバストかつ高精度に位置姿勢の計測が可能となる。
【００９０】
（第二の実施形態）
第一の実施形態では、第一の位置姿勢算出において、まず物体の６自由度パラメータを最適化し、その後第二の位置姿勢算出で、特定自由度であるモデル座標系のＹ軸回転パラメータのみ最適化を行った。これに対して、本実施形態では、第一の位置姿勢算出においては、モデル座標系のＹ軸回転成分以外の５自由度のパラメータのみ最適化する。第一の位置姿勢算出において、推定が困難な自由度成分を削減することで、計算量を減らして効率よく位置姿勢を算出する。なお、本実施形態において、基本的な構成は第一の実施例と同様であるため説明を省略する。以下、本実施形態における各処理部について説明する。
【００９１】
尚、撮像部１１０、概略位置姿勢入力部１２０、三次元形状モデル入力部１３０、画像特徴検出部１４０、第一の姿勢算出部１６０、対応取得部１７０、第二の位置姿勢算出部１８０については、実施例１と同様のため、説明を省略する。
【００９２】
特定自由度設定部１５０は、第一の位置姿勢パラメータ、第二の位置姿勢パラメータをそれぞれ設定する。本実施形態においても、第一の実施形態と同様に、基準座標系を基準とした物体の位置姿勢の６パラメータのうち、モデルの形状に起因して算出が困難なパラメータを第二の位置姿勢パラメータとして記録しておく。ただし、第一の位置姿勢パラメータには、モデル座標系を基準とした位置姿勢の６パラメータのうち、第二の位置姿勢パラメータとして記録したパラメータ以外のパラメータを記録する。
【００９３】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００とステップＳ１４００である。したがって、ステップＳ１１００とステップＳ１４００の説明を行い、その他の処理の説明は省略する。
【００９４】
（ステップＳ１１００）
第一の実施形態と同様の方法でモデル中心軸に対して互いに直交する２つの軸を設定し、モデル中心軸と、設定した２つの軸とが座標軸となるようモデル座標系を変換して新しいモデル座標系とする。その後、変換後のモデル座標系を基準とした位置姿勢パラメータを表す６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔのうち、モデル中心軸まわりの回転成分であるφを第二の位置姿勢パラメータ、それ以外のパラメータを第一の位置姿勢パラメータとして記録する。
【００９５】
（ステップＳ１４００）
二次元画像の情報に基づいて物体の位置及び姿勢の計測を行う。本実施形態においても、画像上で検出されたエッジと、推定される位置及び姿勢に基づいて画像上に投影されるモデルの線分との距離の二乗和が最小になるように位置及び姿勢のパラメータの最適化を行う。ただし、位置姿勢パラメータであるｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔのうち、第二の位置姿勢算出部で推定するφを除いた５つのパラメータｔｘ，ｔｙ，ｔｚ，θ，ψを推定する。ここで、数１により表される、モデル座標系から基準座標系への変換において、姿勢変換を表す行列Ｒを各軸回りの回転に分解することで数２１のように表すことができる。
【００９６】
Ｘｃ＝ＲｘＲｚＲｙ Ｘｍ ＋ Ｔ ・・・［数２１］
ここでＲｘ、Ｒｚ、Ｒｙはそれぞれモデル座標系のＸ軸回転θ、Ｙ軸回転φ、Ｚ軸回転ψを表す行列である。
【００９７】
ここで、概略位置姿勢に対する物体の位置及び姿勢の微小変化量を５次元ベクトルΔｓ ＝［Δｔｘ，Δｔｙ，Δｔｚ，Δθ，Δψ］Ｔとする。Ｘ軸微小回転Δθを表す回転行列をΔＲｘ、Ｚ軸微小回転Δψを表す回転行列をΔＲｚ、位置の微小変化量をΔＴ＝［Δｔｘ，Δｔｙ，Δｔｚ］Ｔとすると、モデル座標系において、５つのパラメータｔｘ，ｔｙ，ｔｚ，θ，ψを微小変化させたときの物体座標系Ｘｍから基準座標系Ｘｃへの座標変換は、数２２のように表される。
【００９８】
Ｘｃ＝ＲｘΔＲｘＲｚΔＲｚＲｙ Ｘｍ ＋ Ｔ＋ΔＴ ・・・［数２２］
実施例１と同様の方法で、この変換式に基づいて微小変化量Δｓを求め、各位置姿勢成分の補正を行うことで、第一の位置及び姿勢の算出を行う。
【００９９】
以上述べたように、本実施形態では、第一の位置姿勢算出においては、第二の位置姿勢算出で推定する自由度のパラメータは算出しない。第一の位置姿勢算出において推定する自由度成分を削減することで、より効率よく位置姿勢の算出が可能となる。
【０１００】
（第三の実施形態）
第一の実施形態では、二次元画像を利用した位置姿勢推定について述べた。本実施形態では、第一の実施形態と同様の物体に対して距離画像を利用して位置姿勢推定を行う例について述べる。なお、本実施形態において、基本的な構成は第一の実施例と同様であるため説明を省略する。以下、本実施形態における各処理部について説明する。なお、概略位置姿勢入力部１２０、三次元形状モデル入力部１３０、特定自由度設定部１５０、対応取得部１７０については、第一の実施形態と同様のため、説明を省略する。
【０１０１】
撮像部１１０は、距離画像を撮像する距離センサである。距離画像は、各画素が奥行きの情報を持つ画像である。距離センサとしては、対象に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。
【０１０２】
画像特徴検出部１４０は、撮像部１１０によって撮像された画像から対応する幾何特徴を検出する。本実施形態では幾何特徴として三次元点を検出してモデル頂点と対応付ける。具体的には、概略位置姿勢入力部１２０から入力される物体の位置及び姿勢の概略値に基づいて、３次元形状モデルの各モデル頂点を画像上に投影し、モデル頂点が投影された画素に対応する距離値からカメラ座標系の三次元座標を算出する。三次元座標が算出された三次元点を投影されたモデル頂点に対応する画像特徴とする。画像特徴は、第一の位置姿勢算出部１６０において、第一の位置姿勢算出部１６０において物体の第一の位置及び姿勢を算出する際に用いられる。
【０１０３】
第一の位置姿勢算出部１６０は、画像特徴検出部１４０が検出した画像特徴を用いて、計測対象物体の第一の位置姿勢を算出する。本実施形態では、モデルの三次元面と、撮像画像から得られる点との距離を最小化することで第一の位置姿勢を算出する。
【０１０４】
対応取得部１７０は、第一の位置姿勢に基づいて、第二の位置姿勢パラメータを算出する手がかりとなる、３次元形状モデルと撮像画像との幾何特徴の対応を取得する。取得した対応は、第二の位置姿勢算出部１８０において、第二の位置姿勢パラメータを推定する際に用いられる。
【０１０５】
第二の位置姿勢算出部１８０は、第一の位置姿勢に基づき、対応取得部１７０によって得られた対応を利用して第二の位置姿勢を算出する。本実施形態では、モデル上の三次元点と、撮像画像から得られる点との距離を最小化することで第二の位置姿勢を算出する。
【０１０６】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。なお、ステップＳ１１００、ステップＳ１３００、ステップＳ１４１０、ステップＳ１４４０、ステップＳ１４５０、ステップＳ１４６０、ステップＳ１６１０、ステップＳ１６３０、ステップＳ１６４０、ステップＳ１６５０については第一の実施形態と同様であるため、説明を省略する。
【０１０７】
（ステップＳ１２００）
撮像部１１０により、計測対象物体の距離画像を撮像し、画像特徴検出部１４０に入力する。
【０１０８】
（ステップＳ１４００）
距離画像の情報に基づいて物体の位置及び姿勢の計測を行う。本実施形態では、距離画像上の特徴である三次元点に３次元形状モデルの投影像が当てはまるように計測対象物体の位置及び姿勢を算出する。本ステップにおける第一の位置姿勢算出の処理手順は、図５と同様であるため省略する。ここでは、距離画像から変換して得られる三次元点と、推定される位置及び姿勢に基づいて画像上に投影される３次元形状モデルの面との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。以下の説明では、物体の位置及び姿勢を、実施例１と同様に６次元ベクトルｓで表す。以下、位置及び姿勢算出の処理手順について順に説明する。なお、実施例１と処理が同一のものについては、説明を省略する。
【０１０９】
（ステップＳ１４２０）
ステップＳ１２００において入力された距離画像とモデルとの対応付けを行う。以下、距離画像とモデルとの対応付け処理について説明する。図１０は、本ステップにおける距離画像とモデルとの対応付け処理を説明する図である。本ステップでは、公知の手法により物体の３次元形状モデル（ポリゴンモデル）を距離画像上に投影することによって対応付けを行う。モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデルのどのポリゴンに対応するかを決定するためである。距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと位置及び姿勢の概略値を用いてポリゴンモデルを描画する。以下、ポリゴンモデルを描画した画像をポリゴン描画画像と呼ぶ。描画する際には、各ポリゴンに固有の色を割り当てて描画を行う。ポリゴンの色はＲＧＢの各色で表す。背景部分と区別するために、黒（Ｒ＝０、Ｇ＝０、Ｂ＝０）以外の色を用いてポリゴンを描画する。これによって、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデルを描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている３次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。
【０１１０】
（ステップＳ１４３０）
３次元形状モデルのポリゴンと距離画像から変換した３次元点との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。以下、距離画像の画素（３次元の点）と面との対応に基づく係数行列・誤差ベクトルの算出方法について説明する。
【０１１１】
３次元空間における平面の方程式は一般にａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１）の形で表される。ここでは、３次元形状モデル中のある面の計測物体の座標系における平面の方程式をａｘ＋ｂｙ＋ｃｚ＝ｅであるとする。この平面の方程式は、位置及び姿勢の概略値に基づいて、基準座標系における平面の方程式ａ’ｘ＋ｂ’ｙ＋ｃ’ｚ＝ｅ’に変換できる。位置及び姿勢の概略値に基づく、モデル座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換を数２３のように表す。
【０１１２】
Ｘｃ＝Ｒ Ｘｍ ＋ Ｔ ・・・［数２３］
ただしＲは姿勢を表す３×３回転行列、Ｔは位置を表す３次元ベクトルである。このとき、基準座標系における平面の方程式の係数は次式のようになる。
【０１１３】
【数１７】
【０１１４】
基準座標系における３次元座標の計測値が（ｘ’，ｙ’，ｚ’）である点Ｐが３次元形状モデル中のある面Ｆと対応付けられているとする。面Ｆに点Ｐから下ろした垂線の足をＱとする。点Ｑの基準座標系における３次元座標（ｘ，ｙ，ｚ）は、計測対象物体の位置及び姿勢ｓによって変換するものであり、ある位置及び姿勢ｓのときの３次元座標（ｘ０，ｙ０，ｚ０）の近傍で１次のテイラー展開によって数２５のように近似できる。
【０１１５】
【数１８】
【０１１６】
正しいｓによって変換される（ｘ，ｙ，ｚ）は、法線ベクトルが面Ｆと同じで点Ｐを通る平面（ａ’ｘ＋ｂ’ｙ＋ｃ’ｚ＝ｅ’， ｅ’＝ａ’ｘ’＋ｂ’ｙ’＋ｃ’ｚ’） 上にあると仮定すると、数２１を平面の方程式に代入することにより数２６が得られる。
【０１１７】
【数１９】
【０１１８】
ただし、
【０１１９】
【数２０】
【０１２０】
（定数）とした。数２６の観測方程式は対応付けが行われたすべての点群データについて立式することができる。
【０１２１】
数２５及び数２６の観測方程式はｓの各成分の微小変化Δｓｉ（ｉ＝１，２，・・・，６）についての方程式になっており、数１２に示すΔｓｉに関する線形連立方程式を立てることができる。
【０１２２】
【数２１】
【０１２３】
この線形連立方程式を数２８のように表す。
【０１２４】
ＪΔｓ ＝ Ｅ ・・・［数２８］
左辺のＪが求める係数行列であり、右辺のＥが誤差ベクトルである。
【０１２５】
（ステップＳ１５００）
第一の位置姿勢に基づき、３次元形状モデル面上の点（モデル特徴点）に対応する距離画像上の三次元座標（画像特徴点）を取得する。なお、モデル特徴点は、実施例１と同様の方法で指定して記録しておく。次に、モデル特徴点に対応する画像特徴点を撮像した距離画像から決定する。具体的には、第一の位置姿勢に基づき、距離画像面上に３次元形状モデルおよび記録されているモデル特徴点の描画を行う。この画像と撮像画像とを比較して、モデル特徴点に対応する画素を撮像画像上で人手により指定する。その画素の距離値を三次元点に変換することで画像特徴点を得る。
【０１２６】
ただし、モデル特徴点と画像特徴点との対応取得方法はこれに限るものではない。たとえば、実施例１と同様に、モデル特徴点に該当する部位の距離画像パターンを保持して置き、撮像画像とのパターンマッチングにより対応する画像特徴点を取得してもよい。以上の処理により、モデル特徴点に対応する画像特徴点を取得する。なお、モデル特徴点と画像特徴点との対応は最低一組必要だが、上述の処理を複数のモデル特徴点に対して行うことで、対応を複数取得してもよい。
【０１２７】
（ステップＳ１６２０）
モデル特徴点と画像特徴点との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。本実施例では、第一の位置および姿勢を位置及び姿勢の概略値として，概略位置姿勢に基づいて基準座標系へ変換したモデル特徴点の三次元座標を（ｘｃ０，ｙｃ０，ｚｃ０）として表す。また、該モデル特徴点に対応する画像特徴の三次元座標を（ｘｃ，ｙｃ，ｚｃ）とする。ここで、モデル点の画像上での位置は計測対象物体の微小変化φにより変化する。したがって、モデル特徴点を画面上に投影した座標（ｘｃ，ｙｃ，ｚｃ）は、（ｘｃ０，ｙｃ０，ｚｃ０）の近傍での１次のテイラー展開によって数２９のように近似できる。ただし、ΔφはＹ軸方向の回転φの微小変化を表す。
【０１２８】
【数２２】
【０１２９】
数２９の観測方程式を対応付けが行われたすべての点群データについて立式する。
【０１３０】
【数２３】
【０１３１】
ただし、ｘｉ’、ｙｉ’ 、ｚｉ’はモデル特徴点の三次元座標、ｘｉ０ 、ｙｉ０ 、ｚｉ０はモデル特徴点に対応する画像特徴の三次元元座標を意味する（ｉ＝１・・・ｎ、ｎ：特徴点数））。この線形連立方程式を数３１のように表す。
【０１３２】
Ｊ２Δφ＝Ｅ２ ・・・［数３１］
左辺のＪ２が求める係数行列であり、右辺のＥ２が誤差ベクトルである。ここで求めた方程式を第一の実施形態のステップＳ１６３０と同様の方法で解くことで、第二の位置姿勢を算出する。
【０１３３】
以上述べたように、本実施形態では、第一の実施形態と同様の物体に対して距離画像を利用して位置姿勢推定を行う例について述べた。まず、モデルと画像との３次元点の対応を利用して一度物体の位置及び姿勢を推定し、その後、モデルと画像との特徴点対応を利用して三次元距離を最小化することでモデルの特定軸回転方向に限定した位置合わせを行う。
【０１３４】
なお、二次元画像と距離画像を同時に取得することが可能な場合には、第一の位置姿勢を算出する前段の位置合わせ、第二の位置姿勢を算出する後段の位置合わせにおいて、二次元画像を利用する手法と三次元画像を利用する手法をそれぞれ切り替えても良いし、同時に利用しても良い。
【０１３５】
（第四の実施形態）
第一の実施形態では、第二の位置姿勢算出において、非線形最適化により、パラメータの補正を繰り返すことで第二の位置姿勢算出を行った。これに対して、ステップＳ１６１０で得られる３次元形状モデルと撮像特徴との幾何特徴の対応から方程式を立て、この方程式を解くことで直接パラメータを算出してもよい。本実施形態では、第二の位置姿勢算出において、特徴対応から直接パラメータを算出して第二の位置姿勢を算出する方法について述べる。
【０１３６】
なお、本実施形態における位置姿勢計測装置１の構成は、第一の実施形態と同様のため、説明を省略する。
【０１３７】
続いて、各処理について説明する。本実施例においては、基本的な処理フローは図３、図５と同様である。実施例１と異なる処理は、ステップＳ１６００のみであるため、その他の処理は説明を省略し、ステップＳ１６００について説明を行う。
【０１３８】
（ステップＳ１６００）
ステップＳ１４００で算出された第一の位置姿勢は、特定自由度に関するパラメータφを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータφに関する方程式を立て、これを解くことで第二の位置姿勢を算出する。
【０１３９】
第一の実施形態で示したように、第一の位置姿勢を概略位置姿勢としてモデル座標系のＹ軸方向にφだけ回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３２のように表される。
【０１４０】
【数２４】
【０１４１】
ただし、ｃ＝ｃｏｓφ、ｓ＝ｓｉｎφである。ここで概略位置姿勢からφだけ回転させたモデル特徴点を画面上に投影したときの正規化画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃにより算出される。このｕ、ｖが対応する画像特徴ｕｏ、ｖｏと等しい、という方程式を立てると、ｃ＝ｃｏｓφ、ｓ＝ｓｉｎφの２変数に関する以下のような方程式が立てられる。
【０１４２】
【数２５】
【０１４３】
ここで、Ａは２×２の行列、ｂは２次元ベクトルである。この方程式を解いて算出したｓｉｎφとｃｏｓφからｔａｎφを計算し、ａｔａｎφによりφを求める。
【０１４４】
以上述べたように、本実施形態では、第二の位置姿勢算出において、特徴対応から直接パラメータを算出して第二の位置姿勢を算出する方法について述べた。この方法では、第二の位置姿勢の算出で繰り返し処理を必要としないため、高速に位置姿勢を算出可能である。
【０１４５】
（第五の実施形態）
第一の実施形態では、特定の軸に対してほぼ回転対称な形状を有する物体の位置姿勢推定を行った。本実施形態では、モデル座標系の特定軸の並進方向を決める特徴が少ない物体の位置姿勢推定について述べる。このような物体では、位置姿勢の６自由度成分を公平に扱って位置姿勢を算出すると、モデルの特定軸並進方向のパラメータが正しく求まらない。そこで、位置姿勢を算出した後で、モデルの特定軸方向の並進パラメータのみ後段で推定する。本実施形態では、まず、エッジの位置合わせにより物体の位置姿勢を算出し、その後、モデルの特定軸の並進パラメータを推定することで位置および姿勢を計測する。
【０１４６】
なお、本実施形態における位置姿勢計測装置１の構成は、第一の実施形態と同様のため、説明を省略する。
【０１４７】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００、ステップＳ１６００ステップＳ１６２０である。したがって、その他の処理は説明を省略し、ステップＳ１１００とステップＳ１６００、ステップＳ１６２０の説明を行う。
【０１４８】
（ステップＳ１１００）
第一の位置姿勢パラメータと、第二の位置姿勢パラメータを設定する。図１１（ａ）に、本実施形態で対象とするモデルの具体例とその座標系を示す。本実施形態では、図１１（ａ）のように、モデルがある軸方向に対して並進方向を決める特徴が少ない形状であることを前提とする。このモデルに対して、モデルの並進方向を定義する軸（以後、モデル中心軸と呼ぶ）を決定する。モデル中心軸の設定は、たとえばユーザからの入力により行う。具体的には、図１１（ａ）のような四角柱形状のモデルの場合、四角柱の相対する２つの面を結ぶモデルのエッジ線分上から２点ａ、ｂをモデル中心軸として記録する（図１１（ｂ））。ただし、モデル中心軸の設定方法はこれに限るものではない。たとえば、モデル面上の頂点から主軸位置と方向を求め、これらにより表されるベクトルをモデル中心軸としてもよい。そのほか、モデル形状の回転対称性を定義する回転中心軸を算出できればいかなる方法であっても構わない。
【０１４９】
次に、モデル中心軸と互いに直交する２つの軸を設定する。ここで、モデル中心軸と、設定した２つの軸とが座標軸となるようにモデル座標系を変換して定義し直す（図１１（ｃ））。変換後のモデル座標系において、Ｙ軸（実線）がモデル中心軸と一致している。ただし、モデル座標系の変換方法はこれに限るものではない。たとえばモデル座標系のＸ軸がモデル中心軸と一致するように変換を行ってもよい。そのほか、モデル中心軸の方向が、モデルの類似形状の方向と同一になるようモデル座標系が変換されていれば、いかなる変換であっても構わない。なお、以後の処理は、変換後のモデル座標系を用いて行うものとする。変換後のモデル座標系を基準として、第一の実施形態と同様に、位置姿勢計測装置１の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。この６パラメータを第一の位置姿勢パラメータとして記録する。さらに、モデル中心軸と一致する座標軸に対する並進成分を第二の位置姿勢パラメータとして記録する。本実施例では、Ｙ軸がモデル中心軸と一致するため、Ｙ軸方向の並進成分であるｔｙを第二の位置姿勢パラメータとして記録する。
【０１５０】
（ステップＳ１６００）
ステップＳ１４００により算出された第一の位置姿勢は、特定自由度に関するパラメータｔｙを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータｔｙのみに関する最適化を行い、位置姿勢を算出する処理を行う。
【０１５１】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。ここで、モデル座標系を基準としたＹ軸方向に対するΔｔｙの微小並進を表す３次元ベクトルをΔＴ ＝ ［０， ｔｙ， ０］Ｔとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向に微小並進させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３４のように表される。
【０１５２】
Ｘｃ＝Ｒ （Ｘｍ＋ΔＴ）＋ Ｔ ・・・［数３４］
そこで、本ステップでは、微小変化量Δｔｙを求めて、モデル座標系Ｙ軸方向の並進パラメータの補正を行うことで位置及び姿勢の算出を行う。
【０１５３】
ここでは、第一の実施形態と同様に、画像上で検出された特徴点の座標と、第一の位置姿勢に基づいて画像上に投影されるモデル特徴点の座標との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。
【０１５４】
（ステップＳ１６２０）
モデル特徴点と画像特徴との対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。本実施例においても、数１４のφをｔｙに置き換えた形で、同様の関係が成立する。
ここで、数３４は以下のように表される。
【０１５５】
【数２６】
【０１５６】
【数２７】
【０１５７】
正規化画像座標系における画像座標ｕ 、ｖはそれぞれｕ ＝ ｘｃ ／ ｚｃ、ｖ ＝ ｙｃ ／ ｚｃで表される。したがって、これらをｔｙで偏微分することで、φをｔｘで置き換えた、数１４と同様の線形連立方程式を立てることができる。この方程式を実施例１と同様の方法で解き、パラメータを更新することで第二の位置姿勢を算出する。
【０１５８】
以上、本実施形態では、モデル座標系の特定軸の並進方向に対して類似形状を有する物体の位置姿勢推定について述べた。一度物体の位置姿勢を算出し、その後、モデルの特定軸の並進パラメータを推定することで、特定軸並進方向のパラメータも正しく位置姿勢を計測する。なお、変形例１−１と同様に、第一の位置姿勢算出では、ｔｙ以外の５パラメータのみの推定を行っても良い。
【０１５９】
（第六の実施形態）
前述の実施形態では、モデル座標系の特定１自由度のパラメータの推定が困難な形状を持つ物体の位置姿勢推定方法について述べた。本実施形態では、モデル座標系を基準とした特定軸に対する回転、並進の２自由度に関するパラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べる。
【０１６０】
なお、本実施形態における位置姿勢計測装置１の構成は、実施例１と同様のため、説明を省略する。
【０１６１】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００、ステップＳ１６００ステップＳ１６２０である。したがって、その他の処理は説明を省略し、ステップＳ１１００とステップＳ１６００、ステップＳ１６２０の説明を行う。
【０１６２】
（ステップＳ１１００）
第一の位置姿勢パラメータと、第二の位置姿勢パラメータを設定する。本実施例では、図１２のように、モデルが特定軸方向に対してほぼ回転対称かつその特定軸の並進方向成分を決める特徴が少ない形状であることを前提とする。
【０１６３】
このモデルに対して、モデル中心軸を設定する。モデル中心軸の設定は、たとえばユーザからの入力により行う。回転対称性を定義する軸に対して垂直なモデル面（図１２中では円）の輪郭線上の４点ａ、ｂ、ｃ、ｄをユーザからの入力として取得する。このとき、線分ａｂの垂直二等分線と、線分ｃｄの垂直二等分線との交点を通り、四角形ａｂｃｄに垂直な方向をもつベクトルをモデル中心軸として算出する（図４（ｂ））。ただし、モデル中心軸の設定方法はこれに限るものではない。たとえば、第一の実施形態と同様に、回転対称な面をユーザからの入力として取得し、その面の中心を通り、かつモデル面に直交する直線をモデル中心軸としてもよい。そのほか、モデル形状の回転対称性を定義する回転中心軸を算出できればいかなる方法であっても構わない。
【０１６４】
さらに、モデル中心軸と、モデル中心軸に対して互いに直交する２つの軸とが座標軸となるようにモデル座標系を変換して定義し直す。図１２は、上述の処理による変換後のモデルを示している。変換後のモデル座標系において、Ｙ軸（実線）がモデル中心軸と一致している。ここで、変換後のモデル座標系を基準として、実施例１と同様に、位置姿勢計測装置１の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。この６パラメータを第一の位置姿勢パラメータとして記録する。さらに、モデル中心軸と一致する座標軸に対する回転成分、並進成分を第二の位置姿勢パラメータとして記録する。本実施例では、Ｙ軸がモデル中心軸と一致するため、Ｙ軸方向回転成分であるφ、Ｙ軸方向並進成分であるｔｙを第二の位置姿勢パラメータとして記録する。
【０１６５】
（ステップＳ１６００）
ステップＳ１４００により算出された第一の位置姿勢は、特定自由度に関するパラメータφ、ｔｙを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、特定自由度に関するパラメータφおよびｔｙに関する最適化を行い、位置姿勢を算出する処理を行う。
【０１６６】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。
【０１６７】
ここで、モデル座標系を基準としたＹ軸方向に対するΔφの微小回転を表す３×３行列をΔＲｙ、Ｙ軸方向に対するΔｔｙの微小並進を表す３次元ベクトルをΔＴ ＝ ［０， ｔｙ， ０］Ｔとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向に微小回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３０のように表される。
【０１６８】
Ｘｃ＝Ｒ （ΔＲｙＸｍ＋ΔＴ）＋ Ｔ ・・・［数３７］
微小変化量Δφ、Δｔｙを求めて、モデル座標系Ｙ軸方向の回転、並進パラメータの補正を行うことで位置及び姿勢の算出を行う。
【０１６９】
（ステップＳ１６２０）
数３７の変換に基づき、第一の実施形態と同様の方法で、線形連立方程式の係数行列を算出する。この方程式は第一の実施形態と同様の方法で解き、パラメータを更新することで第二の位置姿勢を算出する。
【０１７０】
以上、本実施形態では、モデル座標系を基準とした特定軸に対する回転、並進の２自由度に関するパラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べた。なお、第二の実施形態と同様に、第一の位置姿勢算出では、φ、ｔｙ以外の４パラメータのみの推定を行っても良い。
【０１７１】
（第七の実施形態）
第六の実施形態では、モデル座標系を基準とした特定軸に対する回転、並進の２自由度に関するパラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べた。本変形例では、球あるいは正多面体のような形状で、かつモデル座標系における回転３自由度のパラメータに寄与する特徴が少ない物体の位置姿勢推定方法について述べる。
【０１７２】
なお、本実施例における位置姿勢計測装置１の構成は、第一の実施形態と同様のため、説明を省略する。
【０１７３】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図３、図５および図８と同様である。第一の実施形態と異なる処理は、ステップＳ１１００、ステップＳ１５００、ステップＳ１６００ステップＳ１６２０である。したがって、その他の処理は説明を省略し、ステップＳ１１００、ステップＳ１５００、ステップＳ１６００、ステップＳ１６２０の説明を行う。
【０１７４】
（ステップＳ１１００）
第一の位置姿勢パラメータと、第二の位置姿勢パラメータを設定する。本実施形態では、物体が、球のような、３自由度の回転に対する対称性を有することを前提とする。このモデルに対して、モデルの３自由度の回転を定義する原点がモデル座標系の原点となるようモデル座標系を変換して定義し直す。モデルの回転を定義する原点の設定は、たとえばモデルを包含するＢｏｕｎｄｉｎｇ Ｂｏｘの中心座標を利用する。そして、この原点を通り、互いに直交する３つの軸を設定し、この３つの軸がモデル座標系となるよう変換を行う。
【０１７５】
ここで、変換後のモデル座標系を基準として、第一の実施形態と同様に、位置姿勢計測装置１の位置姿勢を６次元ベクトルｓ ＝［ｔｘ，ｔｙ，ｔｚ，θ，φ，ψ］Ｔで表す。この６パラメータを第一の位置姿勢パラメータとして記録する。そして、モデルのＸ軸回転θ、Ｙ軸回転φ、Ｚ軸回転ψを第二の位置姿勢パラメータとして記録する。
【０１７６】
（ステップＳ１５００）
第一の位置姿勢に基づき、３次元形状モデル面上の点（モデル特徴点）に対応する二次元画像上の座標（画像特徴点）を取得する。本実施形態でも第一の実施形態と同様の方法でモデルと画像との対応を取得する。本実施形態では、第二の位置姿勢算出において、３つのパラメータを推定する。二次元画像上の対応一組につき方程式が２つ得られるため、少なくとも２組の対応を取得する。
【０１７７】
（ステップＳ１６００）
ステップＳ１４００により算出された第一の位置姿勢は、θ、φ、ψを除き、正しい位置姿勢が算出されていることが期待される。本ステップでは、第一の位置姿勢を概略位置姿勢として、第二の位置姿勢パラメータθ、φ、ψに関する最適化を行い、位置姿勢を算出する処理を行う。
【０１７８】
第一の位置姿勢を概略位置姿勢とした場合、ステップＳ１４００ と同様に数５の関係が成り立つ。
【０１７９】
ここで、モデル座標系を基準としたＸ軸方向に対するΔθの微小回転を表す３×３行列をΔＲｘ、Ｙ軸方向に対するΔφの微小回転を表す３×３行列をΔＲｙ、Ｚ軸方向に対するΔψの微小回転を表す３×３行列をΔＲｚとする。このとき、第一の位置姿勢からモデル座標系のＹ軸方向に微小回転させたときの、物体座標系の三次元点Ｘｍ＝［ｘｍ，ｙｍ，ｚｍ］Ｔから基準座標系の三次元点Ｘｃ＝［ｘｃ，ｙｃ，ｚｃ］Ｔへの座標変換は、数３８のように表される。
【０１８０】
Ｘｃ＝ＲｘΔＲｘ ＲｙΔＲｙ ＲｚΔＲｚＸｍ＋ Ｔ ・・・［数３８］
ここで、Ｒｘ、Ｒｙ、Ｒｚはそれぞれ数３１の行列Ｒを各軸回りの回転に分解したものであり、Ｒ＝ＲｘＲｙＲｚの関係が成り立つ。
【０１８１】
本ステップでは、微小変化量Δθ、Δφ、Δψを求めて、モデル座標系の回転パラメータの補正を行うことで位置及び姿勢の算出を行う。
【０１８２】
（ステップＳ１６２０）
数３８の変換に基づき、第一の実施形態と同様の方法で、線形連立方程式の係数行列を算出する。この方程式を第一の実施形態と同様の方法で解き、パラメータを更新することで第二の位置姿勢を算出する。
【０１８３】
以上、本実施形態では、モデル座標系を基準とした３自由度もしくは３自由度以下の回転パラメータ推定が困難な形状の物体の位置姿勢を推定する方法について述べた。なお、第二の実施形態と同様に、第一の位置姿勢算出では、θ、φ、ψ以外の３パラメータのみの推定を行っても良い。
【０１８４】
なお、球面上に特徴点が一点存在するような場合、その特徴点と球の中心とを結ぶ軸に対する回転パラメータは不定となる。このような形状に対しては、回転パラメータを２つとして、本実施形態と同様の方法で推定を行っても良い。
【０１８５】
（他の実施形態）
第二の位置姿勢パラメータの組み合わせは、これまで述べた方法に限るものではない。たとえば、薄い板のような平らな面１枚に、微小な特徴を有するような物体においては、物体面に垂直な軸に対する回転パラメータおよび物体面内の並進２パラメータの推定が困難である。そのため、このような物体に対して、これらの３パラメータを第二の位置姿勢パラメータとして推定を行っても良い。その他、推定が困難なパラメータをモデル座標系の特定自由度として分離できれば、どんな組み合わせであっても構わない。また、上記実施形態で説明された位置姿勢計測装置は、その機能のすべてもしくは一部を、ＣＰＵ（セントラルプロセッシングユニット）、コンピュータプログラムが格納されたＲＡＭ（ランダムアクセスメモリ）などから構成されるコンピュータで代用することが可能である。
【０１８６】
（第八の実施形態）
第一の実施形態および第六の実施形態では、ステップＳ１１００において、ユーザからの入力によりモデル中心軸の決定を行った。本実施形態では、物体の３次元設計ＣＡＤデータから、モデル中心軸を自動で決定する方法について述べる。一般的に、モデル全体の形状がほぼ回転対称な場合、回転対称な解析曲面の回転対称軸の多くが、モデル全体形状の回転対称軸と一致する。そこで、物体の３次元設計ＣＡＤデータを構成する解析曲面群の中から、回転対称な面形状の回転中心軸を抽出し、多数の回転対称面について、位置と方向とが一致する回転対称軸をモデル中心軸として設定する。
【０１８７】
図１３は、本実施形態におけるモデル中心軸決定部２の構成を示している。同図に示すように、モデル軸中心軸決定部２は、３次元ＣＡＤデータ保持部２１０、基準面選択および基準幾何情報抽出部２２０、参照面選択および参照幾何情報抽出部２３０、類似度算出部２４０、評価値算出部２５０、モデル幾何情報決定部２６０から構成されている。なお、モデル中心軸決定部２は、図１における特定自由度設定部１５０に、モデル中心軸を入力する。以下、モデル中心軸決定部２を構成する各部について説明する。
【０１８８】
３次元ＣＡＤデータ保持部２１０は、計測対象物体の３次元ＣＡＤデータを保持する。３次元ＣＡＤデータには、たとえば対象物体の設計時のデータを利用する。ここで、３次元ＣＡＤデータは、平面、Ｂ−Ｓｐｌｉｎｅ−曲面、トーラス曲面、円柱、円錐、円、といったいずれかの属性を持つ解析曲面を、輪郭線で切り取った、トリム曲面の集合により構成されるものとする。各解析曲面はその面形状を表現する解析パラメータを保持するものとする。なお、３次元ＣＡＤデータと、位置合わせに利用する三次元形状モデルの座標系は、同一のものであるとする。ただし、３次元ＣＡＤデータの座標系と位置合わせに利用する三次元形状モデルの座標系との相対位置関係が分かっていれば同一でなくてもかまわない。また、解析曲面の集合により形状を表現する３次元ＣＡＤデータであれば、リバースエンジニアリングにより生成したものを利用してもよい。
【０１８９】
基準面選択および基準幾何情報抽出部２２０は、３次元ＣＡＤデータを構成する複数の解析曲面の中から、回転対称軸の類似性を比較する際の基準となる解析曲面を１つ選択する。なお、回転対称な面のみを選択し、これ以外の面は選択しない。抽出した解析曲面を以後、基準面と呼ぶ。さらに、基準面選択および基準幾何情報抽出部２２０は、基準面の解析曲面パラメータから、面に固有の軸を抽出する。各解析曲面は、その面をＵＶパラメータ空間で表現するために、ＵＶ座標系を定義する２つの軸の、モデル座標系における位置と方向とをパラメータとして保持する。２つの軸とは、たとえば、解析曲面が回転対称であれば回転中心軸とそれに直交する軸、平面であれば面に平行な互いに直交する２軸がこれに相当する。そこで、この２軸のうち、回転中心軸を表すものを抽出する。抽出した軸を以後、基準軸と呼ぶことにする。
【０１９０】
参照面選択および参照幾何情報抽出部２３０は、３次元ＣＡＤデータを構成する複数の解析曲面の中から、基準面選択および基準幾何情報抽出部で抽出した解析曲面とは異なる解析曲面を１つ選択し、選択した解析曲面に固有の軸を抽出する。ここで抽出した面および軸を、以後参照面および参照軸と呼ぶことにする。
【０１９１】
類似度算出部２４０は、基準軸と参照軸のパラメータから、基準軸と、参照軸との類似度を算出する。
【０１９２】
評価値算出部２５０は、基準軸に対する各参照軸の類似度に基づいて、基準軸について、類似度の高い回転対称軸の多さを評価する評価値をそれぞれ算出する。
【０１９３】
モデル幾何情報決定部２６０は、各基準軸に対して算出した評価値に基づいて、モデル中心軸を決定する。
【０１９４】
図１４は、第八の実施形態におけるモデル軸決定の手順を示すフローチャートである。
【０１９５】
（ステップＳ２１００）
３次元ＣＡＤデータ保持部２１０により対象物体の３次元ＣＡＤデータを入力する。
【０１９６】
（ステップＳ２２００）
３次元ＣＡＤデータを構成する複数の解析曲面の中から、モデル軸決定に利用するための解析曲面を抽出する。本実施例では、円、円柱、円錐のいずれかの属性を持つ解析曲面を抽出する。なお、回転対称な形状であれば、いかなる解析曲面を抽出してもかまわない。ここで、抽出した解析曲面の数をＭとする。
【０１９７】
（ステップＳ２３００）
基準面選択および基準幾何情報抽出部２２０により、ステップＳ２２００で抽出したＭ個の解析曲面の中から基準面を選択し、基準軸の抽出を行う。まず、Ｍ個の解析曲面の中から１つ、基準とする解析曲面をランダムに選択する。選択した解析曲面を基準面Ｆｉ（ｉ＝１〜Ｍ）とする。次に、基準面Ｆｉを表現する解析曲面パラメータの中から、面の回転中心軸を、基準軸Ａｉとして抽出する。ここで、Ａｉはモデル座標系における始点位置を表す３次元座標ｐｉ、方向を表す３次元ベクトルｎｉにより表されるものとする。
【０１９８】
（ステップＳ２４００）
参照面選択および参照幾何情報抽出部２３０により、ステップＳ２２００で抽出したＭ個の解析曲面の中から、参照面および参照軸の抽出を行う。まず、Ｍ個の解析曲面のうち、基準面とは異なるものから１つ、参照する解析曲面を選択する。選択した解析曲面を参照面Ｆｊ（ｊ＝１〜Ｍ、ただしｊ≠ｉ）とする。さらに、ステップＳ２３００と同様に、参照面Ｆｊを表現する解析曲面パラメータから、回転中心軸をＡｊとして抽出する。なお、参照軸の始点位置を表す３次元座標をｐｊ、方向をあらわす３次元ベクトルをｎｊとする。
【０１９９】
（ステップＳ２５００）
類似度算出部２４０により、基準軸Ａｉと参照軸Ａｊのパラメータから、基準軸Ａｉに対する参照軸Ａｊとの類似度を算出する。具体的には、２軸間の距離類似度と、方向類似度とを算出する。まず、基準軸と参照軸との始点間距離をｄ１、参照軸の始点を基準軸へ投影した際の基準軸の始点との距離をｄ２として、距離類似度αｉｊを以下のように定義し、算出する。
【０２００】
【数２８】
【０２０１】
ここで、
【０２０２】
【数２９】
【０２０３】
【数３０】
【０２０４】
である。
【０２０５】
次に、基準軸と参照軸との方向の類似度βｉｊを以下のように定義し、算出する。
【０２０６】
【数３１】
【０２０７】
なお、類似度αｉｊ、類似度βｉｊは値が大きいほど２つの軸が類似していることを意味する。ここで、算出したｄ１、αｉｊ、βｉｊをそれぞれｉ、ｊとともに記録しておく。
【０２０８】
（ステップＳ２６００）
ステップＳ２２００で抽出したＭ個の解析曲面のうち、基準面以外に、参照面として未選択のものが存在すれば、ステップＳ２４００へ戻り、存在しなければステップＳ２６００へ進む。
【０２０９】
（ステップＳ２７００）
評価値算出部２５０により、基準軸と各参照軸との類似度に基づき、類似度が閾値以上であった参照軸を持つ参照面の面積の和を評価値として算出する。まず、基準軸Ａｉと各参照軸Ａｊとの類似度αｉｊ、βｉｊに基づいて、以下の条件をともに満たすｊを類似する軸を保有する面のＩＤとして記録する。
【０２１０】
θ＜αｉｊ、 φ＜βｉｊ ・・・［数４１］
ここで、θ、φはともに閾値である。θにはたとえばθ＝ｄ１×１０、φにはたとえばφ＝０．９を設定する。次に、記録しておいたＩＤに基づき、各参照面Ｆｊの表面積Ｓｊを求め、その総和を基準軸Ａｉの評価値Ｖｉとして算出する。
ただし、評価値の算出方法はこれに限るものではない。たとえば、αｉｊあるいはβｉｊのどちらか一方の条件のみ満たすもののＩＤを類似する軸とみなしてもよい。あるいは、閾値を利用せずに評価値Ｖｉ＝Σ（Ｓｊ×βｉｊ）としてもよい。あるいは、類似度が閾値以上であった面の総数を評価値としてもよい。その他、２つの軸の類似性を評価できる尺度であれば、これ以外のものであってもかまわない。
【０２１１】
（ステップＳ２８００）
ステップＳ２２００で抽出したＭ個の解析曲面のうち、基準面として未選択のものが存在すれば、ステップＳ２３００へ戻り、存在しなければステップＳ２９００へ進む。この結果、各基準面Ｆｉの軸に対する評価値Ｖｉを得る。
【０２１２】
（ステップＳ２９００）
モデル幾何情報決定部２６０により、各評価値Ｖｉの中から、もっとも大きい評価値をもつ基準軸Ａｉをモデル中心軸として決定する。あるいは、評価値の高い基準軸の中から上位数個を候補として抽出し、三次元ＣＡＤデータの形状と基準軸とを重畳させて画面上に表示し、ＧＵＩによりユーザに選択させてもよい。具体的には、モデル全体の表面積Ｓに対する評価値Ｖｉの割合を閾値ｔｈｒｅｓｈｏｌｄとして設定しておき（たとえばｔｈｒｅｓｈｏｌｄ＝０．１）、以下の関係を満たすものだけを抽出して提示する。
【０２１３】
Ｖｉ ／ Ｓ ＞ ｔｈｒｅｓｈｏｌｄ ・・・［数４０］
このとき、評価値の高い基準軸から順に抽出し、逐次、抽出された基準軸との類似度が閾値以上の参照軸に関しては、次の抽出候補から除外することで、類似した軸のみが候補として複数提示されるのを防いでもよい。
【０２１４】
以上、本実施形態では、物体の３次元設計ＣＡＤデータを構成する解析曲面群の中から、回転対称な面形状の回転対称軸を抽出し、多数の回転対称面について、位置と方向とが一致する回転対称軸をモデル中心軸として設定する方法について述べた。
【０２１５】
なお、回転対称軸を表現する始点位置の３パラメータおよび方向の３パラメータにより表される６自由度空間を適当な大きさに分割してｂｉｎを設定しておき、各解析曲面の回転対称軸の６パラメータに基づいてｂｉｎに投票を行って、類似するものを選別してもよい、この場合、もっとも投票の多かったｂｉｎに投票した全回転対称軸の平均値をとることで、モデル中心軸を決定する。このほか、回転対称な各解析曲面の回転対称軸に対して、方向と位置とが多数類似するものを決定できれば、上に述べた以外のいかなる方法であってもかまわない。
【０２１６】
また、本実施例では、ＣＡＤデータの解析曲面から回転対称な面を抽出したが、解析曲面でなくても正多角形あるいは正多角柱など、回転対称なプリミティブを抽出して同様の方法をとってもよい。
【０２１７】
（第九の実施形態）
第五の実施形態では、ステップＳ１１００において、ユーザからの入力あるいはモデル全体形状の主軸方向に基づいて、特定の１軸並進成分を規定するモデル中心軸の決定を行った。本実施形態では、第八の実施例と同様に、対象物体の３次元設計ＣＡＤデータから、モデルの特定の並進成分を規定するモデル中心軸を設定する方法について述べる。
【０２１８】
特定の１軸方向に関する並進パラメータの推定が困難な物体の場合、モデルを構成する個々の解析曲面のうち、相対的に細長い平面がモデル全体の形状において支配的に分布していることが期待される。そこで、物体の３次元設計ＣＡＤデータを構成する解析曲面群の中から平面を抽出し、その平面の重心を始点としてその平面の長軸方向へ向かうベクトルをモデル中心軸候補となる軸として抽出する。この軸との位置と方向とが一致する軸をモデル中心軸として設定する。
【０２１９】
なお、本実施形態におけるモデル幾何情報決定部２の構成は、第八の実施形態と同様のため、説明を省略する。ただし、本実施例では、基準面選択および基準幾何情報抽出部２２０、参照面選択および参照幾何情報抽出部２３０では、３次元ＣＡＤデータを構成する複数の解析曲面の中から、モデル中心軸の決定に利用する際の解析曲面として、平面のみをそれぞれ抽出する。なお、平面以外の解析曲面については選択しない。さらに、面に固有の軸として、その面の重心を始点として、面の長軸方向へと向かうベクトルを抽出する。
【０２２０】
続いて、各処理について説明する。本実施形態において、基本的な処理フローは図１４と同様である。第八の実施形態と異なる処理は、ステップＳ２２００、ステップＳ２３００、ステップＳ２４００である。したがって、その他の処理は説明を省略し、ステップＳ２２００、ステップＳ２３００、ステップＳ２４００の説明を行う。
【０２２１】
（ステップＳ２２００）
３次元ＣＡＤデータを構成する複数の解析曲面の中から、モデル軸決定に利用するための解析曲面を抽出する。本実施例では、平面のいずれかの属性を持つ解析曲面を抽出する。なお、曲面を持たず、平面のみで構成される面形状であれば、いかなる解析曲面を抽出してもかまわない。ここで、抽出した解析曲面の数をＭとする。
【０２２２】
（ステップＳ２３００）
抽出したＭ個の解析曲面の中から基準面を選択し、基準軸の抽出を行う。まず、Ｍ個の解析曲面の中から１つ、基準とする解析曲面をランダムに選択する。選択した解析曲面を基準面Ｆｉ（ｉ＝１〜Ｍ）とする。次に、基準面Ｆｉを表現する解析曲面パラメータから、面の重心および面に直交するベクトルを求め、重心位置を表す３次元座標をｐｉ、重心位置を始点として面の長軸方向に向かう３次元ベクトルをｎｉとする。ただし、軸の抽出方法はこの限りではない。
【０２２３】
（ステップＳ２４００）
抽出したＭ個の解析曲面の中から、参照面および参照軸の抽出を行う。まず、Ｍ個の解析曲面のうち、基準面とは異なるものから１つ、参照する解析曲面を選択する。選択した解析曲面を参照面Ｆｊ（ｊ＝１〜Ｍ、ただしｊ≠ｉ）とする。さらに、ステップＳ２３００と同様に、参照面Ｆｊを表現する解析曲面パラメータから、参照軸の始点位置を表す３次元座標をｐｊ、方向をあらわす３次元ベクトルをｎｊとして抽出する。
【０２２４】
以上、本実施形態では、３次元設計ＣＡＤデータを構成する解析曲面のうち、平面に関して、その平面の重心を始点として平面の長軸方向に向かうベクトルを軸としてそれぞれ抽出し、この軸の方向と位置とが多数類似するものを、モデル中心軸として設定した。
【０２２５】
なお、ステップＳ２３００において、ｎｉには、面の重心位置から長軸方向へと向かうベクトルの代わりに、面の重心位置を始点として、面に直交するベクトルを用いてもよい。このとき、ステップＳ２４００では、ステップＳ２３００と軸の抽出方法を統一し、面の重心位置を表す３次元座標をｐｉ、重心位置を始点として面に直交する３次元ベクトルをｎｉとして抽出する。さらに、ステップＳ２９００では、各評価値Ｖｉの中から、もっとも大きい評価値をもつ基準軸Ａｉの基準面Ｆｉの重心を始点としてその面の長軸方向に向かうベクトルをモデル中心軸として決定する。
【０２２６】
なお、第七の実施形態で述べたような、球形状の物体における回転３自由度を規定する中心点に関しても、同様の方法で決定可能である。この場合には、ＣＡＤデータを構成する解析曲面群から、球形状の解析曲面を複数抽出し、各面の中心点との距離が多数類似度する点をモデル中心点として決定する。あるいはＣＡＤデータから正多面体の形状を複数抽出して同様に中心点を求めることで、多数の中心点と類似する点をモデル中心点として決定してもよい。
【産業上の利用可能性】
【０２２７】
本発明は、ロボットの自己位置推定や、ロボットと物体との間の相対的な位置及び姿勢を推定するのに利用することができる。
【符号の説明】
【０２２８】
１ 位置姿勢計測装置
１１０ 撮像部
１２０ 概略位置姿勢入力部
１３０ 三次元形状モデル入力部
１４０ 画像特徴検出部
１５０ 特定自由度設定部
１６０ 第一の位置姿勢算出部
１７０ 対応取得部
１８０ 第二の位置姿勢算出部
２ モデル中心幾何情報決定部
２１０ ＣＡＤデータ保持部
２２０ 基準面選択および基準幾何情報抽出部
２３０ 参照面選択および参照幾何情報抽出部
２４０ 類似度算出部
２５０ 評価値算出部
２６０ モデル幾何情報決定部
【特許請求の範囲】
【請求項１】
計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、
前記計測対象物体の画像を取得する画像取得手段と、
前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、
前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする情報処理装置。
【請求項２】
前記第一の座標系は、前記画像を取得した撮像装置の撮像座標系であり、
前記第二の座標系は、前記三次元形状モデルを基準としたモデル座標系であることを特徴とする請求項１に記載の情報処理装置。
【請求項３】
前記第一の位置姿勢は、前記撮像座標系における６自由度の位置姿勢パラメータで表される位置姿勢であることを特徴とする請求項２に記載の情報処理装置。
【請求項４】
前記第二の位置姿勢は、前記モデル座標系を基準とした３自由度以下の位置姿勢パラメータで表される位置姿勢であることを特徴とする請求項２に記載の情報処理装置。
【請求項５】
前記第二の位置姿勢は、前記モデル座標系を基準とした１自由度の回転成分と１自由度の並進成分との少なくともいずれか一つで表される位置姿勢であることを特徴とする請求項４に記載の情報処理装置。
【請求項６】
前記第二の位置姿勢取得手段は、前記三次元形状モデルの第二の幾何特徴の画像を用いたパターンマッチングにより、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得することを特徴とする請求項１に記載の情報処理装置。
【請求項７】
前記第二の位置姿勢取得手段は、非線形最適化により前記第二の位置姿勢を取得することを特徴とする請求項１に記載の情報処理装置。
【請求項８】
前記第二の位置姿勢取得手段は、連立方程式から前記第二の位置姿勢を取得することを特徴とする請求項１に記載の情報処理装置。
【請求項９】
前記画像取得手段は、前記計測対象物体の画像として、前記計測対象物体の二次元画像と距離画像との少なくともいずれか一つを取得することを特徴とする請求項１に記載の情報処理装置。
【請求項１０】
前記第一の位置姿勢のパラメータは、前記モデル座標系における６自由度の位置姿勢パラメータのうち、前記第二の位置姿勢を表すパラメータ以外のパラメータであることを特徴とする請求項１に記載の情報処理装置。
【請求項１１】
更に、前記計測対象物体の３次元設計ＣＡＤデータを保持するデータ保持手段と、
前記３次元設計ＣＡＤデータを基準とした３自由度以下の位置姿勢パラメータを規定するモデル中心軸またはモデル中心点を決定し、当該中心軸またはモデル中心点に基づいて、前記第二の座標系を決定するモデル中心幾何情報決定手段とを有することを特徴とする請求項１乃至１０のいずれか１項に記載の情報処理装置。
【請求項１２】
前記モデル中心幾何情報決定手段は、前記３次元設計ＣＡＤデータを構成する解析曲面群における複数の解析曲面から抽出した回転対称軸との位置と方向との少なくとも一方の類似度が高くなるように、１自由度の回転成分と１自由度の並進成分との少なくとも一方を規定するモデル中心軸を決定することを特徴とする請求項１１記載の情報処理装置。
【請求項１３】
前記モデル中心幾何情報決定手段は、
前記３次元設計ＣＡＤデータを構成する解析曲面群における複数の解析曲面から抽出した並進を規定する軸との位置と方向との少なくとも一方の類似度が高くなるように、１自由度の並進成分を規定するモデル中心軸を決定することを特徴とする請求項１１に記載の情報処理装置。
【請求項１４】
前記モデル中心幾何情報決定手段は、
前記３次元設計ＣＡＤデータを構成する解析曲面群における複数の解析曲面から抽出した中心点との位置の類似度が高くなるように、３自由度の回転成分を規定するモデル中心点を決定することを特徴とする請求項１１に記載の情報処理装置。
【請求項１５】
前記モデル中心幾何情報決定手段は、前記類似度が高い軸または点をＧＵＩによりユーザに１つ以上提示し、ユーザ指示に基づき、前記類似度が高い軸または点から前記モデル中心軸またはモデル中心点を選択することを特徴とする請求項１１に記載の情報処理装置。
【請求項１６】
計測対象物体を撮像する撮像部と、
計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、
前記撮像部で撮像された前記計測対象物体の画像を取得する画像取得手段と、
前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、
前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする情報処理装置。
【請求項１７】
モデル保持手段が、計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持工程と、
画像取得手段が、前記計測対象物体の画像を取得する画像取得工程と、
第一の位置姿勢取得手段が、前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得工程と、
第二の位置姿勢取得手段が、前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得工程と、を有することを特徴とする情報処理装置。
【請求項１８】
コンピュータを、
計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、
前記計測対象物体の画像を取得する画像取得手段と、
前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、
前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする情報処理装置として機能させるためのコンピュータプログラム。
【請求項１】
計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、
前記計測対象物体の画像を取得する画像取得手段と、
前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、
前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする情報処理装置。
【請求項２】
前記第一の座標系は、前記画像を取得した撮像装置の撮像座標系であり、
前記第二の座標系は、前記三次元形状モデルを基準としたモデル座標系であることを特徴とする請求項１に記載の情報処理装置。
【請求項３】
前記第一の位置姿勢は、前記撮像座標系における６自由度の位置姿勢パラメータで表される位置姿勢であることを特徴とする請求項２に記載の情報処理装置。
【請求項４】
前記第二の位置姿勢は、前記モデル座標系を基準とした３自由度以下の位置姿勢パラメータで表される位置姿勢であることを特徴とする請求項２に記載の情報処理装置。
【請求項５】
前記第二の位置姿勢は、前記モデル座標系を基準とした１自由度の回転成分と１自由度の並進成分との少なくともいずれか一つで表される位置姿勢であることを特徴とする請求項４に記載の情報処理装置。
【請求項６】
前記第二の位置姿勢取得手段は、前記三次元形状モデルの第二の幾何特徴の画像を用いたパターンマッチングにより、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得することを特徴とする請求項１に記載の情報処理装置。
【請求項７】
前記第二の位置姿勢取得手段は、非線形最適化により前記第二の位置姿勢を取得することを特徴とする請求項１に記載の情報処理装置。
【請求項８】
前記第二の位置姿勢取得手段は、連立方程式から前記第二の位置姿勢を取得することを特徴とする請求項１に記載の情報処理装置。
【請求項９】
前記画像取得手段は、前記計測対象物体の画像として、前記計測対象物体の二次元画像と距離画像との少なくともいずれか一つを取得することを特徴とする請求項１に記載の情報処理装置。
【請求項１０】
前記第一の位置姿勢のパラメータは、前記モデル座標系における６自由度の位置姿勢パラメータのうち、前記第二の位置姿勢を表すパラメータ以外のパラメータであることを特徴とする請求項１に記載の情報処理装置。
【請求項１１】
更に、前記計測対象物体の３次元設計ＣＡＤデータを保持するデータ保持手段と、
前記３次元設計ＣＡＤデータを基準とした３自由度以下の位置姿勢パラメータを規定するモデル中心軸またはモデル中心点を決定し、当該中心軸またはモデル中心点に基づいて、前記第二の座標系を決定するモデル中心幾何情報決定手段とを有することを特徴とする請求項１乃至１０のいずれか１項に記載の情報処理装置。
【請求項１２】
前記モデル中心幾何情報決定手段は、前記３次元設計ＣＡＤデータを構成する解析曲面群における複数の解析曲面から抽出した回転対称軸との位置と方向との少なくとも一方の類似度が高くなるように、１自由度の回転成分と１自由度の並進成分との少なくとも一方を規定するモデル中心軸を決定することを特徴とする請求項１１記載の情報処理装置。
【請求項１３】
前記モデル中心幾何情報決定手段は、
前記３次元設計ＣＡＤデータを構成する解析曲面群における複数の解析曲面から抽出した並進を規定する軸との位置と方向との少なくとも一方の類似度が高くなるように、１自由度の並進成分を規定するモデル中心軸を決定することを特徴とする請求項１１に記載の情報処理装置。
【請求項１４】
前記モデル中心幾何情報決定手段は、
前記３次元設計ＣＡＤデータを構成する解析曲面群における複数の解析曲面から抽出した中心点との位置の類似度が高くなるように、３自由度の回転成分を規定するモデル中心点を決定することを特徴とする請求項１１に記載の情報処理装置。
【請求項１５】
前記モデル中心幾何情報決定手段は、前記類似度が高い軸または点をＧＵＩによりユーザに１つ以上提示し、ユーザ指示に基づき、前記類似度が高い軸または点から前記モデル中心軸またはモデル中心点を選択することを特徴とする請求項１１に記載の情報処理装置。
【請求項１６】
計測対象物体を撮像する撮像部と、
計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、
前記撮像部で撮像された前記計測対象物体の画像を取得する画像取得手段と、
前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、
前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする情報処理装置。
【請求項１７】
モデル保持手段が、計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持工程と、
画像取得手段が、前記計測対象物体の画像を取得する画像取得工程と、
第一の位置姿勢取得手段が、前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得工程と、
第二の位置姿勢取得手段が、前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得工程と、を有することを特徴とする情報処理装置。
【請求項１８】
コンピュータを、
計測対象物体との位置姿勢合わせにより、当該計測対象物体の位置姿勢を取得するための三次元形状モデルを保持するモデル保持手段と、
前記計測対象物体の画像を取得する画像取得手段と、
前記三次元形状モデルの第一の幾何特徴と前記画像内の第一の幾何特徴とに基づき、第一の座標系において、前記三次元形状モデルの第一の位置姿勢を取得する第一の位置姿勢取得手段と、
前記三次元形状モデルの第二の幾何特徴と前記画像内の第二の幾何特徴と前記第一の位置姿勢とに基づき、前記第一の座標系と異なる第二の座標系において、前記三次元形状モデルの第二の位置姿勢を取得する第二の位置姿勢取得手段と、を有することを特徴とする情報処理装置として機能させるためのコンピュータプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１２−１５０７９６（Ｐ２０１２−１５０７９６Ａ）
【公開日】平成２４年８月９日（２０１２．８．９）
【国際特許分類】
【出願番号】特願２０１１−２７９８０１（Ｐ２０１１−２７９８０１）
【出願日】平成２３年１２月２１日（２０１１．１２．２１）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２４年８月９日（２０１２．８．９）
【国際特許分類】
【出願日】平成２３年１２月２１日（２０１１．１２．２１）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]