(57)【特許請求の範囲】
【請求項１】
時系列映像を撮影する撮影部、
前記時系列映像の各フレームのうちで撮影順の早いものから順に、各フレームに含まれている特徴点を抽出する一方で、同一フレーム内で先に抽出されていた特徴点からの該同一フレーム上での距離が所定距離に満たない点については該抽出の対象から除外する抽出部、
前記時系列映像のうちの時系列で連続している２フレームにおいての前のフレームから前記抽出部によって前記抽出の対象から除外されずに抽出された特徴点と該２フレームにおいての後のフレームにおける該特徴点についての対応点との対応関係を前記時系列映像の各フレームについて順次求めることによって、該特徴点を前記時系列映像上で追跡する追跡部、
前記撮影部の移動量を検出する検出部、
前記移動量が所定量を超える度に、該所定量を超えた直後に撮影された前記時系列映像のフレームであるキーフレームに含まれている特徴点についての該キーフレーム上での位置情報と、該特徴点についての前記追跡部による追跡結果を表している追跡情報との登録を行う登録部、及び
前記移動量と、前記登録部により登録された、前記特徴点についての複数の前記キーフレームの各々での前記位置情報及び該特徴点についての前記追跡情報とを用いて、該特徴点の三次元位置の座標を算出する算出部、
を備え、
前記追跡部は、前記前のフレームから前記抽出部によって抽出された特徴点についての前記後のフレームにおける対応点についての、該後のフレームから前記抽出部によって抽出されていた特徴点からのフレーム上での距離が所定距離に満たなかった場合には、該後のフレームから抽出されていた特徴点を前記抽出部による抽出結果から除外し、除外しなかった特徴点について前記追跡を行う、
ことを特徴とする三次元座標測定装置。
【請求項２】
前記撮影部は移動体に固定されており、
前記検出部は、前記移動体の回転量と並進量とを検出するセンサからの検出出力を用いて、前記移動量として、前記撮影部の回転量と並進量との検出を行い、
前記登録部は、前記撮影部の並進量が所定量を超える度に前記キーフレームについての前記登録を行う、
ことを特徴とする請求項１に記載の三次元座標測定装置。
【請求項３】
前記登録部は、所定数のキーフレームについての情報の登録が可能であり、
前記算出部は、前記登録部に登録されている、前記所定数のキーフレームの各々についての情報を用いて、前記特徴点の三次元位置の座標を算出し、
前記登録部は、前記所定数のキーフレームについての情報の登録が既に行われた後に前記移動量が所定量を超えた場合には、最古に登録されたキーフレームについての情報の登録を削除して、該所定量を超えた直後に撮影されたキーフレームについての情報を代わりに登録する、
ことを特徴とする請求項１又は２に記載の三次元座標測定装置。
【請求項４】
撮影部により撮影された時系列映像の各フレームのうちで撮影順の早いものから順に、各フレームに含まれている特徴点を抽出する一方で、同一フレーム内で先に抽出されていた特徴点からの該同一フレーム上での距離が所定距離に満たない点については該抽出の対象から除外し、
前記時系列映像のうちの時系列で連続している２フレームにおいての前のフレームから前記抽出の対象から除外されずに抽出された特徴点と該２フレームにおいての後のフレームにおける該特徴点についての対応点との対応関係を前記時系列映像の各フレームについて順次求めることによって、該特徴点を前記時系列映像上で追跡し、
前記撮影部の移動量を検出し、
前記移動量が所定量を超える度に、該所定量を超えた直後に撮影された前記時系列映像のフレームであるキーフレームに含まれている特徴点についての該キーフレーム上での位置情報と、該特徴点についての前記追跡の結果を表している追跡情報との登録を行い、
前記移動量と、登録されている、前記特徴点についての複数の前記キーフレームの各々での前記位置情報及び該特徴点についての前記追跡情報とを用いて、該特徴点の三次元位置の座標を算出し、
前記特徴点の追跡では、前記前のフレームから前記特徴点の抽出により抽出された特徴点についての前記後のフレームにおける対応点についての、該後のフレームから前記特徴点の抽出により抽出されていた特徴点からのフレーム上での距離が所定距離に満たなかった場合には、該後のフレームから抽出されていた特徴点を前記特徴点の抽出による抽出結果から除外し、除外しなかった特徴点について前記追跡を行う、
ことを特徴とする三次元座標測定方法。
【請求項５】
前記移動量の検出では、前記撮影部が固定されている移動体の回転量と並進量とを検出するセンサからの検出出力を用いて、前記移動量として、前記撮影部の回転量と並進量との検出を行い、
前記位置情報と前記追跡情報との登録では、前記撮影部の並進量が所定量を超える度に前記キーフレームについての前記登録を行う、
ことを特徴とする請求項４に記載の三次元座標測定方法。
【請求項６】
前記位置情報と前記追跡情報との登録では、所定数のキーフレームについての情報の登録が可能であり、
前記三次元位置の座標の算出では、前記位置情報と前記追跡情報との登録によって登録されている、前記所定数のキーフレームの各々についての情報を用いて、前記特徴点の三次元位置の座標を算出し、
前記位置情報と前記追跡情報との登録において、前記所定数のキーフレームについての情報の登録が既に行われた後に前記移動量が所定量を超えた場合には、最古に登録されたキーフレームについての情報の登録を削除して、該所定量を超えた直後に撮影されたキーフレームについての情報を代わりに登録する、
ことを特徴とする請求項４又は５に記載の三次元座標測定方法。
【請求項７】
撮影部により撮影された時系列映像の各フレームのうちで撮影順の早いものから順に、各フレームに含まれている特徴点を抽出する一方で、同一フレーム内で先に抽出されていた特徴点からの該同一フレーム上での距離が所定距離に満たない点については該抽出の対象から除外し、
前記時系列映像のうちの時系列で連続している２フレームにおいての前のフレームから前記抽出の対象から除外されずに抽出された特徴点と該２フレームにおいての後のフレームにおける該特徴点についての対応点との対応関係を前記時系列映像の各フレームについて順次求めることによって、該特徴点を前記時系列映像上で追跡し、
前記撮影部の移動量を検出し、
前記移動量が所定量を超える度に、該所定量を超えた直後に撮影された前記時系列映像のフレームであるキーフレームに含まれている特徴点についての該キーフレーム上での位置情報と、該特徴点についての前記追跡の結果を表している追跡情報との登録を行い、
前記移動量と、登録されている、前記特徴点についての複数の前記キーフレームの各々での前記位置情報及び該特徴点についての前記追跡情報とを用いて、該特徴点の三次元位置の座標を算出する、
処理を演算処理装置に実行させ、
前記特徴点の追跡では、前記前のフレームから前記特徴点の抽出により抽出された特徴点についての前記後のフレームにおける対応点についての、該後のフレームから前記特徴点の抽出により抽出されていた特徴点からのフレーム上での距離が所定距離に満たなかった場合には、該後のフレームから抽出されていた特徴点を前記特徴点の抽出による抽出結果から除外し、除外しなかった特徴点について前記追跡を行う、
ことを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本明細書で議論される実施態様は、物体の位置を特定する技術に関するものである。
【背景技術】
【０００２】
車両等の移動体に取り付けたカメラで周囲の映像を撮影して得られた映像を用いて、周辺の物体の三次元位置を測定する装置が知られている。この装置は、移動体が時々刻々移動しながらカメラでの撮影が行われ、その撮影のタイミング（映像フレーム）毎に、周囲の物体の三次元位置の算出が行われる。このようにして撮影した映像から物体の三次元位置を測定する手法として、ＳＦＭ（Structure from Motion ）と呼ばれている方式と、移動ステレオ（Motion Stereo ）方式とが知られている。
【０００３】
ＳＦＭ方式は、移動体が移動しながら撮影された一連の時系列映像から、物体の複数の特徴的な部分（特徴点）の時系列での対応をまず求める。そして、それらの特徴点の対応関係を利用して、移動体の移動量（回転量及び並進量）と特徴点の三次元位置とを算出するというものである。
【０００４】
このＳＦＭ方式による特徴点の三次元位置の測定では、ある程度の枚数の時系列映像群が必要となる。言い換えれば、時系列映像がある程度の枚数撮影される間隔毎にしか、特徴点の三次元位置を測定することができない。このため、移動体が移動しながら、新たに１枚の映像を撮影する毎に周囲の物体の三次元位置を定めるという用途において単純にＳＦＭ方式を適用することはできない。但し、図１に示すように、新たな映像を撮影して取得する毎に、直近に取得した所定枚数（図１においては４枚）の時系列映像を用いるようにすれば、各撮影時刻での周囲の物体の三次元位置の算出は可能である。
【０００５】
一方、移動ステレオ方式は、カメラで得た時系列の映像を用い、時々刻々の映像の各々における物体の特徴点を、映像照合により追跡する。そして、参照する２つの撮影時刻の映像間での特徴点の映像内位置と、外部より与えられるその間の移動体の移動量（回転量及び並進量）を用いて、映像に映っている物体の三次元位置を、三角測量の原理で算出するというものである。この移動ステレオ方式は、特徴点の追跡の手法の違いにより、２種類に大別される。この２種類の移動ステレオ方式について、図２Ａ及び図２Ｂを参照しながら説明する。
【０００６】
この移動ステレオの第一の方式は、図２Ａに図解されているように、時系列の映像（時刻ｔ−５の映像）から特徴点を発見したときに、その特徴点が含まれていた映像から、当該特徴点の映像（特徴点を含む比較小領域の映像）を抽出して記録しておく。そして、それ以降の時系列映像の各々（時刻ｔ−４〜時刻ｔの各映像）に対して、この特徴点の映像（時刻ｔ−５の映像）との照合を行って、特徴点の時系列映像での対応関係を求めていくというものである。この手法では、ある映像から新たに取得した特徴点に対応する特徴点の特定を、当該映像に続く時系列映像の各々について順に行い、最初に取得した特徴点からの動きが生じるまで特徴点を追跡し続け、動きが生じた時点で、特徴点の三次元位置を求めるようにする。
【０００７】
また、この移動ステレオの第二の方式は、図２Ｂに図解されているように、時系列映像のうちのある時刻の映像から抽出した特徴点の映像と、時系列映像のうちの当該時刻に時間軸で隣り合っている映像である次時刻映像との照合を行う。そして、この照合を、時間軸で隣り合っている映像間で順次行うことで、特徴点の時系列映像間での対応関係を求めていくというものである。図２Ｂの例では、例えばある時刻の映像が時刻ｔ−５の映像である場合には次時刻映像は時刻ｔ−４の映像となり、ある時刻の映像が時刻ｔ−４の映像である場合には次時刻映像は時刻ｔ−３の映像となる。この手法では、着目した撮影時刻での映像における特徴点の位置と、予め決められた時間間隔に基づき定めた過去の撮影時刻での映像における特徴点の位置とを用いて、特徴点の三次元位置を定めるようにする。
【０００８】
ところで、映像に含まれている特徴点の三次元位置を定めるには、異なる位置から特徴点を撮影して得た映像が必要であり、例えば、カメラを取り付けた移動体が移動しながら当該カメラで映像を撮影する必要がある。このことについて説明する。
【０００９】
単眼カメラの動画像を用いた三次元測距は、基本的な考え方として、図３に示すような基本的な幾何学的な関係による。
図３は、移動体上に取り付けたカメラによって撮影して得られた映像に特徴点Ｐが含まれる場合を図解したものである。
【００１０】
図３においてＸ軸とＺ軸とは直交しており、移動体上で定義されている。移動体はＸ軸の正方向に移動し、カメラの撮影方向はＸ軸とＺ軸との交点（投影点）からＺ軸の正方向に向けられている。このカメラの焦点距離はｆであり、従って、図３においてはＺ＝ｆの位置に撮像面が配置されていると考えることができる。
【００１１】
位置が固定されている（すなわち移動しない）特徴点Ｐは、時刻「１」においてのＸ＝Ｘ１及びＺ＝Ｚ１の地点に位置しており、この時刻でカメラで撮影を行うと、特徴点Ｐは、撮像面上のＸ＝ｘ１の位置に投影される。また、時刻「２」においてカメラで撮影を行うと、特徴点Ｐは撮像面上のＸ＝ｘ２の位置に投影される。なお、時刻「１」から時刻「２」までの期間における移動体の移動量はｂである。
【００１２】
この図３においての幾何学的な関係より、
ｘ１＝ｆ×Ｘ１／Ｚ１
ｘ２＝ｆ×（Ｘ１−ｂ）／Ｚ１
が成立する。
【００１３】
ここで、時刻「１」と時刻「２」とにおける、カメラから見た特徴点Ｐの視差ｄは、
ｄ＝ｘ１−ｘ２＝ｆ×ｂ／Ｚ１
となる。よって、特徴点ＰのＺ軸方向の位置（すなわち、カメラから見た特徴点Ｐまでの奥行き）Ｚ１は、
Ｚ１＝ｆ×ｂ／ｄ
で表される。
【００１４】
ここで、奥行きＺ１の誤差について考える。
今、投影点での観測ずれによって視差ｄが１だけずれた値として観測された特徴点Ｐまでの奥行きをＺ２とする。このとき、距離のずれｄＺは、
ｄＺ＝Ｚ１−Ｚ２
＝ｆ×ｂ／ｄ−ｆ×ｂ／（ｄ＋１）
＝ｆ×ｂ／（ｄ×（ｄ＋１））
＝Ｚ１／（ｆ×ｂ／Ｚ１＋１）
＝（Ｚ１）2 ／（ｆ×ｂ＋Ｚ１）
となる。
【００１５】
よって、カメラ間の距離（すなわち、カメラを取り付けた移動体の移動量）ｂの値が大きいほど、誤差ｄＺが小さくなって奥行きの算出精度が高くなり、逆に、このｂの値が小さいほど、誤差ｄＺが大きくなって奥行きの算出精度が低下することが分かる。更には、特別な場合として、ｂ＝０の場合、すなわち移動体が移動しない場合には、ｄ＝０となってしまい、Ｚ１を算出できなくなる。このように、単眼カメラによる三次元位置の算出では、移動体の移動量がある程度大きいことが求められる。
【先行技術文献】
【特許文献】
【００１６】
【特許文献１】特開２００９−１８０５３６号公報
【非特許文献】
【００１７】
【非特許文献１】出口光一郎、「多視点からの透視投影画像の因子分解による３次元形状復元」、計測自動制御学会論文集、公益社団法人計測自動制御学会、平成１０年１０月、第３４巻、第１０号、ｐ．１３２１−１３２８
【発明の概要】
【発明が解決しようとする課題】
【００１８】
前述したように、単眼カメラによる三次元位置の算出では、移動体の移動量がある程度大きいことが求められる。従って、移動体の移動量が低下し、例えば時系列画像のＭフレーム分以上の撮影時間間隔に亘って移動体が停止していると、この停止区間を含むＭフレームの時系列映像群からでは、物体の三次元位置を定めることができない。ここで、例えば、このフレーム数Ｍの値を、移動体の停止期間よりも長い非常に大きな値とすれば、物体の三次元位置を定めること論理的には可能となる。しかしながら、現実の装置のハードウェアリソースを踏まえると、そのような多大なフレーム数を扱うのは現実的ではない。
【００１９】
この点に関し、前述した移動ステレオの第一の方式では、移動体が移動して映像内での特徴点の位置が変化するのを待ってから三次元位置を定めるようにすることで、上述したような、移動体の停止の問題を回避することができる。また、停止していた移動体が一旦動き出してからは、その後に移動体が停止したとしても、最初の特徴点を発見した時点から見れば特徴点の位置に変化があるので、三次元位置を定めることができる。しかしながら、特徴点を発見した映像の撮影時刻と、照合対象の時系列映像との時間差が大きい場合には、移動体の移動に伴い物体とカメラとの位置関係が変化するため物体の見え方が変化して、比較小領域の映像と時系列映像との違いが大きくなる。この違いが大きくなると、特徴点の対応関係を求めることが困難となり、結果として、特徴点の三次元位置を定めることができなくなってしまう。
【００２０】
また、前述した移動ステレオの第二の方式では、照合に用いる小領域の映像は時系列的に徐々に変化していくので、移動体の移動に伴い物体の見え方に変化があっても、継続的に特徴点の対応を求めることができる。但し、前述したように、この方式で物体の三次元位置を定めるには、着目した撮影時刻の特徴点の位置と、予め決められた時間間隔に基づき定めた過去の撮影時刻での映像における特徴点の位置とを用いて、特徴点の三次元位置を定めるようにする。ここで、当該着目した撮影時刻から当該過去の撮影時刻までの時間間隔をＭフレームとすれば、物体の三次元位置を定めるには、直近Ｍフレーム内で移動体が移動している必要がある。このため、移動体の移動量が低下してＭフレーム以上連続して移動が停止していると、停止している期間では時系列映像が変化しないため、この停止区間を含む時系列映像群では、物体の三次元位置を定めることができない。なお、この問題は、ＳＦＭ方式による特徴点の三次元位置の測定で説明したことと同様である。
【００２１】
上述した問題に鑑み、本明細書で後述する三次元座標測定装置は、特徴点の三次元位置の測定を、撮像装置の動き方の違いに依らず安定して行えるようにする。
【課題を解決するための手段】
【００２２】
本明細書で後述する三次元座標測定装置のひとつに、撮影部、抽出部、追跡部、検出部、登録部、及び算出部を備えるというものがある。ここで、撮影部は、時系列映像を撮影する。抽出部は、この時系列映像の各フレームのうちで撮影順の早いものから順に、各フレームに含まれている特徴点を抽出する一方で、同一フレーム内で先に抽出されていた特徴点からの当該同一フレーム上での距離が所定距離に満たない点については当該抽出の対象から除外する。追跡部は、特徴点を時系列映像上で追跡する。この追跡は、時系列映像のうちの時系列で連続している２フレームにおいての前のフレームから抽出部によって当該抽出の対象から除外されずに抽出された特徴点と後のフレームにおける当該特徴点についての対応点との対応関係を時系列映像の各フレームについて順次求めることによって行われる。検出部は、撮影部の移動量を検出する。登録部は、この移動量が所定量を超える度に、キーフレームに含まれている特徴点についての当該キーフレーム上での位置情報と、当該特徴点についての追跡部による追跡結果を表している追跡情報との登録を行う。なお、キーフレームとは、この移動量が当該所定量を超えた直後に撮影された時系列映像のフレームである。そして、算出部は、この移動量と、登録部により登録された、特徴点についての複数のキーフレームの各々での位置情報及び当該特徴点についての追跡情報とを用いて、当該特徴点の三次元位置の座標を算出する。なお、追跡部は、前のフレームから抽出部によって抽出された特徴点についての後のフレームにおける対応点についての、当該後のフレームから抽出部によって抽出されていた特徴点からのフレーム上での距離が所定距離に満たなかった場合には、当該後のフレームから抽出されていた特徴点を抽出部による抽出結果から除外し、除外しなかった特徴点について追跡を行う。
【００２３】
また、本明細書で後述する三次元座標測定方法のひとつは、まず、撮影部により撮影された時系列映像の各フレームのうちで撮影順の早いものから順に、各フレームに含まれている特徴点を抽出する一方で、同一フレーム内で先に抽出されていた特徴点からの当該同一フレーム上での距離が所定距離に満たない点については当該抽出の対象から除外する。次に、この特徴点を時系列映像上で追跡する。この追跡は、時系列映像のうちの時系列で連続している２フレームにおいての前のフレームから当該抽出の対象から除外されずに抽出された特徴点と後のフレームにおける当該特徴点についての対応点との対応関係を時系列映像の各フレームについて順次求めることによって行われる。次に、記撮影部の移動量を検出する。次に、この移動量が所定量を超える度に、当該所定量を超えた直後に撮影された時系列映像のフレームであるキーフレームに含まれている特徴点についての当該キーフレーム上での位置情報と、当該特徴点についての追跡の結果を表している追跡情報との登録を行う。そして、この移動量と、登録されている、特徴点についての複数のキーフレームの各々での位置情報及び当該特徴点についての追跡情報とを用いて、当該特徴点の三次元位置の座標を算出する。なお、特徴点の追跡では、前のフレームから特徴点の抽出により抽出された特徴点についての後のフレームにおける対応点についての、当該後のフレームから特徴点の抽出により抽出されていた特徴点からのフレーム上での距離が所定距離に満たなかった場合には、当該後のフレームから抽出されていた特徴点を特徴点の抽出による抽出結果から除外し、除外しなかった特徴点について追跡を行う。
【発明の効果】
【００２４】
本明細書で後述する三次元座標測定装置は、特徴点の三次元位置の測定が、撮像装置の動き方の違いに依らず安定して行えるという効果を奏する。
【図面の簡単な説明】
【００２５】
【図１】ＳＦＭ方式による特徴点の三次元位置の測定を、時系列画像の撮像間隔毎に行えるようにする手法の説明図である。
【図２Ａ】移動ステレオの第一の方式の説明図である。
【図２Ｂ】移動ステレオの第二の方式の説明図である。
【図３】単眼カメラの動画像を用いた三次元測距の原理の説明図である。
【図４】三次元座標測定装置の一実施例の機能ブロック図である。
【図５】特徴点の三次元位置の測定の手法の説明図である。
【図６】三次元座標測定装置の一実施例の詳細機能ブロック図である。
【図７】ＤＢＦバッファの構造の説明図である。
【図８】ＤＢＫバッファの構造の説明図である。
【図９】三次元座標測定装置の一実施例のハードウェア構成図である。
【図１０】三次元座標測定処理の処理内容を図解したフローチャートである。
【図１１】特徴点登録処理の処理内容を図解したフローチャートである。
【図１２】ＳＥＡＲＣＨ＿ＳＬＯＴ関数の処理内容を図解したフローチャートである。
【図１３Ａ】時系列照合処理の処理内容を図解したフローチャート（その１）である。
【図１３Ｂ】時系列照合処理の処理内容を図解したフローチャート（その２）である。
【図１４】ＭＡＴＣＨ関数の処理手順を図解したフローチャートである
【図１５】キーフレーム更新処理の処理内容を図解したフローチャートである。
【図１６】ＲＥＧＩＳＴ＿ＤＢＫ関数の処理手順を図解したフローチャートである。
【図１７】三次元位置算出処理の処理内容を図解したフローチャートである。
【図１８】ＳＥＡＲＣＨ＿ＫＥＹＦ関数の処理手順を図解したフローチャートである。
【図１９】ＣＡＬＣ＿ＲＴ関数の処理手順を図解したフローチャートである。
【図２０】移動体の座標系とカメラの座標系との関係の説明図である。
【図２１】特徴点の三次元位置と異なる２時刻での画像上における当該特徴点の投影点との関係の説明図である。
【発明を実施するための形態】
【００２６】
まず図４について説明する。図４は、三次元座標測定装置の一実施例の機能ブロック図である。
この三次元座標測定装置１０は、撮影部１１、抽出部１２、追跡部１３、検出部１４、登録部１５、及び算出部１６を備えている。
【００２７】
撮影部１１は、時系列映像を撮影する。
抽出部１２は、撮影部１１により撮影された時系列映像の各フレームに含まれている特徴点を抽出する。
【００２８】
追跡部１３は、抽出部１２により抽出された特徴点を時系列映像上で追跡する。この追跡は、時系列映像のうちの時系列で連続する２フレームにおいての前フレームにおける特徴点と、当該２フレームにおいての後フレームにおける当該特徴点についての対応点との対応関係を、時系列映像の各フレームについて順次求めることにより行われる。
【００２９】
検出部１４は、撮影部の移動量を検出する。
登録部１５は、検出部１４により検出された移動量が所定量を超える度に、キーフレームに含まれている特徴点についての当該キーフレーム上での位置情報と、当該特徴点についての追跡部１３による追跡結果を表している追跡情報との登録を行う。なお、キーフレームとは、検出部１４により検出された移動量が所定量を超えた直後に撮影部１１により撮影された時系列映像のフレームである。
【００３０】
算出部１６は、検出部１４により検出された移動量と、登録部１５により登録された、特徴点についての複数のキーフレームの各々での位置情報及び当該特徴点についての追跡情報とを用いて、当該特徴点の三次元位置の座標を算出する。
【００３１】
以上の各機能ブロックを備えている三次元座標測定装置１０は、特徴点の三次元位置の測定を、撮影部１１の移動量の多少に依らず安定して行うことができる。このことについて、図５を用いて説明する。
【００３２】
まず、追跡部１３は、時系列映像のうちの時系列で連続する２フレームにおいての前フレームにおける特徴点と、当該２フレームにおいての後フレームにおける当該特徴点についての対応点との対応関係を、時系列映像の各フレームについて順次求める。図５における破線の矩形Ａ内に図解した例では、特徴点１については、抽出部１２により撮影時刻ｔ−５の映像から始めて抽出された後に、撮影時刻ｔ−５の映像と撮影時刻ｔ−４の映像との間で対応関係が求められる。次に、撮影時刻ｔ−４の映像と撮影時刻ｔ−３の映像との間で対応関係が求められる。その後、撮影時刻ｔ−３の映像と撮影時刻ｔ−２の映像との間、撮影時刻ｔ−２の映像と撮影時刻ｔ−１の映像との間、及び、撮影時刻ｔ−１の映像と撮影時刻ｔの映像との間で対応関係が求められ、その後も同様にして対応関係が求められていく。一方、特徴点２については、抽出部１２により撮影時刻ｔ−３の映像から始めて抽出された後に、撮影時刻ｔ−３の映像と撮影時刻ｔ−２の映像との間で対応関係が求められる。次に、撮影時刻ｔ−２の映像と撮影時刻ｔ−１の映像との間で対応関係が求められる。その後、撮影時刻ｔ−１の映像と撮影時刻ｔの映像との間で対応関係が求められ、その後も同様にして対応関係が求められていく。
【００３３】
撮影部１１の移動によって生じる物体（特徴点）の見え方の変化は、時系列で隣接した撮影時刻の映像間では少ない。従って、三次元座標測定装置１０は、以上のようにして、時系列で隣接した撮影時刻の映像間で特徴点の対応関係を順次見つけるようにしたことで、特徴点の継続した追跡を、より確実に行うことができる。
【００３４】
また、登録部１５は、検出部１４により検出された撮影部１１の移動量が所定量を超える度に、キーフレームに含まれている特徴点についての情報の登録を行う。図５における破線の矩形Ｂ内に図解した例では、まず、撮影時刻ｔ−５の映像がキーフレームとされ、その映像から抽出された特徴点１についての位置情報が登録される。続いて、そのキーフレームの撮影後の撮影部１１の移動量が所定量Ｔｋを超えた直後に撮影された、撮影時刻ｔ−３の映像がキーフレームとされ、その映像から抽出された特徴点２についての位置情報が登録される。また、特徴点１については、その直前のキーフレームである、撮影時刻ｔ−５の映像からの追跡情報が、位置情報と共に登録される。続いて、そのキーフレームの撮影後の撮影部１１の移動量が所定量Ｔｋを超えた直後に撮影された、撮影時刻ｔ−１の映像がキーフレームとされる。そして、その映像に含まれている特徴点１及び特徴点２について、位置情報と、その直前のキーフレームである撮影時刻ｔ−３の映像からの追跡情報とが登録される。
【００３５】
算出部１６は、特徴点の三次元位置の座標の算出を、検出部１４により検出された移動量と、登録部１５により登録された、特徴点についての複数のキーフレームの各々での位置情報及び当該特徴点についての追跡情報とを用いて行う。図５の矩形Ｂ内に図解した例では、特徴点１については、撮影時刻ｔ−５、ｔ−３、及びｔ−１の各キーフレームでの特徴点１についての情報と、各キーフレームが撮影される間の撮影部１１の移動量とが用いられて、三次元位置の座標の算出が行われる。また、特徴点２については、撮影時刻ｔ−３及びｔ−１の各キーフレームでの特徴点２についての情報と、各キーフレームが撮影される間の撮影部１１の移動量とが用いられて、特徴点２の三次元位置の座標の算出が行われる。なお、この特徴点の三次元位置の座標の算出には、現時刻ｔに撮影された映像における特徴点についての情報と、撮影時刻が現時刻ｔに最も近い撮影時刻ｔ−１のキーフレームから現時刻ｔまでの撮影部１１の移動量とが更に用いられる。
【００３６】
以上のように、三次元座標測定装置１０は、撮影部１１が所定量移動する度に登録される、キーフレームでの特徴点についての情報を用いて、当該特徴点の三次元位置の座標の算出を行う。従って、撮影部１１の動き方に違いがあっても、特徴点についての情報が当該座標の算出に使用されるキーフレームの撮影位置間の距離が、常にある程度の長さ以上確保されるので、当該座標の算出の精度を、ある程度以上に維持することができる。
【００３７】
また、三次元座標測定装置１０では、時系列映像での特徴点の対応関係を求めるには、現時刻と１時刻前とを撮影時刻とする２つの映像での特徴点の位置が分かればよい。従って、このために必要となる特徴点の位置情報の保存領域としては、現時刻と１時刻前とを撮影時刻とする２つの映像についてのものを保持しておく領域を用意しておけば十分であり、処理のために必要な記憶領域が少なくて済む。
【００３８】
なお、抽出部１２は、時系列映像の各フレームのうち撮影順の早いものから順に特徴点の抽出を行い、先に抽出されていた特徴点からのフレーム上での距離が所定距離に満たない点については、特徴点の抽出の対象から除外するようにしてもよい。
【００３９】
また、前述の前のフレームから抽出部１２によって抽出された特徴点についての後のフレームにおける対応点についての、当該後のフレームから抽出部１２によって抽出されていた特徴点からのフレーム上での距離が所定距離に満たない場合がある。この場合には、追跡部１３は、当該後のフレームから抽出されていた特徴点を、抽出部１２による抽出結果から除外するようにしてもよい。
以上のように、対応関係を有している特徴点に対する追跡部１３による追跡の重複が抑制される。
【００４０】
また、図４の三次元座標測定装置１０において、撮影部１１は移動体に固定されており、検出部１４は、この移動体の回転量と並進量とを検出するセンサからの検出出力を用いて、前述の移動量として、撮影部１１の回転量と並進量との検出を行うようにしてもよい。この場合、登録部１５は、撮影部１１の並進量が所定量を超える度に、キーフレームについての前述の登録を行うようにする。
【００４１】
また、図４の三次元座標測定装置１０において、登録部１５は、所定数のキーフレームについての情報の登録が可能であるものでよい。この場合には、算出部１６は、登録部１５に登録されている、所定数のキーフレームの各々についての情報を用いて、特徴点の三次元位置の座標を算出する。また、登録部１５は、所定数のキーフレームについての情報の登録が既に行われた後に移動量が所定量を超えた場合には、最古に登録されたキーフレームについての情報の登録を削除する。そして、当該所定量を超えた直後に撮影されたキーフレームについての情報を代わりに登録する。
このようにすることで、登録部１５での情報の登録を行うために確保しておく必要がある記憶領域の容量を、所定量に制限することができるようになる。
【００４２】
次に図６について説明する。図６は、三次元座標測定装置１０の一実施例の詳細機能ブロック図である。
図６において、カメラ２１、映像入力部２２、及び映像保存部２３、並びに時系列映像ＤＢ（データベース）３１は、図４においては撮影部１１が備えている。また、特徴点抽出部２４及び特徴点登録部２５並びに時系列特徴点ＤＢ（データベース）３２は、図４においては抽出部１２が備えている。更に、時系列照合部２６は、図４においては追跡部１３が備えている。また、移動量センサ２７及び移動量算出部２８は、図４においては検出部１４が備えている。更に、キーフレーム更新部２９及びキーフレームＤＢ（データベース）３３は、図４においては登録部１５が備えている。そして、三次元位置算出部３０は、図４においては算出部１６が備えている。
【００４３】
カメラ２１は物体の撮影を行って、当該物体が映っている時系列映像（動画像）の映像信号を出力する。なお、本実施例では、カメラ２１は、不図示の移動体（例えば自動車）に固定されており、物体は移動しない（位置が変化しない）ものとし、カメラ２１は、移動体と共に移動しながら当該物体の撮影を行うものとする。
【００４４】
映像入力部２２は、カメラ２１から送られてくる映像信号を取得する。ここで、取得した映像信号がアナログ信号ならばデジタル化する。また、取得した映像信号がカラー映像を表している場合には、当該カラー映像をモノクロ濃淡映像に変換して、各データが映像濃淡値を表す二次元のデジタル映像データとして後段に出力する。なお、以下の説明では、このデジタル映像データを時系列映像と表現することとする。
【００４５】
映像保存部２３は、映像入力部２２から送られてくる時系列映像を、１フレームずつ時系列映像ＤＢ３１に保存すると共に特徴点抽出部２４に出力する。
特徴点抽出部２４は、時系列映像の各フレームから特徴点を抽出し、特徴点のフレーム上の位置を表す情報（（ｘ、ｙ）の直交二次元座標データ）を特徴点登録部２５へ出力する。
【００４６】
特徴点登録部２５は、時系列特徴点ＤＢ３２を参照して、特徴点抽出部２４から送られてきた位置情報で表されている特徴点が新規に検出されたものであるか否かの判定を、当該位置情報に基づいて行う。特徴点登録部２５は、ここで特徴点が新規であると判定した場合には、当該特徴点の情報を、時系列特徴点ＤＢ３２に新規に登録する。
【００４７】
時系列照合部２６は、時系列映像ＤＢ３１に保存されている、時系列映像における撮影時刻が現時刻のフレームと１時刻前のフレームとに対して、時系列特徴点ＤＢ３２に登録されている、当該１時刻前のフレームに含まれる特徴点についての映像照合を行う。なお、この映像照合は、１時刻前のフレームにおける当該特徴点を含む小領域の映像との類似度が、所定の閾値以上の高さであって且つ最高である領域を、当該現時刻のフレームから探索することによって行われる。そして、この探索によって発見された領域を当該現時刻のフレーム上で特定する位置情報を求め、得られた位置情報を、特徴点の追跡結果として、時系列特徴点ＤＢ３２に登録する。なお、この特徴点の追跡結果である位置情報で特定される位置の近傍に位置する特徴点が、時系列特徴点ＤＢ３２に既に登録されていた場合には、その既存の特徴点の情報を無効とする。
【００４８】
移動量センサ２７は、カメラ２１が固定されている移動体の移動量の検出を行う。
移動量算出部２８は、移動量センサ２７により検出された移動体の移動量より、時系列映像における撮影時刻が１時刻前であるフレームの撮影時刻から現時刻のフレームの撮影時刻までの期間における移動体の回転量と並進量を算出し、その算出結果を出力する。
【００４９】
キーフレーム更新部２９は、直近キーフレーム（撮影時刻が現時刻に最も近いキーフレーム）の撮影時刻から現時刻までの期間における移動体の回転量及び並進量を算出する。
なお、この算出には、移動量算出部２８による算出結果と、キーフレームＤＢ３３に格納されている、直近キーフレームの撮影時刻から１時刻前のフレームの撮影時刻までの移動体の回転量及び並進量とが用いられる。ここで、直近キーフレームの撮影時刻から現時刻までの期間の並進量が所定量を超えていた場合には、時系列特徴点ＤＢ３２に記録されている撮影時刻が現時刻のフレームについての情報を、新たなキーフレームの情報として、キーフレームＤＢ３３に追加する。なお、新たなキーフレームの情報を追加する余裕がキーフレームＤＢ３３にない場合には、キーフレーム更新部２９は、キーフレームＤＢ３３において撮影時刻が最古であるキーフレームについての情報を削除して、新たなキーフレームの情報を追加する。
【００５０】
三次元位置算出部３０は、時系列特徴点ＤＢ３２に登録されている現時刻のフレームに含まれている各特徴点について、キーフレームＤＢ３３に情報が格納されているキーフレームの中から、追跡結果を遡ることのできる最古の撮影時刻のキーフレームを特定する。次に、三次元位置算出部３０は、その最古の撮影時刻のキーフレームから現時刻のフレームまでを撮影部１１が撮影するまでの移動体の回転量と並進量とを算出する。そして、三次元位置算出部３０は、その最古の撮影時刻のキーフレームでの特徴点の位置と、現時刻を撮影時刻とするフレームでの特徴点の位置とから、三角測量の原理により、当該特徴点の三次元位置の座標を算出し、算出結果を出力する。
【００５１】
時系列映像ＤＢ３１は、時系列映像のうちの、現時刻を撮影時刻とするフレーム（現時刻フレーム）と、撮影時刻が現時刻の直前であるフレーム（前時刻フレーム）との映像データを保持しておくための２つの記憶領域を備えている。以下の説明では、この２つの記憶領域（バッファメモリ）をＤＢＰ（１）及びＤＢＰ（２）とする。この２つのバッファメモリは、処理対象のフレームが１フレーム進む度に循環して使用され、映像保存部２３によって現時刻フレームの映像データが２つのバッファメモリに交互に書き込まれる。
【００５２】
時系列特徴点ＤＢ３２は、時系列映像のうちの現時刻フレームと前時刻フレームとの各々から抽出された特徴点についての情報を保持しておくための、２つの記憶領域を備えている。以下の説明では、この２つの記憶領域（バッファメモリ）をＤＢＦ（１）及びＤＢＦ（２）とする。この２つのバッファメモリは、処理対象のフレームが１フレーム進む度に循環して使用され、特徴点登録部２５によって、現時刻フレームから抽出された特徴点についての情報が２つのバッファメモリに交互に書き込まれる。このＤＢＦの構造について、図７を参照しながら説明する。
【００５３】
ＤＢＦは、最大ＮＦ個の特徴点の各々についての情報を保持するための配列ＦＴＰ（）を保持する。なお、ＮＦの値は予め定めておく。
配列ＦＴＰ（）は、Ｆ＿ＩＤ、Ｆ＿ＵＳＥ、Ｆ＿ＰＸ、Ｆ＿ＰＹ、及びＦ＿ＫＣＮＴの各要素を有している。
【００５４】
Ｆ＿ＩＤはＤＢＦ内での特徴点を識別するための通し番号であり、ＮＦ個の配列ＦＰＴ（）に対し、その先頭を「１」番として、昇順に番号が割り付けられるものとする。
Ｆ＿ＵＳＥは、この配列の要素のデータが有効なものであるか否かを表すフラグである。このフラグの初期値は「０」とされて、配列の要素のデータは無効であることが表される。
【００５５】
Ｆ＿ＰＸ及びＦ＿ＰＹは、この配列により表されている特徴点についての、時系列映像のフレーム上の位置を表すｘ座標値及びｙ座標値である。
Ｆ＿ＫＣＮＴは、この配列により表されている特徴点についての追跡結果であって、この配列により表されている特徴点の対応関係を追って遡ることのできるキーフレームの数を表すものであり、初期値は「０」とされる。
【００５６】
キーフレームＤＢ３３は、キーフレームについての情報を保持しておくための記憶領域として、バッファメモリＤＢＫを備えている。このＤＢＫの構造について、図８を参照しながら説明する。
【００５７】
バッファメモリＤＢＫは、ＮＫ個のキーフレームの各々についての情報を保持するための配列ＫＥＹＦ（）を保持する。なお、ＮＫの値は予め定めておく。
配列ＫＥＹＦ（）は、Ｋ＿ＦＰＴＳＥＴ、Ｋ＿Ｒ、及びＫ＿Ｔの各要素を有している。
【００５８】
Ｋ＿ＦＰＴＳＥＴは、このキーフレームから抽出された特徴点についての情報であり、図７に図解したＤＢＦと同一のデータ構造を有しており、最大ＮＦ個の特徴点の各々についての情報を保持する。
【００５９】
Ｋ＿Ｒ及びＫ＿Ｔは、それぞれ、このキーフレームから次のキーフレーム（撮影時刻が時間軸上で次のキーフレーム）をカメラ２１が撮影するまでの移動体の回転量及び並進量である。なお、直近キーフレーム（撮影時刻が現時刻に最も近いキーフレーム）に関しては、このＫ＿Ｒ及びＫ＿Ｔは、直近キーフレームから現時刻フレームをカメラ２１が撮影するまでの移動体の回転量と並進量とを表すものとしている。
【００６０】
次に三次元座標測定装置１０のハードウェア構成について説明する。図９は、三次元座標測定装置１０の一実施例のハードウェア構成図である。
図９において、三次元座標測定装置１０は、ＭＰＵ４１、ＲＯＭ４２、ＲＡＭ４３、インタフェース装置４４、出力装置４５を備えている。なお、これらの構成要素は、システムバス４６を介して接続されており、ＭＰＵ４１の管理の下で各種のデータを相互に授受することができる。更に、三次元座標測定装置１０は、前述したカメラ２１及び移動量センサ２７を備えており、これらはインタフェース装置４４に接続されている。
【００６１】
ＭＰＵ（Micro Processing Unit）４１は、三次元座標測定装置１０全体の動作を制御する演算処理装置である。
ＲＯＭ（Read Only Memory）４２は、所定の制御プログラムや各種の定数値が予め記録されている読み出し専用半導体メモリである。ＭＰＵ４１は、この制御プログラムを三次元座標測定装置１０の起動時に読み出して実行することにより、この三次元座標測定装置１０の各構成要素の動作制御が可能となり、更に、後述する制御処理を行えるようにもなる。
【００６２】
ＲＡＭ（Random Access Memory）４３は、ＭＰＵ４１が各種の制御プログラムを実行する際に、必要に応じて作業用記憶領域として使用する、随時書き込み読み出し可能な半導体メモリである。このＲＡＭ４３は、図６における時系列映像ＤＢ３１、時系列特徴点ＤＢ３２、及びキーフレームＤＢ３３として機能する。
【００６３】
インタフェース装置４４は、カメラ２１及び移動量センサ２７との間での各種データの授受の管理を行う。
出力装置４５は、ＭＰＵ４１から送付されるデータの出力を行う装置であり、例えば、ＭＰＵ４１から送付される表示データに応じて各種のテキストや画像を表示する液晶ディスプレイ装置である。
【００６４】
上述した構成により、ＭＰＵ４１を、映像入力部２２、映像保存部２３、特徴点抽出部２４、特徴点登録部２５、時系列照合部２６、移動量センサ２７、移動量算出部２８、キーフレーム更新部２９、及び三次元位置算出部３０として機能させることができる。このためには、まず、後述する、三次元座標測定装置１０の各機能ブロックにより行われる三次元座標測定処理の処理内容をＭＰＵ４１に行わせるための制御プログラムを作成する。作成した制御プログラムはＲＯＭ４２に予め格納しておく。そして、ＭＰＵ４１に所定の指示を与えてこの制御プログラムを読み出させて実行させる。こうすることで、ＭＰＵ４１を、上述した各構成要素として機能させることが可能となる。
【００６５】
なお、図９に破線を用いて図解されているように、三次元座標測定装置１０に記録媒体駆動装置４７を備えるようにしてもよい。記録媒体駆動装置４７は、可搬型記録媒体４８に記録されているデータを読み出す装置であり、システムバス４６に接続される。例えば、ＲＯＭ４２としてフラッシュメモリを使用すると共に、前述した制御プログラムは可搬型記録媒体４８に記録させておくようにし、この制御プログラムを記録媒体駆動装置４７で可搬型記録媒体４８から読み出してＲＯＭ４２に書き込むようにしてもよい。なお、可搬型記録媒体４８としては、ＣＤ−ＲＯＭ（Compact Disc Read Only Memory）やＤＶＤ−ＲＯＭ（Digital Versatile Disc Read Only Memory）、ＵＳＢ（Universal Serial Bus）規格のコネクタを備えたフラッシュメモリなどが利用可能である。
【００６６】
次に、三次元座標測定装置１０で行われる三次元座標測定処理について、図１０に図解したフローチャートを参照しながら説明する。
図１０の処理が開始されると、まず、Ｓ１０１において、ＩＮＩＴ＿Ｆ、Ｐ＿ＣＵＲ、Ｐ＿ＬＡＳＴ、及びＤＢＫ＿Ｐの各々に初期値を設定する処理が行われる。
【００６７】
ＩＮＩＴ＿Ｆは、進行中の処理が、この図１０の処理の開始後の最初に行われるものであるか否かを表すフラグである。Ｓ１０１の初期化処理では、このフラグが「０」にセットされて、進行中の処理が、図１０の処理の開始後の最初に行われるものであることが表される。
【００６８】
Ｐ＿ＣＵＲ及びＰ＿ＬＡＳＴは、時系列映像ＤＢ３１及び時系列特徴点ＤＢ３２にそれぞれ２つずつ備えられているバッファメモリに保持されているデータが、現時刻フレームと前時刻フレームとのどちらについてのものかを表すために使用される変数である。Ｓ１０１の初期化処理では、現時刻フレームを表す値「１」がＰ＿ＣＵＲに設定され、前時刻フレームを表す値「２」がＰ＿ＬＡＳＴに設定される。
【００６９】
ＤＢＫ＿Ｐは、キーフレームＤＢ３３で保持される配列ＫＥＹＦ（）を、新たなキーフレームの登録の度に循環して使用するために、直近キーフレームについての情報が保持されている配列ＫＥＹＦ（）を特定するための変数である。Ｓ１０１の初期化処理では、この変数ＤＢＫ＿Ｐに値「１」が設定される。
【００７０】
次に、Ｓ１０２において、映像入力処理を映像入力部２２が行う。この処理は、カメラ２１から送られてくる、時系列映像における現時刻フレームの映像信号を取得する処理である。ここで、取得した映像信号がアナログ信号ならば、信号をデジタル化して取得する処理が行われる。また、映像信号がカラー映像を表している場合には、当該カラー映像をモノクロ濃淡映像に変換して、映像濃淡値を表す二次元のデジタル映像データとする処理が行われる。
【００７１】
次に、Ｓ１０３において、映像保存処理を映像保存部２３が行う。この処理では、Ｓ１０２の処理により取得された現時刻フレームの映像データを、時系列映像ＤＢ３１におけるバッファメモリＤＢＰ（Ｐ＿ＣＵＲ）に保存する処理である。
【００７２】
次に、Ｓ１０４において、特徴点抽出処理を特徴点抽出部２４が行う。この処理は、Ｓ１０２の処理により取得された現時刻フレームから特徴点を抽出して、抽出された特徴点の現時刻フレーム上の位置を表す二次元座標（ｘ，ｙ）を取得する処理である。なお、映像上の特徴点を抽出する手法としては、コーナーを抽出する手法やエッジを抽出する手法など、様々な手法を採用することができる。本実施例では、広く知られているハリスのコーナー検出（Harris corner detector）を適用して検出されたコーナー点を特徴点として抽出することとする。ここで、抽出されたコーナー点の位置の座標をＣＮＲ＝｛（ｘｃ１，ｙｃ１）、（ｘｃ２，ｙｃ２）、…、（ｘｃＮＣ，ｙｃＮＣ）｝とし、ＮＣ個の特徴点が抽出されたものとする。
【００７３】
次に、Ｓ１０５において、特徴点登録処理を特徴点登録部２５が行う。この処理は、時系列特徴点ＤＢ３２で保持されている現時刻フレームについての各特徴点を参照し、Ｓ１０４の処理により得られたコーナー点が新規のものか否かを判定し、新規のものであれば、そのコーナー点の座標を時系列特徴点ＤＢ３２に登録する処理である。この特徴点登録処理の具体的な処理内容については後述する。
【００７４】
次に、Ｓ１０６において、時系列照合処理を時系列照合部２６が行う。この処理は、前時刻フレームの特徴点の対応点を現時刻フレームから特定する処理である。この処理では、まず、時系列映像ＤＢ３１に保持されている現時刻フレームと前時刻フレームとに対して、時系列特徴点ＤＢ３２に登録されている、前時刻フレームの特徴点についての映像照合を行って、当該特徴点の対応点を当該現時刻フレームから特定する。そして、この対応点の現時刻フレーム上の位置を表している座標情報を、現時刻フレームにおける当該特徴点の追跡結果として、時系列特徴点ＤＢ３２に登録する。ここで、この追跡結果の座標の近傍である特徴点の情報が、時系列特徴点ＤＢ３２に既に登録されていた場合には、その既登録の特徴点の情報を無効とする。この時系列照合処理の具体的な処理内容については後述する。
【００７５】
次に、Ｓ１０７において、移動量算出処理を移動量算出部２８が行う。この処理は、移動量センサ２７によって検出される情報より、カメラ２１が前時刻フレームを撮影してから現時刻フレームの撮影までの期間における、カメラ２１が固定されている移動体の回転量Ｒｓ及び並進量Ｔｓを算出する処理である。
【００７６】
本実施例では、移動量センサ２７によって、移動体の回転変化を表している３軸の角速度ベクトルＷｓ（ｗｘ，ｗｙ，ｗｚ）（但し、ｗｘ、ｗｙ、及びｗｚの単位はｒａｄ／ｓ）と、並進移動量Ｔｓ（Ｔｓｘ，Ｔｓｙ，Ｔｓｚ）とが得られるものとする。
【００７７】
ここで、移動体の回転量Ｒｓを角速度ベクトルＷｓから求めるために、四元数表現を用いて、
ｑ１＝（０，０，０，１）
ｑ２＝（ｗｘ，ｗｙ，ｗｚ，０）
と表すと、回転変換の四元数表現は、
ｑｑ＝（１／２）×ｑ１×ｑ２×ｄｔ
で表される。なお、ここで、ｄｔは、時系列画像の撮影時間間隔を表しており、例えば一般的なビデオレートでの処理ならば、１／３０秒である。
【００７８】
この四元数表現の回転量を行列表現に変換するために、ｑｑの四元数表現を、
ｑｑ＝（ｘ，ｙ，ｚ，ｗ）
と表すこととすると、Ｒｓは下記の［数１］式のように表現される。
【００７９】
【数１】
0005915130.tif 000002
移動量算出部２８は、以上のようにして回転量Ｒｓ及び並進量Ｔｓを求める。
【００８０】
次に、Ｓ１０８において、キーフレーム更新処理をキーフレーム更新部２９が行う。この処理は、時系列映像からキーフレームを特定すると共に、キーフレームＤＢ３３を更新して、特定されたキーフレームについての情報を登録する処理である。この処理では、まず、キーフレーム更新部２９は、直近キーフレームの撮影時刻から現時刻までの期間における移動体の回転量及び並進量を算出する。ここで、この並進量が所定量を超えていた場合には、時系列特徴点ＤＢ３２に記録されている、現時刻フレームについての特徴点の情報を、新たなキーフレームの特徴点の情報として、キーフレームＤＢ３３に追加する。なお、新たなキーフレームについての情報を追加する余裕がキーフレームＤＢ３３にない場合には、キーフレーム更新部２９は、キーフレームＤＢ３３において撮影時刻が最古であるキーフレームについての情報を削除して、新たなキーフレームの情報を追加する。このキーフレーム更新処理の具体的な処理内容については後述する。
【００８１】
次に、Ｓ１０９では、三次元位置算出処理を三次元位置算出部３０が行う。この処理は、特徴点の三次元位置の座標を算出する処理である。この処理では、まず、時系列特徴点ＤＢ３２に登録されている現時刻フレームに含まれている各特徴点について、追跡結果を遡ることのできる最古の撮影時刻のキーフレームを、キーフレームＤＢ３３に情報が格納されている複数のキーフレームから特定する。次に、三次元位置算出部３０は、その最古の撮影時刻のキーフレームから現時刻フレームまでを撮影部１１が撮影するまでの移動体の回転量と並進量とを算出する。そして、三次元位置算出部３０は、その最古の撮影時刻のキーフレームでの特徴点の位置と、現時刻フレームでの特徴点の位置とから、三角測量の原理により、当該特徴点の三次元位置の座標を算出して算出結果を出力する。この三次元位置算出処理の具体的な処理内容についても後述する。
【００８２】
次に、Ｓ１１０では、前述したフラグＩＮＩＴ＿Ｆに「１」をセットして、この処理以降に行う処理は、図１０の処理が開始されてから２回目以降であることを表す処理が行われる。
【００８３】
次に、Ｓ１１１では、Ｐ＿ＣＵＲとＰ＿ＬＡＳＴとの間で、Ｐ＿ＣＵＲのこの処理時点での値と、Ｐ＿ＬＡＳＴのこの処理時点での値とを交換する処理が行われ、その後はＳ１０２に処理が戻り、以上までに説明した処理が繰り返される。
以上までの処理が、三次元座標測定処理である。
【００８４】
なお、この図１０の処理では、Ｓ１０４の処理における特徴点の抽出と、Ｓ１０６の処理における特徴点の追跡（映像照合）とを並行して行うようにしてもよい。また、Ｓ１０７の移動量算出処理を、Ｓ１０２からＳ１０６までの処理と並行して実行するようにしてもよい。
【００８５】
次に、図１０に図解した幾つかの処理の具体的な処理内容と、各処理において使用されている関数についての具体的な処理手順の内容を説明する。
なお、以降で説明する各関数の処理手順において使用されている変数ｉ、ｊ、ｉｉ、及びｉｊ、は、その関数の処理において局所的に使用される変数であって、大域的に使用されるものではないものとする。
【００８６】
まず、図１０の処理におけるＳ１０５の処理である、特徴点登録処理の具体的な処理内容について、図１１に図解したフローチャートを参照しながら説明する。
図１１において、まず、Ｓ２０１では、変数ｉに初期値「０」を代入する処理を特徴点登録部２５が行う。
【００８７】
次に、Ｓ２０２において、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入する処理を特徴点登録部２５が行う。
次に、Ｓ２０３において、この処理時点での変数ｉの値が、図１０のＳ１０４の処理において特徴点抽出部２４により抽出された特徴点（コーナー点）の個数ＮＣ以下であるか否かを判定する処理を特徴点登録部２５が行う。ここで、特徴点登録部２５は、変数ｉの値がＮＣ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ２０４に処理を進める。一方、特徴点登録部２５は、変数ｉの値がＮＣを超えたと判定したとき（判定結果がＮｏのとき）には、この特徴点登録処理を終了して図１０に処理を戻す。
【００８８】
次に、Ｓ２０４において、特徴点抽出部２４により抽出された特徴点（コーナー点）のうちのｉ番目の位置の座標のうち、ｘ座標（ｘｃｉ）を変数ｘｃに代入し、ｙ座標（ｙｃｉ）を変数ｙｃに代入する処理を特徴点登録部２５が行う。
【００８９】
Ｓ２０５からＳ２１０にかけての処理は、Ｓ２０４の処理により座標（ｘｃ，ｙｃ）を取得したコーナー点が新規の特徴点であるか否かを判定する処理である。この判定は、コーナー点の座標（ｘｃ，ｙｃ）からの距離がｃｃＲ以内の範囲に、現時刻フレームから既に抽出されていた特徴点が存在するか否かを判定することによって行い、存在しない場合には、当該コーナー点は新規の特徴点であるとの判定が下される。なお、距離ｃｃＲの値については予め設定しておく。
【００９０】
まず、Ｓ２０５において、変数ｊに初期値「０」を代入する処理を特徴点登録部２５が行う。
次に、Ｓ２０６において、この処理時点での変数ｊの値に「１」を加算した結果の値を改めて変数ｊに代入する処理を特徴点登録部２５が行う。
【００９１】
次に、Ｓ２０７において、この処理時点での変数ｊの値が、時系列特徴点ＤＢ３２のバッファメモリＤＢＦに情報を保持させることのできる特徴点の個数ＮＦ以下であるか否かを判定する処理を特徴点登録部２５が行う。ここで、特徴点登録部２５は、変数ｊの値がＮＦ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ２０８に処理を進める。一方、特徴点登録部２５は、変数ｊの値がＮＦを超えたと判定したとき（判定結果がＮｏのとき）には、上述した新規の特徴点についての判定処理を終了して、Ｓ２１１に処理を進める。
【００９２】
次に、Ｓ２０８では、時系列特徴点ＤＢ３２のバッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｊ）を参照し、この配列における要素Ｆ＿ＵＳＥに「１」がセットされているか否かを判定する処理を特徴点登録部２５が行う。ここで、特徴点登録部２５は、Ｆ＿ＵＳＥに「１」がセットされていると判定したとき（判定結果がＹｅｓのとき）には、この配列ＦＴＰ（ｊ）の要素のデータは有効であると判断し、Ｓ２０９に処理を進める。一方、特徴点登録部２５は、ここで、Ｆ＿ＵＳＥに「１」がセットされていないと判定したとき（判定結果がＮｏのとき）には、この配列ＦＴＰ（ｊ）の要素のデータは無効であると判断してＳ２０６に処理を戻し、上述したＳ２０６以降の処理を繰り返す。
【００９３】
次に、Ｓ２０９では、以下の処理を特徴点登録部２５が行う。
まず、特徴点登録部２５は、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｊ）を参照し、要素Ｆ＿ＰＸ及びＦ＿ＰＹの値を取得して変数ｘｐ及びｙｐにそれぞれ代入する処理を行う。そして、次に、特徴点登録部２５は、下記の式の計算を実行してＬの値を算出する処理を行う。
Ｌ＝｛（ｘｐ−ｘｃ）2 ＋（ｙｐ−ｙｃ）2 ｝1/2 
特徴点登録部２５は、Ｓ２０９において、以上の処理を行う。
【００９４】
次に、Ｓ２１０では、Ｓ２０９の処理により算出されたＬの値が、前述した距離ｃｃＲ以下であるか否かを判定する処理を特徴点登録部２５が行う。ここで、特徴点登録部２５は、Ｌが距離ｃｃＲ以下であると判定したとき（判定結果がＹｅｓのとき）には、直近に実行されたＳ２０４の処理により座標値を取得したコーナー点は新規の特徴点ではないと判断してＳ２０２に処理を戻して上述した処理を繰り返す。一方、特徴点登録部２５は、ここで、Ｌが距離ｃｃＲよりも長いと判定したとき（判定結果がＮｏのとき）には、Ｓ２０６に処理を戻し、上述したＳ２０６以降の処理を繰り返す。
【００９５】
Ｓ２１１の処理は、上述したＳ２０５からＳ２１０にかけての処理によって、直近に実行されたＳ２０４の処理により座標値を取得したコーナー点は新規の特徴点であると判断されたことによって実行される処理である。このＳ２１１では、関数ＳＥＡＲＣＨ＿ＳＬＯＴ（ＤＢＦ（Ｐ＿ＣＵＲ））の戻り値を取得して変数ｋに代入する処理を特徴点登録部２５が行う。
【００９６】
ＳＥＡＲＣＨ＿ＳＬＯＴ関数は、時系列特徴点ＤＢ３２のバッファメモリＤＢＰが保持している配列ＦＴＰ（）のうちで、空いている（すなわち、要素のデータが無効ある）ものを探し出し、空いている配列ＦＴＰ（）の番号を戻り値として返す関数である。但し、ＳＥＡＲＣＨ＿ＳＬＯＴ関数は、空いている配列ＦＴＰ（）がなかった場合には、戻り値として、値「−１」を返すものとする。このＳＥＡＲＣＨ＿ＳＬＯＴ関数の具体的な処理手順については後述する。
【００９７】
次に、Ｓ２１２では、変数ｋの値（すなわち、関数ＳＥＡＲＣＨ＿ＳＬＯＴ（ＤＢＦ（Ｐ＿ＣＵＲ））の戻り値）が正の値であったか否かを判定する処理を特徴点登録部２５が行う。ここで、特徴点登録部２５は、変数ｋの値が正の値であったと判定したとき（判定結果がＹｅｓのとき）には、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）が保持している配列ＦＴＰ（）に空きがあったと判断してＳ２１３に処理を進める。一方、ここで、特徴点登録部２５は、変数ｋの値が正の値でなかったと判定したとき（判定結果がＮｏのとき）には、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）が保持している配列ＦＴＰ（）には空きがなかったとの判断を下す。この場合には、特徴点登録部２５は、新規の特徴点の登録を断念して、この特徴点登録処理を終了し、図１０に処理を戻す。
【００９８】
次に、Ｓ２１３では、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）が保持している配列ＦＴＰ（ｋ）の要素の値を設定する処理を特徴点登録部２５が行う。具体的には、Ｆ＿ＵＳＥが「１」にセットされると共に、Ｆ＿ＰＸ及びＦ＿ＰＹには、それぞれｘｃ及びｙｃの値が格納され、更に、Ｆ＿ＫＣＮＴに初期値「０」が格納される。特徴点登録部２５は、このＳ２１３の処理の終了後は、Ｓ２０２に処理を戻して上述した処理を繰り返す。
以上までの処理が特徴点登録処理である。
【００９９】
次に、上述した特徴点登録処理におけるＳ２１１の処理において用いられているＳＥＡＲＣＨ＿ＳＬＯＴ関数の具体的な処理手順について、図１２に図解したフローチャートを参照しながら説明する。
【０１００】
図１２において、まず、Ｓ２２１では、変数ｉに初期値「０」を代入する処理を特徴点登録部２５が行う。
次に、Ｓ２２２において、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入する処理を特徴点登録部２５が行う。
【０１０１】
次に、Ｓ２２３において、この処理時点での変数ｉの値が、前述した個数ＮＦ以下であるか否かを判定する処理を特徴点登録部２５が行う。ここで、特徴点登録部２５は、変数ｉの値がＮＦ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ２２４に処理を進め、変数ｉの値がＮＦを超えたと判定したとき（判定結果がＮｏのとき）にはＳ２２６に処理を進める。
【０１０２】
次に、Ｓ２２４では、ＳＥＡＲＣＨ＿ＳＬＯＴ関数が呼ばれたときの引数によって特定されるバッファメモリＤＢＰが保持している配列ＦＴＰ（ｉ）の要素Ｆ＿ＵＳＥが「０」にセットされているか否かを判定する処理を特徴点登録部２５が行う。特徴点登録部２５は、ここで、配列ＦＴＰ（ｉ）の要素Ｆ＿ＵＳＥが「０」にセットされていると判定したとき（判定結果がＹｅｓのとき）にはＳ２２５に処理を進める。一方、特徴点登録部２５は、ここで、配列ＦＴＰ（ｉ）の要素Ｆ＿ＵＳＥが「０」にセットされていないと判定したとき（判定結果がＮｏのとき）にはＳ２２２へ処理を戻して前述した処理を繰り返す。
【０１０３】
次に、Ｓ２２５では、ＳＥＡＲＣＨ＿ＳＬＯＴ関数の戻り値として、空いている配列ＦＴＰ（）の番号を表している変数ｉのこの処理時点での値を返す処理を特徴点登録部２５が行い、その後はこの図１２の処理が終了する。
【０１０４】
一方、Ｓ２２６では、ＳＥＡＲＣＨ＿ＳＬＯＴ関数の戻り値として、配列ＦＴＰ（）には空きがなかったことを表している値「−１」を返す処理を特徴点登録部２５が行い、その後はこの図１２の処理が終了する。
【０１０５】
ＳＥＡＲＣＨ＿ＳＬＯＴ関数は以上のように処理される。
次に、図１０の処理におけるＳ１０６の処理である、時系列照合処理の具体的な処理内容について、図１３Ａ及び図１３Ｂに図解したフローチャートを参照しながら説明する。
【０１０６】
まず、図１３Ａにおいて、Ｓ３０１では、変数ｉに初期値「０」を代入する処理を時系列照合部２６が行う。
次に、Ｓ３０２において、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入する処理を時系列照合部２６が行う。
【０１０７】
次に、Ｓ３０３において、この処理時点での変数ｉの値が、前述した個数ＮＦ以下であるか否かを判定する処理を時系列照合部２６が行う。ここで、時系列照合部２６は、変数ｉの値がＮＦ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ３０４に処理を進め、変数ｉの値がＮＦを超えたと判定したとき（判定結果がＮｏのとき）にはＳ３１０（図１３Ｂ）に処理を進める。
【０１０８】
次に、Ｓ３０４では、時系列特徴点ＤＢ３２のバッファメモリＤＢＦ（Ｐ＿ＬＡＳＴ）における配列ＦＴＰ（ｉ）を参照し、この配列における要素Ｆ＿ＵＳＥに「０」がセットされているか否かを判定する処理を時系列照合部２６が行う。ここで、時系列照合部２６は、Ｆ＿ＵＳＥに「０」がセットされていると判定したとき（判定結果がＹｅｓのとき）には、この配列ＦＴＰ（ｉ）の要素のデータは無効であると判断し、Ｓ３０５に処理を進める。一方、時系列照合部２６は、ここで、Ｆ＿ＵＳＥに「０」がセットされていないと判定したとき（判定結果がＮｏのとき）には、この配列ＦＴＰ（ｉ）の要素のデータは有効であると判断してＳ３０６に処理を戻し、上述したＳ３０６以降の処理を繰り返す。
【０１０９】
Ｓ３０５の処理は、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）が保持している配列ＦＴＰ（ｉ）で情報が表されている現時刻フレームの特徴点が、前時刻フレームには含まれておらず、フレーム間での対応関係がなかったと判定された場合に実行される処理である。このＳ３０５では、この配列ＦＴＰ（ｉ）における要素Ｆ＿ＵＳＥを「０」にセットする処理を時系列照合部２６が行い、その後はＳ３０２へ処理を戻して前述した処理を繰り返す。
【０１１０】
一方、Ｓ３０６では、以下の処理を時系列照合部２６が行う。
まず、時系列照合部２６は、バッファメモリＤＢＦ（Ｐ＿ＬＡＳＴ）における配列ＦＴＰ（ｉ）を参照し、要素Ｆ＿ＰＸ及びＦ＿ＰＹの値を取得して変数ｐｘ１及びｐｙ１にそれぞれ代入する処理を行う。そして、次に、時系列照合部２６は、関数ＭＡＴＣＨ（ｐｘ１，ｐｙ１）の戻り値を取得して変数ｐｘ２、ｐｙ２、及びＲＥＳに代入する処理を行う。
【０１１１】
ＭＡＴＣＨ関数は、座標（ｐｘ１、ｐｙ１）を引数とし、前時刻フレームの映像上におけるこの座標（ｐｘ１、ｐｙ１）に位置している特徴点についての対応点を現時刻フレームから探し出す関数である。より具体的には、このＭＡＴＣＨ関数は、まず、前時刻フレームの映像上の座標（ｐｘ１、ｐｙ１）を中心位置とする所定サイズの矩形領域映像を、当該矩形領域映像と同一サイズである現時刻フレームの映像上での部分映像と比較する処理を行う。この比較処理における比較対象の一方である現時刻フレームの映像上の部分映像は、座標（ｐｘ１、ｐｙ１）を中心位置とする所定の探索範囲を所定量ずつ移動させながら複数枚取得し、取得された部分映像の全てについて比較処理が行われる。この比較処理では、２つの映像の類似度の高さを表す指標が算出される。ここで、この指標が表している類似度が最高となったときに比較対象としていた部分映像の中心位置の座標（ｐｘ２、ｐｙ２）が取得される。ＭＡＴＣＨ関数は、この最高の類似度が所定の閾値以上の場合には、座標（ｐｘ２、ｐｙ２）の値を戻り値として返すと共に、判定結果を表す戻り値ＲＥＳとして、特徴点の対応点が見つかったことを表す値「１」を返す。また、ＭＡＴＣＨ関数は、この最高の類似度が所定の閾値未満の場合には、判定結果を表す戻り値ＲＥＳとして、特徴点の対応点が見つからなかったことを表す値「０」を返す。このＭＡＴＣＨ関数の具体的な処理手順については後述する。
【０１１２】
時系列照合部２６は、Ｓ３０６において、以上の処理を行う。
次に、Ｓ３０７では、Ｓ３０６の処理により得られた関数ＭＡＴＣＨの戻り値のうちのＲＥＳの値が「１」であったか否かを判定する処理を時系列照合部２６が行う。時系列照合部２６は、ここで、ＲＥＳの値が「１」であったと判定したとき（判定結果がＹｅｓのとき）にはＳ３０８に処理を進め、ＲＥＳの値が「１」ではなかったと判定したとき（判定結果がＮｏのとき）にはＳ３０９に処理を進める。
【０１１３】
次に、Ｓ３０８では、特徴点の対応点が見つかったことにより、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）が保持している配列ＦＴＰ（ｉ）の要素の値を設定する処理を時系列照合部２６が行う。具体的には、Ｆ＿ＵＳＥが「１」にセットされると共に、Ｆ＿ＰＸ及びＦ＿ＰＹには、それぞれｐｘ２及びｐｙ２の値が格納される。更に、Ｆ＿ＫＣＮＴに、バッファメモリＤＢＦ（Ｐ＿ＬＡＳＴ）が保持している配列ＦＴＰ（ｉ）における要素Ｆ＿ＣＫＮＴの値が格納される。時系列照合部２６は、このＳ３０８の処理を終えたときにはＳ３０２へ処理を戻して前述した処理を繰り返す。
【０１１４】
一方、Ｓ３０９では、特徴点の対応点が見つからなかったことにより、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）が保持している配列ＦＴＰ（ｉ）におけるＦ＿ＵＳＥを「０」にセットする処理を時系列照合部２６が行う。時系列照合部２６は、このＳ３０９の処理を終えたときにはＳ３０２へ処理を戻して前述した処理を繰り返す。
【０１１５】
図１３Ｂに処理が進み、Ｓ３１０からＳ３２１にかけての処理は、図１３Ａの処理により見つかった対応点の近傍（距離がｃｃＲ未満）に他の特徴点が位置していた場合に、時系列特徴点ＤＢ３２に登録されていた当該他の特徴点の情報を無効とする処理である。
【０１１６】
図１３Ｂにおいて、まず、Ｓ３１０では、変数ｉに初期値「０」を代入する処理を時系列照合部２６が行う。
次に、Ｓ３１１において、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入する処理を時系列照合部２６が行う。
【０１１７】
次に、Ｓ３１２において、この処理時点での変数ｉの値が、前述した個数ＮＦ以下であるか否かを判定する処理を時系列照合部２６が行う。ここで、時系列照合部２６は、変数ｉの値がＮＦ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ３１３に処理を進める。一方、時系列照合部２６は、変数ｉの値がＮＦを超えたと判定したとき（判定結果がＮｏのとき）には、この時系列照合処理を終了して図１０に処理を戻す。
【０１１８】
次に、Ｓ３１３では、時系列特徴点ＤＢ３２のバッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｉ）を参照し、この配列における要素Ｆ＿ＵＳＥに「１」がセットされているか否かを判定する処理を時系列照合部２６が行う。ここで、時系列照合部２６は、Ｆ＿ＵＳＥに「１」がセットされていると判定したとき（判定結果がＹｅｓのとき）には、この配列ＦＴＰ（ｉ）の要素のデータは有効であると判断し、Ｓ３１４に処理を進める。一方、時系列照合部２６は、ここで、Ｆ＿ＵＳＥに「１」がセットされていないと判定したとき（判定結果がＮｏのとき）には、この配列ＦＴＰ（ｉ）の要素のデータは無効であると判断してＳ３１１に処理を戻し、上述したＳ３１１以降の処理を繰り返す。
【０１１９】
次に、Ｓ３１４では、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｉ）を参照し、要素Ｆ＿ＰＸ及びＦ＿ＰＹの値を取得して変数ｐｘ１及びｐｙ１にそれぞれ代入する処理を時系列照合部２６が行う。
【０１２０】
次に、Ｓ３１５において、この処理時点での変数ｉの値を変数ｊに代入する処理を時系列照合部２６が行う。
次に、Ｓ３１６において、この処理時点での変数ｊの値に「１」を加算した結果の値を改めて変数ｊに代入する処理を時系列照合部２６が行う。
【０１２１】
次に、Ｓ３１７において、この処理時点での変数ｊの値が、前述した個数ＮＦ以下であるか否かを判定する処理を時系列照合部２６が行う。ここで、時系列照合部２６は、変数ｊの値がＮＦ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ３１８に処理を進める。一方、時系列照合部２６は、変数ｊの値がＮＦを超えたと判定したとき（判定結果がＮｏのとき）には、Ｓ３１１に処理を戻し、上述したＳ３１１以降の処理を繰り返す。
【０１２２】
次に、Ｓ３１８では、時系列特徴点ＤＢ３２のバッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｊ）を参照し、この配列における要素Ｆ＿ＵＳＥに「１」がセットされているか否かを判定する処理を時系列照合部２６が行う。ここで、時系列照合部２６は、Ｆ＿ＵＳＥに「１」がセットされていると判定したとき（判定結果がＹｅｓのとき）には、この配列ＦＴＰ（ｊ）の要素のデータは有効であると判断し、Ｓ３１９に処理を進める。一方、時系列照合部２６は、ここで、Ｆ＿ＵＳＥに「１」がセットされていないと判定したとき（判定結果がＮｏのとき）には、この配列ＦＴＰ（ｊ）の要素のデータは無効であると判断してＳ３１６に処理を戻し、上述したＳ３１６以降の処理を繰り返す。
【０１２３】
次に、Ｓ３１９では、以下の処理を時系列照合部２６が行う。
まず、時系列照合部２６は、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｊ）を参照し、要素Ｆ＿ＰＸ及びＦ＿ＰＹの値を取得して変数ｐｘ２及びｐｙ２にそれぞれ代入する処理を行う。そして、次に、時系列照合部２６は、下記の式の計算を実行してＬの値を算出する処理を行う。
Ｌ＝｛（ｐｘ２−ｐｘ１）2 ＋（ｐｙ２−ｐｙ１）2 ｝1/2 
時系列照合部２６は、Ｓ３１９において、以上の処理を行う。
【０１２４】
次に、Ｓ３２０では、Ｓ３２０の処理により算出されたＬの値が、前述した距離ｃｃＲよりも小さいか否かを判定する処理を時系列照合部２６が行う。ここで、時系列照合部２６は、Ｌが距離ｃｃＲよりも小さいと判定したとき（判定結果がＹｅｓのとき）には、図１３Ａの処理により見つかった対応点の近傍（距離がｃｃＲ以内）に他の特徴点が位置していたと判断してＳ３２１に処理を進める。一方、時系列照合部２６は、ここで、Ｌが距離ｃｃＲ以上であると判定したとき（判定結果がＮｏのとき）には、Ｓ３１６に処理を戻し、上述したＳ３１６以降の処理を繰り返す。
【０１２５】
Ｓ３２１では、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）が保持している配列ＦＴＰ（ｊ）における要素Ｆ＿ＵＳＥを「０」にセットする処理を時系列照合部２６が行い、その後はＳ３１６に処理を戻し、上述したＳ３１６以降の処理を繰り返す。
【０１２６】
以上までの処理が時系列照合処理である。
次に、上述した時系列照合処理におけるＳ３０６（図１３Ａ）の処理において用いられているＭＡＴＣＨ関数の具体的な処理手順について、図１４に図解したフローチャートを参照しながら説明する。
【０１２７】
図１４のフローチャートでは、２つの映像の類似度の高さを表す指標として、広く知られているＳＡＤ（Sum of Absolute Difference：差の絶対値和）を用いるが、他の指標を用いてもよい。ＳＡＤは、２つの映像の類似度が高いほど、その値が小さくなる。
【０１２８】
まず、この図１４のフローチャートで使用されている定数について説明する。
ＳＲＣＨｘ及びＳＲＣＨｙは、現時刻フレームの映像上における前述した比較対象の部分映像の探索範囲に関する定数である。本実施例では、座標（ｐｘ１、ｐｙ１）の位置を中心とし、ｘ方向に（ＳＲＣＨｘ×２＋１）画素、ｙ方向に（ＳＲＣＨｙ×２＋１）画素の矩形を、この探索範囲とする。これらの具体的な値は予め定めておくものとし、例えば、ＳＲＣＨｘを２０画素とし、ＳＲＣＨｙを５画素とする。
【０１２９】
ＲＥＧｘ及びＲＥＧｙは、前時刻フレームの映像上における特徴点を中心とした前述の矩形領域映像のサイズに関する定数である。本実施例では、座標（ｐｘ１、ｐｙ１）の位置を中心とし、ｘ方向に（ＲＥＧｘ×２＋１）画素、ｙ方向に（ＲＥＧｙ×２＋１）画素の矩形領域の映像を、矩形領域映像とする。これらの具体的な値は予め定めておくものとし、例えば、ＲＥＧｘを５画素とし、ＲＥＧｙを５画素とする。
【０１３０】
ＴｈＳは、前述した比較処理により求める類似度の最高値との間での大小判定が行われる閾値であり、この値は、例えば１５×（ＲＥＧｘ×２＋１）×（ＲＥＧｙ×２＋１）などと、予め定めておくものとする。
【０１３１】
図１４において、Ｓ３３１では、変数Ｓｍｉｎに初期値「１０００００」を代入すると共に、下記の式の計算を行って変数ｉの値を初期設定する処理を時系列照合部２６が行う。
ｉ＝ｐｙ１−ＳＲＣＨｙ−１
なお、上記の式におけるｐｙ１は、ＭＡＴＣＨ関数が呼ばれたときの引数のひとつである。
【０１３２】
次に、Ｓ３３２では、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入する処理を時系列照合部２６が行う。
次に、Ｓ３３３では、この処理時点での変数ｉの値が、ｐｙ１とＳＲＣＨｙとの和の値以下であるか否かを判定する処理を時系列照合部２６が行う。時系列照合部２６は、ここで、ｉがｐｙ１とＳＲＣＨｙとの和以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ３３４に処理を進め、ｉがｐｙ１とＳＲＣＨｙとの和よりも大きいと判定したとき（判定結果がＮｏのとき）にはＳ３４６に処理を進める。
【０１３３】
次に、Ｓ３３４では、下記の式の計算を行って変数ｊの値を設定する処理を時系列照合部２６が行う。
ｊ＝ｐｘ１−ＳＲＣＨｘ−１
なお、上記の式におけるｐｘ１は、ＭＡＴＣＨ関数が呼ばれたときの引数のひとつである。
【０１３４】
次に、Ｓ３３５では、この処理時点での変数ｊの値に「１」を加算した結果の値を改めて変数ｊに代入する処理を時系列照合部２６が行う。
次に、Ｓ３３６では、この処理時点での変数ｊの値が、ｐｘ１とＳＲＣＨｘとの和の値以下であるか否かを判定する処理を時系列照合部２６が行う。時系列照合部２６は、ここで、ｊがｐｘ１とＳＲＣＨｘとの和以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ３３７に処理を進める。一方、時系列照合部２６は、ここで、ｊがｐｘ１とＳＲＣＨｘとの和よりも大きいと判定したとき（判定結果がＮｏのとき）には、Ｓ３３２へと処理を戻して前述した処理を繰り返す。
【０１３５】
次に、Ｓ３３７では、下記の式の計算を行って変数ｉｉの値を設定すると共に、変数Ｓの値に「０」を代入する処理を時系列照合部２６が行う。
ｉｉ＝−ＲＥＧｙ−１
次に、Ｓ３３８では、この処理時点での変数ｉｉの値に「１」を加算した結果の値を改めて変数ｉｉに代入する処理を時系列照合部２６が行う。
【０１３６】
次に、Ｓ３３９では、この処理時点での変数ｉｉの値が、定数ＲＥＧｙ以下であるか否かを判定する処理を時系列照合部２６が行う。時系列照合部２６は、ここで、ｉｉがＲＥＧｙ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ３４０に処理を進め、ｉｉがＲＥＧｙよりも大きいと判定したとき（判定結果がＮｏのとき）にはＳ３４４に処理を進める。
【０１３７】
次に、Ｓ３４０では、下記の式の計算を行って変数ｊｊの値を設定する処理を時系列照合部２６が行う。
ｊｊ＝−ＲＥＧｘ−１
次に、Ｓ３４１では、この処理時点での変数ｊｊの値に「１」を加算した結果の値を改めて変数ｊｊに代入する処理を時系列照合部２６が行う。
【０１３８】
次に、Ｓ３４２では、この処理時点での変数ｊｊの値が、定数ＲＥＧｘ以下であるか否かを判定する処理を時系列照合部２６が行う。時系列照合部２６は、ここで、ｊｊがＲＥＧｘ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ３４３に処理を進める。一方、時系列照合部２６は、ここで、ｊｊがＲＥＧｘよりも大きいと判定したとき（判定結果がＮｏのとき）にはＳ３３８へ処理を戻して前述した処理を繰り返す。
【０１３９】
次に、Ｓ３４３では、以下の処理を時系列照合部２６が行う。
まず、時系列照合部２６は、前時刻フレームの映像データＤＢＰ（Ｐ＿ＬＡＳＴ）における座標（ｐｘ＋ｊｊ，ｐｙ＋ｉｉ）の位置の画素値を取得して変数ｖ１に代入する。
【０１４０】
次に、時系列照合部２６は、現時刻フレームの映像データＤＢＰ（Ｐ＿ＣＵＲ）における座標（ｊ＋ｊｊ，ｉ＋ｉｉ）の位置の画素値を取得して変数ｖ２に代入する。
その後、時系列照合部２６は、この変数ｖ１と変数ｖ２との値の差の絶対値を算出し、この算出結果と、この処理時点での変数Ｓの値とを加算した結果の値を改めて変数Ｓに代入する。
【０１４１】
時系列照合部２６は、Ｓ３４３において以上の処理を行い、その後はＳ３４１へ処理を戻して前述した処理を繰り返す。この処理の繰り返しにより、前時刻フレームの映像上における座標（ｐｘ１、ｐｙ１）を中心位置とする矩形領域映像と、現時刻フレームの映像上における座標（ｊ、ｉ）を中心位置とする矩形領域映像とのＳＡＤの値が、変数Ｓの値として得られる。
【０１４２】
次に、Ｓ３４４では、この処理時点での変数Ｓの値が、この処理時点での変数Ｓｍｉｎの値よりも小さいか否かを判定する処理を時系列照合部２６が行う。時系列照合部２６は、ここで、ＳがＳｍｉｎよりも小さいと判定したとき（判定結果がＹｅｓのとき）にはＳ３４５に処理を進める。一方、時系列照合部２６は、ここで、ＳがＳｍｉｎ以上であると判定したとき（判定結果がＮｏのとき）にはＳ３３５へ処理を戻して前述した処理を繰り返す。
【０１４３】
次に、Ｓ３４５では、変数Ｓｍｉｎ、変数ｉｍｉｎ、及び変数ｊｍｉｎに、この処理時点での変数Ｓの値、変数ｉの値、及び変数ｊの値を、それぞれ代入する処理を時系列照合部２６が行い、その後はＳ３３５へ処理を戻して前述した処理を繰り返す。この処理の繰り返しにより、ＳＡＤの値が最小となる（すなわち、類似度が最大である）矩形領域映像の現時刻フレーム上の位置を表す座標（ｉｍｉｎ、ｊｍｉｎ）が得られる。
【０１４４】
次に、Ｓ３４６では、この処理時点での変数Ｓｍｉｎの値が、前述した定数閾値ＴｈＳ以下であるか否かを判定する処理を時系列照合部２６が行う。時系列照合部２６は、ここで、ＳｍｉｎがＴｈＳ以下であると判定したとき（判定結果がＹｅｓのとき）には、類似度が最大である矩形領域映像の当該類似度が所定の閾値以上の高さであると判断して、Ｓ３４７に処理を進める。一方、時系列照合部２６は、ここで、ＳｍｉｎがＴｈＳよりも大きいと判定したとき（判定結果がＮｏのとき）には、類似度が最大である矩形領域映像の当該類似度が所定の閾値未満の高さであると判断して、Ｓ３４８に処理を進める。
【０１４５】
次に、Ｓ３４７では、変数ｐｘ２及び変数ｐｙ２に、この処理時点での変数ｊｍｉｎ及び変数ｉｍｉｎの値を、それぞれ代入すると共に、変数ＲＥＳに値「１」を代入する処理を時系列照合部２６が行い、その後はＳ３４９に処理を進める。
次に、Ｓ３４８では、変数ＲＥＳに値「０」を代入する処理を時系列照合部２６が行う。
【０１４６】
次に、Ｓ３４９では、ＭＡＴＣＨ関数の戻り値として、この処理時点での変数ｐｘ２、ｐｙ２、及びＲＥＳの各値を、ＭＡＴＣＨ関数の戻り値として返す処理を時系列照合部２６が行い、その後はこの図１４の処理が終了する。
ＭＡＴＣＨ関数は以上のように処理される。
【０１４７】
次に、図１０の処理におけるＳ１０８の処理である、キーフレーム更新処理の具体的な処理内容について、図１５に図解したフローチャートを参照しながら説明する。
【０１４８】
図１５において、まず、Ｓ４０１では、図１０の処理に関連して説明したフラグＩＮＩＴ＿Ｆに「１」がセットされているか否かを判定する処理をキーフレーム更新部２９が行う。キーフレーム更新部２９は、ここで、ＩＮＩＴ＿Ｆに「１」がセットされていると判定したとき（判定結果がＹｅｓのとき）には、実行中のキーフレーム更新処理は図１０の処理が開始されてから２回目以降の実行であると判断してＳ４０４に処理を進める。一方、キーフレーム更新部２９は、ここで、ＩＮＩＴ＿Ｆに「１」がセットされてないと判定したとき（判定結果がＮｏのとき）には、実行中のキーフレーム更新処理は図１０の処理の開始後に初めて実行されていると判断してＳ４０２に処理を進める。
【０１４９】
次に、Ｓ４０２では、関数ＲＥＧＩＳＴ＿ＤＢＫ（ＤＢＦ（Ｐ＿ＣＵＲ），ＤＢＫ＿Ｐ）を呼び出す処理をキーフレーム更新部２９が行う。なお、この処理時点における変数ＤＢＫ＿Ｐの値は「１」である。
【０１５０】
ＲＥＧＩＳＴ＿ＤＢＫ関数は、ＤＢＦ（ＤＢＦ（Ｐ＿ＣＵＲ）及びＤＢＫ＿Ｐを引数とする場合に、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）で保持されている特徴点についての情報を、バッファメモリＤＢＫの配列ＫＥＹＦ（ＤＢＫ＿Ｐ）に登録する関数である。このＲＥＧＩＳＴ＿ＤＢＫ関数の具体的な処理手順については後述する。
【０１５１】
次に、Ｓ４０３では、バッファメモリＤＢＫの配列ＫＥＹＦ（ＤＢＫ＿Ｐ）における要素Ｋ＿Ｒ及びＫ＿Ｔに、それぞれ、単位行列Ｉ及び値「０」を代入する処理をキーフレーム更新部２９が行う。その後はこのキーフレーム更新処理を終了して図１０に処理を戻す。
【０１５２】
Ｓ４０４からＳ４０６にかけての処理は、直近キーフレームの撮影時刻から現時刻までの期間における移動体の回転量Ｒｃ及び並進量Ｔｃを算出する処理である。
キーフレームＤＢ３３で保持されている直近キーフレームについての情報である配列ＫＥＹＦ（ＤＢＫ＿Ｐ）のうちの要素Ｋ＿Ｒ及びＫ＿Ｔには、当該キーフレームの撮影時刻から、前時刻フレームの撮影時刻までの移動体の移動量が格納されている。また、図１０のＳ１０７の処理において移動量算出部２８により取得された回転量Ｒｓ及び並進量Ｔｓは、前時刻フレームの撮影時刻から現時刻フレームの撮影時刻までの移動体の移動量を表している。ここで、直近キーフレームの撮影時の座標系をＸ１とし、前時刻フレームの撮影時の座標系をＸ２とし、現時刻フレームの撮影時の座標系をＸ３とする。このとき、以下の関係が成り立つ。
Ｘ２＝Ｋ＿Ｒ×Ｘ１＋Ｋ＿Ｔ
Ｘ３＝Ｒｓ×Ｘ２＋Ｔｓ
【０１５３】
よって、
Ｘ３＝Ｒｓ×（Ｋ＿Ｒ×Ｘ１＋Ｋ＿Ｔ）＋Ｔｓ
＝Ｒｓ×Ｋ＿Ｒ×Ｘ１＋Ｒｓ×Ｋ＿Ｔ＋Ｔｓ
となる。
【０１５４】
ここで、
Ｘ３＝Ｒｃ×Ｘ１＋Ｔｃ
と置くと、
Ｒｃ＝Ｒｓ×Ｋ＿Ｒ
Ｔｃ＝Ｒｓ×Ｋ＿Ｔ＋Ｔｓ
が成立する。
【０１５５】
Ｓ４０４からＳ４０６にかけての処理は、上記の式の計算を行ってＲｃ及びＴｃの値を得る処理である。
Ｓ４０４では、この処理時点におけるバッファメモリＤＢＫの配列ＫＥＹＦ（ＤＢＫ＿Ｐ）における要素Ｋ＿Ｒ及びＫ＿Ｔの値を、回転量変数Ｒｕ及び並進量変数Ｔｕに代入する処理をキーフレーム更新部２９が行う。
【０１５６】
次に、Ｓ４０５では、下記の２つの式の計算を行ってＲｃ及びＴｃを算出する処理をキーフレーム更新部２９が行う。
Ｒｃ＝Ｒｓ×Ｒｕ
Ｔｃ＝Ｒｓ×Ｔｕ＋Ｔｓ
【０１５７】
次に、Ｓ４０６では、バッファメモリＤＢＫの配列ＫＥＹＦ（ＤＢＫ＿Ｐ）における要素Ｋ＿Ｒ及びＫ＿Ｔに、それぞれＲｃ及びＴｃの値を代入して更新する処理をキーフレーム更新部２９が行う。
【０１５８】
次に、Ｓ４０７では、Ｓ４０５の処理により算出された移動体の並進量Ｔｃが、予め定めておいた並進量閾値ＴｈＴｃ以上となったか否かを判定する処理をキーフレーム更新部２９が行う。キーフレーム更新部２９は、ここで、ＴｃがＴｈＴｃ以上となったと判定したとき（判定結果がＹｅｓのとき）にはＳ４０８に処理を進める。一方、キーフレーム更新部２９は、ここで、Ｔｃが依然としてＴｈＴｃ未満であると判定したとき（判定結果がＮｏのとき）には、このキーフレーム更新処理を終了して図１０に処理を戻す。
【０１５９】
Ｓ４０８では、関数ＲＥＧＩＳＴ＿ＤＢＫ（ＤＢＦ（Ｐ＿ＣＵＲ），ＤＢＫ＿Ｐ＋１）を呼び出す処理をキーフレーム更新部２９が行う。この処理により、現時刻フレームがキーフレームとして新規に登録される。ＲＥＧＩＳＴ＿ＤＢＫ関数の具体的な処理手順については後述する。
【０１６０】
次に、Ｓ４０９では、この処理時点での変数ＤＢＫ＿Ｐの値に「１」を加算した結果の値を改めて変数ＤＢＫ＿Ｐに代入する処理をキーフレーム更新部２９が行う。
次に、Ｓ４１０では、この処理時点での変数ＤＢＫ＿Ｐの値が、前述した個数ＮＫよりも大きくなったか否かを判定する処理をキーフレーム更新部２９が行う。キーフレーム更新部２９は、ここで、ＤＢＫ＿ＰがＮＫよりも大きくなったと判定したとき（判定結果がＹｅｓのとき）にはＳ４１１に処理を進める。一方、キーフレーム更新部２９は、ここで、ＤＢＫ＿ＰがＮＫ以下であると判定したとき（判定結果がＮｏのとき）には、このキーフレーム更新処理を終了して図１０に処理を戻す。
【０１６１】
次に、Ｓ４１１では、変数ＤＢＫ＿Ｐに改めて「１」を代入する処理をキーフレーム更新部２９が行い、その後はこのキーフレーム更新処理を終了して図１０に処理を戻す。
以上までの処理がキーフレーム更新処理である。
【０１６２】
次に、上述したキーフレーム更新処理におけるＳ４０２及びＳ４０８の処理において用いられているＲＥＧＩＳＴ＿ＤＢＫ関数の具体的な処理手順について、図１６に図解したフローチャートを参照しながら説明する。
【０１６３】
なお、以下の説明では、ＲＥＧＩＳＴ＿ＤＢＫ関数の引数を（Ｆ，Ｐ）とする。ここで、引数Ｆは、時系列特徴点ＤＢ３２が備えている２つのバッファメモリのうちの一方を特定するものである。また、引数Ｐは、キーフレームＤＢ３３が備えているバッファメモリＤＢＫにおける配列ＫＥＹＦ（）のうちの１つを特定するものである。
【０１６４】
図１６において、まず、Ｓ４２１では、引数Ｐが前述した個数ＮＫよりも大きいか否かを判定する処理をキーフレーム更新部２９が行う。キーフレーム更新部２９は、ここで、ＰがＮＫよりも大きいと判定したとき（判定結果がＹｅｓのとき）にはＳ４２２に処理を進め、ＰがＮＫ以下であると判定したとき（判定結果がＮｏのとき）にはＳ４２３に処理を進める。
【０１６５】
次に、Ｓ４２２では、引数Ｐの値を「１」とする処理をキーフレーム更新部２９が行う。
上述したＳ４２１及びＳ４２２の処理によって、新たなキーフレームの情報を追加する余裕がキーフレームＤＢ３３にない場合に、キーフレームＤＢ３３において撮影時刻が最古であるキーフレームの情報に、新たなキーフレームの情報が上書きされるようになる。従って、当該撮影時刻が最古であるキーフレームについての情報がキーフレームＤＢ３３から削除されて、新たなキーフレームの情報が追加される。
【０１６６】
次に、Ｓ４２３では、変数ｉに初期値「０」を代入する処理をキーフレーム更新部２９が行う。
次に、Ｓ４２４では、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入する処理をキーフレーム更新部２９が行う。
【０１６７】
次に、Ｓ４２５では、この処理時点での変数ｉの値が、前述した個数ＮＦ以下であるか否かを判定する処理をキーフレーム更新部２９が行う。ここで、キーフレーム更新部２９は、変数ｉの値がＮＦ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ４２６に処理を進める。一方、キーフレーム更新部２９は、変数ｉの値がＮＦを超えたと判定したとき（判定結果がＮｏのとき）には、この図１６の処理を終了させる。
【０１６８】
次にＳ４２６では、キーフレームＤＢ３３のバッファメモリＤＢＫにおける配列ＫＥＹＦ（Ｐ）の要素Ｋ＿ＦＰＴＳＥＴに、時系列特徴点ＤＢ３２のバッファメモリＦで保持されている配列ＦＴＰ（ｉ）の要素の値を格納する処理をキーフレーム更新部２９が行う。より具体的には、バッファメモリＦの配列ＦＴＰ（ｉ）のＦ＿ＩＤ、Ｆ＿ＵＳＥ、Ｆ＿ＰＸ、Ｆ＿ＰＹ、及びＦ＿ＫＣＮＴの各要素の値を、配列ＫＥＹＦ（Ｐ）の要素Ｋ＿ＦＰＴＳＥＴに代入する処理をキーフレーム更新部２９が行う。
【０１６９】
次に、Ｓ４２７では、バッファメモリＦの配列ＦＴＰ（ｉ）における要素Ｆ＿ＫＣＮＴのこの処理時点での値に「１」を加算した結果の値を、改めてこの要素Ｆ＿ＫＣＮＴに代入する処理をキーフレーム更新部２９が行う。その後、キーフレーム更新部２９は、Ｓ４２４へ処理を戻して前述した処理を繰り返す。
ＲＥＧＩＳＴ＿ＤＢＫ関数は以上のように処理される。
【０１７０】
次に、図１０の処理におけるＳ１０９の処理である、三次元位置算出処理の具体的な処理内容について、図１７に図解したフローチャートを参照しながら説明する。
まず、Ｓ５０１では、変数ｉに初期値「０」を代入する処理を三次元位置算出部３０が行う。
次に、Ｓ５０２では、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入する処理を三次元位置算出部３０が行う。
【０１７１】
次に、Ｓ５０３において、この処理時点での変数ｉの値が、前述した個数ＮＦ以下であるか否かを判定する処理を三次元位置算出部３０が行う。ここで、三次元位置算出部３０は、変数ｉの値がＮＦ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ５０４に処理を進める。一方、三次元位置算出部３０は、ここで、変数ｉの値がＮＦを超えたと判定したとき（判定結果がＮｏのとき）には、この三次元位置算出処理を終了して図１０に処理を戻す。
【０１７２】
次に、Ｓ５０４において、時系列特徴点ＤＢ３２のバッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｉ）の各要素（Ｆ＿ＩＤ、Ｆ＿ＵＳＥ、Ｆ＿ＰＸ、Ｆ＿ＰＹ、及びＦ＿ＫＣＮＴ）の値を配列変数Ｆ１に代入する処理を三次元位置算出部３０が行う。
【０１７３】
次に、Ｓ５０５では、配列変数Ｆ１における要素Ｆ＿ＵＳＥに「１」がセットされており、且つ、配列変数Ｆ１における要素Ｆ＿ＫＣＮＴの値が「０」以外の値であるか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、要素Ｆ＿ＵＳＥに「１」がセットされており、且つ、要素Ｆ＿ＫＣＮＴが「０」以外の値であると判定したとき（判定結果がＹｅｓのとき）にはＳ５０６に処理を進める。一方、三次元位置算出部３０は、ここで、要素Ｆ＿ＵＳＥに「１」がセットされていないと判定したとき、若しくは、要素Ｆ＿ＫＣＮＴの値が「０」であると判定したとき（判定結果がＮｏのとき）には、Ｓ５０２に処理を戻して前述した処理を繰り返す。
【０１７４】
このＳ５０５の判定処理の結果がＹｅｓとなる場合は、バッファメモリＤＢＦ（Ｐ＿ＣＵＲ）における配列ＦＴＰ（ｉ）で情報が保持されている、現時刻フレームに含まれている特徴点が、キーフレームにも含まれている場合である。
【０１７５】
次に、Ｓ５０６では、以下の処理を三次元位置算出部３０が行う。
まず、三次元位置算出部３０は、配列変数Ｆ１における要素Ｆ＿ＰＸ及びＦ＿ＰＹの値を取得して変数ｐｘ１及びｐｙ１にそれぞれ代入する処理を行う。そして、次に、三次元位置算出部３０は、関数ＳＥＡＲＣＨ＿ＫＥＹＦ（Ｆ１）の戻り値を取得して変数Ｋに代入する処理を行う。
【０１７６】
ＳＥＡＲＣＨ＿ＫＥＹＦ関数は配列ＦＴＰ（）を引数とする関数である。この関数は、配列ＦＴＰ（）に情報が格納されている特徴点についての情報（配列ＫＥＹＦ（））を、バッファメモリＤＢＫ上で、新規に登録されたものから逆順に遡って追いかけ、遡ることのできた最古の配列ＫＥＹＦ（）を特定する番号を戻り値とする関数である。このＳＥＡＲＣＨ＿ＫＥＹＦ関数の具体的な処理手順については後述する。
【０１７７】
次に、Ｓ５０７では、この処理時点での変数Ｋの値が「１」以上であるか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、Ｋの値が「１」以上であると判定したとき（判定結果がＹｅｓのとき）にはＳ５０８に処理を進める。一方、三次元位置算出部３０は、ここで、Ｋの値が「１」未満であると判定したとき（判定結果がＮｏのとき）には、Ｓ５０２に処理を戻して前述した処理を繰り返す。
【０１７８】
Ｓ５０８では、以下の処理を三次元位置算出部３０が行う。
まず、三次元位置算出部３０は、関数ＣＡＬＣ＿ＲＴ（Ｋ，ＤＢＫ＿Ｐ）の戻り値を取得して変数Ｒｍ及びＴｍに代入する処理を行う。
【０１７９】
次に、三次元位置算出部３０は、キーフレームＤＢ３３のバッファメモリＤＢＫにおける配列ＫＥＹＦ（Ｋ）の要素Ｆ＿ＦＰＴＳＥＴにおける配列ＦＴＰ（ｉ）の各要素の値を配列変数Ｆ２に代入する処理を行う。
【０１８０】
次に、三次元位置算出部３０は、配列変数Ｆ２におけるにおける要素Ｆ＿ＰＸ及びＦ＿ＰＹの値を取得して変数ｐｘ２及びｐｙ２にそれぞれ代入する処理を行う。
そして、次に、三次元位置算出部３０は、関数ＣＡＬＣ＿３Ｄ（ｐｘ１，ｐｙ１，ｐｘ２，ｐｙ２，Ｒｍ，Ｔｍ）の戻り値を取得して変数Ｘ、Ｙ、及びＺに代入する処理を行う。
【０１８１】
三次元位置算出部３０は、Ｓ５０８において、以上の処理を行う。
ＣＡＬＣ＿ＲＴ関数は、キーフレームについての情報が保持されている配列ＫＥＹＦ（）を特定する２つの番号を引数とする。この関数は、引数で特定される２つのキーフレームのうちの撮影時刻の前のものを撮影してから、当該２つのキーフレームのうちの撮影時刻の後のものを撮影するまでの移動体の移動量を算出して、その移動量（回転量及び並進量）を戻り値として返す関数である。従って、Ｓ５０８の処理では、変数Ｋで特定されるキーフレームを撮影してから直近キーフレームを撮影するまでの移動体の回転量及び並進量が、それぞれ変数Ｒｍ及びＴｍに代入される。このＣＡＬＣ＿ＲＴ関数の具体的な処理手順については後述する。
【０１８２】
また、ＣＡＬＣ＿３Ｄ関数は、特徴点についての、第一の撮影時刻での映像上における位置座標（ｐｘ１，ｐｙ１）と、第二の撮影時刻での映像上における位置座標（ｐｘ２，ｐｙ２）と、この２つの時刻の間の移動体の移動量（Ｒｍ，Ｔｍ）とを引数とする。この関数は、このときの特徴点の三次元位置座標を算出して戻り値として返す関数である。そのＣＡＬＣ＿３Ｄ関数による特徴点の三次元位置座標の算出の詳細については後述する。
【０１８３】
次に、Ｓ５０９では、この処理時点での変数ｉの値と、直近に実行されたＳ５０８の処理で得られた特徴点の三次元位置座標（Ｘ、Ｙ、Ｚ）とを、変数ｉで特定される特徴点についての、三次元位置座標の測定結果として出力する処理を三次元位置算出部３０が行う。その後、三次元位置算出部３０は、Ｓ５０２に処理を戻して前述した処理を繰り返す。
以上までの処理が三次元位置算出処理である。
【０１８４】
次に、上述した三次元位置算出処理におけるＳ５０６の処理において用いられているＳＥＡＲＣＨ＿ＫＥＹＦ関数の具体的な処理手順について、図１８に図解したフローチャートを参照しながら説明する。
【０１８５】
なお、以下の説明では、ＳＥＡＲＣＨ＿ＫＥＹＦ関数の引数を、配列Ｆ（バッファメモリＤＢＦが保持している配列ＦＴＰ（））とする。
まず、Ｓ５１１において、変数ｉに初期値「０」を代入すると共に、この処理時点での変数ＤＢＫ＿Ｐの値に「１」を加算した結果の値を変数Ｐに代入する処理を三次元位置算出部３０が行う。
【０１８６】
次に、Ｓ５１２では、この処理時点での変数ｉの値に「１」を加算した結果の値を改めて変数ｉに代入すると共に、この処理時点での変数Ｐの値から「１」を減算した結果の値を改めて変数Ｐに代入する処理を三次元位置算出部３０が行う。
【０１８７】
次に、Ｓ５１３では、この処理時点での変数Ｐの値が「１」よりも小さいか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、Ｐの値が「１」よりも小さくなったと判定したとき（判定結果がＹｅｓのとき）にはＳ５１４に処理を進め、Ｐの値が「１」以上の値であると判定したとき（判定結果がＮｏのとき）にはＳ５１５に処理を進める。
【０１８８】
次に、Ｓ５１４では、変数Ｐに前述した個数ＮＫの値を代入する処理を三次元位置算出部３０が行う。
次に、Ｓ５１５では、この処理時点での変数ｉの値が個数ＮＫ以下であるか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、ｉの値がＮＫ以下であると判定したとき（判定結果がＹｅｓのとき）にはＳ５１６に処理を進め、ｉの値がＮＫよりも大きくなったと判定したとき（判定結果がＮｏのとき）にはＳ５１７に処理を進める。
【０１８９】
次に、Ｓ５１６では、配列Ｆにおける要素Ｆ＿ＫＣＮＴの値が、この処理時点での変数ｉの値以上であるか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、Ｆ＿ＫＣＮＴの値が変数ｉの値以上であると判定したとき（判定結果がＹｅｓのとき）には、Ｓ５１２へ処理を戻して前述したＳ５１２以降の処理を繰り返す。一方、三次元位置算出部３０は、ここで、Ｆ＿ＫＣＮＴの値よりも変数ｉの値が大きくなったと判定したとき（判定結果がＮｏのとき）には、Ｓ５１７に処理を進める。
【０１９０】
次に、Ｓ５１７では、この処理時点での変数Ｐの値に「１」を加算した結果の値を変数Ｋに代入する処理を三次元位置算出部３０が行う。
次に、Ｓ５１８では、この処理時点での変数Ｋの値が、個数ＮＫの値よりも大きくなったか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、Ｋの値がＮＫの値よりも大きくなったと判定したとき（判定結果がＹｅｓのとき）にはＳ５１９に処理を進め、Ｋの値がＮＫの値以下であると判定したとき（判定結果がＮｏのとき）にはＳ５２０に処理を進める。
【０１９１】
次に、Ｓ５１９では、変数Ｋに「１」を代入する処理を三次元位置算出部３０が行う。
次に、Ｓ５２０では、ＳＥＡＲＣＨ＿ＫＥＹＦ関数の戻り値として、この処理時点での変数Ｋの値を返す処理を三次元位置算出部３０が行い、その後はこの図１８の処理が終了する。
ＳＥＡＲＣＨ＿ＫＥＹＦ関数は以上のように処理される。
【０１９２】
次に、上述した三次元位置算出処理におけるＳ５０８の処理において用いられているＣＡＬＣ＿ＲＴ関数の具体的な処理手順について、図１９に図解したフローチャートを参照しながら説明する。
【０１９３】
なお、以下の説明では、ＣＡＬＣ＿ＲＴ関数の２つの引数（キーフレームの情報が保持されている配列ＫＥＹＦ（）を特定する番号）のうち、撮影時刻が前であるキーフレームについての引数をＫ１とし、後であるキーフレームについての引数をＫ２とする。
【０１９４】
まず、Ｓ５３１において、変数Ｒｍに単位行例Ｉを初期値として代入すると共に、変数Ｔｍに値「０」を初期値として代入し、更に、引数Ｋ１から値「１」を減算した結果の値を変数Ｋに代入する処理を三次元位置算出部３０が行う。
【０１９５】
次に、Ｓ５３２では、この処理時点での変数Ｋの値に「１」を加算した結果の値を改めて変数Ｋに代入する処理を三次元位置算出部３０が行う。
次に、Ｓ５３３では、この処理時点での変数Ｋの値が前述した個数ＮＫの値よりも大きくなったか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、Ｋの値がＮＫの値よりも大きくなったと判定したとき（判定結果がＹｅｓのとき）にはＳ５３４に処理を進め、Ｋの値がＮＫの値以下であると判定したとき（判定結果がＮｏのとき）にはＳ５３５に処理を進める。
【０１９６】
次に、Ｓ５３４では、変数Ｋに値「１」を代入する処理を三次元位置算出部３０が行う。
次に、Ｓ５３５では、配列ＫＥＹＦ（Ｋ）における要素Ｋ＿Ｒ及びＫ＿Ｔの値を、変数Ｒｕ及びＴｕにそれぞれ代入する処理を三次元位置算出部３０が行う。
【０１９７】
次に、Ｓ５３６では、この処理時点での変数Ｒｍ、Ｔｍ、Ｒｕ、及びＴｕの値を用いて下記の２つの式の右辺の計算をそれぞれ行い、その算出結果の値を、それぞれ、変数Ｒｍ及びＴｍに改めて代入する処理を三次元位置算出部３０が行う。
Ｒｍ＝Ｒｕ×Ｒｍ
Ｔｍ＝Ｒｕ×Ｔｍ＋Ｔｕ
なお、上記の式は、前述したキーフレーム更新処理におけるＳ４０６（図１５）と同様の式である。
【０１９８】
次に、Ｓ５３７では、この処理時点での変数Ｋの値が、引数Ｋ２の値よりも小さいか否かを判定する処理を三次元位置算出部３０が行う。三次元位置算出部３０は、ここで、Ｋの値がＫ２の値よりも小さいと判定したとき（判定結果がＹｅｓのとき）には、Ｓ５３２へ処理を戻して前述したＳ５３２以降の処理を繰り返す。一方、三次元位置算出部３０は、ここで、Ｋの値がＫ２の値以上になった判定したとき（判定結果がＮｏのとき）には、Ｓ５３８に処理を進める。
【０１９９】
次に、Ｓ５３９では、この処理時点での変数Ｒｍ及びＴｍの値を、それぞれ、ＣＡＬＣ＿ＲＴ関数の戻り値である、移動体の回転量及び並進量として返す処理を三次元位置算出部３０が行い、その後はこの図１９の処理が終了する。
【０２００】
ＣＡＬＣ＿ＲＴ関数は以上のように処理される。
次に、上述した三次元位置算出処理におけるＳ５０８の処理において用いられているＣＡＬＣ＿３Ｄ関数による特徴点の三次元位置座標の算出の手法について、図２０及び図２１を用いて説明する。
【０２０１】
まず、図２０に図解するように、三次元空間に、移動体の座標系（移動体座標系）Ｏ−ＸＹＺと、当該移動体に固定されているカメラ２１の座標系（カメラ座標系）ｏ−ｘｙｚとを定義する。また、移動体座標系からカメラ座標系への変換を、Ｒｗ（回転）及びＴｗ（並進移動）で表すものとする。ここで、移動体の移動量（回転量Ｒｍ、並進量Ｔｍ）は移動体座標系で記述されるものである。
【０２０２】
今、時刻ｔ１での移動体座標系で表記した三次元位置をＸｗ１、時刻ｔ２での移動体座標系で表記した三次元位置をＸｗ２とし、これらの三次元位置の点を、カメラ座標系で表記したものを、それぞれｘｈ１及びｘｈ２とする。また、時刻ｔ１から時刻ｔ２までの移動体の移動量をＲｍ（回転量）及びＴｍ（並進量）とする。
【０２０３】
このときに、移動体座標系Ｘｗ（Ｘ，Ｙ，Ｚ）からカメラ座標系ｘｈ（ｘ、ｙ、ｚ）への変換は、Ｒｗ及びＴｗを用いて以下のように表されるものとする。
ｘｈ＝Ｒｗ×（Ｘｗ−Ｔｗ）
このとき、時刻ｔ１においてのＸｗ１とｘｈ１との関係は、
ｘｈ１＝Ｒｗ×（Ｘｗ１−Ｔｗ）
と表され、時刻ｔ２においてのＸｗ２とｘｈ２との関係は、
ｘｈ２＝Ｒｗ×（Ｘｗ２−Ｔｗ）
と表される。
【０２０４】
ここで、時刻ｔ１と時刻ｔ２との間でのＸｗ１とＸｗ２の関係が、
Ｘｗ２＝Ｒｍ×Ｘｗ１＋Ｔｍ
と表されるとすると、
Ｒｗ．ｔ×ｘｈ２＋Ｔｗ＝Ｒｍ×（Ｒｗ．ｔ×ｘｈ１＋Ｔｗ）＋Ｔｍ
＝Ｒｍ×Ｒｗ．ｔ×ｘｈ１＋Ｒｍ×Ｔｗ＋Ｔｍ
が成り立つ。なお、Ｒｗ．ｔは、Ｒｗの転置行列を表している。この式を整理すると、
ｘｈ２＝Ｒｗ×Ｒｍ×Ｒｗ．ｔ×ｘｈ１＋Ｒｗ×（Ｒｍ×Ｔｗ＋Ｔｍ−Ｔｗ）
＝（Ｒｗ×Ｒｍ×Ｒｗ．ｔ）×ｘｈ１＋Ｒｗ×（（Ｒｍ−Ｉ）×Ｔｗ＋Ｔｍ）
となる。ここで、
Ｒｖ＝Ｒｗ×Ｒｍ×Ｒｗ．ｔ
Ｔｖ＝Ｒｗ×（（Ｒｍ−Ｉ）×Ｔｗ＋Ｔｍ）
と置けば、
ｘｈ２＝Ｒｖ×ｘｈ１＋Ｔｖ
と表される。
【０２０５】
これは、カメラ座標系における、移動体が時刻ｔ１から時刻ｔ２まで回転量Ｒｍ且つ並進量Ｔｍの移動をする場合の座標間の関係を表している。
次に、図２１に図解するように、特徴点Ｐが、移動体座標系における三次元位置（Ｘ，Ｙ，Ｚ）に位置しているものとする。また、このときに、時刻ｔ１でカメラ座標系の原点から見た特徴点Ｐの撮像面上の投影位置ｎ１を（ｐｘ１、ｐｙ１）とし、時刻ｔ２でカメラ座標系の原点から見た特徴点Ｐ撮像面上の投影位置ｎ２を（ｐｘ２、ｐｙ２）とする。
【０２０６】
ここで、カメラ２１の焦点距離をｆとすれば、ｎ１及びｎ２のカメラ座標系の座標位置は、それぞれ、
ｎ１＝（ｐｘ１、ｐｙ１、ｆ）
ｎ２＝（ｐｘ２、ｐｙ２、ｆ）
となる。
【０２０７】
このとき、図２１に図解するように、特徴点Ｐは、時刻ｔ１におけるカメラ座標系の原点と投影位置ｎ１とを結ぶ直線の延長線と、時刻ｔ２におけるカメラ座標系の原点と投影位置ｎ２とを結ぶ直線の延長線との交点に位置している。ここで、特徴点Ｐの時刻ｔ１でのカメラ座標系での三次元位置をＰ１とし、このときのカメラ座標系の原点からＰ１までの距離をｌ１とする。また、特徴点Ｐの時刻ｔ２でのカメラ座標系での三次元位置をＰ２とし、このときのカメラ座標系の原点からＰ２までの距離をｌ２とする。この場合、
Ｐ１＝ｌ１×ｎ１
Ｐ２＝ｌ２×ｎ２
と表される。
【０２０８】
ところで、Ｐ１とＰ２との間には、前述したように、
Ｐ２＝Ｒｖ×Ｐ１＋Ｔｖ
の関係がある。従って、
ｌ２×ｎ２＝Ｒｖ×ｌ１×ｎ１＋Ｔｖ
が成立する。ここで、Ｎ１＝Ｒｖ×ｎ１と置くと、
ｌ２×ｎ２＝ｌ１×Ｎ１＋Ｔｖ
となる。
【０２０９】
ここで、表記を以下のように改める。
Ｎ１＝（ＮＸ１，ＮＹ１，ＮＺ１）
ｎ２＝（ｎｘ２，ｎｙ２，ｎｚ２）
【０２１０】
また、Ｔｖを、成分表示として
Ｔｖ＝（Ｔｖｘ，Ｔｖｙ，Ｔｖｚ）
と表現する。すると、上記の式は、以下のように成分表示される。
ｌ２×ｎｘ２＝ｌ１×ＮＸ１＋Ｔｖｘ
ｌ２×ｎｙ２＝ｌ１×ＮＹ１＋Ｔｖｙ
ｌ２×ｎｚ２＝ｌ１×ＮＺ１＋Ｔｖｚ
上記の連立方程式の未知数はｌ１とｌ２との２つであり、過拘束の関係にあり、一意に解が求まらないので、最小二乗法を適用して未知数の最適な解を求めるようにする。
【０２１１】
上記の連立方程式を、行列を用いて書き直すと、
【数２】
0005915130.tif 000003
となる。ここで、
【数３】
0005915130.tif 000004
と置くと、
【数４】
0005915130.tif 000005
となる。従って、最小二乗法によるＬの解は、
【数５】
0005915130.tif 000006
で与えられる。
これにより、ｌ１及びｌ２が求まる。
【０２１２】
時刻ｔ２でのカメラ座標系で表した三次元位置Ｐ２は、
Ｐ２＝ｎ２×ｌ２
で与えられる。カメラ座標系ｘｈと移動体座標系Ｘｗとの関係は
ｘｈ＝Ｒｗ×（Ｘｗ−Ｔｗ）
であるので、
【数６】
0005915130.tif 000007
となる。
【０２１３】
従って、カメラ座標系での三次元位置Ｐ２を移動体座標系で表した特徴点の座標Ｐ（Ｘ，Ｙ，Ｚ）は、
【数７】
0005915130.tif 000008
の計算を行うことで算出することができる。
【０２１４】
ＣＡＬＣ＿３Ｄ関数は、前述した座標（ｐｘ１，ｐｙ１）及び（ｐｘ２，ｐｙ２）と、移動体の移動量（Ｒｍ，Ｔｍ）とを用いて、上記の式を三次元位置算出部３０が計算し、算出される座標Ｐ（Ｘ，Ｙ，Ｚ）を戻り値として返す関数である。
【０２１５】
三次元座標測定装置１０は、以上の処理を行うことによって、カメラ２１が固定されている移動体が所定量移動する度に登録されるキーフレームでの特徴点についての情報を用いた、当該特徴点の三次元位置の座標の算出を行う。従って、移動体の動き方に違いがあっても、特徴点についての情報が当該座標の算出に使用されるキーフレームの撮影位置間の距離が、常にある程度の長さ以上確保されるので、当該座標の算出の精度が、ある程度以上に維持される。
【符号の説明】
【０２１６】
１０  三次元座標測定装置
１１  撮影部
１２  抽出部
１３  追跡部
１４  検出部
１５  登録部
１６  算出部
２１  カメラ
２２  映像入力部
２３  映像保存部
２４  特徴点抽出部
２５  特徴点登録部
２６  時系列照合部
２７  移動量センサ
２８  移動量算出部
２９  キーフレーム更新部
３０  三次元位置算出部
３１  時系列映像ＤＢ
３２  時系列特徴点ＤＢ
３３  キーフレームＤＢ
４１  ＭＰＵ
４２  ＲＯＭ
４３  ＲＡＭ
４４  インタフェース装置
４５  出力装置
４６  システムバス
４７  記録媒体駆動装置
４８  可搬型記録媒体
【図１】
0005915130.tif 000009
【図２Ａ】
0005915130.tif 000010
【図２Ｂ】
0005915130.tif 000011
【図３】
0005915130.tif 000012
【図４】
0005915130.tif 000013
【図５】
0005915130.tif 000014
【図６】
0005915130.tif 000015
【図７】
0005915130.tif 000016
【図８】
0005915130.tif 000017
【図９】
0005915130.tif 000018
【図１０】
0005915130.tif 000019
【図１１】
0005915130.tif 000020
【図１２】
0005915130.tif 000021
【図１３Ａ】
0005915130.tif 000022
【図１３Ｂ】
0005915130.tif 000023
【図１４】
0005915130.tif 000024
【図１５】
0005915130.tif 000025
【図１６】
0005915130.tif 000026
【図１７】
0005915130.tif 000027
【図１８】
0005915130.tif 000028
【図１９】
0005915130.tif 000029
【図２０】
0005915130.tif 000030
【図２１】
0005915130.tif 000031