Fusion map construction method, apparatus, robot, and storage medium

Abstract

This application discloses a fusion map construction method, apparatus, robot and storage medium, the fusion map construction method acquires an image frame set collected by an image acquisition device and a laser position and orientation data set mapped by a laser radar, performs interpolation operations on each image frame in the image frame set based on the laser position and orientation data set to obtain each position and orientation corresponding to each image frame (S100), obtains feature matching relationships between each adjacent image frame, screens key frame sets from the image frame set based on the position and orientation and feature matching relationships corresponding to each image frame, obtains an initial map point set corresponding to the key frame set, and each key in the key frame set - The process includes constructing a correspondence between each initial map point in the initial map point set and obtaining a correspondence set (S102), sequentially selecting the current keyframe and the related keyframe set corresponding to the current keyframe from the keyframe set, updating the initial map point set and the correspondence set based on the feature matching relationship between the current keyframe and the related keyframe set, and obtaining an updated map point set and an updated correspondence set (S104), converting the updated map point set into a three-dimensional map point set, and constructing a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the position and orientation of each keyframe in the updated correspondence set (S106).

Claims

1. A method for constructing a fusion map, which is used in a robot equipped with laser radar and image acquisition equipment. The steps include: acquiring an image frame set collected by an image acquisition device and a laser position and attitude dataset mapped by a laser radar; performing an interpolation operation on each image frame in the image frame set based on the laser position and attitude dataset to obtain the position and attitude corresponding to each image frame; The steps include: obtaining feature matching relationships between adjacent image frames; screening keyframe sets from the image frame set based on the position and orientation of each image frame and the feature matching relationships; obtaining an initial map point set corresponding to the keyframe set; constructing a correspondence relationship between each keyframe in the keyframe set and each initial map point in the initial map point set; and obtaining a correspondence relationship set. The steps include sequentially selecting the current keyframe and the related keyframe set corresponding to the current keyframe from the keyframe set, updating the initial map point set and the correspondence relationship set based on the feature matching relationship between the current keyframe and the related keyframe set, and obtaining the updated map point set and the updated correspondence relationship set. A method for constructing a fusion map, characterized by comprising the steps of: converting the updated map point set into a three-dimensional map point set; and constructing a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the position and orientation of each keyframe in the updated correspondence set.

2. The step of performing an interpolation operation on each image frame in the image frame set based on the aforementioned laser position and orientation dataset to obtain the position and orientation corresponding to each image frame is: The steps include sequentially selecting the current image frame from the image frame set, and based on the current timestamp corresponding to the current image frame, obtaining the forward laser position and orientation corresponding to the current timestamp, the forward timestamp corresponding to the forward laser position and orientation, the backward laser position and orientation, and the backward timestamp corresponding to the backward laser position and orientation from the laser position and orientation dataset, The method according to claim 1, characterized by comprising the step of interpolating the current timestamp, forward timestamp, backward timestamp, forward laser position and orientation, and backward laser position and orientation to obtain a position and orientation corresponding to the current image frame.

3. The step of obtaining the feature matching relationship between each adjacent image frame, as described above, The steps include: performing a feature point extraction operation on each image frame in the aforementioned image frame set to obtain each feature point, including a descriptor, corresponding to each image frame; The steps include sequentially selecting adjacent first and second image frames from the image frame set, and obtaining a first descriptor corresponding to the first image frame and a second descriptor corresponding to the second image frame, The steps include traversing the first descriptor corresponding to the first image frame, selecting the second descriptor that is most similar to the first descriptor as the first target descriptor, and constructing a first correspondence between the first descriptor and the first target descriptor, The steps include traversing the second descriptor corresponding to the second image frame, selecting the descriptor among the first descriptors that is most similar to the second descriptor as the second target descriptor, and constructing a second correspondence relationship between the second descriptor and the second target descriptor, The method according to claim 1, characterized by including the step of matching corresponding feature points of descriptors whose relationships in the first correspondence relationship and the second correspondence relationship match, and obtaining a feature matching relationship corresponding to each of the first image frames and each of the second image frames in the image frame set.

4. The step of screening keyframe sets from the image frame set based on the position and orientation corresponding to each image frame and the feature matching relationship described above is: Based on the position and orientation of each image frame in the image frame set corresponding to the adjacent first image frame and the position and orientation of the adjacent second image frame, the corresponding change distance between each first image frame and each second image frame in the image frame set is calculated, and if the change distance is greater than a preset change distance, the corresponding second image frame is set as a key frame in the key frame set. Or, The method according to any one of claims 1 to 3, characterized in that it includes the step of calculating a corresponding feature matching rate between each first image frame and each second image frame in the image frame set based on the corresponding feature matching relationship between adjacent first image frames and second image frames of each image frame in the image frame set, and setting the corresponding second image frame as a key frame in the key frame set if the feature matching rate is smaller than a preset matching rate.

5. The step of calculating the corresponding feature matching rate between each first image frame and each second image frame in the image frame set, based on the corresponding feature matching relationship between adjacent first and second image frames in the image frame set described above, The steps include statistically determining the number of matching feature points corresponding to each image frame based on the corresponding feature matching relationship between adjacent first and second image frames in the aforementioned image frameset, The method according to 4, further comprising the step of calculating the corresponding feature matching rate between adjacent first and second image frames of each image frame based on the total number of corresponding feature points of the first image frame corresponding to the image frame and the number of matching feature points corresponding to the image frame.

6. The steps of sequentially selecting the current keyframe and the related keyframe set corresponding to the current keyframe from the aforementioned keyframe set, updating the initial map point set and the correspondence relationship set based on the feature matching relationship between the current keyframe and the related keyframe set, and obtaining the updated map point set and the updated correspondence relationship set are as follows: The steps include: obtaining a pre-set distance, sequentially selecting the current keyframe from the keyframe set, and selecting a related keyframe set corresponding to the current keyframe from the keyframe set based on the position and orientation corresponding to the current keyframe and the pre-set distance; The steps include: performing feature matching on the current keyframe and each related keyframe in the related keyframe set to obtain a feature matching relationship between the current keyframe and each of the related keyframes; If the initial map point in the initial map point set corresponding to the current matching feature point of the current keyframe does not match the initial map point corresponding to the current related matching feature point of the related keyframe, the steps include: merging the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point to obtain an updated map point; The steps include adding the updated map points to the initial map point set, removing the initial map points corresponding to the current matching feature points and the initial map points corresponding to the current related matching feature points from the initial map point set, changing the initial map points corresponding to the current matching feature points and the initial map points corresponding to the current related matching feature points in the correspondence relationship set to the updated map points, and obtaining the updated correspondence relationships and updated map points. The method according to claim 1, characterized by comprising the steps of traversing the keyframes in the keyframe set to obtain the updated map point set and the updated correspondence set.

7. The updated map point set includes the pixel coordinates in the corresponding keyframe of each updated map point, and the step of converting the updated map point set into a three-dimensional map point set is: The method according to claim 1, further comprising the step of converting the updated map point set into a three-dimensional map point set based on the position and orientation of the keyframe corresponding to each updated map point in the updated map point set and the pixel coordinates in the corresponding keyframe of each updated map point.

8. The step of constructing a target fusion map based on the aforementioned three-dimensional map point set, updated correspondence set, and the position and orientation of each keyframe in the updated correspondence set is as follows: The steps include: calculating the total reprojection error for each three-dimensional map point in the three-dimensional map point set to its corresponding keyframe; if the total reprojection error is greater than a predetermined number of pixels, removing the correspondence between the three-dimensional map point and its corresponding keyframe from the updated correspondence set to obtain an intermediate correspondence set; The steps include sequentially selecting the current 3D map points from the aforementioned 3D map point set, calculating the number of corresponding keyframes for the current 3D map points, and if the number of corresponding keyframes is smaller than a preset number, deleting the current 3D map point from the aforementioned 3D map point set, deleting the correspondence between the current 3D map point and the corresponding keyframe from the aforementioned intermediate correspondence relationship set, and obtaining the target 3D map point set and the target correspondence relationship set. The method according to claim 1, further comprising the step of constructing the target fusion map based on the target three-dimensional map point set, the target correspondence set, and the position and orientation of each keyframe in the target correspondence set.

9. The step of calculating the total reprojection error for each three-dimensional map point in the three-dimensional map point set to the corresponding keyframe, as described above, The steps include: reprojecting the three-dimensional map points in the three-dimensional map point set onto the corresponding keyframes to obtain the reprojected points corresponding to the three-dimensional map points; The method according to 8, comprising the step of statistically calculating the reprojection errors of the three-dimensional map points to each of the keyframes, and obtaining the total projection errors of the three-dimensional map points to each corresponding keyframe.

10. A fusion map construction device, An interpolation module for acquiring an image frame set collected by an image acquisition device and a laser position and attitude dataset map constructed by a laser radar, performing interpolation operations on each image frame in the image frame set based on the laser position and attitude dataset, and obtaining the position and attitude corresponding to each image frame, A matching module for obtaining a set of correspondences, which acquires feature matching relationships between adjacent image frames, screens a set of keyframes from the set of image frames based on the position and orientation of each image frame and the feature matching relationships, acquires an initial map point set corresponding to the keyframe set, constructs a correspondence relationship between each keyframe in the keyframe set and each initial map point in the initial map point set, and obtains a set of correspondences, An update module for sequentially selecting the current keyframe and related keyframe sets corresponding to the current keyframe from a keyframe set, updating the initial map point set and correspondence relationship set based on the feature matching relationship between the current keyframe and related keyframe sets, and obtaining the updated map point set and updated correspondence relationship set. The system includes a construction module for converting the updated map point set into a three-dimensional map point set, and for constructing a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the position and orientation of each keyframe in the updated correspondence set.

11. The interpolation module further, The current image frame is sequentially selected from the image frame set, and based on the current timestamp corresponding to the current image frame, the forward laser position and orientation corresponding to the current timestamp, the forward timestamp corresponding to the forward laser position and orientation, the backward laser position and orientation, and the backward timestamp corresponding to the backward laser position and orientation are obtained from the laser position and orientation dataset, and The apparatus according to claim 10, characterized in that it is used to interpolate the current timestamp, forward timestamp, backward timestamp, forward laser position and orientation, and backward laser position and orientation to obtain a position and orientation corresponding to the current image frame.

12. The matching module further, A feature point extraction operation is performed on each image frame in the aforementioned image frame set to obtain each feature point containing a descriptor corresponding to each image frame. From the image frame set, sequentially select the first and second image frames adjacent to each image frame, and obtain the first descriptor corresponding to the first image frame and the second descriptor corresponding to the second image frame. The first descriptor corresponding to the first image frame is traversed, and the descriptor among the second descriptors that is most similar to the first descriptor is set as the first target descriptor, and a first correspondence relationship is constructed between the first descriptor and the first target descriptor. The second descriptor corresponding to the second image frame is traversed, the descriptor among the first descriptors that is most similar to the second descriptor is set as the second target descriptor, a second correspondence relationship is constructed between the second descriptor and the second target descriptor, and The apparatus according to claim 10, characterized in that it is used to match the corresponding feature points of descriptors whose relationships in the first correspondence relationship and the second correspondence relationship match, and to obtain a feature matching relationship corresponding to each of the first image frames and each of the second image frames in the image frame set.

13. The matching module further, Based on the position and orientation of each image frame in the image frame set corresponding to the adjacent first image frame and the position and orientation of the adjacent second image frame, the corresponding change distance between each first image frame and each second image frame in the image frame set is calculated, and if the change distance is greater than a preset change distance, the corresponding second image frame is set as the key frame in the key frame set. Or, The apparatus according to any one of claims 10 to 12, characterized in that it calculates a corresponding feature matching rate between each first image frame and each second image frame in the image frame set based on the corresponding feature matching relationship between adjacent first image frames and second image frames of each image frame in the image frame set, and is used to set the corresponding second image frame as a key frame in the key frame set when the feature matching rate is smaller than a preset matching rate.

14. The aforementioned update module further, Based on the position and orientation of each image frame in the image frame set corresponding to the adjacent first image frame and the position and orientation of the adjacent second image frame, the corresponding change distance between each first image frame and each second image frame in the image frame set is calculated, and if the change distance is greater than a preset change distance, the corresponding second image frame is set as the key frame in the key frame set. Or, The apparatus according to claim 10, characterized in that, based on the corresponding feature matching relationship between adjacent first and second image frames in the image frame set, the corresponding feature matching rate between each first and second image frame in the image frame set is calculated, and the second image frame is used as a keyframe in the key frame set when the feature matching rate is smaller than a preset matching rate.

15. The aforementioned update module further, The system obtains a pre-set distance, sequentially selects the current keyframe from the keyframe set, and based on the position and orientation corresponding to the current keyframe and the pre-set distance, selects the related keyframe set corresponding to the current keyframe from the keyframe set. Feature matching is performed on the current keyframe and each related keyframe in the related keyframe set to obtain the feature matching relationship between the current keyframe and each of the related keyframes. If the initial map point in the initial map point set corresponding to the current matching feature point of the current keyframe does not match the initial map point corresponding to the current related matching feature point of the related keyframe, the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point are merged to obtain an updated map point. The updated map points are added to the initial map point set, the initial map points corresponding to the current matching feature points and the initial map points corresponding to the current related matching feature points are removed from the initial map point set, the initial map points corresponding to the current matching feature points and the initial map points corresponding to the current related matching feature points in the correspondence set are changed to the updated map points, the updated correspondence and updated map points are obtained, and, The apparatus according to claim 10, characterized in that it is used to traverse keyframes in the keyframe set and to obtain the updated map point set and the updated correspondence set.

16. The aforementioned construction module further includes, The apparatus according to claim 10, characterized in that it is used to convert the updated map point set into a three-dimensional map point set based on the position and orientation of the keyframe corresponding to each updated map point in the updated map point set and the pixel coordinates in the corresponding keyframe of each updated map point.

17. The aforementioned construction module further, The total reprojection error for each three-dimensional map point in the three-dimensional map point set to its corresponding keyframe is calculated, and if the total reprojection error is greater than a predetermined number of pixels, the correspondence between the three-dimensional map point and its corresponding keyframe is removed from the updated correspondence set to obtain an intermediate correspondence set. The current 3D map points are sequentially selected from the aforementioned 3D map point set, the number of corresponding keyframes for the current 3D map points is calculated, and if the number of corresponding keyframes is smaller than a preset number, the current 3D map point is deleted from the aforementioned 3D map point set, the correspondence between the current 3D map point and the keyframe corresponding to the current 3D map point is deleted from the aforementioned intermediate correspondence relationship set, and the target 3D map point set and the target correspondence relationship set are obtained, and The apparatus according to claim 10, characterized in that it is used to construct the target fusion map based on the target three-dimensional map point set, the target correspondence set, and the position and orientation of each keyframe in the target correspondence set.

18. The aforementioned construction module further, The three-dimensional map points in the three-dimensional map point set are reprojected onto the corresponding keyframes to obtain the reprojected points corresponding to the three-dimensional map points. The apparatus according to claim 17, characterized in that it is used to statistically calculate the reprojection error of the three-dimensional map points to each of the keyframes and to obtain the total projection error of the three-dimensional map points to each of the corresponding keyframes.

19. It is a robot, Equipped with laser radar and image acquisition equipment, A memory device that stores computer-readable instructions, A robot further comprising a processor for realizing the steps of the method according to any one of claims 1 to 9 when the computer-readable instruction is executed.

20. A computer-readable storage medium in which computer programs are stored, A computer-readable storage medium characterized in that, when the computer program is executed by a processor, it realizes the steps of the method according to any one of claims 1 to 9.

Images