A three-dimensional point cloud registration method, system and storage medium

Abstract

The present invention belongs to the field of 3D detection and measurement, and specifically relates to a 3D point cloud registration method, system and storage medium. The present invention overcomes the problem that the existing point cloud registration algorithm has low registration accuracy when the two sets of point clouds to be registered have scale transformation. The present invention mainly includes the following steps: first, determine the point cloud {p i}The scale transformation factor, rotation axis vector, and cosine and sine values ​​of the rotation angle of the points A, B, and C in the registration unit are transformed to points A', B', and C'; then the point cloud {p i} to point cloud {p i '}; Finally, using the point cloud {p i} to point cloud {p i '} scale transformation factor and angle transformation matrix to calculate the point cloud {p i} to point cloud {p i '} displacement transformation matrix. In the registration process, the possible scale transformation in different point clouds is considered, and the displacement change matrix is ​​calculated based on the scale transformation factor, which improves the registration accuracy and reduces the registration error.

Description

Technical Field

[0001] The present invention belongs to the field of 3D detection and measurement, and in particular relates to a three-dimensional point cloud registration method system and a storage medium. Background Art

[0002] With the rapid development of computer hardware and computing power, especially the substantial improvement in computing power, 3D point cloud processing technology has also been further developed and applied, including in industrial production, autonomous driving, cultural relics restoration, digital factories, film and television entertainment and other fields.

[0003] However, due to the limitation of the depth sensor's field of view, as well as the influence of factors such as occlusion and high reflection, it is difficult for a single depth sensor to completely reconstruct the object being measured. Therefore, multiple depth sensors are usually required to measure the object being measured from multiple angles and positions. The measurement results are then converted to the same coordinate system and fused into a complete point cloud, which requires point cloud registration.

[0004] Point cloud registration is a challenging task in computer vision, robotics, pattern recognition and image processing. Its goal is to find the transformation matrix of the point cloud of the same object in different coordinate systems, so that the point clouds in different coordinate systems can be transformed into the same coordinate system to form a complete point cloud of the measured object. The point cloud registration algorithm is usually implemented in three steps: the first step is to determine the point correspondence between the two point clouds; the second step is to calculate the transformation matrix between the two point clouds based on the correspondence between the two point clouds; the third step is to use optimization or iterative methods to optimize the transformation.

[0005] Among these three steps, there are many methods for the first and third steps. Among them, the first step is mainly divided into distance-based methods and feature-based methods. The third step is mainly divided into distance-based optimization methods, annealing and soft correspondence-based optimization methods, similarity measurement-based optimization methods, and probability density estimation-based optimization methods. However, in the second step, when calculating the transformation matrix between two point clouds, most algorithms almost choose the SVD algorithm because it not only has fast calculation speed, but also has small registration error when there are only rotation and translation transformations between point clouds.

[0006] When calculating the transformation matrix between two point clouds based on the SVD algorithm, there are three main steps: point cloud decentralization, using the SVD decomposition method to calculate the angle transformation matrix, and calculating the translation matrix. This method works well when there are only rotation and translation transformations between point clouds, but when there is a scale transformation between point clouds due to the errors of different depth sensors (which is almost inevitable), this method not only fails to calculate the scale transformation, but also increases the errors of rotation and translation transformations, resulting in lower registration accuracy. Summary of the invention

[0007] The purpose of the present invention is to provide a three-dimensional point cloud registration method, system and storage medium based on the Rodrigues rotation formula and statistical method, so as to overcome the problem of low registration accuracy in the existing point cloud registration algorithm when there is a scale transformation between the two groups of point clouds to be registered.

[0008] The technical solution of the present invention is to provide a three-dimensional point cloud registration method, which is special in that it includes the following steps:

[0009] Step 1: Define two sets of point clouds to be registered, namely point clouds {p i} and point cloud {p i '}, where i = 1, 2, ..., N, and for the point cloud {p i Any point p in i In the point cloud {p' i There is always a point p' i Corresponding thereto; N is an integer greater than 2;

[0010] Step 2: Point cloud {p i All points in} are grouped into groups of three points, and each group is defined as a registration unit;

[0011] Step 3: Define the three points in each registration unit as point A, point B and point C; in any registration unit, point A, point B and point C are in the point cloud {p' i}The corresponding points in are point A', point B' and point C';

[0012] Calculate point cloud {p i}The scaling factor of points A, B and C in any registration unit to points A', B' and C';

[0013] Calculate point cloud {p i The rotation axis vector of points A, B and C in any registration unit transformed to points A', B' and C';

[0014] Calculate point cloud {p i} The cosine and sine of the rotation angles from points A, B and C in any registration unit to points A', B' and C';

[0015] Step 4: According to the method in step 3, calculate the point cloud {p i} The scale transformation factors, rotation axis vectors, and cosine and sine values ​​of the rotation angles of points A, B, and C in each registration unit transformed to points A', B', and C';

[0016] Step 5: According to the result obtained in step 4, obtain the scale transformation factor histogram, the rotation axis vector histogram, and the rotation angle cosine and sine value histogram; use the scale transformation factor histogram, the rotation axis vector histogram, and the rotation angle cosine and sine value histogram to calculate the point cloud {p i} to point cloud {p' i} scale transformation factor, rotation axis vector, and cosine and sine values ​​of the rotation angle;

[0017] Step 6: Based on the calculation results of step 5, the point cloud {p i} to point cloud {p' i}The angle transformation matrix.

[0018] Step 7: Use the point cloud obtained in step 5 i} to point cloud {p' i} and the angle transformation matrix in step 6 to calculate the point cloud {p i} to point cloud {p' i}'s displacement transformation matrix.

[0019] This gives us the point cloud {p i} to point cloud {p' i}'s transformation matrix (including scale transformation factor, angle transformation matrix and translation transformation matrix).

[0020] Furthermore, in step 3, the point cloud {p i The scale transformation factor scale of points A, B and C in any registration unit to points A', B' and C' is:

[0021]

[0022] Among them, (x A ,y A ,z A ),(x B ,y B ,z B ),(x A' ,y A' ,z A' ),(x B' ,y B' ,z B' ) are the spatial coordinates of points A, B, A', and B' respectively.

[0023] Furthermore, in step 3, the point cloud {p i The rotation axis vector of points A, B and C in any registration unit transformed to points A', B' and C'

[0024]

[0025] This is because if the unit vector About the axis of rotation Rotate to Vector Then the vector Perpendicular to vector Similarly, vector Perpendicular to vector Also perpendicular to the vector So the unit vector The calculation formula is Where normalize() represents normalizing the vector in the brackets.

[0026] Furthermore, the point cloud {p i The cosine value of the rotation angle cosθ of points A, B and C in any registration unit transformed to points A', B' and C':

[0027]

[0028] This is because when calculating the rotation angle of two vectors about the rotation axis, it is necessary to first remove the components of the two vectors in the direction of the rotation axis, and then the rotation angle of the two vectors about the rotation axis can be calculated based on the angle formula of the vectors.

[0029] Furthermore, in step 3, the point cloud {p i The sine value of the rotation angle sinθ of points A, B and C in any registration unit transformed to points A', B' and C':

[0030]

[0031] Furthermore, in step 5, the scale transformation factor histogram is used to calculate the point cloud {p i} to point cloud {p i '}, the scale transformation factor with the largest scale transformation factor histogram data (i.e. the scale transformation factor with the largest probability) is selected as the point cloud {p i} to point cloud {p' i} is the scaling factor.

[0032] Furthermore, in step 5, the point cloud {p i} to point cloud {p i '}, the rotation axis vector with the largest rotation axis vector histogram data (that is, the rotation axis vector with the largest probability) is selected as the point cloud {p i} to point cloud {p' i}'s rotation axis vector.

[0033] Furthermore, in step 5, the point cloud {p i} to point cloud {p' i}, the cosine and sine values ​​of the rotation angle with the largest cosine and sine values ​​in the histogram of the rotation angle cosine and sine values ​​(i.e. the cosine value of the rotation angle with the largest probability) are selected as the point cloud {p i} to point cloud {p' i}The cosine and sine of the rotation angle.

[0034] Furthermore, in step 6, the point cloud {p i} to point cloud {p' i}'s angle transformation matrix R:

[0035] R=I+(1-cosθ)K 2 +sinθK

[0036] Among them, I is the 3*3 identity matrix, and K is expressed as:

[0037]

[0038] Among them, (k x ,k y ,k z ) is the point cloud {p i} to point cloud {p' i}The rotation axis vector coordinates.

[0039] Furthermore, in step 7, the point cloud {p i} to point cloud {p' i}The displacement transformation matrix

[0040]

[0041] In this way, the point cloud {p i} to point cloud {p' i}'s scale transformation factor, angle transformation matrix and displacement vector.

[0042] The present invention also provides a three-dimensional point cloud registration system, including a memory and a processor, wherein the memory stores a computer program, and the special feature thereof is that when the computer program is executed by the processor, any of the above-mentioned three-dimensional point cloud registration methods is implemented.

[0043] The present invention also provides a computer-readable storage medium storing a computer program, the special feature of which is that when the computer program is executed by a processor, any of the above-mentioned three-dimensional point cloud registration methods is implemented.

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

[0045] 1. The three-dimensional point cloud registration method based on the Rodriguez rotation formula and the statistical method of the present invention takes into account the possible scale transformation in different point clouds during the registration process (because different depth sensors have different accuracies, even the same depth sensor will have different accuracies at different distances, so the size of the model they reconstruct for the same object will be different, so the registration of different point clouds generally involves scale transformation), and calculates the displacement change matrix based on the scale transformation factor, thereby improving the registration accuracy and reducing the registration error.

[0046] 2. The present invention eliminates the displacement component from the source point cloud to the destination point cloud by subtracting the points in the point cloud, and eliminates the scale transformation component from the source point cloud to the destination point cloud by normalizing the vector, thereby simplifying the difficulty of calculating the rotation component from the source point cloud to the destination point cloud.

[0047] 3. The present invention uses the relationship between vectors to directly find the rotation axis and rotation angle from the source point cloud to the destination point cloud, and uses the Rodriguez rotation formula to calculate the angle transformation matrix. This method does not involve the process of matrix decomposition, i.e., inversion, so there is no need to consider the failure of matrix decomposition and inversion, which reduces the computational complexity and eliminates the possibility of errors in the final calculation results due to matrix decomposition and inversion failure.

[0048] 4. The present invention uses a histogram (i.e., statistical) method, using the rotation axis vector with the maximum probability, the rotation angle and the scale transformation factor as the global rotation axis, rotation angle and scale transformation factor to eliminate the influence of local outliers and local noise. Compared with the SVD algorithm (which uses global points to participate in the calculation of the transformation matrix), it can greatly reduce the influence of noise on the registration process. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 This is a flow chart of a point cloud registration method based on the Rodrigues rotation formula of an embodiment;

[0050] Figure 2 is a curve diagram showing the relationship between the registration error and the scale transformation factor of the embodiment method and the SVD decomposition method;

[0051] Figure 3 This is a Bremen point cloud image used in the simulation in the embodiment. DETAILED DESCRIPTION

[0052] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments.

[0053] Example

[0054] For point cloud {p i} and point cloud {p' i}, this embodiment calculates the point cloud {p i} to point cloud {p' i} scale transformation factor, rotation transformation matrix and translation transformation vector, such as Figure 1 .

[0055] Step 1: Point cloud {p i The points in} are grouped into groups of three. For the convenience of description, each group can be defined as a registration unit, and the three points in each registration unit can be defined as point A, point B and point C.

[0056] Step 2: For the point cloud {p i Any registration unit in the point cloud {p' i} The corresponding points in are points A', B' and C'. Find the scale transformation factor scale of points A, B and C to points A', B' and C'. The formula is:

[0057]

[0058] Among them, (x A ,y A ,z A ),(x B ,y B ,z B ),(x A' ,y A' ,z A' ),(x B' ,y B' ,z B' ) are the spatial coordinates of points A, B, A', and B' respectively.

[0059] Step 3: Calculate the rotation axis vectors of points A, B and C to points A', B' and C' The formula is:

[0060]

[0061] Where normalize() represents normalizing the vector in the brackets.

[0062] Step 4: Calculate the cosine and sine of the rotation angle from points A, B and C to points A', B' and C'. The formula is:

[0063]

[0064]

[0065] Step 5: According to the calculation methods of steps 2 to 4, calculate the point cloud {pi}The scale transformation factors, rotation axis vectors, and cosine and sine values ​​of the rotation angles of points A, B, and C in each registration unit to points A', B', and C'.

[0066] Step 6: Based on the result of step 5, use the histogram to calculate the point cloud {p i} to point cloud {p' i}The scale transformation factor and rotation axis vector as well as the cosine and sine values ​​of the rotation angle.

[0067] Specifically, based on the result obtained in step 5, a histogram of scale transformation factors, a histogram of rotation axis vectors, and a histogram of cosine and sine values ​​of rotation angles are obtained;

[0068] In the histogram of the scale transformation factor, the rotation axis vector, the cosine value and the sine value of the rotation angle, the scale transformation factor, the rotation axis vector, the cosine value and the sine value of the rotation angle corresponding to the maximum value are calculated as the point cloud {p i} to point cloud {p' i}The scale transformation factor and rotation axis vector as well as the cosine and sine values ​​of the rotation angle.

[0069] Step 7: Using the calculation results of step 6, calculate the point cloud {p i} to point cloud {p' i The angle transformation matrix R of} is as follows:

[0070] R=I+(1-cosθ)K 2 +sinθK

[0071] Here I is the 3*3 identity matrix. K is expressed as:

[0072]

[0073] Among them, the point cloud {p i} to point cloud {p' i The coordinates of the rotation axis of the x ,k y ,k z ).

[0074] Step 8: Use the scale transformation factor in step 6 and the angle transformation matrix in step 7 to calculate the point cloud {p i} to point cloud {p' i}The displacement transformation matrix The formula is:

[0075]

[0076] In this way, the point cloud {p i} to point cloud {p'i}'s scale transformation factor, angle transformation matrix and displacement vector.

[0077] This embodiment also discloses a three-dimensional point cloud registration system based on the Rodrigues rotation formula and statistical method, including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the three-dimensional point cloud registration method based on the Rodrigues rotation formula and statistical method is implemented.

[0078] This embodiment also discloses a computer-readable storage medium for storing a program, which, when executed, implements the above-mentioned three-dimensional point cloud registration method based on the Rodrigues rotation formula and the statistical method. In some possible implementations, the present invention can also be implemented in the form of a program product, which includes a program code, and when the program product is run on a terminal device, the program code is used to enable the terminal device to execute the steps according to various exemplary embodiments of the present invention described in the above method section of this specification.

[0079] The program product for implementing the above method may adopt a portable compact disk read-only memory (CD-ROM) and include program code, and can be run on a terminal device, such as a personal computer. However, the program product of the present invention is not limited thereto. In the present invention, a computer-readable storage medium may be any tangible medium containing or storing a program, which may be used by or in combination with an instruction execution system, apparatus, or device.

[0080] The program product may use any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0081] The method of the present invention is compared with the SVD algorithm. Figure 3 , using the bremen point cloud as the source point cloud, and the point cloud after scale, angle and displacement transformation as the target point cloud, the method of the present invention and the SVD algorithm are compared respectively. The comparison results are shown in Figure 2 As shown. Among them, the calculation formula of the registration error ε is:

[0082]

[0083] Since the SVD algorithm cannot calculate the scale transformation factor, we set its scale transformation factor to 1. Figure 2 It can be seen that when there is no scale transformation between the source point cloud and the target point cloud, the registration errors of the method of the present invention and the SVD decomposition method are very small. However, as the degree of scale transformation increases, the registration error of the SVD algorithm increases rapidly, while the registration error of the method of the present invention remains almost unchanged.

Claims

1. A three-dimensional point cloud registration method, characterized in that: The following steps are involved: Step 1: Define two sets of point clouds to be registered, namely point clouds {p i } and point cloud {p i '}, where i = 1, 2, ..., N, and for the point cloud {p i Any point p in i In the point cloud {p i There is always a point p in i 'Corresponding thereto; N is an integer greater than 2; Step 2: Point cloud {p i All points in} are grouped into groups of three points, and each group is defined as a registration unit; Step 3: Define the three points in each registration unit as point A, point B and point C; in any registration unit, point A, point B and point C are in the point cloud {p i The corresponding points in '} are point A', point B' and point C'; Calculate point cloud {p i The scale transformation factor scale of points A, B and C in any registration unit to points A', B' and C' is: Among them, (x A ,y A ,z A ),(x B ,y B ,z B ),(x A' ,y A' ,z A' ),(x B' ,y B' ,z B' ) are the spatial coordinates of points A, B, A', B' respectively; Calculate point cloud {p i The rotation axis vector of points A, B and C in any registration unit transformed to points A', B' and C' Where normalize() represents normalizing the vector in brackets; Calculate point cloud {p i The cosine value cosθ and sine value sinθ of the rotation angle of points A, B and C in any registration unit transformed to points A', B' and C' are: Step 4: According to the calculation method of step 3, calculate the point cloud {p i } The scale transformation factor, rotation axis vector, and cosine and sine values ​​of the rotation angle of each registration unit from points A, B and C to points A', B' and C'; Step 5: Based on the results obtained in step 4, obtain a histogram of scale transformation factors, a histogram of rotation axis vectors, and a histogram of cosine and sine values ​​of rotation angles; The point cloud {p i } to point cloud {p i '} scale transformation factor, rotation axis vector, and rotation angle cosine and sine values; Step 6: Based on the calculation results of step 5, the point cloud {p i } to point cloud {p i '}'s angle transformation matrix; Step 7: Use the point cloud obtained in step 5 i } to point cloud {p i '} and the angle transformation matrix in step 6 to calculate the point cloud {p i } to point cloud {p i '}'s displacement transformation matrix.

2. The three-dimensional point cloud registration method according to claim 1, characterized in that: In step 5, the scale transformation factor histogram is used to calculate the point cloud {p i } to point cloud {p i '}, the scale transformation factor with the largest scale transformation factor histogram data is selected as the point cloud {p i } to point cloud {p i '} scale transformation factor.

3. The three-dimensional point cloud registration method according to claim 2, characterized in that: In step 5, the point cloud {p i } to point cloud {p i '}, the rotation axis vector with the largest rotation axis vector histogram data is selected as the point cloud {p i } to point cloud {p i '}'s rotation axis vector.

4. The three-dimensional point cloud registration method according to claim 3, characterized in that: In step 5, the point cloud {p i } to point cloud {p i '}, the maximum cosine and sine values ​​of the rotation angle cosine and sine histogram data are selected as the point cloud {p i } to point cloud {p i '}The cosine and sine of the rotation angle.

5. The three-dimensional point cloud registration method according to claim 4, characterized in that: In step 6, the point cloud {p i } to point cloud {p i '}'s angle transformation matrix R: R=I+(1-cosθ)K 2 +sinθK Among them, I is the 3*3 identity matrix, and K is expressed as: Among them, (k x ,k y ,k z ) is the point cloud {p i } to point cloud {p i '}The rotation axis vector coordinates.

6. The three-dimensional point cloud registration method according to claim 5, characterized in that: In step 7, the point cloud {p i } to point cloud {p i '}'s displacement transformation matrix in, and Point p i ' and p i The coordinates of .

7. A three-dimensional point cloud registration system, comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the computer program is executed by a processor, the steps of the three-dimensional point cloud registration method described in any one of claims 1-6 are implemented.

8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the three-dimensional point cloud registration method described in any one of claims 1-6 are implemented.

Images