JP2015135333A - 情報処理装置、情報処理装置の制御方法、およびプログラム - Google Patents

Abstract

【課題】対象物体の位置及び姿勢の算出を高速化できるようにした３次元計測装置を提供する。【解決手段】３次元計測装置１０は、対象物体の３次元形状モデルの幾何特徴上の点を選択し、対象物体の位置及び姿勢を示す概略値と距離画像の撮影時の撮影パラメータとに基づいて当該選択された幾何特徴上の複数の点を距離画像上に投影し、投影点から所定範囲内の領域で３次元形状モデルの幾何特徴に対応する距離画像上の幾何特徴を探索して対応付ける。そして、当該対応付けられた３次元形状モデルの幾何特徴と距離画像上の幾何特徴との３次元空間における距離の差を用いて対象物体の位置及び姿勢を算出する。【選択図】図１

Description

本発明は、３次元計測装置、処理方法及びプログラムに関する。

近年、ロボット技術の発展とともに、これまで人間が行なっていた複雑なタスク（例えば、工業製品の組立）をロボットが代わりに行なうようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行なう。

ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測（推定）する必要がある。このような位置及び姿勢の計測は、ロボットが部品を把持する場合だけでなく、ロボットが自律移動するための自己位置推定や、拡張現実感における現実空間と仮想物体との位置合わせなど様々な目的に利用される。

位置及び姿勢の計測では、カメラが撮影する２次元画像や距離センサから得られる距離画像を利用する手法が知られている。例えば、モデルフィッティングによる計測が知られている。この計測においては、２次元画像上から検出される特徴や距離画像に対して、物体の３次元形状モデルを当てはめる。

２次元画像に対するモデルフィッティングでは、３次元形状モデルを画像上に投影した投影像を、２次元画像から検出された特徴に当てはめることにより位置及び姿勢を計測する。距離画像に対するモデルフィッティングでは、距離画像の各点を３次元座標を持つ３次元点群に変換し、３次元空間において、当該３次元点群に３次元形状モデルを当てはめることにより位置及び姿勢を計測する。

２次元画像を用いて位置及び姿勢を計測する方法として、エッジを利用してカメラの位置及び姿勢を計測する手法が知られている（非特許文献１）。この方法では、物体の３次元形状を線分の集合（ワイヤフレームモデル）で表し、画像上で検出されるエッジに３次元の線分の投影像を当てはめることにより、物体の位置及び姿勢を計測する。具体的には、まず、位置及び姿勢の概略値をもとに３次元の線分を画像上へ投影し、投影された線分の近傍でエッジ検出を行なう。次に、位置及び姿勢の概略値に基づいた線分の投影像とそれに対応するエッジとの画像上における距離の総和が最小化されるように、非線形最適化により対象物体の位置及び姿勢を計測する。

一方、距離画像を用いて位置及び姿勢を計測する方法として、ＩＣＰ（Iterative Closest Point）アルゴリズムを利用する手法が知られている（非特許文献２）。この方法では、距離画像から変換された３次元点群データに物体の３次元形状モデルをあてはめることにより、物体の位置及び姿勢を計測する。位置及び姿勢の概略値に基づいて各３次元点に最も近い３次元形状モデルの幾何特徴の探索と、点と３次元形状モデルの幾何特徴との距離の総和が最小となるような位置及び姿勢の更新を繰り返し行なう。

距離画像を用いて物体の位置及び姿勢を計測する手法では、点群データの各点に対応する３次元形状モデルの面の探索時に高負荷な演算処理が生じる。これに対処するため、特許文献１には、複数の距離画像の位置合わせ時の対応探索処理を高速化した技術が開示されている。この手法では、距離画像にインデックス値を割り当てた微小面（メッシュ）を当てはめ、インデックス値を重複しない色に変換して各メッシュの色とし、距離画像の撮影位置及び姿勢に基づいてメッシュを描画したインデックス画像を生成する。そして、別の距離画像から変換された３次元点群を、撮影時の位置及び姿勢に基づいてインデックス画像に投影し、投影された３次元点の座標値から対応するインデックス画像上の色を取得する。その後、当該取得した色に対応するインデックス値を逆変換することによってメッシュと距離画像から変換した３次元点とを対応付ける。これにより、対応探索の高速化を実現している。

特開２００６−２０２１５２号公報

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. 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.

上述したインデックス画像を用いた位置合わせ手法（特許文献１）では、距離画像同士を位置合わせするため、距離画像から抽出した全ての３次元点を２次元画像に投影して対応付けを行なう。そのため、例えば、対象物体以外の物体を含む画像の中から当該対象物体の位置及び姿勢を計測する場合、対象物体以外の計測点を含む全ての計測点を密に投影しなければならないので、処理に無駄が多い。

また、特許文献１の手法の場合、３次元形状モデルによっては、メッシュが投影時につぶれないように当該モデルを再メッシュ化する必要がある。そのため、対象物体のＣＡＤモデルを３次元形状モデルとしてそのまま利用することは難しい。また更に、メッシュを２次元平面に高速に描画するには、ＧＰＵ（Graphics Processing Unit）などの専用のハードウェアが必要となる。

また更に、非特許文献１の手法の場合、３次元形状モデルの幾何特徴である線分を、対象物体の裏面（計測されない部分）にある線分も含めて全て画像上へ投影しなければならない。そのため、非特許文献１の手法の場合にも、処理に無駄が生じる。

ここで、２次元画像を用いる手法は、例えば、直線を基調としたような人工的な物体が多い環境などに適しており、距離画像を利用する手法は、例えば、滑らかな面を複数持つような物体に適しているといえる。

２次元画像を用いる場合と、距離画像を用いる場合では、計測される情報の性質が異なるため、２次元画像へのモデルフィッティングと距離画像へのモデルフィッティングとを組み合わせることにより位置及び姿勢の計測精度の向上が期待できる。２次元画像へのモデルフィッティングでは、上述した通り、２次元画像に対してエッジなどの３次元形状モデルの幾何特徴を投影し、対応する２次元画像上の幾何特徴を探索する処理を行なう。距離画像へのモデルフィッティングでは、上述した通り、３次元形状モデルの幾何特徴ではなく計測点を投影する。すなわち、両者における対応付けの方法は異なっている。

そのため、同じ枠組みで対応付け処理を行なうことができず、２次元画像と距離画像とを併用して対象物体の位置及び姿勢を計測する際には、それぞれ独立に対応付けを行わなければならなかった。

本発明は、上記課題に鑑みてなされたものであり、３次元形状モデルと計測データとの対応付け時に実施する演算の効率化を図り、対象物体の位置及び姿勢の算出を高速化できるようにした技術を提供する。

上記課題を解決するため、本発明の一態様による３次元計測装置は、各画素が対象物体との間の距離を示す距離画像を取得する画像取得手段と、前記対象物体の位置及び姿勢を示す概略値を取得する概略値取得手段と、前記対象物体の３次元形状モデルの幾何特徴上の点を選択する選択手段と、前記対象物体の位置及び姿勢を示す概略値と前記距離画像の撮影時の撮影パラメータとに基づいて前記選択手段により選択された幾何特徴上の複数の点を前記距離画像上に投影し、該投影点から所定範囲内の領域で前記３次元形状モデルの幾何特徴に対応する前記距離画像上の幾何特徴を探索して対応付ける対応探索手段と、前記対応探索手段により対応付けられた前記３次元形状モデルの幾何特徴と前記距離画像上の幾何特徴との３次元空間における距離の差を用いて前記対象物体の位置及び姿勢を算出する位置姿勢算出手段とを具備する。

本発明によれば、３次元形状モデルと計測データとの対応付け時に実施する演算の効率化を図れる。これにより、対象物体の位置及び姿勢の算出速度を向上させられる。

本発明の一実施の形態に係わる３次元計測装置の構成の一例を示す図。 図１に示す対応探索部１５０による対応付け処理の一例を示す図。 図１に示す対応探索部１５０による対応付け処理の一例を示す図。 図１に示す３次元計測装置１０における動作の一例を示すフローチャート。 実施形態２に係わる対応付け処理の一例を示す図。

以下、図面を参照して本発明の好適な実施形態について詳細に説明する。

（実施形態１）
図１は、本発明の一実施の形態に係わる３次元計測装置の構成の一例を示している。なお、３次元計測装置１０には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御手段、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていても良い。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。

３次元計測装置１０は、３次元形状モデルの幾何特徴上の（任意の）点を２次元平面に投影し、当該２次元平面上における投影点から所定範囲内の領域にある２次元平面（距離画像）上の幾何特徴を探索する。これにより、３次元形状モデルの幾何特徴と距離画像上の幾何特徴とを対応付け、その対応付けに基づいて対象物体の位置及び姿勢を算出する。

ここで、３次元計測装置１０は、画像取得部１１０と、概略値取得部１２０と、モデル保持部１３０と、幾何特徴選択部１４０と、対応探索部１５０と、位置姿勢算出部１６０とを具備して構成される。

画像取得部１１０は、距離画像を取得する。本実施形態においては、画像取得部１１０は、例えば、距離画像を撮影する距離センサにより実現されるが、これに限られず、外部の距離センサにより撮影された距離画像を単に取得する構成であっても良い。距離画像は、各画素が奥行きの情報（距離情報）を持つ画像である。距離センサには、対象に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式を採用すれば良い。なお、距離センサは、距離画像を撮影できればよく、その方式は特に問わない。例えば、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式を採用しても良いし、ステレオカメラが撮影した画像から三角測量によって各画素の奥行きを計算するパッシブ式を採用しても良い。また、距離センサの撮影パラメータ（撮影時の画角や解像度、焦点距離など）は、既知であるものとする。撮影パラメータは、後述する対応探索部１５０において利用される。

概略値取得部１２０は、３次元計測装置１０に対する物体の位置及び姿勢の概略値を取得する。本実施形態において、３次元計測装置１０に対する物体の位置及び姿勢とは、画像取得部１１０を基準とした物体の位置及び姿勢を表すが、必ずしも画像取得部１１０を基準にする必要はない。例えば、画像取得部１１０の座標系に対する物体の相対的な位置及び姿勢が既知であり、且つその位置及び姿勢が変化しないのであれば、３次元計測装置１０におけるその他の部分を基準としても良い。

また、本実施形態においては、物体の位置及び姿勢の概略値として、３次元計測装置１０が当該物体から過去（例えば、直前）に計測した計測値を用いるが、物体の位置及び姿勢の概略値は、必ずしもこのような値である必要はない。例えば、過去に計測した物体の位置及び姿勢の計測値に対して時系列フィルタリング処理（例えば、線形フィルタ、カルマンフィルタ）を実施し、物体の運動速度や角速度を推定する。そして、その推定結果に基づいて物体の位置及び姿勢を予測した値を概略値としても良い。また、センサから得られる物体の位置及び姿勢を概略値としても良い。ここで、センサは、物体の位置及び姿勢を６自由度で計測できればよく、その方式（例えば、磁気式、光学式、超音波式）は、特に問わない。なお、物体の置かれているおおよその位置や姿勢が予め分かっているのであれば、その値を概略値として用いても良い。また更に、物体を含む情景を撮影した撮影画像や距離画像に対して物体認識処理を実施することにより、物体の位置及び姿勢の概略値を計測するようにしても良い。

モデル保持部１３０は、対象物体（単に物体と呼ぶ場合もある）の３次元幾何モデルデータ（以下、３次元形状モデルと呼ぶ場合もある）を保持する。３次元形状モデルは、従来同様のものを用いればよいため、ここでは詳細な説明については省略するが、例えば、点の集合や各点を結んで構成される面の情報、面を構成する線分の情報によって定義される。

幾何特徴選択部１４０は、対象物体の３次元形状モデルから任意の幾何特徴上の点を選択する。本実施形態においては、３次元形状モデルは、複数のＮＵＲＢＳ（Non-Uniform Rational B-Spline）曲面で構成されているものとし、当該モデルの幾何特徴は、各ＮＵＲＢＳ曲面上から均一にサンプリングした点と法線との組とする。勿論、これに限られず、例えば、３次元形状モデルの幾何特徴は、ＮＵＲＢＳ曲面そのものであっても良いし、３次元形状モデルがメッシュ表現されていれば、メッシュであっても良い。

また、幾何特徴選択部１４０では、３次元形状モデルや計測データから計測し易い幾何特徴や、位置及び姿勢の算出時の計算に有効な幾何特徴が予め規定された情報に基づいて上記選択を行なっても良い。すなわち、後述する位置姿勢算出部１６０において、対象物体の位置及び姿勢を算出する際に利用可能な幾何特徴であれば良い。幾何特徴選択部１４０により任意の幾何特徴を選択するため、対象物体以外の物体が撮影された画像に対して位置及び姿勢の計測を行なう場合であっても、計算量を増やすことなく対応付けを行なえる。

幾何特徴選択部１４０では、距離センサの撮影パラメータと、概略値取得部１２０により取得された物体の位置及び姿勢の概略値とに基づいて３次元形状モデルの幾何特徴上の点を選択する。これにより、距離画像から計測可能な幾何特徴のみが選択される。

具体的には、３次元形状モデルを全方向から描画し、各方向から見える３次元形状モデルの幾何特徴を各方向に対応付けて登録する。これにより、幾何特徴選択部１４０では、物体の位置及び姿勢の概略値と撮影パラメータとから算出される視線ベクトルに最も近い方向に登録された幾何特徴を選択する。なお、各方向のベクトルと幾何特徴の法線との内積を比較し、方向ベクトルと幾何特徴の法線とが対向している幾何特徴のみを登録するようにしても良い。

なお、幾何特徴選択部１４０による幾何特徴上の点の選択は、ユーザ指示に基づいて行なわれても良い。すなわち、ユーザが、対象物体の３次元形状モデルが表示されたＧＵＩを参照しながら、手動で選択しても良い。また、各幾何特徴の表面の中心を３次元形状モデルの幾何特徴上の点として選択しても良い。その他、「A.E. Johnson and M. Hebert, “Efficient multiple model recognition in cluttered 3-d scenes, " Proc. Computer Vision and Pattern Recognition, pp. 671-677, 1998.］で開示されているような特徴検出器により検出される点を３次元形状モデルの幾何特徴上の点として選択しても良い。また更に、概略値取得部１２０により取得された物体の位置及び姿勢の概略値に基づいて、距離画像上で均一になるような点を選択しても良い。具体的には、物体の位置及び姿勢の概略値と距離センサの撮影パラメータとに基づき３次元形状モデルを２次元画像上に投影し、２次元画像上で均一にサンプリングした３次元形状モデル上の点を３次元空間に逆投影する。これにより、距離画像上で均一になる幾何特徴上の点を算出しそれを選択する。その他、３次元形状モデルの表面上から当該モデルの幾何特徴上の点を選択するのであれば、その方法は特に問わない。

対応探索部１５０は、３次元形状モデルの幾何特徴と、距離画像上の幾何特徴とを対応付ける。そして、当該対応付けた３次元形状モデルの幾何特徴と、距離画像上の幾何特徴との組を出力する。

対応探索部１５０では、図２に示すように、幾何特徴選択部１４０により選択された点２４０を２次元平面（投影面）２２０に投影する。なお、この投影は、距離画像の既知である撮影パラメータ（画角や解像度、焦点距離など）と、概略値取得部１２０により取得された物体の位置及び姿勢の概略値とに基づいて行なわれる。「式１」は、焦点距離がｆである場合に、幾何特徴選択部１４０により選択された点の３次元座標（ｘ，ｙ，ｚ）を２次元平面（投影面）上の座標（ｕ，ｖ）に透視投影変換する式である。
［式１］
（ｕ，ｖ）^Ｔ＝（ｆ・ｘ／ｚ，ｆ・ｙ／ｚ）^Ｔ

投影面と距離画像とにおける各画素は、一意に対応しており、投影面に投影された点（投影点）に対応する距離画像上の２次元座標の近傍２７０から、対応付け対象となる距離画像上の幾何特徴が探索される。点の投影計算は、「式１」により計算できるため、対応探索も投影点周辺の２次元探索で済む。そのため、計算量が少なくて済むので、３次元形状モデルの幾何特徴と距離画像上の幾何特徴との対応付けを高速化できる。

距離画像上の幾何特徴の探索は、図３に示すように、投影点を中心とした楕円領域で形成される領域Ω３３０内で行なわれる。なお、探索範囲は、投影点の画素であっても良いし、任意の大きさの矩形形状であっても良い。探索領域Ω３３０の大きさや形状は、例えば、物体の位置及び姿勢の概略値、画像取得部１１０の撮影パラメータ、物体又は画像取得部１１０の動き、の少なくともいずれかに基づいて決定すればよい。すなわち、探索領域Ω３３０は、画像取得部１１０の視線方向と幾何特徴の法線方向とに基づいて決められた矩形領域や楕円領域であっても良いし、動きベクトルを長軸とする楕円領域であっても良い。

本実施形態においては、距離画像上の幾何特徴は、投影点に対応する画素に含まれる奥行き情報（距離情報）から得られる３次元点の座標とする。距離画像上の幾何特徴の探索方法としては、計測点３４０の投影点から最短距離を持つ探索領域Ω３３０内の計測点を選択すれば良い。「式２」は、投影点ｐ＝（ｕ，ｖ）^Ｔで計測された計測点の距離画像上での座標ｇ_ｉの最短距離を求める式である。
［式２］
ａｒｇｍｉｎ｛｜｜ｇ_ｉ−ｐ｜｜｝，ｇ_ｉ∈Ω

なお、必ずしもこの探索方法を採用する必要はなく、例えば、探索領域Ω３３０内の各画素に対して特徴検出器により特徴量を算出し、３次元形状モデルの幾何特徴の特徴量と最も似ているものを選択するようにしても良い。また、例えば、探索領域Ω３３０内における計測点の３次元空間内における座標の中央値や平均値など統計的処理によって算出した空間中の座標を選択するようにしても良い。その他、距離画像上の幾何特徴は、３次元点に限られず、探索領域Ω内の計測点に対して当てはめた曲面や平面のパラメータであっても良く、後述する位置姿勢算出部１６０において、対象物体の位置及び姿勢を算出する際に利用可能な幾何特徴であれば良い。

また、距離画像上で選択された幾何特徴の法線を算出し、当該算出した法線と３次元形状モデルの幾何特徴の法線とを比較し、互いの法線方向が所定値を越えて異なっていれば、当該幾何特徴を対応付けないといった誤対応排除処理を行なうようにしても良い。なお、距離画像上の幾何特徴の法線は、幾何特徴の近傍の３次元点群に対して主成分分析し、第３の主成分を法線とすることで求められる。誤対応排除処理を実施するように構成した場合、３次元形状モデルの幾何特徴に対して、異なる幾何特徴の計測データが対応付けられる誤りを軽減させられる。

位置姿勢算出部１６０は、対応探索部１５０により対応付けられた３次元形状モデルの幾何特徴と距離画像上の幾何特徴との組を用いて、対象物体の位置及び姿勢を算出する。本実施形態における対象物体の位置及び姿勢の算出では、演算処理を繰り返し、それにより得られる値を最適化する。これにより、対応付けられた３次元形状モデルの幾何特徴と距離画像上の幾何特徴との組の３次元空間における距離の差を最小化する。

本実施形態においては、３次元形状モデルの幾何特徴は、３次元空間における座標と法線情報との組であり、距離画像上の幾何特徴は、３次元点の座標である。３次元形状モデルの幾何特徴を微小な平面と考え、３次元形状モデル上の平面と距離画像から算出した３次元点との距離を差とする。そして、この差を最小化することにより、対象物体の位置及び姿勢の計測を行なう。

ここで、上記差は、幾何特徴の表現方法に依存する。例えば、３次元形状モデルの幾何特徴が点であり距離画像上の幾何特徴が点であれば、点と点との距離であっても良い。また、例えば、３次元形状モデルの幾何特徴が面であり距離画像上の幾何特徴が面であれば、面と面との距離であっても良い。また、例えば、３次元形状モデルの幾何特徴が面であり、距離画像上の幾何特徴が点であれば、面と点との距離であっても良い。また更に、上記差は、陰多項式などを利用して近似的に算出される距離であっても良い。すなわち、３次元形状モデルの幾何特徴と距離画像上の幾何特徴との間の距離の差に基づく評価関数を用いて、対象物体の位置及び姿勢を計測する構成であれば、その方法は特に問わない。

次に、図４のフローチャートを用いて、図１に示す３次元計測装置１０における動作の一例について説明する。ここでは、物体の位置及び姿勢を計測する際の処理の流れについて説明する。

３次元計測装置１０は、画像取得部１１０において、距離センサからの奥行き情報（距離情報）を含む距離画像を撮影する。これにより、距離画像を取得する（Ｓ１１０）。

距離画像の取得が済むと、３次元計測装置１０は、概略値取得部１２０において、３次元計測装置１０に対する物体の位置及び姿勢の概略値を取得する（Ｓ１２０）。上述した通り、物体の位置及び姿勢の概略値は、過去に計測された位置及び姿勢、速度や角速度などの運動の推定結果を加味した位置及び姿勢の予測値、他のセンサによる計測値、２次元画像や距離画像からの物体認識結果などを用いることができる。なお、本実施形態においては、物体の位置及び姿勢の概略値として、過去に計測された位置及び姿勢の概略値を用いる。なお、Ｓ１１０及びＳ１２０の処理の順番は、入れ替わっても良い。

次に、３次元計測装置１０は、Ｓ１１０の処理で取得した距離画像やＳ１２０の処理で取得した概略値等を用いた対象物体の位置姿勢計測処理を開始する。本実施形態に係わる位置姿勢計測処理では、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いる。具体的には、対象物体の位置及び姿勢の概略値を反復演算により繰り返し補正する。これにより、３次元形状モデルの幾何特徴と距離画像上の幾何特徴とにおける３次元空間での距離の差を最小化し、それにより得られた値を対象物体の位置及び姿勢とする。勿論、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法に限られず、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法や最急降下法などの最適化手法を用いても良い。また、共役勾配法など、他の非線型最適化計算手法を用いても良い。

位置姿勢計測処理が開始すると、３次元計測装置１０は、まず、Ｓ１２０の処理で得られた対象物体の概略値を位置姿勢計測処理における初期値として取得する。そして、幾何特徴選択部１４０において、当該概略値に基づいて、３次元形状モデルの幾何特徴を選択する（Ｓ１３０）。

当該選択が済むと、３次元計測装置１０は、対応探索部１５０において、対象物体の位置及び姿勢の概略値と距離センサの撮影パラメータ（画角や解像度、焦点距離など）とに基づいて、３次元形状モデルの幾何特徴を距離画像（２次元平面）に投影する。そして、３次元形状モデルの幾何特徴と距離画像上の幾何特徴とを対応付け、当該対応付けた３次元形状モデルの幾何特徴と距離画像上の幾何特徴との組を出力する（Ｓ１４０）。そして、３次元計測装置１０は、位置姿勢算出部１６０において、非線型最適化手法により、対象物体の位置及び姿勢の概略値を更新する（Ｓ１５０）。

最後に、３次元計測装置１０は、位置姿勢算出部１６０において、収束判定を行なう。そして、算出値が収束していれば終了し（Ｓ１６０でＹＥＳ）、そうでなければ（Ｓ１６０でＮＯ）、再度、Ｓ１３０の処理に戻り、上述したＳ１３０〜Ｓ１５０までの処理を繰り返す。なお、収束判定では、例えば、位置及び姿勢の更新前後の誤差ベクトルの二乗和の差がほぼ「０」であれば、収束したと判定すればよい。

以上説明したように実施形態１によれば、３次元形状モデルと距離画像とにおける幾何特徴の対応付けに際して、３次元形状モデルの幾何特徴上の点を選択し、当該選択された幾何特徴上の点を距離画像上に投影する。そして、当該距離画像上における投影点から所定範囲内の領域にある幾何特徴を探索することにより、３次元形状モデルの幾何特徴と距離画像上の幾何特徴とを対応付ける。これにより、３次元形状モデルと計測データとの対応付けに際して実施する演算を効率的に行なえる。

（実施形態２）
次に、実施形態２について説明する。実施形態２においては、入力画像として、距離画像の他、２次元画像を入力する場合について説明する。図５は、３次元形状モデルの幾何特徴を２次元平面に投影し、２次元画像及び距離画像において、対応する幾何特徴を探索する処理の概要を示す図である。

距離画像４４０及び２次元画像４５０のいずれに対しても、３次元形状モデルの幾何特徴４１０、４２０を２次元平面（投影面）４３０に投影し、対応する画像上での幾何特徴を２次元平面上から探索すれば、上記対応付けを行なうことができる。

そのため、３次元形状モデルと計測データとの対応付けの演算処理を効率化できる。なお、本実施形態においては。距離画像４４０及び２次元画像４５０は、同一視点で撮影されたものとする。この場合、２次元画像上での幾何特徴と距離画像上での幾何特徴との比較が容易であり、影などの影響による誤対応の軽減の他、ジャンプエッジ（孤立したエッジ）の判定も可能になる。

ここで、実施形態２に係わる３次元計測装置１０における全体的な構成は、実施形態１同様となるが、各部における処理が多少相違する。この点について説明する。なお、ここでは相違する処理について重点的に説明する。

画像取得部１１０は、２次元画像及び距離画像を取得する。距離画像に関しては実施形態１の場合と同様であり、距離センサによって撮影される。２次元画像は、通常の２次元画像を撮影するカメラ（撮像装置）によって撮影される。撮影される２次元画像は、濃淡画像であっても良いし、カラー画像であっても良い。カメラの焦点距離や主点位置、レンズ歪みパラメータなどのカメラモデルは、例えば［R. Y. Tsai, “A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses, " IEEE Journal of Robotics and Automation, vol.RA-3, no.4, 1987.］で開示されている方法によって事前にキャリブレーションしておく。

実施形態２においては、例えば、実施形態１で例示したアクティブ式の距離センサを用いる。これは、アクティブ式の距離センサや、ステレオカメラによる距離センサなどは、通常の２次元画像も同時に撮影可能であるためである。そのため、２次元画像及び距離画像は、同一視点から撮影された画像となる。但し、２次元画像及び距離画像は、必ずしも同一視点から撮影されていなくてもよい。なお、２次元画像と距離画像とにおける撮像装置の撮影位置や姿勢が近く、且つ両装置の幾何関係が既知であれば、両者の幾何関係に基づいて２次元画像及び距離画像のいずれか一方を射影変換し、それにより得られた画像を同一視点から撮影した画像としても良い。

対応探索部１５０は、幾何特徴選択部１４０により選択された３次元形状モデルの幾何特徴と、２次元画像及び距離画像上の幾何特徴とを対応付け、当該対応付けた３次元形状モデルの幾何特徴と、距離画像上及び２次元画像上の幾何特徴の組を出力する。対応探索部１５０では、実施形態１同様に、幾何特徴選択部１４０により選択された３次元形状モデルの幾何特徴を２次元平面（投影面）に投影することにより上記対応付けを行なう。なお、３次元形状モデルと距離画像との対応付けは、実施形態１同様の処理となる。３次元形状モデルと２次元画像との対応付けには、幾何特徴としてエッジを利用すればよい。エッジによる対応付けは、従来技術を用いればよいため、ここでは詳細な説明については省略する。なお、［C. Harris and M. Stephens, “A combined corner and edge detector, " Proc. 4th Alvey Vision Conf., pp.147-151, Manchester, U.K., 1988.］で開示されているような特徴検出器により検出される特徴点であっても良い。その他、位置姿勢算出部１６０において、対象物体の位置及び姿勢を計測する際に利用可能な幾何特徴であれば良い。

本実施形態に係わる２次元画像及び距離画像は、同一視点から撮影された画像であるので、投影面と２次元画像及び距離画像とにおける各画素は、一意に対応する。そのため、実施形態１同様に、投影点の所定範囲内（領域周辺）を探索領域Ωとして、３次元形状モデルの幾何特徴に対応する画像上の幾何特徴を２次元探索する。探索領域Ωは、実施形態１同様に、矩形や楕円形状の領域であっても良い。なお、２次元画像上のエッジを探索する場合には、エッジの法線方向に任意の長さの線上を１次元探索しても良い。

また更に、実施形態１で説明した誤対応排除処理を２次元画像に対して実施してもよい。誤対応排除処理は、例えば、特徴量の比較やエッジの方向の比較に基づいて行なう。また、例えば、２次元画像上で検出されたエッジや特徴量に対応する特徴が、距離画像から検出されなければ、影などによる影響を起因としたエッジや特徴量であるとして対応付けを行なわないといった誤対応排除処理を行なうようにしても良い。

また、距離画像を用いて、２次元画像で検出されたエッジがジャンプエッジであるか否かを判定する処理を行なうようにしても良い。本実施形態に係わる距離画像及び２次元画像は、同一視点から撮影されているため、２次元画像と距離画像との併用により誤対応排除処理を効率よく行なえる。

位置姿勢算出部１６０は、対応探索部１５０により対応付けられた３次元形状モデルの幾何特徴と、２次元画像及び距離画像上の幾何特徴との組を用いて、対象物体の位置及び姿勢を算出する。本実施形態における対象物体の位置及び姿勢の算出では、演算処理を繰り返してそれにより得られる値を最適化する。これにより、３次元形状モデルの幾何特徴と２次元画像上の幾何特徴との３次元空間における距離の差、及び３次元形状モデルの幾何特徴と距離画像上の幾何特徴との３次元空間における距離の差の双方に基づく評価関数を最小化する。

なお、２次元画像には、奥行き情報が含まれていないため、２次元画像の幾何特徴を３次元空間に投影する場合、例えば、奥行き情報がない状態で投影する。勿論、所定のアルゴリズムや予め決められた値等を用いて２次元画像の幾何特徴各々に対して奥行き情報を付与するように構成しても良い。

その他、３次元形状モデルの幾何特徴と２次元画像上及び距離画像上の幾何特徴との間の差に基づく評価関数に基づいて対象物体の位置及び姿勢を計測する手法であれば、その方法は特に問わない。なお、実施形態２に係わる位置及び姿勢の計測処理の流れは、実施形態１と同様であるため、ここでは、その説明については省略する。

なお、実施形態２においては、対応付ける３次元形状モデルの幾何特徴は、曲面や平面、曲線であっても良い。しかし、ＣＡＤモデルでよく利用されているＮＵＲＢＳ曲面など複雑な形状を２次元平面に投影するには、テセレーションや輪郭線で曲面を切り取る処理などが必要となり、ＧＰＵなどの幾何計算用の専用ハードウェアがなければ、演算処理に要する時間が多大になる。そこで、実施形態１同様に、３次元形状モデルの幾何特徴を点又は線分として表現し、当該点又は線分を２次元平面に投影する。この投影により、専用ハードウェアを必要とせずに、対応付けを高速化できる。

以上説明したように実施形態２によれば、実施形態１同様に、３次元形状モデルと計測データとの対応付けに際して実施する演算を効率的に行なえる。

以上が本発明の代表的な実施形態の例であるが、本発明は、上記及び図面に示す実施形態に限定することなく、その要旨を変更しない範囲内で適宜変形して実施できるものである。

なお、本発明は、例えば、システム、装置、方法、プログラム若しくは記憶媒体等としての実施態様を採ることもできる。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給する。そして、そのシステム或いは装置のコンピュータ（又はＣＰＵやＭＰＵ、ＧＰＵ等）がプログラムを読み出して実行する処理である。

１０：３次元計測装置、１１０：画像取得部、１２０：概略値取得部、１３０：モデル保持部、１４０：幾何特徴選択部、１５０：対応探索部、１６０：位置姿勢算出部

上記課題を解決するため、本発明の一態様による情報処理装置は、
対象物体までの距離を画素に格納する距離画像及び前記対象物体を含む二次元画像を取得する画像取得手段と、
前記対象物体の三次元形状モデルを保持する保持手段と、
前記対象物体の概略位置姿勢を取得する取得手段と、
前記対象物体の概略位置姿勢に基づいて、前記対象物体の三次元形状モデルの第１の幾何特徴と、該第１の幾何特徴に対応する前記距離画像上の第１の特徴を対応付ける第１の対応付け手段と、
前記対象物体の概略位置姿勢に基づいて、前記対象物体の三次元形状モデルの第２の幾何特徴と、該第２の幾何特徴に対応する前記二次元画像上の第２の特徴を対応づける第２の対応付け手段と、
前記対応づけられた第１の幾何特徴と前記第１の特徴とのずれを取得する第１のずれ取得手段と、
前記対応づけられた第２の幾何特徴と前記第２の特徴とのずれを取得する第２のずれ取得手段と、
前記第１のずれ取得手段で取得したずれと前記第２の取得手段で取得したずれとがそれぞれ小さくなるように前記概略位置姿勢を更新することにより、前記対象物体の位置姿勢を導出する導出手段と
を備えることを特徴とする。

Claims (7)

1. 各画素が対象物体との間の距離を示す距離画像を取得する画像取得手段と、
   前記対象物体の位置及び姿勢を示す概略値を取得する概略値取得手段と、
   前記対象物体の３次元形状モデルの幾何特徴上の点を選択する選択手段と、
   前記対象物体の位置及び姿勢を示す概略値と前記距離画像の撮影時の撮影パラメータとに基づいて前記選択手段により選択された幾何特徴上の複数の点を前記距離画像上に投影し、該投影点から所定範囲内の領域で前記３次元形状モデルの幾何特徴に対応する前記距離画像上の幾何特徴を探索して対応付ける対応探索手段と、
   前記対応探索手段により対応付けられた前記３次元形状モデルの幾何特徴と前記距離画像上の幾何特徴との３次元空間における距離の差を用いて前記対象物体の位置及び姿勢を算出する位置姿勢算出手段と
   を具備することを特徴とする３次元計測装置。
2. 前記画像取得手段は、
   前記対象物体が撮影された２次元画像を更に取得し、
   前記対応探索手段は、
   前記対象物体の位置及び姿勢を示す概略値と前記距離画像及び前記２次元画像それぞれの撮影時の撮影パラメータとに基づいて前記選択手段により選択された幾何特徴上の複数の点を前記距離画像上及び前記２次元画像上に投影し、該投影点から所定範囲内の領域で前記３次元形状モデルの幾何特徴に対応する前記距離画像上及び前記２次元画像上の幾何特徴を探索して対応付け、
   前記位置姿勢算出手段は、
   前記対応探索手段により対応付けられた前記３次元形状モデルの幾何特徴と前記距離画像上及び２次元画像上の幾何特徴との３次元空間における距離の差を用いて前記対象物体の位置及び姿勢を算出する
   ことを特徴とする請求項１記載の３次元計測装置。
3. 前記対応探索手段は、
   前記選択手段により選択された点における前記３次元形状モデルの幾何特徴上の法線の方向と、前記距離画像上における前記投影点の所定範囲内から探索された前記幾何特徴の法線の方向とを比較し、互いの法線方向が所定値を越えて異なっていれば、当該幾何特徴については前記対応付けを行なわない
   ことを特徴とする請求項１又は２記載の３次元計測装置。
4. 前記２次元画像は、前記距離画像と同一視点から撮影されており、
   前記対応探索手段は、
   前記２次元画像の幾何特徴として前記２次元画像から前記対象物体のエッジを検出し、該検出したエッジと前記３次元形状モデルの幾何特徴との対応付けに際して、前記距離画像上の幾何特徴にないエッジと前記３次元形状モデルの幾何特徴との対応付けを行なわない
   ことを特徴とする請求項２記載の３次元計測装置。
5. 前記３次元形状モデルの幾何特徴は、
   点又は線分により表現される
   ことを特徴とする請求項１から４いずれか１項に記載の３次元計測装置。
6. 対象物体の位置及び姿勢を算出する３次元計測装置の処理方法であって、
   画像取得手段が、各画素が対象物体との間の距離を示す距離画像を取得する工程と、
   概略値取得手段が、前記対象物体の位置及び姿勢を示す概略値を取得する工程と、
   選択手段が、前記対象物体の３次元形状モデルの幾何特徴上の点を選択する工程と、
   対応探索手段が、前記対象物体の位置及び姿勢を示す概略値と前記距離画像の撮影時の撮影パラメータとに基づいて前記選択手段により選択された幾何特徴上の複数の点を前記距離画像上に投影し、該投影点から所定範囲内の領域で前記３次元形状モデルの幾何特徴に対応する前記距離画像上の幾何特徴を探索して対応付ける工程と、
   位置姿勢算出手段が、前記対応探索手段により対応付けられた前記３次元形状モデルの幾何特徴と前記距離画像上の幾何特徴との３次元空間における距離の差を用いて前記対象物体の位置及び姿勢を算出する工程と
   を含むことを特徴とする処理方法。
7. 対象物体の位置及び姿勢を算出する３次元計測装置に内蔵されたコンピュータを、
   各画素が対象物体との間の距離を示す距離画像を取得する画像取得手段、
   前記対象物体の位置及び姿勢を示す概略値を取得する概略値取得手段、
   前記対象物体の３次元形状モデルの幾何特徴上の点を選択する選択手段、
   前記対象物体の位置及び姿勢を示す概略値と前記距離画像の撮影時の撮影パラメータとに基づいて前記選択手段により選択された幾何特徴上の複数の点を前記距離画像上に投影し、該投影点から所定範囲内の領域で前記３次元形状モデルの幾何特徴に対応する前記距離画像上の幾何特徴を探索して対応付ける対応探索手段、
   前記対応探索手段により対応付けられた前記３次元形状モデルの幾何特徴と前記距離画像上の幾何特徴との３次元空間における距離の差を用いて前記対象物体の位置及び姿勢を算出する位置姿勢算出手段
   として機能させるためのプログラム。