Mapping loopback verification method, electronic equipment and storage medium

Abstract

The invention provides a mapping loopback verification method, electronic equipment and a storage medium, and the method comprises the steps: obtaining a first track point and a second track point corresponding to a current frame pose and a historical frame pose in a mapping track in response to the detection that the current frame pose and the historical frame pose form a loopback relation, the method comprises the following steps: performing track consistency check through a first track point and a second track point, acquiring a first local semantic map corresponding to the first track point and a second local semantic map corresponding to the second track point under the condition that the track consistency check is passed, and performing semantic consistency check, thereby realizing check of loop detection in a mapping process. The loop checking precision is improved through track consistency checking and semantic consistency checking, and the problems of error checking, high checking cost and the like in the prior art are solved.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, and particularly to a mapping loop verification method, an electronic device, and a storage medium. Background Art

[0002] During the learning mapping process for memory parking in a parking lot scenario, long-distance mapping will cause continuous accumulation of mapping trajectory errors, resulting in obvious non-coincidence between the mapping starting point and the ending point, thus affecting vehicle autonomous cruising and parking. Therefore, after detecting a loop in the vehicle's driving trajectory, loop optimization can be performed to eliminate the cumulative error. There is a possibility of false detection in the loop detection method. If the falsely detected loop points cannot be filtered out, it will cause map chaos due to incorrect loops.

[0003] The existing verification after loop detection is generally divided into the following categories: 1. Detection based on time and space. This method has incorrect verification when the vehicle cannot ensure loops occur in multiple consecutive frames; 2. Detection based on the mapping trajectory. This method has incorrect verification when the starting point and the ending point are respectively located on two adjacent roads; 3. Deep learning-based method. This method may have false detection for the same or similar locations, and moreover, this method relies on a large amount of data to train the model, and the implementation cost of the whole process is high.

[0004] In addition, Patent 1 (CN113744236A) discloses determining the transformation matrix between two frames, using the transformation matrix for conversion and obtaining the confidence verification result to achieve loop detection; Patent 2 (CN116698046B) discloses constructing an optimization problem using the constraint relationship between two state quantities to achieve loop detection; Patent 3 (CN117146795A) discloses loop verification through point cloud registration; Patent 4 (CN118397093A) discloses loop detection through the similarity of features in two images. The above patents can perform loop detection or loop verification. However, they do not analyze the accuracy of the loop from multiple perspectives, resulting in low accuracy of loop detection or verification. Summary of the Invention

[0005] In view of the above defects or deficiencies in the prior art, this application aims to provide a mapping loop verification method, an electronic device, and a storage medium to solve the problems of incorrect verification and high verification cost in the prior art.

[0006] An embodiment of this application provides a mapping loop verification method, which includes:

[0007] In response to detecting that the current frame pose forms a loop relationship with the historical frame pose, obtain a first trajectory point corresponding to the current frame pose and a second trajectory point corresponding to the historical frame pose in the mapping trajectory, and determine a trajectory consistency test result based on the first trajectory point and the second trajectory point;

[0008] In response to the trajectory consistency test result passing the test, obtain a first local semantic map corresponding to the first trajectory point and a second local semantic map corresponding to the second trajectory point;

[0009] Based on the first local semantic map and the second local semantic map, determine a semantic consistency test result between the current frame pose and the historical frame pose.

[0010] Optionally, obtaining a first trajectory point corresponding to the current frame pose and a second trajectory point corresponding to the historical frame pose in the mapping trajectory includes:

[0011] Obtain a first timestamp corresponding to the current frame pose and a second timestamp corresponding to the historical frame pose;

[0012] In the mapping trajectory, find the trajectory point at the moment closest to the first timestamp as the first trajectory point, and find the trajectory point at the moment closest to the second timestamp as the second trajectory point.

[0013] Optionally, determining a trajectory consistency test result based on the first trajectory point and the second trajectory point includes:

[0014] Determine adjacent trajectory points of the first trajectory point in the mapping trajectory. If there is a point identical to the second trajectory point among the adjacent trajectory points, determine the cumulative distance difference between the first trajectory point and the second trajectory point;

[0015] Determine the trajectory consistency test result based on the cumulative distance difference.

[0016] Optionally, after determining the adjacent trajectory points of the first trajectory point in the mapping trajectory, it further includes:

[0017] Judge whether there is a point identical to the second trajectory point among the adjacent trajectory points;

[0018] Among them, judging whether there is a point identical to the second trajectory point among the adjacent trajectory points includes:

[0019] According to the coordinate values of the adjacent trajectory points in the preset first dimension, perform a first sorting on the adjacent trajectory points;

[0020] Take the neighboring trajectory points at the middle position in the first sorting result as the root node, take the neighboring trajectory points on the left side of the middle position as the left subtree set, and take the neighboring trajectory points on the right side of the middle position as the right subtree set;

[0021] According to the coordinate values of each of the neighboring trajectory points in the preset second dimension, perform a second sorting on the points in the left subtree set and the right subtree set;

[0022] Construct the left subtree and the right subtree of the root node according to the second sorting result to obtain a search tree;

[0023] Start traversing downward from the root node of the search tree to search for whether there is a node identical to the second trajectory point.

[0024] Optionally, determining the trajectory consistency test result based on the cumulative distance difference includes:

[0025] According to the cumulative distance difference and a preset distance difference threshold, determine the trajectory consistency test result between the current frame pose and the historical frame pose.

[0026] Optionally, determining the semantic consistency test result between the current frame pose and the historical frame pose based on the first local semantic map and the second local semantic map includes:

[0027] Determine the matching error score between the first local semantic map and the second local semantic map;

[0028] If the matching error score is less than a preset error score threshold, determine that the semantic consistency test result is passed.

[0029] Optionally, obtaining the first local semantic map corresponding to the first trajectory point and the second local semantic map corresponding to the second trajectory point includes:

[0030] In the mapping trajectory, with the first trajectory point as the midpoint, obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the first trajectory point to form a first local point cloud map;

[0031] In the mapping trajectory, with the second trajectory point as the midpoint, obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the second trajectory point to form a second local point cloud map;

[0032] Input the first local point cloud map and the second local point cloud map into a pre-trained semantic segmentation model respectively to obtain the semantic labels of each point in the first local point cloud map and the second local point cloud map, and form the first local semantic map and the second local semantic map respectively.

[0033] Optionally, the method further includes:

[0034] If the result of the semantic consistency check passes, update the first trajectory point based on the second trajectory point.

[0035] An embodiment of the present application further provides an electronic device, where the electronic device includes:

[0036] A processor and a memory;

[0037] The processor is configured to execute the steps of the mapping loop closure verification method provided in any embodiment of the present application by calling the program or instruction stored in the memory.

[0038] An embodiment of the present application further provides a computer-readable storage medium, where the computer-readable storage medium stores a program or instruction, and the program or instruction causes a computer to execute the steps of the mapping loop closure verification method provided in any embodiment of the present application.

[0039] In summary, the present application proposes a mapping loop closure verification method. In response to detecting that the current frame pose forms a loop relationship with the historical frame pose, the first trajectory point and the second trajectory point corresponding to the current frame pose and the historical frame pose are obtained in the mapping trajectory. The trajectory consistency is verified through the first trajectory point and the second trajectory point. Further, when the result of the trajectory consistency verification passes, the first local semantic map corresponding to the first trajectory point and the second local semantic map corresponding to the second trajectory point are obtained, and the semantic consistency is verified through the first local semantic map and the second local semantic map, so as to realize the verification of the loop detection in the mapping process. Considering that the two points with an actual loop relationship are very close, when a loop is detected, the corresponding trajectory points in the mapping trajectory can be verified first to initially verify the loop detection. Further, considering the semantic proximity between the two points with an actual loop relationship, the corresponding local semantic maps are further verified when the trajectory consistency verification passes to further verify the loop detection, ensuring the verification accuracy after the loop detection. The method analyzes from the trajectory consistency and the semantic consistency respectively, can perform verification from the perspectives of trajectory and semantics respectively, and does not require model training, solving the problems of incorrect verification and high verification cost in the related art. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] In order to more clearly illustrate the specific embodiments of the present application or the technical solutions in the prior art, the following will briefly introduce the drawings required for the description of the specific embodiments or the prior art. Obviously, the following drawings are some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0041] Figure 1 It is a flowchart of a mapping loop closure verification method provided by an embodiment of the present application;

[0042] Figure 2 It is a schematic diagram of loop closure verification provided by an embodiment of the present application;

[0043] Figure 3 It is a schematic structural diagram of a mapping loop closure verification device provided by an embodiment of the present application;

[0044] Figure 4 It is a schematic structural diagram of an electronic device provided by an embodiment of the present application. Detailed implementation manners

[0045] The present application will be further described in detail below with reference to the accompanying drawings and embodiments. It can be understood that the specific embodiments described herein are only used to explain the related invention, rather than limiting the invention. Additionally, it should be noted that for the sake of description, only the parts related to the invention are shown in the drawings.

[0046] It should be noted that, without conflict, the embodiments in the present application and the features in the embodiments can be combined with each other. The present application will be described in detail below with reference to the drawings and embodiments.

[0047] As mentioned in the background art, in view of the problems in the prior art, the present application proposes a mapping loop closure verification method. Figure 1 It is a flowchart of a mapping loop closure verification method provided by an embodiment of the present application. Refer to Figure 1 , the mapping loop closure verification method specifically includes:

[0048] S110. In response to detecting that the current frame pose forms a loop closure relationship with the historical frame pose, obtain the first trajectory point corresponding to the current frame pose and the second trajectory point corresponding to the historical frame pose in the mapping trajectory, and determine the trajectory consistency verification result based on the first trajectory point and the second trajectory point.

[0049] Among them, the current frame pose may be the real-time vehicle pose detected by the visual perception system carried by the vehicle. Specifically, the vehicle pose can be detected in real time by the visual sensor in the visual perception system, and then the loop closure detection is performed on the detected current frame pose to determine whether there is a historical frame pose that has a loop closure relationship with the current frame pose.

[0050] In the embodiments of the present application, a loop can refer to a vehicle returning to a historical position after movement. Therefore, the pose of the current frame can be compared with the poses of each historical frame. If there is a historical frame pose whose pose difference from the current frame pose is less than a preset difference, it can be considered that the current frame pose is similar to the historical frame pose, and the vehicle returns to the historical position after movement, forming a loop relationship between the current frame pose and the historical frame pose.

[0051] Specifically, if it is detected that a loop relationship is formed between the current frame pose and the historical frame pose, the detected loop relationship can be verified to ensure the accuracy of loop detection and avoid the influence of incorrect loop detection on map confusion.

[0052] In the embodiments of the present application, the verification of loop detection can be divided into two steps. The first step is trajectory consistency verification, and the second step is semantic consistency detection. In the trajectory consistency verification, verification can be performed through the trajectory points corresponding to the current frame pose and the historical frame pose in the mapping trajectory. Among them, the mapping trajectory can be the vehicle trajectory obtained by mapping using the odometer measurement values, and this trajectory can be composed of various points.

[0053] It should be noted that after loop detection is performed using the vehicle pose detected by the vision sensor, verification after loop detection using the data detected by the odometer sensor can ensure the reliability of verification and avoid loop detection errors and verification errors caused by errors in a single sensor.

[0054] In a specific implementation manner, obtaining the first trajectory point corresponding to the current frame pose and the second trajectory point corresponding to the historical frame pose in the mapping trajectory includes the following steps:

[0055] Step 11: Obtain the first timestamp corresponding to the current frame pose and the second timestamp corresponding to the historical frame pose;

[0056] Step 12: In the mapping trajectory, find the trajectory point at the moment closest to the first timestamp as the first trajectory point, and find the trajectory point at the moment closest to the second timestamp as the second trajectory point.

[0057] Among them, in Step 11, the first timestamp corresponding to the current frame pose can be determined first, and the second timestamp corresponding to the historical frame pose can be determined.

[0058] Furthermore, in Step 12, the differences between each moment in the mapping trajectory and the first timestamp can be compared, and the trajectory point at the moment closest to the first timestamp among them is used as the first trajectory point. Also, the differences between each moment in the mapping trajectory and the second timestamp can be compared, and the trajectory point at the moment closest to the second timestamp among them is used as the second trajectory point.

[0059] Through the above-mentioned Step 11 - Step 12, when there is a loop phenomenon in the poses detected by the visual sensor, the trajectory points in the mapping trajectory obtained by odometry mapping can be used for verification to ensure the verification accuracy after loop detection. Moreover, considering that the data acquisition frequencies of the visual sensor and the odometry sensor are different, which may result in the non-existence of trajectory points at the moments corresponding to the poses, therefore, the trajectory points at the closest moment can be used for subsequent verification to ensure the verification reliability.

[0060] After obtaining the first trajectory point and the second trajectory point, the trajectory consistency can be verified through the first trajectory point and the second trajectory point to obtain the trajectory consistency verification result.

[0061] In a specific implementation manner, determining the trajectory consistency verification result based on the first trajectory point and the second trajectory point includes:

[0062] Determine the neighboring trajectory points of the first trajectory point in the mapping trajectory. If there is a point identical to the second trajectory point among the neighboring trajectory points, determine the cumulative distance difference between the first trajectory point and the second trajectory point; determine the trajectory consistency verification result based on the cumulative distance difference.

[0063] Specifically, the neighboring trajectory points close to the first trajectory point can be searched from the mapping trajectory. Exemplarily, the search range can be obtained, and with the first trajectory point as the center in the mapping trajectory, all other trajectory points except the first trajectory point within the search range are used as the neighboring trajectory points of the first trajectory point.

[0064] Among them, the search range can be a preset default value, or the search range can be determined according to the real-time driving speed of the vehicle. The higher the driving speed, the farther the vehicle moves per unit time, and the larger the search range.

[0065] After determining all the neighboring trajectory points, each neighboring trajectory point can be compared with the second trajectory point to determine whether there is a point identical to the second trajectory point among the neighboring trajectory points. If so, it means that the straight-line distance between the second trajectory point and the first trajectory point is close, and the cumulative distance difference between the first trajectory point and the second trajectory point can be further determined.

[0066] If there is no point identical to the second trajectory point among the neighboring trajectory points, it means that the straight-line distance between the second trajectory point and the first trajectory point is far. Considering that the positions between two frames with an actual loop relationship are close, while the first trajectory point and the second trajectory point are far apart, it can reflect that the position deviation of the vehicle between the current frame and the historical frame at these two moments is large. Therefore, it can be determined that the verification of the loop relationship between the current frame pose and the historical frame pose is abnormal, that is, the verification of the loop relationship between the current frame pose and the historical frame pose fails.

[0067] In the embodiments of the present application, to determine whether there is a point identical to the second trajectory point among the respective adjacent trajectory points, it is possible to compare each adjacent trajectory point with the second trajectory point in sequence. Alternatively, to improve the comparison efficiency, a two-dimensional search tree of each adjacent trajectory point can be constructed, and the adjacent trajectory points are compared with the second trajectory point in sequence from near to far to improve the overall search efficiency.

[0068] In a specific implementation manner, after determining the adjacent trajectory points of the first trajectory point in the mapping trajectory, it further includes: determining whether there is a point identical to the second trajectory point among the respective adjacent trajectory points;

[0069] Among them, determining whether there is a point identical to the second trajectory point among the respective adjacent trajectory points includes the following steps:

[0070] Step 21: Perform a first sorting on each adjacent trajectory point according to the coordinate values of each adjacent trajectory point in the preset first dimension;

[0071] Step 22: Use the adjacent trajectory point located in the middle position in the first sorting result as the root node, use the adjacent trajectory points located on the left side of the middle position as the left subtree set, and use the adjacent trajectory points located on the right side of the middle position as the right subtree set;

[0072] Step 23: Perform a second sorting on the points in the left subtree set and the right subtree set according to the coordinate values of each adjacent trajectory point in the preset second dimension;

[0073] Step 24: Construct the left subtree and the right subtree of the root node according to the second sorting result to obtain a search tree;

[0074] Step 25: Traverse downward from the root node of the search tree to search for whether there is a node identical to the second trajectory point.

[0075] Among them, the x-axis can be used as the preset first dimension and the y-axis can be used as the preset second dimension, or the y-axis can be used as the preset first dimension and the x-axis can be used as the preset second dimension.

[0076] In step 21, each adjacent trajectory point can be sorted according to the coordinate values of each adjacent trajectory point in the preset first dimension. Further, in step 22, in the first sorting result, the adjacent trajectory point at the middle position (i.e., the median) is used as the root node, and the adjacent trajectory points with coordinate values less than the root node in the preset first dimension are used as the left subtree set for constructing the left subtree of the root node, and the adjacent trajectory points with coordinate values greater than the root node in the preset first dimension are used as the right subtree set for constructing the right subtree of the root node.

[0077] Further, in step 23, the adjacent trajectory points can be sorted according to their coordinate values in a preset second dimension. In step 24, in the second sorting result, the median of the left subtree set is taken as the left child node of the root node, and the remaining adjacent trajectory points in the left subtree set are continuously extended downward along this left child node (for example, they can be sorted according to the coordinate values in the preset first dimension and the median is taken), and the median of the right subtree set is taken as the right child node of the root node, and the remaining adjacent trajectory points in the right subtree set are continuously extended downward along this right child node (for example, they can be sorted according to the coordinate values in the preset first dimension and the median is taken), to obtain the entire search tree.

[0078] Among them, the search tree can reflect the distribution of all adjacent trajectory points in a two-dimensional space. The search tree is a data structure for space partitioning, and each node in it can represent a hyper-rectangular region in a two-dimensional space.

[0079] Further, in step 25, starting from the root node of the search tree, it can be traversed downward to search for whether there is a node identical to the second trajectory point until reaching the leaf node or a node identical to the second trajectory point is found.

[0080] Through the above steps 21 - 22, the efficiency of searching for the second trajectory point among all adjacent trajectory points can be improved, and the complexity of the search can be significantly reduced, thereby being able to improve the verification efficiency of loop detection.

[0081] In the embodiment of the present application, if a point identical to the second trajectory point is found among the adjacent trajectory points, the cumulative distance difference between the first trajectory point and the second trajectory point can be further determined. Among them, the cumulative distance difference can be the absolute value of the difference between the cumulative distance of the first trajectory point and the cumulative distance of the second trajectory point. The cumulative distance refers to all the mileage traveled by the vehicle since the current mapping started.

[0082] Considering that the cumulative distance difference between two points with an actual loop relationship is relatively large, therefore, the loop relationship can be further verified according to the cumulative distance difference.

[0083] In one example, determining the trajectory consistency verification result based on the cumulative distance difference includes:

[0084] According to the cumulative distance difference and a preset distance difference threshold, determine the trajectory consistency verification result between the current frame pose and the historical frame pose.

[0085] Specifically, the cumulative travel distance difference can be compared with a preset travel distance threshold, which can be the minimum travel distance difference preset for the vehicle to reach a loop. If the cumulative travel distance difference is greater than the preset travel distance threshold, it means that the vehicle returns to a similar position after traveling a certain distance. Considering that the aforementioned second trajectory point exists among the neighboring trajectory points of the first trajectory point, the trajectory consistency test result can be determined as passing at this time, that is, the current frame pose and the historical frame pose satisfy the trajectory consistency.

[0086] If the cumulative travel distance difference is less than the preset travel distance threshold, it means that the vehicle returns to a similar position after traveling a certain distance, which means that the vehicle has not traveled enough distance to reach a similar position. At this time, the trajectory consistency test result can be determined as failing, that is, the current frame pose and the historical frame pose do not satisfy the trajectory consistency.

[0087] Through the above implementation, the trajectory consistency test can be realized by first checking whether the second trajectory point exists among the neighboring trajectory points of the first trajectory point and the cumulative travel distance difference between the second trajectory point and the first trajectory point. This can verify whether there is consistency among the trajectory points in the loop relationship and whether enough distance has been traveled to reach a similar position, ensuring the accuracy of the loop check.

[0088] S120. In response to the trajectory consistency test result being passed, obtain the first local semantic map corresponding to the first trajectory point and the second local semantic map corresponding to the second trajectory point.

[0089] Specifically, if the trajectory consistency test result is passed, semantic consistency test can be performed to improve the accuracy of the loop check. Among them, the semantic consistency test can be realized through the local semantic maps of the two trajectory points. Of course, if the trajectory consistency test result is failed, the loop relationship can be determined as an abnormal loop.

[0090] Among them, the first local semantic map includes the point cloud data of the surrounding trajectory points of the first trajectory point and the corresponding semantic labels, and the second local semantic map includes the point cloud data of the surrounding trajectory points of the second trajectory point and the corresponding semantic labels.

[0091] In a specific implementation, obtaining the first local semantic map corresponding to the first trajectory point and the second local semantic map corresponding to the second trajectory point includes the following steps:

[0092] Step 31. In the mapping trajectory, with the first trajectory point as the midpoint, obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the first trajectory point to form the first local point cloud map;

[0093] Step 32: In the mapping trajectory, with the second trajectory point as the midpoint, obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the second trajectory point to form a second local point cloud map.

[0094] Step 33: Input the first local point cloud map and the second local point cloud map into a pre-trained semantic segmentation model respectively to obtain the semantic labels of each point in the first local point cloud map and the second local point cloud map, and form a first local semantic map and a second local semantic map respectively.

[0095] Among them, in step 31, the first trajectory point can be used as the midpoint to obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the first trajectory point in the mapping trajectory to constitute the first local point cloud map. And in step 32, the second trajectory point can be used as the midpoint to obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the second trajectory point in the mapping trajectory to constitute the second local point cloud map.

[0096] Among them, the preset distance can be determined in real time according to the driving speed of the vehicle. The higher the driving speed, the larger the preset distance. Or the preset distance can be a preset fixed value.

[0097] Further, in step 33, the first local point cloud map and the second local point cloud map can be input into a pre-trained semantic segmentation model respectively to obtain the semantic labels of each point in the first local point cloud map and the semantic labels of each point in the second local point cloud map, so as to form a first local semantic map and a second local semantic map including semantic labels and local point cloud maps. Among them, the semantic labels can include categories and category probabilities.

[0098] Through the above steps 31 - step 33, the construction of the first local semantic map and the second local semantic map can be realized, which is convenient for subsequent matching according to the local point cloud and the semantics in the local point cloud, and ensures the accuracy of the matching.

[0099] S130: Based on the first local semantic map and the second local semantic map, determine the semantic consistency test result between the current frame pose and the historical frame pose.

[0100] After obtaining the first local semantic map and the second local semantic map, further, the first local semantic map and the second local semantic map can be matched, and the semantic consistency test result between the current frame pose and the historical frame pose can be determined according to the matching result.

[0101] In one example, determining the semantic consistency test result between the current frame pose and the historical frame pose based on the first local semantic map and the second local semantic map includes:

[0102] Determine the matching error score between the first local semantic map and the second local semantic map; if the matching error score is less than the preset error score threshold, determine that the semantic consistency test result is passed.

[0103] Among them, the first local semantic map and the second local semantic map can be matched to obtain a matching error score, which can reflect the gap between the first local semantic map and the second local semantic map.

[0104] Exemplarily, points of the same category can be selected from the first local semantic map and the second local semantic map as candidate points for matching. Then, for each pair of candidate points, the geometric error is determined according to the Euclidean distance between the candidate points, and the semantic error is determined according to the category probability gap between the candidate points. The geometric error and the semantic error are fused through the weights corresponding to the geometric error and the weights corresponding to the semantic error to obtain the error score of this pair of candidate points.

[0105] Furthermore, the overall matching error score is obtained by synthesizing the error scores of all candidate points. For example, a high weight can be set for static object categories (such as buildings, ground), and a low weight can be set for dynamic object categories (such as vehicles, pedestrians). Then, according to the weights corresponding to the categories of the candidate points, the error scores of all candidate points are fused.

[0106] Considering that there are errors in the measurement of dynamic objects, points of the dynamic object category can also be removed from the first local semantic map and the second local semantic map before matching to avoid the interference of dynamic objects on the matching result.

[0107] After obtaining the matching error score, it can be compared with the preset error score threshold. The preset error score threshold is the maximum error score for which the two local semantic maps are well-matched. If the matching error score is less than the preset error score threshold, it is determined that the semantic consistency test result is passed. Through this implementation, the accuracy of semantic map matching can be improved, thereby further improving the verification accuracy of loop detection.

[0108] Furthermore, if the semantic consistency test result is passed, the loop relationship can be determined as a correct loop; if the semantic consistency test result fails, the loop relationship can be determined as an abnormal loop.

[0109] Exemplarily, Figure 2 is a schematic diagram of loop verification provided by an embodiment of the present application, as Figure 2As shown, first, if a loop relationship is detected, further search for the first trajectory point and the second trajectory point for trajectory consistency verification. If the trajectory consistency verification passes, construct the first local semantic map and the second local semantic map for semantic consistency verification. If the trajectory consistency verification fails, it can be determined as a false detected loop.

[0110] If the semantic consistency verification passes, it can be determined as a correct loop. Otherwise, it can be determined as a false detected loop. By trajectory consistency verification and semantic consistency detection, the verification accuracy of loop detection can be further improved.

[0111] In the embodiment of the present application, if the verification result of the loop relationship is a correct loop, trajectory optimization can be performed to reduce the cumulative error in long-distance mapping.

[0112] In some embodiments, the method provided by the embodiment of the present application further includes:

[0113] If the semantic consistency verification result is passed, update the first trajectory point based on the second trajectory point.

[0114] Specifically, if the semantic consistency verification result is passed, it indicates that the loop relationship is a correct loop. At this time, it can be determined that the current frame pose coincides with the historical frame pose, and the vehicle returns to the position of the historical frame at the current frame. Therefore, the second trajectory point can be used to update the first trajectory point, and then update the map according to the updated first trajectory point.

[0115] Exemplarily, the relative pose transformation matrix between the first trajectory point and the second trajectory point can be calculated, a loop edge (constraint) is added to the trajectory, and an information matrix (weight) is obtained. The information matrix can reflect the confidence of the constraint, and the information matrix can be calculated based on the sensor accuracy or the matching error score. Further, the loop edge, the first trajectory point, and the inverse of the information matrix (covariance matrix) can be substituted into the optimization problem to obtain the optimized second trajectory point.

[0116] Through the above embodiments, after completing the verification of loop detection, the first trajectory point can be updated to make the first trajectory point approach the second trajectory point, eliminate the trajectory distortion caused by sensor noise and odometer drift, correct the cumulative error, and ensure the alignment of the trajectory at the loop closure, so as to ensure the global consistency of the map.

[0117] The mapping loop detection verification method provided by the embodiment of the present application, in response to detecting that the current frame pose and the historical frame pose form a loop relationship, obtains the first trajectory point and the second trajectory point corresponding to the current frame pose and the historical frame pose respectively in the mapping trajectory, performs trajectory consistency verification through the first trajectory point and the second trajectory point, and then, in the case where the trajectory consistency verification result is passed, obtains the first local semantic map corresponding to the first trajectory point and the second local semantic map corresponding to the second trajectory point, and performs semantic consistency verification through the first local semantic map and the second local semantic map, so as to realize the verification of loop detection in the mapping process. This method takes into account that the two points with an actual loop relationship are very close. When a loop is detected, it can first verify the corresponding trajectory points in the mapping trajectory to preliminarily verify the loop detection. Then, considering the semantic proximity between the two points with an actual loop relationship, it further verifies the corresponding local semantic maps in the case where the trajectory consistency verification is passed to further verify the loop detection, ensuring the verification accuracy after loop detection, without the need for model training, and solving the problems of incorrect verification and high verification cost in the prior art.

[0118] Compared with the consistency detection based on time and space, this method sets up a loop caching mechanism, believing that a single detected loop is not sufficient to form a good constraint, and only the loops continuously detected over a period of time are correct loops. This can be regarded as consistency detection in terms of time. Feature matching is performed on the two frames detected in the loop to estimate the movement of the camera. Then, the movement is placed into the previous pose graph to check if there is a large difference from the previous estimate. In the memory parking scenario, loops may also occur when passing through intersections. The intersections in the parking lot are often only six meters wide. When the vehicle passes through at a normal speed, it cannot guarantee that loops occur in multiple consecutive frames, resulting in false detection. The method provided by the embodiment of the present application can ensure the loop verification accuracy in the memory parking scenario.

[0119] Moreover, compared with the consistency detection based on the mapping trajectory, this method believes that the odometer has errors when learning mapping, and these errors can be quantitatively calculated. After detecting a loop, the trajectory error at the loop is calculated according to the odometer error, and at the same time, it is detected whether the vehicle's previous trajectory passes through this point within this error range and compared with the result of the loop detection. If the result of the loop detection is consistent with the result of the trajectory detection, it is considered that the result of this loop detection is correct. When the starting point and the ending point of the mapping are on two adjacent roads respectively and the distance is within the error range of the odometer, this method cannot correctly perform the post-loop verification. The method provided by the embodiment of the present application can ensure the loop verification accuracy in the case where the starting point and the ending point of the mapping are on two adjacent roads respectively.

[0120] Moreover, compared with the post-loop verification implemented by the deep learning method, this method uses deep learning for image similarity detection. If the similarity between the images near the pre-loop and the images near the post-loop exceeds the threshold, it is considered that a correct loop has occurred. This method is prone to false detection for the same or similar locations, and the deep learning-based method requires a large amount of data to train the model, resulting in a high R & D cost. The method provided in the embodiments of this application can ensure the accuracy of loop verification for the same or similar locations, and the implementation cost of the method is low.

[0121] Figure 3 FIG. 4 is a schematic structural diagram of a mapping loop verification device provided by an embodiment of this application. The device includes a trajectory consistency verification module 310, a semantic map acquisition module 320, and a semantic consistency verification module 330, where:

[0122] The trajectory consistency verification module 310 is configured to, in response to detecting that a loop relationship is formed between the current frame pose and the historical frame pose, obtain a first trajectory point corresponding to the current frame pose and a second trajectory point corresponding to the historical frame pose in the mapping trajectory, and determine a trajectory consistency verification result based on the first trajectory point and the second trajectory point;

[0123] The semantic map acquisition module 320 is configured to, in response to the trajectory consistency verification result being passed, obtain a first local semantic map corresponding to the first trajectory point and a second local semantic map corresponding to the second trajectory point;

[0124] The semantic consistency verification module 330 is configured to determine a semantic consistency verification result between the current frame pose and the historical frame pose based on the first local semantic map and the second local semantic map.

[0125] On the basis of the above embodiments, optionally, the trajectory consistency verification module 310 is specifically configured to:

[0126] Obtain a first timestamp corresponding to the current frame pose and a second timestamp corresponding to the historical frame pose;

[0127] In the mapping trajectory, find the trajectory point at the moment closest to the first timestamp as the first trajectory point, and find the trajectory point at the moment closest to the second timestamp as the second trajectory point.

[0128] Based on the above embodiments, optionally, the trajectory consistency checking module 320 is further configured to determine adjacent trajectory points of the first trajectory point in the mapping trajectory, and if there is a point identical to the second trajectory point among the adjacent trajectory points, determine the cumulative path difference between the first trajectory point and the second trajectory point; and determine the trajectory consistency checking result based on the cumulative path difference.

[0129] Based on the above embodiments, optionally, the trajectory consistency checking module 320 is further configured to determine whether there is a point identical to the second trajectory point among the adjacent trajectory points; perform a first sorting on the adjacent trajectory points according to the coordinate values of the adjacent trajectory points in a preset first dimension;

[0130] Take the adjacent trajectory point at the middle position in the first sorting result as the root node, take the adjacent trajectory points on the left side of the middle position as the left subtree set, and take the adjacent trajectory points on the right side of the middle position as the right subtree set;

[0131] Perform a second sorting on the points in the left subtree set and the right subtree set according to the coordinate values of the adjacent trajectory points in a preset second dimension;

[0132] Construct the left subtree and the right subtree of the root node according to the second sorting result to obtain a search tree;

[0133] Start traversing downward from the root node of the search tree to search for a node identical to the second trajectory point.

[0134] Based on the above embodiments, optionally, the trajectory consistency checking module 320 is further configured to determine the trajectory consistency checking result between the current frame pose and the historical frame pose according to the cumulative path difference and a preset path difference threshold.

[0135] Based on the above embodiments, optionally, the semantic consistency checking module 330 is specifically configured to:

[0136] Determine the matching error score between the first local semantic map and the second local semantic map;

[0137] If the matching error score is less than a preset error score threshold, determine that the semantic consistency checking result is passed.

[0138] Based on the above embodiments, optionally, the semantic map acquisition module 320 is further configured to:

[0139] In the mapping trajectory, with the first trajectory point as the midpoint, obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the first trajectory point to form a first local point cloud map;

[0140] In the mapping trajectory, taking the second trajectory point as the midpoint, obtain the point cloud data corresponding to each trajectory point within a preset distance before and after the second trajectory point, and form a second local point cloud map;

[0141] Input the first local point cloud map and the second local point cloud map into a pre-trained semantic segmentation model respectively, obtain the semantic labels of each point in the first local point cloud map and the second local point cloud map, and form a first local semantic map and a second local semantic map respectively.

[0142] Based on the above embodiments, optionally, the device provided in the embodiments of the present application further includes an optimization module, which is used to update the first trajectory point based on the second trajectory point if the semantic consistency check result is passed.

[0143] The mapping loop verification device provided in the embodiments of the present application can execute the steps in the mapping loop verification method provided in the method embodiments of the present application, and the implementation steps and beneficial effects are not described herein again.

[0144] Figure 4 It is a schematic structural diagram of an electronic device provided in the embodiments of the present application. As Figure 4 shown, the electronic device 400 includes one or more processors 401 and a memory 402.

[0145] The processor 401 may be a central processing unit (CPU) or other forms of processing units with data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 400 to perform desired functions.

[0146] The memory 402 may include one or more computer program products, and the computer program products may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 401 may run the program instructions to implement the mapping loop verification method of any embodiment of the present application described above and / or other desired functions. Various contents such as initial external parameters and thresholds may also be stored in the computer-readable storage medium.

[0147] In one example, the electronic device 400 may further include: an input device 403 and an output device 404, and these components are interconnected through a bus system and / or other forms of connection mechanisms (not shown). The input device 403 may include, for example, a keyboard, a mouse, and the like. The output device 404 may output various information to the outside, including warning prompt information, braking force, and the like. The output device 404 may include, for example, a display, a speaker, a printer, and a communication network and its connected remote output devices, and the like.

[0148] Of course, for simplicity, Figure 4 only some of the components related to the present application in the electronic device 400 are shown, and components such as a bus, an input / output interface, and the like are omitted. In addition, according to specific application scenarios, the electronic device 400 may further include any other appropriate components.

[0149] In addition to the above methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, and when the computer program instructions are run by a processor, the processor is caused to execute the steps of the map building and loop closure verification method provided in any embodiment of the present application.

[0150] The computer program product may be written in any combination of one or more programming languages for programming code to perform the operations of the embodiments of the present application. The programming languages include object-oriented programming languages such as Java, C++, etc., and also include conventional procedural programming languages such as the "C" language or similar programming languages. The programming code may be executed entirely on the user computing device, partially on the user device, executed as an independent software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0151] Furthermore, an embodiment of the present application may also be a computer-readable storage medium, on which computer program instructions are stored, and when the computer program instructions are run by a processor, the processor is caused to execute the steps of the map building and loop closure verification method provided in any embodiment of the present application.

[0152] The computer-readable storage medium may adopt any combination of one or more readable media. The readable media may be a readable signal medium or a readable storage medium. The readable storage medium may include, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples (non-exhaustive list) of the readable storage medium include: an electrical connection having one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0153] It should be noted that the terms used in this application are only for describing specific embodiments and do not limit the scope of this application. As shown in the specification and claims of this application, unless the context clearly indicates otherwise, words such as "a", "an", "one", and / or "the" are not specifically singular and may also include the plural. The term "comprising", "including", or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, or device including a series of elements not only includes those elements but also includes other elements not explicitly listed, or also includes elements inherent to such a process, method, or device. Without further limitation, an element defined by the statement "including one..." does not exclude the existence of additional identical elements in the process, method, or device including the said element.

[0154] It should also be noted that the orientation or positional relationship indicated by terms such as "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing this application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus cannot be understood as a limitation of this application. Unless otherwise clearly specified and defined, terms such as "installed", "connected", "coupled" should be understood in a broad sense. For example, it may be a fixed connection, a detachable connection, or an integral connection; it may be a mechanical connection or an electrical connection; it may be directly connected or indirectly connected through an intermediate medium, and it may be the internal communication of two elements. For those of ordinary skill in the art, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0155] In this text, specific examples are used to illustrate the principles and implementation manners of the present application. The description of the above embodiments is only used to help understand the method and its core idea of the present application. The above is only the preferred implementation manner of the present application. It should be noted that due to the limitation of literal expression and objectively infinite specific structures, for those of ordinary skill in the art, without departing from the principles of the present application, several improvements, refinements or changes can be made, or the above technical features can be combined in an appropriate manner; these improvements, refinements, changes or combinations, or directly applying the inventive concept and technical solution to other occasions without improvement, shall all be regarded as the protection scope of the present application.

Claims

1. A mapping loop check method, characterized in that, Including: In response to detecting that the current frame pose and the historical frame pose form a loop relationship, obtain a first trajectory point corresponding to the current frame pose and a second trajectory point corresponding to the historical frame pose in the mapping trajectory, and determine a trajectory consistency test result based on the first trajectory point and the second trajectory point; In response to the trajectory consistency test result passing the test, obtain a first local semantic map corresponding to the first trajectory point and a second local semantic map corresponding to the second trajectory point; Based on the first local semantic map and the second local semantic map, determine a semantic consistency test result between the current frame pose and the historical frame pose.

2. The method according to claim 1, wherein Obtaining a first trajectory point corresponding to the current frame pose and a second trajectory point corresponding to the historical frame pose in the mapping trajectory includes: Obtain a first timestamp corresponding to the current frame pose and a second timestamp corresponding to the historical frame pose; In the mapping trajectory, find the trajectory point at the moment closest to the first timestamp as the first trajectory point, and find the trajectory point at the moment closest to the second timestamp as the second trajectory point.

3. The method according to claim 1, wherein Determining a trajectory consistency test result based on the first trajectory point and the second trajectory point includes: Determine adjacent trajectory points of the first trajectory point in the mapping trajectory. If there is a point identical to the second trajectory point among the adjacent trajectory points, determine the cumulative distance difference between the first trajectory point and the second trajectory point; Determine the trajectory consistency test result based on the cumulative distance difference.

4. The method according to claim 3, characterized in that After determining the adjacent trajectory points of the first trajectory point in the mapping trajectory, it further includes: Judge whether there is a point identical to the second trajectory point among the adjacent trajectory points; Among them, judging whether there is a point identical to the second trajectory point among the adjacent trajectory points includes: According to the coordinate values of the adjacent trajectory points in the preset first dimension, perform a first sorting on the adjacent trajectory points; Take the adjacent trajectory point in the middle position in the first sorting result as the root node, take the adjacent trajectory points on the left side of the middle position as the left subtree set, and take the adjacent trajectory points on the right side of the middle position as the right subtree set; According to the coordinate values of the adjacent trajectory points in the preset second dimension, perform a second sorting on the points in the left subtree set and the right subtree set; Construct a left subtree and a right subtree of the root node according to the second sorting result to obtain a search tree; Start traversing downward from the root node of the search tree to search for whether there is a node identical to the second trajectory point.

5. The method according to claim 3, characterized in that Determining a trajectory consistency test result based on the cumulative distance difference includes: According to the cumulative distance difference and a preset distance difference threshold, determine a trajectory consistency test result between the current frame pose and the historical frame pose.

6. The method according to claim 1, wherein Determining a semantic consistency test result between the current frame pose and the historical frame pose based on the first local semantic map and the second local semantic map includes: Determine the matching error score between the first local semantic map and the second local semantic map; If the matching error score is less than a preset error score threshold, determine that the semantic consistency test result is passed.

7. The method according to claim 1, characterized in that, Obtaining the first local semantic map corresponding to the first trajectory point and the second local semantic map corresponding to the second trajectory point includes: In the mapping trajectory, taking the first trajectory point as the midpoint, obtaining the point cloud data corresponding to each trajectory point within a preset distance before and after the first trajectory point to form a first local point cloud map; In the mapping trajectory, taking the second trajectory point as the midpoint, obtaining the point cloud data corresponding to each trajectory point within a preset distance before and after the second trajectory point to form a second local point cloud map; Inputting the first local point cloud map and the second local point cloud map into a pre-trained semantic segmentation model respectively to obtain the semantic labels of each point in the first local point cloud map and the second local point cloud map, and forming a first local semantic map and a second local semantic map respectively.

8. The method according to claim 1, wherein The method further includes: If the semantic consistency test result is passed, update the first trajectory point based on the second trajectory point.

9. An electronic device, characterized in that, The electronic device includes: A processor and a memory; The processor is configured to execute the steps of the mapping loop verification method according to any one of claims 1 to 8 by calling the program or instructions stored in the memory.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a program or instructions, and the program or instructions cause a computer to execute the steps of the mapping loop verification method according to any one of claims 1 to 8.

Images