(57)【特許請求の範囲】
【請求項１】
複数の剛体と該剛体間を接続する関節によって表される計測対象物体を、前記剛体の数に応じた複数の近似レベルで表した複数の３次元形状モデルを保持する保持手段と、
前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を入力する概略値入力手段と、
前記計測対象物体が撮像された距離画像データを入力する距離画像データ入力手段と、
前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとに基づき、前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとの差が小さくなるようcoarse-to-fineアプローチにより前記概略値を更新していくことにより、前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する計測手段とを有することを特徴とする計測装置。
【請求項２】
複数の剛体と該剛体間を接続する関節によって表される計測対象物体を、前記剛体の数に応じた複数の近似レベルで表した複数の３次元形状モデルを保持する保持手段と、
前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を入力する概略値入力手段と、
前記計測対象物体が撮像された距離画像データを入力する距離画像データ入力手段と、
前記距離画像データ入力手段により入力された距離画像データに対して前記複数の近似レベルで表した複数の３次元形状モデルを投影して、複数のモデル距離画像データを生成する投影手段と、
前記複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記複数のモデル距離画像データと、当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとに基づき、前記複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記複数のモデル距離画像データと、当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとの差が小さくなるようcoarse-to-fineアプローチにより前記概略値を更新していくことにより、前記物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する計測手段とを有することを特徴とする計測装置。
【請求項３】
前記投影手段は、前記概略値入力手段により入力された前記概略値に基づいて、前記距離画像データ入力手段により入力された距離画像データに対して前記複数の近似レベルで表した複数の３次元形状モデルを投影することを特徴とする請求項２に記載の計測装置。
【請求項４】
前記解像度の変換は、前記距離画像データにおける着目画素の近傍の範囲において平滑化を行う変換であることを特徴とする請求項１乃至３の何れか１項に記載の計測装置。
【請求項５】
前記解像度の変換は、前記距離画像データのサイズを縮小することにより平滑化を行う変換であることを特徴とする請求項１乃至３の何れか１項に記載の計測装置。
【請求項６】
前記複数の３次元形状モデルには、単一の剛体によって表されるモデルが含まれることを特徴とする請求項１乃至５の何れか１項に記載の計測装置。
【請求項７】
前記計測手段は、前記更新前後の前記概略値の類似度に基づく拘束条件を付して前記概略値の更新を行うことを特徴とする請求項１乃至６の何れか１項に記載の計測装置。
【請求項８】
複数の剛体と該剛体間を接続する関節によって表される計測対象物体を、前記剛体の数に応じた複数の近似レベルで表した複数の３次元形状モデルを保持する保持手段を有する計測装置によって実行される計測方法であって、
前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を入力する概略値入力ステップと、
前記計測対象物体が撮像された距離画像データを入力する距離画像データ入力ステップと、
前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとに基づき、前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとの差が小さくなるようcoarse-to-fineアプローチにより前記概略値を更新していくことにより、前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する計測ステップとを有することを特徴とする計測方法。
【請求項９】
複数の剛体と該剛体間を接続する関節によって表される計測対象物体を、前記剛体の数に応じた複数の近似レベルで表した複数の３次元形状モデルを保持する保持手段を有する計測装置によって実行される計測方法であって、
前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を入力する概略値入力ステップと、
前記計測対象物体が撮像された距離画像データを入力する距離画像データ入力ステップと、
前記距離画像データ入力ステップにより入力された距離画像データに対して前記複数の近似レベルで表した複数の３次元形状モデルを投影して、複数のモデル距離画像データを生成する投影ステップと、
前記複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記複数のモデル距離画像データと、当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとに基づき、前記複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記複数のモデル距離画像データと、当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとの差が小さくなるようcoarse-to-fineアプローチにより前記概略値を更新していくことにより、前記物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する計測ステップとを有することを特徴とする計測方法。
【請求項１０】
複数の剛体と該剛体間を接続する関節によって表される計測対象物体を、前記剛体の数に応じた複数の近似レベルで表した複数の３次元形状モデルを保持する保持手段を有する計測装置によって実行される計測方法をコンピュータに実行させるためのプログラムであって、
前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータのうちの少なくとも何れか一つの概略値を入力する概略値入力ステップと、
前記計測対象物体が撮像された距離画像データを入力する距離画像データ入力ステップと、
前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとに基づき、前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとの差が小さくなるようcoarse-to-fineアプローチにより前記概略値を更新していくことにより、前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する計測ステップとをコンピュータに実行させるためのプログラム。
【請求項１１】
複数の剛体と該剛体間を接続する関節によって表される計測対象物体を、前記剛体の数に応じた複数の近似レベルで表した複数の３次元形状モデルを保持する保持手段を有する計測装置によって実行される計測方法をコンピュータに実行させるためのプログラムであって、
前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータのうちの少なくとも何れか一つの概略値を入力する概略値入力ステップと、
前記計測対象物体が撮像された距離画像データを入力する距離画像データ入力ステップと、
前記距離画像データ入力ステップにより入力された距離画像データに対して前記複数の近似レベルで表した複数の３次元形状モデルを投影して、複数のモデル距離画像データを生成する投影ステップと、
前記複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記複数のモデル距離画像データと、当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとに基づき、前記複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記複数のモデル距離画像データと、当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとの差が小さくなるようcoarse-to-fineアプローチにより前記概略値を更新していくことにより、前記物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する計測ステップとをコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の位置、姿勢及び変形のうちの少なくとも何れか一つを計測する技術に関するものである。
【背景技術】
【０００２】
近年、ロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このような目的には主にアーム型のロボットが利用され、ロボットはアームの先端に装着されたハンド等のエンドエフェクタによって部品を把持して組立を行う。部品が変形する場合、その部品上の特定の場所を把持するためには、部品の位置及び姿勢に加えてその形状を知る必要がある。非特許文献１には、複数の剛体と剛体間をつなぐ関節とによって物体をモデル化し、濃淡画像を用いたモデルフィッティングにより物体の位置、姿勢及び変形を推定する方法が開示されている。この方法では、剛体を３次元の線分の集合としてモデル化する。そして、予め与えられる位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を初期値として、各３次元の線分の投影像が撮像された画像データ上のエッジに当てはまるようにモデルフィッティングが行われる。
【０００３】
上述した複数の剛体と関節とからなるモデルを用いて、部品の位置及び姿勢を計測する場合、高精度な計測を行うためには部品の形状の近似度が上がるように剛体の数を増やす必要がある。剛体の数を増やすと関節数が増えるため、モデルフィッティング時に最適化するパラメータの数が増大する。これにより、最適化計算に時間がかかる、オーバーフィッティングが起こるため、局所解に陥りやすいという問題が生じる。非特許文献２には、関節モデルを用いて人間の運動を推定する際に、全ての関節角を同時に求めるのではなく、関節構造を階層的に表現して上位階層から逐次的に関節角を求めることで局所解を回避する方法が開示されている。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】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.
【非特許文献２】小川原，李，池内，“関節構造を持つ柔軟変形モデルを用いた人体運動の推定”，第9回画像の認識・理解シンポジウム（MIRU2006）予稿集，pp.994-999，2006.
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、非特許文献２に開示される技術では、大局的な運動を表現する上位階層の関節を推定する際にも、下位階層の関節と同じ解像度のデータを用いているため、計算に時間がかかるという問題がある。
【０００６】
そこで、本発明の目的は、物体の位置、姿勢及び変形のうちの少なくとも何れか一つを、局所解に陥らず高速に計測することにある。
【課題を解決するための手段】
【０００７】
本発明の計測装置は、複数の剛体と該剛体間を接続する関節によって表される計測対象物体を、前記剛体の数に応じた複数の近似レベルで表した複数の３次元形状モデルを保持する保持手段と、前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を入力する概略値入力手段と、前記計測対象物体が撮像された距離画像データを入力する距離画像データ入力手段と、前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとに基づき、前記複数の３次元形状モデルと当該複数の３次元形状モデルの近似レベルに対応する解像度に変換された前記距離画像データとの差が小さくなるようcoarse-to-fineアプローチにより前記概略値を更新していくことにより、前記計測対象物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する計測手段とを有することを特徴とする。
【発明の効果】
【０００８】
本発明によれば、物体の位置、姿勢及び変形のうちの少なくとも何れか一つを、局所解に陥らず高速に計測することが可能となる。
【図面の簡単な説明】
【０００９】
【図１】本発明の第１の実施形態に係る位置姿勢形状計測装置の構成を示す図である。
【図２】物体の３次元形状モデルを説明するための図である。
【図３】本発明の第１の実施形態に係る位置姿勢形状計測装置の処理を示すフローチャートである。
【図４】本発明の第２の実施形態に係る位置姿勢形状計測装置の構成を示す図である。
【図５】本発明の第２の実施形態に係る位置姿勢形状計測装置の処理を示すフローチャートである。
【発明を実施するための形態】
【００１０】
以下、本発明を適用した好適な実施形態を、添付図面を参照しながら詳細に説明する。
【００１１】
先ず、本発明の第１の実施形態について説明する。第１の実施形態では、局所的な剛体が１自由度を持つ関節によって接続されている物体の位置パラメータ、姿勢パラメータ及び変形パラメータを推定する方法について説明する。本実施形態では、このような位置パラメータ、姿勢パラメータ及び変形パラメータの計測対象となる物体として、長方形の集合によって表される長細い平面状の物体を想定する。なお、以下の説明においては、位置パラメータ、姿勢パラメータ及び変形パラメータの計測対象となる物体のことを、単に「物体」と称す。
【００１２】
図１は、第１の実施形態に係る位置姿勢形状計測装置１の構成を示す図である。図１に示すように、位置姿勢形状計測装置１は、３次元形状モデル保存部１１０、概略位置姿勢形状入力部１２０、距離画像入力部１３０、スムージング部１５０及び位置姿勢形状算出部１６０を備える。また、位置姿勢形状計測装置１には、距離画像撮像装置１００が接続されている。
【００１３】
距離画像撮像装置１００は、物体を含むシーン中の複数の点と当該距離画像撮像装置１００との距離を計測し、距離画像データを出力する。ここでいう距離画像データとは、画素毎に距離画像撮像装置１００からの距離値が格納されている画像データである。本実施形態の距離画像撮像装置１００では、液晶プロジェクタから物体に照射された２次元のパターン光を撮影し、三角測量により画素毎に距離画像撮像装置１００からの距離を計測するアクティブ方式の距離センサを使用する。距離画像撮像装置１００において使用可能な距離センサはこれに限るものではない。即ち、光切断法やTime-of-flight方式等の他のアクティブ式のものや、ステレオカメラによるパッシブ式のもの等、距離画像データを出力することができれば、いかなる距離センサを用いてもよい。
【００１４】
また、距離画像撮像装置１００の内部パラメータ（焦点距離、主点位置、レンズ歪みパラメータ）は、物体の３次元形状モデルを距離画像データ上に投影するために必要となるパラメータである。そのため、例えば、Zhangの方法（Z. Zhang, "A flexible new technique for camera calibration", IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.22, No.11, pages 1330-1334, 2000.）によって事前に距離画像撮像装置１００をキャリブレーションしておく。距離画像撮像装置１００により生成された距離画像データは、距離画像入力部１３０を介して位置姿勢形状計測装置１に入力される。
【００１５】
３次元形状モデル保存部１１０は、物体の３次元形状モデルを保存する。本実施形態では、曲げによる変形のみを考慮し、伸縮による変形は無視できる程度に小さいと仮定する。物体を構成する複数の部品がほとんど伸縮しない場合、各部品は局所的には剛体であると仮定することができる。そのため、本実施形態では、図２（ａ）に示すように、物体の形状を、複数の剛体と２つの剛体間を接続する関節とから構成される３次元形状モデルによって記述する。各剛体は１つ以上の３次元面から構成されるものとして、点の集合及び各点を結んで構成される面の集合によって、各剛体の３次元形状モデルが定義される。剛体間を接続する関節は１又は２自由度を持つ関節である。本実施形態では、１自由度の関節によって剛体が接続されているとする。この場合、関節毎に１自由度の関節角が変形パラメータとして推定される。また本実施形態では、位置パラメータ、姿勢パラメータ及び変形パラメータの計測を行う際に、距離画像データの解像度と物体の形状の近似レベルとを連動させる。そのため、３次元形状モデル保存部１１０は、複数の近似レベルに対応して複数の３次元形状モデルを保持している。近似レベルは、物体を構成する剛体の数（即ち、関節数）によって変更される。図２（ｂ）に示すように、本実施形態では、近似レベルの異なる３つの３次元形状モデルを保持するものとする。本実施形態では、先ず、粗く近似された３次元形状モデルを用いて物体の位置パラメータ、姿勢パラメータ及び変形パラメータを推定する。その後、徐々に高精度な３次元形状モデルを用いた推定を行うcoarse-to-fineアプローチにより、高速に物体の位置パラメータ、姿勢パラメータ及び変形パラメータを推定する。また、近似レベルが低い場合には、低解像度の距離画像データを利用することで局所解に陥ることを防止する。
【００１６】
概略位置姿勢形状入力部１２０は、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を入力する。位置姿勢形状計測装置１には、位置及び姿勢の計測の基準となる３次元座標系（基準座標系）が規定されているものとする。なお、物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を意味する。基準座標系は、距離画像撮像装置１００の中心を原点とし、距離画像撮像装置１００により撮像される距離画像データの水平方向をｘ軸、垂直方向をｙ軸とし、距離画像撮像装置１００の光軸方向をｚ軸とする座標系で定義される。なお、概略位置姿勢形状入力部１２０は、概略値入力手段の適用例となる構成である。
【００１７】
位置姿勢形状計測装置１は、様々な姿勢パラメータや変形パラメータで撮像された物体の距離画像データを予めテンプレートとして保持しておく。そして、概略位置姿勢形状入力部１２０は、入力される距離画像データと予め保持しているテンプレートとのマッチング（テンプレートマッチング）により、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を推定して入力する。但し、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値の入力方法は、これに限るものではない。例えば、時間軸方向に連続して位置パラメータ、姿勢パラメータ及び変形パラメータの計測を行う場合には、前回（前時刻）の計測値を、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値として用いてもよい。また、過去の位置パラメータ、姿勢パラメータ及び変形パラメータの計測値を基に夫々の変化量を推定し、過去の計測値と推定した変化量とから現在の計測値を予測したものを、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値として用いてもよい。また、物体のおおよその位置パラメータ、姿勢パラメータ及び変形パラメータが予めわかっている場合には、その値を、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値として用いてもよい。
【００１８】
距離画像入力部１３０は、距離画像撮像装置１００から距離画像データを入力する。スムージング部１５０は、距離画像入力部１３０において入力された距離画像データに対してスムージング処理を施す。位置姿勢形状算出部１６０は、スムージング処理が施された距離画像データと、３次元形状モデル保存部１１０に保存される３次元形状モデルとに基づいて、物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する。
【００１９】
図３は、本発明の第１の実施形態に係る位置姿勢形状計測装置１の処理を示すフローチャートである。以下、図３を参照しながら、第１の実施形態に係る位置姿勢形状計測装置１の処理について詳細に説明する。
【００２０】
ステップＳ１０１０において、位置姿勢形状計測装置１は初期化処理を行う。即ち、概略位置姿勢形状入力部１２０は、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を初期値として入力する。本実施形態では、この初期値として入力された位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を、撮像された距離画像データに基づいて逐次更新していく。なお、本実施形態では、上述したテンプレートマッチングにより推定された位置パラメータ、姿勢パラメータ及び変形パラメータを上記概略値として用いるものとする。
【００２１】
ステップＳ１０２０において、距離画像入力部１３０は、距離画像撮像装置１００により撮像された距離画像データ（以下、計測距離画像データと称す）を入力する。なお、ここで入力される計測距離画像データの各画素には、距離画像撮像装置１００からの距離値が格納されている。また、ステップＳ１０２０は、画像データ入力手段の処理例である。
【００２２】
ステップＳ１０３０において、位置姿勢形状算出部１６０は、位置パラメータ、姿勢パラメータ及び変形パラメータを推定する際の近似レベルＬを１に設定する。近似レベルＬは正の整数値をとる変数であり、最も粗い場合の近似レベルＬは１であり、高精度になるにつれその値が増加するものとする。本実施形態では、物体の形状の近似レベルＬを３段階で表すため、近似レベルＬは１、２、３の何れかの値をとる。Ｌmaxは近似レベルＬの最大値を表し、ここでは３である。３次元形状モデル保存部１１０には、各近似レベルＬに対応する物体の３次元形状モデルが保存されている。本実施形態では、図２（ｂ）に示すように、近似レベルＬ＝１の場合、剛体が１個（関節数０）、近似レベルＬ＝２の場合、剛体が４個（関節数３）、近似レベルＬ＝３の場合、剛体が８個（関節数７）によって構成される３次元形状モデルが保存されている。
【００２３】
ステップＳ１０５０において、スムージング部１５０は、ステップＳ１０２０で入力された計測距離画像データに対し、物体の形状が全体として滑らかになるように、画像平面上での近傍データを用いたスムージング処理を施す。本実施形態では、スムージング処理として、着目画素近傍Ｎ×Ｎピクセルの範囲において、標準偏差σのガウシアンフィルタによる平滑化が行われる。ガウシアンカーネルの標準偏差σは、計測距離画像データをどの程度平滑化されるかを決定するパラメータであり、大きくなればなるほど物体の形状は滑らかになる。その結果、位置パラメータ、姿勢パラメータ及び形状パラメータの推定処理は安定するが、形状を平滑化した分だけ推定される位置パラメータ、姿勢パラメータ及び変形パラメータが曖昧になる。本実施形態では、近似レベルＬが小さい場合はσを大きくとって処理を安定化し、近似レベルＬが大きくなるにつれて標準偏差σを小さくすることで最終的に位置パラメータ、姿勢パラメータ及び変形パラメータを高精度に推定できるようにする。Ｎは近傍を規定する範囲を示しており、その値は任意に決めてもよいし、ガウシアンカーネルの標準偏差σから求めてもよい。計測距離画像データの平滑化が行える限り、Ｎの設定に制限はない。また、スムージング処理は、ガウシアンフィルタの適用に限らず、平均化フィルタやメディアンフィルタを適用したり、計測距離画像データのサイズを縮小することにより、画像データの平滑化を行う処理であれば何でもよく、特に制限はない。本実施形態では、近似レベルＬ＝１の場合、標準偏差σ＝１．２５、範囲Ｎ＝５であり、近似レベルＬ＝２の場合、標準偏差σ＝０．７５、範囲Ｎ＝３に設定してガウシアンフィルタによる平滑化処理を行い、近似レベルＬ＝３の場合は平滑化を行わない。以上のようなスムージング処理を計測距離画像データ上の全画素に対して行うことにより、形状が滑らかに処理された計測距離画像データ（以下、平滑化計測距離画像データと称す）が得られる。
【００２４】
次に、スムージング部１５０は、平滑化計測距離画像データから３次元点群の座標を算出する。即ち、スムージング部１５０は、平滑化計測距離画像データ上の各画素について、画素の位置に対応する視線ベクトルと、当該画素に格納される距離値とを乗算することにより、各画素に対応する３次元点群の座標を求める。以上の処理を、平滑化計測距離画像データの全画素に対して行うことにより、滑らかな形状に変換された３次元点群の座標が得られる。
【００２５】
ステップＳ１０６０において、位置姿勢形状算出部１６０は、ステップＳ１０５０において生成された平滑化計測距離画像データに基づいて、物体の位置パラメータ、姿勢パラメータ及び変形パラメータを算出する。ここでは、近似レベルＬが１より大きい場合、近似レベルＬ−１において算出された変形パラメータを近似レベルＬにおける変形パラメータに変換したものを、変形パラメータの概略値として用いる。
【００２６】
先ず、位置姿勢形状算出部１６０は、ステップＳ１０５０で生成された平滑化計測距離画像データと３次元形状モデルとの対応付けを行う。即ち、位置姿勢形状算出部１６０は、近似レベルＬに対応する物体の３次元形状モデルを構成する剛体の各３次元面上において、画像平面上で等間隔となるように、又は３次元空間上で等間隔となるようにモデル点を設定する。なお、モデル点は、３次元座標とともに３次元の法線ベクトルも保持している。３次元空間上で等間隔になるようなモデル点の設定は、事前に行っておいてもよい。一方、画像平面上で等間隔になるようにモデル点を設定する場合、位置姿勢形状算出部１６０は、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値と、距離画像撮像装置１００の内部パラメータとを用いて、計測距離画像データ上に物体をレンダリングする。そして位置姿勢形状算出部１６０は、画像平面上の物体の領域内に等間隔にモデル点を配置する。位置姿勢形状算出部１６０は、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値と、距離画像撮像装置１００の内部パラメータとを用いて、計測距離画像データ上に各モデル点を投影する。そして、計測距離画像データ上でモデル点と同じ２次元座標となる平滑化計測距離画像データの３次元点が参照されることにより、モデル点と平滑化計測距離画像データの３次元点とが対応付けられる。
【００２７】
次に、位置姿勢形状算出部１６０は、モデル点と平滑化計測距離画像データの３次元点との対応付けに基づいて、非線形最適化計算を用いて物体の位置パラメータ、姿勢パラメータ及び変形パラメータを算出する。本実施形態では、非線形最適化手法としてGauss-Newton法を用いるものとする。なお、非線形最適化手法はGauss-Newton法に限るものではない。例えば、より計算がロバストであるLevenberg-Marquardt法を用いてもよいし、よりシンプルな方法である最急降下法を用いてもよい。また、共役勾配法やＩＣＣＧ法等、他の非線形最適化計算手法を用いてもよい。
【００２８】
物体の位置パラメータ、姿勢パラメータ及び変形パラメータの算出は、各モデル点に対応する平滑化計測距離画像データの３次元点が３次元形状モデルに当てはまるように、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を反復して更新することにより行われる。より具体的には、位置姿勢形状算出部１６０は、平滑化計測距離画像データ中の３次元点と、モデル点を含む平面との符号付距離を、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの関数として線形近似する。
【００２９】
次に、位置姿勢形状算出部１６０は、符号付距離を０としたときに位置パラメータ、姿勢パラメータ及び変形パラメータの各計測値について成り立つ線形方程式を組立て、この線形方程式を連立方程式として解く。これにより、位置姿勢形状算出部１６０は、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの微小変化を求め、位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を更新する。以上の処理を繰り返すことにより、最終的な位置パラメータ、姿勢パラメータ及び変形パラメータが算出される。
【００３０】
物体の形状の近似レベルＬが増加するとともに推定される変形パラメータ（関節角）が増えることにより、最初は大まかな推定を行い、徐々に高精度に推定していくcoarse-to-fineによる推定が可能となる。また、計測距離画像データの解像度と物体の形状の近似レベルＬとを連動させることにより、局所解に陥らず且つ高速に推定を行うことができる。計測距離画像データと３次元形状モデルとの誤差の最小化処理に関しては、本発明の本質に関わる事項ではないため、説明を省略する。最小化処理の詳細は、非特許文献［Y．Chen and G．Medioni，“Object modeling by registration of multiple range images，”Image and Vision Computing，10（3），pp．145−155,1992.］を参照されたい。
【００３１】
ステップＳ１０７０において、位置姿勢形状算出部１６０は、近似レベルＬに１を加算する。ステップＳ１０８０において、位置姿勢形状算出部１６０は、近似レベルＬが上限値Ｌmax（ここでは、３）を超えたか否かを判定する。近似レベルＬが上限値Ｌmaxを超えた場合、処理は終了する。一方、近似レベルＬが上限値Ｌmaxを超えていない場合、処理はステップＳ１０５０に戻る。そして、位置姿勢形状計測装置１は、より高い近似レベルＬの３次元形状モデルを用いて、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの更新を行う。
【００３２】
以上説明したように、本実施形態においては、距離画像データの解像度と物体の形状の近似レベルＬとを連動させたcoarse-to-fineアプローチにより、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの推定を行う。これにより、本実施形態によれば、局所解に陥らず且つ高速に、物体の位置パラメータ、姿勢パラメータ及び変形パラメータを推定することができる。
【００３３】
次に、本発明の第２の実施形態について説明する。第１の実施形態では、３次元形状モデルの近似レベルＬを距離画像データの解像度と連動させて推定を行った。これに対し、第２の実施形態では、３次元形状モデルの近似レベルＬだけではなく、３次元形状モデルが表す形状の詳細度（空間周波数）も距離画像データの解像度と連動させる。これにより、第２の実施形態では、さらに安定した物体の位置パラメータ、姿勢パラメータ及び変形パラメータの推定を行うことができる。
【００３４】
図４は、本発明の第２の実施形態に係る位置姿勢形状計測装置２の構成を示す図である。図４に示すように、本発明の第２の実施形態に係る位置姿勢形状計測装置２は、３次元形状モデル保存部２１０、概略位置姿勢形状入力部２２０、距離画像入力部２３０、モデル投影部２４０、スムージング部２５０、及び、位置姿勢形状算出部２６０を備える。また、位置姿勢形状計測装置２には距離画像撮像装置２００が接続されている。距離画像撮像装置２００、３次元形状モデル保存部２１０、概略位置姿勢形状入力部２２０、距離画像入力部２３０はそれぞれ、第１の実施形態に係る距離画像撮像装置１００、３次元形状モデル保存部１１０、概略位置姿勢形状入力部１２０、距離画像入力部１３０と同一の構成であるため、これらについての説明は省略する。
【００３５】
モデル投影部２４０は、３次元形状モデル保存部２１０に保存された３次元形状モデルを、概略位置姿勢形状入力部２２０により入力された位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を用いて、計測距離画像データの画像平面上に投影する。この投影処理の結果として、視点から３次元形状モデルの表面までの距離が格納された計測距離画像データ（以下、モデル距離画像データと称す）が生成される。
【００３６】
スムージング部２５０は、距離画像入力部２３０から入力された計測距離画像データと、モデル投影部２４０により生成されたモデル距離画像データとに対して、スムージング処理を施す。位置姿勢形状算出部２６０は、スムージング処理が施された計測距離画像データ及びモデル距離画像データに基づいて、物体の位置パラメータ、姿勢パラメータ及び変形パラメータを計測する。
【００３７】
図５は、本発明の第２の実施形態に係る位置姿勢形状計測装置２の処理を示すフローチャートである。以下、図５を参照しながら、本発明の第２の実施形態に係る位置姿勢形状計測装置２の処理について説明する。なお、ステップＳ２０１０、Ｓ２０２０、Ｓ２０３０、Ｓ２０７０及びＳ２０８０はそれぞれ、第１の実施形態におけるステップＳ１０１０、Ｓ１０２０、Ｓ１０３０、Ｓ１０７０及びＳ１０８０と同一の処理であるため、これらについての説明は省略する。
【００３８】
ステップＳ２０４０において、モデル投影部２４０は、３次元形状モデルを計測距離画像データと同等のデータ量及び性質になるように変換を行う。具体的には、モデル投影部２４０は、ステップＳ２０２０において入力された計測距離画像データの画像平面上に該当する近似レベルの３次元形状モデルを投影する。これにより、計測距離画像データと同じように視点から３次元形状モデルまでの距離が格納されたモデル距離画像データが生成される。本実施形態では、物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値と、キャリブレーション済みの距離画像撮像装置２００の内部パラメータとを用いて、近似レベルＬの３次元形状モデルをレンダリングする。レンダリングにより、視点から３次元形状モデルの表面までの距離がデプスバッファに格納される。モデル投影部２４０は、デプスバッファに格納された距離をモデル距離画像データにおいて格納される距離として取り扱う。以上の処理により、３次元形状モデルが計測距離画像データと同等のデータ形式に変換される。
【００３９】
ステップＳ２０５０において、スムージング部２５０は、ステップＳ２０４０で生成されたモデル距離画像データとステップＳ２０２０で入力された計測距離画像データとに対し、物体の形状が全体として滑らかになるように、画像平面上での近傍データを用いたスムージング処理を行う。スムージング処理については、第１の実施形態と同様であるため、説明を省略する。スムージング部２５０は、モデル距離画像データに対しても計測距離画像データと同様の平滑化処理を行うことにより平滑化モデル距離画像データを生成し、平滑化モデル距離画像データの各画素に対応する３次元座標を求める。但し、平滑化モデル距離画像データの場合、３次元形状モデル領域でない部分には無効な距離値が格納されているため、この部分の３次元座標は求めない。また、平滑化モデル距離画像データに関しては、各画素における３次元法線も求めておく。具体的には、着目画素を中心とする３×３画素領域内に含まれる９画素の３次元座標に平面当てはめを行い、その平面の法線ベクトルを着目画素の３次元法線とする。
【００４０】
以上の処理を、モデル距離画像データと計測距離画像データとの全画素に対して行うことにより、モデル距離画像データと計測距離画像データとのそれぞれについて、滑らかな形状に変換された３次元点群が得られる。
【００４１】
ステップＳ２０６０において、位置姿勢形状算出部２６０は、ステップＳ２０５０でスムージング処理が施されたモデル距離画像データと計測距離画像データとに基づいて、物体の位置パラメータ、姿勢パラメータ及び変形パラメータを算出する。以下では、スムージング処理が施されたモデル距離画像データを、平滑化モデル距離画像データと称す。
【００４２】
先ず、位置姿勢形状算出部２６０は、ステップＳ２０５０で生成された平滑化計測距離画像データと平滑化モデル距離画像データ間の対応付けを行う。本実施形態では、平滑化モデル距離画像データ上の画素と３次元点群との関係は既知であるとして、平滑化モデル距離画像データ上の３次元形状モデルに該当する領域の全画素に対して、計測距離画像データ上で同じ２次元座標となる平滑化計測距離画像データの３次元点を参照する。これにより、平滑化モデル距離画像データと平滑化計測距離画像データとの３次元点が対応付けられる。但し、平滑化モデル距離画像データ上の３次元形状モデルに該当する領域内の全画素に対して処理を行うのではなく、当該領域内で画像上の等間隔や３次元空間上で等間隔になるように間引いて処理してもよい。対応付け処理を行う３次元点は、データ量と計算量のトレードオフとなる。位置パラメータ、姿勢パラメータ及び変形パラメータの算出処理に十分なだけの対応付けが得られる限り、処理する３次元点の選択は何れの場合でよく、特に制限はない。以上の処理により、平滑化計測距離画像データと平滑化モデル距離画像データ間の３次元点の対応付けが行われる。
【００４３】
次に、位置姿勢形状算出部２６０は、平滑化計測距離画像データと平滑化モデル距離画像データ間の３次元点の対応付けに基づき、非線形最適化計算を用いて物体の位置パラメータ、姿勢パラメータ及び変形パラメータを算出する。物体の位置パラメータ、姿勢パラメータ及び変形パラメータの算出は、平滑化モデル距離画像データ中の各３次元点が、対応する平滑化計測距離画像データ中の３次元点に当てはまるように物体の位置パラメータ、姿勢パラメータ及び変形パラメータの概略値を反復演算により補正することにより行う。より具体的には、位置姿勢形状算出部２６０は、平滑化計測距離画像データ中の３次元点と、平滑化モデル距離画像データ中の３次元点及び３次元法線とから、３次元空間中での点と平面との符号付距離を物体の位置パラメータ、姿勢パラメータ及び変形パラメータの関数として線形近似する。具体的な位置パラメータ、姿勢パラメータ及び変形パラメータの算出方法は、第１の実施形態と同様であるため、説明を省略する。
【００４４】
以上のように、本実施形態においては、３次元形状モデルの近似度Ｌだけでなく、３次元形状モデルが表す形状の詳細度（空間周波数）も計測距離画像データの解像度と連動させる。これにより、本実施形態によれば、３次元形状モデルと計測距離画像データとの空間周波数の違いが原因で発生する、位置パラメータ、姿勢パラメータ及び形状パラメータの推定の不安定性が減少し、より安定したこれらのパラメータの推定が可能となる。
【００４５】
上述した実施形態では、画像サイズは変えずにスムージング処理をかけることによって計測距離画像データの実効的な解像度を小さくしていた。しかしながら、計測距離画像データの解像度を小さくする方法はこれに限るものではなく、画像サイズが小さな計測距離画像データを生成するようにしてもよい。具体的には、近似レベルＬに応じた画像サイズを予め設定しておき、そのサイズの計測距離画像データをバイリニア補間等の内挿アルゴリズムを用いて生成してもよい。画像サイズを小さくすることにより画素アクセスが高速化されるため、全体として処理が高速化されることが期待される。
【００４６】
また、上述した実施形態では、近似レベルに依らず、複数の剛体と剛体間を結ぶ関節とによって物体をモデル化していた。しかしながら、モデル化の方法はこれに限るものではなく、近似レベルが粗い場合には関節を持たずに単一の剛体だけでモデル化してもよい。変形量が少ない物体の場合には、計測距離画像データの解像度が低い場合には剛体のモデルを用いる方がオーバーフィッティングの問題がなく推定が安定することが期待される。
【００４７】
また、剛体が関節によって接続される３次元形状モデルでなくとも、形状を表す自由度が可変な３次元形状モデルであればどのようなモデルであってもよい。例えば、３次元形状モデルはNURBS曲面であり、制御点の数によって自由度を変更してもよい。さらには、近似レベルの異なる３次元形状モデルをオフラインで作っておく必要は必ずしもなく、位置パラメータ、姿勢パラメータ及び変形パラメータを計測する際にオンラインで生成してもよい。
【００４８】
また、上述した実施形態においては、特に推定するパラメータの範囲に制限を設けることなく近似レベルＬ毎に自由に推定を行っていた。しかし、計測距離画像データの空間周波数が高くなるように解像度を変えると、場合によっては間違った推定を行ってしまうことがある。そのため、前の近似レベルＬの３次元形状モデルにおける変形パラメータを、現在の近似レベルＬの３次元形状モデルにおける変形パラメータに変換した値から極端に離れないような最適化を行う。これにより、coarse-to-fineアプローチによる位置パラメータ、姿勢パラメータ及び変形パラメータの推定をより安定化させることができる。具体的には、［D. G. Lowe, "Fitting parameterized three-dimensional models to images," IEEE Trans. on PAMI, vol.13, no.5, pp.441-450, 1991. ］に開示されるように、各パラメータが更新前の値に拘束されるという拘束条件を付すことにより、更新前の値からあまり離れない解を得るようにする。
【００４９】
また、上述した実施形態においては、剛体と剛体間を結ぶ１自由度の関節とによって物体をモデル化していた。しかしながら、位置パラメータ、姿勢パラメータ及び変形パラメータの計測対象となる物体は、このような３次元形状モデルによって表現される物体に限るものではない。即ち、例えば丸いケーブル状の物体のように、複数の円筒と２自由度の関節とによってモデル化される物体であってもよい。
【００５０】
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【符号の説明】
【００５１】
１、２：位置姿勢形状計測装置、１００、２００：距離画像撮像装置、１１０、２１０：３次元形状モデル保存部、１２０、２２０：概略位置姿勢形状入力部、１３０、２３０：距離画像入力部、２４０：モデル投影部、１５０、２５０：スムージング部、１６０、２６０：位置姿勢形状算出部
【図１】
0005991730.tif 000002
【図２】
0005991730.tif 000003
【図３】
0005991730.tif 000004
【図４】
0005991730.tif 000005
【図５】
0005991730.tif 000006