ホーム > 特許ランキング > 株式会社豊田中央研究所
▶ 株式会社豊田中央研究所の特許一覧 ▶ トヨタ自動車株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024140879
(43)【公開日】2024-10-10
(54)【発明の名称】相対位置姿勢補正装置及びプログラム
(51)【国際特許分類】
G06T 7/70 20170101AFI20241003BHJP
【ＦＩ】
G06T7/70 Z
【審査請求】未請求
【請求項の数】7
【出願形態】ＯＬ
(21)【出願番号】P 2023052238
(22)【出願日】2023-03-28
(71)【出願人】
【識別番号】000003609
【氏名又は名称】株式会社豊田中央研究所
(71)【出願人】
【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
(74)【代理人】
【識別番号】110001519
【氏名又は名称】弁理士法人太陽国際特許事務所
(72)【発明者】
【氏名】渡邉 章弘
(72)【発明者】
【氏名】横山 明久
【テーマコード（参考）】
5L096
【Ｆターム（参考）】
5L096AA06
5L096BA04
5L096CA04
5L096CA05
5L096EA26
5L096EA39
5L096FA32
5L096FA67
5L096FA69
5L096FA74
5L096GA32
(57)【要約】
【課題】複数の情報収集装置の相対位置及び姿勢を補正する情報を導出する。
【解決手段】相対位置姿勢補正装置１０Ａは、物標の位置情報を含む複数の周辺環境情報を取得する取得部（１２）と、情報収集装置の位置及び姿勢を各々含むノード、及び周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報を対応付けたノード間を結ぶ辺によるグラフを生成する生成部（１６）と、辺の数が３つ以上の閉路に含まれる各辺の相対位置補正情報を合成して閉路の第１信頼度を算出する第１信頼度算出部（１８Ａ）と、第１信頼度に基づいて、各辺の相対位置補正情報の第２信頼度を算出する第２信頼度算出部（１８Ｂ）と、第２信頼度に基づいて、ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部（２２）と、を備える。
【選択図】図１
【解決手段】相対位置姿勢補正装置１０Ａは、物標の位置情報を含む複数の周辺環境情報を取得する取得部（１２）と、情報収集装置の位置及び姿勢を各々含むノード、及び周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報を対応付けたノード間を結ぶ辺によるグラフを生成する生成部（１６）と、辺の数が３つ以上の閉路に含まれる各辺の相対位置補正情報を合成して閉路の第１信頼度を算出する第１信頼度算出部（１８Ａ）と、第１信頼度に基づいて、各辺の相対位置補正情報の第２信頼度を算出する第２信頼度算出部（１８Ｂ）と、第２信頼度に基づいて、ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部（２２）と、を備える。
【選択図】図１
【特許請求の範囲】
【請求項1】
複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部と、
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部と、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部と、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部と、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部と、
を備えた相対位置姿勢補正装置。
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部と、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部と、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部と、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部と、
を備えた相対位置姿勢補正装置。
【請求項2】
前記第１信頼度算出部は、前記閉路として、前記複数地点のうちの始点を示すノードから他のノードを経由して終点を示すノードとして前記始点を示すノードに戻るまでの経路を、前記地点間の座標系を座標変換する座標変換用の閉路として設定し、前記始点のノードにより示される地点の座標系と、前記座標変換用の閉路に従って座標変換された結果の座標系との相対位置及座標系の相対姿勢の誤差が小さくなるに従って高くなるように前記座標変換用の閉路の信頼度を算出し、
前記第２信頼度算出部は、前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記座標変換用の閉路の信頼度が大きくなるに従って高くなるように前記第２信頼度を算出する
請求項１に記載の相対位置姿勢補正装置。
前記第２信頼度算出部は、前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記座標変換用の閉路の信頼度が大きくなるに従って高くなるように前記第２信頼度を算出する
請求項１に記載の相対位置姿勢補正装置。
【請求項3】
前記選択部は、前記辺の各々に対応する相対位置補正情報に対して前記第２信頼度が高いほど小さくなるように設定した重みの総和が予め定めた閾値以下の経路による相対位置補正情報を選択する
請求項１に記載の相対位置姿勢補正装置。
請求項１に記載の相対位置姿勢補正装置。
【請求項4】
前記情報収集装置は、車両の周辺を撮影する撮影装置を含み、
前記周辺環境情報は、前記撮影装置で撮影された撮影画像である
請求項１に記載の相対位置姿勢補正装置。
前記周辺環境情報は、前記撮影装置で撮影された撮影画像である
請求項１に記載の相対位置姿勢補正装置。
【請求項5】
前記複数地点の各々における情報収集装置は、時間経過前後の同じ情報収集装置である
請求項１に記載の相対位置姿勢補正装置。
請求項１に記載の相対位置姿勢補正装置。
【請求項6】
前記第２信頼度算出部で算出された前記第２信頼度に基づいて、予め定めた閾値未満の値の第２信頼度に対応する辺の相対位置補正情報及び当該辺に繋がるノードの少なくとも一方を無効化する無効化部、
をさらに備えた請求項１に記載の相対位置姿勢補正装置。
をさらに備えた請求項１に記載の相対位置姿勢補正装置。
【請求項7】
コンピュータを、
複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部、
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部、および、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部、
として機能させるためのプログラム。
複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部、
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部、および、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部、
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、相対位置姿勢補正装置及びプログラムに関する。
【背景技術】
【0002】
複数台のカメラの各々の撮影画像を用いて、各カメラの姿勢を含む外部パラメータを推定する技術が知られている。特許文献１では、複数のカメラ間で撮影領域がオーバーラップしている領域内に複数のマーカを固定的に設置し、当該複数のマーカを検出してマーカ間の相対的な位置関係に基づいて、各カメラの外部パラメータを推定する。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２０２２－１５４０７６号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、複数カメラ画像の各々に共通に含まれるマーカ等の共通の物標を対応付ける場合、対応付けた物標の対応関係に誤対応などが生じることがある。このように、物標の対応付けに誤対応が生じると、複数カメラ画像の各々から複数カメラの間における相対位置を補正する場合、想定外の局所的な値が最適解として推定される場合がある。従って、複数のマーカを検出してマーカ間の相対的な位置関係に基づいて、複数カメラの間における関係を考慮することには、改善の余地がある。
【0005】
本開示は、複数の情報収集装置による複数の周辺環境情報に共通の物標を対応付ける場合に誤対応が生じても、複数の情報収集装置の相対位置及び姿勢を補正する情報を導出することができる相対位置姿勢補正装置及びプログラムを提供する。
【課題を解決するための手段】
【0006】
本開示の第１態様は、複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部と、前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部と、前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部と、前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部と、前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部と、を備えた相対位置姿勢補正装置である。
【0007】
第２態様は、第１態様の相対位置姿勢補正装置において、前記第１信頼度算出部は、前記閉路として、前記複数地点のうちの始点を示すノードから他のノードを経由して終点を示すノードとして前記始点を示すノードに戻るまでの経路を、前記地点間の座標系を座標変換する座標変換用の閉路として設定し、前記始点のノードにより示される地点の座標系と、前記座標変換用の閉路に従って座標変換された結果の座標系との相対位置及座標系の相対姿勢の誤差が小さくなるに従って高くなるように前記座標変換用の閉路の信頼度を算出し、前記第２信頼度算出部は、前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記座標変換用の閉路の信頼度が大きくなるに従って高くなるように前記第２信頼度を算出する。
【0008】
第３態様は、第１態様の相対位置姿勢補正装置において、前記選択部は、前記辺の各々に対応する相対位置補正情報に対して前記第２信頼度が高いほど小さくなるように設定した重みの総和が予め定めた閾値以下の経路による相対位置補正情報を選択する。
【0009】
第４態様は、第１態様の相対位置姿勢補正装置において、前記情報収集装置は、車両の周辺を撮影する撮影装置を含み、前記周辺環境情報は、前記撮影装置で撮影された撮影画像である。
【0010】
第５態様は、第１態様の相対位置姿勢補正装置において、前記複数地点の各々における情報収集装置は、時間経過前後の同じ情報収集装置である。
【0011】
第６態様は、第１態様の相対位置姿勢補正装置において、前記第２信頼度算出部で算出された前記第２信頼度に基づいて、予め定めた閾値未満の値の第２信頼度に対応する辺の相対位置補正情報及び当該辺に繋がるノードの少なくとも一方を無効化する無効化部、をさらに備える。
【0012】
第７態様は、コンピュータを、複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部、前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部、前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部、前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部、および、前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部、として機能させるためのプログラムである。
【発明の効果】
【0013】
本開示によれば、複数の情報収集装置による複数の周辺環境情報に共通の物標を対応付ける場合に誤対応が生じても、複数の情報収集装置の相対位置及び姿勢を補正する情報を導出することができる、という効果が得られる。
【図面の簡単な説明】
【0014】
【図1】第１実施形態に係る相対位置姿勢補正装置の構成の一例を示すブロック図である。
【図2】第１実施形態に係る相対位置姿勢補正装置として機能するコンピュータの構成の一例を示すブロック図である。
【図3】複数の地点で周辺環境情報を収集する際の概念図である。
【図4】ローカル座標系と世界座標系の関係性を示す概念図である。
【図5】複数地点の関係性を示す概念図である。
【図6】１対１の組み合わせによる地点間の関係を示す概念図である。
【図7】エッジ及びノードの組み合わせにおける閉路の一例を示す概念図である。
【図8】ノード間の補正で経路の選択結果の一例を示す概念図である。
【図9】第１実施形態に係る相対位置姿勢推定処理プログラムの処理の流れの一例を示すフローチャートである。
【図10】複数の周辺環境の地図を統合することに関する概念図である。
【図11】第２実施形態に係る相対位置姿勢補正装置の構成の一例を示すブロック図である。
【図12】第２実施形態に係る相対位置姿勢推定処理プログラムの処理の流れの一例を示すフローチャートである。
【図13】第３実施形態に係る相対位置姿勢補正装置の構成の一例を示すブロック図である。
【図14】第３実施形態に係る相対位置姿勢推定処理プログラムの処理の流れの一例を示すフローチャートである。
【発明を実施するための形態】
【0015】
以下、図面を参照して本開示の技術を実現する実施形態を詳細に説明する。
なお、本実施形態では、作用、機能が同じ働きを担う構成要素及び処理には、全図面を通して同じ符合を付与し、重複する説明を適宜省略する場合がある。各図面は、本開示の技術を十分に理解できる程度に、概略的に示してあるに過ぎない。よって、本開示の技術は、図示例のみに限定されるものではない。また、本実施形態では、本発明と直接的に関連しない構成や周知な構成については、説明を省略する場合がある。
なお、本実施形態では、作用、機能が同じ働きを担う構成要素及び処理には、全図面を通して同じ符合を付与し、重複する説明を適宜省略する場合がある。各図面は、本開示の技術を十分に理解できる程度に、概略的に示してあるに過ぎない。よって、本開示の技術は、図示例のみに限定されるものではない。また、本実施形態では、本発明と直接的に関連しない構成や周知な構成については、説明を省略する場合がある。
【0016】
[第１実施形態]
図１は、第１実施形態に係る相対位置姿勢補正装置１０Ａの構成の一例を示すブロック図である。
図１は、第１実施形態に係る相対位置姿勢補正装置１０Ａの構成の一例を示すブロック図である。
【0017】
図１に示すように、相対位置姿勢補正装置１０Ａは、取得部１２、相対位置補正情報演算部１４、グラフ生成部１６、信頼度演算部１８、及び統合部２２を備えている。
【0018】
図２は、相対位置姿勢補正装置１０Ａとして機能するコンピュータの構成の一例を示すブロック図である。
【0019】
図２に示すように、相対位置姿勢補正装置１０Ａとして機能するコンピュータは、ＣＰＵ（Central Processing Unit）３１、ＲＡＭ（Random Access Memory）３２、およびＲＯＭ（Read Only Memory）３３を備えた汎用的なコンピュータ本体３０を含んで構成されている。ＲＯＭ３３には、相対位置姿勢推定処理プログラム３３Ｐが格納されている。コンピュータ本体３０は、入出力インタフェース（Ｉ／Ｏ）３４を備えており、ＣＰＵ３１、ＲＡＭ３２、ＲＯＭ３３、及びＩ／Ｏ３４は各々コマンド及びデータを授受可能にバス３５を介して接続されている。また、Ｉ／Ｏ３４には、移動体の一例である車両４０等に装備されたの外部装置と、有線接続通信及び無線接続通信の少なくとも一方の通信により通信する通信部３６が接続されている。
【0020】
相対位置姿勢推定処理プログラム３３Ｐは、例えば、相対位置姿勢補正装置１０Ａに予めインストールされていてもよい。また、相対位置姿勢推定処理プログラム３３Ｐは、不揮発性の記憶媒体に記憶して、又はネットワークを介して配布し、相対位置姿勢補正装置１０Ａに適宜インストールすることで実現してもよい。なお、不揮発性の記憶媒体の例としては、ＣＤ-ＲＯＭ(Compact Disc Read Only Memory)、光磁気ディスク、ＨＤＤ(Hard Disk Drive)、ＤＶＤ-ＲＯＭ(Digital Versatile Disc Read Only Memory)、フラッシュメモリ、メモリカード等が想定される。
【0021】
コンピュータ本体３０は、相対位置姿勢推定処理プログラム３３ＰがＲＯＭ３３から読み出されてＲＡＭ３２に展開され、ＲＡＭ３２に展開された相対位置姿勢推定処理プログラム３３ＰがＣＰＵ３１によって実行されることで、図１に示す相対位置姿勢補正装置１０Ａとして動作する。なお、相対位置姿勢推定処理プログラム３３Ｐは、図１に示す相対位置姿勢補正装置１０Ａの各部を実現するためのプロセスを含む。すなわち、ＣＰＵ３０は、図１に示す取得部１２、相対位置補正情報演算部１４、グラフ生成部１６、信頼度演算部１８、及び統合部２２として機能する。
【0022】
相対位置姿勢補正装置１０Ａは、車両４０から各種の情報を取得する。
本実施形態では、相対位置姿勢補正装置１０Ａが車両４０と通信して情報を授受する場合を一例として説明する。しかし、本開示の技術は、相対位置姿勢補正装置１０Ａが車両４０の外部に設置されることに限定されない。例えば、相対位置姿勢補正装置１０Ａは車両４０に搭載（すなわち車両４０の内部に設置）されてもよい。
本実施形態では、相対位置姿勢補正装置１０Ａが車両４０と通信して情報を授受する場合を一例として説明する。しかし、本開示の技術は、相対位置姿勢補正装置１０Ａが車両４０の外部に設置されることに限定されない。例えば、相対位置姿勢補正装置１０Ａは車両４０に搭載（すなわち車両４０の内部に設置）されてもよい。
【0023】
車両４０は、カメラ４２、メモリ４４及び通信部４６を備えている。通信部４６は、カメラ４２及びメモリ４４に接続されている。カメラ４２は、撮影装置の一例であり、車両４０の周辺を撮影する。なお、カメラ４２は、１台でもよいし、複数台でもよい。また、カメラ４２の撮影範囲は、車両４０から何れの方向に向いてもよく、車両４０の進行方向（前方）、横向き、後ろ向き及び全方位の少なくとも１つの方位であればよい。また、カメラ４２の画角及び解像度は、予め設定されてもよく、入力された任意の設定値に設定されてもよい。なお、カメラ４２で撮影されて得られた画像情報は、メモリ４４に記憶することが可能である。
【0024】
メモリ４４には、自車両に関する車両情報が格納される。車両情報は、自車両を識別するための自車両固有の識別情報を含む。画像情報は、識別情報と関連付けて記憶することが可能である。画像情報は、撮影した時間を示す時間情報を対応付けて記憶することが可能である。時間情報は、現在の日時を示す時計（図示省略）を備えて当該時計から取得してもよいし、通信によって外部から取得してもよい。
【0025】
通信部４６は、相対位置姿勢補正装置１０Ａへ、カメラ４２で撮影された画像を示す画像情報を送信する。
【0026】
本実施形態では、複数の車両４０の各々が走行又は停止している環境において、相対位置姿勢補正装置１０Ａが複数の車両４０の各々から情報を取得する場合を説明する。相対位置姿勢補正装置１０Ａは、車両４０に代えて、予め定めた位置に設定されている撮影画像を含む周辺環境情報を収集する固定装置（図示省略）から情報を取得してもよい。
【0027】
なお、相対位置姿勢補正装置１０Ａが、予め定めた所定範囲の車両４０に対して通信可能な通信機能を有することで、その所定範囲内に存在する複数の車両４０の各々から、情報を取得することが可能となる。これによって、相対位置姿勢補正装置１０Ａは、所定範囲内に存在する複数の車両４０の各々で撮影する撮影画像に対する座標系の相対的な位置及び姿勢を推定することが可能となる。
【0028】
また、本実施形態では、複数の車両４０の各々を区別が必要な場合は符号（Ａ、Ｂ等）を付して説明するが、区別不要の場合は車両４０と称して説明する。
車両４０及びカメラ４２、そして固定装置（図示省略）は、情報収集装置の一例である。
車両４０及びカメラ４２、そして固定装置（図示省略）は、情報収集装置の一例である。
【0029】
図１に示す相対位置姿勢補正装置１０Ａの取得部１２は、複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報として撮影画像を示す画像情報を取得する機能部である。この取得部１２は、複数の地点の各々における周辺環境情報を取得する。周辺環境情報は物標の位置、及び種類等を含み、周辺環境情報を収集する情報収集装置について少なくとも情報収集装置の座標系における位置情報を含む。取得部１２は、本開示の取得部の一例である。
【0030】
すなわち、取得部１２は、少なくとも車両４０のメモリ４４に記憶された画像情報を取得する。具体的には、複数の地点の各々に周辺環境を示す周辺環境情報を収集するセンサを備えた情報収集装置が配置され、当該複数の情報収集装置から複数の地点における周辺環境情報を取得する。周辺環境情報は物標の位置、種類などであり、周辺環境情報を収集する情報収集装置について少なくともセンサの座標系における位置情報を含む。本実施形態では、当該センサを備えた情報収集装置として車両４０を適用し、センサとしてカメラ４２を適用し、周辺環境情報として車両４０のカメラ４２により撮影された物標を含む車両４０の周辺の画像情報を適用する。すなわち、取得部１２は、複数の車両４０のカメラ４２により撮影された物標を含む車両４０の周辺の画像情報を取得する。なお、取得部１２は、画像情報に対応付けた車両情報と時間情報とを取得可能なものとする。
【0031】
相対位置補正情報演算部１４は、複数の地点の各々の間における相対位置及び姿勢を補正する相対位置補正情報を演算する機能部である。相対位置補正情報演算部１４は、取得部１２で取得された複数の周辺環境情報に基づいて、周辺環境情報を収集した地点間の情報収集装置である車両４０間におけるカメラ４２の相対位置及び姿勢を補正する相対位置補正情報の各々を演算する。本実施形態では、相対位置補正情報演算部１４は、複数の地点間の１対１の組合せの組に対して、相対的な位置及び姿勢を算出する。具体的には周辺環境情報に共通に含まれる物標を対応付けし、相対的な位置姿勢を補正する座標変換行列を算出する。
【0032】
ここで、相対位置補正情報について説明する。
図３は、１又は複数の車両４０が、複数の地点で周辺環境情報を収集する際の位置および姿勢に関する概念図である。
図３に示す異なる地点Ａｐ、Ｂｐ、Ｃｐのように、周辺環境情報を取得するカメラ４２を備えた車両４０が複数存在する場合、又は車両４０が移動した場合は、複数の地点の各々で、センサの座標系が異なる位置及び異なる向きの姿勢となる。よって、複数の地点間では相対的な位置及び姿勢の関係となる。
図３は、１又は複数の車両４０が、複数の地点で周辺環境情報を収集する際の位置および姿勢に関する概念図である。
図３に示す異なる地点Ａｐ、Ｂｐ、Ｃｐのように、周辺環境情報を取得するカメラ４２を備えた車両４０が複数存在する場合、又は車両４０が移動した場合は、複数の地点の各々で、センサの座標系が異なる位置及び異なる向きの姿勢となる。よって、複数の地点間では相対的な位置及び姿勢の関係となる。
【0033】
カメラ４２の位置及び姿勢は、車両４０にカメラ４２を固定した際の座標系により表現できる。例えば、次の（１）式に示すように、地点Ａｐでは、カメラ４２の座標系（以下、ローカル座標系という。）は、世界座標系Ｗ上で定義した原点に対応する地点Ａｐにおけるローカル座標系の原点Ａ０と、直交基底ベクトル（Ａｘ，Ａｙ，Ａｚ）で表現できる。これらをまとめて４ｘ４の行列で表現すると、行列Ａは，回転と並進を含む、同次座標系で表現した座標をローカル座標系から世界座標系へ変換する合同変換行列（剛体変換行列）で表現可能となる。
・・・（１）
【数1】
【0034】
図４は、ローカル座標系と世界座標系とは関係性を示す概念図である。
図４に示すように、カメラ４２の地点Ａｐにおけるローカル座標系と世界座標系とは関係性を有し、また、カメラ４２で撮影した画像に含まれる物標は、ローカル座標系と世界座標系とで表現可能である。これらのカメラ４２の地点Ａｐにおける座標系から世界座標系へ変換する合同変換行列ＷＴＡ、点Ａｐの座標系における物標位置（同次座標）ＡＬ、及び世界座標における物標位置（同次座標）ＷＬは、次の（２）式で表すことができる。
・・・（２）
図４に示すように、カメラ４２の地点Ａｐにおけるローカル座標系と世界座標系とは関係性を有し、また、カメラ４２で撮影した画像に含まれる物標は、ローカル座標系と世界座標系とで表現可能である。これらのカメラ４２の地点Ａｐにおける座標系から世界座標系へ変換する合同変換行列ＷＴＡ、点Ａｐの座標系における物標位置（同次座標）ＡＬ、及び世界座標における物標位置（同次座標）ＷＬは、次の（２）式で表すことができる。
【数2】
【0035】
上述した地点Ａｐのローカル座標系と地点Ｂｐのローカル座標系との間の相対位置関係は、撮影画像に含まれる共通の物標の位置により推定可能である。
【0036】
図５は、世界座標系上における地点ＡｐとＢｐの各ローカル座標系の関係性を示す概念図である。図５は、周辺環境を２次元で表現した図である。
各地点のローカル座標系による周辺環境情報は異なるので、まず、各ローカル座標系で周辺環境情報を取得し、共通する物標の対応付けを行う。具体的には、図５に示すように、各ローカル座標系に共通する物標の対応付けを行う。図に示す例では、三角図形及び四角図形の各々を対応付けている例である。
各地点のローカル座標系による周辺環境情報は異なるので、まず、各ローカル座標系で周辺環境情報を取得し、共通する物標の対応付けを行う。具体的には、図５に示すように、各ローカル座標系に共通する物標の対応付けを行う。図に示す例では、三角図形及び四角図形の各々を対応付けている例である。
【0037】
地点Ａｐのローカル座標系と地点Ｂｐのローカル座標系との間で対応付ける物標（例えばＮ個）の位置は、各地点のローカル座標系で表現可能である。
具体的には、地点Ａｐのローカル座標系では、次の（３）式で表現できる。
ＡＬｉ＝[Ａｘｉ，Ａｙｉ，Ａｚｉ，１] （ｉ＝１，・・・，Ｎ） ・・・（３）
また、地点Ｂｐのローカル座標系では、次の（４）式で表現できる。
ＢＬｉ＝[Ｂｘｉ，Ｂｙｉ，Ｂｚｉ，１] （ｉ＝１，・・・，Ｎ） ・・・（４）
具体的には、地点Ａｐのローカル座標系では、次の（３）式で表現できる。
ＡＬｉ＝[Ａｘｉ，Ａｙｉ，Ａｚｉ，１] （ｉ＝１，・・・，Ｎ） ・・・（３）
また、地点Ｂｐのローカル座標系では、次の（４）式で表現できる。
ＢＬｉ＝[Ｂｘｉ，Ｂｙｉ，Ｂｚｉ，１] （ｉ＝１，・・・，Ｎ） ・・・（４）
【0038】
各ローカル座標系で物標の位置情報に誤差が無く、対応付けに誤りが無ければ、対応付けたＮ個の物標の相対位置補正結果は一致する。しかし、物標の位置情報は誤差を含み、合同変換行列により２つのローカル座標系は一致しない場合がある。また、対応付けに誤りが含まれる場合もある。
【0039】
そこで、相対位置補正情報演算部１４（図１）は、対応付けされたＮ組の物標の座標を、地点Ａｐのローカル座標系から地点Ｂｐのローカル座標系へ変換した際の残差の和が最小となる合同変換行列を演算する。演算された合同変換行列を地点Ａｐのローカル座標系から地点Ｂｐのローカル座標系への相対位置補正行列とする。例えば、Ｎ個の物標の距離の二乗和が最小となるよう算出する合同変換行列（すなわち、相対位置補正行列）は、次の（５）式で表現可能である。
・・・（５）
【数3】
【0040】
図５には地点Ｂｐのローカル座標系に対して、地点Ａｐのローカル座標系を補正する補正結果ＣＲの一例も示している。補正結果ＣＲに示すように、地点ＡｐとＢｐの各ローカル座標系の座標軸は一致しているが、物標の位置は一致しない場合もあることを示している。
【0041】
なお、本実施形態では、物標の距離の二乗和が最小となるよう算出する場合を説明するが、本開示はこれに限定されるものではない。例えば、物標の距離が最小となるように演算する行列を用いてもよい。
【0042】
次に、グラフ生成部１６（図１）について説明する。グラフ生成部１６は、各地点間の関係をグラフ表現する機能部である。このグラフ生成部１６は、取得部１２で取得された複数の周辺環境情報、及び相対位置補正情報演算部１４で演算された相対位置補正情報に基づいて、ノードとノード間を繋ぐ辺とからなるグラフを生成する。ノードは、各地点、すなわち各車両４０が周辺環境を撮影し収集した地点での情報を示す。このノードは、車両４０が周辺環境を撮影し収集した地点の車両４０のカメラ４２の位置及び姿勢を含む装置情報の各々を含む。辺は、複数の車両４０各々が周辺環境を撮影し収集した地点間における情報の関係を示し、地点間のカメラ４２の相対位置及び姿勢を補正する相対位置補正情報が対応付けられる。すなわち、辺は、ノード間を結ぶつながりを表現したもので、以下、エッジと称して説明する。
【0043】
図６は、複数の地点の一例として、６つの地点Ａｐ、Ｂｐ、Ｃｐ、Ｄｐ、Ｅｐ、Ｆｐにおいて、１対１の組み合わせの組による地点間の関係を示す概念図である。図６では、各地点についての行列Ａ、Ｂ、Ｃ、Ｄ、Ｅ、Ｆをノードとし、地点間のエッジを相対位置補正行列ｉＴｊ（ｉ＝Ｂ～Ｆ，ｊ＝Ａ～Ｅ）で表現した図である。
【0044】
図６に示すように、グラフ生成部１６で生成するグラフは、ローカル座標系をノード（頂点［Ａ，Ｂ，Ｃ，Ｄ，Ｅ，Ｆ］）で表現し、ローカル座標系間の相対位置関係をエッジ（辺［ＡＢ，ＡＣ，ＡＤ，ＡＥ，ＡＦ，ＢＣ，ＢＤ，ＢＥ，ＢＦ，ＣＤ，ＣＥ，ＣＦ，ＤＥ，ＤＦ，ＥＦ］）で表現している。図６に示す例では全てのノード間にエッジが存在するグラフを一例として示している。なお、グラフ生成部１６で生成するグラフは、全てのノード間にエッジが存在することに限定されない。例えば、物標の対応付けが困難であって相対位置補正行列を得ることが困難な場合は、エッジを非設定としてもよい。また、地点Ａｐ、Ｂｐの間のエッジに関する詳細は後述する。
【0045】
従って、相対位置姿勢補正装置１０Ａは、取得部１２が取得した複数の周辺環境情報を用い、情報収集装置であるカメラ４２が収集した際のカメラ４２の位置及び姿勢を各々含む複数の装置情報の各々をノードとして定める。また、複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、各組の周辺環境情報を収集した地点間のカメラ４２の相対位置及び姿勢を補正する各組の相対位置補正情報の各々を、ノード間を結ぶエッジ（辺）と定める。これら複数のノードとノード間をつなぐエッジとによってグラフが生成される。
【0046】
なお、図６に示す例では、ローカル座標系をノード（頂点［Ａ，Ｂ，Ｃ，Ｄ，Ｅ，Ｆ］）で表現しているが、頂点Ａ～Ｆにより示されるノードは、ローカル座標系自体を表現することに限定されない。すなわち、各ノードは、ノード間で物標の対応付けを行うための情報の集合であって、物標の位置情報の集合と考えることも可能である。
【0047】
上述した相対位置補正情報演算部１４及びグラフ生成部１６は、本開示の生成部の一例である。
【0048】
次に、信頼度演算部１８（図１）について説明する。信頼度演算部１８は、グラフ生成部１６で生成されたグラフを用いて、各エッジに対応する相対位置補正情報の信頼度を算出する機能部である。
【0049】
図１に示すように、信頼度演算部１８は、第１信頼度演算部１８Ａ、及び第２信頼度演算部１８Ｂを備えている。第１信頼度演算部１８Ａは、グラフ生成部１６で生成されたグラフから抽出されたエッジの数が３つ以上の閉路に対して閉路に含まれるエッジの各々に対応する相対位置補正情報を合成して閉路の相対位置補正情報による補正の第１信頼度を算出する。第２信頼度演算部１８Ｂは、第１信頼度演算部１８Ａで算出された第１信頼度に基づいて、ノード間を結ぶエッジの各々に対応する相対位置補正情報による補正の第２信頼度を算出する。
【0050】
具体的には、第１信頼度演算部１８Ａは、グラフ生成部１６で生成されたグラフに含まれる閉路を抽出する。次に第１信頼度演算部１８Ａは、抽出した各閉路上の辺（エッジ）に対応する相対位置補正情報を示す相対位置補正行列（座標変換のための行列）を合成し、合成した行列を用いて閉路の始点の座標と、当該座標を合成した行列を用いて座標変換した結果の座標との残差を算出する。次に第１信頼度演算部１８Ａは、算出した残差を、閉路上の座標変換の信頼度、すなわち閉路の相対位置補正情報による補正の信頼度とし、当該閉路上の座標変換の信頼度を第１信頼度として算出する。
【0051】
次に、第２信頼度演算部１８Ｂは、各閉路の含まれるエッジに対応する信頼度を集約する統計量(平均値、中央値等)を算出する。第２信頼度演算部１８Ｂは、算出した統計量を各エッジに対応する相対位置補正情報の信頼度とし、当該各エッジに対応する相対位置補正情報の信頼度を第２信頼度として算出する。
【0052】
次に、上述した信頼度の算出についてさらに説明する。
まず、ローカル座標系をノードで表現し、ローカル座標系間の相対位置関係をエッジで表現すると（図６）、複数（例えば、３つ以上）のノードを頂点とし頂点をエッジでつなぐ複数の閉路を抽出することが可能となる。本開示では、３つ以上のエッジを有する閉路を抽出することが可能であるが、本実施形態では説明を簡単にするため、３つのエッジを有する閉路を抽出する場合を説明する。
まず、ローカル座標系をノードで表現し、ローカル座標系間の相対位置関係をエッジで表現すると（図６）、複数（例えば、３つ以上）のノードを頂点とし頂点をエッジでつなぐ複数の閉路を抽出することが可能となる。本開示では、３つ以上のエッジを有する閉路を抽出することが可能であるが、本実施形態では説明を簡単にするため、３つのエッジを有する閉路を抽出する場合を説明する。
【0053】
グラフ生成部１６で生成されたグラフにおいて、エッジの数が３つの閉路のうち、ノードの始点と終点が同じでノードが重複しない閉路は、次に示すノードの組み合わせとなる。
【0054】
（Ａ，Ｂ，Ｃ，Ａ），（Ａ，Ｂ，Ｄ，Ａ），（Ａ，Ｂ，Ｅ，Ａ），（Ａ，Ｂ，Ｆ，Ａ），
（Ａ，Ｃ，Ｄ，Ａ），（Ａ，Ｃ，Ｅ，Ａ），（Ａ，Ｃ，Ｆ，Ａ），
（Ａ，Ｄ，Ｅ，Ａ），（Ａ，Ｄ，Ｆ，Ａ），
（Ａ，Ｅ，Ｆ，Ａ），
（Ｂ，Ｃ，Ｄ，Ｂ），（Ｂ，Ｃ，Ｅ，Ｂ），（Ｂ，Ｃ，Ｆ，Ｂ），
（Ｂ，Ｄ，Ｅ，Ｂ），（Ｂ，Ｄ，Ｆ，Ｂ），
（Ｂ，Ｅ，Ｆ，Ｂ），
（Ｃ，Ｄ，Ｅ，Ｃ），（Ｃ，Ｄ，Ｆ，Ｃ），
（Ｃ，Ｅ，Ｆ，Ｃ），
（Ｄ，Ｅ，Ｆ，Ｄ），
（Ａ，Ｃ，Ｄ，Ａ），（Ａ，Ｃ，Ｅ，Ａ），（Ａ，Ｃ，Ｆ，Ａ），
（Ａ，Ｄ，Ｅ，Ａ），（Ａ，Ｄ，Ｆ，Ａ），
（Ａ，Ｅ，Ｆ，Ａ），
（Ｂ，Ｃ，Ｄ，Ｂ），（Ｂ，Ｃ，Ｅ，Ｂ），（Ｂ，Ｃ，Ｆ，Ｂ），
（Ｂ，Ｄ，Ｅ，Ｂ），（Ｂ，Ｄ，Ｆ，Ｂ），
（Ｂ，Ｅ，Ｆ，Ｂ），
（Ｃ，Ｄ，Ｅ，Ｃ），（Ｃ，Ｄ，Ｆ，Ｃ），
（Ｃ，Ｅ，Ｆ，Ｃ），
（Ｄ，Ｅ，Ｆ，Ｄ），
【0055】
図７は、エッジの数が３つのノードの組み合わせにおける閉路の一例を示す概念図である。図７では、ノードの組み合わせ（Ａ，Ｂ，Ｃ，Ａ）において、始点から終点のノードによる閉路（Ａ－Ｂ－Ｃ－Ａ）を表現している。
【0056】
第１信頼度演算部１８Ａは、閉路（Ａ－Ｂ－Ｃ－Ａ）における第１信頼度を算出する。図７に示す閉路（Ａ－Ｂ－Ｃ－Ａ）は、３つのエッジ（Ａ－Ｂ，Ｂ－Ｃ，Ｃ－Ａ）を有する。ここで、閉路（Ａ－Ｂ－Ｃ－Ａ）上のエッジ（Ａ－Ｂ，Ｂ－Ｃ，Ｃ－Ａ）に対応する合同変換行列を合成して、始点のノードに対応するローカル座標系上の物標位置を、合成した合同変換行列で変換すると、始点のノードに対応するローカル座標系上の物標位置となる。すなわち、閉路上のエッジに対応する相対位置補正情報に誤差が無ければ、始点のノードに対応するローカル座標系上の物標位置と閉路におけるノードを経由した変換後の物標の座標は一致する。始点のノードに対応するローカル座標系上の物標位置と閉路におけるノードを経由した変換後の物標の座標とは、次の式で表現可能である。
ＡＬｉ＝ＡＴＣ・ＣＴＢ・ＢＴＡ・ＡＬｉ （ｉ＝１，・・・，Ｎ）
ＡＬｉ＝ＡＴＣ・ＣＴＢ・ＢＴＡ・ＡＬｉ （ｉ＝１，・・・，Ｎ）
【0057】
一方、相対位置補正情報が誤差を有する場合、始点のノードに対応するローカル座標系上の物標位置と上述した変換後の物標の座標は位置せずに誤差に応じて座標がずれる。すなわち、誤差が大きくなるにしたがって、ずれが大きくなる。このため、座標がずれるズレ量は、閉路上の相対位置補正情報、すなわち合同変換行列の信頼度の指標として用いることができる。この段階では、閉路上のどのエッジの信頼度が低いのか高いのかを判別困難のため、閉路に対して信頼度を設定、例えば、各辺に閉路の信頼度を共通に設定する。
【0058】
始点のノードに対応するローカル座標系上の物標位置と閉路におけるノードを経由した変換後の物標の座標とが不一致であることは、次の（６）式で表現可能である。
ＡＬｉ≠ＡＴＣ・ＣＴＢ・ＢＴＡ・ＡＬｉ （ｉ＝１，・・・，Ｎ） ・・・（６）
ＡＬｉ≠ＡＴＣ・ＣＴＢ・ＢＴＡ・ＡＬｉ （ｉ＝１，・・・，Ｎ） ・・・（６）
【0059】
また、上述した不一致である場合のズレ量は、座標の残差Ｅとして次の（７）式で表現可能である。
・・・（７）
【数4】
【0060】
次に、第２信頼度演算部１８Ｂは、第１信頼度演算部１８Ａで算出した第１信頼度を用いて各エッジに対応する相対位置補正情報の信頼度により示される第２信頼度を算出する。
【0061】
まず、上述した閉路（Ａ－Ｂ－Ｃ－Ａ）上のエッジ（Ａ－Ｂ，Ｂ－Ｃ，Ｃ－Ａ）に対応する合同変換行列を合成し、物標の位置情報を、合成した合同変換行列で変換することで、当該閉路による座標変換後の座標を算出する。ここで、1つの閉路だけでは，閉路上の何れのエッジの信頼度が低いか高いかを判別することは困難である。一方、閉路に含まれるエッジの信頼度の高低は閉路の残差Ｅに影響する。例えば、図６に示す複数のノード及びエッジで表現するグラフに含まれる各閉路について、座標の残差Ｅを算出すると、エッジの信頼度の低くなるに従って当該エッジを含む閉路の残差Ｅが大きくなる。従って、エッジを含む閉路の集合の信頼度の統計量（例えば、平均値、中央値等）を算出することによって、各エッジの信頼度に対応する値を得ることが可能である。なお、残差Ｅの算出に用いる物標座標は何れの座標値でもよいが、各閉路で共通にする必要がある。例えば、正規直交基底に対応する［１，０，０］、［０，１，０］、［０，０，１］を用いることができる。
【0062】
次に、第２信頼度について、エッジＡＢに対応する合同変換行列による演算、すなわち相対位置補正情報にのみ誤差が含まれる場合を一例として説明する。なお、他のエッジに対しては誤差が含まれないものとする。また、エッジ（Ａ－Ｂ）を含む閉路で残差Ｅを含み（Ｅ＝ｅｒｒ＞０）、エッジＡＢを含まない閉路では残差Ｅを０とする。さらに、移行では、エッジ（Ａ－Ｂ）をエッジＡＢと称し、閉路（Ａ－Ｂ－Ｃ－Ａ）を閉路ＡＢＣＡと称して説明する。
【0063】
具体的には、閉路ＡＢＣＡ、ＡＢＤＡ、ＡＢＥＡ、ＡＢＦＡの残差Ｅはｅｒｒ（＞０）である。一方、他の閉路の残差Ｅは０となる。
他の閉路は、
ＡＣＤＡ、ＡＣＥＡ、ＡＣＦＡ、
ＡＤＥＡ、ＡＤＦＡ、
ＡＥＦＡ、
ＢＣＤＢ、ＢＣＥＢ、ＢＣＦＢ、
ＢＤＥＢ、ＢＤＦＢ、
ＢＥＦＢ、
ＣＤＥＣ、ＣＤＦＣ、
ＣＥＦＣ、
ＤＥＦＤ
である。
他の閉路は、
ＡＣＤＡ、ＡＣＥＡ、ＡＣＦＡ、
ＡＤＥＡ、ＡＤＦＡ、
ＡＥＦＡ、
ＢＣＤＢ、ＢＣＥＢ、ＢＣＦＢ、
ＢＤＥＢ、ＢＤＦＢ、
ＢＥＦＢ、
ＣＤＥＣ、ＣＤＦＣ、
ＣＥＦＣ、
ＤＥＦＤ
である。
【0064】
これによって、各エッジの残差Ｅの集合｛ ｝は次に示す値となる。
ＡＢ：｛ｅｒｒ，ｅｒｒ，ｅｒｒ，ｅｒｒ｝
ＡＣ：｛ｅｒｒ，０，０，０｝
ＡＤ：｛ｅｒｒ，０，０，０｝
ＡＥ：｛ｅｒｒ，０，０，０｝
ＡＦ：｛ｅｒｒ，０，０，０｝
ＢＣ：｛０，０，０，０｝
ＢＤ：｛０，０，０，０｝
ＢＥ：｛０，０，０，０｝
ＢＦ：｛０，０，０，０｝
ＣＤ：｛０，０，０，０｝
ＣＥ：｛０，０，０，０｝
ＣＦ：｛０，０，０，０｝
ＤＥ：｛０，０，０，０｝
ＤＦ：｛０，０，０，０｝
ＥＦ：｛０，０，０，０｝
ＡＢ：｛ｅｒｒ，ｅｒｒ，ｅｒｒ，ｅｒｒ｝
ＡＣ：｛ｅｒｒ，０，０，０｝
ＡＤ：｛ｅｒｒ，０，０，０｝
ＡＥ：｛ｅｒｒ，０，０，０｝
ＡＦ：｛ｅｒｒ，０，０，０｝
ＢＣ：｛０，０，０，０｝
ＢＤ：｛０，０，０，０｝
ＢＥ：｛０，０，０，０｝
ＢＦ：｛０，０，０，０｝
ＣＤ：｛０，０，０，０｝
ＣＥ：｛０，０，０，０｝
ＣＦ：｛０，０，０，０｝
ＤＥ：｛０，０，０，０｝
ＤＦ：｛０，０，０，０｝
ＥＦ：｛０，０，０，０｝
【0065】
各エッジの残差Ｅの中央値を算出すると、エッジＡＢの残差Ｅはｅｒｒ（Ｅ（ＡＢ）＝ｅｒｒ）となり、その他のエッジの残差Ｅは「０」となる。従って、誤差を含むエッジＡＢの残差Ｅが相対的に大きくなる。一方、各エッジに対応する合同変換行列による座標変換、すなわち相対位置補正情報の各々は誤差を含むと考えられるが、残差Ｅの集合である統計量を算出することで、当該統計量を相対的な信頼度、すなわち第２信頼度の指標として得ることが可能となる。
【0066】
上述した信頼度演算部１８は本開示の第１信頼度算出部及び第２信頼度算出部の一例であり、第１信頼度演算部１８Ａは本開示の第１信頼度算出部の一例で、第２信頼度演算部１８Ｂは及び第２信頼度算出部の一例である。
【0067】
次に、統合部２２（図１）について説明する。統合部２２は、第２信頼度演算部１８Ｂで算出された第２信頼度に基づいて、地点間の情報収集装置であるカメラ４２の相対位置及び姿勢を補正する相対位置補正情報について、ノード間を結ぶ経路により定まるエッジの各々による相対位置補正情報を選択する機能部である。統合部２２は、本開示の選択部の一例である。
【0068】
具体的には、統合部２２は、各エッジの第２信頼度が高いほど小さくなるような値を各エッジの重みとして設定し、例えば信頼度が高い経路、すなわち、第２信頼度で重み付けした経路の重みの総和が例えば最小値の経路を導出する。例えば、ノード間の経路上のエッジの重みの和が最小の経路に対応する座標変換行列を選択する。この経路上のエッジの重みの和は、複数経路に対応する変換結果の重みづけ平均値を用いてもよい。これによって、ノード間の最適な座標変換に対応する経路を導出することが可能となる。
【0069】
図８は、地点Ａｐのノードと地点Ｂｐのノード間の補正について、最適な経路を選択した結果の一例を示す概念図である。図８では、エッジＡＢが誤差を有し、エッジＡＢより誤差が小さい経路のエッジＡＣとエッジＣＢとの経路が選択された場合を示している。このように、統合部２２は、残差ＥがｅｒｒであるエッジＡＢを回避して、エッジＡＣとエッジＣＢとの経路を選択する。
【0070】
【0071】
まず、ステップＳ１００では、車両４０から送信される周辺環境情報を取得する。すなわち、ステップＳ１００では、上述した取得部１２が、少なくとも車両４０のメモリ４４に記憶された物標を含んで撮影された画像情報を取得する。すなわち、ステップＳ１００では、複数の車両４０のカメラ４２により撮影された物標を含む車両４０の周辺の画像情報を取得する。
【0072】
ステップＳ１０２では、ステップＳ１００で取得した周辺環境情報を用いて相対位置補正情報を演算する。このステップＳ１０２では、上述した相対位置補正情報演算部１４が、取得部１２で取得された周辺環境情報を用いて複数の地点の各々の間における相対位置及び姿勢を補正する相対位置補正情報を演算する。すなわち、ステップＳ１０２では、複数の地点間の１対１の組合せの組に対して、周辺環境情報に共通に含まれる物標を対応付けし、相対的な位置姿勢を補正する座標変換行列を算出する。
【0073】
ステップＳ１０４では、ステップＳ１０２で演算された相対位置補正情報を用いてグラフを生成する。このステップＳ１０４では、上述したグラフ生成部１６が、相対位置補正情報演算部１４により演算された相対位置補正情報を用いて、各地点間の関係をグラフ表現したグラフを生成する。生成されるグラフは、取得部１２で取得された複数の周辺環境情報、及び相対位置補正情報演算部１４で演算された相対位置補正情報に基づいて、ノードとノード間を繋ぐエッジとから構成される。
【0074】
ステップＳ１０６では、ステップＳ１０４で生成されたグラフを用いて相対位置補正情報の信頼度を演算する。このステップＳ１０６では、上述した信頼度演算部１８が、グラフ生成部１６で生成されたグラフを用いて、各エッジに対応する相対位置補正情報の信頼度を算出する。ステップＳ１０６では、第１信頼度演算部１８Ａが閉路上の座標変換の第１信頼度を算出し、第２信頼度演算部１８Ｂが各閉路の含まれるエッジの第２信頼度を算出する。具体的には、第１信頼度演算部１８Ａが、グラフに含まれる閉路を抽出し、各閉路上のエッジに対応する相対位置補正情報を示す座標変換のための相対位置補正行列を合成し、閉路の始点の座標と、閉路を経由して座標変換した結果の座標との残差を算出する。次に第１信頼度演算部１８Ａは、算出した残差を、閉路上の座標変換の信頼度、すなわち閉路の相対位置補正情報による補正の信頼度である第１信頼度を算出する。第２信頼度演算部１８Ｂは、各閉路の含まれるエッジに対応する信頼度を集約する統計量(平均値、中央値等)を算出し、算出した統計量を各エッジに対応する相対位置補正情報の信頼度である第２信頼度を算出する。
【0075】
ステップＳ１１０では、ステップＳ１０６で演算された信頼度を用いて、相対位置補正情報を統合し、ノード間を結ぶ経路を選択する。このステップＳ１１０では、上述した統合部２２が、第２信頼度演算部１８Ｂで算出された第２信頼度に基づいて、地点間の情報収集装置であるカメラ４２の相対位置及び姿勢を補正する相対位置補正情報について、ノード間を結ぶ経路により定まるエッジの各々による相対位置補正情報を選択する。具体的には、各エッジの第２信頼度が高いほど小さくなるような値を各エッジの重みとして設定し、例えば信頼度が高い経路、すなわち、第２信頼度で重み付けした経路の重みの総和が例えば最小値の経路を導出する。これによって、ノード間の最適な座標変換に対応する経路を導出することが可能となる。
【0076】
このように、相対位置姿勢全体最適化部２６は、車両毎に推定した自車両から他車両への相対方位、及び各撮影画像内の他車両に対応付けられた情報が矛盾なく整合するように、例えば、５台の車両４０Ａ～４０Ｅの車両間の相対的な位置及び姿勢を推定する。この車両毎に推定した自車両から他車両への相対方位及び各撮影画像内の他車両に対応付けられた情報が矛盾なく整合するとは、５台の車両４０Ａ～４０Ｅの各々が、自車両と各画像に撮影された他車両の各々の関係から、推定された方位及び姿勢角で存在する幾何学的な相対位置及び姿勢角の対応関係にあることである。このことは、上記の（１）式からも明らかである。
【0077】
本実施形態によれば、複数の地点において車両４０により撮影された物標を含む撮影画像から複数の地点間における各座標系間の相対的な位置及び姿勢を補正する相対位置補正情報を導出可能である。つまり、誤対応などにより誤った相対位置補正結果を含む場合であっても適切な相対位置補正情報の信頼度を得ることが可能となる。すなわち、閉路上の複数の相対位置補正情報による座標変換の結果を連結することで、閉路の誤差である残差Ｅで閉路の座標変換機能を評価することが可能となる。また、複数の閉路における変換行列を統合して統計量を算出することで、個々の相対位置補正情報の信頼度を算出することができる。この信頼度によって、地点間で座標変換するための相対位置補正情報を選択することができる。
【0078】
なお、上記では座標系の２次元的な位置及び姿勢について説明したが、３次元への拡張も容易に行うことが可能である。
【0079】
（応用例）
次に、上述した相対位置姿勢補正装置１０Ａを、周辺環境の地図を生成する場合に適用する応用例を説明する。この応用例では、移動する車両４０の移動経路で撮影された周辺環境情報（画像情報）を用いて周辺地図を生成し、移動経路毎に生成された複数の周辺地図を統合する。
次に、上述した相対位置姿勢補正装置１０Ａを、周辺環境の地図を生成する場合に適用する応用例を説明する。この応用例では、移動する車両４０の移動経路で撮影された周辺環境情報（画像情報）を用いて周辺地図を生成し、移動経路毎に生成された複数の周辺地図を統合する。
【0080】
図１０は、複数（ここでは２つ）の移動経路による画像情報から生成された複数の周辺環境の地図を統合する場合の一例を示すイメージ図である。
図１０では、車両４０として、周辺環境６０を、異なる移動経路で移動する場合の車両４０の移動経路６４Ａ、６４Ｂ及び物標６８を示す。
図１０では、車両４０として、周辺環境６０を、異なる移動経路で移動する場合の車両４０の移動経路６４Ａ、６４Ｂ及び物標６８を示す。
【0081】
地図６２は、車両４０Ａが周辺環境６０を移動経路６４Ａで移動しているときに、各地点６６Ａにおいてカメラ４２Ａで撮影した物標６８を含む周辺環境６０の複数の画像情報から生成している。地図６２は、車両４０Ａのカメラ４２Ａによるローカル座標系で生成される。地図６４は、車両４０Ｂが周辺環境６０を移動経路６４Ｂで移動しているときに、各地点６６Ｂにおいてカメラ４２Ｂで撮影した物標６８を含む周辺環境６０の複数の画像情報から生成している。地図６４は、車両４０Ｂのカメラ４２Ｂによるローカル座標系で生成される。図１０では、地図６２、６４を世界座標に合せて描画して、物標の位置および座標系の向き（姿勢）が異なる状態となるようにしている。
【0082】
これら生成された地図６２、６４を上述した技術と同様に、地図内に共通に含まれる物標を用いて統合する（統合地図７０）。すなわち、車両４０Ａのカメラ４２Ａによるローカル座標系と、車両４０Ｂのカメラ４２Ｂによるローカル座標系とを相対位置及び姿勢を補正する相対位置補正情報を導出する。このように、異なる移動経路で移動する車両４０から得られる画像情報から生成される地図であっても、地図内に共通に含まれる物標を用いて統合することで、相対位置及び姿勢を補正する相対位置補正情報を導出することが可能となる。
【0083】
具体的には、移動する車両４０により周辺環境情報を取得して地図を生成する場合、移動中に生成した各地図内の物標間の相対精度は高いが絶対位置の誤差が大きい場合がある。このため、地図内に共通に含まれる物標の位置情報を用いて、地図間における相対的な位置を補正する。その際に移動経路全体の物標の位置情報を統合する際に、上述した本開示の技術を適用する。これによって、絶対位置のずれに基づく相対的な位置ずれを補正することが可能となる。
【0084】
従って、応用例では，周辺環境情報収集装置として機能するカメラ４２を搭載した車両４０の移動経路と、収集した周辺環境情報である画像情報をまとめた情報を、異なる時刻または異なる周辺環境情報収集装置により収集し、相対位置及び姿勢を補正することが可能である。
【0085】
［第２実施形態］
次に、第２実施形態を説明する。第２実施形態は、統合部２２で選択・統合するエッジに対応する相対位置補正情報の一部を無効化するか否かを判定する無効判定部を第１実施形態の構成に追加する構成である。
次に、第２実施形態を説明する。第２実施形態は、統合部２２で選択・統合するエッジに対応する相対位置補正情報の一部を無効化するか否かを判定する無効判定部を第１実施形態の構成に追加する構成である。
【0086】
図１１は、第２実施形態に係る相対位置姿勢補正装置１０Ｂの構成の一例を示すブロック図である。
図１１に示すように、相対位置姿勢補正装置１０Ｂは、第１実施形態における信頼度演算部１８と統合部２２との間に無効判定部２０を備えている。この無効判定部２０以外の構成については、第１実施形態の構成と同様であるので、同様な部分には同一の符号を付して詳細な説明を省略する。なお、無効判定部２０は、本開示の無効化部の一例である。
図１１に示すように、相対位置姿勢補正装置１０Ｂは、第１実施形態における信頼度演算部１８と統合部２２との間に無効判定部２０を備えている。この無効判定部２０以外の構成については、第１実施形態の構成と同様であるので、同様な部分には同一の符号を付して詳細な説明を省略する。なお、無効判定部２０は、本開示の無効化部の一例である。
【0087】
無効判定部２０は、信頼度演算部１８で演算された相対位置補正情報の信頼度（第２信頼度）に基づいて、演算された相対位置補正情報を無効化するか否かを判定する機能部である。具体的には、信頼度の低い相対位置補正情報、すなわち、上述した第２信頼度が予め定めた閾値未満であるか否かを判断し、第２信頼度が閾値以上の場合は後処理、すなわち統合部２２で扱うように相対位置補正情報を有効とする判定を行う。一方、閾値未満の場合は統合部２２で扱うことを禁止するように相対位置補正情報を無効とする判定を行う。例えば、上述した統計量の値により、信頼度の低い閾値未満の相対位置補正情報を無効判定し、上述した平均値の算出から除外する。したがって、閾値未満の信頼度である相対位置補正情報は統合部２２で扱う情報から除外され、統合部２２における処理精度を向上させることが可能となる。
【0088】
次に、図１２を参照して、相対位置姿勢補正装置１０Ｂの作用を説明する。なお、図１２は、第１実施形態に係る相対位置姿勢推定処理プログラムの処理において、ステップＳ１０８の処理を追加した処理の流れの一例を示すフローチャートである。
【0089】
ステップＳ１０８では、相対位置補正情報の信頼度（第２信頼度）に基づいて、エッジに対応する相対位置補正情報を無効化するか否かを判定する。すなわち、ステップＳ１０８では、無効判定部２０が、信頼度演算部１８で演算された相対位置補正情報の信頼度（第２信頼度）が低いか否かを判定する処理を行う。すなわち、ステップＳ１０８では、第２信頼度が閾値以上の場合は相対位置補正情報を有効と判定し、閾値未満の場合は無効と判定する。
【0090】
本実施形態によれば、演算された相対位置補正情報の信頼度の大きさに応じて有効又は無効の判定を行うことが可能となる。よって、物標を含んで撮影された画像情報から補正に不適な情報を排除可能となり、座標系の相対的な位置及び姿勢の補正精度を向上させることが可能となる。
【0091】
なお、本実施形態において、相対位置補正情報が低い物標の情報を排除するだけでなく、誤差が大きくなった地物認識機能に対して、相対位置補正情報を用いて認識処理のキャリブレーションを行う構成としてもよい。
【0092】
[第３実施形態]
次に、第３実施形態を説明する。第３実施形態は、上述したノードを無効化するか否かを判定する無効判定部を第１実施形態の構成に追加する構成である。
次に、第３実施形態を説明する。第３実施形態は、上述したノードを無効化するか否かを判定する無効判定部を第１実施形態の構成に追加する構成である。
【0093】
図１３は、第３実施形態に係る相対位置姿勢補正装置１０Ｃの構成の一例を示すブロック図である。
図１３に示すように、相対位置姿勢補正装置１０Ｃは、第１実施形態における信頼度演算部１８と統合部２２との間に外界情報無効判定部２１を備えている。この外界情報無効判定部２１以外の構成については、第１実施形態の構成と同様であるので、同様な部分には同一の符号を付して詳細な説明を省略する。なお、外界情報無効判定部２１は、本開示の無効化部の一例である。
図１３に示すように、相対位置姿勢補正装置１０Ｃは、第１実施形態における信頼度演算部１８と統合部２２との間に外界情報無効判定部２１を備えている。この外界情報無効判定部２１以外の構成については、第１実施形態の構成と同様であるので、同様な部分には同一の符号を付して詳細な説明を省略する。なお、外界情報無効判定部２１は、本開示の無効化部の一例である。
【0094】
外界情報無効判定部２１は、信頼度演算部１８で演算された相対位置補正情報の信頼度（第２信頼度）に基づいて、演算された相対位置補正情報が連結されるノードを無効化するか否かを判定する機能部である。具体的には、ノード単位で相対位置補正情報の信頼度（第２信頼度）を評価する。ノード単位での評価とは１又は複数のエッジが繋がるノードに注目して、当該注目したノードに繋がるエッジについて相対位置補正情報の信頼度（第２信頼度）を評価することである。例えば、ノードに繋がるエッジの相対位置補正情報の総和、及び平均値等の演算値が、予め定めた閾値未満である場合に、信頼度の低いノードであると判定する。すなわち、ノードに対するエッジについて上述した第２信頼度の演算値が予め定めた閾値未満であるか否かを判断し、第２信頼度の演算値が閾値以上の場合は後処理、すなわち統合部２２で扱うようにノードを有効とする判定を行う。一方、閾値未満の場合は統合部２２で扱うことを禁止するようにノードを無効とする判定を行う。したがって、閾値未満の演算値であるノードは統合部２２で扱う情報から除外され、統合部２２における処理精度を向上させることが可能となる。
【0095】
【0096】
ステップＳ１０９では、相対位置補正情報の信頼度（第２信頼度）に基づいて、エッジに対するノードを無効化するか否かを判定する。すなわち、ステップＳ１０９では、外界情報無効判定部２１が、信頼度演算部１８で演算された相対位置補正情報の信頼度（第２信頼度）の上述した演算値が低いか否かを判定する処理を行う。すなわち、ステップＳ１０９では、グラフで繋がっているエッジに対する第２信頼度の演算値が閾値以上のノードを有効と判定し、閾値未満のノードは無効と判定する。
【0097】
本実施形態によれば、エッジとして繋がるノードについて当該エッジに対する相対位置補正情報の信頼度の大きさに応じてノードが有効か無効かの判定を行うことが可能となる。よって、物標を含んで撮影された画像情報から補正に不適な情報を排除可能となり、座標系の相対的な位置及び姿勢の補正精度を向上させることが可能となる。
【0098】
[その他の実施形態]
以上、実施形態として相対位置姿勢補正装置を例示して説明した。実施形態は、コンピュータを、相対位置姿勢補正装置が備える各部として機能させるためのプログラムの形態としてもよい。実施形態は、このプログラムを記憶したコンピュータが読み取り可能な記憶媒体の形態としてもよい。
以上、実施形態として相対位置姿勢補正装置を例示して説明した。実施形態は、コンピュータを、相対位置姿勢補正装置が備える各部として機能させるためのプログラムの形態としてもよい。実施形態は、このプログラムを記憶したコンピュータが読み取り可能な記憶媒体の形態としてもよい。
【0099】
その他、上記実施形態で説明した相対位置姿勢補正装置の構成は、一例であり、主旨を逸脱しない範囲内において状況に応じて変更してもよい。
【0100】
また、上記実施形態で説明したプログラムの処理の流れも、一例であり、主旨を逸脱しない範囲内において不要なステップを削除したり、新たなステップを追加したり、処理順序を入れ替えたりしてもよい。
【0101】
また、上記実施形態では、プログラムを実行することにより、実施形態に係る処理がコンピュータを利用してソフトウェア構成により実現される場合について説明したが、これに限らない。実施形態は、例えば、ハードウェア構成や、ハードウェア構成とソフトウェア構成との組み合わせによって実現してもよい。
【0102】
さらに、上記実施形態における処理は、プログラムとして光ディスク等の記憶媒体等に記憶して流通するようにしてもよい。
【0103】
上記実施形態において、ＣＰＵとは広義的なプロセッサを指し、汎用的なプロセッサや、専用のプロセッサ（例えばGPU：Graphics Processing Unit、ASIC： Application Specific Integrated Circuit、FPGA： Field Programmable Gate Array、プログラマブル論理デバイス、等）を含むものである。
【0104】
この場合、プロセッサの動作は、１つのプロセッサによって成すのみでなく、物理的に離れた位置に存在する複数のプロセッサが協働して成すものであってもよい。また、プロセッサの各動作の順序は上記各実施形態において記載した順序のみに限定されるものではなく、適宜変更してもよい。
【0105】
本明細書に記載された全ての文献、特許出願、及び技術規格は、個々の文献、特許出願、及び技術規格が参照により取り込まれることが具体的かつ個々に記された場合と同程度に、本明細書中に参照により取り込まれる。
【0106】
（実施態様）
本開示の技術は、以下の付記に示す従属関係の実施態様を構成し得る。
本開示の技術は、以下の付記に示す従属関係の実施態様を構成し得る。
【0107】
［付記１］
複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部と、
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部と、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部と、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部と、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部と、
を備えた相対位置姿勢補正装置。
複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部と、
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部と、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部と、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部と、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部と、
を備えた相対位置姿勢補正装置。
【0108】
［付記２］
前記第１信頼度算出部は、前記閉路として、前記複数地点のうちの始点を示すノードから他のノードを経由して終点を示すノードとして前記始点を示すノードに戻るまでの経路を、前記地点間の座標系を座標変換する座標変換用の閉路として設定し、前記始点のノードにより示される地点の座標系と、前記座標変換用の閉路に従って座標変換された結果の座標系との相対位置及座標系の相対姿勢の誤差が小さくなるに従って高くなるように前記座標変換閉路の信頼度を算出し、
前記第２信頼度算出部は、前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記座標変換用の閉路の信頼度が大きくなるに従って高くなるように前記第２信頼度を算出する
付記１に記載の相対位置姿勢補正装置。
前記第１信頼度算出部は、前記閉路として、前記複数地点のうちの始点を示すノードから他のノードを経由して終点を示すノードとして前記始点を示すノードに戻るまでの経路を、前記地点間の座標系を座標変換する座標変換用の閉路として設定し、前記始点のノードにより示される地点の座標系と、前記座標変換用の閉路に従って座標変換された結果の座標系との相対位置及座標系の相対姿勢の誤差が小さくなるに従って高くなるように前記座標変換閉路の信頼度を算出し、
前記第２信頼度算出部は、前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記座標変換用の閉路の信頼度が大きくなるに従って高くなるように前記第２信頼度を算出する
付記１に記載の相対位置姿勢補正装置。
【0109】
［付記３］
前記選択部は、前記辺の各々に対応する相対位置補正情報に対して前記第２信頼度が高いほど小さくなるように設定した重みの総和が予め定めた閾値以下の経路による相対位置補正情報を選択する
付記１又は付記２に記載の相対位置姿勢補正装置。
前記選択部は、前記辺の各々に対応する相対位置補正情報に対して前記第２信頼度が高いほど小さくなるように設定した重みの総和が予め定めた閾値以下の経路による相対位置補正情報を選択する
付記１又は付記２に記載の相対位置姿勢補正装置。
【0110】
［付記４］
前記情報収集装置は、車両の周辺を撮影する撮影装置を含み、
前記周辺環境情報は、前記撮影装置で撮影された撮影画像である
付記１から付記３の何れか１つに記載の相対位置姿勢補正装置。
前記情報収集装置は、車両の周辺を撮影する撮影装置を含み、
前記周辺環境情報は、前記撮影装置で撮影された撮影画像である
付記１から付記３の何れか１つに記載の相対位置姿勢補正装置。
【0111】
［付記５］
前記複数地点の各々における情報収集装置は、時間経過前後の同じ情報収集装置である
付記１から付記４の何れか１つに記載の相対位置姿勢補正装置。
前記複数地点の各々における情報収集装置は、時間経過前後の同じ情報収集装置である
付記１から付記４の何れか１つに記載の相対位置姿勢補正装置。
【0112】
［付記６］
前記第２信頼度算出部で算出された前記第２信頼度に基づいて、予め定めた閾値未満の値の第２信頼度に対応する辺の相対位置補正情報及び当該辺に繋がるノードの少なくとも一方を無効化する無効化部、
をさらに備えた付記１から付記３の何れか１つに記載の相対位置姿勢補正装置。
前記第２信頼度算出部で算出された前記第２信頼度に基づいて、予め定めた閾値未満の値の第２信頼度に対応する辺の相対位置補正情報及び当該辺に繋がるノードの少なくとも一方を無効化する無効化部、
をさらに備えた付記１から付記３の何れか１つに記載の相対位置姿勢補正装置。
【0113】
［付記７］
コンピュータを、
複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部、
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部、および、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部、
として機能させるためのプログラム。
コンピュータを、
複数地点の各々において情報収集装置が収集した周辺環境内に存在する物標の位置情報を含む複数の周辺環境情報を取得する取得部、
前記取得部が取得した複数の周辺環境情報に基づいて、前記情報収集装置が収集した際の前記情報収集装置の位置及び姿勢を各々含む複数の装置情報の各々をノードとし、かつ前記複数の周辺環境情報の中で物標が共通に含まれる周辺環境情報の各組について、各組の周辺環境情報に共通に含まれる物標を対応付けることにより求められ、前記各組の周辺環境情報を収集した地点間の情報収集装置の相対位置及び姿勢を補正する前記各組の相対位置補正情報の各々を、ノード間を結ぶ辺とするグラフを生成する生成部、
前記生成部で生成されたグラフから抽出された辺の数が３つ以上の閉路に対して前記閉路に含まれる辺の各々に対応する相対位置補正情報を合成して前記閉路の相対位置補正情報による補正の第１信頼度を算出する第１信頼度算出部、
前記第１信頼度算出部で算出された前記第１信頼度に基づいて、前記ノード間を結ぶ辺の各々に対応する相対位置補正情報による補正の第２信頼度を算出する第２信頼度算出部、および、
前記第２信頼度算出部で算出された第２信頼度に基づいて、前記地点間の情報収集装置の相対位置及び姿勢を補正する相対位置補正情報について、前記ノード間を結ぶ経路により定まる辺の各々による相対位置補正情報を選択する選択部、
として機能させるためのプログラム。
【符号の説明】
【0114】
１０Ａ、１０Ｂ、１０Ｃ 相対位置姿勢補正装置
１２ 取得部
１４ 相対位置補正情報演算部
１６ グラフ生成部
１８ 信頼度演算部
２０ 無効判定部
２２ 外界情報無効判定部
３０ コンピュータ本体
３３Ｐ 相対位置姿勢推定処理プログラム
４０ 車両
４２ カメラ
４４ メモリ
１２ 取得部
１４ 相対位置補正情報演算部
１６ グラフ生成部
１８ 信頼度演算部
２０ 無効判定部
２２ 外界情報無効判定部
３０ コンピュータ本体
３３Ｐ 相対位置姿勢推定処理プログラム
４０ 車両
４２ カメラ
４４ メモリ
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
