pcl欧式聚类
欧式聚类实现方法大致是:
1、找到空间中某点 p 1 p_1 p1,用KD-Tree找到离他最近的n个点,判断这n个点到 p 1 p_1 p1的距离。将距离小于阈值r的点 p 2 、 p 3 、 p 4 p_2、p_3、p_4 p2、p3、p4…放在类Q里
2、在 Q ( p 1 ) Q(p_1) Q(p1)里找到一点 p 2 p_2 p2 ,重复步骤1
3、在 Q ( p 1 , p 2 ) Q(p_1,p_2) Q(p1,p2)找到一点,重复步骤1,找到 p 22 、 p 23 p_{22}、p_{23} p22、p23… 全部放进Q里
4、当Q再也不能有新点加入了,则完成搜索了
使用pcl库的欧式聚类:
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction ec;
ec.setClusterTolerance (0.02); //设置近邻搜索的搜索半径为2cm
ec.setMinClusterSize (100);//设置一个聚类需要的最少点数目为100
ec.setMaxClusterSize (25000); //设置一个聚类需要的最大点数目为25000
ec.setSearchMethod (tree);//设置点云的搜索机制
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);//从点云中提取聚类,并将点云索引保存在cluster_indices中
//迭代访问点云索引cluster_indices,直到分割出所有聚类
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
//创建新的点云数据集cloud_cluster,将所有当前聚类写入到点云数据集中
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
{
cloud_cluster->points.push_back (cloud_filtered->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
}
pcl::visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(cloud_cluster);
pause();
}
pcl实现原理:
pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
const typename search::Search<PointT>::Ptr &tree,
float tolerance, std::vector<PointIndices> &clusters,
unsigned int min_pts_per_cluster,
unsigned int max_pts_per_cluster)
{
if (tree->getInputCloud ()->points.size () != cloud.points.size ()) // 点数量检查
{
PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
return;
}
int nn_start_idx = tree->getSortedResults () ? 1 : 0;
std::vector<bool> processed (cloud.points.size (), false);
std::vector<int> nn_indices;
std::vector<float> nn_distances;
for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i) //遍历点云中的每一个点
{
if (processed[i])
continue;
std::vector<int> seed_queue;
int sq_idx = 0;
seed_queue.push_back (i);
processed[i] = true;
while (sq_idx < static_cast<int> (seed_queue.size ())) //遍历每一个种子
{
if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
{
sq_idx++;
continue;
}
for (size_t j = nn_start_idx; j < nn_indices.size (); ++j)
{
if (nn_indices[j] == -1 || processed[nn_indices[j]])
continue;
seed_queue.push_back (nn_indices[j]);
processed[nn_indices[j]] = true;
}
sq_idx++;
}
if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
{
pcl::PointIndices r;
r.indices.resize (seed_queue.size ());
for (size_t j = 0; j < seed_queue.size (); ++j)
r.indices[j] = seed_queue[j];
std::sort (r.indices.begin (), r.indices.end ());
r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
r.header = cloud.header;
clusters.push_back (r);
}
}
}