JP5991166B2 - ３次元位置計測装置、３次元位置計測方法および３次元位置計測プログラム - Google Patents

Description

本発明は、３次元位置計測装置、３次元位置計測方法および３次元位置計測プログラムに関する。

物体の３次元位置を１台のカメラで計測する方法として、移動ステレオ方式が知られている（例えば、特許文献１参照。）。移動ステレオ方式では、３次元位置計測装置は、１台のカメラにより撮影された静止している物体を時系列で追跡する。そして、３次元位置計測装置は、互いに異なる時刻（例えば、現時刻と過去時刻）の画像上の物体の位置の差（視差量）とカメラの移動量（過去時刻のカメラの位置に対する現時刻のカメラの相対的な位置）とを用い、物体の３次元位置を三角測量の原理に基づいて算出する。

例えば、３次元位置計測装置は、画像から特徴点を抽出する。そして、３次元位置計測装置は、現時刻の画像から抽出した特徴点（以下、抽出特徴点とも称する）とメモリ等に記憶されている登録特徴点とを照合する。登録特徴点は、現時刻（例えば、ｎ番目のフレーム）より前の時刻（例えば、ｎ−１番目のフレーム）の画像から抽出された特徴点である。抽出特徴点と登録特徴点との照合では、例えば、登録特徴点に対応する抽出特徴点を探索する処理が登録特徴点毎に実行される。

登録特徴点に対応する抽出特徴点の３次元位置は、互いに対応する登録特徴点および抽出特徴点の画像上の位置の差とカメラの移動量とに基づいて算出される。そして、登録特徴点に対応する抽出特徴点は、追跡中の特徴点としてメモリ等に記憶される。一方、登録特徴点との対応が取れない抽出特徴点は、新規の特徴点としてメモリ等に記憶される。これにより、追跡中の特徴点および新規の特徴点は、登録特徴点としてメモリ等に記憶される。３次元位置計測装置は、例えば、特徴点の抽出から３次元位置の算出までの一連の処理を、カメラが移動する度に繰り返し実行する。これにより、カメラの周囲の物体までの３次元位置が算出される。

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

３次元位置計測装置は、抽出特徴点と登録特徴点との照合において、特徴点毎に探索を実行する。このため、３次元位置を算出する際の処理時間は、特徴点の数に比例して増加する。

１つの側面では、本発明の目的は、３次元位置を算出する際の処理時間の増加を抑制することである。

本発明の一形態では、互いに異なる時刻にカメラで撮影された物体の画像上の位置の差とカメラの移動量とに基づいて物体の３次元位置を計測する３次元位置計測装置は、カメラから画像を順次取得する画像取得部と、処理対象の画像から特徴点を抽出する特徴点抽出部と、登録特徴点と、処理対象の画像から抽出された特徴点である抽出特徴点とを照合する特徴点照合部と、登録特徴点に対応する抽出特徴点の３次元位置を算出し、抽出特徴点の３次元位置を確定する３次元位置確定部と、抽出特徴点のうち、３次元位置が確定した抽出特徴点を除く抽出特徴点を、次の処理対象の画像から抽出される特徴点との照合に使用する登録特徴点として、登録する特徴点登録部と、３次元位置が既に確定している前記特徴点の３次元位置を、前記カメラが搭載される移動体の移動量に応じて更新する位置算出部とを有している。

３次元位置を算出する際の処理時間の増加を抑制できる。

一実施形態における３次元位置計測装置の例を示している。 図１に示した３次元位置計測装置の動作の概要を示している。 図１に示した３次元位置計測装置の動作の一例を示している。 別の実施形態における３次元位置計測装置の一例を示している。 抽出リストの一例を示している。 登録リストの一例を示している。 確定リストの一例を示している。 登録禁止リストの一例を示している。 カメラから特徴点までの距離と誤差との関係の一例を示している。 特徴点の３次元位置の誤差の時系列推移の一例を示している。 特徴点の３次元位置の誤差を算出する際のモデルの一例を示している。 特徴点の３次元位置を確定するタイミングの一例を示している。 特徴点の画像上の位置とカメラの位置との関係の一例を示している。 新規の抽出特徴点に対する登録判定処理の概要を示している。 図４に示した３次元位置計測装置の動作の一例を示している。 図４に示した特徴点抽出部の動作の一例を示している。 図４に示した特徴点照合部の動作の一例を示している。 図４に示した３次元位置確定部の動作の一例を示している。 図４に示した投影位置算出部の動作の一例を示している。 図４に示した判定部の動作の一例を示している。

以下、実施形態を図面を用いて説明する。

図１は、一実施形態における３次元位置計測装置１０の例を示している。図１の３次元位置計測装置１０内の破線の矢印は、画像ＩＭＧ、特徴点の情報ＩＮＦ等のデータの流れの一例を示している。なお、画像ＩＭＧ、特徴点の情報ＩＮＦ等のデータの流れは、図１の例に限定されない。

３次元位置計測装置１０は、例えば、車両等の移動体に搭載されたカメラ１００で撮影された画像に基づいて、移動体の周辺の物体の３次元位置を計測する。例えば、３次元位置計測装置１０は、駐車作業を支援する駐車支援システムに適用される。駐車支援システムは、例えば、車両の前後左右に搭載された４台のカメラ１００で撮影された４枚の画像を合成して、１枚の俯瞰画像を生成する。俯瞰画像は、車両の上から見た画像である。例えば、運転者は、俯瞰画像を見ることにより、自車両の３６０度の全周囲の状況を把握できる。

なお、この種の俯瞰画像（合成画像）では、自車両周辺の物体が伸びて表示される傾向にあるため、自車両周辺の物体との位置関係が視覚的に分かり難い。このため、例えば、駐車支援システムは、３次元位置計測装置１０で計測した物体の３次元位置を俯瞰画像（合成画像）に重畳し、物体までの距離感を分りやすくする。なお、複数のカメラ１００（例えば、４台のカメラ１００）が用いられる場合、３次元位置計測装置１０は、例えば、３次元位置の計測をカメラ１００毎に独立して実行する。

例えば、３次元位置計測装置１０は、移動ステレオ方式を用いて、物体の３次元位置を算出する。移動ステレオ方式では、３次元位置計測装置１０は、１台のカメラ１００により撮影された物体を時系列で追跡する。そして、３次元位置計測装置１０は、互いに異なる時刻（例えば、現時刻と過去時刻）の画像ＩＭＧ上の物体の位置の差（視差量）とカメラ１００の移動量とを用い、物体の３次元位置を三角測量の原理に基づいて算出する。カメラ１００の移動量は、例えば、過去時刻のカメラ１００の位置に対する現時刻のカメラ１００の相対的な位置に対応する。

３次元位置計測装置１０は、例えば、画像取得部２０、特徴点抽出部３０、特徴点照合部４０、３次元位置確定部５０、特徴点登録部６０および記憶部７０、７２を有している。画像取得部２０は、画像ＩＭＧをカメラ１００から順次取得する。例えば、画像取得部２０は、カメラ１００で撮影された画像ＩＭＧを順次受け、受けた画像ＩＭＧを特徴点抽出部３０に順次出力する。画像取得部２０が受ける画像ＩＭＧは、カラー画像でもよいし、白黒画像でもよい。

特徴点抽出部３０は、画像取得部２０で取得した画像ＩＭＧ（以下、取得画像ＩＭＧとも称する）から特徴点を抽出する。すなわち、特徴点抽出部３０は、処理対象の画像ＩＭＧから特徴点を抽出する。例えば、特徴点抽出部３０は、取得画像ＩＭＧからエッジを抽出する。エッジは、例えば、明るさが急激に変化している箇所である。例えば、特徴点抽出部３０は、閾値以上のエッジ強度を有する画素を抽出する。エッジ強度は、例えば、Ｓｏｂｅｌフィルタ等のエッジ抽出フィルタを用いて抽出される。以下、特徴点抽出部３０により抽出された特徴点を抽出特徴点とも称する。

特徴点照合部４０は、例えば、抽出特徴点の情報ＩＮＦ１０を特徴点抽出部３０から受ける。また、特徴点照合部４０は、例えば、特徴点登録部６０により登録された特徴点（以下、登録特徴点とも称する）の情報ＩＮＦ０４を記憶部７０から受ける。そして、特徴点照合部４０は、登録特徴点と、処理対象の画像ＩＭＧから抽出された抽出特徴点とを照合する。

なお、情報ＩＮＦ１０は、例えば、抽出特徴点の画像ＩＭＧ上の位置、抽出特徴点を中心としたパターン情報等を有している。また、情報ＩＮＦ０４は、例えば、登録特徴点の画像ＩＭＧ上の位置、登録特徴点を中心としたパターン情報等を有している。パターン情報は、例えば、所定領域内の輝度値の情報である。例えば、特徴点照合部４０は、登録特徴点を中心とした所定領域内の輝度値（登録特徴点を中心としたパターン情報）と抽出特徴点を中心とした所定領域内の輝度値（抽出特徴点を中心としたパターン情報）との差分絶対値和や差分二乗和等を用いて、照合処理を実行する。

例えば、カメラ１００の移動により画像ＩＭＧ上の位置が移動した特徴点では、移動前の特徴点が登録特徴点に対応し、移動後の特徴点が抽出特徴点に対応する。この場合、登録特徴点に対応する抽出特徴点は、登録特徴点の移動後の特徴点である。例えば、特徴点照合部４０は、登録特徴点に対応する抽出特徴点を検出した場合、互いに対応する登録特徴点および抽出特徴点を、照合に成功した特徴点として処理する。

なお、例えば、対応する抽出特徴点がない登録特徴点は、照合に失敗した特徴点として処理される。また、例えば、対応する登録特徴点がない抽出特徴点は、新規の特徴点として処理される。登録特徴点と抽出特徴点との照合結果は、３次元位置確定部５０に転送される。例えば、特徴点照合部４０から３次元位置確定部５０に転送される情報ＩＮＦ１１は、登録特徴点と抽出特徴点との照合結果、照合に成功した登録特徴点の画像ＩＭＧ上の位置等が情報ＩＮＦ１０に追加されている。

３次元位置確定部５０は、例えば、登録特徴点と抽出特徴点との照合結果等を含む情報ＩＮＦ１１を特徴点照合部４０から受ける。そして、３次元位置確定部５０は、登録特徴点に対応する抽出特徴点の３次元位置を算出し、抽出特徴点の３次元位置を確定する。

例えば、３次元位置確定部５０は、特徴点照合部４０による照合に成功した特徴点に対して、特徴点の視差量を算出する。そして、３次元位置確定部５０は、特徴点の３次元位置を、特徴点の視差量、カメラ１００の移動量等に基づいて算出する。なお、３次元位置計測装置１０は、例えば、取得画像ＩＭＧ等の画像からカメラ１００の移動量を算出してもよいし、カメラ１００が搭載されている移動体の移動量からカメラ１００の移動量を算出してもよい。移動体の移動量は、例えば、車速や舵角等のセンサ類から直接得られる値に基づいて算出される。移動体の移動に伴いカメラ１００が移動するため、以下の説明では、例えば、移動体の移動量をカメラ１００の移動量に読み替えてもよい。

また、３次元位置確定部５０は、所定の条件に基づいて、特徴点の３次元位置を確定する。例えば、３次元位置確定部５０は、特徴点の３次元位置を算出する際に含まれる誤差に基づいて、特徴点の３次元位置を確定するか否かを判定する。以下、特徴点の３次元位置を算出する際に含まれる誤差を３次元位置の誤差とも称する。

例えば、３次元位置確定部５０は、３次元位置の誤差を特徴点毎に時系列で比較する。そして、３次元位置確定部５０は、３次元位置の誤差が最小になるタイミング（時刻）に算出した特徴点の３次元位置に基づいて、特徴点の３次元位置を確定する。これにより、特徴点として抽出された物体（物体点）の３次元位置が計測される。なお、３次元位置確定部５０は、３次元位置の誤差が予め設定された閾値以下のとき、特徴点の３次元位置を確定してもよい。あるいは、３次元位置確定部５０は、３次元位置の誤差に拘わらず、３次元位置を算出する度に、特徴点の３次元位置を確定してもよい。

３次元位置確定部５０の処理結果に基づく情報ＩＮＦ１２、ＩＮＦ１３は、記憶部７２および特徴点登録部６０にそれぞれ転送される。例えば、３次元位置確定部５０は、抽出特徴点の３次元位置を確定した場合、特徴点の確定後の３次元位置を示す情報ＩＮＦ１２を記憶部７２に記憶する。また、３次元位置確定部５０は、例えば、３次元位置が確定していない抽出特徴点の情報ＩＮＦ１３を、特徴点登録部６０に転送する。情報ＩＮＦ１３は、例えば、３次元位置が確定していない抽出特徴点の画像ＩＭＧ上の位置、抽出特徴点を中心としたパターン情報等を有している。なお、３次元位置が確定していない抽出特徴点のうち、３次元位置が算出された抽出特徴点に関しては、算出された３次元位置が情報ＩＮＦ１３に含まれてもよい。

特徴点登録部６０は、例えば、３次元位置が確定していない抽出特徴点の情報ＩＮＦ１３を、３次元位置確定部５０から受ける。そして、特徴点登録部６０は、３次元位置が確定していない抽出特徴点を登録特徴点として、登録特徴点の情報ＩＮＦ１４を記憶部７０に記憶する。例えば、情報ＩＮＦ１４は、登録特徴点の画像ＩＭＧ上の位置、登録特徴点を中心としたパターン情報等を有している。

すなわち、特徴点登録部６０は、抽出特徴点のうち、３次元位置が確定した抽出特徴点を除く抽出特徴点を、次の処理対象の画像ＩＭＧから抽出される特徴点との照合に使用する登録特徴点として、登録する。なお、特徴点登録部６０は、例えば、対応する登録特徴点がない抽出特徴点の情報を、特徴点照合部４０から受けてもよい。そして、特徴点登録部６０は、例えば、照合に成功した抽出特徴点のうちの３次元位置が確定していない抽出特徴点の情報を、３次元位置確定部５０から受けてもよい。

記憶部７０は、登録特徴点を記憶する。すなわち、特徴点抽出部３０で抽出される抽出特徴点と照合される登録特徴点が記憶部７０に記憶される。また、記憶部７２は、例えば、特徴点の確定後の３次元位置を記憶する。記憶部７０、７２は、例えば、半導体メモリ、ハードディスク、光ディスク等の記憶装置である。例えば、記憶部７０、７２は、共通の記憶装置内の異なる領域にそれぞれ割り当てられてもよいし、異なる記憶装置内にそれぞれ割り当てられてもよい。

なお、取得画像ＩＭＧから３次元位置を算出する方法については、周知の方法（例えば、特開２０１１−１１２５０７号公報等）を適時選択して用いることができる。以下、取得画像ＩＭＧから特徴点の３次元位置を算出する方法の一例を説明する。

先ず、３次元位置計測装置１０は、互いに異なる時刻（例えば、現時刻と過去時刻）の画像ＩＭＧ上の特徴点の位置とカメラ１００の投影モデルに基づいて、カメラ１００を基準にした局所的な座標系での投影面上の特徴点の位置を算出する。これにより、例えば、局所的な座標系での投影面上の特徴点の現時刻“ｔ”の位置ｎ（ｔ）と、投影面上の特徴点の過去時刻“ｔ−ｍ”の位置ｎ（ｔ−ｍ）とが算出される。そして、式（１）−式（７）に示すように、位置ｎ（ｔ）、ｎ（ｔ−ｍ）等に基づいて、カメラ１００を基準にしたカメラ座標系ｘｃ（ｔ）での特徴点の３次元位置Ｑ（ｔ）が算出される。また、式（８）に示すように、移動体を基準にした基準座標系Ｘｗ（ｔ）での特徴点の３次元位置Ｑ（ｔ）は、３次元位置Ｑ（ｔ）から算出される。

式（１）−式（８）では、変換パラメータＲｋ、Ｔｋは、基準座標系Ｘｗ（ｔ）からカメラ座標系ｘｃ（ｔ）への変換パラメータを示している。また、変換パラメータＲｔ^ｍ、Ｔｔ^ｍは、時刻“ｔ−ｍ”での基準座標系Ｘｗ（ｔ−ｍ）から時刻“ｔ”での基準座標系Ｘｗ（ｔ）への変換パラメータを示している。なお、変換パラメータＲｋ、Ｒｔ^ｍは、回転行列であり、変換パラメータＴｋ、Ｔｔ^ｍは、並進移動ベクトルである。

例えば、カメラ座標系ｘｃ（ｔ）と基準座標系Ｘｗ（ｔ）との関係は、変換パラメータＲｋ、Ｔｋを用いて、“ｘｃ（ｔ）＝Ｒｋ＊（Ｘｗ（ｔ）−Ｔｋ）”で表される。また、例えば、基準座標系Ｘｗ（ｔ）と基準座標系Ｘｗ（ｔ−ｍ）との関係は、変換パラメータＲｔ^ｍ、Ｔｔ^ｍを用いて、“Ｘｗ（ｔ）＝Ｒｔ^ｍ＊（Ｘｗ（ｔ−ｍ）−Ｔｔ^ｍ）”で表される。

このとき、回転行列Ｒｔ^ｍは、回転角ａ^ｍを用いて、式（１）で表される。

また、並進移動ベクトルＴｔ^ｍは、式（２）で表される。

ここで、式（３）、式（４）による変換を導入する。

なお、式（３）のＲｋ^Ｔは、回転行列Ｒｋの転置行列を示している。以下の式においても、上付のＴは、転置行列を示している。また、式（４）のＩは、単位行列を示している。

さらに、時刻“ｔ”での特徴点の位置ｎ（ｔ）を、Ｒｖ^ｍを用いて、式（５）のように変換する。

このとき、時刻“ｔ”のカメラ座標系ｘｃ（ｔ）での特徴点の距離ｌ（ｔ）は、式（６）で表される。

したがって、カメラ座標系ｘｃ（ｔ）での特徴点の３次元位置Ｑ（ｔ）は、式（７）で表される。

そして、基準座標系Ｘｗ（ｔ）での特徴点の３次元位置Ｐ（ｔ）は、式（８）で表される。

このように、特徴点の３次元位置Ｐ（ｔ）は、取得画像ＩＭＧから算出される。特徴点の確定後の３次元位置Ｐ（ｔ）は、例えば、記憶部７２に記憶される。なお、特徴点の確定後の３次元位置Ｑ（ｔ）が記憶部７２に記憶されてもよい。このように、３次元位置計測装置１０は、互いに異なる時刻にカメラ１００で撮影された物体の画像上の位置の差とカメラ１００の移動量とに基づいて物体の３次元位置を計測する。

なお、３次元位置計測装置１０の構成は、この例に限定されない。例えば、記憶部７０、７２は、３次元位置計測装置１０の外部に設けられてもよい。あるいは、特徴点抽出部３０、特徴点照合部４０、３次元位置確定部５０、特徴点登録部６０等は、特徴点の情報ＩＮＦ等の転送を、互いにアクセス可能な記憶装置を介して実行してもよい。

図２は、図１に示した３次元位置計測装置１０の動作の概要を示している。なお、図２は、特徴点の登録に関する動作の一例を示している。図の“ｎ”フレームの画像ＩＭＧは、現時刻の画像を示し、“ｎ−１”フレームの画像ＩＭＧは、前時刻（現時刻の前の時刻）の画像を示している。また、特徴点ＦＰｅ（ＦＰｅ１、ＦＰｅ２、ＦＰｅ３）は、抽出特徴点を示し、特徴点ＦＰｒ（ＦＰｒ１、ＦＰｒ２、ＦＰｒ３、ＦＰｒ１’、ＦＰｒ２’）は、登録特徴点を示している。例えば、登録特徴点ＦＰｒ１、ＦＰｒ２、ＦＰｒ３は、前時刻に登録された特徴点を示している。以下、抽出特徴点ＦＰｅおよび登録特徴点ＦＰｒを、特徴点ＦＰとも称する。

“ｎ”フレーム目の特徴点抽出処理では、特徴点抽出部３０は、“ｎ”フレームの画像ＩＭＧから特徴点ＦＰｅ１、ＦＰｅ２、ＦＰｅ３を抽出する。図２の例では、抽出特徴点ＦＰｅ１は、登録特徴点ＦＰｒ２の移動後の特徴点であり、抽出特徴点ＦＰｅ２は、登録特徴点ＦＰｒ３の移動後の特徴点である。

特徴点照合処理では、特徴点照合部４０は、前時刻（“ｎ−１”フレーム）で登録された登録特徴点ＦＰｒ毎に、現時刻（“ｎ”フレーム）で抽出された抽出特徴点ＦＰｅとの照合を実行する。例えば、特徴点照合部４０は、登録特徴点ＦＰｒ１に一致する抽出特徴点ＦＰｅは存在しないと判定する。照合に失敗した登録特徴点ＦＰｒ１は、追跡に失敗した特徴点として処理され、記憶部７０に記憶されない。

また、例えば、特徴点照合部４０は、登録特徴点ＦＰｒ２は抽出特徴点ＦＰｅ１に一致すると判定し、登録特徴点ＦＰｒ３は抽出特徴点ＦＰｅ２に一致すると判定する。照合に成功した抽出特徴点ＦＰｅ１、ＦＰｅ２は、追跡に成功した特徴点であり、記憶部７０に記憶される登録特徴点ＦＰｒの候補である。

例えば、追跡に成功した抽出特徴点ＦＰｅ１、ＦＰｅ２に対しては、３次元位置確定部５０により、３次元位置の算出および確定の処理が実行される。図２の例では、３次元位置確定部５０は、抽出特徴点ＦＰｅ２の３次元位置を確定する。また、例えば、現時刻では、３次元位置確定部５０は、抽出特徴点ＦＰｅ１の３次元位置を確定していない。

なお、特徴点照合部４０は、登録特徴点ＦＰｒと抽出特徴点ＦＰｅとの距離が閾値以内であるか否かを照合条件に追加してもよい。例えば、特徴点照合部４０は、登録特徴点ＦＰｒと抽出特徴点ＦＰｅとの距離が閾値より大きい場合、特徴点を中心としたパターンの比較結果に拘わらず、不一致と判定してもよい。

また、特徴点照合部４０は、３次元位置が算出されている登録特徴点ＦＰｒに対しては、登録特徴点ＦＰｒの現時刻（“ｎ”フレーム）の予測位置と抽出特徴点ＦＰｅとの距離が閾値以内であるか否かを照合条件に追加してもよい。例えば、３次元位置が算出されている登録特徴点ＦＰの現時刻（“ｎ”フレーム）の予測位置は、移動体の移動量等に基づいて算出される。

特徴点登録処理では、抽出特徴点ＦＰｅ１、ＦＰｅ２、ＦＰｅ３のうち、登録特徴点ＦＰｒとの対応が取れなかった特徴点ＦＰｅ３は、登録特徴点ＦＰｒ２’として、記憶部７０に記憶される。例えば、登録特徴点ＦＰｒ２’は、現時刻で初めて抽出された新規の特徴点である。また、登録特徴点ＦＰｒとの対応が取れた抽出特徴点ＦＰｅ１、ＦＰｅ２のうち、３次元位置が確定していない抽出特徴点ＦＰｅ１は、登録特徴点ＦＰｒ１’として、記憶部７０に記憶される。例えば、登録特徴点ＦＰｒ１’は、前時刻から継続して追跡される特徴点である。

すなわち、特徴点登録部６０は、抽出特徴点ＦＰｅ１、ＦＰｅ２、ＦＰｅ３のうち、３次元位置が確定した抽出特徴点ＦＰｅ２を除く抽出特徴点ＦＰｅ１、ＦＰｅ３を、登録特徴点ＦＰｒ１’、ＦＰｒ２’として、記憶部７０に記憶する。図の破線は、登録特徴点ＦＰｒの候補の１つ（抽出特徴点ＦＰｅ２）が登録対象から除外されことを示している。このように、登録特徴点ＦＰｒとの対応が取れた抽出特徴点ＦＰｅ１、ＦＰｅ２のうち、３次元位置が確定した抽出特徴点ＦＰｅ２は、記憶部７０に記憶されない。

これにより、この実施形態では、次時刻（現時刻の次の時刻）の特徴点照合処理の時間を短縮できる。なお、現時刻（“ｎ”フレーム）での３次元位置が確定した特徴点ＦＰｅ２の次時刻（“ｎ”フレームの次のフレーム）での３次元位置は、例えば、移動体の移動量に基づいて更新されてもよい。

図３は、図１に示した３次元位置計測装置１０の動作の一例を示している。図３の動作は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。例えば、３次元位置計測プログラム等のソフトウエアは、コンピュータに図３の動作を実行させてもよい。すなわち、３次元位置計測装置１０は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。処理Ｓ１０から処理Ｓ５０までの一連の処理は、例えば、画像ＩＭＧ毎に実行される。

処理Ｓ１０では、画像取得部２０は、画像ＩＭＧをカメラ１００から取得する。処理Ｓ２０では、特徴点抽出部３０は、処理Ｓ２０で取得した画像ＩＭＧの特徴点ＦＰを抽出する。例えば、特徴点抽出部３０は、閾値以上のエッジ強度を有する画素を取得画像ＩＭＧから抽出する。エッジ強度は、例えば、Ｓｏｂｅｌフィルタ等のエッジ抽出フィルタを用いて抽出される。

処理Ｓ３０では、特徴点照合部４０は、記憶部７０に記憶されている登録特徴点ＦＰｒと、処理Ｓ２０で抽出された抽出特徴点ＦＰｅとを照合する。すなわち、特徴点照合部４０は、互いに異なる時刻に撮影された画像ＩＭＧの特徴点ＦＰを照合する。

処理Ｓ４０では、３次元位置確定部５０は、処理Ｓ３０で照合が成功した特徴点ＦＰの３次元位置を、互いに対応する特徴点ＦＰ（抽出特徴点ＦＰｅ、登録特徴点ＦＰｒ）の視差量、カメラ１００の移動量等に基づいて算出する。そして、３次元位置確定部５０は、所定の条件に基づいて、特徴点ＦＰの３次元位置を確定する。

処理Ｓ５０では、特徴点登録部６０は、処理Ｓ２０で抽出された抽出特徴点ＦＰｅのうち、処理Ｓ４０で３次元位置が確定した抽出特徴点ＦＰｅを除く抽出特徴点ＦＰｅを、登録特徴点ＦＰｒとして記憶部７０に記憶する。すなわち、特徴点登録部６０は、例えば、現時刻の画像ＩＭＧに対する処理Ｓ５０において、次時刻の画像ＩＭＧに対する処理Ｓ３０で使用される登録特徴点ＦＰｒを登録する。

このように、この実施形態では、処理Ｓ２０で抽出された抽出特徴点ＦＰｅのうち、３次元位置が確定した抽出特徴点ＦＰｅは、記憶部７０に記憶されない。これにより、この実施形態では、現時刻の画像ＩＭＧから抽出された抽出特徴点ＦＰｅを全て登録特徴点ＦＰｒとして登録する方法に比べて、次時刻の画像ＩＭＧの特徴点照合の処理時間を短縮できる。

また、この実施形態では、３次元位置が算出されない特徴点ＦＰの数が増加することを抑制できる。例えば、照合対象の特徴点ＦＰの数を削減するために、互いに近接する複数の特徴点ＦＰの代表点のみを照合する方法では、代表点以外の特徴点ＦＰの３次元位置は、算出されない。これに対し、この実施形態では、特徴点照合の対象から除外される特徴点ＦＰは、３次元位置が確定した特徴点ＦＰである。すなわち、この実施形態では、３次元位置が算出されない特徴点ＦＰの数が増加することを抑制しつつ、照合対象の特徴点ＦＰの数を削減できる。

なお、３次元位置計測装置１０の動作は、この例に限定されない。例えば、３次元位置計測装置１０は、記憶部７２に記憶されている特徴点ＦＰの３次元位置を、移動体の移動量に基づいて更新してもよい。

以上、この実施形態では、３次元位置計測装置１０は、画像取得部２０、特徴点抽出部３０、特徴点照合部４０、３次元位置確定部５０および特徴点登録部６０を有している。画像取得部２０は、互いに異なる時刻の画像ＩＭＧをカメラ１００から順次取得する。特徴点抽出部３０は、画像取得部２０で取得した画像ＩＭＧの特徴点ＦＰを抽出する。特徴点照合部４０は、登録特徴点ＦＰｒと、特徴点抽出部３０で抽出された抽出特徴点ＦＰｅとを照合する。３次元位置確定部５０は、登録特徴点ＦＰｒに対応する抽出特徴点ＦＰｅの３次元位置を算出し、抽出特徴点ＦＰｅの３次元位置を確定する。特徴点登録部６０は、抽出特徴点ＦＰｅのうち、３次元位置が確定した抽出特徴点ＦＰｅを除く抽出特徴点ＦＰｅを、次の処理対象の画像ＩＭＧから抽出される特徴点ＦＰとの照合に使用する登録特徴点ＦＰｒとして、記憶部７０に記憶する。

したがって、この実施形態では、３次元位置が確定した抽出特徴点ＦＰｅは、次の画像ＩＭＧの特徴点照合に使用される登録特徴点ＦＰｒとして、登録されない。すなわち、この実施形態では、特徴点抽出部３０で抽出された抽出特徴点ＦＰｅを全て登録特徴点ＦＰｒとして登録する方法に比べて、照合対象の特徴点ＦＰの数を削減できる。これにより、この実施形態では、例えば、現時刻の画像ＩＭＧから抽出した抽出特徴点ＦＰｅの３次元位置が確定した場合、次時刻の画像ＩＭＧでの特徴点照合処理の時間を短縮できる。すなわち、この実施形態では、３次元位置を算出する際の処理時間の増加を抑制できる。また、この実施形態では、３次元位置が確定していない抽出特徴点ＦＰｅは、照合対象から除外されずに、登録特徴点ＦＰｒとして登録される。このため、この実施形態では、３次元位置が算出されない特徴点ＦＰの数が増加することを抑制できる。

図４は、別の実施形態における３次元位置計測装置１０Ａの一例を示している。図１−図３で説明した要素と同様の要素については、同様の符号を付し、これ等については、詳細な説明を省略する。例えば、３次元位置計測装置１０Ａは、図１に示した３次元位置確定部５０および特徴点登録部６０の代わりに、３次元位置確定部５０Ａおよび特徴点登録部６０Ａを有している。３次元位置計測装置１０Ａのその他の構成は、図１−図３で説明した実施形態と同様である。

この実施形態では、３次元位置計測装置１０Ａは、例えば、特徴点ＦＰの情報等を各リストＥＬＳＴ、ＲＬＳＴ、ＦＬＳＴ、ＡＬＳＴに登録する。図４の３次元位置計測装置１０Ａ内の破線の矢印は、画像ＩＭＧ、リストＥＬＳＴ、ＲＬＳＴ、ＦＬＳＴ、ＡＬＳＴ等のデータの流れの一例を示している。なお、図４では、リスト内の一部のデータの流れもリストと同じ符号を用いて示している。また、画像ＩＭＧ、リストＥＬＳＴ、ＲＬＳＴ、ＦＬＳＴ、ＡＬＳＴ等のデータの流れは、図４の例に限定されない。

３次元位置計測装置１０Ａは、例えば、画像取得部２０、特徴点抽出部３０、特徴点照合部４０、３次元位置確定部５０Ａ、特徴点登録部６０Ａおよび記憶部７０、７２を有している。画像取得部２０は、画像ＩＭＧをカメラ１００から順次取得する。

特徴点抽出部３０は、画像取得部２０から取得画像ＩＭＧを受け、取得画像ＩＭＧの特徴点ＦＰを抽出する。例えば、特徴点抽出部３０は、取得画像ＩＭＧの各画素のエッジ強度を算出する。そして、特徴点抽出部３０は、エッジ強度が閾値以上の箇所を、抽出特徴点ＦＰｅとして抽出リストＥＬＳＴに追加する。また、特徴点抽出部３０は、抽出特徴点ＦＰｅの位置等を登録した抽出リストＥＬＳＴを、特徴点照合部４０に転送する。

特徴点照合部４０は、例えば、抽出特徴点ＦＰｅの位置等が登録された抽出リストＥＬＳＴを受ける。また、特徴点照合部４０は、例えば、登録特徴点ＦＰｒの位置等が登録された登録リストＲＬＳＴを記憶部７０から受ける。そして、特徴点照合部４０は、登録特徴点ＦＰｒと抽出特徴点ＦＰｅとを照合し、照合結果を抽出リストＥＬＳＴに反映する。また、特徴点照合部４０は、照合結果を反映した抽出リストＥＬＳＴを、３次元位置確定部５０Ａに転送する。

３次元位置確定部５０Ａは、照合結果が反映された抽出リストＥＬＳＴを受ける。そして、３次元位置確定部５０Ａは、照合に成功した特徴点ＦＰの３次元位置を、抽出リストＥＬＳＴの情報、カメラ１００の移動量等に基づいて算出する。また、３次元位置確定部５０Ａは、例えば、算出した３次元位置の誤差が最小になるタイミング（時刻）の３次元位置に基づいて、特徴点ＦＰの３次元位置を確定する。

例えば、３次元位置確定部５０Ａは、３次元位置算出部５２、誤差算出部５４および確定部５６を有している。３次元位置算出部５２は、例えば、照合に成功した特徴点ＦＰの視差量を抽出リストＥＬＳＴの情報に基づいて算出する。そして、３次元位置算出部５２は、特徴点ＦＰの３次元位置を、特徴点の視差量、カメラ１００の移動量等に基づいて算出する。

誤差算出部５４は、特徴点ＦＰの３次元位置を算出する際に含まれる誤差を算出する。例えば、誤差算出部５４は、図１２で説明する式（１３）、式（１４）等を用いて、３次元位置の誤差を算出する。

確定部５６は、例えば、３次元位置の誤差を特徴点ＦＰ毎に時系列で比較する。そして、確定部５６は、３次元位置の誤差が最小になるタイミング（時刻）に算出した３次元位置に基づいて、特徴点の３次元位置を確定する。すなわち、確定部５６は、抽出特徴点ＦＰｅの３次元位置を算出する際に含まれる誤差を時系列で比較し、誤差が最小のときに算出した抽出特徴点ＦＰｅの３次元位置に基づいて抽出特徴点ＦＰｅの３次元位置を確定する。

また、確定部５６は、特徴点ＦＰの３次元位置が確定した場合、特徴点ＦＰの確定後の３次元位置等を確定リストＦＬＳＴに追加する。これにより、特徴点ＦＰの確定後の３次元位置等が確定リストＦＬＳＴに登録される。確定リストＦＬＳＴは、例えば、記憶部７２に記憶されている。

このように、３次元位置確定部５０Ａは、特徴点ＦＰの確定後の３次元位置等を確定リストＦＬＳＴに追加する。また、３次元位置確定部５０Ａは、３次元位置算出部５２、誤差算出部５４、確定部５６等の処理結果に基づいて、抽出リストＥＬＳＴを更新する。そして、３次元位置確定部５０Ａは、更新した抽出リストＥＬＳＴを特徴点登録部６０Ａに転送する。

特徴点登録部６０Ａは、例えば、現時刻の画像ＩＭＧから抽出された抽出特徴点ＦＰｅのうち、３次元位置が現時刻以前に確定した抽出特徴点ＦＰｅを除く抽出特徴点ＦＰｅを、次時刻の画像ＩＭＧで使用する登録特徴点ＦＰｒとして、記憶部７０に記憶する。また、特徴点登録部６０Ａは、例えば、移動体の移動量に基づいて、確定リストＦＬＳＴを更新する。例えば、特徴点登録部６０Ａは、投影位置算出部６２および判定部６４を有している。

投影位置算出部６２は、例えば、特徴点ＦＰの確定後の３次元位置等が登録された確定リストＦＬＳＴを、記憶部７２から受ける。そして、投影位置算出部６２は、確定リストＦＬＳＴに登録されている３次元位置を、移動体の移動量に基づいて更新する。すなわち、投影位置算出部６２は、３次元位置が既に確定している特徴点ＦＰの３次元位置を、移動体の移動量に応じて更新する。

例えば、特徴点ＦＰの更新後の３次元位置Ｐ’は、特徴点ＦＰの更新前の３次元位置Ｐ、移動体の移動量Ｒｊ、Ｔｊを用いて、式（９）で表される。移動体の移動量は、例えば、特徴点ＦＰの更新前の３次元位置Ｐの基準点（座標系の原点）と特徴点ＦＰの更新後の３次元位置Ｐ’の基準点（座標系の原点）との距離に対応している。例えば、移動量のパラメータＲｊは、回転行列であり、移動量のパラメータＴｊは、並進移動ベクトルである。

投影位置算出部６２は、確定リストＦＬＳＴに登録されている３次元位置Ｐを、移動体の移動量に基づいて算出した３次元位置Ｐ’に更新する。なお、現時刻の画像ＩＭＧから算出された３次元位置は、移動体の移動量が０のときに対応する。すなわち、投影位置算出部６２は、現時刻の画像ＩＭＧから算出された３次元位置に対する更新値の算出処理を省略できる。

また、投影位置算出部６２は、特徴点ＦＰの更新後の３次元位置Ｐ’に基づいて、特徴点ＦＰの投影位置を算出する。投影位置算出部６２により算出される投影位置は、例えば、特徴点ＦＰの画像ＩＭＧ上の位置がカメラ１００の移動に伴い移動したときの移動後の特徴点ＦＰの画像ＩＭＧ上の位置に対応している。すなわち、投影位置算出部６２は、３次元位置が既に確定している特徴点ＦＰの画像ＩＭＧ上の投影位置を、移動体の移動量に応じて更新した３次元位置Ｐ’に基づいて算出する。そして、投影位置算出部６２は、投影位置に基づいて、特徴点ＦＰの登録禁止範囲を設定する。例えば、投影位置算出部６２は、特徴点ＦＰの投影位置付近を、特徴点ＦＰの登録禁止範囲に設定する。

なお、特徴点ＦＰの登録禁止範囲は、任意の形状でよい。例えば、特徴点ＦＰの登録禁止範囲は、特徴点ＦＰの投影位置を中心とした円形でもよいし、矩形でもよい。また、特徴点ＦＰの登録禁止範囲の大きさは、固定値でもよいし、特徴点ＦＰとカメラ１００との距離に応じた可変値でもよい。

特徴点ＦＰの登録禁止範囲を示す画素位置は、登録禁止リストＡＬＳＴに登録される。例えば、投影位置算出部６２は、特徴点ＦＰの登録禁止範囲を示す画素位置を登録禁止リストＡＬＳＴに登録し、登録禁止リストＡＬＳＴを判定部６４に転送する。

判定部６４は、例えば、登録禁止リストＡＬＳＴを投影位置算出部６２から受ける。また、判定部６４は、３次元位置確定部５０Ａで更新された抽出リストＥＬＳＴを受ける。そして、判定部６４は、登録禁止リストＡＬＳＴに登録されている登録禁止範囲に新規の抽出特徴点ＦＰｅが含まれるか否かを判定する。新規の抽出特徴点ＦＰｅは、例えば、抽出リストＥＬＳＴに登録されている抽出特徴点ＦＰｅのうち、対応する登録特徴点ＦＰｒがない抽出特徴点ＦＰｅである。

新規の抽出特徴点ＦＰｅのうち、登録禁止範囲外の新規の抽出特徴点ＦＰｅは、登録特徴点ＦＰｒとして、登録リストＲＬＳＴに追加される。例えば、判定部６４は、登録禁止範囲外と判定した新規の抽出特徴点ＦＰｅを、登録特徴点ＦＰｒとして登録リストＲＬＳＴに追加する。

これにより、例えば、現時刻の画像ＩＭＧから抽出された新規の抽出特徴点ＦＰｅのうち、登録禁止範囲外の新規の抽出特徴点ＦＰｅは、次時刻の画像ＩＭＧの特徴点照合に使用される登録特徴点ＦＰｒとして、登録リストＲＬＳＴに追加される。換言すれば、登録禁止範囲に含まれる新規の抽出特徴点ＦＰｅは、登録リストＲＬＳＴに追加されない。すなわち、判定部６４は、画像ＩＭＧ上の投影位置に基づく登録禁止範囲ＰＡＲＡ内の抽出特徴点ＦＰｅを、次の処理対象の画像ＩＭＧから抽出される特徴点ＦＰとの照合に使用する登録特徴点ＦＰｒから除外する。

また、判定部６４は、照合に成功した特徴点ＦＰのうち、３次元位置が確定していない抽出特徴点ＦＰｅを、登録特徴点ＦＰｒとして登録リストＲＬＳＴに追加する。これにより、例えば、現時刻の画像ＩＭＧから抽出された追跡中の抽出特徴点ＦＰｅのうち、３次元位置が確定していない抽出特徴点ＦＰｅは、次時刻の画像ＩＭＧの特徴点照合に使用される登録特徴点ＦＰｒとして、登録リストＲＬＳＴに追加される。追跡中の抽出特徴点ＦＰｅは、例えば、抽出リストＥＬＳＴに登録されている抽出特徴点ＦＰｅのうち、照合に成功した抽出特徴点ＦＰｅである。

このように、判定部６４は、例えば、現時刻の画像ＩＭＧから抽出された抽出特徴点ＦＰｅのうち、３次元位置が現時刻以前に確定した抽出特徴点ＦＰｅを除く抽出特徴点ＦＰｅを、次時刻の画像ＩＭＧで使用する登録特徴点ＦＰｒとして、登録リストＲＬＳＴに登録する。そして、判定部６４は、登録リストＲＬＳＴを記憶部７０に記憶する。

したがって、この実施形態では、３次元位置が現時刻以前に確定した抽出特徴点ＦＰｅは、次時刻の画像ＩＭＧの特徴点照合に使用される登録特徴点ＦＰｒとして、登録されない。すなわち、この実施形態では、図１−図３で説明した３次元位置計測装置１０に比べて、照合対象の特徴点ＦＰの数を削減できる。

なお、３次元位置計測装置１０Ａの構成は、この例に限定されない。例えば、記憶部７０、７２は、３次元位置計測装置１０Ａの外部に設けられてもよい。あるいは、特徴点抽出部３０、特徴点照合部４０、３次元位置確定部５０Ａ、特徴点登録部６０Ａ等は、抽出リストＥＬＳＴ等の転送を、互いにアクセス可能な記憶装置を介して実行してもよい。例えば、抽出リストＥＬＳＴは、特徴点抽出部３０、特徴点照合部４０、３次元位置確定部５０Ａ、特徴点登録部６０Ａ等からアクセス可能な記憶装置に記憶されてもよい。

また、特徴点抽出部３０は、画像ＩＭＧ上の投影位置に基づく登録禁止範囲を特徴点ＦＰの抽出対象から除外してもよい。この場合にも、図１−図３で説明した３次元位置計測装置１０に比べて、照合対象の特徴点ＦＰの数を削減できる。さらに、この場合、図１−図３で説明した３次元位置計測装置１０に比べて、画像ＩＭＧから特徴点ＦＰを抽出する際の処理時間を短縮できる。したがって、登録禁止範囲を特徴点ＦＰの抽出対象から除外した場合にも、３次元位置を算出する際の処理時間の増加を抑制できる。

３次元位置計測装置１０Ａは、３次元位置確定部５０Ａの代わりに図１に示した３次元位置確定部５０を有してもよい。あるいは、３次元位置計測装置１０Ａは、特徴点登録部６０Ａの代わりに図１に示した特徴点登録部６０を有してもよい。また、特徴点登録部６０Ａは、登録禁止範囲を算出する機能が省かれてもよい。あるいは、投影位置算出部６２は、特徴点登録部６０の外部に設けられてもよい。

図５は、抽出リストＥＬＳＴの一例を示している。抽出リストＥＬＳＴは、例えば、抽出特徴点ＦＰｅに対応する要素ｅの集合である。なお、図５は、ｉ個目の抽出特徴点ＦＰｅに対応する要素ｅ［ｉ］の一例を示している。要素ｅ［ｉ］は、現在の特徴点位置ｕｘ、ｕｙと、過去の特徴点位置ｒｘ、ｒｙと、特徴点パターンｐｏｗと、照合成功フラグｆｌａｇ１と、３次元位置ｐｘ、ｐｙ、ｐｚと、３次元位置の誤差ｅｒｒと、カウンタ値ｃｎｔとを有している。

例えば、現在の特徴点位置ｕｘ、ｕｙは、特徴点ＦＰの現時刻の画像ＩＭＧ上の位置に対応している。すなわち、現在の特徴点位置ｕｘ、ｕｙは、抽出特徴点ＦＰｒの画像ＩＭＧ上の位置に対応している。過去の特徴点位置ｒｘ、ｒｙは、特徴点ＦＰの前時刻の画像ＩＭＧ上の位置に対応している。すなわち、過去の特徴点位置ｒｘ、ｒｙは、登録特徴点ＦＰｒの画像ＩＭＧ上の位置に対応している。特徴点パターンｐｏｗは、現時刻の画像ＩＭＧ上の特徴点ＦＰを中心としたパターン情報である。すなわち、特徴点パターンｐｏｗは、現時刻の画像ＩＭＧの位置ｕｘ、ｕｙを中心としたパターン情報である。

照合成功フラグｆｌａｇ１は、照合が成功したか否かを示すフラグである。例えば、“０”に設定された照合成功フラグｆｌａｇ１は、照合が失敗したことを示している。また、例えば、“１”あるいは“２”に設定された照合成功フラグｆｌａｇ１は、照合が成功したことを示している。なお、この実施形態では、“２”に設定された照合成功フラグｆｌａｇ１は、３次元位置が確定したことを示している。

３次元位置ｐｘ、ｐｙ、ｐｚは、特徴点ＦＰの３次元位置を示している。なお、３次元位置ｐｘ、ｐｙ、ｐｚは、例えば、カメラ１００が搭載される移動体を基準にした基準座標系で表された３次元位置Ｐでもよいし、カメラ１００を基準にしたカメラ座標系で表された３次元位置Ｑでもよい。３次元位置の誤差ｅｒｒは、特徴点ＦＰの３次元位置の誤差を示している。カウンタ値ｃｎｔは、特徴点ＦＰの３次元位置を確定するか否かの判定に使用される。

抽出リストＥＬＳＴは、例えば、特徴点抽出部３０が画像ＩＭＧの特徴点ＦＰを抽出する前に、初期化される。例えば、初期化後の抽出リストＥＬＳＴの要素数は、“０”である。そして、例えば、特徴点抽出処理により、抽出特徴点ＦＰｅに対応する要素ｅ［ｉ］が抽出リストＥＬＳＴに追加される。特徴点抽出処理では、例えば、特徴点位置ｕｘ、ｕｙ、特徴点パターンｐｏｗ等が、抽出リストＥＬＳＴの要素ｅ［ｉ］に登録される。特徴点抽出処理で登録されなかった情報（例えば、特徴点位置ｒｘ、ｒｙ）は、特徴点照合処理等により、登録される。

図６は、登録リストＲＬＳＴの一例を示している。登録リストＲＬＳＴは、例えば、登録特徴点ＦＰｒに対応する要素ｒの集合である。なお、図６は、ｉ個目の登録特徴点ＦＰｒに対応する要素ｒ［ｉ］の一例を示している。要素ｒ［ｉ］は、特徴点位置ｒｘ、ｒｙと、特徴点パターンｐｏｗと、３次元位置ｐｘ、ｐｙ、ｐｚと、３次元位置の誤差ｅｒｒと、カウンタ値ｃｎｔとを有している。

例えば、特徴点位置ｒｘ、ｒｙは、登録特徴点ＦＰｒの画像ＩＭＧ上の位置に対応している。特徴点パターンｐｏｗは、前時刻の画像ＩＭＧの位置ｒｘ、ｒｙを中心としたパターン情報である。前時刻の画像ＩＭＧは、例えば、登録特徴点ＦＰｒとして登録される抽出特徴点ＦＰｅが抽出された画像ＩＭＧである。すなわち、特徴点パターンｐｏｗは、前時刻の画像ＩＭＧ上の特徴点ＦＰを中心としたパターン情報である。

３次元位置ｐｘ、ｐｙ、ｐｚは、登録特徴点ＦＰｅの３次元位置を示している。３次元位置の誤差ｅｒｒは、登録特徴点ＦＰｅの３次元位置の誤差を示している。カウンタ値ｃｎｔは、特徴点ＦＰの３次元位置を確定するか否かの判定に使用される。登録リストＲＬＳＴの特徴点位置ｒｘ、ｒｙ等の情報は、照合結果に応じて、抽出リストＥＬＳＴに引き継がれる。

図７は、確定リストＦＬＳＴの一例を示している。確定リストＦＬＳＴは、例えば、３次元位置が確定した特徴点ＦＰに対応する要素ｆの集合である。なお、図７は、ｉ個目の特徴点ＦＰに対応する要素ｆ［ｉ］の一例を示している。要素ｆ［ｉ］は、３次元位置ｐｘ、ｐｙ、ｐｚおよび更新フラグｆｌａｇ２を有している。

例えば、３次元位置ｐｘ、ｐｙ、ｐｚは、特徴点ＦＰの確定後の３次元位置を示している。更新フラグｆｌａｇ２は、３次元位置ｐｘ、ｐｙ、ｐｚがカメラ１００（あるいは、カメラ１００が搭載されている移動体）の移動量に基づいて更新された否かを示している。例えば、“０”に設定された更新フラグｆｌａｇ２は、３次元位置ｐｘ、ｐｙ、ｐｚが更新されていないことを示している。すなわち、“０”に設定された更新フラグｆｌａｇ２は、例えば、３次元位置ｐｘ、ｐｙ、ｐｚが前時刻の移動体の位置を基準にした座標系での３次元位置であることを示している。また、例えば、“１”に設定された更新フラグｆｌａｇ２は、３次元位置ｐｘ、ｐｙ、ｐｚが更新されていることを示している。すなわち、“１”に設定された更新フラグｆｌａｇ２は、例えば、３次元位置ｐｘ、ｐｙ、ｐｚが現時刻の移動体の位置を基準にした座標系での３次元位置であることを示している。

図８は、登録禁止リストＡＬＳＴの一例を示している。登録禁止リストＡＬＳＴは、例えば、画像位置ｄｘ、ｄｙを有する要素ａの集合である。なお、図８は、ｉ個目の要素ａ［ｉ］の一例を示している。画像位置ｄｘ、ｄｙは、特徴点ＦＰの登録禁止範囲を示す情報である。例えば、登録禁止範囲の大きさが固定されている場合、画像位置ｄｘ、ｄｙは、登録禁止範囲の中心を示す座標でもよい。あるいは、登録禁止範囲の大きさが固定された矩形領域の場合、画像位置ｄｘ、ｄｙは、矩形領域の角を示す座標でもよい。登録禁止リストＡＬＳＴは、例えば、投影位置算出部６２が特徴点ＦＰの投影位置を算出する前に初期化される。例えば、初期化後の登録禁止リストＡＬＳＴの要素数は、“０”である。

図９は、カメラ１００から特徴点ＦＰまでの距離と誤差との関係の一例を示している。図９の特徴点ＦＰは、カメラ１００（１００ａ、１００ｂ）で撮影される物体点を示している。図９のケース１は、カメラ１００（１００ａ、１００ｂ）から特徴点ＦＰまでの距離がケース２に比べて遠い場合を示している。すなわち、図９のケース２は、カメラ１００（１００ａ、１００ｂ）から特徴点ＦＰまでの距離がケース１に比べて近い場合を示している。

カメラ１００の画像ＩＭＧ（ＩＭＧ１ａ、ＩＭＧ１ｂ、ＩＭＧ２ａ、ＩＭＧ２ｂ）の１画素には、一定の大きさがある。このため、特徴点ＦＰが図９の網掛けの範囲ＲＮＧ（ＲＮＧ１、ＲＮＧ２）内の場合、特徴点ＦＰの画像ＩＭＧ上の位置は変化しない。すなわち、特徴点ＦＰの３次元位置を画像ＩＭＧから算出する場合、特徴点ＦＰの３次元位置に誤差が含まれる。図９に示したように、ケース１の範囲ＲＮＧ１は、ケース２の範囲ＲＮＧ２より大きい。すなわち、カメラ１００から特徴点ＦＰまでの距離が遠くなるにしたがい、誤差は大きくなる。換言すれば、カメラ１００から特徴点ＦＰまでの距離が近くなるにしたがい、誤差は小さくなる。

図１０は、特徴点ＦＰの３次元位置の誤差の時系列推移の一例を示している。図１０の特徴点ＦＰは、カメラ１００で撮影される物体点を示している。時刻ｔ１から時刻ｔ３までの期間では、カメラ１００から特徴点ＦＰまでの距離は、カメラ１００の移動に伴い近くなる。そして、時刻ｔ３から時刻ｔ５までの期間では、カメラ１００から特徴点ＦＰまでの距離は、カメラ１００の移動に伴い遠くなる。すなわち、時刻ｔ３に、カメラ１００から特徴点ＦＰまでの距離が最も近くなる。

したがって、時刻ｔ１から時刻ｔ３までの期間では、特徴点ＦＰの３次元位置の誤差は、時間の経過に伴い小さくなる。そして、時刻ｔ３から時刻ｔ５までの期間では、特徴点ＦＰの３次元位置の誤差は、時間の経過に伴い大きくなる。すなわち、時刻ｔ３に、特徴点ＦＰの３次元位置の誤差が最も小さくなる。したがって、３次元位置確定部５０Ａは、誤差の時系列推移を観測することにより、特徴点ＦＰの３次元位置の誤差が最も小さくなるタイミング（図１０では、時刻ｔ３）を判断できる。

図１１は、特徴点ＦＰの３次元位置の誤差を算出する際のモデルの一例を示している。図１１の特徴点ＦＰは、カメラ１００（ｎ−１）、１００（ｎ）で撮影される物体点を示している。距離ｌ１は、時刻“ｎ−１”のときのカメラ１００（ｎ−１）と特徴点ＦＰとの距離である。距離ｌ２は、時刻“ｎ”のときのカメラ１００（ｎ）と特徴点ＦＰとの距離である。距離ｌｂは、時刻“ｎ−１”のときのカメラ１００（ｎ−１）と時刻“ｎ”のときのカメラ１００（ｎ）との距離である。角度ａ１は、カメラ１００（ｎ−１）と特徴点ＦＰとを結ぶ線と、カメラ１００（ｎ−１）とカメラ１００（ｎ）とを結ぶ線とがなす角である。角度ａ２は、カメラ１００（ｎ）と特徴点ＦＰとを結ぶ線と、カメラ１００（ｎ）とカメラ１００（ｎ−１）とを結ぶ線の延長線とがなす角である。

距離ｌ１、ｌ２の関係は、角度ａ１、ａ２を用いて、式（１０）で表される。また、距離ｌ１、ｌ２、ｌｂの関係は、角度ａ１、ａ２を用いて、式（１１）で表される。

さらに、式（１０）および式（１１）より、距離ｌ２は、式（１２）で表される。

距離ｌ２に対する角度ａ１の変化での微分係数は、式（１３）で表される。

ここで、角度ａ１の最大変化量をｄａとした場合、特徴点ＦＰの３次元位置の誤差ｌ２ｅｒｒは、式（１４）で表される。なお、最大変化量ｄａは、例えば、０．５画素に相当する角度であり、特徴点ＦＰの画像ＩＭＧ上の位置に基づいて算出される。

例えば、誤差算出部５４は、式（１３）、式（１４）等を用いて、３次元位置の誤差ｌ２ｅｒｒを算出する。そして、確定部５６は、例えば、誤差ｌ２ｅｒｒが最小のときに算出した抽出特徴点ＦＰｅの３次元位置に基づいて、抽出特徴点ＦＰｅの３次元位置を確定する。

図１２は、特徴点ＦＰの３次元位置を確定するタイミングの一例を示している。図の横軸は時間を示し、図の縦軸は３次元位置の誤差を示している。例えば、時刻ｔ１０に、特徴点ＦＰの照合が初めて成功し、３次元位置の誤差の計測が開始される。時刻ｔ１０では、３次元位置の誤差は、閾値より大きい。

時刻ｔ１０から時刻ｔ２０までの期間では、特徴点ＦＰの３次元位置の誤差は、時間の経過に伴い小さくなり、閾値以下になる。時刻ｔ２０以降では、特徴点ＦＰの３次元位置の誤差は、時間の経過に伴い大きくなる。すなわち、時刻ｔ２０に、特徴点ＦＰの３次元位置の誤差が最も小さくなる。

したがって、時刻ｔ１０から時刻ｔ２０までの期間では、誤差の最小値は、順次更新される。そして、時刻ｔ２０以降では、誤差の最小値は、更新されない。例えば、確定部５６は、誤差の最小値が更新されなかった時刻ｔ２０を判断開始タイミングとして、時間の計測を開始する。そして、確定部５６は、例えば、時刻ｔ２０から判断時間ＴＪＤ経過後の時刻ｔ３０まで、誤差の最小値が更新されない場合、誤差が最小の時刻ｔ２０に算出した３次元位置の精度が高いと判定する。

このように、時刻ｔ２０から判断時間ＴＪＤ経過後の時刻ｔ３０に、特徴点ＦＰの３次元位置が確定する。例えば、確定部５６は、時刻ｔ２０に算出された特徴点ＦＰの３次元位置の更新値を、特徴点ＦＰの確定後の３次元位置として確定リストＦＬＳＴに登録する。時刻ｔ２０に算出された特徴点ＦＰの３次元位置の更新値は、例えば、時刻ｔ２０に算出された特徴点ＦＰの３次元位置、移動体の時刻ｔ２０から時刻ｔ３０までの移動量等に基づいて、算出される。

図１３は、特徴点ＦＰの画像ＩＭＧ上の位置とカメラ１００の位置との関係の一例を示している。図１３のカメラ１００（ｎ）は現時刻“ｎ”のカメラ１００の位置を示し、カメラ１００（ｎ−１）は前時刻“ｎ−１”のカメラ１００の位置を示している。また、図１３の黒丸の特徴点ＦＰは、カメラ１００（ｎ−１）、１００（ｎ）で撮影される物体点を示し、網掛けの丸の特徴点ＦＰは、画像ＩＭＧに投影された物体点を示している。例えば、画像ＩＭＧ（ｎ−１）上の特徴点ＦＰ（ｎ−１）は、特徴点ＦＰを前時刻“ｎ−１”の画像ＩＭＧ（ｎ−１）に投影したときの位置を示している。そして、画像ＩＭＧ（ｎ）上の特徴点ＦＰ（ｎ）は、特徴点ＦＰを現時刻“ｎ”の画像ＩＭＧ（ｎ）に投影したときの位置を示している。

例えば、特徴点ＦＰの３次元位置が前時刻“ｎ−１”に算出されている場合、投影位置算出部６２は、特徴点ＦＰを現時刻“ｎ”の画像ＩＭＧ（ｎ）に投影したときの位置を、移動体の移動量等に基づいて予測できる。例えば、投影位置算出部６２は、上述した式（９）を用いて、特徴点ＦＰの３次元位置の更新値を算出する。特徴点ＦＰの３次元位置の更新値が時刻“ｎ”の移動体を基準にした基準座標系の場合、投影位置算出部６２は、特徴点ＦＰの３次元位置の更新値を、時刻“ｎ”のカメラ１００を基準にしたカメラ座標系に変換する。そして、投影位置算出部６２は、カメラ座標系に変換した特徴点ＦＰの３次元位置に基づいて、特徴点ＦＰの画像ＩＭＧ（ｎ）上の位置（予測投影位置）を算出する。

このように、特徴点ＦＰの時刻“ｎ”の画像ＩＭＧ（ｎ）上の予測投影位置は、前時刻“ｎ−１”に算出された特徴点ＦＰの３次元位置に基づいて予測される。したがって、前時刻“ｎ−１”に算出された特徴点ＦＰの３次元位置の誤差が大きくなるにしたがい、予測投影位置の誤差も大きくなる。このため、この実施形態では、投影位置算出部６２は、３次元位置が確定した特徴点ＦＰ（３次元位置の精度が高いと判定された特徴点ＦＰ）に対して、予測投影位置を算出する。これにより、この実施形態では、予測投影位置の誤差が大きくなることを抑制できる。予測投影位置は、例えば、特徴点ＦＰの登録禁止範囲を設定する際に使用される。

図１４は、新規の抽出特徴点ＦＰｅに対する登録判定処理の概要を示している。抽出特徴点ＦＰｅ１０、ＦＰｅ１１、ＦＰｅ１２は、対応する登録特徴点ＦＰｒがない新規の抽出特徴点ＦＰｅを示している。特徴点ＦＰｆ（ｎ−１）は、特徴点ＦＰｆの前時刻“ｎ−１”の画像ＩＭＧ上の位置に対応する位置を示している。なお、特徴点ＦＰｆは、例えば、現時刻“ｎ”より前の時刻に３次元位置が確定した特徴点ＦＰである。また、特徴点ＦＰｆ’（ｎ）は、特徴点ＦＰｆの時刻“ｎ”の画像ＩＭＧ上の予測投影位置を示している。

例えば、投影位置算出部６２は、特徴点ＦＰｆの予測投影位置（図の特徴点ＦＰｆ’（ｎ））を中心とした５画素四方の矩形領域（図の網掛け部分）を、特徴点ＦＰの登録禁止範囲ＰＡＲＡに設定する。そして、判定処理では、例えば、判定部６４は、新規の抽出特徴点ＦＰｅ１０、ＦＰｅ１１、ＦＰｅ１２の画像ＩＭＧの位置が、登録禁止範囲ＰＡＲＡに含まれるか否かを判定する。抽出特徴点ＦＰｅ１０は、登録禁止範囲ＰＡＲＡに含まれている。抽出特徴点ＦＰｅ１１、ＦＰｅ１２は、登録禁止範囲ＰＡＲＡに含まれていない。

したがって、図の判定結果に示しように、登録禁止範囲ＰＡＲＡ外の抽出特徴点ＦＰｅ１１、ＦＰｅ１２が登録リストＲＬＳＴに登録される。換言すれば、登録禁止範囲ＰＡＲＡに含まれている抽出特徴点ＦＰｅ１０は、登録リストＲＬＳＴに登録されない。すなわち、この実施形態では、例えば、３次元位置が時刻“ｎ−１”に確定した特徴点ＦＰｆは、新規の抽出特徴点ＦＰｅとして時刻“ｎ”以降に再度抽出された場合でも、登録リストＲＬＳＴに登録されない。

したがって、この実施形態では、図１−図３で説明した３次元位置計測装置１０に比べて、照合対象の特徴点ＦＰの数を削減できる。これにより、この実施形態では、画像ＩＭＧの特徴点照合処理の時間を短縮できる。なお、照合対象から除外された特徴点ＦＰ（３次元位置が確定した特徴点ＦＰ）の３次元位置は、例えば、移動体の移動量に応じて更新される。

図１５は、図４に示した３次元位置計測装置１０Ａの動作の一例を示している。図１５の動作は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。例えば、３次元位置計測プログラム等のソフトウエアは、コンピュータに図１５の動作を実行させてもよい。すなわち、３次元位置計測装置１０Ａは、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。

図１５の処理Ｓ１０は、図３の処理Ｓ１０と同様である。また、図１５の処理Ｓ２０、Ｓ３０は、抽出リストＥＬＳＴ等のリストを用いることを除いて、図３の処理Ｓ２０、Ｓ３０と同様である。図３で説明した処理については、詳細な説明を省略する。処理Ｓ１０から処理Ｓ５４までの一連の処理は、例えば、画像ＩＭＧ毎に実行される。

処理Ｓ１０では、画像取得部２０は、画像ＩＭＧをカメラ１００から取得する。処理Ｓ２０では、特徴点抽出部３０は、処理Ｓ２０で取得した画像ＩＭＧの特徴点ＦＰを抽出する。そして、特徴点抽出部３０は、抽出特徴点ＦＰｅに対応する要素ｅを抽出リストＥＬＳＴに追加する。処理Ｓ２０で追加される要素ｅには、例えば、特徴点位置ｕｘ、ｕｙ、過特徴点パターンｐｏｗ、照合成功フラグｆｌａｇ１等の情報が含まれる。これにより、抽出特徴点ＦＰｅに対応する要素ｅを有する抽出リストＥＬＳＴが生成される。

処理Ｓ３０では、特徴点照合部４０は、登録リストＲＬＳＴに登録されている登録特徴点ＦＰｒと、処理Ｓ２０で生成された抽出リストＥＬＳＴに登録されている抽出特徴点ＦＰｅとを照合する。そして、例えば、特徴点照合部４０は、照合結果に応じて、抽出リストＥＬＳＴを更新する。例えば、照合が成功した特徴点ＦＰでは、登録リストＲＬＳＴの要素ｒの情報が抽出リストＥＬＳＴの要素ｅに登録される。

処理Ｓ４２では、３次元位置確定部５０は、照合が成功した特徴点ＦＰの３次元位置を、処理Ｓ３０で更新された抽出リストＥＬＳＴに基づいて算出する。また、３次元位置確定部５０は、照合が成功した特徴点ＦＰの３次元位置の誤差ｌ２ｅｒｒを算出する。そして、３次元位置確定部５０は、処理Ｓ４２で算出した３次元位置の誤差ｌ２ｅｒｒと抽出リストＥＬＳＴに登録されている３次元位置の誤差ｅｒｒとの比較結果に応じて、抽出リストＥＬＳＴを更新する。

また、３次元位置確定部５０は、特徴点ＦＰの３次元位置を確定するか否かを、３次元位置の誤差の時系列推移に基づいて判定する。３次元位置確定部５０は、特徴点ＦＰの３次元位置を確定する場合、特徴点ＦＰの確定後の３次元位置を、確定リストＦＬＳＴに登録する。

処理Ｓ５２では、投影位置算出部６２は、確定リストＦＬＳＴに登録されている３次元位置を、移動体の移動量に基づいて更新する。例えば、投影位置算出部６２は、確定リストＦＬＳＴに登録されている３次元位置ｐｘ、ｐｙ、ｐｚを、上述した式（９）を用いて算出した３次元位置に更新する。

また、投影位置算出部６２は、特徴点ＦＰの更新後の３次元位置に基づいて、特徴点ＦＰの投影位置を算出する。そして、投影位置算出部６２は、例えば、特徴点ＦＰの投影位置付近を、特徴点ＦＰの登録禁止範囲ＰＡＲＡに設定する。例えば、投影位置算出部６２は、登録禁止範囲ＰＡＲＡを示す画素位置ｄｘ、ｄｙを有する要素ａを、登録禁止リストＡＬＳＴに追加する。

処理Ｓ５４では、判定部６４は、特徴点登録判定を実行する。例えば、判定部６４は、新規の抽出特徴点ＦＰｅを登録リストＲＬＳＴに登録するか否かを、登録禁止リストＡＬＳＴに基づいて判定する。これにより、新規の抽出特徴点ＦＰｅのうち、登録禁止範囲ＰＡＲＡ外の新規の抽出特徴点ＦＰｅは、登録特徴点ＦＰｒとして、登録リストＲＬＳＴに追加される。換言すれば、登録禁止範囲ＰＡＲＡに含まれる新規の抽出特徴点ＦＰｅは、登録リストＲＬＳＴに追加されない。また、判定部６４は、照合に成功した特徴点ＦＰのうち、３次元位置が確定していない抽出特徴点ＦＰｅを、登録特徴点ＦＰｒとして登録リストＲＬＳＴに追加する。

図１６は、図４に示した特徴点抽出部３０の動作の一例を示している。なお、図１６は、特徴点抽出部３０による特徴点抽出処理の一例を示している。図１６の動作は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。図の要素ｅ［ｉ］は、例えば、任意の要素ｅを示している。

処理Ｓ１００では、例えば、特徴点抽出部３０は、抽出リストＥＬＳＴを初期化する。初期化により、例えば、抽出リストＥＬＳＴの要素数Ｎｅは、“０”に設定される。処理Ｓ１１０では、特徴点抽出部３０は、取得画像ＩＭＧに対して、エッジ処理を実行する。例えば、特徴点抽出部３０は、取得画像ＩＭＧの画素のエッジ強度を算出する。

処理Ｓ１２０では、特徴点抽出部３０は、画素のエッジ強度が予め設定された閾値以上か否かを判定する。閾値以上のエッジ強度を有する画素は、特徴点ＦＰとして処理される。すなわち、特徴点抽出部３０は、処理Ｓ１００でエッジ強度を算出した画素が特徴点ＦＰか否かを判定する。画素のエッジ強度が閾値以上のとき（処理Ｓ１２０のＹｅｓ）、特徴点抽出部３０の動作は、処理Ｓ１３０に移る。

一方、エッジ強度が閾値より小さいとき（処理Ｓ１２０のＮｏ）、特徴点抽出部３０の動作は、処理Ｓ１５０に移る。すなわち、処理Ｓ１３０、Ｓ１４０は、画素のエッジ強度が閾値より小さいときには（処理Ｓ１２０のＮｏ）、実行されない。

処理Ｓ１３０では、特徴点抽出部３０は、特徴点ＦＰ（処理Ｓ１２０で閾値以上のエッジ強度を有すると判定された画素）に対応する要素ｅ［ｉ］を作成し、作成した要素ｅ［ｉ］を抽出リストＥＬＳＴに追加する。処理Ｓ１３０で作成される要素ｅ［ｉ］は、例えば、特徴点位置ｕｘ、ｕｙ、特徴点パターンｐｏｗおよび照合成功フラグｇｌａｇ１の情報を有している。

例えば、要素ｅ［ｉ］の特徴点位置ｕｘ、ｕｙは、処理Ｓ１２０で閾値以上のエッジ強度を有すると判定された画素の位置である。要素ｅ［ｉ］の特徴点パターンｐｏｗは、処理Ｓ１２０で閾値以上のエッジ強度を有すると判定された画素（位置ｕｘ、ｕｙ）を中心としたパターン情報である。要素ｅ［ｉ］の照合成功フラグｇｌａｇ１は、“０”である。処理Ｓ１４０では、特徴点抽出部３０は、抽出リストＥＬＳＴの要素数Ｎｅを更新する。例えば、特徴点抽出部３０は、要素数Ｎｅを“１”増加させる。

処理Ｓ１５０では、特徴点抽出部３０は、取得画像ＩＭＧの全ての画素に対してエッジ処理を実行したか否かを判定する。エッジ処理が実行されていない画素が存在するとき（処理Ｓ１５０のＮｏ）、特徴点抽出部３０の動作は、処理Ｓ１１０に戻る。一方、取得画像ＩＭＧの全ての画素に対するエッジ処理が終了しているとき（処理Ｓ１５０のＹｅｓ）、特徴点抽出部３０による特徴点抽出処理は、終了する。

このように、特徴点抽出処理が全ての画素に対して実行されることにより、抽出リストＥＬＳＴの要素ｅ［ｉ］が、特徴点ＦＰ（処理Ｓ１２０で閾値以上のエッジ強度を有すると判定された画素）毎に作成される。

図１７は、図４に示した特徴点照合部４０の動作の一例を示している。なお、図１７は、特徴点照合部４０による特徴点照合処理の一例を示している。図１７の動作は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。図の要素ｒ［ｉ］、ｅ［ｊ］は、例えば、任意の要素ｒ、ｅを示している。

処理Ｓ２００では、特徴点照合部４０は、登録リストＲＬＳＴに未処理の要素ｒが存在するか否かを判定する。登録リストＲＬＳＴに未処理の要素ｒが存在するとき（処理Ｓ２００のＹｅｓ）、特徴点照合部４０の動作は、処理Ｓ２１０に移る。一方、登録リストＲＬＳＴに未処理の要素ｒが存在しないとき（処理Ｓ２００のＮｏ）、特徴点照合部４０による特徴点照合処理は、終了する。

処理Ｓ２１０では、特徴点照合部４０は、登録リストＲＬＳＴから未処理の要素ｒ［ｉ］を選択する。処理Ｓ２２０では、特徴点照合部４０は、抽出リストＥＬＳＴに未処理の要素ｅが存在するか否かを判定する。未処理の要素ｅは、例えば、処理Ｓ２１０で選択された要素ｒ［ｉ］に対する照合処理が実行されていない要素ｅである。抽出リストＥＬＳＴに未処理の要素ｅが存在するとき（処理Ｓ２２０のＹｅｓ）、特徴点照合部４０の動作は、処理Ｓ２３０に移る。一方、抽出リストＥＬＳＴに未処理の要素ｅが存在しないとき（処理Ｓ２２０のＮｏ）、特徴点照合部４０の動作は、処理Ｓ２７０に移る。

処理Ｓ２３０では、特徴点照合部４０は、抽出リストＥＬＳＴから未処理の要素ｅ［ｊ］を選択する。処理Ｓ２４０では、特徴点照合部４０は、処理Ｓ２１０で選択した要素ｒ［ｉ］と処理Ｓ２３０で選択した要素ｅ［ｊ］との照合処理を実行する。例えば、徴点照合部４０は、要素ｒ［ｉ］の特徴点パターンｐｏｗと要素ｅ［ｊ］の特徴点パターンｐｏｗとを照合する。

処理Ｓ２５０では、特徴点照合部４０は、要素ｒ［ｉ］と要素ｅ［ｊ］との照合が成功したか否かを判定する。要素ｒ［ｉ］と要素ｅ［ｊ］との照合が成功したとき（処理Ｓ２５０のＹｅｓ）、特徴点照合部４０の動作は、処理Ｓ２６０に移る。一方、要素ｒ［ｉ］と要素ｅ［ｊ］との照合が失敗したとき（処理Ｓ２５０のＮｏ）、特徴点照合部４０の動作は、処理Ｓ２２０に戻る。

処理Ｓ２６０では、特徴点照合部４０は、処理Ｓ２１０で選択した要素ｒ［ｉ］の情報を、処理Ｓ２１０で選択した要素ｒ［ｉ］との照合に成功した要素ｅ［ｊ］に登録する。さらに、特徴点照合部４０は、処理Ｓ２１０で選択した要素ｒ［ｉ］との照合に成功した要素ｅ［ｊ］の照合成功フラグｇｌａｇ１を、“１”に更新する。これにより、照合に成功した特徴点ＦＰｅに対応する要素ｅ［ｊ］の情報が更新される。

例えば、特徴点照合部４０は、要素ｒ［ｉ］の特徴点位置ｒｘ、ｒｙ、３次元位置ｐｘ、ｐｙ、ｐｚ、３次元位置の誤差ｅｒｒおよびカウンタ値ｃｎｔを、要素ｅ［ｊ］の特徴点位置ｒｘ、ｒｙ、３次元位置ｐｘ、ｐｙ、ｐｚ、３次元位置の誤差ｅｒｒおよびカウンタ値ｃｎｔにそれぞれ登録する。

処理Ｓ２７０では、例えば、特徴点照合部４０は、登録リストＲＬＳＴから要素ｒ［ｉ］を除去する。さらに、特徴点照合部４０は、登録リストＲＬＳＴの要素数Ｎｒを更新する。例えば、特徴点照合部４０は、要素数Ｎｒを“１”減少させる。特徴点照合処理が実行される前の要素数Ｎｒは、例えば、図２０に示す特徴点登録判定により設定されている。処理Ｓ２７０の後、特徴点照合部４０の動作は、処理Ｓ２００に戻る。

すなわち、特徴点照合部４０は、登録リストＲＬＳＴの全ての要素ｒに対して、要素ｅとの照合を実行する。登録リストＲＬＳＴの全ての要素ｒに対して処理が終了したとき、要素数Ｎｒは、例えば、“０”である。

このように、特徴点照合部４０は、全ての登録特徴点ＦＰｒに対して、抽出特徴点ＦＰｅとの照合を実行する。そして、特徴点照合部４０は、登録特徴点ＦＰｒとの照合に成功した抽出特徴点ＦＰｅに対応する要素ｅ［ｊ］の情報を更新する。これにより、抽出リストＥＬＳＴが更新される。

図１８は、図４に示した３次元位置確定部５０Ａの動作の一例を示している。なお、図１８は、特徴点ＦＰの３次元位置を確定するための処理（以下、３次元位置確定処理とも称する）の一例を示している。図１８の動作は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。図の要素ｅ［ｉ］、ｆ［ｊ］は、例えば、任意の要素ｅ、ｆを示している。

処理Ｓ３００では、３次元位置確定部５０Ａは、抽出リストＥＬＳＴに未処理の要素ｅが存在するか否かを判定する。抽出リストＥＬＳＴに未処理の要素ｅが存在するとき（処理Ｓ３００のＹｅｓ）、３次元位置確定部５０Ａの動作は、処理Ｓ３１０に移る。一方、抽出リストＥＬＳＴに未処理の要素ｅが存在しないとき（処理Ｓ３００のＮｏ）、３次元位置確定部５０Ａによる３次元位置確定処理は、終了する。

処理Ｓ３１０では、３次元位置確定部５０Ａは、抽出リストＥＬＳＴから未処理の要素ｅ［ｉ］を選択する。処理Ｓ３２０では、３次元位置確定部５０Ａは、処理Ｓ３１０で選択した要素ｅ［ｉ］の照合成功フラグｇｌａｇ１が“１”である否かを判定する。すなわち、３次元位置確定部５０Ａは、要素ｅ［ｉ］が照合に成功した要素ｅか否かを判定する。

照合成功フラグｇｌａｇ１が“１”でないとき（処理Ｓ３２０のＮｏ）、３次元位置確定部５０Ａの動作は、処理Ｓ３００に戻る。一方、照合成功フラグｇｌａｇ１が“１”のとき（処理Ｓ３２０のＹｅｓ）、３次元位置確定部５０Ａの動作は、処理Ｓ３３０に移る。すなわち、照合に成功した要素ｅに対して、処理Ｓ３３０以降の処理が実行される。

処理Ｓ３３０では、３次元位置算出部５２は、要素ｅ［ｉ］に対応する特徴点ＦＰの３次元位置Ｐｘ、Ｐｙ、Ｐｚを、要素ｅ［ｉ］の情報等に基づいて算出する。処理Ｓ３３２では、誤差算出部５４は、特徴点ＦＰの３次元位置の誤差ｌ２ｅｒｒを、要素ｅ［ｉ］の情報等に基づいて算出する。

処理Ｓ３４０では、例えば、３次元位置確定部５０Ａは、処理Ｓ３３２で算出された３次元位置の誤差ｌ２ｅｒｒが要素ｅ［ｉ］に登録されている３次元位置の誤差ｅｒｒより小さいか否かを判定する。誤差ｌ２ｅｒｒが誤差ｅｒｒより小さいとき（処理Ｓ３４０のＹｅｓ）、３次元位置確定部５０Ａの動作は、処理Ｓ３５０に移る。すなわち、３次元位置の誤差の最小値が更新されるとき、３次元位置確定部５０Ａの動作は、処理Ｓ３５０に移る。一方、誤差ｌ２ｅｒｒが誤差ｅｒｒ以上のとき（処理Ｓ３４０のＮｏ）、３次元位置確定部５０Ａの動作は、処理Ｓ３６０に移る。すなわち、３次元位置の誤差の最小値が更新されないとき、処理Ｓ３６０以降の処理が実行される。

処理Ｓ３５０では、３次元位置確定部５０Ａは、処理Ｓ３１０で選択した要素ｅ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚ、３次元位置の誤差ｅｒｒおよびカウンタ値ｃｎｔを更新する。例えば、３次元位置確定部５０Ａは、要素ｅ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚを、処理Ｓ３３０で算出した３次元位置Ｐｘ、Ｐｙ、Ｐｚに更新する。また、３次元位置確定部５０Ａは、要素ｅ［ｉ］の３次元位置の誤差ｅｒｒを、処理Ｓ３３２で算出した３次元位置の誤差ｌ２ｅｒｒに更新する。そして、３次元位置確定部５０Ａは、要素ｅ［ｉ］のカウンタ値ｃｎｔを“０”に設定する。

これにより、要素ｅ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚおよび３次元位置の誤差ｅｒｒは、３次元位置の誤差が最小値のときの特徴点ＦＰに基づく情報に更新される。処理Ｓ３５０の後、３次元位置確定部５０Ａの動作は、処理Ｓ３００に戻る。

処理Ｓ３６０では、３次元位置確定部５０Ａは、処理Ｓ３１０で選択した要素ｅ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚの更新値Ｐｘ’、Ｐｙ’、Ｐｚ’を算出する。例えば、３次元位置確定部５０Ａは、更新値Ｐｘ’、Ｐｙ’、Ｐｚ’を、上述した式（９）等を用いて算出する。そして、３次元位置確定部５０Ａは、要素ｅ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚを、更新値Ｐｘ’、Ｐｙ’、Ｐｚ’に更新する。すなわち、３次元位置確定部５０Ａは、３次元位置の誤差が最小値のときに算出された３次元位置ｐｘ、ｐｙ、ｐｚを、移動体の移動量に応じて更新する。また、３次元位置確定部５０Ａは、要素ｅ［ｉ］のカウンタ値ｃｎｔを“１”増加させる。

処理Ｓ３７０では、例えば、確定部５６は、要素ｅ［ｉ］のカウンタ値ｃｎｔが所定値以上で、かつ、要素ｅ［ｉ］の３次元位置の誤差ｅｒｒが閾値以下か否かを判定する。カウンタ値ｃｎｔが所定値以上で、かつ、要素ｅ［ｉ］の３次元位置の誤差ｅｒｒが閾値以下である条件を満たさないとき（処理Ｓ３７０のＮｏ）、３次元位置確定部５０Ａの動作は、処理Ｓ３００に戻る。すなわち、カウンタ値ｃｎｔが所定値より小さいとき、あるいは、誤差ｅｒｒが閾値より大きいとき、３次元位置確定部５０Ａの動作は、処理Ｓ３００に戻る。一方、カウンタ値ｃｎｔが所定値以上で、かつ、要素ｅ［ｉ］の３次元位置の誤差ｅｒｒが閾値以下のとき（処理Ｓ３７０のＹｅｓ）、３次元位置確定部５０Ａの動作は、処理Ｓ３８０に移る。すなわち、３次元位置の誤差の最小値が所定時間更新されないとき、処理Ｓ３８０以降の処理が実行される。

処理Ｓ３８０では、例えば、確定部５６は、３次元位置ｐｘ、ｐｙ、ｐｚおよび更新フラグｆｌａｇ２の情報を有する要素ｆ［ｊ］を作成し、作成した要素ｆ［ｊ］を確定リストＦＬＳＴに追加する。例えば、確定部５６は、要素ｆ［ｊ］の３次元位置ｐｘ、ｐｙ、ｐｚに、処理Ｓ３７０の条件を満たした要素ｅ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚを登録し、要素ｆ［ｊ］の更新フラグｆｌａｇ２を“１”に設定する。

すなわち、確定部５６は、３次元位置の誤差の最小値が所定時間更新されないとき、処理Ｓ３６０で算出された更新値Ｐｘ’、Ｐｙ’、Ｐｚ’を、要素ｆ［ｊ］の３次元位置ｐｘ、ｐｙ、ｐｚに登録する。処理Ｓ３８２では、３次元位置確定部５０Ａは、確定リストＦＬＳＴの要素数Ｎｆを更新する。例えば、３次元位置確定部５０Ａは、要素数Ｎｆを“１”増加させる。

処理Ｓ３９０では、３次元位置確定部５０Ａは、処理Ｓ３７０の条件を満たした要素ｅ［ｉ］の照合成功フラグｇｌａｇ１を、“２”に更新する。すなわち、３次元位置確定部５０Ａは、処理Ｓ３８０で作成した要素ｆ［ｊ］に対応する要素ｅ［ｉ］の照合成功フラグｇｌａｇ１を、“２”に更新する。処理Ｓ３９０の後、３次元位置確定部５０Ａの動作は、処理Ｓ３００に戻る。これにより、３次元位置を確定するための処理が、抽出リストＥＬＳＴの全ての要素ｅに対して実行される。

このように、３次元位置確定部５０Ａは、３次元位置の誤差の最小値が所定時間更新されず、かつ、３次元位置の誤差が閾値以下のとき、特徴点ＦＰの３次元位置等を確定リストＦＬＳＴに追加する。なお、３次元位置確定部５０Ａの動作は、この例に限定されない。例えば、確定部５６は、処理Ｓ３７０において、要素ｅ［ｉ］の３次元位置の誤差ｅｒｒが閾値以下であるか否かの判定を省いてもよい。あるいは、確定部５６は、処理Ｓ３７０を省いてもよい。

図１９は、図４に示した投影位置算出部６２の動作の一例を示している。なお、図１９は、投影位置算出部６２による投影位置算出処理の一例を示している。図１９の動作は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。図の要素ｆ［ｉ］、ａ［ｊ］は、例えば、任意の要素ｆ、ａを示している。

処理Ｓ４００では、例えば、投影位置算出部６２は、登録禁止リストＡＬＳＴを初期化する。初期化により、例えば、登録禁止リストＡＬＳＴの要素数Ｎａは、“０”に設定される。処理Ｓ４１０では、投影位置算出部６２は、確定リストＦＬＳＴに未処理の要素ｆが存在するか否かを判定する。確定リストＦＬＳＴに未処理の要素ｆが存在するとき（処理Ｓ４１０のＹｅｓ）、投影位置算出部６２の動作は、処理Ｓ４２０に移る。一方、確定リストＦＬＳＴに未処理の要素ｆが存在しないとき（処理Ｓ４１０のＮｏ）、投影位置算出部６２による投影位置算出処理は、終了する。

処理Ｓ４２０では、投影位置算出部６２は、確定リストＦＬＳＴから未処理の要素ｆ［ｉ］を選択する。処理Ｓ４３０では、投影位置算出部６２は、処理Ｓ４２０で選択した要素ｆ［ｉ］の更新フラグｇｌａｇ２が“０”である否かを判定する。すなわち、投影位置算出部６２は、要素ｆ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚが未更新か否かを判定する。更新フラグｇｌａｇ２が“０”でないとき（処理Ｓ４３０のＮｏ）、投影位置算出部６２の動作は、処理Ｓ４６０に移る。一方、更新フラグｇｌａｇ２が“０”のとき（処理Ｓ４３０のＹｅｓ）、投影位置算出部６２の動作は、処理Ｓ４４０に移る。すなわち、要素ｆ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚが未更新のとき、処理Ｓ４４０、Ｓ４５０が実行される。

処理Ｓ４４０では、投影位置算出部６２は、処理Ｓ４２０で選択した要素ｆ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚの更新値Ｐｘ’、Ｐｙ’、Ｐｚ’を算出する。例えば、投影位置算出部６２は、更新値Ｐｘ’、Ｐｙ’、Ｐｚ’を、上述した式（９）等を用いて算出する。処理Ｓ４５０では、投影位置算出部６２は、要素ｆ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚを、更新値Ｐｘ’、Ｐｙ’、Ｐｚ’に更新する。すなわち、投影位置算出部６２は、確定リストＦＬＳＴに登録されている３次元位置ｐｘ、ｐｙ、ｐｚを、移動体の移動量に応じて更新する。処理４５０の後は、例えば、処理Ｓ４７０が実行される。

処理Ｓ４６０では、投影位置算出部６２は、処理Ｓ４３０で選択した要素ｆ［ｉ］の更新フラグｇｌａｇ２を、“０”に更新する。これにより、例えば、現時刻の画像ＩＭＧの処理で確定した特徴点ＦＰの３次元位置は、次時刻の画像ＩＭＧの投影位置算出処理で更新される。

処理Ｓ４７０では、投影位置算出部６２は、処理Ｓ４２０で選択した要素ｆ［ｉ］等に基づいて、特徴点ＦＰの投影位置を算出する。例えば、投影位置算出部６２は、要素ｆ［ｉ］の３次元位置ｐｘ、ｐｙ、ｐｚ等に基づいて、特徴点ＦＰの投影位置を算出する。そして、投影位置算出部６２は、特徴点ＦＰの登録禁止範囲ＰＡＲＡを投影位置に基づいて算出し、特徴点ＦＰの登録禁止範囲ＰＡＲＡを示す画素位置ｄｘ、ｄｙを算出する。

処理Ｓ４８０では、投影位置算出部６２は、画素位置ｄｘ、ｄｙの情報を有する要素ａ［ｊ］を作成し、作成した要素ａ［ｊ］を登録禁止リストＡＬＳＴに追加する。例えば、投影位置算出部６２は、要素ａ［ｊ］の画素位置ｄｘ、ｄｙに、処理Ｓ４７０で算出した登録禁止範囲ＰＡＲＡを示す情報を登録する。

処理Ｓ４９０では、投影位置算出部６２は、登録禁止リストＡＬＳＴの要素数Ｎａを更新する。例えば、投影位置算出部６２は、要素数Ｎａを“１”増加させる。処理Ｓ４９０の後、投影位置算出部６２の動作は、処理Ｓ４１０に戻る。これにより、投影位置を算出するための処理が、確定リストＦＬＳＴの全ての要素ｆに対して実行される。このように、投影位置算出部６２は、特徴点ＦＰの登録禁止範囲ＰＡＲＡを登録禁止リストＡＬＳＴに追加する。

図２０は、図４に示した判定部６４の動作の一例を示している。なお、図２０は、判定部６４による特徴点登録判定処理の一例を示している。図２０の動作は、ハードウエアのみで実現されてもよく、ハードウエアをソフトウエアにより制御することにより実現されてもよい。図の要素ｅ［ｉ］、ｒ［ｊ］は、例えば、任意の要素ｅ、ｒを示している。

処理Ｓ５００では、判定部６４は、抽出リストＥＬＳＴに未処理の要素ｅが存在するか否かを判定する。抽出リストＥＬＳＴに未処理の要素ｅが存在するとき（処理Ｓ５００のＹｅｓ）、判定部６４の動作は、処理Ｓ５１０に移る。一方、抽出リストＥＬＳＴに未処理の要素ｅが存在しないとき（処理Ｓ５００のＮｏ）、判定部６４による特徴点登録判定処理は、終了する。

処理Ｓ５１０では、判定部６４は、抽出リストＥＬＳＴから未処理の要素ｅ［ｉ］を選択する。処理Ｓ５２０では、判定部６４は、処理Ｓ５１０で選択した要素ｅ［ｉ］の照合成功フラグｇｌａｇ１が“１”である否かを判定する。例えば、要素ｅ［ｉ］の照合成功フラグｇｌａｇ１が“１”の場合、要素ｅ［ｉ］に対応する特徴点ＦＰは、３次元位置ｐｘ、ｐｙ、ｐｚが確定していない追跡中の特徴点ＦＰである。すなわち、要素ｅ［ｉ］の照合成功フラグｇｌａｇ１が“０”の場合、要素ｅ［ｉ］に対応する特徴点ＦＰは、３次元位置ｐｘ、ｐｙ、ｐｚが確定した特徴点か新規に抽出された特徴点のいずれかである。

照合成功フラグｇｌａｇ１が“１”でないとき（処理Ｓ５２０のＮｏ）、判定部６４の動作は、処理Ｓ５５０に移る。一方、照合成功フラグｇｌａｇ１が“１”のとき（処理Ｓ５２０のＹｅｓ）、判定部６４の動作は、処理Ｓ５３０に移る。すなわち、照合に成功した要素ｅのうち、特徴点ＦＰの３次元位置が確定していない要素ｅに対して、処理Ｓ５３０が実行される。

処理Ｓ５３０では、判定部６４は、処理Ｓ５１０で選択した要素ｅ［ｉ］の情報を引き継いだ要素ｒ［ｊ］を作成し、作成した要素ｒ［ｊ］を登録リストＲＬＳＴに追加する。処理Ｓ５３０で作成される要素ｒ［ｊ］は、例えば、特徴点位置ｒｘ、ｒｙ、特徴点パターンｐｏｗ、３次元位置ｐｘ、ｐｙ、ｐｚ、３次元位置の誤差ｅｒｒおよびカウンタ値ｃｎｔの情報を有している。

例えば、要素ｅ［ｉ］の特徴点位置ｕｘ、ｕｙ、特徴点パターンｐｏｗ、３次元位置ｐｘ、ｐｙ、ｐｚ、３次元位置の誤差ｅｒｒおよびカウンタ値ｃｎｔが、要素ｒ［ｊ］の特徴点位置ｒｘ、ｒｙ、特徴点パターンｐｏｗ、３次元位置ｐｘ、ｐｙ、ｐｚ、３次元位置の誤差ｅｒｒおよびカウンタ値ｃｎｔにそれぞれ引き継がれる。このように、判定部６４は、照合に成功した特徴点ＦＰのうち、３次元位置が確定していない抽出特徴点ＦＰｅを、登録特徴点ＦＰｒとして登録リストＲＬＳＴに追加する。

処理Ｓ５４０では、判定部６４は、登録リストＲＬＳＴの要素数Ｎｒを更新する。例えば、判定部６４は、要素数Ｎｒを“１”増加させる。特徴点登録判定処理が実行される前の要素数Ｎｒは、例えば、図１７に示した特徴点照合処理により、“０”に設定されている。処理Ｓ５４０の後、判定部６４の動作は、処理Ｓ５００に戻る。

処理Ｓ５５０では、判定部６４は、処理Ｓ５１０で選択した要素ｅ［ｉ］の照合成功フラグｇｌａｇ１が“０”である否かを判定する。すなわち、判定部６４は、要素ｅ［ｉ］に対応する特徴点ＦＰが新規に抽出された特徴点ＦＰｅか否かを判定する。

照合成功フラグｇｌａｇ１が“０”でないとき（処理Ｓ５５０のＮｏ）、判定部６４の動作は、処理Ｓ５００に戻る。一方、照合成功フラグｇｌａｇ１が“０”のとき（処理Ｓ５５０のＹｅｓ）、判定部６４の動作は、処理Ｓ５６０に移る。すなわち、現時刻の画像ＩＭＧで新規に抽出された抽出特徴点ＦＰｅに対応する要素ｅに対して、処理Ｓ５６０が実行される。

処理Ｓ５６０では、判定部６４は、処理Ｓ５１０で選択した要素ｅ［ｉ］の特徴点位置ｕｘ、ｕｙが登録禁止範囲ＰＡＲＡ外か否かを、登録禁止リストＡＬＳＴの各要素ａの画素位置ｄｘ、ｄｙに基づいて判定する。要素ｅ［ｉ］の特徴点位置ｕｘ、ｕｙが登録禁止範囲ＰＡＲＡ内のとき（処理Ｓ５６０のＮｏ）、判定部６４の動作は、処理Ｓ５００に戻る。一方、要素ｅ［ｉ］の特徴点位置ｕｘ、ｕｙが登録禁止範囲ＰＡＲＡ外のとき（処理Ｓ５６０のＹｅｓ）、判定部６４の動作は、処理Ｓ５７０に移る。すなわち、現時刻の画像ＩＭＧで新規に抽出された抽出特徴点ＦＰｅのうち、登録禁止範囲ＰＡＲＡ外の抽出特徴点ＦＰｅに対応する要素ｅに対して、処理Ｓ５７０が実行される。

処理Ｓ５７０では、判定部６４は、処理Ｓ５１０で選択した要素ｅ［ｉ］の情報を引き継いだ要素ｒ［ｊ］を作成し、作成した要素ｒ［ｊ］を登録リストＲＬＳＴに追加する。処理Ｓ５７０で作成される要素ｒ［ｊ］は、例えば、特徴点位置ｒｘ、ｒｙおよび特徴点パターンｐｏｗの情報を有している。

例えば、要素ｅ［ｉ］の特徴点位置ｕｘ、ｕｙおよび特徴点パターンｐｏｗが、要素ｒ［ｊ］の特徴点位置ｒｘ、ｒｙおよび特徴点パターンｐｏｗにそれぞれ引き継がれる。このように、判定部６４は、現時刻の画像ＩＭＧで新規に抽出された抽出特徴点ＦＰｅのうち、登録禁止範囲ＰＡＲＡ外の抽出特徴点ＦＰｅを、登録特徴点ＦＰｒとして登録リストＲＬＳＴに追加する。

処理Ｓ５８０では、判定部６４は、登録リストＲＬＳＴの要素数Ｎｒを更新する。例えば、判定部６４は、要素数Ｎｒを“１”増加させる。処理Ｓ５８０の後、判定部６４の動作は、処理Ｓ５００に戻る。

このように、現時刻の画像ＩＭＧから抽出された抽出特徴点ＦＰｅのうち、３次元位置が現時刻以前に確定した抽出特徴点ＦＰｅを除く抽出特徴点ＦＰｅが、次時刻の画像ＩＭＧで使用する登録特徴点ＦＰｒとして、登録リストＲＬＳＴに登録される。

以上、この実施形態においても、図１−図３で説明した実施形態と同様の効果を得ることができる。また、この実施形態では、３次元位置確定部５０Ａは、抽出特徴点ＦＰｅの３次元位置を算出する際に含まれる誤差ｌ２ｅｒｒを時系列で比較する。そして、３次元位置確定部５０Ａは、誤差ｌ２ｅｒｒが最小のときに算出した抽出特徴点ＦＰｅの３次元位置に基づいて抽出特徴点ＦＰｅの３次元位置を確定する。これにより、この実施形態では、精度の高い３次元位置を特徴点ＦＰの３次元位置として登録できる。

さらに、この実施形態では、投影位置算出部６２は、３次元位置が既に確定している特徴点ＦＰの３次元位置を、カメラ１００が搭載される移動体の移動量に応じて更新する。これにより、この実施形態では、特徴点ＦＰの確定後の３次元位置の精度が移動体の移動に伴い低下することを抑制できる。

また、この実施形態では、判定部６４は、画像ＩＭＧ上の投影位置に基づく登録禁止範囲ＰＡＲＡ内の抽出特徴点ＦＰｅを、次の処理対象の画像ＩＭＧから抽出される特徴点ＦＰとの照合に使用する登録特徴点ＦＰｒから除外する。これにより、この実施形態では、図１−図３で説明した３次元位置計測装置１０に比べて、照合対象の特徴点ＦＰの数を削減できる。この結果、この実施形態では、画像ＩＭＧの特徴点照合処理の時間を短縮できる。すなわち、この実施形態では、３次元位置を算出する際の処理時間の増加を抑制できる。

なお、この実施形態では、特徴点抽出部３０は、画像ＩＭＧ上の投影位置に基づく登録禁止範囲ＰＡＲＡを特徴点ＦＰの抽出対象から除外してもよい。この場合にも、図１−図３で説明した３次元位置計測装置１０に比べて、照合対象の特徴点ＦＰの数を削減できる。したがって、登録禁止範囲ＰＡＲＡを特徴点ＦＰの抽出対象から除外した場合にも、３次元位置を算出する際の処理時間の増加を抑制できる。

以上の実施形態において説明した発明を整理して、付記として開示する。
（付記１）
互いに異なる時刻にカメラで撮影された物体の画像上の位置の差と前記カメラの移動量とに基づいて前記物体の３次元位置を計測する３次元位置計測装置において、
前記カメラから画像を順次取得する画像取得部と、
処理対象の前記画像から特徴点を抽出する特徴点抽出部と、
登録特徴点と、処理対象の前記画像から抽出された前記特徴点である抽出特徴点とを照合する特徴点照合部と、
前記登録特徴点に対応する前記抽出特徴点の３次元位置を算出し、前記抽出特徴点の３次元位置を確定する３次元位置確定部と、
前記抽出特徴点のうち、３次元位置が確定した前記抽出特徴点を除く前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点として、登録する特徴点登録部と
を備えていることを特徴とする３次元位置計測装置。
（付記２）
前記３次元位置確定部は、前記抽出特徴点の３次元位置を算出する際に含まれる誤差を時系列で比較し、前記誤差が最小のときに算出した前記抽出特徴点の３次元位置に基づいて前記抽出特徴点の３次元位置を確定する
ことを特徴とする付記１記載の３次元位置計測装置。
（付記３）
３次元位置が既に確定している前記特徴点の３次元位置を、前記カメラが搭載される移動体の移動量に応じて更新する位置算出部を備えている
ことを特徴とする付記１または付記２に記載の３次元位置計測装置。
（付記４）
前記位置算出部は、３次元位置が既に確定している前記特徴点の前記画像上の投影位置を、移動体の移動量に応じて更新した３次元位置に基づいて算出し、
前記特徴点登録部は、前記画像上の前記投影位置に基づく第１範囲内の前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点から除外する
ことを特徴とする付記３記載の３次元位置計測装置。
（付記５）
前記特徴点抽出部は、前記第１範囲を前記特徴点の抽出対象から除外する
ことを特徴とする付記４記載の３次元位置計測装置。
（付記６）
前記位置算出部は、３次元位置が既に確定している前記特徴点の前記画像上の投影位置を、移動体の移動量に応じて更新した３次元位置に基づいて算出し、
前記特徴点抽出部は、前記画像上の前記投影位置に基づく第１範囲を、前記特徴点の抽出対象から除外する
ことを特徴とする付記３記載の３次元位置計測装置。
（付記７）
互いに異なる時刻にカメラで撮影された物体の画像上の位置の差と前記カメラの移動量とに基づいて前記物体の３次元位置を計測する３次元位置計測方法において、
前記カメラから画像を順次取得し、
処理対象の前記画像から特徴点を抽出し、
登録特徴点と、処理対象の前記画像から抽出された前記特徴点である抽出特徴点とを照合し、
前記登録特徴点に対応する前記抽出特徴点の３次元位置を算出し、前記抽出特徴点の３次元位置を確定し、
前記抽出特徴点のうち、３次元位置が確定した前記抽出特徴点を除く前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点として、登録する
ことを特徴とする３次元位置計測方法。
（付記８）
前記抽出特徴点の３次元位置を算出する際に含まれる誤差を時系列で比較し、前記誤差が最小のときに算出した前記抽出特徴点の３次元位置に基づいて前記抽出特徴点の３次元位置を確定する
ことを特徴とする付記７記載の３次元位置計測方法。
（付記９）
３次元位置が既に確定している前記特徴点の３次元位置を、前記カメラが搭載される移動体の移動量に応じて更新する
ことを特徴とする付記７または付記８に記載の３次元位置計測方法。
（付記１０）
３次元位置が既に確定している前記特徴点の前記画像上の投影位置を、前記移動体の移動量に応じて更新した３次元位置に基づいて算出し、
前記画像上の前記投影位置に基づく第１範囲内の前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点から除外する
ことを特徴とする付記９記載の３次元位置計測方法。
（付記１１）
前記第１範囲を前記特徴点の抽出対象から除外する
ことを特徴とする付記１０記載の３次元位置計測方法。
（付記１２）
３次元位置が既に確定している前記特徴点の前記画像上の投影位置を、前記移動体の移動量に応じて更新した３次元位置に基づいて算出し、
前記画像上の前記投影位置に基づく第１範囲を、前記特徴点の抽出対象から除外する
ことを特徴とする付記９記載の３次元位置計測方法。
（付記１３）
互いに異なる時刻にカメラで撮影された物体の画像上の位置の差と前記カメラの移動量とに基づいて前記物体の３次元位置を計測する処理をコンピュータに実行させる３次元位置計測プログラムであって、
前記カメラから画像を順次取得し、
処理対象の前記画像から特徴点を抽出し、
登録特徴点と、処理対象の前記画像から抽出された前記特徴点である抽出特徴点とを照合し、
前記登録特徴点に対応する前記抽出特徴点の３次元位置を算出し、前記抽出特徴点の３次元位置を確定し、
前記抽出特徴点のうち、３次元位置が確定した前記抽出特徴点を除く前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点として、登録する
処理をコンピュータに実行させることを特徴とする３次元位置計測プログラム。
（付記１４）
前記抽出特徴点の３次元位置を算出する際に含まれる誤差を時系列で比較し、前記誤差が最小のときに算出した前記抽出特徴点の３次元位置に基づいて前記抽出特徴点の３次元位置を確定する
処理をコンピュータに実行させることを特徴とする付記１３記載の３次元位置計測プログラム。
（付記１５）
３次元位置が既に確定している前記特徴点の３次元位置を、前記カメラが搭載される移動体の移動量に応じて更新する
処理をコンピュータに実行させることを特徴とする付記１３または付記１４に記載の３次元位置計測プログラム。
（付記１６）
３次元位置が既に確定している前記特徴点の前記画像上の投影位置を、前記移動体の移動量に応じて更新した３次元位置に基づいて算出し、
前記画像上の前記投影位置に基づく第１範囲内の前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点から除外する
処理をコンピュータに実行させることを特徴とする付記１５記載の３次元位置計測プログラム。
（付記１７）
前記第１範囲を前記特徴点の抽出対象から除外する
処理をコンピュータに実行させることを特徴とする付記１６記載の３次元位置計測プログラム。
（付記１８）
３次元位置が既に確定している前記特徴点の前記画像上の投影位置を、前記移動体の移動量に応じて更新した３次元位置に基づいて算出し、
前記画像上の前記投影位置に基づく第１範囲を、前記特徴点の抽出対象から除外する
処理をコンピュータに実行させることを特徴とする付記１５記載の３次元位置計測プログラム。

以上の詳細な説明により、実施形態の特徴点および利点は明らかになるであろう。これは、特許請求の範囲がその精神および権利範囲を逸脱しない範囲で前述のような実施形態の特徴点および利点にまで及ぶことを意図するものである。また、当該技術分野において通常の知識を有する者であれば、あらゆる改良および変更に容易に想到できるはずであり、発明性を有する実施形態の範囲を前述したものに限定する意図はなく、実施形態に開示された範囲に含まれる適当な改良物および均等物に拠ることも可能である。

１０、１０Ａ‥３次元位置計測装置；２０‥画像取得部；４０‥特徴点照合部；５０、５０Ａ‥３次元位置確定部；５２‥３次元位置算出部；５４‥誤差算出部；５６‥確定部；６０、６０Ａ‥特徴点登録部；６２‥投影位置算出部；６４‥判定部；７０、７２‥記憶部；１００‥カメラ

Claims (6)

1. 互いに異なる時刻にカメラで撮影された物体の画像上の位置の差と前記カメラの移動量とに基づいて前記物体の３次元位置を計測する３次元位置計測装置において、
   前記カメラから画像を順次取得する画像取得部と、
   処理対象の前記画像から特徴点を抽出する特徴点抽出部と、
   登録特徴点と、処理対象の前記画像から抽出された前記特徴点である抽出特徴点とを照合する特徴点照合部と、
   前記登録特徴点に対応する前記抽出特徴点の３次元位置を算出し、前記抽出特徴点の３次元位置を確定する３次元位置確定部と、
   前記抽出特徴点のうち、３次元位置が確定した前記抽出特徴点を除く前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点として、登録する特徴点登録部と、
   ３次元位置が既に確定している前記特徴点の３次元位置を、前記カメラが搭載される移動体の移動量に応じて更新する位置算出部と
   を備えていることを特徴とする３次元位置計測装置。
2. 前記３次元位置確定部は、前記抽出特徴点の３次元位置を算出する際に含まれる誤差を時系列で比較し、前記誤差が最小のときに算出した前記抽出特徴点の３次元位置に基づいて前記抽出特徴点の３次元位置を確定する
   ことを特徴とする請求項１記載の３次元位置計測装置。
3. 前記位置算出部は、３次元位置が既に確定している前記特徴点の前記画像上の投影位置を、移動体の移動量に応じて更新した３次元位置に基づいて算出し、
   前記特徴点登録部は、前記画像上の前記投影位置に基づく第１範囲内の前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点から除外する
   ことを特徴とする請求項１または請求項２記載の３次元位置計測装置。
4. 前記特徴点抽出部は、前記第１範囲を前記特徴点の抽出対象から除外する
   ことを特徴とする請求項３記載の３次元位置計測装置。
5. 互いに異なる時刻にカメラで撮影された物体の画像上の位置の差と前記カメラの移動量とに基づいて前記物体の３次元位置を計測する３次元位置計測方法において、
   前記カメラから画像を順次取得し、
   処理対象の前記画像から特徴点を抽出し、
   登録特徴点と、処理対象の前記画像から抽出された前記特徴点である抽出特徴点とを照合し、
   前記登録特徴点に対応する前記抽出特徴点の３次元位置を算出し、前記抽出特徴点の３次元位置を確定し、
   前記抽出特徴点のうち、３次元位置が確定した前記抽出特徴点を除く前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点として、登録し、
   ３次元位置が既に確定している前記特徴点の３次元位置を、前記カメラが搭載される移動体の移動量に応じて更新する
   ことを特徴とする３次元位置計測方法。
6. 互いに異なる時刻にカメラで撮影された物体の画像上の位置の差と前記カメラの移動量とに基づいて前記物体の３次元位置を計測する処理をコンピュータに実行させる３次元位置計測プログラムであって、
   前記カメラから画像を順次取得し、
   処理対象の前記画像から特徴点を抽出し、
   登録特徴点と、処理対象の前記画像から抽出された前記特徴点である抽出特徴点とを照合し、
   前記登録特徴点に対応する前記抽出特徴点の３次元位置を算出し、前記抽出特徴点の３次元位置を確定し、
   前記抽出特徴点のうち、３次元位置が確定した前記抽出特徴点を除く前記抽出特徴点を、次の処理対象の前記画像から抽出される前記特徴点との照合に使用する前記登録特徴点として、登録し、
   ３次元位置が既に確定している前記特徴点の３次元位置を、前記カメラが搭載される移動体の移動量に応じて更新する
   処理をコンピュータに実行させることを特徴とする３次元位置計測プログラム。

Images