文章目录
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=m∑i=1mxiycentroid=m∑i=1myizcentroid=m∑i=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.243s | 856 |
PCL:VoxelGrid | 0.058s | 836 |
原始点云 | 本文下采样点云 | 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}
xmin、ymin、zmin, 则该点所在体素的中心
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+(col−0.5)∗stepycenter=ymin+(row−0.5)∗stepzcenter=zmin+(lay−0.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.243s | 856 |
体素中心 | 0.201s | 856 |
原始点云 | 体素质心下采样点云 | 体素中心下采样点云 |
![]() | ![]() | ![]() |
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++) //列扫描
{
···
}
}
}