PCL_点云分割_基于法线微分分割

一、概述

PCL_点云分割_基于法线微分分割_点云法向量微分-CSDN博客

利用不同的半径（大的半径、小半径）来计算同一个点的法向量差值P。判断P的范围，从而进行分割。

看图理解： 

 

二、计算流程 

1、计算P点小半径的法向量Ns

2、计算P点大半径的法向量Nl(P 点和1中的P点是同一个点)

3、计算deltN=(Nl-Ns)/2;

4、deltN 和输入的阈值做对比，判断

原始点云：

Code 

重点代码

// 计算法向量查来分割DON
	pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;
	don.setInputCloud(cloud);
	don.setNormalScaleLarge(normals_large_scale);
	don.setNormalScaleSmall(normals_small_scale);
	if (!don.initCompute())
	{
		exit(EXIT_FAILURE);
	}
	don.computeFeature(*doncloud);


	cout << "don.computeFeature(*doncloud);       " << doncloud->size() << "..." << endl;
	pcl::io::savePCDFileBinaryCompressed("DifferenceOfNormalsEstimation.pcd", *doncloud);






  // 生成滤波条件，将法线差和阈值对比
	pcl::ConditionOr<PointNormal>::Ptr range_cond(new pcl::ConditionOr<PointNormal>());
	range_cond->addComparison(pcl::FieldComparison<PointNormal>::ConstPtr(new pcl::FieldComparison<PointNormal>("curvature", pcl::ComparisonOps::GT, threshold)));

	// 统计滤波
	pcl::PointCloud<PointNormal>::Ptr doncloud_filtered(new pcl::PointCloud<PointNormal>);
	pcl::ConditionalRemoval<PointNormal> condrem;
	condrem.setCondition(range_cond);
	condrem.setInputCloud(doncloud);
	condrem.filter(*doncloud_filtered);
	doncloud = doncloud_filtered;

全部代码：

int CalcCloudMeanRadius(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int  k, double& MeanRadius)
{
	if (cloud->size() < 1)
	{
		cout << "   ==================== ========================== " << endl;
		return -1;
	}

	pcl::search::Search<pcl::PointXYZRGB>::Ptr tree;
	// 判断点云无序  还是有序
	if (cloud->isOrganized())
	{
		tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZRGB>());
	}
	else
	{
		tree.reset(new pcl::search::KdTree<pcl::PointXYZRGB>(false));
	}

	tree->setInputCloud(cloud);
	//计算点云的平均点半径
	{
		int size_cloud = cloud->size();
		int step = size_cloud / 100;
		double total_distance = 0;
		int i, j = 1;
		int K = k;
		for (i = 0; i < size_cloud; i += step, j++)
		{
			std::vector<int> pointIdxNKNSearch(K);
			std::vector<float> pointNKNSquaredDistance(K);
			double distance = 0;
			if (tree->nearestKSearch(cloud->points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
			{

				for (size_t m = 0; m < pointIdxNKNSearch.size(); ++m)
				{
					std::cout << j << " 号点 " << cloud->points[pointIdxNKNSearch[m]].x
						<< " " << cloud->points[pointIdxNKNSearch[m]].y
						<< " " << cloud->points[pointIdxNKNSearch[m]].z
						<< " (squared distance: " << pointNKNSquaredDistance[m] << ")" << std::endl;
					distance += pointNKNSquaredDistance[m];
				}
			}
			total_distance += (distance / (K - 1));
		}
		MeanRadius = sqrt((total_distance / j));
	}
}

int     main(int argc, char* argv[])
{

	double scale1 = 5;
	double mean_radius;
	double scale2 = 10;

	///The minimum DoN magnitude to threshold by
	double threshold = 0.22;

	///segment scene into clusters with given distance tolerance using euclidean clustering
	double segradius = 10;
	pcl::PointCloud<PointXYZRGB>::Ptr cloud(new pcl::PointCloud<PointXYZRGB>);

	pcl::io::loadPCDFile("D:\\work\\Pointclouds\\clouds\\trees.pcd", *cloud);
	CalcCloudMeanRadius(cloud, 20, mean_radius);

	cerr << "平均半径     :   " << mean_radius  << endl;
	scale1 *= mean_radius;
	scale2 *= mean_radius;
	segradius *= mean_radius;

	if (scale1 >= scale2)
	{
		cerr << "Error: Large scale must be > small scale!" << endl;
		exit(EXIT_FAILURE);
	}

	// 计算法向量
	pcl::search::Search<pcl::PointXYZRGB>::Ptr tree;
	pcl::PointCloud<PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<PointNormal>);
	pcl::PointCloud<PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<PointNormal>);
	pcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne;
	ne.setInputCloud(cloud);
	ne.setSearchMethod(tree);
	ne.setRadiusSearch(scale1);
	ne.compute(*normals_small_scale);
	ne.setRadiusSearch(scale2);
	ne.compute(*normals_large_scale);

	
	PointCloud<PointNormal>::Ptr doncloud(new pcl::PointCloud<PointNormal>);
	copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud);

	// 计算法向量查来分割DON
	pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;
	don.setInputCloud(cloud);
	don.setNormalScaleLarge(normals_large_scale);
	don.setNormalScaleSmall(normals_small_scale);
	if (!don.initCompute())
	{
		exit(EXIT_FAILURE);
	}
	don.computeFeature(*doncloud);


	cout << "don.computeFeature(*doncloud);       " << doncloud->size() << "..." << endl;
	pcl::io::savePCDFileBinaryCompressed("DifferenceOfNormalsEstimation.pcd", *doncloud);

  // 生成滤波条件，将法线差和阈值对比
	pcl::ConditionOr<PointNormal>::Ptr range_cond(new pcl::ConditionOr<PointNormal>());
	range_cond->addComparison(pcl::FieldComparison<PointNormal>::ConstPtr(new pcl::FieldComparison<PointNormal>("curvature", pcl::ComparisonOps::GT, threshold)));

	// 统计滤波
	pcl::PointCloud<PointNormal>::Ptr doncloud_filtered(new pcl::PointCloud<PointNormal>);
	pcl::ConditionalRemoval<PointNormal> condrem;
	condrem.setCondition(range_cond);
	condrem.setInputCloud(doncloud);
	condrem.filter(*doncloud_filtered);
	doncloud = doncloud_filtered;

	// Save filtered output
	std::cout << "Filtered Pointcloud: " << doncloud->points.size() << " data points." << std::endl;
	pcl::io::savePCDFileBinaryCompressed("don_filtered.pcd", *doncloud);

	

	// 欧氏距离提取
	cout << "Clustering using EuclideanClusterExtraction with tolerance <= " << segradius << "..." << endl;
	pcl::EuclideanClusterExtraction<PointNormal> ec;
	pcl::search::KdTree<PointNormal>::Ptr segtree(new pcl::search::KdTree<PointNormal>);
	segtree->setInputCloud(doncloud);
	std::vector<pcl::PointIndices> cluster_indices;
	ec.setClusterTolerance(segradius);
	ec.setMinClusterSize(50);
	ec.setMaxClusterSize(100000);
	ec.setSearchMethod(segtree);
	ec.setInputCloud(doncloud);
	ec.extract(cluster_indices);


	  cout << "个数：   " << cluster_indices.size()<<endl;

		int j = 0;
		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it, j++)
		{
			pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don(new pcl::PointCloud<PointNormal>);
			for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
			{
				cloud_cluster_don->points.push_back(doncloud->points[*pit]);
			}
			cloud_cluster_don->width = int(cloud_cluster_don->points.size());
			cloud_cluster_don->height = 1;
			cloud_cluster_don->is_dense = true;

			//Save cluster
			cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size() << " data points." << std::endl;
			stringstream ss;
			ss << "don_cluster_" << j << ".pcd";
			pcl::io::savePCDFileBinaryCompressed(ss.str(), *cloud_cluster_don);
		}


}