JP2021093670A - 姿勢推定装置、異常検出装置、補正装置、および、姿勢推定方法 - 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を利用して実行することができる。

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

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

姿勢推定に対する特徴点の抽出誤差の影響度合いがオプティカルフローの位置によって異なるため、上述した通り、選択部１２３は、フロー導出部１２２で導出されたオプティカルフローの位置に基づいて、フロー導出部１２２で導出されたオプティカルフローの中から第１特徴点のオプティカルフロー及び第２特徴点のオプティカルフローを選択している。これにより、姿勢推定に対する特徴点の抽出誤差の影響度合いが小さくなる第１特徴点のオプティカルフロー及び第２特徴点のオプティカルフローを姿勢推定に用いることが可能となる。したがって、姿勢推定装置１４は、姿勢推定に用いるオプティカルフローの位置に依存して姿勢推定の精度が低下することを抑制できる。

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

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

＜４．オプティカルフローの選択＞
次に、選択部１２３の詳細動作について説明する。

図１０は、姿勢推定装置１４がフロントカメラ２１の撮影画像を用いてフロントカメラ２１の姿勢を推定した結果の期待値に対する誤差（ＰＡＮ、ＴＩＬＴ、ＲＯＬＬの各誤差）を縦軸にとり、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離を横軸にとったグラフである。

図１０に示すグラフから明らかな通り、姿勢推定装置１４がフロントカメラ２１の撮影画像を用いてフロントカメラ２１の姿勢を推定する場合には、選択部１２３は、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との所定の方向における距離が第１所定値Ｖ１（図１０参照）以上になるように、第１特徴点のオプティカルフロー及び第２特徴点のオプティカルフローを選択することが好ましい。これにより、姿勢推定に対する特徴点の抽出誤差の影響度合いが小さくなり、姿勢推定の精度が高くなるからである。

ここで所定の方向とは、具体的には路面に平行かつ、車両の進行方向に対して垂直な方向とするとよい。例えばフロントカメラ２１およびバックカメラ２２の場合、カメラの光軸と車両の進行方向が一致するため、所定の方向は画像上で水平方向（左右方向）である。また左サイドカメラ２３および右サイドカメラ２４の場合、カメラの光軸と車両の進行方向が直交するため、所定の方向は画像上で垂直方向（奥行き方向）である。

また、所定の方向における距離とは、ある２点が所定の方向に沿ってどれだけ離れているかの指標である。具体的には、２点間に位置ベクトル形成した場合に、前記位置ベクトルを所定の方向に対して投影したベクトルの長さとして算出する。

ただし、図１０に示すグラフから明らかな通り、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との所定の方向における距離が第２所定値Ｖ２（Ｖ２＞Ｖ１、図１０参照）以上になると、歪みの影響で姿勢推定の精度が低下してしまう。

したがって、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との所定の方向における距離が第２所定値Ｖ２以上になることを回避するために、選択部１２３は、フロントカメラ２１の光軸中心から所定距離以内の領域に位置する第１特徴点のオプティカルフロー及び第２特徴点のオプティカルフローを選択することが好ましい。言い換えると、選択部１２３は、撮影画像の周辺部から第１特徴点のオプティカルフロー及び第２特徴点のオプティカルフローを選択しないことが好ましい。

ここでは、姿勢推定装置１４がフロントカメラ２１の撮影画像を用いてフロントカメラ２１の姿勢を推定する場合について説明したが、姿勢推定装置１４がバックカメラ２２の撮影画像を用いてフロントカメラ２２の姿勢を推定する場合も同様である。

図１１は、姿勢推定装置１４が右サイドカメラ２４の撮影画像を用いて右サイドカメラ２４の姿勢を推定した結果の期待値に対する誤差（ＰＡＮ、ＴＩＬＴ、ＲＯＬＬの各誤差）を横軸にとり、第１特徴点及び第２特徴点のうち車両４に近い側の特徴点を基準とし、すなわち、第１特徴点及び第２特徴点のうち車両４に近い側の特徴点の実世界上の位置を固定し、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離を縦軸にとったグラフである。

図１２は、姿勢推定装置１４が右サイドカメラ２４の撮影画像を用いて右サイドカメラ２４の姿勢を推定した結果の期待値に対する誤差（ＰＡＮ、ＴＩＬＴ、ＲＯＬＬの各誤差）を横軸にとり、第１特徴点及び第２特徴点のうち車両４に近い側の特徴点を基準とし、すなわち、第１特徴点及び第２特徴点のうち車両４に遠い側の特徴点の実世界上の位置を固定し、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離を縦軸にとったグラフである。

図１１に示すグラフ及び図１２に示すグラフから明らかな通り、姿勢推定装置１４が右サイドカメラ２４の撮影画像を用いて右サイドカメラ２４の姿勢を推定する場合には、選択部１２３は、右サイドカメラ２４の光軸中心から所定距離以内の領域に位置する第１特徴点のオプティカルフロー及び右サイドカメラ２４の光軸中心から上記所定距離より離れた領域に位置する第２特徴点のオプティカルフローを選択することが好ましい。これにより、姿勢推定に対する特徴点の抽出誤差の影響度合いが小さくなり、姿勢推定の精度が高くなるからである。

なお、図１１に示すグラフ及び図１２に示すグラフの比較から明らかな通り、選択部１２３は、第１特徴点及び第２特徴点のうち車両４に近い側の特徴点を基準とし、すなわち、第１特徴点及び第２特徴点のうち車両４に近い側の特徴点の実世界上の位置を固定して、選択処理を行うことが好ましい。第１特徴点及び第２特徴点のうち車両４に近い側の特徴点を基準とした方が、第１特徴点及び第２特徴点のうち車両４に遠い側の特徴点を基準とするよりも、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離が小さいときに歪みの影響で姿勢推定の精度が低下してしまうことを抑制できるからである。

ここでは、姿勢推定装置１４が右サイドカメラ２４の撮影画像を用いて右サイドカメラ２４の姿勢を推定する場合について説明したが、姿勢推定装置１４が左サイドカメラ２３の撮影画像を用いて左サイドカメラ２３の姿勢を推定する場合も同様である。

図１３〜図１５は、姿勢推定装置１４が右サイドカメラ２４の撮影画像を用いて右サイドカメラ２４の姿勢を推定した結果の期待値に対する誤差（ＰＡＮ、ＴＩＬＴ、ＲＯＬＬの各誤差）を第１縦軸（紙面左側の縦軸）にとり、第１特徴点のオプティカルフローと第２特徴点のオプティカルフローとの撮影画像の水平方向におけるずれ量を第２縦軸（紙面右側の縦軸）にとったグラフである。

図１３は、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離が図１３〜図１５の中で最も小さい場合のグラフである。図１４は、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離が図１３〜図１５の中で２番目に小さい場合のグラフである。図１５は、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離が図１３〜図１５の中で最も小さい場合のグラフである。

図１３〜図１５に示すグラフから明らかな通り、姿勢推定装置１４が右サイドカメラ２４の撮影画像を用いて右サイドカメラ２４の姿勢を推定する場合には、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との左右方向における距離所定値以上になるように、第１特徴点のオプティカルフロー及び第２特徴点のオプティカルフローを選択し、第１特徴点の実世界上の位置と第２特徴点の実世界上の位置との前後方向に垂直な方向における距離が大きいほど、上記所定値を大きくすることが好ましい。これにより、第１特徴点のオプティカルフローと第２特徴点のオプティカルフローとの撮影画像の水平方向におけるずれ量が大きいときに姿勢推定の精度が低下してしまうことを抑制できるからである。

ここでは、姿勢推定装置１４が右サイドカメラ２４の撮影画像を用いて右サイドカメラ２４の姿勢を推定する場合について説明したが、姿勢推定装置１４が左サイドカメラ２３の撮影画像を用いて左サイドカメラ２３の姿勢を推定する場合も同様である。また、フロントカメラ２１の撮影画像を用いてフロントカメラ２１の姿勢を推定する場合、或いは、バックカメラ２２の撮影画像を用いてバックカメラ２２の姿勢を推定する場合も、左右方向と前後方向を入れ替えれば、同様である。

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

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

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

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

１ 異常検出装置
１’ 補正装置
１４ 姿勢推定装置
２１〜２４ 車載カメラ
１１ 取得部
１２１ 抽出部
１２２ フロー導出部
１２３ 選択部
１２４ 特定部
１２５ 推定部
１２６ 判定部
１２７ 補正部

Claims (10)

1. 移動体に搭載されたカメラで撮影された撮影画像を取得する取得部と、
   前記撮影画像から第１特徴点及び第２特徴点を含む複数の特徴点を抽出する抽出部と、
   前記複数の特徴点ごとにオプティカルフローを導出するフロー導出部と、
   前記フロー導出部で導出されたオプティカルフローの位置に基づいて、前記フロー導出部で導出されたオプティカルフローの中から前記第１特徴点のオプティカルフロー及び前記第２特徴点のオプティカルフローを選択する選択部と、
   前記選択部で選択された前記第１特徴点のオプティカルフロー及び前記第２特徴点のオプティカルフローに基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定部と、
   前記特定部で特定した面の組に基づき前記カメラの姿勢を推定する推定部と、
   を備える、姿勢推定装置。
2. 前記選択部は、前記第１特徴点の実世界上の位置と前記第２特徴点の実世界上の位置との所定の方向における距離が所定値以上になるように、前記第１特徴点のオプティカルフロー及び前記第２特徴点のオプティカルフローを選択する、請求項１に記載の姿勢推定装置。
3. 前記選択部は、前記第１特徴点の実世界上の位置と前記第２特徴点の実世界上の位置との前記所定の方向に垂直な方向における距離が大きいほど、前記所定値を大きくする、請求項２に記載の姿勢推定装置。
4. 前記選択部は、前記カメラの光軸中心から所定距離以内の領域に位置する前記第１特徴点のオプティカルフロー及び前記第２特徴点のオプティカルフローを選択する、請求項２又は請求項３に記載の姿勢推定装置。
5. 前記カメラは、前記移動体の前方又は後方を臨む、請求項４に記載の姿勢推定装置。
6. 前記選択部は、前記カメラの光軸中心から所定距離以内の領域に位置する前記第１特徴点のオプティカルフロー及び前記カメラの光軸中心から前記所定距離より離れた領域に位置する前記第２特徴点のオプティカルフローを選択する、請求項２又は請求項３に記載の姿勢推定装置。
7. 前記カメラは、前記移動体の左方又は右方を臨む、請求項６に記載の姿勢推定装置。
8. 請求項１〜７のいずれか一項に記載の姿勢推定装置と、
   前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラの取付けのずれが生じた状態であるか否かを判定する判定部と、
   を備える、異常検出装置。
9. 請求項１〜７のいずれか一項に記載の姿勢推定装置と、
   前記推定部によって推定された前記カメラの姿勢に基づき、前記カメラのパラメータを補正する補正部と、
   を備える、補正装置。
10. 移動体に搭載されたカメラで撮影された撮影画像を取得する取得工程と、
    前記撮影画像から第１特徴点及び第２特徴点を含む複数の特徴点を抽出する抽出工程と、
    前記複数の特徴点ごとにオプティカルフローを導出するフロー導出工程と、
    前記フロー導出工程で導出されたオプティカルフローの位置に基づいて、前記フロー導出工程で導出されたオプティカルフローの中から前記第１特徴点のオプティカルフロー及び前記第２特徴点のオプティカルフローを選択する選択工程と、
    前記選択工程で選択された前記第１特徴点のオプティカルフロー及び前記第２特徴点のオプティカルフローに基づき、所定の平面との交線が互いに平行となる面の組を２組特定する特定工程と、
    前記特定工程で特定した面の組に基づき前記カメラの姿勢を推定する推定工程と、
    を備える、姿勢推定方法。

Images