点云随笔（一）单个格网滤波

最新推荐文章于 2024-08-14 11:22:43 发布

North ship

最新推荐文章于 2024-08-14 11:22:43 发布

阅读量306

点赞数 4

文章标签：算法

本文链接：https://blog.csdn.net/qq_45861949/article/details/140077585

版权

单个格网滤波主要跟格网滤波原理基本一致，主要是将格网滤波的八邻域坡度平均值作为中心格网坡度替换为了基于当前检索格网最低点与其他检索点计算坡度。


//int grid_size{ 5 };
//pcl::PCDReader reader;
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
//cloud2(new pcl::PointCloud<pcl::PointXYZRGB>), cloud3(new pcl::PointCloud<pcl::PointXYZRGB>);
//reader.read("D:\\c++\\dumb_ridge.pcd", *cloud);
//std::cout << "The cloud size is: " << cloud->points.size() << " data points." << std::endl;
//pcl::PointXYZRGB minPt, maxPt;
//pcl::getMinMax3D(*cloud, minPt, maxPt);
//
//int row = ((maxPt.y - minPt.y + 4 * grid_size) / grid_size);
//int col = ((maxPt.x - minPt.x + 4 * grid_size) / grid_size);
//int grid_count = { row * col };
//
//multimap<int, int>hashmap;
//for (int i = 0; i < cloud->size(); i++)
//{
//	int row_number = int((cloud->points[i].y - minPt.y) / grid_size) + 1;
//	int col_number = int((cloud->points[i].x - minPt.x) / grid_size) + 1;
//	int rank = (row_number - 1) * col + col_number;
//	hashmap.insert(make_pair(rank, i));
//}
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr non_ground(new pcl::PointCloud<pcl::PointXYZRGB>), ground(new pcl::PointCloud<pcl::PointXYZRGB>);
//int non{ 0 }, yes{ 0 };
//for (int i = 0; i < grid_count; i++)
//{
//	pcl::PointCloud<pcl::PointXYZRGB>::Ptr grid_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//	int k{ 0 };
//	multimap<int, int>::iterator m;
//	m = hashmap.find(i);
//	for (k = 0; k != hashmap.count(i); k++, m++)
//	{
//		grid_cloud->push_back(cloud->points[m->second]);
//	}
//	if (grid_cloud->size() > 0) {
//
//		pcl::PointXYZRGB grid_min, grid_max;
//		pcl::getMinMax3D(*grid_cloud, grid_min, grid_max);
//		for (int c = 0; c < grid_cloud->size(); c++) {
//			double slope = (grid_cloud->points[c].z - grid_min.z) /
//				sqrt(pow((grid_cloud->points[c].x - grid_min.x), 2)) + pow((grid_cloud->points[c].y - grid_min.y), 2);
//			if (slope < 1) {
//				ground->push_back(grid_cloud->points[c]);
//				yes += 1;
//			}
//			else {
//				non_ground->push_back(grid_cloud->points[c]);
//				non += 1;
//			}
//		}
//	}
//	else
//	{
//		continue;
//	}
//}
//cout << "地面点有: " << ground->size() << endl;
//cout << "非地面点有: " << non_ground->size() << endl;
//ground->width = yes;
//ground->height = 1;
//ground->points.resize(ground->width * ground->height);
//pcl::io::savePCDFileASCII("D:\\c++\\ground.pcd", *ground);
//
//non_ground->width = non;
//non_ground->height = 1;
//non_ground->points.resize(non_ground->width * non_ground->height);
//pcl::io::savePCDFileASCII("D:\\c++\\non_ground.pcd", *non_ground);