A Point Cloud Registration Method, System, Device and Medium

Abstract

The present invention discloses a point cloud registration method, system, device and medium, which relates to the technical field of three-dimensional reconstruction of scenes. The method includes obtaining a source point cloud and a target point cloud; performing semantic segmentation according to the spatial structure characteristics of the point cloud to respectively obtain the planar points and non-planar points of the source point cloud and the target point cloud; applying constraints of bidirectional distance search to the planar points and non-planar points in the source point cloud and the target point cloud respectively to establish a corresponding relationship of bidirectional matching constraints; performing point-to-plane registration and point-to-point registration based on the maximum correlation entropy criterion on the planar points and non-planar points after the bidirectional constraints, and respectively obtaining the rotation matrix and translation vector of the planar points and non-planar points after multiple iterations; weighting the rotation matrix and translation vector according to an adaptive weight and applying them to the source point cloud to obtain the source point cloud after pose transformation; this method considers the nearest neighbor correspondence relationship from the source point cloud to the target point cloud and from the target point cloud to the source point cloud, improving the accuracy of corresponding point matching.

Description

Technical Field

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

[0002] Point cloud registration is a key research task in the fields of computer vision and pattern recognition, and plays a crucial role in applications such as three-dimensional reconstruction, mobile robot positioning and pose estimation. The registration algorithm aims to determine the optimal pose transformation between two point clouds. However, due to the interference of dynamic pedestrian noise and degraded scenes, there are huge challenges in performing registration in large-scale scenes.

[0003] From the perspective of establishing corresponding relationships, 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 metric functions to assume initial corresponding relationships, 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 corresponding relationships, and the above steps of finding matching points and estimating the transformation matrix are repeatedly executed 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, since it does not utilize structural features such as surface normal vectors, its accuracy will decrease. The point-to-plane ICP algorithm introduces the normal vector into the optimization function, enabling it to better adapt to multi-plane environments. However, due to the interference of noisy spatial information on the normal vector estimation, this algorithm cannot provide particularly accurate error metrics in unstructured scenes with fewer planar structures. In addition, iterative optimization-based point cloud registration methods have significant initial value dependence, and in practical application scenarios, point cloud data is often interfered by various factors, including a large amount of noise, outliers, and data missing. These interference factors will have an adverse impact on the optimization process of the algorithm, making it easy for the algorithm to fall into local minima during the process of finding the optimal solution, and thus generating incorrect registration results. Feature-based registration algorithms combine the spatial position information and geometric shape information of the point cloud, generate high-dimensional feature descriptors through technologies such as deep learning, and use these descriptors to establish corresponding relationships between point pairs. Since such methods can determine more accurate corresponding relationships, they do not require iterative optimization, but instead use a robust pose transformation estimation method to complete registration in one step. However, such registration algorithms often face the problem of difficult to achieve precise alignment. In addition, the application of deep learning in feature extraction is often limited by the lack of suitable real datasets for training, and will generate additional training time overhead. Therefore, they sometimes cannot meet the requirements of scene reconstruction and positioning.

[0004] In summary, in the current point cloud registration method used in practical application scenarios, due to the lack of utilization of the structural features of the point cloud, its registration accuracy will be reduced; and the point cloud data is easily interfered by various factors, making it difficult to achieve precise alignment. Summary of the Invention

[0005] Aiming at the problems in the prior art that the structural features of the point cloud are not utilized, resulting in reduced registration accuracy; and the point cloud data is easily interfered by various factors, making it difficult to achieve precise alignment, the present invention proposes a point cloud registration method, system, device and medium. By introducing semantic and bidirectional matching constraints, the accuracy of corresponding point matching is improved; at the same time, by introducing the maximum correlation entropy, point-to-plane registration and point-to-point registration are respectively performed, reducing the limitations of a single error metric and improving the accuracy of point cloud registration, thereby greatly improving the problems existing in the prior art.

[0006] A point cloud registration method includes the following steps:

[0007] Obtain the source point cloud and the target point cloud; and perform semantic segmentation on the spatial structure characteristics of the source point cloud and the target point cloud to respectively obtain the planar points and non-planar points of the source point cloud and the target point cloud;

[0008] Apply the constraint of bidirectional distance search to the planar points and non-planar points in the source point cloud and the target point cloud respectively; perform point-to-plane registration based on the maximum correlation entropy on the planar points after the bidirectional constraint, and perform point-to-point registration based on the maximum correlation entropy on the non-planar points after the bidirectional constraint. After multiple iterations, respectively obtain the rotation matrix and translation vector of the planar points and non-planar points between the source point cloud and the target point cloud in the ith iteration; weight the rotation matrix and translation vector of the planar points and non-planar points 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 pose transformation;

[0009] Judge whether the difference between the mean square error of the distance between the source point cloud after pose transformation and the target point cloud in the current iteration and the mean square error of the distance in the previous iteration is less than a given threshold. If it is less, the registration is completed.

[0010] Further, the semantic segmentation of the spatial structure characteristics of the source point cloud and the target point cloud to respectively obtain the planar points and non-planar points of the source point cloud and the target point cloud specifically includes the following steps:

[0011] Use the K-nearest neighbor algorithm to find the neighbor points of each point in the point cloud, and based on the principal component analysis method, estimate the surface normal of the point by fitting the local plane features of the neighbor points, and obtain the standard eigenvalue equation as:

[0012] ;

[0013] where represents Covariance matrix, representing adjacent data points, representing the mean vector of the K-nearest neighbor algorithm, T is the matrix transpose, k representing the number of neighbor points;

[0014] Solve the standard eigenvalue equation by the singular value decomposition method:

[0015] ;

[0016] where, represents the eigenvector matrix, and the third eigenvector represents the normal of the point ; represents the eigenvalue matrix, and the third eigenvalue represents the curvature of the point ;

[0017] Sort each point in the point cloud in ascending order of curvature, starting from the unprocessed point with the smallest curvature and create a list to store points of the same semantics; for each unprocessed point in , traverse each unprocessed point in its neighborhood , and if the constraint conditions are met, add it to the list ; the constraint conditions are:

[0018] ;

[0019] where, represents the normal vector angle threshold, represents the orthogonal distance threshold, represents the parallel distance threshold, and are both adaptive according to the scale ; when all points in are traversed, mark as processed;

[0020] Process the point cloud using region growing in a loop until all points in are processed, obtaining a set of planar points and a set of non-planar points .

[0021] Furthermore, the constraints of applying bidirectional distance search to the planar points and non-planar points in the source point cloud and the target point cloud specifically include the following steps:

[0022] For the points in the source point cloud Apply a rotation transformation and a translation transformation , and search for the nearest neighbor points in the target point cloud ; At the first iteration, is set to the identity matrix of , and the initial value of is set to

[0023] the zero vector of and ; Calculate the inverse matrices of and , and apply the corresponding pose transformation to , and search for the nearest neighbor points in the source point cloud ;

[0024] Judge whether the Euclidean distance between and the corresponding is less than the specified threshold . If it is less, the point in the source point cloud and its nearest neighbor point in the target point cloud after pose transformation are a pair of matching points that meet the two-way matching constraint; the corresponding relationship of its two-way matching constraint satisfies:

[0025] ;

[0026] Among them, represents a set of corresponding relationships that meet the two-way matching constraint, 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.

[0027] Furthermore, perform point-to-plane registration based on the maximum correlation entropy for the plane points after two-way constraint, and perform point-to-point registration based on the maximum correlation entropy for the non-plane points after two-way constraint; after multiple iterations, obtain the rotation matrix and translation vector of the plane points and non-plane points between the source point cloud and the target point cloud in the th iteration respectively, specifically including the following steps:

[0028] For the plane points and non-plane points after two-way constraint, introduce the maximum correlation entropy criterion and construct a point cloud rigid body registration optimization function:

[0029] ;

[0030] Among them, and respectively represent the rotation matrix and the translation vector, and SO(3) is the group consisting 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 for controlling the relevant entropy distribution; represents the L2 norm;

[0031] Construct the point-to-plane error metric function based on the maximum correlation entropy and the point-to-point error metric function based on the maximum correlation entropy for planar points and non-planar points respectively and the point-to-point error metric function based on the maximum correlation entropy :

[0032] ;

[0033] ;

[0034] where, and respectively represent the planar points in the source point cloud and their corresponding planar points in the target point cloud; represents the normal vector of the corresponding planar 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;

[0035] According to the point-to-plane error metric function and the point-to-point error metric function, represent the optimization function to be solved as:

[0036] ;

[0037] where, and respectively represent the weights of planar points and non-planar points;

[0038] Solve the planar points and non-planar points in the optimization function respectively to obtain the rotation matrix in the , and the translation vector , .

[0039] Furthermore, weight the rotation matrices and translation vectors of planar points and non-planar points according to the adaptive weights; specifically, it includes the following steps:

[0040] Convert the rotation matrix and into the corresponding quaternions and and then perform weighting to obtain the weighted ;

[0041] Pair After normalization, it is converted into a rotation matrix to obtain the weighted in the ;

[0042] For the translation vector , perform adaptive linear weighting to obtain the weighted in the .

[0043] The present invention also includes a point cloud registration system, including:

[0044] An acquisition module for acquiring a source point cloud and a target point cloud; and performing semantic segmentation according to the spatial structure characteristics of the point cloud to respectively obtain the planar points and non-planar points of the source point cloud and the target point cloud;

[0045] A registration module for respectively imposing constraints of bidirectional distance search on the planar points and non-planar points in the source point cloud and the target point cloud to establish a corresponding relationship of bidirectional matching constraints; performing point-to-plane registration based on the maximum correlation entropy on the planar points after the bidirectional constraints and performing point-to-point registration based on the maximum correlation entropy on the non-planar points after the bidirectional constraints. After multiple iterations, respectively obtain the rotation matrix and translation vector of the planar points and non-planar points in the iteration; weight the rotation matrix and translation vector of the planar points and non-planar points according to an adaptive weight; apply the weighted rotation matrix and translation vector to the source point cloud to obtain the source point cloud after pose transformation;

[0046] An output module for determining whether the difference between the mean square error of the distance between the source point cloud after pose transformation and the target point cloud in the current iteration and the mean square error of the distance in the previous iteration is less than a given threshold. If it is less, the registration is completed; otherwise, continue iterative optimization.

[0047] The present invention also includes a point cloud registration computer device, including: a memory, a processor, and a computer program stored in the memory. When the processor executes the computer program, the steps of the point cloud registration method are implemented.

[0048] The present invention also includes a readable storage medium. The readable storage medium 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.

[0049] The present invention provides a point cloud registration method, which has the following beneficial effects:

[0050] The present invention performs semantic segmentation based on the spatial structure characteristics of point clouds. Compared with traditional methods that only consider eigenvalue, the present invention also focuses on the scale information and coplanarity of point clouds, avoiding incorrect segmentation caused by noise, sparse point clouds or boundary points. When establishing the correspondence relationship between point clouds, the present invention introduces semantic and bidirectional matching constraints. For semantic constraints, the present invention searches for matching point pairs among points with the same semantics after segmentation, reducing the interference of the dynamic and static relationships between points with different semantics. For bidirectional matching constraints, the present invention simultaneously considers the correspondence relationship 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 condition in both directions are considered valid corresponding points, significantly improving the accuracy of corresponding point matching. For the different characteristics of planar points and non-planar points, point-to-plane registration and point-to-point registration are respectively performed by introducing the maximum correlation entropy, and weighted joint optimization is carried out to utilize their advantages in a complementary manner, reducing the limitations of a single error metric, thereby improving the accuracy of point cloud registration. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a flowchart of the point cloud registration method in an embodiment of the present invention;

[0052] Figure 2 is a schematic diagram of establishing the correspondence relationship of bidirectional matching constraints in an embodiment of the present invention; Figure 2 (a) is a schematic diagram of establishing the correspondence relationship of unidirectional matching constraints, Figure 2 and (b) is a schematic diagram of establishing the correspondence relationship of bidirectional matching constraints;

[0053] Figure 3 is a schematic diagram of the distribution of the mean square error function and the maximum correlation entropy function in an embodiment of the present invention; Figure 3 (a) is a schematic diagram of the distribution of the mean square error function, Figure 3 and (b) is a schematic diagram of the distribution of the maximum correlation entropy function;

[0054] Figure 4 is a schematic diagram of the visualization results in the ETH Hauptgebäude public dataset and the self-collected indoor dataset in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

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

[0056] The present invention proposes a point cloud registration method based on semantic constraints and maximum correlation entropy, which specifically includes the following steps:

[0057] Step 1: Voxel downsample the input point cloud according to a certain voxel size to ensure uniform point cloud density and reduce the computational complexity of subsequent registration. Specifically:

[0058] Obtain the source point cloud and the target point cloud from the lidar or locally. Downsample the input point cloud according to a certain voxel size. On the ETH Hauptgebaude public dataset and the self - collected indoor dataset, the voxel size is set to 0.3 meters. The ETH Hauptgebaude dataset is a corridor about 60m long, containing a large number of repetitive structures and dynamic pedestrians. After different perturbations, a total of 945 frames of point clouds are formed. The self - collected indoor dataset has 533 frames of point clouds collected by a robot platform at a constant speed in a large - scale teaching building scene, also containing a large number of dynamic pedestrians, and there are significant differences in human postures and body proportions. The above two datasets are both highly challenging datasets in the field of point cloud registration.

[0059] Step 2: Use spatial structure characteristics such as the normal vector and curvature of the point cloud for semantic segmentation. For the convenience of subsequent calculations, and are represented as the planar points in the source point cloud and the target point cloud, and are represented as the non - planar points in the source point cloud and the target point cloud. Based on the calculation of standard eigenvalues, seed points are selected according to the curvature size, and semantic division is performed according to the size relationship between the normal vector angle, orthogonal distance, and parallel distance from the seed point to its neighboring points and the threshold. Then, the region growing method is used to process the point cloud cyclically, and finally, the set of planar points and the set of non - planar points are obtained. Specifically, the downsampled point cloud is semantically segmented according to the spatial structure characteristics of the point cloud. The semantic segmentation algorithm proposed in the present invention mainly includes two parts: 1. Geometric spatial feature calculation; 2. Region growing.

[0060] For geometric spatial feature calculation, first, the K - Nearest Neighbor (KNN) 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:

[0061] (1);

[0062] In the formula represents the covariance matrix, represents the 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.

[0063] Then, solve the standard eigenvalue equation (1) by Singular Value Decomposition (SVD):

[0064] (2);

[0065] 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 K-nearest neighbors of the point have been obtained, the distribution scale of the points in the neighborhood can be briefly estimated , which is defined as the distance between it and its third nearest neighbor.

[0066] For region growing, first, sort all the points in ascending order of curvature. Starting from the unprocessed point with the smallest curvature, create a list to store the points with the same semantics. For each unprocessed point in , traverse each unprocessed point in its neighborhood . If the following conditions are met, add it to the list

[0067] (3)

[0068] In the formula represents the normal vector angle threshold, represents the orthogonal distance threshold, represents the parallel distance threshold, and are both adaptive according to the scale of . When all the points in have been traversed, mark as processed. The region grows point by point until all the points in are processed, obtaining the set of planar points and the set of non-planar points .

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

[0070] Establish the corresponding relationship of bidirectional matching constraints between the source point cloud and the target point cloud of planar points and non-planar points respectively, that is, while searching for the nearest neighbor semantic points that can be matched from the source point cloud to the target point cloud, a same matching relationship should also be constructed from the target point cloud to the source point cloud to establish a bidirectional search mode, improving the stability and reliability of the overall registration. In the traditional Iterative Closest Point (ICP) algorithm system, usually each point in the source point cloud searches for one-way matching towards the entire target point cloud. This one-way matching mode has a certain degree of intuitiveness and efficiency, and can quickly and accurately establish the corresponding relationship between two point clouds in a simple scenario. However, the ICP algorithm has the inherent property of local convergence, resulting in the algorithm being prone to falling into a local optimal solution in scenarios with a large amount of noise and partial overlap, leading to registration failure, such as Figure 2 (a) is a schematic diagram for establishing the corresponding relationship of one-way matching constraints. The fundamental reason is that the traditional ICP algorithm has a narrow convergence domain. When the geometric structures of the point clouds are highly similar and the distribution density of the same semantic points is high, during the process of the source point cloud searching for the corresponding relationship, according to the one-way distance measurement criterion, it is very easy to concentrate the corresponding points in a certain local area of the target point cloud. This phenomenon of local convergence makes it difficult for the algorithm to obtain the globally optimal corresponding relationship, and thus it is impossible to achieve accurate global registration.

[0071] To address this problem, the present invention introduces a bidirectional distance matching strategy to optimize the construction of the corresponding relationship. Specifically, as Figure 2 (b) is a schematic diagram for establishing the corresponding relationship of bidirectional matching constraints. While the source point cloud searches for the nearest neighbor of the same semantic point in the target point cloud based on the Euclidean distance measurement, a reverse search link from the target point cloud to the source point cloud is constructed to form a bidirectional matching constraint. Taking the point in the source point cloud and the point in the target point cloud as an example, the bidirectional matching satisfies the following formula:

[0072] (4)

[0073] In the formula, represents a set of corresponding relationships 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 the rotation transformation and the translation transformation to the point in the source point cloud, and search for the The nearest neighbor point in the target point cloud . At the first iteration, The initial value of is set to the identity matrix of The initial value of is set to the zero vector of and The inverse matrix of and are calculated, and the corresponding pose transformation is applied to to find the nearest neighbor point of the transformed in the source point cloud . Finally, it is judged whether the Euclidean distance between and the corresponding is less than the specified threshold . If it is less, it is considered that the point in the source point cloud and its nearest neighbor point in the target point cloud after pose transformation form a pair of matching points that meet the bidirectional correspondence relationship. b ( i ) represents the index of the nearest neighbor point of the pose-transformed in the target point cloud.

[0074] Step 4: For the planar points and non-planar points after bidirectional constraint, perform point-to-plane registration based on the maximum correlation entropy criterion and point-to-point registration based on the maximum correlation entropy criterion respectively, to obtain the rotation matrix and translation vector of the planar points and non-planar points in the th iteration. The rotation matrix and translation vector obtained in the th iteration in the planar point registration are denoted as and , and the rotation matrix and translation vector obtained in the th iteration in the non-planar point registration are denoted as and .

[0075] In the framework of the classical ICP algorithm, the rigid body transformation problem of 3D point clouds can be reduced to an optimization problem in mathematics. Usually, the mean square error (MSE) of the least square (LS) is used as the error metric function between point pairs. By Figure 3Schematic diagram of the mean squared error function distribution shown in (a). When the source point cloud and the target point cloud gradually approach each other as a whole, the Euclidean space distance metric value corresponding to the MSE function will decrease accordingly. However, due to the characteristics of the quadratic function itself, the trend of its distribution change 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 prone to confusing inliers and outliers, resulting in registration failure. Even with the constraint of semantic information, it is difficult to completely eliminate the influence of inlier noise points on the solution of the point cloud spatial transformation. In addition, since large-scale scene point clouds may simultaneously have planar structures, non-planar structures, and dynamic pedestrian noise, and the point cloud densities are different in different scenarios, only point-to-point registration may mis-match dense and regular planar features, leading to falling into a local optimal solution and unable to accurately align the plane; while only point-to-plane registration may not be able to fit a single plane in the non-planar region, resulting in a large deviation in the registration result.

[0076] To address the above problems, the present invention will introduce the Maximum Correntropy Criterion (MCC). As Figure 3 Schematic diagram of the maximum correntropy function distribution shown in (b). When the source point cloud and the target point cloud approach each other in the overall situation and the distance between corresponding point pairs is small, under the measurement system of the maximum correntropy, this corresponding relationship can obtain a large value; for points with a large distance between the two point clouds, especially noise points and outliers, the maximum correntropy function will assign a very small function value, thereby effectively suppressing their interference on the registration process.

[0077] For planar points and non-planar points that meet the bidirectional constraints, introduce the maximum correntropy criterion to construct a point cloud rigid body registration optimization function:

[0078] (5)

[0079] In the formula, and respectively represent the rotation matrix and the translation vector; SO(3) is the group composed of all 3×3 real orthogonal matrices with a determinant of 1; represents the number of corresponding point pairs; represents the error metric function; represents the bandwidth that controls the correntropy distribution; is the three-dimensional real number space; represents the L2 norm. represents that it is necessary to ensure that the result of the dot product of the transpose of the rotation matrix and the rotation matrix is the identity matrix; represents that it is necessary to ensure that the determinant of the rotation matrix is 1.

[0080] On this basis, for planar points and non-planar points, a point-to-plane error metric function based on maximum correlation entropy and a point-to-point error metric function based on maximum correlation entropy are constructed respectively:

[0081] (6)

[0082] (7)

[0083] In the formula, and respectively represent the planar points in the source point cloud and their corresponding planar points in the target point cloud; represents the normal vector of the corresponding planar 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. represents the point-to-plane error metric; represents the point-to-point error metric.

[0084] Combining formula (6) and formula (7), the optimization function to be solved can be expressed as:

[0085] (8)

[0086] In the formula, and respectively represent the weights of planar points and non-planar points. is a function of the rotation matrix R and the translation vector The purpose is to optimize the values of R and to make the function value reach the maximum; N represents the number of point pairs that meet the two-way constraints.

[0087] Solve the planar point part and the non-planar point part in the optimization function respectively to obtain the rotation matrix and the translation vector , and , of planar points and non-planar points in the

[0088] For the non-planar point part of the optimization function, in order to solve the extreme values of and , first set the partial derivative of to zero:

[0089] (9)

[0090] In order to solve the approximate value of in the iteration, it is necessary to use the In the and are substituted into the exponential term of formula (7). Assume this exponential term is , specifically expressed as:

[0091] (10)

[0092] Substitute into formula (9), and we can get:

[0093] (11)

[0094] Substitute formula (11) back into formula (5) to get:

[0095] (12)

[0096] Where:

[0097] (13)

[0098] (14)

[0099] Then for the solution of is equivalent to the solution of . Assume:

[0100] (15)

[0101] To derive the rotation matrix , the Lagrange multiplier method can be introduced for calculation. The function is defined as:

[0102] (16)

[0103] In the formula, is the Lagrange scalar operator; is the Lagrange matrix operator of ; is the trace of the matrix, which is the sum of all eigenvalues in the matrix. is the correlation entropy weight function, which calculates the distance between the rotated vector and the target vector and weights it using the exponential function. represents using the identity matrix to measure whether the rotation matrix R satisfies All elements in the matrix are 0. Incorporating it into the objective function forces R to satisfy the orthogonality constraint of the rotation matrix during the solution of R. is used to measure whether the determinant of the rotation matrix R is equal to 1. Add to the objective function.It is to force its determinant to be 1 when optimizing R to ensure the orientation-preserving property of the rotation operation.

[0104] To solve the extreme value problem by the method of Lagrange multipliers, it is necessary to find the partial derivatives of the three variables in the function respectively, set each partial derivative to zero, construct the following system of equations and solve it:

[0105] (17)

[0106] Let , and it can be calculated that:

[0107] (18)

[0108] This formula can be represented in matrix form, and the singular value decomposition (SVD algorithm) is used to solve this expression:

[0109] (19)

[0110] In the formula, and are both dimensional orthogonal matrices; is a dimensional diagonal matrix, and its diagonal elements are all non-negative numbers and arranged in decreasing order.

[0111] By maximizing formula (15), performing operations such as transposing the matrix and calculating the determinant, the following results can be obtained:

[0112] (20)

[0113] Among them

[0114] (21)

[0115] In the formula, is expressed as a diagonal matrix and the diagonal elements are composed of .

[0116] Substitute the rotation matrix obtained from the th iteration calculation back into formula (11), so as to solve :

[0117] (22)

[0118] Finally, the specific values of the rotation matrix and the translation vector corresponding to the th iteration in point-to-point registration can be obtained.

[0119] For the planar point part of the optimization function, in order to solve the and extremum, the present invention adopts a geometric method to approximate the pose transformation between two adjacent iterations with relatively small relative displacement from a non-linear optimization problem to a linear optimization problem for solution. First, assume that the rotation of the source point cloud in each iteration can be composed of three Euler angle parameters , and , and the translation can be composed of three Cartesian coordinate parameters , and . Then, a variable to be solved can be defined and expressed as:

[0120] (23)

[0121] Since the rotation angle in each iteration is very small, that is, , it can be obtained that:

[0122] (24)

[0123] In the formula, is the Euler angle. According to the properties of Lie algebra, the rotation matrix can be approximated as:

[0124] (25)

[0125] Therefore, the point-to-plane registration optimization function based on the maximum correlation entropy is rewritten as:

[0126] (26)

[0127] Where:

[0128] (27)

[0129] (28)

[0130] (29)

[0131] Among them, represents the three components in the xyz coordinate axis directions of the vector obtained by taking the cross product of the normal of the planar point in the source point cloud and the nearest neighbor planar point in the target point cloud; represents the three components in the xyz coordinate axis directions of the normal of the nearest neighbor planar point in the target point cloud.

[0132] To obtain the extremum of formula (26), it is necessary to make The derivative of is zero, i.e.:

[0133] (30)

[0134] In order to solve the variable to be solved in the th iteration, it is necessary to substitute the in the th iteration into the exponential term of formula (30), and express the exponential term as:

[0135] (31)

[0136] Let , , and , then the optimal solution in the th iteration can be calculated as:

[0137] (32)

[0138] Finally, the specific values of the rotation matrix and the translation vector corresponding to the th iteration in point-to-plane registration can be obtained according to . respectively represent the row vectors of matrix A and are used to represent the coefficient matrix in the registration optimization function; respectively represent the elements of vector b and are used to represent the constant term in the registration optimization function; respectively represent the elements of vector w and are used to measure the weights of each data point; represents the diagonal matrix generated by vector w .

[0139] Step 5, Convert the rotation matrix and to quaternions and and weight them according to the adaptive weights and , and then convert the normalized weighted quaternion to a rotation matrix . And the translation vectors and are directly weighted adaptively to obtain the translation vector . Specifically, it includes the following steps:

[0140] The rotation matrix obtained from the th iteration in plane point registration Translation vector In the non-planar point registration, the rotation matrix obtained in the ith iteration and the translation vector are weighted and fused according to the adaptive weight. For the sake of convenience of description, the rotation matrix obtained in the ith iteration of planar point registration and the translation vector are denoted as and respectively. In the non-planar point registration, the rotation matrix obtained in the ith iteration and the translation vector are denoted as and .

[0141] Since the rotation matrix belongs to the Lie group and has a non-linear structure, the rotation matrix cannot be linearly weighted directly. Therefore, the rotation matrices and need to be converted into the corresponding quaternions and before weighting:

[0142] (33).

[0143] For the rotation matrices , they need to be adaptively weighted after being converted into quaternions to obtain the weighted . The adaptive weight is expressed as:

[0144] (34)

[0145] (35)

[0146] In the formula, represents the number of planar points; represents the number of planar points.

[0147] After is normalized and then converted into a rotation matrix, the weighted in the ith iteration is obtained.

[0148] While the translation vector is in the three-dimensional real space , and it can be linearly weighted directly; for the translation vectors , they can be adaptively linearly weighted to obtain the weighted in the ith iteration:

[0149] (36).

[0150] Step 6: Apply the rotation matrix and translation vector obtained in the -th iteration to the source point cloud, and determine whether the difference between the mean square error of the distances between the two point clouds after registration in the current iteration and the mean square error of the distances in the previous iteration is less than a given threshold. If it is less, it is considered that the registration is completed; otherwise, return to Step 3 to continue iterative optimization. Specifically: In the -th iteration, apply the rotation matrix and the translation vector to the source point cloud to obtain the source point cloud after pose transformation. Subsequently, calculate the mean square error of the nearest points between the transformed source point cloud and the target point cloud. By comparing the error values obtained in the current iteration and the previous iteration, find the difference between the two. If this difference is less than the pre-given threshold, it is determined that the iterative process has converged, and the optimal pose transformation matrix is output. Otherwise, return to Step 3 to continue iterative optimization.

[0151] Experimental analysis:

[0152] Referring to Table 1, Table 2 and Figure 4 , the present invention has conducted comparative experiments with a variety of classic point cloud registration algorithms based on optimization on the ETH Hauptgebaude public dataset and the self-collected indoor dataset. These methods all 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 performance of the registration. The calculation method of the average translation error is , and the calculation method of the average rotation error is , where and represent the translation vector and the true value translation vector obtained by the algorithm respectively, and They respectively represent the rotation matrix obtained by the algorithm and the true value rotation matrix. 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 both the rotation error and the translation error are lower than the preset threshold, it is determined that the registration is successful. To systematically evaluate the adaptability of the registration algorithm in complex scenarios, the present invention tests the stability of the algorithm through the registration recall rate. On this basis, the average translation error and the average rotation error are respectively calculated for the registered point clouds to verify the accuracy of the algorithm. It should be particularly noted that the point clouds with failed registration will introduce significant translation errors and rotation errors, and such outliers will seriously affect the reliability of the accuracy evaluation. Based on the above considerations, the present invention only conducts error statistical analysis on the point clouds that meet the successful matching conditions, so as to ensure the effectiveness of the evaluation results. In the present invention, a pair of point clouds with a translation error less than 0.5 meters and a rotation error less than is regarded as a pair of successfully registered point clouds.

[0153] Experimental data show that classical registration algorithms perform poorly in the current dataset, especially in terms of robustness, with a general lack. Consistent with the expected results, algorithms based on the ICP framework failed to complete registration successfully 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 construct an effective anti-noise optimization mechanism. The point-to-plane ICP algorithm showed improved performance compared to the standard ICP algorithm because its measurement method was more adaptable to scenarios with obvious plane features. MCC-ICP achieved registration results similar to those of the point-to-plane ICP on the basis of the basic ICP algorithm framework through an outlier rejection strategy. GICP considered the local geometric structure of the point cloud and used a probability model to weight the error, thus improving the registration accuracy and robustness. However, in scenarios with dynamic object interference and sparse point cloud density, this algorithm could not calculate the accurate covariance matrix, resulting in incorrect pose estimation. CobigICP introduced both the cross-correlation entropy criterion and bidirectional error, enabling it to establish more accurate correspondence relationships, but there were still mismatches between points with different semantics. Generally speaking, most ICP-based algorithms have the following inherent defects - being highly sensitive to the initial pose and prone to local optimal solution problems in complex scenarios. Different from local optimization strategies, 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 dual optimization strategy. Although its robustness has been improved, the registration accuracy is poor. Compared with traditional methods, the present invention introduces semantic constraints and bidirectional constraints. At the same time, for the different characteristics of plane points and non-plane points, point-to-plane and point-to-point error measurement functions based on the maximum correlation entropy are established respectively, and weighted joint optimization is carried out, making it have significant advantages in rotational error, translational 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 has increased. However, the above modules can help the algorithm establish correct correspondence relationships, improve the convergence speed of the algorithm, and thus reduce the overall number of iterations. Therefore, the present invention only takes slightly more time 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.

[0154] Table 1 Registration Results of ETH Hauptgebaude Public Dataset

[0155]

[0156] Table 2 Registration Results of Self-Collected Indoor Dataset

[0157]

[0158] The present invention uses multi-geometric space features and region growth for semantic segmentation. Compared with the traditional method that only considers eigenvalue, the present invention also focuses on the scale information and coplanarity of the point cloud, avoiding incorrect segmentation caused by noise, sparse point cloud or boundary points; when establishing the correspondence relationship between point clouds, the present invention introduces semantic and bidirectional matching constraints; for semantic constraints, the present invention searches for matching point pairs among points with the same semantics after segmentation, reducing the interference of the dynamic and static relationships between points with different semantics; for bidirectional matching constraints, the present invention simultaneously considers the nearest neighbor correspondence relationship 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 condition in both directions are considered valid corresponding points, significantly improving the accuracy of corresponding point matching; when constructing the optimization function, for the different characteristics of planar points and non-planar points, point-to-plane and point-to-point error metric functions based on maximum correlation entropy are respectively established and weighted and jointly optimized to utilize their advantages in a complementary manner and reduce the limitations of a single error metric; when solving the pose transformation, the weights are adaptively adjusted according to the geometric characteristics of the environment to perform robust pose estimation.

[0159] Based on the same inventive concept, the present invention also provides a point cloud registration system, including:

[0160] An acquisition module, configured to acquire a source point cloud and a target point cloud; and perform semantic segmentation according to the spatial structure characteristics of the point cloud to respectively obtain the planar points and non-planar points of the source point cloud and the target point cloud.

[0161] A registration module, configured to respectively impose constraints of bidirectional distance search on the planar points and non-planar points in the source point cloud and the target point cloud to establish a correspondence relationship with bidirectional matching constraints; perform point-to-plane registration based on the maximum correlation entropy criterion on the planar points after bidirectional constraints and perform point-to-point registration based on the maximum correlation entropy criterion on the non-planar points after bidirectional constraints. After multiple iterations, respectively obtain the rotation matrix and translation vector of the planar points and non-planar points in the nth iteration; weight the rotation matrix and translation vector of the planar points and non-planar points according to the adaptive weights; apply the weighted rotation matrix and translation vector to the source point cloud to obtain the source point cloud after pose transformation.

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

[0163] The present invention also provides a point cloud registration computer device, including: a memory, a processor, and a computer program stored in the memory. When the processor executes the computer program, the steps of the point cloud registration method are implemented.

[0164] The present invention also provides a readable storage medium storing a computer program, the computer program including program instructions, which are used to execute the steps of the point cloud registration method when being executed by a processor.

[0165] The above are only the preferred specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention should cover the protection scope of the present invention by making equivalent substitutions or changes according to the technical solution and inventive concept 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 the iteration include the following steps: for the bidirectionally constrained planar points and non-planar points, 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 a special orthogonal group; 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 , ; 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.

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 neighboring points of each point in the point cloud, and the normal of the neighboring surface is estimated by principal component analysis, 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 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 .

5. 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 the iteration include the following steps: for the bidirectionally constrained planar points and non-planar points, 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 a special orthogonal group; 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 , ; 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 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.

6. 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 4 when executing the computer program.

7. 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 4.

Images