Point cloud layered detection method and device, electronic equipment and readable medium

Abstract

The invention discloses a point cloud layered detection method and device, electronic equipment and a readable medium, and belongs to the technical field of image processing. The point cloud hierarchical detection method comprises the following steps: based on a to-be-detected track and corresponding point cloud frame data, extracting a key frame in the track; inputting the position information of the key frame into a k-d tree, and carrying out loopback pair dense search in a track according to a set step length; calculating an initial relative pose of the searched loopback pair, and calculating a calibration relative pose of the searched loopback pair after feature matching; and calculating the pose change between the initial relative pose and the calibration relative pose, and judging whether point cloud layering exists or not based on the pose change. According to the point cloud hierarchical detection method provided by the invention, a thought based on loopback search and feature matching is adopted, dense loopback search is carried out on the track points, and possible hierarchical regions can be accurately detected among a plurality of track frames.

Description

Technical Field

[0001] The present invention belongs to the technical field of image processing, and in particular relates to a method, device, electronic device and readable medium for point cloud layered detection. Background Art

[0002] Point cloud layered detection technology has received extensive attention in the fields of computer vision, robotics, and autonomous driving in recent years. With the development of sensor technology, point cloud data has become an important form of representing three-dimensional spatial information and is widely used in many fields such as object detection, environmental modeling, and scene reconstruction. The core goal of point cloud layered detection technology is to extract point clouds that may appear in the same scene from three-dimensional point cloud data, that is, to detect and identify repeated or overlapping point cloud areas caused by sensor errors, environmental interference, or object motion.

[0003] Existing point cloud layered detection methods can be mainly divided into two categories: based on traditional feature fitting algorithms and based on deep learning. Traditional algorithms based on feature fitting usually analyze point cloud data by extracting geometric features, such as plane, surface or edge features. The advantages of this type of method are simple implementation, fast calculation speed, and effective point cloud layering under certain conditions. However, these methods usually rely on the design of manual features, are sensitive to the quality and noise of point cloud data, and are prone to errors in complex scenes.

[0004] On the other hand, point cloud layered detection methods based on deep learning have gradually gained attention in recent years. Deep learning methods can automatically learn features from large amounts of data through neural networks, and can perform point cloud layered detection in more complex scenes. In particular, advanced deep learning models such as convolutional neural networks (CNNs) and graph neural networks (GNNs) have made certain progress in the application of point cloud data processing. However, these methods usually require a large amount of labeled data for training, and the training process has high requirements on computing resources and a long training time. In addition, in the case of unbalanced data or more noise, the detection accuracy will be greatly reduced.

[0005] Therefore, it is necessary to provide a new solution to the above technical problems. Summary of the invention

[0006] The object of the present invention is to provide a method, device, electronic device and readable medium for point cloud layer detection, which can efficiently detect the point cloud layer situation and improve the quality of point cloud mapping.

[0007] To achieve the above purpose, the technical solution provided by the present invention is as follows: In a first aspect, the present invention provides a method for point cloud layered detection, comprising: Based on the trajectory to be detected and its corresponding point cloud frame data, the key frames in the trajectory are extracted; the position information of the key frames is input into the kd tree, and a dense search for loop pairs is performed in the trajectory with a set step size; the initial relative pose of the searched loop pairs is calculated, and the calibrated relative pose of the searched loop pairs after feature matching is calculated; the pose change between the initial relative pose and the calibrated relative pose is calculated, and whether there is point cloud stratification based on the pose change.

[0008] In one or more embodiments, extracting key frames in the trajectory includes: extracting point cloud frames whose relative poses between adjacent frames in the trajectory exceed a set threshold as key frames.

[0009] In one or more implementations, the method further includes: after searching for the loop pair, selecting adjacent frames of the loop pair to form a sub-image pair together with the loop pair for matching.

[0010] In one or more embodiments, feature matching is performed on the searched loop pairs, including: using the GICP algorithm to extract line and surface features in the point cloud, and performing feature matching on the searched loop pairs.

[0011] In one or more embodiments, the posture change calculation formula between the initial relative posture and the calibration relative posture is:

[0012] in, T left2right_raw is the initial relative pose, T left2right_align To calibrate the relative pose, For posture changes.

[0013] In one or more embodiments, judging whether there is point cloud stratification based on the posture change includes: The last column is the position change before and after feature matching, and the 3×3 rotation matrix in the upper left corner is the posture change before and after feature matching; determine whether the position change and posture change exceed the preset threshold; if so, there is point cloud stratification.

[0014] In one or more embodiments, the method further includes: optimizing the detected trajectory by using trajectory points with point cloud stratification as graph optimization loop factors; and re-performing point cloud stratification detection and graph optimization on the optimized trajectory until the point cloud stratification is eliminated.

[0015] In the second aspect, the present invention provides a point cloud layer detection device, which includes: an extraction module, a search module, a calculation module and a judgment module; the extraction module is used to extract key frames in the trajectory based on the trajectory to be detected and its corresponding point cloud frame data; the search module is used to input the position information of the key frames into the kd tree, and perform a dense search for loop pairs in the trajectory with a set step size; the calculation module is used to calculate the initial relative pose of the searched loop pairs, and calculate the calibrated relative pose of the searched loop pairs after feature matching; the judgment module is used to calculate the pose change between the initial relative pose and the calibrated relative pose, and judge whether there is point cloud layering based on the pose change.

[0016] In a third aspect, the present invention provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the method for point cloud layered detection as described above when executing the program.

[0017] In a fourth aspect, the present invention provides a computer-readable medium, wherein the computer-readable medium carries computer-executable instructions, and when the computer-executable instructions are executed by a processor, they are used to implement the method for point cloud layered detection as described above.

[0018] Compared with the prior art, the method, device, electronic device and readable medium for point cloud layered detection provided by the present invention adopt an idea based on loop search and feature matching. By performing dense loop search on trajectory points, possible layered areas can be accurately detected between multiple trajectory frames. This method not only takes into account relative pose changes, but also introduces matching of point cloud features, further improving the accuracy and stability of layered detection. Compared with traditional detection methods based on single pose estimation, the use of loop search and graph optimization can more comprehensively and robustly process point cloud data in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the prior art descriptions are briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0020] Figure 1 It is a flow chart of a method for point cloud layered detection in one embodiment of the present invention; Figure 2 This is a rendering of the effect of point cloud layered detection in one embodiment of the present invention; Figure 3 Schematic diagram of a point cloud layered detection device in one embodiment of the present invention; Figure 4 FIG. 4 is a schematic diagram of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0021] In order to enable those skilled in the art to better understand the technical solutions in the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0022] Point cloud layered detection technology has received extensive attention and research in the fields of computer vision, robotics, and autonomous driving in recent years. As a discrete representation of three-dimensional space, point cloud data has rich spatial information. Therefore, it has been widely used in many fields such as environmental modeling, object detection, and scene reconstruction. However, in practical applications, point cloud data often has ghosting phenomena. These ghostings are caused by similar point cloud data obtained from multiple scans or different perspectives, resulting in layered point cloud data affecting subsequent point cloud processing and analysis. Although the existing point cloud layered detection methods provide a theoretical basis, they have problems such as low detection accuracy, poor robustness, and high dependence on computing resources in practical applications. A new technical solution is urgently needed to improve the existing methods.

[0023] The technical idea of ​​the present invention is derived from an in-depth analysis of existing technical problems. Traditional point cloud hierarchical detection methods mainly rely on feature matching and fitting algorithms. Although these methods have certain advantages in theory, they often cannot provide sufficient accuracy and reliability in complex dynamic environments. In addition, although the detection method based on deep learning can achieve higher detection accuracy, it has a relatively demanding demand for a large amount of labeled data and computing resources, and is not suitable for use in environments with scarce data or limited computing resources.

[0024] The technical idea of ​​the present invention is to start from the root cause of point cloud stratification and propose a point cloud stratification detection method based on dense loop search. Point cloud stratification is common in environmental modeling in the field of autonomous driving or robotics. Usually, when a vehicle or robot passes the same place multiple times, due to the difference in posture at different time points, the collected point cloud data overlaps and differs, resulting in two or more layers of point cloud data at the same place. This phenomenon is usually related to the lack of loop factors or insufficient constraints on loop factors, which in turn leads to the inability to correctly align point cloud data during global optimization, resulting in an inaccurate stratification effect.

[0025] Existing layered detection methods mostly focus on using traditional feature matching technology or deep learning methods to determine the layering of point cloud data. However, these methods often fail to effectively capture layering problems caused by missing loops or insufficient optimization. The present invention proposes a solution for the generation mechanism of point cloud layering from a more fundamental perspective. The core idea is to identify and correct layering problems caused by missing loops or insufficient constraints through dense loop searches of trajectory points.

[0026] The core technology of the present invention is to find recurring point cloud areas by performing dense loop searches on key frames in trajectory data. For these loop areas, the change in posture between the initial relative posture and the calibrated relative posture is calculated to determine whether point cloud stratification exists. Specifically, the present invention improves the ability to recognize point cloud stratification in complex scenes while ensuring detection accuracy by combining loop-pair subgraph matching and the GICP algorithm. Through multiple iterations of loop detection and subgraph splicing and matching, the present invention can accurately evaluate the posture differences between different areas, thereby effectively identifying and separating point cloud stratification.

[0027] The present invention avoids the excessive reliance on feature extraction of traditional methods, and can maintain high detection accuracy in complex scenes through dense loop search and sub-graph splicing; the method combines the analysis of posture changes, effectively reduces the interference of environmental noise and dynamic factors on the detection results, and improves the robustness of hierarchical detection; finally, the method of the present invention does not rely on a large amount of labeled data, can maintain high accuracy in the absence of annotations, has strong adaptability, and can be flexibly used in different application scenarios.

[0028] Please refer to Figure 1 FIG. 1 is a flow chart of a method for point cloud layered detection in an embodiment of the present invention. The method for point cloud layered detection specifically includes the following steps: S101: extracting key frames in the trajectory based on the trajectory to be detected and its corresponding point cloud frame data.

[0029] During the autonomous driving process, the vehicle will generate a large number of track points and corresponding point cloud data. These point cloud data record the environmental information of the vehicle at different locations and time points, reflecting the three-dimensional structure of the surrounding scene. However, not every frame of point cloud data can provide valuable information for layered detection. In many cases, only track points that have changed significantly within a specific time period are the focus of analysis. Therefore, extracting key frames becomes an important step.

[0030] In the specific implementation, you first need to input the vehicle's trajectory data and the corresponding point cloud frame. The trajectory data is usually collected by the autonomous driving system through GPS, IMU or other sensors, indicating the vehicle's driving route and its posture information on the route. The point cloud frame is obtained by LiDAR or other sensors, representing the three-dimensional point cloud data of the vehicle's location and surrounding environment at a certain moment. These point cloud data usually contain a lot of redundant information, especially when the vehicle is driving, the changes between many point cloud frames are very small, which is not enough to reflect the significant changes in the scene.

[0031] In an exemplary embodiment, extracting key frames in the trajectory includes: extracting point cloud frames in which the relative posture between adjacent frames in the trajectory exceeds a set threshold as key frames.

[0032] In order to extract key frames from it, a threshold can be set to filter according to the relative pose changes between the track points. When the pose change between two adjacent frames exceeds the set threshold, it is considered that the environmental scene corresponding to the point cloud data of these two frames has changed significantly, so these frames are extracted as key frames. In this way, the selection of key frames is no longer arbitrary, but based on the actual scene changes, ensuring that the extracted key frames represent important features in the environment. For example, when a vehicle suddenly turns or enters a new area, the pose change between the track points will be very significant. At this time, the corresponding point cloud frame can provide more scene change information and become the key data for subsequent analysis.

[0033] When a vehicle is driving, it often experiences different scene changes, including acceleration, deceleration, stationary, slow driving, etc. In these different states, the point cloud data collected by the vehicle often have significant differences, and these differences mainly come from the changes in the vehicle's motion state. In order to ensure the accuracy and effectiveness of the point cloud data, the vehicle state needs to be considered when extracting key frames.

[0034] In practical applications, the trajectory data of the vehicle is usually continuously collected and recorded by on-board sensors (such as lidar or cameras). As the vehicle moves, the vehicle's posture (position and orientation) will change continuously. These posture changes are usually continuous, but in certain specific cases (such as when the vehicle stops, slows down, or turns), the posture changes between adjacent frames may be small. In these cases, the data of the adjacent frames before and after do not bring about significant scene changes. Continuing to include these frames for processing may not add more information, but will increase the computational complexity and even introduce errors.

[0035] The present invention selects key frames by calculating the relative posture changes between adjacent frames and setting a threshold. Specifically, when the posture change between two adjacent frames exceeds the threshold, it is considered that there are sufficient displacements and rotations between the two frames, indicating that the two frames show significant differences in space, and this difference may represent an important change in the scene. Therefore, the point cloud data in these two frames will be considered as key frames for subsequent loop closure search and matching. On the contrary, if the posture change between adjacent frames is small, it means that the scene change between the two frames is not large, which may be just a small movement of the vehicle in a certain area, or the vehicle has not changed significantly due to its slow speed, and these frames can be ignored.

[0036] For example, when an autonomous vehicle is driving at a low speed on a straight road, the vehicle's posture changes may be very slow, and the posture differences between adjacent frames are also small. These frames will be considered redundant and do not need to be saved as key frames. When the vehicle drives to an intersection or makes a sharp turn, the vehicle's posture changes suddenly become larger, and the posture differences between adjacent frames are significant. At this time, these frames will be extracted as key frames because they represent significant changes in the scene during the vehicle's driving process.

[0037] In this way, the present invention effectively filters out those meaningless and relatively stable frames when selecting key frames, retains those representative frames, and greatly improves the processing efficiency and accuracy of point cloud data. For point cloud layered detection, only when the point cloud data comes from key frames can the detected scene changes and position changes be ensured to have sufficient reliability and validity, and the hierarchical distribution in the point cloud data can be judged more accurately to avoid false detection or missed detection.

[0038] S102: Input the position information of the key frame into the kd tree, and perform a dense search for loop pairs in the trajectory with a set step size.

[0039] The kd tree (K-dimensional tree) can be used to store the position information of the key frames, and the kd tree can be used to perform dense loop pair search to ensure that loop pairs similar to the current trajectory can be found.

[0040] In practical applications, autonomous vehicles often encounter repeated scenes during driving. For example, when a vehicle is circling a certain road section, it may pass the same place many times, and the trajectory points at adjacent moments may show similar location information. If these repeated trajectory points are not identified, it is easy to cause the problem of point cloud stratification. Therefore, how to efficiently and accurately identify these loop points and use this information to determine whether there is point cloud stratification has become one of the key points of the present invention.

[0041] In order to achieve this goal, the present invention proposes a dense loop pair search method based on a kd tree. Specifically, the position information of each key frame (i.e., the spatial coordinates of the vehicle's current position) is first stored in a kd tree. The kd tree is a multidimensional space data structure that can efficiently store and retrieve a large amount of spatial data, and is particularly suitable for processing point cloud data, trajectory data, etc. In the kd tree, the position information of each key frame is used as a data point, and the structure of the tree enables key frames at similar positions to be quickly accessed, thereby accelerating the search for loop pairs.

[0042] The specific operation is to set a search step size (for example, 1m), and perform a dense loop pair search on the key frames within a certain radius (such as 40m) around the position every 1m of the vehicle's travel. The dense loop pair search here means that on the vehicle's trajectory, each key frame will be compared with other key frames within a certain range around it to find out whether there are loop pairs in similar positions. In this way, it can be ensured that potential loop pairs are searched, rather than just sparsely searching for distant points.

[0043] For example, in actual operation, if the vehicle is currently located at a certain location and the step length is set to 1m, then every time the vehicle travels 1m, the system will retrieve all historical key frames within 40m around the location of the point. Assuming that the vehicle has passed a certain location before and recorded key frames, the current location information will be compared with the positions of these historical key frames. If a historical key frame with location information close to the current frame is found, this historical key frame will be considered a loop point, thus forming a loop pair. By matching these loop pairs, it can be identified whether the vehicle has re-entered a scene that it has passed before.

[0044] The advantage of this process is that the structure of the kd tree greatly improves the search efficiency of loop pairs. Compared with the traditional brute force search method, the kd tree can narrow the search range to a limited range by partitioning the space, thereby significantly reducing the amount of calculation. Each time the search is performed, the system only needs to retrieve the area close to the current key frame position, avoiding unnecessary global searches and improving search efficiency.

[0045] In an exemplary embodiment, after searching for a loop pair, adjacent frames of the loop pair are selected to form a sub-image pair together with the loop pair for matching. By expanding the matching range of the loop pair, more overlapping areas are obtained to improve the matching accuracy and the accuracy of point cloud layer detection.

[0046] A loop pair refers to two key frames that are close in position and have strong similarity in the trajectory. Through dense loop pair search, the system can find key frames that may be repeated scenes. In order to improve the accuracy of matching, it does not rely solely on the matching results of the two frames, but selects adjacent frames of the loop pair and includes these adjacent frames in the matching process. The purpose of this operation is to expand the area range of the loop pair, by introducing more frame data and enhancing the overlapping area of ​​the sub-graphs, so as to obtain more reliable matching information.

[0047] Taking practical applications as an example, suppose an autonomous vehicle travels on a certain route and records multiple key frames. In this trajectory, the system detects a possible loop relationship between two frames through dense loop pair search. These two frames represent the trajectory data of the vehicle passing through similar locations twice. At this time, selecting the adjacent frames of these two frames and adding them to the matching range will form a subgraph consisting of multiple adjacent key frames. This subgraph will cover a wider range of scenes and contain more information, so that the matching of loop pairs is not limited to the two frames, but also extends to other frames around them. This method can effectively make up for the limitations of matching two frames alone, reduce the errors that may exist in single frame matching, and thus improve the overall matching accuracy.

[0048] Selecting adjacent frames and forming sub-images for matching can improve the reliability of detection on two levels. On the one hand, after adding adjacent frames, the point cloud data in the sub-image will cover a wider spatial area, and the matching process can be constrained by richer information. For example, the two frames of the loop pair may contain some incompletely overlapping areas, but the information provided by adjacent frames can make up for these deficiencies, enhance the overall overlapping parts, and make the final matching results more accurate. On the other hand, matching multiple frames can help eliminate deviations caused by noise or local feature errors. Matching between single frames may be affected by certain local changes (such as lighting, occlusion, motion blur, etc.), resulting in large matching errors. Through the joint matching of multiple adjacent frames, the system can obtain more consistent results overall, thereby reducing the impact of accidental errors on the results.

[0049] In this process, the sub-image matching is achieved through the geometric constraints and pose information of the point cloud. Specifically, in the data of the loop closure pair and the adjacent frames, the algorithm calculates the matching relationship between the relative pose, geometric shape and feature points to obtain an optimized matching result. This matching result can better reflect the relative position and spatial relationship between different frames, and further enhance the overlapping area of ​​the point cloud. This sub-image matching not only improves the matching accuracy of the loop closure points, but also effectively reduces the errors caused by inconsistent local features by optimizing the matching results.

[0050] In point cloud layer detection, by expanding the matching range and combining multiple adjacent frames, it is possible to more accurately identify whether the point cloud has layered phenomena. For example, at a certain location, if a vehicle has captured similar point cloud data multiple times when passing through the location, these point cloud data will be layered due to the lack of loop relationships or incomplete matching. By forming subgraphs of loop pairs and adjacent frames and matching them, these repeated point cloud data can be detected, thereby accurately determining whether there is a layered phenomenon. Only by jointly matching more frames can the accuracy of layered detection be ensured to avoid misjudgments due to the limitations of single-frame data.

[0051] S103: Calculate the initial relative pose of the searched loop pair, and calculate the calibrated relative pose of the searched loop pair after feature matching.

[0052] Assume that the searched loop pair includes the left sub-image (left) and the right sub-image (right), which are actually local point clouds obtained at two different times at the same location. In the SLAM or mapping process, these two keyframe sub-images each have their own pose estimates. Based on these initial pose estimates, a relative pose transformation from the left sub-image coordinate system to the right sub-image coordinate system can be calculated, which is recorded as the initial relative pose T left2right_raw . T left2right_raw It is a preliminary relative transformation relationship from the left sub-image to the right sub-image calculated based on the existing odometer, IMU or roughly estimated trajectory posture information before further feature matching of the point cloud.

[0053] With the initial relative pose T left2right_raw Finally, the feature matching algorithm is used to perform high-precision feature matching on the two sub-images to accurately align the two sub-images, thereby obtaining a more accurate calibration relative pose. T left2right_align The feature matching process can be achieved by detecting key feature points (such as corners, edges, etc.) and calculating their correspondence in two frames of images or point cloud data. The purpose of this process is to obtain more accurate spatial pose changes by accurately matching the feature points in the two frames of data.

[0054] In an exemplary embodiment, feature matching is performed on the searched loop pairs, including: using a GICP (Generalized Iterative Closest Point) algorithm to extract line and surface features in the point cloud, and performing feature matching on the searched loop pairs.

[0055] The goal of feature matching is to accurately determine the spatial relationship between two frames of loopback point cloud data, especially their relative pose. In point cloud data processing, accurate pose estimation is the prerequisite for layered detection. If the pose matching of the loopback pair is not accurate enough, the subsequent point cloud layered analysis may produce errors, resulting in a decrease in the accuracy of layered detection.

[0056] The traditional ICP algorithm achieves alignment by minimizing the distance between two point clouds point by point to find the optimal transformation matrix. Although the ICP algorithm can provide good matching results in some cases, it usually relies on the initial alignment of the point cloud and is sensitive to noise, external changes (such as occlusion, dynamic objects) and sparse features. Therefore, when the features in the point cloud are sparse or there is obvious noise, the ICP algorithm may have difficulty in obtaining accurate results.

[0057] In order to solve this problem, the present invention introduces the GICP algorithm in the feature matching process. The difference between the GICP algorithm and the traditional ICP algorithm is that it not only considers the distance between points in the point cloud, but also introduces the constraints of surface information and topological structure. By extracting line and surface features in the point cloud, GICP can use these more stable and expressive geometric features to improve the accuracy and robustness of matching. In actual operation, GICP further constrains the matching process through line and surface features. Compared with ICP, which only matches through the distance of points, it has better geometric consistency and less calculation errors.

[0058] Specifically, the GICP algorithm can use the geometric features in the point cloud (such as straight lines, planes, etc.) to extract line and surface features. These features are usually more stable and can effectively overcome the influence of noise and sparsity in the point cloud. Then, GICP optimizes the relative pose between the two-frame loop pairs while ensuring the calculation speed through iteration. In each iteration, the algorithm will perform alignment calculations based on the extracted line and surface features, rather than relying solely on point matching. Finally, through multiple iterations, GICP will converge to the optimal solution, thereby calculating the calibrated relative pose of the loop pair. T left2right_align .

[0059] S104: Calculate the posture change between the initial relative posture and the calibration relative posture, and determine whether point cloud stratification exists based on the posture change.

[0060] In practical applications, as the vehicle or sensor moves, the point cloud data will change with the different poses of each acquisition. Especially in loop closure scenarios, the sensor may pass through the area that was previously collected again, and then multiple point cloud layers will appear at the same location. This phenomenon is especially common in complex scenarios. For example, when an autonomous vehicle passes through a section of a city street, it may capture multiple point clouds around the same building at different time points. These point clouds may have slight pose changes and overlaps due to being collected at different times. Without precise calibration of these point clouds, these point clouds will cause data redundancy and stratification problems, affecting subsequent processing and analysis.

[0061] The core of step S104 is to calculate the pose change between the initial relative pose and the calibration relative pose, and to determine whether there is a stratification of the point cloud through this change. The initial relative pose is a rough estimate estimated by the searched loop pair, which usually has a certain error because the search process of the loop pair is affected by noise, external changes and other factors. The calibration relative pose is a relative pose obtained by further optimizing feature matching (such as the GICP algorithm), which can provide a more accurate spatial relationship between the two frames of loop point clouds. The pose change between the two represents the pose difference caused by errors or the complexity of the loop scene.

[0062] The calculation of posture change can be completed through matrix operations. Specifically, the difference between the rotation matrix and the displacement vector can be calculated to obtain the change between the two. If the posture change exceeds a certain predetermined threshold, it is determined that the point cloud stratification phenomenon occurs. This process is achieved through a threshold judgment, which can be flexibly set according to the actual scenario and needs, and can be adjusted based on empirical data. Figure 2 This is a diagram showing the effect of point cloud layered detection in one embodiment of the present invention. Figure 2 The red dot area in the middle is the detected point cloud layer area.

[0063] In an exemplary embodiment, the posture change calculation formula between the initial relative posture and the calibration relative posture is:

[0064] in, T left2right_raw is the initial relative pose, T left2right_align To calibrate the relative pose, For posture changes. is the inverse transformation of the initial relative pose, is the inverse transformation of the calibration relative pose. When the two inverse transformations are multiplied, we get represents the difference from the initial estimate to the precise matching result, that is, the change before and after the feature matching. If there is no stratification, then these two relative poses should be very close. Should be close to unity (i.e. no significant change).

[0065] Specifically, judging whether there is point cloud stratification based on the posture change includes: The last column is the position change before and after feature matching, and the 3×3 rotation matrix in the upper left corner is the posture change before and after feature matching; determine whether the position change and posture change exceed the preset threshold; if so, there is point cloud stratification.

[0066] Posture changes is a 4×4 matrix that includes position changes and rotation changes. For ease of analysis, the pose change matrix can be decomposed into two main parts: ① Position change: The last column in the pose change matrix represents the translation between the loop pairs. This part describes the spatial position change between the two loop point clouds.

[0067] ② Posture change: The 3×3 matrix in the upper left corner of the posture change matrix is ​​a rotation matrix, which represents the posture change between the loop pairs, or their relative rotation. In order to facilitate comparison with the threshold, the rotation matrix can be converted into Euler angle representation.

[0068] If the change in position and angle before and after feature matching exceeds the preset threshold, it can be considered that there is point cloud stratification in the area. Specifically, the posture change is mainly composed of position and posture (angle) changes. When either of the two exceeds the set threshold, it will be judged as stratification.

[0069] In order to improve the accuracy of layered detection, especially in areas near the ground, stricter thresholds can be set for position changes on the Z axis. Because the ground is usually the focus area in autonomous driving systems, if point cloud stratification occurs on the Z axis, it may mean that the relative height between the vehicle and the ground has changed significantly, or there are multiple parallel ground layers, which in turn affects the accuracy of environmental perception. Therefore, by setting stricter thresholds for position changes on the Z axis, it can effectively reduce misjudgments caused by other factors and improve the sensitivity and accuracy of the detection system to ground stratification.

[0070] In an exemplary embodiment, the method further includes: optimizing the detected trajectory by using trajectory points with point cloud stratification as graph optimization loop factors; and re-performing point cloud stratification detection and graph optimization on the optimized trajectory until the point cloud stratification is eliminated.

[0071] After the point cloud stratification is detected, these stratified trajectory points will be added to the graph optimization as loop factors, and then the entire trajectory will be adjusted through the optimization algorithm. In graph optimization, the loop factors will affect the adjustment of the overall trajectory as constraints, and finally achieve the optimization of the entire trajectory. Through graph optimization, the trajectory error can be effectively reduced and the trajectory deviation caused by point cloud stratification can be corrected. In this process, graph optimization not only relies on traditional visual odometry data, but can also be combined with other sensor data, such as lidar, IMU, etc., so that the optimization results are more accurate and robust.

[0072] After the first optimization, although the trajectory error is reduced, the stratification phenomenon may still exist. Therefore, the system will perform the aforementioned point cloud stratification detection on the optimized trajectory again, and use the new stratified trajectory points as loop factors for graph optimization again. This process can be iterated continuously until the point cloud stratification problem is completely eliminated, and finally an optimized trajectory without obvious stratification is obtained.

[0073] For example, in actual applications, when an autonomous vehicle is driving in a complex environment, due to the lack of local loops or the accumulation of errors, layering may occur in the point cloud data. At this time, the system will first detect these layers and correct them through graph optimization. For example, when a vehicle passes a certain location, the point cloud data captured by the sensor may overlap multiple times, causing the superposition of data from different frames to form layers. By using these layered trajectory points as loop factors, graph optimization will adjust the vehicle's trajectory and eliminate unnecessary posture deviations to ensure that the final generated point cloud data truly reflects the vehicle's actual path.

[0074] Through graph optimization and multiple iterations, the trajectory deviation caused by point cloud stratification can be effectively corrected, thereby improving the accuracy of point cloud data. In particular, during long-term, high-speed autonomous driving, the point cloud stratification problem that may be caused by error accumulation can be gradually eliminated through this method, ultimately achieving accurate map construction and path planning.

[0075] In summary, the method for point cloud hierarchical detection provided by the present invention adopts an idea based on loop closure search and feature matching. By performing dense loop closure search on trajectory points, it can accurately detect possible hierarchical areas between multiple trajectory frames; this method not only takes into account relative pose changes, but also introduces matching of point cloud features, further improving the accuracy and stability of hierarchical detection; compared with traditional detection methods based on single pose estimation, the use of loop closure search and graph optimization can more comprehensively and robustly process point cloud data in complex environments.

[0076] Please refer to Figure 3As shown, based on the same inventive concept as the aforementioned point cloud layered detection method, the present invention provides a point cloud layered detection device 300, which includes: an extraction module 301, a search module 302, a calculation module 303 and a judgment module 304.

[0077] The extraction module 301 is used to extract the key frames in the trajectory based on the trajectory to be detected and its corresponding point cloud frame data. The search module 302 is used to input the position information of the key frames into the kd tree, and perform a dense search for loop pairs in the trajectory with a set step size. The calculation module 303 is used to calculate the initial relative pose of the searched loop pairs, and calculate the calibrated relative pose of the searched loop pairs after feature matching. The judgment module 304 is used to calculate the pose change between the initial relative pose and the calibrated relative pose, and judge whether there is point cloud stratification based on the pose change.

[0078] Please refer to Figure 4 As shown, an embodiment of the present invention further provides an electronic device 400, which includes at least one processor 401, a memory 402 (such as a non-volatile memory), a memory 403, and a communication interface 404, and the at least one processor 401, the memory 402, the memory 403, and the communication interface 404 are connected together via an internal bus 405. At least one processor 401 is used to call at least one program instruction stored or encoded in the memory 402, so that the at least one processor 401 performs various operations and functions of the method for point cloud layered detection described in various embodiments of this specification.

[0079] In the embodiments of the present specification, the electronic device 400 may include, but is not limited to, personal computers, server computers, workstations, desktop computers, laptop computers, notebook computers, mobile electronic devices, smart phones, tablet computers, cellular phones, personal digital assistants (PDAs), handheld devices, messaging devices, wearable electronic devices, consumer electronic devices, and the like.

[0080] An embodiment of the present invention also provides a computer-readable medium, which carries computer execution instructions. When the computer execution instructions are executed by a processor, they can be used to implement various operations and functions of the point cloud layered detection method described in various embodiments of this specification.

[0081] The computer-readable medium in the present invention can be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium can be, for example, but not limited to, a system, device or component of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples of computer-readable storage media can include, but are not limited to: an electrical connection with one or more wires, a portable computer 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. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program, which can be used by an instruction execution system, device or device or used in combination with it.

[0082] In the present invention, a computer-readable signal medium may include a data signal propagated in a baseband or as part of a carrier wave, which carries a computer-readable program code. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, device, or device. The program code contained on the computer-readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wire, optical cable, RF, etc., or any suitable combination of the above.

[0083] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0084] The present invention is described with reference to flowcharts and / or block diagrams of methods, apparatuses, systems, and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as a combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the process in the flowchart. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0085] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0086] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A method for point cloud layered detection, characterized in that: include: Based on the trajectory to be detected and its corresponding point cloud frame data, extract the key frame in the trajectory; Input the position information of the key frame into the kd tree, and perform a dense search of loop pairs in the trajectory with a set step size; Calculate the initial relative pose of the searched loop pair, and calculate the calibrated relative pose of the searched loop pair after feature matching; A pose change between the initial relative pose and the calibration relative pose is calculated, and whether point cloud stratification exists is determined based on the pose change.

2. The method for point cloud layered detection according to claim 1, characterized in that: Extract keyframes from the trajectory, including: The point cloud frames whose relative poses between adjacent frames in the extracted trajectory exceed the set threshold are taken as key frames.

3. The method for point cloud layered detection according to claim 1, characterized in that: The method further comprises: After searching for a loop pair, adjacent frames of the loop pair are selected to form a sub-image pair together with the loop pair for matching.

4. The method for point cloud layered detection according to claim 1, characterized in that: Perform feature matching on the searched loop pairs, including: The GICP algorithm is used to extract line and surface features in the point cloud, and feature matching is performed on the searched loop pairs.

5. The method for point cloud layered detection according to claim 1, characterized in that: The calculation formula for the posture change between the initial relative posture and the calibration relative posture is: ; in, T left2right_raw is the initial relative pose, T left2right_align To calibrate the relative pose, For posture changes.

6. The method for point cloud layered detection according to claim 5, characterized in that: Judging whether there is point cloud stratification based on the posture change includes: Change in posture The last column in the figure is the position change before and after feature matching, and the 3×3 rotation matrix in the upper left corner is the posture change before and after feature matching; Determining whether the position change and posture change exceed a preset threshold; If so, there is point cloud stratification.

7. The method for point cloud layered detection according to claim 1, characterized in that: The method further comprises: The loop closure factor is optimized based on the trajectory points with point cloud stratification, and the detected trajectory is optimized; Re-perform point cloud layer detection and graph optimization on the optimized trajectory until the point cloud layer is eliminated.

8. A point cloud layered detection device, characterized in that: include: An extraction module, used for extracting key frames in the trajectory based on the trajectory to be detected and its corresponding point cloud frame data; A search module, used for inputting the position information of the key frame into the kd tree, and performing a dense search of loop pairs in the trajectory with a set step size; A calculation module is used to calculate the initial relative pose of the searched loop pair and calculate the calibrated relative pose of the searched loop pair after feature matching; The judgment module is used to calculate the posture change between the initial relative posture and the calibration relative posture, and judge whether there is point cloud stratification based on the posture change.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the method for point cloud layered detection according to any one of claims 1 to 7 is implemented.

10. A computer-readable medium, characterized in that The computer-readable medium carries computer-executable instructions, and when the computer-executable instructions are executed by a processor, they are used to implement the method for point cloud layered detection according to any one of claims 1 to 7.

Images