JP2011174879A - 位置姿勢推定装置及びその方法 - Google Patents

Abstract

【課題】 物体・撮影環境において、安定的に物体の位置及び姿勢を計測することを目的とする。
【解決手段】 物体を撮影した撮影画像を入力する。物体の３次元座標を含む距離画像を入力する。撮影画像から画像特徴を抽出する。画像特徴に対応する距離画像上の位置における３次元座標から、画像特徴が物体の形状を表すか否かを判別する。物体の形状を表すと判別された画像特徴と物体の形状を有する３次元モデルの一部とを対応付ける。対応付けに基づいて、物体の位置姿勢を推定する。
【選択図】 図１

Description

本発明は、３次元形状が既知である物体の位置及び姿勢を推定する技術に関する。

近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。ハンドなどのエンドエフェクタによってロボットが部品を把持するためには、把持の対象となる部品とロボットとの間の相対的な位置及び姿勢を計測する必要がある。このような位置及び姿勢の計測は、ロボットが部品を把持する場合だけでなく、ロボットが自律移動するための自己位置推定や、拡張現実感における現実空間と仮想物体の位置合わせなど様々な目的に応用される。

位置及び姿勢を計測する方法として、カメラが撮影する２次元画像を用いて行う方法がある。例えば、２次元画像上から検出される特徴に対して物体の３次元形状モデルをあてはめるモデルフィッティングによる計測が一般的である。このとき、２次元画像上で検出される特徴と３次元形状モデル中の特徴が正しく対応付ける必要がある。

非特許文献１では、画像上で検出されるエッジに直線をフィッティングし、画像上の直線と３次元モデルの線分との対応に基づいて、物体の概略の位置姿勢を必要とせずに物体の位置及び姿勢を算出する方法が開示されている。この方法では、少なくとも８本以上の直線の対応に基づいて立式される線形方程式を解くことにより、物体の位置及び姿勢を算出する。以上に示すようなエッジを利用する方法は、テクスチャのない直線を基調としたような人工的な物体が多い環境などに適している。位置姿勢推定を行うためには、画像上の直線と３次元モデルの線分との対応がまったくの未知の状態から、それらの対応を求める必要がある。このような場合、３次元モデルの線分をランダムに対応付けて位置及び姿勢を複数算出しその中から最も整合性の高いものを選択する方法が一般的に行われる。

非特許文献２では、２次元画像上から検出される特徴として、エッジを利用した物体の位置及び姿勢の計測を行う方法が開示されている。この方法では、物体の３次元形状モデルを線分の集合（ワイヤフレームモデル）によって表し、物体の概略の位置姿勢は既知であるとし、画像上で検出されるエッジに３次元の線分の投影像を当てはめることにより物体の位置及び姿勢を計測する。３次元モデルの線分の投影像近傍に存在するエッジのみに対応の探索を限定することで、対応候補となるエッジを削減することが可能である。

非特許文献３では、エッジ周辺の輝度値を利用することで３次元モデルの線分との対応付けの精度を向上させる手法が開示されている。具体的には、濃淡画像中から検出されるエッジ周辺の輝度分布を３次元モデルの線分上に保持しておき、保持している輝度分布に最も類似する輝度分布をもつエッジを対応付ける。これにより、投影位置周辺から対応候補となるエッジが複数検出された場合でも、エッジの誤対応を減少させることができる。また、３次元モデルの線分上に保持する輝度分布を、濃淡画像中から時系列的に取得・更新することで、画像上のエッジ周辺の輝度分布に若干の変化が生じた場合にも、エッジを識別することが可能である。また、特徴点を利用する場合は、特徴点の周辺の画像情報に基づいて対応付けを行っているため、一般的にエッジに比べて識別性が高いといえる。

非特許文献４では、物体の３次元形状モデルを、単純な形状（プリミティブ）の集合として表現し、距離画像から局所面や角などの形状特徴を抽出し、３次元形状モデルとマッチングすることで物体の位置及び姿勢を計測する。距離画像を利用する方法は、３次元形状に特徴があるような物体に適している。非特許文献４では、濃淡画像以外の情報に基づいて識別する。距離画像は、物体の見た目の画像ではなく、物体から撮像装置までの距離値を格納しているため、光源の変化や、物体の表面情報などによる輝度変化の影響を受けることが少ない。

Ｙ．Ｌｉｕ、Ｔ．Ｓ．Ｈｕａｎｇ、ａｎｄ Ｏ．Ｄ．Ｆａｕｇｅｒａｓ、"Ｄｅｔｅｒｍｉｎａｔｉｏｎ ｏｆ ｃａｍｅｒａ ｌｏｃａｔｉｏｎ ｆｒｏｍ ２−Ｄ ｔｏ ３−Ｄ ｌｉｎｅ ａｎｄ ｐｏｉｎｔ ｃｏｒｒｅｓｐｏｎｄｅｎｃｅｓ、"ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ、ｖｏｌ．１２、ｎｏ．１、 ｐｐ．２８−３７、１９９０． Ｔ．Ｄｒｕｍｍｏｎｄ ａｎｄ Ｒ．Ｃｉｐｏｌｌａ、"Ｒｅａｌ−ｔｉｍｅ ｖｉｓｕａｌ ｔｒａｃｋｉｎｇ ｏｆ ｃｏｍｐｌｅｘ ｓｔｒｕｃｔｕｒｅｓ、"ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ、ｖｏｌ．２４、ｎｏ．７、ｐｐ．９３２−９４６、２００２． Ｈ．Ｗｕｅｓｔ、Ｆ．Ｖｉａｌ、ａｎｄ Ｄ．Ｓｔｒｉｃｋｅｒ、"Ａｄａｐｔｉｖｅ ｌｉｎｅ ｔｒａｃｋｉｎｇ ｗｉｔｈ ｍｕｌｔｉｐｌｅ ｈｙｐｏｔｈｅｓｅｓ ｆｏｒ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ、"Ｐｒｏｃ．Ｔｈｅ Ｆｏｕｒｔｈ Ｉｎｔ’ｌ Ｓｙｍｐ． ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ０５）、ｐｐ．６２−６９、２００５． Ｔ．Ｆｕｊｉｔａ、Ｋ．Ｓａｔｏ、Ｓ．Ｉｎｏｋｕｃｈｉ、"Ｒａｎｇｅ ｉｍａｇｅ ｐｒｏｃｅｓｓｉｎｇ ｆｏｒ ｂｉｎ−ｐｉｃｋｉｎｇ ｏｆ ｃｕｒｖｅｄ ｏｂｊｅｃｔ、"ＩＡＰＲ ｗｏｒｋｓｈｏｐ ｏｎ ＣＶ、１９８８． Ｒ．Ｙ．Ｔｓａｉ、"Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ"ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ、ｖｏｌ．ＲＡ−３、ｎｏ．４、１９８７． Ｋ．Ｓａｔｏｈ、Ｓ．Ｕｃｈｉｙａｍａ、Ｈ．Ｙａｍａｍｏｔｏ、ａｎｄ Ｈ．Ｔａｍｕｒａ、"Ｒｏｂｕｓｔ ｖｉｓｉｏｎ−ｂａｓｅｄ ｒｅｇｉｓｔｒａｔｉｏｎ ｕｔｉｌｉｚｉｎｇ ｂｉｒｄ’ｓ−ｅｙｅ ｖｉｅｗ ｗｉｔｈ ｕｓｅｒ’ｓ ｖｉｅｗ、" Ｐｒｏｃ． Ｔｈｅ ２ｎｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ０３）、ｐｐ．４６−５５、２００３． Ｉ．Ｓｋｒｙｐｎｙｋ ａｎｄ Ｄ．Ｇ．Ｌｏｗｅ、"Ｓｃｅｎｅ ｍｏｄｅｌｌｉｎｇ、ｒｅｃｏｇｎｉｔｉｏｎ ａｎｄ ｔｒａｃｋｉｎｇ ｗｉｔｈ ｉｎｖａｒｉａｎｔ ｉｍａｇｅ ｆｅａｔｕｒｅｓ、"Ｐｒｏｃ．Ｔｈｅ ３ｒｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ０４）、ｐｐ．１１０−１１９、２００４．

非特許文献１には、３次元モデルの線分が多い場合や、画像上から多くの直線が検出される場合には、対応の組み合わせ数が膨大な数となるため、正しい位置姿勢を算出する対応を探索するために膨大な計算が必要となるという課題がある。

非特許文献２には、３次元線分の投影像の最近傍に検出されたエッジを対応するエッジとみなしている。このため、最近傍に検出されたエッジが本来対応するはずのないエッジであった場合には、位置姿勢の計算が破綻、または推定精度が低下するという課題がある。特に、概略位置姿勢が不正確な場合や、対象の２次元画像が複雑で対応候補となるようなエッジが多量に検出される場合には、３次元形状モデルの線分とエッジとの対応付けに誤対応が発生し、位置姿勢の推定が破綻するという課題がある。

非特許文献３には、繰り返しパターンが多い場合には、エッジを利用する手法と同じように対応の曖昧さが残る。また、濃淡画像上の輝度を利用した対応付けは、対象物体にテクスチャ情報が少ない場合には、画像特徴の識別性が低下し、特徴の対応に誤対応が発生する可能性が高くなるという課題がある。さらに、図２に示すような急激な光源変化が発生した場合でも、輝度に基づく画像特徴の識別が有効に働かず、特徴対応付けの精度が低下する。

これらは、濃淡画像上の輝度に基づいて特徴を識別していることに起因していると考えられる。濃淡画像上の輝度は、物体の表面情報、光源状態、物体を観察する視点位置によって、多様に変化するため、輝度に基づいて対応付けを行う手法では、それら要因の影響を受けることは避けられない。

非特許文献４には、距離画像は、３次元モデルを当てはめる対象として扱われ、濃淡画像から検出される特徴の対応付けに利用されることはなかった。

本発明は、濃淡画像から検出した画像情報の識別のため、距離データに基づく対象物体の形状情報を利用することにより、精度の高い位置姿勢推定を行うことを目的とする。

上記目的は、以下の装置によって達成できる。

物体の形状を有する３次元モデルを保持する保持手段と、
前記物体を撮影した撮影画像を入力する２次元画像入力手段と、
前記物体の３次元座標を含む距離画像を入力する３次元データ入力手段と、
前記撮影画像から画像特徴を抽出する抽出手段と、
前記画像特徴に対応する前記距離画像の位置における前記３次元座標から、該画像特徴が前記物体の形状を表すか否かを判別する判別手段と、
前記物体の形状を表すと判別された画像特徴と、前記３次元モデルの一部とを対応付ける対応付け手段と、
前記対応付けに基づいて、前記物体の位置姿勢を推定する推定手段と、
を備えることを特徴とする位置姿勢推定装置。

本発明によれば、濃淡画像から検出した画像特徴と３次元モデルとの対応付けにおいて、距離画像を参照することによって、精度の良い特徴対応付けを行い、安定的な位置姿勢推定を実現することができる。

第一の実施形態における位置姿勢推定装置１の構成を示す図である。 対象物体と光源環境との相対的な位置姿勢変化に伴うエッジ周辺の輝度分布の変化を示す図である。 第一の実施形態における３次元モデルデータの定義方法を説明する図である。 第一の実施形態における、位置姿勢推定方法の処理手順を示すフローチャートである。 第一の実施形態における、濃淡画像からのエッジ検出の詳細な処理手順を示すフローチャートである。 第一の実施形態における、濃淡画像からのエッジ検出を表す図である。 第一の実施形態における、対応候補エッジの３次元的属性を判別する処理を説明する図である。 本発明の第二の実施形態における位置姿勢推定装置２の構成を示す図である。 第二の実施形態における、概略位置姿勢を利用しない位置姿勢推定方法の処理手順を示すフローチャートである。 第二の実施形態における、直線検出方法について詳細な処理手順を示すフローチャートである。 第二の実施形態における、濃淡画像上の直線の３次元的属性を判別するための処理を説明する図である。 画像上の直線と３次元空間中の直線との関係を説明する図である。 本願の実施形態を実現するためのコンピュータの構成例である。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。

〔第一の実施形態〕
（距離画像を利用したエッジの対応付けに基づく位置姿勢推定）
本実施形態では、物体の概略の位置姿勢は既知であると仮定し、本実施形態の位置姿勢推定装置を、３次元モデルおよび実写画像から検出したエッジの対応に基づいて位置姿勢推定する手法に適用した場合について説明する。

図１は、観察対象物体の形状を表す３次元モデルデータ１０を利用して位置姿勢推定を行う位置姿勢推定装置１の構成を示している。位置姿勢推定装置１は、保持部１１０と、２次元画像入力部１２０と、３次元データ入力部１３０と、概略位置姿勢入力部１４０と、抽出部１５０と、判別部１６０と、対応付け部１７０と、位置姿勢推定部１８０からなる。また、保持部１１０は、３次元モデルデータ１０を保持し、特徴抽出部１７０に接続されている。２次元画像撮像装置２０は、２次元画像入力部１２０に接続されている。３次元座標計測装置３０は、３次元データ入力部１３０に接続されている。位置姿勢推定装置１は、保持部１１０に保持されている観察対象物体の形状を表す３次元モデルデータ１０をもとに、２次元画像中に撮像される観察対象物体の位置および姿勢を計測する。なお、本実施形態において、位置姿勢推定装置１が適用できる条件としては、保持部に保持された３次元モデルデータ１０が、現実に撮像される観察対象物体の形状に即していることを前提としている。

位置姿勢推定装置１を構成する各部について説明する。

２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラである。撮影される２次元画像は濃淡画像であってもよいしカラー画像であってもよい。本実施形態では２次元画像撮像装置２０は濃淡画像を出力する。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えば非特許文献５に示す方法によって事前にキャリブレーションしておく。

２次元画像入力部１２０は、２次元画像撮像装置２０が撮影した画像を位置姿勢推定装置１に入力する。

３次元座標計測装置３０は、計測対象である物体表面上の点の３次元情報を計測する。３次元座標計測装置３０として距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。本実施形態では、距離センサとしては、対象に照射したレーザ光の反射光をカメラで撮影し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。これらのアクティブ式の距離センサは、表面のテクスチャが少ない物体が対象であるとき好適である。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。パッシブ式の距離センサは、表面のテクスチャが十分ある物体を対象とする場合に好適である。その他、距離画像を計測するものであればいかなるものであっても本実施形態の本質を損なうものではない。３次元座標計測装置３０が計測した３次元座標は、３次元データ入力部１３０を介して位置姿勢推定装置１に入力される。また、３次元座標計測装置３０と２次元画像撮像装置２０の光軸は一致しており、２次元画像撮像装置２０が出力する２次元画像の各画素と、３次元座標計測装置が出力する距離画像の各画素の対応は既知であるとする。

３次元データ入力部１３０は、３次元座標計測装置３０が計測する距離画像を位置姿勢推定装置１に入力する。なお、カメラによる画像の撮影と、距離センサによる距離の計測は同時に行われると仮定する。ただし、対象物体が静止している場合など、位置姿勢推定装置１と対象物体との位置及び姿勢が変化しない場合には、必ずしも同時に行う必要はない。

保持部１１０は、位置及び姿勢を計測する対象である物体の３次元形状モデル１０を保持する。３次元形状モデルは、位置姿勢推定部１８０において物体の位置及び姿勢を算出する際に用いられる。本実施形態では、物体を線分と面から構成される３次元形状モデルとして記述する。３次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。さらに、３次元形状モデルは、線分の３次元的属性の情報を保持する。ここで、線分の３次元的属性とは、線分周辺の形状によって判別されるエッジの３次元的属性である。線分周辺の形状によって、凸型の形状（凸ルーフエッジ）、凹型の形状（凹ルーフエッジ）、崖のように不連続に変化する形状（ジャンプエッジ）、形状変化のない平坦な形状（テクスチャエッジ）の４つに分類する。凸型のルーフエッジであるかジャンプエッジであるかの３次元的属性の情報は、物体を観察する方向によって変化するため、物体を観察する姿勢依存の情報となる。本実施形態では、観察方向依存の情報は除き、エッジの３次元的属性としては、形状変化部分のエッジ（ルーフエッジまたはジャンプエッジ）か、平坦部分のエッジ（テクスチャエッジ）であるかの２パターンの情報のみを保持する。

図３は本実施形態における３次元モデルの定義方法を説明する図である。３次元モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。図３（ａ）に示すように、３次元モデルは点Ｐ１〜点Ｐ１４の１４点から構成される。３次元モデルに規定される基準座標系の原点をＰ１２に、ｘ軸を点Ｐ１２から点Ｐ１３に向かう方向に、ｙ軸を点Ｐ１２から点Ｐ８に向かう方向に、ｚ軸を点Ｐ１２から点Ｐ１１に向かう方向に取る。ｙ軸は鉛直上方向（重力軸の反対方向）であるとする。また図３（ｂ）に示すように、３次元モデルは線分Ｌ１〜Ｌ１６により構成されている。図３（ｃ）に示すように、点Ｐ１〜点Ｐ１４は３次元座標値によって表される。また図３（ｄ）に示すように、線分Ｌ１〜Ｌ１６は、線分を構成する点のＩＤによって表される。さらに、図３（ｅ）に示すように、線分Ｌ１〜Ｌ１６は、線分の３次元的属性の情報を保持する。

概略位置姿勢入力部１４０は、位置姿勢推定装置１に対する物体の位置及び姿勢の概略値を入力する。ここで位置姿勢推定装置１に対する物体の位置及び姿勢とは、カメラ座標系における物体の位置及び姿勢を表す。しかしながら、カメラ座標系に対する相対的な位置及び姿勢が既知でかつ変化しなければ、位置姿勢推定装置１のいずれの部分を基準としてもよい。本実施形態では、位置姿勢推定装置１は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。例えば、過去の位置及び姿勢の計測をもとに物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢と推定された速度・加速度から現在の位置及び姿勢を予測したものでもよい。また、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、該センサによる出力値を位置及び姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめわかっている場合にはその値を概略値として用いる。

画像特徴検出部１５０は、２次元画像入力部１２０から入力された２次元画像から画像特徴を検出する。本実施形態では画像特徴としてエッジを検出する。

判別部１６０は、距離画像から検出された画像特徴が、物体の形状を表すものか否かを判別する。例えば、光のあたる部位と影による境界線である画像特徴は、物体の形状を表すものではない。距離画像を利用することで、物体のエッジの画像特徴か、影による画像特徴かを判別することができ、形状を表す画像特徴を絞り込むことができる。

対応付け部１７０は、画像特徴検出部１５０で検出されたエッジと保持部１１０に保持される３次元形状モデルの一部となる線分とを、３次元データ入力部１３０によって入力された３次元点群情報に基づいて、対応付ける。特徴の対応付け方法については後述する。

位置姿勢推定部１８０は、対応付け部１７０で対応付けられた情報に基づいて物体の位置及び姿勢を計測する。処理の詳細については、後述する。

本実施形態における位置姿勢推定方法の処理手順について説明する。

図４は、本実施形態における位置姿勢推定方法の処理手順を示すフローチャートである。

ステップＳ１０１０では、初期化を行う。位置姿勢推定装置１（カメラ）に対する物体の位置及び姿勢の概略値を概略位置姿勢入力部１２０により三次元計測装置１に入力する。本実施形態における位置姿勢推定方法は、概略の撮像装置の位置姿勢を、撮影画像上に撮像される観察対象物体のエッジ情報を利用して逐次更新していく方法である。そのため、位置姿勢推定を開始する前に予め撮像装置の概略の位置及び姿勢を初期位置及び初期姿勢として与える必要がある。前述したように、本実施形態では、前の時刻において計測された位置及び姿勢を用いる。

ステップＳ１０２０では、モデルフィッティングにより物体の位置及び姿勢を算出するための計測データを取得する。具体的には、対象物体の２次元画像と３次元座標を取得する。本実施形態では、２次元画像撮像装置２０は２次元画像として濃淡画像を出力するものとする。また、３次元座標計測装置３０は３次元座標として距離画像を出力するものとする。距離画像は、各画素に濃淡値やカラー値が記録されている２次元画像と異なり、各画素に視点位置からの奥行き値が記録されている。前述のように、２次元画像撮像装置２０と３次元座標計測装置３０の光軸は一致しているため、濃淡画像の各画素と距離画像の各画素の対応は既知である。

ステップＳ１０３０では、ステップＳ１０２０において入力された２次元画像上において、画像特徴の検出を行う。本実施形態では、画像特徴としてエッジを検出する。エッジは濃度勾配の極値となる点である。本実施形態では、非特許文献３で開示される方法によりエッジ検出を行う。

ステップＳ１０３０について詳細に説明する。

図５は、本実施形態における濃淡画像のエッジ特徴の検出方法について詳細な処理手順を示すフローチャートである。

ステップＳ１１１０では、ステップＳ１０１０で入力された計測対象物体の概略位置及び姿勢と校正済みの２次元画像撮像装置２０の内部パラメータを用いて、３次元形状モデルを構成する各線分の画像上への投影像を算出する。線分の投影像は画像上でも線分となる。

ステップＳ１１２０では、ステップＳ１１１０で算出した投影線分上に制御点を設定する。ここで、制御点とは、投影線分を等間隔に分割するような、投影線分上の点である。制御点は、投影結果として得られる制御点の２次元座標と線分の２次元方向、および、３次元モデル上の制御点の３次元座標と線分の３次元方向の情報を保持する。また、制御点の分割元となる３次元モデルの線分が保持する３次元的属性の情報も同様に保持する。投影線分の制御点は、総数をＮとして、各制御点をＤＦｉ（ｉ＝１、２、…、Ｎ）で表す。制御点の数Ｎが多いほど、処理時間が長くなるため、制御点の総数が一定になるように、制御点の間隔を逐次変更しても良い。

ステップＳ１１３０では、ステップＳ１１２０で求めた投影線分の制御点ＤＦｉ（ｉ＝１、２、…、Ｎ）に対応する、２次元画像中のエッジを検出する。

図６は、本実施形態におけるエッジ検出を説明する図である。エッジの検出は、制御点ＤＦｉの探索ライン（制御点の２次元方向の法線方向）上において、撮影画像上の濃度勾配から極値を算出することにより行う（図６（ａ））。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。探索ライン上で検出されたエッジが一つのみである場合は、そのエッジを対応点とし、その２次元座標を保持する。また、図６（ｂ）に示すように、探索ライン上で検出されたエッジが複数存在する場合には、非特許文献３で開示される方法と同様に、対応候補エッジとして複数の２次元座標を保持する。以上の処理を全ての制御点ＤＦｉに対して繰り返し、処理が終了すればステップＳ１０３０の処理を終了し、ステップＳ１０４０に進む。

ステップＳ１０４０では、ステップＳ１０３０で求めた投影線分の制御点ＤＦｉ（ｉ＝１、２、…、Ｎ）に対する対応候補エッジの３次元的属性を判別し、対応候補エッジの絞込みを行う。

図７は、対応候補エッジの３次元的属性を判別する処理を説明する図である。図７（ａ）（ｂ）に示すように、制御点の対応候補エッジ周辺領域の距離値を取得する。本実施形態では、対応候補エッジ周辺領域として、対応候補エッジを中心として制御点の法線方向１０ｐｉｘｅｌの距離値を取得する。次に、図７（ｃ）に示すように、対応候補エッジ周辺領域の距離値に対して２次微分値を算出する。計算した距離値の２次微分値のうち、絶対値が一定以上高い値が含まれている場合は、その対応候補エッジは、距離値が不連続に変化する部分、すなわち形状変化部分のエッジであると判別することが出来る。一方、計算した２次微分値の絶対値が全て一定以下である場合は、平坦形状部分のエッジであると判別することが出来る。また、対応候補エッジ周辺領域に、距離値が取得できない領域（未計測領域）を含む場合は、該対応候補エッジは形状変化部分のエッジであると判別する。以上の処理を制御点が保持する全ての対応候補エッジに対して行い、対応候補エッジの３次元的属性を判別する。

次に、制御点Ｄｆｉの対応候補エッジの絞込みを行う。上述の処理により判別した対応候補エッジの３次元的属性と、制御点が保持する制御点の３次元的属性とを比較し、両種が別種であれば、該対応候補エッジを対応候補から除外する。この処理により、本来対応しないはずのエッジが対応候補エッジとして保持されることを防ぐことが可能となる。結果、制御点と同種の対応候補エッジのみが、対応候補として保持される。また、この段階で複数の対応候補エッジが残っている場合は、制御点から最も近傍に検出された対応候補エッジを対応エッジとして選択する。以上の処理を全ての制御点ＤＦｉに対して繰り返し、全ての制御点ＤＦｉについて対応候補エッジの絞込み処理が終了したら、ステップＳ１０４０の処理を終了し、ステップＳ１０５０に進む。

ステップＳ１０５０では、非線形最適化計算を用いて、対象物体の概略の位置姿勢を反復演算により補正することにより対象物体の位置姿勢を算出する。ここで、３次元線分の制御点ＤＦｉのうち、ステップＳ１０４０において対応候補エッジが求まった制御点の総数をＬｃとする。また、画像の水平方向、垂直方向をそれぞれｘ軸、ｙ軸とする。また、ある制御点の投影された画像座標を（ｕ_０，ｖ_０）、制御点の方向に相当する画像上での傾きをｘ軸に対する傾きθと表す。傾きθは、投影された３次元線分の端点（始点と終点）の撮影画像上での２次元座標を結んだ直線の傾きとして算出する。制御点の直線の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、該制御点の対応点の画像座標を（ｕ’，ｖ’）とする。

ここで、点（ｕ，ｖ）を通り、傾きがθである直線の方程式は、

と表せる。制御点の撮影画像上での画像座標は撮像装置の位置及び姿勢により変化する。また、撮像装置の位置及び姿勢の自由度は６自由度である。ここで撮像装置の位置及び姿勢を表すパラメータをｓで表す。ｓは６次元ベクトルであり、撮像装置の位置を表す３つの要素と、姿勢を表す３つの要素からなる。姿勢を表す３つの要素は、例えばオイラー角による表現や、方向が回転軸を表して大きさが回転角を表す３次元ベクトルなどによって表現される。制御点の画像座標（ｕ，ｖ）は（ｕ_０，ｖ_０）の近傍で１次のテイラー展開によって（３）のように近似できる。

ｕ、ｖの偏微分δｕ／δｓ_ｉ、δｖ／δｓ_ｉの導出方法は例えば非特許文献６に開示されるように広く知られているのでここではその詳細は述べない。（２）を（１）に代入することにより、（３）が得られる。

ここで、（３）に示す直線が該制御点の対応点の画像座標（ｕ’，ｖ’）を通過するように、撮像装置の位置及び姿勢ｓの補正値Δｓを算出する。

（定数）とすると、

が得られる。（４）はＬｃ個の制御点について成り立つため、（５）のようなΔｓに対する線形連立方程式が成り立つ。

ここで（５）を（６）のように簡潔に表す。

（６）をもとにＧａｕｓｓ−Ｎｅｗｔｏｎ法などによって、行列Ｊの一般化逆行列（Ｊ^Ｔ・Ｊ）^−１Ｊ^Ｔを用いてΔｓが求められる。これにより得られたΔｓを用いて、物体の位置及び姿勢を更新する。次に、物体の位置及び姿勢の反復演算が収束しているかどうかを判定する。補正値Δｓが十分に小さい、誤差ｒ−ｄの総和が十分小さい、誤差ｒ−ｄの総和が変化しないといった場合には、物体の位置及び姿勢の計算が収束したと判定する。収束していないと判定された場合には、更新された物体の位置及び姿勢を用いて再度線分の傾きθ、ｒ_０、ｄ及びｕ、ｖの偏微分を計算し直し、（６）より再度補正値Δｓを求め直す。なお、ここでは非線形最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いた。しかしながら、非線形最適化手法はこれに限るものではなく、Ｎｅｗｔｏｎ−Ｒａｐｈｓｏｎ法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。以上、ステップＳ１０５０における撮像装置の位置姿勢算出方法について説明した。

ステップＳ１０６０では、位置姿勢算出を終了する指示が入力されたかどうかを判定し、入力された場合には終了し、入力されなかった場合にはステップＳ１０１０に戻り、新たな画像を取得して再度位置姿勢算出を行う。

以上述べたように、本実施形態では、距離画像を利用して画像上から検出されたエッジの３次元的属性を識別し、対応候補となるエッジの絞込みを行うことで、画像上のエッジと３次元モデルとの対応に誤対応が発生することを低減することが可能となる。これにより、光源変化が発生する場合や、濃淡画像から多量の誤対応候補のエッジが検出される場合でも、精度の高い位置姿勢の推定が可能となる。

〔変形例１−１〕
第一の実施形態では、２次元画像撮像装置２０が撮像する２次元画像の各画素と３次元座標計測装置が計測する距離画像の各画素の対応をとるために、光軸を一致させた２次元画像撮像装置２０と３次元座標計測装置３０を用いていた。しかし、３次元座標計測装置３０と２次元画像撮像装置２０の相対関係はこれに限るものでなく、例えば、光軸が一致していない３次元座標計測装置３０と２次元画像撮像装置２０を用いても良い。この場合、ステップＳ１０２０において２次元画像と距離画像を計測した後、２次元画像の各画素に対応した距離値を算出する処理を行う。具体的には、まず３次元座標計測装置３０と２次元画像撮像装置２０の相対位置姿勢を利用し、３次元座標計測装置３０が計測する点群のカメラ座標系における３次元座標を、２次元画像撮像装置のカメラ座標系に変換する。そして、３次元座標を２次元画像上に投影して２次元画像の各画素に対応付けることで、２次元画像の各画素に対応した距離値を求める。このとき、２次元画像のある画素に対して複数の３次元点が写像された場合は、最も視点位置に近い３次元点のデータを対応付ける。また、２次元画像のある画素に対して、３次元座標が投影されずに対応が求まらない場合は、無効な値を距離値に設定し、未計測画素として扱う。なお、この処理は、２次元画像撮像装置２０と３次元座標計測装置３０が相互に固定されており、両者の相対的な位置及び姿勢を事前にキャリブレーションできることを前提としている。以上の処理を行うことで、光軸が一致していない２次元画像撮像装置２０と３次元座標計測装置３０を用いて、２次元画像の各画素に対応した距離値を算出することができる。

〔変形例１−２〕
第一の実施形態では、対応候補エッジの３次元的属性を判別するために、対応候補エッジ周辺領域の距離値を参照し、距離値の２次微分値を算出することで不連続領域を判定、対応候補エッジの３次元的属性を判別していた。しかし、３次元的属性を判別する方法は、これに限るものではない。例えば、距離画像からエッジ検出を行い、その結果に基づいて、３次元的属性を判別してもよい。具体的には、対応候補エッジの近傍に、距離画像中からエッジが検出される場合は、形状変化部分のエッジであると判別し、距離画像中からエッジが検出されない場合は、平坦部分のエッジであると判別する。なお、対応候補エッジの３次元的属性の判別方法は、以上に挙げた方法に限るのものでなく、対応候補エッジの３次元形状に基づいて３次元的属性を判別することが出来る限り、いずれの手法を用いても良い。

〔変形例１−３〕
第一の実施形態では、３次元モデルとして、３次元線分モデルを利用していた。しかし、３次元モデルは、３次元線分モデルに限るものではない。３次元モデルから、３次元線分と線分の３次元的属性を算出することができる限り、３次元モデルの方式に制限はない。例えば、頂点情報および各頂点を結んだ面の情報で構成されたメッシュモデルを利用してもよいし、ＮＵＲＢＳ曲面などパラメトリックな曲面表現を利用してもよい。これらの場合、形状情報の中から直接的に３次元線分情報を参照することが出来ないため、３次元線分情報をランタイムに算出する必要がある。また、３次元線分投影処理の代わりに、３次元線分の３次元的属性の計算を行う必要がある。具体的には、計測対象物体の概略位置及び姿勢に基づいて、３次元モデルをコンピュータグラフィックス（ＣＧ）により描画し、描画結果からエッジ検出を行う。検出されたエッジが等間隔になるように、制御点を求める。そして、制御点の２次元位置を３次元メッシュモデルに逆投影することで、３次元座標を求める。ただし、距離画像の代わりに、描画結果として副次的に得られるデプス画像（３次元モデルと視点間の距離が格納される）を利用して、エッジの３次元的属性を算出する。以上の処理により、エッジの３次元的属性とともに制御点が算出される。求めた制御点を利用して、位置姿勢を推定する。この方法は、３次元モデルとしてあらかじめ、線分の種類を保持する必要がないという点で、準備が簡便であるという利点がある。

〔変形例１−４〕
第一の実施形態では、濃淡画像から検出したエッジの幾何的な種類として、形状変化部分のエッジか、平坦部分のエッジかの２パターンのみを判別していた。しかし、エッジの３次元的属性はこれに限るものではない。例えば、凸型形状部分に検出される凸ルーフエッジ、凹形状部分に検出される凹ルーフエッジ、不連続な形状変化部分に検出されるジャンプエッジのように、形状変化部分のエッジをさらに細かい種類に分けて判別してもよい。判別する３次元的属性を増やすことで、より特徴の絞込みを厳密に行うことが可能となる。凸ルーフエッジとジャンプエッジは、物体を観察する方向によって、凸ルーフエッジとして検出されるか、ジャンプエッジとして検出されるかが変化する。そのため、凸ルーフエッジとジャンプエッジを区別してあつかう場合、物体を観察する姿勢に応じて複数の３次元的属性の情報を保持する必要がある。また、凸ルーフエッジとジャンプエッジは、物体を観察する距離によっても変化する。しかし、姿勢による変化ほどには大幅に変化しないため、ある程度観察距離を限定しておけば距離ごとに３次元的属性を保持する必要はない。

また、エッジの３次元的属性の情報は以上に限るものでなく、さらに細分化してもよい。例えば、なだらかなルーフエッジと急峻なルーフエッジを区別する、形状変化量自体を特徴量としてしまってもよい。エッジを識別する情報として、距離情報を利用する限り、いかなる形式でもよく、エッジの３次元的属性の情報に特に制限はない。

〔変形例１−５〕
第一の実施形態では、エッジの対応付けに距離画像から検出される形状情報を利用したが、これに限るものではない。例えば、エッジの３次元的属性の情報に加えて、非特許文献３のように濃淡画像中の輝度分布を併用してもよい。輝度の分布を利用することで、テクスチャエッジなどの対象物体の輝度変化に基づくエッジの識別を行うことが可能となる。

〔第二の実施形態〕
（概略位置姿勢を必要としない位置姿勢推定）
第一の実施形態では、物体の概略の位置姿勢は既知である場合に、距離画像を用いて複数の対応候補を絞る方法について説明した。第二の実施形態では、物体の概略位置姿勢と千分の対応が未知の場合に、距離画像を利用して、濃淡画像から検出したエッジと３次元モデルの線分を対応付けて、位置姿勢を算出する方法について説明する。第一の実施形態では、物体の概略の位置姿勢は既知であったため、３次元モデルの線分の近傍に存在するエッジを探索することで、あらかじめ、対応候補となるエッジの数が限定されていた。しかし、第二の実施形態では、物体の概略位置姿勢が未知であるため、３次元モデルの線分と濃淡画像上のエッジとの対応は完全に未知の段階から、対応付けを行う必要がある。そこで、第二の実施形態では、距離画像を利用して濃淡画像上のエッジの３次元的属性を算出することで、濃淡画像上のエッジと３次元モデルの線分の組み合わせ数を削減する。削減された組み合わせの中からランダムに選択し、位置及び姿勢を複数算出して、その中から最も整合性の高いものを選択することにより３次元位置姿勢を求める。

図８は、本実施形態における位置姿勢推定装置２の構成を示している。位置姿勢推定装置２は、保持部２１０と、２次元画像入力部２２０と、３次元データ入力部２３０と、抽出部２４０と、判別部２５０と、対応付け部２６０と、位置姿勢推定部２７０からなる。２次元画像撮像装置２０は、２次元画像入力部２２０に接続されている。３次元座標計測装置３０は、３次元データ入力部２３０に接続されている。位置姿勢推定装置２は、保持部２１０に保持されている観察対象物体の形状を表す３次元モデルデータ１０をもとに、２次元画像中に撮像される観察対象物体の位置および姿勢を計測する。

次に、位置姿勢推定装置２を構成する各部について説明する。

保持部２１０では、位置及び姿勢を計測する対象である物体の３次元形状モデル１０を保持する。本実施形態における、３次元形状モデルの表現方法は、第一の実施形態とほぼ同様である。異なる点としては、エッジを識別するための３次元的属性として、凸型に形状変化するエッジ（凸ルーフエッジ）と、凹型に形状変化するエッジ（凹ルーフエッジ）と、平坦部分のエッジ（テクスチャエッジ）の、３パターンの情報を保持する。
抽出部２４０では、２次元画像入力部２２０が取得した２次元画像から、画像特徴を抽出する。
判別部２５０では、距離画像から検出された画像特徴が、物体の形状を表すものか否かを判別する。
対応付け部２６０では、抽出部２４０が抽出した画像特徴の幾何的情報を、３次元データ入力部が入力する３次元距離データを利用して算出し、３次元モデルデータ１０中の線分と対応付ける。
位置姿勢推定部２７０では、対応付け部２６０において対応付けた情報に基づいて、直接解法により物体の位置及び姿勢を算出する。
２次元画像入力部２２０、３次元データ入力部２３０は、第一の実施形態における２次元画像入力部１２０、３次元データ入力部１３０と同様である。
本実施形態における位置姿勢推定方法の処理手順について説明する。

図９は、本実施形態における位置姿勢推定方法の処理手順を示すフローチャートである。
ステップＳ２０１０では、濃淡画像および距離画像を取得する。第一の実施形態におけるステップＳ１０２０の処理と同様である。
ステップＳ２０２０では、ステップＳ２０１０において取得した濃淡画像から、エッジ検出を行い、折れ線近似することで、直線の検出を行う。
ステップＳ２０２０について詳細に説明する。

図１０は、本実施形態における直線検出方法について詳細な処理手順を示すフローチャートである。
ステップＳ２１１０では、濃淡画像からエッジ検出を行う。エッジを検出する手法としては、例えば、ｓｏｂｅｌフィルタなどのエッジ検出フィルタを利用しても良いし、Ｃａｎｎｙアルゴリズム等を利用しても良い。画像の画素値が不連続に変化する領域を検出することができれば、いずれの手法でも良く、手法の選択に特に制限は無い。本実施形態では、Ｃａｎｎｙアルゴリズムを利用してエッジ検出を行う。濃淡画像に対してＣａｎｎｙアルゴリズムによるエッジ検出を行うことで、エッジ領域と非エッジ領域に分けられた２値画像が得られる。
ステップＳ２１２０では、ステップＳ２１１０で生成された２値化画像について、隣接エッジのラベリングを行う。ラベリングは、例えばある画素の周囲８画素の中にエッジが存在すれば、同一ラベルを割り当てることにより行う。
ステップＳ２１３０では、ステップＳ２１２０において同一ラベルに分類された隣接エッジの中から複数の枝が分岐する点を探索し、該分岐点において枝を切断し、各枝に異なるラベルを割り当てる。
ステップＳ２１４０では、ステップＳ２１３０においてラベルを割り当てられた各枝について、折れ線近似を行う。折れ線近似は例えば以下の方法により行う。まず、枝の両端を線分で結び、枝上の点のうち該線分との距離が最大かつ閾値以上の点に新たに分割点を設ける。次に、この分割点と枝の両端を線分で結び、該線分との距離が最大の点に分割点を設ける。この処理を、枝が折れ線によって十分近似されるまで再帰的に繰り返す。その後、折れ線を構成する各線分について、両端の座標を画像上の直線の通過点として出力する。

なお、ここではラベリング処理と折れ線近似により直線検出を行ったが、直線検出方法はこれに限るものではなく、画像上から直線を検出する方法であればいかなる方法であってもよい。例えば、ハフ変換によって直線を検出してもよい。

ステップＳ２１５０では、ステップＳ２１４０で、算出した直線に対して、３次元的属性を判別する。

図１１は、濃淡画像上の直線の３次元的属性を判別するための処理を説明する図である。図１１（ａ）に示すように、注目する直線の周辺領域の距離値を取得する。本実施形態では、直線の法線方向１０ｐｉｘｅｌ、直線並行方向ｎ／２ｐｉｘｅｌの領域を直線の周辺領域として取得する。ｎは注目線分の長さを表す。次に、図１１（ｂ）に示すように、直線平行方向に関して、距離値の平均値を算出する。この処理により、直線法線方向１０ｐｉｘｅｌの距離値の平均値ベクトルが算出される。この距離値のベクトルから、直線の３次元的属性を求める。距離値のベクトルが、凸型形状あるいは崖形状（ジャンプエッジ）である場合は凸ルーフエッジ、凹型形状である場合は、凹ルーフエッジ、平坦形状である場合は、テクスチャエッジとして判別する。上述のように、本実施形態では、凸ルーフエッジとジャンプエッジは区別せずにどちらも凸ルーフエッジとして扱う。すべての直線に対して、３次元的属性の判別処理が終了したら、ステップＳ２０３０の処理に進む。

ステップＳ２０３０では、ステップＳ２０２０における直線の検出結果と、保持部２１０に保持されている３次元モデルの線分との対応付けを行う。まず、３次元モデルの線分の３次元的属性と、ステップＳ２０２０において検出した直線の３次元的属性を比較し、両方の属性が同種となる組み合わせを算出する。３次元モデルの線分と画像上の直線全ての組み合わせに対して、３次元的属性の比較を行い、３次元的属性が同種の組み合わせの算出が全て終了したら、全組み合わせを保持し、ステップＳ２０４０の処理に進む。

ステップＳ２０４０では、ステップＳ２０３０において算出した３次元モデルの線分と画像上の直線の組み合わせから、ランダムに選択した８対の対応情報に基づいて、物体の位置及び姿勢を算出する。まず、ステップＳ２０３０において算出した３次元モデルの線分と画像上の直線との全組み合わせからランダムに８対の組み合わせを選択し、３次元モデルの線分と画像上の直線との対応として保持する。この対応に基づいて、物体の位置及び姿勢を算出する。

図１２は、画像上の直線と３次元空間中の直線との関係を説明する図である。一般に、３次元空間中の直線を撮像装置により撮像すると、その画像平面上における投影像は直線となる。図１２に示すように、３次元空間中の点Ｐ及びＱを通過する直線Ｌの画像平面上における投影像である直線ｌは、直線Ｌ及び視点Ｃを通る平面πと画像平面との交線である。また平面πの法線ベクトルｎは、ベクトルＣＰ、ＣＱ、ＰＱと直交する。点Ｐ、点Ｑの基準座標系における位置を表す３次元ベクトルをそれぞれｐ、ｑ、直線Ｌの基準座標系における方向ベクトルをｄ（＝ｑ−ｐ）とすると、この３つの直交条件は（７）〜（９）のように表される。

また、Ｒ_ｃｗはカメラ座標系に対する基準座標系の姿勢を表す３×３回転行列、ｔ_ｃｗはカメラ座標系に対する基準座標系の位置を表す３次元ベクトルである。ここで、Ｒ_ｃｗは（１０）のように表される

ｎ＝［ｎ_ｘ ｎ_ｙ ｎ_ｚ］^ｔ、ｐ＝［ｐ_ｘ ｐ_ｙ ｐ_ｚ］^ｔ、ｑ＝［ｑ_ｘ ｑ_ｙ ｑ_ｚ］^ｔ、ｔ_ｃｗ＝［ｔ_ｘ ｔ_ｙ ｔ_ｚ］^ｔとして、（１０）を（７）、（８）に代入すると、（１１）、（１２）が得られる。

（１１）、（１２）は、未知変数ｒ１１、ｒ１２、ｒ１３、ｒ２１、ｒ２２、ｒ２３、ｒ３１、ｒ３２、ｒ３３、ｔ_ｘ、ｔ_ｙ、ｔ_ｚについての線形方程式になっている。また、画像上で検出された直線の２つの通過点の、前述の焦点距離を１とした画像平面の座標系における座標を（ｘ_１、ｙ_１）、（ｘ_２、ｙ_２）とすると、そのカメラ座標は

となる。法線ベクトルｎは、ｘ_ｃ１、ｘ_ｃ２の双方に直交するベクトルであるので、ｎ＝ｘ_ｃ１×ｘ_ｃ２により求められる。これによって、画像上で検出される直線と、３次元空間中の直線とが法線ベクトルｎを介して方程式として対応付けられる。複数の画像上の直線と３次元空間中の直線との対応について成り立つ（１１）、（１２）をｒ１１、ｒ１２、ｒ１３、ｒ２１、ｒ２２、ｒ２３、ｒ３１、ｒ３２、ｒ３３、ｔ_ｘ、ｔ_ｙ、ｔ_ｚについて、連立方程式として解くことで、位置姿勢を算出する。以上の処理により算出された回転行列は、本来独立でない回転行列の要素を独立に求めるため、正規直交基底の条件を満たさない。そこで、回転行列を特異値分解し、正規直交化を行うことで、軸の直交性が保証された回転行列を算出する。以上、ステップＳ２０４０における撮像装置の位置姿勢算出方法について説明した。

ステップＳ２０５０では、ステップＳ２０４０において算出した位置姿勢の評価値の算出を行う。ここでは、３次元モデルの線分をステップＳ２０４０にて算出された位置姿勢に基づいて投影し、投影した画素がエッジ領域であるかどうかを判定する。本実施形態では、評価値として、３次元モデルの投影線分に位置する画素がエッジである数を用いる。この評価値は、画像上のエッジと３次元モデルの投影線分が重なっているほど高くなる。また、位置姿勢の評価値は、上述の方式に限るものでなく、物体の位置姿勢の妥当性を測る指標であれば、いずれの方式でもよく、評価値の決定に特に制限はない。

ステップＳ２０６０では、ステップＳ２０５０において算出した評価値から、ステップＳ２０４０において算出した位置姿勢の妥当性を判別する。正しい位置姿勢が算出されたと判別された場合には処理を終了し、正しくないと判別された場合にはステップＳ２０４０に戻り、新たな組み合わせを算出して再度位置姿勢算出を行う。

妥当性の判別は、ステップＳ２０５０で算出した評価値が一定以上に高いか否かを判断することにより行う。評価値の妥当性の判別は、例えば、あらかじめ経験的に与えた閾値を利用しても良い。また、ステップＳ２０４０とステップＳ２０５０を繰り返して、３次元モデルの線分と画像上の直線の、全ての組み合わせの中から最大の評価値をとるものを選出しても良い。また、ステップＳ２０４０において一定数の組み合わせを選定し、その中で最大となる評価値をとるものを選出しても良い。３次元モデルの線分と画像上の直線との組み合わせの中で、正しい位置姿勢を算出する組み合わせを選出できる限り、評価値の判別方法に制限はなく、いずれの手法でも良い。本実施形態では、ステップＳ２０５０において算出した評価値とそのときの位置姿勢を保持しておき、ステップＳ２０４０からステップＳ２０６０を１０００回繰り返した中で、最大の評価値となる位置姿勢を最終的な位置姿勢として選択する。

以上述べたように、本実施形態では、画像上から検出される直線と、３次元モデル中の線分とを、距離画像から抽出した距離分布に基づいて対応付け、対応付けられた直線と３次元モデルの線分とから、直接的に撮像装置の位置及び姿勢を算出する。

〔変形例１〕
以上説明した実施形態・変形例では、２次元画像上の特徴としてエッジ特徴を用いていた。しかしながら、２次元画像上の特徴はエッジ特徴に限るものではなく、その他の特徴であってもよい。例えば、非特許文献７のように、対象物体の３次元形状モデルを点特徴の３次元位置座標の集合によって表し、画像特徴として点特徴を検出して、特徴点の３次元座標と、画像上の２次元座標との対応に基づいて位置及び姿勢を算出してもよい。ＨａｒｒｉｓやＳＩＦＴに代表されるような画像特徴としての点特徴は、大抵、点特徴領域が局所的に平面であることを前提として特徴量が記述されている。距離画像を参照し、点特徴の局所平面性のチェックすることで、局所平面でない点特徴を除くことが可能となる。これにより、非平面物体の位置姿勢推定において、点特徴の誤対応を削減すること可能である。また、上述の点特徴に限らず、他の形式の点特徴を利用して位置姿勢の算出を行っても、複数の特徴（特徴点とエッジ）を組み合わせて位置姿勢の算出を行っても、本実施形態の本質は損なわれない。

〔変形例２〕
以上説明した実施形態・変形例では、３次元座標計測装置として、密（ｄｅｎｓｅ）な距離画像を出力する距離センサの利用を想定していた。しかしながら、３次元座標計測装置はこれに限るものではなく、疎な計測を行うものであってもよい。例えば、スポット光を用いた距離計測装置であっても、画像特徴の３次元的属性を判別することは可能である。しかし、この場合、３次元座標は単なる３次元点群情報として表現され、画像の体をなしていないため、ステップＳ１０４０において、制御点近傍の３次元座標の２次微分値から３次元的属性を判別することが困難となる。これに対しては、例えば、画像特徴周辺に分布する３次元点群を探索し、その３次元点群に対し、線フィッティングあるいは面フィッティングするなどによって形状を判別するなどをすればよい。また、３次元点群を特異値分解し、その結果から３次元点群の平面性を判別してもよいし、３次元点群を主成分分析し、その主軸方向と分散から平面性を判別してもよい。画像特徴周辺の形状の特徴を推定できる限り、形状推定の方法に制限はなく、いずれの手法でもよい。

〔その他の実施形態〕
図１３は、本願の実施形態を実現するためのコンピュータの構成例である。

発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク１３０４、１３０７又は各種記憶媒体１３０２、１３０３を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ１３０１（ＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記憶媒体に記憶して提供してもよい。なお、本装置のコンピュータには、入力部１３０５から処理を実行する指示を入力し、表示部１３０６で指示した処理の結果を表示してもよい。

Claims (6)

1. 物体の形状を有する３次元モデルを保持する保持手段と、
   前記物体を撮影した撮影画像を入力する２次元画像入力手段と、
   前記物体の３次元座標を含む距離画像を入力する３次元データ入力手段と、
   前記撮影画像から画像特徴を抽出する抽出手段と、
   前記画像特徴に対応する前記距離画像の位置における前記３次元座標から、該画像特徴が前記物体の形状を表すか否かを判別する判別手段と、
   前記物体の形状を表すと判別された画像特徴と、前記３次元モデルの一部とを対応付ける対応付け手段と、
   前記対応付けの結果に基づいて、前記物体の位置姿勢を推定する推定手段と、
   を備えることを特徴とする位置姿勢推定装置。
2. 物体の概略の位置及び姿勢を入力する概略位置姿勢入力手段と
   を更に備え、
   前記推定手段が、前記概略の位置及び姿勢を補正することによって前記物体の位置及び姿勢を推定することを特徴とする請求項１に記載の位置姿勢推定装置。
3. 前記判別手段が、前記位置の周辺が平坦でない画像特徴を、形状を表す画像特徴として判別することを特徴とする請求項１又は２に記載の位置姿勢推定装置。
4. 前記画像特徴は、エッジ特徴若しくは点特徴であることを特徴とする請求項１乃至３の何れか１項に記載の位置姿勢推定装置。
5. ２次元画像入力手段が、前記物体を撮影した撮影画像を入力する２次元画像入力工程と、
   ３次元データ入力手段が、前記物体の３次元座標を含む距離画像を入力する３次元データ入力工程と、
   抽出手段が、前記撮影画像から画像特徴を抽出する抽出工程と、
   判別手段が、前記画像特徴に対応する前記距離画像の位置における前記３次元座標から、該画像特徴が前記物体の形状を表すか否かを判別する判別工程と、
   対応付け手段が、前記物体の形状を表すと判別された画像特徴と、保持手段に保持された物体の形状を有する３次元モデルの一部とを対応付ける対応付け工程と、
   推定手段が、前記対応付けの結果に基づいて、前記物体の位置姿勢を推定する推定工程と、
   を備えることを特徴とする位置姿勢推定方法。
6. 請求項５に記載の位置姿勢推定方法をコンピュータに実行させることを特徴とするプログラム。