点云体素下采样 ❤️(体素质心 | 体素中心)

本文详细介绍了点云下采样的两种方法:体素质心下采样和体素中心下采样。通过计算非空体素的质心或中心来实现点云的下采样。体素质心方法虽然耗时较长,但结果更准确;而体素中心方法则更快,但可能导致失真。文章提供了代码实现,并对比了PCL的VoxelGrid下采样结果。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

1 体素质心下采样

1.1 原理

体素质心下采样的原理很简单,首先将点云进行 体素划分,然后计算非空体素的质心代替该体素内的所有点,实现点云的下采样。

1.2 体素质心的计算

已知一个非空体素 V V V,其中包含 m m m 个点,则该体素的质心 P c e n t r o i d ( x c e n t r o i d , y c e n t r o i d , z c e n t r o i d ) P_{centroid}(x_{centroid},y_{centroid},z_{centroid}) Pcentroid(xcentroid,ycentroid,zcentroid) 为:

{ x c e n t r o i d = ∑ i = 1 m x i m y c e n t r o i d = ∑ i = 1 m y i m z c e n t r o i d = ∑ i = 1 m z i m \begin{cases} x_{centroid}=\cfrac{\sum_{i=1}^{m}{x_i}}{m}\\ y_{centroid}=\cfrac{\sum_{i=1}^{m}{y_i}}{m}\\ z_{centroid}=\cfrac{\sum_{i=1}^{m}{z_i}}{m}\\ \end{cases} xcentroid=mi=1mxiycentroid=mi=1myizcentroid=mi=1mzi

1.2 代码实现

代码分为三部分:

  • main.cpp
  • voxel_centroid_down_sampling.h
  • voxel_centroid_down_sampling.cpp

main.cpp

#include "voxel_centroid_down_sampling.h"
#include <pcl/console/time.h>

int main()
{
	//----------------------------- 加载点云 -----------------------------
	pcl::PointCloud<PointT>::Ptr cloud_in(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("grid.pcd", *cloud_in) < 0)
	{
		PCL_ERROR("/a->点云文件不存在!/n");
		system("pause");
		abort();
	}
	cout << "->共加载 " << cloud_in->points.size() << " 个数据点" << endl;
	//===================================================================

	pcl::console::TicToc time;
	time.tic();
	//------------------------- 三维体素格网化点云 ------------------------
	pcl::PointCloud<PointT>::Ptr cloud_downSample(new pcl::PointCloud<PointT>);
	VoxelCentriodDownSample vcds;		//创建三维体素格网化对象
	vcds.setInputCloud(cloud_in);		//设置输入点云
	vcds.setGridStep(0.2);				//设置体素格网边长
	vcds.downSample(cloud_downSample);	//执行体素质心下采样,并将采样结果保存到cloud_downSample中

	cout << "->体素质心下采样用时:" << time.toc() / 1000 << " s" << endl;
	//===================================================================

	//------------------------- 保存体素质心下采样点云 ------------------------
	if (!cloud_downSample->empty())
	{
		pcl::io::savePCDFileASCII("downSample_centroid.pcd", *cloud_downSample);
		cout << "->下采样点云的点数为:" << cloud_downSample->points.size() << endl;
	}
	else
	{
		PCL_ERROR("\a->下采样点云为空!\n");
		system("pause");
	}
	//===================================================================
	
	return 0;
}

输出结果:

->共加载 5746 个数据点
->体素质心下采样用时:0.243 s
->下采样点云的点数为:856

voxel_centroid_down_sampling.h

#pragma once
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>

using namespace std;

typedef pcl::PointXYZ PointT;

//体素质心下采样类
class VoxelCentriodDownSample
{
public:
	/**
	* @brief   :设置输入点云
	* @param[I]:cloud_in(输入点云)
	* @param[O]:none
	* @return  : none
	* @note    :
	**/
	void setInputCloud(pcl::PointCloud<PointT>::Ptr cloud_in);

	/**
	* @brief   :设置体素格网边长
	* @param[I]:step(体素格网边长)
	* @param[O]:none
	* @return  : none
	* @note    :
	**/
	void setGridStep(double step);

	/**
	* @brief   :体素下采样
	* @param[I]:none
	* @param[O]:downSampleCloud(下采样点云)
	* @return  : none
	* @note    :
	**/
	void downSample(pcl::PointCloud<PointT>::Ptr downSampleCloud);

private:
	pcl::PointCloud<PointT>::Ptr m_cloud_in;	//输入点云
	bool is_setInputCloud = false;

	double m_step;		//格网边长
	bool is_setGridStep = false;

	int m_row;			//格网总行数
	int m_col;			//格网总列数
	int m_lay;			//格网总层数

};

voxel_centroid_down_sampling.cpp

#include "voxel_centroid_down_sampling.h"


/**
* @brief   :设置输入点云
* @param[I]:cloud_in(输入点云)
* @param[O]:none
* @return  : none
* @note    :
**/
void VoxelCentriodDownSample::setInputCloud(pcl::PointCloud<PointT>::Ptr cloud_in)
{
	m_cloud_in = cloud_in;
	is_setInputCloud = true;
}

/**
* @brief   :设置体素格网边长
* @param[I]:step(体素格网边长)
* @param[O]:none
* @return  : none
* @note    :
**/
void VoxelCentriodDownSample::setGridStep(double step)
{
	if (step > 0)
	{
		m_step = step;
		is_setGridStep = true;
	}
	else
	{
		PCL_ERROR("\a->格网边长应为正数!\n");
		system("pause");
		abort();
	}
}

/**
* @brief   :体素质心下采样
* @param[I]:none
* @param[O]:downSampleCloud(下采样点云)
* @return  : none
* @note    :
**/
void VoxelCentriodDownSample::downSample(pcl::PointCloud<PointT>::Ptr downSampleCloud)
{
	if (!is_setGridStep)
	{
		PCL_ERROR("\a->请先设置格网边长!\n");
		system("pause");
		abort();
	}
	pcl::PointXYZ min;
	pcl::PointXYZ max;
	pcl::getMinMax3D(*m_cloud_in, min, max);

	m_row = (int)((max.y - min.y) / m_step) + 1;
	m_col = (int)((max.x - min.x) / m_step) + 1;
	m_lay = (int)((max.z - min.z) / m_step) + 1;

	int row_i;			//每一点的行号,从1开始
	int col_i;			//每一点的列号,从1开始
	int lay_i;		//每一点的层号,从1开始
	int grid_id_pt;		//逐行对应的一维格网编号id,从1开始
	multimap<int, PointT> m_grid3D;	//存放水平面格网点云的容器

	//遍历点云,进行三维体素格网化
	size_t num_cp = m_cloud_in->points.size();
	for (size_t i = 0; i < num_cp; i++)
	{
		row_i = (int)((m_cloud_in->points[i].y - min.y) / m_step) + 1;	//每一点的行号,从1开始
		col_i = (int)((m_cloud_in->points[i].x - min.x) / m_step) + 1;	//每一点的列号,从1开始
		lay_i = (int)((m_cloud_in->points[i].z - min.z) / m_step) + 1;//每一点的列号,从1开始

		grid_id_pt = (lay_i - 1) * (m_row * m_col) + (row_i - 1) * m_col + col_i;	//格网一维索引,从1开始
		m_grid3D.insert(pair<int, PointT>(grid_id_pt, m_cloud_in->points[i]));		//将每一个id对应的点坐标存入容器grids2D中
	}

	//判断体素是否为空,若非空,则计算体素内点云质心,以质心代替该体素内的所有点
	for (int lay = 1; lay <= m_lay; lay++)			//层扫描
	{
		for (int row = 1; row <= m_row; row++)		//行扫描
		{
			for (int col = 1; col <= m_col; col++)	//列扫描
			{
				int grid_id;	//逐行对应的一维格网编号id,从1开始
				grid_id = (lay - 1) * (m_row * m_col) + (row - 1) * m_col + col;
				if (m_grid3D.count(grid_id))		//若体素格网内有点,则计算体素质心
				{
					float sum_x, sum_y, sum_z;
					sum_x = sum_y = sum_z = 0;

					auto range = m_grid3D.equal_range(grid_id);
					for(auto it = range.first;it!=range.second; ++it)
					{
						sum_x += (*it).second.x;
						sum_y += (*it).second.y;
						sum_z += (*it).second.z;
					}

					PointT temp;	//临时存放体素质心
					temp.x = sum_x / m_grid3D.count(grid_id);
					temp.y = sum_y / m_grid3D.count(grid_id);
					temp.z = sum_z / m_grid3D.count(grid_id);

					downSampleCloud->push_back(temp);
				}
			}
		}
	}
}

1.3 与 PCL:VoxelGrid 点云体素下采样结果对比

PCL提供了 VoxelGrid点云体素下采样 的接口,直接调用即可,输出结果如下:

->加载了 5746 个数据点
->正在进行体素下采样...
->体素下采样用时:0.058 s
->下采样点云的点数为:836
下采样结果对比
方法用时点数
本文方法0.243s856
PCL:VoxelGrid0.058s836
原始点云本文下采样点云PCL下采样点云

2 体素中心下采样

2.1 原理

与体素质心下采样类似,首先将点云进行 体素划分,然后计算非空体素的中心代替该体素内的所有点,实现点云的下采样。

2.2 体素中心的计算

已知一个点的行列层数 ( r o w , c o l , l a y ) (row,col,lay) (row,col,lay),体素边长 s t e p step step,坐标最小值 x m i n 、 y m i n 、 z m i n x_{min}、y_{min}、z_{min} xminyminzmin, 则该点所在体素的中心 P c e n t e r ( x c e n t e r , y c e n t e r , z c e n t e r ) P_{center}(x_{center},y_{center},z_{center}) Pcenter(xcenter,ycenter,zcenter) 为:
{ x c e n t e r = x m i n + ( c o l − 0.5 ) ∗ s t e p y c e n t e r = y m i n + ( r o w − 0.5 ) ∗ s t e p z c e n t e r = z m i n + ( l a y − 0.5 ) ∗ s t e p \begin{cases} x_{center}=x_{min}+(col-0.5)*step\\ y_{center}=y_{min}+(row-0.5)*step\\ z_{center}=z_{min}+(lay-0.5)*step\\ \end{cases} xcenter=xmin+(col0.5)stepycenter=ymin+(row0.5)stepzcenter=zmin+(lay0.5)step

2.2 代码实现

代码分为三部分:

  • main.cpp
  • voxel_center_down_sampling.h
  • voxel_center_down_sampling.cpp

main.cpp

#include "voxel_center_down_sampling.h"
#include <pcl/console/time.h>

int main()
{
	//----------------------------- 加载点云 -----------------------------
	pcl::PointCloud<PointT>::Ptr cloud_in(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("grid.pcd", *cloud_in) < 0)
	{
		PCL_ERROR("/a->点云文件不存在!/n");
		system("pause");
		abort();
	}
	cout << "->共加载 " << cloud_in->points.size() << " 个数据点" << endl;
	//===================================================================

	pcl::console::TicToc time;
	time.tic();
	//------------------------- 体素中心下采样点云 ------------------------
	pcl::PointCloud<PointT>::Ptr cloud_downSample(new pcl::PointCloud<PointT>);
	VoxelCenterDownSample vcds;
	vcds.setInputCloud(cloud_in);
	vcds.setGridStep(0.2);
	vcds.downSample(cloud_downSample);
	
	cout << "->体素中心下采样用时:" << time.toc() / 1000 << " s" << endl;
	//===================================================================

	//------------------------- 保存体素下采样点云 ------------------------
	if (!cloud_downSample->empty())
	{
		pcl::io::savePCDFileASCII("downSample_center.pcd", *cloud_downSample);
		cout << "->下采样点云的点数为:" << cloud_downSample->points.size() << endl;
	}
	else
	{
		PCL_ERROR("\a->下采样点云为空!\n");
		system("pause");
	}
	//===================================================================

	return 0;
}

输出结果:

->共加载 5746 个数据点
->体素中心下采样用时:0.201 s
->下采样点云的点数为:856

voxel_center_down_sampling.h

#pragma once
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>

using namespace std;

typedef pcl::PointXYZ PointT;

//体素中心下采样类
class VoxelCenterDownSample
{
public:
	/**
	* @brief   :设置输入点云
	* @param[I]:cloud_in(输入点云)
	* @param[O]:none
	* @return  : none
	* @note    :
	**/
	void setInputCloud(pcl::PointCloud<PointT>::Ptr cloud_in);

	/**
	* @brief   :设置体素格网边长
	* @param[I]:step(体素格网边长)
	* @param[O]:none
	* @return  : none
	* @note    :
	**/
	void setGridStep(double step);

	/**
	* @brief   :体素中心下采样
	* @param[I]:none
	* @param[O]:downSampleCloud(下采样点云)
	* @return  : none
	* @note    :
	**/
	void downSample(pcl::PointCloud<PointT>::Ptr downSampleCloud);

private:
	pcl::PointCloud<PointT>::Ptr m_cloud_in;	//输入点云
	bool is_setInputCloud = false;

	double m_step;		//格网边长
	bool is_setGridStep = false;

	int m_row;			//格网总行数
	int m_col;			//格网总列数
	int m_lay;			//格网总层数

};

voxel_center_down_sampling.cpp

#include "voxel_center_down_sampling.h"

/**
* @brief   :设置输入点云
* @param[I]:cloud_in(输入点云)
* @param[O]:none
* @return  : none
* @note    :
**/
void VoxelCenterDownSample::setInputCloud(pcl::PointCloud<PointT>::Ptr cloud_in)
{
	m_cloud_in = cloud_in;
	is_setInputCloud = true;
}

/**
* @brief   :设置体素格网边长
* @param[I]:step(体素格网边长)
* @param[O]:none
* @return  : none
* @note    :
**/
void VoxelCenterDownSample::setGridStep(double step)
{
	if (step > 0)
	{
		m_step = step;
		is_setGridStep = true;
	}
	else
	{
		PCL_ERROR("\a->格网边长应为正数!\n");
		system("pause");
		abort();
	}
}

/**
* @brief   :体素中心下采样
* @param[I]:none
* @param[O]:downSampleCloud(下采样点云)
* @return  : none
* @note    :
**/
void VoxelCenterDownSample::downSample(pcl::PointCloud<PointT>::Ptr downSampleCloud)
{
	if (!is_setGridStep)
	{
		PCL_ERROR("\a->请先设置格网边长!\n");
		system("pause");
		abort();
	}
	pcl::PointXYZ min;
	pcl::PointXYZ max;
	pcl::getMinMax3D(*m_cloud_in, min, max);

	m_row = (int)((max.y - min.y) / m_step) + 1;
	m_col = (int)((max.x - min.x) / m_step) + 1;
	m_lay = (int)((max.z - min.z) / m_step) + 1;

	int row_i;			//每一点的行号,从1开始
	int col_i;			//每一点的列号,从1开始
	int lay_i;		//每一点的层号,从1开始
	int grid_id_pt;		//逐行对应的一维格网编号id,从1开始
	multimap<int, PointT> m_grid3D;	//存放水平面格网点云的容器

	//遍历点云,进行三维体素格网化
	size_t num_cp = m_cloud_in->points.size();
	for (size_t i = 0; i < num_cp; i++)
	{
		row_i = (int)((m_cloud_in->points[i].y - min.y) / m_step) + 1;	//每一点的行号,从1开始
		col_i = (int)((m_cloud_in->points[i].x - min.x) / m_step) + 1;	//每一点的列号,从1开始
		lay_i = (int)((m_cloud_in->points[i].z - min.z) / m_step) + 1;//每一点的列号,从1开始

		grid_id_pt = (lay_i - 1) * (m_row * m_col) + (row_i - 1) * m_col + col_i;	//格网一维索引,从1开始
		m_grid3D.insert(pair<int, PointT>(grid_id_pt, m_cloud_in->points[i]));		//将每一个id对应的点坐标存入容器grids2D中
	}

	//判断体素是否为空,若非空,则计算体素内点云中心,以中心代替该体素内的所有点
	for (int lay = 1; lay <= m_lay; lay++)			//层扫描
	{
		for (int row = 1; row <= m_row; row++)		//行扫描
		{
			for (int col = 1; col <= m_col; col++)	//列扫描
			{
				int grid_id;	//逐行对应的一维格网编号id,从1开始
				grid_id = (lay - 1) * (m_row * m_col) + (row - 1) * m_col + col;
				
				//若体素格网内有点,则计算体素中心
				if (m_grid3D.count(grid_id))		
				{
					PointT temp;
					temp.x = min.x + (col - 0.5)*m_step;
					temp.y = min.y + (row - 0.5)*m_step;
					temp.z = min.z + (lay -0.5)*m_step;

					downSampleCloud->push_back(temp);
				}
			}
		}
	}
}

2.3 与体素质心下采样结果对比

下采样结果对比
方法用时点数
体素质心0.243s856
体素中心0.201s856
原始点云体素质心下采样点云体素中心下采样点云

3 总结

  • 体素质心下采样方法虽然耗时长,但采样结果更加准确;体素中心下采样方法耗时短,但容易失真。(由于本次实验数据点数较少,耗时接近,可尝试采样大规模点云验证)
  • 体素质心下采样和体素中心下采样,.h 文件是一样的,只是下采样实现的方式(.cpp)不同。
  • 注意下面的代码,行列层都是从1开始,并非0。因此for循环的判断条件是 “<=”,并非“=”。
for (int lay = 1; lay <= m_lay; lay++)				//层扫描
	{
		for (int row = 1; row <= m_row; row++)		//行扫描
		{
			for (int col = 1; col <= m_col; col++)	//列扫描
			{
				···
			}
		}
	}
评论 7
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

孙 悟 空

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值