"計測対象となる物体の位置姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保持するモデル保持手段と、前記3次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを指定する特定パラメータ指定手段と、前記特定のパラメータに基づいて、前記3次元形状モデルの幾何特徴それぞれが該特定のパラメータの算出時に与える影響を示す感度を各幾何特徴について算出する感度算出手段と、前記3次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する位置姿勢算出手段とを具備し、前記位置姿勢算出手段は、前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記3次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出することを特徴とする位置姿勢計測装置。前記位置姿勢算出手段は、前記感度算出手段により算出された感度に基づいて前記3次元形状モデルの各幾何特徴をそれぞれ重み付けし、当該重み付けされた幾何特徴と、前記物体の画像から検出された画像特徴との対応関係に基づいて、前記特定のパラメータを算出することを特徴とする請求項1記載の位置姿勢計測装置。前記位置姿勢算出手段は、前記感度算出手段により算出された感度が所定の閾値を越えた幾何特徴と、前記物体の画像から検出された画像特徴との対応関係に基づいて、前記特定のパラメータを算出することを特徴とする請求項1記載の位置姿勢計測装置。前記3次元形状モデルの幾何特徴は、前記3次元形状モデル上の線、面又は方向を持つ点であり、前記感度算出手段は、前記3次元形状モデルの各幾何特徴を規定する方向と前記特定のパラメータの値を変化させたときの各幾何特徴の移動方向とに基づいて前記感度を算出することを特徴とする請求項1から3のいずれか1項に記載の位置姿勢計測装置。前記画像は、2次元画像及び距離画像の少なくとも一方を含むことを特徴とする請求項1から4のいずれか1項に記載の位置姿勢計測装置。前記物体は、特定の軸に対してほぼ回転対称であり、且つその軸回りの回転を決めるための特徴を有し、前記特定のパラメータは、前記軸回りの回転成分を示すパラメータであることを特徴とする請求項1から5のいずれか1項に記載の位置姿勢計測装置。前記物体は、特定の軸への並進方向を決める特徴を有し、前記特定のパラメータは、前記特定の軸への並進成分を示すパラメータであることを特徴とする請求項1から5のいずれか1項に記載の位置姿勢計測装置。計測対象となる物体の位置姿勢を計測する位置姿勢計測装置の処理方法であって、特定パラメータ指定手段が、前記物体の3次元形状モデルを保持するモデル保持手段に保持された3次元形状モデルの座標系を基準として、前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを指定する工程と、感度算出手段が、前記特定のパラメータに基づいて、前記3次元形状モデルの幾何特徴それぞれが該特定のパラメータの算出時に与える影響を示す感度を各幾何特徴について算出する工程と、位置姿勢算出手段が、前記3次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する工程とを含み、前記位置姿勢算出手段は、前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記3次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出することを特徴とする処理方法。請求項8に記載された各工程をコンピュータに実行させるためのプログラム。ここで、他のパラメータに寄与する幾何特徴に比べて、特定のパラメータに寄与する幾何特徴が少ない物体の一例について挙げてみる。例えば、工業製品に使われる部品の中で考えると、回転対称に近い形状で、且つその回転成分を一意に決定するための幾何特徴が微小な切り欠きや突起しかない部品が挙げられる。このような部品では、位置姿勢を規定する6パラメータを同時に推定しようとすると、他の成分に比べて、特定の成分この場合、物体の回転対称性を定義する軸まわりの姿勢の推定に必要な情報が不足してしまう。そのため、この特定の成分を推定するのが困難となり、その結果、位置姿勢のロバスト性や精度が低くなるという問題がある。なお、上述した非特許文献1の方法では、カメラを基準とした座標系で規定される位置姿勢成分に基づいて幾何特徴を分類するため、物体3次元形状モデルを基準とした座標系で規定される特定の位置姿勢の成分に寄与する幾何特徴を分離することができない。本発明は、上記課題に鑑みてなされたものであり、情報の不足により物体基準で定義される特定の位置姿勢の成分パラメータを推定するのが困難な物体においても、ロバスト且つ高速に位置姿勢の計測を行なえるようにした技術を提供することを目的とする。上記課題を解決するため、本発明の一態様は、計測対象となる物体の位置姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保持するモデル保持手段と、前記3次元形状モデルの座標系を基準とした前記物体の位置姿勢を示す複数のパラメータのうち、特定のパラメータを指定する特定パラメータ指定手段と、前記特定のパラメータに基づいて、該特定のパラメータ算出時の前記3次元形状モデルの幾何特徴それぞれの影響力を示す感度を各幾何特徴について算出する感度算出手段と、前記3次元形状モデルの幾何特徴と前記物体の画像から検出された画像特徴とを対応付け、当該対応関係に基づいて、前記複数のパラメータを算出することにより前記物体の位置姿勢を算出する位置姿勢算出手段とを具備し、前記位置姿勢算出手段は、前記物体の位置姿勢の算出に際して、前記感度算出手段により算出された感度に基づいて前記特定のパラメータ算出時の前記3次元形状モデルの各幾何特徴の寄与度を設定して当該特定のパラメータを算出する。本発明によれば、情報の不足により物体基準で定義される特定の位置姿勢の成分を推定するのが困難な物体においても、ロバスト且つ高速に位置姿勢の計測を行なえる。以下、図面を参照して本発明の好適な実施形態について詳細に説明する。実施形態1まず、実施形態1について説明する。実施形態1においては、特定の軸に対してほぼ回転対称であり、且つその軸回りの回転を決めるための特徴が少ない形状を有する物体の位置姿勢の計測推定を行なう場合について説明する。ここで、実施形態1に係わる計測対象物体の位置姿勢の計測推定方法の概要について簡単に説明する。実施形態1においては、2次元画像上のエッジに対してモデルのエッジをあてはめることにより6自由度の位置姿勢を算出する。但し、上述した形状の物体では、6自由度の位置姿勢を同時に算出すると、物体の軸回りの回転を決定する特徴が少ないため、回転軸まわりの位置姿勢パラメータが正しく求まらない。そこで、実施形態1においては、まず、6自由度の位置姿勢を算出し前段の処理、その後、軸回りの回転成分の推定に影響するエッジのみを利用して、軸回りの回転成分のみ別途算出を行なう後段の処理。前段処理後には、軸回りの回転成分を除き、正しい位置姿勢が算出されていることが期待される。そのため、後段の処理では、軸回りの回転成分の推定に影響力のある3次元形状モデルの幾何特徴のみを利用することで、ロバスト且つ効率良く位置姿勢の推定を行なう。図1は、本発明の一実施の形態に係わる位置姿勢計測装置10の構成の一例を示す図である。位置姿勢計測装置10は、3次元情報本実施形態においては、物体の位置姿勢の計測対象となる物体以下、計測対象物体、又は単に物体と呼ぶ場合もあるの位置姿勢を計測する。位置姿勢計測装置10には、コンピュータが内蔵されている。コンピュータには、CPUCentralProcessingUnit等の主制御手段、ROMReadOnlyMemory、RAMRandomAccessMemory、HDDHardDiskDrive等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていても良い。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。ここで、位置姿勢計測装置10は、モデル保持部11と、特定パラメータ指定部12と、感度算出部13と、撮像部14と、概略値入力部15と、位置姿勢算出部18とを具備して構成される。モデル保持部11は、計測対象物体の3次元幾何モデルデータ以下、3次元形状モデルと呼ぶを保持する。ここで、図2a及び図2bを用いて、本実施形態に係わる3次元形状モデルの定義方法の一例について説明する。3次元形状モデルは、点の集合や各点を結んで構成される線分の集合によって定義される。本実施形態に係わる3次元形状モデルは、図2aに示すように、点P1~点P15の15点から構成される。点P1~点P15は、3次元座標値により表される。また、本実施形態に係わる3次元形状モデルは、図2bに示すように、線分L1~L21により構成されている。線分L1~L21は、線分を構成する点のID識別子により表される。また、ここでは図示を省略するが、3次元形状モデルは、面の情報も保持する。各面は、当該各面を構成する点のIDにより表される。この図2a及び図2bに示す3次元形状モデルでは、物体を構成する8つの面の情報が保持されている。なお、図2a及び図2bに示す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次元画像を撮像する。カメラの内部パラメータ焦点距離、主点位置、レンズ歪みパラメータは、例えば、ZZhangAflexiblenewtechniqueforcameracalibrationIEEETransactionsonPatternAnalysisandMachineIntelligencevol22no11pp133013342000に開示された方法により事前に校正しておけば良い。概略値入力部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の位置姿勢パラメータを指定する。ここで、図4aには、本実施形態において対象とする物体の具体例とその座標系とが示される。本実施形態においては、図4aに示すように、物体が、特定軸に対してほぼ回転対称な形状を有することを前提とする。特定パラメータ指定部12においては、まず、この物体の回転対称性を定義する軸以下、モデル中心軸と呼ぶを指定する。モデル中心軸の指定は、例えば、ユーザからの操作部例えば、マウスやキーボードを介した入力に基づいて行なう。具体的には、図4aに示すような3次元形状モデルの場合、当該モデルの回転対称な面上から対面する2点a、b及びc、dの2組をユーザからの入力として取得する。このとき、線分abと線分cdとの交点を通り、且つ四角形acbdに垂直な方向を持つベクトルをモデル中心軸として算出する図4b参照。但し、モデル中心軸の指定方法は、このような方法に限られない。例えば、三角柱のような回転対称形状の場合には、回転対称な三角形の3頂点をユーザからの入力として取得し、当該三角形の重心を通り、且つ三角形に垂直なベクトルをモデル中心軸として指定しても良い。或いは、モデル面上で回転対称性のある面図4aの場合、八角形をユーザからの入力として取得し、その重心を通り、且つ入力された面に垂直なベクトルをモデル中心軸としても良い。その他、モデル形状の回転対称性を定義する回転軸を算出できるのであれば、いかなる方法を用いても構わない。モデル中心軸の指定が済むと、特定パラメータ指定部12は、モデル中心軸に対して互いに直交する2つの軸を設定し、モデル中心軸と、当該設定した2つの軸とが座標軸となるようにモデル座標系を変換して新しいモデル座標系とする図4c。ここでは、新しいモデル座標系において、y軸実線がモデル中心軸と一致するようにする。なお、モデル座標系の変換方法は、このような方法に限られない。例えば、モデル座標系のx軸がモデル中心軸と一致するように変換しても良い。その他、モデル中心軸がモデル原点を通るようにモデル座標系が変換されるのであれば、いかなる変換であっても構わない。以降の処理は、y軸がモデル中心軸と一致するように変換した、新しいモデル座標系を用いて行なうものとする。基準座標系に対する物体の位置姿勢を6次元ベクトルs=[tx,ty,tz,θ,φ,ψ]Tで表す。ここで、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とを結ぶ線分Ljj=1,2,,NN:モデル上の線分の数の感度wjを次のように算出する。まず、点psをモデル中心軸に対してφだけ微小例えばφ=0.01radian回転させた点をps’で表すものとする。モデル中心軸回りにφ微小回転させる回転行列をRφと表すと、ps’は、以下の数1で示される。ps’=Rφps数1ここで、点psから点ps’へ向かう長さ1の3次元ベクトルをvec1とする。更に、点psから点peへ向かう長さ1の3次元ベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1vec2を用いて、以下の数2により定義する。wj=1-|vec1vec2|数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次元点Xm=[xm,ym,zm]Tから基準座標系の3次元点Xc=[xc,yc,zc]Tへの座標変換は、数3のように表される。Xc=RXmT数3但し、Rは物体の姿勢を表す3つの成分θ、φ、ψで表現される3×3行列であり、Tはモデルの位置を表す3つの成分から成る3次元ベクトルT=[tx,ty,tz]Tである。更に、概略位置姿勢s0に対する物体の位置姿勢の微小変化量を6次元ベクトルΔs=[Δtx,Δty,Δtz,Δθ,Δφ,Δψ]Tとし、微小変化量Δsを求めて、各成分を補正する処理を繰り返す。これにより、計測対象物体の位置姿勢の算出を行なう。ここで、図5を用いて、S106の処理における第1の位置姿勢パラメータの算出処理の詳細について説明する。[S201]位置姿勢計測装置10は、第1の位置姿勢算出部16において、初期化処理を行なう。この初期化処理では、例えば、図3のS105の処理で得られた計測対象物体の概略的な位置姿勢を示す概略値を入力する。S202位置姿勢計測装置10は、第1の位置姿勢算出部16において、S104の処理で入力された2次元画像上から画像特徴を検出し、当該画像特徴と、3次元形状モデルのモデル幾何特徴との対応付けを行なう。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。本実施形態においては、TDrummondandRCipollaRealtimevisualtrackingofcomplexstructuresIEEETransactionsonPatternAnalysisandMachineIntelligencevol24no7pp9329462002に開示された技術を用いてエッジの検出を行なう。このエッジ検出方法は、3次元形状モデル側から対応するエッジを探索する、トップダウン型の方法であるため、エッジの検出と対応付けとが一度に行なわれる。ここで、図6を用いて、本実施形態に係わるエッジ検出方法の概要の一例について説明する。まず、計測対象物体が撮像された2次元画像に対して3次元形状モデルを投影する。この投影像線分は、S201の処理で入力された計測対象物体の位置姿勢の概略値と、校正済みのカメラの内部パラメータとに基づいて求められる。次に、投影された各線分上において、等間隔に制御点を設定する。そして、投影線分の法線方向に平行で且つ制御点を通過する線分以下、探索ライン上からエッジを1次元探索する図6a。エッジは、画素値の濃度勾配の極値として検出されるため、複数のエッジが検出される場合がある図6b。本実施形態においては、検出されるエッジのうち、最も濃度勾配の大きいものを対応するエッジとして検出する。[S203]位置姿勢計測装置10は、第1の位置姿勢算出部16において、3次元形状モデルの線分上の制御点と2次元画像上のエッジとの対応関係に基づいて、計測対象物体の位置姿勢を算出するための係数行列誤差ベクトルを算出する。ここで、図7は、線分の投影像と検出されたエッジとの関係の一例を示す図である。この場合、画像の水平方向をu軸とし、垂直方向をv軸としている。ある制御点投影された各線分を画像上で等間隔に分割した点の画像上での位置をu0,v0と表し、当該制御点が属する線分の画像上での傾きをu軸に対する傾きθと表す。傾きθは、線分の両端の3次元座標をsの概略値S0に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。線分の画像上での法線ベクトルはsinθ,-cosθとなる。また、制御点と対応する対応点エッジの座標をu’,v’とする。ここで、点u’,v’を通り、傾きがθである直線図7の破線上の点u,vは、usinθ-vcosθ=d数5と表せる。なお、θは定数とする。また、d=u’sinθ-v’cosθ定数である。画像上における制御点の位置は、計測対象物体の位置姿勢sにより変化する。従って、制御点の座標u,vは、u0,v0の近傍での1次のテイラー展開によって数6のように近似できる。但し、Δsii=1,2,...,6はsの各成分の微小変化を表す。つまり、Δs1=Δtx、Δs2=Δty、Δs3=Δtz、Δs4=Δθ、Δs5=Δφ、Δs6=Δψである。正しいsによって得られる画像上での制御点の位置が、数5が表す直線上にあると仮定すると、数6を数5に代入することにより、観測方程式数7が得られる。但し、r=u0sinθ-v0cosθ定数である。数7の観測方程式は、対応付けが行なわれた全ての制御点について立式することができる。ここで、数7は、sの各成分の微小変化Δsii=1,2,,6についての方程式となっているため、数8に示すΔsiに関する線形連立方程式を立式できる。正規化画像座標系における画像座標u、vは、それぞれu=xc/zc、v=yc/zcで表される。従って、数3に基づいて算出されたu、vから、左辺の行列の各要素を算出する。この線形連立方程式を数9のように表す。J1Δs=E1数9左辺のJ1が算出対象となる係数行列であり、右辺のE1が誤差ベクトルである。[S204]位置姿勢計測装置10は、第1の位置姿勢算出部16において、計測対象物体の位置姿勢の補正値Δsを求める。すなわち、行列J1の一般化逆行列J1TJ1-1J1Tを用いて、数9を数10のように解く。Δs=J1TJ1-1J1TE1数10[S205]位置姿勢計測装置10は、第1の位置姿勢算出部16において、S204の処理で算出された補正値Δsを用いて、概略値を補正する。s←sΔs[S206]位置姿勢計測装置10は、第1の位置姿勢算出部16において、収束しているか否かの判定を行なう。収束していれば、この処理は終了し、そうでなければ、再度、S203の処理に戻る。なお、収束しているか否かは、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ0である場合に収束したと判定する。以上の処理によって、図3のS105の処理で入力された概略値が、2次元画像の情報を用いて更新される。本実施形態においては、図5に示す処理で最終的に算出された物体の位置姿勢を第1の位置姿勢とする。なお、S106S201~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行列をΔRyとする。このとき、第1の位置姿勢からモデル座標系のy軸方向にΔφだけ微小回転させたときの、モデル座標系物体座標系の3次元点Xm=[xm,ym,zm]Tから基準座標系の3次元点Xc=[xc,yc,zc]Tへの座標変換は、数11のように表される。Xc=RΔRyXmT数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のような線形連立方程式を立てることができる。この線形連立方程式を数13のように表す。J2Δφ=E2数13左辺のJ2が算出対象となる係数行列であり、右辺のE2が誤差ベクトルである。[S304]位置姿勢計測装置10は、第2の位置姿勢算出部17において、図3のS103の処理で算出された第2の位置姿勢パラメータに対する各線分Ljの感度に基づき、各モデル幾何特徴のパラメータ算出への寄与度を重み付けする。モデル幾何特徴は、3次元形状モデルの線分の2次元画像上への投影像を、当該画像上で等間隔になるように分割した制御点とその方向により表される。従って、各制御点は、3次元形状モデルを構成する線分のいずれかの投影像である。そこで、3次元形状モデルの線分Ljの投影により生成された制御点には、そのモデル線分Ljと組にしておいたwjを重みとして利用する。ここで、各制御点の感度wjを用いて、数14のように重み行列Wを定義する。重み行列Wは、対角成分以外は全て0の正方行列である。対角成分ws,wt,には、行列J2の各行に対応するモデル幾何特徴の感度wjが入る。これにより、各幾何特徴に対して、感度の値が大きいデータには大きな重みを、値が小さいデータには小さな重みを与えるようにして、Δφの推定に寄与しないデータの影響を抑制する。なお、各幾何特徴の重みの設定方法は、これに限られない。例えば、wjの代わりに、wj×wjを用いても良い。この他、各幾何特徴が特定軸回転方向の推定に寄与する度合いを感度の値に基づいて設定できる方法であれば、いかなる方法であっても構わない。この重み行列Wを用いて、数13を数15のように変形する。WJ2Δφ=WE2数15[S305]位置姿勢計測装置10は、数15を数16のように解くことにより補正値Δφを求める。Δφ=J2TWJ2-1J2TWE2数16[S306]位置姿勢計測装置10は、第2の位置姿勢算出部17において、S305の処理で算出された位置姿勢の補正値ΔRyにより、概略値Rを補正する。R←RΔRy[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次元形状モデルの各幾何特徴を入力する。ここで、図10aは、本実施形態において対象とする物体の具体例とその座標系を示している。[S102]位置姿勢計測装置10は、特定パラメータ指定部12において、第1の位置姿勢パラメータと、第2の位置姿勢パラメータとを指定する。本実施形態では、図10aに示すように、3次元形状モデルが、特定軸への並進方向を決める特徴の少ない形状であることを前提とする。このモデルに対して、モデルの並進方向を定義する軸以後、モデル中心軸と呼ぶを決定する。モデル中心軸の設定は、例えば、ユーザからの入力により行なう。具体的には、図10aに示すような四角柱形状のモデルの場合、四角柱の相対する2つの面を結ぶモデルのエッジ線分上から2点a、bをモデル中心軸として保持する図10b。但し、モデル中心軸の設定方法は、これに限られない。例えば、モデル面上の頂点から主軸位置と方向とを求め、これらにより表されるベクトルをモデル中心軸としても良い。その他、モデル上で連続する類似形状により推定が困難な並進成分を定義する軸を算出できるのであれば、いかなる方法であっても良い。次に、モデル中心軸と互いに直交する2つの軸を設定する。ここで、モデル中心軸と、設定した2つの軸とが座標軸となるようにモデル座標系を変換して定義し直す図10c。変換後のモデル座標系において、y軸実線がモデル中心軸と一致している。但し、モデル座標系の変換方法は、これに限られない。例えば、モデル座標系のx軸がモデル中心軸と一致するように変換を行なっても良い。その他、モデル中心軸の方向が、モデルの類似形状の方向と同一になるようにモデル座標系が変換されるのであれば、いかなる変換方法であっても良い。なお、以後の処理は、変換後のモデル座標系を用いて行なうものとする。変換後のモデル座標系を基準として、実施形態1と同様に、位置姿勢計測装置10の位置姿勢を6次元ベクトルs=[tx,ty,tz,θ,φ,ψ]Tで表す。この6パラメータが第1の位置姿勢パラメータとなる。更に、特定パラメータ指定部12は、モデル中心軸と一致する座標軸に対する並進成分を第2の位置姿勢パラメータとして指定する。本実施形態においては、y軸がモデル中心軸と一致するため、y軸方向の並進成分であるtyが第2の位置姿勢パラメータとして指定される。[S103]位置姿勢計測装置10は、感度算出部13において、第2の位置姿勢パラメータを算出する際の各モデル線分3次元形状モデルにおける各幾何特徴の感度を算出する。具体的には、モデル上の二つの3次元点ps、peとを結ぶ線分Ljj=1,2,,NN:モデル上の線分の数の感度wjを次のように算出する。まず、点psをモデル中心軸に対してtyだけ微小例えば、tyはy軸方向のモデル長さの1/100など移動させた点をps’により表すものとする。モデル軸方向にty微小移動させる並進ベクトルをTyと表すと、ps’は、以下の数17で表される。ps’=psTy数17ここで、点psから点ps’へ向かう長さ1の3次元ベクトルをvec1とする。更に、点psから点peへ向かう長さ1の3次元ベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1vec2を用いて以下の数18により定義する。wj=1-|vec1vec2|数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]Tとする。このとき、第1の位置姿勢からモデル座標系のy軸方向に微小並進させたときの、モデル座標系物体座標系の3次元点Xm=[xm,ym,zm]Tから基準座標系の3次元点Xc=[xc,yc,zc]Tへの座標変換は、数19のように表される。Xc=RXmΔTT数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とを結ぶ線分Ljj=1,2,,NN:モデル上の線分の数の感度wjを以下に示すように算出する。まず、モデル上の二つの3次元点ps、peを第1の位置姿勢に基づいて画像上に投影した2次元座標をus、ueとする。次に、実施形態1と同様に、点psをモデル中心軸に対してφだけ微小例えば、φ=0.01radian回転させた点をps’により表すものとする。更に、ps’を第1の位置姿勢に基づいて画像面上に投影した2次元座標をus’とする。ここで、点usから点us’へ向かう長さ1の2次元ベクトルをvec1とする。また更に、点usから点ueへ向かう長さ1の2次元ベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1vec2を用いて数2と同様の方法で算出する。なお、変形実施形態1-1の例に対して、微小変化量φをtyに置き換えることでps’を算出して感度を算出しても良い。以上説明したように変形実施形態1-3によれば、位置姿勢推定に用いる2次元エッジの特性に合わせた感度が算出できるため、ロバスト且つ効率良く位置姿勢の計測が可能となる。変形実施形態1-4上述した実施形態1においては、実施形態1を説明した図3のS103における各幾何特徴の感度算出において、モデル座標系でモデル幾何特徴をφだけ回転させたときの微小変位ベクトルと、モデル線分の方向とに基づいて感度の算出を行なった。これに対して、各幾何特徴の偏微分を算出することで幾何特徴の移動方向を求め、これを利用して感度を算出しても良い。具体的には、モデル上の二つの3次元点ps、peとを結ぶ線分Ljj=1,2,,NN:モデル上の線分の数の感度wjを次のように算出する。まず、変形実施形態1-3と同様に、モデル上の二つの3次元点ps、peを第1の位置姿勢に基づいて画像面上に投影した2次元座標をus、ueとする。ここでus=u,vと表す。モデル中心軸に対する微小回転をφにより表すものとして、φに対するusの偏微分を求め、微小変位ベクトルvec1=[∂u/∂φ,∂v/∂φ]Tを算出する。なお、長さは1になるよう正規化するものとする。更に、usとueを結ぶ長さ1のベクトルをvec2とする。このとき、線分Ljの感度wjを、vec1とvec2との内積vec1vec2を用いて数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の位置姿勢パラメータを算出する際の各モデル幾何特徴の感度を算出する。具体的には、モデルを構成する面Sjj=1,2,,NN:モデル上の面の数の感度wjを次のように算出する。なお、ここでは、各モデル面は、同一の法線を持つように区分されているものとする。まず、モデル面Sj上の点psをモデル中心軸に対してtyだけ微小例えばtyはy軸方向のモデル長さの1/100など移動させた点をps’により表すものとする。モデル軸方向にty微小移動させる並進ベクトルをTyと表すと、ps’は、以下の数20で表される。ps’=psTy数20ここで、点psから点ps’へ向かう長さ1の3次元ベクトルをvec1とする。更に、面Sjの法線を表す長さ1の3次元ベクトルをvec2とする。このとき、面Sjの感度wjを、vec1とvec2との内積vec1vec2を用いて以下の数21により定義する。wj=|vec1vec2|数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以外の色を用いてポリゴンを描画する。これにより、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をGPUGraphicsProcessingUnit上で高速に行ない、描画された結果の画像を読み出しても良い。ポリゴンモデルを描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている3次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。[S203]位置姿勢計測装置10は、第1の位置姿勢算出部16において、3次元形状モデルのポリゴンと距離画像から変換した3次元点との対応をもとに、位置姿勢を算出するための係数行列誤差ベクトルを算出する。以下、距離画像の画素3次元の点と面との対応に基づく係数行列誤差ベクトルの算出方法について説明する。3次元空間における平面の方程式は、一般に、axbycz=ea2b2c2=1の形で表される。ここでは、3次元形状モデル中のある面の計測物体の座標系における平面の方程式をaxbycz=eであるとする。この平面の方程式は、位置姿勢の概略値に基づいて、基準座標系における平面の方程式a’xb’yc’z=e’に変換できる。位置姿勢の概略値に基づく、モデル座標系物体座標系の3次元点Xm=[xm,ym,zm]Tから基準座標系の3次元点Xc=[xc,yc,zc]Tへの座標変換を数22のように表す。Xc=RXmT数22但し、Rは姿勢を表す3×3回転行列であり、Tは位置を表す3次元ベクトルである。このとき、基準座標系における平面の方程式の係数は次式のようになる。平面の座標変換については、例えば、金谷形状CADと図形の数学共立出版で詳しく説明がなされている。基準座標系における3次元座標の計測値がx’,y’,z’である点Pが3次元形状モデル中のある面Fと対応付けられているとする。面Fに点Pから下ろした垂線の足をQとする。点Qの基準座標系における3次元座標x、y、zは、計測対象物体の位置姿勢sによって変換されるものであり、ある位置姿勢sのときの3次元座標x0,y0,z0の近傍で1次のテイラー展開によって数24のように近似できる。正しいsによって変換されるx、y、zは、法線ベクトルが面Fと同じで点Pを通る平面a’xb’yc’z=e’,e’=a’x’b’y’c’z’上にあると仮定すると、数24を平面の方程式に代入することにより数25が得られる。数25の観測方程式は対応付けが行なわれた全ての点群データについて立式することができる。数25の観測方程式は、sの各成分の微小変化Δsii=1,2,,6についての方程式になっており、数26に示すΔsiに関する線形連立方程式を立てることができる。この線形連立方程式を数27のように表す。J1Δs=E1数27左辺のJ1が算出対象となる係数行列であり、右辺のE1が誤差ベクトルである。次に、図8を参照して、S303及びS304の処理について説明する。[S303]位置姿勢計測装置10は、第2の位置姿勢算出部17において、微小変化するパラメータをΔtyのみに限定して、S302と同様の処理を行なう。これにより、モデル線分上の制御点と画像上のエッジとの対応から、Δtyに関する数28のような線形連立方程式を立てることができる。この線形連立方程式を数29のように表す。J2Δty=E2数29左辺のJ2が算出対象となる係数行列であり、右辺のE2が誤差ベクトルである。[S304]位置姿勢計測装置10は、第2の位置姿勢算出部17において、S103の処理で算出された第2の位置姿勢パラメータに対する各面Sjの感度に基づき、各モデル幾何特徴のパラメータ算出への寄与度を重み付けする。モデル幾何特徴は、画像上の各画素に投影されるモデル面上の点と、その面の法線により表される。つまり、各幾何特徴はモデルを構成する面のいずれかの投影像である。そこで、モデル面Sjの投影により生成されたモデル点には、そのモデル面Sjと組にしてあるwjを重みとして組にする。ここで、各制御点の重みを用いて数30のように重み行列Wを定義する。重み行列Wは、対角成分以外は全て0の正方行列である。対角成分ws,wt,には、行列J2の各行に対応するモデル幾何特徴の重みが入る。これにより、各幾何特徴に対して、感度の値が大きいデータには大きな重みを、値が小さいデータには小さな重みを与えるようにして、Δtyの推定に寄与しないデータの影響を抑制する。この重み行列Wを用いて、数29を数31のように変形する。WJ2Δty=WE2数31以上説明したように実施形態2によれば、変形実施形態1-1と同様の物体に対して、距離画像を利用して位置姿勢の計測推定を行なえる。これにより、モデル座標系の特定軸の並進方向を決める特徴が少ない物体に対してもロバスト且つ効率良く位置姿勢の算出が可能となる。具体的には、前段の処理において、3次元形状モデルと画像との3次元点の対応を利用して物体の位置姿勢を推定し、後段の処理において、モデルの特定軸並進方向に限定した位置合わせを行なう。モデルの幾何特徴に関して、特定軸並進方向の位置合わせへの影響度を予め算出しておき、後段のパラメータ推定時に重み付けして利用する。また、2次元画像及び距離画像を同時に取得できる場合、第1の位置姿勢を算出する前段の位置合わせ、第2の位置姿勢を算出する後段の位置合わせにおいて、2次元画像を利用する手法と距離画像を利用する手法とをそれぞれ切り替えて行なっても良い。また、それら画像を同時に利用しても良い。すなわち、2次元画像及び距離画像の少なくとも一方を用いて計測対象物体の位置姿勢の算出を行なっても良い。その他の実施形態本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェアプログラムを、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ又はCPUやMPU等がプログラムを読み出して実行する処理である。"