Point cloud registration method based on feature point normal and global point cloud curvature optimization

Abstract

The application discloses a point cloud registration method based on feature point normal and global point cloud curvature optimization, and comprises the following steps: extracting surface key points from target point cloud and reference point cloud, and searching and constructing one-to-one corresponding feature point pairs by using an FPFH descriptor; constructing a comprehensive elimination condition based on distance difference, normal vector angle difference and curvature difference adaptive threshold based on normal distribution, and eliminating outlier pairs in the feature point pairs; constructing a Mahalanobis distance error metric containing a normal vector constraint by using the remaining feature point pairs, and realizing coarse alignment of a pose after optimizing the Mahalanobis distance error metric; and realizing fine registration of point clouds by cross iteration based on the pose coarse alignment algorithm and the improved ICP algorithm with the comprehensive elimination condition. The method is helpful to improve the convergence speed and accuracy of point cloud registration. Various experiments prove that the method has high tolerance to the initial position difference of point clouds, and has obvious inhibition effect on the local minimum phenomenon in the iteration process.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of machine vision point cloud registration, and particularly relates to a point cloud registration method based on feature point normal and global point cloud curvature optimization. BACKGROUND

[0002] Point cloud, as a description method of object surface contour, can well represent the geometric features and spatial positions of the target. With the rapid development of three-dimensional vision, point cloud is widely used in reverse engineering, robotics and automatic driving fields. In the process of actually obtaining point cloud data, due to factors such as equipment and environment, only the local area of the object to be measured can be scanned by a single sensor or a single scan, so it is necessary to scan at different angles and then register the multiple point clouds scanned at different angles to restore the complete appearance of the object to be measured. Point cloud registration is a process of unifying two sets of point clouds to the same reference coordinate system by solving the transformation matrix between the two sets of corresponding point clouds. The existing point cloud registration algorithm usually cannot solve the problem of easily falling into local optimum in the case of non-ideal initial pose, and the disorder in the registration iteration process leads to the problems of low registration speed and efficiency, so the efficient and fast point cloud registration algorithm has always been a hot research topic for scholars.

[0003] Point cloud registration has been widely studied. In the existing methods, the development of registration methods can generally be divided into two categories: feature-based and optimization-based registration algorithms. Feature-based registration algorithms mainly focus on neighborhood information (such as points, lines or surfaces in point cloud). Feature-based registration is a three-dimensional extension of image matching, which includes two main steps: feature matching and geometric estimation.

[0004] In feature matching, 3D feature keypoints are first detected from point cloud by a feature detector, such as internal shape feature (ISS), KeypointNet and USIP. Then, each keypoint is encoded into a compact feature vector through local shape analysis based on descriptors. Finally, the corresponding point pair relationship is established by calculating pairwise similarity scores and chi-square test. In geometric estimation, the main task is to estimate the six degrees of freedom (DoF) rigid transformation based on robust fitting techniques.

[0005] Most of the existing optimization-based registration methods are achieved by minimizing the geometric projection error through two processes: correspondence search and transformation estimation. Typical optimization-based methods include ICP variant-based, graph model-based, probability model-based and semi-definite registration methods.

[0006] At present, there are many methods to realize the registration of 3D point cloud, in general, most point cloud registration methods adopt the strategy from coarse to fine, and are mainly divided into coarse registration based on local features and fine registration algorithm of iterative optimization according to the scale. As a method for obtaining the optimal solution by using constraint iteration, the ICP algorithm (Iterative Closest Point algorithm) is the most widely used and mature algorithm at present. The standard ICP algorithm establishes the distance error measurement of the nearest point pair by the Euclidean distance, minimizes to solve the new relative transformation, and after continuous iteration, a registration result with high accuracy can be obtained. However, the good operation of the ICP algorithm requires that the reference point cloud and the target point cloud have an approximate initial position close to each other; at the same time, due to the disorder of the direction in the iteration process, the iterative error is easy to fall into a local minimum value, which leads to slow convergence speed and finally reduces the registration accuracy. SUMMARY

[0007] In order to solve the technical problem that the iterative error of the ICP algorithm is easy to fall into a local minimum value, the application provides a point cloud registration method based on feature point normal and global point cloud curvature optimization, which specifically comprises the following steps:

[0008] S1: extracting surface key points from the target point cloud and the reference point cloud respectively, searching and constructing a one-to-one corresponding feature point pair by using the FPFH descriptor;

[0009] S2: constructing a comprehensive elimination condition based on distance difference, normal vector angle difference and curvature difference adaptive threshold based on normal distribution, and eliminating the outlier pairs in the feature point pairs by using the comprehensive elimination condition;

[0010] S3: constructing a Mahalanobis distance error measurement containing a normal vector constraint by using the remaining feature point pairs, and realizing the coarse alignment of the pose after optimizing the Mahalanobis distance error measurement;

[0011] S4: realizing the fine registration of the point cloud by cross iteration based on the pose coarse alignment algorithm and the improved ICP algorithm with the comprehensive elimination condition.

[0012] Further, step S1 specifically comprises:

[0013] S11: extracting the surface key points of the reference point cloud and the target point cloud as candidate points by using the ISS algorithm, setting a threshold condition, and selecting the candidate points meeting the threshold condition as the key points;

[0014] S12: constructing the FPFH descriptor for the key points extracted from the reference point cloud and the target point cloud, and the FPFH descriptor is a 33-dimensional feature vector;

[0015] S13: For the key point FPFH descriptor in the reference point cloud, a 33-dimensional KD tree is used to search the FPFH descriptor of the key point in the target point cloud in space to find the most similar corresponding point, and a feature point pair is formed.

[0016] Further, step S11 specifically comprises:

[0017] S111: For each candidate point p i , query all points p j within the neighborhood radius , and calculate the weight w ij ;

[0018] S112: Calculate the covariance matrix cov(p i ) of each candidate point p i in combination with the weight:

[0019]

[0020] S113: Calculate the eigenvalues {λ1, λ2, λ3} of the covariance matrix of each candidate point p i , sort them in order of size, set a threshold condition according to the change relationship of the eigenvalues, and select the candidate points that meet the threshold condition as key points; the threshold condition is:

[0021]

[0022] In the formula, ε1 and ε2 represent the first threshold and the second threshold respectively.

[0023] Further, step S2 specifically comprises:

[0024] S21: Take the Euclidean distance between the transformed feature points in the reference point cloud and the corresponding feature points in the target point cloud as the calculated distance difference value;

[0025] S22: Take the angle between the normal vectors of the transformed feature points in the reference point cloud and the corresponding feature points in the target point cloud as the normal vector angle difference value;

[0026] S23: Take the logarithmic difference between the curvatures of the transformed feature points in the reference point cloud and the corresponding feature points in the target point cloud as the curvature logarithmic difference value;

[0027] S24: Calculate the distance difference, normal vector angle difference, and curvature difference of the feature point pairs obtained in S21, S22, and S23. Set a threshold interval to approximately represent the interval of correct point pairs. Perform normal distribution fitting to obtain the mean and standard deviation of the distribution. The threshold interval is selected by the Wilson confidence interval. The final calculation of the three types of confidence threshold intervals is The expression is:

[0028]

[0029]

[0030]

[0031] where distance, normal, and curvature represent the three types of threshold intervals for distance difference, normal vector angle difference, and curvature difference, respectively. M represents the known transformation matrix, represent the corresponding points on the reference point cloud and the target point cloud, respectively, represent the corresponding normal vectors of the corresponding points, represent the curvature of the neighborhood at the corresponding points, and N represents the dimension size of the corresponding transformation, represents the product of the transformation matrix;

[0032] S25: Use the distance difference, normal vector angle difference, and curvature difference threshold intervals calculated in S24. If the distance difference, normal vector angle difference, and curvature difference of the feature point pairs are all within the three types of threshold intervals, the feature point pairs are retained. Otherwise, they are removed as outliers.

[0033] Further, step S3 specifically includes:

[0034] S31: According to the feature point pairs retained in S2, a rough alignment Mahalanobis distance error metric function is constructed. The expression of the Mahalanobis distance error metric function E is as follows:

[0035]

[0036] where p s is the retained feature point in the reference point cloud, p t is the corresponding feature point retained in the target point cloud, and the feature points p s and p t contain 6 dimensions, ∑ st is the covariance matrix between the transformed reference point cloud and the target point cloud, where among the 6 dimensions: x i , y i , and z irespectively represent the coordinate values of the three dimensions x, y, z of the point cloud, respectively represent the values of the three dimensions x, y, z of the normal vector of the point cloud;

[0037] S32: The Mahalanobis distance error metric function E is solved by using the LM optimization algorithm (Levenberg-Marquardt) to obtain the transformation matrix M of the coarse alignment.

[0038] Further, step S32 specifically includes:

[0039] S321: The incremental equation of the Mahalanobis distance error metric function is calculated

[0040]

[0041] wherein the expression for calculating the Jacobian matrix is The tail term is ΔM k respectively represent the increment of the transformation matrix and the error metric E in the coarse alignment algorithm, I represents the unit matrix, and μ is the damping factor;

[0042] S322: The damping factor is updated according to the scale factor ρ, and in each iteration:

[0043]

[0044]

[0045] wherein M k represents the transformation matrix at the kth time, the numerator represents the size of the actual increment, and the denominator uses the size of the increment generated by the approximate model; the damping factor μ is determined by the scale factor, the parameter v represents the change trend parameter, and when the scale factor ρ is greater than 0, the parameter v is defined as 2, otherwise it is defined as 2v;

[0046] S323: Iterative update is performed until the obtained is less than a set third threshold value, at which time the coarse alignment transformation matrix M converges.

[0047] Further, step S4 specifically includes:

[0048] S41: The reference point cloud is transformed according to the coarse alignment transformation matrix M obtained in S32 to obtain an updated point cloud after alignment;

[0049] S42: Corresponding point pairs on the updated point cloud and the target point cloud are obtained, after outlier rejection, a constraint function is constructed, and a fine registration result M' is obtained by nonlinear optimization of the constraint function;

[0050] S43: cross iteration is performed by using the pose coarse alignment algorithm and the improved ICP algorithm; in the cross iteration process, when the pose coarse alignment algorithm converges to a certain degree, the convergence precision cannot be further improved, at this time, the registration error has a fourth threshold value, when the registration error is less than the fourth threshold value, the pose coarse alignment algorithm stops, and only the improved ICP algorithm is further iterated until convergence;

[0051] S44: the final transformation matrix M is obtained through cross iteration total , and the reference point cloud is transformed by using the final transformation matrix to obtain the registered point cloud.

[0052] Further, step S42 specifically comprises:

[0053] S421: for each point in the updated point cloud, a corresponding point in the target point cloud is obtained through KD tree search to form a corresponding point pair;

[0054] S422: all corresponding point pairs are filtered by means of the adaptive comprehensive rejection condition, and the outlier point pairs are removed, and the accurate corresponding point pairs are reserved;

[0055] S423: the remaining corresponding point pairs are used to construct a constraint function, and the minimum of the post-transformation matrix is M ps :

[0056]

[0057] Wherein, R k and T k are the rotation matrix and the translation matrix of the transformation process, s i and t i respectively represent a corresponding point on the updated point cloud and the target point cloud, and are normal vectors of the corresponding point, represents a three-dimensional rotation space, belongs to a three-dimensional translation space;

[0058] S424: parameterized rotation is performed by using the Rodriguez rotation formula, and the equation is simplified by using a small rotation increment:

[0059]

[0060] Wherein, s i and t i respectively represent a point on the updated point cloud and a corresponding point on the target point cloud, and are normal vectors of the corresponding point, and v' represents a rotation vector at this time;

[0061] S425: R k and Tk to obtain the fine registration transformation matrix M'.

[0062] In addition, in order to realize the registration method, the application further provides a point cloud registration device based on feature point normal and global point cloud curvature optimization, comprising the following modules:

[0063] The feature point pair extraction module is used for extracting surface key points from the target point cloud and the reference point cloud respectively, and searching and constructing one-to-one corresponding feature point pairs by using the FPFH descriptor;

[0064] The outlier adaptive elimination module is used for constructing a comprehensive elimination condition based on adaptive thresholds of distance difference, normal vector angle difference and curvature difference according to normal distribution, and eliminating outlier pairs in the feature point pairs by using the comprehensive elimination condition;

[0065] The Mahalanobis constraint coarse alignment module is used for constructing a Mahalanobis distance error metric containing a normal vector constraint by using the remaining feature point pairs, and realizing coarse alignment of the pose after optimization of the Mahalanobis distance error metric;

[0066] The cross iteration module is used for realizing fine registration of the point clouds by cross iteration according to combination of the pose coarse alignment algorithm and the improved ICP algorithm with the comprehensive elimination condition.

[0067] In addition, in order to realize the registration method, the application further provides an electronic device comprising a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the processor realizes the steps of the point cloud registration method when executing the program.

[0068] The technical scheme provided by the application has the following beneficial effects:

[0069] In the present application, a point cloud registration method based on feature point normal and global point cloud curvature optimization is proposed. First, the feature points on the surface of the point cloud are extracted, and their normal vectors are calculated. Then, the Mahalanobis distance is used to construct a six-dimensional feature error measurement constraint containing the normal vector, which is used for rough alignment of the pose. At the same time, a new adaptive threshold comprehensive rejection condition is proposed, including the distance difference, the normal vector angle difference and the curvature difference of the point pair, which can significantly improve the accuracy of the point cloud pair by removing the noise interference of outliers. Then the pose rough alignment algorithm is combined with the improved ICP algorithm with the comprehensive rejection condition to cross iterate, which helps to improve the convergence speed and accuracy of the point cloud registration. Various experiments show that the algorithm has high tolerance to the initial position difference of the point cloud, and has obvious inhibition effect on the local minimum phenomenon in the iteration process. Compared with the traditional method, even when dealing with samples with poor initial pose of point cloud, the algorithm of the present application can achieve faster convergence speed and higher final convergence accuracy. In addition, the algorithm of the present application has satisfactory robustness and shows good application potential in the point cloud registration system. BRIEF DESCRIPTION OF DRAWINGS

[0070] The present application will be further described below in conjunction with the drawings and examples, wherein:

[0071] Figure 1 is a flowchart of a point cloud registration method based on feature point normal and global point cloud curvature optimization of the present application;

[0072] Figure 2 is a schematic diagram of rough alignment of pose based on feature point pair to construct Mahalanobis distance error measurement containing normal vector constraint in the present application;

[0073] Figure 3 is a comprehensive rejection condition based on adaptive threshold of distance difference value, normal vector angle difference value and curvature difference value based on normal distribution in the present application;

[0074] Figure 4 is a schematic diagram of improved ICP algorithm considering target point cloud surface constraint and outlier point pair rejection in the present application;

[0075] Figure 5 is a structural schematic diagram of a point cloud registration device based on feature point normal and global point cloud curvature optimization in the present application. DETAILED DESCRIPTION

[0076] In order to have a clearer understanding of the technical features, objects and effects of the present application, the specific embodiments of the present application will be described in detail with reference to the drawings.

[0077] REFERENCE Figure 1 The embodiment of the present application provides a point cloud registration method based on feature point normal and global point cloud curvature optimization, which specifically comprises the following steps:

[0078] S1: extracting surface key points from the target point cloud and the reference point cloud respectively, searching and constructing one-to-one corresponding feature point pairs by using the FPFH descriptor;

[0079] Step S1 specifically comprises:

[0080] S11: extracting surface key points of the reference point cloud and the target point cloud as candidate points by using the ISS algorithm, setting a threshold condition, and selecting the candidate points satisfying the threshold condition as key points;

[0081] Step S11 specifically comprises:

[0082] S111: for each candidate point p i , querying all points p j within a neighborhood radius of the candidate point p i , and calculating the weight w ij of the points p j ;

[0083] S112: calculating the covariance matrix cov(p i ) of each candidate point p i in combination with the weight:

[0084]

[0085] S113: calculating the eigenvalues {λ1, λ2, λ3} of the covariance matrix of each candidate point p i , sorting them in order of size, setting a threshold condition according to the change relationship of the eigenvalues, and selecting the candidate points satisfying the threshold condition as key points; the threshold condition is:

[0086]

[0087] In the formula, ε1 and ε2 respectively represent the first threshold and the second threshold, and the first threshold and the second threshold are generally selected to be between 0.5 and 1 according to the required number of feature point pairs.

[0088] S12: constructing the FPFH descriptor for the extracted key points in the reference point cloud and the target point cloud, the FPFH descriptor being a 33-dimensional feature vector;

[0089] S13: referring to Figure 2 , for the key point FPFH descriptor in the reference point cloud, using the 33-dimensional KD tree to perform spatial search on the key point FPFH descriptor in the target point cloud, finding the most similar corresponding point by key point Kd-tree search, constructing a feature point pair, and subsequently realizing the alignment and coincidence of the reference point cloud and the target point cloud through coarse alignment.

[0090] S2: referring to Figure 3, based on the normal distribution, a comprehensive elimination condition based on distance difference, normal vector angle difference and curvature difference adaptive threshold is constructed, and the outlier pairs in the feature point pairs are eliminated by using the comprehensive elimination condition;

[0091] S21: the Euclidean distance between the feature points in the reference point cloud after transformation and the corresponding feature points in the target point cloud is taken as the calculated distance difference value;

[0092] S22: the angle between the normal vectors of the feature points in the reference point cloud after transformation and the corresponding feature points in the target point cloud is taken as the normal vector angle difference value;

[0093] S23: the logarithmic difference between the curvatures of the feature points in the reference point cloud after transformation and the corresponding feature points in the target point cloud is taken as the curvature logarithmic difference value;

[0094] S24: the distance difference value, normal vector angle difference value and curvature logarithmic difference value of the feature point pairs obtained in S21, S22 and S23 are calculated, a threshold interval is set to approximately represent the interval of correct point pairs, and the mean and standard deviation of the distribution are obtained by normal distribution fitting, the threshold interval is selected by Wilson confidence interval, and reference Figure 3 , wherein 3(a), 3(b) and 3(c) respectively correspond to the error distribution diagrams of distance difference value, normal vector angle difference and curvature difference value, and the dotted line is the normal distribution curve fitted. The three types of confidence threshold intervals finally calculated are The expression is:

[0095]

[0096]

[0097]

[0098] In the formula, distance, normal and curvature represent three types of threshold intervals of distance difference value, normal vector angle difference value and curvature difference value respectively, M represents a known transformation matrix, respectively represent the corresponding points on the reference point cloud and the target point cloud, respectively represent the corresponding normal vectors of the corresponding points, respectively represent the curvature of the corresponding points, and N represents the dimension size of the corresponding transformation, represents the product of the transformation matrix;

[0099] S25: using the distance difference value, the normal vector angle difference value, and the curvature difference value calculated in S24 as three types of threshold intervals, if the distance difference value, the normal vector angle difference value, and the curvature difference value of the feature point pair are simultaneously located in the three types of threshold intervals, the feature point pair is retained; otherwise, the feature point pair is removed as an outlier.

[0100] S3: referring to Figure 2 , using the retained feature point pairs to construct a Mahalanobis distance error metric containing a normal vector constraint, and optimizing the Mahalanobis distance error metric to achieve coarse alignment of the pose;

[0101] Step S3 specifically includes:

[0102] S31: constructing a Mahalanobis distance error metric function for coarse alignment according to the feature point pairs retained in S2, and the expression of the Mahalanobis distance error metric function E is as follows:

[0103]

[0104] wherein p s is a retained feature point in the reference point cloud, p t is a corresponding feature point retained in the target point cloud, the feature points p s and p t contain 6 dimensions, ∑ st is a covariance matrix between the reference point cloud after transformation and the target point cloud, and in the 6 dimensions: x i , y i , and z i respectively represent coordinate values of three dimensions xyz of the point cloud, respectively represent values of normal vectors of three dimensions xyz of the point cloud;

[0105] S32: solving the Mahalanobis distance error metric function E by using an LM optimization algorithm to obtain a transformation matrix M for coarse alignment.

[0106] Step S32 specifically includes:

[0107] S321: calculating the incremental equation of the Mahalanobis distance error metric function

[0108]

[0109] wherein the expression for calculating the Jacobian matrix is the tail term is ΔM k respectively represent the increment of the transformation matrix and the error metric E in the coarse alignment algorithm, I represents a unit matrix, and μ is a damping factor;

[0110] S322: Update the damping factor based on the scaling factor ρ in each iteration:

[0111]

[0112]

[0113] Among them, M k Let represent the transformation matrix of the kth time. The numerator represents the actual increment, and the denominator represents the increment generated by the approximate model. The damping factor μ is determined by the scaling factor. The parameter ν represents the trend parameter. When the scaling factor ρ is greater than 0, the parameter ν is defined as 2. Otherwise, it is defined as 2ν.

[0114] S323: Iterative update until the desired result is obtained. Less than the set third threshold, which can be 10. -8 At this point, we obtain the convergent coarse alignment transformation matrix M.

[0115] S4: Fine registration of point clouds is achieved by combining the pose coarse alignment algorithm with the improved ICP algorithm with the addition of comprehensive culling conditions through cross-iteration.

[0116] Step S4 specifically includes:

[0117] S41: Transform the reference point cloud according to the coarse alignment transformation matrix M obtained in S32 to obtain the aligned updated point cloud;

[0118] S42: Reference Figure 4 The corresponding point pairs on the updated point cloud and the target point cloud are obtained. After removing outliers, a constraint function is constructed, and the constraint function is nonlinearly optimized to obtain a fine registration result M′.

[0119] Step S42 specifically includes:

[0120] S421: For each point in the updated point cloud, the corresponding point in the target point cloud is obtained through KD-tree search, forming a corresponding point pair, such as... Figure 4 As shown, Figure 4 In the diagram, s1 and t1 are a pair of corresponding points;

[0121] S422: Using the aforementioned adaptive comprehensive elimination condition, all corresponding point pairs are filtered to remove outlier point pairs and retain accurate corresponding point pairs;

[0122] S423: Construct constraint functions using the retained corresponding point pairs, and minimize the transformation matrix to M. ps :

[0123]

[0124] Among them, R k With Tk is a rotation matrix and t i is a translation matrix, i and t is a normal vector of the corresponding point, represents a three-dimensional rotation space, belongs to a three-dimensional translation space, such as Figure 4 As shown, the improved ICP includes a constraint relationship, which approximates a micro-surface in the target point cloud as a circular curve, and calculates the normal vector of the target point so that the convergence is in the convergence domain near the curved surface;

[0125] S424: parameterized rotation using the Rodrigues rotation formula, and simplifies the equation by rotating a small increment:

[0126]

[0127] where s i and t i represent a point on the updated point cloud and a corresponding point on the target point cloud, respectively, and are normal vectors of the corresponding point, and v' represents the rotation vector at this time;

[0128] S425: solving to obtain the corresponding R k and T k , to obtain the fine registration transformation matrix M'.

[0129] S43: cross iteration using the pose coarse alignment algorithm and the improved ICP algorithm in S42, and in the cross iteration process, when the pose coarse alignment algorithm converges to a certain extent, its convergence accuracy cannot be further improved, at this time the registration error exists A fourth threshold, less than the fourth threshold (the threshold is generally selected according to the size of the point cloud), the coarse alignment algorithm stops, and only the improved ICP algorithm is further iterated until convergence;

[0130] S44: cross iteration to obtain the final transformation matrix M total , and using the final transformation matrix to transform the reference point cloud to obtain the registered point cloud.

[0131] As an optional implementation, the application also provides a point cloud registration device based on feature point normal and global point cloud curvature optimization, referring to Figure 5 The device includes the following modules:

[0132] A feature point pair extraction module 1 is used to extract surface key points from the target point cloud and the reference point cloud respectively, and to search and construct a one-to-one corresponding feature point pair using the FPFH descriptor; ​

[0133] The outlier adaptive elimination module 2 is used to construct a comprehensive elimination condition based on distance difference, normal vector angle difference and curvature difference adaptive threshold according to normal distribution, and to eliminate outlier pairs in the feature point pairs by using the comprehensive elimination condition.

[0134] The Mahalanobis constraint coarse alignment module 3 is used to construct a Mahalanobis distance error metric containing a normal vector constraint by using the remaining feature point pairs, and to realize coarse alignment of the pose after optimization of the Mahalanobis distance error metric.

[0135] The cross iteration module 4 is used to realize fine registration of the point clouds by cross iteration of the pose coarse alignment algorithm and the improved ICP algorithm with the comprehensive elimination condition.

[0136] As an optional implementation, the application further provides an electronic device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor realizes the steps of the point cloud registration method when executing the program, and the steps specifically comprise: extracting surface key points from a target point cloud and a reference point cloud respectively, and constructing one-to-one corresponding feature point pairs by using an FPFH descriptor search; constructing a comprehensive elimination condition based on distance difference, normal vector angle difference and curvature difference adaptive threshold based on normal distribution, and eliminating outlier pairs in the feature point pairs by using the comprehensive elimination condition; constructing a Mahalanobis distance error metric containing a normal vector constraint by using the remaining feature point pairs, and realizing coarse alignment of the pose after optimization of the Mahalanobis distance error metric; and realizing fine registration of the point clouds by cross iteration of the pose coarse alignment algorithm and the improved ICP algorithm with the comprehensive elimination condition.

[0137] It should be noted that, in this document, the terms "comprising", "containing", or any other similar term are intended to encompass non-exclusive inclusion, such that a process, method, article or system that comprises a list of elements does not necessarily include only those elements in the list, but can include other elements not expressly listed or inherent to such process, method, article or system. Without further limitation, an element preceded by "comprises a" does not, without more constraints, foreclose the existence of additional identical elements in the process, method, article or system that includes the element.

[0138] The above-mentioned embodiment numbers of the application are only for description, and do not represent the advantages and disadvantages of the embodiments. In the unit claims in which several devices are listed, several of the devices can be embodied by the same hardware item. The use of the words first, second and third does not represent any order, and these words can be interpreted as identifiers.

[0139] The above merely describes the preferred embodiments of the present application, and is not intended to limit the patent scope of the present application, and any equivalent structure or equivalent process conversion, or direct or indirect application in other related technical fields, which are made by using the content of the present application specification and drawings, are also included in the patent protection scope of the present application.

Claims

1. A point cloud registration method based on feature point normals and global point cloud curvature optimization, characterized in that, Includes the following steps: S1: Extract surface key points from the target point cloud and the reference point cloud respectively, and construct one-to-one feature point pairs using FPFH descriptor search; S2: Based on the normal distribution, a comprehensive elimination condition is constructed based on the adaptive threshold of distance difference, normal vector angle difference and curvature difference. The comprehensive elimination condition is used to eliminate outlier pairs in the feature point pairs. S3: Construct a Mahalanobis distance error metric with normal vector constraints using the retained feature point pairs, and optimize the Mahalanobis distance error metric to achieve coarse alignment of the pose. S4: Based on the pose coarse alignment algorithm and the improved ICP algorithm with the addition of the comprehensive culling condition, cross-iteration is used to achieve fine registration of point clouds; Step S2 specifically includes: S21: The Euclidean distance between the transformed feature points in the reference point cloud and the corresponding feature points in the target point cloud is used as the calculated distance difference. S22: The angle between the normal vectors of the feature points in the reference point cloud and the corresponding feature points in the target point cloud after transformation is taken as the normal vector angle difference. S23: The logarithmic difference between the curvature of the feature points in the reference point cloud after transformation and the corresponding feature points in the target point cloud is taken as the logarithmic difference of curvature. S24: Calculate the distribution of the three data items for feature point pairs obtained in S21, S22, and S23: distance difference, normal vector angle difference, and logarithmic curvature difference. Set a threshold interval to approximate the interval where correct point pairs are located. Perform normal distribution fitting to obtain the mean and standard deviation of the distribution. The threshold interval is selected from a standard interval by Wilson confidence interval. The three types of confidence threshold intervals are finally calculated as follows: Its expression is: In the formula, distance, normal, and curvature represent the three threshold intervals for distance difference, normal vector angle difference, and curvature difference, respectively, and M represents the known transformation matrix. These represent the corresponding points on the reference point cloud and the target point cloud, respectively. These represent the corresponding normal vectors of the corresponding points. Let represent the curvature of the neighborhood at the corresponding point, and N represent the dimension of the corresponding transformation. Represents the product of transformation matrices; S25: Using the three threshold intervals of distance difference, normal vector angle difference, and curvature difference calculated in S24, if the distance difference, normal vector angle difference, and curvature difference of a feature point pair are all within the three threshold intervals, then the feature point pair is retained; otherwise, it is removed as an outlier.

2. The point cloud registration method according to claim 1, characterized in that, Step S1 specifically includes: S11: Use the ISS algorithm to extract surface key points of the reference point cloud and the target point cloud as candidate points, set threshold conditions, and select candidate points that meet the threshold conditions as key points. S12: Construct FPFH descriptors for key points extracted from the reference point cloud and the target point cloud. The FPFH descriptors are 33-dimensional feature vectors. S13: For the FPFH descriptors of key points in the reference point cloud, a 33-dimensional KD tree is used to perform a spatial search on the FPFH descriptors of key points in the target point cloud to find the most similar corresponding points and form feature point pairs.

3. The point cloud registration method according to claim 2, characterized in that, Step S11 specifically includes: S111: For each candidate point p i Query the neighborhood radius All points p inside j Calculate its weight w ij ; S112: Calculate the weight of each candidate point p i cov(p i ): Covariance matrix of p S113: Calculate p for each candidate point i The eigenvalues ​​{λ1, λ2, λ3} of the covariance matrix are sorted in order of magnitude. A threshold condition is set based on the variation of the eigenvalues, and candidate points that meet the threshold condition are selected as key points. for: In the formula, ε1 and ε2 represent the first threshold and the second threshold, respectively.

4. The point cloud registration method according to claim 1, characterized in that, Step S3 specifically includes: S31: Based on the feature point pairs retained in S2, construct a coarsely aligned Mahalanobis distance error metric function. The expression for the Mahalanobis distance error metric function E is as follows: Where, p s p is the feature point preserved in the reference point cloud. t The corresponding feature points retained in the target point cloud, feature point p s and p t It includes 6 dimensions. ∑ st The covariance matrix between the transformed reference point cloud and the target point cloud is given by the following six dimensions: x i y i , z i These represent the coordinate values ​​of the point cloud in the three dimensions x, y, and z, respectively. These represent the values ​​of the three dimensions (x, y, z) of the point cloud normal vector; S32: The Mahalanobis distance error metric function E is solved using the LM optimization algorithm to obtain the coarse alignment transformation matrix M.

5. The point cloud registration method according to claim 4, characterized in that, Step S32 specifically includes: S321: Calculate the incremental equation of the Mahalanobis distance error metric function. The expression for calculating the Jacobian matrix is ​​as follows: Tail item is ΔM k Let I represent the transformation matrix and the increment of the error metric E in the coarse alignment algorithm, respectively, where I represents the identity matrix and μ is the damping factor. S322: Update the damping factor based on the scaling factor ρ in each iteration: where M k denotes the k-th transformation matrix, the numerator denotes the actual increment size, and the denominator uses the increment size generated by the approximation model; the damping factor μ is determined by the scaling factor, and the parameter v represents the change trend parameter. When the scaling factor p is greater than 0, the parameter v is defined as 2, otherwise, it is defined as 2v; S323: Iterative update until the desired result is obtained. If the value is less than the set third threshold, then a convergent coarse alignment transformation matrix M is obtained.

6. The point cloud registration method according to claim 4, characterized in that, Step S4 specifically includes: S41: Transform the reference point cloud according to the coarse alignment transformation matrix M obtained in S32 to obtain the aligned updated point cloud; S42: Obtain the corresponding point pairs on the updated point cloud and the target point cloud, remove outliers, construct a constraint function, and perform nonlinear optimization on the constraint function to obtain a refined registration result M′; S43: The pose coarse alignment algorithm and the improved ICP algorithm are cross-iterated. During the cross-iteration process, when the pose coarse alignment algorithm converges to a certain extent, its convergence accuracy cannot be improved further. At this time, there is a fourth threshold for the registration error. When it is less than the fourth threshold, the pose coarse alignment algorithm stops, and only the improved ICP algorithm continues to iterate until convergence. S44: cross iteration to obtain the final transformation matrix M total , and the reference point cloud is transformed by using the final transformation matrix to obtain the registered point cloud.

7. The point cloud registration method according to claim 6, characterized in that, Step S42 specifically includes: S421: For each point in the updated point cloud, the corresponding point in the target point cloud is obtained by KD tree search, forming a corresponding point pair; S422: Using adaptive synthesis elimination conditions, all corresponding point pairs are filtered to remove outlier point pairs and retain accurate corresponding point pairs; S423: construct constraint function with the remaining pairs of corresponding points, minimize the post-transformation matrix to M ps : Among them, R k With T k Let s be the rotation and translation matrices for the transformation process. i With t i These represent the previous corresponding point in the updated point cloud and the target point cloud, respectively. and Let be the normal vector of the corresponding point. Represents a three-dimensional rotational space. It belongs to a three-dimensional translation space; S424: Parametric rotation is performed using the Rodriguez rotation formula, and the equations are simplified by small rotation increments: Among them, s i and t i These represent updating a point in the point cloud and the corresponding point in the target point cloud, respectively. and These are the normal vectors of the corresponding points, and v′ represents the rotation vector at this point. S425: Solve to obtain the corresponding R k With T k We obtain the fine registration transformation matrix M′.

8. A point cloud registration device based on feature point normals and global point cloud curvature optimization, used to implement the steps of the point cloud registration method as described in any one of claims 1 to 7, characterized in that, Includes the following modules: The feature point pair extraction module is used to extract surface key points from the target point cloud and the reference point cloud respectively, and to construct one-to-one corresponding feature point pairs using FPFH descriptor search. The outlier adaptive removal module is used to construct a comprehensive removal condition based on the distance difference, normal vector angle difference and curvature difference adaptive threshold according to the normal distribution, and to remove outlier pairs in the feature point pairs using the comprehensive removal condition; The Mahalanobis constraint coarse alignment module is used to construct a Mahalanobis distance error metric containing normal vector constraints using the retained feature point pairs, and to achieve coarse alignment of pose after optimizing the Mahalanobis distance error metric. The cross-iteration module is used to achieve fine registration of point clouds by combining the pose coarse alignment algorithm with the improved ICP algorithm with the comprehensive culling condition.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the point cloud registration method as described in any one of claims 1 to 7.

Images