しかしながら、上記の特許文献1に記載の技術では、同一平面上にある4点の対応点を検出することで撮像装置の位置関係を計算しているが、撮像した画像についての知識がない場合、同一平面上にある4点を検出することが困難である、という問題がある。
また、上記の特許文献2に記載の技術では、撮像装置以外に、加速センサや磁気センサなどの他のセンサが必要となる、という問題がある。
本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる運動推定装置及びプログラムを提供することを目的とする。
上記の目的を達成するために第1の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段と、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点を含む所定領域内に存在する前記特徴点の数が少ないほど大きい重み係数を決定する重み係数決定手段と、対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。
第2の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点を含む所定領域内に存在する前記特徴点の数が少ないほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。
第1の発明及び第2の発明によれば、撮像手段が第1の位置及び第1の姿勢となっているときに撮影手段によって第1の画像を撮像し、撮像手段が第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮影手段によって第2の画像を撮像する。
そして、特徴点抽出手段によって、第1の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第1の画像から抽出された特徴点の各々に対応する点を、第2の画像から検索する。
また、重み係数決定手段によって、第1の画像の前記特徴点の各々について、第1の画像の特徴点を含む所定領域内に存在する特徴点の数が少ないほど大きい重み係数を決定する。
そして、推定手段によって、対応する第1の画像の特徴点と第2の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第1の画像と第2の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第1の位置及び第1の姿勢と第2の位置及び第2の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。
このように、特徴点について、特徴点を含む所定領域内に存在する特徴点の数が少ないほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。
第3の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段と、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する重み係数決定手段と、対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。
第4の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。
第3の発明及び第4の発明によれば、撮像手段が第1の位置及び第1の姿勢となっているときに撮影手段によって第1の画像を撮像し、撮像手段が第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮影手段によって第2の画像を撮像する。
そして、特徴点抽出手段によって、第1の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第1の画像から抽出された特徴点の各々に対応する点を、第2の画像から検索する。
また、重み係数決定手段によって、第1の画像の前記特徴点の各々について、第1の画像の特徴点と他の特徴点との距離が長いほど大きい重み係数を決定する。
そして、推定手段によって、対応する第1の画像の特徴点と第2の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第1の画像と第2の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第1の位置及び第1の姿勢と第2の位置及び第2の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。
このように、特徴点について、他の特徴点との距離が長いほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。
第5の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段と、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する重み係数決定手段と、対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。
第6の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。
第5の発明及び第6の発明によれば、撮像手段が第1の位置及び第1の姿勢となっているときに撮影手段によって第1の画像を撮像し、撮像手段が第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮影手段によって第2の画像を撮像する。
そして、特徴点抽出手段によって、第1の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第1の画像から抽出された特徴点の各々に対応する点を、第2の画像から検索する。
また、重み係数決定手段によって、第1の画像の前記特徴点の各々について、第1の画像の前記特徴点と周囲の点との濃度又は輝度の変化度合いを示す特徴量の大きさが大きいほど大きい重み係数を決定する。
そして、推定手段によって、対応する第1の画像の特徴点と第2の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第1の画像と第2の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第1の位置及び第1の姿勢と第2の位置及び第2の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。
このように、特徴点について、周囲の点との濃度又は輝度の変化度合いを示す特徴量が大きいほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。
第7の発明に係る運動推定装置は、移動体に取り付けられて所定領域を撮像する撮像手段と、前記撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段と、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点を含む所定領域の画像と、前記第1の画像の前記特徴点に対応する前記第2の画像の前記特徴点を含む所定領域の画像とが類似するほど大きい重み係数を決定する重み係数決定手段と、対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段とを含んで構成されている。
第8の発明に係るプログラムは、コンピュータを、移動体に取り付けられて所定領域を撮像する撮像手段が第1の位置及び第1の姿勢となっているときに撮像された第1の画像から、複数の特徴点を抽出する特徴点抽出手段、前記特徴点抽出手段によって前記第1の画像から抽出された前記特徴点の各々に対応する点を、前記撮像手段が前記第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮像された第2の画像から検索する検索手段、前記第1の画像の前記特徴点の各々について、前記第1の画像の前記特徴点を含む所定領域の画像と、前記第1の画像の前記特徴点に対応する前記第2の画像の前記特徴点を含む所定領域の画像とが類似するほど大きい重み係数を決定する重み係数決定手段、及び対応する前記第1の画像の前記特徴点と前記第2の画像の点との複数の組に基づいて推定される前記撮像手段の位置及び姿勢の変化であって、前記位置及び姿勢が変化したときに前記第1の画像と前記第2の画像との間で対応する前記特徴点と前記点との組の前記重み係数の総和が所定値以上又は最大値となる前記位置及び姿勢の変化を、前記第1の位置及び前記第1の姿勢と前記第2の位置及び前記第2の姿勢との間の前記撮像手段の位置及び姿勢の変化として推定する推定手段として機能させるためのプログラムである。
第7の発明及び第8の発明によれば、撮像手段が第1の位置及び第1の姿勢となっているときに撮影手段によって第1の画像を撮像し、撮像手段が第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢となっているときに撮影手段によって第2の画像を撮像する。
そして、特徴点抽出手段によって、第1の画像から、複数の特徴点を抽出し、検索手段によって、特徴点抽出手段によって第1の画像から抽出された特徴点の各々に対応する点を、第2の画像から検索する。
また、重み係数決定手段によって、第1の画像の前記特徴点の各々について、第1の画像の特徴点を含む所定領域の画像と、第1の画像の特徴点に対応する第2の画像の点を含む所定領域の画像とが類似するほど大きい重み係数を決定する。
そして、推定手段によって、対応する第1の画像の特徴点と第2の画像の点との複数の組に基づいて推定される撮像手段の位置及び姿勢の変化であって、位置及び姿勢が変化したときに第1の画像と第2の画像との間で対応する特徴点と点との組の重み係数の総和が所定値以上又は最大値となる位置及び姿勢の変化を、第1の位置及び第1の姿勢と第2の位置及び第2の姿勢との間の撮像手段の位置及び姿勢の変化として推定する。
このように、特徴点について、特徴点を含む所定領域と、対応する第2の画像の点を含む所定領域とが類似するほど大きい重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる。
誤対応である可能性の高い特徴点の対応の組についての重みを高く設定しないようにするために、第5、6の発明では、特徴点と周囲の点との濃度または輝度の変化度合いを示す特微量の大きさが大きいほど大きい重み係数を設定し、第7、8の発明では、第1の画像の特徴点を含む所定領域の画像と、その特微点に対応する第2の画像の点を含む所定領域の画像とが類似するほど大きい重み係数を設定する。
特微点の特微量の大きさが大きい点は、画像間で対応を求めやすいため、誤対応となる可能性が小さいことに基づき重みを設定し、あるいは、2枚の画像間の対応する点を含むそれぞれの所定領域の画像が類似していれば、誤対応である可能性が小さいことに基づき重みを設定することで、誤対応の可能性が高い特徴点の組に高い重みを設定することを防ぐことができ、正しい運動推定が可能となる。
以上説明したように、本発明の運動推定装置及びプログラムによれば、特徴点について重み係数を決定し、撮像手段の位置及び姿勢の変化として、位置及び姿勢が変化したときに画像間で対応する特徴点と点との組の重み係数の総和が大きくなる位置及び姿勢の変化を推定することにより、簡易な構成で、撮像手段の位置及び姿勢の変化を高精度に推定することができる、という効果が得られる。
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された運動推定装置に本発明を適用した場合を例に説明する。
図1に示すように、本実施の形態に係る運動推定装置10は、車両(図示省略)に取り付けられ、かつ、車両の前方を撮像して画像を生成する画像撮像装置12と、画像撮像装置12から得られる画像に基づいて、画像撮像装置12の位置及び姿勢の変化としての移動量及び回転量を推定するコンピュータ14とを備えている。
画像撮像装置12は、車両の前方を撮像し、画像の画像信号を生成する撮像部18と、撮像部18で生成された画像信号をA/D変換するA/D変換部20と、A/D変換された画像信号を一時的に格納するための画像メモリ22とを備えている。
コンピュータ14は、CPUと、RAMと、後述する運動推定処理ルーチンを実行するためのプログラムを記憶したROMとを備え、機能的には次に示すように構成されている。コンピュータ14は、画像撮像装置12により得られた第1の画像から、画像上で追跡しやすい特徴点を複数抽出する特徴点抽出部30と、特徴点抽出部30によって抽出された第1の画像の特徴点に対応する点を、第1の画像を撮像した第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢で撮像して画像撮像装置12により得られた第2の画像上で検索して追跡し、第1の画像の特徴点に対応する点の画像座標を検出する対応点検索部32と、特徴点抽出部30によって抽出された第1の画像の複数の特徴点の画像座標を入力として、各特徴点に対する重み係数を決定する特徴点重み決定部34と、対応点検索部32で得られた対応する第1の画像の特徴点の画像座標及び第2の画像の点の画像座標の組、及び特徴点重み決定部34で決定された各特徴点に対する重み係数を入力として、第1の画像を撮像したときの第1の位置から第2の画像を撮像したときの第2の位置への変化を、画像撮像装置12の運動の3軸方向の移動量として推定すると共に、第1の画像を撮像したときの第1の姿勢を基準とした第2の画像を撮像したときの第2の姿勢への変化を、画像撮像装置12の運動の3軸を基準とする回転量として推定する運動推定部36と、運動推定部36の結果を出力する出力部38とを備えている。
特徴点抽出部30は、画像撮像装置12から得られる第1の画像から、特徴点を複数抽出する。特徴点とは、周囲の点と区別でき、異なる画像間で対応関係を求めることが容易な点のことを指す。特徴点は、2次元的に濃淡変化の勾配値が大きくなる画素を検出する方法(例えばHarrisオペレータなど)を用いて、自動的に抽出される。Harrisオペレータを用いる方法では、以下に説明するように、特徴点を抽出する。
まず、画像の点(u,v)の輝度をI(u,v)として、以下の(1)式によって、行列Mを計算する。
ただし、Iu,Ivはそれぞれ水平方向、垂直方向の微分、Gσは標準偏差σのガウス分布による平滑化を表す。
そして、上記(1)式で計算された行列Mの固有値λ1、λ2を用いて、以下の(2)式によりコーナー強度を計算する。
ただし、kは、予め設定される定数であって、0.04〜0.06の値が一般に用いられる。そして、このコーナー強度がしきい値以上であってかつ極大となる点を選択し、選択された点を特徴点として抽出する。
対応点検索部32は、特徴点抽出部30により第1の画像から抽出された特徴点の各々について、第1の画像及び第2の画像の間で対応付けを行って、第1の画像の特徴点に対応する点を第2の画像から検索する。なお、第1の画像と第2の画像とで対応する点を検索するときには、第1の画像と第2の画像とで対応する点及びその周囲の点の輝度は変化しないという仮定を用いて、第1の画像と第2の画像との輝度のマッチングを行えばよく、例えば、Lucas−Kanade法を用いて、第1の画像上の特徴点に対応する点を、第2の画像上から検索する。
Lucas−Kanade法では、第1の画像の座標(x、y)の輝度値をI(x、y)、第2の画像の座標(x、y)の輝度値をJ(x、y)と表すとき、第1の画像の座標(px、py)に対応する第2の画像の座標(px+u、py+v)を、以下の(3)式で表される関数を最小化する(u、v)を求めることにより、求める。
ここで、Ωは、(px、py)を中心とするウィンドウであり、w(x、y)は、重み関数でありウィンドウの中心であるほど大きい値をとる関数(ガウシアン関数など)である。また、Ix(x、y)は、I(x、y)をx軸方向に微分した値であり、Iy(x、y)は、y軸方向に微分した値である。
特徴点重み決定部34は、以下に説明するように、特徴点抽出部30で抽出された複数の特徴点の各々について重み係数を決定する。まず、図2に示すように、特徴点を中心とするウィンドウ(図2における特徴点p1の周りに設定されたウィンドウw1、及び特徴点p2の周りのウィンドウw2を参照)を設定し、設定されたウィンドウ内にある他の特徴点の数をカウントする(例えば、図2のウィンドウw1について1点カウントされ、ウィンドウw2について4点カウントされる。)。そして、カウントされた特徴点の数に応じて、ウィンドウ内にある他の特徴点の数が多いほど小さい重み係数を決定し、他の特徴点の数が少ないほど大きい重み係数を決定する。例えば、設定されたウィンドウ内にある特徴点の数がnのとき、重み係数を以下の(4)式に従って決定する。
(重み係数)=k・exp(−n) ・・・(4)
ただし、kは定数である。なお、重み係数を決定するための式は、上記の(5)式に限定されるものではなく、例えば、以下の(5)式に従って決定してもよい。
(重み係数)=k/(n+1) ・・・(5)
運動推定部36は、対応点検索部32より得られる第1の画像及び第2の画像における複数組の対応点の画像座標と、特徴点重み決定部34から得られる各特徴点の重み係数とから、以下に説明するように、第1の画像及び第2の画像の各々が撮像されたときの画像撮像装置12の位置及び姿勢の変化(XYZ軸方向の移動量及びXYZ軸を基準とする回転量)を計算する。
位置姿勢の変化は、図3に示すように、第1の画像から第2の画像への回転行列R(X軸を基準とする回転量、Y軸を基準とする回転量、Z軸を基準とする回転量)と、並進ベクトルt(X軸方向の移動量tx、Y軸方向の移動量ty、Z軸方向の移動量tz)との6要素から構成される運動である。なお、回転行列R及び並進ベクトルtの要素は、2つの画像間の画像座標の変換を表す物理量である。
ここで、第1の画像から第2の画像への回転行列Rと並進ベクトルtとの計算方法について説明する。第1の画像におけるn点の対応点の画像座標Iiと第2の画像におけるn点の対応点の画像座標Ii’とについて(n≧8)、対応点の対応関係が正しくて誤差がなければ、位置及び姿勢の変化を示す行列として、以下の(6)式を満たす3×3行列Fが存在する。
ただし、Ii=(ui,vi,1)T、Ii’=(ui’,vi’,1)Tであり、第1の画像での画像座標(ui,vi)の点に対応する第2の画像での点の画像座標が(ui’,vi’)である。
ここで、上記(6)式を満たす行列Fは、定数倍の不定性を持っている。すなわち、Fsが上記(6)式を満たす場合には、αFsも上記(6)式を満たす(ただし、αは実数)。よって、行列Fを以下の(7)式のように表すことができる。
また、上記(6)式、(7)式より、以下の(8)式が得られる。
ここで、8組以上の対応点Ii、Ii’があれば、上記(8)式が少なくとも8つ得られるため、8つの変数f11〜f32を求めることができる。
また、実画像から特徴点の対応を求めた場合には、対応する画像座標には誤差が含まれ、特徴点の中には、誤った点と対応付けられた特徴点も存在する。そこで、運動推定部36は、対応する点の複数の組の画像座標、及び決定された各特徴点の重み係数に基づいて、以下の(9)式で表される重み係数の総和Zが最大となるような行列Fを特定する。
ただし,wiは、特徴点重み決定部34で決定されたi番目の特徴点の重み係数であり、δ(x)は、xが真なら1、そうでないなら0をとる関数である。また、d(a,b)は、aとbとの距離を表し、θは予め設定されたしきい値である。
上記(9)式の重み係数の総和Zが最大となる行列を特定することによって、対応する点の複数の組に基づいて計算される行列Fであって、行列Fを適用したときに第1の画像と第2の画像との間で対応する点の組の特徴点に対して決定された重み係数の総和が最大となる行列Fが特定される。
また、撮像部18のキャリブレーション行列Kが既知である場合には、上述したように特定された行列Fに基づいて、以下の(10)式、(11)式に従って、回転行列Rと並進ベクトルtとを計算し、第1の画像及び第2の画像の各々が撮像されたときの画像撮像装置12の位置及び姿勢の変化として出力する。
上記の方法により、運動推定部36は、第1の画像及び第2の画像における複数組の対応点の画像座標と、各特徴点の重み係数とに基づいて、画像撮像装置12の位置及び姿勢の変化を計算する。
次に、本実施の形態の原理について説明する。複数の画像を用いて運動を推定する場合には、複数の画像間での特徴点の対応を求めることが必要となるが、自動的な処理で特徴点の対応関係を求めると、誤った特徴点の対応が含まれる。そこで、運動の推定に、誤った特徴点の対応の影響を与えないようにするために、できるだけ多くの特徴点の対応に従った運動を選択する方法が一般的に用いられる。
正しい運動が推定されれば、様々な距離にある物体上の特徴点の対応が、推定された運動に従うはずであるが、車両に搭載したカメラで道路環境を撮影した画像を用いた場合、距離の近い路面部分から抽出される特徴点は少なく、一方、距離の遠い建物などの背景部分からは、多くの特徴点が抽出されるため、従う特徴点数が最大となる運動を選択すると、誤った運動を推定してしまう可能性がある。
例えば、距離の近い特徴点が3点抽出され、遠方の同じような距離にある特徴点が30点抽出され、遠方の30点の特徴点のうち5点の特徴点について対応が誤っていた場合、従う特徴点数が最大となる運動を選択すると、近傍の3点の特徴点の対応には合わないが、誤った対応を含む遠方の30点の特徴点の対応には合った運動が推定される。すなわち、誤対応を除いた28点の特徴点の対応に従う正しい運動より、遠方の誤った対応を含む30点の特徴点の対応には合う運動の方が、運動に従う特徴点の対応が多くなるため、正しい運動が推定されない。
上記より、様々な距離の特徴点の対応に従う運動を選択することが望ましいが、画像から抽出された特徴点の距離を予め知ることは困難である。そこで、画像上の距離が近い特徴点は、3次元空間でも距離が近いという知識を利用し、特徴点を抽出した画像上における特徴点の密度に応じて、特徴点に対して重みを付け、運動に従う対応点の特徴点の重みの総和が最大となる運動を選択することで、正しい運動推定を実現する。
前の例において、距離の近い3点の特徴点については特徴点の密度が低いため、重み係数を大きくし、遠方の30点の特徴点については特徴点の密度が高いため、重み係数を小さくする。これによって、遠方の5点の重み係数より近傍の3点の重み係数の方が大きくなるため、誤った対応の5点を含んだ特徴点の対応に従う運動は選択されない。このように、画像上で特徴点の密度が低い領域にある特徴点に対する重み係数を大きくし、運動に従う対応点の特徴点の重みの総和が最大となるような運動を推定することで、画像中から抽出される特徴点の距離の分布に偏りがある場合であっても、正しい運動推定が可能となる。
次に、第1の実施の形態に係る運動推定装置10の作用について説明する。なお、運動推定装置10を搭載した車両の走行中に、画像撮像装置12の運動を推定する場合を例に説明する。
まず、画像撮像装置12が、自車両の前方に向けられ、推定する運動の開始時における画像撮像装置12の第1の位置及び第1の姿勢で、画像撮像装置12によって、自車両の前方領域が撮像されて第1の画像が生成される。そして、推定する運動の終了時における画像撮像装置12の第1の位置及び第1の姿勢と異なる第2の位置及び第2の姿勢で、画像撮像装置12によって、自車両の前方領域が撮像されて第2の画像が生成されると、コンピュータ14において、図4に示す運動推定処理ルーチンが実行される。
まず、ステップ100において、画像撮像装置12から第1の画像を取得し、ステップ102において、上記ステップ100で取得した第1の画像から特徴点を複数抽出する。そして、ステップ104において、画像撮像装置12から第2の画像を取得し、次のステップ106で、上記ステップ102で抽出された第1の画像上の複数の特徴点の各々に対応する点を、上記ステップ104で得られた第2の画像上で検索して追跡し、各特徴点に対応する第2の画像上の点の画像座標を検出する。
そして、ステップ108において、特徴点を識別するための変数iを初期値1に設定すると共に、特徴点数を表わす定数Nに、上記ステップ102で抽出された特徴点の数を設定する。次のステップ110では、第1の画像上において、特徴点piを中心とした所定のウィンドウを設定し、ウィンドウ内に存在する特徴点pi以外の他の特徴点の数をカウントし、ステップ112において、上記ステップ110でカウントされたウィンドウ内の他の特徴点の数に基づいて、上記(4)式に従って、特徴点piに対する重み係数wiを決定する。
そして、ステップ114において、変数iが定数N未満であるか否かを判定し、変数iが定数N未満であり、重み係数が決定されていない特徴点がある場合には、ステップ116で、変数iをインクリメントして、ステップ110へ戻る。一方、上記ステップ114において、変数iが定数N以上であり、全ての特徴点について重み係数を決定した場合には、ステップ118へ移行する。
上記ステップ100〜ステップ116を実行することにより、以下の(12)式に示すような、対応する第1の画像上の特徴点と第2の画像上の点とのn個の組と、n個の特徴点の重み係数とからなるデータが得られる。
(Ii,Ii’,wi),i=1,・・・,N ・・・(12)
ここで、Iiは、第1の画像での画像座標の同次座標ベクトルであり、Ii=(ui,vi,1)Tである。また、Ii’は、第2の画像での画像座標の同次座標ベクトルであり、Ii’=(ui’,vi’,1)Tである。また、第1の画像での特徴点の画像座標(ui,vi)の点に対応する第2の画像での点の画像座標が(ui’,vi’)である。
ステップ118では、繰り返しの数を示す変数rを初期値の1に設定し、ステップ120において、N組の対応する点からランダムに8組の対応する点を選択する。そして、ステップ122において、上記ステップ120で選択された8組の対応する点の画像座標から、上記(8)式に従って、行列Fの要素f11〜f32を算出し、算出された要素f11〜f32から行列Frを生成する。
次のステップ124では、上記ステップ120で生成された行列Fr、N組の対応する点の画像座標、及び上記ステップ112で決定された各特徴点の重み係数に基づいて、上記(9)に従って、行列Frを適用したときに第1の画像と第2の画像との間で対応する点の組の特徴点の重み係数の総和Zrを算出する。
そして、ステップ126では、変数rが、最大繰り返し数を示す定数T未満であるか否かを判定し、変数rが定数T未満である場合には、ステップ128において、変数rをインクリメントして、ステップ120へ戻る。一方、上記ステップ126で、変数rが定数T以上である場合には、ステップ130へ移行する。
ステップ130では、上記ステップ124で算出された重み係数の総和Zrが最大となる行列Frを特定し、ステップ132において、上記ステップ130で特定された行列Frに基づいて、上記(10)式及び(11)式に従って、第1の画像を撮像したときの画像撮像装置12の第1の位置及び第1の姿勢と第2の画像を撮像したときの画像撮像装置12の第2の位置及び第2の姿勢との変化として、画像撮像装置12の運動のXYZ軸方向の移動量及びXYZ軸を基準とする回転量を算出する。
次のステップ134では、運動推定結果として、ステップ132で算出された画像撮像装置12の移動量及び回転量を、コンピュータ14のディスプレイ(図示省略)に表示して、運動推定処理ルーチンを終了する。
以上説明したように、本実施の形態に係る運動推定装置によれば、抽出された各特徴点について、第1の画像上の特徴点を中心としたウィンドウ内に存在する他の特徴点の数が少ないほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに画像間で対応する第1の画像上の特徴点と第2の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。
また、画像撮像装置以外に他のセンサを用いることなく、簡易な構成で、画像撮像装置の運動を高精度に推定することができる。
画像に基づいて運動を推定するとき、画像全体から特徴点を抽出すると、パターンの多い物体などが存在する一部の画像領域から集中して多くの特徴点が抽出され、一方、路面などのパターンの少ない領域から抽出される特徴点は少なくなる。そのような特徴点の対応に基づいて運動を推定すると精度が劣化する。本実施の形態では、パターンの多い領域から抽出された特徴点とパターンの少ない領域から抽出された特徴点に対して異なる重み係数を与えて運動を推定することにより、特徴点の数に関わらず、様々な距離にある点の誤差を最小化するように運動を推定することができ、運動の推定精度が向上する。
次に、第2の実施の形態に係る運動推定装置について説明する。なお、第2の実施の形態に係る運動推定装置の構成は、第1の実施の形態と同様であるため、同一符号を付して説明を省略する。
第2の実施の形態では、他の特徴点との距離に応じて、特徴点の重み係数を決定している点が第1の実施の形態と異なっている。
第2の実施の形態に係る運動推定装置では、特徴点重み決定部34において、特徴点から周囲にある他の特徴点までの第1の画像上での距離を計算し、距離値を小さい順に並べたときのm番目までの距離値の平均値dを求め、dが小さいほど小さい重み係数を決定し、dが大きいほど重み係数を決定する。
例えば、図5に示すように、特徴点の重み係数を決定するときに距離値を小さい順に並べたときの3番目までの距離値を利用する場合には、特徴点p1の重みを決定するとき、特徴点p1との距離が近い3つの特徴点を求め、それら3つの特徴点と決定対象の特徴点p1との距離d1、d2、d3の平均値dを計算する。決定対象の特徴点p1について、この平均距離dが小さいほど小さい重み係数を決定し、平均距離が大きいほど大きい重み係数を決定する。
第2の実施の形態に係る運動推定処理ルーチンでは、まず、画像撮像装置12から第1の画像を取得して、第1の画像から特徴点を複数抽出する。そして、画像撮像装置12から第2の画像を取得して、第1の画像上の複数の特徴点の各々に対応する点を、第2の画像上で検索して追跡し、各特徴点に対応する第2の画像上の点の画像座標を検出する。
そして、抽出されたN個の特徴点pi(i=1〜N)について、第1の画像上において、特徴点piからの距離が近い3つの特徴点を、特徴点piの周囲から抽出し、抽出された3つの特徴点と特徴点piとの距離の平均値を算出し、算出された距離の平均値の大きさに応じて、特徴点piに対する重み係数wiを決定する。
そして、N組の対応する点からランダムに8組の対応する点を選択し、選択された8組の対応する点の画像座標から、行列Fの要素f11〜f32を算出し、算出された要素f11〜f32から行列Frを生成する。次に、生成された行列Fr、N組の対応する点の画像座標、及び決定された各特徴点の重み係数に基づいて、行列Frを適用したときに第1の画像と第2の画像との間で対応する点の組の特徴点の重み係数の総和Zrを算出する。
上記のランダムに選択された8組の対応する点から、重み係数の総和Zrを算出する処理を、所定回数T繰り返すと、算出された重み係数の総和Zrが最大となる行列Frを特定し、特定された行列Frに基づいて、画像撮像装置12の運動のXYZ軸方向の移動量及びXYZ軸を基準とする回転量を算出する。
このように、抽出された各特徴点について、第1の画像上の特徴点の周囲にある特徴点との平均距離が長いほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに画像間で対応する第1の画像上の特徴点と第2の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、簡易な構成で、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。
次に、第3の実施の形態に係る運動推定装置について説明する。なお、第3の実施の形態に係る運動推定装置の構成は、第1の実施の形態と同様であるため、同一符号を付して説明を省略する。
第3の実施の形態では、周囲の点との輝度の変化度合いを示す特徴量に応じて、特徴点の重み係数を決定している点が第1の実施の形態と異なっている。
第3の実施の形態に係る運動推定装置では、特徴点重み決定部34において、周囲の点との輝度の変化度合いを示す特徴量として、特徴点抽出部30でHarrisオペレータを用いて特徴点を抽出するときに得られたコーナー強度を取得し、特徴点のコーナー強度が大きいほど大きく重み係数を決定し、コーナー強度が小さいほど小さい重み係数を決定する。
第3の実施の形態に係る運動推定処理ルーチンでは、まず、画像撮像装置12から第1の画像及び第2の画像を取得して、第1の画像から特徴点を複数抽出し、また、第1の画像上の複数の特徴点の各々に対応する点を、第2の画像上で検索して追跡し、各特徴点に対応する第2の画像上の点の画像座標を検出する。
そして、抽出されたN個の特徴点pi(i=1〜N)について、特徴点piを抽出するときに得られたコーナー強度を取得し、取得されたコーナー強度の大きさに応じて、特徴点piに対する重み係数wiを決定する。
そして、N組の対応する点からランダムに8組の対応する点を選択し、選択された8組の対応する点の画像座標から、行列Fの要素f11〜f32を算出し、要素f11〜f32から行列Frを生成する。次に、生成された行列Fr、N組の対応する点の画像座標、及び決定された各特徴点の重み係数に基づいて、行列Frを適用したときに第1の画像と第2の画像との間で対応する点の組の特徴点の重み係数の総和Zrを算出する。
上記のランダムに選択された8組の対応する点から、重み係数の総和Zrを算出する処理を、所定回数T繰り返すと、算出された重み係数の総和Zrが最大となる行列Frを特定し、特定された行列Frに基づいて、画像撮像装置12の運動のXYZ軸方向の移動量及びXYZ軸を基準とする回転量を算出する。
このように、抽出された各特徴点について、第1の画像上の特徴点のコーナー強度が大きいほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに画像間で対応する第1の画像上の特徴点と第2の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、簡易な構成で、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。
なお、上記の実施の形態では、周囲の点との輝度の変化度合いを示す特徴量として、コーナー強度を用いた場合を例に説明したが、これに限定されるものではなく、周囲の点との濃度又は輝度の変化度合いを示す特徴量を用いればよく、例えば、周囲の点との濃度又は輝度の変化度合いを示す特徴量であるエッジや二次微分量を算出し、エッジや二次微分量が大きいほど大きい重み係数を決定するようにしてもよい。
次に、第4の実施の形態に係る運動推定装置について説明する。なお、第4の実施の形態に係る運動推定装置の構成は、第1の実施の形態と同様であるため、同一符号を付して説明を省略する。
第4の実施の形態では、2つの画像において対応する点の周辺領域の画像が類似しているかによって、特徴点の重み係数を決定している点が第1の実施の形態と異なっている。
第4の実施の形態に係る運動推定装置では、特徴点重み決定部34において、対応点検索部32により得られた第1の画像上の特徴点の周囲の小領域と第2の画像上の対応する点の周囲の小領域とについて、輝度の類似度を示す指標として、例えば、小領域の画素ごとの輝度の差を2乗和した値であるSSD(Sum of Squared Distances)の値を計算し、SSDの値が大きいほど小さい重み係数を決定し、SSDの値が小さいほど大きい重み係数を決定する。
例えば、図6(A)、(B)に示すように、第1の画像の特徴点pと第2の画像の点p’とが対応点として検出された場合、特徴点pの周囲に設定した小領域wの輝度と、対応する点p’の周囲に設定した同じ大きさの小領域w’の輝度とに基づいて、SSDを計算する。ここで、第1の画像の特徴点と対応する第2の画像の点とが、2つの画像間で正しい対応であれば、周囲のパターンの変化も小さくなるため、SSDの値は小さくなる。よって、SSDの値が小さければ、第1の画像の特徴点の周囲の小領域と、対応する第2の画像の点の周囲の小領域とが類似しており、対応が正しい可能性が高く、対応の信頼度が高いため、大きい重み係数を決定する。一方、計算されるSSDの値が大きければ、第1の画像の特徴点の周囲の小領域と、対応する第2の画像の点の周囲の小領域とが類似しておらず、対応が誤っている可能性が高く、対応の信頼度が低いため、小さい重み係数を決定する。これにより、誤対応の可能性が高い特徴点の重み係数を小さくすることができる。
第4の実施の形態に係る運動推定処理ルーチンでは、まず、画像撮像装置12から第1の画像及び第2の画像を取得して、第1の画像から特徴点を複数抽出し、また、第1の画像上の複数の特徴点の各々に対応する点を、第2の画像上で検索して追跡し、各特徴点に対応する第2の画像上の点の画像座標を検出する。
そして、抽出されたN個の特徴点pi(i=1〜N)について、第1の画像の特徴点piの周囲の小領域を抽出すると共に、特徴点piに対応する第2の画像の点の周囲の小領域を抽出する。そして、抽出された第1の画像の特徴点piの周囲の小領域の輝度、及び抽出された第2の画像の点の周囲の小領域の輝度に基づいて、SSDを算出し、算出されたSSDの値の大きさに応じて、特徴点piに対する重み係数wiを決定する。
そして、N組の対応する点からランダムに8組の対応する点を選択し、選択された8組の対応する点の画像座標から、行列Fの要素f11〜f32を算出し、要素f11〜f32から行列Frを生成する。次に、生成された行列Fr、N組の対応する点の画像座標、及び決定された各特徴点の重み係数に基づいて、行列Frを適用したときに第1の画像と第2の画像との間で対応する点の組の特徴点の重み係数の総和Zrを算出する。
上記のランダムに選択された8組の対応する点から、重み係数の総和Zrを算出する処理を、所定回数T繰り返すと、算出された重み係数の総和Zrが最大となる行列Frを特定し、特定された行列Frに基づいて、画像撮像装置12の運動のXYZ軸方向の移動量及びXYZ軸を基準とする回転量を算出する。
このように、抽出された各特徴点について、第1の画像上の特徴点の周囲の小領域と、第2の画像上の対応する点の周囲の小領域とに基づくSSDの値が小さいほど大きい重み係数を決定し、画像撮像装置の位置及び姿勢の変化を示す行列を適用したときに第1の画像と第2の画像との間で対応する第1の画像上の特徴点と第2の画像上の点との組の重み係数の総和が最大となる位置及び姿勢の変化を示す行列を特定して、画像撮像装置の位置及び姿勢の変化を推定することにより、簡易な構成で、画像撮像装置の位置及び姿勢の変化を高精度に推定することができる。
なお、上記の実施の形態では、2つの画像において対応する点の周辺領域の画像が類似しているか否かを示す指標として、SSDを用いた場合を例に説明したが、これに限定されるものではなく、2つの画像において対応する点の周辺領域の画像の類似度を示す他の指標を用いてもよい。この場合には、求められた指標に基づいて、2つの画像において対応する点の周辺領域の画像が類似するほど大きい重み係数を、特徴点に対して決定すればよい。
また、上記の第1の実施の形態〜第4の実施の形態では、第1の画像の特徴点に対応する点を、第2の画像上から検索する場合を例に説明したが、第2の画像からも特徴点を抽出し、第1の画像の特徴点に対応する点を、第2の画像上の特徴点から検索するようにしてもよい。
また、行列Fを適用したときに第1の画像と第2の画像との間で対応する点の組の特徴点に対する重み係数の総和が最大となる行列Fを特定し、特定された行列Fを用いて画像撮像装置の位置及び姿勢の変化を求める場合を例に説明したが、これに限定されるものではなく、例えば、行列Fを適用したときに第1の画像と第2の画像との間で対応する点の組の特徴点する重み係数の総和が所定値以上となる行列Fを特定し、特定された行列Fを用いて画像撮像装置の位置及び姿勢の変化を求めるようにしてもよい。
また、本発明に係るプログラムを、CDROMなどの記憶媒体に格納して提供するようにしてもよい。