JP7173471B2 - ３次元位置推定装置及びプログラム - Google Patents

３次元位置推定装置及びプログラム Download PDF

Info

Publication number
JP7173471B2
JP7173471B2 JP2019015930A JP2019015930A JP7173471B2 JP 7173471 B2 JP7173471 B2 JP 7173471B2 JP 2019015930 A JP2019015930 A JP 2019015930A JP 2019015930 A JP2019015930 A JP 2019015930A JP 7173471 B2 JP7173471 B2 JP 7173471B2
Authority
JP
Japan
Prior art keywords
feature point
photographing
node
orientation
unit
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2019015930A
Other languages
English (en)
Other versions
JP2020122754A (ja
Inventor
彰司 浅井
周 浅海
和孝 早川
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Central R&D Labs Inc
Aisin Corp
Original Assignee
Aisin Seiki Co Ltd
Toyota Central R&D Labs Inc
Aisin Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Aisin Seiki Co Ltd, Toyota Central R&D Labs Inc, Aisin Corp filed Critical Aisin Seiki Co Ltd
Priority to JP2019015930A priority Critical patent/JP7173471B2/ja
Priority to CN202010074956.1A priority patent/CN111508025A/zh
Priority to US16/776,669 priority patent/US11263774B2/en
Publication of JP2020122754A publication Critical patent/JP2020122754A/ja
Application granted granted Critical
Publication of JP7173471B2 publication Critical patent/JP7173471B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/114—Yaw movement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/757—Matching configurations of points or features
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10004—Still image; Photographic image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Computing Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Mathematical Physics (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Description

本発明は、３次元位置推定装置及びプログラムに関する。
従来から、車両に搭載されたカメラで周辺環境を撮影し、撮影された画像から環境地図を作成する技術がある（非特許文献１）。
非特許文献１では、画像から特徴点を検出し、当該特徴点の局所特徴量を用いて、複数地点で撮影された画像間の特徴点を対応付け、対応付けた特徴点の画像上の位置から、カメラの位置・姿勢と特徴点の３次元位置を算出し、複数の特徴点の３次元位置を規定する特徴点ノードと、複数の撮影地点におけるカメラの位置・姿勢を規定するカメラノードと、各撮影地点における画像上の特徴点位置を規定する特徴点エッジと、隣接するカメラノード間における車両の位置・姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジと撮影エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化することにより、環境地図を作成する。
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.
そして、特徴点検出部３５０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点を、特徴量算出部３６０に渡す。
特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点の各々の特徴量を算出する。
具体的には、特徴量算出部３６０は、複数のキーフレームの各々についての複数の特徴点から、ＯＲＢ（参考文献１）等の局所特徴量を算出する。
そして、特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点と、当該複数の特徴点の各々についての特徴量を、特徴点対応付け部３７０に渡す。
特徴点対応付け部３７０は、特徴量算出部３６０により算出された特徴点の特徴を示す特徴量を用いて、連続するキーフレーム間における特徴点を対応付ける。
具体的には、特徴点対応付け部３７０は、特徴点の各々についての特徴量を用いて、連続するキーフレーム間での特徴点を対応付ける。
そして、特徴点対応付け部３７０は、複数のキーフレームの各々について、対応付けた特徴点を、３次元位置計算部３８０に渡す。
３次元位置計算部３８０は、特徴点対応付け部３７０により対応付けられた特徴点の各々についての３次元位置と、複数の画像の各々についての撮影装置１００の位置及び姿勢とを計算する。
具体的には、３次元位置計算部３８０は、２番目のキーフレームから順番に、一つ前のキーフレームとの間の特徴点の対応付けに基づいて、各特徴点の３次元位置を計算する。
３次元位置計算部３８０は、２番目のキーフレームの場合、まず、１番目のキーフレームとの間で、８点アルゴリズムとＲＡＮＳＡＣ（Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓｕｓ）とを用いて、１番目のキーフレームの撮影位置と、２番目のキーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量（無次元量）を計算し、車両の位置変化量（ΔＸｖ，ΔＹｖ）を用いて有次元化（絶対的なスケールを決定）する。
次に、３次元位置計算部３８０は、図３に示すように、対応付けた特徴点のキーフレーム上の位置と撮影装置１００の位置及び姿勢の変化量とから、三角測量の原理により当該特徴点の３次元位置（ワールド座標系）を計算する。
３次元位置計算部３８０は、３番目以降のキーフレームの場合、図４に示すように、まず、１つ前のキーフレームとの間で対応付けた特徴点のうち、３次元位置が既知の特徴点を抽出し、その再投影誤差の和が最小となる撮影装置１００の位置及び姿勢の変化量を計算する。
次に、３次元位置計算部３８０は、１つ前のキーフレームとの間で対応付けた特徴点のうち、３次元位置が未知の特徴点について、２番目のキーフレームと同様に、キーフレーム上の特徴点の位置と撮影装置１００の位置及び姿勢の変化量とから三角測量の原理により当該特徴点の３次元位置を計算する。
そして、３次元位置計算部３８０は、計算して得られた複数の特徴点の各々についての３次元位置と、複数のキーフレームの各々についての、当該キーフレームの１つ前のキーフレームの撮影位置と、当該キーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量とを、グラフ構造データ生成部３９０に渡す。
グラフ構造データ生成部３９０は、特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、複数のキーフレームの各々についての、当該キーフレームを撮影した撮影装置１００の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、当該撮影ノードのキーフレーム上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、当該撮影ノードのキーフレーム間における車両の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。
具体的には、グラフ構造データ生成部３９０は、図５に示すように、特徴点Ｐｉの３次元位置を規定する特徴点ノードと、キーフレームＫＦｊのカメラ位置・姿勢を規定するカメラノードと、キーフレームＫＦｊ上の特徴点Ｐｉの位置を規定する特徴点エッジと、隣接するキーフレーム間における車両の位置変化量とヨー角変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。
具体的には、グラフ構造データ生成部３９０は、３次元位置計算部３８０により得られた各特徴点の３次元位置を用いて、グラフ構造データを生成する。
そして、グラフ構造データ生成部３９０は、生成したグラフ構造データを、誤差関数設定部４００に渡す。
誤差関数設定部４００は、グラフ構造データ生成部３９０により生成されたグラフ構造データに含まれる特徴点エッジの各々について、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置１００の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数ＣＰｉｊを設定すると共に、グラフ構造データに含まれる撮影エッジの各々について、当該撮影エッジが規定する位置及び姿勢の変化量とオドメトリ計算部３２０による計算結果との誤差を表す第２誤差関数ＣＫＦｊを設定する。
具体的には、誤差関数設定部４００は、特徴点エッジの第１誤差関数ＣＰｉｊを下記式（１）、（２）のように設定する。
Figure 0007173471000001
ｅＰｉｊはキーフレームＫＦｊ上における特徴点Ｐｉの（ｘ，ｙ）座標の再投影誤差を表す２行１列のベクトルであり、
Figure 0007173471000002

と表すことができる。ここで、（ｘＰｉｊ，ｙＰｉｊ）は、特徴点Ｐｉの実際の座標であり、’＾’（ハット記号）付きは画像上に投影したときの計算値である。
ＷＰｉｊは再投影誤差に対する重みであり、画像ピラミッドの解像度が高い階層から検出した特徴点に対しては大きく設定する。
また、誤差関数設定部４００は、撮影エッジの第２誤差関数ＣＫＦｊを下記式（３）、（４）のように設定する。
Figure 0007173471000003
ここで、ｅＫＦｊはキーフレームＫＦｊ－１とＫＦｊの間おける、カメラ位置・姿勢の変化量から換算した車両の位置変化量とヨー角変化量の、計測値に対する誤差を表す３行１列のベクトルであり、
Figure 0007173471000004

と表すことができる。ここで、’＾’付きは計算値である。
また、ｗＸｊは、車両のＸ方向の位置変化量の誤差に対する重みであり、ｗＹｊは、車両のＹ方向の位置変化量の誤差に対する重みであり、ｗθｊは、車両のヨー角変化量の誤差に対する重みである。
ここで、誤差関数設定部４００は、撮影エッジの各々について、撮影エッジに対応するオドメトリ計算部３２０による車両のヨー角変化量Δθｖが小さいほど大きな重みｗＸｊ、ｗＹｊ、及びｗθｊを含んで表される第２誤差関数ＣＫＦｊを設定する。
すなわち、誤差関数設定部４００は、車両のヨー角変化量Δθｖが小さい撮影エッジほど重みｗＸｊ、ｗＹｊ、及びｗθｊを大きく設定する。
ここで、ｗＸｊ＝ｗＹｊとなるように設定してもよい。これにより、メモリ容量を節約することができる。
そして、誤差関数設定部４００は、グラフ構造データと、第１誤差関数ＣＰｉｊと、第２誤差関数ＣＫＦｊとを、グラフ構造データ最適化部４１０に渡す。
グラフ構造データ最適化部４１０は、特徴点エッジの各々についての第１誤差関数ＣＰｉｊと、撮影エッジの各々についての第２誤差関数ＣＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。
具体的には、グラフ構造データ最適化部４１０は、非特許文献１と同様に、特徴点エッジの各々についての第１誤差関数ＣＰｉｊと、撮影エッジの各々についての第２誤差関数ＣＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。
ここで、車両のヨー角変化量Δθｖが小さい撮影エッジほど第２誤差関数ＣＫＦｊの重みｗＸｊ、ｗＹｊ、及びｗθｊが大きく設定されていることにより、車両の位置変化量（ΔＸｖ，ΔＹｖ）の取得精度が比較的高い撮影エッジの誤差関数が小さくなるようにグラフ構造データは最適化される。これにより、グラフ構造データを構成する特徴点３次元位置とカメラ位置・姿勢の精度を全体的に高めることができる。
そして、グラフ構造データ最適化部４１０は、最適化したグラフ構造データを、環境地図作成部４２０に渡す。
環境地図作成部４２０は、グラフ構造データ最適化部４１０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。
具体的には、環境地図作成部４２０は、非特許文献１と同様に、グラフ構造データ最適化部４１０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。ここで、非特許文献１に比べて特徴点の３次元位置の精度が高いため、歪みの小さい地図を作成できる。
そして、環境地図作成部４２０は、作成した環境地図を、出力部４３０に渡す。
出力部４３０は、環境地図作成部４２０により作成された環境地図を出力する。
＜地図作成装置３００の動作＞
次に、図６を参照して、本実施形態の地図作成装置３００の地図作成処理ルーチンについて説明する。
まず、ステップＳ１００において、地図作成装置３００は、ｉ＝１とする。ここで、ｉは、選択するキーフレームの数を数えるカウンタである。
ステップＳ１１０において、センサ情報入力部３１０は、車輪速センサ２００により検出された車輪速、及び操舵角センサ２１０により検出された操舵角の入力を受け付ける。
ステップＳ１２０において、オドメトリ計算部３２０は、上記ステップＳ１１０で得られた車両に関するセンサ情報に基づいて、車両の位置変化量（ΔＸｖ，ΔＹｖ）及びヨー角変化量Δθｖを計算する。
ステップＳ１３０において、画像入力部３３０は、撮影装置１００から画像の入力を受け付ける。
ステップＳ１４０において、キーフレーム選択部３４０は、ｉ＝１であるか否かを判定する。
ｉ＝１である場合（上記ステップＳ１４０のＹＥＳ）、ステップＳ１６０に進む。
一方、ｉ＝１でない場合（上記ステップＳ１４０のＮＯ）、ステップＳ１５０において、キーフレーム選択部３４０は、車両の走行距離が所定値以上であるか否かを判定する。
走行距離が所定値以上である場合（上記ステップＳ１５０のＹＥＳ）、ステップＳ１６０に進む。
一方、走行距離が所定値以上でない場合（上記ステップＳ１５０のＮＯ）、ステップＳ１１０に戻る。
ステップＳ１６０において、キーフレーム選択部３４０は、上記ステップＳ１３０により取得した画像をｉ番目のキーフレームとして選択する。
ステップＳ１７０において、キーフレーム選択部３４０は、全てのキーフレームを選択したか否かを判定する。
全てのキーフレームを選択していない場合（上記ステップＳ１７０のＮＯ）、ステップＳ１８０において、キーフレーム選択部３４０は、ｉに１を加算し、ステップＳ１１０に戻る。
一方、全てのキーフレームを選択している場合（上記ステップＳ１７０のＹＥＳ）、ステップＳ１９０において、特徴点検出部３５０は、複数のキーフレームの各々から複数の特徴点を検出する。
ステップＳ２００において、特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点の各々の特徴量を算出する。
ステップＳ２１０において、特徴点対応付け部３７０は、上記ステップＳ２００により算出された特徴点の特徴を示す特徴量を用いて、複数のキーフレーム間における特徴点を対応付ける。
ステップＳ２２０において、３次元位置計算部３８０は、上記ステップＳ２１０により対応付けられた特徴点の各々についての３次元位置と、複数のキーフレームの各々についての撮影装置１００の位置及び姿勢とを計算する。
ステップＳ２３０において、グラフ構造データ生成部３９０は、特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、複数のキーフレームの各々についての、当該キーフレームを撮影した撮影装置１００の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、当該撮影ノードのキーフレーム上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、当該撮影ノードのキーフレーム間における車両の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。
ステップＳ２４０において、誤差関数設定部４００は、上記ステップＳ２３０により生成されたグラフ構造データに含まれる特徴点エッジの各々について、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置１００の位置及び姿勢を用いて撮影ノードのキーフレーム上に投影したときの投影誤差を表す第１誤差関数ＣＰｉｊを設定すると共に、グラフ構造データに含まれる撮影エッジの各々について、当該撮影エッジが規定する位置及び姿勢の変化量とオドメトリ計算部３２０による計算結果との誤差を表す第２誤差関数ＣＫＦｊを設定する。
ステップＳ２５０において、グラフ構造データ最適化部４１０は、特徴点エッジの各々についての第１誤差関数ＣＰｉｊと、撮影エッジの各々についての第２誤差関数ＣＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。
ステップＳ２６０において、環境地図作成部４２０は、上記ステップＳ２５０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。
ステップＳ２７０において、出力部４３０は、上記ステップＳ２６０により作成された環境地図を出力する。
また、上記ステップＳ２２０は、図７に示す３次元位置計算処理ルーチンによって実現される。
ステップＳ２２１において、３次元位置計算部３８０は、ｉ＝２とする。ここで、ｉは、キーフレームが撮影された番号を示すカウンタである。
ステップＳ２２２において、３次元位置計算部３８０は、ｉ番目のキーフレームを選択する。
ステップＳ２２３において、３次元位置計算部３８０は、ｉ＝２であるか否かを判定する。
ｉ＝２である場合（上記ステップＳ２２７のＹＥＳ）、ステップＳ２２４において、３次元位置計算部３８０は、１番目のキーフレームとの間で、８点アルゴリズムとＲＡＮＳＡＣ（Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓｕｓ）とを用いて、１番目のキーフレームの撮影位置と、２番目のキーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量（無次元量）を計算し、車両の位置変化量（ΔＸｖ，ΔＹｖ）を用いて有次元化し、ステップＳ２２６に進む。
一方、ｉ＝２でない場合（上記ステップＳ２２７のＮＯ）、ステップＳ２２５において、３次元位置計算部３８０は、ｉ－１番目のキーフレームとの間で対応付けた特徴点のうち、３次元位置が既知の特徴点を抽出し、その再投影誤差の和が最小となる撮影装置１００の位置及び姿勢の変化量を計算し、ステップＳ２２６に進む。
ステップＳ２２６において、３次元位置計算部３８０は、ｉ－１番目のキーフレームとの間で対応付けた特徴点のうち、３次元位置が未知の特徴点について、ｉ番目のキーフレーム上の特徴点の位置と撮影装置１００の位置及び姿勢の変化量とから三角測量の原理により当該特徴点の３次元位置を計算する。
ステップＳ２２７において、３次元位置計算部３８０は、全てのキーフレームについて処理を行ったか否かを判定する。
全てのキーフレームについて処理を行っていない場合（上記ステップＳ２２７のＮＯ）、ステップＳ２２８において、３次元位置計算部３８０は、ｉに１を加算し、ステップＳ２２２に戻る。
一方、全てのキーフレームについて処理を行っている場合（上記ステップＳ２２７のＹＥＳ）、リターンする。
以上説明したように、本発明の実施の形態に係る地図作成装置によれば、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、撮影ノードの画像上の、特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、撮影ノードの画像間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データの特徴点の３次元位置、及び撮影装置の位置及び姿勢を最適化することにより、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる。
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
例えば、上記実施の形態では、地図作成装置３００が車両に搭載されている場合を例に説明したが、地図作成装置３００がインターネット上のサーバとして構成されてもよい。
この場合、撮影装置１００、車輪速センサ２００、及び操舵角センサ２１０は、無線通信などにより、インターネットを介して地図作成装置３００に画像、車輪速、及び操舵角をそれぞれ送信するように構成すればよい。
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
１０ 地図作成システム
１００ 撮影装置
２００ 車輪速センサ
２１０ 操舵角センサ
３００ 地図作成装置
３１０ センサ情報入力部
３２０ オドメトリ計算部
３３０ 画像入力部
３４０ キーフレーム選択部
３５０ 特徴点抽出部
３６０ 特徴量算出部
３７０ 特徴点対応付け部
３８０ ３次元位置計算部
３９０ グラフ構造データ生成部
４００ 誤差関数設定部
４１０ グラフ構造データ最適化部
４２０ 環境地図作成部
４３０ 出力部

Claims (3)

  1. 移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部と、
    前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部と、
    前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部と、
    前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部と、
    前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、
    前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部と、
    前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部と、
    を含み、
    前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
    ３次元位置推定装置。
  2. 前記位置姿勢変化量計算部は、前記移動体に搭載された操舵角センサにより検出された操舵角に基づいて、前記移動体のヨー角変化量を計算し、
    前記誤差関数設定部は、前記位置姿勢変化量計算部による計算結果に基づいて得られる、前記撮影エッジに対応する前記移動体のヨー角変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
    請求項１記載の３次元位置推定装置。
  3. コンピュータを、
    移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部、
    前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部、
    前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部、
    前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部、
    前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部、
    前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部、及び
    前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部
    として機能させ、
    前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
    プログラム。
JP2019015930A 2019-01-31 2019-01-31 ３次元位置推定装置及びプログラム Active JP7173471B2 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
JP2019015930A JP7173471B2 (ja) 2019-01-31 2019-01-31 ３次元位置推定装置及びプログラム
CN202010074956.1A CN111508025A (zh) 2019-01-31 2020-01-22 三维位置估计装置以及程序
US16/776,669 US11263774B2 (en) 2019-01-31 2020-01-30 Three-dimensional position estimation device and program

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2019015930A JP7173471B2 (ja) 2019-01-31 2019-01-31 ３次元位置推定装置及びプログラム

Publications (2)

Publication Number Publication Date
JP2020122754A JP2020122754A (ja) 2020-08-13
JP7173471B2 true JP7173471B2 (ja) 2022-11-16

Family

ID=71837577

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2019015930A Active JP7173471B2 (ja) 2019-01-31 2019-01-31 ３次元位置推定装置及びプログラム

Country Status (3)

Country Link
US (1) US11263774B2 (ja)
JP (1) JP7173471B2 (ja)
CN (1) CN111508025A (ja)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11361466B2 (en) 2018-11-30 2022-06-14 Casio Computer Co., Ltd. Position information acquisition device, position information acquisition method, recording medium, and position information acquisition system
CN114531580B (zh) * 2020-11-23 2023-11-21 北京四维图新科技股份有限公司 图像处理方法及装置
KR102421067B1 (ko) * 2020-12-02 2022-07-13 한국항공대학교산학협력단 카메라 자세와 대상 물체의 3d 좌표를 동시에 추정하는 동시 추정 장치 및 방법
CN113390408A (zh) * 2021-06-30 2021-09-14 深圳市优必选科技股份有限公司 一种机器人定位方法、装置、机器人及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004198212A (ja) 2002-12-18 2004-07-15 Aisin Seiki Co Ltd 移動体周辺監視装置
US20090228204A1 (en) 2008-02-04 2009-09-10 Tela Atlas North America, Inc. System and method for map matching with sensor detected objects
WO2015049717A1 (ja) 2013-10-01 2015-04-09 株式会社日立製作所 移動***置推定装置および移動***置推定方法
JP2017156154A (ja) 2016-02-29 2017-09-07 株式会社豊田中央研究所 状態推定装置、情報端末、情報処理システム、及びプログラム
JP2018205950A (ja) 2017-06-01 2018-12-27 株式会社豊田中央研究所 自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、及び自車位置推定プログラム

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102005037841B4 (de) * 2005-08-04 2010-08-12 Gesellschaft zur Förderung angewandter Informatik e.V. Verfahren und Anordnung zur Bestimmung der relativen Lage eines ersten Objektes bezüglich eines zweiten Objektes, sowie ein entsprechendes Computerprogramm und ein entsprechendes computerlesbares Speichermedium
JP4957134B2 (ja) * 2006-09-12 2012-06-20 株式会社日立製作所 距離計測装置
EP1972893A1 (en) * 2007-03-21 2008-09-24 Universiteit Gent System and method for position determination
US9286810B2 (en) 2010-09-24 2016-03-15 Irobot Corporation Systems and methods for VSLAM optimization
US9462446B2 (en) * 2012-12-28 2016-10-04 Trimble Navigation Limited Collecting external accessory data at a mobile data collection platform that obtains raw observables from an internal chipset
US9646201B1 (en) * 2014-06-05 2017-05-09 Leap Motion, Inc. Three dimensional (3D) modeling of a complex control object
US10048058B2 (en) * 2015-07-29 2018-08-14 Microsoft Technology Licensing, Llc Data capture system for texture and geometry acquisition
US9946264B2 (en) * 2016-03-22 2018-04-17 Sharp Laboratories Of America, Inc. Autonomous navigation using visual odometry
CN107449434B (zh) * 2016-05-31 2020-10-16 法拉第未来公司 使用定位估计误差界限进行安全车辆导航
JP2019011971A (ja) * 2017-06-29 2019-01-24 株式会社東芝 推定システムおよび自動車
US10515458B1 (en) * 2017-09-06 2019-12-24 The United States Of America, As Represented By The Secretary Of The Navy Image-matching navigation method and apparatus for aerial vehicles
DE112019001421T5 (de) * 2018-03-20 2021-02-18 Mobileye Vision Technologies Ltd. Systeme und verfahren zum navigieren eines fahrzeugs

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004198212A (ja) 2002-12-18 2004-07-15 Aisin Seiki Co Ltd 移動体周辺監視装置
US20090228204A1 (en) 2008-02-04 2009-09-10 Tela Atlas North America, Inc. System and method for map matching with sensor detected objects
WO2015049717A1 (ja) 2013-10-01 2015-04-09 株式会社日立製作所 移動***置推定装置および移動***置推定方法
JP2017156154A (ja) 2016-02-29 2017-09-07 株式会社豊田中央研究所 状態推定装置、情報端末、情報処理システム、及びプログラム
JP2018205950A (ja) 2017-06-01 2018-12-27 株式会社豊田中央研究所 自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、及び自車位置推定プログラム

Also Published As

Publication number Publication date
US20200250849A1 (en) 2020-08-06
US11263774B2 (en) 2022-03-01
JP2020122754A (ja) 2020-08-13
CN111508025A (zh) 2020-08-07

Similar Documents

Publication Publication Date Title
JP7173471B2 (ja) ３次元位置推定装置及びプログラム
CN112785702B (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN108986037B (zh) 基于半直接法的单目视觉里程计定位方法及定位***
US10762643B2 (en) Method for evaluating image data of a vehicle camera
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
JP6830140B2 (ja) 運動ベクトル場の決定方法、運動ベクトル場の決定装置、機器、コンピュータ読み取り可能な記憶媒体及び車両
JP4943034B2 (ja) ステレオ画像処理装置
JP2018124787A (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
KR101890612B1 (ko) 적응적 관심영역 및 탐색창을 이용한 객체 검출 방법 및 장치
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
WO2023004956A1 (zh) 一种在高动态环境下的激光slam方法、***设备及存储介质
JP2015011032A (ja) ３次元物体形状及び姿勢の推定並びに追跡の方法及び装置
JP6905390B2 (ja) 自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、及び自車位置推定プログラム
CN112017236A (zh) 一种基于单目相机计算目标物位置的方法及装置
CN114919584A (zh) 机动车定点目标测距方法、装置及计算机可读存储介质
CN115015956A (zh) 一种室内无人车的激光与视觉slam***
JP4921847B2 (ja) 対象物の三次元位置推定装置
CN111553342A (zh) 一种视觉定位方法、装置、计算机设备和存储介质
CN117015719A (zh) 用于确定刚体的运动状态的方法
CN113124906A (zh) 基于在线标定的测距方法、装置及电子设备
CN114119885A (zh) 图像特征点匹配方法、装置及***、地图构建方法及***
CN116934829B (zh) 无人机目标深度估计的方法、装置、存储介质及电子设备
JP7462847B1 (ja) 位置推定装置、位置推定方法、及び位置推定プログラム
WO2022198603A1 (en) Real-time simultaneous localization and mapping using an event camera
CN113870367B (zh) 相机外参的生成方法、装置、设备、存储介质和程序产品

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20211216

TRDD Decision of grant or rejection written
A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20220930

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20221004

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20221025

R150 Certificate of patent or registration of utility model

Ref document number: 7173471

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150