JP2011112507A - ３次元位置計測装置および３次元位置計測方法 - Google Patents

Abstract

【課題】３次元位置の計測精度を向上させる。
【解決手段】３次元位置計測装置１は、移動体に設置された複数のカメラ＃１〜＃ｎと、カメラ＃１〜＃ｎで撮影された映像から任意の対象物の３次元位置を計測する３次元位置計測部１ａとを備える。カメラ＃１〜＃ｎは、少なくとも１組のカメラ対が共通視野を持つように配置する。また、３次元位置計測部１ａは、共通視野の映像に対して複眼ステレオ視制御により周囲の３次元位置を計測して移動体の移動量を推定する。そして、推定した移動量にもとづき、単眼移動ステレオ視制御により対象物の３次元位置を計測する。
【選択図】図１

Description

本発明は、撮影映像から任意物体の３次元位置を計測する３次元位置計測装置および３次元位置計測方法に関する。

近年、車両にカメラを取り付け、車両前方や後方の映像を撮影して、車両周辺の物体の３次元位置を計測する装置が開発されている。カメラで撮影した映像から３次元位置を計測する方法としては、複眼ステレオ視を用いるのが一般的である。

複眼ステレオ視では、空間的に異なる位置に設置した複数台のカメラで撮影した映像群のそれぞれから、物体の同じ位置を特定することで、映像中の位置の違い（視差）を算出して、三角測量の原理で３次元位置を定めることができる。

また、１台のカメラで３次元位置を計測する方法として、単眼移動ステレオ視あるいはSFM（Structure From Motion）と呼ばれる方法がある。これは、１台のカメラによる時系列の映像を用いて、得られた複数の映像から物体の特徴的な部分（特徴点）を追跡して、映像に映る物体の３次元位置を算出するものである。

３次元位置を計測する従来技術として、複眼ステレオ視を用いた技術が例えば、特許文献１、２に提案されている。また、単眼移動ステレオ視を用いた技術が例えば、特許文献３に提案されている。さらに、複眼ステレオ視および単眼移動ステレオ視の両方を用いた技術が例えば、特許文献４に提案されている。

特開平５−１１４０９９号公報 特開２００５−２４４６３号公報 特開２００７−２５６０２９号公報 特開２００７−２６３６５７号公報

複眼ステレオ視で３次元位置を計測する場合、計測精度は、ステレオを構成するカメラ間の設置距離（ベースライン長）に依存する。ベースライン長を大きく取ればそれだけ計測距離の精度は向上するが、ステレオが形成されるようにカメラ装置を取り付ける対象の大きさは一般に限られており、ベースライン長をあまり大きく取れないことが多い。

また、カメラ装置から離れた位置にある物体の計測精度を向上させるべく、ベースライン長を大きく取ったとしても、ステレオカメラから近い場所に計測対象が位置するときは、それぞれのカメラから対象物を映したときの視線方向の違いが大きくなる。

このため、それぞれのカメラ映像で物体の異なる位置を撮影することになるため、映像中の同一物体の形状等のずれが大きくなり、ステレオ視で重要な映像間の照合に失敗して、視差を算出できないおそれがあった。

さらに、近方物体に合わせてベースライン長を小さくした場合、遠方物体では視差が小さくなり、遠方物体の３次元位置の計測精度が下がってしまう。このように複眼ステレオ視を単体で使用した場合では、固定ベースライン長に起因する欠点があった。

一方、単眼移動ステレオ視の場合は、３次元位置の計測精度の基準となるベースライン長は、装置を取り付けた移動体の移動そのものである。このため、観測時間を長く取りつつ、移動に伴う映像中の特徴点の位置の追跡を行えば、事実上のベースライン長を大きくすることができ、３次元位置の計測精度を高めることができる。

しかし、単眼移動ステレオを単体で使用した場合、移動体の移動量が不明であるため、特徴点間の３次元空間での相対的な位置関係はわかるものの、移動量の絶対値すなわちスケールがわからないと、具体的な３次元位置が算出できない。

このため、一般的には、外部に別のセンサーを設けて移動体の移動量を計測し、これを利用して物体の３次元位置を算出する方法を採っている。しかしながら、外部センサーによる移動体の移動量の計測では、ノイズや時間遅れを伴うことが多いため、映像の撮像フレームレートに同期した精度の高い移動量の取得が困難であった。

このように、複眼ステレオ視と単眼移動ステレオ視とを単体で使用した場合には、それぞれ上記のような問題点があり、従来においては、これら問題点をカバーするように、複眼ステレオ視と単眼移動ステレオ視の両方を適切に用いて、３次元位置の計測精度を高める制御が行われていなかった。

なお、特許文献４では、複眼ステレオ視と単眼移動ステレオ視の両方を使用している。しかし、複眼ステレオ視と単眼移動ステレオ視とを単純に組み合わせて、それぞれから個別に３次元位置を取得する構成となっているため、複眼ステレオ視と単眼移動ステレオ視の持つそれぞれの問題点を継承しており、３次元位置の計測精度を高めることができない。

本発明はこのような点に鑑みてなされたものであり、複眼ステレオ視と単眼移動ステレオ視の両方を適切に組み合わせて、３次元位置の高品質な計測を行うことで、３次元位置の計測精度を向上させた３次元位置計測装置を提供することを目的とする。

また、本発明の他の目的は、複眼ステレオ視と単眼移動ステレオ視の両方を適切に組み合わせて、３次元位置の高品質な計測を行うことで、３次元位置の計測精度を向上させた３次元位置計測方法を提供することである。

上記課題を解決するために、３次元位置計測装置が提供される。この３次元位置計測装置は、移動体に設置され、少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラで撮影された映像を受け付け、共通視野の映像に基づいて移動体の移動量を推定する複眼ステレオ視制御部と、複眼ステレオ視制御部によって推定された移動量にもとづき、映像内の物体の３次元位置を計測する単眼移動ステレオ視制御部とを備える。

３次元位置の計測精度を向上させることが可能になる。

３次元位置計測装置の構成例を示す図である。 ３次元位置計測装置の構成例を示す図である。 カメラの配置例を示す図である。 特徴点の検出例を示す図である。 ２時刻間での時系列の対応決定を説明するための図である。 映像照合を説明するための図である。 特徴点の新規追加処理を説明するための図である。 ステレオ対応点探索の動作例を示す図である。 ３次元位置推定の動作を説明するための図である。 時刻ｔでのカメラの局所座標系から移動体の基準座標系への変換を説明するための図である。 時刻（ｔ−ｍ）と現時刻ｔでの移動体の座標関係を説明するための図である。 単眼映像での特徴点に対する距離の算出に関する時系列探索を説明するための図である。 特徴点の追跡状況と使用する時刻間隔の関係を説明するための図である。 時刻（ｔ−ｍ）と時刻ｔでの単眼カメラの座標系の関係を示す図である。 ３次元位置計測装置の構成例を示す図である。 単眼移動ステレオ処理部での追跡結果を複眼ステレオ処理部で利用することを示す図である。 特徴点登録部の動作を示す図である。

以下、本発明の実施の形態を図面を参照して説明する。図１は３次元位置計測装置の構成例を示す図である。３次元位置計測装置１は、移動体に設置された複数のカメラ＃１〜＃ｎと、カメラ＃１〜＃ｎで撮影された映像から任意の対象物の３次元位置を計測する３次元位置計測部１ａとを備える。３次元位置計測部１ａは、単眼移動ステレオ視制御部１ａ−１と複眼ステレオ視制御部１ａ−２を含む。

ここで、カメラ＃１〜＃ｎは、少なくとも１組のカメラ対が共通視野を持つように配置する（図の例ではカメラ＃１、＃２）。また、複眼ステレオ視制御部１ａ−２は、共通視野の映像に対して複眼ステレオ視制御により周囲の３次元位置を計測して移動体の移動量を推定する。単眼移動ステレオ視制御部１ａ−１は、推定した移動量にもとづき、単眼移動ステレオ視制御により対象物の３次元位置を計測する。

次に３次元位置計測装置１の構成について詳しく説明する。図２は３次元位置計測装置の構成例を示す図である。３次元位置計測装置１−１では、単眼移動ステレオ視制御と複眼ステレオ視制御とを並列的に実行する形態を取っている。

３次元位置計測装置１−１は、単眼移動ステレオ処理部１０−１〜１０−４、複眼ステレオ処理部２０−１、２０−２、移動量推定処理部３０および移動体全体３次元位置推定処理部４０を備える。なお、これら構成要素は、図１で示した３次元位置計測部１ａの機能に含まれるものである。

また、図１で上述した、単眼移動ステレオ視制御部１ａ−１は、単眼移動ステレオ処理部１０−１〜１０−４および移動体全体３次元位置推定処理部４０の動作を行い、複眼ステレオ視制御部１ａ−２は、複眼ステレオ処理部２０−１、２０−２および移動量推定処理部３０の動作を行う。

単眼移動ステレオ処理部１０−１〜１０−４はそれぞれ、フロー推定部１１−１〜１１−４および３次元位置推定部１２−１〜１２−４を有する。複眼ステレオ処理部２０−１、２０−２はそれぞれ、時系列特徴点対応処理部２１−１、２１−２、ステレオ対応点探索部２２−１、２２−２および３次元位置推定部２３−１、２３−２を有する。

ここで、複眼ステレオ処理部２０−１、２０−２は、共通視野を持つ２台のカメラごとに、時系列的にその２台のカメラから同時に取得される２枚の画像から、移動体周辺にある同じ物体を特定し、その物体の時系列的な３次元位置を計測する。

移動量推定処理部３０は、複眼ステレオ処理部２０−１、２０−２に接続され、時系列的に得られた移動体周辺にある物体の移動体からの相対的な３次元位置の変化から、移動体の移動量（回転成分、並進成分）を算出する。

単眼移動ステレオ処理部１０−１〜１０−４は、少なくとも２台以上のカメラの各々について時系列的に取得した２枚以上の映像と、移動量推定処理部３０で算出された移動量から、移動体の周辺物体の３次元位置を計測する。

移動体全体３次元位置推定処理部４０は、単眼移動ステレオ処理部１０−１〜１０−４に接続され、少なくとも２台以上のカメラのそれぞれから取得した移動体の周辺物の３次元位置を、所定の基準で選択、統合して、周辺物体の３次元位置を算出し、結果を出力する。

図３はカメラの配置例を示す図である。ここでは、４台のカメラ＃１〜＃４が具備されるものとする。カメラ＃１とカメラ＃２、カメラ＃２とカメラ＃３がそれぞれ互いに共通視野を持つように配置される例を示している。

図２の説明に戻り、各カメラ＃１〜＃４は映像を出力し、その出力は各々の単眼移動ステレオ処理部１０−１〜１０−４に入力される。また、共通視野を持つカメラセットについては、２つのカメラごとに映像出力が各々の複眼ステレオ処理部２０−１、２０−２に入力される（本例では、カメラ＃１とカメラ＃２、カメラ＃２とカメラ＃３が該当する）。

複眼ステレオ処理部２０−１、２０−２は、該当するカメラの共通視野内に対して、時系列間で対応する特徴点を抽出すると共に、それら特徴点についてステレオ処理を行い、周辺物体の３次元位置を計測して、後段の移動量推定処理部３０に出力する（複眼ステレオ処理部２０−１、２０−２内の各構成要素については後述）。

移動量推定処理部３０では、複眼ステレオ処理部２０−１、２０−２で算出した時系列で対応の取れた３次元位置情報を元に、自移動体の移動量（回転成分、並進成分）を算出し、自移動体の移動量を出力する。

一方、単眼移動ステレオ処理部１０−１〜１０−４において、フロー推定部１１−１〜１１−４は、入力されたそれぞれのカメラ映像から特徴点を抽出し、前時刻までに追跡を完了している特徴点群と比較して、現時刻で新たに発生した特徴点群を特定する。

そして、新たな特徴点群を前時刻までに追跡を完了している特徴点群に加えた特徴点群全体について、現時刻の入力映像との照合を行うことで、特徴点についてのオプティカルフローを算出し、後段に出力する。

なお、特徴点の各々は過去の複数の時刻でフロー算出を行っているので、現時刻を基準として、過去のＮｓ時刻までの各時刻を開始時刻としたときのフローを保持しているものとする。ただし、遡れる時刻Ｎｓは、特徴点を追跡し始めた時刻までや、追跡をし始めた時刻までとシステムであらかじめ定めた時刻長Ｎの短い方までのいずれかがあり、遡れる時刻Ｎｓは特徴点ごとに異なる。すなわち、前者では、追跡し始めた時刻までが最長となる。一方、後者では、追跡し始めた時刻がＮｓ以下のときは、その追跡数が最長で、追跡し始めた時刻がＮ以上のときはＮが最長となる。

３次元位置推定部１２−１〜１２−４では、前段の特徴点のフロー情報と移動量推定処理部３０で算出された自移動体の移動量情報とにもとづいて、各特徴点に関して３次元位置を算出する。

移動体全体３次元位置推定処理部４０は、各々のカメラに接続された単眼移動ステレオ処理部１０−１〜１０−４の出力である特徴点の３次元位置を入力として、それら３次元位置を統合／選択して、移動体全体としての３次元位置を算出する。

次に複眼ステレオ処理部２０−１、２０−２の各構成要素の動作について詳しく説明する。時系列特徴点対応処理部２１−１、２１−２では、入力される２つの濃淡映像（カメラ映像）のどちらか一方（本例では、カメラ＃１とカメラ＃２のステレオ組ではカメラ＃１の映像を、カメラ＃２とカメラ＃３のステレオ組では、カメラ＃２の映像とする）について、例えばコーナ点のような特徴点を抽出する。

図４は特徴点の検出例を示す図である。入力濃淡映像ａ１に対し、特徴点として、コーナ点を検出している様子を示している。検出方法としては例えば、Harrisオペレータによるコーナ点検出方法などがある。なお、図中の点線は説明のために示しているもので特徴点とは無関係である。

図５は２時刻間での時系列の対応決定を説明するための図である。前時刻（ｔ−１）の濃淡映像ｂ１と、現時刻ｔの濃淡映像ｂ２との対応決定の様子を示している。
最初の時刻では、時系列対応を行わず、抽出された特徴点全体が対応付けの対象となる。また、最初以外の時刻では、前時刻（ｔ−１）で求まっている特徴点のそれぞれについて、図に示すように、前時刻（ｔ−１）の濃淡映像ｂ１と現時刻ｔでの濃淡映像ｂ２とを比較して、同じ箇所を決定する。

図６は映像照合を説明するための図である。前時刻（ｔ−１）の濃淡映像ｂ１と、現時刻ｔの濃淡映像ｂ２との時系列間における対応探索（映像照合）を示している。前時刻（ｔ−１）でのｉ番目の特徴点の位置を（ｘｉ（ｔ−１），ｙｉ（ｔ−１））として、時刻（ｔ−１）での濃淡映像ｂ１の（ｘｉ（ｔ−１），ｙｉ（ｔ−１））を中心とする一辺の大きさがＨの小さな矩形領域Ｒｉ（ｔ−１）を設ける。同様に、現時刻ｔでの濃淡映像ｂ２においても、一辺の大きさが同じＨの矩形領域Ｒｉ（ｔ）を設ける。

そして、Ｒｉ（ｔ）に対して、現時刻ｔでの濃淡映像ｂ２内を２次元的に走査しながら、Ｒｉ（ｔ−１）とＲｉ（ｔ）の２つの領域の濃淡映像の類似性を評価していき、最も類似した箇所を、ｉ番目の特徴点の対応位置として決定する（図６では点線枠内が対応している位置と決定する）。

なお、小領域の濃淡映像の類似性の評価としては、例えばＳＡＤ（Sum of Absolute Differences（差の絶対値和））、ＳＳＤ（Sum of Squared Differences（差の自乗和））、正規化相関など、一般的な映像照合の方式が使用できる。

上記のような操作によって、対応の求まった特徴点は、現時刻ｔでの対応位置を新たな位置とする特徴点となる。また、特徴点は、追跡できた回数をカウントするものとし、この操作によって対応が求まると追跡回数を１だけ加算する。

図７は特徴点の新規追加処理を説明するための図である。現時刻ｔの濃淡映像ｂ１に対して、先と同じ方法で特徴点（例えばコーナ点）を検出しておき、現時刻ｔで対応の求まった特徴点群の位置と比較して、新たに発生した特徴点を特定して、これらを特徴点群に加える新規特徴点の追加処理を行う。

図中に現時刻で検出された特徴点の映像ｂ２−１を示す。なお、新規追加の特徴点の追跡回数は０である。新規特徴点を加えた特徴点群全体を、時系列特徴点対応処理部２１−１、２１−２の出力とする。

次に複眼ステレオ処理部２０−１、２０−２内のステレオ対応点探索部２２−１、２２−２について説明する。図８はステレオ対応点探索の動作例を示す図である。現時刻の一方のカメラ映像ｂ３−１と、現時刻の他方のカメラ映像ｂ３−２との間の映像照合を示している。

ステレオ対応点探索部２２−１、２２−２では、先の時系列特徴点対応処理部２１−１、２１−２で求まった特徴点群を対象に、他方カメラの濃淡映像との間でステレオ照合処理（ステレオ間での対応点探索）を実施する。

例えば、カメラ＃１とカメラ＃２のステレオ組の映像が入力する複眼ステレオ処理部２０−１内のステレオ対応点探索部２２−１では、カメラ＃１の映像の特徴点群を現時刻の一方のカメラ映像ｂ３−１とすれば、カメラ＃２の映像側を現時刻の他方のカメラ映像ｂ３−２として、ステレオ照合処理を行う。

また、カメラ＃２とカメラ＃３のステレオ組の映像が入力する複眼ステレオ処理部２０−２内のステレオ対応点探索部２２−２では、カメラ＃２の映像の特徴点群を現時刻の一方のカメラ映像ｂ３−１とすれば、カメラ＃３の映像側を現時刻の他方のカメラ映像ｂ３−２として、ステレオ照合処理を行う。

なお、照合方法は、特徴点を中央にして一定の大きさの小領域を設け、領域内の輝度値の差の絶対値和（ＳＡＤ）や、輝度差の自乗和（ＳＳＤ）など、一般的なステレオ対応付け処理が適用できる。照合スコアが閾値以上ならば、照合成功としてその特徴点を残し、閾値未満なら現時刻において追跡に失敗したとして、特徴点を削除する。

次に複眼ステレオ処理部２０−１、２０−２内の３次元位置推定部２３−１、２３−２について説明する。図９は３次元位置推定の動作を説明するための図である。一方のカメラ映像ｂ４−１と、他方のカメラ映像ｂ４−２との間での３次元位置推定の様子を示している。ステレオ照合に成功した特徴点は、当該カメラを中心とした座標系での３次元位置を算出する。

ここで、ｋ番目のステレオカメラセットの現時刻ｔにおける、ｉ番目の特徴点の映像での横縦の座標位置を（ｕｘ^t _k,i，ｕｙ^t _k,i）（各画素位置）とし、カメラの焦点距離をｆ（画素）、ステレオを形成するカメラのベースライン長をｂ（ｍｍ）とする。さらに、ステレオ対応で求まった視差量をｄ^t _k,i（画素）とすると、対応する３次元位置Ｕ^t _k,i＝（ＵＸ^t _k,i，ＵＹ^t _k,i，ＵＹ^t _k,i）は式（１ａ）〜（１ｃ）で与えられる。

ＵＸ^t _k,i＝ｕｘ^t _k,i・ｄ^t _k,i／ｂ・・・（１ａ）
ＵＹ^t _k,i＝ｕｙ^t _k,i・ｄ^t _k,i／ｂ・・・（１ｂ）
ＵＺ^t _k,i＝ｆ・ｂ／ｄ^t _k,i ・・・（１ｃ）
次に移動量推定処理部３０について説明する。移動量推定処理部３０では、複眼ステレオ処理部２０−１、２０−２から得られた特徴点の３次元位置情報を用いて、現時刻ｔを基準として、過去時刻（ｔ−ｍ）（ただし、ｍは１、２、３、・・・、Ｎで、Ｎはシステムで定義する最大時間である）とのそれぞれの間での移動体の移動量を推定する。

図１０は時刻ｔでのカメラの局所座標系から移動体の基準座標系への変換を説明するための図である。ここで、ｋ番目の複眼ステレオ処理部のｉ番目の特徴点について、現時刻ｔでの３次元位置をＵ^t _k,i＝（ＵＸ^t _k,i，ＵＹ^t _k,i，ＵＺ^t _k,i）、過去時刻（ｔ−ｍ）での３次元位置をＵ^(t-m) _k,i＝（ＵＸ^(t-m) _k,i，ＵＹ^(t-m) _k,i，ＵＺ^(t-m) _k,i）とする。

これら３次元座標は、ｋ番目のカメラについての局所的な座標系で表記されているので、これを移動体に付する基準となる座標系に移すことになる。
時刻ｔでの３次元位置Ｕ^t _k,iを時刻ｔでの基準座標系Ｘｗ^(t)に移した座標をＰ^t _k,i、時刻（ｔ−ｍ）での３次元位置Ｕ^(t-m) _k,iを時刻（ｔ−ｍ）での基準座標系Ｘｗ^(t-m)に移した座標をＰ^(t-m) _k,iとする。

このとき、ｋ番目のカメラへの座標変換パラメータＲｋ，Ｔｋ（Ｒｋは回転行列を、Ｔｋは並進移動ベクトルを表し、ｘｃ^(t)＝Ｒｋ・（Ｘｗ^(t)−Ｔｋ））を使って、以下の式（２ａ）、（２ｂ）のように表される。

図１１は時刻（ｔ−ｍ）と現時刻ｔでの移動体の座標関係を説明するための図である。時刻（ｔ−ｍ）から現時刻ｔでの移動体の並進成分（Ｔｘ^m，Ｔｙ^m）は、以下の拘束式を満足する。

（Ｔｘ^m−ＰＸ^t _k,i）²＋（Ｔｙ^m−ＰＹ^t _k,i）²＝（ＰＸ^(t-m) _k,i）²＋（ＰＹ^(t-m) _k,i）²・・・（３）
つまり、（Ｔｘ^m，Ｔｙ^m）は、中心（ＰＸ^t _k,i，ＰＹ^t _k,i）で半径（（ＰＸ^(t-m) _k,i）²＋（ＰＹ^(t-m) _k,i）²）^1/2の円上の点が解となる。これは、３次元点が１点だと無数に解が存在することになる。

理論的には、３点の対応点を用いれば、３つの円が１点で重なり、一意に（Ｔｘ^m，Ｔｙ^m）を定めることができる。実際には、観測される３次元位置は誤差を含んでいるので、全てのステレオカメラセットの全ての特徴点について、（Ｔｘ，Ｔｙ）空間に上記の円の位置に投票を行い、（Ｔｘ，Ｔｙ）投票空間でピークを与えるＴｘ，Ｔｙを定めることで、全ての特徴点について尤もらしい並進成分（Ｔｘ^m，Ｔｙ^m）が求まる。並進成分が求まれば、ｉ番目の３次元特徴点に対応する回転成分（回転角）（ａ^m _i）が以下の式（４）で求まる。

また、全ての３次元特徴点について、先に算出した回転角（ａ^m _i）の平均を求め、尤もらしい回転角ａ^mを以下の式（５）のように定める。この演算をｍ＝１、２、３、・・・、Ｎの各時刻間で行う。

次に単眼移動ステレオ処理部１０−１〜１０−４の各構成要素の動作について詳しく説明する。フロー推定部１１−１〜１１−４では、入力される濃淡映像について、例えばコーナ点のような特徴点を抽出する。コーナ点検出としては、例えばHarrisオペレータによるコーナ点検出方法がある。

最初の時刻での処理では、抽出されたコーナ点全体が特徴点となり、追跡回数は０となる。最初以外の時刻では、まず、前時刻までに求まっている特徴点群の各位置に対して、前時刻の濃淡映像と現時刻の濃淡映像との間で映像照合処理を実施する。

照合方法は、特徴点を中央にして一定の大きさの小領域を設け、領域内の輝度値の差の絶対値和（ＳＡＤ）や、輝度差の自乗和（ＳＳＤ）など、一般的な映像間の対応付け処理が適用できる。

そして、照合スコアが閾値以上ならば、照合成功としてその特徴点を残し、閾値未満なら現時刻において追跡に失敗したとして、特徴点を削除する。さらに、照合に成功した特徴点群と、現時刻で求めたコーナ点などの特徴点群とを比較して、新たに発生した特徴点を特定して、これらを前時刻までに対応が求まっている特徴点群に加えた特徴点群全体を定める。ただし、新たに発生した特徴点の追跡数は０である。これらの動作は、先の複眼ステレオ処理部で説明した内容とほぼ同じである。

なお、単眼移動ステレオ視制御と複眼ステレオ視制御とで行う、映像内の同じ物体からの投影像を時系列的に対応付けて追跡する処理は、上述のように、単眼移動ステレオ視制御と複眼ステレオ視制御とで各々独立して行うものである。これにより、並列動作を実行することとなり処理効率を高めることが可能になる。

次に単眼移動ステレオ処理部１０−１〜１０−４内の３次元位置推定部１２−１〜１２−４について説明する。３次元位置推定部１２−１〜１２−４は、現時刻までの時系列的なフローが求まった特徴点に対して、複眼ステレオ処理部２０−１、２０−２によって現時刻ｔと過去時刻（ｔ−ｍ）との間で求まった自移動体の移動量情報（回転成分、並進成分）を使って、特徴点の３次元位置を定める。

図１２は単眼映像での特徴点に対する距離の算出に関する時系列探索を説明するための図である。図１２では、最大過去時刻（ｔ−Ｎ）から現時刻ｔの順に調べていき、追跡が行えており、かつ距離が定まっていない最大の過去時刻間で３次元位置を算出する様子を示している。

具体的なステップとしては、最も過去の時刻ｍ＝Ｎ（Ｎはシステムで決めた最大時刻）から順に現時刻に近付く順番にｍを順次変えていく（ｍ＝Ｎ、Ｎ−１、Ｎ−２、・・・、１の順）。

そして、その過去時刻と現時刻との間で追跡が行えており、かつｍの走査によって３次元位置が定まっていない特徴点群を対象に、３次元位置を定める操作を繰り返す。このように、時間的にできるだけ離れた時刻間での特徴点を用いることで、距離精度を決定する自移動体の移動量をできるだけ大きくし、距離の精度を向上させる。

ただし、ある過去時刻（ｔ−ｍ^*）（Ｎ≦ｍ^*＜１）で３次元位置が求まった特徴点は、それ以降の過去時刻（ｔ−ｍ^p）（ｍ^p＜ｍ^*）でも追跡が行えているので、３次元位置を定めることが可能である。

この場合、時刻ｍ^*とそれ以降の時刻ｍ^p（１＜ｍ^p＜ｍ^*）の追跡ができている時刻間で求められる全ての３次元位置を使って、例えば平均値のように、その特徴点の尤もらしい３次元位置を定めることもできる。また、ある特徴点に着目して既に３次元位置が定まっている場合には、それ以降の時刻間では３次元位置を算出しないこともできる。後者は、最も過去の時刻との間とだけで３次元位置を定めることを意味する。

図１３は特徴点の追跡状況と使用する時刻間隔の関係を説明するための図である。特徴点Ａは、過去時刻（ｔ−４）から現時刻ｔまで時系列的な対応が求まっており、この場合は時刻（ｔ−４）と時刻ｔとの間の移動量を用いて３次元位置を算出する。

一方、特徴点Ｂは、過去時刻（ｔ−２）からしか追跡が行えていないので、特徴点Ｂについては、時刻（ｔ−２）と時刻ｔとの間の移動量を用いて３次元位置を算出する。このように、特徴点ごとに異なる最大の追跡量を使って３次元位置を算出する。

次に過去時刻（ｔ−ｍ）の場合について、具体的な操作について説明する。図１４は時刻（ｔ−ｍ）と時刻ｔでの単眼カメラの座標系の関係を示す図である。ｋ番目のカメラのｉ番目の特徴点について、時刻（ｔ−ｍ）での映像特徴点の位置をｎ^(t-m) _k,i＝（ｎｘ^(t-m) _k,i，ｎｙ^(t-m) _k,i，ｎｚ^(t-m) _k,i）、時刻ｔでの映像特徴点の位置をｎ^(t) _k,i＝（ｎｘ^(t) _k,i，ｎｙ^(t) _k,i，ｎｚ^(t) _k,i）とする。

また、ｋ番目のカメラについての移動体上に設けた基準座標系Ｘｗ^(t)からカメラ座標系ｘｃ^(t)への変換パラメータをＲｋ、Ｔｋ（Ｒｋは回転行列を、Ｔｋは並進移動ベクトルを表し、ｘｃ^(t)＝Ｒｋ・（Ｘｗ^(t)−Ｔｋ））とし、時刻（ｔ−ｍ）での移動体の基準座標系Ｘｗ^(t-m)から、時刻ｔでの基準座標系Ｘｗ^(t)への変換パラメータをＲｔ^m、Ｔｔ^m（Ｒｔ^mは回転行列を、Ｔｔ^mは並進移動ベクトルを表し、Ｘｗ^t＝Ｒｔ^m・（Ｘｗ^(t-m)−Ｔｔ^m））とする。

このとき、回転行列Ｒｔ^mは、式（６）となり、並進移動ベクトルＴｔ^mは、式（７）となる。

ここで、以下の式（８）、（９）による変換を導入する。

さらに、時刻ｔでの映像特徴点の位置ｎ^(t) _k,iをＲｖ^mを用いて、以下の式（１０）のように変換する。

このとき、時刻ｔでのカメラ座標系でのｉ番目の特徴点の距離ｌ^(t) _k,iは以下の式（１１）で与えられる。

したがって、ｉ番目の特徴点のｋ番目のカメラ座標系での３次元位置Ｑ^(t) _k,iは以下の式（１２）となる。

次に移動体全体３次元位置推定処理部４０について説明する。移動体全体３次元位置推定処理部４０では、各カメラ映像から単眼移動ステレオ処理部１０−１〜１０−４で得られた特徴点の３次元位置Ｑ^(t) _k,iについて、時刻ｔでの移動体に取った基準座標系Ｘｗ^(t)に座標変換し、全てのカメラからの特徴点の３次元位置の座標軸を合わせる。

具体的には、移動体の基準座標系Ｘｗ^(t)からｋ番目のカメラの座標系への変換パラメータをＲｋ、Ｔｋ（Ｒｋは回転行列を、Ｔｋは並進移動ベクトルを表す）とするとき、ｋ番目のカメラのｉ番目の特徴点のカメラ座標系での３次元位置Ｑ^(t) _k,iは、以下の変換式（１３）によって、移動体基準座標系Ｘｗ^(t)での３次元位置Ｓ^(t) _k,i に移される。

Ｓ^(t) _k,i＝Ｒｋ^T・Ｑ^(t) _k,i＋Ｔｋ・・・（１３）
特にカメラ間の共通視野に含まれる特徴点は、それぞれのカメラで特徴位置と３次元位置が求まっているので、重複する３次元位置については、いずれか一方のカメラの特徴点位置で代表させる（選択）、あるいは複数のカメラでの特徴点の３次元位置の平均を取る（統合）など任意の処理を行うことができる。

次に３次元位置計測装置１−１の変形例について説明する。図１５は３次元位置計測装置の構成例を示す図である。３次元位置計測装置１−２は、単眼移動ステレオ処理部１０−１〜１０−４、複眼ステレオ処理部２０ａ−１、２０ａ−２、移動量推定処理部３０および移動体全体３次元位置推定処理部４０を備える。

単眼移動ステレオ処理部１０−１〜１０−４はそれぞれ、フロー推定部１１−１〜１１−４および３次元位置推定部１２−１〜１２−４を含む。複眼ステレオ処理部２０ａ−１、２０ａ−２はそれぞれ、特徴点登録部２１ａ−１、２１ａ−２、ステレオ対応点探索部２２−１、２２−２および３次元位置推定部２３−１、２３−２を含む。

なお、３次元位置計測装置１−２は、図２で示した構成を元に、複眼ステレオ処理部の時系列特徴点対応処理部２１−１、２１−２に代わって、特徴点登録部２１ａ−１、２１ａ−２を設けている。また、この入力として、複眼ステレオ処理部に接続される２つのカメラのうち一方のカメラの映像を処理する単眼移動ステレオ処理部でのフロー推定部１１−１、１１−２で処理された２時刻間の特徴点の追跡結果を入力することを特徴としている。

図１６は単眼移動ステレオ処理部１０−１〜１０−４での追跡結果を複眼ステレオ処理部２０ａ−１、２０ａ−２で利用することを示す図である。３次元位置計測装置１−２は、単眼移動ステレオ処理部１０−１〜１０−４でなされる時系列での特徴点の対応付け（追跡）結果を、複眼ステレオ処理部２０ａ−１、２０ａ−２で直接使用する。

すなわち、単眼移動ステレオ処理では、時系列的に同じ物体からの投影像を対応付けて追跡した結果である、過去時刻と対応の取れた映像上の位置座標群を求め、複眼ステレオ処理では、位置座標群についてステレオ対応付け処理で３次元位置を計測する。

これにより、図２の３次元位置計測装置１−１で行っていた複眼ステレオ処理部での特徴点の時系列対応付けの処理を省き、処理の効率化を図ることが可能になる。なお、その他の構成については、図２の３次元位置計測装置１−１と同じなので説明を省略する。

次に特徴点登録部２１ａ−１、２１ａ−２について説明する。図１７は特徴点登録部２１ａ−１、２１ａ−２の動作を示す図である。特徴点登録部２１ａ−１、２１ａ−２では、単眼移動ステレオ処理部１０−１、１０−２のフロー推定部１１−１、１１−２で決定された、時刻ｔで時系列の対応が求まった特徴点と、時刻ｔで新たに加えられた特徴点とについて、ステレオ照合を行うための映像上に特徴点位置を配置する処理を行う。

ここで上記の「配置」とは、単眼移動ステレオ処理部１０−１、１０−２では、時系列での照合がやりやすいように独自の映像形状に形状変換してから特徴を抽出して時系列照合を行う場合や、複眼ステレオ処理部２０ａ−１、２０ａ−２でも、ステレオ照合がやりやすいように独自の映像形状に形状変換してから処理する場合などがある。このように、単眼移動ステレオ処理部１０−１、１０−２で算出される特徴位置と、複眼ステレオ処理部２０ａ−１、２０ａ−２で必要とする特徴位置とが一致しない場合に備えての処置である。

すなわち、特徴点登録部２１ａ−１、２１ａ−２は、フロー推定部１１−１、１１−２で決定された特徴位置を複眼ステレオ処理部２０ａ−１、２０ａ−２のステレオ照合に必要な特徴位置に変換する機能を実現するためのものである。

仮に、単眼移動ステレオ処理部１０−１、１０−２での映像形状と、複眼ステレオ処理部２０ａ−１、２０ａ−２での映像形状とが一致する場合は、特徴点登録部２１ａ−１、２１ａ−２は特別な処理を必要としない。よって、単純に単眼移動ステレオ処理部１０−１、１０−２で得られた時刻ｔでの時系列的な対応が求まった特徴位置をそのまま使用するだけである。

以上説明したように、本発明によれば、共通視野での複眼ステレオ視で求まった外界の３次元位置情報を元に移動体の移動情報（回転成分と並進成分）を計測することができるので、従来の単眼移動ステレオで問題であったスケールファクタ未知の問題を解決し、単眼移動ステレオによって外界物体の完全な３次元位置を計測することができる。

また、このとき、移動体の移動情報は、既に搭載されているカメラを使って獲得するので、特別な外部センサーを用いる必要がない。また、カメラの映像から移動情報を算出するため、撮影間隔と完全に同期した移動情報を獲得することができる。

さらに、単眼移動ステレオによる３次元位置計測では、映像内の特徴点を時系列的に追跡することで３次元位置を計測するが、時間的に近接した映像間では映像の変化は小さく視線方向の変化も少ないため、映像中の多くの特徴点について時系列の追跡が容易にできる。この追跡を移動体の移動に伴い、適切な回数だけ繰り返すことで、カメラから物体までの距離に寄らず、空間的に密に精度の高い３次元位置を計測することが可能となる。

つまり、カメラからの距離が近い外界物体は、時系列での映像上の移動量が大きく、このような近い物体には短い時間間隔（すなわち短い移動ベースライン長）で特徴点の時系列的な移動量（視差）を算出して３次元位置を計測する。

また、カメラからの距離が遠い外界物体は、時系列での映像上の移動量が小さいため、長い時間間隔（すなわち長い移動ベースライン長）で映像上での特徴点の移動量を確保し、遠方物体の３次元位置を精度良く計測する。

このように、対象の距離に応じたマルチベースライン方式を適用することになるので、カメラから物体までの距離に寄らず、高精度に周辺物体の３次元位置を計測することが可能となる。

なお、このことは、複眼ステレオ視が持っている固定ベースライン長に起因する問題（対象との距離の関係による物体の３次元位置の計測可能性と計測精度の不均一性との問題）を回避することになる。

なお、上記の処理機能は、コンピュータによって実現することができる。その場合、３次元位置計測装置１が有すべき機能の処理内容を記述したプログラムが提供される。そのプログラムをコンピュータで実行することにより、上記処理機能がコンピュータ上で実現される。処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。

コンピュータで読み取り可能な記録媒体としては、磁気記憶装置、光ディスク、光磁気記録媒体、半導体メモリなどがある。磁気記憶装置には、ハードディスク装置（ＨＤＤ）、フレキシブルディスク（ＦＤ）、磁気テープなどがある。光ディスクには、ＤＶＤ、ＤＶＤ−ＲＡＭ、ＣＤ−ＲＯＭ／ＲＷなどがある。光磁気記録媒体には、ＭＯ（Magneto-Optical disc）などがある。

プログラムを流通させる場合には、例えば、そのプログラムが記録されたＤＶＤ、ＣＤ−ＲＯＭなどの可搬型記録媒体が販売される。また、プログラムをサーバコンピュータの記憶装置に格納しておき、ネットワークを介して、サーバコンピュータから他のコンピュータにそのプログラムを転送することもできる。

また、上記の処理機能の少なくとも一部を、ＤＳＰ（Digital Signal Processor）、ＡＳＩＣ（Application Specific Integrated Circuit）、ＰＬＤ（Programmable Logic Device）などの電子回路で実現することもできる。

なお、３次元位置計測装置１が適用される製品としては、例えば、車両の前後左右や、後と横など一部分などに取り付けた死角補助カメラで、車両周囲の物体の３次元位置を計測し、立体感のある俯瞰表示を行う表示装置などに適用可能である。または、周囲の衝突可能性のある物体を検出して運転者に知らせる安全支援システムなどにも幅広く適用可能である。

以上、実施の形態を例示したが、実施の形態で示した各部の構成は同様の機能を有する他のものに置換することができる。また、他の任意の構成物や工程が付加されてもよい。さらに、前述した実施の形態のうちの任意の２以上の構成（特徴）を組み合わせたものであってもよい。

（付記１） 移動体に設置され、少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラで撮影された映像を受け付け、前記共通視野の映像に基づいて前記移動体の移動量を推定する複眼ステレオ視制御部と、
前記複眼ステレオ視制御部によって推定された前記移動量にもとづき、前記映像内の物体の３次元位置を計測する単眼移動ステレオ視制御部と、
を有することを特徴とする３次元位置計測装置。

（付記２） 前記単眼移動ステレオ視制御部と前記複眼ステレオ視制御部とで行う、前記映像内の物体からの投影像を時系列的に対応付けて追跡する処理を、各々独立して行うことを特徴とする付記１記載の３次元位置計測装置。

（付記３） 前記単眼移動ステレオ視制御部は、時系列的に同じ物体からの投影像を対応付けて追跡した結果である、過去時刻と対応の取れた映像上の位置座標群を求め、前記複眼ステレオ視制御部では、前記求められた位置座標群について任意の対象物の３次元位置を計測することにより前記移動体の移動量を推定することを特徴とする付記１記載の３次元位置計測装置。

（付記４） 前記複眼ステレオ視制御部は、前記複眼ステレオ視制御部が特定した特徴点の３次元位置情報を用いて、現時刻ｔを基準として、過去時刻（ｔ−ｍ）（ｍは１、２、３、・・・、Ｎで、Ｎはシステムで定義する最大時間）の時刻とのそれぞれの間での前記移動体の前記移動量を推定し、
前記単眼移動ステレオ視制御部は、現時刻までの時系列的なフローが求まった前記特徴点に対して、最も過去の時刻ｍ＝Ｎから順に現時刻に近付く順番に、ｍを順次変えていきながら（ｍ＝Ｎ、Ｎ−１、Ｎ−２、・・・１の順）、過去時刻と現時刻との間で追跡が行えており、かつｍの走査によって３次元位置が定まっていない特徴点群を対象に、３次元位置を定める操作を繰り返し、最も過去の時刻との間とだけで３次元位置を定める、
ことを特徴とする付記１記載の３次元位置計測装置。

（付記５） ３次元位置計測方法において、
移動体に少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラで撮影された映像の、
前記共通視野の映像に基づき、前記移動体の周囲の任意の対象物の３次元位置を計測して前記移動体の移動量を推定し、推定した前記移動量にもとづき、前記対象物の３次元位置を計測する、
ことを特徴とする３次元位置計測方法。

（付記６） 任意の対象物の３次元位置の計測制御をコンピュータに実行させる３次元位置計測プログラムにおいて、
前記コンピュータに、
移動体に少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラで撮影された映像の、前記共通視野の映像に基づいて前記移動体の周囲の任意の対象物の３次元位置を計測して前記移動体の移動量を推定し、
推定した前記移動量にもとづき、前記対象物の３次元位置を計測する、
処理を実行させることを特徴とする３次元位置計測プログラム。

（付記７） 移動体に設置され、少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラと、
前記カメラで撮影された映像を受け付け、前記共通視野の映像に基づいて前記移動体の移動量を推定する複眼ステレオ視制御部と、
複眼ステレオ視制御部によって推定された前記移動量にもとづき、前記映像内の物体の３次元位置を計測する単眼移動ステレオ視制御部と、
を有することを特徴とする３次元位置計測装置。

１ ３次元位置計測装置
＃１〜＃ｎ カメラ
１ａ ３次元位置計測部
１ａ−１ 単眼移動ステレオ視制御部
１ａ−２ 複眼ステレオ視制御部

Claims (6)

1. 移動体に設置され、少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラで撮影された映像を受け付け、前記共通視野の映像に基づいて前記移動体の移動量を推定する複眼ステレオ視制御部と、
   前記複眼ステレオ視制御部によって推定された前記移動量にもとづき、前記映像内の物体の３次元位置を計測する単眼移動ステレオ視制御部と、
   を有することを特徴とする３次元位置計測装置。
2. 前記単眼移動ステレオ視制御部と前記複眼ステレオ視制御部とで行う、前記映像内の物体からの投影像を時系列的に対応付けて追跡する処理を、各々独立して行うことを特徴とする請求項１記載の３次元位置計測装置。
3. 前記単眼移動ステレオ視制御部は、時系列的に同じ物体からの投影像を対応付けて追跡した結果である、過去時刻と対応の取れた映像上の位置座標群を求め、前記複眼ステレオ視制御部では、前記求められた位置座標群について任意の対象物の３次元位置を計測することにより前記移動体の移動量を推定することを特徴とする請求項１記載の３次元位置計測装置。
4. 前記複眼ステレオ視制御部は、前記複眼ステレオ視制御部が特定した特徴点の３次元位置情報を用いて、現時刻ｔを基準として、過去時刻（ｔ−ｍ）（ｍは１、２、３、・・・、Ｎで、Ｎはシステムで定義する最大時間）の時刻とのそれぞれの間での前記移動体の前記移動量を推定し、
   前記単眼移動ステレオ視制御部は、現時刻までの時系列的なフローが求まった前記特徴点に対して、最も過去の時刻ｍ＝Ｎから順に現時刻に近付く順番に、ｍを順次変えていきながら（ｍ＝Ｎ、Ｎ−１、Ｎ−２、・・・１の順）、過去時刻と現時刻との間で追跡が行えており、かつｍの走査によって３次元位置が定まっていない特徴点群を対象に、３次元位置を定める操作を繰り返し、最も過去の時刻との間とだけで３次元位置を定める、
   ことを特徴とする請求項１記載の３次元位置計測装置。
5. ３次元位置計測方法において、
   移動体に少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラで撮影された映像の、
   前記共通視野の映像に基づき、前記移動体の周囲の任意の対象物の３次元位置を計測して前記移動体の移動量を推定し、推定した前記移動量にもとづき、前記対象物の３次元位置を計測する、
   ことを特徴とする３次元位置計測方法。
6. 移動体に設置され、少なくとも１組のカメラ対が共通視野を持つように配置された複数のカメラと、
   前記カメラで撮影された映像を受け付け、前記共通視野の映像に基づいて前記移動体の移動量を推定する複眼ステレオ視制御部と、
   複眼ステレオ視制御部によって推定された前記移動量にもとづき、前記映像内の物体の３次元位置を計測する単眼移動ステレオ視制御部と、
   を有することを特徴とする３次元位置計測装置。

Images