(57)【要約】【課題】計測対象物の３次元位置を高精度に計測することができるようにする。【解決手段】画角を広い画角と狭い画角とに切り替えて、広い画角の画像と狭い画角の画像とを交互に撮像する。そして、狭い画角の画像に基づいて、狭角画像間の画像上の対応点の動きを精度よく検出する。また、広い画角の画像に基づいて、広角画像間の位置姿勢の変化を表わす並進ベクトル及び回転行列を精度よく算出し、広角画像間の並進ベクトル及び回転行列を線形補間することにより、狭角画像間の位置姿勢の変化を表わす並進ベクトル及び回転行列を精度よく推定する。そして、狭角画像間の画像上の対応点の動きと、狭角画像間の並進ベクトル及び回転行列とに基づいて、計測対象物上の対応点の３次元座標を高精度に計測する。【選択図】図２
2008298685.tif 000002
【特許請求の範囲】
【請求項１】
第１画角で計測対象物を撮像した複数の画像、及び前記第１画角より小さい第２画角で前記計測対象物を撮像した複数の画像を出力する移動体に搭載された撮像手段と、
前記撮像手段によって前記第２画角で撮像された少なくとも２つの画像の各々から、前記計測対象物上の特徴点であって、かつ、前記少なくとも２つの画像間で対応した点を検索する検索手段と、
前記撮像手段によって前記第１画角で撮像した複数の画像に基づいて、前記複数の画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を算出する位置姿勢算出手段と、
前記位置姿勢算出手段によって算出された前記位置及び姿勢の相対関係に基づいて、前記第２画角で撮像された前記少なくとも２つの画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を推定する位置姿勢推定手段と、
前記検索手段によって検索された点と前記位置姿勢推定手段によって推定された前記位置及び姿勢の相対関係とに基づいて、前記検索された点の３次元位置を計測する位置計測手段と、
を含む計測装置。
【請求項２】
前記撮像手段は、画角を前記第１画角及び前記第１画角より小さい第２画角の何れか一方から他方に切り替えながら、前記計測対象物を連続して撮像する請求項１記載の計測装置。
【請求項３】
前記撮像手段は、前記第１画角の画像と前記第２画角の画像とを交互に撮像する請求項２記載の計測装置。
【請求項４】
前記位置姿勢算出手段は、前記撮像手段によって前記第１画角で撮像した２つの画像の各々から、特徴点であって、かつ、前記２つの画像間で対応した点を少なくとも８組検索し、前記検索された少なくとも８組の点に基づいて、前記２つの画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を算出する請求項１〜請求項３の何れか１項記載の計測装置。
【請求項５】
前記位置計測手段は、前記第２画角で撮像した画像上の点ではなく、かつ、前記第１画角で撮像した画像上の点について、３次元位置を計測する場合には、前記位置姿勢算出手段によって前記対応した点を検索した前記第１画角で撮像した２つの画像の各々から、特徴点であって、かつ、前記２つの画像間で対応した点を検索し、前記検索された点と前記位置姿勢算出手段によって算出された前記位置及び姿勢の相対関係とに基づいて、該検索された点の３次元位置を計測する請求項４記載の計測装置。
【請求項６】
前記位置及び姿勢の相対関係を、３軸方向の移動量及び３軸を基準とする回転量とした請求項１〜請求項５の何れか１項記載の計測装置。
【請求項７】
前記検索手段は、前記撮像手段によって前記第２画角で撮像された第１画像及び前記第１画像を撮像した後に前記撮像手段によって前記第２画角で撮像された第２画像の各々から、前記計測対象物上の特徴点であって、前記第１画像と前記第２画像との間で対応した点を検索し、
前記位置姿勢算出手段は、前記第１画像を撮像する前に前記撮像手段によって前記第１画角で第３画像を撮像したときの前記撮像手段の位置及び姿勢と、前記第１画像の撮像と前記第２画像の撮像との間に前記撮像手段によって前記第１画角で第４画像を撮像したときの前記撮像手段の位置及び姿勢との相対関係を表わす３軸方向の第１移動量及び３軸を基準とする第１回転量を算出すると共に、前記第４画像を撮像したときの前記撮像手段の位置及び姿勢と、前記第２画像を撮像した後に前記撮像手段によって前記第１画角で第５画像を撮像したときの前記撮像手段の位置及び姿勢との相対関係を表わす３軸方向の第２移動量及び３軸を基準とする第２回転量を算出し、
前記位置姿勢推定手段は、前記位置姿勢算出手段によって算出された前記第１移動量、前記第２移動量、前記第１回転量、及び前記第２回転量に基づいて、前記第１画像を撮像したときの前記撮像手段の位置及び姿勢と、前記第２画像を撮像したときの前記撮像手段の位置及び姿勢との相対関係を表わす前記移動量及び前記回転量を推定する請求項１〜請求項６の何れか１項記載の計測装置。
【請求項８】
前記位置姿勢算出手段は、前記第３画像及び前記第４画像の各々から、特徴点であって、かつ、前記第３画像と前記第４画像との間で対応した点を少なくとも８組検索し、前記検索された少なくとも８組の点に基づいて、前記第３画像及び前記第４画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を表わす前記第１移動量及び前記第１回転量を算出すると共に、前記第４画像及び前記第５画像の各々から、特徴点であって、かつ、前記第４画像と前記第５画像との間で対応した点を少なくとも８組検索し、前記検索された少なくとも８組の点に基づいて、前記第４画像及び前記第５画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を表わす前記第２移動量及び前記第２回転量を算出する請求項７記載の計測装置。
【請求項９】
前記位置計測手段は、前記３次元位置を連続して計測し、
前回計測された前記３次元位置の前記移動体からの距離が遠いほど、小さくなるように前記第２画角を決定する画角決定手段を更に含む請求項１〜請求項８の何れか１項記載の計測装置。
【請求項１０】
前記移動体の移動速度を計測する速度計測手段と、
前記速度計測手段によって計測された移動速度が遅いほど、前記撮像手段による撮像間隔が広くなるように前記撮像手段を制御する撮像制御手段と、
を更に含む請求項１〜請求項９の何れか１項記載の計測装置。
【請求項１１】
前記位置計測手段は、前記３次元位置を連続して計測し、
前回計測された前記３次元位置の前記移動体からの距離が遠いほど、前記撮像手段による撮像間隔が広くなるように前記撮像手段を制御する撮像制御手段を更に含む請求項１〜請求項９の何れか１項記載の計測装置。
【請求項１２】
コンピュータを、
第１画角で計測対象物を撮像した複数の画像、及び前記第１画角より小さい第２画角で前記計測対象物を撮像した複数の画像を出力する移動体に搭載された撮像手段によって前記第２画角で撮像された少なくとも２つの画像の各々から、前記計測対象物上の特徴点であって、かつ、前記少なくとも２つの画像間で対応した点を検索する検索手段、
前記撮像手段によって前記第１画角で撮像した複数の画像に基づいて、前記複数の画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を算出する位置姿勢算出手段、
前記位置姿勢算出手段によって算出された前記位置及び姿勢の相対関係に基づいて、前記第２画角で撮像された前記少なくとも２つの画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を推定する位置姿勢推定手段、及び
前記検索手段によって検索された点と前記位置姿勢推定手段によって推定された前記位置及び姿勢の相対関係とに基づいて、前記検索された点の３次元位置を計測する位置計測手段
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、計測装置及びプログラムに係り、特に、計測対象物の３次元位置を計測する計測装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、移動体に搭載した単眼カメラを用いて３次元位置の計測を行う場合、単眼カメラの運動を推定すると共に、物体の画像上での動きを検出して、３次元位置を計算する。物体の３次元位置の計測精度は、運動推定の精度と画像上での動き検出精度との両方に影響される。
【０００３】
また、画像上での物体の大きさが一定となるように画角を制御する自動画角制御機能を有する撮像装置が知られている（特許文献１）。
【特許文献１】特開平９−１３５３８０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、運動推定の精度を向上させるために、広い画角の画像を用いる必要があり、動き検出精度を向上させるために、対象の物体にズームした狭い画角の画像を用いる必要があるため、特許文献１に記載の技術では、画像上での物体の大きさを一定とするように画角を制御することにより、物体が遠方にある場合にも、画像上での動き検出の精度を向上させることができるが、そのときには画角が狭くなるため、運動推定の精度が劣化し、３次元位置の計測精度を十分に向上させることができない、という問題がある。
【０００５】
本発明は、上記の問題点を解決するためになされたもので、計測対象物の３次元位置を高精度に計測することができる計測装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記の目的を達成するために本発明に係る計測装置は、第１画角で計測対象物を撮像した複数の画像、及び前記第１画角より小さい第２画角で前記計測対象物を撮像した複数の画像を出力する移動体に搭載された撮像手段と、前記撮像手段によって前記第２画角で撮像された少なくとも２つの画像の各々から、前記計測対象物上の特徴点であって、かつ、前記少なくとも２つの画像間で対応した点を検索する検索手段と、前記撮像手段によって前記第１画角で撮像した複数の画像に基づいて、前記複数の画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を算出する位置姿勢算出手段と、前記位置姿勢算出手段によって算出された前記位置及び姿勢の相対関係に基づいて、前記第２画角で撮像された前記少なくとも２つの画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を推定する位置姿勢推定手段と、前記検索手段によって検索された点と前記位置姿勢推定手段によって推定された前記位置及び姿勢の相対関係とに基づいて、前記検索された点の３次元位置を計測する位置計測手段とを含んで構成されている。
【０００７】
また、本発明に係るプログラムは、コンピュータを、第１画角で計測対象物を撮像した複数の画像、及び前記第１画角より小さい第２画角で前記計測対象物を撮像した複数の画像を出力する移動体に搭載された撮像手段によって前記第２画角で撮像された少なくとも２つの画像の各々から、前記計測対象物上の特徴点であって、かつ、前記少なくとも２つの画像間で対応した点を検索する検索手段、前記撮像手段によって前記第１画角で撮像した複数の画像に基づいて、前記複数の画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を算出する位置姿勢算出手段、前記位置姿勢算出手段によって算出された前記位置及び姿勢の相対関係に基づいて、前記第２画角で撮像された前記少なくとも２つの画像の各々を撮像したときの前記撮像手段の位置及び姿勢の相対関係を推定する位置姿勢推定手段、及び前記検索手段によって検索された点と前記位置姿勢推定手段によって推定された前記位置及び姿勢の相対関係とに基づいて、前記検索された点の３次元位置を計測する位置計測手段として機能させるためのプログラムである。
【０００８】
本発明によれば、撮像手段によって、第１画角で計測対象物を撮像した複数の画像、及び第１画角より小さい第２画角で計測対象物を撮像した複数の画像を出力する。そして、検索手段によって、撮像手段によって第２画角で撮像された少なくとも２つの画像の各々から、計測対象物上の特徴点であって、かつ、少なくとも２つの画像間で対応した点を検索する。
【０００９】
また、位置姿勢算出手段によって、撮像手段によって第１画角で撮像した複数の画像に基づいて、複数の画像の各々を撮像したときの撮像手段の位置及び姿勢の相対関係を算出し、位置姿勢推定手段によって、位置姿勢算出手段によって算出された位置及び姿勢の相対関係に基づいて、第２画角で撮像された少なくとも２つの画像の各々を撮像したときの撮像手段の位置及び姿勢の相対関係を推定する。
【００１０】
そして、位置計測手段によって、検索手段によって検索された点と位置姿勢推定手段によって推定された位置及び姿勢の相対関係とに基づいて、検索された点の３次元位置を計測する。
【００１１】
このように、狭い第２画角で撮像された画像から、計測対象物上の特徴点の対応点を検索し、広い第１画角で撮像された画像から撮像手段の位置及び姿勢の相対関係を推定することにより、撮像手段の運動推定の精度と計測対象物上の対応点の検出精度とを向上させることができるため、計測対象物の３次元位置を高精度に計測することができる。
【００１２】
本発明に係る撮像手段は、画角を第１画角及び第１画角より小さい第２画角の何れか一方から他方に切り替えながら、計測対象物を連続して撮像することができる。これによって、一つの撮像手段によって、広い第１画角で撮像された画像と狭い第２画角で撮像された画像とを得ることができる。
【００１３】
また、上記の撮像手段は、第１画角の画像と第２画角の画像とを交互に撮像することができる。
【００１４】
上記の位置姿勢算出手段は、撮像手段によって第１画角で撮像した２つの画像の各々から、特徴点であって、かつ、２つの画像間で対応した点を少なくとも８組検索し、検索された少なくとも８組の点に基づいて、２つの画像の各々を撮像したときの撮像手段の位置及び姿勢の相対関係を表わす移動量及び回転量を算出することができる。これによって、２つの画像間で対応した少なくとも８組の点に基づいて、位置及び姿勢の相対関係を精度よく算出することができる。
【００１５】
また、本発明に係る位置計測手段は、第２画角で撮像した画像上の点ではなく、かつ、第１画角で撮像した画像上の点について、３次元位置を計測する場合には、位置姿勢算出手段によって対応した点を検索した第１画角で撮像した２つの画像の各々から、特徴点であって、かつ、２つの画像間で対応した点を検索し、検索された点と位置姿勢算出手段によって算出された位置及び姿勢の相対関係とに基づいて、該検索された点の３次元位置を計測することができる。これによって、第１画角の画像のみに写る特徴点の３次元位置も計測できるため、広角な視野で計測対象物の３次元位置を計測することができる。
【００１６】
上記の位置及び姿勢の相対関係を、３軸方向の移動量及び３軸を基準とする回転量とすることができる。
【００１７】
本発明に係る検索手段は、撮像手段によって第２画角で撮像された第１画像及び第１画像を撮像した後に撮像手段によって第２画角で撮像された第２画像の各々から、計測対象物上の特徴点であって、第１画像と第２画像との間で対応した点を検索し、位置姿勢算出手段は、第１画像を撮像する前に撮像手段によって第１画角で第３画像を撮像したときの撮像手段の位置及び姿勢と、第１画像の撮像と第２画像の撮像との間に撮像手段によって第１画角で第４画像を撮像したときの撮像手段の位置及び姿勢との相対関係を表わす３軸方向の第１移動量及び３軸を基準とする第１回転量を算出すると共に、第４画像を撮像したときの撮像手段の位置及び姿勢と、第２画像を撮像した後に撮像手段によって第１画角で第５画像を撮像したときの撮像手段の位置及び姿勢との相対関係を表わす３軸方向の第２移動量及び３軸を基準とする第２回転量を算出し、位置姿勢推定手段は、位置姿勢算出手段によって算出された第１移動量、第２移動量、第１回転量、及び第２回転量に基づいて、第１画像を撮像したときの撮像手段の位置及び姿勢と、第２画像を撮像したときの撮像手段の位置及び姿勢との相対関係を表わす移動量及び回転量を推定することができる。これによって、狭い第２画角の画像を撮像した前後で撮像された広い第１画角の画像から算出される移動量及び回転量に基づいて、狭い第２画角の画像を撮像したときの撮像手段の移動量及び回転量を精度よく推定することができる。
【００１８】
また、上記の位置姿勢算出手段は、第３画像及び第４画像の各々から、特徴点であって、かつ、第３画像と第４画像との間で対応した点を少なくとも８組検索し、検索された少なくとも８組の点に基づいて、第３画像及び第４画像の各々を撮像したときの撮像手段の位置及び姿勢の相対関係を表わす第１移動量及び第１回転量を算出すると共に、第４画像及び第５画像の各々から、特徴点であって、かつ、第４画像と第５画像との間で対応した点を少なくとも８組検索し、検索された少なくとも８組の点に基づいて、第４画像及び第５画像の各々を撮像したときの撮像手段の位置及び姿勢の相対関係を表わす第２移動量及び第２回転量を算出することができる。これによって、第３画像と第４画像との間で対応した少なくとも８組の点及び第４画像と第５画像との間で対応した少なくとも８組の点に基づいて、移動量及び回転量を精度よく算出することができる。
【００１９】
上記の位置計測手段は、３次元位置を連続して計測し、計測装置は、前回計測された３次元位置の移動体からの距離が遠いほど、小さくなるように第２画角を決定する画角決定手段を更に含むことができる。これによって、狭い第２画角で撮像された画像間で検索される計測対象物上の対応点の検出精度を向上させることができる。
【００２０】
上記の計測装置は、移動体の移動速度を計測する速度計測手段と、速度計測手段によって計測された移動速度が遅いほど、撮像手段による撮像間隔が広くなるように撮像手段を制御する撮像制御手段とを更に含むことができる。これによって、画像間で検索される対応点の動きが大きくなるため、撮像手段の運動推定の精度を向上させることができる。
【００２１】
上記の位置計測手段は、３次元位置を連続して計測し、計測装置は、前回計測された３次元位置の移動体からの距離が遠いほど、撮像手段による撮像間隔が広くなるように撮像手段を制御する撮像制御手段を更に含むことができる。これによって、画像間で検索される対応点の動きが大きくなるため、撮像手段の運動推定の精度を向上させることができる。
【発明の効果】
【００２２】
以上説明したように、本発明の計測装置及びプログラムによれば、狭い第２画角で撮像された画像から、計測対象物上の特徴点の対応点を検索し、広い第１画角で撮像された画像から撮像手段の位置及び姿勢の相対関係を推定することにより、撮像手段の運動推定の精度と計測対象物上の対応点の検出精度とを向上させることができるため、計測対象物の３次元位置を高精度に計測することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００２３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２４】
図１に示すように、本実施の形態に係る計測装置１０は、車両（図示省略）に搭載され、かつ、画角を切り替えながら計測対象物を撮像して画像を生成する画像撮像装置１２と、車両の走行速度を計測する速度計測部１４と、画像撮像装置１２から得られる画像に基づいて、計測対象物の３次元座標を計測する計測処理ルーチンを実現するための計測プログラムを格納したコンピュータ１６とを備えている。
【００２５】
画像撮像装置１２は、計測対象物を撮像し、画像の画像信号を生成する単眼のカメラで構成される撮像部１８と、撮像部１８で生成された画像信号をＡ／Ｄ変換するＡ／Ｄ変換部２０と、Ａ／Ｄ変換された画像信号を一時的に格納するための画像メモリ２２と、ズーム倍率を変更して画角を制御する画角制御部２４と、撮像間隔に基づく撮像タイミングや撮像タイミングに応じた画角制御部２４による画角の変更タイミングを制御する撮像制御部２６とを備えている。
【００２６】
コンピュータ１６に格納されている計測プログラムは、画像撮像装置１２により得られた複数の画像の各々から、画像上で追跡しやすい特徴点を複数抽出する特徴点抽出部３０と、特徴点抽出部３０により得られた２つの画像の各々における特徴点から、２つ画像の間で対応する対応点を検索する対応点検索部３２と、対応点検索部３２で得られた各対応点における各画像の画像座標を入力として、対応点が検索された一方の画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした、対応点が検索された他方の画像を撮像したときの画像撮像装置１２の位置姿勢への変化（位置姿勢の相対関係）を、画像撮像装置１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量として算出する運動算出部３４と、運動算出部３４によって算出された移動量及び回転量に基づいて、線形補間によって、対応点が検索された一方の画像を撮像してから対応点が検索された他方の画像を撮像するまでの間における画像撮像装置１２の移動量及び回転量を補間する運動補間部３６と、計測対象物の３次元座標を計測する３次元座標計測部３８と、３次元座標計測部３８の結果を出力する出力部４０とを備えている。
【００２７】
また、計測プログラムは、速度計測部１４によって計測された走行速度に基づいて、画像撮像装置１２の撮像間隔を決定する撮像間隔決定部４２と、３次元座標計測部３８によって前回計測された計測対象物の３次元座標に基づいて、画像撮像装置１２の画角を決定する画角決定部４４とを備えている。
【００２８】
画像撮像装置１２は、図２に示すように、画角制御部２４によって、画角を広い画角と狭い画角とに切り替えて、広い画角の画像と狭い画角の画像とを交互に撮像する。また、撮像制御部２６によって、撮像間隔時間ｔが制御され、撮像間隔時間ｔで連続して画像が撮像される。
【００２９】
コンピュータ１６の撮像間隔決定部４２は、撮像間隔として、広い画角の画像間での対応点の画像上での動き量の予測値の平均又は最大値が、しきい値以上となる時間を決定する。例えば、図２の時刻ｔ２の広角画像を撮像したときに、撮像間隔を決定することを考える。
【００３０】
このとき、速度計測部１４により計測した走行速度を利用する。速度計測部１４から推定される時刻ｔ０から時刻ｔ２まで（上記図２参照）の２ｔの間の車両の３次元移動量を（ｍｘ，ｍｙ，ｍｚ）とする。また、広角画像撮像時の撮像部１８のキャリブレーション行列Ｋを以下の（１）式で定義する。
【００３１】
【数１】
2008298685.tif 000003
【００３２】
ただし、ｆｘは、撮像部１８のＸ方向の焦点距離、ｆｙは、撮像部１８のＹ方向の焦点距離、（ｃｕ，ｃｖ）は画像中心である。なお、画像上の画像座標は、Ｘ方向に対応するＵ方向の座標ｕとＹ方向に対応するＶ方向の座標ｖとで表されるものとする。
【００３３】
このとき、時刻ｔ０において３次元座標が（ｘ，ｙ，ｚ）と計測された対応点の時刻ｔ２での画像上での予測位置の画像座標を以下の（２）式で表わすと、この画像座標は、以下の（３）式を用いて計算される。
【００３４】
【数２】
2008298685.tif 000004
【００３５】
この対応点の時刻ｔ０での画像座標を（ｕ，ｖ）としたとき、動き量の予測値は以下の（５）式で表わされる。
【００３６】
【数３】
2008298685.tif 000005
【００３７】
従って、上記（５）式で表される動き量の予測値の平均又は最大値が、しきい値以上となるように、３次元移動量（ｍｘ，ｍｙ，ｍｚ）を求め、求められた３次元移動量及び速度計測部１４により計測された走行速度から、撮像間隔時間ｔを決定する。これにより、車両の走行速度が遅いほど、撮像間隔が広くなるように撮像間隔時間ｔが決定される。
【００３８】
また、画角決定部４４は、前回の計測タイミングで３次元座標計測部３８により得られた対応点の３次元座標の分布に基づいて、３次元座標の車両からの距離が遠いほど、狭くなるように狭い画角を決定する。例えば、上述した方法で撮像間隔時間ｔを決定したとき、時間２ｔの間における正面付近に存在する画像上での計測対象物上の対応点の動き量の予測値の平均又は最大値が、しきい値以上となるように、焦点距離を設定し、設定された焦点距離に基づいて、狭い画角を決定する。なお、広い画角については、撮像部１８で設定可能な最大の画角に決定すればよい。
【００３９】
特徴点抽出部３０は、画像撮像装置１２から得られる同じ画角で異なる時刻に撮像した２枚の画像から、それぞれ特徴点を抽出する。特徴点とは、周囲の点と区別でき、異なる画像間で対応関係を求めることが容易な点のことを指す。特徴点は、２次元的に濃淡変化の勾配値が大きくなる画素を検出する方法（例えばＨａｒｒｉｓオペレータなど）を用いて、自動的に抽出される。Ｈａｒｒｉｓオペレータを用いる方法では、以下に説明するように、特徴点を抽出する。まず、画像の点（ｕ，ｖ）の輝度をＩ（ｕ，ｖ）として、以下の（６）式によって、行列Ｍを計算する。
【００４０】
【数４】
2008298685.tif 000006
【００４１】
ただし、Ｉｕ，Ｉｖはそれぞれ水平方向、垂直方向の微分、Ｇσは標準偏差σのガウス分布による平滑化を表す。
【００４２】
そして、上記（６）式で計算された行列Ｍの固有値λ１、λ２を用いて、以下の（７）式によりコーナー強度を計算する。
【００４３】
【数５】
2008298685.tif 000007
【００４４】
ただし、ｋは予め設定される定数であって、０．０４〜０．０６の値が一般に用いられる。Ｈａｒｒｉｓオペレータを用いる方法では、このコーナー強度がしきい値以上でかつ極大となる点を選択し、選択された点を特徴点として抽出する。
【００４５】
対応点検索部３２は、特徴点抽出部３０において２つの画像の各々から抽出された特徴点について、２つの画像間での対応付けを行って、２つの画像間の対応点を検索する。画像間での特徴点の対応付けでは、特徴点周辺に設定した小領域での輝度分布が似ている点の組を選択し、選択された点の組を対応点とする。２つの特徴点が似ているかどうかの判定にはＳＳＤ（Ｓｕｍ  ｏｆ  Ｓｑｕａｒｅｄ  Ｄｉｆｆｅｒｅｎｃｅｓ）などの値を用いればよい。
【００４６】
また、対応点検索部３２は、特徴点抽出部３０において２つの広角画像の各々から抽出された特徴点について、２つの広角画像間の対応点を少なくとも８組検索する。
【００４７】
運動算出部３４は、対応点検索部３２より得られる２つの広角画像における少なくとも８組の対応点の画像座標から、２つの広角画像の各々が撮像されたときの画像撮像装置１２の位置及び姿勢の変化（ＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量）を計算する。位置姿勢の変化は、図３に示すように、第１の画像から第２の画像への回転行列Ｒ（Ｘ軸を基準とする回転量、Ｙ軸を基準とする回転量、Ｚ軸を基準とする回転量）と、並進ベクトルｔ（Ｘ軸方向の移動量ｔｘ、Ｙ軸方向の移動量ｔｙ、Ｚ軸方向の移動量ｔｚ）との６要素から構成される運動である。なお、回転行列Ｒ及び並進ベクトルｔの要素は、２つの画像間の画像座標の変換を表す物理量である。
【００４８】
ここで、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとの計算方法について説明する。第１の画像におけるｎ点の対応点の画像座標Ｉｉと第２の画像におけるｎ点の対応点の画像座標Ｉｉ’とについて（ｎ≧８）、対応点が正しくて誤差がなければ、以下の（８）式を満たす３×３行列Ｆが存在する。
【００４９】
【数６】
2008298685.tif 000008
【００５０】
ただし、Ｉｉ＝(ｕｉ，ｖｉ，１)Ｔ、Ｉｉ’＝(ｕｉ’，ｖｉ’，１)Ｔであり、第１の画像での画像座標（ｕｉ，ｖｉ）の点に対応する第２の画像での点の画像座標が（ｕｉ’，ｖｉ’）である。
【００５１】
ここで、上記（８）式を満たす行列Ｆは、定数倍の不定性を持っている。すなわち、Ｆｓが上記（８）式を満たす場合には、αＦｓも上記（８）式を満たす（ただし、ａは実数）。よって、行列Ｆを以下の（９）式のように表すことができる。
【００５２】
【数７】
2008298685.tif 000009
【００５３】
また、上記（８）式、（９）式より、以下の（１０）式が成り立つ。
【００５４】
【数８】
2008298685.tif 000010
【００５５】
ここで、８組以上の対応点Ｉｉ、Ｉｉ’があれば、上記（１０）式が少なくとも８つ得られるため、８つの変数ｆ１１〜ｆ３２を求めることができる。なお、得られる８つの式が互いに独立である必要があり、また、誤差が含まれる場合であっても安定して計算するために、他の対応点の組となるべく異なる動きをしている特徴点の組を対応点として検索することが好ましい。
【００５６】
上述したように行列Ｆが計算でき、また、撮像部１８のキャリブレーション行列Ｋが既知である場合には、以下の（１１）式、（１２）式より、回転行列Ｒと並進ベクトルｔとを計算することができる．
【００５７】
【数９】
2008298685.tif 000011
【００５８】
運動補間部３６は、運動算出部３４により位置姿勢の変化が算出された２枚の画像の撮像時刻の間に撮像された画像について、画像撮像装置１２の位置姿勢の変化（回転行列及び並進ベクトル）を、運動算出部３４により得られた位置姿勢の変化を線形補間することによって求める。例えば、図２において時刻ｔ０とｔ２の広角画像について算出された回転行列が表わす回転量及び並進ベクトルの各々を線形補間して、時刻ｔ０の広角画像を基準とした時刻ｔ１の狭角画像の回転行列が表わす回転量及び並進ベクトルの各々を算出する。また、時刻ｔ２とｔ４の広角画像について算出された回転行列及び並進ベクトルの各々を線形補間して、時刻ｔ２の広角画像を基準とした時刻ｔ３の狭角画像の回転行列及び並進ベクトルの各々を算出する。そして、時刻ｔ１、ｔ３の狭角画像の回転行列及び並進ベクトルに基づいて、時刻ｔ１とｔ３の狭角画像間の回転行列及び並進ベクトルの各々を算出する。
【００５９】
３次元座標計測部３８は、対応点検索部３２から得られる２つの狭角画像間での対応点の画像座標、及び運動補間部３６から得られる２つの狭角画像間の回転行列及び並進ベクトルを用いて、計測対象物上の対応点の３次元座標を計算する。対応点の３次元座標は以下の方法により計算できる。
【００６０】
まず、２枚の狭角画像の各々の対応点の画像座標を（ｕ，ｖ）、（ｕ’，ｖ’）とし、狭角画像間の回転行列をＲとし、並進ベクトルをｔとし、撮像部１８のキャリブレーション行列をＫとしたとき、以下の（１３）式、（１４）式のような行列Ｐ、Ｐ’を定義する。
【００６１】
【数１０】
2008298685.tif 000012
【００６２】
そして、ｐｉ，ｐ’ｉをそれぞれ行列Ｐ，Ｐ’の第ｉ行のベクトルとすると、対応点の３次元座標Ｘ＝（ｘ，ｙ，ｚ，１）Ｔは以下の（１５）式の解として求めることができる。
【００６３】
【数１１】
2008298685.tif 000013
【００６４】
次に、本実施の形態に係る計測装置１０の作用について説明する。なお、計測装置１０を搭載した車両の走行中に、計測対象物の３次元座標を計測する場合を例に説明する。
【００６５】
まず、画像撮像装置１２が、計測対象物に向けられ、画像撮像装置１２によって計測対象物の連続撮像が開始されると、コンピュータ１６において、図４に示す計測対象物の３次元座標を計測するための計測処理ルーチンが実行される。
【００６６】
まず、ステップ１００において、初期設定として、広い画角と狭い画角とを、撮像部１８の予め定められた最大画角に設定し、ステップ１０２で、速度計測部１４で計測された車両の走行速度を取得する。そして、ステップ１０４において、上記ステップ１０２で取得された走行速度に基づいて、撮像間隔時間ｔを決定し、決定された撮像間隔時間ｔで連続撮像するように、画像撮像装置１２の撮像間隔を制御する。これにより、画像撮像装置１２は、画角を設定された広い画角と狭い画角とに切り替えながら、設定された撮像間隔時間ｔで、広い画角の画像と狭い画角の画像とを交互に撮像する。
【００６７】
次のステップ１０６では、画像撮像装置１２から、広い画角の第１広角画像（例えば撮像時刻ｔ０）、狭い画角の第１狭角画像（撮像時刻ｔ１＝ｔ０＋ｔ）、広い画角の第２広角画像（撮像時刻ｔ２＝ｔ０＋２ｔ）、狭い画角の第２狭角画像（撮像時刻ｔ３＝ｔ０＋３ｔ）、及び広い画角の第３広角画像（撮像時刻ｔ４＝ｔ０＋４ｔ）を順に取得する。
【００６８】
そして、ステップ１０８において、第１広角画像と第２広角画像との各々から特徴点を複数抽出し、ステップ１１０で、上記ステップ１０８で抽出された複数の特徴点から、第１広角画像と第２広角画像との間で対応する対応点を少なくとも８組検索する。そして、ステップ１１２において、上記ステップ１１０で検索された少なくとも８組の対応点の画像座標に基づいて、第１広角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第２広角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。
【００６９】
そして、次のステップ１１４において、第２広角画像と第３広角画像との各々から特徴点を複数抽出し、ステップ１１６で、上記ステップ１１４で抽出された特徴点から、第２広角画像と第３広角画像との間で対応する対応点を少なくとも８組検索する。そして、ステップ１１８において、上記ステップ１１６で検索された少なくとも８組の対応点の画像座標に基づいて、第２広角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第３広角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。
【００７０】
そして、ステップ１２０において、上記ステップ１１２で算出された並進ベクトル及び回転行列を補間して、第１広角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第１狭角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出し、次のステップ１２２において、上記ステップ１１８で算出された並進ベクトル及び回転行列を補間して、第２広角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第２狭角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。
【００７１】
そして、ステップ１２４において、上記ステップ１２０、１２２で算出された並進ベクトル及び回転行列に基づいて、第１狭角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第２狭角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。
【００７２】
そして、ステップ１２６において、第１狭角画像と第２狭角画像との各々から特徴点を複数抽出し、ステップ１２８で、上記ステップ１２６で抽出された特徴点から、第１狭角画像と第２狭角画像との間で対応する対応点を少なくとも１組検索する。
【００７３】
次のステップ１３０では、上記ステップ１２４で算出された第１狭角画像と第２狭角画像との間における並進ベクトル及び回転行列、及び上記ステップ１２８で検索された第１狭角画像と第２狭角画像とにおける対応点の画像座標に基づいて、この対応点が示す計測対象物上の特徴点の３次元座標を計算し、ステップ１３２において、計算された計測対象物の特徴点の３次元座標を、コンピュータ１６のディスプレイ（図示省略）に表示する。
【００７４】
そして、ステップ１３４において、上記ステップ１３４で計算された３次元座標に基づいて、狭い画角を決定し、画像撮像装置１２の狭い画角が、決定された狭い画角となるように制御して、ステップ１０２へ戻って、設定された広い画角の画像と新たに決定された狭い画角の画像とに基づいて、次の計測タイミングにおける計測対象物上の特徴点の３次元座標を計測する。
【００７５】
以上説明したように、第１の実施の形態に係る計測装置によれば、狭い画角で撮像された２つの画像に基づいて、計測対象物上の特徴点から対応点を検索し、広い画角で撮像された複数の画像から、２つの狭角画像の各々を撮像したときの画像撮像装置の位置姿勢の変化を表わす並進ベクトル及び回転行列を推定することにより、画像撮像装置の運動推定の精度と計測対象物上の対応点の検出精度とを向上させることができるため、計測対象物の３次元座標を高精度に計測することができる。
【００７６】
また、画角を広角と狭角とに切り替えて撮像し、広角画像から幅広い範囲で検索された複数の対応点に基づいて高精度に運動推定した結果と、狭角画像で高精度に特徴点の動きを検出した結果とから、計測対象物の３次元座標を高精度に計測することができる。
【００７７】
また、撮像部の画角を広角と狭角とに切り替えることにより、一つの撮像部によって、広い画角で撮像された画像と狭い画角で撮像された画像とを得ることができる。
【００７８】
また、２つの広角画像間で対応した少なくとも８組の対応点に基づいて、並進ベクトル及び回転行列の各々を精度よく算出することができる。
【００７９】
狭い画角の画像を撮像した前後で撮像された２枚の広い画角の画像から算出された並進ベクトル及び回転行列に基づいて、狭い画角の画像を撮像したときの画像撮像装置の並進ベクトル及び回転行列を精度よく推定することができる。
【００８０】
また、前回計測された計測対象物の３次元座標の車両からの距離が遠いほど、小さくなるように狭い画角を決定することにより、狭い画角で撮像された画像間で検索される計測対象物上の対応点の検出精度を向上させることができる。
【００８１】
また、車両の走行速度が遅くなるほど、運動推定および動き検出に用いる画像間の撮像間隔が広くなるように決定することで、画像間で検索される対応点の動きが大きくなるため、撮像手段の運動推定の精度及び動き検出の制度を向上させることができる。
【００８２】
また、単眼カメラで撮像した連続画像を用いて計測対象物の３次元座標を高精度に計測することができる。
【００８３】
なお、上記の実施の形態では、２つの画像から抽出された複数の特徴点から、２つの画像の間で対応する点を検索する場合を例に説明したが、２つの画像の一方の画像から特徴点を抽出し、他方の画像から、抽出された特徴点に対応する対応点を検索するようにしてもよい。
【００８４】
また、狭角画像から検索された対応点についてのみ３次元座標を計測する場合を例に説明したが、狭角画像に写っていない範囲の対応点であって、かつ、広角画像に写っている対応点についても、３次元座標を計測するようにしてもよい。この場合には、並進ベクトル及び回転行列を算出するために対応点を検索した第１広角画像及び第２広角画像の特徴点から、第１広角画像と第２広角画像との間で対応する対応点を検索し、検索された第１広角画像と第２広角画像とにおける対応点の画像座標、及び第１広角画像と第２広角画像との間における並進ベクトル及び回転行列に基づいて、対応点が示す計測対象物上の特徴点の３次元座標を計算すればよい。これによって、狭角画像を用いることで中心部（正面付近）にある計測対象物の距離精度を高くしながら、広角画像のみに写る特徴点の３次元座標についても計算し、広角な視野で計測対象物の３次元座標を得ることができる。
【００８５】
また、交互に狭い画角と広い画角とを切り替えて撮像する場合を例に説明したが、これに限定されることなく、狭い画角の画像又は広い画角の画像が連続して撮像されるように、画角を切り替えてもよい。この場合には、前後に撮像された広角画像、前に撮像された２つの広角画像、又は後に撮像された２つの広角画像における移動ベクトル及び回転行列を線形補間して、狭角画像を撮像したときの画像撮像装置の並進ベクトル及び回転行列を求めるようにすればよい。
【００８６】
また、２つの狭角画像の各々について、並進ベクトル及び回転行列を求める際に、共通となる第２広角画像を用いて算出された並進ベクトル及び回転行列の各々を補間した場合を例に説明したが、これに限定されるものではなく、例えば、第１広角画像及び第２広角画像を用いて算出された並進ベクトル及び回転行列の各々を補間して、第１狭角画像における並進ベクトル及び回転行列を算出し、また、第３広角画像及び第４広角画像を用いて算出された並進ベクトル及び回転行列の各々を補間して、第２狭角画像における並進ベクトル及び回転行列を算出するようにしてもよい。この場合に、基準となる広角画像を一つ定めておき（例えば第１広角画像を基準とする）、この基準となる広角画像と第１狭角画像との間の並進ベクトル及び回転行列、基準となる広角画像と第２狭角画像との間の並進ベクトル及び回転行列と、第１、２狭角画像の各々における並進ベクトル及び回転行列とに基づいて、第１、２狭角画像間の並進ベクトル及び回転行列を算出するようにすればよい。また、２つの広角画像間の並進ベクトル及び回転行列を線形補間して、第１狭角画像における並進ベクトル及び回転行列と、第２狭角画像における並進ベクトル及び回転行列とを算出するようにしてもよい。
【００８７】
また、画像撮像装置に単眼のカメラを用いた場合を例に説明したが、広い画角の画像を撮像するカメラと狭い画角の画像を撮像するカメラとを備えた複眼カメラを用いるようにしてもよい。この場合には、複眼カメラが独立して動かないようして、同じように位置姿勢が変化するようにすればよい。
【００８８】
また、計測対象物上の対応点の３次元座標を計測して出力する場合を例に説明したが、計測された３次元座標に基づいて、車両からの対応点までの距離を計測し、計測された距離を出力するようにしてもよい。
【００８９】
また、２つの狭角画像から、対応点を検索する場合を例に説明したが、これに限定されるものではなく、３つ以上の狭角画像から、対応点を検索するようにしてもよい。３枚の狭角画像から対応点を検索する場合には、第１狭角画像と第２狭角画像との間で対応する対応点を検索すると共に、第２狭角画像と第３狭角画像との間で対応する対応点を検索して、第１狭角画像、第２狭角画像、及び第３狭角画像の間で対応する点を検索する。そして、対応点の第１狭角画像及び第２狭角画像の各々の座標から、３次元座標を計算し、第１狭角画像及び第３狭角画像の各々の座標から、３次元座標を計算する。計算された２つの３次元座標のズレが小さい場合には、最も距離が離れた第１狭角画像及び第３狭角画像から計算された３次元座標を、正しい計測値として採用し、一方、計算された２つの３次元座標のズレが大きい場合には、対応点の対応に誤りがあるとして、計算された２つの３次元座標を棄却する。
【００９０】
また、上記の実施の形態では、画像撮像装置の位置姿勢の変化を表わすものとして、並進ベクトル及び回転行列を算出する場合を例に説明したが、これに限定されるものではなく、画像撮像装置の位置姿勢の変化を表わすものとして他の指標を算出するようにしてもよい。
【００９１】
次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【００９２】
第２の実施の形態では、前回計測された計測対象物上の対応点の３次元座標に基づいて、撮像間隔を決定している点が第１の実施の形態と異なっている。
【００９３】
図５に示すように、第２の実施の形態に係る計測装置２１０は、画像撮像装置１２と、コンピュータ２１６とを備えており、コンピュータ２１６に格納されている計測プログラムは、特徴点抽出部３０と、対応点検索部３２と、運動算出部３４と、運動補間部３６と、３次元座標計測部３８と、出力部４０と、３次元座標計測部３８によって前回計測された計測対象物の３次元座標に基づいて、画像撮像装置１２の撮像間隔を決定する撮像間隔決定部２４２と、画角決定部４４とを備えている。
【００９４】
撮像間隔決定部２４２は、前回計測された計測対象物の３次元座標に基づいて、撮像間隔として、広い画角の画像間での対応点の画像上での動き量の予測値の平均又は最大値が、しきい値以上となる時間を決定する。例えば、前回計測された計測対象物の３次元座標から得られる車両からの距離が遠いほど、動き量の予測値が小さくなるため、撮像間隔が広くなるように撮像間隔時間を長く決定し、広い画角の画像間での対応点の画像上での動き量の予測値の平均又は最大値が、しきい値以上となるようにする。
【００９５】
次に第２の実施の形態に係る計測処理ルーチンについて図６を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【００９６】
まず、ステップ１００において、初期設定として、広い画角と狭い画角とを、撮像部１８の予め定められた最大画角に設定し、ステップ２５０で、撮像間隔時間ｔとして初期値を設定し、初期値の撮像間隔時間ｔで連続撮像するように、画像撮像装置１２の撮像間隔を制御する。
【００９７】
次のステップ１０６では、画像撮像装置１２から、第１広角画像、第１狭角画像、第２広角画像、第２狭角画像、及び第３広角画像を順に取得する。そして、ステップ１０８において、第１広角画像と第２広角画像との各々から特徴点を複数抽出し、ステップ１１０で、第１広角画像と第２広角画像との間で対応する対応点を少なくとも８組検索し、そして、ステップ１１２において、第１広角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第２広角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。
【００９８】
そして、ステップ１１４において、第２広角画像と第３広角画像との各々から特徴点を複数抽出し、ステップ１１６で、第２広角画像と第３広角画像との間で対応する対応点を少なくとも８組検索し、そして、ステップ１１８において、第２広角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第３広角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。
【００９９】
そして、ステップ１２０において、第１狭角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出し、次のステップ１２２において、第２狭角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。そして、ステップ１２４において、第１狭角画像を撮像したときの画像撮像装置１２の位置姿勢を基準とした第２狭角画像を撮像したときの画像撮像装置１２の位置姿勢の変化を表わす並進ベクトル及び回転行列を算出する。
【０１００】
次のステップ１２６では、第１狭角画像と第２狭角画像との各々から特徴点を複数抽出し、ステップ１２８で、第１狭角画像と第２狭角画像との間で対応する対応点を少なくとも１組検索し、次のステップ１３０において、対応点が示す計測対象物上の特徴点の３次元座標を計算し、ステップ１３２において、計算された計測対象物の特徴点の３次元座標を、コンピュータ１６のディスプレイに表示する。
【０１０１】
そして、ステップ１３４において、上記ステップ１３４で計算された３次元座標に基づいて、狭い画角を決定し、画像撮像装置１２の狭い画角が、決定された狭い画角となるように制御する。次のステップ２５２では、上記ステップ１３４で計算された３次元座標に基づいて、撮像間隔時間を決定し、画像撮像装置１２の撮像間隔が、決定された撮像間隔時間となるように制御する。これにより、設定された広い画角と新たに決定された狭い画角とに切り替えながら、新たに決定された撮像間隔時間ｔで、広い画角の画像と狭い画角の画像とを交互に撮像する。そして、ステップ１０６へ戻り、設定された広い画角の画像と新たに決定された狭い画角の画像とに基づいて、次の計測タイミングにおける計測対象物上の特徴点の３次元座標を計測する。
【０１０２】
このように、前回計測された３次元座標の車両からの距離が遠いほど、広くなるように撮像間隔を決定することにより、画像間で検索される対応点の動きが大きくなるため、画像撮像装置の運動推定の精度を向上させることができる。
【図面の簡単な説明】
【０１０３】
【図１】本発明の第１の実施の形態に係る計測装置を示すブロック図である。
【図２】画角を狭い画角と広い画角とに切り替えながら連続して撮像する様子を示すイメージ図である。
【図３】位置姿勢の変化を表わす並進ベクトル及び回転行列を説明するための図である。
【図４】本発明の第１の実施の形態に係る計測装置における計測処理ルーチンの内容を示すフローチャートである。
【図５】本発明の第２の実施の形態に係る計測装置を示すブロック図である。
【図６】本発明の第２の実施の形態に係る計測装置における計測処理ルーチンの内容を示すフローチャートである。
【符号の説明】
【０１０４】
１０、２１０  計測装置
１２   画像撮像装置
１４   速度計測部
１６、２１６  コンピュータ
１８   撮像部
２４   画角制御部
２６   撮像制御部
３０   特徴点抽出部
３２   対応点検索部
３４   運動算出部
３６   運動補間部
３８   ３次元座標計測部
４２、２４２  撮像間隔決定部
４４   画角決定部
【図１】
2008298685.tif 000014
【図２】
2008298685.tif 000015
【図３】
2008298685.tif 000016
【図４】
2008298685.tif 000017
【図５】
2008298685.tif 000018
【図６】
2008298685.tif 000019