WO2022130618A1 - 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム - Google Patents

位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム Download PDF

Info

Publication number
WO2022130618A1
WO2022130618A1 PCT/JP2020/047417 JP2020047417W WO2022130618A1 WO 2022130618 A1 WO2022130618 A1 WO 2022130618A1 JP 2020047417 W JP2020047417 W JP 2020047417W WO 2022130618 A1 WO2022130618 A1 WO 2022130618A1
Authority
WO
WIPO (PCT)
Prior art keywords
posture
absolute
unit
frame
absolute position
Prior art date
Application number
PCT/JP2020/047417
Other languages
English (en)
French (fr)
Inventor
健 宮本
Original Assignee
三菱電機株式会社
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 三菱電機株式会社 filed Critical 三菱電機株式会社
Priority to JP2022569662A priority Critical patent/JP7258250B2/ja
Priority to CN202080107829.5A priority patent/CN116635891A/zh
Priority to KR1020237018897A priority patent/KR20230087613A/ko
Priority to DE112020007700.4T priority patent/DE112020007700T5/de
Priority to PCT/JP2020/047417 priority patent/WO2022130618A1/ja
Priority to TW110117868A priority patent/TWI817124B/zh
Publication of WO2022130618A1 publication Critical patent/WO2022130618A1/ja
Priority to US18/138,300 priority patent/US20230260149A1/en

Links

Images

Classifications

    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
    • 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
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • 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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • G01C21/1656—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 combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • 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
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3844—Data obtained from position sensors only, e.g. from inertial navigation
    • 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/3863—Structures of map data
    • G01C21/387—Organisation of map data, e.g. version management or database structures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • 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
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching
    • 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/761—Proximity, similarity or dissimilarity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/20—Scenes; Scene-specific elements in augmented reality scenes
    • 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
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04N—PICTORIAL COMMUNICATION, e.g. TELEVISION
    • H04N23/00—Cameras or camera modules comprising electronic image sensors; Control thereof
    • H04N23/60—Control of cameras or camera modules
    • 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/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • 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

Definitions

  • This disclosure relates to a position / attitude estimation device, a position / attitude estimation method, and a program.
  • Position / attitude by combining the result of relative position / attitude calculation and the result of absolute position / attitude calculation in a device to which Augmented Reality (AR) is applied and a robot to which Automated Guided Vehicle (AGV) is applied.
  • AR Augmented Reality
  • AGV Automated Guided Vehicle
  • the calculation of relative position / posture is the calculation of the relative amount of movement from a certain position / posture, and is a process that is repeatedly executed at regular intervals (generally, short cycles).
  • a Simultaneus Localization And Mapping SLAM
  • a gyro sensor gyro sensor
  • an acceleration sensor etc.
  • the method using the Industrial Measurement Unit (IMU) integrated with the above, or the autonomous navigation that obtains the amount of movement based on the number of rotations of the wheels, etc. are used.
  • the calculation of the absolute position / posture is performed using a three-dimensional map prepared in advance, and is executed in front of the object indicated by the three-dimensional map, for example.
  • the absolute position / attitude is calculated using, for example, a three-dimensional map and a camera image.
  • FIG. 1 shows an example in which a terminal 111 estimates a position / posture by using a relative position / posture calculation and an absolute position / posture calculation when a user 112 carrying a terminal 111 to which AR is applied moves. It is a figure which shows.
  • the terminal 111 is, for example, a tablet terminal or a terminal using a Head Mounted Display (HMD).
  • HMD Head Mounted Display
  • the terminal 111 moves while executing the relative position / attitude calculation (1), and the error accumulated using the result of the absolute position / attitude calculation (2) executed in front of the object 113 shown by the three-dimensional map. After that, move while executing the relative position / posture calculation (3).
  • the position and attitude can be estimated accurately even if the object 113 is separated from the object 113 shown by the 3D map, and as a result, the AR content is continuously superimposed and displayed at an appropriate position on the actual image displayed on the screen of the terminal. be able to.
  • FIG. 2 is a diagram showing an example in which when the robot 121 to which the AGV is applied moves, the robot 121 estimates the position / posture using the calculation of the relative position / posture and the calculation of the absolute position / posture.
  • the robot 121 moves while executing the relative position / attitude calculation (4), and is accumulated using the result of the absolute position / attitude calculation (5) executed in front of the object 123 shown by the three-dimensional map. After removing the error, move while executing the relative position / attitude calculation (6).
  • the robot 121 can accurately estimate its own position and posture even if it is away from the object 123 shown in the three-dimensional map, and as a result, it can accurately reach the target position.
  • the accuracy of absolute position / posture calculation may vary depending on the pattern of the subject. That is, if the pattern of the subject is characteristic, the absolute position / orientation can be calculated with high accuracy, but if the pattern of the subject is a repeating pattern such as a stripe or a border, or the subject is a pure white wall. In the case of an object without a pattern, such as, the accuracy of calculating the absolute position / posture decreases.
  • the accuracy of the calculation of the absolute position / posture varies greatly depending on the shape of the subject. That is, if the shape of the subject is characteristic, the absolute position / posture can be calculated with high accuracy, but if the shape of the subject is not characteristic, the calculation of the absolute position / posture is accurate. Decreases.
  • the present disclosure has been made to solve the above-mentioned problems, and provides a position / posture estimation device, a position / posture estimation method, and a program capable of improving the accuracy of position / posture estimation.
  • the purpose is a position / posture estimation device, a position / posture estimation method, and a program capable of improving the accuracy of position / posture estimation. The purpose.
  • the position / orientation estimation device of the present disclosure is a process of selecting a frame to be used for position / orientation calculation from a database reading unit that reads 3D map data from a database and frames of images taken from a plurality of different viewpoints.
  • a frame selection unit that performs processing, a relative position / attitude acquisition unit that performs processing for acquiring a plurality of relative positions / postures for a plurality of frames selected by the frame selection unit, and a plurality of frames selected by the frame selection unit.
  • the absolute position / attitude calculation unit that performs processing to acquire a plurality of absolute positions / attitudes about the frame, the relative position / attitude acquired by the relative position / attitude acquisition unit, and the absolute position / attitude calculation unit acquired by the absolute position / attitude calculation unit. It is characterized by having an absolute position / posture integration unit that integrates the absolute position / posture and acquires the final absolute position / posture.
  • the position / orientation estimation method of the present disclosure is a method executed by the position / attitude estimation device, in which a step of reading 3D map data from a position database and a position among frames of images taken from a plurality of different viewpoints.
  • a step for selecting a frame to be used for calculating the posture a step for acquiring a plurality of relative positions / postures for the selected plurality of frames, and a step for acquiring the plurality of selected frames.
  • the final absolute position / posture is acquired by integrating the step of acquiring a plurality of absolute positions / postures, the acquired relative position / posture, and the acquired absolute position / posture. It is characterized by having a step and.
  • the accuracy of position / posture estimation can be improved.
  • FIG. 1 It is a figure which shows an example of the process which adds a random pattern to an image. It is a figure which shows the process of aligning and registering a 3D map on a floor map. It is a functional block diagram which shows schematic structure of the position / posture estimation apparatus which concerns on Embodiment 1.
  • FIG. It is a flowchart which shows the example of the process for making a 3D map by the 3D map making apparatus which concerns on Embodiment 1.
  • FIG. It is a flowchart which shows the example of the process for estimating the position / posture by the position / posture estimation apparatus which concerns on Embodiment 1.
  • FIG. 1 It is a flowchart which shows the other example of the process for estimating the position / posture by the position / posture estimation apparatus which concerns on Embodiment 1.
  • FIG. 2 It is a functional block diagram which shows schematic structure of the 3D map making apparatus which concerns on Embodiment 2. It is a figure which shows the calculation method of the variance used by the 3D cartography apparatus which concerns on Embodiment 2.
  • FIG. 3 is a functional block diagram schematically showing a configuration of a position / posture estimation device according to a second embodiment. It is a flowchart which shows the example of the process for making a 3D map by the 3D map making apparatus which concerns on Embodiment 2.
  • FIG. 3 is a functional block diagram schematically showing a configuration of a position / posture estimation device according to a third embodiment. It is a flowchart which shows the example of the process for estimating the position / posture by the position / posture estimation apparatus which concerns on Embodiment 3.
  • FIG. 3 is a functional block diagram schematically showing a configuration of a position / posture estimation device according to a fourth embodiment. It is a flowchart which shows the other example of the process for estimating the position / posture by the position / posture estimation apparatus which concerns on Embodiment 4.
  • position / posture estimation device means a position and a posture.
  • Position means, for example, the position of a terminal or robot equipped with a camera.
  • Posture means, for example, a shooting direction by a camera or a measurement direction by a distance sensor.
  • improvement of the accuracy of absolute position / posture calculation using a camera image that is, an captured image which is an image taken by a camera is obtained. Describe.
  • a first estimation method is known as a method of estimating a position / posture using a camera image. See, for example, Non-Patent Document 1.
  • the first estimation method uses direct matching.
  • the position / orientation of the camera is directly calculated based on the set of local features of the image.
  • a second estimation method is known as a method of estimating a position / posture using a camera image.
  • the position and orientation of the camera are estimated from the image by Convolutional Neural Network (CNN). See, for example, Non-Patent Document 2.
  • CNN Convolutional Neural Network
  • the second estimation method uses two-step matching.
  • a plurality of images having different viewpoints are prepared, first, from among the plurality of images, the image most similar to the camera image obtained by shooting is specified as a similar image, and then similar.
  • the position and orientation of the camera are obtained based on the correspondence between the local features extracted from the image and the local features extracted from the camera image.
  • a method based on the second estimation method is used.
  • FIG. 3 is a diagram showing an example of the hardware configuration of the position / posture estimation device 101 and the position / posture estimation system 100 including the position / posture estimation device 101 according to the first embodiment.
  • the position / posture estimation device 101 according to the first embodiment has a computer that is a computer that executes a calculation for estimating the position / posture.
  • the position / attitude estimation system 100 according to the first embodiment includes a position / attitude estimation device 101, a three-dimensional map database (three-dimensional map DB) 102 stored in a storage device, and a distance sensor 103. It has a camera 104 as an image pickup device and a display 105 such as a liquid crystal display device. Further, in the example of FIG.
  • the position / attitude estimation system 100 has a gyro sensor 106, an acceleration sensor 107, and a geomagnetic sensor 108.
  • a device having a gyro sensor 106, an acceleration sensor 107, and a geomagnetic sensor 108 is also referred to as an "IMU".
  • the position / attitude estimation device 101 and the other configurations shown in FIG. 3 are connected by, for example, a network.
  • the three-dimensional map DB 102 may be a part of the position / attitude estimation device 101.
  • the 3D map DB 102 includes 3D map information prepared in advance, which is used when calculating the absolute position / posture.
  • the three-dimensional map DB 102 does not have to be a part of the position / posture estimation system 100 according to the first embodiment, and may be information stored in an external storage device. Further, the three-dimensional map DB 102 may be created by the position / attitude estimation device 101. In this case, the position / attitude estimation device 101 has a function as a three-dimensional map creation device. That is, the three-dimensional map creating device according to the first embodiment is a part of the position / attitude estimation device 101. However, the three-dimensional map creating device according to the first embodiment may be a device separate from the position / attitude estimation device 101.
  • the distance sensor 103 is a device that measures a distance using infrared rays, a laser, or the like.
  • the camera 104 is a device for acquiring a camera image.
  • the position / attitude estimation system 100 may have only one of the camera 104 and the distance sensor 103.
  • the display 105 is a display device necessary for superimposing and displaying AR contents on a camera image.
  • the position / attitude estimation system 100 may not include the display 105.
  • the gyro sensor 106, the acceleration sensor 107, and the geomagnetic sensor 108 constitute an IMU that is a device for calculating relative positions and attitudes by autonomous navigation. However, if the relative position / attitude calculation is not calculated by autonomous navigation, it is possible not to have an IMU. Further, only one or two of the gyro sensor 106, the acceleration sensor 107, and the geomagnetic sensor 108 may be provided.
  • the device connected to the position / attitude estimation device 101 may be a part of the device shown in FIG. 3, or may include other devices not shown in FIG.
  • FIG. 4 is a diagram showing an example of the hardware configuration of the position / attitude estimation device 101.
  • the position / orientation estimation device 101 includes a CPU (Central Processing Unit) 1011 as an information processing unit, a memory 1012 as a storage device, and an interface 1013.
  • the three-dimensional map DB 102, the distance sensor 103, the camera 104, the display 105, the gyro sensor 106, the acceleration sensor 107, and the geomagnetic sensor 108 are connected to the CPU 1011 via the interface 1013 and the data bus.
  • a CPU Central Processing Unit
  • the three-dimensional map DB 102, the distance sensor 103, the camera 104, the display 105, the gyro sensor 106, the acceleration sensor 107, and the geomagnetic sensor 108 are connected to the CPU 1011 via the interface 1013 and the data bus.
  • the processing circuit may be dedicated hardware or a CPU 1011 that executes a program (for example, a position / attitude estimation program) that is software stored in the memory 1012.
  • the CPU 1011 may be any of a processing device, an arithmetic unit, a microprocessor, a microcomputer, a processor, and a DSP (Digital Signal Processor).
  • the processing circuit may be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), or an FPGA (Field-Programmable Gate Array). ), Or a combination of any of these.
  • the processing circuit When the processing circuit is CPU 1011 the function of the position / attitude estimation device 101 is realized by software, firmware, or a combination of software and firmware.
  • the software and firmware are described as a program and stored in the memory 1012.
  • the processing circuit realizes the functions of each part by reading and executing the program stored in the memory 1012. That is, the position / posture estimation device 101 executes the position / posture estimation method according to the first embodiment when the processing is executed by the processing circuit.
  • the memory 1012 is, for example, a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read Only Memory), an EEPROM (Electrically) It is one of a volatile semiconductor memory, a magnetic disk, an optical disk, a compact disk, a DVD (Digital Versaille Disc), or the like.
  • RAM Random Access Memory
  • ROM Read Only Memory
  • flash memory an EPROM (Erasable Programmable Read Only Memory)
  • EEPROM Electrically
  • It is one of a volatile semiconductor memory, a magnetic disk, an optical disk, a compact disk, a DVD (Digital Versaille Disc), or the like.
  • a part of the position / attitude estimation device 101 may be realized by dedicated hardware, and a part may be realized by software or firmware.
  • the processing circuit can realize each function by hardware, software, firmware, or a combination thereof.
  • FIG. 5 is a functional block diagram schematically showing the configuration of the 3D map creation device according to the first embodiment.
  • the three-dimensional map creation device shown in FIG. 5 is a device capable of executing the three-dimensional map creation method according to the first embodiment.
  • the three-dimensional map creation device is a part of the position / attitude estimation device 101 (that is, the map creation registration unit)
  • the three-dimensional map creating device may be a device separate from the position / attitude estimation device 101.
  • the hardware configuration of the three-dimensional cartography apparatus is the same as that shown in FIG.
  • the three-dimensional map creating device has a keyframe detection unit 10, a keyframe position / posture calculation unit 11, a position / posture distribution calculation unit 12, and a correspondence relationship registration. It has a unit 13 and a database storage unit (DB storage unit) 14. These configurations use the camera image taken by the camera 104 (FIG. 3), the distance information acquired by the distance sensor 103 (FIG. 3), and the sensor values acquired by the IMU (FIG. 3). Build a 3D map.
  • the minimum required data in the first embodiment is a camera image. Even if the distance information or IMU is not provided, it is possible to create a three-dimensional map.
  • the key frame detection unit 10 moves (that is, rotates) when the position of the camera 104 moves by a predetermined parallel movement amount threshold or more, or when the posture of the camera 104 moves by or more than a predetermined rotation amount threshold value.
  • a process of detecting the acquired image (for example, a color image) and the detected distance information as a key frame is executed.
  • the key frame position / attitude calculation unit 11 is the position of the camera 104 or the distance sensor 103 that captures the key frame detected by the key frame detection unit 10 by a relative position / attitude calculation method using an image such as SLAM. -Execute the process of calculating the posture.
  • the key frame detection unit 10 and the key frame position / attitude calculation unit 11 execute the same processing as the processing in the case of the conventional SLAM technique (for example, the processing described in Non-Patent Document 3).
  • the position / posture distribution calculation unit 12 executes a process of obtaining the distribution of the position / attitude for each of the key frames detected by the key frame detection unit 10.
  • a method for calculating the variance of the position / posture for example, there are a first calculation method and a second calculation method shown below.
  • the first calculation method is a method of adding noise (that is, a random pattern) to an image.
  • FIG. 6 is a diagram showing an example of a method of adding a random pattern to an image of a key frame.
  • the process of adding a random pattern to the key frame image and calculating the position / posture is executed multiple times, and the position / posture is distributed based on the obtained calculation results of the position / posture.
  • the second calculation method is a method using random numbers.
  • Random Sample Consensus (RANSAC) is used for the purpose of excluding outliers.
  • the main purpose of RANSAC is to exclude outlier data from the observed data that are out of the data used for absolute position / attitude calculation.
  • RANSAC Random Sample Consensus
  • a sample to be calculated is randomly selected, and the calculation result has a different value each time. Therefore, RANSAC can be used for calculating the variance of position / posture.
  • Equation (1) and (2) Shows the variance of position ( ⁇ tk 2 ) and the variance of posture ( ⁇ Rk 2 ), respectively, and is calculated by Eqs. (1) and (2), respectively.
  • N is a positive integer and indicates the number of trials used to determine the variance.
  • k indicates a positive integer.
  • n is an integer of 1 or more and N or less.
  • FIG. 7 is a diagram showing a process of aligning and registering a three-dimensional map on the floor map.
  • the floor map of FIG. 7 is a floor layout on which a layout showing the arrangement of equipment is drawn.
  • the installation area which is the area where the equipment should be placed, is indicated by a broken line rectangle.
  • the correspondence relationship registration unit 13 performs a process of defining a relationship with another three-dimensional map or an overall map.
  • a 3D map (indicated by a solid rectangle) is registered on the floor map layout, the positional relationship between the 3D maps and the 3D Chitou being created are already constructed.
  • the correspondence with the existing 3D map can be obtained.
  • the database storage unit 14 stores the three-dimensional map created by the above method (that is, data for each key frame obtained by, for example, SLAM) in the three-dimensional map DB 102 (FIG. 3).
  • the position / orientation, the camera image, the distance information (that is, the distance image), and the point group of the local feature obtained for each key frame are stored for the number of key frames.
  • FIG. 8 is a functional block diagram schematically showing the configuration of the position / posture estimating device 101 according to the first embodiment.
  • the position / posture estimation device 101 performs a process of calculating the position / posture based on a plurality of sensor data taken from different viewpoints.
  • the sensor data includes a camera image and detection data of the distance sensor.
  • the position / attitude estimation device 101 includes a database reading unit 15, a frame selection unit 16, a relative position / attitude acquisition unit 17, an absolute position / attitude calculation unit 18, and an absolute position / attitude. It has an integrated unit 19.
  • the database reading unit 15 performs a process of reading a three-dimensional map (that is, a three-dimensional map prepared in advance) stored in the database stored in the DB storage unit 14.
  • the frame selection unit 16 performs a process of selecting a frame to be used for calculating the position / orientation from the frames of the camera images taken from a plurality of different viewpoints.
  • the frame selection method for example, there are a first selection method, a second selection method, and a third selection method.
  • the relative movement amount is used.
  • the condition that the change in the relative position obtained by the calculation of the relative position / posture is equal to or more than the threshold value of the change in the predetermined position and the change in the relative posture are the changes in the predetermined posture. It is determined whether or not to select a frame (key frame) based on the condition that the value becomes equal to or higher than the threshold value. For example, the condition that the change in the relative position obtained by the calculation of the relative position / posture is equal to or more than the threshold value for the change in the predetermined position and the change in the relative posture is equal to or greater than the threshold value for the change in the predetermined posture. Select a keyframe that meets at least one of the conditions.
  • frames that differ in time are used.
  • frames of viewpoints adjacent in time or frames (key frames) taken at time intervals equal to or longer than the threshold value in time are selected.
  • the third selection method dispersion is used.
  • the data to be used is selected based on the variance obtained at the time of creating the three-dimensional map. For example, a frame (key frame) with a viewpoint whose variance is smaller than a predetermined variance threshold is selected.
  • the relative position / posture acquisition unit 17 performs a process of acquiring the relative position / posture corresponding to the frame.
  • the relative position / posture acquisition unit 17 acquires the result of calculating the relative position / posture by any of the methods of calculating the relative position / posture in FIG.
  • the absolute position / posture calculation unit 18 performs a process of calculating the absolute position / posture using a plurality of selected frames. At this time, for example, the position / posture is calculated using Perceptive n Points (PnP) or the like.
  • PnP Perceptive n Points
  • the absolute position / posture integration unit 19 integrates the calculation results of a plurality of positions / postures and performs a process of calculating the final absolute position / posture.
  • the integration method performed by the absolute position / posture integration unit 19 for example, there are a first integration method, a second integration method, and a third integration method.
  • the first integration method uses the "Winner takes all” method. That is, in the first integration method, the position / orientation estimated by the key frame with the smallest variance is adopted as the final result.
  • the second integration method uses a weighted linear sum. That is, in the second integration method, weighting is performed based on the variance.
  • the weight w tk for the position of the k-th frame and the weight w Rk for the posture of the k-th frame are the variance of the key frames used for the calculation of the position / posture, the variance of the position ⁇ tk 2 and the variance of the posture ⁇ Rk 2 . Is calculated using. Also, the weight is calculated using the standard deviation that can be regarded as a statistical index equivalent to the position variance ⁇ tk 2 and the attitude variance ⁇ Rk 2 , that is, the position standard deviation ⁇ tk and the posture standard deviation ⁇ Rk . You may. When the standard deviation is used, the weight w tk for the position of the k-th frame and the weight w Rk for the posture of the k-th frame are calculated by Eqs. (9) and (10), respectively.
  • the position t'k of the kth frame and the attitude R'k of the kth frame directly input the position / attitude calculated by the absolute position / attitude calculation unit 18. Do not mean. From the position / posture calculated by the absolute position / posture calculation unit 18, the position / posture when moving to an arbitrary frame is used. For example, when K frames (K is a positive integer) are selected by the frame selection unit 16 and integrated according to the Kth frame, the position t'of the kth frame (k is a positive integer). The posture R'k of the k and the kth frame is expressed by the equation (11). In the equation (11), the position t k of the k-th frame and the posture R k of the k-th frame are the positions / postures obtained by the absolute position / posture calculation unit 18.
  • the third integration method is a method of obtaining an absolute position / orientation by nonlinear optimization. For example, as shown in the equation (12), the position t k of the k-th frame and the attitude R k of the k-th frame are obtained so that the reprojection error is minimized.
  • N k indicates the number of matched local feature pairs in the kth frame.
  • w k is a weight corresponding to the kth frame, and either the weight w tk or w Rk , or a weight obtained by integrating these is used.
  • Absolute position / attitude can be obtained by solving equation (12) by a nonlinear optimization method such as the steepest descent method.
  • FIG. 9 is a flowchart showing an example of processing for creating a three-dimensional map by the three-dimensional map creating device according to the first embodiment. ..
  • the key frame detection unit 10 and the key frame position / attitude calculation unit 11 execute the creation of a three-dimensional map (step S101).
  • the creation of the three-dimensional map is executed while detecting the key frame using, for example, SLAM.
  • the position / posture dispersion calculation unit 12 calculates the position / posture variance for each key frame (step S102) (step S103).
  • the correspondence relationship registration unit 13 registers the correspondence relationship as shown in FIG. 8 (step S104).
  • the correspondence relationship registration unit 13 registers, for example, a three-dimensional map on the floor map, and executes a process of defining a positional relationship with the whole map or another three-dimensional map.
  • the database storage unit 14 performs a process of storing the map created in the above process in the three-dimensional map DB 102 (step S105).
  • FIG. 10 is a flowchart showing an example of processing for estimating position / posture by the position / posture estimation device 101 according to the first embodiment.
  • the database reading unit 15 reads a three-dimensional map as data from the DB storage unit 14 (step S111).
  • the frame selection unit 16 selects a frame (step S112).
  • the frame selected is a frame to be processed based on a predetermined rule.
  • the relative position / posture acquisition unit 17 performs a process of acquiring the relative position / posture corresponding to the frame for each selected frame (step S113).
  • the absolute position / posture calculation unit 18 calculates the absolute position / posture based on the data of the selected frame (step S115). When the number of selected frames reaches a predetermined number, the process proceeds to the integrated process.
  • the absolute position / attitude integration unit 19 integrates the results of the absolute position / attitude based on the dispersion (step S116).
  • FIG. 11 is a flowchart showing another example of the process for estimating the position / posture by the position / posture estimation device according to the first embodiment.
  • the database reading unit 15 reads a three-dimensional map as data from the database storage unit 14 (step S121).
  • the frame selection unit 16 determines whether or not to select a frame (steps S122 and S123), and when selecting a frame, selects the frame.
  • the frame selected is a frame to be processed based on a predetermined rule.
  • the relative position / posture acquisition unit 17 performs a process of acquiring the relative position / posture corresponding to the selected frame (step S124).
  • the absolute position / posture calculation unit 18 calculates the absolute position / posture for each selected data (step S125).
  • the frame selection unit 16 determines whether or not sufficient frame detection is completed (step S126), performs integrated processing when sufficient frame detection is completed, and performs integrated processing when sufficient frame detection is not completed. Returns the process to step S122.
  • "Sufficient frame detection is completed" means, for example, when a predetermined number of frames are detected, or when a predetermined number of absolute positions / postures are obtained.
  • the absolute position / attitude integration unit 19 integrates the absolute position / attitude results based on the dispersion (step S116).
  • ⁇ 1-3 Effect As described above, according to the position / posture estimation device or the position / posture estimation method according to the first embodiment, based on the position / posture data obtained by using a plurality of images. Since the position / posture is estimated, the accuracy of the position / posture estimation can be improved.
  • the integrated processing is performed without using the position / posture obtained from the image with a large dispersion of the calculation result of the position / posture, or the position / posture obtained from the image with a large dispersion of the calculation result of the position / posture.
  • the integrated processing is performed without using the position / posture obtained from the image with a large dispersion of the calculation result of the position / posture, or the position / posture obtained from the image with a large dispersion of the calculation result of the position / posture.
  • Embodiment 2 ⁇ 2-1 >> configuration ⁇ 2-1-1
  • first calculation method using direct matching and a second calculation method using two-step matching as a calculation method of absolute position / posture using a camera image.
  • second calculation method is used as a calculation method of absolute position / posture using a camera image.
  • a method of integrating the position / orientation obtained by a direct matching method using a camera image and a position / orientation integration method using a laser sensor such as LiDAR (Light Detection and Ringing) will be described.
  • FIGS. 4 and 5 are also referred to.
  • FIG. 12 is a functional block diagram schematically showing the configuration of the 3D map creation device according to the second embodiment.
  • the three-dimensional map creation device shown in FIG. 12 is a device capable of executing the three-dimensional map creation method according to the second embodiment.
  • the 3D map creation device is a part of the position / attitude estimation device (that is, the map creation registration unit)
  • the 3D map creation device is a position / attitude estimation device. It may be a device separate from the device.
  • the three-dimensional map creation device includes a three-dimensional map creation unit 21, a position / attitude distribution calculation unit 22, a correspondence registration unit 23, and a database storage unit (DB). It has a storage unit) 24.
  • the three-dimensional data was managed for each key frame, but in the second embodiment, the point cloud is managed as a three-dimensional map.
  • the three-dimensional map creation unit 21 creates a local feature obtained from the image and its position as a three-dimensional map.
  • the observed point cloud is created as a three-dimensional map.
  • the three-dimensional map creation unit 21 of FIG. 12 performs the process of creating the above-mentioned three-dimensional map data.
  • FIG. 13 is a diagram showing a dispersion calculation method used by the three-dimensional cartography apparatus according to the second embodiment.
  • FIG. 13 shows an example in which when the robot 131 to which the AGV is applied moves, the robot estimates the position / posture by using the calculation of the relative position / posture and the calculation of the absolute position / posture.
  • the robot 131 calculates the variance, but the method of calculating the variance is different from the calculation method in the case of the first embodiment.
  • the calculation of the variances ⁇ 1 , ⁇ 2 , and ⁇ 3 for each region surrounded by the ellipse (for example, regions # 1, # 2, and # 3) will be described.
  • the method of adding noise is a method of adding a random pattern in the case of an image as in the first embodiment.
  • noise is added by adding a random pattern to a local region.
  • the "random pattern” also includes a pattern for removing data in the region.
  • the correspondence registration unit 23 defines the relationship with the whole map or another three-dimensional map by the same method as the correspondence registration unit 13 of the first embodiment.
  • the DB storage unit 24 stores the three-dimensional map and the distribution for each area in the database.
  • FIG. 14 is a functional block diagram schematically showing the configuration of the position / posture estimating device according to the second embodiment.
  • the position / posture estimation device shown in FIG. 14 is a device capable of executing the position / posture estimation method according to the second embodiment.
  • the position / attitude estimation device includes a database reading unit 25, a frame selection unit 26, a relative position / attitude acquisition unit 27, an absolute position / attitude calculation unit 28, and an absolute position / attitude integration unit 29. ..
  • the database reading unit 25 performs a process of reading the three-dimensional map data stored in the database.
  • the frame selection unit 26 selects a frame in the same manner as the frame selection unit 16 of the first embodiment or from a plurality of predivided areas so that there is no overlap. For example, as shown in FIG. 13, an example will be described in which the variances ⁇ 1 , ⁇ 2 , and ⁇ 3 are calculated and managed by dividing them into three regions # 1, # 2, and # 3. An example in which the number of frames in which the calculation result of the position / posture is included in each of the areas # 1, # 2, and # 3 is M will be described.
  • the relative position / posture acquisition unit 27 acquires the relative position / posture by the same method as the relative position / posture acquisition unit 17 in the first embodiment.
  • the absolute position / posture calculation unit 28 uses a method of directly matching the local features of the image to calculate the position / posture (see, for example, Non-Patent Document 4) and the like. To calculate.
  • the position / orientation is calculated by matching the three-dimensional map with the shape information observed by the distance sensor.
  • Torsten Sattler two other authors, "Efficient & Effective Prioritized Matching for Large-Scale Image-Based Location", IEEE Transition Engineer 39, No. 9, September 2017.
  • the absolute position / posture integration unit 29 integrates a plurality of positions / postures in the same manner as the absolute position / posture integration unit 19 in the first embodiment.
  • the absolute position / posture integration unit 29 obtains the final position / posture based on the variance set for each area.
  • FIG. 15 is a flowchart showing an example of processing for creating a 3D map by the 3D map creation device according to the second embodiment.
  • the 3D map creation unit 21 and the position / attitude distribution calculation unit 22 create a 3D map, and when using an image, a map showing local features and their positions, and when using LiDAR, a point group (position). Only) is created (step S201).
  • the correspondence relationship registration unit 23 registers the correspondence relationship, which is a process corresponding to the three-dimensional map creation unit 21 (step S202).
  • the database storage unit 24 performs a process of storing the correspondence in the database (step S203).
  • FIG. 16 is a flowchart showing an example of processing for estimating position / posture by the position / posture estimation device according to the second embodiment.
  • the database reading unit 25 performs a process of reading data from the database (step S211).
  • the relative position / posture acquisition unit 27 acquires the relative position / posture, and the absolute position / posture calculation unit 28 calculates the absolute position / posture (steps S212 to S214).
  • the frame selection unit determines whether or not frame selection is necessary (step S215), and if necessary, determines whether or not sufficient frame detection is completed (step S216). When sufficient frame detection is completed, the absolute position / attitude integration unit 29 integrates the absolute position / attitude (step S217).
  • ⁇ 2-3 Effect As described above, according to the position / posture estimation device or the position / posture estimation method according to the second embodiment, when an image is used and a direct matching method is used, or LiDAR In the example of calculating the absolute position / posture using the shape observed in the above, since a plurality of frames are used, the accuracy of the position / posture estimation can be improved.
  • Embodiment 3 ⁇ 3-1 >> configuration ⁇ 3-1-1
  • the terminal or robot used in AR may manage the relative position / posture in the device.
  • the content is superimposed and displayed on the image by converting the position of the content defined by the absolute position / posture into the coordinate system of the relative position / posture managed by the terminal.
  • the third embodiment describes a method of calculating an external parameter, which is a matrix for converting an absolute position / posture coordinate system to a relative position / posture coordinate system, with high accuracy using a plurality of frames.
  • an external parameter which is a matrix for converting an absolute position / posture coordinate system to a relative position / posture coordinate system, with high accuracy using a plurality of frames.
  • a method of obtaining external parameters with high accuracy using a plurality of frames will be described based on the first embodiment.
  • the configuration of the third embodiment may be combined with the second or fourth embodiment.
  • FIGS. 4 and 5 are also referred to.
  • 3D map creation device The 3D map creation device according to the second embodiment is the same as that of the first embodiment.
  • FIG. 17 is a functional block diagram schematically showing the configuration of the position / posture estimating device according to the third embodiment.
  • the position / posture estimation device shown in FIG. 17 is a device capable of executing the position / posture estimation method according to the third embodiment.
  • the position / attitude estimation device includes a database reading unit 35, a frame selection unit 36, a relative position / attitude acquisition unit 37, an absolute position / attitude calculation unit 38, an external parameter calculation unit 38a, and an absolute position / attitude integration unit. It has an external parameter integration unit 39 as a.
  • the third embodiment differs from the first embodiment in that it has an external parameter calculation unit 38a for calculating an external parameter for each selected frame and an external parameter integration unit 39 for integrating a plurality of external parameters. Regarding the processes other than these, the process of the third embodiment is the same as the process of the first embodiment.
  • the external parameter calculation unit 38a calculates the external parameters on the assumption that the relative position / posture calculation and the absolute position / posture calculation are performed based on the same frame. At this time, the external parameter is calculated by the equation (13).
  • the processing of the external parameter integration unit 39 is substantially the same as that of the absolute position / attitude integration unit 19 (FIG. 8) of the first embodiment. Therefore, the external parameter integration unit 39 is also referred to as an absolute position / attitude integration unit.
  • the external parameter integration unit 39 is also referred to as an absolute position / attitude integration unit.
  • a plurality of absolute positions / postures are integrated, but in the third embodiment, external parameters are integrated.
  • t'k and R'k obtained by the equation (13) are used for integration. For example, when integrating by a weighted linear sum, t'k and R'k of the equation (13) are substituted into the equations (5) and (6) for integration.
  • FIG. 18 is a flowchart showing an example of processing for estimating position / posture by the position / posture estimation device according to the third embodiment.
  • the database reading unit 35 performs a process of reading data from the database (step S301).
  • the relative position / posture acquisition unit 37 acquires the device position / posture (steps S302 and S303).
  • the absolute position / posture calculation unit 38 calculates the absolute position / posture (step S304).
  • the external parameter calculation unit 38a calculates the external parameter (step S305).
  • the frame selection unit 36 determines whether or not frame selection is necessary (step S306), and if necessary, determines whether or not sufficient frame detection is completed (step S307).
  • the external parameter integration unit 39 integrates the absolute position and orientation (step S308).
  • ⁇ 3-3 Effect As described above, according to the position / posture estimation device or the position / posture estimation method according to the third embodiment, the coordinate system of the absolute position / posture is changed to the coordinate system of the relative position / posture. Since the conversion matrix can be obtained with high accuracy, the content can be displayed with high accuracy on the terminal applied to AR. Further, when applied to a robot, the destination of the robot can be obtained with high accuracy.
  • Embodiment 4 ⁇ 4-1 >> configuration ⁇ 4-1-1
  • a mode in which error processing using a plurality of frames is added to the position / posture estimation method in the first embodiment will be described.
  • the position / attitude estimation device executes error processing.
  • FIGS. 4 and 5 are also referred to.
  • ⁇ 4-1-2 Three-dimensional map creation device
  • the configuration of the three-dimensional map creation device according to the fourth embodiment is the same as that of the first embodiment.
  • FIG. 19 is a functional block diagram schematically showing the configuration of the position / posture estimating device according to the fourth embodiment.
  • the position / posture estimation device shown in FIG. 19 is a device capable of executing the position / posture estimation method according to the fourth embodiment.
  • the position / attitude estimation device according to the fourth embodiment is different from that of the first embodiment in that an error processing unit 48a is added.
  • the position / attitude estimation device includes a database reading unit 45, a frame selection unit 46, a relative position / attitude acquisition unit 47, and an absolute position / attitude calculation unit 48. It has an error processing unit 48a and an absolute position / attitude integration unit 49.
  • the database reading unit 45, the frame selection unit 46, the relative position / attitude acquisition unit 47, the absolute position / attitude calculation unit 48, and the absolute position / attitude integration unit 49 are the database reading unit 15 and the frame selection unit 16 shown in FIG. , Relative position / posture acquisition unit 17, absolute position / posture calculation unit 18, and absolute position / posture integration unit 19.
  • the error processing unit 48a performs error processing. In the error processing, by comparing a plurality of calculation results of the absolute position / posture obtained in a plurality of frames, the calculation result of the absolute position / posture whose error is larger than the predetermined threshold value is excluded from the integrated processing.
  • An example of error handling is shown in equations (14) and (15).
  • the jth frame (j is a positive integer less than or equal to K) is a frame satisfying j ⁇ k among the K frames selected by the frame selection unit 46. Multiple calculation results of absolute position / attitude obtained in multiple frames should indicate the same position / attitude. Therefore, the error processing unit 48a compares the calculation result of the absolute position / posture obtained in one frame with the calculation result of the absolute position / posture obtained in another frame.
  • the error processing unit 48a is in the case where the difference in the calculated position is larger than the predetermined threshold value th_t (that is, when the equation (14) is satisfied), or when the difference in the calculated posture is larger than the predetermined threshold value th_r (when the difference in the calculated posture is larger than the predetermined threshold value th_r). That is, when the equation (15) is satisfied), the calculation result of the frame is excluded.
  • the operation of the three-dimensional map creating device according to the fourth embodiment is the same as that of the first embodiment.
  • FIG. 20 is a flowchart showing another example of the process for estimating the position / posture by the position / posture estimation device according to the fourth embodiment.
  • the operation of the position / posture estimation device shown in FIG. 20 is different from the operation of the position / posture estimation device according to the first embodiment shown in FIG. 11 in that an error processing (step S406a) is added.
  • the processes of steps S401 to S406 shown in FIG. 20 and the processes of steps S407 are the same as the processes of steps S101 to S107 shown in FIG.
  • ⁇ 4-3 Effect As described above, according to the position / posture estimation device or the position / posture estimation method according to the fourth embodiment, error processing is performed, so that the environment resistance is higher than that of the first embodiment. It is possible to realize highly accurate estimation of absolute position / posture (that is, it is possible to realize high estimation accuracy of absolute position / posture for various environments).

Abstract

位置・姿勢推定装置（１０１）は、データベースから３次元地図のデータを読み込むデータベース読込部（１５）と、複数の異なる視点から撮影した画像のフレームの中から位置・姿勢の計算に使用するフレームを選択する処理を行うフレーム選択部（１６）と、前記フレーム選択部によって選択された複数のフレームについての複数の相対位置・姿勢を取得する処理を行う相対位置・姿勢取得部（１７）と、前記フレーム選択部によって選択された複数のフレームについての複数の絶対位置・姿勢を取得する処理を行う絶対位置・姿勢計算部（１８）と、前記相対位置・姿勢取得部（１７）によって取得された相対位置・姿勢と、前記絶対位置・姿勢計算部（１８）によって取得された絶対位置・姿勢と、を統合して、最終的な絶対位置・姿勢を取得する絶対位置・姿勢統合部（１９）とを有する。

Description

位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム
　本開示は、位置・姿勢推定装置、位置・姿勢推定方法、及びプログラムに関する。
　Ａｕｇｍｅｎｔｅｄ　Ｒｅａｌｉｔｙ（ＡＲ）が適用された装置及びＡｕｔｏｍａｔｅｄ　Ｇｕｉｄｅｄ　Ｖｅｈｉｃｌｅ（ＡＧＶ）が適用されたロボットなどにおいて、相対位置・姿勢の計算の結果と絶対位置・姿勢の計算の結果とを組み合わせることで位置・姿勢の計算（すなわち、推定）の精度を向上させる方法が提案されている。例えば、特許文献１を参照。
　相対位置・姿勢の計算は、ある位置・姿勢からの相対的な移動量の計算であり、一定周期（一般には、短周期）ごとに繰り返し実行される処理である。例えば、相対位置・姿勢の計算には、カメラによって撮影された画像であるカメラ画像又は距離センサによって検出された距離に基づいて移動量を求めるＳｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　Ａｎｄ　Ｍａｐｐｉｎｇ（ＳＬＡＭ）、ジャイロセンサ若しくは加速度センサなどが統合されたＩｎｅｒｔｉａｌ　Ｍｅａｓｕｒｅｍｅｎｔ　Ｕｎｉｔ（ＩＭＵ）を用いる方法、又は車輪の回転数に基づいて移動量を求める自律航法、などが使用される。これらの方法では、相対位置・姿勢の計算の度に誤差が加算されるため、長距離移動すると累積した誤差が大きくなる問題がある。そのため、相対位置・姿勢の計算の結果と絶対位置・姿勢の計算の結果とを組み合わせて、相対位置・姿勢の計算によって累積した誤差を、定期的に取り除く処理が行われる。
　絶対位置・姿勢の計算は、予め用意した３次元地図を用いて行われ、例えば、３次元地図が示す物体の前で実行される。絶対位置・姿勢の計算は、例えば、３次元地図及びカメラ画像を使って行われる。
　図１は、ＡＲが適用された端末１１１を携行するユーザ１１２が移動した場合に、端末１１１が相対位置・姿勢の計算と絶対位置・姿勢の計算とを用いて位置・姿勢を推定する例を示す図である。端末１１１は、例えば、タブレット端末又はＨｅａｄ　Ｍｏｕｎｔｅｄ　Ｄｉｓｐｌａｙ（ＨＭＤ）を用いた端末、などである。この場合、端末１１１は、相対位置・姿勢計算（１）を実行しながら移動し、３次元地図が示す物体１１３の前で実行した絶対位置・姿勢計算（２）の結果を用いて累積した誤差を取り除き、その後、相対位置・姿勢計算（３）を実行しながら移動する。これにより、３次元地図が示す物体１１３から離れても、位置・姿勢を精度良く推定でき、その結果、端末の画面に表示された現実の画像上の適切な位置にＡＲコンテンツを重畳表示し続けることができる。
　図２は、ＡＧＶが適用されたロボット１２１が移動した場合に、ロボット１２１が相対位置・姿勢の計算と絶対位置・姿勢の計算とを用いて位置・姿勢を推定する例を示す図である。この場合、ロボット１２１は、相対位置・姿勢計算（４）を実行しながら移動し、３次元地図が示す物体１２３の前で実行された絶対位置・姿勢計算（５）の結果を用いて累積した誤差を取り除き、その後、相対位置・姿勢計算（６）を実行しながら移動する。これにより、３次元地図が示す物体１２３から離れても、ロボット１２１は、自己の位置・姿勢を精度良く推定でき、その結果、目標とする位置に正確に到達することができる。
特開２０１９－１６０１４７号公報
　しかしながら、従来の絶対位置・姿勢の推定は、１フレーム（すなわち、１枚の画像フレーム）のみを使用して行われているため、計算の精度（すなわち、精度の推定）は１フレームに含まれる被写体に依存して大きく変動するという課題がある。
　例えば、カメラ画像を用いる場合、被写体の模様に依存して絶対位置・姿勢の計算の精度が変動することがある。つまり、被写体の模様が特徴的である場合には、絶対位置・姿勢の計算を高い精度で行うことができるが、被写体の模様がストライプ若しくはボーダーなどの繰り返し模様である場合又は被写体が真っ白な壁などのように模様が無い物体である場合には、絶対位置・姿勢の計算の精度は低下する。
　また、レーザ又は赤外線などを利用して得られた被写体までの距離情報を用いて絶対位置・姿勢を計算する場合、被写体の形状に依存して絶対位置・姿勢の計算の精度が大きく変動する。つまり、被写体の形状が特徴的である場合には、絶対位置・姿勢の計算を高い精度で行うことができるが、被写体の形状が特徴的ではない場合には、絶対位置・姿勢の計算の精度は低下する。
　本開示は、上記した課題を解決するためになされたものであり、位置・姿勢の推定の精度を向上させることができる位置・姿勢推定装置、位置・姿勢推定方法、及びプログラムを提供することを目的とする。
　本開示の位置・姿勢推定装置は、データベースから３次元地図のデータを読み込むデータベース読込部と、複数の異なる視点から撮影した画像のフレームの中から位置・姿勢の計算に使用するフレームを選択する処理を行うフレーム選択部と、前記フレーム選択部によって選択された複数のフレームについての複数の相対位置・姿勢を取得する処理を行う相対位置・姿勢取得部と、前記フレーム選択部によって選択された複数のフレームについての複数の絶対位置・姿勢を取得する処理を行う絶対位置・姿勢計算部と、前記相対位置・姿勢取得部によって取得された相対位置・姿勢と、前記絶対位置・姿勢計算部によって取得された絶対位置・姿勢と、を統合して、最終的な絶対位置・姿勢を取得する絶対位置・姿勢統合部と、を有することを特徴とする。
　本開示の位置・姿勢推定方法は、位置・姿勢推定装置が実行する方法であって、位置データベースから３次元地図のデータを読み込むステップと、複数の異なる視点から撮影した画像のフレームの中から位置・姿勢の計算に使用するフレームを選択する処理を行うステップと、選択された前記複数のフレームについての複数の相対位置・姿勢を取得する処理を行うステップと、前記選択された複数のフレームについての複数の絶対位置・姿勢を取得する処理を行うステップと、取得された前記相対位置・姿勢と、取得された前記絶対位置・姿勢と、を統合して、最終的な絶対位置・姿勢を取得するステップと、を有することを特徴とする。
　本開示の装置、方法、又はプログラムによれば、位置・姿勢の推定の精度を向上させることができる。
ＡＲが適用された端末を携行するユーザが移動した場合に、端末が相対位置・姿勢の計算と絶対位置・姿勢の計算とを用いて位置・姿勢を推定する例を示す図である。 ＡＧＶが適用されたロボットが移動した場合に、ロボットが相対位置・姿勢の計算と絶対位置・姿勢の計算とを用いて位置・姿勢を推定する例を示す図である。 実施の形態１に係る位置・姿勢推定装置及びそれを含む位置・姿勢推定システムのハードウェア構成の例を示す図である。 図３に示される位置・姿勢推定装置のハードウェア構成の例を示す図である。 実施の形態１に係る３次元地図作成装置の構成を概略的に示す機能ブロック図である。 画像にランダムパターンを付加する処理の一例を示す図である。 フロアマップに３次元地図を位置合わせして登録する処理を示す図である。 実施の形態１に係る位置・姿勢推定装置の構成を概略的に示す機能ブロック図である。 実施の形態１に係る３次元地図作成装置による３次元地図の作成のための処理の例を示すフローチャートである。 実施の形態１に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の例を示すフローチャートである。 実施の形態１に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の他の例を示すフローチャートである。 実施の形態２に係る３次元地図作成装置の構成を概略的に示す機能ブロック図である。 実施の形態２に係る３次元地図作成装置が使用する分散の計算方法を示す図である。 実施の形態２に係る位置・姿勢推定装置の構成を概略的に示す機能ブロック図である。 実施の形態２に係る３次元地図作成装置による３次元地図の作成のための処理の例を示すフローチャートである。 実施の形態２に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の例を示すフローチャートである。 実施の形態３に係る位置・姿勢推定装置の構成を概略的に示す機能ブロック図である。 実施の形態３に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の例を示すフローチャートである。 実施の形態４に係る位置・姿勢推定装置の構成を概略的に示す機能ブロック図である。 実施の形態４に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の他の例を示すフローチャートである。
　以下に、実施の形態に係る位置・姿勢推定装置、位置・姿勢推定方法、及びプログラムを、図面を参照しながら説明する。以下の実施の形態は、例にすぎず、実施の形態を適宜組み合わせること及び実施の形態を適宜変更することが可能である。なお、本出願において「位置・姿勢」は、位置及び姿勢を意味する。「位置」は、例えば、カメラを搭載した端末又はロボットの位置を意味する。「姿勢」は、例えば、カメラによる撮影方向又は距離センサによる測定方向を意味する。
《１》実施の形態１
《１－１》構成
《１－１－１》概要
　実施の形態１では、カメラによって撮影された画像であるカメラ画像（すなわち、撮像画像）を用いた絶対位置・姿勢の計算の精度の向上について述べる。カメラ画像を用いて位置・姿勢を推定する方法として、第１の推定方法が知られている。例えば、非特許文献１を参照。
Ｐａｕｌ－Ｅｄｏｕａｒｄ　Ｓａｒｌｉｎ、外３名著、　"Ｆｒｏｍ　Ｃｏａｒｓｅ　ｔｏ　Ｆｉｎｅ：　Ｒｏｂｕｓｔ　Ｈｉｅｒａｒｃｈｉｃａｌ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｔ　Ｌａｒｇｅ　Ｓｃａｌｅ"、　２０１９　ＩＥＥＥ／ＣＶＦ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ　ａｎｄ　Ｐａｔｔｅｒｎ　Ｒｅｃｏｇｎｉｔｉｏｎ．
　第１の推定方法では、直接的なマッチングを用いる。第１の推定方法では、画像の局所特徴の集合に基づいて、直接的にカメラの位置・姿勢を計算する。
　また、カメラ画像を用いて位置・姿勢を推定する方法として、第２の推定方法が知られている。第２の推定方法では、Ｃｏｎｖｏｌｕｔｉｏｎａｌ　Ｎｅｕｒａｌ　Ｎｅｔｗｏｒｋ（ＣＮＮ）で画像からカメラの位置・姿勢を推定する。例えば、非特許文献２を参照。
Ｓａｍａｒｔｈ　Ｂｒａｈｍｂｈａｔｔ、外４名著、　"Ｇｅｏｍｅｔｒｙ－Ａｗａｒｅ　Ｌｅａｒｎｉｎｇ　ｏｆ　Ｍａｐｓ　ｆｏｒ　Ｃａｍｅｒａ　Ｌｏｃａｌｉｚａｔｉｏｎ"、　２０１８　ＩＥＥＥ／ＣＶＦ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ　ａｎｄ　Ｐａｔｔｅｒｎ　Ｒｅｃｏｇｎｉｔｉｏｎ．
　第２の推定方法では、２段階のマッチングを用いる。第２の推定方法では、視点が異なる複数の画像を用意し、最初に、複数の画像の中から、撮影で得られたカメラ画像に最も類似した画像を類似画像として特定し、次に、類似画像から抽出した局所特徴とカメラ画像から抽出した局所特徴との対応関係に基づいて、カメラの位置・姿勢を求める。実施の形態１では、第２の推定方法に基づく方法を用いる。
　図３は、実施の形態１に係る位置・姿勢推定装置１０１及びそれを含む位置・姿勢推定システム１００のハードウェア構成の例を示す図である。実施の形態１に係る位置・姿勢推定装置１０１は、位置・姿勢の推定のための計算を実行する計算機であるコンピュータを有している。図３の例では、実施の形態１に係る位置・姿勢推定システム１００は、位置・姿勢推定装置１０１と、記憶装置に格納された３次元地図データベース（３次元地図ＤＢ）１０２と、距離センサ１０３と、撮像装置としてのカメラ１０４と、液晶表示装置などのディスプレイ１０５とを有している。また、図３の例では、位置・姿勢推定システム１００は、ジャイロセンサ１０６と、加速度センサ１０７と、地磁気センサ１０８とを有している。ジャイロセンサ１０６、加速度センサ１０７、及び地磁気センサ１０８を有する装置は、「ＩＭＵ」とも称される。位置・姿勢推定装置１０１と、図３に示される他の構成とは、例えば、ネットワークで接続されている。３次元地図ＤＢ１０２は、位置・姿勢推定装置１０１の一部であってもよい。
　３次元地図ＤＢ１０２は、絶対位置・姿勢の計算をするときに使用される、予め用意された３次元地図情報を含む。３次元地図ＤＢ１０２は、実施の形態１に係る位置・姿勢推定システム１００の一部である必要はなく、外部の記憶装置に格納された情報であってもよい。また、３次元地図ＤＢ１０２は、位置・姿勢推定装置１０１によって作成されてもよい。この場合、位置・姿勢推定装置１０１は、３次元地図作成装置としての機能を持つ。つまり、実施の形態１に係る３次元地図作成装置は、位置・姿勢推定装置１０１の一部である。ただし、実施の形態１に係る３次元地図作成装置は、位置・姿勢推定装置１０１と別個の装置であってもよい。
　距離センサ１０３は、赤外線又はレーザなどを用いて距離を計測する機器である。カメラ１０４は、カメラ画像を取得する機器である。位置・姿勢推定システム１００は、カメラ１０４及び距離センサ１０３のうちの一方のみを有してもよい。
　ディスプレイ１０５は、ＡＲコンテンツをカメラ画像に重畳表示する場合に必要な表示機器である。位置・姿勢推定システム１００は、ディスプレイ１０５を備えないことも可能である。
　ジャイロセンサ１０６、加速度センサ１０７、及び地磁気センサ１０８は、自律航法により相対位置・姿勢の計算をするための機器であるＩＭＵを構成する。ただし、自律航法により相対位置・姿勢の計算を計算しない場合には、ＩＭＵを備えないことも可能である。また、ジャイロセンサ１０６、加速度センサ１０７、及び地磁気センサ１０８のうちの１つのみ又は２つのみを備えてもよい。なお、位置・姿勢推定装置１０１に接続される機器は、図３に示されるものの一部であってもよく、又は、図３に示されていない他の機器を備えてもよい。
　図４は、位置・姿勢推定装置１０１のハードウェア構成の例を示す図である。位置・姿勢推定装置１０１は、情報処理部としてのＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）１０１１と、記憶装置であるメモリ１０１２と、インタフェース１０１３とを有する。３次元地図ＤＢ１０２、距離センサ１０３、カメラ１０４、ディスプレイ１０５、ジャイロセンサ１０６、加速度センサ１０７、及び地磁気センサ１０８は、インタフェース１０１３及びデータバスを介してＣＰＵ１０１１に接続される。
　位置・姿勢推定装置１０１の各機能は、処理回路により実現される。処理回路は、専用のハードウェアであっても、メモリ１０１２に格納されるソフトウェアであるプログラム（例えば、位置・姿勢推定プログラム）を実行するＣＰＵ１０１１であってもよい。ＣＰＵ１０１１は、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、及びＤＳＰ（Ｄｉｇｉｔａｌ　Ｓｉｇｎａｌ　Ｐｒｏｃｅｓｓｏｒ）のいずれであってもよい。
　処理回路が専用のハードウェアである場合、処理回路は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）、又はこれらのうちのいずれかを組み合わせたものである。
　処理回路がＣＰＵ１０１１の場合、位置・姿勢推定装置１０１の機能は、ソフトウェア、ファームウェア、又はソフトウェアとファームウェアとの組み合わせにより実現される。ソフトウェア及びファームウェアは、プログラムとして記述され、メモリ１０１２に格納される。処理回路は、メモリ１０１２に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。すなわち、位置・姿勢推定装置１０１は、処理回路により処理が実行されるときに、実施の形態１に係る位置・姿勢推定方法を実行するものである。
　ここで、メモリ１０１２は、例えば、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ　Ｐｒｏｇｒａｍｍａｂｌｅ　Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ　Ｅｒａｓａｂｌｅ　Ｐｒｏｇｒａｍｍａｂｌｅ　Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）などの、不揮発性又は揮発性の半導体メモリ、或いは、磁気ディスク、光ディスク、コンパクトディスク、ＤＶＤ（Ｄｉｇｉｔａｌ　Ｖｅｒｓａｔｉｌｅ　Ｄｉｓｃ）などのうちのいずれかである。
　なお、位置・姿勢推定装置１０１の一部を専用のハードウェアで実現し、一部をソフトウェア又はファームウェアで実現するようにしてもよい。このように、処理回路は、ハードウェア、ソフトウェア、ファームウェア、又はこれらのうちのいずれかの組み合わせによって、各機能を実現することができる。
《１－１－２》３次元地図作成装置
　図５は、実施の形態１に係る３次元地図作成装置の構成を概略的に示す機能ブロック図である。図５に示される３次元地図作成装置は、実施の形態１に係る３次元地図作成方法を実行することができる装置である。なお、以下の説明では、３次元地図作成装置が、位置・姿勢推定装置１０１の一部（すなわち、地図作成登録部）である例を説明する。ただし、３次元地図作成装置は、位置・姿勢推定装置１０１とは別個の装置であってもよい。この場合、３次元地図作成装置のハードウェア構成は、図４に示されるものと同様である。
　図５に示されるように、実施の形態１に係る３次元地図作成装置は、キーフレーム検出部１０と、キーフレーム位置・姿勢計算部１１と、位置・姿勢分散計算部１２と、対応関係登録部１３と、データベース保存部（ＤＢ保存部）１４とを有している。これらの構成は、カメラ１０４（図３）によって撮影されたカメラ画像と、距離センサ１０３（図３）によって取得された距離情報と、ＩＭＵ（図３）によって取得されたセンサ値とを用いて３次元地図を構築する。実施の形態１で最低限必要なデータは、カメラ画像である。距離情報又はＩＭＵを備えない場合であっても、３次元地図を作成することが可能である。
　キーフレーム検出部１０は、カメラ１０４の位置が予め定められた平行移動量の閾値以上動いた場合又はカメラ１０４の姿勢が予め定められた回転量の閾値以上動いた（すなわち、回転した）場合に取得した画像（例えば、カラー画像）及び検出された距離情報を、キーフレームとして検出する処理を実行する。
　キーフレーム位置・姿勢計算部１１は、ＳＬＡＭなどのような画像を用いた相対位置・姿勢の計算方法により、キーフレーム検出部１０によって検出されたキーフレームを撮影したカメラ１０４又は距離センサ１０３の位置・姿勢を計算する処理を実行する。キーフレーム検出部１０及びキーフレーム位置・姿勢計算部１１は、従来のＳＬＡＭ技術の場合の処理（例えば、非特許文献３に記載の処理）と同様の処理を実行する。
Ｒａｕｌ　Ｍｕｒ－Ａｒｔａｌ、外１名著、　"ＯＲＢ－ＳＬＡＭ２：Ａｎ　Ｏｐｅｎ－Ｓｏｕｒｃｅ　ＳＬＡＭ　Ｓｙｓｔｅｍ　ｆｏｒ　Ｍｏｎｏｃｕｌａｒ，　Ｓｔｅｒｅｏ，　ａｎｄ　ＲＧＢ－Ｄ　Ｃａｍｅｒａｓ"、　ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ｒｏｂｏｔｉｃｓ、　Ｖｏｌ．３３、Ｎｏ．５、　２０１７年１０月．
　位置・姿勢分散計算部１２は、キーフレーム検出部１０によって検出されたキーフレームの各々について位置・姿勢の分散を求める処理を実行する。位置・姿勢の分散の計算方法としては、例えば、以下に示される第１の計算方法と第２の計算方法とがある。
　第１の計算方法は、画像にノイズ（すなわち、ランダムパターン）を付加する方法である。図６は、キーフレームの画像にランダムパターンを付加する方法の一例を示す図である。第１の計算方法では、キーフレームの画像にランダムパターンを加えて位置・姿勢を計算する処理を複数回実行し、得られた位置・姿勢の複数の計算結果に基づいて位置・姿勢の分散を求める。
　第２の計算方法は、乱数を用いる方法である。第２の計算方法では、例えば、絶対位置・姿勢の計算の処理では、外れ値を除外する目的でＲａｎｄｏｍ　Ｓａｍｐｌｅ　Ｃｏｎｓｅｎｓｕｓ（ＲＡＮＳＡＣ）が使用される。ＲＡＮＳＡＣの主目的は、観測されたデータのうちの、絶対位置・姿勢の計算に用いられるデータから外れた外れ値のデータを除外することである。しかし、ＲＡＮＳＡＣでは、ランダムに計算対象のサンプルが選択され、計算結果は、毎回、異なる値になるので、ＲＡＮＳＡＣを位置・姿勢の分散の計算に用いることができる。
Figure JPOXMLDOC01-appb-M000001
 は、それぞれ位置の分散（σｔｋ ２）及び姿勢の分散（σＲｋ ２）を示し、それぞれ式（１）及び式（２）で計算される。式（１）及び式（２）において、Ｎは、正の整数であり、分散を求めるときに使用される試行回数を示す。また、ｋは、正の整数を示す。
Figure JPOXMLDOC01-appb-M000002
 は、それぞれｎ回目の試行で求めた絶対的な位置（ｔｎ）及び絶対的な姿勢（Ｒｎ）を示す。なお、ｎは、１以上、Ｎ以下の整数である。
Figure JPOXMLDOC01-appb-M000003
 は、それぞれ位置の平均（μｔ）及び姿勢の平均（μＲ）を示し、それぞれ式（３）及び式（４）で計算される。
Figure JPOXMLDOC01-appb-M000004
　図７は、フロアマップに３次元地図を位置合わせして登録する処理を示す図である。図７のフロアマップは、設備の配置を示すレイアウトが描かれたフロアレイアウトである。フロアマップにおいて、設備が配置されるべき領域である設置領域が破線の矩形で示されている。対応関係登録部１３は、他の３次元地図又は全体地図との関係性を定義する処理を行う。図７に示されるように、フロアマップのレイアウト上に３次元地図（実線の矩形で示される。）を登録すると、３次元地図間の位置関係と、作成中の３次元智頭と既に構築されている３次元地図との対応関係と、が得られる。フロアマップに対して、３次元地図を位置合わせして登録することで、全体地図と３次元地図との整合性又は複数の３次元地図間の位置関係が定義される。
　データベース保存部１４は、上記の方法で作成された３次元地図（すなわち、例えば、ＳＬＡＭで得られるキーフレーム毎のデータ）を３次元地図ＤＢ１０２（図３）に保存する。３次元地図ＤＢ１０２には、キーフレーム毎に得られた位置・姿勢、カメラ画像、距離情報（すなわち、距離画像）、及び局所特徴の点群が、キーフレームの個数分保存される。
《１－１－３》位置・姿勢推定装置
　図８は、実施の形態１に係る位置・姿勢推定装置１０１の構成を概略的に示す機能ブロック図である。位置・姿勢推定装置１０１は、異なる視点で撮影した複数のセンサデータに基づいて、位置・姿勢を計算する処理を行う。ここで、センサデータは、カメラ画像、距離センサの検出データを含む。図８に示されるように、位置・姿勢推定装置１０１は、データベース読込部１５と、フレーム選択部１６と、相対位置・姿勢取得部１７と、絶対位置・姿勢計算部１８と、絶対位置・姿勢統合部１９とを有している。
　データベース読込部１５は、ＤＢ保存部１４に記憶されているデータベースに格納されている３次元地図（つまり、予め用意された３次元地図）を読み込む処理を行う。
　フレーム選択部１６は、複数の異なる視点から撮影したカメラ画像のフレームの中から位置・姿勢の計算に使用するフレームを選択する処理を行う。フレーム選択方法としては、例えば、第１の選択方法、第２の選択方法、及び第３の選択方法がある。
　第１の選択方法では、相対移動量が用いられる。第１の選択方法では、相対位置・姿勢の計算により得られた相対位置の変化が予め定められた位置の変化の閾値以上になるという条件及び相対姿勢の変化が予め定められた姿勢の変化の閾値以上になるという条件に基づいて、フレーム（キーフレーム）を選択するか否かを判定する。例えば、相対位置・姿勢の計算により得られた相対位置の変化が予め定められた位置の変化の閾値以上になるという条件及び相対姿勢の変化が予め定められた姿勢の変化の閾値以上になるという条件の少なくとも一方を満たすキーフレームを選択する。
　第２の選択方法では、時間的に異なるフレームが用いられる。第２の選択方法では、時間的に隣接する視点のフレーム、又は、時間的に閾値以上の時間間隔で撮影したフレーム（キーフレーム）が選択される。
　第３の選択方法では、分散が用いられる。第３の選択方法では、３次元地図作成時に求められ分散を元に、使用するデータを選択する。例えば、分散が予め決められた分散の閾値よりも小さい視点のフレーム（キーフレーム）が選択される。
　相対位置・姿勢取得部１７は、フレームに対応する相対位置・姿勢を取得する処理を行う。相対位置・姿勢取得部１７は、図１の相対位置・姿勢の計算のいずれかの方法で相対位置・姿勢を計算した結果を取得する。
　絶対位置・姿勢計算部１８は、選択した複数のフレームを用いて絶対位置・姿勢を計算する処理を行う。このとき、例えば、Ｐｅｒｓｐｅｃｔｉｖｅ　ｎ　Ｐｏｉｎｔｓ（ＰｎＰ）などを用いて位置・姿勢を計算する。
　絶対位置・姿勢統合部１９は、複数の位置・姿勢の計算結果を統合して、最終的な絶対位置・姿勢を計算する処理を行う。絶対位置・姿勢統合部１９が行う統合方法としては、例えば、第１の統合方法、第２の統合方法、及び第３の統合方法がある。
　第１の統合方法は、「Ｗｉｎｎｅｒ　ｔａｋｅｓ　ａｌｌ」方式を用いる。つまり、第１の統合方法では、分散が最も小さいキーフレームで推定した位置・姿勢を最終的な結果として採用する。
　第２の統合方法は、重み付け線形和を用いる。つまり、第２の統合方法では、分散を元に重み付けする。
Figure JPOXMLDOC01-appb-M000005
 は、それぞれ最終的に得られる位置及び姿勢を示し、それぞれ式（５）及び式（６）で計算される。
Figure JPOXMLDOC01-appb-M000006
 は、それぞれフレーム選択部１６で得られたフレームのうち、ｋ番目のフレームの位置（ｔ´ｋ）とｋ番目のフレームの姿勢（Ｒ´ｋ）を示す。
Figure JPOXMLDOC01-appb-M000007
 は、それぞれｋ番目のフレームの位置に対する重み（ｗｔｋ）及びｋ番目のフレームの姿勢に対する重み（ｗＲｋ）を示し、それぞれ式（７）及び式（８）で計算される。
　ｋ番目のフレームの位置に対する重みｗｔｋ及びｋ番目のフレームの姿勢に対する重みｗＲｋは、位置・姿勢の計算に用いたキーフレームの分散である位置の分散σｔｋ ２及び姿勢の分散σＲｋ ２を用いて計算される。また、位置の分散σｔｋ ２及び姿勢の分散σＲｋ ２と等価な統計指標とみなすことができる標準偏差、すなわち、位置の標準偏差σｔｋ及び姿勢の標準偏差σＲｋを用いて重みを計算してもよい。標準偏差を用いる場合、ｋ番目のフレームの位置に対する重みｗｔｋ及びｋ番目のフレームの姿勢に対する重みｗＲｋは、それぞれ式（９）及び式（１０）で計算される。
Figure JPOXMLDOC01-appb-M000008
　式（５）及び式（６）において、ｋ番目のフレームの位置ｔ´ｋとｋ番目のフレームの姿勢Ｒ´ｋは、絶対位置・姿勢計算部１８で計算された位置・姿勢を直接入力するわけではない。絶対位置・姿勢計算部１８で計算された位置・姿勢から、任意のフレームに移動したときの位置・姿勢が用いられる。例えば、フレーム選択部１６でＫ枚（Ｋは、正の整数）のフレームを選択し、Ｋ番目のフレームに合わせて統合する場合、ｋ番目（ｋは、正の整数）のフレームの位置ｔ´ｋとｋ番目のフレームの姿勢Ｒ´ｋは、式（１１）で示される。式（１１）において、ｋ番目のフレームの位置ｔｋとｋ番目のフレームの姿勢Ｒｋは、絶対位置・姿勢計算部１８で求めた位置・姿勢である。
Figure JPOXMLDOC01-appb-M000009
 は、それぞれｋ番目のフレームからＫ番目のフレームへの相対的な姿勢の移動量及び相対的な位置の移動量を示し、相対位置・姿勢取得部１７で得られた位置・姿勢から導出される。式（１１）を用いれば、Ｋ番目のフレームの絶対位置・姿勢が求められる。
Figure JPOXMLDOC01-appb-M000010
　第３の統合方法は、非線形最適化により、絶対位置・姿勢を求める方法である。例えば、式（１２）に示されるように、再投影誤差が最小になるように、ｋ番目のフレームの位置ｔｋとｋ番目のフレームの姿勢Ｒｋを求める。
Figure JPOXMLDOC01-appb-M000011
 は、カメラの内部パラメータ（Ｌ）を示す。
Figure JPOXMLDOC01-appb-M000012
 は、それぞれマッチングした局所特徴の３次元的な位置と画像上の点を示す。
　Ｎｋは、ｋ番目のフレームのマッチングした局所特徴ペアの数を示す。ｗｋは、ｋ番目のフレームに対応する重みであり、重みｗｔｋ又はｗＲｋのいずれか、又は、これらを統合した重みが用いられる。
　式（１２）を最急降下法などの非線形最適化手法で解くと、絶対位置・姿勢を得ることができる。
Figure JPOXMLDOC01-appb-M000013
《１－２》動作
《１－２－１》３次元地図の生成
　図９は、実施の形態１に係る３次元地図作成装置による３次元地図の作成のための処理の例を示すフローチャートである。図９に示されるように、キーフレーム検出部１０及びキーフレーム位置・姿勢計算部１１は、３次元地図の作成を実行する（ステップＳ１０１）。３次元地図の作成は、例えば、ＳＬＡＭを用いて、キーフレームを検出しながら実行される。
　位置・姿勢分散計算部１２は、キーフレーム毎（ステップＳ１０２）に位置・姿勢の分散を計算する（ステップＳ１０３）。次に、対応関係登録部１３は、図８に示されるように、対応関係の登録を行う（ステップＳ１０４）。対応関係登録部１３は、例えば、フロアマップ上に３次元地図を登録して、全体地図又は他の３次元地図との位置関係を定義する処理を実行する。データベース保存部１４は、上記の処理で作成した地図を３次元地図ＤＢ１０２に保存する処理を行う（ステップＳ１０５）。
《１－２－２》位置・姿勢の推定
　図１０は、実施の形態１に係る位置・姿勢推定装置１０１による位置・姿勢の推定のための処理の例を示すフローチャートである。データベース読込部１５は、ＤＢ保存部１４からデータとして３次元地図を読み込む（ステップＳ１１１）。フレーム選択部１６は、フレームを選択する（ステップＳ１１２）。選択されるフレームは、予め決められたルールに基づいて処理するフレームである。
　相対位置・姿勢取得部１７は、選択されたフレーム毎に（ステップＳ１１３）、フレームに対応する相対位置・姿勢を取得する処理を行う（ステップＳ１１４）。絶対位置・姿勢計算部１８は、選択されたフレームのデータに基づいて、絶対位置・姿勢を計算する（ステップＳ１１５）。選択されたフレームの数が、予め定められた枚数に達すると、処理は統合処理に進む。
　絶対位置・姿勢統合部１９は、分散を元に絶対位置・姿勢の結果を統合する（ステップＳ１１６）。
　図１１は、実施の形態１に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の他の例を示すフローチャートである。データベース読込部１５は、データベース保存部１４からデータとして３次元地図を読み込む（ステップＳ１２１）。フレーム選択部１６は、フレームを選択するか否かを判断し（ステップＳ１２２、Ｓ１２３）、フレームを選択する場合に、フレームを選択する。選択されるフレームは、予め決められたルールに基づいて処理するフレームである。
　相対位置・姿勢取得部１７は、選択されたフレームに対応する相対位置・姿勢を取得する処理を行う（ステップＳ１２４）。絶対位置・姿勢計算部１８は、選択された各データに対して、絶対位置・姿勢を計算する（ステップＳ１２５）。フレーム選択部１６は、充分なフレーム検出が完了したか否かを判断し（ステップＳ１２６）、充分なフレーム検出が完了した場合には統合処理を行い、充分なフレーム検出が完了していない場合には処理をステップＳ１２２に戻す。「充分なフレーム検出が完了した」とは、例えば、予め定められた個数のフレーム検出をしたとき、予め定められた数の絶対位置・姿勢が得られたとき、などである。
　充分なフレーム検出が完了した場合には、絶対位置・姿勢統合部１９は、分散を元に絶対位置・姿勢の結果を統合する（ステップＳ１１６）。
《１－３》効果
　以上に説明したように、実施の形態１に係る位置・姿勢推定装置又は位置姿勢推定方法によれば、複数の画像を用いて得られた位置・姿勢のデータに基づいて位置・姿勢の推定を行うので、位置・姿勢の推定の精度を向上させることができる。
　また、位置・姿勢の計算の結果の分散が大きい画像から求めた位置・姿勢を使わないで統合処理を行う、若しくは、位置・姿勢の計算の結果の分散が大きい画像から求めた位置・姿勢についての重みを小さくして統合処理を行うことで、精度の高い絶対位置・姿勢を計算することができる。
　また、画像内の被写体が特徴に乏しい場合であっても、絶対座標系における、精度の高い自己の位置・姿勢の推定が実現でき、かつ演算量をリアルタイム処理が可能な範囲に抑えることができる。
《２》実施の形態２
《２－１》構成
《２－１－１》
　実施の形態１では、カメラ画像を使った絶対位置・姿勢の計算方法として、直接的なマッチングを用いる第１の計算方法と、２段階のマッチングを用いる第２の計算方法とがあることを説明した。また、実施の形態１では、第２の計算方法を用いる例を説明した。実施の形態２では、カメラ画像を使った直接的なマッチング方法で求めた位置・姿勢を統合する方法と、ＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ）などレーザセンサを使った位置・姿勢統合方法を説明する。
　実施の形態２に係る位置・姿勢推定装置及び位置姿勢推定システムのハードウェア構成は、実施の形態１（図４及び図５）で説明したものと同じである。したがって、実施の形態２の説明に際しては、図４及び図５も参照する。
《２－１－２》３次元地図作成装置
　図１２は、実施の形態２に係る３次元地図作成装置の構成を概略的に示す機能ブロック図である。図１２に示される３次元地図作成装置は、実施の形態２に係る３次元地図作成方法を実行することができる装置である。なお、以下の説明では、３次元地図作成装置が、位置・姿勢推定装置の一部（すなわち、地図作成登録部）である場合を説明するが、３次元地図作成装置は、位置・姿勢推定装置とは別個の装置であってもよい。
　図１２に示されるように、実施の形態２に係る３次元地図作成装置は、３次元地図作成部２１と、位置・姿勢分散計算部２２と、対応関係登録部２３と、データベース保存部（ＤＢ保存部）２４とを有している。
　実施の形態１では、キーフレーム毎に３次元データを管理していたが、実施の形態２では、点群を３次元地図として管理する。例えば、３次元地図作成部２１は、画像を用いる場合、画像から得られる局所特徴と、その位置を３次元地図として作成する。ＬｉＤＡＲなどレーザセンサを用いる場合、観測した点群（位置のみ）を３次元地図として作成する。図１２の３次元地図作成部２１は、上述の３次元地図データを作成する処理を行う。
　図１３は、実施の形態２に係る３次元地図作成装置が使用する分散の計算方法を示す図である。図１３は、ＡＧＶが適用されたロボット１３１が移動した場合に、ロボットが相対位置・姿勢の計算と絶対位置・姿勢の計算とを用いて位置・姿勢を推定する例を示す。ロボット１３１は、分散を計算するが、分散の計算方法が、実施の形態１の場合の計算方法と異なる。図１３では、楕円で囲われた領域ごと（例えば、領域＃１、＃２、＃３）の分散σ１、σ２、σ３の計算について説明する。
　具体的な分散の計算では、３次元地図を作るときに観測したデータの中から、領域ごとに任意の個数ずつデータを取得（すなわち、サンプリング）する。サンプリングしたデータに対して、複数回異なる方法でノイズを加えて絶対位置・姿勢の推定を行う。求める分散は、ある特定領域ごとの位置・姿勢の分散である。ノイズを加える方法は、実施の形態１と同様に画像の場合には、ランダムパターンを付加する方法である。ＬｉＤＡＲの場合も同様に、局所的な領域にランダムパターンを付加することでノイズを付加する。ここで「ランダムパターン」とは、その領域におけるデータを取り除くためのパターンも含まれる。
　対応関係登録部２３は、実施の形態１の対応関係登録部１３と同様の方法で、全体地図又は他の３次元地図との関係性を定義する。
　ＤＢ保存部２４は、３次元地図と、領域ごとの分散をデータベースに保存する。
《２－１－３》位置・姿勢推定装置
　図１４は、実施の形態２に係る位置・姿勢推定装置の構成を概略的に示す機能ブロック図である。図１４に示される位置・姿勢推定装置は、実施の形態２に係る位置・姿勢推定方法を実行することができる装置である。位置・姿勢推定装置は、データベース読込部２５と、フレーム選択部２６と、相対位置・姿勢取得部２７と、絶対位置・姿勢計算部２８と、絶対位置・姿勢統合部２９とを有している。
　データベース読込部２５は、データベースに格納されている３次元地図データを読み込む処理を行う。
　フレーム選択部２６は、実施の形態１のフレーム選択部１６と同様に、又は、予め区分けした複数の領域から重複がないようにフレームを選択する。例えば、図１３に示されるように、３つの領域＃１、＃２、＃３に区分けして分散σ１、σ２、σ３を算出し、管理している例で説明する。位置・姿勢の計算結果が領域＃１、＃２、＃３の各々に含まれるフレームの数がＭ枚の例を説明する。
　相対位置・姿勢取得部２７は、実施の形態１における相対位置・姿勢取得部１７と同じ方法で相対位置・姿勢を取得する。
　絶対位置・姿勢計算部２８は、画像の場合は、画像の局所特徴を直接的にマッチングして位置・姿勢の計算する方法（例えば、非特許文献４を参照）などを用いて絶対位置・姿勢を計算する。ＬｉＤＡＲを用いた距離センサのデータを使用する場合、３次元地図と距離センサで観測した形状情報とのマッチングを行うことで、位置・姿勢を計算する。
Ｔｏｒｓｔｅｎ　Ｓａｔｔｌｅｒ、外２名著、　"Ｅｆｆｉｃｉｅｎｔ　＆　Ｅｆｆｅｃｔｉｖｅ　Ｐｒｉｏｒｉｔｉｚｅｄ　Ｍａｔｃｈｉｎｇ　ｆｏｒ　Ｌａｒｇｅ－Ｓｃａｌｅ　Ｉｍａｇｅ－Ｂａｓｅｄ　Ｌｏｃａｌｉｚａｔｉｏｎ"、　ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ｐａｔｔｅｒｎ　Ａｎａｌｙｓｉｓ　ａｎｄ　Ｍａｃｈｉｎｅ　Ｉｎｔｅｌｌｉｇｅｎｃｅ、　Ｖｏｌ．３９、　Ｎｏ．９，　２０１７年９月．
　絶対位置・姿勢統合部２９は、実施の形態１における絶対位置・姿勢統合部１９と同様の方法で、複数の位置・姿勢を統合する。絶対位置・姿勢統合部２９は、領域毎に設定された分散を元に、最終的位置・姿勢を求める。
《２－２》動作
《２－２－１》３次元地図の作成
　図１５は、実施の形態２に係る３次元地図作成装置による３次元地図の作成のための処理の例を示すフローチャートである。３次元地図作成部２１及び位置・姿勢分散計算部２２は、３次元地図を作成し、画像を用いる場合には、局所特徴とその位置を示す地図、ＬｉＤＡＲを用いる場合には、点群（位置のみ）を示す地図を作成する（ステップＳ２０１）。対応関係登録部２３は、３次元地図作成部２１に対応する処理である対応関係の登録を行う（ステップＳ２０２）。データベース保存部２４は、対応関係をデータベースに保存する処理を行う（ステップＳ２０３）。
《２－２－２》位置・姿勢の推定
　図１６は、実施の形態２に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の例を示すフローチャートである。データベース読込部２５は、データベースからデータを読み込む処理を行う（ステップＳ２１１）。相対位置・姿勢取得部２７は、相対位置・姿勢を取得し、絶対位置・姿勢計算部２８は、絶対位置・姿勢の計算を行う（ステップＳ２１２～Ｓ２１４）。
　フレーム選択部は、フレーム選択の要否を判断して（ステップＳ２１５）、必要な場合には、充分なフレーム検出が完了しているか否かを判断する（ステップＳ２１６）。充分なフレーム検出が完了している場合には、絶対位置・姿勢統合部２９は、絶対位置・姿勢の統合を行う（ステップＳ２１７）。
《２－３》効果
　以上に説明したように、実施の形態２に係る位置・姿勢推定装置又は位置姿勢推定方法によれば、画像を使用しかつ直接的なマッチング方法を用いる場合、又は、ＬｉＤＡＲで観測した形状を用いて絶対位置・姿勢を計算する例において、複数のフレームを用いているので位置・姿勢の推定の精度を向上させることができる。
《３》実施の形態３
《３－１》構成
《３－１－１》
　ＡＲで用いる端末又はロボットは、相対位置・姿勢を機器内で管理している場合がある。ＡＲの場合、絶対位置・姿勢で定義されたコンテンツの位置を端末で管理している相対位置・姿勢の座標系に変換することで、コンテンツを画像上に重畳表示する。ロボットの場合も同様で、ロボットの目的地が絶対位置で定義されている場合、この位置をロボットが管理する相対位置・姿勢の座標系に変換する必要がある。
　実施の形態３は、絶対位置・姿勢の座標系から相対位置・姿勢の座標系に変換する行列である外部パラメータを、複数のフレームを用いて高精度に計算する方法を述べる。ここでは、実施の形態１を基本として、複数のフレームを用いて外部パラメータを高精度に求める方法を述べる。実施の形態３の構成は、実施の形態２又は４と組み合わせてもよい。
　実施の形態２に係る位置・姿勢推定装置及び位置姿勢推定システムのハードウェア構成は、実施の形態１（図４及び図５）で説明したものと同じである。したがって、実施の形態２の説明に際しては、図４及び図５も参照する。
《３－１－２》３次元地図作成装置
　実施の形態２に係る３次元地図作成装置は、実施の形態１のものと同じである。
《３－１－３》位置・姿勢推定装置
　図１７は、実施の形態３に係る位置・姿勢推定装置の構成を概略的に示す機能ブロック図である。図１７に示される位置・姿勢推定装置は、実施の形態３に係る位置・姿勢推定方法を実行することができる装置である。位置・姿勢推定装置は、データベース読込部３５と、フレーム選択部３６と、相対位置・姿勢取得部３７と、絶対位置・姿勢計算部３８と、外部パラメータ計算部３８ａと、絶対位置・姿勢統合部としての外部パラメータ統合部３９とを有している。
　実施の形態３が実施の形態１と異なる点は、選択したフレーム毎に外部パラメータを計算する外部パラメータ計算部３８ａと、複数の外部パラメータを統合する外部パラメータ統合部３９を有する点である。これら以外の処理に関し、実施の形態３の処理は、実施の形態１の処理と同じである。
　外部パラメータ計算部３８ａは、同じフレームを元に相対位置・姿勢の計算と、絶対位置・姿勢の計算が行われている想定で外部パラメータを計算する。このとき、外部パラメータは、式（１３）で計算される。
Figure JPOXMLDOC01-appb-M000014
 は、ｋ番目のフレームを用いて求めた絶対位置及び絶対姿勢、を示す。
Figure JPOXMLDOC01-appb-M000015
 は、ｋ番目のフレームの相対位置及び相対姿勢を示す。
Figure JPOXMLDOC01-appb-M000016
 は、ｋ番目のフレームの外部パラメータ（ｔ´ｋ及びＲ´ｋ）を示し、式（１３）で計算される。
Figure JPOXMLDOC01-appb-M000017
　外部パラメータ統合部３９の処理は、実施の形態１の絶対位置・姿勢統合部１９（図８）と概ね同じである。したがって、外部パラメータ統合部３９は、絶対位置・姿勢統合部とも称する。実施の形態１では、複数の絶対位置・姿勢を統合しているが、実施の形態３では、外部パラメータを統合する。具体的には、式（１３）で求めたｔ´ｋ及びＲ´ｋを用いて統合する。例えば、重みづけ線形和で統合する場合、式（１３）のｔ´ｋ、Ｒ´ｋを式（５）及び式（６）に代入して統合する。
《３－２》動作
《３－２－１》３次元地図の作成
　３次元地図作成の処理の流れは、実施の形態１と同じであるため割愛する。外部パラメータを統合する方法の処理の流れを述べる。
《３－２－２》位置・姿勢の推定
　図１８は、実施の形態３に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の例を示すフローチャートである。データベース読込部３５は、データベースからデータを読み込む処理を行う（ステップＳ３０１）。相対位置・姿勢取得部３７は、装置位置・姿勢を取得する（ステップＳ３０２、Ｓ３０３）。絶対位置・姿勢計算部３８は、絶対位置・姿勢の計算を行う（ステップＳ３０４）。外部パラメータ計算部３８ａは、外部パラメータを計算する（ステップＳ３０５）。
　フレーム選択部３６は、フレーム選択の要否を判断して（ステップＳ３０６）、必要な場合には、充分なフレーム検出が完了しているか否かを判断する（ステップＳ３０７）。充分なフレーム検出が完了している場合には、外部パラメータ統合部３９は、絶対位置・姿勢の統合を行う（ステップＳ３０８）。
《３－３》効果
　以上に説明したように、実施の形態３に係る位置・姿勢推定装置又は位置姿勢推定方法によれば、絶対位置・姿勢の座標系から相対位置・姿勢の座標系への変換行列を高精度に求めることができるので、ＡＲに適用した端末にコンテンツを高精度に表示することができる。また、ロボットに適用した場合、ロボットの目的地を高精度に求めることができる。
《４》実施の形態４
《４－１》構成
《４－１－１》
　実施の形態４では、実施の形態１における位置・姿勢推定方法に、複数のフレームを用いたエラー処理を加えた形態を述べる。画像を用いた絶対位置・姿勢の計算では、被写体に特徴的な模様が少ない場合、出力される位置・姿勢の値に大きな誤差が含まれる可能性がある。このような結果を除外するために、実施の形態４に係る位置・姿勢推定装置は、エラー処理を実行する。
　実施の形態４に係る位置・姿勢推定装置及び位置姿勢推定システムのハードウェア構成は、基本的には、実施の形態１（図４及び図５）で説明したものと同じである。したがって、実施の形態４の説明に際しては、図４及び図５も参照する。
《４－１－２》３次元地図作成装置
　実施の形態４に係る３次元地図作成装置の構成は、実施の形態１のものと同じである。
《４－１－３》位置・姿勢推定装置
　図１９は、実施の形態４に係る位置・姿勢推定装置の構成を概略的に示す機能ブロック図である。図１９に示される位置・姿勢推定装置は、実施の形態４に係る位置・姿勢推定方法を実行することができる装置である。実施の形態４に係る位置・姿勢推定装置は、エラー処理部４８ａが追加された点で、実施の形態１のものと異なる。
　図１９に示されるように、実施の形態４に係る位置・姿勢推定装置は、データベース読込部４５と、フレーム選択部４６と、相対位置・姿勢取得部４７と、絶対位置・姿勢計算部４８と、エラー処理部４８ａと、絶対位置・姿勢統合部４９とを有している。データベース読込部４５、フレーム選択部４６、相対位置・姿勢取得部４７、絶対位置・姿勢計算部４８、及び絶対位置・姿勢統合部４９は、図８に示されるデータベース読込部１５、フレーム選択部１６、相対位置・姿勢取得部１７、絶対位置・姿勢計算部１８、及び絶対位置・姿勢統合部１９と同様である。
　エラー処理部４８ａは、エラー処理を行う。エラー処理では、複数のフレームで求めた絶対位置・姿勢の複数の計算結果を比較することで、誤差が予め決めされた閾値より大きい絶対位置・姿勢の計算結果を、統合処理から除外する。エラー処理の一例を式（１４）及び式（１５）に示す。ｊ番目（ｊはＫ以下の正の整数である。）のフレームは、フレーム選択部４６で選択されたＫ枚のフレームの中でｊ≠ｋを満たすフレームである。複数のフレームで求めた絶対位置・姿勢の複数の計算結果は、同じ位置・姿勢を指し示すはずである。したがって、エラー処理部４８ａは、あるフレームで求めた絶対位置・姿勢の計算結果を他のフレームで求めた絶対位置・姿勢の計算結果と比較する。エラー処理部４８ａは、計算した位置の差が予め決められた閾値ｔｈ＿ｔより大きい場合（すなわち、式（１４）を満たす場合）、又は計算した姿勢の差が予め決められた閾値ｔｈ＿ｒより大きい場合（すなわち、式（１５）を満たす場合）には、そのフレームの計算結果を除外する。
Figure JPOXMLDOC01-appb-M000018
《４－２》動作
《４－２－１》３次元地図の作成
　実施の形態４に係る３次元地図作成装置の動作は、実施の形態１のものと同じである。
《４－２－２》位置・姿勢の推定
　図２０は、実施の形態４に係る位置・姿勢推定装置による位置・姿勢の推定のための処理の他の例を示すフローチャートである。図２０に示される位置・姿勢推定装置の動作は、エラー処理（ステップＳ４０６ａ）が追加されている点で、図１１に示される実施の形態１に係る位置・姿勢推定装置の動作と相違する。図２０に示されるステップＳ４０１～Ｓ４０６の処理、及びステップＳ４０７の処理は、図１１に示される、ステップＳ１０１～Ｓ１０７の処理と同様である。
《４－３》効果
　以上に説明したように、実施の形態４に係る位置・姿勢推定装置又は位置姿勢推定方法によれば、エラー処理が行われるので、実施の形態１の場合よりも耐環境性の高い絶対位置・姿勢の推定を実現できる（すなわち、各種の環境に対して絶対位置・姿勢の高い推定精度を実現できる）。
　１０　キーフレーム検出部、　１１　キーフレーム位置・姿勢計算部、　１２、２２　位置・姿勢分散計算部、　１３、２３　対応関係登録部、　１４、２４　ＤＢ保存部、　１５、２５、３５、４５　データベース読込部、　１６、２６、３６、４６　フレーム選択部、　１７、２７、３７、４７　相対位置・姿勢取得部、　１８、２８、３８、４８　絶対位置・姿勢計算部、　１９、２９、４９　絶対位置・姿勢統合部、　２１　３次元地図作成部、　３８ａ　外部パラメータ計算部、　３９　外部パラメータ統合部（絶対位置・姿勢統合部）、　４８ａ　エラー処理部、　１００　位置・姿勢推定システム、　１０１　位置・姿勢推定装置、　１０２　３次元地図ＤＢ、　１０３　距離センサ、　１０４　カメラ、　１０５　ディスプレイ、　１０６　ジャイロセンサ、　１０７　加速度センサ、　１０８　地磁気センサ。

Claims (13)

  1. 　データベースから３次元地図のデータを読み込むデータベース読込部と、
    　複数の異なる視点から撮影した画像のフレームの中から位置・姿勢の計算に使用するフレームを選択する処理を行うフレーム選択部と、
    　前記フレーム選択部によって選択された複数のフレームについての複数の相対位置・姿勢を取得する処理を行う相対位置・姿勢取得部と、
    　前記フレーム選択部によって選択された複数のフレームについての複数の絶対位置・姿勢を取得する処理を行う絶対位置・姿勢計算部と、
    　前記相対位置・姿勢取得部によって取得された相対位置・姿勢と、前記絶対位置・姿勢計算部によって取得された絶対位置・姿勢と、を統合して、最終的な絶対位置・姿勢を取得する絶対位置・姿勢統合部と、
    　を有する位置・姿勢推定装置。
  2. 　地図作成登録部をさらに有し、
    　前記地図作成登録部は、
    　カメラで撮影されたカメラ画像からキーフレームを検出するキーフレーム検出部と、
    　前記キーフレームを撮影した前記カメラの位置及び姿勢を計算するキーフレーム位置・姿勢計算部と、
    　前記キーフレーム毎に位置・姿勢の分散を計算する処理を実行する位置・姿勢分散計算部と、
    　フロアマップに３次元地図を位置合わせして登録データを作成する処理を行う対応関係登録部と、
    　前記データベースに前記登録データを保存する処理を行うデータベース保存と、を有する
    　請求項１に記載の位置・姿勢推定装置。
  3. 　前記絶対位置・姿勢統合部は、前記キーフレーム毎に計算された前記位置・姿勢の分散に基づいて、前記複数の絶対位置・姿勢を統合する
    　請求項２に記載の位置・姿勢推定装置。
  4. 　前記絶対位置・姿勢統合部は、前記キーフレームのうちの、前記分散が最も小さいキーフレームで推定した位置・姿勢を前記最終的な絶対位置・姿勢として採用する
    　請求項３に記載の位置・姿勢推定装置。
  5. 　前記絶対位置・姿勢統合部は、前記キーフレーム毎に、前記分散に基づく重みを計算し、前記重みを用いる重み付け線形和に基づいて、前記複数の絶対位置・姿勢を統合する
    　請求項３に記載の位置・姿勢推定装置。
  6. 　前記絶対位置・姿勢統合部は、非線形最適化を用いて前記複数の絶対位置・姿勢を統合する
    　請求項２に記載の位置・姿勢推定装置。
  7. 　地図作成登録部をさらに有し、
    　前記地図作成登録部は、
    　カメラで撮影されたカメラ画像又は距離センサで測定された距離情報から局所的な領域毎に３次元地図を作成する３次元地図作成部と、
    　前記カメラ画像又は前記距離情報から前記領域毎に位置・姿勢の分散を計算する処理を実行する位置・姿勢分散計算部と、
    　フロアマップに３次元地図を位置合わせして登録データを作成する処理を行う対応関係登録部と、
    　前記データベースに前記登録データを保存する処理を行うデータベース保存と、を有する
    　請求項１に記載の位置・姿勢推定装置。
  8. 　前記絶対位置・姿勢統合部は、前記キーフレーム毎に計算された前記領域毎の位置・姿勢の分散に基づいて、前記複数の絶対位置・姿勢を統合する
    　請求項２に記載の位置・姿勢推定装置。
  9. 　前記キーフレーム毎に外部パラメータを計算する外部パラメータ計算部をさらに有し、
    　前記絶対位置・姿勢統合部は、前記外部パラメータ計算部で計算された複数の外部パラメータを統合することで前記複数の絶対位置・姿勢を統合する
    　請求項２に記載の位置・姿勢推定装置。
  10. 　前記絶対位置・姿勢統合部は、前記キーフレーム毎に計算された前記位置・姿勢の分散に基づいて、前記複数の外部パラメータを統合する
    　請求項９に記載の位置・姿勢推定装置。
  11. 　前記キーフレーム毎に計算された絶対位置・姿勢の誤差が予め決めされた閾値より大きい場合、前記閾値より大きい誤差の前記絶対位置・姿勢の計算結果を統合処理に使用させないエラー処理部をさらに有する
    　請求項１から１０のいずれか１項に記載の位置・姿勢推定装置。
  12. 　位置・姿勢推定装置が実行する位置・姿勢推定方法であって、
    　位置データベースから３次元地図のデータを読み込むステップと、
    　複数の異なる視点から撮影した画像のフレームの中から位置・姿勢の計算に使用するフレームを選択する処理を行うステップと、
    　選択された前記複数のフレームについての複数の相対位置・姿勢を取得する処理を行うステップと、
    　前記選択された複数のフレームについての複数の絶対位置・姿勢を取得する処理を行うステップと、
    　取得された前記相対位置・姿勢と、取得された前記絶対位置・姿勢と、を統合して、最終的な絶対位置・姿勢を取得するステップと、
    　を有する位置・姿勢推定方法。
  13. 　位置データベースから３次元地図のデータを読み込むステップと、
    　複数の異なる視点から撮影した画像のフレームの中から位置・姿勢の計算に使用するフレームを選択する処理を行うステップと、
    　選択された前記複数のフレームについての複数の相対位置・姿勢を取得する処理を行うステップと、
    　前記選択された複数のフレームについての複数の絶対位置・姿勢を取得する処理を行うステップと、
    　取得された前記相対位置・姿勢と、取得された前記絶対位置・姿勢と、を統合して、最終的な絶対位置・姿勢を取得するステップと、
    　をコンピュータに実行させるプログラム。
PCT/JP2020/047417 2020-12-18 2020-12-18 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム WO2022130618A1 (ja)

Priority Applications (7)

Application Number Priority Date Filing Date Title
JP2022569662A JP7258250B2 (ja) 2020-12-18 2020-12-18 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム
CN202080107829.5A CN116635891A (zh) 2020-12-18 2020-12-18 位置/姿态估计装置、位置/姿态估计方法及程序
KR1020237018897A KR20230087613A (ko) 2020-12-18 2020-12-18 위치·자세 추정 장치, 위치·자세 추정 방법, 및 프로그램
DE112020007700.4T DE112020007700T5 (de) 2020-12-18 2020-12-18 Positions-lage-schätzeinrichtung, positions-lage-schätzverfahren und programm
PCT/JP2020/047417 WO2022130618A1 (ja) 2020-12-18 2020-12-18 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム
TW110117868A TWI817124B (zh) 2020-12-18 2021-05-18 位置/姿勢推定裝置、位置/姿勢推定方法及記錄其程式之記錄媒體
US18/138,300 US20230260149A1 (en) 2020-12-18 2023-04-24 Position-posture estimation device, position-posture estimation method, and storage medium storing program

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2020/047417 WO2022130618A1 (ja) 2020-12-18 2020-12-18 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/138,300 Continuation US20230260149A1 (en) 2020-12-18 2023-04-24 Position-posture estimation device, position-posture estimation method, and storage medium storing program

Publications (1)

Publication Number Publication Date
WO2022130618A1 true WO2022130618A1 (ja) 2022-06-23

Family

ID=82059339

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2020/047417 WO2022130618A1 (ja) 2020-12-18 2020-12-18 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム

Country Status (7)

Country Link
US (1) US20230260149A1 (ja)
JP (1) JP7258250B2 (ja)
KR (1) KR20230087613A (ja)
CN (1) CN116635891A (ja)
DE (1) DE112020007700T5 (ja)
TW (1) TWI817124B (ja)
WO (1) WO2022130618A1 (ja)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018142580A1 (ja) * 2017-02-03 2018-08-09 三菱電機株式会社 表示制御装置および表示制御方法
JP2018147095A (ja) * 2017-03-02 2018-09-20 富士通株式会社 カメラ位置姿勢推定装置、方法およびプログラム
WO2019098002A1 (ja) * 2017-11-20 2019-05-23 ソニー株式会社 情報処理装置、情報処理方法、プログラム、及び移動体
WO2020137312A1 (ja) * 2018-12-28 2020-07-02 パナソニックＩｐマネジメント株式会社 測位装置及び移動体

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2803060A1 (en) * 2012-01-13 2014-11-19 Pulse Function F6 Limited Telematics system with 3d inertial sensors
JP7077691B2 (ja) 2018-03-16 2022-05-31 トヨタ自動車株式会社 自己位置検出装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018142580A1 (ja) * 2017-02-03 2018-08-09 三菱電機株式会社 表示制御装置および表示制御方法
JP2018147095A (ja) * 2017-03-02 2018-09-20 富士通株式会社 カメラ位置姿勢推定装置、方法およびプログラム
WO2019098002A1 (ja) * 2017-11-20 2019-05-23 ソニー株式会社 情報処理装置、情報処理方法、プログラム、及び移動体
WO2020137312A1 (ja) * 2018-12-28 2020-07-02 パナソニックＩｐマネジメント株式会社 測位装置及び移動体

Also Published As

Publication number Publication date
JP7258250B2 (ja) 2023-04-14
CN116635891A (zh) 2023-08-22
JPWO2022130618A1 (ja) 2022-06-23
DE112020007700T5 (de) 2023-08-03
TW202226003A (zh) 2022-07-01
KR20230087613A (ko) 2023-06-16
US20230260149A1 (en) 2023-08-17
TWI817124B (zh) 2023-10-01

Similar Documents

Publication Publication Date Title
WO2018177159A1 (zh) 运动物体的位置确定方法及系统
JP5548482B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、プログラム及び記憶媒体
JP5671281B2 (ja) 位置姿勢計測装置、位置姿勢計測装置の制御方法及びプログラム
CN112567201A (zh) 距离测量方法以及设备
WO2017077925A1 (en) Method and system for estimating three-dimensional pose of sensor
US10895458B2 (en) Method, apparatus, and system for determining a movement of a mobile platform
JP5023186B2 (ja) ３ｄワーピング手法と固有対象物運動（ｐｏｍ）の検出の組み合わせに基づく対象物の動き検出システム
US10451403B2 (en) Structure-based camera pose estimation system
US11062475B2 (en) Location estimating apparatus and method, learning apparatus and method, and computer program products
KR102169309B1 (ko) 정보 처리장치 및 그 제어방법
JP2015184767A (ja) 情報処理装置、情報処理方法、位置姿勢推定装置、ロボットシステム
CN112750168B (zh) 事件相机内参的标定方法、装置、计算机设备和存储介质
JP6817742B2 (ja) 情報処理装置およびその制御方法
CN114179788A (zh) 自动泊车方法、系统、计算机可读存储介质及车机端
KR20180027242A (ko) 무인 차량을 위한 주변 환경 매핑 방법 및 장치
US20190371001A1 (en) Information processing apparatus, method of controlling information processing apparatus, and non-transitory computer-readable storage medium
JP5083715B2 (ja) 三次元位置姿勢計測方法および装置
Kokovkina et al. The algorithm of EKF-SLAM using laser scanning system and fisheye camera
WO2022130618A1 (ja) 位置・姿勢推定装置、位置・姿勢推定方法、及びプログラム
CN115862124A (zh) 视线估计方法、装置、可读存储介质及电子设备
CN111489384B (zh) 基于互视角的遮挡评估方法及装置、设备、系统和介质
GB2605621A (en) Monocular depth estimation
Elgazzar et al. Active range sensing for indoor environment modeling
CN115683109B (zh) 基于cuda和三维栅格地图的视觉动态障碍物检测方法
US20230030596A1 (en) Apparatus and method for estimating uncertainty of image coordinate

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20966005

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2022569662

Country of ref document: JP

Kind code of ref document: A

ENP Entry into the national phase

Ref document number: 20237018897

Country of ref document: KR

Kind code of ref document: A

WWE Wipo information: entry into national phase

Ref document number: 202080107829.5

Country of ref document: CN

122 Ep: pct application non-entry in european phase

Ref document number: 20966005

Country of ref document: EP

Kind code of ref document: A1