特許 6989276 位置計測装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6989276

(24)【登録日】2021年12月6日

(45)【発行日】2022年1月5日

(54)【発明の名称】位置計測装置

(51)【国際特許分類】

   G01B 11/245 20060101AFI20211220BHJP

   G06T 7/55 20170101ALI20211220BHJP

【ＦＩ】

   G01B11/245 H

   G06T7/55

【請求項の数】7

【全頁数】15

(21)【出願番号】特願2017-75014(P2017-75014)

(22)【出願日】2017年4月5日

(65)【公開番号】特開2018-179577(P2018-179577A)

(43)【公開日】2018年11月15日

【審査請求日】2020年3月18日

(73)【特許権者】

【識別番号】000004695

【氏名又は名称】株式会社ＳＯＫＥＮ

(73)【特許権者】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(73)【特許権者】

【識別番号】304021277

【氏名又は名称】国立大学法人 名古屋工業大学

(74)【代理人】

【識別番号】110000578

【氏名又は名称】名古屋国際特許業務法人

(72)【発明者】

【氏名】石丸 和寿

(72)【発明者】

【氏名】白井 孝昌

(72)【発明者】

【氏名】坂上 文彦

(72)【発明者】

【氏名】佐藤 淳

【審査官】 櫻井 仁

(56)【参考文献】

【文献】 特開２０１７−０３７０５３（ＪＰ，Ａ）

【文献】 特開２００５−３２２１２８（ＪＰ，Ａ）

【文献】 特開昭６３−２３８５１０（ＪＰ，Ａ）

【文献】 特開２０１６−２２５９５３（ＪＰ，Ａ）

【文献】 特開２０１７−２１５２５５（ＪＰ，Ａ）

【文献】 米国特許出願公開第２０１７／０３６３４１６（ＵＳ，Ａ１）

【文献】 米国特許出願公開第２０１４／０２４０５２０（ＵＳ，Ａ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｂ １１／００−１１／３０

Ｇ０１Ｃ ３／０６

Ｇ０６Ｔ ７／００− ７／９０

(57)【特許請求の範囲】

【請求項1】

車両に搭載された位置計測装置であって、
複数の異なる時刻にて、互いに異なる視点から同一の撮影領域を含むように同時に撮影された複数組の画像を取得するように構成された画像取得部（２１：Ｓ１１０）と、
前記異なる時刻毎に、各組の画像のそれぞれから、同一の３次元位置を表す画像平面上の点である対応点を検出するように構成された対応検出部（２１：Ｓ１２０）と、
前記異なる時刻毎に、各組の画像についての各画像平面とワールド座標系の奥行きが異なる位置に設定される複数の共通平面との全ての組み合わせについて、前記画像平面から前記共通平面への非線形写像を得るための対応関係をカメラパラメータとして、前記異なる時刻毎に前記対応検出部にて検出された前記対応点のそれぞれを、予め設定されたカメラパラメータを用いて、前記複数の共通平面のそれぞれに投影した投影点を求めるように構成された投影点算出部（２１：Ｓ１５０）と、
一つの前記画像平面上の前記対応点から求められる前記複数の共通平面上の各投影点を結ぶ線を光線として、前記異なる時刻毎に前記複数の画像平面のそれぞれについて求められる複数の光線に対する距離が最小となる点を、前記対応点の３次元位置を表す復元点として求めるように構成された復元点算出部（２１：Ｓ１６０〜Ｓ１８０）と、
を備える位置計測装置（１）。

【請求項2】

請求項１に記載の位置計測装置であって、
前記カメラパラメータを校正前のカメラパラメータとして、該校正前のカメラパラメータおよび前記復元点算出部にて求められる復元点に対してバンドル調整を行い、前記校正前のカメラパラメータを該バンドル調整によって得られる校正された後のカメラパラメータに更新するように構成された校正部（２１：Ｓ２１０）、
をさらに備える位置計測装置。

【請求項3】

複数の異なる時刻にて、互いに異なる視点から同一の撮影領域を含むように同時に撮影された複数組の画像を取得するように構成された画像取得部（２１：Ｓ１１０）と、
前記異なる時刻毎に、各組の画像のそれぞれから、同一の３次元位置を表す画像平面上の点である対応点を検出するように構成された対応検出部（２１：Ｓ１２０）と、
前記異なる時刻毎に、各組の画像についての各画像平面とワールド座標系の奥行きが異なる位置に設定される複数の共通平面との全ての組み合わせについて、前記画像平面から前記共通平面への非線形写像を得るための対応関係をカメラパラメータとして、前記異なる時刻毎に前記対応検出部にて検出された前記対応点のそれぞれを、予め設定されたカメラパラメータを用いて、前記複数の共通平面のそれぞれに投影した投影点を求めるように構成された投影点算出部（２１：Ｓ１５０）と、
一つの前記画像平面上の前記対応点から求められる前記複数の共通平面上の各投影点を結ぶ線を光線として、前記異なる時刻毎に前記複数の画像平面のそれぞれについて求められる複数の光線に対する距離が最小となる点を、前記対応点の３次元位置を表す復元点として求めるように構成された復元点算出部（２１：Ｓ１６０〜Ｓ１８０）と、
前記カメラパラメータを校正前のカメラパラメータとして、該校正前のカメラパラメータおよび前記復元点算出部にて求められる復元点に対してバンドル調整を行い、前記校正前のカメラパラメータを該バンドル調整によって得られる校正された後のカメラパラメータに更新するように構成された校正部（２１：Ｓ２１０）、
を備える位置計測装置。

【請求項4】

請求項２又は請求項３に記載の位置計測装置であって、
前記校正部は、前記バンドル調整に用いる評価関数として、校正前の前記復元点に関する投影点と、該投影点を通る前記光線に沿って、校正後の前記復元点を前記共通平面に投影した再投影点との距離を、前記異なる時刻において得られた全ての投影点について積分した値を用いる
ように構成された位置計測装置。

【請求項5】

請求項２から請求項４のいずれか１項に記載の位置計測装置であって、
前記校正部は、前記バンドル調整に用いる評価関数として、前記複数の光線に対する距離を示す距離項と、時刻の遷移に伴うカメラパラメータの変動を示すパラメータ項とをコストとして表現したコスト関数を用いる
ように構成された位置計測装置。

【請求項6】

請求項５に記載の位置計測装置であって、
前記校正部は、前記パラメータ項として、高次の項および低次の項を備え、前記高次の項に予め設定された係数を乗じ、前記提示の項には前記係数よりも重みづけが小さく設定された他の係数を乗じた値を用いる
ように構成された位置計測装置。

【請求項7】

請求項１〜請求項６のいずれか１項に記載の位置計測装置であって、
前記復元点から前記画像上の各点の３次元距離を求めるように構成された距離算出部（２１：Ｓ２２０）、
をさらに備える位置計測装置。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、複数のカメラから得られる画像を用いて物体の３次元位置を計測する位置計測装置に関する。

【背景技術】

【0002】

複数のカメラから得られる画像を用いて物体の３次元位置を計測する位置計測装置が知られている。この種の位置計測装置は、基本的には、カメラに入射する光がレンズ中心に収束するカメラモデルである、いわゆる単焦点カメラモデルに基づいている。

【0003】

しかし、単焦点カメラモデルは、例えば、車載カメラ等を用いてフロントガラス越しに撮影を行う等、カメラと被写体との間に光を屈曲させる物体が存在する場合に、計測精度が大幅に低下する。

【0004】

これに対して、例えば特許文献１には、線形写像を記述したモデルである単焦点カメラモデルの代わりに、非線形写像を記述したモデルである単焦点カメラモデルを用いることが提案されている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２０１２−７５０６０号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

ところで、単焦点カメラモデルでは、カメラ毎に、計測対象となる物体が存在する空間を挟み込むように配置された２枚の平面を仮定し、これらの両平面に対する非線形写像を表す内部パラメータを定義すると共に、カメラ間の位置関係を表す外部パラメータを定義する。そして、内部パラメータによって取得画像上の一点から両平面への投影点を求め、その投影点間を結ぶ線を逆投影光線とし、各カメラの取得画像上に映った同一点についての逆投影光線をカメラ毎に求める。さらに、外部パラメータを用いて逆投影光線の位置関係を調整して逆投影光線の交点を求め、この交点を復元点としている。

【0007】

このように、従来の単焦点カメラモデルでは、内部パラメータと外部パラメータとの両方を定義する必要があり、カメラを校正する際には、両パラメータを連動させて調整する必要があるため、その作業が非常に煩雑であるという問題があった。

【0008】

また、特許文献１に記載の装置では、カメラの校正に特殊な機材が必要となるため、車載カメラ等として実際の使用中に、振動などによってカメラの位置関係が変化しても、その変化に動的に対応することができないという問題もあった。

【0009】

本開示は、カメラの校正が容易な位置計測装置を提供する。

【課題を解決するための手段】

【0010】

本開示の位置計測装置は、画像取得部（２１：Ｓ１１０）と、対応検出部（２１：Ｓ１２０）と、投影点算出部（２１：Ｓ１５０）と、復元点算出部（２１：Ｓ１６０〜Ｓ１８０）と、を備える。

【0011】

画像取得部は、複数の異なる時刻にて、互いに異なる視点から同一の撮影領域を含むように同時に撮影された複数組の画像を取得するように構成される。対応検出部は、異なる時刻毎に、各組の画像のそれぞれから、同一の３次元位置を表す画像平面上の点である対応点を検出するように構成される。

【0012】

投影点算出部は、異なる時刻毎に対応検出部にて検出された対応点のそれぞれを、予め設定されたカメラパラメータを用いて、複数の共通平面のそれぞれに投影した投影点を求めるように構成される。なお、カメラパラメータは、異なる時刻毎に、各組の画像についての各画像平面とワールド座標系の奥行きが異なる位置に設定される複数の共通平面との全ての組み合わせについて、画像平面から共通平面への非線形写像を得るための対応関係を示す。

【0013】

復元点算出部は、一つの画像平面上の対応点から求められる複数の共通平面上の各投影点を結ぶ線を光線として、異なる時刻毎に複数の画像平面のそれぞれについて求められる複数の光線に対する距離が最小となる点を、対応点の３次元位置を表す復元点として求めるように構成される。

【0014】

このような構成によれば、投影用の平面をカメラ毎に設定する従来の単焦点カメラモデルとは異なり、カメラ間の関係を記述する外部パラメータが不要となるため、復元点の算出に必要なパラメータを削減することができる。さらには、カメラパラメータとして、従来技術の内部パラメータに相当するものだけを扱えばよいため、カメラパラメータの校正も簡略化することができ、その結果、復元点の算出精度も向上させることができる。

【0015】

また、複数の時刻での画像を用いて復元点を求めるので、単一の時刻での画像を用いて復元点を求める構成と比較して、より復元点の算出精度を向上させることができる。
なお、この欄及び特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。

【図面の簡単な説明】

【0016】

【図1】位置計測装置の構成を示すブロック図である。

【図2】非単焦点カメラモデルに関する説明図である。

【図3】カメラパラメータの初期値の設定や、実験で使用する計測環境を示す説明図である。

【図4】テストパターンを例示する説明図である。

【図5】距離演算処理のフローチャートである。

【図6】復元点や再投影点に関する説明図である。

【図7】位置計測に利用する撮影画像の組数である時刻数Ｔと復元点の平均二乗誤差との関係の実験結果を示すグラフである。

【図8】正則化項の係数αと復元点の平均二乗誤差の大きさとの関係の実験結果を示すグラフである。

【発明を実施するための形態】

【0017】

以下、図面を参照しながら、発明を実施するための形態を説明する。
［１．構成］
図１に示す位置計測装置１は、複数の撮影画像を用いて撮影画像中の各点までの３次元距離を検出する装置である。

【0018】

位置計測装置１は、乗用車等の車両に搭載され、画像取得部１０と処理部２０とを備える。また、位置計測装置１は、車載ネットワーク３を介して、図示しない他の車載装置と接続されている。

【0019】

画像取得部１０は、カメラアレイを構成する複数のカメラを備える。カメラアレイとは、カメラを格子状に並べて配置したものであり、車載カメラ等で用いられる２台のカメラを水平方向に並べて配置した平行ステレオカメラもその一種として挙げられる。以下では、平行ステレオカメラを構成する１対のカメラ１１，１２を備えるものとして説明する。なお、カメラの数は２つに限定されるものではなく、３つ以上であってもよい。各カメラ１１，１２は、フロントガラスを介して車両の進行方向に位置する同一の撮影領域を含む領域を撮影するように車室内に配置される。つまり、画像取得部１０は、互いに異なる視点から同一の撮影領域を含むように同時に撮影された複数の画像を取得して、処理部２０に供給するように構成されている。

【0020】

処理部２０は、ＣＰＵ２１と、ＲＡＭ、ＲＯＭ、フラッシュメモリ等の半導体メモリ（以下、メモリ２２）と、を有する周知のマイクロコンピュータを中心に構成される。処理部２０の各種機能は、ＣＰＵ２１が非遷移的実体的記録媒体に格納されたプログラムを実行することにより実現される。この例では、メモリ２２が、プログラムを格納した非遷移的実体的記録媒体に該当する。また、このプログラムの実行により、プログラムに対応する方法が実行される。なお、非遷移的実体的記録媒体とは、記録媒体のうちの電磁波を除く意味である。また、処理部２０を構成するマイクロコンピュータの数は１つでも複数でもよい。

【0021】

処理部２０は、ＣＰＵ２１がプログラムを実行することで実現される機能として、距離演算処理を少なくとも実現する。距離演算処理の詳細については後述する。なお、処理部２０が機能を実現する手法はソフトウェアに限るものではなく、その一部又は全部の要素を、論理回路やアナログ回路等を組み合わせたハードウェアを用いて実現してもよい。

【0022】

［２．カメラモデル］
ここで、距離演算処理の前提となる非単焦点カメラモデルについて説明する。非単焦点カメラモデルは、カメラ前方に配置されたガラス等で光線が屈折する状況下においても、屈折後の光線の経路を正確に記述するためのモデルである。なお、カメラモデルは、カメラが撮影を行った時刻毎に個別に設定される。ここでは１つの時刻におけるカメラモデルについて説明する。

【0023】

このカメラモデルでは、図２に示すように、複数のカメラによる３次元点の２次元画像平面への投影を２枚の平面を用いて記述する。なお、図２では、カメラが３台である場合を示す。ただし、カメラは３台に限るものではなく２台以上であればよい。投影用の平面として、復元対象となる空間を挟み込むように配置された２枚の共通平面Ｈ_１，Ｈ_２を仮定し、各カメラの画像平面Ｇ_ｎから各共通平面Ｈ_ｊへの非線形写像をそれぞれ定義する。ただし、ｎ＝１，２，３、ｊ＝１，２である。

【0024】

つまり、従来の単焦点カメラモデルでは、投影用の２枚の平面をカメラ毎に個別に定義していたが、ここでは、各カメラに共通の２枚の共通平面Ｈ_１，Ｈ_２を定義している点が大きく異なる。この共通平面Ｈ_１，Ｈ_２を用いることにより、カメラ間の関係を規定する外部パラメータが不要となる。

【0025】

このような２枚の共通平面Ｈ_１，Ｈ_２として、ここでは、ワールド座標系において、奥行き方向を表すＺ軸方向の座標が、Ｚ＝Ｚ_１となる平面を共通平面Ｈ_１と、Ｚ＝Ｚ_２となる平面を共通平面Ｈ_２とする。そして、共通平面Ｈ_ｊ上の点Ｘ_ｊを（１）式で表すものとする。ただし、ｘ_1jは、ワールド座標系におけるＸ軸方向の座標（以下、水平座標）、ｘ_2jは、同じくＹ軸方向の座標（以下、垂直座標）を表すものとする。

【0026】

【数1】

【0027】

カメラの画像平面Ｇ_ｎ上の点である画像点Ｍから共通平面Ｈ_１，Ｈ_２への各投影点Ｘ_１，Ｘ_２への変換においては、固定値であるＺ_ｊを無視して考えることができる。ここでは、画像平面Ｇ_ｎ上の画像点Ｍ＝（ｍ_１，ｍ_２）から各共通平面Ｈ_ｊ上の水平座標ｘ_1jまたは垂直座標ｘ_2jへの非線形写像を、（２）式に示すように、Ｋ次多項式を用いて定義する。ただし、ｍ_１は画像平面Ｇ_ｎ上の水平座標、ｍ_２は画像平面Ｇ_ｎ上の垂直座標である。ａ_klは、変換に用いる係数であり、これがカメラモデルを構成する個々のカメラパラメータとなる。ただし、ａ_klは、共通平面Ｈ_１の水平座標ｘ₁₁および垂直座標ｘ₂₁、共通平面Ｈ_２の水平座標ｘ₁₂および垂直座標ｘ₂₂のそれぞれについて個別に定義される。

【0028】

【数2】

【0029】

なお、この変換は、Ｋ次多項式による非線形変換と平面射影変換を組み合わせたものである。特にＫ＝１の場合、上記変換は、平面射影変換と等しくなる。このような非線形Ｋ次多項式に平面射影変換を組み合わせることで、カメラの回転等を効果的に表現することができる。

【0030】

なお、カメラパラメータａ_klの初期値は、実験等によって予め求められた値が設定され、以後、距離演算処理を実行する毎に更新される。初期値を求める際には、図３に示すように、フロントガラス等の非線形歪要因に対して実際の使用時と同様の位置関係となるようにカメラ１１，１２を配置し、共通平面Ｈ_１，Ｈ_２に対応する位置に配置されたテストパターンＰを、非線形歪要因越しに撮影する。なお、テストパターンＰとしては、例えば、図４に示すように、格子状のパターンが用いられる。そして、撮影結果から、各カメラ１１，１２の画像平面Ｇ_ｎ上の対応点を検出し、その画像平面Ｇ_ｎ上での対応点の位置、すなわち（ｍ_１，ｍ_２）と、既知の値である共通平面Ｈ_１，Ｈ_２に配置されたテストパターン上での対応点の実際の位置、すなわち（ｘ₁₁，ｘ₂₁）および（ｘ₁₂，ｘ₂₂）との関係から、上記（２）式を用いて、カメラパラメータａ_klを求める。

【0031】

ここで、Ｋ＝２の場合の変換式を行列形式にて表現したものを（３）式に示す。ただし、この式は、共通平面Ｈ_１，Ｈ_２のいずれか一方への変換式である。ここででは、式を見易くするため、共通平面Ｈ_ｊを特定するパラメータであるｊの表記を省略している。また、水平座標Ｘ_１への変換に用いるカメラパラメータをａ_klで表し、垂直座標Ｘ_２への変換に用いるカメラパラメータをｂ_klで表している。

【0032】

【数3】

【0033】

［３．距離演算処理］
次に、処理部２０のＣＰＵ２１が実行する距離演算処理を、図５のフローチャートを用いて説明する。本処理は、予め設定された一定周期で、繰り返し実行される。

【0034】

なお、メモリ２２には、距離演算処理のプログラムの他、事前に実験的に求められたカメラパラメータａ_klの初期値が少なくとも記憶されている。ただし、カメラパラメータａ_klは、カメラ毎にｘ₁₁，ｘ₂₁，ｘ₁₂，ｘ₂₂を求めるため４組ずつ必要であり、二つのカメラ１１，１２を用いる本実施形態では合計８組のカメラパラメータが用意される。ただし、（３）式の形式で考えた場合は、カメラ毎に、（ｘ_1j，ｘ_2j）を求めるためのカメラパラメータが２組ずつ必要であり、カメラが二つの場合は、合計４組のカメラパラメータが用意されることになる。

【0035】

本処理が起動すると、処理部２０は、まずＳ１１０にて、画像取得部１０を構成するカメラ１１，１２から、それぞれ同じタイミングで撮影した画像（以下、撮影画像）を取得する。この際、処理部２０は、撮影画像をメモリ２２に格納するとともに、メモリ２２に過去に格納された撮像画像を取得する。本処理では、例えば、直前の７時刻分の撮影画像をメモリ２２から取得し、最新の撮影画像を含めて合計８時刻分の撮影画像を用いて処理を行う。

【0036】

Ｓ１２０では、各時刻、各カメラ１１，１２の撮影画像から、同一の３次元位置を表す点であると推定される対応点を抽出する。なお、対応点の抽出には、例えば、各点における画像特徴量を求め、その特徴量が類似する点を対応点として抽出する等、公知の方法を用いることができる。以下では、抽出された対応点の数をＷ個とする。Ｗは正整数を表す。

【0037】

Ｓ１３０では、Ｓ１２０で抽出された対応点の一つを、復元対象点として選択する。
Ｓ１４０では、各時刻において、各カメラ１１，１２から得られた撮影画像のうち、いずれか一つを対象画像として選択する。

【0038】

Ｓ１５０では、メモリ２２に記憶されたカメラパラメータａ_klを用いて、（２）式に従い、図６に示すように、対象画像の画像平面Ｇ_ｎ上の復元対象点Ｍ＝（ｍ_１，ｍ_２）から、各共通平面Ｈ_ｊへの投影点Ｘ_ｊ＝（ｘ_1j，ｘ_2j）をそれぞれ算出する。

【0039】

Ｓ１６０では、Ｓ１５０で求められた各共通平面Ｈ_１，Ｈ_２上の二つの投影点の３次元座標Ｘ_１＝（ｘ₁₁，ｘ₂₁，Ｚ_１）、Ｘ_２＝（ｘ₁₂，ｘ₂₂，Ｚ_２）から、両投影点Ｘ_１，Ｘ_２間を結ぶ３次元直線である逆投影光線Ｌを算出する。

【0040】

Ｓ１７０では、Ｓ１３０にて選択された復元対象点に対するＳ１４０〜Ｓ１６０の処理を、全てのカメラ１１，１２からの撮影画像について実施したか否かを判断する。全て処理済みであればＳ１８０に進み、未処理のものがあれば、Ｓ１４０に戻って、Ｓ１４０〜Ｓ１５０の処理を繰り返す。

【0041】

Ｓ１８０では、一つの復元対象点について、カメラ毎に算出された合計Ｎ本の逆投影光線Ｌを用いて、復元対象点Ｍの３次元位置を表す復元点ＲＸを算出する。３次元位置は、測定誤差がなければ、Ｎ本の逆投影光線Ｌの交点に位置する。しかし、実際には測定誤差により交点が存在しない場合がある。従って、（４）式により、Ｎ本の逆投影光線Ｌとの２乗距離の和が最小となる３次元点を復元点ＲＸとして求める。

【0042】

【数4】

【0043】

なお、図６に示すように、ｎ番目のカメラの画像平面Ｇ_ｎから共通平面Ｈ_ｊへの投影点をＸ_jn、投影点Ｘ_jnを通る逆投影光線Ｌ_ｎの向きを表す単位ベクトルである光線ベクトルをＢ_ｎとする。このとき３次元空間中の任意の復元候補点Ｘ_ｒと光線Ｌとの距離を計算するため、復元候補点Ｘ_ｒを光線Ｌ_ｎに投影することで得られるＬＸ_ｎを（５）式により計算する。また、光線ベクトルＢ_ｎは（６）式で表される。そして、（４）式に示すような全てのカメラから得られる光線Ｌ_ｎとの距離を最小とする復元候補点Ｘ_ｒが、最終的に復元される３次元点である復元点ＲＸとなる。

【0044】

【数5】

【0045】

Ｓ１９０では、Ｓ１２０で抽出した全ての対応点について、Ｓ１３０〜Ｓ１８０の処理を実施したか否かを判断する。全ての対応点について処理を実施済みであればＳ２００に進み、未処理の対応点があればＳ１３０に戻って、Ｓ１３０〜Ｓ１８０の処理を繰り返す。

【0046】

Ｓ２００では、Ｓ１８０で算出されたＷ個の復元点を復元点群｛ＲＸ｝、復元点群｛ＲＸ｝の算出に用いた８組のカメラパラメータをカメラパラメータ群｛Ａ｝で表すものとして、これら復元点群｛ＲＸ｝およびカメラパラメータ群｛Ａ｝の校正を行う。これと共に、Ｓ２１０で、校正されたカメラパラメータ群｛Ａ｝により、メモリ２２に記憶されたカメラパラメータを更新する。

【0047】

具体的には、まず、Ｗ個の復元点のそれぞれについて、再投影誤差Ｅ_ｗを算出する。ただし、ｗ＝１，２，…，Ｗである。再投影誤差Ｅ_ｗは、図６に示すように、ｗ番目の復元点ＲＸを、その算出に用いた光線ベクトルＢ_ｎに沿って各共通平面Ｈ_ｊに投影した点である再投影点Ｒ_jnを求め、（７）式を用いて算出する。つまり、時刻Ｔ毎、カメラ毎、かつ共通平面毎に求めた投影点Ｘ_jntと再投影点Ｒ_jntとの距離の総和を、再投影誤差Ｅ_ｗとしている。再投影誤差Ｅ_ｗは、投影点と再投影点との距離を、異なる時刻において得られた全ての投影点について積分した値であり、複数の光線に対する２乗距離を示す距離項である。

【0048】

【数6】

【0049】

また、バンドル調整では、（８）式に示すように、カメラパラメータａ_klが時系列に従って変化することを制限するパラメータ項Ｒ_ｗも考慮する。

【0050】

【数7】

【0051】

カメラパラメータａ_klには、上記（３）式に示すように、ｍの次元が２次以上となる高次の項、およびｍの次元が１または０となる低次の項が含まれる。（８）式での係数αは、カメラパラメータａ_klの全ての項に同じ値を乗じる係数ではなく、高次の項に予め設定された係数を乗じ、低次の項には係数よりも重みづけが小さく設定された他の係数を乗じた値を用いるように設定される。特に、本実施形態では、例えば、高次の項にはα＝１００００を採用し、低次の項にはα＝０を採用する。

【0052】

ここで、フロントガラス等の屈折媒体による非線形な歪みによるカメラパラメータの変化は高次の項に現れやすく、カメラの運動（移動や回転）によるカメラパラメータの変化は低次の項に現れやすい傾向がある。このため、本実施形態では、屈折媒体による非線形な歪みをより考慮し、また、カメラの運動による影響を軽減するために、高次の項に乗じる係数に対して、低次の項には重みづけが小さな係数を乗じる。

【0053】

次に、（９）式に示すように、復元点群｛ＲＸ｝に属する全ての復元点についての再投影誤差Ｅ_ｗの総和と、カメラパラメータの変化の大きさを示すパラメータ項Ｒ_ｗとの和であるコストを最小化するように、復元点群｛ＲＸ｝およびカメラパラメータ群｛Ａ｝の値を調整する、いわゆるバンドル調整を行う。つまり、バンドル調整では、校正後のカメラパラメータ群｛Ａ｝を用いて校正後のコストを逐次求めることが行われるが、その具体的な手法は公知であるため、ここでは説明を省略する。

【0054】

【数8】

【0055】

Ｓ２２０では、Ｓ２１０で求められた校正された復元点群｛ＲＸ｝を用いて、画像中の各種物体までの３次元距離を表す距離情報を生成する。さらに、この距離情報を、車載ネットワーク３を介して各車載装置に提供して本処理を終了する。

【0056】

［４．実験１］
上述の位置計測装置１を用いて、３次元距離の計測を行った結果について説明する。この実験では、処理に利用する異なる時刻における撮影画像の組数、つまり、撮影画像の時刻数を変化させて、真の３次元点に対する平均二乗誤差を計測した。

【0057】

図７に示すように、撮影画像の時刻数が増加するにつれて、誤差は減少に向かう傾向があることが分かる。特に、時刻数が１の場合よりも、時刻数が複数であれば、誤差が大幅に減少することが分かる。

【0058】

［５．実験２］
（８）式にて示す正則化項の係数αを変更させて、真の３次元点に対する平均二乗誤差を計測した。なお、本実験では、低次の項に対する係数はα＝０とし、高次の項に対する係数を変動させる。図８に示すように、高次の項の係数をα＝０、つまり、正則化項が存在しない場合の誤差と比較して、高次の項のみに対する重みを大きくし、α＝１０００以上とすると、誤差が小さくなることが分かる。

【0059】

この結果から、運動するカメラにおいて連続する時刻ではカメラパラメータの高次の項が大きく変化しないという仮定が有効であることが示された。よって、カメラパラメータの高次の項の変動に対してコストを設定する上記の処理は意義があると言える。

【0060】

ただし、αの値を大きくしすぎると、相対的に再投影誤差Ｅ_ｗのウエイトが小さくなり、復元点の校正がされにくくなるため、αの値は誤差が小さくなる効果が得られる程度に小さいほうが好ましい。例えば、α＝１０００−５００００程度の範囲内であるとよい。

【0061】

［６．効果］
以上詳述した本実施形態によれば、以下の効果が得られる。
（６ａ）上記実施形態の位置計測装置１において、処理部２０は、Ｓ１１０で、複数の異なる時刻にて、互いに異なる視点から同一の撮影領域を含むように同時に撮影された複数組の画像を取得する。また、処理部２０は、Ｓ１２０で、異なる時刻毎に、各組の画像のそれぞれから、同一の３次元位置を表す画像平面上の点である対応点を検出する。

【0062】

また、処理部２０は、Ｓ１５０で、異なる時刻毎に検出された対応点のそれぞれを、予め設定されたカメラパラメータを用いて、複数の共通平面のそれぞれに投影した投影点を求める。なお、カメラパラメータは、異なる時刻毎に、各組の画像についての各画像平面とワールド座標系の奥行きが異なる位置に設定される複数の共通平面との全ての組み合わせについて、画像平面から共通平面への非線形写像を得るための対応関係を示す。

【0063】

また、処理部２０は、Ｓ１６０〜Ｓ１８０で、一つの画像平面上の対応点から求められる複数の共通平面上の各投影点を結ぶ線を光線として、異なる時刻毎に複数の画像平面のそれぞれについて求められる複数の光線に対する２乗距離が最小となる点を、対応点の３次元位置を表す復元点として求める。

【0064】

このような構成によれば、投影用の平面をカメラ毎に設定する従来の単焦点カメラモデルとは異なり、カメラ間の関係を記述する外部パラメータが不要となるため、復元点の算出に必要なパラメータを削減することができる。さらには、カメラパラメータとして、従来技術の内部パラメータに相当するものだけを扱えばよいため、カメラパラメータの校正も簡略化することができ、その結果、復元点の算出精度も向上させることができる。

【0065】

また、複数の時刻での画像を用いて復元点を求めるので、単一の時刻での画像を用いて復元点を求める構成と比較して、より復元点の算出精度も向上させることができる。なお、２乗距離に限らず、絶対値距離等の任意の距離が最小となる点を求めてもよい。

【0066】

（６ｂ）本実施形態では、カメラモデルとして、非単焦点カメラモデルを利用している。
このような構成によれば、撮影画像中に光線の屈折の影響が含まれる状況下においても、精度よく３次元距離計測を実現することができる。

【0067】

（６ｃ）本実施形態では、非単焦点カメラモデルにおいて、復元空間を挟むように配置され、画像平面上の点を投影するために設定される二つの平面として、カメラ毎に異なる平面を定義するのではなく、全てのカメラに共通の平面Ｈ_１，Ｈ_２を定義している。

【0068】

このような構成によれば、全てのカメラの状態を、外部パラメータを用いることなく記述することができ、外部パラメータが必要な従来の単焦点カメラモデルと比較して、カメラパラメータの数を削減することができる。

【0069】

（６ｄ）上記実施形態において、処理部２０は、Ｓ２１０で、カメラパラメータを校正前のカメラパラメータとして、該校正前のカメラパラメータおよび復元点に対してバンドル調整を行い、校正前のカメラパラメータを該バンドル調整によって得られる校正された後のカメラパラメータに更新する。

【0070】

本実施形態では、再投影誤差Ｅ_ｗが（７）式のように一意に定義されているため、復元点群｛ＲＸ｝およびカメラパラメータ群｛Ａ｝の校正にバンドル調整を適用することができる。つまり、復元点群｛ＲＸ｝およびカメラパラメータ群｛Ａ｝の校正を同時に実現することができる。このため、例えば、振動等でカメラの位置姿勢が変動したような場合でも、そのような変動を自動的に補正しながら３次元位置計測、ひいては３次元距離計測を継続して行うことができる。

【0071】

（６ｅ）上記実施形態において、処理部２０は、Ｓ２１０で、バンドル調整に用いる評価関数として、校正前の復元点に関する投影点と、該投影点を通る光線に沿って、校正後の復元点を共通平面に投影した再投影点との距離を、異なる時刻において得られた全ての投影点について積分した値を用いる。

【0072】

このような構成によれば、評価関数として投影点と再投影点との距離を異なる時刻において得られた全ての投影点について積分した値を用いるので、異なる時刻において得られた全ての投影点を考慮して復元点を最適化することができる。

【0073】

（６ｆ）上記実施形態において、処理部２０は、Ｓ２１０で、バンドル調整に用いる評価関数として、複数の光線に対する２乗距離を示す距離項と、時刻の遷移に伴うカメラパラメータの変動を示すパラメータ項とをコストとして表現したコスト関数を用いる。

【0074】

このような構成によれば、評価関数としてコスト関数を用いるので、コストを最小化する距離項およびパラメータ項を探索する簡素な処理で復元点およびカメラパラメータを求めることができる。

【0075】

（６ｇ）上記実施形態において、処理部２０は、Ｓ２１０で、パラメータ項として、高次の項および低次の項を備え、高次の項に予め設定された係数を乗じ、提示の項には係数よりも重みづけが小さく設定された他の係数を乗じた値を用いるように構成される。

【0076】

ここで、フロントガラス等の屈折媒体による非線形な歪みは高次の項に現れやすく、カメラの運動（移動や回転）による変動は低次の項に現れやすい傾向があることが分かった。このため、本実施形態では、屈折媒体による非線形な歪みをより考慮するために、高次の項に乗じる係数に対して、低次の項には重みづけが小さな係数を乗じている。

【0077】

このような構成によれば、カメラの運動による影響を軽減しつつ非線形な歪みの変化を考慮して校正を行う構成とすることができる。
（６ｈ）上記実施形態において、処理部２０は、Ｓ２２０で、復元点から画像上の各点の３次元距離を求めるように構成される。

【0078】

このような構成によれば、復元点から画像上の各点の３次元距離を求めることができる。
［７．他の実施形態］
以上、本発明を実施するための形態について説明したが、本発明は上述の実施形態に限定されることなく、種々変形して実施することができる。

【0079】

（７ａ）上記実施形態では、カメラパラメータとして、（２）（３）式で定義されるもの、すなわち、Ｋ次多項式による非線形変換と平面射影変換を組み合わせたものを用いたが、これに限定されるものではない。

【0080】

（７ｂ）上記実施形態では、バンドル調整に用いる評価関数を（９）式で定義したが、例えば、下記の（１０）式を用いてもよい。

【0081】

【数9】

【0082】

（１０）式は、未知点でのコストを示す項、基底点でのコストを示す項、および前述の正則化項のコストを示す項で構成される。
未知点は、SHIFT法等によって撮影画像で対応する点の画像座標、つまり対応点が分かっている点であって３次元位置が未知な点を示す。基底点は、未知点に対してレーザレーダ等を組み合わせることで３次元位置が既知となった点を示す。基底点は、多数存在し、かつ基底点同士の奥行きが異なっていることが望ましい。

【0083】

未知点でのコストは、復元点の位置が不明であるため、予め準備された規定値を用いるか、或いは状況に応じた近似値を用いるなど、任意の手法で求められる。基底点でのコストは上記実施形態と同様の手法が採用できる。

【0084】

（７ｃ）上記実施形態における１つの構成要素が有する複数の機能を、複数の構成要素によって実現したり、１つの構成要素が有する１つの機能を、複数の構成要素によって実現したりしてもよい。また、複数の構成要素が有する複数の機能を、１つの構成要素によって実現したり、複数の構成要素によって実現される１つの機能を、１つの構成要素によって実現したりしてもよい。また、上記実施形態の構成の一部を省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加又は置換してもよい。なお、特許請求の範囲に記載の文言から特定される技術思想に含まれるあらゆる態様が本発明の実施形態である。

【0085】

（７ｄ）本開示は、上述した位置計測装置の他、当該位置計測装置を構成要素とするシステム、当該位置計測装置としてコンピュータを機能させるためのプログラム、このプログラムを記録した半導体メモリ等の非遷移的実態的記録媒体、位置計測方法など、種々の形態で本発明を実現することもできる。

【0086】

［８．実施形態の構成と本開示との対応関係］
上記実施形態において処理部２０のＣＰＵ２１が実行する処理のうちのＳ１１０の処理は本開示でいう画像取得部に相当し、Ｓ１２０の処理は本開示でいう対応検出部に相当する。また、上記実施形態においてＳ１５０の処理は本開示でいう投影点算出部に相当する。また、上記実施形態においてＳ１６０〜Ｓ１８０の処理は本開示でいう復元点算出部に相当し、Ｓ２１０の処理は本開示でいう校正部に相当する。また、上記実施形態においてＳ２２０の処理は本開示でいう距離算出部に相当する。

【符号の説明】

【0087】

１…位置計測装置、３…車載ネットワーク、１０…画像取得部、１１，１２…カメラ、２０…処理部、２１…ＣＰＵ、２２…メモリ。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】