JP7259454B2 - Mobile position estimation system and mobile position estimation method - Google Patents

Description

The present invention relates to a mobile body position estimation system and a mobile body position estimation method.

Conventionally, there is a technology called SLAM (Simultaneous Localization and Mapping) that simultaneously creates a travel route of a mobile object and a map of the surrounding environment by inputting data about the surrounding situation acquired while the mobile object is moving. Among the SLAM techniques, a technique for estimating the camera position/orientation of a mobile body while it is running based on an image captured by the mobile body is called Visual-SLAM (hereinafter referred to as "V-SLAM").

As a related prior art, there is a technique of extracting and tracking feature points from captured images that change in time series, and using these feature points to estimate the camera position/orientation of a moving object during travel.

JP 2017-53795 A Japanese Patent Application Laid-Open No. 2007-322403

However, the conventional technique has a problem that the generated environment map (three-dimensional position of the feature point group) is inaccurate. For this reason, the estimation accuracy of shooting position and orientation using the environment map deteriorates, and the discrepancy between the environment map and the actual shot image is too large to optimize correction, and the shooting position and orientation estimation itself. fails, and there is a problem that an interval that cannot be estimated occurs.

In one aspect, an object of the present invention is to perform highly accurate position and orientation estimation.

In one embodiment, for an arbitrary image out of the photographed time-series images, from the characteristics of the arbitrary image, a moving body position for performing information processing for estimating the photographing position/orientation of the arbitrary image and generating an environment map. An estimation system, comprising: an information processing device that obtains retrograde images obtained by reversing the time-series images with respect to time, and performs the information processing using the retrograde images. A system is provided.

Further, in one embodiment, for an arbitrary image out of the photographed time-series images, from the characteristics of the arbitrary image, the movement for performing information processing such as estimating the photographing position/orientation of the arbitrary image and generating an environment map. A system for estimating body position, wherein said information processing is performed by using an image captured near the latest time in a group of images showing image feature points having mutually distinguishable feature amounts. A position estimation system is provided.

According to one aspect of the present invention, highly accurate position and orientation estimation can be performed.

FIG. 1A is an explanatory diagram schematically showing an example of a posture graph and optimization in a mobile body position estimation method according to an embodiment; FIG. 1B is an explanatory diagram schematically showing an example of a posture graph and optimization in a moving body position estimation method according to the prior art. FIG. 2A is a flowchart (part 1) illustrating an example of a procedure of processing of the mobile body position estimation method according to the embodiment; FIG. 2B is a flowchart (part 2) showing an example of the procedure of the mobile body position estimation method according to the embodiment; FIG. 3A is an explanatory diagram showing an example of a V-SLAM environment map in reverse time in the moving body position estimation method according to the embodiment. FIG. 3B is an explanatory diagram showing an example of a V-SLAM environment map based on forward time in the mobile body position estimation method according to the prior art. FIG. 4A is an explanatory diagram showing an example of a surrounding environment map in the mobile body position estimation method according to the embodiment. FIG. 4B is an explanatory diagram showing an example of a surrounding environment map in the mobile body position estimation method according to the prior art. FIG. 5 is an explanatory diagram of an example of the system configuration of the mobile position estimation system according to the embodiment. FIG. 6 is a block diagram showing an example of the hardware configuration of the mobile position estimation device (server). FIG. 7 is a block diagram showing an example of the hardware configuration of the vehicle-mounted device. FIG. 8 is an explanatory diagram showing an example of the data configuration of the real coordinate environment map. FIG. 9 is an explanatory diagram showing an example of the data configuration of all image position/orientation data. FIG. 10 is an explanatory diagram showing an example of contents of the mobile body position estimation system and the mobile body position estimation method according to the embodiment. 11A is an explanatory diagram (part 1) showing an example of conversion matrix calculation in the initial orientation/coordinate system setting unit; FIG. FIG. 11B is an explanatory diagram (part 2) showing an example of conversion matrix calculation in the initial posture/coordinate system setting unit; 11C is an explanatory diagram (part 3) showing an example of conversion matrix calculation in the initial attitude/coordinate system setting unit; FIG. FIG. 11D is an explanatory diagram showing an example of calculation of the scale conversion matrix M1. FIG. 11E is an explanatory diagram showing an example of calculation of the rotation transformation matrix M2. FIG. 12 is a flowchart illustrating an example of a procedure of processing by a KF (key frame) updating unit.

Embodiments of a mobile body position estimation system and a mobile body position estimation method according to the present invention will be described in detail below with reference to the drawings.

(Embodiment)
A large amount of data (for example, video data, etc.) from on-board devices of moving general vehicles is collected (probed). From this video data of general vehicles, the position and posture during driving are estimated and used for in-vehicle data analysis. A generally installed GPS (Global Positioning System) device can only measure the position of the vehicle with a large error, so it cannot be applied to services that require a detailed position of the vehicle.

For this general vehicle data, for example, if the position and posture at the time of shooting while driving are estimated with high accuracy and added to the in-vehicle image, features around the road can be extracted from the image and used for autonomous driving. It can be applied to new service fields such as creating and updating maps and other maps, and analyzing surrounding conditions at the time of shooting for autonomous driving. Therefore, as a premise for the use of new services using general vehicle images, there is a need for technology that can accurately estimate the position and orientation of the camera that captured the in-vehicle image (the position and orientation of the video).

SLAM receives in-vehicle data related to surrounding conditions acquired during movement, such as LIDAR (Laser Imaging Detection and Ranging) data, etc. Dimensional position map, etc.) is a general term for technologies that simultaneously create.

Among them, V-SLAM uses images captured by an in-vehicle camera as an input, and uses changes in the subject reflected in the captured image to determine the vehicle's driving route (vehicle position/posture) and the surrounding environment map (the location of the surrounding subject). It is a technology that can estimate and create a three-dimensional position map of image feature point groups (hereinafter referred to as an environment map), and it is possible to estimate the position and attitude of a vehicle from a monocular image of a general vehicle.

In V-SLAM, initial three-dimensional positions are estimated by triangulation for image feature points newly observed on a plurality of images, and added to the environment map. On that basis, while considering the difference between the assumed reflection state of the image feature points of the environment map in the captured image and the actual captured image, the shooting position and orientation of the image group and the image feature point group of the environment map are calculated. The three-dimensional position of is optimized and corrected to obtain the final photographing position and orientation of the group of photographed images.

A moving body position estimation system 500 according to an embodiment shown in FIG. ) Nine V-SLAM-based processing units including an update unit 522, a 3D map feature point update unit 531, a graph constraint generation unit 532, a KF posture/feature point map optimization unit 533, and a loop detection/closing unit 541 In addition, it has two internally held data: an actual coordinate environment map 550 (KF group information 551, feature point group information 552) and all image position/orientation data 560. FIG.

KFs (key frames) are particularly important images among all images included in a video. In V-SLAM, the shooting position and posture of KF images are estimated using detailed analysis techniques using an environment map so that there is no contradiction in both global and local areas. In many cases, a method of simply estimating using the relative relationship from . Therefore, in the mobile body position estimation method according to the embodiment, a similar method is adopted, but the entire image group may be regarded as KF.

Also, the environment map (actual coordinate environment map 550) has information about which image feature points are used in which images, in addition to the three-dimensional positions of each image feature point. This is KF group information 551 and feature point group information 552 shown in FIG. 5 to be described later. The KF group information 551 is two pieces of information on the image (KF) group in the main video and information on the two-dimensional position where each image feature is shown on the KF image, and the environment map can be used for any image. This is essential information for use in position and orientation estimation. The contents of the real coordinate environment map 550 will be described in detail in FIG. 8, which will be described later.

In general, the feature point-based V-SLAM environment map includes image features and three-dimensional positions of the image feature point group, information on the image KF viewing the feature point group, and an image similar to the image KF. image features in the image KF to enable retrieval (many have more image features than feature points that hold 3D positions).

In addition, as shown in all image position/orientation data 560 shown in FIG. 5 to be described later, the image position/orientation is the shooting position/orientation of all image frames of the video. The KF shooting position/orientation data included in the KF group information 551 of the real coordinate environment map 550 is also part of the entire image position/orientation, and overlaps. is described together with the KF information of Note that the contents of the total image position/orientation data 560 will be described in detail later with reference to FIG.

Further, an image 1001 shown in FIG. 10, which will be described later, is an image of a camera mounted on a moving body (vehicle). It is obtained by an arbitrary method such as using communication means of the vehicle such as an in-vehicle device or manually via a recording medium, and is used as an input for the photographing position estimation system of the present invention. Also, although not specifically described, since it is used for image distortion correction, etc., the internal parameters of the camera that captured the image may be assumed to be known, and distortion correction may be performed as appropriate.

(Outline of mobile body position estimation method)
First, an outline of a mobile body position estimation system and a mobile body position estimation method according to the present embodiment will be described with reference to FIGS. 1A and 1B.

In the moving body position estimation method according to the present embodiment, two processes are executed for V-SLAM processing. The first process is a process (reverse image acquisition process) of acquiring an image in a reverse direction so as to process the image in a direction opposite to the passage of time. This retrograde image acquisition process is performed, for example, by the retrograde image acquisition unit 512, which will be described later.

In this retrograde image acquisition process, for example, each image is extracted from the input video file so that the shooting time goes back, and the extracted image is passed to the subsequent V-SLAM processing unit. When the input video is a live input, a group of images for an arbitrary time is buffered and stored, and the video for the stored time is acquired at any time in reverse to the acquisition time by the V-SLAM processing unit. can be passed to

The second process is to time-reverse the input video (time-reverse ) is a process (reverse determination process). Therefore, whether or not to execute the retrograde image acquisition process is determined based on the result determined by the retrograde determination process. This retrograde determination process is performed, for example, by the retrograde determination unit 511, which will be described later.

Specifically, in the retrograde determination process, for example, it is determined whether or not the target image is the image of the camera installed facing the front of the moving body. (Retrograde image) acquisition processing is determined to be performed, and if otherwise (for example, an image of the rear of a moving object), it is determined that retrograde image acquisition processing is not to be performed.

Here, whether or not the camera is a front-facing camera may be obtained by any known method, for example, from camera installation position information on a moving object, or may be obtained by optical flow as a subject change in a captured image. It may be possible to estimate whether the background is approaching or going away, and based on the estimation result, it may be determined whether or not the camera is forward-facing shooting.

As will be described later, in the conventional method, the two images in which the image feature points of the subject are first detected in the group of images to be V-SLAM processed are processed by the initial posture/coordinate system setting unit 513 and the 3D map feature point updating unit, which will be described later. 531, the image feature calculation, image feature matching and triangulation.

For this reason, when processing an image captured by a front-facing camera, in most cases, the conventional method uses two images when the subject starts to appear far away in the processing units 513 and 531. In the method for estimating the position of a moving object according to the form of (1), two images are used when the subject is closest to the camera in order to obtain a retrograde image. Since the subject appears large in these two images, errors in image feature calculation and association are less likely to occur, and it is possible to make incorrect associations, as in conventional cases when the subject appears far away. few.

For this reason, when processing images from a front-facing camera, in the conventional method, image features that should have already been associated remain due to errors in association even when the subject is captured close. The method for estimating the position of a mobile object according to the present embodiment is used to prevent the association with erroneous features that should not be associated, such as fine unevenness, resulting in a large number of erroneous map features. Then such a thing is unlikely to occur.

In addition, in the method for estimating the position of a moving object according to the present embodiment, triangulation for obtaining the 3D initial positions of image features can also be performed using a plurality of images with significantly larger changes in appearance (angle of intersection) than in the conventional art. Therefore, the accuracy of triangulation for estimating the 3D position from the amount of change is improved, so the 3D positional accuracy of the image feature in the environment map is improved.

Furthermore, the 3D position of the feature point has improved initial positional accuracy before optimization, and small changes in the 3D position appear as large changes in the projected position of the feature point on an image in which the feature point appears large. As a result, the accuracy of fine adjustment optimization of the 3D position of the feature point using the difference (reprojection error) between the projected position where the 3D feature point is reflected in the image and the position where the feature actually appears on the image. Easy to improve. This results in improved 3D positional accuracy of the image feature of interest in the environment map (see FIG. 4A below).

FIG. 1A is an explanatory diagram schematically showing an example of a posture graph and optimization in a mobile body position estimation method according to an embodiment; Also, FIG. 1B is an explanatory diagram schematically showing an example of a posture graph and optimization in a moving body position estimation method according to the prior art.

FIGS. 1A and 1B show images 101 to 110 of a subject (feature point A) 100 taken at respective shooting positions of a moving body. The arrow indicates the traveling direction of the moving object. Therefore, each triangle 101 to 110 indicates the position where each image was taken, and the image 101 was taken first, and in chronological order, 102 → 103 → 104 → . . . →109→110.

The feature point A100 is not captured in the triangles 101 and 102 of the moving object, and the feature point A100 appears for the first time in the image of the triangle 103 . After that, the feature point A100 is reflected up to the image of the triangle 103 . Therefore, the group of images in which the feature point A100 is shown is images 103-109.

As shown in FIG. 1B, the conventional method generally uses an image when the subject (feature point A) 100 begins to appear far away. That is, image 103 and image 104 were the image pair for which triangulation was performed. In this case, since triangulation is performed on a pair of images with a small change in appearance (angle of intersection) θb, the accuracy of triangulation for estimating a position from the amount of change is poor.

On the other hand, the moving object position estimation method according to the embodiment uses the image when the subject (feature point A) 100 finally started to be seen, that is, when it was the closest, as shown in FIG. 1A. . That is, images 108 and 109 are taken as an image pair for triangulation. Since triangulation is performed on a pair of images having a large change in appearance (angle of intersection) θa, the accuracy of triangulation for estimating a position from the amount of change is improved.

Note that in the present embodiment, since the image captured by the rearward-facing camera is originally captured in order from the "image in which the subject appears in the camera at its largest" to the image in which the subject appears in the distance, it is necessary to acquire the retrograde image. It is determined that there is no However, in the case of cameras other than the front-back camera, for example, a slightly oblique camera, it may be determined by judging which of the front and rear cameras is closer.

In addition, the retrograde determination process may determine that the frame rate of the target video is not retrograde when the frame rate is smaller than a specified value, and determines that it is retrograde when the frame rate is greater than the specified value.

In a video with a low frame rate, the subject changes greatly between two images, and in particular, when the subject appears near the camera, the change in appearance is greater than when the subject appears far away. For this reason, in video with a very low frame rate, if you try to match the image feature points with two images in which the subject is captured near the camera or perform triangulation using that, the appearance position of the subject's feature points and the Since the appearance has changed greatly, there is a possibility that it cannot be determined as the same feature point.

As a result, even if processing is performed from an image in which a subject is captured near the camera in retrograde image acquisition, the same feature points cannot be determined until the subject is captured at a moderate distance. In addition, the feature points of the environment map can only be used in the V-SLAM after being reflected in the environment map until the subject moves away and the feature disappears. The number is reduced, and the image closest to the camera, which captures the best subject, cannot be used at all.

For this reason, although the initial 3D feature point position accuracy is slightly improved, the registration of the feature points to the environment map is delayed. The benefit of doing the acquisition is greatly reduced.

That is, in FIG. 1B, if the image when the subject (feature point A) 100 begins to appear far away is used, the accuracy is low, but even at a low frame rate, the image 103 including the image when the subject is closest to the camera is used. A full seven images of ~109 are available for V-SLAM. Therefore, the near-field image can be used for optimization with reprojection error.

On the other hand, in FIG. 1A, if the image when the subject (feature point A) 100 was seen last, that is, when the closest image was used, the group of images captured at the close distance would have too much change in appearance and correspond to the feature. The initial 3D position (triangulation accuracy) is somewhat high, but only 5 images, images 103 to 107, can be used for V-SLAM, and the closest object, which has the best image quality, can be used for V-SLAM. Accuracy suffers because the image cannot be used even for optimal computation.

Thus, when the frame rate is low, triangulation is delayed due to time retrogression, the number of images that can use the feature point A100 is reduced, and as a result the error becomes large and there is no retrogression advantage. Therefore, in the method for estimating the position of a moving object according to the present embodiment, it is possible to determine not to obtain a retrograde image for a video with a low frame rate in the case of a front-facing camera.

Also, with a rear-facing camera, when the frame rate is low, the same problem as when acquiring a retrograde image with a front-facing camera occurs, and the environment map is delayed. You may decide to do By performing retrograde image acquisition with a rear-facing camera, V-SLAM processing is performed in order from the image in which the subject is most distant. Since the registration on the map can be performed relatively quickly, there is an advantage that the feature points can be fully utilized in calculating the camera shooting position and orientation using the feature points.

Note that even for video with a low frame rate, parameters for matching feature points, for example, the search range threshold for searching for feature points to be matched, can be set in frames. By changing to an appropriate value that is different from high-rate video, it may be possible to perform feature point matching using images close to the camera, in which case retrograde determination based on the frame rate may be omitted.

In general, the larger the search range, the higher the processing load for the matching process. A judgment is desirable.

In the retrograde determination process, in addition to this, V-SLAM without retrograde image acquisition, that is, the same V-SLAM as the conventional technique is performed once in advance, and the estimation accuracy of the shooting position and posture estimated at that time, or Either the estimation failure situation or the estimation accuracy of the created environment map may be examined, and only if it is particularly degraded may it be decided to implement V-SLAM with time retrogression.

By judging in this way, it is possible to detect retrograde images such as scenes in which road features are often hidden by other subjects (such as other vehicles running in parallel), such as in traffic jams, and the features around the road are often not captured when they are closest to the camera. Even when it is difficult to determine whether acquisition is effective, retrograde images can be acquired only when necessary.

For example, the accuracy of estimating the photographing position/orientation is determined to be low when there are large variations in position or orientation change. In addition, the estimation failure state is obtained by calculating the ratio of the number of images for which the shooting position/orientation can be calculated to the number of images of the video to be processed input by the V-SLAM. The estimation accuracy of the environment map can be determined by, for example, the degree of variation in the position of the 3D position group of feature points in the environment map, and the percentage of feature point groups whose height and position are clearly estimated as abnormal values. It is determined that the accuracy is poor when is large.

In the retrograde determination process, one or more of the determinations shown in these examples may be combined arbitrarily for determination.

(Procedure for retrograde determination processing)
2A and 2B are flowcharts showing an example of the procedure of processing of the mobile position estimation method according to the embodiment. FIG. 2A is a flow chart showing an example of the procedure of the retrograde determination process, and FIG. 2B is a flow chart showing an example of the procedure of the process after the retrograde image acquisition process.

In the flowchart of FIG. 2A, the retrograde determination unit 511 first determines whether or not the camera is facing forward (step S201). Here, if the camera is not facing forward (step S201: No), the process proceeds to step S205. On the other hand, if the camera faces forward (step S201: Yes), it is determined whether or not the shooting frame rate of the camera is equal to or higher than the threshold (step S202).

In step S202, if the shooting frame rate of the camera is equal to or higher than the threshold value (step S202: Yes), it is determined that "backward image acquisition" is performed (step S203). On the other hand, if the imaging frame rate of the camera is not equal to or greater than the threshold value in step S202 (step S202: No), it is determined that "video is not retrogradely acquired" (step S206).

After it is determined in step S203 that "retrograde image acquisition is to be performed", it is determined whether or not there is a conventional V-SLAM result without time retrogression and the shooting position/orientation calculation is satisfactory (step S204). Here, if there is a V-SLAM result and the imaging position/orientation calculation is good (step S204: Yes), there is no need to acquire a retrograde image, and the series of V-SLAM processing ends.

On the other hand, in step S204, if there is no V-SLAM result, or if there is a V-SLAM result but the imaging position/orientation calculation is not good (step S204: No), it is not necessary to acquire a retrograde image. Therefore, the process proceeds to step S211 in FIG. 2B.

Also in step S205 to which the camera is not facing forward (step S201: No), it is determined whether or not the shooting frame rate of the camera is equal to or higher than the threshold value (step S205). Here, if the imaging frame rate of the camera is not equal to or greater than the threshold value (step S205: No), it is determined that "backward image acquisition" is performed (step S203). On the other hand, in step S205, if the shooting frame rate of the camera is equal to or higher than the threshold value (step S205: Yes), it is determined that "images are not retrogradely acquired" (step S206).

After determining in step S206 that "images are not retrograded", it is determined whether or not there is a conventional V-SLAM result without time retrograde (step S207). Here, if there is a V-SLAM result (step S207: Yes), the series of V-SLAM processing ends. On the other hand, if there is no V-SLAM result in step S207 (step S207: No), the process proceeds to step S211 in FIG. 2B.

In the flowchart of FIG. 2B, as a result of the determination, it is determined whether or not it is necessary to acquire the video backward (step S211). The result of the determination is the determination result of "obtain retrograde image" (step S203 in FIG. 2A) or "not retrograde acquire image" (step S206 in FIG. 2A).

In step S211, if it is necessary to acquire images in reverse order (step S211: Yes), the "reverse acquisition flag" for acquiring images in reverse chronological order is turned ON (step S212), and the process proceeds to step S214. On the other hand, if it is not necessary to acquire the video retrograde (step S211: No), the "retrograde acquisition flag" is turned off (step S213), and the process proceeds to step S214.

In step S214, it is determined whether or not all images have been processed (step S214). Here, if all the images have not been processed yet (step S214: No), one image taken last is acquired from the group of unprocessed images in the image using the "backward acquisition flag". (step S215). This is a case where the "reverse acquisition flag" is ON, and if the "reverse acquisition flag" is OFF, it is preferable to acquire one image that has been photographed foremost. The above is the processing executed by the retrograde image acquisition unit 512 .

Then, a feature point group is acquired in the acquired image (step S216), and the shooting position and orientation of the image are estimated from the shooting position and orientation/feature point group map of the latest KF (step S217). In this manner, the photographing position and orientation are estimated from the projection positions of the feature point cloud map, and since the map accuracy is improved, the estimation accuracy is also improved. However, if the frame rate is low, two nearby images are required to be registered on the map, so there is a possibility that nearby features may not be present on the map. The processing up to this point is executed by the frame orientation estimation unit 521, which will be described later.

Next, it is determined whether or not the acquired image is the new KF image (step S218). Whether or not it is a new KF image can be determined by, for example, whether or not the difference from the latest KF is large, or whether or not there is a specified time difference. Here, if the acquired image is not the new KF image (step S218: No), the process returns to step S214 without doing anything.

On the other hand, if the acquired image is the new KF image (step S218: Yes), the process proceeds to step S219. The above is the processing executed by the KF (key frame) update unit 522, which will be described later.

Next, it is determined whether or not there is an uncorrelated feature that is the same as the feature point in the existing KF image (step S219). Here, if there is no uncorrelated feature (step S219: No), the process returns to step S214.

On the other hand, if there are unmatched features (step S219: Yes), triangulation is performed using the in-image feature positions and shooting positions and orientations of the found existing KF image and the current (KF) image ( step S220). Then, the features of the feature points and the three-dimensional positions obtained by triangulation are additionally updated to the environment map (step S221). The above is the processing executed by the 3D map feature point updating unit 531, which will be described later.

After that, the additionally updated environment map and KFs are used to optimize the pose and environment map of the current (KF) image (step S222). The processes up to this point are executed by the graph constraint generation unit 532 and the KF posture/feature point map optimization unit 533, which will be described later.

After that, the process returns to step S214. Then, in step S214, if all the images have been processed (step S214: Yes), the process returns to FIG. 2A and the series of processes ends.

As described above, the method for estimating the position of a moving object according to the present embodiment determines whether or not the video is time-reversed based on the installation position (shooting direction) of the camera video, the frame rate, the result of conventional V-SLAM, and the like. By appropriately performing V-SLAM processing on a group of images that are reversed in time, triangulation can be performed using an image in which the subject and its feature points appear larger in the image, and the 3D position of the feature point This improves the accuracy of the constructed environment map and improves the accuracy of estimating the photographing position/orientation of the camera that is estimated and calculated using the map.

(Comparison of V-SLAM environment map)
FIG. 3A is an explanatory diagram showing an example of a V-SLAM environment map in reverse time in the moving body position estimation method according to the embodiment. FIG. 3B is an explanatory diagram showing an example of a V-SLAM environment map based on forward time in the mobile body position estimation method according to the prior art. Figures 3A and 3B show the comparison.

3A and 3B are overhead images of the 3D positions of the image features of the environment map. 3A and 3B, like FIGS. 4A and 4B to be described later, V-SLAM is used for the same processing section using an image of a mobile object running on a road extending from top to bottom in the center of the image. The created environment map is shown, and each dot represents a map feature point group.

In FIG. 3A, when the reverse image is acquired, the reverse time map feature point group is a map of only the original features because feature correspondence and triangulation are performed on the image in which the subject is shown in a large size, and the 3D position is correctly located around the road. are found to be concentrated. On the other hand, in FIG. 3B, when the reverse image is not acquired, the feature points of the environment map are scattered to places far away from the road vertically existing in the center of the image, and as a result of erroneous feature matching, The erroneous feature correspondence has also increased, and the number of map feature points corresponding to the features of minute unevenness on the road surface has also increased.

In this way, it can be seen that by acquiring the reverse image, the positional accuracy of the surrounding environment map, which is a group of three-dimensional positions of feature points to be generated, is improved, and the scattering of feature point positions due to positional errors is eliminated.

(Comparison of estimation results of shooting position/orientation)
FIG. 4A is an explanatory diagram showing an example of a surrounding environment map in the mobile body position estimation method according to the embodiment. Also, FIG. 4B is an explanatory diagram showing an example of a surrounding environment map in the mobile body position estimation method according to the prior art. FIG. 4A and FIG. 4B show a comparison of estimation results of shooting position/orientation.

4A and 4B are aerial overhead images showing the same travel segment of the processed video. In FIG. 4A, when the reverse image is acquired, the shooting position/orientation can be estimated in almost the entire traveling section. On the other hand, in FIG. 4B, when the reverse image is not acquired, the environment map accuracy is low, so the number of images for which the shooting position/orientation can be estimated is very small.

In this way, the initial timing for calculating the 3D positions of the feature points is "when the subject appears at the farthest point (when the subject starts to appear far away)" in the conventional method, but the moving object according to the embodiment of the present invention is Since the position estimation method makes it possible to change to "when the subject is closest (when it was last seen)", depending on the input image situation, if necessary, the subject can be positioned within a larger image. It can be implemented using an image from which the change is obtained. Therefore, the accuracy of 3D position estimation of feature points estimated from intra-video changes is improved, and a highly accurate surrounding environment map (3D position group of surrounding feature points) can be obtained.

As a result, the positional accuracy of the generated surrounding environment map (three-dimensional position group of image feature points), which had been a problem, has been improved, and the discrepancy between the image content estimated from the environment map and the actual image has not increased. Since SLAM processing can be performed, failure of shooting position/orientation estimation processing (estimation processing cannot be performed thereafter, resulting in an interval where the shooting position/orientation cannot be estimated) is less likely to occur, and the interval for estimating the shooting position/orientation is extended, and accuracy is also improved. can do.

(System configuration example)
Next, the system configuration of the mobile body position estimation system 500 according to the embodiment will be described. FIG. 5 is an explanatory diagram of an example of the system configuration of the mobile position estimation system according to the embodiment.

In FIG. 5, a mobile body position estimation system 500 includes a server 501 that is an example of a mobile body position estimation device, and an on-vehicle device 502 that is an example of an information collection device that collects images and is mounted on a mobile body 503. . The server 501 and the vehicle-mounted device 502 are connected via a network 504 to form a mobile body position estimation system 500 . Further, although not shown, the mobile body position estimation system 500 may realize its functions by a cloud computing system. In-vehicle device 502 may also collect GNSS information from satellite 505 .

The server 501 includes a retrograde determination unit 511, a retrograde image acquisition unit 512, an initial orientation/coordinate system setting unit 513, a frame orientation estimation unit 521, a KF (key frame) update unit 522, and a 3D map feature point update unit. 531 , graph constraint generation unit 532 , KF posture/feature point map optimization unit 533 , and loop detection/closing unit 541 . A control unit of the server 501 can be configured by the configuration units 511 to 513, 521, 522, 531 to 533, and 541. FIG. Details of these components will be described later.

The server 501 also has a real coordinate environment map 550 that stores KF group information 551, feature point group information 552, and the like. Alternatively, server 501 is operably connected to real coordinate environment map 550 .

That is, the real coordinate environment map 550 may be provided (stored) in the server 501, or the real coordinate environment map 550 may be provided in another server (not shown) and stored in the network 504 or the like. It may be connected to the server 501 via a network. Details of the real coordinate environment map 550 will be described later.

The above configuration can be roughly divided into four functional units. The system initialization processing function 510 can be realized by the retrograde determination unit 511 , the retrograde image acquisition unit 512 , and the initial posture/coordinate system setting unit 513 . A position/orientation estimation (tracking) processing function 520 can be implemented by the frame orientation estimation unit 521 and the KF update unit 522 .

An environment map creation (local mapping) processing function 530 can be realized by the 3D map feature point update unit 531, the graph constraint generation unit 532, and the KF posture/feature point map optimization unit 533. A loop close processing function 540 can be implemented by the loop detection/closing unit 541 .

Mobile object 503 is, for example, a connected car that collects information, but is not limited to this. Commercial vehicles such as general passenger cars and taxis, motorcycles (motorcycles and bicycles), large vehicles (buses and trucks), and the like may be used. The moving object 503 may be a ship that moves on water, an aircraft that moves in the sky, an unmanned aerial vehicle (drone), an automatic traveling robot, or the like.

The in-vehicle device 502 collects information about captured images. Also, the vehicle-mounted device 502 may collect information on the mobile object 503 including GNSS information. The information on the mobile object 503 includes posture information of the mobile object 503 collected from the mobile object 503 .

A mobile unit 503 is provided with an on-vehicle device 502 . The in-vehicle device 502 may be a dedicated device mounted on the moving body 503, or may be a removable device. Also, a mobile terminal device having a communication function such as a smart phone or a tablet may be used in the moving object 503 . Also, the functions of the vehicle-mounted device 502 may be realized using the functions of the moving object 503 .

Therefore, the expression "vehicle-mounted" of the vehicle-mounted device 502 is not limited to the meaning of a dedicated device mounted on a mobile object. The in-vehicle device 502 may be any type of device as long as it has a function of collecting information on the mobile body 503 and transmitting the collected information to the server 501 .

The vehicle-mounted device 502 may acquire information (vehicle-mounted data) of the moving object 503 including information on the captured image and GNSS information, and store the acquired vehicle-mounted data. Then, the stored in-vehicle data is transmitted to the server 501 via the network 504 by wireless communication. Also, various data including programs distributed from the server 501 are received by wireless communication via the network 504 .

Also, the vehicle-mounted device 502 may acquire information on another moving object 503 running nearby by using the short-range communication function, and transmit the information to the server 501 . Also, the vehicle-mounted devices 502 may communicate with each other by the short-range communication function, and communicate with the server 501 via other vehicle-mounted devices 502 .

In this manner, in the mobile body position estimation system 500 , the server 501 can acquire vehicle data from the vehicle-mounted device 502 mounted on the mobile body 503 and distribute various data to each vehicle-mounted device 502 .

Also, the vehicle-mounted device 502 does not have to have communication means. That is, the in-vehicle device 502 does not have to be connected to the server 501 via the network 504 . In that case, the data accumulated in the vehicle-mounted device 502 can be input to the server 501 off-line (for example, manually via recording media).

5, the server 501 includes a retrograde determination unit 511, a retrograde image acquisition unit 512, an initial orientation/coordinate system setting unit 513, a frame orientation estimation unit 521, a KF update unit 522, and a 3D map feature point update unit. 531 , graph constraint generation unit 532 , KF posture/feature point map optimization unit 533 , and loop detection/closing unit 541 . Although illustration is omitted, at least one of these functional units may be included in the vehicle-mounted device 502 in addition to the server 501 or instead of the server.

If the vehicle-mounted device 502 has at least one of the functional units 511 to 513, 521, 522, 531, 532, 533, and 541, the contents of the processing performed by the server 501 may be the same. However, the 3D map information may be stored in any medium (DVD/BL disc, HDD, etc.) and used, or may be obtained from an external server (not shown) via a wireless network or the like as appropriate. can be

As described above, the moving body position estimation system 500 includes a retrograde determination unit 511, a retrograde image acquisition unit 512, an initial orientation/coordinate system setting unit 513, a frame orientation estimation unit 521, a key frame (KF) update unit 522, a 3D map feature It has nine V-SLAM-based processing units: a point update unit 531, a graph constraint generation unit 532, a KF posture/feature point map optimization unit 533, and a loop detection/closing unit 541, and a real coordinate environment map 550 (KF group information 551, feature point group information 552) and all image position/orientation data 560 are held internally.

(Hardware configuration example of mobile position estimation device)
FIG. 6 is a block diagram showing an example of the hardware configuration of the mobile body position estimation device. A server 501, which is an example of a mobile position estimation device, has a CPU (Central Processing Unit) 601, a memory 602, a network I/F (Interface) 603, a recording medium I/F 604, and a recording medium 605. . Also, each component is connected by a bus 600 .

Here, the CPU 601 controls the entire server (mobile body position estimation device) 501 . The memory 602 has, for example, ROM (Read Only Memory), RAM (Random Access Memory), flash ROM, and the like. Specifically, for example, a flash ROM or ROM stores various programs, and a RAM is used as a work area for the CPU 601 . A program stored in the memory 602 is loaded into the CPU 601 to cause the CPU 601 to execute coded processing.

The network I/F 603 is connected to the network 504 via a communication line, and via the network 504, another device (for example, the vehicle-mounted device 502, the device in which the real coordinate environment map 550 and the total image position and orientation data 560 are stored, or other servers or systems). A network I/F 603 serves as an interface between the network 504 and the inside of the device itself, and controls input/output of data from other devices. For network I/F 603, for example, a modem, LAN adapter, or the like can be adopted.

A recording medium I/F 604 controls reading/writing of data from/to the recording medium 605 under the control of the CPU 601 . The recording medium 605 stores data written under control of the recording medium I/F 604 . Examples of recording medium 605 include a magnetic disk and an optical disk.

Note that the server 501 may have, for example, an SSD (Solid State Drive), a keyboard, a pointing device, a display, etc., in addition to the components described above.

(Hardware configuration example of in-vehicle device)
FIG. 7 is a block diagram showing an example of the hardware configuration of the vehicle-mounted device. A vehicle-mounted device 502 , which is an example of an information collection device, has a CPU 701 , a memory 702 , a wireless communication device 703 , a mobile body I/F 704 , an imaging device 705 , and a receiving device 706 . Also, each component is connected by a bus 700 .

The CPU 701 controls the entire vehicle-mounted device 502 . The memory 702 has, for example, ROM, RAM and flash ROM. Specifically, for example, a flash ROM or ROM stores various programs, and a RAM is used as a work area for the CPU 701 . A program stored in the memory 702 is loaded into the CPU 701 to cause the CPU 701 to execute coded processing.

The wireless communication device 703 receives and transmits radio waves that have been transmitted. A configuration including an antenna and a receiving device, and a function of transmitting and receiving communication such as mobile communication according to various communication standards (specifically, 3G, 4G, 5G, PHS communication, etc.), Wi-Fi (registered trademark), etc. It has

The mobile unit I/F 704 serves as an interface between the mobile unit 503 and the vehicle-mounted device 502 and controls input/output of data from the mobile unit 503. Therefore, the vehicle-mounted unit 502 uses the mobile unit I/F 704 Information is collected from an ECU (including various sensors and the like) 707 provided in the moving body 503 via the mobile body 503 . Specifically, the mobile unit I/F 704 may be, for example, a connector used for wired connection, a short-range wireless communication (specifically, for example, Bluetooth (registered trademark)) device, or the like.

A receiver (for example, a GNSS receiver such as a GPS (Global Positioning System) receiver) 706 receives radio waves from a plurality of satellites 505 and calculates the current position on the earth from information included in the received radio waves.

An imaging device (for example, a camera) 705 is a device that captures still images and moving images. Specifically, for example, it is a configuration including a lens and an imaging device. An image captured by the imaging device 705 is stored in the memory 702 . In addition, the imaging device 705 such as a camera may have an image recognition function, a barcode and QR code (registered trademark) reading function, an OMR (Optical Mark Reader) function, an OCR (Optical Character Reader) function, and the like. .

As shown in FIG. 7, an imaging device (such as a camera) 705 and a receiving device (such as a GNSS receiving device) 706 may be included in the vehicle-mounted device 502, or may be included in the mobile unit 503, or may be separately provided externally. You may make it use what was attached. At this time, data exchange with the imaging device 705, the receiving device 706, and the vehicle-mounted device 502 may be performed by wired or wireless communication.

If the vehicle-mounted device 502 does not have the imaging device 705 or the GNSS receiver 706, the information thereof may be acquired via the mobile I/F 704 or the like. Although not shown, the in-vehicle device 502 may include various input devices, a display, an interface for reading and writing recording media such as memory cards, and various input terminals.

(Contents of real coordinate environment map)
FIG. 8 is an explanatory diagram showing an example of the data configuration of the real coordinate environment map. 8, the real coordinate environment map 550 has KF group information 551, feature point group information (three-dimensional position information) 552a, and feature point group information (KF intra-image position information) 552b.

Here, what corresponds to the environment map of the existing technology is the real coordinate environment map 550, and in addition to the three-dimensional position of each image feature point (feature point group information (three-dimensional position information) 552a), which image feature A point has information about which image was viewed (used). This is represented by two feature point group information (KF image position information) 552 b and KF group information 551 . KF group information 551, which is information on the image (KF) group in the main video, and feature point group information (KF image position information), which is information on the two-dimensional position where each image feature appears on the KF image. 552b is information essential for using the environment map for estimating the position and orientation of any image.

As shown in FIG. 8, the KF group information 551 includes "ID", "parent KF ID", "child KF ID", "loop KF ID", "attitude information", "position information", "feature amount", It has various information including "GNSS position" and "video frame number". In addition, it is not necessary to have information about the "GNSS position".

Here, "ID" is unique identification information that identifies the information of the KF, "parent KF ID" and "child KF ID" are information that connects KFs, and "loop KF ID" is This information is used for loop closing processing and the like.

"Position information" and "Position information" are estimated shooting position and orientation information of KF, and "Feature amount" is the entire image used to judge whether or not an arbitrary image is similar. "GNSS position" is the GNSS position at the time of shooting of the KF corresponding to the new input GNSS information, and "video frame number" is the frame number of the corresponding video.

As shown in FIG. 8, the feature point group information (three-dimensional position information) 552a has various information including "ID", "position coordinates", "feature amount" and "ID group of observed KF".

Here, the “ID” is unique identification information for identifying the feature point information, the “positional coordinates” are the actual coordinates of the estimated feature point, and the “feature amount” is the image feature. , “ID group of observed KF” is information of the KF in which the feature point is shown, and “ID” of the corresponding KF information in the KF group information 551 is associated. Note that the actual coordinate position coordinates may be stored as local values assuming that they are converted into actual coordinates using arbitrary real coordinate transformation created by the initial attitude/coordinate system setting unit.

The feature point group information (positional information in the KF image) 552b is information of the image feature point group extracted from the KF image, and is simultaneously viewed from a plurality of KF images and selected to have a feature point group having a three-dimensional position. , there are two types of feature points that do not have 3D positions. A KF feature point group that does not have a three-dimensional position is used for detailed evaluation of whether an arbitrary image resembles the KF image, or is newly selected when a new KF image is obtained in the future, and the three-dimensional position is used. It is held in preparation for becoming a feature point group having.

As shown in FIG. 8, the feature point group information (KF intra-image position information) 552b includes "ID", "KF ID", "map point ID", "feature point position", "feature point angle" and "reduction It has various information including "hierarchy number".

Here, "ID" is unique identification information for identifying the feature point information. The “KF ID” is information for specifying the KF that extracted the KF feature point, and is associated with the “ID” of the corresponding KF information in the KF group information 551 . "Map point ID" is reference information to the feature point group information (three-dimensional position information) 552a, and "ID" of the corresponding feature point information in the feature point group information (three-dimensional position information) 552a is associated be done. This "map point ID" is possessed only by feature point groups that are simultaneously browsed and selected from a plurality of KF images and have three-dimensional positions, and not possessed by feature point groups that do not have three-dimensional positions.

Also, the "feature point position" and "feature point angle" are, for example, information relating to the barycentric position and direction vector of ORB (Oriented FAST and Rotated Brief) features. Also, the "reduced hierarchy number" is information regarding the extraction status within the KF image. For example, when assuming ORB feature points calculated using a group of reduced images obtained in a pyramid hierarchy with different reduction ratios as image features, this "reduced layer number" is extracted from which group of reduced images. This is information about whether the If other image features are used, these "feature point positions", "feature point angles", and "reduced hierarchy numbers" may be information matching the features.

Thus, the real coordinate environment map 550 is formed, and the KF group information and the feature point group information are associated and stored. In general, the feature point-based V-SLAM environment map includes image features and three-dimensional positions of the image feature point group, information on the image KF viewing the feature point group, and information similar to the image KF. The real coordinate environment map 550 may be the same data as the conventional V-SLAM environment map, although it contains the image features in the image KF to allow the image to be searched. Also, the real coordinate environment map 550 may hold “GNSS position” information in the KF group information 551 .

(Contents of all image position and orientation data)
FIG. 9 is an explanatory diagram showing an example of the data configuration of all image position/orientation data. The all-image position/orientation data 560 holds the estimated photographing positions and orientations for all the images in the video, unlike the KF composed of the main images. Here, the all-image position/orientation data 560 corresponds to the photographing positions/orientations of all the images in the video of the existing technology.

As shown in FIG. 9, the all image position/posture data 560 has various information including "ID", "parent KF ID", "posture information", "position information", and "video frame number". Here, “ID” is unique identification information that identifies the position and orientation data. "Parent KF ID" is information of a KF that refers to a position/orientation that is visually nearby. "Posture information" and "position information" are the relative position and posture from the parent KF, and "video frame number" is the frame number of the corresponding video.

The position/orientation information is stored, for example, as a relative position/orientation with respect to the KF that is close to the image, and when the V-SLAM result is finally output, the position/orientation of the KF is reflected in the real coordinate values. do. In this way, when V-SLAM is sequentially processed, the position/orientation of all images can be adjusted to the final position/orientation without worrying that the position/orientation of the KF changes during the optimization process. It can be easily calculated according to the position and orientation of the KF. Further, the position and orientation information may be held as local values even in real coordinate values, like the KF.

As can be seen from FIGS. 8 and 9, in this example, information about the position and orientation of the KF is held separately from all image positions and orientations together with other information of the KF. The all-image position/orientation is the shooting position/orientation of all image frames of the video, and the KF position-orientation information included in the KF group information 551 of the real coordinate environment map 550 is the shooting position/orientation of the KF image, which is a partial image in the video. Since it is a position/orientation, it may be included in the total image position/orientation data 560 .

Also, the total image position and orientation data 560 may be the same data as in conventional V-SLAM.

Although not shown, the moving object position estimation system 500 of FIG. 5 also includes various information such as the real coordinate environment map 550 and the all image position/orientation data 560 shown in FIGS. , may additionally hold various information for speeding up the V-SLAM calculation using the real coordinate environment map. For example, KFs sharing a map feature point group having a three-dimensional position in the image KF group, and among them, a KF group with the largest number of feature point groups shared, etc. are held, and each KF is may refer to each other with

More specifically, for example, it is a covisibility graph, and may be held as graph-structured data in which each KF is a node, a group of KFs sharing map feature points with edges, and the number of map feature points sharing the edge weights. . These speed up the search for the optimization calculation target of the KF position/orientation and environment map in local mapping processing, etc., which will be described later, and search for an image similar to the current image frame in loop closing processing, etc. can be used for

(Details of mobile object position estimation system)
FIG. 10 is an explanatory diagram showing an example of contents of the mobile body position estimation system and the mobile body position estimation method according to the embodiment.

10, input data of an image 1001 from a camera or the like, a retrograde determination unit 511, a retrograde image acquisition unit 512, an initial orientation/coordinate system setting unit 513, a frame orientation estimation unit 521, a key frame (KF) update unit 522, a 3D map feature point updating unit 531, a graph constraint generation unit 532, a KF pose/feature point map optimization unit 533, and a loop detection/closing unit 541. The environment map 550 (KF group information 551, feature point group information 552), all image position/orientation data 560, two internally stored data, and the data of the initial environment map 1010 may be included. At least one of the internally held data can be output as output data (real coordinate environment map 550', all image position/orientation data 560'). Also, the input data of the GNSS information 1002 acquired at the same time as the image 1001 may be included.

Since the mobile position estimation system 500 according to the present embodiment is based on the conventional V-SLAM technology, part of the processing of each processing unit performs the same processing as the conventional V-SLAM processing. You may do so. In this embodiment, a basic processing example of feature point-based V-SLAM, particularly ORB-SLAM using ORB features, will be given as conventional V-SLAM, and the difference from conventional V-SLAM processing will be shown. are described below.

(Contents of input information)
Information of image 1001 , GNSS information 1002 , and attitude information 1003 is input to mobile body position estimation system 500 . Note that the GNSS information 1002 may not be input. The image 1001 is input to the retrograde determination unit 511 , the GNSS information 1002 is input to the initial attitude/coordinate system setting unit 513 , and the attitude information 1003 is input to the graph constraint generation unit 532 . However, the GNSS information 1002 input to the initial attitude/coordinate system setting unit 513 and the attitude information 1003 input to the graph constraint generation unit 532 may not be essential input information.

An image 1001 is an image captured by an imaging device 705 of an in-vehicle device 502 mounted on a moving body 503 such as a vehicle. It can be obtained by an arbitrary method such as using communication means of the vehicle such as the in-vehicle device 502 or manually using a recording medium, and can be used as an input of the system 500 . Further, since the internal parameters of the imaging device 705 that captured the image are known, and the distortion correction is performed as appropriate, the internal parameters are assumed to be known since they are used for image distortion correction and the like.

The GNSS information 1002 is the position of the moving object 503 at the time of video shooting, is data obtained by any existing positioning means such as GPS, is obtained by any method equivalent to video, and is input to the system 500. be able to.

Note that the GNSS information 1002 is newly used to correct the scale drift of V-SLAM due to video, and is preferably held in all frames of the video as much as possible, but is not necessarily held in all frames. good too. The more frames that are retained, the better the position and pose accuracy of the overall image pose and real coordinate environment map output by the system.

In addition, as will be described later, at least two image frames near the video analysis start point used in the initialization of this system must hold GNSS information. , the initialization process can be completed at an early stage from the start of the video, and the shooting position/orientation estimation process can be performed. However, the GNSS information 1002 may not be held.

Similarly, the GNSS information 1002 should be as accurate as possible, and the higher the accuracy, the better the position and attitude accuracy of the output of the system. Also, GNSS information is often the position of a GPS receiver or the like, but it is desirable to convert it into camera position information as much as possible using the relative positional relationship between the GPS receiver and the camera.

Also, the posture information 1003 is camera posture information obtained from an arbitrary IMU (inertial measurement unit) or the like when the image was shot. The IMU is specifically an acceleration sensor, a gyro sensor, or the like. For example, it is the rotation angle, roll, pitch, yaw angle, etc. with respect to the coordinate axes such as the front, right, and vertically above the vehicle centered on the camera. As with the GNSS information, it may be held for all images of video, or may be held only for arbitrary images.

Note that the GNSS information 1002 and the orientation information 1003 are not separately obtained from the sensor group as described above, but are corrected by any method, such as manual work, after each camera shooting position and orientation estimated by V-SLAM. Then, the corrected photographing position/orientation of each camera may be read again as the GNSS information 1002 and the orientation information 1003 of the same image.

In addition, a local mapping function, which will be described later, may be used to appropriately merge and reflect both the position based on the input GNSS information 1002 and the position based on the analysis result of the image 1001 for estimation. As a result, it is possible to smoothly create a real coordinate environment map including a group of feature points in accordance with the result of manual correction through re-implementation using the output result of manual correction as input. However, such an estimation in which both the position based on the GNSS information 1002 and the position based on the analysis result of the image 1001 are properly merged and reflected may not be used.

In the first execution, the orientation information is estimated and output together with the position information even if the orientation information is not input. Location information alone may be used as input. For example, unlike the manually corrected position information, if the posture information cannot be manually corrected at all, the likelihood of the two pieces of information is different. Inputs can be used during execution to create a real coordinate environment map.

Also, the real coordinate environment map 550' that has been output once may be used again as an input. For example, when estimating the photographing position/orientation of the first running video on a certain track, there is no real coordinate environment map, so the processing in this system is executed without inputting the real coordinate environment map, and then the same In the case of estimating the photographing position/orientation of the running video for the second time or later after running on the track, the real coordinate environment map 550′ output as the processing result of the first running video is input, and created as if by processing the relevant video. can be used as if it were internal data. At this time, the vehicle, the camera, the position in the driving lane, and the like may be different between the first and second and subsequent driving images. Note that input of the GNSS information 1002 may be omitted when the real coordinate environment map 550' is input.

Note that if the onboard device 502 has at least one of the functional units 511 to 513, 521, 522, 531, 532, 533, and 541, the video 1001 and the GNSS information 1002 are held inside the onboard device 502. may be used to process V-SLAM.

In the following explanation of this system, unless otherwise specified, the case where there is no real coordinate environment map input and the real coordinate environment map is created from scratch (when the input of GNSS information 1002 is required) will be explained. However, the input of the GNSS information 1002 may not necessarily be required. Also, as the GNSS information 1002, the values of the planar rectangular coordinate system will be described as an example.

(Contents of retrograde determination unit 511)
The retrograde determination unit 511, which is in charge of the system initialization processing function 510, is a processing unit that determines the operation of the retrograde image acquisition unit 512. Based on the results of SLAM, etc., it is determined whether or not to time-reverse (reverse in time) the input video.

Specifically, the function of the retrograde determination unit 511 can be realized by the CPU 601 executing a program stored in the memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

(Contents of retrograde image acquisition unit 512)
When the retrograde image acquisition unit 512, which is in charge of the system initialization processing function 510, determines to “reverse time” in the retrograde determination unit 511, the subsequent V-SLAM processing unit moves the image in the direction opposite to the passage of time. The video is acquired backwards so that it can be processed into For example, each image is extracted from the input video file so that the shooting time goes back, and is passed to the subsequent V-SLAM processing unit.

Specifically, the retrograde image acquisition unit 512 can realize its function by the CPU 601 executing a program stored in the memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

(Contents of Initial Posture/Coordinate System Setting Unit 513)
An initial posture/coordinate system setting unit 513 in charge of the system initialization processing function 510 determines the coordinate system to be calculated, and creates internal data necessary for subsequent processing functions such as tracking as initialization processing. Specifically, the three-dimensional position of the feature point group near the start of the video is estimated, and the initial KF position and orientation are estimated. Create a neighborhood real coordinate environment map. The processing of the initial posture/coordinate system setting unit may be the same as the initial processing of the conventional V-SLAM except for the processing of determining the coordinate system used for calculation. If this initialization process is not completed, subsequent processes including frame attitude estimation are not executed, as in the conventional V-SLAM.

Specifically, initial attitude/coordinate system setting unit 513 can realize its function by CPU 601 executing a program stored in memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

The initial posture/coordinate system setting unit 513 first acquires an arbitrary image feature group for each image of the distortion-corrected video. Next, each image feature group is searched for feature points (pairs of feature points in each image) that appear simultaneously in the first two frames. The method of searching for a pair depends on the image feature to be used, and an existing method of searching for the same feature pair of two images may be used. When the calculated number of pairs is sufficiently large, the change in the image of the feature point group is used to estimate the position/posture change of the camera and the three-dimensional position of each feature point group that appears in both images.

That is, from the position and orientation changes of each feature point pair reflected in two images, existing methods, for example, homography assuming a plane or a method using a geometric model such as a basic matrix assuming a non-plane, are used to convert two images. In addition to estimating the transformation that expresses the change in the camera position/orientation of the two images, the estimated camera position/orientation of the two images and the position of each feature pair on each image are used to calculate each Estimate the 3D position of the feature.

If the number of pairs is insufficient, one of the two images (for example, the image at the later time) is replaced with the other image (for example, the image at the later time) and this process is performed. Also, the first two frames to be used may not strictly be the image at the start of the video, but may be arbitrary two frames that may include the same subject. For example, if it is known that the image is taken while the vehicle is stopped, an image that seems to have been taken with a different camera position may be selected as an image at a later time.

At this time, instead of calculating the three-dimensional positions of all feature point group pairs, feature points with large errors compared to other feature point groups may be omitted, or a specified number of feature points may be obtained evenly over the entire image. In the image part where the feature point group is concentrated, the feature point group is thinned out, and the feature point with a small angle (angle of intersection) formed by the two camera positions and the feature point is omitted. Selection may be performed.

Further, the initial attitude/coordinate system setting unit 513 may perform optimization calculations in the same manner as in the conventional V-SLAM, and may add processing to update the calculated initial values to more accurate values. Specifically, for each of the two images, the camera position and the three-dimensional position of the feature point group are known, and it is possible to calculate how the feature point group is reflected in each image. Investigate the difference (called reprojection error) between the position of the feature point group reflected in the camera image and the position of the feature point in the actual camera image. Alternatively, optimization correction (BA (Bundle Adjustment)) for finely adjusting the camera position and orientation may be performed.

Subsequently, the initial attitude/coordinate system setting unit 513 creates an initial environment map 1010 from the calculation results. That is, the two images used are registered in the initial environment map 1010 together with the shooting positions and orientations of the images estimated as KF, and the information of the similarly estimated feature point group (the position on the two images and the three-dimensional position) is also Register in the initial environment map 1010 . This initial environment map 1010 is an initialization process, and estimates the KF position/orientation and the three-dimensional position of the feature point group by a method slightly different from the method performed by the subsequent functional units such as tracking and local mapping. , slightly less accurate.

In addition, in the processing of calculating the initial values of the camera position/orientation of these two images and the three-dimensional position of the feature point group around the vehicle, one of the two images (often the An earlier image, hereinafter referred to as the ``initial camera image'') may be calculated in a local system with the camera position/orientation (hereinafter referred to as the ``initial camera position/orientation'') as the origin and the reference coordinate system. .

For example, in a pixel coordinate system for indicating pixel positions on an image generally used in image processing, it is often the case that X is the horizontal direction of a photographed image and Y is the downward direction of the image. For this reason, in order to define a reference coordinate system similar to this in the conventional V-SLAM, the camera position in the initial frame is defined as the origin (0, 0, 0), the vehicle's right-hand direction X, and the vehicle's vertical downward direction Y , and Z in the forward direction of the vehicle are often defined as right-handed system (SLAM local system) definitions. Also in this system, the SLAM local system calculates the camera position/orientation of the two images and the three-dimensional position of the group of feature points that appear in common in the two images.

In this way, the initial orientation/coordinate system setting unit 513 performs the process of creating the initial environment map 1010 (estimating the KF position/orientation and estimating the three-dimensional position of the feature point group) in the same manner as in the conventional V-SLAM. Do.

Next, the initial orientation/coordinate system setting unit 513 sets the input GNSS information so that the KF imaging position/orientation of the environment map calculated in the SLAM local coordinate system and the three-dimensional position of the feature point group correspond to the real coordinate system. GNSS position coordinate values corresponding to the two images are obtained from 1002, and a conversion matrix between the SLAM local (coordinate) system and the real coordinate system is calculated. Note that when calculating the conversion matrix between the SLAM local (coordinate) system and the real coordinate system, it is not necessary to obtain the GNSS position coordinate values corresponding to the two images from the input GNSS information 1002 .

11A to 11C are explanatory diagrams showing an example of conversion matrix calculation in the initial attitude/coordinate system setting unit 513. FIG. As shown in FIG. 11A, the present system 500 uses a planar rectangular coordinate system as the real coordinate system. Specifically, reference numeral 1101 indicates (a) SLAM local system (right-handed system). Specifically, with respect to the origin (initial camera), the X direction indicates the right direction, the Y direction indicates the downward direction, and the Z direction indicates the traveling direction.

On the other hand, reference numeral 1102 indicates (b) a real coordinate system, that is, a planar rectangular coordinate system (left-handed system). Specifically, the X direction is "north" with respect to the planar rectangular coordinate system origin (0, 0, 0), that is, the planar rectangular coordinate system X value [m], and the Y direction is "east". , that is, the plane rectangular coordinate system Y value [m] is indicated, and the Z direction is "up", that is, the altitude value [m] is indicated.

However, this is only an example, and instead of using a left-handed planar rectangular coordinate system different from the right-handed SLAM local system of the conventional V-SLAM, any right-handed coordinate system may be used. good.

FIG. 11B shows motion vectors. Reference numeral 1103 denotes a movement vector A of the local system, and reference numeral 1104 denotes a movement vector B of the real coordinate system. A motion vector is a difference ( F2-F1) is a traveling direction vector. As shown in FIG. 11B, the same movement vector is expressed in two coordinate systems (movement vector A1103 and movement vector B1104). Therefore, the initial posture/coordinate system setting unit 513 calculates a transformation matrix (transformation matrix M from the local system to the real coordinate system) for transformation into values of another system.

FIG. 11C shows the contents of the conversion matrix M for conversion from the SLAM local system to the values in the real coordinate system. In FIG. 11C, the conversion matrix M for converting from the SLAM local system to the real coordinate system is a scale conversion matrix M1 for absorbing the scale difference between the coordinate systems, Rotation matrix M2 for converting the coordinate axis to values, M3 for changing the XYZ coordinate axis definition, M4 for converting from the right-handed system to the left-handed system, and the origin from the initial camera position to the planar rectangular coordinate system It consists of the multiplication of the five matrices of M5, which transforms the values modified to the origin of M5.

The scale conversion matrix M1 is a conversion matrix that uses an arbitrary scale due to image change as the scale of the real coordinates. (1) SLAM local system 1111 can be transformed into (2) m-scale SLAM local system 1112 by scale transformation matrix M1.

FIG. 11D is an explanatory diagram illustrating an example of calculation of a scale conversion matrix M1 that converts an arbitrary scale derived from an image change to a scale [m] of a coordinate system of latitude and longitude.

In FIG. 11D, first, from the SLAM local system two-image camera positions Q1 (corresponding to previous time image F1) and Q2 (same time image F2), the difference (each position difference, Q2-Q1) is a movement vector A1103 Calculate The component definition may be the SLAM local system itself (conventional V-SLAM output value).

Next, from the GNSS positions S1 (corresponding to the previous time image F1) and S2 (same time image F2) of the two images, the values of the real coordinate system (rectangular plane coordinate system) are used, but the component (axis) definition is different. A movement vector B1104 (=S2-S1) in the special real coordinate system is calculated. The special real coordinate system is (X component=longitude coordinate value difference with positive east, Y component=-(elevation value difference), Z component=latitude coordinate value difference with north positive).

Then, the magnitude of motion vector A1103=lenA and the magnitude of motion vector B1104=lenB are obtained. A size ratio Rate=(lenB/lenA) is obtained from the obtained magnitude, and a scale conversion matrix multiplied by Rate is obtained as a scale conversion matrix M1. When Eye (m, n) is represented as a unit matrix of m rows and n columns, the scale conversion matrix M1 is
M1=Rate×Eye(3, 3);

Returning to FIG. 11C, the rotation matrix M2 is a conversion matrix that changes the coordinate system derived from the traveling direction to that derived from latitude and longitude. (2) m-scale SLAM local system 1112 can be transformed into (3) special real coordinate system 1113 by rotation matrix M2.

FIG. 11E is an explanatory diagram showing an example of calculation of the rotation transformation matrix M2. In FIG. 11E, first, the motion vectors are divided by their respective lengths to obtain a normalized local system motion vector A'=A/lenA and a normalized real coordinate system motion vector B'=B/lenB.

Next, as indicated by reference numeral 1105, (a) the angle Θ between vector A' and vector B' is obtained from the inner product.
Θ = acos (inner product (A', B'))

Then, as indicated by reference numeral 1106, (b) the cross product of vector A' and vector B'=A'.times.B', an upward vector (VectorUP) is obtained, and an angle .THETA.' is calculated in consideration of the direction. If the Y value of the upward vector is positive, the angle Θ'=−Θ, and if negative, the angle Θ'=Θ.

Since the conversion of the axis definition and the conversion of the coordinate values are reversed, the (-Θ') rotation matrix around the Y-axis is the matrix M2.

Returning to FIG. 11C, the real coordinate system definition transformation matrix M3 is a transformation matrix that rotates −90 degrees around the X axis. (3) special real coordinate system 1113 can be transformed into (4) special real coordinate systems 2 and 1114 by the real coordinate system definition transformation matrix M3.

The real coordinate system definition transformation matrix M4 is a transformation matrix for transforming from the right-handed system to the left-handed system. Convert X and Y to each other. The (4) special real coordinate system 2, 1114 can be transformed into (5) a real coordinate system, that is, a planar rectangular coordinate system (left-handed system) 1115 by the real coordinate system definition transformation matrix M4. Here, the origin position differs from the original planar rectangular coordinate system.

The position movement transformation matrix M5 is a transformation matrix for moving the origin position. Specifically, the initial actual coordinate position of the camera (initial camera position) is translated, and the initial camera position is set to X [m] in the plane rectangular coordinate system, Y [m] in the plane rectangular coordinate system, and the altitude [m]. The (5) planar rectangular coordinate system (left-handed system) 1115 can be transformed into (6) the original planar rectangular coordinate system (left-handed system) 1116 by the positional movement transformation matrix M5.

In this way, the SLAM local system 1111 (1101 shown in FIG. 11A) can be made into real coordinates (planar rectangular coordinate system (left-handed system)) 1116 (1102 shown in FIG. 11A).

By holding this coordinate system conversion matrix M, the initial posture/coordinate system setting unit 513 converts the initial environment map in the SLAM local coordinate system, which has been calculated in the same manner as in the conventional V-SLAM, into the plane rectangular coordinate system. It can be converted into a real coordinate environment map. As a result, by using the scale of the SLAM local system, which is derived from an image and has no particular meaning in terms of size, in this system, it becomes possible to unify the scale to the real coordinate scale in units of m.

If necessary, the initial orientation/coordinate system setting unit 513 not only has a transformation matrix, but also sets the three-dimensional initial position of the feature point group that has actually been calculated, the shooting position/orientation position of the two images, This conversion matrix may be used to convert to values in the real coordinate system. In particular, the positions of feature point groups that are frequently referenced should be stored as values in the real coordinate system, as they will be used as unified values before and after reinitialization when the tracking processing function (frame orientation estimation), which will be described later, fails. is desirable. If the values of the real coordinate system are stored in advance, the projection position onto each image can be calculated without the conversion.

On the other hand, the values of the real coordinate system such as the plane Cartesian coordinate system often have very large numerical values, so the 3D position of the feature point group in the environment map is kept as the value of the local coordinate system, which is the same as before. In addition, by newly holding a transformation matrix, the transformation matrix may be used to transform into values in the real coordinate system only when necessary. Alternatively, it may be a value in the real coordinate system or a difference value from an appropriate initial value.

In this system 500, the initial attitude and coordinate system are set in the same local coordinate system as in the conventional system, and then information for conversion to the real coordinate system is created. Description will be made assuming that the values in the real coordinate system used are held.

When inputting an existing real coordinate environment map, the processing of the initial attitude/coordinate system setting section is skipped. do.

In this system 500, as in the conventional V-SLAM, when the processing of the initial attitude/coordinate system setting unit 513 is performed on two images (initial KF), it is assumed that the initialization is completed, and the subsequent processing is performed. are sequentially performed on images that have not yet been processed. Therefore, subsequent processing is not performed on the two images (initial KF) used for initialization, but is performed on subsequent images.

It should be noted that the initial attitude/coordinate system setting unit 513 determines the coordinate system to be calculated and creates internal data necessary for subsequent processing functions such as tracking as initialization processing, and is not limited to the above method. , a method other than the above method may also be used.

The tracking processing function, the mapping processing function, and the loop closing processing function, which are subsequent processes, are assumed to be sequentially processed for the sake of simplicity of explanation. In practice, it may be simultaneous processing using multiple threads. In that case, each processing function mutually references the internally held KF position/orientation and the real coordinate environment map, so it is necessary to prevent simultaneous editing in multiple processes by appropriately using the existing edit lock function. can be done. Each processing function sequentially processes each image of the video until there are no more images to be processed.

(Contents of Frame Orientation Estimation Unit 521)
In FIG. 10, a frame orientation estimation unit 521 in charge of a position/orientation estimation (tracking) processing function 520 performs the conventional V-frame orientation estimation unit 521 except for processing when normal processing fails (processing when relocalization fails, which will be described later). The same processing as SLAM is performed. That is, the frame orientation estimation unit 521 calculates an image feature group for the input new image (distortion corrected) after camera movement, and compares the image feature amounts to calculate the calculated 3D feature point group that is considered to be the same feature point. position (real coordinate environment map 550).

At this time, assuming that the vehicle is traveling at a constant speed, the initial position/orientation of the new image camera is estimated, and the 3D feature point group used in the previous image is projected onto the new image using the estimated initial position/orientation. Then, by retrieving corresponding feature points in the vicinity thereof, 3D feature point candidates that are considered to be the same feature point may be narrowed down.

After that, the position/orientation of the new image camera is optimized so that the reprojection error onto the new image is small for the entire 3D feature point group found as the same feature point. That is, BA is performed to optimize only the camera position/orientation without changing the position of the 3D feature point group. Subsequently, searching for a first KF group sharing a 3D feature point group with the new image, then searching for a second KF group sharing a 3D feature point group with the first KF group 1, One KF group, a second KF group of 3D feature points are obtained.

At this time, for the obtained 3D feature point group, the distance from the camera position of the new image (within the range of specified distance) and the difference in the viewing direction from the camera (for example, from the camera position of the new image to the 3D feature point The size of the inner product of the directed viewing direction vector and the viewing direction vector directed from the camera position of the KF group so far to the feature point is greater than or equal to a specified value). . Using more 3D feature point groups obtained from the first KF group and the second KF group, the frame orientation estimation unit 521 projects again onto the new image, and reprojects the position such that the reprojection error becomes small. - Perform posture optimization.

Specifically, the function of frame orientation estimation section 521 can be realized by CPU 601 executing a program stored in memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

Note that if the frame orientation estimation unit 521 fails to estimate the position/orientation due to reasons such as insufficient 3D feature point groups being obtained, the position/orientation restoration processing is performed in the same manner as in the conventional V-SLAM. Some relocalization process may be performed. In the relocalization process, all KF groups are searched for KFs similar in image characteristics, and when a similar KF candidate group is found, the 3D feature point group of these KFs and the feature point group of the new image are matched. , a KF with a large matching number may be selected as the final KF.

The relocalization process then uses the matched feature points of the KF and the new image to perform an initial position and pose estimation by solving a known PnP problem with a smaller number of feature points. Then, from the initial position/posture of the obtained new image, the position/posture is optimized using an arbitrary optimization method such as the nonlinear least-squares method using a larger number of feature points. This is the estimated camera position/orientation of the image.

Up to this point, the frame orientation estimation unit 521 of the present system 500 performs the same processing as in conventional V-SLAM. On the other hand, frame orientation estimation section 521 of system 500 differs from conventional V-SLAM in the processing performed when the relocalization processing described above also fails. If the relocalization process also fails, the conventional V-SLAM cannot continue the process, so the process is terminated. However, in the case of the present system 500, instead of terminating the process, it returns to the initial orientation/coordinate system setting unit 513, leaving the existing internally calculated data such as the actual coordinate environment map 550 and all image position and orientation data 560. , the initialization process can be re-performed.

In the conventional V-SLAM, relocalization failure means that the images and the KF group that have been tracked so far cannot be matched at all. As described above, in the conventional V-SLAM, the SLAM local system to be calculated is a coordinate system related to the initial image used in the initialization. However, since the calculation is started in a coordinate system related to a new initial image that is different from the environment map calculated so far, the environment to be calculated will be different until the point where correspondence is practically lost and after starting initialization again. The map and the camera position/orientation values cannot be matched, so they are essentially separate pieces.

Therefore, in the conventional V-SLAM, when relocalization fails, there is no point in performing the initialization process, so the process ends without performing the initialization process. However, in the present system 500, the coordinate system after initialization and V-SLAM values can all be real coordinate systems as an initialization process, so that the correspondence between the images and the KF group tracked so far is Even if they cannot be obtained, the values of the environment map and the camera position/orientation to be calculated are consistent values as long as they are the values of the real coordinate system.

As a result, there is no problem even if the values of the environment map and the camera position/orientation calculated before and after the initialization are mixed and held as they are. When the localization fails, the processing by the initial attitude/coordinate system setting unit 513 is performed again. At this time, as described above, if the three-dimensional positions of feature points that are particularly frequently referred to are stored as values in the SLAM local coordinate system and conversion matrices to the real coordinate system, initialization processing is performed. Each time it is executed, both values may change (the integrated values in the real coordinate system are the same), which is complicated, so it is desirable to hold the values in the real coordinate system as much as possible.

(Contents of KF update unit 522)
In FIG. 10, a KF (key frame) updating unit 522 in charge of a position/orientation estimation (tracking) processing function 520 determines whether or not to make a new image into a KF based on image characteristics like conventional V-SLAM. .

The determination of whether or not to use a KF that is characteristic of a conventional V-SLAM image is made, for example, when the elapsed time or the number of elapsed frames from the last KF exceeds a specified value, or when the KF acquired by the frame posture estimation unit For example, in group 1, the number of shared 3D feature point groups with the KF that shares the 3D feature point group most with the new image is less than or equal to the specified number. After that, for the newly added KF, the KF updating unit adds the new image, which is KF, to the KF group of the real coordinate environment map. As described above, when a separate graph structure (KF group 1) is held between groups of KFs sharing feature points, the graph structure is updated as appropriate for newly added KFs (new images). .

Further, the KF update unit 522 of the system 500 may select a new KF image based on whether or not the image holds the GNSS position. That is, when all the images have no GNSS position and the number of images without the GNSS position continues for a specified number or more, and the image with the GNSS position is input as a new image, the result of the conventional determination characteristic of the image is Regardless, the new image is taken as the new KF.

Specifically, the KF updating unit 522 can realize its function by the CPU 601 executing a program stored in the memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

FIG. 12 is a flowchart illustrating an example of a procedure of processing by the KF updating unit; Note that the procedure of the processing of the KF updating unit 522 is not limited to the procedure of this flowchart. In the flowchart of FIG. 12, the KF updating unit 522 determines whether or not the current image is separated from the currently used KF by a specified number of frames or more (step S1201). Here, if the distance is less than the specified number of frames (step S1201: No), the series of processing ends without doing anything.

On the other hand, if the distance is greater than or equal to the specified number of frames (step S1201: Yes), then it is determined whether or not the current image has a common feature point with the KF currently in use less than or equal to the specified number (step S1202). . Here, if the number of common feature points is not equal to or less than the specified number (step S1202: No), the process proceeds to step S1204. On the other hand, if the number of common feature points is equal to or less than the specified number (step S1202: Yes), next, the current image frame is compared with the currently used KF and another KF having the largest number of common feature points. is equal to or less than a specified number (step S1203).

In step S1203, if the number of common feature points is not equal to or less than the specified number (step S1203: No), the process proceeds to step S1204. On the other hand, if the number of common feature points is equal to or less than the specified number (step S1203: Yes), the process proceeds to step S1206. Next, in step S1204, it may be determined whether or not the current image holds GNSS position information (step S1204). Here, if the GNSS position information is not held (step S1204: No), a series of processing ends. On the other hand, when the position information of GNSS is held (step S1204: Yes), it transfers to step S1205.

In step S1205, it may be determined whether or not the current KF is separated from the latest KF holding GNSS position information by a specified number of KFs or more (step S1205). Here, if the distance from the newest KF is greater than or equal to the prescribed number of KFs (step S1205: Yes), the process proceeds to step S1206. On the other hand, if they are not separated (step S1205: No), the series of processing ends.

At step S1206, the current image is set as the new KF (step S1206). Then, the new KF is added to the KF group of the real coordinate environment map (step S1207). Furthermore, the new KF is added to the graph structure of the feature point sharing relationship of the KF group to update the graph (step S1208). This completes a series of processes.

Only the KF addition determination is performed by one of the processing units in charge of the tracking processing function 520 (for example, the KF updating unit 522), the actual KF addition processing is made independent, and the processing unit 531 in charge of the local mapping processing function 530 533 may be implemented.

Moreover, in the present embodiment, the GNSS position information may not be acquired, and in that case, the processes of steps S1204 and S1205 may be omitted without being executed.

In the present system 500, the KF addition process has been described as being performed by the KF updating unit 522. FIG. However, the tracking processing function 520 is a process for all image frames, and the local mapping processing function 530 determines whether or not to implement the KF addition process if priority is given to the process related to the KF to be executed at the KF addition timing. It is preferable that only the determination is performed by one of the processing units 521 and 522 in charge of the tracking processing function 520, and the actual KF addition processing is performed by one of the processing units 531 to 533 in charge of the local mapping processing function 530. good. Also in conventional V-SLAM, the KF addition process itself is often carried out by one of the processing units 531 to 533 in charge of the local mapping processing function 530 .

(Contents of 3D map feature point update unit 531)
A 3D map feature point updating unit 531 in charge of the environment map creation (local mapping) processing function 530 uses the added KF to determine the removal of recently added 3D map points, as in the conventional V-SLAM. , performs the process of adding new 3D map points.

Specifically, the 3D map feature point updating unit 531 can realize its function by the CPU 601 executing a program stored in the memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

As the 3D map point removal determination process, the 3D map feature point updating unit 531 determines whether or not the recently added 3D map point group can be browsed from a specified number or more of KFs in the entire KF group including the newly added KF. , to determine if a 3D map point is being used. Then, if it is determined that the 3D map point is not used, the 3D map point is removed.

Note that the 3D map feature point update unit 531 performs only removal determination, and the actual removal process is performed by separately investigating whether or not 3D map points such as BA in the subsequent KF posture/feature point map optimization unit 533 are used. You may carry out simultaneously with processing.

As the new 3D map addition process, the 3D map feature point update unit 531 searches for feature points that are not associated with the 3D feature point group in the added new KF, and shares the feature points with the new KF updated by the KF update unit 522. A feature point that is the same as the feature point that is not associated with the first KF group and the image feature amount is searched for. At this time, it may be further narrowed down whether or not the feature points are the same by an arbitrary method such as epipolar constraint or reprojection error in the KF. When the same feature point is found, using the camera positions of the two KFs and the in-image position of the same feature point in the image on the KF, a known triangulation method is used to determine the feature point. A 3D position is obtained and added to the real coordinate environment map as a new 3D feature point.

(Contents of graph constraint generator 532)
A graph constraint generation unit 532 in charge of the environment map creation (local mapping) processing function 530 performs the following KF posture/feature point map optimization unit 533 to perform the three-dimensional positions of the current key frame and surrounding feature points as in the conventional case. is calculated by BA (local BA), the position of the current key frame and the 3D position of the surrounding feature point group are newly corrected in advance using the posture graph according to the input GNSS information. It is a processing unit that prepares for implementation.

Specifically, the graph constraint generation unit 532 can realize its function when the CPU 601 executes a program stored in the memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

The concept of posture graph and the optimization calculation itself using the graph structure may be the same as the optimization calculation based on general graph theory (posture graph structure), and the existing optimization such as g2o (General Graph Optimization) A library may be used.

The graph constraint generation unit 532 utilizes this general graph structure to optimize only the KF position/orientation (rough optimization of the KF information group in the real coordinate environment map) and the optimized KF group and surrounding feature point groups (detailed optimization of the entire real coordinate environment map). ) to create two different pose graphs.

(Contents of the KF posture/feature point map optimization unit 533)
A KF posture/feature point map optimization unit 533 in charge of the environment map creation (local mapping) processing function 530 uses two new posture graphs generated by the graph constraint generation unit 532 to perform general graph optimization. calculation.

Specifically, the KF posture/feature point map optimization unit 533 can realize its function by causing the CPU 601 to execute a program stored in the memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

(Contents of the loop detection/closing unit 541)
A loop detection/closing unit 541 in charge of the loop closing processing function 540 compares the image feature amounts of the whole image of the new KF and the retained KF image group to check the similarity in the same way as in the conventional V-SLAM. , Make sure that the vehicle does not travel in the same place multiple times (whether a loop occurs) on the driving route at the time of image acquisition. Then, if the similarity is high and it is thought that they are traveling in the same place, the "Loop KF ID" of the KF group information 551 of the related real coordinate environment map 550 indicates the time when they traveled in the same place in the past. , so that they can refer to each other.

In addition, the loop detection/closing unit 541 performs either local BA using the KF group near the new KF or global BA using the entire KF group for the new KF at the time of loop occurrence. Adjust the positional relationship of KF when traveling to a place. The KF group in the vicinity of the new KF may be selected from the sharing state of the map feature points, etc., or the sharing state with the KF group when traveling at the same place in the past may be used.

Specifically, the function of the loop detection/closing unit 541 can be realized by the CPU 601 executing a program stored in the memory 602 shown in FIG. 6, for example. More specifically, for example, the function may be realized by causing the CPU 701 to execute a program stored in the memory 702 shown in FIG.

In the present system 500, the actual coordinate environment map 550 has been constructed through the above-described processes using the input GNSS information 1002 so that scale drift does not occur. Therefore, the processing in the loop detection/closing unit 541, which mainly deals with scale drift countermeasures, may be omitted.

As described above, according to the present embodiment, the server (information processing device/moving body position estimation device) 501 processes any image out of the captured time-series images (video) 1001 as In performing information processing (S216 to S222) for estimating the shooting position and orientation of the arbitrary image and generating the environment map from the characteristics of the image, the retrograde image acquisition unit 512 obtains a retrograde image in which the captured image is reversed with respect to time. is acquired (S212), and the information processing is performed using the retrograde image.

In addition, the server (information processing device/moving body position estimation device) 501 calculates the shooting position/location of the arbitrary image from the characteristics of the arbitrary image among the photographed time-series images (1001). When performing information processing (S216 to S222) for posture estimation and environment map generation, the retrograde image acquisition unit 512 captures images near the latest time in a group of images showing image feature points having mutually distinguishable feature amounts. The image is processed (S215).

As a result, the image to be used is changed from "when the image feature point of the subject appears at the farthest point (when it starts to appear far away)" to "when the image feature point of the subject appears closest ( This will improve the accuracy of 3D position estimation of feature points estimated from changes in the image, and create a highly accurate surrounding environment map (3D map of surrounding feature points). Position group) can be created.

Further, according to the present embodiment, the retrograde determination unit 511 of the server (information processing device/mobile body position estimation device) 501 determines whether or not to acquire a retrograde image. Specifically, for example, based on the shooting direction of the time-series images (S201), it is determined whether or not to acquire a retrograde image. do. Also, based on the frame rate of the time-series images (S202, S205), it is determined whether or not to acquire the retrograde image, and if the frame rate is equal to or higher than a predetermined threshold, it is determined to acquire the retrograde image. Also, it is determined whether or not to acquire a retrograde image based on the state or accuracy of the shooting position/orientation estimation result when retrograde is not performed (S204, S207), or based on the accuracy of the environment map (S204, S207). do. Therefore, in either case, the retrograde image can be used only when it is effective to use the retrograde image.

Specifically, the moving body position estimation system according to the present embodiment includes, for example, a high-precision map update system for automatic driving, an application for pasting an image captured outdoors onto a map, an automatic It can be applied to a wide range of systems and applications such as estimation applications and CG reproduction systems for arbitrary road scenes.

The moving body position estimation method described in the present embodiment can be realized by executing a prepared program on a computer such as a personal computer or a work station. The program distribution program is stored in a computer-readable recording medium such as a hard disk, flexible disk, CD (Compact Disc)-ROM, MO (Magneto-Optical Disk), DVD (Digital Versatile Disk), USB (Universal Serial Bus) memory, etc. It is executed by being recorded and read from a recording medium by a computer. Also, the mobile body position estimation program may be distributed via a network such as the Internet.

Further, the following additional remarks are disclosed with respect to the above-described embodiment.

(Appendix 1) A mobile object position estimation system that performs information processing for estimating the shooting position and orientation of an arbitrary image from the characteristics of the arbitrary image among the time-series images taken and for generating an environment map. and
Acquiring a retrograde image obtained by reversing the time-series image with respect to time, and performing the information processing using the retrograde image;
A mobile position estimation system comprising an information processing device.

(Appendix 2) The information processing device is
Using the retrograde image,
Information processing that extracts a plurality of image feature points that have mutually distinguishable feature amounts,
Information processing that associates the image feature points having similar feature amounts among a plurality of images in the sequence of the time-series images; and
Information processing for estimating the photographing position using the two-dimensional position of each associated feature point on each image;
The mobile position estimation system according to appendix 1, characterized by performing at least one of:

(Supplementary Note 3) The mobile body position estimation system according to Supplementary Note 1 or 2, wherein the information processing device determines whether or not to acquire the retrograde image.

(Supplementary note 4) The moving body position estimation system according to Supplementary note 3, wherein the information processing device determines whether or not to acquire the retrograde image based on the shooting direction of the time-series images.

(Supplementary Note 5) The information processing device according to Supplementary note 4, wherein the information processing device determines to acquire the retrograde image when the front of the moving object is captured based on the shooting direction of the time-series images. mobile object position estimation system.
(Appendix 6) The information processing device according to any one of appendices 3 to 5, characterized in that it determines whether or not to acquire the retrograde image based on the frame rate of the time-series images. Mobile position estimation system.

(Supplementary note 7) Supplementary note 6 characterized in that the information processing device determines to acquire the retrograde image based on the frame rate of the time-series images when the frame rate is equal to or greater than a predetermined threshold value. 3. The mobile body position estimation system according to .

(Supplementary note 8) The information processing device determines whether or not to acquire the retrograde image based on the state or accuracy of the shooting position and orientation estimation result when retrograde is not performed. The mobile position estimation system according to any one.

(Supplementary Note 9) The moving body position according to any one of Supplementary Notes 3 to 8, wherein the information processing device determines whether or not to acquire the retrograde image based on the accuracy of the environment map. estimation system.

(Appendix 10) A moving body position estimation system that performs information processing for estimating the shooting position and orientation of an arbitrary image from the characteristics of the arbitrary image among the captured time-series images and generating an environment map. and
A moving body position estimation system, wherein said information processing is performed using an image captured near the latest time in a group of images in which image feature points having mutually distinguishable feature amounts are captured.

(Appendix 11) The information processing device
In performing information processing for estimating the shooting position/orientation of an arbitrary image from the characteristics of the arbitrary image among the photographed time-series images and generating an environment map,
Acquiring a retrograde image obtained by reversing the time-series image with respect to time, and performing the information processing using the retrograde image;
A moving body position estimation method characterized by:

(Appendix 12) The information processing device
In performing information processing for estimating the shooting position/orientation of an arbitrary image from the characteristics of the arbitrary image among the photographed time-series images and generating an environment map,
Performing the information processing using an image captured near the latest time in a group of images showing image feature points having mutually distinguishable feature amounts,
A moving body position estimation method characterized by:

100 subject (feature point A)
101-110 images (shooting position)
500 Mobile body position estimation system 501 Server (information processing device/mobile body position estimation device)
502 on-vehicle device (information collection device)
503 Mobile body 504 Network 505 Satellite 510 System initialization processing function 511 Retrograde determination unit 512 Retrograde image acquisition unit 513 Initial attitude/coordinate system setting unit 520 Position and orientation estimation (tracking) processing function 521 Frame attitude estimation unit 522 KF (key frame ) update unit 530 environment map creation (local mapping) processing function 531 3D map feature point update unit 532 graph constraint generation unit 533 KF (key frame) posture/feature point map optimization unit 540 loop close processing function 541 loop detection/closing unit 550 real coordinate environment map 551 KF (key frame) group information 552 feature point group information 560 all image position and orientation data 1001 video 1002 GNSS information 1003 orientation information

Claims (10)

A mobile body position estimation system that performs information processing for estimating the shooting position and orientation of an arbitrary image from the characteristics of the arbitrary image and generating an environment map for an arbitrary image out of the captured time-series images,
Whether or not to acquire the retrograde image based on the shooting direction of the time-series image when acquiring the retrograde image obtained by reversing the time-series image and performing the information processing using the retrograde image determine whether
A mobile position estimation system comprising an information processing device. A mobile body position estimation system that performs information processing for estimating the shooting position and orientation of an arbitrary image from the characteristics of the arbitrary image and generating an environment map for an arbitrary image out of the captured time-series images,
Whether or not to acquire the retrograde image based on the frame rate of the time-series image when acquiring the retrograde image obtained by reversing the time-series image and performing the information processing using the retrograde image determine whether
A mobile position estimation system comprising an information processing device. A mobile body position estimation system that performs information processing for estimating the shooting position and orientation of an arbitrary image from the characteristics of the arbitrary image and generating an environment map for an arbitrary image out of the captured time-series images,
In acquiring a retrograde image obtained by reversing the time-series images in a retrograde manner and performing the information processing using the retrograde image, the determining whether to acquire a retrograde image ;
A mobile position estimation system comprising an information processing device. A mobile body position estimation system that performs information processing for estimating the shooting position and orientation of an arbitrary image from the characteristics of the arbitrary image and generating an environment map for an arbitrary image out of the captured time-series images,
A retrograde image obtained by reversing the time-series images in time is acquired, and in performing the information processing using the retrograde image, it is determined whether or not to acquire the retrograde image based on the accuracy of the environmental map. do
A mobile position estimation system comprising an information processing device. The information processing device is
Using the retrograde image,
Information processing that extracts a plurality of image feature points that have mutually distinguishable feature amounts,
Information processing that associates the image feature points having similar feature amounts among a plurality of images in the sequence of the time-series images; and
Information processing for estimating the photographing position using the two-dimensional position of each associated feature point on each image;
5. The mobile body position estimation system according to any one of claims 1 to 4, wherein at least one of the following is performed. 6. The information processing according to any one of claims 1 to 5, wherein an image taken near the latest time in a group of images showing image feature points having mutually distinguishable feature amounts is used for the information processing. 3. The mobile body position estimation system according to 1 . The information processing device
A mobile body position estimation method for performing information processing for estimating the shooting position and orientation of an arbitrary image among the captured time-series images and generating an environment map from the characteristics of the arbitrary image, comprising:
Whether or not to acquire the retrograde image based on the shooting direction of the time-series image when acquiring the retrograde image obtained by reversing the time-series image and performing the information processing using the retrograde image determine whether
A moving body position estimation method characterized by: The information processing device
A mobile body position estimation method for performing information processing for estimating the shooting position and orientation of an arbitrary image among the captured time-series images and generating an environment map from the characteristics of the arbitrary image, comprising:
Whether or not to acquire the retrograde image based on the frame rate of the time-series image when acquiring the retrograde image obtained by reversing the time-series image and performing the information processing using the retrograde image determine whether
A moving body position estimation method characterized by: The information processing device
A mobile body position estimation method for performing information processing for estimating the shooting position and orientation of an arbitrary image among the captured time-series images and generating an environment map from the characteristics of the arbitrary image, comprising:
In acquiring a retrograde image obtained by reversing the time-series images in a retrograde manner and performing the information processing using the retrograde image, the determining whether to acquire a retrograde image ;
A moving body position estimation method characterized by: The information processing device
A mobile body position estimation method for performing information processing for estimating the shooting position and orientation of an arbitrary image among the captured time-series images and generating an environment map from the characteristics of the arbitrary image, comprising:
A retrograde image obtained by reversing the time-series images in time is acquired, and in performing the information processing using the retrograde image, it is determined whether or not to acquire the retrograde image based on the accuracy of the environmental map. do
A moving body position estimation method characterized by:

Images