VoxelGrid体素滤波实现

原理

实现

/**
 * @description:			体素滤波
 * @param cloud				输入点云
 * @param cloud_filtered	滤波点云
 * @param leafsize			体素大小
 */
void voxelgrid(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, float leafsize)
{
	pcl::PointXYZ minPt, maxPt;
	pcl::getMinMax3D(*cloud, minPt, maxPt);
	float lx = maxPt.x - minPt.x;
	float ly = maxPt.y - minPt.y;
	float lz = maxPt.z - minPt.z;
	int nx = lx / leafsize + 1;
	int ny = ly / leafsize + 1;
	int nz = lz / leafsize + 1;

    std::vector<std::pair<int,int>> v;
	for (size_t i = 0; i < cloud->points.size(); i++)
	{
		int ix = (cloud->points[i].x - minPt.x) / leafsize;
		int iy = (cloud->points[i].y - minPt.y) / leafsize;
		int iz = (cloud->points[i].z - minPt.z) / leafsize;
		v.push_back(std::pair<int, int>{ix + iy*nx + iz*nx*ny, i});
	}

    std::sort(v.begin(), v.end(), [](std::pair<int, int> p1, std::pair<int, int>p2) {return p1.first < p2.first; });

	int start = 0;
    std::vector<int> point_id;
	pcl::PointCloud<pcl::PointXYZ>::Ptr ptcloud;
	Eigen::Vector4f centroid;
	for (int i = start; i < v.size() - 1; ++i)
	{
		if (v[i].first != v[i + 1].first)
		{
			for (int id = start; id <= i; ++id)	point_id.push_back(v[i].second);
			ptcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cloud, point_id, *ptcloud);
			pcl::compute3DCentroid(*ptcloud, centroid);
			cloud_filtered->push_back(pcl::PointXYZ(centroid[0], centroid[1], centroid[2]));
			start = i + 1;
			point_id.clear();
		}
		else if (v[i].first == v[v.size() - 1].first)
		{
			point_id.push_back(v[i].second);
		}
	}
	ptcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(*cloud, point_id, *ptcloud);
	pcl::compute3DCentroid(*ptcloud, centroid);
	cloud_filtered->push_back(pcl::PointXYZ(centroid[0], centroid[1], centroid[2]));
}

今天看了下之前写的代码，什么玩意真的那么复杂，明明不到30行就能实现：

/**
 * @description:			体素滤波
 * @param cloud				输入点云
 * @param cloud_filtered	滤波点云
 * @param leafsize			体素大小
 */
void voxelgrid(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, float leafsize)
{
	pcl::PointXYZ minPt, maxPt;
	pcl::getMinMax3D(*cloud, minPt, maxPt);
	float lx = maxPt.x - minPt.x;
	float ly = maxPt.y - minPt.y;
	float lz = maxPt.z - minPt.z;
	int nx = lx / leafsize + 1;
	int ny = ly / leafsize + 1;
	int nz = lz / leafsize + 1;

	std::vector<pcl::PointCloud<pcl::PointXYZ>> v(nx * ny * nz);
	for (size_t i = 0; i < cloud->points.size(); i++)
	{
		int ix = (cloud->points[i].x - minPt.x) / leafsize;
		int iy = (cloud->points[i].y - minPt.y) / leafsize;
		int iz = (cloud->points[i].z - minPt.z) / leafsize;
		v[ix + iy * nx + iz * nx * ny].push_back(cloud->points[i]);
	}

	for (int i = 0; i < v.size(); ++i)
	{
		if (v[i].size())
		{
			Eigen::Vector4f centroid;
			pcl::compute3DCentroid(v[i], centroid);
			cloud_filtered->push_back(pcl::PointXYZ(centroid.x(), centroid.y(), centroid.z()));
		}
	}
}

代码传送门：https://github.com/taifyang/PCL-algorithm