Point cloud registration method, electronic equipment and readable storage medium
Through semantic type hierarchy and multi-resolution voxel grid map, combining the global optimal pose to determine the matching relationship, the problem of point cloud registration method being sensitive to transform initial value and data accuracy deviation is solved, and point cloud registration is improved accuracy and accuracy of crowdsourcing maps.
Patent Information
- Application Number
- CN202411079685.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-07
- Publication Date
- 2024-11-22
AI Technical Summary
The existing point cloud registration methods are sensitive to transform initial values, easily fall into local optimality, and have severe deviations in the positional accuracy of data collected in areas with weak GPS signals, making it difficult to build and update high-precision crowdsourcing maps.
Using semantic type hierarchy method, the source semantic point cloud and the target semantic point cloud are characterized by feature layering, multi-resolution voxel grid map is established, and the matching relationship is determined based on the global optimal pose, and point cloud matching is completed through the matching relationship. Accurately, use semantic types and perceptual distance to impart weights.
Improve the accuracy of point cloud registration, avoid local optimal problems, enhance the accuracy of data alignment in weak areas of GPS signal, and ensure high-precision crowdsourcing map construction and update.
Smart Images

Figure CN119006535A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the technical field of target registration, and in particular to a point cloud registration method, an electronic device, and a readable storage medium. Background Art
[0002] In the field of crowdsourced maps, data is usually collected by ordinary vehicles rather than professional collection vehicles. However, due to the cost constraints of multi-source collection equipment for ordinary vehicles, raw data is generally obtained through GPS (Global Positioning System) and cameras, and then the road-related visual semantic features are extracted using deep learning models. In areas with weak GPS signals, such as occlusions and indoor parking lots, there will be serious deviations in the location accuracy of data such as collected trajectories. In order to build and update high-precision crowdsourced maps, these deviations need to be matched and corrected so that multi-source data can be aligned on the same road network.
[0003] The point cloud registration method in related technologies is sensitive to the initial value of the transformation and is prone to fall into local optimality. Summary of the invention
[0004] In order to solve the above problems, the point cloud registration method, electronic device, and readable storage medium provided in the present application can improve the accuracy of point cloud registration.
[0005] To solve the above technical problems, a technical solution adopted in the present application is: to provide a point cloud registration method, the method comprising: stratifying the source semantic point cloud and the target semantic point cloud according to the semantic type; establishing a multi-resolution voxel grid map based on the target semantic point cloud; determining the global optimal pose by combining the source semantic point cloud and the multi-resolution voxel grid map; determining the matching relationship between the source semantic point cloud and the target semantic point cloud by combining the global optimal pose, the source semantic point cloud and the semantic type corresponding to the target semantic point cloud; and completing the registration between the source semantic point cloud and the target semantic point cloud according to the matching relationship.
[0006] Among them, establishing a multi-resolution voxel grid map based on the target semantic point cloud also includes: setting a corresponding weight score for each voxel based on the reliability of each semantic type.
[0007] The global optimal pose is determined by combining the source semantic point cloud and the multi-resolution voxel grid map, including: obtaining the target position of the source semantic point cloud under the voxel grid maps of different resolutions; according to the target position, obtaining an initial candidate pose set in the lowest resolution voxel grid map; wherein each candidate pose in the initial candidate pose set has a corresponding score; and the global optimal pose is determined according to the scores of the candidate poses in the initial candidate pose set.
[0008] Among them, obtaining the target position of the source semantic point cloud under the voxel grid map of different resolutions includes: rotating the source semantic point cloud according to the search range and search step of the yaw angle in sequence, and determining the position of the rotated source semantic point cloud under the voxel grid map of different resolutions.
[0009] Among them, according to the target position, an initial candidate pose set is obtained in the lowest resolution voxel grid map, including: in the lowest resolution voxel grid map, the source semantic point cloud is rotated and translated according to the search range and search step of the yaw angle, X direction, Y direction, and Z direction in sequence to obtain the upper bound of the total score of all data points in the source semantic point cloud; and the initial candidate pose set is obtained according to the upper bound of the total score.
[0010] Among them, according to the scores of the candidate poses in the initial candidate pose set, the global optimal pose is determined, including: sorting the candidate poses according to the scores of the initial candidate pose set to establish a max-heap; and using the elements in the max-heap in turn to determine the global optimal pose.
[0011] Among them, the global optimal posture is determined by using the elements in the max-heap in sequence, including: in response to the score of the target element in the max-heap being greater than the minimum score threshold and not in the highest resolution map, the posture nodes corresponding to the target element in the previous high-resolution map are added to the max-heap in sequence; in response to the score of the target element in the max-heap being greater than the minimum score threshold and in the highest resolution map, the posture corresponding to the target element is determined as the global optimal posture.
[0012] Among them, the matching relationship between the source semantic point cloud and the target semantic point cloud is determined by combining the global optimal pose, the source semantic point cloud and the semantic types corresponding to the target semantic point cloud, including: establishing corresponding KD trees according to the semantic types corresponding to the target semantic point cloud; transforming the source semantic point cloud using the global optimal pose; for each data point in the transformed source semantic point cloud, taking out the KD tree of the corresponding type of the target semantic point cloud in turn; based on the KD tree, searching for the nearest neighbors of the data points in the target semantic point cloud to determine the matching relationship between the source semantic point cloud and the target semantic point cloud.
[0013] Among them, the registration between the source semantic point cloud and the target semantic point cloud is completed according to the matching relationship, including: constructing a least square equation of the weighted sum of distance residuals according to the matching relationship; obtaining a posture increment using the least square equation; and completing the registration between the source semantic point cloud and the target semantic point cloud based on the posture increment.
[0014] Among them, for point type features, the distance from the source point in the source semantic point cloud to the target point in the target semantic point cloud is used as the residual. For line type features, the distance from the source point in the source semantic point cloud to the direction vector of the target point in the target semantic point cloud is used as the residual, and different weights are assigned according to the semantic type.
[0015] The method further includes: in response to the source semantic point cloud being a local single-frame data, weighting is performed according to the perceived distance of the source point in the source semantic point cloud; wherein the weight is negatively correlated with the perceived distance.
[0016] To solve the above technical problems, another technical solution adopted in the present application is: to provide an electronic device, the electronic device comprising a processor and a memory coupled to the processor; wherein the memory is used to store computer programs, and the processor is used to execute the computer programs to implement the method provided by the above technical solution.
[0017] To solve the above technical problems, another technical solution adopted in the present application is: providing a computer-readable storage medium, which is used to store a computer program, and when the computer program is executed by a processor, it is used to implement the method provided by the above technical solution.
[0018] The beneficial effects of the embodiments of the present application are as follows: different from the prior art, the point cloud registration method, electronic device, and readable storage medium provided by the present application respectively perform feature stratification on the source semantic point cloud and the target semantic point cloud according to the semantic type; establish a multi-resolution voxel grid map based on the target semantic point cloud; determine the global optimal pose in combination with the source semantic point cloud and the multi-resolution voxel grid map; determine the matching relationship between the source semantic point cloud and the target semantic point cloud in combination with the global optimal pose, the source semantic point cloud, and the semantic types corresponding to the target semantic point cloud; complete the registration between the source semantic point cloud and the target semantic point cloud according to the matching relationship, and can perform point cloud registration by distinguishing the semantic types of the point clouds, thereby improving the accuracy of point cloud registration. BRIEF DESCRIPTION OF THE DRAWINGS
[0019] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work. Among them:
[0020] Figure 1 It is a flow chart of an embodiment of a point cloud registration method provided by the present application;
[0021] Figure 2 It is a flowchart of another embodiment of the point cloud registration method provided by the present application;
[0022] Figure 3 It is a flowchart of an embodiment of step 24 provided by the present application;
[0023] Figure 4 It is a flowchart of an embodiment of step 25 provided by the present application;
[0024] Figure 5 It is a flow chart of an embodiment of a big top heap provided by the present application;
[0025] Figure 6 It is a flowchart of an embodiment of step 26 provided by the present application;
[0026] Figure 7 It is a structural diagram of an embodiment of a KD tree provided by the present application;
[0027] Figure 8 It is a flowchart of an embodiment of step 27 provided by the present application;
[0028] Fig. 9 It is a structural schematic diagram of an embodiment of an electronic device provided by the present application;
[0029] Fig.10 It is a structural schematic diagram of an embodiment of a computer-readable storage medium provided by the present application. DETAILED DESCRIPTION
[0030] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. It will be understood that the specific embodiments described herein are only used to explain the present application, rather than to limit the present application. It should also be noted that, for ease of description, only some but not all structures related to the present application are shown in the drawings. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.
[0031] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.
[0032] First, let’s define some of the terms below:
[0033] The source semantic point cloud refers to the point cloud data set that needs to be transformed to align with the target semantic point cloud during the point cloud registration process. During the registration process, the source semantic point cloud undergoes a series of coordinate transformations until its shape, position, and orientation match those of the target semantic point cloud. The source semantic point cloud may be point cloud data acquired through different devices, at different times, or from different perspectives, so there may be problems such as position deviation, shape distortion, or scale inconsistency before registration.
[0034] The target semantic point cloud refers to the point cloud data set selected as the reference or benchmark in the point cloud registration process. During the registration process, the target semantic point cloud remains unchanged, while the source semantic point cloud is aligned with the target point cloud through a series of transformations (such as rotation, translation, etc.). The target semantic point cloud can be a pre-defined point cloud data with high accuracy and integrity, which is used to provide positioning and shape reference for other point cloud data.
[0035] A voxel grid map is a fixed-resolution 3D grid map, also known as a voxel grid.
[0036] A multi-resolution voxel grid map refers to a voxel grid map with different resolutions, that is, the same point cloud can obtain corresponding voxel grid maps according to different resolutions.
[0037] The global optimal posture refers to a problem or goal under certain conditions or environments. If a decision is the best compared to all the decisions that solve the problem, then the posture determined by this decision is the global optimal posture. The determination of the global optimal posture usually involves a comprehensive consideration of the problem, including various possible solutions and their contributions to the goal, so as to select the optimal posture in the largest range or highest dimension. The determination of this posture helps to maximize the achievement of the goal and ensure the best results in any situation.
[0038] In fields such as robotics and computer vision, the concept of global optimal pose is particularly important. For example, in robot positioning and navigation, by constructing a global optimal pose estimation method, the robot's position and pose can be determined more accurately, thereby improving the robot's autonomous navigation and operation capabilities. This method can obtain the optimal pose estimation result in the maximum likelihood sense by establishing and solving a constrained weighted least squares optimization problem, which is of great significance for improving the robot's work efficiency and accuracy.
[0039] In addition, the global optimization problem also involves the construction and solution of loss functions, including the calculation of loss functions for attitude angles and positions, as well as optimizing pose estimation by solving mathematical tools such as partial derivatives, Jacobian matrices, and Hessian matrices. These technologies are not only used in robotics, but also play a key role in computer vision, 3D reconstruction, and other fields, improving the accuracy and efficiency of the system by determining the global optimal pose.
[0040] In the field of crowdsourced maps, data is usually collected by ordinary vehicles rather than professional collection vehicles. However, due to the cost constraints of multi-source collection equipment for ordinary vehicles, raw data is generally obtained through GPS (Global Positioning System) and cameras, and then the road-related visual semantic features are extracted using deep learning models. In areas with weak GPS signals, such as occlusions and indoor parking lots, there will be serious deviations in the location accuracy of data such as collected trajectories. In order to build and update high-precision crowdsourced maps, these deviations need to be matched and corrected so that multi-source data can be aligned on the same road network.
[0041] Point cloud registration methods in related technologies are sensitive to initial values of transformation and are prone to fall into local optimum. Before using these methods, it is usually necessary to perform rough registration to provide better initial values of transformation. Rough registration is still a difficult point. Point clouds used in related technologies are usually of the same type, accuracy and confidence, or are assumed to be consistent.
[0042] The inventors of the present application have discovered that the visual semantic features extracted by deep learning models are limited by the perceived distance and model performance. Generally, semantic features of different types and distances have different perceived accuracy and confidence levels. Treating these semantic features equally will lead to registration errors.
[0043] Based on this, the present application proposes to perform feature stratification of the source semantic point cloud and the target semantic point cloud according to the semantic type; establish a multi-resolution voxel grid map based on the target semantic point cloud; determine the global optimal pose by combining the source semantic point cloud and the multi-resolution voxel grid map; determine the matching relationship between the source semantic point cloud and the target semantic point cloud by combining the global optimal pose, the source semantic point cloud and the semantic type corresponding to the target semantic point cloud; complete the registration between the source semantic point cloud and the target semantic point cloud according to the matching relationship, and perform point cloud registration by distinguishing the semantic type of the point cloud, so as to improve the accuracy of the point cloud registration. Please refer to any of the following embodiments for details.
[0044] See also Figure 1 , Figure 1 1 is a flow chart of an embodiment of a point cloud registration method provided by the present application. The method comprises:
[0045] Step 11: Feature stratification of the source semantic point cloud and the target semantic point cloud according to the semantic type.
[0046] In some embodiments, there are usually several corresponding data points in the source semantic point cloud and the target semantic point cloud. These data points can be collected by different sensors. These data points can be identified by corresponding deep learning models to obtain the semantic types corresponding to these data points. For example, these data points can be obtained by collecting objects such as trees, buildings, and roads. Therefore, trees, buildings, and roads can be the semantic types corresponding to the data points.
[0047] In some embodiments, the feature may be a visual semantic feature. The visual semantic feature may be obtained by recognizing the corresponding image and / or point cloud through a deep learning model.
[0048] In some embodiments, the source semantic point cloud and the target semantic point cloud can be obtained through corresponding semantic maps. For example, the source semantic point cloud can be obtained through the source semantic map, and the target semantic point cloud can be obtained through the target semantic map.
[0049] In some embodiments, the semantic features of the source semantic map and the target semantic map can be assigned different heights (such as 0, 10, 20, 30, etc.) according to the semantic type, and discretized into point clouds respectively to obtain the corresponding layered source semantic point clouds and target semantic point clouds.
[0050] It can be understood that in the same scene, different objects have corresponding height information. For example, in a road scene, the road surface, roadside flower beds, roadside trees, buildings, and signboards all have different corresponding heights. Therefore, different heights can be assigned in sequence according to the semantic type.
[0051] In some embodiments, the target semantic Figure 1 Generally, it comes from single-trip trajectory data, multi-trip crowdsourced maps or high-precision maps. Figure 1 Generally, it comes from single-trip track data or local real-time / offline single-frame data. In some embodiments, the source semantic map and the target semantic map can be collected by a camera and / or radar sensor arranged on the vehicle.
[0052] Among them, semantic maps are usually generated in the following ways:
[0053] Data collection: First, the vehicle’s sensors collect data about its surroundings, including lidar data, visual data, and other sensor data.
[0054] Object Recognition: The collected data is passed to the semantic map generation module, which uses visual recognition algorithms to identify surrounding objects and uses lidar data to determine the geometry and location of these objects.
[0055] Information integration: The semantic map generation module combines this information and converts it into the data structure of the semantic map. Each object in the semantic map will have a type (such as road, tree, building, etc.) and may also contain other information such as color, size, etc.
[0056] In some embodiments, the target semantic point cloud can be used as the reference point cloud, and the source semantic point cloud is the point cloud to be registered. That is, the entire registration method is to register the source semantic point cloud with the target semantic point cloud.
[0057] In some embodiments, in the field of autonomous driving, visual semantic features are typically extracted using a BEV (Birds-Eyes-View) + Transformer framework, including road edges, lane lines, stop lines, arrows, speed bumps, crosswalks, etc. Semantic features of different semantic types are highly identical under the BEV view.
[0058] Step 12: Create a multi-resolution voxel grid map based on the target semantic point cloud.
[0059] In some embodiments, the target semantic point cloud may be voxelized to obtain a corresponding voxel grid map, and then the voxels are assigned corresponding weight scores according to the semantic type. The voxel grid map is further downsampled to obtain a multi-resolution voxel grid map.
[0060] Voxelization is the process of converting a data structure storing geometric information into a discrete voxel grid. Voxelization is the process of converting geometric information in a continuous domain (such as a 3D triangle grid) into a discrete voxel grid. A voxel is a three-dimensional pixel used to represent a small cube in three-dimensional space.
[0061] The voxelization process can be implemented through a variety of algorithms, such as triangle-voxel intersection or rasterization. The triangle-voxel intersection method traverses each voxel and triangle to perform intersection calculations to determine whether the voxel is covered by the triangle. The rasterization method restores the position of the surface voxel by obtaining the depth of the current pixel through rasterization of the orthogonal view.
[0062] Voxelization has a wide range of applications in computer graphics. For example, in 3D point cloud processing, voxelization can convert disordered 3D point clouds into regular grid data, which is convenient for subsequent surface fitting and rendering. In addition, voxelization is also used in tasks such as rendering volumetric fog and Lego-ization of game assets.
[0063] Voxel downsampling principle: Create a 3D voxel grid from the input point cloud data (think of the voxel grid as a collection of tiny 3D cubes in space), and then use the centroid of all points in each voxel (i.e. a small 3D cube with side lengths of m, n, o) to approximate the other points in the voxel, so that all points in the voxel are ultimately represented by one centroid point. This can indeed reduce the amount of point cloud data.
[0064] In some embodiments, a corresponding weight score is set for each voxel according to the reliability of each semantic type. The reliability of each semantic type can be determined according to the accuracy of the deep learning model that identifies the semantic type. For example, the higher the accuracy of the deep learning model, the higher the reliability, and the higher the corresponding weight score. That is, the weight score is positively correlated with the reliability. The reliability is positively correlated with the accuracy of the deep learning model.
[0065] Step 13: Determine the global optimal pose by combining the source semantic point cloud and the multi-resolution voxel grid map.
[0066] In some embodiments, the source semantic point cloud can be rotated, and the position of the rotated source semantic point cloud under the voxel grid map of different resolutions can be calculated. In some embodiments, the source semantic point cloud can be rotated based on the target semantic point cloud. It can be understood that due to the different parameter positions of the sensors used to collect the source semantic point cloud and the target semantic point cloud, the coordinate system may be different, so the same rotation is required to make it correspond to the target semantic point cloud.
[0067] Then, starting from the lowest resolution voxel grid map, the source semantic point cloud is rotated and translated to obtain the corresponding initial candidate pose set. Then, the elements in the initial candidate pose set are screened in combination with the remaining resolution voxel networks to determine the global optimal pose.
[0068] It can be understood that after the source semantic point cloud is rotated and translated, the data points in the source semantic point cloud have new poses. Therefore, the poses of these data points can be screened to obtain an initial candidate pose set.
[0069] Step 14: Determine the matching relationship between the source semantic point cloud and the target semantic point cloud by combining the global optimal pose, the source semantic point cloud, and the semantic types corresponding to the target semantic point cloud.
[0070] In some embodiments, KD trees can be established for the target semantic point cloud according to the semantic type, and the source semantic point cloud can be transformed using the global optimal pose. For each point in the transformed source semantic point cloud, the KD tree of the corresponding type of the target semantic point cloud is taken out in turn, and its nearest neighbor in the target semantic point cloud is searched to establish a matching relationship.
[0071] Step 15: Complete the registration between the source semantic point cloud and the target semantic point cloud based on the matching relationship.
[0072] In some embodiments, a corresponding optimization equation may be constructed based on the matching relationship, and a corresponding pose increment may be determined by iterating the optimization equation, and then the registration between the source semantic point cloud and the target semantic point cloud is completed based on the pose increment.
[0073] In this embodiment, the source semantic point cloud and the target semantic point cloud are feature-stratified according to the semantic type; a multi-resolution voxel grid map is established based on the target semantic point cloud; the global optimal pose is determined in combination with the source semantic point cloud and the multi-resolution voxel grid map; the matching relationship between the source semantic point cloud and the target semantic point cloud is determined in combination with the global optimal pose, the source semantic point cloud, and the semantic types corresponding to the target semantic point cloud; the registration between the source semantic point cloud and the target semantic point cloud is completed according to the matching relationship, and the point cloud registration can be performed by distinguishing the semantic types of the point clouds, thereby improving the accuracy of the point cloud registration.
[0074] See also Figure 2 , Figure 2 1 is a flow chart of an embodiment of a point cloud registration method provided by the present application. The method comprises:
[0075] Step 21: Feature stratification of the source semantic point cloud and the target semantic point cloud according to the semantic type.
[0076] Step 22: Create a multi-resolution voxel grid map based on the target semantic point cloud.
[0077] Step 21 to step 22 have the same or similar technical solutions as any embodiment of the present application, and will not be repeated here.
[0078] In some embodiments, a corresponding weight score is set for each voxel according to the reliability of each semantic type.
[0079] In some embodiments, because a corresponding weight score is set for each voxel, the upper bound of the score of the voxel at different grid size resolutions can be pre-calculated, that is, the upper bound of the score of the voxel at each resolution voxel grid map.
[0080] The principle of pre-calculating the upper bound of the voxel score is as follows: when the grid size increases, the resolution decreases, each large grid in the low resolution is composed of 8 smaller grids in the high resolution, and the upper bound of the score of the large grid is assigned to the maximum score of the 8 smaller grids.
[0081] Step 23: Obtain the target position of the source semantic point cloud under voxel grid maps of different resolutions.
[0082] In some embodiments, the source semantic point cloud may be rotated in sequence according to the search range and the search step of the yaw angle, and the target position of the rotated source semantic point cloud under the voxel grid map of different resolutions may be determined.
[0083] Step 24: According to the target position, an initial candidate pose set is obtained in the lowest resolution voxel grid map, wherein each candidate pose in the initial candidate pose set has a corresponding score.
[0084] See also Figure 3 , step 24 may be the following process:
[0085] Step 241: In the lowest resolution voxel grid map, the source semantic point cloud is rotated and translated according to the search range and search step of the yaw angle, X direction, Y direction, and Z direction in turn to obtain the upper bound of the total score of all data points in the source semantic point cloud.
[0086] In some embodiments, it is necessary to query the voxel grid at the corresponding resolution according to the position of the data point to obtain the upper bound of the score of the data point.
[0087] In some embodiments, in the lowest resolution voxel grid map, the source semantic point cloud is rotated and translated according to the search range and search step size in the yaw angle, X direction, Y direction, and Z direction, and the upper limit of the score of each point in the transformed point cloud in the lowest resolution voxel grid map is obtained, and the upper limit of the total score is added up.
[0088] In some embodiments, when the source semantic point cloud is a local single-frame data, when the scores are added, it is necessary to weight the perceived distance of the source point to obtain the upper bound of the total score. The farther the perceived distance, the lower the weight, and the closer the perceived distance, the higher the weight.
[0089] Step 242: Obtain an initial candidate pose set based on the upper bound of the total score.
[0090] In some embodiments, an initial candidate pose set is obtained by comparing an upper bound of the total score with a minimum score threshold.
[0091] Step 25: Determine the global optimal pose based on the scores of the candidate poses in the initial candidate pose set.
[0092] In some embodiments, the scores of the candidate poses in the initial candidate pose set are traversed from the lowest resolution voxel grid map to the high resolution voxel grid map until the global optimal pose is determined.
[0093] In some embodiments, see Figure 4 , step 25 may be the following process:
[0094] Step 251: Sort the candidate poses in the initial candidate pose set according to their scores and create a max-heap.
[0095] Since all candidate poses in the initial candidate pose set have corresponding scores, they can be sorted according to the scores to establish a max-heap.
[0096] Among them, the root node (top element) of the max-heap is the maximum value of all nodes (the parent node is larger than the left and right child nodes).
[0097] In one application scenario, combined with Figure 5 To explain:
[0098] The scores of the candidate poses in the initial candidate pose set include A, B, C, D, E, F and G. Among them, the scores can be calculated according to Figure 5 As shown, a large top heap is created. Figure 5 In the equation, A is greater than B and C. B is greater than D and E. C is greater than F and G.
[0099] Step 252: Use the elements in the max-heap in sequence to determine the global optimal posture.
[0100] In some embodiments, the elements in the max-heap may be taken out in sequence according to the order of the max-heap and compared to determine the global optimal posture.
[0101] In some embodiments, in response to the score of the target element in the max-heap being greater than the minimum score threshold, and the target element not being in the highest resolution map, the pose nodes corresponding to the target element in the previous high-resolution map are added to the max-heap in sequence. That is, the newly added pose nodes are used as elements to participate in the determination of the global optimal pose. Figure 5 Explanation: If score A is greater than the minimum score threshold, and the target element corresponding to score A is not in the highest resolution map, it means that the target element may correspond to the global optimal pose. Therefore, the pose nodes corresponding to the target element in the previous high-resolution map are added to the max-heap in sequence, and the scores of the pose nodes corresponding to the target element in the previous high-resolution map are used to participate in the calculation of the minimum score threshold to determine whether the pose corresponding to the target element is the global optimal pose.
[0102] In some embodiments, in response to the score of the target element in the max-heap being greater than a minimum score threshold and in the highest resolution map, the pose corresponding to the target element is determined as the global optimal pose. Figure 5 To illustrate: If the score A is greater than the minimum score threshold, and the target element corresponding to the score A is in the highest resolution map, the pose corresponding to the score A is determined as the global optimal pose.
[0103] In the above steps, using the sorting logic of the max-heap, elements with high scores will be taken out first to determine the global optimal posture, which can speed up the efficiency of determining the global optimal posture.
[0104] In other embodiments, if the score of the target element in the max-heap is less than the minimum score threshold, it means that the target element does not belong to the global optimal pose.
[0105] Step 26: Determine the matching relationship between the source semantic point cloud and the target semantic point cloud by combining the global optimal pose, the source semantic point cloud, and the semantic types corresponding to the target semantic point cloud.
[0106] In some embodiments, see Figure 6 , step 26 may be the following process:
[0107] Step 261: Establish corresponding KD trees according to the semantic types corresponding to the target semantic point cloud.
[0108] KD tree (K-dimension tree) is a data structure that divides data points in k-dimensional space. It is mainly used for searching key data in multidimensional space, such as range search and nearest neighbor search.
[0109] Construction process: When constructing a KD tree, first select a dimension (usually the first dimension), then find the median of all points in this dimension, and split the hyperplane at this point to divide the space into two. The left subspace contains all points less than the median, and the right contains points greater than the median. This process is performed recursively, but the next dimension is selected as the basis for segmentation in turn at each recursion to ensure a balanced division of the space. In order to balance the tree, it is usually necessary to select the median point of the dimension where the current node is located, so that the depth of the tree can be relatively small.
[0110] In some embodiments, because the target semantic point cloud has been layered according to semantic types, a corresponding KD tree may be established for each layer of the target semantic point cloud.
[0111] Step 262: Transform the source semantic point cloud using the global optimal pose.
[0112] In some embodiments, since the corresponding global optimal pose is determined in the target semantic point cloud, the source semantic point cloud can be transformed using the global optimal pose so that the pose of the source semantic point cloud is closest to the pose of the target point cloud, thereby improving the accuracy of subsequent matching.
[0113] Step 263: For each data point in the transformed source semantic point cloud, the KD tree of the corresponding type of the target semantic point cloud is retrieved in turn.
[0114] Step 264: Based on the KD tree, search for the nearest neighbors of the data points in the target semantic point cloud to determine the matching relationship between the source semantic point cloud and the target semantic point cloud.
[0115] When performing a nearest neighbor search on a KD tree, the properties of the KD tree can save the search for most data points, thereby reducing the amount of search calculations. Starting from the root node, the search range is gradually narrowed along the tree path until the nearest neighbor point is found.
[0116] Combination Figure 7 To illustrate: For a set of points {(2,3),(5,4),(9,6),(4,7),(8,1),(7,2)}, use the KDtree algorithm to find the nearest neighbor of the point (6,5).
[0117] First, a KD tree is constructed from a given set of points. Since this is a two-dimensional space, you can choose the x-axis or the y-axis as the split axis. For simplicity, you can choose to alternate between using the x-axis and the y-axis for splitting.
[0118] Here, it is assumed that the segmentation is performed in the order of x-axis -> y-axis -> x-axis...:
[0119] Root node: Select the median point of the x coordinate (5,4), and divide the space into two parts with x = 5 as the boundary. Note: In the above figure, (7,2) is used as the root node.
[0120] Left subtree: Contains points {(2,3),(4,7)} whose x coordinates are less than 5. The median point of the y coordinate (4,7) is selected for splitting.
[0121] Right subtree: Contains points {(9,6),(8,1),(7,2)} whose x coordinates are greater than or equal to 5. The median point of the y coordinate (7,2) is selected for splitting.
[0122] ...and so on, we can get the structure of the entire tree.
[0123] Furthermore, a KD tree is used to search for the nearest neighbors of the point (6,5).
[0124] Starting from the root node: Compare the distance between point (6,5) and the root node (5,4). The current nearest neighbor is (5,4).
[0125] Search down along the search path: Because 6>5, enter the right subtree. In the right subtree, compare the distance between point (6,5) and the current node (7,2). Since (7,2) is farther away, keep (5,4) as the current nearest neighbor.
[0126] Backtrack and check other branches: Since we are currently in the right subtree, and x=6 is closer to x=5 (the x coordinate of the root node), we need to check if there is a closer point in the left subtree (the space to the left of the root node). Go down the left subtree and find the point (4,7). Calculate the distance between (6,5) and (4,7) and update the nearest neighbor (if necessary).
[0127] Continue backtracking and checking: We have checked the two subtrees of the root node, but since the x coordinate of (6,5) is greater than the x coordinate of the root node, we need to check the sibling nodes of the root node (if any). But in this case, the root node has no sibling nodes, so the search ends.
[0128] In some embodiments, when each data point in the source semantic point cloud has a nearest neighbor (a data point in the target semantic point cloud) determined in the target semantic point cloud, a matching relationship between the data point and the nearest neighbor may be established.
[0129] Step 27: Complete the registration between the source semantic point cloud and the target semantic point cloud based on the matching relationship.
[0130] In some embodiments, see Figure 8 , step 27 may be the following process:
[0131] Step 271: construct a least squares equation of the weighted sum of distance residuals based on the matching relationship.
[0132] Step 272: Obtain the pose increment using the least squares equation.
[0133] Step 273: Complete the alignment between the source semantic point cloud and the target semantic point cloud based on the pose increment.
[0134] For example, the pose increment is used to multiply the global optimal pose by left-multiplying it to obtain the final pose, and then the final pose is used to complete the alignment between the source semantic point cloud and the target semantic point cloud.
[0135] In some embodiments, in the above manner, steps 26 and 27 are looped until an exit condition is met, such as when the residual or the pose increment is less than a threshold. The final pose is obtained by multiplying the pose increment by the global optimal pose.
[0136] In some embodiments, for point type features, the distance from the source point in the source semantic point cloud to the target point in the target semantic point cloud is used as the residual; for line type features, the distance from the direction vector of the source point in the source semantic point cloud to the target point in the target semantic point cloud is used as the residual, and different weights are assigned according to the semantic type.
[0137] The method further includes: in response to the source semantic point cloud being a local single-frame data, weighting is performed according to the perceived distance of the source point in the source semantic point cloud; wherein the weight is negatively correlated with the perceived distance.
[0138] In some embodiments, the least squares equation for the weighted sum of distance residuals may be expressed as follows:
[0139]
[0140] Among them, R* , t * is the rotation matrix and translation vector corresponding to the optimal pose, w i is the type weight corresponding to semantic type i, w j is the perceptual distance d corresponding to sample j in a certain semantic type j Weight, that is:
[0141]
[0142] For the case of local single-frame data, the specific function form includes but is not limited to continuous function, piecewise function, etc., that is, w j Inversely proportional to the perceived distance d j .P s and P t is the corresponding point in the source semantic map and the target semantic map, that is, P t ij represents the jth point of point semantic type i in the target semantic map, represents the jth point of semantic type i in the source semantic map. T represents transposition. t represents a random translation vector. represents the first endpoint of the jth line of line semantic type i in the target semantic map, represents the first endpoint of the jth line of line semantic type i in the source semantic map, represents the second endpoint of the jth line of line semantic type i in the source semantic map. Assume that the two points of the line segment in the target semantic map are and Then the normal vector of the line is For the least squares problem, the numerator and denominator constants do not affect the result and can be ignored. Therefore, the weights do not need to be normalized, and only the relative size ratio between the weights needs to be reflected.
[0143] In this embodiment, different semantic features are layered, that is, different semantic features are assigned different heights in a three-dimensional probability grid (for the case where the features are two-dimensional) or different semantic features are layered by establishing probability grids respectively (for the case where the features are three-dimensional). Semantic features of the same type are divided into one layer, and the same layer can match each other, but cannot match each other across layers.
[0144] And weight different semantics by type and distance, that is, according to the prior information of the performance of the semantic feature perception model, different weights are assigned in the probability grid (voxel grid map) and the residual function (the least squares equation of the weighted sum of distance residuals). The more reliable the feature type, the higher the weight, and the less reliable the feature type, the lower the weight. In the case where the source semantic map is a local single-frame data, it is weighted according to the perceived distance of the source point. The farther the perceived distance, the lower the weight, and the closer the perceived distance, the higher the weight.
[0145] Through the above method, the accuracy of point cloud registration is improved.
[0146] See also Fig. 9 , Fig. 9 1 is a schematic diagram of the structure of an electronic device according to an embodiment of the present application. The electronic device 70 includes a processor 71 and a memory 72 coupled to the processor 71; wherein the memory 72 is used to store a computer program, and the processor 71 is used to execute the computer program to implement the following method:
[0147] The source semantic point cloud and the target semantic point cloud are feature-stratified according to the semantic type; a multi-resolution voxel grid map is established based on the target semantic point cloud; the global optimal pose is determined by combining the source semantic point cloud and the multi-resolution voxel grid map; the matching relationship between the source semantic point cloud and the target semantic point cloud is determined by combining the global optimal pose, the source semantic point cloud and the semantic types corresponding to the target semantic point cloud; the registration between the source semantic point cloud and the target semantic point cloud is completed based on the matching relationship.
[0148] It can be understood that the processor 71 is also used to execute computer programs to implement the method of any of the above embodiments, which will not be elaborated here.
[0149] See also Fig.10 , Fig.10 1 is a schematic diagram of a computer readable storage medium according to an embodiment of the present application. The computer readable storage medium 80 is used to store a computer program 81. When the computer program 81 is executed by a processor, it is used to implement the following method:
[0150] The source semantic point cloud and the target semantic point cloud are feature-stratified according to the semantic type; a multi-resolution voxel grid map is established based on the target semantic point cloud; the global optimal pose is determined by combining the source semantic point cloud and the multi-resolution voxel grid map; the matching relationship between the source semantic point cloud and the target semantic point cloud is determined by combining the global optimal pose, the source semantic point cloud and the semantic types corresponding to the target semantic point cloud; the registration between the source semantic point cloud and the target semantic point cloud is completed based on the matching relationship.
[0151] It can be understood that when the computer program 81 is executed by the processor, it is also used to implement the method of any of the above embodiments, which will not be elaborated here.
[0152] In summary, the point cloud registration method, electronic device, and readable storage medium provided by the present application respectively stratify the features of the source semantic point cloud and the target semantic point cloud according to the semantic type; establish a multi-resolution voxel grid map based on the target semantic point cloud; determine the global optimal pose by combining the source semantic point cloud and the multi-resolution voxel grid map; determine the matching relationship between the source semantic point cloud and the target semantic point cloud by combining the global optimal pose, the source semantic point cloud, and the semantic types corresponding to the target semantic point cloud; complete the registration between the source semantic point cloud and the target semantic point cloud according to the matching relationship, and can perform point cloud registration by distinguishing the semantic types of the point clouds, thereby improving the accuracy of point cloud registration.
[0153] Furthermore, in the registration process, corresponding weight scores can be assigned according to the semantic type and the perceptual distance, and then the semantic type and the perceptual distance can be comprehensively considered for point cloud registration to improve the accuracy of point cloud registration.
[0154] In the several embodiments provided in this application, it should be understood that the disclosed methods and devices can be implemented in other ways. For example, the device implementation described above is only illustrative, for example, the division of the circuit or unit is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.
[0155] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.
[0156] In addition, each functional unit in each embodiment of the present application may be integrated into a first processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.
[0157] The above description is only an implementation method of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made according to the description and drawings of the present application, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.
Claims
1. A point cloud registration method, characterized in that: The method comprises: The source semantic point cloud and the target semantic point cloud are respectively layered according to the semantic type; Establishing a multi-resolution voxel grid map based on the target semantic point cloud; Determining a global optimal pose by combining the source semantic point cloud and the multi-resolution voxel grid map; Determine a matching relationship between the source semantic point cloud and the target semantic point cloud by combining the global optimal pose, the source semantic point cloud, and the semantic types corresponding to the target semantic point cloud; The registration between the source semantic point cloud and the target semantic point cloud is completed according to the matching relationship.
2. The method according to claim 1, characterized in that: The method of establishing a multi-resolution voxel grid map based on the target semantic point cloud further includes: setting a corresponding weight score for each voxel based on the reliability of each semantic type.
3. The method according to claim 1, characterized in that The determining of the global optimal pose by combining the source semantic point cloud and the multi-resolution voxel grid map comprises: Obtaining the target position of the source semantic point cloud in voxel grid maps of different resolutions; According to the target position, an initial candidate pose set is obtained in the lowest resolution voxel grid map; wherein each candidate pose in the initial candidate pose set has a corresponding score; The global optimal pose is determined according to the scores of the candidate poses in the initial candidate pose set.
4. The method according to claim 3, characterized in that: The obtaining of the target position of the source semantic point cloud in voxel grid maps of different resolutions includes: The source semantic point cloud is rotated in turn according to the search range and the search step of the yaw angle, and the position of the rotated source semantic point cloud under the voxel grid map of different resolutions is determined.
5. The method according to claim 3, characterized in that: The step of obtaining an initial candidate pose set in a lowest resolution voxel grid map according to the target position includes: In the lowest resolution voxel grid map, the source semantic point cloud is rotated and translated according to the search range and search step size of the yaw angle, X direction, Y direction, and Z direction in sequence to obtain the upper bound of the total score of all data points in the source semantic point cloud; An initial candidate pose set is obtained according to the upper bound of the total score.
6. The method according to claim 3, characterized in that The step of determining the global optimal pose according to the scores of the candidate poses in the initial candidate pose set includes: Sorting the candidate poses in the initial candidate pose set according to their scores to establish a max-heap; The elements in the max-heap are used sequentially to determine the global optimal pose.
7. The method according to claim 6, characterized in that The step of sequentially using the elements in the max-heap to determine the global optimal posture includes: In response to the score of the target element in the max-heap being greater than a minimum score threshold and not being in the highest-resolution map, the pose nodes corresponding to the target element in the previous high-resolution map are sequentially added to the max-heap; In response to the score of the target element in the max-heap being greater than a minimum score threshold and in the highest resolution map, the pose corresponding to the target element is determined as the global optimal pose.
8. The method according to claim 1, characterized in that The combining the global optimal pose, the source semantic point cloud, and the semantic types corresponding to the target semantic point cloud to determine the matching relationship between the source semantic point cloud and the target semantic point cloud includes: Establish corresponding KD trees according to the semantic types corresponding to the target semantic point cloud; Transforming the source semantic point cloud using the global optimal pose; For each data point in the transformed source semantic point cloud, sequentially extract the KD tree of the corresponding type of the target semantic point cloud; According to the KD tree, the nearest neighbor of the data point in the target semantic point cloud is searched to determine the matching relationship between the source semantic point cloud and the target semantic point cloud.
9. The method according to claim 1, characterized in that: The completing the registration between the source semantic point cloud and the target semantic point cloud according to the matching relationship includes: Constructing a least squares equation of a weighted sum of distance residuals according to the matching relationship; Obtaining a posture increment using the least squares equation; The registration between the source semantic point cloud and the target semantic point cloud is completed according to the pose increment.
10. The method according to claim 9, characterized in that For point type features, the distance from the source point in the source semantic point cloud to the target point in the target semantic point cloud is used as the residual; for line type features, the distance from the direction vector of the source point in the source semantic point cloud to the target point in the target semantic point cloud is used as the residual, and different weights are assigned according to the semantic type.
11. The method according to claim 10, characterized in that The method further comprises: In response to the source semantic point cloud being local single-frame data, weighting is performed according to the perceived distance of the source points in the source semantic point cloud; wherein the weight is negatively correlated with the perceived distance.
12. An electronic device, characterized in that: The electronic device comprises a processor and a memory coupled to the processor; wherein the memory is used to store a computer program, and the processor is used to execute the computer program to implement the method according to any one of claims 1-11.
13. A computer-readable storage medium, characterized in that: The computer-readable storage medium is used to store a computer program, and when the computer program is executed by a processor, it is used to implement the method according to any one of claims 1 to 11.
Citation Information
Patent Citations
Point cloud registration method and device
CN110264502A
Semantic-assisted multi-resolution point cloud registration
WO2021056190A1
