JP7297595B2 - 姿勢推定装置、異常検出装置、補正装置、および、姿勢推定方法 - Google Patents

Description

本発明は、カメラの姿勢を推定する姿勢推定装置、カメラの異常を検出する異常検出装置、カメラのパラメータを補正する補正装置、および、カメラの姿勢を推定する姿勢推定方法に関する。

従来、車載カメラを用いて、例えば車両の駐車支援等の運転支援が行われている。車載カメラは、車両を工場から出荷する前に、車両に固定状態で取り付けられる。しかしながら、車載カメラは、例えば不意の接触や経年変化等によって、工場出荷時の取付状態から位置ずれを起こすことがある。車載カメラの位置（車両のボディに対する車載カメラの相対位置）がずれると、カメラ画像を利用して判断されるハンドルの操舵量等に誤差が生じるために、車載カメラの位置ずれを検出することは重要である。

特許文献１には、車載カメラの光軸ずれを検出する技術が開示される。特許文献１における車載カメラの光軸ずれ検出装置は、画像処理手段と判断手段とを備える。画像処理手段は、運転支援する車体部上のマーキングを含む範囲を撮影する車載カメラの撮影画像から該マーキングの位置情報を検出する。判断手段は、初期設定されたマーキング位置情報と、新たに検出されたマーキングの位置情報とを比較することにより、撮影光軸ずれを判断する。

特開２００４－１７３０３７号公報

車載カメラの撮影画像には、車両周囲の風景等が映るために、例えばマーキングがボンネットの一部の特定形状である場合、当該特定形状を簡単に抽出できないことがある。当該特定形状が誤検出されると、車載カメラの光軸ずれが正確に判断されない可能性がある。なお、車載カメラの光軸ずれとは、車載カメラの姿勢が設計通りでないことを意味している。

また、車載カメラの姿勢が設計通りであるか否かを判断するのに要する時間が長ければ、車載カメラの姿勢が設計通りでない状態が発生した場合に当該状態に対する対策の実施が遅れることになる。したがって、車載カメラの姿勢が設計通りであるか否かを判断するのに要する時間は短い方が望ましい。

本発明は、上記課題に鑑みて、カメラの姿勢を精度良く推定でき、推定に要する時間の長期化を抑制できる技術を提供することを目的とする。

本発明に係る姿勢推定装置は、移動体に搭載されたカメラで撮影された撮影画像を取得する取得部と、第１時刻と前記第１時刻より後の第２時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出し、前記第１時刻より後の第３時刻と前記第３時刻より後の第４時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出する抽出部と、前記第１時刻と前記第２時刻で撮影された２つの前記撮影画像から抽出された特徴点から、互いに対応する第１特徴点を選定し、該第１特徴点の位置変化を算出し、前記第３時刻と前記第４時刻で撮影された２つの前記撮影画像から抽出された特徴点から、前記第１特徴点とは異なり、互いに対応する第２特徴点を選定し、該第２特徴点の位置変化を算出し、前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定部と、前記特定部で特定した面の組に基づき前記カメラの姿勢を推定する推定部と、を備える構成（第１の構成）である。

上記第１の構成の姿勢推定装置において、前記第１特徴点の位置変化と前記第２特徴点の位置変化との組合せは、前記移動体の移動情報に基づき決定される構成（第２の構成）であってもよい。

上記第２の構成の姿勢推定装置において、前記移動情報は、前記移動体の舵角情報を含む構成（第３の構成）であってもよい。

上記第２又は第３の構成の姿勢推定装置において、前記移動情報は、前記移動体の速度情報を含む構成（第４の構成）であってもよい。

上記第２～第４いずれかの構成の姿勢推定装置において、前記移動情報は、通信バスを経由して、前記姿勢推定装置に供給される構成（第５の構成）であってもよい。

上記第２～第４いずれかの構成の姿勢推定装置において、前記移動情報は、前記第１特徴点の位置変化を前記所定の平面上に座標変換した結果、及び、前記第２特徴点の位置変化を前記所定の平面上に座標変換した結果に基づき推定された情報である構成（第６の構成）であってもよい。

上記第１～第６いずれかの構成の姿勢推定装置において、前記第１特徴点の位置変化と前記第２特徴点の位置変化との組合せは、前記第１時刻及び前記第２時刻の少なくとも一方に関連する時刻と前記第３時刻及び前記第４時刻の少なくとも一方に関連する時刻との時間差に基づき決定される構成（第７の構成）であってもよい。

上記第１～第７いずれかの構成の姿勢推定装置において、前記特定部は、前記第１時刻から前記第２時刻までの前記移動体の速度と前記第３時刻から前記第４時刻までの前記移動体の速度との違いに基づき、前記第１特徴点の位置変化及び前記第２特徴点の位置変化の少なくとも一方を補正し、少なくとも一方が補正された前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、前記所定の平面との交線が互いに平行となる面の組を２組特定する構成（第８の構成）であってもよい。

本発明に係る異常検出装置は、上記第１～第８いずれかの構成の姿勢推定装置と、前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラの取付けのずれが生じた状態であるか否かを判定する判定部と、を備える構成（第９の構成）である。

本発明に係る補正装置は、上記第１～第８のいずれかの構成の姿勢推定装置と、前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラのパラメータを補正する補正部と、を備える構成（第１０の構成）である。

本発明に係る姿勢推定方法は、移動体に搭載されたカメラで撮影された撮影画像を取得する取得工程と、第１時刻と前記第１時刻より後の第２時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出し、前記第１時刻より後の第３時刻と前記第３時刻より後の第４時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出する抽出工程と、前記第１時刻と前記第２時刻で撮影された２つの前記撮影画像から抽出された特徴点から、互いに対応する第１特徴点を選定し、該第１特徴点の位置変化を算出し、前記第３時刻と前記第４時刻で撮影された２つの前記撮影画像から抽出された特徴点から、前記第１特徴点とは異なり、互いに対応する第２特徴点を選定し、該第２特徴点の位置変化を算出し、前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定工程と、前記特定工程で特定した面の組に基づき前記カメラの姿勢を推定する推定工程と、を備える構成（第１１の構成）である。

本発明によると、カメラの姿勢を精度良く推定でき、推定に要する時間の長期化を抑制できる。

異常検出システムの構成を示すブロック図 車載カメラが車両に配置される位置を例示する図 撮影画像の一例を模式的に示す図 撮影画像の他の例を模式的に示す図 撮影画像の更に他の例を模式的に示す図 姿勢推定装置によるカメラの姿勢推定フローの一例を示すフローチャート 現フレームよりＬフレーム前の撮影画像を模式的に示す図 現フレームを模式的に示す図 現フレームよりＮフレーム前の撮影画像を模式的に示す図 現フレームよりＭフレーム前の撮影画像を模式的に示す図 現フレームよりＬフレーム前の撮影画像を模式的に示す図 現フレームを模式的に示す図 仮想的に形成される四角形を模式的に示す図 所定の平面との交線が互いに平行になる面の組を２組特定する手法を説明するための図 特定した面の組の一方に基づき、面同士の交線の方向を求める手法を説明するための図 特定した面の組の他方に基づき、面同士の交線の方向を求める手法を説明するための図 補正システムの構成を示すブロック図

以下、本発明の例示的な実施形態について、図面を参照しながら詳細に説明する。以下の説明では、移動体として車両を例にとり説明するが、移動体は車両に限定されない。車両には、例えば自動車、電車、無人搬送車等の車輪のついた乗り物が広く含まれる。車両以外の移動体として、例えば船舶や航空機等が挙げられる。

また以下の説明では、車両の直進進行方向であって、運転席からハンドルに向かう方向を「前方向」とする。また、車両の直進進行方向であって、ハンドルから運転席に向かう方向を「後方向」とする。また、車両の直進進行方向及び鉛直線に垂直な方向であって、前方向を向いている運転者の右側から左側に向かう方向を「左方向」とする。また、車両の直進進行方向及び鉛直線に垂直な方向であって、前方向を向いている運転者の左側から右側に向かう方向を「右方向」とする。

＜１．異常検出システム＞
図１は、実施形態に係る異常検出システムＳＹＳ１の構成を示すブロック図である。本実施形態において、異常は、カメラの取付けのずれが生じた状態である。すなわち、異常検出システムＳＹＳ１は、車両に搭載されたカメラ（車載カメラ）の取付けのずれを検出するシステムである。図１に示すように、異常検出システムＳＹＳ１は、異常検出装置１及び撮影部２を備える。

異常検出装置１は、車載カメラで撮影された撮影画像に基づいて異常を検出する装置である。異常検出装置１は、車載カメラを搭載する車両ごとに備えられる。本実施形態では、異常検出装置１は、撮影部２から撮影画像を取得する。

撮影部２は、車両周辺の状況を監視する目的で設けられる。撮影部２は、４つのカメラ２１～２４を備える。４つのカメラ２１～２４は、車載カメラである。図２は、４つの車載カメラ２１～２４が車両５に配置される位置を例示する図である。

車載カメラ２１は車両５の前端に設けられる。このため、車載カメラ２１をフロントカメラ２１とも呼ぶ。フロントカメラ２１の光軸２１ａは上からの平面視で車両５の前後方向に沿っている。フロントカメラ２１は車両５の前方向を撮影する。車載カメラ２２は車両５の後端に設けられる。このため、車載カメラ２２をバックカメラ２２とも呼ぶ。バックカメラ２２の光軸２２ａは上からの平面視で車両５の前後方向に沿っている。バックカメラ２２は車両５の後方向を撮影する。フロントカメラ２１及びバックカメラ２２の取付位置は、車両５の左右中央であることが好ましいが、左右中央から左右方向に多少ずれた位置であってもよい。

車載カメラ２３は車両５の左側ドアミラー５１に設けられる。このため、車載カメラ２３を左サイドカメラ２３とも呼ぶ。左サイドカメラ２３の光軸２３ａは上からの平面視で車両５の左右方向に沿っている。左サイドカメラ２３は車両５の左方向を撮影する。車載カメラ２４は車両５の右側ドアミラー５２に設けられる。このため、車載カメラ２４を右サイドカメラ２４とも呼ぶ。右サイドカメラ２４の光軸２４ａは上からの平面視で車両５の左右方向に沿っている。右サイドカメラ２４は車両５の右方向を撮影する。なお、車両５がいわゆるドアミラーレス車である場合には、左サイドカメラ２３は左サイドドアの回転軸（ヒンジ部）の周辺にドアミラーを介することなく取り付けられ、右サイドカメラ２４は右サイドドアの回転軸（ヒンジ部）の周辺にドアミラーを介することなく取り付けられる。

各車載カメラ２１～２４の水平方向の画角θは１８０度以上である。このため、車載カメラ２１～２４によって、車両５の水平方向における全周囲を撮影することができる。また、車載カメラ２１～２４によって撮影される画像には、車載カメラ２１～２４を搭載する車両５のボディが映り込む。

なお、本実施形態では、車載カメラの数を４つとしているが、この数は適宜変更されてよく、複数であっても単数であってもよい。例えば、車両５がバックで駐車することを支援する目的で車載カメラが搭載されている場合には、車載カメラは、バックカメラ２２、左サイドカメラ２３、右サイドカメラ２４の３つで構成されてもよい。

図１に戻って、本実施形態では、ステアリングホイール（ハンドル）の回転角を検出する舵角センサ３の出力及び車両５の速度を検出する車速度センサ４の出力は、ＣＡＮ（Controller Area Network）バス等の通信バスＢ１を介して異常検出装置１に入力される。

＜２．異常検出装置＞
図１に示すように、異常検出装置１は、取得部１１と、制御部１２と、記憶部１３と、を備える。

取得部１１は、車載カメラ２１～２４からアナログ又はデジタルの撮影画像を所定の周期（例えば、１／３０秒周期）で時間的に連続して取得する。すなわち、取得部１１によって取得される撮影画像の集合体が車載カメラ２１～２４で撮影された動画像である。そして、取得した撮影画像がアナログの場合には、取得部１１は、そのアナログの撮影画像をデジタルの撮影画像に変換（Ａ／Ｄ変換）する。取得部１１は、取得した撮影画像、或いは、取得及び変換した撮影画像を制御部１２に出力する。取得部１１から出力される１つの撮影画像が１つのフレーム画像となる。

制御部１２は、例えばマイクロコンピュータであり、異常検出装置１の全体を統括的に制御する。制御部１２は、不図示のＣＰＵ（Central Processing Unit）、ＲＡＭ（Random Access Memory）、及びＲＯＭ（Read Only Memory）を含む。記憶部１３は、例えば、フラッシュメモリ等の不揮発性のメモリであり、各種の情報を記憶する。記憶部１３は、ファームウェアとしてのプログラムや各種のデータを記憶する。

制御部１２は、抽出部１２１、特定部１２２、推定部１２３、及び判定部１２４を備える。なお、制御部１２の抽出部１２１、特定部１２２、推定部１２３、及び判定部１２４の少なくともいずれか一つは、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field Programmable Gate Array）等のハードウェアで構成されてもよい。また、抽出部１２１、特定部１２２、推定部１２３、及び判定部１２４は、概念的な構成要素である。１つの構成要素が実行する機能を複数の構成要素に分散させたり、複数の構成要素が有する機能を１つの構成要素に統合したりしてよい。また、取得部１１は、制御部１２のＣＰＵがプログラムに従って演算処理を行うことによって実現される構成でもよい。

抽出部１２１は、各車載カメラ２１～２４からの撮影画像から第１特徴点及び第２特徴点を含む複数の特徴点を抽出する。特徴点は、撮影画像中のエッジの交点など、撮影画像において際立って検出できる点である。特徴点は、例えば路面に描かれた白線のエッジ、路面上のひび、路面上のしみ、路面上の砂利などである。特徴点は、通常、１つの撮影画像の中に多数存在する。抽出部１２１は、例えば、ハリスオペレータ等の公知の手法を用いて特徴点を抽出する。抽出部１２１は、路面上の特徴点の中から第１特徴点及び第２特徴点を選定する。第１特徴点と第２特徴点とは互いに異なる特徴点である。なお、本実施形態では、所定の平面として路面を想定する。

特定部１２２は、異なる時刻で撮影された２つの撮影画像から抽出された第１特徴点及び第２特徴点の各オプティカルフローを導出する。オプティカルフローは、異なる時刻で撮影された２つの撮影画像間における特徴点の動きを示す動きベクトルである。異なる時刻の間隔は、取得部１１のフレーム周期と同一であってもよく、取得部１１のフレーム周期の複数倍であってもよい。

特定部１２２は、第１特徴点及び第２特徴点の各オプティカルフローを用いて、異なる時刻で撮影された２つの撮影画像から抽出された第１特徴点及び第２特徴点の各位置変化を算出する。

特定部１２２は、第１特徴点の位置変化及び第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する。

なお、単一の撮影画像から第１特徴点及び第２特徴点のいずれか一方のみしか抽出できない場合がある。また、単一の撮影画像から抽出された第１特徴点及び第２特徴点が適切な位置関係でない場合がある。

単一の撮影画像から第１特徴点及び第２特徴点のいずれか一方のみしか抽出できない場合の例として、図３に示す撮影画像及び図４に示す撮影画像を挙げることができる。

図３は、フロントカメラ２１で撮影される撮影画像を模式的に示している。図３に示す撮影画像は、車両５のボディが映り込む領域ＢＯを含む。図３に示す撮影画像内の斜線部は影を示しており、図３に示す撮影画像内の点線四角内が特徴点の抽出可能領域となるＲＯＩ（region of interest）である。図３に示す撮影画像では、影の影響により路面ＲＳ上に存在する特徴点ＦＰを一つしか抽出できない。

図４は、左サイドカメラ２３で撮影される撮影画像を模式的に示している。図４に示す撮影画像は、車両５のボディが映り込む領域ＢＯを含む。図４に示す撮影画像内の点線四角内が特徴点の抽出可能領域となるＲＯＩである。図４に示す撮影画像では、ＲＯＩ内に白線のコーナーが１つしか存在しないため路面ＲＳ上に存在する特徴点ＦＰを一つしか抽出できない。

単一の撮影画像から抽出された第１特徴点及び第２特徴点が適切な位置関係でない場合の例として、図５に示す撮影画像を挙げることができる。

図５は、フロントカメラ２１で撮影される撮影画像を模式的に示している。図５に示す撮影画像は、車両５のボディが映り込む領域ＢＯを含む。図５に示す撮影画像内の斜線部は影を示しており、図５に示す撮影画像内の点線四角内が特徴点の抽出可能領域となるＲＯＩである。図５に示す撮影画像では、影の影響により路面ＲＳ上に存在する特徴点ＦＰを局所的な場所でしか抽出できない。第１特徴点と第２特徴点が局所的な場所に偏在していると、ＰＡＮ軸、ＴＩＬＴ軸、及びＲＯＬＬ軸の一部でカメラの姿勢推定の精度が劣るおそれがある。

単一の撮影画像から第１特徴点及び第２特徴点のいずれか一方のみしか抽出できない場合、又は、単一の撮影画像から抽出された第１特徴点及び第２特徴点が適切な位置関係でない場合、抽出部１２１は、第１時刻と第１時刻より後の第２時刻で撮影された２つの撮影画像それぞれから特徴点を抽出し、第１時刻より後の第３時刻と第３時刻より後の第４時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出すればよい。そして、特定部１２２は、第１時刻と第２時刻で撮影された２つの撮影画像から抽出された特徴点から、互いに対応する第１特徴点を選定し、第１特徴点の位置変化を算出し、第３時刻と第４時刻で撮影された２つの撮影画像から抽出された特徴点から、第１特徴点とは異なり、互いに対応する第２特徴点を選定し、第２特徴点の位置変化を算出し、第１特徴点の位置変化及び第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定すればよい。

推定部１２３は、特定部１２２で特定した面の組に基づきカメラの姿勢（カメラの取付け角度）を推定する。例えば抽出部１２１がフロントカメラ２１から第１特徴点及び第２特徴点を抽出した場合、推定部１２３はフロントカメラ２１の姿勢（カメラの取付け角度）を推定する。

判定部１２４は、推定部１２３によって推定されたカメラの姿勢に基づき、カメラの取付けのずれが生じた状態であるか否かを判定する。カメラの取付けのずれが生じた状態であると判定されると、異常検出装置１がカメラの異常を検出したことになる。例えば、この異常を異常検出装置１が車両５のユーザに報知することで、車両５のユーザはディーラによるカメラの取付調整を依頼する等の対策をとることが可能となる。

取得部１１、抽出部１２１、特定部１２２、及び推定部１２３によって姿勢推定装置１４が構成される。言い換えると、異常検出装置１は姿勢推定装置１４を備える。図６は、姿勢推定装置１４によるカメラの姿勢推定フローの一例を示すフローチャートである。以下、フロントカメラ２１の姿勢を推定する場合を例に挙げて説明する。

図６に示すように、抽出部１２１は特徴点を抽出する（ステップＳ１）。より詳細には、ステップＳ１において、抽出部１２１は、最新の撮影画像である現フレームの特徴点と現フレームよりＬ（Ｌは自然数）フレーム前の撮影画像の特徴点を抽出する。

次に、特定部１２２は、オプティカルフローを導出する（ステップＳ２）。より詳細には、ステップＳ２において、抽出部１２１は、最新の撮影画像（現フレーム）と現フレームよりＬ（Ｌは自然数）フレーム前の撮影画像から抽出された特徴点のオプティカルフローを導出する。

なお、ステップＳ２においてオプティカルフローが１つも導出されない場合は、現フレームより１フレーム後のフレームを新たな現フレームとし、その新たな現フレームに関して、ステップＳ１及びＳ２の処理を実行する。

記憶部１３は、ステップＳ２で導出されたオプティカルフローを最新のオプティカルフローとして記憶する。最新のオプティカルフローは、現フレームの更新により過去のオプティカルフローに変化する。記憶部１３は、最新のオプティカルフローが過去のオプティカルフローに変化した時点から一定時間が経過する迄の間、過去のオプティカルフローを記憶する。

ステップＳ２におけるオプティカルフローの導出が終了すると、特定部１２２は、特徴点に対し、記憶部１３に記憶されているフロントカメラ２１に内部パラメータを用いて座標変換を行う。座標変換では、フロントカメラ２１の収差補正と、歪補正とが行われる。収差補正は、フロントカメラ２１の光学系の収差による歪みを補正するために行われる。具体的には樽型歪みや糸巻歪みなど歪曲収差の補正である。歪み補正は、フロントカメラ２１の光学系そのものの歪みを補正するために行われる。具体的には魚眼補正などである。座標変換により、特徴点の座標は、被写体が透視投影によって撮影された場合に二次元画像上で得られる座標に変換される。

上述した特徴点の座標変換が終了すると、特定部１２２は、最新のオプティカルフローのみで四角形を仮想的に形成できるか否かを判定する（ステップＳ３）。

最新のオプティカルフローのみで四角形を仮想的に形成できると判定した場合（ステップＳ３のＹＥＳ）、特定部１２２は、最新のオプティカルフローのみで四角形を仮想的に形成する（ステップＳ４）。以降、説明のために四角形や、該四角形の辺などの幾何的要素を仮想的に形成して用いる。しかし、実際の処理では、特徴点の座標や直線の方向などのベクトル演算に基づき、同等の作用を持つ幾何的要素に基づかない処理としてもよい。

ここで、ステップＳ１、Ｓ２、Ｓ３、及びＳ４の順で処理が実行される場合の具体例について図７Ａ、図７Ｂ、及び図８を参照して説明する。図７Ａは現フレームよりＬ（Ｌは自然数）フレーム前の撮影画像を模式的に示す図である。図７Ｂは現フレームを模式的に示す図である。図８は仮想的に形成される四角形を模式的に示す図である。

図７Ａに示す撮影画像Ｐ１及び図７Ｂに示す撮影画像Ｐ２はフロントカメラ２１で撮影される。撮影画像Ｐ１及びＰ２は、車両５のボディが映り込む領域ＢＯを含む。第１特徴点ＦＰ１及び第２特徴点ＦＰ２は路面ＲＳ上に存在する。図７Ａ及び図７Ｂにおいては、路面に描かれている白線のコーナーに第１特徴点ＦＰ１及び第２特徴点ＦＰ２が存在する。

撮影画像Ｐ１の撮影時点から撮影画像Ｐ２の撮影時点迄の間に、車両５は前方に直進している。図７Ｂに示す丸印ＦＰ１Ｐは、撮影画像Ｐ１の撮影時点における第１特徴点ＦＰ１の位置を示す。図７Ｂに示す丸印ＦＰ２Ｐは、撮影画像Ｐ１の撮影時点における第２特徴点ＦＰ２の位置を示す。

図７Ｂに示すように、車両５が前方に直進すると、車両５の前方に存在する第１特徴点ＦＰ１及び第２特徴点ＦＰ２は車両５に近づく。すなわち、第１特徴点ＦＰ１及び第２特徴点ＦＰ２は、撮影画像Ｐ２と撮影画像Ｐ１とで異なる位置に現れる。特定部１２２は、撮影画像Ｐ２の第１特徴点ＦＰ１と撮影画像Ｐ１の第１特徴点ＦＰ１とを、その近傍の画素値に基づいて対応付け、対応付けた第１特徴点ＦＰ１のそれぞれの位置に基づいて、第１特徴点ＦＰ１のオプティカルフローＯＦ１を導出する。同様に、特定部１２２は、撮影画像Ｐ２の第２特徴点ＦＰ２と撮影画像Ｐ１の第２特徴点ＦＰ２とを、その近傍の画素値に基づいて対応付け、対応付けた第２特徴点ＦＰ２のそれぞれの位置に基づいて、第２特徴点ＦＰ２のオプティカルフローＯＦ２を導出する。

第１特徴点ＦＰ１のオプティカルフローＯＦ１と第２特徴点ＦＰ２のオプティカルフローＯＦ２が導出されると、特定部１２２は、特徴点に対し、記憶部１３に記憶されているフロントカメラ２１に内部パラメータを用いて座標変換を行う。座標変換では、フロントカメラ２１の収差補正と、歪補正とが行われる。収差補正は、フロントカメラ２１の光学系の収差による歪みを補正するために行われる。具体的には樽型歪みや糸巻歪みなど歪曲収差の補正である。歪み補正は、フロントカメラ２１の光学系そのものの歪みを補正するために行われる。具体的には魚眼補正などである。座標変換により、特徴点の座標は、被写体が透視投影によって撮影された場合に二次元画像上で得られる座標に変換される。

特徴点の座標変換が行われると、特定部１２２は、図８に示すように、第１特徴点ＦＰ１のオプティカルフローＯＦ１の始点を頂点ＳＰ１、第１特徴点ＦＰ１のオプティカルフローＯＦ１の終点を頂点ＥＰ１、第２特徴点ＦＰ１のオプティカルフローＯＦ２の始点を頂点ＳＰ２、及び第２特徴点ＦＰ２のオプティカルフローＯＦ２の終点を頂点ＥＰとする四角形ＱＬを仮想的に形成する。

図６に戻ってフローチャートのステップＳ５に関して説明する。最新のオプティカルフローのみで四角形を仮想的に形成できないと判定した場合（ステップＳ３のＮＯ）、特定部１２２は、過去のオプティカルフローと最新のオプティカルフローを用いて四角形を仮想的に形成する（ステップＳ５）。

ここで、ステップＳ１、Ｓ２、Ｓ３、及びＳ５の順で処理が実行される場合の具体例について図７Ｃ～図７Ｆ及び図８を参照して説明する。図７Ｃは現フレームよりＮ（Ｎは自然数）フレーム前の撮影画像を模式的に示す図である。図７Ｄは現フレームよりＭ（Ｍは自然数）フレーム前の撮影画像を模式的に示す図である。図７Ｅは現フレームよりＬ（Ｌは自然数）フレーム前の撮影画像を模式的に示す図である。図７Ｆは現フレームを模式的に示す図である。本実施形態では、上記Ｎ、Ｍ、及びＬについてＮ＞Ｍ＞Ｌであるが、ＭとＬは同一であってもよく、ＬがＭより大きく且つＮより小さくてもよい。

図７Ｃに示す撮影画像Ｐ１１は第１時刻で撮影された撮影画像の一例である。図７Ｄに示す撮影画像Ｐ１２は第１時刻より後の第２時刻で撮影された撮影画像の一例である。図７Ｅに示す撮影画像Ｐ１３は第２時刻以後の第３時刻で撮影された撮影画像の一例である。図７Ｆに示す撮影画像Ｐ１４は第３時刻より後の第４時刻で撮影された撮影画像の一例である。

図７Ｃ～図７Ｆに示す撮影画像Ｐ１１～Ｐ１４はフロントカメラ２１で撮影される。撮影画像Ｐ１１～Ｐ１４は、車両５のボディが映り込む領域ＢＯを含む。第１特徴点ＦＰ１及び第２特徴点ＦＰ２は路面ＲＳ上に存在する。図７Ｃ及び図７Ｄにおいては、路面に描かれている白線のコーナーに第１特徴点ＦＰ１が存在する。図７Ｅ及び図７Ｆにおいては、路面に描かれている白線のコーナーに第２特徴点ＦＰ２が存在する。

撮影画像Ｐ１１の撮影時点から撮影画像Ｐ１２の撮影時点迄の間に、車両５は前方に直進している。図７Ｄに示す丸印ＦＰ１Ｐは、撮影画像Ｐ１１の撮影時点における第１特徴点ＦＰ１の位置を示す。同様に、撮影画像Ｐ１３の撮影時点から撮影画像Ｐ１４の撮影時点迄の間に、車両５は前方に直進している。図７Ｆに示す丸印ＦＰ２Ｐは、撮影画像Ｐ１３の撮影時点における第２特徴点ＦＰ２の位置を示す。

図７Ｄに示すように、車両５が前方に直進すると、車両５の前方に存在する第１特徴点ＦＰ１は車両５に近づく。すなわち、第１特徴点ＦＰ１は、撮影画像Ｐ１２と撮影画像Ｐ１１とで異なる位置に現れる。特定部１２２は、撮影画像Ｐ１２の第１特徴点ＦＰ１と撮影画像Ｐ１１の第１特徴点ＦＰ１とを、その近傍の画素値に基づいて対応付け、対応付けた第１特徴点ＦＰ１のそれぞれの位置に基づいて、第１特徴点ＦＰ１のオプティカルフローＯＦ１を導出する。

図７Ｆに示すように、車両５が前方に直進すると、車両５の前方に存在する第２特徴点ＦＰ２は車両５に近づく。すなわち、第２特徴点ＦＰ２は、撮影画像Ｐ１４と撮影画像Ｐ１３とで異なる位置に現れる。特定部１２２は、撮影画像Ｐ１４の第２特徴点ＦＰ２と撮影画像Ｐ１３の第２特徴点ＦＰ２とを、その近傍の画素値に基づいて対応付け、対応付けた第２特徴点ＦＰ２のそれぞれの位置に基づいて、第２特徴点ＦＰ２のオプティカルフローＯＦ２を導出する。

第１特徴点ＦＰ１のオプティカルフローＯＦ１と第２特徴点ＦＰ２のオプティカルフローＯＦ２が導出されると、特定部１２２は、特徴点に対し、記憶部１３に記憶されているフロントカメラ２１に内部パラメータを用いて座標変換を行う。座標変換では、フロントカメラ２１の収差補正と、歪補正とが行われる。収差補正は、フロントカメラ２１の光学系の収差による歪みを補正するために行われる。具体的には樽型歪みや糸巻歪みなど歪曲収差の補正である。歪み補正は、フロントカメラ２１の光学系そのものの歪みを補正するために行われる。具体的には魚眼補正などである。座標変換により、特徴点の座標は、被写体が透視投影によって撮影された場合に二次元画像上で得られる座標に変換される。

特徴点の座標変換が行われると、特定部１２２は、図８に示すように、第１特徴点ＦＰ１のオプティカルフローＯＦ１の始点を頂点ＳＰ１、第１特徴点ＦＰ１のオプティカルフローＯＦ１の終点を頂点ＥＰ１、第２特徴点ＦＰ１のオプティカルフローＯＦ２の始点を頂点ＳＰ２、及び第２特徴点ＦＰ２のオプティカルフローＯＦ２の終点を頂点ＥＰとする四角形ＱＬを仮想的に形成する。

図６に戻ってフローチャートのステップＳ５より後の処理に関して説明する。

四角形ＱＬが仮想的に形成されると、特定部１２２は、四角形ＱＬと、記憶部１３に記憶されているフロントカメラ２１の内部パラメータとを用いて、三次元空間におけるフロントカメラ２１の投影面ＩＭＧ上に四角形ＱＬを移動させ、投影面ＩＭＧ上での四角形ＱＬ１を仮想的に生成する（ステップＳ６）。

なお説明のため、以下のように四角形ＱＬ１の辺を定義する（図９参照）。四角形ＱＬ１の第１辺ＳＤ１は、四角形ＱＬにおいて、頂点ＳＰ１および頂点ＳＰ２を結んだ辺に対応する。つまり、第１特徴点ＦＰ１のオプティカルフローＯＦ１の始点と第２特徴点ＦＰ２のオプティカルフローＯＦ２の始点とを結んだ辺に相当する。同様に、四角形ＱＬ１の第２辺ＳＤ２は、四角形ＱＬにおいて、頂点ＳＰ２および頂点ＥＰ２を結んだ辺に対応する。つまり、第２特徴点ＦＰ２のオプティカルフローＯＦ２に相当する。同様に、四角形ＱＬ１の第３辺ＳＤ３は、四角形ＱＬにおいて、頂点ＥＰ１および頂点ＥＰ２を結んだ辺に対応する。つまり、第１特徴点ＦＰ１のオプティカルフローＯＦ１の終点と第２特徴点ＦＰ２のオプティカルフローＯＦ２の終点とを結んだ辺に相当する。同様に、四角形ＱＬ１の第４辺ＳＤ４は、四角形ＱＬにおいて、頂点ＳＰ１および頂点ＥＰ１を結んだ辺に対応する。つまり、第１特徴点ＦＰ１のオプティカルフローＯＦ１に相当する。

また、以下のように面を定義する（図９参照）。四角形ＱＬ１の第１辺ＳＤ１とフロントカメラ２１の光学中心ＯＣとが含まれる面を第１面Ｆ１とする。同様に、四角形ＱＬ１の第２辺ＳＤ２とフロントカメラ２１の光学中心ＯＣとが含まれる面を第２面Ｆ２とする。同様に、四角形ＱＬ１の第３辺ＳＤ３とフロントカメラ２１の光学中心ＯＣとが含まれる面を第３面Ｆ３とする。同様に、四角形ＱＬ１の第４辺ＳＤ４とフロントカメラ２１の光学中心ＯＣとが含まれる面を第４面Ｆ４とする。

ステップＳ６に続くステップＳ７において、特定部１２２は、所定の平面との交線が互いに平行になる面の組を２組特定する。所定の平面とはあらかじめ平面の法線が分かっている面である。具体的には車両５が移動を行っている平面であり、つまり路面である。所定の平面は、厳密な平面でなくてもよく、特定部１２２が所定の平面との交線が互いに平行になる面の組を２組特定する際に平面とみなすことができるものであればよい。

本実施形態では、第１特徴点及び第２特徴点は路面などの所定の平面上に位置している静止物から抽出される。したがって、特定部１２２によって導出される第１特徴点及び第２特徴点の各オプティカルフローは実世界上では、車両５に対する静止物の相対的な位置変化を表す。つまり向きが逆となった車両５の移動ベクトルである。

四角形ＱＬ１の第２辺ＳＤ２と第４辺ＳＤ４とは、共に特徴点のオプティカルフローに対応するので、共に実世界上では車両５の移動ベクトルに相当する。したがって、路面上では互いに平行となると想定される。

また、四角形ＱＬ１の第１辺ＳＤ１と第３辺ＳＤ３とは、共に特徴点同士の位置関係なので、実世界上では車両５の移動に伴う静止物同士の位置関係に相当する。移動前の位置関係が第１辺ＳＤ１に相当し、移動後の位置関係が第３辺ＳＤ３に相当する。このとき静止物の位置は変わらないため、移動前後で位置関係は変わらない。したがって、路面上ではこれも互いに平行となると想定される。

したがって、特定部１２２は、路面との交線が平行な面として、第２面Ｆ２と第４面Ｆ４との組と、第１面Ｆ１と第３面Ｆ３との組と、の２つの組を特定する。つまり、特定部１２２は、オプティカルフローを含む面同士を１つの組とし、同時刻に撮影された特徴点を含む面同士を他の組として、計２つの組を特定する。

なお、図９において四角形ＱＬ２は、図７Ａに示す撮影画像Ｐ１及び図７Ｂに示す撮影画像Ｐ２で得られるオプティカルフローを用いて仮想的に形成した場合、撮影画像Ｐ２の撮影時点での第１特徴点ＦＰ１の３次元空間（実世界）上の位置、撮影画像Ｐ２の撮影時点での第２特徴点ＦＰ２の３次元空間上の位置、撮影画像Ｐ１の撮影時点での第１特徴点ＦＰ１の３次元空間上の位置、及び撮影画像Ｐ１の撮影時点での第２特徴点ＦＰ２の３次元空間上の位置を頂点とする四角形である。

一方、図９において四角形ＱＬ２は、図７Ｃに示す撮影画像Ｐ１１、図７Ｄに示す撮影画像Ｐ１２、図７Ｅに示す撮影画像Ｐ１３、及び図７Ｆに示す撮影画像Ｐ１４で得られるオプティカルフローを用いて仮想的に形成した場合、撮影画像Ｐ１２の撮影時点での第１特徴点ＦＰ１の３次元空間（実世界）上の位置、撮影画像Ｐ１４の撮影時点での第２特徴点ＦＰ２の３次元空間上の位置、撮影画像Ｐ１１の撮影時点での第１特徴点ＦＰ１の３次元空間上の位置、及び撮影画像Ｐ１３の撮影時点での第２特徴点ＦＰ２の３次元空間上の位置を頂点とする四角形である。

第１面Ｆ１は、四角形ＱＬ２の第１辺ＳＤ１１を含む。同様に、第２面Ｆ２は四角形ＱＬ２の第２辺ＳＤ１２を含み、第３面Ｆ３は四角形ＱＬ２の第３辺ＳＤ１３を含み、第４面Ｆ４は四角形ＱＬ２の第４辺ＳＤ１４を含む。このとき、上記のように四角形ＱＬ２は路面上に形成される平行四辺形であると想定される。

次に、推定部１２３は、路面の法線を算出する（ステップＳ８）。まず、推定部１２３は、特定部１２２で特定した面の組の一方である第１面Ｆ１と第３面Ｆ３とに基づき、面同士の交線の方向を求める。詳細には、第１面Ｆ１と第３面Ｆ３との交線ＣＬ１の向きを求める（図１０参照）。交線ＣＬ１の方向ベクトルＶ１は、第１面Ｆ１の法線ベクトル及び第３面Ｆ３の法線ベクトルそれぞれと垂直なベクトルである。したがって、推定部１２３は、第１面Ｆ１の法線ベクトルと第３面Ｆ３の法線ベクトルとの外積により、交線ＣＬ１の方向ベクトルＶ１を求める。第１面Ｆ１と第３面Ｆ３は、路面との交線が平行となるため、方向ベクトルＶ１は路面と平行になる。

同様に、推定部１２３は、特定部１２２で特定した面の組の他方である第２面Ｆ２と第４面Ｆ４との交線の方向を求める。詳細には第２面Ｆ２と第４面Ｆ４との交線ＣＬ２の向きを求める（図１１参照）。交線ＣＬ２の方向ベクトルＶ２は、第２面Ｆ２の法線ベクトル及び第４面Ｆ４の法線ベクトルそれぞれと垂直なベクトルである。したがって、推定部１２３は、第２面Ｆ２の法線ベクトルと第４面Ｆ４の法線ベクトルとの外積により、交線ＣＬ２の方向ベクトルＶ２を求める。第２面Ｆ２と第４面Ｆ４も同様に、路面との交線が平行となるため、方向ベクトルＶ２は路面と平行になる。

推定部１２３は、方向ベクトルＶ１と方向ベクトルＶ２との外積により、四角形ＱＬ２の面の法線、すなわち路面の法線を算出する。推定部１２３が算出した路面の法線はフロントカメラ２１のカメラ座標系で算出されるため、実際の路面の法線である垂直方向との違いから３次元空間の座標系を求め、路面に対するフロントカメラ２１の姿勢を推定することができる。その推定結果から推定部１２３は車両５に対するフロントカメラ２１の姿勢を推定する（ステップＳ９）。なお、ステップＳ７の算出処理は、例えば公知のARToolkitを利用して実行することができる。

ステップＳ９におけるフロントカメラ２１の姿勢推定が終了すると、図６に示すフローが終了する。

姿勢推定装置１４は、車両５の移動を利用して、自律的に所定の平面との交線が互いに平行になる面の組を２組特定することで、特徴点の抽出誤差のみが推定精度に影響するカメラの姿勢推定を行うことができる。すなわち、姿勢推定装置１４は、誤差要因が少ないカメラの姿勢推定を行うことができる。したがって、姿勢推定装置１４は、カメラの姿勢を精度良く推定することができる。

また、姿勢推定装置１４は、上述した通り、最新のオプティカルフローのみで四角形を仮想的に形成できない場合に、過去のオプティカルフローと最新のオプティカルフローを用いて四角形を仮想的に形成する。したがって、姿勢推定装置１４は、最新のオプティカルフローのみで四角形を仮想的に形成することができない構成、すなわち、図６に示すフローチャートからステップＳ３及びＳ５を取り除いた動作を行う構成に比べて、推定に要する時間の長期化を抑制できる。

しかしながら、過去のオプティカルフローと最新のオプティカルフローを用いて四角形を仮想的に形成する場合、適切なオプティカルフローを用いなければ、最新のオプティカルフローのみで四角形を仮想的に形成する場合に比べて、カメラの姿勢推定の精度が大幅に低下するおそれがある。

図６に示すステップＳ５の処理で用いるオプティカルフローが適切であるかは車両５の移動具合に依存する。そこで、図６に示すステップＳ５の処理を実行する場合、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せは、車両５の移動情報に基づき決定されることが望ましい。

上述した通り、姿勢推定装置１４は、四角形ＱＬ２が路面上に形成される平行四辺形であるとの想定の下、カメラの姿勢推定を行っている。

第１特徴点のオプティカルフローＯＰ１が発生するときの車両５の舵角と第２特徴点のオプティカルフローＯＰ２が発生するときの車両５の舵角との違いが少ないほど、上記の想定と現実との乖離が小さくなる。したがって、上述した車両５の移動情報に車両５の舵角情報を含め、図６に示すステップＳ５の処理を実行する場合、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せは、車両５の舵角情報に基づき決定されることが望ましい。以下、この組合せ決定手法を第１決定手法という。例えば、舵角センサ３から通信バスＢ１を経由して姿勢推定装置１４に供給される舵角センサ３の出力を車両５の舵角情報として用いると、車両５の舵角情報が舵角の推定情報ではなく舵角の実測情報になるので、車両５の舵角情報の精度を高めることができる。

例えば、図７Ｄに示す撮影画像Ｐ１２の撮影時点における車両５の舵角と図７Ｆに示す撮影画像Ｐ１４の撮影時点における車両５の舵角との差が所定角度以内になるように、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せを決定すればよい。また、例えば、図７Ｃに示す撮影画像Ｐ１１の撮影時点から図７Ｄに示す撮影画像Ｐ１２の撮影時点までの車両５の舵角の平均値と図７Ｅに示す撮影画像Ｐ１３の撮影時点から図７Ｆに示す撮影画像Ｐ１４の撮影時点までの車両５の舵角の平均値との差が所定角度以内になるように、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せを決定してもよい。

第１特徴点のオプティカルフローＯＰ１が発生するときの車両５の速度と第２特徴点のオプティカルフローＯＰ２が発生するときの車両５の速度との違いが少ないほど、上記の想定と現実との乖離が小さくなる。したがって、上述した車両５の移動情報に車両５の速度情報を含め、図６に示すステップＳ５の処理を実行する場合、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せは、車両５の速度情報に基づき決定されることが望ましい。以下、この組合せ決定手法を第２決定手法という。例えば、車速度センサ４から通信バスＢ１を経由して姿勢推定装置１４に供給される車速度センサ４の出力を車両５の速度情報として用いると、車両５の速度情報が速度の推定情報ではなく速度の実測情報になるので、車両５の速度情報の精度を高めることができる。

例えば、図７Ｄに示す撮影画像Ｐ１２の撮影時点における車両５の速度と図７Ｆに示す撮影画像Ｐ１４の撮影時点における車両５の速度との差が所定速度以内になるように、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せを決定すればよい。また、例えば、図７Ｃに示す撮影画像Ｐ１１の撮影時点から図７Ｄに示す撮影画像Ｐ１２の撮影時点までの車両５の速度の平均値と図７Ｅに示す撮影画像Ｐ１３の撮影時点から図７Ｆに示す撮影画像Ｐ１４の撮影時点までの車両５の速度の平均値との差が所定速度以内になるように、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せを決定してもよい。

なお、第１特徴点のオプティカルフローＯＰ１が発生するときと第２特徴点のオプティカルフローＯＰ１が発生するときとの時間差が短ければ、車両５の移動具合の変化も小さいことが期待できる。したがって、図６に示すステップＳ５の処理を実行する場合、第１特徴点のオプティカルフローＯＰ１と第２特徴点のオプティカルフローＯＰ２との組合せは、図７Ｃに示す撮影画像Ｐ１１の撮影時点と図７Ｄに示す撮影画像Ｐ１２の撮影時点の少なくとも一方に関連する時刻と図７Ｅに示す撮影画像Ｐ１３の撮影時点と図７Ｆに示す撮影画像Ｐ１４の撮影時点の少なくとも一方に関連する時刻との時間差に基づき決定されることが望ましい。以下、この組合せ決定手法を第３決定手法という。

図７Ｃに示す撮影画像Ｐ１１の撮影時点と図７Ｄに示す撮影画像Ｐ１２の撮影時点の少なくとも一方に関連する時刻としては、例えば、図７Ｃに示す撮影画像Ｐ１１の撮影時点の時刻、図７Ｄに示す撮影画像Ｐ１２の撮影時点の時刻、図７Ｃに示す撮影画像Ｐ１１の撮影時点と図７Ｄに示す撮影画像Ｐ１２の撮影時点との中点の時刻等を挙げることができる。同様に、図７Ｅに示す撮影画像Ｐ１３の撮影時点と図７Ｆに示す撮影画像Ｐ１４の撮影時点の少なくとも一方に関連する時刻としては、例えば、図７Ｅに示す撮影画像Ｐ１３の撮影時点の時刻、図７Ｆに示す撮影画像Ｐ１４の撮影時点の時刻、図７Ｅに示す撮影画像Ｐ１３の撮影時点と図７Ｆに示す撮影画像Ｐ１４の撮影時点との中点の時刻等を挙げることができる。

上述した第１決定手法、第２決定手法、第３決定手法はそれぞれ単独で実施してもよく、任意の２つ以上を組み合わせて実施してもよい。

また、第１特徴点及び第２特徴点の各オプティカルフローは、向きが逆となった車両５の移動ベクトルであるので、上述した移動情報を、第１特徴点及び第２特徴点の各オプティカルフローを路面上に座標変換した結果に基づき推定された情報としてもよい。第１特徴点及び第２特徴点の各オプティカルフローを路面上に座標変換し、路面上での長さを算出することで車両５の速度を推定でき、路面上での傾きを算出することで車両５の舵角を推定できる。つまり、上述した第１決定手法、第２決定手法の代わりとして実施することができる。上述した移動情報を、第１特徴点及び第２特徴点の各オプティカルフローを路面上に座標変換した結果に基づき推定された情報とする場合、姿勢推定装置１４と通信バスＢ１との接続が不要になる。

また、特定部１２２は、第１特徴点のオプティカルフローＯＰ１が発生するときの車両５の速度（車速度センサ４で検出される実測値であってもよく上述した推定速度であってもよい）と第２特徴点のオプティカルフローＯＰ２が発生するときの車両５の速度（車速度センサ４で検出される実測値であってもよく上述した推定速度であってもよい）との違いに基づき、第１特徴点のオプティカルフローＯＰ１及び第２特徴点のオプティカルフローＯＰ２の少なくとも一方を補正し、少なくとも一方が補正された第１特徴点及び第２特徴点の各オプティカルフローに基づき、路面との交線が互いに平行となる面の組を２組特定することが望ましい。これにより、カメラの姿勢推定の精度を向上させることができる。

例えば、第１特徴点のオプティカルフローＯＰ１が発生するときの車両５の速度が第２特徴点のオプティカルフローＯＰ２が発生するときの車両５の速度より大きい場合、第１特徴点のオプティカルフローＯＰ１が短くなるように補正してよく、第２特徴点のオプティカルフローＯＰ２が長くなるように補正してよく、第１特徴点のオプティカルフローＯＰ１が短くなり且つ第２特徴点のオプティカルフローＯＰ２が長くなるように補正してよい。

なお、例えば路面ＲＳがコンクリートで構成される場合、特徴点が抽出され難い。このため、特徴点の抽出誤差が大きくなり易い。しかしながら、例えば、コンクリートで構成される路面ＲＳに汚れが存在する場合、その汚れ部分から第１特徴点ＦＰ１及び第２特徴点ＦＰ２を抽出すると、特徴度が高い特徴点を第１特徴点ＦＰ１及び第２特徴点ＦＰ２として選定することになり、第１特徴点ＦＰ１及び第２特徴点ＦＰ２の抽出誤差を抑制できると考えられる。

汚れに限らず、特徴点が他に比べて集中して存在する高密度領域が存在する場合、その高密度領域の中から、第１特徴点ＦＰ１及び第２特徴点ＦＰ２を抽出することにより、第１特徴点ＦＰ１及び第２特徴点ＦＰ２の抽出誤差を抑制できると考えられる。したがって、特徴点が他に比べて集中して存在する高密度領域が存在する場合、その高密度領域の中から、第１特徴点ＦＰ１及び第２特徴点ＦＰ２が抽出部１２１によって抽出されてもよい。

また、コーナー度が高い特徴点の抽出では、抽出誤差が小さくなる。したがって、記第１特徴点及び前記第２特徴点を、コーナーらしさを示すコーナー度が所定のコーナー度閾値以上となる特定の特徴点としてもよい。コーナーとは２つのエッジが交わる部分である。コーナーでは、コーナー度が高くなる。コーナー度は、例えばＨａｒｒｉｓオペレータやＫＬＴ（Kanade-Lucas-Tomasi）トラッカーなどの公知の検出手法を用いて求めることができる。

なお、上記の説明ではフロントカメラ２１の光学中心ＯＣと、四角形ＱＬ１の１つの辺を含む平面を特定するとしたがこの限りではない。当該平面の法線方向の特定をもって平面を特定するとしてもよい。例えば光学中心ＯＣから各頂点への方向ベクトルの外積により平面の法線方向を求め、該法線方向によって面を特定するとしてもよい。この場合、第１面Ｆ１の法線方向と第３面Ｆ３の法線方向とを１つの組とし、第２面Ｆ２の法線方向と第４面Ｆ４の法線方向とを他の組として２つの組を特定するとよい。

また、面は平行移動させてもよい。例えば第１面Ｆ１の代わりに、第１面Ｆ１を平行移動させた面を第３面Ｆ３と組としてもよい。平行移動しても所定の平面との交線の向きは変わらないからである。

＜３．補正システム＞
図１２は、実施形態に係る補正システムＳＹＳ２の構成を示すブロック図である。図１２において、図１と同一の部分には同一の符号を付し詳細な説明を省略する。図１２に示すように、補正システムＳＹＳ２は、補正装置１’及び撮影部２を備える。

補正装置１’は、図１に示す異常検出装置１から判定部１２４を取り除き補正部１２５を追加した構成と同一である。すなわち、補正装置１’は、姿勢推定装置１４及び補正部１２５を備える。

補正部１２５は、推定部１２３によって推定されたカメラの姿勢に基づき、カメラのパラメータを補正する。これにより、カメラの取付けのずれが生じても、そのずれに応じたカメラのキャリブレーションが実行されることになり、カメラの取付けのずれが撮影画像に及ぼす悪影響を抑制することができる。

＜４．留意事項＞
本明細書における実施形態や実施例の構成は、本発明の例示にすぎない。実施形態や変形例の構成は、本発明の技術的思想を超えない範囲で適宜変更されてもよい。また、複数の実施形態及び変形例は、可能な範囲で組み合わせて実施されてよい。

本発明は、上述した実施形態のように、自動駐車等の移動体の運転支援を行うカメラの姿勢推定を行うために利用することができる。また、本発明は、ドライブレコーダ等の運転情報を記録するカメラの姿勢推定を行うために利用することができる。また、本発明は、上述した実施形態のように、カメラの姿勢の推定情報を利用して撮影画像を補正する補正装置等に利用することができる。また、本発明は、複数の移動体とネットワークにより通信可能に設けられるセンターと連携して動作する装置等に利用することができる。当該装置は、例えば、センターに撮影画像を送信する場合に、カメラの異常情報や姿勢の推定情報を撮影画像とセットにして送信する構成であってよい。そしてセンターでは、カメラの姿勢の推定情報を用いて、各種画像処理（カメラの姿勢も考慮した画像の視点・視方向を変更する処理、例えば車両の車体前方方向の画像に視点・視方向変換した画像を生成する等）、画像を用いた計測処理におけるカメラの姿勢に対する補正処理、カメラ姿勢の経年変化を統計処理（多くの車両のデータ）、等を行いユーザへの有用な提供データを生成する等する。

１ 異常検出装置
１’ 補正装置
１４ 姿勢推定装置
２１～２４ 車載カメラ
１１ 取得部
１２１ 抽出部
１２２ 特定部
１２３ 推定部
１２４ 判定部
１２５ 補正部

Claims (11)

1. 移動体に搭載されたカメラで撮影された撮影画像を取得する取得部と、
   第１時刻と前記第１時刻より後の第２時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出し、前記第１時刻より後の第３時刻と前記第３時刻より後の第４時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出する抽出部と、
   前記第１時刻と前記第２時刻で撮影された２つの前記撮影画像から抽出された特徴点から、互いに対応する第１特徴点を選定し、該第１特徴点の位置変化を算出し、
   前記第３時刻と前記第４時刻で撮影された２つの前記撮影画像から抽出された特徴点から、前記第１特徴点とは異なり、互いに対応する第２特徴点を選定し、該第２特徴点の位置変化を算出し、
   前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定部と、
   前記特定部で特定した面の組に基づき前記カメラの姿勢を推定する推定部と、
   を備える、姿勢推定装置。
2. 前記第１特徴点の位置変化と前記第２特徴点の位置変化との組合せは、前記移動体の移動情報に基づき決定される、請求項１に記載の姿勢推定装置。
3. 前記移動情報は、前記移動体の舵角情報を含む、請求項２に記載の姿勢推定装置。
4. 前記移動情報は、前記移動体の速度情報を含む、請求項２又は請求項３に記載の姿勢推定装置。
5. 前記移動情報は、通信バスを経由して、前記姿勢推定装置に供給される、請求項２～４のいずれか一項に記載の姿勢推定装置。
6. 前記移動情報は、前記第１特徴点の位置変化を前記所定の平面上に座標変換した結果、及び、前記第２特徴点の位置変化を前記所定の平面上に座標変換した結果に基づき推定された情報である、請求項２～４に記載の姿勢推定装置。
7. 前記第１特徴点の位置変化と前記第２特徴点の位置変化との組合せは、前記第１時刻及び前記第２時刻の少なくとも一方に関連する時刻と前記第３時刻及び前記第４時刻の少なくとも一方に関連する時刻との時間差に基づき決定される、請求項１～６のいずれか一項に記載の姿勢推定装置。
8. 前記特定部は、前記第１時刻から前記第２時刻までの前記移動体の速度と前記第３時刻から前記第４時刻までの前記移動体の速度との違いに基づき、前記第１特徴点の位置変化及び前記第２特徴点の位置変化の少なくとも一方を補正し、少なくとも一方が補正された前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、前記所定の平面との交線が互いに平行となる面の組を２組特定する、請求項１～７のいずれか一項に記載の姿勢推定装置。
9. 請求項１～８のいずれか一項に記載の姿勢推定装置と、
   前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラの取付けのずれが生じた状態であるか否かを判定する判定部と、
   を備える、異常検出装置。
10. 請求項１～８のいずれか一項に記載の姿勢推定装置と、
    前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラのパラメータを
    補正する補正部と、
    を備える、補正装置。
11. 移動体に搭載されたカメラで撮影された撮影画像を取得する取得工程と、
    第１時刻と前記第１時刻より後の第２時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出し、前記第１時刻より後の第３時刻と前記第３時刻より後の第４時刻で撮影された２つの前記撮影画像それぞれから特徴点を抽出する抽出工程と、
    前記第１時刻と前記第２時刻で撮影された２つの前記撮影画像から抽出された特徴点から、互いに対応する第１特徴点を選定し、該第１特徴点の位置変化を算出し、前記第３時刻と前記第４時刻で撮影された２つの前記撮影画像から抽出された特徴点から、前記第１特徴点とは異なり、互いに対応する第２特徴点を選定し、該第２特徴点の位置変化を算出し、前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定工程と、
    前記特定工程で特定した面の組に基づき前記カメラの姿勢を推定する推定工程と、
    を備える、姿勢推定方法。

Images