Point pair registration method and system based on feature guidance and dual constraints

Abstract

The application discloses a point pair registration method and system based on feature guidance and double constraints, and belongs to the technical field of computer vision and robot grabbing. The method aims to solve the problems of recognition failure and insufficient estimation accuracy of traditional methods when parts are blocked and stacked, and point cloud features are not obvious. The scheme adopts a two-stage optimization strategy. Firstly, in the coarse registration stage, an adaptive sampling PPF algorithm guided by geometric features is proposed. By preferentially sampling points in the edge, curvature and other feature obvious areas, the robustness and efficiency of matching are improved. Secondly, in the fine registration stage, an ICP algorithm with multi-constraint optimization is proposed. The pose result obtained by coarse registration is used as the initial value. Through rotation and angle constraint, a multi-objective optimization function is constructed to guide the fine registration process to converge in the correct direction and avoid falling into local optimum, thereby ensuring the accuracy and physical feasibility of the final pose estimation. The application significantly improves the accuracy of industrial part pose recognition.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of computer vision, and particularly relates to a point pair registration method and system based on feature guidance and double constraints. BACKGROUND

[0002] In order to realize an efficient production process and intelligent operation, a robot needs to have the ability of recognizing and positioning objects in a dynamic and complex environment. In this context, 6D pose estimation technology, as an effective means of accurately obtaining the position (translation) and orientation (rotation) of objects in three-dimensional space, has become a bridge connecting machine vision and physical execution, involving robot grasping, assembly, handling and other basic tasks, and is gradually becoming one of the key technologies of industrial robot autonomous perception and operation. For example, in the automatic assembly process, the robot needs to accurately grasp and place various parts, which requires real-time and accurate estimation of the pose of these objects.

[0003] At present, with the rapid development of three-dimensional sensing technology, pose estimation algorithms based on point clouds have gradually become a research hotspot. In traditional pose estimation algorithms, global feature matching methods are often helpless in the face of problems such as occlusion and partial target loss in complex scenes because they rely too much on the overall geometric information of objects. Although local feature matching can alleviate the above limitations to some extent, it still has the problem of insufficient stability in the presence of sensor noise interference and a large number of repeated features in the scene, especially when dealing with symmetric objects. At the same time, the actual industrial environment also faces multiple complex challenges: occlusion and stacking between target objects are common, sensor noise interference is significant, and the background environment is chaotic, etc. These factors can greatly reduce the accuracy and robustness of pose registration.

[0004] In addition, industrial parts themselves have significant diversity characteristics, not only in material and shape, but also in surface characteristics. Some parts have rich textures, while others are smooth and textureless. For objects with rich textures, image feature point matching methods can be used to effectively extract 6D pose information. However, for smooth and textureless objects, traditional methods have difficulty in constructing effective features, leading to deviations or even failures in pose estimation. In summary, how to accurately estimate the 6D pose of objects in complex industrial scenes has become a key technical problem that needs to be solved in the field of industrial robots. SUMMARY

[0005] The technical problem solved by the present application is that the existing point cloud registration methods have high computational complexity, large memory consumption, low voting efficiency and multi-target interference in complex industrial scenes, which greatly affects the accuracy of pose estimation. A point pair registration method and system based on feature guidance and double constraints are proposed.

[0006] The technical scheme provided by the application is as follows:

[0007] In a first aspect, a point pair registration method based on feature guidance and double constraints comprises:

[0008] S1, geometric feature guided PPF coarse registration;

[0009] Feature analysis is performed on the point cloud of the target part, key point clouds with significant geometric features are extracted, and invalid point clouds on flat surfaces are filtered out; based on the extracted key point clouds, an adaptive sampling strategy is used to construct a point pair feature set with high distinguishability, and then hash matching of the model and the scene is performed to obtain an initial coarse registration pose;

[0010] Hash matching is one of the core steps of the classic PPF algorithm, and the process is disclosed in the PPF algorithm proposed by Drost et al.: the PPF algorithm maps continuous point pair features to a discrete hash space through a hash table, constructs a mapping relationship from the feature space to the model point pair space, and in the online matching stage, the hash table is searched with the scene point pair feature as the key to realize fast matching of the model and the scene point pair.

[0011] S2, multi-constraint optimization ICP fine registration;

[0012] Taking the coarse registration pose as the initial value, a double constraint of distance constraint mechanism and angle constraint mechanism is constructed to identify and eliminate the error point pairs generated in the hash matching, and pose fine optimization is performed through the iterative closest point algorithm;

[0013] S3, pose output: when the pose fine optimization process converges or reaches the number of iterations, the final 6D pose estimation result is output.

[0014] Further, the extraction process of the key point cloud with significant geometric features is as follows:

[0015] The voxel size adjustment coefficient is constructed by four types of local geometric features of the covariance matrix of the point cloud in the local region, local curvature, normal vector change rate and point density , and the voxel unit edge length is compared, and according to the comparison result, all points in the geometric feature significant region and core points in the transition region are taken as key point clouds:

[0016] When , this region is a geometric feature significant region, i.e. an edge / local curvature mutation region;

[0017] When , this region is a geometric feature insignificant region, i.e. a flat region;

[0018] When , this region is a transition region;

[0019] wherein, represents a set high-priority refinement threshold coefficient, is a set low-priority merging threshold coefficient.

[0020] is a voxel size adjustment coefficient, is a quantitative value obtained by weighted fusion of the three indexes of normalized local curvature, normal vector change rate and point density, and the numerical value directly reflects the geometric feature saliency of the voxel unit. The larger the local curvature, normal vector change rate and point density are, the higher the value is, and the more significant the geometric feature of the voxel is.

[0021] The value of is set based on the geometric scale of the target model itself, and the target model itself is taken as a benchmark, = , which can be fine-tuned according to the actual point cloud density and the precision requirement of the part.

[0022] Further, the specific process of constructing the point pair feature set with high distinguishability by using the adaptive sampling strategy is as follows:

[0023] S11, a fixed-size voxel grid is constructed, and the point cloud data is divided into a voxel grid for fast distribution of the point cloud data;

[0024] S12, the local geometric features of each voxel unit obtained by division are calculated, and discriminative information reflecting local spatial geometric changes, such as covariance matrix, local curvature, normal vector change rate and point density, is extracted from the point cloud data;

[0025] The four types of discriminative information extract the local geometric characteristics of the voxel unit from different dimensions.

[0026] The covariance matrix is calculated by the voxel center and the coordinates of the points in the voxel, the local curvature is solved by the normalized ratio of the eigenvalues of the covariance matrix, the normal vector change rate is calculated by the average angle between the normal vectors of the points in the voxel, and the point density is calculated by the ratio of the number of points in the voxel to the volume of the voxel.

[0027] S13, based on the local geometric features of each voxel unit, the point density and the local curvature and the normal vector change rate are combined to evaluate the importance of the voxel unit by constructing a comprehensive weight function representing the voxel size adjustment coefficient, and the size of the voxel unit is dynamically adjusted to adapt to the geometric characteristics of different regions;

[0028] By utilizing point density, local curvature, and normal vector change rate, we can evaluate the different representations of the significance of voxel geometric features based on these three indicators. Local curvature represents the curvature of the point cloud surface, normal vector change rate represents the directional consistency and geometric complexity of the point cloud, and point density represents the data reliability and sampling sparsity. The three indicators are combined to form a comprehensive measure of voxel units.

[0029] S14. Convert the voxel size adjustment coefficient into voxel priority, and then ensure the smoothness of all voxel boundaries through a global partitioning scheme. Finally, complete the point cloud clustering and merging within the adjusted voxel units.

[0030] The magnitude of the voxel size adjustment factor directly reflects the significance of the geometric features of the voxel unit. The significance of the geometric features is the importance of the voxel, and the level of importance directly corresponds to the level of voxel priority.

[0031] Global partitioning scheme: The global partitioning scheme refers to the overall operation scheme that refines, merges and retains voxel units based on the initially constructed global voxel mesh.

[0032] Cluster merging: Cluster merging is a point cloud simplification operation performed on the selected key point clouds within the adjusted voxel units after global partitioning and boundary smoothing. The core purpose is to remove point cloud redundancy, retain core geometric features, and avoid invalid or duplicate points interfering with the construction of subsequent high-discrimination point pair feature sets.

[0033] After obtaining the voxel priority, voxel re-partitioning is completed through voxel refinement / merging / preservation operations.

[0034] Adjustment coefficient by voxel size Size of the basal voxel Compare. Set. It is a high-priority refined threshold coefficient. This is the low-priority merging threshold coefficient. When At that time, the basic voxel is further refined into smaller daughter voxels. When refining, neighboring base voxels are merged into a larger voxel; otherwise, the voxel size remains unchanged. During the refinement or merging process, the boundaries of the voxel partitions are strictly constrained.

[0035] For the repartitioned voxels, the angle between the normal vectors As a clustering criterion, key point clouds are clustered, and each cluster group retains only one representative point, simplifying the point cloud while preserving core geometric features;

[0036] Finally, point pairs are constructed only between the cluster representative points of high / medium priority voxels, and the four-dimensional PPF features of each point pair are calculated to form a highly discriminative point pair feature set.

[0037] The voxel size adjustment coefficient is a core quantitative indicator characterizing the importance of voxels. It is calculated by combining normalized point density, curvature, and normal vector change rate with preset weighting coefficients. Its value directly reflects the significance of the voxel's geometric features (i.e., voxel importance). Subsequently, this coefficient will be converted into voxel priority, serving as the core criterion for voxel refinement, merging, and retention.

[0038] Furthermore, the formula for the comprehensive weighting function of the voxel size adjustment coefficient is as follows:

[0039]

[0040] in, Let be the voxel size adjustment factor for the k-th voxel unit; is the side length of the voxel unit; Preset weighting coefficients for local curvature; The preset weighting coefficients for the rate of change of the normal vector; Let be the rate of change of the normalized normal vector of the k-th voxel unit; Let be the normalized local curvature of the k-th voxel unit; Let be the normalized point density of the k-th voxel unit.

[0041] The voxel unit is based on the initial voxel size. = = The cube element, with a diameter of the target model Based on the reference, the point cloud of the target part is segmented into parts with a size of The smallest unit of a square voxel grid. , , These represent the basic dimensions of the voxel unit in the x, y, and z directions, respectively.

[0042] Furthermore, the voxel units are re-divided based on the voxel size adjustment factor, as follows:

[0043] when hour, For high priority, the basic voxel unit is refined into smaller sub-voxel units; when hour, For low priority, neighboring basic voxel units are merged into a larger voxel unit; otherwise, the voxel size remains unchanged.

[0044] The magnitude of the voxel size adjustment coefficient represents the significance of the feature, and the significance of the feature corresponds to its priority.

[0045] During refinement: all voxel units are aligned to the boundary of the initial voxel unit mesh and do not exceed the range of the basic voxel unit mesh, and are refined to a maximum of four equal parts;

[0046] During merging: Only complete basic voxel units are allowed to be merged, ensuring that the boundaries of the merged voxel units are aligned with the global voxel grid, and a maximum of four cells can be merged.

[0047] After re-division, the point cloud is clustered and merged based on the normal vectors, using the angle between the normal vectors as the basis. As a clustering criterion, for each pair of point clouds Calculate the cosine of the angle between the normal vectors. ,when At that time, the clouds will be dotted. and Grouped into the same cluster group , The preset clustering threshold is used; after the key point set is obtained by clustering and merging, the four-dimensional PPF features between representative points are calculated to finally obtain the point pair feature set.

[0048] Furthermore, the specific process of constructing a dual constraint mechanism of distance constraint and angle constraint to identify and eliminate erroneous point pairs generated in hash matching, and performing fine-tuning of pose through an iterative nearest-point algorithm, is as follows:

[0049] S21. Perform a rigid body transformation on the initial coarse registration pose to obtain the transformed point cloud as the initial state of ICP registration.

[0050] Rigid body transformation is a fundamental transformation method for 3D pose estimation. It includes rotation transformation (rotation matrix R) and translation transformation (translation vector t). Using the rotation matrix R and translation vector t of coarse registration as parameters, rigid body transformation is performed on the original model point cloud to obtain the transformed model point cloud. This point cloud serves as the initial reference point cloud for ICP fine registration. It does not generate a new point cloud, but rather transforms the coordinate space of the original model point cloud.

[0051] S22. Using the point cloud transformed in step S21 as the reference point cloud, and the target part scene point cloud collected in the scene as the matching object, the K-nearest neighbor search algorithm is used to establish a point pair correspondence between the two sets of point clouds. During the matching process, the key point cloud extracted in step S1 is used as the reference point cloud. Each key point cloud in the scene point cloud is used to accurately find the nearest point in the scene point cloud to form a one-to-one initial point pair set.

[0052] S23. Based on Euclidean distance constraints and normal vector angle constraints, filter out erroneous corresponding point pairs and update the objects to be matched; ensure that the corresponding points entering the optimization stage have high quality and reduce the impact of outliers.

[0053] S24. Based on the point pair set filtered by S23, construct a nonlinear least squares error equation and solve for the pose. Repeat S22-S24 to iteratively optimize and obtain the current optimal pose.

[0054] The iteration termination condition for fine registration in multi-constraint optimization ICP is that the pose transformation amount is less than a preset threshold or the maximum number of iterations is reached. When the pose transformation amount (translation / rotation angle) in a single iteration is less than the preset threshold, it indicates that the pose estimation result has converged and the iteration terminates; or when the number of iterations of the algorithm reaches the preset maximum number of iterations, it is forcibly terminated and the current optimal pose is output.

[0055] Furthermore, the specific process of filtering out erroneous corresponding point pairs and updating the object to be matched based on Euclidean distance constraints and normal vector angle constraints is as follows:

[0056] Record points The coordinates of the two points are The initial three-dimensional Euclidean distance between the matched point pairs is L, and this Euclidean distance is used as a distance constraint.

[0057]

[0058] Set the initial point pairs that satisfy L> Pairs of points L ≤ 0 are identified as erroneous and discarded directly; only pairs of points L ≤ 0 are retained. The point pairs are then subjected to subsequent verification of normal vector angle constraints to ensure the spatial consistency between the point pairs. This indicates that a distance threshold has been preset. For the target point in the point cloud after rigid body transformation of the target part point cloud 3D coordinates; Matching points in scene point cloud 3D coordinates;

[0059] Constraints are set by the angle between the normal vectors, denoted as... The normal vectors are respectively The included angle is ;

[0060]

[0061] when The angle is mapped to the interval [0°, 90°] to eliminate the ambiguity caused by the directionality of the normal vector, thereby realizing the angle constraint.

[0062] Distance and angle are combined to constrain feature points so that both distance and angle are less than a set threshold.

[0063]

[0064] in , These are the distance threshold and the angle threshold, respectively.

[0065] Furthermore, when performing fine-grained pose optimization using the iterative nearest point algorithm, a nonlinear least squares error equation is constructed using the selected set of point pairs and iterative optimization is performed. In each iteration, the rotation matrix and translation matrix are updated. The iteration stops when the pose transformation is less than the set pose transformation convergence threshold or when the maximum number of iterations is reached.

[0066] The pose transformation convergence threshold is a preset critical value used to determine whether the iteration has converged when the Iterative Closest Point (ICP) algorithm performs fine-grained pose optimization. This threshold includes translation transformation threshold and rotation transformation threshold, which are preset manually based on experience.

[0067] After the iteration is completed, the final optimized pose transformation matrix T is output, which is the accurate registration result of the model point cloud relative to the scene point cloud.

[0068] Furthermore, the covariance matrix is ​​used to quantify the spatial distribution characteristics within a local region of the point cloud:

[0069]

[0070] Where C is the covariance matrix of the voxel unit, and N is the total number of point cloud points in the current voxel unit. Let i be the three-dimensional coordinate vector of the i-th point within the current voxel. Let N be the centroid of the voxel grid, and N be the number of voxel points;

[0071] The local curvature, calculated using the normalized ratio of the eigenvalues ​​of the covariance matrix, is an indicator used to distinguish between flat / abrupt regions in the point cloud.

[0072]

[0073] in, Indicates local curvature. , as well as These represent the eigenvalues ​​of the covariance matrix after eigendecomposition, arranged in ascending order.

[0074] High curvature: local curvature The value is relatively large. Compared to Larger, the point cloud exhibits strong three-dimensional features, corresponding to regions with significant geometric features such as edges and abrupt changes in surface curvature;

[0075] Low curvature: local curvature The value is extremely small, the smallest eigenvalue much smaller Point clouds are distributed in a two-dimensional planar pattern, corresponding to flat surfaces and other areas where geometric features are not significant.

[0076] In practical applications, quantification can be achieved by setting a curvature threshold. κ greater than the threshold indicates a high curvature region, while κ less than the threshold indicates a low curvature region.

[0077] The rate of change of the normal vector is characterized by the statistical analysis of the average angle between the normal vectors of points within the voxel, representing the local geometric complexity.

[0078]

[0079] in, is the rate of change of the normal vector of the voxel unit, and N is the number of point cloud points in the current voxel unit; Let be the unit normal vector of the i-th point within the current voxel; This is the unit normal vector of the j-th point within the current voxel;

[0080] The point density is calculated as the ratio of the number of points within a voxel to the volume of the voxel, and is used to evaluate the sparsity of the point cloud distribution.

[0081]

[0082] in, This represents the side length of a voxel unit.

[0083] Secondly, a system employing the aforementioned feature-guided and dual-constraint-based point-to-point registration method includes:

[0084] The geometric feature-guided PPF coarse registration module performs feature analysis on the point cloud of the target part, extracts key point clouds with significant geometric features, and filters out invalid point clouds on flat surfaces. Based on the extracted key point clouds, an adaptive sampling strategy is used to construct a set of point-pair features with high discriminativeness. Then, hash matching between the model and the scene is performed to obtain the initial coarse registration pose.

[0085] Multi-constraint optimized ICP fine registration module: Using the coarse registration pose as the initial value, a dual constraint mechanism of distance constraint mechanism and angle constraint mechanism is constructed to identify and eliminate erroneous point pairs generated in hash matching, and pose fine optimization is performed through iterative nearest point algorithm;

[0086] Pose output module: When the pose refinement optimization process converges or reaches the number of iterations, it outputs the final 6D pose estimation result.

[0087] The technical effects of the technical solution provided by this invention compared to the prior art are as follows:

[0088] (1) Stronger anti-interference and robustness: In response to the problems of parts being occluded and stacked in industrial scenarios and point cloud features not being obvious, geometric features are used to guide sampling in significant areas such as edges and curvature abrupt changes, which greatly improves the matching stability in complex scenarios and avoids recognition failure.

[0089] (2) Better registration efficiency and resource consumption: Adaptive voxel sampling strategy is adopted to refine feature regions, merge flat and redundant regions, filter out invalid point clouds, reduce computational complexity and memory consumption, and make hash matching and iterative optimization faster.

[0090] (3) Significantly improved pose accuracy and convergence: The fine registration stage adds dual constraints of distance and angle to accurately eliminate erroneous matching point pairs, avoids the traditional ICP algorithm from getting stuck in local optima, makes the pose convergence direction more accurate, and finally the 6D pose estimation accuracy is higher and the physical feasibility is stronger. Attached Figure Description

[0091] Figure 1 This is an overall flowchart of the present invention;

[0092] Figure 2 The flowchart for the sub-steps of step S1 is shown below.

[0093] Figure 3 The images show a comparison before and after adaptive downsampling of the model. (a) is the point cloud map of the model, and (b) is the point cloud map after adaptive downsampling.

[0094] Figure 4 The images are scene point cloud diagrams: (a) is the DB9-virtual scene point cloud, and (b) is the DB9-real scene point cloud.

[0095] Figure 5 This is a graph showing the clustering results;

[0096] Figure 6 The following are visualizations of point cloud maps after coarse registration in the embodiments: (a) is the visualization result of DB9-virtual coarse registration, and (b) is the visualization result of DB9-real coarse registration.

[0097] Figure 7 The flowchart for the sub-steps of step S2 is shown below.

[0098] Figure 8 The following are visualizations of point cloud images after fine registration in the examples: (a) is the visualization result of DB9-virtual fine registration, and (b) is the visualization result of DB9-real fine registration. Detailed Implementation

[0099] The present invention will be further described below with reference to the accompanying drawings and embodiments. However, this should not be construed as limiting the scope of the above-described subject matter of the present invention to the following embodiments; all technologies implemented based on the content of the present invention fall within the scope of the present invention.

[0100] like Figure 1 As shown, this embodiment of the invention uses a self-made dataset DB9 based on a nine-pin serial port model, and provides a point-to-point registration method based on feature guidance and dual constraints, including:

[0101] S1. Geometric feature-guided PPF coarse registration;

[0102] Feature analysis is performed on the point cloud of the target part to extract key point clouds with significant geometric features, and invalid point clouds on flat surfaces are filtered out. Based on the extracted key point clouds, an adaptive sampling strategy is used to construct a highly discriminative set of point-pair features. Then, hash matching between the model and the scene is performed to obtain the initial coarse registration pose, and the corresponding point cloud is shown below. Figure 3 As shown;

[0103] Hash matching is one of the core steps of the classic PPF algorithm. The process has been disclosed in the PPF algorithm proposed by Drost et al.: The PPF algorithm maps continuous point-pair features to discrete hash space through a hash table, constructs the mapping relationship between feature space and model point-pair space, and retrieves the hash table with scene point-pair features as the key in the online matching stage to achieve fast matching between model and scene point pairs.

[0104] Specifically, the original point cloud is first denoised by using statistical filtering to remove outliers and retain valid point cloud data. Then, adaptive sampling is performed on the point cloud, and the sampling results are as follows: Figure 4 .

[0105] Set the initial voxel size to , , and with the target model diameter Divide into dimensions based on the reference. A square voxel mesh is used to reduce the complexity of the point cloud over a large area by decomposing it into a series of smaller local units. .

[0106]

[0107] Then, based on the divided voxel units, the local geometric features of each voxel unit are calculated, and discriminative information reflecting local spatial geometric changes is extracted from the point cloud data: covariance matrix, local curvature, rate of change of normal vector, and point density.

[0108] The covariance matrix is ​​used to quantify the spatial distribution characteristics of a local region of the point cloud:

[0109]

[0110] Where C is the covariance matrix of the voxel unit, and N is the number of voxel points. Let i be the three-dimensional coordinate vector of the i-th point within the current voxel. Let N be the centroid of the voxel grid, and N be the number of voxel points;

[0111] The eigenvalues ​​and eigenvectors of the covariance matrix provide important geometric information. The eigenvalues ​​reflect the intensity of the point cloud distribution along the principal directions, while the eigenvectors describe the specific orientation of these directions. Based on the relative magnitudes of the eigenvalues, the morphological characteristics of the point cloud within a voxel can be determined, such as a two-dimensional distribution in a planar region, a one-dimensional distribution in a linear region, and a nearly uniform distribution in a spherical region.

[0112] Eigenvalues ​​of the covariance matrix are obtained by eigenvalue decomposition in ascending order. , , Among them, the smallest eigenvalue The corresponding eigenvector is the direction of the normal vector of the point cloud.

[0113] The local curvature, calculated using the normalized ratio of the eigenvalues ​​of the covariance matrix, is an indicator used to distinguish between flat / abrupt regions in the point cloud.

[0114]

[0115] in, Indicates local curvature. , as well as These represent the eigenvalues ​​of the covariance matrix after eigendecomposition, arranged in ascending order.

[0116] High curvature: local curvature The value is relatively large. Compared to Larger, the point cloud exhibits strong three-dimensional features, corresponding to regions with significant geometric features such as edges and abrupt changes in surface curvature;

[0117] Low curvature: local curvature The value is extremely small, the smallest eigenvalue much smaller Point clouds are distributed in a two-dimensional planar pattern, corresponding to flat surfaces and other areas where geometric features are not significant.

[0118] In practical applications, quantification can be achieved by setting a curvature threshold. κ greater than the threshold indicates a high curvature region, while κ less than the threshold indicates a low curvature region.

[0119] The rate of change of the normal vector: by statistically analyzing the average angle between the normal vectors of points within the voxel, it characterizes the local geometric complexity and can distinguish between flat regions and high feature density regions.

[0120]

[0121] in, is the rate of change of the normal vector of the voxel unit, and N is the number of point cloud points in the current voxel unit; Let be the unit normal vector of the i-th point within the current voxel; This is the unit normal vector of the j-th point within the current voxel;

[0122] The point density is calculated as the ratio of the number of points within a voxel to the volume of the voxel, and is used to evaluate the sparsity of the point cloud distribution.

[0123]

[0124] in, This represents the side length of a voxel unit.

[0125] Point density The size of the point cloud directly reflects the density of the local point cloud: in high-density areas, the point cloud is more concentrated, usually corresponding to important surface details of the model or areas with low noise, while in low-density areas, the point cloud is sparse, which may indicate that the local area is less important, or that it is a missing area due to factors such as noise or occlusion.

[0126] After obtaining the extracted features, the point density is... and local curvature and the rate of change of the normal vector By combining these methods, a comprehensive weighting function is constructed to more fully evaluate the importance of voxels and dynamically adjust voxel sizes to adapt to the geometric characteristics of different regions. Specifically, the point density is first... Used as a key parameter to reflect the sparsity of point distribution, high-density regions indicate higher redundancy in point cloud data, while low-density regions typically contain more important geometric details. Secondly, it is combined with local curvature. High curvature regions correspond to complex boundaries or sharp surfaces, and these regions are crucial to the integrity of the model's geometric information. Therefore, it is necessary to reduce the voxel size to ensure that more feature points are retained. Conversely, low curvature regions, such as flat surfaces, can appropriately increase the voxel size to reduce redundant points while maintaining downsampling efficiency. Furthermore, the rate of change of the normal vector... This is used to measure the consistency of the distribution of normal vectors of points within a voxel. When the rate of change is large, it indicates that the voxel may cross important boundaries or complex surfaces. In this case, the voxel should be refined first to capture more local information. When the rate of change is small, the distribution of normal vectors of points within the voxel is relatively consistent. It can be considered that the geometric characteristics of this region are simple, and the voxel size can be appropriately increased.

[0127] The formula for the comprehensive weighting function of the voxel size adjustment coefficient is as follows:

[0128]

[0129] in, Let be the voxel size adjustment factor for the k-th voxel unit; is the side length of the voxel unit; Preset weighting coefficients for local curvature; The preset weighting coefficients for the rate of change of the normal vector; Let be the rate of change of the normalized normal vector of the k-th voxel unit; Let be the normalized local curvature of the k-th voxel unit; Let be the normalized point density of the k-th voxel unit.

[0130] The voxel unit is based on the initial voxel size. = = The cube element, with a diameter of the target model Based on the reference, the point cloud of the target part is segmented into parts with a size of The smallest unit of a square voxel grid. , , These represent the basic dimensions of the voxel unit in the x, y, and z directions, respectively.

[0131] The extraction process of the key point cloud with significant geometric features is as follows:

[0132] The voxel size adjustment coefficient is constructed using four types of local geometric features of the point cloud in a local region: covariance matrix, local curvature, rate of change of normal vector, and point density. The size of the basal voxel A comparison is made, and based on the comparison results, all points in the geometrically salient regions and the core points in the transition regions are selected as the key point cloud:

[0133] when At this time, this region is a region with significant geometric features, that is, a region with abrupt changes in edge / local curvature;

[0134] when At this time, the region is a region with insignificant geometric features, that is, a flat region;

[0135] when At that time, this area is a transitional area;

[0136] in, This indicates the set high-priority refinement threshold coefficient. The set low-priority merging threshold coefficient.

[0137] The voxel size adjustment coefficient is a quantized value obtained by weighting and fusing three indicators: normalized local curvature, rate of change of normal vector, and point density. Its magnitude directly reflects the significance of the geometric features of the voxel unit. The larger the local curvature, rate of change of normal vector, and point density, the more significant the result. The higher the value, the more pronounced the geometric features of the voxel.

[0138] The value is set based on the geometric scale of the target model itself, using the target model's own dimensions as a reference. = , It can be finely adjusted according to the actual point cloud density and the precision requirements of the parts.

[0139] The specific process of constructing a highly discriminative point-pair feature set using an adaptive sampling strategy is as follows: Figure 2 As shown:

[0140] S11. Construct a fixed-size voxel grid to divide the point cloud data into voxel grids for rapid allocation of point cloud data.

[0141] S12. Calculate the local geometric features of each voxel unit obtained by division, and extract discriminative information reflecting local spatial geometric changes from the point cloud data: covariance matrix, local curvature, rate of change of normal vector and point density;

[0142] Four types of discriminative information extract the local geometric properties of voxel units from different dimensions.

[0143] The covariance matrix is ​​calculated using the coordinates of the voxel's center and in-voxel points. The local curvature is solved by the normalized ratio of the eigenvalues ​​of the covariance matrix. The rate of change of the normal vector is calculated by the average angle between the normal vectors of the in-voxel points. The point density is calculated by the ratio of the number of in-voxel points to the volume of the voxel.

[0144] S13. Based on the local geometric features of each voxel unit, the point density is... and local curvature and the rate of change of the normal vector In combination, the importance of voxel units is evaluated by constructing a comprehensive weighting function that represents the voxel size adjustment coefficient, and the size of voxel units is dynamically adjusted to adapt to the geometric characteristics of different regions.

[0145] By utilizing point density, local curvature, and normal vector change rate, we can evaluate the different representations of the significance of voxel geometric features based on these three indicators. Local curvature represents the curvature of the point cloud surface, normal vector change rate represents the directional consistency and geometric complexity of the point cloud, and point density represents the data reliability and sampling sparsity. The three indicators are combined to form a comprehensive measure of voxel units.

[0146] S14, such as Figure 3 As shown, the voxel size adjustment coefficient is converted into voxel priority, and then the global partitioning scheme ensures that the boundaries of all voxels are smooth. Finally, point cloud clustering and merging are completed within the adjusted voxel units.

[0147] The magnitude of the voxel size adjustment factor directly reflects the significance of the geometric features of the voxel unit. The significance of the geometric features is the importance of the voxel, and the level of importance directly corresponds to the level of voxel priority.

[0148] Global partitioning scheme: The global partitioning scheme refers to the overall operation scheme that refines, merges and retains voxel units based on the initially constructed global voxel mesh.

[0149] Cluster merging: Cluster merging is a point cloud simplification operation performed on the selected key point clouds within the adjusted voxel units after global partitioning and boundary smoothing. The core purpose is to remove point cloud redundancy, retain core geometric features, and avoid invalid or duplicate points interfering with the construction of subsequent high-discrimination point pair feature sets.

[0150] After obtaining the voxel priority, voxel re-partitioning is completed through voxel refinement / merging / preservation operations.

[0151] Adjustment coefficient by voxel size Size of the basal voxel Compare. Set. It is a high-priority refined threshold coefficient. This is the low-priority merging threshold coefficient. When At that time, the basic voxel is further refined into smaller daughter voxels. When refining, neighboring base voxels are merged into a larger voxel; otherwise, the voxel size remains unchanged. During the refinement or merging process, the boundaries of the voxel partitions are strictly constrained.

[0152] For the repartitioned voxels, the angle between the normal vectors As a clustering criterion, key point clouds are clustered, and each cluster group retains only one representative point, simplifying the point cloud while preserving core geometric features;

[0153] Finally, point pairs are constructed only between the cluster representative points of high / medium priority voxels, and the four-dimensional PPF features of each point pair are calculated to form a highly discriminative point pair feature set.

[0154] The voxel size adjustment coefficient is a core quantitative indicator characterizing the importance of voxels. It is calculated by combining normalized point density, local curvature, and normal vector change rate with preset weighting coefficients. Its value directly reflects the significance of the voxel's geometric features (i.e., voxel importance). Subsequently, this coefficient will be converted into voxel priority, serving as the core criterion for voxel refinement, merging, and retention.

[0155] In the adjusted voxel unit, points are further clustered and merged based on normal vectors, using the angle between the normal vectors. As a clustering criterion, for each pair of points Calculate the cosine of the angle between the normal vectors. ,when ( When the preset clustering threshold is used, the points are... and Grouped into the same cluster group After completing the clustering, the weighted average point and normal vector are calculated for each group, where the weighted average point... This indicates the geometric center of the group.

[0156]

[0157] Weighted average normal vector This represents the global normal property, and the calculation is... Normalization is required.

[0158]

[0159] Ultimately, only one point is retained in each cluster group. and its corresponding normalized normal vector This reduces the number of points, simplifies point cloud representation, and retains key geometric information. For example... Figure 5 As shown.

[0160] Finally, the optimal clustering result is selected to obtain the coarse registration pose result, such as... Figure 6 As shown.

[0161] S2, ICP fine registration with multi-constraint optimization;

[0162] Using the coarse registration pose as the initial value, a dual constraint mechanism of distance constraint and angle constraint is constructed to identify and eliminate erroneous point pairs generated in hash matching, and the pose is finely optimized by iterative nearest point algorithm;

[0163] The specific process of constructing a dual constraint mechanism of distance and angle to identify and eliminate erroneous point pairs generated in hash matching, and performing fine-tuning of pose through an iterative nearest-point algorithm, is as follows:Figure 7 As shown:

[0164] S21. Perform a rigid body transformation on the initial coarse registration pose to obtain the transformed point cloud as the initial state of ICP registration.

[0165] Rigid body transformation is a fundamental transformation method for 3D pose estimation. It includes rotation transformation (rotation matrix R) and translation transformation (translation vector t). Using the rotation matrix R and translation vector t of coarse registration as parameters, rigid body transformation is performed on the original model point cloud to obtain the transformed model point cloud. This point cloud serves as the initial reference point cloud for ICP fine registration. It does not generate a new point cloud, but rather transforms the coordinate space of the original model point cloud.

[0166] The rotation matrix is:

[0167]

[0168] S22. Using the point cloud transformed in step S21 as the reference point cloud, and the target part scene point cloud collected in the scene as the matching object, the K-nearest neighbor search algorithm is used to establish a point pair correspondence between the two sets of point clouds. During the matching process, the key point cloud extracted in step S1 is used as the reference point cloud. Each key point cloud in the scene point cloud is used to accurately find the nearest point in the scene point cloud to form a one-to-one initial point pair set.

[0169] S23. Based on Euclidean distance constraints and normal vector angle constraints, filter out erroneous corresponding point pairs and update the objects to be matched; ensure that the corresponding points entering the optimization stage have high quality and reduce the impact of outliers.

[0170] The specific process of filtering out erroneous point pairs and updating the objects to be matched based on Euclidean distance constraints and normal vector angle constraints is as follows:

[0171] Record points The coordinates of the two points are The initial three-dimensional Euclidean distance between the matched point pairs is L, and this Euclidean distance is used as a distance constraint.

[0172]

[0173] Set the initial point pairs that satisfy L> Pairs of points L ≤ 0 are identified as erroneous and discarded directly; only pairs of points L ≤ 0 are retained. The point pairs are then subjected to subsequent verification of normal vector angle constraints to ensure the spatial consistency between the point pairs. This indicates that a distance threshold has been preset. For the target point in the point cloud after rigid body transformation of the target part point cloud 3D coordinates; Matching points in scene point cloud 3D coordinates;

[0174] Constraints are set by the angle between the normal vectors, denoted as... The normal vectors are respectively The included angle is ;

[0175]

[0176] when The angle is mapped to the interval [0°, 90°] to eliminate the ambiguity caused by the directionality of the normal vector, thereby realizing the angle constraint.

[0177] Distance and angle are combined to constrain feature points so that both distance and angle are less than a set threshold.

[0178] in , These are the distance threshold and the angle threshold, respectively.

[0179] S24. Based on the point pair set filtered in S23, construct a nonlinear least squares error equation and solve for the pose. Repeat S22-S24, iteratively optimizing to obtain the current optimal pose. Figure 8 As shown.

[0180] The iteration termination condition for fine registration in multi-constraint optimization ICP is that the pose transformation amount is less than a preset threshold or the maximum number of iterations is reached. When the pose transformation amount (translation / rotation angle) in a single iteration is less than the preset threshold, it indicates that the pose estimation result has converged and the iteration terminates; or when the number of iterations of the algorithm reaches the preset maximum number of iterations, it is forcibly terminated and the current optimal pose is output.

[0181] When performing fine-grained pose optimization using the iterative nearest point algorithm, a nonlinear least squares error equation is constructed using the selected set of point pairs and iterative optimization is performed. In each iteration, the rotation matrix and translation matrix are updated. The iteration stops when the pose transformation is less than the set pose transformation convergence threshold or when the maximum number of iterations is reached.

[0182] The pose transformation convergence threshold is a preset critical value used to determine whether the iteration has converged when the Iterative Closest Point (ICP) algorithm performs fine-grained pose optimization. This threshold includes translation transformation threshold and rotation transformation threshold, which are preset manually based on experience.

[0183] After the iteration is completed, the final optimized pose transformation matrix T is output, which is the accurate registration result of the model point cloud relative to the scene point cloud.

[0184] S3, Pose Output: When the pose refinement optimization process converges or reaches the number of iterations, the final 6D pose estimation result is output.

[0185] Example 2

[0186] A system employing the aforementioned feature-guided and dual-constraint point-to-point registration method includes:

[0187] The geometric feature-guided PPF coarse registration module performs feature analysis on the point cloud of the target part, extracts key point clouds with significant geometric features, and filters out invalid point clouds on flat surfaces. Based on the extracted key point clouds, an adaptive sampling strategy is used to construct a set of point-pair features with high discriminativeness. Then, hash matching between the model and the scene is performed to obtain the initial coarse registration pose.

[0188] Multi-constraint optimized ICP fine registration module: Using the coarse registration pose as the initial value, a dual constraint mechanism of distance constraint mechanism and angle constraint mechanism is constructed to identify and eliminate erroneous point pairs generated in hash matching, and pose fine optimization is performed through iterative nearest point algorithm;

[0189] Pose output module: When the pose refinement optimization process converges or reaches the number of iterations, it outputs the final 6D pose estimation result.

[0190] It should also be understood that the specific implementation process of each module is described in the above method. This invention will not repeat it here. The above division of functional modules is only for illustrative purposes. In some embodiments, some functional modules can be combined and some functional modules can be separated. Each functional module can be implemented in software, hardware, or a combination of software and hardware. The software and hardware devices include, but are not limited to, general-purpose computer equipment, programmable gate arrays, digital signal processors, microprocessors and their corresponding programming or burning software.

[0191] Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned readable storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0192] 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 embodied on one or more computer-readable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. This application refers to flowchart illustrations and / or instructions executed by a processor of a method, apparatus (system), and computer program product according to embodiments of this application to create means for implementing the functions specified in one or more flowchart illustrations and / or one or more block diagrams. These computer program instructions may also be stored in a computer-readable storage medium capable of directing a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means that implement the functions specified in one or more flowchart illustrations and / or one or more block diagrams. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, such that the instructions, which execute on the computer or other programmable apparatus, provide steps for implementing the functions specified in one or more flowcharts and / or one or more blocks of a block diagram.

[0193] The above embodiments are preferred embodiments of this application. Those skilled in the art can make various changes or improvements based on them. Without departing from the overall concept of this application, these changes or improvements should fall within the scope of protection claimed in this application.

Claims

1. A point-to-point registration method based on feature guidance and dual constraints, characterized in that, include: S1. Geometric feature-guided PPF coarse registration; Feature analysis is performed on the point cloud of the target part to extract key point clouds with significant geometric features, and invalid point clouds on flat surfaces are filtered out. Based on the extracted key point clouds, an adaptive sampling strategy is used to construct a set of point-pair features with high discriminativeness. Then, hash matching between the model and the scene is performed to obtain the initial coarse registration pose. S2, ICP fine registration with multi-constraint optimization; Using the coarse registration pose as the initial value, a dual constraint mechanism of distance constraint and angle constraint is constructed to identify and eliminate erroneous point pairs generated in hash matching, and the pose is finely optimized by iterative nearest point algorithm; S3, Pose Output: When the pose refinement optimization process converges or reaches the number of iterations, the final 6D pose estimation result is output. The extraction process of the key point cloud with significant geometric features is as follows: The voxel size adjustment coefficient is constructed using four types of local geometric features of the point cloud in a local region: covariance matrix, local curvature, rate of change of normal vector, and point density. , with the side length of the voxel unit A comparison is made, and based on the comparison results, all points in the geometrically salient regions and the core points in the transition regions are selected as the key point cloud: when At this time, this region is a region with significant geometric features, that is, a region with abrupt changes in edge / local curvature; when At this time, the region is a region with insignificant geometric features, that is, a flat region; when At that time, this area is a transitional area; in, This indicates the set high-priority refinement threshold coefficient. The set low-priority merging threshold coefficient; The specific process of constructing a highly discriminative point-pair feature set using an adaptive sampling strategy is as follows: S11. Construct a fixed-size voxel grid to divide the point cloud data into voxel grids for rapid allocation of point cloud data. S12. Calculate the local geometric features of each voxel unit obtained by division, and extract discriminative information reflecting local spatial geometric changes from the point cloud data: covariance matrix, local curvature, rate of change of normal vector and point density; S13. Based on the local geometric features of each voxel unit, the point density is... and local curvature and the rate of change of the normal vector In combination, the importance of voxel units is evaluated by constructing a comprehensive weighting function that represents the voxel size adjustment coefficient, and the size of voxel units is dynamically adjusted to adapt to the geometric characteristics of different regions. S14. Convert the voxel size adjustment coefficient into voxel priority, and then ensure the smoothness of all voxel boundaries through a global partitioning scheme. Finally, complete the point cloud clustering and merging within the adjusted voxel units. The specific process of constructing a dual constraint mechanism of distance constraint and angle constraint to identify and eliminate erroneous point pairs generated in hash matching, and performing fine-tuning of pose through an iterative nearest point algorithm is as follows: S21. Perform a rigid body transformation on the initial coarse registration pose to obtain the transformed point cloud as the initial state of ICP registration. S22. Using the point cloud transformed in step S21 as the reference point cloud, and the target part scene point cloud collected in the scene as the matching object, the K-nearest neighbor search algorithm is used to establish a point pair correspondence between the two sets of point clouds. During the matching process, the key point cloud extracted in step S1 is used as the reference point cloud. Each key point cloud in the scene point cloud is used to accurately find the nearest point in the scene point cloud to form a one-to-one initial point pair set. S23. Based on Euclidean distance constraints and normal vector angle constraints, filter out incorrect corresponding point pairs and update the objects to be matched; S24. Based on the point pair set filtered by S23, construct a nonlinear least squares error equation and solve for the pose. Repeat S22-S24 to iteratively optimize and obtain the current optimal pose.

2. The method according to claim 1, characterized in that, The formula for the comprehensive weighting function of the voxel size adjustment coefficient is as follows: ； in, Let be the voxel size adjustment factor for the k-th voxel unit; is the side length of the voxel unit; Preset weighting coefficients for local curvature; The preset weighting coefficients for the rate of change of the normal vector; Let be the rate of change of the normalized normal vector of the k-th voxel unit; Let be the normalized local curvature of the k-th voxel unit; Let be the normalized point density of the k-th voxel unit.

3. The method according to claim 2, characterized in that, The voxel units are re-divided based on the voxel size adjustment factor, as follows: when hour, For high priority, the basic voxel unit is refined into smaller sub-voxel units; when hour, For low priority, neighboring basic voxel units are merged into a larger voxel unit; otherwise, the voxel size remains unchanged.

4. The method according to claim 1, characterized in that, The specific process of filtering out erroneous point pairs and updating the objects to be matched based on Euclidean distance constraints and normal vector angle constraints is as follows: Record points The coordinates of the two points are The initial three-dimensional Euclidean distance between the matched point pairs is L, and L is used as the distance constraint. ； Set the initial point pairs that satisfy L> Pairs of points L ≤ 0 are identified as erroneous and discarded directly; only pairs of points L ≤ 0 are retained. The point pairs are then subjected to subsequent verification of normal vector angle constraints to ensure the spatial consistency between the point pairs. This indicates that a distance threshold has been preset. For the target point in the point cloud after rigid body transformation of the target part point cloud 3D coordinates; Matching points in scene point cloud 3D coordinates; Constraints are set by the angle between the normal vectors, denoted as... The normal vectors are respectively The included angle is ; ； when The angle is mapped to the interval [0°, 90°] to eliminate the ambiguity caused by the directionality of the normal vector, thereby realizing the angle constraint.

5. The method according to claim 4, characterized in that, When performing fine-grained pose optimization using the iterative nearest point algorithm, a nonlinear least squares error equation is constructed using the selected set of point pairs and iterative optimization is performed. In each iteration, the rotation matrix and translation matrix are updated. The iteration stops when the pose transformation is less than the set pose transformation convergence threshold or when the maximum number of iterations is reached.

6. The method according to claim 1, characterized in that, The covariance matrix is ​​used to quantify the spatial distribution characteristics of a local region of the point cloud: ； Where C is the covariance matrix of the voxel unit, and N is the total number of point cloud points in the current voxel unit. Let i be the three-dimensional coordinate vector of the i-th point within the current voxel. Here, N is the centroid of the voxel mesh, N is the number of voxels, and T is the matrix transpose symbol. The local curvature, calculated using the normalized ratio of the eigenvalues ​​of the covariance matrix, is an indicator used to distinguish between flat / abrupt regions in the point cloud. ； in, Indicates local curvature. , as well as These represent the eigenvalues ​​of the covariance matrix after eigendecomposition, arranged in ascending order. The rate of change of the normal vector is characterized by the statistical analysis of the average angle between the normal vectors of points within the voxel, representing the local geometric complexity. ； in, is the rate of change of the normal vector of the voxel unit, and N is the number of point cloud points in the current voxel unit; Let be the unit normal vector of the i-th point within the current voxel; This is the unit normal vector of the j-th point within the current voxel; The point density is calculated as the ratio of the number of points within a voxel to the volume of the voxel, and is used to evaluate the sparsity of the point cloud distribution. ； in, This represents the side length of a voxel unit.

7. A system employing the point-to-point registration method based on feature guidance and dual constraints as described in any one of claims 1-6, characterized in that, include: The geometric feature-guided PPF coarse registration module performs feature analysis on the point cloud of the target part, extracts key point clouds with significant geometric features, and filters out invalid point clouds on flat surfaces. Based on the extracted key point clouds, an adaptive sampling strategy is used to construct a set of point-pair features with high discriminativeness. Then, hash matching between the model and the scene is performed to obtain the initial coarse registration pose. Multi-constraint optimized ICP fine registration module: Using the coarse registration pose as the initial value, a dual constraint mechanism of distance constraint mechanism and angle constraint mechanism is constructed to identify and eliminate erroneous point pairs generated in hash matching, and pose fine optimization is performed through iterative nearest point algorithm; Pose output module: When the pose refinement optimization process converges or reaches the number of iterations, it outputs the final 6D pose estimation result.

Images