点云引导滤波

最新推荐文章于 2024-09-05 00:00:00 发布

给算法爸爸上香

最新推荐文章于 2024-09-05 00:00:00 发布

阅读量215

点赞数

分类专栏： PCL 文章标签：点云 PCL C++

本文链接：https://blog.csdn.net/taifyang/article/details/129466842

版权

PCL 专栏收录该内容

26 篇文章 69 订阅

订阅专栏

在这里插入图片描述

/**
 * @description:			最远点采样
 * @param cloud				输入点云
 * @param cloud_filtered	滤波点云
 * @param k					k邻近搜索点数
 * @param epsilon			惩罚值
 */
void guidedfilter(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, int k, float epsilon)
{
	std::vector<int> indices;
	std::vector<float> dist;
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud);
	for (size_t i = 0; i < cloud->size(); i++)
	{
		if (kdtree.nearestKSearch(cloud->points[i], k, indices, dist) > 0)
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cloud, indices, *neighbors);

			pcl::PointXYZ mean(0.0, 0.0, 0.0);
			float mean2 = 0.0;
			for (size_t j = 0; j < neighbors->size(); j++)
			{
				mean.x += neighbors->points[j].x;
				mean.y += neighbors->points[j].y;
				mean.z += neighbors->points[j].z;
				mean2 += pow(neighbors->points[j].x, 2) + pow(neighbors->points[j].y, 2) + pow(neighbors->points[j].z, 2);

			}
			mean.x /= neighbors->size();
			mean.y /= neighbors->size();
			mean.z /= neighbors->size();
			mean2 /= neighbors->size();

			float mean_2 = pow(mean.x, 2) + pow(mean.y, 2) + pow(mean.z, 2);
			float a = (mean2 - mean_2) / (mean2 - mean_2 + epsilon);
			pcl::PointXYZ b((1.0 - a) * mean.x, (1.0 - a) * mean.y, (1.0 - a) * mean.z);
			pcl::PointXYZ filtered_point(a * cloud->points[i].x + b.x, a * cloud->points[i].y + b.y, a * cloud->points[i].z+ b.z);
			cloud_filtered->push_back(filtered_point);
		}
	}
}