Disclosure of Invention
The invention aims to provide a method for extracting target point cloud aiming at the defects of the prior art, which can accurately and effectively extract the three-dimensional point cloud data extracted from a complex scene.
In order to achieve the above object, the present invention provides a method for extracting a target point cloud, the method comprising:
acquiring first point cloud data, and performing drying treatment on the first point cloud data to generate second point cloud data;
calculating a normal vector of the second point cloud data;
selecting a reference normal vector;
calculating an included angle between a normal vector of the second point cloud data and the reference normal vector;
judging the second point cloud data with the included angle smaller than a first threshold value as an external point; judging the second point cloud data with the included angle larger than or equal to a first threshold value as an interior point, and generating third point cloud data;
and extracting the target point cloud from the third point cloud data based on a random sample consensus (RANSAC) algorithm.
Further, the first point cloud data is three-dimensional coordinates captured by a time-of-flight sensor.
Further, the drying the first point cloud data to generate second point cloud data specifically includes:
extracting depth data of the first point cloud data, and establishing a two-dimensional point cloud matrix;
extracting K3 multiplied by 3 sub-matrixes of the two-dimensional point cloud matrix;
establishing a position index of a central element of the 3 x 3 sub-matrix in the two-dimensional point cloud matrix;
adding the central element in the 3 multiplied by 3 sub-matrix with the absolute value of the difference of other elements respectively, and recording as M;
if the M is larger than a second threshold value, judging that the central element is a noise point, finding the position of the noise point in the two-dimensional point cloud matrix according to the position index, and discarding the element corresponding to the noise point;
if the M is smaller than or equal to a second threshold value, retaining an element corresponding to the position of the central element in the two-dimensional point cloud matrix;
and generating the second point cloud data from the first point cloud data corresponding to the elements reserved in the two-dimensional point cloud matrix.
Further, if the M is greater than a second threshold value:
extracting 4 2 × 2 sub-matrices of the 3 × 3 sub-matrices;
comparing the absolute value of the difference between the element in each 2 x 2 sub-matrix and the central element, and recording the minimum value as N;
if N is larger than or equal to a third threshold value, judging that the central element is a noise point, finding the position of the noise point in the two-dimensional point cloud matrix according to the position index, and discarding the element corresponding to the noise point;
if N is smaller than a third threshold value, retaining an element corresponding to the position of the central element in the two-dimensional point cloud matrix;
and generating the second point cloud data by using the first point cloud data corresponding to the elements reserved in the two-dimensional point cloud matrix when the M is less than or equal to a second threshold and the N is less than a third threshold.
Further, the third threshold is not greater than half of the second threshold.
Further, the selecting of the reference normal vector specifically comprises:
and selecting a reference normal vector according to the normal vector characteristics of the object to be extracted.
Furthermore, in the K3 × 3 sub-matrices, the number of K is the number of internal elements surrounded by the first row, the last row, the first column, and the last column of the two-dimensional point cloud matrix.
Further, the step of establishing a position index of the central element of the 3 × 3 sub-matrix in the two-dimensional point cloud matrix specifically includes:
marking a row mark and a column mark of the central element of each 3 multiplied by 3 sub-matrix in the two-dimensional point cloud matrix, and matching corresponding depth data in the two-dimensional point cloud matrix according to the row mark and the column mark.
Further, the specific steps of performing target point cloud extraction on the third point cloud data based on the random sample consensus RANSAC algorithm are as follows:
randomly selecting a subset from the third point cloud data as an inner group;
generating an estimation model with the subset;
traversing the third point cloud data with the estimation model;
dividing the third point cloud data meeting constraint conditions into the inner groups;
and circulating the steps, selecting the most inner cluster points in the estimation model as an optimal model, and extracting all third point cloud data which accord with the optimal model.
Further, the termination condition of the above steps of the cycle is as follows: and jumping out of the cycle when a new subset cannot be randomly selected, or setting an inner group point threshold value which accords with the estimation model, and if the inner group point threshold value is greater than or equal to the inner group point threshold value, determining the estimation model as an optimal model and jumping out of the cycle.
The method for extracting the target point cloud provided by the embodiment of the invention can be used for accurately and effectively extracting the three-dimensional point cloud data extracted from a complex scene.
Detailed Description
The technical solution of the present invention is further described in detail by the accompanying drawings and embodiments.
Fig. 1 is a flowchart of a method for extracting a target point cloud according to an embodiment of the present invention, as shown in fig. 1, the method includes:
step S110, first point cloud data is obtained, and drying processing is carried out on the first point cloud data to generate second point cloud data.
Specifically, the first point cloud data is three-dimensional point cloud data captured by a time-of-flight sensor, coordinates of the three-dimensional point cloud data are stored in a sensor chip in a pixel array mode, the coordinates of the first point cloud data are values generated according to a sensor coordinate system, the coordinates include position information of a measured scene on a projection plane and depth information of the measured scene, and the projection plane is perpendicular to the depth data direction.
The method comprises the following steps of performing drying treatment on first point cloud data:
and extracting the depth data of the first point cloud data, and establishing a two-dimensional point cloud matrix.
Specifically, since the first point cloud data is stored in an array form, and the pixel array is the resolution of the sensor, the number of columns and the number of rows of the two-dimensional point cloud matrix extracted from the depth data of the first point cloud data are consistent with the resolution of the time-of-flight sensor. In a specific example, when the resolution of the time-of-flight sensor is 32 × 48, that is, the number of horizontal pixels of the time-of-flight sensor is 32 and the number of vertical pixels is 48, the number of rows in the two-dimensional point cloud matrix is 48 and the number of columns in the two-dimensional point cloud matrix is 32.
Specifically, the element position arrangement in the two-dimensional point cloud matrix is consistent with the storage position of the time-of-flight sensor array, and each adjacent element in the two-dimensional point cloud matrix is also adjacent in an actual scene.
And extracting K3 multiplied by 3 sub-matrixes of the two-dimensional point cloud matrix.
Specifically, the number K of 3 × 3 submatrices that can be extracted in the two-dimensional point cloud matrix and are not repeated most is the number of internal elements surrounded by the first row, the last row, the first column, and the last column of the two-dimensional point cloud matrix. In a specific example, when the two-dimensional point cloud matrix is 32 × 48, there are 1536 elements in total, 156 elements of the first row, the last row, the first column and the last column of the matrix are removed, that is, K is 1380, so that 1380 3 × 3 sub-matrices can be extracted, and when K is the maximum value, the dryness judgment of each point can be guaranteed to the maximum extent.
And establishing a position index of the central element for establishing the 3 x 3 sub-matrix in the two-dimensional point cloud matrix.
Specifically, a row mark and a column mark of a central element of each 3 × 3 sub-matrix in the two-dimensional point cloud matrix are marked, and corresponding depth data in the two-dimensional point cloud matrix is matched according to the row mark and the column mark. Because the drying judgment is carried out on the central element of the 3 multiplied by 3 sub-matrix, only the position of the central element needs to be marked, thereby greatly reducing the calculation amount of the system.
And adding the central element in the 3 multiplied by 3 sub-matrix with the absolute value of the difference of other elements respectively, and recording the sum as M. Wherein, the other elements except the central element in the 3 × 3 sub-matrix are 8 elements of the first row, the third row, the first column and the third column.
If the M is smaller than or equal to a second threshold value, retaining an element corresponding to the position of the central element in the two-dimensional point cloud matrix;
if the M is larger than a second threshold value, judging that the central element is a noise point, finding the position of the noise point in the two-dimensional point cloud matrix according to the position index, and discarding the element corresponding to the noise point;
specifically, the size of the second threshold is in inverse proportion to the number of noise points, when the second threshold is smaller, more noise points are determined to be noise points, but when the threshold is too small, supersaturation occurs, target point cloud data in the scene is discarded by mistake, when the second threshold is larger, less noise points are determined, and when the second threshold is too large, too many noise points are retained, and the drying effect is not obvious, so that a suitable second threshold can be found by selecting different thresholds for multiple times according to a standard measurement scene, and in a specific embodiment, when the two-dimensional point cloud matrix is 240 × 320, preferably, the second threshold is 0.2.
And generating the second point cloud data from the first point cloud data corresponding to the elements reserved in the two-dimensional point cloud matrix.
In a preferred embodiment, when M is greater than the second threshold, making a further determination:
extracting 4 2 × 2 sub-matrices of the 3 × 3 sub-matrices. Wherein each of said 2 x 2 sub-matrices necessarily includes a central element of said 3 x 3 sub-matrix, and the other elements of each of said 2 x 2 sub-matrices are adjacent to the central element;
comparing the absolute value of the difference between the element in each 2 x 2 sub-matrix and the central element, and recording the minimum value as N;
if N is larger than or equal to a third threshold value, judging that the central element is a noise point, finding the position of the noise point in the two-dimensional point cloud matrix according to the position index, and discarding the element corresponding to the noise point;
if N is smaller than a third threshold value, retaining an element corresponding to the position of the central element in the two-dimensional point cloud matrix;
specifically, a suitable third threshold value can be found by selecting different threshold values for multiple times according to a standard detected scene, preferably, the third threshold value is not more than half of the second threshold value, and the false deletion rate of the noise point can be effectively reduced by further judging that M is more than the second threshold value.
And generating the second point cloud data by using the first point cloud data corresponding to the elements reserved in the two-dimensional point cloud matrix when the M is less than or equal to a second threshold and the N is less than a third threshold.
Step S120, calculating a normal vector of the second point cloud data.
Specifically, the normal vector of the second point cloud data is calculated by fitting a local total least square plane of the point and the neighboring points thereof through a principal component analysis method, the normal vector of the local plane is used for approximating the normal vector of the substitute point, and the calculated normal vector is subjected to feature classification, and due to the influence of factors such as errors, the normal vector of the point cloud on the same plane is approximately parallel and perpendicular to the plane. Since the first point cloud data has been subjected to the drying process in step S110, it is avoided that the estimated normal vector has a large deviation due to the existence of outliers.
Step S130, selecting a reference normal vector. The selected reference normal vector is specifically as follows:
and selecting a reference normal vector according to the normal vector characteristics of the object to be extracted. In an actual example, a scene is composed of a background plate, a cylindrical object to be measured and a test bed, first point cloud data composed of the background plate, the cylindrical object to be measured and the test bed are acquired simultaneously through a flight time sensor, wherein the background plate and the test bed both have plane characteristics, the test bed is extracted firstly, then the background plate is extracted, and finally the purpose of retaining the required cylindrical object to be measured is achieved. In consideration of the influence of acquisition errors and the like, the normal vector of the point cloud data of the test bed is approximately perpendicular to the test bed, so the direction of the reference normal vector is selected to be perpendicular to the direction of the test bed.
Step S140, calculating an included angle between the normal vector of the second point cloud data and the reference normal vector.
Specifically, in the embodiment of step S130, an included angle between the normal vector of the second point cloud data and the reference normal vector perpendicular to the test bed is calculated, and since the normal vector of the collected point cloud data of the test bed is similar to the perpendicular test bed, the included angle between the normal vector of the point cloud data of the test bed and the reference normal vector is very small.
Step S150, judging the second point cloud data with the included angle smaller than a first threshold value as an external point; judging the second point cloud data with the included angle larger than or equal to the first threshold value as an interior point, and generating third point cloud data
Specifically, in the embodiment of step S130, an included angle between the normal vector of the extracted point cloud data of the test bed and the reference normal vector is small, so that the first threshold is set in a small range, but when an error of the point cloud data acquired by the time-of-flight sensor is large, if the first threshold is set to be too small, extraction is not complete enough, and therefore, in order to obtain a relatively ideal extraction effect, it is preferable to set the first threshold to be 20 degrees. When the included angle is larger than a first threshold value, the part of point cloud data and the test bed are not on the same parallel plane, the part of point cloud data is judged to be internal points to be reserved, when the included angle is smaller than the first threshold value, the part of point cloud is judged to be collected test bed point cloud data, and the part of point cloud data is judged to be external points to be deleted. The remaining second point cloud data is composed of the three-dimensional point cloud data of the background plate and the cylindrical object to be measured, and the processing method for judging and deleting the point cloud data of the background plate is the same as that of the experiment table, and is not repeated here. And after the experiment table and the background plate are extracted and deleted, generating third point cloud data by the remaining point cloud data.
Step S160, performing target point cloud extraction on the third point cloud data based on a Random Sample Consensus (RANSAC) algorithm. The method comprises the following specific steps:
and randomly selecting a subset from the third point cloud data as an inner group.
Specifically, the number of the point clouds of the selected subset is not less than the number of unknown parameters of the estimation model.
An estimation model is generated using the subset.
Specifically, model parameters are calculated according to the point cloud coordinates in the selected subset, and therefore an estimation model is generated.
Traversing the third point cloud data with the estimation model.
Specifically, the cloud data of other third points than the subset are substituted into the estimation model.
And dividing the third point cloud data meeting the constraint condition into the inner group.
Specifically, a minimum error value is set, and the value error of the third point cloud data substituted into the estimation model is smaller than the constraint condition, and is classified as an inner cluster point. The minimum error for use as a constraint can be determined by a number of experiments.
And circulating the steps, selecting the most inner cluster points in the estimation model as an optimal model, extracting all third point cloud data which accord with the optimal model, and extracting the final target point cloud.
Specifically, in the process of cycling through the above steps, two situations occur to cause the cycle to jump out, and the first situation is to jump out the cycle when a new non-repeating subset cannot be randomly selected. The second situation is that an inner group point threshold which accords with the estimation model is set, if the number of the inner group points merged into the subset is larger than or equal to the threshold, the estimation model is determined as the optimal model and the circulation is jumped out, the efficiency of the circulation jumping out under the second situation is higher than that under the first situation, when the point cloud data volume is large, the circulation frequency is also large, and the circulation can be terminated as long as the estimation model in the threshold is found in the circulation process, the circulation is not required to be continued, and the system resources are greatly saved.
According to the method for extracting the target point cloud, provided by the embodiment of the invention, the redundant scenes are removed through drying and normal vector, and then the RANSAC algorithm is combined to realize high-speed extraction of the target point cloud, so that the phenomenon of boundary error extraction caused by singly adopting the RANSAC algorithm is effectively improved, when a plurality of target objects are adopted, the extraction of the target point cloud through the method has the characteristics of high stability and effectiveness, and the average time for extracting the target point cloud is less than 1 second through multiple tests and averaging.
Those of skill would further appreciate that the various illustrative components and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative components and steps have been described above generally in terms of their functionality in order to clearly illustrate this interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied in hardware, a software module executed by a processor, or a combination of the two. A software module may reside in Random Access Memory (RAM), memory, Read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are merely exemplary embodiments of the present invention, and are not intended to limit the scope of the present invention, and any modifications, equivalent substitutions, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.