(57)【要約】【課題】自装置の運動を安定に算出することができる運動算出装置及び運動算出方法を提供する。【解決手段】運動算出装置１０が、平面を含む範囲を撮像し、撮像した画像を出力する撮像部１１と、平面が撮像されている領域を、画像から抽出する抽出部１３と、撮像部１１が所定の時間間隔で撮像した複数の画像から、特徴点と当該特徴点の移動ベクトルとを検出する追尾部１２と、特徴点についてのエピポーラ拘束と、第１領域についてのホモグラフィと、の両方に基づいて、自装置の運動を算出する算出部１４と、を備える。【選択図】図１
2011146017.tif 000002
【特許請求の範囲】
【請求項１】
平面を含む範囲を撮像し、撮像した画像を出力する撮像部と、
前記平面が撮像されている領域を、前記画像から抽出する抽出部と、
前記撮像部が所定の時間間隔で撮像した複数の前記画像から、特徴点と当該特徴点の移動ベクトルとを検出する検出部と、
前記特徴点についてのエピポーラ拘束と、前記領域についてのホモグラフィと、の両方に基づいて、自装置の運動を算出する算出部と、
を備えることを特徴とする運動算出装置。
【請求項２】
前記算出部は、前記エピポーラ拘束及び前記ホモグラフィに基づくコスト関数を最小化することにより、前記運動を算出することを特徴とする請求項１に記載の運動算出装置。
【請求項３】
自装置の運動を算出する運動算出装置における運動算出方法であって、
撮像部が、平面を含む範囲を撮像し、撮像した画像を出力する過程と、
抽出部が、前記平面が撮像されている領域を、前記画像から抽出する過程と、
検出部が、前記撮像部が所定の時間間隔で撮像した複数の前記画像から、特徴点と当該特徴点の移動ベクトルとを検出する過程と、
算出部が、前記特徴点についてのエピポーラ拘束と、前記領域についてのホモグラフィと、の両方に基づいて、自装置の運動を算出する過程と、
を含むことを特徴とする運動算出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像された画像に基づいて、自装置の運動を算出する運動算出装置及び運動算出方法に関する。
【背景技術】
【０００２】
画像撮像手段が撮像した画像についてのエピポーラ拘束に基づいて、その画像撮像手段を備えた移動体の運動を計測する運動計測装置が、特許文献１に開示されている。
【０００３】
また、カメラが撮像した画像についてのホモグラフィに基づいて、そのカメラを備えた輸送手段の運動（エゴモーション）を予測するシステムが、特許文献２に開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００６−３５０８９７号公報
【特許文献２】特表２００３−５１５８２７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、撮像部が撮像した画像において、例えば、静止している物体に対応する特徴点が少ない場合、特許文献１に開示された運動計測装置は、その特徴点についてのエピポーラ拘束に基づいて、自装置の運動を算出することができないという問題があった。
【０００６】
また、撮像部が撮像した画像において、例えば、平面が撮像されている領域にテクスチャが不十分な場合、特許文献２に開示されたシステムは、その平面についてのホモグラフィに基づいて、自装置の運動を算出することができないという問題があった。
【０００７】
本発明は、前記の諸点に鑑みてなされたものであり、自装置の運動を安定に算出することができる運動算出装置及び運動算出方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明は、上記の課題を解決するためになされたものであり、平面を含む範囲を撮像し、撮像した画像を出力する撮像部と、前記平面が撮像されている領域を、前記画像から抽出する抽出部と、前記撮像部が所定の時間間隔で撮像した複数の前記画像から、特徴点と当該特徴点の移動ベクトルとを検出する検出部と、前記特徴点についてのエピポーラ拘束と、前記領域についてのホモグラフィと、の両方に基づいて、自装置の運動を算出する算出部と、を備えることを特徴とする運動算出装置である。
【０００９】
前記算出部は、前記エピポーラ拘束及び前記ホモグラフィに基づくコスト関数を最小化することにより、前記運動を算出することを特徴とする運動算出装置である。
【００１０】
自装置の運動を算出する運動算出装置における運動算出方法であって、撮像部が、平面を含む範囲を撮像し、撮像した画像を出力する過程と、抽出部が、前記平面が撮像されている領域を、前記画像から抽出する過程と、検出部が、前記撮像部が所定の時間間隔で撮像した複数の前記画像から、特徴点と当該特徴点の移動ベクトルとを検出する過程と、算出部が、前記特徴点についてのエピポーラ拘束と、前記領域についてのホモグラフィと、の両方に基づいて、自装置の運動を算出する過程と、を含むことを特徴とする運動算出方法である。
【発明の効果】
【００１１】
本発明によれば、運動算出装置は、画像における特徴点についてのエピポーラ拘束と、平面が撮像されている領域についてのホモグラフィと、の両方に基づいて、自装置の運動を算出する。これにより、運動算出装置は、エピポーラ拘束及びホモグラフィのいずれか一方のみに基づいて自装置の運動を算出する場合と比較して、自装置の運動を安定に算出することができる。
【図面の簡単な説明】
【００１２】
【図１】一実施形態における、カメラシステムの構成を示すブロック図である。
【図２】一実施形態における、運動算出装置の動作手順を示すフローチャートである。
【発明を実施するための形態】
【００１３】
［第１実施形態］
本発明の第１実施形態について図面を参照して詳細に説明する。図１には、カメラシステムの構成がブロック図で示されている。カメラシステムは、移動体に備えられる。移動体は、例えば、車両である。カメラシステムは、運動算出装置１０と、制御装置２０とを備える。
【００１４】
運動算出装置１０は、平面（例えば、地面）を含む範囲を撮像し、撮像した画像に基づいて、自装置の運動（以下、「カメラモーション」という）を算出する。また、運動算出装置１０は、算出したカメラモーションを制御装置２０に出力する。
【００１５】
運動算出装置１０は、撮像部１１と、追尾部（検出部）１２と、抽出部１３と、算出部１４とを備える。撮像部１１は、受光素子を備え、その受光面に複数の画素を備える。受光面には、光学系によって、光学像が結像される。撮像部１１は、平面を含む範囲を所定周期で撮像し、撮像した画像を追尾部１２及び抽出部１３に出力する。以下、時刻ｔに撮像部１１が撮像した画像Ｉを、画像Ｉｔと表記する。また、以下、撮像部１１の主点を原点とする座標系であって、光軸方向をＺ軸とする座標系を「カメラ座標系」という。また、カメラ座標系の座標（ｘｐ，ｙｐ）に入射した光の方向を、方向ベクトルｐ＝［ｘ，ｙ，１］Ｔと表記する（上付きＴは、転置を意味する）。また、画像Ｉｔを構成する点ｐの画素値を、方向ベクトルｐを用いて、Ｉｔ（ｐ）と表記する。なお、画素値は、周辺画素の画素値から補間されることで算出されてもよい。また、撮像部１１は、事前にキャリブレーションされているものとする。
【００１６】
追尾部１２には、撮像部１１が所定周期で撮像した画像Ｉｔが入力される。追尾部１２は、ハリスオペレータ等を用い、画像Ｉｔから特徴点を検出する。また、追尾部１２は、その特徴点の移動ベクトルを検出する。以下、追尾部１２が抽出した特徴点の集合を、集合Ｓｔと表記する。追尾部１２は、時刻ｔ−１に撮像された画像Ｉｔ−１に含まれる特徴点の集合Ｓｔー１を構成するｋ番目の特徴点ｐｋについて、画像Ｉｔに撮像された観測シーン（例えば、被写体）における同一位置に対応する対応点ｑｋを、Ｌｕｃａｓ−Ｋａｎａｄｅ法などを用いて検出する。以下、特徴点対応（ｐｋ，ｑｋ）の集合を、Ψｔと表記する。
【００１７】
また、追尾部１２は、ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）等を用い、静的背景（例えば、撮像された地面又は建造物）に含まれる特徴点についてのエピポーラ拘束に従わない特徴点対応（ｐｋ，ｑｋ）を、Ψｔから除外する。また、追尾部１２は、特徴点対応の集合Ψｔを算出部１４に出力する。追尾部１２は、これらの処理を画像Ｉｔ毎に実行することで、画像内で特徴点を追尾する。
【００１８】
算出部１４には、特徴点対応の集合Ψｔが追尾部１２から入力される。また、算出部１４には、画像Ｉｔにおいて平面が撮像されている領域（以下、「平面領域」という）が、抽出部１３から入力される。ここで、平面領域は、画像Ｉｔが分割された複数のブロックの集合Πｔ＝｛Ｂｉ｜ｉ＝１…ｍ｝により表現されるものとする。なお、集合Πｔの初期値は、予め定められているものとする。例えば、運動算出装置１０が車両に備えられている場合、平面領域Πｔの初期値は、画像Ｉｔにおいて地面が撮像されている頻度が高い領域（例えば、画像の下半分）のブロック｛Ｂｉ｜ｉ＝１…ｍ｝に定められる。
【００１９】
また、算出部１４は、所定の位置を原点とするＸ−Ｙ−Ｚ座標系における平面ｎｘＸ＋ｎｙＹ＋ｎｚＺ＝１を示す３次元パラメータのベクトルｎ＝［ｎｘ，ｎｙ，ｎｚ］Ｔを記憶する。ここで、ベクトルｎの初期値ｎ０は、移動体が接地する平面に対する運動算出装置１０の取り付け姿勢に基づいて、予め定められる。例えば、初期値ｎ０は、車両が接地する地面に対する運動算出装置１０の取り付け姿勢に基づいて、予め定められる。
【００２０】
算出部１４は、特徴点についてのエピポーラ拘束と、平面領域についてのホモグラフィと、の両方に基づいて、カメラモーション、及び自装置に対する平面の姿勢（以下、「平面姿勢」という）を算出する。具体的には、算出部１４は、特徴点対応の集合Ψｔと平面領域Πｔとの両方に基づくコスト関数を最小化することにより、カメラモーション及び平面姿勢を示すパラメータμ＝［ωＴ，ｔＴ，ｎＴ］を算出する。ここで、カメラモーションは、回転パラメータωＴ＝［ωｘ，ωｙ，ωｚ］と、並進ベクトルｔＴ＝［ｔｘ，ｔｙ，ｔｚ］とから成る。また、平面姿勢は、ｎＴ＝［ｎｘ，ｎｙ，ｎｚ］で表される。
【００２１】
また、コスト関数は、式（１）で表される。
【００２２】
【数１】
2011146017.tif 000003
【００２３】
式（１）において、［ｔ］ｘは、３×３行列で表されるベクトルｔの外積オペレータである。また、Ｒ（ω）は、回転パラメータωにより定まる回転行列である。また、第１項のワープ関数Ｗは、平面領域についてのホモグラフィ変換を示し、カメラモーション及び平面姿勢を示すパラメータμに応じて定まる関数である。また、第１項は、ホモグラフィ拘束項と呼ばれ、画像Ｉｔ−１の平面領域から画像Ｉｔの平面領域へのホモグラフィ変換における誤差を表す。
【００２４】
ここで、運動算出装置１０が運動したことにより、画像Ｉｔ−１の平面領域における特徴点ｐが、画像Ｉｔの平面領域における特徴点ｑに移動したとする。この場合、特徴点ｐと特徴点ｑとには、式（２）に示す関係が成り立つ。
【００２５】
【数２】
2011146017.tif 000004
【００２６】
式（２）において、記号「〜」は、両辺のベクトルが、平行であり且つスケール以外が等しいことを表す。また、ベクトルｐ及びベクトルｎは、時刻ｔ−１におけるカメラ座標系により表される。一方、ベクトルｑ及びベクトルｔは、時刻ｔにおけるカメラ座標系により表される。
【００２７】
ここで、３×３行列であるＨ＝Ｒ（ω）−ｔｎＴを考える。行列Ｈの行ベクトルを、それぞれｈ１、ｈ２、ｈ３と表記すれば、特徴点ｐ＝［ｘ，ｙ，１］Ｔから対応点ｑ＝［ｘ’，ｙ’，１］Ｔへの変換を表すワープ関数Ｗは、式（３）で表される。
【００２８】
【数３】
2011146017.tif 000005
【００２９】
また、式（１）の第２項は、エピポーラ拘束項と呼ばれ、エピポーラ拘束を表す。座標ｐから座標ｑに特徴点が移動した場合、エピポーラ拘束により、式（４）が満たされる。
【００３０】
【数４】
2011146017.tif 000006
【００３１】
一般に、パラメータの誤差、ベクトルｐ、及びベクトルｑの観測誤差により、左辺は値「０」とはならないので、式（４）の左辺は、エピポーラ拘束に対する代数的誤差と考えられる。よって、式（１）の第２項は、エピポーラ拘束に関する二乗誤差の和を表す。
【００３２】
また、式（１）の第３項は、並進ベクトルの大きさをｖとする正則化項である。ここで、式（１）のαｐ、βｐ、及びλは、重み係数である。式（１）の第３項は、並進ベクトルｔのスケール曖昧性を解決するために、式（１）に加えられている。例えば、移動体の速度情報が得られない場合には、ｖ＝１としてもよい。この場合、運動算出装置１０から平面までの推定距離、及び運動算出装置１０から各特徴点までの推定距離は、並進ベクトルｔのノルムに比例する値となる。
【００３３】
算出部１４は、コスト関数を最小化するため、変量δを用いて、パラメータμの推定値を改善する。算出部１４は、例えば、ガウス・ニュートン法により、コスト関数の値を最小化する。
【００３４】
式（１）の各二乗誤差の内部を、パラメータμの変量δについて一次近似すると、式（５）が成り立つ。ここで、ｒｗ、ｒｅ及びｒｖは、それぞれ式（５）の各項のパラメータμについての残差である。また、ｒｗは、式（６）で表され、ｒｅは、式（７）で表され、ｒｖは、式（８）で表される。
【００３５】
【数５】
2011146017.tif 000007
【００３６】
また、式（９）と置けば、特徴点ｐでのヤコビアンＪｗは、現在のパラメータ推定値μ＝［ωＴ，ｔＴ，ｎＴ］を用いて、式（１０）で表される。
【００３７】
【数６】
2011146017.tif 000008
【００３８】
【数７】
2011146017.tif 000009
【００３９】
式（５）の第２項の微分に現れるＪｅは、式（１１）で表される。ここで、式（１２）が成り立つ。また、式（５）の第３項の微分に現れるＪｖは、式（１３）で表される。
【００４０】
【数８】
2011146017.tif 000010
【００４１】
式（５）のδについての微分を値「０」と置くと、式（１４）が成り立つ。
【００４２】
【数９】
2011146017.tif 000011
【００４３】
δについて整理すると、式（１４）は、式（１５）で表される。
【００４４】
【数１０】
2011146017.tif 000012
【００４５】
式（１５）の左辺は、９×９行列と９次元パラメータベクトルδとの積である。また、式（１５）の右辺は、９次元ベクトルである。このように、式（１５）は、線形連立方程式となっているため、δについて解くことができる。このδは、パラメータ推定値μの最適値の近似値（μ＋δ）を与える。
【００４６】
このように、算出部１４は、コスト関数の値が改善しなくなるまで、パラメータの推定値μの更新を繰り返し、コスト関数の値が改善しなくなった場合に、解が収束したと判定する。また、解が収束した場合、算出部１４は、パラメータの推定値μ＝［ωＴ，ｔＴ，ｎＴ］を抽出部１３に出力する。
【００４７】
重み係数αｐ、βｐ、及びλは、経験的に定められた値でもよいが、次のように定められてもよい。式（１）の各項が二乗誤差最小化の形であるため、式（１）に表されたコスト関数は、各誤差ｒｗ、ｒｅ及びｒｖについての確率分布に基づく負の対数尤度を表す式（Ｉ）の形になっていると捉えることができる。
【００４８】
【数１１】
2011146017.tif 000013
【００４９】
式（Ｉ）において、記法ｐ（ｒ｜Ａ）は、条件Ａの下で、観測値ｒが発生する確率密度を表す。また、式（Ｉ）の第１項の添え字ｐは、画像に含まれる点ｐ毎の確率密度関数であることを表す。また、式（Ｉ）の第２項のｐの添え字ｐ及びｑは、特徴点対応（ｐ，ｑ）毎の確率分布関数であることを表す。
【００５０】
各項の確率分布を正規分布と仮定すると、式（Ｉ）は、式（ＩＩ）で表される。
【００５１】
【数１２】
2011146017.tif 000014
【００５２】
式（ＩＩ）において、記法Ａ｜ｐは、画像に含まれる点ｐについて、式Ａを評価することを表す。また、記法Ａ｜（ｐ，ｑ）は、画像に含まれる特徴点対応（ｐ，ｑ）について、式Ａを評価することを表す。また、σｗは、ホモグラフィ変換についての誤差の標準偏差である。また、σｅは、エピポーラ拘束についての誤差の標準偏差である。また、σｖは、速度についての誤差の標準偏差である。なお、定数項Ｃは、式（ＩＩ）の最小化については無関係であるため、無視してよい。
【００５３】
式（ＩＩ）を最小化すれば、観測値の尤度を最大化するパラメータが推定される。ゆえに、ノイズ標準偏差σｗ、σｅ、σｖが与えられた場合、式（１）のα、β、γは、それぞれσｗ、σｅ、σｖの二乗の逆数とするのが、最尤値を推定する意味で最適である。
【００５４】
このように、画像における輝度、勾配、及び特徴点の位置などの情報とノイズ標準偏差との関係を近似したモデル（式（ＩＩ））を用いて、重み係数αｐ、βｐ、及びλを定めることにより、重み係数を経験的に定めた場合と比較して、カメラモーション及び平面姿勢を推定する性能の改善が期待できる。
【００５５】
図１に戻り、構成の説明を続ける。抽出部１３には、算出部１４が算出したパラメータの推定値μが入力される。また、抽出部１３には、撮像部１１が所定周期で撮像した画像Ｉｔが入力される。抽出部１３は、平面が撮像されている領域を画像Ｉｔから抽出するため、予め定められた大きさの複数のブロックに画像Ｉｔを分割する。また、抽出部１３は、平面姿勢のパラメータ（ベクトル）ｎについての推定を各ブロックに対して実行し、平面姿勢と一致するブロックの集合を、平面領域として抽出する。
【００５６】
また、式（１）において平面姿勢に関係する唯一の項である第１項を、平面姿勢についてのコスト関数として採用する。平面姿勢についてのコスト関数は、式（１６）で表される。
【００５７】
【数１３】
2011146017.tif 000015
【００５８】
ブロックＢの平面姿勢のパラメータｎＢについて、式（１６）のコスト関数の値を最小化する。ここで、パラメータｎＢは、式（１）の場合と同様に、式（１５）の両辺のそれぞれにおいて平面姿勢に関係する第１項についての連立方程式（１７）を解くことで更新される。
【００５９】
【数１４】
2011146017.tif 000016
【００６０】
式（１７）において、Ｊ’ｗは、平面姿勢のパラメータｎに関する３成分のみを持つベクトルである。また、δ’は、平面姿勢のパラメータｎに関する３成分を更新するための３次元ベクトルである。すなわち、Ｊ’ｗは、式（１０）に含まれている３成分であり、式（１８）で表される。
【００６１】
【数１５】
2011146017.tif 000017
【００６２】
式（１７）の左辺は、３×３正方行列と３次元ベクトルδ’との積である。また、式（１７）の右辺は、３次元ベクトルである。したがって、線形連立方程式の解として、δ’が算出される。
【００６３】
抽出部１３は、ブロックＢの平面姿勢のパラメータｎＢと、平面領域全体の平面姿勢のパラメータｎとの差を算出する。平面姿勢のパラメータｎＢと平面姿勢のパラメータｎとの差が、所定の閾値以下である場合、抽出部１３は、そのブロックＢを平面領域に属するブロックであると判定し、そのブロックＢをブロックの集合Πｔに含めることで、集合Πｔを更新する。ここで、所定の閾値とは、例えば、パラメータｎＢとパラメータｎとのユークリッド距離｜ｎＢ−ｎ｜である。また、抽出部１３は、更新した集合Πｔを算出部１４に出力する。
【００６４】
算出部１４は、抽出部１３が更新した集合Πｔに基づいて、カメラモーション及び平面姿勢に含まれる９つのパラメータを算出する。ここで、算出部１４は、式（１）に表されたコスト関数の値が改善しなくなることを停止条件として、この停止条件が成立するまで、カメラモーション及び平面姿勢に含まれる９つのパラメータの算出を繰り返す。また、算出部１４は、最終的に算出したカメラモーション及び平面姿勢を、制御装置２０に出力する。
【００６５】
ここで、平面領域を表すブロックの数が少なくなるに従い、式（１）に表されたコスト関数の値は小さくなる。また、平面領域を表すブロックの数が少なくなるに従い、カメラモーション及び平面姿勢を推定する精度は低くなる。そこで、カメラモーション及び平面姿勢を算出する処理を停止させるための停止条件には、式（１）の第１項をブロック数で正規化した式（１９）を用いるとよい。
【００６６】
【数１６】
2011146017.tif 000018
【００６７】
制御装置２０には、カメラモーション及び平面姿勢が、運動算出装置１０の算出部１４から入力される。制御装置２０は、カメラモーション及び平面姿勢に基づいて、所定の処理を実行する。ここで、所定の処理とは、例えば、カメラモーションに基づいて、特徴点の３次元位置を三角測量により算出し、算出した特徴点の塊が、平面姿勢に基づく平面領域以外に位置する塊である場合には、その塊を障害物として画面（不図示）に表示する処理である。また、例えば、所定の処理とは、平面領域以外に位置する塊を回避するよう、移動体に回避指示を出す処理であってもよい。この場合、所定の処理とは、カメラシステム（運動算出装置１０及び制御装置２０）を備えた車両に、前方を走行している他の車両を回避するよう回避指示を出す処理であってもよい。
【００６８】
次に、運動算出装置１０の動作手順を説明する。
図２は、一実施形態における、運動算出装置の動作手順を示すフローチャートである。算出部１４は、カメラモーション及び平面姿勢の推定値μを初期化する（ステップＳ１）。そして、撮像部１１は、平面を含む範囲を撮像する（ステップＳ２）。また、追尾部１２は、画像内で特徴点ｐを追尾する。さらに、追尾部１２は、特徴点対応の集合Ψｔを算出部１４に出力する（ステップＳ３）。また、算出部１４は、カメラモーション及び平面姿勢の推定値μを算出する（ステップＳ４）。
【００６９】
抽出部１３は、平面領域を抽出する（ステップＳ５）。そして、算出部１４は、カメラモーション及び平面姿勢の推定値μを算出する（ステップＳ６）。また、算出部１４は、解が収束したか否かを判定する（ステップＳ７）。解が収束していない場合（ステップＳ７−Ｎｏ）、算出部１４の動作は、ステップＳ５に戻る。一方、解が収束した場合（ステップＳ７−Ｙｅｓ）、算出部１４は、算出したカメラモーション及び平面姿勢を、制御装置２０に出力する。そして、制御装置２０は、所定の処理（シーン解析、表示、警告、制御等）を実行する（ステップＳ７）。
【００７０】
以上のように、運動算出装置１０は、平面を含む範囲を撮像し、撮像した画像を出力する撮像部１１と、平面が撮像されている領域を、画像から抽出する抽出部１３と、撮像部１１が所定の時間間隔で撮像した複数の画像から、特徴点と当該特徴点の移動ベクトルとを検出する追尾部１２と、特徴点についてのエピポーラ拘束と、領域についてのホモグラフィと、の両方に基づいて、自装置の運動（カメラモーション）を算出する算出部１４と、を備える。
【００７１】
これにより、運動算出装置は、エピポーラ拘束及びホモグラフィのいずれか一方のみに基づいてカメラモーションを算出する場合と比較して、カメラモーションを安定に算出することができる。
【００７２】
また、算出部１４は、エピポーラ拘束及びホモグラフィに基づくコスト関数を最小化することにより、自装置の運動を算出する。これにより、運動算出装置は、コスト関数に基づいて、カメラモーションを安定に算出することができる。
【００７３】
［第２実施形態］
本発明の第２実施形態について図面を参照して詳細に説明する。第２実施形態では、コスト関数の正則項が、第１実施形態と異なる。以下では、第１実施形態との相違点についてのみ説明する。
【００７４】
撮像部１１（図１を参照）の主点から平面までの距離ｄが、ほぼ一定であると想定される場合、式（１）の正則項（第３項）に自装置の速度情報（並進ベクトルの大きさ）ｖを含めなくてよい。撮像部１１の主点から、ｎＴＰ＝１で表される平面までの距離ｄは、ｄ＝−１／｜ｎ｜という関係により、｜ｎ｜２＝ｄ−２が成り立つ。したがって、撮像部１１の主点から平面までの距離ｄが、ほぼ一定であると想定される場合、コスト関数は、式（２０）で表される。
【００７５】
【数１７】
2011146017.tif 000019
【００７６】
また、算出部１４は、式（８）の代わりに式（２１）を用いて、コスト関数の値を最小化する。
【００７７】
【数１８】
2011146017.tif 000020
【００７８】
また、算出部１４は、式（１３）の代わりに式（２２）を用いて、コスト関数の値を最小化する。
【００７９】
以上のように、算出部１４は、自装置の速度情報ｖを含まないコスト関数を最小化することにより、自装置の運動を算出する。これにより、運動算出装置は、速度情報ｖを用いずに、カメラモーションを安定に算出することができる。
【００８０】
以上、この発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計等も含まれる。
【００８１】
なお、以上に説明した運動算出装置を実現するためのプログラムを、コンピュータ読み取り可能な記録媒体に記録し、そのプログラムをコンピュータシステムに読み込ませて実行するようにしてもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（ＲＡＭ）のように、一定時間プログラムを保持しているものも含むものとする。また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
【符号の説明】
【００８２】
１０…運動算出装置、１１…撮像部、１２…追尾部（検出部）、１３…抽出部、１４…算出部、２０…制御装置
【図１】
2011146017.tif 000021
【図２】
2011146017.tif 000022