(57)【要約】【課題】物体の形状に高周波の凹凸が含まれる場合でも、安定的に物体の位置及び姿勢を測定する。【解決手段】距離画像入力部１２０は、位置及び姿勢のうちの少なくとも何れか一つの計測対象となる物体が撮像された画像データを入力する。３次元モデル投影部１４０は、入力された画像データに対して物体を表したモデルを投影する。スムージング部１５０は、画像データと、画像データに対して投影されたモデルとを平滑化する。対応付け部１６０は、平滑化された画像データ上の座標と画像データに対して投影されたモデル上の座標とを対応付ける。位置姿勢算出部１７０は、対応付け部１６０において対応付けられた結果に基づいて、物体の位置及び姿勢のうちの少なくとも何れか一方を計測する。【選択図】図１
2013156038.tif 000002
【特許請求の範囲】
【請求項１】
位置及び姿勢のうちの少なくとも何れか一つの計測対象となる物体が撮像された画像データを入力する入力手段と、
前記入力手段により入力された前記画像データに対して、前記物体を表したモデルを投影する投影手段と、
前記画像データと、前記投影手段により前記画像データに対して投影されたモデルとを平滑化する平滑化手段と、
前記平滑化手段により平滑化された前記画像データ上の座標と前記画像データに対して投影されたモデル上の座標とを対応付ける対応付け手段と、
前記対応付け手段により対応付けられた結果に基づいて、前記物体の位置及び姿勢のうちの少なくとも何れか一方を計測する計測手段とを有することを特徴とする計測装置。
【請求項２】
前記画像データは、前記画像データを撮像した撮像装置から前記物体までの距離を画素毎に含む画像データであり、前記投影手段は、前記画像データに対して前記モデルを投影することにより、前記撮像装置から前記モデルまでの距離を画素毎に含む画像データを生成することを特徴とする請求項１に記載の計測装置。
【請求項３】
前記平滑化手段は、前記画像データと、前記画像データに対して投影されたモデルとに対して、着目画素の近傍の範囲において平滑化を行うことを特徴とする請求項１又は２に記載の計測装置。
【請求項４】
前記平滑化手段は、当該計測装置による前記物体の位置及び姿勢のうちの少なくとも何れか一方の計測処理の経過に応じて、平滑化の度合いを変更することを特徴とする請求項１乃至３の何れか１項に記載の計測装置。
【請求項５】
計測装置によって実行される計測方法であって、
位置及び姿勢のうちの少なくとも何れか一つの計測対象となる物体が撮像された画像データを入力する入力ステップと、
前記入力ステップにより入力された前記画像データに対して、前記物体を表したモデルを投影する投影ステップと、
前記画像データと、前記投影ステップにより前記画像データに対して投影されたモデルとを平滑化する平滑化ステップと、
前記平滑化ステップにより平滑化された前記画像データ上の座標と前記画像データに対して投影されたモデル上の座標とを対応付ける対応付けステップと、
前記対応付けステップにより対応付けられた結果に基づいて、前記物体の位置及び姿勢のうちの少なくとも何れか一方を計測する計測ステップとを有することを特徴とする計測方法。
【請求項６】
位置及び姿勢のうちの少なくとも何れか一つの計測対象となる物体が撮像された画像データを入力する入力ステップと、
前記入力ステップにより入力された前記画像データに対して、前記物体を表したモデルを投影する投影ステップと、
前記画像データと、前記投影ステップにより前記画像データに対して投影されたモデルとを平滑化する平滑化ステップと、
前記平滑化ステップにより平滑化された前記画像データ上の座標と前記画像データに対して投影されたモデル上の座標とを対応付ける対応付けステップと、
前記対応付けステップにより対応付けられた結果に基づいて、前記物体の位置及び姿勢のうちの少なくとも何れか一方を計測する計測ステップとをコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の位置及び姿勢のうちの少なくとも何れか一方を計測する技術に関するものである。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンド等のエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。位置及び姿勢を計測する方法としては、カメラによって撮影される２次元画像データ上から検出される特徴や、距離センサから得られる距離画像データに対して物体の３次元形状モデルを当てはめるモデルフィッティングによる計測が一般的である。非特許文献１には、距離画像データを用いて位置及び姿勢を計測する方法が開示されている。非特許文献１に開示される方法では、予め物体を３次元形状モデルによって表現し、距離画像データから変換された３次元点群データに３次元形状モデルを当てはめることにより物体の位置及び姿勢を計測する。具体的には、非特許文献２に開示されるIterative Closest Point（ICP）アルゴリズムを用い、位置及び姿勢の概略値に基づいた最近傍ポリゴンの探索と、点とポリゴンとの距離の総和が最小となるような位置及び姿勢の更新とが繰り返し行われる。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】Ｄ．Ａ．Ｓｉｍｏｎ，Ｍ．Ｈｅｂｅｒｔ，ａｎｄ Ｔ．Ｋａｎａｄｅ，“Ｒｅａｌ−ｔｉｍｅ ３−Ｄ ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｕｓｉｎｇ ａ ｈｉｇｈ−ｓｐｅｅｄ ｒａｎｇｅ ｓｅｎｓｏｒ，”Ｐｒｏｃ．１９９４ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ‘９４），ｐｐ．２２３５−２２４１，１９９４．
【非特許文献２】Ｐ．Ｊ．Ｂｅｓｌ ａｎｄ Ｎ．Ｄ．ＭｃＫａｙ，“Ａ ｍｅｔｈｏｄ ｆｏｒ ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ３−Ｄ ｓｈａｐｅｓ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．１４，ｎｏ．２，ｐｐ．２３９−２５６，１９９２．
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述した従来技術は、最近傍となる３次元点と３次元形状モデル面との距離(残差)という局所的な情報を最小化することで位置及び姿勢を算出するアルゴリズムである。そのため、高周波の凹凸形状が含まれる物体等の滑らかではない形状の物体に対しては、全体としては残差が残っている場合でも、一部の凹凸部分の局所的な残差が小さくなることによって、誤った位置姿勢に計算が収束してしまう可能性が高い。
【０００５】
このような問題に対し、[Ｋｏｋ−Ｌｉｍ Ｌｏｗ ａｎｄ Ａｎｓｅｌｍｏ Ｌａｓｔｒａ，“Ｒｅｌｉａｂｌｅ ａｎｄ Ｒａｐｉｄｌｙ−Ｃｏｎｖｅｒｇｉｎｇ ＩＣＰ Ａｌｇｏｒｉｔｈｍ Ｕｓｉｎｇ Ｍｕｌｔｉｒｅｓｏｌｕｔｉｏｎ Ｓｍｏｏｔｈｉｎｇ，”Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ ３−Ｄ Ｄｉｇｉｔａｌ Ｉｍａｇｉｎｇ ａｎｄ Ｍｏｄｅｌｉｎｇ（３ＤＩＭ），２００３．]に開示されるように、物体の形状を滑らかな形状に変換して位置及び姿勢を推定する手法が知られている。この手法は、対象となる点群に対して、Ｂ−Ｓｐｌｉｎｅ曲面を当てはめ、点群をスムージングして、凹凸形状を滑らかな面に変換することにより、高周波の凹凸形状に起因する位置及び姿勢の推定に関する問題を解決している。
【０００６】
しかし、この手法は、同じようなデータ量及び形状であるような３次元点群同士を用いた位置及び姿勢の推定を前提とした手法であるため、距離画像データと３次元形状モデルという異種のデータを用いた位置及び姿勢の推定に対して適用することはできない。一般的に、３次元形状モデルは物体の全周の情報を備えているのに対し、距離画像データは１視点から観察できる物体表面の３次元情報しか格納していない。そのため、３次元形状モデルに対して当てはめたＢ−Ｓｐｌｉｎｅの曲面情報と、距離画像データに対して当てはめたＢ−Ｓｐｌｉｎｅの情報とは大きく異なる。異なるパラメータのＢ−Ｓｐｌｉｎｅ曲面に基づいてスムージングした場合、その結果となる形状は、両者で大きく異なってしまい、スムージングした結果同士を用いてモデルフィッティングを行うことが有効でなくなる。従って、距離画像データと３次元形状モデルという異種のデータを用いたモデルフィッティングにおける高周波の凹凸形状の問題に対し、従来の手法を適用することはできない。
【０００７】
そこで、本発明の目的は、物体の形状に高周波の凹凸が含まれる場合でも、安定的に物体の位置及び姿勢を測定することにある。
【課題を解決するための手段】
【０００８】
本発明の計測装置は、位置及び姿勢のうちの少なくとも何れか一つの計測対象となる物体が撮像された画像データを入力する入力手段と、前記入力手段により入力された前記画像データに対して、前記物体を表したモデルを投影する投影手段と、前記画像データと、前記投影手段により前記画像データに対して投影されたモデルとを平滑化する平滑化手段と、前記平滑化手段により平滑化された前記画像データ上の座標と前記画像データに対して投影されたモデル上の座標とを対応付ける対応付け手段と、前記対応付け手段により対応付けられた結果に基づいて、前記物体の位置及び姿勢のうちの少なくとも何れか一方を計測する計測手段とを有することを特徴とする。
【発明の効果】
【０００９】
本発明によれば、物体の形状に高周波の凹凸が含まれる場合でも、安定的に物体の位置及び姿勢を測定することが可能となる。
【図面の簡単な説明】
【００１０】
【図１】本発明の第１の実施形態に係る情報処理装置の構成を示す図である。
【図２】本発明の第１の実施形態に係る情報処理装置の処理を示すフローチャートである。
【図３】本発明の第２の実施形態に係る情報処理装置の構成を示す図である。
【図４】本発明の第２の実施形態に係る情報処理装置の処理を示すフローチャートである。
【発明を実施するための形態】
【００１１】
以下、本発明を適用した好適な実施形態を、添付図面を参照しながら詳細に説明する。
【００１２】
先ず、本発明の第１の実施形態について説明する。第１の実施形態では、物体の位置及び姿勢の概略は既知であると仮定し、３次元形状モデルと距離画像データとを用いて物体の位置及び姿勢を推定するものである。即ち、第１の実施形態では、対象となる３次元形状モデルが距離画像データの画像平面上に投影されることにより、視点から３次元形状モデルまでの距離が格納された距離画像データと同じデータ形式に３次元形状モデルが変換される。そして、両者のデータがスムージングされた後、計測対象の物体（以下、単に「物体」と称す）の位置及び姿勢が推定される。これにより、物体の位置及び姿勢を安定的に計測することが可能となる。
【００１３】
図１は、本発明の第１の実施形態に係る情報処理装置１の構成を示す図である。図１に示すように、情報処理装置１は、３次元モデル保存部１１０、距離画像入力部１２０、概略位置姿勢入力部１３０、３次元モデル投影部１４０、スムージング部１５０、対応付け部１６０及び位置姿勢算出部１７０を備える。３次元モデルデータ保存部１１０は３次元形状モデル１０を保存する。３次元データ計測装置２０は距離画像入力部１２０に接続される。情報処理装置１は、３次元モデル保存部１１０に保存される物体の形状を表す３次元形状モデル１０に基づいて、距離画像データ中に撮像される物体の位置及び姿勢を計測する。なお、本実施形態において、３次元モデル保存部１１０に保存される３次元形状モデル１０は、例えば工業部品の設計３次元ＣＡＤデータのように、現実に撮像される物体の形状に即していることが好ましい。情報処理装置１は、計測装置の適用例となる構成である。
【００１４】
３次元データ計測装置２０は、物体を含むシーン中の複数の点と当該３次元データ計測装置２０との距離を計測し、距離画像データを出力する。ここでいう距離画像データとは、画素毎に３次元データ計測装置２０からの距離が格納されている画像データである。本実施形態の３次元データ計測装置２０では、対象に照射したレーザ光の反射光を撮影し、三角測量により距離を計測するアクティブ式の距離センサを使用している。但し、距離センサはこれに限るものではない。即ち、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式の距離センサを使用してもよいし、ステレオカメラによって撮影される画像データから三角測量によって３次元データ計測装置２０からの距離を計算するパッシブ式の距離センサを使用してもよい。その他、距離画像データを出力することができれば、いかなる距離センサを用いてもよい。
【００１５】
３次元データ計測装置２０から出力される距離画像データは、距離画像入力部１２０を介して情報処理装置１に入力される。３次元データ計測装置２０の焦点距離、主点位置及びレンズ歪みパラメータ等の内部パラメータは、当該３次元データ計測装置２０の仕様を参照することによって得られる。また、[Ｒ．Ｙ．Ｔｓａｉ，"Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ，"ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｖｏｌ．ＲＡ−３，ｎｏ．４，１９８７．]に開示される方法によって事前に３次元データ計測装置２０をキャリブレーションして内部パラメータを得るようにしてもよい。
【００１６】
距離画像入力部１２０は、３次元データ計測装置２０から距離画像データを入力する。３次元形状モデル保存部１１０は、物体の３次元形状モデル１０を保存する。本実施形態では、３次元面から構成される３次元形状モデル１０によって物体を表現している。即ち、３次元形状モデル１０は、点の集合と、各点を結んで構成される面の集合とによって定義される。
【００１７】
概略位置姿勢入力部１３０は、情報処理装置１に対する物体の位置及び姿勢の概略値を入力する。ここで情報処理装置１に対する物体の位置及び姿勢とは、距離画像データを撮像する３次元データ計測装置２０のカメラ座標系における物体の位置及び姿勢を意味する。但し、カメラ座標系に対する相対的な位置及び姿勢が既知で且つ変化しなければ、情報処理装置１の何れの部分を基準としてもよい。本実施形態では、情報処理装置１は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を位置及び姿勢の概略値として用いるものとする。但し、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測を基に物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢について推定された速度及び角速度から、現在の位置及び姿勢を予測したものでもよい。また、様々な姿勢で撮像された物体の距離画像データをテンプレートとして保持しておき、入力される距離画像データとテンプレートとのマッチング（テンプレートマッチング）により、物体の位置及び姿勢の概略値を推定してもよい。その他、他のセンサによって物体の位置及び姿勢の計測が可能である場合、当該センサの出力値を位置及び姿勢の概略値として用いてもよい。物体の位置及び姿勢の計測が可能なセンサとして、例えば、トランスミッタが発する磁界を物体に装着されるレシーバで検出することにより位置及び姿勢を計測する磁気式センサが挙げられる。また、物体上に配置されたマーカを固定されたカメラで撮影することにより位置及び姿勢を計測する光学式センサを使用してもよい。また、６自由度の位置及び姿勢の計測が可能なセンサであれば、いかなるセンサを用いてもよい。さらに、物体の置かれているおおよその位置や姿勢が予めわかっている場合には、その値を概略値として用いてもよい。
【００１８】
モデル投影部１４０は、３次元形状モデル保存部１１０に保存される３次元形状モデルを、概略位置姿勢入力部１３０により入力された位置及び姿勢の概略値を用いて、距離画像データの画像平面上に投影する。この投影処理の結果として、視点から３次元形状モデルの表面までの距離が格納された距離画像データ（以下、モデル距離画像データと称す）が生成される。
【００１９】
スムージング部１５０は、距離画像入力部１３０から入力された距離画像データと、モデル投影部１４０により生成されたモデル距離画像データとに対して、スムージング処理を施す。対応付け部１６０は、スムージング部１５０においてスムージング処理が施された距離画像データとモデル距離画像データとの対応付けを行う。位置姿勢算出部１７０は、対応付け部１６０において対応付けられた結果に基づいて、物体の位置及び姿勢を計測する。
【００２０】
図２は、本発明の第１の実施形態に係る情報処理装置１の処理を示すフローチャートである。以下、図２を参照しながら、第１の実施形態に係る情報処理装置１の処理について説明する。
【００２１】
ステップＳ１０１０において、情報処理装置１は初期化処理を行う。即ち、概略位置姿勢入力部１２０は、物体の位置及び姿勢の概略値を初期値として入力する。本実施形態では、この初期値として入力された位置及び姿勢の概略値を、撮像された距離画像データに基づいて逐次更新していくことにより、物体の位置及び姿勢を計測する。そのため、物体の位置及び姿勢の計測を開始する前に、予め物体の位置及び姿勢の概略値を初期値として与えておく必要がある。本実施形態においては、上述したように、前回（前時刻）の計測値を位置及び姿勢の概略値として用いるものとする。
【００２２】
ステップＳ１０２０において、距離画像入力部１３０は、モデルフィッティングにより物体の位置及び姿勢を算出するための距離画像データを３次元データ計測装置２０から入力する。以下では、距離画像入力部１３０において入力された距離画像データを計測距離画像データと称す。なお、計測距離画像データは、画素毎に３次元データ計測装置２０からの物体を含む対象までの距離を格納している。
【００２３】
ステップＳ１０３０において、３次元モデル投影部１４０は、３次元形状モデル１０を計測距離画像データと同等のデータ量及び性質になるように変換を行う。具体的には、３次元モデル投影部１４０は、ステップＳ１０２０において入力された計測距離画像データの画像平面上に３次元形状モデル１０を投影する。これにより、計測距離画像データと同じように視点から３次元形状モデル１０までの距離が格納されたモデル距離画像データが生成される。本実施形態では、ステップＳ１０１０において入力された物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値と、キャリブレーション済みの３次元データ計測装置２０の内部パラメータとを用いて、３次元形状モデル１０をレンダリングする。レンダリングにより、視点から３次元形状モデル１０の表面までの距離がデプスバッファに格納される。３次元モデル投影部１４０は、デプスバッファに格納された距離をモデル距離画像データにおいて格納される距離として取り扱う。以上の処理により、３次元形状モデル１０が計測距離画像データと同等のデータ形式に変換される。
【００２４】
ステップＳ１０４０において、スムージング部１５０は、ステップＳ１０３０で生成されたモデル距離画像データとステップＳ１０２０で入力された計測距離画像データとに対し、画像平面上での近傍の画素に基づいたスムージング処理を施す。本実施形態では、スムージング処理として、着目画素の近傍のＮ×Ｎピクセルの範囲において、標準偏差σのガウシアンフィルタによる平滑化が行われる。ガウシアンカーネルの標準偏差σは、モデル距離画像データ及び計測距離画像データをどの程度平滑化するかを決定するパラメータであり、σが大きくなればなるほど物体の形状は滑らかになる。その結果、位置及び姿勢の推定処理は安定するが、形状を平滑化した分だけ推定される位置及び姿勢が曖昧になる。Ｎは近傍を規定する範囲を示しており、その値は任意に決めてもよいし、ガウシアンカーネルの標準偏差σから求めてもよい。モデル距離画像データ及び計測距離画像データの平滑化が行える限り、Ｎの設定に制限はない。また、スムージング処理は、ガウシアンフィルタの適用に限らず、平均化フィルタやメディアンフィルタを適用する等、画像データの平滑化を行う処理であれば何でもよく、特に制限はない。本実施形態では、標準偏差σを１、近傍の範囲Ｎを３に設定してガウシアンフィルタによる平滑化処理を行うものとする。以上のようなスムージング処理をモデル距離画像データ及び計測距離画像データ上の全画素に対して行うことにより、形状が滑らかに処理されたモデル距離画像データ及び計測距離画像データが得られる。以下では、形状が滑らかに処理されたモデル距離画像データを平滑化モデル距離画像データと称し、同じく形状が滑らかに処理された計測距離画像データを平滑化計測処理画像データと称す。
【００２５】
次に、スムージング部１５０は、平滑化モデル距離画像データ及び平滑化計測距離画像データから３次元点群の座標を算出する。即ち、スムージング部１５０は、平滑化計測距離画像データの各画素について、画素の位置に対応する視線ベクトルと、当該画素に格納される距離とを乗算することにより、各画素に対応する３次元点群の座標を求める。また、スムージング部１５０は、平滑化モデル距離画像データに対しても同様の処理を行い、平滑化モデル距離画像データの各画素に対応する３次元点群を求める。但し、平滑化モデル距離画像データの場合、３次元形状モデル１０に該当する領域でない部分には無効な距離が格納されているため、この部分の３次元点群は求めない。また、平滑化モデル距離画像データに関しては、各画素の３次元法線も求めておく。具体的には、着目画素と隣り合う画素の３点の３次元座標の外積を計算することにより、着目画素の３次元法線が求められる。
【００２６】
以上の処理を、平滑化モデル距離画像データと平滑化計測距離画像データとの全画素に対して行うことにより、平滑化モデル距離画像データと平滑化計測距離画像データとのそれぞれについて、滑らかな形状に変換された３次元点群が得られる。
【００２７】
ステップＳ１０５０において、対応付け部１６０は、ステップＳ１０４０で生成された平滑化計測距離画像データと平滑化モデル距離画像データとの対応付けを行う。本実施形態では、平滑化モデル距離画像データ上の画素と３次元点群との関係は既知であるとして、平滑化モデル距離画像データ上の３次元形状モデル１０に該当する領域の全画素に対して、画像平面上で同じ２次元座標となる平滑化計測距離画像データの３次元点を参照する。これにより、平滑化モデル距離画像データと平滑化計測距離画像データとの３次元点が対応付けられる。但し、平滑化モデル距離画像データ上の３次元形状モデル１０に該当する領域内の全画素に対して処理を行うのではなく、当該領域内の画像平面上で等間隔になるように、或いは、３次元空間上で等間隔になるように間引いて処理してもよい。なお、対応付け処理を行う３次元点は、データ量と計算量とのトレードオフとなる。位置及び姿勢の算出処理に十分なだけの対応付けが得られる限り、処理する３次元点の選択は何れの場合でよく、特に制限はない。以上の処理により、平滑化計測距離画像データと平滑化モデル距離画像データ間の３次元点の対応付けが行われる。
【００２８】
ステップＳ１０６０において、位置姿勢算出部１７０は、平滑化計測距離画像データと平滑化モデル距離画像データ間の３次元点の対応付けに基づいて、非線形最適化計算を用いて物体の位置及び姿勢を算出する。本実施形態では、非線形最適化手法としてGauss-Newton法を用いるものとする。なお、非線形最適化手法はGauss-Newton法に限るものではない。例えば、より計算がロバストであるLevenberg-Marquardt法を用いてもよいし、よりシンプルな方法である最急降下法を用いてもよい。また、共役勾配法やＩＣＣＧ法等、他の非線形最適化計算手法を用いてもよい。
【００２９】
また、物体の位置及び姿勢の算出は、平滑化モデル距離画像データ中の各３次元点が、対応する平滑化計測距離画像データ中の３次元点に当てはまるように物体の位置及び姿勢の概略値を反復して更新することにより行われる。より具体的には、位置姿勢算出部１７０は、平滑化計測距離画像データ中の３次元点と、平滑化モデル距離画像データ中の３次元点及び３次元法線とから、３次元空間中での点と平面との符号付距離を物体の位置及び姿勢の関数として線形近似する。
【００３０】
次に、位置姿勢算出部１７０は、符号付距離を０としたときに位置及び姿勢について成り立つ線形方程式を組立て、この線形方程式を連立方程式として解く。これにより、位置姿勢算出部１７０は、物体の位置及び姿勢の微小変化を求め、位置及び姿勢の概略値を更新する。以上の処理を繰り返すことにより、最終的な位置及び姿勢が算出される。なお、平滑化モデル距離画像データと平滑化計測距離画像データとの誤差の最小化処理に関しては、本発明の本質に関わる話ではないため、記述を省略する。最小化処理の詳細は、非特許文献［Ｙ．Ｃｈｅｎ ａｎｄ Ｇ．Ｍｅｄｉｏｎｉ，“Ｏｂｊｅｃｔ ｍｏｄｅｌｉｎｇ ｂｙ ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ｍｕｌｔｉｐｌｅ ｒａｎｇｅ ｉｍａｇｅｓ，”Ｉｍａｇｅ ａｎｄ Ｖｉｓｉｏｎ Ｃｏｍｐｕｔｉｎｇ，１０（３），ｐｐ．１４５−１５５，１９９２．］を参照されたい。
【００３１】
ステップＳ１０７０において、位置姿勢算出部１７０は、位置及び姿勢の算出処理の終了が指示されたか否かを判定する。位置及び姿勢の算出処理の終了が指示された場合、処理は終了する。一方、位置及び姿勢の算出処理の終了が指示されていない場合、処理はステップＳ１０１０に戻り、情報処理装置１は、新たな計測距離画像データを取得して、物体の位置及び姿勢を再度計測する。
【００３２】
以上のように、本実施形態では、３次元形状モデルを計測距離画像データと同じ形式に変換し、モデル距離画像データと計測距離画像データとの双方に対してスムージング処理を実行するようにしている。これにより、両者の形状を類似したまま平滑化を施すことが可能となる。本実施形態では、物体の形状が滑らかになるように変換することにより、物体の形状の凹凸が原因で発生する位置及び姿勢の測定処理の不安定性が減少し、物体の形状に凹凸がある物体に対して安定した位置及び姿勢の推定が可能となる。
【００３３】
第１の実施形態では、ステップＳ１０４０における平滑化処理のパラメータを固定させていた。但し、平滑化処理のパラメータはこれに限るものではない。例えば、位置及び姿勢の測定処理の経過、即ちループ回数（ステップＳ１０７０で分岐した回数）に応じて、平滑化処理の度合い、即ちパラメータを変更させてもよい。具体的には、ループ回数が少ないうちは、大きく平滑化するように平滑化処理のパラメータを設定し、ループ回数が多くなるにつれ、平滑化を小さくするように平滑化処理のパラメータを設定してもよい。これにより、繰り返し計算の最初は物体の形状を大きく平滑化することにより、安定した位置及び姿勢の測定処理を行い、繰り返し計算の後半は、物体の形状の平滑化を少なくすることにより、位置及び姿勢の測定処理の精度を高めることが可能になる。平滑化処理のパラメータを設定する方法は、以上に挙げた方法に限るものでなく、正常な平滑化処理が行えるパラメータを設定する限り、何れの方法を用いてもよい。
【００３４】
第１の実施形態では、３次元形状モデル１０は、面の集合によって構成されるモデルを前提としていた。しかし、物体を表現するモデル形式はこれに限るものではない。例えば、３次元点の集合により構成されるモデルを用いてもよい。この場合、ステップＳ１０３０における３次元形状モデル１０の変換処理では、モデルのレンダリング処理を行わずに、モデルを構成する各３次元点を、計測距離画像データの画像平面上に投影することで、モデル距離画像データを生成する。具体的には、ステップＳ１０１０において入力された物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値と、キャリブレーション済みの３次元データ計測装置２０の内部パラメータとを用いて、モデルを構成する３次元点を画像平面に投影する。そして、３次元点に対応する画像平面上の画素に、視点からモデルを構成する点までの距離を格納することでモデル距離画像データを生成する。なお、３次元形状モデル１０を構成する形式は、以上に挙げた方法に限るものでなく、３次元形状モデル１０から計測距離画像データと同等のデータ形式に変換することができる形式である限り、何れの形式を用いてもよい。
【００３５】
次に、本発明の第２の実施形態について説明する。第１の実施形態では、３次元形状モデルを距離画像データ形式に変換し、モデル距離画像データ全体に対してスムージング処理を実行することにより、物体の位置及び姿勢を計測する手法について説明した。これに対し、第２の実施形態では、３次元形状モデルを距離画像データ形式には変換せず、３次元形状モデル上で定義されるモデル表面上の３次元点毎に平滑化することにより、物体の位置及び姿勢を計測する手法について説明する。
【００３６】
図３は、本発明の第２の実施形態に係る情報処理装置２の構成を示す図である。図３に示すように、情報処理装置２は、３次元モデル保存部２１０、距離画像入力部２２０、概略位置姿勢入力部２３０、３次元モデル投影部２４０、スムージング部２５０、対応付け部２６０及び位置姿勢算出部２７０を備える。３次元モデルデータ保存部２１０は３次元形状モデル１０を保存する。３次元データ計測装置２０は距離画像入力部２２０に接続される。情報処理装置２は、３次元モデル保存部２１０に保存されている物体の形状を表す３次元形状モデル１０に基づいて、距離画像データ中に撮像される物体の位置及び姿勢を計測する。なお、第２の実施形態における３次元形状モデル１０も、第１の実施形態における３次元形状モデル１０と同様に、現実に撮像される物体の形状に即していることが好ましい。
【００３７】
３次元形状モデル保存部２１０は、位置及び姿勢の計測対象となる物体の３次元形状モデル１０を保存する。本実施形態では、法線情報を持った３次元点から構成される３次元形状モデル１０によって物体を表現している。即ち、３次元形状モデル１０は、３次元点の集合と、３次元点それぞれに対するその点の法線方向の集合とによって定義される。
【００３８】
３次元モデル投影部２４０は、３次元形状モデル保存部２１０に保存された３次元形状モデル１０を、概略位置姿勢入力部１３０により入力された位置及び姿勢の概略値を用いて、距離画像データの画像平面上に投影する。この投影処理の結果として、３次元形状モデル中の３次元点群の、画像平面上での２次元座標が算出される。
【００３９】
対応付け部２５０は、３次元形状モデル１０中の３次元点群と距離画像データとの対応付けを行う。スムージング部２６０は、３次元形状モデル１０中の３次元点群と距離画像データ中の３次元点群とに対してスムージング処理を行う。
【００４０】
なお、距離画像入力部２２０、概略位置姿勢入力部２３０及び位置姿勢算出部２７０は、第１の実施形態における距離画像入力部１２０、概略位置姿勢入力部１３０及び位置姿勢算出部１７０と同一であるため、これらについては説明を省略する。
【００４１】
図４は、第２の実施形態に係る情報処理装置２の処理を示すフローチャートである。以下、図４を参照しながら、第２の実施形態に係る情報処理装置２の処理について説明する。
【００４２】
ステップＳ２０１０において、情報処理装置２は初期化処理を行う。なお、ステップＳ２０１０は、第１の実施形態におけるステップＳ１０１０と同様の処理であるため、詳細な説明は省略する。
【００４３】
ステップＳ２０２０において、距離画像入力部２２０は、モデルフィッティングにより物体の位置及び姿勢を算出するための距離画像データを３次元データ計測装置２０から入力する。なお、ステップＳ２０２０は、第１の実施形態におけるステップＳ１０２０と同様の処理であるため、詳細な説明は省略する。
【００４４】
ステップＳ２０３０において、対応付け部２５０は、３次元形状モデル１０中の３次元点群と距離画像データとを対応付ける。具体的には、対応付け部２５０は、３次元形状モデル中の３次元点群を、ステップＳ２０１０において入力された物体の位置及び姿勢の概略値と、キャリブレーション済みの３次元データ計測装置２０の内部パラメータとを用いて、距離画像データの画像平面に投影する。これにより、３次元形状モデル１０の３次元点群と距離画像データ中の３次元点群とが対応付けられる。なお、３次元点群の座標の算出方法は、ステップＳ１０４０で説明した処理と同様である。３次元形状モデル１０中の３次元点群全点について対応付けが終わると、処理はステップＳ２０４０に移行する。
【００４５】
ステップＳ２０４０において、スムージング部２６０は、ステップＳ２０３０にて対応付けられた３次元形状モデル１０中の３次元点群と距離画像データ中の３次元点群とに対し、画像平面上の近傍の画素に基づいたスムージング処理を行う。即ち、スムージング部２６０は、３次元形状モデル１０の３次元点それぞれに対し、画像平面上でＮ×Ｎピクセル以内の近傍となる他の３次元点の座標を算出し、近傍の３次元点の座標の平均を新たな３次元点の座標の値として採用することでスムージング処理を行う。この処理は、３次元形状モデル１０の３次元点群と距離画像データ中の３次元点群とに対して同じように行われる。なお、画像平面上での近傍を定義するＮの値の設定に特に制限はない。また、本実施形態では、３次元点の座標のスムージング処理として、近傍の３次元点の座標の平均を採用したが、これに限るものではない。例えば、３次元空間中の距離を係数として、近いものほど重みを大きくさせてもよい。以上の処理を３次元形状モデル１０中の３次元点群と距離画像データ中の３次元点群とに対して施すことにより、３次元形状モデル１０と距離画像データとのそれぞれについて、滑らかな形状に変換された３次元点群が得られる。
【００４６】
ステップＳ２０５０において、位置姿勢算出部２７０は、ステップＳ２０４０でスムージング処理が施された３次元形状モデル中の３次元点群と距離画像データの３次元点群とに基づいて、非線形最適化計算を用いて物体の位置及び姿勢を算出する。最適化計算の処理は、第１の実施形態におけるステップＳ１０６０と同様の処理であるため、詳細な説明は省略する。
【００４７】
ステップＳ２０６０において、位置姿勢算出部２７０は、位置及び姿勢の算出処理を終了する指示が入力されたか否かを判定する。位置及び姿勢の算出処理を終了する指示が入力された場合、処理は終了する。一方、位置及び姿勢の算出処理を終了する指示が入力されていない場合、処理はステップＳ２０１０に戻り、情報処理装置２は、新たな距離画像データを取得して、物体の位置及び姿勢を再度計測する。
【００４８】
以上のように、本実施形態では、３次元形状モデル中の３次元点群と距離画像データ中の３次元点群とに対し、画像平面上の近傍に基づくスムージング処理を行い、滑らかな形状に変換している。これにより、物体の形状の凹凸が原因で発生する位置及び姿勢の計測処理の不安定性が減少し、物体の形状に凹凸がある物体に対して安定した位置及び姿勢の推定が可能となる。
【００４９】
第１及び第２の実施形態では、位置姿勢算出部１７０及び位置姿勢算出部２７０は、物体の位置及び姿勢の両方を算出することを前提としていた。しかし、本発明は位置及び姿勢の両パラメータを推定する方式に限るものでない。例えば、位置及び姿勢のいずれか一方だけを算出しても特に問題ない。この場合、ステップＳ１０６０及びＳ２０５０において、推定しないパラメータ（位置パラメータを推定する場合は姿勢パラメータ、姿勢パラメータを推定する場合は位置パラメータ）に関しては、ステップＳ１０１０又はＳ２０１０において入力された概略の値を設定し、固定値として扱って繰り返し処理を行えばよい。
【００５０】
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【符号の説明】
【００５１】
１、２：情報処理装置、１０：３次元形状モデル、２０：３次元データ計測装置、１１０、２１０：３次元モデル保存部、１２０、２２０：距離画像入力部、１３０、２３０：概略位置姿勢入力部、１４０、２４０：３次元モデル投影部、１５０、２５０：スムージング部、１６０、２６０：対応付け部、１７０、２７０：位置姿勢算出部
【図１】
2013156038.tif 000003
【図２】
2013156038.tif 000004
【図３】
2013156038.tif 000005
【図４】
2013156038.tif 000006