JP5480667B2

JP5480667B2 - 位置姿勢計測装置、位置姿勢計測方法、プログラム

Info

Publication number: JP5480667B2
Application number: JP2010043061A
Authority: JP
Inventors: 真和藤木; 祐介中里
Original assignee: Canon Inc
Current assignee: Canon Inc
Priority date: 2010-02-26
Filing date: 2010-02-26
Publication date: 2014-04-23
Anticipated expiration: 2030-02-26
Also published as: JP2011179909A

Description

本発明は、３次元形状が既知である対象物体の位置及び姿勢を計測する技術に関する。特に対象物体を撮影した距離画像に基づいて対象物体の位置及び姿勢を算出する位置姿勢計測装置、位置姿勢計測方法、およびプログラムに関する。

近年、ロボット技術の発展とともに、これまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。複雑なタスクの代表的な例として、工業製品の組み立てが挙げられる。ロボットが組み立て作業を行うためには、ロボットハンドなどのエンドエフェクタによって部品を把持したり、他の部品と嵌合させたりすることが必要となる。そのためには、把持の対象となる部品あるいは嵌合の対象となる部品とロボットとの相対的な位置及び姿勢を計測し、計測結果に基づいてロボットハンドの移動計画を策定して、実際にロボットハンドを駆動するためのアクチュエータを制御する必要がある。

ロボットの位置姿勢計測は、これまでロボットに搭載されたカメラや距離センサを用いて行われており、２次元画像や距離画像を用いる方法が代表的である。特に、オフィスや工場などのシーンや工業製品の部品など、対象が直線や曲線、濃淡変化の少ない面など単調な特徴しか持たない場合には距離画像を用いることが多い。

非特許文献１では、距離画像へのモデルフィッティングによる物体の位置及び姿勢の計測を行う方法が開示されている。この方法では、距離画像を３次元点群データに変換し、点群データに対象物体の３次元形状モデルを当て嵌めることにより物体の位置及び姿勢を計測する。すなわち、位置及び姿勢の概略値をもとに、点群データの各点について３次元形状モデルの近傍の面を探索し、点と面との距離の和を最小化するように位置及び姿勢の最適化を繰り返すことにより位置及び姿勢を算出するものである。

非特許文献２では、時系列で得られる距離画像をもとに、非特許文献１の方法を用いて、フレーム間での物体の位置及び姿勢の変化を推定する手法が開示されている。非特許文献３では、距離画像へのモデルフィッティングによる物体の位置及び姿勢を推定する手法が開示されている。この方法では、距離画像を３次元点群データに変換し、点群データに陰多項式で表現された対象物体の３次元形状モデルを当て嵌めることにより物体の位置及び姿勢を高速に推定する。すなわち、位置及び姿勢の概略値をもとに、点群データの各点について、陰多項式が定義する距離場で近似される点と３次元モデル表面との距離の和を最小化するように位置及び姿勢の最適化を繰り返すことにより位置及び姿勢を算出するものである。

P. J. Besl and N. D. McKay, "A method for registration of 3-D shapes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.14, no.2, pp.239-256, 1992. D. A. Simon, M. Hebert, and T. Kanade, "Real-time 3-D pose estimation using a high-speed range sensor," Proc. 1994 IEEE International Conference on Robotics and Automation (ICRA '94), pp.2235-2241, 1994. B. Zheng, R. Ishikawa, T. Oishi, J. Takamatsu, K. Ikeuchi, "6-DOF Pose Estimation from Single Ultrasound Image Using 3D IP Models, " Proc. IEEE International Workshop on Object Tracking and Classification Beyond the Visible Spectrum (OTCBVS08), pp.1-8, 2008. M. M. Blane, Z. Lie, H. Civi, D. B. Cooper, "The 3L algorithm for fitting implicit polynomial curves and surfaces to data, " IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.22, no.3, pp.298-313, 2000. T. Sahin, M. Unel, "Fitting Globally Stabilized Algebraic Surfaces to Range Data, " Proc. IEEE International Conference on Computer Vision, vol.2, pp.1083-1088, 2005. Z. Zhang, "A flexible new technique for camera calibration," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.22, no.11, pp.1330-1334, 2000. X. Jiang and H. Bunke, "Range image segmentation: Adaptive grouping of edges into regions," Proceedings of Asian Conference on Computer Vision 1998 (ACCV '98), pp.299-306, 1998. A. Bab-Hadiashar, N. Gheissari, "Range image segmentation using surface selection criterion," IEEE Transactions on Image Processing, Vol.15, No.7, pp.2006-2018, 2006.

一般に、位置姿勢を推定しようとする対象物体を撮影した距離画像には、対象物体のデータだけではなく、対象物体の周囲や背後に存在する対象物体以外の物体（非対象物体）のデータが混在する。このような距離画像に基づいて対象物体の位置及び姿勢を推定する場合、形状モデル当て嵌めの算出結果は非対象物体の３次元点群データの影響を受ける。このため、距離画像に対象物体のデータのみを含む場合に比べて位置姿勢計測（算出）精度が低下するという問題がある。この問題に対処する方法として、前述のロバスト推定を用いることができる。対象物体に比べ、対象物体以外の物体（非対象物体）はモデルとの対応間距離が大きくなる傾向があるので、ロバスト推定を適用すると、最適化計算に対する非対象物体のデータによる影響が低下するためである。

ところが、前述したような位置姿勢計測（算出）方法では、処理対象とする３次元点群の数が多くなるほど、処理負荷が増大し、かつメモリ消費量が増大する。従って、物体の位置姿勢を推定する際は、処理対象の３次元点群を対象物体のみに限定するのが理想的である。しかしながら、従来のロバスト推定を用いた方法では、本来は不必要な非対象物体のデータをモデル当て嵌めの処理から除外していない。すなわち、従来の方法では、処理速度の低下・メモリ消費量の増大という課題があった。

本発明は、対象物体と非対象物体の像を含む距離画像データを入力データとして対象物体の位置及び姿勢を推定する処理の高速化、及びメモリ消費量を軽減した位置姿勢計測技術の提供を目的とする。

本発明に係る位置姿勢計測装置は、撮像手段により撮像された対象物体の距離画像を取得する取得手段と、
取得された前記距離画像において、物体領域を設定する設定手段と、
設定された前記物体領域の３次元点群に、前記対象物体をモデル化した物体モデルの表面を当て嵌めることによって、前記対象物体の位置及び姿勢を推定する推定手段と、
前記推定手段によって推定された前記位置及び姿勢に基づいて、前記物体モデルと前記３次元点群との距離を示す情報を導出し、該導出された距離を示す情報を用いて、前記物体領域の設定を更新する更新手段と、
前記更新手段による前記物体領域の設定の更新に従い、前記推定手段に、当該更新後の物体領域の３次元点群を用いて、前記対象物体の位置及び姿勢を推定させる制御手段と
を備えることを特徴とする。

本発明によれば、位置姿勢推定処理の対象とする距離画像データを対象物体の領域に限定することにより、処理速度を向上させ、メモリ消費量を抑制した位置姿勢計測技術の提供が可能になる。

本発明の実施形態にかかる位置姿勢計測装置のハードウェア構成を説明する図。第１実施形態にかかる位置姿勢計測方法の処理を説明する図。第１実施形態における距離画像の領域分割工程の処理を説明する図。第１実施形態におけるモデル当て嵌め工程の処理を説明する図。第１実施形態における物体領域更新工程の処理を説明する図。複数の物体が撮影された距離画像から１つの物体の位置姿勢の推定を例示的に説明する図。第２実施形態にかかる位置姿勢計測方法の処理の流れを説明する図。第２実施形態にかかる物体領域更新処理の流れを説明する図。

（第１実施形態）
本実施形態では、山積み状態になっている同一形状の物体群から、ロボットハンドで１個ずつ物体を把持して取り上げるロボットの動作における物体の位置姿勢計測を想定する。そのために、物体群の上方から距離画像を撮影する。撮影した距離画像には複数の物体までの距離値が記録されているが、その中から一番上方にある物体が占める領域を検出しつつ、その物体の位置姿勢を計測する。

本実施形態における処理の概略は以下のようになる。まず、距離画像を同一の面に属すると推定される部分的な領域に分割する。分割された領域のうち、距離値が最小の点を含む領域、すなわち最上部にある領域から開始して、把持対象とする物体に属すると推定される領域を追加しながら物体の形状モデルを当て嵌め、対象物体の位置姿勢を推定する。このとき、追加する領域はモデルに当て嵌めたときの誤差が小さくなるように選択する。この選択方法により、選択された領域群の中に複数の物体のデータ（非対象物体のデータ）が含まれる危険性を軽減することができる。

以下の説明において、「部分領域」とは、距離画像を同一の面に属すると推定される領域に分割した各々の領域をいう。また、「物体領域」とは、距離画像上で対象物体に属すると推定された領域で、少なくとも１つ以上の部分領域から成る領域をいう。

図１の参照により、本発明の実施形態にかかる位置姿勢計測装置のハードウェア構成を説明する。ＣＰＵ１０１は、装置全体の動作を制御する。メモリ１０２は、ＣＰＵ１０１の動作に用いるプログラムやデータを格納する。バス１０３は、各構成モジュール間のデータ転送を司る。バス１０３と各種装置とはインタフェース１０４に接続されている。外部記憶装置１０５はＣＰＵ１０１に読み込むプログラムやデータを格納する。キーボード１０６およびマウス１０７はプログラムを起動したり、プログラムの動作を指定したりするための入力装置として機能する。表示部１０８はプロセスの動作結果を表示するための表示部である。データ入出力部１０９は外部装置との間でデータの送受信を行なうことが可能である。距離画像計測装置はデータ入出力部１０９を介して位置姿勢計測装置に接続されている。

図２の参照により第１実施形態にかかる位置姿勢計測方法の処理の流れを説明する。本処理は、ＣＰＵ１０１の全体的な制御の下に実行される。ステップＳ２０１において、処理に用いるデータの初期設定を行う。具体的には、外部記憶装置１０５から形状モデルデータをメモリ１０２にロードする。また、距離画像計測装置の撮像部のカメラパラメータ（主点位置、焦点距離、画像歪み係数）、及び距離画像の画素値と距離値の変換比率を、データ入出力部１０９に接続された不図示の距離画像計測装置から入力する。

物体の形状モデルは陰多項式モデルであり、（数１）に示す式で定義される。ここで、ｘ＝（ｘ、ｙ、ｚ）が３次元空間中の点の座標、ｎが陰多項式の次数である。（数１）の式が「０」になる座標（ｘ、ｙ、ｚ）が、陰多項式モデルが表す物体の表面を表す。（数１）の式が「０」より大きい値になる座標が対象物体の外部を表す。（数１）の式が「０」より小さい値になる座標が対象物体の内部を表す。

この陰多項式モデルは対象物体のＣＡＤモデルやレンジファインダ等で計測した３次元点群から、非特許文献４や非特許文献５で開示されている方法を用いてあらかじめ生成しておく。非特許文献４では、陰多項式で表現された３次元モデルを構築する手法が開示されている。陰多項式モデルは対象となる３次元形状のみを適切に表現するように構築することが難しい。この方法では、対象形状の表面上の点、対象形状の表面からある距離ｃだけ外側の点、対象形状の表面から距離ｃだけ内側の点の３組の点群と、対応する陰多項式の値の２乗が最小となるよう陰多項式の係数を決定する。非特許文献５では、非特許文献４で開示されている方法にリッジ回帰を用いることで、複雑な形状の陰多項式をより安定して求める方法が開示されている。

カメラパラメータは、例えば、非特許文献６で開示される方法により事前に校正しておく。ステップＳ２０１で入力するカメラパラメータを用いることによって，距離画像の各画素の２次元座標および画素値から、その画素に対応する３次元空間中の点の撮像部座標系における３次元座標を算出することができる。

ステップＳ２０２では、データ入出力部１０９に接続された不図示の距離画像計測装置によって計測された距離画像データ（以下、単に「距離画像」ともいう）を取得し、メモリ１０２に格納する。なお、距離画像を撮影する手段は任意のものを用いることができるが、例えば、光切断法やステレオ法により撮影することが可能である。

次のステップＳ２０３では，距離画像を部分領域に分割する。本ステップの処理内容の詳細は後述する。分割結果は正の整数値が記録された２次元のマップ（以下「部分領域マップ」と呼ぶ）の形式でメモリ１０２に格納される。部分領域マップの大きさは距離画像の縦・横の画素数に等しい。部分領域マップに記録された正の整数値は同一の部分領域に対しては同一の値であり、異なる部分領域は異なる値を持つ。すなわち、部分領域マップに記録されているのは、距離画像の各画素が属する部分領域の識別番号ということになる。したがって、部分領域マップの値を参照することにより、参照したマップ上の位置に相当する距離画像上の画素がいずれの部分領域に属するかを一意に決定することができる。

ステップＳ２０４では、先のステップＳ２０３で抽出された部分領域群のうち、距離値が最小の画素を共通に含む部分領域を探索し、物体領域（初期物体領域）として設定する。このことにより，最も上方（距離画像撮影装置に近い側）に存在する部分領域が物体領域となる。以降、本ステップで物体領域に設定した部分領域を含む物体を位置姿勢の計測対象とする。物体領域のデータは，その物体領域に属する部分領域の識別番号のリストとしてメモリ１０２に記録される。本ステップの処理完了後、物体領域データは少なくとも１つの部分領域の識別番号を含むことになる。

ステップＳ２０５では，物体の形状を表す陰多項式モデルを物体領域内の距離計測点群に当て嵌めることによって、計測対象物体の位置姿勢を推定する。本ステップについては、詳細を後述する。

ステップＳ２０６では、設定済みの物体領域に新たに部分領域を加えるか否かを判定して、判定結果が真なら物体領域を更新する。本ステップの処理の詳細についても後述する。

ステップＳ２０７では、先のステップＳ２０６において物体領域データが更新されたか否かを判定し、更新があれば（Ｓ２０７−Ｙｅｓ）、ステップＳ２０５に処理を戻して、対象物体の位置姿勢を再度推定する。物体領域データの更新が無ければ（Ｓ２０７−Ｎｏ）、物体領域が確定し、かつ対象物体の位置姿勢推定も完了したとして処理全体を終了する。

（距離画像の領域分割工程の処理）
距離画像の領域分割工程（ステップＳ２０３）の詳細を図３の参照により説明する。本工程は、非特許文献７で開示された方法に基づいて、距離画像を後述するエッジによって囲まれた閉領域ごとに分割する。ここで検出された領域を部分領域とする。

処理を開始すると、ステップＳ３０１でエッジを抽出する。ここで、「エッジ」とは距離値が不連続に変化する「ジャンプエッジ」、または物体表面の法線方向が不連続に変化する「クリーズエッジ」である。具体的には、まず距離画像上の直線（「スキャンライン」と呼ぶ）に相当する被写体上の曲線を複数の２次曲線セグメントに分割する。そして、曲線セグメントの各境界点に関して「エッジの強さ」（ジャンプエッジらしさを表す奥行き値の不連続度，クリーズエッジらしさを表すセグメントの法線方向の不連続度）を算出し、エッジの強さが閾値以上の点をエッジとして抽出する。なお、エッジの方向は様々であるので、スキャンラインは距離画像の縦方向・横方向・斜め方向（４５°、１３５°）の４方向をとり、４方向でのエッジの強さが最も高い接続点をエッジとする。以上のようにして、距離画像上のエッジ画素の位置を表したエッジマップを作成し、メモリ１０２に記録する。

次に、ステップＳ３０２では、先のステップＳ３０１で作成したエッジマップをラベリングする。その結果、距離画像は連結したエッジ画素によって区切られる閉領域群に分割される。分割結果は、距離画像と同じサイズのラベル画像としてメモリ１０２に記録する。なお、ラベルの値は閉領域ごとに一意に設定される。続くステップＳ３０３では、先のステップＳ３０２で検出された閉領域の中から面積Ｔ（「Ｔ」は部分領域の面積閾値）以上の領域を選択し、選択された閉領域のラベル値のリスト（以下、このリストを「領域リスト」と呼ぶ）としてメモリ１０２に記録する。

ステップＳ３０４では、領域リストに登録された閉領域の数が「０」であるか否かを判定する。閉領域の数が「０」ならば（Ｓ３０４−Ｙｅｓ）、処理はステップＳ３１２に進められる。一方，閉領域の数が「０」でなければ（Ｓ３０４−Ｎｏ）、処理はステップＳ３０５に進められる。ステップＳ３０５では、領域リストに登録された閉領域群から未処理の閉領域Ｒを削除し、続いて閉領域Ｒに３次元平面または３次元曲面を当て嵌める（ステップＳ３０６）。

ステップＳ３０７では、ステップＳ３０６にて平面または曲面を当て嵌めた際の当て嵌め誤差が所定の閾値未満であるか否かを判定する。閾値未満であれば（Ｓ３０７−Ｙｅｓ）、閉領域Ｒには複数の面が含まれないと推定されるので、部分領域として登録し（Ｓ３０８）、ステップＳ３１２に処理を進める。一方、閾値以上と判定されれば（Ｓ３０７−Ｎｏ）、処理はステップＳ３０９に進められる。なお、「当て嵌め誤差」とは、閉領域Ｒの各画素に相当する３次元点と、当て嵌めた平面または曲面上の距離が最小となる最近傍点との間の距離の２乗和を領域Ｒの全画素について平均をとったものである。ステップＳ３０７において、当て嵌め誤差が閾値以上と判定された場合、閉領域Ｒに複数の面が含まれる可能性がある。これは、距離画像に含まれるノイズなどの影響により、閉領域内の複数の面の境界に存在するエッジが途切れているためと推定される。このように途切れたエッジを接続して、複数の面を区分するため、ステップＳ３０９では、エッジマップに膨張処理を施す。続いて、ステップＳ３１０でエッジマップをラベリングし、ステップＳ３１１にて閉領域の候補を抽出する。ステップＳ３１０およびＳ３１１の処理内容は，それぞれステップＳ３０２およびＳ３０３と同様である。

一方、ステップＳ３０７で当て嵌め誤差が閾値未満と判定された場合、閉領域Ｒには単一の面のみ含まれると推定される。そこで、ステップＳ３０８では、閉領域Ｒを部分領域として登録する。最後に、ステップＳ３１２にて、エッジ部の画素など閉領域として未登録の画素を周囲の閉領域に統合する。このステップでも平面あるいは曲面を当て嵌めて、当て嵌め誤差が閾値未満なら統合する。

（モデル当て嵌め工程の処理）
次に、モデル当て嵌め工程（ステップＳ２０５）の詳細を図４の参照により説明する。本工程では、計測対象物体の位置及び姿勢の概略値を反復演算により繰り返し補正することにより位置及び姿勢を算出する。ステップＳ４０１では、データ入出力部１０９に接続された不図示の距離画像計測装置に対する対象物体の位置及び姿勢の概略値を推定する。この処理は物体領域の距離画像データを入力として、特開平０１−００９３０７号公報に開示される物体認識的方法により実現することが可能である。ただし、対象物体の位置姿勢の概略値を求める方法は、これに限るものではない。例えば、モデル当て嵌め工程を一度以上経過していれば、その工程で算出した対象物体の位置姿勢を表す値をメモリ１０２に記憶しておき、その値を次回のモデル当て嵌め工程において対象物体の概略位置姿勢の値として使うことが可能である。あるいは、過去の計測値から対象物体の運動の速度や角速度を線形フィルタやカルマンフィルタなど時系列フィルタリング処理により推定し、推定された速度や角速度によって現在の時刻における対象物体の位置及び姿勢を予測したものを用いてもよい。また、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、センサによる出力値を位置及び姿勢の概略値として用いてもよい。センサとしては、例えば、トランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、対象物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであって使用することが可能である。また、対象物体の置かれている、おおよその位置や姿勢が予めわかっている場合には、その値を概略値として用いてもよい。

ステップＳ４０２では、対象物体の位置及び姿勢の概略値を用いて、物体領域内の各画素に対応する３次元空間中の座標を距離画像計測装置の撮像部座標系から物体モデルの座標系に変換する。なお、以下の説明では、本ステップで座標変換の結果、座標を算出した点を「物体領域の３次元点」と呼ぶことにする。

ステップＳ４０３では、陰多項式モデルから構築される距離場と物体領域の３次元点群の各座標に基づいて、３次元点群と対象物体をモデル化した物体モデルの表面との距離の総和を最小にする位置および姿勢を求める。具体的には、（数２）に示すエネルギー関数が最小化されるように位置及び姿勢を最適化する。

ここでΩは物体領域の３次元点群であり、ｘ∈Ωである。また，distは陰多項式モデルから計算される距離であり、モデル表面上では「０」、内部では負の値、外部では正の値をとり、（数３）のように定義される。

位置及び姿勢の最適化は具体的には、最急降下法に基づき（数４）の式による勾配から３次元点群座標Ｘ^k+1を求める（数６）。ここで、Ｘは、Ｘ∈Ｒ^N×3であり、Ｎ個の物体領域の３次元点の座標を表す。

元の３次元点群座標Ｘ^kと、求められたＸ^k+1とから（数７）に示す行列を作成し、Ａ＝ＵＳＶ^Tのように特異値分解を行い、位置及び姿勢の概略値を更新する。ここでＸ^―はＸの平均を示し、ｔは対象物体の位置、Ｒは姿勢を表す行列である。

最後にステップＳ４０４では、収束判定を行い、収束していれば（Ｓ４０４−Ｙｅｓ）、モデル当て嵌め処理全体を終了し、収束していなければステップＳ４０２に処理を戻し、同様の処理を繰り返す。収束判定では、（数４）の式の値がほぼ０である場合や、位置及び姿勢に関する更新前後での誤差ベクトルの二乗和の差がほぼ０である場合に収束したと判定する。

なお、上記の説明ではエネルギー最適化処理に最急降下法を用いているが、これに限るものではない。より収束の速いGauss-Newton法やLevenberg-Marquardt法によって収束判定を行ってもよい。また、共役勾配法など、他の非線型最適化計算手法を収束判定に用いてもよい。

（物体領域更新工程の処理）
物体領域更新工程（ステップＳ２０６）の詳細を図５の参照により説明する。まず、ステップＳ５０１では、物体領域の３次元点群について（数１０）で定義される物体の陰多項式モデルとの平均距離を算出する。ただし、ここではＮは物体領域の３次元点の数であり、Φは物体領域の３次元点群である。

ただし、Ｎは算出の対象とする距離画像上の領域内の３次元点数で、e_iiは以下の（数１１）により求められる。

ここでΦは算出の対象とする距離画像上の領域内の３次元点群である。

次に、ステップＳ５０２では、物体領域に含まれない部分領域の中から、物体領域に追加する部分領域の候補Ｃを選ぶ。具体的には、物体領域及び物体領域に含まれない各部分領域のそれぞれに属する画素群に対応する３次元点群の重心位置を算出する。そして、物体領域に関する重心位置と各部分領域に関する重心位置との距離値を算出し、距離値の小さい順にｎ個の部分領域を物体領域に追加する部分領域の候補Ｃとして選択する。なお、ｎは１以上の正の整数である。

ステップＳ５０３では、候補Ｃの中から未処理の部分領域を１つ選択する。ここで選択した部分領域をａとする。続くステップＳ５０４では、部分領域ａを物体領域に追加するか否か判定するための評価値を算出する。具体的には、部分領域ａに関して、（数１０）で表される物体を表す陰多項式モデルと部分領域との平均距離を算出する。ただし、ここで、Ｎは部分領域ａ内の３次元点の数、Φは部分領域ａ内の３次元点群である。

ステップＳ５０５では、ステップＳ５０１で求めた平均距離に所定比率（＞1.0）をかけたものと、ステップＳ５０４で求めた平均距離とを比較する。後者が前者未満であれば（Ｓ５０５−Ｙｅｓ）、ステップＳ５０６に処理は進められる。一方、後者が前者以上と判定されれば（Ｓ５０５−Ｎｏ）、処理はステップＳ５０７に進められる。

ステップＳ５０６では、部分領域ａを物体領域に追加する。すなわち、物体領域に含まれる部分領域群を表す識別番号のリストに、部分領域ａの識別番号を加える。ステップＳ５０７では、追加する部分領域候補Ｃに含まれる部分領域すべてを処理したか否か判定し、処理済であれば（Ｓ５０７−Ｙｅｓ）、物体領域更新処理を終了する。一方、未処理の部分領域があれば（Ｓ５０７−Ｎｏ）、ステップＳ５０３に処理は戻され、同様の処理が繰り返される。このようにして、すべての追加候補の部分領域に関して物体領域に追加するか否かを判定する処理を繰り返して、物体領域を更新する。

図６の参照により、複数の物体が撮影された距離画像から１つの物体の位置姿勢の推定を例示的に説明する。ただし、物体領域に追加する物体領域の候補の数ｎは１とする。図６（ａ）は距離画像を撮影する対象物体の像であり、同一形状の物体が２つ含まれている。これらの物体を撮影した距離画像を部分領域に分割した様子が図６（ｂ）である。異なる領域を区別して表示するため、領域ごとに塗りのパターンが異なるように表してある。図６（ｂ）に表示した番号は各部分領域に設定された識別番号である。このうち、（６）番は背景領域に相当する。図６（ｃ）から（ｅ）は物体領域を更新する各ステップを表しており、ハッチングを付した部分が物体領域に相当する。図６（ｃ）は、初期の物体領域（ステップＳ２０４の終了後の状態）が示されており、距離画像撮影装置から見て最も手前側の測定点を含む部分領域（１）が物体領域として抽出されている。次に物体領域の更新工程（ステップＳ２０６）を経ると、物体領域は図６（ｄ）に示すようになり、初期の物体領域に部分領域（２）が加えられる。同様に、２度目の物体領域の更新工程（ステップＳ２０６）を経ると、図６（ｅ）のように部分領域（３）がさらに加えられ、物体領域が３つの部分領域（部分領域（１）〜（３））によって構成される。図６（ｅ）の状態から、さらに物体領域の更新工程（ステップＳ２０６）に進むと、部分領域（４）および（５）を物体領域に加えるか否かを判定する。しかし、部分領域（４）および（５）は部分領域（１）から部分領域（３）が属する物体とは異なる物体上の領域なので、ステップＳ５０４で算出するモデルとの平均距離が比較的大きな値をとる。そのため、部分領域（４）および（５）は物体領域に加えないように判定される。以上の様な処理経過を経て、最終的には部分領域（１）から部分領域（３）を含む物体が距離画像から抽出され、その位置姿勢を求めることができる。

本実施形態によれば、位置姿勢推定処理の対象とする距離画像データを対象物体の領域に限定することにより、処理速度を向上させ、メモリ消費量を抑制することができる。

（変形例１−１−１）
本発明の実施形態は上記に限らず様々な変形例が存在する。第１実施形態ではステップＳ２０６において、物体領域内の３次元点群の重心近傍に重心を持つ部分領域を、物体領域に追加する領域の候補として選択している。しかし、追加する部分領域候補の選択方法はこれに限るものではない。例えば、ステップＳ２０４で設定する初期物体領域内の３次元点群の重心近傍に重心を持つ部分領域を選択してもよい。あるいは、３次元座標の重心ではなく、距離画像上における２次元座標の重心が近い部分領域を選択してもよい。

（変形例１−１−２）
第１実施形態では、物体領域に近接する度合いが高い部分領域を物体領域に追加する候補として選定している。しかし、追加候補の部分領域の選定方法はこれに限らない。例えば、面積、すなわち領域に属する画素数が大きい部分領域を候補として選択してもよい。これは、面積の大きい領域ほど距離画像の計測ノイズなどのランダムな誤差の影響を受けにくいため、形状モデルとの距離の算出や物体領域更新後のモデル当て嵌めを高精度に行うことができるからである。

（変形例１−１−３）
他の部分領域の候補選定方法としては、例えば、距離画像上で対象物体が占める領域の情報を利用するものが挙げられる。具体的には、モデル当て嵌め工程にて算出された対象物体の位置及び姿勢に基づいて、形状モデルを距離画像上に投影して、投影像が存在する領域を対象物体が占める領域とする。そして、各部分領域と対象物体が占める領域との重複率（すなわち両領域が重複する画素の数を各部分領域に属する画素の数で割った値）が高い順に，物体領域に追加する部分領域の候補として選定することも可能である。

（変形例１−１−４）
さらには、追加候補の部分領域を選定する基準は１つである必然性はなく、複数の基準を組み合わせてもよい。例えば、変形例１−１から変形例１−３で説明した物体領域への近接度、面積、距離画像上の対象物体領域との重複率の３種類の評価値に重みをかけて加算した値に基づいて候補部分領域を選定することも可能である。

（変形例１−２−１）
物体領域の更新工程（図５）では、物体領域と部分領域に関する形状モデルとの平均距離の大小を比較することによって部分領域を物体領域に追加する可否を判定している。しかし、追加の可否の判定方法はこれに限らず、例えば、平均距離が小さい部分領域が優先的に追加されれば、他の方法を適用することができる。例えば、追加候補の部分領域群のうち、平均距離が最小の部分領域を物体領域に追加してもよい。

（変形例１−２−２）
部分領域の追加可否を判定する基準は１つである必然性はなく、複数の基準を組み合わせてもよい。例えば、変形例１−１から変形例１−３で説明した物体領域への近接度、面積、距離画像上の対象物体領域との重複率、及び形状モデルとの平均距離の４種類の評価値に重みをかけて加算した値に基づいて追加可否を判定することも可能である。

（変形例１−２−３）
また、部分領域は１つずつ追加する必然性はなく、複数の部分領域を同時に物体領域に追加してもよい。その場合、追加する部分領域の候補群から、評価値（例えば部分領域の３次元点群とモデルとの平均距離）が上位複数個の部分領域を物体領域に追加すべく選択することも可能である。

（変形例１−３−１）
第１実施形態では、距離画像を部分領域に分割するため、非特許文献７の方法を用いた。しかし、部分領域の生成方法はこれに限るものでなく，例えば，非特許文献８に基づく方法でもよい。この方法は、所定の評価基準に基づいて距離画像の部分領域を近似するパラメトリックな面の種類（種類ごとに面の数式表現が異なる）を選択し、選択された種類の面で距離画像の表す３次元形状を近似するものである。本手法によれば、近似誤差の少ない領域を一続きの面として抽出することができる。

（変形例１−３−２）
距離データに応じて距離画像を分割するのではなく、所定の分割方法で分割してもよい。例えば、１画素ごとに分割してもよいし、あるいは距離画像の縦横をそれぞれ１０画素ごとに格子状に区切るなどしてもよい。このように所定の分割方法を用いる場合、距離画像の領域分割工程（ステップS203）を省略することが可能になる。

（変形例１−４−１）
第１実施形態では、初期の物体領域に含まれる部分領域として、最も距離値が小さい画素を含む部分領域を選択している。しかし、初期物体領域を設定する方法はこれに限るものではなく、撮像装置に最も近いと見なされる、最も上方に配置されている物体上の領域である可能性のある部分領域を選択すればよい。例えば、距離値の平均が最小の部分領域を選択してもよいし、距離値が小さい順に上位ｍ個（ｍは２以上の正の整数）の画素の平均距離値が最小の部分領域を選択することも可能である。

（変形例１−４−２）
初期の物体領域として距離値が小さい部分領域を優先するのは、物体を上方から撮像し、上方からロボットハンドで把持することを想定しているためである。撮像条件が異なれば、その撮像条件に適合した初期物体領域の設定方法を採用することが可能である。例えば、特定色の物体の位置姿勢を推定するのであれば、距離画像のほかに、異なる第２の撮像装置を用いてカラー画像を撮像する。そして、当該の特定色に最も色が合致するカラー画像上の領域に対応する距離画像上の領域を探索し、その距離画像上の領域を含む部分領域を初期物体領域とすればよい。

（変形例１−４−３）
対象物体の概略の位置及び姿勢が予め分かっていれば、各部分領域について３次元点群の座標と（数１０）で表される平均距離を算出し、平均距離が最小の部分領域を初期物体領域としてもよい。

（変形例１−４−４）
変形１−４―３と同様に対象物体の概略の位置及び姿勢が予め分かっていれば、距離画像上で対象物体が占める領域の情報を利用して初期物体領域を決定することができる。具体的には、対象物体の位置及び姿勢の概略値に基づいて、形状モデルを距離画像上に投影して、投影像が存在する領域を対象物体が占める領域とする。そして，各部分領域と対象物体が占める領域との重複率（両領域が重複する画素の数を各部分領域に属する画素の数で割った値）が最も高い部分領域を初期の物体領域として選択することも可能である。

（変形例１−４−５）
また、初期の物体領域に含まれる部分領域の数は１つに限るものではなく、複数でもよい。

例えば、距離値の平均が小さい順（変形例１−４−１）、特定色との合致度が高い順（変形例１−４−２）に所定の数の部分領域を物体領域として選択することも可能である。また、平均距離が小さい順（変形例１−４―３）、重複率の高い順（変形例１−４−４）に所定の数の部分領域を物体領域として選択することも可能である。

（変形例１−５）
第１実施形態では（数１０）で定義される陰多項式モデルと３次元点群との平均距離を、部分領域を追加するか否か判定するなどの用途で評価値として用いている。しかしながら、判定基準とする評価値はこれに限るものではなく、物体のモデルと計測点群との乖離の度合いを示す値であればよい。例えば、平均距離に替えて、（数１２），（数１３）で表される評価値を用いてもよい．

ただし、Ｎは評価値の算出対象とする距離画像上の領域内の３次元点の数であり、Φは当該領域内の３次元点群である。

ただし、Ｎは評価値の算出対象とする距離画像上の領域内の３次元点の数であり、Φは当該領域内の３次元点群である。これらの評価値Ｄ_RMS（数１２）、Ｄ_average（数１３）は形状モデルと３次元点群との面の位置の一致度合いを表すものである。

以上の変形例によっても、位置姿勢推定処理の対象とする距離画像データを対象物体の領域に限定することにより、処理速度を向上させ、メモリ消費量を抑制することができる。

（第２実施形態）
第１実施形態では、ある部分領域を物体領域に追加する前に、その部分領域が対象物体に相当する領域か否かを判定し、判定結果が真ならばその部分領域を以後一貫して物体領域の一部として処理する構成を説明した。本実施形態では、物体領域に追加済みのある部分領域が非対象物体に相当する部分領域であるか否か判定し、判定結果が真ならば、その部分領域を物体領域から削除する構成を説明する。

図７の参照により、第２実施形態にかかる位置姿勢計測方法の処理の流れを説明する。本処理は、ＣＰＵ１０１の全体的な制御の下に実行される。ステップＳ７０１からＳ７０４の処理は，それぞれステップＳ２０１からステップＳ２０４と同じ内容である。

ステップＳ７０５はステップＳ２０５と同様であるが、物体の位置姿勢を算出した段階で、（数１０）で表される計測点と物体モデルとの平均距離を物体領域に関して算出し、メモリ１０２に格納する。この際、前回の物体モデル当て嵌め処理において算出され、メモリ１０２に格納されていた平均距離データを上書きせずに、メモリ１０２内の別の記憶領域にコピーしておく。

ステップＳ７０６では、物体領域に含まれる部分領域を更新する。本ステップの処理においては、物体領域に部分領域が追加されるか、あるいは追加済みの部分領域が物体領域から除外される。本ステップについては後に詳述する。

ステップＳ７０７では、ステップＳ７０６において物体領域から部分領域が除外されたか否か判定する。除外されたと判定した場合（Ｓ７０７−Ｙｅｓ）、物体の位置姿勢推定処理全体を終了する。ここで、処理を終了するのは、対象物体に相当する部分領域はすべて物体領域として選択済みであり、直近に追加された部分領域が非対象物体に相当すると想定されるからである。

一方、ステップＳ７０７において、部分領域が除外されていないと判定した場合（Ｓ７０７−Ｎｏ）、処理はステップＳ７０５に戻され、物体領域の３次元点群に物体の形状モデルを当て嵌めることによって対象物体の位置姿勢を更新する。

図８の参照により、第２実施形態にかかる物体領域更新処理の流れを説明する。処理を開始すると、物体領域の３次元点群と物体モデルとの平均距離について、直近および前回の物体モデル当て嵌め工程（ステップＳ７０５）で算出された値を比較し、前回よりも平均距離が増加しているか否かの増加判定を行う。距離が増加していれば（Ｓ８０１−Ｙｅｓ）、ステップＳ８０２に処理は進められる。一方、距離が増加していないと判定された場合（Ｓ８０１−Ｎｏ）、処理はステップＳ８０３に進められる。

ステップＳ８０２では、直近に物体領域に追加した部分領域を物体領域から取り除く。すなわち、物体領域に含まれる部分領域群を表す識別番号のリストから、取り除く部分領域の識別番号を削除する。

ステップＳ８０３では、物体領域に含まれない部分領域の中から選んだ部分領域を物体領域に追加する。この処理は図５のフローチャートに示す流れと同じである。

以上に説明した処理を行うことにより、物体領域に追加すべく選択した部分領域の妥当性、すなわち部分領域が対象物体に相当する度合いを、物体モデルを当て嵌めたときの当て嵌め誤差で確かめることができる。その結果、第１実施形態のように部分領域ごとの評価値で追加する部分領域を選択する構成に比べ、誤って非対象物体に相当する部分領域を物体領域として選択する危険性が軽減され、より正確に物体領域を決定することが可能である。

なお、上記の説明では、追加済みの部分領域を削除するか否かの判定基準として（数１０）で表される平均距離を用いる例を説明しているがこの例に限定されるものではない。例えば、物体モデルの当て嵌め誤差に比例して増加する値であり、かつ物体領域のサイズに依存しないように規格化された値ならば任意の値を用いることができる。例えば、ステップＳ４０３（図４）の処理における位置姿勢更新後の誤差ベクトルの大きさを、物体領域に含まれる画素で平均化した値を基準値としてもよい。

また、上記の説明では部分領域を物体領域から削除した場合に処理全体を終了しているが、さらに物体領域の更新を継続してもよい。この場合、除外した部分領域が次回の物体領域更新工程で再び物体領域に加えられて無限ループに陥らないように、除外された部分領域は物体領域として追加する候補から除外してもよい。あるいは物体領域に追加する部分領域の候補の中で選択順位を下げる（例えば最下位に設定する）という処理を追加してもよい。

（変形例２−１）
本発明の実施形態は上記に限らず様々な変形例が存在する。第２実施形態では、部分領域を物体領域から除外するか否か判定する基準を平均距離の増減としている。しかし、判定基準はこれに限らず、部分領域を追加する前後でのモデル当て嵌めの精度の変化が評価できればよい。例えば、直近に物体領域に追加した部分領域に関する平均距離（Ａ）と、物体領域に関する平均距離（Ｂ）とを比較し、前者（Ａ）が後者（Ｂ）以上であれば直近に追加した部分領域を削除すると判定することも可能である。逆に、前者（Ａ）が後者（Ｂ）未満であれば部分領域を削除しないようにする。

（変形例２−２）
あるいは、物体領域に関する平均距離に替えて、物体領域から直近に追加した部分領域を除外した領域に関する平均距離を用いてもよい。

（変形例２−３）
さらに、平均距離に替えて、第１実施形態の変形例１−４−１〜１−４−５で説明した評価値を用いて部分領域を除外するか否か判定してもよい。

（第３実施形態）
前述の第１及び第２実施形態では、陰多項式で表現した物体の形状モデルを物体領域の３次元点群に当て嵌めることによって、物体の位置姿勢を推定している。しかしながら、位置姿勢の推定方法はこれに限るものではなく、物体の形状モデルおよび物体領域の３次元点群の情報に基づくものであれば、任意の方法を用いることが出来る．
本実施形態では、形状モデルを、三角形の平面パッチ群から構成されるパッチモデルとし、このモデルと物体領域の３次元点群との間で対応点の対を求め、対応点対間の距離の和が最小になるように物体の位置姿勢を最適化する。

本実施形態の処理全体の流れは図２のフローチャートと同様である。ただし、ステップＳ２０１で外部記憶装置１０５から入力する形状モデルデータは、三角形パッチの各頂点の識別番号と座標、各三角形パッチの識別番号とそのパッチを構成する頂点の識別番号から構成されるものとする。

また、位置姿勢の算出工程（ステップＳ４０３）では、物体領域の３次元点群および形状モデル上の点のうち、距離が最小となる点の組からなる最近傍点対を求める。求められた最近傍点対はメモリ１０２に最近傍点対のリストとして記憶される。

具体的には、まず物体領域の３次元点群から最近傍点対の候補となるサンプル点を選択する。サンプル点は、距離画像上の物体領域内の１画素ごとに選択しても良いし、所定数の画素（例えば、５画素など、複数が素）ごとに画像領域を間引きして選択しても良い。次に、サンプル点ｐ^→に対応する形状モデル上の最近傍点を求める。サンプル点ｐ^→と、３頂点r₁ ^→, r₂ ^→, r₃ ^→からなる三角形パッチｔとの間の最小距離ｄは（数１４）のように表される。

ただし、u∈[0,1], v∈ [0,1], w∈[0,1]である。形状モデルＴがＮ個の三角形パッチｔ₁,・・・ｔ_Nから構成されるとすると、サンプル点ｐ^→と形状モデルＴ上の最近傍点との間の距離は（数１５）のように表される。

すなわちサンプル点の最近傍点を求めるには、サンプル点と各三角形パッチとの最小距離および最近傍点の座標を表す係数の組（u,v,w）を算出する。そして、すべての三角形パッチの中でサンプル点との最小距離が最小となる三角形パッチと係数の組（u,v,w）を探索する。以上説明した最近傍点対の探索処理の結果は、サンプル点の３次元座標，形状モデル上の最近傍点が存在する三角形パッチの識別番号、最近傍点の座標を表す係数（u,v,w）の組を全てのサンプル点に対して羅列したリストとしてメモリ１０２に記録される。

次に、最近傍点対リストに登録された最近傍点対間の距離の総和が最小になるようにするモデルの位置姿勢を算出し、算出結果を対象物体の位置姿勢とする。距離画像計測装置の撮像部の座標系における３次元座標は、対象物体の位置姿勢ｓによって、対象物体の座標系における３次元座標（ｘ、ｙ、ｚ）に変換することができる。ここで、計測対象物体の位置及び姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。ステップＳ４０３で算出された対象物体の概略の位置姿勢により、撮像部座標系のある点が対象物体座標系の点（ｘ₀、ｙ₀、ｚ₀）に変換されるとする。（ｘ、ｙ、ｚ）は対象物体の位置および姿勢により変化するものであり、（ｘ₀、ｙ₀、ｚ₀）の近傍で１次のテイラー展開によって（数１６）のように近似できる。

メモリ１０２に格納されたある最近傍点対の形状モデル側最近傍点が存在する三角形パッチ面の対象物体座標系における方程式をax+by+cz=e(a²+ b²+ c²=1, a,b,c,eは定数)とする。正しいｓによって変換される（ｘ、ｙ、ｚ）は、平面の方程式ax+by+cz=e(a²+ b²+ c²=1)を満たすと仮定する。（数１６）を平面の方程式に代入すると（数１７）が得られる。

ただし、ｑ＝ax₀+by₀+cz₀（定数）である。

（数１７）は最近傍点対すべてについて成り立つため、（数１８）のようなΔｓ_iに関する線形連立方程式が成り立つ。

ここで、（数１８）を（数１９）のように表す。

（数１９）をもとに，行列Jの一般化行列(J^T・J) ^-1・J^Tを用いてΔｓを求める。しかし、距離画像の計測点群に含まれるノイズなどによる外れ値が多いため、次に説明するようなロバスト推定手法を用いる。一般に、外れ値である計測点群データでは誤差e−qが大きくなる。そのため、（数６）、（数７）の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。そこで、誤差e−qが大きいデータには小さな重みを与え、誤差e−qが小さいデータには大きな重みを与える。重みは、例えば（数２０）に示すようなTukeyの関数により与えることが可能である。

cは定数である。なお、重みを与える関数はTukeyの関数である必要はなく、例えば、Huberの関数など、誤差の大きいデータには小さな重みを与え、誤差の小さいデータには大きい重みを与える関数であればよい。各データに対応する重みをw_iとする。ここで（数２１）のように重み行列Wを定義する。

重み行列Ｗは、対角成分以外はすべて「０」の正方行列であり、対角成分には重みw_iが入る。Ｎ_cは最近傍点対の数（欠損モデル領域上の対応点対を含まない）である。この重み行列Wを用いて、（数１９）を（数２２）のように変形する。

（数２３）のように（数２２）を解くことにより補正値Δsを求める。

このようにして算出された補正値Δsによりモデルの位置・姿勢の概略値を補正し、結果をメモリ１０２に格納するか、あるいはデータ入出力部１０９を介して外部の装置に出力する。なお、上述した位置及び姿勢の最適化計算手法はGauss-Newton法に基づくものであるが、これに限らずLevenberg-Marquardt法や最急降下法などを用いてもよい。

ステップＳ４０４では、収束判定を行い、収束していれば（Ｓ４０４−Ｙｅｓ）、位置姿勢の推定処理を終了する。収束していなければ（Ｓ４０４−Ｎｏ）、ステップＳ４０２に処理は戻され、同様の処理が繰り返される。収束判定では、補正値Δｓがほぼ０である場合や、誤差ベクトルＥの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。

物体領域の更新処理は第１実施形態と同様であるが、部分領域を追加するか否かを判定するための評価値は（数１０）に表される平均距離に限定されるものではない。例えば、RMS誤差（誤差ベクトルの要素の二乗和をサンプル点数で除算した値の平方根）を用いることが可能である。物体領域に関する誤差ベクトルはステップＳ４０３で算出済みである。また、物体領域に追加する候補の部分領域に関する誤差ベクトルは、上述の物体領域の場合と同様に算出することが可能である。

（その他の実施例）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。

Claims

撮像手段により撮像された対象物体の距離画像を取得する取得手段と、
取得された前記距離画像において、物体領域を設定する設定手段と、
設定された前記物体領域の３次元点群に、前記対象物体をモデル化した物体モデルの表面を当て嵌めることによって、前記対象物体の位置及び姿勢を推定する推定手段と、
前記推定手段によって推定された前記位置及び姿勢に基づいて、前記物体モデルと前記３次元点群との距離を示す情報を導出し、該導出された距離を示す情報を用いて、前記物体領域の設定を更新する更新手段と、
前記更新手段による前記物体領域の設定の更新に従い、前記推定手段に、当該更新後の物体領域の３次元点群を用いて、前記対象物体の位置及び姿勢を推定させる制御手段と
を備えることを特徴とする位置姿勢計測装置。
前記更新手段は、前記推定手段によって推定された前記位置及び姿勢に基づいて、前記物体モデルと前記３次元点群との距離を示す情報を導出し、該導出された距離を示す情報を用いて、前記距離画像の中で前記物体領域として設定されていない領域のうち一部の領域を前記物体領域に追加することにより、前記物体領域を更新することを特徴とする請求項１に記載の位置姿勢計測装置。
前記更新手段は、前記設定手段により前記物体領域として設定されていない領域と前記物体モデルとの距離を示す情報が、前記物体モデルと前記物体領域との距離を示す情報より小さい場合、前記物体領域に、距離値の小さい順に新たな領域を追加することにより、前記物体領域を更新することを特徴とする請求項１または２に記載の位置姿勢計測装置。
前記更新手段は、前記領域を追加する前の物体領域と前記物体モデルとの距離を示す情報に対して、前記領域を追加した後の物体領域と前記物体モデルとの距離を示す情報が増加するか否かを判定する増加判定手段と、
前記増加判定手段により増加しないと判定される場合に、前記物体領域に対して距離値の小さい順に新たな領域を追加する追加手段と、
を有することを特徴とする請求項２または３に記載の位置姿勢計測装置。
前記更新手段は、前記物体領域に追加する領域を決定する決定手段を更に有することを特徴とする請求項４に記載の位置姿勢計測装置。
前記決定手段は、前記物体領域として設定されていない領域と前記物体モデルとの距離の近接度を示す距離値の小さい順に、前記領域を決定することを特徴とする請求項５に記載の位置姿勢計測装置。
前記決定手段は、前記推定手段によって推定された位置姿勢に基づいて、前記物体モデルを距離画像上に投影した場合に、前記投影された物体モデルと領域とが重複する度合いの高い順に、前記領域を決定することを特徴とする請求項５に記載の位置姿勢計測装置。
前記更新手段は、前記推定手段の推定の結果を用いて、前記領域が追加された前記物体領域から、前記追加された領域を削除することにより前記物体領域を更新することを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢計測装置。
前記更新手段は、前記領域を追加する前の物体領域と前記物体モデルとの距離を示す情報に対して、前記領域を追加した後の物体領域と前記物体モデルとの距離を示す情報が増加すると前記増加判定手段により判定される場合、前記追加された前記領域を前記物体領域から削除することを特徴とする請求項４に記載の位置姿勢計測装置。
前記推定手段は、前記物体領域の３次元点群と前記物体モデルの表面との距離の総和を最小にするように、前記物体モデルを前記物体領域の前記３次元点群に当て嵌めることによって、前記対象物体の位置及び姿勢を推定することを特徴とする請求項１乃至９のいずれか１項に記載の位置姿勢計測装置。
位置姿勢計測装置の位置姿勢計測方法であって、
取得手段が、撮像手段により撮像された対象物体の距離画像を取得する取得工程と、
設定手段が、取得された前記距離画像において、物体領域を設定する設定工程と、
推定手段が、設定された前記物体領域の３次元点群に、前記対象物体をモデル化した物体モデルの表面を当て嵌めることによって、前記対象物体の位置及び姿勢を推定する推定工程と、
更新手段が、前記推定工程によって推定された前記位置及び姿勢に基づいて、前記物体モデルと前記３次元点群との距離を示す情報を導出し、該導出された距離を示す情報を用いて、前記物体領域の設定を更新する更新工程と、を有し、
前記推定手段が、前記更新工程による前記物体領域の設定の更新に従い、当該更新後の物体領域の３次元点群を用いて、前記対象物体の位置及び姿勢を推定する第２の推定工程と、
を有することを特徴とする位置姿勢計測方法。
コンピュータを請求項１乃至１０のいずれか１項に記載の位置姿勢計測装置の各手段として機能させるためのプログラム。