JP4982410B2 - 空間移動量算出装置及びその方法 - Google Patents

Description

本発明は、空間中を運動している物体を例えばＴＶカメラ等で撮影して、その物体の３次元運動を推定する空間移動量算出装置及びその方法に関するものである。

従来、ステレオカメラなどを用いて空間中の移動体の追跡をするときに、移動物体上の特徴点の画像位置とその空間位置とその特徴量（例えば、濃度値）の関係を全て考慮して追跡していなかった（例えば、特許文献１、非特許文献１参照）。なお、「画像位置」とは、画像座標上での位置を意味し、「空間位置」とは、空間座標上の位置または視差を意味し、「視差」とは、空間中の同一点が２枚の画像に投影されたとき、それらの点の画像位置のずれ量をいう。

なぜならば、空間移動量と画像変化量は、それぞれ独立に決められるものではないため、両方を考慮したものは実現されていなかった。なお、「画像変化量」とは、移動体が空間中を移動することによる画像上で観測される移動体の位置ずれ及び大きさの変化を意味する。

ところが、画像位置や特徴量は画像から直接観測されるため、ノイズが比較的少ないが、空間位置は例えば複数のＴＶカメラ等から三角測量の原理に基づいて推定するため、ノイズが多く含まれている可能性があり、そのような場合には従来の方法では安定して空間移動量を算出することが困難であった。
特開平１０‐２７２４９号公報 Ｔｙａｇｉほか３名：「Ｆｕｓｉｏｎ ｏｆ Ｍｕｌｔｉｐｌｅ Ｃａｍｅｒａ Ｖｉｅｗｓ ｆｏｒ Ｋｅｒｎｅｌ−Ｂａｓｅｄ ３Ｄ Ｔｒａｃｋｉｎｇ」、ＩＥＥＥ Ｗｏｒｋｓｈｏｐ ｏｎ Ｍｏｔｉｏｎ ｏｎ Ｖｉｄｅｏ Ｃｏｍｐｕｔｉｎｇ、２００７

上述したように、従来技術には移動体の空間移動量を推定するときに、移動物体上の特徴点の画像位置とその空間位置と特徴量を全て考慮して推定するものがないという問題点があった。また、空間位置にノイズが入ったりした場合には、安定に空間移動量を推定することが困難であるという問題点があった。

そこで本発明は、上記従来技術の問題点を解決するためになされたものであって、空間中を運動している物体の特徴点の画像位置とその空間位置とその特徴量を全て考慮した空間移動量算出装置及びその方法を提供することを目的とする。

本発明は、対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力部と、前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力部と、前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出部と、前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出部と、前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出部と、前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合部と、前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出部と、を有する空間移動量算出装置である。

本発明によれば、物体の特徴点の画像位置とその空間位置と特徴量を全て考慮することにより、空間位置成分にノイズが多く含まれる場合にも安定かつ高速に空間移動量を算出できる。

以下、本発明の実施形態に係わる空間移動量算出装置について図面に基づいて説明する。

（第１の実施形態）
本発明の第１の実施形態を係わる空間移動量算出装置について、図１〜図５に基づいて説明する。

空間移動量算出装置は、空間上を移動する物体（例えば、人間、車等）１０１の移動前の重心と、移動後の重心の空間移動量を算出する。

（１）空間移動量算出装置の構成
図１は、空間移動量算出装置を示すブロック図である。

空間移動量算出装置は、物体１０１の移動前の追跡対象領域内の複数の特徴点（以下、「特徴点群１」という）の画像位置と空間位置と特徴量を入力する入力部１０と、物体１０１の移動後の追跡対象を含む領域内の複数の特徴点（以下、「特徴点群２」という）の画像位置と空間位置と特徴量を入力する入力部１１と、入力された特徴点群１と特徴点群２の間の画像位置関係から第１の類似度を算出する画像位置関係算出部１２と、空間位置関係から第２の類似度を算出する空間位置関係算出部１３と、特徴量関係から第３の類似度を算出する特徴量関係算出部１４と、第１の類似度〜第３の類似度を統合する統合部１５と、統合部１５から空間移動量を推定する空間移動量算出部１６と、空間移動量を出力する出力部１７とを備えている。

なお、各部１０〜１７の機能は、コンピュータに格納されたプログラムによっても実現可能である。

（２）空間移動量算出装置の動作
次に、図１〜図３を用いて、空間移動量算出装置の動作について次に説明する。なお、図２は、空間移動量算出装置の動作を示すフローチャートである。

（２−１）座標系
ここで、本実施形態における座標系について説明する。

図３に示すように画像面１００に対して、空間座標系Ｏ−ＸＹＺと画像座標系ｏ−ｕｖとする。なお、ベクトル量は、明細書中では、太字を表記できないので、図面と式では太字で表記する。また、スカラー量は、図面、式、明細書共に、例えば、「Ａ」と表記する。

（２−２）特徴量
次に、本実施形態における特徴量について説明する。

「特徴量」は、特徴点の位置のカラー値、濃度値、ヒストグラム、曲率など特徴となるものであれば何でも構わない。なお、特徴点群１と特徴点群２に、物体１０１上の同一の特徴点が含まれている必要はない。また、特徴点群１と特徴点群２のそれぞれの特徴点の対応がついている必要もない。

（３）ステップ１０
入力部１０に特徴点群１の情報が入力する。その情報とは、画像座標上の位置ｘ＝（ｕ，ｖ）、空間座標上の位置Ｘ＝（Ｘ，Ｙ，Ｚ）、特徴量ｆである。特徴点群１は、図４（ａ）に示すように移動前の物体１０１上から選択する。

入力部１１に特徴点群２の情報が入力する。その情報とは、画像座標上の位置ｘ’＝（ｕ’，ｖ’）、空間座標上の位置Ｘ’＝（Ｘ’，Ｙ’，Ｚ’）、特徴量ｆ’が入力する。特徴点群２は、図４（ｂ）に示すように移動後の物体１０１を包含する領域１０２から選択する。

ここで、画像座標上の位置は、撮影したカメラ画像中の特徴点の画素位置から求める。

また、空間座標上の位置は、空間上の実際の物体１０１にレーザを照射してレーザ測光を行い、前記画像座標上の位置と対応させる。なお、空間座標上の位置は、ステレオカメラで物体１０１を撮影して、三角測量の原理で求めてもよい。

（４）ステップ２０
入力部１０は、特徴点群１から、移動物体１０１の空間座標上の重心と画像座標上の重心を算出する。

空間座標上の重心Ｘ_＊＝（Ｘ_＊，Ｙ_＊，Ｚ_＊）は、式（２０−１）により与えられる。

ここで、Ｘ_ｉは全Ｎ個ある特徴点群１の特徴点のうちのｉ番目の特徴点の空間位置を表している。

続いて、Ｘ_＊が画像へ投影された位置ｘ_＊＝（ｕ_＊，ｖ_＊）を式（２０−２）から求める。

式（２０−２）は、空間位置が画像面１００に投影されるカメラモデルを表している。ｆはカメラの焦点距離を有効画素間隔で除算したものであり、（ｕ_０，ｖ_０）は光軸が画像面１００と交わった位置である。

（５）ステップ３０
入力部１１は、特徴点群２から、移動後の物体の仮の空間座標上の重心Ｙと画像座標上の仮の重心ｙを算出する。「仮」というのは、真の重心が求められていないための表記であるとする。仮の重心ｙは、特徴点群１の画像上の重心ｘ_＊から、ｐ＝（ｐ_１，ｐ_２）ずれた位置であるとする。すなわち、

ｙ＝ｘ_＊＋ｐ

である。

なお、仮の重心Ｙは、式（３０−１）で示すようにｐ（画像上の位置ずれ）とｓ（画像上におけるスケール変化）で決まる。

仮の重心ｙについて、なんらかの事前知識がある場合には、その値を仮の重心として入力し、ない場合には特徴点群１と同じ重心（すなわちｐ＝（０，０）、ｓ＝１）としてもよいし、特徴点群２の重心と同じとしてもよい。

（６）ステップ４０
画像位置関係算出部１２は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの画像座標上の位置関係に関する第１の類似度の関数ｗ_ｉｊを式（４０−１）に従い定義する。

但し、関数ｗは、ＲＢＦ（Ｒａｄｉａｌ Ｂａｓｉｓ Ｆｕｎｃｔｉｏｎ）カーネルである。ｓは画像１００におけるスケール変化を表し、σは予め定められたバンド幅を表す。Δｘ_ｉ＝ｘ_ｉ―ｘ_＊であり、重心ｘ_＊を中心としたベクトル、Δｘ_ｊ’＝ｘ_ｊ’−ｙであり、仮の重心ｙを中心としたベクトルである。

そして、ｓとｐ＝（ｐ_１，ｐ_２）を含むΔｘ_ｊ’が未知数である。

本実施形態では、関数ｗの形として、式（４０−２）に示す形を利用する。

式（４０−２）から明らかなように、ｘが大きくなるほど関数ｗの大きさは小さくなる関係にあり、式（４０−１）では各特徴点群の重心からの距離と片方をｓ倍したものが同じ方向、長さのベクトルのとき、第１の類似度の関数ｗ_ｉｊが大きくなる。式の説明は後述するステップ７０にてまとめて行う。

（７）ステップ５０
空間位置関係算出部１３は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの空間座標の位置関係に関する第２の類似度の関数ｍ_ｉｊを式（５０−１）に従い定義する。

但し、式（５０−１）において、関数ｍは、ステップ４０の関数ｗと同様にＲＢＦカーネルである。ｎは予め定められたバンド幅である。また、Ｘ_ｉは特徴点ｉの空間座標上の位置、Ｘ’_ｊは特徴点ｊの空間座標上の位置であり、ΔＸは移動前の物体１０１と移動後の物体１０１間の空間移動量である。

そして、ｓとｐ＝（ｐ_１，ｐ_２）を含むΔＸが未知数である。

ΔＸはカメラモデルを用いることで、画像面１００と空間座標間の関係を決めることができ、ステップ４０のｐ＝（ｐ_１，ｐ_２）とｓを用いて、

と表される。

（８）ステップ６０
特徴量関係算出部１４は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの特徴量に関する第３の類似度ｋ_ｉｊを式（６０−１）に従い定義する。

式（６０−１）において、ベクトルｆ_ｉは特徴点ｉの特徴量、ベクトルｆ_ｊ’は特徴点ｊの特徴量である。関数ｋは、ステップ４０の関数ｗと同様にＲＢＦカーネルである。ｈは予め定められたバンド幅である。

（９）ステップ７０
統合部１５は、画像位置関係算出部１２、空間位置関係算出部１３、特徴量関係算出部１４でそれぞれ算出された３種類の類似度の関数ｗ_ｉｊ、ｍ_ｉｊ、ｋ_ｉｊを統合する。

まず、特徴点ｉと特徴点ｊ間の統合類似度Ｊ_ｉｊは、ステップ５０〜７０の３種類の類似度の関数ｗ_ｉｊ、ｍ_ｉｊ、ｋ_ｉｊの積（式（７０−１））によって求める。

特徴点群１に含まれる点と特徴点群２に含まれる点の全ての組み合わせについて、ｉとｊを変えることにより統合類似度Ｊ_ｉｊを求め、式（７０−２）に従い、それらの和を求めることで、特徴点群１と特徴点群２の統合類似度Ｊを求める。

この統合類似度Ｊの意味について説明する。

簡単のため、最初に統合類似度Ｊはｗのみの関数とし、ｓは１であるとする。

図５（ａ）に示すように移動前の物体上の点ｉ，ｉ’，ｉ''と移動後の物体上の点ｊ，ｊ’，ｊ''と、移動前の物体の重心ｘ_＊と、移動後物体の仮の重心ｙ（ｐを移動量とすればｙ＝ｘ_＊＋ｐで記述できる）とする。このとき、求めたい変数は移動量ｐである。式（４０−１）に示す関数ｗの括弧の中を見ると、例えば点ｉの重心ｘ_＊からのベクトル（ｘ_ｉ−ｘ_＊）と点ｊの仮の重心ｙからのベクトル（ｘ_ｊ’−ｙ）の差となっている。その様子を図示したものが図５（ｂ）である。関数ｗは、ベクトルのノルムが小さいときに大きな値を取るため、直観的には図５（ｂ）に示す点ｉ，ｉ’，ｉ''を中心として半径が大きくなるほど濃度の薄まるようなものである。統合類似度ＪのΣ記号を見ると点ｉ，ｉ’，ｉ''と点ｊ，ｊ’，ｊ''間の関数ｗの全ての組み合わせの和である。仮の重心ｙが移動後の真の重心と遠い場合には、（ｘ_ｉ−ｘ_＊）と（ｘ_ｊ’−ｙ）とが離れてしまい、関数ｗが小さい値を取ることが多くなるため、全体として統合類似度Ｊの値は小さくなる。そのため、統合類似度Ｊの値を最大にする重心ｙ、すなわち真の重心を移動量ｐを推定することで求める。

式（７０−２）の統合類似度Ｊには、関数ｗのほかに関数ｋや関数ｍが積の形で付加されているが、関数ｋは点同士がなんらの特徴量を尺度として測ったときに近いときは大きく（例えば似た色なら大きく）、遠いときは小さくなり、関数ｍは空間的な位置の近さを評価している。よって、統合類似度Ｊは、ある重心が与えられたときの点同士の画像位置と特徴と空間位置を考慮した類似度とみなせる。

（１０）ステップ８０
空間移動量算出部１６は、空間移動量ΔＸを算出する。この算出方法について複数説明する。

（１０−１）第１の算出方法
第１の算出方法について説明する。

空間移動量ΔＸは、式（５０−２）よりｐ＝（ｐ_１，ｐ_２）とｓで決まる。そのため、統合部１５のステップ７０で統合された式（７０−２）を

に従い、ｐ＝（ｐ_１，ｐ_２）とｓをある間隔で設定し、全てのｐ＝（ｐ_１，ｐ_２）とｓに対して３種類の類似度の関数ｗ_ｉｊ、ｍ_ｉｊ、ｋ_ｉｊを求めつつ、統合類似度Ｊを算出することで統合類似度Ｊを最大にするｐ＝（ｐ_１，ｐ_２）とｓを探索する。

（１０−２）第２の算出方法
第２の算出方法について説明する。この算出方法は、ｐ＝（ｐ_１，ｐ_２）とｓを、第１の算出方法より効率的に探す方法である。

まず、統合類似度Ｊを式（８０−２）の関数とし、関数Ｌと置き直す。

関数Ｊの最大値は関数Ｌの最小値であるので、関数Ｌの最小値を関数Ｌの極値を求めることで推定する。

最初に、ｐ＝（ｐ_１，ｐ_２）のｐ_１の求め方について説明する。

まず、関数Ｌの極値を求めるには、関数Ｌをｐ_１で偏微分（∇）したものが０となるので、

が０となればよい。

そこで、式（８０−３）の∇Ｊについて計算すると、

となる。式（８０−３）に式（８０−４）を代入し、∇Ｌ＝０の下で式を整理すると、

となる。よって、式（８０−５）からｐ_１を求めると、式（８０−２）の関数Ｌを最小にするｐ_１を求めることができる。

同様に、ｐ＝（ｐ_１，ｐ_２）のｐ_２についても計算すると、

となる。

続いて、ｓに関しても同様の手順で計算すると、

となる。ｓに関しては３次方程式となっているため、ｓを求めるために例えばカルダノの公式などを用い、０以上の実数をｓの解として選択すればよい。これは、正の実数解は必ず１つしかないからである。

なお、式（８０−５）、式（８０−６）、式（８０−７）の右辺にあるｐ及びｓは移動後の物体の重心を算出する際に利用した値を用いる。

（１１）ステップ９０
出力部１８では、ステップ８０で求められたｐ＝（ｐ_１，ｐ_２）及びｓを用いて物体１０１の移動後の仮の重心を更新する。

具体的には、ｐ＝（ｐ_１，ｐ_２）及びｓを空間移動量算出部１６のステップ８０で求められた値にそれぞれ更新する。

また、式（５０−２）によれば、ｐ＝（ｐ_１，ｐ_２）及びｓが求められたとき、空間移動量であるΔＸの関係について表してある式であるため、式（５０−２）からΔＸを求めることもできる。

（１２）効果
本実施形態によれば、物体の特徴点の画像位置とその空間位置と特徴量を全て考慮することにより、空間位置成分にノイズが多く含まれる場合にも安定かつ高速に空間移動量を算出できる。

（第２の実施形態）
第２の実施形態の空間移動量算出装置について図６と図７に基づいて説明する。

本実施形態を係わる空間移動量算出装置の構成は、第１の実施形態と同じである（図１参照）。但し、本実施形態は、第１の実施形態とは異なり、空間座標上の位置Ｘを使わずに視差ｄを利用して空間移動量を算出する。

視差ｄとは、図７に示すように基準画像１０３と参照画像１０４の間に投影された位置のｕ軸方向の差である。例えば、Ｘ_＊の視差はｕ_＊−ｕ''_＊である。Ｏ−ＸＹＺ座標を基準カメラを中心とした基準座標系、Ｏ''−Ｘ''Ｙ''Ｚ''を参照カメラを中心とした参照座標系であるとし、ＯとＯ''間の基線長をｂとする。Ｘ軸とＸ''軸が同一線上にあり、かつ、Ｚ軸とＺ''軸が平行で、かつ、基準画像１０３と参照画像１０４が同一平面上にある場合（一般に平行ステレオ配置である）には、視差ｄと画像位置ｘ＝（ｕ，ｖ）から空間座標上の位置Ｘを

として求めることができる。

そのため、視差ｄを空間位置として用いることで空間移動量を算出する。以下では、第１の実施形態において空間位置として空間座標上の位置Ｘを用いていたところを視差ｄと置き換える。各ステップについて、図７のフローチャートに基づいて説明する。

（１）ステップ１１
空間座標上の位置Ｘを視差ｄとして空間位置に利用する以外はステップ１０と同様である。

すなわち、入力部１０に、特徴点群１の情報が入力する。その情報とは、画像座標上の位置ｘ＝（ｕ，ｖ）、視差ｄ、特徴量ｆである。

入力部１１に、特徴点群２の情報が入力する。その情報とは、画像座標上の位置ｘ’＝（ｕ’，ｖ’）、視差ｄ’、特徴量ｆ’である。

（２）ステップ２１
入力部１０は、特徴点群１から、物体１０１の視差の重心と画像座標上の重心を算出する。

視差の重心ｄ_＊は式（２１−１）により与えられる。ｄ_ｉは全Ｎ個ある特徴点群１の特徴点のうち、ｉ番目の特徴点の視差を表している。またｄ_ｊ’は全Ｍ個ある特徴点群２の特徴点のうち、ｊ番目の特徴点の視差を表している。

続いて、画像位置の重心ｘ_＊＝（ｕ_＊，ｖ_＊）を特徴点群１の画像位置ｘ_ｉから式（２１−２）を使って求める。

（３）ステップ３１
入力部１１は、特徴点群２から、移動後の物体の視差の仮の重心Ｙと画像座標上の仮の重心ｙを算出する。

ステップ３０では、物体１０１の仮の空間座標上の重心Ｙ（ベクトル）であったが、本ステップでは視差の重心として、重心Ｙ（スカラー）とするほかは同じ手順である。

ここで視差の重心Ｙは、

である。なお、ｓとは、前記画像上における前記対象物体の移動前後の視差のスケール変化であって、未知数である。

（４）ステップ４１
本ステップは、ステップ４０と同じ手順である。すなわち、画像位置関係算出部１２は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの画像座標上の位置関係に関する第１の類似度の関数ｗ_ｉｊを定義する。

（５）ステップ５１
空間位置関係算出部１３は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの空間位置の関係に関する第２の類似度を式（５１−１）に従い定義する。

式（５１−１）において、関数ｍは、ステップ４０の関数ｗと同様にＲＢＦカーネルである。ｎは予め定められたバンド幅である。また、ｄ_ｉは特徴点ｉの視差、ｄ_ｊ’は特徴点ｊの視差であり、Δｄは移動前の物体１０１と移動後の物体１０１間の視差の変化量である。空間移動量ΔＸはカメラモデルを用いることで求めることができ、ステップ４１のｐ＝（ｐ_１，ｐ_２）とｓを用いて、

と表される。

（６）ステップ６１
本ステップは、ステップ６０と同じ手順である。すなわち、徴量関係算出部１４は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの特徴量に関する第３の類似度ｋ_ｉｊを定義する。

（７）ステップ７１
本ステップは、ステップ７０と同じ手順である。すなわち、統合部１５は、画像位置関係算出部１２、空間位置関係算出部１３、特徴量関係算出部１４でそれぞれ算出された３種類の類似度の関数ｗ_ｉｊ、ｍ_ｉｊ、ｋ_ｉｊを統合する。

（８）ステップ８１
空間移動量算出部１６は、空間移動量ΔＸを算出する。本ステップは、ステップ８０とほぼ同じ手順であるが、空間座標については空間位置Ｘから視差ｄに代わったので、それを考慮して空間移動量を算出する。

（８−１）第１の算出方法
第１の算出方法について説明する。

算出したい空間移動量ΔＸは式（５１−２）より、ｐ＝（ｐ_１，ｐ_２）とｓで決まる。そのため、ステップ７０で統合された式（７０−２）を

に従い、ｐ＝（ｐ_１，ｐ_２）とｓをある間隔で設定し、全てのｐ＝（ｐ_１，ｐ_２）とｓに対して３種類の類似度の関数ｗ_ｉｊ、ｍ_ｉｊ、ｋ_ｉｊを求めつつ、統合類似度Ｊを算出することで統合類似度Ｊを最大にするｐ＝（ｐ_１，ｐ_２）とｓを探索する。

（８−２）第２の算出方法
第２の算出方法について説明する。本算出方法は、ｐ＝（ｐ_１，ｐ_２）とｓを、第１の算出方法より効率的に探す方法である。

まず、統合類似度Ｊを式（８１−２）の関数とし、関数Ｊを関数Ｌと置き直す。

関数Ｊの最大値は関数Ｌの最小値であるので、関数Ｌの最小値を関数Ｌの極値を求めることで推定する。

最初に、ｐ＝（ｐ_１，ｐ_２）のｐ_１の求め方について説明する。

まず、関数Ｌの極値を求めるには、関数Ｌをｐ_１で偏微分（∇）したものが０となるので、

が０となればよい。

そこで、式（８１−３）の∇Ｊについて計算すると、

となる。

式（８１−３）に式（８１−４）を代入し、∇Ｌ＝０の下で式を整理すると、

となる。よって、式（８１−５）からｐ_１を求めると、式（８１−２）の関数Ｌを最小にするｐ_１を求めることができる。

同様に、ｐ＝（ｐ_１，ｐ_２）のｐ_２についても計算すると、

となる。

続いて、ｓに関しても同様の手順で計算すると、

となる。ｓに関しては３次方程式となっているため、ｓを求めるために例えばカルダノの公式などを用い、０以上の実数をｓの解として選択すればよい。これは、正の実数解は必ず１つしかないからである。

なお、式（８１−５）、式（８１−６）、式（８１−７）の右辺にあるｐ及びｓは、移動後の物体の重心を算出する際に利用した値を用いる。

（９）ステップ９１
出力部１７は、空間移動量算出部１６のステップ８１によって求められたｐ＝（ｐ_１，ｐ_２）及びｓを用いて物体１０１の移動後の仮の重心を更新する。

具体的には、ｐ及びｓをステップ８１で求められた値にそれぞれ更新する。

また、式（５１−２）は、ｐ＝（ｐ_１，ｐ_２）及びｓが求められたとき、空間移動量であるΔＸの関係について表している式であるため、式（５１−２）からΔＸを求める。

（１０）効果
本実施形態によれば、物体の特徴点の画像位置とその視差と特徴量を全て考慮することにより、空間位置成分にノイズが多く含まれる場合にも安定かつ高速に空間移動量を算出できる。

（第３の実施形態）
第２の実施形態の空間移動量算出装置について図８と図９に基づいて説明する。

図８は、本実施形態を係わる空間移動量算出装置の構成を示すブロック図であり、図９は、そのフローチャートである。

（１）空間移動量算出装置の構成
図８は、本実施形態を係わる空間移動量算出装置の構成を示すブロック図である。

空間移動量算出装置は、特徴点群１の画像位置と空間位置と特徴量を入力する入力部１０と、特徴点群２の画像位置と空間位置と特徴量を入力する入力部１１と、入力された特徴点群１と特徴点群２の間の画像位置関係から第１の類似度を算出する画像位置関係算出部１２と、空間位置関係から第２の類似度を算出する空間位置関係算出部１３と、特徴量関係から第３の類似度を算出する特徴量関係算出部１４と、第１の類似度〜第３の類似度を統合する統合部１５と、統合部１５から空間移動量を推定する空間移動量算出部１６と、空間移動量の妥当性を判断する判定部１８と、妥当でない場合に空間移動量から求まる重心を更新する空間移動量更新部１８と、空間移動量を出力する出力部１７とを備えている。

ここでは、第１の実施形態である空間位置に空間座標上の位置を用いた場合の実施形態について説明する。

（２）ステップ１２〜９２
ステップ１２〜９２は、ステップ１０〜８０と同じであるが、下記の追加項目がある。

第１の追加項目は、ステップ１２に空間移動量の繰り返し回数を保存する変数Ｃを用意し、Ｃ＝０としておく。

第２の追加項目は、ステップ９２に空間移動量を算出する毎に繰り返し回数Ｃに１を足す。

（３）ステップ１０２
判定部１８は、ステップ８２で算出されたｐ＝（ｐ_１，ｐ_２）及びｓについて、算出結果が妥当であるかの判定を行う。例えば、以下の条件が考えられる。

第１の条件は、ステップ９２で更新された繰り返し回数Ｃが期待する回数以上になったかどうかである。

第２の条件は、前回（すなわち、Ｃ−１回目）算出されたｐ＝（ｐ_１，ｐ_２）及びｓと今回（Ｃ回目に）算出されたｐ＝（ｐ_１，ｐ_２）及びｓの差のノルムが期待する値より小さいかどうかである。

第３の条件は、式（７０−２）が期待する値を超えたかどうかである。

第４の条件は、算出されたｐ＝（ｐ_１，ｐ_２）及びｓが期待する範囲に収まったかどうかである。

なお、これらの条件を単独で採用してもよいし、複数組み合わせてもよい。

判定部１８の判定により、仮に定めた重心が正確でないと判定された場合にはステップ４２へ戻る。

前回の算出で定めた空間移動量に基づいて、重心の位置を再計算して、ステップ４２〜ステップ１０２を繰り返し行うことによって、空間移動量をより精度よく算出することが可能となる。

（４）効果
本実施形態によれば、重心の更新を行うことにより、空間位置成分にノイズが多く含まれる場合にもより安定に空間移動量を算出できる。

（変更例）
なお、本発明は上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態にわたる構成要素を適宜組み合わせてもよい。

（１）変更例１
第３の実施形態の更新において、第２の実施形態を採用してもよい。

（２）変更例２
上記各実施形態では、重心を用いたが、これに限らず、特徴点群から定められる特定の位置を用いても良い。

本発明の第１の実施形態の空間移動量算出装置を示すブロック図である。 同じく空間移動量算出装置の動作を示すフローチャートである。 空間座標系及び画像座標系の説明図である。 特徴点群１と特徴点群２の説明図である。 移動量ｐに限定した場合の類似度を説明図である。 第２の実施形態の空間移動量算出装置の動作を示すフローチャートである。 空間座標系及び画像座標系及び視差の説明図である。 第３の実施形態の空間移動量算出装置を示すブロック図である。 同じく空間移動量算出装置の動作を示すフローチャートである。

符号の説明

１０ 入力部
１１ 入力部
１２ 画像位置関係算出部
１３ 空間位置関係算出部
１４ 特徴量関係算出部
１５ 統合部
１６ 空間移動量算出部
１７ 空間移動量出力部
１８ 判定部
１９ 空間移動量更新部

Claims (9)

1. 対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力部と、
   前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力部と、
   前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出部と、
   前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出部と、
   前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出部と、
   前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合部と、
   前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出部と、
   を有する空間移動量算出装置。
2. 前記統合部は、前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度の関数の極値を求めて、前記統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める、
   請求項１記載の空間移動量算出装置。
3. 前記空間位置とは、前記対象物体の空間座標上の位置である、
   請求項１記載の空間移動量算出装置。
4. 前記空間位置とは、前記対象物体をステレオカメラで撮影したときの基準画像と参照画像との視差である、
   請求項１記載の空間移動量算出装置。
5. 前記奥行き方向に関するパラメータとは、前記画像上における前記対象物体の移動前後のスケール変化、または、前記画像上における前記対象物体の移動前後の視差のスケール変化である、
   請求項１記載の空間移動量算出装置。
6. 前記統合類似度の関数には、前記複数の第２の特徴点から定められる第２の特定位置を未知数としてさらに含み、
   前記統合部から算出された前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記第２の特定位置を算出し直す更新部をさらに有する、
   請求項１記載の空間移動量算出装置。
7. 前記更新部は、
   （１）任意の回数を更新した場合、
   （２）前記更新前後の前記画像上の移動量と前記奥行き方向に関するパラメータの差が任意の値より小さい場合、
   （３）前記統合類似度が任意の値を超えた場合、または、
   （４）前記更新後の前記画像上の移動量と前記奥行き方向に関するパラメータが任意の範囲に収まった場合、
   前記更新を終了する、
   請求項６記載の空間移動量算出装置。
8. 対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力ステップと、
   前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力ステップと、
   前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出ステップと、
   前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出ステップと、
   前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出ステップと、
   前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合ステップと、
   前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出ステップと、
   を有する空間移動量算出方法。
9. 対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力機能と、
   前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力機能と、
   前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出機能と、
   前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出機能と、
   前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出機能と、
   前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合機能と、
   前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出機能と、
   をコンピュータによって実現する空間移動量算出プログラム。