PCL库学习（12）_基于voxel_grid的欧式聚类算法

本算法在autoware中有相应的实现，这里对代码作相应的的理解

void VoxelGridBasedEuclideanClusterNodelet::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr & input_msg)
{
// ros2pcl
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
// 创建体素栅格对象并滤波，在不改变点云特征的前提下，减少点云的数量
pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0);
voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_);
voxel_grid_.setInputCloud(raw_pointcloud_ptr);
voxel_grid_.setSaveLeafLayout(true);
voxel_grid_.filter(*voxel_map_ptr);
// 将滤波后点的z值设置为0，这样欧式空间上的分割变成二维空间，进一步提高分割效率
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_2d_ptr(new pcl::PointCloud<pcl::PointXYZ>);
for (size_t i = 0; i < voxel_map_ptr->points.size(); ++i)
{
pcl::PointXYZ point;
point.x = voxel_map_ptr->points.at(i).x;
point.y = voxel_map_ptr->points.at(i).y;
point.z = 0.0;
pointcloud_2d_ptr->push_back(point);
}
// 创建点云的kdtree数据结构
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(pointcloud_2d_ptr);
// 调用PCL的欧式聚类对象
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> pcl_euclidean_cluster;
pcl_euclidean_cluster.setClusterTolerance(tolerance_);
pcl_euclidean_cluster.setMinClusterSize(1);
pcl_euclidean_cluster.setMaxClusterSize(max_cluster_size_);
pcl_euclidean_cluster.setSearchMethod(tree);
pcl_euclidean_cluster.setInputCloud(pointcloud_2d_ptr);
pcl_euclidean_cluster.extract(cluster_indices);
//重点在这里----------
//这里创建了一个map容器从栅格点索引去查找聚类索引，键：voxel_grid_index,值:cluster_index
std::unordered_map< int,
int> map;
int cluster_idx = 0;
//遍历每一个聚类
for (std::vector<pcl::PointIndices>::const_iterator cluster_itr = cluster_indices.begin();cluster_itr != cluster_indices.end(); ++cluster_itr)
{
//遍历每个聚类里面的栅格点索引
for (std::vector<int>::const_iterator point_itr = cluster_itr->indices.begin();point_itr != cluster_itr->indices.end(); ++point_itr)
{
//把每一个聚类里面的点索引标记为相同的聚类索引cluster_idx。eg.point_itr迭代器指向的栅格点索引都属于cluster_idx=0
map[*point_itr] = cluster_idx;
}
++cluster_idx;
}
//以上就完成了一个栅格点索引的分类，那么如何在真实世界下表示？
std::vector<pcl::PointCloud<pcl::PointXYZ>> v_cluster;//最终的聚类
v_cluster.resize(cluster_idx);
for (size_t i = 0; i < raw_pointcloud_ptr->points.size(); ++i)
{
//把真实世界下（原始输入点云）的一个点，通过voxel_grid转换为栅格索引值index，也即是在聚类算法下的点索引。真实世界下的点与栅格索引是多对一的关系。
const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(raw_pointcloud_ptr->points.at(i).x, raw_pointcloud_ptr->points.at(i).y,raw_pointcloud_ptr->points.at(i).z));
if (map.find(index) != map.end())//在map容器中去查找是否存在index的索引值
//如果存在这样的一个索引值，就把相应的真实点放到对应的聚类中去；map[index]也即聚类的索引
v_cluster.at(map[index]).points.push_back(raw_pointcloud_ptr->points.at(i));
}
//后面的代码比较容易理解
// build output msg
{
autoware_perception_msgs::DynamicObjectWithFeatureArray output;
output.header = input_msg->header;
for (std::vector<pcl::PointCloud<pcl::PointXYZ>>::const_iterator cluster_itr =v_cluster.begin();cluster_itr != v_cluster.end(); ++cluster_itr)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator point_itr = cluster_itr->points.begin();point_itr != cluster_itr->points.end(); ++point_itr)
{
cloud_cluster->points.push_back(*point_itr);
}
if (min_cluster_size_ <= cloud_cluster->points.size() &&cloud_cluster->points.size() <= max_cluster_size_)
continue;
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
sensor_msgs::PointCloud2 ros_pointcloud;
autoware_perception_msgs::DynamicObjectWithFeature feature_object;
pcl::toROSMsg(*cloud_cluster, ros_pointcloud);
ros_pointcloud.header = input_msg->header;
feature_object.feature.cluster = ros_pointcloud;
output.feature_objects.push_back(feature_object);
}
cluster_pub_.publish(output);
}
}

其实也就是多了一个三维体素的索引过渡，当然也可以先通过体素滤波，再用原始的欧式聚类，比较容易理解。

最后

以上就是虚幻时光为你收集整理的PCL库学习（12）_基于voxel_grid的欧式聚类算法的全部内容，希望文章能够帮你解决PCL库学习（12）_基于voxel_grid的欧式聚类算法所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。