以下、本発明の例示的な実施形態について、図面を参照しながら詳細に説明する。以下の説明では、移動体として車両を例にとり説明するが、移動体は車両に限定されない。車両には、例えば自動車、電車、二輪車、無人搬送車等の車輪のついた乗り物が広く含まれる。車両以外の移動体として、例えば船舶や航空機等が挙げられる。
また以下の説明では、車両の直進進行方向であって、運転席からハンドルに向かう方向を「前方向」とする。また、車両の直進進行方向であって、ハンドルから運転席に向かう方向を「後方向」とする。また、車両の直進進行方向及び鉛直線に垂直な方向であって、前方向を向いている運転者の右側から左側に向かう方向を「左方向」とする。また、車両の直進進行方向及び鉛直線に垂直な方向であって、前方向を向いている運転者の左側から右側に向かう方向を「右方向」とする。
<1.姿勢推定装置の概要>
図1は、本発明の実施形態に係る姿勢推定装置1の構成を説明するための図である。図1には、姿勢推定装置1に情報を入力する撮影部2、舵角センサ3、および、速度センサ4も示されている。本実施形態では、姿勢推定装置1は、撮影部2が搭載される車両ごとに備えられる。以下、姿勢推定装置1が備えられる車両のことを自車両と表現することがある。なお、姿勢推定装置1は、車載装置であっても、車両から持ち出し可能な携帯型の装置であってもよい。また、姿勢推定装置1は、必ずしも車両等の移動体に搭載されなくてよい。
撮影部2は、車両外部の状況を監視する目的で車両に設けられる。撮影部2は、少なくとも1つのカメラ21を備える。カメラ21は、いわゆる車載カメラであり、車両に固定配置される。カメラ21は、姿勢推定装置1に有線又は無線により接続され、撮影画像を姿勢推定装置1に出力する。
なお、撮影部2が車両の周囲の状況を監視する目的で設けられる場合には、撮影部2は、好ましくは、フロントカメラと、バックカメラと、左サイドカメラと、右サイドカメラとの4つのカメラ21を備える。フロントカメラは、車両の前方を撮影するカメラである。バックカメラは、車両の後方を撮影するカメラである。左サイドカメラは、車両の左方を撮影するカメラである。右サイドカメラは、車両の右方を撮影するカメラである。これら4つのカメラ21は、例えば魚眼レンズを用いて構成され、水平方向の画角θは180度以上とされる。これにより、4つのカメラ21によって、車両の水平方向における全周囲を撮影することができる。
舵角センサ3は、姿勢推定装置1および撮影部2が搭載される車両に備えられ、当該車両のステアリングホイール(ハンドル)の回転角を検出する。舵角センサ3の出力は、CAN(Controller Area Network)バス等の通信バスB1を介して姿勢推定装置1に入力される。
速度センサ4は、姿勢推定装置1および撮影部2が搭載される車両に備えられ、当該車両の速度を検出する。速度センサ4の出力は、通信バスB1を介して姿勢推定装置1に入力される。
姿勢推定装置1は、車両(移動体)に搭載されるカメラ21で撮影された撮影画像に基づき、カメラ21の姿勢を推定する。車両に固定されるカメラ21は、例えば、経年劣化や外部からの衝撃等によって車両への取付け姿勢が変化することがある。姿勢推定装置1は、カメラ21の姿勢の推定を行うことにより、カメラ21の取付け姿勢の変化(異常)を検出することができる。
図1に示すように、姿勢推定装置1は、取得部11と、制御部12と、記憶部13と、を備える。
取得部11は、車両に搭載されるカメラ21から撮影画像を取得する。取得部11は、カメラ21からアナログ又はデジタルの撮影画像を所定の周期(例えば、1/30秒周期)で時間的に連続して取得する。すなわち、取得部11によって取得される撮影画像の集合体がカメラ21で撮影された動画像である。そして、取得した撮影画像がアナログの場合には、取得部11は、そのアナログの撮影画像をデジタルの撮影画像に変換(A/D変換)する。取得部11は、取得した撮影画像(A/D変換が行われた場合には変換後の撮影画像)を制御部12に出力する。取得部11から出力される1つの撮影画像が1つのフレーム画像となる。
制御部12は、例えばマイクロコンピュータ等のコンピュータであり、姿勢推定装置1の全体を統括的に制御するコントローラである。制御部12は、不図示のCPU(Central Processing Unit)、RAM(Random Access Memory)、及びROM(Read Only Memory)を含む。記憶部13は、例えば、フラッシュメモリ等の不揮発性のメモリであり、各種の情報を記憶する。記憶部13は、ファームウェアとしてのプログラムや各種のデータを記憶する。
図1に示す抽出部121、算出部122、および、推定部123は、制御部12のCPUが記憶部13に記憶されるプログラムに従って演算処理を実行することにより実現される制御部12の機能である。言い換えると、姿勢推定装置1は、抽出部121と、算出部122と、推定部123とを備える。
なお、制御部12の、抽出部121、算出部122、および、推定部123の少なくともいずれか一つは、ASIC(Application Specific Integrated Circuit)やFPGA(Field Programmable Gate Array)等のハードウェアで構成されてもよい。また、抽出部121、算出部122、および、推定部123は、概念的な構成要素である。1つの構成要素が実行する機能を複数の構成要素に分散させたり、複数の構成要素が有する機能を1つの構成要素に統合させたりしてよい。また、取得部11は、制御部12のCPUがプログラムに従って演算処理を行うことによって実現される構成でもよい。
抽出部121は、車両に搭載されるカメラ21で撮影された撮影画像から特徴点を抽出する。特徴点は、撮影画像中のエッジの交点など、撮影画像において際立って検出できる点である。特徴点は、例えば路面に描かれた白線のエッジ、路面上のひび、路面上のしみ、路面上の砂利などである。本実施形態では、好ましい形態として、コーナーらしさを示すコーナー度が高い特徴点を抽出する。コーナーとは2つのエッジが交わる部分である。コーナー度は、例えばHarrisオペレータやKLT(Kanade-Lucas-Tomasi)トラッカーなどの公知の検出手法を用いて求めることができる。
本実施形態では、抽出部121は、撮影画像中に設定される抽出領域から特徴点を抽出する。抽出領域は、撮影画像中の狙った対象から特徴点を効率良く抽出する等の目的で設定される。本実施形態では、狙った対象は、自車両が走行する路面である。このために、抽出領域は、撮影画像の路面が映る領域に設定される。なお、本明細書では、抽出領域のことをROI(Region Of Interest)と記載することがある。
算出部122は、異なる時刻で撮影された2つの撮影画像間における特徴点の移動量を算出する。本実施形態では、算出部122は、異なる時刻で撮影された2つの撮影画像から、カメラ21のカメラ座標系における特徴点の移動量を算出する。異なる時刻の間隔は、取得部11のフレーム周期と同一である。ただし、これに限らず、異なる時刻の間隔は、例えば、取得部11のフレーム周期の複数倍であってもよい。カメラ座標系における特徴点の移動量は、異なる時刻で撮影された2つの撮影画像間における特徴点の動きを表す動きベクトルであるオプティカルフローの長さに該当する。算出部122は、撮影画像から抽出された特徴点ごとにオプティカルフローを算出する。そして、算出部122は、算出したオプティカルフローごとにオプティカルフローの長さ(すなわち特徴点の移動量)を算出する。オプティカルフローを算出する方法の詳細ついては後述する。
推定部123は、算出部122で算出された移動量が所定値以下となる特徴点を除外して、残りの特徴点の動きに基づきカメラ21の姿勢の推定を行う。所定値は、例えば、異なる時刻で撮影された2つの撮影画像間において特徴点が動いていないとみなすことができる上限値であってよい。所定値は、例えば車両の振動等の影響を考慮して設定されてよい。また、所定値は、特徴点が動いていないとみなすことができる上限値よりも少し大きな値であってもよい。車両の速度が超低速度である場合、得られるオプティカルフローのばらつきが大きくなり易いことを考慮するものである。
所定値は、例えば撮影画像のNピクセルの長さに相当する。この場合、所定値以下とは、Nピクセル以内と同様の意味である。Nは数値であり、例えば実験やシミュレーションを行うことによって決められる。詳細には、推定部123は、除外対象となった特徴点から得られるオプティカルフローを除外した残りのオプティカルフローの少なくとも一部を用いて、カメラ21の姿勢の推定を行う。カメラ21の姿勢推定処理の詳細については後述する。
本実施形態では、撮影画像は車両の移動時に撮影される。また、推定部123は、特徴点が路面から抽出されることを前提として、特徴点の動き(オプティカルフロー)を用いたカメラ21の姿勢の推定を行う。このために、本実施形態においては、異なる時刻で撮影された2つの撮影画像間で動かない特徴点は異常な特徴点と言え、このような異常な特徴点を用いた姿勢の推定処理は誤推定の原因となる。この点、本実施形態では、姿勢の推定処理に際して、撮影画像から抽出される特徴点のうち、2つの撮影画像間において静止している、或いは、極端に動きが小さいと判断される特徴点は異常な特徴点として除外される構成となっている。このために、本実施形態では、誤推定の要因を減らして、カメラ21の姿勢推定の精度を向上することができる。また、本実施形態によれば、車両の速度が極端に遅い(ほぼ静止している)状態では姿勢の推定が行われないようにすることができ、姿勢推定の精度を向上することができる。
なお、推定部123は、車両の移動速度に応じて、所定値を変動させてよい。詳細には、車両の移動速度は、カメラ21にて撮影画像を取得した時の車両の速度のことである。車両の速度は、例えば速度センサ4から取得することができる。これによれば、除外対象となる特徴点を車両の走行状況に合わせて適切に選択することができる。例えば、車両の速度が速くなった場合には所定値が大きくされ、車両の速度が遅くなった場合には所定値が小さくされることが好ましい。
<2.姿勢推定処理>
次に、姿勢推定装置1によって実行されるカメラ21の姿勢推定処理について説明する。図2は、姿勢推定装置1において実行されるカメラ21の姿勢推定処理の一例を示すフローチャートである。なお、撮影部2が複数のカメラ21を備える場合には、姿勢推定装置1は、各カメラ21に対して、図2に示す姿勢推定処理を実行する。ここでは、カメラ21がフロントカメラである場合を例に、姿勢推定装置1によって実行されるカメラ21の姿勢推定処理について説明する。
図2に示すように、まず、制御部12は、カメラ21を搭載する自車両が直進しているか否かを監視する(ステップS1)。自車両が直進しているか否かは、例えば、舵角センサ3から得られるステアリングホイールの回転角情報に基づいて判断することができる。例えば、ステアリングホイールの回転角がゼロのときに自車両が完全にまっすぐに進むとした場合に、回転角がゼロの場合だけでなく、回転角がプラス方向とマイナス方向の一定範囲内の値である場合を含めて、自車両が直進していると判断してよい。なお、直進には、前方方向の直進と、後退方向の直進との両方が含まれる。
制御部12は、自車両の直進を検出するまで、ステップS1の監視を繰り返す。言い換えると、制御部12は、自車両が直進しない限り、カメラ21の姿勢の推定処理を進めない。これによれば、直進移動中における特徴点の位置変化を用いて姿勢の推定が行われることになり、自車両の進行方向が曲がっている場合の情報を用いて姿勢の推定が行われないので、姿勢の推定処理が複雑になることを避けることができる。ただし、自車両の進行方向が曲がっているときの特徴点の位置変化を用いて、姿勢の推定処理が進められる構成としてもよい。
なお、自車両が直進走行しているか否かの判断に加えて、制御部12は、自車両の速度が所定の速度範囲内であるか否かの判断を行う構成とすることが好ましい。自車両の速度が遅すぎると、例えば特徴点の動きが小さくオプティカルフローの算出の精度が低下する可能性がある。一方、自車両の速度が速すぎると、例えば特徴点の動きが速すぎて特徴点を追従できない可能性がある。自車両の速度が所定の速度範囲内である場合にのみ姿勢の推定処理を進めることにより、このような不具合の発生を抑制することができる。なお、所定の速度範囲は、例えば、時速10km以上、時速40km以下等であってよい。自車両の速度は、速度センサ4を利用して取得されてよい。
自車両が直進していると判断される場合(ステップS1でYes)、制御部12は、取得部11を介してカメラ21から撮影画像を取得する(ステップS2)。制御部12は、例えば数フレームから数十フレームの撮影画像を取得する。
次に、抽出部121が、各フレーム画像について、抽出領域(ROI)から特徴点の抽出を行う(ステップS3)。図3は、特徴点の抽出について説明するための図である。図3は、カメラ21で撮影された撮影画像Pを模式的に示している。撮影画像Pにおいては、自車両のボディBOの一部が映り込んでいる。図3において、符号5はROIである。ROI5は、撮影画像Pの路面RSが映る範囲に設定されている。図3に示す例では、ROI5は矩形状であるが、例えば円形状や楕円状等の矩形状以外の形状であってもよい。
図3に示す例では、路面RSに描かれている制限速度を示す数字の部分にコーナー度が高い第1特徴点FP1及び第2特徴点FP2が存在する。制限速度を示す数字は、ROI5内に存在する。このために、抽出部121は、路面RS上に存在する第1特徴点FP1及び第2特徴点FP2を抽出する。
なお、図3では、便宜的に2つの特徴点FP1、FP2のみが示されているが、コーナー度が所定の特徴点閾値を超える特徴点が存在すれば、これ以外にも特徴点は抽出される。逆に、コーナー度が所定の特徴点閾値を超える特徴点が存在しない場合には、特徴点は抽出されない。各フレーム画像において、特徴点が1つも抽出されないこともあり得る。
図2に戻って、特徴点の抽出処理が全てのフレーム画像に対して完了すると、算出部122は、オプティカルフローの算出を行う(ステップS4)。オプティカルフローは、上述のように、異なる時刻で撮影された2つの撮影画像間における特徴点の動きを示す動きベクトルである。本例では、異なる時刻の間隔は、取得部11のフレーム周期と同一である。
図4は、オプティカルフローの算出について説明するための図である。図4は、図3と同様に便宜的に示された模式図である。図4は、図3に示す撮影画像P(便宜的に前フレームPとする)の撮影後、1フレーム周期経過した後にカメラ21で撮影された撮影画像P’(便宜的に現フレームP’とする)である。図3に示す撮影画像Pの撮影後、1フレーム周期の間、自車両は前方に直進している。図4に示す丸印FP1Pは、図3に示す前フレームPの撮影時点における第1特徴点FP1の位置を示す。図4に示す丸印FP2Pは、図3に示す前フレームPの撮影時点における第2特徴点FP2の位置を示す。
図4に示すように、自車両が前方に直進すると、自車両の前方に存在する第1特徴点FP1及び第2特徴点FP2は自車両に近づく。すなわち、第1特徴点FP1及び第2特徴点FP2は、現フレームP’と前フレームPとで異なる位置に現れる。算出部122は、現フレームP’の第1特徴点FP1と前フレームPの第1特徴点FP1とを、その近傍の画素値に基づいて対応付け、対応付けた第1特徴点FP1のそれぞれの位置に基づいて、第1特徴点FP1のオプティカルフローOF1を算出する。同様に、算出部122は、現フレームP’の第2特徴点FP2と前フレームPの第2特徴点FP2とを、その近傍の画素値に基づいて対応付け、対応付けた第2特徴点FP2のそれぞれの位置に基づいて、第2特徴点FP2のオプティカルフローOF2を算出する。
なお、算出部122は、取得した複数のフレーム画像のそれぞれについて上述のようにしてオプティカルフローの算出を行う。また、各フレーム画像において2つより多くのオプティカルフローが得られる場合がある。逆に、各フレーム画像において、オプティカルフローが1つしか得られない場合や、1つも得られない場合がある。
図2に戻って、オプティカルフローが求められると、以後に行われる処理に使用するオプティカルフローが所定の選択条件に従って選択される(ステップS5)。所定の選択条件にしたがって、以後に行われる姿勢の推定に適した、2つのオプティカルフローで構成されるオプティカルフローの組が選択される。選択される当該組の数は、1つの場合もあるし、複数の場合もある。また、選択される組の数は、ゼロの場合もある。図2には記載されないが、選択される組の数がゼロの場合には、ステップS1に処理が戻される。
オプティカルフローの選択処理の詳細については後述する。ここでは、ステップS5で選択された使用オプティカルフローの組に、図4に示す第1オプティカルフローOF1と第2オプティカルフローOF2との組が含まれるものして、ステップS6以降の処理について説明する。
推定部123は、第1特徴点FP1のオプティカルフローOF1と第2特徴点FP2のオプティカルフローOF2を用いた処理を進める。推定部123は、特徴点に対し、記憶部13に記憶されているカメラ21の内部パラメータを用いて座標変換を行う。座標変換では、カメラ21の収差補正と、歪み補正とが行われる。収差補正は、カメラ21の光学系の収差による歪みを補正するために行われる。具体的には樽型歪みや糸巻歪みなど歪曲収差の補正である。歪み補正は、カメラ21の光学系そのものの歪みを補正するために行われる。具体的には魚眼補正などである。座標変換により、特徴点の座標は、被写体が透視投影によって撮影された場合に二次元画像上で得られる座標に変換される。
特徴点の座標変換が行われると、推定部123は、図5に示すように、第1特徴点FP1のオプティカルフローOF1の始点を頂点SP1、第1特徴点FP1のオプティカルフローOF1の終点を頂点EP1、第2特徴点FP1のオプティカルフローOF2の始点を頂点SP2、及び第2特徴点FP2のオプティカルフローOF2の終点を頂点EP2とする四角形QLを仮想的に形成する(ステップS6)。以降、説明のために四角形や、該四角形の辺などの幾何的要素を仮想的に形成して用いる。しかし、実際の処理では、特徴点の座標や直線の方向などのベクトル演算に基づき、同等の作用を持つ幾何的要素に基づかない処理としてもよい。
四角形QLが仮想的に形成されると、推定部123は、四角形QLと、記憶部13に記憶されているカメラ21の内部パラメータとを用いて、三次元空間におけるカメラ21の投影面IMG(図6参照)上に四角形QLを移動させ、投影面IMG上での四角形QL1を仮想的に生成する(ステップS7)。
なお説明のため、以下のように辺を定義する。四角形QL1の第1辺SD1は、四角形QLにおいて、頂点SP1および頂点SP2を結んだ辺に対応する。つまり、前フレームPにおける第1特徴点FP1と第2特徴点FP2を結んだ辺に相当する。同様に、四角形QL1の第2辺SD2は、四角形QLにおいて、頂点SP2および頂点EP2を結んだ辺に対応する。つまり、第2特徴点FP2のオプティカルフローOF2に相当する。同様に、四角形QL1の第3辺SD3は、四角形QLにおいて、頂点EP1および頂点EP2を結んだ辺に対応する。つまり、現フレームP’における第1特徴点FP1と第2特徴点FP2とを結んだ辺に相当する。同様に、四角形QL1の第4辺SD4は、四角形QLにおいて、頂点SP1および頂点EP1を結んだ辺に対応する。つまり、第1特徴点FP1のオプティカルフローOF1に相当する。
また、以下のように面を定義する(図6参照)。四角形QL1の第1辺SD1とカメラ21の光学中心OCとが含まれる面を第1面F1とする。同様に、四角形QL1の第2辺SD2とカメラ21の光学中心OCとが含まれる面を第2面F2とする。同様に、四角形QL1の第3辺SD3とカメラ21の光学中心OCとが含まれる面を第3面F3とする。同様に、四角形QL1の第4辺SD4とカメラ21の光学中心OCとが含まれる面を第4面F4とする。
次に、推定部123は、所定の平面との交線が互いに平行になる面の組を2組特定する(ステップS8)。所定の平面とはあらかじめ平面の法線が分かっている面である。具体的には車両が移動を行っている平面であり、つまり路面である。所定の平面は、厳密な平面でなくてもよく、推定部123が所定の平面との交線が互いに平行になる面の組を2組特定する際に平面とみなすことができるものであればよい。
本実施形態では、姿勢推定装置1は、自車両が直進している場合に、異なる時刻に撮影された2つの画像から特徴点を抽出し、該特徴点のオプティカルフローを算出する。また、該特徴点は所定の平面(本実施形態では路面)上に位置している静止物から抽出される。したがって、算出されるオプティカルフローは実世界上では、自車両に対する静止物の相対的な位置変化を表す。つまり向きが逆となった自車両の移動ベクトルである。
四角形QL1の第2辺SD2と第4辺SD4とは、共に特徴点のオプティカルフローに対応するので、共に実世界上では自車両の移動ベクトルに相当する。したがって、路面上では互いに平行となると想定される。
また、四角形QL1の第1辺SD1と第3辺SD3とは、共に特徴点同士の位置関係なので、実世界上では自車両の移動に伴う静止物同士の位置関係に相当する。移動前の位置関係が第1辺SD1に相当し、移動後の位置関係が第3辺SD3に相当する。このとき静止物の位置は変わらないため、移動前後で位置関係は変わらない。したがって、路面上ではこれも互いに平行となると想定される。
したがって、推定部123は、路面との交線が平行な面として、第2面F2と第4面F4との組と、第1面F1と第3面F3との組と、の2つの組を特定する。つまり、推定部123は、オプティカルフローを含む面同士を1つの組とし、同時刻に撮影された特徴点を含む面同士を他の組として、計2つの組を特定する。
なお、図6において四角形QL2は、現フレームP’の撮影時点での第1特徴点FP1の3次元空間(実世界)上の位置、現フレームP’の撮影時点での第2特徴点FP2の3次元空間上の位置、前フレームPの撮影時点での第1特徴点FP1の3次元空間上の位置、及び前フレームPの撮影時点での第2特徴点FP2の3次元空間上の位置を頂点とする四角形である。第1面F1は、四角形QL2の第1辺SD11を含む。同様に、第2面F2は四角形QL2の第2辺SD12を含み、第3面F3は四角形QL2の第3辺SD13を含み、第4面F4は四角形QL2の第4辺SD14を含む。このとき、上記のように四角形QL2は路面上に形成される平行四辺形であると想定される。
次に、推定部123は、路面の法線を算出する(ステップS9)。まず、推定部123は、先に特定した面の組の一方である第1面F1と第3面F3とに基づき、面同士の交線の方向を求める。詳細には、第1面F1と第3面F3との交線CL1の向きを求める(図7参照)。交線CL1の方向ベクトルV1は、第1面F1の法線ベクトル及び第3面F3の法線ベクトルそれぞれと垂直なベクトルである。したがって、推定部123は、第1面F1の法線ベクトルと第3面F3の法線ベクトルとの外積により、交線CL1の方向ベクトルV1を求める。第1面F1と第3面F3は、路面との交線が平行となるため、方向ベクトルV1は路面と平行になる。
同様に、推定部123は、先に特定した面の組の他方である第2面F2と第4面F4との交線の方向を求める。詳細には第2面F2と第4面F4との交線CL2の向きを求める(図8参照)。交線CL2の方向ベクトルV2は、第2面F2の法線ベクトル及び第4面F4の法線ベクトルそれぞれと垂直なベクトルである。したがって、推定部123は、第2面F2の法線ベクトルと第4面F4の法線ベクトルとの外積により、交線CL2の方向ベクトルV2を求める。第2面F2と第4面F4も同様に、路面との交線が平行となるため、方向ベクトルV2は路面と平行になる。
推定部123は、方向ベクトルV1と方向ベクトルV2との外積により、四角形QL2の面の法線、すなわち路面の法線を算出する。推定部123が算出した路面の法線はカメラ21のカメラ座標系で算出されるため、実際の路面の法線である垂直方向との違いから3次元空間の座標系を求め、路面に対するカメラ21の姿勢を推定することができる。その推定結果から推定部123は自車両に対するカメラ21の姿勢を推定する(ステップS10)。なお、ステップS9の算出処理は、例えば公知のARToolkitを利用して実行することができる。
ステップS10における姿勢推定が終了すると、推定部123は、ステップS5で選択した全てのオプティカルフローの組について、姿勢の推定が完了したか否かを確認する(ステップS11)。全ての組に対して姿勢の推定が完了している場合には(ステップS11でYes)、図2に示すフローが終了する。一方、全ての組に対して姿勢の推定が完了していない場合には(ステップS11でNo)、ステップS6に戻ってステップS6以降の処理が繰り返される。
なお、上記の説明ではカメラ21の光学中心OCと、四角形QL1の1つの辺を含む平面を特定するとしたがこの限りではない。当該平面の法線方向の特定をもって平面を特定するとしてもよい。例えば光学中心OCから各頂点への方向ベクトルの外積により平面の法線方向を求め、該法線方向によって面を特定するとしてもよい。この場合、第1面F1の法線方向と第3面F3の法線方向とを1つの組とし、第2面F2の法線方向と第4面F4の法線方向とを他の組として2つの組を特定するとよい。
また、面は平行移動させてもよい。例えば第1面F1の代わりに、第1面F1を平行移動させた面を第3面F3と組としてもよい。平行移動しても所定の平面との交線の向きは変わらないからである。
また、姿勢推定装置1は、複数の推定結果に基づいて姿勢の推定を確定させることが好ましい。この場合、所定数以上の姿勢の推定結果が得られるまで図2の処理を繰り返す構成としてよい。所定数以上の姿勢の推定結果が蓄積された時点で、例えば、平均値、又は、ヒストグラムの中央値やピーク値を求めて、当該求めた結果を姿勢推定の確定結果としてよい。姿勢推定の確定結果は、姿勢推定装置1における更なる処理に利用されてもよいし、外部装置に出力されてもよい。
姿勢推定の確定結果に基づいて、例えば、カメラ21の取付けのずれが生じた状態であるか否かが判定されてよい。カメラ21の取付けのずれが生じた状態であると判定された場合、例えば、当該異常事態が自車両のユーザに報知される。これにより、自車両のユーザはディーラによるカメラの取付け調整を依頼する等の対策をとることが可能となる。
また、カメラ21の姿勢推定の確定結果は、カメラ21のパラメータの補正に利用されてよい。これにより、カメラ21の取付けのずれが生じても、そのずれに応じたカメラのキャリブレーションが実行されることになり、カメラ21の取付けのずれが撮影画像に及ぼす悪影響を抑制することができる。また、姿勢の推定結果は、複数の車両とネットワークにより通信可能に設けられるセンターに撮影画像を送信する場合に、撮影画像とセットにしてセンターに送信されてもよい。これにより、センターでの撮影情報の取り扱いを適切に行うことができる。
<3.オプティカルフローの選択処理>
図9は、オプティカルフローの選択処理の一例を示すフローチャートである。図9は、図2のステップS5の処理の詳細例を示す図である。
ステップS51では、算出部122が、オプティカルフローの算出を行った複数のフレーム画像の中から処理の対象となるフレーム画像を選択する。本例では、算出部122は、複数のフレーム画像の中から順番に処理対象となるフレーム画像を選択する。処理対象のフレーム画像が選択されると、次のステップS52に処理が進められる。
ステップS52では、算出部122が、処理対象のフレーム画像において先に算出したオプティカルフローの中から、未処理のオプティカルフローを選択してオプティカルフローの長さを算出する。なお、オプティカルフローの長さは、特徴点の移動量に相当する。すなわち、ステップS52では、特徴点の移動量が算出される。また、未処理とは、オプティカルフローの長さの算出が未だ行われていないことを指す。移動量の算出が完了すると、ステップS53に処理が進められる。
ステップS53では、推定部123が、算出したオプティカルフローの長さ(移動量)が所定値以下であるか否かを確認する。オプティカルフローの長さが所定値以下である場合(ステップS53でYes)、次のステップS54に処理が進められる。オプティカルフローの長さが所定値を超える場合(ステップS53でNo)、ステップS55に処理が進められる。
ステップS54では、推定部123が、長さが所定値以下であるオプティカルフローを選択対象から除外する。除外処理が完了すると、次のステップS55に処理が進められる。
ステップS55では、推定部123が、現在の処理対象のフレーム画像における全てのオプティカルフローに対して処理(所定値との比較処理)が完了したか否かを確認する。全てのオプティカルフローに対して処理が完了している場合(ステップS55でYes)、ステップS56に処理が進められる。一方、全てのオプティカルフローに対して処理が完了していない場合(ステップS55でNo)、ステップS52に戻って、ステップS52以降の処理が繰り返される。
ステップS56では、推定部123が、現在の処理対象のフレーム画像において選択対象から除去されずに残ったオプティカルフローの中から、2つのオプティカルフローで構成される推定処理用のオプティカルフロー組を選択する。推定部123は、複数のオプティカルフロー組を選択してよい。推定部123は、組み合わせ可能な全てのオプティカルフロー組を選択してよい。オプティカルフロー組の選択が完了すると、次のステップS57に処理が進められる。
ステップS57では、推定部123は、オプティカルフローの算出を行った複数のフレーム画像の全てについて処理が完了したか否かを確認する。全フレーム画像について処理が完了している場合(ステップS57でYes)、図9に示すオプティカルフローの選択処理が終了される。全フレーム画像について処理が完了していない場合(ステップS57でNo)、ステップS51に戻ってステップS51以降の処理が繰り返される。
図10は、図9に示すオプティカルフローの選択処理の効果について説明するための図である。図10は、フロントカメラにおいて或るタイミングで取得された撮影画像Pである。図10では、ROI5内から5つの特徴点FP11、FP12、FP13、FP14、FP15が抽出されている。特徴点FP11と特徴点FP12とは、第1白線101の後方の2つのコーナーから抽出された特徴点である。特徴点FP13と特徴点FP14とは、第1白線101の後方にある第2白線102の前方の2つのコーナーから抽出された特徴点である。特徴点FP15は、自車両のボディBOのエッジと、第2白線102のエッジとが交差する箇所から抽出された特徴点である。
図10に示す例において、特徴点FP15は、自車両が前進しても同一の位置に現れる。すなわち、特徴点FP15から得られるオプティカルフローの長さは、ゼロ又はゼロに近い値になる。このために、特徴点FP15から算出されるオプティカルフローが姿勢の推定に使用された場合、誤った姿勢の推定が行われる可能性が高くなる。また、自車両が前進して、例えば特徴点FP14がROI5外となるような場合に、特徴点FP15が誤って特徴点FP14の追跡結果として捉えられることも起こり得る。このような場合、特徴点FP14から得られるオプティカルフローは本来の長さより短くなる。特徴点FP14から得られるオプティカルフローが姿勢の推定に使用された場合、誤った姿勢の推定が行われる可能性がある。なお、特徴点FP15のような特徴点は、例えば、自車影のエッジと、白線のエッジとが交差した場合にも生じることがあり、同様の現象を起こす。
本実施形態では、長さが極端に短い(ゼロを含む)オプティカルフローは姿勢推定処理の対象から除去されて用いられない構成となっている。このために、特徴点FP15のような特徴点が抽出される場合でも、当該特徴点を利用した姿勢の推定処理が行われることを避けることができ、適切に姿勢の推定を行うことができる。なお、ボディBOをROI5外とすることで、特徴点FP15のような特徴点の発生を抑制することができるが、カメラ21の姿勢が本来の位置からずれた場合に、特徴点FP15のような特徴点を抽出する可能性があり、本実施形態の構成は有効である。
<4.変形例>
以下、2つの変形例について説明する。変形例の説明に際して、上述の実施形態と重複する内容については、原則としてその説明を省略する。
(4-1.第1変形例)
図11は、第1変形例の特徴点の抽出について説明するための図である。図11に示すように、第1変形例においては、抽出部121は、撮影画像Pに複数設定される抽出領域(ROI)5から特徴点を抽出する。図11に示す例では、設定されるROI5の数は3つである。3つのROI5は、一例として左右方向に並ぶ。3つのROI5は、全て同じサイズでもよいが、少なくとも一つが他のROI5と異なるサイズであってよい。図11に示す例では、中央のROI5aがその左右に位置する2つのROI5b、5cより大きく、中央のROI5aの左右に位置する2つのROI5b、5cは互いに同じサイズである。複数のROI5の形状は、矩形状に限らず、その形状は適宜変更されてよい。
推定部123は、算出部122で算出された移動量が所定値以下となる特徴点が抽出されたROI5を除外対象領域とする。上述の実施形態では、移動量が所定値以下となる特徴点が除外対象とされた。これに対し、本変形例では、移動量が所定値以下となる特徴点が抽出されると、当該特徴点が抽出されたROI5内の全ての特徴点が除外対象となる。
そして、推定部123は、除外対象領域とされなかったROI5から抽出される特徴点の動きに基づきカメラ21の姿勢の推定を行う。言い換えると、推定部123は、除外対象領域以外のROI5から得られるオプティカルフローの少なくとも一部を用いてカメラ21の姿勢の推定を行う。例えば、図11に示す例において、中央のROI5aが除外対象領域とされ、その他のROI5b、5cが除外対象領域とされていない場合、推定部123は、ROI5b、5cから得られるオプティカルフローの少なくとも一部を用いてカメラ21の姿勢の推定を行う。
本変形例によれば、推定処理を行うに際して不適切なオプティカルフローを使用する可能性を更に低減することができ、カメラ21の姿勢の推定精度を更に向上することができる。
なお、推定部123は、ROI5の撮影画像上における設定位置に応じて、所定値を変動させてよい。例えば、図11に示す例において、中央のROI5aと、左右のROI5b、5cとで、異なる所定値を用いて移動量との比較が行われてよい。詳細には、中央のROI5aに設定される所定値は、左右のROI5b、5cに設定される所定値より大きく設定されてよい。これによれば、除外対象となる特徴点をカメラ21のレンズの歪み等を考慮して適切に選択することができる。
図12は、第1変形例におけるオプティカルフローの選択処理を示すフローチャートである。図12は、図9に示す処理の変形例である。
ステップS51~ステップS53は、上述の図9の場合と同様である。すなわち、オプティカルフローの算出を行った複数のフレーム画像の中から処理の対象となるフレーム画像が選択される(ステップS51)。そして、処理対象のフレーム画像において先に算出したオプティカルフローの中から、未処理のオプティカルフローが抽出されてオプティカルフローの長さ(移動量)が算出される(ステップS52)。算出されたオプティカルフローの長さが所定値と比較されて(ステップS53)、オプティカルフローの長さが所定値以下であればステップS54Aに処理が進められ、オプティカルフローの長さが所定値を超えればステップS55に処理が進められる。
ステップS54Aでは、長さが所定値以下であるオプティカルフローが得られたROI5が、除外対象領域に設定される。この点、オプティカルフローごとに除外対象か否かが判断される図9の場合と異なる。除外対象領域の設定が完了すると、次のステップS55に処理が進められる。
ステップS55では、図9の場合と同様に、現在の処理対象のフレーム画像における全てのオプティカルフローに対して処理(所定値との比較処理)が完了したか否かが確認される。そして、ステップS56Aに処理を進めるか、ステップS52に処理を戻すかが判断される。
ただし、本変形例では、除外対象領域に設定されたROI5から得られるオプティカルフローについては、除外対象領域が設定された時点で、自動的に処理済みのオプティカルフローとされる。これにより、不要な処理を省いて効率良く処理を行うことができる。
ステップS56Aでは、現在の処理対象のフレーム画像において除外対象領域とされなかったROI5(非除外対象ROI)から得られるオプティカルフローの中から、2つのオプティカルフローで構成される推定処理用のオプティカルフロー組が選択される。図9の場合と同様に、複数のオプティカルフロー組が選択されてよく、組み合わせ可能な全てのオプティカルフロー組が選択されてもよい。オプティカルフロー組の選択が完了すると、次のステップS57に処理が進められる。
ステップS57では、図9の場合と同様に、オプティカルフローの算出を行った複数のフレーム画像の全てについて処理が完了したか否かを確認される。全フレーム画像について処理が完了している場合には図12に示すが終了され、全フレーム画像について処理が完了していない場合にはステップS51に処理が戻される。
(4-2.第2変形例)
第2変形例では、第1変形例と同様に、抽出部121は、撮影画像に複数のROI5を設定して特徴点の抽出を行う。そして、第1変形例と同様に、除外対象とするオプティカルフローをROI単位で設定する。
ただし、本変形例は、次の点で第1変形例と異なる。本変形例では、除外対象領域と第1の所定画像数連続して判断されたROI5は、除外対象領域でないと第2の所定画像数連続して判断されるまで、除外対象領域として扱われる。換言すると、本変形例では、複数のフレーム画像にわたって連続して除外対象領域とされたROI5は、オプティカルフローを得る対象として適していない信頼性の低いROIと判断される。そして、信頼性が低いと判断されたROIは、複数のフレーム画像にわたって除外対象領域とされない場合に限り信頼性を回復できる。信頼性を回復できた場合に、初めて、当該ROIから得られるオプティカルフローが、カメラ21の姿勢の推定に利用可能となる。
本変形例の構成によれば、推定処理を行うに際して不適切なオプティカルフローを使用する可能性を更に低減することができ、カメラ21の姿勢の推定精度を更に向上することができる。
図13は、第2変形例におけるオプティカルフローの選択処理を示すフローチャートである。図13は、図12に示す変形例の処理の更なる変形例である。
ステップS51~ステップS54Aは、上述の図12の場合と同様である。すなわち、オプティカルフローの算出を行った複数のフレーム画像の中から処理の対象となるフレーム画像が選択される(ステップS51)。そして、処理対象のフレーム画像において先に算出したオプティカルフローの中から、未処理のオプティカルフローが抽出されてオプティカルフローの長さ(移動量)が算出される(ステップS52)。算出されたオプティカルフローの長さが所定値と比較されて(ステップS53)、オプティカルフローの長さが所定値以下であれば、長さが所定値以下であるオプティカルフローが得られたROIが除外対象領域に設定される(ステップS54A)。オプティカルフローの長さが所定値を超えれば、ステップS55に処理が進められる。
ステップS54Aの次に行われるステップS541では、除外対象領域に設定されたROI5が第1の所定画像数連続して除外対象領域に設定されたか否かが確認される。第1の所定画像数は、適宜決められてよい。ここで、第1の所定画像数をNとする。この場合、除外対象領域に設定されたROI5が、現在の処理対象のフレーム画像よりN-1回前のフレーム画像から現在のフレーム画像まで連続して除外対象領域に設定されている場合、当該ROIは、第1の所定画像数連続して除外対象領域に設定されたことになる。第1の所定画像数連続して除外対象領域に設定された場合(ステップS541でYes)、次のステップS542に処理が進められる。一方、第1の所定画像数連続して除外対象領域に設定されていない場合(ステップS541でNo)、ステップS55に処理が進められる。
ステップS542では、第1の所定画像数連続して除外対象領域に設定されたROI5が、特定ROIに指定される。特定ROIに指定されたROI5は、その指定を解除されるまで以降のフレーム画像においても特定ROIである。当該指定が完了すると、次のステップS55に処理が進められる。なお、ステップS542において、特定ROIに指定されるROI5の数は、単数のことも複数のこともある。
ステップS55では、図12の場合と同様に、現在の処理対象のフレーム画像における全てのオプティカルフローに対して処理(所定値との比較処理)が完了したか否かが確認される。本例でも、除外対象領域に設定されたROI5から得られるオプティカルフローについては、除外対象領域が設定された時点で、自動的に処理済みのオプティカルフローとされる。なお、以前のフレーム画像にて特定ROIに指定されているROI5であっても、現在のフレーム画像で除外対象領域に設定されていないROI5から得られるオプティカルフローについては、自動的に処理済みのオプティカルフローとされない。全てのオプティカルフローに対する処理が完了した場合(ステップS55でYes)、ステップS551に処理を進められ。全てのオプティカルフローに対する処理が完了していない場合(ステップS55でNo)、ステップS52に処理が戻される。
ステップS551では、現在の処理対象のフレーム画像において、特定ROIがあるか否かが確認される。特定ROIがある場合(ステップS551でYes)、ステップS552に処理が進められる。特定ROIがない場合(ステップS551でNo)、ステップS56Aに処理が進められる。
ステップS552では、特定ROIに指定されているROI5が、第2の所定画像数連続して除外対象領域に設定されていないか否かが確認される。第2の所定画像数は、適宜決められてよい。第2の所定画像数は、第1の所定画像数と同じでもよいし、異なってもよい。ここで、第2の所定画像数をMとする。この場合、特定ROIに指定されているROI5が、現在の処理対象のフレーム画像よりM-1回前のフレーム画像から現在のフレーム画像まで連続して除外対象領域に設定されていない場合、当該ROIは、第2の所定画像数連続して除外対象領域に設定されていないことになる。第2の所定画像数連続して除外対象領域に設定されていない場合(ステップS552でYes)、次のステップS553に処理が進められる。一方、第2の所定画像数連続して除外対象領域に設定されていないと判断されなかった場合(ステップS552でNo)、ステップS554に処理が進められる。
ステップS553では、特定ROIに指定されているROI5が、特定ROIの指定を解除される。すなわち、当該ROIは、除外対象領域でなくなる。指定解除が完了すると、次のステップS56Aに処理が進められる。
ステップS554では、特定ROIに指定されているROI5を除外対象領域に設定する。すなわち、特定ROIに指定されているROI5は、除外対象として扱われる。なお、ステップS54Aで除外対象領域に設定された特定ROIは、この時点で既に除外対象領域となっているが、ステップS54Aで除外対象領域に設定されていない特定ROIは、このステップS554で除外対象領域となる。除外対象領域の設定が完了すると、次のステップS56Aに処理が進められる。
ステップS56Aでは、図12の場合と同様に、現在の処理対象のフレーム画像において除外対象領域とされなかったROI5(非除外対象ROI)から得られるオプティカルフローの中から、2つのオプティカルフローで構成される推定処理用のオプティカルフロー組が選択される。オプティカルフロー組の選択が完了すると、次のステップS57に処理が進められる。
ステップS57では、図12の場合と同様に、オプティカルフローの算出を行った複数のフレーム画像の全てについて処理が完了したか否かを確認される。全フレーム画像について処理が完了している場合には図13に示すが終了され、全フレーム画像について処理が完了していない場合にはステップS51に処理が戻される。
<5.留意事項>
本明細書における実施形態や変形例の構成は、本発明の例示にすぎない。実施形態や変形例の構成は、本発明の技術的思想を超えない範囲で適宜変更されてよい。また、複数の実施形態及び変形例は、可能な範囲で組み合わせて実施されてよい。
本発明は、自動駐車等の移動体の運転支援を行うカメラの姿勢推定を行うために利用することができる。また、本発明は、ドライブレコーダ等の運転情報を記録するカメラの姿勢推定を行うために利用することができる。また、本発明は、カメラの姿勢の推定情報を利用して撮影画像を補正する補正装置等に利用することができる。
また、本発明は、複数の移動体とネットワークにより通信可能に設けられるセンターと連携して動作する装置等に利用することができる。当該装置は、例えば、センターに撮影画像を送信する場合に、カメラの姿勢の推定情報を撮影画像とセットにして送信する構成であってよい。そして、センターでは、カメラの姿勢の推定情報を用いて、各種画像処理(カメラの姿勢も考慮した画像の視点・視方向を変更する処理、例えば車両の車体前方方向の画像に視点・視方向変換した画像を生成する等)、画像を用いた計測処理におけるカメラの姿勢に対する補正処理、カメラ姿勢の経年変化の統計処理(多くの車両のデータ)等を行い、ユーザへの有用な提供データを生成する等を行う。