Point cloud registration method, system, equipment and medium

Abstract

The invention discloses a point cloud registration method, system and device and a medium, and relates to the technical field of three-dimensional reconstruction of scenes, and the method comprises the steps: obtaining a source point cloud and a target point cloud; semantic segmentation is carried out according to spatial structure characteristics of the point clouds, and plane points and non-plane points of the source point clouds and the target point clouds are obtained respectively; two-way distance search constraints are applied to planar points and non-planar points in the source point cloud and the target point cloud respectively, and a corresponding relation of two-way matching constraints is established; point-to-surface registration and point-to-point registration based on the maximum correlation entropy criterion are carried out on the plane points and the non-plane points after bidirectional constraint, and rotation matrixes and translation vectors of the plane points and the non-plane points are respectively obtained through multiple iterations; weighting the rotation matrix and the translation vector according to an adaptive weight, and applying the weighted rotation matrix and translation vector to the source point cloud to obtain the source point cloud after pose transformation; according to the method, the nearest neighbor corresponding relation from the source point cloud to the target point cloud and from the target point cloud to the source point cloud is considered, and the matching accuracy of the corresponding points is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of three-dimensional reconstruction of scenes, and in particular to a point cloud registration method, system, equipment and medium. Background Art

[0002] Point cloud registration is a key research task in the field of computer vision and pattern recognition, and plays a vital role in applications such as 3D reconstruction, mobile robot localization, and pose estimation. Registration algorithms aim to determine the optimal pose transformation between two point clouds. However, registration in large-scale scenes is challenging due to the interference of dynamic pedestrian noise and degraded scenes.

[0003] From the perspective of establishing correspondence, existing point cloud registration methods can be divided into two categories: iterative optimization-based registration methods and feature-based registration methods; iterative optimization-based methods use similarity measurement functions to assume the initial correspondence and search for matching points of each point in the source point cloud in the target point cloud. Subsequently, the corresponding transformation matrix is ​​calculated using the established correspondence, and the above steps of finding matching points and estimating the transformation matrix are repeatedly performed to obtain the optimal transformation. Among them, the classic point-to-point iterative closest point (ICP) algorithm performs well in environments with significant geometric features such as obvious edges and corners; however, in scenes with rich planar structures, its accuracy is reduced because it does not utilize structural features such as surface normal vectors. The point-to-plane ICP algorithm introduces normal vectors into the optimization function, allowing it to better adapt to multi-plane environments, but because noisy spatial information interferes with normal vector estimation, the algorithm cannot provide particularly accurate error metrics in unstructured scenes with fewer planar structures. In addition, the point cloud registration method based on iterative optimization has significant initial value dependence, and in practical application scenarios, point cloud data is often disturbed by various factors, including a large amount of noise, outliers, and missing data. These interference factors will have an adverse effect on the optimization process of the algorithm, making the algorithm easy to fall into local minima in the process of finding the optimal solution, and then produce incorrect registration results. Feature-based registration algorithms combine the spatial position information of point clouds with geometric morphological information, generate high-dimensional feature descriptors through deep learning and other technologies, and use these descriptors to establish correspondences between point pairs. Since such methods can determine more accurate correspondences, they do not require iterative optimization, but use a robust pose transformation estimation method to complete the registration in one step. However, such registration algorithms often face the problem of difficulty in achieving accurate alignment. In addition, the application of deep learning in feature extraction is often limited by the lack of real data sets suitable for training, and it will incur additional training time overhead. Therefore, they sometimes cannot meet the needs of scene reconstruction and positioning.

[0004] In summary, the point cloud registration method currently used in actual application scenarios has a reduced registration accuracy because it does not utilize the structural features of the point cloud; and the point cloud data is easily interfered by various factors, making it difficult to achieve precise alignment. Summary of the invention

[0005] In view of the fact that the existing technology does not utilize the structural features of point clouds, its registration accuracy will be reduced; and point cloud data is easily disturbed by various factors, making it difficult to achieve precise alignment. The present invention proposes a point cloud registration method, system, device and medium, which improves the accuracy of corresponding point matching by introducing semantic and bidirectional matching constraints; at the same time, by introducing maximum correlation entropy to perform point-to-surface registration and point-to-point registration respectively, the limitations of a single error metric are reduced, and the accuracy of point cloud registration is improved, thereby greatly improving the problems existing in the existing technology.

[0006] A point cloud registration method comprises the following steps: Obtain source point cloud and target point cloud; and perform semantic segmentation on the spatial structural characteristics of the source point cloud and target point cloud to obtain planar points and non-planar points of the source point cloud and target point cloud respectively; Bidirectional distance search constraints are applied to the planar points and non-planar points in the source point cloud and the target point cloud respectively; the planar points after the bidirectional constraints are point-to-plane registration based on the maximum correlation entropy, and the non-planar points after the bidirectional constraints are point-to-point registration based on the maximum correlation entropy. After multiple iterations, the planar points and non-planar points between the source point cloud and the target point cloud are obtained in the first The rotation matrix and translation vector in the iteration; weight the rotation matrix and translation vector of the planar point and the non-planar point according to the adaptive weight; apply the weighted rotation matrix and translation vector to the source point cloud to obtain the source point cloud after the pose transformation; Determine whether the difference between the mean square error of the distance between the source point cloud and the target point cloud after the pose transformation in the current iteration and the mean square error of the distance in the previous iteration is less than a given threshold. If so, the registration is completed.

[0007] Furthermore, the semantic segmentation of the spatial structural characteristics of the source point cloud and the target point cloud to obtain the planar points and non-planar points of the source point cloud and the target point cloud respectively includes the following steps: The K nearest neighbor algorithm is used to find the neighbor points of each point in the point cloud, and based on the principal component analysis method, the surface normal of the point is estimated by fitting the local plane features of the neighbor points, and the standard eigenvalue equation is obtained as follows: ; in, express The covariance matrix, represents adjacent data points, represents the mean vector of the K nearest neighbor algorithm, Tis the matrix transpose, k Indicates the number of neighbor points; Solve the standard eigenvalue equation via singular value decomposition: ; in, Represents the eigenvector matrix, where the third eigenvector represents the normal of the point ; Represents the eigenvalue matrix, where the third eigenvalue represents the curvature of the point ; Sort each point in the point cloud in ascending order of curvature, starting from the unprocessed point with the smallest curvature. To get started, create a list To store points with the same semantics; Each unprocessed point in , traverse its neighborhood Each unprocessed point in , if the constraints are met, add it to the list The constraints are: ; in, represents the normal vector angle threshold, represents the orthogonal distance threshold, represents the parallel distance threshold, and According to The scale Adaptive; when If all the points in are traversed, then Mark as processed; The point cloud is processed using a region growing loop until After all points are processed, the set of plane points is obtained and the set of non-planar points .

[0008] Furthermore, the constraints of bidirectional distance search are imposed on the planar points and non-planar points in the source point cloud and the target point cloud respectively, specifically including the following steps: For points in the source point cloud Apply a rotation transformation and translation transformations , and find the transformed Nearest neighbor point in the target point cloud ; In the first iteration, The initial value of The identity matrix of The initial value of The zero vector of ; calculate and The inverse matrix and , and Apply the corresponding posture transformation and find the transformed Nearest neighbor in the source point cloud ; judge With the corresponding Is the Euclidean distance between them less than the specified threshold? , if it is less than, then the point in the source point cloud The nearest neighbor point in the target point cloud after its pose transformation is a set of matching point pairs that meet the corresponding relationship of the bidirectional matching constraints; the corresponding relationship of the bidirectional matching constraints satisfies: ; in, represents a set of corresponding relations that satisfy the bidirectional matching constraints, represents the index of the point in the source point cloud, Represents the number of points in the target point cloud, Indicates the number of points in the source point cloud.

[0009] Furthermore, the planar points after the bidirectional constraints are subjected to point-to-plane registration based on the maximum correlation entropy, and the non-planar points after the bidirectional constraints are subjected to point-to-point registration based on the maximum correlation entropy; after multiple iterations, the planar points and non-planar points between the source point cloud and the target point cloud are obtained in the first The rotation matrix and translation vector in the iteration include the following steps: For the planar points and non-planar points after bidirectional constraints, the maximum correlation entropy criterion is introduced to construct the point cloud rigid body registration optimization function: ; in, and denote the rotation matrix and translation vector respectively, SO(3) is the group of all 3×3 real orthogonal matrices with determinant 1; represents the number of corresponding point pairs; represents the error metric function; represents the bandwidth that controls the distribution of the correlation entropy; represents the L2 norm; Construct point-to-surface error metric functions based on maximum correlation entropy for planar points and non-planar points respectively Point-to-point error measurement function based on maximum correlation entropy : ; ; in, and Respectively represent the plane points in the source point cloud and their corresponding plane points in the target point cloud; Represents the normal vector of the corresponding plane point in the target point cloud; and Respectively represent the non-planar points in the source point cloud and their corresponding non-planar points in the target point cloud; According to the point-to-surface error metric function and the point-to-point error metric function, the optimization function to be solved is expressed as: ; in, and Represent the weights of planar points and non-planar points respectively; Solve the plane points and non-plane points in the optimization function separately, and get the plane points and non-plane points in the first The rotation matrix in iteration , and translation vectors , .

[0010] Furthermore, the weighting of the rotation matrices and translation vectors of the planar points and the non-planar points according to the adaptive weights specifically includes the following steps: The rotation matrix and Convert to the corresponding quaternion and Then weight it and get the weighted ; right After normalization, it is converted into a rotation matrix and the first The weighted ; Translation vector , Perform adaptive linear weighting to obtain The weighted .

[0011] The present invention also includes a point cloud registration system, comprising: An acquisition module is used to acquire the source point cloud and the target point cloud; and to perform semantic segmentation according to the spatial structural characteristics of the point cloud to obtain the planar points and non-planar points of the source point cloud and the target point cloud respectively; The registration module is used to impose two-way distance search constraints on the planar points and non-planar points in the source point cloud and the target point cloud, respectively, to establish the corresponding relationship of the two-way matching constraints; the planar points after the two-way constraints are point-to-plane registered based on the maximum correlation entropy, and the non-planar points after the two-way constraints are point-to-point registered based on the maximum correlation entropy. After multiple iterations, the planar points and non-planar points are respectively obtained in the first The rotation matrix and translation vector in the iteration; weight the rotation matrix and translation vector of the planar point and the non-planar point according to the adaptive weight; apply the weighted rotation matrix and translation vector to the source point cloud to obtain the source point cloud after the pose transformation; The output module is used to determine whether the difference between the mean square error of the distance between the source point cloud and the target point cloud after the pose transformation in the current iteration and the mean square error of the distance in the previous iteration is less than a given threshold. If so, the registration is completed, otherwise, the iterative optimization continues.

[0012] The present invention also includes a point cloud registration computer device, including: a memory, a processor, and a computer program stored in the memory, and the processor implements the steps of the point cloud registration method when executing the computer program.

[0013] The present invention also includes a readable storage medium, wherein the readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, they are used to execute the steps of the point cloud registration method.

[0014] The present invention provides a point cloud registration method, which has the following beneficial effects: The present invention performs semantic segmentation according to the spatial structural characteristics of point clouds. Compared with the traditional method that only considers eigenvalues, the present invention also pays attention to the scale information and coplanarity of point clouds to avoid erroneous segmentation caused by noise, sparse point clouds or boundary points. When establishing the correspondence between point clouds, the present invention introduces semantic and bidirectional matching constraints. For semantic constraints, the present invention searches for matching point pairs between the same semantic points after segmentation, reducing the interference of dynamic and static relationships between different semantic points. For bidirectional matching constraints, the present invention simultaneously considers the correspondence from the source point cloud to the target point cloud and from the target point cloud to the source point cloud, ensuring that only point pairs that meet the nearest neighbor conditions in both directions are considered to be valid corresponding points, significantly improving the accuracy of corresponding point matching. In view of the different characteristics of planar points and non-planar points, the maximum correlation entropy is introduced to perform point-to-surface registration and point-to-point registration respectively, and weighted joint optimization is performed to utilize their advantages in a complementary manner, reduce the limitations of a single error metric, and thus improve the accuracy of point cloud registration. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 This is a flowchart of a point cloud registration method according to an embodiment of the present invention; Figure 2 A schematic diagram is provided for establishing a corresponding relationship of bidirectional matching constraints in an embodiment of the present invention; Figure 2 (a) is a schematic diagram of establishing the corresponding relationship of the one-way matching constraint. Figure 2 (b) is a schematic diagram of establishing the corresponding relationship of the bidirectional matching constraint; Figure 3 Schematic diagram of the distribution of mean square error function and maximum correlation entropy function in an embodiment of the present invention; Figure 3 (a) is a schematic diagram of the mean square error function distribution. Figure 3 (b) is a schematic diagram of the maximum correlation entropy function distribution; Figure 4 Schematic diagram of visualization results in the ETH Hauptgebaude public dataset and the self-collected indoor dataset in an embodiment of the present invention. DETAILED DESCRIPTION

[0016] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0017] The present invention proposes a point cloud registration method based on semantic constraints and maximum correlation entropy, which specifically includes the following steps: Step 1: Downsample the input point cloud according to a certain voxel size to ensure uniform density of the point cloud and reduce the computational complexity of subsequent registration; specifically: The source point cloud and target point cloud are obtained from the lidar or locally, and the input point cloud is downsampled according to a certain voxel size. The voxel size is set to 0.3 meters in the ETH Hauptgebaude public dataset and the self-collected indoor dataset. The ETH Hauptgebaude dataset is a corridor of about 60 meters, containing a large number of repeated structures and dynamic pedestrians. After different disturbances, it forms a total of 945 frames of point clouds. The self-collected indoor dataset has 533 frames of point clouds collected at a uniform speed by a robot platform in a large-scale teaching building scene, which also contains a large number of dynamic pedestrians, and there are large differences in character postures and body proportions. The above two datasets are both highly challenging datasets in the field of point cloud registration.

[0018] Step 2: Use the spatial structural characteristics of the point cloud, such as normal vectors and curvature, to perform semantic segmentation. and Represented as plane points in the source point cloud and the target point cloud, and Represented as non-planar points in the source point cloud and the target point cloud. Based on the calculation of the standard eigenvalue, the seed point is selected according to the curvature, and semantic division is performed based on the normal vector angle, orthogonal distance and parallel distance from the seed point to its neighboring point and the size relationship with the threshold. Then, the point cloud is processed using the region growth loop to finally obtain a set of planar points. and the set of non-planar points Specifically, the downsampled point cloud is semantically segmented according to the spatial structural characteristics of the point cloud. The semantic segmentation algorithm proposed in the present invention mainly includes two parts: 1. geometric space feature calculation; 2. region growing.

[0019] For the calculation of geometric space features, first, the K nearest neighbor algorithm is used to find the neighbor points of each point in the point cloud, and based on the principal component analysis (PCA), the surface normal of the point is estimated by fitting the local plane features of the neighbor points: (1); In the formula express The covariance matrix, represents adjacent data points, represents the mean vector of the K nearest neighbor algorithm, k represents the number of neighbor points, T is the matrix transpose, i Indicates the index of the neighbor point.

[0020] Then, the standard eigenvalue equation (1) is solved by singular value decomposition (SVD): (2); In the formula Represents the eigenvector matrix, where the third eigenvector represents the normal of the point . Represents the eigenvalue matrix, where the third eigenvalue represents the curvature of the point In addition, since the point The K nearest neighbors of the neighborhood can thus briefly estimate the distribution scale of the points in the neighborhood. , which is defined in the present invention as The distance to its third neighbor.

[0021] For region growing, first, sort all points in ascending order of curvature, starting from the unprocessed point with the smallest curvature. To get started, create a list To store points with the same semantics. Each unprocessed point in , traverse its neighborhood Each unprocessed point in , add it to the list if the following conditions are met middle: (3) In the formula represents the normal vector angle threshold, represents the orthogonal distance threshold, represents the parallel distance threshold, and According to The scale Adaptive. If all the points in are traversed, then Mark as processed. The region grows point by point until After all points are processed, the set of plane points is obtained and the set of non-planar points . Indicate point With point The normal angle of .

[0022] Step 3: Apply bidirectional distance search constraints to the planar points and non-planar points in the source point cloud and the target point cloud respectively to build a more accurate correspondence.

[0023] A corresponding relationship of two-way matching constraints is established between the source point cloud and the target point cloud for planar points and non-planar points respectively. That is, while the source point cloud searches for the nearest semantic point that can be matched to the target point cloud, the same matching relationship is also constructed from the target point cloud to the source point cloud, and a two-way search mode is established to improve the stability and reliability of the overall registration. In the traditional iterative closest point (ICP) algorithm system, a one-way search and matching is usually performed from each point in the source point cloud to the entire target point cloud. This one-way matching mode is intuitive and efficient, and can quickly and accurately establish a corresponding relationship between two point clouds in simple scenarios. However, the ICP algorithm has the inherent property of local convergence, which makes it very easy for the algorithm to fall into a local optimal solution in scenarios with a lot of noise and partial overlap, resulting in registration failure, such as Figure 2 (a) is a schematic diagram of establishing the correspondence relationship of the one-way matching constraint. The fundamental reason is that the traditional ICP algorithm has a narrow convergence domain. When the geometric structure of the point cloud is highly similar and the distribution density of the same semantic points is high, the source point cloud will easily concentrate the corresponding points in a local area of ​​the target point cloud based on the one-way distance measurement criterion when searching for the correspondence relationship. This local convergence phenomenon makes it difficult for the algorithm to obtain the global optimal correspondence relationship, and thus cannot achieve accurate global registration.

[0024] To solve this problem, the present invention introduces a two-way distance matching strategy to optimize the construction of the corresponding relationship. Figure 2 (b) is a schematic diagram of the corresponding relationship establishment of the bidirectional matching constraint. While the source point cloud searches for the nearest neighbor with the same semantic point to the target point cloud based on the Euclidean distance metric, a reverse search link is constructed from the target point cloud to the source point cloud to form a bidirectional matching constraint. and the target point cloud midpoint For example, bidirectional matching satisfies the following formula: (4) In the formula, represents a set of corresponding relations that satisfy the bidirectional matching constraints, represents the index of the point in the source point cloud, Represents the number of points in the target point cloud, Represents the number of points in the source point cloud. First, Apply a rotation transformation and translation transformations , and find the transformed Nearest neighbor point in the target point cloud . In the first iteration, The initial value of The identity matrix of The initial value of Then, calculate and The inverse matrix and , and Apply the corresponding posture transformation and find the transformed Nearest neighbor in the source point cloud Finally, judge With the corresponding Is the Euclidean distance between them less than the specified threshold? If it is less than, then the point in the source point cloud is considered The nearest neighbor point in the target point cloud after its pose transformation is a set of matching point pairs that conform to the bidirectional correspondence. b ( i ) represents the transformed pose The index of the nearest neighbor point in the target point cloud.

[0025] Step 4: For the bidirectionally constrained planar points and non-planar points, perform point-to-surface registration based on the maximum correlation entropy criterion and point-to-point registration based on the maximum correlation entropy criterion respectively, and obtain the planar points and non-planar points in the first The rotation matrix and translation vector in the iteration. The rotation matrix and translation vector obtained by the iteration are expressed as and , in non-planar point registration The rotation matrix and translation vector obtained by the iteration are expressed as and .

[0026] In the framework of the classic ICP algorithm, the rigid body transformation problem of 3D point clouds can be reduced to a mathematical optimization problem, and the mean square error (MSE) of the least square distance (LS) is usually used as the error metric function between point pairs. Figure 3 (a) shows a schematic diagram of the mean square error function distribution. When the source point cloud and the target point cloud gradually approach each other as a whole, the Euclidean space distance metric corresponding to the MSE function will decrease accordingly. However, due to the characteristics of the quadratic function itself, its distribution change trend is relatively gentle. If the point cloud data is disturbed by a large amount of noise or outliers and presents a complex situation similar to a heavy-tailed distribution, the MSE function is easy to confuse the internal points and external points, resulting in registration failure. Even if semantic information is used for constraints, it is difficult to completely eliminate the influence of noise points with the same semantics on the point cloud spatial transformation solution. In addition, since large-scale scene point clouds may have planar structures, non-planar structures and dynamic pedestrian noise at the same time, and the point cloud density in different scenes is different, only point-to-point registration may mismatch dense and regular planar features, resulting in falling into a local optimal solution and failing to accurately align the plane; and only point-to-plane registration may not be able to fit a single surface in a non-planar area, resulting in a large deviation in the registration result.

[0027] To solve the above problems, the present invention introduces the Maximum Correntropy Criterion (MCC). Figure 3 (b) shows a schematic diagram of the maximum correlation entropy function distribution. When the source point cloud and the target point cloud are close in overall situation and the distance between the corresponding point pairs is small, the corresponding relationship can obtain a larger value under the maximum correlation entropy measurement system; for points far away from the two point clouds, especially noise points and abnormal points, the maximum correlation entropy function will assign an extremely small function value, thereby effectively suppressing their interference with the registration process.

[0028] For planar points and non-planar points that meet the bidirectional constraints, the maximum correlation entropy criterion is introduced to construct the point cloud rigid body registration optimization function: (5) In the formula, and denote the rotation matrix and translation vector respectively; SO(3) is the group of all 3×3 real orthogonal matrices with determinant 1; represents the number of corresponding point pairs; represents the error metric function; represents the bandwidth that controls the distribution of the correlation entropy; is a three-dimensional real number space; represents the L2 norm. It means that the result of the transpose of the rotation matrix and the dot product of the rotation matrix must be the unit matrix; Indicates that the determinant of the rotation matrix must be 1.

[0029] On this basis, for planar points and non-planar points, the point-to-plane error measurement function based on maximum correlation entropy and the point-to-point error measurement function based on maximum correlation entropy are constructed respectively: (6) (7) In the formula, and Respectively represent the plane points in the source point cloud and their corresponding plane points in the target point cloud; Represents the normal vector of the corresponding plane point in the target point cloud; and They represent the non-planar points in the source point cloud and their corresponding non-planar points in the target point cloud, respectively. Represents the point-to-surface error metric; Represents the point-to-point error metric.

[0030] Combining formula (6) and formula (7), the optimization function to be solved can be expressed as: (8) In the formula, and denote the weights of planar points and non-planar points respectively. is about the rotation matrix R and the translation vector The purpose of this function is to Optimize the value to maximize the function value; N represents the number of point pairs that meet the bidirectional constraints.

[0031] Solve the plane point part and the non-plane point part in the optimization function respectively, and get the plane point and non-plane point in the first The rotation matrix in iteration , and translation vectors , ; Specifically include the following steps: For the non-planar part of the optimization function, in order to solve and The extreme value of The partial derivatives of are set to zero: (9) In order to solve the In iteration The approximate value of In the iteration and Substitute into the exponential term of formula (7). Assume that the exponential term is , specifically expressed as: (10) Will Substituting into formula (9), we can get: (11) Substituting formula (11) back into formula (5) yields: (12) in: (13) (14) For The solution of is equivalent to Solution, assuming: (15) To derive the rotation matrix , we can introduce the Lagrange multiplier method to calculate. The function is defined as: (16) In the formula, is the Lagrangian scalar operator; for The Lagrangian matrix operator of ; is the trace of the matrix, which is the sum of all the eigenvalues ​​in the matrix. is the relevant entropy weight function, by calculating the rotated vector With the target vector The distance between them is weighted using an exponential function. Represents the use of the identity matrix Measure whether the rotation matrix R satisfies All elements in the matrix are 0, and incorporating them into the objective function forces R to satisfy the orthogonality constraint of the rotation matrix during the process of solving R. It is used to measure whether the determinant of the rotation matrix R is equal to 1. Add This is to force the determinant of R to be 1 when optimizing it, to ensure the directionality of the rotation operation.

[0032] In order to solve the extreme value problem by Lagrange multiplier method, it is necessary to analyze the function Find partial derivatives of the three variables in , set each partial derivative to zero, construct the following system of equations and solve them: (17) set up , we can calculate: (18) This formula can be expressed in the form of a matrix and solved using the singular value decomposition (SVD algorithm): (19) In the formula, and Both dimensional orthogonal matrix; for dimensional diagonal matrix whose diagonal elements are all non-negative and arranged in decreasing order.

[0033] By maximizing formula (15), transposing the matrix and calculating the determinant, we can get the following results: (20) in (twenty one) In the formula, is represented as a diagonal matrix with diagonal elements represented by composition.

[0034] The first The rotation matrix calculated by the iteration Substituting back into formula (11), we can solve : (twenty two) Finally, we can get the first The rotation matrix corresponding to the iteration and translation vectors The specific value of .

[0035] For the plane point part of the optimization function, in order to solve and The present invention adopts a geometric method to approximate the pose transformation between two adjacent iterations with relatively small relative displacement from a nonlinear optimization problem to a linear optimization problem for solution. First, it is assumed that the rotation of each iteration of the source point cloud can be obtained by three Euler angle parameters: , and The translation can be composed of three Cartesian coordinate parameters , and Then, we can define a variable to be solved , expressed as: (twenty three) Since the rotation angle of each iteration is small, that is, , we can get: (twenty four) In the formula, is the Euler angle. According to the properties of Lie algebra, the rotation matrix can be approximated as: (25) Therefore, the point-to-surface registration optimization function based on maximum correlation entropy is rewritten as: (26) in: (27) (28) (29) in, Represents a plane point in the source point cloud The normal of the nearest neighboring plane point in the target point cloud The three components of the vector obtained by cross multiplication in the direction of the xyz coordinate axis; Represents the normal of the nearest neighbor plane point in the target point cloud Three components in the xyz coordinate axis direction.

[0036] In order to obtain the extreme value of formula (26), we need to right The derivative of is zero, that is: (30) In order to solve the The variables to be solved in the iteration , you need to In the iteration Substituting into the exponential term of formula (30), the exponential term is expressed as: (31) make , , and , then we can calculate the The optimal solution in the iteration for: (32) Finally, according to Get the first The rotation matrix corresponding to the iteration and translation vectors The specific value of . They represent the row vectors of matrix A, which are used to represent the coefficient matrix in the registration optimization function; Respectively represent the elements of vector b, which are used to represent the constant term in the registration optimization function; Represents vectors w The elements of are used to measure the weight of each data point; Represented by vector w The resulting diagonal matrix.

[0037] Step 5: Rotate the matrix and Convert to quaternion and And according to the adaptive weight and Weighted, and then the normalized weighted quaternion is converted into a rotation matrix . The translation vector and Directly perform adaptive weighting to obtain the translation vector The specific steps include: In the plane point registration The rotation matrix obtained by iteration and translation vectors With non-planar point registration The rotation matrix obtained by iteration and translation vectors Weighted fusion is performed according to the adaptive weights. For the convenience of description, the first The rotation matrix obtained by iteration and translation vectors is represented as and , the first The rotation matrix obtained by iteration and translation vectors is represented as and .

[0038] Since the rotation matrix belongs to the Lie group and has a nonlinear structure, it is not possible to directly perform linear weighting on the rotation matrix. Therefore, the rotation matrix needs to be and Convert to the corresponding quaternion and Then weight it: (33).

[0039] For the rotation matrix , , needs to be converted to quaternion and then adaptively weighted to obtain the weighted . The adaptive weight is expressed as: (34) (35) In the formula, Represents the number of plane points; Represents the number of plane points.

[0040] right After normalization, it is converted into a rotation matrix to obtain The weighted .

[0041] The translation vector is located in the three-dimensional real space , can be directly linearly weighted; for the translation vector , It can be adaptively linearly weighted to obtain The weighted : (36).

[0042] Step 6: The rotation matrix and translation vector obtained by iteration and Apply it to the source point cloud and determine whether the difference between the mean square error of the distance between the two point clouds in the current iteration and the mean square error of the distance between the two point clouds in the previous iteration is less than a given threshold. If so, the registration is considered complete, otherwise return to step 3 to continue iterative optimization. Specifically: At the iteration, the rotation matrix with translation vector Apply it to the source point cloud to obtain the source point cloud after pose transformation. Then, calculate the mean square error of the closest point between the transformed source point cloud and the target point cloud. By comparing the error value obtained by the current iteration with that obtained by the previous iteration, the difference between the two is calculated. If the difference is less than a predetermined threshold, it is determined that the iterative process has converged and the optimal pose transformation matrix is ​​output. Otherwise, return to step 3 and continue iterative optimization.

[0043] Experimental analysis: See Table 1, Table 2 and Figure 4The present invention conducts comparative experiments with various optimization-based classical point cloud registration algorithms on the ETH Hauptgebaude public dataset and self-collected indoor dataset. These methods are run in the environment of MATLAB 2023b, Intel Core CPU i5-6500, and 8-GB RAM. In addition, the present invention uses the average translation error, average rotation error, and registration recall rate to evaluate the registration performance. The average translation error is calculated as , the average rotation error is calculated as ,in and Represent the translation vector obtained by the algorithm and the true translation vector respectively, and They represent the rotation matrix and the true rotation matrix obtained by the algorithm respectively. The registration recall rate is defined as the ratio of the number of point cloud frames that meet the registration accuracy threshold to the total number of point cloud frames. When the rotation error and the translation error are both lower than the preset threshold, the registration is considered successful. In order to systematically evaluate the adaptability of the registration algorithm in complex scenes, the present invention verifies the stability of the algorithm through the registration recall rate. On this basis, the average translation error and the average rotation error are calculated for the successfully registered point cloud to verify the accuracy of the algorithm. It should be noted that point clouds that fail to register will introduce significant translation errors and rotation errors. Such outliers will seriously affect the reliability of the accuracy assessment. Based on the above considerations, the present invention only performs error statistical analysis on point clouds that meet the successful matching conditions, so as to ensure the validity of the evaluation results. In the present invention, point clouds with a translation error less than 0.5 meters and a rotation error less than A pair of point clouds is regarded as a pair of successfully registered point clouds.

[0044] Experimental data show that the performance of classical registration algorithms in the current dataset is relatively poor, especially in terms of robustness. Consistent with the expected results, the algorithms based on the ICP framework failed to successfully complete the registration in most test scenarios. Specifically, the standard point-to-point ICP algorithm performed the worst, mainly because the initial pose deviation of the point cloud to be registered was large and the algorithm did not build an effective anti-noise optimization mechanism. The performance of the point-to-plane ICP algorithm was improved compared with the standard ICP algorithm because the algorithm's measurement method was more adaptable to scenes with obvious planar features. MCC-ICP achieved similar registration results to point-to-plane ICP on the basis of the basic ICP algorithm framework through an outlier removal strategy. GICP takes into account the local geometric structure of the point cloud and uses a probabilistic model to weight the error, thereby improving the accuracy and robustness of the registration. However, this algorithm cannot calculate an accurate covariance matrix in scenes with dynamic object interference and sparse point cloud density, resulting in incorrect pose estimation. CobigICP introduces the cross-correlation entropy criterion and bidirectional error at the same time, so that it can establish a more accurate correspondence, but there is still a mismatch between different semantic points. In summary, most ICP algorithms have the following inherent defects: they are highly sensitive to the initial posture and are prone to local optimal solution problems in complex scenes. Unlike the local optimization strategy, the FGR algorithm performs point cloud registration through global spatial features. It uses the FPFH feature descriptor to find matching point pairs, and combines the Geman-McClure kernel function and the dual optimization strategy. Although the robustness is improved, the registration accuracy is poor. Compared with the traditional method, the present invention introduces semantic constraints and bidirectional constraints, and at the same time, for the different characteristics of planar points and non-planar points, respectively establishes point-to-surface and point-to-point error metric functions based on maximum correlation entropy, and weighted joint optimization, so that it has significant advantages in rotation error, translation error and registration recall rate. In addition, since this algorithm adds semantic segmentation and bidirectional correspondence search compared to the standard ICP algorithm, the time overhead increases, but the above modules can help the algorithm establish the correct correspondence, improve the convergence speed of the algorithm, and thus reduce the overall number of iterations. Therefore, the time consumption of the present invention is only slightly longer than the fastest point-to-point ICP, indicating that this algorithm has good real-time performance and execution efficiency while ensuring high robustness and high precision.

[0045] Table 1 Registration results of ETH Hauptgebaude public dataset

[0046] Table 2 Registration results of self-collected indoor datasets

[0047] The present invention adopts multiple geometric spatial features and region growth for semantic segmentation. Compared with the traditional method that only considers eigenvalues, the present invention also pays attention to the scale information and coplanarity of point clouds to avoid erroneous segmentation caused by noise, sparse point clouds or boundary points. When establishing the correspondence between point clouds, the present invention introduces semantic and bidirectional matching constraints. For semantic constraints, the present invention searches for matching point pairs between the same semantic points after segmentation, reducing the interference of dynamic and static relationships between different semantic points. For bidirectional matching constraints, the present invention simultaneously considers the nearest neighbor correspondence from the source point cloud to the target point cloud and from the target point cloud to the source point cloud, ensuring that only point pairs that meet the nearest neighbor conditions in both directions are considered to be valid corresponding points, significantly improving the accuracy of corresponding point matching. When constructing the optimization function, point-to-surface and point-to-point error metric functions based on maximum correlation entropy are established respectively for the different characteristics of planar points and non-planar points, and weighted joint optimization is performed to utilize their advantages in a complementary manner and reduce the limitations of a single error metric. When solving the posture transformation, the weights are adaptively adjusted according to the geometric features of the environment to perform robust posture estimation.

[0048] Based on the same inventive concept, the present invention also proposes a point cloud registration system, comprising: The acquisition module is used to obtain the source point cloud and the target point cloud; and perform semantic segmentation according to the spatial structure characteristics of the point cloud to obtain the planar points and non-planar points of the source point cloud and the target point cloud respectively.

[0049] The registration module is used to impose two-way distance search constraints on the planar points and non-planar points in the source point cloud and the target point cloud, respectively, to establish the corresponding relationship of the two-way matching constraints; the planar points after the two-way constraints are point-to-plane registered based on the maximum correlation entropy criterion, and the non-planar points after the two-way constraints are point-to-point registered based on the maximum correlation entropy criterion. After multiple iterations, the planar points and non-planar points are respectively obtained in the first The rotation matrix and translation vector in the iteration; weight the rotation matrix and translation vector of the planar point and the non-planar point according to the adaptive weight; apply the weighted rotation matrix and translation vector to the source point cloud to obtain the source point cloud after posture transformation.

[0050] The output module is used to determine whether the difference between the mean square error of the distance between the source point cloud and the target point cloud after pose transformation in the current iteration and the mean square error of the distance in the previous iteration is less than a given threshold. If so, the registration is completed, otherwise continue the iterative optimization.

[0051] The present invention also proposes a point cloud registration computer device, comprising: a memory, a processor, and a computer program stored in the memory, and the processor implements the steps of the point cloud registration method when executing the computer program.

[0052] The present invention also proposes a readable storage medium, which stores a computer program. The computer program includes program instructions. When the program instructions are executed by a processor, they are used to execute the steps of the point cloud registration method.

[0053] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. A point cloud registration method, characterized in that: The following steps are involved: Obtain source point cloud and target point cloud; and perform semantic segmentation on the spatial structural characteristics of the source point cloud and target point cloud to obtain planar points and non-planar points of the source point cloud and target point cloud respectively; Bidirectional distance search constraints are applied to the planar points and non-planar points in the source point cloud and the target point cloud respectively; the planar points after the bidirectional constraints are point-to-plane registration based on the maximum correlation entropy, and the non-planar points after the bidirectional constraints are point-to-point registration based on the maximum correlation entropy. After multiple iterations, the planar points and non-planar points between the source point cloud and the target point cloud are obtained in the first The rotation matrix and translation vector in iterations; The rotation matrix and translation vector of the planar point and the non-planar point are weighted according to the adaptive weights; the weighted rotation matrix and translation vector are applied to the source point cloud to obtain the source point cloud after the pose transformation; Determine whether the difference between the mean square error of the distance between the source point cloud and the target point cloud after the pose transformation in the current iteration and the mean square error of the distance in the previous iteration is less than a given threshold. If so, the registration is completed.

2. A point cloud registration method according to claim 1, characterized in that: The semantic segmentation of the spatial structural characteristics of the source point cloud and the target point cloud to obtain the planar points and non-planar points of the source point cloud and the target point cloud respectively includes the following steps: The K nearest neighbor algorithm is used to find the neighbor points of each point in the point cloud, and based on the principal component analysis method, the surface normal of the point is estimated by fitting the local plane features of the neighbor points, and the standard eigenvalue equation is obtained as follows: ； in, express The covariance matrix, represents adjacent data points, represents the mean vector of the K nearest neighbor algorithm, T is the matrix transpose, k Indicates the number of neighbor points; Solve the standard eigenvalue equation via singular value decomposition: ； in, Represents the eigenvector matrix, where the third eigenvector represents the normal of the point ; Represents the eigenvalue matrix, where the third eigenvalue represents the curvature of the point ; Sort each point in the point cloud in ascending order of curvature, starting from the unprocessed point with the smallest curvature. To get started, create a list To store points with the same semantics; Each unprocessed point in , traverse its neighborhood Each unprocessed point in , if the constraints are met, add it to the list The constraints are: ； in, represents the normal vector angle threshold, represents the orthogonal distance threshold, represents the parallel distance threshold, and According to The scale Adaptive; when If all the points in are traversed, then Mark as processed; The point cloud is processed using a region growing loop until After all points are processed, the set of plane points is obtained and the set of non-planar points .

3. A point cloud registration method according to claim 2, characterized in that: The constraints of bidirectional distance search are respectively imposed on the planar points and non-planar points in the source point cloud and the target point cloud, specifically comprising the following steps: For points in the source point cloud Apply a rotation transformation and translation transformations , and find the transformed Nearest neighbor point in the target point cloud ; In the first iteration, The initial value of The identity matrix of The initial value of The zero vector of ; calculate and The inverse matrix and , and Apply the corresponding posture transformation and find the transformed Nearest neighbor in the source point cloud ; judge With the corresponding Is the Euclidean distance between them less than the specified threshold? , if it is less than, then the point in the source point cloud The nearest neighbor point in the target point cloud after its pose transformation is a set of matching point pairs that meet the corresponding relationship of the bidirectional matching constraints; the corresponding relationship of the bidirectional matching constraints satisfies: ； in, represents a set of corresponding relations that satisfy the bidirectional matching constraints, represents the index of the point in the source point cloud, Represents the number of points in the target point cloud, Indicates the number of points in the source point cloud.

4. A point cloud registration method according to claim 1, characterized in that: The point-to-plane registration based on maximum correlation entropy is performed on the bidirectionally constrained planar points, and the point-to-point registration based on maximum correlation entropy is performed on the bidirectionally constrained non-planar points; After multiple iterations, the plane points and non-plane points between the source point cloud and the target point cloud are obtained in the The rotation matrix and translation vector in the iteration include the following steps: For the planar points and non-planar points after bidirectional constraints, the maximum correlation entropy criterion is introduced to construct the point cloud rigid body registration optimization function: ； in, and denote the rotation matrix and translation vector respectively, SO(3) is the group of all 3×3 real orthogonal matrices with determinant 1; represents the number of corresponding point pairs; represents the error metric function; represents the bandwidth that controls the distribution of the correlation entropy; represents the L2 norm; Construct point-to-surface error metric functions based on maximum correlation entropy for planar points and non-planar points respectively Point-to-point error measurement function based on maximum correlation entropy : ； ； in, and Respectively represent the plane points in the source point cloud and their corresponding plane points in the target point cloud; Represents the normal vector of the corresponding plane point in the target point cloud; and Respectively represent the non-planar points in the source point cloud and their corresponding non-planar points in the target point cloud; According to the point-to-surface error metric function and the point-to-point error metric function, the optimization function to be solved is expressed as: ； in, and Represent the weights of planar points and non-planar points respectively; Solve the plane points and non-plane points in the optimization function separately, and get the plane points and non-plane points in the first The rotation matrix in iteration , and translation vectors , .

5. A point cloud registration method according to claim 4, characterized in that: The method of weighting the rotation matrix and translation vector of the planar point and the non-planar point according to the adaptive weight comprises the following steps: The rotation matrix and Convert to the corresponding quaternion and Then weight it and get the weighted ; right After normalization, it is converted into a rotation matrix and the first The weighted ; Translation vector , Perform adaptive linear weighting to obtain The weighted .

6. A point cloud registration system, characterized in that: include: An acquisition module is used to acquire source point cloud and target point cloud; The spatial structural characteristics of the source point cloud and the target point cloud are semantically segmented to obtain the planar points and non-planar points of the source point cloud and the target point cloud respectively; The registration module is used to impose two-way distance search constraints on the planar points and non-planar points in the source point cloud and the target point cloud, respectively, to establish the corresponding relationship of the two-way matching constraints; the planar points after the two-way constraints are point-to-plane registered based on the maximum correlation entropy, and the non-planar points after the two-way constraints are point-to-point registered based on the maximum correlation entropy. After multiple iterations, the planar points and non-planar points are respectively obtained in the first The rotation matrix and translation vector in iterations; The rotation matrix and translation vector of the planar point and the non-planar point are weighted according to the adaptive weights; the weighted rotation matrix and translation vector are applied to the source point cloud to obtain the source point cloud after the pose transformation; The output module is used to determine whether the difference between the mean square error of the distance between the source point cloud and the target point cloud after pose transformation in the current iteration and the mean square error of the distance in the previous iteration is less than a given threshold. If so, the registration is completed, otherwise, the iterative optimization continues.

7. A point cloud registration computer device, characterized in that: include: A memory, a processor, and a computer program stored in the memory, wherein the processor implements the steps of the point cloud registration method according to any one of claims 1 to 5 when executing the computer program.

8. A readable storage medium, characterized in that: The readable storage medium stores a computer program, which includes program instructions. When the program instructions are executed by a processor, they are used to execute the steps of the point cloud registration method according to any one of claims 1 to 5.

Images