位置姿勢計測装置、位置姿勢計測方法、およびプログラム
【課題】距離画像から対象物体の位置姿勢を算出する位置姿勢算出の精度低下を防ぎながら処理速度を維持向上させる。
【解決手段】対象物体までの距離画像を取得する取得部と、距離画像から対象物体の概略位置姿勢を決定する概略位置姿勢決定部と、形状モデル上の所定領域に対応する、距離画像における領域の距離情報のばらつきの程度を示すばらつき値を算出する算出部と、ばらつき値が小さい程、サンプル点の数が少なくなるように、形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング部と、概略位置姿勢からサンプル点と距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け部と、各サンプル点と各３次元計測点との間の距離の総和が最小になるように対象物体の位置姿勢を決定する位置姿勢決定部と、を備える。
【解決手段】対象物体までの距離画像を取得する取得部と、距離画像から対象物体の概略位置姿勢を決定する概略位置姿勢決定部と、形状モデル上の所定領域に対応する、距離画像における領域の距離情報のばらつきの程度を示すばらつき値を算出する算出部と、ばらつき値が小さい程、サンプル点の数が少なくなるように、形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング部と、概略位置姿勢からサンプル点と距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け部と、各サンプル点と各３次元計測点との間の距離の総和が最小になるように対象物体の位置姿勢を決定する位置姿勢決定部と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置及び姿勢を計測する技術に関し、特に対象物体を撮影した距離画像に基づいて対象物体の位置及び姿勢を算出する位置姿勢計測装置、位置姿勢計測方法、およびプログラムに関する。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、従来人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。複雑なタスクの代表的な例として、工業製品の組立が挙げられる。ロボットが自律的に組立作業を行うためには、ハンドなどのエンドエフェクタによって部品を把持することが必要となる。ロボットハンドによって部品を把持するためには、把持の対象となる部品とロボットとの相対的な位置及び姿勢を計測し、計測結果に基づいて移動計画を策定して実際にアクチュエータを制御する必要がある。ロボットの位置姿勢計測は、ロボットに搭載されたカメラや距離センサを用いて行われており、２次元画像や距離画像を用いる方法が代表的である。特にオフィスや工場などのシーンや工業製品の部品など、対象が直線や曲線、濃淡変化の少ない面など単調な特徴しか持たない場合には距離画像を用いることが多い。
【０００３】
非特許文献１では、距離画像へのモデルフィッティングによる物体の位置及び姿勢の計測を行う方法が開示されている。この方法では、距離画像を３次元点群データに変換し、点群データに対象物体の３次元形状モデルを当て嵌めることにより物体の位置及び姿勢を計測する。すなわち、位置及び姿勢の概略値をもとに、点群データの各点について３次元形状モデルの近傍の面を探索し、点と面との距離の和を最小化するように位置及び姿勢の最適化を繰り返すことにより位置及び姿勢を算出する。
【０００４】
特許文献１では、非特許文献１と同様に観測画像へのモデルフィッティングにより、物体の位置及び姿勢を推定する方法が開示されている。このとき、物体のモデルから低曲率の面を選択し、選択した面と対応する距離計測データ上の面との距離の差を最小化する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】米国特許6504957号公報
【特許文献２】特開平01-009307号公報
【非特許文献】
【０００６】
【非特許文献１】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.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
非特許文献１のように距離画像を用いて物体の位置及び姿勢を推定する手法は、距離画像の点群データの各点に対応する３次元モデルの面の探索に多くの計算時間が必要であるという問題があった。
【０００８】
この問題の解決策として、対応の数を減らす方法が考えられる。しかしながら、計測データには必ず誤差が含まれる。対応の数を減らすと、それぞれの対応点が位置姿勢推定の計算に寄与する度合いが相対的に高まる。従って、対応点の中に誤差の大きい計測データに基づくものが含まれている場合、位置姿勢推定の精度が低下するという問題がある。
【０００９】
上記の課題に対して、特許文献１では、計測対象物体上で低曲率の部位は距離計測誤差が小さいと想定し、モデル上の低曲率の部位に処理対象を限定することによって、精度を低下させずに処理速度を向上することを狙っている。
【００１０】
しかしながら、実際には低曲率の部位であっても距離計測の精度が高いとは限らない。図１を参照して説明する。図１は、上記の現象を説明する原理図である。図１において、撮像部１０１は距離計測装置を構成する撮像部である。計測対象物体１０２は低曲率の部位を有する計測対象物体である。計測対象物体１０２の表面上の２点として点１０３および点１０４を考える。この場合、点１０４の方が点１０３よりも曲率が低い。しかしながら、点１０３および点１０４を含む領域に相当する画素を観測する領域１０５および領域１０６を考えると、領域１０６内に含まれる計測対象物体上の表面の撮像部１０１からの距離の分布は、領域１０５よりも広範囲に渡る。従って、点１０４の距離計測値のばらつき誤差は、点１０３の距離計測値のばらつき誤差よりも大きい。このように、面の傾きによっては、高曲率の部位よりも低曲率の部位の方が、距離計測精度が低いことがある。そのような場合、特許文献１の方法に従えば、距離計測値のばらつき誤差が大きい領域（低曲率な領域）を選択して物体の位置姿勢を推定することになるため、特許文献１の方法を用いない場合よりも、位置姿勢推定の精度が低下してしまう。
【００１１】
つまり、特許文献１の方法では、計測誤差の少ないデータを確実に選択できないために、位置姿勢推定精度が低下することがあるという問題があった。
【００１２】
上記の課題に鑑み、本発明は、位置姿勢推定の精度低下を防ぎながら、処理速度を維持向上させることを目的とする。
【課題を解決するための手段】
【００１３】
上記の目的を達成する本発明に係る位置姿勢計測装置は、
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報のばらつきの程度を示すばらつき値を算出する算出手段と、
前記ばらつき値が小さい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、を備えることを特徴とする。
【００１４】
また、上記の目的を達成する本発明に係る位置姿勢計測装置は、
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報に含まれる計測誤差を示すオフセット値を算出するオフセット値算出手段と、
前記オフセット値が大きい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、を備えることを特徴とする。
【発明の効果】
【００１５】
本発明によれば、位置姿勢計測のために用いる対応点の密度を距離計測の誤差に応じて制御することにより、位置姿勢計測精度の低下を抑えながら、処理速度を向上させることができる。
【図面の簡単な説明】
【００１６】
【図１】低曲率の部位であっても距離計測の精度が高いとは限らないことを説明する原理図。
【図２】本発明の装置の構成を表すブロック図。
【図３】第１実施形態に係る処理全体の流れを示すフローチャート。
【図４】距離画像計測における偶然誤差発生の原理を説明する図。
【図５】第１実施形態に係る距離計測誤差推定処理の流れを示すフローチャート。
【図６】三角形パッチを小領域に分割する方法の原理を説明する図。
【図７】距離画像計測における系統誤差発生の第１の原理を説明する図。
【図８】第２実施形態に係る距離計測誤差推定処理の流れを示すフローチャート。
【図９】距離画像計測における系統誤差発生の第２の原理を説明する図。
【図１０】距離画像計測における系統誤差発生の第３の原理を説明する図。
【発明を実施するための形態】
【００１７】
（第１実施形態）
以下、対象物体のモデル上から距離画像と対応付ける局所領域を距離計測誤差の特性と大きさに応じてサンプリングすることにより、位置姿勢推定精度を保ちつつ、対応付けの処理負荷を軽減する方法を説明する。距離画像とは、対象物体までの距離情報が画素値として保持された画像である
本実施形態では、距離計測の計測値に偶然誤差（距離計測値のばらつき）が含まれる場合を説明する。ここでは、距離計測値の系統誤差（真値からのオフセット）は微小であるとする。
【００１８】
本実施形態では、対象物体の形状モデルの表面上からサンプル点を選択し、距離画像から変換した３次元計測点群とサンプル点とを対応付ける。そして、対応点対間の距離の総和を最小化することによって、対象物体の位置姿勢決定（推定）を行う。
【００１９】
サンプル点を選択する際は、形状モデル表面上の小領域（所定領域）ごとに、その小領域に相当する計測対象物体表面上の領域を距離計測したときの偶然誤差の大きさ（計測値のばらつきの程度）を推定する。そして、距離計測値のばらつきの大きさに応じて、当該小領域からサンプリングする点の密度を決定する。具体的には、距離計測値のばらつきが大きいほど、サンプル点の密度を高くする。つまり、ばらつき値が大きい程多くサンプリングする。逆にばらつき値が小さい程少なくサンプリングする。ただし、何れの小領域に関しても、距離画像の画素ごとに３次元計測点群と形状モデルを対応付ける場合よりもサンプル点の数が少なくなるようにサンプル点の上限数を設定する。このようにサンプリング密度を制御することにより、距離画像の画素ごとに対応付けを行う方法に比べて対応点の数を減らすことができる。そして、計測値のばらつきが大きい領域については多数の点の平均をとる効果を作用させることにより、誤差の影響を軽減し、位置姿勢計測の結果が真値に近くなることが期待される。
【００２０】
なお、距離計測の測定誤差は、距離計測装置の計測方向と計測対象物体表面上の各小領域の法線方向とのなす角度に基づいて推定する。具体的には、計測方向と物体表面が垂直に近いほど計測値のばらつきが小さく、逆に計測方向と物体表面が平行に近いほど計測値のばらつきが大きいと推定する。
【００２１】
以下、本実施形態に係る位置姿勢計測装置の詳細について説明する。
【００２２】
まず、図２を参照して、本発明に係る位置姿勢計測装置のハードウェア構成について説明する。図２において、ＣＰＵ２０１はＣＰＵであり、装置全体の動作を制御する。メモリ２０２はメモリであり、ＣＰＵ２０１の動作に用いるプログラムやデータを格納する。バス２０３はバスであり、各構成モジュール間を接続してデータ転送を可能にする。インタフェース２０４はバス２０３と各種装置とのインタフェースである。また、外部記憶装置２０５はＣＰＵ２０１に読み込まれるプログラムやデータを格納する外部記憶装置である。キーボード２０６およびマウス２０７はプログラムを起動したり、プログラムの動作を指定したりするための入力装置を構成するキーボードおよびマウスである。表示部２０８はプロセスの動作結果を表示するための表示部であり、液晶画面等で構成される。データ入出力部２０９は装置外部とのデータ入出力部である。不図示の距離画像計測装置はデータ入出力部２０９を介して本発明に係る位置姿勢計測装置に接続されている。
【００２３】
次に、図３を参照して、本実施形態に係る位置姿勢計測装置の全体処理の流れを示すフローチャートについて説明する。ステップＳ３０１において、処理で用いるデータの初期設定を行う。具体的には、外部記憶装置２０５から形状モデルデータ、標準サンプル数、及び基準距離をメモリ２０２にロードする。標準サンプル数と基準距離については後述する。また、不図示の距離画像計測装置の撮像部のカメラパラメータ（主点位置、焦点距離、画像歪み係数、画素数等）をデータ入出力部２０９に接続された距離画像計測装置から取得する。
【００２４】
形状モデルは、物体の表面を三角形平面の集合として表す三角形ポリゴンパッチモデルであるとする。形状モデルデータは、三角形ポリゴンの各頂点の識別番号と座標、各三角形ポリゴンの識別番号とそのポリゴンを構成する頂点の識別番号、各三角形ポリゴンの法線ベクトルから構成される。三角形ポリゴンの法線ベクトルは、形状モデルの内部から外部へ向かう方向であるとする。
【００２５】
カメラパラメータは例えば非特許文献２で開示される方法により事前に校正しておく。ステップＳ３０１において入力されるカメラパラメータを用いることによって、距離画像計測装置の撮像部の３次元座標系における点を距離画像上（２次元座標系上）に投影した２次元座標を算出することができる。
【００２６】
続くステップＳ３０２において、データ入出力部２０９に接続された不図示の距離画像計測装置から計測対象物体を計測した距離画像データを取得し、メモリ２０２に格納する。なお、距離画像の撮影は、例えば光切断法やステレオ法により撮影する。
【００２７】
ステップＳ３０３において、計測対象物体の概略位置姿勢（距離計測装置の撮像部に対する計測対象物体の概略の相対位置姿勢）を決定する。この概略位置姿勢決定処理には、例えば特許文献２に開示される方法を用いることができる。
【００２８】
ステップＳ３０４において、ステップＳ３０２で入力された距離画像データの誤差を推定する。本ステップの処理については後述する。
【００２９】
続いて、ステップＳ３０５において、ステップＳ３０４で推定された距離画像データの誤差に基づいて、形状モデル表面上から距離画像データと対応付ける所定領域（局所領域）、すなわち点群を選択する。ステップＳ３０５における形状モデル上からの点群選択処理の詳細については後述する。
【００３０】
次に、ステップＳ３０６において、ステップＳ３０５で選択されたサンプル点を距離画像と対応付ける。具体的には、ステップＳ３０３で算出された計測対象物体の概略位置姿勢に基づいて、対象物体の座標系における各サンプル点の３次元座標を距離計測装置の撮像部の座標系における３次元座標に変換する。さらに、ステップＳ３０１で入力されたカメラパラメータに基づいて、撮像部の座標系における各サンプル点の３次元座標を距離画像上に投影した点の２次元座標を算出する。このようにして算出された座標値は距離画像の画素の座標と必ずしも一致しないので、投影点の最近傍（最近接）の画素を距離画像上で探索する。そして、探索結果の画素の２次元座標（２次元計測点）および距離値から該画素の撮像部座標系における３次元座標を算出し、ステップＳ３０３で算出された計測対象物体の概略位置姿勢に基づいて、対象物体の座標系における３次元座標に変換する。このようにして算出された距離計測点の３次元座標は、対応するサンプル点の３次元座標と組にして、メモリ２０２に記憶領域を確保した対応点対リストに追加する。
【００３１】
なお、上述したサンプル点と距離画像の対応付け処理に加えて、誤対応を排除する処理を加えてもよい。具体的には、距離画像上の対応点において近傍の距離計測点群データから当該対応点における法線方向を算出し、対象物体の座標系における方向に変換する。このようにして算出された法線と、当該対応点に対応するサンプル点が含まれる三角形パッチの法線方向とを比較して、法線方向が大きく異なる場合は当該対応点を対応点対リストに追加しないようにすればよい。
【００３２】
最後に、ステップＳ３０６で求められた対応点対間の距離の総和が最小となるように最適化問題を解くことによって、計測対象物体の位置姿勢を算出する。本ステップの詳細については、後述する。
【００３３】
ここで、距離計測の誤差量推定処理（ステップＳ３０４）の詳細を説明する。図４を参照して、本実施形態に係る計測値のばらつきの程度を示すばらつき値を推定する方法について説明する。
【００３４】
図４は、距離画像計測装置の撮像部４０１の観測方向と計測対象物体の表面４０３又は表面４０５との位置関係を示した図である。表面４０３が撮像部４０１の観測方向と垂直に近い場合、表面４０５が撮像部４０１の観測方向と平行に近い場合を表している。図４において、領域４０２は撮像部４０１のある１画素に相当する領域であるとする。計測対象物体の表面領域が、それぞれ範囲４０４又は範囲４０６に分布している。すなわち、当該画素の距離情報のばらつき値は、範囲４０４又は範囲４０６の間隔に等しい。従って、距離情報の値は、撮像装置の観測方向と計測対象物体の面の法線とのなす角度によって計量できる。
【００３５】
図５を参照して、距離計測の誤差量推定処理の流れを示すフローチャートについて説明する。ステップＳ５０１において、形状モデルに含まれる三角形ポリゴンの中から未処理のポリゴンを１つ選択する。ここで、ステップＳ５０２からステップＳ５０６までの各ステップでは、ステップＳ５０１で選択された三角形ポリゴンを処理の対象とする。なお、本ステップで選択された三角形ポリゴンの識別番号はｉであるとする。以下、添え字のｉは本ステップで選択された三角形ポリゴンに関するものであることを示す指標である。
【００３６】
ステップＳ５０２において、距離画像計測装置の観測方向と処理対象ポリゴンの法線方向とのなす角度αｉ（−π≦αｉ≦π）を算出する。なお、距離画像計測装置の観測方向は、ステップＳ３０３で算出された対象物体の概略位置姿勢に基づいて、対象物体の座標系における方向ベクトルとして表されているとする。
【００３７】
次に、ステップＳ５０３において、ステップＳ５０２で算出された角度αｉに基づいて、処理対象のポリゴンが距離画像計測装置から可視であるか、あるいは不可視であるかを判定する。具体的には、−（π／２＋φ）≦αｉ≦π／２＋φを満たさない場合は、距離画像計測装置が処理対象ポリゴンの表側を観察しているので可視と判定し、ステップＳ５０４に進む。一方、−（π／２＋φ）≦αｉ≦π／２＋φを満たす場合、距離画像計測装置が処理対象ポリゴンの裏側を観察しているか、あるいは観察方向とポリゴン平面が平行である。従って、処理対象ポリゴンは不可視と判定し、ステップＳ５０５に進む。なお、φは距離計測装置の観測方向が対象物体表面と平行に近い場合に（例えば物体表面の反射光量が低下することにより）計測不能になる限界を表す角度定数である。
【００３８】
上記の説明では、距離画像計測装置の観測方向と処理対象ポリゴンの法線方向とのなす角度のみで、当該ポリゴンの可視／不可視を判定している。さらに、距離画像計測装置から見て当該ポリゴンが他のポリゴンに隠されている場合に当該ポリゴンを不可視と判定する処理を加えてもよい。このような判定処理はコンピュータグラフィクスにおけるオクルージョン判定技術を用いて実現できる。
【００３９】
ステップＳ５０４において、以下の式（１）に基づいて、距離計測値のばらつき度ｕｉを算出する。ただし、βｉ＝π−αｉである。
【００４０】
【数１】
【００４１】
一方、ポリゴンが不可視の場合、ステップＳ５０５において、距離計測値のばらつき度ｕｉを−１に設定する。ただし、このステップで設定するばらつき度の値は処理対象ポリゴンが可視の場合に取り得ない値であればよく、−１に限らない。
【００４２】
なお、ステップＳ５０４で算出されたばらつき度あるいはステップＳ５０５で設定されたばらつき度は、ポリゴンの識別番号とセットにしてメモリ２０２に格納する。
【００４３】
ステップＳ５０４またはステップＳ５０５の処理が終了すると、処理はステップＳ５０６に進む。
【００４４】
ステップＳ５０６において、形状モデルに含まれる三角形ポリゴンの全てに対して、ばらつき度を設定する処理を完了したか否か判定する。完了したと判定された場合は（ステップＳ５０６；ＹＥＳ）、誤差量推定処理全体を終了する。一方、全てのポリゴンを処理済みでないと判定された場合は（ステップＳ５０６；ＮＯ）、ステップＳ５０１に戻る。
【００４５】
以上が距離計測の誤差量推定処理の流れである。
【００４６】
続いて、局所領域の選択処理（ステップＳ３０５）の詳細を説明する。
【００４７】
ステップＳ３０５では、形状モデルに含まれる各三角形ポリゴンに関して、式（２）に基づいてサンプル点の数Ｎｉを算出する。
【００４８】
【数２】
【００４９】
式（２）において、ＳおよびＤは、ステップＳ３０１でメモリ２０２に格納された標準サンプル数および基準距離である。標準サンプル数とは、距離計測装置から基準距離の位置にある距離計測装置の計測方向に垂直な単位面積の平面を距離画像に投影した領域に含まれる画素の数であり、サンプル点の上限数を規定している。ｄｉはステップＳ３０３で推定された対象物体の概略位置姿勢に基づいて算出される距離計測装置と識別番号ｉの三角形ポリゴンとの距離である。（ｄｉ）／Ｄ）２を掛け合わせることによって、ある三角形ポリゴンを距離画像上に投影した領域に含まれる画素数の増減の効果をサンプル点の数に反映させることができる。
【００５０】
また、Ａｉは識別番号がｉのポリゴンの面積である。そして｜cosβｉ｜は三角形ポリゴンを距離画像上に投影した領域の面積が当該ポリゴンの向きによって減少する比率である。ｕｉはステップＳ３０４で算出された距離計測値のばらつき度である、Ｃｕはサンプル点の密度に対する距離計測値のばらつき度の寄与度を調整する定数である。
【００５１】
なお、処理対象のポリゴンが不可視の場合（ｕｉ＝−１）、そのポリゴンからはサンプル点を選ばないので、Ｎｉ＝０とする。
【００５２】
サンプル点は、三角形ポリゴンを同一面積で三角形の小領域に分割したときの各小領域から１点ずつ配置する。
【００５３】
図６を参照して、上記の三角形ポリゴンの分割方法の原理について説明する。図６（Ａ）に示される三角形ポリゴンの各辺を２分割した点を結んで４つの小領域に分割した例が図６（Ｂ）である。図６（Ａ）に示される三角形ポリゴンの各辺を３分割して９つの小領域に分割した例が図６（Ｃ）である。同様にして辺の分割数を増やすことによって、三角形ポリゴンをべき乗数個の小領域に分割できる。そこで、式（２）で求めたサンプル点数Ｎｉに最も近いべき乗数を求め、図６に示すようにべき乗数個にポリゴンを分割した各小領域の重心をサンプル点とする。ここで求められたサンプル点群の座標（対象物体の座標系における座標値）は、三角形ポリゴンの識別番号とともにメモリ２０２に格納される。
【００５４】
最後に、対象物体の位置姿勢算出処理（ステップＳ３０７）の詳細について説明する。本ステップでは、対応点対リストに登録された対応点対間の距離の総和が最小になるようにするモデルの位置姿勢を算出し、その結果を対象物体の位置姿勢とする。
【００５５】
距離画像計測装置の撮像部の座標系における３次元座標は、対象物体の位置姿勢ｓによって、対象物体の座標系における３次元座標（ｘ、ｙ、ｚ）に変換することが出来る。ここで、計測対象物体の位置及び姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。いま、ステップＳ３０３で算出された対象物体の概略の位置姿勢により、撮像部座標系のある点が対象物体座標系の点（ｘ０、ｙ０、ｚ０）に変換されるとする。（ｘ、ｙ、ｚ）は対象物体の位置および姿勢により変化するものであり、（ｘ０、ｙ０、ｚ０）の近傍における１次のテイラー展開によって式（３）のように近似できる。
【００５６】
【数３】
【００５７】
メモリ２０２に格納されたある対応点対の形状モデル側の対応点が存在する三角形パッチ面の対象物体座標系における平面の方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１、a、b、c、eは定数）とする。正しいｓによって変換される（ｘ、ｙ、ｚ）は、平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１）を満たすと仮定する。式（３）をこの平面の方程式に代入すると式（４）が得られる。
【００５８】
【数４】
【００５９】
ただし、ｑ＝ａｘ０＋ｂｙ０＋ｃｚ０（定数）とした。式（４）は対応点対の全てについて成り立つため、式（５）のようなΔｓｊに関する線形連立方程式が成り立つ。
【００６０】
【数５】
【００６１】
ここで、式（５）を式（６）のように表す。
【００６２】
【数６】
【００６３】
式（６）に基づいて、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いてΔsを求める。なお、距離画像の計測点群に含まれるノイズなどによる外れ値の影響を緩和するため、次に述べるようなロバスト推定手法を用いてもよい。一般に、外れ値である計測点群データでは誤差値e−qが大きくなる。そのため、式（５）、式（６）の連立方程式に対する寄与度が大きくなり、その結果得られるΔsの精度が低下してしまう。そこで、誤差値e−qが大きいデータには小さな重みを与え、誤差e−qが小さいデータには大きな重みを与える。重みは例えば式（７）に示すようなＴｕｋｅｙの関数により与える。
【００６４】
【数７】
【００６５】
ここでνは定数である。なお、重みを与える関数は必ずしもＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差の大きいデータには小さな重みを与え、誤差の小さいデータには大きい重みを与える関数であればよい。
【００６６】
各データに対応する重みをｗｊとする。ここで式（８）のように重み行列Ｗを定義する。
【００６７】
【数８】
【００６８】
重み行列Ｗは、対角成分以外は全て０の正方行列であり、対角成分は重みｗｊである。Ｎｃは対応点対の数である。この重み行列Ｗを用いて、式（６）を以下に示す式（９）のように変形する。
【００６９】
【数９】
【００７０】
そして式（１０）のように式（９）を解くことにより補正値Δsを求める。
【００７１】
【数１０】
【００７２】
このようにして算出された補正値Δsによりモデルの位置・姿勢の概略値を補正し、その結果をメモリ２０２に格納するか、あるいはデータ入出力部２０９を介して外部の装置に出力する。
【００７３】
なお、上述した位置及び姿勢の最適化計算手法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に基づくものであるが、これに限らずＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法や最急降下法などを用いてもよい。
【００７４】
以上説明したように本実施形態の方法を用いれば、距離画像計測値のばらつきに応じて適応的に距離画像と対応付けるモデル面上のサンプル点を選択することが出来る。これによって、対応点の数を減らして処理を高速化しながら、推定する位置姿勢の精度を保つことが可能である。
【００７５】
なお、本発明は上記に限らず様々な変形例が存在する。以下の第２実施形態から第８実施形態が第１実施形態の変形例である。
【００７６】
（第２実施形態）
上記の説明では、対象物体の概略位置姿勢の算出処理（ステップＳ３０３）、距離計測の誤差推定処理（ステップＳ３０４）、局所領域の選択処理（ステップＳ３０５）、局所領域と距離画像の対応付け処理（ステップＳ３０６）、対象物体の位置姿勢推定処理（ステップＳ３０７）の各処理ステップを１回ずつ行っている。しかし、必ずしも１回ずつに限定するものではなく、これらの処理を繰り返し行って、漸近的に位置及び姿勢の推定精度を高めてもよい。このとき、ステップＳ３０７における対象物体の位置姿勢推定処理の終了後に繰り返し回数が所定回数に達したか否か判定し、所定回数に達していれば処理全体を終了し、達していなければステップＳ３０３に戻ってもよい。あるいは、ステップＳ３０７における処理の終了後に、モデルの当て嵌め誤差を算出して所定の閾値との大小関係を比較し、誤差が閾値未満であれば処理全体を終了し、閾値以上であればステップＳ３０３に戻ってもよい。ここで、モデルの当て嵌め誤差とは、例えば対応点対間の距離を全ての対応点対について足した値とする。上記フローの１回の繰り返しで推定された対象物体の位置姿勢を、次の繰り返しのステップＳ３０３において物体の概略位置姿勢として用いればよい。
【００７７】
上記の方法を用いると、位置姿勢の推定精度が高まるにつれて、距離計測誤差の推定精度も高くなるので、計測誤差の影響を軽減する効果が高くなる。
【００７８】
（第３実施形態）
計測対象物体の概略位置姿勢を算出する方法として、前述の説明では特許文献２に開示される方法を用いたが、これに限るものではない。
【００７９】
例えば、過去の計測値から物体の運動の速度や角速度を線形フィルタまたはカルマンフィルタ等の時系列フィルタリング処理により推定し、推定された速度や角速度によって現在の時刻における物体の位置及び姿勢を予測したものを用いてもよい。また、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、当該センサによる出力値を位置及び姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢が既知である場合には、その値を概略値として用いてもよい。
【００８０】
（第４実施形態）
物体の３次元形状モデルは三角形ポリゴンのみから構成されるとしているが、これに限らず、他の形状のポリゴンのみから構成されてもよい。さらに、複数種類の形状のポリゴンが混在して構成されてもよい。また、３次元形状モデルは平面パッチではなく、パラメトリック曲面パッチから構成されてもよい。さらに、平面パッチと曲面パッチが混在して構成されてもよい。ただし、曲面パッチを用いる場合は、１つのパッチの中で距離計測装置の計測方向とモデル表面との傾きが一様でない。そこで、曲面パッチを小領域に分割して、それぞれの小領域に対して距離計測の誤差を推定して、サンプル点を選択すればよい。
【００８１】
（第５実施形態）
上記の説明では物体モデル表面上の単位領域を同一面積の小領域に分割して、それぞれの小領域からサンプル点を同一個数選択する方法を説明した（ステップＳ３０５）が、サンプル点を決定する方法はこれに限られない。例えば、物体モデル表面上の単位領域をランダムにサンプリングしてもよい。
【００８２】
あるいは、各単位領域に対して予め密にサンプル点を選んでおき、距離計測の誤差のばらつきに基づいて決定されたサンプル点の個数を密なサンプル点の個数で割った「間引き率」に従って、密なサンプル点を間引いてもよい。
【００８３】
さらに、予め決められた複数の間引き率に対してサンプル点の座標を単位領域ごとに決定しておき、それらのサンプル点群の座標をモデルに登録しておいてもよい。そして、距離計測の誤差に基づいて算出された間引き率に最も近い間引き率で登録されたサンプル点をメモリ２０２から読み出して用いてもよい。
【００８４】
このように、サンプル点の決定に関する処理を予め行っておく方法を採用すると、第２実施形態で説明したように対象物体の位置姿勢推定を繰り返し行う場合に計算負荷の軽減による高速化効果が高くなる。
【００８５】
（第６実施形態）
上記の説明では、標準サンプル数は、サンプル点を距離画像上に投影した際の密度が画素密度と同等になるように設定している。しかし、標準サンプル数をさらに少なく設定することも可能である。ただし、標準サンプル数を少なくするに従って、サンプル点の数が減少して処理速度が向上するが、一般的な傾向としては位置姿勢推定の精度は低下する。
【００８６】
（第７実施形態）
上記の説明では、距離計測値のばらつき度を計測対象物体の面の傾きにより評価した。しかし、ばらつき度の評価方法はこれに限るものではない。
【００８７】
例えば、半透明物体に照射された光は物体内部で散乱して物体表面の様々な箇所で輝度が高くなる。従って、照射された光が直接当たった位置を画像上で探索したときに、誤って散乱光により輝度が高くなった部分が抽出されることがある。これが、距離計測値がばらつく原因となる。そこで、計測対象物体の部位ごとの内部散乱率によって距離計測誤差を評価してもよい。
【００８８】
あるいは、黒色の物体は光の反射が少ないため、照射された光が当たった位置の探索がノイズに埋もれて失敗することがある。これも距離計測値がばらつく原因となる。そこで、計測対象物体の部位ごとの反射率によって距離計測誤差を評価してもよい。
【００８９】
（第８実施形態）
上記の説明では、ステップＳ３０５で計測対象物体のモデルから選択する局所領域は点であった。しかし、本発明の方法は点に限らず、他の種類の局所領域を選択する場合にも適用できる。
【００９０】
例えば、局所領域として平面領域が選択されてもよい。この場合局所領域の選択工程（ステップＳ３０５）で、各三角形パッチを等しい面積の平面群に分割した中から距離計測誤差の推定工程（ステップＳ３０４）で算出された当該三角形パッチに関する距離計測値のばらつき度に比例する数の小平面を選択すればよい。
【００９１】
小平面と距離画像との対応付け及び対象物体の位置姿勢推定は、特許文献１と同様にして実現できる。
【００９２】
（第９実施形態）
第１実施形態乃至第８実施形態では、距離計測の計測値に偶然誤差（計測値のばらつき）が含まれる場合を考慮した。そして、モデル上から距離画像と対応付ける局所領域を距離計測誤差の特性とその大きさに応じてサンプリングすることにより、位置姿勢推定精度を保ちつつ、対応付けの処理負荷を軽減する方法を説明した。
【００９３】
本実施形態では、距離計測の計測値に系統誤差（真値からのオフセット値）が含まれる場合の位置姿勢計測方法について説明する。
【００９４】
距離計測は、例えばスポット光方式（三角測量式）を用いる。図７を参照して、この際に系統誤差が発生する仕組みを説明する。図７において、表面７０１は距離計測の対象物体を構成する表面の一部である。照明７０３は距離計測装置を構成するスポット光を発生する光源である。撮影装置７０４は距離計測装置を構成する撮影装置である。スポット光方式では、照明７０３から照射された光が対象物体に直接当たったスポット（以下、「１次反射スポット又は１次反射点」と称する。）７０５の像を撮影装置７０４で撮影した画像上で探索する。そして、その像の画像座標に基づいて三角測量の原理で１次反射スポット７０５までの距離を算出する。
【００９５】
また、スポット光が照射される表面７０１の近傍に別の表面７０２がさらに存在する場合を考える。その場合、スポット光は表面７０２上で反射されて、スポット（以下、「２次反射スポット又は２次反射点」と称する。）７０６が形成され、画像中で観察されることがある。このとき、２次反射スポット７０６の像が十分明るければ、スポット像を探索した結果が誤って２次反射スポット７０６を指すことがある。この誤ったスポット像の座標から算出される距離値は真の距離値からオフセットしており、スポット像の座標の誤差が大きくなるにつれて距離計測値のオフセットが大きくなる。
【００９６】
そこで、本実施形態では、真のスポット（１次反射スポット）像の座標と偽のスポット（１次反射スポット）像の座標との差異から計測誤差を推定し、計測誤差の大きさに基づいてモデル表面上のサンプル点の密度を決定する。このとき、系統誤差を含むサンプル点の数が増加すると、位置姿勢推定の結果もオフセットするので、系統誤差が大きいほどサンプル点の密度を減らすようにする。
【００９７】
本実施形態に係る処理全体の流れは第１実施形態で説明した図３のフローチャートと同様である。ただし、初期化処理（ステップＳ３０１）、距離計測誤差の推定処理（ステップＳ３０４）、及びサンプル点の決定処理（ステップＳ３０５）の内容は第１実施形態と異なるので、以下、それらの３つのステップのみ説明する。
【００９８】
初期化処理（ステップＳ３０１）では、第１実施形態の初期化処理で外部記憶装置から読み込むデータに加えて、距離画像計測装置における照明部と撮像部との相対位置姿勢を外部記憶装置２０５から読み込む。
【００９９】
次に図８を参照して、本実施形態に係る距離計測誤差の推定処理（ステップＳ３０４）の流れを示すフローチャートについて説明する。
【０１００】
ステップＳ８０１において、形状モデルに含まれる三角形ポリゴンの中から未処理のポリゴンを１つ選択する。ステップＳ８０２からステップＳ７１０までの各ステップでは、ステップＳ８０１で選択された三角形ポリゴンを処理の対象とする。なお、本ステップで選択された三角形ポリゴンの識別番号はｉであるとする。添え字のｉは本ステップで選択された三角形ポリゴンに関するものであることを示す指標である。
【０１０１】
ステップＳ８０２において、１次反射スポットの像が距離計測装置の撮像部によって観測されるか否かを判定する（第１判定）。具体的には、ステップＳ８０１で選択された三角形ポリゴンの重心と、光源／撮像部とを結ぶ２直線と、ステップ８０１で選択された三角形ポリゴン以外の三角形ポリゴンとが交差しなければ可視、交差すれば不可視と判定する。なお、この判定を行うときの光源／撮像部の座標は、ステップＳ３０３で算出された対象物体の概略位置姿勢、およびステップＳ３０１で読み込まれた撮像部に対する光源の相対位置姿勢に基づいて、対象物体の座標系に変換したものを用いる。この判定処理において可視であると判定された場合は（ステップＳ８０２；ＹＥＳ）、ステップＳ８０３に進む。一方、不可視であると判定された場合は（ステップＳ８０２；ＮＯ）、ステップＳ８０９に進む。
【０１０２】
次にステップＳ８０３において、撮像画像上に１次反射スポット像の座標を投影する（第１投影）。具体的には、ステップＳ８０１で選択された三角形パッチの重心位置を、ステップＳ３０１で読み込まれたカメラパラメータおよびステップＳ３０３で算出された対象物体の概略位置姿勢とに基づいて、撮影画像上に投影した２次元座標を算出する。
【０１０３】
続くステップＳ８０４において、２次反射スポット像の撮影画像上での座標を算出する。このために、光源位置と識別番号ｉの三角形ポリゴンの重心を結ぶ直線に対して当該ポリゴンの法線方向を二等分角とする方向、すなわちスポット光の反射方向を算出する。そして、１次反射スポット位置を始点として当該反射方向に延びる直線と形状モデルとの交点を求める。この交点が２次反射スポットとなる。この交点が存在するか否かが判定される（第２判定）。交点が存在すると判定された場合、ステップＳ３０１で読み込まれたカメラパラメータおよびステップＳ３０３で算出された対象物体の概略位置姿勢とに基づいて、この２次反射スポットを撮影画像上に投影する（第２投影）。この２次元座標がステップ８０４で求める座標となる。
【０１０４】
続いてステップＳ８０３およびステップＳ８０４で算出された１次反射スポット像および２次反射スポット像の座標に基づいて、２次反射スポット像が偽のスポット像として探索される可能性を評価する（ステップＳ８０５およびステップＳ８０６）。
【０１０５】
具体的にはステップＳ８０５において、２次反射スポット像の位置と、光源からのスポット光照射方向によって決定される撮影画像上のエピポーラ線（投影線）との距離が、所定の閾値未満か否かが判定される（第１判別）。閾値は、距離計測装置におけるスポット探索処理の対象範囲によって決定される値である。そして、１次反射スポットと２次反射スポットとの間の３次元的な距離が所定の閾値未満か否かが評価される（第２判別）。閾値は、対象物体表面の反射率に依存し、実験によって決定される値である。ステップＳ８０６において、これらの評価値の全てが閾値未満である場合に探索誤りが発生すると判定されて（ステップＳ８０６；ＹＥＳ）、ステップＳ８０７に進む。一方、何れかの評価値が閾値以上である場合は、２次反射スポット像が偽のスポットとして探索される可能性はないと判定されて（ステップＳＳ８０６；ＮＯ）、ステップＳ８０８に進む。
【０１０６】
ステップＳ８０７において、距離計測結果のオフセットの度合いであるｓｉを、以下の式（１１）に基づいてオフセット値算出する。
【０１０７】
【数１１】
【０１０８】
ここで、ΔＺは距離Ｚの位置におけるオフセットの大きさである。ｇｉはステップＳ８０３およびステップＳ８０４で算出された座標間の距離である。ｆは距離計測装置における撮像部の焦点距離である。Ｌは照明部と撮像部の相対位置姿勢から算出される撮像部の焦点と照明の光軸との距離である。
【０１０９】
一方、ステップＳ８０８では、距離計測結果のオフセットの度合いｓｉが０に設定される。オフセットの度合いが０であることは、距離値の系統誤差が存在しないことを示している。
【０１１０】
ステップＳ８０９では、距離計測結果のオフセットの度合いｓｉを−１に設定する。オフセットの度合いが−１であることは、三角形ポリゴンが不可視であることを示す特別な値であることを意味する。
【０１１１】
なお、ステップＳ８０７、ステップＳ８０８、ステップＳ８０９で設定されたオフセットの度合いｓｉは、ポリゴンの識別番号ｉとセットにされてメモリ２０２に格納される。
【０１１２】
最後にステップＳ８１０において、形状モデルに含まれるすべての三角形ポリゴンに対して、距離計測のオフセット度が設定されたか否かが判定される。すべてのポリゴンを処理済みである場合は、距離計測誤差推定の処理を終了する。一方、未処理のポリゴンが残っている場合は、ステップＳ８０１に戻る。
【０１１３】
以上が本実施形態に係る距離計測誤差の推定処理（ステップＳ３０４）の流れである。
【０１１４】
最後に、本実施形態に係る局所領域の選択処理、つまりサンプル点の決定処理（ステップＳ３０５）について説明する。ステップＳ３０５では、形状モデルに含まれる各三角形ポリゴンに関して、式（１２）に基づいてサンプル点の数Ｎｉが算出される。
【０１１５】
【数１２】
【０１１６】
式（１２）における記号Ｓ、Ｄ、ｄｉ、Ａｉ、βｉの意味は第１実施形態で説明した式（２）の各記号と同様の意味である。ｓｉはステップＳ３０４で算出された距離計測値のオフセット度である。Ｃｓはサンプル点数に対するオフセットの度合いの寄与度を調整するための係数（定数）である。
【０１１７】
ここで算出されたサンプル数数Ｎｉに基づいて、処理対象の三角形ポリゴン上でのサンプル点の位置を決定する方法は第１実施形態で説明した方法と同様であるので、説明を省略する。
【０１１８】
以上説明したように、本実施形態の方法を用いれば、距離画像計測値のオフセットの度合いに応じて適応的に距離画像と対応付けるモデル面上のサンプル点を選択することが出来る。よって、対応点の数を減らして処理を高速化しながら、推定する位置姿勢の精度を保つことが可能である。
【０１１９】
なお、本発明の実施形態は上記に限らず様々な変形例が存在する。以下の第１０実施形態から第１３実施形態が第２実施形態の変形例である。
【０１２０】
（第１０実施形態）
第１実施形態の変形例である第２実施形態乃至第８実施形態で説明した内容は、本実施形態にも適用できる。
【０１２１】
（第１１実施形態）
上記の説明では、それぞれの三角形ポリゴンに関する距離計測のオフセット度を各三角形ポリゴンの重心位置で評価している。しかし、誤差の評価に用いる点の位置はこれに限るものではない。また、評価に用いる点の数も１点に限らず、複数の点であってもよい。このとき、複数の点で算出されたオフセット度を平均するなどして、三角形ポリゴン１つにつき１つのオフセット度を算出してもよい。あるいは、１つの三角形ポリゴンを複数の小領域に分割し、それぞれの小領域についてオフセット度を算出して、小領域ごとにサンプル数を決定してもよい。
【０１２２】
（第１２実施形態）
上記の説明では、２次反射したスポット光による誤差の大きさを推定しているが、さらに他の要因による誤差を評価してもよい。
【０１２３】
例えば、距離計測装置の光源以外の光源（例えば照明装置）が存在するとき、光源の位置、照射方向、及び計測対象物体の鏡面反射特性が分かれば、その光源の光によって物体表面に生ずる鏡面反射像の位置を推定できる。ここで推定された鏡面反射像および１次反射スポット像の画像上での位置関係から、距離計測値のオフセット度合いを計算できる。
【０１２４】
また、図９に示すように、不透明な素材の部分９０１および透明な素材の部分９０２から構成されている計測対象物体があるとする。このとき、距離計測装置を構成する光源９０３から照射されたスポット光は透明部分９０２の表面上の点９０５にスポットを発生させる。また、透明部分９０２を通って、不透明部分９０１と透明部分９０２との境界上の点９０６にもスポットを発生させる。このような場合も、スポット光の光路をシミュレートして画像上でのスポット間の距離を算出することで、その距離に応じたオフセット度合いを計算できる。
【０１２５】
上記の他、図１０に示すように距離の異なる複数点が観測され、距離計測値にオフセット誤差が発生する場合がある。図１０において、計測対象物体１００１および計測対象物体１００７は計測対象となる物体を示す。光源１００２および光源１００８は距離計測装置を構成するスポット光源である。撮像部１００３および撮像部１００９は距離計測装置を構成する撮像部である。図１０（Ａ）は、１筋のスポット光が照射される領域１００４の中に存在する複数の位置に複数のスポット（１００５および１００６）が生じて、距離値のオフセット誤差の原因となる場合の図である。また、図１０（Ｂ）は、１つの画素に異なる２つの方向のスポット光によるスポット（１０１１および１０１２）が観察されて、距離値のオフセット誤差の原因となる場合の図である。図１０（Ａ）のような場合、距離値のオフセット度合いは例えば以下の方法により算出される。すなわち、光源１００２からモデルを構成する小領域の重心方向に、スポット光の走査方向に微小角度をなす２本の線分を延ばし、対象物体のモデルと交差した２点の撮像部からの距離の差をオフセット度合いとすればよい。図１０（Ｂ）の場合は、同様に、撮像部１００９から線分を伸ばせばよい。
【０１２６】
（第１３実施形態）
上記の説明では、距離画像計測がスポット光方式によるものであるとしている。しかし、距離計測方式はこれに限るものではない。光切断方式など他のアクティブ方式であってもよい。また、アクティブ方式に限らず、ステレオ方式などパッシブ方式であってもよい。パッシブ方式の場合、例えば計測対象物体上に繰り返し模様があることによって２つの画像間の対応点探索を誤ることを原因として想定し、模様の繰り返し間隔に基づいて距離計測値のオフセット度を評価することができる。
【０１２７】
（第１４実施形態）
第１実施形態および第９実施形態では、距離計測の誤差（ばらつきやオフセット）をシミュレーションによって求めた。しかし、誤差を推定する方法はこれに限るものではなく、実際に対象物体を計測した距離データに基づいて決定することができる。
【０１２８】
本実施形態の方法に基づく処理は、モデル上のサンプル点決定に必要なオフセット誤差データを準備するオフライン処理と、誤差データに基づいてサンプル点を決定して対象物体の位置姿勢を算出するオンライン処理とに分かれる。なお、本実施形態で想定するモデルは三角形パッチから構成されるパッチモデルとする。また、距離計測の方式は任意であるが、例えば光切断法を用いる。
【０１２９】
オフライン処理では、対象物体を中心位置に配置した仮想的なＧｅｏｄｅｓｉｃ ｓｐｈｅｒｅを想定し、その各頂点に相当する位置に配置した距離計測装置で対象物体の距離画像を撮影する。その撮影されたそれぞれの距離画像に対して、以下の(1)〜(5)の処理を行う。
【０１３０】
(1) 対象物体の概略の位置姿勢を推定する。その方法は第１実施形態または第９実施形態と同様である。
【０１３１】
(2) 距離画像の各画素に対応するモデル上の点を探索し、距離画像の画素に相当する３次元点と、対応するモデル上の点を対応点対として記憶する。
【０１３２】
(3) 対応点対間の距離の総和が最小となるようにモデルの位置姿勢を最適化することによって、対象物体の位置姿勢を推定する。本ステップで算出された値を対象物体の位置姿勢の真値とする。
【０１３３】
(4) モデルを真値の位置姿勢に配置した状態で、距離画像の各画素に対応するモデル上の点の位置から算出された距離値と、距離画像上の当該画素の距離値との差を求め、ポリゴンパッチごとに距離値の差の平均値を算出する。
【０１３４】
(5) Ｇｅｏｄｅｓｉｃ ｓｐｈｅｒｅの頂点ごと（距離画像ごと）に用意したモデルデータのポリゴンパッチごとに、(4)で算出された平均値を距離画像計測のオフセット誤差として登録する。
【０１３５】
一方、オンライン処理では、以下の(6)〜(11)の処理を行う。
【０１３６】
(6) 対象物体の距離画像を計測し、対象物体の概略の位置姿勢を推定する。
【０１３７】
(7) (6)で算出された概略位置姿勢に基づいて、(6)の距離画像撮影時の視点に最も近い頂点をＧｅｏｄｅｓｉｃ ｓｐｈｅｒｅ上の頂点から選択する。
【０１３８】
(8) (7)で選択された頂点に登録されたモデルデータから各ポリゴンのオフセット誤差を読み出し、誤差値に基づいてサンプル点数を算出する（サンプル点数の決定方法の詳細は第９実施形態と同様である）。
【０１３９】
(9) (8)で算出されたサンプル点数に基づいて、各ポリゴンのサンプル点の位置を決定する（サンプル点位置の決定方法の詳細は第１実施形態、第９実施形態と同様である）。
【０１４０】
(10) (9)で位置が決定されたサンプル点と距離画像とを対応付けて、対応点対を求める（サンプル点と距離画像との対応付け方法の詳細は第１実施形態、第９実施形態と同様である）。
【０１４１】
(11) (10)で求められた対応点対間の距離の総和を最小化するように最適化計算して、対象物体の位置姿勢を算出する（位置姿勢算出方法の詳細は実施形態１、第９実施形態と同様である）。
【０１４２】
本実施形態に述べた方法によれば、対象物体を実際に距離計測したデータに基づいてサンプル点を決定することができる。その結果、シミュレーションで距離計測誤差を推定する方法に比べて誤差の推定精度が向上し、ひいては位置姿勢推定の精度を保ちながら処理速度を向上する効果が高くなる。
【０１４３】
なお、本発明の実施形態は上記に限らず様々な変形例が存在する。以下の第１５実施形態または第１６実施形態が第１４実施形態の変形例である。
【０１４４】
（第１５実施形態）
第１実施形態の変形例である第２実施形態乃至第８実施形態は、本実施形態にも適用できる。
【０１４５】
（第１６実施形態）
上記の説明では、距離計測のオフセット誤差を実測の距離画像から推定する方法を述べたが、ばらつき誤差を推定することも可能である。具体的には、実測と真値の距離値の差について、ポリゴンパッチごとに求めた標本分散をもって、当該ポリゴンのばらつき誤差とすればよい。
【０１４６】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置及び姿勢を計測する技術に関し、特に対象物体を撮影した距離画像に基づいて対象物体の位置及び姿勢を算出する位置姿勢計測装置、位置姿勢計測方法、およびプログラムに関する。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、従来人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。複雑なタスクの代表的な例として、工業製品の組立が挙げられる。ロボットが自律的に組立作業を行うためには、ハンドなどのエンドエフェクタによって部品を把持することが必要となる。ロボットハンドによって部品を把持するためには、把持の対象となる部品とロボットとの相対的な位置及び姿勢を計測し、計測結果に基づいて移動計画を策定して実際にアクチュエータを制御する必要がある。ロボットの位置姿勢計測は、ロボットに搭載されたカメラや距離センサを用いて行われており、２次元画像や距離画像を用いる方法が代表的である。特にオフィスや工場などのシーンや工業製品の部品など、対象が直線や曲線、濃淡変化の少ない面など単調な特徴しか持たない場合には距離画像を用いることが多い。
【０００３】
非特許文献１では、距離画像へのモデルフィッティングによる物体の位置及び姿勢の計測を行う方法が開示されている。この方法では、距離画像を３次元点群データに変換し、点群データに対象物体の３次元形状モデルを当て嵌めることにより物体の位置及び姿勢を計測する。すなわち、位置及び姿勢の概略値をもとに、点群データの各点について３次元形状モデルの近傍の面を探索し、点と面との距離の和を最小化するように位置及び姿勢の最適化を繰り返すことにより位置及び姿勢を算出する。
【０００４】
特許文献１では、非特許文献１と同様に観測画像へのモデルフィッティングにより、物体の位置及び姿勢を推定する方法が開示されている。このとき、物体のモデルから低曲率の面を選択し、選択した面と対応する距離計測データ上の面との距離の差を最小化する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】米国特許6504957号公報
【特許文献２】特開平01-009307号公報
【非特許文献】
【０００６】
【非特許文献１】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.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
非特許文献１のように距離画像を用いて物体の位置及び姿勢を推定する手法は、距離画像の点群データの各点に対応する３次元モデルの面の探索に多くの計算時間が必要であるという問題があった。
【０００８】
この問題の解決策として、対応の数を減らす方法が考えられる。しかしながら、計測データには必ず誤差が含まれる。対応の数を減らすと、それぞれの対応点が位置姿勢推定の計算に寄与する度合いが相対的に高まる。従って、対応点の中に誤差の大きい計測データに基づくものが含まれている場合、位置姿勢推定の精度が低下するという問題がある。
【０００９】
上記の課題に対して、特許文献１では、計測対象物体上で低曲率の部位は距離計測誤差が小さいと想定し、モデル上の低曲率の部位に処理対象を限定することによって、精度を低下させずに処理速度を向上することを狙っている。
【００１０】
しかしながら、実際には低曲率の部位であっても距離計測の精度が高いとは限らない。図１を参照して説明する。図１は、上記の現象を説明する原理図である。図１において、撮像部１０１は距離計測装置を構成する撮像部である。計測対象物体１０２は低曲率の部位を有する計測対象物体である。計測対象物体１０２の表面上の２点として点１０３および点１０４を考える。この場合、点１０４の方が点１０３よりも曲率が低い。しかしながら、点１０３および点１０４を含む領域に相当する画素を観測する領域１０５および領域１０６を考えると、領域１０６内に含まれる計測対象物体上の表面の撮像部１０１からの距離の分布は、領域１０５よりも広範囲に渡る。従って、点１０４の距離計測値のばらつき誤差は、点１０３の距離計測値のばらつき誤差よりも大きい。このように、面の傾きによっては、高曲率の部位よりも低曲率の部位の方が、距離計測精度が低いことがある。そのような場合、特許文献１の方法に従えば、距離計測値のばらつき誤差が大きい領域（低曲率な領域）を選択して物体の位置姿勢を推定することになるため、特許文献１の方法を用いない場合よりも、位置姿勢推定の精度が低下してしまう。
【００１１】
つまり、特許文献１の方法では、計測誤差の少ないデータを確実に選択できないために、位置姿勢推定精度が低下することがあるという問題があった。
【００１２】
上記の課題に鑑み、本発明は、位置姿勢推定の精度低下を防ぎながら、処理速度を維持向上させることを目的とする。
【課題を解決するための手段】
【００１３】
上記の目的を達成する本発明に係る位置姿勢計測装置は、
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報のばらつきの程度を示すばらつき値を算出する算出手段と、
前記ばらつき値が小さい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、を備えることを特徴とする。
【００１４】
また、上記の目的を達成する本発明に係る位置姿勢計測装置は、
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報に含まれる計測誤差を示すオフセット値を算出するオフセット値算出手段と、
前記オフセット値が大きい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、を備えることを特徴とする。
【発明の効果】
【００１５】
本発明によれば、位置姿勢計測のために用いる対応点の密度を距離計測の誤差に応じて制御することにより、位置姿勢計測精度の低下を抑えながら、処理速度を向上させることができる。
【図面の簡単な説明】
【００１６】
【図１】低曲率の部位であっても距離計測の精度が高いとは限らないことを説明する原理図。
【図２】本発明の装置の構成を表すブロック図。
【図３】第１実施形態に係る処理全体の流れを示すフローチャート。
【図４】距離画像計測における偶然誤差発生の原理を説明する図。
【図５】第１実施形態に係る距離計測誤差推定処理の流れを示すフローチャート。
【図６】三角形パッチを小領域に分割する方法の原理を説明する図。
【図７】距離画像計測における系統誤差発生の第１の原理を説明する図。
【図８】第２実施形態に係る距離計測誤差推定処理の流れを示すフローチャート。
【図９】距離画像計測における系統誤差発生の第２の原理を説明する図。
【図１０】距離画像計測における系統誤差発生の第３の原理を説明する図。
【発明を実施するための形態】
【００１７】
（第１実施形態）
以下、対象物体のモデル上から距離画像と対応付ける局所領域を距離計測誤差の特性と大きさに応じてサンプリングすることにより、位置姿勢推定精度を保ちつつ、対応付けの処理負荷を軽減する方法を説明する。距離画像とは、対象物体までの距離情報が画素値として保持された画像である
本実施形態では、距離計測の計測値に偶然誤差（距離計測値のばらつき）が含まれる場合を説明する。ここでは、距離計測値の系統誤差（真値からのオフセット）は微小であるとする。
【００１８】
本実施形態では、対象物体の形状モデルの表面上からサンプル点を選択し、距離画像から変換した３次元計測点群とサンプル点とを対応付ける。そして、対応点対間の距離の総和を最小化することによって、対象物体の位置姿勢決定（推定）を行う。
【００１９】
サンプル点を選択する際は、形状モデル表面上の小領域（所定領域）ごとに、その小領域に相当する計測対象物体表面上の領域を距離計測したときの偶然誤差の大きさ（計測値のばらつきの程度）を推定する。そして、距離計測値のばらつきの大きさに応じて、当該小領域からサンプリングする点の密度を決定する。具体的には、距離計測値のばらつきが大きいほど、サンプル点の密度を高くする。つまり、ばらつき値が大きい程多くサンプリングする。逆にばらつき値が小さい程少なくサンプリングする。ただし、何れの小領域に関しても、距離画像の画素ごとに３次元計測点群と形状モデルを対応付ける場合よりもサンプル点の数が少なくなるようにサンプル点の上限数を設定する。このようにサンプリング密度を制御することにより、距離画像の画素ごとに対応付けを行う方法に比べて対応点の数を減らすことができる。そして、計測値のばらつきが大きい領域については多数の点の平均をとる効果を作用させることにより、誤差の影響を軽減し、位置姿勢計測の結果が真値に近くなることが期待される。
【００２０】
なお、距離計測の測定誤差は、距離計測装置の計測方向と計測対象物体表面上の各小領域の法線方向とのなす角度に基づいて推定する。具体的には、計測方向と物体表面が垂直に近いほど計測値のばらつきが小さく、逆に計測方向と物体表面が平行に近いほど計測値のばらつきが大きいと推定する。
【００２１】
以下、本実施形態に係る位置姿勢計測装置の詳細について説明する。
【００２２】
まず、図２を参照して、本発明に係る位置姿勢計測装置のハードウェア構成について説明する。図２において、ＣＰＵ２０１はＣＰＵであり、装置全体の動作を制御する。メモリ２０２はメモリであり、ＣＰＵ２０１の動作に用いるプログラムやデータを格納する。バス２０３はバスであり、各構成モジュール間を接続してデータ転送を可能にする。インタフェース２０４はバス２０３と各種装置とのインタフェースである。また、外部記憶装置２０５はＣＰＵ２０１に読み込まれるプログラムやデータを格納する外部記憶装置である。キーボード２０６およびマウス２０７はプログラムを起動したり、プログラムの動作を指定したりするための入力装置を構成するキーボードおよびマウスである。表示部２０８はプロセスの動作結果を表示するための表示部であり、液晶画面等で構成される。データ入出力部２０９は装置外部とのデータ入出力部である。不図示の距離画像計測装置はデータ入出力部２０９を介して本発明に係る位置姿勢計測装置に接続されている。
【００２３】
次に、図３を参照して、本実施形態に係る位置姿勢計測装置の全体処理の流れを示すフローチャートについて説明する。ステップＳ３０１において、処理で用いるデータの初期設定を行う。具体的には、外部記憶装置２０５から形状モデルデータ、標準サンプル数、及び基準距離をメモリ２０２にロードする。標準サンプル数と基準距離については後述する。また、不図示の距離画像計測装置の撮像部のカメラパラメータ（主点位置、焦点距離、画像歪み係数、画素数等）をデータ入出力部２０９に接続された距離画像計測装置から取得する。
【００２４】
形状モデルは、物体の表面を三角形平面の集合として表す三角形ポリゴンパッチモデルであるとする。形状モデルデータは、三角形ポリゴンの各頂点の識別番号と座標、各三角形ポリゴンの識別番号とそのポリゴンを構成する頂点の識別番号、各三角形ポリゴンの法線ベクトルから構成される。三角形ポリゴンの法線ベクトルは、形状モデルの内部から外部へ向かう方向であるとする。
【００２５】
カメラパラメータは例えば非特許文献２で開示される方法により事前に校正しておく。ステップＳ３０１において入力されるカメラパラメータを用いることによって、距離画像計測装置の撮像部の３次元座標系における点を距離画像上（２次元座標系上）に投影した２次元座標を算出することができる。
【００２６】
続くステップＳ３０２において、データ入出力部２０９に接続された不図示の距離画像計測装置から計測対象物体を計測した距離画像データを取得し、メモリ２０２に格納する。なお、距離画像の撮影は、例えば光切断法やステレオ法により撮影する。
【００２７】
ステップＳ３０３において、計測対象物体の概略位置姿勢（距離計測装置の撮像部に対する計測対象物体の概略の相対位置姿勢）を決定する。この概略位置姿勢決定処理には、例えば特許文献２に開示される方法を用いることができる。
【００２８】
ステップＳ３０４において、ステップＳ３０２で入力された距離画像データの誤差を推定する。本ステップの処理については後述する。
【００２９】
続いて、ステップＳ３０５において、ステップＳ３０４で推定された距離画像データの誤差に基づいて、形状モデル表面上から距離画像データと対応付ける所定領域（局所領域）、すなわち点群を選択する。ステップＳ３０５における形状モデル上からの点群選択処理の詳細については後述する。
【００３０】
次に、ステップＳ３０６において、ステップＳ３０５で選択されたサンプル点を距離画像と対応付ける。具体的には、ステップＳ３０３で算出された計測対象物体の概略位置姿勢に基づいて、対象物体の座標系における各サンプル点の３次元座標を距離計測装置の撮像部の座標系における３次元座標に変換する。さらに、ステップＳ３０１で入力されたカメラパラメータに基づいて、撮像部の座標系における各サンプル点の３次元座標を距離画像上に投影した点の２次元座標を算出する。このようにして算出された座標値は距離画像の画素の座標と必ずしも一致しないので、投影点の最近傍（最近接）の画素を距離画像上で探索する。そして、探索結果の画素の２次元座標（２次元計測点）および距離値から該画素の撮像部座標系における３次元座標を算出し、ステップＳ３０３で算出された計測対象物体の概略位置姿勢に基づいて、対象物体の座標系における３次元座標に変換する。このようにして算出された距離計測点の３次元座標は、対応するサンプル点の３次元座標と組にして、メモリ２０２に記憶領域を確保した対応点対リストに追加する。
【００３１】
なお、上述したサンプル点と距離画像の対応付け処理に加えて、誤対応を排除する処理を加えてもよい。具体的には、距離画像上の対応点において近傍の距離計測点群データから当該対応点における法線方向を算出し、対象物体の座標系における方向に変換する。このようにして算出された法線と、当該対応点に対応するサンプル点が含まれる三角形パッチの法線方向とを比較して、法線方向が大きく異なる場合は当該対応点を対応点対リストに追加しないようにすればよい。
【００３２】
最後に、ステップＳ３０６で求められた対応点対間の距離の総和が最小となるように最適化問題を解くことによって、計測対象物体の位置姿勢を算出する。本ステップの詳細については、後述する。
【００３３】
ここで、距離計測の誤差量推定処理（ステップＳ３０４）の詳細を説明する。図４を参照して、本実施形態に係る計測値のばらつきの程度を示すばらつき値を推定する方法について説明する。
【００３４】
図４は、距離画像計測装置の撮像部４０１の観測方向と計測対象物体の表面４０３又は表面４０５との位置関係を示した図である。表面４０３が撮像部４０１の観測方向と垂直に近い場合、表面４０５が撮像部４０１の観測方向と平行に近い場合を表している。図４において、領域４０２は撮像部４０１のある１画素に相当する領域であるとする。計測対象物体の表面領域が、それぞれ範囲４０４又は範囲４０６に分布している。すなわち、当該画素の距離情報のばらつき値は、範囲４０４又は範囲４０６の間隔に等しい。従って、距離情報の値は、撮像装置の観測方向と計測対象物体の面の法線とのなす角度によって計量できる。
【００３５】
図５を参照して、距離計測の誤差量推定処理の流れを示すフローチャートについて説明する。ステップＳ５０１において、形状モデルに含まれる三角形ポリゴンの中から未処理のポリゴンを１つ選択する。ここで、ステップＳ５０２からステップＳ５０６までの各ステップでは、ステップＳ５０１で選択された三角形ポリゴンを処理の対象とする。なお、本ステップで選択された三角形ポリゴンの識別番号はｉであるとする。以下、添え字のｉは本ステップで選択された三角形ポリゴンに関するものであることを示す指標である。
【００３６】
ステップＳ５０２において、距離画像計測装置の観測方向と処理対象ポリゴンの法線方向とのなす角度αｉ（−π≦αｉ≦π）を算出する。なお、距離画像計測装置の観測方向は、ステップＳ３０３で算出された対象物体の概略位置姿勢に基づいて、対象物体の座標系における方向ベクトルとして表されているとする。
【００３７】
次に、ステップＳ５０３において、ステップＳ５０２で算出された角度αｉに基づいて、処理対象のポリゴンが距離画像計測装置から可視であるか、あるいは不可視であるかを判定する。具体的には、−（π／２＋φ）≦αｉ≦π／２＋φを満たさない場合は、距離画像計測装置が処理対象ポリゴンの表側を観察しているので可視と判定し、ステップＳ５０４に進む。一方、−（π／２＋φ）≦αｉ≦π／２＋φを満たす場合、距離画像計測装置が処理対象ポリゴンの裏側を観察しているか、あるいは観察方向とポリゴン平面が平行である。従って、処理対象ポリゴンは不可視と判定し、ステップＳ５０５に進む。なお、φは距離計測装置の観測方向が対象物体表面と平行に近い場合に（例えば物体表面の反射光量が低下することにより）計測不能になる限界を表す角度定数である。
【００３８】
上記の説明では、距離画像計測装置の観測方向と処理対象ポリゴンの法線方向とのなす角度のみで、当該ポリゴンの可視／不可視を判定している。さらに、距離画像計測装置から見て当該ポリゴンが他のポリゴンに隠されている場合に当該ポリゴンを不可視と判定する処理を加えてもよい。このような判定処理はコンピュータグラフィクスにおけるオクルージョン判定技術を用いて実現できる。
【００３９】
ステップＳ５０４において、以下の式（１）に基づいて、距離計測値のばらつき度ｕｉを算出する。ただし、βｉ＝π−αｉである。
【００４０】
【数１】
【００４１】
一方、ポリゴンが不可視の場合、ステップＳ５０５において、距離計測値のばらつき度ｕｉを−１に設定する。ただし、このステップで設定するばらつき度の値は処理対象ポリゴンが可視の場合に取り得ない値であればよく、−１に限らない。
【００４２】
なお、ステップＳ５０４で算出されたばらつき度あるいはステップＳ５０５で設定されたばらつき度は、ポリゴンの識別番号とセットにしてメモリ２０２に格納する。
【００４３】
ステップＳ５０４またはステップＳ５０５の処理が終了すると、処理はステップＳ５０６に進む。
【００４４】
ステップＳ５０６において、形状モデルに含まれる三角形ポリゴンの全てに対して、ばらつき度を設定する処理を完了したか否か判定する。完了したと判定された場合は（ステップＳ５０６；ＹＥＳ）、誤差量推定処理全体を終了する。一方、全てのポリゴンを処理済みでないと判定された場合は（ステップＳ５０６；ＮＯ）、ステップＳ５０１に戻る。
【００４５】
以上が距離計測の誤差量推定処理の流れである。
【００４６】
続いて、局所領域の選択処理（ステップＳ３０５）の詳細を説明する。
【００４７】
ステップＳ３０５では、形状モデルに含まれる各三角形ポリゴンに関して、式（２）に基づいてサンプル点の数Ｎｉを算出する。
【００４８】
【数２】
【００４９】
式（２）において、ＳおよびＤは、ステップＳ３０１でメモリ２０２に格納された標準サンプル数および基準距離である。標準サンプル数とは、距離計測装置から基準距離の位置にある距離計測装置の計測方向に垂直な単位面積の平面を距離画像に投影した領域に含まれる画素の数であり、サンプル点の上限数を規定している。ｄｉはステップＳ３０３で推定された対象物体の概略位置姿勢に基づいて算出される距離計測装置と識別番号ｉの三角形ポリゴンとの距離である。（ｄｉ）／Ｄ）２を掛け合わせることによって、ある三角形ポリゴンを距離画像上に投影した領域に含まれる画素数の増減の効果をサンプル点の数に反映させることができる。
【００５０】
また、Ａｉは識別番号がｉのポリゴンの面積である。そして｜cosβｉ｜は三角形ポリゴンを距離画像上に投影した領域の面積が当該ポリゴンの向きによって減少する比率である。ｕｉはステップＳ３０４で算出された距離計測値のばらつき度である、Ｃｕはサンプル点の密度に対する距離計測値のばらつき度の寄与度を調整する定数である。
【００５１】
なお、処理対象のポリゴンが不可視の場合（ｕｉ＝−１）、そのポリゴンからはサンプル点を選ばないので、Ｎｉ＝０とする。
【００５２】
サンプル点は、三角形ポリゴンを同一面積で三角形の小領域に分割したときの各小領域から１点ずつ配置する。
【００５３】
図６を参照して、上記の三角形ポリゴンの分割方法の原理について説明する。図６（Ａ）に示される三角形ポリゴンの各辺を２分割した点を結んで４つの小領域に分割した例が図６（Ｂ）である。図６（Ａ）に示される三角形ポリゴンの各辺を３分割して９つの小領域に分割した例が図６（Ｃ）である。同様にして辺の分割数を増やすことによって、三角形ポリゴンをべき乗数個の小領域に分割できる。そこで、式（２）で求めたサンプル点数Ｎｉに最も近いべき乗数を求め、図６に示すようにべき乗数個にポリゴンを分割した各小領域の重心をサンプル点とする。ここで求められたサンプル点群の座標（対象物体の座標系における座標値）は、三角形ポリゴンの識別番号とともにメモリ２０２に格納される。
【００５４】
最後に、対象物体の位置姿勢算出処理（ステップＳ３０７）の詳細について説明する。本ステップでは、対応点対リストに登録された対応点対間の距離の総和が最小になるようにするモデルの位置姿勢を算出し、その結果を対象物体の位置姿勢とする。
【００５５】
距離画像計測装置の撮像部の座標系における３次元座標は、対象物体の位置姿勢ｓによって、対象物体の座標系における３次元座標（ｘ、ｙ、ｚ）に変換することが出来る。ここで、計測対象物体の位置及び姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。いま、ステップＳ３０３で算出された対象物体の概略の位置姿勢により、撮像部座標系のある点が対象物体座標系の点（ｘ０、ｙ０、ｚ０）に変換されるとする。（ｘ、ｙ、ｚ）は対象物体の位置および姿勢により変化するものであり、（ｘ０、ｙ０、ｚ０）の近傍における１次のテイラー展開によって式（３）のように近似できる。
【００５６】
【数３】
【００５７】
メモリ２０２に格納されたある対応点対の形状モデル側の対応点が存在する三角形パッチ面の対象物体座標系における平面の方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１、a、b、c、eは定数）とする。正しいｓによって変換される（ｘ、ｙ、ｚ）は、平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１）を満たすと仮定する。式（３）をこの平面の方程式に代入すると式（４）が得られる。
【００５８】
【数４】
【００５９】
ただし、ｑ＝ａｘ０＋ｂｙ０＋ｃｚ０（定数）とした。式（４）は対応点対の全てについて成り立つため、式（５）のようなΔｓｊに関する線形連立方程式が成り立つ。
【００６０】
【数５】
【００６１】
ここで、式（５）を式（６）のように表す。
【００６２】
【数６】
【００６３】
式（６）に基づいて、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いてΔsを求める。なお、距離画像の計測点群に含まれるノイズなどによる外れ値の影響を緩和するため、次に述べるようなロバスト推定手法を用いてもよい。一般に、外れ値である計測点群データでは誤差値e−qが大きくなる。そのため、式（５）、式（６）の連立方程式に対する寄与度が大きくなり、その結果得られるΔsの精度が低下してしまう。そこで、誤差値e−qが大きいデータには小さな重みを与え、誤差e−qが小さいデータには大きな重みを与える。重みは例えば式（７）に示すようなＴｕｋｅｙの関数により与える。
【００６４】
【数７】
【００６５】
ここでνは定数である。なお、重みを与える関数は必ずしもＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差の大きいデータには小さな重みを与え、誤差の小さいデータには大きい重みを与える関数であればよい。
【００６６】
各データに対応する重みをｗｊとする。ここで式（８）のように重み行列Ｗを定義する。
【００６７】
【数８】
【００６８】
重み行列Ｗは、対角成分以外は全て０の正方行列であり、対角成分は重みｗｊである。Ｎｃは対応点対の数である。この重み行列Ｗを用いて、式（６）を以下に示す式（９）のように変形する。
【００６９】
【数９】
【００７０】
そして式（１０）のように式（９）を解くことにより補正値Δsを求める。
【００７１】
【数１０】
【００７２】
このようにして算出された補正値Δsによりモデルの位置・姿勢の概略値を補正し、その結果をメモリ２０２に格納するか、あるいはデータ入出力部２０９を介して外部の装置に出力する。
【００７３】
なお、上述した位置及び姿勢の最適化計算手法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に基づくものであるが、これに限らずＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法や最急降下法などを用いてもよい。
【００７４】
以上説明したように本実施形態の方法を用いれば、距離画像計測値のばらつきに応じて適応的に距離画像と対応付けるモデル面上のサンプル点を選択することが出来る。これによって、対応点の数を減らして処理を高速化しながら、推定する位置姿勢の精度を保つことが可能である。
【００７５】
なお、本発明は上記に限らず様々な変形例が存在する。以下の第２実施形態から第８実施形態が第１実施形態の変形例である。
【００７６】
（第２実施形態）
上記の説明では、対象物体の概略位置姿勢の算出処理（ステップＳ３０３）、距離計測の誤差推定処理（ステップＳ３０４）、局所領域の選択処理（ステップＳ３０５）、局所領域と距離画像の対応付け処理（ステップＳ３０６）、対象物体の位置姿勢推定処理（ステップＳ３０７）の各処理ステップを１回ずつ行っている。しかし、必ずしも１回ずつに限定するものではなく、これらの処理を繰り返し行って、漸近的に位置及び姿勢の推定精度を高めてもよい。このとき、ステップＳ３０７における対象物体の位置姿勢推定処理の終了後に繰り返し回数が所定回数に達したか否か判定し、所定回数に達していれば処理全体を終了し、達していなければステップＳ３０３に戻ってもよい。あるいは、ステップＳ３０７における処理の終了後に、モデルの当て嵌め誤差を算出して所定の閾値との大小関係を比較し、誤差が閾値未満であれば処理全体を終了し、閾値以上であればステップＳ３０３に戻ってもよい。ここで、モデルの当て嵌め誤差とは、例えば対応点対間の距離を全ての対応点対について足した値とする。上記フローの１回の繰り返しで推定された対象物体の位置姿勢を、次の繰り返しのステップＳ３０３において物体の概略位置姿勢として用いればよい。
【００７７】
上記の方法を用いると、位置姿勢の推定精度が高まるにつれて、距離計測誤差の推定精度も高くなるので、計測誤差の影響を軽減する効果が高くなる。
【００７８】
（第３実施形態）
計測対象物体の概略位置姿勢を算出する方法として、前述の説明では特許文献２に開示される方法を用いたが、これに限るものではない。
【００７９】
例えば、過去の計測値から物体の運動の速度や角速度を線形フィルタまたはカルマンフィルタ等の時系列フィルタリング処理により推定し、推定された速度や角速度によって現在の時刻における物体の位置及び姿勢を予測したものを用いてもよい。また、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、当該センサによる出力値を位置及び姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢が既知である場合には、その値を概略値として用いてもよい。
【００８０】
（第４実施形態）
物体の３次元形状モデルは三角形ポリゴンのみから構成されるとしているが、これに限らず、他の形状のポリゴンのみから構成されてもよい。さらに、複数種類の形状のポリゴンが混在して構成されてもよい。また、３次元形状モデルは平面パッチではなく、パラメトリック曲面パッチから構成されてもよい。さらに、平面パッチと曲面パッチが混在して構成されてもよい。ただし、曲面パッチを用いる場合は、１つのパッチの中で距離計測装置の計測方向とモデル表面との傾きが一様でない。そこで、曲面パッチを小領域に分割して、それぞれの小領域に対して距離計測の誤差を推定して、サンプル点を選択すればよい。
【００８１】
（第５実施形態）
上記の説明では物体モデル表面上の単位領域を同一面積の小領域に分割して、それぞれの小領域からサンプル点を同一個数選択する方法を説明した（ステップＳ３０５）が、サンプル点を決定する方法はこれに限られない。例えば、物体モデル表面上の単位領域をランダムにサンプリングしてもよい。
【００８２】
あるいは、各単位領域に対して予め密にサンプル点を選んでおき、距離計測の誤差のばらつきに基づいて決定されたサンプル点の個数を密なサンプル点の個数で割った「間引き率」に従って、密なサンプル点を間引いてもよい。
【００８３】
さらに、予め決められた複数の間引き率に対してサンプル点の座標を単位領域ごとに決定しておき、それらのサンプル点群の座標をモデルに登録しておいてもよい。そして、距離計測の誤差に基づいて算出された間引き率に最も近い間引き率で登録されたサンプル点をメモリ２０２から読み出して用いてもよい。
【００８４】
このように、サンプル点の決定に関する処理を予め行っておく方法を採用すると、第２実施形態で説明したように対象物体の位置姿勢推定を繰り返し行う場合に計算負荷の軽減による高速化効果が高くなる。
【００８５】
（第６実施形態）
上記の説明では、標準サンプル数は、サンプル点を距離画像上に投影した際の密度が画素密度と同等になるように設定している。しかし、標準サンプル数をさらに少なく設定することも可能である。ただし、標準サンプル数を少なくするに従って、サンプル点の数が減少して処理速度が向上するが、一般的な傾向としては位置姿勢推定の精度は低下する。
【００８６】
（第７実施形態）
上記の説明では、距離計測値のばらつき度を計測対象物体の面の傾きにより評価した。しかし、ばらつき度の評価方法はこれに限るものではない。
【００８７】
例えば、半透明物体に照射された光は物体内部で散乱して物体表面の様々な箇所で輝度が高くなる。従って、照射された光が直接当たった位置を画像上で探索したときに、誤って散乱光により輝度が高くなった部分が抽出されることがある。これが、距離計測値がばらつく原因となる。そこで、計測対象物体の部位ごとの内部散乱率によって距離計測誤差を評価してもよい。
【００８８】
あるいは、黒色の物体は光の反射が少ないため、照射された光が当たった位置の探索がノイズに埋もれて失敗することがある。これも距離計測値がばらつく原因となる。そこで、計測対象物体の部位ごとの反射率によって距離計測誤差を評価してもよい。
【００８９】
（第８実施形態）
上記の説明では、ステップＳ３０５で計測対象物体のモデルから選択する局所領域は点であった。しかし、本発明の方法は点に限らず、他の種類の局所領域を選択する場合にも適用できる。
【００９０】
例えば、局所領域として平面領域が選択されてもよい。この場合局所領域の選択工程（ステップＳ３０５）で、各三角形パッチを等しい面積の平面群に分割した中から距離計測誤差の推定工程（ステップＳ３０４）で算出された当該三角形パッチに関する距離計測値のばらつき度に比例する数の小平面を選択すればよい。
【００９１】
小平面と距離画像との対応付け及び対象物体の位置姿勢推定は、特許文献１と同様にして実現できる。
【００９２】
（第９実施形態）
第１実施形態乃至第８実施形態では、距離計測の計測値に偶然誤差（計測値のばらつき）が含まれる場合を考慮した。そして、モデル上から距離画像と対応付ける局所領域を距離計測誤差の特性とその大きさに応じてサンプリングすることにより、位置姿勢推定精度を保ちつつ、対応付けの処理負荷を軽減する方法を説明した。
【００９３】
本実施形態では、距離計測の計測値に系統誤差（真値からのオフセット値）が含まれる場合の位置姿勢計測方法について説明する。
【００９４】
距離計測は、例えばスポット光方式（三角測量式）を用いる。図７を参照して、この際に系統誤差が発生する仕組みを説明する。図７において、表面７０１は距離計測の対象物体を構成する表面の一部である。照明７０３は距離計測装置を構成するスポット光を発生する光源である。撮影装置７０４は距離計測装置を構成する撮影装置である。スポット光方式では、照明７０３から照射された光が対象物体に直接当たったスポット（以下、「１次反射スポット又は１次反射点」と称する。）７０５の像を撮影装置７０４で撮影した画像上で探索する。そして、その像の画像座標に基づいて三角測量の原理で１次反射スポット７０５までの距離を算出する。
【００９５】
また、スポット光が照射される表面７０１の近傍に別の表面７０２がさらに存在する場合を考える。その場合、スポット光は表面７０２上で反射されて、スポット（以下、「２次反射スポット又は２次反射点」と称する。）７０６が形成され、画像中で観察されることがある。このとき、２次反射スポット７０６の像が十分明るければ、スポット像を探索した結果が誤って２次反射スポット７０６を指すことがある。この誤ったスポット像の座標から算出される距離値は真の距離値からオフセットしており、スポット像の座標の誤差が大きくなるにつれて距離計測値のオフセットが大きくなる。
【００９６】
そこで、本実施形態では、真のスポット（１次反射スポット）像の座標と偽のスポット（１次反射スポット）像の座標との差異から計測誤差を推定し、計測誤差の大きさに基づいてモデル表面上のサンプル点の密度を決定する。このとき、系統誤差を含むサンプル点の数が増加すると、位置姿勢推定の結果もオフセットするので、系統誤差が大きいほどサンプル点の密度を減らすようにする。
【００９７】
本実施形態に係る処理全体の流れは第１実施形態で説明した図３のフローチャートと同様である。ただし、初期化処理（ステップＳ３０１）、距離計測誤差の推定処理（ステップＳ３０４）、及びサンプル点の決定処理（ステップＳ３０５）の内容は第１実施形態と異なるので、以下、それらの３つのステップのみ説明する。
【００９８】
初期化処理（ステップＳ３０１）では、第１実施形態の初期化処理で外部記憶装置から読み込むデータに加えて、距離画像計測装置における照明部と撮像部との相対位置姿勢を外部記憶装置２０５から読み込む。
【００９９】
次に図８を参照して、本実施形態に係る距離計測誤差の推定処理（ステップＳ３０４）の流れを示すフローチャートについて説明する。
【０１００】
ステップＳ８０１において、形状モデルに含まれる三角形ポリゴンの中から未処理のポリゴンを１つ選択する。ステップＳ８０２からステップＳ７１０までの各ステップでは、ステップＳ８０１で選択された三角形ポリゴンを処理の対象とする。なお、本ステップで選択された三角形ポリゴンの識別番号はｉであるとする。添え字のｉは本ステップで選択された三角形ポリゴンに関するものであることを示す指標である。
【０１０１】
ステップＳ８０２において、１次反射スポットの像が距離計測装置の撮像部によって観測されるか否かを判定する（第１判定）。具体的には、ステップＳ８０１で選択された三角形ポリゴンの重心と、光源／撮像部とを結ぶ２直線と、ステップ８０１で選択された三角形ポリゴン以外の三角形ポリゴンとが交差しなければ可視、交差すれば不可視と判定する。なお、この判定を行うときの光源／撮像部の座標は、ステップＳ３０３で算出された対象物体の概略位置姿勢、およびステップＳ３０１で読み込まれた撮像部に対する光源の相対位置姿勢に基づいて、対象物体の座標系に変換したものを用いる。この判定処理において可視であると判定された場合は（ステップＳ８０２；ＹＥＳ）、ステップＳ８０３に進む。一方、不可視であると判定された場合は（ステップＳ８０２；ＮＯ）、ステップＳ８０９に進む。
【０１０２】
次にステップＳ８０３において、撮像画像上に１次反射スポット像の座標を投影する（第１投影）。具体的には、ステップＳ８０１で選択された三角形パッチの重心位置を、ステップＳ３０１で読み込まれたカメラパラメータおよびステップＳ３０３で算出された対象物体の概略位置姿勢とに基づいて、撮影画像上に投影した２次元座標を算出する。
【０１０３】
続くステップＳ８０４において、２次反射スポット像の撮影画像上での座標を算出する。このために、光源位置と識別番号ｉの三角形ポリゴンの重心を結ぶ直線に対して当該ポリゴンの法線方向を二等分角とする方向、すなわちスポット光の反射方向を算出する。そして、１次反射スポット位置を始点として当該反射方向に延びる直線と形状モデルとの交点を求める。この交点が２次反射スポットとなる。この交点が存在するか否かが判定される（第２判定）。交点が存在すると判定された場合、ステップＳ３０１で読み込まれたカメラパラメータおよびステップＳ３０３で算出された対象物体の概略位置姿勢とに基づいて、この２次反射スポットを撮影画像上に投影する（第２投影）。この２次元座標がステップ８０４で求める座標となる。
【０１０４】
続いてステップＳ８０３およびステップＳ８０４で算出された１次反射スポット像および２次反射スポット像の座標に基づいて、２次反射スポット像が偽のスポット像として探索される可能性を評価する（ステップＳ８０５およびステップＳ８０６）。
【０１０５】
具体的にはステップＳ８０５において、２次反射スポット像の位置と、光源からのスポット光照射方向によって決定される撮影画像上のエピポーラ線（投影線）との距離が、所定の閾値未満か否かが判定される（第１判別）。閾値は、距離計測装置におけるスポット探索処理の対象範囲によって決定される値である。そして、１次反射スポットと２次反射スポットとの間の３次元的な距離が所定の閾値未満か否かが評価される（第２判別）。閾値は、対象物体表面の反射率に依存し、実験によって決定される値である。ステップＳ８０６において、これらの評価値の全てが閾値未満である場合に探索誤りが発生すると判定されて（ステップＳ８０６；ＹＥＳ）、ステップＳ８０７に進む。一方、何れかの評価値が閾値以上である場合は、２次反射スポット像が偽のスポットとして探索される可能性はないと判定されて（ステップＳＳ８０６；ＮＯ）、ステップＳ８０８に進む。
【０１０６】
ステップＳ８０７において、距離計測結果のオフセットの度合いであるｓｉを、以下の式（１１）に基づいてオフセット値算出する。
【０１０７】
【数１１】
【０１０８】
ここで、ΔＺは距離Ｚの位置におけるオフセットの大きさである。ｇｉはステップＳ８０３およびステップＳ８０４で算出された座標間の距離である。ｆは距離計測装置における撮像部の焦点距離である。Ｌは照明部と撮像部の相対位置姿勢から算出される撮像部の焦点と照明の光軸との距離である。
【０１０９】
一方、ステップＳ８０８では、距離計測結果のオフセットの度合いｓｉが０に設定される。オフセットの度合いが０であることは、距離値の系統誤差が存在しないことを示している。
【０１１０】
ステップＳ８０９では、距離計測結果のオフセットの度合いｓｉを−１に設定する。オフセットの度合いが−１であることは、三角形ポリゴンが不可視であることを示す特別な値であることを意味する。
【０１１１】
なお、ステップＳ８０７、ステップＳ８０８、ステップＳ８０９で設定されたオフセットの度合いｓｉは、ポリゴンの識別番号ｉとセットにされてメモリ２０２に格納される。
【０１１２】
最後にステップＳ８１０において、形状モデルに含まれるすべての三角形ポリゴンに対して、距離計測のオフセット度が設定されたか否かが判定される。すべてのポリゴンを処理済みである場合は、距離計測誤差推定の処理を終了する。一方、未処理のポリゴンが残っている場合は、ステップＳ８０１に戻る。
【０１１３】
以上が本実施形態に係る距離計測誤差の推定処理（ステップＳ３０４）の流れである。
【０１１４】
最後に、本実施形態に係る局所領域の選択処理、つまりサンプル点の決定処理（ステップＳ３０５）について説明する。ステップＳ３０５では、形状モデルに含まれる各三角形ポリゴンに関して、式（１２）に基づいてサンプル点の数Ｎｉが算出される。
【０１１５】
【数１２】
【０１１６】
式（１２）における記号Ｓ、Ｄ、ｄｉ、Ａｉ、βｉの意味は第１実施形態で説明した式（２）の各記号と同様の意味である。ｓｉはステップＳ３０４で算出された距離計測値のオフセット度である。Ｃｓはサンプル点数に対するオフセットの度合いの寄与度を調整するための係数（定数）である。
【０１１７】
ここで算出されたサンプル数数Ｎｉに基づいて、処理対象の三角形ポリゴン上でのサンプル点の位置を決定する方法は第１実施形態で説明した方法と同様であるので、説明を省略する。
【０１１８】
以上説明したように、本実施形態の方法を用いれば、距離画像計測値のオフセットの度合いに応じて適応的に距離画像と対応付けるモデル面上のサンプル点を選択することが出来る。よって、対応点の数を減らして処理を高速化しながら、推定する位置姿勢の精度を保つことが可能である。
【０１１９】
なお、本発明の実施形態は上記に限らず様々な変形例が存在する。以下の第１０実施形態から第１３実施形態が第２実施形態の変形例である。
【０１２０】
（第１０実施形態）
第１実施形態の変形例である第２実施形態乃至第８実施形態で説明した内容は、本実施形態にも適用できる。
【０１２１】
（第１１実施形態）
上記の説明では、それぞれの三角形ポリゴンに関する距離計測のオフセット度を各三角形ポリゴンの重心位置で評価している。しかし、誤差の評価に用いる点の位置はこれに限るものではない。また、評価に用いる点の数も１点に限らず、複数の点であってもよい。このとき、複数の点で算出されたオフセット度を平均するなどして、三角形ポリゴン１つにつき１つのオフセット度を算出してもよい。あるいは、１つの三角形ポリゴンを複数の小領域に分割し、それぞれの小領域についてオフセット度を算出して、小領域ごとにサンプル数を決定してもよい。
【０１２２】
（第１２実施形態）
上記の説明では、２次反射したスポット光による誤差の大きさを推定しているが、さらに他の要因による誤差を評価してもよい。
【０１２３】
例えば、距離計測装置の光源以外の光源（例えば照明装置）が存在するとき、光源の位置、照射方向、及び計測対象物体の鏡面反射特性が分かれば、その光源の光によって物体表面に生ずる鏡面反射像の位置を推定できる。ここで推定された鏡面反射像および１次反射スポット像の画像上での位置関係から、距離計測値のオフセット度合いを計算できる。
【０１２４】
また、図９に示すように、不透明な素材の部分９０１および透明な素材の部分９０２から構成されている計測対象物体があるとする。このとき、距離計測装置を構成する光源９０３から照射されたスポット光は透明部分９０２の表面上の点９０５にスポットを発生させる。また、透明部分９０２を通って、不透明部分９０１と透明部分９０２との境界上の点９０６にもスポットを発生させる。このような場合も、スポット光の光路をシミュレートして画像上でのスポット間の距離を算出することで、その距離に応じたオフセット度合いを計算できる。
【０１２５】
上記の他、図１０に示すように距離の異なる複数点が観測され、距離計測値にオフセット誤差が発生する場合がある。図１０において、計測対象物体１００１および計測対象物体１００７は計測対象となる物体を示す。光源１００２および光源１００８は距離計測装置を構成するスポット光源である。撮像部１００３および撮像部１００９は距離計測装置を構成する撮像部である。図１０（Ａ）は、１筋のスポット光が照射される領域１００４の中に存在する複数の位置に複数のスポット（１００５および１００６）が生じて、距離値のオフセット誤差の原因となる場合の図である。また、図１０（Ｂ）は、１つの画素に異なる２つの方向のスポット光によるスポット（１０１１および１０１２）が観察されて、距離値のオフセット誤差の原因となる場合の図である。図１０（Ａ）のような場合、距離値のオフセット度合いは例えば以下の方法により算出される。すなわち、光源１００２からモデルを構成する小領域の重心方向に、スポット光の走査方向に微小角度をなす２本の線分を延ばし、対象物体のモデルと交差した２点の撮像部からの距離の差をオフセット度合いとすればよい。図１０（Ｂ）の場合は、同様に、撮像部１００９から線分を伸ばせばよい。
【０１２６】
（第１３実施形態）
上記の説明では、距離画像計測がスポット光方式によるものであるとしている。しかし、距離計測方式はこれに限るものではない。光切断方式など他のアクティブ方式であってもよい。また、アクティブ方式に限らず、ステレオ方式などパッシブ方式であってもよい。パッシブ方式の場合、例えば計測対象物体上に繰り返し模様があることによって２つの画像間の対応点探索を誤ることを原因として想定し、模様の繰り返し間隔に基づいて距離計測値のオフセット度を評価することができる。
【０１２７】
（第１４実施形態）
第１実施形態および第９実施形態では、距離計測の誤差（ばらつきやオフセット）をシミュレーションによって求めた。しかし、誤差を推定する方法はこれに限るものではなく、実際に対象物体を計測した距離データに基づいて決定することができる。
【０１２８】
本実施形態の方法に基づく処理は、モデル上のサンプル点決定に必要なオフセット誤差データを準備するオフライン処理と、誤差データに基づいてサンプル点を決定して対象物体の位置姿勢を算出するオンライン処理とに分かれる。なお、本実施形態で想定するモデルは三角形パッチから構成されるパッチモデルとする。また、距離計測の方式は任意であるが、例えば光切断法を用いる。
【０１２９】
オフライン処理では、対象物体を中心位置に配置した仮想的なＧｅｏｄｅｓｉｃ ｓｐｈｅｒｅを想定し、その各頂点に相当する位置に配置した距離計測装置で対象物体の距離画像を撮影する。その撮影されたそれぞれの距離画像に対して、以下の(1)〜(5)の処理を行う。
【０１３０】
(1) 対象物体の概略の位置姿勢を推定する。その方法は第１実施形態または第９実施形態と同様である。
【０１３１】
(2) 距離画像の各画素に対応するモデル上の点を探索し、距離画像の画素に相当する３次元点と、対応するモデル上の点を対応点対として記憶する。
【０１３２】
(3) 対応点対間の距離の総和が最小となるようにモデルの位置姿勢を最適化することによって、対象物体の位置姿勢を推定する。本ステップで算出された値を対象物体の位置姿勢の真値とする。
【０１３３】
(4) モデルを真値の位置姿勢に配置した状態で、距離画像の各画素に対応するモデル上の点の位置から算出された距離値と、距離画像上の当該画素の距離値との差を求め、ポリゴンパッチごとに距離値の差の平均値を算出する。
【０１３４】
(5) Ｇｅｏｄｅｓｉｃ ｓｐｈｅｒｅの頂点ごと（距離画像ごと）に用意したモデルデータのポリゴンパッチごとに、(4)で算出された平均値を距離画像計測のオフセット誤差として登録する。
【０１３５】
一方、オンライン処理では、以下の(6)〜(11)の処理を行う。
【０１３６】
(6) 対象物体の距離画像を計測し、対象物体の概略の位置姿勢を推定する。
【０１３７】
(7) (6)で算出された概略位置姿勢に基づいて、(6)の距離画像撮影時の視点に最も近い頂点をＧｅｏｄｅｓｉｃ ｓｐｈｅｒｅ上の頂点から選択する。
【０１３８】
(8) (7)で選択された頂点に登録されたモデルデータから各ポリゴンのオフセット誤差を読み出し、誤差値に基づいてサンプル点数を算出する（サンプル点数の決定方法の詳細は第９実施形態と同様である）。
【０１３９】
(9) (8)で算出されたサンプル点数に基づいて、各ポリゴンのサンプル点の位置を決定する（サンプル点位置の決定方法の詳細は第１実施形態、第９実施形態と同様である）。
【０１４０】
(10) (9)で位置が決定されたサンプル点と距離画像とを対応付けて、対応点対を求める（サンプル点と距離画像との対応付け方法の詳細は第１実施形態、第９実施形態と同様である）。
【０１４１】
(11) (10)で求められた対応点対間の距離の総和を最小化するように最適化計算して、対象物体の位置姿勢を算出する（位置姿勢算出方法の詳細は実施形態１、第９実施形態と同様である）。
【０１４２】
本実施形態に述べた方法によれば、対象物体を実際に距離計測したデータに基づいてサンプル点を決定することができる。その結果、シミュレーションで距離計測誤差を推定する方法に比べて誤差の推定精度が向上し、ひいては位置姿勢推定の精度を保ちながら処理速度を向上する効果が高くなる。
【０１４３】
なお、本発明の実施形態は上記に限らず様々な変形例が存在する。以下の第１５実施形態または第１６実施形態が第１４実施形態の変形例である。
【０１４４】
（第１５実施形態）
第１実施形態の変形例である第２実施形態乃至第８実施形態は、本実施形態にも適用できる。
【０１４５】
（第１６実施形態）
上記の説明では、距離計測のオフセット誤差を実測の距離画像から推定する方法を述べたが、ばらつき誤差を推定することも可能である。具体的には、実測と真値の距離値の差について、ポリゴンパッチごとに求めた標本分散をもって、当該ポリゴンのばらつき誤差とすればよい。
【０１４６】
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【特許請求の範囲】
【請求項１】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報のばらつきの程度を示すばらつき値を算出する算出手段と、
前記ばらつき値が小さい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項２】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報に含まれる計測誤差を示すオフセット値を算出するオフセット値算出手段と、
前記オフセット値が大きい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項３】
前記対応付け手段は、
前記概略位置姿勢に基づいて前記対象物体を基準とした３次元座標系における前記サンプル点を前記撮像手段を基準とした３次元座標系におけるサンプル点に変換して、
前記変換されたサンプル点を前記距離画像上に投影し、
前記投影された前記距離画像上の２次元座標系の投影点に最近接する前記距離画像上の画素の２次元計測点と、当該画素に保持される距離情報とから当該画素の前記撮像手段を基準とした３次元計測点を求め、
前記概略位置姿勢に基づいて、前記撮像手段を基準とした３次元計測点を前記対象物体を基準とした３次元座標における３次元計測点に変換し、
前記サンプル点と、前記対象物体の３次元座標における３次元計測点とを対応付けることを特徴とする請求項１又は２に記載の位置姿勢計測装置。
【請求項４】
前記形状モデルの表面を構成する単位領域を選択する選択手段と、
前記形状モデル上で光源からの光が照射される１次反射点の像が前記撮像手段で観測されるか否かを判定する第１判定手段と、
前記第１判定手段により１次反射点の像が前記撮像手段で観測されると判定された場合、前記撮像手段により撮像された撮像画像上に前記１次反射点の像を２次元座標として投影する第１投影手段と、
前記１次反射点で反射された光により照射される前記形状モデルの他の表面上の２次反射点の像が前記撮像手段で観測されるか否かを判定する第２判定手段と、
前記第２判定手段により２次反射点の像が前記撮像手段で観測されると判定された場合、前記撮像画像上に前記２次反射点の像を２次元座標として投影する第２投影手段と、
前記２次反射点の前記撮像画像上における前記２次元座標と、前記光源からの光の照射方向に沿った線が前記撮像画像上に投影された投影線と、の距離が閾値未満であるか否か判別する第１判別手段と、
前記１次反射点と前記２次反射点との３次元的な距離が閾値未満であるか否か判別する第２判別手段と、をさらに備え、
前記オフセット値算出手段は、前記第１及び前記第２判別手段により共に閾値未満であると判別された場合は、前記オフセット値を、前記１次反射点の２次元座標と前記２次反射点の２次元座標との距離を、前記撮像手段の焦点距離、および、前記光源の光軸と前記撮像手段の焦点との距離、の積で除した値として算出することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項５】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測方法であって、
取得手段が、撮像工程で撮像された前記対象物体までの距離画像を取得する取得工程と、
概略位置姿勢決定手段が、前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定工程と、
算出手段が、前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報のばらつきの程度を示すばらつき値を算出する算出工程と、
サンプリング手段が、前記ばらつき値が小さい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング工程と、
対応付け手段が、前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け工程と、
位置姿勢決定手段が、各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定工程と、
を有することを特徴とする位置姿勢計測方法。
【請求項６】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測方法であって、
取得手段が、撮像工程で撮像された前記対象物体までの距離画像を取得する取得工程と、
概略位置姿勢決定手段が、前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定工程と、
オフセット値算出手段が、前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報に含まれる計測誤差を示すオフセット値を算出するオフセット値算出工程と、
サンプリング手段が、前記オフセット値が大きい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング工程と、
対応付け手段が、前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元に変換した３次元計測点とを対応付ける対応付け工程と、
位置姿勢決定手段が、各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定工程と、
を有することを特徴とする位置姿勢計測方法。
【請求項７】
請求項５又は６に記載の位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【請求項８】
コンピュータに請求項５又は６に記載の位置姿勢計測方法を実行させるためのプログラムが記憶されたコンピュータで読み取り可能な記憶媒体。
【請求項１】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報のばらつきの程度を示すばらつき値を算出する算出手段と、
前記ばらつき値が小さい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項２】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測装置であって、
撮像手段により撮像された前記対象物体までの距離画像を取得する取得手段と、
前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定手段と、
前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報に含まれる計測誤差を示すオフセット値を算出するオフセット値算出手段と、
前記オフセット値が大きい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング手段と、
前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元に変換した３次元計測点とを対応付ける対応付け手段と、
各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項３】
前記対応付け手段は、
前記概略位置姿勢に基づいて前記対象物体を基準とした３次元座標系における前記サンプル点を前記撮像手段を基準とした３次元座標系におけるサンプル点に変換して、
前記変換されたサンプル点を前記距離画像上に投影し、
前記投影された前記距離画像上の２次元座標系の投影点に最近接する前記距離画像上の画素の２次元計測点と、当該画素に保持される距離情報とから当該画素の前記撮像手段を基準とした３次元計測点を求め、
前記概略位置姿勢に基づいて、前記撮像手段を基準とした３次元計測点を前記対象物体を基準とした３次元座標における３次元計測点に変換し、
前記サンプル点と、前記対象物体の３次元座標における３次元計測点とを対応付けることを特徴とする請求項１又は２に記載の位置姿勢計測装置。
【請求項４】
前記形状モデルの表面を構成する単位領域を選択する選択手段と、
前記形状モデル上で光源からの光が照射される１次反射点の像が前記撮像手段で観測されるか否かを判定する第１判定手段と、
前記第１判定手段により１次反射点の像が前記撮像手段で観測されると判定された場合、前記撮像手段により撮像された撮像画像上に前記１次反射点の像を２次元座標として投影する第１投影手段と、
前記１次反射点で反射された光により照射される前記形状モデルの他の表面上の２次反射点の像が前記撮像手段で観測されるか否かを判定する第２判定手段と、
前記第２判定手段により２次反射点の像が前記撮像手段で観測されると判定された場合、前記撮像画像上に前記２次反射点の像を２次元座標として投影する第２投影手段と、
前記２次反射点の前記撮像画像上における前記２次元座標と、前記光源からの光の照射方向に沿った線が前記撮像画像上に投影された投影線と、の距離が閾値未満であるか否か判別する第１判別手段と、
前記１次反射点と前記２次反射点との３次元的な距離が閾値未満であるか否か判別する第２判別手段と、をさらに備え、
前記オフセット値算出手段は、前記第１及び前記第２判別手段により共に閾値未満であると判別された場合は、前記オフセット値を、前記１次反射点の２次元座標と前記２次反射点の２次元座標との距離を、前記撮像手段の焦点距離、および、前記光源の光軸と前記撮像手段の焦点との距離、の積で除した値として算出することを特徴とする請求項２に記載の位置姿勢計測装置。
【請求項５】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測方法であって、
取得手段が、撮像工程で撮像された前記対象物体までの距離画像を取得する取得工程と、
概略位置姿勢決定手段が、前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定工程と、
算出手段が、前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報のばらつきの程度を示すばらつき値を算出する算出工程と、
サンプリング手段が、前記ばらつき値が小さい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング工程と、
対応付け手段が、前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元座標に変換した３次元計測点とを対応付ける対応付け工程と、
位置姿勢決定手段が、各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定工程と、
を有することを特徴とする位置姿勢計測方法。
【請求項６】
対象物体の３次元形状を示す形状モデルが既知である当該対象物体の３次元の位置姿勢を計測する位置姿勢計測方法であって、
取得手段が、撮像工程で撮像された前記対象物体までの距離画像を取得する取得工程と、
概略位置姿勢決定手段が、前記距離画像に基づいて前記対象物体の概略位置姿勢を決定する概略位置姿勢決定工程と、
オフセット値算出手段が、前記形状モデル上の所定領域に対応する、前記距離画像における領域の前記距離情報に含まれる計測誤差を示すオフセット値を算出するオフセット値算出工程と、
サンプリング手段が、前記オフセット値が大きい程、サンプル点の数が少なくなるように、前記形状モデル上の所定領域ごとに当該所定領域からサンプル点をサンプリングするサンプリング工程と、
対応付け手段が、前記概略位置姿勢に基づいて、前記サンプル点と、前記距離画像上の２次元計測点を３次元に変換した３次元計測点とを対応付ける対応付け工程と、
位置姿勢決定手段が、各サンプル点と各３次元計測点との間の距離の総和が最小になるように前記対象物体の位置姿勢を決定する位置姿勢決定工程と、
を有することを特徴とする位置姿勢計測方法。
【請求項７】
請求項５又は６に記載の位置姿勢計測方法をコンピュータに実行させるためのプログラム。
【請求項８】
コンピュータに請求項５又は６に記載の位置姿勢計測方法を実行させるためのプログラムが記憶されたコンピュータで読み取り可能な記憶媒体。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１１−１７９９１０（Ｐ２０１１−１７９９１０Ａ）
【公開日】平成２３年９月１５日（２０１１．９．１５）
【国際特許分類】
【出願番号】特願２０１０−４３０６４（Ｐ２０１０−４３０６４）
【出願日】平成２２年２月２６日（２０１０．２．２６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２３年９月１５日（２０１１．９．１５）
【国際特許分類】
【出願日】平成２２年２月２６日（２０１０．２．２６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]