CN119006543A

CN119006543A - Point cloud registration method based on neighborhood normal vector and curvature

Info

Publication number: CN119006543A
Application number: CN202411097504.XA
Authority: CN
Inventors: 宋成航; 曹镭; 郑涛; 马鑫海; 姚永忠
Original assignee: Jiangsu Kuangbo Intelligent Technology Co ltd
Current assignee: Jiangsu Kuangbo Intelligent Technology Co ltd
Priority date: 2024-08-12
Filing date: 2024-08-12
Publication date: 2024-11-22

Abstract

The invention discloses a point cloud registration method based on a neighborhood normal vector and curvature, which comprises the following steps: preprocessing source point cloud and target point cloud data; constructing a constraint condition of the number of adjacent points for the preprocessed source point cloud and the preprocessed target point cloud according to the degree of the density between the points; extracting initial characteristic points from the preprocessed point cloud according to the normal vector inner product mean value between the sampling points and the adjacent points; extracting the initial characteristic points for the second time through the average curvature of the neighborhood to form characteristic points; forming a final feature point set according to the curvature feature parameter; performing FPFH feature description on the final feature point set, performing initial feature matching according to the ratio of the bidirectional minimum Euclidean distance between feature vectors and the secondary Euclidean distance, and adopting the Euclidean distance constraint between point pairs to remove mismatching point pairs; and calculating initial values of initial transformation parameters by taking the correct matching point pairs as unit quaternions, carrying out initial registration on the source point cloud and the target point cloud, constructing a bidirectional k-dimensional tree improved ICP algorithm, and finishing accurate registration.

Description

Point cloud registration method based on neighborhood normal vector and curvature

Technical Field

The invention relates to the technical field of three-dimensional point cloud registration, in particular to a point cloud registration method based on a neighborhood normal vector and curvature.

Background

With the rapid development of three-dimensional laser scanning technology, measurement methods and technologies are increasingly advanced. The spatial coordinate information (x, y, z) obtained by measuring the surface of the object to be measured is generally referred to as three-dimensional point cloud data. As the point cloud data in practical application is in a mass trend, the problems of noise interference, incomplete data, uneven distribution of the point cloud and the like are accompanied. Therefore, in order to ensure accuracy and robustness of the model, the large-scale point cloud data needs to be preprocessed before modeling. The point cloud preprocessing mainly comprises denoising, filtering, registering, segmentation and the like, wherein the point cloud registering is a core process, and the registering precision of the point cloud registering directly influences the reconstruction effect of the follow-up entity three-dimensional model.

The traditional ICP algorithm is used for searching the closest point by calculating the Euclidean distance between two point clouds, establishing a corresponding relation and solving ideal transformation parameters of an objective function, but the algorithm has the problems of low registration efficiency, poor precision, high requirement on the initial pose of the point clouds and the like. Therefore, relevant scholars at home and abroad propose a large number of improvement methods, and four main steps of the ICP algorithm are summarized: 1) Extracting feature points; 2) Describing characteristics; 3) Feature matching and rejecting mismatching point pairs; 4) Registering. Although each of the improved algorithms is innovative in one or more of the above steps, some problems remain unavoidable. For example, in the feature point extraction stage, most feature points can be effectively extracted according to the geometric features of the point cloud, but feature points with similarity areas cannot be accurately extracted only by means of single features and are easy to be interfered by noise, so that a large number of non-feature points are extracted; in the characteristic description stage, most of improved descriptors can improve the description capability of local characteristics, but cannot resist the influence of noise interference and data resolution variation; in the stage of feature matching and mismatching point pair rejection, most feature matching point pairs can be obtained through the similarity between descriptors, but the factors such as limited overlapping area of data, noise interference and the like cause that a large number of mismatching point pairs are still doped in the matching sets; most of the improved algorithms have difficulty in balancing the high efficiency against the high accuracy of the registration stage.

Disclosure of Invention

Aiming at the defects of the prior art, the invention provides a point cloud registration method based on a neighborhood normal vector and curvature, which can effectively improve the efficiency, precision and robustness of point cloud registration under the conditions of noise interference, incomplete data and large rotation angle.

The invention discloses a point cloud registration method based on a neighborhood normal vector and curvature, which comprises the following steps:

step 1: preprocessing source point cloud and target point cloud data;

Step 2: constructing a constraint condition of the number of adjacent points for the preprocessed source point cloud and the preprocessed target point cloud according to the degree of the density between the points; extracting initial characteristic points from the preprocessed point cloud according to the normal vector inner product mean value between the sampling points and the adjacent points; extracting the initial characteristic points for the second time through the average curvature of the neighborhood to form characteristic points; forming a final feature point set according to the curvature feature parameter;

step 3: performing FPFH feature description on the final feature point set, performing initial feature matching according to the ratio of the bidirectional minimum Euclidean distance between feature vectors and the secondary Euclidean distance, and adopting the Euclidean distance constraint between point pairs to remove mismatching point pairs;

Step 4: and calculating initial values of initial transformation parameters by taking the correct matching point pairs as unit quaternions, carrying out initial registration on the source point cloud and the target point cloud, constructing a bidirectional k-dimensional tree improved ICP algorithm, and finishing accurate registration.

Further, the preprocessing the source point cloud and the target point cloud data specifically includes:

and establishing a bounding box with the length, width and height of L multiplied by w multiplied by h for the original point cloud P, dividing the bounding box into M cube voxel grids by combining the point cloud density to estimate the voxel side length L, calculating the gravity centers of all points in each voxel grid, and taking the gravity centers as filtering points, thereby reducing the complexity of the point cloud on the premise of keeping most of characteristic information of the point cloud.

Further, step 2.1: extracting initial feature points from the preprocessed point cloud according to the normal vector inner product mean value between the sampling points and the adjacent points, and specifically further comprising:

defining the bending degree of the neighborhood surface according to the density conditions of adjacent points in the neighborhood with different radiuses;

Taking the sampling point p _i as a circle center, and the nearest points of the Euclidean distance between the adjacent points in the search spaces with different radiuses and the sampling point p _i;

And carrying out least square local plane fitting on any point P _i(x_i,y_i,z_i) in the preprocessed source point cloud P= { P _i|p_i∈R³, i=1, 2,3, …, n } and A adjacent points in the adjacent points to obtain a plane equation F, wherein the formula is as follows:

Wherein F (n _F, d) is a plane equation, n _F is a normal vector of the plane F, d is a distance from the plane F to an origin, and p _i is a sampling point;

constructing a covariance matrix C for any point P _i(x_i,y_i,z_i) in the preprocessed source point cloud P and A adjacent points in the adjacent area of the point P:

Wherein, P _i is the sampling point at which,Centroid of sampling point p _i and its a neighboring points;

eigenvalues obtained by solving covariance matrix C Recording deviceMinimum feature valueCorresponding feature vectorThe normal vector of the point p _i is calculated, and the normal vector inner product mean value of the point p _i and the adjacent point represents the normal vector included angle characteristic degree f;

Where f ε [0,1], n _i is the normal vector of point p _i and n _ij is the normal vector of the adjacent point p _ij. For point p _i, the following is satisfied:

f(p_i)＝max[f(p_i1),f(p_i2),f(p_i3),...,f(p_ik)]

wherein f (p _i) is the feature degree of the sampling point p _i, and the point p _i is the feature point, wherein

F (P _i1),f(p_i2),f(p_i3),...,f(p_ik) is the feature degree of k adjacent points of the point P _i, so that a source point cloud P and a target point cloud Q are extracted as initial feature point sets P _t and Q _t;

Step 2.2: the secondary extraction is carried out on the initial characteristic points through the average curvature of the neighborhood to form the characteristic points, and the method further comprises the following steps:

Performing surface fitting on any point P of the initial feature point set P _t by a least square method to obtain a quadratic surface equation r (u, v) of a neighborhood at the point P, obtaining a normal vector n _P at the point P, deriving a first basic quantity E, F, G and a second basic quantity L, M, N, and calculating Gaussian curvature K and average curvature H of the neighborhood at the point P of the curved surface;

Wherein, r _u、r_v is the first order bias of quadric, and r _uu、r_uv、r_vv is the second order bias of quadric ,E＝r_u·r_u,F＝r_u·r_v,G＝r_v·r_v,L＝n_P·r_uu,M＝n_P·r_uv,N＝n_P·r_vv;

Calculating a principal curvature K ₁、k₂ at a point p of the curved surface by using the Gaussian curvature K and the average curvature H;

Comparing the average curvature H of the neighborhood at the point p with the point curvature H _i, and eliminating the point if H _i is smaller than H; conversely, if H _i > H, points greater than the average curvature of the neighborhood are retained as feature point sets P _ω and Q _ω.

Step 2.3: the forming of the final feature point set according to the curvature feature parameter construction further comprises:

In order to remove non-feature points that result in similar curvature for feature point sets P _ω and Q _ω from the estimated neighborhood curvature, the point with the largest curvature change in the neighborhood is extracted for P _ω、Q_ω as the feature point, and the curvature feature parameters of point P _i are:

Wherein, in the formula, the chemical formula, K ₁(p_i)、k₂(p_i) represent the maximum principal curvature and the minimum principal curvature of point p _i, respectively;

The condition whether the set point p _i is a feature point is:

Wherein, The curvature characteristic parameters of the points adjacent to the point P _i meet one of the requirements, and P _i is the point with the largest curvature change in the neighborhood and is extracted as a characteristic point set P _γ、Q_γ.

Further, the performing FPFH feature description on the final feature point set, performing initial feature matching according to a ratio of bidirectional minimum euclidean distance between feature vectors, and removing mismatching point pairs by adopting inter-point pair euclidean distance constraint, and further including:

Step 3.1: FPFH characterization;

re-determining SPFH feature values of each feature point p _i and k adjacent points in the adjacent points, adding a distance weight value to obtain an FPFH feature, and recording the FPFH feature as an FPFH (p _i):

Wherein w _k represents a distance weight, which is the Euclidean distance between the feature point p _i and the k adjacent point in the adjacent domain;

Step 3.2: estimating the corresponding relation;

calculating the Euclidean distance between the FPFH vector of each point in the characteristic point set P _γ and the FPFH vectors of all points in the characteristic point set Q _γ;

For each point in P _γ Find the distance in Q _γ Nearest feature pointAnd next nearest feature pointAnd calculating the ratio r _d, wherein the calculating method comprises the following steps:

Setting a threshold alpha E (0, 1), if r _d(p_i,q_j) is less than alpha, then And (3) withMatching;

Step 3.3: eliminating mismatching point pairs;

Any two pairs of matching point pairs in the initial matching point pair set C _γ are selected AndThe Euclidean distance constraint between a set point pair is:

Wherein epsilon=0.02, satisfies the above formula, Matching point pairs meeting distance constraint are

Calculate AND in C _γ The number of matching points satisfying the above condition is k _γ, and the pairs of points in C _γ are arranged from large to small according to the value of k _γ, a threshold value beta is set, and the number of correct matching points is n _m: n _m＝β·N_m,β∈(0,1),N_m is the number of initial matching point pairs.

Further, the calculating the initial value of the initial transformation parameter by using the correct matching point pair as the unit quaternion, performing initial registration on the source point cloud and the target point cloud, constructing a bidirectional k-dimensional tree improved ICP algorithm, and completing accurate registration, and further comprises:

Step 4.1: initial registration;

The expression of the quaternion is:

Wherein q ₀、q₁、q₂、q₃ is a real number, i, j, k are virtual unit vectors orthogonal to each other, q ₀ is a real part of a quaternion, q ₁、q₂、q₃ is an imaginary part of the quaternion, and the corresponding rotation matrix is:

on the basis of the matching point pair set C _m, calculating an optimal solution of the rigid body change, wherein the objective function of the optimal solution meets the following conditions:

wherein R is an orthogonal matrix, then R ^T r=1, when At maximum, the objective function f (R) is at a minimum, andRepresented by quaternions and converted into matrix form:

Wherein N ₄ is a 4-order matrix. When q ^TN₄ q is the maximum, i.e., f (R) is the maximum;

The eigenvalue obtained by solving the matrix N ₄ is lambda ₁、λ₂、λ₃、λ₄, and lambda ₁＜λ₂＜λ₃＜λ₄ is recorded, then the eigenvector corresponding to the maximum eigenvalue lambda ₄ is the unit quaternion Is a matrix form of (a); solving the unit quaternionAfter substituting it into the quaternion expression and solving for the rotation matrix R, the translation parameter t= [ ΔxΔyΔz ] ^T can be expressed as:

Wherein [ X _i Y_i Z_i]^T ] is the center of gravity point coordinate under the source point cloud coordinate system, and [ X y _i z_i]^T ] is the center of gravity point coordinate under the target point cloud coordinate system;

step 4.2: accurate registration;

Constructing a bi-directional k-dimensional tree search closest point pair (p _i,q_i) on the basis of initial registration, calculating transformation parameters R, T, and calculating the sum of squares of distances between corresponding transformed point pairs:

wherein, the d _k+1-d_k represents the distance between two adjacent iterations;

Setting a threshold delta, and when the absolute value d _k+1-d_k is smaller than delta, ending the iteration to obtain an optimal rigid body transformation parameter R, T; otherwise, repeating iterative calculation of d _k+1-d_k until d _k+1-d_k < delta is satisfied or the maximum iteration number omega is reached, and selecting a minimum transformation matrix R, T of d _k+1-d_k as an optimal transformation parameter.

The invention has the beneficial effects that:

1) Feature point extraction: performing adjacent point number constraint on the density degree between the sampling points and the adjacent points in different neighborhood radiuses, and selecting a point with larger characteristic degree as an initial characteristic point by taking the average value of the inner products of normal vector included angles as the characteristic degree; then calculating the neighborhood curvature and the point curvature for comparison, and screening out points with larger curved surface bending degree as characteristic points; finally, the feature points are optimized by taking the point with the largest curvature change as the feature point through the curvature feature parameters.

2) Feature description and feature matching: and performing FPFH feature description on the extracted feature point set, performing initial feature matching according to the ratio of the bidirectional minimum Euclidean distance between the FPFH feature vectors, calculating Euclidean distance constraint between the initial matching point pairs, and eliminating error matching point pairs with similar features, thereby improving the matching accuracy.

3) Registering: and calculating an initial transformation parameter R ₀、T₀ by taking the correct matching point pair as an initial value and adopting a unit quaternion coordinate conversion model, so as to provide a good initial value for the follow-up accurate registration. And on the basis, a bidirectional k-dimensional tree is constructed to find the corresponding point pair, so that the iterative calculation efficiency of the ICP algorithm is improved, and the accurate registration is completed.

Drawings

Fig. 1 is a schematic illustration of the registration process of the present invention.

Fig. 2 is a schematic diagram of the domain of point p _i.

FIG. 3 is a schematic view of normal vector distribution in different regions

FIG. 4 is a diagram showing the FPFH range of influence of the feature point p _i and its neighboring points

FIG. 5 is a schematic diagram of initial positions of on-vehicle laser point cloud data at different periods

FIG. 6 is a graph showing the result of registration of the present invention to different period of vehicle-mounted laser point cloud data

FIG. 7 is a graph showing the result of registration of different algorithms to different periods of vehicle-mounted laser point cloud data

FIG. 8 is a schematic diagram of initial position of ground and vehicle-mounted laser point cloud data

FIG. 9 is a schematic diagram of the registration result of the present invention to ground and vehicle-mounted laser point cloud data

FIG. 10 is a schematic diagram of the registration results of different algorithms to ground and vehicle-mounted laser point cloud data

Detailed Description

The following describes the embodiments of the present invention further with reference to the accompanying drawings:

in order to improve the efficiency, precision and robustness of point cloud registration under the conditions of noise interference, incomplete data and large rotation angle, the invention provides a point cloud registration method based on a neighborhood normal vector and curvature. The registration flow chart of the method is shown in fig. 1, and the whole flow chart is divided into the following four main steps:

step 1: preprocessing source point cloud and target point cloud data;

In the step, a bounding box with the length width of l×w×h is established for the source point cloud P, the bounding box is divided into M cube voxel grids by combining the point cloud density to estimate the voxel side length L, the gravity centers of all points in each voxel grid are calculated and used as filtering points, and therefore the complexity of the point cloud is reduced on the premise that most characteristic information of the point cloud is maintained.

Step 2: constructing a constraint condition of the number of adjacent points for the preprocessed source point cloud and the preprocessed target point cloud according to the degree of the density between the points;

Extracting initial characteristic points from the preprocessed point cloud according to the normal vector inner product mean value between the sampling points and the adjacent points;

extracting the initial characteristic points for the second time through the average curvature of the neighborhood to form characteristic points;

And forming a final feature point set according to the construction curvature feature parameters.

It should be noted that the number of the substrates,

Extracting initial characteristic points from the preprocessed point cloud according to the normal vector inner product mean value between the sampling points and the adjacent points, and specifically further comprising the following steps:

(1) Defining the bending degree of the neighborhood surface according to the density conditions of adjacent points in the neighborhood with different radiuses, and if the sampling points are denser than the surrounding adjacent points, obtaining a characteristic region with larger surface fluctuation; otherwise, the adjacent points are sparsely distributed, and the surface is flat, namely the non-characteristic area.

(2) Taking the sampling point p _i as a circle center, and the nearest points of the Euclidean distance between the adjacent points in the search spaces with different radiuses and the sampling point p _i;

Wherein the number of adjacent points in the search radius R is k _i, the number of adjacent points in the search radius R is k _j, and R > R, k _j＞k_i. Assuming that the number of required neighboring points a is a specific value, preferably a specific value of 10, if k _i＜N＜k_j, selecting R as a radius neighborhood to search for p _i neighboring points, i.e., k _j =a, as shown in fig. 2 (a); if a is less than or equal to k _i＜k_j, p _i searches for a neighbor point within the radius r neighborhood using the k nearest neighbor, i.e., k _i =a, as shown in fig. 2 (b).

(3) And carrying out least square local plane fitting on any point P _i(x_i,y_i,z_i) in the preprocessed source point cloud P= { P _i|p_i∈R³, i=1, 2,3, …, n } and A adjacent points in the adjacent points to obtain a plane equation F, wherein the formula is as follows:

Wherein F (n _F, d) is a plane equation, n _F is a normal vector of the plane F, d is a distance from the plane F to the origin, and p _i is a sampling point.

(4) Constructing a covariance matrix C for any point P _i(x_i,y_i,z_i) in the preprocessed source point cloud P= { P _i|p_i∈R³, i=1, 2,3, …, n } and A adjacent points in the adjacent area:

Wherein, P _i is the sampling point at which,Is the centroid of the sampling point p _i with its a neighbors.

(5) Eigenvalues obtained by solving covariance matrix CRecording deviceMinimum feature valueCorresponding feature vectorIs the normal vector of p _i.

(6) According to the normal vector distribution characteristics of the point cloud, the normal vector included angle between the sampling point p _i and the adjacent point is defined to describe the bending degree of the surface of the neighborhood, the characteristic region of the point cloud is extracted according to the normal vector included angle, and the normal vector inner product mean value between the point p _i and the adjacent point represents the normal vector included angle characteristic degree f.

Where f ε [0,1], n _i is the normal vector of point p _i and n _ij represents the normal vector of the adjacent point p _ij. For point p _i, if:

f(p_i)＝max[f(p_i1),f(p_i2),f(p_i3),...,f(p_ik)]

wherein f (p _i) is the feature of the sampling point p _i.

Then point p _i is a feature point where f (p _i1),f(p_i2),f(p_i3),...,f(p_ik) is the feature of the k neighboring points of point p _i. As shown in fig. 3 (a), if the included angle between p _i and the normal vector of the adjacent point is smaller, the surface at p _i is bent to a smaller extent, the area is a non-characteristic area, and p _i is a non-characteristic point; as shown in fig. 3 (b), if the included angle between p _i and the normal vector of the adjacent point is larger, the surface at p _i is curved to a larger extent, and this region is a feature region, and p _i is a feature point. Thereby extracting the source point cloud p= { P _i|p_i∈R³, i=1, 2,..n } and the target point cloud q= { Q _j|q_j∈R³, j=1, 2,..m } as an initial feature point set P _t＝{p_t1,p_t2,p_t3,...,p_tn′ } and Q _t＝{q_t1,q_t2,q_t3,...,q_tm′ }, wherein m 'is the number of feature points extracted from the source point cloud after pretreatment, n' is the number of feature points extracted from the target point cloud after pretreatment, and m '< m, n' < n.

Step 2.2: and extracting the initial characteristic points for the second time through the average curvature of the neighborhood to form a characteristic point set.

The method specifically comprises the following steps:

(1) Performing surface fitting on any point P of the initial characteristic point set P _t by a least square method to obtain a quadric equation r (u, v) of the neighborhood at the point P:

(2) Obtaining a normal vector n _p at a p point through a quadric surface r (u, v), and deducing a first basic quantity E, F, G and a second basic quantity L, M, N, so as to calculate a Gaussian curvature K and an average curvature H of a neighborhood at the p point of the curved surface;

Where Q represents a coefficient matrix. r _u、r_v is the first order partial derivative of the quadric surface, and r _uu、r_uv、r_vv is the second order partial derivative of the quadric surface ,E＝r_u·r_u,F＝r_u·r_v,G＝r_v·r_v,L＝n_p·r_uu,M＝n_p·r_uv,N＝n_p·r_vv

(3) Calculating a principal curvature K ₁、k₂ at a point p of the curved surface by using the Gaussian curvature K and the average curvature H;

(4) Comparing the average curvature H of the neighborhood at the point p calculated by the method with the point curvature, if H _i is smaller than H, the surface bending degree at the point p is smaller, the point position distribution of the area is flat, and then eliminating the point smaller than the average curvature of the neighborhood as a non-characteristic area; otherwise, if H _i > H, the surface bending degree at the point P is larger, the point position distribution of the region is steep, points larger than the average curvature of the neighborhood are reserved as feature point sets P _ω＝{p_ω1,p_ω2,p_ω3,...,p_ωn″ and Q _ω＝{q_ω1,q_ω2,q_ω3,...,q_ωm″, wherein m 'is the number of feature points extracted from the initial feature point set in the source point cloud, n' is the number of feature points extracted from the initial feature point set in the target point cloud, and m '< m', n '< n'.

Step 2.3: and forming a final feature point set according to the construction curvature feature parameters.

The main purpose of this step is to eliminate the influence of the estimated curvature, remove redundant data, and obtain the final feature point set, and specifically further includes the following steps:

(1) In order to remove non-feature points that result in similar curvature for feature point sets P _ω and Q _ω from the estimated neighborhood curvature, a curvature feature parameter pair P _ω、Q_ω is constructed to extract a point with the largest curvature change in the neighborhood as a feature point, and the curvature feature parameters of point P _i are:

Wherein, in the formula, the chemical formula, K ₁(p_i)、k₂(p_i) represents the point p _i maximum principal curvature, minimum principal curvature.

(2) The condition whether the set point p _i is a feature point is:

Wherein, The curvature characteristic parameter representing the point adjacent to the point p _i. If the point p _i is the maximum value of curvature characteristic parameters of the adjacent points, the neighborhood surface is convex, and p _i represents a neighborhood salient point; if the point p _i is the minimum value of curvature characteristic parameters of the adjacent points, the neighborhood surface is concave, and p _i represents the neighborhood concave. If one of the above conditions is satisfied, p _i is the point in the neighborhood where the curvature change is the greatest, and it is extracted as feature point set P_γ＝{p_γ1,p_γ2,p_γ3,...,p_γn″′}、Q_γ＝{q_γ1,q_γ2,q_γ3,...,q_γm″′}, and m ' "< m", n ' "< n '".

Step 3: and performing FPFH feature description on the final feature point set, performing initial feature matching according to the ratio of the bidirectional minimum Euclidean distance between feature vectors, and removing mismatching point pairs by adopting the Euclidean distance constraint between the point pairs.

In this step, the FPFH is the feature description of the FPFH, which refers to parameterizing the degree of difference of the feature descriptions of the normal vectors between the neighboring points by calculating the spatial difference between the feature points and the neighboring points, and forming a 33-dimensional feature histogram, so as to obtain the geometric attributes of the feature point p _i and the neighboring points thereof.

And the two FPFH feature vector sets search for the FPFH feature vector closest to the two FPFH feature vectors and closest to the two FPFH feature vector sets, and the distance ratio between the closest FPFH feature vector and the next closest FPFH feature vector is smaller than the specified parameter value, so that the two points are specified as a pair of correct matching point pairs.

And carrying out condition constraint on the Euclidean distance between any two pairs of matching point pairs according to the fact that the ratio of the distances between any two pairs of correct matching point pairs is equal, and setting a threshold value to reject the mismatching point pairs.

Step 3.1: the FPFH calculates the influence range, as shown in fig. 4, which includes the following steps:

(1) Calculating SPFH features of the feature point p _i and k adjacent points in the adjacent points, and marking the features as SPFH (p _i);

(2) Redetermining each feature point p _i and k adjacent points in the adjacent region, adding a distance weight value to obtain an FPFH feature, and recording the FPFH feature as an FPFH (p _i):

Wherein w _k represents a distance weight, which is the euclidean distance between the feature point p _i and the k neighboring point in its neighborhood.

Step 3.2: the corresponding relation estimation is carried out by carrying out FPFH feature description on the extracted feature point set P _γ、Q_γ and generating a corresponding FPFH feature vector, and carrying out initial feature matching by adopting a method of bidirectional minimum and secondary minimum Euclidean distance ratio, wherein the method comprises the following steps:

(1) Calculating the Euclidean distance between the FPFH vector of each point in the characteristic point set P _γ and the FPFH vectors of all points in the characteristic point set Q _γ;

(2) Positive sequence search: for each point in P _γ Find the distance in Q _γ Nearest feature pointAnd next nearest feature pointAnd calculateTo the point ofAndRatio r _d of euclidean distance:

setting a proper threshold alpha E (0, 1), if r _d(p_i,q_j) is less than alpha, then And (3) withMatching; conversely, description pointsThe feature points that match it cannot be found in Q _γ.

(3) And (5) reverse order search: for each point in Q _γ Find the distance in P _γ Nearest feature pointAnd next nearest feature pointAnd calculating the ratio, if r _d(p_i,q_j) < alpha, consideringAnd (3) withMatching; conversely, description pointsThe feature points matching it cannot be found in P _γ.

(4) Only when the point in P _γ And a point in Q _γ Match, and point in Q _γ Also with the point in P _γ When matching, the matching pair is reserved as an initial matching point pair set C _γ.

Step 3.3: and eliminating the mismatching point pairs. In the initial matching point pair set C _γ, there still exist matching point pairs with similar features, and especially in a scene with complex features, due to limitations of noise interference, data loss, acquisition environment and the like, erroneous matching between the point pairs is more likely to be caused. Therefore, in order to reject matching point pairs with similar characteristics, the invention rejects mismatching points by adopting Euclidean distance between the point pairs, and comprises the following steps:

(1) If the matching point pair is correct, any two matching point pairs AndThe distance between them satisfies the following conditions:

(2) Any two pairs of matching point pairs in the initial matching point pair set C _γ are selected AndThe Euclidean distance constraint between a set point pair is:

(3) Calculate AND in C _γ The number of matching points satisfying the above condition is k _γ, and the pairs of points in C _γ are arranged from large to small according to the value of k _γ, a threshold value beta is set, and the number of correct matching points is n _m: n _m＝β·N_m,β∈(0,1),N_m is the number of initial matching point pairs. Removing the mismatching point pair, and updating the initial matching point pair set C _γ to obtain a correct matching point pair set as

Step 4: calculating initial values of initial transformation parameters by taking the correct matching point pairs as unit quaternions, carrying out initial registration on a source point cloud and a target point cloud, constructing a bidirectional k-dimensional tree improved ICP algorithm, and finishing accurate registration; the method comprises the following specific steps:

Step 4.1: calculating an initial value of an initial transformation parameter by taking a correct matching point pair C _m as a unit quaternion, and carrying out initial registration on a source point cloud and a target point cloud, wherein the method comprises the following steps of:

(1) The expression of the quaternion is:

(2) On the basis of the matching point pair set C _m, calculating an optimal solution of the rigid body change, wherein the objective function of the optimal solution meets the following conditions:

Wherein, R is an orthogonal matrix, and R ^T r=1, which is expanded to:

Wherein when At maximum, the objective function f (R) is at a minimum, andExpressed in quaternion as:

And converts it into a matrix form:

Wherein N ₄ is a 4-order matrix. When q ^TN₄ q is at a maximum, i.e., f (R) is at a maximum. The eigenvalue obtained by solving the matrix N ₄ is lambda ₁、λ₂、λ₃、λ₄, and lambda ₁＜λ₂＜λ₃＜λ₄ is recorded, then the eigenvector corresponding to the maximum eigenvalue lambda ₄ is the unit quaternion Is a matrix form of (c). Solving the unit quaternionAfter substituting it into the quaternion expression and solving for the rotation matrix R, the translation parameter t= [ ΔxΔyΔz ] ^T can be expressed as:

Wherein [ X _i Y_i Z_i]^T ] is the center of gravity point coordinate under the source point cloud coordinate system, and [ X y _i z_i]^T ] is the center of gravity point coordinate under the target point cloud coordinate system.

Step 4.2: the method comprises the following specific steps of constructing a bidirectional k-dimensional tree improved ICP algorithm on the basis of initial registration to improve the searching efficiency of the nearest points:

(1) Respectively constructing a k-dimensional tree for a source point cloud P and a target point cloud Q;

(2) Searching the nearest point P _i of q _i in P;

(3) If the closest point of the search p _i in Q is Q _i, the (p _i,q_i) is formed into the closest point pair of the corresponding relation;

(4) If the closest point to search for P _i within Q is not Q _i, then find the closest point to the next point, P _i+1, within P;

(5) All points within the point cloud Q are traversed.

(6) Calculating transformation parameters R, T, and calculating the sum of squares of the distances between the transformed corresponding point pairs:

Where d _k+1-d_k represents the distance between two adjacent iterations. Setting a threshold delta, and when the absolute value d _k+1-d_k is smaller than delta, ending the iteration to obtain an optimal rigid body transformation parameter R, T; otherwise, repeating iterative calculation of d _k+1-d_k until d _k+1-d_k < delta is satisfied or the maximum iteration number omega is reached, and selecting a minimum transformation matrix R, T of d _k+1-d_k as an optimal transformation parameter.

The one-to-one correspondence relation of the nearest points of the bidirectional k-dimensional tree can be determined, and the one-to-one correspondence of the nearest points is twice as long as the k-dimensional tree, so that the iterative calculation time is greatly reduced, and the searching method for constructing the bidirectional k-dimensional tree is more suitable for point cloud data with large data volume.

In order to prove the effectiveness of the point cloud registration method based on the neighborhood normal vector and the curvature, verification is carried out by taking vehicle-Mounted Laser (MLS) point cloud data and ground laser (TLS) point cloud data in different periods as objects, and the method is described in experiment 1 and experiment 2. Because the acquisition quantity is large and is interfered by external environment, a large number of noise points are doped in the original point cloud data, so that the later registration efficiency is affected. Therefore, a pre-processing work is required for the original point cloud before registration. The pre-processing work mainly comprises point cloud denoising and point cloud filtering, namely, drift points, outlier points, redundant points and ground points are removed by combining the existing mature point cloud processing tool with a visual interaction mode in original point cloud data. In addition, for quantifying the registration result, root mean square error of the euclidean distance between the same-name point pairs in the overlapped area after registration is used as an evaluation index, and θ _RMSE is defined as:

Wherein B represents the number of pairs of homonymous points in the overlapping region.

Experiment 1: vehicle-mounted laser point clouds of different periods are registered.

The MLS point cloud initial position is shown in fig. 5, where the maximum deviation of the point cloud initial position is about 6.3m, red is the source point cloud, and blue is the target point cloud. The specific implementation mode is as follows: firstly, carrying out voxel downsampling on two-period vehicle-mounted point cloud data, wherein the sampling interval is 2.5cm; secondly, extracting feature points through neighborhood feature parameters, wherein the neighborhood radius is 3mr, and the number of neighboring points is constrained to be 20; the number of the source point cloud characteristic points is 1644, and the number of the target point cloud characteristic points is 1740; then, the threshold value alpha of the bidirectional minimum and second-smallest Euclidean distance ratio between the FPFH feature vectors is 0.8, the threshold value beta of the correct matching point pair number is 0.3, and the correct matching point pair is 460 pairs; and finally, carrying out initial registration on the correct matching point pair serving as a unit quaternion calculation initial transformation parameter, constructing a bidirectional k-dimensional tree to improve an ICP algorithm, and realizing accurate registration of two-stage vehicle-mounted point cloud data, wherein the registration effect is shown in figure 6.

As can be seen from fig. 6, the two-phase vehicle-mounted point cloud can still achieve accurate registration in the presence of noise interference and data loss. In order to evaluate the performance of the algorithm, the algorithm of the invention is compared with the registration results of the ICP algorithm, the ICP algorithm (TICP) introducing a k-dimensional tree, and the ICP algorithm implementing registration by extracting feature points with Normal Vector (NV) and curvature (NC), the results of which are shown in fig. 7 and table 1. As can be seen from FIG. 7, the ICP algorithm, the NV-ICP algorithm and the NC-ICP algorithm can not accurately register the two-stage MLS point cloud data, but the method is superior to other algorithms in registration accuracy and efficiency, and the effectiveness of the method under the conditions of noise interference and data missing is verified.

Table 1 registration results of different algorithms on different periods of vehicle-mounted laser point cloud data are compared

Experiment 2: the ground is registered with the vehicle-mounted laser point cloud.

The TLS point cloud data and the MLS point cloud data are shown in fig. 8, wherein red is TLS point cloud data as a source point cloud, and blue is MLS point cloud data as a target point cloud. The specific implementation mode is as follows: firstly, performing voxel downsampling on TLS point cloud data and MLS point cloud data, wherein the sampling interval is 2.5cm; secondly, extracting feature points through neighborhood feature parameters, wherein the neighborhood radius is 5mr, the number of adjacent points is constrained to be 20, the number of source point cloud feature points is 1726, and the number of target point cloud feature points is 2143; then, the threshold value alpha of the bidirectional minimum and second-smallest Euclidean distance ratio between the FPFH feature vectors is 0.7, the threshold value beta of the correct matching point pair number is 0.4, and the correct matching point pair is 345 pairs; finally, initial registration is carried out on the correct matching point pair serving as an initial value of a unit element, a bidirectional k-dimensional tree is constructed to improve an ICP algorithm, accurate registration of two groups of point cloud data is achieved, and the registration effect is shown in fig. 9.

As can be seen from fig. 9, the two sets of point cloud data can still be accurately registered with a low local overlap ratio and a large rotation angle and translation value. To evaluate the performance of the inventive algorithm, a comparative analysis of the inventive algorithm was also performed with respect to the registration results of the comparative algorithm of experiment 1, the results of which are shown in fig. 10 and table 2. It can be found from fig. 10 that, the ICP algorithm, the NV-ICP algorithm and the NC-ICP algorithm cannot accurately register the TLS point cloud data with the MLS point cloud data, but the registration accuracy and efficiency of the present invention are superior to those of other algorithms, and the robustness of the present invention under the conditions of low local overlap ratio, large north bias angle and large translation value is verified.

Table 2 comparison of registration results of different algorithms to ground and vehicle-mounted laser point cloud data

Claims

1. A point cloud registration method based on neighborhood normal vector and curvature, characterized by comprising the following steps:

Step 1: Preprocess the source point cloud and target point cloud data;

Step 2: According to the density between each point, the constraint condition of the number of neighboring points is constructed for the preprocessed source point cloud and target point cloud; the initial feature points are extracted from the preprocessed point cloud according to the mean inner product of the normal vectors between the sampling point and the neighboring points; the initial feature points are extracted again through the neighborhood average curvature to form feature points; the final feature point set is formed according to the constructed curvature feature parameters;

Step 3: Perform FPFH feature description on the final feature point set, perform initial feature matching based on the ratio of the bidirectional minimum and second minimum Euclidean distances between feature vectors, and use the Euclidean distance constraint between point pairs to eliminate mismatched point pairs;

Step 4: Use the correct matching point pairs as the unit quaternion to calculate the initial values of the initial transformation parameters, perform initial registration on the source point cloud and the target point cloud, and construct a bidirectional k-dimensional tree to improve the ICP algorithm to complete accurate registration.

2. The point cloud registration method based on neighborhood normal vector and curvature according to claim 1, characterized in that the preprocessing of the source point cloud and the target point cloud data specifically includes:

A bounding box with a length, width and height of l×w×h is established for the original point cloud P. The bounding box is divided into M cubic voxel grids by combining the point cloud density to estimate the voxel side length L. The center of gravity of all points in each voxel grid is calculated and used as the filtering point, thereby reducing the complexity of the point cloud while retaining most of the feature information of the point cloud.

3. The point cloud registration method based on neighborhood normal vector and curvature according to claim 1 is characterized in that the initial feature points are extracted from the preprocessed point cloud according to the mean inner product of the normal vectors between the sampling point and the neighboring points, and specifically further comprises:

The curvature of the neighborhood surface is defined according to the density of neighboring points in neighborhoods of different radii;

Taking sampling point p _i as the center, the neighboring points in the search space with different radii are the points with the closest Euclidean distance to sampling point p _i ;

For any point p _i (x _i ,y _i ,z _i ) in the preprocessed source point cloud P = {p _i |p _i ∈R ³ ,i = 1,2,3,…,n} and its A neighboring points in the neighborhood, the plane equation F is obtained by least squares local plane fitting, and its formula is as follows:

Construct the covariance matrix C for any point p _i (x _i ,y _i ,z _i ) in the preprocessed source point cloud P and its A neighboring points in the neighborhood:

in, p _i is the sampling point, is the centroid of the sampling point _pi and its A neighboring points;

Solving the covariance matrix C for the eigenvalues remember Minimum eigenvalue The corresponding eigenvector is the normal vector of p _i , and the mean of the inner product of the normal vectors of point p _i and its neighboring points is calculated to represent the normal vector angle characteristic f;

Where f∈[0,1], _ni is the normal vector of point _pi , _nij is the normal vector of the neighboring point _pi , and for point _pi , it satisfies:

f(p _i )=max[f(p _i1 ),f(p _i2 ),f(p _i3 ),...,f(p _ik )]

Among them, f( _pi ) is the feature degree of sampling point p _i , point p _i is taken as the feature point, among which f( _pi1 ), f( _pi2 ), f( _pi3 ), ..., f( _pik ) are the feature degrees of the k neighboring points of point p _i , so that the source point cloud P and the target point cloud Q are extracted as the initial feature point sets P _t and Q _t .

4. The point cloud registration method based on neighborhood normal vector and curvature according to claim 3 is characterized in that the initial feature points are extracted again by neighborhood average curvature to form feature points, and further comprises:

For any point p of the initial feature point set _Pt , the surface is fitted by the least square method to obtain the quadratic surface equation r(u,v) of the neighborhood at point p, and the normal vector _nP at point p is obtained. The first basic quantities E, F, G and the second basic quantities L, M, N are derived, and the Gaussian curvature K and mean curvature H of the neighborhood at point p are calculated.

Among them, r _u and r _v are the first-order partial derivatives of the quadratic surface, r _uu , r _uv , and r _vv are the second-order partial derivatives of the quadratic surface, E = r _u ·r _u , F = r _u ·r _v , G = r _v ·r _v , L = n _P ·r _uu , M = n _P ·r _uv , N = n _P ·r _vv ;

Calculate the principal curvatures k ₁ and k ₂ at point p of the surface using the Gaussian curvature K and the mean curvature H;

Compare the average curvature H of the neighborhood at point p with the point curvature _Hi. If _Hi < H, remove the point. Otherwise, if _Hi > H, retain the points with a curvature greater than the neighborhood average curvature as the feature point sets P _ω and Q _ω .

5. The point cloud registration method based on neighborhood normal vector and curvature according to claim 4 is characterized in that the method of constructing curvature feature parameters to form a final feature point set further comprises:

In order to remove the non-feature points that cause the feature point sets P _ω and Q _ω to have similar curvatures due to the estimated neighborhood curvature, the points with the largest curvature changes in the neighborhood of P _ω and Q _ω are extracted as feature points. The curvature feature parameters of point p _i are:

Among them, k ₁ (p _i ) and k ₂ (p _i ) represent the maximum principal curvature and the minimum principal curvature of point p _{i ,} respectively;

The condition for setting whether point _pi is a feature point is:

in, is the curvature characteristic parameter of the neighboring points of point _pi .

6. A point cloud registration method based on neighborhood normal vector and curvature according to claim 1, characterized in that the final feature point set is described by FPFH features, initial feature matching is performed according to the ratio of the bidirectional minimum and second minimum Euclidean distances between feature vectors, and the Euclidean distance constraint between point pairs is used to eliminate mismatched point pairs, and further comprises:

Step 3.1: FPFH feature description;

Step 3.2: Correspondence estimation;

Step 3.3: Eliminate mismatched point pairs.

7. The point cloud registration method based on neighborhood normal vector and curvature according to claim 6, characterized in that the FPFH feature description further includes:

Re-determine the SPFH feature value of each feature point p _i and its k neighboring points in the neighborhood, and add the distance weight to obtain the FPFH feature, which is recorded as formula FPFH(p _i ):

Among them, _wk represents the distance weight, which is the Euclidean distance between the feature point _pi and its k neighboring points in the neighborhood.

8. The point cloud registration method based on neighborhood normal vector and curvature according to claim 6, characterized in that the correspondence relationship estimation further comprises:

Calculate the Euclidean distance between the FPFH vector of each point in the feature point set P _γ and the FPFH vector of all points in the feature point set Q _γ ;

For every point in P _γ Find the distance between Q _γ The nearest feature point and the next closest feature point And calculate the ratio r _d as follows:

Set the threshold α∈(0,1), if r _d (p _i ,q _j )＜α, it is considered and match.

9. The point cloud registration method based on neighborhood normal vector and curvature according to claim 6, characterized in that the step of eliminating mismatched point pairs further comprises:

Select any two pairs of matching points from the initial matching point set C _γ and Set the Euclidean distance constraint between point pairs as:

Among them, ε＝0.02, which satisfies the above formula, The matching point pairs that meet the distance constraint are

Calculate _Cγ and The number of matching point pairs that meet the above conditions is k _γ , and the point pairs in C _γ are arranged from large to small according to the k _γ value. The threshold β is set, and the number of correct matching point pairs _nm should satisfy: _nm = β·N _m , β∈(0,1), where N _m is the number of initial matching point pairs.

10. The point cloud registration method based on neighborhood normal vector and curvature according to claim 1 is characterized in that the correct matching point pairs are used as the initial values of the unit quaternion to calculate the initial transformation parameters, the source point cloud and the target point cloud are initially registered, and a bidirectional k-dimensional tree improved ICP algorithm is constructed to complete the precise registration, and further comprising:

Step 4.1: Initial registration;

Step 4.2: Accurate registration;

The initial registration also includes that the expression of the quaternion is:

Among them, q ₀ , q ₁ , q ₂ , q ₃ are real numbers, i, j, k are mutually orthogonal imaginary unit vectors, q ₀ is the real part of the quaternion, q ₁ , q ₂ , q ₃ are the imaginary parts of the quaternion, and the corresponding rotation matrix is:

The optimal solution of the rigid body change is calculated based on the matching point pair set C _m , and its objective function satisfies:

Where R is an orthogonal matrix, then R ^T R = 1. When it is the maximum value, the objective function f(R) is the minimum. Represent it as a quaternion and convert it into a matrix form:

Where N ₄ is a 4th-order matrix. When q ^T N ₄ q is the maximum value, f(R) is the maximum;

The eigenvalues obtained by solving the matrix N ₄ are λ ₁ , λ ₂ , λ ₃ , and λ ₄ . Let λ ₁ ＜λ ₂ ＜λ ₃ ＜λ ₄ . The eigenvector corresponding to the maximum eigenvalue λ ₄ is the unit quaternion Matrix form; solve for the unit quaternion After the four parameters are substituted into the quaternion expression and the rotation matrix R is solved, the translation parameter T = [ΔXΔYΔZ] ^T can be expressed as:

Among them, [X _i Y _i Z _i ] ^T is the coordinate of the center of gravity point in the source point cloud coordinate system, and [xy _i z _i ] ^T is the coordinate of the center of gravity point in the target point cloud coordinate system;

The precise registration further includes constructing a bidirectional k-dimensional tree to search for the nearest point pair (p _i ,q _i ) based on the initial registration, calculating transformation parameters R and T, and calculating the sum of squares of distances between corresponding point pairs after transformation:

Among them, ||d _k+1 -d _k || represents the distance between two adjacent iterations;

A threshold δ is set. When ||d _k+1 -d _k ||＜δ, the iteration ends and the optimal rigid body transformation parameters R and T are obtained. Otherwise, the iterative calculation of ||d _k+1 -d _k || is repeated until ||d _k+1 -d _k ||＜δ or the maximum number of iterations ω is reached, and the transformation matrices R and T with the smallest ||d _k+1 -d _k || are selected as the optimal transformation parameters.