虚拟网格索引点云的坡度滤波

虚拟网格索引点云的坡度滤波

虚拟网格的建立

虚拟网格将点云区域分成width×height个大小相等的虚拟网格，使每个网格都包含离散的点，并对每个激光脚点建立索引。下图为二维平面图。
width和height的值是根据激光脚点的x、y值，以及网格的边长S来确定的，公式如下：
width=(xmax-xmin)/S+1
height=(ymax-ymin)/S+1

int width = (int)((Xmax - Xmin) / S) + 1;
int height = (int)((Ymax - Ymin) /S) + 1;

建立索引的具体计算方法如下所示：
col=INT(x-xmin)/S
row=INT(y-ymin)/S

for (int i = 0; i < data.size(); i++)
	{
		int col = (int)((data[i].x - Xmin) / S);
		int row = (int)((data[i].y - Ymin) / S);
		}

遍历所有激光脚点，寻找网格内高程最低的种子点：

    double *XPointGrid = new double[width*height];//动态申请内存
	double *YPointGrid = new double[width*height];//动态申请内存
	double *ZPointGrid = new double[width*height];//动态申请内存
	
	for (int i = 0; i < width*height; i++)
		ZPointGrid[i] = 0;
	for (int i = 0; i < data.size(); i++)
	{
		int col = (int)((data[i].x - Xmin) / grid);
		int row = (int)((data[i].y - Ymin) / grid);
		ZPointGrid[row*width + col] = Zmax;                 
	}

	for (int i = 0; i < data.size(); i++)//遍历每一个点
	{
		int col = (int)((data[i].x - Xmin) / grid);
		int row = (int)((data[i].y - Ymin) / grid);
		if (data[i].z < ZPointGrid[row*width + col])    
		{
			ZPointGrid[row*width + col] = data[i].z;
			XPointGrid[row*width + col] = data[i].x;
			YPointGrid[row*width + col] = data[i].y;
		}
	}

坡度滤波算法

滤波算法如下：

for (int i = 0; i < data.size(); i++)
	{
		int col = (int)((data[i].x - Xmin) / grid);
		int row = (int)((data[i].y - Ymin) / grid);
		double L = sqrt((data[i].x - XPointGrid[row*width + col])*(data[i].x - XPointGrid[row*width + col]) + (data[i].y - YPointGrid[row*width + col])*(data[i].y - YPointGrid[row*width + col]));
		double dis = (data[i].z - ZPointGrid[row*width + col]);
		double threshold = (dis / L);
		if ((threshold <= value)) 
		{
			cout<< data[i].x << " ," << data[i].y << " ," << data[i].z << endl;
		}
	}