A residual cloud plane fitting method based on improved random sampling consensus algorithm

Abstract

The present invention proposes a residual point cloud plane fitting method based on an improved random sampling consensus algorithm, including initialization parameters, three-dimensional point cloud downsampling, plane fitting based on a random sampling consensus algorithm, voxel clustering and voxel center filtering, three-dimensional point cloud upsampling and calculation of plane completeness, plane model parameters are calculated by randomly selecting points and the plane with the most matching points is selected, the completeness of the fitted plane is determined by combining plane grid division and voxel clustering, and the fitted plane with the best plane completeness is obtained through iterative optimization. The present invention provides a new method for continuous plane fitting of missing point clouds in complex workshop environments, and provides important reference for point cloud processing in the fields of autonomous driving, robot navigation, virtual reality, etc.

Description

Technical Field

[0001] The present invention relates to the technical field of point cloud plane fitting algorithms, and in particular to a residual point cloud plane fitting method based on an improved random sampling consensus algorithm. Background Art

[0002] With the widespread application of 3D scanning technology in the era of Industry 4.0, the acquisition of point cloud data has become more and more convenient, and point cloud processing methods and applications have received widespread attention. Among them, point cloud plane fitting is a key technology, which plays an important role in many application fields such as autonomous driving, robot navigation, geographic information systems and virtual reality. The 3D scanning technology based on laser sensors has the characteristics of small sensor size, fast measurement speed, no contact with the object to be measured, and high measurement accuracy. Laser scanning can quickly obtain the 3D coordinate data of the surface of the object to be measured, and obtain the 3D data set of the object to be measured by collecting a large amount of object space point position information quickly to form a point cloud. However, due to the influence of object occlusion, scanning angle limitation, sensor accuracy, environmental conditions, etc., the scanned plane features are incomplete to varying degrees.

[0003] At present, there are three main methods for traditional point cloud plane feature fitting: least squares algorithm, random sampling consistency algorithm, and principal component analysis algorithm. However, traditional algorithms are inefficient when processing large-scale point clouds or complex scenes, are sensitive to noise and outliers, and have no step to determine whether it is a plane, resulting in low algorithm robustness. Summary of the invention

[0004] The purpose of the present invention is to provide a point cloud plane feature fitting method which can improve the efficiency and robustness of plane fitting.

[0005] To achieve the above object, the present invention proposes a residual defect cloud plane fitting method based on an improved random sampling consensus algorithm, comprising the following steps:

[0006] Step 1: Initialize parameters, set the parameters of the fitted plane model for subsequent iterative updates; initialize the maximum number of points that fit the plane model for comparison and improvement of fitting efficiency;

[0007] Step 2: According to the size and density of the original point cloud and the characteristics of the expected fitting plane, the normal vector, voxel grid size, plane grid size, distance threshold and angle threshold of the expected fitting plane are set as the basis for each subsequent expansion step, and an appropriate plane completeness rate threshold is set as the condition for the end of iteration, and the original point cloud is uniformly downsampled;

[0008] Step 3: Randomly select points for fitting the plane, and estimate the normal vector of the fitted plane to determine whether the selected points are collinear. If they are collinear, reselect points to fit the plane until the selected points are non-collinear, thereby selecting three non-collinear points to fit a certain plane model;

[0009] Step 4: Determine the angle between the normal vector of the fitting plane of the non-collinear point and the normal vector of the expected fitting plane. If the angle is greater than the angle threshold, return to step 3 to reselect points until the angle is less than the angle threshold, thereby removing the plane model that is far away from the expected plane in the normal vector direction;

[0010] Step 5: Based on the fitted plane model obtained in step 4, the number of points in the original point cloud that meet the plane model parameters and the distance threshold is calculated, and compared with the maximum number of matching points. If it is less than the maximum number of matching points, steps 3-5 are repeated until the number of matching points is greater than the maximum number of matching points.

[0011] Step 6: Perform voxel clustering and voxel center filtering on the plane point cloud that meets the step 5, and perform upsampling to select the largest clustered point cloud in the fitting plane;

[0012] Step 7: Calculate the plane completeness rate of the maximum cluster point cloud, and determine whether the plane completeness rate is greater than the completeness rate threshold: if it is less than the completeness rate threshold, perform bounding box filtering and return to step 3 to restart the calculation; otherwise, output the plane point cloud.

[0013] Furthermore, in the above step 2, the distance threshold is a distance threshold from a point to a fitting plane; and the angle threshold is an angle threshold between a normal vector of the fitting plane and a normal vector of an expected fitting plane.

[0014] Furthermore, the specific judgment method of step 3 is: randomly select three points P1 (x1, y1, z1), P2 (x2, y2, z2), P3 (x3, y3, z3), based on the plane equation AX + BY + CZ + D = 0, let the vector calculate Get the normal vector N of the three-point fitting plane. If |N| == 0, the three points are collinear. Reselect 3 points to fit the plane until |N| ​​is not 0.

[0015] Furthermore, in the above step 6, the specific principle of the voxel clustering is to downsample the planar point cloud according to the preset voxel grid to obtain voxel centers, and classify the voxel centers that are close to each other into one category; the specific principle of the voxel center filtering is to filter out the category point cloud with a small number of voxel centers from the voxel clustering to reduce the interference of noise on subsequent steps; the specific principle of the upsampling is to select the category point cloud with the largest number of voxel centers from the voxel clustering and upsample it based on the planar point cloud determined by the step 5.

[0016] Furthermore, the specific determination method of the plane completeness in the above step 7 is: mapping the maximum cluster point cloud to the xoy coordinate system, calculating the boundary values ​​of the point cloud on the X-axis and the Y-axis respectively, dividing the maximum bounding box, and then calculating the total number of grids divided by the maximum bounding box according to the size of the plane grid, recorded as the total number of grids; counting the number of grids with points in the plane grid, recorded as the number of valid grids, and finally calculating the ratio of the number of valid grids to the total number of grids, determining whether the ratio is greater than the plane completeness threshold, and if so, outputting the plane point cloud;

[0017] Furthermore, the bounding box filtering method in step 7 is specifically as follows: based on the maximum clustered point cloud whose plane completeness is less than the completeness threshold, a maximum bounding box is established by floating 1 to 5 mm on the Z axis, the original point cloud is returned, and the point cloud inside the maximum bounding box in the original point cloud is removed, the parameters are initialized, and the step 3 is returned to reselect the points for calculation until the plane completeness is greater than the completeness threshold, and the plane point cloud at this time is output.

[0018] Compared with the prior art, the advantages of the present invention are:

[0019] Based on the traditional random sampling consistency algorithm, the present invention judges the plane completeness rate through plane grid division and voxel clustering strategy, and improves the plane fitting efficiency through voxel center filtering and bounding box filtering strategy, so that the improved fitting method can fit a largest continuous complete plane in the case of missing point cloud and complex environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 The present invention is a flowchart of a residual defect cloud plane fitting method based on an improved random sampling consensus algorithm according to an embodiment of the present invention.

[0021] Figure 2 This is a flow chart of the point cloud plane fitting method based on the random sampling consistency algorithm in this embodiment.

[0022] Figure 3 Schematic diagram of a plane grid division strategy in an embodiment of the present invention.

[0023] Figure 4Schematic diagram of the original point cloud in an embodiment of the present invention. DETAILED DESCRIPTION

[0024] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be further described below.

[0025] This embodiment proposes a residual cloud plane fitting method based on an improved random sampling consensus algorithm, such as Figure 1 As shown, the specific process steps are as follows:

[0026] S1: Initialize parameters, initialize and set the plane model parameters A=0, B=0, C=0, D=0, and the maximum number of points that conform to the plane model pre_inliner=0.

[0027] S2: 3D point cloud downsampling, according to Figure 4 The original point cloud shown in the figure sets the expected fitting plane N, voxel grid voxelGrid, plane grid size grid, distance threshold sigma from the point to the fitting plane, angle threshold T between the fitting plane normal vector and the expected plane normal vector, and plane completeness threshold γ; and Figure 4 The original point cloud shown is uniformly downsampled to reduce the number and density of point clouds.

[0028] S3: Point cloud plane fitting based on random sampling consensus algorithm. The specific process is as follows Figure 2 As shown, including:

[0029] S31: After inputting the point cloud, randomly select 3 points P1(x1, y1, z1), P2(x2, y2, z2), and P3(x3, y3, z3), based on the plane equation AX+BY+CZ+D=0, let the vector calculate Get the normal vector N of the three-point fitting plane. If |N| == 0, the three points are collinear. Reselect 3 points to fit the plane until |N| ​​is not 0, so that three non-collinear points are selected to fit a certain plane model.

[0030] S32: Then, the angle between the normal vector of the fitting plane of the non-collinear point and the normal vector of the expected fitting plane is calculated. If the actual angle is greater than the angle threshold T, return to step S31 and reselect points until the actual angle is less than the angle threshold T. Through this step, the plane model that is far away from the expected plane in the normal vector direction is removed, so as to obtain a fitting plane model whose actual angle is less than the angle threshold T by fitting the non-collinear points;

[0031] S33: Based on the fitting plane model obtained in S32 above, the number of points total_inliner in the original point cloud that meet the plane model parameters and the distance threshold sigma is counted, and compared with the maximum number of matching points pre_inliner. If the number of matching points total_inliner is less than the maximum number of matching points pre_inliner, return to the above step S31, reselect points and recalculate until the number of matching points total_inliner is greater than the preset maximum number of matching points pre_inliner, and obtain a three-dimensional plane point cloud that meets steps S31, S32 and S33.

[0032] S4: Perform voxel clustering and voxel center filtering on the plane point cloud obtained in step S3 above, such as Figure 3 As shown, according to the size of the voxel grid voxelGrid, the plane point cloud is divided into voxel grids to obtain voxel centers, and the voxel centers that are close to each other are grouped into a cluster to form a voxel cluster. Finally, the category point cloud with a small number of voxel grids is filtered out from the voxel cluster to reduce the interference of noise on subsequent steps. The cluster point cloud with the largest number of voxel grids is selected, and based on the plane point cloud obtained in step S3, the cluster point cloud is upsampled to select the largest cluster point cloud in the fitting plane.

[0033] S5: Based on the grid division method, calculate the plane completeness of the maximum cluster point cloud selected in the above step S4, map the maximum cluster point cloud to the xoy coordinate system, calculate its maximum length on the X-axis and Y-axis respectively, so as to divide the maximum bounding box, and then calculate the total number of grids divided in the maximum bounding box according to the plane division grid size Grid, count the number of grids with points in these grids, record them as the number of valid grids, and finally calculate the ratio of the number of valid grids to the total number of grids, and determine whether the ratio is greater than the plane completeness threshold γ. If so, output the three-dimensional plane point cloud, otherwise establish the maximum bounding box based on the maximum cluster point cloud floating 1 to 5 mm on the Z axis, return to the original point cloud, and remove the point cloud inside the maximum bounding box in the original point cloud, let pre inliner =0, return to step S3 and start again.

[0034] The above is only a preferred embodiment of the present invention and does not limit the present invention in any way. Any technician in the relevant technical field, without departing from the scope of the technical solution of the present invention, makes any form of equivalent replacement or modification to the technical solution and technical content disclosed in the present invention, which does not depart from the content of the technical solution of the present invention and still falls within the protection scope of the present invention.

Claims

1. A residual cloud plane fitting method based on an improved random sampling consensus algorithm, characterized in that: The steps include: Step 1: Initialize and set the fitting plane model parameters and the maximum number of points that fit the plane model; Step 2: According to the size and density of the original point cloud and the characteristics of the expected fitting plane, the normal vector, voxel grid size, plane grid size, distance threshold and angle threshold of the expected fitting plane are set as the basis for each subsequent expansion step, and an appropriate plane completeness rate threshold is set as the condition for the end of iteration, and the original point cloud is uniformly downsampled; Step 3: Randomly select points to fit the plane, and estimate the normal vector of the fitted plane to determine whether the selected points are collinear. If they are collinear, reselect points to fit the plane until the selected points are not collinear; Step 4: Determine the angle between the normal vector of the fitting plane of the non-collinear point and the expected normal vector of the fitting plane. If the angle is greater than the angle threshold, return to step 3 to reselect points until the angle is less than the angle threshold. Step 5: Based on the fitted plane model obtained in step 4, the number of points in the original point cloud that meet the plane model parameters and the distance threshold is calculated, and compared with the maximum number of matching points. If it is less than the maximum number of matching points, steps 3-5 are repeated until the number of matching points is greater than the maximum number of matching points. Step 6: voxel clustering and voxel center filtering are performed on the plane point cloud that meets the step 5, and up-sampling is performed to select the largest cluster point cloud in the fitting plane; in step 6, the specific principle of the voxel clustering is to down-sample the plane point cloud according to the preset voxel grid to obtain the voxel center, and classify the voxel centers that are close to each other into one category; the specific principle of the voxel center filtering is to filter out the category point cloud with a small number of voxel centers from the voxel clustering to reduce the interference of noise on subsequent steps; the specific principle of the up-sampling is to select the category point cloud with the largest number of voxel centers from the voxel clustering and up-sample it based on the plane point cloud determined by step 5; Step 7: Calculate the plane completeness rate of the maximum cluster point cloud, and determine whether the plane completeness rate is greater than the completeness rate threshold: if it is less than the completeness rate threshold, perform bounding box filtering, and return to step 3 to restart the calculation; otherwise, output the plane point cloud; The specific determination method of the plane completeness rate in step 7 is: map the maximum cluster point cloud to the xoy coordinate system, calculate the boundary values ​​of the point cloud on the X-axis and the Y-axis respectively, divide the maximum bounding box, and then calculate the total number of grids divided by the maximum bounding box according to the size of the plane grid, recorded as the total number of grids; count the number of grids with points in the plane grid, recorded as the effective number of grids, and finally calculate the ratio of the effective number of grids to the total number of grids, and determine whether the ratio is greater than the plane completeness rate threshold. If so, output the plane point cloud.

2. The residual cloud plane fitting method based on the improved random sampling consensus algorithm according to claim 1 is characterized in that: In step 2, the distance threshold is the distance threshold from the point to the fitting plane; the angle threshold is the angle threshold between the fitting plane normal vector and the expected fitting plane normal vector.

3. The residual defect cloud plane fitting method based on the improved random sampling consensus algorithm according to claim 1 is characterized in that: The specific judgment method of step 3 is: Randomly select three points P1(x1,y1,z1), P2(x2,y2,z2), P3(x3,y3,z3), based on the plane equation AX+BY+CZ+D=0, let the vector calculate Get the normal vector N of the three-point fitting plane. If |N| == 0, the three points are collinear. Reselect 3 points to fit the plane until |N| ​​is not 0.

4. The residual cloud plane fitting method based on the improved random sampling consensus algorithm according to claim 1 is characterized in that: The bounding box filtering method described in step 7 is specifically: According to the maximum clustered point cloud whose plane completeness is less than the completeness threshold, a maximum bounding box is established by floating 1 to 5 mm on the Z axis, the original point cloud is returned, and the point cloud inside the maximum bounding box in the original point cloud is removed, the parameters are initialized, and the step 3 is returned to reselect points for calculation until the plane completeness is greater than the completeness threshold, and the plane point cloud at this time is output.

Images