Improved ISS-FPFH feature-based ICP point cloud registration method

Abstract

The invention discloses an ICP point cloud registration method based on an improved ISS-FPFH feature. The method comprises the following steps: obtaining a target point cloud and a point cloud to be matched after down-sampling by using an improved normal vector consistency voxel down-sampling method; carrying out ISS-FPFH feature matching to obtain a rough transformation matrix, and transforming the to-be-matched point cloud after down-sampling to obtain a rough registration point cloud; and performing fine registration by using an improved iterative closest point ICP algorithm to obtain a fine transformation matrix, and converting the down-sampled to-be-matched point cloud to obtain a fine registration point cloud to realize point cloud registration. According to the method, the geometric features of the original point cloud can be reserved as much as possible while the point cloud density is reduced; better rough matching can be carried out by using ISS-FPFH feature matching; in the stage of improving ICP fine registration, a point-to-surface distance error function is used to accelerate the convergence speed, so that the matching efficiency can be remarkably improved while the matching precision is ensured.

Description

Technical Field

[0001] This invention relates to a point cloud registration method, specifically to a three-dimensional point cloud registration technology, and more specifically to an ICP point cloud registration method based on improved ISS-FPFH features. Background Technology

[0002] With the rapid development of technologies such as autonomous driving, robot navigation, and drone positioning, Simultaneous Localization and Mapping (SLAM) technology has been widely applied in various intelligent systems. SLAM systems collect point cloud data from the environment using laser sensors, construct 3D maps in real time, and achieve self-localization. Due to the high precision and rich distance information of laser point cloud data, SLAM has become an important tool for high-precision map construction and dynamic environment modeling.

[0003] However, laser real-time localization and mapping (SLAM) technology faces many challenges in application, especially in dynamic environments. Due to the noise and uneven density of point cloud data, traditional point cloud matching algorithms suffer from bottlenecks in matching accuracy and computational efficiency. Furthermore, the sheer volume of point cloud data in large-scale scenes makes efficient point cloud registration, especially when the initial pose is unknown or undergoes large-scale changes, a problem that remains to be solved.

[0004] Traditional matching algorithms, such as the Iterated Closest Points (ICP) algorithm, can provide high-precision matching results, but their convergence speed is slow and they are prone to getting trapped in local optima when the initial registration error is large. Many improvements to the ICP algorithm have addressed this issue to some extent, but performing ICP matching under traditional neighborhood distance search requires significant computational resources, making direct application unsuitable for the real-time requirements of mobile robot localization and mapping systems. Summary of the Invention

[0005] To address the problems existing in the background art, the present invention provides an ICP point cloud registration method based on improved ISS-FPFH features.

[0006] The technical solution adopted in this invention is:

[0007] The ICP point cloud registration method based on improved ISS-FPFH features of the present invention includes:

[0008] Step S1: After processing the target point cloud and the point cloud to be matched using the improved normal vector consistent voxel downsampling method, the downsampled target point cloud P and the point cloud to be matched Q are obtained.

[0009] Step S2: Based on the downsampled target point cloud P and the point cloud to be matched Q, perform Intrinsic Shape Signatures (ISS) - Fast Point Feature Histogram (FPFH) feature matching to obtain a coarse transformation matrix. Then, transform the downsampled point cloud to be matched Q to obtain a coarse registration point cloud Q′.

[0010] Step S3: After fine registration of the coarse registration point cloud Q′ using the improved iterative nearest point ICP algorithm, a fine transformation matrix is ​​obtained. The fine transformation matrix is ​​then used to transform the coarse registration point cloud Q′ to obtain the fine registration point cloud Q″, thus achieving point cloud registration.

[0011] Specifically, step S1 involves taking the target point cloud and the point cloud to be matched as point clouds to be processed, dividing each point cloud into several voxel cube meshes of the same size, obtaining the normal vector of each point in each voxel cube mesh for each voxel cube mesh, obtaining the average normal vector of all points, obtaining the angle between the normal vector of each point and the average normal vector, retaining the point with the smallest angle as the representative point of the voxel cube mesh, and removing other points in the voxel cube mesh. Finally, the representative points retained from each voxel cube mesh constitute the downsampled point cloud.

[0012] Step S2 specifically involves taking the downsampled target point cloud and the point cloud to be matched as downsampled point clouds, extracting key points from each downsampled point cloud using the 3D Geometry Descriptor (ISS) algorithm to obtain a key point set; then constructing a feature descriptor for each key point in the key point set using the Fast Point Feature Histogram (FPFH) algorithm; based on the feature descriptors of each key point in the two key point sets, matching the two key point sets using the FLANN (Fast Library for Approximate Nearest Neighbors) algorithm to obtain several matching point pairs; and finally, performing coarse point cloud registration on each pair of matching point pairs using the Fast and Proven Point Cloud Registration (TEASER++) algorithm to obtain a coarse transformation matrix.

[0013] The specific steps of S3 are as follows:

[0014] Step S3.1: Establish a k-dimensional kd-tree structure for the downsampled target point cloud P to find nearest neighbor points in space.

[0015] Step S3.2: For each coarse matching point in the coarse matching point cloud Q′, find the three nearest points of the coarse matching point in the k-dimensional tree kd-tree structure, and determine whether the three nearest points form a plane. If so, obtain the plane normal vector of the formed plane; otherwise, skip the current coarse matching point until all coarse matching points have been determined.

[0016] Step S3.3: Replace the point-to-point distance constraints in the original Iterative Closest Point ICP algorithm with point-to-surface distance constraints to construct an iterative error function for iteration, thereby obtaining an updated fine transformation matrix.

[0017] Step S3.4: During the iterative solution process, determine whether the iterative error function is less than the preset error threshold or whether the maximum number of iterations has been reached. If yes, obtain the final updated fine transformation matrix. If no, update the coarse matching point cloud Q′ according to the currently updated fine transformation matrix and return to step S3.2.

[0018] Step S3.5: Obtain the final updated refined transformation matrix.

[0019] In step S3.3, the iterative error function E(R,t) is as follows:

[0020]

[0021] Q″ i =R″·Q′ i +t″

[0022] Where R and t are the rotation matrix and translation vector during the iterative update process, respectively; n is the total number of coarse matching points in the coarse matching point cloud Q′; Q′ i Let q be the i-th coarsely matched point in the coarsely matched point cloud Q′; i For the i-th coarse matching point Q′ i One of the closest points; n i For the i-th coarse matching point Q′ i The plane normal vector of the plane formed by the three nearest points; Q″ i Let R″ be the refined matching point after transformation of the i-th coarse matching point in the coarse matching point cloud Q′ during the iterative update process; R″ and t″ are the rotation matrix and translation vector of the refined transformation matrix during the iterative update process, respectively.

[0023] The ICP point cloud registration system based on improved ISS-FPFH features of the present invention includes:

[0024] The downsampling module is used to process the target point cloud and the point cloud to be matched using an improved normal vector consistent voxel downsampling method to obtain the downsampled target point cloud P and the point cloud to be matched Q.

[0025] The feature matching module is used to obtain a coarse transformation matrix after performing 3D geometric shape descriptor ISS-Fast Point Feature Histogram FPFH feature matching on the downsampled target point cloud P and the point cloud to be matched Q. The coarse registration point cloud Q′ is obtained by transforming the downsampled point cloud Q.

[0026] The point cloud registration module is used to perform fine registration on the coarsely registered point cloud Q′ using the improved iterative nearest point ICP algorithm to obtain the fine transformation matrix. The fine transformation matrix is ​​then used to transform the coarsely registered point cloud Q′ to obtain the finely registered point cloud Q″, thus achieving point cloud registration.

[0027] The electronic device of the present invention includes: a memory and a processor coupled to each other, wherein the memory stores program data, and the processor invokes the program data to execute the method described above.

[0028] The present invention provides a computer-readable storage medium having program data stored thereon, which, when executed by a processor, implements the method described above.

[0029] The beneficial effects of the present invention are:

[0030] 1. This invention introduces normal vector consistency downsampling technology in the point cloud data preprocessing stage, which reduces the point cloud density while preserving the geometric features of the original point cloud as much as possible.

[0031] 2. This invention uses the solid geometry descriptor ISS-Fast Point Feature Histogram FPFH to describe the local geometric information of the point cloud, combines the FLANN algorithm of the approximate nearest neighbor search library for feature matching, and uses the fast and provable point cloud registration TEASER++ algorithm for coarse matching, providing good initial values ​​for the subsequent improved iterative nearest point ICP algorithm.

[0032] 3. In the improved iterative closest point (ICP) fine registration stage, this invention uses a point-to-surface distance error function to replace the point-to-point distance error function in the original algorithm. This simplifies the nonlinear calculations in the algorithm to linear calculations, accelerating the convergence speed and enabling accurate registration. It can significantly improve matching efficiency while maintaining matching accuracy. Attached Figure Description

[0033] Figure 1 This is a flowchart of the present invention;

[0034] Figure 2 This is the initial pose diagram of the point cloud to be matched and the target point cloud in this invention;

[0035] Figure 3 This is the ISS feature extraction map of the stereo geometry descriptor after voxel downsampling in this invention;

[0036] Figure 4 This is a diagram showing the registration result of the point cloud using the method of the present invention. Detailed Implementation

[0037] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0038] like Figure 1 As shown, the ICP point cloud registration method based on the improved ISS-FPFH features of the present invention is as follows:

[0039] Step S1: As Figure 2 As shown, the pose diagram illustrates the point cloud to be matched and the target point cloud before they are matched. Then, an improved normal vector consistency voxel downsampling method is used to process the target point cloud and the point cloud to be matched, resulting in the downsampled target point cloud P and the point cloud to be matched Q. Specifically, the target point cloud and the point cloud to be matched are used as the point clouds to be processed. For each point cloud to be processed, it is divided into several voxel cube meshes of the same size. For each voxel cube mesh, the normal vector of each point in the voxel cube mesh is obtained, and then the average normal vector of all points is obtained. The angle between the normal vector of each point and the average normal vector is obtained, and the point with the smallest angle is retained as the representative point of the voxel cube mesh. Other points in the voxel cube mesh are removed, and finally, the representative points retained from each voxel cube mesh constitute the downsampled point cloud. This method reduces the point cloud density while preserving as much of the original point cloud's geometric features, especially the normal vector information, as possible. This method is particularly important for applications that require subsequent normal vector estimation or feature extraction based on normal vectors (such as the Fast Point Feature Histogram (FPFH) algorithm).

[0040] Step S2: Based on the downsampled target point cloud P and the point cloud to be matched Q, perform 3D geometry descriptor ISS-Fast Point Feature Histogram (FPFH) feature matching to obtain a coarse transformation matrix. Then, transform the downsampled point cloud Q to obtain a coarse registration point cloud Q′. Specifically, the downsampled target point cloud and the point cloud to be matched are used as downsampled point clouds. For each downsampled point cloud, the 3D geometry descriptor ISS algorithm is used to extract key points to obtain a key point set. Then, the Fast Point Feature Histogram (FPFH) algorithm is used to construct a feature descriptor for each key point in the key point set. Based on the feature descriptors of each key point in the two key point sets, the FLANN algorithm (approximate nearest neighbor search library) is used to match the two key point sets to obtain several matching point pairs. Finally, the fast and provable point cloud registration TEASER++ algorithm is used to perform coarse point cloud registration on each pair of matching points to obtain a coarse transformation matrix.

[0041] The ISS (Integrated Solid Geometry Scheme) algorithm specifically involves establishing a local coordinate system for each point in the downsampled point cloud, expanding the search with radius r, constructing a k-dimensional tree (kd-tree) structure for each of the k nearest neighbors, and assigning weights to any found point. Then, the covariance matrix of each keypoint is obtained, and its eigenvalues ​​are derived by decomposing the covariance matrix. A threshold is set; if a point satisfies the constraints, it is considered an ISS feature point, thus obtaining the keypoint set. For example... Figure 3 The image shown is an image of the downsampled point cloud after ISS feature point extraction, with white dots representing the extracted ISS feature points.

[0042] The Fast Point Feature Histogram (FPFH) algorithm specifically involves calculating the relationship with neighboring points and the normal for each keypoint in the keypoint set, obtaining three feature elements related to the angle between the normals, and establishing a simplified point feature histogram (SPFH). The simplified point feature histogram (SPFH) of the keypoint is then calculated together with the simplified feature histogram (SPFH) of the weighted neighboring points to obtain the final fast point feature histogram (FPFH).

[0043] The FLANN algorithm, an approximate nearest neighbor search library, specifically involves: using the FLANN library to build an index of the target point cloud's features; using the features of the source point cloud to query the nearest neighbors; and obtaining the nearest neighbor match for each source point corresponding to the target point.

[0044] The fast and provable point cloud registration TEASER++ algorithm specifically involves taking a pair of corresponding points between two sets of point clouds as input. A measurement map and rotation invariants are constructed from the correspondence, and outlier selection is performed using scale constraints. Translation invariants are constructed to solve for rotation, obtaining the rotation matrix R′. Given the rotation, the translation vector t′ is obtained. Based on the obtained coarse transformation matrix, the downsampled point cloud Q to be matched is finally transformed into the coordinate system of the downsampled target point cloud P, resulting in the coarsely matched point cloud Q′.

[0045] Step S3: After fine registration of the coarsely registered point cloud Q′ using the improved Iterative Closest Point (ICP) algorithm, a fine transformation matrix is ​​obtained. This fine transformation matrix is ​​then used to transform the coarsely registered point cloud Q′ to obtain the finely registered point cloud Q″, thus achieving point cloud registration. The details are as follows:

[0046] Step S3.1: Construct a k-dimensional kd-tree structure for the downsampled target point cloud P to find nearest neighbors in space. The process of constructing the kd-tree structure is as follows: Select one dimension as the "splitting axis" from all dimensions. Usually, the selection is done in turn by dimension. Here, the X-axis is selected first. On the current splitting axis, the points are sorted to find the median point. This point will become the current node. Using the median point as the boundary, the point set is divided into two parts: the left side contains points smaller than the median, and the right side contains points larger than the median. Next, the two subsets are sorted along the Y-axis and Z-axis respectively, and the subsets are further divided according to the median point until each subset has only one point or no points, thus completing the tree construction process.

[0047] Step S3.2: For each coarse matching point in the coarse matching point cloud Q′, find the three nearest points of the coarse matching point in the k-dimensional tree kd-tree structure, and determine whether the three nearest points form a plane. If so, obtain the plane normal vector of the formed plane; otherwise, skip the current coarse matching point until all coarse matching points have been determined.

[0048] Step S3.3: Replace the point-to-point distance constraints in the original Iterative Closest Point ICP algorithm with point-to-surface distance constraints to construct an iterative error function for iteration, thereby obtaining the updated refined transformation matrix. The iterative error function E(R,t) is as follows:

[0049]

[0050] Q″ i =R″·Q′ i +t″

[0051] Where R and t are the rotation matrix and translation vector during the iterative update process, respectively; n is the total number of coarse matching points in the coarse matching point cloud Q′; Q′ i Let q be the i-th coarsely matched point in the coarsely matched point cloud Q′; i For the i-th coarse matching point Q′ i One of the closest points; n i For the i-th coarse matching point Q′ i The plane normal vector of the plane formed by the three nearest points; Q″ i Let R″ be the refined matching point after transformation of the i-th coarse matching point in the coarse matching point cloud Q′ during the iterative update process; R″ and t″ are the rotation matrix and translation vector of the refined transformation matrix during the iterative update process, respectively.

[0052] Step S3.4: During the iterative solution process, determine whether the iterative error function is less than the preset error threshold or whether the maximum number of iterations has been reached. If yes, obtain the final updated fine transformation matrix. If no, update the coarse matching point cloud Q′ according to the currently updated fine transformation matrix and return to step S3.2.

[0053] Step S3.5: Obtain the final updated refined transformation matrix.

[0054] like Figure 4 As shown, the comparison is between the point cloud to be matched and the target point cloud coordinate system after being transformed by the fine matching matrix. It can be seen that the matching effect is good, and there is no ghosting of the wall, column, etc.

[0055] This invention also designs an ICP point cloud registration system based on improved ISS-FPFH features, including a downsampling module, a feature matching module, and a point cloud registration module. The downsampling module is used to process the target point cloud and the point cloud to be matched using an improved normal vector consistent voxel downsampling method to obtain the downsampled target point cloud P and the point cloud to be matched Q. The feature matching module is used to perform solid geometry descriptor ISS-fast point feature histogram FPFH feature matching based on the downsampled target point cloud P and the point cloud to be matched Q to obtain a coarse transformation matrix, and then transforms the downsampled point cloud to be matched Q to obtain a coarse registration point cloud Q′. The point cloud registration module is used to perform fine registration on the coarse registration point cloud Q′ using an improved iterative nearest point ICP algorithm to obtain a fine transformation matrix, and then transforms the coarse registration point cloud Q′ using the fine transformation matrix to obtain a fine registration point cloud Q″, thus realizing point cloud registration.

[0056] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of this application can be implemented using various computer languages. This application is described with flowcharts of methods, systems, and computer program products according to embodiments of this application.

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

[0058] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Therefore, if these modifications and variations of this application fall within the scope of the equivalent technology of this invention, this application also intends to include these modifications and variations.

[0059] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for ICP point cloud registration based on improved ISS-FPFH features, characterized in that, include: Step S1: After processing the target point cloud and the point cloud to be matched using the improved normal vector consistent voxel downsampling method, the downsampled target point cloud P and the point cloud to be matched Q are obtained; Step S2: Based on the downsampled target point cloud P and the point cloud to be matched Q, perform 3D geometry descriptor ISS-Fast Point Feature Histogram FPFH feature matching to obtain a coarse transformation matrix. Then, transform the downsampled point cloud to be matched Q to obtain a coarse registration point cloud Q′. Step S3: After fine registration of the coarse registration point cloud Q′ using the improved iterative nearest point ICP algorithm, a fine transformation matrix is ​​obtained. The fine transformation matrix is ​​then used to transform the coarse registration point cloud Q′ to obtain the fine registration point cloud Q″, thus achieving point cloud registration.

2. The ICP point cloud registration method based on improved ISS-FPFH features according to claim 1, characterized in that: Specifically, step S1 involves taking the target point cloud and the point cloud to be matched as point clouds to be processed, dividing each point cloud into several voxel cube meshes of the same size, obtaining the normal vector of each point in each voxel cube mesh for each voxel cube mesh, obtaining the average normal vector of all points, obtaining the angle between the normal vector of each point and the average normal vector, retaining the point with the smallest angle as the representative point of the voxel cube mesh, and removing other points in the voxel cube mesh. Finally, the representative points retained from each voxel cube mesh constitute the downsampled point cloud.

3. The ICP point cloud registration method based on improved ISS-FPFH features according to claim 1, characterized in that: Specifically, step S2 involves taking the downsampled target point cloud and the point cloud to be matched as downsampled point clouds, extracting key points from each downsampled point cloud using the 3D Geometry Descriptor (ISS) algorithm to obtain a key point set; then constructing a feature descriptor for each key point in the key point set using the Fast Point Feature Histogram (FPFH) algorithm; based on the feature descriptors of each key point in the two key point sets, matching the two key point sets using the Approximate Nearest Neighbor Search (FLANN) algorithm to obtain several matching point pairs; and finally, performing coarse point cloud registration on each pair of matching point pairs using the fast and provable point cloud registration (TEASER++) algorithm to obtain a coarse transformation matrix.

4. The ICP point cloud registration method based on improved ISS-FPFH features according to claim 3, characterized in that: The specific steps of S3 are as follows: Step S3.1: Establish a k-dimensional kd-tree structure for the downsampled target point cloud P; Step S3.2: For each coarse matching point in the coarse matching point cloud Q′, find the three nearest points of the coarse matching point in the k-dimensional tree kd-tree structure, and determine whether the three nearest points form a plane. If so, obtain the plane normal vector of the formed plane; otherwise, skip the current coarse matching point until all coarse matching points have been determined. Step S3.3: Replace the point-to-point distance constraints in the original Iterative Closest Point ICP algorithm with point-to-surface distance constraints to construct an iterative error function for iteration, thereby obtaining an updated fine transformation matrix; Step S3.4: During the iterative solution process, determine whether the iterative error function is less than the preset error threshold or whether the maximum number of iterations has been reached. If yes, obtain the final updated fine transformation matrix. If no, update the coarse matching point cloud Q′ according to the currently updated fine transformation matrix and return to step S3.

2. Step S3.5: Obtain the final updated refined transformation matrix.

5. The ICP point cloud registration method based on improved ISS-FPFH features according to claim 4, characterized in that: In step S3.3, the iterative error function E(R,t) is as follows: Q i ″=R″·Q′ i +t″ Where R and t are the rotation matrix and translation vector during the iterative update process, respectively; n is the total number of coarse matching points in the coarse matching point cloud Q′; Q′ i Let q be the i-th coarsely matched point in the coarsely matched point cloud Q′; i For the i-th coarse matching point Q′ i One of the closest points; n i For the i-th coarse matching point Q′ i The plane normal vector of the plane formed by the three nearest points; Q″ i Let R″ be the refined matching point after transformation of the i-th coarse matching point in the coarse matching point cloud Q′ during the iterative update process; R″ and t″ are the rotation matrix and translation vector of the refined transformation matrix during the iterative update process, respectively.

6. An ICP point cloud registration system based on improved ISS-FPFH features, characterized in that, include: The downsampling module is used to process the target point cloud and the point cloud to be matched using an improved normal vector consistent voxel downsampling method to obtain the downsampled target point cloud P and the point cloud to be matched Q. The feature matching module is used to obtain a coarse transformation matrix after performing 3D geometric shape descriptor ISS-Fast Point Feature Histogram FPFH feature matching based on the downsampled target point cloud P and the point cloud to be matched Q, and then transforming the downsampled point cloud to be matched Q to obtain a coarse registration point cloud Q′. The point cloud registration module is used to perform fine registration on the coarsely registered point cloud Q′ using the improved iterative nearest point ICP algorithm to obtain the fine transformation matrix. The fine transformation matrix is ​​then used to transform the coarsely registered point cloud Q′ to obtain the finely registered point cloud Q″, thus achieving point cloud registration.

7. An electronic device, characterized in that, include: A memory and a processor are coupled to each other, wherein the memory stores program data, and the processor invokes the program data to perform the method as described in any one of claims 1-5.

8. A computer-readable storage medium storing program data thereon, characterized in that, When the program data is executed by the processor, the method as described in any one of claims 1-5 is implemented.

Images