JP7479200B2 - Position and orientation estimation device and program - Google Patents

Description

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

Detecting the position and attitude of each vehicle under various traffic conditions in which multiple vehicles are traveling is important for improving the driving stability of each vehicle. For example, a position and attitude determination device that can determine the position and attitude of an object with high accuracy is known (for example, see Patent Document 1). In this technology, the measurement module has a shape sensor that acquires shape data representing the outer shape of the target object, a satellite signal receiver, an attitude sensor, a processing unit, and a data storage device. In addition, the processing unit compares the shape data of the target object acquired by the shape sensor with a three-dimensional shape database of the target object pre-stored in the data storage device through a matching process, acquires the relative position and attitude of the target object from the shape sensor, and calculates the position and attitude of the target object in the absolute coordinate system based on this relative position and attitude and its own position and attitude in the absolute coordinate system acquired by the satellite signal receiver and the attitude sensor.

JP 2017-227463 A

However, in the technology described in Patent Document 1, the position and attitude determination device calculates its own position and attitude using satellite signals from the Global Positioning System (GPS). However, in environments where GPS signals cannot be received and where positioning accuracy is degraded due to multipath or the like, errors in the estimated position and attitude of a moving body such as a vehicle increase. Therefore, there is room for improvement in determining the position and attitude of a moving body such as a vehicle with high accuracy.

This disclosure has been made in consideration of the above-mentioned facts, and aims to provide a position and attitude estimation device and program that can estimate the position and attitude of a moving object with high accuracy without using satellite signals.

A first aspect of the present disclosure includes, in an environment where a plurality of moving bodies are present, a moving body information acquisition unit that acquires, for each of the plurality of moving bodies, moving body information including the type of the moving body; an image information acquisition unit that acquires, for each of the plurality of moving bodies, image information indicating a captured image of the surroundings captured by an image capture device mounted on the moving body; a detection unit that, for each of the plurality of moving bodies, identifies the moving body that captured the captured image as a first moving body based on the image information and detects a second moving body captured by the first moving body; a discrimination unit that discriminates the type of the second moving body detected by the detection unit based on the image information; and a discrimination unit that discriminates the type of the second moving body detected by the detection unit based on the position of an image indicating the second moving body on the captured image. The position and orientation estimation device includes a relative orientation identification unit that identifies a relative orientation indicating a direction from a first moving body to a second moving body, a storage unit that accumulates information from the moving body information acquisition unit, the relative orientation identification unit, and the discrimination unit, a matching unit that matches the moving body information of a type corresponding to the type of the second moving body detected by the detection unit based on the information accumulated in the storage unit, and a relative position and orientation estimation unit that estimates the relative positions and orientations between the multiple moving bodies based on the relative orientation from the first moving body to the second moving body among the multiple moving bodies and the moving body information associated by the matching unit.

A second aspect of the present disclosure includes, in an environment where a plurality of moving bodies are present, a moving body information acquisition unit that acquires, for each of the plurality of moving bodies, moving body information including the type of the moving body, an image information acquisition unit that acquires, for each of the plurality of moving bodies, image information indicating a captured image of the surroundings captured by an image capture device mounted on the moving body, a model information acquisition unit that acquires model information indicating the shape of the moving body, a detection unit that, for each of the plurality of moving bodies, identifies the moving body that captured the captured image as a first moving body based on the image information and detects a second moving body captured by the first moving body, a discrimination unit that discriminates the type of the second moving body detected by the detection unit based on the image information, and a position of an image indicating the second moving body on the captured image and the model information. The position and orientation estimation device includes a relative position and orientation specification unit that specifies the relative position and orientation of the second moving body with respect to the first moving body based on model information acquired by an information acquisition unit; a storage unit that stores information from the moving body information acquisition unit, the relative position and orientation specification unit, and the discrimination unit; a matching unit that matches the moving body information of a type corresponding to the type of the second moving body detected by the detection unit based on the information stored in the storage unit; and a relative position and orientation estimation unit that estimates the relative positions and orientations between the multiple moving bodies based on the relative position and orientation of the second moving body with respect to the first moving body among the multiple moving bodies and the moving body information associated by the matching unit.

A third aspect of the present disclosure is a method for detecting a moving object, comprising: a moving object information acquisition unit that acquires, in an environment where a plurality of moving objects are present, moving object information including a type of the moving object for each of the plurality of moving objects; an image information acquisition unit that acquires, for each of the plurality of moving objects, image information indicating an image captured by an image capture device mounted on the moving object; a model information acquisition unit that acquires model information indicating the shape of the moving object; an observation device information acquisition unit that acquires observation device information including a position and an attitude of an observation device that captures the environment; an observation image information acquisition unit that acquires observation image information indicating the observation image captured by the observation device; a model information acquisition unit that acquires model information indicating the shape of the moving object; a detection unit that detects, for each of the observation device and the plurality of moving objects, a moving object captured by the observation device based on the image information; a discrimination unit that discriminates the type of the moved object detected by the detection unit based on the information; a relative position and orientation specification unit that specifies the relative position and orientation of the moved object with respect to the observed object based on the position of an image showing the moved object on the captured image and the model information acquired by the model information acquisition unit; a storage unit that stores information on the moving object information acquisition unit, the relative position and orientation specification unit, and the discrimination unit; a matching unit that matches the moving object information of a type corresponding to the type of the moved object with the moved object detected by the detection unit based on the information stored in the storage unit; and a relative position and orientation estimation unit that estimates the relative positions and orientations between the multiple moving objects based on the relative positions and orientations of the moved object with respect to the observed object among the multiple moving objects and the moving object information associated by the matching unit.

A fourth aspect of the present disclosure provides a computer in an environment where a plurality of moving bodies are present, comprising: a moving body information acquisition unit that acquires, for each of the plurality of moving bodies, moving body information including a type of the moving body; an image information acquisition unit that acquires, for each of the plurality of moving bodies, image information indicating a captured image of the surroundings captured by an imaging device mounted on the moving body; a detection unit that, for each of the plurality of moving bodies, identifies a moving body that captured an image as a first moving body based on the image information and detects a second moving body captured by the first moving body based on the image information; a discrimination unit that discriminates the type of the second moving body detected by the detection unit based on the image information; a relative orientation identification unit that identifies a relative orientation indicating a direction from the first moving body to the second moving body based on a position of an image indicating the second moving body on the captured image; a storage unit that accumulates information on the moving body information acquisition unit, the relative orientation identification unit, and the discrimination unit; and an association unit that associates the moving body information of a type corresponding to the type of the second moving body with the second moving body detected by the detection unit based on the information accumulated in the accumulation unit.
and a relative position and attitude estimation unit that estimates the relative position and attitude between the multiple moving bodies based on the relative orientation from the first moving body to the second moving body among the multiple moving bodies and the moving body information that has been associated by the association unit.

As described above, the present invention makes it possible to estimate the position and attitude of a moving object with high accuracy without using satellite signals.

1 is a block diagram showing an example of a configuration of a position and orientation estimation apparatus according to a first embodiment. FIG. 2 is a block diagram showing an example of the configuration of a computer that functions as the position and orientation estimation apparatus according to the first embodiment. FIG. 2 is a diagram for explaining an example of a traffic environment in which a plurality of vehicles are traveling according to the first embodiment. 10 is a flowchart showing an example of a processing flow of a position and orientation estimation processing program according to the first embodiment. FIG. 11 is a block diagram showing an example of the configuration of a position and orientation estimation apparatus according to a second embodiment. FIG. 11 is a block diagram showing an example of the configuration of a computer that functions as a position and orientation estimation apparatus according to the second embodiment. FIG. 13 is a block diagram showing an example of the configuration of a position and orientation estimation apparatus according to a third embodiment. FIG. 13 is a block diagram showing an example of the configuration of a computer functioning as a position and orientation estimation apparatus according to the third embodiment. FIG. 13 is a diagram for explaining an example of a traffic environment in which a plurality of vehicles are traveling according to the third embodiment. 13 is a flowchart illustrating an example of a process flow of a position and attitude estimation processing program according to the third vehicle area embodiment.

Hereinafter, embodiments for implementing the technology of the present disclosure will be described in detail with reference to the drawings.
In this embodiment, a case will be described in which a vehicle such as an automobile is taken as an example of a moving body.
In this embodiment, components and processes that have the same action and function are given the same reference numerals throughout the drawings, and duplicated explanations may be omitted as appropriate. Each drawing is merely a schematic illustration to the extent that the technology of the present disclosure can be fully understood. Therefore, the technology of the present disclosure is not limited to only the illustrated examples. In addition, in this embodiment, explanations of configurations that are not directly related to the present invention or well-known configurations may be omitted.

[First embodiment]
FIG. 1 is a block diagram showing an example of the configuration of a position and orientation estimation device 10A according to the first embodiment.
As shown in FIG. 1, a position and attitude estimation device 10A according to this embodiment includes a vehicle information acquisition unit 12, an image information acquisition unit 14, a vehicle detection unit 16, a relative orientation estimation unit 18, a vehicle type discrimination unit 20, an information sharing unit 22, a matching unit 24, and a relative position and attitude overall optimization unit 26.

Figure 2 is a block diagram showing an example of the configuration of a computer that functions as the position and orientation estimation device 10A according to the first embodiment.

As shown in FIG. 2, the computer functioning as the position and orientation estimation device 10A includes a general-purpose computer main unit 30 equipped with a CPU (Central Processing Unit) 31, a RAM (Random Access Memory) 32, and a ROM (Read Only Memory) 33. A position and orientation estimation processing program 33P is stored in the ROM 33. The computer main unit 30 includes an input/output interface (I/O) 34, and the CPU 31, RAM 32, ROM 33, and I/O 34 are connected via a bus 35 so that commands and data can be exchanged. A communication unit 36 is also connected to the I/O 34, which communicates with an external device such as a vehicle 40, which is an example of a moving body, for example by wireless communication.

The position and orientation estimation processing program 33P may be pre-installed in the position and orientation estimation device 10A, for example. The position and orientation estimation processing program 33P may also be realized by storing it in a non-volatile storage medium or distributing it via a network and installing it appropriately in the position and orientation estimation device 10A. Examples of non-volatile storage media include a CD-ROM (Compact Disc Read Only Memory), a magneto-optical disk, a HDD (Hard Disk Drive), a DVD-ROM (Digital Versatile Disc Read Only Memory), a flash memory, a memory card, etc.

The computer main body 30 operates as the position and attitude estimation device 10A shown in FIG. 1 by reading the position and attitude estimation processing program 33P from the ROM 33 and expanding it in the RAM 32, and executing the position and attitude estimation processing program 33P expanded in the RAM 32 by the CPU 31. The position and attitude estimation processing program 33P includes processes for realizing each part of the position and attitude estimation device 10A shown in FIG. 1. That is, the CPU 30 functions as the vehicle information acquisition unit 12, the image information acquisition unit 14, the vehicle detection unit 16, the relative orientation estimation unit 18, the vehicle type discrimination unit 20, the information sharing unit 22, the association unit 24, and the relative position and attitude overall optimization unit 26 shown in FIG. 1.

The position and attitude estimation device 10A acquires various types of information from a vehicle 40, which is an example of a moving body.
In this embodiment, a case will be described as an example in which the position and orientation estimation device 10A communicates with an external vehicle 40 to exchange information. However, the technology disclosed herein is not limited to the position and orientation estimation device 10A being installed outside the vehicle 40. For example, the position and orientation estimation device 10A may be mounted on the vehicle 40 (i.e., installed inside the vehicle 40).

The vehicle 40 is equipped with a camera 42, a memory 44, and a communication unit 46. The communication unit 46 is connected to the camera 42 and the memory 44. The camera 42 is an example of an imaging device, and captures images of the periphery of the vehicle 40. There may be one camera 42 or multiple cameras. The imaging range of the camera 42 may face in any direction from the vehicle 40, and may be at least one of the traveling direction (forward), sideways, backward, and all directions of the vehicle 40. The angle of view and resolution of the camera 42 may be set in advance, or may be set to any input setting value. The image information captured by the camera 42 can be stored in the memory 44.

Memory 44 stores vehicle information including the vehicle model. The vehicle information can include information such as the vehicle's color, model, and license plate. The vehicle information may also include angular velocities such as steering angle, vehicle speed, yaw rate, pitch angular velocity, and roll angular velocity, which are obtained by a sensor (not shown) mounted on vehicle 40.

The communication unit 46 communicates image information showing images captured by the camera 42 and vehicle information stored in the memory 44 with the position and orientation estimation device 10A and external devices such as other vehicles, for example via wireless communication.

In this embodiment, a case will be described in which the position and attitude estimation device 10A acquires information from each of the multiple vehicles 40 in a traffic environment in which multiple vehicles 40 are traveling or present (see also FIG. 3).

Note that the position and attitude estimation device 10A has a communication function that allows it to communicate with vehicles 40 within a predetermined range, making it possible to acquire information from each of the multiple vehicles 40 that exist within the predetermined range. This allows the position and attitude estimation device 10A to estimate the relative position and attitude of each of the multiple vehicles 40 that exist within the predetermined range, as described below.

FIG. 3 is an image diagram showing an example of a traffic environment in which a plurality of vehicles 40 are traveling.
3, there are five vehicles 40A, 40B, 40C, 40D, and 40E as the multiple vehicles 40. Each of the vehicles 40A to 40E is equipped with a camera 42, a memory 44, and a communication unit 46. When it is necessary to distinguish between the multiple vehicles 40, they will be described using one of the symbols 40A to 40E, but when it is not necessary to distinguish between them, they will be described simply as vehicle 40.

The vehicle information acquisition unit 12 of the position and attitude estimation device 10A is an example of a moving object information acquisition unit, and the image information acquisition unit 14 is an example of an image information acquisition unit. The vehicle information acquisition unit 12 acquires each of the vehicle information stored in the memory 44 of each of the five vehicles 40A-40E. The image information acquisition unit 14 acquires image information of the surroundings of the vehicle 40 captured by the camera 42 of each of the five vehicles 40A-40E. Note that the vehicle information acquired by the vehicle information acquisition unit 12 and the image information acquired by the image information acquisition unit 14 are assumed to be associated with each other. In other words, the image information is acquired from the vehicle 40 of the acquired vehicle information.

The vehicle detection unit 16 is an example of a detection unit. The vehicle detection unit 16 detects the other vehicle 40 captured by the camera 42 from the captured image using the image information acquired by the image information acquisition unit 14. For example, the detection of the other vehicle 40 may be performed by detecting an image area of a predetermined shape corresponding to an image of the vehicle 40 in the captured image as a vehicle area indicating the other vehicle 40. The detection result of the vehicle detection unit 16 is output to the relative orientation estimation unit 18 and the vehicle type discrimination unit 20. Information indicating the detection result of the vehicle detection unit 16 is output as other vehicle image information indicating the other vehicle image, which is image information indicating the area within the contour line of the detected other vehicle 40. Various methods have been proposed as a method for detecting the vehicle area. For example, a vehicle area may be detected using a neural network that has been trained in advance, or other methods may be used.

The relative orientation estimation unit 18 is an example of a relative orientation identification unit. The relative orientation estimation unit 18 estimates the relative orientation of the other vehicles 40 detected by the vehicle detection unit 16 relative to the vehicle (first moving body) based on the vehicle position of the other vehicles 40 in the captured image using image information acquired for each of the multiple vehicles 40. Specifically, for example, if the horizontal angle of view of the captured image is θ, the number of pixels is N, and the X coordinate of the center of gravity of the other vehicle image showing the other vehicle 40 (second moving body) detected by the vehicle detection unit 16 is shifted by a distance Δx from the center of the image in the captured image, the relative orientation from the captured vehicle can be expressed as Δx×θ/N. The center of gravity of the other vehicle image is an example of a feature point that two-dimensionally indicates the orientation of the other vehicle captured by the vehicle. Also, the center of the image in the captured image is an example of a reference point that two-dimensionally indicates the orientation of the vehicle 40 (the vehicle) that captured the surroundings.

The vehicle type discrimination unit 20 is an example of a discrimination unit. The vehicle type discrimination unit 20 discriminates the vehicle type of each of one or more other vehicles detected by the vehicle detection unit 16 using image information acquired for each of the multiple vehicles 40. The vehicle type can be discriminated by using a discrimination method that identifies the vehicle type corresponding to the area image within the contour line of the detected other vehicle 40. Various methods have been proposed as a vehicle type discrimination method. For example, a pre-trained neural network or other discrimination method may be used as a vehicle type discrimination method. The vehicle type discrimination unit 20 only needs to discriminate at least the vehicle type of the other vehicles 40 in the captured image. In addition to the vehicle type, detailed information that can be individually identified, such as the color, model, and vehicle number of the vehicle 40 based on the license plate, may also be included.

The information sharing unit 22 is an example of a storage unit. The information sharing unit 22 stores the vehicle information acquired by the vehicle information acquisition unit 12 for each of the multiple vehicles 40, the vehicle type information determined by the vehicle type determination unit 20, and information indicating the relative direction estimated by the relative direction estimation unit 18 in association with each of the multiple vehicles 40. That is, the information sharing unit 22 uses the acquired information for each of the multiple vehicles 40 to store the vehicle information of the vehicle (own vehicle) that captured the captured image, the vehicle type information of the vehicle (other vehicle) included in the captured image, and information indicating the relative direction from the own vehicle to the other vehicle. By registering the information of each of the multiple vehicles 40 in, for example, one database, information on the multiple vehicles 40 is aggregated in the database. The vehicle information, vehicle type information, and information indicating the relative direction for each of the multiple vehicles 40 may be stored in the RAM 32, or may be stored in an auxiliary storage device (not shown). This information is not limited to being stored in the RAM 32 and the auxiliary storage device in the position and attitude estimation device 10A. For example, information may be aggregated by transmitting the information to an external device such as an external server via communication and storing the information therein, or by transmitting the information to one of a plurality of vehicles 40 and storing the information therein.

The matching unit 24 is an example of a matching unit. The matching unit 24 performs matching to identify other vehicles indicated by other vehicle images included in the captured images of each vehicle 40. For example, in a traffic environment (FIG. 3) in which vehicles 40A to 40E exist, when two other vehicles are captured in an image captured by vehicle 40A, information indicating the vehicle type of the other vehicles by the vehicle type discrimination unit 20 for vehicle 40A is compared with information indicating the vehicle type by the vehicle information acquisition unit 12 acquired from the multiple vehicles 40. Then, the information held by the vehicle 40 corresponding to the other vehicles is matched to the other vehicles present in the captured image by combining information indicating matching vehicle types. As a result, the two other vehicles captured by vehicle 40A can be identified as, for example, "vehicle 40B and vehicle 40C," and the information held by the other vehicles can be matched to the other vehicles present in the image captured by the vehicle itself.

If there are multiple combinations of vehicle model information, the matching results can be narrowed down by adding at least one piece of information such as color, model, and vehicle number from the license plate.

The relative position and attitude overall optimization unit 26 is an example of a relative position and attitude estimation unit. The relative position and attitude overall optimization unit 26 estimates the relative position and attitude between each of the multiple vehicles 40A to 40E. This relative position and attitude overall optimization unit 26 calculates the relative positions and attitudes of the multiple vehicles 40A to 40E so that the relative orientation from the subject vehicle to the other vehicles estimated for each vehicle by the relative orientation estimation unit 18, and the information associated with the other vehicles in the captured images estimated by the association unit 24, are consistent as much as possible without inconsistencies. In this embodiment, the position and attitude estimation method shown below is applied.

The relational equation (observation equation) between the relative positions and attitude angles of each of the multiple vehicles 40 in the horizontal plane is expressed by the following equation (1). The function f(X) expressed by equation (1) is a function that calculates θ=arctan(y/x) from the two-dimensional vector X=(x,y) and outputs θ.

... (1)

Here, X indicates the position of the vehicle 40 (two-dimensional position in a horizontal plane), and θ indicates the attitude angle of the vehicle 40. i indicates the vehicle 40 (host vehicle) that captured the captured image, and j indicates the vehicle 40 (other vehicle) included in the captured image. _{Δθ in,jn} indicates the relative direction of the host vehicle in to the other vehicle jn estimated from the captured image. The subscript n indicates the number of observations.

Specifically, in the example shown in FIG. 3, the positions (two-dimensional positions in a horizontal plane) and attitude angles of the vehicles 40A to 40E are X ₁ to X ₅ and θ ₁ to θ ₅ , respectively. Here, the origin of the coordinate system in the horizontal plane is set to the position X ₁ of the camera 42 mounted on the vehicle 40A, and the orientation θ ₁ of the camera 42 is set to coincide with the coordinate axis. That is, the position and attitude angle of the vehicle 40A are expressed as [X ₁ , θ ₁ ] = [0, 0]. Similarly, the positions and attitude angles of the vehicles 40B to 40E are expressed as [X ₂ , θ ₂ ] for the vehicle 40B, [X ₃ , θ ₃ ] for the vehicle 40C, [X ₄ , θ ₄ ] for the vehicle 40D, and [X ₅ , θ ₅ ] for the vehicle 40E. In addition, the relative orientation of the vehicle in to the other vehicle jn estimated from the captured image is Δθ _in,jn . It is assumed here that the orientation of the cameras 42 mounted on the vehicles 40B to 40E is the same as that of the vehicle 40A.

The association information associated by the association unit 24 is represented as (in, jn). (in, jn) stores information indicating that the other vehicle shown in the image captured by the host vehicle i is j, the number of pieces being the number of observed vehicles (n items). It is assumed here that the association unit 24 has performed the following 13 associations.
(in,jn) = ([1,2],[1,3],[3,2],[2,4],[4,1],[4,2],[3,4],[4,3],[2,5]
,[3,5],[5,2],[5,3],[5,4])

In the example shown in FIG. 3, when 13 correspondences are made, formula (1) is established. In formula (1), the unknowns are 12 (2×4+1×4=12), which are the positions X ₂ , X ₃ , X ₄ , and X ₅ of the vehicles 40B to 40E, and the attitude angles θ ₂ , θ ₃ , θ ₄ , and θ _5. On the other hand, the number of observation equations is 13 (the number of elements of (in, jn)). Therefore, the unknowns can be obtained by solving simultaneous equations using the result of the correspondences made by the correspondence unit 24. Therefore, the relative position and attitude overall optimization unit 26 can estimate the relative positions and attitudes of the multiple vehicles 40A to 40E using information indicating the result of the correspondences made by the correspondence unit 24 and formula (1). Note that, since the observation equations are redundant, a solution with good accuracy can be obtained by using a calculation method such as the least squares method.

In this way, the relative position and attitude overall optimization unit 26 estimates the relative positions and attitudes between, for example, five vehicles 40A-40E, so that the relative orientation from the own vehicle to the other vehicles estimated for each vehicle and the information associated with the other vehicles in each captured image are consistent without contradictions. The relative orientation from the own vehicle to the other vehicles estimated for each vehicle and the information associated with the other vehicles in each captured image are consistent without contradictions means that each of the five vehicles 40A-40E is in a geometrical relative position and attitude angle correspondence that exists at the estimated orientation and attitude angle based on the relationship between the own vehicle and each of the other vehicles captured in each image. This is also clear from the above formula (1).

According to this embodiment, the relative orientation of the other vehicles with respect to the vehicle itself and the vehicle type of the other vehicles are identified from the captured images of each of the multiple vehicles 40, and this information is associated with vehicle information including the vehicle type acquired from each of the multiple vehicles 40, and the relative positions and attitudes between the multiple vehicles 40 are estimated. As a result, even when it is difficult to use GPS satellite signals, the information that is lacking for identifying the position and attitude of each of the multiple vehicles 40 is compensated for, so that the relative positions and attitudes between the multiple vehicles 40 can be estimated with high accuracy.

Note that while the above describes the estimation of two-dimensional position and attitude angle in a horizontal plane, it is also possible to easily extend the calculation to three dimensions.

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

First, in step S100 of FIG. 4, the vehicle information acquisition unit 12 acquires vehicle information stored in the memory 44 installed in the vehicle 40.

In step S102, the image information acquisition unit 14 acquires images of the surroundings of the vehicle 40 captured by the camera 42 mounted on the vehicle 40 as image information.

In step S104, the vehicle detection unit 16 uses the image information acquired by the image information acquisition unit 14 to detect vehicles in the captured image, i.e., other vehicles photographed from the host vehicle.

In step S106, the relative orientation estimation unit 18 estimates the relative orientation of the vehicle to the other vehicle using the distance between the two-dimensional position (feature point) in the captured image of the vehicle 40 (other vehicle) detected by the vehicle detection unit 16 and the two-dimensional position (reference point) in the captured image of the vehicle 40 (the vehicle itself).

In step S108, the vehicle type discrimination unit 20 discriminates the vehicle type of the vehicle 40 (other vehicle) detected by the vehicle detection unit 16 using a discrimination method that identifies the vehicle type corresponding to the area image within the contour line of the vehicle 40 in the captured image. In this step S108, in addition to the vehicle type, detailed information that can individually identify the vehicle 40, such as the color, model, and vehicle number from the license plate, can be included.

In step S110, the information sharing unit 22 stores the vehicle information acquired by the vehicle information acquisition unit 12, the vehicle type information determined by the vehicle type determination unit 20, and information indicating the relative direction estimated by the relative direction estimation unit 18 in association with each of the multiple vehicles 40, for example by registering the information in a database, thereby sharing the information.

The above processing is carried out for each of the multiple vehicles 40A to 40E.

In step S112, the matching unit 24 matches the vehicle type of the other vehicle estimated in the captured image with vehicle information obtained from a vehicle other than the vehicle from which the captured image was obtained.

In step S114, the relative position and attitude overall optimization unit 26 optimizes the estimation of the relative positions and attitudes between each of the multiple vehicles 40A to 40E using the above formula (1) based on the relative orientation from the vehicle to the other vehicles estimated for each vehicle by the relative orientation estimation unit 18 and the information associated with the other vehicles in the captured image estimated by the association unit 24.

[Second embodiment]
FIG. 5 is a block diagram showing an example of the configuration of a position and orientation estimation device 10B according to the second embodiment.
5, a position and attitude estimation device 10B according to this embodiment includes a relative position and attitude estimation unit 17 instead of the relative orientation estimation unit 18 in the first embodiment. A vehicle model information acquisition unit 13 is connected to this relative position and attitude estimation unit 17. Since the rest of the configuration is the same as that of the first embodiment, the same reference numerals are used to designate the same parts, and detailed description thereof will be omitted.

The vehicle model information acquisition unit 13 is an example of a model information acquisition unit. The vehicle model information acquisition unit 13 acquires vehicle model information indicating shape information related to a known vehicle 40, for example, a 3D model (i.e., a vehicle model) represented by the surface shape of each vehicle model of the vehicle 40. The vehicle model information such as a 3D model may be stored in advance in the ROM 33, or, as shown in FIG. 6, an auxiliary storage device 37 in which vehicle model information is stored in advance may be provided and acquired from the auxiliary storage device 37. In addition, the vehicle model information may be acquired from the outside through communication using the communication unit 36, stored in the RAM 32 or the auxiliary storage device 37, and acquired from the RAM 32 or the auxiliary storage device 37.

The relative position and attitude estimation unit 17 is an example of a relative position and attitude specification unit. The relative position and attitude estimation unit 17 estimates the relative position and attitude of the vehicle 40 (other vehicle) in the captured image relative to the vehicle 40 (own vehicle) from which the captured image was obtained by matching the vehicle model information acquired by the vehicle model information acquisition unit 13 with an image in the vehicle area showing the vehicle 40 (other vehicle) detected by the vehicle detection unit 16 for the vehicle 40 (other vehicle) in the captured image. The relative position and attitude estimation unit 17 may use the vehicle model information determined by the vehicle model determination unit 20 to select a vehicle model of the relevant vehicle model from among various vehicle models included in the vehicle model information acquired by the vehicle model information acquisition unit 13.

Specifically, in general, when a vehicle model represented by a three-dimensional surface shape is projected onto a two-dimensional plane, the shape of the vehicle model on the two-dimensional plane is determined by the projection direction, the distance between the vehicle model and the two-dimensional plane, and the attitude of the vehicle model. Therefore, assuming that the vehicle model is projected onto the imaging plane of the camera 42, which is a two-dimensional plane, the shape of the vehicle model on the captured image captured by the camera 42 is determined by the relative position and attitude between the camera 42 and the vehicle model. Therefore, the position and attitude of the vehicle model relative to the host vehicle are changed little by little to search for a state (relative position and attitude) that is most consistent with the shape (e.g., contour) of the vehicle image area showing the vehicle 40 detected by the vehicle detection unit 16. The relative position and attitude of this most consistent state correspond to the relative position and attitude of the vehicle 40 (other vehicle) detected by the vehicle detection unit 16 with respect to the captured vehicle 40 (host vehicle).

According to this embodiment, the position and attitude of the vehicle 40 in the captured image can be estimated by changing the projected image of the vehicle model and matching it with an image showing the vehicle 40 in the captured image. This makes it possible to improve the estimation accuracy of the relative position and attitude of the vehicle 40 (other vehicle) captured by the camera 42 mounted on the vehicle 40 (own vehicle).

[Third embodiment]
7 is a block diagram showing an example of the configuration of a position and orientation estimation device 10C according to the third embodiment. The third embodiment estimates the relative positions and orientations of a plurality of vehicles 40 by further using information including captured images captured at predetermined fixed positions on the infrastructure side, such as buildings and roadside units.

As shown in FIG. 7, the position and attitude estimation device 10C according to this embodiment includes a vehicle model information acquisition unit 13-1 that functions similarly to the vehicle model information acquisition unit 13 of the position and attitude estimation device 10B according to the second embodiment as a functional unit that handles information from the vehicle 40. It also includes an image information acquisition unit 14-1 that functions similarly to the image information acquisition unit 14, a vehicle detection unit 16-1 that functions similarly to the vehicle detection unit 16, a relative position and attitude estimation unit 17-1 that functions similarly to the relative position and attitude estimation unit 17, and a vehicle type discrimination unit 20-1 that functions similarly to the vehicle type discrimination unit 20.

The position and orientation estimation device 10C includes an information sharing unit 23 that functions similarly to the information sharing unit 22 of the position and orientation estimation device 10B. It also includes a matching unit 25 that functions similarly to the matching unit 24, and a relative position and orientation overall optimization unit 27 that functions similarly to the relative position and orientation overall optimization unit 26.

The position and attitude estimation device 10C further includes a vehicle model information acquisition unit 13-2 that functions similarly to the vehicle model information acquisition unit 13 as a functional unit that handles information from the infrastructure side. It also includes an image information acquisition unit 14-2 that functions similarly to the image information acquisition unit 14, a vehicle detection unit 16-2 that functions similarly to the vehicle detection unit 16, a relative position and attitude estimation unit 17-2 that functions similarly to the relative position and attitude estimation unit 17, and a vehicle type discrimination unit 20-2 that functions similarly to the vehicle type discrimination unit 20. The position and attitude estimation device 10C further includes a position and attitude acquisition unit 28.

FIG. 8 is a block diagram mainly showing an example of the configuration of a computer that functions as a position and orientation estimation device 10C according to the third embodiment.
8, in a computer functioning as a position and orientation estimation device 10C, a position and orientation estimation processing program 33PS according to the third embodiment is stored in a ROM 33. A communication unit 36 connected to an I/O 34 communicates with an external device such as a vehicle 40 and an observation device 50 provided at a predetermined fixed position on the infrastructure side, for example, by wireless communication.

The position and attitude estimation device 10C acquires various types of information from the vehicle 40 and the observation device 50.
In the present embodiment, a case will be described as an example in which the position and attitude estimation device 10A communicates with an external vehicle 40 and an observation device 50 to exchange information. However, the technology of the present disclosure is not limited to this. For example, the position and attitude estimation device 10A may be mounted on the observation device 50.

The observation device 50 includes a camera 52, a memory 54, and a communication unit 56. The communication unit 56 is connected to the camera 52 and the memory 54. The camera 52 is an example of an imaging device, and captures images of the surroundings of the observation device 50. There may be one camera 52, or there may be multiple cameras. The camera 52 is installed with a preset orientation, angle of view, and resolution. The orientation, angle of view, and resolution of the camera 52 may be installed with preset fixed values, or may be changed to any inputted setting values. The image information captured by the camera 52 can be stored in the memory 54.

Memory 54 stores observation device information indicating the position and attitude of camera 52 installed in infrastructure such as buildings and roadside units. Information indicating the position and attitude of camera 52 may be obtained by measurement using surveying equipment, or may be information obtained using RTK-GNSS (Real Time Kinematic - Global Navigation Satellite Systems), or may be obtained using other methods. In addition, the observation device information may include camera-specific information such as the angle of view and resolution of camera 52.

The communication unit 56 communicates image information indicating the image captured by the camera 52 and observation device information stored in the memory 54 with the position and orientation estimation device 10C and external devices such as other vehicles, for example via wireless communication.

FIG. 9 is an image diagram showing an example of a traffic environment in which a plurality of vehicles 40 according to this embodiment are traveling.
9, there are four vehicles 40A, 40B, 40C, and 40D as a plurality of vehicles 40. An observation device 50 is installed on the infrastructure side. Each of the vehicles 40A to 40D is equipped with a camera 42, a memory 44, and a communication unit 46.

The vehicle model information acquisition unit 13-1 and the vehicle model information acquisition unit 13-2 acquire vehicle model information in the same manner as the vehicle model information acquisition unit 13. Note that in this embodiment, the vehicle model information acquisition unit 13-1 and the vehicle model information acquisition unit 13-2 are configured as independent functional units that handle information from the vehicle 40 side and the infrastructure side (observation device 50), respectively, but this is not limited to this and they may be configured as a single shared unit.

The image information acquisition unit 14-1, vehicle detection unit 16-1, relative position and attitude estimation unit 17-1, and vehicle type discrimination unit 20-1 are similar to the image information acquisition unit 14, vehicle detection unit 16, relative position and attitude estimation unit 17, and vehicle type discrimination unit 20, so their explanations are omitted.

The image information acquisition unit 14-2 is an example of an observation image information acquisition unit. The image information acquisition unit 14-2 acquires image information of the surroundings of the observation device 50 captured by the camera 52 of the observation device 50.

The vehicle detection unit 16-2 detects the vehicle 40 photographed by the camera 52 of the observation device 50 using the image information captured by the camera 52 of the observation device 50 acquired by the image information acquisition unit 14-2.

The relative position and attitude estimation unit 17-2 estimates the relative position and attitude of the vehicle 40 detected by the vehicle detection unit 16 based on the vehicle position in the captured image using the vehicle model acquired by the vehicle model information acquisition unit 13-2 and image information from the observation device 50. Specifically, for the vehicle 40 in the captured image detected by the vehicle detection unit 16-2, the vehicle model information acquired by the vehicle model information acquisition unit 13-2 is matched with an image within the vehicle area showing the vehicle 40 detected by the vehicle detection unit 16-2 to estimate the relative position and attitude of the vehicle 40 in the captured image of the observation device information to the observation device 50.

The vehicle type discrimination unit 20-2 discriminates the vehicle type of each of one or more vehicles 40 detected by the vehicle detection unit 16 using image information from the observation device 50. In addition to the vehicle type, the vehicle type discrimination unit 20 may also include detailed information that allows individual identification of the vehicle 40, such as the color, model, and vehicle number from the license plate.

The position and orientation acquisition unit 28 is an example of an observation device information acquisition unit. The position and orientation acquisition unit 28 acquires observation device information indicating the positions and orientations of cameras installed in infrastructure such as buildings and roadside units from the observation device 50.

The information sharing unit 23 associates and stores the vehicle information acquired by the vehicle information acquisition unit 12 for each of the multiple vehicles 40, the vehicle type information identified by the vehicle type discrimination unit 20-1, and information indicating the relative position and attitude estimated by the relative position and attitude estimation unit 17-1 for each of the multiple vehicles 40. In addition, the information sharing unit 23 associates and stores, for a vehicle 40 captured in an image captured by the observation device 50, the vehicle type information identified by the vehicle type discrimination unit 20-2, information indicating the relative position and attitude estimated by the relative position and attitude estimation unit 17-2, and the observation device information acquired by the position and attitude acquisition unit 28. In other words, the information sharing unit 23 registers this information in, for example, a single database, and information regarding the multiple vehicles 40 is aggregated in that database.

The matching unit 25 performs matching to identify other vehicles indicated by other vehicle images included in the captured images of each vehicle 40, and vehicles 40 indicated by vehicle images included in the captured images of the observation device 50.

The relative position and attitude overall optimization unit 27 estimates the relative position and attitude between each of the multiple vehicles 40A to 40D. The relative position and attitude overall optimization unit 27 calculates the relative positions and attitudes of the multiple vehicles 40A to 40D so that the relative position and attitude from the subject vehicle (observation body) to the other vehicle (moved body) estimated by the relative position and attitude estimation unit 17-1 using the captured images of each vehicle, the relative position and attitude from the observation device 50 (observation body) to the vehicle 40 (moved body) estimated using the captured images by the observation device 50, and the information associated by the association unit 25 are consistent as much as possible without contradictions. In this embodiment, the position and attitude estimation method shown below is applied.

The relational equation (observation equation) between the relative positions and attitude angles of each of the multiple vehicles 40 in the horizontal plane is expressed by the following equation (2).

... (2)

Here, R(θ) is a two-dimensional rotation matrix. X indicates the position of the vehicle 40 (two-dimensional position in a horizontal plane), and θ indicates the attitude angle of the vehicle 40. i indicates the vehicle 40 (own vehicle) or the observation device 50 that captured the captured image, and j indicates the vehicle 40 included in the captured image. _{ΔX in,jn} and Δθ _in,jn indicate the relative position and attitude of the vehicle jn estimated from the captured image, with the own vehicle or the observation device 50 being in. The subscript n indicates the number of observations.

Specifically, in the example shown in Fig. 9, the positions (two-dimensional positions in a horizontal plane) and attitude angles of the observation device 50 and the multiple vehicles 40A-40D are _X1 - _X5 and _θ1 - _θ5 , respectively. Here, the origin of the coordinate system in the horizontal plane is set to the position _X1 of the camera 52 equipped on the observation device 50, and the orientation _θ1 of the camera 52 is set to coincide with the coordinate axes. In other words, the position and attitude angle of the observation device 50 are expressed as [ _X1 , _θ1 ] = [0, 0]. Similarly, the positions and attitude angles of the vehicles 40A-40D are expressed as [ _X2 , _θ2 ] for vehicle 40B, [ _X3 , _θ3 ] for vehicle 40C, [ _X4 , _θ4 ] for vehicle 40D, and [ _X5 , _θ5 ] for vehicle 40A. Also, the relative position of jn, which indicates a vehicle, estimated from the captured image of in, which indicates the vehicle itself or the observation device 50, is denoted as ΔX _in,jn , and the attitude is denoted as Δθ _in,jn . Note that it is assumed here that the orientations of the cameras 42 mounted on the vehicles 40A to 40D are the same.

The association information associated in the association unit 24 is assumed to be (in, jn). (in, jn) is information that vehicle j appears in an image captured by the camera i of the vehicle 40 or the observation device 50, and the information is stored in the number (n) of all observed vehicles. It should be noted that the association information in the association unit 25 is assumed to be as follows:
(in,jn) = ([1,2],[1,4],[4,2],[3,4],[4,3],[5,2],[5,3],[3,2])

In this embodiment, the distance between vehicles is also derived. For this reason, in the example shown in FIG. 9, the observation results from the subject vehicle to the other vehicle, or from the observation device 50 to the vehicle, are shown as a vector from the subject vehicle to the other vehicle, or from the observation device 50 to the vehicle.

In the example shown in FIG. 9, when 12 correspondences are made, formula (2) is established. In formula (2), the unknowns are the positions _X5 , _X2 , _X3 , and _X4 of the vehicles 40A to 40D, and the attitude angles _θ5 , _θ2 , _θ3 , and _θ4 , which are 12 (2×4+1×4=12). On the other hand, the number of observation equations is 18 (twice the number of elements of (in, jn)) due to the positions and attitude angles. Therefore, the unknowns can be obtained by solving simultaneous equations using the results of the correspondences made by the correspondence unit 25. Therefore, the relative position and attitude overall optimization unit 27 can estimate the relative positions and attitudes of the multiple vehicles 40A to 40D using information indicating the results of the correspondences made by the correspondence unit 25 and formula (2). Note that, since the observation equations are redundant, a solution with good accuracy can be obtained by using a calculation method such as the least squares method.

In this way, the relative position and attitude overall optimization unit 27 estimates the relative positions and attitudes between the four vehicles 40A-40D so that the relative positions and attitudes from the observation device to the vehicle, or from the host vehicle to the other vehicle, estimated for each observation device 50 and vehicle 40, are consistent with the information associated with the vehicles in each captured image. Consistent consistency of this information means that the observation device 50 and each of the four vehicles 40A-40D are in a geometrically consistent correspondence relationship of relative positions and attitude angles. This is also clear from the above formula (2).

According to this embodiment, by using images captured on the infrastructure side and observation device information, even when it is difficult to use GPS satellite signals, the information that is lacking for identifying the position and attitude of each of the multiple vehicles 40 is supplemented, so that the relative positions and attitudes between the multiple vehicles 40 can be estimated with even higher accuracy than in the first embodiment.

Note that while the above describes the estimation of two-dimensional position and attitude angle in a horizontal plane, it is also possible to easily extend the calculation to three dimensions.

Next, the operation of the position and orientation estimation device 10A according to the first embodiment will be described with reference to FIG. 10. Note that FIG. 10 is a flowchart showing an example of the processing flow of a position and orientation estimation processing program according to the third embodiment.

First, in step S300 in FIG. 10, the vehicle information acquisition unit 12 acquires vehicle information stored in the memory 44 mounted on the vehicle 40. In addition, the position and attitude acquisition unit 28 acquires observation device information stored in the memory 54 mounted on the observation device 50.

In step S302, the image information acquisition unit 14-1 acquires images of the surroundings of the vehicle 40 taken by the camera 42 mounted on the vehicle 40. In addition, the image information acquisition unit 14-2 acquires images of the surroundings of the observation device 50 taken by the camera 52 equipped on the observation device 50.

In step S304, the vehicle detection unit 16-1 uses the image information acquired by the image information acquisition unit 14-1 to detect vehicles in the captured image, i.e., other vehicles captured from the host vehicle. The vehicle detection unit 16-2 uses the image information from the observation device 50 acquired by the image information acquisition unit 14-2 to detect vehicles in the captured image, i.e., the vehicle 40 captured by the observation device 50.

In step S306, the relative position and attitude estimation unit 17-1 estimates the relative position and attitude of the other vehicle in the captured image relative to the host vehicle by matching the vehicle model information acquired by the vehicle model information acquisition unit 13-1 with the image in the vehicle area showing the vehicle 40 detected by the vehicle detection unit 16-1 for the vehicle 40 in the captured image detected by the vehicle detection unit 16-1. The relative position and attitude estimation unit 17-2 estimates the relative position and attitude of the vehicle 40 in the captured image relative to the observation device 50 by matching the vehicle model information acquired by the vehicle model information acquisition unit 13-2 with the image in the vehicle area showing the vehicle 40 detected by the vehicle detection unit 16-2 for the vehicle 40 in the captured image detected by the vehicle detection unit 16-2.

In step S308, the vehicle type discrimination unit 20-1 discriminates the vehicle type of the vehicle 40 (other vehicle) detected by the vehicle detection unit 16-1 from the image captured by the vehicle 40 (own vehicle). In addition, the vehicle type discrimination unit 20-2 discriminates the vehicle type of the vehicle 40 detected by the vehicle detection unit 16-2 from the image captured by the observation device 50.

In step S310, the information sharing unit 23 associates and stores the vehicle information acquired by the vehicle information acquisition unit 12, the observation device information acquired by the position and attitude acquisition unit 28, the vehicle type information determined by the vehicle type determination units 20-1 and 20-2, and the information indicating the relative orientation estimated by the relative position and attitude estimation units 17-1 and 17-2, for example by registering the information in a database, thereby sharing the information.

In step S312, the matching unit 25 matches the vehicle model estimated in the captured image with the vehicle information obtained directly from the vehicle.

In step S314, the relative position and attitude overall optimization unit 27 optimizes the estimation of the relative positions and attitudes between each of the multiple vehicles 40A to 40D using the above-mentioned formula (2) based on the relative positions and attitudes of the vehicles estimated by the relative position and attitude estimation units 17-1 and 17-2 and the information associated by the association unit 25.

[Other embodiments]
The position and orientation estimation apparatus has been described as an example of the embodiment. The embodiment may be in the form of a program for causing a computer to function as each unit of the position and orientation estimation apparatus. The embodiment may be in the form of a computer-readable storage medium storing the program.

In addition, the configuration of the position and orientation estimation device described in the above embodiment is merely an example, and may be modified according to circumstances without departing from the spirit of the invention.

The processing flow of the program described in the above embodiment is also an example, and unnecessary steps may be deleted, new steps may be added, or the processing order may be rearranged, without departing from the spirit of the program.

In the above embodiment, a case has been described in which the processing according to the embodiment is realized by a software configuration using a computer by executing a program, but this is not limited to this. The embodiment may be realized, for example, by a hardware configuration or a combination of a hardware configuration and a software configuration.

Furthermore, the processing in the above embodiment may be stored as a program on a storage medium such as an optical disk and distributed.

In the above embodiment, the term CPU refers to a processor in a broad sense, including general-purpose processors and dedicated processors (e.g., GPU: Graphics Processing Unit, ASIC: Application Specific Integrated Circuit, FPGA: Field Programmable Gate Array, programmable logic device, etc.).

In this case, the processor operations may not only be performed by a single processor, but may be performed by multiple processors located at physically separate locations working together. Furthermore, the order of the processor operations is not limited to the orders described in the above embodiments, and may be changed as appropriate.

All publications, patent applications, and technical standards described in this specification are incorporated by reference into this specification to the same extent as if each individual publication, patent application, and technical standard was specifically and individually indicated to be incorporated by reference.

Reference Signs List 10A, 10B, 10C Position and attitude estimation device 12 Vehicle information acquisition unit 13 Vehicle model information acquisition unit 14 Image information acquisition unit 16 Vehicle detection unit 17 Relative position and attitude estimation unit 18 Relative orientation estimation unit 20 Vehicle type discrimination unit 22, 23 Information sharing unit 24, 25 Correspondence unit 26, 27 Relative position and attitude overall optimization unit 28 Position and attitude acquisition unit 30 Computer main unit 33P, 33PS Position and attitude estimation processing program 40 (40A, 40B, 40C, 40D, 40E) Vehicle 42 Camera 44 Memory 50 Observation device 52 Camera 54 Memory

Claims (4)

a mobile object information acquisition unit that acquires, in an environment where a plurality of mobile objects are present, mobile object information including a type of the mobile object for each of the plurality of mobile objects;
an image information acquisition unit that acquires, for each of the plurality of moving bodies, image information indicating a captured image of the surroundings captured by an imaging device mounted on the moving body;
a detection unit that, for each of the plurality of moving bodies, determines a moving body that has captured an image as a first moving body based on the image information, and detects a second moving body that has been captured by the first moving body;
a discrimination unit that discriminates the type of the second moving object detected by the detection unit based on the image information;
a relative orientation identification unit that identifies a relative orientation indicating a direction from the first moving object to a second moving object based on a position of an image showing the second moving object on the captured image;
a storage unit that stores information of the moving object information acquisition unit, the relative orientation identification unit, and the discrimination unit;
a correspondence unit that corresponds the type of moving object information corresponding to the type of the second moving object detected by the detection unit to the second moving object detected by the detection unit based on the information stored in the storage unit;
a relative position and orientation estimation unit that estimates a relative position and orientation between the plurality of moving bodies based on the relative orientation from the first moving body to the second moving body among the plurality of moving bodies and the moving body information associated by the association unit;
A position and orientation estimation device comprising: a mobile object information acquisition unit that acquires, in an environment where a plurality of mobile objects are present, mobile object information including a type of the mobile object for each of the plurality of mobile objects;
an image information acquisition unit that acquires, for each of the plurality of moving bodies, image information indicating a captured image of the surroundings captured by an imaging device mounted on the moving body;
a model information acquisition unit that acquires model information indicating a shape of the moving object;
a detection unit that, for each of the plurality of moving bodies, determines a moving body that has captured an image as a first moving body based on the image information, and detects a second moving body that has been captured by the first moving body;
a discrimination unit that discriminates the type of the second moving object detected by the detection unit based on the image information;
a relative position and orientation specification unit that specifies a relative position and orientation of a second moving object with respect to the first moving object based on a position of an image showing the second moving object on the captured image and model information acquired by the model information acquisition unit;
a storage unit that stores information about the moving object information acquisition unit, the relative position and orientation specification unit, and the discrimination unit;
a correspondence unit that corresponds the type of moving object information corresponding to the type of the second moving object detected by the detection unit to the second moving object detected by the detection unit based on the information stored in the storage unit;
a relative position and orientation estimation unit that estimates a relative position and orientation between the plurality of moving bodies based on a relative position and orientation of a second moving body with respect to the first moving body among the plurality of moving bodies and the moving body information associated by the association unit;
A position and orientation estimation device comprising: a mobile object information acquisition unit that acquires, in an environment where a plurality of mobile objects are present, mobile object information including a type of the mobile object for each of the plurality of mobile objects;
an image information acquisition unit that acquires, for each of the plurality of moving bodies, image information indicating a captured image of the surroundings captured by an image capture device mounted on the moving body;
a model information acquisition unit that acquires model information indicating a shape of the moving object;
an observation device information acquisition unit that acquires observation device information including a position and an attitude of an observation device that captures the environment;
an observation image information acquisition unit that acquires observation image information indicating an observation image captured by the observation device;
a model information acquisition unit that acquires model information indicating a shape of the moving object;
a detection unit that, for each of the observation devices and the plurality of moving bodies, determines the observation device and the moving body that captured the captured image as an observed body based on the image information, and detects a moved body photographed by the observed body;
a discrimination unit that discriminates the type of the moving object detected by the detection unit based on the image information;
a relative position and orientation specification unit that specifies a relative position and orientation of the object to be moved with respect to the observation object based on a position of an image showing the object to be moved on the captured image and model information acquired by the model information acquisition unit;
a storage unit that stores information about the moving object information acquisition unit, the relative position and orientation specification unit, and the discrimination unit;
a matching unit that matches the type of moving object information corresponding to the type of the moving object detected by the detection unit with the moving object detected by the detection unit based on the information stored in the storage unit;
a relative position and orientation estimation unit that estimates a relative position and orientation between the plurality of moving bodies based on a relative position and orientation of a moved body with respect to the observed body in the plurality of moving bodies and the moving body information associated by the association unit;
A position and orientation estimation device comprising: Computer,
a mobile object information acquisition unit that acquires mobile object information including a type of the mobile object for each of the mobile objects in an environment where a plurality of mobile objects are present;
an image information acquisition unit that acquires, for each of the plurality of moving bodies, image information indicating a captured image of the surroundings captured by an imaging device mounted on the moving body;
a detection unit that, for each of the plurality of moving bodies, determines a moving body that has captured an image as a first moving body based on the image information, and detects a second moving body that has been captured by the first moving body;
a discrimination unit that discriminates the type of the second moving object detected by the detection unit based on the image information;
a relative orientation identification unit that identifies a relative orientation indicating a direction from the first moving object to a second moving object based on a position of an image showing the second moving object on the captured image;
a storage unit that stores information about the moving object information acquisition unit, the relative orientation identification unit, and the discrimination unit;
a matching unit that matches a second moving object detected by the detection unit with the moving object information of a type corresponding to the type of the second moving object, based on the information stored in the storage unit;
as well as,
a relative position and orientation estimation unit that estimates a relative position and orientation between the plurality of moving bodies based on the relative orientation from the first moving body to the second moving body among the plurality of moving bodies and the moving body information associated by the association unit;
A program to function as a