JP5938201B2 - 位置姿勢計測装置、その処理方法及びプログラム - Google Patents

位置姿勢計測装置、その処理方法及びプログラム Download PDF

Info

Publication number
JP5938201B2
JP5938201B2 JP2011267255A JP2011267255A JP5938201B2 JP 5938201 B2 JP5938201 B2 JP 5938201B2 JP 2011267255 A JP2011267255 A JP 2011267255A JP 2011267255 A JP2011267255 A JP 2011267255A JP 5938201 B2 JP5938201 B2 JP 5938201B2
Authority
JP
Japan
Prior art keywords
orientation
model
image
sensitivity
dimensional shape
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2011267255A
Other languages
English (en)
Other versions
JP2013120091A (ja
Inventor
大輔 渡邊
大輔 渡邊
小竹 大輔
大輔 小竹
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Canon Inc
Original Assignee
Canon Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Canon Inc filed Critical Canon Inc
Priority to JP2011267255A priority Critical patent/JP5938201B2/ja
Publication of JP2013120091A publication Critical patent/JP2013120091A/ja
Application granted granted Critical
Publication of JP5938201B2 publication Critical patent/JP5938201B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Length Measuring Devices By Optical Means (AREA)
  • Image Processing (AREA)
  • Image Analysis (AREA)

Description

本発明は、位置姿勢計測装置、その処理方法及びプログラムに関する。
近年、ロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行なっていた複雑なタスクをロボットが代わりに行なうようになりつつある。ロボットが、ハンドなどのエンドエフェクタによって部品を把持して組立を行なうためには、把持の対象となる部品とロボット(ハンド)との間の相対的な位置姿勢を計測する必要がある。
ここで、物体の位置姿勢を計測する方法として、カメラで撮影した濃淡画像やカラー画像上から検出される画像特徴や、距離センサで計測して得られた距離データに対して物体の3次元形状モデルをあてはめるモデルフィッティングによる方法が知られている。
モデルフィッティングによる位置姿勢の計測(推定)では、モデルの幾何特徴と画像上の幾何特徴とを利用して計測対象物体の位置姿勢を表す複数のパラメータを推定する。このとき、あるパラメータの推定に寄与する幾何特徴が他のパラメータの推定に寄与する幾何特徴に比べて著しく少ないと、そのパラメータをロバストに推定できないという問題がある。
この問題に対して、非特許文献1では、寄与する推定パラメータごとに各幾何特徴を分類して用いる方法が開示されている。この方法では、位置姿勢の計測時に、各幾何特徴のヤコビアンを算出し、ヤコビアンが類似しているものは同一パラメータに寄与するものとしてまとめて分類する。そして、このようにして分類した幾何特徴を、それぞれ寄与するパラメータの算出のみに分けて使用することで、特定のパラメータに寄与する幾何特徴が少ない場合においても、ロバスト且つ効率良く推定を行なっている。
Kemp C. , Drummond T. , "Dynamic measurement clustering to aid real time tracking", Computer Vision, 2005. ICCV 2005. Tenth IEEE International Conference on, Page(s): 1500-1507 Vol. 2
ここで、他のパラメータに寄与する幾何特徴に比べて、特定のパラメータに寄与する幾何特徴が少ない物体の一例について挙げてみる。例えば、工業製品に使われる部品の中で考えると、回転対称に近い形状で、且つその回転成分を一意に決定するための幾何特徴が微小な切り欠きや突起しかない部品が挙げられる。
このような部品では、位置姿勢を規定する6パラメータを同時に推定しようとすると、他の成分に比べて、特定の成分(この場合、物体の回転対称性を定義する軸まわりの姿勢)の推定に必要な情報が不足してしまう。
そのため、この特定の成分を推定するのが困難となり、その結果、位置姿勢のロバスト性や精度が低くなるという問題がある。なお、上述した非特許文献1の方法では、カメラを基準とした座標系で規定される位置姿勢成分に基づいて幾何特徴を分類するため、物体(3次元形状モデル)を基準とした座標系で規定される特定の位置姿勢の成分に寄与する幾何特徴を分離することができない。
本発明は、上記課題に鑑みてなされたものであり、情報の不足により物体基準で定義される特定の位置姿勢の成分(パラメータ)を推定するのが困難な物体においても、ロバスト且つ高速に位置姿勢の計測を行なえるようにした技術を提供することを目的とする。
上記課題を解決するため、本発明の一態様は、計測対象となる物体の位置姿勢を計測する位置姿勢計測装置であって、
前記物体の3次元形状モデルを保持するモデル保持手段と、
前記3次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを設定する特定パラメータ設定手段と、
前記3次元形状モデルの幾何特徴について、該幾何特徴の方向と、前記特定のパラメータを変化させたときの該幾何特徴の移動方向と、に基づいて、該特定のパラメータ算出時に与える影響を示す感度を算出する感度算出手段と、
前記3次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する位置姿勢算出手段と
を具備し、
前記位置姿勢算出手段は、
前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記3次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する
ことを特徴とする
本発明によれば、情報の不足により物体基準で定義される特定の位置姿勢の成分を推定するのが困難な物体においても、ロバスト且つ高速に位置姿勢の計測を行なえる。
本発明の一実施の形態に係わる位置姿勢計測装置10の構成の一例を示す図。 3次元形状モデルの定義方法の一例を示す図。 位置姿勢計測装置10の処理の流れの一例を示すフローチャート。 計測対象物体の形状とその座標系の一例を示す図。 図3のS106の処理の流れの一例を示すフローチャート。 エッジ検出方法の概要の一例を示す図。 3次元形状モデルの線分の投影像とエッジとの関係の一例を示す図。 図3のS107の処理の流れの一例を示すフローチャート。 エッジ検出方法の概要の一例を示す図。 変形実施形態に係わる計測対象物体の形状とその座標系の一例を示す図。 距離画像と3次元形状モデルとの対応付け処理の概要を示す図。
以下、図面を参照して本発明の好適な実施形態について詳細に説明する。
(実施形態1)
まず、実施形態1について説明する。実施形態1においては、特定の軸に対してほぼ回転対称であり、且つその軸回りの回転を決めるための特徴が少ない形状を有する物体の位置姿勢の計測(推定)を行なう場合について説明する。
ここで、実施形態1に係わる計測対象物体の位置姿勢の計測(推定)方法の概要について簡単に説明する。
実施形態1においては、2次元画像上のエッジに対してモデルのエッジをあてはめることにより6自由度の位置姿勢を算出する。但し、上述した形状の物体では、6自由度の位置姿勢を同時に算出すると、物体の軸回りの回転を決定する特徴が少ないため、回転軸まわりの位置姿勢パラメータが正しく求まらない。
そこで、実施形態1においては、まず、6自由度の位置姿勢を算出し(前段の処理)、その後、軸回りの回転成分の推定に影響するエッジのみを利用して、軸回りの回転成分のみ別途算出を行なう(後段の処理)。前段処理後には、軸回りの回転成分を除き、正しい位置姿勢が算出されていることが期待される。そのため、後段の処理では、軸回りの回転成分の推定に影響力のある3次元形状モデルの幾何特徴のみを利用することで、ロバスト且つ効率良く位置姿勢の推定を行なう。
図1は、本発明の一実施の形態に係わる位置姿勢計測装置10の構成の一例を示す図である。
位置姿勢計測装置10は、3次元情報(本実施形態においては、物体の位置姿勢)の計測対象となる物体(以下、計測対象物体、又は単に物体と呼ぶ場合もある)の位置姿勢を計測する。位置姿勢計測装置10には、コンピュータが内蔵されている。コンピュータには、CPU(Central Processing Unit)等の主制御手段、ROM(Read Only Memory)、RAM(Random Access Memory)、HDD(Hard Disk Drive)等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていても良い。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。
ここで、位置姿勢計測装置10は、モデル保持部11と、特定パラメータ指定部12と、感度算出部13と、撮像部14と、概略値入力部15と、位置姿勢算出部18とを具備して構成される。
モデル保持部11は、計測対象物体の3次元幾何モデルデータ(以下、3次元形状モデルと呼ぶ)を保持する。ここで、図2(a)及び図2(b)を用いて、本実施形態に係わる3次元形状モデルの定義方法の一例について説明する。3次元形状モデルは、点の集合や各点を結んで構成される線分の集合によって定義される。
本実施形態に係わる3次元形状モデルは、図2(a)に示すように、点P1〜点P15の15点から構成される。点P1〜点P15は、3次元座標値により表される。また、本実施形態に係わる3次元形状モデルは、図2(b)に示すように、線分L1〜L21により構成されている。線分L1〜L21は、線分を構成する点のID(識別子)により表される。また、ここでは図示を省略するが、3次元形状モデルは、面の情報も保持する。各面は、当該各面を構成する点のIDにより表される。この図2(a)及び図2(b)に示す3次元形状モデルでは、物体を構成する8つの面の情報が保持されている。
なお、図2(a)及び図2(b)に示す3次元形状モデルにおいて、p19〜L21が軸回りの回転成分を一意に決める特徴となる。3次元形状モデルは、特定パラメータ指定部12による第2の位置姿勢パラメータの指定や、感度算出部13によるモデル幾何特徴の感度算出に用いられる。その他、第1の位置姿勢算出部16による第1の位置姿勢パラメータの算出や、第2の位置姿勢算出部17による第2の位置姿勢パラメータの算出にも用いられる。
特定パラメータ指定部12は、第2の位置姿勢算出部17による算出対象となる位置姿勢パラメータの成分(第2の位置姿勢パラメータ)を指定する。以下の説明においては、第1の位置姿勢算出部16及び第2の位置姿勢算出部17により推定される位置姿勢パラメータの成分をそれぞれ第1の位置姿勢パラメータ及び第2の位置姿勢パラメータと呼ぶ。具体的には、3次元形状モデルの座標系(モデル座標系)を基準とした物体の位置姿勢(6パラメータ)を第1の位置姿勢パラメータとする。更に、6パラメータのうち、そのパラメータの推定に寄与する幾何特徴が物体形状において少ないために算出が困難なパラメータを第2の位置姿勢パラメータとする。本実施形態においては、計測対象となる物体として、ほぼ回転対象で、且つその回転軸周りの姿勢成分を決める幾何特徴が少ない形状を有する物体の位置姿勢の計測(推定)を行なう場合について説明する。そのため、物体の回転軸周りの姿勢成分を第2の位置姿勢パラメータとして求める。
感度算出部13は、特定パラメータ指定部12により指定された第2の位置姿勢パラメータに対する感度を算出する。この感度は、3次元形状モデルを構成する各幾何特徴(各エッジ線分)に対応して算出される。本実施形態で用いるエッジを利用した位置姿勢の計測では、3次元形状モデルのエッジと、それに対応付けられた2次元画像上の点との間の距離を最小化するようにして物体の位置姿勢の推定を行なう。従って、第2の位置姿勢パラメータを微小変化させたときの線分の移動方向と、線分自身の持つ方向とが平行となる場合、その線分に属するエッジは、第2の位置姿勢パラメータの推定に寄与しない。
そこで、感度算出部13においては、第2の位置姿勢パラメータを微小変化させたときの3次元形状モデルの線分の移動方向と、線分自身の持つ方向との差を感度として算出する。感度算出部13により算出された感度は、第2の位置姿勢算出部17による第2の位置姿勢パラメータの算出に際して、各幾何特徴の寄与度を設定する際に用いられる。
撮像部14は、例えば、カメラであり、計測対象物体の2次元画像を撮像する。カメラの内部パラメータ(焦点距離、主点位置、レンズ歪みパラメータ)は、例えば、「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」に開示された方法により事前に校正しておけば良い。
概略値入力部15は、位置姿勢計測装置10に対する計測対象物体の位置姿勢の概略情報(以下、概略値と呼ぶ)を入力する。位置姿勢計測装置10には、位置姿勢の計測の基準となる3次元の座標系(基準座標系)が予め規定されているものとする。本実施形態においては、撮像部14の中心を原点とし、画像の水平方向をx軸とし、垂直方向をy軸とし、撮像部14の光軸をz軸とした座標系を基準座標系とする。これと同様に、物体の3次元形状モデルにも、位置姿勢の計測の基準となる3次元の座標系(モデル座標系、すなわち、物体を基準とした物体座標系)が予め規定されているものとする。なお、位置姿勢計測装置10に対する物体の位置姿勢とは、基準座標系における物体の位置姿勢を示す。
本実施形態においては、位置姿勢計測装置10は、時間軸方向に連続して計測を行なうものとして、位置姿勢計測装置10が当該物体から過去(例えば、直前)に計測した計測値を概略値として用いる。なお、物体の位置姿勢の概略値は、必ずしもこのような値である必要はない。例えば、過去に計測した物体の位置姿勢の計測値に基づいて、物体の位置姿勢の変化量を推定し、過去の位置姿勢と推定された変化量とから現在の位置姿勢を予測することによりこのような概略値を求めるようにしても良い。また、例えば、物体が置かれているおおよその位置や姿勢が予めわかっている場合にはその値を概略値として用いても良い。この概略値は、第1の位置姿勢算出部16による第1の位置姿勢パラメータの算出に用いられる。
位置姿勢算出部18は、計測対象物体の位置姿勢を計測(推定)する機能を果たし、第1の位置姿勢算出部16と、第2の位置姿勢算出部17とを具備して構成される。
第1の位置姿勢算出部16は、概略値入力部15により入力された概略値に基づいて、モデル保持部11に保持されたモデル幾何特徴と、撮像部14により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、計測対象物体の位置姿勢を表す第1の位置姿勢パラメータ(本実施形態においては、6パラメータ)を算出する。以下、第1の位置姿勢パラメータの算出により表される物体の位置姿勢を第1の位置姿勢と呼ぶ。
第2の位置姿勢算出部17は、第1の位置姿勢に基づいて、モデル保持部11に保持されたモデル幾何特徴と、撮像部14により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、計測対象物体の第2の位置姿勢パラメータを算出する。このとき、第2の位置姿勢算出部17は、感度算出部13により算出された感度に基づき、第2の位置姿勢パラメータの推定に影響し易い幾何特徴の寄与度を上げて第2の位置姿勢パラメータの算出を行なう。これにより、ロバスト且つ効率良く算出を行なうことができる。以下、第2の位置姿勢パラメータの算出により推定された物体の位置姿勢を第2の位置姿勢と呼ぶ。
ここで、図3を用いて、図1に示す位置姿勢計測装置10における物体の位置姿勢の計測処理の流れの一例について説明する。なお、位置姿勢計測装置10は、図3に示すS104〜S107の一連の処理を繰り返すことで、時間軸方向に連続して物体の位置姿勢の計測を行なう。
[S101]
位置姿勢計測装置10は、まず、モデル保持部11に保持された3次元形状モデルの各幾何特徴を取得する。より具体的には、特定パラメータ指定部12において、3次元形状モデルの各幾何特徴を入力する。
[S102]
位置姿勢計測装置10は、特定パラメータ指定部12において、3次元形状モデルの各幾何特徴に基づいて、第2の位置姿勢パラメータを指定する。ここで、図4(a)には、本実施形態において対象とする物体の具体例とその座標系とが示される。本実施形態においては、図4(a)に示すように、物体が、特定軸に対してほぼ回転対称な形状を有することを前提とする。特定パラメータ指定部12においては、まず、この物体の回転対称性を定義する軸(以下、モデル中心軸と呼ぶ)を指定する。
モデル中心軸の指定は、例えば、ユーザからの操作部(例えば、マウスやキーボード)を介した入力に基づいて行なう。具体的には、図4(a)に示すような3次元形状モデルの場合、当該モデルの回転対称な面上から対面する2点a、b及びc、dの2組をユーザからの入力として取得する。このとき、線分abと線分cdとの交点を通り、且つ四角形acbdに垂直な方向を持つベクトルをモデル中心軸として算出する(図4(b)参照)。
但し、モデル中心軸の指定方法は、このような方法に限られない。例えば、三角柱のような回転対称形状の場合には、回転対称な三角形の3頂点をユーザからの入力として取得し、当該三角形の重心を通り、且つ三角形に垂直なベクトルをモデル中心軸として指定しても良い。或いは、モデル面上で回転対称性のある面(図4(a)の場合、八角形)をユーザからの入力として取得し、その重心を通り、且つ入力された面に垂直なベクトルをモデル中心軸としても良い。その他、モデル形状の回転対称性を定義する回転軸を算出できるのであれば、いかなる方法を用いても構わない。
モデル中心軸の指定が済むと、特定パラメータ指定部12は、モデル中心軸に対して互いに直交する2つの軸を設定し、モデル中心軸と、当該設定した2つの軸とが座標軸となるようにモデル座標系を変換して新しいモデル座標系とする(図4(c))。ここでは、新しいモデル座標系において、y軸(実線)がモデル中心軸と一致するようにする。
なお、モデル座標系の変換方法は、このような方法に限られない。例えば、モデル座標系のx軸がモデル中心軸と一致するように変換しても良い。その他、モデル中心軸がモデル原点を通るようにモデル座標系が変換されるのであれば、いかなる変換であっても構わない。以降の処理は、y軸がモデル中心軸と一致するように変換した、新しいモデル座標系を用いて行なうものとする。基準座標系に対する物体の位置姿勢を6次元ベクトルs=[tx,ty,tz,θ,φ,ψ]で表す。
ここで、tx、ty、tzは、それぞれx、y、z軸方向の位置(並進)成分を表し、θ、φ、ψは、それぞれx、y、z軸に対する姿勢(回転)成分を表す。この6パラメータが、第1の位置姿勢パラメータとなる。この各回転成分の変換順序を考慮することで、座標変換において、物体軸回りの回転を分離する。より具体的には、モデル座標系上の特定軸回りの回転を表す行列のみを分離して、モデル座標系に直接積算することで、モデル座標系における回転を表現する。続いて、特定パラメータ指定部12は、モデル中心軸に対する回転成分を第2の位置姿勢パラメータとして指定する。本実施形態においては、y軸がモデル中心軸と一致するため、y軸回りの回転成分であるφが第2の位置姿勢パラメータとして指定される。
[S103]
位置姿勢計測装置10は、感度算出部13において、第2の位置姿勢パラメータを算出する際の各モデル線分(3次元形状モデルにおける各幾何特徴)の感度を算出する。具体的には、3次元形状モデル上の2つの3次元点ps、peとを結ぶ線分Lj(j=1,2,・・・,N)(N:モデル上の線分の数)の感度wjを次のように算出する。
まず、点psをモデル中心軸に対してφだけ微小(例えばφ=0.01 radian)回転させた点をps’で表すものとする。モデル中心軸回りにφ微小回転させる回転行列をRφと表すと、ps’は、以下の「数1」で示される。
ps’=Rφps (数1)
ここで、点psから点ps’へ向かう長さ1の3次元ベクトルをvec1とする。更に、点psから点peへ向かう長さ1の3次元ベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1・vec2を用いて、以下の「数2」により定義する。
wj=1−|vec1・vec2| (数2)
「数2」により算出された各幾何特徴の感度wjは、0〜1の間の値を持つ。位置姿勢計測装置10においては、この感度wjを各モデル線分Ljと組にして保持する。なお、S101〜S103の処理は、オフラインでの処理として予め行なっておいても良い。オフラインで事前に処理を行なうことで、位置姿勢算出時の処理を高速に行なうことができる。
[S104]
位置姿勢計測装置10は、撮像部14において、計測対象物体の2次元画像を撮像し、第1の位置姿勢算出部16及び第2の位置姿勢算出部17に入力する。
[S105]
位置姿勢計測装置10は、概略値入力部15において、位置姿勢計測装置10に対する物体の位置姿勢の概略値を入力する。本実施形態においては、計測対象物体から過去(例えば、直前)に計測された計測値を概略値として入力する。
[S106]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、2次元画像の情報に基づいて、第1の位置姿勢を算出する。本実施形態においては、2次元画像上の画像特徴(エッジ)に3次元形状モデル中の線分の投影像を当てはまめるようにすることで第1の位置姿勢パラメータを算出し、それにより、第1の位置姿勢を算出する。
ここで、本実施形態においては、2次元画像上で検出されたエッジと、概略値に基づいて当該2次元画像上に投影される3次元形状モデルの線分との距離の二乗和が最小になるべく、計測対象物体の位置姿勢のパラメータsの最適化を行なう。
このとき、位置姿勢に基づく、モデル座標系(物体座標系)の3次元点X=[x,y,zから基準座標系の3次元点X=[x,y,zへの座標変換は、「数3」のように表される。
=RX+T (数3)
但し、Rは物体の姿勢を表す3つの成分θ、φ、ψで表現される3×3行列であり、Tはモデルの位置を表す3つの成分から成る3次元ベクトルT=[tx,ty,tz]である。更に、概略位置姿勢sに対する物体の位置姿勢の微小変化量を6次元ベクトルΔs=[Δtx,Δty,Δtz,Δθ,Δφ,Δψ]とし、微小変化量Δsを求めて、各成分を補正する処理を繰り返す。これにより、計測対象物体の位置姿勢の算出を行なう。
ここで、図5を用いて、S106の処理における第1の位置姿勢パラメータの算出処理の詳細について説明する。
[S201]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、初期化処理を行なう。この初期化処理では、例えば、図3のS105の処理で得られた計測対象物体の概略的な位置姿勢を示す概略値を入力する。
(S202)
位置姿勢計測装置10は、第1の位置姿勢算出部16において、S104の処理で入力された2次元画像上から画像特徴を検出し、当該画像特徴と、3次元形状モデルのモデル幾何特徴との対応付けを行なう。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。本実施形態においては、「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.」に開示された技術を用いてエッジの検出を行なう。このエッジ検出方法は、3次元形状モデル側から対応するエッジを探索する、トップダウン型の方法であるため、エッジの検出と対応付けとが一度に行なわれる。
ここで、図6を用いて、本実施形態に係わるエッジ検出方法の概要の一例について説明する。まず、計測対象物体が撮像された2次元画像に対して3次元形状モデルを投影する。この投影像(線分)は、S201の処理で入力された計測対象物体の位置姿勢の概略値と、校正済みのカメラの内部パラメータとに基づいて求められる。
次に、投影された各線分上において、等間隔に制御点を設定する。そして、投影線分の法線方向に平行で且つ制御点を通過する線分(以下、探索ライン)上からエッジを1次元探索する(図6(a))。エッジは、画素値の濃度勾配の極値として検出されるため、複数のエッジが検出される場合がある(図6(b))。本実施形態においては、検出されるエッジのうち、最も濃度勾配の大きいものを対応するエッジとして検出する。
[S203]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、3次元形状モデルの線分上の制御点と2次元画像上のエッジとの対応関係に基づいて、計測対象物体の位置姿勢を算出するための係数行列・誤差ベクトルを算出する。
ここで、図7は、線分の投影像と検出されたエッジとの関係の一例を示す図である。この場合、画像の水平方向をu軸とし、垂直方向をv軸としている。ある制御点(投影された各線分を画像上で等間隔に分割した点)の画像上での位置を(u,v)と表し、当該制御点が属する線分の画像上での傾きを(u軸に対する傾き)θと表す。傾きθは、線分の両端の3次元座標をsの概略値(S)に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。線分の画像上での法線ベクトルは(sinθ,−cosθ)となる。また、制御点と対応する対応点(エッジ)の座標を(u’,v’)とする。ここで、点(u’,v’)を通り、傾きがθである直線(図7の破線)上の点(u,v)は、
usinθ−vcosθ=d (数5)
と表せる。
なお、θは定数とする。また、d=u’sinθ−v’cosθ(定数)である。
画像上における制御点の位置は、計測対象物体の位置姿勢sにより変化する。従って、制御点の座標(u,v)は、(u,v)の近傍での1次のテイラー展開によって「数6」のように近似できる。但し、Δs(i=1,2,...,6)はsの各成分の微小変化を表す。つまり、Δs=Δtx、Δs=Δty、Δs=Δtz、Δs=Δθ、Δs=Δφ、Δs=Δψである。
Figure 0005938201
正しいsによって得られる画像上での制御点の位置が、「数5」が表す直線上にあると仮定すると、「数6」を「数5」に代入することにより、観測方程式(数7)が得られる。
Figure 0005938201
但し、r=usinθ−vcosθ(定数)である。
「数7」の観測方程式は、対応付けが行なわれた全ての制御点について立式することができる。ここで、「数7」は、sの各成分の微小変化Δs(i=1,2,・・・,6)についての方程式となっているため、「数8」に示すΔsに関する線形連立方程式を立式できる。
Figure 0005938201
正規化画像座標系における画像座標u、vは、それぞれu=x/z、v=y/zで表される。従って、「数3」に基づいて算出されたu、vから、左辺の行列の各要素を算出する。この線形連立方程式を「数9」のように表す。
Δs=E (数9)
左辺のJが算出対象となる係数行列であり、右辺のEが誤差ベクトルである。
[S204]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、計測対象物体の位置姿勢の補正値Δsを求める。すなわち、行列Jの一般化逆行列(J ・J−1・J を用いて、「数9」を「数10」のように解く。
Δs=(J ・J−1・J (数10)
[S205]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、S204の処理で算出された補正値Δsを用いて、概略値を補正する。
s←s+Δs
[S206]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、収束しているか否かの判定を行なう。収束していれば、この処理は終了し、そうでなければ、再度、S203の処理に戻る。なお、収束しているか否かは、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ0である場合に収束したと判定する。
以上の処理によって、図3のS105の処理で入力された概略値が、2次元画像の情報を用いて更新される。本実施形態においては、図5に示す処理で最終的に算出された物体の位置姿勢を第1の位置姿勢とする。なお、S106(S201〜S206)において、第1の位置姿勢パラメータとして、位置姿勢の6自由度を算出したが、これに限られない。例えば、第1の位置姿勢パラメータとして、第2の位置姿勢パラメータを除く5自由度についてのみ算出するようにしても良い。なお、本実施形態では、概略位置姿勢(概略値)を更新することで第1の位置姿勢を算出する場合について説明したが、第1の位置姿勢の算出方法はこれに限らない。例えば、物体をさまざまな姿勢で観察した画像をテンプレートとして多数用意しておき、このテンプレートと観察画像とのマッチングにより第1の位置姿勢を算出しても良い。この他、第2の位置姿勢パラメータを除く5自由度が正しく推定できる方法であれば、いかなる方法であっても構わない。
[S107]
図3の説明に戻る。S106の処理で算出された第1の位置姿勢に関して、第2の位置姿勢パラメータφを除き、正しい位置姿勢が算出されていることが期待される。そこで、位置姿勢計測装置10は、第1の位置姿勢を概略位置姿勢(計測対象物体の位置姿勢を示す概略値)として、第2の位置姿勢パラメータφを算出する。このとき、第2の位置姿勢パラメータφの推定に影響し易い幾何特徴を利用することで、ロバスト且つ効率良く推定を行なう。
第1の位置姿勢を計測対象物体の概略値とした場合、S106の処理と同様に、「数5」の関係が成り立つ。ここで、モデル座標系を基準としたy軸方向に対するΔφの微小回転を表す3×3行列をΔRとする。このとき、第1の位置姿勢からモデル座標系のy軸方向にΔφだけ微小回転させたときの、モデル座標系(物体座標系)の3次元点Xm=[x,y,zから基準座標系の3次元点X=[x,y,zへの座標変換は、「数11」のように表される。
=RΔR+T (数11)
そこで、S107の処理では、微小変化量Δφを求めて、モデルのy軸回転方向の補正を行なう。これにより、計測対象物体の位置姿勢の算出を行なう。
図8を用いて、S107の処理における第2の位置姿勢パラメータの算出処理の詳細について説明する。この処理では、S103の処理で算出された3次元形状モデルの幾何特徴(各モデル線分)毎の感度に基づき、当該各モデル線分毎に寄与度を変えて最適化を行なう。
[S301]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、初期化処理を行なう。この初期化処理では、例えば、図3のS106の処理で得られた第1の位置姿勢を計測対象物体の概略的な位置姿勢を示す概略値として入力する。
[S302]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、S104の処理で入力された2次元画像上から画像特徴を検出し、当該画像特徴と、3次元形状モデルのモデル幾何特徴との対応付けを行なう。すなわち、図5のS202と同様の処理を行なう。なお、図5のS202の処理では、モデル線分を画像上に投影して得られる各制御点から、当該制御点を通り且つ投影された線分に直交する線分上で1次元のエッジ検出を行なっていた。これに対して、S302の処理では、第1の位置姿勢に関して、第2の位置姿勢パラメータφを除き、正しい位置姿勢が算出されていることが期待される。従って、第1の位置姿勢に基づいて、各制御点に対応する幾何特徴の存在しうる軌跡を算出したものをエッジ検出の探索範囲としても良い。具体的には、第1の位置姿勢を基準として第2の位置姿勢パラメータφのみ変化させた場合、各制御点の軌跡は、楕円軌跡上に乗る。そこで、その楕円軌跡上で画素値の濃度勾配の極値を探索することでエッジを検出する。
図9は、第1の位置姿勢を基準として、第2の位置姿勢パラメータφを変化させたときの線分上の制御点の軌跡を示している。この楕円軌跡上で、エッジの検出を行なうことで対応する画像上のエッジが探索される。
[S303]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、微小変化するパラメータをΔφのみに限定して、図5のS203と同様の処理を行なう。これにより、3次元形状モデル線分上の制御点と2次元画像上のエッジとの対応関係から、Δφに関する「数12」のような線形連立方程式を立てることができる。
Figure 0005938201
この線形連立方程式を「数13」のように表す。
Δφ=E (数13)
左辺のJが算出対象となる係数行列であり、右辺のEが誤差ベクトルである。
[S304]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、図3のS103の処理で算出された第2の位置姿勢パラメータに対する各線分Ljの感度に基づき、各モデル幾何特徴のパラメータ算出への寄与度を重み付けする。モデル幾何特徴は、3次元形状モデル(の線分)の2次元画像上への投影像を、当該画像上で等間隔になるように分割した制御点とその方向により表される。従って、各制御点は、3次元形状モデルを構成する線分のいずれかの投影像である。そこで、3次元形状モデルの線分Ljの投影により生成された制御点には、そのモデル線分Ljと組にしておいたwjを重みとして利用する。ここで、各制御点の感度wjを用いて、「数14」のように重み行列Wを定義する。
Figure 0005938201
重み行列Wは、対角成分以外は全て0の正方行列である。対角成分(ws,wt,・・・)には、行列Jの各行に対応するモデル幾何特徴の感度wjが入る。これにより、各幾何特徴に対して、感度の値が大きいデータには大きな重みを、値が小さいデータには小さな重みを与えるようにして、Δφの推定に寄与しないデータの影響を抑制する。なお、各幾何特徴の重みの設定方法は、これに限られない。例えば、wjの代わりに、wj×wjを用いても良い。この他、各幾何特徴が特定軸回転方向の推定に寄与する度合いを感度の値に基づいて設定できる方法であれば、いかなる方法であっても構わない。この重み行列Wを用いて、「数13」を「数15」のように変形する。
WJΔφ=WE (数15)
[S305]
位置姿勢計測装置10は、「数15」を「数16」のように解くことにより補正値Δφを求める。
Δφ=(J WJ−1 WE (数16)
[S306]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、S305の処理で算出された位置姿勢の補正値ΔRにより、概略値Rを補正する。
R←RΔR
[S307]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、収束しているか否かの判定を行なう。収束していれば、この処理は終了し、そうでなければ、再度、S302の処理に戻る。なお、収束しているか否かは、補正値Δφがほぼ0である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ0である場合に収束したと判定する。
以上説明したように実施形態1によれば、特定の軸に対してほぼ回転対称であり、その軸回りの回転を決めるための特徴が少ない形状を有する物体に対しても、ロバスト且つ効率良く位置姿勢の計測が行なえる。具体的には、前段の処理において、物体の位置姿勢を推定し、後段の処理において、モデル座標系を基準とした特定軸回転方向に限定した位置合わせを行なう。後段の処理では、モデル座標系を基準とした特定軸回転成分の推定への影響度に応じて、各幾何特徴を重み付けを行なう。
なお、本実施形態においては、特定の軸周りの回転を決めるための特徴が少ない形状を有する物体を対象とした。しかし、特定の軸周りの回転を決めるための特徴の数の大小にかかわらず、本手法は応用可能である。すなわち、特定の軸に対してほぼ回転対称であり、その軸回りの回転を決めるための特徴を有する物体であれば、本手法を適用することで、特定の軸周りの回転成分をロバストかつ効率良く推定することができる。
(変形実施形態1−1)
上述した実施形態1においては、特定の軸に対してほぼ回転対称な形状を有する物体の位置姿勢の計測(推定)を行なう場合について説明した。これに対して、変形実施形態1−1では、モデル座標系の特定軸への並進方向を決める特徴が少ない物体の位置姿勢を計測する場合について説明する。
例えば、細長い四角柱のような形状に、その長軸方向の位置成分を決定する特徴が少ない形状を有する物体では、位置姿勢の6自由度成分を公平に扱って位置姿勢を算出すると、特定軸への並進方向のパラメータが正しく求まらない。
そこで、エッジを利用して6自由度の位置姿勢を一度算出した後で、特定軸方向の並進成分の推定に影響するエッジを用いて、3次元形状モデルの特定軸方向の並進パラメータのみ後段の処理で推定する。なお、本実施形態における位置姿勢計測装置10の構成は、実施形態1と同様であるため、その説明については省略する。
続いて、変形実施形態1−1に係わる位置姿勢計測装置10における処理の流れの一例について説明する。なお、基本的な処理の流れは、図3、図5及び図8と同様である。実施形態1と異なる処理は、S101〜S103、S107、S303の処理が挙げられる。そのため、ここでは、実施形態1と相違する処理について重点的に説明する。
まず、図3を参照して、S101〜S103及びS107の処理について説明する。
[S101]
位置姿勢計測装置10は、特定パラメータ指定部12及び感度算出部13において、3次元形状モデルの各幾何特徴を入力する。ここで、図10(a)は、本実施形態において対象とする物体の具体例とその座標系を示している。
[S102]
位置姿勢計測装置10は、特定パラメータ指定部12において、第1の位置姿勢パラメータと、第2の位置姿勢パラメータとを指定する。本実施形態では、図10(a)に示すように、3次元形状モデルが、特定軸への並進方向を決める特徴の少ない形状であることを前提とする。このモデルに対して、モデルの並進方向を定義する軸(以後、モデル中心軸と呼ぶ)を決定する。モデル中心軸の設定は、例えば、ユーザからの入力により行なう。具体的には、図10(a)に示すような四角柱形状のモデルの場合、四角柱の相対する2つの面を結ぶモデルのエッジ線分上から2点a、bをモデル中心軸として保持する(図10(b))。但し、モデル中心軸の設定方法は、これに限られない。例えば、モデル面上の頂点から主軸位置と方向とを求め、これらにより表されるベクトルをモデル中心軸としても良い。その他、モデル上で連続する類似形状により推定が困難な並進成分を定義する軸を算出できるのであれば、いかなる方法であっても良い。
次に、モデル中心軸と互いに直交する2つの軸を設定する。ここで、モデル中心軸と、設定した2つの軸とが座標軸となるようにモデル座標系を変換して定義し直す(図10(c))。変換後のモデル座標系において、y軸(実線)がモデル中心軸と一致している。但し、モデル座標系の変換方法は、これに限られない。例えば、モデル座標系のx軸がモデル中心軸と一致するように変換を行なっても良い。その他、モデル中心軸の方向が、モデルの類似形状の方向と同一になるようにモデル座標系が変換されるのであれば、いかなる変換方法であっても良い。なお、以後の処理は、変換後のモデル座標系を用いて行なうものとする。変換後のモデル座標系を基準として、実施形態1と同様に、位置姿勢計測装置10の位置姿勢を6次元ベクトルs=[tx,ty,tz,θ,φ,ψ]で表す。この6パラメータが第1の位置姿勢パラメータとなる。更に、特定パラメータ指定部12は、モデル中心軸と一致する座標軸に対する並進成分を第2の位置姿勢パラメータとして指定する。本実施形態においては、y軸がモデル中心軸と一致するため、y軸方向の並進成分であるtyが第2の位置姿勢パラメータとして指定される。
[S103]
位置姿勢計測装置10は、感度算出部13において、第2の位置姿勢パラメータを算出する際の各モデル線分(3次元形状モデルにおける各幾何特徴)の感度を算出する。具体的には、モデル上の二つの3次元点ps、peとを結ぶ線分Lj(j=1,2,・・・,N)(N:モデル上の線分の数)の感度wjを次のように算出する。
まず、点psをモデル中心軸に対してtyだけ微小(例えば、tyはy軸方向のモデル長さの1/100など)移動させた点をps’により表すものとする。モデル軸方向にty微小移動させる並進ベクトルをTyと表すと、ps’は、以下の「数17」で表される。
ps’=ps+Ty (数17)
ここで、点psから点ps’へ向かう長さ1の3次元ベクトルをvec1とする。更に、点psから点peへ向かう長さ1の3次元ベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1・vec2を用いて以下の「数18」により定義する。
wj=1−|vec1・vec2| (数18)
各幾何特徴の感度wjは、0〜1の間の値を持つ。位置姿勢計測装置10においては、この感度wjを各モデル線分Ljと組にして保持する。なお、S103の処理は、オフラインでの処理として予め行なっておいても良い。オフラインで事前に処理を行なうことで、位置姿勢算出時の処理を高速に行なうことができる。
[S107]
ここで、S104の処理で算出された第1の位置姿勢は、特定の自由度に関するパラメータtyを除き、正しい位置姿勢が算出されていることが期待される。そのため、S107の処理では、位置姿勢計測装置10は、第1の位置姿勢を概略値として、特定の自由度に関するパラメータtyのみに関する最適化を行ない、計測対象物体の位置姿勢を算出する。このとき、第2の位置姿勢パラメータtyの推定に影響し易い幾何特徴を利用することで、ロバスト且つ効率良く推定を行なう。
第1の位置姿勢を概略値とした場合、S104の処理と同様に「数6」の関係が成り立つ。ここで、モデル座標系を基準としたy軸方向に対するΔtyの微小並進を表す3次元ベクトルをΔT=[0,ty,0]とする。このとき、第1の位置姿勢からモデル座標系のy軸方向に微小並進させたときの、モデル座標系(物体座標系)の3次元点X=[x,y,zから基準座標系の3次元点X=[x,y,zへの座標変換は、「数19」のように表される。
=R(X+ΔT)+T (数19)
そこで、S107の処理では、微小変化量Δtyを求めて、モデル座標系y軸方向の並進パラメータの補正を行なうことで計測対象物体の位置姿勢の算出を行なう。
次に、図8を参照して、S303の処理について説明する。
[S303]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、3次元形状モデルの幾何特徴と画像特徴との対応関係に基づいて、位置姿勢を算出するための係数行列・誤差ベクトルを算出する。本実施形態においても、「数12」のφをtyに置き換えた形で、上記同様の関係が成立する。この方程式を実施形態1と同様の方法で解き、パラメータを更新することで第2の位置姿勢を算出する。
以上説明したように変形実施形態1−1によれば、モデル座標系の特定軸の並進方向を決める特徴が少ない物体に対してもロバスト且つ効率良く位置姿勢の計測が行なえる。具体的には、前段の処理において、物体の位置姿勢を算出し、後段の処理において、モデルの特定軸並進パラメータの推定に影響し易い幾何特徴を用いて並進パラメータのみ推定する。
なお、本実施形態においては、特定軸の並進方向を決めるための特徴が少ない形状を有する物体を対象とした。しかし、特定の軸方向の並進を決めるための特徴の数の大小にかかわらず、本手法は応用可能である。すなわち、特定軸の並進方向に対して類似性の高い形状において、その軸の並進方向を決めるための特徴を有する物体であれば、本手法を適用することで、特定軸の並進成分をロバストかつ効率良く推定することができる。
(変形実施形態1−2)
上述した実施形態1においては、第2の位置姿勢パラメータの算出時に各幾何特徴を重み付けすることで、第2の位置姿勢パラメータの推定に影響し易い幾何特徴の寄与度を上げていた。これに対して、変形実施形態1−2では、各幾何特徴について求めた感度を所定の閾値によって判定し、パラメータの推定に大きく影響する幾何特徴のみを選択して利用する場合について説明する。
例えば、実施形態1を説明した図8のS302の処理において、幾何特徴の感度の閾値wθを予め設定しておき、各幾何特徴の感度wjと比較する。これにより、第2の位置姿勢パラメータの算出に利用する幾何特徴を予め取捨選択する。より具体的には、例えば、wθ=0.9と設定しておき、wj>wθを満たす幾何特徴のみ対応付けを行なう。これにより、第2の位置姿勢パラメータの推定に影響し難い幾何特徴の対応付け処理が省略される。これにより、第2の位置姿勢パラメータを算出する線形方程式の係数行列のサイズを小さくできるため、位置姿勢算出時の処理を高速に行なうことができる。
以上説明したように変形実施形態1−2によれば、3次元形状モデルの各幾何特徴それぞれに対して算出した感度に応じて、上記対応付け処理に利用する幾何特徴を判定する。これにより、上述した実施形態1の構成よりも、位置姿勢算出時の処理を高速に行なうことができる。
(変形実施形態1−3)
上述した実施形態1においては、実施形態1を説明した図3のS103における各幾何特徴の感度算出において、モデル座標系でモデル幾何特徴をφだけ回転させたときの微小変位ベクトルと、モデル線分の方向とに基づいて感度の算出を行なっていた。
これに対して、第1の位置姿勢に基づいて、モデル線分の画像面上への投影像を微小回転させたときの、画像上での微小変位ベクトルと、画像面上でのモデル線分の方向とに基づいて感度の算出を行なっても良い。具体的には、3次元形状モデル上の二つの3次元点ps、peとを結ぶ線分Lj(j=1,2,・・・,N)(N:モデル上の線分の数)の感度wjを以下に示すように算出する。
まず、モデル上の二つの3次元点ps、peを第1の位置姿勢に基づいて画像上に投影した2次元座標をus、ueとする。次に、実施形態1と同様に、点psをモデル中心軸に対してφだけ微小(例えば、φ=0.01 radian)回転させた点をps’により表すものとする。
更に、ps’を第1の位置姿勢に基づいて画像面上に投影した2次元座標をus’とする。ここで、点usから点us’へ向かう長さ1の2次元ベクトルをvec1とする。また更に、点usから点ueへ向かう長さ1の2次元ベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1・vec2を用いて「数2」と同様の方法で算出する。なお、変形実施形態1−1の例に対して、微小変化量φをtyに置き換えることでps’を算出して感度を算出しても良い。
以上説明したように変形実施形態1−3によれば、位置姿勢推定に用いる2次元エッジの特性に合わせた感度が算出できるため、ロバスト且つ効率良く位置姿勢の計測が可能となる。
(変形実施形態1−4)
上述した実施形態1においては、実施形態1を説明した図3のS103における各幾何特徴の感度算出において、モデル座標系でモデル幾何特徴をφだけ回転させたときの微小変位ベクトルと、モデル線分の方向とに基づいて感度の算出を行なった。
これに対して、各幾何特徴の偏微分を算出することで幾何特徴の移動方向を求め、これを利用して感度を算出しても良い。具体的には、モデル上の二つの3次元点ps、peとを結ぶ線分Lj(j=1,2,・・・,N)(N:モデル上の線分の数)の感度wjを次のように算出する。まず、変形実施形態1−3と同様に、モデル上の二つの3次元点ps、peを第1の位置姿勢に基づいて画像面上に投影した2次元座標をus、ueとする。ここでus=(u,v)と表す。モデル中心軸に対する微小回転をφにより表すものとして、φに対するusの偏微分を求め、微小変位ベクトルvec1=[∂u/∂φ,∂v/∂φ]を算出する。
なお、長さは1になるよう正規化するものとする。更に、usとueを結ぶ長さ1のベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1・vec2を用いて「数2」と同様の方法で算出する。なお、変形実施形態1−1の例に対して、φの代わりにtyに対する偏微分を算出してvec1を求め、感度を算出しても良い。
以上説明したように変形実施形態1−4によれば、実施形態1と同様に、ロバスト且つ効率良く位置姿勢の計測が行なえる。
(実施形態2)
次に、実施形態2について説明する。上述した実施形態1においては、2次元画像を利用して計測対象物体の位置姿勢を計測(推定)する場合に説明した。これに対して、実施形態2においては、細長い四角柱のような形状(変形実施形態1−1参照)に、その長軸方向の位置成分を決定する特徴が少ない(図10参照)物体に対して距離画像を利用して位置姿勢の推定を行なう場合について説明する。例えば、細長い四角柱のような形状に、その長軸方向の位置成分を決定する特徴が少ない物体では、位置姿勢の6自由度成分を同時に位置姿勢を算出すると、3次元形状モデルの特定軸への並進方向のパラメータが正しく求まらない。
そこで、実施形態2では、モデルの幾何特徴を利用して位置姿勢を一度算出した後で、特定軸の並進成分の推定に影響するモデル幾何特徴の寄与度を変更して(上げて)モデルの特定軸方向の並進パラメータのみ後段で推定する。なお、実施形態2では、幾何特徴として3次元形状モデルの面を利用する。
なお、実施形態2に係わる位置姿勢計測装置10の基本的な構成は、実施形態1を説明した図1と同様であるため、その説明については省略する。以下、実施形態2に係わる位置姿勢計測装置10における各構成部について説明する。なお、モデル保持部11、特定パラメータ指定部12、概略値入力部15については、変形実施形態1−1と同様の機能を果たすため、その説明については省略する。
感度算出部13は、特定パラメータ指定部12により指定された第2の位置姿勢パラメータに対する感度を算出する。この感度は、3次元形状モデルを構成する各面に対応して算出される。実施形態2に係わる位置合わせでは、3次元形状モデルの面(モデル面)と、距離画像上で対応付けられた3次元点との距離を最小化するように計測対象物体の位置姿勢を調整する。
そのため、第2の位置姿勢パラメータ方向にモデル面上の点を微小変化させたときの移動方向と、モデル面の法線方向とが垂直となる場合、そのモデル面に属す3次元点は、第2の位置姿勢パラメータの推定に寄与しない。従って、第2の位置姿勢パラメータ方向にモデルを微小変化させたときのモデル面の移動方向と、モデル面自身の持つ方向との相違度を感度として算出する。算出した感度は、第2の位置姿勢算出部17において、第2の位置姿勢パラメータを算出する際の各幾何特徴の寄与度を設定する際に用いられる。
撮像部14は、例えば、距離センサであり、距離画像を撮像する。距離画像は、各画素が奥行きの情報を持つ画像である。距離センサとしては、対象に照射したレーザ光やスリット光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものが利用できる。勿論、距離センサは、これに限られず、光の飛行時間を利用するTime−of−flight方式であっても良いし、また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であっても良い。その他、距離画像を計測するものであれば、いかなる形式の装置であっても良い。
第1の位置姿勢算出部16は、計測対象物体の位置姿勢を示す概略値に基づいて、モデル保持部11に保持されたモデル幾何特徴と、撮像部14により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、計測対象物体の位置姿勢を表す第1の位置姿勢パラメータを算出する。実施形態2においては、モデルの3次元面と、撮像画像から得られる点との距離を最小化することで第1の位置姿勢を算出する。
第2の位置姿勢算出部17は、第1の位置姿勢に基づいて、モデル保持部11に保持されたモデル幾何特徴と、撮像部14により撮像された画像から検出される画像特徴とを対応付ける。そして、当該対応関係に基づいて、第2の位置姿勢パラメータを推定して第2の位置姿勢を算出する。このとき、感度算出部13により算出された感度に基づき、第2の位置姿勢パラメータ算出時の各モデル幾何特徴の寄与度を設定する。これにより、ロバスト且つ効率良く算出を行なう。
続いて、実施形態2に係わる位置姿勢計測装置10の処理の流れの一例について説明する。実施形態2に係わる位置姿勢計測装置10の基本的な処理の流れは、実施形態1を説明した図3、図5及び図8と同様である。なお、S101、S102、S105、S201、S204〜S206、S301、S305、S306については、変形実施形態1−1と同様である。そのため、これらの処理についての説明は省略する。
まず、図3を参照して、S103、S104及びS106の処理について説明する。
[S103]
位置姿勢計測装置10は、感度算出部13において、第2の位置姿勢パラメータを算出する際の各モデル幾何特徴の感度を算出する。具体的には、モデルを構成する面Sj(j=1,2,・・・,N)(N:モデル上の面の数)の感度wjを次のように算出する。なお、ここでは、各モデル面は、同一の法線を持つように区分されているものとする。
まず、モデル面Sj上の点psをモデル中心軸に対してtyだけ微小(例えばtyはy軸方向のモデル長さの1/100など)移動させた点をps’により表すものとする。モデル軸方向にty微小移動させる並進ベクトルをTyと表すと、ps’は、以下の「数20」で表される。
ps’=ps+Ty (数20)
ここで、点psから点ps’へ向かう長さ1の3次元ベクトルをvec1とする。更に、面Sjの法線を表す長さ1の3次元ベクトルをvec2とする。このとき、面Sjの感度wjを、vec1とvec2との内積vec1・vec2を用いて以下の「数21」により定義する。
wj=|vec1・vec2| (数21)
算出した各幾何特徴の感度wjは0〜1の間の値を持つ。位置姿勢計測装置10においては、この感度wjを各モデル面Sjと組にして保持する。なお、S103の処理は、オフラインでの処理として予め行なっておいても良い。オフラインで事前に処理を行なうことで、位置姿勢算出時の処理を高速に行なうことができる。
[S104]
位置姿勢計測装置10は、撮像部14において、計測対象物体の距離画像を撮像する。
[S106]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、距離画像の情報に基づいて物体の位置姿勢の計測を行なう。実施形態2においては、距離画像上の特徴である3次元点に3次元形状モデルの投影像を当てはめるようにして計測対象物体の位置姿勢を算出する。S106の処理における第1の位置姿勢の算出処理は、図5と同様であるため、その説明については省略する。ここでは、距離画像から変換して得られる3次元点と、推定される位置姿勢に基づいて画像上に投影される3次元形状モデルの面との距離の二乗和が最小になるように位置姿勢の最適化を行なう。
次に、図5を参照して、S202及びS203の処理について説明する。以下の説明では、計測対象物体の位置姿勢を、実施形態1と同様に6次元ベクトルsで表す。
[S202]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、S104の処理で入力された距離画像と3次元形状モデルとの対応付けを行なう。以下、距離画像とモデルとの対応付け処理について説明する。図11は、S202の処理における距離画像と3次元形状モデルとの対応付け処理の概要を示している。S202の処理では、「大石,中澤,池内,“インデックス画像を用いた複数距離画像の高速同時位置合せ”,vol.J89−D,no.3,pp.513−521,2006.」に開示された技術を用いて、物体の3次元形状モデル(ポリゴンモデル)を距離画像上に投影することによって対応付けを行なう。3次元形状モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデルのどのポリゴンに対応するかを決定するためである。距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと計測対象物体の位置姿勢の概略値とを用いて、ポリゴンモデルを描画する。以下、ポリゴンモデルを描画した画像をポリゴン描画画像と呼ぶ。描画する際には、各ポリゴンに固有の色を割り当てて描画を行なう。ポリゴンの色はRGBの各色で表す。背景部分と区別するために、黒(R=0、G=0、B=0)以外の色を用いてポリゴンを描画する。これにより、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をGPU(Graphics Processing Unit)上で高速に行ない、描画された結果の画像を読み出しても良い。ポリゴンモデルを描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている3次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。
[S203]
位置姿勢計測装置10は、第1の位置姿勢算出部16において、3次元形状モデルのポリゴンと距離画像から変換した3次元点との対応をもとに、位置姿勢を算出するための係数行列・誤差ベクトルを算出する。以下、距離画像の画素(3次元の点)と面との対応に基づく係数行列・誤差ベクトルの算出方法について説明する。
3次元空間における平面の方程式は、一般に、ax+by+cz=e(a+b+c=1)の形で表される。ここでは、3次元形状モデル中のある面の計測物体の座標系における平面の方程式をax+by+cz=eであるとする。この平面の方程式は、位置姿勢の概略値に基づいて、基準座標系における平面の方程式a’x+b’y+c’z=e’に変換できる。位置姿勢の概略値に基づく、モデル座標系(物体座標系)の3次元点X=[x,y,zから基準座標系の3次元点X=[x,y,zへの座標変換を「数22」のように表す。
=RX+T (数22)
但し、Rは姿勢を表す3×3回転行列であり、Tは位置を表す3次元ベクトルである。このとき、基準座標系における平面の方程式の係数は次式のようになる。
Figure 0005938201
平面の座標変換については、例えば、金谷「形状CADと図形の数学」(共立出版)で詳しく説明がなされている。基準座標系における3次元座標の計測値が(x’,y’,z’)である点Pが3次元形状モデル中のある面Fと対応付けられているとする。面Fに点Pから下ろした垂線の足をQとする。点Qの基準座標系における3次元座標(x、y、z)は、計測対象物体の位置姿勢sによって変換されるものであり、ある位置姿勢sのときの3次元座標(x0,y0,z0)の近傍で1次のテイラー展開によって「数24」のように近似できる。
Figure 0005938201
正しいsによって変換される(x、y、z)は、法線ベクトルが面Fと同じで点Pを通る平面(a’x+b’y+c’z=e’,e’=a’x’+b’y’+c’z’)上にあると仮定すると、「数24」を平面の方程式に代入することにより「数25」が得られる。
Figure 0005938201
「数25」の観測方程式は対応付けが行なわれた全ての点群データについて立式することができる。
「数25」の観測方程式は、sの各成分の微小変化Δsi(i=1,2,・・・,6)についての方程式になっており、「数26」に示すΔsに関する線形連立方程式を立てることができる。
Figure 0005938201
この線形連立方程式を「数27」のように表す。
Δs=E (数27)
左辺のJが算出対象となる係数行列であり、右辺のEが誤差ベクトルである。
次に、図8を参照して、S303及びS304の処理について説明する。
[S303]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、微小変化するパラメータをΔtyのみに限定して、S302と同様の処理を行なう。これにより、モデル線分上の制御点と画像上のエッジとの対応から、Δtyに関する「数28」のような線形連立方程式を立てることができる。
Figure 0005938201
この線形連立方程式を「数29」のように表す。
Δty=E (数29)
左辺のJが算出対象となる係数行列であり、右辺のEが誤差ベクトルである。
[S304]
位置姿勢計測装置10は、第2の位置姿勢算出部17において、S103の処理で算出された第2の位置姿勢パラメータに対する各面Sjの感度に基づき、各モデル幾何特徴のパラメータ算出への寄与度を重み付けする。モデル幾何特徴は、画像上の各画素に投影されるモデル面上の点と、その面の法線により表される。つまり、各幾何特徴はモデルを構成する面のいずれかの投影像である。そこで、モデル面Sjの投影により生成されたモデル点には、そのモデル面Sjと組にしてあるwjを重みとして組にする。ここで、各制御点の重みを用いて「数30」のように重み行列Wを定義する。
Figure 0005938201
重み行列Wは、対角成分以外は全て0の正方行列である。対角成分(ws,wt,・・・)には、行列Jの各行に対応するモデル幾何特徴の重みが入る。これにより、各幾何特徴に対して、感度の値が大きいデータには大きな重みを、値が小さいデータには小さな重みを与えるようにして、Δtyの推定に寄与しないデータの影響を抑制する。この重み行列Wを用いて、「数29」を「数31」のように変形する。
WJΔty=WE (数31)
以上説明したように実施形態2によれば、変形実施形態1−1と同様の物体に対して、距離画像を利用して位置姿勢の計測(推定)を行なえる。これにより、モデル座標系の特定軸の並進方向を決める特徴が少ない物体に対してもロバスト且つ効率良く位置姿勢の算出が可能となる。
具体的には、前段の処理において、3次元形状モデルと画像との3次元点の対応を利用して物体の位置姿勢を推定し、後段の処理において、モデルの特定軸並進方向に限定した位置合わせを行なう。モデルの幾何特徴に関して、特定軸並進方向の位置合わせへの影響度を予め算出しておき、後段のパラメータ推定時に重み付けして利用する。
また、2次元画像及び距離画像を同時に取得できる場合、第1の位置姿勢を算出する前段の位置合わせ、第2の位置姿勢を算出する後段の位置合わせにおいて、2次元画像を利用する手法と距離画像を利用する手法とをそれぞれ切り替えて行なっても良い。また、それら画像を同時に利用しても良い。すなわち、2次元画像及び距離画像の少なくとも一方を用いて計測対象物体の位置姿勢の算出を行なっても良い。
(その他の実施形態)
本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア(プログラム)を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ(又はCPUやMPU等)がプログラムを読み出して実行する処理である。

Claims (10)

  1. 計測対象となる物体の位置姿勢を計測する位置姿勢計測装置であって、
    前記物体の3次元形状モデルを保持するモデル保持手段と、
    前記3次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを設定する特定パラメータ設定手段と、
    前記3次元形状モデルの幾何特徴について、該幾何特徴の方向と、前記特定のパラメータを変化させたときの該幾何特徴の移動方向と、に基づいて、該特定のパラメータの算出時に与える影響を示す感度を算出する感度算出手段と、
    前記3次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する位置姿勢算出手段と
    を具備し、
    前記位置姿勢算出手段は、
    前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記3次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する
    ことを特徴とする位置姿勢計測装置。
  2. 前記位置姿勢算出手段は、
    前記感度算出手段により算出された感度に基づいて前記3次元形状モデルの各幾何特徴をそれぞれ重み付けし、当該重み付けされた幾何特徴と、前記物体の画像から検出された画像特徴との対応関係に基づいて、前記特定のパラメータを算出する
    ことを特徴とする請求項1記載の位置姿勢計測装置。
  3. 前記位置姿勢算出手段は、
    前記感度算出手段により算出された感度が所定の閾値を越えた幾何特徴と、前記物体の画像から検出された画像特徴との対応関係に基づいて、前記特定のパラメータを算出する
    ことを特徴とする請求項1記載の位置姿勢計測装置。
  4. 前記3次元形状モデルの幾何特徴は、
    前記3次元形状モデル上の線、面又は方向を持つ点であ
    ことを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。
  5. 前記画像は、2次元画像及び距離画像の少なくとも一方を含む
    ことを特徴とする請求項1乃至4のいずれか1項に記載の位置姿勢計測装置。
  6. 前記物体は、
    特定の軸に対して回転対称であり、且つその軸回りの回転を決めるための特徴を有し、
    前記特定のパラメータは、前記軸回りの回転成分を示すパラメータである
    ことを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。
  7. 前記物体は、
    特定の軸への並進方向を決める特徴を有し、
    前記特定のパラメータは、前記特定の軸への並進成分を示すパラメータである
    ことを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。
  8. 前記位置姿勢算出手段は、前記3次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する第1の算出を行った後、該第1の算出により算出された前記物体の位置姿勢を初期値とし、前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記3次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する第2の算出を行う、ことを特徴とする請求項1乃至7の何れか1項に記載の位置姿勢計測装置。
  9. 計測対象となる物体の位置姿勢を計測する位置姿勢計測装置の処理方法であって、
    特定パラメータ設定手段が、前記物体の3次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを設定する工程と、
    感度算出手段が、前記3次元形状モデルの幾何特徴について、該幾何特徴の方向と、前記特定のパラメータを変化させたときの該幾何特徴の移動方向と、に基づいて、該特定のパラメータの算出時に与える影響を示す感度を算出する工程と、
    位置姿勢算出手段が、前記3次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する工程と
    を含み、
    前記位置姿勢算出手段は、
    前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記3次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する
    ことを特徴とする処理方法。
  10. 請求項に記載された処理方法の各工程をコンピュータに実行させるためのプログラム。
JP2011267255A 2011-12-06 2011-12-06 位置姿勢計測装置、その処理方法及びプログラム Active JP5938201B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2011267255A JP5938201B2 (ja) 2011-12-06 2011-12-06 位置姿勢計測装置、その処理方法及びプログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2011267255A JP5938201B2 (ja) 2011-12-06 2011-12-06 位置姿勢計測装置、その処理方法及びプログラム

Publications (2)

Publication Number Publication Date
JP2013120091A JP2013120091A (ja) 2013-06-17
JP5938201B2 true JP5938201B2 (ja) 2016-06-22

Family

ID=48772789

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2011267255A Active JP5938201B2 (ja) 2011-12-06 2011-12-06 位置姿勢計測装置、その処理方法及びプログラム

Country Status (1)

Country Link
JP (1) JP5938201B2 (ja)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6052103B2 (ja) * 2013-08-26 2016-12-27 株式会社ダイフク 物品認識装置及び物品認識設備

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7330279B2 (en) * 2002-07-25 2008-02-12 Timbre Technologies, Inc. Model and parameter selection for optical metrology
JP5347530B2 (ja) * 2009-01-26 2013-11-20 株式会社ニコン 画像処理プログラム、画像処理装置、および撮像装置
JP5297403B2 (ja) * 2010-02-26 2013-09-25 キヤノン株式会社 位置姿勢計測装置、位置姿勢計測方法、プログラムおよび記憶媒体
JP5746477B2 (ja) * 2010-02-26 2015-07-08 キヤノン株式会社 モデル生成装置、3次元計測装置、それらの制御方法及びプログラム

Also Published As

Publication number Publication date
JP2013120091A (ja) 2013-06-17

Similar Documents

Publication Publication Date Title
US20200284573A1 (en) Position and orientation measuring apparatus, information processing apparatus and information processing method
JP5839971B2 (ja) 情報処理装置、情報処理方法及びプログラム
JP5746477B2 (ja) モデル生成装置、3次元計測装置、それらの制御方法及びプログラム
JP6271953B2 (ja) 画像処理装置、画像処理方法
US20180066934A1 (en) Three-dimensional measurement apparatus, processing method, and non-transitory computer-readable storage medium
US10189162B2 (en) Model generation apparatus, information processing apparatus, model generation method, and information processing method
US9467682B2 (en) Information processing apparatus and method
JP5548482B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、プログラム及び記憶媒体
JP6092530B2 (ja) 画像処理装置、画像処理方法
JP6324025B2 (ja) 情報処理装置、情報処理方法
JP5924862B2 (ja) 情報処理装置、情報処理方法及びプログラム
JP6594129B2 (ja) 情報処理装置、情報処理方法、プログラム
JP5627325B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
JP2011027724A (ja) 3次元計測装置、その計測方法及びプログラム
JP2011179909A (ja) 位置姿勢計測装置、位置姿勢計測方法、プログラム
JP2016170050A (ja) 位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラム
JP5976089B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
JP5698815B2 (ja) 情報処理装置、情報処理装置の制御方法及びプログラム
JP6040264B2 (ja) 情報処理装置、情報処理装置の制御方法、およびプログラム
JP2015007639A (ja) 情報処理装置、情報処理方法およびプログラム
JP5938201B2 (ja) 位置姿勢計測装置、その処理方法及びプログラム
JP2011174891A (ja) 位置姿勢計測装置、位置姿勢計測方法、及びプログラム
JP6548789B2 (ja) 位置姿勢計測装置及び方法
JP2021077290A (ja) 情報処理装置、情報処理方法、プログラム、システム及び物品の製造方法
JP2021012628A (ja) 位置姿勢推定装置及び位置姿勢推定方法

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20141031

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20150811

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20151002

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20151110

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20160415

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20160516

R151 Written notification of patent or utility model registration

Ref document number: 5938201

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151