(57)【特許請求の範囲】
【請求項１】
対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力部と、
前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力部と、
前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出部と、
前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出部と、
前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出部と、
前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合部と、
前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出部と、
を有する空間移動量算出装置。
【請求項２】
前記統合部は、前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度の関数の極値を求めて、前記統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める、
請求項１記載の空間移動量算出装置。
【請求項３】
前記空間位置とは、前記対象物体の空間座標上の位置である、
請求項１記載の空間移動量算出装置。
【請求項４】
前記空間位置とは、前記対象物体をステレオカメラで撮影したときの基準画像と参照画像との視差である、
請求項１記載の空間移動量算出装置。
【請求項５】
前記奥行き方向に関するパラメータとは、前記画像上における前記対象物体の移動前後のスケール変化、または、前記画像上における前記対象物体の移動前後の視差のスケール変化である、
請求項１記載の空間移動量算出装置。
【請求項６】
前記統合類似度の関数には、前記複数の第２の特徴点から定められる第２の特定位置を未知数としてさらに含み、
前記統合部から算出された前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記第２の特定位置を算出し直す更新部をさらに有する、
請求項１記載の空間移動量算出装置。
【請求項７】
前記更新部は、
（１）任意の回数を更新した場合、
（２）前記更新前後の前記画像上の移動量と前記奥行き方向に関するパラメータの差が任意の値より小さい場合、
（３）前記統合類似度が任意の値を超えた場合、または、
（４）前記更新後の前記画像上の移動量と前記奥行き方向に関するパラメータが任意の範囲に収まった場合、
前記更新を終了する、
請求項６記載の空間移動量算出装置。
【請求項８】
対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力ステップと、
前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力ステップと、
前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出ステップと、
前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出ステップと、
前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出ステップと、
前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合ステップと、
前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出ステップと、
を有する空間移動量算出方法。
【請求項９】
対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力機能と、
前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力機能と、
前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出機能と、
前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出機能と、
前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出機能と、
前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合機能と、
前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出機能と、
をコンピュータによって実現する空間移動量算出プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、空間中を運動している物体を例えばＴＶカメラ等で撮影して、その物体の３次元運動を推定する空間移動量算出装置及びその方法に関するものである。
【背景技術】
【０００２】
従来、ステレオカメラなどを用いて空間中の移動体の追跡をするときに、移動物体上の特徴点の画像位置とその空間位置とその特徴量（例えば、濃度値）の関係を全て考慮して追跡していなかった（例えば、特許文献１、非特許文献１参照）。なお、「画像位置」とは、画像座標上での位置を意味し、「空間位置」とは、空間座標上の位置または視差を意味し、「視差」とは、空間中の同一点が２枚の画像に投影されたとき、それらの点の画像位置のずれ量をいう。
【０００３】
なぜならば、空間移動量と画像変化量は、それぞれ独立に決められるものではないため、両方を考慮したものは実現されていなかった。なお、「画像変化量」とは、移動体が空間中を移動することによる画像上で観測される移動体の位置ずれ及び大きさの変化を意味する。
【０００４】
ところが、画像位置や特徴量は画像から直接観測されるため、ノイズが比較的少ないが、空間位置は例えば複数のＴＶカメラ等から三角測量の原理に基づいて推定するため、ノイズが多く含まれている可能性があり、そのような場合には従来の方法では安定して空間移動量を算出することが困難であった。
【特許文献１】特開平１０‐２７２４９号公報
【非特許文献１】Ｔｙａｇｉほか３名：「Ｆｕｓｉｏｎ ｏｆ Ｍｕｌｔｉｐｌｅ Ｃａｍｅｒａ Ｖｉｅｗｓ ｆｏｒ Ｋｅｒｎｅｌ−Ｂａｓｅｄ  ３Ｄ Ｔｒａｃｋｉｎｇ」、ＩＥＥＥ Ｗｏｒｋｓｈｏｐ ｏｎ Ｍｏｔｉｏｎ ｏｎ Ｖｉｄｅｏ Ｃｏｍｐｕｔｉｎｇ、２００７
【発明の開示】
【発明が解決しようとする課題】
【０００５】
上述したように、従来技術には移動体の空間移動量を推定するときに、移動物体上の特徴点の画像位置とその空間位置と特徴量を全て考慮して推定するものがないという問題点があった。また、空間位置にノイズが入ったりした場合には、安定に空間移動量を推定することが困難であるという問題点があった。
【０００６】
そこで本発明は、上記従来技術の問題点を解決するためになされたものであって、空間中を運動している物体の特徴点の画像位置とその空間位置とその特徴量を全て考慮した空間移動量算出装置及びその方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明は、対象物体の移動前の追跡領域に含まれる複数の第１の特徴点の画像位置、空間位置及び特徴量を入力する第１入力部と、前記対象物体の移動後の追跡領域に含まれる複数の第２の特徴点の画像位置、空間位置及び特徴量を入力する第２入力部と、前記複数の第１の特徴点のそれぞれの画像位置と、前記複数の第２の特徴点のそれぞれの画像位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータとを未知数として含み、かつ、（２）前記両画像位置間の距離が近いほど値が大きくなる第１の類似度の関数をそれぞれ求める画像位置関係算出部と、前記複数の第１の特徴点のそれぞれの空間位置と、前記複数の第２の特徴点のそれぞれの空間位置とに基づいて、（１）前記画像上の移動量と奥行き方向に関するパラメータを未知数として含み、かつ、（２）前記両空間位置間の距離が近いほど値が大きくなる第２の類似度の関数をそれぞれ求める空間位置関係算出部と、前記複数の第１の特徴点のそれぞれの特徴量と、前記複数の第２の特徴点のそれぞれの特徴量とに基づいて、前記両特徴量が類似するほど値が大きくなる第３の類似度の関数をそれぞれ求める特徴量関係算出部と、前記各第１の類似度の関数、前記各第２の類似度の関数及び前記各第３の類似度の関数の積和によって求められる統合類似度が最も大きくなる前記画像上の移動量と、前記奥行き方向に関するパラメータを求める統合部と、前記画像上の移動量と、前記奥行き方向に関するパラメータに基づいて、前記対象物体の移動前後の空間移動量を算出する空間移動量算出部と、を有する空間移動量算出装置である。
【発明の効果】
【０００８】
本発明によれば、物体の特徴点の画像位置とその空間位置と特徴量を全て考慮することにより、空間位置成分にノイズが多く含まれる場合にも安定かつ高速に空間移動量を算出できる。
【発明を実施するための最良の形態】
【０００９】
以下、本発明の実施形態に係わる空間移動量算出装置について図面に基づいて説明する。
【００１０】
（第１の実施形態）
本発明の第１の実施形態を係わる空間移動量算出装置について、図１〜図５に基づいて説明する。
【００１１】
空間移動量算出装置は、空間上を移動する物体（例えば、人間、車等）１０１の移動前の重心と、移動後の重心の空間移動量を算出する。
【００１２】
（１）空間移動量算出装置の構成
図１は、空間移動量算出装置を示すブロック図である。
【００１３】
空間移動量算出装置は、物体１０１の移動前の追跡対象領域内の複数の特徴点（以下、「特徴点群１」という）の画像位置と空間位置と特徴量を入力する入力部１０と、物体１０１の移動後の追跡対象を含む領域内の複数の特徴点（以下、「特徴点群２」という）の画像位置と空間位置と特徴量を入力する入力部１１と、入力された特徴点群１と特徴点群２の間の画像位置関係から第１の類似度を算出する画像位置関係算出部１２と、空間位置関係から第２の類似度を算出する空間位置関係算出部１３と、特徴量関係から第３の類似度を算出する特徴量関係算出部１４と、第１の類似度〜第３の類似度を統合する統合部１５と、統合部１５から空間移動量を推定する空間移動量算出部１６と、空間移動量を出力する出力部１７とを備えている。
【００１４】
なお、各部１０〜１７の機能は、コンピュータに格納されたプログラムによっても実現可能である。
【００１５】
（２）空間移動量算出装置の動作
次に、図１〜図３を用いて、空間移動量算出装置の動作について次に説明する。なお、図２は、空間移動量算出装置の動作を示すフローチャートである。
【００１６】
（２−１）座標系
ここで、本実施形態における座標系について説明する。
【００１７】
図３に示すように画像面１００に対して、空間座標系Ｏ−ＸＹＺと画像座標系ｏ−ｕｖとする。なお、ベクトル量は、明細書中では、太字を表記できないので、図面と式では太字で表記する。また、スカラー量は、図面、式、明細書共に、例えば、「Ａ」と表記する。
【００１８】
（２−２）特徴量
次に、本実施形態における特徴量について説明する。
【００１９】
「特徴量」は、特徴点の位置のカラー値、濃度値、ヒストグラム、曲率など特徴となるものであれば何でも構わない。なお、特徴点群１と特徴点群２に、物体１０１上の同一の特徴点が含まれている必要はない。また、特徴点群１と特徴点群２のそれぞれの特徴点の対応がついている必要もない。
【００２０】
（３）ステップ１０
入力部１０に特徴点群１の情報が入力する。その情報とは、画像座標上の位置ｘ＝（ｕ，ｖ）、空間座標上の位置Ｘ＝（Ｘ，Ｙ，Ｚ）、特徴量ｆである。特徴点群１は、図４（ａ）に示すように移動前の物体１０１上から選択する。
【００２１】
入力部１１に特徴点群２の情報が入力する。その情報とは、画像座標上の位置ｘ’＝（ｕ’，ｖ’）、空間座標上の位置Ｘ’＝（Ｘ’，Ｙ’，Ｚ’）、特徴量ｆ’が入力する。特徴点群２は、図４（ｂ）に示すように移動後の物体１０１を包含する領域１０２から選択する。
【００２２】
ここで、画像座標上の位置は、撮影したカメラ画像中の特徴点の画素位置から求める。
【００２３】
また、空間座標上の位置は、空間上の実際の物体１０１にレーザを照射してレーザ測光を行い、前記画像座標上の位置と対応させる。なお、空間座標上の位置は、ステレオカメラで物体１０１を撮影して、三角測量の原理で求めてもよい。
【００２４】
（４）ステップ２０
入力部１０は、特徴点群１から、移動物体１０１の空間座標上の重心と画像座標上の重心を算出する。
【００２５】
空間座標上の重心Ｘ＊＝（Ｘ＊，Ｙ＊，Ｚ＊）は、式（２０−１）により与えられる。
【数１】
0004982410.tif 000002
【００２６】
ここで、Ｘｉは全Ｎ個ある特徴点群１の特徴点のうちのｉ番目の特徴点の空間位置を表している。
【００２７】
続いて、Ｘ＊が画像へ投影された位置ｘ＊＝（ｕ＊，ｖ＊）を式（２０−２）から求める。
【数２】
0004982410.tif 000003
【００２８】
式（２０−２）は、空間位置が画像面１００に投影されるカメラモデルを表している。ｆはカメラの焦点距離を有効画素間隔で除算したものであり、（ｕ０，ｖ０）は光軸が画像面１００と交わった位置である。
【００２９】
（５）ステップ３０
入力部１１は、特徴点群２から、移動後の物体の仮の空間座標上の重心Ｙと画像座標上の仮の重心ｙを算出する。「仮」というのは、真の重心が求められていないための表記であるとする。仮の重心ｙは、特徴点群１の画像上の重心ｘ＊から、ｐ＝（ｐ１，ｐ２）ずれた位置であるとする。すなわち、
ｙ＝ｘ＊＋ｐ
である。
【００３０】
なお、仮の重心Ｙは、式（３０−１）で示すようにｐ（画像上の位置ずれ）とｓ（画像上におけるスケール変化）で決まる。
【数３】
0004982410.tif 000004
【００３１】
仮の重心ｙについて、なんらかの事前知識がある場合には、その値を仮の重心として入力し、ない場合には特徴点群１と同じ重心（すなわちｐ＝（０，０）、ｓ＝１）としてもよいし、特徴点群２の重心と同じとしてもよい。
【００３２】
（６）ステップ４０
画像位置関係算出部１２は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの画像座標上の位置関係に関する第１の類似度の関数ｗｉｊを式（４０−１）に従い定義する。
【数４】
0004982410.tif 000005
【００３３】
但し、関数ｗは、ＲＢＦ（Ｒａｄｉａｌ  Ｂａｓｉｓ  Ｆｕｎｃｔｉｏｎ）カーネルである。ｓは画像１００におけるスケール変化を表し、σは予め定められたバンド幅を表す。Δｘｉ＝ｘｉ―ｘ＊であり、重心ｘ＊を中心としたベクトル、Δｘｊ’＝ｘｊ’−ｙであり、仮の重心ｙを中心としたベクトルである。
【００３４】
そして、ｓとｐ＝（ｐ１，ｐ２）を含むΔｘｊ’が未知数である。
【００３５】
本実施形態では、関数ｗの形として、式（４０−２）に示す形を利用する。
【数５】
0004982410.tif 000006
【００３６】
式（４０−２）から明らかなように、ｘが大きくなるほど関数ｗの大きさは小さくなる関係にあり、式（４０−１）では各特徴点群の重心からの距離と片方をｓ倍したものが同じ方向、長さのベクトルのとき、第１の類似度の関数ｗｉｊが大きくなる。式の説明は後述するステップ７０にてまとめて行う。
【００３７】
（７）ステップ５０
空間位置関係算出部１３は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの空間座標の位置関係に関する第２の類似度の関数ｍｉｊを式（５０−１）に従い定義する。
【数６】
0004982410.tif 000007
【００３８】
但し、式（５０−１）において、関数ｍは、ステップ４０の関数ｗと同様にＲＢＦカーネルである。ｎは予め定められたバンド幅である。また、Ｘｉは特徴点ｉの空間座標上の位置、Ｘ’ｊは特徴点ｊの空間座標上の位置であり、ΔＸは移動前の物体１０１と移動後の物体１０１間の空間移動量である。
【００３９】
そして、ｓとｐ＝（ｐ１，ｐ２）を含むΔＸが未知数である。
【００４０】
ΔＸはカメラモデルを用いることで、画像面１００と空間座標間の関係を決めることができ、ステップ４０のｐ＝（ｐ１，ｐ２）とｓを用いて、
【数７】
0004982410.tif 000008
【００４１】
と表される。
【００４２】
（８）ステップ６０
特徴量関係算出部１４は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの特徴量に関する第３の類似度ｋｉｊを式（６０−１）に従い定義する。
【数８】
0004982410.tif 000009
【００４３】
式（６０−１）において、ベクトルｆｉは特徴点ｉの特徴量、ベクトルｆｊ’は特徴点ｊの特徴量である。関数ｋは、ステップ４０の関数ｗと同様にＲＢＦカーネルである。ｈは予め定められたバンド幅である。
【００４４】
（９）ステップ７０
統合部１５は、画像位置関係算出部１２、空間位置関係算出部１３、特徴量関係算出部１４でそれぞれ算出された３種類の類似度の関数ｗｉｊ、ｍｉｊ、ｋｉｊを統合する。
【００４５】
まず、特徴点ｉと特徴点ｊ間の統合類似度Ｊｉｊは、ステップ５０〜７０の３種類の類似度の関数ｗｉｊ、ｍｉｊ、ｋｉｊの積（式（７０−１））によって求める。
【数９】
0004982410.tif 000010
【００４６】
特徴点群１に含まれる点と特徴点群２に含まれる点の全ての組み合わせについて、ｉとｊを変えることにより統合類似度Ｊｉｊを求め、式（７０−２）に従い、それらの和を求めることで、特徴点群１と特徴点群２の統合類似度Ｊを求める。
【数１０】
0004982410.tif 000011
【００４７】
この統合類似度Ｊの意味について説明する。
【００４８】
簡単のため、最初に統合類似度Ｊはｗのみの関数とし、ｓは１であるとする。
【００４９】
図５（ａ）に示すように移動前の物体上の点ｉ，ｉ’，ｉ''と移動後の物体上の点ｊ，ｊ’，ｊ''と、移動前の物体の重心ｘ＊と、移動後物体の仮の重心ｙ（ｐを移動量とすればｙ＝ｘ＊＋ｐで記述できる）とする。このとき、求めたい変数は移動量ｐである。式（４０−１）に示す関数ｗの括弧の中を見ると、例えば点ｉの重心ｘ＊からのベクトル（ｘｉ−ｘ＊）と点ｊの仮の重心ｙからのベクトル（ｘｊ’−ｙ）の差となっている。その様子を図示したものが図５（ｂ）である。関数ｗは、ベクトルのノルムが小さいときに大きな値を取るため、直観的には図５（ｂ）に示す点ｉ，ｉ’，ｉ''を中心として半径が大きくなるほど濃度の薄まるようなものである。統合類似度ＪのΣ記号を見ると点ｉ，ｉ’，ｉ''と点ｊ，ｊ’，ｊ''間の関数ｗの全ての組み合わせの和である。仮の重心ｙが移動後の真の重心と遠い場合には、（ｘｉ−ｘ＊）と（ｘｊ’−ｙ）とが離れてしまい、関数ｗが小さい値を取ることが多くなるため、全体として統合類似度Ｊの値は小さくなる。そのため、統合類似度Ｊの値を最大にする重心ｙ、すなわち真の重心を移動量ｐを推定することで求める。
【００５０】
式（７０−２）の統合類似度Ｊには、関数ｗのほかに関数ｋや関数ｍが積の形で付加されているが、関数ｋは点同士がなんらの特徴量を尺度として測ったときに近いときは大きく（例えば似た色なら大きく）、遠いときは小さくなり、関数ｍは空間的な位置の近さを評価している。よって、統合類似度Ｊは、ある重心が与えられたときの点同士の画像位置と特徴と空間位置を考慮した類似度とみなせる。
【００５１】
（１０）ステップ８０
空間移動量算出部１６は、空間移動量ΔＸを算出する。この算出方法について複数説明する。
【００５２】
（１０−１）第１の算出方法
第１の算出方法について説明する。
【００５３】
空間移動量ΔＸは、式（５０−２）よりｐ＝（ｐ１，ｐ２）とｓで決まる。そのため、統合部１５のステップ７０で統合された式（７０−２）を
【数１１】
0004982410.tif 000012
【００５４】
に従い、ｐ＝（ｐ１，ｐ２）とｓをある間隔で設定し、全てのｐ＝（ｐ１，ｐ２）とｓに対して３種類の類似度の関数ｗｉｊ、ｍｉｊ、ｋｉｊを求めつつ、統合類似度Ｊを算出することで統合類似度Ｊを最大にするｐ＝（ｐ１，ｐ２）とｓを探索する。
【００５５】
（１０−２）第２の算出方法
第２の算出方法について説明する。この算出方法は、ｐ＝（ｐ１，ｐ２）とｓを、第１の算出方法より効率的に探す方法である。
【００５６】
まず、統合類似度Ｊを式（８０−２）の関数とし、関数Ｌと置き直す。
【数１２】
0004982410.tif 000013
【００５７】
関数Ｊの最大値は関数Ｌの最小値であるので、関数Ｌの最小値を関数Ｌの極値を求めることで推定する。
【００５８】
最初に、ｐ＝（ｐ１，ｐ２）のｐ１の求め方について説明する。
【００５９】
まず、関数Ｌの極値を求めるには、関数Ｌをｐ１で偏微分（∇）したものが０となるので、
【数１３】
0004982410.tif 000014
【００６０】
が０となればよい。
【００６１】
そこで、式（８０−３）の∇Ｊについて計算すると、
【数１４】
0004982410.tif 000015
【００６２】
となる。式（８０−３）に式（８０−４）を代入し、∇Ｌ＝０の下で式を整理すると、
【数１５】
0004982410.tif 000016
【００６３】
となる。よって、式（８０−５）からｐ１を求めると、式（８０−２）の関数Ｌを最小にするｐ１を求めることができる。
【００６４】
同様に、ｐ＝（ｐ１，ｐ２）のｐ２についても計算すると、
【数１６】
0004982410.tif 000017
【００６５】
となる。
【００６６】
続いて、ｓに関しても同様の手順で計算すると、
【数１７】
0004982410.tif 000018
【００６７】
となる。ｓに関しては３次方程式となっているため、ｓを求めるために例えばカルダノの公式などを用い、０以上の実数をｓの解として選択すればよい。これは、正の実数解は必ず１つしかないからである。
【００６８】
なお、式（８０−５）、式（８０−６）、式（８０−７）の右辺にあるｐ及びｓは移動後の物体の重心を算出する際に利用した値を用いる。
【００６９】
（１１）ステップ９０
出力部１８では、ステップ８０で求められたｐ＝（ｐ１，ｐ２）及びｓを用いて物体１０１の移動後の仮の重心を更新する。
【００７０】
具体的には、ｐ＝（ｐ１，ｐ２）及びｓを空間移動量算出部１６のステップ８０で求められた値にそれぞれ更新する。
【００７１】
また、式（５０−２）によれば、ｐ＝（ｐ１，ｐ２）及びｓが求められたとき、空間移動量であるΔＸの関係について表してある式であるため、式（５０−２）からΔＸを求めることもできる。
【００７２】
（１２）効果
本実施形態によれば、物体の特徴点の画像位置とその空間位置と特徴量を全て考慮することにより、空間位置成分にノイズが多く含まれる場合にも安定かつ高速に空間移動量を算出できる。
【００７３】
（第２の実施形態）
第２の実施形態の空間移動量算出装置について図６と図７に基づいて説明する。
【００７４】
本実施形態を係わる空間移動量算出装置の構成は、第１の実施形態と同じである（図１参照）。但し、本実施形態は、第１の実施形態とは異なり、空間座標上の位置Ｘを使わずに視差ｄを利用して空間移動量を算出する。
【００７５】
視差ｄとは、図７に示すように基準画像１０３と参照画像１０４の間に投影された位置のｕ軸方向の差である。例えば、Ｘ＊の視差はｕ＊−ｕ''＊である。Ｏ−ＸＹＺ座標を基準カメラを中心とした基準座標系、Ｏ''−Ｘ''Ｙ''Ｚ''を参照カメラを中心とした参照座標系であるとし、ＯとＯ''間の基線長をｂとする。Ｘ軸とＸ''軸が同一線上にあり、かつ、Ｚ軸とＺ''軸が平行で、かつ、基準画像１０３と参照画像１０４が同一平面上にある場合（一般に平行ステレオ配置である）には、視差ｄと画像位置ｘ＝（ｕ，ｖ）から空間座標上の位置Ｘを
【数１８】
0004982410.tif 000019
【００７６】
として求めることができる。
【００７７】
そのため、視差ｄを空間位置として用いることで空間移動量を算出する。以下では、第１の実施形態において空間位置として空間座標上の位置Ｘを用いていたところを視差ｄと置き換える。各ステップについて、図７のフローチャートに基づいて説明する。
【００７８】
（１）ステップ１１
空間座標上の位置Ｘを視差ｄとして空間位置に利用する以外はステップ１０と同様である。
【００７９】
すなわち、入力部１０に、特徴点群１の情報が入力する。その情報とは、画像座標上の位置ｘ＝（ｕ，ｖ）、視差ｄ、特徴量ｆである。
【００８０】
入力部１１に、特徴点群２の情報が入力する。その情報とは、画像座標上の位置ｘ’＝（ｕ’，ｖ’）、視差ｄ’、特徴量ｆ’である。
【００８１】
（２）ステップ２１
入力部１０は、特徴点群１から、物体１０１の視差の重心と画像座標上の重心を算出する。
【００８２】
視差の重心ｄ＊は式（２１−１）により与えられる。ｄｉは全Ｎ個ある特徴点群１の特徴点のうち、ｉ番目の特徴点の視差を表している。またｄｊ’は全Ｍ個ある特徴点群２の特徴点のうち、ｊ番目の特徴点の視差を表している。
【数１９】
0004982410.tif 000020
【００８３】
続いて、画像位置の重心ｘ＊＝（ｕ＊，ｖ＊）を特徴点群１の画像位置ｘｉから式（２１−２）を使って求める。
【数２０】
0004982410.tif 000021
【００８４】
（３）ステップ３１
入力部１１は、特徴点群２から、移動後の物体の視差の仮の重心Ｙと画像座標上の仮の重心ｙを算出する。
【００８５】
ステップ３０では、物体１０１の仮の空間座標上の重心Ｙ（ベクトル）であったが、本ステップでは視差の重心として、重心Ｙ（スカラー）とするほかは同じ手順である。
【００８６】
ここで視差の重心Ｙは、
【数２１】
0004982410.tif 000022
【００８７】
である。なお、ｓとは、前記画像上における前記対象物体の移動前後の視差のスケール変化であって、未知数である。
【００８８】
（４）ステップ４１
本ステップは、ステップ４０と同じ手順である。すなわち、画像位置関係算出部１２は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの画像座標上の位置関係に関する第１の類似度の関数ｗｉｊを定義する。
【００８９】
（５）ステップ５１
空間位置関係算出部１３は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの空間位置の関係に関する第２の類似度を式（５１−１）に従い定義する。
【数２２】
0004982410.tif 000023
【００９０】
式（５１−１）において、関数ｍは、ステップ４０の関数ｗと同様にＲＢＦカーネルである。ｎは予め定められたバンド幅である。また、ｄｉは特徴点ｉの視差、ｄｊ’は特徴点ｊの視差であり、Δｄは移動前の物体１０１と移動後の物体１０１間の視差の変化量である。空間移動量ΔＸはカメラモデルを用いることで求めることができ、ステップ４１のｐ＝（ｐ１，ｐ２）とｓを用いて、
【数２３】
0004982410.tif 000024
【００９１】
と表される。
【００９２】
（６）ステップ６１
本ステップは、ステップ６０と同じ手順である。すなわち、徴量関係算出部１４は、特徴点群１に属する特徴点ｉと特徴点群２に属する特徴点ｊの特徴量に関する第３の類似度ｋｉｊを定義する。
【００９３】
（７）ステップ７１
本ステップは、ステップ７０と同じ手順である。すなわち、統合部１５は、画像位置関係算出部１２、空間位置関係算出部１３、特徴量関係算出部１４でそれぞれ算出された３種類の類似度の関数ｗｉｊ、ｍｉｊ、ｋｉｊを統合する。
【００９４】
（８）ステップ８１
空間移動量算出部１６は、空間移動量ΔＸを算出する。本ステップは、ステップ８０とほぼ同じ手順であるが、空間座標については空間位置Ｘから視差ｄに代わったので、それを考慮して空間移動量を算出する。
【００９５】
（８−１）第１の算出方法
第１の算出方法について説明する。
【００９６】
算出したい空間移動量ΔＸは式（５１−２）より、ｐ＝（ｐ１，ｐ２）とｓで決まる。そのため、ステップ７０で統合された式（７０−２）を
【数２４】
0004982410.tif 000025
【００９７】
に従い、ｐ＝（ｐ１，ｐ２）とｓをある間隔で設定し、全てのｐ＝（ｐ１，ｐ２）とｓに対して３種類の類似度の関数ｗｉｊ、ｍｉｊ、ｋｉｊを求めつつ、統合類似度Ｊを算出することで統合類似度Ｊを最大にするｐ＝（ｐ１，ｐ２）とｓを探索する。
【００９８】
（８−２）第２の算出方法
第２の算出方法について説明する。本算出方法は、ｐ＝（ｐ１，ｐ２）とｓを、第１の算出方法より効率的に探す方法である。
【００９９】
まず、統合類似度Ｊを式（８１−２）の関数とし、関数Ｊを関数Ｌと置き直す。
【数２５】
0004982410.tif 000026
【０１００】
関数Ｊの最大値は関数Ｌの最小値であるので、関数Ｌの最小値を関数Ｌの極値を求めることで推定する。
【０１０１】
最初に、ｐ＝（ｐ１，ｐ２）のｐ１の求め方について説明する。
【０１０２】
まず、関数Ｌの極値を求めるには、関数Ｌをｐ１で偏微分（∇）したものが０となるので、
【数２６】
0004982410.tif 000027
【０１０３】
が０となればよい。
【０１０４】
そこで、式（８１−３）の∇Ｊについて計算すると、
【数２７】
0004982410.tif 000028
【０１０５】
となる。
【０１０６】
式（８１−３）に式（８１−４）を代入し、∇Ｌ＝０の下で式を整理すると、
【数２８】
0004982410.tif 000029
【０１０７】
となる。よって、式（８１−５）からｐ１を求めると、式（８１−２）の関数Ｌを最小にするｐ１を求めることができる。
【０１０８】
同様に、ｐ＝（ｐ１，ｐ２）のｐ２についても計算すると、
【数２９】
0004982410.tif 000030
【０１０９】
となる。
【０１１０】
続いて、ｓに関しても同様の手順で計算すると、
【数３０】
0004982410.tif 000031
【０１１１】
となる。ｓに関しては３次方程式となっているため、ｓを求めるために例えばカルダノの公式などを用い、０以上の実数をｓの解として選択すればよい。これは、正の実数解は必ず１つしかないからである。
【０１１２】
なお、式（８１−５）、式（８１−６）、式（８１−７）の右辺にあるｐ及びｓは、移動後の物体の重心を算出する際に利用した値を用いる。
【０１１３】
（９）ステップ９１
出力部１７は、空間移動量算出部１６のステップ８１によって求められたｐ＝（ｐ１，ｐ２）及びｓを用いて物体１０１の移動後の仮の重心を更新する。
【０１１４】
具体的には、ｐ及びｓをステップ８１で求められた値にそれぞれ更新する。
【０１１５】
また、式（５１−２）は、ｐ＝（ｐ１，ｐ２）及びｓが求められたとき、空間移動量であるΔＸの関係について表している式であるため、式（５１−２）からΔＸを求める。
【０１１６】
（１０）効果
本実施形態によれば、物体の特徴点の画像位置とその視差と特徴量を全て考慮することにより、空間位置成分にノイズが多く含まれる場合にも安定かつ高速に空間移動量を算出できる。
【０１１７】
（第３の実施形態）
第２の実施形態の空間移動量算出装置について図８と図９に基づいて説明する。
【０１１８】
図８は、本実施形態を係わる空間移動量算出装置の構成を示すブロック図であり、図９は、そのフローチャートである。
【０１１９】
（１）空間移動量算出装置の構成
図８は、本実施形態を係わる空間移動量算出装置の構成を示すブロック図である。
【０１２０】
空間移動量算出装置は、特徴点群１の画像位置と空間位置と特徴量を入力する入力部１０と、特徴点群２の画像位置と空間位置と特徴量を入力する入力部１１と、入力された特徴点群１と特徴点群２の間の画像位置関係から第１の類似度を算出する画像位置関係算出部１２と、空間位置関係から第２の類似度を算出する空間位置関係算出部１３と、特徴量関係から第３の類似度を算出する特徴量関係算出部１４と、第１の類似度〜第３の類似度を統合する統合部１５と、統合部１５から空間移動量を推定する空間移動量算出部１６と、空間移動量の妥当性を判断する判定部１８と、妥当でない場合に空間移動量から求まる重心を更新する空間移動量更新部１８と、空間移動量を出力する出力部１７とを備えている。
【０１２１】
ここでは、第１の実施形態である空間位置に空間座標上の位置を用いた場合の実施形態について説明する。
【０１２２】
（２）ステップ１２〜９２
ステップ１２〜９２は、ステップ１０〜８０と同じであるが、下記の追加項目がある。
【０１２３】
第１の追加項目は、ステップ１２に空間移動量の繰り返し回数を保存する変数Ｃを用意し、Ｃ＝０としておく。
【０１２４】
第２の追加項目は、ステップ９２に空間移動量を算出する毎に繰り返し回数Ｃに１を足す。
【０１２５】
（３）ステップ１０２
判定部１８は、ステップ８２で算出されたｐ＝（ｐ１，ｐ２）及びｓについて、算出結果が妥当であるかの判定を行う。例えば、以下の条件が考えられる。
【０１２６】
第１の条件は、ステップ９２で更新された繰り返し回数Ｃが期待する回数以上になったかどうかである。
【０１２７】
第２の条件は、前回（すなわち、Ｃ−１回目）算出されたｐ＝（ｐ１，ｐ２）及びｓと今回（Ｃ回目に）算出されたｐ＝（ｐ１，ｐ２）及びｓの差のノルムが期待する値より小さいかどうかである。
【０１２８】
第３の条件は、式（７０−２）が期待する値を超えたかどうかである。
【０１２９】
第４の条件は、算出されたｐ＝（ｐ１，ｐ２）及びｓが期待する範囲に収まったかどうかである。
【０１３０】
なお、これらの条件を単独で採用してもよいし、複数組み合わせてもよい。
【０１３１】
判定部１８の判定により、仮に定めた重心が正確でないと判定された場合にはステップ４２へ戻る。
【０１３２】
前回の算出で定めた空間移動量に基づいて、重心の位置を再計算して、ステップ４２〜ステップ１０２を繰り返し行うことによって、空間移動量をより精度よく算出することが可能となる。
【０１３３】
（４）効果
本実施形態によれば、重心の更新を行うことにより、空間位置成分にノイズが多く含まれる場合にもより安定に空間移動量を算出できる。
【０１３４】
（変更例）
なお、本発明は上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態にわたる構成要素を適宜組み合わせてもよい。
【０１３５】
（１）変更例１
第３の実施形態の更新において、第２の実施形態を採用してもよい。
【０１３６】
（２）変更例２
上記各実施形態では、重心を用いたが、これに限らず、特徴点群から定められる特定の位置を用いても良い。
【図面の簡単な説明】
【０１３７】
【図１】本発明の第１の実施形態の空間移動量算出装置を示すブロック図である。
【図２】同じく空間移動量算出装置の動作を示すフローチャートである。
【図３】空間座標系及び画像座標系の説明図である。
【図４】特徴点群１と特徴点群２の説明図である。
【図５】移動量ｐに限定した場合の類似度を説明図である。
【図６】第２の実施形態の空間移動量算出装置の動作を示すフローチャートである。
【図７】空間座標系及び画像座標系及び視差の説明図である。
【図８】第３の実施形態の空間移動量算出装置を示すブロック図である。
【図９】同じく空間移動量算出装置の動作を示すフローチャートである。
【符号の説明】
【０１３８】
１０    入力部
１１    入力部
１２    画像位置関係算出部
１３    空間位置関係算出部
１４    特徴量関係算出部
１５    統合部
１６    空間移動量算出部
１７    空間移動量出力部
１８    判定部
１９    空間移動量更新部
【図１】
0004982410.tif 000032
【図２】
0004982410.tif 000033
【図３】
0004982410.tif 000034
【図４】
0004982410.tif 000035
【図６】
0004982410.tif 000036
【図７】
0004982410.tif 000037
【図８】
0004982410.tif 000038
【図９】
0004982410.tif 000039
【図５】
0004982410.tif 000040