CN111079765B - Sparse point cloud densification and pavement removal method based on depth map - Google Patents

Abstract

The invention discloses a depth map-based sparse point cloud densification and pavement removal method, and relates to the field of laser radar data processing and application. The method comprises the steps of firstly utilizing a sensor parameter matrix to project sparse point cloud to obtain a sparse depth map, obtaining a rough dense depth map by adopting a linear interpolation method based on Delaunay Triangulation (DT), and then obtaining a fine dense depth map by applying a mask extracted from the sparse depth map. RANSAC ground fitting is carried out on the sparse point cloud in a space coordinate system, outliers are removed, then a ground-free sparse depth map is obtained through projection, a mask is generated, the fine dense depth map is further applied, and a road-surface-free fine dense depth map is obtained. And finally, carrying out back projection on the two depth maps to obtain dense point clouds with a road surface and without the road surface. The method can perform densification and pavement removal on the point cloud data of interest on the premise of not combining other data, has high speed and better ensures the validity of point cloud data information.

Description

Sparse point cloud densification and pavement removal method based on depth map

Technical Field

The invention relates to the field of laser radar data processing and application of environment perception and target detection in automatic driving.

Background

In the field of autopilot, lidar is widely used for its precise positioning of target objects. The lidar is an active sensor, which receives laser pulses reflected by an object to realize the detection of the object, can provide accurate distance information at a higher frequency, has strong robustness to the intensity of ambient light and the color of the object, and can make up for the defects of the traditional camera in this respect, so the lidar is often used as an important component member for environment sensing and target detection. Lidar has been used in the past for real-time positioning and mapping (SLAM) of robots. In recent years, with the appearance of multi-beam laser radars, the perception capability of the laser radars is greatly improved, and the obstacles can be accurately perceived in various weather and complex traffic scenes. However, the laser radar data has the characteristics of dispersion and sparseness, and is difficult to directly use in subsequent algorithms. Therefore, the point cloud data is densified, which is a work with practical significance. In the process of target detection, a target is often located on a road surface, and road surface points in the point cloud data are regarded as interference points in the target detection and need to be removed. The point cloud is densified first and then the ground points are removed, which increases the calculation amount and increases the calculation time, while the point cloud accuracy after densification is reduced by removing the ground points and then the point cloud is densified first, and how to densify the sparse point cloud and remove the road surface on the premise of ensuring the accuracy and reducing the calculation amount is a challenging task.

Existing methods for point cloud densification are mainly classified into image-based methods, point cloud-based methods, and image and point cloud-based methods. Rashidi, a et al propose a method of generating a disparity map based on binocular visual matching, then converting the disparity map into a dense depth map, and finally generating a dense point cloud. Similarly, Zhenfeng Shao et al propose a dense point cloud generation algorithm based on multi-view low-altitude remote sensing images. Kai-Han Lo et al propose a method of combining color images and sparse depth maps using a joint trilateration filter to densify the depth maps. However, the algorithm data is mainly derived from or combined with the image, and the generated data is not robust to illumination and color change, so that the advantage of the laser radar is difficult to utilize and utilize subsequently. The traditional method for densifying based on point cloud mainly directly samples and expands the original point cloud in a space coordinate system, and the method has the defects of large calculation amount, poor real-time performance and the like.

Disclosure of Invention

The technical problem to be solved by the invention is to provide a method for quickly densifying and removing a sparse point cloud in an automatic driving environment, wherein a sparse point cloud projection is converted into a sparse depth map, and a DT linear interpolation is used for performing densification operation to obtain an initial dense depth map, RANSAC ground removal operation and outlier filtering are performed on the sparse point cloud at the same time and are converted into a ground-free sparse depth map, then two kinds of sparse depth map generation masks are used for performing fine processing on the initial dense depth map, and finally, reverse projection is performed to obtain the dense point cloud with a road surface and a road surface. The method can be used for quickly carrying out densification and road surface filtering on point clouds, well ensuring the integrity of point cloud data, effectively solving the problem that the existing laser radar acquired data is too sparse, can be used as an effective means for data preprocessing of automatic driving, and can also be used for further carrying out 3D reconstruction on the basis.

In order to solve the above technical problems and achieve the above object, the present invention adopts the following technical solutions.

A sparse point cloud densification and pavement removal method based on a depth map is characterized by comprising the following steps:

step 1: inputting original sparse point cloud C_pointsAnd an internal and external parameter matrix T-point cloud seat obtained by utilizing sensor calibrationThe object is converted and a sparse depth map DM is extracted_originAnd (3) turning to the step (2);

step 2: for the sparse depth map DM extracted in the step 1_originPerforming Delaunay triangulation linear interpolation to obtain an initial rough dense depth map DDM_wildTurning to step 3;

and step 3: sparse depth map DM extracted by step 1_originGenerating a mask_detailedAnd applied to DDM_wildTo obtain a fine dense depth map DDM_detailedTurning to step 4;

and 4, step 4: for the original sparse point cloud C in step 1_pointsPerforming RANSAC ground fitting and removing the ground, then filtering outliers, and extracting a ground-free sparse depth map DM_ngTurning to step 5;

and 5: non-ground sparse depth map DM extracted by step 4_ngGenerating a mask_ngAnd applied to DDM_detailedObtaining a surface-free fine dense depth map DDM_ngTurning to step 6;

step 6: for the fine dense depth map DDM obtained in the step 3_detailedAnd step 5, obtaining a ground-free fine dense depth map DDM_ngPerforming back projection by using the internal and external parameter matrix T to obtain dense point cloud DC_pointsAnd dense point cloud DC_{points_ng}。

In the above technical solution, the step 1 specifically includes the following steps:

step 1.1: inputting original sparse point cloud C_pointsThe data format is [ X, Y, Z]I.e. the spatial coordinate data of the point cloud, calibrating the sensor (refer to the prior art document "A topox for automatic calibration of range and camera sensors using a single shot") to obtain an internal and external parameter matrix T;

step 1.2: utilizing internal and external parameter matrix T to sparse point cloud C_pointsCoordinate projection conversion is carried out to obtain C_img(spatial coordinate system format data for representing point cloud is converted into data in image coordinate system format) in [ xX, yY, X]And is formatted to obtain C'_imgThe data format is [ X, y, X]I.e. C_pointsAt coordinates X, y in image space and X coordinate (depth) in the spatial coordinate system, the transformation formula is as follows:

C_img＝T·C_points

step 1.3: c 'is filtered off'_imgPoints in the image outside the boundary (h, w), pair C'_imgThe X and the y in the point cloud are rounded, then the point cloud is sorted from small to large according to the depth X, and the repeated points of the X and the y coordinates are removed to obtain the corrected sparse point cloud C ″_img；

Step 1.4: constructing an all-zero matrix I of size (h, w)₀Using the corrected sparse point cloud C ″_imgData [ X, y, X ] of (2)]In I₀Is filled with the corresponding depth X to obtain a sparse depth map DM_origin；

In the above technical solution, the specific steps of step 2 are as follows:

step 2.1: sparse depth map DM using Delaunay triangulation_originIs triangulated to produce an isolated set of triangulated meshes, Δ ═ δ { δ₁,…,δ_nEach triangle delta comprises three coordinates n₁、n₂And n₃(ii) a Where n is₁、n₂And n₃In fact, the reference number of the coordinates of the depth map in the triangular mesh indicates that the interpolation operation is to be performed in the triangle formed by the three coordinates;

step 2.2: linear interpolation is carried out by using the depth X on three coordinates of the triangle delta to estimate the depth on the triangle inner point, and finally the rough dense depth map DDM is obtained_wild；

In the above technical solution, the step 3 specifically comprises the following steps:

step 3.1: constructing an all-zero matrix mask of size (h, w)₀；

Step 3.2: for the sparse depth map DM obtained in the step 1_originEach column searches the coordinate index u of the point with the first depth X not being 0 from top to bottom, and masks₀All the u-th element to the last element in each column are set to be 1 to obtain mask'₀；

Step 3.3: mask 'with transverse square structural element pair'₀Performing morphological transverse expansion to obtain a mask_detailed；

Step 3.4: will mask_detailedAnd DDM_wildMultiplying to obtain a fine dense depth map DDM_detailed；

In the above technical solution, the step 4 specifically includes the following steps:

step 4.1: inputting original sparse point cloud C_pointsPerforming ground fitting by using RANSAC algorithm, and removing the ground to obtain a ground-free sparse point cloud C_{points_ng}；

Step 4.2: non-ground sparse point cloud C adopting Euclidean distance statistics_{points_ng}M points around each point in the image and calculating the average distance between each point and its surrounding m points

Calculate the m points relative to

Marking the point with the deviation larger than the threshold value rho as an outlier to be removed to obtain C_{points_ng_d}；

Step 4.3: to C_{points_ng_d}Generating a ground-free sparse depth map DM by adopting the same steps as the step 1_ng；

In the above technical solution, the step 5 specifically includes the following steps:

step 5.1: construct an all-zero matrix mask of size (h, w)₀₁；

Step 5.2: for the non-ground sparse depth map DM obtained in the step 4_ngEach column searches the coordinate index uv of the first point with depth X not being 0 and the last non-zero point from top to bottom, and masks are processed₀₁All the u-th element to the v-th element in each column are set to be 1 to obtain mask'₀₁；

Step 5.3: mask 'is paired by oval structural elements'₀₁Performing morphological vertical open operation to obtain mask_ng；

Step 5.4: will mask_ngAnd DDM_detailedMultiplying to obtain a non-ground fine dense depth map DDM_ng；

In the above technical solution, the reverse specific steps in the step 6 are as follows:

step 6.1: for the fine dense depth map DDM obtained in the step 3_detailedAnd step 5, obtaining a ground-free fine dense depth map DDM_ngRespectively finding out the non-zero coordinates and the depth X corresponding to the coordinates, and constructing in the format of [ X, y, X]The shape is nx3, and n is the number of non-zero points to obtain point cloud C;

step 6.2: and multiplying C by the inverse of the internal and external parameter matrix T of the sensor to obtain the dense point cloud DC.

The invention adopts the technical scheme, so the invention has the following beneficial effects:

according to the method, the point cloud data can be densified and pavement removed on the premise that the point cloud data is not combined with other data, RANSAC ground fitting removal is performed on the sparse point cloud, and densification is performed based on the depth map, so that the method has the characteristics of high speed, capability of better ensuring the effectiveness of point cloud data information and the like.

Drawings

FIG. 1 is a design flow of a sparse point cloud densification and pavement removal method based on a depth map;

FIG. 2 is an input raw point cloud;

FIG. 3 is a dense point cloud and its corresponding depth map;

FIG. 4 is a dense point cloud with road surface removed and its corresponding depth map

Mask2 in FIG. 1 is a mask_ngMask1 is a mask_detailed。

Detailed Description

The present invention will be described in further detail with reference to test examples and specific embodiments. It should be understood that the scope of the above-described subject matter is not limited to the following examples, and any techniques implemented based on the disclosure of the present invention are within the scope of the present invention.

The invention provides a sparse point cloud densification and pavement removal method based on a depth map, which can perform densification and pavement removal on point cloud data on the premise of not combining other data, has the characteristics of high speed, better guarantee of effectiveness of point cloud data information and the like, and the whole algorithm design scheme flow is shown in figure 1 and comprises the following steps:

step 1: inputting original sparse point cloud C_pointsConverting point cloud coordinates by using an internal and external parameter matrix T obtained by sensor calibration, and extracting a sparse depth map DM_originTurning to the step 2;

step 2: for the sparse depth map DM extracted in the step 1_originPerforming Delaunay triangulation linear interpolation to obtain an initial rough dense depth map DDM_wildTurning to step 3;

and step 3: sparse depth map DM extracted by step 1_originGenerating a mask_detailedAnd applied to DDM_wildTo obtain a fine dense depth map DDm_detailedTurning to step 4;

and 4, step 4: for the original sparse point cloud C in step 1_pointsPerforming RANSAC ground fitting and removing the ground, then filtering outliers, and extracting a ground-free sparse depth map DM_ngTurning to step 5;

and 5: non-ground sparse depth map DM extracted by step 4_ngGenerating a mask_ngAnd applied to DDM_detailedTo obtain a surface-free fine dense depth map DM_ngTurning to step 6;

and 6: for the fine dense depth map DDM obtained in the step 3_detailedAnd step 5, obtaining a ground-free fine dense depth map DDM_ngAnd carrying out back projection by utilizing the internal and external parameter matrix T to obtain the dense point cloud DC_pointsAnd dense point cloud DC_{points_ng}。

In the above technical solution, the step 1 specifically includes the following steps:

step 1.1: inputting original sparse point cloud C_pointsThe data format is [ X, Y, Z]The sensor is calibrated to obtain an internal and external parameter matrix T, namely the space coordinate data of the point cloud;

step 1.2: utilizing internal and external parameter matrix T to sparse point cloud C_pointsCoordinate projection conversion is carried out to obtain C_imgThe data format is [ xX, yY, X]And the format is adjusted to obtain C'_imgThe data format is [ X, y, X]I.e. C_pointsAt coordinates X, y in image space and X coordinate (depth) in the spatial coordinate system, the transformation formula is as follows:

C_img＝T.C_points

step 1.3: to filter out C'_imgPoints in the image outside the boundary (h, w), pair C'_imgThe X and the y in the point cloud are rounded, then the point cloud is sorted from small to large according to the depth X, and the repeated points of the X and the y coordinates are removed to obtain the corrected sparse point cloud C ″_img；

Step 1.4: constructing an all-zero matrix I of size (h, w)₀Using the sparse point cloud C_imgData [ X, y, X ] of (2)]In I₀Is filled with the corresponding depth X to obtain a sparse depth map DM_origin；

In the above technical solution, the specific steps of step 2 are as follows:

step 2.1: sparse depth map DM using Delaunay triangulation_originIs triangulated to produce an isolated set of triangulated meshes, Δ ═ δ₁,…,δ_nEach triangle delta comprises three coordinates n_k(k＝1,2,3)；

Step 2.2: linear interpolation is carried out by using the depth X on three coordinates of the triangle delta to estimate the depth on the triangle inner point, and finally the rough dense depth map DDM is obtained_wild；

The linear interpolation comprises the following specific steps: setting point P as triangle delta P₁P₂P₃One point of (1), P₁、P₂、P₃Is known, the value of point P can be represented by the following equation 1.1:

p＝(1-u-v)P₁+uP₂+vP₃ (1.1)

associating the value with the coordinates of the point, P₁、P₂、P₃And the coordinates of P are knownThe following formula can be listed:

and solving parameters u and v, and substituting the parameters into (1.1) to obtain the value of the point P.

In the above technical solution, the step 3 specifically comprises the following steps:

step 3.1: constructing an all-zero matrix mask of size (h, w)₀；

Step 3.2: for the sparse depth map DM obtained in the step 1_originSearching the coordinate of the point with the first depth X not being 0 (the first non-zero point coordinate of each column) index u from top to bottom in each column, and matching the mask₀All the u-th element to the last element in each column are set to be 1 to obtain mask'₀；

Step 3.3: mask 'is a square structural element pair with the height of 2 and the width of 10'₀Performing morphological transverse expansion to obtain a mask_detailed；

Step 3.4: will mask_detailedAnd DDM_wildMultiplying to obtain a fine dense depth map DDM_detailed；

In the above technical solution, the step 4 specifically includes the following steps:

step 4.1: inputting original sparse point cloud C_pointsPerforming ground fitting by using RANSAC algorithm, and removing the ground to obtain C_{points_ng}(ii) a In this example, RANSAC ground fitting is calculated from functions in the PCL library, and the fitting parameter r is set to 0.1;

step 4.2: using Euclidean distance statistics C_{points_ng}The nearest 50 points around each point in the image are calculated

Calculate the 50 points relative to

Marking the points with the deviation larger than 0.1 as outliers to remove to obtain C_{points_ng_d}；

Step 4.3: to C_{points_ng_d}Generating a ground-free sparse depth map DM by adopting the same steps as the step 1_ng；

In the above technical solution, the step 5 specifically includes the following steps:

step 5.1: constructing an all-zero matrix mask of size (h, w)₀₁；

Step 5.2: for the non-ground sparse depth map DM obtained in the step 4_ngEach column searches the coordinate indexes u and v of the first point with the depth X not being 0 and the last non-zero point from top to bottom, and masks are used₀₁All the u-th element to the v-th element in each column are set to be 1 to obtain mask'₀₁；

Step 5.3: the mask is a pair of elliptical structural elements positioned in a rectangle with the height of 15 and the width of 2'₀₁Performing morphological vertical open operation to obtain mask_ng；

Step 5.4: will mask_ngAnd DDM_detailedMultiplying to obtain a non-ground fine dense depth map DDM_ng；

In the above technical solution, the reverse specific steps in the step 6 are as follows:

step 6.1: for the fine dense depth map DDM obtained in the step 3_detailedAnd step 5, obtaining a ground-free fine dense depth map DDM_ngRespectively finding out the non-zero coordinates and the depth X corresponding to the coordinates, and constructing in the format of [ X, y, X]The shape is nx3, and n is the number of non-zero points to obtain point cloud C;

step 6.2: and multiplying C by the inverse of the internal and external parameter matrix T to obtain the dense point cloud DC.

Claims (7)

1. A sparse point cloud densification and pavement removal method based on a depth map is characterized by comprising the following steps:

step 1: inputting original sparse point cloud C_pointsConverting point cloud coordinates by using an internal and external parameter matrix T obtained by sensor calibration, and extracting a sparse depth map DM_originTurning to the step 2;

step 2: for the sparse depth map DM extracted in the step 1_originPerforming Delaunay triangulation linear interpolation to obtain an initial rough dense depth map DDM_wildTurning to step 3;

and step 3: utilizing the sparse depth map DM extracted in the step 1_originGenerating a mask_detailedAnd applied to the initial coarse dense depth map DDM_wildTo obtain a fine dense depth map DDM_detailedTurning to step 4;

and 4, step 4: for the original sparse point cloud C in step 1_pointsPerforming RANSAC ground fitting and removing the ground, then filtering outliers, and extracting a ground-free sparse depth map DM_ngTurning to step 5;

and 5: non-ground sparse depth map DM extracted by step 4_ngGenerating a mask_ngAnd applied to a fine dense depth map DDM_detailedObtaining a surface-free fine dense depth map DDM_ngTurning to step 6;

step 6: for the fine dense depth map DDM obtained in the step 3_detailedAnd step 5, obtaining a ground-free fine dense depth map DDM_ngPerforming back projection by using the internal and external parameter matrix T to obtain dense point cloud DC_pointsAnd dense point cloud DC_{points_ng}。

2. The method for sparse point cloud densification and pavement removal based on the depth map of claim 1, wherein the method comprises the following steps: the step 1 comprises the following specific steps:

step 1.1: inputting original sparse point cloud C_pointsThe data format is [ X, Y, Z ]]The sensor is calibrated to obtain an internal and external parameter matrix T, namely the space coordinate data of the point cloud;

step 1.2: utilizing internal and external parameter matrix T to sparse point cloud C_pointsCoordinate projection conversion is carried out to obtain C_imgAnd is formatted to obtain C'_imgThe data format is [ X, y, X ]]I.e. C_pointsThe transformation formula is as follows at coordinates X, y in image space and X in the spatial coordinate system:

C_img＝T·C_points

step 1.3: c 'is filtered off'_imgPoints in the image outside the boundary (h, w), pair C'_imgThe X and the y in the point cloud are rounded, then the point cloud is sorted from small to large according to the depth X, and the repeated points of the X and the y coordinates are removed to obtain the corrected sparse point cloud C ″_img；

Step 1.4: constructing an all-zero matrix I of size (h, w)₀Using the corrected sparse point cloud C ″_imgData [ X, y, X ] of (2)]In I₀Is filled with the corresponding depth X to obtain a sparse depth map DM_origin。

3. The method for sparse point cloud densification and pavement removal based on the depth map of claim 1, wherein the method comprises the following steps: the specific steps of the step 2 are as follows:

step 2.1: sparse depth map DM using Delaunay triangulation_originIs triangulated to produce an isolated set of triangulated meshes, Δ ═ δ { δ₁,…,δ_nEach triangle delta comprises three coordinates n₁、n₂And n₃(ii) a Where n is₁、n₂And n₃In fact, the reference number of the coordinates of the depth map in the triangular mesh indicates that the interpolation operation is to be performed in the triangle formed by the three coordinates;

step 2.2: linear interpolation is carried out by using the depth X on three coordinates of the triangle delta to estimate the depth on the triangle inner point, and finally the rough dense depth map DDM is obtained_wild。

4. The method for sparse point cloud densification and pavement removal based on the depth map of claim 1, wherein the method comprises the following steps: the step 3 comprises the following specific steps:

step 3.1: construct an all-zero matrix mask of size (h, w)₀；

Step 3.2: for the sparse depth map DM obtained in the step 1_originEach column searches the coordinate index u of the point with the first depth X not being 0 from top to bottom, and masks₀Each one of which isAll the u-th element to the last element in the row are set to be 1 to obtain mask'₀；

Step 3.3: mask 'is paired with transverse square structural elements'₀Performing morphological transverse expansion to obtain a mask_detailed；

Step 3.4: will mask_detailedAnd DDM_wildMultiplying to obtain a fine dense depth map DDM_detailed。

5. The method for sparse point cloud densification and pavement removal based on the depth map of claim 2, wherein the method comprises the following steps: the step 4 comprises the following specific steps:

step 4.1: inputting original sparse point cloud C_pointsPerforming ground fitting by using RANSAC algorithm, and removing the ground to obtain a ground-free sparse point cloud C_{points_ng}；

And 4.2: non-ground sparse point cloud C adopting Euclidean distance statistics_{points_ng}M points around each point in the image and calculating the average distance between each point and its surrounding m points

Calculate the m points relative to

Marking the point with the deviation larger than the threshold value rho as an outlier to be removed to obtain C_{points_ng_d}；

Step 4.3: to C_{points_ng_d}Generating a ground-free sparse depth map DM by adopting the same steps as the step 1_ng。

6. The method for sparse point cloud densification and pavement removal based on the depth map of claim 1, wherein the method comprises the following steps: the step 5 specifically comprises the following steps:

step 5.1: construct an all-zero matrix mask of size (h, w)₀₁；

Step 5.2: for the non-ground sparse depth map DM obtained in the step 4_ngEach column searches the coordinate index uv of the first point with depth X not being 0 and the last non-zero point from top to bottom, and masks are processed₀₁All the u-th element to the v-th element in each column are set to be 1 to obtain mask'₀₁；

Step 5.3: mask 'is paired by oval structural elements'₀₁Performing morphological vertical open operation to obtain mask_ng；

Step 5.4: will mask_ngAnd DDM_detailedMultiplying to obtain a non-ground fine dense depth map DDM_ng。

7. The method for sparse point cloud densification and pavement removal based on the depth map of claim 1, wherein the method comprises the following steps: the back projection in the step 6 comprises the following specific steps:

step 6.1: for the fine dense depth map DDM obtained in the step 3_detailedAnd step 5, obtaining a ground-free fine dense depth map DDM_ngRespectively finding out the non-zero coordinates and the depth X corresponding to the coordinates, and constructing in the format of [ X, y, X]The shape is nx3, and n is the number of non-zero points to obtain point cloud C;

step 6.2: and multiplying C by the inverse of the internal and external parameter matrix T of the sensor to obtain the dense point cloud DC.

Images