PCL中的VoxelGrid体素⽹格化下采样（⾃⼰改的PCL中的源码，这样就不需要
加载PCL库了）
找了⽹上的点云VoxelGrid都是采⽤PCL库实现的，但是项⽬中为了不⽤PCL库，就⾃⼰⽤C++实现了⼀遍；为了省事就写在了cpp⽂件中
/*-------------------------------------
2.基于VoxelGrid的点云下采样算法
3.作者：pcb
4.⽇期：2018.10.3
--------------------------------------*/
厦门自由行攻略//定义3D点的结构体
struct Point3D
{
float x;
float y;
暖味是什么意思
float z;
};
//存放计算点云的idx和点云编号的cloud_point_index的结构头
struct cloud_point_index_idx
{
unsigned int idx;
unsigned int cloud_point_index;
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_) : idx(idx_), cloud_point_index(cloud_point_index_) {}
bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }dp点什么意思
};
贺的组词
//中间参数的结构体
struct Array4f
{
float x;
float y;
float z;
float C;
};
/*----------------------------
*功能：找到输⼊点云中的包围盒两个点的值（右上和左下）
*-----------------------------
*输⼊：vector<Point3D>&InputCloudPoint（Piont3D的原始点云数据）
*输出：点云的min_p和max_p
*/
void GetMaxMin(vector<Point3D>&InputCloudPoint, Array4f&min_p, Array4f&max_p)
{
//主要思路是找到x，y,z的最⼩值，这样就能得到点云⽴体包围的次村
//找x,y,z最⼩值
if (InputCloudPoint.size() == 0)
{
阳光明媚
cout << "输⼊点云为空" << endl;
return;
}
float x_min= (*min_element(InputCloudPoint.begin(), d(), [](Point3D& a, Point3D& b){return a.x < b.x;})).x;
float y_min =(*min_element(InputCloudPoint.begin(), d(), [](Point3D& a, Point3D& b){return a.y < b.y;})).y;
float z_min =(*min_element(InputCloudPoint.begin(), d(), [](Point3D& a, Point3D& b){return a.z < b.z;})).z;
//给min_p赋值
min_p.x = x_min;
min_p.y = y_min;
min_p.z = z_min;
min_p.C = 1;
//找x,y,z的最⼤值
float x_max = (*max_element(InputCloudPoint.begin(), d(), [](Point3D& a, Point3D& b){return a.x < b.x; })).x;
float y_max = (*max_element(InputCloudPoint.begin(), d(), [](Point3D& a, Point3D& b){return a.y < b.y; })).y;
float z_max = (*max_element(InputCloudPoint.begin(), d(), [](Point3D& a, Point3D& b){return a.z < b.z; })).z;
//给max_p赋值
//给max_p赋值
max_p.x = x_max;
不可名状的名
max_p.y = y_max;
max_p.z = z_max;
max_p.C = 1;
return;
}
/*----------------------------
*功能：体素化⽹格⽅法实现下采样（PCL中的源码C++实现）
*-----------------------------
*输⼊：vector<Point3D>&InputCloudPoint(Piont3D的原始点云数据，下采样的体素⼤⼩x,y,z)
*输出：vector<Point3D>&OutPointCloud(采样之后的之后的Point3D结构的点云数据)
*/
void VoxelGrid_ApplyFilter(vector<Point3D>&InputCloudPoint, vector<Point3D>&OutPointCloud, float X_Voxel, float Y_Voxel, float Z_Voxel) {
//先判断输⼊的点云是否为空
if (InputCloudPoint.size()==0)
{
cout << "输⼊点云为空！" << endl;
return;
}
//存放输⼊点云的最⼤与最⼩坐标
Array4f min_p, max_p;
GetMaxMin(InputCloudPoint, min_p, max_p);
Array4f inver_leaf_size_;
inver_leaf_size_.x = 1 / X_Voxel;
inver_leaf_size_.y = 1 / Y_Voxel;
inver_leaf_size_.z = 1 / Z_Voxel;
inver_leaf_size_.C = 1;
//计算最⼩和最⼤边界框值
Array4f min_b_, max_b_, div_b_, divb_mul_;
min_b_.x = static_cast<int> (floor(min_p.x * inver_leaf_size_.x));
max_b_.x = static_cast<int> (floor(max_p.x * inver_leaf_size_.x));
min_b_.y = static_cast<int> (floor(min_p.y * inver_leaf_size_.y));
max_b_.y = static_cast<int> (floor(max_p.y * inver_leaf_size_.y));
min_b_.z = static_cast<int> (floor(min_p.z * inver_leaf_size_.z));
max_b_.z = static_cast<int> (floor(max_p.z * inver_leaf_size_.z));
/
/计算沿所有轴所需的分割数
div_b_.x = max_b_.x - min_b_.x + 1;
div_b_.y = max_b_.y - min_b_.y + 1;
div_b_.z = max_b_.z - min_b_.z + 1;
div_b_.C= 0;
//设置除法乘数
divb_mul_.x = 1;
divb_mul_.y = div_b_.x;
divb_mul_.z =div_b_.x * div_b_.y;
divb_mul_.C = 0;劝学的作者
//⽤于计算idx和pointcloud索引的存储
std::vector<cloud_point_index_idx> index_vector;
ve(InputCloudPoint.size());
//第⼀步：遍历所有点并将它们插⼊到具有计算idx的index_vector向量中；具有相同idx值的点将有助于产⽣CloudPoint的相同点
for (int i = 0; i < InputCloudPoint.size();i++)
{
int ijk0 = static_cast<int> (floor(InputCloudPoint[i].x * inver_leaf_size_.x) - static_cast<float> (min_b_.x));
int ijk1 = static_cast<int> (floor(InputCloudPoint[i].y * inver_leaf_size_.y) - static_cast<float> (min_b_.y));
int ijk2 = static_cast<int> (floor(InputCloudPoint[i].z * inver_leaf_size_.z) - static_cast<float> (min_b_.z));
//计算质⼼叶索引
放任自流什么意思int idx = ijk0 * divb_mul_.x + ijk1 * divb_mul_.y + ijk2 * divb_mul_.z;
int idx = ijk0 * divb_mul_.x + ijk1 * divb_mul_.y + ijk2 * divb_mul_.z;
index_vector.push_back(cloud_point_index_idx(static_cast<unsigned int> (idx), i));
}
//第⼆步：使⽤表⽰⽬标单元格的值作为索引对index_vector向量进⾏排序；实际上属于同⼀输出单元格的所有点都将彼此相邻
std::sort(index_vector.begin(), d(), std::less<cloud_point_index_idx>());
//第三步：计数输出单元格，我们需要跳过所有相同的，相邻的idx值
unsigned int total = 0;
unsigned int index = 0;
unsigned int min_points_per_voxel_ = 0;
/
/first_and_last_indices_vector [i]表⽰属于对应于第i个输出点的体素的index_vector中的第⼀个点的index_vector中的索引，以及不属于第⼀个点的索引 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
first_and_last_ve(index_vector.size()); //分配内存空间
while (index < index_vector.size())
{
unsigned int i = index + 1;
while (i < index_vector.size() && index_vector[i].idx == index_vector[index].idx)
++i;
if (i - index >= min_points_per_voxel_)
{
++total;
first_and_last_indices_vector.push_back(std::pair<unsigned int, unsigned int>(index, i));
}
index = i;
}
//第四步：计算质⼼，将它们插⼊最终位置
//size(total); //给输出点云分配内存空间
float x_Sum, y_Sum, z_Sum;
Point3D PointCloud;
unsigned int first_index, last_index;
for (unsigned int cp = 0; cp < first_and_last_indices_vector.size(); ++cp)
{
/
/ 计算质⼼ - 来⾃所有输⼊点的和值，这些值在index_vector数组中具有相同的idx值
first_index = first_and_last_indices_vector[cp].first;
last_index = first_and_last_indices_vector[cp].cond;
x_Sum = 0;
y_Sum = 0;
z_Sum = 0;
for (unsigned int li = first_index; li < last_index; ++li)
{
x_Sum += InputCloudPoint[index_vector[li].cloud_point_index].x;
y_Sum += InputCloudPoint[index_vector[li].cloud_point_index].y;
z_Sum += InputCloudPoint[index_vector[li].cloud_point_index].z;
}
PointCloud.x = x_Sum / (last_index - first_index);
PointCloud.y = y_Sum / (last_index - first_index);
PointCloud.z = z_Sum / (last_index - first_index);
OutPointCloud.push_back(PointCloud);
}
return;
}
