High-precision plane fitting method for multi-noise point cloud

Abstract

The invention discloses a high-precision plane fitting method for a multi-noise point cloud, and the method comprises the steps: carrying out the plane rough fitting of an original point cloud after down-sampling, taking the longest side of a minimum bounding box of a lower sampling point cloud as a distance threshold value, and calculating the distance weight of each point through a weight mathematical model, then, plane fine fitting is carried out on the weighted down-sampling point cloud, the distance weight of each point is updated after each fitting, plane fitting is carried out on the point cloud of which the weight is updated again, and a fine fitting plane of the down-sampling point cloud is calculated in an iterative manner; and finally, calculating the distance weight of each point in the original point cloud based on the fine fitting plane and the iterated distance threshold value, performing plane fitting on the weighted original point cloud in an iteration mode, and finally obtaining the fine fitting plane of the original point cloud. According to the technical scheme, the method is high in stability and low in calculation amount, can obtain a high-precision fitting plane in a weighted iteration mode under the condition of more noise, and is suitable for different types of point cloud data.

Description

Technical Field

[0001] The present application relates to the technical field of three-dimensional point cloud data processing, and in particular to a high-precision plane fitting method for multi-noise point clouds. Background Art

[0002] Three-dimensional point cloud plane fitting or extraction is an important and commonly used algorithm in computer vision and computer graphics. It can be used in plane correction, point cloud segmentation, plane extraction and other fields.

[0003] Most of the existing traditional algorithms for fitting point cloud planes directly fit the plane to the point cloud, which has poor accuracy for point cloud models with noise or more noise. For example, the principal component analysis (PCA) algorithm moves the center of the coordinate axis to the center of the data, and then calculates the mean and covariance matrix of all points on the x, y, and z axes, and then calculates the eigenvalues ​​and eigenvectors of the covariance matrix. The basis vector corresponding to the minimum eigenvalue is the normal vector of the plane. In this way, the coordinate value of the center point is brought into the plane equation formed by the normal vector, and the constant term can be calculated, thus realizing the fitting and extraction of the plane. This algorithm will be affected by noise, which reduces the accuracy of plane fitting. RANSAC plane fitting extraction algorithm, this algorithm can filter out noise to a certain extent, and achieve the effect of point cloud plane extraction. The main algorithm process is to first use random sampling to randomly sample three points of the original point cloud, and calculate the normal vector formed by these three points, and calculate the distance from all points to this plane, and remove the part exceeding the threshold rate, and randomly sample three points within the threshold again, and then calculate the normal vector formed, and repeat this cycle until most points are within the threshold range, stop iteration, generally no more than 50 times, and plane extraction can be achieved. This algorithm can filter out points outside the threshold range, but the accuracy and stability of this algorithm are general, because the fitted plane must pass through three of the points, and the fittable plane may not pass through three of the points, so it is not applicable to high-precision scenes, and high-precision fitting processing is required again. Therefore, in the existing algorithm, it is also proposed to use the RANSAC algorithm to perform initial plane fitting on the point cloud data, and construct the initial distance weight matrix based on the initial fitting plane parameters. After the WTLSD algorithm iteratively calculates the fitting plane parameters and repeatedly corrects the distance weight matrix, the final plane fitting parameters are obtained. This algorithm can reduce the impact of noise, but for the case of a large amount of point cloud data, RANSAC includes multiple iterations of calculating the distance value. In addition, during fine extraction, multiple iterations are also required to calculate the distance from the point to the plane, so the amount of calculation is large and time-consuming. Summary of the invention

[0004] The purpose of this application is to provide a method that can eliminate noise and perform high-precision plane fitting on three-dimensional point clouds. The method directly downsamples the original point cloud, then fits the plane to obtain a coarse extraction plane, and then sets the distance threshold according to the size of the point cloud bounding box, calculates the mathematical model according to the weight, and iterates the point cloud for a fine extraction algorithm. The method can perform high-precision plane fitting on noise-free, noisy or noisy point clouds, improves the calculation speed, and solves the problems of heavy calculation and time-consuming calculation process while ensuring high-precision fitting.

[0005] The technical solution of the present application is to provide a high-precision plane fitting method for multi-noise point clouds, the method comprising:

[0006] Step 1, randomly downsample the original point cloud to reduce its number to a predetermined number;

[0007] Step 2, using the SVD singular value decomposition method to perform a rough plane fitting on the downsampled point cloud to obtain the fitting parameters corresponding to the rough fitting plane;

[0008] Step 3, calculate the minimum bounding box of the downsampled point cloud, and use the longest side of the minimum bounding box as the distance threshold from the point in the point cloud to the rough fitting plane;

[0009] Step 4, calculate the distance from each point in the downsampled point cloud to the rough fitting plane, and substitute the distance value corresponding to each point into the weight mathematical model to calculate the distance weight of each point;

[0010] Step 5, use the weighted point cloud to perform plane fitting, obtain the first fitting plane, obtain the distance from each point in the current point cloud to the first fitting plane, use the median of the distance value as the new distance threshold and update the distance weight of each point, use the point cloud with updated weights to perform plane fitting again, update the fitting parameters of the first fitting plane, and iterate until the constant term in the fitting parameters changes within the predetermined range;

[0011] Step 6, based on the first fitting plane obtained after iteration and the distance threshold, calculate the distance weight of each point in the original point cloud, use the weighted original point cloud to perform plane fitting to obtain the second fitting plane, update the distance weight of each point based on the second fitting plane, and use the point cloud with updated weights to perform plane fitting again, and iterate until the constant term in the fitting parameter changes within a predetermined range.

[0012] Furthermore, step 2 specifically includes the following steps:

[0013] Step 21, calculating the center point p0 of the point cloud;

[0014] Step 22, let the rough fitting plane pass through the center point p0, make the direction vector formed by other points in the downsampled point cloud to the center point p0 orthogonal to the rough fitting plane normal vector X, and obtain the orthogonal linear equation system:

[0015] A*X=0

[0016] Where A=[p1-p0,p2-p0,p3-p0,…,pn-p0] T , A is the matrix formed by the direction vector, T represents the matrix transpose, n is the number of downsampled point clouds, p1, p2, p3, …, pn are the points in the downsampled point clouds;

[0017] Step 23, based on the orthogonalized linear equations, set an objective function for solving the rough fitting plane normal vector so that the sum of the distances from all points in the downsampled point cloud to the rough fitting plane is minimized, and the objective function is expressed as min||A*X||, where the constraint condition of the objective function is ||X||=1;

[0018] Step 24, perform singular value decomposition on the matrix A in the objective function, and obtain the final rough fitting plane normal vector X according to the decomposition result, X = (a, b, c) T , a, b and c represent the normal components of the normal vector in the three coordinate directions of x, y and z;

[0019] Step 25, based on the normal vector of the rough fitting plane, the plane equation of the rough fitting plane is obtained, and the constant term of the plane equation is obtained using the center point p0, and finally the parameters of the rough fitting plane are obtained. The plane equation of the rough fitting plane is:

[0020] ax+by+cz+d=0

[0021] In the formula, d is the constant term of the plane equation. Substitute the center point p0 into the plane equation to obtain d, d = -(ax0 + by0 + cz0), and finally obtain all the fitting parameters a, b, c, d of the rough fitting plane.

[0022] Furthermore, step 24 specifically includes:

[0023] Perform singular value decomposition on A and get:

[0024] A=UΣV T

[0025] After substituting into the objective function, we get:

[0026] ||AX||=||UΣV T X||=||ΣV T X||

[0027] Where U is an orthogonal matrix whose columns are the left singular vectors of A, Σ is a diagonal matrix, and V is an orthogonal matrix whose columns are the right singular vectors of A;

[0028] The diagonal elements of Σ are singular values, and the last diagonal element is the smallest singular value if and only if V T X=[0,0,0,…,1] T When , ||AX|| reaches the minimum value, and the optimal solution of the objective function is:

[0029] X=V[0,0,0,…,1] T =[v1,v2,v3,…,vn][0,0,0,…,1] T

[0030] Where v1, v2, v3, …, vn are the column vectors from columns 1 to n in the V matrix respectively;

[0031] The rough fitting plane normal vector is:

[0032] X=(a,b,c) T =(v n1 ,v n2 ,v n3 ) T

[0033] In the formula, a, b and c represent the normal components of the normal vector in the three coordinate directions of x, y and z, and the corresponding values ​​of a, b and c are v n1 、v n2 and v n3 .

[0034] Furthermore, step 3 specifically includes: obtaining the maximum coordinate value and the minimum coordinate value of the downsampled point cloud in the three coordinate axis directions of x, y and z, and for each coordinate direction, calculating the difference between the maximum coordinate value and the minimum coordinate value, using the difference as the side length of the point cloud AABB bounding box in the coordinate direction, and using the value of the longest side of the AABB bounding box as the distance threshold D from the point cloud to the rough fitting plane.

[0035] Furthermore, step 4 specifically includes:

[0036] Get the distance dist from each point in the downsampled point cloud to the rough fitting plane. Dist is expressed as:

[0037]

[0038] Substitute the distance value dist corresponding to each point into the weight mathematical model to obtain the distance weight of each point. The weight is calculated for points whose distance value is less than the distance threshold D, and the weight is reset to 0 for points whose distance value is greater than or equal to the distance threshold D. The weight mathematical model is expressed as:

[0039]

[0040] Where w is the weight corresponding to the downsampled point cloud.

[0041] Furthermore, step 5 specifically includes:

[0042] Step 51, multiplying the coordinates and distance weights corresponding to each point in the point cloud for weighting, and performing plane fitting on the weighted down-sampled point cloud using the SVD singular value decomposition method to obtain fitting parameters of the first fitting plane;

[0043] Step 52, obtaining the distance from each point in the current point cloud to the first fitting plane, taking the median of the distance values ​​as a new distance threshold, updating the weight mathematical model based on the new distance threshold, substituting the specific values ​​corresponding to each point in the current point cloud into the updated weight mathematical model, calculating the distance weight of each point, performing plane fitting again using the point cloud after the weight update, updating the fitting parameters of the first fitting plane, and iterating in this way until the constant term in the fitting parameters of the first fitting plane changes within a range less than the fitting deviation threshold e;

[0044] Step 53: Record the distance threshold and the fitting parameters of the first fitting plane after the iterative calculation is completed.

[0045] Further, step 6 specifically includes: using the first fitting plane obtained after the iteration of step 5 as the initial plane of the original point cloud, calculating the distance from each point in the original point cloud to the initial plane, using the distance threshold obtained after the iteration of step 5 as the distance threshold for the first weighted fitting of the original point cloud, substituting the distance value corresponding to each point in the original point cloud into the weight mathematical model to calculate the distance weight, using the weighted original point cloud to perform plane fitting, and obtaining the fitting parameters of the second fitting plane;

[0046] Calculate the distance from each point in the current point cloud to the second fitting plane, use the median of the distance values ​​as the new distance threshold, update the distance weight of each point in the current point cloud, use the point cloud with updated weights to perform plane fitting again, update the fitting parameters of the second fitting plane, and iterate until the constant term in the fitting parameters of the second fitting plane changes within a range less than the fitting deviation threshold e, and output the final fitting parameters of the second fitting plane.

[0047] Furthermore, step 1 includes: randomly downsampling the original point cloud according to the noise ratio, when the noise ratio of the original point cloud is greater than the preset noise ratio threshold, randomly downsampling the original point cloud to a first predetermined value, when the noise ratio of the original point cloud is less than or equal to the preset noise ratio threshold, randomly downsampling the original point cloud to a second predetermined value.

[0048] The beneficial effects of this application are:

[0049] The technical solution of the present application first performs a rough plane fitting on the downsampled point cloud, calculates the weight of the downsampled point cloud using the rough fitting plane parameters, performs a fine plane fitting on the downsampled point cloud by weighted iteration, and then calculates the weight of the original point cloud using the fine fitting plane parameters of the downsampled point cloud, performs plane fitting on the original point cloud by weighted iteration, and finally obtains the fine fitting plane of the original point cloud; the technical solution of the present application can obtain a high-precision fitting plane in the case of more noise by weighted iteration, reduce the influence of noise, and avoid large calculation errors caused by noise. Setting a fitting deviation threshold during iterative calculation can reduce the number of iterations and speed up the completion of fitting; compared with traditional technical solutions, such as the least squares method, the technical solution in the present application has higher stability, and can still stably obtain fitting results in point cloud data with greater noise, and can be used for high-precision plane fitting of noise-free, low-noise and high-noise point clouds.

[0050] The technical solution of the present application can significantly reduce the amount of calculation by reducing the number of point clouds through downsampling. At the same time, the continuous adjustment of the distance threshold during the iteration process further accelerates the calculation process. Compared with the traditional RANSAC and least squares method, the technical solution of the present application can significantly reduce the calculation time; compared with the traditional method, the technical solution in the present application can be applicable to large-scale point cloud data; the technical solution in the present application can also adjust the downsampling ratio and accuracy requirements according to the actual application scenario, and flexibly adapt to different types of point cloud data. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] The advantages of the above and / or additional aspects of the present application will become apparent and easily understood in the description of the embodiments in conjunction with the following drawings, in which:

[0052] Figure 1 is a schematic flow chart of a high-precision plane fitting method for multi-noise point clouds according to an embodiment of the present application;

[0053] Figure 2 is a noisy point cloud plan view according to one embodiment of the present application;

[0054] Figure 3 This is a plane fitting effect diagram according to an embodiment of the present application. DETAILED DESCRIPTION

[0055] In order to more clearly understand the above-mentioned purposes, features and advantages of the present application, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods. It should be noted that the embodiments of the present application and the features in the embodiments can be combined with each other without conflict.

[0056] In the following description, many specific details are elaborated to facilitate a full understanding of the present application. However, the present application may also be implemented in other ways different from those described herein. Therefore, the protection scope of the present application is not limited to the specific embodiments disclosed below.

[0057] like Figure 1 As shown, this embodiment provides a high-precision plane fitting method for multi-noise point cloud, including:

[0058] Step 1: Randomly downsample the original point cloud to reduce its number to a predetermined number.

[0059] According to the noise ratio, the original point cloud is randomly downsampled (i.e., a part of data points are randomly extracted from the original data set to form a smaller subset while retaining the distribution characteristics of the original data) to reduce the number of point clouds to a predetermined number, wherein when the noise ratio of the original point cloud is greater than the preset noise ratio threshold, the original point cloud is randomly downsampled to a first predetermined value, and when the noise ratio of the original point cloud is less than or equal to the preset noise ratio threshold, the original point cloud is randomly downsampled to a second predetermined value.

[0060] In this embodiment, when the original point cloud has more noise, the number of downsampling can be reduced by 50%, and when the noise is less, the number of downsampling can be reduced by 30%. Downsampling can filter out some noise points, reduce the amount of data, and improve the calculation speed.

[0061] Step 2, using the SVD singular value decomposition method to perform a rough plane fitting on the downsampled point cloud to obtain the fitting parameters corresponding to the rough fitting plane, specifically including the following steps:

[0062] Step 21, calculate the center point p0 of the point cloud, the coordinates of p0 are:

[0063]

[0064] Where n is the number of downsampled point clouds, x, y and z represent the three-dimensional coordinates of the points, (x0, y0, z0) are the coordinates of p0, and i is the index label of each point in the downsampled point cloud, i =

[0065] 1,2,3,…,n,(x i ,y i ,z i ) is the coordinate of the i-th point;

[0066] Step 22, let the rough fitting plane pass through the center point p0, make the direction vector formed by other points in the down-sampled point cloud to the center point p0 orthogonal to the rough fitting plane normal vector, and obtain the orthogonal linear equation system;

[0067] The direction vector from other points in the downsampled point cloud to the center point p0 is (pi-p0). The direction vector (pi-p0) is orthogonal to the rough fitting plane normal vector X, and we get:

[0068] (pi-p0)*X=0

[0069] Where pi is the i-th point in the point cloud; (p0-pi) is represented as a matrix A with n rows and 3 columns (where each row of the matrix A represents the difference vector of a point pair, and the 3 columns in each row correspond to the z, y, and z coordinate components of each difference vector), and A is expressed as:

[0070] A=[p1-p0,p2-p0,p3-p0,…,pn-p0] T

[0071] Where T represents matrix transpose, p1, p2, p3, …, pn are points in the downsampled point cloud, and the orthogonal linear equation system is obtained by substituting the matrix A into the above orthogonal equation:

[0072] A*X=0

[0073] Step 23, based on the orthogonalized linear equations, an objective function for solving the rough fitting plane normal vector is set so that the sum of the distances from all points in the downsampled point cloud to the rough fitting plane is minimized. The objective function is expressed as:

[0074] min||A*X||

[0075] Among them, the constraint condition of the objective function is ||X||=1;

[0076] In this embodiment, ideally, all points in the downsampled point cloud should be on the coarse fitting plane, so the formula A*X=0 is valid. However, in actual situations, some points are outside the coarse fitting plane, and the final result of the fitting becomes the normal vector X when the sum of the distances between all points in the downsampled point cloud and the coarse fitting plane is the shortest.

[0077] Step 24, perform singular value decomposition on the matrix A in the objective function, and obtain the final rough fitting plane normal vector according to the decomposition result, as follows:

[0078] Perform singular value decomposition on A and get:

[0079] A=UΣV T

[0080] After substituting into the objective function, we get:

[0081] ||AX||=||UΣV T X||=||ΣV T X||

[0082] Where U is an orthogonal matrix whose columns are the left singular vectors of A, Σ is a diagonal matrix whose elements on the diagonal are called singular values, which are non-negative and arranged in descending order, and V is an orthogonal matrix whose columns are the right singular vectors of A.

[0083] Since the diagonal elements of Σ are singular values, assuming that the last diagonal element is the smallest singular value, then V T X=[0,0,0,…,1] T When , ||AX|| can be minimized, and the optimal solution of the objective function is:

[0084] X=V[0,0,0,…,1] T =[v1,v2,v3,…,vn][0,0,0,…,1] T

[0085] Where v1, v2, v3, …, vn are the column vectors from columns 1 to n in the V matrix.

[0086] Finally, the rough fitting plane normal vector X is obtained:

[0087] X=(a,b,c) T =(v n1 ,v n2 ,v n3 ) T

[0088] In the formula, a, b and c represent the normal components of the normal vector in the three coordinate directions of x, y and z, and the corresponding values ​​of a, b and c are v n1 、v n2 and v n3 , n1, n2 and n3 represent the three coordinate directions of x, y and z respectively.

[0089] Step 25, obtaining the plane equation of the rough fitting plane based on the normal vector of the rough fitting plane, and obtaining the constant term of the plane equation using the center point p0, and finally obtaining the parameters of the rough fitting plane;

[0090] In this embodiment, the minimum eigenvalue of matrix A is the minimum value of the sum of distances from all points to the rough fitting plane, and the eigenvector (including a, b and c) corresponding to the minimum eigenvalue of matrix A is the parameter of the rough fitting plane.

[0091] The plane equation of the rough fitting plane is obtained based on the normal vector of the rough fitting plane. The plane equation of the rough fitting plane (the plane equation is a mathematical method to describe the plane in three-dimensional space, which is determined by the normal vector and the constant term) is expressed as:

[0092] ax+by+cz+d=0

[0093] In the formula, d is the constant term of the plane equation. Using the center point p0, d is obtained and expressed as:

[0094] d=-(ax0+by0+cz0)

[0095] Finally, all parameters of the rough fitting plane are obtained, including a, b, c, and d.

[0096] In this embodiment, the SVD singular value decomposition method is used to process the downsampled point cloud, which can suppress the fitting error caused by noise.

[0097] Step 3: Calculate the minimum bounding box of the downsampled point cloud, and use the longest side of the minimum bounding box as the distance threshold from the point in the point cloud to the rough fitting plane.

[0098] Get the maximum and minimum coordinate values ​​of the downsampled point cloud in the directions of the three coordinate axes x, y, and z. For each coordinate direction, calculate the difference between the maximum and minimum coordinate values, and use the difference as the side length of the point cloud AABB bounding box in that coordinate direction. After calculating all the side lengths of the AABB bounding box, use the value of the longest side as the distance threshold D from the point cloud to the rough fitting plane.

[0099] In this embodiment, the purpose of calculating the AABB bounding box is to determine the dimensions of the point cloud in three directions, and the maximum dimension value is used as the distance threshold D for subsequent weight calculation.

[0100] Step 4: Calculate the distance from each point in the downsampled point cloud to the rough fitting plane, and substitute the distance value corresponding to each point into the weight mathematical model to calculate the distance weight of each point, which includes:

[0101] Get the distance dist from each point in the downsampled point cloud to the rough fitting plane. Dist is expressed as:

[0102]

[0103] Substitute the distance value corresponding to each point into the weight mathematical model to obtain the distance weight of each point. The weight is calculated for points whose distance value is less than the distance threshold D, and the weight is reset to 0 for points whose distance value is greater than or equal to the distance threshold D. The weight mathematical model is expressed as:

[0104]

[0105] Where w is the weight corresponding to the point cloud.

[0106] Step 5, using the weighted point cloud to perform plane fitting, obtain the first fitting plane (that is, obtain the normal vector and constant term d of the fitting plane), obtain the distance dist from each point in the current point cloud to the first fitting plane, use the median of the distance dist as the new distance threshold and update the distance weight of each point in the current point cloud, use the point cloud with updated weights to perform plane fitting again, update the fitting parameters of the first fitting plane, and iterate until the constant term in the fitting parameters changes within a predetermined range; specifically, the following steps are included:

[0107] Step 51, multiplying the coordinates and distance weights corresponding to each point in the point cloud for weighting, and performing plane fitting on the weighted downsampled point cloud using the SVD singular value decomposition method to obtain fitting parameters of the first fitting plane, specifically including the following steps:

[0108] Step 511, calculating the center point p'0 of the weighted down-sampled point cloud;

[0109] Step 512, let the first fitting plane pass through the center point p'0, make the direction vectors formed by other points in the weighted down-sampled point cloud to the center point p'0 orthogonal to the normal vector of the first fitting plane, and obtain an orthogonal linear equation system;

[0110] Step 513: Based on the orthogonalized linear equation group, an objective function for solving the normal vector of the first fitting plane is set so that the sum of the distances from all points in the weighted downsampled point cloud to the first fitting plane is minimized. The objective function for solving the normal vector of the first fitting plane is expressed as:

[0111] min||A w *X||

[0112] In the formula, the constraint condition of the objective function is ||X||=1, A w is the weighted matrix A;

[0113] Step 514, the matrix A in the objective function w Perform singular value decomposition and obtain the final first fitting plane normal vector X1 according to the decomposition result. The first fitting plane normal vector X1 is expressed as:

[0114] X1=(a1,b1,c1) T

[0115] Where a1, b1 and c1 represent the normal components of the normal vector X1 in the three coordinate directions of x, y and z;

[0116] Step 515, obtaining the plane equation of the first fitting plane based on the first fitting plane normal vector, and using the center point p'0 to obtain the constant term d1 of the plane equation, and finally obtaining the fitting parameters a1, b1, c1 and d1 of the first fitting plane.

[0117] In this embodiment, the calculation process of performing plane fitting on the weighted down-sampled point cloud and obtaining the first fitting plane parameters in step 51 is the same as the calculation process of performing plane fitting on the down-sampled point cloud and obtaining the rough fitting plane parameters in step 2. The difference is that the point cloud processed in step 51 is a weighted point cloud, which will not be repeated here.

[0118] Step 52, obtaining the distance dist from each point in the current point cloud to the first fitting plane, taking the median of the distance dist as the new distance threshold D1, updating the weight mathematical model based on the new distance threshold, substituting the distance dist corresponding to each point in the current point cloud into the updated weight mathematical model, calculating the distance weight of each point, performing plane fitting again using the point cloud after the weight update, updating the fitting parameters of the first fitting plane, and iterating in this way until the constant term d1 in the fitting parameters of the first fitting plane changes within a range less than the fitting deviation threshold e;

[0119] The updated weight mathematical model is expressed as:

[0120]

[0121] Where w1 is the weight corresponding to the weighted downsampled point cloud, and D1 is the new distance threshold.

[0122] In this embodiment, a fitting deviation threshold e is set for use during iteration; e is determined according to the desired fitting accuracy, and e is the difference in the d1 value in the plane coefficient a1x+b1y+c1z+d1=0 calculated in each iteration (the difference refers to the difference between the d of the previous plane and the d of the current new plane). The smaller the difference, the higher the accuracy.

[0123] Step 53 , recording the distance threshold D1 after the iterative calculation and the final fitting parameters a1 , b1 , c1 , d1 of the first fitting plane for use in further fine fitting.

[0124] In this embodiment, after each iterative calculation, the fitting parameters of the first fitting plane are updated and gradually move closer to the point cloud position closer to the real plane; the final fitting parameters of the first fitting plane are output at the end of the iteration.

[0125] In this embodiment, in order to reduce the number of iterations and speed up the fitting process, the distance threshold needs to be recalculated, and the median of the dist value calculated in the previous iteration is used as the distance threshold for the next iteration; the fitting is repeated in this way until the fitting result changes within the set range (0, e), and the distance threshold D1 and fitting parameters a1, b1, c1, d1 are recorded for further fine fitting.

[0126] Step 6, based on the first fitting plane obtained after the iteration of step 5 and the distance threshold, calculate the distance weight of each point in the original point cloud, use the weighted original point cloud to perform plane fitting to obtain a second fitting plane, update the distance weight of each point based on the second fitting plane, and use the point cloud with updated weights to perform plane fitting again, and iterate until the constant term in the fitting parameter changes within a predetermined range.

[0127] The first fitting plane obtained after the iteration of step 5 is used as the initial plane of the original point cloud, and the distance from each point in the original point cloud to the initial plane is calculated. The distance threshold D1 obtained after the iteration of step 5 is used as the distance threshold for the first weighted fitting of the original point cloud. The distance value corresponding to each point in the original point cloud is respectively substituted into the weight mathematical model to calculate the distance weight, and the weighted original point cloud is used to perform plane fitting to obtain the fitting parameters of the second fitting plane.

[0128] Calculate the distance from each point in the current point cloud to the second fitting plane, use the median of the distance values ​​as the new distance threshold, update the distance weight of each point in the current point cloud, use the point cloud with updated weights to perform plane fitting again, update the fitting parameters of the second fitting plane, and iterate until the constant term in the fitting parameters of the second fitting plane changes within a range less than the fitting deviation threshold e, output the final fitting parameters a2, b2, c2, d2 of the second fitting plane, and complete the plane fitting of the point cloud.

[0129] In this embodiment, the iterative calculation process of step 6 is the same as the iterative calculation process of step 5, which will not be repeated here; the calculation process of plane fitting in step 6 is the same as the calculation process of plane fitting in step 2, which will not be repeated here.

[0130] Examples:

[0131] Add random noise to a point cloud whose plane normal vector is (0, 0, 1) and passes through the origin (0, 0, 0), such as Figure 2 The method of the present invention is used to perform plane fitting on the point cloud after adding random noise, and the final fitting effect is as shown in Figure 3 As shown, the fitting deviation threshold e is set to 0.01; at the same time, the traditional least squares method is used for plane fitting; after iterative operations of the two methods, the following results are obtained:

[0132] The calculation results of the present invention are as follows: a=-0.000371337, b=0.000410507, c=1.0, d=-0.0419054; the fitting effect is as follows Figure 3 shown.

[0133] The calculation results of the traditional least squares method are: a=0.000889580, b=-0.000343352, c=0.999999523, d=-0.086305052.

[0134] From the comparison of the above calculation results, it can be seen that the data fitted by the method of the present invention is closer to the real data, while the data fitted by the existing least squares method is far from the real data. Therefore, from the final calculation results of the two methods, it can be seen that the algorithm of the present invention has higher accuracy, and the method of the present invention is more suitable for fitting operations of point cloud data with more noise.

[0135] For 500w point cloud data, the present invention takes no more than 200ms to fit the plane, and the result is stable with high precision. The traditional algorithm RANSAC plane fitting has poor precision and is much more time-consuming than the method of the present invention. It can be seen that the present invention ensures both time consumption and precision.

[0136] The steps in this application can be adjusted in order, combined, and deleted according to actual needs.

[0137] The units in the device of the present application can be combined, divided and deleted according to actual needs.

[0138] Although the present application is disclosed in detail with reference to the accompanying drawings, it should be understood that these descriptions are merely exemplary and are not intended to limit the application of the present application. The scope of protection of the present application is defined by the appended claims and may include various modifications, alterations and equivalents made to the invention without departing from the scope and spirit of the present application.

Claims

1. A high-precision plane fitting method for multi-noise point clouds, characterized in that: The method includes: Step 1, randomly downsample the original point cloud to reduce its number to a predetermined number; Step 2, using the SVD singular value decomposition method to perform a rough plane fitting on the downsampled point cloud to obtain the fitting parameters corresponding to the rough fitting plane; Step 3, calculate the minimum bounding box of the downsampled point cloud, and use the longest side of the minimum bounding box as the distance threshold from the point in the point cloud to the rough fitting plane; Step 4, calculate the distance from each point in the downsampled point cloud to the rough fitting plane, and substitute the distance value corresponding to each point into the weight mathematical model to calculate the distance weight of each point; Step 5, use the weighted point cloud to perform plane fitting, obtain the first fitting plane, obtain the distance from each point in the current point cloud to the first fitting plane, use the median of the distance value as the new distance threshold and update the distance weight of each point, use the point cloud with updated weights to perform plane fitting again, update the fitting parameters of the first fitting plane, and iterate until the constant term in the fitting parameters changes within the predetermined range; Step 6, based on the first fitting plane obtained after iteration and the distance threshold, calculate the distance weight of each point in the original point cloud, use the weighted original point cloud to perform plane fitting to obtain the second fitting plane, update the distance weight of each point based on the second fitting plane, and use the point cloud with updated weights to perform plane fitting again, and iterate until the constant term in the fitting parameter changes within a predetermined range.

2. The high-precision plane fitting method for multi-noise point cloud according to claim 1, characterized in that: The step 2 specifically includes the following steps: Step 21, calculating the center point p0 of the point cloud; Step 22, let the rough fitting plane pass through the center point p0, make the direction vector formed by other points in the downsampled point cloud to the center point p0 orthogonal to the rough fitting plane normal vector X, and obtain the orthogonal linear equation system: A*X=0 Where A=[p1-p0,p2-p0,p3-p0,…,pn-p0] T , A is the matrix formed by the direction vector, T represents the matrix transpose, n is the number of downsampled point clouds, p1, p2, p3, …, pn are the points in the downsampled point clouds; Step 23, based on the orthogonalized linear equations, set an objective function for solving the rough fitting plane normal vector so that the sum of the distances from all points in the downsampled point cloud to the rough fitting plane is minimized, and the objective function is expressed as min||A*X||, where the constraint condition of the objective function is ||X||=1; Step 24, perform singular value decomposition on the matrix A in the objective function, and obtain the final rough fitting plane normal vector X according to the decomposition result, X = (a, b, c) T , a, b and c represent the normal components of the normal vector in the three coordinate directions of x, y and z; Step 25, based on the normal vector of the rough fitting plane, the plane equation of the rough fitting plane is obtained, and the constant term of the plane equation is obtained using the center point p0, and finally the parameters of the rough fitting plane are obtained. The plane equation of the rough fitting plane is: ax+by+cz+d=0 In the formula, d is the constant term of the plane equation. Substitute the center point p0 into the plane equation to obtain d, d = -(ax0 + by0 + cz0), and finally obtain all the fitting parameters a, b, c, d of the rough fitting plane.

3. The high-precision plane fitting method for multi-noise point cloud according to claim 2, characterized in that: The step 24 specifically includes: Perform singular value decomposition on A and get: A=UΣV T After substituting into the objective function, we get: ||AX||=||UΣV T X||=||ΣV T X|| Where U is an orthogonal matrix whose columns are the left singular vectors of A, Σ is a diagonal matrix, and V is an orthogonal matrix whose columns are the right singular vectors of A; The diagonal elements of Σ are singular values, and the last diagonal element is the smallest singular value if and only if V T X=[0,0,0,…,1] T When , ||AX|| reaches the minimum value, and the optimal solution of the objective function is: X=V[0,0,0,…,1] T =[v1,v2,v3,…,vn][0,0,0,…,1] T Where v1, v2, v3, …, vn are the column vectors of columns 1 to n in the V matrix respectively; The rough fitting plane normal vector is: X=(a,b,c) T =(v n1 ,v n2 ,v n3 ) T In the formula, a, b and c represent the normal components of the normal vector in the three coordinate directions of x, y and z, and the corresponding values ​​of a, b and c are v n1 、v n2 and v n3 .

4. The high-precision plane fitting method for multi-noise point cloud according to claim 2, characterized in that: The step 3 specifically includes: obtaining the maximum coordinate value and the minimum coordinate value of the downsampled point cloud in the three coordinate axis directions of x, y and z, and for each coordinate direction, calculating the difference between the maximum coordinate value and the minimum coordinate value, using the difference as the side length of the point cloud AABB bounding box in the coordinate direction, and using the value of the longest side of the AABB bounding box as the distance threshold D from the point cloud to the rough fitting plane.

5. The high-precision plane fitting method for multi-noise point cloud according to claim 4, characterized in that: The step 4 specifically includes: Get the distance dist from each point in the downsampled point cloud to the rough fitting plane. Dist is expressed as: Substitute the distance value dist corresponding to each point into the weight mathematical model to obtain the distance weight of each point. The weight is calculated for points whose distance value is less than the distance threshold D, and the weight is reset to 0 for points whose distance value is greater than or equal to the distance threshold D. The weight mathematical model is expressed as: Where w is the weight corresponding to the downsampled point cloud.

6. The high-precision plane fitting method for multi-noise point cloud according to claim 5, characterized in that: The step 5 specifically includes: Step 51, multiplying the coordinates and distance weights corresponding to each point in the point cloud for weighting, and performing plane fitting on the weighted down-sampled point cloud using the SVD singular value decomposition method to obtain fitting parameters of the first fitting plane; Step 52, obtaining the distance from each point in the current point cloud to the first fitting plane, taking the median of the distance values ​​as a new distance threshold, updating the weight mathematical model based on the new distance threshold, substituting the specific values ​​corresponding to each point in the current point cloud into the updated weight mathematical model, calculating the distance weight of each point, performing plane fitting again using the point cloud after the weight update, updating the fitting parameters of the first fitting plane, and iterating in this way until the constant term in the fitting parameters of the first fitting plane changes within a range less than the fitting deviation threshold e; Step 53: Record the distance threshold and the fitting parameters of the first fitting plane after the iterative calculation is completed.

7. The high-precision plane fitting method for multi-noise point cloud according to claim 6, characterized in that: The step 6 specifically includes: The first fitting plane obtained after the iteration of step 5 is used as the initial plane of the original point cloud, and the distance from each point in the original point cloud to the initial plane is calculated. The distance threshold obtained after the iteration of step 5 is used as the distance threshold for the first weighted fitting of the original point cloud. The distance value corresponding to each point in the original point cloud is respectively substituted into the weight mathematical model to calculate the distance weight, and the weighted original point cloud is used to perform plane fitting to obtain the fitting parameters of the second fitting plane. Calculate the distance from each point in the current point cloud to the second fitting plane, use the median of the distance values ​​as the new distance threshold, update the distance weight of each point in the current point cloud, use the point cloud with updated weights to perform plane fitting again, update the fitting parameters of the second fitting plane, and iterate until the constant term in the fitting parameters of the second fitting plane changes within a range less than the fitting deviation threshold e, and output the final fitting parameters of the second fitting plane.

8. The high-precision plane fitting method for multi-noise point cloud according to claim 1, characterized in that: The step 1 includes: randomly downsampling the original point cloud according to the noise ratio, when the noise ratio of the original point cloud is greater than the preset noise ratio threshold, randomly downsampling the original point cloud to a first predetermined value, when the noise ratio of the original point cloud is less than or equal to the preset noise ratio threshold, randomly downsampling the original point cloud to a second predetermined value.

Images