JP7149707B2 - 情報処理装置及びその制御方法及びプログラム、並びに、運転制御システム - Google Patents

Description

本発明は情報処理装置及びその制御方法及びプログラム、並びに、運転制御システムに関する。

画像情報に基づく撮像装置の位置及び姿勢を計測する技術は、自動車やロボットの自己位置推定、複合現実感／拡張現実感における現実空間と仮想物体の位置合わせ、物体や空間の三次元モデリングなど様々な目的で利用される。

特許文献１にはステレオカメラを用いてランドマークを精度良く計測することによって自己位置姿勢を推定する技術が開示されている。また、非特許文献１には単眼のカメラのみを用いて、深層学習で予め学習したモデルを利用してシーンの奥行きを推定することによって、自己位置推定を行う技術が開示されている。

特開２００９－２００１４号公報

K. Tateno, F. Tombari, I. Laina and N. Navab, "CNN-SLAM: Real-time dense monocular SLAM with learned depth prediction", IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR), 2017. J. Engel, T. Schps, and D. Cremers. "LSD-SLAM: Large-Scale Direct Monocular SLAM", In European Conference on Computer Vision (ECCV), 2014.

しかしながら、自動車の自動運転や運転アシストに用いる自己位置推定のためのモジュールとして利用することなどを考えると、特許文献１や非特許文献１の手法よりも、より安定的に位置姿勢推定を行うことが求められる。

本発明は上記問題に鑑みてなされたものであり、より安定的に位置姿勢推定を行う技術を提供しようとするものである。

この課題を解決するため、例えば本発明の情報処理装置は以下の構成を備える。すなわち、
撮像視野の少なくとも一部が重複するように移動体に配置された第１の撮像装置及び第２の撮像装置における、前記第１の撮像装置が得た第１の画像、及び、前記第２の撮像装置が得た第２の画像を入力する入力手段と、
画像と該画像を撮影したシーンの三次元形状を表す距離情報とを教師データとして用いて、入力された画像に対応する撮影したシーンの三次元形状を表す距離情報を出力するように学習済みの学習済みモデルを学習モデル保持手段から取得する取得手段と、
前記第１の画像及び前記第２の画像から第１の撮影したシーンの三次元形状を表す距離情報を推定する第１の推定手段と、
前記第１の画像に対応する第２の撮影したシーンの三次元形状を表す距離情報を、前記学習済みモデルを用いて推定する第２の推定手段と、
前記第２の画像に対応する第３の撮影したシーンの三次元形状を表す距離情報を、前記学習済みモデルを用いて推定する第３の推定手段と、
前記第１、第２、第３の撮影したシーンの三次元形状を表す距離情報の少なくとも一つに基づいて、前記第１の撮像装置及び前記第２の撮像装置を含む撮像部の位置情報、当該撮像部が撮像した画像が表す撮影したシーンの三次元形状を表す距離情報の少なくとも一方を生成する生成手段とを有する。

本発明によれば、撮像装置の不具合に対してロバストに位置姿勢推定を行うことができる。

第１の実施形態における自動車の構成を示した図。 第１の実施形態における情報処理装置と運転処理部の機能構成例を示す図。 第１の実施形態における情報処理装置のハードウェア構成を示す図。 第１の実施形態における情報処理システムが実施する処理手順を示すフローチャート。 第１の実施形態における計測結果処理の手順を示すフローチャート。 第１の実施形態における変形例の処理手順を示すフローチャート。 第１の実施形態における変形例の計測結果処理の手順を示すフローチャート。 第２の実施形態における情報処理システムが実施する処理手順を示すフローチャート。 第２の実施形態における計測結果処理部が位置姿勢の不具合の有無を判定する手順を示すフローチャート。 第２の実施形態における変形例の処理手順を示すフローチャート。 第２の実施形態における計測結果処理部が幾何情報の不具合の有無を判定する手順を示すフローチャート。 第３の実施形態における情報処理装置と運転処理部の機能構成例を示す図。 第３の実施形態における情報処理システムが実施する処理手順を示すフローチャート。 第３の実施形態における計測結果処理部が不具合を解消する手順を示すフローチャート。 第４の実施形態における情報処理装置と運転処理部の機能構成例を示す図。 第４の実施形態における教師データを取得して学習を行う処理手順を示すフローチャート。 第５の実施形態における自動車の構成を示した図。 第５の実施形態における情報処理装置と運転処理部の機能構成例を示す図。 第５の実施形態における情報処理システムが実施する処理手順を示すフローチャート。

以下、本発明にかかる実施形態の情報処理装置を詳細に説明する。

本実施形態を説明するのに先立ち、用語の定義について説明する。

幾何情報とは、撮影したシーンの三次元形状を表すものであり、画像の各画素に距離値を格納した距離画像として表される。距離画像を必ずしも形成する必要はなく、距離画像の各画素を、Ｘ、Ｙ、Ｚの座標値で表すポイントクラウドなど三次元点群として表しても良く、三次元位置を表現可能な距離情報であれば良い。

位置姿勢とは位置を表す３自由度のパラメータと、姿勢を表す３自由度のパラメータとの合計６自由度のパラメータで表される。本実施形態では、主に撮像装置の位置と姿勢を表し、間接的に本装置が搭載される車両の位置姿勢をも表すものとする。

学習モデルは、複数の画像とそれと同時刻に同視野を撮影した対応する距離画像に基づいて、入力画像に対応する距離画像が推定できるように予め学習したモデルを表す。学習モデルはＣＮＮ（Convolutional Neural Network）などの画像を入出力とするモデルとする。ただし、学習モデルはこれに限るものではなく、画像を入力として、三次元点群などの幾何情報を出力するようなモデルでもよい。

不具合（又は非適正）とは、推定した幾何情報や位置姿勢が、理想的な幾何情報や位置姿勢とは大きく異なる状態にあることを示す。不具合が生じる要因としては、撮像装置や撮影シーン環境からくる画像に生じたノイズによるもの、学習モデルを用いた幾何情報推定において、学習が不十分なシーンで推定をすることによるもの、校正パラメータが実際とは異なることによるもの、撮像装置自体が故障しているなどの、様々な要因が考えられる。

異常とは、撮像装置の故障や撮像装置のレンズに傷や汚れが付くなど、正常な状態とは異なる状態にあることを示す。また、不具合についても広義の意味で異常と呼ぶことがある。

［第１の実施形態］
第１の実施形態では、情報処理装置が、車などの自動運転または運転アシストのため、車体の自己位置を推定する例を説明する。車体に取り付けた撮像装置の位置姿勢を安定的に求めるために複数のセンサ情報を用いることがしばしば行われるが、本実施形態では、２つの撮像装置を用いたステレオ構成のセンサを用いて、３系統の仮の幾何情報、３系統の仮の位置姿勢情報を推定し、それらの推定結果から安定的な幾何情報、及び、位置姿勢情報を求める。図１は本実施形態における自動車１００００の運転制御システムの構成を示した図である。本実施形態では、自動車１００００に取り付けられた撮像部１０の位置姿勢を安定的に求め、求めた位置姿勢に従って運転制御を行うことで、自動運転または運転アシストを行う。そのために、情報処理装置１を用いて周囲の幾何情報や撮像部１０の位置姿勢を算出する。算出した幾何情報および位置姿勢は運転処理装置１２に供給される。そして、運転処理装置１２は、それらに基づいてアクチュエータ部１３を動作させることによって自動車１００００を制御する。尚、仮の幾何情報とは、最終的に決定する幾何情報の候補、材料、参考となる幾何情報である。

＜情報処理装置の構成＞
図２は、主として、本実施形態における情報処理装置１と運転処理装置１２のブロック構成図である。

情報処理装置１は、画像入力部１００、学習モデル保持部１０１、幾何情報推定部１０２、位置姿勢算出部１０３、計測結果処理部１０４、表示情報生成部１０５、及び、装置全体の制御を司る制御部１５を有する。この制御部１５は、ＣＰＵ（プロセッサ）、このＣＰＵが実行するプログラムを記憶するため、及び、ワークエリアとして使用するためのメモリ（ＲＯＭ及びＲＡＭ）で構成される。

また、運転処理装置１２は、周囲環境取得部１０６、目的地取得部１０７、及び、運転処理装置１２の全体の制御を司る運転制御部１０８を備えている。この運転制御部１０８も、ＣＰＵと、ＣＰＵが実行するプログラム及びワークエリアとしてするメモリ（ＲＯＭ及びＲＡＭ）とで構成される。

画像入力部１００は、車体に取り付けられた撮像部１０と接続される。表示情報生成部１０５は表示部１１と接続される。そして、運転制御部１０８は、車両の車輪のトルク及び方向等を制御するアクチュエータ部１３と接続されている。ただし、図２は、機器構成の一例であり、本発明の適用範囲を限定するものではない。

撮像部１０は、三角測量に基づくステレオによる幾何情報推定を行うための２つの撮像装置から構成される。つまり、これら２つの撮像装置は、それら光軸方向が平行であり、それぞれの撮像視野範囲の大部分が互いに重複するように配置される。また、これら撮像装置は車の周囲環境を画像として時系列（例えば毎秒６０フレーム）に撮像可能であって、１画素当たりＲ、Ｇ、Ｂの３成分の画像を撮像するビデオカメラである。

画像入力部１００は、撮像部１０の二つの撮像装置それぞれが撮像するシーンの２次元画像の画像データを時系列に入力し、学習モデル保持部１０１、幾何情報推定部１０２、位置姿勢算出部１０３、表示情報生成部１０５、運転処理装置１２に供給する。以下では、二つの撮像装置について、どちらか一方を特定する場合には、第一の撮像装置および第二の撮像装置と言い、それぞれの撮像装置から得られる画像を特定する場合には第一の画像および第二の画像と言う。

学習モデル保持部１０１は、１枚の画像から幾何情報を推定するための学習モデルを保持する。幾何情報推定部１０２は、図示しない校正パラメータ保持部が保持する校正パラメータを用いて、第一の画像と第二の画像（ステレオ画像）から三角測量法によって第一の幾何情報を推定する。更に、幾何情報推定部１０２は、学習モデル保持部１０１に保持された学習モデルを用いて、画像入力部１００から入力した第一の画像に対応する第二の幾何情報を推定する。幾何情報推定部１０２は、同様に、学習モデルを用いて、第二の画像に対応する第三の幾何情報を推定する。そして、幾何情報推定部１０２は、推定した第一、第二、第三の幾何情報を位置姿勢算出部１０３に供給する。

位置姿勢算出部１０３は、画像入力部１００が入力した画像と、幾何情報推定部１０２から入力した第一、第二、第三の幾何情報それぞれに基づいて、撮像部１０の第一、第二、第三の位置姿勢を算出する。そして、位置姿勢算出部１０３は、算出した第一、第二、第三の位置姿勢を計測結果処理部１０４に供給する。

計測結果処理部１０４は、位置姿勢算出部１０３より入力した第一、第二、第三の位置姿勢に基づいて、安定的な位置姿勢を算出する。算出方法については後述する。さらに、計測結果処理部１０４は、算出した位置姿勢に基づいて、前フレームで求めた幾何情報を更新する。つまり、計測結果処理部１０４は、位置姿勢算出部１０３が算出した第一、第二、第三の位置姿勢から、最終的な現在の位置姿勢、及び、最終的な現在の幾何情報を生成（決定）する。そして、計測結果処理部１０４は、決定した位置姿勢及び幾何情報を表示情報生成部１０５及び運転処理装置１２に供給する。

表示情報生成部１０５は、計測結果処理部１０４より入力した幾何情報と位置姿勢に基づいて表示情報を生成する。そして、表示情報生成部１０５は、生成した表示情報を表示部１１に出力する。表示部１１は車内に搭載されたディスプレイであり、表示情報生成部１０５から入力した表示情報を表示する。

運転処理装置１２は、画像入力部１００が入力した画像と、計測結果処理部１０４から受信した幾何情報と位置姿勢とに基づいて、アクチュエータ部１３の制御と表示部１１への表示情報の生成と出力を行う。その詳細について以下に説明する。

周囲環境取得部１０６は、画像入力部１００が入力した画像と、計測結果処理部１０４が入力した幾何情報と位置姿勢に基づいて、車体の周囲環境を表す情報（以下、周囲環境情報）を取得する。そして、周囲環境取得部１０６は、取得した周囲環境情報を運転制御部１０８に供給する。

目的地取得部１０７は、図示しないユーザインターフェースを用いて、車の目的地をユーザから取得する。そして、目的地取得部１０７は、取得した目的地を示す情報を運転制御部１０８に供給する。

運転制御部１０８は、周囲環境取得部１０６が入力した周囲環境と、目的地取得部１０７が入力した目的地に基づいて、安全に目的地に向かうためのアクチュエータ部１３への制御値の算出及び出力を行う。また、運転制御部１０８は、目的地に向けて走行中における各種情報の生成と、表示部１１への出力も行う。アクチュエータ部１３は、運転制御部１０８から受信した制御値や信号に従って車の動きを制御／駆動を行う。

図３は、情報処理装置１の主要ハードウェア構成を示す図である。情報処理装置１はＣＰＵ１５１、ＲＯＭ１５２、ＲＡＭ１５３、外部メモリ１５４、入力部１５５、表示制御部１５６、通信Ｉ／Ｆ１５７、Ｉ／Ｏ１５８、及び、これらを接続するバス１６０を有する。ＲＯＭ１５２は、ＢＩＯＳ（Basic Input/Output System）のプログラムやブートプログラムを記憶する。ＲＡＭ１３は、ＣＰＵ１５１の主記憶装置として使用される。外部メモリ１５４は、情報処理装置１が実行するＯＳ（オペレーティングシステム）や情報処理装置１が運転支援装置として機能するためのアプリケーションプログラム、並びに、学習モデルを保持する記憶装置であって、典型的にはハードディスク装置等の書き込み可能な大容量の不揮発性メモリである。入力部１５５はタッチパネルやキーボード、マウス、ロボットコントローラーであり、情報等の入力に係る処理を行う。表示制御部１５６はＣＰＵ１５１からの指示に従って表示部１１に表示すべきメニュー等の各種画像を生成し、表示部１１に向けて出力する。なお、表示部１１は液晶表示装置やプロジェクタ、ＬＥＤインジケータなど、種類は問わない。通信インターフェイス１５７は、ネットワークを介して情報通信を行うものであり、通信インターフェイスはイーサネットでもよく、ＵＳＢやシリアル通信、無線通信等種類は問わない。Ｉ／Ｏ１５８は、情報処理装置１と外部装置との接続を行う入出力部（Ｉ／Ｏ）であり、実施形態の場合、撮像部１０、運転処理装置１２が接続される。

上記構成において、情報処理装置１のメイン電源がＯＮになると、ＣＰＵ１５１はＲＯＭ１５２に格納されたブートプログラムを実行することにより、外部メモリ１５４からＯＳをＲＡＭ１５３にロードし、実行する。そして、ＣＰＵ１５１は、ＯＳの制御下にて、運転支援アプリケーションプログラムを外部メモリ１５４からＲＡＭ１５３にロードし、実行することで、情報処理装置１は運転支援装置として機能することになる。図２に示した、学習モデル保持部１０１は外部メモリ１５４により実現するものであるが、これ以外の処理部は、上記アプリケーションを実行するＣＰＵ１５１により実現されるものである。なお、図２に示す各種処理部の一部は、ＣＰＵ１５１とは独立したハードウェアで実現させても構わない。

撮像部１０は、先に説明したように、二つの撮像装置からなる。本実施形態における撮像部１０の位置姿勢とは、第一の撮像装置を基準とした座標系における６自由度のパラメータで表されるものとする。第一の撮像装置と第二の撮像装置間は固定されており、一方の撮像装置における他方への相対的な位置姿勢は校正パラメータとして、予めキャリブレーションを行って求めておき、図示しない校正パラメータ保持部に保持されている。そのため、どちらかの撮像装置の位置姿勢を算出すれば、もう一方の撮像装置の位置姿勢を求めることができる。ここでは、撮像部１０の位置姿勢とは第一の撮像装置を基準とした座標系であるとしたが、これに限るものではなく、第二の撮像装置を基準としてもよいし、二つの撮像装置とは異なる点に撮像部１０の基準座標系を設定しても良い。その場合には、基準座標系からそれぞれの撮像装置への相対的な位置姿勢変換パラメータも既知であるとする。また、自動車１００００と撮像部１０との間の位置姿勢の関係については、予めキャリブレーションによって６自由度のパラメータが得られているものとする。このとき、基準座標系を自動車１００００に対して固定しておいてもよい。故に、撮像部１０の位置姿勢を求めることは、自動車１００００の位置姿勢を求めることと等価の意味を持つ。また、先に説明した測定結果処理部１０４が決定する幾何情報も、基準となる撮像装置（実施形態では第一の撮像装置）の視点からの情報である。

＜処理＞
次に、本実施形態における処理手順について説明する。図４は、本実施形態における情報処理装置１及び運転処理装置１２を含む情報処理システムが実施する処理手順を示すフローチャートである。同図において、ステップＳ１０００～Ｓ１０５０、Ｓ１０９０が情報処理装置１（制御部１５）の処理を示し、ステップＳ１０００、Ｓ１０６０～Ｓ１０８０が運転処理装置１２（運転制御部１０８）の処理を表すことになる。

ステップＳ１０００にて、制御部１５はシステムの初期化を行う。すなわち、制御部１５は、外部メモリ１４４からＯＳ、アプリケーションプログラムをＲＡＭ１５３に読み込み実行することで、情報処理装置１を運転支援を行う装置として動作可能な状態にする。また、制御部１５は学習モデル保持部１０１（外部メモリ１５４）が保持する学習モデルをＲＡＭ１５３に読み込む。また、制御部１５は、情報処理装置１に接続された各機器（撮像部１０など）の起動やパラメータの読み込み、撮像部１０が有する二つの撮像装置の初期位置姿勢を読み込む。二つの撮像装置の内部パラメータ（焦点距離、画像中心位置、レンズ歪み等）は、事前に校正したものを用いる。さらに、運転処理装置１２の運転制御部１０８は、ユーザが指定した目的地を目的地取得部１０７から受信し、自動車１００００の目的地として設定する。

ステップＳ１０１０にて、制御部１５は撮像部１０を制御し、第一、第二の撮像装置によるシーン（画像）の撮影を行わせる。ステップＳ１０２０にて、制御部１５は画像入力部１００を制御し、第一の撮像装置と第二の撮像装置が撮像したシーンである、第一の画像と第二の画像を取得させる。

ステップＳ１０３０にて、制御部１５は幾何情報推定部１０２を制御し、第一の画像と第二の画像に基づいて、第一、第二、第三の幾何情報を推定（生成）させる。幾何情報推定部１０２は、ステレオ法を用いて２枚の画像から第一の幾何情報を生成する。具体的には、幾何情報推定部１０２は、第一の画像と第二の画像における対応関係を求めて視差画像を算出し、二つの撮像装置間の既知の校正情報に基づいて三角測量を行って距離画像を推定する。また、幾何情報推定部１０２は、第一の画像を学習モデルに入力して第二の幾何情報を推定する。同様に、幾何情報推定部１０２は、第二の画像を学習モデルに入力して第三の幾何情報を推定する。

ステップＳ１０４０にて、制御部１５は位置姿勢算出部１０３を制御し、ステップＳ１０３０で算出された第一、第二、第三の幾何情報それぞれを用いて、撮像部１０の位置姿勢を算出する。算出される位置姿勢は、第一、第二、第三の幾何情報それぞれから求められる第一、第二、第三の位置姿勢の３つである。具体的には、位置姿勢算出部１０３は前フレームの画像における各画素を、第一の幾何情報に基づいて現フレームに射影する。次に、位置姿勢算出部１０３は、前射影した前フレームの画素の画素値と現フレームの画素値との輝度差が最小となるように、Ｅｎｇｅｌらの方法（非特許文献２）により位置姿勢を求めることで、第一の位置姿勢を得る。位置姿勢算出部１０３は、第二、第三の幾何姿勢についても同様の処理を行うことで、第二の位置姿勢、及び、第三の位置姿勢を得る。また、位置姿勢算出部１０３は位置姿勢を算出した後に、さらに対応する幾何情報を更新してもよい。具体的には、位置姿勢算出部１０３は、求めた第一、第二、第三の位置姿勢に基づいて第一、第二、第三の幾何情報それぞれを現在の時刻ｔにおける幾何情報として、過去の時刻ｔ－ｉからの時系列フィルタリングによって幾何情報を更新する（非特許文献１に記載）。

ステップＳ１０５０にて、制御部１５は計測結果処理部１０４を制御し、ステップＳ１０４０で算出された第一、第二、第三の位置姿勢に基づいて、撮像部１０の最終的な位置姿勢を算出する。具体的な処理については後述する。また、制御部１５は、求めた位置姿勢に基づいて、幾何情報を更新する。

ステップＳ１０６０にて、運転制御部１０８は周囲環境取得部１０６を制御し、ステップＳ１０５０で算出された位置姿勢、更新された幾何情報、第一の画像および第二の画像に基づいて、周囲環境マップを取得させる。周囲環境マップとは、道路や車、建物などが三次元空間のどこに存在しているかを示したマップである。周囲環境取得部１０６は、幾何情報と第一の画像および第二の画像から意味的領域分割を行い、画像中の道路や車、建物などを特定する。そして、周囲環境取得部１０６は、前フレームまでに得られていた周囲環境マップ上で算出された位置姿勢に従って、幾何情報および意味的領域分割結果を用いて、周囲環境マップを更新する。

ステップＳ１０７０にて、運転制御部１０８は表示情報生成部１０５を制御し、ステップＳ１０６０で求められた周囲環境マップに基づいて、指定した視点における周囲環境をレンダリングして得た画像にユーザに必要な情報を付加した表示情報を画像として生成する。そして、運転制御部１０８は、生成した画像を、ユーザが視認できるように、表示部１１のディスプレイに表示させる。ここで、ユーザに必要な情報とは、例えば周囲環境マップと位置姿勢から求めた、周囲環境に対する自動車１００００の位置姿勢である。

ステップＳ１０８０にて、運転制御部１０８は、撮像部１０の位置姿勢と周囲環境マップと目的地に基づいて、目的地に向かって車が安全に走行するための、アクチュエータの制御値を生成する。そして、運転制御部１０８は、生成したアクチュエータの制御値を用いてアクチュエータ部１３を制御し、車を走行させる。

ステップＳ１０９０では、制御部１５は、システムを終了するか否か判定する。具体的には、不図示の入力部によりユーザが終了コマンドを入力していれば終了し、そうでなければステップＳ１０１０に戻り、上記処理を繰り返す。

＜計測結果処理部における位置姿勢を求める処理＞
図５は、本実施形態におけるステップＳ１０５０の処理、すなわち、計測結果処理部１０４の手順を示すフローチャートである。以下、同図を参照して、計測結果処理部１０４の処理内容を説明する。

ステップＳ１１００にて、計測結果処理部１０４は初期化を行う。この初期化において、計測結果処理部１０４は、第一、第二、第三の位置姿勢の読み込みや、この後の処理で必要となる前フレーム画像や、周囲環境マップの幾何情報の読み込み、また、最適化問題を解く際のパラメータなどの読み込みを行う。

ステップＳ１１１０にて、計測結果処理部１０４は、１フレーム前の画像と２フレーム前の画像における位置姿勢から、線形補間によって現フレームの位置姿勢を推定し、これを初期位置姿勢とする。ただし、初期位置姿勢はこれに限るものではなく、加速度センサやＧＰＳセンサなどから初期位置姿勢を求めてもよい。

ステップＳ１１２０にて、計測結果処理部１０４は、第一、第二、第三の位置姿勢に関する信頼度を計算する。信頼度は第一、第二、第三の位置姿勢それぞれについて信頼できる度合いを数値化したものである。信頼度が高いほど、その位置姿勢は正しい位置姿勢からの誤差が小さい可能性が高いことを示す。信頼度の求め方としては、ステップＳ１０４０にて、非特許文献２の方法により位置姿勢を求めた際に、最小化した輝度差を用いて信頼度を表すものとする。つまり、ここで言う輝度差が小さいほど信頼度が高くなる関数を用いるものとする。

ただし、信頼度は輝度差に限るものではなく、例えば、対応する幾何情報を求めた際に、非特許文献１の方法から得られる幾何情報の信頼度や、ステレオのマッチング度合いを信頼度として用いてもよい。

ステップＳ１１３０にて、計測結果処理部１０４は、第一、第二、第三の位置姿勢とそれに対応する信頼度、および初期位置姿勢に基づいて、最終出力の位置姿勢を求める。ここで、第一、第二、第三の位置姿勢をＰ１、Ｐ２、Ｐ３、初期位置姿勢をＰＩ、計測結果処理部１０４が出力する位置姿勢をＰＯとし、第一、第二、第三の位置姿勢に対応する信頼度をｃ１、ｃ２、ｃ３とする。また、関数ｆ（Ｐｉ，Ｐｊ）を位置姿勢ＰｉとＰｊの差を求める関数とする。ＰＯは、以下の式Ｅを最小化する最適化問題を解くことによって得られる。
Ｅ＝ｋ*ｆ（ＰＩ，ＰＯ）＋ｃ１*ｆ（Ｐ１，ＰＯ）＋ｃ２*ｆ（Ｐ２，ＰＯ）＋ｃ３*ｆ（Ｐ３，ＰＯ） …（１）
ここで、ｋは調整パラメータである。ただし、ＰＯの求め方はこの方法に限るものではなく、カルマンフィルタを用いてＰＩ、Ｐ１、Ｐ２、Ｐ３を入力として真の状態ＰＯを求めてもよい。

ステップＳ１１４０にて、計測結果処理部１０４は、ステップＳ１１３０で求めた位置姿勢に基づいて、幾何情報を更新する。具体的には、求めた位置姿勢に基づいて第一、第二、第三の幾何情報を現在の時刻ｔにおける幾何情報として、過去の時刻ｔ－ｉからの幾何情報との信頼度を重みとした時系列フィルタリングによって幾何情報を更新する（非特許文献１に記載）。

上記フローチャートではステップＳ１１１０の次にステップＳ１１２０を行う処理について説明したが、フローチャートは必ずしもこの順序であるとは限らない。ステップＳ１１２０の処理を先に行い、その後にステップＳ１１１０を行ってもよい。以上の処理により、計測結果処理部１０４は位置姿勢と幾何情報を求める。

＜効果＞
以上に述べたように、本第１の実施形態では、複数の計測系統から複数の幾何情報および複数の位置姿勢を推定する。それぞれの計測結果は画像を入力された後、独立して処理を行っているため、いずれかの計測結果に大きなノイズが生じていたとしても、他の計測結果に影響を及ぼすことは無い。そのため、複数の計測結果の全てが大きなノイズを生じる可能性は低く、時系列情報から予測される位置姿勢に対して誤差の少ない計測結果の重みを大きくすることによって、計測結果に何らかのノイズが紛れ込んだ場合であっても、安定的に位置姿勢を推定することができる。さらに、安定的な位置姿勢に基づいて幾何情報を更新することによって、より精度の高い幾何情報を求めることができる。

＜変形例＞
第１の実施形態においては、図４に示すフローチャートに従って処理を行う例について述べたが、これに限るものではない。ここで本実施形態における変形例の機能構成例および処理手順について説明する。変形例の機能構成例は図２に示す第１の実施形態と同様であるため、違いのある幾何情報推定部１０２、位置姿勢算出部１０３、計測結果処理部１０４についてのみ説明する。

幾何情報推定部１０２は、先に説明した第１の実施形態と同様、第一、第二、第三の幾何情報を推定し、推定した第一、第二、第三の幾何情報を計測結果処理部１０４に出力する。または、幾何情報推定部１０２は、第一、第二、第三の幾何情報をいったん位置姿勢算出部１０３に供給し、それぞれの幾何情報に対応する第一、第二、第三の位置姿勢の算出結果に応じて対応する幾何情報を更新してもよい。具体的には、求めた第一、第二、第三の位置姿勢に基づいて、第一、第二、第三の幾何情報それぞれを現在の時刻ｔにおける幾何情報として、過去の時刻ｔ－ｉからの時系列フィルタリングによって幾何情報を更新する（非特許文献１に記載）。

計測結果処理部１０４は、幾何情報推定部１０２が更新した第一、第二、第三の幾何情報に基づいて、正確な幾何情報を算出する。算出方法については後述する。また、計測結果処理部１０４は、算出した幾何情報を位置姿勢算出部１０３に出力する。また、計測結果処理部１０４は、後述する位置姿勢算出部１０３が入力した位置姿勢に基づいて、幾何情報を更新する。さらに計測結果処理部１０４は、更新した幾何情報を表示情報生成部１０５、運転処理装置１２に出力する。また、計測結果処理部１０４は、位置姿勢を表示情報生成部１０５、運転処理装置１２に出力する。

位置姿勢算出部１０３は、画像入力部１００が入力した画像と計測結果処理部１０４が入力した幾何情報とに基づいて、撮像部１０の位置姿勢を算出する。また、算出した位置姿勢を計測結果処理部１０４に入力する。

次に、本変形例の処理手順について説明する。図６は、本変形例における、情報処理装置１を含む情報処理システムが実施する処理手順を示すフローチャートである。本フローチャートの多くのステップは図４のフローチャートと同様であるため、違いのあるステップＳ１０４１、ステップＳ１０５１について説明する。

ステップＳ１０４１にて、制御部１５は計測結果処理部１０４を制御し、ステップＳ１０３０で算出された第一、第二、第三の幾何情報を用いて撮影シーンの幾何情報を算出させる。具体的には、第一、第二、第三の幾何情報はそれぞれ計測点または計測領域ごとに信頼度を保持しており、信頼度の高い点や領域に三次元点や面を成すように幾何情報を構成することで求める。詳細は後述する。

ステップＳ１０５１にて、制御部１５は位置姿勢算出部１０３を制御し、ステップＳ１０４１で算出された幾何情報に基づいて、撮像部１０の位置姿勢を算出させる。具体的には、これまでのフレームで求められている周囲環境マップにおける幾何情報に対して、計測結果処理部１１４が算出した幾何情報の位置合わせを行うことによって周囲環境マップに対する位置姿勢を算出する。また、１フレーム前の画像と２フレーム前の画像における位置姿勢から、線形補間によって現フレームの位置姿勢を求め、これを位置合わせする際の初期位置姿勢として用いる。また、位置合わせを行うことで、周囲環境マップの幾何情報も更新する。

＜計測結果処理部における幾何情報を求める処理＞
ここで、図７は、図６のステップＳ１０４１の計測結果処理部１０４の処理手順を示すフローチャートである。

ステップＳ１２００にて、計測結果処理部１０４は初期化処理を行う。すなわち、計測結果処理部１０４は、第一、第二、第三の幾何情報の読み込みや、この後の処理で必要となる前フレーム画像や、周囲環境マップの幾何情報の読み込み、また、最適化問題を解く際のパラメータなどの読み込みを行う。

ステップＳ１２１０にて、計測結果処理部１０４は、第一、第二、第三の幾何情報それぞれに関する信頼度を計算する。信頼度は第一、第二、第三の幾何情報に対して、各計測点または各計測領域に対してそれぞれ信頼できる度合いを数値化したものである。信頼度が高いほど、その計測点や計測領域の存在する三次元位置は正しい可能性が高いことを示す。信頼度の求め方としては、第一の幾何情報については、ステレオにより第一の画像と第二の画像の対応関係を求める際に、微小領域ごとの類似度を計算する。この類似度が高いほど信頼度の高い領域であるとして信頼度を求める。第二、第三の幾何情報については、非特許文献１に示される方法により、幾何情報の信頼度を得ることが出来る。

ステップＳ１２２０にて、計測結果処理部１０４は、第一、第二、第三の幾何情報とそれに対応する信頼度に基づいて、計測結果処理部１０４が出力する幾何情報を求める。具体的には各計測点または各計測領域における信頼度を重みとして、重み付きのＩＣＰ（Iterative Closest Point）を行うことによって、三つの幾何情報を統合することによって、より正しい幾何情報を推定する。また、ＩＣＰによって第一、第二、第三の幾何情報を統合した際に、幾何情報を移動回転させるために用いた変換行列を用いて、撮像部１０に対して計測結果処理部１０４が求めた幾何情報の位置姿勢を求めておく。以上の処理により、計測結果処理部１０４は幾何情報を求める。

第１の実施形態では計測結果処理部１０４は複数の位置姿勢から、位置姿勢を求めたが、上記の変形例で示したように、計測結果処理部１０４は複数の幾何情報から、幾何情報を求めてもよい。

図１の例では、画像入力部１００は一つしか描かれていないが、これに限るものではなく、二つの撮像装置に対して一つずつ画像入力部を備えていてもよいし、一つの画像入力部が二つの撮像装置から画像を受け取っても良い。同様に、幾何情報推定部１０２や位置姿勢算出部１０３を複数備えていてもよい。また、学習モデル保持部１０１は二つの撮像装置それぞれに対して別々の学習モデルを保持していてもよい。

ステップＳ１０７０にて、表示情報生成部１０５が生成する表示情報は周囲環境に対する自動車１００００の位置姿勢であるとして説明したが、これに限るものではない。例えば、撮像部１０が撮像した画像や、周囲環境マップから得られる意味的領域分割の結果を二次元画像上にレンダリングした画像などでもよい。また、目的地などの情報であってもよい。これにより、ユーザは情報処理システムが認識した自動車１００００に対する周囲の状況を視覚的に確認することができる。

ステップＳ１０９０では、システムを終了する条件として、ユーザが終了コマンドを入力する例を説明した。しかしながら、これに限るものではなく、システムを終了させる条件として、運転手であるユーザが、車の制御をシステムに委ねる自動運転モードから、人が運転するドライビングモードへ変更することとしてもよい。また、運転処理装置１２が、得られた幾何情報や位置姿勢と目的地の不整合などにより、自動運転または運転アシストを行えない状況になったと判断したときに、システムを終了するようにしてもよい。

第１の実施形態では、位置姿勢推定を車の自動運転または運転アシストのアプリケーションに適用した例を説明した。しかしながら、本実施形態で説明した情報処理装置１に適用可能なのは当該アプリケーションに限らず、学習モデルが出力した結果を含む複数の幾何情報、または位置姿勢結果を用いるものであればよい。例えば、工業用ロボットハンドの先端に取り付けてロボットハンドの位置姿勢を計測するロボットシステムとして用いてもよい。このときのロボットシステムには、ロボットアーム等のマニピュレータや、吸着ハンド等の把持装置、および計測結果処理部１０４が算出した位置姿勢に基づいてマニピュレータや把持装置を制御する制御部を備えていてもよい。また、複合現実感システムにおける現実空間と仮想物体との位置合わせとして用いてもよい。このときの複合現実感システムには、周囲環境を取得するための撮像装置が搭載されたヘッドマウントディスプレイを備えていてもよい。室内を清掃するロボット・空中を飛行するドローン・水中を移動する装置などが、上述の学習モデルを保持し学習モデルに基づいた仮の幾何情報を推定し、２つの撮像装置の画像を用いて生成される３次元の仮の幾何情報を推定しても良い。さらに、幾何情報または位置姿勢を推定して、自身の移動や周囲環境の取得に利用しても良い。

また、情報処理装置１の使用用途は位置姿勢推定に限らず、三次元再構成に用いてもよい。例えば、工業部品や建物といったＣＡＤモデルを生成するための計測システムとして用いてもよい。このときの計測システムは、計測結果処理部１０４が更新した幾何情報から三次元モデルを生成する三次元モデル生成部をさらに備えていてもよい。また 、ＲＧＢカメラや濃淡画像を取得するカメラなど、距離画像を取得することのできないカメラから高精度に距離画像を取得する装置として用いてもよい。

第１の実施形態では、車内の情報処理装置１が学習モデル保持部１０１、幾何情報推定部１０２、位置姿勢算出部１０３、計測結果処理部１０４を有する構成を説明した。しかしながら、クラウドサーバが本実施形態で示した情報処理装置１の一部の機能を有し、実行してもよい。例えば、クラウドサーバが学習モデル保持部１０１、幾何情報推定部１０２、位置姿勢算出部１０３、計測結果処理部１０４を有する構成であってもよい。この構成では、まず情報処理装置１が入力画像を不図示の通信部を用いてクラウドサーバに転送する。次に、クラウドサーバ上の学習モデル保持部１０１の保持する学習モデルを用いて幾何情報を推定する。また幾何情報推定部１０２は複数の幾何情報を推定する。そして、位置姿勢算出部１０３複数の位置姿勢を算出し、計測結果処理部１０４は安定的な位置姿勢を求め、幾何情報を更新する。そして、クラウドサーバが推定した幾何情報と位置姿勢を、通信部を用いて情報処理装置１に転送する。このような構成にすることで、情報処理装置１は計算負荷を減らすことができるため、小規模な計算機で省スペース化することができる。

本実施形態においては、画像を撮像する撮像装置がＲＧＢのビデオカメラである構成について説明した。ただし、ＲＧＢのビデオカメラに限るものではなく、現実空間の画像を撮像するカメラであれば何でもよく、たとえば濃淡画像を撮像するカメラでもあってもよいし、赤外画像や紫外画像、距離画像、三次元点群データを撮像できるカメラであってもよい。また、二台の撮像装置からなるステレオ構成として説明したが、これに限るものではなく、三台以上の複数のカメラやセンサを備えるカメラであってもよい。その場合、４つ以上の複数の幾何情報や位置姿勢を算出できるが、計測結果処理部１０４はこれら複数の計測結果に基づいて計算を行う。

［第２の実施形態］
第１の実施形態では、計測結果処理部１０４は複数の幾何情報または位置姿勢から、より正しい幾何情報または位置姿勢を算出した。これに対して、第２の実施形態では、計測結果処理部１０４は幾何情報または位置姿勢に対して判定を行い、幾何情報または位置姿勢に不具合が生じていれば、別の幾何情報または位置姿勢を用いるように切り替えを行う。このようにして、最適な幾何情報または位置姿勢を得られるセンサおよび計算結果を用いて自動運転または運転アシストを行う。本第２の実施形態における自動車の構成は第１の実施形態に示した図１と同様であるため、その説明は省略する。

＜情報処理装置の構成＞
本実施形態における情報処理装置２の機能構成例は第１の実施形態に示した図２と略同様である。異なる点は、幾何情報推定部１０２、位置姿勢算出部１０３、計測結果処理部１０４における処理内容である。よって、以下では、これらについて説明する。他の構成要素については第１の実施形態を参照されたい。

幾何情報推定部１０２は、第１の実施形態と同様に、第一、第二、第三の幾何情報を推定できる機能を有するが、図示しないパラメータ保持部が保持するパラメータに従って、第一、第二、第三の幾何情報のいずれか一つを推定して、位置姿勢算出部１０３と計測結果処理部１０４に出力する。ここでのパラメータとは、第一、第二、第三の幾何情報のうちどの幾何情報を用いるかを指示したものである。

位置姿勢算出部１０３は、画像入力部１００が入力した画像と幾何情報推定部１０２が入力した幾何情報とに基づいて、撮像部１０の位置姿勢を算出する。さらに位置姿勢算出部１０３は、算出した位置姿勢に基づいて幾何情報を更新する。そして、位置姿勢算出部１０３は、算出した位置姿勢および更新した幾何情報を計測結果処理部１０４に供給する。

計測結果処理部１０４は、位置姿勢算出部１０３から入力した位置姿勢における不具合の有無を判定する。計測結果処理部１０４は、位置姿勢に不具合が無いと判定した場合、幾何情報および位置姿勢を表示情報生成部１０５、運転処理装置１２に供給する。また、位置姿勢に不具合があった場合には、計測結果処理部１０４は、図示しないパラメータ保持部のパラメータを変更して、幾何情報推定部１０２に別の幾何情報を推定させ、さらに、位置姿勢算出部１０３にも、新たに推定した幾何情報に基づいて位置姿勢を算出させて、それらの不具合の有無を判定することを繰り返し、不具合の無いと判定された情報を出力対象として決定する。

＜処理＞
次に、本第２の実施形態における処理手順について説明する。図８は、第２の実施形態における情報処理装置１を含む情報処理システムが実施する処理手順を示すフローチャートである。

本実施形態におけるフローチャートは、第１の実施形態における図４のフローチャートと略同様であり、ステップＳ２０００、ステップＳ２０１０、ステップＳ２０２０、ステッステップＳ２０６０、ステップＳ２０７０、ステップＳ２０８０、ステップＳ２０９０はそれぞれステップＳ１０００、ステップＳ１０１０、ステップＳ１０２０、ステップＳ１０６０、ステップＳ１０７０、ステップＳ１０８０、ステップＳ１０９０と同様の処理であるため説明を省略し、違いのあるステップＳ２０３０、ステップＳ２０４０、ステップＳ２０５０について説明を行う。

ステップＳ２０３０にて、制御部１５が幾何情報推定部１０２を制御し、第一の画像と第二の画像に基づいて、第一、第二、第三の幾何情報のうち、指定されたいずれか一つの幾何情報を推定する。なお、それぞれの幾何情報の求め方についてはステップＳ１０３０で説明した処理と同様である。

ステップＳ２０４０にて、制御部１５は位置姿勢算出部１０３を制御し、ステップＳ２０３０で算出された幾何情報を用いて撮像部１０の位置姿勢を算出する。また、位置姿勢算出部１０３は位置姿勢を算出した後に、さらに対応する幾何情報を更新してもよい。

ステップＳ２０５０では、制御部１５は計測結果処理部１０４を制御し、ステップＳ２０４０で算出された位置姿勢における不具合の有無を判定する。不具合が無かった場合はステップＳ２０６０に処理を進める。不具合があった場合には、ステップＳ２０３０に戻って別の幾何情報の推定からやり直す。ここで不具合の有無を判定する具体的な処理については後述する。

＜計測結果処理部における位置姿勢の不具合の有無を判定する処理＞
ここで、図９は、本第２の実施形態におけるステップＳ２０５０において計測結果処理部１０４が位置姿勢の不具合の有無を判定する手順を示すフローチャートである。

ステップＳ２１００にて、計測結果処理部１０４は初期化を行う。すなわち、位置姿勢の読み込みや、この後の処理で必要となる前フレーム画像や、周囲環境マップの幾何情報の読み込み、信頼度や類似度の閾値のパラメータの読み込みなどである。

ステップＳ２１１０では、計測結果処理部１０４は位置姿勢に関する信頼度を計算する。ここでは第一、第二、第三の位置姿勢のいずれか入力された位置姿勢について信頼度を求める。信頼度の求め方については、第１の実施形態におけるステップＳ１１２０で既に説明したので省略する。

ステップＳ２１２０にて、計測結果処理部１０４は、ステップＳ２１１０で求めた信頼度が閾値Ｔｈｐよりも大きいかどうかを確認する。信頼度がＴｈｐよりも大きい場合にはステップＳ２１３０に進む。信頼度がＴｈｐ以下の場合には不具合有りと判定して処理を終了する。

ステップＳ２１３０にて、計測結果処理部１０４は、１フレーム前の画像と２フレーム前の画像における位置姿勢から、線形補間によって現フレームの位置姿勢を求め、これを初期位置姿勢とする。ただし、初期位置姿勢はこれに限るものではなく、加速度センサやＧＰＳセンサなどから初期位置姿勢を求めてもよい。

ステップＳ２１４０にて、計測結果処理部１０４は、ステップＳ２１３０で求めた初期位置姿勢とステップＳ２０４０で算出した位置姿勢との類似度を計算する。ここで、二つの位置姿勢の類似度とは、二つの位置姿勢が似ているほど高く、遠く離れて向きも異なるほど低い。例えば、類似度Ｓは次式（２）で求められる。
Ｓ＝ｋ１*｜ｔｉ－ｔ｜＋ｋ２*｜｜ｑｉ－ｑ｜｜ …（２）
ここで、ｋ１、ｋ２は調整パラメータであり、ｔｉとｔはそれぞれ初期位置姿勢と位置姿勢の位置を表す３次元のベクトル、ｑｉとｑはそれぞれ初期位置姿勢と位置姿勢の姿勢を表す四元数であり、|ｘ|は実数３次元ベクトルｘのノルムを表しており、||ｙ||は四元数ｙのノルムを表している。ここでは、類似度の計算方法として式（２）について述べたが、これに限るものではない。位置姿勢が似ているほど高く、異なるほど低い値になるように計算されるものであれば、類似度はどのような計算式で求めてもよい。

ステップＳ２１５０にて、計測結果処理部１０４は、ステップＳ２１４０で求めた類似度が閾値Ｔｓｐよりも大きいかどうかを確認する。類似度がＴｓｐよりも大きい場合には不具合無しと判定して処理を終了する。類似度がＴｓｐ以下の場合には不具合有りと判定して処理を終了する。

以上の処理により、計測結果処理部１０４は位置姿勢の不具合の有無を判定する。

＜効果＞
以上に述べたように、第２の実施形態では、複数の幾何情報の中から一つを選んで幾何情報および位置姿勢の推定を行い、得られた位置姿勢の不具合の有無を判定することによって、その位置姿勢を用いるか、別の幾何情報を推定し直すかを決める。計算処理中はどれか一つの幾何情報および位置姿勢を計算するだけでよいため、第１の実施形態で説明した複数の幾何情報や複数の位置姿勢を計算する場合と比較して、同時に処理するべき計算コストが低いため、比較的小規模な計算リソースであっても処理を行うことができる。また、計算した位置姿勢の不具合の有無を判定することによって、間違った位置姿勢を用いる可能性が低くなり、自動運転や運転アシストにおける安全性が高まる。

＜変形例＞
第２の実施形態においては、図８に示すフローチャートに従って処理を行う例について述べたが、これに限るものではない。ここで本実施形態における変形例の機能構成例および処理手順について説明する。変形例の機能構成例は第２の実施形態と同様であるため、違いのある幾何情報推定部１０２、位置姿勢算出部１０３、計測結果処理部１０４についてのみ説明する。

幾何情報推定部１０２は、第２の実施形態と同様にして第一、第二、第三の幾何情報のいずれか一つを推定して、計測結果処理部１０４に出力する。または、第一、第二、第三のいずれか一つの幾何情報をいったん位置姿勢算出部１０３に供給し、幾何情報に対応する位置姿勢を算出して対応する幾何情報を更新してもよい。

計測結果処理部１０４は、幾何情報推定部１０２から入力した幾何情報における不具合の有無を判定する。幾何情報に不具合が無かった場合には、幾何情報を位置姿勢算出部１０３に出力する。また、幾何情報に不具合があった場合には、図示しないパラメータ保持部のパラメータを変更して幾何情報推定部１０２に別の幾何情報を推定させて、その幾何情報の不具合の有無を判定することを繰り返す。また、後述する位置姿勢算出部１０３が入力した位置姿勢に基づいて、幾何情報を更新する。さらに更新した幾何情報を表示情報生成部１０５、運転処理装置１２に出力する。また、位置姿勢を表示情報生成部１０５、運転処理装置１２に出力する。

位置姿勢算出部１０３は、画像入力部１００が入力した画像と計測結果処理部１０４が入力した幾何情報とに基づいて、撮像部１０の位置姿勢を算出する。また、位置姿勢算出部１０３は、算出した位置姿勢を計測結果処理部１０４に供給する。

次に、本実施形態における変形例の処理手順について説明する。ここで、図１０は、本第２の実施形態における情報処理装置１を含む情報処理システムが実施する変形例の処理手順を示すフローチャートである。本フローチャートの多くのステップは図８のフローチャートと同様であるため、違いのあるステップＳ２０４１、ステップＳ２０５１について説明する。

ステップＳ２０４１にて、計測結果処理部１０４はステップＳ２０３０で算出された幾何情報における不具合の有無を判定する。判定方法の詳細は後述する。幾何情報に不具合が無かった場合には、ステップＳ２０５１に処理を進める。また、幾何情報に不具合があった場合には、ステップＳ２０３０に戻り、幾何情報推定部１０２に別の幾何情報を推定させる。

ステップＳ２０５１にて、位置姿勢算出部１０３は、ステップＳ２０４１で算出された幾何情報に基づいて、撮像部１０の位置姿勢を算出する。具体的には、これまでのフレームで求められている周囲環境マップにおける幾何情報に対して、計測結果処理部１０４が算出した幾何情報の位置合わせを行うことによって周囲環境マップに対する位置姿勢を算出する。また、１フレーム前の画像と２フレーム前の画像における位置姿勢から、線形補間によって現フレームの位置姿勢を求め、これを位置合わせする際の初期位置姿勢として用いる。また、位置合わせを行うことで、周囲環境マップの幾何情報も更新する。

＜計測結果処理部における幾何情報の不具合の有無を判定する処理＞
ここで、図１１は、本第２の実施形態の変形例における、Ｓ２０４１の計測結果処理部１０４における幾何情報の不具合の有無を判定する手順を示すフローチャートである。

ステップＳ２２００にて、計測結果処理部１０４は初期化を行う。すなわち、幾何情報の読み込みや、この後の処理で必要となる前フレーム画像や、周囲環境マップの幾何情報の読み込み、また、最適化問題を解く際のパラメータなどの読み込みである。

ステップＳ２２１０にて、計測結果処理部１０４は、ステップＳ２０３０で求めた幾何情報に関する信頼度を計算する。計測結果処理部１０４は、第一、第二、第三の幾何情報のいずれが入力されたかに応じて、その入力された幾何情報について信頼度を求める。信頼度の求め方については、第１の実施形態における変形例の処理ステップＳ１２１０で既に説明したので省略する。

ステップＳ２２２０にて、計測結果処理部１０４は、ステップＳ２２１０で求めた信頼度付きの幾何情報と、これまでの周囲環境マップにおける幾何情報との位置合わせを重み付きＩＣＰによって行う。

ステップＳ２２３０にて、計測結果処理部１０４は、重み付きＩＣＰにおいて位置合わせの誤差が収束したかどうかを判定する。収束した場合には幾何情報には不具合無として処理を終了する。収束しなかった場合には幾何情報に不具合有として処理を終了する。以上の処理により、計測結果処理部１０４は幾何情報の不具合の有無を判定する。

以上に述べたように、第２の実施形態では計測結果処理部１０４は位置姿勢の不具合の有無を判定していたが、変形例で示したように、計測結果処理部１０４は幾何情報の不具合の有無を判定してもよい。

第２の実施形態では幾何情報推定部１０２およびステップ２０３０において、第一、第二、第三の幾何情報のいずれかを指定するパラメータをパラメータ保持部が保持しているとして説明したが、これに限るものではない。パラメータとして、第一、第二、第三の幾何情報の信頼度を保持しておき、信頼度が最も高い幾何情報を選んで推定するようにしてもよい。また、信頼度はパラメータ保持部では保持せず、前フレームまでに得られた情報から計算される信頼度を用いてもよい。

第２の実施形態およびその変形例では、図９や図１１のフローチャートを用いて、計測結果処理部１０４における位置姿勢または幾何情報の不具合の有無判定を行う方法について述べた。しなしながら、不具合の有無を判定する方法はこれらに限るものではない。例えば、第一の撮像装置および第二の撮像装置それぞれに異常が生じた場合にはエラー状態を示す信号を出すように設定してある場合、計測結果処理部は撮像装置のどちらかがエラー状態であることを示す信号を受け取った時点で、不具合有と判定し、エラー状態ではない撮像装置に基づいて幾何情報および位置姿勢を推定するように図示しないパラメータ保持部のパラメータを変更してもよい。

第２の実施形態における処理の流れを示す図８および図１０のフローチャートでは、位置姿勢または幾何情報に不具合が無いと判定するまで幾何情報の推定に戻り、運転制御まではたどり着かないような説明を行ったが、これに限るものではない。運転制御は常に行いながら、最新の情報で更新されれば、運転制御のパラメータも更新してもよい。すなわち、運転制御については別スレッドを割り当てて継続的に制御を行っておき、位置姿勢や幾何情報に不具合が無いと判定された時には、それらの情報によって位置姿勢や周囲環境マップの更新を行うことで、運転制御にも反映させるようにしてもよい。これにより、幾何情報や位置姿勢に不具合が生じている場合であっても、自動運転や運転アシスト機能が停止することなく動作を続けることができる。

第２の実施形態における処理の流れを示す図８および図１０のフローチャートでは、位置姿勢または幾何情報に不具合が無いと判定するまで幾何情報の推定に戻るため、第一、第二、第三の幾何情報および位置姿勢のすべてに不具合が生じている場合には、無限ループから抜け出せなくなる可能性があった。不具合有りの判定が３回連続で続いた場合には、画像撮像からやり直してもよい。または、表示情報生成部１０５が計測の不具合を知らせる画像を生成して表示をしてもよい。または、表示情報生成部１０５は人が運転するドライビングモードへ変更することを促すような表示をしてもよい。

第２の実施形態において、ステップＳ２０１０およびステップＳ２０２０では二つの撮像装置が第一の画像と第二の画像を撮像して入力するものとして説明したが、これに限るものではない。例えば、通常は第一の撮像装置のみを用いて第一の画像を入力して用い、以降の処理では第二の幾何情報を推定してもよい。さらに、第二の幾何情報に不具合が有ると判定された場合には、ステップＳ２０１０の画像撮像に戻り、第二の撮像装置のみ、または第一の撮像装置と第二の撮像装置両方でシーンを撮像するようにしてもよい。これにより、通常は撮像装置一つだけを用いるため、電力の消費や計算コストを抑えることができる。

第２の実施形態において、ステップＳ２０７０では、表示情報生成部１０５は不具合の有無を知らせるための表示情報を生成し、表示部１１で表示してもよい。例えば、不具合が無い場合には、どの幾何情報または位置姿勢を用いて運転制御を行っているかを表示してもよい。どの撮像装置で撮像した画像であるかを表示してもよい。また、不具合が有る場合には、第一、第二、第三の幾何情報または位置姿勢のうち、どれに不具合が有るのかを表示してもよい。または、第一の撮像装置または第二の撮像装置の異常を知らせるような表示をしてもよい。または、不具合が一定時間連続して生じるようであれば、原因として考えられる部分の点検やメンテナンスを促す表示を行ってもよい。これにより、ユーザは情報処理システムの状態を視覚的に確認することができる。

第２の実施形態では、幾何情報をいずれか一つ推定する場合について述べたが、これに限るものではない。例えば、第１の実施形態と同様にして、第一、第二、第三の幾何情報または第一、第二、第三の位置姿勢を推定し、それぞれの不具合の有無を確認してもよい。不具合の無い幾何情報または位置姿勢が複数存在する場合には、その中からランダムに一つを選んで次のステップに進んでもよいし、第１の実施形態におけるステップＳ１０５０やステップＳ１０４１のように、不具合の無い複数の幾何情報または位置姿勢から、幾何情報または位置姿勢を求めてもよい。または、不具合の有る幾何情報または位置姿勢は式１における信頼度やＩＣＰを行う際の重みを不具合の無いものよりも低く設定することによって求めてもよい。こうすることにより、不具合の有るデータを明示的に除外することで、第１の実施形態における推定結果よりもロバストな推定を行うことができる。

［第３の実施形態］
上記第２の実施形態では、計測結果処理部は幾何情報または位置姿勢における不具合の有無を判定し、不具合の無い推定結果を用いて幾何情報または位置姿勢を算出していた。これに対して、第３の実施形態では、計測結果処理部は幾何情報または位置姿勢に対して判定を行うだけでなく、不具合が有る場合には、不具合の原因の解消や、不具合の回避によって、不具合の解決を図る。このようにして、不具合が有る場合でもロバストに幾何情報または位置姿勢を推定して自動運転または運転アシストを行う。本第３の実施形態における自動車の構成は第１の実施形態に示した図１と同様であるため省略する。

＜情報処理装置の構成＞
図１２は、本第３の実施形態における情報処理装置１と運転処理装置１２の機器構成例を示す図である。情報処理装置１は、画像入力部３００、学習モデル保持部３０１、幾何情報推定部３０２、位置姿勢算出部３０３、計測結果処理部３０４、表示情報生成部３０５、校正算出部３０９、及び、装置全体の制御を司る制御部３５を備えている。また、運転処理装置１２は、周囲環境取得部３０６、目的地取得部３０７、及び、装置全体の制御を司る運転制御部３０８を備えている。画像入力部３００、学習モデル保持部３０１、幾何情報推定部３０２、位置姿勢算出部３０３、表示情報生成部３０５、周囲環境取得部３０６、目的地取得部３０７、運転制御部３０８、並びに、制御部３５は、第１の実施形態における画像入力部１００、学習モデル保持部１０１、幾何情報推定部１０２、位置姿勢算出部１０３、表示情報生成部１０５、周囲環境取得部１０６、目的地取得部１０７、運転制御部１０８、及び、制御部１５に対応する。同様の機能については説明を省略し、違いのある計測結果処理部３０４、校正算出部３０９について説明を行う。

計測結果処理部３０４は、位置姿勢算出部３０３が算出した第一、第二、第三の位置姿勢と、第一、第二、第三の幾何情報に基づいて、不具合や異常を検知し、それを解消するための動作を行う。そして、位置姿勢および幾何情報を算出し、表示情報生成部３０５、運転処理装置１２に出力する。詳細は後述する。

校正算出部３０９は、計測結果処理部３０４から校正を行う指示を受けると、第一の撮像装置と第二の撮像装置の校正を行い、校正パラメータを求める。求めた校正パラメータによって図示しない校正パラメータ保持部の校正パラメータを更新する。

＜処理＞
次に、本第３の実施形態における処理手順について説明する。図１３は、本実施形態における情報処理装置３を含む情報処理システムが実施する処理手順を示すフローチャートである。

本第３の実施形態におけるフローチャートは、第１の実施形態における図４のフローチャートと略同様であり、ステップＳ３０００、ステップＳ３０１０、ステップＳ３０２０、ステップＳ３０３０、ステップＳ３０４０、ステッステップＳ３０６０、ステップＳ３０７０、ステップＳ３０８０、ステップＳ３０９０は、それぞれステップＳ１０００、ステップＳ１０１０、ステップＳ１０２０、ステップＳ１０３０、ステップＳ１０４０、ステップＳ１０６０、ステップＳ１０７０、ステップＳ１０８０、ステップＳ１０９０と同様の処理であるため説明を省略し、違いのあるステップＳ３０５０、ステップＳ３０５１について説明を行う。

ステップＳ３０５０にて、計測結果処理部３０４は、ステップＳ３０４０で算出された第一、第二、第三の位置姿勢および第一、第二、第三の幾何情報における不具合の有無をそれぞれ判定する。不具合が一つも無かった場合にはステップＳ３０６０に進む。不具合があった場合には、ステップＳ３０５１へ進む。不具合の有無を判定する具体的な処理については、第２の実施形態における図９および図１１のフローチャートを用いて既に述べたため省略する。

ステップＳ３０５１では、計測結果処理部３０４は、第一、第二、第三の位置姿勢および第一、第二、第三の幾何情報において不具合が有ったものに基づいて、その不具合を解消するための処理を行う（詳細後述）。そして、計測結果処理部３０４は、処理をステップＳ３０１０に戻る。

なお、不具合を解消する間の運転制御については、人が運転するドライビングモードに切り替えて自動運転または運転アシストは行わないようにする。ただし、これに限るものではなく、不具合を解消するまでの時間が比較的短く、これまでの推定結果のままでも問題ない場合であれば、これまでの推定結果および時系列的な予測に基づいて運転制御を行ってもよい。例えば、周囲に障害物等がなく直線一本道を走行中で、その状態が続くことが推定可能である場合は、自動運転または運転アシストを行ってもよい。

＜計測結果処理部が不具合を解消する処理＞
ここで、図１４は、本第３の実施形態におけるステップＳ３０５１における、計測結果処理部３０４が不具合を解消する手順を示すフローチャートである。

ステップＳ３１００にて、計測結果処理部３０４は、第一、第二、第三の位置姿勢および第一、第二、第三の幾何情報において不具合の有ったものはどれなのかを示す情報を取得する。

ステップＳ３１１０にて、計測結果処理部３０４は、入力された情報の分類を行う。分類方法の表記として、以下では、第一の位置姿勢または幾何情報に不具合があった場合は第一：×、不具合が無かった場合には第一：○などと表記するものとする。例えば、第一：○、第二：×、第三：×と表記した場合、第一の位置姿勢および幾何情報には不具合は無く、第二の位置姿勢または幾何情報に不具合が有り、第三の位置姿勢または幾何情報に不具合が有ることを示す。不具合の分類により、第一：○、第二：×、第三：×の場合、計測結果処理部３０４はステップＳ３１１１に処理を進める。また、第一：○、第二：○、第三：×または第一：○、第二：×、第三：○の場合、計測結果処理部３０４は処理をステップＳ３１１２に進める。また、第一：×、第二：○、第三：○の場合、計測結果処理部３０４はステップＳ３１１３に処理を進める。また、第一：×、第二：○、第三：×または第一：×、第二：×、第三：○の場合、計測結果処理部３０４はステップＳ３１１４に処理を進める。そして、第一：×、第二：×、第三：×の場合、計測結果処理部３０４はステップＳ３１１５に処理を進める。

ステップＳ３１１１にて、計測結果処理部３０４は、学習モデル保持部３０１が保持する学習モデルの変更を行うことで、幾何情報推定部３０２における第二、第三の幾何情報の推定を改善させる。変更する学習モデルは撮像シーンと類似したシーンで学習を行ったモデルが好ましい。変更方法としては、撮像シーンの画像から特徴量を抽出し、抽出した特徴量に基づいて学習モデルを決定してもよいし、現在の自動車の位置に基づいて、他の自動車が過去に同様の位置で変更した経験から学習モデルを選択してもよい。

ステップＳ３１１２にて、計測結果処理部３０４は、第二、第三の幾何情報を推定する際に用いる学習モデル保持部３０１が保持する学習モデルそれぞれを統一することで、幾何情報推定部３０２における第二、第三の幾何情報いずれか一方の推定を改善する。統一する学習モデルとして採用するのは、不具合の無い幾何情報を推定するのに用いた学習モデルである。ただし、これに限るものではなく、一方の学習モデルに他方の学習モデルを統一するのではなく、ステップＳ３１１１と同様に両方の学習モデルを変更してもよい。

ステップＳ３１１３にて、計測結果処理部３０４は、校正算出部３０９が撮像した時点での第一の撮像装置と第二の撮像装置の間の校正パラメータを算出する。校正パラメータは第二の位置姿勢と第三の位置姿勢に基づいて、一方から他方への変換を求めることで算出する。ただし、算出方法はこれに限るものではなく、第一の画像と第二の画像に基づいて算出してもよいし、第二の幾何情報と第三の幾何情報に基づいて算出してもよい。そして、計測結果処理部３０４はステップＳ３１１６に処理を進める。

ステップＳ３１１６にて、計測結果処理部３０４は、ステップＳ３１１３で算出した校正パラメータと図示しない校正パラメータ保持部が保持する校正パラメータを比較して、その誤差を求める。校正誤差が大きく、第一の画像と第二の画像が撮像するシーンが重なる領域が画像中の半分の領域にも満たない場合、計測結果処理部３０４はステップＳ３１１７に処理を進める。校正誤差は存在するが、その誤差が小さい場合、計測結果処理部３０４はステップＳ３１１８に処理を進める。校正誤差はほぼゼロで算出した校正パラメータと校正パラメータ保持部が保持する校正パラメータがほぼ同一であると見なせる場合、計測結果処理部３０４はステップＳ３１１９に処理を進める。

ステップＳ３１１７にて、表示情報生成部３０５は、第一の撮像装置と第二の撮像装置の少なくとも一つを適切な位置姿勢に設置し直すように、ユーザに指示を行うための画像を生成し、表示部１１を用いて表示する。具体的には、車内における第一の撮像装置と第二の撮像装置それぞれの位置姿勢を求めて、予め設定してある撮像装置の理想的な位置姿勢の範囲に入るように、ユーザが撮像装置を動かす。この時、表示情報生成部３０５は、理想的な撮像装置の位置姿勢と、現在の撮像装置の位置姿勢とがどの程度一致しているか、また、どのように動かせば理想的な位置姿勢に近づくかを示す画像を生成し、表示部３１を用いて表示する。ユーザが撮像装置の設置を終えると、校正算出部３０９は現在の校正パラメータを算出し、これを図示しない校正パラメータ保持部の保持する校正パラメータとして更新する。このようにして、ユーザが撮像装置を適切な位置姿勢に設置し直すことによって、幾何情報推定部３０２における第一の幾何情報の推定を改善する。また、車内における撮像装置の位置姿勢を安定して求めるために、車内で計測した学習データによって学習した学習モデルを予め準備しておき、学習モデル保持部３０１が保持する学習モデルをこれに変更してもよい。

ステップＳ３１１８にて、計測結果処理部３０４は、ステップＳ３１１３で算出した校正パラメータを用いて、図示しない校正パラメータ保持部が保持する校正パラメータを更新する。これにより、幾何情報推定部３０２における第一の幾何情報の推定を改善する。

ステップＳ３１１９にて、幾何情報推定部３０２が第一の幾何情報を推定するために、第一の画像と第二の画像の対応関係を求める際に用いる特徴点の検出方法を変更する。これにより、幾何情報推定部３０２における第一の幾何情報の推定を改善する。特徴点の検出方法としては、ＳＩＦＴやＯＲＢ、ＡＫＡＺＥなど様々な特徴量が存在するが、特に限定するものではない。また、ここでは特徴点の検出方法を変更するものとして説明したが、これに限るものではない。例えば、ステレオの対応関係を求める類似度指標をＳＡＤやＳＳＤ、ＮＣＣ、ＺＮＣＣなどの中で変更を行うことでステレオによる幾何情報推定を改善してもよいし、探索範囲や閾値などを変更してもよい。

ステップＳ３１１４にて、計測結果処理部３０４は、撮像部１０のうち不具合の有る撮像装置の再起動を行うことで撮像装置自体の改善を行う。また、第一の撮像装置および第二の撮像装置それぞれが撮像した第一の画像と第二の画像に不具合の有無をユーザが確認できるように、表示情報生成部３０５は第一の画像と第二の画像を並べて表示するための画像を生成し、表示部１１を用いて表示する。ここでは、撮像装置の再起動によって撮像装置の改善を行う例について説明したが、これに限るものではない。例えば、撮像装置が壊れて撮像できない場合には、撮像装置の交換を行うために、表示情報生成部３０５は撮像装置の交換を促す表示を行ってもよい。また、撮像装置のレンズに異常が生じている場合には、表示情報生成部３０５はレンズの交換やクリーニングを促す表示を行ってもよい。

ステップＳ３１１５にて、表示情報生成部３０５は自動運転や運転アシスト機能を行えない状況にあること、および人が運転するドライビングモードに切り替えることを示す画像を生成して、表示部１１を用いて表示する。さらに、人が運転している間に、ステップＳ３１１１、ステップＳ３１１２、ステップＳ３１１４、ステップＳ３１１７、ステップＳ３１１８、ステップＳ３１１９などの改善策を実行することで、システムの復旧を図る。以上の処理により、計測結果処理部３０４は不具合を解消する。

＜効果＞
以上に述べたように、第３の実施形態によれば、推定した幾何情報や位置姿勢に不具合が有った場合には、それを解消するための対処を行うことによって、より安全かつ安定してシステムを連続的に動作させることができる。また、不具合の状況を表示してユーザに知らせることによって、ユーザは安心して自動運転や運転アシストの機能を用いることができる。

＜変形例＞
第３の実施形態においては、図１３に示すフローチャートに従って処理を行う例について述べたが、これに限るものではない。第３の実施形態ではステップＳ３０４０で位置姿勢を算出した後で、ステップＳ３０５０で不具合の有無を判定して、その対処を行う例について述べたが、第１の実施形態の変形例や第２の実施形態の変形例で示した例と同様にして、ステップＳ３０３０で算出した幾何情報を用いて不具合の有無を判定し、その対処を行い、その後で位置姿勢を算出してもよい。

第３の実施形態において、ステップＳ３０５０では、不具合の有無に応じてステップＳ３０６０またはステップＳ３０５１のどちらかに進む例について述べた。しかしながら、これに限るものではない。例えば、不具合が有った場合でも、第一、第二、第三の幾何情報および位置姿勢のいずれかに不具合が無い場合には、処理を二つに分け、ステップＳ３０５１で不具合への対処を行うとともに、不具合の無い幾何情報および位置姿勢を用いてステップＳ３０６０の処理を行ってもよい。これにより、不具合が有った場合でも、継続して自動運転または運転アシストの機能を使うことができる。

第３の実施形態において、ステップＳ３０５１では、不具合があった場合にはすぐにその対処を行う例について述べた。しかしながら、これに限るものではない。例えば、第一の撮像装置の前を遮蔽物が横切った場合には、第一、第二の幾何情報に不具合が有ると判定されるが、遮蔽物が無くなれば自然と不具合は改善される場合もある。そのため、不具合が数フレームに渡って連続的に生じた場合に対処を行うようにしてもよい。これにより、不要な計算負荷の増大を防ぐことができる。

［第４の実施形態］
第１、第２、第３の実施形態における幾何情報推定部は、既存の学習モデルを用いて幾何情報を推定する例について述べた。これに対して、第４の実施形態では、学習モデルを学習するための教師データを取得して学習を行い、学習モデルを生成する。このようにして、既存の学習モデルでは幾何情報を推定できないような未知のシーンに対しても、幾何情報の推定が可能となる新たな学習モデルを生成して自動運転または運転アシストを行う。本第４の実施形態における自動車の構成は第１の実施形態に示した図１と同様であるため省略する。
＜情報処理装置の構成＞
図１５は本実施形態における情報処理装置１と運転処理装置１２の機器構成例を示す図である。情報処理装置１は、画像入力部４００、学習モデル保持部４０１、幾何情報推定部４０２、位置姿勢算出部４０３、計測結果処理部４０４、表示情報生成部４０５、学習部４０９、並びに制御部２５を備えている。また、運転処理装置１２は、周囲環境取得部４０６、目的地取得部４０７、運転制御部４０８を備えている。画像入力部４００、学習モデル保持部４０１、幾何情報推定部４０２、位置姿勢算出部４０３、計測結果処理部４０４、表示情報生成部４０５、周囲環境取得部４０６、目的地取得部４０７、運転制御部４０８、アクチュエータ部４３、及び、制御部４５は、第３の実施形態における画像入力部３００、学習モデル保持部３０１、幾何情報推定部３０２、位置姿勢算出部３０３、計測結果処理部３０４、表示情報生成部３０５、周囲環境取得部３０６、目的地取得部３０７、運転制御部３０８、アクチュエータ部３３、及び、制御部３５に対応する。同様の機能については説明を省略し、違いのある学習モデル保持部４０１、計測結果処理部４０４、学習部４０９について説明を行う。

学習モデル保持部４０１は、画像から幾何情報を推定するためのモデルを保持する。また、学習モデル保持部４０１は、後述する学習部４０９から供給された学習モデルを保持する。これにより、幾何情報推定部４０２が状況に応じて適切な学習モデルを選択できるようになる。

計測結果処理部４０４は、第３の実施形態における計測結果処理部３０４の機能に加えて、第一の画像および第二の画像と、それらの画像に対応する幾何情報に基づいて、学習モデルを学習する際に用いる教師データを生成する。そして、計測結果処理部４０４は、生成した教師データは学習部４０９に出力する。

学習部４０９は、計測結果処理部４０４から入力した教師データに基づいて学習を行い、学習モデルを生成する。生成した学習モデルは学習モデル保持部４０１に出力する。

＜処理＞
次に、本第４の実施形態における処理手順について説明する。本第４の実施形態における処理手順は、第３の実施形態において図１３を用いて説明したフローチャートに一部処理を追加するだけで、その他は同様であるため、説明を省略する。本第４の実施形態において教師データを取得して学習を行う処理手順は、第３の実施形態における図１３のステップＳ３０６０の後に並行して行われる。ここで、図１６は、この教師データを取得して学習を行う処理手順を示すフローチャートである。

ステップＳ４１００にて、計測結果処理部４０４は、第一の画像と第二の画像を取得し、取得リストに保持する。ただし、これに限るものではなく、どちらか一方の画像のみを取得、保持してもよい。

ステップＳ４１１０にて、計測結果処理部４０４は、位置姿勢を取得し、既に取得リストに保持してある画像と対応付けて取得リストに保持する。ただし、ここでの位置姿勢とは、計測結果処理部４０４が最終的に算出した位置姿勢であり、運転制御に用いられる位置姿勢である。

ステップＳ４１２０にて、計測結果処理部４０４は、取得リストに保持された画像に対応する幾何情報を取得可能であるかどうか判定する。取得可能であると判定した場合、計測結果処理部４０４は処理をステップＳ４１３０へ進める。また、取得不可能である場合には、計測結果処理部４０４は、取得リストは保持したままで処理を終了する。この場合、次のフレームまたは数フレーム後で、教師データを取得する処理を再度開始した際には、保持した取得リストに対してさらに追加していくことになる。ここで、幾何情報を取得可能であるとは、画像に対して更新した幾何情報が得られる場合である。

ステップＳ４１３０にて、計測結果処理部４０４は、取得リストに保持された画像および位置姿勢に基づいて、画像に対応する幾何情報を取得する。

ステップＳ４１４０にて、計測結果処理部４０４は、ステップＳ４１３０で対応付けた画像と幾何情報をセットにし、教師データとして学習部４０９に出力する。学習部４０９は計測結果処理部４０４から入力した教師データを保持する。

ステップＳ４１５０にて、学習部４０９は、学習を開始するかどうか判断し、学習を開始する場合にはステップＳ４１６０に処理を進める。学習を開始しない場合には処理を終了する。この場合、教師データを取得する処理を再度開始した際には、教師データを追加で保持していく。また、学習を開始するかどうかの判断は、保持する教師データの量が十分かどうかで判断する。

ステップＳ４１６０にて、学習部４０９は、保持する教師データ群を用いて学習を行い、学習モデルを生成する。具体的には、学習部４０９は、教師データは画像と幾何情報のペアとして与えられ、入力として画像を与えたときに出力として画像に対応する幾何情報を出力するようなＣＮＮの重みを推定することで学習を行う。ただし、学習方法はこれに限るものではなく、ＣＮＮ以外のモデルであってもよい。

ステップＳ４１７０にて、学習モデル保持部４０１は、ステップＳ４１６０で学習部４０９が生成した学習モデルを追加して保持する。追加して保持することによって、幾何情報推定部４０２が幾何情報を推定する際には、複数の学習モデルの中から学習モデルを選択して幾何情報を推定することができる。ただし、これに限るものではなく、学習モデル保持部４０１は学習モデルを新しい学習モデルに更新してもよい。以上の処理により、教師データを取得して学習を行う。

＜効果＞
以上に述べたように第４の実施形態によれば、学習モデルを生成することによって、既存の学習モデルでは幾何情報を推定できなかったシーンに対して、生成した学習モデルを用いて幾何情報を推定し、安定した位置姿勢の推定を行い、より安全に自動運転または運転アシストを行うことができる。また、本情報処理装置が処理を行う中で教師データを自動的に生成することによって、学習モデルを学習するために必要となる膨大な教師データを別途作成したり、ラベル付けを行ったりする手間を省くことができる。

＜変形例＞
第４の実施形態において、計測結果処理部４０４は第３の実施形態における図１３のステップＳ３０６０の後に教師データを取得する例について述べたが、これに限るものではない。例えば、第３の実施形態における図１４のステップＳ３１１１の前後もしくは並行して画像を取得リストに保持してもよい。すなわち、学習モデルを用いて幾何情報を推定する第二、第三の幾何情報、およびそれらに対応する第二、第三の位置姿勢に不具合が生じている場合に限定して、画像を取得リストに保存してもよい。これにより、既存の学習モデルでは幾何情報を推定できないシーンの画像群を取得できるため、それらに基づいて生成した教師データで学習を行うことによって、より効率良く幾何情報を推定できるシーンを拡大することができる。

第４の実施形態において、計測結果処理部４０４は第３の実施形態における図１３のステップＳ３０６０の後に学習を行う例について述べたが、これに限るものではない。例えば、学習を行うために必要となる画像や位置姿勢、幾何情報などは全てデータとして保存しておき、別途オフラインで学習を行うための情報処理装置を用いて教師データを生成し、学習を行ってもよい。この場合、学習部４０９は情報処理装置１とは別の情報処理装置にその機能を移動させてもよい。これにより、学習を行うための計算リソースを自動車内部に所持する必要がなくなり、情報処理装置４１コストを抑えることができる。

第４の実施形態において、学習部４０９は計測結果処理部４０４が生成した教師データのみを用いて学習を行う例について述べたが、これに限るものではない。例えば、既存の学習モデルを学習した時に用いた教師データ群の一部または全てを加えて学習を行ってもよい。また、既存の学習モデルをベースとして、追加学習を行ってもよい。

第４の実施形態において、ステップＳ４１２０では画像に対して更新した幾何情報が得られる場合に幾何情報が取得可能であるとして説明したが、これに限るものではない。例えば、幾何情報が取得可能である条件として、画像に対して得られている幾何情報の割合が閾値以上としてもよい。または、画像に対応する一定範囲内のエリアから外に車が出た時に、幾何情報を更新した後に取得可能としてもよい。

第４の実施形態において、ステップＳ４１００での画像の取得やステップＳ４１３０での幾何情報の取得は、いずれも自車が計測したものであり、それらを用いて教師データを生成する例について述べたが、これに限るものではない。例えば、サーバー上から必要となるエリアの画像または幾何情報をダウンロードして、それらを用いて教師データを生成してもよい。

第４の実施形態において、幾何情報推定部４０２は学習モデル保持部４０１が保持する複数の学習モデルの中から状況に応じて適切な学習モデルを選択するとして説明したが、その具体例について述べる。学習モデルは予め、学習モデルに対応する教師データの一部の画像をサンプルシーン画像として紐付けておく。幾何情報推定部４０２は入力された画像とサンプルシーン画像との類似度を評価し、評価値の高いサンプルシーン画像に紐付いた学習モデルを用いて幾何情報の推定を行う。また、一つの学習モデルに対して複数のサンプルシーン画像が紐付けられていてもよい。

第４の実施形態において、幾何情報推定部４０２は学習モデル保持部４０１が保持する複数の学習モデルの中から状況に応じて適切な学習モデルを選択するとして説明したが、これに限るものではない。例えば、複数の学習モデルに対して、それぞれ幾何情報を生成して、第四、第五の幾何情報をさらに生成して後の処理を行ってもよい。

［第５の実施形態］
第１、第２、第３、第４の実施形態では、三角測量に基づく幾何情報を推定する方法として、撮像装置を二つ用いたステレオによる推定方法について述べた。これに対して、第５の実施形態では、撮像部が撮像する空間に向けて、所定のパターンを投影する投影部を備え、撮像装置一つと投影部によるアクティブステレオによって幾何情報を推定する。本実施形態における自動車の構成を図１７に示す。本第５の実施形態では、自動車５００００に取り付けられた撮像部５０の位置姿勢を求めるために、投影部５４と撮像部５０によるアクティブステレオによる幾何情報と、第１の実施形態で既に説明した学習モデルを用いて推定する幾何情報の２系統の幾何情報と、それに対応する位置姿勢から、安定的な幾何情報および位置姿勢を求める。
＜情報処理装置の構成＞
図１８は、本第５の実施形態における情報処理装置５と運転処理装置５２の機能構成例を示す図である。情報処理装置５は、画像入力部５００、学習モデル保持部５０１、幾何情報推定部５０２、位置姿勢算出部５０３、計測結果処理部５０４、表示情報生成部５０５、投影制御部５０９、及び、装置全体の制御を司る制御部５５を備えている。また、運転処理装置５２は、周囲環境取得部５０６、目的地取得部５０７、及び装置全体の制御を司る運転制御部５０８を備えている。画像入力部５００、学習モデル保持部５０１、幾何情報推定部５０２、位置姿勢算出部５０３、計測結果処理部５０４、表示情報生成部５０５、周囲環境取得部５０６、目的地取得部５０７、運転制御部５０８、撮像部５０、表示部５１、運転処理装置５２、アクチュエータ部５３、並びに制御部５５は、第１の実施形態における画像入力部１００、学習モデル保持部１０１、幾何情報推定部１０２、位置姿勢算出部１０３、計測結果処理部１０４、表示情報生成部１０５、周囲環境取得部１０６、目的地取得部１０７、運転制御部１０８、撮像部１０、表示部１１、運転処理部１２、アクチュエータ部１３、並びに制御部１５に対応する。同様の機能については説明を省略し、違いのある投影制御部５０９、画像入力部５００、幾何情報推定部５０２について説明を行う。なお、位置姿勢算出部５０３、計測結果処理部５０４については、第１の実施形態における位置姿勢算出部１０３と計測結果処理部１０４と同様で、第１の実施形態では第一、第二、第三と３系統の幾何情報や位置姿勢を扱っていたのに対して、本実施形態では第一と第二の２系統の幾何情報と位置姿勢を扱うという違いだけであるため、説明を省略する。また、撮像部５０は撮像装置一つで構成されており、撮像部５０の撮像装置と投影部５４の間の位置姿勢関係は予めキャリブレーションによって求めておき、図示しない校正パラメータ保持部に校正パラメータが保持されているものとする。

投影制御部５０９は、撮像部の撮像空間に向けたパターン光の投影を制御し、投影部５４を用いて投影を行う。また投影制御部５０９は、投影したパターンに関する情報を幾何情報推定部５０２に出力する。

画像入力部５００は、撮像部５０の撮像装置が、パターンが投影された被写体空間の２次元画像の画像データを時系列に入力し、処理を行った後で、学習モデル保持部５０１、幾何情報推定部５０２、位置姿勢算出部５０３、表示情報生成部５０５、運転処理装置５２に出力する。画像入力部５００が行う処理は、撮像した画像に対して、投影パターンの影響を受けた第一の画像と投影パターンの影響を受けない第二の画像とに分離することである。具体的には、投影部５４が投影するパターンは赤外光とし、撮像部５０は可視光と赤外光の２チャネルを撮像できるものとする。そして、画像入力部５００は、赤外光チャネルの画像を第一の画像、可視光チャネルの画像を第二の画像として処理し、第一の画像、第二の画像を出力する。

幾何情報推定部５０２は、画像入力部５００が入力する第一の画像と投影制御部５０９が入力するパターンに関する情報に基づくアクティブステレオによって第一の幾何情報を推定する。また、幾何情報推定部５０２は、学習モデル保持部５０１が保持する学習モデルを用いて、画像入力部５００から入力した第二の画像から、第二の幾何情報を推定する。推定した第一、第二の幾何情報を位置姿勢算出部１０３に出力する。

＜処理＞
次に、本第５の実施形態における処理手順について説明する。図１９は、本第５の実施形態における情報処理装置５を含む情報処理システムが実施する処理手順を示すフローチャートである。

本第５の実施形態におけるフローチャートは、第１の実施形態における図４のフローチャートと略同様であり、ステップＳ５０４０、ステップＳ５０５０、ステッステップＳ５０６０、ステップＳ５０７０、ステップＳ５０８０、ステップＳ５０９０はそれぞれステップＳ１０４０、ステップＳ１０５０、ステップＳ１０６０、ステップＳ１０７０、ステップＳ１０８０、ステップＳ１０９０と同様の処理であるため説明を省略する。そして、違いのあるステップＳ５０００、ステップＳ５０１０、ステップＳ５０２０、ステップＳ５０３０について以下に説明する。

ステップＳ５０００にて、制御部５５はシステムの初期化を行う。第１の実施形態におけるステップＳ１０００の処理に追加して、制御部５５は、投影部５４の起動および投影制御部５０９が投影するパターンの設定を読み込みも行うことになる。

ステップＳ５０１０にて、制御部５５は投影制御部５０９を制御し、投影部５４からシーンにパターンを投影し、撮像部５０の撮像装置によるシーンの撮影を行わせ、撮像画像を画像入力部５００に出力させる。

ステップＳ５０２０にて、制御部５５は、画像入力部５００を制御し、ステップＳ５０１０で撮像された画像を取得し、第一の画像と第二の画像に分離させる。

ステップＳ５０３０にて、制御部５５は幾何情報推定部５０２を制御し、第一の画像と第二の画像それぞれに基づいて、第一、第二の幾何情報を推定させる。第一の幾何情報はアクティブステレオにより求める。具体的には、幾何情報推定部５０２は、第一の画像と投影制御部５０９が入力する投影パターンの情報に基づいて、第一の画像と投影パターンとの対応関係を求める。そして、幾何情報推定部５０２は、図示しない校正パラメータ保持部から撮像部５０の撮像装置と投影部５４の位置姿勢関係を表す校正パラメータに基づいて三角測量を行って距離画像を推定する。また、幾何情報推定部５０２は、第１の実施形態と同様に、学習モデルを用いて、第二の画像から第二の幾何情報を推定することになる。

＜効果＞
以上に述べたように第５の実施形態によれば、パターン投影によるアクティブステレオを用いて幾何情報を推定する機能を有する。そのため、特徴の少ない未知のシーンにおいても、幾何情報を安定して求めることができるため、自動運転や運転アシストにおける安全性が高まる。

＜変形例＞
第５の実施形態において、画像入力部５００は赤外光と可視光の２チャネルで分離を行う例を述べたが、第一の画像と第二の画像に分離する方法はこれに限らない。例えば、投影部５４は赤外光のランダムドットパターンを投影するものとし、撮像部５０は赤外光のモノクロ画像を撮像できるものとした場合、撮像した画像を第一の画像、ガウシアンブラーやフィルタ処理によってランダムドットパターンの影響が見えなくなるようにブラーさせた画像を第二の画像としてもよい。または、投影部５４と撮像部５０が高速に制御され、投影パターンが有る状態で撮像した画像を第一の画像、投影パターンが無い状態で撮像した画像を第二の画像としてもよい。

第５の実施形態において、画像入力部５００は第一の画像と第二の画像に分離する例について述べたが、これに限るものではない。例えば、画像を分離する処理については、幾何情報推定部５０２、位置姿勢推定部５０３、計測結果処理部５０４などの中で最初に分離する処理を行い、画像入力部５００は一つの画像を入力してもよい。

第５の実施形態において、投影制御部５０９が投影するパターンはランダムドットパターンであるとして述べたが、投影パターンはこれに限るものではない。例えば、投影パターンはグリッドパターンでもよいし、高速に投影と撮像を行えるならば、位相シフトパターンやグレイコードパターンを用いて、複数の投影パターン撮像画像から幾何情報を推定してもよい。

第５の実施形態において、ステップＳ５０５０における計算結果処理は位置姿勢算出の後に行うものとして説明したが、これに限るものではない。第１の実施形態の変形例や第２の実施形態における変形例と同様に、幾何情報推定の後に計算結果処理を行って幾何情報を一つ求め、その後で位置姿勢算出を行ってもよい。

第５の実施形態における処理手順は、第１の実施形態と略同様であると説明したが、これに限るものではない。第２の実施形態に対して、同様に処理を行うことによって、第一の幾何情報と第二の幾何情報を適切に切り替えてもよい。これにより比較的小規模な計算リソースであっても処理を行うことができる。

第５の実施形態における処理手順は、第１の実施形態と略同様であると説明したが、これに限るものではない。第３の実施形態に対して、同様に処理を行うことによって、不具合が有ったとしても、それを解消するための対処を行うことで、より安全かつ安定してシステムを連続的に動作させることができる。

第５の実施形態における処理手順は、第１の実施形態と略同様であると説明したが、これに限るものではない。第４の実施形態に対して、同様に処理を行うことによって、学習モデルを生成し、既存の学習モデルでは幾何情報を推定できなかったシーンであっても、安定した位置姿勢の推定を行い、より安全に自動運転または運転アシストを行うことができる。

第５の実施形態において、投影部５４は撮像するシーンにパターンを投影する役割のみを持つ例について述べたが、これに限るものではない。例えば、投影部５４が表示部５１の機能を兼ねて、ユーザに対する表示を投影によって行ってもよい。また、投影部５４は自動車５００００のフロントライトとして用いてもよい。または、図示しないフロントライトを用いて投影部５４の機能を兼ねてもよい。

第５の実施形態において、他の光源や他の投影パターンが存在する場合であっても、一意にパターンを特定できるように、投影部５４が投影するパターンは変調されていてもよい。これにより、近くに同じ機能を持った自動車が複数台存在している場合であっても、お互いの投影パターンが干渉を起こすことなく、安定して幾何情報を推定することができる。

第５の実施形態において、投影部５４は一つの投影装置で、撮像部５０は一つの撮像装置からなる例について述べた。ただし、これに限るものではない。例えば、投影装置が複数台、撮像装置が複数台備わっていてもよい。その場合、幾何情報や位置姿勢が３つ以上の多数求めることができる。また、多数のうち一部を用いて処理を行ってもよい。これにより、多数の幾何情報や位置姿勢を用いて推定するため、より精度が高くロバスト性も高い幾何情報や位置姿勢を求めることができる。

（その他の実施例）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１：情報処理装置、１０：撮像部、１１：表示部、１２：運転処理装置、１３：アクチュエータ部、１００：画像入力部、１０１：学習モデル保持部、１０２：幾何情報推定部、１０３：位置姿勢算出部、１０４：計測結果処理部

Claims (16)

1. 撮像視野の少なくとも一部が重複するように移動体に配置された第１の撮像装置及び第２の撮像装置における、前記第１の撮像装置が得た第１の画像、及び、前記第２の撮像装置が得た第２の画像を入力する入力手段と、
   画像と該画像を撮影したシーンの三次元形状を表す距離情報とを教師データとして用いて、入力された画像に対応する撮影したシーンの三次元形状を表す距離情報を出力するように学習済みの学習済みモデルを学習モデル保持手段から取得する取得手段と、
   前記第１の画像及び前記第２の画像から第１の撮影したシーンの三次元形状を表す距離情報を推定する第１の推定手段と、
   前記第１の画像に対応する第２の撮影したシーンの三次元形状を表す距離情報を、前記学習済みモデルを用いて推定する第２の推定手段と、
   前記第２の画像に対応する第３の撮影したシーンの三次元形状を表す距離情報を、前記学習済みモデルを用いて推定する第３の推定手段と、
   前記第１、第２、第３の撮影したシーンの三次元形状を表す距離情報の少なくとも一つに基づいて、前記第１の撮像装置及び前記第２の撮像装置を含む撮像部の位置情報、当該撮像部が撮像した画像が表す撮影したシーンの三次元形状を表す距離情報の少なくとも一方を生成する生成手段と、
   を有することを特徴とする情報処理装置。
2. 前記生成手段は、
   前記第１の撮影したシーンの三次元形状を表す距離情報に基づいて前記撮像部の第１の位置情報を算出する第１の算出手段と、
   前記第２の撮影したシーンの三次元形状を表す距離情報に基づいて前記撮像部の第２の位置情報を算出する第２の算出手段と、
   前記第３の撮影したシーンの三次元形状を表す距離情報に基づいて前記撮像部の第３の位置情報を算出する第３の算出手段と、
   前記第１、第２、第３の位置情報の少なくとも一つに基づいて、前記撮像部の位置情報を出力するための処理を行う処理手段と
   を含むことを特徴とする請求項１に記載の情報処理装置。
3. 前記生成手段は、
   前記第１、第２、第３の撮影したシーンの三次元形状を表す距離情報に基づいて、前記撮像部における基準となる撮像装置で撮像した画像が表す奥行き情報を生成する
   ことを特徴とする請求項１に記載の情報処理装置。
4. 前記生成手段は、
   前記第１、第２、第３の撮影したシーンの三次元形状を表す距離情報の１つを前記撮像部の奥行き情報として出力する出力手段と、
   該出力手段で出力する撮影したシーンの三次元形状を表す距離情報の適正、非適正を判定する判定手段と、
   該判定手段が非適正であるとの判定をした場合、他の撮影したシーンの三次元形状を表す距離情報を前記出力手段の出力の対象として設定する設定手段と
   を含むことを特徴とする請求項１に記載の情報処理装置。
5. 前記生成手段は、生成した撮影したシーンの三次元形状を表す距離情報に基づいて前記撮像部の位置情報を算出するとともに、撮影したシーンの三次元形状を表す距離情報の更新を行う
   ことを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 更に、前記生成手段が生成した情報に基づいて、ユーザへの表示情報を生成する表示情報生成手段を有することを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
7. 更に、前記位置情報に基づいて、撮像部に関する異常を検出する検出手段を有する
   ことを特徴とする請求項１乃至５のいずれか１項に記載の情報処理装置。
8. 更に、前記第２、第３の撮影したシーンの三次元形状を表す距離情報に基づいて前記撮像部が有する前記第１、第２の撮像装置間の校正を行う校正算出手段を備え、
   前記検出手段が、前記第１の撮像装置と第２の撮像装置との間のキャリブレーションに異常が生じたことを検出した場合に、前記校正算出手段が校正を行う
   ことを特徴とする請求項７に記載の情報処理装置。
9. 前記第１の撮影したシーンの三次元形状を表す距離情報に異常が生じたことを前記検出手段が検出した場合、前記生成手段は、前記第２、第３の撮影したシーンの三次元形状を表す距離情報の少なくとも一つに基づいて位置情報を生成する
   ことを特徴とする請求項７に記載の情報処理装置。
10. 前記第１の撮影したシーンの三次元形状を表す距離情報に異常が生じたことを前記検出手段が検出した場合、前記生成手段は、前記第２、第３の撮影したシーンの三次元形状を表す距離情報の少なくとも一つに基づいて撮影したシーンの三次元形状を表す距離情報を生成する
    ことを特徴とする請求項７に記載の情報処理装置。
11. 更に、前記撮影したシーンの三次元形状を表す距離情報に基づいて、前記取得手段が新たに取得すべきモデルを学習する学習手段を備えることを特徴とする請求項７乃至１０のいずれか１項に記載の情報処理装置。
12. 前記第２、第３の撮影したシーンの三次元形状を表す距離情報に異常が生じたことを前記検出手段が検出した場合、前記学習手段は、学習に用いるための追加の教師データを取得することを特徴とする請求項１１に記載の情報処理装置。
13. 更に、前記生成手段が生成した情報に基づき表示情報を生成する表示情報生成手段と、
    前記表示情報を表示する表示手段と
    を有すること特徴とする請求項１乃至１２のいずれか１項に記載の情報処理装置。
14. 撮像視野の少なくとも一部が重複するように移動体に配置された第１の撮像装置及び第２の撮像装置における、前記第１の撮像装置が得た第１の画像、及び、前記第２の撮像装置が得た第２の画像を入力する入力工程と、
    画像と該画像を撮影したシーンの三次元形状を表す距離情報とを教師データとして用いて、入力された画像に対応する撮影したシーンの三次元形状を表す距離情報を出力するように学習済みの学習済みモデルを学習モデル保持手段から取得する取得する取得工程と、
    前記第１の画像及び前記第２の画像から第１の撮影したシーンの三次元形状を表す距離情報を推定する第１の推定工程と、
    前記第１の画像に対応する第２の撮影したシーンの三次元形状を表す距離情報を、前記学習済みモデルを用いて推定する第２の推定工程と、
    前記第２の画像に対応する第３の撮影したシーンの三次元形状を表す距離情報を、前記学習済みモデルを用いて推定する第３の推定工程と、
    前記第１、第２、第３の撮影したシーンの三次元形状を表す距離情報の少なくとも一つに基づいて、前記第１の撮像装置及び前記第２の撮像装置を含む撮像部の位置情報、当該撮像部が撮像した画像が表す撮影したシーンの三次元形状を表す距離情報の少なくとも一方を生成する生成工程と、
    を有することを特徴とする情報処理装置の制御方法。
15. コンピュータが読み込み実行することで、前記コンピュータを、請求項１乃至１３のいずれか１項に記載の情報処理装置として機能させるためのプログラム。
16. 請求項１乃至１３のいずれか１項に記載の情報処理装置と、
    前記情報処理装置における前記生成手段が生成した情報に基づいて前記移動体の運転を制御する運転制御手段と、
    当該運転制御手段によって動作するアクチュエータ部とを有する
    ことを特徴とする運転制御システム。

Images