JP2016170050A - 位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラム - Google Patents
位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラム Download PDFInfo
- Publication number
- JP2016170050A JP2016170050A JP2015050094A JP2015050094A JP2016170050A JP 2016170050 A JP2016170050 A JP 2016170050A JP 2015050094 A JP2015050094 A JP 2015050094A JP 2015050094 A JP2015050094 A JP 2015050094A JP 2016170050 A JP2016170050 A JP 2016170050A
- Authority
- JP
- Japan
- Prior art keywords
- orientation
- dimensional
- image
- shape model
- calculated
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
【課題】３次元形状が既知である計測対象の物体の画像から、当該物体の位置及び姿勢を、よりロバストかつ高精度に計測する。
【解決手段】位置姿勢計測装置は、計測対象の物体の２次元画像を取得する第１の取得手段と、前記物体の形状を表す３次元形状データを取得する第２の取得手段と、前記物体の形状モデルを保存する保存手段と、前記第２の取得手段により取得された前記物体の前記３次元形状データと、前記保存手段に保存される前記物体の前記形状モデルとに基づいて、前記物体の主平面の位置及び姿勢を前記物体の第１の位置及び姿勢として算出する第１の算出手段と、前記第１の算出手段により算出された前記第１の位置及び姿勢に基づいて、前記第１の取得手段により取得された前記２次元画像と前記形状モデルとを照合することにより、前記物体の３次元空間上の第２の位置及び姿勢を算出する第２の算出手段と、を具備する。
【選択図】図１
【解決手段】位置姿勢計測装置は、計測対象の物体の２次元画像を取得する第１の取得手段と、前記物体の形状を表す３次元形状データを取得する第２の取得手段と、前記物体の形状モデルを保存する保存手段と、前記第２の取得手段により取得された前記物体の前記３次元形状データと、前記保存手段に保存される前記物体の前記形状モデルとに基づいて、前記物体の主平面の位置及び姿勢を前記物体の第１の位置及び姿勢として算出する第１の算出手段と、前記第１の算出手段により算出された前記第１の位置及び姿勢に基づいて、前記第１の取得手段により取得された前記２次元画像と前記形状モデルとを照合することにより、前記物体の３次元空間上の第２の位置及び姿勢を算出する第２の算出手段と、を具備する。
【選択図】図１
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.
上記非特許文献１に開示される濃淡画像を用いる位置及び姿勢の推定方法は、ワイヤフレームモデル上の制御点で構成される、所与である物体の概略の位置及び姿勢が、撮像された当該物体の実際の位置及び姿勢に十分近い場合には、有効であり得る。
しかしながら、この方法によれば、最初に与えられる物体の概略の位置及び姿勢と、計測対象の物体の濃淡画像上の位置及び姿勢、の差（初期値のずれ）がある程度存在すると、制御点と画像上のエッジとの対応付けに失敗してしまう。すなわち、物体の位置及び姿勢の計測におけるロバスト性が低くなるという課題があった。
しかしながら、この方法によれば、最初に与えられる物体の概略の位置及び姿勢と、計測対象の物体の濃淡画像上の位置及び姿勢、の差（初期値のずれ）がある程度存在すると、制御点と画像上のエッジとの対応付けに失敗してしまう。すなわち、物体の位置及び姿勢の計測におけるロバスト性が低くなるという課題があった。
また、テンプレートマッチングを用いて、物体の位置及び姿勢を濃淡画像から算出する方法もある。しかしながら、この方法によれば、ある位置及び姿勢における物体の見え方の情報を持つテンプレートを、当該物体があらゆる位置及び姿勢にある場合でも検出できるよう、予め多数を準備しておかなければならず、煩雑であった。
本発明は上記課題に鑑みてなされたものであり、３次元形状が既知である計測対象の物体の画像から、当該物体の位置及び姿勢を、よりロバストかつ高精度に計測することができる位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラムを提供することを目的とする。
本発明は上記課題に鑑みてなされたものであり、３次元形状が既知である計測対象の物体の画像から、当該物体の位置及び姿勢を、よりロバストかつ高精度に計測することができる位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラムを提供することを目的とする。
（１）上記課題を解決するために、本願発明の一態様に係る位置姿勢計測装置は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、前記物体の２次元画像を取得する第１の取得手段と、前記物体の形状を表す３次元形状データを取得する第２の取得手段と、前記物体の形状モデルを保存する保存手段と、前記第２の取得手段により取得された前記物体の前記３次元形状データと、前記保存手段に保存される前記物体の前記形状モデルとに基づいて、前記物体の主平面の位置及び姿勢を前記物体の第１の位置及び姿勢として算出する第１の算出手段と、前記第１の算出手段により算出された前記第１の位置及び姿勢に基づいて、前記第１の取得手段により取得された前記２次元画像と前記形状モデルとを照合することにより、前記物体の３次元空間上の第２の位置及び姿勢を算出する第２の算出手段と、を具備する。
（２）上記課題を解決するために、本願発明の一態様に係る位置姿勢計測方法は、計測対象である物体の位置及び姿勢を計測する位置姿勢計測方法であって、前記物体の２次元画像を取得するステップと、前記物体の形状を表す３次元形状データを取得するステップと、前記物体の形状モデルを保存するステップと、前記取得した前記物体の前記３次元形状データと、前記保存される前記物体の前記形状モデルとに基づいて、前記物体の主平面の位置及び姿勢を前記物体の第１の位置及び姿勢として算出するステップと、前記算出された前記第１の位置及び姿勢に基づいて、前記取得された前記２次元画像と前記形状モデルとを照合することにより、前記物体の３次元空間上の第２の位置及び姿勢を算出するステップと、を含む。
本発明によれば、３次元形状が既知である計測対象の物体の画像から、当該物体の位置及び姿勢を、よりロバストかつ高精度に計測することができる。
以下、添付図面を参照して本発明の実施形態を詳細に説明する。
なお、以下に説明する実施形態は、本発明の実現手段としての一例であり、本発明が適用される装置の構成や各種条件によって適宜修正又は変更されるべきものであり、本発明は以下の実施形態に限定されるものではない。
以降の実施形態では、計測対象物体の「主平面」とは、計測対象物体の現在の位置及び姿勢における最も広い平面をいうものとする。
なお、以下に説明する実施形態は、本発明の実現手段としての一例であり、本発明が適用される装置の構成や各種条件によって適宜修正又は変更されるべきものであり、本発明は以下の実施形態に限定されるものではない。
以降の実施形態では、計測対象物体の「主平面」とは、計測対象物体の現在の位置及び姿勢における最も広い平面をいうものとする。
（実施形態１）
実施形態１においては、計測対象物体の位置及び姿勢の推定を段階的及び選択的に実行する。
すなわち、本実施形態１では、まず、計測対象物体の３次元データ（３次元形状データ）の情報、例えば物体の表面上の点群の３次元座標値、を用いて、当該物体の主平面の位置及び姿勢を推定する。その後、計測対象物体の２次元画像（以下、「濃淡画像」として説明するが、カラー画像を用いることもできる。）上の特徴に対して物体の３次元モデルが当てはまるようにフィッティングして位置及び姿勢の推定を行う。
なお、本実施形態１では、物体の位置及び姿勢の初期値はあらかじめ与えられるものとする。また、計測対象の物体が１つの場合を例として説明するが、本実施形態１は計測対象物体が複数である場合にも適用することができる。
実施形態１においては、計測対象物体の位置及び姿勢の推定を段階的及び選択的に実行する。
すなわち、本実施形態１では、まず、計測対象物体の３次元データ（３次元形状データ）の情報、例えば物体の表面上の点群の３次元座標値、を用いて、当該物体の主平面の位置及び姿勢を推定する。その後、計測対象物体の２次元画像（以下、「濃淡画像」として説明するが、カラー画像を用いることもできる。）上の特徴に対して物体の３次元モデルが当てはまるようにフィッティングして位置及び姿勢の推定を行う。
なお、本実施形態１では、物体の位置及び姿勢の初期値はあらかじめ与えられるものとする。また、計測対象の物体が１つの場合を例として説明するが、本実施形態１は計測対象物体が複数である場合にも適用することができる。
＜位置姿勢計測装置の構成＞
図１は、本実施形態１における位置姿勢計測装置１０１の構成を示す。図１に示すように、位置姿勢計測装置１０１は、モデル保存部１１０、２次元画像入力部１３０、３次元データ入力部１５０、第１位置姿勢算出部１６０、第２位置姿勢算出部１６５から構成されている。また、位置姿勢計測装置１０１には、２次元画像撮像部１００、距離計測部１０５が接続される。以下、位置姿勢計測装置１０１を構成する各部、及び、位置姿勢計測装置１０１に接続する各部の構成について説明する。
図１は、本実施形態１における位置姿勢計測装置１０１の構成を示す。図１に示すように、位置姿勢計測装置１０１は、モデル保存部１１０、２次元画像入力部１３０、３次元データ入力部１５０、第１位置姿勢算出部１６０、第２位置姿勢算出部１６５から構成されている。また、位置姿勢計測装置１０１には、２次元画像撮像部１００、距離計測部１０５が接続される。以下、位置姿勢計測装置１０１を構成する各部、及び、位置姿勢計測装置１０１に接続する各部の構成について説明する。
２次元画像撮像部１００は、例えば、物体の通常の２次元画像を撮影するカメラであってよい。撮影される２次元画像は、濃淡画像であってもよいしカラー画像であってもよい。この２次元画像撮像部１００が撮影する画像は、位置姿勢計測装置１０１の２次元画像入力部１３０に入力されることで取得される。
この２次元画像において、カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えばZhangの方法などによって事前にキャリブレーションしておいてよい（Z. Zhang, “A flexible new technique for camera calibration”, IEEE Transactions on Pattern Analysis and Machine Intelligence,Vol.22, No.11, pages 1330-1334, 2000.）。
この２次元画像において、カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、例えばZhangの方法などによって事前にキャリブレーションしておいてよい（Z. Zhang, “A flexible new technique for camera calibration”, IEEE Transactions on Pattern Analysis and Machine Intelligence,Vol.22, No.11, pages 1330-1334, 2000.）。
距離計測部１０５は、計測対象である物体の表面上の点までの距離を計測する。本実施形態１では、距離計測部１０５として距離画像を出力する距離センサを用いるが、他の手段を用いてもよい。ここで、距離画像は、各画素が計測装置からの距離の情報を持つ画像である。
本実施形態１では、距離に基づいて算出される計測装置基準の座標系における３次元座標が保存されているものとする。例えば、座標系としては、２次元画像撮像部１００であるカメラのカメラ中心を原点とし、画像の水平方向をｘ軸、垂直方向をｙ軸、カメラの光軸をｚ軸とした座標系を採用することができる。
上記の距離画像に含まれる距離値から３次元点群への変換は、距離画像の各画素について、画素位置に対応する視線ベクトルに距離値を乗算することにより行う。この結果、物体の表面の点群の３次元座標値が得られる。
本実施形態１では、距離に基づいて算出される計測装置基準の座標系における３次元座標が保存されているものとする。例えば、座標系としては、２次元画像撮像部１００であるカメラのカメラ中心を原点とし、画像の水平方向をｘ軸、垂直方向をｙ軸、カメラの光軸をｚ軸とした座標系を採用することができる。
上記の距離画像に含まれる距離値から３次元点群への変換は、距離画像の各画素について、画素位置に対応する視線ベクトルに距離値を乗算することにより行う。この結果、物体の表面の点群の３次元座標値が得られる。
なお、距離センサとしては、液晶プロジェクタから物体に照射した２次元のパターン光をカメラで撮影し、三角測量により距離を計測するアクティブ方式のものを利用することができる。しかしながら、距離センサはこれに限るものではなく、光切断法やTime-of-flight方式など他のアクティブ式のものや、ステレオカメラによるパッシブ式など、距離画像を出力可能であればいかなる距離センサであってもよい。本実施形態１では、距離センサ用のカメラと２次元画像撮像部１００のカメラは同一のものを用い、同一視点における濃淡画像と距離画像が取得できるものとする。
他方、距離センサ用のカメラと２次元画像撮像部１００のカメラが異なっていてもよいが、この場合は、両者の相対的な位置及び姿勢を事前にキャリブレーションしておく。例えば、３次元形状が既知であるキャリブレーション物体を撮影し、濃淡画像に基づいた物体の位置及び姿勢と、距離画像に基づいた物体の位置及び姿勢から、相対的な位置及び姿勢を求めてもよい。
距離計測部１０５が計測した距離画像は、位置姿勢計測装置１０１中の３次元データ入力部１５０に入力されることで取得される。
モデル保存部１１０には、位置及び姿勢を計測する対象である物体の３次元形状モデル及び各姿勢ごとの主平面の情報等が保存されている。モデル保存部１１０には、さらに、例えば、パターンマッチングに用いるテンプレート、その他の情報などが保存されていてもよい。本実施形態１では、物体を線分と面から構成される３次元形状モデルとして説明する。
距離計測部１０５が計測した距離画像は、位置姿勢計測装置１０１中の３次元データ入力部１５０に入力されることで取得される。
モデル保存部１１０には、位置及び姿勢を計測する対象である物体の３次元形状モデル及び各姿勢ごとの主平面の情報等が保存されている。モデル保存部１１０には、さらに、例えば、パターンマッチングに用いるテンプレート、その他の情報などが保存されていてもよい。本実施形態１では、物体を線分と面から構成される３次元形状モデルとして説明する。
＜３次元形状モデル＞
図２ないし図５は、本実施形態１において利用する３次元形状モデルを説明する図である。これらの図に示すように、３次元形状モデルは、点の集合、及び各点を結んで構成される線分の集合によって定義される。
図２Ａ，図２Ｂにおいては、計測対象である物体１０の３次元形状モデルが点Ｐ１〜点Ｐ１４の１４点から構成されている。図２Ａでは、計測対象である物体１０の３次元形状モデルの点が表面から描かれており、一方、図２Ｂでは、物体１０の裏面から描かれている。
図２ないし図５は、本実施形態１において利用する３次元形状モデルを説明する図である。これらの図に示すように、３次元形状モデルは、点の集合、及び各点を結んで構成される線分の集合によって定義される。
図２Ａ，図２Ｂにおいては、計測対象である物体１０の３次元形状モデルが点Ｐ１〜点Ｐ１４の１４点から構成されている。図２Ａでは、計測対象である物体１０の３次元形状モデルの点が表面から描かれており、一方、図２Ｂでは、物体１０の裏面から描かれている。
図３Ａ、図３Ｂにおいては、図２Ａ，図２Ｂと同じ計測対象物体１０の３次元形状モデルが線分Ｌ１〜Ｌ１６により構成されている。図３Ａでは、物体１０の３次元形状モデルの線分が表面から描かれており、一方、図３Ｂでは、物体１０の裏面から描かれている。
図４においては、物体１０の３次元形状モデルの点Ｐ１〜点Ｐ１４が３次元座標値によって表されている。また、図５においては、物体１０の３次元形状モデルの線分Ｌ１〜Ｌ１６が線分を構成する両端の点のＩＤによって表されている。
図４においては、物体１０の３次元形状モデルの点Ｐ１〜点Ｐ１４が３次元座標値によって表されている。また、図５においては、物体１０の３次元形状モデルの線分Ｌ１〜Ｌ１６が線分を構成する両端の点のＩＤによって表されている。
本実施形態１において、さらに３次元形状モデルは、面の情報を備える。
すなわち、計測対象物体１０の各面は、各面を構成する点のＩＤによって表される。以下、単に物体１０という場合は、特に説明しない限り、計測対象物体１０を示すものとする。図２の３次元形状モデルでは、直方体を構成する６つの面の情報が記憶されている。「主平面」、すなわち物体の現在の位置及び姿勢における最も広い平面、の情報としては、任意の姿勢と主平面となる面の関係を備えることができる。具体的には、物体１０の各面が、その面自身が主平面となる姿勢の範囲を保持するようにする。このように構成される３次元形状モデルは、第１位置姿勢算出部１６０及び第２位置姿勢算出部１６５において、物体１０の位置及び姿勢を算出する際に用いられる。
すなわち、計測対象物体１０の各面は、各面を構成する点のＩＤによって表される。以下、単に物体１０という場合は、特に説明しない限り、計測対象物体１０を示すものとする。図２の３次元形状モデルでは、直方体を構成する６つの面の情報が記憶されている。「主平面」、すなわち物体の現在の位置及び姿勢における最も広い平面、の情報としては、任意の姿勢と主平面となる面の関係を備えることができる。具体的には、物体１０の各面が、その面自身が主平面となる姿勢の範囲を保持するようにする。このように構成される３次元形状モデルは、第１位置姿勢算出部１６０及び第２位置姿勢算出部１６５において、物体１０の位置及び姿勢を算出する際に用いられる。
第１位置姿勢算出部１６０は、３次元データ入力部１５０によって入力された３次元点群と、モデル保存部１１０に保存される３次元形状モデルとを用いて、物体１０の第１の位置及び姿勢を計測（推定）する。この第１の位置及び姿勢とは、例えば、物体１０の主平面の位置及び姿勢である。
第２位置姿勢算出部１６５は、第１位置姿勢算出部１６０から出力された第１の位置及び姿勢をもとに、物体１０の第２の位置及び姿勢を計測（推定）する。この第２の位置及び姿勢の計測において、第２位置姿勢算出部１６５は、２次元画像入力部１３０によって入力された２次元画像と、モデル保存部１１０に保存される３次元形状モデルとを用いる。この第２の位置及び姿勢は、例えば、最終的に計測したい物体１０の位置及び姿勢である。
第２位置姿勢算出部１６５は、第１位置姿勢算出部１６０から出力された第１の位置及び姿勢をもとに、物体１０の第２の位置及び姿勢を計測（推定）する。この第２の位置及び姿勢の計測において、第２位置姿勢算出部１６５は、２次元画像入力部１３０によって入力された２次元画像と、モデル保存部１１０に保存される３次元形状モデルとを用いる。この第２の位置及び姿勢は、例えば、最終的に計測したい物体１０の位置及び姿勢である。
＜位置及び姿勢の計測処理手順概要＞
図６は、本実施形態１における物体１０の位置及び姿勢の計測の処理手順の概略を表すフローチャートである。
まず、物体１０の位置及び姿勢を算出するための計測データを取得する（Ｓ６１０）。この計測データは、物体１０の濃淡画像と距離画像を含む。具体的には、２次元画像撮像部１００によって撮影された濃淡画像を、位置姿勢計測装置１０１の２次元画像入力部１３０が取得する。また、距離計測部１０５から出力された距離画像を、位置姿勢計測装置１０１の３次元データ入力部１５０が取得する。
Ｓ６１０で取得した距離画像の情報に基づいて、第１位置姿勢算出部１６０が、物体１０の第１の位置及び姿勢の算出を行う（Ｓ６２０）。具体的には、第１位置姿勢算出部１６０は、距離画像から得られる物体１０の表面上の３次元点群データに、モデル保存部１１０に保存される物体の３次元形状モデルの主平面が当てはまるように、物体１０の第１の位置及び姿勢を算出する。
図６は、本実施形態１における物体１０の位置及び姿勢の計測の処理手順の概略を表すフローチャートである。
まず、物体１０の位置及び姿勢を算出するための計測データを取得する（Ｓ６１０）。この計測データは、物体１０の濃淡画像と距離画像を含む。具体的には、２次元画像撮像部１００によって撮影された濃淡画像を、位置姿勢計測装置１０１の２次元画像入力部１３０が取得する。また、距離計測部１０５から出力された距離画像を、位置姿勢計測装置１０１の３次元データ入力部１５０が取得する。
Ｓ６１０で取得した距離画像の情報に基づいて、第１位置姿勢算出部１６０が、物体１０の第１の位置及び姿勢の算出を行う（Ｓ６２０）。具体的には、第１位置姿勢算出部１６０は、距離画像から得られる物体１０の表面上の３次元点群データに、モデル保存部１１０に保存される物体の３次元形状モデルの主平面が当てはまるように、物体１０の第１の位置及び姿勢を算出する。
次に、物体１０の第１の位置及び姿勢、すなわち、ステップＳ６２０で更新された主平面の位置及び姿勢の算出結果、を用いて、第２位置姿勢算出部１６５が、物体１０の第２の位置及び姿勢の推定を行う（Ｓ６３０）。具体的には、濃淡画像である２次元画像に対して、物体１０の３次元形状モデルが当てはまるように、物体１０の第２の位置及び姿勢を推定することで第の位置及び姿勢を計測する。この第２の位置及び姿勢が、最終的に求めたい物体１０の位置及び姿勢となり得る位置及び姿勢である。
＜第１の位置及び姿勢算出処理の詳細＞
図７は、図６のＳ６２０における第１の位置及び姿勢の計測の処理手順詳細を表すフローチャートである。ここでは、第１位置姿勢算出部が、計測対象である物体１０の３次元形状モデルの平面と、上記距離画像から得られる物体表面上の３次元点群の３次元空間中における誤差の二乗和が最小になるように、位置及び姿勢の最適化を行う。
物体１０の位置及び姿勢の算出をする方法は、例えばＲＡＮＳＡＣ（RANdom SAmple Consensus）アルゴリズムなどの、はずれ値に対してロバストな方法を用いてもよい（M.Armstrong and A. Zisserman, “Robust object tracking” in Proceedings of Asian Conference on Computer Vision, pp. 58-62, 1995.）。
図７は、図６のＳ６２０における第１の位置及び姿勢の計測の処理手順詳細を表すフローチャートである。ここでは、第１位置姿勢算出部が、計測対象である物体１０の３次元形状モデルの平面と、上記距離画像から得られる物体表面上の３次元点群の３次元空間中における誤差の二乗和が最小になるように、位置及び姿勢の最適化を行う。
物体１０の位置及び姿勢の算出をする方法は、例えばＲＡＮＳＡＣ（RANdom SAmple Consensus）アルゴリズムなどの、はずれ値に対してロバストな方法を用いてもよい（M.Armstrong and A. Zisserman, “Robust object tracking” in Proceedings of Asian Conference on Computer Vision, pp. 58-62, 1995.）。
以下の説明では、３次元形状モデルの主平面の位置および姿勢の３自由度を未知パラメータとして推定する。これら３つのパラメータを３次元ベクトルｓｐで表す。ｓｐは、位置を表す主平面の法線方向の位置の１自由度分の要素（成分）（位置の成分）と、主平面の傾きを表す２自由度分の要素（成分）（主平面と平行な方向周りの回転による２自由度の姿勢の成分）からなる。
まず、第１位置姿勢算出部１６０は、物体１０の位置及び姿勢の概略値を取得する（Ｓ７１０）。本実施形態１では、物体１０が置かれているおおよその位置や姿勢が既知であり、あらかじめ与えられている（所与である）ことを前提としており、その値を概略値（初期値）として用いる。この概略値は、外部の所定の装置から与えられてもよいし、位置姿勢計測装置１０１の操作者がキーボードやタッチパネル等の入力手段を用いて与えてもよい。なお、物体１０の位置及び姿勢の概略値は、予め与えられている場合だけでなく、他の手法で適宜取得することもできる。
まず、第１位置姿勢算出部１６０は、物体１０の位置及び姿勢の概略値を取得する（Ｓ７１０）。本実施形態１では、物体１０が置かれているおおよその位置や姿勢が既知であり、あらかじめ与えられている（所与である）ことを前提としており、その値を概略値（初期値）として用いる。この概略値は、外部の所定の装置から与えられてもよいし、位置姿勢計測装置１０１の操作者がキーボードやタッチパネル等の入力手段を用いて与えてもよい。なお、物体１０の位置及び姿勢の概略値は、予め与えられている場合だけでなく、他の手法で適宜取得することもできる。
例えば、位置姿勢計測装置１０１は、時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を、概略の位置及び姿勢として用いてもよい。この場合は、前回の計測値を記憶する記憶手段を設けて保存しておいてよい。当該記憶手段は、第１位置姿勢算出部１６０の外部に設けられてもよく、モデル保存部１１０等と記憶手段を共用してもよい。
あるいは、過去の位置及び姿勢の計測結果をもとに、位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測したものを概略値として用いてもよい。
あるいは、過去の位置及び姿勢の計測結果をもとに、位置及び姿勢の変化量を推定し、過去の位置及び姿勢と推定された変化量から現在の位置及び姿勢を予測したものを概略値として用いてもよい。
次に、図６のＳ６１０で入力された距離画像と、モデル保存部１１０が保存する３次元形状モデルとの対応付けを行う。具体的には、物体の３次元形状モデル（ポリゴンモデル）を、入力された距離画像上に投影することによって対応付けを行う（Ｓ７２０）。
この対応付けの方法の一例は、大石、中澤、池内、“インデックス画像を用いた複数距離画像の高速同時位置合せ”, 電子情報通信学会論文誌Ｄ, Vol.J89-D, No.3, pp.513-521, 2006.に開示されている。
ここでは、距離画像と同サイズの画像バッファを準備し、当該画像バッファに対して、校正済みであるカメラの内部パラメータと、位置及び姿勢の概略値とを用いてポリゴンモデルを描画していく。以下、ポリゴンモデルを描画した画像を「ポリゴン描画画像」と呼ぶ。
この対応付けの方法の一例は、大石、中澤、池内、“インデックス画像を用いた複数距離画像の高速同時位置合せ”, 電子情報通信学会論文誌Ｄ, Vol.J89-D, No.3, pp.513-521, 2006.に開示されている。
ここでは、距離画像と同サイズの画像バッファを準備し、当該画像バッファに対して、校正済みであるカメラの内部パラメータと、位置及び姿勢の概略値とを用いてポリゴンモデルを描画していく。以下、ポリゴンモデルを描画した画像を「ポリゴン描画画像」と呼ぶ。
描画する際には、各ポリゴンに固有の色を割り当てて描画を行う。ポリゴンの色は例えばＲＧＢの各色で表す。背景部分と区別するために、黒（Ｒ＝０、Ｇ＝０、Ｂ＝０）以外の色を用いてポリゴンを描画する。このような描画処理によって、距離画像の各画素に対応するポリゴンを、画像の色によって同定することができる。
なお、ポリゴンの描画処理をＧＰＵ（Graphics Processing Unit）上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデルを描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている３次元座標とポリゴン描画画像上の色に相当するポリゴンとを対応付ける。この対応付け方法は、広い平面であるほど正しく対応づけられる可能性が高くなるため、本実施形態１の図６のＳ６２０では、物体１０の主平面のみの位置及び姿勢の算出をまず実行する。
なお、ポリゴンの描画処理をＧＰＵ（Graphics Processing Unit）上で高速に行い、描画された結果の画像を読み出してもよい。ポリゴンモデルを描画した後、ポリゴン描画画像の各画素の値を読み取る。黒以外の値が保存されている画素については、距離画像のその画素に保持されている３次元座標とポリゴン描画画像上の色に相当するポリゴンとを対応付ける。この対応付け方法は、広い平面であるほど正しく対応づけられる可能性が高くなるため、本実施形態１の図６のＳ６２０では、物体１０の主平面のみの位置及び姿勢の算出をまず実行する。
次に、ポリゴンと３次元点の対応付けの結果、それらの間の残差を最小化するような主平面の位置及び姿勢を算出する（Ｓ７３０）。
具体的には、まず、ポリゴンの位置及び姿勢の変化による変動と、対応付け結果の残差の変化と、の間の相関を表す係数行列を算出する。さらに、対応付け結果の残差を表す誤差ベクトルを算出する。なお、物体１０の３次元形状モデルに登録された各面は、その面自身が主平面となる姿勢の範囲を保持している。このため、概略位置姿勢が各面に登録された姿勢範囲に入っている場合に主平面と判断することで主平面を特定し、その面のＩＤを取得する。
具体的には、まず、ポリゴンの位置及び姿勢の変化による変動と、対応付け結果の残差の変化と、の間の相関を表す係数行列を算出する。さらに、対応付け結果の残差を表す誤差ベクトルを算出する。なお、物体１０の３次元形状モデルに登録された各面は、その面自身が主平面となる姿勢の範囲を保持している。このため、概略位置姿勢が各面に登録された姿勢範囲に入っている場合に主平面と判断することで主平面を特定し、その面のＩＤを取得する。
３次元空間における平面の方程式は、一般にax+by+cz＝e（a2+b2+c2=1）の形で表される。
ここでは、３次元形状モデル中のある平面の計測物体の座標系における方程式を、ax+by+cz＝eであるとする。なお、計測物体の座標系は、以下、計測物体座標系と称する。この平面の方程式は、位置及び姿勢の概略値に基づいて、位置姿勢を計測する側を基準とした座標系である基準座標系における平面の方程式a'x+b'y+c'z＝e'に変換できる。位置及び姿勢の概略値に基づいて、計測物体座標系から基準座標系への座標変換を行う式は、
ここでは、３次元形状モデル中のある平面の計測物体の座標系における方程式を、ax+by+cz＝eであるとする。なお、計測物体の座標系は、以下、計測物体座標系と称する。この平面の方程式は、位置及び姿勢の概略値に基づいて、位置姿勢を計測する側を基準とした座標系である基準座標系における平面の方程式a'x+b'y+c'z＝e'に変換できる。位置及び姿勢の概略値に基づいて、計測物体座標系から基準座標系への座標変換を行う式は、
このとき、基準座標系における平面の方程式の係数は次式のように表される。
平面の座標変換については、例えば金谷「形状ＣＡＤと図形の数学」（共立出版）で詳しく説明がなされている。基準座標系における３次元座標の計測値が（x'、y'、z'）である点Pが３次元形状モデル中のある面Fと対応付けられているとする。面Fに点Pから下ろした垂線の足をQとする。点Qの基準座標系における３次元座標（x、y、z）は、物体１０の位置及び姿勢ｓｐによって変換するものである。
そして、物体１０が、ある位置及び姿勢ｓｐである場合に、３次元座標（x0、y0、z0）の近傍で１次のテイラー展開によって、下記の式３のように近似できる。
そして、物体１０が、ある位置及び姿勢ｓｐである場合に、３次元座標（x0、y0、z0）の近傍で１次のテイラー展開によって、下記の式３のように近似できる。
正しいｓｐによって、式３により変換される（x、y、z）は、法線ベクトルが面Fの法線ベクトルと同様であり、かつ、点Pを通る平面（a'x+b'y+c'z＝e'、 e'=a'x'+b'y'+c'z'） 上にあると仮定することができる。
この仮定によれば、式３を平面の方程式に代入することによって、下記式４が得られる。この式４は観測方程式と呼ばれる。
この仮定によれば、式３を平面の方程式に代入することによって、下記式４が得られる。この式４は観測方程式と呼ばれる。
式４の観測方程式はｓｐの各成分の微小変化Δｓｐｉi（i=1、2、3）についての方程式になっており、式４の観測方程式を全ての点群データについて立式すると、式６に示すようなΔｓｐｉに関する線形連立方程式を立てることができる。
この式７の左辺のJpが求める係数行列であり、右辺のEpが誤差ベクトルである。この式７をもとにして、行列Jpの一般化逆行列（Jp T・Jp）−1・Jp Tを用いて位置及び姿勢の補正値ΔSpを最小二乗基準で求める。
しかしながら、距離画像の対応付けデータ中に、誤った対応付けその他の原因による外れ値が混入することを完全に避けることは困難である。そこで、本実施形態１では、次に述べるようなロバスト推定手法を用いることができる。
一般に、外れ値を生じるような対応付けにおいては、式６の右辺の誤差ベクトルの値が大きくなる。そこで、本実施形態１においては、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制している。本実施形態１では、重みとして、例えば次の式８に示すようなＴｕｋｅｙの関数により与えることができる。
しかしながら、距離画像の対応付けデータ中に、誤った対応付けその他の原因による外れ値が混入することを完全に避けることは困難である。そこで、本実施形態１では、次に述べるようなロバスト推定手法を用いることができる。
一般に、外れ値を生じるような対応付けにおいては、式６の右辺の誤差ベクトルの値が大きくなる。そこで、本実施形態１においては、誤差の絶対値が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与えるようにして、誤差の大きいデータの影響を抑制している。本実施形態１では、重みとして、例えば次の式８に示すようなＴｕｋｅｙの関数により与えることができる。
ここで、c1は定数であり、いわば誤差の許容範囲を示す。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差が大きいデータには小さな重みを与え、誤差が小さいデータには大きな重みを与える関数であればどのようなものでもよい。
各計測データに対応する重みをwiとする。ここで式９のように重み行列Wpを定義する。
各計測データに対応する重みをwiとする。ここで式９のように重み行列Wpを定義する。
この重み行列Wpを用いて、式７を式１０のように変形する。
補正された主平面の位置及び姿勢を用いて収束判定を行い、収束していれば補正を完了する（Ｓ７５０）。収束していない場合は、Ｓ７３０に戻り、補正を繰り返す。収束判定は、例えば、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない一定値になった場合に収束したと判定することができる。また、例えばΔｓが所定の基準値未満になった場合に収束すると判定してもよい。このようにして、第１の位置及び姿勢の算出処理が完了する。
＜第２の位置及び姿勢算出処理ステップＳ６３０の詳細＞
図８は、図６のステップＳ６３０における第２の位置及び姿勢の計測の処理手順詳細を表すフローチャートである。
本実施形態１では、第２位置姿勢算出部１６５が、濃淡画像上のエッジと推定される位置及び物体１０の姿勢に基づいて、濃淡画像上に投影される線分との距離の二乗和が最小になるように、物体１０の位置及び姿勢の最適化を行うことで物体１０の第２の位置及び姿勢の推定が行われる。
図８は、図６のステップＳ６３０における第２の位置及び姿勢の計測の処理手順詳細を表すフローチャートである。
本実施形態１では、第２位置姿勢算出部１６５が、濃淡画像上のエッジと推定される位置及び物体１０の姿勢に基づいて、濃淡画像上に投影される線分との距離の二乗和が最小になるように、物体１０の位置及び姿勢の最適化を行うことで物体１０の第２の位置及び姿勢の推定が行われる。
まず、図６のＳ６２０で得られた計測対象物体１０の主平面の位置及び姿勢（第１の位置及び姿勢）を概略値（初期値）として入力する（Ｓ８１０）。
次に、図６のＳ６１０において入力された濃淡画像において画像特徴の検出を行い、検出した画像特徴と、モデル保存部１１０が保存する物体１０の３次元形状モデルとの対応付けを行う（Ｓ８２０）。すなわち、物体１０の画像特徴と、３次元形状モデルの幾何特徴と、の照合、対応付けを行う。
本実施形態１では、画像特徴としてエッジを検出する。このエッジは、濃度勾配が極値となる点（又はその点の集合）である。本実施形態１では、例えば、非特許文献１に記載されている方法によって、画像の特徴としてエッジ検出を行うことができる。同文献が開示するエッジ検出方法は、３次元形状モデル側から対応するエッジを探索する方法であるため、検出と対応付けが一度に実行される。
次に、図６のＳ６１０において入力された濃淡画像において画像特徴の検出を行い、検出した画像特徴と、モデル保存部１１０が保存する物体１０の３次元形状モデルとの対応付けを行う（Ｓ８２０）。すなわち、物体１０の画像特徴と、３次元形状モデルの幾何特徴と、の照合、対応付けを行う。
本実施形態１では、画像特徴としてエッジを検出する。このエッジは、濃度勾配が極値となる点（又はその点の集合）である。本実施形態１では、例えば、非特許文献１に記載されている方法によって、画像の特徴としてエッジ検出を行うことができる。同文献が開示するエッジ検出方法は、３次元形状モデル側から対応するエッジを探索する方法であるため、検出と対応付けが一度に実行される。
図９は、本実施形態１におけるエッジ検出を説明する図である。図９では、３次元形状モデルを構成する各線分の濃淡画像上への投影像を算出する様子が示されている。この投影像の算出は、計測対象物体１０の主平面の位置及び姿勢の概略値と、校正済みのカメラの内部パラメータとを用いて行われる。また、計測対象物体１０の主平面の位置及び姿勢の概略値は、図８のＳ８１０において入力された概略値である。
図９に示されように、線分の濃淡画像への投影像は、その濃淡画像上でも線分（投影された線分９００）となる。
図９に示されように、線分の濃淡画像への投影像は、その濃淡画像上でも線分（投影された線分９００）となる。
次に、濃淡画像上で等間隔になるように投影された線分９００上で、制御点９１０を設定する。そして、この制御点９１０を通り、かつ、投影された線分９００に直交する線分９２０（探索ライン）上で１次元のエッジ検出を行い、対応する点であるエッジ９３０を検出する。
図１０は、このエッジ検出の原理を示すグラフを表す図である。エッジ９３０は、画素値の濃度勾配が極値をとる位置の点として検出される。したがって、図１０に示すように、近傍に、濃度勾配が極値１０００を取る点が複数存在する場合には、複数のエッジ９３０が検出されることがある。本実施形態１では、検出されるエッジ９３０のうち最も濃度勾配の大きいものを対応するエッジ９３０と見なしている。
図１０は、このエッジ検出の原理を示すグラフを表す図である。エッジ９３０は、画素値の濃度勾配が極値をとる位置の点として検出される。したがって、図１０に示すように、近傍に、濃度勾配が極値１０００を取る点が複数存在する場合には、複数のエッジ９３０が検出されることがある。本実施形態１では、検出されるエッジ９３０のうち最も濃度勾配の大きいものを対応するエッジ９３０と見なしている。
図８に戻り、３次元形状モデルの線分（投影された線分９００）上の制御点９１０と、濃淡画像上で検出されたエッジ９３０との対応付け結果の残差を最小化するような位置及び姿勢を算出する（Ｓ８３０）。このように、本実施形態１では、画像特徴としての検出されたエッジと、３次元モデル上のエッジを投影した線分（幾何特徴）とが照合され、対応付けがなされる。
まず、３次元形状モデルの線分である投影された線分９００上の制御点９１０の位置及び姿勢の変化による変動と残差の変化の相関を表す係数行列と、対応付け結果の残差を表す誤差ベクトルとを算出する。
図１１は、線分の投影像と検出されたエッジの関係を説明する図である。図１１では、濃淡画像の水平方向、垂直方向をそれぞれu軸、v軸と定義している。位置及び姿勢の概略値ｓ０に基づいて投影された線分９００上のある制御点９１０の濃淡画像上での位置を（u0、v0）、該制御点９１０が属する線分９００の画像上での傾きをu軸に対する傾きθで表す。傾きθは、線分の両端の３次元座標をｓの概略値（ｓ０）に基づいて濃淡画像上に投影し、濃淡画像上での両端の座標を結んだ直線の傾きとして算出する。該線分（投影された線分９００）の濃淡画像上での法線ベクトルは（ｓｉｎθ、−ｃｏｓθ）となる。また、該制御点９１０の対応点（エッジ）９３０の画像座標を（u’、v’）とする。ここで、点（u’、v’）を通り、傾きがθである直線が図１１において破線で示されている。
この破線上の任意の点（u、v）は、
図１１は、線分の投影像と検出されたエッジの関係を説明する図である。図１１では、濃淡画像の水平方向、垂直方向をそれぞれu軸、v軸と定義している。位置及び姿勢の概略値ｓ０に基づいて投影された線分９００上のある制御点９１０の濃淡画像上での位置を（u0、v0）、該制御点９１０が属する線分９００の画像上での傾きをu軸に対する傾きθで表す。傾きθは、線分の両端の３次元座標をｓの概略値（ｓ０）に基づいて濃淡画像上に投影し、濃淡画像上での両端の座標を結んだ直線の傾きとして算出する。該線分（投影された線分９００）の濃淡画像上での法線ベクトルは（ｓｉｎθ、−ｃｏｓθ）となる。また、該制御点９１０の対応点（エッジ）９３０の画像座標を（u’、v’）とする。ここで、点（u’、v’）を通り、傾きがθである直線が図１１において破線で示されている。
この破線上の任意の点（u、v）は、
制御点９１０の画像座標（u、v）は（u0、v0）の近傍で１次のテイラー展開によって式１５のように近似できる。但しΔｓｉ（i=1、2、 … 、6）はｓの各成分の微小変化を表す。
式１６の観測方程式は対応付けが行われたすべての制御点９１０について立式することができる。式１６はｓの各成分の微小変化Δｓｉ（i=1、2、 … 、6）に関する方程式になっているので、複数の制御点９１０について立式することによって、Δｓｉに関する線形連立方程式を立てることができる。
これらの線形連立方程式を合わせると式１８のように表すことができる。
ここで、左辺のJが求める係数行列であり、右辺のEが誤差ベクトルである。
式１９をもとに、位置及び姿勢の補正値Δｓを算出する。しかし、エッジ９３０の対応付けデータに誤対応などの外れ値が混入する可能性がある。
そのため、Ｓ７３０で述べたようなロバスト推定手法により重み行列Wを用いて式２０によって、補正値Δｓを算出する。
式１９をもとに、位置及び姿勢の補正値Δｓを算出する。しかし、エッジ９３０の対応付けデータに誤対応などの外れ値が混入する可能性がある。
そのため、Ｓ７３０で述べたようなロバスト推定手法により重み行列Wを用いて式２０によって、補正値Δｓを算出する。
次に、位置及び姿勢の値の収束判定を行う（Ｓ８５０）。収束していれば、図８の処理を終了する。終了時に得られる位置及び姿勢がＳ６３０における第２の位置及び姿勢であり、これを最終的に求めようとする物体１０の位置及び姿勢としてよい。
他方、収束していない場合は、Ｓ８３０に戻って、残差の最小化の処理からふたたび処理を繰り返す。収束判定は、図７のＳ７５０と同様、種々の手法を採用することができる。例えば、補正値Δｓがほぼ０であると判断する場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定してもよい。また、例えば、補正値Δｓが所定の基準値未満になった場合に収束したと判定してもよい。
他方、収束していない場合は、Ｓ８３０に戻って、残差の最小化の処理からふたたび処理を繰り返す。収束判定は、図７のＳ７５０と同様、種々の手法を採用することができる。例えば、補正値Δｓがほぼ０であると判断する場合や、誤差ベクトルの二乗和が補正前と補正後でほとんど変わらない場合に収束したと判定してもよい。また、例えば、補正値Δｓが所定の基準値未満になった場合に収束したと判定してもよい。
以上述べたように、本実施形態１では、まず、距離画像を用いて物体の主平面の位置及び姿勢を、第１の位置及び姿勢として推定する。次に、３次元モデル中の各線分を画像上に投影し、当該線分の近傍で対応するエッジを探索する対応付けを行うことによって、第２の位置及び姿勢を推定する。正しいスケールで対応付けを決定することができるので、本実施形態１によれば、誤った対応付けをより低減することができる。その結果、物体１０の位置及び姿勢の概略値（初期値）のずれに対して、よりロバストな推定をすることができる。
（実施形態１−変形例１）
実施形態１の変形例１は、主平面に関する３自由度以外の位置及び姿勢の成分を、濃淡画像とモデルから算出するものである。
上述した実施形態１では、前段の処理において、平面の位置及び姿勢の３自由度を推定した後、後段の処理において、濃淡画像と距離画像の両方を利用して計測対象である物体１０の位置及び姿勢を６自由度すべて推定した。
実施形態１の変形例１は、主平面に関する３自由度以外の位置及び姿勢の成分を、濃淡画像とモデルから算出するものである。
上述した実施形態１では、前段の処理において、平面の位置及び姿勢の３自由度を推定した後、後段の処理において、濃淡画像と距離画像の両方を利用して計測対象である物体１０の位置及び姿勢を６自由度すべて推定した。
これに替えて、距離画像から算出される計測対象物体１０が有する平面の位置及び姿勢の精度が高い場合などは、後段の処理においては、平面の位置及び姿勢は固定して平面に対する位置および姿勢を推定してもよい。例えば、具体的には図８のＳ８３０において、３次元形状モデルの主平面の位置および姿勢の３自由度以外の自由度を未知パラメータとして推定する。このパラメータを、例えば３次元ベクトルｓで表すことができる。３次元ベクトルｓは、主平面と水平な位置を表す２自由度分の要素（成分）と、主平面の法線の向きを回転軸とした回転（角度）を表す１自由度分の要素（成分）からなる。このｓを用いて図８のＳ８３０を同様に処理すればよい。
（実施形態１−変形例２）
実施形態１の変形例２は、第２の位置及び姿勢の計算に濃淡画像と距離画像を併用するものである。
実施形態１では、図６のＳ６３０において、第２の位置及び姿勢の算出時に濃淡画像のみを用いた。これに替えて、距離画像と濃淡画像の両方を用いてもよい。この場合、３次元データ入力部１５０から得られる距離画像を、第２位置姿勢算出部１６５でも用いるように構成する。そのうえで、例えば、図８のＳ８３０の説明中の連立方程式（式１８）の算出時に、式１６の条件以外の条件も算出することができる。例えば、式１６だけでなく、Ｓ７３０の説明中の式４の条件も位置及び姿勢の６自由度のパラメータに関して算出し、下記の式２２を係数行列として用いてよい。
実施形態１の変形例２は、第２の位置及び姿勢の計算に濃淡画像と距離画像を併用するものである。
実施形態１では、図６のＳ６３０において、第２の位置及び姿勢の算出時に濃淡画像のみを用いた。これに替えて、距離画像と濃淡画像の両方を用いてもよい。この場合、３次元データ入力部１５０から得られる距離画像を、第２位置姿勢算出部１６５でも用いるように構成する。そのうえで、例えば、図８のＳ８３０の説明中の連立方程式（式１８）の算出時に、式１６の条件以外の条件も算出することができる。例えば、式１６だけでなく、Ｓ７３０の説明中の式４の条件も位置及び姿勢の６自由度のパラメータに関して算出し、下記の式２２を係数行列として用いてよい。
（実施形態２）
以下、本発明の実施形態２を、実施形態１と異なる点について説明する。
本実施形態２においては、まず第１の位置姿勢推定において、距離画像から主平面に相当する平面を検出する。そして、第２の位置姿勢推定において、求めた物体１０の第１の位置及び姿勢に基づいて検出した平面が、正面向きになるように濃淡画像を補正して、パターンマッチングにより物体１０の第２の位置及び姿勢を算出する。
以下、本発明の実施形態２を、実施形態１と異なる点について説明する。
本実施形態２においては、まず第１の位置姿勢推定において、距離画像から主平面に相当する平面を検出する。そして、第２の位置姿勢推定において、求めた物体１０の第１の位置及び姿勢に基づいて検出した平面が、正面向きになるように濃淡画像を補正して、パターンマッチングにより物体１０の第２の位置及び姿勢を算出する。
すなわち、実施形態２では、まず距離画像から主平面に相当する平面を検出し、当該平面がカメラの視軸と垂直になるように濃淡画像を補正する。当該補正後、例えば、パターンマッチングによって物体１０の位置及び姿勢を算出する。
図１２は、本実施形態２に係る位置姿勢計測装置１２０１の構成を説明する図である。図１２に示すように、位置姿勢計測装置１２０１は、モデル保存部１２１０、
２次元画像入力部１３０、３次元データ入力部１５０、第１位置姿勢算出部１２６０、第２位置姿勢算出部１２６５、とから構成されている。また、位置姿勢計測装置１２０１には、２次元画像撮像部１００と、距離計測部１０５とが接続されている。
図１２は、本実施形態２に係る位置姿勢計測装置１２０１の構成を説明する図である。図１２に示すように、位置姿勢計測装置１２０１は、モデル保存部１２１０、
２次元画像入力部１３０、３次元データ入力部１５０、第１位置姿勢算出部１２６０、第２位置姿勢算出部１２６５、とから構成されている。また、位置姿勢計測装置１２０１には、２次元画像撮像部１００と、距離計測部１０５とが接続されている。
以下、位置姿勢計測装置１２０１を構成する各部、及び該装置に接続する各部について説明する。但し、２次元画像撮像部１００、距離計測部１０５、２次元画像入力部１３０、３次元データ入力部１５０に関しては、実施形態１と同様の構成であるため、説明を省略する。
モデル保存部１２１０は、位置及び姿勢を計測する対象の物体１０をパターンマッチングによって検出する際に用いるテンプレートと、当該物体１０の主平面の情報とを保存する。本実施形態２において、このテンプレートは、ある位置及び姿勢における物体１０を画像として取得した場合の、当該画像の各画素の輝度値の情報を持つものとする。
モデル保存部１２１０は、位置及び姿勢を計測する対象の物体１０をパターンマッチングによって検出する際に用いるテンプレートと、当該物体１０の主平面の情報とを保存する。本実施形態２において、このテンプレートは、ある位置及び姿勢における物体１０を画像として取得した場合の、当該画像の各画素の輝度値の情報を持つものとする。
モデル保存部１２１０は、物体１０の様々な位置及び姿勢に対してテンプレートを保存している。これにより、所定の画像に対して、当該画像と相関の高いテンプレートをモデル保存部１２１０から選択することによって、選択されたテンプレートと対応づけられた位置及び姿勢を出力する。
本実施形態２では、テンプレートは、計測対象である物体１０の主平面が視軸と垂直となる場合の輝度値の情報を持つものとする。また、実施形態１と同様に、物体１０を表す３次元形状モデルは、当該物体１０の点の集合、及び、当該物体１０の面を構成する点のＩＤ、によって表現されている。さらに、当該物体１０の各面が、その面自身が主平面となる姿勢の範囲も、モデル保存部１２１０に保持されている。このように、その面が主平面となる姿勢の範囲もモデル保存部１２１０に情報として保持されていることによって、主平面の情報を保持している。テンプレートと、主平面の情報とは、第２位置姿勢算出部１２６５がその処理に用いる。
本実施形態２では、テンプレートは、計測対象である物体１０の主平面が視軸と垂直となる場合の輝度値の情報を持つものとする。また、実施形態１と同様に、物体１０を表す３次元形状モデルは、当該物体１０の点の集合、及び、当該物体１０の面を構成する点のＩＤ、によって表現されている。さらに、当該物体１０の各面が、その面自身が主平面となる姿勢の範囲も、モデル保存部１２１０に保持されている。このように、その面が主平面となる姿勢の範囲もモデル保存部１２１０に情報として保持されていることによって、主平面の情報を保持している。テンプレートと、主平面の情報とは、第２位置姿勢算出部１２６５がその処理に用いる。
第１位置姿勢算出部１２６０は、３次元データ入力部１２５０が入力した３次元点群に対して平面の検出を行い、検出された平面の位置及び姿勢を第１の位置及び姿勢として計測する。ここで検出された平面の位置及び姿勢とは、平面の法線方向の向きを定める２自由度の姿勢成分と平面の法線方向の１自由度の位置成分から成る。
第２位置姿勢算出部１２６５は、第１位置姿勢算出部１２６０が算出した第１の位置及び姿勢を用いて、２次元画像入力部１２３０が入力した２次元画像中の物体１０の主平面が視軸と垂直になるように画像を変換する。当該変換した画像に対して、モデル保存部２１０に保存されている計測する対象の物体１０のテンプレートをもとに、パターンマッチングを行い、物体１０の第２の位置及び姿勢を計測する。
第２位置姿勢算出部１２６５は、第１位置姿勢算出部１２６０が算出した第１の位置及び姿勢を用いて、２次元画像入力部１２３０が入力した２次元画像中の物体１０の主平面が視軸と垂直になるように画像を変換する。当該変換した画像に対して、モデル保存部２１０に保存されている計測する対象の物体１０のテンプレートをもとに、パターンマッチングを行い、物体１０の第２の位置及び姿勢を計測する。
実施形態２における位置及び姿勢の計測の処理手順を示すフローチャートは、実施形態１と同様に、図６に示されている。以下、順次説明する。
図６を参照して、まず、物体１０の位置及び姿勢を算出するための計測データを取得する（Ｓ６１０）。具体的には、物体１０の濃淡画像と距離画像とを取得する。位置姿勢計測装置１２０１中の２次元画像入力部１２３０が、２次元画像撮像部１２００が撮像した濃淡画像を取得する。また、３次元データ入力部１２５０が、距離計測部１２０５が出力する距離画像を取得する。
図６を参照して、まず、物体１０の位置及び姿勢を算出するための計測データを取得する（Ｓ６１０）。具体的には、物体１０の濃淡画像と距離画像とを取得する。位置姿勢計測装置１２０１中の２次元画像入力部１２３０が、２次元画像撮像部１２００が撮像した濃淡画像を取得する。また、３次元データ入力部１２５０が、距離計測部１２０５が出力する距離画像を取得する。
次に、取得された距離画像の情報に基づいて、第１位置姿勢算出部１２６０が、物体１０の第１の位置及び姿勢の計測を行う（Ｓ６２０）。このＳ６２０においては、取得された距離画像から平面を検出し、検出された平面の位置及び姿勢を、第１の位置及び姿勢として算出する。このような平面の算出処理には、例えば、Jiang X., Bunke H., “Fast segmentation of range images into planar regions by scan line grouping”, Machine Vision Application, Vol.7, Issue2, 1994.に記載されている手法を利用することができる。この文献に記載されている方法を用いて、距離画像を面ごとに分割し、検出された複数の面のうちで面積最大の平面の位置及び姿勢を出力するものとする。このように、面のセグメンテーションの方法は、例えば当該文献に記載されているが、他の方法を利用してもよい。
また、面積最大の平面を検出するには、距離画像のセグメンテーションによって得られた各セグメントを、面積の順に順位付けをすればよい。
次に、面積の大きい順番に、セグメントが平面かどうかを、例えば平面フィッティングにより確認する。平面フィッティングの結果、セグメントのすべての点が平面との距離がある閾値（例えば1mmなど）以下であれば、着目しているセグメントが平面であると判断する。以上のような処理によって、距離画像上の３次元点（点群）に基づき、面積最大の平面を検出する。
次に、面積の大きい順番に、セグメントが平面かどうかを、例えば平面フィッティングにより確認する。平面フィッティングの結果、セグメントのすべての点が平面との距離がある閾値（例えば1mmなど）以下であれば、着目しているセグメントが平面であると判断する。以上のような処理によって、距離画像上の３次元点（点群）に基づき、面積最大の平面を検出する。
しかしながら、これらの処理だけでは、例えば背景が平面の場合などは、当該背景を検出してしまい、誤検出が生じるおそれがある。そこで、平面フィッティングの結果、ある平面と検出されたセグメント中のすべての点のＺ座標が、ある閾値より大きくなっている場合に、当該平面は背景であると判断してもよい。この結果、当該平面以外の他の平面から検出結果を出力するように構成してもよい。なお、ある閾値は、例えば、対象の部品のＺ座標の概略位置姿勢が既知の場合、そのＺ座標の値などを利用してよい。
次に、第２位置姿勢算出部１２６５が、Ｓ６２０において算出された第１の位置及び姿勢と濃淡画像に基づいて、物体１０の第２の位置及び姿勢の計測を行う（Ｓ６３０）。本実施形態２では、このＳ６３０において、例えば、濃淡画像を、Ｓ６２０において検出された平面が視軸と垂直になるように変換する。すなわちこの変換は、Ｓ６２０において検出された平面が、物体１０の主平面と一致するものとする。そして、変換後の画像に対して、物体１０の主平面が視軸と垂直になっているパターンを用いてパターンマッチングを行い、物体１０の位置及び姿勢を算出する。ここで算出される位置及び姿勢を、本実施形態２における第２の位置及び姿勢としてよい。
図１３は、図６のＳ６３０における物体１０の第２の位置及び姿勢の計測の処理手順の、本実施形態２における処理詳細を示すフローチャートである。以下、図１３に示すＳ１３１０からＳ１３２０を参照して、図６に示すＳ６３０の詳細な動作を説明する。なお、図１３に示されたフローチャートで表される動作は、第２位致姿勢算出部１２６５が実行する。
まず、Ｓ６２０において算出された物体１０の第１の位置及び姿勢を用いて濃淡画像の変換を行う（Ｓ１３１０）。
ここでは、濃淡画像の現在の視点、すなわち変換前の視点を、以降、「旧視点」と呼ぶ。また、この旧視点の視軸とＳ６２０において検出した平面との交点を、以降、「視軸と平面の交点」と呼ぶ。そして、当該旧視点と、視軸と平面の交点と、の距離が変化せず、かつ、Ｓ６２０において検出した平面が当該視軸と垂直になるような視点を、「正面視点」と呼ぶ。
ここで、濃淡画像を撮像したカメラの内部パラメータは既知であり、濃淡画像の各座標に対応する３次元座標も既知であるので、これらパラメータを利用して、正面視点における画像を計算することができる。
ここでは、濃淡画像の現在の視点、すなわち変換前の視点を、以降、「旧視点」と呼ぶ。また、この旧視点の視軸とＳ６２０において検出した平面との交点を、以降、「視軸と平面の交点」と呼ぶ。そして、当該旧視点と、視軸と平面の交点と、の距離が変化せず、かつ、Ｓ６２０において検出した平面が当該視軸と垂直になるような視点を、「正面視点」と呼ぶ。
ここで、濃淡画像を撮像したカメラの内部パラメータは既知であり、濃淡画像の各座標に対応する３次元座標も既知であるので、これらパラメータを利用して、正面視点における画像を計算することができる。
正面視点から見た画像の計算は、以下のように実行してよい。
まず、正面視点の３次元空間上での座標は、視軸と平面の交点から、図６のＳ６２０において検出した平面の法線方向に、旧視点からの視軸と平面との交点の間の距離だけ移動した場所にある。
次に、座標軸を決定する。旧視点に対するx、y、z軸をそれぞれ決定する。ここでは、旧視点に対するx軸を、濃淡画像上で左から右向き、かつ、z軸と垂直の向きとする。旧視点に対するy軸を、濃淡画像上で上から下向き、かつ、z軸と垂直の向きとする。旧視点に対するz軸を、旧視点から濃淡画像上で奥の向き、と決定する。
同様に、正面視点に対する座標軸x’、 y’、z’軸それぞれ決定する。正面視点に対するz’軸は、正面視点から視軸と平面の交点への向きとする。正面視点に対するx’軸は、濃淡画像上で左から右向き、かつ、z’軸と垂直の向きとする。正面視点に対するy’軸は、濃淡画像上で上から下向き、かつ、z’軸と垂直の向きとする。
まず、正面視点の３次元空間上での座標は、視軸と平面の交点から、図６のＳ６２０において検出した平面の法線方向に、旧視点からの視軸と平面との交点の間の距離だけ移動した場所にある。
次に、座標軸を決定する。旧視点に対するx、y、z軸をそれぞれ決定する。ここでは、旧視点に対するx軸を、濃淡画像上で左から右向き、かつ、z軸と垂直の向きとする。旧視点に対するy軸を、濃淡画像上で上から下向き、かつ、z軸と垂直の向きとする。旧視点に対するz軸を、旧視点から濃淡画像上で奥の向き、と決定する。
同様に、正面視点に対する座標軸x’、 y’、z’軸それぞれ決定する。正面視点に対するz’軸は、正面視点から視軸と平面の交点への向きとする。正面視点に対するx’軸は、濃淡画像上で左から右向き、かつ、z’軸と垂直の向きとする。正面視点に対するy’軸は、濃淡画像上で上から下向き、かつ、z’軸と垂直の向きとする。
次に、距離画像上の各点の座標値を、x、y、z軸で表されている座標値から、x’、y’、z’軸に関する座標値に変換する。そして、このx’、y’、z’軸の座標値で表された距離画像上の各点に対して、カメラの内部パラメータを用いて新たに画像上の座標を計算する。当該計算によって、正面視点から見た場合の画像を計算して求めることができる。画素によっては値が決定できない場合もあるが、そのような画素は数値が未定義であるものとして、パターンマッチングの際に用いないようにすればよい。
図１３に戻り、Ｓ１３１０において変換した濃淡画像に対して、モデル保存部１２１０に保存されているテンプレートを用いて、パターンマッチングを行い、第２の位置および姿勢の推定を行う（Ｓ１３２０）。
ここで、パターンマッチングの方法は、様々なパターンマッチングの方法を利用してよい。本実施形態２では、例えば正規化相関法により計算することができる。この正規化相関法では、テンプレートtと、テンプレートtと同じサイズの画像Iに対して以下のような評価値Rを算出する。
図１３に戻り、Ｓ１３１０において変換した濃淡画像に対して、モデル保存部１２１０に保存されているテンプレートを用いて、パターンマッチングを行い、第２の位置および姿勢の推定を行う（Ｓ１３２０）。
ここで、パターンマッチングの方法は、様々なパターンマッチングの方法を利用してよい。本実施形態２では、例えば正規化相関法により計算することができる。この正規化相関法では、テンプレートtと、テンプレートtと同じサイズの画像Iに対して以下のような評価値Rを算出する。
濃淡画像からテンプレートtと同じサイズの画像I’を抜き出し、上記式を用いて評価値Rを算出することができる。I’の抜き出し方は、要求される精度に依存する。例えば、要求される精度がa画素であり、テンプレートtの高さがh画素、幅がw画素の場合、I’の頂点が（ma、 na）、（ma+w、na）、（ma、na+h）、（ma+w、na+h）（m、nは整数）となるすべての抜き出し方において評価値Rを算出する。算出したすべての評価値のうち、最も値の高くなる評価値を出力するような、テンプレートtに対応している位置及び姿勢を算出してよい。当該算出した位置及び姿勢を、第２の位置及び姿勢として出力してよい。
以上説明したように、本実施形態２では、まず距離画像を用いて距離画像上の平面を検出する。次に、その平面が、視線と垂直になるように、濃淡画像を変換する。このため、計測対象である物体１０の、テンプレートとして保存される位置及び姿勢は、主平面と視線が垂直になっているのみでよい。したがって、モデル保存部１２１０において保存されているテンプレートの量が少なくてもなお、ロバストかつ精度の高い位置及び姿勢の推定が可能になる。
（実施形態２−変形例１）
実施形態２の変形例１では、パターンマッチングを実行した後に、精度が向上するフィッティングを行う。
この変形例１では、上述の実施形態２についての図６のＳ６３０の処理による出力（すなわち、第２の位置及び姿勢）を、物体１０の概略の位置姿勢として、上述の実施形態１についてのＳ６３０の処理を行う。これにより、物体１０の位置及び姿勢の推定の精度を高めることができる。あるいは、パターンマッチングにおけるテンプレートのバリエーション数を減らしても、推定の精度を維持することができる。
実施形態２の変形例１では、パターンマッチングを実行した後に、精度が向上するフィッティングを行う。
この変形例１では、上述の実施形態２についての図６のＳ６３０の処理による出力（すなわち、第２の位置及び姿勢）を、物体１０の概略の位置姿勢として、上述の実施形態１についてのＳ６３０の処理を行う。これにより、物体１０の位置及び姿勢の推定の精度を高めることができる。あるいは、パターンマッチングにおけるテンプレートのバリエーション数を減らしても、推定の精度を維持することができる。
（実施形態２−変形例２）
実施形態２の変形例２は、上述した実施形態２の動作において、第１の位置及び姿勢の算出処理を、実施形態１で説明した処理を用いる。
すなわち、上記実施形態２における第１の位置及び姿勢の算出処理を、実施形態１で説明した図６のＳ６１０、Ｓ６２０の処理を用いて求めてもよい。実施形態１についての図６のＳ６２０で最適化した３自由度のパラメータｓｐから主平面の位置及び姿勢を算出し、図６のＳ６３０の入力として利用すればよい。
実施形態２の変形例２は、上述した実施形態２の動作において、第１の位置及び姿勢の算出処理を、実施形態１で説明した処理を用いる。
すなわち、上記実施形態２における第１の位置及び姿勢の算出処理を、実施形態１で説明した図６のＳ６１０、Ｓ６２０の処理を用いて求めてもよい。実施形態１についての図６のＳ６２０で最適化した３自由度のパラメータｓｐから主平面の位置及び姿勢を算出し、図６のＳ６３０の入力として利用すればよい。
（実施形態２−変形例３）
実施形態２の変形例３は、複数の対象物体の位置及び姿勢を算出する。
取得した画像に複数の計測対象の部品が撮影されている場合、図６のＳ６２０において、距離画像のセグメンテーションによって得られた平面の中で、面積の大きい方から、複数の平面のセグメントを選び出してもよい。当該複数の平面のセグメントに対して、実施形態２で説明した図６のＳ６２０の処理を行い、複数の対象物体１０に対して、第１の位置及び姿勢を算出してもよい。
実施形態２の変形例３は、複数の対象物体の位置及び姿勢を算出する。
取得した画像に複数の計測対象の部品が撮影されている場合、図６のＳ６２０において、距離画像のセグメンテーションによって得られた平面の中で、面積の大きい方から、複数の平面のセグメントを選び出してもよい。当該複数の平面のセグメントに対して、実施形態２で説明した図６のＳ６２０の処理を行い、複数の対象物体１０に対して、第１の位置及び姿勢を算出してもよい。
（実施形態３）
以下、本発明の実施形態３を、上記実施形態と異なる点について説明する。
実施形態３においては、実施形態２における第２位置姿勢算出部１２６５が、特徴点ベースのモデルフィッティングを実行するものである。
この実施形態３においては、実施形態２と同様に、第２位置姿勢算出部１２６５が、主平面が視軸と垂直となるように変換した濃淡画像から特徴点を検出する。そして、第２位置姿勢算出部１２６５は、主平面が視軸と垂直となっている画像における例えばＳＩＦＴ（Scale-Invariant Feature Transform）特徴点のデータベース上の特徴点との対応をもとに、物体１０の位置及び姿勢を算出する。
以下、本発明の実施形態３を、上記実施形態と異なる点について説明する。
実施形態３においては、実施形態２における第２位置姿勢算出部１２６５が、特徴点ベースのモデルフィッティングを実行するものである。
この実施形態３においては、実施形態２と同様に、第２位置姿勢算出部１２６５が、主平面が視軸と垂直となるように変換した濃淡画像から特徴点を検出する。そして、第２位置姿勢算出部１２６５は、主平面が視軸と垂直となっている画像における例えばＳＩＦＴ（Scale-Invariant Feature Transform）特徴点のデータベース上の特徴点との対応をもとに、物体１０の位置及び姿勢を算出する。
本実施形態３において説明する位置姿勢計測装置は、図１２に示す位置姿勢計測装置１２０１と同様の構成であってよい。図１２を参照して、位置姿勢計測装置１２０１は、モデル保存部１２１０、２次元画像入力部１３０、３次元データ入力部１５０、第１位置姿勢算出部１２６０、第２位置姿勢算出部１２６５から構成されている。また、位置姿勢計測装置１２０１には、２次元画像撮像部１００、距離計測部１０５が接続されている。以下、本実施形態３の位置姿勢計測装置を構成する各部について説明する。なお、２次元画像撮像部１００、距離計測部１０５、２次元画像入力部１３０、３次元データ入力部１５０、第１位置姿勢算出部１２６０に関しては、実施形態２と同様のため、説明を省略する。
モデル保存部１２１０には、計測する対象の物体１０の２次元の特徴点のデータベースと主平面の情報とを保存する。本実施形態３においては、例えばＳＩＦＴ特徴点のデータベースを保存する。この２次元特徴点はＳＩＦＴ特徴点以外の特徴点であってよい。ＳＩＦＴ特徴点のデータベースは、例えば、物体１０のそれぞれの主平面が視軸と垂直になっている場合における、当該すべての視点に対して、それぞれＳＩＦＴ特徴点の検出を行い、各視点ごとにその特徴量の列を保存して作成できる。
ＳＩＦＴ特徴点を検出する方法は、例えば、David G. Lowe, “Distinctive image features from scale-invariant keypoints”, Int. Journal of Computer Vision, Vol. 60, No. 2, pp.91-110, 2004.に記載されている方法を採用してよい。主平面の情報は、実施形態２と同様に、モデルを点の集合及び面を構成する点のＩＤによって表現する。さらに、データベース中には、各面が、その面自身が主平面となる姿勢の範囲を保持することで、主平面の情報として保持する。ＳＩＦＴ特徴点のデータベースと、主平面の情報は、モデル保存部１２１０に保存される。そして、第２位置姿勢算出部１２６５が、当該ＳＩＦＴ特徴点のデータベースと、主平面の情報を用いて所定の処理を実行する。
第２位置姿勢算出部１２６５は、第１位置姿勢算出部１２６０が算出した第１の位置及び姿勢を用いて、２次元画像入力部１２３０が入力した２次元画像中の物体１０の主平面が視軸と垂直になるように、当該画像を変換する。
第２位置姿勢算出部１２６５は、変換した画像に対して、モデル保存部１２１０に保存されている計測する対象である物体１０のＳＩＦＴ特徴点のデータベース中のＳＩＦＴ特徴点群を、変換済み画像から抽出したＳＩＦＴ特徴点と対応付ける。そして、第２位置姿勢算出部１２６５は、例えばＤＬＴ（Direct Linear Transform）法によって、物体１０の第２の位置及び姿勢を計測（算出）する。ここで、ＤＬＴ法として、Y. I. Abdel-Aziz and H. M. Karara.,”Direct linear transformation from comparator coordinates into object space coordinates in close-range photogrammetry”, In Proceedings of the Symposium on Close-Range Photogrammetry, pp. 1-18. American Society of Photogrammetry, Falls Church, 1971.”に記載の方法等を利用することができる。
第２位置姿勢算出部１２６５は、変換した画像に対して、モデル保存部１２１０に保存されている計測する対象である物体１０のＳＩＦＴ特徴点のデータベース中のＳＩＦＴ特徴点群を、変換済み画像から抽出したＳＩＦＴ特徴点と対応付ける。そして、第２位置姿勢算出部１２６５は、例えばＤＬＴ（Direct Linear Transform）法によって、物体１０の第２の位置及び姿勢を計測（算出）する。ここで、ＤＬＴ法として、Y. I. Abdel-Aziz and H. M. Karara.,”Direct linear transformation from comparator coordinates into object space coordinates in close-range photogrammetry”, In Proceedings of the Symposium on Close-Range Photogrammetry, pp. 1-18. American Society of Photogrammetry, Falls Church, 1971.”に記載の方法等を利用することができる。
実施形態３における、物体１０の位置及び姿勢の計測の処理手順を示すフローチャートは実施形態１と同様に図６を参照して説明する。図６において、各ステップのうち、Ｓ６１０、Ｓ６２０についてはそれぞれ実施形態２で説明したＳ６１０、Ｓ６２９の処理内容と同一でよいため、説明を省略する。以下では、Ｓ６３０について説明する。
すなわち、第２位置姿勢算出部１２６５は、Ｓ６２０において算出された第１の位置及び姿勢と、濃淡画像とに基づいて物体１０の第２の位置及び姿勢の計測（算出）を行う（Ｓ６３０）。このＳ６３０においては、濃淡画像を、Ｓ６２０において検出した平面が視軸と垂直になるように変換する。変換後の画像に対して、データベース上のＳＩＦＴ特徴点と、視軸と主平面が垂直となっている画像上のＳＩＦＴ特徴点との対応付けを行う。当該対応付けの処理においては、第２位置姿勢算出部１２６５は、モデル保存部１２１０に保存されているＳＩＦＴ特徴量のデータベースを用いてよい。第２位置姿勢算出部１２６５は、当該対応付けの後、例えばＤＬＴ法によって位置及び姿勢を算出する。
すなわち、第２位置姿勢算出部１２６５は、Ｓ６２０において算出された第１の位置及び姿勢と、濃淡画像とに基づいて物体１０の第２の位置及び姿勢の計測（算出）を行う（Ｓ６３０）。このＳ６３０においては、濃淡画像を、Ｓ６２０において検出した平面が視軸と垂直になるように変換する。変換後の画像に対して、データベース上のＳＩＦＴ特徴点と、視軸と主平面が垂直となっている画像上のＳＩＦＴ特徴点との対応付けを行う。当該対応付けの処理においては、第２位置姿勢算出部１２６５は、モデル保存部１２１０に保存されているＳＩＦＴ特徴量のデータベースを用いてよい。第２位置姿勢算出部１２６５は、当該対応付けの後、例えばＤＬＴ法によって位置及び姿勢を算出する。
図１４は、実施形態３についての図６のＳ６３０における第２の位置及び姿勢の計測の処理手順を表すフローチャートを示す。
以下、Ｓ１４１０からＳ１４３０を参照して、実施形態３におけるＳ６３０の処理の詳細を説明する。なお、図１４のＳ１４１０の処理は、図１３に示すＳ１３１０と同様のため説明は省略し、Ｓ１４２０から説明を行う。
まず、Ｓ１４１０において変換した濃淡画像（以降、「正面画像」と呼ぶ。）上のＳＩＦＴ特徴点と、モデル保存部１２１０に保存されているデータベース上のＳＩＦＴ特徴点（以降、「モデル上のＳＩＦＴ特徴点」と呼ぶ。）との対応付けを行う（Ｓ１４２０）。ここで、正面画像からＳＩＦＴ特徴点を検出する方法は、例えば、David G. Lowe, “Distinctive image features from scale-invariant keypoints”, Int. Journal of Computer Vision, Vol. 60, No. 2, pp.91-110, 2004.に記載されている方法を採用できる。
以下、Ｓ１４１０からＳ１４３０を参照して、実施形態３におけるＳ６３０の処理の詳細を説明する。なお、図１４のＳ１４１０の処理は、図１３に示すＳ１３１０と同様のため説明は省略し、Ｓ１４２０から説明を行う。
まず、Ｓ１４１０において変換した濃淡画像（以降、「正面画像」と呼ぶ。）上のＳＩＦＴ特徴点と、モデル保存部１２１０に保存されているデータベース上のＳＩＦＴ特徴点（以降、「モデル上のＳＩＦＴ特徴点」と呼ぶ。）との対応付けを行う（Ｓ１４２０）。ここで、正面画像からＳＩＦＴ特徴点を検出する方法は、例えば、David G. Lowe, “Distinctive image features from scale-invariant keypoints”, Int. Journal of Computer Vision, Vol. 60, No. 2, pp.91-110, 2004.に記載されている方法を採用できる。
対応付けは、例えば、正面画像から検出したＳＩＦＴ特徴点に対し、最も距離が小さくなるモデル上のＳＩＦＴ特徴点を対応付けることで実行できる。本実施形態３では、距離は、ＳＩＦＴ特徴量の差のＬ２ノルムとする。これに替えて、距離を他の方法で算出してもよい。
正面画像から検出されたＳＩＦＴ特徴点群は、ノイズなどの影響で、異なる姿勢から検出されたモデル上のＳＩＦＴ特徴点群に対応付けられてしまう可能性がある。この場合、すべての姿勢Pにおいて、正面画像から検出されたＳＩＦＴ特徴点群に対応付いたモデル上のＳＩＦＴ特徴点群の中から、Pから検出されたモデル上のＳＩＦＴ特徴点の個数を算出する。そして、前述の個数が最も多くなるような姿勢Pmaxを選択する。選択された姿勢Pmaxから検出されたモデル上のＳＩＦＴ特徴点群に含まれる正面画像から、検出されたＳＩＦＴ特徴点を抽出する。そして、当該抽出されたＳＩＦＴ特徴点に対応付いたモデル上のＳＩＦＴ点のみを、最終的な正面画像から検出されたＳＩＦＴ特徴点群に対応付いたモデル上のＳＩＦＴ特徴点群とする。
正面画像から検出されたＳＩＦＴ特徴点群は、ノイズなどの影響で、異なる姿勢から検出されたモデル上のＳＩＦＴ特徴点群に対応付けられてしまう可能性がある。この場合、すべての姿勢Pにおいて、正面画像から検出されたＳＩＦＴ特徴点群に対応付いたモデル上のＳＩＦＴ特徴点群の中から、Pから検出されたモデル上のＳＩＦＴ特徴点の個数を算出する。そして、前述の個数が最も多くなるような姿勢Pmaxを選択する。選択された姿勢Pmaxから検出されたモデル上のＳＩＦＴ特徴点群に含まれる正面画像から、検出されたＳＩＦＴ特徴点を抽出する。そして、当該抽出されたＳＩＦＴ特徴点に対応付いたモデル上のＳＩＦＴ点のみを、最終的な正面画像から検出されたＳＩＦＴ特徴点群に対応付いたモデル上のＳＩＦＴ特徴点群とする。
Ｓ１４２０における対応付け結果に基づき、第２位置姿勢算出部１２６５が、物体１０の第２の位置及び姿勢の算出を行う。本実施形態３では、例えば、ＤＬＴ法を用いて、第２の位置及び姿勢の算出を行ってよい。第２の位置及び姿勢の算出方法はＤＬＴ法に限られるものではなく、例えばLepetit（V. Lepetit, F. Moreno-Noguer, and P. Fua, “Epnp: An accurate o(n) solution to the pnp problem” Int. J. Comput. Vision, vol. 81. No. 2. Pp. 155-166, 2009.）に記載されているアルゴリズムを利用できる。
以上、説明したように、実施形態３においては、まず距離画像を用いて距離画像上の平面を検出して、当該平面が視線と垂直になるように濃淡画像を変換する。この処理によって、データベース中に保存されている対象の物体１０のＳＩＦＴ特徴点により近いＳＩＦＴ特徴点が出現しやすくなる。したがって、対応付けの精度が向上し、よりロバストに、物体１０の位置及び姿勢を算出できる。
以上、説明したように、実施形態３においては、まず距離画像を用いて距離画像上の平面を検出して、当該平面が視線と垂直になるように濃淡画像を変換する。この処理によって、データベース中に保存されている対象の物体１０のＳＩＦＴ特徴点により近いＳＩＦＴ特徴点が出現しやすくなる。したがって、対応付けの精度が向上し、よりロバストに、物体１０の位置及び姿勢を算出できる。
（実施形態３−変形例１）
実施形態３の変形例１においては、実施形態３の第１の位置及び姿勢の算出処理を、実施形態１に替えて実行する。
すなわち、実施形態３における第１の位置及び姿勢算出を、実施形態１において説明した図６のＳ６１０、Ｓ６２０の処理によって求めてもよい。実施形態１のＳ６２０で最適化した３自由度のパラメータｓｐから主平面の位置及び姿勢を算出し、実施形態３についての図６のＳ６３０の入力とすることができる。
実施形態３の変形例１においては、実施形態３の第１の位置及び姿勢の算出処理を、実施形態１に替えて実行する。
すなわち、実施形態３における第１の位置及び姿勢算出を、実施形態１において説明した図６のＳ６１０、Ｓ６２０の処理によって求めてもよい。実施形態１のＳ６２０で最適化した３自由度のパラメータｓｐから主平面の位置及び姿勢を算出し、実施形態３についての図６のＳ６３０の入力とすることができる。
（実施形態３−変形例２）
実施形態３の変形例２においては、複数の計測対象である物体１０の位置及び姿勢を算出する。
取得した画像に複数の計測対象の部品が撮影されている場合、図６のＳ６２０において、距離画像のセグメンテーションによって得られた面積の大きい複数の平面のセグメントを抽出する。この抽出した複数の平面のセグメントに対してＳ６２０の処理をそれぞれ行い、複数の対象である物体１０のそれぞれに関して位置及び姿勢を算出してもよい。
実施形態３の変形例２においては、複数の計測対象である物体１０の位置及び姿勢を算出する。
取得した画像に複数の計測対象の部品が撮影されている場合、図６のＳ６２０において、距離画像のセグメンテーションによって得られた面積の大きい複数の平面のセグメントを抽出する。この抽出した複数の平面のセグメントに対してＳ６２０の処理をそれぞれ行い、複数の対象である物体１０のそれぞれに関して位置及び姿勢を算出してもよい。
（実施形態４）
以下、本発明の実施形態４を、上記実施形態と異なる点につき説明する。
実施形態４は、位置姿勢計測装置だけでなく、ロボット制御装置の構成まで含んだ全体的なシステムである。
図１５は、本実施形態４における情報処理装置１５１０の構成の一例を示す。本実施形態４において、図１５を参照して、情報処理装置１５１０は、位置姿勢計測装置１５０５、センサ部１５００、ロボットコントローラ部１５０１を備えている。さらに、情報処理装置１５１０は、ロボット１５０２、物体抽出・概略位置姿勢取得部１５２０を備えてよい。
以下、本発明の実施形態４を、上記実施形態と異なる点につき説明する。
実施形態４は、位置姿勢計測装置だけでなく、ロボット制御装置の構成まで含んだ全体的なシステムである。
図１５は、本実施形態４における情報処理装置１５１０の構成の一例を示す。本実施形態４において、図１５を参照して、情報処理装置１５１０は、位置姿勢計測装置１５０５、センサ部１５００、ロボットコントローラ部１５０１を備えている。さらに、情報処理装置１５１０は、ロボット１５０２、物体抽出・概略位置姿勢取得部１５２０を備えてよい。
図１６は、位置姿勢計測装置１５０５、及び、ロボットコントローラ部１５０１の詳細な構成図の一例を示す。図１６において、位置姿勢計測装置１０１は、実施形態１、実施形態２、実施形態３と同様に、モデル保存部１１０、２次元画像入力部１３０、３次元データ入力１５０、第１位置姿勢算出部１６０、第２位置姿勢算出部１６５から構成される。ロボットコントローラ部１５０１は、ロボット作業指示部１６７０、ロボット制御部１６８０から構成される。これらの構成によって、パレット１５０４内に山積みした対象物体１５０３の計測を行い、ロボット１５０２によるピック作業を行う。
以下、本実施形態４における情報処理装置１５１０を構成する各部について説明する。なお、位置姿勢計測装置１０１については実施形態１、実施形態２、実施形態３の位置姿勢計測装置１０１、位置姿勢計測装置１２０１と同様であるので説明は省略する。
センサ部１５００は、例えば、小型プロジェクタと２次元画像を撮影する小型カメラから構成することができる。このセンサ部１５００は、パレット１５０４の上方に固定的に配置され、山積みされた対象物体１５０３の画像を撮影し、２次元画像入力部１３０、３次元データ入力部１５０に対して濃淡画像や、距離画像等の出力を行ってよい。センサ部１５００の小型プロジェクタと小型カメラとの相対的な位置関係は予めキャリブレーションにより求まっているものとする。
センサ部１５００は、例えば、小型プロジェクタと２次元画像を撮影する小型カメラから構成することができる。このセンサ部１５００は、パレット１５０４の上方に固定的に配置され、山積みされた対象物体１５０３の画像を撮影し、２次元画像入力部１３０、３次元データ入力部１５０に対して濃淡画像や、距離画像等の出力を行ってよい。センサ部１５００の小型プロジェクタと小型カメラとの相対的な位置関係は予めキャリブレーションにより求まっているものとする。
センサ部１５００の小型プロジェクタは対象物体に対してパターン光を照射する。センサ部１５００の小型カメラは、そのパターンを撮像し、それに基づき算出される距離画像を３次元データ入力部１５０に対して出力する。また、センサ部１５００は、小型カメラが撮像した濃淡画像を２次元画像入力部１３０に対して出力する。
この照射パターンとしては、例えば、空間符号化法の幅の異なる複数の縞模様パターンや、複数の線パターンなどを利用してよい。また、例えば、２次元的なパターンやランダムドットのようなパターンでもよい。また、センサ部１５００は、例えば、回折格子と照明とカメラから構成されてもよい。この場合は回折格子と照明により対象物体にパターン光を投射し、カメラによりパターンを撮影することができる。
この照射パターンとしては、例えば、空間符号化法の幅の異なる複数の縞模様パターンや、複数の線パターンなどを利用してよい。また、例えば、２次元的なパターンやランダムドットのようなパターンでもよい。また、センサ部１５００は、例えば、回折格子と照明とカメラから構成されてもよい。この場合は回折格子と照明により対象物体にパターン光を投射し、カメラによりパターンを撮影することができる。
ロボット１５０２は、例えば、多関節ロボットであってよい。このロボット１５０２は、ロボットコントローラ部１５０１からの制御命令を受信して、当該制御命令に基づき稼働する。ロボット１５０２の先端には、例えばエンドエフェクタであるハンドが装着される。このハンドを用いて、ロボット１５０２は、対象である物体１５０３に対する作業を行うことができる。本実施形態４においては、エンドエフェクタとして、対象である物体１５０３を把持することが可能な、例えばチャック機構を持つハンドを用いてよい。その他、エンドエフェクタにはモータ駆動可能なハンドを用いてもよいし、空気圧で対象物体を吸着する吸着パッドを用いてもよい。
センサ部１５００の位置姿勢、ロボット１５０２やハンドの位置や軌道、ロボット１５０２のアームとセンサ部１５００の相対位置姿勢のキャリブレーション作業等は、本実施形態４の実施に先立って公知の技術によって予め行われているものとする。これによって、第２位置姿勢算出部１６５が計測する物体１５０３の位置及び姿勢を、パレット１５０４が置かれた空間に対して固定的に定義されたワークスペース座標系に変換することができる。
また、情報処理装置１５１０のロボットコントローラ部１５０１は、ワークスペース座標系で指定した位置姿勢に対して、ハンドが設定されるように、ロボット１５０２を制御する。
計測対象である物体１５０３は、例えば、工業製品を構成する部品である。物体１５０３は、ロボット１５０２によって、例えばピックされて製品に組み付けられてよい。物体１５０３の素材は、例えば、プラスチック、金属、ビニールなど、様々なものを用いることができる。図１５は、一例として、複数の対象である物体１５０３が様々な姿勢になるようにパレット１５０４に山積みされている様子を示す。
計測対象である物体１５０３は、例えば、工業製品を構成する部品である。物体１５０３は、ロボット１５０２によって、例えばピックされて製品に組み付けられてよい。物体１５０３の素材は、例えば、プラスチック、金属、ビニールなど、様々なものを用いることができる。図１５は、一例として、複数の対象である物体１５０３が様々な姿勢になるようにパレット１５０４に山積みされている様子を示す。
パレット１５０４は、対象である物体１５０３を収容するための箱である。パレット１５０４の素材に制約はないが、例えば、プラスチックや紙素材を用いてよい。またパレット１５０４の形状にも制約はないが、例えば、作りやすさの点から立方体あるいは直方体にしてよい。パレット１５０４の大きさにも制約はないが、例えば、センサ部１５００で計測可能な範囲内に収まるようにする。
物体抽出・概略位置姿勢取得部１５２０は、センサ部１５００が出力する画像（濃淡画像、距離画像等）上で、パレット１５０４に収容されている対象である物体１５０３を例えば１つ抽出し、その概略位置姿勢を得る。物体抽出・概略位置姿勢取得部１５２０には、計測対象である物体１５０３を、例えばＧｅｏｄｅｓｉｃ Ｓｐｈｅｒｅ（計測を行う表面）上に設定した所定の視点から観察した画像が、あらかじめテンプレートとして保持されている。物体抽出・概略位置姿勢取得部１５２０は、当該テンプレートを用いて、センサ部１５００が出力する画像からテンプレートマッチングによって、概略位置姿勢を得る。すなわち、物体抽出・概略位置姿勢取得部１５２０は、テンプレートマッチングの評価値が最大のテンプレートに対応する位置及び姿勢を、選択した対象である物体１５０３の概略位置姿勢とする。評価値としては、例えば正規化相関法であれば図１３のＳ１３２０において算出するＲの値を用いてよい。
次に、ロボットコントローラ部１５０１内の各構成について説明する。ロボット作業指示部１６７０は、位置姿勢計測装置１０１が出力する計測結果である位置及び姿勢に基づいて、ロボット１５０２に対する作業指示を出力することができる。ロボット作業指示部１６７０は、位置姿勢計測装置１５０５からの出力に応じて、対象物体１５０３を把持や吸着できる位置にハンドを移動させて把持や吸着を行わせる。
なお、ロボット１５０２の作業は移動、把持、吸着に限られるものではなく、対象である物体１５０３の外観検査など、他の作業も含んでよい。さらに、ロボット１５０２も多関節ロボットではなくＮＣ制御可能な可動式の機械でもよい。ロボット制御部１６８０は、ロボット作業指示部１６７０からの指示情報を受けて、ロボット１５０２の制御を行う。
なお、ロボット１５０２の作業は移動、把持、吸着に限られるものではなく、対象である物体１５０３の外観検査など、他の作業も含んでよい。さらに、ロボット１５０２も多関節ロボットではなくＮＣ制御可能な可動式の機械でもよい。ロボット制御部１６８０は、ロボット作業指示部１６７０からの指示情報を受けて、ロボット１５０２の制御を行う。
次に、本実施形態４における処理手順を説明する。まず、パレット１５０４に収容されている対象である物体１５０３をセンサ部１５００が撮像する。次に、物体抽出・概略位置姿勢取得部１５２０が、対象である物体１５０３の概略位置姿勢を決定する。次に、位置姿勢計測装置１５０５が、対象である物体１５０３の位置及び姿勢を決定する。位置及び姿勢を決定する方法は、例えば、実施形態１において説明した方法によって求めてよい。または、物体抽出・概略位置姿勢取得部１５２０が算出した概略位置姿勢付近の画像を抜き出して、実施形態２、実施形態３に記載した方法によって求めてもよい。最後に、ロボット１５０２のエンドエフェクタによって対象である物体１５０３を把持もしくは吸着する。
なお、物体１５０３の位置・姿勢の計測が失敗した場合や、計測の後で実行するべきロボット作業において失敗した場合は、それ以降も、類似した状況下で失敗を繰り返す可能性がある。そこで、実施形態４では、失敗した場合の対象である物体１５０３に対する相対的な位置姿勢を所定の記憶手段に記憶しておき、それ以降の処理においては、その記憶しておいた位置姿勢を避けるように構成してもよい。失敗した時の状況を再現させないことによって、ロバスト性を向上させることができる。
（上記各実施形態の変形例）
また、上記各実施形態は、以下の処理を実行することによっても実現される。即ち、上述した各実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給することによって実現してよい。その場合、当該システム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して処理を実行する。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。
また、上記各実施形態は、以下の処理を実行することによっても実現される。即ち、上述した各実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給することによって実現してよい。その場合、当該システム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して処理を実行する。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。
図１７は、上記各実施形態の各部を構成することのできるコンピュータ１７１０の構成の例を示す。撮像部１７１５を含むコンピュータ１７１０は、例えば、センサ部１５００や、２次元画像取得部１００、１２００、距離計測部１０５、１２０５等を構成する。ＣＰＵ１７１１は、ＲＯＭ１７１２や、ＲＡＭ１７１３、外部メモリ１７１４等に格納されたプログラムを実行することによって、上記各実施形態の各部を実現する。例えば、位置姿勢計測装置の各部の機能を実現する。ＲＯＭ１７１２、ＲＡＭ１７１３、外部メモリ１７１４には、上記各実施形態で説明した各部の機能を実現するプログラムを格納することができる。また、例えば、モデル保存部１１０、１２１０に格納されるモデルやデータベース等を格納する。また、入力部１７１６は、濃淡画像などの２次元画像や、距離画像などを入力する。これによって、２次元画像入力部１３０、１２３０、３次元データ入力部１５０、１２５０等の機能を実現することができる。また、表示部１７１７は、操作者に対して計測の結果、処理の結果等を表示する。通信Ｉ／Ｆ１７１８は、外部と通信を行うインターフェースであり、インターネット等を通じてロボット１５０２を操作することなどを実現する。外部メモリ１７１４は、ハードディスクや、半導体記憶装置等であり、プログラムや、上記実施形態で説明した各所のモデルやデータベースを格納する。コンピュータ１７１０の上記説明した各部はバス１７１９によって相互に接続されている。
また、上記各実施形態では、物体の位置及び姿勢をロバストに計測する技術を説明したが、この技術は、さまざまな応用が考えられる。例えば、ロボットの自己位置推定や、ロボットと物体との間の相対的な位置及び姿勢を推定するのに利用することができる。
以上、本発明の実施形態について詳細に説明したが、これら実施形態、本発明を実施するにあたっての具体例を示したに過ぎず、本発明の技術的範囲は、上記実施形態に限定されるものではない。本発明は、その趣旨を逸脱しない範囲において種々の変更が可能であり、それらも本発明の技術的範囲に含まれる。
以上、本発明の実施形態について詳細に説明したが、これら実施形態、本発明を実施するにあたっての具体例を示したに過ぎず、本発明の技術的範囲は、上記実施形態に限定されるものではない。本発明は、その趣旨を逸脱しない範囲において種々の変更が可能であり、それらも本発明の技術的範囲に含まれる。
１００…２次元画像撮像部、１０１…位置姿勢計測装置、１０５…距離計測部、１１０…モデル保存部、１３０…２次元画像入力部、１５０…３次元データ入力部、１６０…第１位置姿勢算出部、１６５…第２位置姿勢算出部
Claims (20)
- 計測対象である物体の位置及び姿勢を計測する位置姿勢計測装置であって、
前記物体の２次元画像を取得する第１の取得手段と、
前記物体の形状を表す３次元形状データを取得する第２の取得手段と、
前記物体の形状モデルを保存する保存手段と、
前記第２の取得手段により取得された前記物体の前記３次元形状データと、前記保存手段に保存される前記物体の前記形状モデルとに基づいて、前記物体の主平面の位置及び姿勢を前記物体の第１の位置及び姿勢として算出する第１の算出手段と、
前記第１の算出手段により算出された前記第１の位置及び姿勢に基づいて、前記第１の取得手段により取得された前記２次元画像と前記形状モデルとを照合することにより、前記物体の３次元空間上の第２の位置及び姿勢を算出する第２の算出手段と、
を具備することを特徴とする位置姿勢計測装置。 - 前記３次元形状データは、前記物体の表面上の点群の３次元座標であり、
前記第１の算出手段は、前記物体の前記点群の３次元座標のデータが、前記３次元形状モデルの前記主平面に当てはまるように、前記物体の前記第１の距離及び姿勢を算出する、ことを特徴とする請求項１に記載の位置姿勢計測装置。 - 前記３次元形状データは、前記物体上の点群の撮像視点からの距離を表す距離画像から得られる、ことを特徴とする請求項１又は２に記載の位置姿勢計測装置。
- 前記形状モデルは、前記物体の３次元形状モデルを構成する面と、当該面が前記主平面となる前記物体の姿勢の情報と、を含む、ことを特徴とする請求項１から３のいずれか１項に記載の位置姿勢計測装置。
- 前記第１の算出手段は、前記３次元形状データと前記３次元形状モデルとから得られる対応付け結果に基づき、与えられる前記物体の位置及び姿勢の初期値を補正することによって、前記物体の前記第１の位置及び姿勢を算出する、ことを特徴とする請求項１から４のいずれか１項に記載の位置姿勢計測装置。
- 前記第１の算出手段は、前記３次元形状データから、前記物体の平面を検出し、検出された前記平面が、前記形状モデルの前記主平面と一致するように、前記物体の前記第１の位置及び姿勢を算出する、ことを特徴とする請求項１から４のいずれか１項に記載の位置姿勢計測装置。
- 前記第２の算出手段は、前記第１の位置及び姿勢に基づいて、前記形状モデル上の幾何特徴と、前記２次元画像から抽出された画像特徴とを対応付け、当該対応付けの結果に基づいて、前記物体の前記第２の位置及び姿勢を算出する、ことを特徴とする請求項１から６のいずれか１項に記載の位置姿勢計測装置。
- 前記第２の算出手段は、前記第１の位置及び姿勢に基づいて、前記２次元画像を、前記形状モデルの前記主平面が前記２次元画像の撮影における視軸に垂直になる正面画像に変換し、前記主平面が前記視軸に垂直な視点において前記物体を観察した像に相当するパターン群を前記正面画像に照合することによって、前記物体の前記第２の位置及び姿勢を算出する、ことを特徴とする請求項１から６のいずれか１項に記載の位置姿勢計測装置。
- 前記第１の算出手段は、前記物体の前記主平面の法線方向の１自由度の位置の成分、及び前記主平面と平行な方向周りの回転による２自由度の姿勢の成分から成る前記第１の位置及び姿勢を算出し、
前記第２の算出手段は、前記物体の前記主平面と平行な方向の２自由度の位置の成分、及び前記主平面の法線方向周りの回転による１自由度の姿勢の成分から成る前記第２の位置及び姿勢を算出する、ことを特徴とする請求項１から８のいずれか１項に記載の位置姿勢計測装置。 - 前記第２の算出手段は、前記第１の位置及び姿勢に基づいて、前記形状モデル上の幾何特徴と、前記２次元画像から抽出された画像特徴とを対応付け、さらに、前記第１の位置及び姿勢と、前記３次元形状データと前記３次元形状モデルとから得られる対応付け結果とに基づいて、前記物体の前記第２の位置及び姿勢を算出する、ことを特徴とする請求項１から９のいずれか１項に記載の位置姿勢計測装置。
- 請求項１から１０のいずれか１項に記載の位置姿勢計測装置と、
前記位置姿勢計測装置に対して、前記物体の前記２次元画像と、前記物体の前記３次元形状データと、を出力するセンサ部と、
前記位置姿勢計測装置に対して、前記物体の概略の位置及び姿勢を設定する設定部と、
前記位置姿勢計測装置が出力する前記物体の位置及び姿勢に基づき、ロボットを制御するコントローラと、
前記コントローラによって制御されるロボットと、
を具備することを特徴とする情報処理装置。 - 計測対象である物体の位置及び姿勢を計測する位置姿勢計測方法であって、
前記物体の２次元画像を取得するステップと、
前記物体の形状を表す３次元形状データを取得するステップと、
前記物体の形状モデルを保存するステップと、
取得された前記物体の前記３次元形状データと、記憶装置に保存される前記物体の形状モデルとに基づいて、前記物体の主平面の位置及び姿勢を前記物体の第１の位置及び姿勢として算出するステップと、
算出された前記第１の位置及び姿勢に基づいて、取得された前記２次元画像と前記形状モデルとを照合することにより、前記物体の３次元空間上の第２の位置及び姿勢を算出するステップと、
を含むことを特徴とする位置姿勢計測方法。 - 前記３次元形状データは、前記物体の表面上の点群の３次元座標であり、
前記第１の距離及び姿勢は、前記物体の前記点群の３次元座標のデータが、前記３次元形状モデルの前記主平面に当てはまるように、算出される、ことを特徴とする請求項１２に記載の位置姿勢計測方法。 - 前記３次元形状データは、前記物体上の点群の撮像視点からの距離を表す距離画像から得られる、ことを特徴とする請求項１２又は１３に記載の位置姿勢計測方法。
- 前記形状モデルは、前記物体の３次元形状モデルを構成する面と、当該面が前記主平面となる前記物体の姿勢の情報と、を含む、ことを特徴とする請求項１２から１３のいずれか１項に記載の位置姿勢計測方法。
- 前記物体の前記第１の位置及び姿勢は、前記３次元形状データと前記３次元形状モデルとから得られる対応付け結果に基づき、与えられる前記物体の位置及び姿勢の初期値を補正することによって、算出される、ことを特徴とする請求項１２から１５のいずれか１項に記載の位置姿勢計測方法。
- 前記物体の前記第１の位置及び姿勢は、前記３次元形状データから、前記物体の平面を検出し、検出された前記平面が、前記形状モデルの前記主平面と一致するように、算出される、ことを特徴とする請求項１２から１５のいずれか１項に記載の位置姿勢計測方法。
- 前記物体の前記第２の位置及び姿勢は、前記第１の位置及び姿勢に基づいて、前記形状モデル上の幾何特徴と、前記２次元画像から抽出された画像特徴とを対応付け、当該対応付けの結果に基づいて、算出される、ことを特徴とする請求項１２から１７のいずれか１項に記載の位置姿勢計測方法。
- 前記物体の前記第２の位置及び姿勢は、前記第１の位置及び姿勢に基づいて、前記２次元画像を、前記形状モデルの前記主平面が前記２次元画像の撮影における視軸に垂直になる正面画像に変換し、前記主平面が前記視軸に垂直な視点において前記物体を観察した像に相当するパターン群を前記正面画像に照合することによって、算出される、ことを特徴とする請求項１２から１７のいずれか１項に記載の位置姿勢計測方法。
- コンピュータが読み取り実行することで、前記コンピュータを、請求項１から１０のいずれか１項に記載の位置姿勢計測装置の各手段として機能させるためのコンピュータプログラム。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2015050094A JP2016170050A (ja) | 2015-03-12 | 2015-03-12 | 位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラム |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2015050094A JP2016170050A (ja) | 2015-03-12 | 2015-03-12 | 位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラム |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2016170050A true JP2016170050A (ja) | 2016-09-23 |
Family
ID=56982444
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2015050094A Pending JP2016170050A (ja) | 2015-03-12 | 2015-03-12 | 位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラム |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2016170050A (ja) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2018077837A (ja) * | 2016-10-28 | 2018-05-17 | Ｊｆｅスチール株式会社 | 位置認識方法およびシステム、ならびに異常判定方法およびシステム |
| CN109934869A (zh) * | 2017-12-19 | 2019-06-25 | 欧姆龙株式会社 | 位置姿势推断装置、位置姿势推断方法及记录介质 |
| JP2019136828A (ja) * | 2018-02-13 | 2019-08-22 | Ｋｙｏｔｏ Ｒｏｂｏｔｉｃｓ株式会社 | 箱状物ピッキング装置及びその方法 |
| WO2020049766A1 (ja) * | 2018-09-07 | 2020-03-12 | オムロン株式会社 | 対象物認識装置、マニピュレータ、および移動ロボット |
| CN111353416A (zh) * | 2020-02-26 | 2020-06-30 | 广东温氏种猪科技有限公司 | 基于牲畜三维测量的姿态检测方法、系统及存储介质 |
| US10755439B2 (en) | 2018-03-08 | 2020-08-25 | Fujitsu Limited | Estimation device, estimation method and storage medium |
| JP7093881B1 (ja) | 2021-09-06 | 2022-06-30 | Dmg森精機株式会社 | システム、及び自動搬送車 |
| CN115359192A (zh) * | 2022-10-14 | 2022-11-18 | 阿里巴巴（中国）有限公司 | 三维重建与商品信息处理方法、装置、设备及存储介质 |
| JP7502641B2 (ja) | 2020-11-17 | 2024-06-19 | 株式会社デンソーウェーブ | 認識システム |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008217544A (ja) * | 2007-03-06 | 2008-09-18 | Yaskawa Electric Corp | 物体検出方法、物体検出装置、及びそれを備えたロボット |
| JP2010127819A (ja) * | 2008-11-28 | 2010-06-10 | Fuji Electric Holdings Co Ltd | 多面体位置検出装置及び検出方法 |
| JP2012123781A (ja) * | 2010-11-19 | 2012-06-28 | Canon Inc | 情報処理装置、情報処理システム及び情報処理方法 |
| JP2012141962A (ja) * | 2010-12-14 | 2012-07-26 | Canon Inc | 位置姿勢計測装置及び位置姿勢計測方法 |
| JP2014046433A (ja) * | 2012-09-03 | 2014-03-17 | Canon Inc | 情報処理システム、方法及びプログラム |
-
2015
- 2015-03-12 JP JP2015050094A patent/JP2016170050A/ja active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008217544A (ja) * | 2007-03-06 | 2008-09-18 | Yaskawa Electric Corp | 物体検出方法、物体検出装置、及びそれを備えたロボット |
| JP2010127819A (ja) * | 2008-11-28 | 2010-06-10 | Fuji Electric Holdings Co Ltd | 多面体位置検出装置及び検出方法 |
| JP2012123781A (ja) * | 2010-11-19 | 2012-06-28 | Canon Inc | 情報処理装置、情報処理システム及び情報処理方法 |
| JP2012141962A (ja) * | 2010-12-14 | 2012-07-26 | Canon Inc | 位置姿勢計測装置及び位置姿勢計測方法 |
| JP2014046433A (ja) * | 2012-09-03 | 2014-03-17 | Canon Inc | 情報処理システム、方法及びプログラム |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2018077837A (ja) * | 2016-10-28 | 2018-05-17 | Ｊｆｅスチール株式会社 | 位置認識方法およびシステム、ならびに異常判定方法およびシステム |
| CN109934869A (zh) * | 2017-12-19 | 2019-06-25 | 欧姆龙株式会社 | 位置姿势推断装置、位置姿势推断方法及记录介质 |
| JP2019109747A (ja) * | 2017-12-19 | 2019-07-04 | オムロン株式会社 | 位置姿勢推定装置、位置姿勢推定方法及びプログラム |
| JP2019136828A (ja) * | 2018-02-13 | 2019-08-22 | Ｋｙｏｔｏ Ｒｏｂｏｔｉｃｓ株式会社 | 箱状物ピッキング装置及びその方法 |
| JP7065632B2 (ja) | 2018-02-13 | 2022-05-12 | Ｋｙｏｔｏ Ｒｏｂｏｔｉｃｓ株式会社 | 箱状物ピッキング装置及びその方法 |
| US10755439B2 (en) | 2018-03-08 | 2020-08-25 | Fujitsu Limited | Estimation device, estimation method and storage medium |
| JP7031540B2 (ja) | 2018-09-07 | 2022-03-08 | オムロン株式会社 | 対象物認識装置、マニピュレータ、および移動ロボット |
| JP2020042421A (ja) * | 2018-09-07 | 2020-03-19 | オムロン株式会社 | 対象物認識装置、マニピュレータ、および移動ロボット |
| WO2020049766A1 (ja) * | 2018-09-07 | 2020-03-12 | オムロン株式会社 | 対象物認識装置、マニピュレータ、および移動ロボット |
| US11964399B2 (en) | 2018-09-07 | 2024-04-23 | Omron Corporation | Target object recognition device, manipulator, and mobile robot |
| CN111353416A (zh) * | 2020-02-26 | 2020-06-30 | 广东温氏种猪科技有限公司 | 基于牲畜三维测量的姿态检测方法、系统及存储介质 |
| CN111353416B (zh) * | 2020-02-26 | 2023-07-07 | 广东温氏种猪科技有限公司 | 基于牲畜三维测量的姿态检测方法、系统及存储介质 |
| JP7502641B2 (ja) | 2020-11-17 | 2024-06-19 | 株式会社デンソーウェーブ | 認識システム |
| JP7093881B1 (ja) | 2021-09-06 | 2022-06-30 | Dmg森精機株式会社 | システム、及び自動搬送車 |
| WO2023032400A1 (ja) * | 2021-09-06 | 2023-03-09 | Dmg森精機株式会社 | 自動搬送装置、及びシステム |
| JP2023037769A (ja) * | 2021-09-06 | 2023-03-16 | Dmg森精機株式会社 | システム、及び自動搬送車 |
| CN115359192A (zh) * | 2022-10-14 | 2022-11-18 | 阿里巴巴（中国）有限公司 | 三维重建与商品信息处理方法、装置、设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5839971B2 (ja) | 情報処理装置、情報処理方法及びプログラム | |
| JP5567908B2 (ja) | ３次元計測装置、その計測方法及びプログラム | |
| JP6370038B2 (ja) | 位置姿勢計測装置及び方法 | |
| JP2016170050A (ja) | 位置姿勢計測装置、位置姿勢計測方法及びコンピュータプログラム | |
| JP6573354B2 (ja) | 画像処理装置、画像処理方法、及びプログラム | |
| JP6465789B2 (ja) | デプスカメラの内部パラメータを算出するプログラム、装置及び方法 | |
| JP6271953B2 (ja) | 画像処理装置、画像処理方法 | |
| JP5624394B2 (ja) | 位置姿勢計測装置、その計測処理方法及びプログラム | |
| JP6021533B2 (ja) | 情報処理システム、装置、方法及びプログラム | |
| JP6180087B2 (ja) | 情報処理装置及び情報処理方法 | |
| JP6004809B2 (ja) | 位置姿勢推定装置、情報処理装置、情報処理方法 | |
| JP6324025B2 (ja) | 情報処理装置、情報処理方法 | |
| JP5393318B2 (ja) | 位置姿勢計測方法及び装置 | |
| JP5627325B2 (ja) | 位置姿勢計測装置、位置姿勢計測方法、およびプログラム | |
| JP5863440B2 (ja) | 情報処理装置および方法 | |
| JP6092530B2 (ja) | 画像処理装置、画像処理方法 | |
| US20130230235A1 (en) | Information processing apparatus and information processing method | |
| JP2011175477A (ja) | ３次元計測装置、処理方法及びプログラム | |
| JP5976089B2 (ja) | 位置姿勢計測装置、位置姿勢計測方法、およびプログラム | |
| JP5698815B2 (ja) | 情報処理装置、情報処理装置の制御方法及びプログラム | |
| JP2015007639A (ja) | 情報処理装置、情報処理方法およびプログラム | |
| JP6040264B2 (ja) | 情報処理装置、情報処理装置の制御方法、およびプログラム | |
| JP6890422B2 (ja) | 情報処理装置、情報処理装置の制御方法およびプログラム | |
| JP2011174891A (ja) | 位置姿勢計測装置、位置姿勢計測方法、及びプログラム | |
| JP6766229B2 (ja) | 位置姿勢計測装置及び方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20180309 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20190124 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20190129 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20190328 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20190702 |