CN117392166B - Ground plane fitting-based three-stage point cloud ground segmentation method - Google Patents
Ground plane fitting-based three-stage point cloud ground segmentation method Download PDFInfo
- Publication number
- CN117392166B CN117392166B CN202311711321.8A CN202311711321A CN117392166B CN 117392166 B CN117392166 B CN 117392166B CN 202311711321 A CN202311711321 A CN 202311711321A CN 117392166 B CN117392166 B CN 117392166B
- Authority
- CN
- China
- Prior art keywords
- ground
- plane
- point
- point cloud
- point set
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 70
- 230000011218 segmentation Effects 0.000 title claims abstract description 38
- 239000013598 vector Substances 0.000 claims description 24
- 238000012937 correction Methods 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000000638 solvent extraction Methods 0.000 claims description 8
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 4
- 239000006185 dispersion Substances 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 230000001960 triggered effect Effects 0.000 claims description 3
- 230000008030 elimination Effects 0.000 claims description 2
- 238000003379 elimination reaction Methods 0.000 claims description 2
- 230000008447 perception Effects 0.000 abstract description 2
- 238000013528 artificial neural network Methods 0.000 description 4
- 238000013135 deep learning Methods 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 2
- 238000013527 convolutional neural network Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/136—Segmentation; Edge detection involving thresholding
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/60—Analysis of geometric attributes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Geometry (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention discloses a three-stage point cloud ground segmentation method based on ground plane fitting, which belongs to the technical field of intelligent driving perception and comprises the following steps of: s1, dividing an original point cloud by adopting an infinite polar coordinate grid with large resolution, carrying out voxel downsampling on the point cloud in the grid, and dividing the original point cloud into an initial ground point set and a non-ground point set based on a ground plane fitted by the downsampled point cloud; s2, based on ground plane fitting, extracting false negative points in an initial non-ground point set, and adding the false negative points into the initial ground point set to obtain an intermediate ground point set and an intermediate non-ground point set; s3, based on ground plane fitting, false positive points in the middle ground point set are extracted and added into the middle non-ground point set, so that a ground point set and a non-ground point set are obtained; the method provided by the invention has the advantages of high segmentation accuracy, high speed, good stability and easiness in realization, can effectively solve the problems of under segmentation, over segmentation and low calculation efficiency existing in the traditional method at present, and can be used for obstacle avoidance and navigation of intelligent driving.
Description
Technical Field
The invention relates to the technical field of intelligent driving perception, in particular to a three-stage point cloud ground segmentation method based on ground plane fitting.
Background
The rise of intelligent driving technology has led to the transformation of the traffic field, and provides possibility for realizing higher road safety, traffic efficiency and convenience. Intelligent driving vehicles need to have a high degree of context awareness and decision making capabilities in order to achieve autonomous navigation in complex road environments, sensors being a key component to achieving these capabilities. Sensor systems play a vital role in intelligent driving for sensing environmental information around a vehicle, and these sensors can be classified into different types including cameras, radar, lidar, GPS, etc. The laser radar creates the point cloud by emitting laser beams and measuring the return time of the laser beams, and can provide high-resolution distance and environment information for tasks such as point cloud ground segmentation, environment map construction and the like.
The point cloud ground segmentation is one of important preprocessing steps and basic tasks in intelligent driving, and can provide accurate priori information for sensing tasks such as drivable region detection, point cloud clustering, target detection and the like. By separating ground points and non-ground points from the point cloud data, the intelligent driving system can more accurately understand road conditions, make decisions and plan paths, which helps to improve vehicle safety and driving efficiency, and therefore achieving efficient and accurate segmentation is particularly important. At present, in the field of point cloud ground segmentation, many conventional methods and methods based on deep learning are studied. The traditional method is mainly based on geometric features, shape analysis, surface fitting and other technologies, such as RANSAC, hough transformation and the like, and is used for fitting of a ground plane and point cloud segmentation. However, these methods face challenges in processing complex terrains and scenes because the diversity of ground point patterns and the presence of noise interference make accurate identification of the ground difficult, and these conventional methods have problems of under-segmentation, over-segmentation, computational inefficiency, and insufficient robustness in processing point cloud data of large-scale, complex scenes. Meanwhile, the deep learning-based method has remarkable progress in point cloud ground segmentation, such as Convolutional Neural Network (CNN), graph rolling network (GCN) and other methods, and the methods utilize flexible expression capability and feature learning capability of the neural network to improve the precision and efficiency of ground point and non-ground point segmentation tasks. However, there are also some disadvantages to the deep learning based approach. First, neural networks require a large amount of annotation data for supervised learning, whereas the annotation of point cloud data is relatively complex and time-consuming. Second, the neural network training process requires a long time, especially for complex network structures and large-scale point cloud data. Furthermore, neural networks tend to require higher computing resources and memory space, which can present challenges for some resource-constrained devices or scenarios; noise and sparsity problems of point cloud data can also negatively impact the performance of the network.
Disclosure of Invention
Aiming at the defects of the traditional ground segmentation method, the invention provides a three-stage point cloud ground segmentation method based on ground plane fitting, which has the advantages of high segmentation accuracy, high speed, good stability and easy realization, and has important significance for realizing safer and more efficient automatic driving of an intelligent driving system.
In order to achieve the above purpose, the present invention provides the following technical solutions:
a three-stage point cloud ground segmentation method based on ground plane fitting comprises the following steps:
s1: dividing an original point cloud by adopting an infinite polar coordinate grid with large resolution, and carrying out voxel downsampling on the point cloud in each grid; after eliminating the reflection noise of the vehicle body in the down-sampling point cloud, the down-sampling point cloud is used for replacing the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane to judge whether the plane is a ground plane or not; according to the actual ground plane or the ideal ground plane in each grid, the original point cloud is divided into an initial ground point set and a non-ground point set by adopting a large distance threshold corresponding to the first stage;
s2: dividing an initial non-ground point set by adopting an infinite polar coordinate grid, and copying a copy of point cloud in each grid; using the duplicate point cloud to replace the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane and the minimum signed distance from the point to the plane so as to judge whether the plane is a ground plane or not; according to the actual ground plane or the ideal ground plane in each grid, adopting a distance threshold corresponding to the second stage to extract false negative points in the initial non-ground point set and adding the false negative points into the initial ground point set to obtain an intermediate ground point set and an intermediate non-ground point set;
s3: dividing a middle ground point set by adopting an infinite polar coordinate grid, and copying a copy of point cloud in each grid; after eliminating the car body reflection noise in the duplicate point cloud, using the duplicate point cloud to replace the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane to judge whether the plane is a ground plane or not; and extracting false positive points in the middle ground point set by adopting a small distance threshold corresponding to the third stage according to the actual ground plane or the ideal ground plane in each grid, adding the false positive points into the middle non-ground point set, and finally obtaining the ground point set and the non-ground point set.
Further, the specific embodiment of S1 is:
s101: with the origin of the laser radar coordinate system as the center, large radial resolution is adoptedAnd a large angular resolution->Dividing the original point cloud into +.>A grid of->Is at the diameter->The number of ring regions divided by the range interval of the outermost ring region is +.>；/>The number of grids divided for each ring;
s102: for the stage gridPoint cloud inside->Voxel downsampling is performed, and the downsampled point cloud is marked as +.>；
S103: for a pair ofAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S104: from the following componentsReject->In (1) car body reflection noise->Wherein->Respectively set vertical angle of view threshold and height threshold, +.>And->Point +.>Vertical angle of view and height value of +.>；
S105: recordingThe lowest height value +.>；
S106: by usingReplace->Performing ground plane fitting;
s107: judging the fitting result of the plane and partitioning the point cloud: according to->Calculating the degree of inclination +.>Wherein->For the normal vector of the plane, +.>Is a unit normal vector in the vertical Z direction; an angle threshold value for setting the degree of inclination of a plane +.>When->Or->When in use, the point cloud can be divided according to the plane>Obtaining a preliminary ground point set +.>Wherein->A large distance threshold set for this stage; when->When the plane is fitted to the side of the obstacle, the point cloud is split by the ideal ground plane>If->The lowest height value in +.>Less than or equal to->The obtained preliminary ground point set isIf->Is greater than->Then isWherein->For a set height threshold, the coefficient +.>For the ground height proportion ++>Is the ground level relative to the origin of the lidar coordinate system; the preliminary non-ground point set is as follows；
S108: recording the original point cloud of a frame acquired by a laser radar at the time t asAt this stage, the->Is initially segmented into an initial ground point set +.>And an initial set of non-ground points->。
Further, the specific implementation manner of S2 is as follows:
s201: with the origin of the laser radar coordinate system as the center, the adopted resolutionRespectively isAnd->An initial set of non-ground points +.>Division into->A grid with->Wherein->Is at the diameter->The number of ring regions divided by the range interval of the outermost ring region is +.>；/>The number of grids divided for each ring;
s202: for the stage gridPoint cloud inside->Copy one copy, record->；
S203: for duplicate point cloudAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S204: recordingThe lowest height value +.>；
S205: using ordered copy point cloudsSubstitute original point cloud->Performing ground plane fitting;
s206: judging the fitting result of the plane and partitioning the point cloud: according to->Calculating the degree of inclination +.>When->Or->Further judging whether the plane is a ground plane, setting FN point distance threshold +.>When->Minimum signed distance +.>The plane is then a ground plane, from which the plane is taken to be +.>Extracting point set containing FN points>WhereinA distance threshold set for this stage; while->Or->But->In this case, it is not fitted to the ground but to the gentle top of the obstacle, or +.>When the plane is fitted to the side of the obstacle, the two above plane fitting results are incorrect, then the plane is ideally ground from +.>Extracting a point set containing FN points, if ∈>The lowest height value in +.>Less than or equal to->Obtaining a point set containing FN points asIf->Is greater than->Then is；/>The remaining points in (a) are non-ground points +.>；
S207: from this stageExtracting a point set containing FN points as +.>Will->Add->Middle and rear constituting a middle ground point set +.>，/>The remaining points in (a) constitute an intermediate non-ground point set 。
Further, the specific implementation manner of S3 is as follows:
s301: the origin of the laser radar coordinate system is taken as the center, and the adopted resolutions are respectivelyAnd->The middle ground point set ∈>Divided into->A grid with->Wherein->Is at the polar diameterThe number of ring regions divided by the range interval of the outermost ring region is +.>； />The number of grids divided for each ring;
s302: computing gridMaximum height difference>And judge->And a set maximum height difference threshold +.>Is a size relationship of (2); when->When the point cloud in the grid is directly added into the ground point set +.>And continuing to process the point cloud in the next grid at the stage, so as to avoid fitting the ground plane to the grid only containing ground points as much as possible, and improving the segmentation speed; when->When there are false positive points in the grid, it is necessary thatFitting the ground plane and eliminating, and continuing to execute the step S303 and the subsequent steps;
s303: to the gridPoint cloud inside->Copy one copy, record->；
S304: for duplicate point cloudAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S305: from the following componentsReject->In (1) car body reflection noise-> ；
S306: recordingThe lowest height value +.>；
S307: using ordered copy point cloudsSubstitute original point cloud->Performing ground plane fitting;
s308: fitting results to a planeLine judging and partitioning point cloud: according to->Calculating the degree of inclination +.>When->Or->In this case, the plane is taken as the basis>Extracting a point set containing FP points +.>Wherein->A small distance threshold set for this stage, and hasThe method comprises the steps of carrying out a first treatment on the surface of the While->When the plane is fitted to the side of the obstacle, then the plane is made to pass through the ideal ground plane from +.>Extracting a point set containing FP points, if ∈>The lowest height value in +.>Less than or equal to->The obtained F-containing materialThe point set of P points is +.>If->Is greater than->Then is；/>The remaining points in (a) are ground points +.>The method comprises the steps of carrying out a first treatment on the surface of the 3.9 From this stageExtracting a point set containing FP points as +.>Will->Added to->Middle and rear constituting a non-ground point set->，/>The remaining points of (a) constitute a ground point set +.>。
Further, the steps S106, S205, and S307 described in the summary of the invention all use the same ground plane fitting method, and the specific implementation manner is as follows:
s401: selecting an initial seed point: from the gridPoint clouds with ordered interior ∈>Starting to select points in sequence, firstly obtaining a lowest point representative point set +.>The method comprises the steps of carrying out a first treatment on the surface of the Then calculate +.>Average height value of all elements in +.>To obtain an initial seed point set +.>The method comprises the steps of carrying out a first treatment on the surface of the Wherein->Respectively selecting the maximum number represented by the lowest point and the height range of the initial seed point;
s402: singular value decomposition is carried out on a ground point set participating in each iterative operation: recording the ground point set participating in the Nth iterative operation asWherein->And there is->The average point of the ground point set participating in the nth iteration can be obtained>Wherein->The method comprises the steps of carrying out a first treatment on the surface of the By->ObtainingCovariance matrix>To obtain->A degree of dispersion in space; by->For covariance matrixSingular value decomposition is performed, wherein ∈>Is->Left singular vector matrix,/, of (2)>Is a diagonal matrix comprising +.>Singular values of>Is->Right singular vector matrix of (a); at->In which the singular values are ordered from large to small (assumed to be) Minimum singular value ∈>The corresponding feature vector represents the minimum change range of the point cloudDirection corresponding to +.>The last column of (a) is +.>The normal vector of the ground is marked as +.>；
S403: updating the ground point set: at the nth iteration, a simple linear model may be usedTo express the required ground plane by +.>Finding out any point in the point cloud>Signed distance +.>Wherein->The method comprises the steps of carrying out a first treatment on the surface of the Setting a distance threshold value from the ground plane>The ground point set obtained by the nth iteration is；
S404: judging whether iteration converges or not: the plane normal vectors obtained in the n+1th iteration and the N iteration are respectively recorded asAnd->Wherein N is greater than or equal to 1, of the formulaCalculating the included angle change of two adjacent iterations>Setting a small angle thresholdWhen->Or->When the iteration has converged and stopped;
s405: correcting incorrect plane fitting results: after the iteration is finished, calculating the inclination degree of the planeIf (if)A plane fit to the obstacle side; by->Finding out any point in the point cloud>Unsigned distance to the fitted plane +.>Wherein:for the average point coordinates corresponding to the plane, +.>The method comprises the steps of carrying out a first treatment on the surface of the Setting a distance thresholdWhen plane fitting to the side of the obstacle, +.>Less than or equal to->Is the inner point, i.eIn the point cloud->Is greater than->The points of (2) are outer points, i.e. +.>The method comprises the steps of carrying out a first treatment on the surface of the Will->Discarding to stop obstacle points from participating in iterative operation and using +.>Fitting the ground plane again, so that the fitting result of the plane is corrected once; setting maximum correction times->When the fitting result of the plane is correct or the correction number reaches +.>Stopping correction when the correction is performed; if the number of initial seed points selected in the new cycle is less than 3 after the mechanism for correcting the plane is repeatedly triggered, or the correction frequency reaches +.>After that, the correct ground plane can not be fitted, and then an ideal ground plane is constructed manually: setting the normal vector of ideal ground plane->If->Minimum height value of interior point +.>Less than or equal to->Setting the average point coordinate of the ideal ground plane as +.>If->Is greater than->Setting +.>。
Compared with the prior art, the invention has the following beneficial effects;
(1) The invention provides a three-stage point cloud ground segmentation method based on ground plane fitting, and provides a method for eliminating reflection noise of a vehicle body, wherein the method is implemented by adopting the method that the direct view angle of a point Yun Zhongchui is lower than a threshold valueAnd the height value is below the threshold valueThe points of the (2) are removed, so that the negative influence of reflection noise on the fitting result is eliminated, and the stability of segmentation is improved;
(2) According to the three-stage point cloud ground segmentation method based on the ground plane fitting, provided by the invention, the convergence judgment method is provided, and whether the iteration converges or not is judged by calculating the normal vector included angle of two adjacent iterations, so that the algorithm stops iteration in time after the iteration converges, the relation between iteration times and segmentation accuracy is balanced, and the segmentation speed is improved;
(3) According to the ground plane correction method, the internal points are discarded, the external points are used for fitting the ground plane again, and an ideal ground plane is constructed, so that an error plane fitting result is corrected, the problem that the plane is wrongly fitted to the side face of an obstacle is solved, the number of false positive points and false negative points is reduced, and the precision and recall rate are improved;
(4) The invention provides a ground-plane-fitting-based three-stage point cloud ground segmentation method, which provides a method for extracting false negative points in an initial non-ground point set, and the method judges whether a plane is a ground plane or not by calculating the inclination degree of the plane and combining with a FN point distance threshold value, and extracts the false negative points in the initial non-ground point set according to an ideal ground plane when the plane is not the ground plane, so that the recall rate is improved;
(5) The three-stage point cloud ground segmentation method based on the ground plane fitting provides a method for extracting false positive points in a middle ground point set, and the method reduces the number of grids needing to be fitted with the ground plane as much as possible and improves the segmentation speed by calculating the maximum height difference in the grids; for grids containing false positive points, the method improves accuracy by fitting a ground plane and according to an ideal ground plane when the plane is not the ground plane, and simultaneously extracting false positive points in the middle ground point set by adopting a smaller distance threshold.
Drawings
FIG. 1 is a block flow diagram of a three-stage point cloud ground segmentation method based on ground plane fitting in an embodiment of the invention;
fig. 2 is a schematic diagram of an infinity polar grid model employed in an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Embodiment one:
a three-stage point cloud ground segmentation method based on ground plane fitting, as shown in figure 1, comprises the following steps:
s1: dividing an original point cloud by adopting a grid model shown in fig. 2 with large resolution, and carrying out voxel downsampling on the point cloud in each grid; after eliminating the reflection noise of the vehicle body in the down-sampling point cloud, the down-sampling point cloud is used for replacing the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane to judge whether the plane is a ground plane or not; according to the actual ground plane or the ideal ground plane in each grid, the original point cloud is divided into an initial ground point set and a non-ground point set by adopting a large distance threshold corresponding to the first stage; the specific implementation mode is as follows:
s101: with the origin of the laser radar coordinate system as the center, large radial resolution is adoptedAnd a large angular resolution->Dividing the original point cloud into +.>A grid of->Is at the diameter->The number of ring regions divided by the range interval of the outermost ring region is +.>；/>The number of grids divided for each ring;
s102: for the stage gridPoint cloud inside->Voxel downsampling is performed, and the downsampled point cloud is marked as +.>；
S103: for a pair ofAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S104: from the following componentsReject->In (1) car body reflection noise->Wherein->Respectively set vertical angle of view threshold and height threshold, +.>And->Point +.>Vertical angle of view and height value of +.>；
S105: recordingThe lowest height value +.>；
S106: by usingReplace->Performing ground plane fitting;
s107: judging the fitting result of the plane and partitioning the point cloud: according to->Calculating the degree of inclination +.>Wherein->For the normal vector of the plane, +.>Is a unit normal vector in the vertical Z direction; an angle threshold value for setting the degree of inclination of a plane +.>When->Or->When in use, the point cloud can be divided according to the plane>Obtaining a preliminary ground point set +.>Which is provided withMiddle->A large distance threshold set for this stage; when->When the plane is fitted to the side of the obstacle, the point cloud is split by the ideal ground plane>If->The lowest height value in +.>Less than or equal to->The obtained preliminary ground point set isIf->Is greater than->Then isWherein->For a set height threshold, the coefficient +.>For the ground height proportion ++>Is the ground level relative to the origin of the lidar coordinate system; the preliminary non-ground point set is as follows；
S108: recording the original point cloud of a frame acquired by a laser radar at the time t asAt this stage, the->Is initially segmented into an initial ground point set +.>And an initial set of non-ground points->。
S2: dividing an initial non-ground point set by adopting a grid model shown in fig. 2, and copying a copy of the point cloud in each grid; using the duplicate point cloud to replace the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane and the minimum signed distance from the point to the plane so as to judge whether the plane is a ground plane or not; according to the actual ground plane or the ideal ground plane in each grid, adopting a distance threshold corresponding to the second stage to extract false negative points in the initial non-ground point set and adding the false negative points into the initial ground point set to obtain an intermediate ground point set and an intermediate non-ground point set; the specific implementation mode is as follows:
s201: the origin of the laser radar coordinate system is taken as the center, and the adopted resolutions are respectivelyAnd->An initial set of non-ground points +.>Division into->A grid with->Wherein->Is at the diameter->The number of ring regions divided by the range interval of the outermost ring region is +.>；/>The number of grids divided for each ring;
s202: for the stage gridPoint cloud inside->Copy one copy, record->；
S203: for duplicate point cloudAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S204: recordingThe lowest height value +.>；
S205: using ordered copy point cloudsSubstitute original point cloud->Performing ground plane fitting;
s206: judging the fitting result of the plane and partitioning the point cloud: according to->Calculating the degree of inclination +.>When->Or->Further judging whether the plane is a ground plane, setting FN point distance threshold +.>When->Minimum signed distance +.>The plane is then a ground plane, from which the plane is taken to be +.>Extracting point set containing FN points>WhereinA distance threshold set for this stage; while->Or->But->In this case, it is not fitted to the ground but to the gentle top of the obstacle, or +.>When the plane is fitted to the side of the obstacle, the two above plane fitting results are incorrect, then the plane is ideally ground from +.>Extracting a point set containing FN points, if ∈>The lowest height value in +.>Less than or equal to->Obtaining a point set containing FN points asIf->Is greater than->Then is；/>The remaining points in (a) are non-ground points +.>；
S207: from this stageExtracting a point set containing FN points as +.>Will->Add->Middle and rear constituting a middle ground point set +.>，/>The remaining points in (a) constitute an intermediate non-ground point set 。
S3, dividing a middle ground point set by adopting the grid model shown in FIG. 2, and copying a copy of the point cloud in each grid; after eliminating the car body reflection noise in the duplicate point cloud, using the duplicate point cloud to replace the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane to judge whether the plane is a ground plane or not; according to the actual ground plane or the ideal ground plane in each grid, adopting a small distance threshold corresponding to the third stage to extract false positive points in the middle ground point set and add the false positive points into the middle non-ground point set, and finally obtaining a ground point set and a non-ground point set; the specific implementation mode is as follows:
s301: the origin of the laser radar coordinate system is taken as the center, and the adopted resolutions are respectivelyAnd->The middle ground point set ∈>Divided into->A grid with->Wherein->Is at the polar diameterThe number of ring regions divided by the range interval of the outermost ring region is +.>； />The number of grids divided for each ring;
s302: computing gridMaximum height difference>And judge->And a set maximum height difference threshold +.>Is a size relationship of (2); when->When the point cloud in the grid is directly added into the ground point set +.>And continuing to process the point cloud in the next grid at the stage, so as to avoid fitting the ground plane to the grid only containing ground points as much as possible, and improving the segmentation speed; when->When there are false positive points in the grid, the ground plane is required to be fitted for elimination, and S303 and the following steps are continuedA subsequent step;
s303: to the gridPoint cloud inside->Copy one copy, record->；
S304: for duplicate point cloudAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S305: from the following componentsReject->In (1) car body reflection noise-> ；
S306: recordingThe lowest height value +.>；
S307: using ordered copy point cloudsSubstitute original point cloud->Performing ground plane fitting;
s308: judging the fitting result of the plane and partitioning the point cloud: according to->Calculating the degree of inclination +.>When->Or->In this case, the plane is taken as the basis>Extracting a point set containing FP points +.>Wherein->A small distance threshold set for this stage, and hasThe method comprises the steps of carrying out a first treatment on the surface of the While->When the plane is fitted to the side of the obstacle, then the plane is made to pass through the ideal ground plane from +.>Extracting a point set containing FP points, if ∈>The lowest height value in +.>Less than or equal to->The obtained point set containing FP points is +.>If->Is greater than->Then is；/>The remaining points in (a) are ground points +.>The method comprises the steps of carrying out a first treatment on the surface of the 3.9 From this stageExtracting a point set containing FP points as +.>Will->Added to->Middle and rear constituting a non-ground point set->，/>The remaining points of (a) constitute a ground point set +.>。
The specific embodiments of S106, S205, and S307 all adopt the same ground plane fitting method, and the specific embodiments thereof are as follows:
s401: selecting an initial seed point: from the gridPoint clouds with ordered interior ∈>Starting to select points in sequence, firstly obtaining a lowest point representative point set +.>The method comprises the steps of carrying out a first treatment on the surface of the Then calculate +.>Average height value of all elements in +.>To obtain an initial seed point set +.>The method comprises the steps of carrying out a first treatment on the surface of the Wherein->Respectively selecting the maximum number represented by the lowest point and the height range of the initial seed point;
s402: singular value decomposition is carried out on a ground point set participating in each iterative operation: recording the ground point set participating in the Nth iterative operation asWherein->And there is->The average point of the ground point set participating in the nth iteration can be obtained>Wherein->The method comprises the steps of carrying out a first treatment on the surface of the By->ObtainingCovariance matrix>To obtain->A degree of dispersion in space; by->For covariance matrixSingular value decomposition is performed, wherein ∈>Is->Left singular vector matrix,/, of (2)>Is a diagonal matrix comprising +.>Singular values of>Is->Right singular vector matrix of (a); at->In which the singular values are ordered from large to small (assumed to be) Minimum singular value ∈>The corresponding feature vector represents the direction in which the point cloud variation range is smallest, corresponding to +.>The last column of (a) is +.>The normal vector of the ground is marked as +.>；
S403: updating the ground point set: at the nth iteration, a simple linear model may be usedTo express the required ground plane by +.>Finding out any point in the point cloud>Signed distance +.>Wherein->The method comprises the steps of carrying out a first treatment on the surface of the Setting a distance threshold value from the ground plane>The ground point set obtained by the nth iteration is；
S404: judging whether iteration converges or not: the plane normal vectors obtained in the n+1th iteration and the N iteration are respectively recorded asAnd->Wherein N is greater than or equal to 1, of the formulaCalculating the included angle change of two adjacent iterations>Setting a small angle thresholdWhen->Or->When the iteration has converged and stopped;
s405: correcting incorrect plane fitting results: after the iteration is finished, calculating the inclination degree of the planeIf (if)A plane fit to the obstacle side; by->Finding out any point in the point cloud>Unsigned distance to the fitted plane +.>Wherein:for the average point coordinates corresponding to the plane, +.>The method comprises the steps of carrying out a first treatment on the surface of the Setting a distance thresholdWhen plane fitting to the side of the obstacle, +.>Less than or equal to->Is the inner point, i.eIn the point cloud->Is greater than->The points of (2) are outer points, i.e. +.>The method comprises the steps of carrying out a first treatment on the surface of the Will->Discarding to stop obstacle points from participating in iterative operation and using +.>Fitting the ground plane again, so that the fitting result of the plane is corrected once; setting maximum correction times->When the fitting result of the plane is correct or the correction number reaches +.>Stopping correction when the correction is performed; if the number of initial seed points selected in the new cycle is less than 3 after the mechanism for correcting the plane is repeatedly triggered, or the correction frequency reaches +.>After that, the correct ground plane can not be fitted, and then an ideal ground plane is constructed manually: setting the normal vector of ideal ground plane->If->Inner pointsMinimum height value->Less than or equal to->Setting the average point coordinate of the ideal ground plane as +.>If->Is greater than->Setting +.>。
The invention discloses a three-stage point cloud ground segmentation method based on ground plane fitting. Firstly, dividing an original point cloud by adopting an infinite polar coordinate grid with large resolution, carrying out voxel downsampling on the point cloud in the grid, and dividing the original point cloud into an initial ground point set and a non-ground point set based on a ground plane fitted by the downsampled point cloud; secondly, based on ground plane fitting, extracting false negative points in an initial non-ground point set and adding the false negative points into the initial ground point set to obtain an intermediate ground point set and an intermediate non-ground point set; and finally, based on ground plane fitting, extracting false positive points in the middle ground point set, and adding the false positive points into the middle non-ground point set to obtain a ground point set and a non-ground point set.
The three-stage point cloud ground segmentation method based on the ground plane fitting can be used for intelligent driving vehicles to detect feasible areas, barriers and the like in complex terrain environments such as urban environments and the like.
The above embodiments are merely illustrative of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention, and various modifications or equivalent substitutions made by those skilled in the art to which the present invention pertains without departing from the spirit of the invention, should fall within the scope of the present invention as defined in the appended claims.
Claims (5)
1. The three-stage point cloud ground segmentation method based on the ground plane fitting is characterized by comprising the following steps of:
s1: dividing an original point cloud by adopting an infinite polar coordinate grid with large resolution, and carrying out voxel downsampling on the point cloud in each grid; after eliminating the reflection noise of the vehicle body in the down-sampling point cloud, the down-sampling point cloud is used for replacing the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane to judge whether the plane is a ground plane or not; according to the actual ground plane or the ideal ground plane in each grid, the original point cloud is divided into an initial ground point set and a non-ground point set by adopting a large distance threshold corresponding to the first stage;
s2: dividing an initial non-ground point set by adopting an infinite polar coordinate grid, and copying a copy of point cloud in each grid; using the duplicate point cloud to replace the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane and the minimum signed distance from the point to the plane so as to judge whether the plane is a ground plane or not; according to the actual ground plane or the ideal ground plane in each grid, adopting a distance threshold corresponding to the second stage to extract false negative points in the initial non-ground point set and adding the false negative points into the initial ground point set to obtain an intermediate ground point set and an intermediate non-ground point set;
s3: dividing a middle ground point set by adopting an infinite polar coordinate grid, and copying a copy of point cloud in each grid; after eliminating the car body reflection noise in the duplicate point cloud, using the duplicate point cloud to replace the original point cloud in the corresponding grid to fit the ground plane; after the fitting result is obtained, calculating the inclination degree of the plane to judge whether the plane is a ground plane or not; and extracting false positive points in the middle ground point set by adopting a small distance threshold corresponding to the third stage according to the actual ground plane or the ideal ground plane in each grid, adding the false positive points into the middle non-ground point set, and finally obtaining the ground point set and the non-ground point set.
2. The ground plane fitting-based three-stage point cloud ground segmentation method according to claim 1, wherein S1 specifically comprises:
s101: with the origin of the laser radar coordinate system as the center, large radial resolution is adoptedAnd a large angular resolution->Dividing the original point cloud into +.>A grid of->Is at the diameter->The number of ring regions divided by the range interval of the outermost ring region is +.>；/>The number of grids divided for each ring;
s102: for the stage gridPoint cloud inside->Voxel downsampling is performed, and the downsampled point cloud is marked as +.>；
S103: for a pair ofAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S104: from the following componentsReject->In (1) car body reflection noise->WhereinRespectively set vertical angle of view threshold and height threshold, +.>And->Point +.>Vertical angle of view and height value of +.>；
S105: recordingThe lowest height value +.>；
S106: by usingReplace->Performing ground plane fitting;
s107: judging the fitting result of the plane and partitioning the point cloud: according to->Calculating the degree of inclination +.>Wherein->For the normal vector of the plane, +.>Is a unit normal vector in the vertical Z direction; an angle threshold value for setting the degree of inclination of a plane +.>When->Or->When in use, the point cloud can be divided according to the plane>Obtaining a preliminary ground point set +.>Wherein->A large distance threshold set for this stage; when->When the plane is fitted to the side of the obstacle, the point cloud is split by the ideal ground plane>If->The lowest height value in +.>Less than or equal to->The obtained preliminary ground point set isIf->Is greater than->Then isWherein->For a set height threshold, the coefficient +.>For the ground height proportion ++>Is the ground level relative to the origin of the lidar coordinate system; the preliminary non-ground point set is as follows；
S108: recording the original point cloud of a frame acquired by a laser radar at the time t asAt this stage, the->Is initially segmented into an initial ground point set +.>And an initial set of non-ground points->。
3. The ground plane fitting-based three-stage point cloud ground segmentation method according to claim 1, wherein S2 specifically comprises:
s201: the origin of the laser radar coordinate system is taken as the center, and the adopted resolutions are respectivelyAnd->An initial set of non-ground points +.>Division into->A grid with->Wherein->Is at the diameter->The number of ring regions divided by the range interval of the outermost ring region is +.>；/>The number of grids divided for each ring;
s202: for the stage gridPoint cloud inside->Copy one copy, record->；
S203: for duplicate point cloudAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S204: recordingThe lowest height value +.>；
S205: using ordered copy point cloudsSubstitute original point cloud->Performing ground plane fitting;
s206: for a pair ofJudging the fitting result of the plane and dividing the point cloud: according to->Calculating the degree of inclination +.>When->Or->If so, further judging whether the plane is a ground plane, setting FN point distance threshold value +.>When->Minimum signed distance +.>The plane is then a ground plane, from which the plane is taken to be +.>Extracting point set containing FN points>Wherein->A distance threshold set for this stage; while->Or->But->In this case, it is not fitted to the ground but to the gentle top of the obstacle, or +.>When the plane is fitted to the side of the obstacle, the two above plane fitting results are incorrect, then the plane is ideally ground from +.>Extracting a point set containing FN points, if ∈>The lowest height value in +.>Less than or equal to->Obtaining a point set containing FN points asIf->Is greater than->Then is；/>The remaining points in (a) are non-ground points +.>；
S207: from this stageExtracting a point set containing FN points as +.>Will->Add->Middle and rear constituting a middle ground point set +.>，/>The remaining points of (a) constitute an intermediate non-ground point set-> 。
4. The ground plane fitting-based three-stage point cloud ground segmentation method according to claim 1, wherein S3 specifically comprises:
s301: the origin of the laser radar coordinate system is taken as the center, and the adopted resolutions are respectivelyAnd->The middle ground point set ∈>Divided into->A grid with->Wherein->Is at the diameter->The number of ring regions divided by the range interval of the outermost ring region is +.>； />The number of grids divided for each ring;
s302: computing gridMaximum height difference>And judge->And a set maximum height difference thresholdIs a size relationship of (2); when->When the point cloud in the grid is directly added into the ground point set +.>And continuing to process the point cloud in the next grid at the stage, so as to avoid fitting the ground plane to the grid only containing ground points as much as possible, and improving the segmentation speed; when->When the grid has false positive points, the ground plane is required to be fitted for elimination, and the S303 and the subsequent steps are continuously executed;
s303: to the gridPoint cloud inside->Copy one copy, record->；
S304: for duplicate point cloudAscending order sorting is carried out according to the height values, and the ordered point clouds are marked as +.>；
S305: from the following componentsReject->In (1) car body reflection noise-> ；
S306: recordingThe lowest height value +.>；
S307: using ordered copy point cloudsSubstitute original point cloud->Performing ground plane fitting;
s308: judging the fitting result of the plane and partitioning the point cloud: according to->Calculating the degree of inclination +.>When->Or->In this case, the plane is taken as the basis>Extracting point set containing FP pointsWherein->A small distance threshold set for this stage, and there is +.>The method comprises the steps of carrying out a first treatment on the surface of the While->When the plane is fitted to the side of the obstacle, then the plane is made to pass through the ideal ground plane from +.>Extracting the FP point from the extractPoint set of (1)/(2)>The lowest height value in +.>Less than or equal to->The obtained point set containing FP points isIf->Is greater than->Then is；/>The remaining points in (a) are ground points +.>The method comprises the steps of carrying out a first treatment on the surface of the 3.9 From this stageExtracting a point set containing FP points as +.>Will->Added to->Middle and rear constituting a non-ground point set->，/>The remaining points of (a) constitute a ground point set +.>。
5. The ground plane fitting-based three-stage point cloud ground segmentation method according to claim 1, wherein the specific process of fitting the ground plane in S1-S3 is as follows:
s401: selecting an initial seed point: from the gridPoint clouds with ordered interior ∈>Starting to select points in sequence, firstly obtaining a lowest point representative point set +.>The method comprises the steps of carrying out a first treatment on the surface of the Then calculate +.>Average height value of all elements in +.>To obtain an initial seed point set +.>The method comprises the steps of carrying out a first treatment on the surface of the Wherein the method comprises the steps ofRespectively selecting the maximum number represented by the lowest point and the height range of the initial seed point;
s402: singular value decomposition is carried out on a ground point set participating in each iterative operation: recording deviceThe ground point set participating in the Nth iterative operation is as followsWherein->And there is->The average point of the ground point set participating in the nth iteration can be obtained>Wherein->The method comprises the steps of carrying out a first treatment on the surface of the By->Find->Covariance matrix>To obtain->A degree of dispersion in space; by->For covariance matrix->Singular value decomposition is performed, wherein ∈>Is->Left singular direction of (2)Quantity matrix,/->Is a diagonal matrix comprising +.>Singular values of>Is->Right singular vector matrix of (a); at->In which the singular values are ordered from big to small (assuming +.>) Minimum singular value ∈>The corresponding feature vector represents the direction in which the point cloud variation range is smallest, corresponding to +.>The last column of (a) is +.>The normal vector of the ground is marked as +.>；
S403: updating the ground point set: at the nth iteration, a simple linear model may be usedTo express the required ground plane by +.>Finding out any point in the point cloud>Signed distance +.>Wherein->The method comprises the steps of carrying out a first treatment on the surface of the Setting a distance threshold value from the ground plane>The ground point set obtained by the nth iteration is；
S404: judging whether iteration converges or not: the plane normal vectors obtained in the n+1th iteration and the N iteration are respectively recorded asAnd->Wherein N is greater than or equal to 1, is represented by the formula->Calculating the included angle change of two adjacent iterations>Setting a small angle threshold +.>When->Or (b)When the iteration has converged and stopped;
s405: correcting incorrect plane fitting results: after the iteration is finished, calculating the inclination degree of the planeIf (if)A plane fit to the obstacle side; by->Finding out any point in the point cloud>Unsigned distance to the fitted plane +.>Wherein:for the average point coordinates corresponding to the plane, +.>The method comprises the steps of carrying out a first treatment on the surface of the Setting a distance thresholdWhen plane fitting to the side of the obstacle, +.>Less than or equal to->Is the inner point, i.eIn the point cloud->Is greater than->The points of (2) are outer points, i.e. +.>The method comprises the steps of carrying out a first treatment on the surface of the Will->Discarding to stop obstacle points from participating in iterative operation and using +.>Fitting the ground plane again, so that the fitting result of the plane is corrected once; setting maximum correction times->When the fitting result of the plane is correct or the correction number reaches +.>Stopping correction when the correction is performed; if the number of initial seed points selected in the new cycle is less than 3 after the mechanism for correcting the plane is repeatedly triggered, or the correction frequency reaches +.>After that, the correct ground plane can not be fitted, and then an ideal ground plane is constructed manually: setting the normal vector of ideal ground plane->If->Minimum height value of interior point +.>Less than or equal to->Setting the average point coordinate of the ideal ground plane as +.>If->Is greater than->Setting +.>。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311711321.8A CN117392166B (en) | 2023-12-13 | 2023-12-13 | Ground plane fitting-based three-stage point cloud ground segmentation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311711321.8A CN117392166B (en) | 2023-12-13 | 2023-12-13 | Ground plane fitting-based three-stage point cloud ground segmentation method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117392166A CN117392166A (en) | 2024-01-12 |
| CN117392166B true CN117392166B (en) | 2024-02-02 |
Family
ID=89470684
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311711321.8A Active CN117392166B (en) | 2023-12-13 | 2023-12-13 | Ground plane fitting-based three-stage point cloud ground segmentation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117392166B (en) |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109190573A (en) * | 2018-09-12 | 2019-01-11 | 百度在线网络技术（北京）有限公司 | A kind of ground detection method, apparatus, electronic equipment, vehicle and storage medium |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11538135B2 (en) * | 2020-02-25 | 2022-12-27 | Raytheon Company | Automatic multi-image 3D ground control point extraction |
-
2023
- 2023-12-13 CN CN202311711321.8A patent/CN117392166B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109190573A (en) * | 2018-09-12 | 2019-01-11 | 百度在线网络技术（北京）有限公司 | A kind of ground detection method, apparatus, electronic equipment, vehicle and storage medium |
Non-Patent Citations (1)
| Title |
|---|
| 孙朋朋 ; 闵海根 ; 徐志刚 ; 赵祥模 ; .采用延伸顶点的地面点云实时提取算法.计算机工程与应用.2016,(第24期),10-14、40. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117392166A (en) | 2024-01-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022188663A1 (en) | Target detection method and apparatus | |
| CN112184736B (en) | Multi-plane extraction method based on European clustering | |
| CN112101092A (en) | Automatic driving environment sensing method and system | |
| CN111340875B (en) | Space moving target detection method based on three-dimensional laser radar | |
| CN112487919A (en) | 3D target detection and tracking method based on camera and laser radar | |
| CN114488194A (en) | Method for detecting and identifying targets under structured road of intelligent driving vehicle | |
| CN112711034B (en) | Object detection method, device and equipment | |
| CN115620261A (en) | Vehicle environment sensing method, system, equipment and medium based on multiple sensors | |
| CN113822278A (en) | License plate recognition method for unlimited scene | |
| CN114325634A (en) | Method for extracting passable area in high-robustness field environment based on laser radar | |
| CN111880197A (en) | Road boundary detection method based on four-line laser radar | |
| CN116109601A (en) | Real-time target detection method based on three-dimensional laser radar point cloud | |
| CN115205803A (en) | Automatic driving environment sensing method, medium and vehicle | |
| CN116469082A (en) | Obstacle and pothole multi-target clustering method based on road point cloud | |
| CN114280583B (en) | Laser radar positioning accuracy verification method and system without GPS signal | |
| CN114241448A (en) | Method and device for obtaining heading angle of obstacle, electronic equipment and vehicle | |
| CN117392166B (en) | Ground plane fitting-based three-stage point cloud ground segmentation method | |
| CN114170188A (en) | Target counting method and system for overlook image and storage medium | |
| CN114399762B (en) | Road scene point cloud classification method and storage medium | |
| CN117788735A (en) | Dynamic point cloud removing method based on grid division | |
| CN117516560A (en) | Unstructured environment map construction method and system based on semantic information | |
| CN116883973A (en) | Point cloud target detection method and device and electronic equipment | |
| CN116052099A (en) | Small target detection method for unstructured road | |
| CN111338336B (en) | Automatic driving method and device | |
| CN116863325A (en) | Method for multiple target detection and related product |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP03 | Change of name, title or address |
Address after: Workshop 2, No. 1059 Fenglin Avenue, Nanchang Economic Development Zone, Nanchang City, Jiangxi Province, 330000 Patentee after: Jiangxi Dongrui Intelligent Equipment Technology Co.,Ltd. Country or region after: China Address before: Workshop 2, No. 1059 Fenglin Avenue, Nanchang Economic Development Zone, Nanchang City, Jiangxi Province, 330000 Patentee before: Jiangxi Dongrui Machinery Co.,Ltd. Country or region before: China |
|
| CP03 | Change of name, title or address |