PCL点云压薄

一、算法原理

• 1.概述
• 2.参考文献或代码

二、代码实现
三、结果展示

一、算法原理

1.概述

在对地面点云或者墙壁进行分类后需要对分类后的数据进行压薄。

2.参考文献或代码

https://github.com/csiro-robotics/raycloudtools

二、代码实现

main.cpp

int main()
{
	std::string inputCloud= "E:/test.las";
	std::string outputCloud= "E:/out.las";
	//读取点云文件
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(inputCloud, *cloud) == -1)//*打开点云文件
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd\n");
		return(-1);
	}
	pcl::KdTreeFLANN<pcl::PointXYZ> kdTree;
	kdTree.setInputCloud(cloud);
	const int K = 50;

	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;   
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	n.setNumberOfThreads(8);
	//n.setViewPoint(0,0,0);//设置视点，默认为（0，0，0）
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(K);//点云法向计算时，需要所搜的近邻点大小
	n.compute(*normals);//开始进行法向计算
	const int num_neighbours = 50;
	Eigen::MatrixXi neighbour_indices;

	std::vector<Eigen::Vector3d> centroids(cloud->points.size(), { 0,0,0 });
	std::vector<Eigen::Vector3d> eigen_normals;
	eigen_normals.resize(cloud->points.size());

	for (int n = 0; n < cloud->points.size(); ++n)
	{
		Eigen::Vector3d normall;
		normall.x() = normals->at(n).normal[0];
		normall.y() = normals->at(n).normal[1];
		normall.z() = normals->at(n).normal[2];
		eigen_normals[n] = normall;
	}

	int number = 0;
	for (size_t i = 0; i < cloud->points.size(); i++)
	{
		double total_weight = 0.2;  // more averaging if it uses less of the central position, but 0 risks a divide by 0

		Eigen::Vector3d curPoint1;
		curPoint1.x() = cloud->points[i].x;
		curPoint1.y() = cloud->points[i].y;
		curPoint1.z() = cloud->points[i].z;

		Eigen::Vector3d weighted_sum = curPoint1 * total_weight;

		std::vector<int> pointIdxNKNSearch(K);
		std::vector<float>pointNKNSquaredDistance(K);
		if (kdTree.nearestKSearch(cloud->points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance) > /*0*/40)
		{
			Eigen::Vector3d meanPoint = Eigen::Vector3d::Zero();

			for (int j = 0; j < num_neighbours; j++)
			{
				int k = pointIdxNKNSearch[j];
				meanPoint.x() += cloud->points[k].x;
				meanPoint.y() += cloud->points[k].y;
				meanPoint.z() += cloud->points[k].z;
			}
			centroids[i] = meanPoint / num_neighbours;
			//沿着中心点的方向进行移动
			curPoint1 += eigen_normals[i] * (centroids[i] - curPoint1).dot(eigen_normals[i]);
			//curPoint1 += eigen_normals[i] * (curPoint1 - centroids[i]).dot(eigen_normals[i]);  //将点云拉厚了。

			cloud->points[i].x = curPoint1.x();
			cloud->points[i].y = curPoint1.y();
			cloud->points[i].z = curPoint1.z();
		}
	}
	cout << number << std::endl;
	pcl::PCDWriter writer;
	writer.write(outputCloud, *cloud);
	return 0;
}

三、结果展示