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

Abstract

【課題】カメラの姿勢を精度良く推定する。【解決手段】姿勢推定装置は、取得部と、抽出部と、特定部と、推定部と、を備える。前記取得部は、移動体に搭載されたカメラで撮影された撮影画像を取得する。前記抽出部は、前記撮影画像から第１特徴点及び第２特徴点を含む複数の特徴点を抽出する。前記特定部は、異なる時刻で撮影された２つの前記撮影画像から抽出された前記第１特徴点及び前記第２特徴点の各位置変化を算出し、前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する。前記推定部は、前記特定部で特定した面の組に基づき前記カメラの姿勢を推定する。【選択図】図１

Description

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

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

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

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

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

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

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

上記第１の構成の姿勢推定装置において、前記異なる時刻は、前記移動体の直進移動中における時刻である構成（第２の構成）であってもよい。

上記第１又は第２の構成の姿勢推定装置において、前記抽出部は、前記撮影画像において前記特徴点が他に比べて集中して存在する高密度領域の中から前記第１特徴点及び前記第２特徴点を抽出する構成（第３の構成）であってもよい。

上記第１〜第３いずれかの構成の姿勢推定装置において、前記第１特徴点及び前記第２特徴点は、コーナーらしさを示すコーナー度が所定のコーナー度閾値以上となる特定の特徴点である構成（第４の構成）であってもよい。

上記第１〜第４いずれかの構成の姿勢推定装置において、前記抽出部は、所定の条件を満たす特徴点を抽出対象から除外する除外部を有する構成（第５の構成）であってもよい。

上記第５の構成の姿勢推定装置において、前記所定の条件の少なくとも一つは、前記移動体の移動に基づく条件である構成（第６の構成）であってもよい。

上記第５又は第６の構成の姿勢推定装置において、前記所定の条件の少なくとも一つは、各々の特徴点の履歴に基づく条件である構成（第７の構成）であってもよい。

上記第５〜第７いずれかの構成の姿勢推定装置において、前記所定の条件の少なくとも一つは、第１時刻から第２時刻までの複数の特徴点の各位置変化間の比較に基づく条件である構成（第８の構成）であってもよい。

上記第１〜第８いずれかの構成の姿勢推定装置において、前記カメラのパラメータ、前記抽出部によって抽出される特徴点の密度、特徴点の位置変化を算出する処理の内容、前記移動体の状態、前記カメラのレンズの状態、及び特徴点の位置変化間のばらつきの少なくとも一つに基づき信頼度を変更する変更部を備え、前記推定部は、前記信頼度に基づき処理内容を決定する構成（第９の構成）であってもよい。

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

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

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

本発明によると、カメラの姿勢を精度良く推定できる。

異常検出システムの構成を示すブロック図 車載カメラが車両に配置される位置を例示する図 姿勢推定装置によるカメラの姿勢推定フローの一例を示すフローチャート 特徴点を抽出する手法を説明するための図 第１特徴点のオプティカルフロー及び第２特徴点のオプティカルフローを導出する手法を説明するための図 オプティカルフローから仮想的に形成される四角形を示す図 所定の平面との交線が互いに平行になる面の組を２組特定する手法を説明するための図 特定した面の組の一方に基づき、面同士の交線の方向を求める手法を説明するための図 特定した面の組の他方に基づき、面同士の交線の方向を求める手法を説明するための図 補正システムの構成を示すブロック図 姿勢推定装置の変形例の概要を示すブロック図 姿勢推定装置の他の変形例の概要を示すブロック図

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

図３に示すように、まず、制御部１２は、フロントカメラ２１を搭載する車両４が直進しているか否かを監視する（ステップＳ１）。車両４が直進しているか否かは、例えば、舵角センサ３から得られるステアリングホイールの回転角情報に基づいて判断することができる。例えば、ステアリングホイールの回転角がゼロのときに車両４が完全にまっすぐに進むとした場合に、回転角がゼロの場合だけでなく、回転角がプラス方向とマイナス方向の一定範囲内の値である場合を含めて、車両４が直進していると判断してよい。なお、直進には、前方方向の直進と、後退方向の直進との両方が含まれる。

制御部１２は、車両４の直進を検出するまで、ステップＳ１の監視を繰り返す。言い換えると、制御部１２は、車両４が直進しない限り、カメラの姿勢を推定しない。これによれば、移動体の直進移動中における第１特徴点の位置変化及び第２特徴点の位置変化を用いてカメラの姿勢を推定することになり、車両４の進行方向が曲がっている場合の情報を用いてカメラの姿勢推定が行われないので、カメラの姿勢を推定するための情報処理、より詳細には後述するステップＳ３の処理が複雑になることを避けることができる。

なお、後述するステップＳ３の処理において、互いに重なっていない第１特徴点のオプティカルフローと第２特徴点のオプティカルフローが導出できればカメラの姿勢を推定することができるので、本実施形態とは異なり車両４の進行方向が曲がっているときにカメラの姿勢を推定してもよい。

本実施形態では、車両４が直進していると判断される場合（ステップＳ１でＹＥＳ）、抽出部１２１によって特徴点の抽出が行われる（ステップＳ２）。

図４は、第１特徴点ＦＰ１及び第２特徴点ＦＰ２を抽出する手法を説明するための図である。図４は、フロントカメラで撮影される撮影画像Ｐを模式的に示している。撮影画像Ｐは、車両４のボディが映り込む領域ＢＯを含む。第１特徴点ＦＰ１及び第２特徴点ＦＰ２は路面ＲＳ上に存在する。図４においては、路面に描かれている制限速度を示す数字の部分に第１特徴点ＦＰ１及び第２特徴点ＦＰ２が存在する。

第１特徴点ＦＰ１及び第２特徴点ＦＰ２が抽出されると、特定部１２２は、第１特徴点ＦＰ１のオプティカルフローＯＦ１と、第２特徴点ＦＰ２のオプティカルフローＯＦ２と、を導出する（ステップＳ３）。

図５は、第１特徴点ＦＰ１のオプティカルフローＯＦ１及び第２特徴点ＦＰ２のオプティカルフローＯＦ２を導出する手法を説明するための図である。図５は、図４と同様に便宜的に示された模式図である。図５は、図４に示す撮影画像（前フレームＰ）の撮影後、所定時間が経過した後にフロントカメラ２１で撮影された撮影画像（現フレームＰ’）である。図４に示す撮影画像Ｐの撮影後、所定時間が経過するまでの間に、車両４は前方に直進している。図５に示す丸印ＦＰ１Ｐは、図４に示す撮影画像Ｐの撮影時点における第１特徴点ＦＰ１の位置を示す。図５に示す丸印ＦＰ２Ｐは、図４に示す撮影画像Ｐの撮影時点における第２特徴点ＦＰ２の位置を示す。

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

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

特徴点の座標変換が行われると、特定部１２２は、図６に示すように、第１特徴点ＦＰ１のオプティカルフローＯＦ１の始点を頂点ＳＰ１、第１特徴点ＦＰ１のオプティカルフローＯＦ１の終点を頂点ＥＰ１、第２特徴点ＦＰ１のオプティカルフローＯＦ２の始点を頂点ＳＰ２、及び第２特徴点ＦＰ２のオプティカルフローＯＦ２の終点を頂点ＥＰとする四角形ＱＬを仮想的に形成する（ステップＳ４）。以降、説明のために四角形や、該四角形の辺などの幾何的要素を仮想的に形成して用いる。しかし、実際の処理では、特徴点の座標や直線の方向などのベクトル演算に基づき、同等の作用を持つ幾何的要素に基づかない処理としてもよい。

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

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

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

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

本実施形態では、異常検出装置１は、車両４が直進している場合に、異なる時刻に撮影された２つの画像から特徴点を抽出し、該特徴点のオプティカルフローを算出する。また、該特徴点は路面などの所定の平面上に位置している静止物から抽出される。したがって、算出されるオプティカルフローは実世界上では、車両４に対する静止物の相対的な位置変化を表す。つまり向きが逆となった車両４の移動ベクトルである。

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

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

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

なお、図７において四角形ＱＬ２は、現フレームＰ’の撮影時点での第１特徴点ＦＰ１の３次元空間（実世界）上の位置、現フレームＰ’の撮影時点での第２特徴点ＦＰ２の３次元空間上の位置、前フレームＰの撮影時点での第１特徴点ＦＰ１の３次元空間上の位置、及び前フレームＰの撮影時点での第２特徴点ＦＰ２の３次元空間上の位置を頂点とする四角形である。第１面Ｆ１は、四角形ＱＬ２の第１辺ＳＤ１１を含む。同様に、第２面Ｆ２は四角形ＱＬ２の第２辺ＳＤ１２を含み、第３面Ｆ３は四角形ＱＬ２の第３辺ＳＤ１３を含み、第４面Ｆ４は四角形ＱＬ２の第４辺ＳＤ１４を含む。このとき、上記のように四角形ＱＬ２は路面上に形成される平行四辺形であると想定される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

例えば、図１１に示すように、姿勢推定装置１４（図１１において不図示）に設けられる抽出部１２１が、所定の条件を満たす特徴点を抽出対象から除外する除外部１２１ａを有するようにしてもよい。これにより、特定部１２２において誤ったオプティカルフローが導出されることを抑制することができ、推定部１２３での推定精度を向上させることができる。

上記所定の条件の少なくとも一つを、例えば車両４の移動に基づく条件にするとよい。これにより、比較的大きく誤ったオプティカルフローが導出されることを抑制することができる。

具体的には、第１時刻から第２時刻までの或る特徴点のオプティカルフローを路面上の座標系に変換した結果（変換後のベクトルの大きさ）と、第１時刻から第２時刻までの車両４の移動量との差が第１所定値以上である場合に、除外部１２１ａは、当該或る特徴点を上記所定の条件を満たす特徴点として抽出対象から除外すればよい。同様に、第１時刻から第２時刻までの或る特徴点のオプティカルフローを路面上の座標系に変換した結果（変換後のベクトルの向き）と、第１時刻から第２時刻までの車両４の移動方向の逆方向との差が第２所定値以上である場合に、除外部１２１ａは、当該或る特徴点を上記所定の条件を満たす特徴点として抽出対象から除外すればよい。なお、上述した第１時刻及び第２時刻は、互いに異なる時刻である。

除外部１２１ａは、例えばＣＡＮバス等の通信バスＢ１（図１１において不図示）を用いて、車両４の移動量及び移動方向に関する情報を取得すればよい。しかしながら、ＣＡＮバス等の通信バスＢ１（図１１において不図示）を用いて取得された車両４の移動量及び移動方向に関する情報は、車両４の車速の誤差を含んでおり、且つ、撮影画像から得られるデータとの間に遅延量の差が生じる。したがって、車両４の移動に基づく条件を用いた場合には、比較的大きく誤ったオプティカルフローが導出されることしか抑制することができない。

そこで、上記所定の条件の少なくとも一つを、例えば各々の特徴点の履歴に基づく条件にすることが望しい。これにより、比較的小さく誤ったオプティカルフローが導出されることも抑制することができる。

具体的には、第１時刻から第２時刻までの或る特徴点のオプティカルフローの大きさと第２時刻から第３時刻までの或る特徴点のオプティカルフローの大きさとの差が第１所定値以上である場合に、除外部１２１ａは、当該或る特徴点を上記所定の条件を満たす特徴点として抽出対象から除外すればよい。同様に、第１時刻から第２時刻までの或る特徴点のオプティカルフローの向きと第２時刻から第３時刻までの或る特徴点のオプティカルフローの向きとの差が第２所定値以上である場合に、除外部１２１ａは、当該或る特徴点を上記所定の条件を満たす特徴点として抽出対象から除外すればよい。なお、上述した第１時刻、第２時刻、第３時刻はそれぞれ異なる時刻であり、上述した第２時刻は、上述した第１時刻より後であり、上述した第３時刻より前の時刻である。

また、上記所定の条件の少なくとも一つを、例えば第１時刻から第２時刻までの複数の特徴点の各位置変化間の比較に基づく条件にしてもよい。これにより、比較的小さく誤ったオプティカルフローが導出されることも抑制することができる。

具体的には、第１時刻から第２時刻までの或る特徴点のオプティカルフローの大きさと第１時刻から第２時刻までの他の或る特徴点のオプティカルフローの大きさとの差が第１所定値以上である場合に、除外部１２１ａは、当該或る特徴点を上記所定の条件を満たす特徴点として抽出対象から除外すればよい。同様に、第１時刻から第２時刻までの或る特徴点のオプティカルフローの向きと第１時刻から第２時刻までの他の或る特徴点のオプティカルフローの向きとの差が第２所定値以上である場合に、除外部１２１ａは、当該或る特徴点及び当該他の或る特徴点を上記所定の条件を満たす特徴点として抽出対象から除外すればよい。なお、上述した第１時刻及び第２時刻は、互いに異なる時刻である。ここでは、２つの特徴点のオプティカルフロー同士を比較したが、３つ以上の特徴点のオプティカルフロー同士を比較するようにし、他の特徴点と比較してオプティカルフローの大きさや向きが或るレベル以上異なっている特徴点のみが除外されるようにしてもよい。

また例えば、図１２に示すように、姿勢推定装置１４（図１２において不図示）に設けられる変更部１２６が信頼度に関する情報を推定部１２４に供給するようにしてもよい。この場合、変更部１２６が、カメラのパラメータ、抽出部１２１によって抽出される特徴点の密度、特徴点の位置変化を算出する処理の内容、車両４の状態、カメラのレンズの状態、及び複数の特徴点の位置変化間のばらつきの少なくとも一つに基づき信頼度を変更し、推定部１２３が、上述した信頼度に基づき処理内容を決定すればよい。これにより、画素レベルでの誤差によって特徴点のオプティカルフローの導出結果の精度が低下し、カメラの姿勢の推定精度が低下することを防止することができる。

例えば、変更部１２６が、カメラのシャッター速度（カメラのパラメータの一例）が遅いほど、信頼度を低下させるとよい。カメラのシャッター速度が遅ければ、画像がぼやけ易くなり、特徴点の抽出や追跡の精度が低下するためである。なお、シャッターとは、メカシャッターに限定されず、電子シャッターを含む。

具体的には、変更部１２６が、カメラのシャッター速度と信頼度との関係を式やデータテーブルの形式で予め記憶しておき、カメラのシャッター速度に関する情報を取得すればよい。なお、カメラが、カメラ周辺の照度に応じてシャッター速度を調整する構成である場合には、変更部１２６が、カメラのシャッター速度に関する情報の代わりに、カメラ周辺の照度に関する情報を取得するようにしてもよい。すなわち、変更部１２６が、カメラ周辺の照度を間接的なカメラのパラメータとして用いてもよい。

例えば、変更部１２６が、カメラのＡＧＣ（Auto Gain Control）によるゲイン（カメラのパラメータの一例）の変化率が大きいほど、信頼度を低下させるとよい。ゲインの変化率が大きいと、特徴点の輝度が急激に変化するため特徴点の追跡精度が低下するためである。

具体的には、変更部１２６が、カメラのＡＧＣによるゲインの変化率と信頼度との関係を式やデータテーブルの形式で予め記憶しておき、カメラのＡＧＣによるゲインの変化率に関する情報を取得すればよい。

例えば、変更部１２６が、特徴点の密度（特定範囲内に存在する特徴点の個数）が多いほど、信頼度を低下させるとよい。特徴点同士の距離が近いと、追跡対象とは別の特徴点を誤って追跡してしまう可能性が高まり、特徴点の追跡精度が低下するためである。

具体的には、変更部１２６が、特徴点の密度と信頼度との関係を式やデータテーブルの形式で予め記憶しておき、特徴点の密度に関する情報を取得すればよい。

例えば、変更部１２６が、特徴点のオプティカルフローを算出する際の収束回数（特徴点の位置変化を算出する処理の内容の一例）が多いほど、信頼度を低下させるとよい。収束回数が多いと、無理にオプティカルフローを算出しており、オプティカルフローの算出精度が低下するためである。

具体的には、変更部１２６が、特徴点のオプティカルフローを算出する際の収束回数と信頼度との関係を式やデータテーブルの形式で予め記憶しておき、特徴点のオプティカルフローを算出する際の収束回数に関する情報を取得すればよい。

例えば、変更部１２６が、車両４の車速（車両４の状態の一例）が速いほど、信頼度を低下させるとよい。車両４の車速が速ければ、画像がぼやけ易くなり、特徴点の抽出や追跡の精度が低下するためである。

具体的には、変更部１２６が、車両４の車速と信頼度との関係を式やデータテーブルの形式で予め記憶しておき、車両４の車速に関する情報を取得すればよい。

例えば、変更部１２６が、車両４の舵角（車両４の状態の一例）が大きいほど、信頼度を低下させるとよい。車両４の舵角が大きければ、仮想的に生成する四角形の形状精度が低下するためである。

具体的には、変更部１２６が、車両４の舵角と信頼度との関係を式やデータテーブルの形式で予め記憶しておき、車両４の舵角に関する情報を取得すればよい。

例えば、変更部１２６が、カメラのレンズの汚れ（カメラのレンズの状態の一例）がひどいほど、信頼度を低下させるとよい。カメラのレンズの汚れがひどければ、画像がぼやけ易くなり、特徴点の抽出や追跡の精度が低下するためである。

具体的には、変更部１２６が、カメラのレンズの汚れと信頼度との関係を式やデータテーブルの形式で予め記憶しておき、カメラのレンズの汚れに関する情報を取得すればよい。カメラのレンズの汚れ度合いは、例えば撮影画像の全画素に対する、撮影画像の複数フレーム間において輝度変化が所定範囲内である画素の割合とすることができる。

例えば、変更部１２６が、連続する３つ以上のフレームにおける隣接フレーム間での特徴点の各オプティカルフローの長さのばらつき（特徴点の位置変化間のばらつきの一例）が大きいほど、信頼度を低下させるとよい。特徴点の位置変化間のばらつきが大きければ、特徴点の抽出や追跡の精度が低下している可能性が高いためである。

具体的には、変更部１２６が、連続する３つ以上のフレームにおける隣接フレーム間での特徴点の各オプティカルフローの長さのばらつきと信頼度との関係を式やデータテーブルの形式で予め記憶しておき、連続する３つ以上のフレームにおける隣接フレーム間での特徴点の各オプティカルフローの長さのばらつきに関する情報を取得すればよい。

推定部１２３は、例えば信頼度が所定レベル以上であれば推定処理を実行し、信頼度が所定レベル未満であれば推定処理を実行しないようにすればよい。この場合、信頼度が低ければカメラの姿勢が推定されないので、カメラの姿勢の推定精度が低下することを防止することができる。

また、推定部１２３は、例えば信頼度が所定レベル以上であれば異なる時刻や異なる特徴点を用いてＭ回の角度推定を実行してＭ回の角度推定に基づく角度推定結果（例えばＭ回の平均）を求め、信頼度が所定レベル未満であれば異なる時刻や異なる特徴点を用いてＮ（＞Ｍ）回の角度推定を実行してＮ回の角度推定に基づく角度推定結果（例えばＮ回の平均）を求めてもよい。この場合、信頼度が低くても、統計的処理によってカメラの姿勢の推定精度が低下することを防止することができる。

１ 異常検出装置
１’ 補正装置
１４ 姿勢推定装置
２１〜２４ 車載カメラ
１１ 取得部
１２１ 抽出部
１２１ａ 除外部
１２２ 特定部
１２３ 推定部
１２４ 判定部
１２５ 補正部
１２６ 変更部

Claims (12)

1. 移動体に搭載されたカメラで撮影された撮影画像を取得する取得部と、
   前記撮影画像から第１特徴点及び第２特徴点を含む複数の特徴点を抽出する抽出部と、
   異なる時刻で撮影された２つの前記撮影画像から抽出された前記第１特徴点及び前記第２特徴点の各位置変化を算出し、前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定部と、
   前記特定部で特定した面の組に基づき前記カメラの姿勢を推定する推定部と、
   を備える、姿勢推定装置。
2. 前記異なる時刻は、前記移動体の直進移動中における時刻である、請求項１に記載の姿勢推定装置。
3. 前記抽出部は、前記撮影画像において前記特徴点が他に比べて集中して存在する高密度領域の中から前記第１特徴点及び前記第２特徴点を抽出する、請求項１又は請求項２に記載の姿勢推定装置。
4. 前記第１特徴点及び前記第２特徴点は、コーナーらしさを示すコーナー度が所定のコーナー度閾値以上となる特定の特徴点である、請求項１〜３のいずれか一項に記載の姿勢推定装置。
5. 前記抽出部は、所定の条件を満たす特徴点を抽出対象から除外する除外部を有する、請求項１〜４のいずれか一項に記載の姿勢推定装置。
6. 前記所定の条件の少なくとも一つは、前記移動体の移動に基づく条件である、請求項５に記載の姿勢推定装置。
7. 前記所定の条件の少なくとも一つは、各々の特徴点の履歴に基づく条件である、請求項５又は請求項６に記載の姿勢推定装置。
8. 前記所定の条件の少なくとも一つは、第１時刻から第２時刻までの複数の特徴点の各位置変化間の比較に基づく条件である、請求項５〜７のいずれか一項に記載の姿勢推定装置。
9. 前記カメラのパラメータ、前記抽出部によって抽出される特徴点の密度、特徴点の位置変化を算出する処理の内容、前記移動体の状態、前記カメラのレンズの状態、及び特徴点の位置変化間のばらつきの少なくとも一つに基づき信頼度を変更する変更部を備え、
   前記推定部は、前記信頼度に基づき処理内容を決定する、請求項１〜８のいずれか一項に記載の姿勢推定装置。
10. 請求項１〜９のいずれか一項に記載の姿勢推定装置と、
    前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラの取付けのずれが生じた状態であるか否かを判定する判定部と、
    を備える、異常検出装置。
11. 請求項１〜９のいずれか一項に記載の姿勢推定装置と、
    前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラのパラメータを補正する補正部と、
    を備える、補正装置。
12. 移動体に搭載されたカメラで撮影された撮影画像を取得する取得工程と、
    前記撮影画像から第１特徴点及び第２特徴点を含む複数の特徴点を抽出する抽出工程と、
    異なる時刻で撮影された２つの前記撮影画像から抽出された前記第１特徴点及び前記第２特徴点の各位置変化を算出し、前記第１特徴点の位置変化及び前記第２特徴点の位置変化に基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定工程と、
    前記特定工程で特定した面の組に基づき前記カメラの姿勢を推定する推定工程と、
    を備える、姿勢推定方法。