JP5976089B2

JP5976089B2 - 位置姿勢計測装置、位置姿勢計測方法、およびプログラム

Info

Publication number: JP5976089B2
Application number: JP2014257828A
Authority: JP
Inventors: 藤木　真和; 真和藤木; 祐介中里; 内山　晋二; 晋二内山
Original assignee: Canon Inc
Current assignee: Canon Inc
Priority date: 2014-12-19
Filing date: 2014-12-19
Publication date: 2016-08-23
Anticipated expiration: 2030-08-20
Also published as: JP2015111128A

Description

本発明は、物体モデルの特徴と画像特徴との対応付けを行い、モデルと画像データとのフィッティングやマッチングを行う位置姿勢計測装置、位置姿勢計測方法、およびプログラムに関する。

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

位置及び姿勢を計測する方法として、カメラが撮影する２次元画像を用いて行う方法がある。その中でも、２次元画像上から検出される特徴に対して物体の３次元形状モデルを当て嵌めるモデルフィッティングによる計測が一般的である。例えば、非特許文献１では、２次元画像上から検出される特徴として、エッジを利用した物体の位置及び姿勢の計測を行う方法が開示されている。この方法では、物体の３次元形状モデルを線分の集合（ワイヤフレームモデル）によって表し、物体の概略の位置及び姿勢は既知であるとして、画像上で検出されるエッジに３次元の線分の投影像を当て嵌めることにより物体の位置及び姿勢を計測する。

上述のようなモデルフィッティング法では、計測データとモデルとの対応を探索し、対応対間の差異を最小化する。しかし、計測データのノイズ・初期の位置姿勢推定値の誤差などの要因により、明らかに誤った対応が検出されてしまうことがある。このような明らかな誤り（以下、「外れ値」と称する）に小さな重みを設定して、外れ値が推定結果に悪影響を及ぼすことを防ぐロバスト推定と呼ばれる手法群が利用されている。ロバスト推定の代表的な手法としては、Ｍ推定が挙げられる。

また、従来から物体の特徴モデルと画像から検出した特徴とをマッチングすることにより、物体の種別や個体を識別する物体認識技術がある。例えば、非特許文献２では、画像特徴としてＳＩＦＴ（Ｓｃａｌｅ-ＩｎｖａｒｉａｎｔＦｅａｔｕｒｅＴｒａｎｓｆｏｒｍ）と呼ばれる局所的画像特徴点の抽出法を用いる物体認識手法が開示されている。

特開平０１−００９３０７号公報

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. Lowe, D.G., "Object Recognition from Local Scale-Invariant Features", Proc. of IEEE International Conference on Computer Vision, pp.1150-1157, 1999. 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. H. Wuest, F. Vial, and D. Stricker, "Adaptive line tracking with multiple hypotheses for augmented reality," Proc. The Fourth Int'l Symp. on Mixed and Augmented Reality (ISMAR05), pp.62-69, 2005. K. Satoh, S. Uchiyama, H. Yamamoto, and H. Tamura, "Robust vision-based registration utilizing bird’s-eye view with user’s view," Proc. The 2nd IEEE/ACM International Symposium on Mixed and Augmented Reality (ISMAR03), pp.46-55, 2003.

位置姿勢推定や物体認識の対象とする物体の画像を撮影するため、対象物体を照明下に配置すると、対象物体の一部や他の物体が照明光を遮り、影が形成されることがある。その情景を撮影した画像からは影に相当する画像領域、特に影の領域と影でない領域の境界部分において、画像特徴が検出される可能性が高い。これは、影の境界に明るさレベルの大きな勾配が発生するためである。

そして、物体のモデルと画像データとをフィッティングあるいはマッチングするために、モデルに記述された特徴と画像特徴との対応を探索すると、モデルの特徴が誤って影に起因する擬似的な画像特徴と対応付けられてしまうことがある。このような誤対応が起こると、位置姿勢推定や物体認識の処理が破綻したり、位置姿勢推定精度や物体認識精度が低下したりするという問題がある。

このような擬似的画像特徴との誤対応に対して前述のロバスト推定を適用しても、必ずしも誤対応の影響を軽減できない。ロバスト推定では対応対間の距離を計量する何らかの評価値（例えば、対応対間の画面上での距離）に基づいて、それぞれの対応が処理結果に与える重み係数を調整する。具体的には、距離の評価値が大きいほど重み係数を高く設定する。ただし、距離の評価値の算出の際には画像特徴が影の領域内にあるか否かは考慮されていないため、影領域内の特徴点に大きな重み係数が設定されることを防止できない。したがって、影領域から誤った対応点が抽出されても、ロバスト推定ではその悪影響を排除することは困難である。ロバスト推定は、あくまで数値計算的な外れ値の除外方法であり、画像特徴が影の影響を受けているか否かという幾何学的・光学的な判断はしていない。そのため、誤対応であっても偶発的に対応間の距離が小さくなる場合には、その悪影響を軽減することは困難である。

上記の課題に鑑み、本発明は、影が落ちる領域に相当する画像領域から抽出される擬似的な画像特徴の影響を軽減し、フィッティング／マッチングの安定性・精度を向上することを目的とする。

上記の目的を達成する本発明に係る位置姿勢計測装置は、
対象物体の３次元形状情報を含む３次元モデルを保持する保持手段と、
前記対象物体の２次元画像を取得する２次元画像取得手段と、
前記２次元画像において前記対象物体に照射される照明が遮蔽される影領域の先端部と、前記影領域ではない非影領域との境界領域である、影先端境界領域を特定する特定手段と、
前記特定手段により特定された影先端境界領域と、前記対象物体の位置および姿勢の概略値とに基づいて、前記３次元モデルから抽出される第１の幾何特徴と、該第１の幾何特徴に対応する、前記２次元画像上の第２の幾何特徴とを対応付ける対応付け手段と、
前記対応付け手段により対応づけられた前記第１の幾何特徴と前記第２の幾何特徴とのずれが小さくなるように前記概略値を補正することにより、前記対象物体の位置および姿勢を計測する位置姿勢計測手段と、
を備えることを特徴とする。

本発明によれば、影が落ちる領域に相当する画像領域から抽出される擬似的な画像特徴の影響を軽減し、フィッティング／マッチングの安定性・精度を向上することができる。

（ａ）計測対象物体の形状を３次元的に表現した模式図、（ｂ）計測対象物体を撮影した濃淡画像の中で計測対象物体部分を取り出した様子を示す図、（ｃ）影線端境界領域の抽出を説明するための図。（ａ）位置姿勢計測装置のハードウェア構成を示す図、（ｂ）第１実施形態に係る位置姿勢計測装置の機能構成を示す図。第１実施形態に係る処理の流れを示すフローチャート。計測対象物体のモデルと画像特徴との対応付け処理の詳細を示すフローチャート。第２実施形態に係る位置姿勢計測装置の機能構成を示す図。第２実施形態に係る処理の全体の流れを示すフローチャート。影先端境界の抽出処理の手順を示すフローチャート。２次元画像中のエッジ検出処理を説明する図。

（第１実施形態）
本実施形態では、物体を撮影した画像に既知である物体の３次元形状モデルをフィッティングすることによって物体の位置姿勢を推定する位置姿勢計測装置について説明する。３次元モデルの３次元形状情報が既知である対象物体の位置および姿勢を計測する。以下、位置姿勢を推定する対象となる物体を「計測対象物体」と称する。

計測対象物体の位置姿勢の推定は、まず形状モデル上の複数の特徴点に対応する画像上の特徴点を探索し、これらの対応点対間の画像上における距離の和が最小となるように計測対象物体の位置および姿勢を最適化する。その際に、対応点対の位置姿勢計測における影響にばらつきを持たせることで、誤った位置姿勢計測につながってしまうような対応点対の位置姿勢計測への寄与を低減することができる。例えば、各３次元モデルを構成する線分を投影した投影線分を構成する点と、２次元画像上のエッジを構成する点との、２次元画像上における距離に重み係数を乗じた値の総和が最小となる位置および姿勢を対象物体の位置および姿勢として計測する。その際、悪影響を及ぼす可能性のある対応点対の重み係数を小さく設定すればよい。

本実施形態では、計測対象物体の画像を２種類撮影する。ここでいう２種類とは、距離画像と濃淡画像である。なお、濃淡画像に替えて、カラー画像を撮影してもよい。その場合は距離画像とカラー画像との２種類である。

距離画像は通常の２次元画像とは異なり、各画素に撮影装置から被写体までの距離が格納される。本実施形態では距離画像はパターン投影法により撮影する。すなわち、計測対象物体に明暗の縞模様などの照明パターンを投射し、その情景を照明とは異なる方向から撮影して、三角測量の原理で距離を算出する。ただし、距離計測方法はパターン投影法に限るものではなく、三角測量を原理とする任意のアクティブ照明方式であれば他の方法であってもよい。このような三角測量ベースの距離計測方法では、カメラと照明との位置・向きが互いに異なるので、撮影した画像には写りこんでいる空間領域であっても、照明からの光が遮られて当たらないことが有り得る。このような領域については投射された照明パターンが観測されないため、距離計測が不可能になる。本実施形態で想定する距離画像計測装置では、計測が不可能な画素については画素値が０になるとする。計測不能を表す方法としては、画素値を通常距離値としては使わない値（例えば、本実施形態のように０など）にする方法や、距離画像とは別に各画素に対して距離計測不能か否かを表すフラグ値を持たせる方法が考えられる。いずれにしろ、画素値やフラグ値を参照することによって、容易に距離計測不能な画素を特定できる。

濃淡画像は距離画像計測装置の撮像部および照明部を用いて撮影する。濃淡画像を撮影するときは、明暗パターンを含まない均一な明るさの照明を対象物体に照射する。距離画像と濃淡画像とは同一のカメラで撮影するので、２次元座標が同じ画素は３次元空間の同じ箇所に対応することになる。したがって、パターン光が遮られて距離計測不能になる距離画像上の領域は、濃淡画像上で影になる領域と一致する。

まず、図１（ａ）および図１（ｂ）を参照して、実施形態における発明の原理を示す模式図について説明する。図１（ａ）は、計測対象物体の形状を３次元的に表現した模式図である。また、図１（ｂ）は、計測対象物体を撮影した濃淡画像の中で計測対象物体部分を取り出した様子を示す図である。ここでは計測対象物体を上部から撮影した様子を示す。

図１（ａ）において、計測対象物体１０１ａは計測対象となる物体である。計測対象物体１０１ａは、凸部１０２ａを有する。この凸部１０２ａにより、距離画像計測装置の照明部からの光が遮られて、領域１０３ａに影を落としている。図１（ｂ）に示す濃淡画像上では、計測対象物体１０１ａが領域１０１ｂ、凸部１０２ａが領域１０２ｂ、影が落ちている領域１０３ａが領域１０３ｂに相当する。そして、線分１０４ａが線分１０４ｂに対応し、線分１０５ａが線分１０５ｂに対応する。線分１０６ａが線分１０６ｂに対応する。なお、以下では領域１０３ｂのように濃淡画像上で影が落ちている領域を「被遮蔽領域」と称する。領域１０３ａは距離画像を撮影する際にパターン光が投影されない部分に当たるので距離計測が不可能である。また、距離画像の画素と濃淡画像の画素とは一対一に対応する。つまり、距離画像において画素値が計測不能を表す０である領域を検出することによって、被遮蔽領域１０３ｂを検出できる。

本実施形態に係る方法では、位置姿勢推定の計算に用いる形状モデル上の点として、形状モデルを構成する線分上の点を用いる。線分１０４ａ上の点に対応する画像上の特徴点を求めるために、計測対象物体の概略位置姿勢に基づいて線分１０４ａを画像上に投影した線分１０７ｂ上に複数の制御点をとり、制御点に最も近い画像エッジを探索する。本来、線分１０４ａに対応するのは線分１０４ｂであるが、図１（ｂ）の場合画像上で線分１０７ｂにより近いのは線分１０５ｂであるので、誤った対応点である線分１０５ｂ上の点が検出されてしまう。

そこで、探索の結果見つかった対応点が被遮蔽領域内にあるか否か判定して、被遮蔽領域内にあれば、その対応点に関する位置姿勢計算への寄与度（重み係数）を比較的低く設定する。一方、モデルの線分１０６ａ上の点に対応する画像上の特徴点を探索する場合、線分１０６ａを画像上に投影した線分１０８ｂ上の点に最も近い画像上のエッジを探索する。図１（ｂ）の場合、探索結果は線分１０６ｂ上の点群となる。これらの点群は被遮蔽領域の外部にあるので、モデル線分１０６ａと画像線分１０６ｂ上の対応点対に関する位置姿勢計算への寄与度（重み係数）は比較的高く設定する。

上記のように画像上で探索した対応点が被遮蔽領域内にあるか否かによって位置姿勢計測への重み係数を制御することにより、擬似的なエッジである影の境界が悪影響を及ぼして位置姿勢計算が破綻したり、精度が低下したりする可能性を軽減できる。なお、図１（ｃ）については後述する。

次に図２（ａ）を参照して、本発明に係る位置姿勢計測装置のハードウェア構成について説明する。位置姿勢計測装置は、ＣＰＵ２０１とメモリ２０２と、バス２０３と、インタフェース２０４と、外部記憶装置２０５と、キーボード２０６と、マウス２０７と、表示部２０８と、データ入出力部２０９とを備える。

ＣＰＵ２０１は、後述の各処理部による動作を制御する。メモリ２０２は、ＣＰＵ２０１の動作に用いるプログラムやデータを格納する。バス２０３は、各構成モジュール間のデータ転送を司る。インタフェース２０４は、バス２０３と各種装置とのインタフェースである。外部記憶装置２０５は、ＣＰＵ２０１に読み込むプログラムやデータを格納する。キーボード２０６およびマウス２０７は、プログラムを起動したり、プログラムの動作を指定したりするための入力装置を構成する。表示部２０８は、プロセスの動作結果を表示する。データ入出力部２０９は、装置外部との間でデータを入出力する。距離画像計測装置はデータ入出力部２０９を介して本発明に係る位置姿勢計測装置に接続されている。

図２（ｂ）を参照して、位置姿勢計測装置２５０の機能構成を説明する。位置姿勢計測装置２５０は、画像取得部２５１と、被遮蔽領域抽出部２５２と、対応付け部２５３と、最適化処理部２５４とを備える。画像取得部２５１は、データ入出力部２０９に接続された不図示の距離画像計測装置から距離画像データおよび濃淡画像データを取得して、メモリ２０２に格納する。被遮蔽領域抽出部２５２は、濃淡画像上の被遮蔽領域を抽出する。対応付け部２５３は、計測対象物体のモデルと画像特徴との対応付けを行う。最適化処理部２５４は、計測対象物体の概略の位置姿勢を補正することにより計測対象物体の位置姿勢を最適化して算出する。

図３を参照して、第１実施形態に係る処理の流れを示すフローチャートについて説明する。ステップＳ３０１において、処理に用いるデータの初期設定を行う。具体的には、外部記憶装置２０５から、計測対象物体の３次元モデルデータと、計測対象物体の位置姿勢の概略値データと、距離画像計測装置の撮像部のカメラパラメータ（例えば、主点位置、焦点距離）をメモリ２０２にロードする。なお、以下では撮像部を「カメラ」とも称する。

３次元モデルは頂点の集合および頂点を結ぶ線分の集合によって定義される。したがって、３次元モデルデータは、頂点の識別番号と座標、各線分の識別番号と両端の頂点の識別番号から構成される。

ステップＳ３０１の概略値取得処理で取得された計測対象物体の概略位置姿勢データは、位置姿勢計測装置に対する計測対象物体の位置及び姿勢の概略値を示す。ここで、位置姿勢計測装置に対する物体の位置及び姿勢とは、カメラ座標系における物体の位置及び姿勢である。概略位置姿勢の算出方法は任意であるが、本実施形態では、時間軸方向に連続して位置姿勢の計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。初回の概略位置姿勢は例えば、特許文献１に開示される方法を用いて算出する。また、距離画像計測装置の撮像部のカメラパラメータは例えば非特許文献３で開示される方法により事前に校正しておく。

ステップＳ３０２において、画像取得部２５１は、データ入出力部２０９に接続された不図示の距離画像計測装置から距離画像データおよび２次元画像データ（濃淡画像データまたはカラー画像データ）を取得して、メモリ２０２に格納する。

ステップＳ３０３において、被遮蔽領域抽出部２５２は、ステップＳ３０２の２次元画像取得処理で取得された濃淡画像上から被遮蔽領域を抽出する。具体的には、ステップＳ３０２の距離画像取得処理で取得された距離画像データを参照して、画素値が計測不能を示す０になっている画素を抽出する。抽出結果は濃淡画像・距離画像と同じサイズの２値画像（以下、「被遮蔽領域画像」と称する）としてメモリ２０２に格納する。被遮蔽領域画像では画素値は１または０であり、画素値が１の場合は当該画素が被遮蔽領域内の画素であることを表し、画素値が０の場合は当該画素が被遮蔽領域外の画素であることを表す。

ステップＳ３０４において、対応付け部２５３は、計測対象物体のモデルと画像特徴との対応付けを行う。ステップＳ３０４の処理の詳細は後述する。

ステップＳ３０５において、最適化処理部２５４は、計測対象物体の概略の位置姿勢を補正することにより計測対象物体の位置姿勢を算出する。ステップＳ３０５の処理の詳細は後述する。

ここで、図４を参照して、ステップＳ３０４の計測対象物体のモデルと画像特徴との対応付け処理の詳細を示すフローチャートについて説明する。本処理では、３次元モデルを構成する線分と２次元画像上（濃淡画像上またはカラー画像上）のエッジとを対応付ける。対応付けの方法としては、非特許文献４で開示される方法と同様の方法を用いる。

ステップＳ４０１において、３次元モデルを構成する線分と２次元画像上（濃淡画像上またはカラー画像上）エッジとの対応付けが済んでいないモデル上の１つの線分を選択する。

ステップＳ４０２において、図３のステップＳ３０１で入力された計測対象物体の概略位置及び姿勢とカメラの内部パラメータとを用いて、ステップＳ４０１で選択された線分の画像上への投影像を算出する。線分の投影像は画像上でも線分となる。

ステップＳ４０３において、ステップＳ４０２で算出された投影線分上に制御点を設定する。ここで、制御点とは、投影線分を等間隔に分割するような、投影線分上の点である。

制御点は、投影結果として得られる制御点の２次元座標および線分の２次元方向の情報と、３次元モデル上の制御点の３次元座標および線分の３次元方向の情報とを保持する。また、制御点の分割元となる３次元モデルの線分が保持する３次元的属性の情報も同様に保持する。投影線分の制御点は、線分ごとの総数をｎとして、各制御点をＤＦｉ（ｉ＝１、２、…、ｎ）として表す。

ステップＳ４０４において、ステップＳ４０３で算出された投影線分の制御点ＤＦｉ（ｉ＝１、２、…、ｎ）のうち、まだ対応点を検出していない制御点１点を選択する。

ステップＳ４０５において、ステップＳ４０４で選択された制御点に対応する２次元画像中のエッジを検出する。

図８を参照して、ステップＳ４０５における２次元画像中のエッジ検出方法を説明する。エッジの検出は、図８（ａ）に示すように、制御点ＤＦｉの探索ライン(制御点の２次元方向の法線方向)上において、濃淡画像上の濃度勾配から極値を算出することにより行う。エッジは、探索ライン上において濃度勾配が極値をとる位置に存在する。探索ライン上で検出されたエッジが一つのみである場合は、そのエッジを対応点とする。また、図８（ｂ）に示すように、探索ライン上で検出されたエッジが複数存在する場合には、制御点に最も近い点を対応点とする。

ステップＳ４０６において、ステップＳ４０５で検出された対応点の２次元座標を参照し、当該対応点が図３のステップＳ３０３で抽出された被遮蔽領域の範囲内に存在するか否かを判定する。具体的には、対応点と同一座標の被遮蔽領域画像の画素値を参照し、１なら被遮蔽領域内、０なら被遮蔽領域外と判定する。対応点が被遮蔽領域の範囲内に存在すると判定された場合は（ステップＳ４０６；ＹＥＳ）、ステップＳ４０７に進む。一方、対応点が被遮蔽領域の範囲内に存在しないと判断された場合は（ステップＳ４０６；ＮＯ）、ステップＳ４０８に進む。

ステップＳ４０７において、ステップＳ４０４で選択された制御点の２次元座標と、ステップＳ４０５で検出されたエッジの２次元座標と、これらの対応点対に関する位置姿勢計算への重み係数の値α（＜１．０）とを、対応点対データとしてメモリ２０２に保持する。

一方、ステップＳ４０８において、ステップＳ４０４で選択された制御点の２次元座標と、ステップＳ４０５で検出されたエッジの２次元座標と、これらの対応点対に関する位置姿勢計算への重み係数の値１．０とを、対応点対データとしてメモリ２０２に保持する。

ステップＳ４０９において、ステップＳ４０４で設定された制御点のすべてについて対応するエッジを検出したか否かを判定する。制御点のすべてについて対応するエッジを検出したと判定された場合（ステップＳ４０９；ＹＥＳ）、ステップＳ４１０に進む。一方、制御点のすべてについて対応するエッジを検出していないと判定された場合（ステップＳ４０９；ＮＯ）、ステップＳ４０４に戻る。

ステップＳ４１０において、モデル上のすべての線分についてエッジ検出を完了したか否かを判定する。モデル上のすべての線分についてエッジ検出を完了したと判定された場合（ステップＳ４１０；ＹＥＳ）、処理が終了する。これにより、図３のステップＳ３０４の処理の全体が終了する。一方、モデル上のすべての線分についてエッジ検出を完了していないと判定された場合（ステップＳ４１０；ＮＯ）、ステップＳ４０１に戻る。

以上の処理を通じて、モデルの線分上の点に対応する画像上のエッジの対が求められ、そのうちエッジが被遮蔽領域内にある対に関しては、位置姿勢計算への重み係数が比較的低く設定される。

最後に、ステップＳ３０４で検出された対応点対を用いて、計測対象物体の位置姿勢の値を最適化する処理（ステップＳ３０５の処理）の詳細について説明する。

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

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

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

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

なお、ｒ_０＝ｕ_０ｓｉｎθ−ｖ_０ｃｏｓθ（＝定数）、ｄ＝ｕ’ｓｉｎθ−ｖ’ｃｏｓθ（＝定数）とする。式（４）はＬｃ個の制御点について成り立つため、式（５）のようなΔｓに対する線形連立方程式が成り立つ。

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

ここで、各対応点対、すなわち各制御点とその制御点に対応するエッジとの組に対してステップＳ３０４で設定された重み係数をｗ_ｉとする。ここで重み行列Ｗを式（７）のように定義する。

重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗ_ｉが入る。この重み行列Ｗを用いて、式（６）を式（８）のように変形する。

Ｇａｕｓｓ−Ｎｅｗｔｏｎ法などを用いて、式（８）を解くことにより補正値Δsを求めると、式（９）のようになる。

このようにして算出された補正値Δsによりモデルの位置・姿勢の概略値を補正して位置・姿勢を更新する。

次に、物体の位置及び姿勢の反復演算が収束しているか否かを判定する。補正値Δsが十分に小さい場合、誤差r−dの総和が十分小さい場合、または誤差r−dの総和が変化しない場合は、物体の位置及び姿勢の計算が収束したと判定する。収束していないと判定された場合には、更新された物体の位置及び姿勢を用いて再度線分の傾きθ、ｒ_０、d、およびｕ、ｖの偏微分を計算し直し、式（８）を用いて再度補正値Δsを求め直す。

このようにして物体の位置・姿勢を求めた結果は、メモリ２０２に格納するか、あるいはデータ入出力部２０９を介して外部の装置に出力する。

なお、ここでは非線形最適化手法としてＧａｕｓｓ−Ｎｅｗｔｏｎ法を用いている。しかしながら、非線形最適化手法はこれに限るものではなく、Ｎｅｗｔｏｎ−Ｒａｐｈｓｏｎ法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。以上、ステップＳ３０５における物体の位置姿勢の最適化処理について説明した。

上述の処理を行うことにより、位置姿勢を算出するために必要な濃淡画像上の特徴点が照明の影により発生した擬似的な特徴点であるか否かを判定することが可能となる。そして、擬似的な特徴点に関しては位置姿勢の算出における重み係数を比較的低く設定することにより、ノイズの影響を低減させ、結果として位置姿勢算出の失敗や位置姿勢推定精度の低下の危険性を軽減することが可能となる。

なお、本実施形態は上記に限らず様々な変形例が存在する。

上記の説明では、エッジ検出後にエッジが被遮蔽領域内か否かを判定している。しかし、被遮蔽領域をエッジ検出処理の対象外にしてもよい。この場合、モデルの線分に対応する対応点の中に被遮蔽領域内のエッジは存在しないことになる。これは、上記の説明において被遮蔽領域内のエッジの重み係数を０に設定することと等価であるが、被遮蔽領域内でエッジを探索しないので処理が高速化される。

また、上記の説明では、モデル線分上の制御点１点に対してエッジを１点検出しており、検出したエッジは被遮蔽領域内に存在する可能性がある。しかし、制御点１点に対してエッジを複数点検出し、その中から被遮蔽領域外に存在する制御点に最も近いエッジを対応点として選択してもよい。

また、上記の説明では、距離画像と濃淡画像の撮影とで利用するカメラおよび照明は同一であり、位置や方向のずれはない。カメラまたは照明、あるいはカメラと照明の両方に位置や方向のずれがあっても、ずれが微小であれば上記の説明と同様の方法が利用できる。また、ずれが存在する場合は、ステップＳ３０３において距離画像の計測不能領域を膨張させて、被遮蔽領域としてもよい。

また、上記の説明では、計測対象物体のモデルとして、３次元線分モデルを利用している。しかし、必ずしも３次元線分モデルに限定されない。３次元モデルから３次元線分を算出することができる限り、３次元モデルの方式に制限はない。例えば、頂点情報および各頂点を結んだ面の情報で構成されたメッシュモデルを利用してもよいし、ＮＵＲＢＳ曲面などパラメトリックな曲面表現を利用してもよい。これらの場合、形状情報の中から直接的に３次元線分情報を参照することが出来ないため、３次元線分情報をランタイムに算出する必要がある。この処理は、ステップＳ４０２における３次元線分投影処理の代わりに行う。具体的には、計測対象物体の概略位置及び姿勢に基づいて、３次元モデルをコンピュータグラフィックス（ＣＧ）により描画し、描画結果からエッジ検出を行う。検出されたエッジが等間隔になるように、ステップＳ４０３と同様に、制御点を求める。

また、上記の説明では画像特徴として点特徴を用いている。しかし、これに限るものではなく、位置属性を持つのであれば、線特徴や面特徴であってもよい。線特徴や面特徴を画像特徴として用いるときも、画像特徴と被遮蔽領域とが包含関係にある場合や画像特徴と被遮蔽領域とが交差する場合に、当該画像特徴に関する位置姿勢計算における重み係数を比較的低く設定する。

（第２実施形態）
第１実施形態では、被遮蔽領域内の対応点については一様に位置姿勢推定計算への重み係数を比較的低く設定した。しかし、誤った対応点が検出される可能性は被遮蔽領域内で一様ではなく、影の先端部（たとえば、図１の例では線分１０５ｂ上）で誤った対応点が検出される可能性が特に高い。

そこで、本実施形態では、影の先端であり擬似的なエッジが検出され易い領域（以下、「影先端境界」と称する）を抽出し、影先端境界上の画素に限定して位置姿勢推定計算への重み係数を比較的低く設定する。

なお、影先端境界の抽出は、影を落としている凸部よりも影先端境界の方がカメラからの距離が遠い、すなわち距離画像上の距離値が大きいことを利用して実現する。

図５を参照して、第２実施形態に係る位置姿勢計測装置５００の機能構成を説明する。位置姿勢計測装置５００の機能構成は第１実施形態と同様である。位置姿勢計測装置５００は、画像取得部２５１と、被遮蔽領域抽出部２５２と、対応付け部２５３と、最適化処理部２５４と、影先端領域抽出部５０１とを備える。画像取得部２５１と、被遮蔽領域抽出部２５２と、対応付け部２５３と、最適化処理部２５４と、の各処理部は、第１実施形態の各処理部と同様であるので説明を省略する。影先端領域抽出部５０１は、影先端境界を抽出する。

図６を参照して、第２実施形態に係る処理の全体の流れを示すフローチャートについて説明する。

ステップＳ６０１において、処理に用いるデータの初期設定を行う。具体的には第１実施形態のステップＳ３０１でロードされるデータに加えて、不図示の距離計測装置の照明部のプロジェクタパラメータ、カメラと照明部との相対的な位置姿勢を外部記憶装置２０５からメモリ２０２にロードする。

ステップＳ６０２およびステップＳ６０３の処理は、それぞれ第１実施形態のステップＳ３０２およびステップＳ３０３の処理と同様である。

ステップＳ６０４において、影先端領域抽出部５０１は、影先端境界を抽出する。ステップＳ６０４の処理の詳細は後述する。

ステップＳ６０５において、対応付け部２５３は、計測対象物体のモデルと画像特徴との対応付けを行う。ステップＳ６０５の処理は第１実施形態のステップＳ３０４の処理とほぼ同様である。ただし、被遮蔽領域内の対応点に関する重み係数をα（＜１．０）として小さく設定するだけではなく、対応点が影先端境界上にある場合に、その対応点に関する寄与度をさらに低く設定する。すなわち、第１実施形態で設定された被遮蔽領域の範囲内にエッジが含まれる場合に設定される重み係数α（＜１．０）よりも小さい重み係数β（＜α）を、対応点が影先端境界上にある場合の重み係数として設定する。

ステップＳ６０６において、最適化処理部２５４は、計測対象物体の概略の位置姿勢を補正することにより計測対象物体の位置姿勢を算出する。ステップＳ６０６の処理は、第１実施形態のステップＳ３０５の処理と同様である。

ここで、ステップＳ６０４の影先端境界の抽出処理について詳しく説明する。濃淡画像を撮影するときに計測対象物体に照射する均一光を平面状の光の集合体として考えると、その平面が計測対象物体の表面で影が落ちている領域と交差する線分上で不図示の距離計測装置から最も遠い点が影先端境界上の画素に相当する。距離画像あるいは濃淡画像上でこの交差線分に相当するのは、被遮蔽領域を通るエピポーラ線である。そこで、影先端境界を求めるには、各エピポーラ線と被遮蔽領域の輪郭との２つの交点近傍の距離値を比較して、距離値が大きい（カメラから遠い）方の点を検出すればよい。具体的な処理については、図１（ｃ）および図７を参照して説明する。

影先端境界の抽出処理では、ステップＳ６０３で抽出された被遮蔽領域のうち、連結していない被遮蔽領域の部分領域（以下、「要素被遮蔽領域」と称する）のそれぞれに対して、図７のフローチャートに示す処理を施す。被遮蔽領域から要素被遮蔽領域を分離するには、ステップＳ６０３で作成した被遮蔽領域画像に対して、連結している画素同士に同一のラベル値を付与するようにしてラベリングする。この結果、異なる要素被遮蔽領域に対して異なるラベル値が画素値として付与される。ラベル値は１以上であるとする。なお、要素被遮蔽領域以外の領域の画素値は０である。

ステップＳ７０１において、影先端領域抽出部５０１は、要素被遮蔽領域の輪郭を抽出する。図１（ｃ）の領域１０３ｃが要素被遮蔽領域に相当する場合を考える。

ステップＳ７０２において、ステップＳ７０１の輪郭抽出処理で抽出された輪郭上の画素のうち、未処理の画素を第１の点として１つ選択する。ここで選択された画素を線分１０５ｃ上の「画素Ａ」とする。なお、線分１０５ｃは図１（ｂ）における線分１０５ｂに相当する。

ステップＳ７０３において、被遮蔽領域画像を参照して、被遮蔽領域外の画素から第２の点として、ステップＳ７０２で選択された画素Ａの最近傍の画素を探索する（第１探索処理）。ステップＳ７０３で探索された画素を「画素Ｂ」とする。被遮蔽領域外の画素から画素Ａに最近傍となる画素を探索するのは、画素Ａが要素被遮蔽領域上に存在し、画素Ａについての距離値が取得できない可能性があるためである。

ステップＳ７０４において、ステップＳ７０２で選択された輪郭上の画素Ａを通るエピポーラ線１０９ｃと、ステップＳ７０１で抽出された要素被遮蔽領域の輪郭との交点を第３の点として検出する。この交点における画素を画素Ｃとする。さらに被遮蔽領域外の画素から画素Ｃに最近傍の画素を第４の点として探索する（第２探索）。ここで探索された画素を「画素Ｄ」とする。

ステップＳ７０５において、画素Ｂおよび画素Ｄと同じ座標の距離画像上の画素の距離値を参照して、画素Ｂに相当する距離値と画素Ｄに相当する距離値とを比較する。すなわち、画素Ｂに相当する距離値が画素Ｄに相当する距離値よりも大きいか否かを判別する。画素Ｂに相当する距離値が画素Ｄに相当する距離値よりも大きいと判別された場合（ステップＳ７０５；ＹＥＳ）、ステップＳ７０６に進む。一方、画素Ｂに相当する距離値が画素Ｃに相当する距離値以下であると判別された場合（ステップＳ７０５；ＮＯ）、ステップＳ７０７に進む。図１（ｃ）の例では、凸部１０２ａの影響により画素Ｂに相当する距離値が画素Ｄに相当する距離値よりも大きい場合に該当するので（ステップＳ７０５；ＹＥＳ）、ステップＳ７０６に進む。

ステップＳ７０６において、画素Ｂに対応する要素被遮蔽領域内の画素Ａを影先端境界上の画素として登録する。すなわち、画素Ａに対応する画素Ｂの座標をメモリ２０２上の影先端領域上の画素リストに追加する。このようにして線分１０５ｃ上の画素が影先端領域の画素として抽出される。

ステップＳ７０７において、ステップＳ７０１で抽出された要素被遮蔽領域の輪郭上の全ての画素に対して影先端画素の検出処理が完了したか否かを判定する。影先端画素の検出処理が完了したと判定された場合（ステップＳ７０７；ＹＥＳ）、処理は終了する。すなわち、ステップＳ６０４の処理が終了する。一方、影先端画素の検出処理が完了していないと判定された場合（ステップＳ７０７；ＮＯ）、ステップＳ７０２に戻る。

ステップＳ６０５において、ステップＳ６０４で第２の点（画素Ｂ）に対応する距離画像上の距離値が第４の点（画素Ｄ）に対応する距離画像上の距離値よりも大きいと判別された場合、第１の点であって２次元画像上のエッジを構成する点に関して対応付けられた組が位置姿勢計測へ寄与する重み係数を、第１の点以外の点に関する組の重み係数よりも小さく設定する。

上述した処理を行うことにより、計測対象物体の位置姿勢を推定する上で推定失敗や精度低下の原因となる、照明の影により発生する擬似的な特徴点が存在する位置を検出することができる。その結果、擬似的でない特徴点が位置姿勢推定処理から除外される危険性が低減する。本実施形態に係る方法は第１実施形態に係る方法に比べて、より位置姿勢推定精度の低下を軽減することが可能である。

本実施形態では、第１実施形態のステップＳ３０３の処理に相当するステップＳ６０３の処理において、被遮蔽領域の重み係数を低く設定した後に、さらに影先端境界の抽出処理を行って影先端境界領域の重み係数をさらに低く設定する構成である。しかしながら、ステップＳ６０３では重み係数の設定は実行せずに被遮蔽領域の抽出のみを実行して、ステップＳ６０４において影先端境界領域に対してのみ、影先端境界領域以外の点（第１の点以外の点）の重み係数を低く設定する構成としてもよい。

上記の説明では、エッジ検出処理の対象から影先端境界の画素を除外せず、エッジ検出後にエッジが影先端画素か否かを判定している。しかし、影先端境界上の画素をエッジ検出処理の対象外にしてもよい。この場合、モデルの線分に対応する対応点の中に影先端境界上の画素は存在しないことになる。これは、影先端に存在する対応点の重み係数を０に設定することと等価である。

上記の説明では、モデル線分上の制御点１点に対してエッジを１点検出している。しかし、制御点１点に対してエッジを複数点検出し、その中から影先端境界上に存在しない制御点に最も近いエッジを対応点として選択してもよい。

上記の説明では画像特徴として点特徴を用いている。しかし、必ずしも点特徴に限定されない。位置属性を持つのであれば、画像特徴は線特徴や面特徴であってもよい。線特徴や面特徴を画像特徴として用いるときも、画像特徴が影先端境界との包含関係にある場合や画像特徴と影先端境界とが交差する場合に、当該画像特徴に関する位置姿勢計算における重み係数を比較的低く設定する。

上記の説明では、距離画像の撮影と濃淡画像の撮影とで利用するカメラおよび照明は同一であり、位置や方向のずれはない。カメラまたは照明、あるいはカメラと照明の両方に位置や方向のずれがあっても、ずれが微小であれば上記の説明と同様の方法を利用できる。また、ずれが存在する場合は、ステップＳ６０４の後に、影先端境界の領域を膨張させてもよい。

上記の説明では、影先端に限定して対応点の位置姿勢推定計算への重み係数を低く設定した。しかし、誤った対応点が検出される可能性は影先端よりも低いものの、影先端境界以外の被遮蔽領域で誤対応が起こる可能性もある。そこで、被遮蔽領域内の対応点について重み係数を比較的低く設定し、さらに影先端境界上で検出された対応点は影先端境界以外の領域に比べて重み係数を低くしてもよい。

上記の説明では、影の先端を検出し、影先端境界上の対応点については位置姿勢推定計算への重み係数を比較的低く設定している。これとは逆に、濃淡画像上で影を落としている凸部と影との境界（例えば、図１（ｂ）における線分１０４ｂ、以下、「遮蔽領域・被遮蔽領域境界」と称する）を検出し、この境界において検出された対応点の重み係数は比較的高く設定してもよい。この場合、遮蔽領域・被遮蔽領域境界以外の被遮蔽領域で検出された対応点の重み係数は比較的低く設定する。さらに、遮蔽領域・被遮蔽領域境界を膨張させてもよい。

また、これに加えて、被遮蔽領域から影先端境界と遮蔽領域・被遮蔽領域境界を除いた領域に比べて、影先端境界については対応点の重み係数を低く設定してもよい。

なお、遮蔽領域・被遮蔽領域境界を抽出するには、影先端の抽出と同様に被遮蔽領域の輪郭を通るエピポーラ線を想定している。エピポーラ線と被遮蔽領域輪郭との交点近傍の画素に対応する距離値を比較して、より距離値の小さい（つまり、カメラに近い）側の輪郭上の画素を選択すればよい。

（第３実施形態）
第１実施形態および第２実施形態の方法は、計測対象物体の位置姿勢推定において被遮蔽領域の悪影響を軽減する方法である。しかし、本発明の方法が適用できるのは位置姿勢推定に限定されず、位置属性を持つ画像特徴を用いる任意の画像処理に適用できる。

例えば、局所的な画像特徴をモデルとマッチングすることによって物体を認識する手法群において、モデルとの類似度を算出する計算に対する被遮蔽領域内の画像特徴の重み係数を比較的低く設定することによって、認識の失敗の可能性を軽減することができる。あるいは、被遮蔽領域を画像特徴の抽出処理の対象外とすることによっても同様の効果が得られる。

（第４実施形態）
上述の実施形態では、モデルフィッティングやマッチング処理における重み係数を低く設定したり、処理対象から除外する対象となる対応が存在する領域（被遮蔽領域、影先端部、遮蔽領域・被遮蔽領域境界）を、対応の探索処理に先立って抽出したりしている。しかしながら、探索された対応の候補がこれらの領域内あるいは近傍に存在するか否かを逐一判定してもよい。

例えば第１実施形態では、ステップＳ４０５において、制御点に対応するエッジ候補を探索する際に、近傍に距離画像が計測不能である画素が存在するエッジは重み係数を比較的低く設定すればよい。

また、第２実施形態ではステップＳ６０５において、距離画像が計測不能な画素近傍にあるエッジ候補について、そのエッジ候補を通るエピポーラ線上での距離値に基づいて当該エッジ候補が影先端部に属するか否かを判定すればよい。すなわち、距離画像が計測不能な領域の両端での距離値を比べて、より大きい場合は影先端部に属すると判定する。

なお、第２実施形態において、距離値の比較をエピポーラ線上で行うのではなく、簡易的にエッジの探索ライン上で行うことも可能である。エピポーラ線を用いる場合に比べて、影先端部を判定する正確さは低下するものの、計算量が低減するので、処理の高速化が可能である。

（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。

２５１：画像取得部、２５２：被遮蔽領域抽出部、２５３：対応付け部、２５４：最適化処理部

Claims

対象物体の３次元形状情報を含む３次元モデルを保持する保持手段と、
前記対象物体の２次元画像を取得する２次元画像取得手段と、
前記２次元画像において前記対象物体に照射される照明が遮蔽される影領域の先端部と、前記影領域ではない非影領域との境界領域である、影先端境界領域を特定する特定手段と、
前記特定手段により特定された影先端境界領域と、前記対象物体の位置および姿勢の概略値とに基づいて、前記３次元モデルから抽出される第１の幾何特徴と、該第１の幾何特徴に対応する、前記２次元画像上の第２の幾何特徴とを対応付ける対応付け手段と、
前記対応付け手段により対応づけられた前記第１の幾何特徴と前記第２の幾何特徴とのずれが小さくなるように前記概略値を補正することにより、前記対象物体の位置および姿勢を計測する位置姿勢計測手段と、
を備えることを特徴とする位置姿勢計測装置。
前記対応付け手段は、更に、前記第１の幾何特徴と、前記第２の幾何特徴とを対応付けた結果に対して、重みを設定する重み設定手段を備え、
前記重み設定手段は、前記対象物体の位置および姿勢の概略値に基づいて、前記３次元モデルから抽出される第１の幾何特徴に対応する、前記２次元画像上の第２の幾何特徴を抽出し、該抽出された第２の幾何特徴が前記影先端境界領域に含まれていない場合には、前記抽出された第２の幾何特徴が前記影先端境界領域に含まれている場合よりも、前記第１の幾何特徴と、前記第２の幾何特徴とが対応付けられた結果に対して大きな重みを設定し、
前記位置姿勢計測手段は、前記設定された重みと、前記対応づけられた結果とに基づいて、前記対象物体の位置姿勢を計測することを特徴とする請求項１に記載の位置姿勢計測装置。
前記重み設定手段は、前記抽出された第２の幾何特徴が前記影先端境界領域に含まれている場合には、前記第１の幾何特徴と、前記第２の幾何特徴とが対応付けられた結果に対して、重みとして０を設定することを特徴とする請求項２に記載の位置姿勢計測装置。
前記対応付け手段は、前記対象物体の位置および姿勢の概略値に基づいて、前記３次元モデルから抽出される第１の幾何特徴に対応する、前記２次元画像上の第２の幾何特徴を抽出し、該抽出された第２の幾何特徴が前記影先端境界領域に含まれていない場合には、前記第１の幾何特徴と、前記第２の幾何特徴とを対応付け、前記抽出された第２の幾何特徴が前記影先端境界領域に含まれている場合には、前記第１の幾何特徴と、前記第２の幾何特徴とを対応付けないことを特徴とする請求項１に記載の位置姿勢計測装置。
前記対応付け手段は、前記対象物体の位置および姿勢の概略値に基づいて、前記２次元画像において前記影先端境界領域を除いた領域から、前記３次元モデルから抽出される第１の幾何特徴に対応する、第２の幾何特徴を抽出し、前記第１の幾何特徴と、前記第２の幾何特徴とを対応づけることを特徴とする請求項１に記載の位置姿勢計測装置。
前記２次元画像が、濃淡画像またはカラー画像であることを特徴とする請求項１乃至５の何れか１項に記載の位置姿勢計測装置。
更に、前記対象物体の距離画像を取得する距離画像取得手段を備え、
前記特定手段は、前記距離画像に基づいて、前記境界領域を特定することを特徴とする請求項１乃至６のいずれか１項に記載の位置姿勢計測装置。
前記２次元画像と、前記距離画像とは、同一の撮像装置により取得されることを特徴とする請求項７に記載の位置姿勢計測装置。
対象物体の３次元形状情報を含む３次元モデルを保持する保持手段を備える位置姿勢計測装置における位置姿勢計測方法であって、
２次元画像取得手段が、前記対象物体の２次元画像を取得する工程と、
特定手段が、前記２次元画像において前記対象物体に照射される照明が遮蔽される影領域の先端部と、前記影領域ではない非影領域との境界領域である、影先端境界領域を特定する工程と、
対応付け手段が、前記特定された影先端境界領域と、前記対象物体の位置および姿勢の概略値とに基づいて、前記３次元モデルから抽出される第１の幾何特徴と、該第１の幾何特徴に対応する、前記２次元画像上の第２の幾何特徴とを対応付ける工程と、
位置姿勢計測手段が、前記対応づけられた前記第１の幾何特徴と前記第２の幾何特徴とのずれが小さくなるように前記概略値を補正することにより、前記対象物体の位置および姿勢を計測する工程と、
を有することを特徴とする位置姿勢計測方法。
コンピュータを、請求項１乃至８の何れか１項に記載の位置姿勢計測装置の各手段として機能させるためのプログラム。