autoware 点云聚类 四 分段聚类


这是一个随处可见的聚类方案

segmentByDistance

void segmentByDistance(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                       pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud_ptr,
                       autoware_msgs::Centroids &in_out_centroids, autoware_msgs::CloudClusterArray &in_out_clusters)

// cluster the pointcloud according to the distance of the points using different thresholds (not only one for the
// entire pc)
// in this way, the points farther in the pc will also be clustered

  std::vector<ClusterPtr> all_clusters;//用于存放所有检测物
  • 下面的code设定了条件if (!_use_multiple_thres)其实没有太理解,没有看见加速的部分代码
  • #ifdef GPU_CLUSTERING在code里没有定义,但是在cmake文件中有相关配置,实际情况中是运行了这个条件下的code。

if (!_use_multiple_thres)

利用一个for循环转换点云类型。这里有没有直接xyzrgb转xyz类型的函数呢。如果有估计内部也是如此一个循环,哈哈。
转换函数pcl::copyPointCloud(*cloud_xyz, *cloud_xyzrgba);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);

    for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
    {
      pcl::PointXYZ current_point;
      current_point.x = in_cloud_ptr->points[i].x;
      current_point.y = in_cloud_ptr->points[i].y;
      current_point.z = in_cloud_ptr->points[i].z;

      cloud_ptr->points.push_back(current_point);
      }

聚类

没有定义 GPU_CLUSTERING

利用EuclideanClusterExtraction对输入点云进行聚类,返回聚类指针类型的容器。

      //聚类  in_out_centroids未用到  
    all_clusters =
        clusterAndColor(cloud_ptr, out_cloud_ptr, in_out_centroids, _clustering_distance);

clusterAndColor()

其实这里可以注意到传入点云类型为pcl::PointCloud<pcl::PointXYZ>::Ptr而传出点云类型pcl::PointCloud<pcl::PointXYZRGB>::Ptr

std::vector<ClusterPtr> clusterAndColor(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                                        pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud_ptr,
                                        autoware_msgs::Centroids &in_out_centroids,
                                        double in_max_cluster_distance = 0.5)
{
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);

  // create 2d pc  2D-Tree 不关心z方向的排列
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d);
  // make it flat
  for (size_t i = 0; i < cloud_2d->points.size(); i++)
  {
    cloud_2d->points[i].z = 0;
  }

  if (cloud_2d->points.size() > 0)
    tree->setInputCloud(cloud_2d);

  std::vector<pcl::PointIndices> cluster_indices;

  // perform clustering on 2d cloud
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance(in_max_cluster_distance);  //
  ec.setMinClusterSize(_cluster_size_min);
  ec.setMaxClusterSize(_cluster_size_max);
  ec.setSearchMethod(tree);
  ec.setInputCloud(cloud_2d);
  ec.extract(cluster_indices); //得到聚类索引cluster_indices
  // use indices on 3d cloud

  /
  //---	3. Color clustered points
  /
  unsigned int k = 0;
  // pcl::PointCloud<pcl::PointXYZRGB>::Ptr final_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);

  std::vector<ClusterPtr> clusters;
  // pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);//coord + color
  // cluster  通过聚类索引将原点云分类
  for (auto it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
  {
  //it 聚类索引 it->indices 该聚类下点云点索引
    ClusterPtr cluster(new Cluster());
    cluster->SetCloud(in_cloud_ptr, it->indices, _velodyne_header, k, 
                      (int) _colors[k].val[0],
                      (int) _colors[k].val[1],
                      (int) _colors[k].val[2], "", _pose_estimation);
    clusters.push_back(cluster);
    k++;
  }
  // std::cout << "Clusters: " << k << std::endl;
  return clusters;
}

定义 GPU_CLUSTERING

其实就是利用cuda加速聚类

all_clusters = clusterAndColorGpu(cloud_ptr, out_cloud_ptr, in_out_centroids,
                                        _clustering_distance);

涉及到文件gpu_euclidean_clustering.cu

这里的SetCloud函数是Cluster的一个方法见另一篇博客。

if(_use_multiple_thres)

// 0 => 0-15m d=0.5
// 1 => 15-30 d=1
// 2 => 30-45 d=1.6
// 3 => 45-60 d=2.1
// 4 => >60 d=2.6

//空点指针类型云容器
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_segments_array(5);
    for (unsigned int i = 0; i < cloud_segments_array.size(); i++)
    {
      pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      cloud_segments_array[i] = tmp_cloud;
    }

    for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
    {
      pcl::PointXYZ current_point;
      current_point.x = in_cloud_ptr->points[i].x;
      current_point.y = in_cloud_ptr->points[i].y;
      current_point.z = in_cloud_ptr->points[i].z;

      float origin_distance = sqrt(pow(current_point.x, 2) + pow(current_point.y, 2));
//按距离点云分段
      if (origin_distance < _clustering_ranges[0])
      {
        cloud_segments_array[0]->points.push_back(current_point);
      }
      else if (origin_distance < _clustering_ranges[1])
      {
        cloud_segments_array[1]->points.push_back(current_point);

      }else if (origin_distance < _clustering_ranges[2])
      {
        cloud_segments_array[2]->points.push_back(current_point);

      }else if (origin_distance < _clustering_ranges[3])
      {
        cloud_segments_array[3]->points.push_back(current_point);

      }else
      {
        cloud_segments_array[4]->points.push_back(current_point);
      }
    }
//分别给别段点云 聚类
    std::vector<ClusterPtr> local_clusters;
    for (unsigned int i = 0; i < cloud_segments_array.size(); i++)
    {
#ifdef GPU_CLUSTERING
    ROS_INFO("use GPU_CLUSTERING!!!!!!!!!!!! ");
      if (_use_gpu)
      {
        local_clusters = clusterAndColorGpu(cloud_segments_array[i], out_cloud_ptr,
                                            in_out_centroids, _clustering_distances[i]);
      } else
      {
        local_clusters = clusterAndColor(cloud_segments_array[i], out_cloud_ptr,
                                         in_out_centroids, _clustering_distances[i]);
      }
#else
ROS_INFO("no no use GPU_CLUSTERING!!!!!!!!!!!! ");
      local_clusters = clusterAndColor(
          cloud_segments_array[i], out_cloud_ptr, in_out_centroids, _clustering_distances[i]);
#endif
      all_clusters.insert(all_clusters.end(), local_clusters.begin(), local_clusters.end());
    }

关于insert

c.insert(pos,beg,end) // 在pos位置插入在[beg,end)区间的数据。无返回值。

合并聚类

如果_use_multiple_thres=false其实是没有必要合并聚类的。因为就没有分开聚类

这里定义的容器类型为ClusterPtr(后面补充,是一个自定义的类)

  // check for mergable clusters
  std::vector<ClusterPtr> mid_clusters;
  std::vector<ClusterPtr> final_clusters;

两次检查并合并点云。合并结果为:final_clusterspictogram

  if (all_clusters.size() > 0)
    checkAllForMerge(all_clusters, mid_clusters, _cluster_merge_threshold);
  else
    mid_clusters = all_clusters;

  if (mid_clusters.size() > 0)
    checkAllForMerge(mid_clusters, final_clusters, _cluster_merge_threshold);
  else
    final_clusters = mid_clusters;

checkClusterMerge()

根据两个聚类质心距离判断是否要合并 这是一个嵌套函数 返回merge_indices

//根据两个聚类质心距离判断是否要合并 这是一个嵌套函数 返回merge_indices
void checkClusterMerge(size_t in_cluster_id, std::vector<ClusterPtr> &in_clusters,
                       std::vector<bool> &in_out_visited_clusters, std::vector<size_t> &out_merge_indices,
                       double in_merge_threshold)
{
  // std::cout << "checkClusterMerge" << std::endl;
  //返回一个质心坐标
  pcl::PointXYZ point_a = in_clusters[in_cluster_id]->GetCentroid();
  for (size_t i = 0; i < in_clusters.size(); i++)
  {
    if (i != in_cluster_id && !in_out_visited_clusters[i])
    {
      pcl::PointXYZ point_b = in_clusters[i]->GetCentroid();
      double distance = sqrt(pow(point_b.x - point_a.x, 2) + pow(point_b.y - point_a.y, 2));
      if (distance <= in_merge_threshold)
      {
        in_out_visited_clusters[i] = true;
        out_merge_indices.push_back(i);
        // std::cout << "Merging " << in_cluster_id << " with " << i << " dist:" << distance << std::endl;
        checkClusterMerge(i, in_clusters, in_out_visited_clusters, out_merge_indices, in_merge_threshold);
      }
    }
  }
}

mergeClusters()合并聚类

 //合并聚类 
void mergeClusters(const std::vector<ClusterPtr> &in_clusters, std::vector<ClusterPtr> &out_clusters,
                   std::vector<size_t> in_merge_indices, const size_t &current_index,
                   std::vector<bool> &in_out_merged_clusters)
{
  // std::cout << "mergeClusters:" << in_merge_indices.size() << std::endl;
  pcl::PointCloud<pcl::PointXYZRGB> sum_cloud;
  pcl::PointCloud<pcl::PointXYZ> mono_cloud;
  ClusterPtr merged_cluster(new Cluster());
  for (size_t i = 0; i < in_merge_indices.size(); i++)
  {
    //根据索引in_merge_indices[i]获取点云
    sum_cloud += *(in_clusters[in_merge_indices[i]]->GetCloud());
    //记录合并操作的点云
    in_out_merged_clusters[in_merge_indices[i]] = true;
  }
  //容器初始化 0
  std::vector<int> indices(sum_cloud.points.size(), 0);
 // 生成以连续的序列号,应该是为后卫合并作准备
  for (size_t i = 0; i < sum_cloud.points.size(); i++)
  {
    indices[i] = i;
  }
//点云合并
  if (sum_cloud.points.size() > 0)
  {
    pcl::copyPointCloud(sum_cloud, mono_cloud);
    merged_cluster->SetCloud(mono_cloud.makeShared(), indices, _velodyne_header, current_index,
                             (int) _colors[current_index].val[0], (int) _colors[current_index].val[1],
                             (int) _colors[current_index].val[2], "", _pose_estimation);
    out_clusters.push_back(merged_cluster);
  }
}

checkAllForMerge()

输入输出都是引用类型和一个合并的阀值。

void checkAllForMerge(std::vector<ClusterPtr> &in_clusters, std::vector<ClusterPtr> &out_clusters,
                      float in_merge_threshold)
{
  // std::cout << "checkAllForMerge" << std::endl;
 //注意连个容器类型是布尔类型 且初始化为false
  std::vector<bool> visited_clusters(in_clusters.size(), false);
  std::vector<bool> merged_clusters(in_clusters.size(), false);
  size_t current_index = 0;
  for (size_t i = 0; i < in_clusters.size(); i++)
  {
    if (!visited_clusters[i])
    {
      visited_clusters[i] = true;
      std::vector<size_t> merge_indices;
      //根据两个聚类质心距离判断是否要合并 这是一个嵌套函数 返回merge_indices
      checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, in_merge_threshold);
     
      mergeClusters(in_clusters, out_clusters, merge_indices, current_index++, merged_clusters);
    }
  }
 
  for (size_t i = 0; i < in_clusters.size(); i++)
  {
    // check for clusters not merged, add them to the output
    if (!merged_clusters[i])
    {
      out_clusters.push_back(in_clusters[i]);
    }
  }

  // ClusterPtr cluster(new Cluster());
}

聚类属性整理输出

  // Get final PointCloud to be published
    for (unsigned int i = 0; i < final_clusters.size(); i++)
    {
      *out_cloud_ptr = *out_cloud_ptr + *(final_clusters[i]->GetCloud());

      jsk_recognition_msgs::BoundingBox bounding_box = final_clusters[i]->GetBoundingBox();
      geometry_msgs::PolygonStamped polygon = final_clusters[i]->GetPolygon();
      jsk_rviz_plugins::Pictogram pictogram_cluster;
      pictogram_cluster.header = _velodyne_header;

      // PICTO
      pictogram_cluster.mode = pictogram_cluster.STRING_MODE;
      pictogram_cluster.pose.position.x = final_clusters[i]->GetMaxPoint().x;
      pictogram_cluster.pose.position.y = final_clusters[i]->GetMaxPoint().y;
      pictogram_cluster.pose.position.z = final_clusters[i]->GetMaxPoint().z;
      tf::Quaternion quat(0.0, -0.7, 0.0, 0.7);
      tf::quaternionTFToMsg(quat, pictogram_cluster.pose.orientation);
      pictogram_cluster.size = 4;
      std_msgs::ColorRGBA color;
      color.a = 1;
      color.r = 1;
      color.g = 1;
      color.b = 1;
      pictogram_cluster.color = color;
      pictogram_cluster.character = std::to_string(i);
      // PICTO

      // pcl::PointXYZ min_point = final_clusters[i]->GetMinPoint();
      // pcl::PointXYZ max_point = final_clusters[i]->GetMaxPoint();
      pcl::PointXYZ center_point = final_clusters[i]->GetCentroid();
      geometry_msgs::Point centroid;
      centroid.x = center_point.x;
      centroid.y = center_point.y;
      centroid.z = center_point.z;
      bounding_box.header = _velodyne_header;
      polygon.header = _velodyne_header;

      if (final_clusters[i]->IsValid())
      {

        in_out_centroids.points.push_back(centroid);

        autoware_msgs::CloudCluster cloud_cluster;
        final_clusters[i]->ToROSMessage(_velodyne_header, cloud_cluster);
        in_out_clusters.clusters.push_back(cloud_cluster);
      }
    }

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
社会演员多的头像社会演员多普通用户
上一篇 2022年5月21日
下一篇 2022年5月21日

相关推荐