位置姿勢計測装置及び位置姿勢計測方法
【課題】画像と3次元形状モデルの誤対応を減らし、高速でロバストな物体の位置及び姿勢の計測を実現する。
【解決手段】物体の3次元形状モデルを保存する保存部110と、物体の濃淡画像を入力する濃淡画像入力部130と、物体の距離画像を入力する距離画像入力部150と、位置姿勢計測装置100に対する物体の概略の位置及び姿勢を入力する概略位置姿勢入力部120と、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出するとともに、第1の位置及び姿勢を用いて算出される3次元形状モデルの画像面上での投影位置をもとに、距離画像から得られる3次元点群または当該3次元点群及び濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出部160を備える。
【解決手段】物体の3次元形状モデルを保存する保存部110と、物体の濃淡画像を入力する濃淡画像入力部130と、物体の距離画像を入力する距離画像入力部150と、位置姿勢計測装置100に対する物体の概略の位置及び姿勢を入力する概略位置姿勢入力部120と、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出するとともに、第1の位置及び姿勢を用いて算出される3次元形状モデルの画像面上での投影位置をもとに、距離画像から得られる3次元点群または当該3次元点群及び濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出部160を備える。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置及び位置姿勢計測方法に関する。
【背景技術】
【0002】
近年、ロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このような目的には主にアーム型のロボットが利用され、ロボットはアームの先端に装着されたハンドなどのエンドエフェクタによって部品を把持し組立を行う。ロボットが適切に部品を把持するためには、部品とロボット(エンドエフェクタ)との間の相対的な位置及び姿勢を高精度に計測する必要がある。また、ロボットによる組立を実場面に適用するためには、部品の位置及び姿勢の計測は、高精度に加えて高速である必要がある。このような位置及び姿勢の計測技術は、ロボットによる工業製品の組立だけでなく、ロボットが自律移動するための自己位置推定や、実測データからの3次元モデルデータ作成など様々な場面で必要とされる。
【0003】
工業製品の生産現場における部品の位置及び姿勢の計測には、カメラから得られる濃淡(カラー)画像や非接触式の距離センサから得られる距離画像が主に利用される。物体の個体ごとの位置及び姿勢の計測は、観測データ(濃淡画像・距離画像)に対する物体の3次元形状モデルの当てはめによって行われるのが一般的である。
【0004】
下記の非特許文献1では、物体の3次元形状モデルを線分の集合であるワイヤフレームモデルで表し、線分の投影像を濃淡画像上のエッジに当てはめることで、物体の位置及び姿勢を計測する方法が開示されている。この濃淡画像上のエッジを利用する方法では、物体の輪郭部分や面の切り替わり部分など画像上でエッジとなる部分の投影像と画像上のエッジとの「2次元画像平面上での距離」が最小になるように物体の位置及び姿勢を計測する。そのため、2次元画像平面上での距離を大きく変化させる位置及び姿勢の成分の計測精度は高いが、それ以外の成分の計測精度は必ずしも高くない。即ち、カメラの光軸に垂直な方向の位置成分や光軸まわりの姿勢成分の計測精度が高いのに対し、カメラの光軸方向の位置成分(奥行き)などの計測精度は低い。
【0005】
下記の非特許文献2では、距離画像から得られる物体表面上の3次元点群データに物体の3次元形状モデル(ポリゴンモデル)を当てはめることで、物体の位置及び姿勢を計測する方法が開示されている。この非特許文献2で開示されるような距離画像を利用する方法は、点群とモデルの「3次元空間内での」距離を直接最小化する方法であるため、基本的には位置及び姿勢の計測精度は高い。しかしながら、下記の非特許文献3で開示されるような能動ステレオ法では、物体の輪郭部分の距離を安定に計測できないことが多いため、物体の形状や観察方向によっては輪郭部分が正確に一致するような位置及び姿勢の計測ができない。
【0006】
以上、述べた濃淡画像を利用する方法と距離画像を利用する方法の特性を鑑みると、位置及び姿勢の推定においては、濃淡画像から得られる情報と距離画像から得られる情報の間には相補的な関係があると言える。そのため、濃淡画像と距離画像の双方に対して同時に3次元形状モデルが当てはまるように位置及び姿勢の計測を行うことで、位置及び姿勢の計測精度を向上させることができる。下記の非特許文献1及び下記の非特許文献2で開示される方法では、それぞれモデル投影像とエッジの2次元画像面上での誤差の二乗和、モデルと点群の3次元空間中における誤差の二乗和を評価関数として最小化する。2次元画像面上の距離と3次元空間中の誤差はスケールが異なるため、2つの評価関数の和が最小となるように位置及び姿勢を推定するような単純な方法では、どちらか一方の影響が大きくなるという問題がある。従来では、異なるスケールの誤差を共通の尺度によって統一的に評価することで、濃淡画像と距離画像の情報を相補的に利用する位置及び姿勢の計測方法が提案されている。ここでは、共通の尺度の1つとして、2次元画像面上の誤差及び3次元空間中の誤差を、夫々が従う確率分布における発生確率(尤度)によって表し、尤度の積を最大化することで高精度な位置及び姿勢の計測を行っている。
【先行技術文献】
【非特許文献】
【0007】
【非特許文献1】T. Drummond and R. Cipolla, "Real-time visual tracking of complex structures," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002.
【非特許文献2】D. A. Simon, M. Hebert, and T. Kanade, "Real-time 3-D pose estimation using a high-speed range sensor," Proc. 1994 IEEE International Conference on Robotics and Automation (ICRA'94), pp.2235-2241, 1994.
【非特許文献3】佐藤,井口,"空間コードによる距離画像入力",電子情報通信学会論文誌,vol.J68-D,no.3,pp.369-375,1985.
【非特許文献4】Z. Zhang, "A flexible new technique for camera calibration", IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.22, No.11, pages 1330-1334, 2000.
【非特許文献5】大石,中澤,池内,"インデックス画像を用いた複数距離画像の高速同時位置合せ",vol.J89-D,no.3,pp.513-521,2006.
【非特許文献6】Y. Liu, T. S. Huang, and O. D. Faugeras, "Determination of camera location from 2-D to 3-D line and point correspondences," Proc. CVPR'88, pp.82-88, 1988.
【非特許文献7】A. Johnson and M. Hebert, "Using spin images for efficient object recognition in cluttered 3D scenes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.21, no.5, pp.433-449, 1999.
【非特許文献8】C. Harris and M. Stephens, "A combined corner and edge detector" Proc. the 4th Alvey Vision Conference, pp.147-151, 1988.
【非特許文献9】M. Ulrich, C. Wiedemann, and C. Steger, "CAD-based recognition of 3D objects in monocular images" Proc. 2009 IEEE International Conference on Robotics and Automation (ICRA'09), pp.1191-1198, 2009.
【発明の概要】
【発明が解決しようとする課題】
【0008】
工業製品の生産過程では時間に対する制約が厳しいため、その一工程である部品の位置及び姿勢の計測はできる限り高速に行われる必要がある。また、ロボットの自己位置推定など、位置及び姿勢の計測を高速に処理しなければならない場面は他にも多い。前述した濃淡画像・距離画像に対する3次元形状モデルの当てはめによる位置及び姿勢の推定は、(1)モデルと画像の対応付け、(2)対応付け結果に基づく位置及び姿勢の算出、の2つのステップからなる。高精度な位置及び姿勢を推定するため、これらのステップは複数回繰り返されるのが一般的である。これら2つのステップのうち、モデルと画像の対応付け処理にかかる計算時間が問題になることが多い。
【0009】
上記非特許文献1に開示される方法では、位置及び姿勢の初期値に基づいて3次元形状モデル中の各線分を画像上に投影し、画像上の投影像の近傍で対応するエッジを探索することにより、モデルと画像の対応付け処理を高速化している。一方、距離画像と3次元形状モデルの対応付けでは、3次元空間において最も近傍にある点を探索する必要がある。
【0010】
上記非特許文献2では、kd−treeを用いて最近傍点の探索を行っている。しかしこの方法では、オーダーがO(NlogM)の計算が必要になる(Nはデータ側の点数、Mはモデル側の点数)ため、各画素が計測点となる距離画像のように計測点数が非常に多いデータを利用する場合に時間がかかる。これに対し、従来では、上記非特許文献1の方法と同様に、画像上にモデルを投影することにより対応付けを高速化する方法が提案されている。具体的には、まず位置及び姿勢の初期値に基づいて3次元形状モデル内の幾何特徴の画像への投影計算を行い、幾何特徴の画像上での位置を算出する。次に、この幾何特徴の画像上での位置をもとに、幾何特徴と距離画像の画素とを対応付けすることにより、高速な対応付けを実現している。
【0011】
しかしながら、前述の3次元形状モデルの画像上への投影に基づいた画像と3次元形状モデルの対応付けでは、3次元形状モデルの投影像と実際の物体の像とが十分にオーバーラップしていることが前提になっている。そのため、投影に用いる位置及び姿勢の初期値と実際の位置及び姿勢とのずれが大きい場合には誤対応が多く生じ、後段の位置及び姿勢の算出に失敗する。濃淡画像のエッジの対応付けでは明示的な特徴(エッジ)を用いているため、初期値のずれに対して比較的ロバストである。
【0012】
一方、距離画像の対応付けでは、濃淡画像上のエッジのような明示的な特徴を用いていないため、初期値のずれに対してロバスト性が低く、位置及び姿勢の算出に失敗する要因となる。逆に初期値のずれが小さく距離画像とモデルが正しく対応付けできる場合には、距離画像の利用によって高精度な位置及び姿勢の計測が可能になる。このように、初期値のずれに対するロバスト性は、濃淡画像と距離画像で夫々特性が異なる。言い換えると、従来においては、この特性を考慮することまでは行っておらず、良い特性を持つ計測データを段階的・選択的に使うことによる初期値ずれに対するロバスト化に改善の余地があった。
【0013】
本発明は、このような問題点に鑑みてなされたものであり、初期値のずれ具合の段階に応じてよい特性を持つ計測データを利用することにより、画像と3次元形状モデルの誤対応を減らし、高速でロバストな物体の位置及び姿勢の計測を実現することを目的とする。
【課題を解決するための手段】
【0014】
本発明の位置姿勢計測装置は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の距離画像を入力する距離画像入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有する。
本発明の位置姿勢計測装置における他の態様は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の表面上における点群の3次元座標を入力する3次元データ入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力手段から得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有する。
また、本発明は、上述した位置姿勢計測装置による位置姿勢計測方法を含む。
【発明の効果】
【0015】
本発明によれば、濃淡画像と距離画像を段階的・選択的に用いることにより、位置及び姿勢の初期値と実際の位置及び姿勢とのずれに対してロバストな物体の位置及び姿勢の計測が可能になる。
【図面の簡単な説明】
【0016】
【図1】本発明の第1の実施形態に係る位置姿勢計測装置を含む位置姿勢計測システムの概略構成の一例を示すブロック図である。
【図2】本発明の第1の実施形態を示し、3次元形状モデルを説明する模式図である。
【図3】本発明の第1の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【図4】図3のステップS1030における位置姿勢計測(位置姿勢算出)の処理手順の一例を示すフローチャートである。
【図5】本発明の第1の実施形態を示し、エッジ検出を説明する模式図である。
【図6】本発明の第1の実施形態を示し、線分の投影像と検出されたエッジとの関係を説明する模式図である。
【図7】図3のステップS1040における位置姿勢算出の処理手順の一例を示すフローチャートである。
【図8】本発明の第1の実施形態を示し、図7のステップS1220における距離画像と3次元形状モデルとの対応付け処理を説明する模式図である。
【図9】本発明の第2の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【図10】本発明の第3の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【図11】図10のステップS3040における位置姿勢算出の処理手順の一例を示すフローチャートである。
【発明を実施するための形態】
【0017】
以下に、図面を参照しながら、本発明を実施するための形態(実施形態)について説明する。
【0018】
(第1の実施形態)
第1の実施形態では、まず濃淡画像の情報を用いて物体の位置及び姿勢を推定した後、濃淡画像上の特徴と距離画像に対して同時にモデルが当てはまるように位置及び姿勢の推定を行う。本実施形態では、位置及び姿勢の初期値は与えられるものとする。実際の位置及び姿勢と初期値とのずれが大きい場合、モデルを投影することによる距離画像(3次元点群)と3次元形状モデルの対応付けは、誤対応が頻発する。第1の実施形態では、距離画像の対応付けを行う前に、画像特徴を利用した位置及び姿勢の推定を行ってモデルの投影像と実際の物体像との画像上のオーバーラップ領域をできるだけ広げ、距離画像の対応付けにおける誤対応を削減する。
【0019】
図1は、本発明の第1の実施形態に係る位置姿勢計測装置を含む位置姿勢計測システムの概略構成の一例を示すブロック図である。
図1に示すように、位置姿勢計測システムは、位置姿勢計測装置100、濃淡画像撮像部200、及び、距離計測部300を有して構成されている。
【0020】
また、図1に示すように、位置姿勢計測装置100は、3次元形状モデル保存部110、概略位置姿勢入力部120、濃淡画像入力部130、画像特徴検出部140、距離画像入力部150、及び、位置姿勢算出部160を有して構成されている。また、位置姿勢計測装置100には、濃淡画像撮像部200及び距離計測部300が接続されている。
【0021】
濃淡画像撮像部200は、通常の濃淡画像を撮影するカメラである。撮影される画像は、濃淡画像であってもよいしカラー画像であってもよい。濃淡画像撮像部200が撮影する画像は、濃淡画像入力部130を介して位置姿勢計測装置100に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば、上記非特許文献4に係るZhangの方法によって事前にキャリブレーションしておく。
【0022】
距離計測部300は、計測対象である物体表面上の点までの距離を計測する。本実施形態では、距離計測部300として距離画像を出力する距離センサを用いる。距離画像は、各画素が距離計測部300からの距離の情報を持つ画像である。本実施形態では、各画素には距離の情報ではなく、距離をもとに算出される計測装置基準の座標系における3次元座標が保存されているものとする。即ち、距離画像からは、3次元点群データが得られる。距離値から3次元点群への変換は、距離画像の各画素について、画素位置に対応する視線ベクトルに距離値を乗算することにより行う。距離センサとして、例えば、液晶プロジェクタから物体に照射した2次元のパターン光をカメラで撮影し、三角測量により距離を計測するアクティブ方式のものを利用する。なお、距離センサはこれに限るものではなく、光切断法やTime-of-flight方式など他のアクティブ式のものや、ステレオカメラによるパッシブ式など、距離画像を出力可能であれば如何なる距離センサであってもよい。本実施形態では、距離センサ用のカメラと濃淡画像撮像部200のカメラは同一のものを用いる。即ち、同一視点における濃淡画像と距離画像が取得できると仮定する。距離センサ用のカメラと濃淡画像撮像部200のカメラが異なる場合には、両者の相対的な位置及び姿勢を事前にキャリブレーションしておく。例えば3次元形状が既知であるキャリブレーション物体を撮影し、濃淡画像に基づいた位置及び姿勢と、距離画像に基づいた物体の位置及び姿勢から相対的な位置及び姿勢を求める。距離計測部300が計測した距離画像は、距離画像入力部150を介して位置姿勢計測装置100に入力される。
【0023】
次に、位置姿勢計測装置100を構成する各部について説明する。
【0024】
3次元形状モデル保存部110には、位置及び姿勢を計測する対象である物体の3次元形状モデルを保存する。本実施形態では、物体を線分と面から構成される3次元形状モデルとして記述する。
【0025】
図2は、本発明の第1の実施形態を示し、3次元形状モデルを説明する模式図である。3次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図2(a)に示すように、計測対象物体10−1の3次元形状モデルは、点P1〜点P14の14点から構成される。また、図2(b)に示すように、計測対象物体10−2の3次元形状モデルは、線分L1〜L16により構成されている。また、図2(c)に示すように、点P1〜点P14は3次元座標値によって表される。また、図2(d)に示すように、線分L1〜L16は、線分を構成する点のIDによって表される。さらに、3次元形状モデルは、面の情報を保持する。各面は、各面を構成する点のIDによって表される。図2に示す3次元形状モデルでは、直方体を構成する6つの面の情報が記憶されている。3次元形状モデルは、位置姿勢算出部160において物体の位置及び姿勢を算出する際に用いられる。
【0026】
概略位置姿勢入力部120は、位置姿勢計測装置100に対する物体の位置及び姿勢の概略値を入力する。位置姿勢計測装置100には、位置及び姿勢計測の基準となる3次元の座標系(基準座標系)が規定されているものとする。位置姿勢計測装置100に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、濃淡画像撮像部200であるカメラのカメラ中心を原点とし、画像の水平方向をx軸、垂直方向をy軸、カメラの光軸をz軸とした座標系を基準座標系とする。本実施形態では、位置姿勢計測装置100は、時間軸方向に連続して計測を行うものとして、前回(前時刻)の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果をもとに位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測したものでもよい。また、物体が置かれているおおよその位置や姿勢が予めわかっている場合には、その値を概略値として用いる。この概略位置姿勢入力部120は、濃淡画像または距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される物体の位置及び姿勢を概略の位置及び姿勢として入力する。
【0027】
濃淡画像入力部130は、濃淡画像撮像部200で撮影された濃淡画像を位置姿勢計測装置100に入力する処理を行う。
【0028】
画像特徴検出部140は、濃淡画像入力部130から入力された濃淡画像から画像特徴を検出する。本実施形態では、画像特徴としてエッジを検出する。具体的には、概略位置姿勢入力部120から入力される物体の位置及び姿勢の概略値に基づいて、3次元形状モデルの各線分を画像上に投影し、投影された線分の近傍で対応するエッジ探索を行う。
【0029】
距離画像入力部150は、距離計測部300で得られた距離画像を位置姿勢計測装置100に入力する処理を行う。
【0030】
位置姿勢算出部160は、画像特徴検出部140で検出された画像特徴、及び距離画像入力部150によって入力された3次元点群と、3次元形状モデル保存部110に保存される3次元形状モデルをもとに、物体の位置及び姿勢を計測する。
【0031】
図3は、本発明の第1の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【0032】
<ステップS1010>
概略位置姿勢入力部120は、位置姿勢計測装置100(カメラ)に対する物体の位置及び姿勢の概略値を位置姿勢計測装置100に入力する。前述したように、本実施形態では、前回計測された位置及び姿勢を概略値として用いる。
【0033】
<ステップS1020>
濃淡画像入力部130及び距離画像入力部150は、物体の位置及び姿勢を算出するための計測データを取得する。具体的には、濃淡画像入力部130及び距離画像入力部150は、計測対象物体の濃淡画像と距離画像を取得する。前述したように、距離画像の各画素には、基準座標系における点の3次元座標が記録されている。濃淡画像撮像部200によって撮影された濃淡画像は、濃淡画像入力部130を介して位置姿勢計測装置100に入力される。また、距離計測部300から出力された距離画像は、距離画像入力部150を介して位置姿勢計測装置100に入力される。
【0034】
<ステップS1030>
画像特徴検出部140及び位置姿勢算出部160は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測(算出)を行う。ステップS1030では、濃淡画像上の特徴であるエッジに3次元形状モデル中の線分の投影像が当てはまるように、計測対象物体の位置及び姿勢を算出する。
【0035】
図4は、図3のステップS1030における位置姿勢計測(位置姿勢算出)の処理手順の一例を示すフローチャートである。ここでは、画像上で検出されたエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。以下の説明では、物体の位置及び姿勢を6次元ベクトルsで表す。sは、計測対象物体の位置を表す3つの要素と、姿勢を表す3つの要素からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す3次元ベクトルなどによって表現される。以下、図3のステップS1030における位置姿勢算出の処理手順について順に説明する。
【0036】
≪ステップS1110≫
まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS1010で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【0037】
≪ステップS1120≫
続いて、画像特徴検出部140は、ステップS1020において入力された濃淡画像上において画像特徴の検出・画像特徴と3次元形状モデルとの対応付けを行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。本実施形態では、上記非特許文献1で開示される方法によりエッジ検出を行う。この非特許文献1で開示されるエッジ検出方法は、トップダウン型の方法であり、モデル側から対応するエッジを探索する方法であるため、検出と対応付けが一度に行われる。
【0038】
図5は、本発明の第1の実施形態を示し、エッジ検出を説明する模式図である。
まず、ここでは、ステップS1110において入力された計測対象物体の位置及び姿勢の概略値と校正済みのカメラの内部パラメータを用いて、3次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は、画像上でも線分となる。次に、画像上で等間隔になるように投影線分上に制御点を設定し、制御点を通りかつ投影された線分に直交する線分上で1次元のエッジ検出を行う(図5(a))。エッジは、画素値の濃度勾配の極値として検出されるため、図5(b)に示すように、近傍にエッジが存在する場合には、複数のエッジが検出されることがある。本実施形態では、検出されるエッジのうち最も濃度勾配の大きいものを対応するエッジとする。
【0039】
≪ステップS1130≫
続いて、例えば位置姿勢算出部160は、3次元形状モデルの線分上の制御点と画像上のエッジとの対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。
【0040】
図6は、本発明の第1の実施形態を示し、線分の投影像と検出されたエッジとの関係を説明する模式図である。
図6では、画像の水平方向、垂直方向をそれぞれu軸、v軸としている。位置及び姿勢の概略値s0に基づいて投影された線分上のある制御点の画像上での位置を(u0,v0)、当該制御点が所属する線分の画像上での傾きをu軸に対する傾きθと表す。傾きθは、線分の両端の3次元座標をsの概略値(s0)に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。当該線分の画像上での法線ベクトルは(sinθ,−cosθ)となる。また、当該制御点の対応点(エッジ)の画像座標を(u',v')とする。ここで、点(u',v')を通り、傾きがθである直線(図6の破線)上の点(u,v)は、以下の数式(1)と表せる(θは定数とする)。
【0041】
【数1】
【0042】
制御点の画像上での位置は、計測対象物体の位置及び姿勢により変化する。制御点の画像座標(u,v)は(u0,v0)の近傍で1次のテイラー展開によって以下の数式(2)のように近似できる。但しΔsi(i=1,2,・・・,6)はsの各成分の微小変化を表す。
【0043】
【数2】
【0044】
正しいsによって得られる制御点の画像上での位置が数式(1)によって表される直線上にあると仮定すると、数式(2)を数式(1)に代入することにより、観測方程式である以下の数式(3)が得られる。
【0045】
【数3】
【0046】
数式(3)の観測方程式は、対応付けが行われたすべての制御点について立式することができる。数式(3)はsの各成分の微小変化Δsi(i=1,2,・・・,6)についての方程式になっており、以下の数式(4)に示すΔsiに関する線形連立方程式を立てることができる。
【0047】
【数4】
【0048】
この線形連立方程式を以下の数式(5)のように表す。
【0049】
【数5】
【0050】
数式(5)において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。
【0051】
≪ステップS1140≫
続いて、例えば位置姿勢算出部160は、数式(5)をもとに、位置及び姿勢の補正値Δsを最小二乗基準で求める。最小二乗基準では、以下の数式(6)を最小化するように位置及び姿勢の補正値Δsを求める。
【0052】
【数6】
【0053】
ただし、Jijは係数行列Jのi行j列目の要素を表し、Eiは誤差ベクトルEのi番目の要素を表す。数式(6)を最小にするΔsは、行列Jの一般化逆行列(Jt・J)-1・Jtを用いて、以下の数式(7)のように求める。
【0054】
【数7】
【0055】
しかしながら、エッジの対応付けでは、誤対応(外れ値)が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは、数式(4)の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制する。重みは、例えば以下の数式(8)に示すようなTukeyの関数により与える。
【0056】
【数8】
【0057】
ここで、数式(8)において、c1は定数である。なお、重みを与える関数は、Tukeyの関数である必要はなく、例えばHuberの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であれば何でもよい。そして、各計測データに対応する重みをwiとする。この重みwiを用いて、数式(6)を以下の数式(9)のように変形する。
【0058】
【数9】
【0059】
そして、例えば位置姿勢算出部160は、数式(9)を最小にする補正値Δsを以下の数式(10)のように解くことにより求める。
【0060】
【数10】
【0061】
ただし、数式(10)の行列Wは、以下の数式(11)で定義される重み行列である。
【0062】
【数11】
【0063】
数式(11)の重み行列Wは、対角成分以外はすべて0の正方行列であり、対角成分には重みwiが入る。
【0064】
≪ステップS1150≫
続いて、例えば位置姿勢算出部160は、ステップS1140において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。
s←s+Δs
【0065】
≪ステップS1160≫
続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS1130に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。
【0066】
以上の処理によって、ステップS1010で入力された位置及び姿勢の概略値が濃淡画像の情報を用いて更新される。この結果、位置及び姿勢の概略値に基づく画像上における3次元形状モデルの投影像と実際の物体の像とのオーバーラップ領域が広がるため、3次元形状モデルの投影に基づく距離画像と3次元形状モデルの対応付けにおける誤対応が減少する。
【0067】
<ステップS1040>
次に、濃淡画像及び距離画像を用いた位置及び姿勢の算出処理について説明する。本ステップでは、画像特徴検出部140及び位置姿勢算出部160は、ステップS1030で更新された位置及び姿勢の概略値を用いて、濃淡画像、距離画像の双方に対して3次元形状モデルが当てはまるような位置及び姿勢の推定(算出)を行う。ここでは、画像上のエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和と3次元点群と3次元形状モデルとの距離の二乗和の和が最小になるように位置及び姿勢の最適化を行う。
【0068】
図7は、図3のステップS1040における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図4のフローチャートで示されるステップS1030の処理手順と同一であるため、説明が重複する部分は省略する。
【0069】
≪ステップS1210≫
まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS1030で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【0070】
≪ステップS1220≫
続いて、画像特徴検出部140は、ステップS1020において入力された濃淡画像上において画像特徴の検出・画像特徴と3次元形状モデルとの対応付けを行う。また、同様にステップS1020において入力された距離画像と3次元形状モデルとの対応付けを行う。濃淡画像に対する処理は、ステップS1120の処理と同じであるため、説明を省略する。以下、距離画像と3次元形状モデルとの対応付け処理について説明する。
【0071】
図8は、本発明の第1の実施形態を示し、図7のステップS1220における距離画像と3次元形状モデルとの対応付け処理を説明する模式図である。
本ステップでは、上記非特許文献5で開示されている方法と同様に、物体の3次元形状モデル(ポリゴンモデル)810を距離画像面820上に投影することによって対応付けを行う。ここで、3次元形状モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデル810のどのポリゴンに対応するかを決定するためである。ここでは、距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと位置及び姿勢の概略値を用いてポリゴンモデル810を描画する。以下、ポリゴンモデル810を描画した画像をポリゴン描画画像と呼ぶ。
【0072】
描画する際には、各ポリゴンに固有の色を割り当てて描画を行う。ポリゴンの色はRGBの各色で表す。背景部分と区別するために、黒(R=0、G=0、B=0)以外の色を用いてポリゴンを描画する。これによって、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をGPU(Graphics Processing Unit)上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデル810を描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている3次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。
【0073】
≪ステップS1230≫
続いて、例えば位置姿勢算出部160は、3次元形状モデルの線分上の制御点と画像上のエッジとの対応、ポリゴンと3次元点の対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。モデルの線分上の制御点と画像上のエッジとの対応に基づく係数行列・誤差ベクトルの算出については、ステップS1130と同じであるため説明を省略する。以下では、距離画像の画素(3次元の点)と面との対応に基づく係数行列・誤差ベクトルの算出方法について説明する。
【0074】
3次元空間における平面の方程式は一般に、ax+by+cz=e(a2+b2+c2=1)の形で表される。ここでは、3次元形状モデル中のある面の計測物体の座標系における平面の方程式をax+by+cz=eであるとする。この平面の方程式は、位置及び姿勢の概略値に基づいて、基準座標系における平面の方程式a'x+b'y+c'z=e'に変換できる。位置及び姿勢の概略値に基づく計測物体座標系から基準座標系への座標変換を、以下の数式(12)のように表す。
【0075】
【数12】
【0076】
ただし、数式(12)において、Rは姿勢を表す3×3回転行列、tは位置を表す3次元ベクトルである。このとき、基準座標系における平面の方程式の係数は、以下の数式(13)のようになる。
【0077】
【数13】
【0078】
平面の座標変換については、例えば金谷「形状CADと図形の数学」(共立出版)で詳しく説明がなされている。基準座標系における3次元座標の計測値が(x',y',z')である点Pが3次元形状モデル中のある面Fと対応付けられているとする。面F上にある点をQとする。点Qの基準座標系における3次元座標(x,y,z)は、計測対象物体の位置及び姿勢sによって変換するものであり、ある位置及び姿勢sのときの3次元座標(x0,y0,z0)の近傍で1次のテイラー展開によって、以下の数14のように近似できる。
【0079】
【数14】
【0080】
正しいsによって変換される(x,y,z)は、法線ベクトルが面Fと同じで点Pを通る平面(a'x+b'y+c'z=e',e'=a'x'+b'y'+c'z') 上にあると仮定すると、数式(14)を平面の方程式に代入することで以下の数式(15)が得られる。
【0081】
【数15】
【0082】
数式(15)の観測方程式は、対応付けが行われたすべての点群データについて立式することができる。
数式(3)及び数式(15)の観測方程式は、sの各成分の微小変化Δsi(i=1,2,・・・,6)についての方程式になっており、数式(15)に示すΔsiに関する線形連立方程式を、以下の数式(16)のように立てることができる。
【0083】
【数16】
【0084】
この数式(16)に示す線形連立方程式を、以下の数式(17)のように表す。
【0085】
【数17】
【0086】
数式(17)において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。
【0087】
≪ステップS1240≫
続いて、例えば位置姿勢算出部160は、数式(17)をもとに、位置及び姿勢の補正値Δsを算出する。しかしながら、エッジの対応付けデータや距離画像の対応付けデータに誤対応などの外れ値が混入することは避けられない。そのため、ステップS1140で述べたようなロバスト推定手法により重み行列Wを用いて、以下の数式(18)により補正値Δsを算出する。
【0088】
【数18】
【0089】
≪ステップS1250≫
続いて、例えば位置姿勢算出部160は、ステップS1240において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。
s←s+Δs
【0090】
≪ステップS1260≫
続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS1230に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。
【0091】
上述したように、第1の実施形態に係る位置姿勢計測装置100では、3次元形状モデルが濃淡画像の画像特徴及び距離画像に当てはまるように、位置姿勢計測装置100に対する物体の概略の位置及び姿勢を更新する位置姿勢算出部160を有している。
そして、この位置姿勢算出部160では、少なくとも濃淡画像の情報を利用して画像面上での物体像と3次元形状モデルの投影像が最もオーバーラップするように第1の位置及び姿勢を算出する(図3のS1030)。なお、第1の位置及び姿勢は、位置及び姿勢の一部の成分であり、具体的には、当該位置姿勢計測装置100のカメラの光軸に垂直な位置及び光軸まわりの姿勢である。また、位置姿勢算出部160では、第1の位置及び姿勢を用いて画像面上に投影される3次元形状モデルの投影像をもとに3次元形状モデルと濃淡画像の画像特徴及び距離画像との対応付けを行って、第2の位置及び姿勢を算出する(図3のS1040)。
【0092】
このように、第1の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定(算出)して位置及び姿勢の概略値を更新した後、3次元形状モデルを投影することによる距離画像と3次元形状モデルの対応付けを行うことで誤対応を削減する。その結果、濃淡画像と距離画像を用いることで高精度に位置及び姿勢の推定(算出)が行えるとともに、位置及び姿勢の概略値のずれに対してロバストになる。
【0093】
[変形例1−1](2Dの自由度を減らす)
上述した第1の実施形態では、前段の処理である濃淡画像のみを利用する位置及び姿勢の推定において、位置及び姿勢の6自由度すべてを推定した。しかしながら、この処理においてはすべての自由度を推定する必要はなく、濃淡画像を用いて精度高く推定できる成分のみを推定してもよい。背景や物体の構造が複雑な場合には、濃淡画像上のエッジとモデルが誤対応することは避けられない。推定する成分を限定することで計測データに対するオーバーフィッティングが緩和されるため、誤対応に対するロバスト性の向上が期待できる。
【0094】
本変形例では、濃淡画像上のエッジを利用して、カメラの光軸に垂直な方向の位置成分と光軸まわりの姿勢成分の3つを未知パラメータとして推定する。これら3つのパラメータを3次元ベクトルs'で表す。制御点の画像座標(u,v)は(u0,v0)の近傍で1次のテイラー展開によって、以下の数式(19)のように近似できる。但しΔs'i(i=1,2,3)はs'の各成分の微小変化を表す。
【0095】
【数19】
【0096】
数式(19)を数式(1)に代入することにより、以下の観測方程式である数式(20)が得られる。
【0097】
【数20】
【0098】
対応付けが行われたすべての制御点について立式される数式(20)を連立方程式として、第1の実施形態と同様の処理によって補正値Δs'i(i=1,2,3)を算出する。本変形例における処理は、概略の位置及び姿勢に基づく投影像の形状と実際の物体の画像上での形状が類似している場合に有効である。
【0099】
[変形例1−2](繰り返し計算の中で対応付けをやり直してもよい)
上述した第1の実施形態では、ステップS1030、S1040の位置及び姿勢の算出処理において、計測データと3次元形状モデルとの対応付け情報は、固定した情報として取り扱った。しかし、実際には、推定する位置及び姿勢の変化によって対応付け情報は変化する。そのため、位置及び姿勢を更新した後で、再度、対応付け処理をやり直してもよい。ただし、対応付け処理は時間がかかる処理であるため、位置及び姿勢が更新されるたびに対応付けをやり直すのではなく、数回更新された後に対応付け処理を行ってもよい。
【0100】
[変形例1−3](概略値はclosed-formで求めてもよい)
上述した第1の実施形態では、位置及び姿勢の概略値を、過去の計測結果から入力した。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではなく、濃淡画像や距離画像の情報を元に繰り返し計算なしに直接的に算出してもよい。例えば、画像全体から検出された線分と3次元形状モデル中の線分と対応付けて位置及び姿勢を直接的に算出してもよい。具体的には、まず画像全体に対して例えばCannyのエッジ検出器によってエッジ検出を行い、隣接するエッジ群をラベリングする。次に、同一ラベルを持つエッジ群に対して、折れ線近似によって線分当てはめを行う。このようにして得られた画像上の線分と3次元形状モデル中の線分の対応をもとに上記非特許文献6に係るLiuの手法により位置及び姿勢を算出してもよい。また、例えば、上記非特許文献7に係るJohnsonの方法により、距離画像に基づいて概略の位置及び姿勢を算出してもよい。また、例えば、上記非特許文献9に係るUlrichらの方法のように、物体の姿勢に応じた固有のパターンを画像上で網羅的に探索する方法によって位置及び姿勢の概略値を算出してもよい。固有のパターンは、濃淡値からなるテンプレート画像であってもよいし、二値のエッジテンプレート画像であってもよい。また、網羅的に探索する方法は、姿勢クラスを識別するパターン認識による方法であってもよい。
【0101】
[変形例1−4](第1の位置姿勢を濃淡+距離から算出)
上述した第1の実施形態では、前段の処理において濃淡画像のみを利用して位置及び姿勢を推定した。しかしながら、濃淡画像の情報だけでなく、誤対応が悪い影響を与えない程度に距離画像の情報を利用してもよい。具体的には、第1の実施形態のステップS1030の処理のかわりにS1040の処理を実施する。このとき、距離画像の誤対応が位置及び姿勢の算出に大きく影響しないように、数式(8)における閾値c1の値を大きくして全体的に重みが小さくなるようにする。
【0102】
(第2の実施形態)(2Dの前に奥行きを求める)
上述した第1の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定して位置及び姿勢の概略値を更新した後、3次元形状モデルを投影することによる距離画像と3次元形状モデルの対応付けを行った。しかしながら、位置及び姿勢の概略値の更新方法は、これに限るものではない。例えば、距離画像のデータを用いて、位置及び姿勢のうち、カメラからの奥行きに相当する位置成分を推定(算出)してもよい。位置及び姿勢の概略値の真の値に対する誤差が大きい場合は、3次元形状モデルを投影することにより距離画像と3次元形状モデルを正確に対応付けることは難しい。しかしながら、カメラから物体表面までの距離がほぼ同一とみなせる場合には、多少対応付けが間違っていたとしても、奥行きを真の値に近づけることは可能である。
【0103】
そこで本実施形態では、まず距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第1の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値が真の値と大きく異なる場合であっても、ロバストに位置及び姿勢を計測(算出)することが可能になる。
【0104】
図9は、本発明の第2の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
図9の各ステップのうち、ステップS2010、S2020、S2040については、それぞれ、第1の実施形態における図3のステップS1010、S1020、S1040と同一であるため、その説明を省略する。以下では、図9の処理のうち、ステップS2025、S2030について説明する。
【0105】
<ステップS2025>
ステップS2025では、例えば位置姿勢算出部160は、距離画像の情報に基づいて物体の位置及び姿勢のうち、奥行きに相当する位置成分の概略値を更新する。
【0106】
具体的な処理としては、まずステップS2010において入力される位置及び姿勢の概略値をもとに、第1の実施形態のステップS1220において説明した方法により、距離画像と3次元形状モデルとの対応付けを行う。次に、例えば位置姿勢算出部160は、対応付け結果に基づいて、物体の位置を更新する。物体の位置は、次のように更新する。ここで、基準座標系における物体の位置の概略値を表す3次元ベクトルをt0=[tx ty tz]tとする。次に、例えば位置姿勢算出部160は、モデル面との対応付けがなされている距離画像上の画素について、基準座標系の原点(カメラの投影中心)から出て当該画素を通る光線と3次元形状モデル面との交点の3次元座標を算出する。光線上の点の3次元座標xは、光線の方向ベクトルをd=[dx dy dz]tとすると、x=αdによって表される(αは媒介変数)。この点が基準座標系におけるモデル面a'x+b'y+c'z=e'上にあるとして、αを求めてxを算出することにより、交点の3次元座標を算出する。距離画像上の画素に保持されている3次元座標をxdata、算出された交点の3次元座標をxcalcとする。xcalcのz成分の平均値/zcalcがxdataのz成分の平均値/zdataに一致するように物体の位置t0を更新する。更新した位置t'は、以下の数式(21)により算出する。単純に位置t0のz成分のみを更新しないのは、物体の観察方向に応じてz成分の位置への影響が変わるためである。
【0107】
【数21】
【0108】
<ステップS2030>
画像特徴検出部140及び位置姿勢算出部160は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測(算出)を行う。基本的な処理は、第1の実施形態におけるステップS1030と同一であるが、入力する位置及び姿勢の概略値として、ステップS2025によって更新された位置及び姿勢の概略値を利用する。以降の処理の説明は省略する。
【0109】
このように、第2の実施形態では、距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第1の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値のずれが補正され、後段の処理において誤対応を削減することが可能である。
【0110】
[変形例2−1](エッジは特徴点であってもよい)
以上、説明した本発明の実施形態、変形例では、濃淡画像上の特徴はエッジであり、3次元形状モデル中の線分をエッジに当てはめることにより位置及び姿勢の計測を行った。しかしながら、濃淡画像上の特徴はエッジに限るものではなく、例えば2次元的な特徴である特徴点であってもよい。特徴点は、コーナー点など画像上で特徴的な点であり、例えば、上記非特許文献8に係るHarrisの検出器によって検出することができる。3次元形状モデルを3次元点群によって表すことで、各特徴点をモデル中の3次元点群の点と対応付けることができる。特徴点を用いる場合には、まず画像全体からHarrisの検出器によって特徴点を検出する。次に、モデル中の3次元点群の各点を位置及び姿勢の概略値に基づいて画像上に投影し、画像上で最も近い特徴点と対応付けを行う。
【0111】
(第3の実施形態)(後段の位置姿勢推定は2.5Dのみ利用)
上述した実施形態では、後段の位置及び姿勢の推定においては濃淡画像と距離画像の双方の情報を用いていた。しかしながら、後段の位置及び姿勢の推定方法はこれに限るものではない。計測対象物体が多面体である場合、少なくとも3面以上の十分な面積を持つ面が撮像装置から見えていれば、距離画像のみから精度よく位置及び姿勢を推定することができる。また、計測対象物体が曲面から構成される場合には、撮像装置から観察される物体表面上の点の法線ベクトルが十分に3次元的に分布していれば距離画像のみから精度よく位置及び姿勢を推定することができる。
【0112】
そこで本実施形態では、まず、第1の実施形態で述べたように、濃淡画像を用いて物体の位置及び姿勢の概略値を更新した後に、更新された概略値を初期値として距離画像のみを用いて位置及び姿勢を推定する。
【0113】
図10は、本発明の第3の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
図10の各ステップのうち、ステップS3010、S3020、S3030については、それぞれ、第1の実施形態における図3のステップS1010、S1020、S1030と同一であるため、その説明を省略する。以下では、図10の処理のうち、ステップS3040について説明する。
【0114】
<ステップS3040>
ステップS3040では、位置姿勢算出部160は、ステップS3030で更新された位置及び姿勢の概略値を用いて、距離画像に対して3次元形状モデルが当てはまるような位置及び姿勢の推定(算出)を行う。ここでは、距離画像から得られる3次元点群と3次元形状モデルとの距離の二乗和が最小になるように位置及び姿勢の最適化を行う。
【0115】
図11は、図10のステップS3040における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図7のフローチャートで示されるステップS1040の処理手順から濃淡画像に対する処理を除いた処理手順であるため、説明が重複する部分は省略する。
【0116】
≪ステップS3110≫
まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS3030で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【0117】
≪ステップS3120≫
続いて、画像特徴検出部140は、ステップS3020で入力された距離画像と3次元形状モデルとの対応付けを行う。距離画像と3次元形状モデルとの対応付けについては、ステップS1220の処理と同じであるため、説明を省略する。
【0118】
≪ステップS3130≫
続いて、例えば位置姿勢算出部160は、ポリゴンと3次元点の対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。3次元形状モデルの面と3次元点との対応に基づく係数行列・誤差ベクトルの算出方法については、ステップS1230と同じであるため、説明を省略する。本ステップでは、距離画像の情報のみを用いるため、sの各成分の微小変化Δsi(i=1,2,・・・,6)に関する線形連立方程式は以下の数式(22)になる。
【0119】
【数22】
【0120】
この数式(22)に示す線形連立方程式を、以下の数式(23)のように表す。
【0121】
【数23】
【0122】
数式(23)において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。
【0123】
≪ステップS3140≫
続いて、例えば位置姿勢算出部160は、数式(23)をもとに、位置及び姿勢の補正値Δsを算出する。距離画像と3次元形状モデルの対応付けデータに誤対応などの外れ値が混入することは避けられないため、ステップS1140で述べたようなロバスト推定手法により重み行列Wを用いて、以下の数式(24)により補正値Δsを算出する。
【0124】
【数24】
【0125】
≪ステップS3150≫
続いて、例えば位置姿勢算出部160は、ステップS3140において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。
s←s+Δs
【0126】
≪ステップS3160≫
続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS3130に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。
【0127】
以上述べたように、第3の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢の概略値を推定した後に、推定した概略値を初期値として距離画像を用いて物体の位置及び姿勢を推定する。これにより、距離画像の情報のみから高精度に位置及び姿勢を推定する場合には、後段の処理において濃淡画像に関する処理を行う必要がないため、高速に位置及び姿勢を推定することが可能になる。
【0128】
(第4の実施形態)
上述した実施形態では、濃淡画像とともに対象物体の3次元の計測データとして距離画像を位置姿勢推定に利用していた。しかしながら、位置姿勢推定に利用する3次元の計測データは画素ごとに3次元座標を持つ距離画像に限るものではなく、物体の表面上における点群の3次元座標を表す計測データであれば他の形態であってもよい。この場合、位置姿勢計測装置100は、物体の表面上における点群の3次元座標を入力する3次元データ入力部(不図示)を具備する構成となる。そして、位置姿勢算出部160は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出する。さらに、位置姿勢算出部160は、第1の位置及び姿勢をもとに、3次元データ入力部(不図示)から得られる3次元座標の点群である3次元点群または当該3次元点群及び濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する。ここで、例えば、物体の表面上における点群の3次元座標を表す計測データとしては、3次元座標の集合であってもよい。なお、本実施形態と他の実施形態との違いは、3次元形状モデルと3次元の計測データの対応付けだけであるため、以下では対応付け方法のみを説明し、重複する説明は割愛する。
【0129】
ステップS1220の説明において述べたように、3次元の計測データが距離画像の場合には、物体の位置及び姿勢の概略値に基づいて3次元形状モデルを距離画像上に投影することで距離画像上の各画素と3次元形状モデルのポリゴンとの対応づけを行った。3次元計測データが3次元座標の集合である場合には画像上への投影を行うことができないため、各計測点に最も近いポリゴンを3次元的に探索する。
【0130】
具体的な方法を以下に示す。
まず、予め3次元形状モデルを構成する各ポリゴン上に計測点の密度より細かく複数の点(以降、面モデル点と呼ぶ)を設定し、各面モデル点の物体座標系における3次元座標と属するポリゴンのIDを保存しておく。対応付けを行う際には、非特許文献2に開示されるように、kd−treeを用いて各計測点に対応する面モデル点を探索する。面モデル点にはポリゴンのIDが紐づけられているため、この探索を行うことで計測点とポリゴンが対応付けされる。
【0131】
以上述べたように、第4の実施形態では、対象物体の3次元の計測データが画素ごとに3次元座標を持つ距離画像でなくとも、本発明による位置姿勢推定を適用することが可能である。
【0132】
上述した本発明の各実施形態では、位置姿勢算出部160は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出する(図3のS1030、図9のS2030、図10のS3030)。また、位置姿勢算出部160は、第1の位置及び姿勢を用いて算出される3次元形状モデルの画像面上での投影位置をもとに、距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する(図3のS1040、図9のS2040、図10のS3040)。
【0133】
(その他の実施形態)
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア(プログラム)を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ(またはCPUやMPU等)がプログラムを読み出して実行する処理である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記録媒体は、本発明に含まれる。
【産業上の利用可能性】
【0134】
本発明は、例えば、ロボットの自己位置推定や、ロボットと物体との間の相対的な位置及び姿勢を推定するのに利用することができる。
【符号の説明】
【0135】
100 位置姿勢計測装置、110 3次元形状モデル保存部、120 概略位置姿勢入力部、130 濃淡画像入力部、140 画像特徴検出部、150 距離画像入力部、160 位置姿勢算出部、200 濃淡画像撮像部、300 距離計測部
【技術分野】
【0001】
本発明は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置及び位置姿勢計測方法に関する。
【背景技術】
【0002】
近年、ロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このような目的には主にアーム型のロボットが利用され、ロボットはアームの先端に装着されたハンドなどのエンドエフェクタによって部品を把持し組立を行う。ロボットが適切に部品を把持するためには、部品とロボット(エンドエフェクタ)との間の相対的な位置及び姿勢を高精度に計測する必要がある。また、ロボットによる組立を実場面に適用するためには、部品の位置及び姿勢の計測は、高精度に加えて高速である必要がある。このような位置及び姿勢の計測技術は、ロボットによる工業製品の組立だけでなく、ロボットが自律移動するための自己位置推定や、実測データからの3次元モデルデータ作成など様々な場面で必要とされる。
【0003】
工業製品の生産現場における部品の位置及び姿勢の計測には、カメラから得られる濃淡(カラー)画像や非接触式の距離センサから得られる距離画像が主に利用される。物体の個体ごとの位置及び姿勢の計測は、観測データ(濃淡画像・距離画像)に対する物体の3次元形状モデルの当てはめによって行われるのが一般的である。
【0004】
下記の非特許文献1では、物体の3次元形状モデルを線分の集合であるワイヤフレームモデルで表し、線分の投影像を濃淡画像上のエッジに当てはめることで、物体の位置及び姿勢を計測する方法が開示されている。この濃淡画像上のエッジを利用する方法では、物体の輪郭部分や面の切り替わり部分など画像上でエッジとなる部分の投影像と画像上のエッジとの「2次元画像平面上での距離」が最小になるように物体の位置及び姿勢を計測する。そのため、2次元画像平面上での距離を大きく変化させる位置及び姿勢の成分の計測精度は高いが、それ以外の成分の計測精度は必ずしも高くない。即ち、カメラの光軸に垂直な方向の位置成分や光軸まわりの姿勢成分の計測精度が高いのに対し、カメラの光軸方向の位置成分(奥行き)などの計測精度は低い。
【0005】
下記の非特許文献2では、距離画像から得られる物体表面上の3次元点群データに物体の3次元形状モデル(ポリゴンモデル)を当てはめることで、物体の位置及び姿勢を計測する方法が開示されている。この非特許文献2で開示されるような距離画像を利用する方法は、点群とモデルの「3次元空間内での」距離を直接最小化する方法であるため、基本的には位置及び姿勢の計測精度は高い。しかしながら、下記の非特許文献3で開示されるような能動ステレオ法では、物体の輪郭部分の距離を安定に計測できないことが多いため、物体の形状や観察方向によっては輪郭部分が正確に一致するような位置及び姿勢の計測ができない。
【0006】
以上、述べた濃淡画像を利用する方法と距離画像を利用する方法の特性を鑑みると、位置及び姿勢の推定においては、濃淡画像から得られる情報と距離画像から得られる情報の間には相補的な関係があると言える。そのため、濃淡画像と距離画像の双方に対して同時に3次元形状モデルが当てはまるように位置及び姿勢の計測を行うことで、位置及び姿勢の計測精度を向上させることができる。下記の非特許文献1及び下記の非特許文献2で開示される方法では、それぞれモデル投影像とエッジの2次元画像面上での誤差の二乗和、モデルと点群の3次元空間中における誤差の二乗和を評価関数として最小化する。2次元画像面上の距離と3次元空間中の誤差はスケールが異なるため、2つの評価関数の和が最小となるように位置及び姿勢を推定するような単純な方法では、どちらか一方の影響が大きくなるという問題がある。従来では、異なるスケールの誤差を共通の尺度によって統一的に評価することで、濃淡画像と距離画像の情報を相補的に利用する位置及び姿勢の計測方法が提案されている。ここでは、共通の尺度の1つとして、2次元画像面上の誤差及び3次元空間中の誤差を、夫々が従う確率分布における発生確率(尤度)によって表し、尤度の積を最大化することで高精度な位置及び姿勢の計測を行っている。
【先行技術文献】
【非特許文献】
【0007】
【非特許文献1】T. Drummond and R. Cipolla, "Real-time visual tracking of complex structures," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.24, no.7, pp.932-946, 2002.
【非特許文献2】D. A. Simon, M. Hebert, and T. Kanade, "Real-time 3-D pose estimation using a high-speed range sensor," Proc. 1994 IEEE International Conference on Robotics and Automation (ICRA'94), pp.2235-2241, 1994.
【非特許文献3】佐藤,井口,"空間コードによる距離画像入力",電子情報通信学会論文誌,vol.J68-D,no.3,pp.369-375,1985.
【非特許文献4】Z. Zhang, "A flexible new technique for camera calibration", IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.22, No.11, pages 1330-1334, 2000.
【非特許文献5】大石,中澤,池内,"インデックス画像を用いた複数距離画像の高速同時位置合せ",vol.J89-D,no.3,pp.513-521,2006.
【非特許文献6】Y. Liu, T. S. Huang, and O. D. Faugeras, "Determination of camera location from 2-D to 3-D line and point correspondences," Proc. CVPR'88, pp.82-88, 1988.
【非特許文献7】A. Johnson and M. Hebert, "Using spin images for efficient object recognition in cluttered 3D scenes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.21, no.5, pp.433-449, 1999.
【非特許文献8】C. Harris and M. Stephens, "A combined corner and edge detector" Proc. the 4th Alvey Vision Conference, pp.147-151, 1988.
【非特許文献9】M. Ulrich, C. Wiedemann, and C. Steger, "CAD-based recognition of 3D objects in monocular images" Proc. 2009 IEEE International Conference on Robotics and Automation (ICRA'09), pp.1191-1198, 2009.
【発明の概要】
【発明が解決しようとする課題】
【0008】
工業製品の生産過程では時間に対する制約が厳しいため、その一工程である部品の位置及び姿勢の計測はできる限り高速に行われる必要がある。また、ロボットの自己位置推定など、位置及び姿勢の計測を高速に処理しなければならない場面は他にも多い。前述した濃淡画像・距離画像に対する3次元形状モデルの当てはめによる位置及び姿勢の推定は、(1)モデルと画像の対応付け、(2)対応付け結果に基づく位置及び姿勢の算出、の2つのステップからなる。高精度な位置及び姿勢を推定するため、これらのステップは複数回繰り返されるのが一般的である。これら2つのステップのうち、モデルと画像の対応付け処理にかかる計算時間が問題になることが多い。
【0009】
上記非特許文献1に開示される方法では、位置及び姿勢の初期値に基づいて3次元形状モデル中の各線分を画像上に投影し、画像上の投影像の近傍で対応するエッジを探索することにより、モデルと画像の対応付け処理を高速化している。一方、距離画像と3次元形状モデルの対応付けでは、3次元空間において最も近傍にある点を探索する必要がある。
【0010】
上記非特許文献2では、kd−treeを用いて最近傍点の探索を行っている。しかしこの方法では、オーダーがO(NlogM)の計算が必要になる(Nはデータ側の点数、Mはモデル側の点数)ため、各画素が計測点となる距離画像のように計測点数が非常に多いデータを利用する場合に時間がかかる。これに対し、従来では、上記非特許文献1の方法と同様に、画像上にモデルを投影することにより対応付けを高速化する方法が提案されている。具体的には、まず位置及び姿勢の初期値に基づいて3次元形状モデル内の幾何特徴の画像への投影計算を行い、幾何特徴の画像上での位置を算出する。次に、この幾何特徴の画像上での位置をもとに、幾何特徴と距離画像の画素とを対応付けすることにより、高速な対応付けを実現している。
【0011】
しかしながら、前述の3次元形状モデルの画像上への投影に基づいた画像と3次元形状モデルの対応付けでは、3次元形状モデルの投影像と実際の物体の像とが十分にオーバーラップしていることが前提になっている。そのため、投影に用いる位置及び姿勢の初期値と実際の位置及び姿勢とのずれが大きい場合には誤対応が多く生じ、後段の位置及び姿勢の算出に失敗する。濃淡画像のエッジの対応付けでは明示的な特徴(エッジ)を用いているため、初期値のずれに対して比較的ロバストである。
【0012】
一方、距離画像の対応付けでは、濃淡画像上のエッジのような明示的な特徴を用いていないため、初期値のずれに対してロバスト性が低く、位置及び姿勢の算出に失敗する要因となる。逆に初期値のずれが小さく距離画像とモデルが正しく対応付けできる場合には、距離画像の利用によって高精度な位置及び姿勢の計測が可能になる。このように、初期値のずれに対するロバスト性は、濃淡画像と距離画像で夫々特性が異なる。言い換えると、従来においては、この特性を考慮することまでは行っておらず、良い特性を持つ計測データを段階的・選択的に使うことによる初期値ずれに対するロバスト化に改善の余地があった。
【0013】
本発明は、このような問題点に鑑みてなされたものであり、初期値のずれ具合の段階に応じてよい特性を持つ計測データを利用することにより、画像と3次元形状モデルの誤対応を減らし、高速でロバストな物体の位置及び姿勢の計測を実現することを目的とする。
【課題を解決するための手段】
【0014】
本発明の位置姿勢計測装置は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の距離画像を入力する距離画像入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有する。
本発明の位置姿勢計測装置における他の態様は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の表面上における点群の3次元座標を入力する3次元データ入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力手段から得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有する。
また、本発明は、上述した位置姿勢計測装置による位置姿勢計測方法を含む。
【発明の効果】
【0015】
本発明によれば、濃淡画像と距離画像を段階的・選択的に用いることにより、位置及び姿勢の初期値と実際の位置及び姿勢とのずれに対してロバストな物体の位置及び姿勢の計測が可能になる。
【図面の簡単な説明】
【0016】
【図1】本発明の第1の実施形態に係る位置姿勢計測装置を含む位置姿勢計測システムの概略構成の一例を示すブロック図である。
【図2】本発明の第1の実施形態を示し、3次元形状モデルを説明する模式図である。
【図3】本発明の第1の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【図4】図3のステップS1030における位置姿勢計測(位置姿勢算出)の処理手順の一例を示すフローチャートである。
【図5】本発明の第1の実施形態を示し、エッジ検出を説明する模式図である。
【図6】本発明の第1の実施形態を示し、線分の投影像と検出されたエッジとの関係を説明する模式図である。
【図7】図3のステップS1040における位置姿勢算出の処理手順の一例を示すフローチャートである。
【図8】本発明の第1の実施形態を示し、図7のステップS1220における距離画像と3次元形状モデルとの対応付け処理を説明する模式図である。
【図9】本発明の第2の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【図10】本発明の第3の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【図11】図10のステップS3040における位置姿勢算出の処理手順の一例を示すフローチャートである。
【発明を実施するための形態】
【0017】
以下に、図面を参照しながら、本発明を実施するための形態(実施形態)について説明する。
【0018】
(第1の実施形態)
第1の実施形態では、まず濃淡画像の情報を用いて物体の位置及び姿勢を推定した後、濃淡画像上の特徴と距離画像に対して同時にモデルが当てはまるように位置及び姿勢の推定を行う。本実施形態では、位置及び姿勢の初期値は与えられるものとする。実際の位置及び姿勢と初期値とのずれが大きい場合、モデルを投影することによる距離画像(3次元点群)と3次元形状モデルの対応付けは、誤対応が頻発する。第1の実施形態では、距離画像の対応付けを行う前に、画像特徴を利用した位置及び姿勢の推定を行ってモデルの投影像と実際の物体像との画像上のオーバーラップ領域をできるだけ広げ、距離画像の対応付けにおける誤対応を削減する。
【0019】
図1は、本発明の第1の実施形態に係る位置姿勢計測装置を含む位置姿勢計測システムの概略構成の一例を示すブロック図である。
図1に示すように、位置姿勢計測システムは、位置姿勢計測装置100、濃淡画像撮像部200、及び、距離計測部300を有して構成されている。
【0020】
また、図1に示すように、位置姿勢計測装置100は、3次元形状モデル保存部110、概略位置姿勢入力部120、濃淡画像入力部130、画像特徴検出部140、距離画像入力部150、及び、位置姿勢算出部160を有して構成されている。また、位置姿勢計測装置100には、濃淡画像撮像部200及び距離計測部300が接続されている。
【0021】
濃淡画像撮像部200は、通常の濃淡画像を撮影するカメラである。撮影される画像は、濃淡画像であってもよいしカラー画像であってもよい。濃淡画像撮像部200が撮影する画像は、濃淡画像入力部130を介して位置姿勢計測装置100に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば、上記非特許文献4に係るZhangの方法によって事前にキャリブレーションしておく。
【0022】
距離計測部300は、計測対象である物体表面上の点までの距離を計測する。本実施形態では、距離計測部300として距離画像を出力する距離センサを用いる。距離画像は、各画素が距離計測部300からの距離の情報を持つ画像である。本実施形態では、各画素には距離の情報ではなく、距離をもとに算出される計測装置基準の座標系における3次元座標が保存されているものとする。即ち、距離画像からは、3次元点群データが得られる。距離値から3次元点群への変換は、距離画像の各画素について、画素位置に対応する視線ベクトルに距離値を乗算することにより行う。距離センサとして、例えば、液晶プロジェクタから物体に照射した2次元のパターン光をカメラで撮影し、三角測量により距離を計測するアクティブ方式のものを利用する。なお、距離センサはこれに限るものではなく、光切断法やTime-of-flight方式など他のアクティブ式のものや、ステレオカメラによるパッシブ式など、距離画像を出力可能であれば如何なる距離センサであってもよい。本実施形態では、距離センサ用のカメラと濃淡画像撮像部200のカメラは同一のものを用いる。即ち、同一視点における濃淡画像と距離画像が取得できると仮定する。距離センサ用のカメラと濃淡画像撮像部200のカメラが異なる場合には、両者の相対的な位置及び姿勢を事前にキャリブレーションしておく。例えば3次元形状が既知であるキャリブレーション物体を撮影し、濃淡画像に基づいた位置及び姿勢と、距離画像に基づいた物体の位置及び姿勢から相対的な位置及び姿勢を求める。距離計測部300が計測した距離画像は、距離画像入力部150を介して位置姿勢計測装置100に入力される。
【0023】
次に、位置姿勢計測装置100を構成する各部について説明する。
【0024】
3次元形状モデル保存部110には、位置及び姿勢を計測する対象である物体の3次元形状モデルを保存する。本実施形態では、物体を線分と面から構成される3次元形状モデルとして記述する。
【0025】
図2は、本発明の第1の実施形態を示し、3次元形状モデルを説明する模式図である。3次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図2(a)に示すように、計測対象物体10−1の3次元形状モデルは、点P1〜点P14の14点から構成される。また、図2(b)に示すように、計測対象物体10−2の3次元形状モデルは、線分L1〜L16により構成されている。また、図2(c)に示すように、点P1〜点P14は3次元座標値によって表される。また、図2(d)に示すように、線分L1〜L16は、線分を構成する点のIDによって表される。さらに、3次元形状モデルは、面の情報を保持する。各面は、各面を構成する点のIDによって表される。図2に示す3次元形状モデルでは、直方体を構成する6つの面の情報が記憶されている。3次元形状モデルは、位置姿勢算出部160において物体の位置及び姿勢を算出する際に用いられる。
【0026】
概略位置姿勢入力部120は、位置姿勢計測装置100に対する物体の位置及び姿勢の概略値を入力する。位置姿勢計測装置100には、位置及び姿勢計測の基準となる3次元の座標系(基準座標系)が規定されているものとする。位置姿勢計測装置100に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、濃淡画像撮像部200であるカメラのカメラ中心を原点とし、画像の水平方向をx軸、垂直方向をy軸、カメラの光軸をz軸とした座標系を基準座標系とする。本実施形態では、位置姿勢計測装置100は、時間軸方向に連続して計測を行うものとして、前回(前時刻)の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果をもとに位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測したものでもよい。また、物体が置かれているおおよその位置や姿勢が予めわかっている場合には、その値を概略値として用いる。この概略位置姿勢入力部120は、濃淡画像または距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される物体の位置及び姿勢を概略の位置及び姿勢として入力する。
【0027】
濃淡画像入力部130は、濃淡画像撮像部200で撮影された濃淡画像を位置姿勢計測装置100に入力する処理を行う。
【0028】
画像特徴検出部140は、濃淡画像入力部130から入力された濃淡画像から画像特徴を検出する。本実施形態では、画像特徴としてエッジを検出する。具体的には、概略位置姿勢入力部120から入力される物体の位置及び姿勢の概略値に基づいて、3次元形状モデルの各線分を画像上に投影し、投影された線分の近傍で対応するエッジ探索を行う。
【0029】
距離画像入力部150は、距離計測部300で得られた距離画像を位置姿勢計測装置100に入力する処理を行う。
【0030】
位置姿勢算出部160は、画像特徴検出部140で検出された画像特徴、及び距離画像入力部150によって入力された3次元点群と、3次元形状モデル保存部110に保存される3次元形状モデルをもとに、物体の位置及び姿勢を計測する。
【0031】
図3は、本発明の第1の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
【0032】
<ステップS1010>
概略位置姿勢入力部120は、位置姿勢計測装置100(カメラ)に対する物体の位置及び姿勢の概略値を位置姿勢計測装置100に入力する。前述したように、本実施形態では、前回計測された位置及び姿勢を概略値として用いる。
【0033】
<ステップS1020>
濃淡画像入力部130及び距離画像入力部150は、物体の位置及び姿勢を算出するための計測データを取得する。具体的には、濃淡画像入力部130及び距離画像入力部150は、計測対象物体の濃淡画像と距離画像を取得する。前述したように、距離画像の各画素には、基準座標系における点の3次元座標が記録されている。濃淡画像撮像部200によって撮影された濃淡画像は、濃淡画像入力部130を介して位置姿勢計測装置100に入力される。また、距離計測部300から出力された距離画像は、距離画像入力部150を介して位置姿勢計測装置100に入力される。
【0034】
<ステップS1030>
画像特徴検出部140及び位置姿勢算出部160は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測(算出)を行う。ステップS1030では、濃淡画像上の特徴であるエッジに3次元形状モデル中の線分の投影像が当てはまるように、計測対象物体の位置及び姿勢を算出する。
【0035】
図4は、図3のステップS1030における位置姿勢計測(位置姿勢算出)の処理手順の一例を示すフローチャートである。ここでは、画像上で検出されたエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。以下の説明では、物体の位置及び姿勢を6次元ベクトルsで表す。sは、計測対象物体の位置を表す3つの要素と、姿勢を表す3つの要素からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す3次元ベクトルなどによって表現される。以下、図3のステップS1030における位置姿勢算出の処理手順について順に説明する。
【0036】
≪ステップS1110≫
まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS1010で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【0037】
≪ステップS1120≫
続いて、画像特徴検出部140は、ステップS1020において入力された濃淡画像上において画像特徴の検出・画像特徴と3次元形状モデルとの対応付けを行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。本実施形態では、上記非特許文献1で開示される方法によりエッジ検出を行う。この非特許文献1で開示されるエッジ検出方法は、トップダウン型の方法であり、モデル側から対応するエッジを探索する方法であるため、検出と対応付けが一度に行われる。
【0038】
図5は、本発明の第1の実施形態を示し、エッジ検出を説明する模式図である。
まず、ここでは、ステップS1110において入力された計測対象物体の位置及び姿勢の概略値と校正済みのカメラの内部パラメータを用いて、3次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は、画像上でも線分となる。次に、画像上で等間隔になるように投影線分上に制御点を設定し、制御点を通りかつ投影された線分に直交する線分上で1次元のエッジ検出を行う(図5(a))。エッジは、画素値の濃度勾配の極値として検出されるため、図5(b)に示すように、近傍にエッジが存在する場合には、複数のエッジが検出されることがある。本実施形態では、検出されるエッジのうち最も濃度勾配の大きいものを対応するエッジとする。
【0039】
≪ステップS1130≫
続いて、例えば位置姿勢算出部160は、3次元形状モデルの線分上の制御点と画像上のエッジとの対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。
【0040】
図6は、本発明の第1の実施形態を示し、線分の投影像と検出されたエッジとの関係を説明する模式図である。
図6では、画像の水平方向、垂直方向をそれぞれu軸、v軸としている。位置及び姿勢の概略値s0に基づいて投影された線分上のある制御点の画像上での位置を(u0,v0)、当該制御点が所属する線分の画像上での傾きをu軸に対する傾きθと表す。傾きθは、線分の両端の3次元座標をsの概略値(s0)に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。当該線分の画像上での法線ベクトルは(sinθ,−cosθ)となる。また、当該制御点の対応点(エッジ)の画像座標を(u',v')とする。ここで、点(u',v')を通り、傾きがθである直線(図6の破線)上の点(u,v)は、以下の数式(1)と表せる(θは定数とする)。
【0041】
【数1】
【0042】
制御点の画像上での位置は、計測対象物体の位置及び姿勢により変化する。制御点の画像座標(u,v)は(u0,v0)の近傍で1次のテイラー展開によって以下の数式(2)のように近似できる。但しΔsi(i=1,2,・・・,6)はsの各成分の微小変化を表す。
【0043】
【数2】
【0044】
正しいsによって得られる制御点の画像上での位置が数式(1)によって表される直線上にあると仮定すると、数式(2)を数式(1)に代入することにより、観測方程式である以下の数式(3)が得られる。
【0045】
【数3】
【0046】
数式(3)の観測方程式は、対応付けが行われたすべての制御点について立式することができる。数式(3)はsの各成分の微小変化Δsi(i=1,2,・・・,6)についての方程式になっており、以下の数式(4)に示すΔsiに関する線形連立方程式を立てることができる。
【0047】
【数4】
【0048】
この線形連立方程式を以下の数式(5)のように表す。
【0049】
【数5】
【0050】
数式(5)において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。
【0051】
≪ステップS1140≫
続いて、例えば位置姿勢算出部160は、数式(5)をもとに、位置及び姿勢の補正値Δsを最小二乗基準で求める。最小二乗基準では、以下の数式(6)を最小化するように位置及び姿勢の補正値Δsを求める。
【0052】
【数6】
【0053】
ただし、Jijは係数行列Jのi行j列目の要素を表し、Eiは誤差ベクトルEのi番目の要素を表す。数式(6)を最小にするΔsは、行列Jの一般化逆行列(Jt・J)-1・Jtを用いて、以下の数式(7)のように求める。
【0054】
【数7】
【0055】
しかしながら、エッジの対応付けでは、誤対応(外れ値)が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは、数式(4)の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制する。重みは、例えば以下の数式(8)に示すようなTukeyの関数により与える。
【0056】
【数8】
【0057】
ここで、数式(8)において、c1は定数である。なお、重みを与える関数は、Tukeyの関数である必要はなく、例えばHuberの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であれば何でもよい。そして、各計測データに対応する重みをwiとする。この重みwiを用いて、数式(6)を以下の数式(9)のように変形する。
【0058】
【数9】
【0059】
そして、例えば位置姿勢算出部160は、数式(9)を最小にする補正値Δsを以下の数式(10)のように解くことにより求める。
【0060】
【数10】
【0061】
ただし、数式(10)の行列Wは、以下の数式(11)で定義される重み行列である。
【0062】
【数11】
【0063】
数式(11)の重み行列Wは、対角成分以外はすべて0の正方行列であり、対角成分には重みwiが入る。
【0064】
≪ステップS1150≫
続いて、例えば位置姿勢算出部160は、ステップS1140において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。
s←s+Δs
【0065】
≪ステップS1160≫
続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS1130に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。
【0066】
以上の処理によって、ステップS1010で入力された位置及び姿勢の概略値が濃淡画像の情報を用いて更新される。この結果、位置及び姿勢の概略値に基づく画像上における3次元形状モデルの投影像と実際の物体の像とのオーバーラップ領域が広がるため、3次元形状モデルの投影に基づく距離画像と3次元形状モデルの対応付けにおける誤対応が減少する。
【0067】
<ステップS1040>
次に、濃淡画像及び距離画像を用いた位置及び姿勢の算出処理について説明する。本ステップでは、画像特徴検出部140及び位置姿勢算出部160は、ステップS1030で更新された位置及び姿勢の概略値を用いて、濃淡画像、距離画像の双方に対して3次元形状モデルが当てはまるような位置及び姿勢の推定(算出)を行う。ここでは、画像上のエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和と3次元点群と3次元形状モデルとの距離の二乗和の和が最小になるように位置及び姿勢の最適化を行う。
【0068】
図7は、図3のステップS1040における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図4のフローチャートで示されるステップS1030の処理手順と同一であるため、説明が重複する部分は省略する。
【0069】
≪ステップS1210≫
まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS1030で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【0070】
≪ステップS1220≫
続いて、画像特徴検出部140は、ステップS1020において入力された濃淡画像上において画像特徴の検出・画像特徴と3次元形状モデルとの対応付けを行う。また、同様にステップS1020において入力された距離画像と3次元形状モデルとの対応付けを行う。濃淡画像に対する処理は、ステップS1120の処理と同じであるため、説明を省略する。以下、距離画像と3次元形状モデルとの対応付け処理について説明する。
【0071】
図8は、本発明の第1の実施形態を示し、図7のステップS1220における距離画像と3次元形状モデルとの対応付け処理を説明する模式図である。
本ステップでは、上記非特許文献5で開示されている方法と同様に、物体の3次元形状モデル(ポリゴンモデル)810を距離画像面820上に投影することによって対応付けを行う。ここで、3次元形状モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデル810のどのポリゴンに対応するかを決定するためである。ここでは、距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと位置及び姿勢の概略値を用いてポリゴンモデル810を描画する。以下、ポリゴンモデル810を描画した画像をポリゴン描画画像と呼ぶ。
【0072】
描画する際には、各ポリゴンに固有の色を割り当てて描画を行う。ポリゴンの色はRGBの各色で表す。背景部分と区別するために、黒(R=0、G=0、B=0)以外の色を用いてポリゴンを描画する。これによって、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をGPU(Graphics Processing Unit)上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデル810を描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている3次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。
【0073】
≪ステップS1230≫
続いて、例えば位置姿勢算出部160は、3次元形状モデルの線分上の制御点と画像上のエッジとの対応、ポリゴンと3次元点の対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。モデルの線分上の制御点と画像上のエッジとの対応に基づく係数行列・誤差ベクトルの算出については、ステップS1130と同じであるため説明を省略する。以下では、距離画像の画素(3次元の点)と面との対応に基づく係数行列・誤差ベクトルの算出方法について説明する。
【0074】
3次元空間における平面の方程式は一般に、ax+by+cz=e(a2+b2+c2=1)の形で表される。ここでは、3次元形状モデル中のある面の計測物体の座標系における平面の方程式をax+by+cz=eであるとする。この平面の方程式は、位置及び姿勢の概略値に基づいて、基準座標系における平面の方程式a'x+b'y+c'z=e'に変換できる。位置及び姿勢の概略値に基づく計測物体座標系から基準座標系への座標変換を、以下の数式(12)のように表す。
【0075】
【数12】
【0076】
ただし、数式(12)において、Rは姿勢を表す3×3回転行列、tは位置を表す3次元ベクトルである。このとき、基準座標系における平面の方程式の係数は、以下の数式(13)のようになる。
【0077】
【数13】
【0078】
平面の座標変換については、例えば金谷「形状CADと図形の数学」(共立出版)で詳しく説明がなされている。基準座標系における3次元座標の計測値が(x',y',z')である点Pが3次元形状モデル中のある面Fと対応付けられているとする。面F上にある点をQとする。点Qの基準座標系における3次元座標(x,y,z)は、計測対象物体の位置及び姿勢sによって変換するものであり、ある位置及び姿勢sのときの3次元座標(x0,y0,z0)の近傍で1次のテイラー展開によって、以下の数14のように近似できる。
【0079】
【数14】
【0080】
正しいsによって変換される(x,y,z)は、法線ベクトルが面Fと同じで点Pを通る平面(a'x+b'y+c'z=e',e'=a'x'+b'y'+c'z') 上にあると仮定すると、数式(14)を平面の方程式に代入することで以下の数式(15)が得られる。
【0081】
【数15】
【0082】
数式(15)の観測方程式は、対応付けが行われたすべての点群データについて立式することができる。
数式(3)及び数式(15)の観測方程式は、sの各成分の微小変化Δsi(i=1,2,・・・,6)についての方程式になっており、数式(15)に示すΔsiに関する線形連立方程式を、以下の数式(16)のように立てることができる。
【0083】
【数16】
【0084】
この数式(16)に示す線形連立方程式を、以下の数式(17)のように表す。
【0085】
【数17】
【0086】
数式(17)において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。
【0087】
≪ステップS1240≫
続いて、例えば位置姿勢算出部160は、数式(17)をもとに、位置及び姿勢の補正値Δsを算出する。しかしながら、エッジの対応付けデータや距離画像の対応付けデータに誤対応などの外れ値が混入することは避けられない。そのため、ステップS1140で述べたようなロバスト推定手法により重み行列Wを用いて、以下の数式(18)により補正値Δsを算出する。
【0088】
【数18】
【0089】
≪ステップS1250≫
続いて、例えば位置姿勢算出部160は、ステップS1240において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。
s←s+Δs
【0090】
≪ステップS1260≫
続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS1230に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。
【0091】
上述したように、第1の実施形態に係る位置姿勢計測装置100では、3次元形状モデルが濃淡画像の画像特徴及び距離画像に当てはまるように、位置姿勢計測装置100に対する物体の概略の位置及び姿勢を更新する位置姿勢算出部160を有している。
そして、この位置姿勢算出部160では、少なくとも濃淡画像の情報を利用して画像面上での物体像と3次元形状モデルの投影像が最もオーバーラップするように第1の位置及び姿勢を算出する(図3のS1030)。なお、第1の位置及び姿勢は、位置及び姿勢の一部の成分であり、具体的には、当該位置姿勢計測装置100のカメラの光軸に垂直な位置及び光軸まわりの姿勢である。また、位置姿勢算出部160では、第1の位置及び姿勢を用いて画像面上に投影される3次元形状モデルの投影像をもとに3次元形状モデルと濃淡画像の画像特徴及び距離画像との対応付けを行って、第2の位置及び姿勢を算出する(図3のS1040)。
【0092】
このように、第1の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定(算出)して位置及び姿勢の概略値を更新した後、3次元形状モデルを投影することによる距離画像と3次元形状モデルの対応付けを行うことで誤対応を削減する。その結果、濃淡画像と距離画像を用いることで高精度に位置及び姿勢の推定(算出)が行えるとともに、位置及び姿勢の概略値のずれに対してロバストになる。
【0093】
[変形例1−1](2Dの自由度を減らす)
上述した第1の実施形態では、前段の処理である濃淡画像のみを利用する位置及び姿勢の推定において、位置及び姿勢の6自由度すべてを推定した。しかしながら、この処理においてはすべての自由度を推定する必要はなく、濃淡画像を用いて精度高く推定できる成分のみを推定してもよい。背景や物体の構造が複雑な場合には、濃淡画像上のエッジとモデルが誤対応することは避けられない。推定する成分を限定することで計測データに対するオーバーフィッティングが緩和されるため、誤対応に対するロバスト性の向上が期待できる。
【0094】
本変形例では、濃淡画像上のエッジを利用して、カメラの光軸に垂直な方向の位置成分と光軸まわりの姿勢成分の3つを未知パラメータとして推定する。これら3つのパラメータを3次元ベクトルs'で表す。制御点の画像座標(u,v)は(u0,v0)の近傍で1次のテイラー展開によって、以下の数式(19)のように近似できる。但しΔs'i(i=1,2,3)はs'の各成分の微小変化を表す。
【0095】
【数19】
【0096】
数式(19)を数式(1)に代入することにより、以下の観測方程式である数式(20)が得られる。
【0097】
【数20】
【0098】
対応付けが行われたすべての制御点について立式される数式(20)を連立方程式として、第1の実施形態と同様の処理によって補正値Δs'i(i=1,2,3)を算出する。本変形例における処理は、概略の位置及び姿勢に基づく投影像の形状と実際の物体の画像上での形状が類似している場合に有効である。
【0099】
[変形例1−2](繰り返し計算の中で対応付けをやり直してもよい)
上述した第1の実施形態では、ステップS1030、S1040の位置及び姿勢の算出処理において、計測データと3次元形状モデルとの対応付け情報は、固定した情報として取り扱った。しかし、実際には、推定する位置及び姿勢の変化によって対応付け情報は変化する。そのため、位置及び姿勢を更新した後で、再度、対応付け処理をやり直してもよい。ただし、対応付け処理は時間がかかる処理であるため、位置及び姿勢が更新されるたびに対応付けをやり直すのではなく、数回更新された後に対応付け処理を行ってもよい。
【0100】
[変形例1−3](概略値はclosed-formで求めてもよい)
上述した第1の実施形態では、位置及び姿勢の概略値を、過去の計測結果から入力した。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではなく、濃淡画像や距離画像の情報を元に繰り返し計算なしに直接的に算出してもよい。例えば、画像全体から検出された線分と3次元形状モデル中の線分と対応付けて位置及び姿勢を直接的に算出してもよい。具体的には、まず画像全体に対して例えばCannyのエッジ検出器によってエッジ検出を行い、隣接するエッジ群をラベリングする。次に、同一ラベルを持つエッジ群に対して、折れ線近似によって線分当てはめを行う。このようにして得られた画像上の線分と3次元形状モデル中の線分の対応をもとに上記非特許文献6に係るLiuの手法により位置及び姿勢を算出してもよい。また、例えば、上記非特許文献7に係るJohnsonの方法により、距離画像に基づいて概略の位置及び姿勢を算出してもよい。また、例えば、上記非特許文献9に係るUlrichらの方法のように、物体の姿勢に応じた固有のパターンを画像上で網羅的に探索する方法によって位置及び姿勢の概略値を算出してもよい。固有のパターンは、濃淡値からなるテンプレート画像であってもよいし、二値のエッジテンプレート画像であってもよい。また、網羅的に探索する方法は、姿勢クラスを識別するパターン認識による方法であってもよい。
【0101】
[変形例1−4](第1の位置姿勢を濃淡+距離から算出)
上述した第1の実施形態では、前段の処理において濃淡画像のみを利用して位置及び姿勢を推定した。しかしながら、濃淡画像の情報だけでなく、誤対応が悪い影響を与えない程度に距離画像の情報を利用してもよい。具体的には、第1の実施形態のステップS1030の処理のかわりにS1040の処理を実施する。このとき、距離画像の誤対応が位置及び姿勢の算出に大きく影響しないように、数式(8)における閾値c1の値を大きくして全体的に重みが小さくなるようにする。
【0102】
(第2の実施形態)(2Dの前に奥行きを求める)
上述した第1の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定して位置及び姿勢の概略値を更新した後、3次元形状モデルを投影することによる距離画像と3次元形状モデルの対応付けを行った。しかしながら、位置及び姿勢の概略値の更新方法は、これに限るものではない。例えば、距離画像のデータを用いて、位置及び姿勢のうち、カメラからの奥行きに相当する位置成分を推定(算出)してもよい。位置及び姿勢の概略値の真の値に対する誤差が大きい場合は、3次元形状モデルを投影することにより距離画像と3次元形状モデルを正確に対応付けることは難しい。しかしながら、カメラから物体表面までの距離がほぼ同一とみなせる場合には、多少対応付けが間違っていたとしても、奥行きを真の値に近づけることは可能である。
【0103】
そこで本実施形態では、まず距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第1の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値が真の値と大きく異なる場合であっても、ロバストに位置及び姿勢を計測(算出)することが可能になる。
【0104】
図9は、本発明の第2の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
図9の各ステップのうち、ステップS2010、S2020、S2040については、それぞれ、第1の実施形態における図3のステップS1010、S1020、S1040と同一であるため、その説明を省略する。以下では、図9の処理のうち、ステップS2025、S2030について説明する。
【0105】
<ステップS2025>
ステップS2025では、例えば位置姿勢算出部160は、距離画像の情報に基づいて物体の位置及び姿勢のうち、奥行きに相当する位置成分の概略値を更新する。
【0106】
具体的な処理としては、まずステップS2010において入力される位置及び姿勢の概略値をもとに、第1の実施形態のステップS1220において説明した方法により、距離画像と3次元形状モデルとの対応付けを行う。次に、例えば位置姿勢算出部160は、対応付け結果に基づいて、物体の位置を更新する。物体の位置は、次のように更新する。ここで、基準座標系における物体の位置の概略値を表す3次元ベクトルをt0=[tx ty tz]tとする。次に、例えば位置姿勢算出部160は、モデル面との対応付けがなされている距離画像上の画素について、基準座標系の原点(カメラの投影中心)から出て当該画素を通る光線と3次元形状モデル面との交点の3次元座標を算出する。光線上の点の3次元座標xは、光線の方向ベクトルをd=[dx dy dz]tとすると、x=αdによって表される(αは媒介変数)。この点が基準座標系におけるモデル面a'x+b'y+c'z=e'上にあるとして、αを求めてxを算出することにより、交点の3次元座標を算出する。距離画像上の画素に保持されている3次元座標をxdata、算出された交点の3次元座標をxcalcとする。xcalcのz成分の平均値/zcalcがxdataのz成分の平均値/zdataに一致するように物体の位置t0を更新する。更新した位置t'は、以下の数式(21)により算出する。単純に位置t0のz成分のみを更新しないのは、物体の観察方向に応じてz成分の位置への影響が変わるためである。
【0107】
【数21】
【0108】
<ステップS2030>
画像特徴検出部140及び位置姿勢算出部160は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測(算出)を行う。基本的な処理は、第1の実施形態におけるステップS1030と同一であるが、入力する位置及び姿勢の概略値として、ステップS2025によって更新された位置及び姿勢の概略値を利用する。以降の処理の説明は省略する。
【0109】
このように、第2の実施形態では、距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第1の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値のずれが補正され、後段の処理において誤対応を削減することが可能である。
【0110】
[変形例2−1](エッジは特徴点であってもよい)
以上、説明した本発明の実施形態、変形例では、濃淡画像上の特徴はエッジであり、3次元形状モデル中の線分をエッジに当てはめることにより位置及び姿勢の計測を行った。しかしながら、濃淡画像上の特徴はエッジに限るものではなく、例えば2次元的な特徴である特徴点であってもよい。特徴点は、コーナー点など画像上で特徴的な点であり、例えば、上記非特許文献8に係るHarrisの検出器によって検出することができる。3次元形状モデルを3次元点群によって表すことで、各特徴点をモデル中の3次元点群の点と対応付けることができる。特徴点を用いる場合には、まず画像全体からHarrisの検出器によって特徴点を検出する。次に、モデル中の3次元点群の各点を位置及び姿勢の概略値に基づいて画像上に投影し、画像上で最も近い特徴点と対応付けを行う。
【0111】
(第3の実施形態)(後段の位置姿勢推定は2.5Dのみ利用)
上述した実施形態では、後段の位置及び姿勢の推定においては濃淡画像と距離画像の双方の情報を用いていた。しかしながら、後段の位置及び姿勢の推定方法はこれに限るものではない。計測対象物体が多面体である場合、少なくとも3面以上の十分な面積を持つ面が撮像装置から見えていれば、距離画像のみから精度よく位置及び姿勢を推定することができる。また、計測対象物体が曲面から構成される場合には、撮像装置から観察される物体表面上の点の法線ベクトルが十分に3次元的に分布していれば距離画像のみから精度よく位置及び姿勢を推定することができる。
【0112】
そこで本実施形態では、まず、第1の実施形態で述べたように、濃淡画像を用いて物体の位置及び姿勢の概略値を更新した後に、更新された概略値を初期値として距離画像のみを用いて位置及び姿勢を推定する。
【0113】
図10は、本発明の第3の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。
図10の各ステップのうち、ステップS3010、S3020、S3030については、それぞれ、第1の実施形態における図3のステップS1010、S1020、S1030と同一であるため、その説明を省略する。以下では、図10の処理のうち、ステップS3040について説明する。
【0114】
<ステップS3040>
ステップS3040では、位置姿勢算出部160は、ステップS3030で更新された位置及び姿勢の概略値を用いて、距離画像に対して3次元形状モデルが当てはまるような位置及び姿勢の推定(算出)を行う。ここでは、距離画像から得られる3次元点群と3次元形状モデルとの距離の二乗和が最小になるように位置及び姿勢の最適化を行う。
【0115】
図11は、図10のステップS3040における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図7のフローチャートで示されるステップS1040の処理手順から濃淡画像に対する処理を除いた処理手順であるため、説明が重複する部分は省略する。
【0116】
≪ステップS3110≫
まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS3030で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。
【0117】
≪ステップS3120≫
続いて、画像特徴検出部140は、ステップS3020で入力された距離画像と3次元形状モデルとの対応付けを行う。距離画像と3次元形状モデルとの対応付けについては、ステップS1220の処理と同じであるため、説明を省略する。
【0118】
≪ステップS3130≫
続いて、例えば位置姿勢算出部160は、ポリゴンと3次元点の対応をもとに、位置及び姿勢を算出するための係数行列・誤差ベクトルを算出する。3次元形状モデルの面と3次元点との対応に基づく係数行列・誤差ベクトルの算出方法については、ステップS1230と同じであるため、説明を省略する。本ステップでは、距離画像の情報のみを用いるため、sの各成分の微小変化Δsi(i=1,2,・・・,6)に関する線形連立方程式は以下の数式(22)になる。
【0119】
【数22】
【0120】
この数式(22)に示す線形連立方程式を、以下の数式(23)のように表す。
【0121】
【数23】
【0122】
数式(23)において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。
【0123】
≪ステップS3140≫
続いて、例えば位置姿勢算出部160は、数式(23)をもとに、位置及び姿勢の補正値Δsを算出する。距離画像と3次元形状モデルの対応付けデータに誤対応などの外れ値が混入することは避けられないため、ステップS1140で述べたようなロバスト推定手法により重み行列Wを用いて、以下の数式(24)により補正値Δsを算出する。
【0124】
【数24】
【0125】
≪ステップS3150≫
続いて、例えば位置姿勢算出部160は、ステップS3140において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。
s←s+Δs
【0126】
≪ステップS3160≫
続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS3130に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。
【0127】
以上述べたように、第3の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢の概略値を推定した後に、推定した概略値を初期値として距離画像を用いて物体の位置及び姿勢を推定する。これにより、距離画像の情報のみから高精度に位置及び姿勢を推定する場合には、後段の処理において濃淡画像に関する処理を行う必要がないため、高速に位置及び姿勢を推定することが可能になる。
【0128】
(第4の実施形態)
上述した実施形態では、濃淡画像とともに対象物体の3次元の計測データとして距離画像を位置姿勢推定に利用していた。しかしながら、位置姿勢推定に利用する3次元の計測データは画素ごとに3次元座標を持つ距離画像に限るものではなく、物体の表面上における点群の3次元座標を表す計測データであれば他の形態であってもよい。この場合、位置姿勢計測装置100は、物体の表面上における点群の3次元座標を入力する3次元データ入力部(不図示)を具備する構成となる。そして、位置姿勢算出部160は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出する。さらに、位置姿勢算出部160は、第1の位置及び姿勢をもとに、3次元データ入力部(不図示)から得られる3次元座標の点群である3次元点群または当該3次元点群及び濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する。ここで、例えば、物体の表面上における点群の3次元座標を表す計測データとしては、3次元座標の集合であってもよい。なお、本実施形態と他の実施形態との違いは、3次元形状モデルと3次元の計測データの対応付けだけであるため、以下では対応付け方法のみを説明し、重複する説明は割愛する。
【0129】
ステップS1220の説明において述べたように、3次元の計測データが距離画像の場合には、物体の位置及び姿勢の概略値に基づいて3次元形状モデルを距離画像上に投影することで距離画像上の各画素と3次元形状モデルのポリゴンとの対応づけを行った。3次元計測データが3次元座標の集合である場合には画像上への投影を行うことができないため、各計測点に最も近いポリゴンを3次元的に探索する。
【0130】
具体的な方法を以下に示す。
まず、予め3次元形状モデルを構成する各ポリゴン上に計測点の密度より細かく複数の点(以降、面モデル点と呼ぶ)を設定し、各面モデル点の物体座標系における3次元座標と属するポリゴンのIDを保存しておく。対応付けを行う際には、非特許文献2に開示されるように、kd−treeを用いて各計測点に対応する面モデル点を探索する。面モデル点にはポリゴンのIDが紐づけられているため、この探索を行うことで計測点とポリゴンが対応付けされる。
【0131】
以上述べたように、第4の実施形態では、対象物体の3次元の計測データが画素ごとに3次元座標を持つ距離画像でなくとも、本発明による位置姿勢推定を適用することが可能である。
【0132】
上述した本発明の各実施形態では、位置姿勢算出部160は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出する(図3のS1030、図9のS2030、図10のS3030)。また、位置姿勢算出部160は、第1の位置及び姿勢を用いて算出される3次元形状モデルの画像面上での投影位置をもとに、距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する(図3のS1040、図9のS2040、図10のS3040)。
【0133】
(その他の実施形態)
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア(プログラム)を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ(またはCPUやMPU等)がプログラムを読み出して実行する処理である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記録媒体は、本発明に含まれる。
【産業上の利用可能性】
【0134】
本発明は、例えば、ロボットの自己位置推定や、ロボットと物体との間の相対的な位置及び姿勢を推定するのに利用することができる。
【符号の説明】
【0135】
100 位置姿勢計測装置、110 3次元形状モデル保存部、120 概略位置姿勢入力部、130 濃淡画像入力部、140 画像特徴検出部、150 距離画像入力部、160 位置姿勢算出部、200 濃淡画像撮像部、300 距離計測部
【特許請求の範囲】
【請求項1】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記物体の3次元形状モデルを保存する保存手段と、
前記物体の濃淡画像を入力する濃淡画像入力手段と、
前記物体の距離画像を入力する距離画像入力手段と、
当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段と
を有することを特徴とする位置姿勢計測装置。
【請求項2】
前記第1の位置及び姿勢は、位置及び姿勢の一部の成分であることを特徴とする請求項1に記載の位置姿勢計測装置。
【請求項3】
前記第1の位置及び姿勢は、当該位置姿勢計測装置のカメラの光軸に垂直な位置及び光軸まわりの姿勢であることを特徴とする請求項2に記載の位置姿勢計測装置。
【請求項4】
前記概略位置姿勢入力手段は、前記濃淡画像または前記距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される前記物体の位置及び姿勢を概略の位置及び姿勢として入力することを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。
【請求項5】
前記位置姿勢算出手段は、前記第1の位置及び姿勢を算出する前に、前記距離画像の情報を用いて位置及び姿勢のうち、当該位置姿勢計測装置からの奥行きに相当する位置を算出し、前記概略の位置及び姿勢を更新することを特徴とする請求項1乃至4のいずれか1項に記載の位置姿勢計測装置。
【請求項6】
前記画像特徴群における画像特徴は、エッジであることを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。
【請求項7】
前記画像特徴群における画像特徴は、特徴点であることを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。
【請求項8】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記物体の3次元形状モデルを保存する保存手段と、
前記物体の濃淡画像を入力する濃淡画像入力手段と、
前記物体の表面上における点群の3次元座標を入力する3次元データ入力手段と、
当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力手段から得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段と
を有することを特徴とする位置姿勢計測装置。
【請求項9】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、
前記物体の3次元形状モデルを保存する保存ステップと、
前記物体の濃淡画像を入力する濃淡画像入力ステップと、
前記物体の距離画像を入力する距離画像入力ステップと、
前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出ステップと
を有することを特徴とする位置姿勢計測方法。
【請求項10】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、
前記物体の3次元形状モデルを保存する保存ステップと、
前記物体の濃淡画像を入力する濃淡画像入力ステップと、
前記物体の表面上における点群の3次元座標を入力する3次元データ入力ステップと、
前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力ステップから得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出ステップと
を有することを特徴とする位置姿勢計測方法。
【請求項1】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記物体の3次元形状モデルを保存する保存手段と、
前記物体の濃淡画像を入力する濃淡画像入力手段と、
前記物体の距離画像を入力する距離画像入力手段と、
当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段と
を有することを特徴とする位置姿勢計測装置。
【請求項2】
前記第1の位置及び姿勢は、位置及び姿勢の一部の成分であることを特徴とする請求項1に記載の位置姿勢計測装置。
【請求項3】
前記第1の位置及び姿勢は、当該位置姿勢計測装置のカメラの光軸に垂直な位置及び光軸まわりの姿勢であることを特徴とする請求項2に記載の位置姿勢計測装置。
【請求項4】
前記概略位置姿勢入力手段は、前記濃淡画像または前記距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される前記物体の位置及び姿勢を概略の位置及び姿勢として入力することを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。
【請求項5】
前記位置姿勢算出手段は、前記第1の位置及び姿勢を算出する前に、前記距離画像の情報を用いて位置及び姿勢のうち、当該位置姿勢計測装置からの奥行きに相当する位置を算出し、前記概略の位置及び姿勢を更新することを特徴とする請求項1乃至4のいずれか1項に記載の位置姿勢計測装置。
【請求項6】
前記画像特徴群における画像特徴は、エッジであることを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。
【請求項7】
前記画像特徴群における画像特徴は、特徴点であることを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。
【請求項8】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記物体の3次元形状モデルを保存する保存手段と、
前記物体の濃淡画像を入力する濃淡画像入力手段と、
前記物体の表面上における点群の3次元座標を入力する3次元データ入力手段と、
当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力手段から得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段と
を有することを特徴とする位置姿勢計測装置。
【請求項9】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、
前記物体の3次元形状モデルを保存する保存ステップと、
前記物体の濃淡画像を入力する濃淡画像入力ステップと、
前記物体の距離画像を入力する距離画像入力ステップと、
前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出ステップと
を有することを特徴とする位置姿勢計測方法。
【請求項10】
計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、
前記物体の3次元形状モデルを保存する保存ステップと、
前記物体の濃淡画像を入力する濃淡画像入力ステップと、
前記物体の表面上における点群の3次元座標を入力する3次元データ入力ステップと、
前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、
前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力ステップから得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出ステップと
を有することを特徴とする位置姿勢計測方法。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【公開番号】特開2012−141962(P2012−141962A)
【公開日】平成24年7月26日(2012.7.26)
【国際特許分類】
【出願番号】特願2011−267109(P2011−267109)
【出願日】平成23年12月6日(2011.12.6)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
【公開日】平成24年7月26日(2012.7.26)
【国際特許分類】
【出願日】平成23年12月6日(2011.12.6)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
[ Back to top ]