Pose estimation method and device based on point pair feature matching, medium and product

Abstract

The invention discloses a pose estimation method and device based on point pair feature matching, a medium and a product. The method comprises the following steps: performing coarse registration on a fusion scene point cloud and each template point cloud, and calculating point pair features; according to the corresponding hash key values, retrieving from each hash table to establish registration point pairs; calculating a translation vector and a rotation quaternion matched with each registration point pair according to the reference point coordinate system and the template coordinate system, and performing clustering processing on each registration point pair to obtain each initial corresponding set; screening out a target pose candidate transformation matrix and a matched target template point cloud from the pose candidate transformation matrixes matched with the initial corresponding sets respectively; a target optimization function between the fusion scene point cloud and the target template point cloud is constructed, iterative optimization is performed on parameters in the target pose candidate transformation matrix, and the target pose of the fusion scene point cloud is obtained at the end of iteration, so that the precision, real-time performance and reliability of six-dimensional pose estimation are improved.

Description

Technical Field

[0001] This invention relates to the field of computer vision technology, and in particular to a pose estimation method, device, medium, and product based on point-pair feature matching. Background Technology

[0002] Six-dimensional pose estimation of an object, namely its position and orientation in three-dimensional space, has wide application value in many fields such as intelligent manufacturing and automated loading and unloading in the production of roll packaging and sealing machine workshops. In particular, in the automated transfer operation from material pallet to sealing machine hopper, accurately obtaining the spatial position and orientation of the target object relative to the sensor or robot end effector is a prerequisite for achieving precise operation and control.

[0003] In related technologies, pose estimation techniques use point clouds as the core data carrier. This involves pre-constructing a template point cloud, extracting geometric features from the acquired scene point cloud, establishing point-to-point correspondences between the template and the scene, and finally solving the rigid body transformation to obtain the six-dimensional pose. However, this method results in matching information including incorrectly matched point pairs, and the matching results are relatively coarse, slowing down computation and amplifying errors. This makes it difficult to meet the high-precision requirements of pose estimation, reducing its accuracy, stability, and real-time performance. Summary of the Invention

[0004] This invention provides a pose estimation method, device, medium, and product based on point-pair feature matching to solve the problems of low pose estimation accuracy, low precision, low stability, and low real-time performance.

[0005] According to one aspect of the present invention, a pose estimation method based on point-pair feature matching is provided, comprising:

[0006] The fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives is coarsely registered with each pre-stored template point cloud, and the point pair features of each reference point pair after coarse registration are calculated.

[0007] Based on the hash key value corresponding to the features of each point pair, the template point pairs that match each reference point pair are retrieved from each hash table pre-built based on each template point cloud, and registration point pairs are established based on the retrieval results; wherein, each registration point pair includes a fused scene point cloud point and a template point cloud point belonging to the set template point cloud;

[0008] Based on the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair, calculate the translation vector and rotation quaternion that match each registration point pair.

[0009] Based on each translation vector and rotation quaternion, each registration point pair is clustered to obtain each initial correspondence set; wherein, the template point cloud points in each registration point pair of the same initial correspondence set belong to the same template point cloud.

[0010] Among the pose candidate transformation matrices that match each initial corresponding set, the target pose candidate transformation matrix is ​​selected, and the target template point cloud that matches the target pose candidate transformation matrix is ​​obtained.

[0011] A target optimization function is constructed between the fused scene point cloud and the target template point cloud. Based on the target optimization function, the parameters in the target pose candidate transformation matrix are iteratively optimized, and the target pose of the fused scene point cloud is obtained at the end of the iteration.

[0012] According to another aspect of the present invention, a pose estimation apparatus based on point-pair feature matching is provided, comprising:

[0013] The coarse registration module is used to perform coarse registration of the fused scene point cloud obtained by fusing raw point cloud data from multiple views with each pre-stored template point cloud, and to calculate the point pair features of each reference point pair after coarse registration.

[0014] The hash module is used to retrieve template point pairs that match each reference point pair from each hash table pre-built based on each template point cloud according to the hash key value corresponding to the features of each point pair, and to establish registration point pairs according to the retrieval results; wherein, each registration point pair includes a fused scene point cloud point and a template point cloud point belonging to the set template point cloud;

[0015] The translation and rotation module is used to calculate the translation vector and rotation quaternion that match each registration point pair, based on the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair.

[0016] The clustering module is used to cluster each registration point pair according to each translation vector and rotation quaternion to obtain each initial correspondence set; wherein, the template point cloud points in each registration point pair of the same initial correspondence set belong to the same template point cloud.

[0017] The pose matrix module is used to filter out the target pose candidate transformation matrix from the pose candidate transformation matrices that match each initial corresponding set, and to obtain the target template point cloud that matches the target pose candidate transformation matrix.

[0018] The iterative optimization module is used to construct a target optimization function between the fused scene point cloud and the target template point cloud, and to iteratively optimize the parameters in the target pose candidate transformation matrix based on the target optimization function, and obtain the target pose of the fused scene point cloud at the end of the iteration.

[0019] According to another aspect of the present invention, an electronic device is provided, the electronic device comprising:

[0020] At least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores a computer program executable by the at least one processor, the computer program being executed by the at least one processor to enable the at least one processor to perform the pose estimation method based on point-pair feature matching according to any embodiment of the present invention.

[0021] According to another aspect of the present invention, a computer-readable storage medium is provided, the computer-readable storage medium storing computer instructions for causing a processor to execute and implement the pose estimation method based on point-pair feature matching as described in any embodiment of the present invention.

[0022] According to another aspect of the present invention, a computer program product is also provided, including a computer program that, when executed by a processor, implements the steps of the method as described in any embodiment of the present invention.

[0023] The technical solution of this invention involves coarsely registering a fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives with pre-stored template point clouds, and calculating the point pair features of each reference point pair after coarse registration. Based on the hash key values ​​corresponding to the point pair features, template point pairs matching each reference point pair are retrieved from pre-constructed hash tables based on each template point cloud, and registration point pairs are established based on the retrieval results. Finally, the coordinate system of each registration point pair is calculated based on the coordinate system of the reference points corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair. The process involves matching translation vectors and rotation quaternions; clustering each registration point pair based on the translation vectors and rotation quaternions to obtain initial correspondence sets; selecting target pose candidate transformation matrices from the pose candidate transformation matrices matched with each initial correspondence set, and obtaining target template point clouds that match the target pose candidate transformation matrices; constructing a target optimization function between the fused scene point cloud and the target template point cloud, and iteratively optimizing the parameters in the target pose candidate transformation matrix based on the target optimization function, obtaining the target pose of the fused scene point cloud at the end of the iteration. By fusing the original point clouds from multiple perspectives, coarse registration is first performed with the pre-stored template point cloud, and registration point pairs are constructed using a hash table to reduce the registration search space; subsequently, the obtained translation vectors and rotation quaternions are clustered and filtered to retain the target template point clouds that match the target pose candidate transformation matrices, reducing the subsequent optimization search space and providing initial optimization values ​​for subsequent optimization. By iteratively optimizing the parameters, the accuracy, precision, and stability of target pose estimation are improved, ensuring the real-time performance and reliability of six-dimensional pose estimation.

[0024] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0025] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0026] Figure 1 This is a flowchart of a pose estimation method based on point-pair feature matching according to Embodiment 1 of the present invention;

[0027] Figure 2 This is a flowchart of another pose estimation method based on point-pair feature matching provided in Embodiment 2 of the present invention;

[0028] Figure 3 This is a flowchart of another pose estimation method based on point-pair feature matching provided in Embodiment 3 of the present invention;

[0029] Figure 4 This is a schematic diagram of a pose estimation device based on point-pair feature matching according to Embodiment 4 of the present invention;

[0030] Figure 5 This is a schematic diagram of the structure of an electronic device that implements the pose estimation method based on point-pair feature matching according to an embodiment of the present invention. Detailed Implementation

[0031] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0032] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0033] Example 1

[0034] Figure 1 This is a flowchart of a pose estimation method based on point-pair feature matching provided in Embodiment 1 of the present invention. This embodiment is applicable to estimating the pose state of an object. This method can be executed by a pose estimation device based on point-pair feature matching. This pose estimation device based on point-pair feature matching can be implemented in hardware and / or software, and is generally configured in an electronic device. Figure 1 As shown, the method includes:

[0035] S110. The fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives is coarsely registered with each pre-stored template point cloud, and the point pair features of each reference point pair after coarse registration are calculated.

[0036] In this embodiment of the invention, multi-view raw point cloud data can be specifically understood as a collection of point cloud data acquired from different viewpoints or positions by multiple 3D scanning devices such as LiDAR or depth cameras. The point cloud data from different viewpoints contain overlapping redundant information, which may include information from different angles of the target object, noise information in the scene, and point cloud information from non-target areas. Fusing scene point clouds can be specifically understood as reducing noise, removing redundant information, and filling in missing parts by fusing multi-view raw point cloud data, thereby obtaining more complete and accurate point cloud data. For example, based on the relative positions and orientations between viewpoints, point cloud data from multiple viewpoints can be aligned to the same coordinate system, and the aligned point cloud data can be fused into a complete fused scene point cloud using voxel mesh fusion or surface feature-based fusion methods.

[0037] Template point cloud can be understood as a pre-stored standard point cloud model containing precise scanning information of the target object, used for matching and comparison with the fused scene point cloud. Coarse point cloud registration can be understood as the initial alignment process between the fused scene point cloud and the template point cloud. Reference point pairs can be understood as the corresponding point pairs established between the fused scene point cloud and the template point cloud after coarse registration. Point pair features can be understood as features calculated based on feature vectors extracted from the reference point pairs, used to describe the geometric and directional relationships between the point pairs, specifically including curvature and the angle between normal vectors, etc.

[0038] Specifically, from the pre-stored template point clouds, template point clouds that may match the target object are selected. This can be done by extracting the target object's shape features (such as length, width, height, volume, or surface area) or visual appearance features (such as color histograms or texture features), comparing the target object and the template point clouds, and finding template point clouds with high similarity in shape or appearance features for matching. Alternatively, the fused scene point cloud can be directly matched with each of the pre-stored template point clouds, and initial alignment between the fused scene point cloud and the template point cloud is performed based on a fast registration algorithm (such as principal component analysis). Then, features (such as the centroid, bounding box, curvature, or normal vector of the principal plane of the point cloud) are extracted from the template point cloud and the fused scene point cloud. The features of the point cloud are mapped to the parameter space, and the points corresponding to parameters with high feature frequency are statistically analyzed. Alternatively, the distance from each point in the scene point cloud to the centroid of the template point cloud is calculated, and points in the template point cloud that are close to the centroid are statistically analyzed as reference point pairs between the fused scene point cloud and the template point cloud, thus achieving coarse registration of the point clouds.

[0039] In the fused scene point cloud and template point cloud after coarse registration, the point pair features of each reference point pair are calculated, which may include: for point n in the fused scene point cloud i And template point cloud point n j For each pair of reference points, the normal vectors (reflecting the local surface orientation of the point cloud at that point) corresponding to the scene point cloud points and template point cloud points are calculated and fused within the neighborhood of each reference point. i and n j According to θ ij =arccos(n i ·n j Calculate the angle θ between the normal vectors. ij The range is [0, π]. The curvature (reflecting the degree of bending of the point cloud at that point) k is calculated and fused with the curvature of the scene point cloud and the template point cloud based on the distribution of neighboring points. i and k j The point-pair feature f is formed by combining features such as the angle between normal vectors and curvature into an eigenvector form. ij =(θ ij ,k i ,kj ), used to describe the geometric properties of each reference point pair.

[0040] Furthermore, based on the above embodiments, before performing coarse point cloud registration with the pre-stored template point clouds, the following may also be included:

[0041] Unreliable regions in the fused scene point cloud are filtered out based on the point cloud segmentation algorithm and removed from the fused scene point cloud.

[0042] In this embodiment of the invention, unreliable regions in the fused scene point cloud can be specifically understood as regions where feature extraction and matching are prone to errors due to factors such as noise, occlusion, or high curvature. The point cloud segmentation algorithm can be specifically understood as an algorithm for identifying and removing unreliable regions from the fused scene point cloud, which may include algorithms such as plane segmentation algorithms based on normal vector consistency and curvature-based segmentation algorithms.

[0043] Specifically, before performing coarse registration with the pre-stored template point clouds, the normal vector of each point in the fused scene point cloud can be calculated. The point clouds are clustered based on the consistency of the normal vector direction. By setting a threshold for the consistency of the normal vector direction, point clouds belonging to the background plane are clustered and removed. The curvature of each point in the fused scene point cloud is calculated, and threshold segmentation is performed based on the magnitude of the curvature.

[0044] In cigarette production scenarios, fused scene point clouds are susceptible to interference from unreliable regions such as background planes and occlusions, leading to deviations in feature extraction and matching. This results in accumulated pose estimation errors, impacting operational accuracy. By using point cloud segmentation algorithms to filter and remove these unreliable regions according to preset rules, the accuracy of the fused scene point cloud in reflecting the target object's pose can be improved. This enhances the precision and reliability of subsequent feature extraction and matching processes, reduces pose estimation errors, and ultimately improves operational accuracy and efficiency.

[0045] Furthermore, based on the above embodiments, before filtering unreliable regions in the fused scene point cloud based on the point cloud segmentation algorithm and removing them from the fused scene point cloud, the method may further include: performing the same preprocessing operations on both the fused scene point cloud and the pre-stored template point cloud, such as outlier removal and downsampling. Outlier removal eliminates isolated points caused by noise or errors, improving the accuracy of subsequent feature extraction and matching. Downsampling reduces the amount of point cloud data, reducing computational complexity and improving subsequent processing efficiency while maintaining the main features of the target object. Performing the same preprocessing on the template point cloud ensures consistency with the fused scene point cloud in format and accuracy, guaranteeing the accuracy of subsequent matching and estimation. This ensures both data standardization and improved reliability of matching and estimation.

[0046] Optionally, based on the above embodiments, the unreliable regions in the fused scene point cloud may include: the background plane and high curvature edge regions;

[0047] Accordingly, filtering unreliable regions in the fused scene point cloud based on point cloud segmentation algorithms and removing them from the fused scene point cloud can include:

[0048] Based on the random sampling consistency plane fitting algorithm, the target background plane containing the most points in the fused scene point cloud is selected and removed from the fused scene point cloud;

[0049] After removing the target background plane, calculate the local covariance matrix of each point in the fused scene point cloud, obtain the eigenvalues ​​of the local covariance matrix, and construct the contour feature tuples based on the eigenvalues.

[0050] Contour feature clusters are extracted from contour feature tuples using Euclidean clustering. Based on these contour feature clusters, high curvature edge regions are selected and removed from the fused scene point cloud.

[0051] In this embodiment of the invention, the background plane can be specifically understood as a large planar region unrelated to the target object, such as the ground and walls, which occupies a large proportion in the point cloud data and will interfere with the feature extraction and matching of the target object. The high curvature edge region can be specifically understood as an edge part with high curvature, which may be caused by occlusion, sharp edges of objects, or noise. Its geometry is complex and unstable, which will interfere with the feature extraction and matching of the target object.

[0052] Specifically, based on the Random Sample Consensus (RANSAC) plane fitting algorithm, several points are randomly selected from the fused scene point cloud to fit a plane model. Other points in the point cloud are then substituted into this plane model to calculate their distances to the plane. Where (a,b,c,d) are plane parameters, (x i ,y i ,z i Let d be the coordinates of the i-th point in the fused scene point cloud. i Let be the distance from the i-th point in the fused scene point cloud to the fitted plane. Points with a distance less than a set threshold are considered interior points that conform to the plane model. Repeat the above sampling and evaluation process until the preset maximum number of repetitions is reached, and select the plane model containing the most interior points as the target background plane. Remove the points in the fitted target background plane from the fused scene point cloud.

[0053] For each point in the fused scene point cloud after removing the background plane, calculate its local covariance matrix with its neighborhood points. Where N is a point and its neighborhood set, that is, a set of points selected with the current point as the center, which includes the current point and points within a certain range around it. Let p be the centroid of the neighborhood point, which can be obtained by calculating the average coordinates of all points in the neighborhood. Let C be the coordinates of a point in the neighborhood. C is the local covariance matrix of all points in the neighborhood of a given point in the point cloud, relative to the centroid. This matrix reflects the spatial distribution of the neighborhood points, including their dispersion and orientation. Eigenvalue decomposition is performed on the local covariance matrix. Based on the magnitude and relationship of the eigenvalues, contour feature tuples are constructed to describe the local geometric features of the point cloud.

[0054] Typically, a contour feature tuple can be (α, β, γ, δ), where α represents anisotropy, reflecting the dispersion of neighboring points in different directions; β represents linearity, reflecting the linearity of the distribution of neighboring points; γ represents surface transformation, reflecting the uniformity of surface changes in the distribution of neighboring points; and δ represents the maximum angular difference, reflecting the degree of angular change of neighboring points in different directions. δ=max(∠(n f p f ), ∠(n f q f ), where λ1≥λ2≥λ3 are the eigenvalues ​​of the local covariance matrix, and n f p f q f The corresponding eigenvector is the eigenvalue. The magnitude of the eigenvalue reflects the degree of dispersion of the neighborhood points along the direction of the corresponding eigenvector. A larger eigenvalue indicates that the neighborhood points are more dispersed along the direction of the eigenvector, and vice versa.

[0055] Based on the Euclidean distance between points in the point cloud data and using the feature information in the contour feature tuple, points with similar geometric features are clustered into clusters (using the weighted sum of distance and features in the contour feature tuple, considering both the spatial distance between points and the differences between feature vectors), forming contour feature clusters. In the clustered contour feature clusters, edge regions with high curvature are identified (this can be achieved by calculating the curvature of each point and comparing it with a threshold), and these are removed from the fused scene point cloud.

[0056] The RANSAC algorithm is used to filter out and remove the target background plane containing the most points, reducing background interference. By calculating the local covariance matrix of the remaining point cloud, contour feature tuples are constructed to quantify the local geometric characteristics of each point. Then, Euclidean clustering is used to extract contour feature clusters, and high-curvature edge regions are filtered out and removed based on the geometric consistency within the clusters. This ensures that the fused scene point cloud data can accurately reflect the contour features of the target object, improves the data quality of subsequent pose estimation, enhances the accuracy and reliability of pose estimation, and ensures the precision of automated operations.

[0057] S120. Based on the hash key value corresponding to the features of each point pair, retrieve the template point pairs that match each reference point pair from each hash table pre-built based on each template point cloud, and establish registration point pairs based on the retrieval results.

[0058] Each registration point pair includes a fused scene point cloud point and a template point cloud point belonging to a set template point cloud.

[0059] Specifically, the continuous floating-point feature values ​​of each point pair are mapped to a discrete integer range, and the discretized features are combined into a unique hash key (bit operations or concatenation can be used). The hash key is retrieved from each hash table pre-built based on each template point cloud, and template point pairs with the same hash key value after discretization of the features of each reference point pair are selected. Based on the reference point pair (composed of a fused scene point cloud point and a template point cloud point) and the template point pair (composed of a template point cloud point in the reference point pair and another point in the template point cloud), a registration point pair (composed of a fused scene point cloud point in the reference point pair and another template point cloud point in the template point pair) is established.

[0060] In a specific example, for each reference point pair (s, m), where s is a point in the fused scene point cloud and m is a template point cloud point corresponding to point s after coarse registration, a hash key is generated using the point pair features. This key is then retrieved from the hash table corresponding to each template point cloud to find the feature-matching template point cloud internal point pair (m, m'), i.e., the point pair in the template point cloud whose features are similar to (s, m), where m' is a template point cloud point. In the retrieved template point cloud internal point pair (m, m'), m is an existing template point cloud point in the reference point pair, and m' is a point in the template point cloud whose features match s. Therefore, the fused scene point cloud point s and the template point cloud point m' form a registration point pair (s, m').

[0061] Furthermore, based on the above embodiments, before retrieving the template point pair matching each reference point pair from each hash table pre-built based on each template point cloud, the process may further include:

[0062] Discretize all point pair features within the template point cloud and store them in a hash table; where the key in the hash table is the discrete feature code of the point pair feature, and the value is the point pair within the template point cloud that matches the discrete feature code of the point pair feature.

[0063] Specifically, the point pair features (the angle between the normal vectors and the curvature at the two points) of any two points in the same template point cloud are discretized and combined to generate discrete feature codes as the keys of the hash table, and the values ​​are the point pairs inside the template point cloud corresponding to the discrete feature codes, thereby generating a hash table that matches the template point cloud.

[0064] By pre-compiling the features of each point pair in the template point cloud offline and constructing a hash table, the bucket search of the hash table is used to replace the traversal search method, which improves the matching efficiency of the fused scene point cloud and template point cloud. It is especially suitable for multi-template application scenarios, expands application scenarios such as multi-target recognition, and improves the recognition and registration efficiency of large-scale point clouds with multiple templates.

[0065] S130. Based on the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair, calculate the translation vector and rotation quaternion that match each registration point pair.

[0066] In this embodiment of the invention, the translation vector can be specifically understood as representing the translation transformation from the reference point coordinate system to the template coordinate system. It can be determined by calculating the difference between the coordinates of the template point cloud points in the reference point coordinate system and the coordinates of the fused scene point cloud points. The rotation quaternion can be specifically understood as representing the rotation transformation from the reference point coordinate system to the template coordinate system. It can be obtained by solving the rotation relationship between the registration point pairs. The rotation relationship can be constructed by creating a rotation matrix and converting it into a quaternion representation. The rotation matrix can be solved using the least squares method or other optimization methods to minimize the error between the registration point pairs.

[0067] Specifically, the points in the fused scene point cloud of each registration point pair are located in the reference point coordinate system corresponding to the fused scene point cloud, and the points in the template point cloud of each registration point pair are located in the template coordinate system corresponding to the template point cloud. Based on the registration point pairs, a system of transformation equations from the reference point coordinate system to the template coordinate system is constructed. For each registration point pair, the following relationship exists: x = R·y + t, where y is the coordinate of a point in the reference point coordinate system, x is the coordinate of the corresponding point in the template coordinate system, R is the rotation matrix, and t is the translation vector. The above system of equations is solved using the least squares method or other optimization methods to obtain the rotation matrix R and the translation vector t. The rotation matrix R is converted into a quaternion representation using q = quat(R) to obtain the rotation quaternion q, where quat(·) is the conversion formula for converting the rotation matrix into a quaternion. Then, the calculated rotation matrix R and translation vector t can be combined to construct a 4x4 homogeneous transformation matrix. This matrix can represent both rotation and translation transformations. R is a 3x3 rotation matrix, t is a 3x1 translation vector, and 0 and 1 are used to extend the matrix to 4x4. It is a closed matrix that supports homogeneous coordinate transformations.

[0068] S140. Based on each translation vector and rotation quaternion, perform clustering on each registration point pair to obtain each initial correspondence set.

[0069] In this context, the template point cloud points in each registration point pair of the same initial correspondence set belong to the same template point cloud.

[0070] Specifically, all registered point pairs are grouped according to the consistency of their transformation parameters, so that point pairs within the same group correspond to the same object instance, while different groups correspond to different object instances or mismatches. Specifically, the transformation similarity of two registered point pairs (such as the weighted sum of the Euclidean distance of the translation vector and the rotation angle difference represented by quaternions) can be calculated and clustered using a clustering algorithm (such as density-based clustering or K-means clustering). After clustering, each cluster corresponds to an initial corresponding set. Point pairs within the same set have highly similar translation vectors and rotation quaternions, and all template point cloud points within the same set come from the same template point cloud.

[0071] S150. Among the pose candidate transformation matrices that match each initial corresponding set, select the target pose candidate transformation matrix and obtain the target template point cloud that matches the target pose candidate transformation matrix.

[0072] Specifically, for all registration point pairs within the set, their average pose transformation (including average translation vector and average rotation quaternion) or other pose transformation features can be calculated, and these features can be concatenated into a matrix as the corresponding candidate pose transformation matrix for the set.

[0073] Understandably, the initial correspondence set of a real target usually contains a large number of registration point pairs, while the noise set has fewer point pairs. Therefore, we can select sets with a number of point pairs greater than or equal to a preset threshold. We can calculate the deviation of the transformation parameters of all registration point pairs in the set from the average transformation (such as the standard deviation of the translation vector and the angle difference of the rotation quaternion), and select combinations with errors less than a preset error threshold. Based on this, we select the pose candidate transformation matrix corresponding to the set with the most point pairs and the smallest error as the target pose candidate transformation matrix. Since the template points of the initial correspondence set are uniquely assigned, the corresponding template point cloud can be determined as the target template point cloud through the target pose candidate transformation matrix.

[0074] In a specific example, based on the translation vectors and rotation quaternions of all registered point pairs within the set, their pose transformation features are calculated, and matching pose candidate transformation matrices are constructed. For the translation vectors and rotation quaternions of each pose transformation matrix, the distance (residual) r between the template point cloud points and the fused scene point cloud points after transformation by each pose candidate matrix is ​​calculated. i =||T k ·m i -s i ||2, where m i It is a fusion of scene point cloud points, s i These are the corresponding template point cloud points, T k ·m i It is a fusion scene point cloud point memory T kThe transformed coordinates, ||·||², are the Euclidean norm. The penalty value for each residual is calculated using the Huber penalty function ρ(r), and then summed to obtain the total loss of the pose matrix. Here, the Huber penalty function... Where r is the residual and δ is the switching threshold. When the residual is less than or equal to the switching threshold, it indicates a small error and corresponds to a correct matching point. When the residual is greater than the switching threshold, it indicates a large error and corresponds to a mismatch point. The residuals are sorted according to the total loss and the cluster size of the initial corresponding set. The weighted sum is calculated based on the sorting of total loss and cluster size as the comprehensive score. The pose transformation matrix with the best comprehensive score (the pose transformation matrix with the small total loss and the large number of point pairs in the cluster corresponding to the initial corresponding set) is selected as the candidate pose transformation matrix.

[0075] S160. Construct a target optimization function between the fused scene point cloud and the target template point cloud, and iteratively optimize the parameters in the target pose candidate transformation matrix based on the target optimization function, and obtain the target pose of the fused scene point cloud at the end of the iteration.

[0076] Specifically, a target optimization function is constructed between the fused scene point cloud and the target template point cloud. The objective is to minimize the overlap error (which can be calculated as point-to-surface distance or point-to-point distance) between the fused scene point cloud transformed by the target pose candidate transformation matrix and the target region point cloud in the template point cloud. The form of the loss function can be selected according to business needs or data characteristics, such as the Huber penalty function, Cauchy loss function, or least squares loss function. An iterative optimization algorithm (such as the Gauss-Newton method) is used to minimize this optimization function. When the update amount of the target pose candidate transformation matrix is ​​less than a set threshold, or the change amount of the optimization function value is less than a set threshold, the iteration is considered to have converged, and the optimization process is stopped. The rotation matrix R and translation vector t are obtained. Based on the rotation matrix R and translation vector t, the fused scene point cloud is transformed to the position and pose that are best aligned with the target template point cloud, thus obtaining the target pose of the fused scene point cloud. This achieves accurate registration of the point cloud and meets the requirements of high-precision pose estimation.

[0077] The technical solution of this invention involves coarsely registering a fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives with pre-stored template point clouds, and calculating the point pair features of each reference point pair after coarse registration. Based on the hash key values ​​corresponding to the point pair features, template point pairs matching each reference point pair are retrieved from pre-constructed hash tables based on each template point cloud, and registration point pairs are established based on the retrieval results. Finally, the coordinate system of each registration point pair is calculated based on the coordinate system of the reference points corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair. The process involves matching translation vectors and rotation quaternions; clustering each registration point pair based on the translation vectors and rotation quaternions to obtain initial correspondence sets; selecting target pose candidate transformation matrices from the pose candidate transformation matrices matched with each initial correspondence set, and obtaining target template point clouds that match the target pose candidate transformation matrices; constructing a target optimization function between the fused scene point cloud and the target template point cloud, and iteratively optimizing the parameters in the target pose candidate transformation matrix based on the target optimization function, obtaining the target pose of the fused scene point cloud at the end of the iteration. By fusing the original point clouds from multiple perspectives, coarse registration is first performed with the pre-stored template point cloud, and registration point pairs are constructed using a hash table to reduce the registration search space; subsequently, the obtained translation vectors and rotation quaternions are clustered and filtered to retain the target template point clouds that match the target pose candidate transformation matrices, reducing the subsequent optimization search space and providing initial optimization values ​​for subsequent optimization. By iteratively optimizing the parameters, the accuracy, precision, and stability of target pose estimation are improved, ensuring the real-time performance and reliability of six-dimensional pose estimation.

[0078] Example 2

[0079] Figure 2 This is a flowchart of another pose estimation method based on point-pair feature matching provided in Embodiment 2 of the present invention. This embodiment is a refinement of the above embodiment's step of "clustering each registered point pair according to each translation vector and rotation quaternion to obtain each initial correspondence set". Specifically, it may include: applying a density-based noisy spatial clustering algorithm and a preset translation vector clustering deviation threshold to cluster translation vectors to obtain translation clusters; applying a density-based noisy spatial clustering algorithm and a preset quaternion clustering deviation threshold to cluster the rotation quaternions of all corresponding registered point pairs in the translation clusters to obtain rotation clusters; counting the number of registered point pairs contained in each rotation cluster, and taking the rotation cluster with the most registered point pairs as the initial correspondence set.

[0080] Correspondingly, such as Figure 2 As shown, the method includes:

[0081] S210. The fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives is coarsely registered with each pre-stored template point cloud, and the point pair features of each reference point pair after coarse registration are calculated.

[0082] S220. Based on the hash key value corresponding to the features of each point pair, retrieve the template point pairs that match each reference point pair from each hash table pre-built based on each template point cloud, and establish registration point pairs based on the retrieval results.

[0083] Each registration point pair includes a fused scene point cloud point and a template point cloud point belonging to a set template point cloud.

[0084] S230. Based on the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair, calculate the translation vector and rotation quaternion that match each registration point pair.

[0085] S240. A density-based noisy spatial clustering algorithm and a preset translation vector clustering deviation threshold are used to cluster translation vectors to obtain translation clusters.

[0086] Specifically, density-based spatial clustering of applications with noise (DBSCAN) and a preset translation vector clustering bias threshold (i.e., neighborhood radius) are used to limit the translation vector clustering bias, for example... Among them, t i Let i be the translation vector. Let be the cluster center translation vector, ∈ t This is the threshold for clustering bias in translation vectors.

[0087] For each translation vector, the number of points in its neighborhood (distance less than or equal to the translation vector clustering bias threshold) is calculated. If the number of points in the neighborhood is greater than or equal to a preset minimum number of neighborhood points, the point is marked as a core point (corresponding to the translation vector of the cluster center). Starting from the core point, all points in its neighborhood (including core and non-core points) are grouped into a cluster. Then, the neighborhoods of these neighboring points are checked, and the clusters are expanded until no new points can be added. Translation vectors that do not belong to any cluster are marked as noise points, thus obtaining multiple translation clusters, each containing similar translation vectors.

[0088] S250. A density-based noisy spatial clustering algorithm and a preset quaternion clustering deviation threshold are used to cluster the rotated quaternions of all corresponding registration point pairs in the translation cluster to obtain the rotated cluster.

[0089] Specifically, the rotation quaternions of all registration point pairs are extracted from the translation clusters, based on DBSCAN and a preset quaternion clustering bias threshold used to limit quaternion clustering bias, for example... Where, q i Let i be the i-th quaternion. Let ε be the quaternion of cluster centers. q This is the threshold for quaternion clustering bias. Let Δq represent the dot product of two quaternions, where Δq is the angle difference between the two quaternions.

[0090] For each rotated quaternion, calculate its angle with all other quaternions. If the angle is less than or equal to the quaternion clustering bias threshold, and the number of points in its neighborhood is greater than or equal to a preset minimum number of neighborhood points, then the quaternion is marked as a core point (corresponding to the cluster center quaternion). Starting from the core point, all quaternions in its neighborhood (with an angle less than or equal to the quaternion clustering bias threshold) are grouped into a cluster. Then, the neighborhoods of these neighboring points are checked, and the clusters are expanded until no new points can be added. Quaternions that do not belong to any cluster are marked as noise points, thus obtaining multiple rotated clusters, each of which includes similar rotated quaternions based on similar translation vectors.

[0091] S260. Count the number of registration point pairs contained in each rotation cluster, and take the rotation cluster with the most registration point pairs as the initial correspondence set.

[0092] In this context, the template point cloud points in each registration point pair of the same initial correspondence set belong to the same template point cloud.

[0093] Understandably, correctly matched point pairs often exhibit geometric consistency and tend to cluster around specific rotation parameters due to the true pose of the target object, forming large clusters. In contrast, incorrectly matched point pairs are scattered and irregular, making it difficult to form large-scale clusters. Therefore, we count the number of registered point pairs contained in each rotation cluster and select the rotation cluster with the largest number of registered point pairs as the initial correspondence set.

[0094] S270. Among the pose candidate transformation matrices that match each initial corresponding set, select the target pose candidate transformation matrix and obtain the target template point cloud that matches the target pose candidate transformation matrix.

[0095] Furthermore, based on the above embodiments, before selecting the candidate transformation matrix for the target pose, the following may also be included:

[0096] Perform interchangeability checks and distance consistency checks on each registration point pair in the initial correspondence set. Then, filter and remove registration point pairs that cannot meet the reverse matching or distance consistency requirements from the initial correspondence set.

[0097] In this embodiment of the invention, the interchangeability check can be specifically understood as: a method to verify whether any pair of points can also be reverse-matched in the other party's point set. The three-pair point distance consistency check can be specifically understood as: a method to randomly select three pairs of corresponding points and calculate whether the distance difference between these pairs meets a preset requirement.

[0098] Specifically, in the initial correspondence set, for each registration point pair (e i ,f i ), where e i To integrate scene point cloud points, f i Let e ​​be the point cloud point of the template. i The best matching point is f i Check f i Is the best matching point e? i That is, finding f in the fused scene point cloud i Optimal matching point e i The best matching point can be found by calculating the distance between feature vectors (such as Euclidean distance) and selecting the point with the smallest distance. This can be verified by... i 'Whether or not it is with e' i If they are the same point, then the registration point pair is considered unreliable and is removed from the initial correspondence set.

[0099] Three sets of registration point pairs are randomly selected from the initial correspondence set (e i ,f i ), (e j ,f j ) and (e k ,f k ), where e i e j and e k To integrate scene point cloud points, f i f j and f k Given a template point cloud, calculate the distance difference between these point pairs and verify whether |||e i -e j ||-||f i -f j |||≤τ, and |||e i -e k ||-||f i -f k |||≤τ, and |||e j -e k ||-||f j -f k|||≤τ, where τ is a preset distance consistency threshold. If the above condition is not met, it is considered that there are inconsistent registration point pairs among these three sets of point pairs, and these point pairs are removed from the initial correspondence set.

[0100] By performing interchangeability checks and three-pair distance consistency checks, registration point pairs that cannot meet the reverse matching or distance consistency requirements can be eliminated, improving the reliability and accuracy of the initial correspondence set. This ensures that the subsequent pose estimation process is based on more reliable registration point pairs, thereby improving the final pose estimation accuracy.

[0101] S280. Construct a target optimization function between the fused scene point cloud and the target template point cloud, and iteratively optimize the parameters in the target pose candidate transformation matrix based on the target optimization function, and obtain the target pose of the fused scene point cloud at the end of the iteration.

[0102] The technical solution of this invention involves coarsely registering the fused scene point cloud with each template point cloud and calculating point pair features; retrieving and establishing registered point pairs from each hash table based on the corresponding hash key values; calculating the translation vector and rotation quaternion matching each registered point pair based on the reference point coordinate system and the template coordinate system; applying a density-based noisy spatial clustering algorithm and a preset translation vector clustering deviation threshold to cluster the translation vectors to obtain translation clusters; and applying a density-based noisy spatial clustering algorithm and a preset quaternion clustering deviation threshold to further refine the translation clusters. The rotation quaternions of all corresponding registration point pairs are clustered to obtain rotation clusters; the number of registration point pairs contained in each rotation cluster is counted, and the rotation cluster with the most registration point pairs is taken as the initial correspondence set; among the pose candidate transformation matrices matched with each initial correspondence set, the target pose candidate transformation matrix and the matched target template point cloud are selected; a target optimization function between the fused scene point cloud and the target template point cloud is constructed, and the parameters in the target pose candidate transformation matrix are iteratively optimized. At the end of the iteration, the target pose of the fused scene point cloud is obtained. By fusing the original point clouds from multiple perspectives, coarse registration is first performed with the pre-stored template point cloud, and a hash table is used to retrieve and construct the registered point pairs, thus reducing the registration search space. Subsequently, the obtained translation vectors and rotation quaternions are clustered separately. Density-based clustering first uses the translation vector threshold to obtain translation clusters, and then uses the quaternion threshold to further rotate and cluster within the clusters to form rotation clusters. The number of point pairs in the rotation cluster directly reflects the support of the transformation in the global context. Selecting the largest cluster retains the most universal matching set, thereby automatically eliminating sparse outlier point pairs and improving the robustness and accuracy of subsequent pose estimation. The target template point cloud matching the candidate transformation matrix of the target pose is selected and filtered to reduce the search space for subsequent optimization and provide initial values ​​for subsequent optimization. By iteratively optimizing the parameters, the accuracy, precision, and stability of the target pose estimation are improved, ensuring the real-time performance and reliability of six-dimensional pose estimation.

[0103] Example 3

[0104] Figure 3 This is a flowchart of another pose estimation method based on point-pair feature matching provided in Embodiment 3 of the present invention. This embodiment is a refinement of the above embodiment's "constructing a target optimization function between the fused scene point cloud and the target template point cloud, and iteratively optimizing the parameters in the target pose candidate transformation matrix based on the target optimization function". Specifically, it may include: constructing a least-squares optimization target function between the fused scene point cloud and the target template point cloud; iteratively optimizing the rotation component parameters in the target pose candidate transformation matrix by maximizing the matrix trace and singular value decomposition; and iteratively optimizing the translation component parameters in the target pose candidate transformation matrix by a system of linear equations for the translation components.

[0105] Correspondingly, such as Figure 3 As shown, the method includes:

[0106] S310. The fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives is coarsely registered with each pre-stored template point cloud, and the point pair features of each reference point pair after coarse registration are calculated.

[0107] S320. Based on the hash key value corresponding to the features of each point pair, retrieve the template point pairs that match each reference point pair from each hash table pre-built based on each template point cloud, and establish registration point pairs based on the retrieval results.

[0108] Each registration point pair includes a fused scene point cloud point and a template point cloud point belonging to a set template point cloud.

[0109] S330. Based on the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair, calculate the translation vector and rotation quaternion that match each registration point pair.

[0110] S340. Based on each translation vector and rotation quaternion, cluster each registration point pair to obtain each initial correspondence set.

[0111] In this context, the template point cloud points in each registration point pair of the same initial correspondence set belong to the same template point cloud.

[0112] S350. Among the pose candidate transformation matrices that match each initial corresponding set, select the target pose candidate transformation matrix and obtain the target template point cloud that matches the target pose candidate transformation matrix.

[0113] S360. Construct a least-squares optimization objective function between the fused scene point cloud and the target template point cloud. Iteratively optimize the rotation component parameters in the target pose candidate transformation matrix by maximizing the matrix trace and singular value decomposition. Iteratively optimize the translation component parameters in the target pose candidate transformation matrix by the linear equation system of translation components. At the end of the iteration, the target pose of the fused scene point cloud is obtained.

[0114] Specifically, based on the transformation relationship between the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the target template point cloud, a least squares optimization objective function is constructed to minimize the sum of the point-to-point errors and corresponding point errors of all points between the fused scene point cloud and the template point cloud. Typically, the least squares optimization objective function can be: in, The point-to-point error represents the deviation between all points in the fused scene point cloud after transformation and the corresponding points in the template point cloud. p represents the matching deviation of corresponding points, ensuring accurate alignment of key feature points. k and p l The coordinates of a point in the reference coordinate system, q l and q k Here, R is the coordinates of the corresponding point in the template coordinate system, t is the rotation matrix, k is the number of point-to-point combinations, and l is the number of corresponding point combinations. The optimal rotation matrix is ​​found by maximizing the trace of the matrix. It's understandable that the trace of the matrix is ​​related to the degree of matching between the matrix and the point cloud, and a larger trace indicates a better match. The rotation component parameters in the candidate transformation matrix of the target pose are iteratively optimized by maximizing the matrix trace and using singular value decomposition. Maximizing the matrix trace can be specifically expressed as: Where S is the point-to-point covariance matrix, q k Let p be the coordinate vector of the k-th point in the template point cloud. k To fuse the coordinate vector of the k-th point in the scene point cloud, and q k and p k Mutually corresponding and matching.

[0115] Singular value decomposition can be specifically represented as: Where U represents an orthogonal matrix, which corresponds to rotation or reflection transformation in the coordinate space of the template point cloud; Σ represents a diagonal matrix, with singular values ​​on the diagonal reflecting the scaling of matrix S in each principal direction; and V represents the transpose of the orthogonal matrix, which corresponds to rotation or reflection transformation in the coordinate space of the fused scene point cloud. The optimal rotation matrix R is obtained through singular value decomposition.

[0116] By solving the linear equations of the translation components, a closed-form solution for the translation vector t is obtained. The translation component parameters in the candidate transformation matrix of the target pose are then iteratively optimized. The linear equations of the translation components can be specifically described as follows: in, and These are the centroids of the fused scene point cloud set and the template point cloud set, respectively. By repeating the above steps, the rotation matrix R and translation vector t are gradually updated until a convergence condition is met, such as the change in the transformation matrix (which can be calculated as a weighted sum of the changes in the rotation matrix R and the translation vector t) being lower than a set convergence threshold. At the end of the iteration, the optimal rotation matrix R and translation vector t are obtained, thus yielding the target pose of the fused scene point cloud.

[0117] The technical solution of this invention involves coarsely registering the fused scene point cloud with each template point cloud and calculating point pair features; retrieving and establishing registered point pairs from each hash table based on the corresponding hash key values; calculating the translation vector and rotation quaternion matching each registered point pair based on the reference point coordinate system and the template coordinate system; performing clustering processing on each registered point pair to obtain each initial correspondence set; selecting the target pose candidate transformation matrix and the matching target template point cloud from the pose candidate transformation matrices that match each initial correspondence set; constructing a least-squares optimization objective function between the fused scene point cloud and the target template point cloud; iteratively optimizing the rotation component parameters in the target pose candidate transformation matrix by maximizing the matrix trace and singular value decomposition; iteratively optimizing the translation component parameters in the target pose candidate transformation matrix by a system of linear equations for the translation components; and obtaining the target pose of the fused scene point cloud at the end of the iteration. By fusing the original point clouds from multiple perspectives, coarse registration is first performed with the pre-stored template point cloud, and a hash table is used to retrieve and construct the registration point pairs, thus reducing the registration search space. Subsequently, the obtained translation vectors and rotation quaternions are clustered and filtered to retain the target template point cloud that matches the candidate transformation matrix of the target pose, reducing the search space for subsequent optimization and providing initial values ​​for subsequent optimization. The least squares objective function quantifies the error, and trace maximization ensures that the rotation component approaches along the direction of maximum correlation at each step. Singular value decomposition ensures that the rotation component is orthogonal and unique. The translation component is updated through a closed-loop linear equation system to avoid nonlinear iterative oscillations. Rotation and translation are optimized alternately, so that the error decreases until convergence. The final output candidate transformation matrix of the target pose has both orthogonality and minimum residual, improving the accuracy, precision and stability of target pose estimation, and ensuring the real-time performance and reliability of six-dimensional pose estimation.

[0118] Example 4

[0119] Figure 4 This is a schematic diagram of a pose estimation device based on point-pair feature matching provided in Embodiment 4 of the present invention. Figure 4As shown, the device includes: a coarse registration module 410, a hashing module 420, a translation and rotation module 430, a clustering module 440, a pose matrix module 450, and an iterative optimization module 460, wherein:

[0120] The coarse registration module 410 is used to perform coarse registration of the fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives with each pre-stored template point cloud, and to calculate the point pair features of each reference point pair after coarse registration.

[0121] The hash module 420 is used to retrieve template point pairs that match each reference point pair from each hash table pre-built based on each template point cloud according to the hash key value corresponding to the features of each point pair, and to establish registration point pairs according to the retrieval results; wherein, each registration point pair includes a fused scene point cloud point and a template point cloud point belonging to the set template point cloud.

[0122] The translation and rotation module 430 is used to calculate the translation vector and rotation quaternion that match each registration point pair based on the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair.

[0123] Clustering module 440 is used to cluster each registration point pair according to each translation vector and rotation quaternion to obtain each initial correspondence set; wherein, the template point cloud points in each registration point pair of the same initial correspondence set belong to the same template point cloud.

[0124] The pose matrix module 450 is used to filter out the target pose candidate transformation matrix from the pose candidate transformation matrices that are matched with each initial corresponding set, and to obtain the target template point cloud that matches the target pose candidate transformation matrix.

[0125] The iterative optimization module 460 is used to construct a target optimization function between the fused scene point cloud and the target template point cloud, and to iteratively optimize the parameters in the target pose candidate transformation matrix based on the target optimization function, and obtain the target pose of the fused scene point cloud at the end of the iteration.

[0126] The technical solution of this invention involves coarsely registering a fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives with pre-stored template point clouds, and calculating the point pair features of each reference point pair after coarse registration. Based on the hash key values ​​corresponding to the point pair features, template point pairs matching each reference point pair are retrieved from pre-constructed hash tables based on each template point cloud, and registration point pairs are established based on the retrieval results. Finally, the coordinate system of each registration point pair is calculated based on the coordinate system of the reference points corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair. The process involves matching translation vectors and rotation quaternions; clustering each registration point pair based on the translation vectors and rotation quaternions to obtain initial correspondence sets; selecting target pose candidate transformation matrices from the pose candidate transformation matrices matched with each initial correspondence set, and obtaining target template point clouds that match the target pose candidate transformation matrices; constructing a target optimization function between the fused scene point cloud and the target template point cloud, and iteratively optimizing the parameters in the target pose candidate transformation matrix based on the target optimization function, obtaining the target pose of the fused scene point cloud at the end of the iteration. By fusing the original point clouds from multiple perspectives, coarse registration is first performed with the pre-stored template point cloud, and registration point pairs are constructed using a hash table to reduce the registration search space; subsequently, the obtained translation vectors and rotation quaternions are clustered and filtered to retain the target template point clouds that match the target pose candidate transformation matrices, reducing the subsequent optimization search space and providing initial optimization values ​​for subsequent optimization. By iteratively optimizing the parameters, the accuracy, precision, and stability of target pose estimation are improved, ensuring the real-time performance and reliability of six-dimensional pose estimation.

[0127] Furthermore, based on the above embodiments, the pose estimation device based on point-pair feature matching may further include: a rejection module, wherein:

[0128] The removal module is used to filter unreliable regions in the fused scene point cloud based on the point cloud segmentation algorithm and remove them from the fused scene point cloud before performing coarse registration with each pre-stored template point cloud.

[0129] Based on the above embodiments, unreliable regions in the fused scene point cloud may include: background plane and high curvature edge regions;

[0130] Accordingly, based on the above embodiments, the module is removed and is specifically used for:

[0131] Based on the random sampling consistency plane fitting algorithm, the target background plane containing the most points in the fused scene point cloud is selected and removed from the fused scene point cloud;

[0132] After removing the target background plane, calculate the local covariance matrix of each point in the fused scene point cloud, obtain the eigenvalues ​​of the local covariance matrix, and construct the contour feature tuples based on the eigenvalues.

[0133] Contour feature clusters are extracted from contour feature tuples using Euclidean clustering. Based on these contour feature clusters, high curvature edge regions are selected and removed from the fused scene point cloud.

[0134] Furthermore, based on the above embodiments, the pose estimation device based on point-pair feature matching may further include: a discrete module, wherein:

[0135] The discrete module is used to discretize all point pair features within the template point cloud and store them in hash tables before retrieving template point pairs that match each reference point pair from each hash table pre-built based on each template point cloud. In the hash table, the key is the discrete feature code of the point pair feature, and the value is the internal point pair of the template point cloud that matches the discrete feature code of the point pair feature.

[0136] Based on the above embodiments, the clustering module 440 is specifically used for:

[0137] A density-based noisy spatial clustering algorithm and a preset translation vector clustering bias threshold are used to cluster translation vectors to obtain translation clusters.

[0138] Based on a density-based noisy spatial clustering algorithm and a preset quaternion clustering bias threshold, the rotation quaternions of all corresponding registration point pairs in the translation cluster are clustered to obtain rotation clusters.

[0139] Count the number of registration point pairs contained in each rotation cluster, and take the rotation cluster with the most registration point pairs as the initial correspondence set.

[0140] Furthermore, based on the above embodiments, the pose estimation device based on point-pair feature matching may further include: an interchangeable consistency module, wherein:

[0141] The interchangeability and consistency module is used to perform interchangeability checks and three-pair distance consistency checks on each registration point pair in the initial correspondence set before selecting the target pose candidate transformation matrix. Registration point pairs that cannot meet the reverse matching or distance consistency requirements are filtered out from the initial correspondence set.

[0142] Based on the above embodiments, the iterative optimization module 460 is specifically used for:

[0143] A least-squares optimization objective function is constructed between the fused scene point cloud and the target template point cloud. The rotation component parameters in the target pose candidate transformation matrix are iteratively optimized by maximizing the matrix trace and singular value decomposition. The translation component parameters in the target pose candidate transformation matrix are iteratively optimized by a system of linear equations for the translation components.

[0144] The pose estimation device based on point-pair feature matching provided in the embodiments of the present invention can execute the pose estimation method based on point-pair feature matching provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the method.

[0145] The collection, storage, use, processing, transmission, provision, and disclosure of user personal information involved in the technical solution disclosed herein comply with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0146] Example 5

[0147] Figure 5 A schematic diagram of an electronic device 10 that can be used to implement embodiments of the present invention is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices (e.g., helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0148] like Figure 5 As shown, the electronic device 10 includes at least one processor 11 and a memory, such as a read-only memory (ROM) 12 or a random access memory (RAM) 13, communicatively connected to the at least one processor 11. The memory stores computer programs executable by the at least one processor. The processor 11 can perform various appropriate actions and processes based on the computer program stored in the ROM 12 or loaded from storage unit 18 into the RAM 13. The RAM 13 may also store various programs and data required for the operation of the electronic device 10. The processor 11, ROM 12, and RAM 13 are interconnected via a bus 14. An input / output (I / O) interface 15 is also connected to the bus 14.

[0149] Multiple components in electronic device 10 are connected to I / O interface 15, including: input unit 16, such as keyboard, mouse, etc.; output unit 17, such as various types of displays, speakers, etc.; storage unit 18, such as disk, optical disk, etc.; and communication unit 19, such as network card, modem, wireless transceiver, etc. Communication unit 19 allows electronic device 10 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0150] Processor 11 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. Processor 11 performs the various methods and processes described above, such as pose estimation methods based on point-pair feature matching, i.e.:

[0151] The fused scene point cloud, obtained by fusing raw point cloud data from multiple perspectives, is coarsely registered with pre-stored template point clouds, and the point pair features of each reference point pair after coarse registration are calculated. Based on the hash key value corresponding to each point pair feature, template point pairs matching each reference point pair are retrieved from pre-built hash tables based on each template point cloud, and registration point pairs are established based on the retrieval results. Each registration point pair includes one fused scene point cloud point and one template point cloud point belonging to a set template point cloud. Based on the coordinate system of the reference point corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud point in each registration point pair, the matching coordinates of each registration point pair are calculated. Translation vectors and rotation quaternions are used to cluster each registration point pair to obtain initial correspondence sets. Template point cloud points in each registration point pair within the same initial correspondence set belong to the same template point cloud. From the pose candidate transformation matrices matched with each initial correspondence set, target pose candidate transformation matrices are selected, and target template point clouds matching the target pose candidate transformation matrices are obtained. A target optimization function is constructed between the fused scene point cloud and the target template point cloud. Based on this function, the parameters in the target pose candidate transformation matrix are iteratively optimized, and the target pose of the fused scene point cloud is obtained at the end of the iteration.

[0152] In some embodiments, the pose estimation method based on point-pair feature matching can be implemented as a computer program tangibly contained in a computer-readable storage medium, such as storage unit 18. In some embodiments, part or all of the computer program can be loaded and / or installed on electronic device 10 via ROM 12 and / or communication unit 19. When the computer program is loaded into RAM 13 and executed by processor 11, one or more steps of the pose estimation method based on point-pair feature matching described above can be performed. Alternatively, in other embodiments, processor 11 can be configured to perform the pose estimation method based on point-pair feature matching by any other suitable means (e.g., by means of firmware).

[0153] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0154] Computer programs used to implement the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be performed. The computer programs may be executed entirely on a machine, partially on a machine, or as a standalone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0155] In the context of this invention, a computer-readable storage medium can be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0156] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0157] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or computing systems that include middleware components (e.g., application servers), or computing systems that include frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet.

[0158] A computing system can include clients and servers. Clients and servers are generally located far apart and typically interact through communication networks. The client-server relationship is created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, which is a hosting product within the cloud computing service system to address the shortcomings of traditional physical hosts and VPS services, such as high management difficulty and weak business scalability.

[0159] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0160] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A pose estimation method based on point-pair feature matching, characterized in that, include: The fused scene point cloud obtained by fusing raw point cloud data from multiple perspectives is coarsely registered with each pre-stored template point cloud, and the point pair features of each reference point pair after coarse registration are calculated. Based on the hash key value corresponding to the features of each point pair, the template point pairs that match each reference point pair are retrieved from each hash table pre-built based on each template point cloud, and registration point pairs are established based on the retrieval results; wherein, each registration point pair includes a fused scene point cloud point and a template point cloud point belonging to the set template point cloud; Based on the reference point coordinate system corresponding to the fused scene point cloud and the template coordinate system corresponding to the template point cloud points in each registration point pair, calculate the translation vector and rotation quaternion that match each registration point pair. Based on each translation vector and rotation quaternion, each registration point pair is clustered to obtain each initial correspondence set; wherein, the template point cloud points in each registration point pair of the same initial correspondence set belong to the same template point cloud. Among the pose candidate transformation matrices that match each initial corresponding set, the target pose candidate transformation matrix is ​​selected, and the target template point cloud that matches the target pose candidate transformation matrix is ​​obtained. A target optimization function is constructed between the fused scene point cloud and the target template point cloud. Based on the target optimization function, the parameters in the target pose candidate transformation matrix are iteratively optimized, and the target pose of the fused scene point cloud is obtained at the end of the iteration.

2. The method according to claim 1, characterized in that, Before performing coarse point cloud registration with the pre-stored template point clouds, the following steps are also included: Unreliable regions in the fused scene point cloud are filtered out based on the point cloud segmentation algorithm and removed from the fused scene point cloud.

3. The method according to claim 2, characterized in that, Unreliable regions in the fused scene point cloud include: the background plane and high-curvature edge regions; Accordingly, based on point cloud segmentation algorithms, unreliable regions in the fused scene point cloud are filtered out and removed from the fused scene point cloud, including: Based on the random sampling consistency plane fitting algorithm, the target background plane containing the most points in the fused scene point cloud is selected and removed from the fused scene point cloud; After removing the target background plane, calculate the local covariance matrix of each point in the fused scene point cloud, obtain the eigenvalues ​​of the local covariance matrix, and construct the contour feature tuples based on the eigenvalues. Contour feature clusters are extracted from contour feature tuples using Euclidean clustering. Based on these contour feature clusters, high curvature edge regions are selected and removed from the fused scene point cloud.

4. The method according to claim 1, characterized in that, Before retrieving template point pairs that match each reference point pair from the hash tables pre-built based on each template point cloud, the process also includes: Discretize all point pair features within the template point cloud and store them in a hash table; where the key in the hash table is the discrete feature code of the point pair feature, and the value is the point pair within the template point cloud that matches the discrete feature code of the point pair feature.

5. The method according to claim 1, characterized in that, Based on each translation vector and rotation quaternion, clustering is performed on each registration point pair to obtain each initial correspondence set, including: A density-based noisy spatial clustering algorithm and a preset translation vector clustering bias threshold are used to cluster translation vectors to obtain translation clusters. Based on a density-based noisy spatial clustering algorithm and a preset quaternion clustering bias threshold, the rotation quaternions of all corresponding registration point pairs in the translation cluster are clustered to obtain rotation clusters. Count the number of registration point pairs contained in each rotation cluster, and take the rotation cluster with the most registration point pairs as the initial correspondence set.

6. The method according to claim 1, characterized in that, Before selecting candidate transformation matrices for the target pose, the following steps are also included: Perform interchangeability checks and distance consistency checks on each registration point pair in the initial correspondence set. Then, filter and remove registration point pairs that cannot meet the reverse matching or distance consistency requirements from the initial correspondence set.

7. The method according to claim 1, characterized in that, Construct a target optimization function that merges the scene point cloud and the target template point cloud, and iteratively optimize the parameters in the target pose candidate transformation matrix based on the target optimization function, including: A least-squares optimization objective function is constructed between the fused scene point cloud and the target template point cloud. The rotation component parameters in the target pose candidate transformation matrix are iteratively optimized by maximizing the matrix trace and singular value decomposition. The translation component parameters in the target pose candidate transformation matrix are iteratively optimized by a system of linear equations for the translation components.

8. An electronic device, characterized in that, The electronic device includes: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores a computer program that can be executed by the at least one processor, the computer program being executed by the at least one processor to enable the at least one processor to perform the pose estimation method based on point-pair feature matching as described in any one of claims 1-7.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions that cause a processor to execute the pose estimation method based on point-pair feature matching as described in any one of claims 1-7.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the pose estimation method based on point-pair feature matching according to any one of claims 1-7.

Images