CN117788735A - Dynamic point cloud removing method based on grid division - Google Patents
Dynamic point cloud removing method based on grid division Download PDFInfo
- Publication number
- CN117788735A CN117788735A CN202311823142.3A CN202311823142A CN117788735A CN 117788735 A CN117788735 A CN 117788735A CN 202311823142 A CN202311823142 A CN 202311823142A CN 117788735 A CN117788735 A CN 117788735A
- Authority
- CN
- China
- Prior art keywords
- grid
- point cloud
- key frame
- point
- current key
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 59
- 230000008569 process Effects 0.000 claims description 13
- 241001632427 Radiola Species 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000000903 blocking effect Effects 0.000 claims description 10
- 238000005192 partition Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 3
- 238000000638 solvent extraction Methods 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 abstract description 12
- 238000005286 illumination Methods 0.000 abstract description 8
- 238000010276 construction Methods 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 5
- 238000013507 mapping Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 238000000513 principal component analysis Methods 0.000 description 1
- 230000007480 spreading Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Landscapes
- Optical Radar Systems And Details Thereof (AREA)
Abstract
A dynamic point cloud removing method based on grid division belongs to the technical field of high-precision map construction, navigation and planning. The method solves the problem that the existing method is difficult to realize high-precision and real-time dynamic point cloud data removal under the conditions of no illumination and no priori information. According to the method, the grid with the dynamic point cloud is identified by calculating and comparing the point cloud descriptors of the corresponding grids of the current key frame and the local map in the region of interest, so that the dynamic point cloud in the current scanning frame of the laser radar is effectively removed, the accuracy of removing the dynamic point cloud data is improved, and the accurate global point cloud map after filtering the dynamic point cloud can be finally constructed. The method is not limited by illumination and priori conditions, can be realized under the conditions of no illumination and no priori information, and can work all the day. The method can be used for removing the dynamic point cloud.
Description
Technical Field
The invention belongs to the technical field of high-precision map construction, navigation and planning, and particularly relates to a dynamic point cloud removal method based on grid division.
Background
With the development of technology, the demands of positioning and mapping are also increasing, and more accurate mapping and positioning are the trend of development nowadays. With the development of SLAM (Simultaneous Localization and Mapping) technology, SLAM technology is one of the fundamental technologies in the automatic driving technology today. However, in today's real environment, due to the existence of some dynamic point clouds, such as pedestrians, bicycles, and automobiles, the accuracy of building a map is reduced, and thus the positioning accuracy is affected.
The main current dynamic point cloud filtering method comprises the following steps: 1. based on semantic prior information: clustering clusters which are possibly dynamic point clouds are marked by using semantic priori, and the reprojection error of edge pixels in each clustering cluster is calculated to construct an optimization function, but the algorithm needs to use priori information to better identify and reject some dynamic point clouds which do not appear. And it relies on external illumination conditions, and cannot filter dynamic point clouds for dark environments. 2. Based on the point cloud clustering: and estimating the movement condition point by utilizing a point cloud scene flow estimation algorithm, and eliminating the dynamic point cloud by combining point cloud clustering and principal component analysis. The method has the advantages of being independent of a vision system and capable of operating under a dark condition. However, in scene flow pushing, the speed of the algorithm is low, so that the time consumption of the whole algorithm cannot be guaranteed to be within 0.1s, the real-time performance is difficult to guarantee, and a more flexible matching mechanism is lack for a bounding box of a dynamic point cloud. 3. Based on ground detection: and detecting dynamic point cloud in a non-ground area by using the Euclidean clustering method, and removing dynamic point cloud data. However, the dynamic point cloud is easy to misjudge, so that the static point cloud is filtered, and the drawing construction precision is affected.
In summary, under the conditions of no illumination and no prior information, the existing method is difficult to realize high-precision and real-time dynamic point cloud data removal, so that it is necessary to provide a new dynamic point cloud removal method.
Disclosure of Invention
The invention aims to solve the problem that the existing method is difficult to realize high-precision and real-time dynamic point cloud data removal under the conditions of no illumination and no priori information, and provides a dynamic point cloud removal method based on grid division.
The technical scheme adopted for solving the technical problems is as follows:
a dynamic point cloud removing method based on grid division specifically comprises the following steps:
step one, setting a time threshold T and a space threshold S, and selecting a historical key frame from a key frame database according to the time threshold T and the space threshold S for the current key frame; forming a current local map by using the current key frame and the selected historical key frame;
step two, converting the current local map into a laser radar coordinate system according to the pose of the laser radar coordinate system relative to a world coordinate system under the current key frame;
thirdly, constructing a space cylinder structure by taking the laser radar as the center of the cylinder, taking the height difference of the region of interest as the height of the cylinder and taking the radius of the region of interest as the radius of the cylinder, wherein the region contained in the constructed space cylinder structure is the region of interest;
step four, radial and axial blocking is carried out on the point cloud data of the current key frame in the interested area, so as to obtain the point cloud of each grid of the current key frame; radial and axial blocking is carried out on the local map point cloud data in the region of interest to obtain point clouds of grids of the local map;
step five, respectively calculating descriptors of grids of the current key frame and descriptors of grids of the current local map;
step six, classifying each grid of the current key frame according to the descriptors calculated in the step five, and determining the grid where the dynamic point cloud is located;
step seven, performing ground fitting on a grid where the dynamic point cloud is located, and removing dynamic point cloud data in the grid;
and step eight, writing the rest point clouds in the grids of the step seven, the point clouds in other grids and the current key frame point clouds positioned outside the region of interest into a key frame database together, and constructing a global map.
Further, in the first step, a historical key frame is selected from a key frame database for the current key frame according to a time threshold T and a space threshold S; the specific process is as follows:
and selecting historical key frames with the time distance from the current key frame being smaller than a time threshold T, respectively constructing a pose KD tree for each selected historical key frame, respectively calculating the space distance between the current key frame and each historical key frame according to the constructed pose KD tree, and selecting the historical key frames with the space distance from the current key frame being smaller than the space threshold S from the selected historical key frames.
Further, the pose of the laser radar coordinate system relative to the world coordinate system under the current key frame is obtained through a laser radar inertial odometer system.
Further, converting the current local map into a laser radar coordinate system; the method comprises the following steps:
wherein P is World The point cloud coordinates of the local map;the pose of the laser radar coordinate system relative to the world coordinate system under the current key frame; the upper corner mark-1 represents the inverse of the matrix; p (P) LiDAR To convert to point cloud coordinates in the lidar coordinate system.
Further, in the fourth step, the method for partitioning the current key frame point cloud data and the local map point cloud data in the interested area is consistent; the specific blocking method comprises the following steps:
step four, taking the center of the upper surface of the space cylinder structure as the center of the circle, and making N in the upper surface r Concentric circles, and setting the radius of the largest circle as the radius of the spatial cylinder structure;
sector division is carried out on the maximum circle, and the maximum circle is divided into N with equal area θ Sector, N θ Radius of each sector and N r The concentric circles all intersect, i.e. for each circle the area inside the circle is divided into N of equal area θ A plurality of sectors;
the upper surface of the space cylinder structure is divided into N r N θ A (i, j) th region obtained by dividing is expressed as G' (i,j) ，i＝1,2,…,N r I is the number of concentric circles, j=1, 2, …, N θ ，N θ The number of sectors divided for each circle;
step four, for any area obtained by dividing, projecting the area to the lower surface of the space cylinder structure to obtain a projection area; then the part between the area and the projection area is used as the grid corresponding to the area, and the grid corresponding to the (i, j) th area is marked as the (i, j) th grid G (i,j) ；
And similarly, processing each area obtained by dividing to obtain grids corresponding to each area.
Further, the grid G (i,j) Point p in (a) k ＝{x k ,y k ,z k The } satisfies:
wherein P is t To fall on grid G (i,j) A set of point clouds within; r is (r) k To p in polar coordinate system k The radius of the projected point in the xy plane,θ k ＝arctan(y k ,x k )，θ k to p in polar coordinate system k Polar angle of the projection point in xy plane; n (N) r Is the number of rings in the concentric region model; n (N) θ The number of sectors in the concentric region model; l (L) max Is a grid G (i,j) Is the maximum diameter of the lens; l (L) min Is a grid G (i,j) Is the minimum pole diameter, deltaL m Is a grid G (i,j) Is the difference of the polar diameters, deltaL m ＝L max -L min 。
Further, the number of sectors N θ The value of (2) is 24, the number of concentric circles N r The value of (2) is 3.
Further, the specific process of the fifth step is as follows:
wherein,representing the difference in height of the point cloud in the (i, j) th grid under the current key frame, i.e., the descriptor of the (i, j) th grid under the current key frame,>representing the difference in height of the point cloud in the (i, j) th grid under the local map, i.e., the descriptor of the (i, j) th grid under the local map,/>Z-axis coordinate set representing all point clouds in the (i, j) th grid under the current key frame, +.>Z-axis coordinates representing all point clouds within the (i, j) th grid under the local mapAnd (5) collecting.
Further, the specific process of the step six is as follows:
for the (i, j) th grid under the current key frame:
wherein,is the difference in height of the point cloud in the (i, j) th grid under the current key frame, +.>Is the height difference of the point cloud in the (i, j) th grid under the local map, S r Is an intermediate variable;
according to S r The grid where the dynamic point cloud is located is determined by the following steps: if the number of the point clouds contained in the (i, j) th grid is more than or equal to 10 and the condition (1) or the condition (2) is met, the (i, j) th grid is the grid where the dynamic point clouds are located;
condition (1): s is more than 0 and less than r < 0.2 and
condition (2): s is S r > 0.2 and there is a grid around the (i, j) th grid where the dynamic point cloud is located.
Further, the specific process of the step seven is as follows:
seventhly, selecting M point clouds with the smallest height from any grid where the dynamic point clouds are located as seed points to obtain a seed point set;
step seven, initializing iteration times l=1;
seventhly, calculating the average height value H of all seed points in the seed point set mean Then the height lower than H is selected mean +H ground Is taken as a ground candidate point, wherein H ground Is the ground threshold height;
performing ground fitting on all seed points and ground candidate points:
ax+by+cz+d＝0
wherein a, b, c, d are coefficients of the planar model, and x, y, z are three-dimensional coordinates of points in the planar model; i.e.
Wherein,the upper corner mark T represents a transposition;
seventhly, calculatingIs the value of (1):
calculating a covariance matrix C of the seed point set:
wherein,is the mean value of three-dimensional coordinates of all seed points in the seed point set, s 1 ,s 2 ,…,s M Three-dimensional coordinates of the 1 st, 2 nd, … th and M th seed points in the seed point set respectively;
after the feature decomposition of the covariance matrix C,the feature vector corresponding to the minimum feature value is equal to the feature vector;
seventy-five steps ofSubstituting the fitting equation in the seventh and third steps to obtain a fitting plane pi 0 ；
Step (a)76. Traversing all points in the grid, and calculating each point to the fitting plane pi 0 Distance of (2):
where D is the point (x 0 ,y 0 ,z 0 ) To the fitting plane pi 0 Is a distance of (2);
if D is less than the threshold, point (x 0 ,y 0 ,z 0 ) Is a ground point, and the point (x 0 ,y 0 ,z 0 ) Adding seed points for concentration; otherwise, D is equal to or greater than the threshold, then the point (x 0 ,y 0 ,z 0 ) Not ground points;
seventhly, judging whether the set maximum iteration times are reached;
if the set maximum iteration times are reached, taking all point clouds contained in the seed point set after the last iteration update as ground points, namely removing dynamic point cloud data;
if the set maximum iteration number is not reached, let l=l+1, and return to execute the seventh two by using the seed point set updated in the seventh six step.
The beneficial effects of the invention are as follows:
the invention provides a real-time dynamic point cloud filtering algorithm, which is used for identifying grids with dynamic point clouds by calculating and comparing point cloud descriptors of corresponding grids of a current key frame and a local map in an interested region, so that the dynamic point clouds in the current scanning frame of a laser radar are effectively removed, the accuracy of dynamic point cloud data removal is improved, and a precise global point cloud map after filtering the dynamic point clouds can be finally constructed. The method is not limited by illumination and priori conditions, can be realized under the conditions of no illumination and no priori information, and can work all the day.
Drawings
FIG. 1 is a flow chart of a grid partition based dynamic point cloud removal method of the present invention;
FIG. 2 is a schematic diagram of a ROI;
FIG. 3 is a concentric region model diagram;
FIG. 4 is a schematic diagram of a descriptor;
FIG. 5 is a schematic view of a ground fit;
in the figure, the triangle symbols represent the first selected seed point set;
FIG. 6a is a graph comparing the before and after filtering of the CACOLITower dataset;
FIG. 6b is a comparison of the CALombardstreet dataset before and after filtering;
FIG. 6c is a graph comparing the KITTI07 dataset filtered out before and after;
fig. 6d is a comparison of garden dataset filtering before and after.
Detailed Description
The first embodiment is as follows: this embodiment will be described with reference to fig. 1. The method for removing the dynamic point cloud based on grid division in the embodiment specifically comprises the following steps:
step one, setting a time threshold T and a space threshold S (the time threshold is 0.3S and the space threshold is 0.8 m) and selecting a historical key frame from a key frame database according to the time threshold T and the space threshold S as the current key frame; forming a current local map by using the current key frame and the selected historical key frame;
the initial frame can be used as a first key frame, then one frame is selected to be used as the key frame every fixed time step, and only the key frame is processed in the invention;
step two, converting the current local map into a laser radar coordinate system according to the pose of the laser radar coordinate system relative to a world coordinate system under the current key frame;
step three, as shown in fig. 2, constructing a spatial cylinder structure by taking the laser radar as the cylinder center, taking the height difference of the region of interest (region of interest, ROI) as the cylinder height and taking the radius of the region of interest as the cylinder radius, wherein the region contained in the constructed spatial cylinder structure is the region of interest;
height difference setting rules: setting according to the height difference between the laser radar and the ground and the height difference between the highest distance between the laser radar and the dynamic object, for example, the laser radar is 1m away from the ground, the height difference between the highest height of all dynamic objects and the laser radar is not more than 3m, and the ROI height range is set to be-1 m-3 m;
radius setting rules: according to different environments, for example, on an open road, 30-50m is generally taken, and the algorithm has a good effect;
step four, radial and axial blocking is carried out on the point cloud data of the current key frame in the interested area, so as to obtain the point cloud of each grid of the current key frame; radial and axial blocking is carried out on the local map point cloud data in the region of interest to obtain point clouds of grids of the local map;
step five, respectively calculating descriptors of grids of the current key frame and descriptors of grids of the current local map;
step six, classifying each grid of the current key frame according to the descriptors calculated in the step five, and determining the grid where the dynamic point cloud is located;
step seven, performing ground fitting on a grid where the dynamic point cloud is located, and removing dynamic point cloud data in the grid;
and step eight, writing the rest point clouds in the grids of the step seven, the point clouds in other grids (except the grid where the dynamic point cloud is located) and the current key frame point clouds outside the region of interest into a key frame database together, and constructing a global map.
The eighth step is specifically as follows:
the pose relation of the laser radar relative to the world coordinate system can be obtained through the laser radar odometer, the point cloud of each key frame is converted into the world coordinate system through the pose relation, and the global map after the dynamic point cloud is finally filtered can be obtained through accumulation.
The specific calculation formula is as follows:
wherein P is the point set of the global map, P LiDAR For a point coordinate in the lidar coordinate system at a key frame,p is the pose of the laser radar relative to the world coordinate system under a certain key frame World To the coordinates of the point in the world coordinate system.
The invention provides a dynamic point cloud real-time filtering method based on grid occupation condition difference, which can be widely used in various current environments and has good robustness.
The second embodiment is as follows: the first difference between this embodiment and the specific embodiment is that: in the first step, a historical key frame is selected from a key frame database according to a time threshold T and a space threshold S for the current key frame; the specific process is as follows:
and selecting historical key frames with the time distance from the current key frame being smaller than a time threshold T, respectively constructing a pose KD tree (K-dimension tree) for each selected historical key frame, respectively calculating the space distance between the current key frame and each historical key frame according to the constructed pose KD tree, and selecting the historical key frames with the space distance from the current key frame being smaller than a space threshold S from the selected historical key frames.
Other steps and parameters are the same as in the first embodiment.
And a third specific embodiment: this embodiment differs from the first or second embodiment in that: and the pose of the laser radar coordinate system relative to the world coordinate system under the current key frame is obtained through a laser radar inertial odometer system.
Other steps and parameters are the same as in the first or second embodiment.
The specific embodiment IV is as follows: this embodiment differs from one of the first to third embodiments in that: converting the current local map into a laser radar coordinate system; the method comprises the following steps:
wherein P is World The point cloud coordinates of the local map;the pose of the laser radar coordinate system relative to the world coordinate system under the current key frame; the upper corner mark-1 represents the inverse of the matrix; p (P) LiDAR To convert to point cloud coordinates in the lidar coordinate system.
Other steps and parameters are the same as in one to three embodiments.
Fifth embodiment: this embodiment will be described with reference to fig. 3. This embodiment differs from one to four embodiments in that: in the fourth step, the method for partitioning the current key frame point cloud data and the local map point cloud data in the interested area is consistent; the specific blocking method comprises the following steps:
step four, taking the center of the upper surface of the space cylinder structure as the center of the circle, and making N in the upper surface r Concentric circles, and setting the radius of the largest circle as the radius of the spatial cylinder structure;
sector division is carried out on the maximum circle, and the maximum circle is divided into N with equal area θ Sector, N θ Radius of each sector and N r The concentric circles all intersect, i.e. for each circle the area inside the circle is divided into N of equal area θ A plurality of sectors;
the upper surface of the space cylinder structure is divided into N r N θ A (i, j) th region obtained by dividing is expressed as G' (i,j) ，i＝1,2,…,N r I is the number of concentric circles, j=1, 2, …, N θ ，N θ The number of sectors divided for each circle;
step four, for any area obtained by dividing, projecting the area to the lower surface of the space cylinder structure to obtain a projection area; then the part between the area and the projection area is used as the grid corresponding to the area, and the (i, j) th area is corresponding toThe grid being denoted as (i, j) th grid G (i,j) ；
And similarly, processing each area obtained by dividing to obtain grids corresponding to each area.
Other steps and parameters are the same as in one to four embodiments.
The upper surface and the lower surface of the cylinder in this embodiment are parallel to the ground, and for any divided area, the grid corresponding to the area is divided by a plane parallel to the ground and having any height, and the obtained cross section is the same as the area, that is, each grid is a cylinder penetrating through the whole space cylinder in the height direction, and the shape of any horizontal cross section of the cylinder is the same. After the grids are divided by the method of the embodiment, the point clouds of the current key frame in each grid and the point clouds of the local map in each grid are obtained, and the point cloud blocking operation is completed.
Specific embodiment six: this embodiment differs from one of the first to fifth embodiments in that: the grid G (i,j) Point p in (a) k ＝{x k ,y k ,z k The } satisfies:
wherein P is t To fall on grid G (i,j) A set of point clouds within; r is (r) k To p in polar coordinate system k The radius of the projected point in the xy plane,θ k ＝arctan(y k ,x k )，θ k to p in polar coordinate system k Polar angle of the projection point in xy plane; n (N) r Is the number of rings in the concentric region model; n (N) θ The number of sectors in the concentric region model; l (L) max Is a grid G (i,j) Is the maximum diameter of the lens; l (L) min Is a grid G (i,j) Is the minimum pole diameter, deltaL m Is a gridG (i,j) Is the difference of the polar diameters, deltaL m ＝L max -L min 。
Other steps and parameters are the same as in one of the first to fifth embodiments.
Seventh embodiment: this embodiment differs from one of the first to sixth embodiments in that: the number N of the sectors θ The value of (2) is 24, the number of concentric circles N r The value of (2) is 3.
Other steps and parameters are the same as in one of the first to sixth embodiments.
Eighth embodiment: this embodiment will be described with reference to fig. 4. This embodiment differs from one of the first to seventh embodiments in that: the specific process of the fifth step is as follows:
wherein,representing the difference in height of the point cloud in the (i, j) th grid under the current key frame, i.e., the descriptor of the (i, j) th grid under the current key frame,>representing the difference in height of the point cloud in the (i, j) th grid under the local map, i.e., the descriptor of the (i, j) th grid under the local map,/>Z-axis coordinate set representing all point clouds in the (i, j) th grid under the current key frame, +.>Is expressed in the (i, j) th grid under the local mapAnd a z-axis coordinate set of all point clouds.
Other steps and parameters are the same as those of one of the first to seventh embodiments.
Detailed description nine: this embodiment differs from one to eight of the embodiments in that: the specific process of the step six is as follows:
for the (i, j) th grid under the current key frame:
wherein,is the difference in height of the point cloud in the (i, j) th grid under the current key frame, +.>Is the height difference of the point cloud in the (i, j) th grid under the local map, S r Is an intermediate variable;
according to S r The grid where the dynamic point cloud is located is determined by the following steps: if the number of the point clouds contained in the (i, j) th grid is more than or equal to 10 and the condition (1) or the condition (2) is met, the (i, j) th grid is the grid where the dynamic point clouds are located;
condition (1): s is more than 0 and less than r < 0.2 and
condition (2): s is S r > 0.2 and there is a grid around the (i, j) th grid where the dynamic point cloud is located.
Other steps and parameters are the same as in one to eight of the embodiments.
The fifth step can obtain the grid in the current key frame and the corresponding descriptors of the grid in the local map, and the following rule is used to classify the grid of the current key frame, and finally the grids in the current key frame are classified into four categories, which are respectively: sparse point cloud grid G little Dynamic point cloud grid G of current key frame dynamic Not dynamic points in the current keyframe, but dynamic point cloud grid G in the local map map Uncertain grid G uncertainty . The pseudocode for the classification process is shown in table 1:
TABLE 1
Detailed description ten: this embodiment will be described with reference to fig. 5. This embodiment differs from one of the embodiments one to nine in that: the specific process of the step seven is as follows:
seventhly, for any grid where the dynamic point cloud is located, selecting M point clouds with the smallest height from the grid as seed points to obtain a seed point set (namely, all the point clouds in the grid are ordered according to the order from small height to large height, and the point clouds arranged in the front M bits are used as seed points);
step seven, initializing iteration times l=1;
seventhly, calculating the average height value H of all seed points in the seed point set mean Then the height lower than H is selected mean +H ground Is taken as a ground candidate point, wherein H ground Is the ground threshold height;
performing ground fitting on all seed points and ground candidate points:
ax+by+cz+d＝0
wherein a, b, c, d are coefficients of the planar model, and x, y, z are three-dimensional coordinates of points in the planar model; i.e.
Wherein,the upper corner mark T represents a transposition;
seventhly, calculatingIs the value of (1):
calculating a covariance matrix C of the seed point set:
wherein,for the mean of the three-dimensional coordinates of all seed points in the seed point set (i.e. taking the mean of the x-axis coordinates of all seed points, taking the mean of the y-axis coordinates of all seed points, taking the mean of the z-axis coordinates of all seed points, three means make up->)，s 1 ,s 2 ,…,s M Three-dimensional coordinates of the 1 st, 2 nd, … th and M th seed points in the seed point set respectively;
the spreading condition of the seed point set can be obtained through the covariance matrix C, and the covariance matrix C is subjected to eigenvalue decomposition to obtain three eigenvectors, and the plane model is used for fitting the ground, so that the normal vector perpendicular to the planeCorresponds to the feature vector with the smallest feature value. Finally obtain the initial plane pi 0 。
After the feature decomposition of the covariance matrix C,the feature vector corresponding to the minimum feature value is equal to the feature vector;
seventy-five steps ofSubstituting the fitting equation in the seventh and third steps to obtain a fitting plane pi 0 ；
Seventhly, traversing all points in the grid, and respectively calculating each point to a fitting plane pi 0 Distance of (2):
where D is the point (x 0 ,y 0 ,z 0 ) To the fitting plane pi 0 Is a distance of (2);
if D is less than the threshold, point (x 0 ,y 0 ,z 0 ) Is a ground point, and the point (x 0 ,y 0 ,z 0 ) Adding seed points for concentration; otherwise, D is equal to or greater than the threshold, then the point (x 0 ,y 0 ,z 0 ) Not ground points;
seventhly, judging whether the set maximum iteration number (set as 5 times in the invention) is reached;
if the set maximum iteration times are reached, taking all point clouds contained in the seed point set after the last iteration update as ground points, namely removing dynamic point cloud data;
if the set maximum iteration number is not reached, let l=l+1, and return to execute the seventh two by using the seed point set updated in the seventh six step.
Other steps and parameters are the same as in one of the first to ninth embodiments.
Experimental part
The method can be used for filtering the laser radar in real time and dynamic point cloud, and the effectiveness of the method is described below by using the public data set and the private data set. And calculating and comparing the filtering rate of the dynamic point cloud and the track error of the mobile robot.
Firstly, an experiment is carried out by using an UrbanLoco unmanned data set, and a CACOLITower data set (high dynamic grade) and a CALombardstreet data set (medium dynamic grade) are used; second, a KITTI data set (dynamic class, etc.) is used; finally experiments were performed on the garden dataset collected by itself (dynamic level low).
The data set specific information is shown in table 2.
Table 2 dataset data table
In the track precision, compared with the current mainstream algorithms LeGO-LOAM and LIO_SAM, the track precision is calculated by using APE (absolute pose error), and the experimental result is shown in Table 3.
TABLE 3 absolute pose error RMSE (m) comparison Table
The improvement in the CACOLITower dataset was about 20.96%, the improvement in the CALombardstreet dataset was about 20.04%, and the improvement in the KITTI07 dataset was 28.24% over LIO-SAM. For the garden dataset, the track accuracy of ODF-LINS (method of the invention) was improved by about 5.3%. These results indicate that ODF-LINS has a greater impact on improving track accuracy in high-dynamic or medium-dynamic scenarios, and a smaller impact in low-dynamic scenarios, consistent with our expectations.
For dynamic point cloud filtering, the final mapping effect pairs are shown in fig. 6a, 6b, 6c and 6 d. In order to evaluate the filtering effect of the method of the invention, the following indices are defined:
the index data are shown in table 4:
table 4 dynamic point cloud removal test results
To illustrate the real-time nature of the algorithm, the algorithm was time-consuming to calculate by a program, and the experimental results are shown in table 5.
Table 5 average time consumption table for different data sets
As can be seen from Table 5, the time consumption of the method of the present invention in all data sets can be guaranteed to be within 0.1s, and the real-time requirement is satisfied.
The above examples of the present invention are only for describing the calculation model and calculation flow of the present invention in detail, and are not limiting of the embodiments of the present invention. Other variations and modifications of the above description will be apparent to those of ordinary skill in the art, and it is not intended to be exhaustive of all embodiments, all of which are within the scope of the invention.
Claims (10)
1. The dynamic point cloud removing method based on grid division is characterized by comprising the following steps of:
step one, setting a time threshold T and a space threshold S, and selecting a historical key frame from a key frame database according to the time threshold T and the space threshold S for the current key frame; forming a current local map by using the current key frame and the selected historical key frame;
step two, converting the current local map into a laser radar coordinate system according to the pose of the laser radar coordinate system relative to a world coordinate system under the current key frame;
thirdly, constructing a space cylinder structure by taking the laser radar as the center of the cylinder, taking the height difference of the region of interest as the height of the cylinder and taking the radius of the region of interest as the radius of the cylinder, wherein the region contained in the constructed space cylinder structure is the region of interest;
step four, radial and axial blocking is carried out on the point cloud data of the current key frame in the interested area, so as to obtain the point cloud of each grid of the current key frame; radial and axial blocking is carried out on the local map point cloud data in the region of interest to obtain point clouds of grids of the local map;
step five, respectively calculating descriptors of grids of the current key frame and descriptors of grids of the current local map;
step six, classifying each grid of the current key frame according to the descriptors calculated in the step five, and determining the grid where the dynamic point cloud is located;
step seven, performing ground fitting on a grid where the dynamic point cloud is located, and removing dynamic point cloud data in the grid;
and step eight, writing the rest point clouds in the grids of the step seven, the point clouds in other grids and the current key frame point clouds positioned outside the region of interest into a key frame database together, and constructing a global map.
2. The grid-partition-based dynamic point cloud removal method according to claim 1, wherein in the first step, a history key frame is selected from a key frame database for a current key frame according to a time threshold T and a space threshold S; the specific process is as follows:
and selecting historical key frames with the time distance from the current key frame being smaller than a time threshold T, respectively constructing a pose KD tree for each selected historical key frame, respectively calculating the space distance between the current key frame and each historical key frame according to the constructed pose KD tree, and selecting the historical key frames with the space distance from the current key frame being smaller than the space threshold S from the selected historical key frames.
3. The grid division-based dynamic point cloud removal method of claim 2, wherein the pose of the lidar coordinate system relative to the world coordinate system under the current keyframe is obtained by a lidar inertial odometer system.
4. The grid-partition-based dynamic point cloud removal method of claim 3, wherein said converting a current local map into a lidar coordinate system; the method comprises the following steps:
wherein P is World The point cloud coordinates of the local map;the pose of the laser radar coordinate system relative to the world coordinate system under the current key frame; the upper corner mark-1 represents the inverse of the matrix; p (P) LiDAR To convert to point cloud coordinates in the lidar coordinate system.
5. The grid-division-based dynamic point cloud removal method according to claim 4, wherein in the fourth step, the method for partitioning the current key frame point cloud data and the local map point cloud data in the region of interest is consistent; the specific blocking method comprises the following steps:
step four, taking the center of the upper surface of the space cylinder structure as the center of the circle, and making N in the upper surface r Concentric circles, and setting the radius of the largest circle as the radius of the spatial cylinder structure;
sector division is carried out on the maximum circle, and the maximum circle is divided into N with equal area θ Sector, N θ Radius of each sector and N r The concentric circles all intersect, i.e. for each circle the area inside the circle is divided into N of equal area θ A plurality of sectors;
the upper surface of the space cylinder structure is co-located with the upper surface of the space cylinder structureDivided into N r N θ A (i, j) th region obtained by dividing is expressed as G' (i,j) ，i＝1,2,…,N r I is the number of concentric circles, j=1, 2, …, N θ ，N θ The number of sectors divided for each circle;
step four, for any area obtained by dividing, projecting the area to the lower surface of the space cylinder structure to obtain a projection area; then the part between the area and the projection area is used as the grid corresponding to the area, and the grid corresponding to the (i, j) th area is marked as the (i, j) th grid G (i,j) ；
And similarly, processing each area obtained by dividing to obtain grids corresponding to each area.
6. The grid-partition-based dynamic point cloud removal method of claim 5, wherein said grid G (i,j) Point p in (a) k ＝{x k ,y k ,z k The } satisfies:
wherein P is t To fall on grid G (i,j) A set of point clouds within; r is (r) k To p in polar coordinate system k The radius of the projected point in the xy plane,θ k ＝arctan(y k ,x k )，θ k to p in polar coordinate system k Polar angle of the projection point in xy plane; n (N) r Is the number of rings in the concentric region model; n (N) θ The number of sectors in the concentric region model; l (L) max Is a grid G (i,j) Is the maximum diameter of the lens; l (L) min Is a grid G (i,j) Is the minimum pole diameter, deltaL m Is a grid G (i,j) Is the difference of the polar diameters, deltaL m ＝L max -L min 。
7. The grid-partition-based dynamic point cloud removing method as claimed in claim 6, wherein said number of sectors N θ The value of (2) is 24, the number of concentric circles N r The value of (2) is 3.
8. The grid division-based dynamic point cloud removal method according to claim 7, wherein the specific process of the fifth step is as follows:
wherein,representing the difference in height of the point cloud in the (i, j) th grid under the current key frame, i.e., the descriptor of the (i, j) th grid under the current key frame,>representing the difference in height of the point cloud in the (i, j) th grid under the local map, i.e., the descriptor of the (i, j) th grid under the local map,/>Z-axis coordinate set representing all point clouds in the (i, j) th grid under the current key frame, +.>Representing the set of z-axis coordinates of all point clouds within the (i, j) th grid under the local map.
9. The grid division-based dynamic point cloud removing method according to claim 8, wherein the specific process of the step six is:
for the (i, j) th grid under the current key frame:
wherein,is the difference in height of the point cloud in the (i, j) th grid under the current key frame, +.>Is the height difference of the point cloud in the (i, j) th grid under the local map, S r Is an intermediate variable;
according to S r The grid where the dynamic point cloud is located is determined by the following steps: if the number of the point clouds contained in the (i, j) th grid is more than or equal to 10 and the condition (1) or the condition (2) is met, the (i, j) th grid is the grid where the dynamic point clouds are located;
condition (1): s is more than 0 and less than r < 0.2 and
condition (2): s is S r > 0.2 and there is a grid around the (i, j) th grid where the dynamic point cloud is located.
10. The grid division-based dynamic point cloud removing method according to claim 9, wherein the specific process of the step seven is:
seventhly, selecting M point clouds with the smallest height from any grid where the dynamic point clouds are located as seed points to obtain a seed point set;
step seven, initializing iteration times l=1;
seventhly, calculating all seed points in the seed point setAverage height value H of (2) mean Then the height lower than H is selected mean +H ground Is taken as a ground candidate point, wherein H ground Is the ground threshold height;
performing ground fitting on all seed points and ground candidate points:
ax+by+cz+d＝0
wherein a, b, c, d are coefficients of the planar model, and x, y, z are three-dimensional coordinates of points in the planar model; i.e.
Wherein,the upper corner mark T represents a transposition;
seventhly, calculatingIs the value of (1):
calculating a covariance matrix C of the seed point set:
wherein,is the mean value of three-dimensional coordinates of all seed points in the seed point set, s 1 ,s 2 ,…,s M Three-dimensional coordinates of the 1 st, 2 nd, … th and M th seed points in the seed point set respectively;
after the feature decomposition of the covariance matrix C,the feature vector corresponding to the minimum feature value is equal to the feature vector;
seventy-five steps ofSubstituting the fitting equation in the seventh and third steps to obtain a fitting plane pi 0 ；
Seventhly, traversing all points in the grid, and respectively calculating each point to a fitting plane pi 0 Distance of (2):
where D is the point (x 0 ,y 0 ,z 0 ) To the fitting plane pi 0 Is a distance of (2);
if D is less than the threshold, point (x 0 ,y 0 ,z 0 ) Is a ground point, and the point (x 0 ,y 0 ,z 0 ) Adding seed points for concentration; otherwise, D is equal to or greater than the threshold, then the point (x 0 ,y 0 ,z 0 ) Not ground points;
seventhly, judging whether the set maximum iteration times are reached;
if the set maximum iteration times are reached, taking all point clouds contained in the seed point set after the last iteration update as ground points, namely removing dynamic point cloud data;
if the set maximum iteration number is not reached, let l=l+1, and return to execute the seventh two by using the seed point set updated in the seventh six step.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311823142.3A CN117788735A (en) | 2023-12-27 | 2023-12-27 | Dynamic point cloud removing method based on grid division |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311823142.3A CN117788735A (en) | 2023-12-27 | 2023-12-27 | Dynamic point cloud removing method based on grid division |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117788735A true CN117788735A (en) | 2024-03-29 |
Family
ID=90397635
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311823142.3A Pending CN117788735A (en) | 2023-12-27 | 2023-12-27 | Dynamic point cloud removing method based on grid division |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117788735A (en) |
-
2023
- 2023-12-27 CN CN202311823142.3A patent/CN117788735A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111563442B (en) | Slam method and system for fusing point cloud and camera image data based on laser radar | |
| CN113168717B (en) | Point cloud matching method and device, navigation method and equipment, positioning method and laser radar | |
| Ji et al. | A novel simplification method for 3D geometric point cloud based on the importance of point | |
| Wang et al. | Modeling indoor spaces using decomposition and reconstruction of structural elements | |
| CN113432600A (en) | Robot instant positioning and map construction method and system based on multiple information sources | |
| CN114926699B (en) | Indoor three-dimensional point cloud semantic classification method, device, medium and terminal | |
| CN112070769A (en) | Layered point cloud segmentation method based on DBSCAN | |
| CN108764157B (en) | Building laser foot point extraction method and system based on normal vector Gaussian distribution | |
| Chen et al. | The mixed kernel function SVM-based point cloud classification | |
| Tang et al. | Multiple-kernel based vehicle tracking using 3D deformable model and camera self-calibration | |
| CN113484875B (en) | Laser radar point cloud target hierarchical identification method based on mixed Gaussian ordering | |
| Ortega et al. | Generating 3D city models from open LiDAR point clouds: Advancing towards smart city applications | |
| CN115620261A (en) | Vehicle environment sensing method, system, equipment and medium based on multiple sensors | |
| WO2023031620A1 (en) | Incremental dense 3-d mapping with semantics | |
| CN116109601A (en) | Real-time target detection method based on three-dimensional laser radar point cloud | |
| CN114066773B (en) | Dynamic object removal based on point cloud characteristics and Monte Carlo expansion method | |
| Sun et al. | Oriented point sampling for plane detection in unorganized point clouds | |
| Cai et al. | A lightweight feature map creation method for intelligent vehicle localization in urban road environments | |
| CN113420648B (en) | Target detection method and system with rotation adaptability | |
| Sun et al. | Automated segmentation of LiDAR point clouds for building rooftop extraction | |
| CN112200248A (en) | Point cloud semantic segmentation method, system and storage medium under urban road environment based on DBSCAN clustering | |
| CN117053779A (en) | Tightly coupled laser SLAM method and device based on redundant key frame removal | |
| Xin et al. | Accurate and complete line segment extraction for large-scale point clouds | |
| Su et al. | SLIBO-Net: Floorplan Reconstruction via Slicing Box Representation with Local Geometry Regularization | |
| CN117788735A (en) | Dynamic point cloud removing method based on grid division |
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 |