A point cloud matching method, device, computer equipment and storage medium

Abstract

The application discloses a point cloud matching method and device, computer equipment and a storage medium. The method comprises the following steps: acquiring a basic point cloud corresponding to an external environment of a target device, and performing feature segmentation on the basic point cloud; performing ground-based matching on the feature-segmented basic point cloud to obtain a first matching parameter; performing occupancy grid-based matching on the feature-segmented basic point cloud based on the first matching parameter to obtain a second matching parameter; performing feature-based point cloud accurate matching on the feature-segmented basic point cloud based on the second matching parameter to obtain a third matching parameter; and matching an original point cloud and a target point cloud based on the third matching parameter. The application matches the feature-segmented basic point cloud step by step, gradually reduces the matching error, solves the problem that a traditional algorithm cannot handle an excessively large initial matching error, and realizes fast convergence of matching. Feature segmentation on the basic point cloud can reduce the matching calculation amount and enhance the robustness of loopback matching calculation.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of data processing, and in particular to a point cloud matching method and device, computer equipment and a storage medium. BACKGROUND

[0002] In recent years, with the popularization and application of chips, visual sensors, consumer-grade inertial navigation, and the development of computer vision technology, autonomous driving has become increasingly integrated into human life, such as digital mines, digital ports, sweeping robots, and RoboTAXI. In these autonomous driving applications, positioning and mapping (SLAM) is a key technology that enables positioning and reconstruction of the scene during driving, which enables unmanned vehicles or robots to execute the planned path, and also facilitates scene perception to avoid safety risks during driving.

[0003] However, since positioning and mapping are essentially probabilistic calculations, these calculations inevitably have errors. These errors may not affect positioning and mapping in the short term, but over time, they can cause deviations in positioning and deterioration in mapping results.

[0004] To solve the problem of accumulated errors, an effective strategy is to establish constraints between the current observation and the known historical observation, and then correct the real-time pose based on graph optimization. This technique is called loop closure detection. In visual SLAM, the commonly used loop closure detection method is the bag-of-words model, which first establishes a feature description set. For each image, the feature vector corresponding to the image is encoded according to the number of feature occurrences. When performing loop closure detection, only the feature vectors of the two images need to be matched. If the number of matched features is greater than a certain number, it is considered that there is a loop. Visual SLAM requires depth estimation, which is more computationally intensive and less robust than laser SLAM. It is also susceptible to light, weather, and other factors. Laser SLAM is currently a more widely used solution. In laser SLAM, the commonly used loop closure detection method is the ICP algorithm and its variants. However, the ICP algorithm requires good initial matching values, otherwise it may not converge, which makes ICP and its variants unsuitable as a more general loop closure detection algorithm.

[0005] A loop closure detection algorithm for 2D laser SLAM is introduced in the laser SLAM algorithm framework Cartographer of Google. The algorithm performs loop closure detection and matching based on grid occupancy, discretizes the matching solution space, searches for a set of matching parameters (x, y, theta) that makes the grid have the maximum matching probability as the optimal matching pose, and judges the effectiveness of the matching based on the size of the matching probability. In actual implementation, to accelerate the convergence of the algorithm, the solution space is organized into a tree structure, the Depth First Search (DFS) algorithm is used to search the tree nodes, and based on the iterative optimization and branch and bound method, the invalid search of part of the solution space is avoided. The calculation of the upper bound of the matching probability of the solution space corresponding to the tree node is the key of the algorithm, to avoid the slow solving speed caused by the direct search calculation of the solution space, the loop closure detection algorithm converts to search each matching under each point of the solution space, so as to solve the optimal matching probability of the point, and the sum of the optimal matching probabilities of all points is the upper bound of the node matching probability.

[0006] In related research, some documents propose a method of converting laser point cloud into Birds Eye Views (BEV) image for similarity calculation to detect loop closure. In order to make the image have translational, rotational and scaling invariance, Fourier transform, Log-Gabor filtering and other methods are used on the image to obtain the feature description of the image, and finally the existence of loop closure is judged by calculating the Hamming distance of the features of the two images. Although the method of calculating the similarity of the image through the feature distance is simple and can realize the quantitative description of the similarity, the processing process of the image of the algorithm is still time-consuming, and it is difficult to realize the real-time processing of loop closure detection. Moreover, the algorithm only answers the existence of loop closure, and does not give the relative pose relationship between the loop closures, and many times, it is particularly important to give the relative pose between the loop closures, which is related to whether a set of accurate loop closure matching constraints can be established.

[0007] Therefore, the prior art has the technical problems of slow point cloud matching speed, low precision and poor robustness.

[0008] The content described in the background technology is only for the convenience of understanding the related technology in the art, and is not regarded as the recognition of the prior art. SUMMARY

[0009] In order to solve the above technical problems, the present application provides a point cloud matching method, device, computer equipment and storage medium, and the specific scheme is as follows:

[0010] In a first aspect, the embodiments of the present application provide a point cloud matching method, which comprises:

[0011] obtain a basic point cloud corresponding to an external environment of a target device, and perform feature segmentation on the basic point cloud, wherein the basic point cloud comprises an original point cloud and a target point cloud;

[0012] perform ground-based matching on the basic point cloud after feature segmentation to obtain first matching parameters, wherein the first matching parameters comprise a rotation transformation matrix and a translation transformation matrix;

[0013] perform occupancy grid-based matching on the basic point cloud after feature segmentation based on the first matching parameters to obtain second matching parameters;

[0014] perform feature-based point cloud accurate matching on the basic point cloud after feature segmentation based on the second matching parameters to obtain third matching parameters;

[0015] match the original point cloud and the target point cloud based on the third matching parameters.

[0016] According to a specific embodiment disclosed in the present application, the step of performing feature segmentation on the basic point cloud comprises:

[0017] a 2D grid is used to cover a single-frame basic point cloud, plane fitting is performed based on the lowest point in each grid, and an initial ground is detected;

[0018] a three-dimensional point within a preset distance range from the initial ground is determined as a ground point, and a three-dimensional point outside the preset distance range from the initial ground is determined as a non-ground point;

[0019] a covariance of the basic point cloud within a preset radius range of each non-ground point is calculated, and a feature decomposition is performed on the covariance matrix to obtain plane features and straight line features of each non-ground point;

[0020] a wall point and a rod-shaped point are determined from all the non-ground points based on a preset feature threshold and a feature vector.

[0021] According to a specific embodiment disclosed in the present application, the step of performing occupancy grid-based matching on the basic point cloud after feature segmentation based on the first matching parameters to obtain second matching parameters comprises:

[0022] In step S31, an initial occupancy grid corresponding to the target point cloud is created based on an initial resolution;

[0023] In step S32, a solution space is calculated based on an initial resolution, a plane distance error and an angle error between the original point cloud and the target point cloud;

[0024] In step S33, based on the first matching parameters, the solution space is traversed in a parallel manner, and a solution corresponding to a maximum matching score is determined as an optimal solution.

[0025] Step S34, if the maximum matching score is less than a preset score threshold, determining that the loop closing does not exist;

[0026] Step S35, if the maximum matching score is greater than or equal to the preset score threshold, determining that the loop closing exists, updating the first matching parameter, updating the initial resolution to a preset resolution, and updating the plane distance error and the angle error;

[0027] Step S36, repeating steps S31-S35, and determining the updated first matching parameter as the second matching parameter.

[0028] According to an embodiment disclosed in the present application, the matching score is calculated based on a formula score(w|w∈W)=∑ p∈Source score(p) of a pixel in an occupancy grid corresponding to the Source point cloud, wherein score(p) is preset according to user input.

[0029] According to an embodiment disclosed in the present application, the step of performing feature-based point cloud accurate matching on the feature-segmented base point cloud based on the second matching parameter to obtain a third matching parameter includes:

[0030] determining the second matching parameter as an initial matching parameter of the point cloud accurate matching process, and calculating the third matching parameter based on a formula and the initial matching parameter, wherein, are respectively a matching residual error between any two three-dimensional points, a matching residual error of a three-dimensional point to a ground point, and a matching residual error of a three-dimensional point to a rod-shaped point, w i , w j , and w k are weights of the corresponding residual errors.

[0031] In a second aspect, an embodiment of the present application provides a point cloud matching device, which includes:

[0032] a feature segmentation module configured to acquire a base point cloud corresponding to an external environment of a target device, and perform feature segmentation on the base point cloud, wherein the base point cloud includes an original point cloud and a target point cloud;

[0033] a first matching module configured to perform ground-based matching on the feature-segmented base point cloud to obtain a first matching parameter, wherein the first matching parameter includes a rotation transformation matrix and a translation transformation matrix;

[0034] a second matching module, configured to perform occupancy grid-based matching on the feature-segmented basic point cloud based on the first matching parameter to obtain a second matching parameter;

[0035] a third matching module, configured to perform feature-based point cloud accurate matching on the feature-segmented basic point cloud based on the second matching parameter to obtain a third matching parameter;

[0036] a point cloud matching module, configured to match the original point cloud and the target point cloud based on the third matching parameter.

[0037] According to a specific embodiment disclosed in the present application, the feature segmentation module is specifically configured to:

[0038] a 2D grid is used to cover a single-frame basic point cloud, plane fitting is performed based on the lowest point in each grid, and an initial ground is detected;

[0039] a three-dimensional point within a preset distance range from the initial ground is determined as a ground point, and a three-dimensional point outside the preset distance range from the initial ground is determined as a non-ground point;

[0040] a covariance of the basic point cloud within a preset radius range of each non-ground point is calculated, and eigenvalue decomposition is performed on the covariance matrix to obtain plane features and linear features of each non-ground point;

[0041] a wall point and a rod-shaped point are determined from all the non-ground points based on a preset feature threshold and a feature vector.

[0042] According to a specific embodiment disclosed in the present application, the second matching module is specifically configured to:

[0043] In step S31, an initial occupancy grid corresponding to a target point cloud is created based on an initial resolution;

[0044] In step S32, a solution space is calculated based on an initial resolution, a plane distance error and an angle error between the original point cloud and the target point cloud;

[0045] In step S33, based on the first matching parameter, the solution space is traversed in a parallel manner, and a solution corresponding to a maximum matching score is determined as an optimal solution;

[0046] In step S34, if the maximum matching score is less than a preset score threshold, it is determined that a loop does not exist;

[0047] In step S35, if the maximum matching score is greater than or equal to the preset score threshold, it is determined that a loop exists, the first matching parameter is updated, the initial resolution is updated to a preset resolution, and the plane distance error and the angle error are updated;

[0048] Step S36, repeating steps S31-S35, determining the updated first matching parameter as the second matching parameter.

[0049] In a third aspect, an embodiment of the present application provides a computer device, including a processor and a memory, the memory storing a computer program, the computer program implementing the point cloud matching method in any one of the first aspect when executed on the processor.

[0050] In a fourth aspect, an embodiment of the present application provides a computer readable storage medium, the computer readable storage medium storing a computer program, the computer program implementing the point cloud matching method in any one of the first aspect when executed on a processor.

[0051] Compared with the prior art, the present application has the following beneficial effects:

[0052] The point cloud matching method provided by the present application obtains a basic point cloud corresponding to an external environment of a target device, and performs feature segmentation on the basic point cloud; performs ground-based matching on the feature segmented basic point cloud to obtain a first matching parameter; performs occupancy grid-based matching on the feature segmented basic point cloud based on the first matching parameter to obtain a second matching parameter; performs feature-based point cloud accurate matching on the feature segmented basic point cloud based on the second matching parameter to obtain a third matching parameter; and matches the original point cloud and the target point cloud based on the third matching parameter. The present application gradually reduces the matching error by performing ground-based matching, occupancy grid-based matching and feature-based point cloud accurate matching on the feature segmented basic point cloud in steps, solves the problem that the traditional ICP and related algorithms cannot handle the problem of too large initial matching error and realizes fast convergence of matching. Feature segmentation of the basic point cloud can reduce the matching calculation amount and enhance the robustness of loopback matching calculation.

[0053] Some of the other optional features and technical effects of the embodiments of the present application are described below, and some can be understood by reading this document. BRIEF DESCRIPTION OF DRAWINGS

[0054] In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the following will briefly introduce the drawings needed to be used in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present disclosure, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0055] Figure 1 One of the flowcharts of the point cloud matching method provided by the embodiments of the present application;

[0056] Figure 2Fig. 2 is a flowchart of a point cloud matching method according to an embodiment of the present application;

[0057] Figure 3 Fig. 3 is a flowchart of a point cloud matching method according to an embodiment of the present application;

[0058] Figure 4 Fig. 4 is a block diagram of a point cloud matching device according to an embodiment of the present application;

[0059] Figure 5 Fig. 5 is a schematic diagram of an exemplary structure of a computer device capable of implementing the method according to an embodiment of the present application. DETAILED DESCRIPTION

[0060] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present application, but not all the embodiments of the present application.

[0061] The components of the embodiments of the present application generally described and illustrated in the accompanying drawings can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present application.

[0062] Hereinafter, the terms "include", "have", and their conjugates used in various embodiments of the present application are only intended to denote that specific features, numbers, steps, operations, elements, components, or combinations thereof are present, and should not be construed as excluding the presence or addition of one or more other features, numbers, steps, operations, elements, components, or combinations thereof.

[0063] In addition, the terms "first", "second", "third", and the like are used only to distinguish descriptions, and should not be understood as indicating or implying relative importance.

[0064] Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which various embodiments of the present application belong. The terms (such as terms defined in a generally used dictionary) will be interpreted as having the same meaning as the contextual meaning in the relevant technical field and will not be interpreted as having an idealized or overly formal meaning, unless clearly defined in various embodiments of the present application.

[0065] The loop detection algorithm of the laser SLAM introduced in the foregoing background introduces some significant shortcomings:

[0066] The algorithm is a loop detection algorithm for 2D laser SLAM and cannot be directly used in a 3D scenario. Although the algorithm can be extended to a 3D scenario, for example, 2D Pose(x, y, θ) is extended to 6D Pose(x, y, z, roll, pitch, yaw) for a 3D scenario, the increase in dimensions means that the amount of calculation increases geometrically, which makes it difficult for the algorithm to achieve real-time performance. It should be noted that roll, pitch and yaw are components of Euler angles, used to express rotation, which is a three-dimensional vector in form, and the values thereof respectively represent the rotation angles of an object around the three axes of a coordinate system. Among them, pitch is the rotation around the horizontal axis, also known as the pitch angle, yaw is the rotation around the vertical axis, also known as the yaw angle, and roll is the rotation around the longitudinal axis, also known as the roll angle;

[0067] The algorithm uses all points when calculating the occupancy grid, which is not conducive to improving the calculation efficiency on the one hand, and on the other hand, due to the existence of a large number of vegetation, dynamic objects and the like in the autonomous driving scene, these objects will seriously reduce the robustness of loop matching;

[0068] Although the algorithm proposes a method to calculate the upper bound of the matching probability of the tree node corresponding to the solution space, this matching logic based on DFS is actually not conducive to parallelism of the algorithm and is not conducive to improving the efficiency of the algorithm by using the characteristics of multi-threaded work of the computing power platform;

[0069] In addition, the accuracy of the grid-based point cloud matching is limited by the grid resolution. Generally, the accuracy of the grid-based point cloud matching is difficult to be very high, such as difficult to reach centimeter level, and in the SLAM application, the accuracy of loop matching is directly related to the accuracy of positioning and mapping.

[0070] In view of the above problems, the present application proposes an efficient loop detection algorithm and a point cloud matching method suitable for 3D laser SLAM. The main innovations of the present application include:

[0071] 1. The progressive matching idea is adopted, the point cloud matching is performed in steps, and the matching error is gradually reduced, so as to solve the problem that the traditional ICP and related algorithms cannot handle the too large initial matching error;

[0072] 2. The ground, wall, rod and other geometric features commonly seen in the autonomous driving scene are adopted, the amount of matching calculation is reduced, and the robustness of loop matching calculation is enhanced;

[0073] 3. The breadth-first algorithm (Breadth-First-Search, BFS for short) is used to scan the solution space, and the correct matching is quickly searched in a parallel manner.

[0074] Some embodiments of the present application will be described in detail with reference to the drawings. The following embodiments and features can be combined with each other in the case of no conflict.

[0075] Referring to Figure 1 and Figure 2 , Figure 1 Figure 1 is a flowchart of a point cloud matching method according to an embodiment of the present application, Figure 2 Figure 2 is a flowchart of a point cloud matching method according to another embodiment of the present application. As shown in Figure 1 , the point cloud matching method mainly includes:

[0076] In step S101, a basic point cloud corresponding to an external environment of a target device is obtained, and the basic point cloud is segmented by features. The basic point cloud includes an original point cloud and a target point cloud.

[0077] Specifically, the step of segmenting the basic point cloud by features includes:

[0078] A single frame of basic point cloud is covered by a 2D grid, and a plane fitting is performed based on the lowest point in each grid to detect an initial ground surface.

[0079] Three-dimensional points within a preset distance range from the initial ground surface are determined as ground points, and three-dimensional points outside the preset distance range from the initial ground surface are determined as non-ground points.

[0080] The covariance of the basic point cloud within a preset radius range of each non-ground point is calculated, and the covariance matrix is decomposed to obtain the plane features and linear features of each non-ground point.

[0081] Based on a preset feature threshold and a feature vector, wall points and rod points are determined from all the non-ground points.

[0082] Compared with the traditional ICP algorithm, the point cloud matching based on feature segmentation adopted by the present application can generally achieve higher accuracy. The purpose of feature segmentation of the basic point cloud is to segment ground points and non-ground points, wherein the non-ground points include but are not limited to wall points and rod points.

[0083] In the urban scene, the ground points can generally be extracted by covering a single frame of point cloud with a 2D grid, taking the lowest point in each grid for RANdom SAmple Consensus (RANSAC) plane fitting, and then detecting the initial ground surface. The three-dimensional points within a certain distance range from the initial ground surface are the ground points.

[0084] To extract the wall points and rod points, the covariance of the point cloud in a certain radius range can be calculated for the non-ground points, and the covariance matrix is subjected to eigenvalue decomposition, the planar feature and linear feature of the points are calculated according to the eigenvalues, and the wall points and rod points are extracted based on the corresponding feature threshold and feature vector.

[0085] Specifically, assuming that a set of neighboring points of a three-dimensional point P within a radius r is S, the covariance matrix corresponding to S is C, and C is subjected to singular value decomposition (SVD). Let the eigenvalues and eigenvectors be λ i 、e i (i = 1, 2, 3; λ1≥ λ2≥ λ3), the planar feature f p , linear feature f l and scattered feature f s of the three-dimensional point P can be defined as:

[0086]

[0087]

[0088]

[0089] In step S102, ground-based matching is performed on the basic point cloud after feature segmentation, to obtain first matching parameters, wherein the first matching parameters include a rotation transformation matrix and a translation transformation matrix.

[0090] Point cloud matching is essentially transforming the original point cloud from the corresponding original coordinate system to the target coordinate system corresponding to the target point cloud. Point cloud matching usually requires two point cloud data. The first type of point cloud data is called the original point cloud, usually represented by S (source); the second type of point cloud data is called the target point cloud, usually represented by T (Target). Matching is to align the original point cloud with the target point cloud, so that the original point cloud S is displayed on the coordinates of the target point cloud T to achieve data supplementation. The transformation relationship of coordinates can be determined by finding the three-dimensional point set with similar features in the original point cloud and the target point cloud. For example, the three-dimensional point set corresponding to the same object appears in both the original point cloud and the target point cloud, and there are similar feature points in the two point clouds. Then, the transformation relationship can be calculated according to the similar point cloud information.

[0091] Generally, the rigid transformation occurs from the original point cloud to the target point cloud, that is, the target point cloud can be obtained by rotating and translating the original point cloud. The rotation and translation process can be represented by a rotation transformation matrix R and a translation transformation matrix t. Therefore, the key point of point cloud matching is to calculate the appropriate rotation transformation matrix R and translation transformation matrix t. The point cloud matching method provided in the present application adopts a step-by-step matching idea to calculate the optimal matching parameters, i.e. the rotation transformation matrix R and the translation transformation matrix t, through ground-based matching, occupancy grid-based matching, and feature point cloud-based accurate matching.

[0092] Step S102 corresponds to ground-based matching, mainly solving the coarse matching residual of the point cloud in z, roll and pitch;

[0093] 1. Let the plane equation corresponding to the ground points of the original point cloud be:

[0094]

[0095] where N is a 3x1 unit vector, and c is a constant. s s

[0096] 2. Let the plane equation corresponding to the ground points of the target point cloud be:

[0097]

[0098] where N is a 3x1 unit vector, and c is a constant. t t

[0099] 3. Let the coordinate system transformation from the original point cloud to the target point cloud be T = {R, t}, then the transformed point cloud satisfies:

[0100] N t T (RX s +t)=c t

[0101] that is:

[0102] N t T RX s =c t -N t T t

[0103] then:

[0104]

[0105] The preliminary rotation transformation matrix R and the translation transformation matrix t, i.e. the first matching parameters, can be solved through the above equation set.​​​​

[0106] t = (c t -c s )N t

[0107] R = exp(θu)

[0108] where, θ = arccos(N s ,N t ).

[0109] In step S103, the feature segmented base point cloud is matched based on the occupation grid based on the first matching parameter, and a second matching parameter is obtained.

[0110] The point cloud matching based on the 2D occupation grid mainly solves the coarse matching residual error of the point cloud in x, y and yaw. In order to improve the efficiency and robustness of the algorithm, the application only uses wall points and rod points for point cloud matching based on the 2D occupation grid.

[0111] 1. The 2D occupation grid is a method for describing the distribution of point clouds with a 2D grid. The application uses a 2D grid to cover the point cloud. For each grid, the occupation situation can be described by a probability value or a label value, and the occupation situation can be filtered based on a certain threshold to enhance robustness.

[0112] In specific implementation, the occupation grid value strategy used by the application can be:

[0113]

[0114] Wherein, value(cell) represents the grid value, n plane is the number of ground points in the grid, n pole is the number of rod points in the grid, N is the number threshold, and the specific value of N can be customized by the user according to actual needs or specific application scenarios, which is not limited further here.

[0115] 2. The solution space refers to the discretization of the possible matching value range, and the description obtained about the matching value can be understood as the set of all solutions. Assuming that the plane distance error of the original point cloud and the target point cloud is D (D>0), the angle error is θ (θ>0), and the accuracy of the point cloud matching based on the 2D occupation grid is d, then the size of the solution space of the point cloud matching based on the 2D occupation grid is:

[0116]

[0117]

[0118]

[0119] where r max is the maximum range of laser point cloud observation.

[0120] At this time, the solution space is:

[0121]

[0122] where,

[0123] 3. Matching score: for a set of solutions in the solution space, the matching score of the original point cloud relative to the target point cloud can be calculated.

[0124] In specific implementation, the matching score can be calculated based on the formula score(w|w∈W)=∑ p∈Source score(p) where W is the solution space, w is a set of solutions in the solution space, p is a pixel in the occupancy grid corresponding to the Source point cloud, score(p) is the matching score of the pixel relative to the target point cloud occupancy grid, and the value of score(p) is pre-set according to user input.

[0125] In the point cloud matching process based on 2D occupancy grid, only wall points and rod points can be used for point cloud matching. The value strategy of score(p) can refer to Table 1:

[0126] Table 1

[0127]

[0128] The step of performing occupancy grid-based matching on the feature-segmented base point cloud based on the first matching parameter to obtain a second matching parameter includes:

[0129] Step S31, creating an initial occupancy grid corresponding to the target point cloud based on an initial resolution;

[0130] Step S32, calculating a solution space based on the initial resolution, the planar distance error and the angle error between the original point cloud and the target point cloud;

[0131] Step S33, based on the first matching parameter, traversing the solution space in parallel to determine the solution corresponding to the maximum matching score as the optimal solution;

[0132] Step S34, if the maximum matching score is less than a pre-set score threshold, determining that the loop is not present.

[0133] Step S35, if the maximum matching score is greater than or equal to the preset score threshold, it is determined that the loop exists, the first matching parameter is updated, the initial resolution is updated to a preset resolution, and the plane distance error and the angle error are updated.

[0134] Step S36, repeating steps S31-S35, the updated first matching parameter is determined as the second matching parameter.

[0135] Due to the characteristics of point cloud matching based on the occupancy grid, the point cloud matching based on the 2D occupancy grid is not used for accurate matching of point clouds, but only for rough matching of points. In order to improve the matching efficiency, the present application is based on the branch and bound idea, first, a larger grid resolution is used for matching, and then a smaller grid resolution is used for more accurate matching; at the same time, when scanning the solution space, a parallel method is used to achieve the purpose of quickly searching for the optimal solution.

[0136] Referring to Figure 3 , Figure 3 A flowchart of the matching based on the occupancy grid involved in the point cloud matching method provided by the embodiment of the present application. Assuming that the initial plane distance error of the matching is D (D>0) and the angle error is θ (θ>0), the algorithm steps are mainly:

[0137] 1. Create an initial occupancy grid based on the target point cloud based on the initial resolution d, wherein the initial value of the initial resolution d can be 2, and the larger the initial resolution d, the larger the initial occupancy grid;

[0138] 2. Calculate the solution space based on the plane distance error D, the angle error θ and the initial resolution d between the original point cloud and the target point cloud;

[0139] 3. Traverse the solution space in parallel to obtain the optimal solution as the solution with the maximum matching score;

[0140]

[0141]

[0142] Wherein R, t is the first matching parameter obtained based on the ground matching.

[0143] 4) Determine whether the loop exists based on the maximum matching score score, if the maximum matching score score is less than a preset score threshold, for example, 4, it is determined that the loop does not exist, and the algorithm ends. It should be noted that the specific value of the preset score threshold can be customized by the user according to the actual demand or specific application scene, which is not limited further here;

[0144] 5) When the loop is present, update the first matching parameters R, t, improve the grid resolution to d = 1, update D, theta, repeat the above steps, return the maximum matching score corresponding to {R, t} as the optimal solution, that is, the second matching parameters.

[0145] wherein the update formula is:

[0146]

[0147] D = d

[0148]

[0149] Step S104, performing feature-based point cloud accurate matching on the feature segmented base point cloud based on the second matching parameters to obtain third matching parameters.

[0150] To achieve accurate loop matching, the matching results provided by the ground-based matching and the 2D occupancy grid-based point cloud matching are used as the initial matching parameters of the feature-based point cloud accurate matching, and the feature-based point cloud matching is used to obtain more accurate matching parameters.

[0151] In specific implementation, the step of performing feature-based point cloud accurate matching on the feature segmented base point cloud based on the second matching parameters to obtain third matching parameters includes:

[0152] The second matching parameters are determined as the initial matching parameters of the point cloud accurate matching process, and the third matching parameters are calculated based on the formula and the initial matching parameters, wherein, are respectively the matching residual error between any two three-dimensional points, the matching residual error between a three-dimensional point and a ground point, and the matching residual error between a three-dimensional point and a rod-shaped point, and w i , w j , w k are the weights of the corresponding residual errors.

[0153] Step S105, matching the original point cloud and the target point cloud based on the third matching parameters.

[0154] After the above ground-based matching, 2D occupancy grid-based matching, and feature-based point cloud accurate matching, the third matching parameters obtained can more accurately reflect the transformation relationship between the original point cloud and the target point cloud. Therefore, the original point cloud and the target point cloud are matched based on the third matching parameters, and the matching degree is higher.

[0155] The step-by-step matching idea is adopted in the application, that is, ground-based matching+occupancy grid-based matching+feature point cloud-based accurate matching, the existence of loop closure is quickly detected with a small cost, and accurate matching about loop closure is given by gradually reducing the matching residual; common geometric features such as ground, wall and rod in the autonomous driving scene are adopted to reduce the matching calculation amount and enhance the robustness of loop closure matching calculation; the solution space is scanned in the BFS mode, and the correct matching is quickly searched in the parallel mode, so that the efficiency of loop closure detection is improved. The point cloud matching method provided in the application is fast, high-precision and strong robustness.

[0156] Corresponding to the method embodiments described above, referring to Figure 4 The application also provides a point cloud matching device 400, which comprises:

[0157] A feature segmentation module 401 is configured to acquire a basic point cloud corresponding to an external environment of a target device, and perform feature segmentation on the basic point cloud, wherein the basic point cloud comprises an original point cloud and a target point cloud.

[0158] A first matching module 402 is configured to perform ground-based matching on the basic point cloud after feature segmentation to obtain first matching parameters, wherein the first matching parameters comprise a rotation transformation matrix and a translation transformation matrix.

[0159] A second matching module 403 is configured to perform occupancy grid-based matching on the basic point cloud after feature segmentation based on the first matching parameters to obtain second matching parameters.

[0160] A third matching module 404 is configured to perform feature-based point cloud accurate matching on the basic point cloud after feature segmentation based on the second matching parameters to obtain third matching parameters.

[0161] A point cloud matching module 405 is configured to match the original point cloud and the target point cloud based on the third matching parameters.

[0162] In specific implementation, the feature segmentation module is specifically configured to:

[0163] A single frame of basic point cloud is covered by a 2D grid, plane fitting is performed based on the lowest point in each grid, and an initial ground is detected;

[0164] Three-dimensional points within a preset distance range from the initial ground are determined as ground points, and three-dimensional points outside the preset distance range from the initial ground are determined as non-ground points;

[0165] The covariance of the basic point cloud within a preset radius range of each non-ground point is calculated, and the covariance matrix is subjected to eigenvalue decomposition to obtain the plane features and straight line features of each non-ground point.

[0166] determine wall points and rod points from all the non-ground points based on a preset feature threshold and a feature vector.

[0167] In particular implementation, the second matching module is specifically configured to:

[0168] Step S31, create an initial occupancy grid corresponding to the target point cloud based on an initial resolution;

[0169] Step S32, calculate a solution space based on the initial resolution, a plane distance error and an angle error between the original point cloud and the target point cloud;

[0170] Step S33, based on the first matching parameter, traverse the solution space in a parallel manner, and determine a solution corresponding to a maximum matching score as an optimal solution;

[0171] Step S34, if the maximum matching score is less than a preset score threshold, determine that the loop closing does not exist;

[0172] Step S35, if the maximum matching score is greater than or equal to the preset score threshold, determine that the loop closing exists, update the first matching parameter, update the initial resolution to a preset resolution, and update the plane distance error and the angle error;

[0173] Step S36, repeat steps S31-S35, and determine the updated first matching parameter as the second matching parameter.

[0174] Those skilled in the art will understand that the point cloud matching device according to the embodiments of the present application can combine the features of the point cloud matching method according to the embodiments of the present application, and vice versa.

[0175] In addition, a computer device is also provided, which includes a processor and a memory, the memory stores a computer program, and the computer program implements the above point cloud matching method when executed on the processor.

[0176] Figure 5 A schematic diagram of a computer device 500 that can implement the method or realize the embodiments of the present application is shown, which can include more or fewer computer devices than illustrated in some embodiments. In some embodiments, it can be implemented with a single or multiple computer devices. In some embodiments, it can be implemented with a cloud or distributed computer device.

[0177] As Figure 5As shown, the computer device 500 includes a processor 501, which can perform various appropriate operations and processes according to programs and / or data stored in a read only memory (ROM) 502 or loaded from a storage section 508 into a random access memory (RAM) 503. The processor 501 can be a multi-core processor or can include multiple processors. In some embodiments, the processor 501 can include a general purpose main processor and one or more special purpose co-processors, such as a central processing unit (CPU), a graphics processing unit (GPU), a neural processing unit (NPU), a digital signal processor (DSP), and the like. Various programs and data required by the computer device 500 for operation are also stored in the RAM 503. The processor 501, the ROM 502, and the RAM 503 are connected to each other through a bus 504. An input / output (I / O) interface 505 is also connected to the bus 504.

[0178] The above processor and memory are used together to execute programs stored in the memory, which, when executed by the computer, can implement the methods, steps or functions described in the above embodiments.

[0179] The following components are connected to the I / O interface 505: an input section 506 including a keyboard, a mouse, a touch screen, and the like; an output section 507 including a display such as a cathode ray tube (CRT), a liquid crystal display (LCD), and the like, and a speaker, and the like; a storage section 508 including a hard disk, and the like; and a communication section 509 including a network interface card such as a LAN card, a modem, and the like. The communication section 509 performs communication processing via a network such as the Internet. A drive 510 is also connected to the I / O interface 505 as necessary. A removable medium 511 such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, and the like is attached to the drive 510 as necessary, so that a computer program read therefrom is installed into the storage section 508 as necessary. Figure 5 Only some of the components of the computer system 500 are shown, and it is not meant to be a comprehensive list of components. Figure 4 The components shown.

[0180] The systems, apparatuses, modules or units illustrated by the above embodiments can be implemented by a computer or its associated components. The computer may, for example, be a mobile terminal, a smart phone, a personal computer, a laptop computer, an in-vehicle human-machine interaction device, a personal digital assistant, a media player, a navigation device, a game console, a tablet computer, a wearable device, a smart television, an Internet of Things system, a smart home, an industrial computer, a server, or a combination thereof.

[0181] Although not shown, in the embodiments of the present application, a storage medium is provided, which stores a computer program configured to be executed to perform the point cloud matching method of any of the embodiments of the present application.

[0182] The storage medium of the embodiments of the present application includes permanent and non-permanent, removable and non-removable, information storage can be realized by any method or technology. Examples of storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassette, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transmission medium that can be used to store information that can be accessed by a computing device.

[0183] The method, program, system, device, etc. of the embodiments of the present application can be executed or implemented in a single or multiple networked computers, or can be practiced in a distributed computing environment. In the embodiments of the present specification, in these distributed computing environments, tasks can be performed by remote processing devices connected through a communication network.

[0184] The specific implementation process of the computer device and the computer readable storage medium provided can refer to the specific implementation process of the point cloud matching method provided by the above-mentioned embodiments, which will not be described one by one here.

[0185] The computer device and the computer readable storage medium provided by the present application adopt the step-by-step matching idea, that is, ground-based matching + occupation grid-based matching + feature point cloud-based accurate matching, adopt the smallest cost to quickly detect the existence of loop closure, and give the accurate matching about loop closure by gradually reducing the matching residual; adopt the common geometric features such as ground, wall and rod in the autonomous driving scene, reduce the matching calculation amount, and at the same time enhance the robustness of loop closure matching calculation; adopt the BFS scanning of solution space, adopt parallel mode to quickly search for correct matching, and improve the efficiency of loop closure detection. The point cloud matching method provided by the present application is fast, accurate and robust.

[0186] Those skilled in the art should understand that the embodiments of the present specification can be provided as a method, system or computer program product. Therefore, those skilled in the art can think that the implementation of the functional modules / units or controllers and related method steps described in the above embodiments can be realized by software, hardware and software / hardware combination.

[0187] Unless specifically stated, the actions or steps of the method, program described in the embodiments of the present application do not have to be performed in a specific order and still can achieve the desired results. In some embodiments, multi-task processing and parallel processing are possible or can be advantageous.

[0188] In this document, various embodiments of the application are described, but the description is not exhaustive, and similar or equivalent features or parts of different embodiments can be omitted for the sake of brevity. In this document, "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," means that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one embodiment, but not necessarily all embodiments, of the application. The various characteristics or features described in conjunction with the described embodiments or examples can be combined, and the description of different embodiments or examples can be combined, to form additional embodiments or examples, unless the features or characteristics are mutually exclusive.

[0189] The exemplary systems and methods of this application have been described with reference to the specific embodiments and examples described herein, it will be understood by those skilled in the art that various changes can be made to the embodiments and examples described while remaining within the spirit and scope of the application as defined in the appended claims.

Claims

1. A method of point cloud matching, the method comprising: The point cloud matching method comprises: obtaining a basic point cloud corresponding to an external environment of a target device, and performing feature segmentation on the basic point cloud, wherein the basic point cloud comprises an original point cloud and a target point cloud; The basic point cloud after feature segmentation is matched based on the ground to obtain first matching parameters, wherein the first matching parameters include a rotation transformation matrix and a translation transformation matrix ; performing occupancy grid-based matching on the basic point cloud after feature segmentation based on the first matching parameter to obtain a second matching parameter; performing feature-based point cloud accurate matching on the basic point cloud after feature segmentation based on the second matching parameter to obtain a third matching parameter; matching the original point cloud and the target point cloud based on the third matching parameter; wherein the step of performing occupancy grid-based matching on the basic point cloud after feature segmentation based on the first matching parameter to obtain a second matching parameter comprises: Step S31: creating an initial occupancy grid corresponding to the target point cloud based on an initial resolution; Step S32: calculating a solution space based on an initial resolution, a plane distance error and an angle error between the original point cloud and the target point cloud; Step S33: determining an optimal solution corresponding to a maximum matching score as the optimal solution by traversing the solution space in a parallel manner based on the first matching parameter; Step S34: if the maximum matching score is less than a preset score threshold, determining that a loop does not exist; Step S35: if the maximum matching score is greater than or equal to the preset score threshold, determining that a loop exists, updating the first matching parameter, updating the initial resolution to a preset resolution, and updating the plane distance error and the angle error; Step S36: repeating steps S31-S35, and determining the updated first matching parameter as the second matching parameter.

2. The point cloud matching method of claim 1, wherein, The step of performing feature segmentation on the basic point cloud comprises: covering a single-frame basic point cloud with a 2D grid, fitting a plane based on the lowest point in each grid, and detecting an initial ground surface; determining three-dimensional points within a preset distance range from the initial ground surface as ground points, and determining three-dimensional points outside the preset distance range from the initial ground surface as non-ground points; calculating the covariance of the basic point cloud within a preset radius range of each non-ground point, and performing eigenvalue decomposition on the covariance matrix to obtain plane features and linear features of each non-ground point; determining wall surface points and rod-shaped object points from all the non-ground points based on a preset feature threshold and a feature vector.

3. The point cloud matching method of claim 1, wherein, The matching score is based on the formula is calculated, where W is a solution space, w is a set of solutions in the solution space, p is a pixel in the occupancy grid corresponding to the Source point cloud, is the matching score of the pixel with respect to the target point cloud occupancy grid, is pre-set according to user input.

4. The point cloud matching method of claim 1, wherein, The step of performing feature-based point cloud accurate matching on the basic point cloud after feature segmentation based on the second matching parameter to obtain a third matching parameter comprises: The second matching parameter is determined as an initial matching parameter of a point cloud accurate matching process, based on a formula and the initial matching parameter to calculate the third matching parameter, wherein, is the third matching parameter, , , respectively, are the matching residuals between any two three-dimensional points, the matching residuals of three-dimensional points to ground points, and the matching residuals of three-dimensional points to rod-shaped points, , , are the weights of the corresponding residuals.

5. A point cloud matching apparatus characterized by comprising: The point cloud matching device comprises: a feature segmentation module configured to obtain a basic point cloud corresponding to an external environment of a target device, and perform feature segmentation on the basic point cloud, wherein the basic point cloud comprises an original point cloud and a target point cloud; a first matching module configured to perform ground-based matching on the basic point cloud after feature segmentation to obtain a first matching parameter, wherein the first matching parameter comprises a rotation transformation matrix and a translation transformation matrix; a second matching module configured to perform occupancy grid-based matching on the basic point cloud after feature segmentation based on the first matching parameter to obtain a second matching parameter; a third matching module, configured to perform feature-based point cloud accurate matching on the feature-segmented base point cloud based on the second matching parameter, to obtain a third matching parameter; a point cloud matching module, configured to match the original point cloud and the target point cloud based on the third matching parameter; wherein the second matching module is specifically configured to: in step S31, an initial occupancy grid corresponding to the target point cloud is created based on an initial resolution; in step S32, a solution space is calculated based on the initial resolution, a plane distance error and an angle error between the original point cloud and the target point cloud; in step S33, based on the first matching parameter, the solution space is traversed in a parallel manner, and a solution corresponding to a maximum matching score is determined as an optimal solution; in step S34, if the maximum matching score is less than a preset score threshold, it is determined that the loop closing does not exist; in step S35, if the maximum matching score is greater than or equal to the preset score threshold, it is determined that the loop closing exists, the first matching parameter is updated, the initial resolution is updated to a preset resolution, and the plane distance error and the angle error are updated; in step S36, steps S31-S35 are repeated, and the updated first matching parameter is determined as the second matching parameter.

6. The point cloud matching apparatus according to claim 5, wherein the feature segmentation module is specifically configured to: a single frame of base point cloud is covered by a 2D grid, plane fitting is performed based on the lowest point in each grid, and an initial ground is detected; three-dimensional points within a preset distance range from the initial ground are determined as ground points, and three-dimensional points outside the preset distance range from the initial ground are determined as non-ground points; a covariance of the base point cloud within a preset radius range of each non-ground point is calculated, a covariance matrix is feature-decomposed, and plane features and linear features of each non-ground point are obtained; based on a preset feature threshold and a feature vector, wall points and rod points are determined from all the non-ground points.

7. A computer device, comprising: The computer device includes a processor and a memory, and the memory stores a computer program, which, when executed on the processor, implements the point cloud matching method of any one of claims 1-4.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, which, when executed on a processor, implements the point cloud matching method of any one of claims 1-4.

Images