点云地面提取/分割

Posted on Edited on

点云地面分割

主要包含的程序

Extract_ground_pts()会调用Get_grid()与Seg_ground_nground_pts()
Get_grid()
Seg_ground_nground_pts()

代码解释

Extract_ground_pts

源码说明

这段代码实现了一个地面点提取的流程。首先，它计算了体素网格的参数，然后用输入点云数据填充了体素网格，并根据地面点的特性将点云分割成地面点和非地面点，最后将它们分别存储在不同的点云容器中。这有助于进一步的地面分析和处理。

源码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
void Ground_Extraction::Extract_ground_pts(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
	                                       pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud,
	                                       pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud,
	                                       Bounds bounds, CenterPoint center_pt)
{
	int row, list, num_voxel; 
	row = ceil((bounds.max_y - bounds.min_y) / grid_res_);
	list = ceil((bounds.max_x - bounds.min_x) / grid_res_);
	num_voxel = row*list;

	Voxel* grid = new Voxel[num_voxel];
	for (int i = 0; i < num_voxel; i++)
	{
		grid[i].min_z = FLT_MAX;
	}
	Get_grid(cloud, bounds.max_x, bounds.max_y, bounds.min_x, bounds.min_y, row, list, num_voxel, grid, no_ground_cloud);
	Seg_ground_nground_pts(cloud, ground_clo`ud, no_ground_cloud, grid, num_voxel);
	delete[]grid;
}

源码解释

  1. 计算用于体素网格的行数 row 和列数 list，以及体素的总数 num_voxel。这些参数是根据地图区域的边界信息和体素的分辨率 grid_res_ 计算得到的。
  2. 创建一个名为 grid 的动态数组，用于存储体素网格的信息。每个体素包含了最小高度信息，用于后续的地面点提取。
  3. 初始化 grid 数组中的每个体素的 min_z 值为 FLT_MAX，这是为了确保在后续的处理中可以正确更新最小高度。
  4. 调用 Get_grid 函数，将输入点云数据 cloud 以及计算得到的行数、列数、体素数量等信息传递给它。Get_grid 函数的目的是根据输入的点云数据，填充并更新体素网格 grid，并将非地面点云存储在 no_ground_cloud 中
  5. 调用 Seg_ground_nground_pts 函数，将输入的点云数据 cloud、地面点云容器 ground_cloud、非地面点云容器 no_ground_cloud，以及填充好的体素网格 grid 和体素数量传递给它。Seg_ground_nground_pts 函数的作用是根据地面点的特性将点云数据分为地面点和非地面点，并将它们分别存储在 ground_cloud 和 no_ground_cloud 中。
  6. 最后，释放动态分配的 grid 数组的内存。

参数说明

  • Bounds bounds:包含地图区域的边界信息，包括 max_x、max_y、min_x 和 min_y，用于定义提取地面点的地图区域。
  • pcl::PointCloudpcl::PointXYZI::Ptr cloud:输入的点云数据。
  • pcl::PointCloudpcl::PointXYZI::Ptr ground_cloud:用于存储地面点的点云容器。
  • pcl::PointCloudpcl::PointXYZI::Ptr no_ground_cloud:用于存储非地面点的点云容器。
  • CenterPoint center_pt:地图中的中心点信息。

Get_grid

源码说明

这段代码的目的是将输入的点云分为地面点和非地面点，并将非地面点保存在 no_ground_cloud 中，同时更新地面点所在的体素网格的信息。
地面点的高度通过 non_ground_z_min 来进行判断，而体素网格用于进一步的地面点云处理和分析。
这是地面提取算法中的一部分。

源码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
void  Ground_Extraction::Get_grid(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
	double max_x, double max_y, double min_x, double min_y,
	int row, int list, int num_voxel, Voxel* grid, pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud)
{
	int temp_num_voxel;
	int ExpandGrid_List, ExpandGrid_Row;
	ExpandGrid_List = list + 2;
	ExpandGrid_Row = row + 2;
	temp_num_voxel = ExpandGrid_Row*ExpandGrid_List;
	Voxel *temp_grid = new Voxel[temp_num_voxel];

	float non_ground_z_min = FLT_MAX;

	for (int i = 0; i < cloud->points.size(); i++) //判断每一个点
	{
		if (cloud->points[i].z > non_ground_z_min) //如果满足这个条件，那就是非地面点云
		{
			no_ground_cloud->points.push_back(cloud->points[i]);
			continue;
		}
        
        // 进一步判断是否为非地面点云
        // 判断当前点在体素的哪个位置
		int temp_row, temp_list, temp_num;
		temp_list = floor((cloud->points[i].x - min_x) / grid_res_);
		temp_row = floor((cloud->points[i].y - min_y) / grid_res_);
		temp_num = temp_row*list + temp_list;
		if (temp_num >= 0 && temp_num < num_voxel)
		{
			grid[temp_num].point_id.push_back(i);
			grid[temp_num].PointsNumber++;
			if (cloud->points[i].z < grid[temp_num].min_z)
			{
				grid[temp_num].min_z = cloud->points[i].z;
				grid[temp_num].NeighborMin_z = cloud->points[i].z;
			}
		}
	}
	delete[] temp_grid;
}

源码解释

  1. 定义了一些局部变量，如 temp_num_voxel、ExpandGrid_List 和 ExpandGrid_Row 用于存储扩展后的体素网格的维度。
  2. 创建了一个临时的体素网格 temp_grid，大小为 temp_num_voxel，用于存储点云中的数据。
  3. 定义了一个 non_ground_z_min 变量，并将其初始化为 FLT_MAX，该变量用于存储非地面点云中的最小高度。
  4. 开始迭代处理输入的点云中的每个点，通过对每个点进行条件判断来区分地面点和非地面点。
  5. 如果点的高度（z坐标）大于 non_ground_z_min，则被视为非地面点，并将其添加到 no_ground_cloud 中。
  6. 如果点的高度小于等于 non_ground_z_min，则进一步判断该点位于哪个体素格子中。通过计算 temp_list 和 temp_row 来确定该点在体素网格中的位置，然后根据这个位置更新 grid 数组中对应格子的信息。
  7. 在处理完所有点云后，释放 temp_grid 的内存，函数执行完毕。

参数说明

  • pcl::PointCloudpcl::PointXYZI::Ptr cloud
  • double max_x：点云在x方向上的边界上限
  • double max_y：点云在y方向上的边界上限
  • double min_x：点云在x方向上的边界下限
  • double min_y：点云在y方向上的边界下限
  • int row：将点云划分为多少行
  • int list：将点云划分为多少列
  • int num_voxel：体素的个数
  • Voxel* grid：体素的容器
  • pcl::PointCloudpcl::PointXYZI::Ptr no_ground_cloud：非地面点的指针

Seg_ground_nground_pts

源码说明

这段代码用于基于体素网格和高度差值将输入的点云数据分割为地面点和非地面点，以进行后续的地面特征提取或其他地面相关的任务。

源码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void Ground_Extraction::Seg_ground_nground_pts(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
	pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud,
	pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud,
	Voxel* grid, int num_voxel)
{
	int n = 0, m = 0;
	for (int i = 0; i < num_voxel; i++)
	{
		for (int j = 0; j < grid[i].point_id.size(); j++)
		{
			if (cloud->points[grid[i].point_id[j]].z - grid[i].min_z < max_height_diff_
				&&grid[i].min_z - grid[i].NeighborMin_z < max_nei_height_diff_)
				ground_cloud->points.push_back(cloud->points[grid[i].point_id[j]]);
			else
				no_ground_cloud->points.push_back(cloud->points[grid[i].point_id[j]]);
		}
	}
}

源码解释

函数执行的操作如下：

  1. 初始化 n 和 m 为0，它们用于计数地面点和非地面点的数量。
  2. 通过嵌套循环遍历每个体素格子（通过 num_voxel 控制循环次数），然后再遍历每个格子中的点。
  3. 对于每个格子中的点，检查点的高度（z坐标）与该格子中的最小高度 grid[i].min_z 之间的差值是否小于 max_height_diff_，以及该格子中的最小高度 grid[i].min_z 与相邻格子中的最小高度 grid[i].NeighborMin_z 之间的差值是否小于 max_nei_height_diff_。
  4. 如果满足上述两个条件，将该点视为地面点，并将其添加到 ground_cloud 中。否则，将其视为非地面点，并将其添加到 no_ground_cloud 中。
    5 .在循环结束后，ground_cloud 中将包含所有被认定为地面点的点，而 no_ground_cloud 中将包含所有被认定为非地面点的点。函数执行完毕后，你可以分别使用 ground_cloud 和 no_ground_cloud 进行进一步的处理或分析。

参数说明

  • pcl::PointCloudpcl::PointXYZI::Ptr cloud:输入的点云数据
  • pcl::PointCloudpcl::PointXYZI::Ptr ground_cloud:存储地面点的点云容器
  • pcl::PointCloudpcl::PointXYZI::Ptr no_ground_cloud:存储非地面点的点云容器
  • Voxel* grid:体素网格，包含点云的体素化信息。
  • int num_voxel:体素网格的总数

以下为点云分割源码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
#ifndef EXTRACTGROUND_H
#define EXTRACTGROUND_H
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/ModelCoefficients.h>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <pcl/visualization/cloud_viewer.h>

#include "utility.h"

//Reference: Two-step adaptive extraction method for ground points and breaklines from lidar point clouds, Bisheng Yang, Ronggang Huang, et al. ISPRS Journal of Photogrammetry and Remote Sensing
namespace roadmarking
{  
	class Ground_Extraction
	{
	public:
		Ground_Extraction();
		void Extract_ground_pts(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
			pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud,
			pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud,
			Bounds bounds, CenterPoint center_pt);//提取地面点函数		
	protected:
	private:
		void Get_grid(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
			double max_x, double max_y, double min_x, double min_y,
			int row, int list, int num_voxel, Voxel* grid, pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud);
			
		void Seg_ground_nground_pts(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
			pcl::PointCloud<pcl::PointXYZI>::Ptr ground_cloud,
			pcl::PointCloud<pcl::PointXYZI>::Ptr no_ground_cloud,
			Voxel* grid, int num_voxel);
		float grid_res_;
		int   min_pt_num_grid_;
		float max_height_diff_;
		float max_nei_height_diff_; 
	};
}
#endif

其他点云数据处理

对点云数据的处理主要包含

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
#ifndef SEGMENTATION_H
#define SEGMENTATION_H

#include "utility.h"

using namespace std;
namespace roadmarking
{
	class Csegmentation
	{
	public:
		Csegmentation(float res){ resolution = res; };
		void GroundFilter_PMF(const pcXYZIPtr &cloud, pcXYZIPtr &gcloud, pcXYZIPtr &ngcloud);   // PMF 
		void GroundFilter_PMF(const pcXYZIPtr &cloud, pcXYZIPtr &gcloud, pcXYZIPtr &ngcloud, int max_window_size, float slope, float initial_distance, float max_distance);   // PMF �������˲� ��������;
		pcXYZIPtr planesegRansac(const pcXYZIPtr &cloud, float threshold);   //Ransac 
		pcXYZIPtr groundprojection(const pcXYZIPtr &cloud);                 
		void cloudFilter(const vector<pcXYZI> &inclouds, vector<pcXYZI> &outclouds, int N, int MeanK, double std);  //Otsu Intensity Thresholding + SOR Filter 
		void SORFilter(const pcXYZI &inclouds, pcXYZI &outclouds, int MeanK, double std); //SOR (Statisics Outliers Remover)  
		void NFilter(const vector<pcXYZI> &inclouds, vector<pcXYZI> &outclouds, int K);
		void BoundingInformation(const vector<pcXYZI> &clouds, vector<vector<pcl::PointXYZI>> & boundingdatas); //bounding 4 points 
		void BoundingFeatureCalculation(const vector<vector<pcl::PointXYZI>> & boundingdatas, vector<BoundingFeature> & boundingfeatures); //bounding 4 points
		void BoundaryExtraction(const vector<pcXYZI> &clouds, vector<pcXYZI> &boundaryclouds, int down_rate=1, float alpha_value_scale = 0.8);                 
		void CornerExtraction(const vector<pcXYZI> &boundaryclouds, vector<pcXYZI> &cornerclouds, bool UseRadius, int K, float radius, float dis_threshold, float maxcos);
		void CategoryJudgementBox_highway(const vector<BoundingFeature> & boundingfeatures, RoadMarkings & roadmarkings);
		void CategoryJudgementBox_cityroad(const vector<BoundingFeature> & boundingfeatures, RoadMarkings & roadmarkings);
		void CombineSideLines(const RoadMarkings & roadmarkings, double combine_length, RoadMarkings & combine_sideline_markings); 
		void Find_head2tail(int head_index, const vector<vector<double>> & d_head_tail, vector<bool> & line_used, vector<int> & combineline, double combine_length);   
		void Find_tail2head(int tail_index, const vector<vector<double>> & d_tail_head, vector<bool> & line_used, vector<int> & combineline, double combine_length);   		
		void MarkingVectorization_highway(const vector<pcXYZI> &clouds, const vector<vector<pcl::PointXYZI>> &boundingdatas,RoadMarkings & roadmarkings,  double line_sample_dl, double ambiguousRatio);  
		void MarkingVectorization_cityroad(const vector<pcXYZI> &clouds, const vector<vector<pcl::PointXYZI>> &boundingdatas, RoadMarkings & roadmarkings, double line_sample_dl, double ambiguousRatio);  
		void GetRoadmarkingsForVect(RoadMarkings & roadmarkings, RoadMarkings & roadmarkings_sideline, RoadMarkings & roadmarkings_vect);
		pcXYZI alphashape(const pcXYZI &cloud, float alpha_value);   //Concave Hull Generation		
		pcXYZI CornerpointKNN(const pcXYZI &boundarycloud, int K, float disthreshold, float maxcos);                   //KNN corner point extraction 
		pcXYZI CornerpointRadius(const pcXYZI &boundarycloud, float radius, float disthreshold, float maxcos);         //Radius corner point extraction 

		//pcXYZI CornerClusteringKMeans(const pcXYZI &cornercloud, int K);

		
	protected:
	
	private:
		float resolution;
		
	};
}
#endif