CN116012423A - Point cloud registration method based on characteristic point normal and global point cloud curvature optimization - Google Patents
Point cloud registration method based on characteristic point normal and global point cloud curvature optimization Download PDFInfo
- Publication number
- CN116012423A CN116012423A CN202211450587.7A CN202211450587A CN116012423A CN 116012423 A CN116012423 A CN 116012423A CN 202211450587 A CN202211450587 A CN 202211450587A CN 116012423 A CN116012423 A CN 116012423A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- difference value
- point
- pairs
- threshold
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Image Analysis (AREA)
Abstract
The invention discloses a point cloud registration method based on feature point normal and global point cloud curvature optimization, which comprises the following steps: extracting surface key points from the target point cloud and the reference point cloud, and searching and constructing one-to-one corresponding characteristic point pairs by using the FPFH descriptors; constructing a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold value based on normal distribution, and eliminating outlier pairs in the feature point pairs; constructing a Markov distance error metric containing normal vector constraint by utilizing the reserved characteristic point pairs, and realizing coarse alignment of the pose after optimizing the Markov distance error metric; and combining the pose coarse alignment algorithm with an improved ICP algorithm added with comprehensive rejection conditions for cross iteration to realize fine point cloud registration. The method is beneficial to improving the convergence speed and the accuracy of point cloud registration. A plurality of experiments prove that the method has high tolerance to the initial position difference of the point cloud and has obvious inhibition effect on the local minimum phenomenon in the iterative process.
Description
Technical Field
The invention relates to the technical field of machine vision point cloud registration, in particular to a point cloud registration method based on feature point normal and global point cloud curvature optimization.
Background
The point cloud is used as a description method of the surface profile of the object, and can well represent the geometric characteristics and the spatial position of the object. Along with the rapid development of three-dimensional vision, the point cloud is widely used in the fields of reverse engineering, robotics, automatic driving and the like. In the process of actually acquiring the point cloud data, due to factors such as equipment, environment and the like, only a local area of an object to be detected can be scanned by one scanning or a single sensor, so that the scanning is required to be performed at different angles, and then the scanned point clouds with different visual angles are registered to restore the complete appearance of the object to be detected. The point cloud registration is a process of unifying two groups of point clouds under the same reference coordinate system by solving a transformation matrix between the two groups of corresponding point clouds. The existing point cloud registration algorithm generally cannot solve the problem that the initial pose is prone to being in local optimum under the condition of non-ideal initial pose, and meanwhile registration speed and efficiency are reduced due to disorder in the registration iteration process, so that the efficient and rapid point cloud registration algorithm is always a hot spot for mass scholars to study.
Point cloud registration has been widely studied. In 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 focus mainly on neighborhood information (e.g., points, lines, or surfaces in a point cloud). Feature-based registration is a three-dimensional extension of image matching, which includes two main steps: feature matching and geometric estimation.
In feature matching, 3D feature keypoints, such as internal shape features (ISS), keypointNet, and USIP, are first detected from the point cloud by a feature detector. Each keypoint is then encoded into a compact feature vector by descriptor-based local shape analysis. And finally, establishing a corresponding point-to-point relation by calculating the pairwise similarity score and chi-square test. In geometric estimation, six degrees of freedom (DoF) rigid transformations are estimated mainly based on robust fitting techniques.
Most of the existing optimization-based registration methods are realized by minimizing geometric projection errors through two processes: corresponding search and transform estimates. Typical optimization-based methods include ICP variant-based, graph model-based, probability model-based, and semi-positive registration methods.
Currently, there are many methods for realizing the registration of 3D point clouds, and in general, most point cloud registration methods adopt a coarse-to-fine strategy, and are mainly classified into coarse registration based on local features and fine registration algorithms based on iterative optimization according to scales. The ICP algorithm (Iterative Closest Point iterative closest point algorithm) is a method for obtaining an optimal solution by using a constraint iterative calculation mode, and is the most widely applied and mature algorithm at present. The standard ICP algorithm establishes the distance error measurement of the closest point pair through Euclidean distance, minimizes to solve the new relative transformation, and can obtain registration results with higher precision after continuous iteration. However, good operation of the ICP algorithm requires an approximate close initial position of the reference point cloud and the target point cloud; meanwhile, due to the direction disorder in the iteration process, the iteration error is easy to be trapped in a local minimum value, so that the convergence speed is slow, and the final registration accuracy is reduced.
Disclosure of Invention
In order to solve the technical problem that the iterative error of the ICP algorithm is easy to be trapped in a local minimum value, the invention provides a point cloud registration method based on characteristic point normal and global point cloud curvature optimization, which specifically comprises the following steps:
s1: extracting surface key points from the target point cloud and the reference point cloud respectively, and searching and constructing one-to-one corresponding characteristic point pairs by using the FPFH descriptors;
s2: constructing a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold value based on normal distribution, and eliminating outlier pairs in the feature point pairs by using the comprehensive eliminating condition;
s3: constructing a mahalanobis distance error metric containing normal vector constraint by using the reserved characteristic point pairs, and realizing coarse alignment of the pose after optimizing the mahalanobis distance error metric;
s4: and combining a pose coarse alignment algorithm with an improved ICP algorithm added with the comprehensive rejection condition for cross iteration to realize fine registration of the point cloud.
Further, the step S1 specifically includes:
s11: extracting surface key points of the reference point cloud and the target point cloud by adopting an ISS algorithm as candidate points, setting a threshold condition, and selecting the candidate points meeting the threshold condition as key points;
s12: constructing an FPFH descriptor for key points extracted from a reference point cloud and a target point cloud, wherein the FPFH descriptor is a 33-dimensional feature vector;
s13: and for the FPFH descriptors of the key points in the reference point cloud, performing space search on the FPFH descriptors of the key points in the target point cloud by using a 33-dimensional KD tree, and finding out the most similar corresponding points to form the characteristic point pairs.
Further, step S11 specifically includes:
s111: for each candidate point p i Querying the neighborhood radius
All points p in j Calculate its weight w ij ；
S112: calculating each candidate point p by combining weights i Covariance matrix cov (p) i )：
S113: calculate each candidate point p i Eigenvalues { lambda } of covariance matrix of (2) 1 ，λ 2 ，λ 3 Sorting according to the order of magnitude, setting a threshold condition according to the change relation of the characteristic values, and selecting candidate points meeting the threshold condition as key points; the threshold condition
The method comprises the following steps:
wherein ε 1 And epsilon 2 Representing a first threshold and a second threshold, respectively.
Further, step S2 specifically includes:
s21: the Euclidean distance between the feature points in the reference point cloud after transformation and the feature points corresponding to the target point cloud is used as a calculated distance difference value;
s22: the included angle between normal vectors of the feature points in the reference point cloud and the feature points corresponding to the target point cloud after transformation is used as a normal vector angle difference value;
s23: taking the logarithmic difference between curvatures of the feature points in the reference point cloud and the feature points corresponding to the target point cloud after transformation as a curvature logarithmic difference value;
s24: calculating three data distributions of the distance difference value, the normal vector angle difference value and the curvature logarithm difference value obtained in S21, S22 and S23 of the characteristic point pairs, setting a threshold interval to approximately represent the intervals of the correct point pairs, performing normal distribution fitting to obtain the mean value and standard deviation of the distribution, wherein the threshold interval is a standard interval selected by Wilson confidence intervals, and the three types of confidence threshold intervals finally obtained by calculation are
The expression is as follows:
wherein distance, normal, curvatures respectively represent three types of threshold intervals of distance difference value, normal vector included angle difference value and curvature difference value, M represents a known transformation matrix,
respectively representing corresponding points on the reference point cloud and the target point cloud, < ->
Respective normal vectors representing the corresponding points, respectively +.>
Respectively representing the curvature of the neighborhood at the corresponding point, N representing the dimension of the corresponding transformation, +.>
Representing a transform matrix product;
s25: the three types of threshold intervals of the distance difference value, the normal vector angle difference value and the curvature difference value calculated in the step S24 are utilized, and if the distance difference value, the normal vector angle difference value and the curvature difference value of the characteristic point pair are simultaneously located in the three types of threshold intervals, the characteristic point pair is reserved; otherwise, rejecting as outliers.
Further, the step S3 specifically includes:
s31: and constructing a coarse alignment Markov distance error measurement function according to the characteristic point pairs reserved in the step S2, wherein the Markov distance error measurement function E has the following expression:
wherein p is s The feature points reserved for the reference point cloud are p t For the corresponding feature point reserved in the target point cloud, the feature point p s And p t Comprising a number of dimensions of 6,
∑ st the covariance matrix between the transformed reference point cloud and the target point cloud is obtained by the method, wherein the covariance matrix is formed between the transformed reference point cloud and the target point cloud in 6 dimensions: x is x i ，y i ，z i Respectively representing coordinate values of three dimensions x, y and z of the point cloud,
values of three dimensions xyz of the point cloud normal vector are respectively represented;
s32: and solving the Markov distance error metric function E by adopting an LM optimization algorithm (Levenberg-Marquardt) to obtain a coarse alignment transformation matrix M.
Further, the step S32 specifically includes:
s321: calculating an incremental equation of the mahalanobis distance error metric function
Wherein the jacobian matrix is calculated as the expression
The tail item is
ΔM k Respectively representing the increment of a transformation matrix and an error measurement E in a coarse alignment algorithm, wherein I represents an identity matrix, and mu is a damping factor;
s322: the damping factor is updated according to the scaling factor ρ, in each iteration:
wherein M is k The k-th transformation matrix is represented, the numerator represents the actual increment size, and the denominator uses the increment size generated by the approximate model; the damping factor mu is determined by the scale factor, the parameter v represents the variation trend parameter,when the scale factor rho is greater than 0, the parameter v is defined as 2, otherwise, the parameter v is defined as 2v;
s323: iterative updating until obtained
And is smaller than a set third threshold value, and a converged coarse alignment transformation matrix M is obtained at the moment.
Further, the step S4 specifically includes:
s41: transforming the reference point cloud according to the coarse alignment transformation matrix M obtained in the step S32 to obtain an aligned updated point cloud;
s42: acquiring corresponding point pairs on the updated point cloud and the target point cloud, removing outliers, constructing a constraint function, and performing nonlinear optimization on the constraint function to obtain a fine registration result M';
s43: the pose coarse alignment algorithm and the improved ICP algorithm are utilized for cross iteration, in the cross iteration process, when the pose coarse alignment algorithm converges to a certain degree, the convergence accuracy cannot be continuously improved, at the moment, a fourth threshold value exists in the registration error, and when the registration error is smaller than the fourth threshold value, the pose coarse alignment algorithm is stopped, and only the improved ICP algorithm further iterates until convergence;
s44: cross-iterating to obtain the final transformation matrix M total And transforming the reference point cloud by using a final transformation matrix to obtain the registered point cloud.
Further, step S42 specifically includes:
s421: for each point in the updated point cloud, searching through the KD tree to obtain a corresponding point in the target point cloud, and forming a corresponding point pair;
s422: filtering all corresponding point pairs by means of the self-adaptive comprehensive eliminating conditions, removing outlier pairs, and reserving accurate corresponding point pairs;
s423: constructing constraint functions by using the reserved corresponding point pairs, and minimizing a post-transformation matrix to M ps ：
Wherein R is k And T is k For the transformation process of rotation and translation matrices s i And t i Respectively representing a corresponding point on the update point cloud and the target point cloud,
and->
For the normal vector of the corresponding point, +.>
Representing a three-dimensional rotational space>
Belongs to a three-dimensional translation space;
s424: parameterized rotation is performed using the rodgers rotation equation and the equation is simplified by rotating small increments:
wherein s is i And t i Representing updating a point on the point cloud and a corresponding point on the target point cloud,
and->
The normal vectors corresponding to the points are respectively, and v' represents the rotation vector at the moment;
s425: solving to obtain corresponding R k And T is k A fine registration transformation matrix M' is obtained.
In addition, in order to realize the registration method, the invention also provides a point cloud registration device based on characteristic point normal and global point cloud curvature optimization, which comprises the following modules:
the characteristic point pair extraction module is used for extracting surface key points from the target point cloud and the reference point cloud respectively, and utilizing the FPFH descriptor to search and construct one-to-one corresponding characteristic point pairs;
the outlier self-adaptive eliminating module is used for constructing a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold according to normal distribution, and eliminating outlier pairs in the characteristic point pairs by utilizing the comprehensive eliminating condition;
the March constraint coarse alignment module is used for constructing a March distance error metric containing normal vector constraint by utilizing the reserved characteristic point pairs, and realizing coarse alignment of the pose after optimizing the March distance error metric;
and the cross iteration module is used for realizing the fine registration of the point cloud according to the combination and cross iteration of the pose coarse alignment algorithm and the improved ICP algorithm added with the comprehensive rejection condition.
In addition, in order to realize the above registration method, the invention also provides an electronic device, which comprises a memory, a processor and a computer program stored on 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.
The technical scheme provided by the invention has the following beneficial effects:
the invention provides a point cloud registration method based on characteristic point normal and global point cloud curvature optimization. First, feature points of the point cloud surface are extracted, and the normal vector thereof is calculated. Six-dimensional feature error metric constraints containing normal vectors are then constructed using mahalanobis distance for coarse alignment of poses. Meanwhile, a new comprehensive eliminating condition of the self-adaptive threshold is provided, wherein the condition comprises the distance difference, the normal vector angle difference and Qu Lvcha of the point pairs, and the noise interference of the outlier is removed, so that the accuracy of the point cloud pairs can be remarkably improved. And then, combining the pose coarse alignment algorithm with an improved ICP algorithm added with comprehensive rejection conditions for cross iteration, thereby being beneficial to improving the convergence speed and precision of point cloud registration. Various experiments prove 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 iterative process. Compared with the traditional method, the algorithm can realize faster convergence speed and higher final convergence precision even when processing samples with poorer initial postures of the point cloud. In addition, the algorithm provided by the invention has satisfactory robustness and shows good application potential in a point cloud registration system.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
FIG. 1 is a flow chart of a point cloud registration method based on feature point normal and global point cloud curvature optimization of the present invention;
FIG. 2 is a schematic diagram of achieving coarse alignment of poses based on feature point pairs to construct a Markov distance error metric including a normal vector constraint in the present invention;
FIG. 3 is a graph showing the construction of a comprehensive rejection condition based on a distance difference value, a normal vector angle difference value and a curvature difference value adaptive threshold based on normal distribution in the invention;
FIG. 4 is a schematic diagram of an improved ICP algorithm taking account of target point cloud surface constraints and outlier pair rejection in the present invention;
fig. 5 is a schematic structural diagram of a point cloud registration device based on feature point normal and global point cloud curvature optimization in the present invention.
Detailed Description
For a clearer understanding of technical features, objects and effects of the present invention, a detailed description of embodiments of the present invention will be made with reference to the accompanying drawings.
Referring to fig. 1, an embodiment of the present invention provides a point cloud registration method based on feature point normal and global point cloud curvature optimization, which specifically includes the following steps:
s1: extracting surface key points from the target point cloud and the reference point cloud respectively, and searching and constructing one-to-one corresponding characteristic point pairs by using the FPFH descriptors;
the step S1 specifically comprises the following steps:
s11: extracting surface key points of the reference point cloud and the target point cloud by adopting an ISS algorithm as candidate points, setting a threshold condition, and selecting the candidate points meeting the threshold condition as key points;
the step S11 specifically includes:
s111: for each candidate point p i Querying the neighborhood radius
All points p in j Calculate its weight w ij ；
S112: calculating each candidate point p by combining weights i Covariance matrix cov (p) i )：
S113: calculate each candidate point p i Eigenvalues { lambda } of covariance matrix of (2) 1 ，λ 2 ，λ 3 Sorting according to the order of magnitude, setting a threshold condition according to the change relation of the characteristic values, and selecting candidate points meeting the threshold condition as key points; the threshold condition
The method comprises the following steps:
wherein ε 1 And epsilon 2 The first threshold and the second threshold are respectively represented, and are generally selected according to the number of the demand characteristic point pairs, and the first threshold and the second threshold are between 0.5 and 1.
S12: constructing an FPFH descriptor for the extracted key points in the reference point cloud and the target point cloud, wherein the FPFH descriptor is a 33-dimensional feature vector;
s13: referring to fig. 2, for the key point FPFH descriptors in the reference point cloud, spatial search is performed on the key point FPFH descriptors in the target point cloud by using a 33-dimensional KD tree, and the key point KD-tree search finds the most similar corresponding point to form a feature point pair, and alignment coincidence of the reference point cloud and the target point cloud can be realized through coarse alignment.
S2: referring to fig. 3, a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold is constructed based on normal distribution, and outlier pairs in the feature point pairs are eliminated by using the comprehensive eliminating condition;
s21: the Euclidean distance between the feature points in the reference point cloud after transformation and the feature points corresponding to the target point cloud is used as a calculated distance difference value;
s22: the included angle between normal vectors of the feature points in the reference point cloud and the feature points corresponding to the target point cloud after transformation is used as a normal vector angle difference value;
s23: taking the logarithmic difference between curvatures of the feature points in the reference point cloud and the feature points corresponding to the target point cloud after transformation as a curvature logarithmic difference value;
s24: calculating three data distributions of the distance difference value, the normal vector angle difference value and the curvature logarithm difference value, which are obtained in S21, S22 and S23, of the feature point pairs, setting a threshold interval to approximately represent the intervals where the correct point pairs are located, performing normal distribution fitting to obtain the mean value and standard deviation of the distribution, wherein the threshold interval is a standard interval selected by a Wilson confidence interval, referring to FIG. 3, 3 (a), 3 (b) and 3 (c) respectively correspond to error distribution diagrams of the distance difference value, the normal vector angle difference value and the curvature difference value, and a dotted line is a fitted normal distribution curve. The three confidence threshold intervals finally calculated are
The expression is as follows:
wherein distance, normal, curvatures respectively represent three types of threshold intervals of distance difference value, normal vector included angle difference value and curvature difference value, M represents a known transformation matrix,
respectively representing corresponding points on the reference point cloud and the target point cloud, < ->
Respective normal vectors representing the corresponding points, respectively +.>
Respectively representing the curvature of the neighborhood at the corresponding point, N representing the dimension of the corresponding transformation, +.>
Representing a transform matrix product;
s25: the three types of threshold intervals of the distance difference value, the normal vector angle difference value and the curvature difference value calculated in the step S24 are utilized, and if the distance difference value, the normal vector angle difference value and the curvature difference value of the characteristic point pair are simultaneously located in the three types of threshold intervals, the characteristic point pair is reserved; otherwise, outlier rejection is performed as outlier.
S3: referring to fig. 2, a mahalanobis distance error metric containing normal vector constraint is constructed by using the reserved characteristic point pairs, and coarse alignment of the pose is realized after the mahalanobis distance error metric is optimized;
the step S3 specifically comprises the following steps:
s31: and constructing a coarse alignment Markov distance error measurement function according to the characteristic point pairs reserved in the step S2, wherein the Markov distance error measurement function E has the following expression:
wherein p is s The feature points reserved for the reference point cloud are p t For the corresponding feature point reserved in the target point cloud, the feature point p s And p t Comprising a number of dimensions of 6,
∑ st the covariance matrix between the transformed reference point cloud and the target point cloud is obtained by the method, wherein the covariance matrix is formed between the transformed reference point cloud and the target point cloud in 6 dimensions: x is x i ，y i ，z i Coordinate values respectively representing three dimensions xyz of the point cloud,/->
Values of three dimensions xyz of the point cloud normal vector are respectively represented;
s32: and solving the Markov distance error measurement function E by adopting an LM optimization algorithm to obtain a roughly aligned transformation matrix M.
The step S32 specifically includes:
s321: calculating an incremental equation of the mahalanobis distance error metric function
Wherein the jacobian matrix is calculated as the expression
The tail item is
ΔM k Respectively representing the increment of a transformation matrix and an error measurement E in a coarse alignment algorithm, wherein I represents an identity matrix, and mu is a damping factor;
s322: the damping factor is updated according to the scaling factor ρ, in each iteration:
wherein M is k The k-th transformation matrix is represented, the numerator represents the actual increment size, and the denominator uses the increment size generated by the approximate model; the damping factor mu is determined by a scale factor, the parameter v represents a variation trend parameter, and when the scale factor rho is greater than 0, the parameter v is defined as 2Otherwise, defining as 2v;
s323: iterative updating until obtained
Is smaller than a set third threshold value, which can be 10 -8 At this time, a converged coarse alignment transformation matrix M is obtained.
S4: and combining the pose coarse alignment algorithm with an improved ICP algorithm added with comprehensive rejection conditions for cross iteration to realize fine registration of the point cloud.
The step S4 specifically comprises the following steps:
s41: transforming the reference point cloud according to the coarse alignment transformation matrix M obtained in the step S32 to obtain an aligned updated point cloud;
s42: referring to fig. 4, corresponding point pairs on an updated point cloud and a target point cloud are obtained, after outlier rejection is carried out, a constraint function is constructed, and nonlinear optimization is carried out on the constraint function to obtain a fine registration result M';
the step S42 specifically includes:
s421: for each point in the updated point cloud, searching through the KD tree to obtain the corresponding point in the target point cloud to form a corresponding point pair, as shown in FIG. 4, s in FIG. 4 1 And t 1 A set of corresponding points;
s422: filtering all corresponding point pairs by means of the self-adaptive comprehensive eliminating conditions, removing outlier pairs, and reserving accurate corresponding point pairs;
s423: constructing constraint functions by using the reserved corresponding point pairs, and minimizing a post-transformation matrix to M ps ：
Wherein R is k And T is k For the transformation process of rotation and translation matrices s i And t i Respectively representing a corresponding point on the update point cloud and the target point cloud,
and->
For the normal vector of the corresponding point, +.>
Representing a three-dimensional rotational space>
In the constraint relation included in the improved ICP, as shown in fig. 4, a tiny curved surface in a target point cloud is approximated to be a circular curve, and the normal vector of the target point is calculated to enable the target point cloud to be in a convergence domain of adjacent curved surfaces during convergence;
s424: parameterized rotation is performed using the rodgers rotation equation and the equation is simplified by rotating small increments:
wherein s is i And t i Representing updating a point on the point cloud and a corresponding point on the target point cloud,
and->
The normal vectors corresponding to the points are respectively, and v' represents the rotation vector at the moment;
s425: solving to obtain corresponding R k And T is k A fine registration transformation matrix M' is obtained.
S43: the pose coarse alignment algorithm is utilized to carry out cross iteration with the improved ICP algorithm in the S42, in the cross iteration process, when the pose coarse alignment algorithm converges to a certain degree, the convergence accuracy cannot be continuously improved, at the moment, a fourth threshold value exists in the registration error, when the registration error is smaller than the fourth threshold value (the threshold value is generally selected according to the point cloud size), the coarse alignment algorithm is stopped, and only the improved ICP algorithm is further iterated until convergence;
s44: cross-iterating to obtain the final transformation matrix M total Using final transformationsAnd transforming the reference point cloud by the matrix to obtain the registered point cloud.
As an optional implementation manner, the invention further provides a point cloud registration device based on feature point normal and global point cloud curvature optimization, and referring to fig. 5, the device comprises the following modules:
the characteristic point pair extraction module 1 is used for extracting surface key points from a target point cloud and a reference point cloud respectively, and utilizing FPFH descriptor search to construct one-to-one corresponding characteristic point pairs;
the outlier self-adaptive eliminating module 2 is used for constructing a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold according to normal distribution, and eliminating outlier pairs in the characteristic point pairs by utilizing the comprehensive eliminating condition;
the mahalanobis constraint coarse alignment module 3 is used for constructing a mahalanobis distance error metric containing normal vector constraint by utilizing the reserved characteristic point pairs, and realizing the coarse alignment of the pose after optimizing the mahalanobis distance error metric;
and the cross iteration module 4 is used for realizing the fine registration of the point cloud according to the combination and cross iteration of the pose coarse alignment algorithm and the improved ICP algorithm added with the comprehensive rejection condition.
As an optional implementation manner, the present invention further provides an electronic device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor, where the processor implements the steps of the point cloud registration method when executing the program, and the steps specifically include: extracting surface key points from the target point cloud and the reference point cloud respectively, and searching and constructing one-to-one corresponding characteristic point pairs by using the FPFH descriptors; constructing a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold value based on normal distribution, and eliminating outlier pairs in the feature point pairs by using the comprehensive eliminating condition; constructing a mahalanobis distance error metric containing normal vector constraint by using the reserved characteristic point pairs, and realizing coarse alignment of the pose after optimizing the mahalanobis distance error metric; and combining a pose coarse alignment algorithm with an improved ICP algorithm added with the comprehensive rejection condition for cross iteration to realize fine registration of the point cloud.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or system that comprises the element.
The foregoing embodiment numbers of the present invention are merely for the purpose of description, and do not represent the advantages or disadvantages of the embodiments. In the unit claims enumerating several means, several of these means may be embodied by one and the same item of hardware. The use of the terms first, second, third, etc. do not denote any order, but rather the terms first, second, third, etc. are used to interpret the terms as labels.
The foregoing description is only of the preferred embodiments of the present invention, and is not intended to limit the scope of the invention, but rather is intended to cover any equivalents of the structures or equivalent processes disclosed herein or in the alternative, which may be employed directly or indirectly in other related arts.
Claims (10)
1. The point cloud registration method based on feature point normal and global point cloud curvature optimization is characterized by comprising the following steps of:
s1: extracting surface key points from the target point cloud and the reference point cloud respectively, and searching and constructing one-to-one corresponding characteristic point pairs by using the FPFH descriptors;
s2: constructing a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold value based on normal distribution, and eliminating outlier pairs in the feature point pairs by using the comprehensive eliminating condition;
s3: constructing a mahalanobis distance error metric containing normal vector constraint by using the reserved characteristic point pairs, and realizing coarse alignment of the pose after optimizing the mahalanobis distance error metric;
s4: and combining a pose coarse alignment algorithm with an improved ICP algorithm added with the comprehensive rejection condition for cross iteration to realize fine registration of the point cloud.
2. The point cloud registration method according to claim 1, wherein step S1 specifically includes:
s11: extracting surface key points of the reference point cloud and the target point cloud by adopting an ISS algorithm as candidate points, setting a threshold condition, and selecting the candidate points meeting the threshold condition as key points;
s12: constructing an FPFH descriptor for key points extracted from a reference point cloud and a target point cloud, wherein the FPFH descriptor is a 33-dimensional feature vector;
s13: and for the FPFH descriptors of the key points in the reference point cloud, performing space search on the FPFH descriptors of the key points in the target point cloud by using a 33-dimensional KD tree, and finding out the most similar corresponding points to form the characteristic point pairs.
3. The point cloud registration method according to claim 2, wherein step S11 specifically includes:
s111: for each candidate point p i Querying the neighborhood radius
All points p in j Calculate its weight w ij ；
S112: calculating each candidate point p by combining weights i Covariance matrix cov (p) i )：
S113: calculate each candidate point p i Eigenvalues { lambda } of covariance matrix of (2) 1 ，λ 2 ，λ 3 Sorting according to the order of magnitude, according to the change of characteristic valueSetting a threshold condition, and selecting candidate points meeting the threshold condition as key points; the threshold condition
The method comprises the following steps:
wherein ε 1 And epsilon 2 Representing a first threshold and a second threshold, respectively.
4. The point cloud registration method according to claim 1, wherein step S2 specifically includes:
s21: the Euclidean distance between the feature points in the reference point cloud after transformation and the feature points corresponding to the target point cloud is used as a calculated distance difference value;
s22: the included angle between normal vectors of the feature points in the reference point cloud and the feature points corresponding to the target point cloud after transformation is used as a normal vector angle difference value;
s23: taking the logarithmic difference between curvatures of the feature points in the reference point cloud and the feature points corresponding to the target point cloud after transformation as a curvature logarithmic difference value;
s24: calculating three data distributions of the distance difference value, the normal vector angle difference value and the curvature logarithm difference value obtained in S21, S22 and S23 of the characteristic point pairs, setting a threshold interval to approximately represent the intervals of the correct point pairs, performing normal distribution fitting to obtain the mean value and standard deviation of the distribution, wherein the threshold interval is a standard interval selected by Wilson confidence intervals, and the three types of confidence threshold intervals finally obtained by calculation are
The expression is as follows: />
Wherein distance, normal, curvatures respectively represent three types of threshold intervals of distance difference value, normal vector included angle difference value and curvature difference value, M represents a known transformation matrix,
representing the corresponding points on the reference point cloud and the target point cloud respectively,
respective normal vectors representing the corresponding points, respectively +.>
Respectively representing the curvature of the neighborhood at the corresponding point, N representing the dimension of the corresponding transformation, +.>
Representing a transform matrix product;
s25: the three types of threshold intervals of the distance difference value, the normal vector angle difference value and the curvature difference value calculated in the step S24 are utilized, and if the distance difference value, the normal vector angle difference value and the curvature difference value of the characteristic point pair are simultaneously located in the three types of threshold intervals, the characteristic point pair is reserved; otherwise, rejecting as outliers.
5. The point cloud registration method according to claim 1, wherein step S3 specifically includes:
s31: and constructing a coarse alignment Markov distance error measurement function according to the characteristic point pairs reserved in the step S2, wherein the Markov distance error measurement function E has the following expression:
wherein p is s The feature points reserved for the reference point cloud are p t For the corresponding feature point reserved in the target point cloud, the feature point p s And p t Comprising a number of dimensions of 6,
∑ st the covariance matrix between the transformed reference point cloud and the target point cloud is obtained by the method, wherein the covariance matrix is formed between the transformed reference point cloud and the target point cloud in 6 dimensions: x is x i ，y i ，z i Coordinate values respectively representing three dimensions x, y and z of the point cloud, < >>
Values of three dimensions xyz of the point cloud normal vector are respectively represented;
s32: and solving the Markov distance error measurement function E by adopting an LM optimization algorithm to obtain a roughly aligned transformation matrix M.
6. The point cloud registration method according to claim 5, wherein step S32 specifically includes:
s321: calculating an incremental equation of the mahalanobis distance error metric function
Wherein the jacobian matrix is calculated as the expression
The tail item is
ΔM k Respectively representing the increment of a transformation matrix and an error measurement E in a coarse alignment algorithm, wherein I represents an identity matrix, and mu is a damping factor;
s322: the damping factor is updated according to the scaling factor ρ, in each iteration:
wherein M is k The k-th transformation matrix is represented, the numerator represents the actual increment size, and the denominator uses the increment size generated by the approximate model; the damping factor mu is determined by a scale factor, the parameter v represents a variation trend parameter, when the scale factor rho is greater than 0, the parameter v is defined as 2, otherwise, the parameter v is defined as 2v;
s323: iterative updating until obtained
And is smaller than a set third threshold value, and a converged coarse alignment transformation matrix M is obtained at the moment. />
7. The point cloud registration method according to claim 5, wherein step S4 specifically includes:
s41: transforming the reference point cloud according to the coarse alignment transformation matrix M obtained in the step S32 to obtain an aligned updated point cloud;
s42: acquiring corresponding point pairs on the updated point cloud and the target point cloud, removing outliers, constructing a constraint function, and performing nonlinear optimization on the constraint function to obtain a fine registration result M';
s43: the pose coarse alignment algorithm and the improved ICP algorithm are utilized for cross iteration, in the cross iteration process, when the pose coarse alignment algorithm converges to a certain degree, the convergence accuracy cannot be continuously improved, at the moment, a fourth threshold value exists in the registration error, and when the registration error is smaller than the fourth threshold value, the pose coarse alignment algorithm is stopped, and only the improved ICP algorithm further iterates until convergence;
s44: cross-iterating to obtain the final transformation matrix M total And transforming the reference point cloud by using a final transformation matrix to obtain the registered point cloud.
8. The point cloud registration method as recited in claim 7, wherein step S42 specifically includes:
s421: for each point in the updated point cloud, searching through the KD tree to obtain a corresponding point in the target point cloud, and forming a corresponding point pair;
s422: filtering all corresponding point pairs by means of the self-adaptive comprehensive eliminating conditions, removing outlier pairs, and reserving accurate corresponding point pairs;
s423: constructing constraint functions by using the reserved corresponding point pairs, and minimizing a post-transformation matrix to M ps ：
Wherein R is k And T is k For the transformation process of rotation and translation matrices s i And t i Respectively representing a corresponding point on the update point cloud and the target point cloud,
and->
For the normal vector of the corresponding point, +.>
Representing a three-dimensional rotational space>
Belongs to a three-dimensional translation space;
s424: parameterized rotation is performed using the rodgers rotation equation and the equation is simplified by rotating small increments:
wherein s is i And t i Representing updating a point on the point cloud and a corresponding point on the target point cloud,
and->
The normal vectors corresponding to the points are respectively, and v' represents the rotation vector at the moment;
s425: solving to obtain corresponding R k And T is k A fine registration transformation matrix M' is obtained.
9. The point cloud registration device based on characteristic point normal and global point cloud curvature optimization is characterized by comprising the following modules:
the characteristic point pair extraction module is used for extracting surface key points from the target point cloud and the reference point cloud respectively, and utilizing the FPFH descriptor to search and construct one-to-one corresponding characteristic point pairs;
the outlier self-adaptive eliminating module is used for constructing a comprehensive eliminating condition based on a distance difference value, a normal vector angle difference value and a curvature difference value self-adaptive threshold according to normal distribution, and eliminating outlier pairs in the characteristic point pairs by utilizing the comprehensive eliminating condition;
the March constraint coarse alignment module is used for constructing a March distance error metric containing normal vector constraint by utilizing the reserved characteristic point pairs, and realizing coarse alignment of the pose after optimizing the March distance error metric;
and the cross iteration module is used for realizing the fine registration of the point cloud according to the combination and cross iteration of the pose coarse alignment algorithm and the improved ICP algorithm added with the comprehensive rejection condition.
10. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the steps of the point cloud registration method according to any of claims 1 to 8 when the program is executed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211450587.7A CN116012423A (en) | 2022-11-18 | 2022-11-18 | Point cloud registration method based on characteristic point normal and global point cloud curvature optimization |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211450587.7A CN116012423A (en) | 2022-11-18 | 2022-11-18 | Point cloud registration method based on characteristic point normal and global point cloud curvature optimization |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116012423A true CN116012423A (en) | 2023-04-25 |
Family
ID=86034255
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211450587.7A Pending CN116012423A (en) | 2022-11-18 | 2022-11-18 | Point cloud registration method based on characteristic point normal and global point cloud curvature optimization |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116012423A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116977331A (en) * | 2023-09-22 | 2023-10-31 | 武汉展巨华科技发展有限公司 | 3D model surface detection method based on machine vision |
| CN117454672A (en) * | 2023-12-22 | 2024-01-26 | 湖南大学 | Robot operation allowance calculation method based on curved surface assembly constraint |
-
2022
- 2022-11-18 CN CN202211450587.7A patent/CN116012423A/en active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116977331A (en) * | 2023-09-22 | 2023-10-31 | 武汉展巨华科技发展有限公司 | 3D model surface detection method based on machine vision |
| CN116977331B (en) * | 2023-09-22 | 2023-12-12 | 武汉展巨华科技发展有限公司 | 3D model surface detection method based on machine vision |
| CN117454672A (en) * | 2023-12-22 | 2024-01-26 | 湖南大学 | Robot operation allowance calculation method based on curved surface assembly constraint |
| CN117454672B (en) * | 2023-12-22 | 2024-04-12 | 湖南大学 | Robot operation allowance calculation method based on curved surface assembly constraint |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Lei et al. | Fast descriptors and correspondence propagation for robust global point cloud registration | |
| CN116012423A (en) | Point cloud registration method based on characteristic point normal and global point cloud curvature optimization | |
| CN111784770B (en) | Three-dimensional attitude estimation method in disordered grabbing based on SHOT and ICP algorithm | |
| Yao et al. | Point cloud registration algorithm based on curvature feature similarity | |
| Sharp et al. | ICP registration using invariant features | |
| EP2081133B1 (en) | System and method for deformable object recognition | |
| US9141871B2 (en) | Systems, methods, and software implementing affine-invariant feature detection implementing iterative searching of an affine space | |
| US9984280B2 (en) | Object recognition system using left and right images and method | |
| CN101493889B (en) | Method and apparatus for tracking video object | |
| CN105551015A (en) | Scattered-point cloud image registering method | |
| CN114677418B (en) | Registration method based on point cloud feature point extraction | |
| CN101770566B (en) | Quick three-dimensional human ear identification method | |
| CN109544603B (en) | Target tracking method based on deep migration learning | |
| CN107316328B (en) | Closed loop detection method based on angular point characteristics of two-dimensional laser scanner | |
| CN108229416A (en) | Robot SLAM methods based on semantic segmentation technology | |
| CN113313701B (en) | Electric vehicle charging port two-stage visual detection positioning method based on shape prior | |
| CN113706381A (en) | Three-dimensional point cloud data splicing method and device | |
| CN109255815B (en) | A kind of object detection and recognition methods based on order spherical harmonic | |
| CN112070832A (en) | Non-cooperative target pose tracking method and device based on point cloud DCA (distributed computing architecture) features | |
| CN110472651B (en) | Target matching and positioning method based on edge point local characteristic value | |
| Hara et al. | 6DOF iterative closest point matching considering a priori with maximum a posteriori estimation | |
| CN117372480A (en) | Point cloud registration method utilizing self-adaptive downsampling and curvature enhancement point-to-point characteristics | |
| Pennec | Toward a generic framework for recognition based on uncertain geometric features | |
| Lu et al. | Three-dimensional object recognition using an extensible local surface descriptor | |
| CN115601408B (en) | Point cloud registration method based on particle swarm optimization and topological graph |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |