JP5960642B2 - ３次元情報取得方法及び３次元情報取得装置 - Google Patents

Description

本発明は可動部を含む物体の３次元情報取得技術に関し、特に３次元情報取得方法及び３次元情報取得装置に関する。

可動部を含む物体をセンサにより計測し、当該可動部の３次元情報を取得する方法が提案されている。非特許文献１は、例えば引出部を備える引出や、扉を備える冷蔵庫などの物体に関し、それら可動部の３次元情報を取得する方法を開示している。当該方法は、物体の可動部を直方体と仮定した上で当該可動部を面群として検出し、時間に伴う面群の変化に基づいて可動部の３次元情報を取得する。

J. Sturm, K. Konolige, C. Stachniss, and W. Burgard, "3D Pose Estimation, Tracking and Model Learning of Articulated Objects from Dense Depth Video using Projected Texture Stereo,".(http://www.willowgarage.com/sites/default/files/sturm10rssws%5B1%5D.pdf).

しかしながら、上述した方法は、「直方体」等の可動部の形状情報を予め入力する必要があった。このため、可動部の形状情報を事前に必要とせずに、任意の形状を有する可動部に対して適用できないという問題があった。

本発明は、このような問題を解決するためになされたものであり、可動部の形状情報を事前に必要とせずに、任意の形状を有する可動部の３次元情報を取得可能な３次元情報取得方法及び３次元情報取得装置を提供することを目的とするものである。

一実施の形態に係る３次元情報取得方法は、対象物体までの距離値データを計測して当該距離値データの集合である距離点群を取得する距離センサを用いて、可動部を含む対象物体の動きを各時刻の距離点群として時系列に取得し、当該取得した各時刻の距離点群を記憶手段に記憶する時系列距離点群取得ステップと、前記各時刻の距離点群からそれぞれ複数の面を検出し、当該検出した各時刻の複数の面を各時刻の面群として前記記憶手段に記憶する面検出ステップと、前記各時刻の面群から第１の時刻の面群及び第２の時刻の面群を抽出し、当該抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較し、当該比較結果に基づいて同一の面同士を時刻間で対応付けする面対応付けステップと、前記対応付けされた面の中から第１の面を選定する第１の面選定ステップと、前記対応付けされた面の中から前記第１の面と一定時間の間同時に検出された第２の面を選定する第２の面選定ステップと、前記一定時間における前記第１の面の姿勢変化と前記第２の面の姿勢変化を比較する姿勢変化比較ステップと、前記姿勢変化比較ステップにおける比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一であった場合には、前記第２の面は前記第１の面と同一の前記可動部を構成する面であると判定する判定ステップと、前記姿勢変化比較ステップにおける比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一でなかった場合には、前記第２の面選定ステップからの処理を再び繰り返す繰り返し処理ステップと、を有する、ことを特徴とする。

これによって、可動部を含む物体の動きを時系列ごとに複数の面として記録し、記録した複数の面同士を時刻間で比較して対応付け、姿勢変化が互いに同一の面を同一の可動部を構成する面であると判定することによって、物体の可動部の表面や裏面を特定することができる。このため、可動部の形状情報を事前に必要とせずに、任意の形状を有する可動部の３次元情報を取得することができる。

また、前記判定ステップにおいて同一の前記可動部を構成する面であると判定された面の集合に基づいて、当該可動部の可動軸を推定する可動軸推定ステップを更に有する、ようにしてもよい。これによって、可動部の可動軸を推定することができる。

さらにまた、前記姿勢変化比較ステップは、前記一定時間における２つの時刻間の前記第１の面の位置及び姿勢の変化をそれぞれ示す平行移動ベクトル及び回転行列を計算し、前記２つの時刻のうちの１つの時刻の前記第２の面の位置及び姿勢に対して前記計算した平行移動ベクトル及び回転行列を作用させて前記第２の面の仮想的な位置及び姿勢を計算し、当該計算した前記第２の面の仮想的な位置及び姿勢と、前記２つの時刻のうちの他の１つの時刻の前記第２の面の位置及び姿勢とを比較する、ようにしてもよい。これにより、同時に移動していると考えられる２つの面を容易に判定することができる。

また、前記面対応付けステップは、前記各時刻の面群から第１の時刻の面群及び第２の時刻の面群を抽出し、当該抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較し、当該比較の結果、同一の面であると判定した場合には同一の識別番号をそれら面に対して割当て、同一の面でないと判定した場合にはそれら面に対して互いに異なる識別番号を割当て、前記割当てられた識別番号ごとに前記各時刻において当該識別番号の面が検出されたか否かを示す面追跡行列を生成して前記記憶手段に記憶し、前記第１の面選定ステップは、前記面追跡行列を参照して、前記対応付けされた面の中から最も長期間にわたって検出された前記第１の面を選定し、前記第２の面選定ステップは、前記面追跡行列を参照して、前記対応付けされた面の中から前記第１の面と一定時間の間同時に検出された第２の面を選定する、ようにしてもよい。これにより、どの面がどの期間に存在していたかを容易に調べることができ、また、同期間に存在していた面を容易に探索することができる。

さらにまた、前記面検出ステップは、領域拡張法を利用することにより、前記各時刻の距離点群からそれぞれ複数の面を検出する、ようにしてもよい。これにより、平面検出法を適用して面を検出する場合と比較して、ある程度の曲率を持った面であっても検出することができる。

また、前記面対応付けステップは、前記各時刻の面群の各面に対して、各面の輪郭線情報に基づく特徴量ヒストグラムを前記各時刻の距離点群から計算し、当該特徴量ヒストグラムを比較することにより、前記抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較する、ようにしてもよい。これにより、特徴量ヒストグラム同士の比較により、２つの面同士の比較を行うことができる。

他の一実施の形態に係る３次元情報取得装置は、対象物体までの距離値データを計測して当該距離値データの集合である距離点群を取得する距離センサと、記憶手段と、を備える３次元情報取得装置であって、可動部を含む対象物体の動きを各時刻の距離点群として時系列に取得し、当該取得した各時刻の距離点群を前記記憶手段に記憶する時系列距離点群取得手段と、前記各時刻の距離点群からそれぞれ複数の面を検出し、当該検出した各時刻の複数の面を各時刻の面群として前記記憶手段に記憶する面検出手段と、前記各時刻の面群から第１の時刻の面群及び第２の時刻の面群を抽出し、当該抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較し、当該比較結果に基づいて同一の面同士を時刻間で対応付けする面対応付け手段と、前記対応付けされた面の中から第１の面を選定する第１の面選定手段と、前記対応付けされた面の中から前記第１の面と一定時間の間同時に検出された第２の面を選定する第２の面選定手段と、前記一定時間における前記第１の面の姿勢変化と前記第２の面の姿勢変化を比較する姿勢変化比較手段と、前記姿勢変化比較手段による比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一であった場合には、前記第２の面は前記第１の面と同一の前記可動部を構成する面であると判定する判定手段と、前記姿勢変化比較手段による比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一でなかった場合には、前記第２の面選定手段からの処理を再び繰り返す繰り返し処理手段と、を有する、ものである。

これによって、可動部を含む物体の動きを時系列ごとに複数の面として記録し、記録した複数の面同士を時刻間で比較して対応付け、姿勢変化が互いに同一の面を同一の可動部を構成する面であると判定することによって、物体の可動部の表面や裏面を特定することができる。このため、可動部の形状情報を事前に必要とせずに、任意の形状を有する可動部の３次元情報を取得することができる。

本発明によれば、可動部の形状情報を事前に必要とせずに、任意の形状を有する可動部の３次元情報を取得可能な３次元情報取得方法及び３次元情報取得装置を提供することを目的とすることができる。

実施の形態１に係る３次元情報取得装置の構成を示す機能ブロック図である。 実施の形態１に係る３次元情報取得方法による処理を示すフローチャートである。 実施の形態１に係る３次元情報取得方法による処理を示すフローチャートである。 実施の形態１に係る可動部を操作する様子を示す図である 実施の形態１に係る距離センサにより取得される距離点群を示す模式図である。 実施の形態１に係る同一面に属する距離点群の分類手法を説明するための図である。 実施の形態１に係る同一面の特定手法を説明するための図である。 実施の形態１に係る面追跡行列を示す図である。 実施の形態１に係る代表面の選定手法を説明するための図である。

実施の形態１．
図１に、本実施の形態に係る３次元情報取得装置１００の構成例を示す。同図に示すように、本実施の形態に係る３次元情報取得装置１００は、距離センサ１０と、時系列計測距離データ記憶部１１と、面群検出部１２と、面基準座標系計算部１３と、時系列面群基準座標記憶部１４と、面識別番号割当部１５と、同一面特定部１６と、面追跡行列生成部１７と、代表面選定部１８と、２面同時検出時刻特定部１９と、２座標間の姿勢変化計算部２０と、２面間マッチング評価部２１と、可動軸推定部２２と、記憶部３０と、を備えている。

なお、図１に示した各部（時系列計測距離データ記憶部１１、時系列計測距離データ記憶部１１、面群検出部１２、面基準座標系計算部１３、時系列面群基準座標記憶部１４、面識別番号割当部１５、同一面特定部１６、面追跡行列生成部１７、代表面選定部１８、２面同時検出時刻特定部１９、２座標間の姿勢変化計算部２０、２面間マッチング評価部２１、可動軸推定部２２）の少なくとも一部は、独立した装置として実現してもよい。またこれら各部を、ＣＰＵ（Central Processing Unit）、ＭＰＵ（Micro Processing Unit）などのプロセッサを含むコンピュータシステムにインストールし、１又は複数のプログラムを実行させることで、その機能を実現するソフトウェアとして実現してもよい。

距離センサ１０は、距離センサ１０の視点位置からの対象物体までの距離値データを計測し、この距離値データの集合である距離点群を取得する。距離センサ１０としては、例えば、TOF（Time Of Flight）方式のカメラ、ステレオカメラ、Microsoft社製のKinect（登録商標）などを用いることができる。記憶部３０は、例えば、メモリやハードディスクなどのデータ記憶手段である。

時系列計測距離データ記憶部１１は、距離センサ１０により取得した距離点群である計測距離データを、当該距離点群を取得した時刻と対応付けて記憶部３０に記憶する。なお、時系列計測距離データ記憶部１１は、時系列距離点群取得手段の一例に対応する。

面群検出部１２は、各時刻フレームの距離点群からそれぞれ複数の面を検出し、検出した複数の面を面群として記憶部３０に記憶する。なお、面群検出部１２は、面検出手段の一例に対応する。

面基準座標系計算部１３は、各面に属する距離点群に基づいて、各面の基準座標及びその基準座標系を計算する。また、時系列面群基準座標記憶部１４は、面基準座標系計算部１３により計算した各面の基準座標及びその基準座標系を、当該各面の距離点群を取得した時刻と対応付けて記憶部３０に記憶する。

面識別番号割当部１５は、面群の各面に対して一意の識別番号をそれぞれ割当てる。同一面特定部１６は、ある入力面と同一の面を特定する。面追跡行列生成部１７は、どの面がいつからいつまで検出されたかを記述した面追跡行列を生成して、当該面追跡行列を記憶部３０に記憶する。なお、面識別番号割当部１５、同一面特定部１６、面追跡行列生成部１７は、面対応付け手段の一例に対応する。

代表面選定部１８は、検出された全ての面の中から注目する代表面を選定する。なお、代表面選定部１８は、第１の面選定手段の一例に対応する。２面同時検出時刻特定部１９は、入力された２つの面が同時に検出された時刻を特定する。

２座標間の姿勢変化計算部２０は、入力された面について、２つの時刻における面の基準座標及び基準座標系に基づき、２つ時刻間での基準座標間の平行移動ベクトル及び回転行列を計算する。２面間マッチング評価部２１は、入力された２つの面を比較してその一致度（マッチング）を評価する。なお、２座標間の姿勢変化計算部２０及び２面間マッチング評価部２１は、姿勢変化比較手段及び判定手段の一例に対応する。

可動軸推定部２２は、同一の可動部を構成する面であると判定された面の集合に基づいて、その可動部の可動軸を推定する。なお、可動軸推定部２２は、可動軸推定手段の一例に対応する。

図２及び３は、３次元情報取得装置１００により実行される３次元情報取得方法の処理手順を示すフローチャートである。なお、本実施の形態では、図に示すフローチャートは、ＣＰＵが各部の機能を実現するプログラムを実行することにより実現される。

Ｓ１０１：３次元情報取得装置１００は、人又はロボットが物体の可動部を操作する様子を、距離センサ１０により計測する。距離センサ１０は、時刻ごとに各時刻フレームの距離点群を取得する。時系列計測距離データ記憶部１１は、距離センサ１０により取得した距離点群である計測距離データを、当該距離点群を取得した時刻と対応付けて記憶部３０に記憶する。

図４に、物体の可動部の一例としての冷蔵庫の扉を示す。図４では、人が冷蔵庫の扉を開ける様子を示す。図４の各図は各時刻における様子を示しており、左図は扉を開ける前の状態、中央図は扉を開ける途中の状態、右図は扉を開けた後の状態をそれぞれ示す。距離センサ１０により扉が開けられる様子が計測されて、各時刻における距離点群が取得される。

なお、図４において、実線及び破線により囲って示す範囲は、後述する処理によって検出される面群の例を示す。丸により囲って示す数字１〜４は、後述する処理によって割当てられる面の識別番号の例を示す。また、実線により囲って示す範囲は、後述する処理によって検出される可動部の面群の例を示す。破線により囲って示す範囲は、後述する処理によって検出される可動部ではない面群の例を示す。

３次元情報取得装置１００は、記憶部３０に記憶された時刻０〜Ｔの各時刻フレームにおける距離点群に対して、以下のＳ１０２〜Ｓ１０４に示す処理をそれぞれ実行する。
Ｓ１０２：面群検出部１２は、同一の時刻フレームにおける距離点群に基づいてＮｔ個の面を検出する。これらＮｔ個の面からなる集合を面群と称する。また、本実施の形態では、面群検出部１２は、同一の時刻フレームにおける距離点群を、Ｎｔ個のグループに対して分類する。これらＮｔ個のグループは、Ｎｔ個の面にそれぞれ対応する。即ち、同一のグループに属する距離点群は、同一の面に属する。面群検出部１２は、各時刻における距離点群がそれぞれ属する複数のグループ（即ち、各時刻において検出する複数の面に対応する。）を記憶部３０に記憶する。

図５及び６を参照して、面群検出部１２による処理の具体例を説明する。なお、面群検出部１２による処理はこれに限定されず、距離点群から面を検出する他の周知の処理を実行するものとしてもよい。また、距離点群を分類する他の周知の処理を実行するものとしてもよい。

本実施の形態では、面群検出部１２は、Region Growing（領域拡張法）と呼ばれる手法を利用して、距離点群からの面の検出と距離点群の分類を行う。当該手法は、ある基点を選択した後に、その基点の近傍において基点と性質が近いものは同じグループに属するとして分類してゆき、同一グループの領域を拡張していく。このような手法を採用することにより、平面検出法を適用して面要素を抽出する手法と比較して、ある程度の曲率を有する面に対しても適用できるという利点を奏する。

面群検出部１２は、同一の時刻フレームにおける距離点群について、ある１つの距離点の距離値ｄ_１とその近傍の距離点の距離値ｄ_２について、それぞれの距離点の法線ベクトルであるｎ_１及びｎ_２を計算する。そして、面群検出部１２は、類似する法線ベクトルを互いに有し、かつ、距離値が互いに大きく変化しない距離点を同一のグループに分類する。具体的には、面群検出部１２は、近傍の距離点について、距離点間の距離及び法線ベクトルの向きの類似を判定する以下の式１を満たす場合に、同一のグループに分類する。なお、Ｄ_{Ｔｈｒｅｓｈｏｌｄ}及びＣ_{Ｔｈｒｅｓｈｏｌｄ}については、予め適当な値が設定される。

図５の左図は計測対象の物体を例示し、右図は距離センサ１０により取得された距離点群を例示する。図６は、図５の右図に示した距離点群に基づいて面群検出部１２により計算された法線を示す。類似する法線ベクトルを有する距離点が同一のグループに分類されて、同一のグループに属する距離点が同一の平面に属する。

Ｓ１０３：面基準座標系計算部１３は、同一の時刻の時刻フレームにおいて検出されたＮｔ個の面について、それぞれＮｔ個の基準座標及びその基準座標系を計算する。

面基準座標系計算部１３による処理の具体例を説明する。面基準座標系計算部１３は、同一時刻の同一グループの距離点群について、当該距離点群に含まれる距離点の距離値の平均値（３次元位置）を計算し、この平均値を基準座標として計算する。また、面基準座標系計算部１３は、同一時刻の同一グループの距離点群について、当該距離点群に含まれる距離点の距離値（３次元位置）について主成分分析を行い３つの固有ベクトルを計算する。面基準座標系計算部１３は、基準座標を原点とし、主成分分析により得た３つの固有ベクトルをＸ、Ｙ、Ｚ軸とする面基準座標系を得る。なお、面基準座標系計算部１３による処理はこれに限定されず、距離点群に基づいて各面の基準座標及びその基準座標系を計算する他の周知の処理を実行するものとしてもよい。

時系列面群基準座標記憶部１４は、面基準座標系計算部１３により計算した各面の基準座標及びその基準座標系を、当該各面の距離点群を取得した時刻と対応付けて記憶部３０に記憶する。

ここまでの処理により、記憶部３０には、各時刻における距離点群と、各時刻における面群と、各時刻における各面の基準座標及び基準座標系と、が記憶される。本実施の形態では、記憶部３０に記憶された時系列の面群Ｓ_ｔを、以下に示す。
ｓ^ｉ _ｔ∈Ｓ_ｔ＝｛ｓ^１ _ｔ，ｓ^２ _ｔ，...，ｓ^ｎｔ _ｔ｝
ここで、ｓ^ｉ _ｔは、時刻ｔの時刻フレームに含まれるｉ番目の面を示す。Ｓ_ｔは、時刻ｔの時刻フレームに含まれる面群の集合を示す。ｎ_ｔは、時刻ｔの時刻フレームにおいて検出された面の総数を示す。

Ｓ１０５：面識別番号割当部１５は、まず、時刻０の時刻フレームにおいて検出された面群の各面に対して、一意に識別可能な識別番号をそれぞれ割当てる。

３次元情報取得装置１００は、以下、時刻０から開始して、時刻ｔ及び時刻ｔ＋１の時刻フレームに含まれる面群について、以下のＳ１０６〜Ｓ１０８に示す処理をそれぞれ順に実行する。

Ｓ１０６：３次元情報取得装置１００は、時刻ｔの面群Ｓ_ｔと時刻ｔ＋１の面群Ｓ_ｔ＋１を記憶部３０から読み出す。
Ｓ１０７：同一面特定部１６は、時刻ｔの面群Ｓ_ｔ＝｛ｓ^１ _ｔ，ｓ^２ _ｔ，...，ｓ^ｎｔ _ｔ｝に含まれる面と、時刻ｔ＋１の面群Ｓ_ｔ＝｛ｓ^１ _ｔ＋１，ｓ^２ _ｔ＋１，...，ｓ^ｎｔ _ｔ＋１｝に含まれる面とを比較し、同一の面を特定する。同一面特定部１６による比較の結果、時刻ｔ＋１の面群Ｓ_ｔ＋１において、"時刻ｔの面群Ｓ_ｔに含まれる面"と同一の面が存在する場合には、面識別番号割当部１５は、時刻ｔ＋１の面群Ｓ_ｔ＋１におけるその同一の面に対して、時刻ｔの面群Ｓ_ｔにおける面の識別番号と同一の識別番号を割当てる。他方で、時刻ｔ＋１の面群Ｓ_ｔ＋１において、"時刻ｔの面群Ｓ_ｔに含まれない面"が存在する場合には、面識別番号割当部１５は、時刻ｔ＋１の面群Ｓ_ｔ＋１における"時刻ｔの面群Ｓ_ｔに含まれない面"に対して、新たな識別番号を割当てる。

同一面特定部１６による処理の具体例を説明する。同一面特定部１６は、各面の輪郭線情報に基づく特徴量ヒストグラムを計算し、当該特徴量ヒストグラムを比較することにより、時刻ｔの面群Ｓ_ｔに含まれる面と時刻ｔ＋１の面群Ｓ_ｔ＋１に含まれる面との間で、同一面を特定することができる。この特徴量ヒストグラムは、Shape Context（形状コンテキスト）と呼ばれる。即ち、異なる時刻間において同一面を特定できるため、異なる時刻間にわたって同一面を追跡することができる。

同一面特定部１６によるShape Contextの計算方法をより詳細に説明する。まず、同一面特定部１６は、各面に属する距離点群について主成分分析を行い第１主成分と第２主成分を計算する。同一面特定部１６は、第１主成分と第２主成分にそれぞれ対応する固有ベクトルが張る２次元平面を求める。同一面特定部１６は、この２次元平面上に距離点群を２次元射影する。そして、同一面特定部１６は、この２次元平面の輪郭部分に位置する点のみを抽出する。この処理は、距離点群における近傍探索を行ったときに、近傍の数が一定以下であった距離点のみを残すことにより実現する。

そして、この輪郭情報に対してShape Contextと同様の方法に特徴量ヒストグラムを生成する。即ち、同一面特定部１６は、面の中心から放射状に区切った平面領域において、輪郭点が存在する区切り（ｂｉｎ）には、距離点の数だけの投票を行い、この投票結果から、中心からの距離ｒと水平線に対するｂｉｎの角度θを軸とする２次元ヒストグラムを生成する。なお、このヒストグラムのｒ軸方向のｂｉｎ数は面の大きさに依存した値になる。このようにして生成した２次元ヒストグラムを、特徴量ヒストグラムと称する。

図７は、特徴量ヒストグラムの生成過程を説明する図である。同図（１）は３次元の面を示し、同図（２）は２次元平面上に射影した面を示し、同図（３）は特徴量ヒストグラムを示す。なお、同一面特定部１６による処理はこれに限定されず、ある入力面と同一の面を特定する他の周知の処理を実行するものとしてもよい。

Ｓ１０８：面追跡行列生成部１７は、面追跡行列を生成して記憶部３０に記憶する。面追跡行列は、識別番号ごとに各時刻において当該識別番号の面が検出されたか否かを示す行列である。図８に、面追跡行列の例を示す。図において、各時刻において各識別番号の面が検出されなかった場合には０を、検出された場合には１を記入している。なお、面追跡行列生成部１７による処理はこれに限定されず、各面が検出された時間を記述する他の周知のデータ形式の情報を計算して記憶するものとしてもよい。

Ｓ１０９：代表面選定部１８は、記憶部３０に記憶された面追跡行列を参照して、第１の面の一例としての代表面Ｓ^ｒｅｆを選定する。

代表面選定部１８による処理の具体例を説明する。代表面選定部１８は、面追跡行列を参照して、代表面Ｓ^ｒｅｆを選定する。代表面選定部１８は、例えば最も長時間にわたって検出された面であり、かつ、動いている面であるという条件を満たす面を、代表面Ｓ^ｒｅｆとして選定する。なお、面が動いているか否かは、記憶部３０に記憶された各面の基準座標を参照し、基準座標に変化があった場合には面が動いていると判定することができる。代表面選定部１８は、例えば図９に示すようにして、上記条件を満たす代表面Ｓ^ｒｅｆを選定する。なお、代表面選定部１８による処理はこれに限定されず、他の条件を満たす面を代表面として選定するものとしてもよい。

３次元情報取得装置１００は、代表面Ｓ^ｒｅｆ以外の各面について、以下のＳ１１０〜Ｓ１１７に示す処理を繰り返して実行する。なお、この繰り返し処理を実行する場合の３次元情報取得装置１００は、繰り返し処理手段の一例に対応する。
Ｓ１１０：３次元情報取得装置１００は、第２の面の一例として、代表面Ｓ^ｒｅｆの識別番号と異なる識別番号を有する面Ｓ^ｃを選択する。なお、この選択の際の３次元情報取得装置１００は、第２の面選定手段の一例に対応する。

Ｓ１１１：２面同時検出時刻特定部１９は、代表面Ｓ^ｒｅｆと面Ｓ^ｃとが同時に検出された時刻を特定する。なお、代表面Ｓ^ｒｅｆと面Ｓ^ｃとが同時に検出された時刻が無かった場合には、Ｓ１０１へと戻り、他の面を選択する。

２面同時検出時刻特定部１９による処理の具体例を説明する。２面同時検出時刻特定部１９は、記憶部３０に記憶された面追跡行列を参照して、代表面Ｓ^ｒｅｆと面Ｓ^ｃとが同時に検出された時刻を特定する。２面同時検出時刻特定部１９は、例えば図９に示す面追跡行列を参照して、代表面Ｓ^ｒｅｆと面Ｓ^ｃとが同時に検出された時刻２〜６を容易に特定することができる。なお、２面同時検出時刻特定部１９による処理はこれに限定されず、入力された２つの面が同時に検出された時刻を特定する他の周知の処理を実行するものとしてもよい。

Ｓ１１２：３次元情報取得装置１００は、代表面Ｓ^ｒｅｆと面Ｓ^ｃとが同時に検出された時刻があった場合には、代表面Ｓ^ｒｅｆと面Ｓ^ｃとが同時に検出された時刻の中から、２つの時刻ｔ，ｔｄを選択する。２つの時刻ｔ，ｔｄを選択する処理としては様々な選択処理を採用することができる。例えば同時に検出された時刻の中から最も長い時間にわたって検出されたその開始時刻ｔと終了時刻ｔｄを選択する。また、例えばランダムサンプリングにより２つの時刻ｔ，ｔｄを選択するものとしてもよい。

Ｓ１１３：２座標間の姿勢変化計算部２０は、時刻ｔ，ｔｄにおける面Ｓ^ｒｅｆの基準座標及び基準座標系を記憶部３０からそれぞれ読み出し、読み出した面Ｓ^ｒｅｆの基準座標及び基準座標系に基づいて、時刻ｔから時刻ｔｄへの面Ｓ^ｒｅｆの平行移動ベクトルＴ^Ｓｃ _{ｔｄ（ｖｉｒｔｕａｌ）}及び回転行列Ｒ^Ｓｃ _{ｔｄ（ｖｉｒｔｕａｌ）}を計算する。

２座標間の姿勢変化計算部２０による処理の具体例を説明する。２座標間の姿勢変化計算部２０は、例えば以下の式２に基づいて、時刻ｔから時刻ｔｄにかけての代表面Ｓ^ｒｅｆの位置及び姿勢の変化をそれぞれ示す平行移動ベクトル及び回転行列を計算する。ここで、Ｔ^Ｓｒｅｆ _ｔ、Ｔ^Ｓｒｅｆ _ｔｄは、時刻ｔ、ｔｄの面Ｓ^ｒｅｆの基準座標に対する、原点からの位置ベクトルにそれぞれ対応し、Ｒ^Ｓｃ _ｔ、Ｒ^Ｓｃ _ｔｄは、時刻ｔ、ｔｄの面Ｓ^ｒｅｆの基準座標系に対する、原点座標系からの回転行列にそれぞれ対応する。また、Ｔ^Ｓｃ _ｔは、時刻ｔの面Ｓ^ｃの基準座標に対する原点からの位置ベクトルに対応し、Ｒ^Ｓｃ _ｔは、時刻ｔの面Ｓ^ｒｅｆの基準座標系に対する原点座標系からの回転行列に対応する。なお、２座標間の姿勢変化計算部２０による処理はこれに限定されず、２つの基準座標間の平行移動ベクトル及び回転行列を計算する他の周知の処理を実行するものとしてもよい。

Ｓ１１４：３次元情報取得装置１００は、時刻ｔの面Ｓ^ｃ _ｔに対して、２座標間の姿勢変化計算部２０により計算した平行移動ベクトルＴ^Ｓｃ _{ｔｄ（ｖｉｒｔｕａｌ）}及び回転行列Ｒ^Ｓｃ _{ｔｄ（ｖｉｒｔｕａｌ）}を作用することにより、時刻ｔｄにおける仮想的な面Ｓ^ｃ' _ｔｄを計算する。

Ｓ１１５：２面間マッチング評価部２１は、時刻ｔ＋１における実際の面Ｓ^ｃ _ｔｄと仮想的な面Ｓ^ｃ' _ｔｄとを比較し、その一致度を評価する。
Ｓ１１６：２面間マッチング評価部２１は、時刻ｔ＋１における実際の面Ｓ^ｃ _ｔｄと仮想的な面Ｓ^ｃ' _ｔｄとが一致したか否かを判定する。なお、これら２つの面が一致しないと判定した場合には、Ｓ１１０へと戻り、他の面を選択する。３次元情報取得装置１００は、Ｓ１１５及びＳ１１６に示した処理を実行することにより、代表面Ｓ^ｒｅｆと面Ｓ^ｃとが同じ運動（姿勢変化）をしているか否かを判定することができる。

２面間マッチング評価部２１による処理の具体例を説明する。入力された２つの面の一致度を評価及び判定する処理としては様々な評価及び判定処理を採用することができる。例えば、面Ｓ^ｃ _ｔｄに属する距離点群と面Ｓ^ｃ' _ｔｄに属する距離点群との間で誤差を計算し、その平均誤差が所定の閾値より小さい場合に、２つの面は一致するものと判定する。また、例えば２つの面の基準座標及び基準座標系の間の差が所定の閾値より小さい場合に、２つの面は一致するものと単純に判定してもよい。なお、２面間マッチング評価部２１による処理はこれに限定されず、入力された２つの面の一致度を評価及び判定する他の周知の処理を実行するものとしてもよい。

Ｓ１１７：３次元情報取得装置１００は、時刻ｔ＋１における実際の面Ｓ^ｃ _ｔｄと仮想的な面Ｓ^ｃ' _ｔｄとが一致したと判定した場合には、面Ｓ^ｃは「代表面Ｓ^ｒｅｆと同じ可動部を構成する面」であると判定し、「代表面Ｓ^ｒｅｆと同じ可動部を構成する面」の集合に対して面Ｓ^ｃを追加して、記憶部３０に記憶する。なお、入力された２つの面の一致度を評価及び判定する処理に関して、ステップＳ１１２に戻り他の時刻を再びサンプリングした上で、ステップＳ１１３〜Ｓ１１６に示した処理を繰り返し、２つの面の一致度の評価値を積算するものとしてもよい。

Ｓ１１８：３次元情報取得装置１００は、「代表面Ｓ^ｒｅｆと同じ可動部を構成する面」の集合に面が存在するか否かを判定する。なお、面が存在しないと判定した場合には、Ｓ１０９へと戻り、他の代表面Ｓ^ｒｅｆを選択する。

以上の処理により、可動部に属する面を時系列データとして抽出することができる。即ち、物体の可動部について、可動部の形状を面の集合によりモデル化することができる。

Ｓ１１９：可動軸推定部２２は、「代表面Ｓ^ｒｅｆと同じ可動部を構成する面」の集合に面が存在すると判定された場合には、その可動部を構成する面であると判定された面の集合に基づいて、可動部の可動軸を推定する。これにより、可動軸パラメータについてもモデル化することが可能であるため、可動軸を推定することができる。

可動軸推定部２２による処理の具体例を説明する。同一の可動部を構成する面について、それら面の動き方向のベクトルに垂直な方向に可動軸は存在するものと考えられる。このため、可動軸推定部２２は、同一の可動部を構成する面の動きの軌跡に基づいて、可動軸を推定することができる。

可動軸推定部２２は、例えば代表面Ｓ^ｒｅｆの時系列変化を利用して可動軸を推定する。可動軸推定部２２は、まず、時刻ｓｔからｅｔにおける各ｓ^ｒｅｆから、面の中心座標のリストＸ＝｛ｘ_ｓｔ，ｘ_ｓｔ＋１，...，ｘ_ｅｔ｝を得る。そして、可動軸推定部２２は、リストＸの差分ベクトルＶ＝｛ｖ_１，ｖ_２，...，ｘ_{ｅｔ−ｓｔ−１}｝を計算する。そして、可動軸推定部２２は、ｖ_ｉに垂直で、ａ_ｉ＝ｘ_ｓｔ＋ｉ＋０．５×ｖ_ｉを通る平面を計算し、平面群を計算する。この平面群から２つの平面を取り出し、その交線を計算することを繰り返し、直線群Ｌ＝｛ｌ_１，ｌ_２，...，ｌ_{ｅｔ−ｓｔ−１}｝を計算する。可動軸推定部２２は、直線ｌを順次選択し、リストＸとの距離を計算する。各点の軸からの距離がほぼ等しいのであれば、その直線は円筒の中心軸を近似したものと見なすことができ、この直線を可動軸として推定する。

また、可動軸推定部２２は、代表面Ｓ^ｒｅｆに加えて、同一の可動部を構成する他の面Ｓ^ｃに基づいて可動軸を推定するものとしてもよい。可動軸推定部２２は、例えば同一の可動部を構成する代表面Ｓ^ｒｅｆ及び面Ｓ^ｃを全て読み出し、それら読み出した面の時刻間での動き方向のベクトルを計算し、当該計算した動き方向のベクトルに垂直な方向の面を計算して、当該面の交線を可動軸として計算してもよい。

なお、可動軸推定部２２による処理はこれに限定されず、同一の可動部を構成する面であると判定された面の動きの軌跡に基づいて、可動部の可動軸を推定する他の周知の処理を実行するものとしてもよい。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述した実施の形態に係る３次元情報取得装置１００は、距離センサ１を有するロボットに搭載して実行することもできる。

１００ ３次元情報取得装置、
１０ 距離センサ、
１１ 時系列計測距離データ記憶部、
１２ 面群検出部、
１３ 面基準座標系計算部、
１４ 時系列面群基準座標記憶部、
１５ 面識別番号割当部、
１６ 同一面特定部、
１７ 面追跡行列生成部、
１８ 代表面選定部、
１９ ２面同時検出時刻特定部、
２０ ２座標間の姿勢変化計算部、
２１ ２面間マッチング評価部、
２２ 可動軸推定部、
３０ 記憶部、

Claims (12)

1. 対象物体までの距離値データを計測して当該距離値データの集合である距離点群を取得する距離センサを用いて、可動部を含む対象物体の動きを各時刻の距離点群として時系列に取得し、当該取得した各時刻の距離点群を記憶手段に記憶する時系列距離点群取得ステップと、
   前記各時刻の距離点群からそれぞれ複数の面を検出し、当該検出した各時刻の複数の面を各時刻の面群として前記記憶手段に記憶する面検出ステップと、
   前記各時刻の面群から第１の時刻の面群及び第２の時刻の面群を抽出し、当該抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較し、当該比較結果に基づいて同一の面同士を時刻間で対応付けする面対応付けステップと、
   前記対応付けされた面の中から第１の面を選定する第１の面選定ステップと、
   前記対応付けされた面の中から前記第１の面と一定時間の間同時に検出された第２の面を選定する第２の面選定ステップと、
   前記一定時間における前記第１の面の姿勢変化と前記第２の面の姿勢変化を比較する姿勢変化比較ステップと、
   前記姿勢変化比較ステップにおける比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一であった場合には、前記第２の面は前記第１の面と同一の前記可動部を構成する面であると判定する判定ステップと、
   前記姿勢変化比較ステップにおける比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一でなかった場合には、前記第２の面選定ステップからの処理を再び繰り返す繰り返し処理ステップと、を有する、
   ことを特徴とする３次元情報取得方法。
2. 前記判定ステップにおいて同一の前記可動部を構成する面であると判定された面の集合に基づいて、当該可動部の可動軸を推定する可動軸推定ステップを更に有する、
   ことを特徴とする請求項１に記載の３次元情報取得方法。
3. 前記姿勢変化比較ステップは、
   前記一定時間における２つの時刻間の前記第１の面の位置及び姿勢の変化をそれぞれ示す平行移動ベクトル及び回転行列を計算し、前記２つの時刻のうちの１つの時刻の前記第２の面の位置及び姿勢に対して前記計算した平行移動ベクトル及び回転行列を作用させて前記第２の面の仮想的な位置及び姿勢を計算し、当該計算した前記第２の面の仮想的な位置及び姿勢と、前記２つの時刻のうちの他の１つの時刻の前記第２の面の位置及び姿勢とを比較する、
   ことを特徴とする請求項１又は２に記載の３次元情報取得方法。
4. 前記面対応付けステップは、
   前記各時刻の面群から第１の時刻の面群及び第２の時刻の面群を抽出し、当該抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較し、当該比較の結果、同一の面であると判定した場合には同一の識別番号をそれら面に対して割当て、同一の面でないと判定した場合にはそれら面に対して互いに異なる識別番号を割当て、前記割当てられた識別番号ごとに前記各時刻において当該識別番号の面が検出されたか否かを示す面追跡行列を生成して前記記憶手段に記憶し、
   前記第１の面選定ステップは、
   前記面追跡行列を参照して、前記対応付けされた面の中から最も長期間にわたって検出された前記第１の面を選定し、
   前記第２の面選定ステップは、
   前記面追跡行列を参照して、前記対応付けされた面の中から前記第１の面と一定時間の間同時に検出された第２の面を選定する、
   ことを特徴とする請求項１から３いずれか１項に記載の３次元情報取得方法。
5. 前記面検出ステップは、
   領域拡張法を利用することにより、前記各時刻の距離点群からそれぞれ複数の面を検出する、
   ことを特徴とする請求項１から３いずれか１項に記載の３次元情報取得方法。
6. 前記面対応付けステップは、
   前記各時刻の面群の各面に対して、各面の輪郭線情報に基づく特徴量ヒストグラムを前記各時刻の距離点群から計算し、当該特徴量ヒストグラムを比較することにより、前記抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較する、
   ことを特徴とする請求項１から４いずれか１項に記載の３次元情報取得方法。
7. 対象物体までの距離値データを計測して当該距離値データの集合である距離点群を取得する距離センサと、記憶手段と、を備える３次元情報取得装置であって、
   可動部を含む対象物体の動きを各時刻の距離点群として時系列に取得し、当該取得した各時刻の距離点群を前記記憶手段に記憶する時系列距離点群取得手段と、
   前記各時刻の距離点群からそれぞれ複数の面を検出し、当該検出した各時刻の複数の面を各時刻の面群として前記記憶手段に記憶する面検出手段と、
   前記各時刻の面群から第１の時刻の面群及び第２の時刻の面群を抽出し、当該抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較し、当該比較結果に基づいて同一の面同士を時刻間で対応付けする面対応付け手段と、
   前記対応付けされた面の中から第１の面を選定する第１の面選定手段と、
   前記対応付けされた面の中から前記第１の面と一定時間の間同時に検出された第２の面を選定する第２の面選定手段と、
   前記一定時間における前記第１の面の姿勢変化と前記第２の面の姿勢変化を比較する姿勢変化比較手段と、
   前記姿勢変化比較手段による比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一であった場合には、前記第２の面は前記第１の面と同一の前記可動部を構成する面であると判定する判定手段と、
   前記姿勢変化比較手段による比較の結果、前記一定時間における前記第１の面の姿勢変化及び前記第２の面の姿勢変化が同一でなかった場合には、前記第２の面選定手段からの処理を再び繰り返す繰り返し処理手段と、を有する、
   ことを特徴とする３次元情報取得装置。
8. 前記判定手段により同一の前記可動部を構成する面であると判定された面の集合に基づいて、当該可動部の可動軸を推定する可動軸推定手段を更に有する、
   ことを特徴とする請求項７に記載の３次元情報取得装置。
9. 前記姿勢変化比較手段は、
   前記一定時間における２つの時刻間の前記第１の面の位置及び姿勢の変化をそれぞれ示す平行移動ベクトル及び回転行列を計算し、前記２つの時刻のうちの１つの時刻の前記第２の面の位置及び姿勢に対して前記計算した平行移動ベクトル及び回転行列を作用させて前記第２の面の仮想的な位置及び姿勢を計算し、当該計算した前記第２の面の仮想的な位置及び姿勢と、前記２つの時刻のうちの他の１つの時刻の前記第２の面の位置及び姿勢とを比較する、
   ことを特徴とする請求項７又は８に記載の３次元情報取得装置。
10. 前記面対応付け手段は、
    前記各時刻の面群から第１の時刻の面群及び第２の時刻の面群を抽出し、当該抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較し、当該比較の結果、同一の面であると判定した場合には同一の識別番号をそれら面に対して割当て、同一の面でないと判定した場合にはそれら面に対して互いに異なる識別番号を割当て、前記割当てられた識別番号ごとに前記各時刻において当該識別番号の面が検出されたか否かを示す面追跡行列を生成して前記記憶手段に記憶し、
    前記第１の面選定手段は、
    前記面追跡行列を参照して、前記対応付けされた面の中から最も長期間にわたって検出された前記第１の面を選定し、
    前記第２の面選定手段は、
    前記面追跡行列を参照して、前記対応付けされた面の中から前記第１の面と一定時間の間同時に検出された第２の面を選定する、
    ことを特徴とする請求項７から９いずれか１項に記載の３次元情報取得装置。
11. 前記面検出手段は、
    領域拡張法を利用することにより、前記各時刻の距離点群からそれぞれ複数の面を検出する、
    ことを特徴とする請求項７から１０いずれか１項に記載の３次元情報取得装置。
12. 前記面対応付け手段は、
    前記各時刻の面群の各面に対して、各面の輪郭線情報に基づく特徴量ヒストグラムを前記各時刻の距離点群から計算し、当該特徴量ヒストグラムを比較することにより、前記抽出した前記第１の時刻の面群に含まれる面と前記第２の時刻の面群に含まれる面とを比較する、
    ことを特徴とする請求項７から１１いずれか１項に記載の３次元情報取得装置。