JP2010038753A - 運動推定装置及びプログラム - Google Patents

Abstract

【課題】簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができるようにする。
【解決手段】画像撮像装置１２によって第１の画像及び第２の画像を撮像し、特徴点抽出部３０によって、第１の画像から、複数の特徴点を抽出し、対応点検索部３２によって、抽出された特徴点の各々に対応する点を、第２の画像から検索する。また、特徴点重み決定部３４によって、特徴点の各々について、第１の画像の特徴点を中心としたウィンドウ内に存在する特徴点の数が少ないほど大きい重み係数を決定する。そして、運動推定部３６によって、対応する第１の画像の特徴点と第２の画像の点との複数の組に基づいて推定される画像撮像装置１２の位置及び姿勢の変化であって、第１の画像と第２の画像との間で対応する特徴点と点との組の重み係数の総和が最大となる位置及び姿勢の変化を、画像撮像装置１２の位置及び姿勢の変化として推定する。
【選択図】図１

Description

本発明は、運動推定装置及びプログラムに係り、特に、撮像された画像に基づいて、撮像手段の位置及び姿勢の変化を推定する運動推定装置及びプログラムに関する。

従来より、撮像手段の相対的な位置および姿勢を特定する移動体周辺監視装置が知られている（特許文献１）。この移動体周辺監視装置では、画像から特定平面上にある４点以上の特徴点を抽出し、それらの特徴点を追跡して、２枚の画像平面での特徴点の座標値に基づき、位置姿勢を計算している。

また、撮像手段から得られた２枚以上の画像から対応点を求め、加速度センサ及び磁気センサで得られた位置姿勢情報から、画像の対応点の拘束条件を算出し、拘束条件により対応点を修正することで、撮像装置の相対的な位置関係を精度良く算出する画像入力装置が知られている（特許文献２）。
特開２００４−１９８２１１号公報 特開２００４−７８３１５号公報

しかしながら、上記の特許文献１に記載の技術では、同一平面上にある４点の対応点を検出することで撮像装置の位置関係を計算しているが、撮像した画像についての知識がない場合、同一平面上にある４点を検出することが困難である、という問題がある。

また、上記の特許文献２に記載の技術では、撮像装置以外に、加速センサや磁気センサなどの他のセンサが必要となる、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる運動推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために第１の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域内に存在する前記特徴点の数が少ないほど大きい重み係数を決定する重み係数決定手段と、対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。

第２の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域内に存在する前記特徴点の数が少ないほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。

第１の発明及び第２の発明によれば、撮像手段が第１の位置及び第１の姿勢となっているときに撮影手段によって第１の画像を撮像し、撮像手段が第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮影手段によって第２の画像を撮像する。

そして、特徴点抽出手段によって、第１の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第１の画像から抽出された特徴点の各々に対応する点を、第２の画像から検索する。

また、重み係数決定手段によって、第１の画像の前記特徴点の各々について、第１の画像の特徴点を含む所定領域内に存在する特徴点の数が少ないほど大きい重み係数を決定する。

そして、推定手段によって、対応する第１の画像の特徴点と第２の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第１の画像と第２の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第１の位置及び第１の姿勢と第２の位置及び第２の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。

このように、特徴点について、特徴点を含む所定領域内に存在する特徴点の数が少ないほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。

第３の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する重み係数決定手段と、対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。

第４の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。

第３の発明及び第４の発明によれば、撮像手段が第１の位置及び第１の姿勢となっているときに撮影手段によって第１の画像を撮像し、撮像手段が第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮影手段によって第２の画像を撮像する。

そして、特徴点抽出手段によって、第１の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第１の画像から抽出された特徴点の各々に対応する点を、第２の画像から検索する。

また、重み係数決定手段によって、第１の画像の前記特徴点の各々について、第１の画像の特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する。

そして、推定手段によって、対応する第１の画像の特徴点と第２の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第１の画像と第２の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第１の位置及び第１の姿勢と第２の位置及び第２の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。

このように、特徴点について、他の特徴点との距離が長いほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。

第５の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する重み係数決定手段と、対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。

第６の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。

第５の発明及び第６の発明によれば、撮像手段が第１の位置及び第１の姿勢となっているときに撮影手段によって第１の画像を撮像し、撮像手段が第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮影手段によって第２の画像を撮像する。

そして、特徴点抽出手段によって、第１の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第１の画像から抽出された特徴点の各々に対応する点を、第２の画像から検索する。

また、重み係数決定手段によって、第１の画像の前記特徴点の各々について、第１の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する。

そして、推定手段によって、対応する第１の画像の特徴点と第２の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第１の画像と第２の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第１の位置及び第１の姿勢と第２の位置及び第２の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。

このように、特徴点について、周囲の点との濃度又は輝度の変化度合いを示す特徴量が大きいほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。

第７の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域の画像と、前記第１の画像の前記特徴点に対応する前記第２の画像の前記特徴点を含む所定領域の画像とが類似するほど大きい重み係数を決定する重み係数決定手段と、対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。

第８の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域の画像と、前記第１の画像の前記特徴点に対応する前記第２の画像の前記特徴点を含む所定領域の画像とが類似するほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。

第７の発明及び第８の発明によれば、撮像手段が第１の位置及び第１の姿勢となっているときに撮影手段によって第１の画像を撮像し、撮像手段が第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮影手段によって第２の画像を撮像する。

そして、特徴点抽出手段によって、第１の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第１の画像から抽出された特徴点の各々に対応する点を、第２の画像から検索する。

また、重み係数決定手段によって、第１の画像の前記特徴点の各々について、第１の画像の特徴点を含む所定領域の画像と、第１の画像の特徴点に対応する第２の画像の点を含む所定領域の画像とが類似するほど大きい重み係数を決定する。

そして、推定手段によって、対応する第１の画像の特徴点と第２の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第１の画像と第２の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第１の位置及び第１の姿勢と第２の位置及び第２の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。

このように、特徴点について、特徴点を含む所定領域と、対応する第２の画像の点を含む所定領域とが類似するほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。

誤対応である可能性の高い特徴点の対応の組についての重みを高く設定しないようにするために、第５、６の発明では、特徴点と周囲の点との濃度または輝度の変化度合いを示す特微量の大きさが大きいほど大きい重み係数を設定し、第７、８の発明では、第１の画像の特徴点を含む所定領域の画像と、その特微点に対応する第２の画像の点を含む所定領域の画像とが類似するほど大きい重み係数を設定する。

特微点の特微量の大きさが大きい点は、画像間で対応を求めやすいため、誤対応となる可能性が小さいことに基づき重みを設定し、あるいは、２枚の画像間の対応する点を含むそれぞれの所定領域の画像が類似していれば、誤対応である可能性が小さいことに基づき重みを設定することで、誤対応の可能性が高い特徴点の組に高い重みを設定することを防ぐことができ、正しい運動推定が可能となる。

以上説明したように、本発明の運動推定装置及びプログラムによれば、特徴点について重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる、という効果が得られる。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された運動推定装置に本発明を適用した場合を例に説明する。

図１に示すように、本実施の形態に係る運動推定装置１０は、車両（図示省略）に取り付けられ、かつ、車両の前方を撮像して画像を生成する画像撮像装置１２と、画像撮像装置１２から得られる画像に基づいて、画像撮像装置１２の位置及び姿勢の変化としての移動量及び回転量を推定するコンピュータ１４とを備えている。

画像撮像装置１２は、車両の前方を撮像し、画像の画像信号を生成する撮像部１８と、撮像部１８で生成された画像信号をＡ／Ｄ変換するＡ／Ｄ変換部２０と、Ａ／Ｄ変換された画像信号を一時的に格納するための画像メモリ２２とを備えている。

コンピュータ１４は、ＣＰＵと、ＲＡＭと、後述する運動推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１４は、画像撮像装置１２により得られた第１の画像から、画像上で追跡しやすい特徴点を複数抽出する特徴点抽出部３０と、特徴点抽出部３０によって抽出された第１の画像の特徴点に対応する点を、第１の画像を撮像した第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢で撮像して画像撮像装置１２により得られた第２の画像上で検索して追跡し、第１の画像の特徴点に対応する点の画像座標を検出する対応点検索部３２と、特徴点抽出部３０によって抽出された第１の画像の複数の特徴点の画像座標を入力として、各特徴点に対する重み係数を決定する特徴点重み決定部３４と、対応点検索部３２で得られた対応する第１の画像の特徴点の画像座標及び第２の画像の点の画像座標の組、及び特徴点重み決定部３４で決定された各特徴点に対する重み係数を入力として、第１の画像を撮像したときの第１の位置から第２の画像を撮像したときの第２の位置への変化を、画像撮像装置１２の運動の３軸方向の移動量として推定すると共に、第１の画像を撮像したときの第１の姿勢を基準とした第２の画像を撮像したときの第２の姿勢への変化を、画像撮像装置１２の運動の３軸を基準とする回転量として推定する運動推定部３６と、運動推定部３６の結果を出力する出力部３８とを備えている。

特徴点抽出部３０は、画像撮像装置１２から得られる第１の画像から、特徴点を複数抽出する。特徴点とは、周囲の点と区別でき、異なる画像間で対応関係を求めることが容易な点のことを指す。特徴点は、２次元的に濃淡変化の勾配値が大きくなる画素を検出する方法（例えばＨａｒｒｉｓオペレータなど）を用いて、自動的に抽出される。Ｈａｒｒｉｓオペレータを用いる方法では、以下に説明するように、特徴点を抽出する。

まず、画像の点（ｕ，ｖ）の輝度をＩ（ｕ，ｖ）として、以下の（１）式によって、行列Ｍを計算する。

ただし、Ｉ_ｕ，Ｉ_ｖはそれぞれ水平方向、垂直方向の微分、Ｇ_σは標準偏差σのガウス分布による平滑化を表す。

そして、上記（１）式で計算された行列Ｍの固有値λ_１、λ_２を用いて、以下の（２）式によりコーナー強度を計算する。

ただし、ｋは、予め設定される定数であって、０．０４〜０．０６の値が一般に用いられる。そして、このコーナー強度がしきい値以上であってかつ極大となる点を選択し、選択された点を特徴点として抽出する。

対応点検索部３２は、特徴点抽出部３０により第１の画像から抽出された特徴点の各々について、第１の画像及び第２の画像の間で対応付けを行って、第１の画像の特徴点に対応する点を第２の画像から検索する。なお、第１の画像と第２の画像とで対応する点を検索するときには、第１の画像と第２の画像とで対応する点及びその周囲の点の輝度は変化しないという仮定を用いて、第１の画像と第２の画像との輝度のマッチングを行えばよく、例えば、Ｌｕｃａｓ−Ｋａｎａｄｅ法を用いて、第１の画像上の特徴点に対応する点を、第２の画像上から検索する。

Ｌｕｃａｓ−Ｋａｎａｄｅ法では、第１の画像の座標（ｘ、ｙ）の輝度値をＩ（ｘ、ｙ）、第２の画像の座標（ｘ、ｙ）の輝度値をＪ（ｘ、ｙ）と表すとき、第１の画像の座標（ｐ_ｘ、ｐ_ｙ）に対応する第２の画像の座標（ｐ_ｘ＋ｕ、ｐ_ｙ＋ｖ）を、以下の（３）式で表される関数を最小化する（ｕ、ｖ）を求めることにより、求める。

ここで、Ωは、（ｐ_ｘ、ｐ_ｙ）を中心とするウィンドウであり、ｗ（ｘ、ｙ）は、重み関数でありウィンドウの中心であるほど大きい値をとる関数（ガウシアン関数など）である。また、Ｉ_ｘ（ｘ、ｙ）は、Ｉ（ｘ、ｙ）をｘ軸方向に微分した値であり、Ｉ_ｙ（ｘ、ｙ）は、ｙ軸方向に微分した値である。

特徴点重み決定部３４は、以下に説明するように、特徴点抽出部３０で抽出された複数の特徴点の各々について重み係数を決定する。まず、図２に示すように、特徴点を中心とするウィンドウ（図２における特徴点ｐ１の周りに設定されたウィンドウｗ１、及び特徴点ｐ２の周りのウィンドウｗ２を参照）を設定し、設定されたウィンドウ内にある他の特徴点の数をカウントする（例えば、図２のウィンドウｗ１について１点カウントされ、ウィンドウｗ２について４点カウントされる。）。そして、カウントされた特徴点の数に応じて、ウィンドウ内にある他の特徴点の数が多いほど小さい重み係数を決定し、他の特徴点の数が少ないほど大きい重み係数を決定する。例えば、設定されたウィンドウ内にある特徴点の数がｎのとき、重み係数を以下の（４）式に従って決定する。
（重み係数）＝ｋ・ｅｘｐ（−ｎ） ・・・（４）

ただし、ｋは定数である。なお、重み係数を決定するための式は、上記の（５）式に限定されるものではなく、例えば、以下の（５）式に従って決定してもよい。
（重み係数）＝ｋ／（ｎ＋１) ・・・（５）

運動推定部３６は、対応点検索部３２より得られる第１の画像及び第２の画像における複数組の対応点の画像座標と、特徴点重み決定部３４から得られる各特徴点の重み係数とから、以下に説明するように、第１の画像及び第２の画像の各々が撮像されたときの画像撮像装置１２の位置及び姿勢の変化（ＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量）を計算する。

位置姿勢の変化は、図３に示すように、第１の画像から第２の画像への回転行列Ｒ（Ｘ軸を基準とする回転量、Ｙ軸を基準とする回転量、Ｚ軸を基準とする回転量）と、並進ベクトルｔ（Ｘ軸方向の移動量ｔ_ｘ、Ｙ軸方向の移動量ｔ_ｙ、Ｚ軸方向の移動量ｔ_ｚ）との６要素から構成される運動である。なお、回転行列Ｒ及び並進ベクトルｔの要素は、２つの画像間の画像座標の変換を表す物理量である。

ここで、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとの計算方法について説明する。第１の画像におけるｎ点の対応点の画像座標Ｉ_ｉと第２の画像におけるｎ点の対応点の画像座標Ｉ_ｉ’とについて（ｎ≧８）、対応点の対応関係が正しくて誤差がなければ、位置及び姿勢の変化を示す行列として、以下の（６）式を満たす３×３行列Ｆが存在する。

ただし、Ｉ_ｉ＝(ｕ_ｉ，ｖ_ｉ，１)^Ｔ、Ｉ_ｉ’＝(ｕ_ｉ’，ｖ_ｉ’，１)^Ｔであり、第１の画像での画像座標（ｕ_ｉ，ｖ_ｉ）の点に対応する第２の画像での点の画像座標が（ｕ_ｉ’，ｖ_ｉ’）である。

ここで、上記（６）式を満たす行列Ｆは、定数倍の不定性を持っている。すなわち、Ｆ_ｓが上記（６）式を満たす場合には、αＦ_ｓも上記（６）式を満たす（ただし、αは実数）。よって、行列Ｆを以下の（７）式のように表すことができる。

また、上記（６）式、（７）式より、以下の（８）式が得られる。

ここで、８組以上の対応点Ｉ_ｉ、Ｉ_ｉ’があれば、上記（８）式が少なくとも８つ得られるため、８つの変数ｆ_１１〜ｆ_３２を求めることができる。

また、実画像から特徴点の対応を求めた場合には、対応する画像座標には誤差が含まれ、特徴点の中には、誤った点と対応付けられた特徴点も存在する。そこで、運動推定部３６は、対応する点の複数の組の画像座標、及び決定された各特徴点の重み係数に基づいて、以下の（９）式で表される重み係数の総和Ｚが最大となるような行列Ｆを特定する。

ただし，ｗ_iは、特徴点重み決定部３４で決定されたｉ番目の特徴点の重み係数であり、δ(ｘ)は、ｘが真なら1、そうでないなら０をとる関数である。また、ｄ（ａ,ｂ）は、ａとｂとの距離を表し、θは予め設定されたしきい値である。

上記（９）式の重み係数の総和Ｚが最大となる行列を特定することによって、対応する点の複数の組に基づいて計算される行列Ｆであって、行列Ｆを適用したときに第１の画像と第２の画像との間で対応する点の組の特徴点に対して決定された重み係数の総和が最大となる行列Ｆが特定される。

また、撮像部１８のキャリブレーション行列Ｋが既知である場合には、上述したように特定された行列Ｆに基づいて、以下の（１０）式、（１１）式に従って、回転行列Ｒと並進ベクトルｔとを計算し、第１の画像及び第２の画像の各々が撮像されたときの画像撮像装置１２の位置及び姿勢の変化として出力する。

上記の方法により、運動推定部３６は、第１の画像及び第２の画像における複数組の対応点の画像座標と、各特徴点の重み係数とに基づいて、画像撮像装置１２の位置及び姿勢の変化を計算する。

次に、本実施の形態の原理について説明する。複数の画像を用いて運動を推定する場合には、複数の画像間での特徴点の対応を求めることが必要となるが、自動的な処理で特徴点の対応関係を求めると、誤った特徴点の対応が含まれる。そこで、運動の推定に、誤った特徴点の対応の影響を与えないようにするために、できるだけ多くの特徴点の対応に従った運動を選択する方法が一般的に用いられる。

正しい運動が推定されれば、様々な距離にある物体上の特徴点の対応が、推定された運動に従うはずであるが、車両に搭載したカメラで道路環境を撮影した画像を用いた場合、距離の近い路面部分から抽出される特徴点は少なく、一方、距離の遠い建物などの背景部分からは、多くの特徴点が抽出されるため、従う特徴点数が最大となる運動を選択すると、誤った運動を推定してしまう可能性がある。

例えば、距離の近い特徴点が３点抽出され、遠方の同じような距離にある特徴点が３０点抽出され、遠方の３０点の特徴点のうち５点の特徴点について対応が誤っていた場合、従う特徴点数が最大となる運動を選択すると、近傍の３点の特徴点の対応には合わないが、誤った対応を含む遠方の３０点の特徴点の対応には合った運動が推定される。すなわち、誤対応を除いた２８点の特徴点の対応に従う正しい運動より、遠方の誤った対応を含む３０点の特徴点の対応には合う運動の方が、運動に従う特徴点の対応が多くなるため、正しい運動が推定されない。

上記より、様々な距離の特徴点の対応に従う運動を選択することが望ましいが、画像から抽出された特徴点の距離を予め知ることは困難である。そこで、画像上の距離が近い特徴点は、３次元空間でも距離が近いという知識を利用し、特徴点を抽出した画像上における特徴点の密度に応じて、特徴点に対して重みを付け、運動に従う対応点の特徴点の重みの総和が最大となる運動を選択することで、正しい運動推定を実現する。

前の例において、距離の近い３点の特徴点については特徴点の密度が低いため、重み係数を大きくし、遠方の３０点の特徴点については特徴点の密度が高いため、重み係数を小さくする。これによって、遠方の５点の重み係数より近傍の３点の重み係数の方が大きくなるため、誤った対応の５点を含んだ特徴点の対応に従う運動は選択されない。このように、画像上で特徴点の密度が低い領域にある特徴点に対する重み係数を大きくし、運動に従う対応点の特徴点の重みの総和が最大となるような運動を推定することで、画像中から抽出される特徴点の距離の分布に偏りがある場合であっても、正しい運動推定が可能となる。

次に、第１の実施の形態に係る運動推定装置１０の作用について説明する。なお、運動推定装置１０を搭載した車両の走行中に、画像撮像装置１２の運動を推定する場合を例に説明する。

まず、画像撮像装置１２が、自車両の前方に向けられ、推定する運動の開始時における画像撮像装置１２の第１の位置及び第１の姿勢で、画像撮像装置１２によって、自車両の前方領域が撮像されて第１の画像が生成される。そして、推定する運動の終了時における画像撮像装置１２の第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢で、画像撮像装置１２によって、自車両の前方領域が撮像されて第２の画像が生成されると、コンピュータ１４において、図４に示す運動推定処理ルーチンが実行される。

まず、ステップ１００において、画像撮像装置１２から第１の画像を取得し、ステップ１０２において、上記ステップ１００で取得した第１の画像から特徴点を複数抽出する。そして、ステップ１０４において、画像撮像装置１２から第２の画像を取得し、次のステップ１０６で、上記ステップ１０２で抽出された第１の画像上の複数の特徴点の各々に対応する点を、上記ステップ１０４で得られた第２の画像上で検索して追跡し、各特徴点に対応する第２の画像上の点の画像座標を検出する。

そして、ステップ１０８において、特徴点を識別するための変数ｉを初期値１に設定すると共に、特徴点数を表わす定数Ｎに、上記ステップ１０２で抽出された特徴点の数を設定する。次のステップ１１０では、第１の画像上において、特徴点ｐ_ｉを中心とした所定のウィンドウを設定し、ウィンドウ内に存在する特徴点ｐ_ｉ以外の他の特徴点の数をカウントし、ステップ１１２において、上記ステップ１１０でカウントされたウィンドウ内の他の特徴点の数に基づいて、上記（４）式に従って、特徴点ｐ_ｉに対する重み係数ｗ_ｉを決定する。

そして、ステップ１１４において、変数ｉが定数Ｎ未満であるか否かを判定し、変数ｉが定数Ｎ未満であり、重み係数が決定されていない特徴点がある場合には、ステップ１１６で、変数ｉをインクリメントして、ステップ１１０へ戻る。一方、上記ステップ１１４において、変数ｉが定数Ｎ以上であり、全ての特徴点について重み係数を決定した場合には、ステップ１１８へ移行する。

上記ステップ１００〜ステップ１１６を実行することにより、以下の（１２）式に示すような、対応する第１の画像上の特徴点と第２の画像上の点とのｎ個の組と、ｎ個の特徴点の重み係数とからなるデータが得られる。
（Ｉ_ｉ，Ｉ_ｉ’，ｗ_ｉ），ｉ＝１，・・・，Ｎ ・・・（１２）

ここで、Ｉ_ｉは、第１の画像での画像座標の同次座標ベクトルであり、Ｉ_ｉ＝(ｕ_ｉ，ｖ_ｉ，１)^Ｔである。また、Ｉ_ｉ’は、第２の画像での画像座標の同次座標ベクトルであり、Ｉ_ｉ’＝(ｕ_ｉ’，ｖ_ｉ’，１)^Ｔである。また、第１の画像での特徴点の画像座標（ｕ_ｉ，ｖ_ｉ）の点に対応する第２の画像での点の画像座標が（ｕ_ｉ’，ｖ_ｉ’）である。

ステップ１１８では、繰り返しの数を示す変数ｒを初期値の１に設定し、ステップ１２０において、Ｎ組の対応する点からランダムに８組の対応する点を選択する。そして、ステップ１２２において、上記ステップ１２０で選択された８組の対応する点の画像座標から、上記（８）式に従って、行列Ｆの要素ｆ_１１〜ｆ_３２を算出し、算出された要素ｆ_１１〜ｆ_３２から行列Ｆ_ｒを生成する。

次のステップ１２４では、上記ステップ１２０で生成された行列Ｆ_ｒ、Ｎ組の対応する点の画像座標、及び上記ステップ１１２で決定された各特徴点の重み係数に基づいて、上記（９）に従って、行列Ｆ_ｒを適用したときに第１の画像と第２の画像との間で対応する点の組の特徴点の重み係数の総和Ｚ_ｒを算出する。

そして、ステップ１２６では、変数ｒが、最大繰り返し数を示す定数Ｔ未満であるか否かを判定し、変数ｒが定数Ｔ未満である場合には、ステップ１２８において、変数ｒをインクリメントして、ステップ１２０へ戻る。一方、上記ステップ１２６で、変数ｒが定数Ｔ以上である場合には、ステップ１３０へ移行する。

ステップ１３０では、上記ステップ１２４で算出された重み係数の総和Ｚ_ｒが最大となる行列Ｆ_ｒを特定し、ステップ１３２において、上記ステップ１３０で特定された行列Ｆ_ｒに基づいて、上記（１０）式及び（１１）式に従って、第１の画像を撮像したときの画像撮像装置１２の第１の位置及び第１の姿勢と第２の画像を撮像したときの画像撮像装置１２の第２の位置及び第２の姿勢との変化として、画像撮像装置１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量を算出する。

次のステップ１３４では、運動推定結果として、ステップ１３２で算出された画像撮像装置１２の移動量及び回転量を、コンピュータ１４のディスプレイ（図示省略）に表示して、運動推定処理ルーチンを終了する。

以上説明したように、本実施の形態に係る運動推定装置によれば、抽出された各特徴点について、第１の画像上の特徴点を中心としたウィンドウ内に存在する他の特徴点の数が少ないほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに画像間で対応する第１の画像上の特徴点と第２の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。

また、画像撮像装置以外に他のセンサを用いることなく、簡易な構成で、画像撮像装置の運動を高精度に推定することができる。

画像に基づいて運動を推定するとき、画像全体から特徴点を抽出すると、パターンの多い物体などが存在する一部の画像領域から集中して多くの特徴点が抽出され、一方、路面などのパターンの少ない領域から抽出される特徴点は少なくなる。そのような特徴点の対応に基づいて運動を推定すると精度が劣化する。本実施の形態では、パターンの多い領域から抽出された特徴点とパターンの少ない領域から抽出された特徴点に対して異なる重み係数を与えて運動を推定することにより、特徴点の数に関わらず、様々な距離にある点の誤差を最小化するように運動を推定することができ、運動の推定精度が向上する。

次に、第２の実施の形態に係る運動推定装置について説明する。なお、第２の実施の形態に係る運動推定装置の構成は、第１の実施の形態と同様であるため、同一符号を付して説明を省略する。

第２の実施の形態では、他の特徴点との距離に応じて、特徴点の重み係数を決定している点が第１の実施の形態と異なっている。

第２の実施の形態に係る運動推定装置では、特徴点重み決定部３４において、特徴点から周囲にある他の特徴点までの第１の画像上での距離を計算し、距離値を小さい順に並べたときのｍ番目までの距離値の平均値ｄを求め、ｄが小さいほど小さい重み係数を決定し、ｄが大きいほど重み係数を決定する。

例えば、図５に示すように、特徴点の重み係数を決定するときに距離値を小さい順に並べたときの３番目までの距離値を利用する場合には、特徴点ｐ１の重みを決定するとき、特徴点ｐ１との距離が近い３つの特徴点を求め、それら３つの特徴点と決定対象の特徴点ｐ１との距離ｄ１、ｄ２、ｄ３の平均値ｄを計算する。決定対象の特徴点ｐ１について、この平均距離ｄが小さいほど小さい重み係数を決定し、平均距離が大きいほど大きい重み係数を決定する。

第２の実施の形態に係る運動推定処理ルーチンでは、まず、画像撮像装置１２から第１の画像を取得して、第１の画像から特徴点を複数抽出する。そして、画像撮像装置１２から第２の画像を取得して、第１の画像上の複数の特徴点の各々に対応する点を、第２の画像上で検索して追跡し、各特徴点に対応する第２の画像上の点の画像座標を検出する。

そして、抽出されたＮ個の特徴点ｐ_ｉ（ｉ＝１〜Ｎ）について、第１の画像上において、特徴点ｐ_ｉからの距離が近い３つの特徴点を、特徴点ｐ_ｉの周囲から抽出し、抽出された３つの特徴点と特徴点ｐ_ｉとの距離の平均値を算出し、算出された距離の平均値の大きさに応じて、特徴点ｐ_ｉに対する重み係数ｗ_ｉを決定する。

そして、Ｎ組の対応する点からランダムに８組の対応する点を選択し、選択された８組の対応する点の画像座標から、行列Ｆの要素ｆ_１１〜ｆ_３２を算出し、算出された要素ｆ_１１〜ｆ_３２から行列Ｆ_ｒを生成する。次に、生成された行列Ｆ_ｒ、Ｎ組の対応する点の画像座標、及び決定された各特徴点の重み係数に基づいて、行列Ｆ_ｒを適用したときに第１の画像と第２の画像との間で対応する点の組の特徴点の重み係数の総和Ｚ_ｒを算出する。

上記のランダムに選択された８組の対応する点から、重み係数の総和Ｚ_ｒを算出する処理を、所定回数Ｔ繰り返すと、算出された重み係数の総和Ｚ_ｒが最大となる行列Ｆ_ｒを特定し、特定された行列Ｆ_ｒに基づいて、画像撮像装置１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量を算出する。

このように、抽出された各特徴点について、第１の画像上の特徴点の周囲にある特徴点との平均距離が長いほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに画像間で対応する第１の画像上の特徴点と第２の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、簡易な構成で、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。

次に、第３の実施の形態に係る運動推定装置について説明する。なお、第３の実施の形態に係る運動推定装置の構成は、第１の実施の形態と同様であるため、同一符号を付して説明を省略する。

第３の実施の形態では、周囲の点との輝度の変化度合いを示す特徴量に応じて、特徴点の重み係数を決定している点が第１の実施の形態と異なっている。

第３の実施の形態に係る運動推定装置では、特徴点重み決定部３４において、周囲の点との輝度の変化度合いを示す特徴量として、特徴点抽出部３０でＨａｒｒｉｓオペレータを用いて特徴点を抽出するときに得られたコーナー強度を取得し、特徴点のコーナー強度が大きいほど大きく重み係数を決定し、コーナー強度が小さいほど小さい重み係数を決定する。

第３の実施の形態に係る運動推定処理ルーチンでは、まず、画像撮像装置１２から第１の画像及び第２の画像を取得して、第１の画像から特徴点を複数抽出し、また、第１の画像上の複数の特徴点の各々に対応する点を、第２の画像上で検索して追跡し、各特徴点に対応する第２の画像上の点の画像座標を検出する。

そして、抽出されたＮ個の特徴点ｐ_ｉ（ｉ＝１〜Ｎ）について、特徴点ｐ_ｉを抽出するときに得られたコーナー強度を取得し、取得されたコーナー強度の大きさに応じて、特徴点ｐ_ｉに対する重み係数ｗ_ｉを決定する。

そして、Ｎ組の対応する点からランダムに８組の対応する点を選択し、選択された８組の対応する点の画像座標から、行列Ｆの要素ｆ_１１〜ｆ_３２を算出し、要素ｆ_１１〜ｆ_３２から行列Ｆ_ｒを生成する。次に、生成された行列Ｆ_ｒ、Ｎ組の対応する点の画像座標、及び決定された各特徴点の重み係数に基づいて、行列Ｆ_ｒを適用したときに第１の画像と第２の画像との間で対応する点の組の特徴点の重み係数の総和Ｚ_ｒを算出する。

上記のランダムに選択された８組の対応する点から、重み係数の総和Ｚ_ｒを算出する処理を、所定回数Ｔ繰り返すと、算出された重み係数の総和Ｚ_ｒが最大となる行列Ｆ_ｒを特定し、特定された行列Ｆ_ｒに基づいて、画像撮像装置１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量を算出する。

このように、抽出された各特徴点について、第１の画像上の特徴点のコーナー強度が大きいほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに画像間で対応する第１の画像上の特徴点と第２の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、簡易な構成で、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。

なお、上記の実施の形態では、周囲の点との輝度の変化度合いを示す特徴量として、コーナー強度を用いた場合を例に説明したが、これに限定されるものではなく、周囲の点との濃度又は輝度の変化度合いを示す特徴量を用いればよく、例えば、周囲の点との濃度又は輝度の変化度合いを示す特徴量であるエッジや二次微分量を算出し、エッジや二次微分量が大きいほど大きい重み係数を決定するようにしてもよい。

次に、第４の実施の形態に係る運動推定装置について説明する。なお、第４の実施の形態に係る運動推定装置の構成は、第１の実施の形態と同様であるため、同一符号を付して説明を省略する。

第４の実施の形態では、２つの画像において対応する点の周辺領域の画像が類似しているかによって、特徴点の重み係数を決定している点が第１の実施の形態と異なっている。

第４の実施の形態に係る運動推定装置では、特徴点重み決定部３４において、対応点検索部３２により得られた第１の画像上の特徴点の周囲の小領域と第２の画像上の対応する点の周囲の小領域とについて、輝度の類似度を示す指標として、例えば、小領域の画素ごとの輝度の差を２乗和した値であるＳＳＤ（Ｓｕｍ ｏｆ Ｓｑｕａｒｅｄ Ｄｉｓｔａｎｃｅｓ）の値を計算し、ＳＳＤの値が大きいほど小さい重み係数を決定し、ＳＳＤの値が小さいほど大きい重み係数を決定する。

例えば、図６（Ａ）、（Ｂ）に示すように、第１の画像の特徴点ｐと第２の画像の点ｐ’とが対応点として検出された場合、特徴点ｐの周囲に設定した小領域ｗの輝度と、対応する点ｐ’の周囲に設定した同じ大きさの小領域ｗ’の輝度とに基づいて、ＳＳＤを計算する。ここで、第１の画像の特徴点と対応する第２の画像の点とが、２つの画像間で正しい対応であれば、周囲のパターンの変化も小さくなるため、ＳＳＤの値は小さくなる。よって、ＳＳＤの値が小さければ、第１の画像の特徴点の周囲の小領域と、対応する第２の画像の点の周囲の小領域とが類似しており、対応が正しい可能性が高く、対応の信頼度が高いため、大きい重み係数を決定する。一方、計算されるＳＳＤの値が大きければ、第１の画像の特徴点の周囲の小領域と、対応する第２の画像の点の周囲の小領域とが類似しておらず、対応が誤っている可能性が高く、対応の信頼度が低いため、小さい重み係数を決定する。これにより、誤対応の可能性が高い特徴点の重み係数を小さくすることができる。

第４の実施の形態に係る運動推定処理ルーチンでは、まず、画像撮像装置１２から第１の画像及び第２の画像を取得して、第１の画像から特徴点を複数抽出し、また、第１の画像上の複数の特徴点の各々に対応する点を、第２の画像上で検索して追跡し、各特徴点に対応する第２の画像上の点の画像座標を検出する。

そして、抽出されたＮ個の特徴点ｐ_ｉ（ｉ＝１〜Ｎ）について、第１の画像の特徴点ｐ_ｉの周囲の小領域を抽出すると共に、特徴点ｐ_ｉに対応する第２の画像の点の周囲の小領域を抽出する。そして、抽出された第１の画像の特徴点ｐ_ｉの周囲の小領域の輝度、及び抽出された第２の画像の点の周囲の小領域の輝度に基づいて、ＳＳＤを算出し、算出されたＳＳＤの値の大きさに応じて、特徴点ｐ_ｉに対する重み係数ｗ_ｉを決定する。

そして、Ｎ組の対応する点からランダムに８組の対応する点を選択し、選択された８組の対応する点の画像座標から、行列Ｆの要素ｆ_１１〜ｆ_３２を算出し、要素ｆ_１１〜ｆ_３２から行列Ｆ_ｒを生成する。次に、生成された行列Ｆ_ｒ、Ｎ組の対応する点の画像座標、及び決定された各特徴点の重み係数に基づいて、行列Ｆ_ｒを適用したときに第１の画像と第２の画像との間で対応する点の組の特徴点の重み係数の総和Ｚ_ｒを算出する。

上記のランダムに選択された８組の対応する点から、重み係数の総和Ｚ_ｒを算出する処理を、所定回数Ｔ繰り返すと、算出された重み係数の総和Ｚ_ｒが最大となる行列Ｆ_ｒを特定し、特定された行列Ｆ_ｒに基づいて、画像撮像装置１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量を算出する。

このように、抽出された各特徴点について、第１の画像上の特徴点の周囲の小領域と、第２の画像上の対応する点の周囲の小領域とに基づくＳＳＤの値が小さいほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに第１の画像と第２の画像との間で対応する第１の画像上の特徴点と第２の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、簡易な構成で、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。

なお、上記の実施の形態では、２つの画像において対応する点の周辺領域の画像が類似しているか否かを示す指標として、ＳＳＤを用いた場合を例に説明したが、これに限定されるものではなく、２つの画像において対応する点の周辺領域の画像の類似度を示す他の指標を用いてもよい。この場合には、求められた指標に基づいて、２つの画像において対応する点の周辺領域の画像が類似するほど大きい重み係数を、特徴点に対して決定すればよい。

また、上記の第１の実施の形態〜第４の実施の形態では、第１の画像の特徴点に対応する点を、第２の画像上から検索する場合を例に説明したが、第２の画像からも特徴点を抽出し、第１の画像の特徴点に対応する点を、第２の画像上の特徴点から検索するようにしてもよい。

また、行列Ｆを適用したときに第１の画像と第２の画像との間で対応する点の組の特徴点に対する重み係数の総和が最大となる行列Ｆを特定し、特定された行列Ｆを用いて画像撮像装置の位置及び姿勢の変化を求める場合を例に説明したが、これに限定されるものではなく、例えば、行列Ｆを適用したときに第１の画像と第２の画像との間で対応する点の組の特徴点する重み係数の総和が所定値以上となる行列Ｆを特定し、特定された行列Ｆを用いて画像撮像装置の位置及び姿勢の変化を求めるようにしてもよい。

また、本発明に係るプログラムを、ＣＤＲＯＭなどの記憶媒体に格納して提供するようにしてもよい。

本発明の第１の実施の形態に係る運動推定装置の構成を示す概略図である。 特徴点を中心としたウィンドウ内に存在する特徴点の数をカウントする様子を示すイメージ図である。 位置姿勢の変化を表わす並進ベクトル及び回転行列を説明するための図である。 本発明の第１の実施の形態に係る運動推定装置における運動推定処理ルーチンの内容を示すフローチャートである。 特徴点の周辺に存在する他の特徴点との距離を求める様子を示すイメージ図である。 （Ａ）第１の画像上の特徴点の周囲の小領域を抽出する様子を示すイメージ図、及び（Ｂ）第２の画像上の対応する点の周囲の小領域を抽出する様子を示すイメージ図である。

符号の説明

１０ 運動推定装置
１２ 画像撮像装置
１４ コンピュータ
３０ 特徴点抽出部
３２ 対応点検索部
３４ 特徴点重み決定部
３６ 運動推定部

Claims (8)

1. 移動体に取り付けられて所定領域を撮像する撮像手段と、
   前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域内に存在する前記特徴点の数が少ないほど大きい重み係数を決定する重み係数決定手段と、
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段と、
   を含む運動推定装置。
2. 移動体に取り付けられて所定領域を撮像する撮像手段と、
   前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する重み係数決定手段と、
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段と、
   を含む運動推定装置。
3. 移動体に取り付けられて所定領域を撮像する撮像手段と、
   前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する重み係数決定手段と、
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段と、
   を含む運動推定装置。
4. 移動体に取り付けられて所定領域を撮像する撮像手段と、
   前記撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段と、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段と、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域の画像と、前記第１の画像の前記特徴点に対応する前記第２の画像の点を含む所定領域の画像とが類似するほど大きい重み係数を決定する重み係数決定手段と、
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段と、
   を含む運動推定装置。
5. コンピュータを、
   移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域内に存在する前記特徴点の数が少ないほど大きい重み係数を決定する重み係数決定手段、及び
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段
   として機能させるためのプログラム。
6. コンピュータを、
   移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する重み係数決定手段、及び
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段
   として機能させるためのプログラム。
7. コンピュータを、
   移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する重み係数決定手段、及び
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段
   として機能させるためのプログラム。
8. コンピュータを、
   移動体に取り付けられて所定領域を撮像する撮像手段が第１の位置及び第１の姿勢となっているときに撮像された第１の画像から、複数の特徴点を抽出する特徴点抽出手段、
   前記特徴点抽出手段によって前記第１の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第１の位置及び第１の姿勢と異なる第２の位置及び第２の姿勢となっているときに撮像された第２の画像から検索する検索手段、
   前記第１の画像の前記特徴点の各々について、前記第１の画像の前記特徴点を含む所定領域の画像と、前記第１の画像の前記特徴点に対応する前記第２の画像の点を含む所定領域の画像とが類似するほど大きい重み係数を決定する重み係数決定手段、及び
   対応する前記第１の画像の前記特徴点と前記第２の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第１の画像と前記第２の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第１の位置及び前記第１の姿勢と前記第２の位置及び前記第２の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段
   として機能させるためのプログラム。

Images