JP2011090465A - 物体位置推定装置および物体位置推定プログラム - Google Patents

Abstract

【課題】少ない計算量で物体の三次元位置を推定することを課題とする。
【解決手段】物体位置推定装置１は、局所特徴抽出部２、三次元座標算出部３、サンプル設定部４、サンプル遷移部５、尤度算出部６、推定部７を有する。局所特徴抽出部２は、撮像装置により撮像されたステレオ画像から、所定の領域の中で特徴のある画素を局所特徴として抽出する。三次元座標算出部３は、ステレオ画像から局所特徴の三次元座標を算出する。サンプル設定部４は、三次元座標系へ物体の状態量を推定するために分散配置されるサンプルを設定する。サンプル遷移部５は、所定のアルゴリズムに基づいてサンプルを遷移させる。尤度算出部６は、サンプル遷移部５により遷移させたサンプルについて、三次元座標算出部３により算出された局所特徴の三次元座標から物体の存在確率を示す尤度を算出する。推定部７は、尤度算出部６により算出された尤度から物体の三次元位置を推定する。
【選択図】図１

Description

本発明は、物体位置推定装置および物体位置推定プログラムに関する。

従来より、映像監視システムやテレビ電話などにおいて、物体（人物等）を追跡する物体追跡機能により撮影領域を自動的に変更しながら物体を撮影する技術がある。このような技術においては、単一カメラによって取得した画像から物体の状態を推定する目的で、パーティクルフィルタというアルゴリズムが二次元座標系に対して広く利用されている。

上述したパーティクルフィルタについて簡単に説明する。まず、パーティクルフィルタは、例えば、背景差分から算出される尤度と状態量を持つサンプルを用いて離散的な確率密度で追跡対象を表現する。そして、パーティクルフィルタは、状態遷移モデルにしたがってサンプルを遷移させることで、動きの変動や観測のノイズに対して物体の頑健な追跡を実現する。なお、状態遷移モデルとは、物体の位置と時間との関係を予め規定したものであり、例えば、物体の移動が等速直線運動であるか、等加速度直線運動であるかといった情報を用いることで、異なる時刻間における位置関係の予測を可能にする。なお、パーティクルフィルタは、モンテカルロフィルタあるいはコンデンセーションとも称される。

特開２００８−１４０２９０号公報 特開２００９−１５６７１号公報

ところで、上述した物体追跡機能は、様々なサービスへの応用が期待されている。例えば、ロボットに個人を追跡させることで適切な情報を個人に提供するなど、種々の対人サービスを提供する試みが行われている。また、例えば、自動車に対向車を追跡させることで、安全な運転を補助する試みが行われている。このようなサービスを実現するためには、自装置から物体までの距離を推定する必要があるが、上述した従来技術は単一カメラによって物体位置を推定するため、物体の三次元位置を推定することはできない。

仮に、ステレオカメラにより撮影したステレオ画像から全画素の三次元位置情報を取得し、取得した三次元位置情報に対して上述した物体追跡機能を適用することで、物体の三次元位置を推定することも考えられる。しかしながら、全画素の三次元位置情報を取得する際の計算量が大きいという問題点があり、さらに、取得した三次元位置情報を用いた三次元処理の計算量も大きくなってしまうという問題点があった。

開示の技術は、上記に鑑みてなされたものであって、少ない計算量で物体の三次元位置を推定することが可能な物体位置推定装置および物体位置推定プログラムを提供することを目的とする。

本願の開示する技術は、一つの態様において、撮像装置により撮像されたステレオ画像から、所定の領域の中で特徴のある画素を局所特徴として抽出する局所特徴抽出部と、ステレオ画像から局所特徴の三次元座標を算出する三次元座標算出部と、三次元座標系に対して物体の状態量を推定するために分散配置されるサンプルを設定するサンプル設定部と、所定のアルゴリズムに基づいてサンプルを遷移させるサンプル遷移部と、前記サンプル遷移部により遷移させたサンプルについて、前記三次元座標算出部により算出された局所特徴の三次元座標から物体の存在確率を示す尤度を算出する尤度算出部と、前記尤度算出部により算出された尤度から物体の三次元位置を推定する推定部とを有することを特徴とする。

本願の開示する技術の一つの態様によれば、少ない計算量で物体の三次元位置を推定することができるという効果を奏する。

図１は、実施例１に係る物体位置推定装置を示す図である。 図２は、実施例２に係る物体位置推定装置の構成を示す図である。 図３は、実施例２に係る局所特徴の一例を示す図である。 図４は、実施例２に係る局所領域を説明するための図である。 図５は、実施例２に係る局所特徴の抽出方法を説明するための図である。 図６は、実施例２に係る局所特徴抽出部により抽出される局所特徴の例を示す図である。 図７は、実施例２に係る新規局所特徴リストの一例を示す図である。 図８は、実施例２に係る新規局所特徴リストの一例を示す図である。 図９は、実施例２に係る追跡局所特徴リストの一例を示す図である。 図１０は、実施例２に係る局所特徴の追跡方法を説明するための図である。 図１１は、実施例２に係る局所特徴の追跡方法を説明するための図である。 図１２は、実施例２に係る追跡局所特徴リストの一例を示す図である。 図１３は、実施例２に係る三次元運動データリストの一例を示す図である。 図１４は、実施例２に係る三次元座標データリストの一例を示す図である。 図１５は、実施例２に係る尤度の算出方法を説明するための図である。 図１６は、実施例２に係る尤度の算出方法を説明するための図である。 図１７は、実施例２に係る尤度の算出方法を説明するための図である。 図１８は、実施例２に係る物体位置推定装置における処理の流れを示す図である。 図１９は、物体位置推定プログラムを実行するコンピュータを示す図である。

以下に、図面を参照しつつ、本願の開示する物体位置推定装置および物体位置推定プログラムの一実施形態について詳細に説明する。なお、本願の開示する物体位置推定装置および物体位置推定プログラムの一実施形態として以下に説明する実施例により、本願の開示する技術が限定されるものではない。

図１は、実施例１に係る物体位置推定装置を示す図である。同図に示すように、実施例１に係る物体位置推定装置１は、局所特徴抽出部２と、三次元座標算出部３と、サンプル設定部４と、サンプル遷移部５と、尤度算出部６と、推定部７とを有する。

局所特徴抽出部２は、撮像装置により撮像されたステレオ画像から、所定の領域の中で特徴のある画素を局所特徴として抽出する。三次元座標算出部３は、ステレオ画像から局所特徴の三次元座標を算出する。サンプル設定部４は、三次元座標系に対して物体の状態量を推定するために分散配置されるサンプルを設定する。サンプル遷移部５は、所定のアルゴリズムに基づいてサンプルを遷移させる。尤度算出部６は、サンプル遷移部５により遷移させたサンプルについて、三次元座標算出部３により算出された局所特徴の三次元座標から物体の存在確率を示す尤度を算出する。推定部７は、尤度算出部６により算出された尤度から物体の三次元位置を推定する。

上述してきたように、実施例１に係る物体位置推定装置は、ステレオ画像から局所特徴を抽出し、続いて局所特徴の三次元座標を算出する。そして、物体位置推定装置は、局所特徴の三次元座標から遷移させたサンプルの尤度を算出し、さらに尤度から物体の三次元位置を推定する。このようなことから、実施例１に係る物体位置推定装置は、三次元座標系の全座標点ではなく局所特徴の座標点に限って処理を行うことで処理データ数を減少させることができるため、少ない計算量で物体の三次元位置を推定できる。

［物体位置推定装置の構成］
図２は、実施例２に係る物体位置推定装置の構成を示す図である。同図に示すように、実施例２に係る物体位置推定装置１００は、フレームバッファ１１０と、局所特徴抽出部１２０と、局所特徴ステレオ処理部１３０と、局所特徴記憶部１４０と、局所特徴追跡部１５０とを有する。さらに、実施例２に係る物体位置推定装置１００は、サンプル設定部１６０と、サンプル遷移部１７０と、尤度算出部１８０と、推定部１９０とを有する。また、実施例２に係る物体位置推定装置１００は、カメラＲ１０およびカメラＬ２０に接続されている。また、Ｒとは右側を意味し、Ｌとは左側を意味する。つまり、例えば、カメラＲはステレオカメラの右側のカメラを示し、後述する画像ＲはカメラＲにより撮像した画像を示す。

カメラＲ１０およびカメラＬ２０は、周囲の環境を撮影することで画像信号を生成し、画像信号をステレオ画像に変換してフレームバッファ１１０に逐次送る。なお、画像信号とは、例えば、ビデオ信号である。

フレームバッファ１１０は、カメラＲ１０およびカメラＬ２０から受け付けたステレオ画像を保持する。なお、ステレオ画像とは、同時に撮影された視差のある一対の画像であり、画像Ｒと画像Ｌとを含む。また、フレームバッファ１１０が保持する画像は、少なくとも時刻（ｔ）および時刻（ｔ−１）におけるステレオ画像を含む。また、時刻（ｔ）に撮影した画像をステレオ画像（ｔ）、画像Ｒ（ｔ）、画像Ｌ（ｔ）と表記する。また、時刻（ｔ−１）に撮影した画像をステレオ画像（ｔ−１）、画像Ｒ（ｔ−１）、画像Ｌ（ｔ−１）と表記する。

局所特徴抽出部１２０は、フレームバッファ１１０に保持されたステレオ画像から特徴のある画素を局所特徴として抽出する。例えば、局所特徴抽出部１２０は、フレームバッファ１１０に保持した画像Ｒ（ｔ）を読み出し、読み出した画像Ｒ（ｔ）から局所特徴を抽出する。そして、局所特徴抽出部１２０は、抽出した局所特徴を用いて、画像Ｒ（ｔ）における新規局所特徴リストを生成し、生成した新規局所特徴リストを局所特徴ステレオ処理部１３０に送る。

ここで、図３から図６を用いて、局所特徴抽出部１２０による局所特徴の抽出動作について説明する。図３は、実施例２に係る局所特徴の一例を示す図である。同図は、局所特徴抽出部１２０によって画像内から抽出される局所特徴の一例を表す。例えば、同図に示すように、局所特徴抽出部１２０によって、物体の輪郭を形成するコーナーや線分（同図破線部参照）に相当する画像Ｒ（ｔ）の画素部分が局所特徴として抽出される。

図４は、実施例２に係る局所領域を説明するための図である。同図は、局所特徴抽出部１２０が、画像内の各領域に関し、輝度勾配に基づいた固有値を求める場合に用いる局所領域（例えば、８×８画素）の一例を表す。なお、局所領域とは、特定の画素について輝度勾配に基づく固有値を算出するために特定の画素を中心として指定した画素の領域である。また、局所領域のサイズは任意に設定できる。また、図５は、実施例２に係る局所特徴の抽出方法を説明するための図である。同図は、ｘ方向（水平）およびｙ方向（垂直）の座標系に、局所特徴抽出部１２０により算出された局所領域内の各画素についての輝度勾配をプロットした状態を表す。なお、水平方向の輝度勾配をｇ_ｘ、垂直方向の輝度勾配をｇ_ｙとする。

例えば、局所特徴抽出部１２０は、フレームバッファ１１０に保持された画像Ｒ（ｔ）を読み出し、読み出した画像Ｒ（ｔ）内の局所領域ごとに輝度勾配の分散行列を生成する。つまり、局所特徴抽出部１２０は、局所領域を画像Ｒ（ｔ）の縦方向および横方向に一画素ずつ移動させることで、局所領域ごとに輝度勾配の分散行列を生成する。そして、局所特徴抽出部１２０は、図５に示すように、生成した分散行列の固有値λ_ｍａｘおよびλ_ｍｉｎを算出し、算出した固有値λ_ｍａｘおよびλ_ｍｉｎが所定の条件を満たすか否かによって、局所領域から局所特徴の抽出を行う。例えば、局所特徴抽出部１２０は、以下に示す条件（１）を満たす局所領域内の画素を局所特徴「コーナー」として抽出する。また、局所特徴抽出部１２０は、以下に示す条件（２）および（３）を同時に満たす局所領域内の画素を局所特徴「線分」として抽出する。なお、以下の条件（１）、（２）あるいは（３）に示す「ｔｈ_{ｃｏｒｎｅｒ}」および「ｔｈ_ｌｉｎｅ」は、局所特徴抽出部１２０が局所特徴の抽出に用いる任意の閾値である。

図６は、実施例２に係る局所特徴抽出部により抽出された局所特徴の例を示す図である。例えば、局所特徴抽出部１２０は、同図に示すように、人物が撮影された画像Ｒ（ｔ）から局所特徴を抽出する場合には、人物の輪郭を形成するコーナーや線分に相当する画素部分を局所特徴として抽出する。なお、図６において、×印は局所特徴を示す。また、図６において、水平方向の座標を「ｈ」、垂直方向の座標を「ｖ」と表記する。

局所特徴の抽出後、局所特徴抽出部１２０は、フレームバッファ１１０に保持された画像から抽出した局所特徴の一覧である新規局所特徴リストを生成する。そして、局所特徴抽出部１２０は、生成した新規局所特徴リストを局所特徴ステレオ処理部１３０に送る。新規局所特徴リストは、局所特徴抽出部１２０により局所特徴ごとに一意に付与される情報に対応付けて、平面座標および属性情報を保持する。

図７は、実施例２に係る新規局所特徴リストの一例を示す図である。同図に示すように、新規局所特徴リストは、局所特徴抽出部１２０により局所特徴ごとに一意に付与される番号「１」に対応付けて、平面座標（ｈ^１ _ｔ，ｖ^１ _ｔ）および属性情報「コーナー特徴」を保持する。また、例えば、同図に示すように、新規局所特徴リストは、番号「２」に対応付けて、平面座標（ｈ^２ _ｔ，ｖ^２ _ｔ）および属性情報「コーナー特徴」を保持する。また、同図に示すように、新規局所特徴リストは、番号「３」に対応付けて、平面座標（ｈ^３ _ｔ，ｖ^３ _ｔ）および属性情報「線分特徴」を保持する。また、例えば、同図に示すように、新規局所特徴リストは、番号「ｍ」に対応付けて、平面座標（ｈ^ｍ _ｔ，ｖ^ｍ _ｔ）および属性情報「線分特徴」を保持する。なお、平面座標とは、画像平面における水平座標（例えば、ｈ^１〜ｍ _ｔ）および垂直座標（例えば、ｖ^１〜ｍ _ｔ）によって示される局所特徴の位置情報である。また、属性情報とは、コーナーあるいは線分のどちらに相当する局所特徴であるかを示す情報である。

図２に戻り、局所特徴ステレオ処理部１３０は、ステレオ画像から局所特徴の三次元座標を算出する。例えば、局所特徴ステレオ処理部１３０は、フレームバッファ１１０から画像Ｒ（ｔ）および画像Ｌ（ｔ）を読み出し、読み出した画像Ｒ（ｔ）および画像Ｌ（ｔ）についてテンプレートマッチングを行うことにより、各画像から局所特徴に対応する画素を抽出する。例えば、局所特徴ステレオ処理部１３０は、画像Ｒ（ｔ）の局所特徴に対応する領域をテンプレートとして設定し、このテンプレートを少しずつ移動させつつ画像Ｌ（ｔ）の各領域にあてはめて、テンプレートと各領域との照合度をそれぞれ算出する。そして、局所特徴ステレオ処理部１３０は、算出した各照合度と所定の閾値とをそれぞれ比較して、テンプレートとの照合度が所定の閾値を超える画像Ｌ（ｔ）内の領域を局所特徴に対応する画素として抽出する。テンプレートマッチングとの照合度を評価する方法としては、例えば、差分二乗和（ＳＳＤ：Sum of Squared Differences）や差分絶対値和（ＳＡＤ：Sum of Absolute Differences）を用いることができる。

局所特徴に対応する画素の抽出後、局所特徴ステレオ処理部１３０は、画像Ｒ（ｔ）内の局所特徴に対応する画素と、画像Ｌ（ｔ）内の局所特徴に対応する画素との視差を求める。そして、局所特徴ステレオ処理部１３０は、求めた視差を用いた三角測量により局所特徴の三次元座標を算出する。三次元座標算出後、局所特徴ステレオ処理部１３０は、算出した三次元座標を、局所特徴抽出部１２０から取得した新規局所特徴リストの対応箇所に登録する。

図８は、実施例２に係る新規局所特徴リストの一例を示す図である。例えば、同図に示すように、局所特徴ステレオ処理部１３０は、番号「１」、平面座標（ｈ^１ _ｔ，ｖ^１ _ｔ）および属性情報「コーナー特徴」に対応付けて、三次元座標（ｘ^１ _ｔ，ｙ^１ _ｔ，ｚ^１ _ｔ）を新規局所特徴リストに登録する。また、例えば、同図に示すように、局所特徴ステレオ処理部１３０は、番号「２」、平面座標（ｈ^２ _ｔ，ｖ^２ _ｔ）および属性情報「コーナー特徴」に対応付けて、三次元座標（ｘ^２ _ｔ，ｙ^２ _ｔ，ｚ^２ _ｔ）を新規局所特徴リストに登録する。また、例えば、同図に示すように、局所特徴ステレオ処理部１３０は、番号「３」、平面座標（ｈ^３ _ｔ，ｖ^３ _ｔ）および属性情報「線分特徴」に対応付けて、三次元座標（ｘ^３ _ｔ，ｙ^３ _ｔ，ｚ^３ _ｔ）を新規局所特徴リストに登録する。また、例えば、同図に示すように、局所特徴ステレオ処理部１３０は、番号「ｍ」、平面座標（ｈ^ｍ _ｔ，ｖ^ｍ _ｔ）および属性情報「線分特徴」に対応付けて、三次元座標（ｘ^ｍ _ｔ，ｙ^ｍ _ｔ，ｚ^ｍ _ｔ）を新規局所特徴リストに登録する。なお、三次元座標とは、三次元空間における水平座標（ｘ_ｔ）、奥行座標（ｙ_ｔ）および垂直座標（ｚ_ｔ）によって示される局所特徴の位置情報である。

また、局所特徴ステレオ処理部１３０は、時刻（ｔ−１）において生成した新規局所特徴リストからコーナー特徴のデータを読み出し、追跡すべき局所特徴のリストとして追跡局所特徴リスト（ｔ−１）を生成する。そして、局所特徴ステレオ処理部１３０は、生成した追跡局所特徴リスト（ｔ−１）を局所特徴記憶部１４０に格納する。追跡局所特徴リストは、局所特徴抽出部１２０により局所特徴ごとに一意に付与される情報に対応付けて、平面座標、属性情報および三次元座標を保持する。なお、ここで（ｔ−１）と表記した理由は、時刻（ｔ−１）において抽出されたコーナー特徴が後述する局所特徴追跡部１５０における処理対象となるためである。

図９は、実施例２に係る追跡局所特徴リストの一例を示す図である。同図に示すように、追跡局所特徴リスト（ｔ−１）は、番号「１」に対応付けて、平面座標（ｈ^１ _ｔ-１，ｖ^１ _ｔ-１）、属性情報「コーナー特徴」および三次元座標（ｘ^１ _ｔ-１，ｙ^１ _ｔ-１，ｚ^１ _ｔ-１）を保持する。また、例えば、同図に示すように、追跡局所特徴リスト（ｔ−１）は、番号「２」に対応付けて、平面座標（ｈ^２ _ｔ-１，ｖ^２ _ｔ-１）、属性情報「コーナー特徴」および三次元座標（ｘ^２ _ｔ-１，ｙ^２ _ｔ-１，ｚ^２ _ｔ-１）を保持する。また、例えば、同図に示すように、追跡局所特徴リスト（ｔ−１）は、番号「３」に対応付けて、平面座標（ｈ^３ _ｔ-１，ｖ^３ _ｔ-１）、属性情報「コーナー特徴」および三次元座標（ｘ^３ _ｔ-１，ｙ^３ _ｔ-１，ｚ^３ _ｔ-１）を保持する。また、例えば、同図に示すように、追跡局所特徴リスト（ｔ−１）は、番号「ｍ」に対応付けて、平面座標（ｈ^ｍ _ｔ-１，ｖ^ｍ _ｔ-１）、属性情報「コーナー特徴」および三次元座標（ｘ^ｍ _ｔ-１，ｙ^ｍ _ｔ-１，ｚ^ｍ _ｔ-１）を保持する。

局所特徴記憶部１４０は、局所特徴ステレオ処理部１３０により格納された追跡局所特徴リスト（ｔ−１）を記憶する（図９参照）。

局所特徴追跡部１５０は、時間の経過による局所特徴の動きを追跡する。例えば、局所特徴追跡部１５０は、局所特徴記憶部１４０から追跡局所特徴リスト（ｔ−１）を読み出すとともに、フレームバッファ１１０から画像Ｒ（ｔ−１）および画像Ｒ（ｔ）を読み出す。そして、局所特徴追跡部１５０は、画像Ｒ（ｔ−１）から抽出したコーナー特徴に対応する画素を画像Ｒ（ｔ）内から探索することで、コーナー特徴の動きを追跡する。

図１０および図１１は、実施例２に係る局所特徴の追跡方法を説明するための図である。ここで、図１０および図１１を用いて、局所特徴追跡部１５０によるコーナー特徴の追跡方法について説明する。図１０は、例えば、カメラＲ１０により時刻（ｔ−１）に撮影された人物の画像Ｒ（ｔ−１）を例示する。また、図１１は、例えば、カメラＲ１０により時刻（ｔ）に撮影された人物の画像Ｒ（ｔ）を例示する。図１０および図１１は、同一被写体のある人物を時刻（ｔ−１）、時刻（ｔ）にて撮影した連続画像であり、図１０には、水平右方向に移動する前の人物が撮影され、図１１には、水平右方向に移動した後の人物が撮影されているものとする。

例えば、局所特徴追跡部１５０は、画像Ｒ（ｔ−１）と画像Ｒ（ｔ）のテンプレートマッチングを行うことにより、画像Ｒ（ｔ−１）内のコーナー特徴に対応する画素を画像Ｒ（ｔ）から抽出する。例えば、局所特徴追跡部１５０は、画像Ｒ（ｔ−１）のコーナー特徴に対応する領域をテンプレートとして設定し、このテンプレートを少しずつ移動させつつ画像Ｒ（ｔ）の各領域にあてはめて、テンプレートと各領域との照合度をそれぞれ算出する。そして、局所特徴追跡部１５０は、算出した各照合度と所定の閾値とをそれぞれ比較して、テンプレートとの照合度が所定の閾値を超える画像Ｒ（ｔ）内の領域をコーナー特徴に対応する画素として抽出する。そして、局所特徴追跡部１５０は、画像Ｒ（ｔ）から抽出した画素の水平座標（ｈ_ｔ）と垂直座標（ｖ_ｔ）を算出する。座標算出後、局所特徴追跡部１５０は、算出した水平座標（ｈ_ｔ）および垂直座標（ｖ_ｔ）を用いて、追跡局所特徴リスト（ｔ−１）内の平面座標を更新することにより追跡局所特徴リスト（ｔ）を生成する。そして、局所特徴追跡部１５０は、生成した追跡局所特徴リスト（ｔ）を局所特徴ステレオ処理部１３０に送る。なお、テンプレートとの照合度が所定の閾値を超える領域が存在しない場合には、追跡処理に失敗したものと判定して、該当するコーナー特徴を追跡局所特徴リストから削除する。

図１２は、実施例２に係る追跡局所特徴リストの一例を示す図である。なお、同図は、局所特徴追跡部１５０により更新生成される追跡局所特徴リスト（ｔ）の一例を表す。同図に示すように、追跡局所特徴リスト（ｔ）は、局所特徴ごとに付与された番号「１」に対応付けて、平面座標（ｈ^１ _ｔ，ｖ^１ _ｔ）、属性情報「コーナー特徴」および三次元座標（ｘ^１ _ｔ−１，ｙ^１ _ｔ−１，ｚ^１ _ｔ−１）を保持する。また、例えば、同図に示すように、追跡局所特徴リスト（ｔ）は、番号「２」に対応付けて、平面座標（ｈ^２ _ｔ，ｖ^２ _ｔ）、属性情報「コーナー特徴」および三次元座標（ｘ^２ _ｔ−１，ｙ^２ _ｔ−１，ｚ^２ _ｔ−１）を保持する。また、同図に示すように、追跡局所特徴リスト（ｔ）は、番号「３」に対応付けて、平面座標（ｈ^３ _ｔ，ｖ^３ _ｔ）、属性情報「コーナー特徴」および三次元座標（ｘ^３ _ｔ−１，ｙ^３ _ｔ−１，ｚ^３ _ｔ−１）を保持する。また、例えば、同図に示すように、追跡局所特徴リスト（ｔ）は、番号「ｍ」に対応付けて、平面座標（ｈ^ｍ _ｔ，ｖ^ｍ _ｔ）、属性情報「コーナー特徴」および三次元座標（ｘ^ｍ _ｔ−１，ｙ^ｍ _ｔ−１，ｚ^ｍ _ｔ−１）を保持する。

局所特徴追跡部１５０による追跡局所特徴リスト（ｔ）の生成後、局所特徴ステレオ処理部１３０は、ステレオ画像から局所特徴の三次元座標を算出する。例えば、局所特徴ステレオ処理部１３０は、局所特徴追跡部１５０における追跡に成功したコーナー特徴について、ステレオ画像（ｔ）における三次元座標（ｘ^ｍ _ｔ，ｙ^ｍ _ｔ，ｚ^ｍ _ｔ）を算出する。そして、局所特徴ステレオ処理部１３０は、算出した三次元座標（ｘ^ｍ _ｔ，ｙ^ｍ _ｔ，ｚ^ｍ _ｔ）と追跡局所特徴リスト（ｔ）の三次元座標（ｘ^ｍ _ｔ−１，ｙ^ｍ _ｔ−１，ｚ^ｍ _ｔ−１）との差分を算出し、算出結果を三次元運動データ（ｄｘ^ｍ _ｔ，ｄｙ^ｍ _ｔ，ｄｚ^ｍ _ｔ）として記憶する。なお、三次元運動データ（ｄｘ^ｍ _ｔ，ｄｙ^ｍ _ｔ，ｄｚ^ｍ _ｔ）とは、時刻（ｔ−１）から時刻（ｔ）にかけて局所特徴が移動した三次元座標系における変化量を示す。そして、局所特徴ステレオ処理部１３０は、記憶した三次元運動データを集計し、集計結果を三次元運動データリストとしてサンプル遷移部１７０に送る。

図１３は、実施例２に係る三次元運動データリストの一例を示す図である。同図に示すように、局所特徴ステレオ処理部１３０により生成された三次元運動データリストは、局所特徴ごとに付与された番号「１」に対応付けて、三次元運動データ（ｄｘ^１ _ｔ，ｄｙ^１ _ｔ，ｄｚ^１ _ｔ）を保持する。また、例えば、同図に示すように、三次元運動データリストは、番号「２」に対応付けて、三次元運動データ（ｄｘ^２ _ｔ，ｄｙ^２ _ｔ，ｄｚ^２ _ｔ）を保持する。また、同図に示すように、三次元運動データリストは、番号「３」に対応付けて、三次元運動データ（ｄｘ^３ _ｔ，ｄｙ^３ _ｔ，ｄｚ^３ _ｔ）を保持する。また、例えば、同図に示すように、三次元運動データリストは、番号「ｍ」に対応付けて、三次元運動データ（ｄｘ^ｍ _ｔ，ｄｙ^ｍ _ｔ，ｄｚ^ｍ _ｔ）を保持する。

また、局所特徴ステレオ処理部１３０は、ステレオ画像（ｔ）において抽出した全ての局所特徴の三次元座標を集計し、集計した三次元座標から三次元座標データリストを生成する。そして、局所特徴ステレオ処理部１３０は、生成した三次元座標データリストを尤度計算部１８０に送る。

図１４は、実施例２に係る三次元座標データリストの一例を示す図である。同図に示すように、局所特徴ステレオ処理部１３０により生成された三次元座標データリストは、局所特徴ごとに付与された番号「１」に対応付けて、属性情報「コーナー特徴」および三次元座標（ｘ^１ _ｔ、ｙ^１ _ｔ、ｚ^１ _ｔ）を保持する。また、例えば、同図に示すように、三次元座標データリストは、番号「２」に対応付けて、属性情報「コーナー特徴」および三次元座標（ｘ^２ _ｔ、ｙ^２ _ｔ、ｚ^２ _ｔ）を保持する。また、例えば、同図に示すように、三次元座標データリストは、番号「３」に対応付けて、属性情報「線分特徴」および三次元座標（ｘ^３ _ｔ、ｙ^３ _ｔ、ｚ^３ _ｔ）を保持する。また、例えば、同図に示すように、三次元座標データリストは、番号「ｍ」に対応付けて、属性情報「線分特徴」および三次元座標（ｘ^ｍ _ｔ、ｙ^ｍ _ｔ、ｚ^ｍ _ｔ）を保持する。

サンプル設定部１６０は、三次元座標系に対してサンプルを設定する。なお、サンプルとは、物体の状態量を推定するために座標上に分散配置される仮説である。例えば、サンプル設定部１６０は、パーティクルフィルタのアルゴリズムを用いることで、後述する尤度算出部１８０により時刻（ｔ−１）時点で算出されたサンプルの尤度の大きさに対応する数のサンプルを配置する。そして、サンプル設定部１６０は、配置した各サンプルに対してサンプル番号ｉ（例えば、「０」〜「Ｎ−１」）を付与する。

サンプル遷移部１７０は、サンプル設定部１６０により設定されたサンプルを遷移させる。例えば、サンプル遷移部１７０は、局所特徴ステレオ処理部１３０から受け付けた三次元運動データリストを利用して三次元運動状態遷移量を決定し、サンプル設定部１６０により設定された各サンプルを遷移させる。なお、三次元運動状態遷移量とは、各サンプルの近傍にある局所特徴の三次元運動データの平均値を利用する。また、時刻（ｔ）における状態推定量Ｓ_{ｔ｜ｔ−１} ^（ｉ）は、時刻（ｔ−１）における状態推定量をＳ_{ｔ−１｜ｔ−１} ^（ｉ）、三次元運動状態遷移量をＶ_ｔ、ノイズ成分をＷ_ｔ ^（ｉ）とすると、以下に示す式（４）で表される。

尤度算出部１８０は、サンプル遷移部１７０により遷移されたサンプルの尤度を算出する。なお、尤度とは、サンプル遷移位置に物体が存在する事後確率である。例えば、尤度算出部１８０は、局所特徴ステレオ処理部１３０から受け付けた局所特徴の三次元座標データリストから、サンプル遷移部１７０により遷移された各サンプルの尤度を算出する。そして、尤度算出部１８０は、算出された各サンプルの尤度をサンプル設定部１６０および推定部１９０に送る。

ここで、図１５から図１７を用いて、尤度算出部１８０による尤度の算出方法について説明する。図１５および図１６は、実施例２に係る尤度の算出方法を説明するための図である。図１７は、実施例２に係る尤度の算出方法を説明するための図である。

例えば、尤度算出部１８０は、図１５および図１６に示すように、三次元座標系にサンプルと局所特徴を配置し、さらに、特定のサンプルを中心点として中心領域Ａおよび周辺領域Ｂを設定する。なお、図１５は、中心領域Ａを設定した三次元座標系の正面図を表し、図１６は、周辺領域Ｂを設定した三次元座標系の上面図を表す。また、中心領域Ａとは、中心点の座標を（ｘｃ、ｙｃ、ｚｃ）、任意の半径をｒ１、任意の高さをＨとした場合に、以下に示す式（５）および式（７）を満たす円柱形の領域（ｘ、ｙ、ｚ）である。また、周辺領域Ｂとは、中心点の座標を（ｘｃ、ｙｃ、ｚｃ）、任意の半径をｒ２、任意の高さをＨとした場合に、以下に示す式（６）および式（７）を満たす円筒形の領域（ｘ、ｙ、ｚ）である。

そして、尤度算出部１８０は、中心領域Ａに含まれる局所特徴の数を計測し、計測した局所特徴の数を領域の体積で除算することで計測点密度を算出する。また、尤度算出部１８０は、中心領域Ａと同様に、周辺領域Ｂについても計測点密度を算出する。そして、尤度算出部１８０は、中心領域Ａの計測点密度から周辺領域Ｂの計測点密度を減算することで計測点密度差を算出する。尤度算出部１８０が算出する計測点密度差は、周辺領域Ｂに対して中心領域Ａにどれほどの局所特徴が存在するかを示す情報である。

続いて、尤度算出部１８０は、図１７に示すように、三次元座標系における平面座標上（ｘ、ｙ）において物体が存在する事後確率を算出する。図１７には、三次元座標系における平面座標上（ｘ、ｙ）でのサンプル位置を横軸として、各サンプルの計測点密度差を縦軸として示す。そして、尤度算出部１８０は、例えば、サンプルの計測点密度差から所定のアルゴリズムによって尤度を決定する。なお、所定のアルゴリズムとは、例えば、サンプルの計測点密度差を入力すると計測点密度差の値に対応する特定の値を尤度として出力するプログラムである。

推定部１９０は、尤度算出部１８０により算出された尤度から物体の三次元位置を推定する。例えば、推定部１９０は、尤度算出部１８０から受け付けた各サンプルの尤度からパーティクルフィルタのアルゴリズムを用いて重みを算出する。なお、重みπ_ｔ ^（ｉ）は、尤度をＰ３ｄとすると以下に示す式（８）で表される。また、例えば、推定部１９０は、各サンプルの重みからパーティクルフィルタのアルゴリズムを用いて重みつき平均を算出することで物体の三次元位置を推定する。なお、推定量ε（ｘ_ｔ）は、重みをπ_ｔ ^（ｉ）、状態推定量をＳ_{ｔ｜ｔ−１} ^（ｉ）とすると以下に示す式（９）で表される。

［物体位置推定装置による処理］
図１８は、実施例２に係る物体位置推定装置における処理の流れを示す図である。同図に示すように、フレームバッファ１１０にステレオ画像（ｔ）が受け付けられると（ステップＳ１０１肯定）、局所特徴抽出部１２０は、ステレオ画像（ｔ）から局所特徴を抽出する（ステップＳ１０２）。そして、局所特徴抽出部１２０は、抽出した局所特徴のリストとして新規局所特徴リストを生成し、生成した新規局所特徴リストを局所特徴ステレオ処理部１３０に送る。

局所特徴追跡部１５０は、局所特徴抽出部１２０により抽出されたコーナー特徴の動きを追跡する（ステップＳ１０３）。そして、局所特徴追跡部１５０は、追跡した局所特徴のリストとして追跡局所特徴リスト（ｔ）を生成し、生成した追跡局所特徴リスト（ｔ）を局所特徴ステレオ処理部１３０に送る。

局所特徴ステレオ処理部１３０は、フレームバッファ１１０に保持されたステレオ画像（ｔ）を用いてステレオ計測を行う（ステップＳ１０４）。すなわち、局所特徴ステレオ処理部１３０は、ステレオ計測により、フレームバッファ１１０に保持されたステレオ画像（ｔ）から局所特徴の三次元座標を算出する。さらに、局所特徴ステレオ処理部１３０は、追跡に成功したコーナー特徴の三次元座標と追跡局所特徴リスト（ｔ）の三次元座標との差分から三次元運動データを算出する。そして、局所特徴ステレオ処理部１３０は、ステレオ計測により算出した三次元座標から三次元座標データリストを生成し、生成した三次元座標データリストを尤度計算部１８０に送る。さらに、局所特徴ステレオ処理部１３０は、ステレオ計測により算出した三次元運動データのリストとして三次元運動データリストを生成し、生成した三次元運動データリストをサンプル遷移部１７０に送る。

サンプル設定部１６０は、パーティクルフィルタのアルゴリズムを用いて、尤度算出部１８０により時刻（ｔ−１）時点で算出されたサンプルの尤度の大きさに対応する数のサンプルを配置する（ステップＳ１０５）。そして、サンプル設定部１６０は、配置した各サンプルに対してサンプル番号ｉ（例えば、「０」〜「Ｎ−１」）を付与する。サンプル設定部１６０は、パーティクルフィルタの処理をサンプル番号が小さい順に処理を行う目的で、処理番号「ｉ＝０」を設定する（ステップＳ１０６）。

サンプル遷移部１７０は、局所特徴ステレオ処理部１３０から受け付けた三次元運動データリストを利用して、三次元運動状態遷移量、すなわち、サンプル遷移モデルを決定する（ステップＳ１０７）。そして、サンプル遷移部１７０は、サンプル設定部１６０において設定したサンプル番号「ｉ＝０」のサンプルを遷移させる（ステップＳ１０８）。

尤度算出部１８０は、局所特徴ステレオ処理部１３０から受け付けた三次元座標データリストを利用してサンプル番号「ｉ」の尤度を算出する（ステップＳ１０９）。そして、尤度算出部１８０は、算出したサンプル番号「ｉ＝０」の尤度をサンプル設定部１６０および推定部１９０に送る。

続いて、尤度算出部１８０は、サンプル番号「ｉ＝０」のサンプルの次のサンプルについてパーティクルフィルタの処理を行う。すなわち、尤度算出部１８０は、パーティクルフィルタの次の処理対象サンプルとしてサンプル番号「ｉ＝ｉ＋１」を設定した後（ステップＳ１１０）、設定したサンプル番号「ｉ＝ｉ＋１」が条件「サンプル番号ｉ＜Ｎ」を満たすか否かを判定する（ステップＳ１１１）。

尤度算出部１８０は、判定の結果、設定したサンプル番号「ｉ＝ｉ＋１」が条件「サンプル番号ｉ＜Ｎ」を満たす場合には（ステップＳ１１１肯定）、ステップＳ１０７に戻り、上述したステップＳ１０７〜ステップＳ１１０までの処理を行う。

一方、尤度算出部１８０による判定の結果、設定したサンプル番号「ｉ＝ｉ＋１」が条件「サンプル番号ｉ＜Ｎ」を満たさない場合には（ステップＳ１１１否定）、推定部１９０は次の処理を実行する。すなわち、推定部１９０は、算出したサンプル番号ｉ（例えば、「０」〜「Ｎ−１」）の全てのサンプルの尤度からパーティクルフィルタのアルゴリズムを用いて物体の三次元位置を推定する（ステップＳ１１２）。ステップＳ１１２の処理を完了すると、物体位置推定装置１００は、上述したステップＳ１０１に戻り、次のステレオ画像を取得次第、上述したステップＳ１０１〜ステップＳ１１２の処理を実行する。

なお、実施例２に係る物体位置推定装置１００における処理では、ステップＳ１０２の処理を行った後にステップＳ１０３の処理を行ったが、必ずしもこの順序で行われることを要しない。つまり、実施例２に係る物体位置推定装置１００における処理においては、ステップＳ１０３の処理を行った後にステップＳ１０２の処理行っても良い。

［実施例２による効果］
上述してきたように、実施例２によれば、物体位置推定装置１００は、ステレオ画像（ｔ）から抽出した局所特徴について三次元座標データリストを生成する。そして、物体位置推定装置１００は、生成した三次元座標データリストを利用して尤度を算出し、算出した尤度から物体の三次元位置を推定する。したがって、実施例２に係る物体位置推定装置は、局所特徴についてのみ三次元処理を行うことで三次元処理を行うデータ量を減少させることができる。また、物体位置推定装置１００は、連続画像から局所特徴の動きを追跡し、追跡結果から三次元運動データリストを生成する。そして、物体位置推定装置１００は、生成した三次元運動データリストを利用して三次元運動状態遷移量を決定する。したがって、物体位置推定装置１００は、決定した三次元運動状態遷移量を利用することでサンプルの遷移範囲を限定できるので、離散させるサンプルの総数を減少させることができる。このようなことから、実施例２に係る物体位置推定装置１００は、離散させるサンプルの総数を減少させることができるので、少ない計算量で物体の三次元位置を推定できる。

なお、上述した物体位置推定装置１００は、例えば、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field Programmable Gate Array）などの集積回路、パーソナルコンピュータやワークステーションなどのコンピュータシステムである。例えば、物体位置推定装置１００は、ロボットに搭載することで個人に対して適切な情報を提供するなどの対人サービスが可能となる。また、物体位置推定装置１００は、例えば、自動車に搭載することで安全な運転の支援が可能となる。

また、フレームバッファ１１０および局所特徴記憶部１４０は、例えば、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、フラッシュメモリ（Flash Memory）などの半導体メモリ素子、ハードディスクや光ディスクなどの記憶装置である。また、局所特徴抽出部１２０、局所特徴ステレオ処理部１３０および局所特徴追跡部１５０は、ＣＰＵ（Central Processing Unit）やＭＰＵ（Micro Processing Unit）などの電子回路である。また、サンプル設定部１６０、サンプル遷移部１７０、尤度算出部１８０および推定部１９０は、ＣＰＵやＭＰＵなどの電子回路である。

以下、本願の開示する物体位置推定装置および物体位置推定プログラムの他の実施形態を説明する。

（１）ステレオ画像による処理
上述した実施例２において、「画像Ｒ」と表記した部分は、「画像Ｌ」と置き換えてもよい。例えば、局所特徴抽出部１２０は、フレームバッファ１１０に保持した画像Ｌ（ｔ）を読み出して画像Ｌ（ｔ）から局所特徴を抽出するようにしてもよい。つまり、これらの表記は「画像Ｒ」や「画像Ｌ」に限定する趣旨ではなく、ステレオ画像のうちの一方の画像を必要とする趣旨である。

（２）局所特徴抽出部における抽出処理
上述した実施例２において、局所特徴抽出部１２０は、図５に示すように、分散行列から算出した固有値λ_ｍａｘおよびλ_ｍｉｎが所定の条件（閾値）を満たすか否かによって局所特徴を抽出したが、さらに局所特徴を選別するための条件を加えてもよい。例えば、局所特徴抽出部１２０は、所定の領域内（例えば、１６×１６画素）に含まれる画素の中で固有値と閾値との乖離度が最も高い画素を局所特徴として抽出する。ある画像上の特徴（物体の輪郭を示すコーナーなど）から複数の局所特徴が抽出されることを防止する趣旨である。

（３）局所特徴追跡部における追跡処理
上述した実施例２において、局所特徴追跡部１５０は、時間の経過によるコーナー特徴の動きを追跡したが、局所特徴追跡部１５０が追跡する局所特徴はコーナー特徴に限らない。例えば、局所特徴追跡部１５０は、線分特徴を追跡することも可能である。

また、局所特徴追跡部１５０は、画像Ｒ（ｔ−１）と画像Ｒ（ｔ）とを含む連続画像を用いて局所特徴の追跡処理を行ったが、局所特徴追跡部１５０の処理対象は連続画像に限らない。例えば、局所特徴追跡部１５０は、時刻（ｔ−２）において撮像された画像Ｒ（ｔ−２）および画像Ｒ（ｔ）を用いて局所特徴の追跡処理を行うことができる。すなわち、局所特徴追跡部１５０は、異なる時刻に撮像された２枚の画像があれば追跡処理を行えるという趣旨である。

（４）物体の三次元位置推定処理
上述した実施例２において、サンプル遷移部１７０は、三次元運動データの平均値を算出することで三次元運動状態遷移量を決定したが、三次元運動状態遷移量の決定方法はこの方法に限定しない。例えば、サンプル遷移部１７０は、三次元運動データのヒストグラムが最大となる運動量を算出することで三次元運動状態遷移量を決定してもよい。あるいは、サンプル遷移部１７０は、三次元運動データをランダムに選択することで三次元運動状態遷移量を決定してもよい。すなわち、三次元運動データリストを利用して三次元運動状態遷移量を決定する趣旨である。

また、サンプル遷移部１７０は、従来のパーティクルフィルタのアルゴリズムを用いて、サンプル設定部１６０において設定したサンプルを遷移させてもよい。例えば、サンプル遷移部１７０は、等速直線運動モデルや等加速度直線運動モデルを用いてサンプルを遷移させることができる。

また、尤度算出部１８０は、局所特徴ステレオ処理部１３０から受け付けた局所特徴の三次元座標データリストを用いることで各サンプルの尤度を算出したが、必ずしも全ての局所特徴の三次元座標データリストを用いることを要しない。例えば、尤度算出部１８０は、コーナー特徴あるいは線分特徴のいずれか一方の三次元座標データリストを用いることでサンプルの尤度を算出することができる。

また、尤度算出部１８０は、局所特徴ステレオ処理部１３０から受け付けた三次元座標データリストを用いることで各サンプルの尤度を算出したが、必ずしも三次元座標データリストを用いることを要しない。つまり、尤度算出部１８０は、従来のパーティクルフィルタのアルゴリズムを用いることでサンプルの尤度を算出することができる。例えば、尤度算出部１８０は、画像Ｒ（ｔ−１）と画像Ｒ（ｔ）の背景差分を用いることで物体の存在を推定し、サンプルの尤度を算出することができる。

また、尤度算出部１８０は、円柱形の中心領域Ａを設定することで各サンプルの尤度を算出したが、中心領域Ａの形状は円柱形に限定しない。例えば、尤度算出部１８０は、自動車の三次元位置を推定する場合には、直方体の中心領域Ａを設定することでサンプルの尤度を算出することができる。すなわち、中心領域の形状は追跡対象の形状に合わせて適宜変更することができるという趣旨である。また、中心領域Ａの形状と同様に、周辺領域Ｂの形状も変更することができる。

（５）装置構成等
例えば、図２に示した物体位置推定装置１００の各構成要素は機能概念的なものであり、必ずしも物理的に図示の如く構成されていることを要しない。すなわち、物体位置推定装置１００の分散・統合の具体的形態は図示のものに限られず、例えば、局所特徴記憶部１４０、局所特徴追跡部１５０の各機能を集約して統合する。このように、物体位置推定装置１００の全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。

（６）物体位置推定プログラム
また、上記の実施例で説明した物体位置推定装置１００の各種の処理（例えば、図１９参照）は、あらかじめ用意されたプログラムをパーソナルコンピュータやワークステーションなどのコンピュータシステムで実行することによって実現することもできる。そこで、以下では、図１９を用いて、上記の実施例で説明した物体位置推定装置と同様の機能を有する物体位置推定プログラムを実行するコンピュータの一例を説明する。図１９は、物体位置推定プログラムを実行するコンピュータを示す図である。

同図に示すように、物体位置推定装置としてコンピュータ２００は、通信制御部２１０、ＨＤＤ２２０、ＲＡＭ２３０およびＣＰＵ２４０をバス３００で接続して構成される。

ここで、通信制御部２１０は、各種情報のやり取りに関する通信を制御する。ＨＤＤ２２０は、ＣＰＵ２４０による各種処理の実行に必要な情報を記憶する。ＲＡＭ２３０は、各種情報を一時的に記憶する。ＣＰＵ２４０は、各種演算処理を実行する。

そして、ＨＤＤ２２０には、図１９に示すように、上記の実施例で説明した物体位置推定装置１００の各処理部と同様の機能を発揮する物体位置推定プログラム２２１と、物体位置推定用データ２２２とがあらかじめ記憶されている。なお、この物体位置推定プログラム２２１を適宜分散させて、ネットワークを介して通信可能に接続された他のコンピュータの記憶部に記憶させておくこともできる。

そして、ＣＰＵ２４０が、この物体位置推定プログラム２２１をＨＤＤ２２０から読み出してＲＡＭ２３０に展開することにより、図１３に示すように、物体位置推定プログラム２２１は物体位置推定プロセス２３１として機能するようになる。

すなわち、物体位置推定プロセス２３１は、物体位置推定用データ２２２等をＨＤＤ２２０から読み出して、ＲＡＭ２３０において自身に割り当てられた領域に展開し、この展開したデータ等に基づいて各種処理を実行する。

なお、物体位置推定プロセス２３１は、例えば、図２に示した物体位置推定装置１００の局所特徴抽出部１２０、局所特徴ステレオ処理部１３０、局所特徴追跡部１５０、サンプル遷移部１７０、尤度算出部１８０および推定部１９０において実行される処理に対応する。

なお、上記した物体位置推定プログラム２２１については、必ずしも最初からＨＤＤ２２０に記憶させておく必要はない。

例えば、コンピュータ２００に挿入されるフレキシブルディスク（ＦＤ）、ＣＤ−ＲＯＭ、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」に各プログラムを記憶させておく。そして、コンピュータ２００がこれらから各プログラムを読み出して実行するようにしてもよい。

さらには、公衆回線、インターネット、ＬＡＮ、ＷＡＮなどを介してコンピュータ２００に接続される「他のコンピュータ（またはサーバ）」などに各プログラムを記憶させておく。そして、コンピュータ２００がこれらから各プログラムを読み出して実行するようにしてもよい。

１ 物体位置推定装置
２ 局所特徴抽出部
３ 三次元座標算出部
４ サンプル設定部
５ サンプル遷移部
６ 尤度算出部
７ 推定部
１０ カメラＲ
２０ カメラＬ
１００ 物体位置推定装置
１１０ フレームバッファ
１２０ 局所特徴抽出部
１３０ 局所特徴ステレオ処理部
１４０ 局所特徴記憶部
１５０ 局所特徴追跡部
１６０ サンプル設定部
１７０ サンプル遷移部
１８０ 尤度算出部
１９０ 推定部
２００ コンピュータ
２１０ 通信制御部
２２０ ＨＤＤ
２２１ 物体位置推定プログラム
２２２ 物体位置推定用データ
２３０ ＲＡＭ
２３１ 物体位置推定プロセス
２４０ ＣＰＵ
３００ バス

Claims (3)

1. 撮像装置により撮像されたステレオ画像から、所定の領域の中で特徴のある画素を局所特徴として抽出する局所特徴抽出部と、
   前記ステレオ画像から局所特徴の三次元座標を算出する三次元座標算出部と、
   三次元座標系に対して物体の状態量を推定するために分散配置されるサンプルを設定するサンプル設定部と、
   所定のアルゴリズムに基づいてサンプルを遷移させるサンプル遷移部と、
   前記サンプル遷移部により遷移させたサンプルについて、前記三次元座標算出部により算出された局所特徴の三次元座標から物体の存在確率を示す尤度を算出する尤度算出部と、
   前記尤度算出部により算出された尤度から物体の三次元位置を推定する推定部と
   を有することを特徴とする物体位置推定装置。
2. 前記物体位置推定装置は、
   前記三次元座標算出部により算出された三次元座標と算出済みの三次元座標から、前記三次元座標系における局所特徴の状態変化量を三次元運動データとして算出する三次元運動データ算出部を有し、
   前記サンプル遷移部は、さらに、前記三次元運動データ算出部により算出された三次元運動データを用いてサンプルを遷移させることを特徴とする請求項１に記載の物体位置推定装置。
3. コンピュータを、物体位置推定装置として機能させ、
   前記コンピュータに、
   撮像装置により撮像されたステレオ画像から、所定の領域の中で特徴のある画素を局所特徴として抽出する局所特徴抽出手順と、
   前記ステレオ画像から局所特徴の三次元座標を算出する三次元座標算出手順と、
   三次元座標系に対して物体の状態量を推定するために分散配置されるサンプルを設定するサンプル設定手順と、
   所定のアルゴリズムに基づいてサンプルを遷移させるサンプル遷移手順と、
   前記サンプル遷移手順により遷移させたサンプルについて、前記三次元座標算出部により算出された局所特徴の三次元座標から物体の存在確率を示す尤度を算出する尤度算出手順と、
   前記尤度算出手順により算出された尤度から物体の三次元位置を推定する推定手順と
   を実行させることを特徴とする物体位置推定プログラム。

Images