位置姿勢計測装置、位置姿勢計測方法、及びプログラム
【課題】距離画像の欠損領域（無効領域）を特徴として利用した、対象物体の３次元位置姿勢の推定を可能にすることを目的とする。
【解決手段】３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像部と、距離画像から、撮像部と対象物体との距離情報が取得できない領域である無効領域を選別する選別部と、無効領域の特徴を示す特徴量を抽出する抽出部と、距離画像を撮像した時の撮像部と対象物体との相対的な位置姿勢を示す情報と、撮像部により複数の異なる方向から撮像された距離画像ごとの特徴量とを関連付けて記憶する記憶部と、抽出部により抽出された特徴量に対応する記憶部に記憶された特徴量を求めて、当該求められた特徴量と関連付けられた位置姿勢を示す情報から対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定部と、を備える。
【解決手段】３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像部と、距離画像から、撮像部と対象物体との距離情報が取得できない領域である無効領域を選別する選別部と、無効領域の特徴を示す特徴量を抽出する抽出部と、距離画像を撮像した時の撮像部と対象物体との相対的な位置姿勢を示す情報と、撮像部により複数の異なる方向から撮像された距離画像ごとの特徴量とを関連付けて記憶する記憶部と、抽出部により抽出された特徴量に対応する記憶部に記憶された特徴量を求めて、当該求められた特徴量と関連付けられた位置姿勢を示す情報から対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定部と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置及び姿勢を計測する位置姿勢計測装置、位置姿勢計測方法、及びプログラムに関する。
【背景技術】
【０００２】
近年のロボット技術の発展に従い、従来は人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。複雑なタスクの代表的な例として、工業製品の組立が挙げられる。ロボットが自律的に組立作業を行うためには、ハンドなどのエンドエフェクタによって部品を把持することが必要となる。ロボットハンドによって部品を把持するためには、把持の対象となる部品とロボットとの相対的な位置及び姿勢を計測し、計測結果に基づいて移動計画を策定して実際にアクチュエータを制御する必要がある。また、ロボット以外の分野においても、環境や物体に対する位置及び姿勢を計測することが必要とされている。例えば、複合現実感技術（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ、ＭＲ）においては、現実の画像にコンピュータによって生成された仮想のオブジェクトを合成するために、位置及び姿勢の計測が必要である。ロボットの位置姿勢計測は、これまでロボットに搭載されたカメラや距離センサを用いて行われており、近年では対象物体までの距離情報が画素ごとに保持された距離画像を用いる方法が盛んになってきている。
【０００３】
距離画像を用いる方法の１つとして、非特許文献１がある。この方法では、距離画像を３次元点群データに変換し、点群データに物体の３次元モデルをあてはめることにより物体の位置及び姿勢を計測する。すなわち、位置及び姿勢の概略値に基づいて、点群データの各点について３次元モデルの近傍の面を探索し、点と面との距離の和を最小化するように位置及び姿勢の最適化を繰り返すことにより位置及び姿勢を算出する。さらに、非特許文献２では、時系列で得られる距離画像をもとに、非特許文献１の方法を用いて、フレーム間での物体の位置及び姿勢の変化を推定する手法が開示されている。
【０００４】
距離画像を用いる別の方法として、特許文献１がある。この方法では、対象物体の輪郭を関数表現してデータベースとして予め作成しておき、距離画像撮像装置で撮影した距離画像から求めた輪郭とデータベースに蓄積された輪郭関数との相関性を求めることで、対象物体の位置姿勢を推定する。データベースには、対象物体のＣＡＤデータを様々な姿勢で描画し、姿勢毎に輪郭を抽出して関数表現したものが蓄積されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００４−３２６３１４号公報
【特許文献２】特開２０００−２３０８０号公報
【特許文献３】特開平１０−９３８１７号公報
【特許文献４】特開平５−２６４６号公報
【非特許文献】
【０００６】
【非特許文献１】P. J. Besl and N. D. McKay, “A method for registration of ３-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.１４, no.２, pp.２３９-２５６, １９９２
【非特許文献２】D. A. Simon, M. Hebert, and T. Kanade, “Real-time ３-D pose estimation using a high-speed range sensor,” Proc. １９９４ IEEE International Conference on Robotics and Automation (ICRA ’９４), pp.２２３５-２２４１, １９９４.
【非特許文献３】C.-S. Chen, Y.-P. Hung, J.-B. Cheng, “A fast automatic method for registration of partially overlapping range images”, in: International Conference on Computer Vision, Bombay, １９９８, pp. ２４２-２４８.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
実際の距離画像では、対象物体の形状または反射特性の影響で距離値が取得できない領域（欠損領域）が発生する。このような場合、非特許文献１、非特許文献２、及び特許文献１で開示されている方法では、距離画像から得られる情報量が不足し、正しい位置姿勢が求められない場合がありうる。例えば、非特許文献１及び非特許文献２では、点群データが減少することで、点群データの各点と３次元モデルの近傍の面との対応数が減少する。対応数の減少により、正しい位置姿勢が求められないことがある。また、特許文献１では、輪郭に該当する点群データが欠如してしいる場合には、位置姿勢は求められない。
【０００８】
上記の問題を解決する方法として、カラー画像を用いて距離画像を補間する方法がある（特許文献２参照）。しかし、高速な処理を要求される場面においては、この方法は処理時間の問題から適さない。一方、欠損領域は、対象物体の形状や反射特性によって異なるため、欠損領域そのものを物体固有の特徴として捉えることもできる。これを特徴として利用することで、不足した情報量を増やすこともできる。
【０００９】
上記の課題に鑑み、本発明は、距離画像の欠損領域を特徴として利用した、対象物体の３次元位置姿勢の推定及び物体識別を可能にすることを目的としている。
【課題を解決するための手段】
【００１０】
上記の目的を達成する本発明に係る位置姿勢計測装置は、
３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
前記対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像手段と、
前記距離画像から、前記撮像手段と前記対象物体との距離情報が取得できない領域である無効領域を選別する選別手段と、
前記無効領域の特徴を示す特徴量を抽出する抽出手段と、
前記距離画像を撮像した時の前記撮像手段と前記対象物体との相対的な位置姿勢を示す情報と、前記撮像手段により複数の異なる方向から撮像された前記距離画像ごとの前記特徴量とを関連付けて記憶する記憶手段と、
前記抽出手段により抽出された特徴量に対応する前記記憶手段に記憶された特徴量を求めて、
当該求められた特徴量と関連付けられた前記位置姿勢を示す情報から前記対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定手段と、
を備えることを特徴とする。
【発明の効果】
【００１１】
本発明によれば、距離画像の欠損領域を特徴として利用した、対象物体の３次元位置姿勢の推定及び物体識別が可能となる。
【図面の簡単な説明】
【００１２】
【図１】（ａ）本発明に係る位置姿勢計測装置１００の機能構成図、（ｂ）本発明に係る処理が適用可能なコンピュータのハードウェア構成を示すブロック図。
【図２】本発明に係る全体の処理の流れを説明したフローチャート。
【図３】（ａ）２次元無効領域特徴データベースの構成例を示した図、（ｂ）３次元特徴データベースの構成例を示した図。
【図４】２次元特徴データベースを生成する流れを示すフローチャート。
【図５】球面に沿った距離画像撮像装置の移動を示した図。
【図６】（ａ）２次元無効領域特徴を対象物体の３次元形状を表すモデル上に投影する様子を示した図、（ｂ）参照線分と検出線分の関係の説明図。
【図７】第３実施形態に係る処理の流れを示すフローチャート。
【図８】有効領域が特徴として追加された３次元特徴データベースを示す図。
【図９】第４実施形態に係る処理の流れを示すフローチャート。
【図１０】第５実施形態に係る２次元特徴モデルデータベースの構成例を示す図。
【図１１】第５実施形態に係る処理の流れを示すフローチャート。
【発明を実施するための形態】
【００１３】
（第１実施形態）
第１実施形態では、対象物体までの距離情報が画素値として保持された距離画像の画素値が無効になっている領域を選別する。そして、複数の視点から撮影して検出した領域特徴を視点位置ごとに記述したモデルを登録したデータベースと照合することで、物体の概略位置姿勢を推定する方法を説明する。距離画像の画素値が無効になっている領域とは、対象物体の形状または反射特性の影響で距離値が取得できない領域（欠損領域）を意味する。以下、距離画像の画素値が無効になっている領域特徴を「２次元無効領域特徴」と称する。複数の視点から撮影して検出した領域特徴（２次元無効領域特徴）を視点の位置ごとに記述したモデルを「２次元無効領域特徴モデル」と称する。２次元無効領域特徴モデルを登録したデータベースを「２次元無効領域特徴データベース」と称する。
【００１４】
各２次元無効領域特徴モデルには、検出した複数の２次元無効領域と共に、撮影時の距離画像撮影装置と対象物体の相対位置姿勢が記述されている。
【００１５】
図１（ａ）を参照して、本発明に係る位置姿勢計測装置１００の機能構成図について説明する。距離画像取得部１０１は、対象物体を撮像して距離画像を取得する。特徴量抽出部１０２は、距離画像中の無効領域から２次元無効領域特徴を検出して特徴量を抽出する。無効領域とは、距離が取得できなかった計測点である。２次元無効領域特徴の特徴量として、距離画像面から無効領域、無効領域の輪郭又は重心を抽出する。位置姿勢算出部１０３は、物体の概略位置姿勢または位置姿勢を算出する。具体的な処理については後述する。誤差算出部１０４は、誤差を数値として算出する。
【００１６】
図１（ｂ）を参照して、本発明に係る処理が適用可能なコンピュータのハードウェア構成を示すブロック図について説明する。ＣＰＵ１１１は、ＲＡＭ１１２やＲＯＭ１１３に格納されているプログラムやデータを用いて本コンピュータ全体の制御を行うと共に、本コンピュータが行う後述の各処理を実行する。
【００１７】
ＲＡＭ１１２は、外部記憶装置１１４からロードされたプログラムやデータ、Ｉ／Ｆ（インタフェース）１１５を介して外部から取得したデータ（本実施形態の場合には距離画像）を一時的に記憶するためのエリアを有する。さらに、ＲＡＭ１１２は、ＣＰＵ１１１が各種の処理を実行する際に用いるワークエリアを有する。このように、ＲＡＭ１１２は、各種の用途に応じて適宜エリアを提供することができる。
【００１８】
ＲＯＭ１１３は、本コンピュータの設定データやブートプログラムなどを格納する。外部記憶装置１１４は外部記憶装置であり、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置１１４には、ＯＳ（オペレーティングシステム）や本コンピュータが行う後述の各処理をＣＰＵ１１１に実行させるためのプログラムやデータが保存されている。データには、特徴モデルが複数登録されたデータベースが含まれる。これらのプログラムやデータはＣＰＵ１１１による制御に従って適宜ＲＡＭ１１２にロードされる。そしてＣＰＵ１１１はこのロードされたプログラムやデータを用いて処理を実行する。
【００１９】
Ｉ／Ｆ（インタフェース）１１５は、本実施形態の場合には、距離画像撮像装置を接続するためのインタフェースや、外部のネットワーク接続するためのインターフェースとして機能するものである。距離画像撮像装置が撮像した画像は、このＩ／Ｆ１１５を介して本コンピュータのＲＡＭ１１２や外部記憶装置１１４に入力される。バス１１６は、上述の各部を繋ぐバスである。
【００２０】
図２を参照して、本実施形態に係る全体の処理の流れを説明したフローチャートについて説明する。本処理では、検出した２次元無効領域特徴と２次元無効領域特徴データベースとを照合し、照合した２次元無効領域特徴モデルに記述されている撮影時の距離画像撮影装置と対象物体との相対位置姿勢を物体の概略位置姿勢とする。以下、詳細を説明する。
【００２１】
ステップＳ２０１において、距離画像を取得する。
【００２２】
ステップＳ２０２において、取得された距離画像中の無効領域から２次元無効領域特徴を検出し、特徴量を抽出する。無効領域とは、距離が取得できなかった計測点であり、画素値には無効である旨を示す値（例えば“０”など）が格納されている。２次元無効領域特徴の特徴量として、距離画像面から無効領域、無効領域の輪郭、及び重心を抽出する。無効領域及び無効領域の輪郭抽出には、２値化の輪郭抽出に用いられるラベリング手法（特許文献３）を用いることができる。
【００２３】
ステップＳ２０３において、２次元無効領域特徴と２次元無効領域特徴データベースを照合することで、対象物体の初期位置姿勢とする概略位置姿勢を算出する。まず、２次元無効領域特徴データベースより、ステップＳ２０２で抽出された２次元無効領域特徴と類似する２次元無効領域特徴モデルを抽出する。そして、抽出された２次元無効領域特徴モデルに記述されている撮影時の距離画像撮影装置と対象物体の相対位置姿勢を概略位置姿勢として用いる。ステップＳ２０２で抽出された２次元無効領域特徴と類似する２次元特徴モデルを抽出する具体的な方法を、以下で説明する。
【００２４】
ステップＳ２０２で抽出された複数の２次元特徴と２次元特徴モデルに記述された複数の２次元特徴について特徴間の対応を求め、その対応する特徴同士の一致度から類似度を計測する。抽出された２次元特徴と類似する２次元特徴モデルとは、距離画像を撮影したときの視点位置が近く、互いに類似した距離画像と想定される。このため、特徴間の対応は特徴間の距離が近い特徴同士となる。この対応を求めることで、個々の特徴の識別も行っている。
【００２５】
重心を２次元特徴として用いる場合には、まず、ステップＳ２０２で抽出された各重心と２次元特徴モデルの各重心との距離を算出する。ステップＳ２０２で抽出された各重心に対する距離が最小となる２次元特徴モデルの重心を対応する重心とし、対応する重心間の距離の総和を求める。この処理を全２次元特徴モデルに対して行い、距離の総和が最小となるものを最も類似する２次元特徴モデルとする。
【００２６】
次に、輪郭を２次元特徴として用いる場合には、まず、ステップＳ２０２で抽出された各輪郭と２次元特徴モデルの各輪郭間のハウスドルフ距離（特許文献４）を算出する。ステップＳ２０２で抽出された輪郭に対してハウスドルフ距離が最小となる２次元特徴モデルの輪郭を対応する輪郭とし、対応する輪郭間のハウスドルフ距離の総和を求める。この処理を全２次元特徴モデルに対して行い、距離の総和が最小となる２次元特徴モデルを最も類似するモデルとして抽出する。
【００２７】
そして、無効領域を２次元特徴として用いる場合には、まず、ステップＳ２０２で抽出された各無効領域と２次元特徴モデルの各無効領域が重なっているか否かを判定し、両者の無効領域が重なっている領域同士を対応する無効領域とする。ステップＳ２０２で抽出された無効領域の全画素数に対する、両者の無効領域が重複している画素数の比を算出する。つまり，距離画像全体に占める無効領域の割合を算出する。この処理を全２次元特徴モデルに対して行い、この比が最大となる２次元特徴モデルを最も類似するモデルとして抽出する。ステップＳ２０３において、対象物体の概略位置姿勢の算出が完了すると、ステップＳ２０４に進む。
【００２８】
ステップＳ２０４において、位置姿勢算出部１０３はより精度よく位置姿勢を算出する。ステップＳ２０５において、誤差算出部１０４は誤差を算出する。ステップＳ２０６において、ステップＳ２０５で算出された誤差が閾値未満であるか否かが判定される。誤差が閾値未満である場合（ステップＳ２０６；ＹＥＳ）、処理は終了する。一方、誤差が閾値未満でない場合（ステップＳ２０６；ＮＯ）、ステップＳ２０４に戻る。
【００２９】
上記のステップＳ２０４からステップＳ２０５までの各処理については後の実施形態で詳述する。
【００３０】
図３（ａ）を参照して、２次元無効領域特徴データベースの構成例について説明する。距離画像撮影時の対象物体の位置姿勢３０２、及び複数の２次元無効領域特徴３０３が登録された２次元無効領域特徴モデル３０１が視点（対象物体の位置姿勢）ごとに登録されている。対象物体の位置姿勢３０２は、距離画像撮像装置と対象物体との相対位置姿勢とする。２次元無効領域特徴３０３には、輪郭画素位置３０４、輪郭画素数３０５、領域内画素位置３０６、領域内画素数３０７、重心画素３０８が特徴量として記述されている。輪郭画素位置３０４は輪郭に属する画素の画素位置を示す。輪郭画素数３０５は輪郭に属する画素の総画素数を示す。領域内画素位置３０６は領域に属する画素の画素位置を示す。領域内画素数３０７は領域に属する画素の総画素数を示す。そして重心画素３０８は領域の重心画素位置を示す。図３（ｂ）については第２実施形態で説明する。
【００３１】
図４を参照して、上述の２次元特徴データベースを生成する流れを示すフローチャートについて説明する。ステップＳ４０１において、距離画像取得部１０１は距離画像を取得する。
【００３２】
ステップＳ４０２において、特徴抽出部１０２は２次元無効領域特徴を複数検出し、これらの特徴量を抽出する。無効領域は、距離が取得できなかった計測点であり、画素値には無効である旨を示す値（例えば“０”など）が格納されている。２次元無効領域特徴の特徴量として、距離画像面から無効領域、無効領域の輪郭及び重心を抽出する。無効領域及び無効領域の輪郭抽出に、２値化の輪郭抽出に用いられるラベリング手法（特許文献３）を用いる点は図２のフローチャートと同様である。
【００３３】
ステップＳ４０３において、ステップＳ４０２で求めた２次元無効領域特徴と、距離画像装置と対象物体の相対位置姿勢を２次元無効領域特徴モデルとし、２次元無効領域特徴データベースに追加登録する。つまり、特徴量と、位置姿勢を示す情報とが関連付けられて記憶装置等に記憶される。
【００３４】
ステップＳ４０４において、データベース生成を終了する場合には、本処理を終了し、続行する場合にはステップＳ４０５へ進む。
【００３５】
ステップＳ４０５において、距離画像撮像装置を図５で示した球面に沿って、または球面の半径方向に移動して、距離画像撮像装置又は対象物体の位置姿勢を変更する。その後、ステップＳ４０１へ戻る。
【００３６】
これにより２次元特徴データベースが追加登録される。
【００３７】
（第２実施形態）
第２実施形態では、物体の概略位置姿勢が既知であると仮定し、より精度の高い物体の位置姿勢を推定する方法を説明する。処理は大きく２つに分けられる。なお、概略位置姿勢を算出してから精度の高い位置姿勢算出処理を行ってもよい。
【００３８】
第１に、２次元無効領域特徴モデルの各２次元無効領域特徴を、対象物体の３次元形状を表すモデル上の相当する領域で表した特徴に変換した特徴モデルが視点ごとに登録されたデータベースを考える。このデータベースから、物体の概略位置姿勢に近い位置姿勢で生成された３次元特徴モデルを抽出する。以下、２次元無効領域特徴モデルの各２次元無効領域特徴を、対象物体の３次元形状を表すモデル上の相当する領域で表した特徴を「３次元無効領域特徴」と称する。この３次元無効領域特徴に変換した特徴モデルを「３次元無効領域特徴モデル」と称する。そして、この３次元無効領域特徴モデルが視点ごとに登録されたデータベースを「３次元無効領域特徴モデルデータベース」と称する。第２に、抽出された３次元無効領域モデルの各無効領域特徴と、検出された各２次元無効領域特徴との対応をとることで、物体の位置姿勢を算出する。
【００３９】
図３（ｂ）を参照して、上述した３次元特徴データベースの構成例について説明する。パラメータ３５１は距離画像撮影装置に関するパラメータであり、例えば焦点距離、内部歪、画角等である。距離画像撮影時の対象物体の位置姿勢３５３、及び複数の３次元無効領域特徴３５４が登録された３次元無効領域特徴モデル３５２が視点（対象物体の位置姿勢）ごとに登録されている。対象物体の位置姿勢３５３は、距離画像撮像装置と対象物体との相対的な位置姿勢とする。３次元無効領域特徴３５４には、輪郭点位置３５５、輪郭点数３５６、領域内点位置３５７、領域内点数３５８、重心位置３５９が特徴量として記述されている。輪郭画素位置３０４は輪郭に属する画素の画素位置を示す。輪郭画素数３０５は輪郭に属する画素の総画素数を示す。領域内画素位置３０６は領域に属する画素の画素位置を示す。領域内画素位置３０６は領域に属する画素の総画素数を示す。そして重心画素３０８は領域の重心画素位置を示す。
【００４０】
一方、図６（ａ）は、２次元無効領域特徴を対象物体の３次元形状を表すモデル上に投影する様子を示したものである。図６に示すように、視点６０１と距離画像６０２上の２次元無効領域６０５の各点を結ぶ線分６０４と３次元形状モデル６０３とが交差する点を求めることで３次元形状モデル上での３次元無効領域８０６が得られる。以下、具体的に説明する。まず、３次元特徴データベースより、ステップＳ２０３で算出された対象物体の概略位置姿勢と同じ位置姿勢で登録されている３次元特徴モデルを選択する。
【００４１】
次に、３次元特徴モデルに記述されている無効領域の輪郭を、式（１）に示す関係式を用いて距離画像と同じ画像面に射影し、画像面での位置を算出する。u,vは距離画像面での画素位置、ｚは特徴の奥行き方向の距離、ｆは焦点距離、x,y,zは、特徴の３次元位置を示している。式（１）は、距離画像撮像装置に内部歪が存在しない場合を想定した関係式である。内部歪が存在する距離画像装置については、式（２）に示す関係式を用いる。式（２）のa,b,cは、画像中心、焦点距離などから得られる値、d,eは画像中心である。
【００４２】
【数１】
【００４３】
【数２】
【００４４】
次に、３次元特徴モデルに記述された輪郭を投影した輪郭を複数の線分に分割し、各線分ごとに直線の式を算出する。以後、説明のため、３次元特徴モデルに記述された輪郭を投影した輪郭を分割して得られた線分を参照線分と称する。また、ステップＳ２０２で抽出された輪郭を分割して得られた線分を検出線分と称する。
【００４５】
図６（ｂ）は、参照線分と検出線分の関係を説明する図である。図６（ｂ）では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。ある制御点（参照線分上のある１点）の画像上での位置を（ｕ0，ｖ0）と表す。また、該制御点が所属する参照線分の画像上での傾きをu軸に対する傾きをθとして表す。該参照線分の画像上での法線ベクトルは（sinθ，−cosθ）となる。また、該制御点の対応点の画像座標を（ｕ’，ｖ’）とする。対応点は、該参照線分に最も距離が近い検出線分と、参照線分の制御点から伸ばした法線との交点とする。ここで、点（ｕ’，ｖ’）を通り、傾きがθである直線上の点（ｕ，ｖ）は、
【００４６】
【数３】
【００４７】
と表せる（θは定数）。ここで、ｄは以下のような定数である。
【００４８】
【数４】
【００４９】
次に、対象物体の位置及び姿勢（以下、sとする）の微小変化と、画像上での位置姿勢の微小変化の関係式を求める。制御点の画像上での位置は計測対象物体の位置及び姿勢により変化する。また、対象物体の位置及び姿勢の自由度は６自由度である。すなわち、sは６次元ベクトルであり、対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が回転軸を表し、大きさが回転角を表す３次元ベクトルなどによって表現される。位置及び姿勢により変化する制御点の画像座標（ｕ，ｖ）は（ｕ０，ｖ０）の近傍で１次のテイラー展開によって以下の式（５）のように近似できる。但しΔｓｉ（ｉ＝１,２,・・・,６）はsの各成分の微小変化量を表す。
【００５０】
【数５】
【００５１】
次に、式（５）によって近似されるｕ、ｖを式（３）に代入して、以下の式（６）を得る。
【００５２】
【数６】
【００５３】
ただし、
【００５４】
【数７】
【００５５】
ここでｒは定数とした。
【００５６】
次に、すべての制御点とそれに対応する点について式（６）を求め、式（８）に示す線形連立方程式を導き出す。
【００５７】
【数８】
【００５８】
ここで式（８）を式（９）のように表す。
【００５９】
【数９】
【００６０】
式（９）の線形連立方程式の係数行列Ｊを算出するための偏微分係数の算出は、例えば非特許文献１に開示されている方法によって行う。
【００６１】
最後に、式（９）に基づいて、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いてΔsを求める。このΔsをステップＳ２０３で求めた概略位置姿勢に加算することで、ステップＳ２０４における物体の位置姿勢が算出される。
【００６２】
しかしながら、上記特徴は、場合によっては誤って抽出される場合もあるため、次に述べるようなロバスト推定手法をさらに用いてもよい。一般に、誤って抽出される特徴では誤差d−rが大きくなる。そのため式（８）、式（９）に示す連立方程式に対する誤差の寄与度が大きくなり、その結果得られるΔsの精度が低下してしまう。そこで、誤差d−rが大きいデータには小さな重みを与え、誤差d−rが小さいデータには大きな重みを与える。重みは例えば式（１０）に示すようなＴｕｋｅｙの関数により与えることができる。
【００６３】
【数１０】
【００６４】
ここでｃ１は定数である。なお、重みを与える関数は必ずしもＴｕｋｅｙの関数である必要はない。例えばＨｕｂｅｒの関数など、誤差が 大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であればいかなる関数であってもよい。
【００６５】
各データ（エッジまたは点群データ）に対応する重みをｗｉとする。ここで式（１１）に示すように重み行列Ｗを定義する。
【００６６】
【数１１】
【００６７】
重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗｉが入る。この重み行列Ｗを用いて、式（１０）を以下の式（１２）のように変形する。
【００６８】
【数１２】
【００６９】
式（１２）を解くことにより以下の式（１３）に示すように補正値Δsを算定できる。
【００７０】
【数１３】
【００７１】
この誤差の小さなΔsをステップＳ２０３で求めた概略位置姿勢に加算することで、ステップＳ２０４における物体の位置姿勢がより精度よく算出される。
【００７２】
次に誤差の判定手法について説明する。ステップＳ２０５において、式（１４）に示すように誤差ｄ−ｒの２乗和σを算出する。ここで、ｎは制御点数である。
【００７３】
【数１４】
【００７４】
ステップＳ２０６において、式（１４）を用いて誤差を判定する。誤差の２乗和が予め定められた閾値未満であった場合には（ステップＳ２０６；ＹＥＳ）、処理を終了する。一方、閾値を超えていた場合には（ステップＳ２０６；ＮＯ）、ステップＳ２０５で算出された対象物体の位置姿勢を初期位置として、ステップＳ２０４へ戻る。
【００７５】
ステップＳ２０４では、無効領域の輪郭を特徴として用いた場合の位置姿勢算出方法を説明した。以下では、無効領域または重心を特徴として用いる場合について説明する。
【００７６】
まず、３次元特徴データベースより、ステップＳ２０２で算出された対象物体の概略位置姿勢と同じ位置姿勢で登録されている３次元特徴モデルを抽出する。
【００７７】
次に、３次元特徴モデルの無効領域の重心または無効領域内に属する各点を、式（１）または式（２）の関係式を用いて距離画像と同じ画像面に射影したときの位置を算出する。
【００７８】
次に、式（１５）に示すように、対象物体の位置及び姿勢（以下、sとする）の微小変化と、画像上での位置姿勢の微小変化の関係式を求める。
【００７９】
【数１５】
【００８０】
ｕ０、ｖ０は、３次元特徴モデルの各重心または領域内に属する各点を射影した点、ｕ、ｖはｕ０、ｖ０から最も距離の近いステップＳ２０２で抽出された重心または領域内に属する点とする。つまり、ｕ−ｕ０、ｖ−ｖ０は、３次元特徴モデルの各重心または領域内に属する各点を射影した各点と、これと最も近い距離にあるステップＳ２０２で抽出された重心または領域内に属する点との距離となる。
【００８１】
次に、すべての重心または領域内の点について式（１５）を求めて、式（１６）を得る。
【００８２】
【数１６】
【００８３】
∂ｕｎ／∂ｓｉ、∂ｖｎ／∂ｓｉは、式（１）又は式（２）で表した関係式を、ｓiで偏微分した解となる。
【００８４】
次に、式（１６）に対して式（８）と同様の方法を用いることで補正量Δsを求める。そして、ステップＳ２０３で求めた概略位置姿勢に補正量Δsを加算することで、物体の位置姿勢が算出される。
【００８５】
ステップＳ２０３での概略位置姿勢算出は、２次元特徴及び２次元特徴データベースを用いた方法に限定しない。概略位置姿勢が求められる方法で適用可能なものであれば、何でもよい。例えば、対象物体に位置姿勢計測センサを取り付け、このセンサから得られる位置姿勢を初期値として用いてもよい。また、ステップＳ２０４以降の処理をせず、対象物体の初期位置を求めた時点で処理を終了してもよい。
【００８６】
図４を参照して、上述の２次元特徴データベース及び３次元特徴データベースを生成する流れを示すフローチャートについて説明する。
【００８７】
ステップＳ４０１において、距離画像を取得する。ステップＳ４０２において、距離画像が無効となる領域から特徴を抽出する。抽出方法は、距離画像中の無効となる領域をラベリング手法（特許文献３）を用いて、距離画像面での領域、輪郭を求める。また、求められた領域からその重心も求める。そして求められた無効領域、無効領域の輪郭、及び無効領域の重心を２次元特徴とする。
【００８８】
ステップＳ４０３において、ステップＳ４０２で求められた２次元特徴と距離画像装置との相対位置姿勢をセットにした２次元特徴モデルを、２次元特徴データベースに追加登録する。また、２次元特徴を３次元に変換した特徴も同様に、距離画像との相対位置姿勢とセットにして、３次元特徴データベースに追加登録する。
【００８９】
ステップＳ４０４において、データベース生成を終了する場合には、本処理を終了し、続行する場合にはステップＳ４０５へ進む。
【００９０】
ステップＳ４０５において、距離画像撮像装置を、図５で示した球面に沿って移動する。または、図５で示した球面の半径の大きさを、半径方向に距離撮像装置の撮影可能範囲内で移動させる。距離画像装置又は対象物体の位置姿勢を変更してステップＳ４０１に戻る。
【００９１】
（第３実施形態）
第３の実施形態では、無効領域から抽出した特徴に加えて距離画像の距離値が有効な領域（以下、「有効領域」と称する。）から抽出した特徴を用いて位置姿勢を求める場合を説明する。
【００９２】
図７を参照して、本実施形態に係る処理の流れを示すフローチャートについて説明する。ステップＳ７０１において、距離画像を取得する。ステップＳ７０２において、距離画像中の無効領域から２次元特徴を抽出する。さらに有効領域を抽出する。２次元特徴として、距離画像面から無効領域、無効領域の輪郭及び重心を抽出する。無効領域及び無効領域の輪郭抽出には、２値化の輪郭抽出に用いられるラベリング手法（特許文献３）を用いることができる。有効領域は、距離が取得できた計測点であり、画素値には３次元位置が格納されている。
【００９３】
ステップＳ７０３において、２次元特徴及び２次元特徴データベースを用いて対象物体の概略位置姿勢を算出する。算出方法は、ステップＳ２０３と同様である。
【００９４】
ステップＳ７０４において、ステップＳ７０３で求められた概略位置姿勢から、有効領域、２次元特徴及び３次元特徴データベースを用いて対象物体の位置姿勢を求める。算出方法は、まず、有効領域の点群に関して、複数の距離画像の位置合わせに用いられるＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）の手法（非特許文献１参照）を用いて対象物体の位置姿勢を求める。
【００９５】
次に、ＩＣＰによって求められた位置姿勢を概略位置姿勢とし、ステップＳ２０４と同様の方法で３次元特徴及び３次元特徴データベースを用いて対象物体の位置姿勢を算出する。さらに、これらの処理を繰り返し行っても良い。
【００９６】
図８を参照して、有効領域が特徴として追加された３次元特徴データベースについて説明する。３次元特徴（有効領域）は、点の３次元位置８０１及び面を表すインデックス８０２から構成される。その他の構成は図３（ｂ）と同様であり、同一の参照番号を用いている。
【００９７】
さらにステップＳ７０５における誤差を算出する処理、ステップＳ７０６の誤差の閾値との比較処理を行ってもよい。
【００９８】
（第４実施形態）
第４実施形態では、無効領域の特徴のみを用いた結果、無効領域と有効領域の両方の特徴を用いた結果、又は有効領域の特徴のみを用いた結果をそれぞれ比較することで、求められた位置姿勢の妥当性を評価する方法を説明する。
【００９９】
図９を参照して、本実施形態の処理の流れを示すフローチャートについて説明する。
【０１００】
ステップＳ９０１において、距離画像を取得する。ステップＳ９０２において、ステップＳ７０２と同様に、距離画像中の無効領域から２次元特徴を抽出する。さらに有効領域を抽出する。
【０１０１】
ステップＳ９０３において、２次元特徴及び２次元特徴データベースを用いて対象物体の概略位置姿勢を決定する。概略位置姿勢決定方法は、ステップＳ２０３と同様である。
【０１０２】
ステップＳ９０４において、ステップＳ２０４及びステップＳ７０４を実行し、対象物体の位置姿勢をそれぞれ算出する。またこのとき、有効領域のみを用いた位置姿勢算出も行う。つまり、合計３つの位置姿勢が算出されることになる。
【０１０３】
ステップＳ９０５において、ステップＳ９０４で求められた３つの位置姿勢について、それぞれ誤差を算出する。誤差の算出方法は、ステップＳ２０５と同様である。
【０１０４】
ステップＳ９０６において、ステップＳ９０５で求められた誤差が予め定められた閾値以内か否かを判定する。２つの位置姿勢の誤差がともに閾値内となった場合には（ステップＳ９０６；ＹＥＳ）、ステップＳ９０７へ進む。それ以外の場合には（ステップＳ９０６；ＮＯ）、閾値を超えた位置姿勢に関して、ステップＳ９０４に戻り、再度位置姿勢算出を実行する。
【０１０５】
ステップＳ９０７において、ステップＳ９０４で求められた３つの位置姿勢を互いに比較し、求められた位置姿勢の妥当性を検証する。具体的には、両者の位置姿勢の差を計算し、この差が予め定められた閾値内であるか否かを判定する。閾値の範囲以下である場合には、無効領域の特徴のみを用いて求めた位置姿勢（ステップＳ２０４を実行することで得られた位置姿勢）、無効領域と有効領域の両方の特徴を用いて求めた位置姿勢（ステップＳ７０４を実行することで得られた位置姿勢）、又は有効領域のみを用いて求めた位置姿勢の何れかを対象物体の位置姿勢とする。
【０１０６】
なお、３つの位置姿勢すべてを求めるのではなく、３つのうち２つの位置姿勢だけを求めて、両者を比較することにより検証してもよい。
【０１０７】
（第５実施形態）
第５実施形態では、物体の識別子と共に無効領域の２次元特徴が記述された２次元特徴モデルが登録されている２次元特徴モデルデータベースを用いて、物体の認識を行う場合について説明する。
【０１０８】
図１０を参照して、本実施形態に係る２次元特徴モデルデータベースの構成例について説明する。２次元特徴モデル１００１は、対象物体の識別子１００２及び複数の２次元特徴１００３が記述されている。２次元特徴１００３は、輪郭に属する画素の画素位置を示した輪郭画素位置１００４、輪郭に属する画素の総画素数を示した輪郭画素数１００５、及び領域に属する画素の画素位置を示した領域内画素位置１００６を備える。さらに、領域に属する画素の総画素数を示した領域内画素数１００７、及び領域の重心画素位置を示した重心画素１００８を備える。
【０１０９】
図１１を参照して、本実施形態に係る処理の流れを示すフローチャートについて説明する。
【０１１０】
ステップＳ１１０１において、距離画像を取得する。
【０１１１】
ステップＳ１１０２において、ステップＳ２０２と同様に、取得された距離画像から無効領域の特徴を抽出する。
【０１１２】
ステップＳ１１０３において、物体を識別する。識別方法は、図１０に示した２次元特徴データベースからステップＳ１１０２で抽出した特徴と類似する２次元特徴モデルを抽出して、この２次元特徴モデルに記述されている識別子から、物体を識別する。２次元特徴モデルを抽出する方法は、ステップＳ２０３で述べた方法と同様である。
【０１１３】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置及び姿勢を計測する位置姿勢計測装置、位置姿勢計測方法、及びプログラムに関する。
【背景技術】
【０００２】
近年のロボット技術の発展に従い、従来は人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。複雑なタスクの代表的な例として、工業製品の組立が挙げられる。ロボットが自律的に組立作業を行うためには、ハンドなどのエンドエフェクタによって部品を把持することが必要となる。ロボットハンドによって部品を把持するためには、把持の対象となる部品とロボットとの相対的な位置及び姿勢を計測し、計測結果に基づいて移動計画を策定して実際にアクチュエータを制御する必要がある。また、ロボット以外の分野においても、環境や物体に対する位置及び姿勢を計測することが必要とされている。例えば、複合現実感技術（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ、ＭＲ）においては、現実の画像にコンピュータによって生成された仮想のオブジェクトを合成するために、位置及び姿勢の計測が必要である。ロボットの位置姿勢計測は、これまでロボットに搭載されたカメラや距離センサを用いて行われており、近年では対象物体までの距離情報が画素ごとに保持された距離画像を用いる方法が盛んになってきている。
【０００３】
距離画像を用いる方法の１つとして、非特許文献１がある。この方法では、距離画像を３次元点群データに変換し、点群データに物体の３次元モデルをあてはめることにより物体の位置及び姿勢を計測する。すなわち、位置及び姿勢の概略値に基づいて、点群データの各点について３次元モデルの近傍の面を探索し、点と面との距離の和を最小化するように位置及び姿勢の最適化を繰り返すことにより位置及び姿勢を算出する。さらに、非特許文献２では、時系列で得られる距離画像をもとに、非特許文献１の方法を用いて、フレーム間での物体の位置及び姿勢の変化を推定する手法が開示されている。
【０００４】
距離画像を用いる別の方法として、特許文献１がある。この方法では、対象物体の輪郭を関数表現してデータベースとして予め作成しておき、距離画像撮像装置で撮影した距離画像から求めた輪郭とデータベースに蓄積された輪郭関数との相関性を求めることで、対象物体の位置姿勢を推定する。データベースには、対象物体のＣＡＤデータを様々な姿勢で描画し、姿勢毎に輪郭を抽出して関数表現したものが蓄積されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００４−３２６３１４号公報
【特許文献２】特開２０００−２３０８０号公報
【特許文献３】特開平１０−９３８１７号公報
【特許文献４】特開平５−２６４６号公報
【非特許文献】
【０００６】
【非特許文献１】P. J. Besl and N. D. McKay, “A method for registration of ３-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.１４, no.２, pp.２３９-２５６, １９９２
【非特許文献２】D. A. Simon, M. Hebert, and T. Kanade, “Real-time ３-D pose estimation using a high-speed range sensor,” Proc. １９９４ IEEE International Conference on Robotics and Automation (ICRA ’９４), pp.２２３５-２２４１, １９９４.
【非特許文献３】C.-S. Chen, Y.-P. Hung, J.-B. Cheng, “A fast automatic method for registration of partially overlapping range images”, in: International Conference on Computer Vision, Bombay, １９９８, pp. ２４２-２４８.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
実際の距離画像では、対象物体の形状または反射特性の影響で距離値が取得できない領域（欠損領域）が発生する。このような場合、非特許文献１、非特許文献２、及び特許文献１で開示されている方法では、距離画像から得られる情報量が不足し、正しい位置姿勢が求められない場合がありうる。例えば、非特許文献１及び非特許文献２では、点群データが減少することで、点群データの各点と３次元モデルの近傍の面との対応数が減少する。対応数の減少により、正しい位置姿勢が求められないことがある。また、特許文献１では、輪郭に該当する点群データが欠如してしいる場合には、位置姿勢は求められない。
【０００８】
上記の問題を解決する方法として、カラー画像を用いて距離画像を補間する方法がある（特許文献２参照）。しかし、高速な処理を要求される場面においては、この方法は処理時間の問題から適さない。一方、欠損領域は、対象物体の形状や反射特性によって異なるため、欠損領域そのものを物体固有の特徴として捉えることもできる。これを特徴として利用することで、不足した情報量を増やすこともできる。
【０００９】
上記の課題に鑑み、本発明は、距離画像の欠損領域を特徴として利用した、対象物体の３次元位置姿勢の推定及び物体識別を可能にすることを目的としている。
【課題を解決するための手段】
【００１０】
上記の目的を達成する本発明に係る位置姿勢計測装置は、
３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
前記対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像手段と、
前記距離画像から、前記撮像手段と前記対象物体との距離情報が取得できない領域である無効領域を選別する選別手段と、
前記無効領域の特徴を示す特徴量を抽出する抽出手段と、
前記距離画像を撮像した時の前記撮像手段と前記対象物体との相対的な位置姿勢を示す情報と、前記撮像手段により複数の異なる方向から撮像された前記距離画像ごとの前記特徴量とを関連付けて記憶する記憶手段と、
前記抽出手段により抽出された特徴量に対応する前記記憶手段に記憶された特徴量を求めて、
当該求められた特徴量と関連付けられた前記位置姿勢を示す情報から前記対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定手段と、
を備えることを特徴とする。
【発明の効果】
【００１１】
本発明によれば、距離画像の欠損領域を特徴として利用した、対象物体の３次元位置姿勢の推定及び物体識別が可能となる。
【図面の簡単な説明】
【００１２】
【図１】（ａ）本発明に係る位置姿勢計測装置１００の機能構成図、（ｂ）本発明に係る処理が適用可能なコンピュータのハードウェア構成を示すブロック図。
【図２】本発明に係る全体の処理の流れを説明したフローチャート。
【図３】（ａ）２次元無効領域特徴データベースの構成例を示した図、（ｂ）３次元特徴データベースの構成例を示した図。
【図４】２次元特徴データベースを生成する流れを示すフローチャート。
【図５】球面に沿った距離画像撮像装置の移動を示した図。
【図６】（ａ）２次元無効領域特徴を対象物体の３次元形状を表すモデル上に投影する様子を示した図、（ｂ）参照線分と検出線分の関係の説明図。
【図７】第３実施形態に係る処理の流れを示すフローチャート。
【図８】有効領域が特徴として追加された３次元特徴データベースを示す図。
【図９】第４実施形態に係る処理の流れを示すフローチャート。
【図１０】第５実施形態に係る２次元特徴モデルデータベースの構成例を示す図。
【図１１】第５実施形態に係る処理の流れを示すフローチャート。
【発明を実施するための形態】
【００１３】
（第１実施形態）
第１実施形態では、対象物体までの距離情報が画素値として保持された距離画像の画素値が無効になっている領域を選別する。そして、複数の視点から撮影して検出した領域特徴を視点位置ごとに記述したモデルを登録したデータベースと照合することで、物体の概略位置姿勢を推定する方法を説明する。距離画像の画素値が無効になっている領域とは、対象物体の形状または反射特性の影響で距離値が取得できない領域（欠損領域）を意味する。以下、距離画像の画素値が無効になっている領域特徴を「２次元無効領域特徴」と称する。複数の視点から撮影して検出した領域特徴（２次元無効領域特徴）を視点の位置ごとに記述したモデルを「２次元無効領域特徴モデル」と称する。２次元無効領域特徴モデルを登録したデータベースを「２次元無効領域特徴データベース」と称する。
【００１４】
各２次元無効領域特徴モデルには、検出した複数の２次元無効領域と共に、撮影時の距離画像撮影装置と対象物体の相対位置姿勢が記述されている。
【００１５】
図１（ａ）を参照して、本発明に係る位置姿勢計測装置１００の機能構成図について説明する。距離画像取得部１０１は、対象物体を撮像して距離画像を取得する。特徴量抽出部１０２は、距離画像中の無効領域から２次元無効領域特徴を検出して特徴量を抽出する。無効領域とは、距離が取得できなかった計測点である。２次元無効領域特徴の特徴量として、距離画像面から無効領域、無効領域の輪郭又は重心を抽出する。位置姿勢算出部１０３は、物体の概略位置姿勢または位置姿勢を算出する。具体的な処理については後述する。誤差算出部１０４は、誤差を数値として算出する。
【００１６】
図１（ｂ）を参照して、本発明に係る処理が適用可能なコンピュータのハードウェア構成を示すブロック図について説明する。ＣＰＵ１１１は、ＲＡＭ１１２やＲＯＭ１１３に格納されているプログラムやデータを用いて本コンピュータ全体の制御を行うと共に、本コンピュータが行う後述の各処理を実行する。
【００１７】
ＲＡＭ１１２は、外部記憶装置１１４からロードされたプログラムやデータ、Ｉ／Ｆ（インタフェース）１１５を介して外部から取得したデータ（本実施形態の場合には距離画像）を一時的に記憶するためのエリアを有する。さらに、ＲＡＭ１１２は、ＣＰＵ１１１が各種の処理を実行する際に用いるワークエリアを有する。このように、ＲＡＭ１１２は、各種の用途に応じて適宜エリアを提供することができる。
【００１８】
ＲＯＭ１１３は、本コンピュータの設定データやブートプログラムなどを格納する。外部記憶装置１１４は外部記憶装置であり、ハードディスクドライブ装置に代表される大容量情報記憶装置である。外部記憶装置１１４には、ＯＳ（オペレーティングシステム）や本コンピュータが行う後述の各処理をＣＰＵ１１１に実行させるためのプログラムやデータが保存されている。データには、特徴モデルが複数登録されたデータベースが含まれる。これらのプログラムやデータはＣＰＵ１１１による制御に従って適宜ＲＡＭ１１２にロードされる。そしてＣＰＵ１１１はこのロードされたプログラムやデータを用いて処理を実行する。
【００１９】
Ｉ／Ｆ（インタフェース）１１５は、本実施形態の場合には、距離画像撮像装置を接続するためのインタフェースや、外部のネットワーク接続するためのインターフェースとして機能するものである。距離画像撮像装置が撮像した画像は、このＩ／Ｆ１１５を介して本コンピュータのＲＡＭ１１２や外部記憶装置１１４に入力される。バス１１６は、上述の各部を繋ぐバスである。
【００２０】
図２を参照して、本実施形態に係る全体の処理の流れを説明したフローチャートについて説明する。本処理では、検出した２次元無効領域特徴と２次元無効領域特徴データベースとを照合し、照合した２次元無効領域特徴モデルに記述されている撮影時の距離画像撮影装置と対象物体との相対位置姿勢を物体の概略位置姿勢とする。以下、詳細を説明する。
【００２１】
ステップＳ２０１において、距離画像を取得する。
【００２２】
ステップＳ２０２において、取得された距離画像中の無効領域から２次元無効領域特徴を検出し、特徴量を抽出する。無効領域とは、距離が取得できなかった計測点であり、画素値には無効である旨を示す値（例えば“０”など）が格納されている。２次元無効領域特徴の特徴量として、距離画像面から無効領域、無効領域の輪郭、及び重心を抽出する。無効領域及び無効領域の輪郭抽出には、２値化の輪郭抽出に用いられるラベリング手法（特許文献３）を用いることができる。
【００２３】
ステップＳ２０３において、２次元無効領域特徴と２次元無効領域特徴データベースを照合することで、対象物体の初期位置姿勢とする概略位置姿勢を算出する。まず、２次元無効領域特徴データベースより、ステップＳ２０２で抽出された２次元無効領域特徴と類似する２次元無効領域特徴モデルを抽出する。そして、抽出された２次元無効領域特徴モデルに記述されている撮影時の距離画像撮影装置と対象物体の相対位置姿勢を概略位置姿勢として用いる。ステップＳ２０２で抽出された２次元無効領域特徴と類似する２次元特徴モデルを抽出する具体的な方法を、以下で説明する。
【００２４】
ステップＳ２０２で抽出された複数の２次元特徴と２次元特徴モデルに記述された複数の２次元特徴について特徴間の対応を求め、その対応する特徴同士の一致度から類似度を計測する。抽出された２次元特徴と類似する２次元特徴モデルとは、距離画像を撮影したときの視点位置が近く、互いに類似した距離画像と想定される。このため、特徴間の対応は特徴間の距離が近い特徴同士となる。この対応を求めることで、個々の特徴の識別も行っている。
【００２５】
重心を２次元特徴として用いる場合には、まず、ステップＳ２０２で抽出された各重心と２次元特徴モデルの各重心との距離を算出する。ステップＳ２０２で抽出された各重心に対する距離が最小となる２次元特徴モデルの重心を対応する重心とし、対応する重心間の距離の総和を求める。この処理を全２次元特徴モデルに対して行い、距離の総和が最小となるものを最も類似する２次元特徴モデルとする。
【００２６】
次に、輪郭を２次元特徴として用いる場合には、まず、ステップＳ２０２で抽出された各輪郭と２次元特徴モデルの各輪郭間のハウスドルフ距離（特許文献４）を算出する。ステップＳ２０２で抽出された輪郭に対してハウスドルフ距離が最小となる２次元特徴モデルの輪郭を対応する輪郭とし、対応する輪郭間のハウスドルフ距離の総和を求める。この処理を全２次元特徴モデルに対して行い、距離の総和が最小となる２次元特徴モデルを最も類似するモデルとして抽出する。
【００２７】
そして、無効領域を２次元特徴として用いる場合には、まず、ステップＳ２０２で抽出された各無効領域と２次元特徴モデルの各無効領域が重なっているか否かを判定し、両者の無効領域が重なっている領域同士を対応する無効領域とする。ステップＳ２０２で抽出された無効領域の全画素数に対する、両者の無効領域が重複している画素数の比を算出する。つまり，距離画像全体に占める無効領域の割合を算出する。この処理を全２次元特徴モデルに対して行い、この比が最大となる２次元特徴モデルを最も類似するモデルとして抽出する。ステップＳ２０３において、対象物体の概略位置姿勢の算出が完了すると、ステップＳ２０４に進む。
【００２８】
ステップＳ２０４において、位置姿勢算出部１０３はより精度よく位置姿勢を算出する。ステップＳ２０５において、誤差算出部１０４は誤差を算出する。ステップＳ２０６において、ステップＳ２０５で算出された誤差が閾値未満であるか否かが判定される。誤差が閾値未満である場合（ステップＳ２０６；ＹＥＳ）、処理は終了する。一方、誤差が閾値未満でない場合（ステップＳ２０６；ＮＯ）、ステップＳ２０４に戻る。
【００２９】
上記のステップＳ２０４からステップＳ２０５までの各処理については後の実施形態で詳述する。
【００３０】
図３（ａ）を参照して、２次元無効領域特徴データベースの構成例について説明する。距離画像撮影時の対象物体の位置姿勢３０２、及び複数の２次元無効領域特徴３０３が登録された２次元無効領域特徴モデル３０１が視点（対象物体の位置姿勢）ごとに登録されている。対象物体の位置姿勢３０２は、距離画像撮像装置と対象物体との相対位置姿勢とする。２次元無効領域特徴３０３には、輪郭画素位置３０４、輪郭画素数３０５、領域内画素位置３０６、領域内画素数３０７、重心画素３０８が特徴量として記述されている。輪郭画素位置３０４は輪郭に属する画素の画素位置を示す。輪郭画素数３０５は輪郭に属する画素の総画素数を示す。領域内画素位置３０６は領域に属する画素の画素位置を示す。領域内画素数３０７は領域に属する画素の総画素数を示す。そして重心画素３０８は領域の重心画素位置を示す。図３（ｂ）については第２実施形態で説明する。
【００３１】
図４を参照して、上述の２次元特徴データベースを生成する流れを示すフローチャートについて説明する。ステップＳ４０１において、距離画像取得部１０１は距離画像を取得する。
【００３２】
ステップＳ４０２において、特徴抽出部１０２は２次元無効領域特徴を複数検出し、これらの特徴量を抽出する。無効領域は、距離が取得できなかった計測点であり、画素値には無効である旨を示す値（例えば“０”など）が格納されている。２次元無効領域特徴の特徴量として、距離画像面から無効領域、無効領域の輪郭及び重心を抽出する。無効領域及び無効領域の輪郭抽出に、２値化の輪郭抽出に用いられるラベリング手法（特許文献３）を用いる点は図２のフローチャートと同様である。
【００３３】
ステップＳ４０３において、ステップＳ４０２で求めた２次元無効領域特徴と、距離画像装置と対象物体の相対位置姿勢を２次元無効領域特徴モデルとし、２次元無効領域特徴データベースに追加登録する。つまり、特徴量と、位置姿勢を示す情報とが関連付けられて記憶装置等に記憶される。
【００３４】
ステップＳ４０４において、データベース生成を終了する場合には、本処理を終了し、続行する場合にはステップＳ４０５へ進む。
【００３５】
ステップＳ４０５において、距離画像撮像装置を図５で示した球面に沿って、または球面の半径方向に移動して、距離画像撮像装置又は対象物体の位置姿勢を変更する。その後、ステップＳ４０１へ戻る。
【００３６】
これにより２次元特徴データベースが追加登録される。
【００３７】
（第２実施形態）
第２実施形態では、物体の概略位置姿勢が既知であると仮定し、より精度の高い物体の位置姿勢を推定する方法を説明する。処理は大きく２つに分けられる。なお、概略位置姿勢を算出してから精度の高い位置姿勢算出処理を行ってもよい。
【００３８】
第１に、２次元無効領域特徴モデルの各２次元無効領域特徴を、対象物体の３次元形状を表すモデル上の相当する領域で表した特徴に変換した特徴モデルが視点ごとに登録されたデータベースを考える。このデータベースから、物体の概略位置姿勢に近い位置姿勢で生成された３次元特徴モデルを抽出する。以下、２次元無効領域特徴モデルの各２次元無効領域特徴を、対象物体の３次元形状を表すモデル上の相当する領域で表した特徴を「３次元無効領域特徴」と称する。この３次元無効領域特徴に変換した特徴モデルを「３次元無効領域特徴モデル」と称する。そして、この３次元無効領域特徴モデルが視点ごとに登録されたデータベースを「３次元無効領域特徴モデルデータベース」と称する。第２に、抽出された３次元無効領域モデルの各無効領域特徴と、検出された各２次元無効領域特徴との対応をとることで、物体の位置姿勢を算出する。
【００３９】
図３（ｂ）を参照して、上述した３次元特徴データベースの構成例について説明する。パラメータ３５１は距離画像撮影装置に関するパラメータであり、例えば焦点距離、内部歪、画角等である。距離画像撮影時の対象物体の位置姿勢３５３、及び複数の３次元無効領域特徴３５４が登録された３次元無効領域特徴モデル３５２が視点（対象物体の位置姿勢）ごとに登録されている。対象物体の位置姿勢３５３は、距離画像撮像装置と対象物体との相対的な位置姿勢とする。３次元無効領域特徴３５４には、輪郭点位置３５５、輪郭点数３５６、領域内点位置３５７、領域内点数３５８、重心位置３５９が特徴量として記述されている。輪郭画素位置３０４は輪郭に属する画素の画素位置を示す。輪郭画素数３０５は輪郭に属する画素の総画素数を示す。領域内画素位置３０６は領域に属する画素の画素位置を示す。領域内画素位置３０６は領域に属する画素の総画素数を示す。そして重心画素３０８は領域の重心画素位置を示す。
【００４０】
一方、図６（ａ）は、２次元無効領域特徴を対象物体の３次元形状を表すモデル上に投影する様子を示したものである。図６に示すように、視点６０１と距離画像６０２上の２次元無効領域６０５の各点を結ぶ線分６０４と３次元形状モデル６０３とが交差する点を求めることで３次元形状モデル上での３次元無効領域８０６が得られる。以下、具体的に説明する。まず、３次元特徴データベースより、ステップＳ２０３で算出された対象物体の概略位置姿勢と同じ位置姿勢で登録されている３次元特徴モデルを選択する。
【００４１】
次に、３次元特徴モデルに記述されている無効領域の輪郭を、式（１）に示す関係式を用いて距離画像と同じ画像面に射影し、画像面での位置を算出する。u,vは距離画像面での画素位置、ｚは特徴の奥行き方向の距離、ｆは焦点距離、x,y,zは、特徴の３次元位置を示している。式（１）は、距離画像撮像装置に内部歪が存在しない場合を想定した関係式である。内部歪が存在する距離画像装置については、式（２）に示す関係式を用いる。式（２）のa,b,cは、画像中心、焦点距離などから得られる値、d,eは画像中心である。
【００４２】
【数１】
【００４３】
【数２】
【００４４】
次に、３次元特徴モデルに記述された輪郭を投影した輪郭を複数の線分に分割し、各線分ごとに直線の式を算出する。以後、説明のため、３次元特徴モデルに記述された輪郭を投影した輪郭を分割して得られた線分を参照線分と称する。また、ステップＳ２０２で抽出された輪郭を分割して得られた線分を検出線分と称する。
【００４５】
図６（ｂ）は、参照線分と検出線分の関係を説明する図である。図６（ｂ）では、画像の水平方向、垂直方向をそれぞれｕ軸、ｖ軸としている。ある制御点（参照線分上のある１点）の画像上での位置を（ｕ0，ｖ0）と表す。また、該制御点が所属する参照線分の画像上での傾きをu軸に対する傾きをθとして表す。該参照線分の画像上での法線ベクトルは（sinθ，−cosθ）となる。また、該制御点の対応点の画像座標を（ｕ’，ｖ’）とする。対応点は、該参照線分に最も距離が近い検出線分と、参照線分の制御点から伸ばした法線との交点とする。ここで、点（ｕ’，ｖ’）を通り、傾きがθである直線上の点（ｕ，ｖ）は、
【００４６】
【数３】
【００４７】
と表せる（θは定数）。ここで、ｄは以下のような定数である。
【００４８】
【数４】
【００４９】
次に、対象物体の位置及び姿勢（以下、sとする）の微小変化と、画像上での位置姿勢の微小変化の関係式を求める。制御点の画像上での位置は計測対象物体の位置及び姿勢により変化する。また、対象物体の位置及び姿勢の自由度は６自由度である。すなわち、sは６次元ベクトルであり、対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が回転軸を表し、大きさが回転角を表す３次元ベクトルなどによって表現される。位置及び姿勢により変化する制御点の画像座標（ｕ，ｖ）は（ｕ０，ｖ０）の近傍で１次のテイラー展開によって以下の式（５）のように近似できる。但しΔｓｉ（ｉ＝１,２,・・・,６）はsの各成分の微小変化量を表す。
【００５０】
【数５】
【００５１】
次に、式（５）によって近似されるｕ、ｖを式（３）に代入して、以下の式（６）を得る。
【００５２】
【数６】
【００５３】
ただし、
【００５４】
【数７】
【００５５】
ここでｒは定数とした。
【００５６】
次に、すべての制御点とそれに対応する点について式（６）を求め、式（８）に示す線形連立方程式を導き出す。
【００５７】
【数８】
【００５８】
ここで式（８）を式（９）のように表す。
【００５９】
【数９】
【００６０】
式（９）の線形連立方程式の係数行列Ｊを算出するための偏微分係数の算出は、例えば非特許文献１に開示されている方法によって行う。
【００６１】
最後に、式（９）に基づいて、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いてΔsを求める。このΔsをステップＳ２０３で求めた概略位置姿勢に加算することで、ステップＳ２０４における物体の位置姿勢が算出される。
【００６２】
しかしながら、上記特徴は、場合によっては誤って抽出される場合もあるため、次に述べるようなロバスト推定手法をさらに用いてもよい。一般に、誤って抽出される特徴では誤差d−rが大きくなる。そのため式（８）、式（９）に示す連立方程式に対する誤差の寄与度が大きくなり、その結果得られるΔsの精度が低下してしまう。そこで、誤差d−rが大きいデータには小さな重みを与え、誤差d−rが小さいデータには大きな重みを与える。重みは例えば式（１０）に示すようなＴｕｋｅｙの関数により与えることができる。
【００６３】
【数１０】
【００６４】
ここでｃ１は定数である。なお、重みを与える関数は必ずしもＴｕｋｅｙの関数である必要はない。例えばＨｕｂｅｒの関数など、誤差が 大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であればいかなる関数であってもよい。
【００６５】
各データ（エッジまたは点群データ）に対応する重みをｗｉとする。ここで式（１１）に示すように重み行列Ｗを定義する。
【００６６】
【数１１】
【００６７】
重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗｉが入る。この重み行列Ｗを用いて、式（１０）を以下の式（１２）のように変形する。
【００６８】
【数１２】
【００６９】
式（１２）を解くことにより以下の式（１３）に示すように補正値Δsを算定できる。
【００７０】
【数１３】
【００７１】
この誤差の小さなΔsをステップＳ２０３で求めた概略位置姿勢に加算することで、ステップＳ２０４における物体の位置姿勢がより精度よく算出される。
【００７２】
次に誤差の判定手法について説明する。ステップＳ２０５において、式（１４）に示すように誤差ｄ−ｒの２乗和σを算出する。ここで、ｎは制御点数である。
【００７３】
【数１４】
【００７４】
ステップＳ２０６において、式（１４）を用いて誤差を判定する。誤差の２乗和が予め定められた閾値未満であった場合には（ステップＳ２０６；ＹＥＳ）、処理を終了する。一方、閾値を超えていた場合には（ステップＳ２０６；ＮＯ）、ステップＳ２０５で算出された対象物体の位置姿勢を初期位置として、ステップＳ２０４へ戻る。
【００７５】
ステップＳ２０４では、無効領域の輪郭を特徴として用いた場合の位置姿勢算出方法を説明した。以下では、無効領域または重心を特徴として用いる場合について説明する。
【００７６】
まず、３次元特徴データベースより、ステップＳ２０２で算出された対象物体の概略位置姿勢と同じ位置姿勢で登録されている３次元特徴モデルを抽出する。
【００７７】
次に、３次元特徴モデルの無効領域の重心または無効領域内に属する各点を、式（１）または式（２）の関係式を用いて距離画像と同じ画像面に射影したときの位置を算出する。
【００７８】
次に、式（１５）に示すように、対象物体の位置及び姿勢（以下、sとする）の微小変化と、画像上での位置姿勢の微小変化の関係式を求める。
【００７９】
【数１５】
【００８０】
ｕ０、ｖ０は、３次元特徴モデルの各重心または領域内に属する各点を射影した点、ｕ、ｖはｕ０、ｖ０から最も距離の近いステップＳ２０２で抽出された重心または領域内に属する点とする。つまり、ｕ−ｕ０、ｖ−ｖ０は、３次元特徴モデルの各重心または領域内に属する各点を射影した各点と、これと最も近い距離にあるステップＳ２０２で抽出された重心または領域内に属する点との距離となる。
【００８１】
次に、すべての重心または領域内の点について式（１５）を求めて、式（１６）を得る。
【００８２】
【数１６】
【００８３】
∂ｕｎ／∂ｓｉ、∂ｖｎ／∂ｓｉは、式（１）又は式（２）で表した関係式を、ｓiで偏微分した解となる。
【００８４】
次に、式（１６）に対して式（８）と同様の方法を用いることで補正量Δsを求める。そして、ステップＳ２０３で求めた概略位置姿勢に補正量Δsを加算することで、物体の位置姿勢が算出される。
【００８５】
ステップＳ２０３での概略位置姿勢算出は、２次元特徴及び２次元特徴データベースを用いた方法に限定しない。概略位置姿勢が求められる方法で適用可能なものであれば、何でもよい。例えば、対象物体に位置姿勢計測センサを取り付け、このセンサから得られる位置姿勢を初期値として用いてもよい。また、ステップＳ２０４以降の処理をせず、対象物体の初期位置を求めた時点で処理を終了してもよい。
【００８６】
図４を参照して、上述の２次元特徴データベース及び３次元特徴データベースを生成する流れを示すフローチャートについて説明する。
【００８７】
ステップＳ４０１において、距離画像を取得する。ステップＳ４０２において、距離画像が無効となる領域から特徴を抽出する。抽出方法は、距離画像中の無効となる領域をラベリング手法（特許文献３）を用いて、距離画像面での領域、輪郭を求める。また、求められた領域からその重心も求める。そして求められた無効領域、無効領域の輪郭、及び無効領域の重心を２次元特徴とする。
【００８８】
ステップＳ４０３において、ステップＳ４０２で求められた２次元特徴と距離画像装置との相対位置姿勢をセットにした２次元特徴モデルを、２次元特徴データベースに追加登録する。また、２次元特徴を３次元に変換した特徴も同様に、距離画像との相対位置姿勢とセットにして、３次元特徴データベースに追加登録する。
【００８９】
ステップＳ４０４において、データベース生成を終了する場合には、本処理を終了し、続行する場合にはステップＳ４０５へ進む。
【００９０】
ステップＳ４０５において、距離画像撮像装置を、図５で示した球面に沿って移動する。または、図５で示した球面の半径の大きさを、半径方向に距離撮像装置の撮影可能範囲内で移動させる。距離画像装置又は対象物体の位置姿勢を変更してステップＳ４０１に戻る。
【００９１】
（第３実施形態）
第３の実施形態では、無効領域から抽出した特徴に加えて距離画像の距離値が有効な領域（以下、「有効領域」と称する。）から抽出した特徴を用いて位置姿勢を求める場合を説明する。
【００９２】
図７を参照して、本実施形態に係る処理の流れを示すフローチャートについて説明する。ステップＳ７０１において、距離画像を取得する。ステップＳ７０２において、距離画像中の無効領域から２次元特徴を抽出する。さらに有効領域を抽出する。２次元特徴として、距離画像面から無効領域、無効領域の輪郭及び重心を抽出する。無効領域及び無効領域の輪郭抽出には、２値化の輪郭抽出に用いられるラベリング手法（特許文献３）を用いることができる。有効領域は、距離が取得できた計測点であり、画素値には３次元位置が格納されている。
【００９３】
ステップＳ７０３において、２次元特徴及び２次元特徴データベースを用いて対象物体の概略位置姿勢を算出する。算出方法は、ステップＳ２０３と同様である。
【００９４】
ステップＳ７０４において、ステップＳ７０３で求められた概略位置姿勢から、有効領域、２次元特徴及び３次元特徴データベースを用いて対象物体の位置姿勢を求める。算出方法は、まず、有効領域の点群に関して、複数の距離画像の位置合わせに用いられるＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）の手法（非特許文献１参照）を用いて対象物体の位置姿勢を求める。
【００９５】
次に、ＩＣＰによって求められた位置姿勢を概略位置姿勢とし、ステップＳ２０４と同様の方法で３次元特徴及び３次元特徴データベースを用いて対象物体の位置姿勢を算出する。さらに、これらの処理を繰り返し行っても良い。
【００９６】
図８を参照して、有効領域が特徴として追加された３次元特徴データベースについて説明する。３次元特徴（有効領域）は、点の３次元位置８０１及び面を表すインデックス８０２から構成される。その他の構成は図３（ｂ）と同様であり、同一の参照番号を用いている。
【００９７】
さらにステップＳ７０５における誤差を算出する処理、ステップＳ７０６の誤差の閾値との比較処理を行ってもよい。
【００９８】
（第４実施形態）
第４実施形態では、無効領域の特徴のみを用いた結果、無効領域と有効領域の両方の特徴を用いた結果、又は有効領域の特徴のみを用いた結果をそれぞれ比較することで、求められた位置姿勢の妥当性を評価する方法を説明する。
【００９９】
図９を参照して、本実施形態の処理の流れを示すフローチャートについて説明する。
【０１００】
ステップＳ９０１において、距離画像を取得する。ステップＳ９０２において、ステップＳ７０２と同様に、距離画像中の無効領域から２次元特徴を抽出する。さらに有効領域を抽出する。
【０１０１】
ステップＳ９０３において、２次元特徴及び２次元特徴データベースを用いて対象物体の概略位置姿勢を決定する。概略位置姿勢決定方法は、ステップＳ２０３と同様である。
【０１０２】
ステップＳ９０４において、ステップＳ２０４及びステップＳ７０４を実行し、対象物体の位置姿勢をそれぞれ算出する。またこのとき、有効領域のみを用いた位置姿勢算出も行う。つまり、合計３つの位置姿勢が算出されることになる。
【０１０３】
ステップＳ９０５において、ステップＳ９０４で求められた３つの位置姿勢について、それぞれ誤差を算出する。誤差の算出方法は、ステップＳ２０５と同様である。
【０１０４】
ステップＳ９０６において、ステップＳ９０５で求められた誤差が予め定められた閾値以内か否かを判定する。２つの位置姿勢の誤差がともに閾値内となった場合には（ステップＳ９０６；ＹＥＳ）、ステップＳ９０７へ進む。それ以外の場合には（ステップＳ９０６；ＮＯ）、閾値を超えた位置姿勢に関して、ステップＳ９０４に戻り、再度位置姿勢算出を実行する。
【０１０５】
ステップＳ９０７において、ステップＳ９０４で求められた３つの位置姿勢を互いに比較し、求められた位置姿勢の妥当性を検証する。具体的には、両者の位置姿勢の差を計算し、この差が予め定められた閾値内であるか否かを判定する。閾値の範囲以下である場合には、無効領域の特徴のみを用いて求めた位置姿勢（ステップＳ２０４を実行することで得られた位置姿勢）、無効領域と有効領域の両方の特徴を用いて求めた位置姿勢（ステップＳ７０４を実行することで得られた位置姿勢）、又は有効領域のみを用いて求めた位置姿勢の何れかを対象物体の位置姿勢とする。
【０１０６】
なお、３つの位置姿勢すべてを求めるのではなく、３つのうち２つの位置姿勢だけを求めて、両者を比較することにより検証してもよい。
【０１０７】
（第５実施形態）
第５実施形態では、物体の識別子と共に無効領域の２次元特徴が記述された２次元特徴モデルが登録されている２次元特徴モデルデータベースを用いて、物体の認識を行う場合について説明する。
【０１０８】
図１０を参照して、本実施形態に係る２次元特徴モデルデータベースの構成例について説明する。２次元特徴モデル１００１は、対象物体の識別子１００２及び複数の２次元特徴１００３が記述されている。２次元特徴１００３は、輪郭に属する画素の画素位置を示した輪郭画素位置１００４、輪郭に属する画素の総画素数を示した輪郭画素数１００５、及び領域に属する画素の画素位置を示した領域内画素位置１００６を備える。さらに、領域に属する画素の総画素数を示した領域内画素数１００７、及び領域の重心画素位置を示した重心画素１００８を備える。
【０１０９】
図１１を参照して、本実施形態に係る処理の流れを示すフローチャートについて説明する。
【０１１０】
ステップＳ１１０１において、距離画像を取得する。
【０１１１】
ステップＳ１１０２において、ステップＳ２０２と同様に、取得された距離画像から無効領域の特徴を抽出する。
【０１１２】
ステップＳ１１０３において、物体を識別する。識別方法は、図１０に示した２次元特徴データベースからステップＳ１１０２で抽出した特徴と類似する２次元特徴モデルを抽出して、この２次元特徴モデルに記述されている識別子から、物体を識別する。２次元特徴モデルを抽出する方法は、ステップＳ２０３で述べた方法と同様である。
【０１１３】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【特許請求の範囲】
【請求項１】
３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
前記対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像手段と、
前記距離画像から、前記撮像手段と前記対象物体との距離情報が取得できない領域である無効領域を選別する選別手段と、
前記無効領域の特徴を示す特徴量を抽出する抽出手段と、
前記距離画像を撮像した時の前記撮像手段と前記対象物体との相対的な位置姿勢を示す情報と、前記撮像手段により複数の異なる方向から撮像された前記距離画像ごとの前記特徴量とを関連付けて記憶する記憶手段と、
前記抽出手段により抽出された特徴量に対応する前記記憶手段に記憶された特徴量を求めて、
当該求められた特徴量と関連付けられた前記位置姿勢を示す情報から前記対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項２】
前記特徴量は、前記距離画像における前記無効領域の位置、前記無効領域の重心の位置、又は前記無効領域の輪郭の位置の少なくとも１つを示す情報であることを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項３】
前記特徴量が前記無効領域の位置を示す情報である場合、
前記概略位置姿勢決定手段は、前記抽出手段により前記距離画像から抽出された無効領域の位置及び前記記憶手段に記憶された無効領域の位置が重複する領域の距離画像全体の領域に占める割合が最大となる無効領域の位置を示す情報を求めて、
当該求められた無効領域の位置を示す情報に関連付けられた対象物体の位置姿勢を示す情報から前記対象物体の概略位置姿勢を決定することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項４】
前記特徴量が前記無効領域の重心の位置を示す情報である場合、
前記概略位置姿勢決定手段は、前記抽出手段により前記距離画像から抽出された無効領域の各重心の位置を示す情報と、前記記憶手段に記憶された無効領域の各重心の位置を示す情報とから、対応する重心間の距離の総和を算出し、
前記距離の総和が最小となる重心の位置を示す情報を求めて、
当該求められた重心の位置を示す情報に関連付けられた対象物体の位置姿勢を示す情報から前記対象物体の概略位置姿勢を決定することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項５】
前記特徴量が前記無効領域の輪郭の位置を示す情報である場合、
前記概略位置姿勢決定手段は、前記抽出手段により前記距離画像から抽出された無効領域の輪郭を示す情報と、前記記憶手段に記憶された無効領域の輪郭を示す情報とに基づいて、前記記憶手段に記憶された無効領域の輪郭の位置を示す情報に関連付けられた対象物体の位置姿勢を示す情報から前記対象物体の概略位置姿勢を決定することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項６】
前記撮像手段により撮像された距離画像における前記無効領域を、前記対象物体の３次元の形状を示す３次元形状モデル上に投影した３次元無効領域の位置、輪郭の位置、又は重心の位置の少なくとも１つを示す情報と、前記対象物体の位置姿勢を示す情報とを関連付けて登録する登録手段と、
前記概略位置姿勢決定手段により決定された前記対象物体の概略位置姿勢に対応する位置姿勢で前記登録手段に登録されている、３次元形状モデルを選択する選択手段と、
前記選択手段により選択された３次元形状モデルの３次元無効領域を前記距離画像と同じ画像面に射影する射影手段と、
前記射影手段により前記画像面に射影された無効領域と前記撮像手段により撮像された距離画像における無効領域とのそれぞれにおいて、無効領域の位置、輪郭の位置、又は重心の位置の少なくとも１つを構成する各点から、前記対象物体の前記概略位置姿勢からの変化量を算定する算定手段と、
前記概略位置姿勢と前記変化量とから前記対象物体の位置姿勢を決定する位置姿勢決定手段と、
をさらに備えることを特徴とする請求項３乃至５の何れか１項に記載の位置姿勢計測装置。
【請求項７】
３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測方法であって、
撮像手段が、前記対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像工程と、
選別手段が、前記距離画像から、前記撮像工程と前記対象物体との距離情報が取得できない領域である無効領域を選別する選別工程と、
抽出手段が、前記無効領域の特徴を示す特徴量を抽出する抽出工程と、
記憶手段が、前記距離画像を撮像した時の前記撮像工程と前記対象物体との相対的な位置姿勢を示す情報と、前記撮像工程により複数の異なる方向から撮像された前記距離画像ごとの前記特徴量とを関連付けて記憶する記憶工程と、
概略位置姿勢決定手段が、前記抽出工程により抽出された特徴量に対応する前記記憶工程で記憶された特徴量を求めて、
当該求められた特徴量と関連付けられた前記位置姿勢を示す情報から前記対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定工程と、
を有することを特徴とする位置姿勢計測方法。
【請求項８】
請求項７に記載の位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【請求項１】
３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
前記対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像手段と、
前記距離画像から、前記撮像手段と前記対象物体との距離情報が取得できない領域である無効領域を選別する選別手段と、
前記無効領域の特徴を示す特徴量を抽出する抽出手段と、
前記距離画像を撮像した時の前記撮像手段と前記対象物体との相対的な位置姿勢を示す情報と、前記撮像手段により複数の異なる方向から撮像された前記距離画像ごとの前記特徴量とを関連付けて記憶する記憶手段と、
前記抽出手段により抽出された特徴量に対応する前記記憶手段に記憶された特徴量を求めて、
当該求められた特徴量と関連付けられた前記位置姿勢を示す情報から前記対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項２】
前記特徴量は、前記距離画像における前記無効領域の位置、前記無効領域の重心の位置、又は前記無効領域の輪郭の位置の少なくとも１つを示す情報であることを特徴とする請求項１に記載の位置姿勢計測装置。
【請求項３】
前記特徴量が前記無効領域の位置を示す情報である場合、
前記概略位置姿勢決定手段は、前記抽出手段により前記距離画像から抽出された無効領域の位置及び前記記憶手段に記憶された無効領域の位置が重複する領域の距離画像全体の領域に占める割合が最大となる無効領域の位置を示す情報を求めて、
当該求められた無効領域の位置を示す情報に関連付けられた対象物体の位置姿勢を示す情報から前記対象物体の概略位置姿勢を決定することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項４】
前記特徴量が前記無効領域の重心の位置を示す情報である場合、
前記概略位置姿勢決定手段は、前記抽出手段により前記距離画像から抽出された無効領域の各重心の位置を示す情報と、前記記憶手段に記憶された無効領域の各重心の位置を示す情報とから、対応する重心間の距離の総和を算出し、
前記距離の総和が最小となる重心の位置を示す情報を求めて、
当該求められた重心の位置を示す情報に関連付けられた対象物体の位置姿勢を示す情報から前記対象物体の概略位置姿勢を決定することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項５】
前記特徴量が前記無効領域の輪郭の位置を示す情報である場合、
前記概略位置姿勢決定手段は、前記抽出手段により前記距離画像から抽出された無効領域の輪郭を示す情報と、前記記憶手段に記憶された無効領域の輪郭を示す情報とに基づいて、前記記憶手段に記憶された無効領域の輪郭の位置を示す情報に関連付けられた対象物体の位置姿勢を示す情報から前記対象物体の概略位置姿勢を決定することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項６】
前記撮像手段により撮像された距離画像における前記無効領域を、前記対象物体の３次元の形状を示す３次元形状モデル上に投影した３次元無効領域の位置、輪郭の位置、又は重心の位置の少なくとも１つを示す情報と、前記対象物体の位置姿勢を示す情報とを関連付けて登録する登録手段と、
前記概略位置姿勢決定手段により決定された前記対象物体の概略位置姿勢に対応する位置姿勢で前記登録手段に登録されている、３次元形状モデルを選択する選択手段と、
前記選択手段により選択された３次元形状モデルの３次元無効領域を前記距離画像と同じ画像面に射影する射影手段と、
前記射影手段により前記画像面に射影された無効領域と前記撮像手段により撮像された距離画像における無効領域とのそれぞれにおいて、無効領域の位置、輪郭の位置、又は重心の位置の少なくとも１つを構成する各点から、前記対象物体の前記概略位置姿勢からの変化量を算定する算定手段と、
前記概略位置姿勢と前記変化量とから前記対象物体の位置姿勢を決定する位置姿勢決定手段と、
をさらに備えることを特徴とする請求項３乃至５の何れか１項に記載の位置姿勢計測装置。
【請求項７】
３次元形状が既知である対象物体の３次元の位置姿勢を計測する位置姿勢計測方法であって、
撮像手段が、前記対象物体までの距離情報が画素値として保持された距離画像を撮像する撮像工程と、
選別手段が、前記距離画像から、前記撮像工程と前記対象物体との距離情報が取得できない領域である無効領域を選別する選別工程と、
抽出手段が、前記無効領域の特徴を示す特徴量を抽出する抽出工程と、
記憶手段が、前記距離画像を撮像した時の前記撮像工程と前記対象物体との相対的な位置姿勢を示す情報と、前記撮像工程により複数の異なる方向から撮像された前記距離画像ごとの前記特徴量とを関連付けて記憶する記憶工程と、
概略位置姿勢決定手段が、前記抽出工程により抽出された特徴量に対応する前記記憶工程で記憶された特徴量を求めて、
当該求められた特徴量と関連付けられた前記位置姿勢を示す情報から前記対象物体の３次元の概略位置姿勢を決定するする概略位置姿勢決定工程と、
を有することを特徴とする位置姿勢計測方法。
【請求項８】
請求項７に記載の位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１１−１７４８９１（Ｐ２０１１−１７４８９１Ａ）
【公開日】平成２３年９月８日（２０１１．９．８）
【国際特許分類】
【出願番号】特願２０１０−４１００１（Ｐ２０１０−４１００１）
【出願日】平成２２年２月２５日（２０１０．２．２５）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２３年９月８日（２０１１．９．８）
【国際特許分類】
【出願日】平成２２年２月２５日（２０１０．２．２５）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]