Point cloud registration method and system based on accurate centroid corresponding point search

Abstract

The present invention provides a point cloud registration method and system based on searching for accurate corresponding points based on centroids, comprising: step 1: searching for corresponding points between point clouds by obtaining the centroids; step 2: calculating the rotation and translation matrices between point clouds based on the singular value decomposition method and the corresponding points between point clouds, and then performing point cloud registration based on the rotation and translation matrices. The algorithm for searching for accurate corresponding points proposed by the present invention can quickly and accurately find the corresponding relationship between point clouds without going through an optimization process, greatly improving the efficiency of the point cloud registration algorithm.

Description

Technical Field

[0001] The present invention relates to the technical field of 3D point cloud registration, and in particular to a point cloud registration method and system based on searching for accurate corresponding points of centroids. Background Art

[0002] In recent years, with the development of a new generation of depth sensors, the application of 3D point cloud data has become increasingly popular. Point cloud registration, as a key technology for point cloud data processing, has been widely used in computer vision, target reconstruction, image recognition, robotics and other related fields. The purpose of point cloud registration is to obtain the rotation and translation change matrix between the source point cloud and the target point cloud. Since the corresponding relationship between the source point cloud and the target point cloud is unknown, accurate and fast point cloud registration becomes a very difficult problem.

[0003] The most classic ICP algorithm in point cloud registration is an iterative closest point algorithm proposed by Besl et al. The algorithm takes the Euclidean distance between the corresponding points of two point clouds as the objective function and solves the optimal rotation and translation transformation matrix through iterative updating. After that, people have made a series of improvements to this method. To improve the influence of non-overlapping areas, Chetvreikov et al. proposed a clipping ICP algorithm, which introduces the overlap percentage to automatically clip the non-overlapping areas of the point cloud; to improve the influence of outliers, Magnusson et al. proposed a 3DNDT algorithm, which subdivided the three-dimensional space of the point cloud into regular units and used normal distribution to simulate the distribution of data points; Sodien et al. proposed a sparse ICP algorithm, which replaced the norm with the norm; to solve the problem of local convergence, Shi et al. proposed an algorithm combining point cloud filtering and clipping ICP; to solve the problem of slow convergence, Pavlov et al. proposed an AAICP algorithm based on Anderson acceleration. In addition, many methods using some point features such as the fast point feature histogram (FPFH) have been proposed to provide a good initial value for registration. In order to solve the point cloud registration error caused by point-to-point, Chen et al. proposed an iterative closest point algorithm based on point-to-plane error measurement, but it uses nonlinear least squares method to solve, which makes the algorithm very inefficient. Rusinkiewicz, Low et al. converted it into a linear solution problem, and the efficiency was significantly improved. In addition to ICP algorithms, Gojcic et al. proposed an end-to-end, data-driven point cloud registration algorithm, which directly learns to register all point clouds in a global manner. Lin et al. used a probability-based method to fit the data distribution using a Gaussian mixture model, and then used the expectation maximization algorithm in the probability model to generate data points, directly obtaining the final registration result. Andy et al. used a feature-based deep learning method, which aims to extract the features of three-dimensional points and find accurate correspondences.

[0004] Patent document CN113963129A (application number: CN202111198227.8) discloses a point cloud-based template matching and online recognition method for small ship components, including: placing the template workpiece in the loading scanning area; starting the industrial depth camera; setting the template model, template library save path, and saving the template; placing the workpiece to be polished in the loading scanning area; performing visual recognition of the workpiece; outputting the current workpiece model, posture and center of mass coordinates; and the robotic arm performs subsequent operations on the workpiece.

[0005] Most algorithms use optimization processes to obtain correspondences between point clouds, so the efficiency of the algorithms is poor. Feature-based deep learning algorithms require a large number of training sets. At the same time, if the distribution of the scene and the training data is very different, the registration effect of the algorithm in unknown scenes will be very poor. Existing technologies cannot accurately find correspondences without optimization processes. Summary of the invention

[0006] In view of the defects in the prior art, the object of the present invention is to provide a point cloud registration method and system based on searching for accurate corresponding points of centroid.

[0007] The point cloud registration method based on searching for accurate corresponding points of centroid provided by the present invention comprises:

[0008] Step 1: Find the corresponding points between point clouds by finding the centroid;

[0009] Step 2: Based on the singular value decomposition method and the corresponding points between the point clouds, the rotation and translation matrices between the point clouds are calculated, and then the point clouds are registered according to the rotation and translation matrices;

[0010] For a preset point cloud A, define a series of points {A0, A1, A2, A3…A i , A i+1 …A c}, the centroid A0 of the point cloud is expressed as:

[0011]

[0012] Where N is the number of points in the point cloud; p j is a point in the point cloud A, j = 1, 2…N; (x pj ,y pj ,z pj ) indicates p j The coordinates of

[0013] Set the threshold R, then A i+1 Calculated according to the following formula:

[0014] ||p j' -A i ||2 <R;

[0015] where n i is the number of points in the point cloud within the sphere centered at A i with radius R; p j' represents a point in the point cloud; for the preset point cloud B, {B0, B1, B2... B i , B i+1 ... B c} is obtained in the same way;

[0016] It can be proved by mathematical induction that for the point cloud A and the point cloud B, {A0, A1, A2, A3... A i , A i+1 ... A c} and {B0, B1, B2... B i , B i+1 ... B c} are in one-to-one correspondence and can represent the corresponding relationship between the point cloud A and the point cloud B.

[0017] Preferably, calculate the centroid of the point cloud, and calculate the centroid of the cloud within the sphere centered at this centroid with radius R, then calculate the distance between this centroid and the previous centroid. If the distance is greater than the preset threshold, continue to calculate the centroid of the cloud; if the distance is less than the preset threshold, change the radius size. If the radius is within the preset range, continue to calculate the centroid of the cloud within the sphere; if the radius is outside the preset range, end.

[0018] Preferably, step 1 includes:

[0019] Step 1.1: Calculate the centroids of the point cloud A and the point cloud B, and the expression is:

[0020]

[0021]

[0022] Step 1.2: Set the initial radius R0, then a series of corresponding points A k is expressed as:

[0023]

[0024] represents the number of points in the point cloud A that satisfy ||p i' - A k-1 || 2 < R.

[0025] Step 1.3: Set a radius transformation constant coefficient α;

[0026] If the initial radius R0 takes a preset large value, then α takes a constant less than 1;

[0027] If the initial radius R0 takes a preset small value, then α takes a constant greater than 1.

[0028] Preferably, the initial radius R0 takes a preset small value, and a threshold ε and R are set ε ;

[0029] If ||A k - A k-1 || 2 < ε, then R = αR. If R < R ε , then return to step 1.2 and continue to execute; if R > R ε , then end the calculation process and output the corresponding points {A0, A1, A2... A k-1} and the corresponding points {B0, B1, B2... B k-1};

[0030] If ||A k - A k-1 || 2 > ε, then calculate:

[0031]

[0032] Denote the number of points in point cloud B that satisfy ||p j' - B k-1 || 2 < R as k, and after k = k + 1, return to step 1.2 and continue to execute.

[0033] Preferably, step 2 includes:

[0034] Obtain the rotation and translation transformation matrix of point cloud A and point cloud B based on the singular value decomposition method. The expression is:

[0035]

[0036]

[0037] Through obtain matrix H, and decompose the matrix. The expression is: H = UΛV T ;

[0038] Calculate the rotation transformation matrix The expression is: U and V are matrices in the decomposition process;

[0039] Calculate the translation matrix. The expression is:

[0040] According to the point cloud registration system for accurately finding corresponding points based on the centroid provided by the present invention, it includes:

[0041] Module M1: Find the corresponding points between point clouds by finding the centroid;

[0042] Module M2: Based on the singular value decomposition method and the corresponding points between the point clouds, the rotation and translation matrices between the point clouds are calculated, and then the point clouds are registered according to the rotation and translation matrices;

[0043] For a preset point cloud A, define a series of points {A0, A1, A2, A3…A i , A i+1 …A c}, the centroid A0 of the point cloud is expressed as:

[0044]

[0045] Where N is the number of points in the point cloud; p j is a point in point cloud A, j = 1, 2…N; Indicates p j The coordinates of

[0046] Set the threshold R, then A i+1 Calculated according to the following formula:

[0047] ||p j' -A i || 2 <R;

[0048] Among them, n i A i The number of points in the point cloud within the sphere with R as the radius and p as the center; j' Represents a point in the point cloud; for the preset point cloud B, {B0, B1, B2…B i , B i+1 …B c};

[0049] By mathematical induction, it can be proved that for point cloud A and point cloud B, {A0, A1, A2, A3…A i , A i+1 …A c} and {B0, B1, B2…B i , B i+1 …B c}One-to-one correspondence and can represent the correspondence between point cloud A and point cloud B.

[0050] Preferably, the centroid of the point cloud is calculated, and the centroid of the cloud within the sphere centered at the centroid with a radius of R is calculated. Then, the distance between this centroid and the previous centroid is calculated. If the distance is greater than the preset threshold, the cloud centroid is continuously obtained; if the distance is less than the preset threshold, the radius size is changed. If the radius is within the preset range, the centroid of the cloud within the sphere is continuously calculated; if the radius is outside the preset range, the process ends.

[0051] Preferably, the module M1 includes:

[0052] Module M1.1: Calculate the centroids of point cloud A and point cloud B, and the expression is:

[0053]

[0054]

[0055] Module M1.2: Set the initial radius R0, then a series of corresponding points A k is expressed as:

[0056]

[0057] represents the number of points in point cloud A that satisfy ||p i' -A k-1 || 2 <R;

[0058] Module M1.3: Set a radius transformation constant coefficient α;

[0059] If the initial radius R0 takes a preset large value, α takes a constant less than 1;

[0060] If the initial radius R0 takes a preset small value, α takes a constant greater than 1.

[0061] Preferably, the initial radius R0 takes a preset small value, and thresholds ε and R ε ;

[0062] If ||A k -A k-1 || 2 <ε, then R = αR. If R < R ε , then return to module M1.2; if R > R ε , then end the calculation process and output the corresponding points {A0, A1, A2…A k-1} of A and the corresponding points {B0, B1, B2…B k-1} of B;

[0063] If ||A k -A k-1 || 2> ε, then calculate:

[0064]

[0065] Indicates the number of points in point cloud B that satisfy ||p j' - B k-1 || 2 < R, and then let k = k + 1 and return to module M1.2.

[0066] Preferably, the module M2 includes:

[0067] Obtain the rotation and translation transformation matrix of point cloud A and point cloud B based on the singular value decomposition method. The expression is:

[0068]

[0069]

[0070] Through Obtain matrix H and decompose the matrix. The expression is: H = UΛV T ;

[0071] Calculate the rotation transformation matrix The expression is: U and V are matrices in the decomposition process;

[0072] Calculate the translation matrix. The expression is:

[0073] Compared with the prior art, the present invention has the following beneficial effects:

[0074] (1) The algorithm for finding accurate corresponding points proposed by the present invention can quickly and accurately find the corresponding relationship between point clouds without going through an optimization process, greatly improving the efficiency of the point cloud registration algorithm;

[0075] (2) The algorithm proposed by the present invention is used in the application of pose recognition of the grasping head of a shield machine and then visual feedback control, improving the accuracy of the shield machine in grasping segments. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] By reading the detailed description of the non-restrictive embodiments with reference to the following drawings, other features, objects, and advantages of the present invention will become more apparent:

[0077] Figure 1 It is a schematic diagram of the algorithm for obtaining corresponding points of point clouds. DETAILED DESCRIPTION OF THE INVENTION

[0078] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0079] Example:

[0080] The present invention applies shield machine segment posture recognition and adopts the Tuyang-FS820 depth camera to collect data. After the point cloud data of the segment is collected, the algorithm of the present invention is used for point cloud registration after preprocessing and point cloud segmentation, so as to quickly and accurately obtain the segment's posture.

[0081] The point cloud registration method proposed in the present invention is divided into two steps: 1. Using a method similar to obtaining the centroid to find the corresponding points between the point clouds; 2. Using the SVD-based method and the corresponding points obtained in the first step to calculate the rotation and translation matrix between the point clouds.

[0082] For point cloud A, define a series of points {A0, A1, A2, A3…A i , A i+1 …A c}, the centroid A0 of the point cloud can be expressed as follows:

[0083]

[0084] N is the number of points in the point cloud.

[0085] p j is a point in point cloud A, j = 1, 2…N; Indicates p j The coordinates of .

[0086] Given a threshold R, A i+1 It can be calculated as follows:

[0087]

[0088] Among them, n i A i The number of points in the point cloud within the sphere with R as the radius and p as the center; j' For point cloud B, {B0, B1, B2…B i , B i+1 …B c}.

[0089] By mathematical induction, it can be proved that for point cloud A and point cloud B, {A0, A1, A2, A3…A i , Ai+1 …A c}, which corresponds one-to-one with {B0, B1, B2…B i , B i+1 …B c}, and can represent the corresponding relationship between point cloud A and point cloud B.

[0090] For example Figure 1 , it is the schematic diagram of the algorithm for obtaining corresponding points of the point cloud. Specifically:

[0091] Step 1: Calculate the centroid of the point cloud;

[0092] Step 2: Calculate the centroid of the cloud within the sphere with the centroid as the center and radius R;

[0093] Step 3: Calculate the distance between this centroid and the previous centroid. If the distance is greater than the threshold, return to Step 1 and continue to execute; if the distance is less than the threshold, execute Step 4;

[0094] Step 4: Change the radius size. If the radius is within the specified range, return to Step 2 and continue to execute; if the radius is outside the specified range, end.

[0095] Given two point clouds A, B;

[0096] Step1: Calculate the centroids of point cloud A and point cloud B;

[0097]

[0098]

[0099] Step2: Given an initial radius R0, then a series of corresponding points A k (k = 1, 2, …) are expressed as:

[0100]

[0101] represents the number of points in point cloud A that satisfy ||p i' - A k-1 || 2 < R.

[0102] Step3: Set a radius transformation constant coefficient α;

[0103] If the initial radius R0 takes a preset large value, then α takes a constant less than 1;

[0104] If the initial radius R0 takes a preset small value, then α takes a constant greater than 1.

[0105] The following steps take the case where the initial radius R0 takes a small value as an example, and set the threshold ε and R ε .

[0106] If ||A k -A k-1 || 2 < ε, then R = αR, and then judge the size of R. If R < R ε , then return to Step2; if R > R ε , then end the calculation process and output the corresponding points {A0, A1, A2... A k-1} and the corresponding points {B0, B1, B2... B k-1}.

[0107] If ||A k -A k-1 || 2 > ε, then calculate: Denote the number of points in point cloud B that satisfy ||p j' -B k-1 || 2 < R, and then let k = k + 1 and return to Step2.

[0108] After this process ends, {A0, A1, A2... A N}, {B0, B1, B2... B N} corresponding points will be obtained. After obtaining these corresponding points, the rotation and translation transformation matrices of point cloud A and point cloud B can be obtained through corresponding point alignment algorithms based on singular value decomposition, unit quaternion, biquaternion, rotation matrix orthogonality, etc.

[0109] Take the singular value decomposition as an example:

[0110] In the previous part, the corresponding points {A0, A1, A2... A N}, {B0, B1, B2... B N} have been obtained.

[0111]

[0112]

[0113] Through obtain matrix H, and then decompose the matrix according to the following formula: H = UΛV T ;

[0114] Calculate the rotation transformation matrix through the decomposed matrix

[0115] Calculate the translation matrix through the following formula:

[0116] The point cloud registration system based on accurate corresponding point search of the centroid provided by the present invention includes: Module M1: Search for corresponding points between point clouds by calculating the centroid; Module M2: Calculate the rotation and translation matrix between point clouds based on the singular value decomposition method and the corresponding points between point clouds, and then perform point cloud registration according to the rotation and translation matrix.

[0117] For the preset point cloud A, a series of points {A0, A1, A2, A3... A i , A i+1 ... A c} are defined, and the centroid A0 of the point cloud is expressed as: Where N is the number of points in the point cloud; p j is a point in the point cloud A, and j = 1, 2,..., N; represents the coordinate of p j ; Set the threshold R, then A i+1 is calculated according to the following formula: ||p j' - A i || 2 < R; where n i is the number of points in the point cloud within the sphere centered at A i with a radius of R; p j' represents the points in the point cloud; for the preset point cloud B, {B0, B1, B2... B i , B i+1 ... B c} is obtained in the same way; it can be proved by mathematical induction that for the point cloud A and the point cloud B, {A0, A1, A2, A3... A i , A i+1 ... A c} and {B0, B1, B2... B i , B i+1 ... B c} are in one-to-one correspondence and can represent the corresponding relationship between the point cloud A and the point cloud B.

[0118] Find the centroid of the point cloud, calculate the centroid of the cloud within the sphere centered at this centroid with a radius of R, then calculate the distance between this centroid and the previous centroid. If the distance is greater than the preset threshold, continue to find the centroid of the cloud; if the distance is less than the preset threshold, change the radius size. If the radius is within the preset range, continue to calculate the centroid of the cloud within the sphere; if the radius is outside the preset range, end.

[0119] The module M1 includes: Module M1.1: Calculate the centroids of the point cloud A and the point cloud B, and the expression is:

[0120] Module M1.2: Set the initial radius R0, then a series of corresponding points A kExpressed as: Indicates that ||p i' -A k-1 || 2 ; the number of points in the point cloud A that satisfy <R;

[0121] Module M1.3: Set a constant coefficient α for radius transformation; if the initial radius R0 takes a preset large value, then α takes a constant less than 1; if the initial radius R0 takes a preset small value, then α takes a constant greater than 1.

[0122] The initial radius R0 takes a preset small value, and set the threshold ε and R ε ; if ||A k -A k-1 || 2 <ε, then R = αR, if R < R ε , then return to module M1.2; if R > R ε , then end the calculation process and output the corresponding points {A0, A1, A2... A k-1} and the corresponding points {B0, B1, B2... B k-1}; if ||A k -A k-1 || 2 >ε, then calculate: Indicates that ||p j' -B k-1 || 2 ; the number of points in the point cloud B that satisfy <R, and then return to module M1.2 after k = k + 1.

[0123] The module M2 includes: obtaining the rotation and translation transformation matrix of the point cloud A and the point cloud B based on the singular value decomposition method, and the expression is: Through Obtain the matrix H and decompose the matrix, and the expression is: H = UΛV T ; calculate the rotation transformation matrix The expression is: U and V are the matrices in the decomposition process; calculate the translation matrix expression as:

[0124] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0125] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A point cloud registration method based on finding accurate corresponding points of centroid, characterized in that: include: Step 1: Find the corresponding points between point clouds by finding the centroid. The specific process includes: For a preset point cloud A, define a series of points {A0, A1, A2, A3…A i , A i+1 …A c }, the centroid A0 of the point cloud is expressed as: Where N is the number of points in the point cloud; p j is a point in point cloud A, j = 1, 2…N; Indicates p j The coordinates of Set the threshold R, then A i+1 Calculated according to the following formula: Among them, n i A i The number of points in the point cloud within the sphere with R as the radius and p as the center; j′ Represents a point in the point cloud; for the preset point cloud B, {B0, B1, B2…B i , B i+1 …B c }; By mathematical induction, it can be proved that for point cloud A and point cloud B, {A0, A1, A2, A3…A i , A i+1 …A c } and {B0, B1, B2…B i , B i+1 …B c } One-to-one correspondence and can represent the correspondence between point cloud A and point cloud B; Calculate the centroid of point cloud A and point cloud B. The expression is: Set the initial radius R0, then a series of corresponding points A k It is expressed as: Indicates that the number of points in point cloud A that satisfy ||p i′ -A k-1 || 2 <The number of points in point cloud A that satisfy <R; Set a radius transformation constant coefficient α; If the initial radius R0 takes a preset large value, α takes a constant less than 1; If the initial radius R0 takes a preset small value, α takes a constant greater than 1; The initial radius R0 takes a preset small value, and sets the threshold ε and R ε ; If ‖A k -A k-1 ‖ 2 <ε, then R=αR, if R <R ε , then find the corresponding points between the point clouds by obtaining the centroid again; if R>R ε , then the calculation process ends and the corresponding points of A {A0, A1, A2…A k-1 } and the corresponding points of B {B0,B1,B2…B k-1 }; If ‖A k -A k-1 ‖ 2 >ε, then calculate: Indicates that it satisfies ||p j′ -B k-1 || 2 The number of points in the point cloud B that <R, and after k = k + 1, re - find the corresponding points between the point clouds by finding the centroid; Step 2: Based on the singular value decomposition method and the corresponding points between the point clouds, the rotation and translation matrices between the point clouds are calculated, and then the point clouds are registered according to the rotation and translation matrices; The steps to calculate the rotation and translation matrix between point clouds include: pass Obtain the matrix H and decompose the matrix. The expression is: H = UΛV t ; Calculate the rotation transformation matrix The expression is: U and V are matrices in the decomposition process; The translation matrix expression is:

2. A point cloud registration system based on accurate centroid corresponding point search, characterized in that: include: Module M1: Find the corresponding points between point clouds by finding the centroid. The specific process includes: For a preset point cloud A, define a series of points {A0, A1, A2, A3…A i , A i+1 …A c }, the centroid A0 of the point cloud is expressed as: Where N is the number of points in the point cloud; p j is a point in point cloud A, j = 1, 2…N; Indicates p j The coordinates of Set the threshold R, then A i+1 Calculated according to the following formula: Among them, n i A i The number of points in the point cloud within the sphere with R as the radius and p as the center; j′ Represents a point in the point cloud; for the preset point cloud B, {B0, B1, B2…B i , B i+1 …B c }; By mathematical induction, it can be proved that for point cloud A and point cloud B, {A0, A1, A2, A3…A i , A i+1 …A c } and {B0, B1, B2…B i , B i+1 …B c } One-to-one correspondence and can represent the correspondence between point cloud A and point cloud B; Calculate the centroid of point cloud A and point cloud B. The expression is: Set the initial radius R0, then a series of corresponding points A k It is expressed as: Indicates that the number of points in point cloud A that satisfy ||p i′ -A k-1 || 2 <The number of points in point cloud A of <R Set a radius transformation constant coefficient α; If the initial radius R0 takes a preset large value, α takes a constant less than 1; If the initial radius R0 takes a preset small value, α takes a constant greater than 1; The initial radius R0 takes a preset small value, and sets the threshold ε and R ε ; If ‖A k -A k-1 ‖ 2 <ε, then R=αR, if R <R ε , then find the corresponding points between the point clouds by obtaining the centroid again; if R>R ε , then the calculation process ends and the corresponding points of A {A0, A1, A2…A k-1 } and the corresponding points of B {B0,B1,B2…B k-1 }; If ‖A k -A k-1 ‖ 2 >ε, then calculate: Indicates that it satisfies ||p j′ -B k-1 || 2 The number of points in the point cloud B that <R, and after k = k + 1, re - find the corresponding points between the point clouds by finding the centroid; Module M2: Based on the singular value decomposition method and the corresponding points between the point clouds, the rotation and translation matrices between the point clouds are calculated, and then the point clouds are registered according to the rotation and translation matrices; The steps to calculate the rotation and translation matrix between point clouds include: pass Obtain the matrix H and decompose the matrix. The expression is: H = UΛV T ; Calculate the rotation transformation matrix The expression is: U and V are matrices in the decomposition process; The translation matrix expression is:

Images