JP2015056057A - 姿勢推定方法及びロボット - Google Patents

Abstract

【課題】低コストであり、かつ、精度も確保することができる姿勢推定方法及びロボットを提供すること。【解決手段】本発明にかかる画像認識方法は、カメラ１０（三次元センサ２０）を用いて被写体を撮像することにより生成されたカメラ画像を取得する。カメラ画像内の一定領域に含まれる複数の画素に対応する複数の座標を取得する。複数の画素における被写体からカメラ１０までの距離を示す被写体距離情報を取得する。そして、取得した複数の座標及び複数の被写体距離情報に基づいて、一定領域に含まれる被写体が有する被写体面の姿勢を推定する。【選択図】図６

Description

本発明は姿勢推定方法及びロボットに関する。

周囲の環境に基づいて動作するロボットが提案されている。このようなロボットは、ロボットが活動する環境に存在する様々な平面を認識し、歩行動作、物体の把持動作、及び物体の載置動作等を行う。

例えば、特許文献１には、ステレオカメラを用いた平面推定方法が開示されている。まず、ステレオ画像を撮像し、ステレオ画像のうちの基準画像について複数の特徴点を抽出する。抽出した各特徴点について他の画像中の対応点を探索して得られる視差から三角測量の原理により３次元座標を求める。抽出した各特徴点位置の画像に類似する画像を物体の移動前後の画像から検出し、抽出した各特徴点の３次元的な移動ベクトルから平面の３次元的位置を算出する。

特開２００６−１０５６６１号公報

特許文献１に開示された方法を用いるためには、２枚の画像の視差に関する情報が必要となるため、ステレオカメラを使用しなければならない。しかしながら、ステレオカメラは、単眼カメラに比べて高価なカメラ構成である。そのため、センサ（カメラ）コストを抑えることが難しいという問題がある。なお、単眼カメラのみを用いた姿勢推定方法も提案されているが、推定精度が十分ではない。

本発明は、このような問題を解決するためになされたものであり、低コストであり、かつ、精度も確保することができる姿勢推定方法及びロボットを提供することを目的としている。

本発明の一態様にかかる姿勢推定方法は、撮像装置を用いて被写体を撮像することにより生成された撮像画像を取得し、前記撮像画像内の一定領域に含まれる複数の画素に対応する複数の座標を取得し、前記複数の画素における前記被写体から前記撮像装置までの距離を示す被写体距離情報を取得し、取得した複数の前記座標及び複数の前記被写体距離情報に基づいて、前記一定領域に含まれる前記被写体が有する被写体面の姿勢を推定するものである。これにより、ステレオカメラを用いることなく、低コストで被写体面の姿勢を推定できる。また、平面の座標だけでなく、被写体距離の情報も用いて推定しているため、推定精度も確保できる。

また、各画素が前記被写体距離情報を有する距離画像を取得し、前記撮像画像内の画素と、前記距離画像内の画素と、を対応付け、前記距離画像内の画素のうち、前記一定領域内の前記複数の画素に対応する画素から、前記被写体距離情報を取得してもよい。

また、前記複数の画素の座標及び前記被写体距離情報に基づいて、前記複数の画素の三次元座標を算出し、前記複数の画素の三次元座標に基づいて、前記一定領域に含まれる前記被写体面の姿勢を推定してもよい。

また、前記被写体面にはマーカが付されており、前記撮像画像内における前記マーカを含むマーカ領域を前記一定領域として検出し、検出した前記マーカ領域に含まれる前記マーカの姿勢を推定してもよい。

また、前記複数の画素の前記座標及び前記被写体距離情報を用いて、前記被写体面と平行な投影面の方程式を算出し、前記撮像画像における前記マーカの姿勢を示す特徴点を前記投影面に投影し、前記投影面に投影された前記特徴点の座標に基づいて、前記マーカの姿勢を推定してもよい。

また、前記撮像画像内の前記特徴点の座標をサブピクセル精度で特定し、特定した前記特徴点の座標を用いて前記投影面への投影を行ってもよい。

また、前記投影面に投影された前記特徴点の座標に基づいて、前記マーカの位置を推定し、推定した前記マーカの姿勢、推定した前記マーカの位置、及び予め設定された前記マーカのサイズに関する情報を用いて、前記投影面における前記特徴点の座標を算出し、前記投影面において算出した前記特徴点を、前記撮像画像に投影し、前記撮像画像において、撮像時における前記特徴点の座標と、投影された前記特徴点の座標と、を比較し、比較結果に基づいて、推定精度を判断してもよい。

また、前記投影面に投影された前記特徴点の座標に基づいて、前記マーカの位置を推定し、推定した前記マーカの姿勢、推定した前記マーカの位置、及び予め設定された前記マーカのサイズに関する情報を用いて、前記投影面における前記特徴点の座標を算出し、前記投影面において、算出した前記特徴点の座標と、前記マーカの姿勢を推定するときに前記撮像画像から投影された前記特徴点の座標と、を比較し、比較結果に基づいて、推定精度を判断してもよい。

また、前記マーカは略矩形状であり、前記撮像画像における前記マーカの頂点を前記特徴点として検出し、前記検出した前記特徴点の個数が２つまたは３つの場合、検出した前記特徴点から延びる前記マーカの辺を延長し、延長した前記辺が交わった交点を前記特徴点として推定してもよい。

また、前記マーカは略矩形状であり、前記撮像画像における前記マーカの頂点を前記特徴点として検出し、前記検出した前記特徴点の個数が４つ未満の場合、検出した前記特徴点から延びる前記マーカの辺を延長し、延長した辺の上の点であって、検出した前記特徴点から予め設定された距離だけ離れた点を前記特徴点として推定してもよい。

本発明の一態様にかかるロボットは、前記撮像装置と、前記被写体距離情報を取得する距離センサと、上記の姿勢推定方法を実行する姿勢推定装置と、を備えるものである。

本発明により、低コストであり、かつ、精度も確保することができる姿勢推定方法及びロボットを提供することができる。

実施の形態１にかかる姿勢推定システムのブロック図である。 カメラ画像の一例を示す図である。 距離画像の一例を示す図である。 カメラ画像と距離画像との画素の対応付けを説明するための図である。 カメラ画像と距離画像との画素の対応付けを説明するための図である。 実施の形態１にかかる姿勢推定システムの動作を示すフローチャートである。 実施の形態１にかかるマーカＩＤの読み取り動作を説明するための図である。 実施の形態１にかかるマーカ領域の切り出し動作を説明するための図である。 実施の形態１にかかるマーカが存在する平面の推定動作を説明するための図である。 実施の形態１にかかるマーカの位置及び姿勢の推定動作を説明するための図である。 実施の形態２にかかる姿勢推定システムのブロック図である。 実施の形態２にかかる姿勢推定システムの動作を示すフローチャートである。 実施の形態２にかかるマーカの位置及び姿勢の推定動作を説明するための図である。 実施の形態２にかかる推定精度の評価方法を説明するための図である。 実施の形態２にかかる推定精度の評価方法を説明するための図である。 変形例にかかるカメラ画像の一例を示す図である。 変形例にかかるマーカ領域の切り出し動作を説明するための図である。 変形例にかかるマーカが存在する円柱の推定動作を説明するための図である。 変形例にかかるマーカの位置及び姿勢の推定動作を説明するための図である。 変形例にかかるマーカの位置及び姿勢の推定動作を説明するための図である。 実施の形態３にかかる一部が隠れたマーカを示す図である。 実施の形態３にかかる隠れた特徴点の推定動作を説明するための図である。 実施の形態３にかかるマーカの位置の推定動作を説明するための図である。 実施の形態３にかかるマーカの姿勢の推定動作を説明するための図である。 実施の形態３にかかる一部が隠れたマーカを示す図である。 実施の形態３にかかる隠れた特徴点の推定動作を説明するための図である。 実施の形態３にかかるマーカの位置の推定動作を説明するための図である。 実施の形態３にかかるマーカの姿勢の推定動作を説明するための図である。 実施の形態３にかかる一部が隠れたマーカを示す図である。 実施の形態３にかかる隠れた特徴点の推定動作を説明するための図である。 実施の形態３にかかるマーカの位置の推定動作を説明するための図である。 実施の形態３にかかるマーカの姿勢の推定動作を説明するための図である。 実施の形態３にかかる一部が隠れたマーカを示す図である。 実施の形態３にかかる隠れた特徴点の推定動作を説明するための図である。 実施の形態３にかかるマーカの位置の推定動作を説明するための図である。 実施の形態３にかかるマーカの姿勢の推定動作を説明するための図である。

＜実施の形態１＞
以下、図面を参照して本発明の実施の形態について説明する。本実施の形態にかかる姿勢推定方法は、カメラを用いて撮像されたカメラ画像内に存在するマーカの位置及び姿勢を推定する。

＜姿勢推定システムの構成＞
図１に本実施の形態にかかる画像処理システムのブロック図を示す。画像処理システムは、カメラ１０と、三次元センサ２０と、姿勢推定装置３０と、を備える。

カメラ１０（撮像装置）は、図示しないレンズ群やイメージセンサ等を有する。カメラ１０は、撮像処理を行い、カメラ画像（撮像画像）を生成する。カメラ画像においては、各画素の位置は二次元座標（ｘ，ｙ）を用いて示される。また、カメラ画像とは、例えば、図２に示すような画像であり、各画素がＲＧＢの値（色彩情報）や輝度値等を有する。なお，カメラ１０は、単眼カメラである。

三次元センサ２０は、撮像処理を行い、距離画像を生成する。具体的には、三次元センサ２０は、カメラ１０の画角に対応する画角において、カメラ１０（または三次元センサ２０）から被写体までの距離を示す情報（被写体距離情報）を取得する。より詳細には、三次元センサ２０は、カメラ１０の近傍に配置されており、三次元センサ２０から被写体までの距離を被写体距離情報として取得する。そして、三次元センサ２０は、被写体距離情報を用いて、距離画像を生成する。距離画像においては、各画素の位置は二次元座標を用いて示される。また、距離画像においては、各画素が写体距離情報を有する。つまり、距離画像は、被写体の奥行きに関する情報を含む画像である。例えば、図３に示すように、距離画像は、グレースケールの画像であり、被写体距離情報に応じて画素の色の濃淡が変化する。なお、三次元センサとして、例えば、ＴＯＦ（Time Of Flight）方式のカメラやステレオカメラ等を用いることができる。

姿勢推定装置３０は、制御部３１と、マーカ認識部３２と、平面推定部３３と、を備える。制御部３１は、ＣＰＵ（Central Processing Unit）、各種プログラムが格納されたＲＯＭ（Read Only Memory）、及びワークエリアとしてのＲＡＭ（Random Access Memory）等を含む半導体集積回路により構成される。制御部３１は、姿勢推定装置３０の各ブロックに対して指示し、姿勢推定装置３０全体の処理を統括的に制御する。

マーカ認識部３２は、カメラ画像内からマーカ領域（一定領域）を検出する。マーカ領域とは、カメラ画像、距離画像、及び、後述する推定された平面Ｆ内の一部の領域であり、マーカを含む領域である。つまり、マーカ領域の位置、姿勢及び姿勢は、被写体であるマーカの位置、姿勢及び形状に対応している。マーカ認識部３２は、検出したマーカが有するＩＤ（識別情報）を読み取る。マーカのＩＤは、例えば、バーコードや二次元コード等の形式で被写体に添付されている。つまり、マーカとは被写体の個体や種類等を識別するための標識である。また、マーカ認識部３２は、カメラ画像におけるマーカ領域の位置情報を取得する。マーカ領域の位置情報は、例えば、ｘｙ座標を用いて示される。なお、本実施の形態においては、マーカの形状は略矩形状であるものとする。

平面推定部３３は、距離画像に基づいて、被写体が有する被写体面に付されたマーカの位置及び姿勢を推定する。具体的には、平面推定部３３は、カメラ画像内のマーカ領域に対応する距離画像内の領域を、距離画像から切り出す。平面推定部３３は、距離画像において切り出したマーカ領域に含まれる複数の画素の座標（二次元座標）を取得する。また、平面推定部３３は、複数の画素が有する被写体距離情報を距離画像から取得する。そして、平面推定部３３は、距離画像における複数の画素の座標及び被写体距離情報に基づいて、当該複数の画素の三次元座標を取得し、マーカ領域に含まれるマーカの位置及び姿勢を推定する。

このとき、カメラ１０と三次元センサ２０とは、互いに近接して配置されているものの、同一の位置ではない。そのため、カメラ画像の画角と距離画像の画角との間には、若干のずれが生じている。つまり、それぞれの画像における同じ被写体の同じ点の画素の位置（座標）は、異なっている。しかしながら、カメラ１０と三次元センサ２０との間の間隔は予め測定可能である。そのため、制御部３１は、その間隔の分だけ、いずれか一方の画像における画素の座標をずらして、カメラ画像の各画素と距離画像の各画素とを対応付けることができる。これにより、カメラ画像及び距離画像において、同じ被写体の同じ点の画素が対応付けられる（キャリブレーション）。

なお、カメラ１０と三次元センサ２０の内部パラメータ（焦点距離、画像の原点（中心）位置、ひずみ中心、アスペクト比等）が同一の場合、上記のように、距離画像の画素の座標を基準として、カメラ画像の画素の座標をずらして対応付けるだけでよい（図４参照）。一方、カメラ１０と三次元センサ２０との内部パラメータが異なる場合、内部パラメータに基づいて、距離画像の各画素をカメラ画像の各画素に投影することにより、カメラ画像と距離画像との座標の対応付けを行う（図５参照）。図５に示すように、距離画像における星印の座標に対応するカメラ画像の座標を、カメラ１０及び三次元センサ２０の内部パラメータに基づいて算出する。なお、２台のカメラの内部パラメータが異なる場合のキャリブレーション手法は種々の方法が提案されており、既存の技術を用いることができる。そのため、キャリブレーション手法の詳細な説明は省略する。

＜姿勢推定システムの動作＞
続いて、本実施の形態にかかる姿勢推定方法について、図６に示すフローチャートを参照して説明する。

まず、カメラ１０及び三次元センサ２０は、被写体を撮像する。これにより、カメラ１０は、カメラ画像を生成する。また、三次元センサ２０は、距離画像を生成する。姿勢推定装置３０は、生成されたカメラ画像及び距離画像を取得する（ステップＳ１０１）。

マーカ認識部３２は、カメラ画像からマーカ領域を検出する（ステップＳ１０２）。マーカ認識部３２は、マーカの形状に基づいて、マーカ領域の検出を行う。本実施の形態においては、マーカ認識部３２は、マーカが略矩形状であることを事前に記憶しているため、カメラ画像内の矩形状の領域を検出する。そして、矩形状の内部に読取可能なマーカが存在する場合、マーカ認識部３２は、当該矩形状の領域をマーカ領域として検出する。

次に、マーカ認識部３２は、検出したマーカのＩＤを読み取る（ステップＳ１０３）。本実施の形態においては、図７に示すマーカＭを検出したとする。マーカ認識部３２は、マーカＭを読み取り、マーカのＩＤとして「１３」を取得する。このように、マーカ認識部３２は、マーカＩＤを取得し、マーカが付された物体の個体を識別する。

次に、平面推定部３３は、カメラ画像内のマーカ領域に対応する距離画像内の領域を、距離画像から切り出す（ステップＳ１０４）。具体的には、図８に示すように、平面推定部３３は、マーカ認識部３２からカメラ画像におけるマーカ領域Ｍｃ（斜線領域）の位置情報を取得する。カメラ画像の各画素の座標と、距離画像の各画素の座標と、は予め対応付けられているため、平面推定部３３は、カメラ画像内のマーカ領域の位置に対応する領域を、距離画像からマーカ領域Ｍｄ（斜線領域）として切り出す。

平面推定部３３は、距離画像において切り出したマーカ領域Ｍｄに含まれる複数の画素の被写体距離情報を取得する。また、平面推定部３３は、被写体距離情報を取得した複数の画素に対して、二次元座標（ｘ，ｙ）を取得する。そして、平面推定部３３は、これらの情報を合わせて、それぞれの画素についての三次元座標（ｘ，ｙ，ｚ）を取得する。これにより、マーカ領域Ｍｄの各画素の位置を、三次元座標を用いて表現することができる。このように、各点の座標が三次元座標を用いて表現されたマーカ領域をマーカ領域Ｍｅとする。

平面推定部３３は、マーカ領域Ｍｅに対して最適な平面を推定する（ステップＳ１０５）。具体的には、図９に示すように、平面推定部３３は、マーカ領域Ｍｅに含まれる複数の画素の三次元座標を用いて、最適な平面Ｆの方程式を推定する。なお、マーカ領域Ｍｅに対して最適な平面Ｆとは、マーカ領域Ｍｅと平行な平面であり、マーカ領域Ｍｅを含む平面である。このとき、１つの平面においては、３つ以上の点の三次元座標が決まれば、平面の方程式は一意に決まる。そのため、平面推定部３３は、マーカ領域Ｍｅに含まれる複数（３つ以上）の画素の三次元座標を用いて、平面の方程式を推定する。これにより、マーカ領域Ｍｅを含む平面の方程式、つまり、平面の向きを推定することができる。つまり、マーカの向き（姿勢）を推定することができる。

なお、平面の方程式は、以下の式（１）を用いて示される。Ａ、Ｂ、Ｃ、Ｄは定数パラメータであり、ｘ、ｙ、ｚは、変数（三次元座標）である。最適な平面の方程式の推定は、例えば、ＲＡＮＳＡＣ法（RANdom SAmple Consensus）を用いることができる。ＲＡＮＳＡＣ法とは、無作為に抽出したデータ集合（マーカ領域Ｍｅ内の複数の三次元座標）を用いて、パラメータ（式（１）のＡ、Ｂ、Ｃ、Ｄ）を推定する方法であり、広く知られている手法である。そのため、ＲＡＮＳＡＣ法に関する詳細な説明は省略する。

さらに、平面推定部３３は、マーカの位置及び姿勢を推定する（ステップＳ１０６）。図１０に示すように、平面推定部３３は、推定した平面Ｆにおけるマーカ領域Ｍｅの四隅の画素（Ｘ０、Ｘ１、Ｘ２、Ｘ３）の三次元座標を取得する（なお、Ｘ０＝（ｘ０，ｙ０，ｚ０）。Ｘ１〜Ｘ３も同様）。なお、以下の説明においては、マーカ領域の四隅の画素（マーカの４つの頂点）のことを特徴点と称す。特徴点とは、マーカの位置及び姿勢を示す点である。マーカ領域の四隅の画素の三次元座標が特定できれば、マーカの位置及び姿勢も特定できるため、マーカ領域の四隅の点は特徴点となる。勿論、特徴点とは、マーカ領域の四隅の画素に限られない。

なお、マーカ領域Ｍｅの特徴点を求める方法として、マーカ領域Ｍｅの各辺の式を推定し、各辺の交点を特徴点として推定してもよい。例えば、平面推定部３３は、マーカ領域Ｍｅの辺上の複数の点の三次元座標を取得し、当該複数の点を通る直線として尤もらしい直線の式を推定することにより、各辺の式を推定することができる。

平面推定部３３は、４つの特徴点の三次元座標の平均値を算出することにより、４つの特徴点の中心点Ｘａの三次元座標を取得する。平面推定部３３は、マーカ領域の中心点Ｘａの三次元座標をマーカの位置として推定する。

最後に、平面推定部３３は、マーカ座標系（マーカの姿勢）を推定する。図１０に示すように、平面推定部３３は、マーカ領域の４つの特徴点のうち、隣り合う２点を結ぶベクトルを算出する。つまり、平面推定部３３は、特徴点Ｘ０とＸ３とを結ぶベクトルをマーカのｘ軸方向のベクトル（ｘ´）として推定する。また、平面推定部３３は、特徴点Ｘ０とＸ１とを結ぶベクトルをマーカのｙ軸方向のベクトル（ｙ´）として推定する。さらに、平面推定部３３は、推定した平面Ｆの法線を算出し、当該法線ベクトルをマーカのz軸方向のベクトル（ｚ´）として推定する。

このとき、平面推定部３３は、マーカのｚ軸方向のベクトルを、既に推定したｘ軸方向のベクトルとｙ軸方向のベクトルとの外積を計算することにより推定することもできる。この場合、平面Ｆの推定（ステップＳ１０５）の処理を行うことなく、マーカ領域Ｍｅの４つの特徴点を用いて、マーカの位置及び座標系を推定することができる。つまり、平面推定部３３は、マーカ領域に含まれる複数の画素の二次元座標及び被写体距離情報を取得できれば、これらの情報からマーカ領域の三次元座標を算出し、マーカの位置及び姿勢を推定することができる。

なお、座標系の原点は、例えば、マーカ領域Ｍｅの中心点Ｘａとする。これにより、平面推定部３３は、カメラ座標系とは異なるマーカ座標系を推定する。このように、平面推定部３３は、被写体面に付されたマーカの位置及び姿勢を推定する。

以上のように、本実施の形態にかかる姿勢推定装置３０の構成によれば、マーカ認識部３２が、カメラ１０により生成された撮像画像を取得し、マーカ領域を検出する。平面推定部３３は、マーカ領域の複数の画素の座標と、マーカ領域の複数の画素の被写体距離と、を用いて、マーカ領域に含まれる複数の画素の三次元座標を取得する。そして、平面推定部３３は、マーカ領域に含まれる複数の画素の三次元座標を用いて、マーカ領域と平行な平面の方程式を推定する。つまり、平面推定部３３は、マーカが向いている方向（姿勢）を推定する。また、平面推定部３３は、マーカ領域の４つの特徴点の三次元座標を用いて、マーカの位置及び座標系（姿勢）を推定する。このように、姿勢推定装置３０は、単眼カメラと三次元センサとが生成した画像を用いてマーカの位置及び姿勢を推定することができる。つまり、カメラ１台と三次元センサ１台があれば、被写体面の姿勢を推定することができる。したがって、ステレオカメラを用いること無く、低コストで被写体面の姿勢を推定することができる。また、被写体面の三次元座標を用いて推定を行っているため、推定精度も確保することができる。

＜実施の形態２＞
本発明にかかる実施の形態２について説明する。本実施の形態にかかる姿勢推定装置３０のブロック図を図１１に示す。本実施の形態においては、平面推定部３３によるマーカ領域の特徴点の推定方法が、上述の実施の形態１とは異なる。また、姿勢推定装置３０は、精度評価部３４をさらに備える。なお、その他の構成については、実施の形態１と同様であるため、適宜説明を省略する。

平面推定部３３は、マーカの位置及び姿勢を、距離画像から切り出したマーカ領域Ｍｄの座標から直接推定せず、カメラ画像におけるマーカ領域Ｍｃの座標を、推定した平面Ｆ（以下、投影面Ｆとも称す）に投影することにより、投影面におけるマーカ領域Ｍｅの正確な位置を推定する。

精度評価部３４は、推定したマーカの位置及び姿勢が正確に推定されているか否かを評価する。具体的には、精度評価部３４は、推定されたマーカ領域Ｍｅの位置を、平面推定部３３により平面Ｆに投影されたマーカ領域Ｍｅの位置や、マーカ認識部３２により検出されたカメラ画像におけるマーカ領域Ｍｃの位置と比較する。そして、精度評価部３４は、比較結果に基づいて、推定精度を評価する。

＜姿勢推定システムの動作＞
続いて、図１２のフローチャートを参照して、本実施の形態にかかる平面システムの動作について説明する。なお、ステップＳ２０１〜Ｓ２０５の動作は、図６に示したフローチャートのステップＳ１０１〜Ｓ１０５と同様である。

まず、マーカ認識部３２が、カメラ１０が生成したカメラ画像及び三次元センサが生成した距離画像を取得する（ステップＳ２０１）。そして、マーカ認識部３２は、カメラ画像内のマーカ領域Ｍｃを検出する（ステップＳ２０２）。マーカ認識部３２は、認識したマーカのＩＤを読み取る（ステップＳ２０３）。そして、平面推定部３３は、カメラ画像内のマーカ領域Ｍｃに対応する距離画像内の領域を切り出す（ステップＳ２０４）。平面推定部３３は、距離画像内におけるマーカ領域Ｍｄの画素の座標及び被写体距離情報を用いて三次元座標を取得する。そして、平面推定部３３は、複数の三次元座標に基づいて、マーカ領域Ｍｅが存在する最適な平面の向き（方程式）を推定する（ステップＳ２０５）。

次に、平面推定部３３は、カメラ画像内のマーカ領域Ｍｃを推定した平面Ｆに投影する（ステップＳ２０６）。具体的には、平面推定部３３は、カメラ画像内のマーカ領域Ｍｃの４つの特徴点の座標をサブピクセル精度で取得する。つまり、特徴点のｘ座標及びｙ座標の値に整数だけでなく、小数も含まれる。そして、平面推定部３３は、取得した座標のそれぞれを投影面Ｆに投影し、投影面Ｆにおけるマーカ領域Ｍｅの４つの特徴点の三次元座標を算出する。投影面Ｆにおけるマーカ領域Ｍｅの４つの特徴点の三次元座標の算出は、カメラ１０の内部パラメータ（焦点距離、画像中心座標）を用いた射影変換（中心投影変換）を行うことにより算出することができる。

より詳細には、図１３に示すように、カメラ画像Ｃ内におけるマーカ領域Ｍｃの４つの特徴点Ｔｉ（Ｔ０〜Ｔ３）の座標を（ｕｉ，ｖｉ）とし、投影面におけるマーカ領域Ｍｅの４つの特徴点Ｘｉ（Ｘ０〜Ｘ３）の三次元座標を（ｘｉ，ｙｉ，ｚｉ）とする。このとき、カメラ画像内と投影面内の対応する座標には以下の式（２）〜（４）が成り立つ。なお、ｆｘは、カメラ１０のｘ方向の焦点距離を示し、ｆｙは、カメラ１０のｙ方向の焦点距離を示す。また、Ｃｘ、Ｃｙは、カメラ画像の中心座標を意味する。

上記の式（２）は、マーカ領域Ｍｅを含む平面Ｆ（投影面）の式である。式（３）（４）は、図１３において、カメラ画像Ｃ内の特徴点と、平面Ｆ内の特徴点と、を結ぶ破線の式である。このため、投影面Ｆにおけるマーカ領域Ｍｅの特徴点（Ｘ０〜Ｘ３）の三次元座標を求めるには、式（２）〜（４）の連立方程式を、カメラ画像Ｃ内のマーカ領域Ｍｃの特徴点の各点（Ｔ０〜Ｔ３）について計算すればよい。なお、ｉは特徴点の番号を意味する。

平面推定部３３は、投影面Ｆにおけるマーカ領域Ｍｅの特徴点の三次元座標を算出すると、マーカの位置及び姿勢を推定する（ステップＳ２０７）。具体的には、平面推定部３３は、マーカ領域Ｍｅの４つの特徴点の平均値（マーカ領域Ｍｅの中心点Ｘａの座標）を算出し、マーカの位置を示す三次元座標を取得する。また、平面推定部３３は、マーカ領域Ｍｅの４つの特徴点の三次元座標を用いて、マーカのｘ軸方向のベクトル及びｙ軸方向のベクトルを推定する。さらに、平面推定部３３は、投影面Ｆの法線を算出することにより、ｚ軸方向のベクトルを推定する。なお、平面推定部３３は、ｘ軸方向のベクトルとｙ軸方向のベクトルの外積を計算することにより、ｚ軸方向のベクトルを推定してもよい。これにより、平面推定部３３は、マーカ座標系（マーカの姿勢）を推定する。

最後に、精度評価部３４は、推定したマーカの位置及び姿勢の推定精度の信頼度を評価する（ステップＳ２０８）。評価方法は、三次元空間（推定した平面Ｆ）において評価する方法と、二次元空間（カメラ画像）において評価する方法と、が考えられる。

まず、三次元空間における評価方法について図１４を参照して説明する。精度評価部３４は、平面推定部３３が投影面Ｆに投影したマーカ領域Ｍｅの三次元座標と、推定されたマーカの位置、姿勢、及びマーカのサイズを用いて算出されるマーカ領域の三次元座標と、を比較し、三次元座標の誤差δを算出する。例えば、精度評価部３４は、以下の式（５）を用いて、マーカ領域の特徴点の三次元座標について誤差δを算出する。なお、Ｘはカメラ画像から投影面に投影されたマーカ領域Ｍｅの特徴点の三次元座標、Ｘ´は推定されたマーカ領域の特徴点の三次元座標を意味する。具体的には、特徴点Ｘ´は、推定したマーカの位置（マーカの中心点の三次元座標）と、推定したマーカ座標系と、予め設定されたマーカの各辺の長さや形状（精度評価部３４が予め取得しているものとする）と、に基づいて、推定されるマーカの特徴点である。ｉは特徴点の番号を意味する。

そして、精度評価部３４は、以下の式（６）を用いて、推定されたマーカの位置及び姿勢の信頼性を判断する。なお、αは信頼度、θは信頼度が０となる誤差の閾値を意味する。

精度評価部３４は、信頼度αが閾値より高いか否かを判定する（ステップＳ２０９）。信頼度αが閾値よりも高い場合（ステップＳ２０９：Ｙｅｓ）、精度評価部３４は、推定結果を採用し、フローを終了する。一方、信頼度αが閾値以下である場合（ステップＳ２０９：Ｎｏ）、精度評価部３４は、推定結果を棄却する。そして、姿勢推定装置３０は、再度、マーカ検出処理（ステップＳ２０２）から処理をやり直す。例えば、精度評価部３４は、信頼度αが０．８以上の場合に推定結果を採用し、信頼度αが０．８未満の場合に推定結果を棄却する。

次に、二次元空間における評価方法について図１５を参照して説明する。精度評価部３４は、推定した位置及び姿勢をマーカのサイズを考慮して、投影面において推定したマーカ領域をカメラ画像平面に対して再度投影する。そして、精度評価部３４は、投影したマーカ領域の位置と、カメラ画像におけるマーカ領域Ｍｃの位置と、を比較し、二次元座標の誤差δを算出する。例えば、精度評価部３４は、以下の式（７）を用いて、マーカ領域の特徴点の二次元座標について誤差δを算出する。なお、Ｐはカメラ画像（撮像時）におけるマーカ領域Ｍｃの特徴点の二次元座標、Ｐ´は推定されたマーカの特徴点Ｘ´（図１４のＸ´と同じ）をカメラ画像に投影した際のマーカ領域の特徴点の二次元座標、ｉは特徴点の番号を意味する。

信頼度αについては、上記の式（６）と同様の式を用いて算出できる。なお、精度評価部３４は、上記した２つの評価方法の両方を用いて、推定精度を評価してもよいし、いずれか一方を用いて評価してもよい。

以上のように、本実施の形態にかかる姿勢推定装置３０の構成によれば、平面推定部３３は、マーカを含む平面として推定した投影面に対して、カメラ画像におけるマーカ領域Ｍｃを投影する。そして、平面推定部３３は、投影面Ｆに投影されたマーカ領域Ｍｅの４つの特徴点の三次元座標を用いて、マーカの位置及び姿勢を推定する。このとき、実施の形態１のように、距離画像から切り出したマーカ領域Ｍｄの座標及び被写体距離情報のみを用いて推定を行う場合、切り出す際の誤差の影響で、特徴点の三次元座標の推定に誤差が生じてしまう可能性がある。これに対して、本実施の形態においては、平面推定部３３は、マーカ領域Ｍｅの４つの特徴点の三次元座標は、カメラ画像における特徴点を、推定した投影面Ｆに投影することにより算出している。このため、切り出し誤差の影響を受けずにマーカ領域の位置及び姿勢を推定できる。その結果、推定精度を向上させることができる。

また、平面推定部３３は、マーカ領域Ｍｃの投影の際に、カメラ画像における特徴点の座標を、サブピクセル精度で特定している。このため、画素単位で行う推定よりも、より精度の高い推定をすることができる。

さらに、精度評価部３４が、推定されたマーカ領域の推定精度を評価し、閾値以下の精度の場合には、推定結果を棄却する。このため、精度の高い推定結果のみを採用することができる。また、推定結果の精度が低い場合には、再度推定しなおす等の対応をとることができる。したがって、十分な精度の推定結果を取得することができる。

（変形例）
本実施の形態にかかる変形例について説明する。変形例においては、マーカが付された被写体面が平面ではなく、曲面である場合について説明する。つまり、姿勢推定装置３０は、距離画像から切り出されたマーカ領域に対して、最適な素形状を推定する。なお、姿勢推定装置３０の構成は、図１１に示した構成と同様であるため、詳細な説明は適宜省略する。

変形例にかかる姿勢推定方法も図１２のフローチャートと同様の処理が行われる。まず、マーカ認識部３２が、カメラ画像からマーカを検出する。なお、変形例においては、図１６に示すように、円柱の被写体面にマーカが付されているものとする。つまり、マーカは曲面形状をしている。

マーカ認識部３２は、認識したマーカのＩＤを読み取る。そして、図１７に示すように、平面推定部３３は、カメラ画像内のマーカ領域Ｍｃに対応する距離画像内の領域Ｍｄを切り出す。

次に、平面推定部３３は、切り出したマーカ領域Ｍｄに含まれる複数の画素の座標及び被写体距離情報に基づいて、複数の画素の三次元座標を取得する。そして、平面推定部３３は、取得した三次元座標を用いて、例えばＲＡＮＳＡＣ法によりマーカが付された円柱Ｅの方程式を推定する（図１８参照）。このとき、円柱の方程式は、以下の式（８）により示される。ａ、ｂ、ｒは、定数パラメータである。ｒは、円柱の半径を示す。なお、平面推定部３３は、予めマーカが曲面であることを認識しているものとする。例えば、ユーザにより予めマーカの形状が入力されていてもよいし、読み取ったマーカの情報にマーカの形状に関する情報を含めてもよい。

平面推定部３３は、円柱の方程式を推定すると、カメラ画像におけるマーカ領域Ｍｃを推定した円柱Ｅに投影する。つまり、平面推定部３３は、図１９に示すように、式（３）（４）（８）を用いて、カメラ画像Ｃにおけるマーカ領域Ｍｃの特徴点の画素の座標を、推定した円柱Ｅの側面における三次元座標として投影する。

平面推定部３３は、推定した円柱におけるマーカ領域Ｍｅの特徴点の三次元座標を用いて、マーカの位置を推定する。図２０に示すように、平面推定部３３は、例えば、マーカ領域Ｍｅの中心点Ｘａの三次元座標をマーカの位置として推定する。マーカ領域Ｍｅの中心点Ｘａの三次元座標は、例えば、マーカ領域Ｍｅの特徴点（Ｘ０〜Ｘ３）の三次元座標を用いて、以下の式（９）を計算することにより求めることができる。

つまり、マーカの位置は、４つの特徴点の三次元座標の平均値で示される。なお、Ｘｉはマーカ領域のｉ番目の特徴点を示す。Ｘｉ＝（ｘｉ，ｙｉ，ｚｉ）である。

次に、平面推定部３３は、マーカ座標系（マーカの姿勢）を推定する。具体的には、図２０に示すように、平面推定部３３は、ｘ軸方向において、マーカ領域Ｍｅの特徴点Ｘ０とＸ３を結ぶベクトルと、特徴点Ｘ１とＸ２とを結ぶベクトルと、の平均を以下の式（１０）を用いて算出し、ｘ軸方向のベクトルｎｘを推定する。

同様に、平面推定部３３は、ｙ軸方向において、マーカ領域Ｍｅの特徴点の座標Ｘ０とＸ１を結ぶベクトルと、特徴点Ｘ２とＸ３とを結ぶベクトルと、の平均を以下の式（１１）を用いて算出し、ｙ軸方向のベクトルｎｙを推定する。

さらに、平面推定部３３は、マーカ領域Ｍｅのｚ軸方向のベクトルｎｚを、以下の式（１２）を用いて推定する。つまり、ベクトルｎｚは、既に算出したベクトルｎｘとベクトルｎｙの外積により求めることができる。

最終的に、マーカの姿勢Ｒは、以下の式（１３）を用いて表される。つまり、マーカの姿勢Ｒは、ｘ軸方向、ｙ軸方向、ｚ軸方向のベクトルを正規化し、回転行列を用いて表現される。

そして、下記の式（１４）のように、マーカの位置姿勢行列Σ^ｍｒｋを用いて、算出したマーカの位置及び姿勢を表す。

このように、本実施の形態にかかる姿勢推定装置３０は、マーカが曲面に付されている場合であっても、マーカ領域Ｍｅの三次元座標を取得し、マーカが存在する曲面（円柱）を推定する。そして、姿勢推定装置３０は、カメラ画像内のマーカ領域Ｍｃの特徴点を、推定した曲面に投影し、曲面（円柱Ｅ）におけるマーカの特徴点を算出する。これにより、マーカの位置及び姿勢を推定することができる。

＜実施の形態３＞
本発明にかかる実施の形態３について説明する。本実施の形態にかかる姿勢推定装置３０は、カメラ画像内のマーカの一部が隠れている状態において、マーカの位置及び姿勢を推定する。なお、基本的な推定方法は、図６や図１２のフローチャートと同様であるため、詳細な説明は適宜省略する。

＜マーカの特徴点が１つ隠れている場合＞
はじめに、図２１に示すように、マーカの４つの特徴点のうちの１つの特徴点が隠れている場合における推定方法について説明する。まず、マーカ認識部３２は、カメラ画像におけるマーカ領域Ｍｃを検出する。

このとき、マーカの１つの特徴点が隠れているため、マーカ認識部３２が検出できる特徴点の個数は３つである。そのため、マーカ認識部３２は、矩形状のマーカ領域を認識できない。そのため、図２２に示すように、マーカ認識部３２は、カメラ画像において、隠れている特徴点Ｔ２に延びる２本の辺Ｌ１、Ｌ２を延長する。マーカ認識部３２は、延長した２つの辺Ｌ１、Ｌ２が交わった場合、その交点を隠れている特徴点Ｔ２として推定する。そして、マーカ認識部３２は、４つの点で形成される領域が略矩形状である場合、当該領域をマーカ領域Ｍｃであると判定する。

また、マーカの色が特徴的な場合には、マーカ認識部３２は、マーカ特有の色情報を用いて、マーカ領域Ｍｃを検出してもよい。例えば、図２１に示すように、マーカが白と黒の色だけで構成された矩形である場合、マーカ認識部３２は、白と黒のみで構成された領域をマーカ領域と判定する。

次に、平面推定部３３は、マーカ領域Ｍｅを含む平面を推定する。つまり、平面推定部３３は、カメラ画像においてマーカ領域Ｍｃと判定された領域に対応する領域を、距離画像から切り出す。そして、距離画像におけるマーカ領域Ｍｄに含まれる複数の画素の座標及び被写体距離情報から算出される三次元座標を取得し、マーカ領域Ｍｅを含む平面（平面の方程式）を推定する。

そして、平面推定部３３は、上記の式（２）〜（４）を用いて、カメラ画像において認識できたマーカ領域Ｍｃの３つ特徴点Ｔ０、Ｔ１、Ｔ３を、推定した平面（投影面）に投影する。これにより、平面推定部３３は、マーカ領域Ｍｅの３つの特徴点Ｘ０、Ｘ１、Ｘ３の投影面における三次元座標を取得する。

次に、平面推定部３３は、図２３に示すように、カメラ画像において認識できた３つの特徴点Ｘ０、Ｘ１、Ｘ３のうち、隣接しない２つの特徴点Ｘ１、Ｘ３の三次元座標を用いて、当該２点を結ぶ線分（マーカの対角線）を算出する。そして、平面推定部３３は、対角線の中点Ｘａの三次元座標を取得する。これにより、平面推定部３３は、マーカ領域Ｍｅの中心点Ｘａの三次元座標を、マーカの位置を示す座標として取得する。

また、平面推定部３３は、マーカ座標系を推定する。具体的には、平面推定部３３は、認識できたマーカ領域Ｍｅの３つの特徴点Ｘ０、Ｘ１、Ｘ３の間を結ぶ２本の辺のベクトルを算出する。図２４に示すように、平面推定部３３は、特徴点Ｘ０から特徴点Ｘ３に向かうベクトルをｘ軸方向のベクトルとして推定する。また、平面推定部３３は、特徴点Ｘ０から特徴点Ｘ１に向かうベクトルをｙ軸方向のベクトルとして推定する。そして、平面推定部３３は、マーカ領域Ｍｅを含む平面の法線をｚ軸方向のベクトルとして算出する。これにより、平面推定部３３は、マーカ座標系（マーカの姿勢）を推定する。なお、ｚ軸方向のベクトルの算出については、既に算出したｘ軸方向のベクトルとｙ軸方向のベクトルの外積を算出することによっても求めることができる。

＜マーカの特徴点が２つ隠れている場合＞
続いて、マーカの４つの特徴点のうち２つが隠れている場合における推定方法について説明する。なお、図２５に示すように、マーカの４つの特徴点うち、隣接する２つの特徴点が隠れているものとする。

まず、マーカ認識部３２は、カメラ画像におけるマーカ領域Ｍｃを特定する。マーカの隣接する２つの特徴点が隠れている場合、マーカ認識部３２が検出できる特徴点の個数は２つである。このため、マーカの認識できる辺を延長しただけでは矩形状は形成されないため、形状に基づいてマーカ領域Ｍｃを検出することは困難である。そのため、マーカ認識部３２は、マーカ特有の色情報に基づいて、マーカ領域Ｍｃを検出する。

次に、平面推定部３３は、マーカ領域Ｍｅを含む平面を推定する。つまり、平面推定部３３は、カメラ画像においてマーカ領域Ｍｃと判定された領域に対応する距離画像内の領域を、距離画像から切り出す。そして、平面推定部３３は、距離画像におけるマーカ領域Ｍｄに含まれる複数の画素の座標及び被写体距離情報から算出される三次元座標を取得し、マーカ領域Ｍｅを含む平面（平面の方程式）を推定する。

そして、平面推定部３３は、上記の式（２）〜（４）を用いて、カメラ画像において認識できたマーカ領域Ｍｃの２つ特徴点を、推定した平面（投影面）に投影する。さらに、平面推定部３３は、推定した平面において、隠れている２つの特徴点の三次元座標を推定する。例えば、平面推定部３３は、マーカの形状情報を予め取得しているものとする。ここで、形状情報とは、矩形状のマーカが正方形であるか否かを示す情報、各辺の長さの比、各辺の長さ等を意味する情報である。そして、平面推定部３３は、マーカの形状情報を用いて、隠れている２つの特徴点の三次元座標を推定する。

例えば、図２６に示すように、平面推定部３３が、マーカが正方形であるという形状情報を有しているとする。この場合、平面推定部３３は、推定した平面上に位置すること、隠れている特徴点に延びる辺Ｌ１、Ｌ３の延長上に位置すること、及び、認識された特徴点Ｘ０、Ｘ１からの距離が認識された２点（Ｘ０、Ｘ１）間の距離と同じであること、の全ての条件を満たす点を特徴点Ｘ２、Ｘ３として推定する。これにより、平面推定部３３は、マーカ領域Ｍｅの全ての特徴点の三次元座標を取得する。そして、図２７に示すように、平面推定部３３は、マーカ領域Ｍｅの全ての特徴点の三次元座標の平均値を、マーカの位置を示す中心点Ｘａの座標として推定する。また、平面推定部３３は、対角線の中点または対角線の交点の三次元座標をマーカの位置を示す座標として推定してもよい。

また、平面推定部３３は、マーカ座標系を推定する。具体的には、図２８に示すように、平面推定部３３は、カメラ画像において認識できた特徴点Ｘ０、Ｘ１を結ぶベクトルをｙ軸方向のベクトルとして推定する。また、平面推定部３３は、推定した平面の法線をｚ軸方向のベクトルとして推定する。そして、平面推定部３３は、既に算出したｙ軸方向のベクトルとｚ軸方向のベクトルの外積を、ｘ軸方向のベクトルとして推定する。これにより、平面推定部３３は、マーカ座標系（マーカの姿勢）を推定する。

＜マーカの特徴点が２つ隠れている場合＞
続いて、マーカの４つの特徴点のうちの２つが隠れている場合における推定方法について説明する。なお、図２９に示すように、マーカの４つの特徴点のうち、対角線上に位置する２つの特徴点が隠れているものとする。まず、マーカ認識部３２は、カメラ画像におけるマーカ領域Ｍｃを検出する。マーカ認識部３２が検出できる特徴点の個数は２つである。このため、形状に基づいてマーカ領域Ｍｃを検出することは困難である。このため、マーカ認識部３２は、上記と同様に、マーカ特有の色情報に基づいて、マーカ領域Ｍｃを検出する。

次に、平面推定部３３は、マーカ領域Ｍｅを含む平面を推定する。つまり、平面推定部３３は、カメラ画像においてマーカ領域Ｍｃと判定された領域に対応する距離画像内の領域を、距離画像から切り出す。そして、平面推定部３３は、距離画像におけるマーカ領域Ｍｄに含まれる複数の画素の座標及び被写体距離情報から算出される三次元座標を取得し、マーカ領域Ｍｅを含む平面（平面の方程式）を推定する。

そして、平面推定部３３は、式（２）〜（４）を用いて、カメラ画像において認識できたマーカ領域Ｍｃの２つの特徴点の座標を、推定した平面（投影面）に投影する。

さらに、平面推定部３３は、図３０に示すように、投影面に投影したマーカ領域Ｍｅにおいて、カメラ画像において認識できた特徴点Ｘ０から延びる２つの辺Ｌ０、Ｌ３をそれぞれ延長する。また、平面推定部３３は、カメラ画像において認識できた特徴点Ｘ２から延びる２つの辺Ｌ１、Ｌ２をそれぞれ延長する。そして、平面推定部３３は、延長した辺の交点をマーカ領域Ｍｅの特徴点Ｘ１、Ｘ３であると推定する。より詳細には、平面推定部３３は、推定した平面上に位置すること、かつ、延長した辺Ｌ１、Ｌ２の延長上にあること、の条件を満たす点を隠れている２つの特徴点Ｘ１、Ｘ３として推定する。そして、図３１に示すように、平面推定部３３は、４つの特徴点の三次元座標の平均値を、マーカの位置を示す中心点Ｘａの座標として推定する。つまり、平面推定部３３は、マーカ領域Ｍｅの中心点Ｘａの座標を推定する。

さらに、図３２に示すように、平面推定部３３は、マーカの４つの特徴点のうち、隣接する２点を結ぶベクトルをｘ軸方向のベクトル及びｙ軸方向のベクトルとして推定する。さらに、平面推定部３３は、マーカ領域Ｍｅを含む平面の法線ベクトルをｚ軸方向のベクトルとして推定する。これにより、平面推定部３３は、マーカ座標系（マーカの姿勢）を推定する。

＜マーカの特徴点が３つ隠れている場合＞
続いて、図３３に示すように、マーカの４つの特徴点のうちの３つが隠れている場合における推定方法について説明する。マーカ認識部３２が検出できる特徴点の個数は１つである。このため、形状に基づいてマーカ領域Ｍｃを検出することは困難である。このため、まず、マーカ認識部３２は、マーカ特有の色情報に基づいて、カメラ画像におけるマーカ領域Ｍｃを検出する。

次に、平面推定部３３は、マーカ領域Ｍｅを含む平面を推定する。つまり、平面推定部３３は、カメラ画像においてマーカ領域Ｍｃと認識された領域に対応する距離画像内の領域を、距離画像から切り出す。そして、距離画像におけるマーカ領域Ｍｄに含まれる複数の画素の座標及び被写体距離情報から算出される三次元座標を取得し、マーカ領域Ｍｅを含む平面（平面の方程式）を推定する。

そして、平面推定部３３は、式（２）〜（４）を用いて、カメラ画像において認識できたマーカ領域Ｍｃの１つ特徴点の座標を、推定した平面に投影する。

さらに、図３４に示すように、平面推定部３３は、投影面に投影したマーカ領域Ｍｅにおいて、認識できた特徴点Ｘ０に隣接する２つの特徴点Ｘ１、Ｘ３の三次元座標を推定する。このとき、平面推定部３３は、マーカの形状情報（マーカが正方形であるか否か、及び、各辺の長さ）を用いて、隠れている２つの特徴点Ｘ１、Ｘ３の三次元座標を推定する。つまり、平面推定部３３は、認識できた特徴点Ｘ０から延びる辺Ｌ０、Ｌ３の延長上に位置する点であり、かつ、認識できた特徴点Ｘ０から予め取得している辺の長さの距離ｄだけ離れた点を、隠れている２つの特徴点Ｘ１、Ｘ３として推定する。

平面推定部３３は、推定した２つの特徴点Ｘ１、Ｘ３を結ぶ線分（マーカの対角線）の中点Ｘａの三次元座標を、マーカの位置を示す座標として推定する。

次に、平面推定部３３は、認識できた特徴点Ｘ０から推定した特徴点Ｘ３に延びる辺Ｌ３のベクトルをｘ軸方向のベクトルとして推定する。また、平面推定部３３は、認識できた特徴点Ｘ０から推定した特徴点Ｘ１に延びる辺Ｌ１のベクトルをｙ軸方向のベクトルとして推定する。さらに、平面推定部３３は、推定した平面の法線ベクトルをｚ軸方向のベクトルとして推定する。これにより、平面推定部３３は、マーカ座標系（マーカの姿勢）を推定する。

以上のように、本実施の形態にかかる姿勢推定装置の構成によれば、カメラ画像において、マーカの特徴点が隠れている状態であっても、平面推定部３３がマーカの辺を、隠れた特徴点に向けて延長する。そして、平面推定部３３は、延長させた辺と、マーカの他の辺を延長させた線分と、の交点をマーカの特徴点と推定する。または、平面推定部３３は、延長させた辺上の点であって、認識できた特徴点から予め取得したマーカの辺の長さだけ離れた点をマーカの特徴点と推定する。これにより、カメラ画像において、マーカ認識部３２が、マーカの４つの特徴点の全てを認識できない場合であっても、平面推定部３３は、マーカの位置及び姿勢を推定することができる。

＜その他の実施の形態＞
本発明にかかるその他の実施の形態について説明する。上述の実施の形態においては、被写体面に付されたマーカを認識し、ＩＤを読み取ることにより個体の識別を行う例について説明したが、これに限られるものではない。

例えば、姿勢推定装置３０が、推定対象となる個体毎に異なる図形のテンプレート画像を予め記憶しておき、カメラ画像に対してテンプレート画像を用いたテンプレートマッチングを行ってもよい。このような方法においても、推定対象となる個体を認識することができる。

また、推定対象となる被写体面（カメラ画像内の一定領域）の特定及び選択は、必ずしもマーカや図形等を用いて自動的に行われなくてもよい。例えば、カメラ画像内に存在する被写体のうち、推定対象となる被写体面をユーザが操作キーやタッチパネル等を用いて選択してもよい。さらに、カメラ画像内の所定の領域（カメラの画角の中心領域、右上領域、左下領域等の予め固定された領域）に含まれる被写体面を推定対象として特定してもよい。

さらに、上記の実施の形態においては、姿勢推定装置を含む画像処理システムとして説明を行ったが、これらのシステム全体をロボットに適用してもよい。

例えば、周囲環境から所定の検出物体を検出する必要があるロボットに対して、上記の画像処理システムを適用することができる。具体的には、ロボットは、カメラと、三次元センサと、姿勢推定装置と、を備える。なお、周囲環境に応じて移動するロボットは、通常、周囲環境の状況を把握するために、カメラと三次元センサとを備えているため、これらの装置を流用してもよい。

ロボットは、カメラを用いてカメラ画像を生成する。また、三次元センサを用いて距離画像を生成する。そして、上述したように、姿勢推定装置は、距離画像から被写体距離情報を取得し、マーカ領域内の複数の画素の三次元座標を取得する。

このとき、ロボットは、必ずしも距離画像を生成しなくてもよい。例えば、ロボットは、単純な距離センサ等を用いて、複数の画素に存在する被写体までの被写体距離を、個別に検出してもよい。これにより、距離画像を生成することなく、複数の画素における被写体距離を取得することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更及び組み合わせをすることが可能である。

１０ カメラ
２０ 三次元センサ
３０ 姿勢推定装置
３１ 制御部
３２ マーカ認識部
３３ 平面推定部
３４ 精度評価部

Claims (11)

1. 撮像装置を用いて被写体を撮像することにより生成された撮像画像を取得し、
   前記撮像画像内の一定領域に含まれる複数の画素に対応する複数の座標を取得し、
   前記複数の画素における前記被写体から前記撮像装置までの距離を示す被写体距離情報を取得し、
   取得した複数の前記座標及び複数の前記被写体距離情報に基づいて、前記一定領域に含まれる前記被写体が有する被写体面の姿勢を推定する姿勢推定方法。
2. 各画素が前記被写体距離情報を有する距離画像を取得し、
   前記撮像画像内の画素と、前記距離画像内の画素と、を対応付け、
   前記距離画像内の画素のうち、前記一定領域内の前記複数の画素に対応する画素から、前記被写体距離情報を取得する請求項１に記載の姿勢推定方法。
3. 前記複数の画素の座標及び前記被写体距離情報に基づいて、前記複数の画素の三次元座標を算出し、
   前記複数の画素の三次元座標に基づいて、前記一定領域に含まれる前記被写体面の姿勢を推定する請求項１または２に記載の姿勢推定方法。
4. 前記被写体面にはマーカが付されており、
   前記撮像画像内における前記マーカを含むマーカ領域を前記一定領域として検出し、
   検出した前記マーカ領域に含まれる前記マーカの姿勢を推定する請求項１〜３のいずれか一項に記載の姿勢推定方法。
5. 前記複数の画素の前記座標及び前記被写体距離情報を用いて、前記被写体面と平行な投影面の方程式を算出し、
   前記撮像画像における前記マーカの姿勢を示す特徴点を前記投影面に投影し、
   前記投影面に投影された前記特徴点の座標に基づいて、前記マーカの姿勢を推定する請求項４に記載の姿勢推定方法。
6. 前記撮像画像内の前記特徴点の座標をサブピクセル精度で特定し、特定した前記特徴点の座標を用いて前記投影面への投影を行う請求項５に記載の姿勢推定方法。
7. 前記投影面に投影された前記特徴点の座標に基づいて、前記マーカの位置を推定し、
   推定した前記マーカの姿勢、推定した前記マーカの位置、及び予め設定された前記マーカのサイズに関する情報を用いて、前記投影面における前記特徴点の座標を算出し、
   前記投影面において算出した前記特徴点を、前記撮像画像に投影し、
   前記撮像画像において、撮像時における前記特徴点の座標と、投影された前記特徴点の座標と、を比較し、
   比較結果に基づいて、推定精度を判断する請求項５または６に記載の姿勢推定方法。
8. 前記投影面に投影された前記特徴点の座標に基づいて、前記マーカの位置を推定し、
   推定した前記マーカの姿勢、推定した前記マーカの位置、及び予め設定された前記マーカのサイズに関する情報を用いて、前記投影面における前記特徴点の座標を算出し、
   前記投影面において、算出した前記特徴点の座標と、前記マーカの姿勢を推定するときに前記撮像画像から投影された前記特徴点の座標と、を比較し、
   比較結果に基づいて、推定精度を判断する請求項５または６に記載の姿勢推定方法。
9. 前記マーカは略矩形状であり、
   前記撮像画像における前記マーカの頂点を前記特徴点として検出し、
   前記検出した前記特徴点の個数が２つまたは３つの場合、検出した前記特徴点から延びる前記マーカの辺を延長し、
   延長した前記辺が交わった交点を前記特徴点として推定する請求項５〜８のいずれか一項に記載の姿勢推定方法。
10. 前記マーカは略矩形状であり、
    前記撮像画像における前記マーカの頂点を前記特徴点として検出し、
    前記検出した前記特徴点の個数が４つ未満の場合、検出した前記特徴点から延びる前記マーカの辺を延長し、
    延長した辺の上の点であって、検出した前記特徴点から予め設定された距離だけ離れた点を前記特徴点として推定する請求項５〜８のいずれか一項に記載の姿勢推定方法。
11. 前記撮像装置と、
    前記被写体距離情報を取得する距離センサと、
    請求項１〜１０のいずれか一項に記載の姿勢推定方法を実行する姿勢推定装置と、
    を備えるロボット。