JP2020201586A - 撮像装置、車両及びプログラム - Google Patents

Abstract

【課題】改善された、撮像装置、車両及びプログラムを提供する。【解決手段】撮像装置は、相対的な位置姿勢が定められた左カメラ及び右カメラと、制御部とを備える。制御部は、位置姿勢が未推定である画像等に基づいて、該未推定である画像に対応する位置姿勢を推定し、該対応する特徴点を復元して復元点を取得する追加の推定処理をする。制御部は、追加の推定処理を繰り返し実行した後、バンドル調整により該推定済みの位置姿勢を最適化し、最適化後の位置姿勢に基づいて、左カメラと右カメラとの間の相対的な位置姿勢の推定処理をする。制御部は、追加の推定処理において、新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の位置と、該復元済みの特徴点により復元された復元点の該画像への再投影点の位置との間の距離が閾値以上である場合、位置姿勢の推定処理において、該復元点を、該画像に対応する復元点として扱わない。【選択図】図６

Description

本開示は、撮像装置、車両及びプログラムに関する。

近年、先進運転支援システム（ＡＤＡＳ；Advance Driving Assistant System）のセンサの１つとして、ステレオカメラが利用されている。このようなステレオカメラをキャリブレーションする方法が知られている（例えば、非特許文献１）。

洞山 慶太、他２名、"２時刻間のカメラ運動推定を伴うステレオセルフキャリブレーション"、第24回画像センシングシンポジウム（SSII2018）、pp.ＳＯ１−ＩＳ１−２９−１−３、２０１８年６月

従来のステレオカメラには、改善の余地がある。

かかる点に鑑みてなされた本開示の目的は、改善された、撮像装置、車両及びプログラムを提供することにある。

本開示の一実施形態に係る撮像装置は、
相対的な位置姿勢が定められた左カメラ及び右カメラと、制御部とを備え、
前記制御部は、
移動中の前記左カメラ及び前記右カメラが時系列的に生成した複数の画像から特徴点を抽出し、前記複数の画像において対応する特徴点を特定し、
前記複数の画像の中の２つの画像に含まれる前記特徴点の一部に基づいて前記２つの画像に対応する位置姿勢を推定し、推定した前記位置姿勢に基づいて前記２つの画像に含まれる前記特徴点を復元して復元点を取得する１回目の推定処理をし、
位置姿勢が未推定である前記画像と、該未推定である画像に含まれる前記復元した特徴点に対応する特徴点の少なくとも一部とに基づいて、該未推定である画像に対応する位置姿勢を推定し、推定した位置姿勢に基づいて該対応する特徴点を復元して復元点を取得する追加の推定処理をし、
前記追加の推定処理を繰り返し実行した後、前記複数の画像の少なくとも一部の画像に対応する推定済みの位置姿勢と、該少なくとも一部の画像に対応する前記復元点とに基づいて、バンドル調整により該推定済みの位置姿勢を最適化し、最適化後の位置姿勢に基づいて、前記左カメラと右カメラとの間の相対的な位置姿勢の推定処理をし、
前記追加の推定処理において、新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の位置と、該復元済みの特徴点により復元された復元点の該画像への再投影点の位置との間の距離が閾値以上である場合、該復元点を、前記位置姿勢の推定処理において、該画像に対応する復元点として扱わない。

本開示の一実施形態に係る車両は、上述の撮像装置を備える。

上述したように本開示の解決手段を装置として説明してきたが、本開示は、これらを含む態様としても実現し得るものであり、また、これらに実質的に相当する方法、プログラム、プログラムを記録した記憶媒体としても実現し得るものであり、本開示の範囲にはこれらも包含されるものと理解されたい。

本開示の一実施形態に係るプログラムは、
移動中の相対的な位置姿勢が定められた左カメラ及び右カメラが時系列的に生成した複数の画像から特徴点を抽出し、前記複数の画像において対応する特徴点を特定するステップと、
前記複数の画像の中の２つの画像に含まれる前記特徴点の一部に基づいて前記２つの画像に対応する位置姿勢を推定し、推定した前記位置姿勢に基づいて前記２つの画像に含まれる前記特徴点を復元して復元点を取得する１回目の推定処理をするステップと、
位置姿勢が未推定である前記画像と、該未推定である画像に含まれる前記復元した特徴点に対応する特徴点の少なくとも一部とに基づいて、該未推定である画像に対応する位置姿勢を推定し、推定した位置姿勢に基づいて該対応する特徴点を復元して復元点を取得する追加の推定処理をするステップと、
前記追加の推定処理をするステップを繰り返し実行した後、前記複数の画像の少なくとも一部の画像に対応する推定済みの位置姿勢と、該少なくとも一部の画像に対応する前記復元点とに基づいて、バンドル調整により該推定済みの位置姿勢を最適化し、最適化後の位置姿勢に基づいて、前記左カメラと右カメラとの間の相対的な位置姿勢の推定処理をするステップと、
前記追加の推定処理をするステップにおいて、新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の位置と、該復元済みの特徴点により復元された復元点の該画像への再投影点の位置との間の距離が閾値以上である場合、該復元点を、前記位置姿勢の推定処理において、該画像に対応する復元点として扱わないステップと、を装置に実行させる。

本開示の一実施形態によれば、改善された、撮像装置、車両及びプログラムが提供され得る。

本開示の第１実施形態に係る車両の機能ブロック図である。 本開示の第１実施形態に係る特徴点の一例を示す図である。 本開示の第１実施形態に係る対応する特徴点の一例を示す図である。 本開示の第１実施形態に係る撮像装置の処理の流れを示すフローチャートである。 本開示の第２実施形態に係る被写体の一例を示す図である。 本開示の第２実施形態に係る対応する特徴点の一例を示す図である。 本開示の第２実施形態に係る画像に対応する復元点の一例を示す図である。 本開示の第２実施形態に係る撮像装置の処理の流れを示すフローチャートである（その１）。 本開示の第２実施形態に係る撮像装置の処理の流れを示すフローチャートである（その２）。

ステレオカメラは、相対的な位置姿勢が互いに定められた、複数のカメラを含む。本開示において「カメラの位置姿勢」とは、ワールド座標系において、基準となる位置に対するカメラの位置、及び、基準となる方向に対するカメラの傾き（姿勢）を意味する。カメラの位置姿勢は、外部パラメータともいう。本開示において「ワールド座標系」とは、ステレオカメラの外部の三次元空間に基づいて設定された座標系を意味する。

ステレオカメラは、相対的な位置姿勢が互いに定められた複数のカメラにより、被写体を異なる方向から、同時に撮像可能である。つまり、ステレオカメラは、被写体を異なる方向から同時に撮像して複数の画像を生成可能である。この複数の画像から求めた視差と、複数のカメラ間の相対的な位置姿勢とを用いた、三角測量の原理を利用することにより、被写体までの方向及び距離が測定され得る。

ステレオカメラを先進運転支援システムのセンサの１つとして用いる場合、ステレオカメラは、車両に搭載される。ステレオカメラを車両に搭載させる場合、ステレオカメラに含まれる複数のカメラ間の相対的な位置姿勢は、車両の走行時の振動、温度及び経年劣化により、ずれる場合がある。そのため、ステレオカメラを車両に搭載させる場合、ステレオカメラを定期的にキャリブレーションすることが求められる。本開示において「キャリブレーション」とは、ステレオカメラに含まれる複数のカメラ間の実際の相対的な位置姿勢を推定することである。ステレオカメラに含まれる複数のカメラ間の実際の相対的な位置姿勢を推定することにより、推定した位置姿勢に基づいて、複数のカメラが生成した複数の画像から求めた視差を補正することができる。

ここで、ステレオカメラをキャリブレーションする方法の一例として、オフラインキャリブレーション及びセルフキャリブレーション等が挙げられる。

オフラインキャリブレーションでは、チャッカーボード等の専用器具を用いて、複数のカメラ間の相対的な位置姿勢が推定される。オフラインモードでは専用器具を用いる必要があるため、オフラインキャリブレーションを車両に搭載されたステレオカメラに対して実行する場合、その際の作業は煩雑となり得る。

これに対し、セルフキャリブレーションでは、移動中にステレオカメラが生成した複数の画像を用いて、複数のカメラ間の相対的な位置姿勢が推定される。つまり、ステレオカメラを車両に搭載させる場合、セルフキャリブレーションでは、車両の走行中にステレオカメラが生成した複数の画像を、移動中にステレオカメラが生成した複数の画像として用い、複数のカメラ間の相対的な位置姿勢を推定することができる。そのため、セルフキャリブレーションは、車両に搭載されたステレオカメラに対して、比較的容易に実行され得る。

本開示では、ステレオカメラに含まれる複数のカメラ間の相対的な位置姿勢を、より精度良く推定可能な、セルフキャリブレーションを実行する撮像装置及び車両を提供する。複数のカメラ間の相対的な位置姿勢をより精度良く推定可能なセルフキャリブレーションを実行する撮像装置及び車両を提供することにより、本開示は、被写体までの方向及び距離をより精度良く測定することができる。従って、本開示によれば、改善された、撮像装置及び車両が提供され得る。以下、本開示の一実施形態について、図面を参照して説明する。

（第１実施形態）
図１に示すように、車両１は、電子機器２と、撮像装置３とを備える。車両１は、限定ではないが、自動車、鉄道車両、産業車両又は生活車両等であってよい。

電子機器２は、車両１に搭載される機器である。電子機器２は、ＥＣＵ（Electronic Control Unit）であってよい。電子機器２は、撮像装置３と通信可能である。電子機器２は、撮像装置３から、他車両及び歩行者等の被写体までの方向及び距離を取得する。電子機器２は、取得した被写体までの方向及び距離に基づいて、ブレーキを制御する信号等を生成してよい。

撮像装置３は、車両１の外部の被写体を撮像可能な位置に設置され得る。撮像装置３は、限定ではないが、車両１の前方の周辺領域を撮像可能な位置に搭載されてよい。撮像装置３は、先進運転支援システムのセンサの１つとして、車両１に搭載されてよい。撮像装置３は、電子機器２と通信可能である。

撮像装置３は、左カメラ１０と、右カメラ１１と、記憶部１２と、通信部１３と、制御部１４とを備える。以下、左カメラ１０と右カメラ１１とを特に区別しない場合、左カメラ１０及び右カメラ１１は、単に「カメラ」とも記載する。撮像装置３は、２台のカメラを備える。ただし、撮像装置３は、３台以上のカメラを備えてよい。

左カメラ１０は、車両１の前方に向かって左側に位置する。右カメラ１１は、車両１の前方に向かって右側に位置する。左カメラ１０及び右カメラ１１の各々は、制御部１４の制御に基づいて、被写体を撮像して画像を生成する。左カメラ１０及び右カメラ１１の各々は、生成した画像を、制御部１４に出力する。左カメラ１０及び右カメラ１１の各々は、光学系及び撮像素子を含んで構成されてよい。光学系は、レンズ等を含んで構成され得る。光学系は、被写体の像を結像可能である。撮像素子は、ＣＣＤ（Charge Coupled Device）撮像素子又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）撮像素子等を含んでよい。撮像素子は、光学系が結像した像に相当する撮像画像（画像）を出力する。

左カメラ１０及び右カメラ１１は、例えば互いに固定されることにより、相対的な位置姿勢が互いに定められている。本実施形態では、左カメラ１０の位置姿勢［Ｒ_Ｌ｜ｔ_Ｌ］を基準として、右カメラ１１の位置姿勢［Ｒ_Ｒ｜ｔ_Ｒ］が定められている。Ｒ_Ｌ及びＲ_Ｒの各々は、ワールド座標系における左カメラ１０及び右カメラ１１の各々の姿勢である。ｔ_Ｌ及びｔ_Ｒの各々は、ワールド座標系における左カメラ１０及び右カメラ１１の各々の位置である。例えば、右カメラ１１の位置姿勢［Ｒ_Ｒ｜ｔ_Ｒ］は、左カメラ１０の位置姿勢［Ｒ_Ｌ｜ｔ_Ｌ］を基準として、式（１）によって定められる。
Ｒ_Ｌα＝Ｒ_Ｒ
ｔ_Ｌβ＝ｔ_Ｒ 式（１）

式（１）において、変換式αは、左カメラ１０から右カメラ１１への回転の変換式である。変換式βは、左カメラ１０から右カメラ１１への並進の変換式である。本実施形態では、回転の変換式αと並進の変換式βとによって、左カメラ１０と右カメラ１１との間の互いに定められた相対的な位置姿勢が示される。変換式α及び変換式βの初期値は、撮像装置３の設計時又は撮像装置３の製造時におけるオフラインキャリブレーション時に、与えられ得る。本実施形態では、左カメラ１０と右カメラ１１との間の相対的な位置関係として、変換式α及び変換式βを用いることにより、被写体までの方向及び距離が測定され得る。

左カメラ１０と右カメラ１１との間の相対的な位置姿勢すなわち式（１）の変換式α及び変換式βは、車両１の走行時の振動、温度及び経年劣化により、初期値から、ずれる場合がある。左カメラ１０と右カメラ１１との間の相対的な位置姿勢がずれると、被写体までの方向及び距離を精度良く測定できなくなる。本実施形態に係る撮像装置３は、後述のセルフキャリブレーションを実行することにより、左カメラ１０及び右カメラ１１の現在の位置姿勢を推定して、新たな変換式α及び変換式βを取得する。本実施形態に係る撮像装置３は、新たな変換式α及び変換式βを取得することにより、被写体までの方向及び距離を精度良く測定することができる。

記憶部１２は、例えば、ＲＡＭ（Random Access Memory）及びＲＯＭ（Read Only Memory）等、任意の記憶デバイスを含む。記憶部１２は、制御部１４を機能させる多様なプログラム及び制御部１４が用いる多様な情報を記憶する。

記憶部１２は、式（１）の変換式α及び変換式βを記憶する。記憶部１２は、左カメラ１０及び右カメラ１１の内部パラメータを記憶する。本開示において「カメラの内部パラメータ」は、カメラの焦点距離、及び、カメラが生成した画像における二次元座標（以下、「画像座標系」ともいう）の画像中心を含んでよい。本実施形態では、左カメラ１０及び右カメラ１１の内部パラメータは、固定値である。

通信部１３は、有線又は無線により、電子機器２と通信可能である。通信部１３は、無線により、サーバ等の外部機器と通信可能であってよい。通信部１３は、物理コネクタ及び無線通信モジュールを含んで構成されてよい。物理コネクタは、電気信号による伝送に対応した電気コネクタ、光信号による伝送に対応した光コネクタ、及び、電磁波による伝送に対応した電磁コネクタを含んでよい。無線通信モジュールは、Ｂｌｕｅｔｏｏｔｈ（登録商標）及びＩＥＥＥ８０２．１１を含む各規格に準拠するものであってよい。無線通信モジュールは、少なくとも１つのアンテナを含み得る。

制御部１４は、一以上のプロセッサ及びメモリを含む。プロセッサは、特定のプログラムを読み込ませて特定の機能を実行する汎用のプロセッサ、及び、特定の処理に特化した専用のプロセッサを含んでよい。専用のプロセッサは、特定用途向けＩＣ（ＡＳＩＣ；Application Specific Integrated Circuit）を含んでよい。プロセッサは、プログラマブルロジックデバイス（ＰＬＤ；Programmable Logic Device）を含んでよい。ＰＬＤは、ＦＰＧＡ（Field-Programmable Gate Array）を含んでよい。制御部１４は、１つ又は複数のプロセッサが協働するＳｏＣ（System-on-a-Chip）及びＳｉＰ（System In a Package）の何れかであってもよい。

制御部１４は、通信部１３によって外部機器又は電子機器２から、被写体までの方向及び距離の測定指令を取得する。制御部１４は、測定指令を取得すると、左カメラ１０及び右カメラ１１に、被写体の画像を同時に生成させる。制御部１４は、これらの画像から被写体の視差及び画像座標系における被写体の位置を取得する。制御部１４は、取得した視差及び画像座標系における被写体の位置と、式（１）の変換式α，βと、カメラの外部パラメータと、後述の内部パラメーラとに基づいて、被写体までの方向及び距離を測定する。制御部１４は、測定した被写体までの方向及び距離を、通信部１３によって外部機器又は電子機器２に、出力する。

［セルフキャリブレーション］
制御部１４は、通信部１３によって外部機器又は電子機器２から、キャリブレーション指令を取得し得る。キャリブレーション指令は、外部機器又は電子機器２から、撮像装置３へ例えば定期的に送信され得る。なお、制御部１４は、キャリブレーション指令を取得する代わりに、自身の機能により定期的にキャリブレーションを実行してよい。本実施形態では、制御部１４は、ＳｆＭ（Structure from Motion）を利用して、セルフキャリブレーションを実行する。ＳｆＭとは、複数の画像を用いて、後述の復元点の三次元座標及びカメラの位置姿勢を推定する手法である。

制御部１４は、キャリブレーション指令を取得すると、通信部１３によって電子機器２から、車両１の移動情報を取得する。車両１の移動情報は、電子機器２が測定した車両１の速度等を含んでよい。制御部１４は、車両１の移動情報に基づいて、車両１が移動中であるか否か判定する。例えば、制御部１４は、電子機器２から取得した車両１の速度が、所定値を超えると判定するとき、車両１が移動中であると判定する。又は、制御部１４は、車両１の移動情報の代わりに、車両１が移動中であることを示す情報を、通信部１３によって電子機器２から取得してよい。車両１が移動中であることを示す情報は、電子機器２が測定した車両１の速度に基づいて電子機器２のＥＣＵが判断した車両１が移動中であることを示す情報である。

制御部１４は、車両１が移動中であると判定すると、左カメラ１０及び右カメラ１１に、複数の画像を時系列的に生成させる。本実施形態では、制御部１４は、左カメラ１０及び右カメラ１１に、所定フレーム数の画像を、複数の画像として生成させる。制御部１４は、左カメラ１０及び右カメラ１１に、被写体までの方向及び距離の測定時と同一又は異なる所定フレームレート（ｆｐｓ；frames per second）で画像を生成させてよい。制御部１４は、左カメラ１０及び右カメラ１１が生成した各画像を識別する番号を付加する。

制御部１４は、移動中の左カメラ１０及び右カメラ１１が時系列的に生成した複数の画像から、特徴点を抽出する。本開示において「特徴点」とは、画像に写された被写体の部分のうち、隣接する他の部分から際立っており、例えば画素値の比較等により抽出可能な部分を意味する。特徴点の一例として、画像に写された被写体の輪郭上の一点、及び、画像において画素値が極大又は極小となる点が挙げられる。制御部１４は、特徴点を抽出する任意のアルゴリズムを用いてよい。特徴点を抽出するアルゴリズムの一例として、ＡＫＡＺＥ（Accelerated KAZE）等が挙げられる。

図２に、本開示の第１実施形態に係る特徴点の一例を示す。第１画像２０は、カメラによって生成された画像である。第１画像２０には、被写体として、車両１が走行する道路、及び、住宅が写っている。制御部１４は、第１画像２０に写された住宅の輪郭上の一点を、特徴点２１，２２，２３として抽出する。

制御部１４は、複数の画像において、対応する特徴点を特定する。本開示において「対応する特徴点」とは、異なる画像間において同一の対象点を示す特徴点を意味する。本開示において「対象点」とは、三次元空間上の被写体のうち、特徴点として画像から抽出された部分を意味する。制御部１４は、抽出した特徴点に対して任意のマッチング処理を実行することにより、対応する特徴点を特定してよい。マッチング処理の一例として、ブルート・フォーズ（brute-force）等が挙げられる。

図３に、本開示の第１実施形態に係る対応する特徴点の一例を示す。第１画像２０、第２画像３０、第３画像４０及び第４画像５０は、各々異なる位置姿勢のカメラによって生成された複数の画像の一部である。図３では、第１画像２０、第２画像３０、第３画像４０及び第４画像５０は、説明の簡易化のために、位置姿勢が大きく異なるカメラによって生成された画像として描画されている。ただし、これらの画像は、位置姿勢が実際には微小に異なるカメラによって生成された画像であってよい。カメラの位置姿勢は、例えば、移動中において異なる時刻にカメラが画像を生成することにより異なり得る。また、カメラの位置姿勢は、例えば、画像を生成させるカメラが左カメラ１０又は右カメラ１１であるかによって異なり得る。

例えば、第１画像２０では、三次元空間上の対象点６１，６２，６３が、各々特徴点２１，２２，２３として抽出される。また、第２画像３０では、三次元空間上の対象点６１，６２，６４が、各々特徴点３１，３２，３４として抽出される。また、第３画像４０では、三次元空間上の対象点６１，６２，６３，６４が、各々特徴点４１，４２，４３，４４として抽出される。また、第４画像５０では、三次元空間上の対象点６２，６４が、各々特徴点５２，５４として抽出される。なお、図３には、便宜上、第１画像２０、第２画像３０、第３画像４０及び第４画像５０から抽出される特徴点の一部のみを示す。

制御部１４は、対応する特徴点として、異なる画像間で同一の対象点６１を示す特徴点２１，３１，４１を特定する。また、制御部１４は、対応する特徴点として、異なる画像間で同一の対象点６２を示す特徴点２２，３２，４２を特定する。また、制御部１４は、対応する特徴点として、異なる画像間で同一の対象点６３を示す特徴点２３，４３を特定する。また、制御部１４は、対応する特徴点として、異なる画像間で同一の対象点６４を示す特徴点３４，４４，５４を特定する。

ここで、上述のように、カメラは、移動中である。そのため、生成された時刻の差が大きい画像同士には、異なる物体が写っている可能性が高い。異なる物体が写っている画像から抽出した特徴点に対してマッチング処理を実行すると、対応する特徴点を精度良く特定できない場合がある。そこで、制御部１４は、対応する特徴点を特定する際、所定時間内に生成された画像からすなわちフレームの間隔が所定間隔となる画像から抽出した特徴点に対して、マッチング処理を実行してよい。このような処理により、対応する特徴点が精度良く特定され得る。所定時間は、車両１の速度等を考慮して適宜設定されてよい。

制御部１４は、対応を特定した特徴点から、任意のアルゴリズムを用いて、外れ値を除去してよい。外れ値を除去するアルゴリズムの一例として、８点アルゴリズムを利用したＲＡＮＳＡＣ（Random Sampling Consensus）等が挙げられる。

＜１回目の推定処理＞
制御部１４は、複数の画像の中から、２つの画像を選択する。制御部１４は、２つの画像として、左カメラ１０及び右カメラ１１が同時刻に生成した画像を選択してよい。又は、制御部１４は、複数の画像の中から、対応する特徴点を最も多く含む２つの画像を選択してよい。例えば、制御部１４は、２つの画像として、第１画像２０及び第２画像３０を選択する。

制御部１４は、選択した２つの画像に対応する位置姿勢を推定する。本開示において「画像に対応する位置姿勢」とは、当該画像を生成したカメラの、当該画像を生成したときの位置姿勢を意味する。制御部１４は、２つの画像に含まれる対応する特徴点の一部に基づいて、画像に対応する位置姿勢を推定してよい。例えば、制御部１４は、２つの画像に含まれる対応する特徴点に対して、５点アルゴリズム及びＲＡＮＳＡＣを実行することにより、２つの画像の各々に対応する位置姿勢を推定してよい。例えば、制御部１４は、１回目の推定処理では、第１画像２０に対応する位置姿勢と、第２画像３０に対応する位置姿勢を推定する。

制御部１４は、推定した位置姿勢に基づいて、２つの画像に含まれる対応する特徴点に三次元空間にて対応する点を、復元点とみなして復元する。制御部１４は、復元点のワールド座標系における三次元座標を取得する。なお、画像に含まれるノイズ等の影響により、復元点のワールド座標系における三次元座標と、当該復元点に対応する対象点のワールド座標系における三次元座標との間には、ずれが生じる場合がある。制御部１４は、推定した位置姿勢（すなわち外部パラメータ）と、カメラの内部パラメータとを用いた、三角測量の原理を利用することにより、２つの画像に含まれる対応する特徴点を復元してよい。

例えば、図３に示すように、制御部１４は、第１画像２０に対応する位置姿勢と、第２画像３０に対応する位置姿勢とに基づいて、特徴点２１，３１を復元することにより、復元点７１のワールド座標系における三次元座標を取得する。制御部１４は、第１画像２０に対応する位置姿勢と、第２画像３０に対応する位置姿勢とに基づいて、特徴点２２，３３を復元することにより、復元点７２のワールド座標系における三次元座標を取得する。

制御部１４は、復元した特徴点に基づいて、推定した位置姿勢をバンドル調整により最適化する。バンドル調整とは、復元点のワールド座標系の三次元座標と、当該復元点に対応する対象点のワールド座標系の三次元座標との間のずれが最小化されるように、推定した位置姿勢及び復元点の三次元座標の少なくとも何れかを調整して最適化する処理である。上述のように、例えば画像に含まれるノイズ等の影響により、復元点のワールド座標系における三次元座標と、当該復元点に対応する対象点のワールド座標系における三次元座標との間には、ずれが生じ得る。例えば、図３に示すように、復元点７１と対象点６１との間にずれが生じ、復元点７２と対象点６２との間にずれが生じている。バンドル調整では、このようなずれが最小化されるように、推定した位置姿勢及び復元点の三次元座標の少なくとも何れかを調整して最適化する。本実施形態のバンドル調整では、復元点を画像の画像座標系に再投影させて得られる点（以下、「再投影点」ともいう）の二次元座標と、当該復元点に対応する特徴点の二次元座標との間のずれが最小化されるように、推定した位置姿勢等を調整する。例えば、制御部１４は、再投影点の二次元座標と特徴点の二次元座標との誤差を示すコスト関数Ｅが最小化されるように、推定した位置姿勢及び復元点の三次元座標の少なくとも何れかを調整して最適化する。コスト関数Ｅは、式（２）によって表される。

式（２）において、ｉは、画像を識別する番号である。ｎは、バンドル調整に用いられる画像の枚数である。ｊは、特徴点を識別する番号である。ｍは、バンドル調整に用いられる特徴点の数である。ｘ^ｉ _ｊは、ｉ番の画像から抽出したｊ番の特徴点の二次元座標である。Ｘ_ｊは、特徴点ｘ_ｊに対応する復元点の三次元座標である。Ｐ_ｉは、ｉ番の画像の射影行列（Projection matrix）である。つまり、Ｐ_ｉＸ_ｊは、復元点Ｘ_ｊを番号ｉで識別される画像の画像座標系に再投影させて得られる再投影点である。式（２）における射影行列Ｐは、以下の式（３）によって表される。

ここで、式（３）のＡ及び［Ｒ｜ｔ］は、以下のように表される。

式（３）において、行列Ａは、カメラの内部パラメータ行列である。ｆ_ｘ，ｆ_ｙは、カメラの焦点距離である。ｃ_ｘ，ｃ_ｙは、カメラの画像座標系の中心座標である。上述のように、左カメラ１０及び右カメラ１１の内部パラメータは、固定値である。そのため、行列Ａは、固定値となる。行列［Ｒ｜ｔ］は、カメラの外部パラメータ行列である。つまり、行列［Ｒ｜ｔ］は、推定された位置姿勢である。

制御部１４は、式（２）の復元点Ｘ_ｊの値及び射影行列Ｐ_ｉの値を調整することにより、式（２）のコスト関数Ｅを最小化させるように調整してよい。この場合、上述のように行列Ａは固定値であるため、制御部１４は、式（２）の復元点Ｘ_ｊの値と、射影行列Ｐ_ｉの一成分である式（３）の行列［Ｒ｜ｔ］の値とを調整することにより、式（２）のコスト関数Ｅを最小化させるように調整する。又は、制御部１４は、式（２）の復元点Ｘ_ｊを固定し、射影行列Ｐ_ｉの一成分である式（３）の行列［Ｒ｜ｔ］の値を調整することにより、式（２）のコスト関数Ｅを最小化させるように調整してよい。

＜追加の推定処理＞
制御部１４は、複数の画像の中から、位置姿勢が未推定である画像を１つ選択する。制御部１４は、復元済みの特徴点に対応する特徴点を最も多く含む画像を優先して選択してよい。

例えば、１回目の推定処理が終了した直後では、第３画像４０及び第４画像５０に対応する位置姿勢は、未推定である。第３画像４０は、復元済みの特徴点２１，３１に対応する特徴点４１と、復元済みの特徴点２２，３２に対応する特徴点４２とを含む。つまり、第３画像４０は、復元済みの特徴点に対応する特徴点を２つ含む。これに対し、第４画像５０は、復元済みの特徴点２２，３２に対応する特徴点５２を含む。つまり、第４画像５０は、復元済みの特徴点に対応する特徴点を１つ含む。この場合、制御部１４は、第３画像４０及び第４画像５０のうち、復元済みの特徴点に対応する特徴点を最も多く含む画像として、第３画像４０を優先して選択する。

制御部１４は、選択した位置姿勢が未推定である画像と、当該画像に含まれる特徴点の少なくとも一部とに基づいて、当該画像に対応する位置姿勢を推定する。制御部１４は、当該画像に含まれる特徴点の少なくとも一部として、当該画像に含まれる特徴点のうち、復元済みの特徴点に対応する特徴点を用いる。例えば、制御部１４は、第３画像４０に含まれる特徴点４１〜４４のうち、復元済みの特徴点２１，３１に対応する特徴点４１、及び、復元済みの特徴点２２，３２に対応する特徴点４２を含む少なくとも３点以上の特徴点を用いる。制御部１４は、ＰｎＰ問題（perspective n-Point Problem）を解くことにより、位置姿勢が未推定である画像に対応する位置姿勢を推定してよい。例えば、制御部１４は、第３画像４０と、特徴点４１，４２を含む３点以上の特徴点等とに基づいて、第３画像４０に対応する位置姿勢を推定する。

制御部１４は、新たに推定した位置姿勢をバンドル調整により最適化する。制御部１４は、当該バンドル調整として、ローカルバンドル調整を実施してよい。ローカルバンドル調整とは、１つの位置姿勢のみを最適化する処理である。つまり、制御部１４は、新たに推定した位置姿勢のみを最適化の対象として、ローカルバンドル調整を実施してよい。例えば、制御部１４は、第３画像４０に対応する位置姿勢のみを最適化の対象として、ローカルバンドル調整を実施してよい。

ローカルバンドル調整では、制御部１４は、最適化の対象となる位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の二次元座標と、当該復元済みの特徴点により復元された復元点の当該画像への再投影点の二次元座標との間のずれが最小化されるように、最適化の対象となる位置姿勢を調整して最適化する。例えば、制御部１４は、第３画像４０の特徴点４１，４２の各々の二次元座標と、復元点７１，７２の各々の第３画像４０への再投影点の各々の二次元座標との間のずれが最小化されるように第３画像４０に対応する位置姿勢を調整して最適化する。式（２）のコスト関数Ｅを用いる場合、ｉには、第３画像４０を識別する番号のみが代入される。ｊには、図３に示す特徴点４１，４２を識別する番号の各々が代入される。つまり、ｘ^ｉ _ｊには、図３に示す特徴点４１，４２の二次元座標の各々が代入される。Ｘ_ｊには、図３に示す復元点７１，７２の三次元座標の各々が代入される。また、射影行列Ｐ_ｉの一成分である式（３）の行列［Ｒ｜ｔ］は、第３画像４０に対応する位置姿勢が代入される。ローカルバンドル調整では、式（２）の復元点Ｘ_ｊを固定し、式（３）の行列［Ｒ｜ｔ］の値を調整することにより、式（２）のコスト関数Ｅが最小化させるように調整する。

制御部１４は、ローカルバンドル調整後、未復元の特徴点の中で、新たに推定した位置姿勢によって復元可能な特徴点を、復元する。例えば、制御部１４は、新たに推定した位置姿勢と、推定済みの位置姿勢と、カメラの内部パラメータとを用いた、三角測量の原理を利用することにより、未復元の特徴点の中で、復元可能な特徴点を復元する。

例えば、第３画像４０に対応する位置姿勢は、追加の推定処理にて、新たに推定された位置姿勢である。また、第１画像２０及び第２画像３０に対応する位置姿勢は、１回目の推定処理にて既に推定された、推定済みの位置姿勢である。第１画像２０の特徴点２３と第３画像４０の特徴点４３は、第１画像２０に対応する位置姿勢及び第３画像４０に対応する位置姿勢を用いた三角測量の原理により、復元可能である。制御部１４は、第１画像２０の特徴点２３及び第３画像４０の特徴点４３を復元することにより、復元点７３のワールド座標系における三次元座標を取得する。また、第２画像３０の特徴点３４と第３画像４０の特徴点４４は、第２画像３０に対応する位置姿勢及び第３画像４０に対応する位置姿勢を用いた三角測量の原理により、復元可能である。制御部１４は、第２画像の特徴点３４及び第３画像４０の特徴点４４を復元することにより、復元点７４のワールド座標系における三次元座標を取得する。

＜相対的な位置姿勢の推定処理＞
制御部１４は、上述の追加の推定処理を繰り返し実行する。制御部１４は、複数の画像の中で、位置姿勢が未推定である画像が存在しないと判定するまで、上述の追加の推定処理を繰り返してよい。

制御部１４は、上述の追加の推定処理を繰り返し実行した後、複数の画像の少なくとも一部に対応する推定した位置姿勢をバンドル調整により最適化する。制御部１４は、複数の画像の全てに対応する推定した位置姿勢を、バンドル調整により最適化してよい。制御部１４は、当該バンドル調整として、グローバルバンドル調整を実施してよい。

グローバルバンドル調整では、制御部１４は、複数の画像の各々において、画像に含まれる特徴点の二次元座標と、当該特徴点に対応する復元点の当該画像への再投影点の二次元座標との間のずれが最小化されるように、推定した位置姿勢及び復元点の三次元座標を調整して最適化する。式（２）のコスト関数Ｅを用いる場合、グローバルバンドル調整では、式（２）の復元点Ｘ_ｊの値と、射影行列Ｐ_ｉの一成分である式（３）の行列［Ｒ｜ｔ］の値を調整することにより、コスト関数Ｅが最小化されるように調整する。

制御部１４は、推定した位置姿勢に対して拘束条件を適用して、グローバルバンドル調整を実施してよい。拘束条件は、セルフキャリブレーションを実行する程度の短時間では、左カメラ１０と右カメラ１１との間の相対的な位置姿勢が一定であるとの条件であってよい。つまり、式（１）の変換式α，βが、セルフキャリブレーションを実行する程度の短時間では、変化しないとの条件であってよい。式（２）のコスト関数Ｅに、式（１）を適用することにより、式（２）のコスト関数Ｅに含まれるパラメータが複雑化する蓋然性が低減され得る。パラメータが複雑化する蓋然性が低減されることで、コスト関数Ｅを最小化させる精度が向上され得る。

例えば、式（２）の射影行列Ｐ_ｉの一成分である式（３）の行列［Ｒ｜ｔ］は、左カメラ１０の位置姿勢［Ｒ_Ｌ｜ｔ_Ｌ］及び右カメラ１１の位置姿勢［Ｒ_Ｒ｜ｔ_Ｒ］の何れかに対応する。制御部１４は、式（２）のコスト関数Ｅに含まれる行列［Ｒ_Ｒ｜ｔ_Ｒ］に、式（１）による［Ｒ_Ｌα｜ｔ_Ｌβ］を代入する。この場合、制御部１４は、コスト関数Ｅに含まれる行列［Ｒ｜ｔ］に関するパラメータに関しては、コスト関数Ｅが最小化されるように、パラメータ（Ｒ_Ｌ，ｔ_Ｌ，Ｒ_Ｌα，ｔ_Ｌβ）を調整する。又は、制御部１４は、式（２）のコスト関数Ｅに含まれる行列［Ｒ_Ｌ｜ｔ_Ｌ］に、式（１）による［Ｒ_Ｒ／α｜ｔ_Ｒ／β］を代入してよい。この場合、制御部１４は、コスト関数Ｅに含まれるパラメータに関しては、コスト関数Ｅが最小化されるように、パラメータ（Ｒ_Ｒ，ｔ_Ｒ，Ｒ_Ｒ／α，ｔ_Ｒ／β）を調整する。

制御部１４は、グローバルバンドル調整により最適化された位置姿勢に基づいて、左カメラ１０と右カメラ１１との間の相対的な位置姿勢を推定する。例えば、制御部１４は、上述の拘束条件を適用したグローバルバンドル調整を実行した場合、上述のように、位置姿勢を最適化するために、パラメータの１つとして変換式α，βを調整する。この場合、制御部１４は、左カメラ１０と右カメラ１１との間の相対的な位置姿勢として、調整後の変換式α，βを取得する。又は、制御部１４は、上述の拘束条件を適用しないグローバルバンドル調整を実行した場合、最適化後の左カメラ１０の位置姿勢［Ｒ_Ｌ｜ｔ_Ｌ］及び右カメラ１１の位置姿勢［Ｒ_Ｒ｜ｔ_Ｒ］を取得する。さらに、制御部１４は、最適化後の位置姿勢［Ｒ_Ｌ｜ｔ_Ｌ］及び位置姿勢［Ｒ_Ｒ｜ｔ_Ｒ］を式（１）代入することにより、回転の変換式α及び並進の変換式βを取得する。

制御部１４は、セルフキャリブレーション後、通信部１３によって外部機器又は電子機器２から被写体までの方向及び距離の測定指令を取得すると、新たに取得した変換式α，βを用いて、被写体までの方向及び距離を測定する。新たに取得した変換式α，βを用いることにより、被写体までの方向及び距離がより精度良く測定され得る。

図４は、本開示の第１実施形態に係る撮像装置３の処理の流れを示すフローチャートである。制御部１４は、通信部１３によって外部機器又は電子機器２から、キャリブレーション指令を取得すると、図４に示す処理を開始する。

制御部１４は、車両１が移動中であるか否か判定する（ステップＳ１０）。制御部１４は、車両１が移動中であると判定するとき（ステップＳ１０：Ｙｅｓ）、ステップＳ１１の処理に進む。一方、制御部１４は、車両１が移動中であると判定しないとき（ステップＳ１０：Ｎｏ）、ステップＳ１０の処理を再び実行する。

制御部１４は、移動中の左カメラ１０及び右カメラ１１に、複数の画像を時系列的に生成させる（ステップＳ１１）。

制御部１４は、移動中の左カメラ１０及び右カメラ１１が時系列的に生成した複数の画像から、特徴点を抽出する（ステップＳ１２）。

制御部１４は、複数の画像において、対応する特徴点を特定する（ステップＳ１３）。制御部１４は、対応を特定した特徴点から、外れ値を除去する（ステップＳ１４）。

制御部１４は、複数の画像の中から、２つの画像を選択する（ステップＳ１５）。制御部１４は、選択した２つの画像に対応する位置姿勢を推定する（ステップＳ１６）。制御部１４は、推定した位置姿勢に基づいて、２つの画像に含まれる対応する特徴点に三次元空間にて対応する点を、復元点とみなして復元する（ステップＳ１７）。制御部１４は、復元した特徴点に基づいて、推定した位置姿勢をバンドル調整により最適化する（ステップＳ１８）。

制御部１４は、複数の画像の中から、位置姿勢が未推定である画像を１つ選択する（ステップＳ１９）。制御部１４は、ステップＳ１９の処理で選択した位置姿勢が未推定である画像と、当該画像に含まれる特徴点の少なくとも一部とに基づいて、当該画像に対応する位置姿勢を推定する（ステップＳ２０）。

制御部１４は、新たに推定した位置姿勢を、ローカルバンドル調整により最適化する（ステップＳ２１）。制御部１４は、新たに位置姿勢を推定すると、未復元の特徴点の中で、新たに推定した位置姿勢によって復元可能な特徴点を、復元する（ステップＳ２２）。

制御部１４は、ステップＳ１１の処理で取得した複数の画像の中で、位置姿勢が未推定である画像が存在するか否か判定する（ステップＳ２３）。制御部１４は、位置姿勢が未推定である画像が存在すると判定するとき（ステップＳ２３：Ｙｅｓ）、ステップＳ１９からステップＳ２３までの処理を繰り返し実行する。制御部１４は、位置姿勢が未推定である画像が存在しないと判定するとき（ステップＳ２３：Ｎｏ）、ステップＳ２４の処理に進む。

ステップＳ２４の処理では、制御部１４は、ステップＳ１１の処理で取得した複数の画像の少なくとも一部に対応する推定した位置姿勢を、グローバルバンドル調整により最適化する。ステップＳ２４の処理では、制御部１４は、推定した位置姿勢に対して拘束条件を適用して、グローバルバンドル調整を実施してよい。

制御部１４は、グローバルバンドル調整により最適化された位置姿勢に基づいて、左カメラ１０と右カメラ１１との間の相対的な位置姿勢を推定する（ステップＳ２５）。

以上のように、第１実施形態では、制御部１４は、推定した位置姿勢に対して拘束条件を適用して、グローバルバンドル調整を実施することができる。推定した位置姿勢に対して拘束条件を適用してグローバルバンドル調整を実施することにより、式（２）のコスト関数Ｅに含まれるパラメータが複雑化する蓋然性が低減され得る。パラメータが複雑化する蓋然性が低減されることで、コスト関数Ｅを最小化させる精度が向上され得る。コスト関数Ｅを最小化させる精度が向上することにより、左カメラ１０と右カメラ１１との間の相対的な位置姿勢をより精度良く推定することができる。このような構成により、第１実施形態では、被写体までの方向及び距離をより精度良く測定することができる。従って、第１実施形態によれば、改善された、撮像装置３及び車両１が提供され得る。

（第２実施形態）
第２実施形態に係る撮像装置の構成には、第１実施形態に係る撮像装置３の構成を採用することができる。従って、第２実施形態に係る撮像装置の構成については、図１を参照して説明する。

ＳｆＭでは、三次元空間上の被写体の一部である対象点が動かないとの前提の元、三角測量の原理を利用して、復元点の三次元座標及びカメラの位置姿勢を推定する。それ故、ＳｆＭでは、対象点が突如動き出したりすると、復元点の三次元座標及びカメラの位置姿勢の推定精度が低下する。

本実施形態では、撮像装置３は、車両１に搭載される。車両１に搭載される撮像装置３の被写体は、図２に示すような住宅に加えて、車両１の前を走行する先行車両、及び、車両１の近辺を歩行する歩行者を含み得る。住宅は、地上に固定されているため、動かない。ただし、先行車両及び歩行者等は、突如、動き出したり、停止したりする場合がある。つまり、本実施形態では、撮像装置３の被写体に先行車両及び歩行者等が含まれる場合、被写体の一部である対象点が、突如、動き出したり、停止したりする場合がある。対象点が突如動き出したり停止したりすると、復元点の三次元座標及びカメラの位置姿勢の推定精度が低下する。復元点の三次元座標及びカメラの位置姿勢の推定精度が低下すると、キャリブレーションを精度良く実行できない場合がある。

第２実施形態では、対象点が突如動き出したり停止したりしても、後述の処理により、キャリブレーションを精度良く実行することができる。以下、第２実施形態に係るセルフキャリブレーションについて説明する。

［セルフキャリブレーション］
第１実施形態と同様に、制御部１４は、通信部１３によって外部機器又は電子機器２から、キャリブレーション指令を取得し得る。第１実施形態と同様に、制御部１４は、キャリブレーション指令を取得すると、通信部１３によって電子機器２から、車両１の移動情報を取得する。第１実施形態と同様に、制御部１４は、車両１の移動情報に基づいて、車両１が移動中であるか否か判定する。第１実施形態と同様に、制御部１４は、車両１が移動中であると判定すると、左カメラ１０及び右カメラ１１に、時系列的に複数の画像を生成させる。

図５に、本開示の第２実施形態に係る被写体の一例を示す。車両４は、撮像装置３の被写体の１つである。車両４は、車両１の前を走行する先行車両である。車両４は、時刻Ｔ１で、停止する。車両４は、時刻Ｔ１が経過した時点で、突如、動き出す。車両４は、時刻Ｔ１が経過した時点から時刻Ｔ２まで、前方に進む。車両４は、時刻Ｔ２が経過した時点で、突如、停止する。車両４は、時刻Ｔ２が経過した時点から、時刻Ｔ３まで、停止する。車両４の後方のコーナ部は、対象点６５となり得る。便宜的に、時刻Ｔ２〜Ｔ３における車両４のコーナ部は、対象点６５ａとも記載する。車両４が突如動き出したり停止したりすることにより、対象点６５も突如動き出したり停止したりする。

第１実施形態と同様に、制御部１４は、移動中の左カメラ１０及び右カメラ１１が時系列的に生成した複数の画像から、特徴点を抽出する。第１実施形態と同様に、制御部１４は、複数の画像において、対応する特徴点を特定する。第１実施形態と同様に、制御部１４は、対応する特徴点を特定する際、所定時間内に生成された画像からすなわちフレームの間隔が所定間隔となる画像から抽出した特徴点に対して、マッチング処理を実行してよい。第１実施形態と同様に、制御部１４は、対応を特定した特徴点から、任意のアルゴリズムを用いて、外れ値を除去してよい。

図６に、本開示の第２実施形態に係る対応する特徴点の一例を示す。第１右画像２０Ｒ、第１左画像３０Ｌ、第２右画像４０Ｒ及び第３右画像５０Ｒは、各々異なる位置姿勢のカメラによって生成された複数の画像の一部である。図６では、第１右画像２０Ｒ、第１左画像３０Ｌ、第２右画像４０Ｒ及び第３右画像５０Ｒは、説明の簡易化のために、位置姿勢が大きく異なるカメラによって生成された画像として描画されている。ただし、これらの画像は、位置姿勢が実際には微小に異なるカメラによって生成された画像であってよい。第１右画像２０Ｒ、第２右画像４０Ｒ及び第３右画像５０Ｒは、右カメラ１１によって生成された画像である。第１左画像３０Ｌは、左カメラ１０によって生成された画像である。第１右画像２０Ｒ及び第１左画像３０Ｌは、時刻Ｔ１に生成された画像である。第２右画像４０Ｒ及び第３右画像５０Ｒの各々は、時刻Ｔ２及び時刻Ｔ３の各々に生成された画像である。

例えば、第１右画像２０Ｒでは、三次元空間上の対象点６１，６２，６５が、各々特徴点２１，２２，２５として抽出される。また、第１左画像３０Ｌでは、三次元空間上の対象点６１，６２，６４，６５が、各々特徴点３１，３２，３４，３５として抽出される。また、第２右画像４０Ｒでは、三次元空間上の対象点６１，６２，６４，６５ａが、各々特徴点４１，４２，４４，４５ａとして抽出される。また、第３右画像５０Ｒでは、三次元空間上の対象点６２，６４，６５ａが、各々特徴点５２，５４，５５ａとして抽出される。ここで、対象点６１，６２，６４は、図２に示すような被写体としての住宅の一部であり得る。対象点６５，６５ａは、図５に示すような被写体としての車両４の後方のコーナ部である。なお、図６には、便宜上、第１右画像２０Ｒ、第１左画像３０Ｌ、第２右画像４０Ｒ及び第３右画像５０Ｒから抽出される特徴点の一部のみを示す。

制御部１４は、対応する特徴点として、異なる画像間で同一の対象点６１を示す特徴点２１，３１，４１を特定する。制御部１４は、対応する特徴点として、異なる画像間で同一の対象点６２を示す特徴点２２，３２，４２，５２を特定する。制御部１４は、対応する特徴点として、異なる画像間で同一の対象点６４を示す特徴点３４，４４，５４を特定する。ここで、図５に示すように、対象点６５の三次元空間における位置と、対象点６５ａの三次元空間における位置とは、車両４が動き出すことにより、異なる。だたし、図５に示すように、対象点６５及び対象点６５ａは、車両４の後方のコーナ部分すなわち被写体の同一部分である。そのため、制御部１４は、対応する特徴点として、異なる画像間で、被写体の同一部分を示す特徴点２５，３５，４５ａ，５５ａを特定する。

＜１回目の推定処理＞
第１実施形態と同様に、制御部１４は、複数の画像の中から、２つの画像を選択する。第１実施形態と同様に、制御部１４は、複数の画像の中から、対応する特徴点を最も多く含む２つの画像を選択してよい。

第２実施形態では、２つの画像として、左カメラ１０及び右カメラ１１が同時刻に生成した画像を選択してよい。例えば、制御部１４は、２つの画像として、第１右画像２０Ｒ及び第１左画像３０Ｌを選択する。同時刻に生成された２つの画像を選択することにより、当該２つの画像において対応する特徴点が示す対象点は、停止している点となり得る。例えば、第１右画像２０Ｒ及び第１左画像３０Ｌにおいて対応する特徴点２５及び特徴点３５が示す対象点６５は、停止している点となる。２つの画像において対応する特徴点が示す対象点が停止している点となることで、後述の処理において、２つの画像に対応する位置姿勢をより精度良く推定することができる。

第１実施形態と同様に、制御部１４は、選択した２つの画像に対応する位置姿勢を推定する。第１実施形態と同様に、制御部１４は、推定した位置姿勢に基づいて、２つの画像に含まれる対応する特徴点に三次元空間にて対応する点を、復元点とみなして復元する。さらに、制御部１４は、復元点のワールド座標系における三次元座標を取得する。

例えば、制御部１４は、第１右画像２０Ｒに対応する位置姿勢及び第１左画像３０Ｌに対応する位置姿勢を推定する。制御部１４は、図６に示すように、特徴点２１，３１を復元することにより、復元点７１の三次元座標を取得する。また、制御部１４は、図６に示すように、特徴点２２，３２を復元することにより、復元点７２の三次元座標を取得する。また、制御部１４は、図６に示すように、特徴点２５，３５を復元することにより、復元点７５の三次元座標を取得する。

第１実施形態と同様に、制御部１４は、復元した特徴点に基づいて、推定した位置姿勢をバンドル調整により最適化する。

＜追加の推定処理＞
第１実施形態と同様に、制御部１４は、複数の画像の中から、位置姿勢が未推定である画像を１つ選択する。第１実施形態と同様に、制御部１４は、復元済みの特徴点に対応する特徴点を最も多く含む画像を優先して選択してよい。第１実施形態と同様に、制御部１４は、選択した位置姿勢が未推定である画像と、当該画像に含まれる特徴点の少なくとも一部とに基づいて、当該画像に対応する位置姿勢を推定する。

例えば、１回目の推定処理が終了した直後では、第２右画像４０Ｒ及び第３右画像５０Ｒに対応する位置姿勢は、未推定である。第２右画像４０Ｒは、復元済みの特徴点２１，３１に対応する特徴点４１と、復元済みの特徴点２２，３２に対応する特徴点４２と、復元済みの特徴点２５，３５に対応する特徴点４５ａを含む。つまり、第２右画像４０Ｒは、復元済みの特徴点に対応する特徴点を３つ含む。これに対し、第３右画像５０Ｒは、復元済みの特徴点２２，３２に対応する特徴点５２を含む。つまり、第３右画像５０Ｒは、復元済みの特徴点に対応する特徴点を１つ含む。この場合、制御部１４は、第２右画像４０Ｒ及び第３右画像５０Ｒのうち、復元済みの特徴点に対応する特徴点を最も多く含む画像として、第２右画像４０Ｒを優先して選択する。制御部１４は、第２右画像４０Ｒに対応する位置姿勢を推定する。

第２実施形態では、制御部１４は、新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の位置と、当該復元済みの特徴点により復元された復元点の当該画像への再投影点の位置との間の距離を算出する。例えば、制御部１４は、新たに推定した位置姿勢［Ｒ｜ｔ］を式（３）に代入して算出される射影行列Ｐと、復元点Ｘの三次元座標とを乗算させることにより、再投影点の座標を算出する。さらに、制御部１４は、特徴点の座標及び算出した再投影点の座標に基づいて、特徴点の位置と再投影点の位置との間の距離を算出する。制御部１４は、算出した距離が閾値以上であるか否か判定する。制御部１４は、算出した距離が閾値以上であると判定するとき、当該復元点を、当該画像に対応しない復元点と決定する。本開示において「画像に対応しない復元点」とは、当該復元点に対応する対象点が、例えば動き出したことにより、当該画像が生成された時刻に、当該復元点の位置に存在している可能性が低いと推定される復元点を意味する。一方、制御部１４は、算出した距離が閾値未満であると判定するとき、当該復元点を、当該画像に対応する復元点と決定する。本開示において「画像に対応する復元点」とは、当該復元点に対応する対象点が、例えば停止していることにより、当該画像が生成された時刻に、当該復元点の位置に存在している可能性が高いと推定される復元点を意味する。閾値は、車両１の速度等に基づいて、適宜設定されてよい。

例えば、第２右画像４０Ｒは、当該追加の推定処理にて、新たに位置姿勢が推定された画像である。再投影点９１は、復元済みの特徴点２１，３１により復元された復元点７１の第２右画像４０Ｒへの再投影点である。再投影点９２は、復元済みの特徴点２２，３２により復元された復元点７２の第２右画像４０Ｒへの再投影点である。再投影点９５は、復元済みの特徴点２５，３５により復元された復元点７５の第２右画像４０Ｒへの再投影点である。

再投影点９１の位置と、復元済みの特徴点２１，３１に対応する第２右画像４０Ｒの特徴点４１の位置との間の距離は、閾値未満であるものとする。制御部１４は、再投影点９１の位置と特徴点４１の位置との間の距離が閾値未満であると判定し、復元点７１を、第２右画像４０Ｒに対応する復元点と決定する。

再投影点９２の位置と、復元済みの特徴点２２，３２に対応する第２右画像４０Ｒの特徴点４２の位置との間の距離は、閾値未満であるものとする。制御部１４は、再投影点９２の位置と特徴点４２の位置との間の距離が閾値未満であると判定し、復元点７２を、第２右画像４０Ｒに対応する復元点と決定する。

再投影点９５の位置と、復元済みの特徴点２５，３５に対応する特徴点４５ａの位置との間の距離は、閾値以上であるものとする。制御部１４は、再投影点９１と特徴点４５ａの位置との間の距離が閾値以上であると判定し、復元点７５を、第２右画像４０Ｒに対応しない復元点と決定する。このような処理により、対象点６５に対応する復元点７５は、時刻Ｔ２に生成された第２右画像４０Ｒに対応しないと決定される。

なお、本実施形態では、特徴点の位置と再投影点の位置との間の距離が閾値以上であるか否かの判定処理を行わない画像及び復元点については、当該復元点を、当該画像に対応する復元点として扱う。例えば、制御部１４は、復元点７１を、第１右画像２０Ｒに対応する復元点として扱う。

第１実施形態と同様に、制御部１４は、新たに推定した位置姿勢をバンドル調整により最適化する。制御部１４は、当該バンドル調整として、ローカルバンドル調整を実施してよい。ただし、第２実施形態では、制御部１４は、画像に対応しないと決定した復元点を、ローカルバンドル調整の最小化の対象から除外してよい。例えば、制御部１４は、復元点７５の第２右画像４０Ｒへの再投影点９５と、特徴点４５ａとの間の誤差を、ローカルバンドル調整の最小化の対象から除外してよい。

第１実施形態と同様に、制御部１４は、ローカルバンドル調整後、未復元の特徴点の中で、新たに推定した位置姿勢によって復元可能な特徴点を、復元する。例えば、第２右画像４０Ｒに対応する位置姿勢は、追加の推定処理にて新たに推定された位置姿勢である。また、第１左画像３０Ｌに対応する位置姿勢は、１回目の推定処理にて既に推定された、推定済みの位置姿勢である。制御部１４は、図６に示すように、第１左画像３０Ｌの特徴点３４と第２右画像４０Ｒの特徴点４４を復元することにより、復元点７４のワールド座標系における三次元座標を取得する。

制御部１４は、当該追加の推定処理を終了すると、次の追加の推定処理を実行する。例えば、制御部１４は、第２右画像４０Ｒに対する追加の推定処理を終了すると、第３右画像５０Ｒに対する追加の推定処理を実行する。第３右画像５０Ｒに対する追加の推定処理において、制御部１４は、第３右画像５０Ｒに対応する位置姿勢を推定する。さらに、第３右画像５０Ｒに対する追加の推定処理において、制御部１４は、復元点７２を、第３右画像５０Ｒに対応する復元点と決定する。また、制御部１４は、復元点７４を、第３右画像５０Ｒに対応する復元点と決定する。また、制御部１４は、復元点７５を、第３右画像５０Ｒに対応しない復元点と決定する。加えて、第３右画像５０Ｒに対する追加の推定処理において、制御部１４は、第２右画像４０Ｒに対応する位置姿勢及び第３右画像５０Ｒに対応する位置姿勢を用いた三角測量の原理により、図６に示すように、特徴点４５ａ，５５ａを復元する。制御部１４は、特徴点４５ａ，５５ａを復元して、復元点７５ａのワールド座標系における三次元座標を取得する。

図７に、本開示の第２実施形態に係る画像に対応する復元点の一例を示す。図７には、第３右画像５０Ｒに対する追加の推定処理が終了した時点の結果を示す。第１右画像２０Ｒに対応する復元点は、復元点７１，７２，７５となる。第１左画像３０Ｌに対応する復元点は、復元点７１，７２，７４，７５となる。第２右画像４０Ｒに対応する復元点は、復元点７１，７２，７４，７５ａとなる。第３右画像５０Ｒに対応する復元点は、復元点７２，７４，７５ａとなる。

上述のように、時刻Ｔ１での対象点６５の三次元空間における位置と、時刻Ｔ２及び時刻Ｔ３での対象点６５ａの三次元空間における位置とは、車両４が動き出すことにより、異なる。上述の処理により、図７に示すように、対象点６５に対応する復元点７５は、時刻Ｔ１に生成された第１右画像２０Ｒ及び第１左画像３０Ｌに対応すると決定される。また、対象点６５ａに対応する復元点７５ａは、時刻Ｔ２及び時刻Ｔ３に各々生成された第２右画像４０Ｒ及び第３右画像５０Ｒに対応すると決定される。

＜相対的な位置姿勢の推定処理＞
第１実施形態と同様に、制御部１４は、上述の追加の推定処理を繰り返し実行する。第１実施形態と同様に、制御部１４は、複数の画像の中で、位置姿勢が未推定である画像が存在しないと判定するまで、上述の追加の推定処理を繰り返してよい。

制御部１４は、上述の追加の推定処理を繰り返し実行した後、複数の画像の少なくとも一部の画像に対応する推定済みの位置姿勢と、当該少なくとも一部の画像に対応する復元点とに基づいて、グローバルバンドル調整により当該推定済みの位置姿勢を最適化する。

具体的には、制御部１４は、上述の追加の推定処理において画像に対応すると決定した復元点については、当該画像に対応する推定済みの位置姿勢と、当該画像に対応する復元点とに基づいて、当該復元点の当該画像への再投影点の二次元座標を算出する。さらに、制御部１４は、算出した再投影点の二次元座標と、当該画像に含まれる当該復元点に対応する特徴点の二次元座標との間のずれを算出する。加えて、制御部１４は、算出したずれを、グローバルバンドル調整における最小化の対象にする。

これに対し、制御部１４は、上述の追加の推定処理において画像に対応しないと決定した復元点ついては、当該画像に対応する当該復元点として扱わない。つまり、制御部１４は、当該復元点の当該画像への再投影点の二次元座標を算出しない。より詳細には、制御部１４は、当該復元点の当該画像への再投影点の二次元座標と、当該画像に含まれる当該復元点に対応する特徴点の二次元座標との間のずれを、グローバルバンドル調整における最小化の対象としない。

例えば、複数の画像の少なくとも一部の画像として、第１右画像２０Ｒ、第１左画像３０Ｌ、第２右画像４０Ｒ及び第３右画像５０Ｒが用いられるものとする。また、画像と復元点との対応関係としては、図７に示すような対応関係が用いられるものとする。式（２）のｉに第２右画像４０Ｒを識別する番号を代入する際、制御部１４は、式（２）の特徴点ｘ^ｉ _ｊに特徴点４１，４２，４４，４５ａの二次元座標を各々代入し、式（２）の復元点Ｘ_ｊに復元点７１，７２，７４，７５ａの三次元座標を代入する。さらに、制御部１４は、式（２）のＰ_ｉに、式（３）の［Ｒ｜ｔ］に第２右画像４０Ｒに対応する位置姿勢を代入することにより算出された射影行列を代入する。ここで、制御部１４は、上述の追加の推定処理にて第２右画像４０Ｒに対応しないと決定した復元点７５については、式（２）の復元点Ｘ_ｊに代入しない。つまり、制御部１４は、図６に示すような、復元点７５の第２右画像４０Ｒへの再投影点９５と、復元点７５に対応する特徴点４５ａとの間のずれを、グローバルバンドル調整の最小化の対象としない。

第１実施形態と同様に、制御部１４は、推定した位置姿勢に対して拘束条件を適用してグローバルバンドル調整を実施してよい。

第１実施形態と同様に、制御部１４は、グローバルバンドル調整により最適化された位置姿勢に基づいて、左カメラ１０と右カメラ１１との間の相対的な位置姿勢を推定する。

図８は、本開示の第２実施形態に係る撮像装置３の処理の流れを示すフローチャートである（その１）。図９は、本開示の第２実施形態に係る撮像装置３の処理の流れを示すフローチャートである（その２）。制御部１４は、通信部１３によって外部機器又は電子機器２から、キャリブレーション指令を取得すると、図８に示す処理を開始する。

制御部１４は、図４に示すステップＳ１０〜Ｓ１８の処理と同様にして、ステップＳ３０〜Ｓ３８の処理を実行する。制御部１４は、図８に示すステップＳ３８の処理の終了後、図９に示す処理に進む。

制御部１４は、図４に示すステップＳ１９〜Ｓ２０の処理と同様にして、ステップＳ３９〜Ｓ４０の処理を実行する。

制御部１４は、ステップＳ４０の処理で新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の位置と、当該復元済みの特徴点により復元された復元点の当該画像への再投影点の位置との間の距離を算出する（ステップＳ４１）。

制御部１４は、ステップＳ４１の処理で算出した距離が閾値以上であるか否か判定する（ステップＳ４２）。制御部１４は、ステップＳ４１の処理で算出した距離が閾値未満であると判定するとき（ステップＳ４２：Ｎｏ）、ステップＳ４３の処理に進む。ステップＳ４３の処理では、制御部１４は、ステップＳ４１の処理で用いた復元点を、ステップＳ４０の処理で新たに位置姿勢が推定された画像に対応する復元点と決定する。一方、制御部１４は、ステップＳ４１の処理で算出した距離が閾値以上であると判定するとき（ステップＳ４２：Ｙｅｓ）、ステップＳ４４の処理に進む。ステップＳ４４の処理では、制御部１４は、ステップＳ４１の処理で用いた復元点を、ステップＳ４０の処理で新たに位置姿勢が推定された画像に対応しない復元点と決定する。

制御部１４は、ステップＳ４０の処理で新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の全てに対して、ステップＳ４１〜Ｓ４４の処理を実行してよい。

制御部１４は、図４に示すステップＳ２１，Ｓ２２の処理と同様にして、ステップＳ４５，Ｓ４６の処理を実行する。

制御部１４は、図８に示すステップＳ３１の処理で取得した複数の画像の中で、位置姿勢が未推定である画像が存在するか否か判定する（ステップＳ４７）。制御部１４は、位置姿勢が未推定である画像が存在すると判定するとき（ステップＳ４７：Ｙｅｓ）、ステップＳ３９〜Ｓ４７までの処理を繰り返し実行する。制御部１４は、位置姿勢が未推定である画像が存在すると判定しないとき（ステップＳ４７：Ｎｏ）、ステップＳ４８の処理に進む。

ステップＳ４８の処理では、制御部１４は、複数の画像の少なくとも一部の画像に対応する推定済みの位置姿勢と、当該少なくとも一部の画像に対応する復元点とに基づいて、グローバルバンドル調整により当該推定済みの位置姿勢を最適化する。ステップＳ４８の処理では、制御部１４は、ステップＳ４３の処理で画像に対応しないと決定した復元点ついては、当該画像に対応する復元点として扱わない。

制御部１４は、図４に示すステップＳ２５の処理と同様にして、ステップＳ４９の処理を実行する。

以上のように、第２実施形態では、制御部１４は、追加の推定処理にて、画像に対応しない復元点を決定する。ここで、上述のように、画像に対応しない復元点は、当該復元点に対応する対象点が、例えば動き出したことにより、当該画像が生成された時刻に、当該復元点の位置に存在している可能性が低いと推定されるものである。このような処理により、動き出したと推定される対象点に対応する復元点は、グローバルバンドル調整から除外され得る。動き出したと推定される対象点に対応する復元点がグローバルバンドル調整から除外されることにより、本実施形態によれば、グローバルバンドル調整により、推定した位置姿勢を精度良く最適化することができる。推定した位置姿勢が精度良く最適化されることにより、本実施形態では、セルフキャリブレーションを精度良く実行することができる。

さらに、第２実施形態では、追加の推定処理にて、位置姿勢を推定する新たな画像を追加する毎に、画像に対応しない復元点を決定する。このような処理により、本実施形態では、復元点に対応する対象点が動き出したことにより、当該復元点が画像に対応しなくなった時点で、当該復元点を当該画像に対応しないものとして扱うことができる。このような処理により、グローバルバンドル調整において利用可能な復元点の数が大幅に減る蓋然性が低減され得る。

本開示に係る実施形態について、諸図面及び実施例に基づき説明してきたが、当業者であれば本開示に基づき種々の変形又は修正を行うことが容易であることに注意されたい。従って、これらの変形又は修正は本開示の範囲に含まれることに留意されたい。例えば、各構成部等に含まれる機能等は論理的に矛盾しないように再配置可能であり、複数の構成部等を１つに組み合わせたり、或いは、分割したりすることが可能である。

例えば、上述の実施形態では、式（１）で示されるような、左カメラ１０から右カメラ１１への変換式を用いた。つまり、上述の実施形態では、式（１）で示されるように、右カメラの位置姿勢［Ｒ_Ｒ｜ｔ_Ｒ］が、左カメラ１０の位置姿勢［Ｒ_Ｌ｜ｔ_Ｌ］と、回転の変換式α及び並進の変換式βとによって表されていた。ただし、右カメラ１１から左カメラ１０への変換式を用いてもよい。つまり、左カメラ１０の位置姿勢［Ｒ_Ｌ｜ｔ_Ｌ］が、右カメラ１０の位置姿勢［Ｒ_Ｒ｜ｔ_Ｒ］と、右カメラ１０から左カメラ１１への回転の変換式及び並進の変換式とによって表されてよい。この構成では、制御部１４は、推定した位置姿勢に対して拘束条件を適用して、グローバルバンドル調整を実施する際、式（２）のコスト関数Ｅに含まれる行列［Ｒ_Ｌ｜ｔ_Ｌ］に、行列［Ｒ_Ｒ｜ｔ_Ｒ］と、右カメラ１０から左カメラ１１への回転の変換式及び並進の変換式とによって表される行列［Ｒ_Ｌ｜ｔ_Ｌ］を代入してよい。

１，４ 車両
２ 電子機器
３ 撮像装置
１０ 左カメラ
１１ 右カメラ
１２ 記憶部
１３ 通信部
１４ 制御部
２０ 第１画像
３０ 第２画像
４０ 第３画像
５０ 第４画像
２０Ｒ 第１右画像
３０Ｌ 第１左画像
４０Ｒ 第２右画像
５０Ｒ 第３右画像
２１，２２，２３，２５，３１，３２，３４，３５，４１，４２，４３，４４，４５ａ，５２，５４，５５ａ 特徴点
６１，６２，６３，６４，６５，６５ａ 対象点
７１，７２，７３，７４，７５，７５ａ 復元点
９１，９２，９５ 再投影点

Claims (8)

1. 相対的な位置姿勢が定められた左カメラ及び右カメラと、制御部とを備え、
   前記制御部は、
   移動中の前記左カメラ及び前記右カメラが時系列的に生成した複数の画像から特徴点を抽出し、前記複数の画像において対応する特徴点を特定し、
   前記複数の画像の中の２つの画像に含まれる前記特徴点の一部に基づいて前記２つの画像に対応する位置姿勢を推定し、推定した前記位置姿勢に基づいて前記２つの画像に含まれる前記特徴点を復元して復元点を取得する１回目の推定処理をし、
   位置姿勢が未推定である前記画像と、該未推定である画像に含まれる前記復元した特徴点に対応する特徴点の少なくとも一部とに基づいて、該未推定である画像に対応する位置姿勢を推定し、推定した位置姿勢に基づいて該対応する特徴点を復元して復元点を取得する追加の推定処理をし、
   前記追加の推定処理を繰り返し実行した後、前記複数の画像の少なくとも一部の画像に対応する推定済みの位置姿勢と、該少なくとも一部の画像に対応する前記復元点とに基づいて、バンドル調整により該推定済みの位置姿勢を最適化し、最適化後の位置姿勢に基づいて、前記左カメラと右カメラとの間の相対的な位置姿勢の推定処理をし、
   前記追加の推定処理において、新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の位置と、該復元済みの特徴点により復元された復元点の該画像への再投影点の位置との間の距離が閾値以上である場合、該復元点を、前記位置姿勢の推定処理において、該画像に対応する復元点として扱わない、
   撮像装置。
2. 請求項１に記載の撮像装置において、
   前記制御部は、前記１回目の推定処理において、前記複数の画像の中で、対応する特徴点が最も多い２つの画像を選択し、該２つの画像に対応する前記位置姿勢を推定する、撮像装置。
3. 請求項１又は２に記載の撮像装置において、
   前記制御部は、前記追加の推定処理において、位置姿勢が未推定である前記複数の画像の中で、復元済みの前記特徴点に対応する前記特徴点を最も多く含む画像を優先して位置姿勢を推定する、撮像装置。
4. 請求項１から３までの何れか一項に記載の撮像装置において、
   前記制御部は、前記追加の推定処理を、複数の画像の中で、位置姿勢が未推定である画像が存在しないと判定するまで、実行する、撮像装置。
5. 請求項１から４までの何れか一項に記載の撮像装置において、
   前記制御部は、前記相対的な位置姿勢の推定処理において、前記複数の画像の全ての画像に対応する推定済みの位置姿勢と、該全ての画像に対応する前記復元点とに基づいて、バンドル調整により該推定済みの位置姿勢を最適化する、撮像装置。
6. 請求項１から５までの何れか一項に記載の撮像装置において、
   前記制御部は、前記追加の推定処理において、前記位置姿勢の推定からバンドル調整までの間に、未復元である特徴点の中で復元可能な特徴点を復元する、撮像装置。
7. 請求項１から６までの何れか一項に記載の撮像装置を搭載した車両。
8. 移動中の相対的な位置姿勢が定められた左カメラ及び右カメラが時系列的に生成した複数の画像から特徴点を抽出し、前記複数の画像において対応する特徴点を特定するステップと、
   前記複数の画像の中の２つの画像に含まれる前記特徴点の一部に基づいて前記２つの画像に対応する位置姿勢を推定し、推定した前記位置姿勢に基づいて前記２つの画像に含まれる前記特徴点を復元して復元点を取得する１回目の推定処理をするステップと、
   位置姿勢が未推定である前記画像と、該未推定である画像に含まれる前記復元した特徴点に対応する特徴点の少なくとも一部とに基づいて、該未推定である画像に対応する位置姿勢を推定し、推定した位置姿勢に基づいて該対応する特徴点を復元して復元点を取得する追加の推定処理をするステップと、
   前記追加の推定処理をするステップを繰り返し実行した後、前記複数の画像の少なくとも一部の画像に対応する推定済みの位置姿勢と、該少なくとも一部の画像に対応する前記復元点とに基づいて、バンドル調整により該推定済みの位置姿勢を最適化し、最適化後の位置姿勢に基づいて、前記左カメラと右カメラとの間の相対的な位置姿勢の推定処理をするステップと、
   前記追加の推定処理をするステップにおいて、新たに位置姿勢が推定された画像に含まれる復元済みの特徴点に対応する特徴点の位置と、該復元済みの特徴点により復元された復元点の該画像への再投影点の位置との間の距離が閾値以上である場合、該復元点を、前記位置姿勢の推定処理において、該画像に対応する復元点として扱わないステップと、を装置に実行させる、
   プログラム。

Images