以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の1つである。
[第1の実施形態]
本実施形態では、撮像装置と投影装置によって対象物体の輝度画像と距離画像を同時に取得し、3次元モデル情報を輝度画像に対して2次元の尺度で位置合わせし、距離画像に対して3次元の尺度で位置合わせを行い、2次元と3次元の尺度で同時に評価する際に、本発明を適用する方法を説明する。なお、ここでは2次元と3次元の尺度で同時に位置合わせをする例で説明するが、本発明は2次元の尺度のみであったり、3次元の尺度のみであっても全く同様に適用可能である。
3次元モデル情報の初期位置の設定においては、撮像部で撮影された輝度画像を全探索することで、位置姿勢の取り得る分布(概略位置姿勢)を取得する。取得された概略位置姿勢の分布に基づいて、複数の初期位置姿勢を生成し、それぞれの位置姿勢候補で位置合わせを行い、最も合致した位置姿勢を最終的な位置合わせ結果として出力する。
こうすることで、局所解となり得る位置姿勢を網羅的に検証することとなり、位置合わせのロバスト性を向上できる。以降処理手順の詳細について説明する。なお、本発明における全ての実施形態において、特に明記しない限り、位置合わせとは、計測対象物体の2次元または3次元形状モデルを、画像中の計測対象物体の位置、または、位置および姿勢に合わせる(フィッティングを行う)ことを示す。
図1は、第1の実施形態における位置姿勢計測装置のシステム構成を示す図である。
計測対象物体101は、位置及び姿勢(位置姿勢)を計測する対象としての計測対象物体である。本実施形態では、説明を簡単にするために、計測対象物体101は図1に示すように、一つ置かれているものとする。しかし、以下に説明する位置姿勢計測処理は、計測対象物体の形状、個数、載置形態に大きく依存するものではない。例えば複数の異形状の計測対象物体が混合されて山積みされているような状態において、ある計測対象物体の位置姿勢を計測する場合にも、本発明は適用可能である。
投影装置102は、計測対象物体101にパターン光を投影する。投影装置102につついては詳しくは後述する。
撮像装置103は、静止画もしくは動画像を撮像する装置であり、本実施形態では、計測対象物体101および、投影装置102によってパターン光が投影された計測対象物体101を撮像するために使用する。そして、撮像装置103は、撮像した画像(撮像画像)を位置姿勢計測装置104に対して送出する。撮像装置103については詳しく後述する。
位置姿勢計測装置104は、投影装置102と撮像装置103に接続されており、本発明の具体的な処理が記述されたプログラムを内部に保持し、内蔵するCPUがそのプログラムを実行することで、投影装置102及び撮像装置103を制御し、所望の位置姿勢計測結果を算出する装置である。
次に、位置姿勢計測装置104の機能構成例について、図2のブロック図を用いて説明する。
3次元モデル情報202は、3次元CADソフトで取り扱うことができるCADモデルそのもの、もしくは3次元CADモデルをComputer Graphics分野で使われる複数のポリゴン要素に変換した、計測対象物体の形状情報である。本実施形態では、計測対象物体101の形状を模した3次元幾何モデルであって、ポリゴン要素で構成される3次元幾何モデルを用いる。3次元モデル情報の構成については図3を用いて説明する。
ポリゴン要素で構成される3次元幾何モデルは、図3に示すような点、線、面といった構成要素からなる。図3(a)〜(c)はいずれも同じ3次元幾何モデルを示している。
ポリゴン要素で構成される3次元幾何モデルのモデル情報では、図3(a)に例示している3次元幾何モデルの各頂点について、図3(d)に示すごとく、該頂点のインデックスと、該頂点の3次元座標値とを管理している。
また、このモデル情報では、図3(b)に例示している3次元幾何モデルの各辺について、図3(e)に示すごとく、該辺のインデックスと、該辺の両端における頂点のインデックスとを管理している。
また、このモデル情報では、図3(c)に例示している3次元幾何モデルの各面(ポリゴン)について、図3(f)に示すごとく、該ポリゴンのインデックスと、該ポリゴンのインデックスと、該ポリゴンの法線ベクトルとを管理している。
このような3次元モデル情報202は、位置姿勢計測装置104内の適当なメモリ、もしくは、位置姿勢計測装置104がアクセス可能な外部メモリに格納されている。
概略位置姿勢取得部203は、計測対象物体101のとりうる位置姿勢(概略位置姿勢)を取得する。具体的な方法については、後述する。
位置姿勢候補生成部204は、位置姿勢分布決定部203によって得られた概略位置姿勢の分布に基づいて複数の位置姿勢候補の生成を行う。具体的な方法については、後述する。
位置姿勢候補選択部205は、複数の位置姿勢候補の中から一つ以上、位置姿勢を算出するべき位置姿勢候補を選択する。複数の位置姿勢候補をふるい分けすることで計算時間を短縮する。具体的な方法については、後述する。
位置姿勢算出部206は、位置姿勢選択部205で選択された位置姿勢候補の示す位置姿勢で、3次元モデルを撮像装置103によって得られた輝度画像及び距離画像に位置合わせをし、最終的な位置姿勢を算出する。
撮像装置103は、カメラであり、計測対象物体101が配置されている作業スペースを撮影する。本発明の説明において単に「座標系」や「位置姿勢」といった場合は、それぞれこのカメラの「座標系」、このカメラ座標系における「位置姿勢」をあらわす。一方で「モデルの座標系」は部品の3次元モデルの点や面の位置を表すのに使用するモデル毎に定義される座標系を表す。カメラの内部パラメータ(焦点距離、主点位置、レンズ歪みパラメータ)は例えば、以下の文献に記載されている方法により、事前に校正しておく。
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
投影装置102は、液晶プロジェクタでパターン光を投影する。プロジェクタの内部パラメータ(焦点距離、主点位置、レンズ歪みパラメータ)についてもカメラの校正と同様の原理で事前に校正しておく。投影装置102は、もちろん、液晶プロジェクタに限るものではなく、パターン光の投影ができれば他の方式のものでもよい。例えば、DMD(デジタルミラーデバイス)やLCOSを用いたプロジェクタであってもよい。
位置姿勢計測装置104は、投影装置102、撮像装置103を制御し、計測対象物体の輝度画像及び距離画像を取得する。距離画像は、画像内に写りこんだパターン光の形状に基づいて三角測量の原理で生成される。この方法は一般的にアクティブステレオと呼ばれるが、本発明は、アクティブステレオに限定されるものではなく、パッシブステレオなど他の方法で距離画像を生成しても同様に本発明の実施は可能である。
図4は、以上の要素で構成される情報処理装置のフィッティング処理の手順を示したフローチャートである。本フローチャートで処理手順の概要を説明し、後に本発明の実施において特に重要な点を詳細に説明する。
(ステップS401)
ステップS401では、撮像部103が作業スペースの計測対象物体101を撮影し、計測対象物体101の輝度画像及び距離画像を取得する。
(ステップS402)
ステップS402では、輝度画像を全探索して、計測対象物体101の位置姿勢の取り得る分布(複数の概略位置姿勢)を決定する。画像を全探索して、画像中の計測対象物体101の概略位置姿勢を算出することは広く行われており、例えば、以下の非特許文献では、パターンマッチングを用いて部品の3次元モデルの画像内における位置姿勢を算出する方法が開示されている。
吉井裕人、 “アンサンブル分類木を用いたバラ積み部品の概略位置姿勢検出”、 画像の認識・理解シンポジウム(MIRU2010)、 2010
このような方法では、画像内で複数回パターンマッチングを行い、1回のマッチングで得られた結果を1回の投票として記録し、最後に投票結果を集計して全探索の結果として提示する。この1回の投票は、情報として計測対象物体の位置姿勢の6パラメータを持つ(ただし、実際には計算を効率化するため、一部の次元はクラス化されて代表値で表されることもある)。本発明では、この位置姿勢の6パラメータを伴う複数の投票を対象部品の取り得る位置姿勢分布として利用する。
図5(a)は、このようにして得られた概略位置姿勢の分布の一例である。本実施形態では、簡単のため、位置姿勢をx、yの2パラメータで表すが、実際のフィッティングにおける6パラメータの位置姿勢においても同様に実行することが可能である。
なお、本実施形態における画像内での特定の部品の位置姿勢を探索する手法は、輝度画像のみに限定されるものではなく、距離画像などの3次元情報を探索する手法であってもよい。
さらに本実施形態では、画像を全探索する場合を説明したが、本発明の実施においては必ずしも得られた画像を全探索する必要はない。例えば、部品が常に作業スペース内のほぼ同じ場所にあることが分かっている場合には、その領域に限定して部分的に探索するようにしても問題はない。
(ステップS403)
ステップS403では、ステップS402で得られた概略位置姿勢の分布に基づいて所定の個数の位置姿勢候補を生成する。
まず、概略位置姿勢の分布より平均ベクトルμと共分散行列Σを算出してガウス分布を適用する。図5(b)はこうして得られた2次元ガウス分布の確率密度が一定となる等高線を図5(a)に重ねたものである。
次に求められたガウス分布に従って、所定の個数の位置姿勢候補を生成する。所定の個数とはシステムに要求される許容時間から逆算して適切な個数を設定すればよい。図5(c)はこのようにして生成した位置姿勢候補である。
この例では、ステップS402で得られた概略位置姿勢の分布(図5(c)で黒丸でプロットされている点)と、新たに生成した位置姿勢候補(図5(c)で黒三角でプロットされている点)の合計が20個になるように生成し、これら20個の点を初期位置姿勢とする(初期位置姿勢生成)。
なお、本実施形態では、概略位置姿勢として挙げられている位置姿勢には一定の合理性があると考え、位置姿勢候補として採用した。しかし本発明の実施においては、概略位置姿勢として挙げられたものは採用せずに、新たに生成した位置姿勢だけを採用してもよい。
また、確率密度モデルとして、ガウス分布を適用したが、実際には得られた概略位置姿勢の分布を要約できる分布であれば他の分布(所定の分布関数)であってもよいし、もっと簡単に方形状の分布を適用してもよい。
(ステップS404)
ステップS404では、ステップS403で得られた複数の位置姿勢候補の中からスコア(3次元モデルと輝度画像及び距離画像の合致度を表す)の高い位置姿勢候補を一以上の所定の個数、選択する(初期位置姿勢選択)。具体的には後述する位置姿勢算出における位置姿勢更新処理、ステップS601〜S606を1回行い、その結果得られるスコアを比較して高いものから順に所定の個数、抽出する。
このようにして選択された位置姿勢候補に対してのみ、反復を伴う位置姿勢算出処理を行うことでフィッティングにかかる総時間を短縮する。ここでも所定の個数はシステムに要求される許容時間から逆算して適切な個数を設定すればよい。またここにおけるスコアは、3次元モデルと、輝度画像及び距離画像との合致している度合いを表す値なら何でもよい。
例えば3次元モデルと輝度画像及び距離画像で誤差ベクトル(後述)を求めそのノルムの逆数を用いる方法が考えられるがその他の指標でもよい。ここで説明した選択処理ではスコアで評価する前に位置姿勢更新処理、ステップS601〜ステップS606を1回行っているが(1回対応づけている)、この回数は必ずしも1回である必要はなく、高速且つ妥当な初期位置姿勢の見込みを確かめられる方法ならばなんでもよい。
例えば位置姿勢更新を1回も行わずにスコアを算出してもよいし、または位置姿勢更新を2回以上行ってからスコアを算出してもよい。このように選択処理の具体的な手順については無数の方法が考えられるが本発明の実施においては特にどれでなくてはならないということはない。
(ステップS405)
ステップS405では、ステップS404で選択された一以上の所定の個数の位置姿勢候補のそれぞれで、最終的な位置姿勢を決定する。ここでの位置姿勢算出処理の詳細については図6を用いて説明する。図6はステップS405で示した1つの初期位置姿勢に関する位置姿勢算出処理の詳細な手順を示したフローチャートである。
本実施形態では、Gauss−Newton法により、計測対象物体の位置姿勢候補(以下、sで表す)を反復演算により繰り返し補正することにより、位置姿勢を算出する。ここでは、画像上で検出されたエッジと推定される位置姿勢とに基づいて、画像上に投影される3次元モデル上のエッジとの距離、距離画像の点群データを構成する各点と位置姿勢に基づいて、3次元計測装置の座標系に変換される面との距離の総和が最小化されるように位置姿勢を最適化する。
より具体的には、2次元画像上での点と直線の符号付距離、3次元空間中での点と平面との符号付距離を、1次のテイラー展開によってそれぞれ部品の位置姿勢の微小変化の1次関数として表現する。そして、符号付距離が0になるような位置姿勢の微小変化に関する線形の連立方程式を立式して解くことにより、部品の位置姿勢の微小変化を求めて位置姿勢を補正することを繰り返す。
(ステップS601)
ステップS601では、初期化を行う。ここでは、ステップS404で選択された位置姿勢候補を、これ以降「現在の位置姿勢」として記載する位置姿勢の初期値として設定する。
(ステップS602)
ステップS602では、対応付けを行う。まず、現在の位置姿勢に基づいて、3次元モデルの各エッジの画像への投影及び各面の3次元計測装置の座標系への座標変換を行う。その後エッジ及び点群データの対応付けを行う。
図7はエッジの対応付け方法を示す図である。
図7(a)は輝度画像上に3次元モデルのエッジを投影した図である。まず画像上で等間隔になるように、投影されたエッジ701上に制御点702を設定し、各制御点ごとに、投影されたエッジ701の法線方向に探索ライン703をとる。制御点を原点とした探索ラインの所定の範囲内で1次元のエッジの検出を行い、検出されたエッジの中で制御点に最も近い点を対応点1005として保持する。
図7(b)は原点が制御点で、横軸に探索ライン、縦軸に輝度勾配の絶対値をとるグラフである。先の画像上にエッジは画素値の輝度勾配の絶対値の極値として検出される。ここでは輝度勾配の絶対値の極値が所定の閾値705よりも大きく、且つ最も制御点に近い点704を対応点とする。
次に点群データの対応付けを行う。点群データ中の各点につき、三次元空間において最も近傍する面を探索し対応付ける。
(ステップS603)
ステップS603では、線形連立方程式を解くための係数行列と誤差ベクトルの算出を行う。ここで係数行列の各要素は、位置姿勢の微小変化に対する一次の偏微分係数である。エッジについては、画像座標の偏微分係数を算出し、点群については3次元座標の偏微分係数を算出する。誤差ベクトルは、エッジについては投影されたエッジと検出されたエッジの画像上での距離、点群データについてはモデルの面と点の3次元空間中での距離である。
図8は、エッジの投影像と検出されたエッジとの関係を説明する図である。図8では、画像の水平方向、垂直方向をそれぞれu軸、v軸としている。ある制御点(投影された各エッジを画像上で等間隔に分割する点)の画像上での位置を(u0、v0)、該制御点が所属するエッジの画像上での傾きをu軸に対する傾きθと表す。傾きθは、エッジの両端の3次元座標をsに基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。該エッジの画像上での法線ベクトルは(sinθ、−cosθ)となる。また、該制御点の対応点の画像座標を(u’、v’)とする。ここで、点(u’、v’)を通り、傾きがθである直線上の点(u、v)は、
と表せる(θは定数)。ここで、
とした。
制御点の画像上での位置は、計測対象物体の位置姿勢により変化する。また、計測対象物体の位置姿勢の自由度は6自由度である。すなわちsは6次元ベクトルであり、計測対象物体の位置を表す3つの要素と、姿勢を表す3つの要素からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す3次元ベクトルなどによって表現される。位置姿勢により変化する制御点の画像座標(u、v)は(u0、v0)の近傍で1次のテイラー展開によって数2のように近似できる。但しΔsi(i=1、2、・・・、6)はsの各成分の微小変化を表す。
正しいsによって得られる制御点の画像上での位置は、数1が表す直線上にあると仮定できる。数2によって近似されるu、vを数1に代入することにより、数3が得られる。
ただし、
(定数)とした。
3次元計測装置の座標系における3次元座標は、計測対象部品の位置姿勢sによってモデル座標系における3次元座標(x、y、z)に変換することができる。概略の位置姿勢により、ある点がモデル座標系の点(x0、y0、z0)に変換されるとする。(x、y、z)は、計測対象物体の位置姿勢により変化するものであり、(x0、y0、z0)の近傍で1次のテイラー展開によって数4のように近似できる。
ステップS602において、点群データ中のある点に対応付けられた3次元モデルの面の、モデル座標系における方程式をax+by+cz=e(a2+b2+c2=1、a、b、c、eは定数)とする。正しいsによって変換される(x、y、z)は、平面の方程式ax+by+cz=e(a2+b2+c2=1)を満たすと仮定する。数4を平面の方程式に代入すると数5が得られる。
ただし、
(定数)とした。
数3は、ステップS602において対応付けが行われたすべてのエッジについて成り立つ。また、数5はS602において対応付けが行われたすべての点群データについて成り立つため、数6のようなΔsiに関する線形連立方程式が成り立つ。
ここで数6を数7のように表す。
数6の線形連立方程式の係数行列Jを算出するための偏微分係数の算出は、例えば以下の文献に開示されている方法によって行う。
V. Lepetit and P. Fua, ‘Keypoint recognition using randomized trees,’ IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.28, no.9, 2006.
(ステップS604)
ステップS604では、数7をもとに、行列Jの一般化逆行列(JT・J)−1・JTを用いてΔsを求める。しかしながら、エッジや点群データには誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジ(点群データ)では、誤差d−r(e−q)が大きくなる。そのため、数6、数7の連立方程式に対する寄与度が大きくなり、その結果得られるΔsの精度が低下してしまう。
そこで、誤差d−r(e−q)が大きいデータには小さな重みを与え、誤差d−r(e−q)が小さいデータには、大きな重みを与える。重みは、例えば数8に示すようなTukeyの関数により与える。
c1、c2は定数である。なお、重みを与える関数はTukeyの関数である必要はなく、例えばHuberの関数など、誤差 大きいデータには小さな重みを与え、誤差小さいデータには大きな重みを与える関数であればなんでもよい。
各データ(エッジまたは点群データ)に対応する重みをwiとする。ここで数9のように重み行列Wを定義する。
重み行列Wは、対角成分以外はすべて0の正方行列であり、対角成分には重みwiが入る。この重み行列Wを用いて、数7を数10のように変形する。
数11のように数10を解くことにより補正値Δsを求める。
(ステップS605)
ステップS605では、ステップS604において算出された位置姿勢の補正値Δsにより、位置姿勢を更新する。
(ステップS606)
ステップS606では、位置姿勢が収束したか否かの判定を行い、収束したと判定された場合はその時の位置姿勢をフィッティング結果として処理を終了する。収束していないと判定された場合は、収束するまでステップS602〜S605を繰り返し行う。収束判定は、ステップS605の補正量が所定の値以下でほぼ変化がないと判定された場合に収束したと判定する。なお、収束判定の方法はこの方法に限定されるものではなく、例えば、所定の回数、反復したら収束したものとして次のステップに進むことにしてもよい。
(ステップS406)
ステップS406では、ステップS405で求められた一以上の所定の個数の位置姿勢算出結果のスコアを比較して、高いスコアを示す一以上の位置姿勢を最終的なフィッティング結果として出力する。ここにおけるスコアも、ステップS404で説明したものと同様の指標である。
このように、本実施形態によれば、計測対象物体を画像内で全探索した結果から、計測対象物体の概略位置姿勢の分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢候補を網羅的に検証することで、ノイズの多い環境や複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことができる。
なお、本発明における計測対象物体の位置姿勢の算出方法は、Gauss−Newton法に限るものではない。例えば、より計算がロバストであるLevenberg−Marquardt法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やICCG法など、他の非線形最適化計算手法を用いてもよい。
なお、本実施形態では、位置姿勢計測装置104は、撮像画像を撮像装置103から直接取得しているが、これに限るものではなく、例えば、撮像装置103によって撮像された撮像画像を蓄積しているメモリから取得するようにしても良い。この場合、撮像装置103は必ずしも、位置姿勢計測装置104に接続されている必要はない。
本実施形態においては、フィッティングする形状情報の組み合わせとして計測対象物体の3次元モデル情報と撮像装置で撮影された部品を含む画像の組み合わせを例に説明したが、形状情報の組み合わせは必ずしもこの組み合わせでなくてもよく、形状情報間で対応を取って位置合わせが可能な情報であればなんでもよい。物体の形状情報の例としては3次元モデル情報、画像のほかに、レーザーレンジセンサーなどで計測して得られた3次元点群情報なども考えられる。
[第2の実施形態]
第1の実施形態では、計測対象物体の概略位置姿勢の分布から、多次元ガウス分布を適用し、その分布に従って位置姿勢候補を生成した。しかし、ガウス分布のような確率密度モデルを適用しないで位置姿勢候補を生成する方法も考えられる。
その方法の一例として、第2の実施形態では、概略位置姿勢の分布を所定のサンプリング間隔で集計して、その集計結果に基づいて位置姿勢候補を生成する方法を説明する。以下はこれまでに説明した位置姿勢分布を決定する第1の実施形態の説明におけるステップS403に置き換わる処理である。
まず、位置姿勢分布を集計する範囲を決定する。図9(a)は、図5(a)に示した位置姿勢分布が与えられたときに撮影画像の範囲をx1〜x2、y1〜y2としてサンプリングするbinを区切った様子を示す図である。binの幅dはあらかじめ設定しておく定数で、例えば計測対象物体の長辺サイズを所定の値で割った値などが考えられる。ここでは長辺サイズは50mmの計測対象物体を所定の値10で割って5mmをdとして設定する。または、例えば計測対象物体の長辺サイズではなく撮影範囲を所定の数で割った値をdとして設定してもよい。
次に、上記binを設定した範囲で集計をする。図9(b)は各グリッド内に存在する位置姿勢の数を得票数としてカウントし、グリッドの中央に記載した図である。こうしてカウントされたグリッドの中から最大となる4票を得た左から4番目、上から3番目のbinを選定する。
最後に、選定されたbinに所定の間隔で位置姿勢候補を生成する。所定の間隔は、システムに要求される許容時間から逆算して得られる位置姿勢候補の個数に基づき、適切な値を設定すればよい。また、間隔を必ずしも均等にする必要はなく、位置姿勢候補の個数だけbinの内部にランダムに生成してもよい。図9(c)はこのようにして位置姿勢候補を9個生成した時の様子を示す図である。
こうして生成した位置姿勢候補に対して、ステップS404以降の処理を第1の実施形態同様に実行する。
このように、本実施形態によれば、概略位置姿勢の分布を所定のサンプリング間隔で集計して利用することで、第1の実施形態をより簡便で高速に実施することができる。
[第3の実施形態]
第1および第2の実施形態では、計測対象物体が取り得る複数の概略位置姿勢の分布が得られるものとして、概略位置姿勢の分布に基づいて、位置姿勢候補の生成を行う方法について述べた。しかし、位置姿勢候補の生成は,概略位置姿勢の分布に基づく方式に限るものではない。例えば、テンプレートマッチングなどにより概略位置姿勢を推定する場合には、マッチングするテンプレートの数や探索するステップ幅に応じて、推定される位置姿勢の分解能が決定される。このような場合、実際の位置姿勢に対して、概略位置姿勢の分解能の範囲内で、推定される位置姿勢に曖昧性が発生することになる。第3の実施形態では、この点を鑑み、概略位置姿勢推定における分解能に基づいて位置姿勢候補の生成を行う方法について説明する。以下は、これまでに説明した第1の実施形態の説明における位置姿勢候補の生成工程であるステップS403に置き換わる処理である。
まず、概略位置姿勢の分解能を決定する。ここで、概略位置姿勢の分解能とは,位置と姿勢を表現する最小ステップ幅のことを表す。例えば、位置の分解能が1[mm]である場合は、1[mm]よりも細かい値で概略位置姿勢が推定されることはない。概略位置姿勢の分解能は、概略位置姿勢の標本化幅や精度,計測対象物体のサイズ等により決定される。例えば、概略位置姿勢の推定方法として、第一の実施形態のステップS402で例に挙げたようなパターンマッチングベースの方法を利用する場合、姿勢の分解能はマッチングに利用するパターンとして登録する姿勢のサンプリング幅から決定される。例えば測地ドーム状に8度刻みで姿勢パラメータをサンプリングしてパターンマッチングする場合、サンプリング幅となる8度が姿勢の分解能となる。また、位置の分解能は、姿勢の分解能によって位置の推定を間違いうる範囲を計算することで求める。具体的には、以下の式に基づいて,姿勢パラメータの分解能ω[rad]と計測対象物体の長辺サイズw [mm]に基づいて、姿勢に基づく位置の最大ズレ幅d [mm]を求め,算出したdを位置の分解能として設定する。
本実施形態では、以上に示す方法により、姿勢のサンプリング幅と計測対象物体のサイズに基づいて、概略位置姿勢の分解能を決定する。ただし,概略位置姿勢の決定方法は以上に示す方法には限るものではない。例えば、位置の分解能を,概略位置姿勢の推定に利用する画像の解像度に基づいて設定しても良いし、以下の非特許文献に示す方式によって撮像素子ノイズ量や計測対象物体の形状に基づいて推定した位置姿勢推定の曖昧性に基づいて設定しても良い。
W. Hoff and T. Vincent, “Analysis of head pose accuracy in augmented reality, ”IEEE Transactions on Visualization and Computer Graphics, vol.6, no.4, pp.319−334, 2000.
また、設定パラメータとしてあらかじめ人が設定してもよい。概略位置姿勢の取りうる最小ステップ幅としての分解能を決定できる限り、いかなる決定方法を用いても良く、その決定方法によって本発明の本質が損なわれることはない。
次に、概略位置姿勢の分解能に基づいて、位置姿勢候補を生成する。具体的には、概略位置姿勢を中心とした概略位置姿勢の分解能幅の範囲内で、所定の間隔で位置姿勢をサンプリングすることで、位置姿勢候補を生成する。サンプリングする間隔は、システムに要求される許容時間から逆算して得られる位置姿勢候補の個数に基づき、適切な値を設定すればよい。また、間隔を必ずしも均等にする必要はなく、位置姿勢候補の個数だけランダムに生成してもよい。概略位置姿勢としては、第一の実施形態のステップS402において得られた概略位置姿勢の分布の平均値を用いればよい。また、概略位置姿勢の分布ではなく、単一の概略位置姿勢が取得される場合には、それを用いればよい。なお、位置姿勢候補の生成方法は以上の方法に限るものではなく、例えば,第一の実施形態と同様に,概略位置姿勢の分解能から、位置姿勢が取りうる範囲をガウス分布に近似し、ガウス分布に基づいて位置姿勢を生成しても良い。この場合,位置姿勢6パラメータに関する多次元ガウス分布の平均ベクトルμは、第一の実施形態のステップS402において取得した概略位置姿勢の分布の平均から求める。そして、ガウス分布の分散共分散行列Σを概略位置姿勢の分解能幅から設定することで、概略位置姿勢の分解能に基づいて多次元ガウス分布を算出する。そして、求めたガウス分布に基づいて、第1の実施形態のステップS403と同様に所定の個数の位置姿勢をサンプリングすることで位置姿勢候補を生成する。なお,ガウス分布に基づく位置姿勢の生成は,分解能に基づくガウス分布に加え、第1の実施形態で示すような概略位置姿勢の分布を近似した分布を併用して生成しても良い。この場合、具体的には、第1の実施形態のステップS403の方法で求めたガウス分布に、上述の分解能に基づくガウス分布を足し合わせた分布を算出する。そして、求めた分布に基づいて、第一の実施形態のステップS403と同様の方法で位置姿勢候補を生成すれば良い。
以上に示すように、位置姿勢候補の生成にあたっては、概略位置姿勢の分解能に基づいて位置姿勢候補を生成できる限り、生成方法に特に制限はなく、いかなる方法を用いても特に問題はない。
こうして生成した位置姿勢候補に対して、ステップS404以降の処理を第1の実施形態同様に実行する。
このように、本実施形態によれば、概略位置姿勢およびその分解能が既知であるシーンにおいて、分解能に基づいて決定した位置姿勢の誤りうる分布に基づいて候補となる位置姿勢を生成することで、粗い分解能で概略位置姿勢が取得されるような場合においても、ロバストな位置姿勢計測を行うことが可能になる。
[第4の実施形態]
第1の実施形態では、計測対象物体が取り得る概略位置姿勢分布を全探索によって取得した。しかし、実際にロボットが計測対象物体を把持して組み付ける現場では、部品はあらかじめ整列されており、全探索するまでもなく、配置された部品の位置姿勢がほぼ一定であるとみなせる場合がある。
第4の実施形態では、概略位置姿勢における対象部品の3次元モデル情報に含まれるエッジ分布に基づいてモデル情報の存在し得る位置姿勢分布を決定する方法を説明する。以下はこれまでに説明した位置姿勢候補分布を決定する第1の実施形態の説明における、ステップS402に置き換わる処理である。
図10は既知である概略位置姿勢における計測対象物体を表す図である。
3次元モデル1001は、概略位置姿勢で配置された3次元モデルである。
重心1002は、3次元モデルの重心であり、システムの座標系において3次元モデルの位置を指し示す時はこの重心の座標で示される。
テクスチャ1003は、計測対象物体が持つ縦縞のテクスチャ(テクスチャ情報)である。このようなテクスチャ1003をもつ計測対象物体を、エッジを用いて位置合わせする際は、対応点を探索する際に誤って隣の縦縞に対応してしまい、その結果、縦縞に対して垂直な方向にずれたフィッティングをされてしまうリスクがある。
本実施形態では、計測対象物体の取り得る位置姿勢(概略位置姿勢)は、上記の説明のように縦縞に対して垂直な方向に分布していると考え、この方向に網羅的に初期値を設定し、それぞれ位置姿勢を算出した結果を比較する。そして、よく合致しているものを最終的に正しい位置姿勢として出力する。
以降、その詳細を説明する。
図11(a)は、3次元モデル上の制御点を起点としてエッジを探索する様子を示す図である。制御点1101、1102、1103は、制御点の例である。これは第1の実施形態で説明した制御点702と同様のものである。それぞれの制御点について、制御点が乗っているエッジと垂直方向に探索ラインをとり、エッジの分布を探索する。エッジの探索については、第1の実施形態のステップS602で説明した方法と同じ方法で行う。
検出点1104は、制御点1101を起点に探索した時のエッジの検出点である。このときの検出点1104と制御点1101との相対位置を、重心1002を起点に置き換えた位置として記録する。
次に制御点1102を起点にエッジを探索する。この場合は図に示されるように探索ライン上にエッジがないため何も検出されない。同様にしてすべての制御点でエッジの探索を行い、検出された点と制御点との相対位置を重心を起点にして置き換えた位置として記録する。
図11(b)は、このようにして記録されたエッジ検出点の分布(各制御点から検出されたエッジ検出点の位置の分布を重心を起点に置き換えた位置の分布)を図示したものである。図中にXでプロットされた2つの点は制御点1101から検出された2つの点を示す。図11(a)の1104と図11(b)の1104とが対応している。図中の黒三角でプロットされた点は他の制御点から検出された点を示す。なお、図11(b)では説明のためにプロットの記号を使い分けたが、以降の処理ではこれらはすべて同等の分布を示す点として扱う。こうして得られたエッジ検出点の分布は、3次元モデルの制御点から対応点を探索する際に、どちらの方向にどれだけの3次元モデル内在点が誤対応され得るかを表している。
従って、この分布の位置に局所解が多いこととなり、位置姿勢候補をこの分布に従って生成することでフィッティングのロバスト性を向上させることができる。この分布を第1の実施形態における位置姿勢候補として用いて、ステップS403以降の処理を行う。
本実施形態では誤対応する可能性があるエッジとしてテクスチャのエッジを例に説明したが、本発明の効果はテクスチャに由来するエッジに限るものではない。計測対象物体の形状に由来するエッジなどテクスチャ以外の特徴に起因するものであっても何ら変わりはない。
形状由来の誤対応しやすいエッジとして簡単な例では、のこぎりのギザギザ歯のような繰り返しパターンを持つ形状が考えられる。このような形状においては、本実施形態の手法によると、ギザギザの歯が並んでいる方向に取り得る位置姿勢が分布していると考えて網羅的に位置姿勢候補を生成することで、最適な位置姿勢を決定することができる。
本実施形態では、本発明で扱うフィッティングの支配的な誤差要因がエッジの誤対応であると考え、位置姿勢候補の分布を撮像画像の2次元平面内において、位置姿勢のうちxy方向の並進成分についてのみ求めた。
しかし、この位置姿勢候補の分布の決定は、2次元に限るものではなく、3次元においても同様に行うことができる。図11を用いて説明した3次元モデル上の制御点を起点としたエッジの探索と同等の処理を3次元で行えばよい。具体的には3次元モデル上の制御点が乗る面の法線方向に面を探索して分布を生成することが考えられる。
また、2次元及び3次元の回転についても同様に探索を行って位置姿勢候補の分布を生成することは可能である。具体的にはサンプリングされた複数の回転軸において3次元モデル上の各制御点を所定の回転量の範囲で円弧方向にエッジを探索し、特徴が検出されたらその時の回転軸と回転量を投票する方法が考えられる。つまり、概略位置姿勢と3次元モデルの情報から、位置姿勢候補を求め、複数の位置姿勢候補を生成することでロバスト性の向上を図ることが本発明の第4の実施形態の主旨である。
このように、本実施形態によれば、概略位置姿勢が既知であるシーンにおいて、既知の概略位置姿勢と3次元モデル情報とから位置姿勢候補の分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することで複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。
[第5の実施形態]
第4の実施形態では、計測対象物体の概略位置姿勢と3次元モデルとから仮想環境上で、計測対象物体が取り得る位置姿勢分布を求めた。そして、計測対象物体の概略位置姿勢の3次元モデルと撮像画像の対応点を生成する際に、部品に内在する別のエッジに誤対応してしまう問題に対応した。
この他の問題として、撮像装置によって得られた画像の概略位置姿勢における部品周辺に分布するエッジに誤対応してしまう問題が考えられる。第5の実施形態では配置された計測対象物体の概略位置姿勢が既知である状況下で、位置姿勢候補を、撮像画像に含まれる特徴の、概略位置姿勢における3次元モデル情報の原点を基準とする相対位置分布に基づいて決定することで上記の問題に対応する方法を説明する。
以下はこれまでに説明した位置姿勢分布を決定する第1の実施形態の説明におけるステップS402に置き換わる処理である。
図12は、撮像して得られた輝度画像である。枠1201は撮像部より得られる輝度画像の外枠である。計測対象物体1202は、輝度画像に写りこんだ計測対象物体である。横縞1203は、輝度画像に写りこんだ横縞(ノイズ)である。ここでいうノイズとは、計測対象物体1202自身ではないということを示している。すなわち、計測対象物体の背景に存在する他の物体であったり、背景そのものに起因するものであってもよい。
本実施形態においては、ノイズが、他の物体に起因するものであることを想定している。このような場合において、計測対象物体1202を、エッジを用いてフィッティングする際は、対応点を探索する際に誤って、計測対象物体の上下にある、その他の物体に起因した横縞の背景に対応してしまい、図中の縦方向にずれたフィッティングをされてしまうリスクがある。
本実施形態では、部品の取り得る位置姿勢は上記の説明のように図中の縦方向に分布していると考え、この方向に網羅的に位置姿勢候補を設定し、それぞれ位置姿勢を算出した結果を比較し、よく合致しているものを最終的に正しい位置姿勢として出力する。
以降、その詳細を説明する。(なお、ここでは手法の効果を分かりやすくするために横縞の背景を例に説明するがいかなるテクスチャでも同様の方法で本発明の実施は可能である。)
図13(a)は、輝度画像上で3次元モデル上の制御点を起点としてエッジを探索する方法を示す図である。
座標軸1301は、システムの座標系を示す。投影像1302は、概略位置姿勢における3次元モデルエッジの輝度画像への投影を示す。投影点1303は、概略位置姿勢における3次元モデルの重心を輝度画像に投影した点である。
制御点1304、1305、1306は、制御点の例である。これらは、第1の実施形態で説明した制御点702と同様のものである。これらそれぞれの制御点について制御点が乗っているエッジに垂直な方向に探索ラインをとり、エッジを探索する。エッジの探索については第1の実施形態のS602で説明した方法と同じ方法で行う。検出点1307は、制御点1301を起点に探索した時のエッジの検出点である。このときの制御点1301との相対位置を重心1303を起点とした置き換えた位置として記録する。次に制御点1302を起点にエッジを探索する。この場合は右方向に一つ検出点が記録される。同様にしてすべての制御点でエッジの探索を行い、検出された点と制御点との相対位置を重心を起点にして置き換えた位置として記録する。
図13(b)は、このようにして記録されたエッジ検出点の分布(各制御点から検出されたエッジ検出点の位置の分布を重心を起点に置き換えた位置の分布)を図示したものである。図中にXでプロットされた3つの点は制御点1305から検出された3つの点を示す。図13(a)の1307と図13(b)の1307は対応している。図中の黒三角でプロットされた点は他の制御点から検出された点を示す。
なお、図13(b)では説明のためにプロットの記号を使い分けたが、以降の処理ではこれらはすべて同等の分布を示す点として扱う。こうして得られたエッジ検出点の分布は、3次元モデルの制御点から対応点を探索する際に、どちらの方向にどれだけ誤対応点が検出され得るかを表している。従ってこの分布の位置に局所解が多いこととなり、初期位置姿勢をこの分布に従って生成することでフィッティングのロバスト性を向上させることができる。この分布を第1の実施形態における位置姿勢分布として用いてS403以降の処理を行う。
本実施形態では誤対応する可能性があるエッジとしてテクスチャのエッジを例に説明したが、本発明の効果はテクスチャに由来するエッジに限るものではなく3次元モデルの形状に由来するエッジや背景に由来するエッジなどテクスチャ以外の特徴に起因するものであっても何ら変わりはない。
本実施形態では第4の実施形態同様、位置姿勢分布を撮像画像の2次元平面内において位置姿勢のうちxy方向の並進成分についてのみ求めた。しかし、この位置姿勢分布の決定は2次元に限るものではなく3次元においても同様に行うことができる。
図13を用いて説明した3次元モデル上の制御点を起点としたエッジの探索と同等の処理を3次元で行えばよい。具体的には3次元モデル上の制御点を起点として制御点が乗る面の法線方向に一点の間隔で距離画像上の対応点を探索して対応点の分布をとることが考えられる。
また、2次元及び3次元の回転についても同様に探索を行って位置姿勢分布を生成することは可能である。具体的にはサンプリングされた複数の回転軸において3次元モデル上の各制御点を所定の回転量の範囲で円弧方向にエッジを探索し、特徴が検出されたらその時の回転軸と回転量を投票する方法が考えられる。要は扱う次元に依らず、撮像された画像を全探索して位置姿勢分布を求めるのではなく、撮像された画像のうち、概略位置姿勢で配置された3次元モデルの周辺のみを探索することで複数の初期位置姿勢を生成し、ロバスト性の向上を図ることが本発明における第5の実施形態の主旨である。
このように、本実施形態によれば、概略位置姿勢が既知であるシーンにおいて、画像内の概略位置姿勢における3次元モデルの周辺で探索することで位置姿勢分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することでノイズの多い環境や複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことができる。
[第6の実施形態]
第4〜第5の実施形態では、位置姿勢分布をフィッティングする先の形状情報として画像を対象に、該画像の上に投影された3次元モデルの位置姿勢を求めた。しかし、各実施形態において補足したように、本発明は形状情報の一方が画像でなくてはならないという制約はなく、二つの形状情報の間に対応が取れる領域があれば、任意の形態の形状情報のフィッティングにおいて本発明は適用可能である。
例えば互いに三次元形状情報同士をフィッティングする際に本発明を適用することも可能である。その例としてここでは、ある対象物体を二つの視点から撮影して生成した二つのメッシュモデルを、互いにオーバーラップ(重複)している領域の形状に基づいてフィッティングする例を説明する。つまり、それぞれのメッシュモデルは、少なくとも1部の領域で重複している。このように3次元形状情報同士をフィッティングする場合においても本発明は適用可能であることを説明する。
図14は、点線で描かれているメッシュモデル1401(第1の形状情報)を、実践で描かれているメッシュモデル1402(第2の形状情報)にフィッティングする例を2次元に投影して説明する図である。1403は形状情報1401上に定められた制御点である。各制御点においてその点が乗っている面の法線方向に一定区間対応点候補を探索する。探索して見つかった対応点候補は図11(b)または図13(b)と同様に制御点から対応点候補へのベクトルを形状情報1401の重心を中心にしてプロットして三次元空間上での概略位置姿勢候補の分布を生成する。この分布を第1の実施形態における位置姿勢分布として用いてS403以降の処理を行う。
[第7の実施形態]
第1〜第6の実施形態ではそれぞれ異なる方法で位置姿勢分布を生成した。実際にはこれらを独立に実施するのではなく、これらの一部または全部の和の分布を位置姿勢分布として用いてS403にて複数の初期位置姿勢を生成(初期位置姿勢生成)することでよりロバストな位置姿勢計測を実現できる。
[第8の実施形態]
本発明に係る情報処理装置の好適な適用事例としては、産業用ロボットアームで部品の把持、移動、解放といった動作を行うシステムにおいて対象部品の位置姿勢計測に利用することが挙げられる。以下、図15を用いて、本発明に係る情報処理装置の適用例を説明する。
図15は、本発明の第7の実施形態に係る情報処理装置を含む情報処理システムの概略構成の一例を示す模式図である。1501はPC、1502はカメラ及びプロジェクタである。PC1501とカメラ及びプロジェクタ1502は第1〜第6の実施形態で説明したシステムと同様のものであり、先に説明した処理手順が記述されたプログラムを格納し、対象部品1505の位置姿勢の計測が可能である。1503はロボットコントローラでロボットアームを制御する。1504はロボットアームであり、回転軸及び/または並進移動軸からなる可動軸を有する。ロボットコントローラはPCに接続されており、ロボットアームへの命令はPCから受け取ったものを具体的な制御命令に変換してロボットアームを動かす。
本実施形態におけるPCは上記に加えてさらに次の処理手順を包含する。まず第1の実施形態1〜5で説明したシステム座標系すなわちカメラ座標系とロボットアームで定義されるロボット座標系の相対関係を保持する。PCは対象部品の位置姿勢を計測し、その結果をカメラの座標系からロボット座標系に変換する。次に変換された部品のロボット座標系における位置姿勢を元にロボットコントローラを介してロボットアームが部品を把持できる位置姿勢に移動させる。
このように、本実施形態によれば、カメラとプロジェクトをアーム上に設置したロボットシステムにおいて、本発明による部品の位置姿勢の計測結果に基づいてロボットアームを処理することで未知である部品の把持をすることができる。
[その他の実施形態]
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア(コンピュータプログラム)を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ(またはCPUやMPU等)がプログラムを読み出して実行する処理である。
<実施形態の効果>
以上説明したように、対象物体の概略位置姿勢を取得し、取得した概略位置姿勢に基づいて、新たに初期位置姿勢の候補を生成し、それらに基づいて位置姿勢を導出することで、ロバストな位置姿勢計測を可能にしている。
第1の実施形態では、計測対象物体を画像内で全探索した結果から、概略位置姿勢の分布を決定し、この分布に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することでノイズの多い環境や複雑な形状の計測対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。
第2の実施形態では、概略位置姿勢の分布を所定のサンプリング間隔で集計して利用することで、第1の実施形態をより簡便で高速に実施する方法を示した。
第3の実施形態では、概略位置姿勢およびその分解能が既知であるシーンにおいて、分解能に基づいて決定した位置姿勢が誤りうる分布に基づいて候補となる位置姿勢を生成することで、粗い分解能で概略位置姿勢が取得されるような場合においても、ロバストな位置姿勢計測を行うことが可能になる
第4の実施形態では、概略位置姿勢が既知であるシーンにおいて、既知の概略位置姿勢と3次元モデル情報から位置姿勢候補を生成し、生成された位置姿勢候補に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することで複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。
第5の実施形態では、概略位置姿勢が既知であるシーンにおいて、画像内の概略位置姿勢における3次元モデルの周辺で探索することで位置姿勢候補を生成し、生成された位置姿勢候補に基づいて局所解となり得る複数の位置姿勢を網羅的に検証することでノイズの多い環境や複雑な形状の対象物体に対してもロバストな位置姿勢計測を行うことを可能にした。
第6の実施形態では、二つの形状情報の間に対応が取れる領域があれば、任意の形態の形状情報のフィッティングにおいて本発明は適用可能であるという例を示した。これにより、CADモデルなどの形状データが未知の物体に対して、複数の方向から撮像したデータをフィッティングしてデータを結合することが可能になる。
第7の実施形態では、第1〜第5の実施形態で示された位置姿勢候補の生成方法のうちの全部または一部の方法で生成された位置姿勢候補の和集合を複数の位置姿勢候補として利用することでより、ロバストな位置姿勢計測を行うことを可能にした。
第8の実施形態ではカメラとプロジェクトをアーム上に設置したロボットシステムにおいて、本発明よる部品の位置姿勢の計測結果に基づいてロボットアームを処理することで未知である部品の把持を可能にした。
<定義>
本発明の説明では、フィッティングする形状情報の組み合わせとして第1〜第5の実施形態においては3次元モデル情報とカメラで撮影された計測対象物体を含む画像の組み合わせを、第6の実施形態においてはおよび三次元メッシュモデル同士の組み合わせを例に説明した。しかし、必ずしもこの組み合わせでなくてもよく、形状情報間で対応を取って位置合わせ(フィッティング)可能な情報であればなんでもよい。物体の形状情報の例としては3次元モデル情報、画像のほかに、レーザーレンジセンサーなどで計測して得られた3次元点群情報なども考えられる。
プロジェクタは液晶プロジェクタに限るものではなく、パターン光の投影ができれば他の方式のものでもよい。例えば、DMD(デジタルミラーデバイス)やLCOSを用いたプロジェクタであってもよい。
位置姿勢算出においては、2次元と3次元の尺度で同時に位置合わせをする例を説明したが、本発明は2次元の尺度のみであったり、3次元の尺度のみであっても全く同様に適用可能である。
位置姿勢算出において、計測対象部品の位置姿勢の算出方法はGauss−Newton法に限るものではない。例えば、より計算がロバストであるLevenberg−Marquardt法によって行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やICCG法など、他の非線形最適化計算手法を用いてもよい。
概略位置姿勢の分布決定において、画像内での特定の部品の位置姿勢を探索する際の画像は輝度画像のみに限定されるものではなく距離画像などの3次元情報を探索する手法であってもよい。
概略位置姿勢の分布決定において、画像を全探索する場合を説明したが、本発明の実施においては必ずしも得られた画像を全探索する必要はない。例えば部品が常に作業スペース内のほぼ同じ場所にあることが分かっている場合はその領域に限定して部分的に探索するなどしても何ら問題はない。
本発明の第4の実施形態で説明した位置姿勢分布決定において、位置姿勢分布の探索範囲は2次元平面内におけるxy方向の並進成分のみに限定されない。位置姿勢分布の決定は2次元に限るものではなく3次元においても同様に行うことができる。図11を用いて説明した3次元モデル上の制御点を起点としたエッジの探索と同等の処理を3次元で行えばよい。
具体的には3次元モデル上の制御点が乗る面の法線方向に面を探索して分布を生成することが考えられる。また、2次元及び3次元の回転についても同様に探索を行って位置姿勢分布を生成することは可能である。具体的にはサンプリングされた複数の回転軸において3次元モデル上の各制御点を所定の回転量の範囲で円弧方向にエッジを探索し、特徴が検出されたらその時の回転軸と回転量を投票する方法が考えられる。
本発明の第5の実施形態で説明した概略位置姿勢分布決定において、位置姿勢分布の探索範囲は2次元平面内におけるxy方向の並進成分のみに限定されない。位置姿勢分布の決定は2次元に限るものではなく3次元においても同様に行うことができる。図13を用いて説明した3次元モデル上の制御点を起点としたエッジの探索と同等の処理を3次元で行えばよい。具体的には3次元モデル上の制御点を起点として制御点が乗る面の法線方向に一点の間隔で距離画像上の対応点を探索して対応点の分布をとることが考えられる。また、2次元及び3次元の回転についても同様に探索を行って位置姿勢分布を生成することは可能である。
本発明の第4の実施形態、および第5の実施形態で説明した位置姿勢候補の分布の決定において、形状情報の特徴とはテクスチャに由来するエッジに限るものではなくS602の対応付けの根拠になり得る特徴であれば何でもよい。例えば3次元モデルの形状に由来するエッジなどテクスチャ以外の特徴でもよい。
位置姿勢候補生成において、概略位置姿勢分布として挙げられている位置姿勢には一定の合理性があると考え、位置姿勢候補として採用した。しかし、本発明の実施においては概略位置姿勢の分布として挙げられたものは採用せずに、新たに生成した位置姿勢候補だけを採用してもよい。
位置姿勢候補生成において、確率密度モデルとしてガウス分布を適用したが、実際には得られた分布を要約できる分布であれば他の分布であってもよいし、もっと簡単に方形状の分布を適用してもよい。
本発明の第2の実施形態で説明した位置姿勢候補の生成において、選定されたbinに所定の間隔で初期値を生成する際は間隔を必ずしも均等にする必要はなく、位置姿勢候補の個数だけbinの内部にランダムに生成してもよい。
位置姿勢候補選択において、スコアは3次元モデルと輝度画像及び距離画像の合致している度合いを表す値なら何でもよい。例えば3次元モデルと輝度画像及び距離画像で誤差ベクトルを求めそのノルムの逆数を用いる方法が考えられるがその他の指標でもよい。
位置姿勢候補選択において、位置姿勢候補をスコアで評価する前に位置姿勢更新処理ステップS601〜ステップS606を1回行っているが、この回数は必ずしも1回である必要はなく、位置姿勢候補が最適解に収束する見込みを高速に確かめられる方法ならばなんでもよい。例えば位置姿勢更新を1回も行わずにスコアを算出してもよいし、または位置姿勢更新を2回以上行ってからスコアを算出してもよい。このように選択処理の具体的な手順については無数の方法が考えられるが本発明の実施においては特にどれでなくてはならないということはない。