基于PCL实现点云双边滤波

实习生小白

已于 2022-01-18 17:34:37 修改

阅读量877

点赞数

文章标签：计算机视觉自动驾驶图像处理

于 2021-12-14 21:50:36 首次发布

本文链接：https://blog.csdn.net/qq_40690772/article/details/121940833

版权

#基于PCL实现点云双边滤波
##问题描述：
**目前pcl中实现双边滤波需要PointXYZI信息，而我们想要是实现基于位置信息和法线信息的双边滤波。
由于目前工作只需要对法线进行双边滤波，因此一半的工程（未完待续。。。）
在这里插入图片描述

// PCL实现双边滤波
void bilateral_filter(pcl::PointCloud<PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr maxcurvature, pcl::PointCloud<pcl::Normal>::Ptr maxcurvature2, pcl::PointCloud<PointXYZ>::Ptr filter_cloud,int smoth_K,double showlength) {
	pcl::KdTreeFLANN<pcl::PointXYZ> smoth_kdtree;
	//int smoth_K = 130;
	vector<int>smoth_pointIdxNKNSearch(smoth_K);		
	vector<float>smoth_pointNKNSquaredDistance(smoth_K);
	smoth_kdtree.setInputCloud(cloud);
	for (int i = 0; i < cloud->points.size(); i++) {
		if (smoth_kdtree.nearestKSearch(cloud->points[i], smoth_K, smoth_pointIdxNKNSearch, smoth_pointNKNSquaredDistance) > 0) {
			double fenzixx = 0;
			double fenziyy = 0;
			double fenzizz = 0;
			double fenmuu = 0;	
			//cout << showlength* maxcurvature->points[i].normal_x << "\t" << showlength* maxcurvature->points[i].normal_y << "\t" << showlength* maxcurvature->points[i].normal_z << endl;
			for (int j = 1; j < smoth_pointIdxNKNSearch.size(); j++) {
				if (maxcurvature->points[smoth_pointIdxNKNSearch[j]].normal_x != 0 && maxcurvature->points[smoth_pointIdxNKNSearch[j]].normal_y != 0 && maxcurvature->points[smoth_pointIdxNKNSearch[j]].normal_z != 0) {
					PointNormal pi;
					PointNormal pii;
					pi.x = cloud->points[i].x;
					pi.y = cloud->points[i].y;
					pi.z = cloud->points[i].z;
					pi.normal_x = showlength * maxcurvature->points[i].normal_x;
					pi.normal_y = showlength * maxcurvature->points[i].normal_y;
					pi.normal_z = showlength * maxcurvature->points[i].normal_z;
					pii.x = cloud->points[smoth_pointIdxNKNSearch[j]].x;
					pii.y = cloud->points[smoth_pointIdxNKNSearch[j]].y;
					pii.z = cloud->points[smoth_pointIdxNKNSearch[j]].z;
					pi.normal_x = showlength * maxcurvature->points[smoth_pointIdxNKNSearch[j]].normal_x;
					pi.normal_y = showlength * maxcurvature->points[smoth_pointIdxNKNSearch[j]].normal_y;
					pi.normal_z = showlength * maxcurvature->points[smoth_pointIdxNKNSearch[j]].normal_z;
					double xshu = compute_space(smoth_pointNKNSquaredDistance[j], smoth_K)*compute_normal(pi, pii);
					fenzixx += xshu * pi.normal_x;
					fenziyy += xshu * pi.normal_y;
					fenzizz += xshu * pi.normal_z;
					fenmuu += xshu;
				}
			}
			if (fenmuu != 0) {
				PointXYZ maxp;
				maxp.x = fenzixx / fenmuu + cloud->points[i].x;
				maxp.y = fenziyy / fenmuu + cloud->points[i].y;
				maxp.z = fenzizz / fenmuu + cloud->points[i].z;
				filter_cloud->push_back(maxp);
				Normal maxn;
				maxn.normal_x = fenzixx / fenmuu;
				maxn.normal_y = fenziyy / fenmuu;
				maxn.normal_z = fenzizz / fenmuu;
				maxcurvature2->push_back(maxn);
			}
			else {
				PointXYZ maxp;
				maxp.x = 0 + cloud->points[i].x;
				maxp.y = 0 + cloud->points[i].y;
				maxp.z = 0 + cloud->points[i].z;
				filter_cloud->push_back(maxp);
				Normal maxn;
				maxn.normal_x = 0;
				maxn.normal_y = 0;
				maxn.normal_z = 0;
				maxcurvature2->push_back(maxn);
			}
		}
	}
}