Self-location estimation method, self-location estimation program, and self-location estimation device

Abstract

To provide a self-position estimation method which estimates a self-position more accurately, a self-position estimation program, and a self-position estimation apparatus which estimates the self-position using the self-position estimation program.SOLUTION: A method for estimating a self-position includes: a step S301 of associating a detection object, detected in an SLAM space set based on a plurality of frame images, the position of which has been estimated, with a landmark the position of which is known in a real space defined by another GPS coordinates which are different from coordinates of the SLAM space; a step S302 of deciding a re-projection position at which the detection object associated with the landmark is to be imaged in the frame images; and steps S303 and S304 of estimating imaging information including at least imaging position and imaging angle of the frame images by minimizing a difference between the re-projection position and an actual position of the detection object associated with the landmark in the frame images.SELECTED DRAWING: Figure 10

Description

[Technical Field]

[0001] The present invention relates to a self-location estimation method and a self-location estimation program for estimating one's own location, and a self-location estimation device that estimates one's own location using the self-location estimation program. [Background technology]

[0002] Technologies that capture images while moving and estimate the three-dimensional spatial environment around a moving object (information about the shape of the space and obstacles) based on these images are used in map creation, autonomous robot navigation, and the like. Such technologies include, for example, SfM (Structure from Motion) and v-SLAM (visual-Simultaneous Localization and Mapping). SfM and v-SLAM often differ in whether or not they are based on real-time processing, with SfM often being applied to processing multiple images regardless of the order in which they were captured, and v-SLAM often being applied to processing multiple frame images captured as a video. Such technologies are described, for example, in Patent Document 1 and Patent Document 2.

[0003] Patent Document 1 describes a method in which a moving camera sequentially captures images of the vehicle's surroundings and creates map information based on these images. The map information is represented by a keyframe table and a map point table, and map points refer to the three-dimensional coordinates of feature points. Feature points refer to edge points in an image that represent the shape of an object present in a target area. The information processing device described in Patent Document 1 uses a nonlinear optimization algorithm to calculate the camera's position and orientation that minimizes the distance between the map points projected onto a captured grayscale image and the feature points, thereby estimating the camera's position and orientation. Patent Document 2 also describes a method in which the vehicle's own position and orientation estimated by v-SLAM are corrected using a self-position estimated or determined using another algorithm, such as AMCL (Adaptive Monte Carlo Localization). [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Japanese Patent Application Publication No. 2018-173882 [Patent Document 2] Japanese Patent Publication No. 2020-160594 Summary of the Invention [Problem to be solved by the invention]

[0005] However, the self-localization method using v-SLAM described in Patent Document 1 first associates feature points between grayscale images captured from two viewpoints. Then, the camera position and orientation corresponding to the first viewpoint are set as the origin of the global coordinate system, and initial values ​​for the camera position and orientation corresponding to the second viewpoint are set. These initial values ​​are then set by triangulation. The information processing device described in Patent Document 1 sequentially estimates the camera position and orientation using map points as the camera moves in accordance with the vehicle's movement. The procedure described in Patent Document 1 sets the local coordinate system based on the vehicle as the origin of the global coordinate system. Therefore, if a deviation occurs in the origin, this deviation affects the estimated camera position and orientation, which is detrimental to improving the accuracy of the estimation. Furthermore, the AMCL described in Patent Document 2 estimates the self-localization using ranging data and map data of the moving environment, using a two-dimensional grid map as the map data. This method, like self-localization using v-SLAM, estimates the self-localization based on a relative reference, and is therefore considered insufficient for improving the accuracy of self-localization using v-SLAM.

[0006] The present invention has been made in view of the above points, and relates to a self-location estimation method, a self-location estimation program, and a self-location estimation device that can estimate a self-location with higher accuracy. [Means for solving the problem]

[0007] In order to achieve the above object, a self-localization method according to one aspect of the present invention includes: a correlation step of correlating a detected object, which is detected in a first space set based on a captured image and whose position is estimated, with a landmark, whose position is known in a second space defined by coordinates different from the coordinates of the first space; a determination step of determining, based on the known position of the landmark, a reprojection position where the detected object associated with the landmark is to be captured in the captured image; and an imaging estimation step of estimating imaging information including at least an imaging position and an imaging angle of the captured image by minimizing a difference between the reprojection position and an actual position of the detected object captured in the captured image. fruit , In the associating step, the detected objects are associated with the landmarks based on a similarity between a relative positional relationship between the detected objects and a relative positional relationship between the landmarks in the second space. .

[0008] The self-location estimation program of the present invention includes: an association function that associates a detected object, which is detected in a first space set based on a captured image and whose position is estimated, with a landmark whose position is known in a second space defined by coordinates different from the coordinates of the first space; a determination function that determines a reprojection position, which is a position where the detected object associated with the landmark is to be captured in the captured image, based on the known position of the landmark; and an imaging estimation function that estimates imaging information including at least the imaging position and imaging angle of the captured image by minimizing the difference between the reprojection position and the actual position of the detected object captured in the captured image. wherein the association function associates the detected objects with the landmarks based on a similarity between a relative positional relationship between the detected objects and a relative positional relationship between the landmarks in the second space. Make the computer do that.

[0009] A self-location estimation device according to one aspect of the present invention includes: an association unit that associates a detected object, which is detected in a first space set based on a captured image and whose position is estimated, with a landmark, whose position is known in a second space defined by coordinates different from the coordinates of the first space; a determination unit that determines, based on the known position of the landmark, a reprojection position where the detected object associated with the landmark is to be captured in the captured image; and an imaging estimation unit that estimates imaging information including at least an imaging position and an imaging angle of the captured image by minimizing a difference between the reprojection position and an actual position of the detected object captured in the captured image. and the associating unit associates the detected objects with the landmarks based on a similarity between a relative positional relationship between the detected objects and a relative positional relationship between the landmarks in the second space. . [Effects of the Invention]

[0010] According to the above aspects of the invention, it is possible to provide a self-location estimation method, a self-location estimation program, and a self-location estimation device that can estimate a self-location with higher accuracy. [Brief explanation of the drawings]

[0011] [Figure 1] 1 is a functional block diagram for explaining a self-location estimation device according to an embodiment of the present invention; [Figure 2] FIG. 10 is a diagram for explaining feature points in a frame image. [Figure 3] 1 is a schematic diagram illustrating the application of an ORB according to one embodiment of the present invention to a traffic light and a sign as a detection object to extract feature points. [Figure 4] 4(a), (b) and (c) are diagrams for explaining the estimation of the positions of the feature points extracted by the processing of FIG. 3. [Figure 5] FIG. 10 is a diagram for explaining the creation of a basis pair of positioning information performed in the matching process of one embodiment of the present invention. [Figure 6] FIG. 6 is a diagram showing an example in which the basis pairs described in FIG. 5 are mapped to a common hash table. [Figure 7]FIG. 7 is a diagram for explaining how a group of feature points is processed in the same manner as in the processing explained with reference to FIGS. 5 and 6, and mapped to a common hash table. [Figure 8] FIG. 2 is a diagram in which the arithmetic processing unit in the functional block diagram of FIG. 1 is represented for each processing thread. [Figure 9] 10 is a flowchart illustrating a process executed in the self-position estimation device. [Figure 10] 10 is a flowchart for explaining processing subsequent to FIG. 9, which is executed in the self-position estimation device. [Figure 11] 11 is a flowchart for explaining a series of processing steps performed by a matching unit in the flowchart of FIG. 10. DETAILED DESCRIPTION OF THE INVENTION

[0012] Hereinafter, a self-location estimation method according to one embodiment of the present invention, a self-location estimation program for executing this self-location estimation method on a computer, and a self-location estimation device for executing the self-location estimation method will be described. In this embodiment, an example will be described in which an outdoor map (hereinafter referred to as a "map") is created using ORB (Oriented Fast and Rotated Brief)-SLAM. This embodiment will also describe an example in which a camera is mounted on a moving object, such as a vehicle, and a video captured using this camera is acquired as a series of frame images. In this specification, the "self-location" refers to the imaging position of the camera, which is expressed in two-dimensional or three-dimensional coordinates. The imaging angle also includes the angle in the height direction and the horizontal direction of the camera toward the subject. In this specification, the angle at which the lens of the camera faces the subject will also be referred to as the "attitude." The angle may be defined, for example, by the angle between a horizontal central axis passing through the center point of the lens and an axis passing through the center point of the lens and a reference point (e.g., the center) of the subject.

[0013] [Self-location estimation device] FIG. 1 is a functional block diagram for explaining a self-location estimation device 1 of this embodiment. FIG. 1 shows a system including a landmark database (DB) 55, a frame image database (DB) 56, and a detected object database (DB) 57 in addition to the self-location estimation device 1. The landmark DB 55 stores highly accurate positioning information of features that serve as landmarks in the environment for which a map is to be created. The positioning information may be obtained by positioning, and may be the coordinates of the features expressed as GPS coordinates using latitude and longitude, or may be planar rectangular coordinates without using latitude and longitude. In this embodiment, in addition to the positioning information, attributes such as the type and name of the landmark are stored in the landmark DB 55. The system shown in FIG. 1 also includes a map database (DB) 59 that stores the created map.

[0014] The frame image DB 56 stores multiple images (hereinafter referred to as "frame images") extracted from a video captured by a camera (not shown) mounted on a vehicle or the like. The frame images can be extracted, for example, by cutting out the video at regular time intervals to generate multiple still images. A monocular camera, an RGB-D camera, or a stereo camera can be used to capture the video. The detected object DB 57 stores two-dimensional coordinates indicating the range of a detection box on each frame image of an object previously detected from the frame image. The range of the detection box may be enclosed by a rectangle or a polygon, and may be detected by image analysis, or may be estimated using object detection by machine learning or a segmentation model. In this embodiment, the object is preferably an object that appears in multiple frame images, such as a traffic light or a sign. The position of the object is recorded based on the positions of feature points on the object, and the position is specified by SLAM coordinates defined in SLAM space or GPS coordinates. Furthermore, this embodiment creates or corrects (updates) a map of the environment captured by the camera using the recorded objects. The created or corrected map is stored in the map DB 59. As will be described later, the maps stored in the map DB 59 are corrected by key frame images from among the frame images captured in sequence, thereby improving the accuracy of the maps.

[0015] As shown in FIG. 1, the self-location estimation device 1 has a calculation processing unit 10, a receiving unit 2, and an output unit 3 that outputs the results of the self-location estimation process. The receiving unit 2 is a user segment that receives positioning information from a GPS satellite G. The receiving unit 2 is used to acquire GPS coordinates for converting the SLAM coordinates of feature points into GPS coordinates. The output unit 3 may be a display screen, a printer that prints out the results, or a speaker that outputs audio.

[0016] The arithmetic processing unit 10 includes an extraction unit that sequentially extracts common feature points displayed in at least some of the frame images from the plurality of frame images generated by capturing. In this embodiment using ORB-SLAM, the feature point extraction unit 14 that extracts the feature points functions as the extraction unit. The arithmetic processing unit 10 also includes a feature point position estimation unit 11 that estimates the positions of the feature points in a first space set based on the plurality of frame images, and a matching unit 12 (association unit) that associates the feature points whose positions are estimated by the feature point position estimation unit 11 with landmarks whose positions are known in a second space defined by coordinates different from the coordinates of the first space. In this embodiment, the first space is defined as a SLAM space expressed by relative coordinates, and these coordinates are defined as SLAM coordinates. The second space is defined as a real space based on a geodetic system, and these coordinates are defined as GPS coordinates. However, the real space may be defined based on positioning of the environment, and is not limited to being based on information measured using GPS.

[0017] The arithmetic processing unit 10 also includes a determination unit that determines a reprojection position, which is a position where a feature point associated with a landmark is supposed to be captured in a captured image, based on the known positions of the landmarks, and a self-position estimation unit 13 that estimates imaging information including at least the imaging position and imaging angle of the frame image by minimizing the difference between the reprojection position and the actual position of the feature point associated with the landmark in the frame image. The arithmetic processing unit 10 is realized, for example, by executing a self-position estimation program on a general-purpose personal computer. The feature point extraction unit 14, feature point position estimation unit 11, matching unit 12, and self-position estimation unit 13 of the arithmetic processing unit 10 are each realized by a program, a memory used to execute the program, a CPU (Central Processing Unit), a user interface, etc.

[0018] The self-location estimation device 1 shown in FIG. 1 includes all of the components required to create a map. The self-location estimation device 1 may be divided into multiple components, each of which may be located in a different location and perform its own processing. In such a case, the components of the self-location estimation device 1 may be divided in any combination. Furthermore, the self-estimation device 1 may include multiple functions shown in FIG. 1, each of which may be located in each of the multiple divided components. Furthermore, in this embodiment, the entire self-location estimation device 1 may be located in multiple locations and perform processing independently. Such a configuration can be easily realized, for example, by running the self-estimation location program of this embodiment on each of multiple general-purpose computers located in multiple locations. This embodiment describes an example in which the self-estimation devices 1 located in multiple locations each function and share the overall map creation processing. However, this embodiment is not limited to this configuration. The system shown in FIG. 1 may acquire frame images while moving and create a map in real time.

[0019] (Feature point extraction section) FIG. 2 is a diagram illustrating feature points p in a frame image. In this embodiment, feature points p are extracted using, for example, the ORB method. Feature points that do not change position in successive frame images and are easy to extract are selected. The example shown in FIG. 2 shows frame images in which the square portions of windows and doors, or the corners of corners, are extracted as feature points.

[0020] FIG. 3 is a schematic diagram for explaining the extraction of feature points by applying ORB to traffic lights and signs as objects. In this embodiment, a camera mounted on a vehicle captures images of traffic lights 100, 300 and a sign 200 while moving. Therefore, as shown in FIG. 3, it is possible to obtain images similar to those obtained when the traffic light 100 and the sign 200 are captured by a plurality of cameras at different positions. In FIG. 3, three frame images captured by, for example, one camera at three different capturing positions are respectively represented by f N-1 , f N , f N+1 In the example shown in FIG. N-1 , fN A traffic light 100 and a sign 200 are reflected in the frame image f N+1 In the following description, the frame image f N-1 , f N、 f N+1 When there is no need to distinguish between them, they are simply referred to as frame images f.

[0021] The feature point extraction unit 14 extracts the frame image f N-1 In the figure, multiple corners C of the reflected traffic light 100 100 feature point p N-1,1 As shown, the corners of the sign 200 are C 200 feature point p N-1,2 The feature point extraction unit 14 extracts the frame image f N From traffic light 100, corner C 100 feature point p N,1 As shown, sign 200 corner C 200 feature point p N,2 Extract the frame image f N+1 From sign 200 corner C 200 feature point p N+1,1 As shown, sign 200 corner C 200 feature point p N+1,2 The feature point extraction using ORB is performed by combining FAST (Features from Accelerated Segment Test) and BRIF (Binary Robust Independent Elementary Features). That is, if there are a predetermined number (e.g., 12 points) or more of points (e.g., 16 points) that are brighter than the target point by a threshold or more among a plurality of points (e.g., 16 points) surrounding the target point in the frame image, the feature point extraction unit 14 adopts the target point as a feature point. Furthermore, the feature point extraction unit 14 randomly selects pixel pairs from the surrounding area of ​​the adopted feature point and performs a process of determining 0 or 1 according to their brightness.

[0022] In the above process, in this embodiment, a plurality of feature points p N-1,1 , p N-1,2are recognized as feature points contained in the detection boxes b1 and b2, respectively. Similarly, the frame image f N+1 Feature points p N+1,3 are recognized as feature points contained within detection box b3. The feature points contained within detection boxes b1, b2, and b3 are stored as part of the coordinates of the detected object in the detected object DB 57 in Figure 1. The stored coordinates are SLAM coordinates. Note that the image between detection boxes b1, b2, and b3 can be estimated to be the background of the object.

[0023] In this embodiment, the feature point extraction unit 14 assigns ID (identity) information for identifying each detected object, such as the traffic light 100 defined by the detection boxes b1, b2, and b3. IDs are assigned to objects that appear in multiple frame images f, but not to objects that appear in only one frame image f. In other words, in the example shown in FIGS. 4(a), 4(b), and 4(c), IDs are assigned to objects that appear in two frame images f. N-1 , f N Traffic light 100 reflected in the image, three frame images f N-1 , f N , f N+1 ID information is assigned to the sign 200 reflected in the frame image f N+1 No ID information is assigned to the traffic light 300 that is only reflected in the image.

[0024] (Feature point position estimation unit) 4(a), 4(b), and 4(c) are diagrams for explaining the estimation of the positions of extracted feature points performed by the feature point position estimation unit 11. In FIGS. 4(a) to 4(c), three different imaging positions of one camera are denoted as cameras N-1, N, and N+1, respectively, and the frame images captured at each imaging position are denoted as frame images f N-1 , frame image f N , frame image f N+1 The feature point position estimation unit 11 first identifies feature points whose positions were identified in a previous map creation process. The known map is represented using relative SLAM coordinates.

[0025] Next, the feature point position estimation unit 11 estimates the position of the map point in the known map as a reference, and calculates the position of the frame image f N-1 The camera that captured the N-1、 camera N The camera's posture and self-position are estimated. In this specification, the term "map point" refers to a feature point arranged on a map. N-1、 camera N The self-position of the camera is a known position. N-1、 camera N Based on the self-position of the camera, the position of the feature point whose position is not known (unknown) is estimated. Furthermore, the feature point position estimation unit 11 sets the feature point whose position is obtained by estimation as a known map point, and calculates the position of the camera based on the position of the map point. N+1 The posture and position of the object, i.e., the self-position, are estimated. In this embodiment, the feature point position estimation unit 11 executes this processing each time a frame image f is input. Then, at the timing when a frame image f determined to be a key frame among the frame images is input, a map in which feature points are arranged in the SLAM space is created.

[0026] Next, the map of the SLAM space is mapped onto real space represented by GPS coordinates. The SLAM coordinate system and the GPS coordinate system are both Cartesian coordinate systems and can be converted into each other by similarity transformation. The parameters used in the conversion can be estimated using, for example, the Umeyama method, which is a well-known method. The Umeyama method is disclosed, for example, in S. Umeyama (1991), "Least-squares estimation of transformation parameters between two point patterns," Umeyama, (1991), IEEE volume 13, Page(s): 376-380. The positioning information obtained by the receiver 2 is used to estimate the parameters of the conversion using the Umeyama method.

[0027] (Matching section) The matching unit 12 executes a correspondence process in which the detected object and the landmark are associated based on the similarity between the relative positional relationship of the plurality of feature points and the relative positional relationship of the plurality of landmarks in real space. In this correspondence process, the matching unit 12 performs coordinate transformation between the SLAM space and the real space, and determines the similarity therebetween.

[0028] That is, the matching unit 12 reads out the positioning information stored in the landmark DB 55 shown in Fig. 1 and executes a matching process of associating feature points detected based on the positioning information with features that serve as landmarks. Figs. 5 to 7 are conceptual diagrams for explaining the matching process. The matching explained with Fig. 5 etc. describes an example in which Geometric Hashing is used. However, this embodiment is not limited to such a configuration, and any method may be used to match detected objects with landmarks.

[0029] FIG. 5 is a diagram for explaining the creation of basis pairs of positioning information stored in the landmark DB 55. FIG. 6 shows an example in which basis pairs are mapped to a common hash table. FIG. 7 is a diagram for explaining that a group of feature points is processed in the same manner as described with reference to FIGS. 5 and 6 and mapped to a common hash table. In this embodiment, a group of feature points is determined to be a feature based on a preset condition. This condition may be, for example, as shown in FIG. 3, such that the range of a detection box containing multiple feature points is determined to be a feature, and this may be taken as a detected object. In this embodiment, landmarks and detected objects may hereinafter be collectively referred to as "objects."

[0030] 5 to 7 show multiple maps 61 to 66 and a hash table 71 to explain the processing. In maps 61 to 66, the black, white, and shaded dots (hereinafter also referred to as "density") correspond to object types, and in this embodiment, dots with different densities correspond to different object types. In this embodiment, types represented by black dots are referred to as Type 1, types represented by white dots as Type 2, and types represented by shaded dots as Type 3. In addition, in hash table 71, the types of objects are represented by bracket types. In the description of this embodiment, parentheses () correspond to Type 1, angle brackets <> correspond to Type 2, and square brackets [] correspond to Type 3. Here, the object type refers to, for example, traffic lights and signs. In the example of FIG. 4, where ID information is assigned to traffic lights and signs, the object type is "2."

[0031] As shown in a map 61 in Fig. 5, the matching unit 12 first selects a plurality of landmarks from the landmark DB 55. In the example of Fig. 5, landmarks L1, L2, L3, L4, and L5 are selected in the map 61. Of the landmarks L1 to L5, the landmarks L1, L4, and L5 are of the same type, and the landmarks L2 and L3 are of different types from the others.

[0032] Next, the matching unit 12 selects two landmarks from the selected landmarks as a basis pair. Map 62 shows an example in which landmark L4 and landmark L1 are selected as a basis pair, and map 63 shows an example in which landmark L3 and landmark L1 are selected as a basis pair. In map 62, the distance between each dot corresponding to landmarks L1 to L5 is enlarged or reduced depending on the distance between landmarks L4 and L1 that form the basis pair. In addition, in map 63, the distance between each dot is enlarged or reduced depending on the distance between landmarks L3 and L1 that form the basis pair.

[0033] Next, the matching unit 12 converts the enlarged or reduced maps 62 and 63 into common x- and y-coordinates. The x- and y-coordinate conversion is performed by mapping one of the basis pairs to the origin (0,0) and the other to the coordinates (1,0). In the map 64, the dot corresponding to the landmark L4 is mapped to the origin (0,0), and the dot corresponding to the landmark L1 is mapped to the coordinates (1,0). In the map 65, the dot corresponding to the landmark L3 is mapped to the origin (0,0), and the dot corresponding to the landmark L1 is mapped to the coordinates (1,0). Such mapping is performed by rotating the maps 62 and 63.

[0034] Next, the matching unit 12 maps the positional relationship of each dot obtained in the maps 64 and 65 to the hash table 71, as shown in FIG. 6. The mapping is performed so that the relationship between the basis pair and the type of object can be determined. In the map 64, when landmarks L4 and L1 are considered as a basis pair, the landmark L2 is mapped to the coordinates (3 / 2,0), the landmark L3 is mapped to the coordinates (1,1 / 2), and the landmark L5 is mapped to the coordinates (3 / 2,-1). In this embodiment, the basis pair <4,1> is indicated in angle brackets indicating the type of landmark L2 in the column of the hash table 71 corresponding to the coordinates (3 / 2,0). Furthermore, in this embodiment, the basis pair (4,1) is indicated in round brackets indicating the type of landmark L3 in the column of the hash table 71 corresponding to the coordinates (1,1 / 2), and the basis pair [4,1] is indicated in square brackets indicating the type of landmark L5 in the column of the hash table 71 corresponding to the coordinates (3 / 2,-1). Furthermore, in this embodiment, landmark L4, which constitutes a basis pair, is shown as [4,1] in the column of hash table 71 corresponding to coordinates (0,0), and landmark L1 is shown as [4,1] in the column of hash table 71 corresponding to coordinates (0,1).

[0035] Furthermore, the matching unit 12 maps the relationship between the basis pair and the landmark type shown in the map 65 in the hash table 71. That is, as shown in the map 65, when the landmarks L3 and L1 are considered to be a basis pair, the landmark L2 is mapped to the coordinates (1 / 2, 1 / 2), the landmark L4 is mapped to the coordinates (1, -3 / 2), and the landmark L5 is mapped to the coordinates (3 / 2, 1 / 2). In this embodiment, the basis pair <3,1> is indicated in angle brackets indicating the type of landmark L2 in the column of the hash table 71 corresponding to the coordinates (1 / 2, 1 / 2), and the basis pair [3,1] is indicated in square brackets indicating the type of landmark L4 in the column of the hash table 71 corresponding to the coordinates (1, -3 / 2). In addition, in this embodiment, the basis pair [3,1] is shown in square brackets indicating the type of landmark L5 in the column corresponding to the coordinates (3 / 2,1 / 2) of the hash table 71, the landmark L3 that constitutes the basis pair is shown as (3,1) in the column of the hash table 71 corresponding to the coordinates (0,0), and the landmark L1 is shown as [3,1] in the column of the hash table 71 corresponding to the coordinates (0,1).

[0036] Next, the matching unit 12 reads the coordinates of multiple detected objects within a predetermined range from the detected object DB 57. The predetermined range includes the range in which the landmarks previously read from the landmark DB exist. As shown in FIG. 7, this embodiment reads five detected objects Da, Db, Dc, Dd, and De from the detected object DB 57. Then, similar to the processing for the landmarks, multiple basis pairs are generated for two detected objects. In the map 66 of FIG. 7, the detected objects Da and Db generate a basis pair, and the dot corresponding to the detected object Da is mapped to the origin (0,0), and the dot corresponding to the detected object Da is mapped to the coordinates (1,0). In this embodiment, basis pairs are also generated for the other detected objects, and the dots are mapped to coordinates by appropriately enlarging, reducing, or rotating them. Then, similar to the landmarks, the matching unit 12 maps the dots corresponding to the mapped detected objects to the columns of the hash table 71 corresponding to the coordinates.

[0037] According to the above process, as shown in the hash table 71 of FIG. 7, there are landmarks and detected objects that are mapped to the same column. The matching unit 12 infers that the landmarks that are most frequently mapped to the same column and that have the same object type match the detected object. In this embodiment, there are five columns in which the basis pair of landmarks L4 and L1 and the basis pair of detected objects Da and Db are mapped, and in all five columns, the types of the basis pair of landmarks L4 and L1 and the basis pair of detected objects Da and Db match. In contrast, there are two columns in which the basis pair of landmarks L3 and L1 and the basis pair of detected objects Da and Db are mapped. In both of these two columns, the types of the basis pair of landmarks L3 and L1 and the basis pair of detected objects Da and Db match, but the number of columns in which they are mapped together is fewer than the basis pair of landmarks L4 and L1 and the basis pair of detected objects Da and Db. In such a case, the matching unit 12 infers that the landmark L4 matches the detected object Da, and the landmark L1 matches the detected object Db.

[0038] Furthermore, the matching unit 12 estimates that detected object Dc matches landmark L5, detected object Dd matches landmark L2, and detected object De matches landmark L3. Such estimation may be performed, for example, based on the previously estimated matches between detected object Da and landmark L4 and between detected object Db and landmark L1. Alternatively, estimation may be performed by creating further basis pairs using other objects and mapping the created basis pairs to a hash table.

[0039] (Self-position estimation unit) After the detected object has been associated with the landmark, the self-position estimation unit 13 corrects the self-position estimated from the key frame image among the captured frame images. The correction is performed by calculating the difference between the position where the landmark is actually captured in the key frame image and the reprojection position where it would originally be captured, and correcting (optimizing) the imaging information related to the camera position and angle so as to reduce the reprojection error, which is the calculated difference. The imaging information in this embodiment is sufficient as long as it includes at least information related to the camera angle and position, and may also include other information as appropriate. The reprojection position is determined by projecting the actual position of the landmark onto the key frame image. The correction of the reprojection position is performed in SLAM space.

[0040] Equation (1) is an arithmetic equation used for correcting the self-position in this embodiment, and employs bundle adjustment. Equation (1) is a cost function of the frame position t, the camera angle r, and the landmark (point cloud) position p. The frame position t is expressed in a translational coordinate system, and the camera angle r is expressed in a rotational coordinate system. The right-hand side of Equation (1) includes the first and second terms. The first term is a basic term of ORB-SLAM, which is also included in the well-known bundle adjustment, and indicates the reprojection error of the map point. The second term is the landmark correction term of this embodiment, and indicates the reprojection error of the landmark. In Equation (1), i indicates the number of the keyframe image (which keyframe image is it?), and j indicates the number of the feature point in the keyframe image (which point is it?). The subscript of ij refers to the jth feature point captured in the ith keyframe image.

[0041]

number

[0042] (output section) The output unit 3 outputs the self-position estimated by the above process, i.e., the position of the camera. When outputting the self-position, in this embodiment, the corrected self-position expressed in SLAM space may be converted into GPS space by similarity transformation or the like to generate data to be overlaid on a map of real space. The generated data may then be displayed on a display screen of a computer or the like. This process enables the accurate travel route of a vehicle traveling in real space to be visually confirmed. Furthermore, when the above process is performed in real time while the vehicle is traveling, the route of the traveling vehicle can be accurately estimated, which can be used for, for example, car navigation, automatic vehicle driving, or remote control. The output unit 3 may also output the position of a vehicle equipped with a camera by voice. Furthermore, the output unit 3 may be configured to record the estimated self-position as text information or coordinates and output it at a desired timing. In this way, the self-position estimation device of this embodiment can be used as a vehicle recorder.

[0043] Next, the functions that execute the above-described processing will be described from the perspective of threads. FIG. 8 is a diagram showing the processing unit 10 in the functional block diagram of FIG. 1, with each processing thread represented. As shown in FIG. 8, the processing of the processing unit 10 in FIG. 1 is divided into four threads: a tracking unit 41, a local mapping unit 42, a loop closure unit 43, and a landmark correction unit 44. The tracking unit 41 sequentially inputs multiple frame images f. Then, as described in FIG. 3, for example, the tracking unit 41 extracts feature points from the input frame images f. Furthermore, as shown in FIGS. 4(a), 4(b), and 4(c), the frame images sent from a moving camera are matched with the feature points input immediately before, and the position of the map points and the camera's own position and orientation are estimated by repeating the feature point detection and matching.

[0044] During this processing, the tracking unit 41 extracts key frame images fk from multiple frame images f to be used to correct the map. The extracted key frame images fk are input to the local mapping unit 42. The local mapping unit 42 creates map points to be recorded in the created map. Next, the map points are projected onto the key frame images fk to determine the reprojection positions. Furthermore, the calculated reprojection error is minimized using equation (1) to correct the camera attitude and position. The reprojection error calculated by the local mapping unit 42 is expressed by the first term on the right-hand side of equation (1).

[0045] By performing the above process, this embodiment associates the camera's own position with the position of the map point captured at this own position, and by repeating the process, the accuracy of the association can be improved. In other words, this embodiment recalculates the self-position between more key frame images fk, gradually reducing the reprojection error and improving the accuracy of the self-position. The loop closure unit 43 detects loop trapping from the key frame images fk, and if loop trapping is detected, estimates the self-position from all frame images fk stored in the frame image DB, and optimizes the camera posture and the map being created.

[0046] The tracking unit 41, the local mapping unit 42, and the loop closure unit 43 execute the above processes in parallel on separate threads. The process performed by the tracking unit 41 has a relatively small amount of calculation among the three threads and is executed frequently. On the other hand, the process performed by the local mapping unit 42 has a larger amount of calculation than the tracking unit 41 and is executed less frequently. Furthermore, the process performed by the loop closure unit 43 has the largest amount of calculation among the three threads and is executed less frequently. By dividing the threads and executing processes in parallel in this way, this embodiment can prevent the load related to any one process from becoming too large and interfering with other processes, thereby improving the memory utilization efficiency of the self-location estimation device 1.

[0047] The map generated by the above processing is further corrected by the landmark correction unit 44 to improve its accuracy. As described with reference to FIGS. 5 to 7, the landmark correction unit 44 matches detected objects with landmarks. After matching detected objects with landmarks, the landmark correction unit 44 corrects the position and orientation of the camera estimated from key frame images among the captured frame images. The correction is performed by fine-tuning the position expressed in the camera's translational coordinate system and the angle expressed in the rotational coordinate system so that the value of the second term on the right-hand side of equation (1) is minimized.

[0048] The above-described process is an example in which a map is created and optimized using a known bundle adjustment, followed by further landmark correction. Landmark correction can be performed in real time in parallel with map creation, for example, at the end of the local mapping unit 42. In this case, the local mapping unit 42 minimizes reprojection error to correct the camera's pose and position, and then associates detected objects determined by feature points with landmarks for correction. In this case, the loop closure unit 43 detects loops from the map that has undergone both bundle adjustment and landmark correction, optimizes the camera's pose, and further optimizes the map.

[0049] [Self-location estimation method and program] Next, a self-location estimation method executed by a program running in the self-location estimation device described above will be described. Figures 9 to 11 are flowcharts for explaining the processing executed in the self-location estimation device 1. As shown in Figure 9, the self-location estimation device 1 sequentially acquires frame images by extracting still images at regular intervals from a video captured by a camera (not shown) mounted on a vehicle (step S201). The acquired frame images are stored in the frame image DB 56.

[0050] Next, as described with reference to FIG. 3, the feature point extraction unit 14 sets a point of interest in the frame image and determines whether there are a predetermined number of bright points above a threshold around the point of interest (step S202). If there are a predetermined number of bright points above the threshold, the point of interest is recognized as a feature point (step S202: YES). If the feature point is not present in the frame image (step S202: NO), the feature point extraction unit 14 does not perform any further processing on the frame image and acquires the next frame image. Then, the feature point extraction unit 14 determines a frame image to be determined as a key frame from among the frame images that have feature points (step S203). The key frame image is used to create or correct a map of the SLAM space in which feature points are placed at the positions obtained by estimation.

[0051] Next, feature point extraction unit 14 extracts, from among the feature points, those that are recognized as being included in the detection box shown in Fig. 3 (step S204). The extracted feature points are stored in detected object DB 57 as part of the coordinates of the detected object (step S205).

[0052] As explained with reference to Figs. 4(a) to 4(c), the feature point position estimation unit 11 estimates its own position when each frame image was captured, using the SLAM coordinates and other known feature points in the map as references. Furthermore, the estimated own position is taken as known, and the three-dimensional positions of unknown feature points are estimated (step S206). The feature point position estimation unit 11 alternately repeats the process of taking one of the feature points and the own position as known and estimating the other, and stores the positions of the feature points in the map DB 59 (step S207).

[0053] Next, the self-position estimation unit 13 calculates a reprojection position where the map point would originally be captured in the key frame image (step S208). Then, the self-position estimation unit 13 calculates the difference between the captured position of this map point and the reprojection position to calculate a reprojection error (step S209). Furthermore, the self-position estimation unit 13 iteratively corrects the self-position estimated in step S206, for example, using equation (1), so as to reduce the calculated reprojection error. When the reprojection error reaches a minimum value as a result of the correction, the optimization of the self-position including the camera attitude is completed (step S210). Furthermore, the calculation processing unit 10 determines the end of the process, for example, based on whether a frame image has been input or whether the process has been switched off (step S211). If it is not determined in step S211 that the process has not ended (step S211: NO), the feature point extraction unit 14 acquires the next frame image, and the process continues. On the other hand, if it is determined that the process has ended, the process is completed (step S211: YES).

[0054] Fig. 10 is a flowchart for explaining processing including matching with landmarks, which is performed after the processing described with reference to Fig. 9. Fig. 11 is a flowchart for explaining matching between detected objects and landmarks, which is shown as a subroutine in Fig. 10.

[0055] First, the matching process in step S301 of Fig. 10 will be described. As shown in Fig. 11, the matching unit 12 reads multiple pieces of data from the landmark DB 55 and selects two landmarks that form a basis pair (step S401). Then, the matching unit 12 converts the coordinates of the multiple landmarks so that the positioning information of one of the two landmarks that make up the basis pair is mapped to the origin (0,0) and the positioning information of the other is mapped to (1,0) (step S402). After the coordinate conversion, each landmark is mapped to a hash table (step S403).

[0056] Next, the matching unit 12 creates basis pairs for all combinations of the multiple landmarks whose data has been read from the landmark DB 55, and determines whether mapping to the hash table is complete (step S404). If the result of this determination is that mapping is complete for all combinations of basis pairs (step S404: YES), the matching unit 12 proceeds to processing of mapping the detected object. If mapping is not complete (step S404: NO), the matching unit 12 changes the two landmarks that make up the basis pair (step S401), and repeats the processing of steps S402 to S403 using basis pairs of other landmarks.

[0057] If the determination in step S404 is YES, the matching unit 12 reads out data on multiple detected objects from the detected object DB and selects two detected objects that will form a basis pair, similar to the landmarks (step S405). Then, the matching unit 12 converts the coordinates of the multiple landmarks (step S406) and maps each detected object after the coordinate conversion to the hash table to which the landmarks are mapped (step S407). This process is repeated until mapping based on the basis pairs of all combinations of the read out detected objects is completed (step S408: NO).

[0058] If the determination in step S408 is YES, the matching unit 12 detects the combination of landmark and detected object that are most frequently mapped to the same column in the hash table and that are of the same type, as shown in Fig. 7. Then, the detected combination is associated with the detected object and landmark (step S409). After the matching is completed, the GPS coordinates of the landmark are converted to SLAM coordinates, and the matching process ends (step S410).

[0059] After matching with the landmark is completed, as shown in FIG. 10, the self-position estimation unit 13 calculates a re-projection position where the landmark would originally be captured in the key frame image (step S302). Then, the self-position estimation unit 13 calculates the difference between the position where the landmark was actually captured and the re-projection position to calculate a re-projection error (step S303). Furthermore, the self-position estimation unit 13 iteratively corrects the self-position optimized in step S210 of FIG. 9 using, for example, equation (1) so as to reduce the calculated re-projection error. When the re-projection error reaches a minimum value as a result of the correction, the optimization of the camera attitude and self-position is completed (step S304).

[0060] After the above processing, the self-position estimation unit 13 calculates the SLAM coordinates of the detected object based on the optimized self-position (step S305), and further converts the SLAM coordinates into GPS coordinates (step S306). Furthermore, the calculation processing unit 10 determines whether the processing has ended (step S307). If it is determined in step S307 that the processing has not ended (step S307: NO), the matching unit 13 continues to match the detected object with the landmark. On the other hand, if it is determined that the processing has ended, the processing is ended (step S307: YES). Through the above processing, this embodiment can create a highly accurate map of the real space of the environment in which a video is captured while the camera is moving.

[0061] In the present embodiment described above, the self-location estimated in the SLAM space is corrected based on the reprojection error, and the self-location is also corrected based on the real-space coordinates of landmarks. With this configuration, the self-location estimation method of this embodiment can acquire a self-location that corresponds to the real space with high accuracy and accurately determine the position of a feature based on this self-location. This embodiment is particularly effective for creating a map of the real space outdoors.

[0062] Furthermore, in an embodiment of the present invention, after the accuracy of the created map has been sufficiently improved, the self-position and frame images may be associated and stored in a database. In this way, it becomes possible to compare captured frame images sequentially with the frame images in this database, and determine the self-position associated with the frame image in the database that matches the captured frame image as the self-position of the camera when the frame image was captured. [Explanation of symbols]

[0063] 1 Self-position estimation device 2. Receiving section 3 Output section 10. Processing unit 12 Matching section 13 Self-position estimation part 14 Feature point extraction unit 41 Tracking Section 42 Local Mapping Section 43 Loop closure part 44 Landmark correction section 55 Landmark Data DB 56 Frame Image DB 57 Detected Object DB 59 Map DB 61, 62, 63, 64, 65, 66 Maps 71 Hash Tables 100,300 traffic lights 200 signs

Claims

1. a correlation step of correlating a detected object, which is detected in a first space set based on the captured image and whose position is estimated, with a landmark, whose position is known in a second space defined by coordinates different from the coordinates of the first space; a determination step of determining a reprojection position, which is a position where the detected object associated with the landmark is to be imaged in the captured image, based on the known position of the landmark; an imaging estimation step of estimating imaging information including at least an imaging position and an imaging angle of the captured image by minimizing a difference between the reprojection position and an actual position of the detected object captured in the captured image, In the associating step, the detected objects are associated with the landmarks based on a similarity between a relative positional relationship between the plurality of detected objects and a relative positional relationship between the plurality of landmarks in the second space. Self-localization method.

2. 2. The self-location estimation method according to claim 1, wherein the detection of the detected object in the first space is performed by sequentially detecting a plurality of common detected objects displayed in at least a portion of the captured images from a plurality of captured images generated by imaging.

3. The self-position estimation method according to claim 1 , wherein the imaging estimation step corrects the imaging information so that the calculated difference value decreases.

4. The self-localization method according to claim 1 , wherein the association is performed by performing coordinate transformation between the first space and the second space to determine the similarity.

5. The self-localization method according to claim 1 , wherein the first space is a space expressed by relative coordinates, and the second space is a real space based on a geodetic system.

6. a correspondence function for correlating a detected object, which is detected in a first space set based on a captured image and whose position is estimated, with a landmark, whose position is known in a second space defined by coordinates different from the coordinates of the first space; a determination function for determining a reprojection position, which is a position where the detected object associated with the landmark is to be captured in the captured image, based on the known position of the landmark; an imaging estimation function that estimates imaging information including at least an imaging position and an imaging angle of the captured image by minimizing a difference between the reprojection position and an actual position of the detected object captured in the captured image; the association function associates the detected objects with the landmarks based on a similarity between a relative positional relationship between the detected objects and a relative positional relationship between the landmarks in the second space; A self-location estimation program that causes a computer to perform the following:

7. a correlation unit that correlates a detected object, which is detected in a first space set based on a captured image and whose position is estimated, with a landmark, whose position is known in a second space defined by coordinates different from the coordinates of the first space; a determination unit that determines a reprojection position, which is a position where the detected object associated with the landmark is to be captured in the captured image, based on the known position of the landmark; an imaging estimation unit that estimates imaging information including at least an imaging position and an imaging angle of the captured image by minimizing a difference between the reprojection position and an actual position of the detected object captured in the captured image, the associating unit associates the detected objects with the landmarks based on a similarity between a relative positional relationship between the detected objects and a relative positional relationship between the landmarks in the second space; Self-location estimation device.

Images