(57)【要約】【課題】  距離画像の計測値の欠損に起因した精度低下を軽減することが可能な位置姿勢の計測技術を提供すること。【解決手段】  対象物体の位置姿勢を計測する位置姿勢計測装置は、対象物体の距離画像上の画素に対して距離が最小となる形状モデル上の点が、第１の探索部により得られた欠損モデル領域内に存在するか否かを判定する判定部と、形状モデル上の点が欠損モデル領域内に存在すると判定された場合に、第２の探索部により得られた、距離が最小となる距離画像上の画素と形状モデル上の点との組み合わせに補正を施す補正部と、補正が施された、距離画像上の画素と形状モデル上の点との組み合わせにおいて、距離画像上の画素と形状モデル上の点との間の距離の総和を最小にする形状モデルの位置及び姿勢を、対象物体の位置及び姿勢の計測値として算出する算出部と、を備える。【選択図】  図４
2011179907.tif 000002
【特許請求の範囲】
【請求項１】
撮像手段により撮像された対象物体の距離画像を取得する取得手段と、
取得された前記距離画像のうち、前記撮像手段により距離値が計測されていない欠損画像領域を検出する検出手段と、
前記撮像手段の位置姿勢を基準として、検出された前記欠損画像領域を、前記対象物体の既知の３次元形状を示す形状モデル上に投影し、当該投影された領域を欠損モデル領域として探索する第１の探索手段と、
距離画像上の画素と前記形状モデル上の点との距離が最小となる組み合わせを、前記距離画像上の画素の位置を示す情報と前記形状モデル上の点の位置を示す情報と、に基づき探索する第２の探索手段と、
前記距離画像上の画素に対して距離が最小となる前記形状モデル上の点が、前記欠損モデル領域内に存在するか否かを判定する判定手段と、
前記形状モデル上の点が前記欠損モデル領域内に存在すると前記判定手段により判定された場合に、前記第２の探索手段により得られた、距離が最小となる前記距離画像上の画素と前記形状モデル上の点との組み合わせに補正を施す補正手段と、
前記補正手段により補正が施された、前記距離画像上の画素と前記形状モデル上の点との組み合わせにおいて、当該距離画像上の画素と前記形状モデル上の点との間の距離の総和を最小にする前記形状モデルの位置及び姿勢を、前記対象物体の位置及び姿勢の計測値として算出する算出手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項２】
前記補正手段は、前記形状モデル上の点が前記欠損モデル領域内に存在すると前記判定手段により判定された場合、前記第２の探索手段により得られた組み合わせのうち、前記欠損モデル領域内に存在すると判定された前記形状モデル上の点と前記距離画像上の画素との組み合わせを削除することを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項３】
前記補正手段は、前記形状モデル上の点が前記欠損モデル領域内に存在すると前記判定手段により判定された場合、前記第２の探索手段により得られた組み合わせのうち、前記欠損モデル領域内に存在すると判定された前記形状モデル上の点と前記距離画像上の画素との組み合わせにおける距離が、前記欠損モデル領域内には存在しないと判定された前記形状モデル上の点と前記距離画像上の画素との組み合わせにおける距離よりも小さな重みとなるように重み付けする補正を行うことを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項４】
前記補正手段は、前記形状モデル上の点が前記欠損モデル領域内に存在しないと前記判定手段により判定された場合、前記欠損モデル領域内に存在すると判定された前記形状モデル上の点と前記距離画像上の画素との組み合わせにおける距離に対して第１の重みを設定し、前記欠損モデル領域内に存在しないと判定された前記形状モデル上の点と前記距離画像上の画素との組み合わせにおける距離に対して、前記第１の重みよりも大きい値の第２の重みを設定することを特徴とする請求項３に記載の位置姿勢計測装置。
【請求項５】
前記補正手段は、
前記重み付けにより補正された前記距離画像上の画素と前記形状モデル上の点との組み合わせに対して、前記算出手段により算出された前記距離の総和が予め定められた閾値未満になったか否かを判定し、
当該閾値未満になった後に、前記補正手段は、前記第２の探索手段により得られた組み合わせのうち、前記欠損モデル領域内に存在すると判定された前記形状モデル上の点と前記距離画像上の画素との組み合わせを削除することを特徴とする請求項４に記載の位置姿勢計測装置。
【請求項６】
取得手段が、撮像手段により撮像された対象物体の距離画像を取得する取得工程と、
検出手段が、取得された前記距離画像のうち、前記撮像手段により距離値が計測されていない欠損画像領域を検出する検出工程と、
第１の探索手段が、前記撮像手段の位置姿勢を基準として、検出された前記欠損画像領域を、前記対象物体の既知の３次元形状を示す形状モデル上に投影し、当該投影された領域を欠損モデル領域として探索する第１の探索工程と、
第２の探索手段が、距離画像上の画素と前記形状モデル上の点との距離が最小となる組み合わせを、前記距離画像上の画素の位置を示す情報と前記形状モデル上の点の位置を示す情報と、に基づき探索する第２の探索工程と、
前記距離画像上の画素に対して距離が最小となる前記形状モデル上の点が、前記欠損モデル領域内に存在するか否かを、判定手段が判定する判定工程と、
補正手段が、前記形状モデル上の点が前記欠損モデル領域内に存在すると前記判定工程で判定された場合に、前記第２の探索工程で得られた、距離が最小となる前記距離画像上の画素と前記形状モデル上の点との組み合わせに補正を施す補正工程と、
算出手段が、前記補正工程で補正が施された、前記距離画像上の画素と前記形状モデル上の点との組み合わせにおいて、当該距離画像上の画素と前記形状モデル上の点との間の距離の総和を最小にする前記形状モデルの位置及び姿勢を、前記対象物体の位置及び姿勢の計測値として算出する算出工程と、
を有することを特徴とする位置姿勢計測方法。
【請求項７】
コンピュータを請求項１乃至５のいずれか１項に記載の位置姿勢計測装置の各手段として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元形状が既知である対象物体の位置及び姿勢を計測する技術に関する。特に対象物体を撮影した距離画像に基づいて対象物体の位置姿勢を計測する位置姿勢計測装置、位置姿勢計測方法およびプログラムに関する。
【背景技術】
【０００２】
近年、ロボット技術の発展とともに、これまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。複雑なタスクの代表的な例として、工業製品の組み立てが挙げられる。ロボットが組み立て作業を行うためには、ロボットハンドなどのエンドエフェクタによって部品を把持したり、他の部品と嵌合させたりすることが必要となる。そのためには、把持の対象となる部品あるいは嵌合の対象となる部品とロボットとの相対的な位置及び姿勢を計測し、計測結果に基づいてロボットハンドの移動計画を策定して、実際にロボットハンドを駆動するためのアクチュエータを制御する必要がある。
【０００３】
このような位置姿勢計測は従来より，ロボットに搭載されたカメラや距離センサを用いて行われており、２次元画像や距離画像を用いる方法が代表的である。特に、距離画像を用いる方法は２次元画像を用いる方法に比べ、撮影方向の位置情報が直接得られる，対象物体にテクスチャが乏しい場合も形状情報が得られる，周囲の光源環境に影響を受けにくいなどの利点がある。このため、距離画像を用いる方法は、工場における製品組み立てなど実用的な用途において重要性を増している。
【０００４】
距離画像から撮影したシーン中の物体の位置及び姿勢を推定する方法としては、距離画像に物体の形状モデルを当て嵌める方法が主流であり、従来より多くの研究がなされている。例えば、非特許文献１では、距離画像を３次元点群データに変換し，点群データに３次元モデルを当て嵌めることにより物体の位置及び姿勢を計測する手法（モデルフィッティング法）が開示されている。すなわち、位置及び姿勢の概略値をもとに，点群データの各点について３次元モデルの近傍の面を探索し，点と面との距離の和を最小化するように位置及び姿勢の最適化を繰り返すことにより位置及び姿勢を算出する。
【０００５】
上述のようなモデルフィッティング法では、計測データとモデルとの対応を探索し、対応対間の差異が最小化される。しかし、計測データのノイズ・初期の位置姿勢推定値の誤差などの要因により、明らかに誤った対応対が検出されてしまうことがある。このような明らかな誤り（「外れ値」）に小さな重みを設定して、外れ値が推定結果に悪影響を及ぼすことを防ぐロバスト推定と呼ばれる手法群が利用されている。ロバスト推定の代表的な手法としては、Ｍ推定が挙げられる．
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】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.
【非特許文献２】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.
【非特許文献３】稲垣 宏，杉原 厚吉，制約つき３次元Delaunay図の構成算法，情報処理学会研究報告. AL, アルゴリズム研究会報告, 94(26), pp.41-48, 1994.
【非特許文献４】Y. Chen, G. Medioni, Object modeling by registration of multiple range images, Proceedings of IEEE International Conference on Robotics and Automation, vol.3, pp.2724-2729, 1991.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
距離画像を撮影する撮影装置の測定原理によっては、撮影された距離画像にデータの欠損が発生することがある。例えば，光切断法では照明装置で対象物体に投射した光を、投射方向とは別の方向からカメラで観察する。このとき、光が投射された領域の一部で対象物体自身に隠されてカメラからは観察できない場合がある。このような場合、カメラから観察できない物体上の領域については、距離画像のデータが欠損する。図１は距離画像に欠損が生じている例を示したものである。図１（ａ）は対象物体を撮影した距離画像を示し、図１（ｂ）は、対象物体の表面から見て凹になった部分など、奥行き値が計測できないためにデータ（距離値）が欠損している領域を示している。
【０００８】
このようにデータの欠損が生じた距離画像に対して、対象物体の形状モデルを当て嵌めようとすると，距離画像と形状モデルとの対応付けを誤る場合がある。図２の参照により、対応付けの誤りを概念的に説明する。断面形状２０１は形状モデルを紙面に平行な平面で切断して形成されたものである。断面形状２０２は距離画像の画素に対応する３次元空間中の点群が構成する面の断面形状である。また、空間領域２０３はカメラからの隠れによって距離値が計測できない空間領域を表している。空間領域２０３に含まれる３次元点群（断面形状２０２の中で破線になっている部分）に関しては、距離画像の奥行きデータが欠損する。いま、測定点群のうち、例えば、点Ａの対応点として、点Ａとの距離が最小となるモデル上の点を探索することを考える。点Ａは断面形状の凹部底面上の点なので、点Bなどモデルの底面上の点と対応付けられるのが望ましい。しかしながら、この場合、距離が最短となる点Aの対応点はモデル上の点Ｃとなり、対応付けの誤りが生じる。形状モデルと測定点群の間で誤った対応付けがされると、誤った対応対間の距離を最小化しようとする影響が位置姿勢推定計算に現れるため、位置姿勢推定の精度が低下する。
【０００９】
前述のロバスト推定を用いても、偽対応対の悪影響を軽減できるとは限らない。なぜなら、計測欠損に起因した、誤った対応対間の距離は、正しい対応対間の距離と比較して十分大きいとは限らず、位置姿勢推定計算の寄与度が正しい対応点対と同程度となるからである。
【課題を解決するための手段】
【００１０】
本発明は、上記の課題を鑑み、距離画像中の被写体の位置姿勢を算出する際、距離画像の計測値の欠損に起因した精度低下を軽減することが可能な位置姿勢計測技術の提供を目的とする。
【００１１】
本発明の一つの側面にかかる位置姿勢計測装置は、撮像手段により撮像された対象物体の距離画像を取得する取得手段と、
取得された前記距離画像のうち、前記撮像手段により距離値が計測されていない欠損画像領域を検出する検出手段と、
前記撮像手段の位置姿勢を基準として、検出された前記欠損画像領域を、前記対象物体の既知の３次元形状を示す形状モデル上に投影し、当該投影された領域を欠損モデル領域として探索する第１の探索手段と、
距離画像上の画素と前記形状モデル上の点との距離が最小となる組み合わせを、前記距離画像上の画素の位置を示す情報と前記形状モデル上の点の位置を示す情報と、に基づき探索する第２の探索手段と、
前記距離画像上の画素に対して距離が最小となる前記形状モデル上の点が、前記欠損モデル領域内に存在するか否かを判定する判定手段と、
前記形状モデル上の点が前記欠損モデル領域内に存在すると前記判定手段により判定された場合に、前記第２の探索手段により得られた、距離が最小となる前記距離画像上の画素と前記形状モデル上の点との組み合わせに補正を施す補正手段と、
前記補正手段により補正が施された、前記距離画像上の画素と前記形状モデル上の点との組み合わせにおいて、当該距離画像上の画素と前記形状モデル上の点との間の距離の総和を最小にする前記形状モデルの位置及び姿勢を、前記対象物体の位置及び姿勢の計測値として算出する算出手段と、
を備えることを特徴とする。
【発明の効果】
【００１２】
本発明によれば、距離画像中の被写体の位置姿勢を算出する際、距離画像の計測値の欠損に起因した精度低下を軽減することが可能な位置姿勢計測技術の提供が可能になる。
【図面の簡単な説明】
【００１３】
【図１】距離画像の測定値が欠損する例を示す図。
【図２】距離画像のデータ欠損による形状モデルとの対応付けの誤りを例示的に説明する図。
【図３】本発明の実施形態にかかる位置姿勢計測装置のハードウェア構成を例示的に説明する図。
【図４】第１実施形態にかかる位置姿勢計測方法の処理の流れを説明する図。
【図５】欠損モデル領域探索工程の処理内容を説明する図。
【図６】（ａ）第１実施形態のモデル当て嵌め工程の処理の流れを説明する図、（ｂ）第２実施形態のモデル当て嵌め工程の処理の流れを説明する図。
【発明を実施するための形態】
【００１４】
（第１実施形態）
本実施形態における位置姿勢計測方法は、距離画像上でデータ（距離値）が欠損している領域（以下「欠損画像領域」と呼ぶ）に相当する形状モデル上の領域（以下「欠損モデル領域」）を抽出する。そして、距離画像に形状モデルを当て嵌める際に、欠損モデル領域を当て嵌め、処理の対象外とする。なお、本実施形態で用いる形状モデルは、物体の表面を三角形平面の集合として表す三角形パッチモデルとする。
【００１５】
距離画像中で奥行きデータが欠損している画素は画素値が「０」に設定されているとする。距離画像を計測する計測装置（以下「距離画像計測装置」）の特性によっては欠損部分の画素値は「０」とは限らないが、通常の奥行き値ではないことを示す特殊な一定値として「０」が設定される。したがって、距離画像中の画素値が特殊な一定値と等しければ、その画素は計測データが欠損しており、等しくなければ計測データは欠損していないと判定できる。
【００１６】
図３の参照により、本発明の実施形態にかかる位置姿勢計測装置のハードウェア構成を説明する。ＣＰＵ３０１は、装置全体の動作を制御する。メモリ３０２は、ＣＰＵ３０１の動作に用いるプログラムやデータを格納する。バス３０３は、各構成モジュール間のデータ転送を司る。バス３０３と各種装置とはインタフェース３０４に接続されている。外部記憶装置３０５はＣＰＵ３０１に読み込むプログラムやデータを格納する。キーボード３０６およびマウス３０７はプログラムを起動したり、プログラムの動作を指定したりするための入力装置として機能する。表示部３０８はプロセスの動作結果を表示するための表示部である。データ入出力部３０９は外部装置との間でデータの送受信を行なうことが可能である。距離画像計測装置はデータ入出力部３０９を介して位置姿勢計測装置に接続されている。
【００１７】
図４の参照により第１実施形態にかかる位置姿勢計測方法の処理の流れを説明する。本処理は、ＣＰＵ３０１の全体的な制御の下に実行される。ステップＳ４０１において、処理に用いるデータの初期設定を行う。具体的には、外部記憶装置３０５から形状モデルデータをメモリ３０２にロードする。形状モデルデータは、三角形パッチの各頂点の識別番号と座標、各三角形パッチの識別番号とそのパッチを構成する頂点の識別番号から構成される。また、距離画像計測装置の撮像部のカメラパラメータ（主点位置、焦点距離）をデータ入出力部３０９に接続された不図示の距離画像計測装置から入力する。カメラパラメータは、例えば、非特許文献２で開示される方法により事前に校正しておくことが好ましい。
【００１８】
ステップＳ４０２では、データ入出力部３０９に接続された不図示の距離画像計測装置によって計測された距離画像データ（以下、単に「距離画像」ともいう）を入力し、メモリ３０２に格納する。なお、距離画像を撮影する手段は任意のものを用いることができるが、例えば、光切断法やステレオ法により撮影することが可能である。
【００１９】
ステップＳ４０３では、距離画像上で奥行き値が欠損している領域、すなわち欠損画像領域を検出する。具体的には、各画素の画素値が「０」であるか否か判定する。画素値が「０」であれば、奥行き値が欠損していることになるので、その画素の座標を、メモリ３０２に確保した欠損画像領域のデータに加える。
【００２０】
次のステップＳ４０４では、ステップＳ４０３で検出された欠損画像領域に相当する、形状モデル上の領域である欠損モデル領域を探索する（第１の探索工程）。本ステップの詳細は後述する。最後に、ステップＳ４０５では、ステップＳ４０２で取得された距離画像に形状モデルを当て嵌め、対象物体の位置姿勢を推定する。本ステップの詳細についても後述する。
【００２１】
（欠損モデル領域探索工程の処理）
欠損モデル領域探索工程（ステップＳ４０４）の詳細を図５の参照により説明する。図５において、視点位置５０１は距離画像計測装置が配置されている位置姿勢を基準とした位置を示し、画像面５０２は距離画像上の平面を示し、欠損画像領域５０３は画像面５０２における欠損画像領域を示している。欠損モデル領域探索工程では、図５のように視点位置５０１から欠損画像領域５０３を対象物体の既知の３次元形状を示す形状モデル５０４に投影する。そして、最初に形状モデル５０４と交差した領域（形状モデル５０４に投影された領域）を欠損モデル領域５０５として抽出する。なお、形状モデル５０４と視点位置５０１との相対的な位置姿勢は、距離画像計測装置の撮像部のカメラと対象物体との相対的な位置姿勢と概略一致するものとする。
【００２２】
欠損モデル領域探索工程の最初のステップとして、撮像部のカメラと対象物体との相対的位置姿勢を概略的に算出する。この算出方法は任意であるが、例えば、特開平０１-００９３０７号公報の方法を用いることができる。
【００２３】
欠損モデル領域５０５を抽出する具体的な処理としては、欠損画像領域５０３の各画素に相当する距離画像（画像面５０２）上の四辺形領域を、形状モデル５０４に含まれる各三角形パッチに投影した領域を求める。いま、１つの三角形パッチの３頂点の座標を、r1→, r2→, r3→とすると、距離画像（画像面５０２）上の１画素に相当する四辺形領域を三角形パッチに投影した領域は頂点数が３から６の多角形となり、その頂点座標は（数１）のように表される。
【００２４】
【数１】
2011179907.tif 000003
【００２５】
ここで、iは３から６の整数であり、uk+vk+wk＝１、uk∈[0,1], vk∈ [0,1], wk∈[0,1], (k=1,2,...i)である。抽出された欠損モデル領域５０５は、三角形パッチの識別番号と三角形パッチに投影された画素領域の頂点を（数１）の形式で表した係数の組（uk,vk）(k=1,2,...i)としてメモリ３０２に格納する。１つの三角形パッチに複数の欠損画素が投影される可能性があるため、各三角形パッチに対して登録される領域は単数または複数となる。
【００２６】
（モデル当て嵌め工程の詳細）
モデル当て嵌め工程（ステップＳ４０５）の詳細を図６（ａ）の参照により説明する。ステップＳ６０１で距離画像上の画素および形状モデル上の点との距離が最近傍（最小）となる組み合わせ（最近傍点対）を求める。距離画像上の画素と形状モデル上の点との距離が最小となる組み合わせを、距離画像上の画素の位置を示す情報と形状モデル上の点の位置を示す情報と、に基づき探索する。求められた最近傍点対はメモリ３０２に最近傍点対のリストとして記憶される。具体的には、まず距離画像上の画素から最近傍点対の候補となるサンプル画素を選択する。サンプル画素は距離画像上の１画素ごとに選択しても良いし、所定数の画素（例えば、５画素など、複数画素）ごとに画像領域を間引きして選択しても良い。ただし、計測欠損画素はサンプル画素として選択しない。
【００２７】
次に、サンプル画素に相当する３次元空間中のサンプル点ｐ→の位置座標を対象物体の座標系で算出する。この処理は、ステップＳ４０１で読み込んだ距離画像計測装置の撮像部のカメラパラメータに基づいてサンプル画素の２次元座標および画素値から撮像部の座標系における３次元座標を算出する。そして、ステップＳ４０３で算出された撮像部と対象物体との概略の相対的位置姿勢に基づいて撮像部の座標系から対象物体の座標系に変換することによって行う。
【００２８】
続いて、サンプル点ｐ→に対応する形状モデル上の最近傍点を求める。サンプル点ｐ→と３頂点、r1→, r2→, r3→からなる三角形パッチｔとの間の最小距離ｄは（数２）のように表される。
【００２９】
【数２】
2011179907.tif 000004
【００３０】
ただし、u∈[0,1], v∈ [0,1], w∈[0,1]である。形状モデルＴがＮ個の三角形パッチｔ1,・・・ｔNから構成されるとすると、サンプル点ｐ→と形状モデルＴ上の最近傍点との間の距離は（数３）のように表される。
【００３１】
【数３】
2011179907.tif 000005
【００３２】
すなわちサンプル点の最近傍点を求めるには、サンプル点と各三角形パッチとの最小距離および最近傍点の座標を表す係数の組（u,v,w）を算出する。そして、すべての三角形パッチの中でサンプル点との最小距離が最小となる三角形パッチと係数の組（u,v,w）を探索する。
【００３３】
以上が、最近傍点対の探索工程（ステップＳ６０１：第２の探索工程）の処理内容である。処理結果は、サンプル点の３次元座標、形状モデル上の最近傍点が存在する三角形パッチの識別番号、最近傍点の座標を表す係数（u,v,w）の組を全てのサンプル点に対して羅列したリストとしてメモリ３０２に記録される。
【００３４】
次にステップＳ６０２にて、各最近傍点対に関して、形状モデル側の最近傍点が欠損モデル領域内に存在するか否かを判定する。具体的には、まずステップＳ４０４で求められた欠損モデル領域データから三角形パッチの識別番号が、形状モデル側最近傍点が存在する三角形パッチの識別番号に一致するデータを探索する。次に、その三角形パッチに登録されたuvw空間中の単数または複数の多角形領域内に最近傍点を表すuvw座標が含まれるか否かを判定する。
【００３５】
ステップＳ６０３において、ステップＳ６０２の判定結果が「真」である場合は、ステップＳ６０４へ処理は進められ、ステップＳ６０２の判定結果が「偽」である場合はステップＳ６０５へ処理は進められる。
【００３６】
ステップＳ６０４では、メモリ３０２に格納されている最近傍点対リストを補正する。すなわち、形状モデル側の最近傍点が欠損モデル領域内に存在する場合、該当する最近傍点対のデータを削除することによりメモリ３０２に格納されている最近傍点対リストを補正する。
【００３７】
一方、ステップＳ６０５では、全ての最近傍点対に関してステップＳ６０２からＳ６０４の処理を施したか否か判定する。処理済と判定されればステップＳ６０６へ進み、未処理の対応点対が残っていると判定されればステップＳ６０２へ戻り、同様の処理を繰り返す。
【００３８】
ステップＳ６０６では、最近傍点対リストに登録された最近傍点対間の距離の総和が最小になるようにするモデルの位置姿勢を算出し、算出結果を対象物体の位置姿勢とする。距離画像計測装置の撮像部の座標系における３次元座標は、対象物体の位置姿勢ｓによって、対象物体の座標系における３次元座標（ｘ、ｙ、ｚ）に変換することができる。ここで、計測対象物体の位置及び姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。ステップＳ４０３で算出された対象物体の概略の位置姿勢により、撮像部座標系のある点が対象物体座標系の点（ｘ0、ｙ0、ｚ0）に変換されるとする。（ｘ、ｙ、ｚ）は対象物体の位置および姿勢により変化するものであり、（ｘ0、ｙ0、ｚ0）の近傍で１次のテイラー展開によって（数４）のように近似できる。
【００３９】
【数４】
2011179907.tif 000006
【００４０】
メモリ３０２に格納されたある最近傍点対の形状モデル側最近傍点が存在する三角形パッチ面の対象物体座標系における方程式をax+by+cz=e(a2+ b2+ c2=1, a,b,c,eは定数)とする。正しいｓによって変換される（ｘ、ｙ、ｚ）は、平面の方程式ax+by+cz=e(a2+ b2+ c2=1)を満たすと仮定する。（数４）を平面の方程式に代入すると（数５）が得られる。
【００４１】
【数５】
2011179907.tif 000007
【００４２】
ただし、ｑ＝ax0+by0+cz0（定数）である。
【００４３】
（数５）は最近傍点対すべてについて成り立つため、（数６）のようなΔｓiに関する線形連立方程式が成り立つ。
【００４４】
【数６】
2011179907.tif 000008
【００４５】
ここで、（数６）を（数７）のように表す。
【００４６】
【数７】
2011179907.tif 000009
【００４７】
（数７）をもとに、行列Jの一般化逆行列(JT・J) -1・JTを用いてΔｓを求める。しかし、距離画像の計測点群に含まれるノイズなどによる外れ値が多いため、次に説明するようなロバスト推定手法を用いる。一般に、外れ値である計測点群データでは誤差e−qが大きくなる。そのため、（数６）、（数７）の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。そこで、誤差e−qが大きいデータには小さな重みを与え、誤差e−qが小さいデータには大きな重みを与える。重みは、例えば（数８）に示すようなTukeyの関数により与えることが可能である。
【００４８】
【数８】
2011179907.tif 000010
【００４９】
cは定数である。なお、重みを与える関数はTukeyの関数である必要はなく、例えば、Huberの関数など、誤差の大きいデータには小さな重みを与え、誤差の小さいデータには大きい重みを与える関数であればよい。各データに対応する重みをwiとする。ここで（数９）のように重み行列Wを定義する。
【００５０】
【数９】
2011179907.tif 000011
【００５１】
重み行列Ｗは、対角成分以外はすべて「０」の正方行列であり、対角成分には重みwiが入る。Ｎcは最近傍点対の数（欠損モデル領域上の対応点対を含まない）である。この重み行列Wを用いて、（数７）を（数１０）のように変形する。
【００５２】
【数１０】
2011179907.tif 000012
【００５３】
（数１１）のように（数１０）を解くことにより補正値Δsを求める。
【００５４】
【数１１】
2011179907.tif 000013
【００５５】
このようにして算出された補正値Δsによりモデルの位置・姿勢の概略値を補正し、結果をメモリ３０２に格納するか、あるいはデータ入出力部３０９を介して外部の装置に出力する。なお、上述した位置及び姿勢の最適化計算手法はGauss-Newton法に基づくものであるが、これに限らずLevenberg-Marquardt法や最急降下法などを用いてもよい．
以上が本実施形態にかかる位置姿勢計測装置の処理の流れである。本実施形態に拠れば、距離画像上で奥行き値が欠損している領域を、対象物体の位置姿勢を推定する処理から除外することができる。その結果、対象物体の位置姿勢を求める処理に不正なデータを用いる危険性が軽減され、位置姿勢の誤差を少なくすることができる。
【００５６】
特に、距離画像上の計測欠損領域は画素値が所定の値か否か検査するという単純な判定処理で確実に検出できる。一方、上述したロバスト推定手法は誤差の大きさのみで外れ値の寄与度を調整する。本実施形態においてもロバスト推定を利用している。ただし、ロバスト推定を行う前に距離画像の欠損情報を積極的に利用し、形状モデル側において、計算に使用しない領域を設ける処理を行っている。計測欠損に起因する誤差はその他の原因による誤差と比べて必ずしも値が大きいとは限らない。したがって、ロバスト推定手法のみで計測欠損データの寄与度を低下させることができるとは限らない。すなわち、本発明の手法はロバスト推定手法のみを利用する場合に比べて、より確実に外れ値を除外して、計算誤差を高めることが可能になる。
【００５７】
（変形例１−１）
本発明の実施形態は上記の説明に限らず様々な変形が可能である。例えば、上記の説明では、欠損モデル領域探索（Ｓ４０４）と、モデル当て嵌め工程（Ｓ４０５）と、における対象物体の位置姿勢計算を１回ずつ行っている。しかし、本発明の趣旨はこの例に限定されるものではなく、これらの処理を繰り返し行うことで、漸近的に位置及び姿勢の推定精度を高めることも可能である。この場合、ステップＳ４０５の終了後に、繰り返し回数が所定回数に達したか否か判定し、所定回数に達していれば処理全体を終了し、所定回数に達していなければ、ステップＳ４０４に処理を戻し、同様の処理を繰り返すようにしてもよい。
【００５８】
あるいは、ステップＳ４０５の終了後にモデルの当て嵌め誤差を算出して、所定の閾値との大小関係を比較し、誤差が閾値未満であれば処理全体を終了する。そして、閾値以上であればステップＳ４０４に処理を戻し、同様の処理を繰り返すようにしてもよい。ここで、モデルの当て嵌め誤差は、例えば、最近傍点対間の距離をすべての最近傍点対について加算した値とする。この方法をとると、位置姿勢の推定精度が高まるにつれ、欠損モデル領域の推定精度も高くなるので、計測欠損の影響を軽減することができる。
【００５９】
（変形例１−２）
上記の説明では欠損モデル領域上にある最近傍点を除外して位置姿勢の推定処理を行っている。しかし、欠損モデル領域を処理の対象外にする方法はこれに限るものではない。例えば、欠損モデル領域探索工程（ステップＳ４０４）で欠損領域に含まれると判断された三角形パッチ上の領域を、モデル当て嵌め工程（ステップＳ４０５）において、最近傍点対の探索処理の対象外としても良い。
【００６０】
あるいは、欠損モデル領域内の最近傍点対を処理の対象外とするのではなく、形状モデルから欠損モデル領域を除外したモデルを生成して、モデル当て嵌め工程（ステップＳ４０５）の処理対象データとして用いても良い。ただし、この場合は、最近傍点が欠損モデル領域に含まれるか判定して、最近傍点対を除外する処理（ステップＳ６０２からＳ６０５）は必要ない。形状モデルから欠損モデル領域が除去されているので、最近傍点が欠損モデル領域内に存在する可能性は無いためである。
【００６１】
形状モデルから欠損モデル領域を除外する方法としては、例えば、欠損画像領域の境界部分に相当する形状モデル上のエッジを、新たな平面パッチの辺として、パッチモデルの表面再分割を行えばよい。パッチモデルの再分割は、元の平面パッチ面上に投影された欠損モデル領域の頂点や欠損モデル領域の境界と平面パッチの辺との交点を新たなパッチの頂点として追加する。そして、元の平面パッチの辺および欠損モデル領域の辺を再分割後のパッチの辺とする制約のもと、非特許文献３に記載の方法に従ってドロネー分割によってパッチを生成することによって実現できる。この変形例の方法を用いれば、最近傍点対の数を減らすことなく対象物体の位置姿勢を推定することができるので、より精度高く位置姿勢を算出することができる。
【００６２】
（変形例１−３）
ステップＳ４０４において、各三角形パッチ上の欠損モデル領域は三角形パッチの２辺を基準とする座標系での多角形として検出している。しかし、欠損モデル領域の表現方法はこれに限るものではない。例えば、コンピュータグラフィクスで頻繁に用いられるモデルデータのように各三角形パッチの頂点に２次元のテクスチャ座標を予め割り当てておき、テクスチャ座標系で欠損モデル領域を表しても良い。さらに、三角形パッチにテクスチャ座標だけではなくテクスチャ画像も割り当てておき、欠損モデル領域データを画像データとして蓄積しても良い。具体的には、例えば、欠損モデル領域に相当するテクスチャ画像上の画素の画素値を「０」、それ以外の領域の画素値を「１」に設定する。モデル当て嵌め工程（ステップＳ４０５）では、最近傍点対の形状モデル側の点に対応するテクスチャ画像の画素値を参照して、画素値＝０ならば該当する最近傍点対を対象物体の位置姿勢計算から除外する。
【００６３】
（変形例１−４）
上記の説明では欠損画像領域を画素の集合として扱っている。欠損画像領域のデータの構成はこれに限るものではなく、概略化を行うこともできる。例えば、距離画像上の奥行きデータが欠損する領域を連結することにより形成される領域群を検出して、各連結領域を包絡する多角形・楕円形などの閉領域の集合を欠損画像領域としても良い。この場合、距離画像計測装置の撮像部の視点から各閉領域を形状モデルに投影した領域が欠損モデル領域となる。この方法では欠損モデル領域の形状精度は低くなるものの、欠損モデル領域を算出するための処理負荷を軽減することができる。
【００６４】
（変形例１−５）
欠損モデル領域も欠損画像領域を投影した形状そのものではなく、形状モデルの幾何構成要素の単位で欠損モデル領域を設定するなどの概略化を行っても良い。例えば、形状モデルが三角形パッチから構成されていれば、三角形パッチ単位で欠損モデル領域を設定することができる。この場合、距離画像計測の視点と欠損画素を結ぶ線分が最初に交差する形状モデルの幾何構成要素を欠損モデル領域として抽出しても良い。また、変形例１−４の連結領域を撮像部の視点から３次元空間に投影した錐体に含まれる幾何構成要素を欠損モデル領域として抽出しても良い。この方法では欠損モデル領域の形状精度は低くなるものの、モデル当て嵌め工程において欠損モデル領域に含まれる幾何構成要素の単位で処理の対象外とすることができるので、処理負荷を軽減することが可能である。
【００６５】
（変形例１−６）
形状モデルのデータの形態は三角形パッチに限らず、下記のようなデータの適用が可能である。
【００６６】
（１）線分群：ur1→+(1-u) r2→（r1→, r2→は線分の端点の座標を表すベクトル）
（２）パラメトリック曲線：（ｘ（u）、ｙ（u）、ｚ（u））
（３）パラメトリック曲面：（ｘ（u、ｖ）、ｙ（u、ｖ）、ｚ（u、ｖ））
いずれの場合も、欠損画像領域を３次元空間に投影して形成される錐体と形状モデルの交差を検出した結果として、欠損モデル領域がパラメータuや(u,v)のなす空間として記述できる。また、最近傍点対の形状モデル側の位置もパラメータuや(u,v)で記述できる。したがって、形状モデル側の最近傍点が欠損モデル領域をなすパラメータ空間に含まれるか否を判定して、欠損領域をモデル当て嵌めの対象から除外することも可能である。なお、形状モデルのデータが上記のいずれの形態の場合も、最近傍点は非特許文献１に開示された方法で求めることができる。
【００６７】
（変形例１−７）
距離画像にモデルを当て嵌めるアルゴリズムとしては上記の方法に限らず、例えば、非特許文献４で開示されている方法が利用可能である。また、モデル当て嵌め工程の処理で用いる幾何学的特徴は点に限らない。例えば、距離画像から抽出したエッジ（ルーフエッジやジャンプエッジ）と、モデルから抽出したエッジの当て嵌めによって対象物体の位置及び姿勢を推定しても良い。エッジを用いる場合も、形状モデル上のエッジが欠損モデル領域に含まれるか否かを判定し、含まれていれば当該のエッジを当て嵌め処理の対象外とすることができる。
【００６８】
（第２実施形態）
第１実施形態では、対象物体の位置姿勢を推定する演算処理（位置姿勢計算）により欠損モデル領域を完全に除外している。本実施形態では位置姿勢を推定する演算処理において、欠損モデル領域の寄与度を欠損モデル領域以外の形状モデル領域に比べて相対的に下げることによって、距離画像の計測欠損に起因する誤差を軽減する。ここで、欠損モデル領域の寄与度は、（数９）の重み行列Ｗの重み（wi）として設定される。ここで、最近傍点対の数Ｎｃは欠損モデル領域上の対応点対も含むものとする。
【００６９】
本実施形態の処理全体の流れは第１実施形態のモデル当て嵌め工程（ステップＳ４０５）のみ異なる。以下、第２実施形態におけるモデル当て嵌め工程の処理の流れを図６（ｂ）の参照により説明する。
【００７０】
図６（ｂ）のステップＳ６０１からＳ６０３は、図６（ａ）のステップＳ６０１からＳ６０３と同様なので説明を省略する。ステップＳ６０１で探索された最近傍点が欠損モデル領域内にあると判定された場合（Ｓ６０３−Ｙｅｓ）、重みｗiとして、最近傍点対間の距離に所定の定数ε（第１の重み）を設定する（Ｓ７０４）。定数εは、例えば、「０」に近い正の小数である。形状モデル側の最近傍点が欠損モデル領域内に存在する場合、所定の定数ε（第１の重み）を設定することにより該当する最近傍点対のデータを補正する。
【００７１】
一方、最近傍点が欠損モデル領域外にあると判定された場合（Ｓ６０３−Ｎｏ）、処理はステップＳ７０５に進められ、最近傍点対間の距離に第２の重みとして重みｗiを（数８）に従って設定する（Ｓ７０５）。形状モデル側の最近傍点が欠損モデル領域内に存在しない場合、第２の重みを設定することにより該当する最近傍点対のデータを補正する。ステップＳ７０４で設定される第１の重みに対して、ステップＳ７０５で設定される第２の重みの値を大きくすることで、対象物体の位置及び姿勢の計測値を算出する際に、欠損モデル領域内にある最近傍点が計算結果に与える影響（寄与度）を小さくできる。なお、欠損モデル領域内にある最近傍点の寄与度を小さくするには、欠損モデル領域内にある最近傍点に対して、欠損モデル領域外の最近傍点よりも相対的に小さな重み付けをすればよい。そのため、ステップＳ７０４とＳ７０５のどちらか一方だけを実行するようにしてもよい。
【００７２】
ステップＳ７０６において、重みと最近傍点対とのデータを組にしてメモリ３０２に登録する。そして、すべての最近傍点対に対する重みの設定が終了するまで（Ｓ７０７−Ｎｏ）、上述の処理を繰り返す。一方、ステップＳ７０７の判定で、すべての最近傍点対に対する重みの設定が終了したと判定された場合（Ｓ７０７−Ｙｅｓ）、処理はステップＳ７０８に進められる。
【００７３】
ステップＳ７０８において、最近傍点対間の距離に重みを掛け合わせた値の総和を最小にする形状モデルの位置及び姿勢を算出する。具体的には（式１１）に従って算出された位置姿勢の補正値ΔＳにより、形状モデルの位置・姿勢の概略値を補正し、補正された形状モデルの位置・姿勢の計算結果（位置・姿勢データ）をメモリ３０２に格納する。あるいは、ＣＰＵ３０１の制御の下、位置・姿勢データはデータ入出力部３０９を介して外部の装置に出力される。
【００７４】
本実施形態に拠れば、欠損モデル領域内の最近傍点を除外せずに、低い重みを設定することで、実際には欠損部分に相当しないデータが誤って位置姿勢推定処理から除外されるのを防ぐことができる。特に、対象物体の概略位置姿勢の正確さが低いことが予想される場合、この効果はより大きくなる。尚、第１実施形態で説明した、種々の変形例は、本実施形態においても適用することができ、その変形例の効果を実現することが可能である。
【００７５】
（第３実施形態）
第１実施形態では、対象物体の位置姿勢を推定する演算処理（位置姿勢計算）から欠損モデル領域を完全に除外している。第２実施形態では、欠損モデル領域の寄与度を欠損モデル領域以外の形状モデル領域に比べて相対的に下げることによって、距離画像の計測欠損に起因する誤差を軽減している。両者の方法は排他的なものではなく、両方法を併用することも可能である。
【００７６】
例えば、変形例１−１で説明したように、欠損モデル領域探索工程（Ｓ４０４）およびモデル当て嵌め工程（Ｓ４０５）を繰り返して、対象物体の位置姿勢推定値を補正する場合、両者の方法を組み合わせることも可能である。例えば、所定の繰り返し回数（２Ｍ：Ｍは自然数）のうち、前半の繰り返し回では第２実施形態の方法を適用し、後半の繰り返し回では第１実施形態の方法を適用することが可能である。このとき、第２実施形態の方法による位置姿勢を推定する演算処理を所定の回数繰り返した後に、第１実施形態の方法により位置姿勢を推定する演算処理することが可能である。例えば、ＣＰＵ３０１が第２実施形態の方法による位置姿勢を推定する演算処理の繰り返し回数を計測する。そして、所定回数の演算処理が実行された後、第１実施形態の方法により位置姿勢を推定する演算処理を実行するように、演算処理の切替えを制御することが可能である。あるいは、ＣＰＵ３０１は、第２実施形態の方法による演算処理で最近傍点間の距離の総和が所定の閾値未満になったか否かを判定する。そして、閾値未満になった後に、第１実施形態の方法により位置姿勢を推定する演算処理を実行するように、演算処理の切替えを制御することも可能である。
【００７７】
第１および第２実施形態の方法との組み合わせは、例えば、所定の繰り返し回数（２Ｍ：Ｍは自然数）のうち、前半の繰り返し回では第１実施形態の方法を適用し、後半の繰り返し回では第２実施形態の方法を適用してもよい。
【００７８】
本実施形態に拠れば、対象物体の位置姿勢の正確さが低い段階で誤って欠損部分に相当しないモデル領域が位置姿勢推定計算から除外されるのを防ぎ、位置姿勢の正確さが向上した段階で確実にデータ欠損の影響を除外できる。このため、欠損領域の影響を軽減する２種類の手法を組み合わせることにより、より位置姿勢推定（位置姿勢計算）の精度を向上させることが可能になる。
【００７９】
（第４実施形態）
前述の第１乃至第３実施形態では、欠損モデル領域５０５を形状モデル５０４の表面上の領域としているが、形状モデル５０４の座標系における空間領域であっても良い。例えば、欠損画像領域５０３に属する各画素の距離画像上の領域を３次元空間に投影した四角錐の集合を欠損モデル領域としてもよい。あるいは、変形例１−４のようにデータが欠損している画像領域を包絡する多角形や楕円を３次元空間に投影した錐体によって欠損モデル領域を構成しても良い。
【００８０】
この場合、最近傍点対の形状モデル側の最近傍点が欠損モデル領域を構成するいずれかの錐体の内部にある場合に、該当する最近傍点対を対象物体の位置姿勢推定処理の対象外とする。あるいは、第２実施形態で説明したように、該当する最近傍点対の寄与度を低く設定する。モデル当て嵌め処理で用いる特徴が、例えば、エッジなど点以外の場合も同様に、特徴が欠損モデル領域を構成するいずれかの錐体の内部にある場合に、該当する特徴を当て嵌め処理の対象外としたりすることも可能である。また、第２実施形態で説明したようにその特徴の寄与度を低く設定することも可能である。
【００８１】
一般的に、形状モデルが陰関数により表現された曲線あるいは曲面の場合やメタボールで表現されている場合、モデル表面上の領域をパラメータで表すことは困難である。しかし、こうした形状モデルであっても、本実施形態は適用可能である。更に、形状モデルが３次元の点群の場合も適用可能である。すなわち、欠損モデル領域を構成するいずれかの錐体の内部に個々の３次元の点が含まれる場合に、該当する点をモデル当て嵌め処理の対象外とすれば良い。
【００８２】
（その他の実施例）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【図２】
2011179907.tif 000014
【図３】
2011179907.tif 000015
【図４】
2011179907.tif 000016
【図５】
2011179907.tif 000017
【図６】
2011179907.tif 000018
【図１】
2011179907.tif 000019