Partially overlapped point cloud registration method based on dual consistency
Through low-level consistency generation, high-level consistency screening and sampling matching lines and tetrahedral sets, combined with estimation from thick to thin, the problem of local optimality in partial overlapping point cloud registration is solved, and high-precision and real-time point cloud registration is achieved.
Patent Information
- Application Number
- CN202510484030.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-04-17
- Publication Date
- 2025-08-15
AI Technical Summary
The prior art is prone to falling into local optimization when processing partial overlapping point cloud registration, resulting in inaccurate registration results. In addition, deep learning-based methods require a large amount of training data and resources, making it difficult to adapt to application scenarios with high real-time requirements.
A dual consistency-based approach is adopted to generate matching point sets through low-level consistency, high-level consistency filters and sample matching lines and tetrahedral sets, and obtain the optimal transformation matrix from thick to thin estimation to achieve point cloud registration.
It effectively avoids local optimization, improves the accuracy of partial overlap point cloud registration, reduces interference to non-overlapping areas, adapts to real-time requirements, and reduces dependence on training data and resources.
Smart Images
Figure CN120495361A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to a point cloud registration method, relates to the field of point cloud registration, and particularly relates to a partially overlapping point cloud registration method based on dual consistency. Background Art
[0002] Point cloud registration is an important direction in the field of point cloud research, which aims to unify point clouds obtained from sensors (such as lidar, depth cameras, etc.) from different perspectives or positions into the same coordinate system. This process involves two point clouds: a source point cloud and a target point cloud. The target point cloud remains unchanged, while the source point cloud needs to be aligned to the target point cloud through a pose transformation. Specifically, the core of point cloud registration is to solve the transformation matrix from the source point cloud to the target point cloud, and apply this matrix to achieve precise alignment between the two. Partially overlapping point cloud registration is difficult in the registration process because only some areas of the source point cloud and the target point cloud overlap.
[0003] Traditional point cloud registration methods include iterative closest point, random sampling consistency, and normal distribution transformation. These methods rely on the similarity of geometric structure or other features between the source and target point clouds to establish correspondences between the point clouds and then infer the transformation matrix. However, when processing partially overlapping point clouds, these methods often fall into local optimality, resulting in inaccurate registration results. This is mainly due to the limited ability of traditional methods to identify corresponding matching relationships when processing complex, partially overlapping point clouds.
[0004] With the rise of deep learning, point cloud registration methods based on deep learning have been introduced to the registration of partially overlapping point clouds. These methods automatically find the corresponding matching relationship between the source point cloud and the target point cloud through deep learning models, or directly predict the transformation matrix, thus achieving end-to-end registration. However, deep learning-based methods also face significant challenges: first, they rely on large-scale training data, and the construction of the dataset requires a lot of time and resources; second, the training process is time-consuming and not suitable for application scenarios with high real-time requirements; finally, the generalization ability of deep learning models is limited. For point cloud data from different sources (such as lidar, depth camera, etc.), the model may need to be retrained, which increases the application cost. Summary of the Invention
[0005] In order to solve the problems existing in the background technology, the present invention provides a partially overlapping point cloud registration method based on dual consistency.
[0006] The technical solution adopted in the present invention is:
[0007] The partially overlapping point cloud registration method based on dual consistency of the present invention comprises:
[0008] Step 1) The source point cloud and target point cloud of the current entity object are obtained through the sensor. Each source point in the source point cloud corresponds to several target points in the target point cloud. A low-level consistency method is used to generate a matching point set between the source point cloud and the target point cloud.
[0009] Step 2) Use a high-level consistency method to screen and sample matching tetrahedron sets between the source point cloud and the target point cloud in the matching point set.
[0010] Step 3) Based on the set of matching tetrahedrons, a coarse-to-fine estimation is used to obtain the optimal transformation matrix between the source point cloud and the target point cloud of the current entity object.
[0011] Step 4) When the target entity object is to be registered, the source point cloud of the target entity object is obtained, and the source point cloud is converted to the target point cloud through the optimal transformation matrix and displayed on the display, completing the registration of partially overlapping point clouds.
[0012] In step 1), a low-level consistency method is used to generate a matching point set between the source point cloud and the target point cloud, as follows:
[0013] Step 1.1) Obtain the feature descriptors of each point in the source point cloud and the target point cloud of the current entity object, and then use the L2-norm regularization method to regularize each feature descriptor.
[0014] Step 1.2) For each pair of regularized feature descriptors in the source point cloud and the target point cloud, obtain the feature consistency calculation cost matrix C of the current pair of feature descriptors, and use the Sink-Horn algorithm to obtain the allocation matrix A from the cost matrix.
[0015] Step 1.3) In each row of the distribution matrix A, the index of the k largest elements is selected using the top-K index selection method, thereby extracting several source points and several target points corresponding to each source point, and constructing an initial matching point set.
[0016] In step 1.4), the initial matching point set is screened using geometric consistency based on curvature and distance to construct a matching point set.
[0017] In the step 1.4), for each source point and its target point in the initial matching point set, when the curvature error between the source point and the target point is less than the curvature threshold C th , then retain the current source and target points; for each retained source point and its respective target points, the distance error between them is less than the distance threshold D th The two or more target points are clustered into one target point, so as to obtain the source point and one or more target points after clustering and construct them into a group of matching points, and each group of matching points is constructed as a matching point set.
[0018] The step 2) is as follows:
[0019] Step 2.1) Obtain a number of source point cloud lines and target point cloud lines based on each source point and target point in the matching point set, and then construct a matching line set after re-matching.
[0020] Step 2.2) A plurality of source point cloud tetrahedrons and target point cloud tetrahedrons are obtained according to each source point cloud line and target point cloud line in the matching line set, and then a matching tetrahedron set is constructed after screening and collection.
[0021] The step 2.1) is as follows:
[0022] Step 2.1.1) Connect every two source points in the matching point set to obtain a source point cloud line, and connect every two target points in the matching point set to obtain a target point cloud line. Each source point cloud line and target point cloud line are constructed as an initial matching line set, and the normal vector angles in the initial matching line set are retained to be greater than the preset first angle threshold θ. 1th And the line length is greater than the preset first distance threshold d 1th Several source point cloud lines and target point cloud lines are obtained to construct an initial line set.
[0023] Step 2.1.2) Establish a local coordinate system on each source point cloud line of the initial line set to obtain the quaternion of each source point cloud line α p , β p and They represent the three angle relationships between the local coordinate system of the source point cloud and the normal vectors of the two end points of the source point cloud, d p Indicates the line length of the current source point cloud line; establish a local coordinate system on each target point cloud line in the initial line set to obtain the quaternion of each target point cloud line α q , β q 、 They represent the three angle relationships between the local coordinate system of the target point cloud and the normal vectors of the two end points of the target point cloud, d q Indicates the length of the current target point cloud line; match the quaternion of each source point cloud line and the target point cloud line, and when When , the match is successful, α th , β th 、 and d 2th They respectively represent the preset second angle threshold, the preset third angle threshold, the preset fourth angle threshold and the preset second distance threshold, and construct each successfully matched source point cloud line and target point cloud line into a matching line set.
[0024] The step 2.2) is as follows:
[0025] Step 2.2.1) Every two source point cloud lines in the matching line set are aggregated into a source point cloud tetrahedron, and every two target point cloud lines in the matching point set are aggregated into a target point cloud tetrahedron. Each source point cloud tetrahedron and target point cloud tetrahedron is constructed into an initial tetrahedron set.
[0026] Step 2.2.2) For each source point cloud tetrahedron and target point cloud tetrahedron in the initial tetrahedron set, filter them according to the intersection angle and distance conditions as follows:
[0027] θ>θ 2th
[0028] d r >d 3th str=1,2,3
[0029] Among them, θ represents the intersection angle between two source point cloud lines in the source point cloud tetrahedron or between two target point cloud lines in the target point cloud tetrahedron, θ 2th Indicates the preset fifth angle threshold; d 1 Indicates the first distance between the first source points of two source point cloud lines in the source point cloud tetrahedron or the first target points of two target point cloud lines in the target point cloud tetrahedron, d 2 Indicates the second distance between two source point cloud lines in the source point cloud tetrahedron or between two target point cloud lines in the target point cloud tetrahedron, d 3 Indicates the third distance between the second source points of two source point cloud lines in the source point cloud tetrahedron or the second target points of two target point cloud lines in the target point cloud tetrahedron; d 3th Indicates the preset third distance threshold.
[0030] The source point cloud tetrahedrons and target point cloud tetrahedrons that meet the intersection angle and distance conditions are screened and constructed into a screened tetrahedron set.
[0031] Step 2.2.3) Construct a quintuple of each source point cloud tetrahedron in the screening tetrahedron set And the quintuple of each target point cloud tetrahedron θ p and θ q They represent the intersection angles between two source point cloud lines in the source point cloud tetrahedron and between two target point cloud lines in the target point cloud tetrahedron, and Represent the first, second and third distances of the source point cloud tetrahedron, and Represent the first, second and third distances of the target point cloud tetrahedron, Represents the cross product of two source point cloud line vectors in the source point cloud tetrahedron, that is, the vector and The cross product of represents the direction of the quintuple, Represents the cross product of two target point cloud line vectors in the target point cloud tetrahedron.
[0032] Step 2.2.4) Match the quintuplets of each source point cloud tetrahedron and target point cloud tetrahedron. When , the matching is successful, θ 3th Indicates the preset sixth angle threshold, d 4th Indicates the preset fourth distance threshold, Represents the direction threshold, and the successfully matched source point cloud tetrahedrons and target point cloud tetrahedrons are constructed into a matching tetrahedron set.
[0033] The step 3) is as follows:
[0034] Step 3.1) In the rough estimation stage, for each pair of matching source point cloud tetrahedrons and target point cloud tetrahedrons in the matching tetrahedron set, the singular value decomposition (SVD) algorithm is used to obtain the initial transformation matrix T between the source point cloud tetrahedron and the target point cloud tetrahedron, as follows:
[0035]
[0036] Where R and t represent the rotation matrix and translation vector of the initial transformation matrix T, respectively.
[0037] Step 3.2) For each initial transformation matrix T, convert the rotation matrix R into Euler angles Ea, then combine the translation vector t and the Euler angles Ea into a six-dimensional attitude vector, map all six-dimensional attitude vectors into six-dimensional space, and sparsify each six-dimensional attitude vector through each six-dimensional block in the six-dimensional space to obtain a set of remapping transformation matrices.
[0038] Step 3.3) Based on the matching point set in step 1), the remapping transformation matrix with the largest matching point pair in the remapping transformation matrix set is used as the optimal rough transformation matrix T c .
[0039] Step 3.4) In the fine estimation stage, according to the matching point set in step 1), the optimal coarse transformation matrix T is used c Resample the matching point set to obtain the refined transformation matrix T f , and finally obtain the optimal transformation matrix T o , T o =T f ×T c .
[0040] In the step 3.2), the six-dimensional space is divided into several six-dimensional blocks according to the preset leaf node size, each six-dimensional block contains several six-dimensional posture vectors, and when sparsifying, for each six-dimensional block, the center of the six-dimensional block is used as the representation of all six-dimensional posture vectors in the six-dimensional block, and then remapped into a remapping transformation matrix, and each remapping transformation matrix is constructed into a remapping transformation matrix set.
[0041] In step 3.3), for each source point in the matching point set in step 1) and its remapping transformation matrix, the source point is transformed using the remapping transformation matrix to obtain a number of transformed source points, the distance between the transformed source point and each target point is calculated, and the number of transformed source points whose distance to the target point is less than the fifth distance threshold τ is obtained as the number of matches, and the remapping transformation matrix corresponding to the maximum number of matches is used as the optimal coarse transformation matrix T c .
[0042] In step 3.4), for each source point in the matching point set in step 1), the optimal coarse transformation matrix T is used. c After transforming the source point, several coarse transformation source points are obtained. For each coarse transformation source point, the distance between the coarse transformation source point and the target point is calculated, and the distance between the coarse transformation source point and the target point is retained. 6th The coarse transformation source points of the retained coarse transformation source points and their corresponding target points are constructed as the final matching point set, and the fine transformation matrix T is obtained after the singular value decomposition SVD algorithm is used on the final matching point set. f .
[0043] The electronic device of the present invention comprises: a memory and a processor coupled to each other, wherein the memory stores program data, and the processor calls the program data to execute the method described above.
[0044] The readable storage medium of the present invention stores program data thereon, and when the program data is executed by a processor, the method described above is implemented.
[0045] The dual consistency of the present invention is divided into low-level consistency and high-level consistency. Low-level consistency refers to the consistency of features, curvature, and distance of points in the point cloud. Low-level consistency can be used to generate a robust matching point set. High-level consistency refers to the consistency of the geometric structure of lines and tetrahedrons in the point cloud. High-level consistency can filter and sample matching line sets and matching tetrahedron sets. For the matching tetrahedron set, the present invention uses a coarse-to-fine estimation to obtain the optimal transformation matrix, achieving accurate alignment of partially overlapping point clouds.
[0046] The beneficial effects of the present invention are:
[0047] In order to address the shortcoming that non-overlapping areas of partially overlapping point clouds may interfere with the selection of corresponding matching relationships in the point cloud registration process, the method of the present invention adopts a method of generating matching points based on low-level consistency, screening and sampling matching lines and matching tetrahedrons in sequence based on high-level consistency, and obtaining the optimal transformation matrix through coarse-to-fine estimation. The low-level matching consistency is designed based on feature consistency and the curvature and distance consistency of the points, and the high-level matching consistency is designed by analyzing the geometric structure of the matching lines and matching tetrahedrons. By judging the consistency from low to high levels, the correct matching relationships located in the overlapping areas of the source point cloud and the target point cloud can be screened and sampled to the greatest extent, avoiding interference from non-overlapping areas. The use of dual consistency avoids the shortcoming that the registration process of partially overlapping point clouds is prone to falling into local optimality, resulting in poor registration effect. BRIEF DESCRIPTION OF THE DRAWINGS
[0048] Figure 1 It is a schematic flow chart of the method of the present invention;
[0049] Figure 2 is a schematic diagram of a quadruple of a source point cloud line and a target point cloud line in a matching line of the present invention;
[0050] Figure 3 Schematic diagram of a quintuple of source point cloud tetrahedron and target point cloud tetrahedron in the matching tetrahedron of the present invention;
[0051] Figure 4 is a schematic diagram of a source point cloud and a target point cloud at an initial posture in an embodiment of the present invention;
[0052] Figure 5 is a schematic diagram of corresponding points of a matching point set in an embodiment of the present invention;
[0053] Figure 6 is a schematic diagram of corresponding points of a matching line set in an embodiment of the present invention;
[0054] Figure 7 is a schematic diagram of corresponding points of a matching tetrahedron set in an embodiment of the present invention;
[0055] Figure 8 Schematic diagram of a source point cloud and a target point cloud for achieving registration in an embodiment of the present invention. DETAILED DESCRIPTION
[0056] In order to better understand the technical solution of the present invention, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.
[0057] The present invention uses a pair of partially overlapping indoor scene point clouds. One of the point clouds is selected as the source point cloud and the other point cloud is selected as the target point cloud. The normal vector and feature descriptor of each point of the source point cloud and the target point cloud are extracted respectively. The source point cloud and the target point cloud at the initial posture are as follows: Figure 4 As shown. Figure 1 As shown, the partial overlapping point cloud registration method based on dual consistency of the present invention is specifically as follows:
[0058] Step 1) The source point cloud and target point cloud of the current entity object are obtained through the sensor. Each source point in the source point cloud corresponds to several target points in the target point cloud. A low-level consistency method is used to generate a matching point set between the source point cloud and the target point cloud, as follows:
[0059] Step 1.1) Obtain the feature descriptors of each point in the source point cloud and target point cloud of the current entity object, and then use the L2-norm regularization method to regularize each feature descriptor. Obtain the feature descriptor F of the source point cloud P P , p i represents the i-th source point in the source point cloud P, N p represents the total number of source points in the source point cloud P, Indicates that the source point has 3 dimensions and is located in three-dimensional space. Represents the i-th source point p in the source point cloud P i The feature descriptor of Indicates that the feature descriptor has m dimensions and is located in m-dimensional space; obtain the feature descriptor F of the target point cloud Q Q , q j represents the jth target point in the target point cloud Q, N q represents the total number of target points in the target point cloud Q, Represents the jth target point q in the target point cloud Q j feature descriptor; the feature descriptor can be obtained by traditional methods or deep learning methods.
[0060] Step 1.2) For each pair of regularized feature descriptors in the source and target point clouds, obtain the feature consistency calculation cost matrix C of the current pair of feature descriptors, and use the Sink-Horn algorithm to obtain the allocation matrix from the cost matrix Feature consistency calculation cost matrix as follows:
[0061]
[0062] Among them, c ijRepresents the element in row i and column j of the feature consistency calculation cost matrix C; Represents the i-th source point p in the source point cloud P i The regularized feature descriptor of Represents the jth target point q in the target point cloud Q j The regularized feature descriptor.
[0063] Step 1.3) In each row of the distribution matrix A, the indexes of the 8 largest elements are selected using the top-K index selection method to extract several source points and several target points corresponding to each source point, and construct an initial matching point set.
[0064] Step 1.4) Use the geometric consistency based on curvature and distance to filter the initial matching point set to construct the matching point set. For each source point and its target point in the initial matching point set, when the curvature error between the source point and the target point is less than the curvature threshold C th , then retain the current source and target points; for each retained source point and its respective target points, the distance error between them is less than the distance threshold D th The two or more target points are clustered into one target point, thereby obtaining the source point and one or more target points after clustering and constructing them into a group of matching points. Each group of matching points is constructed as a matching point set, where the curvature threshold C th Set to 0.1m, the distance threshold D th Set to 0.1m. The corresponding points in the matching point set are as follows Figure 5 shown.
[0065] Step 2) Use a high-level consistency method to screen and sample matching tetrahedron sets between the source point cloud and the target point cloud in the matching point set, as follows:
[0066] Step 2.1) Obtain several source point cloud lines and target point cloud lines based on each source point and target point in the matching point set, and then construct a matching line set after re-matching, as follows:
[0067] Step 2.1.1) Connect every two source points in the matching point set to obtain the source point cloud line L <p i p j >, connect every two target points in the matching point set to obtain the target point cloud line L i q j >, each source point cloud line and target point cloud line are constructed as an initial matching line set, and the normal vector angle in the initial matching line set is retained to be greater than the preset first angle threshold θ 1th And the line length is greater than the preset first distance threshold d 1th Several source point cloud lines and target point cloud lines, ‖p i -p j ‖2>d1th , thus constructing the initial line set, where θ 1th Set to 25°, d 1th Set to 0.1m.
[0068] Step 2.1.2) Establish a local coordinate system on each source point cloud line of the initial line set to obtain the quaternion of each source point cloud line like Figure 2 As shown, α p , β p and They represent the three angle relationships between the local coordinate system of the source point cloud and the normal vectors of the two end points of the source point cloud, α p Represents endpoint p j The angle between the projection of the normal vector on the vw plane of the local coordinate system and the v axis, β p Represents endpoint p j The angle between the projection on the uw plane of the local coordinate system and the w axis, Represents endpoint p i The angle between the local coordinate system and the u-axis, d p Indicates the line length of the current source point cloud line; establish a local coordinate system on each target point cloud line in the initial line set to obtain the quaternion of each target point cloud line α q , β q 、 They represent the three angle relationships between the local coordinate system of the target point cloud and the normal vectors of the two endpoints of the target point cloud, α q Represents endpoint q j The angle between the projection of the normal vector on the vw plane of the local coordinate system and the v axis, β q Represents endpoint q j The angle between the projection on the uw plane of the local coordinate system and the w axis, Represents endpoint q i The angle between the local coordinate system and the u-axis, d q Indicates the length of the current target point cloud line; match the quaternion of each source point cloud line and the target point cloud line, and when When , the match is successful, α th , β th 、 and d 2th Respectively represent the preset second angle threshold, the preset third angle threshold, the preset fourth angle threshold and the preset second distance threshold, and construct each successfully matched source point cloud line and target point cloud line into a matching line set, where α th , β th , All are set to 3°, d2th Set to 0.05m, if the quadruple difference is less than the threshold, the matching line is sampled from the line set and placed in the matching line set. The corresponding points in the matching line set are as follows: Figure 6 shown.
[0069] The local coordinate system established on the source point cloud line of the initial line set is as follows:
[0070]
[0071] Among them, μ p 、v p and ω p They respectively represent the X-axis direction, Y-axis direction, and Z-axis direction of the local coordinate system established on the source point cloud line; Represents the normal vector of the i-th source point cloud line.
[0072] Based on the local coordinate system, calculate a set of quaternions as follows:
[0073]
[0074] Step 2.2) Obtain several source point cloud tetrahedrons and target point cloud tetrahedrons based on the source point cloud lines and target point cloud lines in the matching line set, and then construct a matching tetrahedron set after screening and collection, as follows:
[0075] Step 2.2.1) Aggregate each two source point cloud lines in the matching line set to form a source point cloud tetrahedron T{L <p i p j >,L <p m p n >}, aggregate each two target point cloud lines in the matching point set to form a target point cloud tetrahedron T{L i q j >,L m q n >}, each source point cloud tetrahedron and target point cloud tetrahedron is constructed as an initial tetrahedron set.
[0076] Step 2.2.2) For each source point cloud tetrahedron and target point cloud tetrahedron in the initial tetrahedron set, filter them according to the intersection angle and distance conditions as follows:
[0077] θ>θ 2th
[0078] d r >d 3th str=1,2,3
[0079] Among them, θ represents the intersection angle between two source point cloud lines in the source point cloud tetrahedron or between two target point cloud lines in the target point cloud tetrahedron, such as L in the source point cloud tetrahedron <p m p n > and L <p i p j >The crossing angle, θ 2th Indicates the preset fifth angle threshold; d 1 Indicates the first distance between the first source points of two source point cloud lines in the source point cloud tetrahedron or the first target points of two target point cloud lines in the target point cloud tetrahedron, such as from p i to p m The distance, d 2 Indicates the second distance between two source point cloud lines in the source point cloud tetrahedron or between two target point cloud lines in the target point cloud tetrahedron, such as line L <p m p n > to L <p i p j > the distance, d 3 Indicates the third distance between the second source points of two source point cloud lines in the source point cloud tetrahedron or the second target points of two target point cloud lines in the target point cloud tetrahedron, such as from p j to p n distance; d 3th θ represents the preset third distance threshold. 2th Set to 25°, d 3th Set to 0.1m.
[0080] The source point cloud tetrahedrons and target point cloud tetrahedrons that meet the intersection angle and distance conditions are screened and constructed into a screened tetrahedron set.
[0081] Step 2.2.3) Construct a quintuple of each source point cloud tetrahedron in the screening tetrahedron set And the quintuple of each target point cloud tetrahedron like Figure 3 As shown, θ p and θ q They represent the intersection angles between two source point cloud lines in the source point cloud tetrahedron and between two target point cloud lines in the target point cloud tetrahedron, and Represent the first, second and third distances of the source point cloud tetrahedron, and Represent the first, second and third distances of the target point cloud tetrahedron, Represents the cross product of two source point cloud line vectors in the source point cloud tetrahedron, that is, the vector and The cross product of represents the direction of the quintuple, Represents the cross product of two target point cloud line vectors in the target point cloud tetrahedron.
[0082] Step 2.2.4) Match the quintuplets of each source point cloud tetrahedron and target point cloud tetrahedron. When , the matching is successful, θ 3th Indicates the preset sixth angle threshold, d 4th Indicates the preset fourth distance threshold, Represents the direction threshold, and constructs each successfully matched source point cloud tetrahedron and target point cloud tetrahedron into a matching tetrahedron set, where θ 3th Set to 10°, d 4th Set to 0.05m, Set to The corresponding points in the matching tetrahedron set are as follows Figure 7 shown.
[0083] Step 3) Based on the set of matching tetrahedrons, the optimal transformation matrix between the source point cloud and the target point cloud of the current entity object is obtained using a coarse-to-fine estimation, as follows:
[0084] Step 3.1) In the rough estimation stage, for each pair of matching source point cloud tetrahedrons and target point cloud tetrahedrons in the matching tetrahedron set, the singular value decomposition (SVD) algorithm is used to obtain the initial transformation matrix T between the source point cloud tetrahedron and the target point cloud tetrahedron, as follows:
[0085]
[0086] Where R and t represent the rotation matrix and translation vector of the initial transformation matrix T, respectively.
[0087] Step 3.2) For each initial transformation matrix T, the rotation matrix R is converted into Euler angles Ea. The Euler angles Ea are composed of three angles {α, β, γ}, representing the roll angle, pitch angle, and yaw angle, respectively. The translation vector t and the Euler angles Ea are then combined into a six-dimensional attitude vector {x, y, z, α, β, γ}, where {x, y, z} represents the translation in the x, y, and z directions, respectively. All six-dimensional attitude vectors are mapped into six-dimensional space, and each six-dimensional attitude vector is thinned using each six-dimensional block in the six-dimensional space to obtain a set of remapping transformation matrices. The six-dimensional space is partitioned using six-dimensional blocks with a leaf node size of 0.01m. Each six-dimensional block contains several six-dimensional attitude vectors. During thinning, for each six-dimensional block, the center of the six-dimensional block is used as the representation of all six-dimensional attitude vectors within the six-dimensional block. The six-dimensional attitude vectors are then remapped into a remapping transformation matrix. Each remapping transformation matrix is constructed into a set of remapping transformation matrices.
[0088] Step 3.3) Based on the matching point set in step 1), the remapping transformation matrix with the largest matching point pair in the remapping transformation matrix set is used as the optimal rough transformation matrix T c Specifically, for each source point in the matching point set in step 1) and its remapping transformation matrix, the source point is transformed using the remapping transformation matrix to obtain a number of transformed source points. The distance between the transformed source point and each target point is calculated, and the number of transformed source points whose distance to the target point is less than a fifth distance threshold τ is obtained as the matching number, τ = 0.05m. The remapping transformation matrix corresponding to the maximum matching number is taken as the optimal coarse transformation matrix T c , that is, take the coarse transformation matrix T with the largest number of internal points c As a rough estimate.
[0089] Step 3.4) In the fine estimation stage, according to the matching point set in step 1), the optimal coarse transformation matrix T is used c Resample the matching point set to obtain the refined transformation matrix T f , and finally obtain the optimal transformation matrix T o , T o =T f ×T c Specifically, for each source point in the matching point set in step 1), the optimal coarse transformation matrix T is used. c After transforming the source point, several coarse transformation source points are obtained. For each coarse transformation source point, the distance between the coarse transformation source point and the target point is calculated, and the distance between the coarse transformation source point and the target point is retained. 6th The coarse transformation source point, d 6th =0.05m, construct the retained coarse transformation source points and their corresponding target points into the final matching point set, and use the singular value decomposition SVD algorithm on the final matching point set to obtain the fine transformation matrix T f .
[0090] When the target entity object is registered, the source point cloud of the target entity object is obtained and the optimal transformation matrix T is used to calculate the point cloud. o The source point cloud is converted to the target point cloud and displayed on the monitor. The source point cloud and target point cloud after registration are obtained as follows Figure 8 As shown in Figure 2, the registration of partially overlapping point clouds is completed.
[0091] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems or computer program products. Therefore, the application can adopt the form of a complete hardware embodiment, a complete software embodiment or an embodiment in combination with software and hardware. Moreover, the application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, optical storage, etc.) that contain computer-usable program code. The scheme in the embodiments of the present application can be implemented in various computer languages. The application is described according to the flow chart of the method, system and computer program product of the embodiments of the present application.
[0092] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concepts. Therefore, the present invention is intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.
[0093] Obviously, those skilled in the art may make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the equivalent technology of the present invention, the present application is intended to include these modifications and variations.
Claims
1. A partially overlapping point cloud registration method based on dual consistency, characterized in that: include: Step 1) obtaining the source point cloud and target point cloud of the current entity object through the sensor, and generating a matching point set between the source point cloud and the target point cloud using a low-level consistency method; Step 2) using a high-level consistency method to screen and sample matching tetrahedron sets between the source point cloud and the target point cloud in the matching point set; Step 3) Based on the set of matching tetrahedrons, the optimal transformation matrix between the source point cloud and the target point cloud of the current entity object is obtained using a coarse-to-fine estimation; Step 4) When the target entity object is to be registered, the source point cloud of the target entity object is obtained, and the source point cloud is converted to the target point cloud through the optimal transformation matrix and displayed on the display, completing the registration of partially overlapping point clouds.
2. The method for partially overlapping point cloud registration based on dual consistency according to claim 1, characterized in that: In step 1), a low-level consistency method is used to generate a matching point set between the source point cloud and the target point cloud, as follows: Step 1.1) Obtain the feature descriptors of each point in the source point cloud and the target point cloud of the current entity object, and then use the L2-norm regularization method to regularize each feature descriptor; Step 1.2) For each pair of regularized feature descriptors in the source and target point clouds, obtain the feature consistency calculation cost matrix C of the current pair of feature descriptors, and use the Sink-Horn algorithm to obtain the allocation matrix A from the cost matrix; Step 1.3) In each row of the distribution matrix A, use the index selection top-K method to select the indexes of the k largest elements, thereby extracting several source points and several target points corresponding to each source point, and constructing an initial matching point set; In step 1.4), the initial matching point set is screened using geometric consistency based on curvature and distance to construct a matching point set.
3. The method for partially overlapping point cloud registration based on dual consistency according to claim 2, characterized in that: In the step 1.4), for each source point and its target point in the initial matching point set, when the curvature error between the source point and the target point is less than the curvature threshold C th , then keep the current source and target points; For each retained source point and its respective target point, the distance error between them is less than the distance threshold D th The two or more target points are clustered into one target point, so as to obtain the source point and one or more target points after clustering and construct them into a group of matching points, and each group of matching points is constructed as a matching point set.
4. The method for partially overlapping point cloud registration based on dual consistency according to claim 1, characterized in that: The step 2) is as follows: Step 2.1) Obtain a number of source point cloud lines and target point cloud lines based on each source point and target point in the matching point set, and then construct a matching line set after re-matching; Step 2.2) A plurality of source point cloud tetrahedrons and target point cloud tetrahedrons are obtained according to each source point cloud line and target point cloud line in the matching line set, and then a matching tetrahedron set is constructed after screening and collection.
5. The method for partially overlapping point cloud registration based on dual consistency according to claim 4, characterized in that: The step 2.1) is as follows: Step 2.1.1) Connect every two source points in the matching point set to obtain a source point cloud line, and connect every two target points in the matching point set to obtain a target point cloud line. Each source point cloud line and target point cloud line are constructed as an initial matching line set, and the normal vector angles in the initial matching line set are retained to be greater than the preset first angle threshold θ. 1th And the line length is greater than the preset first distance threshold d 1th Several source point cloud lines and target point cloud lines are obtained to construct an initial line set; Step 2.1.2) Establish a local coordinate system on each source point cloud line of the initial line set to obtain the quaternion of each source point cloud line α p , β p and They represent the three angle relationships between the local coordinate system of the source point cloud and the normal vectors of the two end points of the source point cloud, d p Indicates the line length of the current source point cloud line; establish a local coordinate system on each target point cloud line in the initial line set to obtain the quaternion of each target point cloud line α q , β q 、 They represent the three angle relationships between the local coordinate system of the target point cloud and the normal vectors of the two endpoints of the target point cloud, d q Indicates the length of the current target point cloud line; matches the quaternion of each source point cloud line and the target point cloud line. When , the match is successful, α th , β th 、 and d 2th They respectively represent the preset second angle threshold, the preset third angle threshold, the preset fourth angle threshold and the preset second distance threshold, and construct each successfully matched source point cloud line and target point cloud line into a matching line set.
6. The method for partially overlapping point cloud registration based on dual consistency according to claim 4, characterized in that: The step 2.2) is as follows: Step 2.2.1) Every two source point cloud lines in the matching line set are aggregated into a source point cloud tetrahedron, and every two target point cloud lines in the matching point set are aggregated into a target point cloud tetrahedron. Each source point cloud tetrahedron and target point cloud tetrahedron is constructed into an initial tetrahedron set. Step 2.2.2) For each source point cloud tetrahedron and target point cloud tetrahedron in the initial tetrahedron set, filter them according to the intersection angle and distance conditions as follows: θ>θ 2th d r >d 3th s.t.r=1,2,3 Among them, θ represents the intersection angle between two source point cloud lines in the source point cloud tetrahedron or between two target point cloud lines in the target point cloud tetrahedron, θ 2th Indicates the preset fifth angle threshold; d 1 Indicates the first distance between the first source points of two source point cloud lines in the source point cloud tetrahedron or the first target points of two target point cloud lines in the target point cloud tetrahedron, d 2 Indicates the second distance between two source point cloud lines in the source point cloud tetrahedron or between two target point cloud lines in the target point cloud tetrahedron, d 3 Indicates the third distance between the second source points of two source point cloud lines in the source point cloud tetrahedron or the second target points of two target point cloud lines in the target point cloud tetrahedron; d 3th Indicates a preset third distance threshold; The source point cloud tetrahedrons and target point cloud tetrahedrons that meet the intersection angle and distance conditions are screened to form a screened tetrahedron set; Step 2.2.3) Construct a quintuple of each source point cloud tetrahedron in the screening tetrahedron set And the quintuple of each target point cloud tetrahedron θ p and θ q They represent the intersection angles between two source point cloud lines in the source point cloud tetrahedron and between two target point cloud lines in the target point cloud tetrahedron, and Represent the first, second and third distances of the source point cloud tetrahedron, and Represent the first, second and third distances of the target point cloud tetrahedron, represents the cross product of two source point cloud line vectors in the source point cloud tetrahedron, Represents the cross product of two target point cloud line vectors in the target point cloud tetrahedron; Step 2.2.4) Match the quintuplets of each source point cloud tetrahedron and target point cloud tetrahedron. When , the matching is successful, θ 3th Indicates the preset sixth angle threshold, D 4th Indicates the preset fourth distance threshold, Represents the direction threshold, and the successfully matched source point cloud tetrahedrons and target point cloud tetrahedrons are constructed into a matching tetrahedron set.
7. The method for partially overlapping point cloud registration based on dual consistency according to claim 1, characterized in that: The step 3) is as follows: Step 3.1) In the rough estimation stage, for each pair of matching source point cloud tetrahedrons and target point cloud tetrahedrons in the matching tetrahedron set, the singular value decomposition (SVD) algorithm is used to obtain the initial transformation matrix T between the source point cloud tetrahedron and the target point cloud tetrahedron, as follows: Where R and t represent the rotation matrix and translation vector of the initial transformation matrix T respectively; Step 3.2) For each initial transformation matrix T, convert the rotation matrix R into Euler angles Ea. Then combine the translation vector t and the Euler angles Ea into a six-dimensional pose vector. Map all six-dimensional pose vectors into six-dimensional space, and sparsify each six-dimensional pose vector using each six-dimensional block in the six-dimensional space to obtain a set of remapping transformation matrices. Step 3.3) Based on the matching point set in step 1), the remapping transformation matrix with the largest matching point pair in the remapping transformation matrix set is used as the optimal rough transformation matrix T c ; Step 3.4) In the fine estimation stage, according to the matching point set in step 1), the optimal coarse transformation matrix T is used c Resample the matching point set to obtain the refined transformation matrix T f , and finally obtain the optimal transformation matrix T o , T o =T f ×T c .
8. The method for partially overlapping point cloud registration based on dual consistency according to claim 7, characterized in that: In the step 3.2), the six-dimensional space is divided into several six-dimensional blocks according to the preset leaf node size, each six-dimensional block contains several six-dimensional posture vectors, and when sparsifying, for each six-dimensional block, the center of the six-dimensional block is used as the representation of all six-dimensional posture vectors in the six-dimensional block, and then remapped into a remapping transformation matrix, and each remapping transformation matrix is constructed into a remapping transformation matrix set.
9. The method for partially overlapping point cloud registration based on dual consistency according to claim 7, characterized in that: In step 3.3), for each source point in the matching point set in step 1) and its remapping transformation matrix, the source point is transformed using the remapping transformation matrix to obtain a number of transformed source points, the distance between the transformed source point and each target point is calculated, and the number of transformed source points whose distance to the target point is less than the fifth distance threshold τ is obtained as the number of matches, and the remapping transformation matrix corresponding to the maximum number of matches is used as the optimal coarse transformation matrix T c .
10. The method for partially overlapping point cloud registration based on dual consistency according to claim 7, characterized in that: In step 3.4), for each source point in the matching point set in step 1), the optimal coarse transformation matrix T is used. c After transforming the source point, several coarse transformation source points are obtained. For each coarse transformation source point, the distance between the coarse transformation source point and the target point is calculated, and the distance between the coarse transformation source point and the target point is retained. 6th The coarse transformation source points of the retained coarse transformation source points and their corresponding target points are constructed as the final matching point set, and the fine transformation matrix T is obtained after the singular value decomposition SVD algorithm is used on the final matching point set. f .