３次元物体姿勢・位置検出装置、方法、およびプログラム
【課題】参照点等を与えることなく、３次元物体の位置（撮影したカメラから３次元物体の予め定められた点までの距離）を高精度に検出することを可能とした３次元物体姿勢・位置検出装置を提供することである。
【解決手段】提案する３次元物体姿勢・距離位置装置では、２次元画像の対象物中心点の位置を、その２次元画像と、その２次元画像の姿勢に加えた組み合わせとして作成した辞書データを記憶装置８内に有するために、事前に参照点や図形を辞書データの各２次元画像に入力することなく、また、ステレオカメラ１６−１、１６−２を用いることにより、認識対象の３次元姿勢および対象物中心点までの距離を容易に認識することができる。
【解決手段】提案する３次元物体姿勢・距離位置装置では、２次元画像の対象物中心点の位置を、その２次元画像と、その２次元画像の姿勢に加えた組み合わせとして作成した辞書データを記憶装置８内に有するために、事前に参照点や図形を辞書データの各２次元画像に入力することなく、また、ステレオカメラ１６−１、１６−２を用いることにより、認識対象の３次元姿勢および対象物中心点までの距離を容易に認識することができる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象物の画像をステレオビジョンによって撮影し、その対象物に対して、３次元姿勢およびその対象物の予め定められた点までの距離すなわち対象物の位置を検出する技術に関する。
【背景技術】
【０００２】
対象物（３次元物体）の姿勢およびその対象物の位置を認識する技術の１つとして、対象物を中心とした一定距離の球面上でカメラを一定間隔で移動させながら撮影した２次元画像と、その姿勢を併せ持つ辞書データを用意し、認識対象を撮影した２次元画像と、その辞書データ中の各２次元画像との間で、２次元パターンマッチを行なう公知の手法がある。この手法では、パターンマッチの結果として、最も認識対象に類似していると判断された辞書データ中の２次元画像に対する姿勢を、認識対象の姿勢と判断している。なお、この手法は、２次元画像中の対象物の大きさに基づいて、その対象物の位置を判断しているために、対象物の位置を高精度に検出できないという問題がある。
【０００３】
一方、第１および第２のカメラにより構成されたステレオカメラにより、対象物に対してそれぞれ撮影された２つの２次元画像を取得し、各２次元画像について対象物の対応付けを行い、対応付けされた対象物の点等から対象物の位置姿勢を求める手法も古くから知られている。
【０００４】
例えば、特許文献１では、認識対象となる物品について、ある方向から撮影した２次元教示モデルを用意している。そして、この２次元教示モデル上（すなわち、そのモデルが表示されたディスプレイ上）に、マウス等の手段を用いてポイントすることにより、少なくとも３点の参照点あるいは同義の情報を与える図形を指定する。
【０００５】
その後、認識対象に対し、ステレオカメラの第１および第２のカメラによりそれぞれ撮影された２つの２次元画像について、この参照点等が指定された２次元教示モデルに幾何学的変形を施したもの（変形は可変パラメータのセットにより指定される）との間でパターンマッチを行ない、可変パラメータの一部の値が最も近い対象物候補を選出する。さらに、その可変パラメータを用いて先に指定した参照点を撮影された２つの２次元画像の対応点として求め、この対応点を用いて、ステレオ方式により認識対象に対し正確な位置および姿勢の測定を行なうことができる。
【特許文献１】特開２００４−９０１８３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし、特許文献１のように２次元教示モデルに幾何学的変形を行なう代わりに、上記の辞書データを使用してパターンマッチ等の処理を行なうこともある。この場合、辞書データ中に登録される各２次元画像に対し、少なくとも３点の参照点あるいは同義の情報を与える図形をマウス等を用いて指定することは、２次元画像の個数が膨大となることを考慮すると、煩雑かつ困難であるばかりか事実上、不可能に近い。
【０００７】
また、特許文献１に提案される手法では、認識対象をステレオカメラにより撮影した２つの２次元画像に対して、それぞれ独立に２次元パターンマッチを行ない、それぞれ独立に姿勢を求めている。しかし、例えばキャリブレーションされたステレオカメラでは、それぞれのカメラで撮影された２つの２次元画像間の幾何学的な関係が３次元剛体変換（一方が他方に対しどれだけ回転しどれだけ平行移動した位置にいるか）により定義される。
このため、独立に求めた姿勢は必ずしも剛体変換の関係を満たしているとは限らない。
【０００８】
剛体変換の関係が満たされていない場合には、２つの２次元画像のうちのいずれかに対して２次元パターンマッチ処理の結果が正しくないと考えられ、最終的に求められる姿勢も不正確となる。
【０００９】
本発明は、以上の問題点を考慮してなされたものであり、参照点等を与えることなく、３次元物体の位置（撮影したカメラの位置から３次元物体の予め定められた点までの距離）を高精度に検出することを可能とした３次元物体姿勢・位置検出装置、方法、およびプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
提案する３次元物体姿勢・位置検出装置は、３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせ、または、３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶する記憶部と、カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成され、前記対象物を撮影するステレオカメラと、前記ステレオカメラを構成する第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識するとともに、前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出する姿勢・距離認識部と、を有する。
【発明の効果】
【００１１】
提案する３次元物体姿勢・位置検出装置によれば、対象物の２次元画像と、その画像における対象物の姿勢と、その画像における対象物中心点位置とを、組み合わせて作成した辞書データを有するために、改めてマウス等により参照点や図形を辞書データの各２次元画像に入力することなく、また、ステレオカメラを用いることにより、認識対象の３次元姿勢および対象物中心点までの距離を容易に認識することができる。
【発明を実施するための最良の形態】
【００１２】
以下図面に基づいて、本発明の実施形態について詳細を説明する。
始めに、本実施形態の概要について説明する。
本実施形態では、複数の視点からの対象物の２次元画像である辞書データを使用している。この辞書データは、対象物の予め定められた点がその辞書データにおける２次元画像の所定の点となるようにして撮影された画像の集まりである。辞書データの作成方法としては以下の２通りがある。
（１）カメラを固定し、撮影した２次元画像の所定の点を、3次元直交座標系（ｘ軸、ｙ軸、ｚ軸）の原点と設定する。対象物の予め定められた点が、３次元直交座標系の原点と一致するように、その対象物を配置する。対象物の姿勢、すなわち対象物の3次元直交座標系の各軸に対する回転角度を、等間隔で変化させ、先の固定されたカメラにより対象物を撮影し、各姿勢情報（回転角度）と２次元画像を取得する。
（２）対象物の予め定められた点が、３次元直交座標系（ｘ軸、ｙ軸、ｚ軸）の原点と一致する位置に、その対象物を固定する。撮影した２次元画像の所定の点が３次元座標系の原点となるように、原点を中心とした球面上を等間隔にカメラを移動させて、各カメラ位置情報と、２次元画像を取得する。
【００１３】
いずれの方法でも、撮影した２次元画像の所定の点が原点に対応する。よって、取得したすべての２次元画像において、その画像の所定の点は対象物の予め定められた点に対応していることになる。
【００１４】
そして、カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成されたステレオカメラにより撮影したそれぞれの２次元画像に対して、辞書データの２次元画像とパターンマッチを行なうことによりその対象物の姿勢が認識されるとともに、それぞれの２次元画像に対して、対象物の予め定められた点の位置を辞書データに基づき検出し、これらを対応点とみなしてステレオ法により例えば第１のカメラからその対象物の予め定められた点までの距離を算出する。
【００１５】
結果として、その対象物の３次元姿勢およびその対象物の予め定められた点までの距離が検出できたことになる。なお、“対象物の予め定められた点”は、カメラを球面上で移動するときの球の中心であるとともに、対象物を回転させるときの回転中心であることから、これを“対象物中心点”ということもある。なお、この“対象物中心点”を対象物の重心に一致させることもできる。
【００１６】
図１は、本発明の一実施形態に係る辞書データ作成装置の構成を示すブロック図である。
図１に示すように、この辞書データ作成装置は、パーソナルコンピュータ（以下、ＰＣという）等のユーザの操作を通してあるいは自動でシステムの制御を行なうシステム制御機器１と、システム制御機器１からの指示に基づいて、対象物回転装置１１またはカメラ移動装置１２を駆動させる対象物回転制御部５またはカメラ移動制御部６と、を有する。
【００１７】
また、システム制御機器１は、対象物回転制御部５による指示によって対象物回転装置１１により対象物２が位置調整された後、または、カメラ移動制御部６による指示によってカメラ移動装置１２によりカメラ３が球面上を移動した後に、カメラ３に対象物２を撮影する指示を、撮影制御部７を通して出す。
【００１８】
対象物回転装置１１は、対象物回転制御部５からの指示に基づいて、（カメラ３の位置は固定として）対象物２をｘ軸、ｙ軸、ｚ軸の回りにθｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ）だけ回転させる。また、カメラ移動装置１２は、カメラ移動制御部６からの指示に基づいて、（対象物２の位置は固定として）カメラ３を3次元座標系の原点を中心としてｘ軸、ｙ軸、ｚ軸の回りに−θｘ（ｉ）、−θｙ（ｉ）、−θｚ（ｉ）だけ回転させて球面上を移動させる。
【００１９】
撮影された画像は、ｉ番目の画像（画像（ｉ））、ｉ番目の姿勢（θｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ））の組み合わせ、すなわち、ｉ番目の辞書データ（辞書Ｄ（ｉ））として記憶装置８に記憶される。なお、図１の実際の構成としては、対象物回転装置１１またはカメラ移動装置１２（対象物回転制御部５またはカメラ移動制御部６）のいずれか一方を選択的に有する構成となる。
【００２０】
図２は、辞書データ作成処理のフローチャートである。このフローチャートの処理は、辞書データ作成装置によって実行される。なお、このフローチャートは、図１で対象物回転装置１１を使用した場合に対応している。
【００２１】
図２のステップＳ１で、対象物２を原点に設置する前に、カメラ３により原点を撮影し、画像における原点位置を記憶する。そして、ステップＳ２で、上述の対象物中心点が原点に一致するように対象物２を設置する。
【００２２】
ステップＳ２に続くステップＳ３では、対象物回転装置１１により、対象物２を一定量回転させる。そして、その回転した状態の対象物２をステップＳ４でカメラ３により撮影する。
【００２３】
続くステップＳ５では、ステップＳ４で撮影した画像と、回転角度とを記憶装置８に記憶する。そして、ステップＳ６ですべての角度での撮影を完了したかどうかを判定する。
ステップＳ６ですべての角度での撮影を完了したと判定された場合（ステップＳ６の判定結果がＹｅｓの場合）、一連の処理を終了する。
【００２４】
ステップＳ６ですべての角度での撮影が完了していないと判定された場合（ステップＳ６の判定結果がＮｏの場合）、ステップＳ３に戻り、残りの角度位置での対象物２の撮影を続行する。
【００２５】
図３は、本発明の一実施形態に係る３次元物体姿勢・距離検出装置の構成を示すブロック図である。
図３に示すように、この３次元物体姿勢・距離検出装置は、認識対象１８を撮影する、カメラ間の回転関係が既知であるキャリブレーションされたカメラ１６−１および１６−２と、ＰＣ等のユーザの操作を通してあるいは自動でシステムの制御を行なうとともに、カメラ１６−１および１６−２に認識対象１８を撮影する指示を、撮影制御部２１を通して出すシステム制御機器１と、カメラ１６−１および１６−２で撮影したそれぞれの２次元画像に対して、記憶装置８に記憶される辞書データの２次元画像とパターンマッチを行なうことにより認識対象１８の姿勢を認識する２次元マッチング処理部２２と、それぞれの２次元画像に対して、対象物中心点の位置を辞書データに基づき検出し、これらを対応点とみなしてステレオ法により認識対象１８の中心点（予め定められた点）までの距離を算出する距離算出部２３と、を有する。
【００２６】
なお、パターンマッチとは、２つの多次元データ（画像を含む）の類似度を定量的に評価する手法の１つである。画像データは縦方向ｍ画素、横方向ｎ画素で表されるｍ×ｎ次元データと考えることができる。これを、Ｘｐｑ（ｐ＝１〜ｍ，ｑ＝１〜ｎ）と表現する。このとき、画像１（Ｘ１ｐｑ）と画像２（Ｘ２ｐｑ）の類似度を定量的に表す尺度値として、ＳＡＤ（Sum of Absolute Difference）、ＳＳＤ（Sum of Squared intensity Difference）などがある。ＳＡＤおよびＳＳＤの尺度値は数式１および数式２により定義される。
［数１］
ＳＡＤ＝ΣｐΣｑ｜Ｘ１ｐｑ−Ｘ２ｐｑ｜
［数２］
ＳＳＤ＝ΣｐΣｑ（Ｘ１ｐｑ−Ｘ２ｐｑ）２
この他に、Ｘ１ｐｑの平均値とＸ２ｐｑの平均値が等しくなるように上記をそれぞれ正規化した正規化ＳＡＤ、正規化ＳＳＤや、正規化相関と呼ばれるＮＣＣ（Normalized Cross Correlation）などが尺度値としてよく利用される。ＳＡＤやＳＳＤの値が小さいほど、両者（画像１と画像２）は類似しているといえる。
【００２７】
図４は、姿勢・距離認識処理のフローチャートである。このフローチャートの処理は、３次元物体姿勢・距離検出装置によって実行される。このフローチャートの処理に先立って、ステレオカメラ（カメラ１６−１および１６−２）はカメラ間の回転関係が既知であるようにキャリブレーションされているものとする。
【００２８】
図４のステップＳ１１で、認識対象（対象物）１８は、ステレオカメラを構成するカメラ１６−１および１６−２により２つの視点から撮影され、これにより、２つの２次元画像が得られる。
【００２９】
続く、ステップＳ１２では、２次元マッチング処理部２２により、得られた２つの２次元画像に対して、記憶装置８の辞書データの各２次元画像との間で２次元パターンマッチを行なうことで、撮影されたそれぞれの２次元画像の姿勢が推定（認識）される。なお、本実施形態では、辞書データには、対象物中心点位置の情報が付加されているため、ステレオカメラにより撮影された２つの２次元画像それぞれにおける、対象物中心点位置を推定することができ、続く、ステップＳ１３において、距離算出部２３により、推定された２つの２次元画像の対象物中心点位置が、ステレオ法における対応点とみなされて、その２つの２次元画像のうちのいずれか一方の画像の対象物中心点までの距離（あるいは双方の画像の各対象物中心点までの距離）を推定することができる。そして、続く、ステップＳ１４において、２次元パターンマッチ処理の結果として判明した認識対象１８の３次元姿勢と、ステレオ法を適用することにより判明した認識対象１８の対象物中心点位置までの、例えばカメラ１６−１からの距離を出力して、一連の処理を終了する。
【００３０】
以上、説明した内容につき、以下でさらに詳細に説明する。
以下では、対象物の例として、図５に示すような直方体状の物体が用いられる。
２次元画像と姿勢により構成される辞書データの作成方法としては、上述したように２通りある。第１の方法では、図６に示すように、まず、カメラを所定の位置に固定する。次に、対象物の予め定められた点（対象物中心点）と、３次元直交座標系の原点とを一致させる。さらに、上記した対象物回転装置１１により、その３次元直交座標系の原点（対象物中心点）の回りに対象物を回転させることにより、その対象物の姿勢を全方位へ等間隔で変化させ、その各姿勢の対象物を固定カメラで撮影して、それぞれの姿勢情報と２次元画像とを取得し、辞書データを作成する。この第１の方法では、対象物をｘ、ｙ、ｚの各軸方向に所定量ずつ回転させる上述の対象物回転装置１１が必要となる。
【００３１】
これに対し、第２の方法では、図７に示すように、対象物の予め定められた点（対象物中心点）と、３次元直交座標系の原点とを一致させ、その位置に対象物を固定する。そして、対象物中心点（原点）を中心とした球面上で、上記したカメラ移動装置により、等間隔にカメラを移動させて、それぞれのカメラ位置情報と２次元画像とを取得し、辞書データを作成する。この第２の方法では、直交座標系の原点（対象物中心点）を中心とした球面上で所定の位置へカメラ３を移動させる上述のカメラ移動装置１２が必要となる。
【００３２】
なお、上記第１の方法と第２の方法とは、対象物とカメラとのいずれを固定するかによる差異であり、得られる辞書データは等しい。
ここでは、上記第１の方法についてさらに詳細に説明する。
【００３３】
まず、カメラ３の位置を固定する。そして、対象物がない状態で撮影を行ない、撮影した画像上で３次元直交座標系の原点に対応する位置を設定することで、辞書データを作成するための対象物を回転させる３次元直交座標系を設定する。ここでは、簡単のために、図８に示すように、撮影した画像２５の画像中心点２６に原点が対応するように３次元直交座標系を設定する。
【００３４】
カメラ３は、任意の位置に固定可能であるが、固定する位置を決めた後は辞書データ作成のための撮影中はその位置を移動することはできない。
次に、対象物の予め定められた点（例えば重心）が３次元直交座標系の原点と一致するように、対象物を配置する。なお、この対象物の予め定められた点は、対象物を回転させるときの中心でもあるので、回転中心点とも呼ばれる。
【００３５】
対象物の姿勢は、図９に示すように、ｘ軸方向への回転角度θｘ、ｙ軸方向への回転角度θｙ、ｚ軸方向への回転角度θｚを指定することにより規定できる。これら角度θｘ、
θｙ、θｚを一定間隔で刻んで対象物の姿勢を変化させて、条件ごとに、固定カメラ３から対象物を撮影し、撮影した画像と各角度値とをその都度保存することにより、辞書データが作成される。
【００３６】
ｉ番目の辞書データ“辞書（ｉ）”は数式３のように表すことができる。
［数３］
辞書（ｉ） ＝ （画像（ｉ）、θｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ））
例えば角度を１度刻みとすると、辞書データの要素総数は、３６０×３６０×３６０となる。すなわち、上記インデックスｉは、１≦ｉ≦３６０×３６０×３６０、の範囲で変化する。
【００３７】
図１０に、辞書データに登録される画像の例を示す。各画像において、対象物中心点２７の座標は３次元座標系の原点に対応している。なお、この例では、３次元座標系の原点は画像中心点に対応している。
【００３８】
続いて、３次元物体（認識対象）の姿勢・距離検出処理について説明する。
認識対象をキャリブレーションされた２眼ステレオカメラで撮影して得た２次元画像の例を図１１に示す。
【００３９】
左の２次元画像３１および右の２次元画像３２のそれぞれに対して、辞書データの２次元画像との間でパターンマッチ処理を行なう。パターンマッチは、認識対象と辞書データ中の対象物とのサイズが異なる場合にも適用できるＳＩＦＴ演算子（Scale Invariant Feature Transform）などの公知の技術を用いて行なう。なお、ＳＩＦＴ演算子については例えば、D. Loweによる”Distinctive image features from scale-invariant keypoints” （ International Journal of Computer Vision, 60, 2(2004), pp91-110）に記載がある。
【００４０】
辞書データの各２次元画像は、画像の中心点に対象物の予め定められた点が対応しているが、撮影した図１１に示す左右の２次元画像３１、３２においては、通常、画像の中心点に認識対象（対象物）の予め定められた点が対応していない。
【００４１】
マッチング処理の結果として、図１２に示すように、左画像（左の２次元画像）３１に対し、対象物中心点３３に相当する座標（ｘｌ、ｙｌ）および、最も類似する辞書データの番号Ｄ（ｌ）、右画像（右の２次元画像）３２に対し、対象物中心点３４に相当する座標（ｘｒ、ｙｒ）および、最も類似する辞書データの番号Ｄ（ｒ）、が求まる。
【００４２】
このとき、左右画像の座標（ｘｌ、ｙｌ）および（ｘｒ、ｙｒ）は対象物の同一点に対応しており、ステレオ法における対応点となる。よって、ステレオビジョンにおける公知の手法を用いることにより、例えば左カメラを基準とした場合には、左カメラから中心点までの距離ｄｌを算出することができる。
【００４３】
結果として、左カメラを基準として、対象物の姿勢＝（θｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ））、対象物中心点までの距離＝ｄｌとして、対象物（認識対象）の姿勢および距離を認識することができる。
【００４４】
このように、本実施形態によれば、対象物の２次元画像と、その画像における対象物の姿勢と、その画像における対象物中心点位置とを組み合わせて作成した辞書データを有するために、事前に参照点や図形を辞書データの各２次元画像に入力することなく、また、ステレオカメラを用いることにより、認識対象の３次元姿勢および対象物中心点までの距離を容易に認識することができる。
【００４５】
続いて、本実施形態の変形例について説明する。
この変形例においては、図３の２次元マッチング処理部２２によるパターンマッチ処理に対し、両カメラの位置関係とパターンマッチ結果とが一致するように、また、パターンマッチの計算量が減るように改良を加えている。
【００４６】
以下に、この変形例について説明する。
上述したように、ステレオカメラはキャリブレーションされているため、Ｆ行列が求められている。Ｆ行列を両カメラ間の剛体変換（一方が他方に対しどれだけ回転しどれだけ平行移動した位置にいるか）の関係に分解できることは例えば、Richard HartleyとAndrew Zissermanによる”Multiple View Geometry in Computer Vision, Cambridge University Press”（Second Edition(2003), pp239-261）に示されるように公知の技術である。
【００４７】
そこで、ステレオカメラにより撮影された認識対象（対象物）の２つの２次元画像に対して、辞書データの各２次元画像とパターンマッチ処理を行なう場合において、一方の２次元画像に対して順次パターンマッチを行なうとともに、対応する（両カメラ間の）回転角に該当する近傍の辞書データ（すなわち、キャリブレーションにて定義された回転角度近傍の辞書データ）の２次元画像を用いて他方の２次元画像のパターンマッチを行ない、両画像がよりよく類似する辞書データを、パターンマッチ処理の結果として出力する。また、両画像がよりよく類似する辞書データを検索する方法としては、例えば、２つのパターンマッチ結果（すなわち、上記した尺度値）の加算値が最小である辞書データの各２次元画像をパターンマッチ処理の結果として採用する方法がある。
【００４８】
すなわち、撮影したそれぞれの２次元画像に対して、辞書データの各２次元画像とパターンマッチ処理を行なう場合において、一方の２次元画像と辞書番号ｉの２次元画像との間でパターンマッチを行ない、第１のパターンマッチ結果を算出するとともに、他方の２次元画像と辞書番号ｊの近傍の２次元画像との間でパターンマッチを行ない、第２のパターンマッチ結果を算出し、第１および第２のパターンマッチ結果の加算値が最小となる辞書データの２次元画像の組み合わせを姿勢として推定する。
【００４９】
なお、辞書番号ｊの２次元画像は、辞書番号ｉの２次元画像における対象物の姿勢をＲだけ回転させた姿勢である対象物の２次元画像とする。Ｒは、ステレオキャリブレーションにより定義されるカメラ間の回転関係を示す。なお、カメラ間の関係は、Ｆ行列によって定義されるが、これは、回転と平行移動の関係である。
【００５０】
図１３は、カメラ間の回転関係を考慮して、第１の２次元画像と対応する第２の２次元画像とに対するそれぞれの辞書データをパターンマッチ結果として求める処理のフローチャートである。
【００５１】
図１３のステップＳ２１において、辞書番号ｉおよび最小加算値を初期化する。なお、例えば辞書を各軸方向に１度刻みで作成した場合、辞書番号ｉは、１〜３６０×３６０×３６０の範囲をとることになる。別の表現をすると、辞書番号（ｉｒ１，ｉｒ２，ｉｒ３），ｉｒ１＝０〜３５９，ｉｒ２＝０〜３５９，ｉｒ３＝０〜３５９と表すことができる。本フローチャートにおいて、辞書番号ｉと辞書番号（ｉｒ１，ｉｒ２，ｉｒ３）の対応関係は既知であるものとする。また、最小加算値の初期化においては、最小加算値として記憶できる最大の値を入力すればよい。
【００５２】
ステップＳ２１に続く、ステップＳ２２では、カメラ間の回転関係を考慮して、辞書番号ｉに対する辞書番号ｊを求める。例えば、左画像（このフローチャート中では、“第１の２次元画像”として表記）ＸＬｐｑと辞書Ｘ（ｋｌ１，ｋｌ２，ｋｌ３）ｐｑが類似し
ていて、右画像（このフローチャート中では、“第２の２次元画像”として表記）ＸＲｐｑと辞書Ｘ（ｋｒ１，ｋｒ２，ｋｒ３）ｐｑが類似していて、辞書番号（ｋｌ１，ｋｌ２，ｋｌ３）と（ｋｒ１，ｋｒ２，ｋｒ３）が角度Ｒの回転関係にあり、角度Ｒが３次元座標系でＸ，Ｙ，Ｚの各軸角度が（ｒ１，ｒ２，ｒ３）と表されるとき、左画像ＸＬｐｑを基準とすると、数式４を満たすような右画像とパターンマッチさせる辞書Ｘ（ｋｌ１，ｋｌ２，ｋｌ３）ｐｑを、このステップＳ２２で求めることになる。
［数４］
ｋｒ１＝ｋｌ１＋ｒ１
ｋｒ２＝ｋｌ２＋ｒ２
ｋｒ３＝ｋｌ３＋ｒ３
ステップＳ２２に続くステップＳ２３では、第１の２次元画像（左画像）と辞書番号ｉの２次元画像のパターンマッチを行ない、第１のパターンマッチ結果（第１の尺度値）を求める。続く、ステップＳ２４では、第２の２次元画像（右画像）と辞書番号ｊの２次元画像のパターンマッチを行ない、第２のパターンマッチ結果（第２の尺度値）を求める。
【００５３】
ステップＳ２４に続くステップＳ２５では、第１のパターンマッチ結果（第１の尺度値）と、第２のパターンマッチ結果（第２の尺度値）とを加算し、加算値を求める。
続くステップＳ２６では、ステップＳ２５で求めた加算値が最小加算値より小さいか否かが判定される。
【００５４】
ステップＳ２６においてステップＳ２５で求めた加算値が最小加算値以上であると判定された場合（ステップＳ２６の判定結果がＮｏの場合）、ステップＳ２９に進む。
ステップＳ２６においてステップＳ２５で求めた加算値が最小加算値より小さいと判定された場合（ステップＳ２６の判定結果がＹｅｓの場合）、ステップＳ２７において、ステップＳ２５で求めた加算値を最小加算値に設定し、ステップＳ２８において、パターンマッチ処理の結果として、左右の各画像のパターンマッチ結果（尺度値）および辞書番号を記憶し、ステップＳ２９に進む。
【００５５】
ステップＳ２６またはステップＳ２８から制御を渡されたステップＳ２９では、辞書番号ｉが最大値であるかどうかを判定する。
ステップＳ２９において辞書番号ｉが最大値であると判定された場合（ステップＳ２９の判定結果がＹｅｓの場合）、一連の処理を終了する。
【００５６】
ステップＳ２９において辞書番号ｉが最大値ではないと判定された場合（ステップＳ２９の判定結果がＮｏの場合）、ステップＳ３０において辞書番号ｉをインクリメントし、ステップＳ２２に戻り、辞書データを使用したパターンマッチ処理を続行する。
【００５７】
このような一連の処理の結果として、例えば尺度値としてＳＳＤを使用した場合、ｋｒ１＝ｋｌ１＋ｒ１、ｋｒ２＝ｋｌ２＋ｒ２、ｋｒ３＝ｋｌ３＋ｒ３、という条件の下で、数式５を最小化するＸ（ｋｒ１，ｋｒ２，ｋｒ３）が検索されたことになる。
［数５］
ΣｍΣｎ（ＸＬｍｎ−Ｘ（ｋｌ１，ｋｌ２，ｋｌ３）ｍｎ）２
＋ΣｍΣｎ（ＸＲｍｎ−Ｘ（ｋｒ１，ｋｒ２，ｋｒ３）ｍｎ）２
図１３で説明した上記の場合においてさらに、一方の２次元画像に対するパターンマッチ処理により、画像上で対象物中心点の位置が求まると、他方の２次元画像の対象物中心点位置が対応するエピポーラ線上にあるものとして、そのエピポーラ線上に限定して他方の２次元画像に対するパターンマッチを行なうようにしてもよい。
【００５８】
なお、他方の２次元画像上に一方の２次元画像を撮影したカメラの視線（光軸）を引くことで、他方の２次元画像上にエピポーラ線を引くことができる。
このように、本実施形態の変形例によれば、領域を限定されたパターンにおいてパターンマッチを行なうため、計算量が削減されるとともに、誤分類の確率も減少する。
【図面の簡単な説明】
【００５９】
【図１】本発明の一実施形態に係る辞書データ作成装置の構成を示すブロック図である。
【図２】辞書データ作成処理のフローチャートである。
【図３】本発明の一実施形態に係る３次元物体姿勢・距離検出装置の構成を示すブロック図である。
【図４】姿勢・距離認識処理のフローチャートである。
【図５】対象物の例を示す図である。
【図６】辞書データ作成方法（その１）を示す図である。
【図７】辞書データ作成方法（その２）を示す図である。
【図８】辞書データの作成に先立って設定された座標系を示す図である。
【図９】辞書データの作成する際の撮影方法を示す図である。
【図１０】辞書データに登録される画像の例を示した図である。
【図１１】認識対象をキャリブレーションされた２眼ステレオカメラで撮影して得た２次元画像の例を示した図である。
【図１２】図１１の２次元画像の例に対象物中心点を追加した図である。
【図１３】カメラ間の位置関係を考慮して、第１の２次元画像と対応する第２の２次元画像とに対するそれぞれの辞書データをパターンマッチ結果として求める処理のフローチャートである。
【符号の説明】
【００６０】
１ システム制御機器
２ 対象物
３、１６−１、１６−２ カメラ
５ 対象物回転制御部
６ カメラ移動制御部
７、２１ 撮影制御部
８ 記憶装置
１１ 対象物回転装置
１２ カメラ移動装置
１８ 認識対象
２２ ２次元マッチング処理部
２３ 距離算出部
２５ 撮影した画像
２６ 画像中心点
２７、３３、３４ 対象物中心点
３１ 左の２次元画像
３２ 右の２次元画像
【技術分野】
【０００１】
本発明は、対象物の画像をステレオビジョンによって撮影し、その対象物に対して、３次元姿勢およびその対象物の予め定められた点までの距離すなわち対象物の位置を検出する技術に関する。
【背景技術】
【０００２】
対象物（３次元物体）の姿勢およびその対象物の位置を認識する技術の１つとして、対象物を中心とした一定距離の球面上でカメラを一定間隔で移動させながら撮影した２次元画像と、その姿勢を併せ持つ辞書データを用意し、認識対象を撮影した２次元画像と、その辞書データ中の各２次元画像との間で、２次元パターンマッチを行なう公知の手法がある。この手法では、パターンマッチの結果として、最も認識対象に類似していると判断された辞書データ中の２次元画像に対する姿勢を、認識対象の姿勢と判断している。なお、この手法は、２次元画像中の対象物の大きさに基づいて、その対象物の位置を判断しているために、対象物の位置を高精度に検出できないという問題がある。
【０００３】
一方、第１および第２のカメラにより構成されたステレオカメラにより、対象物に対してそれぞれ撮影された２つの２次元画像を取得し、各２次元画像について対象物の対応付けを行い、対応付けされた対象物の点等から対象物の位置姿勢を求める手法も古くから知られている。
【０００４】
例えば、特許文献１では、認識対象となる物品について、ある方向から撮影した２次元教示モデルを用意している。そして、この２次元教示モデル上（すなわち、そのモデルが表示されたディスプレイ上）に、マウス等の手段を用いてポイントすることにより、少なくとも３点の参照点あるいは同義の情報を与える図形を指定する。
【０００５】
その後、認識対象に対し、ステレオカメラの第１および第２のカメラによりそれぞれ撮影された２つの２次元画像について、この参照点等が指定された２次元教示モデルに幾何学的変形を施したもの（変形は可変パラメータのセットにより指定される）との間でパターンマッチを行ない、可変パラメータの一部の値が最も近い対象物候補を選出する。さらに、その可変パラメータを用いて先に指定した参照点を撮影された２つの２次元画像の対応点として求め、この対応点を用いて、ステレオ方式により認識対象に対し正確な位置および姿勢の測定を行なうことができる。
【特許文献１】特開２００４−９０１８３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし、特許文献１のように２次元教示モデルに幾何学的変形を行なう代わりに、上記の辞書データを使用してパターンマッチ等の処理を行なうこともある。この場合、辞書データ中に登録される各２次元画像に対し、少なくとも３点の参照点あるいは同義の情報を与える図形をマウス等を用いて指定することは、２次元画像の個数が膨大となることを考慮すると、煩雑かつ困難であるばかりか事実上、不可能に近い。
【０００７】
また、特許文献１に提案される手法では、認識対象をステレオカメラにより撮影した２つの２次元画像に対して、それぞれ独立に２次元パターンマッチを行ない、それぞれ独立に姿勢を求めている。しかし、例えばキャリブレーションされたステレオカメラでは、それぞれのカメラで撮影された２つの２次元画像間の幾何学的な関係が３次元剛体変換（一方が他方に対しどれだけ回転しどれだけ平行移動した位置にいるか）により定義される。
このため、独立に求めた姿勢は必ずしも剛体変換の関係を満たしているとは限らない。
【０００８】
剛体変換の関係が満たされていない場合には、２つの２次元画像のうちのいずれかに対して２次元パターンマッチ処理の結果が正しくないと考えられ、最終的に求められる姿勢も不正確となる。
【０００９】
本発明は、以上の問題点を考慮してなされたものであり、参照点等を与えることなく、３次元物体の位置（撮影したカメラの位置から３次元物体の予め定められた点までの距離）を高精度に検出することを可能とした３次元物体姿勢・位置検出装置、方法、およびプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
提案する３次元物体姿勢・位置検出装置は、３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせ、または、３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶する記憶部と、カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成され、前記対象物を撮影するステレオカメラと、前記ステレオカメラを構成する第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識するとともに、前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出する姿勢・距離認識部と、を有する。
【発明の効果】
【００１１】
提案する３次元物体姿勢・位置検出装置によれば、対象物の２次元画像と、その画像における対象物の姿勢と、その画像における対象物中心点位置とを、組み合わせて作成した辞書データを有するために、改めてマウス等により参照点や図形を辞書データの各２次元画像に入力することなく、また、ステレオカメラを用いることにより、認識対象の３次元姿勢および対象物中心点までの距離を容易に認識することができる。
【発明を実施するための最良の形態】
【００１２】
以下図面に基づいて、本発明の実施形態について詳細を説明する。
始めに、本実施形態の概要について説明する。
本実施形態では、複数の視点からの対象物の２次元画像である辞書データを使用している。この辞書データは、対象物の予め定められた点がその辞書データにおける２次元画像の所定の点となるようにして撮影された画像の集まりである。辞書データの作成方法としては以下の２通りがある。
（１）カメラを固定し、撮影した２次元画像の所定の点を、3次元直交座標系（ｘ軸、ｙ軸、ｚ軸）の原点と設定する。対象物の予め定められた点が、３次元直交座標系の原点と一致するように、その対象物を配置する。対象物の姿勢、すなわち対象物の3次元直交座標系の各軸に対する回転角度を、等間隔で変化させ、先の固定されたカメラにより対象物を撮影し、各姿勢情報（回転角度）と２次元画像を取得する。
（２）対象物の予め定められた点が、３次元直交座標系（ｘ軸、ｙ軸、ｚ軸）の原点と一致する位置に、その対象物を固定する。撮影した２次元画像の所定の点が３次元座標系の原点となるように、原点を中心とした球面上を等間隔にカメラを移動させて、各カメラ位置情報と、２次元画像を取得する。
【００１３】
いずれの方法でも、撮影した２次元画像の所定の点が原点に対応する。よって、取得したすべての２次元画像において、その画像の所定の点は対象物の予め定められた点に対応していることになる。
【００１４】
そして、カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成されたステレオカメラにより撮影したそれぞれの２次元画像に対して、辞書データの２次元画像とパターンマッチを行なうことによりその対象物の姿勢が認識されるとともに、それぞれの２次元画像に対して、対象物の予め定められた点の位置を辞書データに基づき検出し、これらを対応点とみなしてステレオ法により例えば第１のカメラからその対象物の予め定められた点までの距離を算出する。
【００１５】
結果として、その対象物の３次元姿勢およびその対象物の予め定められた点までの距離が検出できたことになる。なお、“対象物の予め定められた点”は、カメラを球面上で移動するときの球の中心であるとともに、対象物を回転させるときの回転中心であることから、これを“対象物中心点”ということもある。なお、この“対象物中心点”を対象物の重心に一致させることもできる。
【００１６】
図１は、本発明の一実施形態に係る辞書データ作成装置の構成を示すブロック図である。
図１に示すように、この辞書データ作成装置は、パーソナルコンピュータ（以下、ＰＣという）等のユーザの操作を通してあるいは自動でシステムの制御を行なうシステム制御機器１と、システム制御機器１からの指示に基づいて、対象物回転装置１１またはカメラ移動装置１２を駆動させる対象物回転制御部５またはカメラ移動制御部６と、を有する。
【００１７】
また、システム制御機器１は、対象物回転制御部５による指示によって対象物回転装置１１により対象物２が位置調整された後、または、カメラ移動制御部６による指示によってカメラ移動装置１２によりカメラ３が球面上を移動した後に、カメラ３に対象物２を撮影する指示を、撮影制御部７を通して出す。
【００１８】
対象物回転装置１１は、対象物回転制御部５からの指示に基づいて、（カメラ３の位置は固定として）対象物２をｘ軸、ｙ軸、ｚ軸の回りにθｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ）だけ回転させる。また、カメラ移動装置１２は、カメラ移動制御部６からの指示に基づいて、（対象物２の位置は固定として）カメラ３を3次元座標系の原点を中心としてｘ軸、ｙ軸、ｚ軸の回りに−θｘ（ｉ）、−θｙ（ｉ）、−θｚ（ｉ）だけ回転させて球面上を移動させる。
【００１９】
撮影された画像は、ｉ番目の画像（画像（ｉ））、ｉ番目の姿勢（θｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ））の組み合わせ、すなわち、ｉ番目の辞書データ（辞書Ｄ（ｉ））として記憶装置８に記憶される。なお、図１の実際の構成としては、対象物回転装置１１またはカメラ移動装置１２（対象物回転制御部５またはカメラ移動制御部６）のいずれか一方を選択的に有する構成となる。
【００２０】
図２は、辞書データ作成処理のフローチャートである。このフローチャートの処理は、辞書データ作成装置によって実行される。なお、このフローチャートは、図１で対象物回転装置１１を使用した場合に対応している。
【００２１】
図２のステップＳ１で、対象物２を原点に設置する前に、カメラ３により原点を撮影し、画像における原点位置を記憶する。そして、ステップＳ２で、上述の対象物中心点が原点に一致するように対象物２を設置する。
【００２２】
ステップＳ２に続くステップＳ３では、対象物回転装置１１により、対象物２を一定量回転させる。そして、その回転した状態の対象物２をステップＳ４でカメラ３により撮影する。
【００２３】
続くステップＳ５では、ステップＳ４で撮影した画像と、回転角度とを記憶装置８に記憶する。そして、ステップＳ６ですべての角度での撮影を完了したかどうかを判定する。
ステップＳ６ですべての角度での撮影を完了したと判定された場合（ステップＳ６の判定結果がＹｅｓの場合）、一連の処理を終了する。
【００２４】
ステップＳ６ですべての角度での撮影が完了していないと判定された場合（ステップＳ６の判定結果がＮｏの場合）、ステップＳ３に戻り、残りの角度位置での対象物２の撮影を続行する。
【００２５】
図３は、本発明の一実施形態に係る３次元物体姿勢・距離検出装置の構成を示すブロック図である。
図３に示すように、この３次元物体姿勢・距離検出装置は、認識対象１８を撮影する、カメラ間の回転関係が既知であるキャリブレーションされたカメラ１６−１および１６−２と、ＰＣ等のユーザの操作を通してあるいは自動でシステムの制御を行なうとともに、カメラ１６−１および１６−２に認識対象１８を撮影する指示を、撮影制御部２１を通して出すシステム制御機器１と、カメラ１６−１および１６−２で撮影したそれぞれの２次元画像に対して、記憶装置８に記憶される辞書データの２次元画像とパターンマッチを行なうことにより認識対象１８の姿勢を認識する２次元マッチング処理部２２と、それぞれの２次元画像に対して、対象物中心点の位置を辞書データに基づき検出し、これらを対応点とみなしてステレオ法により認識対象１８の中心点（予め定められた点）までの距離を算出する距離算出部２３と、を有する。
【００２６】
なお、パターンマッチとは、２つの多次元データ（画像を含む）の類似度を定量的に評価する手法の１つである。画像データは縦方向ｍ画素、横方向ｎ画素で表されるｍ×ｎ次元データと考えることができる。これを、Ｘｐｑ（ｐ＝１〜ｍ，ｑ＝１〜ｎ）と表現する。このとき、画像１（Ｘ１ｐｑ）と画像２（Ｘ２ｐｑ）の類似度を定量的に表す尺度値として、ＳＡＤ（Sum of Absolute Difference）、ＳＳＤ（Sum of Squared intensity Difference）などがある。ＳＡＤおよびＳＳＤの尺度値は数式１および数式２により定義される。
［数１］
ＳＡＤ＝ΣｐΣｑ｜Ｘ１ｐｑ−Ｘ２ｐｑ｜
［数２］
ＳＳＤ＝ΣｐΣｑ（Ｘ１ｐｑ−Ｘ２ｐｑ）２
この他に、Ｘ１ｐｑの平均値とＸ２ｐｑの平均値が等しくなるように上記をそれぞれ正規化した正規化ＳＡＤ、正規化ＳＳＤや、正規化相関と呼ばれるＮＣＣ（Normalized Cross Correlation）などが尺度値としてよく利用される。ＳＡＤやＳＳＤの値が小さいほど、両者（画像１と画像２）は類似しているといえる。
【００２７】
図４は、姿勢・距離認識処理のフローチャートである。このフローチャートの処理は、３次元物体姿勢・距離検出装置によって実行される。このフローチャートの処理に先立って、ステレオカメラ（カメラ１６−１および１６−２）はカメラ間の回転関係が既知であるようにキャリブレーションされているものとする。
【００２８】
図４のステップＳ１１で、認識対象（対象物）１８は、ステレオカメラを構成するカメラ１６−１および１６−２により２つの視点から撮影され、これにより、２つの２次元画像が得られる。
【００２９】
続く、ステップＳ１２では、２次元マッチング処理部２２により、得られた２つの２次元画像に対して、記憶装置８の辞書データの各２次元画像との間で２次元パターンマッチを行なうことで、撮影されたそれぞれの２次元画像の姿勢が推定（認識）される。なお、本実施形態では、辞書データには、対象物中心点位置の情報が付加されているため、ステレオカメラにより撮影された２つの２次元画像それぞれにおける、対象物中心点位置を推定することができ、続く、ステップＳ１３において、距離算出部２３により、推定された２つの２次元画像の対象物中心点位置が、ステレオ法における対応点とみなされて、その２つの２次元画像のうちのいずれか一方の画像の対象物中心点までの距離（あるいは双方の画像の各対象物中心点までの距離）を推定することができる。そして、続く、ステップＳ１４において、２次元パターンマッチ処理の結果として判明した認識対象１８の３次元姿勢と、ステレオ法を適用することにより判明した認識対象１８の対象物中心点位置までの、例えばカメラ１６−１からの距離を出力して、一連の処理を終了する。
【００３０】
以上、説明した内容につき、以下でさらに詳細に説明する。
以下では、対象物の例として、図５に示すような直方体状の物体が用いられる。
２次元画像と姿勢により構成される辞書データの作成方法としては、上述したように２通りある。第１の方法では、図６に示すように、まず、カメラを所定の位置に固定する。次に、対象物の予め定められた点（対象物中心点）と、３次元直交座標系の原点とを一致させる。さらに、上記した対象物回転装置１１により、その３次元直交座標系の原点（対象物中心点）の回りに対象物を回転させることにより、その対象物の姿勢を全方位へ等間隔で変化させ、その各姿勢の対象物を固定カメラで撮影して、それぞれの姿勢情報と２次元画像とを取得し、辞書データを作成する。この第１の方法では、対象物をｘ、ｙ、ｚの各軸方向に所定量ずつ回転させる上述の対象物回転装置１１が必要となる。
【００３１】
これに対し、第２の方法では、図７に示すように、対象物の予め定められた点（対象物中心点）と、３次元直交座標系の原点とを一致させ、その位置に対象物を固定する。そして、対象物中心点（原点）を中心とした球面上で、上記したカメラ移動装置により、等間隔にカメラを移動させて、それぞれのカメラ位置情報と２次元画像とを取得し、辞書データを作成する。この第２の方法では、直交座標系の原点（対象物中心点）を中心とした球面上で所定の位置へカメラ３を移動させる上述のカメラ移動装置１２が必要となる。
【００３２】
なお、上記第１の方法と第２の方法とは、対象物とカメラとのいずれを固定するかによる差異であり、得られる辞書データは等しい。
ここでは、上記第１の方法についてさらに詳細に説明する。
【００３３】
まず、カメラ３の位置を固定する。そして、対象物がない状態で撮影を行ない、撮影した画像上で３次元直交座標系の原点に対応する位置を設定することで、辞書データを作成するための対象物を回転させる３次元直交座標系を設定する。ここでは、簡単のために、図８に示すように、撮影した画像２５の画像中心点２６に原点が対応するように３次元直交座標系を設定する。
【００３４】
カメラ３は、任意の位置に固定可能であるが、固定する位置を決めた後は辞書データ作成のための撮影中はその位置を移動することはできない。
次に、対象物の予め定められた点（例えば重心）が３次元直交座標系の原点と一致するように、対象物を配置する。なお、この対象物の予め定められた点は、対象物を回転させるときの中心でもあるので、回転中心点とも呼ばれる。
【００３５】
対象物の姿勢は、図９に示すように、ｘ軸方向への回転角度θｘ、ｙ軸方向への回転角度θｙ、ｚ軸方向への回転角度θｚを指定することにより規定できる。これら角度θｘ、
θｙ、θｚを一定間隔で刻んで対象物の姿勢を変化させて、条件ごとに、固定カメラ３から対象物を撮影し、撮影した画像と各角度値とをその都度保存することにより、辞書データが作成される。
【００３６】
ｉ番目の辞書データ“辞書（ｉ）”は数式３のように表すことができる。
［数３］
辞書（ｉ） ＝ （画像（ｉ）、θｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ））
例えば角度を１度刻みとすると、辞書データの要素総数は、３６０×３６０×３６０となる。すなわち、上記インデックスｉは、１≦ｉ≦３６０×３６０×３６０、の範囲で変化する。
【００３７】
図１０に、辞書データに登録される画像の例を示す。各画像において、対象物中心点２７の座標は３次元座標系の原点に対応している。なお、この例では、３次元座標系の原点は画像中心点に対応している。
【００３８】
続いて、３次元物体（認識対象）の姿勢・距離検出処理について説明する。
認識対象をキャリブレーションされた２眼ステレオカメラで撮影して得た２次元画像の例を図１１に示す。
【００３９】
左の２次元画像３１および右の２次元画像３２のそれぞれに対して、辞書データの２次元画像との間でパターンマッチ処理を行なう。パターンマッチは、認識対象と辞書データ中の対象物とのサイズが異なる場合にも適用できるＳＩＦＴ演算子（Scale Invariant Feature Transform）などの公知の技術を用いて行なう。なお、ＳＩＦＴ演算子については例えば、D. Loweによる”Distinctive image features from scale-invariant keypoints” （ International Journal of Computer Vision, 60, 2(2004), pp91-110）に記載がある。
【００４０】
辞書データの各２次元画像は、画像の中心点に対象物の予め定められた点が対応しているが、撮影した図１１に示す左右の２次元画像３１、３２においては、通常、画像の中心点に認識対象（対象物）の予め定められた点が対応していない。
【００４１】
マッチング処理の結果として、図１２に示すように、左画像（左の２次元画像）３１に対し、対象物中心点３３に相当する座標（ｘｌ、ｙｌ）および、最も類似する辞書データの番号Ｄ（ｌ）、右画像（右の２次元画像）３２に対し、対象物中心点３４に相当する座標（ｘｒ、ｙｒ）および、最も類似する辞書データの番号Ｄ（ｒ）、が求まる。
【００４２】
このとき、左右画像の座標（ｘｌ、ｙｌ）および（ｘｒ、ｙｒ）は対象物の同一点に対応しており、ステレオ法における対応点となる。よって、ステレオビジョンにおける公知の手法を用いることにより、例えば左カメラを基準とした場合には、左カメラから中心点までの距離ｄｌを算出することができる。
【００４３】
結果として、左カメラを基準として、対象物の姿勢＝（θｘ（ｉ）、θｙ（ｉ）、θｚ（ｉ））、対象物中心点までの距離＝ｄｌとして、対象物（認識対象）の姿勢および距離を認識することができる。
【００４４】
このように、本実施形態によれば、対象物の２次元画像と、その画像における対象物の姿勢と、その画像における対象物中心点位置とを組み合わせて作成した辞書データを有するために、事前に参照点や図形を辞書データの各２次元画像に入力することなく、また、ステレオカメラを用いることにより、認識対象の３次元姿勢および対象物中心点までの距離を容易に認識することができる。
【００４５】
続いて、本実施形態の変形例について説明する。
この変形例においては、図３の２次元マッチング処理部２２によるパターンマッチ処理に対し、両カメラの位置関係とパターンマッチ結果とが一致するように、また、パターンマッチの計算量が減るように改良を加えている。
【００４６】
以下に、この変形例について説明する。
上述したように、ステレオカメラはキャリブレーションされているため、Ｆ行列が求められている。Ｆ行列を両カメラ間の剛体変換（一方が他方に対しどれだけ回転しどれだけ平行移動した位置にいるか）の関係に分解できることは例えば、Richard HartleyとAndrew Zissermanによる”Multiple View Geometry in Computer Vision, Cambridge University Press”（Second Edition(2003), pp239-261）に示されるように公知の技術である。
【００４７】
そこで、ステレオカメラにより撮影された認識対象（対象物）の２つの２次元画像に対して、辞書データの各２次元画像とパターンマッチ処理を行なう場合において、一方の２次元画像に対して順次パターンマッチを行なうとともに、対応する（両カメラ間の）回転角に該当する近傍の辞書データ（すなわち、キャリブレーションにて定義された回転角度近傍の辞書データ）の２次元画像を用いて他方の２次元画像のパターンマッチを行ない、両画像がよりよく類似する辞書データを、パターンマッチ処理の結果として出力する。また、両画像がよりよく類似する辞書データを検索する方法としては、例えば、２つのパターンマッチ結果（すなわち、上記した尺度値）の加算値が最小である辞書データの各２次元画像をパターンマッチ処理の結果として採用する方法がある。
【００４８】
すなわち、撮影したそれぞれの２次元画像に対して、辞書データの各２次元画像とパターンマッチ処理を行なう場合において、一方の２次元画像と辞書番号ｉの２次元画像との間でパターンマッチを行ない、第１のパターンマッチ結果を算出するとともに、他方の２次元画像と辞書番号ｊの近傍の２次元画像との間でパターンマッチを行ない、第２のパターンマッチ結果を算出し、第１および第２のパターンマッチ結果の加算値が最小となる辞書データの２次元画像の組み合わせを姿勢として推定する。
【００４９】
なお、辞書番号ｊの２次元画像は、辞書番号ｉの２次元画像における対象物の姿勢をＲだけ回転させた姿勢である対象物の２次元画像とする。Ｒは、ステレオキャリブレーションにより定義されるカメラ間の回転関係を示す。なお、カメラ間の関係は、Ｆ行列によって定義されるが、これは、回転と平行移動の関係である。
【００５０】
図１３は、カメラ間の回転関係を考慮して、第１の２次元画像と対応する第２の２次元画像とに対するそれぞれの辞書データをパターンマッチ結果として求める処理のフローチャートである。
【００５１】
図１３のステップＳ２１において、辞書番号ｉおよび最小加算値を初期化する。なお、例えば辞書を各軸方向に１度刻みで作成した場合、辞書番号ｉは、１〜３６０×３６０×３６０の範囲をとることになる。別の表現をすると、辞書番号（ｉｒ１，ｉｒ２，ｉｒ３），ｉｒ１＝０〜３５９，ｉｒ２＝０〜３５９，ｉｒ３＝０〜３５９と表すことができる。本フローチャートにおいて、辞書番号ｉと辞書番号（ｉｒ１，ｉｒ２，ｉｒ３）の対応関係は既知であるものとする。また、最小加算値の初期化においては、最小加算値として記憶できる最大の値を入力すればよい。
【００５２】
ステップＳ２１に続く、ステップＳ２２では、カメラ間の回転関係を考慮して、辞書番号ｉに対する辞書番号ｊを求める。例えば、左画像（このフローチャート中では、“第１の２次元画像”として表記）ＸＬｐｑと辞書Ｘ（ｋｌ１，ｋｌ２，ｋｌ３）ｐｑが類似し
ていて、右画像（このフローチャート中では、“第２の２次元画像”として表記）ＸＲｐｑと辞書Ｘ（ｋｒ１，ｋｒ２，ｋｒ３）ｐｑが類似していて、辞書番号（ｋｌ１，ｋｌ２，ｋｌ３）と（ｋｒ１，ｋｒ２，ｋｒ３）が角度Ｒの回転関係にあり、角度Ｒが３次元座標系でＸ，Ｙ，Ｚの各軸角度が（ｒ１，ｒ２，ｒ３）と表されるとき、左画像ＸＬｐｑを基準とすると、数式４を満たすような右画像とパターンマッチさせる辞書Ｘ（ｋｌ１，ｋｌ２，ｋｌ３）ｐｑを、このステップＳ２２で求めることになる。
［数４］
ｋｒ１＝ｋｌ１＋ｒ１
ｋｒ２＝ｋｌ２＋ｒ２
ｋｒ３＝ｋｌ３＋ｒ３
ステップＳ２２に続くステップＳ２３では、第１の２次元画像（左画像）と辞書番号ｉの２次元画像のパターンマッチを行ない、第１のパターンマッチ結果（第１の尺度値）を求める。続く、ステップＳ２４では、第２の２次元画像（右画像）と辞書番号ｊの２次元画像のパターンマッチを行ない、第２のパターンマッチ結果（第２の尺度値）を求める。
【００５３】
ステップＳ２４に続くステップＳ２５では、第１のパターンマッチ結果（第１の尺度値）と、第２のパターンマッチ結果（第２の尺度値）とを加算し、加算値を求める。
続くステップＳ２６では、ステップＳ２５で求めた加算値が最小加算値より小さいか否かが判定される。
【００５４】
ステップＳ２６においてステップＳ２５で求めた加算値が最小加算値以上であると判定された場合（ステップＳ２６の判定結果がＮｏの場合）、ステップＳ２９に進む。
ステップＳ２６においてステップＳ２５で求めた加算値が最小加算値より小さいと判定された場合（ステップＳ２６の判定結果がＹｅｓの場合）、ステップＳ２７において、ステップＳ２５で求めた加算値を最小加算値に設定し、ステップＳ２８において、パターンマッチ処理の結果として、左右の各画像のパターンマッチ結果（尺度値）および辞書番号を記憶し、ステップＳ２９に進む。
【００５５】
ステップＳ２６またはステップＳ２８から制御を渡されたステップＳ２９では、辞書番号ｉが最大値であるかどうかを判定する。
ステップＳ２９において辞書番号ｉが最大値であると判定された場合（ステップＳ２９の判定結果がＹｅｓの場合）、一連の処理を終了する。
【００５６】
ステップＳ２９において辞書番号ｉが最大値ではないと判定された場合（ステップＳ２９の判定結果がＮｏの場合）、ステップＳ３０において辞書番号ｉをインクリメントし、ステップＳ２２に戻り、辞書データを使用したパターンマッチ処理を続行する。
【００５７】
このような一連の処理の結果として、例えば尺度値としてＳＳＤを使用した場合、ｋｒ１＝ｋｌ１＋ｒ１、ｋｒ２＝ｋｌ２＋ｒ２、ｋｒ３＝ｋｌ３＋ｒ３、という条件の下で、数式５を最小化するＸ（ｋｒ１，ｋｒ２，ｋｒ３）が検索されたことになる。
［数５］
ΣｍΣｎ（ＸＬｍｎ−Ｘ（ｋｌ１，ｋｌ２，ｋｌ３）ｍｎ）２
＋ΣｍΣｎ（ＸＲｍｎ−Ｘ（ｋｒ１，ｋｒ２，ｋｒ３）ｍｎ）２
図１３で説明した上記の場合においてさらに、一方の２次元画像に対するパターンマッチ処理により、画像上で対象物中心点の位置が求まると、他方の２次元画像の対象物中心点位置が対応するエピポーラ線上にあるものとして、そのエピポーラ線上に限定して他方の２次元画像に対するパターンマッチを行なうようにしてもよい。
【００５８】
なお、他方の２次元画像上に一方の２次元画像を撮影したカメラの視線（光軸）を引くことで、他方の２次元画像上にエピポーラ線を引くことができる。
このように、本実施形態の変形例によれば、領域を限定されたパターンにおいてパターンマッチを行なうため、計算量が削減されるとともに、誤分類の確率も減少する。
【図面の簡単な説明】
【００５９】
【図１】本発明の一実施形態に係る辞書データ作成装置の構成を示すブロック図である。
【図２】辞書データ作成処理のフローチャートである。
【図３】本発明の一実施形態に係る３次元物体姿勢・距離検出装置の構成を示すブロック図である。
【図４】姿勢・距離認識処理のフローチャートである。
【図５】対象物の例を示す図である。
【図６】辞書データ作成方法（その１）を示す図である。
【図７】辞書データ作成方法（その２）を示す図である。
【図８】辞書データの作成に先立って設定された座標系を示す図である。
【図９】辞書データの作成する際の撮影方法を示す図である。
【図１０】辞書データに登録される画像の例を示した図である。
【図１１】認識対象をキャリブレーションされた２眼ステレオカメラで撮影して得た２次元画像の例を示した図である。
【図１２】図１１の２次元画像の例に対象物中心点を追加した図である。
【図１３】カメラ間の位置関係を考慮して、第１の２次元画像と対応する第２の２次元画像とに対するそれぞれの辞書データをパターンマッチ結果として求める処理のフローチャートである。
【符号の説明】
【００６０】
１ システム制御機器
２ 対象物
３、１６−１、１６−２ カメラ
５ 対象物回転制御部
６ カメラ移動制御部
７、２１ 撮影制御部
８ 記憶装置
１１ 対象物回転装置
１２ カメラ移動装置
１８ 認識対象
２２ ２次元マッチング処理部
２３ 距離算出部
２５ 撮影した画像
２６ 画像中心点
２７、３３、３４ 対象物中心点
３１ 左の２次元画像
３２ 右の２次元画像
【特許請求の範囲】
【請求項１】
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせ、または、
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶する記憶部と、
カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成され、前記対象物を撮影するステレオカメラと、
前記ステレオカメラを構成する第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識するとともに、前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出する姿勢・距離認識部と、
を有することを特徴とする３次元物体姿勢・位置検出装置。
【請求項２】
前記姿勢・距離認識部は、ステレオカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なう場合において、一方の２次元画像に対して順次パターンマッチを行なうとともに、一方の２次元画像を撮影したカメラと他方の２次元画像を撮影したカメラとの間の回転に該当する近傍の辞書データを用いて他方の２次元画像のパターンマッチを行ない、撮影した双方の２次元画像が最も類似する前記辞書データの各２次元画像の組み合わせを検索することを特徴とする請求項１記載の３次元物体姿勢・位置検出装置。
【請求項３】
前記姿勢・距離認識部はパターンマッチを行なう場合にさらに、一方の２次元画像に対するパターンマッチ結果により、画像上でマッチした位置から対象物中心点を求め、その対象物中心点に対応するエピポーラ線上に限定して他方の２次元画像に対するパターンマッチを行ない、２つのパターンマッチによる尺度値の加算値が最も類似している値となる辞書データの各２次元画像の組み合わせを検索することを特徴とする請求項２記載の３次元物体姿勢・位置検出装置。
【請求項４】
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせ、または、
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶する記憶部と、
カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成され、前記対象物を撮影するステレオカメラと、を有する装置が実行する３次元物体の姿勢・位置検出方法において、
前記ステレオカメラを構成する第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識するステップと、
前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出するステップと、を有することを特徴とする３次元物体姿勢・位置検出方法。
【請求項５】
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物
中心点の位置との組み合わせ、または、
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶した記憶部を有するコンピュータを、
ステレオカメラを構成する、カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識する姿勢認識部、
前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出する距離算出部、として機能させる３次元物体姿勢・位置検出プログラム。
【請求項１】
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせ、または、
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶する記憶部と、
カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成され、前記対象物を撮影するステレオカメラと、
前記ステレオカメラを構成する第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識するとともに、前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出する姿勢・距離認識部と、
を有することを特徴とする３次元物体姿勢・位置検出装置。
【請求項２】
前記姿勢・距離認識部は、ステレオカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なう場合において、一方の２次元画像に対して順次パターンマッチを行なうとともに、一方の２次元画像を撮影したカメラと他方の２次元画像を撮影したカメラとの間の回転に該当する近傍の辞書データを用いて他方の２次元画像のパターンマッチを行ない、撮影した双方の２次元画像が最も類似する前記辞書データの各２次元画像の組み合わせを検索することを特徴とする請求項１記載の３次元物体姿勢・位置検出装置。
【請求項３】
前記姿勢・距離認識部はパターンマッチを行なう場合にさらに、一方の２次元画像に対するパターンマッチ結果により、画像上でマッチした位置から対象物中心点を求め、その対象物中心点に対応するエピポーラ線上に限定して他方の２次元画像に対するパターンマッチを行ない、２つのパターンマッチによる尺度値の加算値が最も類似している値となる辞書データの各２次元画像の組み合わせを検索することを特徴とする請求項２記載の３次元物体姿勢・位置検出装置。
【請求項４】
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせ、または、
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶する記憶部と、
カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラにより構成され、前記対象物を撮影するステレオカメラと、を有する装置が実行する３次元物体の姿勢・位置検出方法において、
前記ステレオカメラを構成する第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識するステップと、
前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出するステップと、を有することを特徴とする３次元物体姿勢・位置検出方法。
【請求項５】
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った視点から撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物
中心点の位置との組み合わせ、または、
３次元位置・姿勢を検出する対象物に対し、全方位を等間隔に区切った位置へ対象物を回転させて撮影した２次元画像と、その２次元画像に対応する対象物の姿勢と、その２次元画像内の対象物中心点の位置との組み合わせである辞書データを記憶した記憶部を有するコンピュータを、
ステレオカメラを構成する、カメラ間の回転関係が既知であるキャリブレーションされた第１および第２のカメラで撮影したそれぞれの２次元画像に対して、前記辞書データの２次元画像とパターンマッチを行なうことにより前記対象物の姿勢を認識する姿勢認識部、
前記それぞれの２次元画像に対して、対象物中心点の位置を前記辞書データに基づき検出し、これらを対応点とみなしてステレオ法により前記対象物中心点までの距離を算出する距離算出部、として機能させる３次元物体姿勢・位置検出プログラム。
【図２】
【図４】
【図８】
【図１３】
【図１】
【図３】
【図５】
【図６】
【図７】
【図９】
【図１０】
【図１１】
【図１２】
【図４】
【図８】
【図１３】
【図１】
【図３】
【図５】
【図６】
【図７】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１０−１４６３０３（Ｐ２０１０−１４６３０３Ａ）
【公開日】平成２２年７月１日（２０１０．７．１）
【国際特許分類】
【出願番号】特願２００８−３２２９９０（Ｐ２００８−３２２９９０）
【出願日】平成２０年１２月１８日（２００８．１２．１８）
【出願人】（０００００５２３４）富士電機ホールディングス株式会社 (3,146)
【Ｆターム（参考）】
【公開日】平成２２年７月１日（２０１０．７．１）
【国際特許分類】
【出願日】平成２０年１２月１８日（２００８．１２．１８）
【出願人】（０００００５２３４）富士電機ホールディングス株式会社 (3,146)
【Ｆターム（参考）】
[ Back to top ]