(57)【要約】【課題】モデルフィッティング処理において、対象物体の形状を示すモデルと、該対象物体を計測して得られた観測点との対応付けを高速に行う。【解決手段】対象物体の複数の観測点の座標値にインデックス値を付し、観測点テーブルとして観測点テーブル保持部140に保持しておく。さらに、観測点テーブルにおける各インデックス値を、各座標値に対応する画素の値とするインデックスマップ画像を、インデックスマップ保持部170に保持しておく。そしてモデル点入力部130がモデルにおける幾何特徴点を示すモデル点の座標値を入力し、モデル点座標変換部180がモデル点を示す座標値をインデックスマップ画像の2次元座標系に変換する。そしてインデックスマップ参照部190が、該変換された座標値に対応するインデックスマップ画像の画素の値から、観測点テーブルのインデックス値を取得する。該取得されたインデックス値に基づいて観測点テーブルを参照することで、モデル点に対応する観測点が特定される。【選択図】  図1
2014120068.tif 000002
【特許請求の範囲】
【請求項１】
対象物体の形状を示すモデルにおけるモデル点と、該対象物体を計測して得られた画像中に含まれる観測点との対応付けを行う情報処理装置であって、
対象物体の幾何特徴を示す観測点の座標値にインデックス値を付し、該インデックス値と座標値の関係を保持する第1の保持手段と、
前記第1の保持手段によって保持された座標値を変換し、該変換された座標値の示す画素に前記インデックス値を格納したインデックスマップを保持する第2の保持手段と、
前記モデルにおける幾何特徴を示すモデル点の座標値を前記インデックスマップの座標系に変換するモデル点座標変換手段と、
前記モデル点座標変換手段によって変換された座標値に対応する前記インデックスマップの画素に格納されたインデックス値を取得する取得手段と、を有し、
前記取得手段で取得されたインデックス値に基づいて前記モデル点に対応する観測点が特定されることを特徴とする情報処理装置。
【請求項２】
さらに、前記観測点の座標値を、前記インデックスマップの座標系に変換する観測点座標変換手段と、
該変換された座標値について、前記インデックスマップにおいて対応する画素に対し、該座標値に対応する前記観測点に付された前記インデックス値を書き込む書き込み手段と、
前記インデックスマップにおいて、前記インデックス値を周辺の画素に拡張する拡張手段と、を有し、
前記第2の保持手段は、前記拡張手段によりインデックス値が拡張された前記インデックスマップを保持することを特徴とする請求項1に記載の情報処理装置。
【請求項３】
前記拡張手段は、前記インデックス値を、隣接する、値が未設定である画素に伝播させることを特徴とする請求項2に記載の情報処理装置。
【請求項４】
さらに、前記モデル点の座標値を前記観測点の座標系に変換するための座標パラメータを入力する座標パラメータ入力手段を有することを特徴とする請求項1乃至3のいずれか1項に記載の情報処理装置。
【請求項５】
さらに、前記取得手段で取得されたインデックス値に基づいて取得される前記観測点の座標値と、前記モデル点座標変換手段で前記モデル点の座標値を前記観測点の座標系に変換した座標値を用いて、前記観測点と前記モデル点の差分を計算する差分計算手段、を有することを特徴とする請求項4に記載の情報処理装置。
【請求項６】
さらに、前記差分に基づいて、前記対象物体の位置姿勢を示す位置姿勢パラメータを推定する推定手段を有することを特徴とする請求項5に記載の情報処理装置。
【請求項７】
さらに、前記対象物体の画像を含む距離画像において該対象物体の幾何特徴を示す距離点の座標値を第1の観測点として入力する第1の観測点入力手段と、
前記対象物体の画像を含む2次元画像において該対象物体の幾何特徴を示すエッジ点の座標値を第2の観測点として入力する第2の観測点入力手段と、を有し、
前記推定手段は、前記第1および第2の観測点に基づいて前記位置姿勢パラメータを推定することを特徴とする請求項6に記載の情報処理装置。
【請求項８】
さらに、前記推定手段により推定された前記位置姿勢パラメータに基づき、前記対象物体の位置姿勢を制御する位置姿勢制御手段、を有することを特徴とする請求項6または7に記載の情報処理装置。
【請求項９】
さらに、前記対象物体を撮影する撮像手段を有し、
前記撮像手段によって撮影された前記対象物体の画像から、前記観測点の座標値を取得することを特徴とする請求項1乃至8のいずれか1項に記載の情報処理装置。
【請求項１０】
前記観測点は、前記対象物体の画像を含む距離画像において該対象物体の幾何特徴を示す距離点であることを特徴とする請求項1乃至9のいずれか1項に記載の情報処理装置。
【請求項１１】
前記観測点は、前記対象物体の画像を含む2次元画像において該対象物体の幾何特徴を示すエッジ点であることを特徴とする請求項1乃至9のいずれか1項に記載の情報処理装置。
【請求項１２】
第1および第2の保持手段、モデル点座標変換手段、および取得手段、を有する情報処理装置において、対象物体の形状を示すモデルにおけるモデル点と、該対象物体を計測して得られた画像中に含まれる観測点との対応付けを行う情報処理方法であって、
前記第1の保持手段が、対象物体の幾何特徴を示す観測点の座標値にインデックス値を付し、該インデックス値と座標値の関係を保持し、
前記第2の保持手段が、前記座標値を変換し、該変換された座標値に付されたインデックス値が書き込まれたインデックスマップを保持し、
前記モデル点座標変換手段が、前記モデルにおける幾何特徴を示すモデル点の座標値を前記インデックスマップの座標系に変換し、
前記取得手段が、該変換された座標値に対応する前記インデックスマップの画素に格納されたインデックス値を取得し、
該取得されたインデックス値に基づいて、前記モデル点に対応する観測点が特定されることを特徴とする情報処理方法。
【請求項１３】
コンピュータ装置で実行されることにより、該コンピュータ装置を請求項1乃至11のいずれか1項に記載の情報処理装置の各手段として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象物体の形状モデルと観測点とを対応付けてモデルフィッティング処理を行う情報処理装置およびその制御方法に関する。
【背景技術】
【０００２】
対象物体の形状モデルと、対象物体を観測した画像(距離画像等)における観測値との差を小さくするように、形状モデルの位置姿勢を推定する方法は、モデルフィッティングと呼ばれ、対象物体の位置姿勢を非接触で推定する技術の一つとして知られている。この技術は、ロボットによる部品のビンピッキング等、不特定の位置姿勢に置かれた部品(対象物体)をビジョンセンサにより位置姿勢を計測する場合等において利用される。
【０００３】
ここで形状モデルとは、対象物体の観測される面の形状を表現したもので、例えばポリゴンモデル等の局所平面の組み合わせで表現される。形状モデルと対象物体の観測値との差を計算する際に、形状を構成する各幾何形状に対して、観測された画像中に含まれる幾何特徴のエッジや、距離画像の距離点との対応付けを行う必要がある。
【０００４】
形状モデルと観測点を対応付ける方法として、各幾何形状に対して最近傍の観測された幾何特徴を対応付ける方法が開示されている(例えば、非特許文献1参照)。この方法は、対象物体の表面にテクスチャなどの特徴が無い部品において有効である。
【０００５】
幾何特徴同士を対応付ける処理は、位置姿勢推定の並進・回転成分の非線形パラメータを推定するステップ毎に行われる。一般に距離画像や画像特徴のデータ量は膨大であるため、この対応付け処理を効率化する技術が要求されている。
【０００６】
距離画像の距離点と形状モデルの面との効率的な対応付けに関しては、空間的に区分分割された範囲に距離点を格納して探索時間を効率化する方法が開示されている(例えば、非特許文献2参照)。また、画像中のエッジ点と形状モデルとの対応付けに関しては、撮影画像から幾何形状の方向を考慮して、画像の探索範囲を1次元に限定して探索する方法が知られている(例えば、特許文献1参照)。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第4649559号公報
【非特許文献】
【０００８】
【非特許文献１】Paul J.Besl,Neil D.McKay,“A Method for Registration of 3-D Shapes”。IEEE Transactions of pattern analysis and machine intelligence,vol.14,no.2,(1992).
【非特許文献２】ZHENYOU ZHANG,“Iterative Point Matching for Registration of Free-Form Curves and Surfaces”。International Journal of Computer Vision,13-2,pp.119-152,(1994).
【発明の概要】
【発明が解決しようとする課題】
【０００９】
上記従来のモデルフィッティング処理においては、形状モデルと観測点との対応付け処理が位置姿勢推定パラメータの更新ステップ毎に行われるため、その処理量は膨大となる。上述した特許文献1に記載の技術では、各注目画素から距離が最短となる特徴部(エッジ部)までの距離を算出するため、その演算負荷が大きいという問題がある。
【００１０】
本発明は上記課題に鑑み、モデルフィッティング処理において、対象物体の形状モデルと観測点との対応付けを高速に行うことを可能とする情報処理装置および情報処理方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するための一手段として、本発明の情報処理装置は以下の構成を備える。
【００１２】
すなわち、対象物体の形状を示すモデルにおけるモデル点と、該対象物体を計測して得られた画像中に含まれる観測点との対応付けを行う情報処理装置であって、対象物体の幾何特徴を示す観測点の座標値にインデックス値を付し、該インデックス値と座標値の関係を保持する第1の保持手段と、前記保持手段によって保持された座標値を変換し、該変換された座標値の示す画素に前記インデックス値を格納したインデックスマップを保持する第2の保持手段と、前記モデルにおける幾何特徴を示すモデル点の座標値を前記インデックスマップの座標系に変換するモデル点座標変換手段と、前記モデル点座標変換手段によって変換された座標値に対応する前記インデックスマップの画素に格納されたインデックス値を取得する取得手段と、を有し、前記取得手段で取得されたインデックス値に基づいて前記モデル点に対応する観測点が特定されることを特徴とする。
【発明の効果】
【００１３】
本発明によれば、モデルフィッティング処理において、対象物体の形状モデルと観測点との対応付けを高速に行うことが可能となる。
【図面の簡単な説明】
【００１４】
【図１】第1実施形態における情報処理装置の構成を示すブロック図、
【図２】第1実施形態におけるインデックスマップ生成処理を示すフローチャート、
【図３】第1実施形態におけるインデックスマップ生成の概念図、
【図４】第1実施形態におけるモデル点と観測点の残差計算処理を示すフローチャート、
【図５】第2実施形態における位置姿勢計測装置の構成を示すブロック図、である。
【発明を実施するための形態】
【００１５】
以下、本発明実施形態について、図面を参照して説明する。なお、以下の実施の形態は特許請求の範囲に関わる本発明を限定するものではなく、また、本実施の形態で説明されている特徴の組み合わせの全てが本発明の解決手段に必須のものとは限らない。
【００１６】
＜第1実施形態＞
本発明は、観測点とモデル点との対応付けを行うために、検出された幾何特徴である観測点の位置情報を格納したテーブルのインデックスを画素として保持し、該画素値を周辺画素に拡張するようにインデックスマップを作成する。このインデックスマップを用いることにより、モデル点と観測点との対応関係をより高速に得ることができ、すなわちより高速なモデルフィッティングが可能となる。
【００１７】
本発明は詳細には、対象物体の形状を示すモデルにおけるモデル点と、該対象物体を計測して得られた画像中に含まれる観測点との対応付けを行う情報処理装置において、以下の機能を実現する。まず、対象物体の幾何特徴を示す観測点の座標値にインデックス値を付し、該インデックス値と座標値の関係を第1の保持手段に保持する。そして、第1の保持手段によって保持された座標値を変換し、該変換された座標値の示す画素にインデックス値を格納したインデックスマップを第2の保持手段に保持する。そして、モデルにおける幾何特徴を示すモデル点の座標値をインデックスマップの座標系に変換し、該変換された座標値に対応するインデックスマップの画素に格納されたインデックス値を取得する。このように取得されたインデックス値に基づいて、モデル点に対応する観測点が特定される。
【００１８】
●装置構成
図1に、本実施形態における情報処理装置のブロック構成例を示す。本実施形態は、
対象物体の形状モデル(以下、モデル)と、対象物体の観測情報との差(残差)が小さくなるように、モデルの位置姿勢を推定するモデルフィッティング方法において、観測情報とモデルとの差を対応付ける処理に関するものである。具体的には、観測点の情報、モデル点、そしてモデル点を観測座標系に合わせるように変換する変換パラメータを入力とし、対応付けした観測点とモデル点、および、その観測座標系での残差を出力とする。
【００１９】
なお、本実施形態における観測点とは、距離画像や濃淡画像中に含まれる幾何特徴点であり、幾何特徴としては、距離画像であれば距離点、濃淡画像であればエッジ等である。本実施形態では濃淡画像を用いた例を説明するが、本発明はこれに限らず、例えばカラー画像等、2次元画像であれば適用可能である。また観測点は、テクスチャの局所的な濃度分布を表す特徴点でもあっても構わない。つまり、計測対象物体のモデルと、実際に計測対象物を観測することで検出される点との対応がとれる幾何特徴であれば、どのような幾何特徴を利用しても構わない。
【００２０】
また、モデル点とは、計測対象物体の形状から実際に観測される幾何特徴との対応がとれる点である。距離画像を対象としたモデル点は、形状表面の点である。また、濃淡画像のエッジを幾何特徴とする場合には、モデル形状から観測される濃度勾配が変化するエッジの位置を、モデル点として利用することができる。観測点の幾何特徴とモデル点の幾何特徴を対応付けることができれば、どのような幾何特徴の組み合わせであっても構わない。
【００２１】
以下、本実施形態における情報処理部100における各構成について説明する。観測点入力部110は、モデルとの残差計算の対象となる距離画像や濃淡画像に含まれる幾何特徴の座標値を観測点として入力する。具体的には、距離画像であれば距離点を、濃淡画像であればエッジ点の位置を示す座標値を入力する。
【００２２】
観測点テーブル保持部140は、観測点入力部110で観測点として入力された座標値を、重複がないようにインデックスを付与してテーブルとして格納する。
【００２３】
観測点座標変換部150は、観測点の座標をインデックスマップ座標系に変換する。観測点が3次元の座標値である場合には、これを2次元(観測画面)の座標値に投影する計算(射影計算)を行う。この射影計算においては、投影される2次元の観測画面が後述するインデックスマップの画像のサイズに合うように、すなわちインデックスマップ座標系への変換が行われるように、パラメータが用意されているものとする。なお、観測点が2次元の座標値である場合には、インデックスマップ座標系への並進・拡大等を行う。
【００２４】
インデックスマップ参照領域処理部160は、観測点座標変換部150で変換された座標値に基づき、観測点テーブル保持部140に保持されたインデックスの値をインデックスマップに書き込み、そのインデックス値を周辺の領域に伝播させる。インデックスマップ保持部170は、インデックスマップ参照領域処理部160による処理後のインデックスマップを、画像として参照できるように保持する。例えば、観測点テーブルのインデックスを示す値を保持する画像形式の配列からなるインデックスマップ画像として保持する。このインデックスの値は直接配列の番号でも良いが、ハッシュ値を計算するなどの所定の参照値の変換値を利用しても良い。
【００２５】
次に、計測対象となる対象物体のモデル表面上の幾何特徴の座標値との比較を行う構成について説明する。
【００２６】
モデル点座標パラメータ入力部120は、モデル座標系で表現されているモデル点を観測点と同じ観測座標系(ビュー座標系)へ幾何変換するための並進・回転のパラメータを、モデル点座標パラメータとして入力する。一般にモデル点座標パラメータは、モデル座標系からビュー座標系への変換を行うモデルビュー変換行列の値であり、カメラの位置姿勢や焦点距離、主点位置などの観測点と対応が取られている値であることが望ましい。なお、モデル点座標パラメータの値としては位置姿勢パラメータの推定値であることが想定されるが、残差量だけを計算したい場合には、比較対象となる位置姿勢のパラメータ値であっても構わない。
【００２７】
モデル点入力部130は、計測対象物体における幾何特徴に対応する、モデル表面上の位置をモデル座標系で表記した座標を、モデル点として入力する。ここでモデル点としては、距離画像を観測対象とする場合には、計測対象物体の形状モデルにおける面ポリゴン上の可視の点を利用する。なお、形状情報として利用できれば、モデル点をポリゴン以外の表現、例えば解析曲面と境界による表現やボクセルでの体積表現としても構わない。すなわち、観測される物体表面の情報が得られる形状表現であれば、モデル点の表現形式は問わない。
【００２８】
また、画像中のエッジを観測対象とする場合には、計測対象物体の構造的に可視のルーフエッジやジャンプエッジのポリゴンの辺上の点を、モデル点として利用する。ここでは、計測対象物体の観測情報から検出できる幾何特徴と対応付けられるものをモデル点情報とし、少なくともモデル座標系での位置座標が含まれていれば、いかなる幾何特徴であっても構わない。例えば、物体表面に貼付されているシールに印字されている文字などの幾何特徴をモデル点情報としてもよい。
【００２９】
モデル点座標変換部180は、まず、モデル点座標パラメータ入力部120で入力されたモデルビュー変換行列に対し、モデル点入力部130で入力されたモデル点の座標値を乗じることで、モデル座標系のモデル点を観測座標系へ変換する。ここで変換された観測座標系のモデル点の座標値(以下、モデルビュー座標値)は、観測点・モデル点残差計算部200へ出力され、残差計算に用いられる。モデル点座標変換部180ではさらに、モデルビュー座標値からインデックスマップ座標値への射影変換が行われる。この座標変換には、射影変換行列を必要とする。この射影変換行列の値としては、インデックスマップ保持部170が保持する画像のパラメータを利用しても良いし、モデル点座標パラメータ入力部120から入力しても良い。
【００３０】
インデックスマップ参照部190は、モデル点座標変換部180で変換されたインデックスマップ座標系の座標値に登録された、インデックスマップの値を参照し、該値を観測点に対応するインデックス値として取得する。これにより、モデル点に対し、観測点テーブルに登録された観測点が実質的に対応付けられる。なお、インデックスマップは画像として参照できるようになっているため、インデックスマップがメモリとして保持されている場合には、座標系のオフセットを計算してアドレスを参照すれば良い。
【００３１】
観測点・モデル点残差計算部200は、対応付けられた観測点とモデル点の差分計算を行うことで、残差を出力する。まずインデックスマップ参照部190から得られたインデックス値に基づき、観測点テーブル保持部140においてインデックス値が同じであるデータを参照することで、現在処理中であるモデル点に対応する観測点の座標値を得る。そして、該取得した観測点の座標値と、モデル点座標変換部180で観測座標系に変換されたモデル点の座標値(モデルビュー座標値)から、この2点間の距離を残差として計算する。残差の計算方法としては例えば、観測対象が距離画像である場合には、3次元の点のユークリッド距離を計算すれば良い。距離画像における観測点の周辺を局所平面として近似することで、その面の法線を算出することができる。もしくは、モデル面の法線も得られる。対応付けられた幾何特徴の2点間のベクトルに対して、どちらかの法線を正規化したものとの内積を計算すると、面の法線方向による残差が算出できる。また、エッジを観測点とする場合には、観測座標系における2次元の距離を計算すれば良い。さらにエッジの場合はSobelフィルタ等の方向検出フィルタを用いることで、画像から勾配を検出することができる。モデルの位置とエッジの位置のベクトルに対して、正規化した勾配方向のベクトルとの内積を計算することで、勾配方向を考慮した残差を算出することができる。なお、残差計算式はユーザが設定できるものとする。
【００３２】
観測点・モデル点残差出力部210は、観測点・モデル点残差計算部200による処理結果として、残差計算値、そこで利用されたモデル点、変換されたモデル点の座標、観測点テーブルの座標値、等を出力する。これらは、モデルフィッティングにおいて位置姿勢推定のパラメータを計算する際のヤコビ行列の値として利用され、残差が小さくなるように位置姿勢のパラメータが推定される。
【００３３】
以上説明した図1に示す情報処理装置の構成は、CPU、メモリ、それらをつなぐバス、入出力装置、等の一般的なプログラム動作のプラットフォーム上に、プログラムとして実行することが可能である。そのため本実施形態を、位置姿勢推定の処理プログラムの一部として動作させることができる。また、装置として入出力のインターフェースを介して観測点、モデル点の情報を入力して、それらの対応付けと残差を出力するようなハードウェア構成として機能させても良い。また、インターフェースにネットワークを介してデータを送信して、別の場所にある装置で実行し、その結果を受信して利用するようにしても構わない。プログラムとしてメディアからのコピーや、ネットワークからダウンロードできるようにして、アプリケーションの一部として利用できるようになっていると、様々な機器で動作させることができるため、有用である。
【００３４】
●インデックスマップ生成処理
以下、観測点入力部110からインデックスマップ保持部170までの構成において行われる、インデックスマップ生成処理の流れについて、図2のフローチャートおよび図3の概念図を用いて説明する。
【００３５】
まずS100で、処理開始時にインデックスマップ保持部170に確保されたインデックスマップを初期化し、次にS110で、観測点テーブル保持部140に確保された観測点テーブルを初期化する。
【００３６】
次にS120で、観測点入力部110から観測点に関する情報を入力する。観測対象が距離画像である場合には、観測点情報はX,Y,Zの3次元の値となる。また、観測点が画像に含まれるエッジである場合には、観測点情報はX,Yの2次元の値となる。
【００３７】
次にS130で、S120で入力された観測点の座標値に対し、観測点テーブルを参照するためのインデックスを付与した後に、該座標値を観測点テーブル保持部140に追加格納する。ここで図3に、S120で得られた距離画像の座標(X,Y,Z)が、観測点テーブルのインデックス値"25"番に格納された例を示す。なお、観測点がメモリ配列を利用して表現されている場合には、観測座標を示すメモリ構造体と、そのインデックスを保持するレジスタを用いて、一連のメモリ空間に該メモリ構造体のサイズ分だけスライドさせて値を書き込めば良い。
【００３８】
そしてS140では、S130で得られた観測点の座標値を、インデックスマップの座標系へ変換する。観測点として画像のエッジを利用する場合には、観測点は撮像画像であり、インデックスマップも同じサイズの画像としておくことで、観測点の座標をそのまま利用することができる。一方、観測点として距離画像を利用する場合には、観測点の3次元情報から2次元情報への射影変換を行う必要がある。したがって、インデックスマップ座標系への変換係数とオフセット値を予め設定しておき、該値に基づいて変換を行うことができる。例えば、インデックスマップの画像の中心を(cx,cy)、変換係数をfとすると、距離画像における任意の点(X,Y,Z)は、以下の(1),(2)式によってインデックスマップ座標系(u,v)へ変換される。なお、座標値は整数値として参照されるため、(1),(2)式では少数点以下を切り捨てるfloor()演算を行うとする。
【００３９】
u=floor(f(X/Z)+cx)      ・・・(1)
v=floor(f(Y/Z)+cy)      ・・・(2)
次にS150では、インデックスマップにおいて、S140で変換された座標値が示す画素の値として、S130で観測点座標に付与したインデックス値を書き込む。したがって、インデックスマップの各画素の情報量としては、観測点テーブルの配列の上限を格納可能なビット数が必要となる。一般に利用される画像サイズであれば、各画素は符号無し16ビットのサイズがあれば良い。このビット数は、処理対象となる画像サイズや解像度に応じて切り替えるものとする。
【００４０】
ここで図3においては、S140で観測点の座標(X,Y,Z)が、上記式(1),(2)によりインデックスマップの座標(u,v)に変換されている。そしてS150で、該観測点座標(X,Y,Z)が対応する観測点テーブルのインデックス値"25"番が、インデックスマップの座標(u,v)に書き込まれる様子を示す。
【００４１】
そしてS160で観測点の座標入力が終了したか否かを確認し、未だ終了していない場合にはS120に戻って上記処理を繰り返すが、終了していればS170へ進む。
【００４２】
S170では、インデックスマップに対する参照領域処理を行う。すなわちインデックスマップにおいて、S150で書き込まれたインデックス値を周辺画素に拡張するように、参照領域を設定する。具体的には、注目画素にインデックスマップの値が書き込まれている場合には、その周辺画素にもそのインデックス値を書き込む処理を反復すれば良い。この画像処理としては、一般的な膨張処理(Dilate)を適用することができる。また、ボロノイ図の生成処理を適用することで周辺画素にインデックスの領域を設定することも可能である。ここでボロノイ図は、注目画素位置で最近傍の画素の値を保持するように作られるものであるため、本実施形態のインデックスマップとして用いることができる。なお、この参照画像処理としては、S150で書き込まれたインデックス値を、隣接する、値が未設定である画素に伝播させるように領域を拡張することができれば、どのような方法を用いても構わない。ここでは、インデックスの数値を周囲画素に伝播させることのみを行えばよく、上記特許文献1に記載されたディスタンスマップのように元の参照点からの距離や勾配を計算する必要がないため、インデックスマップを高速に作成することができる。
【００４３】
そしてS180ではインデックスマップ保持部170において、S170での処理結果であるインデックスマップの画像を、参照可能なように設定して保持する。
【００４４】
ここで図3によれば、S160までの処理によって、例えばインデックス値"24"番と"25"番が書き込まれたインデックスマップに対し、S170で参照領域処理を施すことで各インデックス値の領域が拡張される。そして、最終的に生成されたインデックスマップがS180で保持される。
【００４５】
ここで従来のモデルフィッティング処理においては、参照する注目画素の周辺を部分的に探索する処理が行われていたため、メモリへの複数回のランダムアクセスによる処理時間が必要となり、処理時間が長くなるという課題があった。対して本実施形態では図3に示すように、インデックスマップにおける注目画素に対し、その周辺画素についても注目画素と同じインデックス値が保持される。したがって、注目画素を参照するのみによって最近傍対応付けの演算を行うことが可能となり、すなわちメモリへのアクセス回数が低減され、処理時間を短縮することが可能となる。
【００４６】
なおここでは説明の簡便のため、図2のフローチャートに示す各ステップを逐次処理として説明したが、S120〜S160の入力値を登録する処理や、S170の参照領域処理をデータごとに並列に行うことで、処理時間をさらに短縮することができる。
【００４７】
なお、ここではインデックスマップが画像形式の配列によって構成される例を示したが、注目領域にのみインデックスマップを保持するようにして、各画素要素の情報量が少なくなるようにしても良い。
【００４８】
●残差計算処理
以下、モデル点座標パラメータ入力部120から観測点・モデル点残差計算部200までの構成において行われる、モデル点と観測点を対応付けてその残差を算出する処理について、図4のフローチャートを用いて詳細に説明する。
【００４９】
まずS200でモデル点座標パラメータ入力部120が、インデックスマップ座標系への変換パラメータとして、モデル点をモデルビュー座標系へ変換するモデルビュー変換行列が入力される。そしてS210でモデル点入力部130が、対応付けの対象となるモデル表面の点(モデル点)として、モデル座標系の座標値(X,Y,Z)を入力する。
【００５０】
そしてS220でモデル点座標変換部180が、モデル点の座標値(X,Y,Z)を、インデックスマップ座標系に変換する。観測点が距離画像の点である場合には、まず以下の(3)式に示すように、モデル点の座標値(X,Y,Z)を、モデルビュー変換行列を用いてモデルビュー座標値(X',Y',Z')に変換する。(3)式において、Rが回転成分を変換する3ｘ3行列、tが並進成分を変換する3ｘ1行列であり、S200で入力された変換パラメータである。
【００５１】
2014120068.tif 000003
【００５２】
そして、(3)式により観測座標系に変換されたモデルビュー座標値(X',Y',Z')を、上記(1),(2)式のパラメータを用いることで、以下の(4),(5)式のように、インデックスマップ座標系の座標(u',v')へ変換する。
【００５３】
u'=floor(f(X'/Z')+cx)      ・・・(4)
v'=floor(f(Y'/Z')+cy)      ・・・(5)
なお、観測点がエッジを示す場合にも、モデル点は対象物体表面の3次元の点として保持されているものとし、したがって上記(3)〜(5)式を適用することができる。
【００５４】
そしてS230でインデックスマップ参照部190が、インデックスマップ保持部170に保持されたインデックスマップにおける、座標(u',v')の画素値を参照する。ここで、インデックスマップの各画素には、観測点テーブル保持部140に保持された各観測点テーブルにおけるインデックス値が書き込まれているため、すなわちS230では観測点テーブルのインデックス値が取得される。これにより、S210で入力されたモデル点に対し、観測点テーブルに登録された観測点の実質的な対応付けがなされる。
【００５５】
そしてS240で観測点・モデル点残差計算部200が、S230で取得した観測点テーブルのインデックス値から、対応する観測点を特定する。なお、インデックスマップの画素に空のインデックス値が設定されていた場合には、対応する観測点が無かったものとして処理を行う。
【００５６】
そしてS250で観測点・モデル点残差計算部200が、対応付けられた観測点とモデル点の残差を計算する。観測点が3次元の座標である場合には、上記(3)式で算出された観測座標系のモデルビュー座標値(X',Y',Z')と観測点テーブルに設定された観測点座標値(X,Y,Z)とにおけるユークリッド距離を計算しても良いし、各成分の差を計算しても良い。また観測点が2次元の座標である場合には、上記(4),(5)式で算出される(u',v')の値と、観測点テーブルの(X,Y)のユークリッド距離を計算しても良いし、各成分の差を計算しても良い。ここで、残差の計算方法は設定によって切り替え可能であるとする。そのため、残差計算については別の処理部で行うものとして、対応付けられた座標値やインデックス値を該別の処理部へ出力するようにしても良い。その場合、本実施形態では残差計算を行わない設定であっても構わない。
【００５７】
本実施形態のインデックスマップには、各画素と観測点テーブルとの対応関係のみが保持されている。したがって、対応付けられたモデル点と観測点との距離を示す残差計算はその都度行われるため、画素参照による離散化誤差の計算精度劣化が発生しない。そのため、上記特許文献1に記載されたディスタンスマップのように事前に計算された距離情報を参照する手法に対し、より高精度な残差計算が可能となる。また、インデックスマップの生成処理も単純であるため、kd木(kd tree)のようなデータ構造を生成する処理と比較して、データ保持にかかる容量も処理時間も小さくて済むという点で優れている。
【００５８】
以上説明したように本実施形態によれば、インデックスマップを用いて観測点とモデル点の対応付けを行うことで、高速なモデルフィッティング処理が可能となる。また、幾何特徴を示す観測点として、距離画像に含まれる距離点や、画像に含まれるエッジ等を利用することができるため、適用範囲が広い。
【００５９】
＜第2実施形態＞
以下、本発明に係る第2実施形態について説明する。第2実施形態においては、上述した第1実施形態に示したモデルフィッティング機能を含む位置姿勢推定装置を示す。
【００６０】
図5に、第2実施形態における位置姿勢推定装置のブロック構成例を示す。同図に示すように位置姿勢推定装置は、計測対象物体10に対して、パターンを投影するプロジェクタ300と、その反射像を撮影するカメラ310により、計測対象物体10の位置姿勢のパラメータを計測することを特徴とする。
【００６１】
プロジェクタ300はパターンを投影する機能を有し、一般には光源とパターンを構成するスクリーンで構成され、コンピュータからの映像パターンを照明することができる。カメラ310は投影されたパターンを撮影するように配置され、プロジェクタ300の投影タイミングに合わせた撮影(同期撮影)が行われるように、対象撮影部330で撮影タイミングが制御される。ここでプロジェクタ300における投影パターンとしては、グレイコードを用いた空間コード化法や、位相シフト法などの撮像画像とプロジェクターの投影画像の位置が対応できるようなパターンが想定される。このパターンにより対応付けられた配置関係から、三角測量法により距離の算出が可能とする。
【００６２】
距離画像生成部340は、プロジェクタ300からパターンが投影された画像を利用して、プロジェクタ300の投影座標と、その撮影画像の座標との対応関係から、各画素の距離値を算出して距離画像を生成する。この距離画像における距離点が第1の実施形態における観測点(第1の観測点)として、後述する情報処理装置101に入力される(第1の観測点入力)。なお、距離値の計算にはプロジェクタ300とカメラ310の両方におけるカメラパラメータが必要であるが、これらのパラメータは事前に校正が施された後、設定済みであるとする。
【００６３】
画像エッジ検出部350は、プロジェクタ300がパターンを投影しない状態での計測対象物体10の撮影画像から、ルーフエッジ、ジャンプエッジなどの幾何特徴を検出するための画像処理を行う。エッジの検出には、SobelフィルタやCannyフィルタ等の画像検出フィルタを利用すれば良い。すなわち、該フィルタとして設定した閾値以上の強度を持つエッジを検出し、該エッジの座標値が第1実施形態における観測点(第2の観測点)として、後述する情報処理装置102へ入力する(第2の観測点入力)。
【００６４】
パラメータ初期値360は、計測対象物体10の形状モデルから設定されたサンプル点の座標値の配列をパラメータ初期値として設定したものであり、位置姿勢のパラメータ計算における初期位置姿勢の値として利用される。サンプル点としては、コンピュータグラフィックのレンダリング機能を利用して、事前に設定されている位置姿勢での可視の面やエッジの判定を行い、その値を格納しておいたものを利用すれば良い。
【００６５】
情報処理装置101は、上述した第1実施形態における情報処理部100と同様の構成からなり、距離画像生成部340で生成された距離画像を入力し、その距離点を観測点(第1の観測点)として対応付け残差計算を行う。モデル点(距離)370は、距離画像に対応する、モデル形状の表面において観測されるモデル点の3次元座標配列(第1のモデル点)である。
【００６６】
情報処理装置102も第1実施形態における情報処理部100と同様の構成からなり、画像エッジ検出部350で検出されたエッジ点を観測点(第2の観測点)として対応付け残差計算を行う。モデル点(エッジ)380は、モデル形状におけるルーフエッジ、ジャンプエッジ等の輝度勾配の変化点として観測されるエッジ点を、モデル座標系の3次元座標系で表現した座標値の配列(第2のモデル点)である。
【００６７】
パラメータ更新部390は、各情報処理装置101,102による観測点とモデル点の対応付けと残差計算の結果を用いて、位置姿勢のパラメータを推定するためのヤコビ行列を求め、これを残差行列として最小二乗法を計算する連立方程式に代入する。これによって算出された値を位置姿勢パラメータの更新値とする。すなわち、観測点とモデル点との残差が小さくなるように、位置姿勢のパラメータが推定される。
【００６８】
ここでは、エッジ点と距離点の両方を観測点として利用しているため、情報処理装置101に応じた第1のパラメータと、情報処理装置102に応じた第2のパラメータをそれぞれ推定し、該第1および第2のパラメータから位置姿勢パラメータを推定できる。パラメータの更新時には、それぞれによる推定パラメータのうち評価が良い方、すなわち残差がより小さい方を利用すれば良い。さらに、第1の観測点と第2の観測点は同一の計測対象物体を観察していることから、それぞれの観測点とモデルとの誤差を最尤推定して位置姿勢のパラメータを推定することで、より良いパラメータの推定が可能となる。(立野ら、"ビンピッキングのための距離・濃淡画像を最ゆうに統合する高精度高安定なモデルフィッティング手法"、電子情報通信学会論文誌D、Vol.J94-D、No.8、pp.1410-1422、2011、参照)。なお、ヤコビ行列の算出はVisual Servoなどの研究により周知となっている式を利用すれば良い。また、パラメータの算出にはGauss-Newton法による演算を利用すれば良い。すなわちパラメータ更新部390では、観測点とモデル点の対応付けと残差計算結果に応じた位置姿勢パラメータの更新ができれば良く、その更新方法について特定するものではない。
【００６９】
パラメータ更新部390では、更新の割合が所定の条件を満たす場合や、所定回数の繰り返しが行われた等の収束判定条件に基づいて位置姿勢推定パラメータ値の推定結果を確定し、該推定結果を推定パラメータ出力部400から出力する。
【００７０】
なお、第2実施形態では情報処理装置101,102によって、距離点とエッジ点のそれぞれに基づく残差演算を行う例を示したが、これらを統合し、1台の装置において両方の残差演算を行うように構成しても良い。
【００７１】
以上説明したように第2実施形態によれば、上述した第1実施形態で説明したモデルフィッティングを行う情報処理装置において利用される位置姿勢のパラメータを推定することで、情報処理装置における処理負荷を軽減し、処理速度の向上が望める。
【００７２】
＜第3実施形態＞
以下、本発明に係る第3実施形態について説明する。上述した第1および第2実施形態によって実現される位置姿勢計測は高速処理を可能とすることから、ロボットとビジョンを組み合わせた作業システムでのタクトタイムが問題となる場合に利用することが考えられる。第3実施形態の具体的な構成としては、第2実施形態で示した図5の構成に対し、ロボットと、該ロボットの位置姿勢制御を行うロボットコントローラを追加すればよい。これにより、推定パラメータ出力部400から出力される計測対象物体10の位置姿勢パラメータをロボットコントローラに入力し、ロボット座標系に変換することで、ロボットの手先を計測対象物体10に向けて作業することが可能となる。
【００７３】
以上説明したように第3実施形態によれば、第1および第2実施形態による位置姿勢計測処理において観測点とモデル点の対応付けの処理が高速に行われるため、撮影から推定までの処理時間が短縮される。したがって、ロボットシステムにおける作業全体のタクトタイムを短縮することが可能となる。
【００７４】
第3実施形態からも分かるように、本発明は、精度を向上するために大量の観測値を利用して観測点とモデル点の対応付けを行う必要のある処理について、高速化が可能となる。したがって例えば、工業製品の組み立てにおける部品の自動供給や自動組み付け作業において、部品の位置姿勢計測を高速に処理することができる。
【００７５】
＜他の実施形態＞
本発明は、上述した実施形態の機能(例えば、上記の各部の処理を各工程に対応させたフローチャートにより示される処理)を実現するソフトウェアのプログラムコードを記録した記憶媒体を、システム或いは装置に供給することによっても実現できる。この場合、そのシステム或いは装置のコンピュータ(又はCPUやMPU)が、コンピュータが読み取り可能に記憶媒体に格納されたプログラムコードを読み出し実行することにより、上述した実施形態の機能を実現する。
【図１】
2014120068.tif 000004
【図２】
2014120068.tif 000005
【図３】
2014120068.tif 000006
【図４】
2014120068.tif 000007
【図５】
2014120068.tif 000008