JP2018185239A - Position attitude estimation device and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To highly accurately estimate a position and attitude of a moving body even when using a single camera with an angle of view narrow.SOLUTION: A position attitude estimation device 10A comprises: a characteristic point extraction unit 14 that extracts a characteristic point from each of image information at each time acquired by an image information acquisition unit 12; a three-dimensional position identification unit 20 that collates the characteristic point of the image information on each time with respective characteristic points of a plurality of collation-purpose landmarks acquired from a map DB 16 to identify a three-dimensional position in an actual space of the characteristic point of the landmark included in respective images at each time; a relative position attitude calculation unit 24 that calculates a relative position and attitude with respect to a reference point of a moving body at each time; and a state estimation unit 28 that estimates a three-dimensional position and an attitude in an actual space of the moving body on the basis of the three-dimensional position in the actual space of the characteristic point of the landmark to be included in each of the image information at each time, and time series information representing the relative position and attitude of the moving body at each time.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a position / orientation estimation apparatus and program.

  Conventionally, the position of a moving object is estimated by comparing each of a plurality of verification landmark images stored in advance in a map database with a landmark image photographed by a camera mounted on the moving object. There is technology. In this type of technology, the accuracy of estimating the position of a moving object tends to improve as the number of landmarks to be collated is increased and the landmarks are distributed over a wide range in a three-dimensional space. . For this reason, it is generally considered that the position of the moving object can be estimated with higher accuracy by using an omnidirectional camera having a wider angle of view than a single camera having a narrow angle of view.

  However, since an omnidirectional camera is more expensive than a single camera, it is desirable that the position of a moving object can be estimated with high accuracy even when a single camera is used.

  For example, Patent Document 1 uses a plurality of sensors to acquire information about landmarks located around a moving object, and selectively combines the acquired information to determine the position where the existence probability of the moving object is the highest. A technique for estimating the position of a moving object is described.

Japanese Patent Laid-Open No. 2006-014647

  However, in the technique described in Patent Document 1, when there are few sensor measurement errors or landmarks that can be observed around the moving body, the existence probability of the moving body is distributed gently over a wide range. . For this reason, it is difficult to uniquely identify the position having the highest existence probability, and it may be difficult to estimate the position of the moving object with high accuracy.

  The present invention has been made to solve the above-described problems, and a position and orientation estimation apparatus capable of estimating the position and orientation of a moving body with high accuracy even when a single camera with a narrow angle of view is used. And to provide a program.

  In order to achieve the above object, the position and orientation estimation apparatus according to claim 1 acquires image information at each time when the periphery of the moving body is imaged in time series by an imaging device mounted on the moving body. An acquisition unit, an extraction unit for extracting feature points from each of the image information at each time acquired by the image information acquisition unit, a three-dimensional position of each of the plurality of reference landmarks obtained in advance in real space, and A map database in which each feature point of the plurality of matching landmarks is stored in association with each other, a feature point of image information at each time extracted by the extraction unit, and the plurality of matching obtained from the map database A specifying unit for specifying the three-dimensional position in the real space of the landmark feature point included in each of the image information at each time by collating with the feature point of each landmark, A calculation unit for calculating the relative position and orientation of the moving body with respect to the reference point for each time, and a three-dimensional real space of the landmark feature points included in each of the image information at each time specified by the specifying unit Estimating the three-dimensional position and orientation of the moving object in real space based on time-series information representing the position and the relative position and orientation of the moving object relative to the reference point at each time calculated by the calculation unit And an estimating unit.

  According to a second aspect of the present invention, there is provided the position / orientation estimation apparatus according to the first aspect of the invention, wherein the sensor information includes sensor information including speeds and angular velocities detected by the sensors mounted on the moving body. An acquisition unit is further provided, and the calculation unit calculates a relative position and orientation with respect to a reference point of the moving body for each time based on the sensor information acquired by the sensor information acquisition unit.

  The position and orientation estimation apparatus according to claim 3 is the landmark included in the image information at each time acquired by the image information acquisition unit from the time-series information in the invention according to claim 1 or 2. And a selection unit that selects a range of time-series information to be provided to the estimation unit based on the number.

  According to a fourth aspect of the present invention, in the invention according to the third aspect, the number of landmarks is the total number of landmarks or the number of types of landmarks.

  Furthermore, in order to achieve the above object, the program according to claim 5 obtains image information at each time when the computer is photographed around the moving body in time series by the photographing device mounted on the moving body. An image information acquisition unit, an extraction unit for extracting feature points from each piece of image information at each time acquired by the image information acquisition unit, and feature points of the image information at each time extracted by the extraction unit; Each of the plurality of verification landmarks acquired from the map database storing the three-dimensional position of each of the plurality of verification landmarks in real space and the feature points of each of the plurality of verification landmarks in association with each other. A specifying unit for specifying a three-dimensional position in the real space of the landmark feature point included in each piece of image information at each time by comparing the feature point with the reference point of the moving object A calculation unit that calculates a relative position and orientation with respect to each time, a three-dimensional position in a real space of a landmark feature point included in each of the image information at each time specified by the specifying unit, and An estimation unit for estimating a three-dimensional position and orientation in the real space of the mobile body based on time-series information representing the relative position and orientation of the mobile body relative to a reference point at each time calculated by the calculation unit; It is to function as.

  As described above, according to the present invention, even when a single camera with a narrow angle of view is used, the position and orientation of the moving object can be estimated with high accuracy.

It is a block diagram which shows an example of a structure of the position and orientation estimation apparatus which concerns on 1st Embodiment. It is a block diagram which shows an example of a structure of the computer which functions as a position and orientation estimation apparatus according to the first embodiment. It is a figure for demonstrating an example of the relationship between the vehicle which concerns on embodiment, and several landmarks. It is a flowchart which shows an example of the flow of a process of the position and orientation estimation process program which concerns on 1st Embodiment. It is a block diagram which shows an example of a structure of the position and orientation estimation apparatus which concerns on 2nd Embodiment.

  Hereinafter, an example of an embodiment for carrying out the present invention will be described in detail with reference to the drawings.

[First embodiment]
FIG. 1 is a block diagram illustrating an example of a configuration of a position / orientation estimation apparatus 10A according to the first embodiment.
As shown in FIG. 1, the position / orientation estimation apparatus 10 </ b> A according to the present embodiment includes an image information acquisition unit 12, a feature point extraction unit 14, a map database (hereinafter referred to as a map DB) 16, map information acquisition units 18, 3. A dimension position specifying unit 20, a sensor information acquisition unit 22, a relative position and orientation calculation unit 24, a time series information storage unit 26, and a state estimation unit 28 are provided.

FIG. 2 is a block diagram illustrating an example of a configuration of a computer that functions as the position / orientation estimation apparatus 10A according to the first embodiment.
As shown in FIG. 2, the position / orientation estimation apparatus 10 </ b> A according to the present embodiment is configured as a general-purpose computer including a CPU (Central Processing Unit) 50 and an internal memory 52.

  The internal memory 52 stores a position / orientation estimation processing program according to the present embodiment. This position and orientation estimation processing program may be installed in advance in the position and orientation estimation apparatus 10A, for example. Further, the position / orientation estimation processing program may be realized by being stored in a non-volatile storage medium or distributed via a network and appropriately installed in the position / orientation estimation apparatus 10A. Examples of nonvolatile storage media include CD-ROM (Compact Disc Read Only Memory), magneto-optical disk, HDD (Hard Disk Drive), DVD-ROM (Digital Versatile Disc Read Only Memory), flash memory, memory Cards are assumed. The internal memory 52 also functions as the map DB 16 and the time series information storage unit 26 shown in FIG.

  The CPU 50 includes the image information acquisition unit 12, the feature point extraction unit 14, the map information acquisition unit 18, the three-dimensional position specification unit 20, the sensor information acquisition unit 22, the relative position and orientation calculation unit 24, and the state estimation unit 28 illustrated in FIG. Function as. The CPU 50 functions as these units by reading and executing the position / orientation estimation processing program from the internal memory 52. Further, the CPU 50 is connected to each of the camera 40 and the sensor 42 mounted on the vehicle 60 that is an example of the moving body.

  Hereinafter, in the present embodiment, the case where the position / orientation estimation apparatus 10A is mounted on the vehicle 60 will be described. However, the position / orientation estimation apparatus 10A may be installed outside the vehicle 60. Each of the sensors 42 is connected via wireless communication.

  The camera 40 is an example of a photographing device, and photographs the vicinity of the vehicle 60 at each time. One camera 40 or a plurality of cameras 40 may be used. Further, the shooting range of the camera 40 may face the traveling direction (front) of the vehicle 60, or may be backward or omnidirectional.

  The sensor 42 includes, for example, a speed sensor that detects the speed of the vehicle 60 at each time and an angular speed sensor that detects the angular speed of the vehicle 60 at each time. As an example, the speed sensor detects a wheel speed measured by an odometer of a wheel of the vehicle 60. For example, a gyro sensor is used as the angular velocity sensor, and as an example, the yaw rate is detected as the angular velocity. In the case of a three-axis sensor, the pitch angular velocity and the roll angular velocity may be detected together with the yaw rate.

  The image information acquisition unit 12 acquires image information around the vehicle 60 (hereinafter simply referred to as an image) taken by the camera 40 at each time.

  The feature point extraction unit 14 is an example of an extraction unit. The feature point extraction unit 14 extracts feature points from each of the images at each time acquired by the image information acquisition unit 12.

  Various methods have been proposed for selecting feature points. A desired method such as a SIFT method with high robustness, a Harris method that is easy to implement, or a FAST method with low calculation load may be used. In addition to point features, feature quantities such as lines and surfaces may be used. In addition, as a method for tracking feature points, a general method such as the Lucas-Kanade method may be used, or another method may be used.

  On the other hand, in the map DB 16, an image representing each of a plurality of verification landmarks obtained from a three-dimensional position of each of a plurality of verification landmarks obtained in advance in a real space and a pre-captured image. Are stored in association with the feature points of the landmarks for comparison. Note that the collation landmarks here are buildings, signs, signboards, and other objects that can serve as landmarks in real space, and the feature points are characteristic pixel patterns that represent collation landmarks. Stored with quantity. The three-dimensional position of each verification landmark in the real space is acquired in advance using, for example, a technique called LIDAR (Light Detection and Ranging) that performs distance measurement by irradiating a target with laser light. In addition, as the feature amount of the feature point of the matching landmark, a feature amount such as a line or a surface including a point feature is used as described above. In addition, as the feature points of the matching landmarks, feature points extracted from images taken from the angles are stored for each of the matching landmarks that are observed at predetermined angles (for example, 10 °). Yes.

  The map information acquisition unit 18 reads the three-dimensional position and feature points of the landmarks for comparison located around the vehicle 60 from the map DB 16. For example, a simple position of the vehicle 60 is acquired using a GPS (Global Positioning System) function (not shown), and each of the three-dimensional reference landmarks located within a certain range from the acquired position of the vehicle 60 is displayed. The position and feature points are read from the map DB 16 and temporarily stored in the internal memory 52.

  The three-dimensional position specifying unit 20 is an example of a specifying unit. The three-dimensional position specifying unit 20 collates the feature points of the image at each time extracted by the feature point extracting unit 14 with the feature points of the plurality of matching landmarks acquired from the map DB 16. The three-dimensional position in the real space of the landmark feature point included in each image at each time is specified.

  More specifically, the three-dimensional position specifying unit 20 determines the line of sight from the vehicle 60 to each matching landmark based on the positional relationship between the vehicle 60 and each matching landmark temporarily stored in the internal memory 52. A direction is calculated, and a feature point having an angle corresponding to the calculated line-of-sight direction is acquired for each verification landmark. Then, a matching landmark having a feature point that matches or is similar to the feature point of the image at each time extracted by the feature point extraction unit 14 is identified, and the three-dimensional position of the identified matching landmark in the real space is It is given as a three-dimensional position in the real space of the landmark feature point included in each image at each time.

  The three-dimensional position specifying unit 20 also specifies the two-dimensional position in the image of the feature point of the image at each time extracted by the feature point extracting unit 14. The three-dimensional position specifying unit 20 outputs a set of a two-dimensional position in the image and a three-dimensional position in the real space for the landmark included in each image at each time.

  On the other hand, the sensor information acquisition unit 22 acquires sensor information including the speed and angular velocity at each time detected by the sensor 42 mounted on the vehicle 60. This sensor information may be acquired directly from each sensor of the sensor 42 or may be acquired via wireless communication such as Bluetooth (registered trademark).

  The relative position and orientation calculation unit 24 is an example of a calculation unit. The relative position / orientation calculation unit 24 calculates the relative position and orientation of the vehicle 60 with respect to the reference point for each time based on the sensor information (speed and angular velocity) acquired by the sensor information acquisition unit 22. In the present embodiment, for example, a point where calculation of the relative position and orientation of the vehicle 60 is started is applied as the reference point, but any other point may be applied.

  The time series information storage unit 26 stores time series information. The time series information is calculated by the three-dimensional position in the real space of the landmark feature point included in each of the images at each time specified by the three-dimensional position specifying unit 20 and the relative position and orientation calculation unit 24. It is the information showing the relative position and attitude | position with respect to the reference point of the vehicle 60 of each time. Specifically, each time the time t is updated, the three-dimensional position of the landmark feature point in the real space and the relative position and orientation of the vehicle 60 with respect to the reference point are accumulated in time series. Note that the time-series information accumulation unit 26 accumulates the two-dimensional positions of the landmarks included in each of the images at each time each time the time t is updated. In addition, when a new landmark feature point whose position is specified by the three-dimensional position specifying unit 20 is obtained, the time-series information storage unit 26 3 of the feature point of the new landmark in the real space. Accumulate dimension positions.

  The state estimation unit 28 is an example of an estimation unit. The state estimation unit 28 estimates the three-dimensional position and orientation of the vehicle 60 in the real space based on the time series information accumulated in the time series information accumulation unit 26. In the present embodiment, the following position / orientation estimation method is applied.

  The relational expression (observation equation) of the two-dimensional position of the landmark feature point in the image, the position of the vehicle 60, the attitude of the vehicle 60, and the three-dimensional position of the landmark feature point in the real space is expressed by the following formula (1 ).

(1)

Here, X ′ _{t, m} represents the two-dimensional position of the landmark feature point in the image. K represents a calibration matrix of the camera 40. R _t represents a three-dimensional rotation matrix having the posture (pitch, yaw, roll) of the vehicle 60 as an element. x _t represents a three-dimensional position of the vehicle 60 in the real space. _Xm represents the three-dimensional position of the landmark feature point in the real space. I indicates a unit matrix. The subscript t indicates a time index, and the subscript m indicates a landmark index. Moreover, _xt and _Rt are unknown numbers, and others are known. Furthermore, unknowns _{x t} and _{R t,} the following equation (2), can be expressed as (3).

(2)
(3)

Here, Δx _t indicates the relative position of the vehicle 60 from the reference point in the relative coordinate system. x ₀ indicates the position of the vehicle 60 at the relative coordinate system reference point in the absolute coordinate system (the coordinate system of the real space). ΔR _t indicates the relative posture of the vehicle 60 from the reference point in the relative coordinate system. R ₀ indicates the attitude of the vehicle 60 at the relative coordinate system reference point in the absolute coordinate system.

  The result of substituting the above equations (2) and (3) into the above equation (1) is shown in equation (4).

(4)

Then, the unknowns _{x 0} and _{R 0} in the formula (4), solved using the least squares method. Equation (5) shows the cost function. Since the above equation (2) is non-linear, it can be solved by using a convergence operation such as the Levenberg-Marquardt method.

(5)

  Here, M indicates the number of landmarks used for estimation, and N indicates the number of time series information.

After estimating the _{x 0} and _{R 0} to the equation (5) to a minimum, in the above formula (2), by substituting _{x 0,} the above equation (3), by substituting _{R 0,} in the latest time The three-dimensional position x _t and the posture R _t in the real space of the vehicle 60 can be obtained.

  FIG. 3 is a diagram for explaining an example of the relationship between the vehicle 60 and the plurality of landmarks LM1 to LM10 according to the present embodiment.

  As shown in FIG. 3, a plurality of landmarks are photographed by a camera 40 mounted in front of the vehicle 60 while the vehicle 60 moves in the traveling direction indicated by the arrow. Here, ten landmarks LM1 to LM10 are illustrated, but the number of landmarks is not limited to this.

At time T1, an image including four landmarks LM1 to LM4 is acquired. At time T1, as described above, the feature points extracted from the image are collated with the feature points of the plurality of collation landmarks stored in the map DB 16, and the four landmarks LM1 included in the image are collated. A three-dimensional position (X _m ) of each feature point of LM4 in the real space is specified.

At time T2, an image including four landmarks LM5 to LM8 is acquired. At time T2, as described above, the feature points extracted from the image are checked against the feature points of the plurality of matching landmarks stored in the map DB 16, and the four landmarks LM5 included in the image are checked. A three-dimensional position (X _m ) in the real space of each feature point of LM8 is specified.

At time T3, an image including four landmarks LM7 to LM10 is acquired. At time T3, as described above, the feature points extracted from the image are collated with the respective feature points of the plurality of collation landmarks stored in the map DB 16, and the four landmarks LM7 included in the image are collated. A three-dimensional position (X _m ) in the real space of each feature point of LM10 is specified. In the present embodiment, all landmarks included in the time period from time T1 to T3 are subject to collation.

  According to the present embodiment, the relative position and orientation with respect to the reference point of the vehicle 60 at each time are calculated, and all the landmarks included in the image photographed by the camera 40 at each time are used as collation targets. The position and orientation of the vehicle 60 are estimated. As a result, even when a single camera with a narrow angle of view is used, the number of landmarks used for estimation increases and the distribution range increases, as in the case of using an omnidirectional camera. The posture can be estimated with high accuracy.

  Next, the operation of the position / orientation estimation apparatus 10A according to the first embodiment will be described with reference to FIG. FIG. 4 is a flowchart showing an example of the processing flow of the position / orientation estimation processing program according to the first embodiment.

  First, in step 100 of FIG. 4, the image information acquisition unit 12 acquires an image of the periphery of the vehicle 60 captured at each time by the camera 40 mounted on the vehicle 60.

  In step 102, the feature point extraction unit 14 extracts feature points from the images at each time acquired by the image information acquisition unit 12.

  In step 104, the three-dimensional position specifying unit 20 specifies the two-dimensional position in the image of the feature point of the image at each time extracted by the feature point extracting unit 14. Then, the three-dimensional position specifying unit 20 collates the feature points of the images at each time extracted by the feature point extraction unit 14 with the feature points of the plurality of matching landmarks acquired from the map DB 16. Thus, the three-dimensional position in the real space of the landmark feature point included in the image at each time is specified.

  In step 106, the relative position and orientation calculation unit 24 calculates the relative position and orientation of the vehicle 60 with respect to the reference point for each time based on the sensor information acquired by the sensor information acquisition unit 22.

  In step 108, the time-series information storage unit 26 specifies the two-dimensional position in the image of the feature point of the image at each time specified by the three-dimensional position specifying unit 20 and the feature of the landmark included in the image at each time. Time series information representing the three-dimensional position of the point in the real space and the relative position and orientation of the vehicle 60 relative to the reference point at each time calculated by the relative position and orientation calculation unit 24 is accumulated.

  In step 110, the state estimation unit 28 uses the above-described equations (1) to (5) based on the time-series information accumulated by the time-series information accumulation unit 26 to perform a three-dimensional operation in the real space of the vehicle 60. Estimate position and orientation.

[Second Embodiment]
FIG. 5 is a block diagram showing an example of the configuration of the position / orientation estimation apparatus 10B according to the second embodiment.
As shown in FIG. 5, the position / orientation estimation apparatus 10B according to the present embodiment includes a time-series information selection unit 30 between the time-series information storage unit 26 and the state estimation unit 28 in the first embodiment. . Since the configuration other than this is the same as the configuration of the first embodiment, the same parts are denoted by the same reference numerals, and the repeated description is omitted.

  The time series information selection unit 30 is an example of a selection unit. The time series information selection unit 30 is based on the number of landmarks included in the image at each time acquired by the image information acquisition unit 12 from the time series information stored in the time series information storage unit 26. A range of time-series information to be provided to 28 is selected. The number of landmarks here is, for example, the total number of landmarks. Note that the number of types of landmarks may be used as the number of landmarks. In the case of the example shown in FIG. 3, the total number of landmarks is 12 (4 landmarks / images × 3 images) and the number of types of landmarks is 10 between times T1 and T3. The range of time series information is a range from the present time to a certain past time.

  In general, the relative position and orientation of a moving object calculated using a sensor increases the measurement error of the sensor as the distance traveled to the current position of the moving object increases (that is, the time increases). It tends to go. For this reason, shortening the range of the time-series information is less susceptible to the measurement error of the sensor. On the other hand, the longer the distance traveled to the current point of the moving body, the greater the number of landmarks used for estimation. For this reason, the accuracy of estimation improves when the range of the time-series information is lengthened. Therefore, in the present embodiment, an appropriate time-series information range is selected in consideration of the distance range based on the current location of the vehicle 60 and the number of landmarks used for estimation. For example, it is estimated that the time series information range corresponding to the minimum distance range among the distance ranges in which the number of landmarks is a certain number or more, or that the measurement error of the sensor 42 falls within a certain value. A method of selecting a time-series information range corresponding to a distance range in which the number of landmarks is the largest in the distance range can be considered.

  According to the present embodiment, it is difficult to be affected by the measurement error of the sensor 42, and an optimal time-series information range can be selected so that a certain number of landmarks or more can be obtained. Can be estimated with higher accuracy.

  As described above, the position / orientation estimation apparatus has been described as an embodiment. The embodiment may be in the form of a program for causing a computer to function as each unit included in the position and orientation estimation apparatus. The embodiment may be in the form of a computer-readable storage medium storing this program.

  In addition, the configuration of the position / orientation estimation apparatus described in the above embodiment is merely an example, and may be changed according to the situation without departing from the spirit of the present invention.

  Further, the processing flow of the program described in the above embodiment is an example, and unnecessary steps may be deleted, new steps may be added, or the processing order may be changed within a range not departing from the gist. Good.

  Moreover, although the said embodiment demonstrated the case where the process which concerns on embodiment was implement | achieved by a software structure using a computer by running a program, it is not restricted to this. The embodiment may be realized by, for example, a hardware configuration or a combination of a hardware configuration and a software configuration.

10A, 10B position and orientation estimation device 12 image information acquisition unit 14 feature point extraction unit 16 map DB
18 Map information acquisition unit 20 3D position specifying unit 22 Sensor information acquisition unit 24 Relative position and orientation calculation unit 26 Time series information storage unit 28 State estimation unit 30 Time series information selection unit 40 Camera 42 Sensor 50 CPU
52 Internal memory 60 Vehicle

Claims (5)

An image information acquisition unit that acquires image information at each time when the periphery of the moving body is imaged in time series by an imaging device mounted on the moving body;
An extraction unit for extracting feature points from each piece of image information at each time acquired by the image information acquisition unit;
A map database that stores a three-dimensional position of each of the plurality of matching landmarks obtained in advance in real space and a feature point of each of the plurality of matching landmarks;
By comparing the feature points of the image information at each time extracted by the extraction unit with the feature points of the plurality of reference landmarks acquired from the map database, each of the image information at each time A specifying unit for specifying a three-dimensional position in the real space of the landmark feature points included in
A calculation unit that calculates a relative position and orientation of the moving body with respect to a reference point for each time;
The three-dimensional position in the real space of the landmark feature point included in each of the image information at each time specified by the specifying unit and the relative point with respect to the reference point of the moving object at each time calculated by the calculating unit An estimation unit that estimates a three-dimensional position and orientation of the mobile body in real space based on time-series information representing the position and orientation;
A position and orientation estimation apparatus. A sensor information acquisition unit for acquiring sensor information including the speed and angular velocity at each time detected by the sensor mounted on the moving body;
The position and orientation estimation apparatus according to claim 1, wherein the calculation unit calculates a relative position and orientation with respect to a reference point of the moving body for each time based on the sensor information acquired by the sensor information acquisition unit.   A selection unit that selects a range of time-series information to be provided to the estimation unit based on the number of landmarks included in the image information at each time acquired by the image information acquisition unit from the time-series information. The position and orientation estimation apparatus according to claim 1 or 2.   The position and orientation estimation apparatus according to claim 3, wherein the number of landmarks is the total number of landmarks or the number of types of landmarks. Computer
An image information acquisition unit that acquires image information at each time when the periphery of the moving body is imaged in time series by an imaging device mounted on the moving body;
An extraction unit for extracting feature points from each piece of image information at each time acquired by the image information acquisition unit;
The feature point of the image information at each time extracted by the extraction unit, the three-dimensional position of each of the plurality of verification landmarks obtained in advance in the real space, and the feature point of each of the plurality of verification landmarks 3 in the real space of the landmark feature points included in each of the image information at each time by collating with the feature points of each of the plurality of landmarks for collation acquired from the map database stored in association with each other. A specific part for specifying the dimension position,
A calculation unit that calculates a relative position and orientation of the mobile body with respect to a reference point for each time; and
The three-dimensional position in the real space of the landmark feature point included in each of the image information at each time specified by the specifying unit and the relative point with respect to the reference point of the moving object at each time calculated by the calculating unit An estimation unit that estimates a three-dimensional position and orientation of the moving object in real space based on time-series information representing the position and orientation;
Program to function as.