A point cloud registration method based on second-order spatial compatibility metric

Abstract

The application belongs to the technical field of three-dimensional point cloud registration, and specifically provides a point cloud registration method based on second-order spatial compatibility measurement, comprising the following steps: extracting corner points of source point cloud and target point cloud as key points by using Harris-3D algorithm; describing and preliminarily matching the extracted key points by using FPFH algorithm to obtain matching pairs; calculating a second-order spatial compatibility matrix between the matching pairs, and removing false matching pairs to obtain correct matching pairs; calculating a coarse registration result of two point clouds by using singular value decomposition method in combination with the correct matching pairs to obtain an initial pose; and calculating a fine registration result of point clouds according to the initial pose of two point clouds in combination with K-dimensional tree accelerated ICP algorithm to finally complete the point cloud registration work. The application effectively suppresses the influence of outliers on registration accuracy, and improves the accuracy and speed of point cloud registration.

Description

Technical Field

[0001] This invention relates to the field of 3D point cloud registration technology, and more specifically, to a point cloud registration method based on a second-order spatial compatibility metric. Background Technology

[0002] In recent years, with the rapid development of 3D scanning technology, various 3D point cloud data acquisition devices have emerged. However, due to limitations imposed by external environmental factors during on-site data acquisition, a single 3D scanner scan can only acquire partial point cloud information of an object's surface. To obtain global point cloud information, multiple measurements at multiple stations are required, ultimately achieved through point cloud registration technology. Point cloud registration is a technique that transforms multiple point cloud images from different perspectives into the same coordinate system.

[0003] Point cloud registration has wide applications, including 3D reconstruction, target recognition, and pose estimation. Feature-based point cloud registration methods typically extract feature points with unique properties from the point cloud, such as SURF, SIFT, and ORB feature points, and then achieve registration through feature description and feature matching algorithms. During the matching process, erroneous matching pairs with similar feature descriptions are inevitable. The RANSAC algorithm is commonly used to remove these erroneous pairs, but it requires extensive random sampling and model fitting, resulting in high computational complexity. Furthermore, the RANSAC algorithm is limited by the number of iterations and distance thresholds; if the parameters are not set appropriately, erroneous matching pairs may not be removed. Therefore, existing feature-based point cloud registration algorithms suffer from high computational cost, long processing time, and difficulty in removing erroneous point pairs. Summary of the Invention

[0004] This invention addresses the technical problems of existing feature-based point cloud registration algorithms, such as high computational cost, long processing time, and difficulty in removing erroneous point pairs.

[0005] This invention provides a point cloud registration method based on a second-order spatial compatibility metric, comprising the following steps:

[0006] (1) Use the Harris-3D algorithm to extract the corner points of the source point cloud and the target point cloud as key points;

[0007] (2) The extracted key points are described and initially matched using the FPFH algorithm to obtain matching pairs;

[0008] (3) Calculate the second-order spatial compatibility matrix between the matching pairs, and use it to remove incorrect matching pairs to obtain correct matching pairs;

[0009] (4) Using singular value decomposition and the correct matching pair, calculate the coarse registration result of the two point clouds to obtain the initial pose;

[0010] (5) Based on the initial poses of the two point clouds, the ICP algorithm accelerated by the K-dimensional tree is used to calculate the fine registration result of the point cloud, and finally the point cloud registration work is completed.

[0011] Preferably, step (1) specifically includes:

[0012] The first step is to solve for the point cloud normal vectors using principal component analysis.

[0013] The second step is to solve for the covariance matrix formed by the point cloud normal vectors;

[0014] The third step is to solve for the eigenvalues ​​of the covariance matrix. If the smallest eigenvalue is relatively large, it is identified as a corner point.

[0015] Preferably, step (2) specifically includes:

[0016]

[0017] Among them, SPFH(P q ) and SPHF(P k () are query points P q Simplified point P of the nearest neighbor k The simplified point feature histogram.

[0018] Preferably, step (3) specifically includes:

[0019] Calculate the Euclidean distance between the corner points that form a matching pair in the source point cloud;

[0020] Calculate the Euclidean distance between the corner points that form a matching pair in the target point cloud;

[0021] Calculate the Euclidean distance difference between matched pairs;

[0022] The first-order spatial compatibility between matching pairs is characterized by a monotonically decreasing exponential function. The smaller the Euclidean distance difference between matching pairs, the higher their first-order spatial compatibility.

[0023] Binarize the first-order spatial compatibility matrix according to the set threshold.

[0024] The number of matching pairs that are compatible with the binarized matrix is ​​calculated as a description of its second-order similarity, resulting in a second-order spatial compatibility matrix. Matching pairs that are incompatible with the second-order spatial matrix are then removed.

[0025] Preferably, step (4) specifically includes:

[0026] After obtaining n correct matching pairs, the optimal rotation matrix R and translation vector t are solved iteratively using the least squares method. The calculation process is as follows:

[0027]

[0028] Where, q i To use a K-dimensional tree to search the target point cloud p i The nearest corresponding point is obtained by first treating the rotation matrix R as a constant. Then it is a function of the variable t. Then, take the partial derivative with respect to the variable t and set the partial derivative to zero to solve for t and R.

[0029] Preferably, step (5) specifically includes:

[0030] The first step involves using the source point cloud P′ and the target point cloud, after coarse registration coordinate transformation, as input point clouds for an iterative nearest-point algorithm, which converts all points p in the point cloud P′ to be registered. i In the target point cloud Q, use a K-dimensional tree to search for the nearest corresponding point q. i Form corresponding point pairs;

[0031] The second step is to iteratively calculate the optimal transformation using the least squares method to minimize the mean square error between corresponding point pairs, and then use the singular value decomposition method to solve for the rotation matrix R and the translation vector t.

[0032] The third step is to apply the rotation matrix R and the translation vector t to the point cloud P′ to be registered, while setting the error threshold ε and the maximum number of iterations N. max Calculate the distance error between the two point cloud iterations. If the threshold condition is met or the maximum number of iterations is reached, output the final transformation matrix T.

[0033] Preferably, the process further includes the following after step (5):

[0034] To verify the effectiveness and rationality of the method used in this study, mean square error (MSE) is introduced as an evaluation index for the quality of point cloud registration. The MSE R0 MS The description is as follows:

[0035]

[0036] Where T is the final transformation matrix, p i and q i These represent points in the source point cloud and the target point cloud, respectively, with N being the total number of points.

[0037] Beneficial Effects: This invention provides a point cloud registration method based on a second-order spatial compatibility metric, comprising: extracting corner points of the source and target point clouds as key points using the Harris-3D algorithm; describing and initially matching the extracted key points using the FPFH algorithm to obtain matching pairs; calculating the second-order spatial compatibility matrix between the matching pairs, and eliminating incorrect matching pairs to obtain correct matching pairs; calculating the coarse registration result of the two point clouds using singular value decomposition and the correct matching pairs to obtain the initial pose; calculating the fine registration result of the point clouds based on the initial pose of the two point clouds and using the ICP algorithm accelerated by K-trees, and finally completing the point cloud registration work. This invention effectively suppresses the influence of outliers on registration accuracy, improving the accuracy and speed of point cloud registration. Attached Figure Description

[0038] Figure 1 A flowchart of the point cloud registration method based on second-order spatial compatibility metric provided by the present invention;

[0039] Figure 2 A schematic diagram of a first-order matching approximation quadratic surface provided by the present invention;

[0040] Figure 3 The schematic diagram of the PFH algorithm provided by this invention;

[0041] Figure 4 A diagram illustrating the construction of a local coordinate system provided by this invention;

[0042] Figure 5 The FPFH algorithm schematic diagram provided by this invention;

[0043] Figure 6 This is a schematic diagram of a second-order spatial compatibility matrix matching pair provided by the present invention;

[0044] Figure 7 The point cloud registration process for the rabbit model provided by this invention;

[0045] Figure 8 The point cloud registration process for the dinosaur model provided by this invention;

[0046] Figure 9 The point cloud registration process for the horse model provided by this invention;

[0047] Figure 10 A comparison diagram of various algorithms for the rabbit model provided by this invention;

[0048] Figure 11 A comparison chart of various algorithms for the dinosaur model provided by this invention;

[0049] Figure 12 A comparison diagram of various algorithms for the horse model provided by this invention; Detailed Implementation

[0050] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0051] like Figures 1 to 5 As shown, this invention provides a point cloud registration method based on a second-order spatial compatibility metric, which employs two stages: coarse registration and fine registration. In the coarse registration stage, the Harris-3D algorithm is used to acquire key points for each point cloud image, and its Fast Point Feature Histogram (FPFH) descriptor is calculated to complete the matching between key points. Then, incorrect matching pairs are eliminated by calculating the second-order spatial compatibility matrix of the matched point pairs, and a preliminary transformation matrix is ​​obtained using singular value decomposition. In the fine registration stage, the ICP algorithm accelerated by a kd-tree is used to calculate the final transformation matrix, completing the point cloud registration. Overall, the point cloud registration method based on a second-order spatial compatibility metric includes the following steps:

[0052] (1) Use the Harris-3D algorithm to extract the corner points of the source point cloud and the target point cloud as key points;

[0053] (2) The extracted key points are described and initially matched using the FPFH algorithm to obtain matching pairs;

[0054] (3) Calculate the second-order spatial compatibility matrix between the matching pairs, and use it to remove incorrect matching pairs to obtain correct matching pairs;

[0055] (4) Using singular value decomposition and the correct matching pair, calculate the coarse registration result of the two point clouds to obtain the initial pose;

[0056] (5) Based on the initial poses of the two point clouds, the ICP algorithm accelerated by the K-dimensional tree is used to calculate the fine registration result of the point cloud, and finally the point cloud registration work is completed.

[0057] The specific implementation process of step (1): For a point P on the point cloud, construct a quadratic surface f(x,y,z)=0, move u,v,w units along the three directions of x,y,z to obtain x',y',z' respectively, and construct a loss function within the three-dimensional spatial neighborhood Ω, which can be described as:

[0058]

[0059] Calculate the unit normal vector n = [n] of point P using Principal Component Analysis (PCA). x ,n y ,n z ] TThe quadratic surface is approximated by a first-order plane ax + by + cz + d = 0, where a = n x b=n y c = n z ,use Let f(x',y',z') represent the distance from the moved point P'(x+u,y+v,z+w) to the first-order approximate plane, which can be expressed as:

[0060]

[0061] in Therefore, f(x+u,y+v,z+w) represents the distance from the moved point to the first-order approximate plane, while the loss function characterizes the first-order approximation of the distance from the moved point to the original surface. Using Taylor's formula to perform a first-order expansion of f(x+u,y+v,z+w) yields... Then the loss function can be expressed as:

[0062]

[0063]

[0064] Matrix M is the covariance matrix formed by the normal vectors of point P. The eigenvalues ​​of this covariance matrix are λ1 > λ2 > λ3. The response function R = λ3 is defined. When R is large, the area around point P is determined to be a corner region.

[0065] Next, the extracted corner information needs to be described using feature descriptors. Feature histograms are used to describe these features. The feature point histogram algorithm is based on two points: first, the feature descriptors do not change due to point cloud translation or rotation; second, the geometric information of the surrounding region can be described by the changes in the normal vectors of the region surrounding the feature point.

[0066] The specific implementation process of step (2): Let point P q For the query point. Construct a system with P as the query point. q A spherical neighborhood Ω centered at r and with radius r can be searched for k neighboring points within this region; P q Each node forms a pair with its k nearest neighbors, and these k nearest neighbors then form pairs with each other. For example, in P... q respectively with P k1 ,P k2 ,P k3 ,P k4 ,P k5 Forming point pairs, P k1 ,P k2 ,P k3 ,P k4 ,P k5 Each pair of points forms a point pair. A local coordinate system uvw is constructed within each point pair. Let P...q and P k1 Taking the pair of points as an example, let n1 and n2 represent P respectively. q and P k1 The normal vector, through the scalar triplet The difference between normal vectors n1 and n2 is described as follows:

[0067]

[0068] Assume P q If there are k nearest neighbors, then k needs to be calculated. 2 scalar triplet Build A coordinate system divides three-dimensional space into B... 3 Each small grid is considered as a 3D coordinate system and its values ​​are projected onto a 3D grid space. Then, each small grid is flattened, resulting in a final grid of length B. 3 The vector, after such operation, forms the query point P. q The feature histogram.

[0069] The Fast Point Feature Histogram (FPFH) algorithm is built upon the Simplified Point Feature Histogram (SPFH) algorithm. The SPFH algorithm first calculates the query point P within a spherical region Ω of radius r. q Triples with surrounding neighboring points Next, three histograms of length B are constructed to record α, The sum of θ and θ is finally obtained to obtain a simplified point feature histogram (SPFH) of length 3B.

[0070] Based on the above process, the FPFH algorithm steps are as follows: First, calculate the query point P. q The query point's SPFH is calculated first; then, the SPFHs of its k nearest neighbors are calculated; finally, the FPFH is the weighted average of the query point's SPFH and the SPFHs of its surrounding nearest neighbors. Ultimately, the FPFH algorithm yields a feature histogram of length 3B with three distinct peaks.

[0071]

[0072] Points with similar feature descriptions are grouped into pairs. These pairs may contain points with similar feature descriptions but incorrect matches. The following section uses a second-order spatial compatibility metric matrix to remove these incorrect matches. The principle of spatial compatibility is based on the following two facts:

[0073] First, the distance consistency of rigid body transformation means that the distance between any two points in space remains unchanged after a rigid body transformation (which only involves translation or rotation).

[0074] Secondly, the smaller the distance difference between two matching pairs, the higher their compatibility. Theoretically, the spatial distance difference between correctly matched pairs (interior points) is 0, and their first-order spatial compatibility approaches 1.

[0075] The specific implementation process of step (3): Assume matching pair C i With C j The first-order compatibility is described as follows:

[0076] SC(C i C j )=exp(d(P i ,P j )-d(Q i Q j (7)

[0077] Where d(P) i -P j ) represents P i and P j Euclidean distance between two points, d(Q) i -Q j ) represents Q i and Q j The Euclidean distance between two points. From this, a first-order spatial compatibility matrix sc is constructed, where sc... ij =sc ji When i = j, sc ij =0

[0078]

[0079] However, there are also cases where the distance difference between correct and incorrect matching pairs is close to 0, and the compatibility approaches 1. To address the ambiguity of the first-order spatial compatibility matrix, a second-order spatial compatibility matrix is ​​proposed. For example... Figure 6 As shown, firstly, by setting the partitioning threshold θ th Binarize the first-order spatial compatibility matrix to obtain SC, where SC ij =SC ji When i = j, SC ij =0.

[0080]

[0081]

[0082] Based on the above, a second-order spatial compatibility matrix is ​​constructed. If two matching pairs ij are compatible, then SCij =1, calculate the number of matching pairs that are compatible with them as a description of their second-order similarity, and obtain the second-order spatial compatibility matrix SC. 2 , of which SC 2 ij =SC 2 ji When i = j, SC 2 ij =0

[0083]

[0084]

[0085] Since inliers are mutually compatible, the similarity between any two inliers is at least the number of inliers in all matching pairs (excluding the two matching pairs themselves). However, this property does not exist between inliers and outliers.

[0086] The implementation process of step (4): After removing incorrect matching pairs, the correct matching points are obtained, and then the optimal rotation matrix R and translation vector t are solved iteratively using the least squares method. Assuming there are n pairs of matching points, the mathematical description of the optimization process is as follows:

[0087]

[0088] The implementation process of step (5): The source point cloud P' and the target point cloud after coarse registration coordinate transformation are used as input point clouds for iterative nearest point algorithm. All points p of the point cloud P' to be registered are... i In the target point cloud Q, use a K-dimensional tree to search for the nearest corresponding point q. i The corresponding point pairs are formed; the optimal transformation is calculated iteratively using the least squares method to minimize the mean square error between the corresponding point pairs; the rotation matrix R and translation vector t are solved using the singular value decomposition method; the rotation matrix and translation vector are applied to the point cloud P' to be registered; and the error threshold ε and the maximum number of iterations N are set. max Calculate the distance error between the two point cloud iterations. If the threshold condition is met or the maximum number of iterations is reached, output the final transformation matrix T.

[0089] Compared with the prior art, the beneficial effects of the present invention are: effectively suppressing the influence of external points on registration accuracy, and improving the accuracy and speed of point cloud registration.

[0090] In a specific implementation scenario, the experimental environment consisted of an Intel(R) Core(TM) i5-8250U CPU, 8.00G of RAM, Ubuntu 18.04, PCL 1.8.1 (Point Cloud Library 1.8.1), CMake compilation tool, and C++ programming language.

[0091] Detailed experimental data are shown in Table 1. The second-order spatial compatibility matrix of the initial matching pairs and the number of correctly matched pairs are shown in Table 2.

[0092] Table 1. Experimental Data Details

[0093]

[0094] Table 2 Second-order space compatibility matrix of each model

[0095]

[0096] Based on the above experimental data, the registration process of the rabbit (Bunny) model is as follows: Figure 7 As shown. Figure 7 (a) shows the results of Harris feature extraction, FPFH feature description, and matching for two point clouds. Observing the matching results of the rabbit model from a top-down perspective, it is clear that there is a pair of incorrect matches; Figure 7 (b) is the registration result without removing this erroneous matching pair, and the registration error between the two point clouds is relatively large; Figure 7 (c) Shows the matching pairs after removing outliers; Figure 7 (d) shows the registration results achieved by combining the Kd-ICP algorithm while preserving correct matching pairs. It is evident that the overlap between the two point clouds is significantly higher, effectively suppressing the influence of outliers on registration accuracy. The same experimental approach was applied to the dinosaur and horse models, and their registration processes are as follows: Figure 8 and Figure 9 As shown.

[0097] To verify the effectiveness and rationality of the method used in this study, mean square error (MSE) is introduced as an evaluation index for the quality of point cloud registration. MSE characterizes the degree of spatial proximity between two point cloud datasets; a smaller MSE indicates a higher degree of overlap and higher registration accuracy. MSE R0 MS The description is as follows, where T is the final obtained rotation and translation matrix (i.e., the final transformation matrix), and p i and q i These represent points in the source point cloud and the target point cloud, respectively.

[0098]

[0099] The algorithm is compared with experimental results of FPFH+RANSAC+ICP, FPFH+SAC-IA+ICP, and SHOT+RANSAC+ICP, such as... Figure 10 , Figure 11 and Figure 12As shown, traditional RANSAC and SAC-IA algorithms involve a large number of random sampling and model fitting operations, resulting in low computational efficiency. Furthermore, the RANSAC algorithm is limited by the number of iterations and distance thresholds; if the parameters are not set appropriately, incorrect matching pairs may not be eliminated, requiring continuous adjustment and optimization. Therefore, the algorithm presented in this paper has significant advantages in both registration time and registration accuracy.

[0100] Taking the rabbit model as an example, the algorithm in this study improves speed and accuracy by 56.88% and 70.37% respectively compared to FPFH+SAC-IA+ICP; by 46.04% and 66.15% respectively compared to FPFH+RANSAC+ICP; and by 54.16% and 68.96% respectively compared to SHOT+RANSAC+ICP. Detailed experimental data are shown in Table 3.

[0101] Table 3 Comparison of Evaluation Metrics for Each Algorithm

[0102]

[0103] It should be noted that the descriptions of each embodiment in the above embodiments have different focuses. For parts that are not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

[0104] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0105] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A method for point cloud registration based on a second order spatial compatibility measure, characterized in that, The method comprises the following steps: (1) extracting corner points of the source point cloud and the target point cloud as key points by using a Harris-3D algorithm; (2) describing and preliminarily matching the extracted key points to obtain a matching pair by using an FPFH algorithm; (3) calculating a second-order spatial compatibility matrix between the matching pair, and removing an incorrect matching pair to obtain a correct matching pair according to the second-order spatial compatibility matrix; (4) Using singular value decomposition method, combining the correct matching pairs, the coarse registration result of two point clouds is calculated to obtain the initial pose; specifically, after obtaining n pairs of correct matching pairs, the optimal rotation matrix is solved by least square method iteration and translation vector The calculation process is as follows: ； Where, q i To use K-dimensional tree search for target point cloud p i The nearest corresponding point is obtained by first treating the rotation matrix R as a constant. Then it is about the variable The function, and then the variable Find the partial derivatives and set them to zero, then solve. and R; (5) calculating a point cloud fine registration result according to an initial pose of the two point clouds, combining an ICP algorithm accelerated by a K-dimensional tree, and finally completing point cloud registration.

2. The method of registering point clouds based on a second order spatial compatibility metric of claim 1, wherein, Step (1) specifically comprises: Firstly, solving point cloud normal vectors by using a principal component analysis method; Secondly, solving a covariance matrix composed of the point cloud normal vectors; Thirdly, solving eigenvalues of the covariance matrix, and if the smallest eigenvalue is relatively large, the point is determined as a corner point.

3. The method of registering point clouds based on a second order spatial compatibility metric of claim 1, wherein, Step (2) specifically comprises: ； wherein, and are the simplified point features of the query point and the nearest neighbor point respectively.

4. The method of registering point clouds based on a second order spatial compatibility metric of claim 2, wherein, Step (3) specifically comprises: calculating Euclidean distances between the corner points constituting the matching pair in the source point cloud; calculating Euclidean distances between the corner points constituting the matching pair in the target point cloud; calculating an Euclidean distance difference between the matching pair; using a monotonically decreasing exponential function to describe a first-order spatial compatibility between the matching pair, and the smaller the Euclidean distance difference of the matching pair is, the higher the first-order spatial compatibility is; according to a set threshold, binarizing the first-order spatial compatibility matrix; calculating a number of matching pairs that are compatible with the binarized matrix as a second-order similarity description, obtaining a second-order spatial compatibility matrix, and removing a matching pair that is not compatible in the second order according to the second-order spatial compatibility matrix.

5. The method of registering point clouds based on a second order spatial compatibility metric of claim 1, wherein, Step (5) specifically comprises: First, the source point cloud after coarse registration coordinate transformation and the target point cloud as input point clouds to perform an iterative closest point algorithm to match the point clouds All points in the target point cloud use a K-dimensional tree to search for the nearest corresponding point to form a corresponding point pair; Second, the optimal transformation is calculated by least square method, which makes the mean square error between the corresponding points minimum, and the rotation matrix is solved by singular value decomposition method and translation vector ; Thirdly, the rotation matrix and the translation vector are applied to the point cloud to be registered , while setting an error threshold and a maximum number of iterations , the distance error of the two point clouds is calculated, and if the threshold condition is met or the maximum number of iterations is reached, the final transformation matrix is output.

6. The method of registering point clouds based on a second order spatial compatibility metric of claim 5, wherein, After step (5), the following steps are further included: To verify the effectiveness and rationality of the proposed method, the mean square error is introduced as an evaluation index of point cloud registration, and the mean square error Is described as follows: ； wherein, is the final transformation matrix, and represent points in the source and target point clouds, respectively, and N is the total number of points.

Images