Incremental mapping method and system

Abstract

The present application proposes an incremental mapping method, including: acquiring sensor data, the sensor data including multiple image frames collected in a continuous time period; adding one or more frames of the multiple image frames as key frames of the current map; locally optimizing the current key frame of the current map to obtain an optimized key frame; searching for a closed-loop matching frame that matches the optimized key frame in a historical key frame based on similarity, the historical key frame including the key frame before the current key frame; determining whether the optimized key frame and the closed-loop matching frame belong to the same map; and if the optimized key frame and the closed-loop matching frame do not belong to the same map, merging the current map and the historical map where the closed-loop matching frame is located. An associated incremental mapping system and computer medium are also proposed.

Description

Technical Field

[0001] The present application generally relates to the field of computer vision technology, and more particularly to an incremental mapping method and system. Background Art

[0002] Visual SLAM (Simultaneous Localization and Mapping) technology is a key component in robotics and autonomous driving technology. It combines the functions of positioning and mapping, allowing robots or vehicles to self-locate and build environmental maps in real time in unknown environments. This technology mainly analyzes the image data captured by the camera, extracts and tracks the feature points in the image, and then estimates the position and posture of the robot or vehicle, and builds a three-dimensional map of the surrounding environment.

[0003] In the field of robotics, visual SLAM technology enables robots to autonomously navigate, avoid obstacles, and perform tasks in complex environments. For example, in home service robots, visual SLAM can help robots understand the home layout, plan cleaning paths, and adjust directions in time when encountering obstacles. In industrial automation, visual SLAM technology can be used in automated warehousing systems to achieve precise positioning of robots. In the field of autonomous driving, visual SLAM technology also plays an important role. By combining visual sensors with other sensors (such as lidar, inertial measurement units, etc.), autonomous driving vehicles can perceive and understand the surrounding environment in real time, and achieve high-precision positioning and map construction. This not only improves the safety of the vehicle, but also enables the vehicle to autonomously navigate and avoid obstacles in complex and changing road environments.

[0004] During the mapping process, the existing visual SLAM technology calculates the relative pose based on the feature matching of the current frame and the previous frame, and then numerically integrates the relative pose from the initial moment to the current moment to obtain the global pose at the current moment.

[0005] However, in long-distance and large-scale scenes, conventional visual SLAM mapping requires all image data of the scene to be collected at one time, and then the visual feature map of the scene is built at one time. This means longer data collection, mapping time and lower fault tolerance. If a single mapping fails, the map can only be rebuilt. In addition, if the scene is expanded (such as the expansion of a parking lot), conventional visual SLAM mapping can only rebuild the complete map, and it is impossible to incrementally build a map of the expanded part of the scene on the original local map. In addition, conventional visual SLAM methods are prone to failures in the relative pose solution of the previous and next frames (or front-end tracking) and closed-loop missed detection when mapping in large scenes and long distances. Both of these situations can easily lead to excessive deviations of the entire map and make it unusable. At the same time, closed-loop detection and closed-loop optimization are relatively time-consuming when mapping large scenes and long distances, affecting mapping efficiency.

[0006] In view of this, the present application proposes an incremental mapping method and system to solve the above-mentioned defects of the prior art. Summary of the invention

[0007] A brief summary of one or more aspects is given below to provide a basic understanding of these aspects. This summary is not an exhaustive overview of all conceived aspects, and is neither intended to identify the key or decisive elements of all aspects nor to define the scope of any or all aspects. Its only purpose is to give some concepts of one or more aspects in a simplified form as a preface to a more detailed description given later. The scope of the present invention is defined by the attached claims and their equivalent technical solutions.

[0008] The present application provides an incremental mapping method, including: acquiring sensor data, the sensor data including multiple image frames collected in a continuous time period; adding one or more frames of the multiple image frames as key frames of a current map; locally optimizing the current key frame of the current map to obtain an optimized key frame; searching for a closed-loop matching frame that matches the optimized key frame in historical key frames based on similarity, the historical key frames including key frames before the current key frame; determining whether the optimized key frame and the closed-loop matching frame belong to the same map; and, if the optimized key frame and the closed-loop matching frame do not belong to the same map, merging the current map and the historical map where the closed-loop matching frame is located.

[0009] In some embodiments, adding one or more frames among multiple image frames as key frames of the current map further includes: determining the pose of a current frame among multiple image frames; determining the relative pose of the current frame to a previous key frame; and if the relative pose satisfies a preset condition, adding the current frame as a key frame of the current map.

[0010] In some embodiments, determining the pose of a current frame among multiple image frames further includes: performing visual initialization based on an initial frame and a second frame among the multiple image frames; performing front-end tracking to determine the pose of the current frame; and re-performing visual initialization if the front-end tracking fails, wherein the front-end tracking failure is determined when the number of map points obtained by the front-end tracking is less than a threshold number.

[0011] In some embodiments, performing front-end tracking further includes: performing tracking based on a uniform velocity model; if the tracking based on the uniform velocity model fails, performing reference frame based tracking; if the reference frame based tracking fails, performing DR based tracking; and if the DR based tracking fails, determining that the front-end tracking has failed.

[0012] In some embodiments, locally optimizing the current keyframe of the current map to obtain an optimized keyframe further includes: deleting redundant map points in the current keyframe to obtain a streamlined keyframe, wherein the redundant map points include map points in the current keyframe that are observed by the common view keyframe less than a threshold number of times, and the common view keyframe includes keyframes in historical keyframes that have common view map points with the current keyframe; and performing local optimization on the streamlined keyframe to obtain an optimized keyframe.

[0013] In some embodiments, the method further includes: determining a proportion of redundant map points in the current key frame; and if the proportion is greater than a preset threshold, deleting the current key frame.

[0014] In some embodiments, map merging of the current map and the historical map further includes: constructing a local window based on the common view frame of the optimized key frame and the common view frame of the closed-loop matching frame; using all map points within the local window to determine the relative pose of the optimized key frame and the closed-loop matching frame; modifying the pose of the optimized key frame according to the relative pose; and unifying the current map and the historical map into the same coordinate system to achieve map merging.

[0015] In some embodiments, the method further includes: when the optimized key frame and the closed-loop matching frame belong to the same map, performing closed-loop correction on the current map.

[0016] In some embodiments, the method further includes: performing multiple rounds of global optimization during the execution of the closed-loop correction, wherein only the poses of all map points are adjusted in a portion of the multiple rounds of global optimization, and the poses of all key frames and all map points are adjusted in the remaining rounds.

[0017] The present application also provides an incremental mapping system, including: a data acquisition unit, which is configured to: acquire sensor data, the sensor data including multiple image frames collected within a continuous time period; a key frame adding unit, which is configured to: add one or more frames of the multiple image frames as key frames of the current map; a key frame optimization unit, which is configured to: locally optimize the current key frame of the current map to obtain an optimized key frame; a closed-loop matching unit, which is configured to: search for a closed-loop matching frame that matches the optimized key frame in historical key frames based on similarity, the historical key frames including key frames before the current key frame; and an incremental mapping unit, which is configured to: determine whether the optimized key frame and the closed-loop matching frame belong to the same map; and if the optimized key frame and the closed-loop matching frame do not belong to the same map, map merge the current map and the historical map where the closed-loop matching frame is located.

[0018] In some embodiments, the key frame adding unit is further configured to: determine the pose of a current frame among multiple image frames; determine the relative pose of the current frame to a previous key frame; and if the relative pose satisfies a preset condition, add the current frame as a key frame of the current map.

[0019] In some embodiments, the key frame adding unit is further configured to determine the pose of a current frame among multiple image frames by: performing visual initialization based on an initial frame and a second frame among the multiple image frames; performing front-end tracking to determine the pose of the current frame; and re-performing visual initialization if the front-end tracking fails, wherein when the number of map points obtained by the front-end tracking is less than a threshold number, it is determined that the front-end tracking has failed.

[0020] In some embodiments, the key frame adding unit is further configured to perform front-end tracking by: performing tracking based on a uniform velocity model; if the tracking based on the uniform velocity model fails, performing tracking based on a reference frame; if the tracking based on the reference frame fails, performing DR-based tracking; and if the DR-based tracking fails, determining that the front-end tracking has failed.

[0021] In some embodiments, the key frame optimization unit is further configured to: delete redundant map points in the current key frame to obtain a streamlined key frame, wherein the redundant map points include map points in the current key frame that are observed by the common view key frame less than a threshold number of times, and the common view key frame includes key frames in the historical key frames that have common view map points with the current key frame; and perform local optimization on the streamlined key frame to obtain an optimized key frame.

[0022] In some embodiments, the key frame optimization unit is further configured to: determine a proportion of redundant map points in the current key frame; and if the proportion is greater than a preset threshold, delete the current key frame.

[0023] In some embodiments, the incremental mapping unit is further configured to: construct a local window based on the common view frame of the optimized key frame and the common view frame of the closed-loop matching frame; use all map points within the local window to determine the relative pose of the optimized key frame and the closed-loop matching frame; modify the pose of the optimized key frame according to the relative pose; and unify the current map and the historical map into the same coordinate system to achieve map merging.

[0024] In some embodiments, the incremental mapping unit is further configured to: when the optimized key frame and the closed-loop matching frame belong to the same map, perform closed-loop correction on the current map.

[0025] In some embodiments, the incremental mapping unit is further configured to: perform multiple rounds of global optimization during the execution of closed-loop correction, wherein only the poses of all map points are adjusted in a portion of the multiple rounds of global optimization, and the poses of all key frames and all map points are adjusted in the remaining rounds.

[0026] The present application also proposes a computer-readable storage medium on which instructions for incremental mapping are stored. When these instructions are executed by a processor, the aforementioned incremental mapping method is implemented.

[0027] The technical solution of this application uses map merging to achieve incremental map construction in the same scene, solving the problem of long time and high failure cost of single map construction. By reinitializing in the event of front-end tracking failure, the robustness of front-end tracking can be improved and trajectory interruptions caused by tracking loss can be reduced. In addition, by accelerating global optimization, the time consumption of large-scene map construction can be significantly reduced without affecting the quality of the map. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] The features, nature and advantages of the present application will become more apparent when the detailed description set forth below is understood in conjunction with the accompanying drawings. In the accompanying drawings, the same reference numerals are always used for corresponding identification. It should be noted that the drawings described are only schematic and non-limiting. In the accompanying drawings, the sizes of some components may be exaggerated and are not drawn to scale for illustrative purposes.

[0029] Figure 1 The diagram shows the architecture of the incremental mapping algorithm of the present disclosure.

[0030] Figure 2 An incremental mapping method according to an embodiment of the present disclosure is shown.

[0031] Figure 3 An exemplary process of front-end tracking according to an embodiment of the present disclosure is shown.

[0032] Figure 4An exemplary process of locally optimizing a key frame according to an embodiment of the present disclosure is shown.

[0033] Figure 5 An exemplary process of map merging and closed-loop correction according to an embodiment of the present disclosure is shown.

[0034] Figure 6 An incremental mapping system according to an embodiment of the present disclosure is shown.

[0035] Figure 7 A schematic diagram of a computer-readable medium configured to store computer-executable instructions according to one embodiment of the present disclosure is shown. DETAILED DESCRIPTION

[0036] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings. In the following detailed description, many specific details are set forth to provide a thorough understanding of the described exemplary embodiments. However, it is obvious to those skilled in the art that the described embodiments can be practiced without some or all of these specific details. In other exemplary embodiments, well-known structures are not described in detail to avoid unnecessarily obscuring the concepts of the present application. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. At the same time, in the absence of conflict, the various aspects described in the embodiments can be combined arbitrarily.

[0037] Figure 1 The diagram shows an architecture of an incremental mapping algorithm according to the present disclosure.

[0038] like Figure 1 As shown, the incremental mapping algorithm disclosed in the present invention performs incremental mapping based on sensor input. The sensors can be vehicle-mounted front and rear AVM (Around View Monitor) cameras, DR (Dead Reckoning) modules, GPS (Global Positioning System) / RTK (Real Time Kinematic), etc. The camera is used to collect continuous time image data in the mapping scene. DR is a module that combines a wheel speed meter and an IMU (Inertial Measurement Unit) to provide rough trajectory information of the vehicle. GPS / RTK is a global satellite navigation system that provides absolute position observation information of the vehicle on the ground.

[0039] After completing incremental mapping, the constructed map can be used to generate other layers, such as BEV (Bird's Eye View) semantic map, radar point cloud, vectorized road network, etc.

[0040] like Figure 1 As shown in , the incremental mapping algorithm mainly includes a front-end tracking module, a local map optimization module, a loop closing module, a map management module, and a map post-processing module.

[0041] The front-end tracking module is mainly used to solve the relative pose between the previous frame and the current frame, and then obtain the pose of the current frame in the map coordinate system.

[0042] The local map optimization module is mainly responsible for two tasks: using the newly added keyframes to match the 2D-2D feature points of the existing keyframes, triangulating new map points, and adding both the keyframes and map points to the map; and locally optimizing the most recent keyframes (keyframes that have common map points with the latest keyframe) and the map points they observe to improve map quality.

[0043] The loop closure module is used to detect whether the current keyframe and the historical keyframe are similar enough (i.e., they have returned to the position they passed before). When the loop closure detection succeeds, if the current keyframe and the matching historical keyframe do not belong to the same map, it means that incremental map expansion is being performed on the loaded map. At this time, map merging can be performed. If they belong to the same map, loop closure correction is performed.

[0044] The map management module is used to manage loaded maps and the current map.

[0045] The map post-processing module is used to perform a series of post-processing on the constructed map, such as aligning the map coordinate system and the GPS Gaussian projection coordinate system, recording map quality information (whether the splicing is successful, whether the loop is closed, whether there are sections where the loop fails), etc.

[0046] Figure 2 An incremental mapping method 200 according to an embodiment of the present disclosure is shown.

[0047] like Figure 2 As shown in FIG. 2 , the method 200 starts at step 205. At step 205, sensor data is acquired, the sensor data including a plurality of image frames acquired in a continuous time period.

[0048] As mentioned above, the sensor may include a front and rear camera, DR, etc. Therefore, the sensor data may be obtained by collecting the surrounding environment image through the front and rear camera, DR, etc. For example, during the driving of the vehicle, the on-board camera may capture the image of the surrounding environment of the vehicle, thereby obtaining multiple image frames.

[0049] In step 210, one or more frames of the plurality of image frames are added as key frames of the current map.

[0050] In some embodiments of the present disclosure, adding one or more frames among the multiple image frames as key frames of the current map further includes: determining the pose of a current frame among the multiple image frames; determining the relative pose of the current frame with respect to a previous key frame; and if the relative pose satisfies a preset condition, adding the current frame as a key frame of the current map.

[0051] Taking autonomous driving as an example, during the driving process of the vehicle, multiple image frames of the surrounding environment can be collected by an image acquisition device (for example, a vehicle-mounted camera) for positioning and mapping. Since the difference between adjacent image frames is small and the scenes in which the two are located are basically the same, not all image frames need to be used for positioning and mapping, but image frames with certain differences between each other can be used. Therefore, a part of the image frames can be selected from all the collected image frames for positioning and mapping. These image frames selected for positioning and mapping can be called key frames.

[0052] For example, if the geometric distance between the location of the current frame and the location of the previous key frame is greater than a preset distance threshold (e.g., 1m), the current frame can be added as a key frame of the current map. In another example, if the angle difference between the orientation (i.e., posture) of the current frame and the previous key frame exceeds a preset angle threshold (e.g., 5 degrees), the current frame can be added as a key frame of the current map.

[0053] In some embodiments of the present disclosure, determining the pose of the current frame further includes: performing visual initialization based on an initial frame and a second frame among multiple image frames; performing front-end tracking to determine the pose of the current frame; and re-performing visual initialization if the front-end tracking fails, wherein when the number of map points obtained by the front-end tracking is less than a threshold number, it is determined that the front-end tracking has failed.

[0054] Front-end tracking determines the pose of the current frame by solving the relative pose between the previous and next frames. In the case of front-end tracking failure, re-initialization of vision can improve the robustness of front-end tracking and reduce trajectory interruptions caused by tracking loss.

[0055] In each embodiment of the present disclosure, front-end tracking is performed by combining tracking based on a uniform velocity model, tracking based on a reference frame, and tracking based on DR, thereby increasing the possibility of successful front-end tracking and improving mapping performance. The specific process of front-end tracking will be described below in conjunction with Figure 3 Provide further explanation.

[0056] In step 215, the current key frame of the current map is locally optimized to obtain an optimized key frame.

[0057] In some aspects, before the current keyframe is locally optimized, redundant map points in the current keyframe may be deleted. Redundant map points include map points in the current keyframe that are observed less than a threshold number of times by a common view keyframe, and the common view keyframe includes a keyframe in the historical keyframe that has a common view map point with the current keyframe.

[0058] By deleting redundant map points, map points that are not observed enough times can be removed, thus keeping the map concise and accurate.

[0059] In some embodiments, local BA (Local Bundle Adjustment) optimization may be performed on the current key frame. BA optimization is well known in the field of computer vision and will not be described in detail here.

[0060] In some aspects, in addition to deleting redundant map points, redundant keyframes can also be deleted. Redundant keyframes are keyframes with a large proportion of redundant map points (e.g., greater than a preset threshold). By deleting redundant keyframes, the number of keyframes can be reduced, thereby reducing the complexity and computation of BA while maintaining the accuracy and stability of the map.

[0061] Combine the following Figure 4 To describe the specific process of local optimization of key frames. Figure 4 An exemplary process 400 for locally optimizing a keyframe according to one embodiment of the present disclosure is shown.

[0062] like Figure 4 As shown, for a newly input key frame, all co-view key frames of the key frame may be first extracted according to the co-view relationship, and then all extracted co-view key frames may be traversed.

[0063] For each common view keyframe, its redundant map points can be detected. Specifically, all map points of each common view keyframe can be traversed, and the number of times each map point can be observed by other keyframes can be determined. If the number of observations is insufficient, the map point can be considered redundant. For example, if a map point cannot be observed by at least three other keyframes, the map point can be considered redundant and can be deleted to keep the map concise and accurate.

[0064] In an optional embodiment, in addition to detecting and deleting redundant map points, it is also possible to determine whether a key frame is redundant. For example, if more than 90% of the valid map points of a key frame are determined to be redundant map points, the key frame can be considered redundant and can be deleted from the map. In this way, the number of key frames can be reduced, thereby reducing the complexity and computational complexity of BA optimization.

[0065] After deleting redundant map points (and optionally redundant keyframes), new map points can be created using the undeleted map points. In order to ensure the quality of the map points during the creation of new map points, it is necessary to restrict the angle between the map point projection lines (the line connecting the camera optical center and the feature point) in the two keyframes during the triangulation of the map points. If the angle is too small, the depth uncertainty of the map point is too large and it is not suitable to be added to the map. In some embodiments of the present disclosure, the cosine value of the projection line angle can be restricted to less than 0.996 when triangulating the map points.

[0066] Subsequently, local BA optimization can be performed on the newly created map points to improve the map quality.

[0067] After the above process 400 is performed on each newly-incoming key frame, an optimized key frame can be obtained.

[0068] return Figure 2 After locally optimizing the current key frame, the method 200 proceeds to step 220. In step 220, a closed-loop matching frame matching the optimized key frame in the historical key frames is searched based on similarity, and the historical key frames include the key frames before the current key frame.

[0069] In various embodiments of the present invention, historical key frames may include key frames in the current map before the current key frame (e.g., key frames collected before the current key frame), and may also include key frames in historical maps. Historical maps may include maps that have been built and stored before the current map. Taking autonomous driving as an example, historical maps may be loaded from the vehicle locally (e.g., an onboard storage device) or downloaded from a server before creating the current map.

[0070] The purpose of step 220 is to determine whether the scene is the same as the one previously passed, and then determine whether it is necessary to expand the previously established historical map in this scene. To this end, a key frame with sufficient similarity to the optimized key frame can be found in the historical key frame as a matching closed-loop matching frame.

[0071] If the similarity between a certain historical keyframe and an optimized keyframe meets a preset condition (for example, the similarity is higher than a certain threshold), it means that the scene is currently the same as the one previously passed through, and the historical keyframe can be considered to match the optimized keyframe.

[0072] If the similarity between a certain historical keyframe and an optimized keyframe does not meet a preset condition (for example, the similarity is lower than a certain threshold), it can be considered that the current scene has not been passed before, or that the scene has changed too much although it has passed. In this case, it is impossible to build a map by expanding on the basis of the historical map, and it can be considered that the historical keyframe does not match the optimized keyframe.

[0073] In the specific implementation, the similarity between the historical keyframe and the optimized keyframe can be determined by appearance similarity and geometric consistency. First, the appearance similarity test is performed, and the Bag of Words (BoW) is used to find all the historical keyframes that have co-viewing with the current optimized keyframe. Then, the similarity between the global descriptor of the current optimized keyframe and the global descriptor of the historical keyframe is calculated, and the closed-loop candidate keyframe is obtained after screening. Then, the geometric consistency test can be performed. First, the 3D map points observed by the closed-loop candidate keyframe and the 2D feature points of the current optimized keyframe are matched. The relative pose between the current optimized keyframe and the closed-loop candidate keyframe is obtained by PnP solution using the 3D-2D matching point pairs. If the number of 3D-2D matching internal points after the pose solution is insufficient, the candidate frame is eliminated; if it is sufficient, the map points observed by other historical keyframes that have co-viewing with the closed-loop candidate keyframe are collected, transferred to the candidate frame coordinate system, and then the map points are reprojected to the current frame using the calculated relative pose and camera internal parameters. The 3D-2D inlier pairs are screened based on the reprojection error. If the number of inlier pairs is insufficient, the candidate frame is eliminated.

[0074] By setting the corresponding appearance similarity threshold (e.g., global descriptor similarity threshold) and geometric consistency threshold (e.g., 3D-2D matching inner point number threshold), historical key frames whose similarity with the current optimized key frame meets the preset conditions can be found, and the found historical key frames are used as closed-loop matching frames. If multiple historical key frames are found whose similarity with the current optimized key frame meets the preset conditions, the historical key frame with the highest similarity with the current optimized key frame can be selected as the closed-loop matching frame.

[0075] In step 225, it is determined whether the optimized key frame and the closed-loop matching frame belong to the same map.

[0076] In some embodiments, whether the optimized key frame and the closed-loop matching frame belong to the same map can be determined based on the map ID. For example, if the optimized key frame and the closed-loop matching frame are associated with the same map ID, it means that the two belong to the same map, otherwise it means that the two do not belong to the same map.

[0077] In step 230, when the optimized key frame and the closed-loop matching frame do not belong to the same map (the output of step 225 is "No"), the current map and the historical map where the closed-loop matching frame is located are merged.

[0078] When it is determined that the optimized keyframe and the closed-loop matching frame do not belong to the same map, it means that the closed-loop matching frame is not from the current map, but from a historical map created before the current map. At this time, the current map and the historical map to which the closed-loop matching frame belongs can be merged. In some embodiments, merging the current map and the historical map further includes: constructing a local window based on the common view frame of the optimized keyframe and the common view frame of the closed-loop matching frame; using all map points in the local window to determine the relative pose of the optimized keyframe and the closed-loop matching frame; modifying the pose of the optimized keyframe according to the relative pose; and unifying the current map and the historical map into the same coordinate system to achieve map merging.

[0079] In step 235, when it is determined that the optimized key frame and the closed-loop matching frame belong to the same map (the output of step 225 is "yes"), closed-loop correction is performed on the current map. The steps of closed-loop correction are basically the same as those of traditional ORB-SLAM.

[0080] In some embodiments, the global optimization can be accelerated during the closed-loop correction. Specifically, only the poses of all map points can be adjusted in some rounds of the global optimization, and the poses of all keyframes and all map points can be adjusted in the remaining rounds. In this way, the optimization time can be reduced and the map construction time can be saved.

[0081] The following combination Figure 5 To describe the specific process of map merging and closed-loop correction. Figure 5 An exemplary process 500 of map merging and closed-loop correction is shown according to one embodiment of the present disclosure.

[0082] like Figure 5 As shown, for each locally optimized current key frame ( Figure 5 The current key frame is shown as “current key frame” in FIG. 1 , and firstly, a closed-loop matching frame matching the current key frame can be found in the historical key frames.

[0083] As mentioned above, if no matching closed-loop matching frame is found, incremental mapping is not performed, but conventional mapping is performed instead. When a closed-loop matching frame matching the current key frame is found, it can be further determined whether the current key frame and the closed-loop matching frame belong to the same map. If they do not belong to the same map, it means that the closed-loop matching frame comes from a historical map. In this case, the current map and the historical map can be merged, such as Figure 5 The example process of map merging is as follows:

[0084] 1) Obtain the top 15 common view frames with the largest number of common viewpoints with the current key frame and the top 15 common view frames with the largest number of common viewpoints with the closed-loop matching frame, and construct a local window based on these common view frames.

[0085] 2) Use EPNP (Efficient PnP) pose correction for the local window. Specifically, all map points in the local window can be used to calculate the relative pose of the current keyframe and the closed-loop matching frame. In this process, all map points of the common view keyframe need to be converted to the closed-loop candidate frame coordinate system, and then find the associated matching pairs with the 2D points of the current frame based on the descriptor distance, and then use the EPNP algorithm to calculate the relative pose.

[0086] 3) Perform local BA optimization on the map points in the local window and merge the map points.

[0087] 4) Traverse all key frames of the current map, recursively modify the poses of all key frames in the current map according to the calculated relative poses, unify the current map and the historical map into the same coordinate system, and realize map merging. The recursive method is as follows:

[0088] Assume that the closed-loop relative pose is T_cand_curr, the keyframe queue of the current map is {F1, F2, ..., Fc}, where Fc represents the current keyframe, and the pose queue is {T1, T2, ..., T_curr}, where Tc represents the pose of the current keyframe. The keyframe pose queue after the final transformation is {T1′, T2′, ..., T curr ′}.

[0089] T curr ′=T cand_curr *T curr

[0090] T i ′=T curr ′*T curr_i

[0091] T curr_i =T curr -1 *T i

[0092] If the current key frame and the closed-loop matching frame belong to the same map, the current map can be closed-loop corrected, such as Figure 5 The exemplary process of closed-loop correction is as follows:

[0093] 1) Use the EPNP pose to correct the poses of all common view frames of the current key frame.

[0094] 2) Fuse map points and establish a common view relationship.

[0095] 3) Optimize the essential map to correct the map.

[0096] 4) Create a thread to perform global BA optimization.

[0097] 5) After completing the global BA optimization, update the map.

[0098] During the closed-loop correction process, the global BA optimization can be accelerated. For example, the global BA optimization can be performed as follows:

[0099] 1) Add all keyframes and map points as optimized vertices to the computational graph (Hyper Graph);

[0100] 2) Project the map point to the reprojected coordinates of the keyframe, and subtract the coordinates of the 2D feature point corresponding to the map point in the keyframe to obtain the reprojection error. Add the reprojection error as an edge to the computational graph (Hyper Graph);

[0101] 3) Perform a round of optimization to adjust the poses of all keyframes and map points (which can be called Whole Bundle Adjustment);

[0102] 4) Set all keyframe vertices to fixed (Set Fixed);

[0103] 5) Perform eight rounds of optimization, only adjusting the positions of all map points (which can be called Mappoint BundleAdjustment);

[0104] 6) Change the state of the key frame vertex in step 4) to fixed and the state to non-fixed;

[0105] 7) Perform a round of optimization (Whole Bundle Adjustment).

[0106] Traditional global BA does not have steps 4 and 6, and each round of optimization is a Whole Bundle Adjustment. Due to the existence of steps 4 and 6, the execution speed of the middle eight rounds of Mappoint Bundle Adjustment is very fast, almost only one percent of Whole Bundle Adjustment. Therefore, the above global BA method can almost reduce the running time of Global Bundle Adjustment by 80%, which is very significant when building maps over long distances and large scenes. As for the impact on the effect of Global Bundle Adjustment, since each keyframe vertex is subject to more constraints in the Hyper Graph (hundreds), and the map point vertex is subject to fewer constraints in the Hyper Graph (a few), the pose of the keyframe vertex converges quickly during the optimization process, and the map point position converges slowly. Therefore, compared with the traditional method, the above Global Bundle Adjustment acceleration strategy has almost no decrease in optimization effect.

[0107] return Figure 2 , by continuously collecting image frames (step 205) and executing steps 210-235 for each collected image frame, incremental mapping can be achieved. Continuing with the example of autonomous driving, an existing historical map can be loaded before the vehicle starts driving. Subsequently, during the driving of the vehicle, image frames of the vehicle's surrounding environment can be continuously collected based on the on-board sensors, and incremental mapping can be performed by executing method 200. Thus, in the case where the passing scene is extended, the map of the extended part can be incrementally constructed based on the original map of the scene without rebuilding the entire map. In this way, the problems of long time and high cost of failure in traditional single mapping acquisition can be avoided. At the same time, by combining multiple tracking modes to perform front-end tracking and re-initializing vision when front-end tracking fails, the robustness of front-end tracking can be improved and trajectory interruption caused by tracking loss can be reduced. In addition, by accelerating global optimization, the time consumption of large scene mapping can be significantly reduced without almost affecting the map quality.

[0108] Figure 3 An exemplary process 300 of front-end tracking according to one embodiment of the present disclosure is shown.

[0109] like Figure 3 As shown, at the beginning of process 300, it can be first determined whether initialization has been performed. If not, visual initialization can be attempted first. If initialized, front-end tracking can be performed directly.

[0110] Initialization can be performed based on the initial frame and the second frame in the captured image frames. Specifically, 1000 ORB (Oriented FAST and Rotated BRIEF) feature points and the descriptors corresponding to the feature points can be extracted from each of the two frames of images, and then the distance between the descriptors can be calculated to find the matching feature point pair set Feats in the two frames of images. Each point pair in Feats contains two feature points, which come from the initial frame and the second frame respectively.

[0111] Next, the basic transformation matrix F and the essential matrix E are calculated based on the set of matched feature point pairs (for example, OpenCV third-party libraries can be used to calculate them). The essential matrix E is then decomposed to obtain the relative pose T of the second frame relative to the initial frame. The relative pose T contains the rotation R and the translation t, where R is a 3*3 matrix and t is a 3*1 vector [t x t y t z ] T The relationship between the essential matrix E and the basic matrix F and the relationship between the essential matrix E and R, t are as follows:

[0112]

[0113] E=K T *F*K

[0114] E=[t] × R

[0115]

[0116] To extract R and t from the essential matrix E, we need to perform singular value decomposition (SVD) on E:

[0117] E=UΣV T

[0118] According to the results of SVD, two possible rotation matrices R1 and R2 and two possible translation vectors t1 and t2 can be obtained.

[0119] T1=UWV T ,R2=UW T V T

[0120] t=U[:,3]

[0121] where t is the last column of the U matrix.

[0122]

[0123] Since there may be four combinations of decomposition (respectively [R1t], [R2t], [R1-t], [R2-t]), additional geometric verification (for example, the condition that the depth value is greater than 0) is required to select the correct combination of R and t. The verification method is as follows: randomly select a pair of matching feature points from the set of matching feature point pairs, assuming it is [x1 x2], which are the observed pixel coordinates of the same 3D point in the image for the initial frame and the second frame. Use the results of the four combinations to triangulate the 3D point respectively. If the depth value of the triangulated 3D point in the camera coordinate system is less than 0, it is not the correct solution. Finally, the relative pose is used to solve the 3D coordinates P_w (i.e., map points) in the map coordinate system of all 2D feature points observed in the two frames. Triangulation is well known in the field of computer vision and will not be repeated here.

[0124] If the initialization fails, re-initialize. After successful initialization, tracking based on a constant velocity model (also called "track by motion model") can be performed.

[0125] Assume that T_0, T_1, and T_2 represent the three frames in time order, where T_2 is the current frame, T_1 is the previous frame of T_2, and T_0 is the previous frame of T_1. The uniform velocity model assumes that the relative pose between T_0 and T_1 is equal to the relative pose between T_1 and T_2.

[0126] The general process of tracking based on the uniform velocity model is as follows: First, the predicted pose T_predict of the current frame is calculated based on the relative pose T_delta of the previous and next frames and the pose T_last of the previous frame. Then, the set of map points observed in the previous frame is projected into the current frame image using the predicted pose and the camera internal parameters, thereby obtaining the 2D reprojection coordinate set of the feature point set Feats_last observed in the previous frame in the current frame image. Then, in the neighborhood of each reprojection coordinate (a circular area with a radius of 30 pixels centered on the reprojection coordinate), the feature points matching the feature points in the previous frame Feats are searched (all feature points within the region in the current frame image are traversed, and the descriptor distance is calculated one by one with the feature points in the previous frame Feats, and the feature points corresponding to the minimum descriptor distance are selected), and the matching 3D-2D point pairs are obtained, where the 3D points (map points) come from the previous frame and the 2D points (feature points) come from the current frame. It should be noted that for each frame, the map points and feature points are one-to-one corresponding. Based on the acquired 3D-2D point pairs, the pose of the current frame can be solved by the PnP (Perspective-n-Point) algorithm. The PnP algorithm is well known in the field of computer vision and will not be described in detail here.

[0127] In the above process, T_delta is calculated by the following formula:

[0128] T delta =T prev -1 *T last

[0129] Among them, T last represents the pose corresponding to the previous frame (T_1) of the current frame (T_2), T prev Indicates the pose corresponding to the previous frame (T_0) of T_1.

[0130] T_predict is calculated as follows:

[0131] T predict =T last *T delta

[0132] The map point reprojection formula is as follows:

[0133]

[0134] Among them, u, v are the pixel coordinates after projection, and K is the camera intrinsic parameter matrix.

[0135] If the number of map points matched by the tracking based on the uniform velocity model is insufficient (for example, less than a threshold number (such as 20)), the tracking based on the uniform velocity model can be considered to have failed, otherwise the tracking based on the uniform velocity model can be considered to have succeeded. If the tracking based on the uniform velocity model is successful, the tracking based on the uniform velocity model can continue to track the local map. If the tracking based on the uniform velocity model fails, it can be switched to the tracking based on the reference frame (also called "track by reference keyframe").

[0136] The general process of reference frame-based tracking is as follows: all feature points of the current frame are brute-force matched with all feature points of the previous key frame (that is, each feature point in the current frame calculates the descriptor distance with all feature points of the previous key frame, and selects the matching point with the smallest descriptor distance and meets the threshold), thereby obtaining a set of 3D map point-2D feature point matching pairs, and then continues to use PnP to solve the current frame pose.

[0137] Similarly, if the number of map points matched by the reference frame-based tracking is insufficient, the reference frame-based tracking can be considered to have failed, otherwise the reference frame-based tracking can be considered to have succeeded. If the reference frame-based tracking is successful, the reference frame-based tracking can continue to be performed to track the local map. If the reference frame-based tracking fails, it can be switched to perform DR-based tracking (also called "track by DR").

[0138] The general process of DR-based tracking is as follows: use DR to obtain the relative pose T_delta from the previous frame to the current frame; use the relative pose and the pose of the previous frame to calculate the predicted pose of the current frame; use the predicted pose of the current frame and the camera intrinsic parameters to reproject the map points of the previous frame into the current frame image, search for matching feature point pairs, and obtain a set of 3D-2D point pairs; use the obtained 3D-2D point pair set to perform PnP to solve the current frame pose.

[0139] Similarly, if the number of map points matched by DR-based tracking is insufficient, it can be considered that the DR-based tracking has failed, otherwise it can be considered that the DR-based tracking has succeeded.

[0140] If DR-based tracking fails, it can be considered that the front-end tracking has failed and reinitialization is required. The relative pose between the first keyframe after reinitialization and the last keyframe before the front-end tracking failed is given by DR to ensure the consistency of the trajectory coordinate system before and after reinitialization. After reinitialization, front-end tracking can continue.

[0141] When the front-end tracking is successful, it can be determined whether to add the current frame as a key frame. If it is determined that a key frame is to be added, the current frame can be added as a key frame of the current map.

[0142] The front-end tracking disclosed in the present invention improves the possibility of successful front-end tracking by combining tracking based on a uniform velocity model, tracking based on a reference frame, and tracking based on DE. At the same time, by reinitializing in the event of tracking failure, the robustness of tracking can be improved and the trajectory interruption caused by tracking loss can be reduced.

[0143] Figure 6 An incremental mapping system 600 according to one embodiment of the present disclosure is shown.

[0144] See also Figure 6 , the system 600 may include a data acquisition unit 602, a keyframe adding unit 604, a keyframe optimizing unit 606, a closed-loop matching unit 608, and an incremental mapping unit 610. Each of these units may be connected or communicated with each other directly or indirectly on one or more buses 612.

[0145] The data acquisition unit 602 may be configured to acquire sensor data, where the sensor data includes a plurality of image frames acquired within a continuous time period.

[0146] The key frame adding unit 604 may be configured to add one or more frames of the plurality of image frames as key frames of the current map.

[0147] In some embodiments, the key frame adding unit 604 can be further configured to: determine the pose of the current frame among multiple image frames; determine the relative pose of the current frame and the previous key frame; and if the relative pose meets preset conditions, add the current frame as a key frame of the current map.

[0148] The key frame optimization unit 606 may be configured to perform local optimization on the current key frame of the current map to obtain an optimized key frame.

[0149] In some embodiments, the key frame optimization unit 606 can be further configured to: delete redundant map points in the current key frame to obtain a streamlined key frame, wherein the redundant map points include map points in the current key frame that are observed by the common view key frame less than a threshold number of times, and the common view key frame includes key frames in the historical key frames that have common view map points with the current key frame; and perform local optimization on the streamlined key frame to obtain an optimized key frame.

[0150] In some embodiments, the key frame optimization unit 606 may be further configured to: determine the proportion of redundant map points in the current key frame; and if the proportion is greater than a preset threshold, delete the current key frame.

[0151] The closed-loop matching unit 608 may be configured to search for a closed-loop matching frame in historical key frames that matches the optimized key frame based on similarity, where the historical key frames include key frames before the current key frame.

[0152] The incremental mapping unit 610 can be configured to: determine whether the optimized key frame and the closed-loop matching frame belong to the same map; and if the optimized key frame and the closed-loop matching frame do not belong to the same map, perform map merging on the current map and the historical map where the closed-loop matching frame is located.

[0153] In some embodiments, the incremental mapping unit 610 may be further configured to: when the optimized key frame and the closed-loop matching frame belong to the same map, perform closed-loop correction on the current map.

[0154] Although Figure 6 Specific units of system 600 are shown, but it should be understood that these units are exemplary and non-restrictive. In different implementations, one or more of these units can be combined, split, removed, or additional units can be added. For example, in some implementations, key frame adding unit 604 and key frame optimization unit 606 can be combined into a single unit. In some implementations, system 600 can also include additional units, such as a map loading unit for loading an original map, and a map management unit (not shown in the figure) for managing each map.

[0155] Figure 7A schematic diagram of a computer-readable medium 706 configured to store computer-executable instructions 704 is shown according to one embodiment of the present disclosure.

[0156] like Figure 7 As shown, in this embodiment, an exemplary computer-readable storage medium 706 is provided. Computer-executable instructions 704 (hereinafter also referred to as instructions) are stored on the computer-readable storage medium 706. In this embodiment, the instructions 704 can be configured to implement a method (as shown in the dashed box 702) when executed by the processor, such as Figure 2 In another embodiment, the instructions 704 may also be configured to implement a system such as Figure 6 The system 600 shown. As can be appreciated by those skilled in the art, Figure 7 The step boxes in the dashed box 702 in are merely exemplary and non-restrictive. A person of ordinary skill in the art can design many such computer-readable media that can be configured to perform the desired method or implement the desired system. The term "computer-readable storage medium" used herein includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information such as computer-executable instructions or other data. Memory and storage are examples of computer-readable storage media. Computer-readable storage media can be non-transient and include, but are not limited to: RAM, ROM, EEPROM, flash memory or other storage technology, CD-ROM, digital versatile disk (DVD) or other optical storage, tape cassette, tape, disk storage or other magnetic storage device, or any other medium that can be used to store the desired information and can be accessed by a computing device.

[0157] Although not required, the various embodiments are described in the general context of "computer-executable instructions" executed by one or more processors. Computer-executable instructions can be distributed via computer-readable storage media. Computer-executable instructions can be implemented as program units that perform a particular method or implement a particular system, such as functions, objects, application programming interfaces (APIs), data structures, etc. In general, the functionality of computer-executable instructions can be combined or distributed in various environments as needed.

[0158] The term "computer-readable storage medium" may also include communication media. Communication media typically embody computer-executable instructions or other data in a "modulated data signal" such as a carrier wave or other transmission mechanism, and includes any information transmission medium. The term "modulated data signal" may include a signal that sets or changes one or more of its characteristics in a manner that encodes the information in the signal. Those skilled in the art will recognize that computer-readable storage media used to store computer-executable instructions can be distributed over a network. For example, a computer-readable storage medium accessible via a network may store computer-executable instructions to implement one or more embodiments provided by the present disclosure. A computing device may access a computer-readable storage medium over a network and download part or all of the computer-executable instructions for execution. Alternatively, some instructions may be executed on one computing device, while other instructions may be executed on another computing device.

[0159] The detailed description described above in conjunction with the accompanying drawings describes examples and does not represent all examples that can be implemented or fall within the scope of the claims. The terms "example" and "exemplary" when used in this specification mean "used as an example, instance or illustration" and do not mean "better or better than other examples".

[0160] Reference throughout this specification to "one embodiment" or "an embodiment" means that a particular feature, structure, or characteristic described in conjunction with the embodiment is included in at least one embodiment of the present application. Thus, use of these phrases may refer to more than just one embodiment. Furthermore, the described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0161] The previous description is provided to enable any person skilled in the art to practice the various aspects described herein. Various modifications to these aspects will be readily apparent to those skilled in the art, and the universal principles defined herein may be applied to other aspects. Therefore, the claims are not intended to be limited to the aspects shown herein, but should be granted the full scope consistent with the claims in language, wherein the singular reference to the elements is not intended to mean "there is and only one", but "one or more", unless otherwise specifically stated. Unless otherwise specifically stated, the term "some" refers to one or more. The elements of the various aspects described throughout this application are all structurally and functionally equivalent schemes currently or hereafter known to those of ordinary skill in the art, and are expressly incorporated herein by reference, and are intended to be covered by the claims.

[0162] It should also be noted that these embodiments may be described as a process depicted as a flow chart, flow diagram, structure diagram, or block diagram. Although the flow chart may describe the operations as sequential processes, many of these operations can be performed in parallel or concurrently. In addition, the order of these operations can be rearranged.

[0163] Although various embodiments have been illustrated and described, it should be understood that the embodiments are not limited to the precise configuration and components described above. Various modifications, substitutions and improvements obvious to those skilled in the art may be made in the arrangement, operation and details of the devices disclosed herein without departing from the scope of the claims.

Claims

1. An incremental mapping method, comprising: Acquiring sensor data, the sensor data comprising a plurality of image frames acquired over a continuous period of time; adding one or more frames of the plurality of image frames as key frames of the current map; Locally optimizing a current key frame of the current map to obtain an optimized key frame; Searching for a closed-loop matching frame in historical key frames that matches the optimized key frame based on similarity, wherein the historical key frames include key frames before the current key frame; Determining whether the optimized key frame and the closed-loop matching frame belong to the same map; as well as In a case where the optimized key frame and the closed-loop matching frame do not belong to the same map, a map merge is performed on the current map and the historical map where the closed-loop matching frame is located.

2. The method according to claim 1, wherein: Adding one or more frames of the plurality of image frames as key frames of the current map further comprises: Determining a pose of a current frame among the plurality of image frames; Determining the relative pose of the current frame and the previous key frame; and If the relative position meets a preset condition, the current frame is added as a key frame of the current map.

3. The method according to claim 2, wherein: Determining the pose of a current frame among the plurality of image frames further comprises: Perform visual initialization based on an initial frame and a second frame among the multiple image frames; Performing front-end tracking to determine the pose of the current frame; and If the front-end tracking fails, re-initialize the vision. When the number of map points obtained by the front-end tracking is less than a threshold number, it is determined that the front-end tracking fails.

4. The method according to claim 3, wherein: Executing front-end tracing further includes: Perform tracking based on a constant velocity model; If the tracking based on the constant velocity model fails, the tracking based on the reference frame is performed; If reference frame based tracking fails, performing DR based tracking; and If DR-based tracking fails, it is determined that the front-end tracking has failed.

5. The method according to claim 1, wherein: Locally optimizing the current key frame of the current map to obtain an optimized key frame further includes: Deleting redundant map points in the current key frame to obtain a simplified key frame, wherein the redundant map points include map points in the current key frame that are observed by a common view key frame less than a threshold number of times, and the common view key frame includes a key frame in the historical key frames that has a common view map point with the current key frame; and Local optimization is performed on the reduced key frame to obtain the optimized key frame.

6. The method according to claim 5, further comprising: Determining a proportion of redundant map points in the current key frame; as well as If the proportion is greater than a preset threshold, the current key frame is deleted.

7. The method according to claim 1, wherein: Merging the current map with the historical map further includes: constructing a local window based on the common view frame of the optimized key frame and the common view frame of the closed-loop matching frame; Determine the relative position of the optimized key frame and the closed-loop matching frame using all map points in the local window; modifying the pose of the optimized keyframe according to the relative pose; and The current map and the historical map are unified into the same coordinate system to achieve map merging.

8. The method according to claim 1, further comprising: In a case where the optimized key frame and the closed-loop matching frame belong to the same map, closed-loop correction is performed on the current map.

9. The method according to claim 8, further comprising: During the closed-loop correction, multiple rounds of global optimization are performed, wherein only the poses of all map points are adjusted in a portion of the multiple rounds of global optimization, and the poses of all key frames and all map points are adjusted in the remaining rounds.

10. A system for incremental mapping using the method according to any one of claims 1 to 9, comprising: A data acquisition unit configured to: acquire sensor data, the sensor data comprising a plurality of image frames acquired within a continuous time period; A key frame adding unit, configured to: add one or more frames of the plurality of image frames as key frames of the current map; A key frame optimization unit, configured to: locally optimize a current key frame of the current map to obtain an optimized key frame; A closed-loop matching unit, configured to: search for a closed-loop matching frame in historical key frames that matches the optimized key frame based on similarity, the historical key frames including key frames before the current key frame; and The incremental mapping unit is configured as follows: Determining whether the optimized key frame and the closed-loop matching frame belong to the same map; as well as In a case where the optimized key frame and the closed-loop matching frame do not belong to the same map, a map merge is performed on the current map and the historical map where the closed-loop matching frame is located.

11. A computer-readable storage medium having stored thereon instructions for incremental mapping, wherein the instructions, when executed by a processor, implement the method according to any one of claims 1 to 9.

Images