JP6626338B2 - 情報処理装置、情報処理装置の制御方法、およびプログラム - Google Patents

Description

本発明は、情報処理装置、情報処理装置の制御方法、およびプログラムに関する。

近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。このような物体の位置及び姿勢の計測は、ロボットによる部品把持のためだけでなく、ロボットが自律移動するための自己位置推定や、拡張現実感における現実空間（現実物体）と仮想物体の位置合わせなど様々な目的に応用される。

物体の位置及び姿勢を計測する方法として、３次元形状モデルのモデル特徴とカメラ等の撮像装置から得られる２次元画像における計測データ特徴を用いる方法がある。非特許文献１では、２次元画像上の計測データ特徴であるエッジに対して、線分の集合で表される物体の３次元形状モデルの投影像を当てはめることにより、物体の位置及び姿勢を計測する方法が開示されている。この方法では、既知の情報として与えられる概略の位置及び姿勢に基づいて、３次元形状モデル中の線分を２次元画像上に投影し、投影された線分上に離散的に配置された制御点の夫々に対応するエッジを２次元画像上で検出する。このようにして得られた制御点（モデル特徴）とエッジ（計測データ特徴）との対応に基づいて、制御点が属する線分の投影像と対応するエッジとの画像上での距離の二乗和が最小となるように概略の位置及び姿勢を補正することで最終的な位置及び姿勢が得られる。

T. Drummond and R. Cipolla, "Real-time visual tracking of complex structures," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002. R. Y. Tsai, "A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses," IEEE Journal of Robotics and Automation, vol.RA-3, no.4, 1987. （実施形態で参照される） 馬場、浅田、天野、"Calibrated Computer Graphicsによる画像合成の試み：カメラキャリブレーションに基づく任意フォーカス画像の生成と検証"、情報処理学会論文誌39(7),2180-2188,1998 （実施形態で参照される） Y. Hel-Or and M. Werman, "Pose estimation by fusing noisy data of different dimensions," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.17, no.2, pp.195-201, 1995. （実施形態で参照される）

しかし、上述した手法では、モデル特徴（制御点）と計測データ特徴（エッジ）との間に誤対応が発生した場合に、物体の位置及び姿勢の推定処理が破綻したり、位置及び姿勢の推定精度が低下したりするという課題がある。本発明は、以上の課題を鑑み、形状モデルのモデル特徴と２次元画像上の計測データ特徴との、より高精度な対応付けを実現することを目的とする。

上記の目的を達成するための本発明の一態様による情報処理装置は以下の構成を備える。すなわち、
被測定物体の形状を表す形状モデルに基づいて、前記被測定物体を撮像手段により撮像した画像に生じる該被測定物体の画像劣化を予測する予測手段と、
前記被測定物体を前記撮像手段により撮像することにより得られた２次元画像について、前記形状モデルのモデル特徴に対応する計測データ特徴を探索する探索手段と、
前記探索手段で探索された計測データ特徴について前記２次元画像を用いて画像劣化を評価する評価手段と、
前記予測手段で予測した画像劣化と前記評価手段で評価した画像劣化に基づいて、前記モデル特徴と前記計測データ特徴の対応の正確度を算出する算出手段と、
前記モデル特徴と前記計測データ特徴の対応の正確度に基づいて、前記形状モデルと前記２次元画像中の被測定物体とを照合する照合手段と、を備える。

本発明によれば、被測定物体の形状モデルのモデル特徴と２次元画像上の計測データ特徴との、より高精度な対応付けを実現することができる。そのため、たとえば、被測定物体の位置及び姿勢の推定をより高精度に実現できる。

第１実施形態における情報処理装置の構成を示す図。 実施形態における計測対象物の３次元形状モデルを例示する図。 第１実施形態における、位置姿勢を計測する処理手順を表すフローチャート。 画像からのエッジ検出を説明する図。 第１実施形態における、画像劣化予測値を算出する手順を表すフローチャート。 第１実施形態における、２次元画像の画像劣化評価値を算出する手順を表すフローチャート。 第１実施形態における、対応付け重み算出の処理手順を説明するフローチャート。 第１実施形態における、位置姿勢算出の処理手順を説明するフローチャート。 線分の投影像と検出されたエッジの関係を説明する図。 第３実施形態における情報処理装置の機能構成例を示す図。 第４実施形態におけるロボットシステムの構成例を示す図。

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

＜第１実施形態＞
第１実施形態では、２次元画像で得られる画像のボケ、ブレの大きさを利用して、局所線特徴と計測データ特徴との対応付けを行う。すなわち、局所線特徴毎に、２次元画像において得られるボケ、ブレの大きさを予測しておき、２次元画像から実測したボケ、ブレの大きさと比較し、それらが近い値を示した特徴の寄与度を高くして対応付けする。第１実施形態では、被測定物体を模した形状モデルが表す形状と、事前に決められた撮像装置と被測定物体との相対的な移動方向および速度から、シミュレーションによりボケ、ブレの大きさを計算することでボケ、ブレの大きさを予測する。

図１（ａ）は、本実施形態における情報処理装置１のハードウエア構成例を示すブロック図である。図１（ａ）において、ＣＰＵ１０１は、ＲＯＭ１０２またはＲＡＭ１０３に格納されたプログラムを実行することにより、情報処理装置１における各種制御を実現する。ＲＯＭ１０２は、ＣＰＵ１０１の起動時に実行されるプログラムを含む各種プログラムや各種データを記憶する。ＲＡＭ１０３はＣＰＵ１０１のメインメモリとして機能する。たとえば、ＲＡＭ１０３には、外部記憶装置１０５から読み出されたアプリケーションプログラムがＣＰＵ１０１による実行のために展開される。表示部１０４は、ＣＰＵ１０１の制御下で各種の表示を行う。外部記憶装置１０５は、たとえばハードディスク等で構成され、アプリケーションプログラムや形状モデルのデータ、処理対象となる２次元画像を蓄積する。インタフェース１０６は、外部装置と通信を行う。インタフェース１０６には、たとえば撮像装置２０が接続され、撮像装置２０から２次元画像を受信する。バス１０７は上述した各構成の相互の通信を可能にする。

図１（ｂ）は本実施形態における情報処理装置１の機能構成例を示すブロック図である。図１（ｂ）に示される各機能は、ＣＰＵ１０１がＲＯＭ１０２に記憶されているプログラム、および／または、外部記憶装置１０５からＲＡＭ１０３に展開されたプログラムを実行することにより実現される。なお、各機能部の一部またはすべてを専用のハードウエアで実現してもよいことは明らかである。図１（ｂ）において、情報処理装置１は、モデル入力部１１０、予測値算出部１２０、画像入力部１３０、探索部１４０、評価値算出部１５０、正確度算出部１６０、照合部１７０を備える。また、図１に示す構成は、本発明の情報処理装置の適用例となる構成である。以下、情報処理装置１の各機能部について説明する。

モデル入力部１１０は、被測定物体の形状を表す形状モデル（本実施形態では、外部記憶装置１０５に格納されている３次元形状モデルとする）を入力する。本実施形態における形状モデルは、図２（ａ）に示すような、３次元位置と３次元線分方向から構成される物体輪郭上の局所的な３次元線分情報（以後、局所線特徴と称する）によって構成されるものとする。形状モデルは、モデル入力部１１０を介して、探索部１４０に提供される。

予測値算出部１２０は、被測定物体が撮影された２次元画像の画像劣化予測値を算出する。本実施形態では、被測定物体の３次元形状モデル、概略の位置／姿勢の値、処理対象の２次元画像を撮影した撮像装置の内部パラメータを用いて、焦点外れによるボケの大きさＤと画像平面上での平行移動によって生じるブレの大きさＢを画像劣化予測値とする。ボケの大きさＤ、ブレの大きさＢ、画像劣化予測値を取得する処理の詳細は後述する。

画像入力部１３０は、被測定物体が撮影された２次元画像を取得する。取得される２次元画像はグレー画像であってもよいしカラー画像であってもよい。本実施形態では、予め撮影された２次元画像を記憶する外部の記憶装置（不図示）から２次元画像を取得するが、撮像装置２０を用いて撮影された結果の２次元画像を、画像入力部１３０がその撮像装置２０から直接に取得するようにしてもよい。いずれにしても、２次元画像にはその画像を撮影した撮像装置２０の内部パラメータが対応付けられて保持される。

探索部１４０は、画像入力部１３０から入力された２次元画像において、モデル入力部１１０から入力された被測定物体の形状モデルの、局所線特徴の対応の候補を探索する。この探索は、形状モデルを構成するすべての局所線特徴の位置及び姿勢の概略値、２次元画像を撮影した撮像装置の内部パラメータを用いて行われる。本実施形態では、対応の候補（計測データ特徴）として２次元画像中のエッジ特徴を検出する。エッジ特徴は微分フィルタで検出した濃度勾配の極値となる点である。図４は、本実施形態におけるエッジ検出を説明する図である。被測定物体の概略位置及び姿勢と２次元画像を撮影した撮像装置２０の校正済みの内部パラメータを用いて、形状モデルを構成する各線分の２次元画像上への投影像を算出する。

図４（ａ）に示されるように、線分の投影像は２次元画像上でも線分となる（投影された線分４０１）。次に、２次元画像上で等間隔になるように、投影された線分４０１上に制御点４０２を設定し、設定した制御点４０２ごとに、投影された線分４０１の法線方向に１次元のエッジ検出を行う（図４（ａ））。すなわち、投影された線分４０１上に設定された制御点４０２を通り、投影された線分４０１の法線方向を有する探索ライン４０３に沿って、エッジ検出が行われる。２次元画像のエッジ４０４は画素値の濃度勾配の極値として検出されるため、図４（ｂ）に示すように、近傍に複数のエッジが存在する場合には１つの制御点について複数のエッジが検出されることがある。本実施形態では、検出されるエッジをすべて仮説として保持する。

評価値算出部１５０は、画像入力部１３０で得られた２次元画像のボケの大きさおよびブレの大きさを推定する。本実施形態では、２次元画像中のエッジと直交する方向に沿った画素の輝度変化に対して、ボケ・ブレが生じたときのエッジの輝度変化を表す関数を当てはめることによって、エッジ特徴それぞれについて画像劣化評価値を算出する。２次元画像中のエッジを検出する処理は、探索部１４０によるエッジ検出と同様の処理である。処理の詳細は後述する。

正確度算出部１６０は、被測定物体の形状モデルの局所線特徴との対応付けの仮説として探索部１４０が保持したエッジ特徴のそれぞれの対応について正確度を算出する。正確度の算出は、予測値算出部１２０で得られた画像劣化予測値と、評価値算出部１５０で得られた画像劣化評価値に基づいて行われるが、その処理の詳細な内容は後述する。照合部１７０は、被測定物体の形状モデル中の各局所線特徴に対応する２次元画像上のエッジ特徴と正確度算出部１６０で算出した正確度に基づいて、被測定物体の位置及び姿勢を算出する。照合部１７０による位置及び姿勢の算出処理の詳細は後述する。

図３は第１実施形態におけるエッジ特徴と局所線特徴との対応付け、２次元画像上の被測定物体の位置及び姿勢の算出の処理手順を示すフローチャートである。まず、モデル入力部１１０は、被測定物体の形状モデルを入力し、情報処理装置１に取り込む（ステップＳ３０１）。次に、予測値算出部１２０は、２次元画像の画像劣化予測値を算出する（ステップＳ３０２）。画像劣化予測値の算出には、被測定物体の形状モデルを構成するすべての局所線特徴の位置及び姿勢の概略値、撮像装置と被測定物体との相対的な移動方向・速度、２次元画像の取得時における撮像装置の内部パラメータが用いられる。なお、本実施形態では、物体の置かれているおおよその位置や姿勢の値を局所線特徴の位置及び姿勢の概略値として用いる。２次元画像の画像劣化予測値の算出処理の詳細は図５の参照により後述する。

次に、画像入力部１３０は、２次元画像を入力し、情報処理装置１に取り込む（ステップＳ３０３）。探索部１４０は、画像入力部１３０が入力した２次元画像から形状モデルの局所線特徴の対応の候補を探索する（ステップＳ３０４）。対応の候補の探索方法は、図４により前述したとおりであり、モデル入力部１１０が入力した形状モデルのすべての局所線特徴の位置及び姿勢の概略値、および２次元画像の撮影時の撮像装置の内部パラメータに基づいてなされる。

評価値算出部１５０は、画像入力部１３０により得られた２次元画像のボケの大きさおよびブレの大きさを推定し、画像劣化評価値を算出する（ステップＳ３０５）。より詳細には、評価値算出部１５０は、画像入力部１３０で得られた２次元画像からエッジ検出を行う。そして、評価値算出部１５０は、検出したエッジ特徴と直交する方向に沿った画素の輝度変化に対してボケ・ブレが生じたときのエッジの輝度変化を表す関数を当てはめることでエッジ部分でのボケ・ブレ幅を推定し、画像劣化評価値を得る。この処理の詳細は後述する。次に、正確度算出部１６０は、予測値算出部１２０で得た画像劣化予測値と、評価値算出部１５０で得た画像劣化評価値とを用いて、探索部１４０で得た局所線特徴とエッジ特徴の対応付けの正確度を算出する（ステップＳ３０６）。この正確度の算出処理の詳細は図７の参照により後述する。そして、照合部１７０は、算出された正確度が最大となるエッジ特徴を、形状モデルの局所線特徴に当てはめることにより、計測対象物体の位置及び姿勢を算出する（ステップＳ３０７）。

図５はステップＳ３０２における画像劣化予測値を算出する処理を表すフローチャートである。本実施形態では、モデル特徴の２次元画像のボケおよび／またはブレによる像の広がりに起因する画像劣化を予測する。以下、予測値算出部１２０による画像劣化予測値算出処理を図５のフローチャートを参照して説明する。まず、予測値算出部１２０は、形状モデルのすべての局所線特徴の位置及び姿勢の概略値、撮像装置と被測定物体との相対的な移動方向・速度、および２次元画像の撮影に用いた内部パラメータを取得する（ステップＳ５０1）。本実施形態における撮像装置と被測定物体との相対的な移動方向・速度として、例えば、被測定物体を１軸方向に並進運動を行う機材（たとえばベルトコンベア）に配置したときの設計データに基づいて、その並進運動の移動量と移動方向を取得する。

次に、予測値算出部１２０は、形状モデルの中から、画像劣化予測値σ_０を算出するために、局所線特徴を選択する（ステップＳ５０２）。そして、予測値算出部１２０は、選択された１つの局所線特徴において予測されるボケの大きさＤを以下の式［数１］を用いて算出する（ステップＳ５０３）。

上記の式［数１］において、ｆは撮影レンズの焦点距離、Ｌ_ｏは仮想視点のピント位置、Ｌ_ｎは仮想視点からモデル点までの距離、Ｆはレンズの絞り値、Δｄは画素の大きさを表す。撮影レンズの焦点距離ｆや絞り値Ｆは、使用する撮像装置の仕様を参照することで取得される。ＬｎはステップＳ５０１で入力された概略位置姿勢から算出される。撮影に用いたカメラレンズの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照することにより、または、非特許文献２に記載された方法によって事前にキャリブレーションしておくことにより取得される。

次に、予測値算出部１２０は、ステップＳ５０３でボケの大きさを予測したのと同じ局所線特徴において予測されるブレの大きさを算出する（ステップＳ５０４）。本実施形態ではブレ量として露光時間中における各局所線特徴の２次元画像上での移動量を用いる。具体的には、形状モデルを２次元画像に投影した画像から検出される、投影局所線特徴の露光時間中における画像面での移動量が用いられる。ステップＳ５０４では、モデル投影画像で得られる投影局所線特徴のヤコビアンを算出し、投影局所線特徴のヤコビアンとステップＳ５０２で入力した撮像装置と被測定物体との相対的な移動方向・速度に基づいて、投影局所線特徴のブレの大きさを算出する。

投影局所線特徴のヤコビアンとは、位置姿勢６自由度のパラメータが微小に変化したときに、画像上の投影局所線特徴が変化する割合を表す値である。被測定物体の概略位置姿勢ｓに基づいて、投影局所線特徴に対応するエッジ特徴のブレ量を（ｄｕ、ｄｖ）、投影局所線特徴の法線方向を（ｎ_ｕ、ｎ_ｖ）（単位ベクトル）と表すと、対応間の符号付距離ｅｒｒ_２Ｄは以下の式［数２］より算出できる。なお、ｄｕ＝ｕ’−ｕ、ｄｖ＝ｖ’−ｖとする。

ここで、被測定物体の位置及び姿勢ｓは６次元ベクトルであり、対象物体の位置を表す３つの要素（ｓ_１、ｓ_２、ｓ_３）と、姿勢を表す３つの要素（ｓ_４、ｓ_５、ｓ_６）からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す３次元ベクトルなどによって表現される。この位置及び姿勢ｓの各パラメータで、対応間距離ｅｒｒ_２Ｄを偏微分することによって、以下の式［数３］のように投影局所線特徴のヤコビアンを算出する。

以上のようにしてステップＳ５０２で選択された投影局所線特徴のヤコビアンが計算される。この投影局所線特徴のヤコビアンを用いて、２次元画像の露光時間ｔ_ｉ中に対象物体が相対位置姿勢速度Ｖで動くことによって発生する、局所線特徴と２次元画像中のエッジ特徴との間の距離変化Ｂは、以下の式［数４］により計算できる。

Ｂはスカラーであり、露光時間中に画像平面上で、投影局所線特徴の２次元位置が移動する量を表す。以上の処理を全ての局所線特徴に対して行い、全ての局所線特徴に対してブレ量を算出する。

次に、予測値算出部１２０では、ステップＳ５０３で算出したボケの大きさＤとステップＳ５０４で算出したブレの大きさＢから、ステップＳ５０２で選択した局所線特徴の画像劣化予測値σ_０を、以下の式［数５］を用いて算出する（ステップＳ５０５）。

すべての局所線特徴について画像劣化予測値σ_０の算出が終わるまで、ステップＳ５０２〜Ｓ５０５の処理を繰り返す（ステップＳ５０６）。こうして、全ての局所線特徴に対して画像劣化予測値σ_０の算出を終えたら本処理を終了する。

次に、ステップＳ３０５における、ボケ・ブレの大きさを２次元画像から評価する処理を説明する。本実施形態では、計測データ特徴の２次元画像におけるボケおよび／またはブレによる像の広がりに起因する画像劣化を評価する。図６は、ステップＳ３０５において評価値算出部１５０が実行する画像劣化評価値の算出処理を示すフローチャートである。評価値算出部１５０は、ステップＳ３０４で検出された対応の候補であるエッジ特徴のうちの１つを選択し（ステップＳ６０１）、選択したエッジ特徴について画像劣化評価値を算出する（ステップＳ６０２）。

評価値算出部１５０による画像劣化評価値の算出について説明する。評価値算出部１５０は、ステップＳ６０１で選択したエッジ特徴の位置とエッジの法線方向から、そのエッジ特徴のボケ・ブレの大きさを算出する。本実施形態で用いるエッジモデルとしては、以下の式［数６］で表されるような誤差関数ｅｒｆを用いて当てはめを行い、σをボケ・ブレの大きさとして求める。

数６において、ｘ_０、ｙ_０はエッジのある着目画素位置、ｒは着目画素位置からの距離、θはエッジの法線方向、ｔは着目画素からの探索範囲、σはボケの大きさとブレの大きさを統合した値（画像劣化評価値）を示す。なお、ｔは任意の正の値である。

画像劣化評価値σを、以下の式［数７］で表す評価関数Ｅを最急降下法や、Ｌｅｖｅｎｂｅｒｇ‐Ｍａｒｑｕａｒｄｔ法による繰り返し演算等で最小化することで推定する。

数７において、Ｉ（ｘ，ｙ）は座標（ｘ，ｙ）における撮影画像の輝度値を示す。

ステップＳ３０４で検出されたすべてのエッジ特徴について、上述したボケ・ブレの大きさの評価を終えるまでステップＳ６０１とＳ６０２の処理が繰り返される（ステップＳ６０３）。ステップＳ３０４で検出されたすべてのエッジ特徴についてボケ・ブレの大きさの評価を終えると本処理は終了する。

図７はステップＳ３０６における正確度の算出処理を示すフローチャートである。本実施形態では、予測値算出部１２０が算出した画像劣化の度合いである画像劣化予測値と、評価値算出部１５０が評価した画像劣化の度合いである画像劣化評価値が近いほど高い値となるように正確度が算出される。まず、正確度算出部１６０は、被測定物体の形状モデル中の局所線特徴を一つ選択する（ステップＳ７０１）。次に、正確度算出部１６０は、ステップＳ７０１で選択した局所線特徴との対応付けの候補として、ステップＳ３０４において探索部１４０が探索したエッジ特徴（対応の候補）を一つ選択し（ステップＳ７０２）、その正確度を算出する（ステップＳ７０３）。以下、正確度の算出について説明する。

正確度算出部１６０は、評価値算出部１５０の算出結果を参照して、ステップＳ７０２で選択したエッジ特徴に対応する画像劣化評価値σを取得する。また、正確度算出部１６０は、ステップＳ７０１で選択した局所線特徴の画像劣化予測値σ_０を取得する。正確度算出部１６０は、画像劣化予測値σ_０と画像劣化評価値σとから正確度Ｔを以下の式［数８］に従って算出する。

ステップＳ７０１で選択された局所線特徴について探索部１４０が探索したすべての対応の候補（エッジ特徴）について正確度Ｔを算出し終えるまで、ステップＳ７０２〜Ｓ７０３を繰り返す。すべての対応の候補（エッジ特徴）について正確度Ｔの算出を終えると、処理はステップＳ７０５に進む。正確度算出部１６０は、以上の処理（ステップＳ７０１〜Ｓ７０４）を、被測定物体の形状モデル中のすべての局所線特徴について終えるまで繰り返す。すべての局所線特徴について終えると、本処理を終了する。

図８はステップＳ３０７における、照合部１７０による位置姿勢算出の処理手順を説明するフローチャートである。照合部１７０は、計測データに形状モデルが当てはまるように、非線形最適化手法の一つであるＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いて計測対象物体の位置及び姿勢（以下、６次元ベクトルｓで表す）の概略値を反復演算によって繰り返し補正する。

まず、照合部１７０は、初期化処理を行う（ステップＳ８０１）。本実施形態では、被測定物体の置かれているおおよその位置や姿勢の値が概略値として照合部１７０に入力される。この概略値は、ステップＳ３０２で用いたのと同じものである。次に、照合部１７０は、モデルの局所線特徴と照合するエッジ特徴を選択する（ステップＳ８０２）。選択には正確度算出部１６０が算出した、エッジ特徴と局所線特徴との対応の正確度Ｔを用いる。たとえば、一つの局所線特徴に関して複数のエッジ特徴が対応付け仮説として検出されている場合は、正確度Ｔが一番高い対応の候補（エッジ特徴）を当該局所線特徴に対応づけて以後の処理を行う。

続いて、照合部１７０は、位置及び姿勢を算出するための係数行列と誤差ベクトルの算出を行う（ステップＳ８０３）。ここで係数行列の各要素は、画像上での点と直線の距離を位置及び姿勢の関数としたときの、位置及び姿勢の各要素に関する一次の偏微分係数である。誤差ベクトルは、エッジについては投影された線分と検出されたエッジの画像上での符号付き距離、点群データについては点とモデルの面の３次元空間中での符号付き距離である。

次に、係数行列の導出について説明する。図９は、投影像である線分９０１と検出されたエッジ９０２の関係を説明する図である。図９では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。ある制御点９０３（投影された各線分を画像上で等間隔に分割した点）の画像上での位置を（ｕ_０，ｖ_０）、その制御点９０３が所属する線分９０１の画像上での傾きをｕ軸に対する傾きθと表す。傾きθは、線分の両端の３次元座標を被測定物体の位置姿勢sに基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。線分９０１の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、制御点９０３の対応点９０４の２次元画像上の座標を（ｕ’，ｖ’）とする。ここで、点（ｕ’，ｖ’）を通り、傾きがθである直線（図９の破線により示されるエッジ９０２）上の点（ｕ，ｖ）は、以下の式［数９］のように表される（θは定数とする）。ここで、ｄ＝ｕ’ｓｉｎθ−ｖ’ｃｏｓθ（定数）である。

制御点の画像上での位置は計測対象物体の位置及び姿勢により変化する。また、計測対象物体の位置及び姿勢の自由度は６自由度である。すなわち位置姿勢ｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す３次元ベクトルなどによって表現される。位置及び姿勢により変化する制御点の画像座標（ｕ、ｖ）は（ｕ_０、ｖ_０）の近傍で１次のテイラー展開によって以下の式［数１０］のように近似できる。但し、数１０において、Δｓ_ｉ（ｉ＝１、２、…、６）はｓの各成分の微小変化を表す。

位置及び姿勢の概略値と実際の位置及び姿勢の差がそれほどないと仮定すると、正しいｓによって得られる制御点の画像上での位置は式［数９］が表す直線上にあると仮定できる。式［数１０］によって近似されるｕ、ｖを式［数９］に代入することにより、式［数１１］が得られる。

位置及び姿勢の概略値によって算出された各制御点の奥行き（視点から制御点までの距離＝ｚ）を、画像上の誤差に乗じることによって３次元空間中の誤差に変換する。

解くべき連立方程式は式［数１２］のようになる。

式［数１２］において、z₁、z₂・・・は各エッジの奥行きを表している。ここで式［数１２］を式［数１３］のように表す。［数１３］において、Ｊは係数行列、Ｅは誤差ベクトル、Δｓは補正値である。

線形連立方程式の係数行列Ｊを算出するための偏微分係数の算出は、例えば非特許文献２に開示されている方法によって行うことができる。

次に、照合部１７０は、式［数１３］をもとに、行列Ｊの一般化逆行列（J^T・J）⁻¹・J^Tを用いて位置及び姿勢の補正値Δsを最小二乗基準で求める（ステップＳ８０４）。しかしながら、エッジや点群データには誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジや点群データでは式［数１２］の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える。重みは例えば式［数１４］に示すようなＴｕｋｅｙの関数により与える。

上記式［数１４］において、ｃ_１、ｃ_２は定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であればなんでもよい。

各計測データ（エッジまたは点群データ）に対応する重みをｗ_ｉとする。ここで式［数１５］のように重み行列Ｗを定義する。

重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には、式［数１４］に従って算出された重みｗ_ｉが入る。この重み行列Ｗ用いて、式［数１３］を式［数１６］のように変形する。

式［数１６］を式［数１７］のように解くことにより補正値Δｓを求める。

図８に戻り、照合部１７０は、ステップＳ８０４において算出された位置及び姿勢の補正値Δｓにより、位置及び姿勢の概略値を補正する（ステップＳ８０５）。

続いて、照合部１７０は、収束判定を行い、収束していれば終了し、収束していなければ処理をステップＳ８０２に戻す（ステップＳ８０６）。収束判定では、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合（補正前後の差分が所定の閾値以下になった場合）に収束したと判定する。なお、上記では非線形最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いた例を説明した。しかしながら、非線形最適化手法はこれに限るものではなく、Ｎｅｗｔｏｎ−Ｒａｐｈｓｏｎ法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。

本実施形態における被測定物体のモデルとして保持する形状情報は、対象形状を表す２次元もしくは３次元の幾何情報であれば良く、表現形式に特に制限はない。例えば、２次元の幾何情報を表す場合は単純な２次元点の集合や、２次元ラインの集合などで表現してもよい。また、３次元の幾何情報を表す場合は、単純な３次元点の集合や、稜線を表す３次元ラインの集合、３次元点３点で構成される線の集合で表されるポリゴン形式の形状情報などで表しても良い。

また、ステップＳ５０１において入力される撮像装置と被測定物体との相対的な移動方向・速度として、被測定物体がある方向に並進運動を行うことを想定した移動方向および移動量の情報を入力する例を示したが、本実施形態はこれに限られるものではない。例えば、被測定物体に上述のセンサを取り付け、その情報から撮像装置と被測定物体との相対位置姿勢の速度を算出しても良い。また、撮像装置が動くような場面では、撮像装置の動きを取得してもよい。例えば、ロボットに撮像装置を取り付けた場合、ロボットモーション情報を撮像装置と被測定物体との相対的な移動方向・速度の入力としてもよい。あるいは撮像装置に磁気センサや超音波センサなど６自由度の位置及び姿勢を計測する物理センサを取り付け、それらの情報を入力してもよい。本実施形態では、撮像装置と被測定物体との相対的な位置姿勢の速度が算出できる限り、いずれの方法を用いても良く、手段・機器の選択に特に制限はない。

また、予測値算出部１２０において画像劣化予測値として推定される量は、上述の情報に限るものではない。たとえば、撮像装置と被測定物体の相対的な移動によって２次元画像が劣化する量を表すことが出来る値であれば良く、その計算方法および表現に特に制限はない。例えば、被測定物体が撮影された２次元画像のエッジ特徴のブレの大きさを、３次元空間に逆投影して算出した３次元空間中における移動量として表しても良い。また、ボケの大きさＤやブレの大きさＢを基にして作成した点広がり関数（Point Spread Function：ＰＳＦ）を画像劣化評価値としてもよい。また、上記実施形態では、ボケおよびブレの両方の影響を考慮して予測値を取得したが、どちらか片方のみの影響を考慮した予測値を取得するようにしてもよい。

また、探索部１４０での対応の候補となる計測データ特徴として、本実施形態ではエッジ特徴を用いたが、これに限られるものではない。たとえば、点特徴など２次元画像上の位置を特定できる特徴であればどのような特徴であってもよい。また、特定の種類の特徴のみを用いるのではなく、複数の種類の特徴（たとえば、点とエッジ）とモデル特徴の対応を検出してもよい。

また、評価値算出部１５０において、単一画像からボケ・ブレの大きさを推定する手法は、上述した手法に限るものでない。例えば、ボケ・ブレの広がりをガウス分布状に広がると仮定し、ボケ・ブレが生じているエッジ部分の輝度分布を、最小二乗法を用いてガウス関数にフィッティングしたときに得られる標準偏差からボケ・ブレの大きさを求めてもよい。また本実施形態ではエッジ特徴単位で画像劣化評価値を算出したがこれに限るものではない。例えば、２次元画像を方形の部分領域に分け、部分領域ごとに画像劣化評価値を算出してもよい。

また、正確度算出部１６０において、式［数８］を用いて正確度を算出したが、正確度の算出方法はこれに限られるものではない。正確度を算出する関数は、画像劣化評価値と画像劣化予測値との差が大きいときには正確度を小さく、画像劣化評価値と画像劣化予測値との差が小さいときには正確度を大きく算出するような関数であればよい。たとえば、例えばガウス関数、Ｔｕｋｅｙの関数、Ｈｕｂｅｒの関数などを利用することもできる。

また、照合部１７０において、正確度の一番高い局所線特徴とエッジ特徴のペアを照合したが、これに限られるものではない。算出された正確度の大きいペアが優先して選択されればよく、たとえば、正確度が一番高かったペアを選択したとしても、正確度がある閾値以下であれば照合に利用しないようにしてもよい。同様に、局所線特徴とエッジ特徴のペアが１つしか検出されなかった場合でも、その正確度が閾値以下であれば、当該ペアを照合に利用しないようにしてもよい。さらに、本実施形態において照合部１７０は、被測定物体の位置及び姿勢を取得したがこれに限るものではない。例えば、事前に複数の位置・向きで２次元画像上における局所線特徴の位置を保持しておき、パターンマッチングによって局所線特徴とエッジ特徴を照合し、概略の位置・向きを同定する、あるいは物体の種類を特定してもよい。

また、ステップＳ３０２、ステップＳ５０１、ステップＳ８０１では、被測定物体のモデルの位置および姿勢の概略値として、物体の置かれているおおよその位置や姿勢の値が与えられた。しかしながら概略の位置および姿勢の入力方法はこれに限るものではない。たとえば、前回（前時刻）の計測で得られた計測値を概略の位置及び姿勢として用いてもよい。また、過去の位置及び姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢と推定された速度・加速度から現在の位置及び姿勢を予測して、概略の位置及び姿勢として用いてもよい。また、他のセンサを用いた物体の位置及び姿勢の計測が可能である場合には、該センサによる出力値を位置及び姿勢の概略値として用いてもよい。たとえば、トランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサ、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサがあげられる。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめわかっている場合に、その値を概略値として用いるようにしてもよい。

以上述べたように第１実施形態では、被測定物体が撮影された２次元画像で得られる画像のボケ、ブレの大きさから対応付けの正確度を算出し、モデル特徴と計測データ特徴の照合を行う方法について説明した。これにより、モデル特徴と計測データ特徴を正しく照合することが可能になる。

＜変形例１＞
第１実施形態では、照合部１７０において、正確度の高い局所線特徴とエッジ特徴のペアを選択し、照合していたが、これに限られるものではない。算出された正確度の高いペアが優先して照合処理に用いられれば良く、例えば、各局所線特徴とエッジ特徴の対応の正確度の大きさを、位置姿勢推定を行うときの各エッジ特徴毎の重みとして設定し、この重みづけに基づいて照合を行ってもよい。この重みづけの方法について説明する。まず式［数１５］で算出される重み行列Ｗの対角成分にそれぞれの局所線特徴とエッジ特徴対応の正確度Ｔを与える。この重み行列Ｗを用いて式［数１７］のように解くことにより、位置及び姿勢の補正値Δｓを求め、位置及び姿勢の概略値を補正する。以上の方法により、算出された正確度の高いペアを優先して照合することができる。

なお、重み行列Ｗの対角成分に与える値として正確度Ｔそのものを与えていたが、これに限るものではない。正確度が高いペアは大きな重みを与え、正確度が低いペアには小さな重みを与える値であればなんでもよく、全ての正確度を正確度の最大値で割ったときの比を重みとして与えてもよい。

以上述べたように変形例１では、被測定物体が撮影された２次元画像で得られる画像のボケ、ブレの大きさから対応付けの正確度を各データ特徴の重みとして設定し、局所線特徴とエッジ特徴の照合を行う方法について説明した。これにより、高精度に照合することが可能になる。

＜第２実施形態＞
第１実施形態では、モデルから２次元画像上で起きるボケ、ブレの大きさを予測し、２次元画像で得られる画像のボケ、ブレの大きさと比較して正確度を算出し、その正確度を局所線特徴との対応付けに利用する方法について説明した。第２実施形態では、予めボケ、ブレの大きさを算出した２次元画像から、各局所線特徴の画像劣化予測値を算出し、実測した画像のボケ、ブレの大きさと比較して対応の正確度を算出し、正確度が一番高い値を示した特徴同士を対応付けする。以下、第２実施形態について説明する。第２実施形態における情報処理装置の構成や位置及び姿勢の計測の処理手順は第１実施形態（図１、図３）と同様である。

第２実施形態では、ステップＳ３０２において、予測値算出部１２０は、以前（前時刻）に得られた２次元画像からボケの大きさおよびブレの大きさを推定する。２次元画像からのボケ・ブレの大きさの推定方法は、第１実施形態の評価値算出部１５０によるエッジ特徴部分のボケ・ブレの大きさの推定方法と同様である。但し、エッジ特徴部分の代わりにモデル特徴の投影位置においてボケ・ブレの大きさが計測される。たとえば、予測値算出部１２０は、前回に得られた２次元画像に、被測定物体の概略の位置姿勢を用いてその形状モデル（たとえば局所線特徴）を投影する。そして、その投影位置についてステップＳ６０１、Ｓ６０２で説明した方法で画像劣化評価値を算出し、これを画像劣化予測値とする。したがって予測値算出部１２０により得られた局所線特徴の画像劣化予測値σ_１は、２次元画像上に形状モデルを投影したときの、モデル特徴（たとえば局所線特徴）の座標位置におけるボケ、ブレの大きさである。

正確度算出部１６０は、ステップＳ３０２で予測値算出部１２０により得られた画像劣化予測値と、ステップＳ３０５で評価値算出部１５０により得られた画像劣化評価値とから、探索部１４０が検索したエッジ特徴のそれぞれについて正確度を算出する。なお、探索部１４０が探索したエッジ特徴とは、被測定物体のモデルの局所線特徴の対応の候補であり、仮説としてメモリに記憶されている（ステップＳ３０４）。正確度Ｔは、モデル中の局所線特徴と対応する仮説として保持した２次元画像上のエッジ特徴の画像劣化評価値σとモデル中の局所線特徴の画像劣化予測値σ_１を用いて、以下の式［数１９］にしたがって算出される。なお、２次元画像中のエッジ特徴とモデル中の局所線特徴との対応付けの仮説が複数検出された場合には、候補に挙がったすべてのエッジに対して、以下の式［数１９］を用いて正確度Ｔが算出される。

以上述べたように、第２実施形態では、事前に計測した２次元画像を用いてボケやブレの大きさを算出し、ボケやブレの大きさが近い特徴同士の対応の正確度を高くすることによって対応付けを行う。これにより、特徴同士の照合を正しく行うことが可能になる。

なお、第２実施形態における予測値算出部１２０では、前回（前時刻）に得られた２次元画像から、ボケの大きさおよびブレの大きさを推定したがそれに限るものではない。被測定物体の代わりに、それを模擬できる物体（以下、模擬物体）を撮影した２次元画像を「前回に得られた２次元画像」として用いてもよい。また、事前に複数の位置・姿勢で被測定物体もしくは模擬物体を撮影した２次元画像上で検出したボケの大きさおよびブレの大きさをテーブル化しておき、このテーブルを参照するようにしてもよい。すなわち、予測値算出部１２０は、このテーブルを参照することにより、被測定物体の概略の位置及び姿勢に対応するボケ・ブレの大きさを画像劣化予測値として取得してもよい。このほか、実写の２次元画像の代わりに、形状モデルを２次元に投影して作成したＣＧ画像を用いて画像劣化予測値を得るようにしてもよい。このようなＣＧ画像上でボケ・ブレの現象をシミュレートするに当たっては、公知の手法、例えば非特許文献３に開示されている方法を用いることができる。

以上のように、第２実施形態の方法を用いることで、事前に計測した２次元画像や作成したＣＧ画像を用いてボケやブレの大きさから対応付けの正確度を算出することによって、モデル特徴と計測データ特徴の正しく照合することが可能になる。

＜第３実施形態＞
第１および第２実施形態においては、２次元画像で生じるであろう画像のボケ、ブレの大きさを画像劣化予測値（σ_０、σ_１）として推定し、２次元画像で得られたエッジ特徴とモデルの局所線特徴との対応付けの選択に利用した。第３実施形態では、実測した３次元計測点（以下、３次元点と記載する）とモデルの面特徴との対応付けに画像劣化予測値を利用する。第３実施形態では、局所面特徴の２次元画像上でのボケやブレの大きさと、実測した３次元点の３次元位置を２次元画像上に投影したときの２次元画像上のボケやブレの大きさとが近い特徴の対応について高い正確度を付与し、対応付けの選択を行う。

図１０は第３実施形態における情報処理装置３の機能構成例を表した図である。なお、情報処理装置３のハードウエア構成は、図１（ａ）と同様である。情報処理装置３は、情報処理装置１の構成に加えて、３次元点入力部２１０と位置算出部２２０を有する。また、図１０に示す構成は、本発明の情報処理装置の適用例となる構成である。図１０に示される各機能は、ＣＰＵ１０１がＲＯＭ１０２に記憶されているプログラム、および／または、外部記憶装置１０５からＲＡＭ１０３に展開されたプログラムを実行することにより実現される。なお、各機能部の一部またはすべてを専用のハードウエアで実現してもよいことは明らかである。

３次元点入力部２１０は、被測定物体表面の点群の３次元座標を取得する。第３実施形態では、予め距離センサを用いて取得された３次元点群の３次元座標を外部に記憶された記憶装置から取得するものとするが、これに限られるものではない。たとえば、距離センサ（たとえば３次元計測装置）を用いて取得した３次元点群が入力されてもよい。

位置算出部２２０は、３次元点入力部２１０で入力された点群の３次元座標を２次元画像上に投影したときの２次元画像位置を算出する。３次元座標の２次元画像位置は、２次元画像を撮影した撮像装置の内部パラメータ、および該撮像装置と３次元点を測定するのに用いた距離センサとの相対的な位置及び姿勢とから算出される。なお、距離センサと撮像装置は相互に固定されており、両者の相対的な位置及び姿勢は変化しないと仮定し、事前にキャリブレーションしておく。例えば３次元形状が既知であるキャリブレーション物体を様々な方向から観察し、２次元画像に基づいたキャリブレーション物体の位置及び姿勢と、距離画像に基づいたキャリブレーション物体の位置及び姿勢の差をもとに、距離センサと撮像装置の相対的な位置及び姿勢を求めておく。

なお、第３実施形態におけるモデル入力部１１０で入力されるモデルは、図２（ｂ）に示すような、３次元位置と３次元法線方向から構成される物体表面上の局所的な３次元平面情報（以後、局所面特徴と称する）によって構成されるものとする。予測値算出部１２０は、第１実施形態と同様の方法で局所面特徴の劣化予測値σ_０を算出する。

探索部１４０は、画像入力部１３０が入力した２次元画像と３次元点入力部２１０が入力した３次元点とモデル入力部１１０が入力した形状モデルの局所面特徴との対応を検出する。局所面特徴と３次元点との対応付けにおいては、モデル入力部１１０で入力された被測定物体のモデルを構成するすべての局所面特徴の位置及び姿勢の概略値に基づいて局所面特徴の近傍の３次元点を検出する。近傍に３次元点が複数存在する場合には検出される３次元点のすべてを仮説として保持する。

評価値算出部１５０は、位置算出部２２０が算出した３次元点のそれぞれの２次元画像上の位置におけるボケ・ブレの大きさを画像劣化評価値σとして算出する。画像劣化評価値σの算出方法は第１実施形態と同様である。正確度算出部１６０は、予測値算出部１２０で得られた画像劣化予測値σ_０と、評価値算出部１５０で得られた画像劣化評価値σから、モデルの局所面特徴と対応付けの仮説として保持したそれぞれの３次元点特徴について対応の正確度Ｔを算出する。正確度Ｔの算出方法は第１実施形態と同様である。

第３実施形態における照合部１７０は、正確度算出部１６０で算出した正確度Ｔに基づいて、被測定物体のモデル中の各局所面特徴に対応する３次元点のうち照合に使用する３次元点を選択する。ここでは、正確度Ｔの一番高い局所面特徴と３次元点のペアが選択されるものとするがこれに限られるものではない。たとえば、正確度Ｔが一番高かったペアを選択したとしても、その正確度Ｔがある閾値以下であれば照合に利用しないようにしてもよい。同様に、局所面特徴と３次元点のペアが１つしか検出されなかった場合でも、その正確度が閾値以下であれば、当該ペアを照合に利用しないようにしてもよい。そして、照合部１７０は、選択された３次元点とを用いて、被測定物体の位置及び姿勢を算出する。照合部１７０による位置姿勢の算出方法は第１実施形態と同様の方法を用いることができる。

なお、本実施形態における３次元点の計測に用いる距離センサとしては、対象に照射したレーザ光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するTime-of-flight方式であってもよい。これらのアクティブ式の距離センサは、表面のテクスチャが少ない物体が対象であるとき好適である。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。パッシブ式の距離センサは、表面のテクスチャが十分ある物体を対象とする場合に好適である。その他、３次元点を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

以上述べたように第３実施形態では、局所面特徴の２次元画像上でのボケやブレの大きさと、実測した３次元点の３次元位置を２次元画像上に投影したときの２次元画像上のボケやブレの大きさとが近い特徴の対応について高い正確度を付与し、モデル特徴と３次元計測点の照合を行う方法について説明した。これにより、モデル特徴と３次元計測点を正しく照合することが可能になる。

＜変形例２＞
第３実施形態では、照合部１７０において、正確度の高い局所面特徴と３次元点のペアを選択し、照合していたが、これに限られるものではない。各局所面特徴と３元点の対応の正確度を、位置姿勢推定を行う時の３次元点毎の重みとして設定するようにしてもよい。たとえば、変形例１と同様に、正確度の高さを各３次元点毎の重みとして設定し、この重みづけに基づいて照合を行ってもよい。この重みづけの方法については変形例１と同様に式［数１５］で算出される重み行列Ｗの対角成分にそれぞれの局所面特徴と３次元点の対応の正確度Ｔを与える。この重み行列Ｗを用いて式［数１７］のように解くことにより、位置及び姿勢の補正値Δｓを求め、位置及び姿勢の概略値を補正する。以上の方法により、算出された正確度の高いペアを優先して照合することができる。

なお、重み行列Ｗの対角成分に与える値として正確度Ｔそのものを与えていたが、変形例１と同様にこれに限るものではない。正確度が高いペアは大きな重みを与え、正確度が低いペアには小さな重みを与える値であればなんでもよい。

以上述べたように変形例２では、被測定物体が撮影された２次元画像で得られる画像のボケ、ブレの大きさから対応付けの正確度を各データ特徴の重みとして設定し、局所面特徴と３次元点の照合を行う方法について説明した。これにより、高精度に照合することが可能になる。

＜変形例３＞
第３実施形態における、ステップＳ５０４で行うブレの大きさの算出方法は以下のような方法でもよい。ブレの大きさとして露光時間中における各局所面特徴の３次元空間中の移動量を用いる。具体的には、形状モデルの概略位置姿勢ｓに基づき各局所面特徴のヤコビアンを算出し、各局所面特徴のヤコビアンとステップＳ５０２で入力した撮像装置と被測定物体との相対的な移動方向・速度から各局所面特徴のブレの大きさを算出する。

局所面特徴のヤコビアンとは、位置姿勢６自由度のパラメータが微小に変化したときに、３次元空間中の局所面特徴が変化する割合を表す値である。被測定物体の概略位置姿勢ｓに基づいて、局所面特徴に対応する３次元点のブレ量を（ｄｘ、ｄｙ、ｄｚ）、局所面特徴の法線方向を（ｎ_ｘ、ｎ_ｙ、ｎ_ｚ）（単位ベクトル）と表すと、対応間の符号付距離ｅｒｒ_３Ｄは以下の式［数２０］より算出できる。

そして、局所線特徴のヤコビアンと同様に位置及び姿勢ｓの各パラメータで、対応間距離ｅｒｒ_３Ｄを偏微分することによって、以下の式［数２１］のように局所面特徴のヤコビアン行列を算出する。

以上のようにしてステップＳ５０２で選択された投影局所線特徴のヤコビアンが計算される。この投影局所線特徴のヤコビアンを用いて、２次元画像の露光時間ｔ_ｉ中に対象物体が相対位置姿勢速度Ｖで動くことによって発生する、局所面特徴３次元点との間の距離変化Ｂは、以下の式［数２２］により計算できる。

Ｂはスカラーであり、露光時間中に三次元空間中の局所面特徴の３次元位置が移動する量を表す。以上の処理を全ての局所面特徴に対して行い、全ての局所面特徴に対してブレ量を算出する。

以上述べたように変形例３では、局所面特徴の３次元空間中での移動量からブレの大きさを算出する方法について述べた。この方法を用いることにより、正確にモデル特徴のブレ量の予測値を得ることが可能になる。

＜変形例４＞
第３の実施形態におけるステップＳ８０３で行う、位置及び姿勢を算出するための係数行列と誤差ベクトルの算出方法は以下のような方法でもよい。

カメラ座標で表される点群の３次元座標は、計測対象物体の位置及び姿勢sを用いて計測対象物体の座標系における３次元座標（x，y，z）に変換される。概略の位置及び姿勢によって、点群データ中のある点が計測対象物体座標（x₀，y₀，z₀）に変換されるとする。（x，y，z）は計測対象物体の位置及び姿勢により変化するものであり、（x₀，y₀，z₀）の近傍で１次のテイラー展開によって式［数２３］のように近似できる。

点群データ中のある点に対応付けられた局所面特徴の、計測対象物体座標系における方程式をax+by+cz＝e（a²+b²+c²=1、ａ，ｂ，ｃ，ｅは定数）とする。正しいｓによって変換される（x，y，z）は、平面の方程式ax+by+cz＝eを満たすと仮定する。式［数２３］を平面の方程式に代入すると、式［数２４］が得られる。

式［数２４］はsの各成分の微小変化Δsi（i=1,2,・・・,6）についての方程式であるため、式［数２５］のようなΔsiに関する線形連立方程式を立てることができる。

ここで式［数２５］を式［数１３］のように表す。この係数行列Ｊと誤差ベクトルＥを用いて第１、２の実施形態と同様の方法で位置及び姿勢を算出する。以上の変形例３の方法を用いることにより、より正確に計測物体の位置及び姿勢を算出することができる。

＜第４実施形態＞
第１および第２実施形態では局所線特徴とエッジ特徴の対応のみから、第３実施形態では局所面特徴と３次元点の対応のみから、モデル特徴と計測データ特徴の照合を行ったがこれに限られるものではない。局所線特徴とエッジ特徴の対応および局所面特徴と３次元点の対応の、両方の対応を用いて照合してもよい。本実施形態の方法によって、被測定物体が撮影された２次元画像で得られる画像のボケ、ブレの大きさから対応付けの正確度を算出することによって、実測したエッジ特徴と局所線特徴、および３次元点と局所面特徴との両方を正しく照合することが可能になる。

第４の実施形態における情報処理装置の構成は、第１、２および３の実施形態における構成と同様である。第４の実施形態における照合部１７０は、正確度算出部１６０で算出した、エッジ特徴と局所線特徴との対応の正確度および、３次元点と局所面特徴と対応の正確度に基づいて、
・被測定物体のモデル中の各局所線特徴および局所面特徴に対応するエッジ特徴、
・３次元点のうち照合に使用するエッジ特徴、および３次元点、を選択する。続いて選択した対応に基づいて、位置及び姿勢を算出するための係数行列と誤差ベクトルの算出を行う。本実施形態においては、エッジ特徴と３次元点を併用して位置及び姿勢を求めるために、式［数１２］と式［数２５］を合わせて、式［数２６］のようなΔｓ_ｉに関する線形連立方程式を立てることができる。

ここで式［数２６］を式［数１３］のように表す。この係数行列Ｊと誤差ベクトルＥを用いて第１、２、３実施形態と同様の方法で位置及び姿勢を算出する。

またこれに限らず変形例１および２で述べたように、正確度の高さを各３次元点毎の重みとして設定し、この重みづけに基づいて照合を行ってもよい。正確度Ｔそのものを与えるだけでなく、変形例１、２と同様に正確度が高いペアには大きな重みを与え、正確度が低いペアには小さな重みを与える値であればなんでもよい。

以上述べた第４実施形態の方法を用いることで、局所線特徴とエッジ特徴の対応、および局所面特徴と３次元点の対応、両方を用いて照合することができる。本実施形態の方法によって、それぞれの対応で照合を行うよりもより正確にモデル特徴と計測データの照合を行うことが可能になる。

＜第５実施形態＞
情報処理装置１、３の好適な適用例の一つについて、以下に説明する。すなわち、撮像装置より得られる２次元画像を基に被測定物体の位置姿勢を推定し、産業用ロボットアームによりその把持などを行う利用例があげられる。以下、図１１を用いて、情報処理装置１，３のロボットシステムへの適用例を説明する。図１１は、情報処理装置１、３による位置姿勢の推定結果に基づいてロボット６０を用いて被測定物体４０を把持するロボットシステムの構成例を示す図である。

ロボット６０は、たとえば回転および/または並進移動軸からなる可動軸を有し、各可動軸はロボットコントローラ６１により駆動制御される。たとえば、ロボット６０は、ロボットコントローラ６１により指令された位置に手先を移動させ、物体の把持などを行う。被測定物体４０は、作業台に置かれる位置が変わるため、現在の被測定物体４０の位置姿勢を推定し、ロボットの把持制御を行う必要がある。撮像装置２０は、通常の２次元画像を撮影するカメラであり、産業用ロボットアームの手先等の被測定物体４０を撮像できる位置に設置する。情報処理装置１、３は、撮像装置２０から得られる２次元画像を基に被測定物体４０の位置姿勢を推定する。情報処理装置１、３で推定された被測定物体４０の位置姿勢は、ロボットコントローラ６１に入力され、ロボットコントローラ６１は入力された位置姿勢の推定結果に基づいて被測定物体４０の把持などを行うようにロボットアームを制御する。

以上のように、第５実施形態のロボットシステムによれば、より正確に位置姿勢推定を行う情報処理装置１，３から被測定物体４０の位置姿勢の推定結果を得ることができる。そのため、ロボット６０は被測定物体４０をより確実に把持することが可能である。

＜効果＞
以上説明したように、第１実施形態によれば、被測定物体が撮影された２次元画像で得られる画像のボケ、ブレの大きさから対応付けの正確度を算出することによって、モデル特徴と計測データ特徴を正しく照合することができる。また、第３実施形態によれば、被測定物体が撮影された２次元画像で得られる画像のボケ、ブレの大きさから対応付けの正確度を算出することによって、実測した３次元点と３次元形状モデルの面特徴とを正しく照合することが可能になる。第４実施形態によれば、被測定物体が撮影された２次元画像で得られる画像のボケ、ブレの大きさから対応付けの正確度を算出することによって、２次元画像中のエッジ特徴と３次元形状モデルの線特徴、および実測した３次元点と３次元形状モデルの面特徴とを同時に正しく照合することが可能になる。また、第２実施形態によれば、過去に計測対象となった２次元画像や作成したＣＧ画像を用いて事前に取得されたボケやブレの大きさから対応付けの正確度を算出することによって、モデル特徴と計測データ特徴の照合を正しく行うことが可能になる。さらに、第５実施形態によれば、被測定物体の位置姿勢を推定し、推定結果に基づいてロボットシステムが被測定物体の把持および移動が可能となる。

＜定義＞
なお、被測定物体の形状モデルとして入力する形状情報は、単純な２次元点の集合や、２次元ラインの集合など、また、単純な３次元点の集合や、稜線を表す３次元ラインの集合、３次元点３点で構成されるポリゴンの形状情報などで表しても良い。

また、予測値算出部では、画像劣化予測値として、焦点外れによるボケの大きさと、画像平面上での平行移動によって生じるブレの大きさを、画像劣化予測値としたが、これに限るものではない。撮像装置と被測定物体の相対的な移動によって２次元画像が劣化する量を表すことが出来る値であれば良く、計算方法および表現に特に制限はない。例えば、被測定物体が撮影された２次元画像のエッジ特徴のブレの大きさを、３次元空間に逆投影して算出した３次元空間中における移動量として表しても良い。また、ボケの大きさやブレの大きさを基にして作成した点広がり関数（Point Spread Function：ＰＳＦ）を画像劣化評価値としてもよい。また、ボケおよびブレの両方の影響を考慮した予測値以外に、どちらか片方のみの影響を考慮した予測値でもよい。

画像入力部で入力される被測定物体が撮影された２次元画像は、対象像が得られる限りどの方式でもよい。例えば、グレー画像であってもよいしカラー画像であってもよい。また本発明では予め撮影された２次元画像を入力したが、これに限るものではない。撮像装置を用いて撮影された結果を入力してもよい。探索部１４０で検出する対応の候補となる計測データ特徴としては、エッジ特徴でもよいし、点特徴など２次元画像上の位置を特定できる特徴であればどのような特徴であってもよい。また、特定種類の特徴のみ用いるのではなく、複数種類の特徴（たとえば点とエッジ）とモデル特徴の対応を検出してもよい。

評価値算出部は、例えば、２次元画像中のエッジと直交する方向に沿った画素の輝度変化に対して、ボケ・ブレが生じたときのエッジの輝度変化を表す関数の当てはめによって、エッジ特徴それぞれについて画像劣化評価値を算出する。ここで、輝度変化を表す関数として、式［数６］のような誤差関数に代えてガウス関数などを用いてもよい。また、上記実施形態では、制御点に対応する対応点や３次元点の投影位置といったように特徴単位で劣化評価値を算出したが、これに限られるものではない。たとえば、２次元画像を方形の部分領域に分け、部分領域ごとに劣化評価値を算出してもよい。

正確度算出部における算出方法は、画像劣化評価値（σ）と画像劣化予測値（σ_０、σ_１）との差が大きいときには正確度を小さく、画像劣化評価値と画像劣化予測値との差が小さいときには正確度を大きく算出する方法であればよい。例えば、式［数８］に示したような算出方法の代わりにガウス関数、Ｔｕｋｅｙの関数、Ｈｕｂｅｒの関数などを用いて算出してもよい。

照合部は、正確度がある閾値以上のモデル特徴と画像特徴のペアを選択し、照合する。このときペアが１つしか検出されなかった場合も，閾値以下であれば照合に利用しなくてもよい。ただし、これに限るものではなく、正確度が高いペアを優先して照合処理に用いられるように重みづけを行ってもよい。このとき重みづけで与える値は、正確度が高いペアは大きな重みを与え、正確度が低いペアには小さな重みを与える値であればなんでもよい。また照合部は、被測定物体の位置及び姿勢を算出してもよいし、事前に複数の位置・向きでモデル特徴の位置を保持しておき、パターンマッチングによってモデル特徴と計測データ特徴を照合し、概略の位置・向きを同定、あるいは物体の種類を特定してもよい。

＜その他の実施形態＞
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１，３：情報処理装置、１１０：モデル入力部、１２０：予測値算出部、１３０：画像入力部、１４０：探索部、１５０：評価値算出部、１６０：正確度算出部、１７０：照合部、２１０：３次元点入力部、２２０：位置算出部

Claims (18)

1. 被測定物体の形状を表す形状モデルに基づいて、前記被測定物体を撮像手段により撮像した画像に生じる該被測定物体の画像劣化を予測する予測手段と、
   前記被測定物体を前記撮像手段により撮像することにより得られた２次元画像について、前記形状モデルのモデル特徴に対応する計測データ特徴を探索する探索手段と、
   前記探索手段で探索された計測データ特徴について前記２次元画像を用いて画像劣化を評価する評価手段と、
   前記予測手段で予測した画像劣化と前記評価手段で評価した画像劣化に基づいて、前記モデル特徴と前記計測データ特徴の対応の正確度を算出する算出手段と、
   前記モデル特徴と前記計測データ特徴の対応の正確度に基づいて、前記形状モデルと前記２次元画像中の被測定物体とを照合する照合手段と、を備えることを特徴とする情報処理装置。
2. 前記画像劣化は、２次元画像のボケおよび／またはブレによる像の広がりに起因する画像劣化であることを特徴とする請求項１に記載の情報処理装置。
3. 前記評価手段は、前記２次元画像の前記計測データ特徴へ誤差関数をあてはめることにより前記画像劣化を評価することを特徴とする請求項１または２に記載の情報処理装置。
4. 前記予測手段は、前記形状モデル、与えられた前記被測定物体の位置および姿勢、前記被測定物体と前記撮像手段との相対位置姿勢の変化、および前記撮像手段の内部パラメータに基づいて、前記画像劣化を予測することを特徴とする請求項１乃至３のいずれか１項に記載の情報処理装置。
5. 前記予測手段は、前記被測定物体または前記被測定物体を模擬した物体を撮影することにより予め得られた画像に前記形状モデルを投影したときの、前記予め得られた画像のモデル特徴の座標位置における画像劣化を取得することを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記予測手段は、前記被測定物体の形状モデルを２次元に投影して作成したＣＧ画像に基づいて画像劣化を取得することを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
7. 前記算出手段は、前記予測手段で予測した画像劣化の度合いと前記評価手段が評価した画像劣化の度合いが近いほど高い値となるように前記正確度を算出することを特徴とする請求項１乃至６のいずれか１項に記載の情報処理装置。
8. 前記照合手段は、前記正確度に基づいて、前記モデル特徴と前記計測データ特徴の対応を選択すること、
   を特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
9. 前記照合手段は、前記算出手段により算出された正確度の大きい対応を優先して選択することを特徴とする請求項８に記載の情報処理装置。
10. 前記照合手段は、前記正確度に基づいて、前記計測データ特徴に関する重みを設定すること、を特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
11. 前記照合手段は、前記被測定物体の３次元空間の位置および／または姿勢を算出する手段であることを特徴とする請求項１に記載の情報処理装置。
12. 前記撮像手段と、
    前記撮像手段により前記被測定物体を撮像して前記２次元画像を取得する取得手段と、をさらに備えることを特徴とする請求項１乃至１１のいずれか１項に記載の情報処理装置。
13. 被測定物体の形状を表す形状モデルに基づいて、前記被測定物体を撮像手段により撮像した画像に生じる該被測定物体の画像劣化を予測する予測手段と、
    前記被測定物体の表面上の３次元計測点の３次元座標を取得する取得手段と、
    前記撮像手段により撮像された２次元画像において、前記取得手段で取得された３次元座標に基づいて前記２次元画像に投影された点群の位置における画像劣化を評価する評価手段と、
    前記３次元計測点と前記形状モデルのモデル特徴との対応を探索する探索手段と、
    前記予測手段で予測した画像劣化と前記評価手段で評価した画像劣化に基づいて、前記モデル特徴と前記３次元計測点の対応の正確度を算出する算出手段と、
    前記モデル特徴と前記３次元計測点の対応の正確度に基づいて、前記形状モデルと前記被測定物体の形状を示す３次元計測点とを照合する照合手段と、を備えることを特徴とする情報処理装置。
14. ３次元計測点の３次元座標を計測する計測装置をさらに備え、
    前記取得手段は、前記計測装置から次元座標を取得することを特徴とする請求項１３に記載の情報処理装置。
15. 前記照合手段により推定された前記被測定物体の位置姿勢の推定結果に基づいて、ロボットアームを制御する制御手段と、をさらに備えることを特徴とする請求項１乃至１４のいずれか１項に記載の情報処理装置。
16. 被測定物体の形状を表す形状モデルに基づいて、前記被測定物体を撮像手段により撮像した画像に生じる該被測定物体の画像劣化を予測する予測工程と、
    前記被測定物体を前記撮像手段により撮像することにより得られた２次元画像について、前記形状モデルのモデル特徴に対応する計測データ特徴を探索する探索工程と、
    前記探索工程で探索された計測データ特徴について前記２次元画像を用いて画像劣化を評価する評価工程と、
    前記予測工程で予測した画像劣化と前記評価工程で評価した画像劣化に基づいて、前記モデル特徴と前記計測データ特徴の対応の正確度を算出する算出工程と、
    前記モデル特徴と前記計測データ特徴の対応の正確度に基づいて、前記形状モデルと前記２次元画像中の被測定物体とを照合する照合工程と、を有することを特徴とする情報処理装置の制御方法。
17. 被測定物体の形状を表す形状モデルに基づいて、前記被測定物体を撮像手段により撮像した画像に生じる該被測定物体の画像劣化を予測する予測工程と、
    前記被測定物体の表面上の３次元計測点の３次元座標を取得する取得工程と、
    前記撮像手段により撮像された２次元画像を用いて、前記取得工程で取得された３次元座標に基づいて前記２次元画像に投影された点群の位置における画像劣化を評価する評価工程と、
    前記３次元計測点と前記形状モデルのモデル特徴との対応を探索する探索工程と、
    前記予測工程で予測した画像劣化と前記評価工程で評価した画像劣化に基づいて、前記モデル特徴と前記３次元計測点の対応の正確度を算出する算出工程と、
    前記モデル特徴と前記３次元計測点の対応の正確度に基づいて、前記形状モデルと前記被測定物体の形状を示す３次元計測点とを照合する照合工程と、を有することを特徴とする情報処理装置の制御方法。
18. 請求項１６または１７に記載の情報処理装置の制御方法の各工程をコンピュータに実行させるためのプログラム。

Images