(57)【要約】【課題】特徴点の３次元位置を精度良く推定することができるようにする。【解決手段】グラフ構造データ生成部３９０が、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、撮影ノード上の特徴点ノードの位置を規定する特徴点エッジと、撮影ノード間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、誤差関数設定部４００が、特徴点ノードの特徴点を、撮影ノードを用いて投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、グラフ構造データ最適化部４１０が、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データを最適化する。【選択図】図１
2020122754.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部と、
前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部と、
前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部と、
前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部と、
前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、
前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部と、
前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部と、
を含み、
前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
３次元位置推定装置。
【請求項２】
前記位置姿勢変化量計算部は、前記移動体に搭載された操舵角センサにより検出された操舵角に基づいて、前記移動体のヨー角変化量を計算し、
前記誤差関数設定部は、前記位置姿勢変化量計算部による計算結果に基づいて得られる、前記撮影エッジに対応する前記移動体のヨー角変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
請求項１記載の３次元位置推定装置。
【請求項３】
コンピュータを、
移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部、
前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部、
前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部、
前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部、
前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部、
前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部、及び
前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部
として機能させ、
前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元位置推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来から、車両に搭載されたカメラで周辺環境を撮影し、撮影された画像から環境地図を作成する技術がある（非特許文献１）。
【０００３】
非特許文献１では、画像から特徴点を検出し、当該特徴点の局所特徴量を用いて、複数地点で撮影された画像間の特徴点を対応付け、対応付けた特徴点の画像上の位置から、カメラの位置・姿勢と特徴点の３次元位置を算出し、複数の特徴点の３次元位置を規定する特徴点ノードと、複数の撮影地点におけるカメラの位置・姿勢を規定するカメラノードと、各撮影地点における画像上の特徴点位置を規定する特徴点エッジと、隣接するカメラノード間における車両の位置・姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジと撮影エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化することにより、環境地図を作成する。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】Jing Wang, Zongying Shi, Yisheng Zhong, "Visual SLAM Incorporating Wheel odometer for Indoor Robots",  2017 36th Chinese Control Conference (CCC), 2017.
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかし、上記非特許文献１では、撮影エッジの誤差関数の重み（非特許文献中のｉｍｆｏｒｍａｔｉｏｎ  ｍａｔｒｉｘ  ΣＢ）の設定方法について記述されていない。
【０００６】
このため、車両旋回時のような、オドメトリにより車両の位置・姿勢の変化量を高い精度で取得できない場合、車両が旋回したときの精度が低い車両の位置変化量の計測値に対して撮影エッジの誤差関数が小さくなるようにグラフ構造データを最適化するため、車載カメラの画像から作成した環境地図の歪みが大きくなってしまう、という問題があった。
【０００７】
本発明は、上記問題点を解決するためになされたもので、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる３次元位置推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記の目的を達成するために、本発明の３次元位置推定装置は、移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部と、前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部と、前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部と、前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部と、前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部と、前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部と、を含み、前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定するように構成されている。
【０００９】
また、本発明のプログラムは、コンピュータを、移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部、前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部、前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部、前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部、前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部、前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部、及び前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部として機能させ、前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定するためのプログラムである。
【００１０】
本発明の３次元位置推定装置及びプログラムによれば、特徴点検出部が、移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出し、位置姿勢変化量計算部が、当該移動体に関するセンサ情報に基づいて、当該移動体の位置及び姿勢の変化量を計算し、特徴点対応付け部が、当該特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、当該複数の画像間における特徴点を対応付け、３次元位置取得部が、当該特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、当該複数の画像の各々についての当該撮影装置の位置及び姿勢とを取得し、グラフ構造データ生成部が、当該特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、当該複数の画像の各々についての、当該画像を撮影した当該撮影装置の位置及び姿勢を規定する撮影ノードと、当該特徴点ノードと当該撮影ノードとの間の各々を結ぶ、当該撮影ノードの当該画像上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する当該撮影ノード間の各々を結ぶ、当該撮影ノードの画像間における当該移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。
【００１１】
そして、誤差関数設定部が、グラフ構造データ生成部により生成された当該グラフ構造データに含まれる当該特徴点エッジの各々について、当該特徴点エッジで結ばれた当該特徴点ノードの特徴点を、当該撮影ノードが規定する当該撮影装置の位置及び姿勢を用いて当該撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、当該グラフ構造データに含まれる当該撮影エッジの各々について、当該撮影エッジが規定する当該位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定し、グラフ構造データ最適化部が、当該特徴点エッジの各々についての当該第１誤差関数と、当該撮影エッジの各々についての当該第２誤差関数との和が最小となるように、当該グラフ構造データの当該特徴点ノードの各々が規定する当該特徴点の３次元位置、及び当該撮影ノードの各々が規定する当該撮影装置の位置及び姿勢を最適化し、当該誤差関数設定部は、当該撮影エッジの各々について、当該撮影エッジに対応する当該位置姿勢変化量計算部による当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表される当該第２誤差関数を設定する。
【００１２】
このように、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、撮影ノードの画像上の、特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、撮影ノードの画像間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データの特徴点の３次元位置、及び撮影装置の位置及び姿勢を最適化することにより、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる。
【００１３】
また、本発明の３次元位置推定装置の前記位置姿勢変化量計算部は、前記移動体に搭載された操舵角センサにより検出された操舵角に基づいて、前記移動体のヨー角変化量を計算し、前記誤差関数設定部は、前記位置姿勢変化量計算部による計算結果に基づいて得られる、前記撮影エッジに対応する前記移動体のヨー角変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定することができる。
【発明の効果】
【００１４】
以上説明したように、本発明の３次元位置推定装置及びプログラムによれば、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、撮影ノードの画像上の、特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、撮影ノードの画像間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データの特徴点の３次元位置、及び撮影装置の位置及び姿勢を最適化することにより、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる、という効果が得られる。
【図面の簡単な説明】
【００１５】
【図１】本発明の実施の形態における概略構成を示すブロック図である。
【図２】車両の位置・ヨー角の変化量を示す図である。
【図３】特徴点の３次元位置の算出を示す図である。
【図４】カメラ位置・姿勢の変化量の算出を示す図である。
【図５】グラフ構造データの構成を示す図である。
【図６】本発明の実施の形態における環境地図作成処理ルーチンの内容を示すフローチャートである。
【図７】本発明の実施の形態における３次元位置計算処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００１７】
＜地図作成システムの構成＞
図１に示すように、本発明の実施の形態に係る地図作成システム１０は、撮影装置１００と、車輪速センサ２００と、操舵角センサ２１０と、地図作成装置３００とを備えて構成される。
【００１８】
撮影装置１００は、移動体に搭載された周辺環境を撮影する。本実施の形態では、移動体は、車両である場合を例に説明する。
【００１９】
具体的には、撮影装置１００は、車両の上部等に設けられた単眼カメラであり、車両の前方や後方等を撮影する。撮影装置１００は、例えば、車幅方向の略中央部付近に設けられ、かつ撮影装置１００の光軸が水平方向より若干下側を向くように配置されている。
【００２０】
また、本実施形態では、撮影装置１００として単眼カメラを例示して説明するが、これに限られず他の形態のカメラ、例えばステレオカメラ等であってもよい。
【００２１】
そして、撮影装置１００は、撮影した画像を、地図作成装置３００に渡す。
【００２２】
車輪速センサ２００は、車両の４輪の車輪速を検出し、検出した車輪速を、地図作成装置３００に渡す。
【００２３】
操舵角センサ２１０は、車両の操舵角を検出し、検出した車両の操舵角を、地図作成装置３００に渡す。
【００２４】
地図作成装置３００は、ＣＰＵと、ＲＡＭと、後述する姿勢角補正誤差推定処理ルーチン及び自己位置推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００２５】
地図作成装置３００は、センサ情報入力部３１０と、オドメトリ計算部３２０と、画像入力部３３０と、キーフレーム選択部３４０と、特徴点検出部３５０と、特徴量算出部３６０と、特徴点対応付け部３７０と、３次元位置計算部３８０と、グラフ構造データ生成部３９０と、誤差関数設定部４００と、グラフ構造データ最適化部４１０と、環境地図作成部４２０と、出力部４３０と、を備えて構成される。
【００２６】
センサ情報入力部３１０は、車輪速センサ２００により検出された車輪速、及び操舵角センサ２１０により検出された操舵角の入力を受け付ける。
【００２７】
そして、センサ情報入力部３１０は、受け付けた車輪速及び操舵角を、オドメトリ計算部３２０に渡す。
【００２８】
オドメトリ計算部３２０は、車両に関するセンサ情報に基づいて、車両の位置変化量（ΔＸｖ，ΔＹｖ）及びヨー角変化量Δθｖを計算する。
【００２９】
具体的には、オドメトリ計算部３２０は、まず、車輪速に基づいて、車両の走行距離を算出すると共に、操舵角に基づいて、車両の旋回半径を算出する。
【００３０】
次に、オドメトリ計算部３２０は、図２に示すように、車両の走行距離及び旋回半径から、車両座標系での車両の位置変化量（ΔＸｖ，ΔＹｖ）とヨー角変化量Δθｖとを計算する。
【００３１】
そして、オドメトリ計算部３２０は、算出した位置変化量（ΔＸｖ，ΔＹｖ）とヨー角変化量Δθｖとを、３次元位置計算部３８０、及び誤差関数設定部４００に渡す。また、オドメトリ計算部３２０は、各時刻における車両の走行距離を、キーフレーム選択部３４０に渡す。
【００３２】
画像入力部３３０は、撮影装置１００から画像の入力を受け付ける。
【００３３】
そして、画像入力部３３０は、受け付けた画像を、キーフレーム選択部３４０に渡す。
【００３４】
キーフレーム選択部３４０は、撮影装置１００から入力された画像のうち、環境地図を作成するために用いるキーフレームを複数選択する。
【００３５】
具体的には、キーフレーム選択部３４０は、まず、最初に撮影装置１００により受け付けた画像を１番目のキーフレームとして選択する。
【００３６】
次に、キーフレーム選択部３４０は、オドメトリ計算部３２０から取得した車両の走行距離が所定値（例えば０．１ｍ）を超える度に、所定値を超えた時刻に撮影装置１００により受け付けた画像を次のキーフレームとして選択する。
【００３７】
そして、キーフレーム選択部３４０は、選択した複数のキーフレームを、特徴点検出部３５０に渡す。
【００３８】
特徴点検出部３５０は、車両に搭載された撮影装置１００により撮影された複数のキーフレームの各々から複数の特徴点を検出する。
【００３９】
具体的には、特徴点検出部３５０は、ＦＡＳＴ（参考文献１）等のアルゴリズムを用いて各キーフレームの画像ピラミッドから複数の特徴点を検出する。
［参考文献１］Raul Mur-Artal, J. M. M. Montiel and Juan D. Tardos, "ORB-SLAM: A Versatile and Accurate Monocular SLAM System.", IEEE Transactions on Robotics, vol.31, no.5, 2015, pp.1147-1163.
【００４０】
そして、特徴点検出部３５０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点を、特徴量算出部３６０に渡す。
【００４１】
特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点の各々の特徴量を算出する。
【００４２】
具体的には、特徴量算出部３６０は、複数のキーフレームの各々についての複数の特徴点から、ＯＲＢ（参考文献１）等の局所特徴量を算出する。
【００４３】
そして、特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点と、当該複数の特徴点の各々についての特徴量を、特徴点対応付け部３７０に渡す。
【００４４】
特徴点対応付け部３７０は、特徴量算出部３６０により算出された特徴点の特徴を示す特徴量を用いて、連続するキーフレーム間における特徴点を対応付ける。
【００４５】
具体的には、特徴点対応付け部３７０は、特徴点の各々についての特徴量を用いて、連続するキーフレーム間での特徴点を対応付ける。
【００４６】
そして、特徴点対応付け部３７０は、複数のキーフレームの各々について、対応付けた特徴点を、３次元位置計算部３８０に渡す。
【００４７】
３次元位置計算部３８０は、特徴点対応付け部３７０により対応付けられた特徴点の各々についての３次元位置と、複数の画像の各々についての撮影装置１００の位置及び姿勢とを計算する。
【００４８】
具体的には、３次元位置計算部３８０は、２番目のキーフレームから順番に、一つ前のキーフレームとの間の特徴点の対応付けに基づいて、各特徴点の３次元位置を計算する。
【００４９】
３次元位置計算部３８０は、２番目のキーフレームの場合、まず、１番目のキーフレームとの間で、８点アルゴリズムとＲＡＮＳＡＣ（Ｒａｎｄｏｍ  ｓａｍｐｌｅ  ｃｏｎｓｅｎｓｕｓ）とを用いて、１番目のキーフレームの撮影位置と、２番目のキーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量（無次元量）を計算し、車両の位置変化量（ΔＸｖ，ΔＹｖ）を用いて有次元化（絶対的なスケールを決定）する。
【００５０】
次に、３次元位置計算部３８０は、図３に示すように、対応付けた特徴点のキーフレーム上の位置と撮影装置１００の位置及び姿勢の変化量とから、三角測量の原理により当該特徴点の３次元位置（ワールド座標系）を計算する。
【００５１】
３次元位置計算部３８０は、３番目以降のキーフレームの場合、図４に示すように、まず、１つ前のキーフレームとの間で対応付けた特徴点のうち、３次元位置が既知の特徴点を抽出し、その再投影誤差の和が最小となる撮影装置１００の位置及び姿勢の変化量を計算する。
【００５２】
次に、３次元位置計算部３８０は、１つ前のキーフレームとの間で対応付けた特徴点のうち、３次元位置が未知の特徴点について、２番目のキーフレームと同様に、キーフレーム上の特徴点の位置と撮影装置１００の位置及び姿勢の変化量とから三角測量の原理により当該特徴点の３次元位置を計算する。
【００５３】
そして、３次元位置計算部３８０は、計算して得られた複数の特徴点の各々についての３次元位置と、複数のキーフレームの各々についての、当該キーフレームの１つ前のキーフレームの撮影位置と、当該キーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量とを、グラフ構造データ生成部３９０に渡す。
【００５４】
グラフ構造データ生成部３９０は、特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、複数のキーフレームの各々についての、当該キーフレームを撮影した撮影装置１００の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、当該撮影ノードのキーフレーム上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、当該撮影ノードのキーフレーム間における車両の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。
【００５５】
具体的には、グラフ構造データ生成部３９０は、図５に示すように、特徴点Ｐｉの３次元位置を規定する特徴点ノードと、キーフレームＫＦｊのカメラ位置・姿勢を規定するカメラノードと、キーフレームＫＦｊ上の特徴点Ｐｉの位置を規定する特徴点エッジと、隣接するキーフレーム間における車両の位置変化量とヨー角変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。
【００５６】
具体的には、グラフ構造データ生成部３９０は、３次元位置計算部３８０により得られた各特徴点の３次元位置を用いて、グラフ構造データを生成する。
【００５７】
そして、グラフ構造データ生成部３９０は、生成したグラフ構造データを、誤差関数設定部４００に渡す。
【００５８】
誤差関数設定部４００は、グラフ構造データ生成部３９０により生成されたグラフ構造データに含まれる特徴点エッジの各々について、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置１００の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数ＣＰｉｊを設定すると共に、グラフ構造データに含まれる撮影エッジの各々について、当該撮影エッジが規定する位置及び姿勢の変化量とオドメトリ計算部３２０による計算結果との誤差を表す第２誤差関数ＣＫＦｊを設定する。
【００５９】
具体的には、誤差関数設定部４００は、特徴点エッジの第１誤差関数ＣＰｉｊを下記式（１）、（２）のように設定する。
【００６０】
【数１】
2020122754.tif 000003
【００６１】
ｅＰｉｊはキーフレームＫＦｊ上における特徴点Ｐｉの（ｘ，ｙ）座標の再投影誤差を表す２行１列のベクトルであり、
2020122754.tif 000004
と表すことができる。ここで、（ｘＰｉｊ，ｙＰｉｊ）は、特徴点Ｐｉの実際の座標であり、’＾’（ハット記号）付きは画像上に投影したときの計算値である。
【００６２】
ＷＰｉｊは再投影誤差に対する重みであり、画像ピラミッドの解像度が高い階層から検出した特徴点に対しては大きく設定する。
【００６３】
また、誤差関数設定部４００は、撮影エッジの第２誤差関数ＣＫＦｊを下記式（３）、（４）のように設定する。
【００６４】
【数２】
2020122754.tif 000005
【００６５】
ここで、ｅＫＦｊはキーフレームＫＦｊ−１とＫＦｊの間おける、カメラ位置・姿勢の変化量から換算した車両の位置変化量とヨー角変化量の、計測値に対する誤差を表す３行１列のベクトルであり、
2020122754.tif 000006
と表すことができる。ここで、’＾’付きは計算値である。
【００６６】
また、ｗＸｊは、車両のＸ方向の位置変化量の誤差に対する重みであり、ｗＹｊは、車両のＹ方向の位置変化量の誤差に対する重みであり、ｗθｊは、車両のヨー角変化量の誤差に対する重みである。
【００６７】
ここで、誤差関数設定部４００は、撮影エッジの各々について、撮影エッジに対応するオドメトリ計算部３２０による車両のヨー角変化量Δθｖが小さいほど大きな重みｗＸｊ、ｗＹｊ、及びｗθｊを含んで表される第２誤差関数ＣＫＦｊを設定する。
【００６８】
すなわち、誤差関数設定部４００は、車両のヨー角変化量Δθｖが小さい撮影エッジほど重みｗＸｊ、ｗＹｊ、及びｗθｊを大きく設定する。
【００６９】
ここで、ｗＸｊ＝ｗＹｊとなるように設定してもよい。これにより、メモリ容量を節約することができる。
【００７０】
そして、誤差関数設定部４００は、グラフ構造データと、第１誤差関数ＣＰｉｊと、第２誤差関数ＣＫＦｊとを、グラフ構造データ最適化部４１０に渡す。
【００７１】
グラフ構造データ最適化部４１０は、特徴点エッジの各々についての第１誤差関数ＣＰｉｊと、撮影エッジの各々についての第２誤差関数ＣＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。
【００７２】
具体的には、グラフ構造データ最適化部４１０は、非特許文献１と同様に、特徴点エッジの各々についての第１誤差関数ＣＰｉｊと、撮影エッジの各々についての第２誤差関数ＣＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。
【００７３】
ここで、車両のヨー角変化量Δθｖが小さい撮影エッジほど第２誤差関数ＣＫＦｊの重みｗＸｊ、ｗＹｊ、及びｗθｊが大きく設定されていることにより、車両の位置変化量（ΔＸｖ，ΔＹｖ）の取得精度が比較的高い撮影エッジの誤差関数が小さくなるようにグラフ構造データは最適化される。これにより、グラフ構造データを構成する特徴点３次元位置とカメラ位置・姿勢の精度を全体的に高めることができる。
【００７４】
そして、グラフ構造データ最適化部４１０は、最適化したグラフ構造データを、環境地図作成部４２０に渡す。
【００７５】
環境地図作成部４２０は、グラフ構造データ最適化部４１０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。
【００７６】
具体的には、環境地図作成部４２０は、非特許文献１と同様に、グラフ構造データ最適化部４１０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。ここで、非特許文献１に比べて特徴点の３次元位置の精度が高いため、歪みの小さい地図を作成できる。
【００７７】
そして、環境地図作成部４２０は、作成した環境地図を、出力部４３０に渡す。
【００７８】
出力部４３０は、環境地図作成部４２０により作成された環境地図を出力する。
【００７９】
＜地図作成装置３００の動作＞
次に、図６を参照して、本実施形態の地図作成装置３００の地図作成処理ルーチンについて説明する。
【００８０】
まず、ステップＳ１００において、地図作成装置３００は、ｉ＝１とする。ここで、ｉは、選択するキーフレームの数を数えるカウンタである。
【００８１】
ステップＳ１１０において、センサ情報入力部３１０は、車輪速センサ２００により検出された車輪速、及び操舵角センサ２１０により検出された操舵角の入力を受け付ける。
【００８２】
ステップＳ１２０において、オドメトリ計算部３２０は、上記ステップＳ１１０で得られた車両に関するセンサ情報に基づいて、車両の位置変化量（ΔＸｖ，ΔＹｖ）及びヨー角変化量Δθｖを計算する。
【００８３】
ステップＳ１３０において、画像入力部３３０は、撮影装置１００から画像の入力を受け付ける。
【００８４】
ステップＳ１４０において、キーフレーム選択部３４０は、ｉ＝１であるか否かを判定する。
【００８５】
ｉ＝１である場合（上記ステップＳ１４０のＹＥＳ）、ステップＳ１６０に進む。
【００８６】
一方、ｉ＝１でない場合（上記ステップＳ１４０のＮＯ）、ステップＳ１５０において、キーフレーム選択部３４０は、車両の走行距離が所定値以上であるか否かを判定する。
【００８７】
走行距離が所定値以上である場合（上記ステップＳ１５０のＹＥＳ）、ステップＳ１６０に進む。
【００８８】
一方、走行距離が所定値以上でない場合（上記ステップＳ１５０のＮＯ）、ステップＳ１１０に戻る。
【００８９】
ステップＳ１６０において、キーフレーム選択部３４０は、上記ステップＳ１３０により取得した画像をｉ番目のキーフレームとして選択する。
【００９０】
ステップＳ１７０において、キーフレーム選択部３４０は、全てのキーフレームを選択したか否かを判定する。
【００９１】
全てのキーフレームを選択していない場合（上記ステップＳ１７０のＮＯ）、ステップＳ１８０において、キーフレーム選択部３４０は、ｉに１を加算し、ステップＳ１１０に戻る。
【００９２】
一方、全てのキーフレームを選択している場合（上記ステップＳ１７０のＹＥＳ）、ステップＳ１９０において、特徴点検出部３５０は、複数のキーフレームの各々から複数の特徴点を検出する。
【００９３】
ステップＳ２００において、特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点の各々の特徴量を算出する。
【００９４】
ステップＳ２１０において、特徴点対応付け部３７０は、上記ステップＳ２００により算出された特徴点の特徴を示す特徴量を用いて、複数のキーフレーム間における特徴点を対応付ける。
【００９５】
ステップＳ２２０において、３次元位置計算部３８０は、上記ステップＳ２１０により対応付けられた特徴点の各々についての３次元位置と、複数のキーフレームの各々についての撮影装置１００の位置及び姿勢とを計算する。
【００９６】
ステップＳ２３０において、グラフ構造データ生成部３９０は、特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、複数のキーフレームの各々についての、当該キーフレームを撮影した撮影装置１００の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、当該撮影ノードのキーフレーム上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、当該撮影ノードのキーフレーム間における車両の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。
【００９７】
ステップＳ２４０において、誤差関数設定部４００は、上記ステップＳ２３０により生成されたグラフ構造データに含まれる特徴点エッジの各々について、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置１００の位置及び姿勢を用いて撮影ノードのキーフレーム上に投影したときの投影誤差を表す第１誤差関数ＣＰｉｊを設定すると共に、グラフ構造データに含まれる撮影エッジの各々について、当該撮影エッジが規定する位置及び姿勢の変化量とオドメトリ計算部３２０による計算結果との誤差を表す第２誤差関数ＣＫＦｊを設定する。
【００９８】
ステップＳ２５０において、グラフ構造データ最適化部４１０は、特徴点エッジの各々についての第１誤差関数ＣＰｉｊと、撮影エッジの各々についての第２誤差関数ＣＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。
【００９９】
ステップＳ２６０において、環境地図作成部４２０は、上記ステップＳ２５０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。
【０１００】
ステップＳ２７０において、出力部４３０は、上記ステップＳ２６０により作成された環境地図を出力する。
【０１０１】
また、上記ステップＳ２２０は、図７に示す３次元位置計算処理ルーチンによって実現される。
【０１０２】
ステップＳ２２１において、３次元位置計算部３８０は、ｉ＝２とする。ここで、ｉは、キーフレームが撮影された番号を示すカウンタである。
【０１０３】
ステップＳ２２２において、３次元位置計算部３８０は、ｉ番目のキーフレームを選択する。
【０１０４】
ステップＳ２２３において、３次元位置計算部３８０は、ｉ＝２であるか否かを判定する。
【０１０５】
ｉ＝２である場合（上記ステップＳ２２７のＹＥＳ）、ステップＳ２２４において、３次元位置計算部３８０は、１番目のキーフレームとの間で、８点アルゴリズムとＲＡＮＳＡＣ（Ｒａｎｄｏｍ  ｓａｍｐｌｅ  ｃｏｎｓｅｎｓｕｓ）とを用いて、１番目のキーフレームの撮影位置と、２番目のキーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量（無次元量）を計算し、車両の位置変化量（ΔＸｖ，ΔＹｖ）を用いて有次元化し、ステップＳ２２６に進む。
【０１０６】
一方、ｉ＝２でない場合（上記ステップＳ２２７のＮＯ）、ステップＳ２２５において、３次元位置計算部３８０は、ｉ−１番目のキーフレームとの間で対応付けた特徴点のうち、３次元位置が既知の特徴点を抽出し、その再投影誤差の和が最小となる撮影装置１００の位置及び姿勢の変化量を計算し、ステップＳ２２６に進む。
【０１０７】
ステップＳ２２６において、３次元位置計算部３８０は、ｉ−１番目のキーフレームとの間で対応付けた特徴点のうち、３次元位置が未知の特徴点について、ｉ番目のキーフレーム上の特徴点の位置と撮影装置１００の位置及び姿勢の変化量とから三角測量の原理により当該特徴点の３次元位置を計算する。
【０１０８】
ステップＳ２２７において、３次元位置計算部３８０は、全てのキーフレームについて処理を行ったか否かを判定する。
【０１０９】
全てのキーフレームについて処理を行っていない場合（上記ステップＳ２２７のＮＯ）、ステップＳ２２８において、３次元位置計算部３８０は、ｉに１を加算し、ステップＳ２２２に戻る。
【０１１０】
一方、全てのキーフレームについて処理を行っている場合（上記ステップＳ２２７のＹＥＳ）、リターンする。
【０１１１】
以上説明したように、本発明の実施の形態に係る地図作成装置によれば、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、撮影ノードの画像上の、特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、撮影ノードの画像間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データの特徴点の３次元位置、及び撮影装置の位置及び姿勢を最適化することにより、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる。
【０１１２】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０１１３】
例えば、上記実施の形態では、地図作成装置３００が車両に搭載されている場合を例に説明したが、地図作成装置３００がインターネット上のサーバとして構成されてもよい。
【０１１４】
この場合、撮影装置１００、車輪速センサ２００、及び操舵角センサ２１０は、無線通信などにより、インターネットを介して地図作成装置３００に画像、車輪速、及び操舵角をそれぞれ送信するように構成すればよい。
【０１１５】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【０１１６】
１０   地図作成システム
１００ 撮影装置
２００ 車輪速センサ
２１０ 操舵角センサ
３００ 地図作成装置
３１０ センサ情報入力部
３２０ オドメトリ計算部
３３０ 画像入力部
３４０ キーフレーム選択部
３５０ 特徴点抽出部
３６０ 特徴量算出部
３７０ 特徴点対応付け部
３８０ ３次元位置計算部
３９０ グラフ構造データ生成部
４００ 誤差関数設定部
４１０ グラフ構造データ最適化部
４２０ 環境地図作成部
４３０ 出力部
【図１】
2020122754.tif 000007
【図２】
2020122754.tif 000008
【図３】
2020122754.tif 000009
【図４】
2020122754.tif 000010
【図５】
2020122754.tif 000011
【図６】
2020122754.tif 000012
【図７】
2020122754.tif 000013