"計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の距離画像を入力する距離画像入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。前記第1の位置及び姿勢は、位置及び姿勢の一部の成分であることを特徴とする請求項1に記載の位置姿勢計測装置。前記第1の位置及び姿勢は、当該位置姿勢計測装置のカメラの光軸に垂直な位置及び光軸まわりの姿勢であることを特徴とする請求項2に記載の位置姿勢計測装置。前記概略位置姿勢入力手段は、前記濃淡画像または前記距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される前記物体の位置及び姿勢を概略の位置及び姿勢として入力することを特徴とする請求項1乃至3のいずれか1項に記載の位置姿勢計測装置。前記位置姿勢算出手段は、前記第1の位置及び姿勢を算出する前に、前記距離画像の情報を用いて位置及び姿勢のうち、当該位置姿勢計測装置からの奥行きに相当する位置を算出し、前記概略の位置及び姿勢を更新することを特徴とする請求項1乃至4のいずれか1項に記載の位置姿勢計測装置。前記画像特徴群における画像特徴は、エッジであることを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。前記画像特徴群における画像特徴は、特徴点であることを特徴とする請求項1乃至5のいずれか1項に記載の位置姿勢計測装置。計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の表面上における点群の3次元座標を入力する3次元データ入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力手段から得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有することを特徴とする位置姿勢計測装置。計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、前記物体の3次元形状モデルを保存する保存ステップと、前記物体の濃淡画像を入力する濃淡画像入力ステップと、前記物体の距離画像を入力する距離画像入力ステップと、前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出ステップとを有することを特徴とする位置姿勢計測方法。計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置による位置姿勢計測方法であって、前記物体の3次元形状モデルを保存する保存ステップと、前記物体の濃淡画像を入力する濃淡画像入力ステップと、前記物体の表面上における点群の3次元座標を入力する3次元データ入力ステップと、前記位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力ステップと、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力ステップから得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出ステップとを有することを特徴とする位置姿勢計測方法。工業製品の生産過程では時間に対する制約が厳しいため、その一工程である部品の位置及び姿勢の計測はできる限り高速に行われる必要がある。また、ロボットの自己位置推定など、位置及び姿勢の計測を高速に処理しなければならない場面は他にも多い。前述した濃淡画像距離画像に対する3次元形状モデルの当てはめによる位置及び姿勢の推定は、1モデルと画像の対応付け、2対応付け結果に基づく位置及び姿勢の算出、の2つのステップからなる。高精度な位置及び姿勢を推定するため、これらのステップは複数回繰り返されるのが一般的である。これら2つのステップのうち、モデルと画像の対応付け処理にかかる計算時間が問題になることが多い。上記非特許文献1に開示される方法では、位置及び姿勢の初期値に基づいて3次元形状モデル中の各線分を画像上に投影し、画像上の投影像の近傍で対応するエッジを探索することにより、モデルと画像の対応付け処理を高速化している。一方、距離画像と3次元形状モデルの対応付けでは、3次元空間において最も近傍にある点を探索する必要がある。上記非特許文献2では、kd-treeを用いて最近傍点の探索を行っている。しかしこの方法では、オーダーがONlogMの計算が必要になるNはデータ側の点数、Mはモデル側の点数ため、各画素が計測点となる距離画像のように計測点数が非常に多いデータを利用する場合に時間がかかる。これに対し、従来では、上記非特許文献1の方法と同様に、画像上にモデルを投影することにより対応付けを高速化する方法が提案されている。具体的には、まず位置及び姿勢の初期値に基づいて3次元形状モデル内の幾何特徴の画像への投影計算を行い、幾何特徴の画像上での位置を算出する。次に、この幾何特徴の画像上での位置をもとに、幾何特徴と距離画像の画素とを対応付けすることにより、高速な対応付けを実現している。しかしながら、前述の3次元形状モデルの画像上への投影に基づいた画像と3次元形状モデルの対応付けでは、3次元形状モデルの投影像と実際の物体の像とが十分にオーバーラップしていることが前提になっている。そのため、投影に用いる位置及び姿勢の初期値と実際の位置及び姿勢とのずれが大きい場合には誤対応が多く生じ、後段の位置及び姿勢の算出に失敗する。濃淡画像のエッジの対応付けでは明示的な特徴エッジを用いているため、初期値のずれに対して比較的ロバストである。一方、距離画像の対応付けでは、濃淡画像上のエッジのような明示的な特徴を用いていないため、初期値のずれに対してロバスト性が低く、位置及び姿勢の算出に失敗する要因となる。逆に初期値のずれが小さく距離画像とモデルが正しく対応付けできる場合には、距離画像の利用によって高精度な位置及び姿勢の計測が可能になる。このように、初期値のずれに対するロバスト性は、濃淡画像と距離画像で夫々特性が異なる。言い換えると、従来においては、この特性を考慮することまでは行っておらず、良い特性を持つ計測データを段階的選択的に使うことによる初期値ずれに対するロバスト化に改善の余地があった。本発明は、このような問題点に鑑みてなされたものであり、初期値のずれ具合の段階に応じてよい特性を持つ計測データを利用することにより、画像と3次元形状モデルの誤対応を減らし、高速でロバストな物体の位置及び姿勢の計測を実現することを目的とする。本発明の位置姿勢計測装置は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の距離画像を入力する距離画像入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢を用いて算出される前記3次元形状モデルの画像面上での投影位置をもとに、前記距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有する。本発明の位置姿勢計測装置における他の態様は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の3次元形状モデルを保存する保存手段と、前記物体の濃淡画像を入力する濃淡画像入力手段と、前記物体の表面上における点群の3次元座標を入力する3次元データ入力手段と、当該位置姿勢計測装置に対する前記物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と、前記濃淡画像の情報を利用して、前記物体の概略の位置及び姿勢をもとに前記物体の第1の位置及び姿勢を算出するとともに、前記第1の位置及び姿勢をもとに、前記3次元データ入力手段から得られる3次元座標の点群である3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、前記3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する位置姿勢算出手段とを有する。また、本発明は、上述した位置姿勢計測装置による位置姿勢計測方法を含む。本発明によれば、濃淡画像と距離画像を段階的選択的に用いることにより、位置及び姿勢の初期値と実際の位置及び姿勢とのずれに対してロバストな物体の位置及び姿勢の計測が可能になる。以下に、図面を参照しながら、本発明を実施するための形態実施形態について説明する。第1の実施形態第1の実施形態では、まず濃淡画像の情報を用いて物体の位置及び姿勢を推定した後、濃淡画像上の特徴と距離画像に対して同時にモデルが当てはまるように位置及び姿勢の推定を行う。本実施形態では、位置及び姿勢の初期値は与えられるものとする。実際の位置及び姿勢と初期値とのずれが大きい場合、モデルを投影することによる距離画像3次元点群と3次元形状モデルの対応付けは、誤対応が頻発する。第1の実施形態では、距離画像の対応付けを行う前に、画像特徴を利用した位置及び姿勢の推定を行ってモデルの投影像と実際の物体像との画像上のオーバーラップ領域をできるだけ広げ、距離画像の対応付けにおける誤対応を削減する。図1は、本発明の第1の実施形態に係る位置姿勢計測装置を含む位置姿勢計測システムの概略構成の一例を示すブロック図である。図1に示すように、位置姿勢計測システムは、位置姿勢計測装置100、濃淡画像撮像部200、及び、距離計測部300を有して構成されている。また、図1に示すように、位置姿勢計測装置100は、3次元形状モデル保存部110、概略位置姿勢入力部120、濃淡画像入力部130、画像特徴検出部140、距離画像入力部150、及び、位置姿勢算出部160を有して構成されている。また、位置姿勢計測装置100には、濃淡画像撮像部200及び距離計測部300が接続されている。濃淡画像撮像部200は、通常の濃淡画像を撮影するカメラである。撮影される画像は、濃淡画像であってもよいしカラー画像であってもよい。濃淡画像撮像部200が撮影する画像は、濃淡画像入力部130を介して位置姿勢計測装置100に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば、上記非特許文献4に係るZhangの方法によって事前にキャリブレーションしておく。距離計測部300は、計測対象である物体表面上の点までの距離を計測する。本実施形態では、距離計測部300として距離画像を出力する距離センサを用いる。距離画像は、各画素が距離計測部300からの距離の情報を持つ画像である。本実施形態では、各画素には距離の情報ではなく、距離をもとに算出される計測装置基準の座標系における3次元座標が保存されているものとする。即ち、距離画像からは、3次元点群データが得られる。距離値から3次元点群への変換は、距離画像の各画素について、画素位置に対応する視線ベクトルに距離値を乗算することにより行う。距離センサとして、例えば、液晶プロジェクタから物体に照射した2次元のパターン光をカメラで撮影し、三角測量により距離を計測するアクティブ方式のものを利用する。なお、距離センサはこれに限るものではなく、光切断法やTimeofflight方式など他のアクティブ式のものや、ステレオカメラによるパッシブ式など、距離画像を出力可能であれば如何なる距離センサであってもよい。本実施形態では、距離センサ用のカメラと濃淡画像撮像部200のカメラは同一のものを用いる。即ち、同一視点における濃淡画像と距離画像が取得できると仮定する。距離センサ用のカメラと濃淡画像撮像部200のカメラが異なる場合には、両者の相対的な位置及び姿勢を事前にキャリブレーションしておく。例えば3次元形状が既知であるキャリブレーション物体を撮影し、濃淡画像に基づいた位置及び姿勢と、距離画像に基づいた物体の位置及び姿勢から相対的な位置及び姿勢を求める。距離計測部300が計測した距離画像は、距離画像入力部150を介して位置姿勢計測装置100に入力される。次に、位置姿勢計測装置100を構成する各部について説明する。3次元形状モデル保存部110には、位置及び姿勢を計測する対象である物体の3次元形状モデルを保存する。本実施形態では、物体を線分と面から構成される3次元形状モデルとして記述する。図2は、本発明の第1の実施形態を示し、3次元形状モデルを説明する模式図である。3次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図2aに示すように、計測対象物体10-1の3次元形状モデルは、点P1~点P14の14点から構成される。また、図2bに示すように、計測対象物体10-2の3次元形状モデルは、線分L1~L16により構成されている。また、図2cに示すように、点P1~点P14は3次元座標値によって表される。また、図2dに示すように、線分L1~L16は、線分を構成する点のIDによって表される。さらに、3次元形状モデルは、面の情報を保持する。各面は、各面を構成する点のIDによって表される。図2に示す3次元形状モデルでは、直方体を構成する6つの面の情報が記憶されている。3次元形状モデルは、位置姿勢算出部160において物体の位置及び姿勢を算出する際に用いられる。概略位置姿勢入力部120は、位置姿勢計測装置100に対する物体の位置及び姿勢の概略値を入力する。位置姿勢計測装置100には、位置及び姿勢計測の基準となる3次元の座標系基準座標系が規定されているものとする。位置姿勢計測装置100に対する物体の位置及び姿勢とは、基準座標系における物体の位置及び姿勢を表す。本実施形態では、濃淡画像撮像部200であるカメラのカメラ中心を原点とし、画像の水平方向をx軸、垂直方向をy軸、カメラの光軸をz軸とした座標系を基準座標系とする。本実施形態では、位置姿勢計測装置100は、時間軸方向に連続して計測を行うものとして、前回前時刻の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測結果をもとに位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測したものでもよい。また、物体が置かれているおおよその位置や姿勢が予めわかっている場合には、その値を概略値として用いる。この概略位置姿勢入力部120は、濃淡画像または距離画像の情報から位置及び姿勢の初期値が不要な算出方法によって算出される物体の位置及び姿勢を概略の位置及び姿勢として入力する。濃淡画像入力部130は、濃淡画像撮像部200で撮影された濃淡画像を位置姿勢計測装置100に入力する処理を行う。画像特徴検出部140は、濃淡画像入力部130から入力された濃淡画像から画像特徴を検出する。本実施形態では、画像特徴としてエッジを検出する。具体的には、概略位置姿勢入力部120から入力される物体の位置及び姿勢の概略値に基づいて、3次元形状モデルの各線分を画像上に投影し、投影された線分の近傍で対応するエッジ探索を行う。距離画像入力部150は、距離計測部300で得られた距離画像を位置姿勢計測装置100に入力する処理を行う。位置姿勢算出部160は、画像特徴検出部140で検出された画像特徴、及び距離画像入力部150によって入力された3次元点群と、3次元形状モデル保存部110に保存される3次元形状モデルをもとに、物体の位置及び姿勢を計測する。図3は、本発明の第1の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。<ステップS1010>概略位置姿勢入力部120は、位置姿勢計測装置100カメラに対する物体の位置及び姿勢の概略値を位置姿勢計測装置100に入力する。前述したように、本実施形態では、前回計測された位置及び姿勢を概略値として用いる。<ステップS1020>濃淡画像入力部130及び距離画像入力部150は、物体の位置及び姿勢を算出するための計測データを取得する。具体的には、濃淡画像入力部130及び距離画像入力部150は、計測対象物体の濃淡画像と距離画像を取得する。前述したように、距離画像の各画素には、基準座標系における点の3次元座標が記録されている。濃淡画像撮像部200によって撮影された濃淡画像は、濃淡画像入力部130を介して位置姿勢計測装置100に入力される。また、距離計測部300から出力された距離画像は、距離画像入力部150を介して位置姿勢計測装置100に入力される。<ステップS1030>画像特徴検出部140及び位置姿勢算出部160は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測算出を行う。ステップS1030では、濃淡画像上の特徴であるエッジに3次元形状モデル中の線分の投影像が当てはまるように、計測対象物体の位置及び姿勢を算出する。図4は、図3のステップS1030における位置姿勢計測位置姿勢算出の処理手順の一例を示すフローチャートである。ここでは、画像上で検出されたエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和が最小になるように位置及び姿勢の最適化を行う。以下の説明では、物体の位置及び姿勢を6次元ベクトルsで表す。sは、計測対象物体の位置を表す3つの要素と、姿勢を表す3つの要素からなる。姿勢を表す3つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す3次元ベクトルなどによって表現される。以下、図3のステップS1030における位置姿勢算出の処理手順について順に説明する。≪ステップS1110≫まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS1010で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。≪ステップS1120≫続いて、画像特徴検出部140は、ステップS1020において入力された濃淡画像上において画像特徴の検出画像特徴と3次元形状モデルとの対応付けを行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配が極値となる点である。本実施形態では、上記非特許文献1で開示される方法によりエッジ検出を行う。この非特許文献1で開示されるエッジ検出方法は、トップダウン型の方法であり、モデル側から対応するエッジを探索する方法であるため、検出と対応付けが一度に行われる。図5は、本発明の第1の実施形態を示し、エッジ検出を説明する模式図である。まず、ここでは、ステップS1110において入力された計測対象物体の位置及び姿勢の概略値と校正済みのカメラの内部パラメータを用いて、3次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は、画像上でも線分となる。次に、画像上で等間隔になるように投影線分上に制御点を設定し、制御点を通りかつ投影された線分に直交する線分上で1次元のエッジ検出を行う図5a。エッジは、画素値の濃度勾配の極値として検出されるため、図5bに示すように、近傍にエッジが存在する場合には、複数のエッジが検出されることがある。本実施形態では、検出されるエッジのうち最も濃度勾配の大きいものを対応するエッジとする。≪ステップS1130≫続いて、例えば位置姿勢算出部160は、3次元形状モデルの線分上の制御点と画像上のエッジとの対応をもとに、位置及び姿勢を算出するための係数行列誤差ベクトルを算出する。図6は、本発明の第1の実施形態を示し、線分の投影像と検出されたエッジとの関係を説明する模式図である。図6では、画像の水平方向、垂直方向をそれぞれu軸、v軸としている。位置及び姿勢の概略値s0に基づいて投影された線分上のある制御点の画像上での位置をu0,v0、当該制御点が所属する線分の画像上での傾きをu軸に対する傾きθと表す。傾きθは、線分の両端の3次元座標をsの概略値s0に基づいて画像上に投影し、画像上での両端の座標を結んだ直線の傾きとして算出する。当該線分の画像上での法線ベクトルはsinθ,-cosθとなる。また、当該制御点の対応点エッジの画像座標をu,vとする。ここで、点u,vを通り、傾きがθである直線図6の破線上の点u,vは、以下の数式1と表せるθは定数とする。制御点の画像上での位置は、計測対象物体の位置及び姿勢により変化する。制御点の画像座標u,vはu0,v0の近傍で1次のテイラー展開によって以下の数式2のように近似できる。但しΔsii=1,2,,6はsの各成分の微小変化を表す。正しいsによって得られる制御点の画像上での位置が数式1によって表される直線上にあると仮定すると、数式2を数式1に代入することにより、観測方程式である以下の数式3が得られる。数式3の観測方程式は、対応付けが行われたすべての制御点について立式することができる。数式3はsの各成分の微小変化Δsii=1,2,,6についての方程式になっており、以下の数式4に示すΔsiに関する線形連立方程式を立てることができる。この線形連立方程式を以下の数式5のように表す。数式5において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。≪ステップS1140≫続いて、例えば位置姿勢算出部160は、数式5をもとに、位置及び姿勢の補正値Δsを最小二乗基準で求める。最小二乗基準では、以下の数式6を最小化するように位置及び姿勢の補正値Δsを求める。ただし、Jijは係数行列Jのi行j列目の要素を表し、Eiは誤差ベクトルEのi番目の要素を表す。数式6を最小にするΔsは、行列Jの一般化逆行列JtJ1Jtを用いて、以下の数式7のように求める。しかしながら、エッジの対応付けでは、誤対応外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジでは、数式4の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制する。重みは、例えば以下の数式8に示すようなTukeyの関数により与える。ここで、数式8において、c1は定数である。なお、重みを与える関数は、Tukeyの関数である必要はなく、例えばHuberの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であれば何でもよい。そして、各計測データに対応する重みをwiとする。この重みwiを用いて、数式6を以下の数式9のように変形する。そして、例えば位置姿勢算出部160は、数式9を最小にする補正値Δsを以下の数式10のように解くことにより求める。ただし、数式10の行列Wは、以下の数式11で定義される重み行列である。数式11の重み行列Wは、対角成分以外はすべて0の正方行列であり、対角成分には重みwiが入る。≪ステップS1150≫続いて、例えば位置姿勢算出部160は、ステップS1140において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。s←sΔs≪ステップS1160≫続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS1130に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。以上の処理によって、ステップS1010で入力された位置及び姿勢の概略値が濃淡画像の情報を用いて更新される。この結果、位置及び姿勢の概略値に基づく画像上における3次元形状モデルの投影像と実際の物体の像とのオーバーラップ領域が広がるため、3次元形状モデルの投影に基づく距離画像と3次元形状モデルの対応付けにおける誤対応が減少する。<ステップS1040>次に、濃淡画像及び距離画像を用いた位置及び姿勢の算出処理について説明する。本ステップでは、画像特徴検出部140及び位置姿勢算出部160は、ステップS1030で更新された位置及び姿勢の概略値を用いて、濃淡画像、距離画像の双方に対して3次元形状モデルが当てはまるような位置及び姿勢の推定算出を行う。ここでは、画像上のエッジと推定される位置及び姿勢に基づいて画像上に投影される線分との距離の二乗和と3次元点群と3次元形状モデルとの距離の二乗和の和が最小になるように位置及び姿勢の最適化を行う。図7は、図3のステップS1040における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図4のフローチャートで示されるステップS1030の処理手順と同一であるため、説明が重複する部分は省略する。≪ステップS1210≫まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS1030で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。≪ステップS1220≫続いて、画像特徴検出部140は、ステップS1020において入力された濃淡画像上において画像特徴の検出画像特徴と3次元形状モデルとの対応付けを行う。また、同様にステップS1020において入力された距離画像と3次元形状モデルとの対応付けを行う。濃淡画像に対する処理は、ステップS1120の処理と同じであるため、説明を省略する。以下、距離画像と3次元形状モデルとの対応付け処理について説明する。図8は、本発明の第1の実施形態を示し、図7のステップS1220における距離画像と3次元形状モデルとの対応付け処理を説明する模式図である。本ステップでは、上記非特許文献5で開示されている方法と同様に、物体の3次元形状モデルポリゴンモデル810を距離画像面820上に投影することによって対応付けを行う。ここで、3次元形状モデルを距離画像上に投影する理由は、距離画像の各画素がポリゴンモデル810のどのポリゴンに対応するかを決定するためである。ここでは、距離画像と同サイズの画像バッファに対し、校正済みであるカメラの内部パラメータと位置及び姿勢の概略値を用いてポリゴンモデル810を描画する。以下、ポリゴンモデル810を描画した画像をポリゴン描画画像と呼ぶ。描画する際には、各ポリゴンに固有の色を割り当てて描画を行う。ポリゴンの色はRGBの各色で表す。背景部分と区別するために、黒R=0、G=0、B=0以外の色を用いてポリゴンを描画する。これによって、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。なお、ポリゴンの描画処理をGPUGraphicsProcessingUnit上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデル810を描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている3次元座標とポリゴン描画画像上の色に相当するポリゴンと対応付ける。≪ステップS1230≫続いて、例えば位置姿勢算出部160は、3次元形状モデルの線分上の制御点と画像上のエッジとの対応、ポリゴンと3次元点の対応をもとに、位置及び姿勢を算出するための係数行列誤差ベクトルを算出する。モデルの線分上の制御点と画像上のエッジとの対応に基づく係数行列誤差ベクトルの算出については、ステップS1130と同じであるため説明を省略する。以下では、距離画像の画素3次元の点と面との対応に基づく係数行列誤差ベクトルの算出方法について説明する。3次元空間における平面の方程式は一般に、axbycz=ea2b2c2=1の形で表される。ここでは、3次元形状モデル中のある面の計測物体の座標系における平面の方程式をaxbycz=eであるとする。この平面の方程式は、位置及び姿勢の概略値に基づいて、基準座標系における平面の方程式axbycz=eに変換できる。位置及び姿勢の概略値に基づく計測物体座標系から基準座標系への座標変換を、以下の数式12のように表す。ただし、数式12において、Rは姿勢を表す3×3回転行列、tは位置を表す3次元ベクトルである。このとき、基準座標系における平面の方程式の係数は、以下の数式13のようになる。平面の座標変換については、例えば金谷形状CADと図形の数学共立出版で詳しく説明がなされている。基準座標系における3次元座標の計測値がx,y,zである点Pが3次元形状モデル中のある面Fと対応付けられているとする。面F上にある点をQとする。点Qの基準座標系における3次元座標x,y,zは、計測対象物体の位置及び姿勢sによって変換するものであり、ある位置及び姿勢sのときの3次元座標x0,y0,z0の近傍で1次のテイラー展開によって、以下の数14のように近似できる。正しいsによって変換されるx,y,zは、法線ベクトルが面Fと同じで点Pを通る平面axbycz=e,e=axbycz上にあると仮定すると、数式14を平面の方程式に代入することで以下の数式15が得られる。数式15の観測方程式は、対応付けが行われたすべての点群データについて立式することができる。数式3及び数式15の観測方程式は、sの各成分の微小変化Δsii=1,2,,6についての方程式になっており、数式15に示すΔsiに関する線形連立方程式を、以下の数式16のように立てることができる。この数式16に示す線形連立方程式を、以下の数式17のように表す。数式17において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。≪ステップS1240≫続いて、例えば位置姿勢算出部160は、数式17をもとに、位置及び姿勢の補正値Δsを算出する。しかしながら、エッジの対応付けデータや距離画像の対応付けデータに誤対応などの外れ値が混入することは避けられない。そのため、ステップS1140で述べたようなロバスト推定手法により重み行列Wを用いて、以下の数式18により補正値Δsを算出する。≪ステップS1250≫続いて、例えば位置姿勢算出部160は、ステップS1240において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。s←sΔs≪ステップS1260≫続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS1230に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。上述したように、第1の実施形態に係る位置姿勢計測装置100では、3次元形状モデルが濃淡画像の画像特徴及び距離画像に当てはまるように、位置姿勢計測装置100に対する物体の概略の位置及び姿勢を更新する位置姿勢算出部160を有している。そして、この位置姿勢算出部160では、少なくとも濃淡画像の情報を利用して画像面上での物体像と3次元形状モデルの投影像が最もオーバーラップするように第1の位置及び姿勢を算出する図3のS1030。なお、第1の位置及び姿勢は、位置及び姿勢の一部の成分であり、具体的には、当該位置姿勢計測装置100のカメラの光軸に垂直な位置及び光軸まわりの姿勢である。また、位置姿勢算出部160では、第1の位置及び姿勢を用いて画像面上に投影される3次元形状モデルの投影像をもとに3次元形状モデルと濃淡画像の画像特徴及び距離画像との対応付けを行って、第2の位置及び姿勢を算出する図3のS1040。このように、第1の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定算出して位置及び姿勢の概略値を更新した後、3次元形状モデルを投影することによる距離画像と3次元形状モデルの対応付けを行うことで誤対応を削減する。その結果、濃淡画像と距離画像を用いることで高精度に位置及び姿勢の推定算出が行えるとともに、位置及び姿勢の概略値のずれに対してロバストになる。[変形例1-1]2Dの自由度を減らす上述した第1の実施形態では、前段の処理である濃淡画像のみを利用する位置及び姿勢の推定において、位置及び姿勢の6自由度すべてを推定した。しかしながら、この処理においてはすべての自由度を推定する必要はなく、濃淡画像を用いて精度高く推定できる成分のみを推定してもよい。背景や物体の構造が複雑な場合には、濃淡画像上のエッジとモデルが誤対応することは避けられない。推定する成分を限定することで計測データに対するオーバーフィッティングが緩和されるため、誤対応に対するロバスト性の向上が期待できる。本変形例では、濃淡画像上のエッジを利用して、カメラの光軸に垂直な方向の位置成分と光軸まわりの姿勢成分の3つを未知パラメータとして推定する。これら3つのパラメータを3次元ベクトルsで表す。制御点の画像座標u,vはu0,v0の近傍で1次のテイラー展開によって、以下の数式19のように近似できる。但しΔsii=1,2,3はsの各成分の微小変化を表す。数式19を数式1に代入することにより、以下の観測方程式である数式20が得られる。対応付けが行われたすべての制御点について立式される数式20を連立方程式として、第1の実施形態と同様の処理によって補正値Δsii=1,2,3を算出する。本変形例における処理は、概略の位置及び姿勢に基づく投影像の形状と実際の物体の画像上での形状が類似している場合に有効である。[変形例1-2]繰り返し計算の中で対応付けをやり直してもよい上述した第1の実施形態では、ステップS1030、S1040の位置及び姿勢の算出処理において、計測データと3次元形状モデルとの対応付け情報は、固定した情報として取り扱った。しかし、実際には、推定する位置及び姿勢の変化によって対応付け情報は変化する。そのため、位置及び姿勢を更新した後で、再度、対応付け処理をやり直してもよい。ただし、対応付け処理は時間がかかる処理であるため、位置及び姿勢が更新されるたびに対応付けをやり直すのではなく、数回更新された後に対応付け処理を行ってもよい。[変形例1-3]概略値はclosedformで求めてもよい上述した第1の実施形態では、位置及び姿勢の概略値を、過去の計測結果から入力した。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではなく、濃淡画像や距離画像の情報を元に繰り返し計算なしに直接的に算出してもよい。例えば、画像全体から検出された線分と3次元形状モデル中の線分と対応付けて位置及び姿勢を直接的に算出してもよい。具体的には、まず画像全体に対して例えばCannyのエッジ検出器によってエッジ検出を行い、隣接するエッジ群をラベリングする。次に、同一ラベルを持つエッジ群に対して、折れ線近似によって線分当てはめを行う。このようにして得られた画像上の線分と3次元形状モデル中の線分の対応をもとに上記非特許文献6に係るLiuの手法により位置及び姿勢を算出してもよい。また、例えば、上記非特許文献7に係るJohnsonの方法により、距離画像に基づいて概略の位置及び姿勢を算出してもよい。また、例えば、上記非特許文献9に係るUlrichらの方法のように、物体の姿勢に応じた固有のパターンを画像上で網羅的に探索する方法によって位置及び姿勢の概略値を算出してもよい。固有のパターンは、濃淡値からなるテンプレート画像であってもよいし、二値のエッジテンプレート画像であってもよい。また、網羅的に探索する方法は、姿勢クラスを識別するパターン認識による方法であってもよい。[変形例1-4]第1の位置姿勢を濃淡距離から算出上述した第1の実施形態では、前段の処理において濃淡画像のみを利用して位置及び姿勢を推定した。しかしながら、濃淡画像の情報だけでなく、誤対応が悪い影響を与えない程度に距離画像の情報を利用してもよい。具体的には、第1の実施形態のステップS1030の処理のかわりにS1040の処理を実施する。このとき、距離画像の誤対応が位置及び姿勢の算出に大きく影響しないように、数式8における閾値c1の値を大きくして全体的に重みが小さくなるようにする。第2の実施形態2Dの前に奥行きを求める上述した第1の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢を推定して位置及び姿勢の概略値を更新した後、3次元形状モデルを投影することによる距離画像と3次元形状モデルの対応付けを行った。しかしながら、位置及び姿勢の概略値の更新方法は、これに限るものではない。例えば、距離画像のデータを用いて、位置及び姿勢のうち、カメラからの奥行きに相当する位置成分を推定算出してもよい。位置及び姿勢の概略値の真の値に対する誤差が大きい場合は、3次元形状モデルを投影することにより距離画像と3次元形状モデルを正確に対応付けることは難しい。しかしながら、カメラから物体表面までの距離がほぼ同一とみなせる場合には、多少対応付けが間違っていたとしても、奥行きを真の値に近づけることは可能である。そこで本実施形態では、まず距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第1の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値が真の値と大きく異なる場合であっても、ロバストに位置及び姿勢を計測算出することが可能になる。図9は、本発明の第2の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。図9の各ステップのうち、ステップS2010、S2020、S2040については、それぞれ、第1の実施形態における図3のステップS1010、S1020、S1040と同一であるため、その説明を省略する。以下では、図9の処理のうち、ステップS2025、S2030について説明する。<ステップS2025>ステップS2025では、例えば位置姿勢算出部160は、距離画像の情報に基づいて物体の位置及び姿勢のうち、奥行きに相当する位置成分の概略値を更新する。具体的な処理としては、まずステップS2010において入力される位置及び姿勢の概略値をもとに、第1の実施形態のステップS1220において説明した方法により、距離画像と3次元形状モデルとの対応付けを行う。次に、例えば位置姿勢算出部160は、対応付け結果に基づいて、物体の位置を更新する。物体の位置は、次のように更新する。ここで、基準座標系における物体の位置の概略値を表す3次元ベクトルをt0=[txtytz]tとする。次に、例えば位置姿勢算出部160は、モデル面との対応付けがなされている距離画像上の画素について、基準座標系の原点カメラの投影中心から出て当該画素を通る光線と3次元形状モデル面との交点の3次元座標を算出する。光線上の点の3次元座標xは、光線の方向ベクトルをd=[dxdydz]tとすると、x=αdによって表されるαは媒介変数。この点が基準座標系におけるモデル面axbycz=e上にあるとして、αを求めてxを算出することにより、交点の3次元座標を算出する。距離画像上の画素に保持されている3次元座標をxdata、算出された交点の3次元座標をxcalcとする。xcalcのz成分の平均値/zcalcがxdataのz成分の平均値/zdataに一致するように物体の位置t0を更新する。更新した位置tは、以下の数式21により算出する。単純に位置t0のz成分のみを更新しないのは、物体の観察方向に応じてz成分の位置への影響が変わるためである。<ステップS2030>画像特徴検出部140及び位置姿勢算出部160は、濃淡画像の情報に基づいて物体の位置及び姿勢の計測算出を行う。基本的な処理は、第1の実施形態におけるステップS1030と同一であるが、入力する位置及び姿勢の概略値として、ステップS2025によって更新された位置及び姿勢の概略値を利用する。以降の処理の説明は省略する。このように、第2の実施形態では、距離画像を用いて物体の奥行きに相当する位置成分を推定し、その後で第1の実施形態において述べた処理を行う。これによって、奥行きに相当する位置成分の概略値のずれが補正され、後段の処理において誤対応を削減することが可能である。[変形例2-1]エッジは特徴点であってもよい以上、説明した本発明の実施形態、変形例では、濃淡画像上の特徴はエッジであり、3次元形状モデル中の線分をエッジに当てはめることにより位置及び姿勢の計測を行った。しかしながら、濃淡画像上の特徴はエッジに限るものではなく、例えば2次元的な特徴である特徴点であってもよい。特徴点は、コーナー点など画像上で特徴的な点であり、例えば、上記非特許文献8に係るHarrisの検出器によって検出することができる。3次元形状モデルを3次元点群によって表すことで、各特徴点をモデル中の3次元点群の点と対応付けることができる。特徴点を用いる場合には、まず画像全体からHarrisの検出器によって特徴点を検出する。次に、モデル中の3次元点群の各点を位置及び姿勢の概略値に基づいて画像上に投影し、画像上で最も近い特徴点と対応付けを行う。第3の実施形態後段の位置姿勢推定は2.5Dのみ利用上述した実施形態では、後段の位置及び姿勢の推定においては濃淡画像と距離画像の双方の情報を用いていた。しかしながら、後段の位置及び姿勢の推定方法はこれに限るものではない。計測対象物体が多面体である場合、少なくとも3面以上の十分な面積を持つ面が撮像装置から見えていれば、距離画像のみから精度よく位置及び姿勢を推定することができる。また、計測対象物体が曲面から構成される場合には、撮像装置から観察される物体表面上の点の法線ベクトルが十分に3次元的に分布していれば距離画像のみから精度よく位置及び姿勢を推定することができる。そこで本実施形態では、まず、第1の実施形態で述べたように、濃淡画像を用いて物体の位置及び姿勢の概略値を更新した後に、更新された概略値を初期値として距離画像のみを用いて位置及び姿勢を推定する。図10は、本発明の第3の実施形態に係る位置姿勢計測装置により行われる位置姿勢計測処理の処理手順の一例を示すフローチャートである。図10の各ステップのうち、ステップS3010、S3020、S3030については、それぞれ、第1の実施形態における図3のステップS1010、S1020、S1030と同一であるため、その説明を省略する。以下では、図10の処理のうち、ステップS3040について説明する。<ステップS3040>ステップS3040では、位置姿勢算出部160は、ステップS3030で更新された位置及び姿勢の概略値を用いて、距離画像に対して3次元形状モデルが当てはまるような位置及び姿勢の推定算出を行う。ここでは、距離画像から得られる3次元点群と3次元形状モデルとの距離の二乗和が最小になるように位置及び姿勢の最適化を行う。図11は、図10のステップS3040における位置姿勢算出の処理手順の一例を示すフローチャートである。本ステップの処理手順は、基本的には、図7のフローチャートで示されるステップS1040の処理手順から濃淡画像に対する処理を除いた処理手順であるため、説明が重複する部分は省略する。≪ステップS3110≫まず、位置姿勢計測装置100では、初期化処理を行う。ここでは、ステップS3030で得られた計測対象物体の概略の位置及び姿勢を位置及び姿勢算出における概略値として入力する。≪ステップS3120≫続いて、画像特徴検出部140は、ステップS3020で入力された距離画像と3次元形状モデルとの対応付けを行う。距離画像と3次元形状モデルとの対応付けについては、ステップS1220の処理と同じであるため、説明を省略する。≪ステップS3130≫続いて、例えば位置姿勢算出部160は、ポリゴンと3次元点の対応をもとに、位置及び姿勢を算出するための係数行列誤差ベクトルを算出する。3次元形状モデルの面と3次元点との対応に基づく係数行列誤差ベクトルの算出方法については、ステップS1230と同じであるため、説明を省略する。本ステップでは、距離画像の情報のみを用いるため、sの各成分の微小変化Δsii=1,2,,6に関する線形連立方程式は以下の数式22になる。この数式22に示す線形連立方程式を、以下の数式23のように表す。数式23において、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。≪ステップS3140≫続いて、例えば位置姿勢算出部160は、数式23をもとに、位置及び姿勢の補正値Δsを算出する。距離画像と3次元形状モデルの対応付けデータに誤対応などの外れ値が混入することは避けられないため、ステップS1140で述べたようなロバスト推定手法により重み行列Wを用いて、以下の数式24により補正値Δsを算出する。≪ステップS3150≫続いて、例えば位置姿勢算出部160は、ステップS3140において算出された位置及び姿勢の補正値Δsにより、位置及び姿勢の概略値を補正する。s←sΔs≪ステップS3160≫続いて、例えば位置姿勢算出部160は、収束判断を行い、収束している場合には、本フローチャートの処理を終了し、一方、収束していない場合には、ステップS3130に戻る。この収束判断では、補正値Δsがほぼ0である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判断する。以上述べたように、第3の実施形態では、まず濃淡画像を用いて物体の位置及び姿勢の概略値を推定した後に、推定した概略値を初期値として距離画像を用いて物体の位置及び姿勢を推定する。これにより、距離画像の情報のみから高精度に位置及び姿勢を推定する場合には、後段の処理において濃淡画像に関する処理を行う必要がないため、高速に位置及び姿勢を推定することが可能になる。第4の実施形態上述した実施形態では、濃淡画像とともに対象物体の3次元の計測データとして距離画像を位置姿勢推定に利用していた。しかしながら、位置姿勢推定に利用する3次元の計測データは画素ごとに3次元座標を持つ距離画像に限るものではなく、物体の表面上における点群の3次元座標を表す計測データであれば他の形態であってもよい。この場合、位置姿勢計測装置100は、物体の表面上における点群の3次元座標を入力する3次元データ入力部不図示を具備する構成となる。そして、位置姿勢算出部160は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出する。さらに、位置姿勢算出部160は、第1の位置及び姿勢をもとに、3次元データ入力部不図示から得られる3次元座標の点群である3次元点群または当該3次元点群及び濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する。ここで、例えば、物体の表面上における点群の3次元座標を表す計測データとしては、3次元座標の集合であってもよい。なお、本実施形態と他の実施形態との違いは、3次元形状モデルと3次元の計測データの対応付けだけであるため、以下では対応付け方法のみを説明し、重複する説明は割愛する。ステップS1220の説明において述べたように、3次元の計測データが距離画像の場合には、物体の位置及び姿勢の概略値に基づいて3次元形状モデルを距離画像上に投影することで距離画像上の各画素と3次元形状モデルのポリゴンとの対応づけを行った。3次元計測データが3次元座標の集合である場合には画像上への投影を行うことができないため、各計測点に最も近いポリゴンを3次元的に探索する。具体的な方法を以下に示す。まず、予め3次元形状モデルを構成する各ポリゴン上に計測点の密度より細かく複数の点以降、面モデル点と呼ぶを設定し、各面モデル点の物体座標系における3次元座標と属するポリゴンのIDを保存しておく。対応付けを行う際には、非特許文献2に開示されるように、kd-treeを用いて各計測点に対応する面モデル点を探索する。面モデル点にはポリゴンのIDが紐づけられているため、この探索を行うことで計測点とポリゴンが対応付けされる。以上述べたように、第4の実施形態では、対象物体の3次元の計測データが画素ごとに3次元座標を持つ距離画像でなくとも、本発明による位置姿勢推定を適用することが可能である。上述した本発明の各実施形態では、位置姿勢算出部160は、濃淡画像の情報を利用して、物体の概略の位置及び姿勢をもとに物体の第1の位置及び姿勢を算出する図3のS1030、図9のS2030、図10のS3030。また、位置姿勢算出部160は、第1の位置及び姿勢を用いて算出される3次元形状モデルの画像面上での投影位置をもとに、距離画像から得られる3次元点群または当該3次元点群及び前記濃淡画像の画像特徴群と、3次元形状モデルとの対応付けを行い、当該対応付け結果に基づいて第2の位置及び姿勢を算出する図3のS1040、図9のS2040、図10のS3040。その他の実施形態また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェアプログラムを、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータまたはCPUやMPU等がプログラムを読み出して実行する処理である。このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記録媒体は、本発明に含まれる。"