文章目录

引言
一、点云回调函数：
二、预处理
三、核心内容：聚类
四、发布聚类结果
五、runtime_manager聚类参数解析

引言

在自动驾驶感知层中，点云聚类具有重要的意义。点云聚类是将三维点云数据进行分组或分类，将距离较近的点归为一类的过程。以下是点云聚类的一些意义：

障碍物检测与跟踪：点云聚类可以帮助识别和区分环境中的不同物体，特别是障碍物。通过聚类，可以将同一个障碍物的点归为一类，并估计其位置、形状和大小，从而实现对障碍物的检测和跟踪。
建立环境模型：通过点云聚类，可以将点云数据分割为不同的物体群组，并将这些群组的特征提取出来。这些特征可以用于建立环境模型，包括道路、建筑物、交通标志等，为自动驾驶车辆提供更准确和详细的环境信息。
动态物体检测：点云聚类可以帮助识别和区分稳定物体和动态物体。通过将点云数据进行聚类，可以发现移动的物体，并将其与稳定的环境进行区分。这对于自动驾驶来说至关重要，因为它可以帮助车辆预测和适应动态物体的行为。
传感器融合：自动驾驶系统通常会使用多种不同类型的传感器，如激光雷达、摄像头等。点云聚类可以将多个传感器获取的点云数据进行融合，提供更全面和一致的环境感知。

总的来说，点云聚类在自动驾驶感知层中的意义在于帮助将三维点云数据转化为更易处理的物体信息，并为障碍物检测、环境建模、动态物体检测和传感器融合等任务提供基础数据和准确性。它是实现自动驾驶环境感知与决策的关键步骤之一。

Euclidean聚类算法是Autoware中常用的点云聚类算法之一。这种算法基于欧氏距离度量点之间的相似性。首先，将点云分成一个个独立的点云簇。然后，通过计算每个点与相邻点之间的欧氏距离，对点云簇进行增长，直到满足一定的距离阈值，形成一个完整的聚类。最终，每个聚类被视为一个独立的物体。Euclidean聚类算法简单直观，可以帮助识别障碍物。下面是其源码梳理（autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect）

读代码发现的几个问题以及修改办法：

构建了聚类标识，但是并没有发布（猜测是kf也发了，避免重复就不发了，但是没有去掉代码），不重要的东西，没有改。
有聚类的朝向值（类成员变量），但是并没有计算过这个变量！！！添加：通过最小包围盒计算朝向，并给成员变量赋值；
聚类中心点发布代码写错了，并没有发布出去，改正！！！
计算了包围盒但是并没有输出，添加：包围盒发布（话题：/detection/tracked_boxes，使用上面计算的朝向，结果非常准确且稳定，如下图）：

一、点云回调函数：

通过订阅的点云回调函数进入主程序，可以看到整个节点分为三大步：预处理、聚类、发布结果，具体流程查看注释：

cpp 复制代码

void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr &in_sensor_cloud)
{
  //_start = std::chrono::system_clock::now();

  if (!_using_sensor_cloud)
  {
    // 0.0 点云输入：两种来源，原始扫描点云或者已经去除地面的点云，他们的参数设置不同
    _using_sensor_cloud = true;

    pcl::PointCloud<pcl::PointXYZ>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr removed_points_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr inlanes_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr nofloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr onlyfloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr diffnormals_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr clipped_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_clustered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

    autoware_msgs::Centroids centroids;
    autoware_msgs::CloudClusterArray cloud_clusters;

    pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr);

    _velodyne_header = in_sensor_cloud->header;

    // 1.0 移除过近的点云（默认不移除）
    if (_remove_points_upto > 0.0)
    {
      // ZARD：界面最大是2.5，加了2乃权宜之计
      removePointsUpTo(current_sensor_cloud_ptr, removed_points_cloud_ptr, _remove_points_upto + 2);
    }
    else
    {
      removed_points_cloud_ptr = current_sensor_cloud_ptr;
    }

    // 2.0 降采样（默认不需要）
    if (_downsample_cloud)
      downsampleCloud(removed_points_cloud_ptr, downsampled_cloud_ptr, _leaf_size);
    else
      downsampled_cloud_ptr = removed_points_cloud_ptr;

    // 3.0 裁剪雷达上下距离远的点
    clipCloud(downsampled_cloud_ptr, clipped_cloud_ptr, _clip_min_height, _clip_max_height);

    // 4.0 裁剪雷达左右方向较远的点（行驶线两侧非路面上的物体）
    if (_keep_lanes)
      keepLanePoints(clipped_cloud_ptr, inlanes_cloud_ptr, _keep_lane_left_distance, _keep_lane_right_distance);
    else
      inlanes_cloud_ptr = clipped_cloud_ptr;

    // 5.0 去除地面点，并发布地面点和非地面点
    if (_remove_ground)
    {
      removeFloor(inlanes_cloud_ptr, nofloor_cloud_ptr, onlyfloor_cloud_ptr);
      publishCloud(&_pub_ground_cloud, onlyfloor_cloud_ptr);
    }
    else
    {
      nofloor_cloud_ptr = inlanes_cloud_ptr;
    }

    publishCloud(&_pub_points_lanes_cloud, nofloor_cloud_ptr);

    // 6.0 采用差分法线特征的算法再进行一次地面点去除
    if (_use_diffnormals)
      differenceNormalsSegmentation(nofloor_cloud_ptr, diffnormals_cloud_ptr);
    else
      diffnormals_cloud_ptr = nofloor_cloud_ptr;

    // 7.0 核心内容：点云聚类
    segmentByDistance(diffnormals_cloud_ptr, colored_clustered_cloud_ptr, centroids,
                      cloud_clusters);

    // 8.0 发布聚类结果
    publishColorCloud(&_pub_cluster_cloud, colored_clustered_cloud_ptr);

    centroids.header = _velodyne_header;

    publishCentroids(&_centroid_pub, centroids, _output_frame, _velodyne_header);

    cloud_clusters.header = _velodyne_header;

    publishCloudClusters(&_pub_clusters_message, cloud_clusters, _output_frame, _velodyne_header);

    _using_sensor_cloud = false;
  }
}

二、预处理

（1）裁剪距离雷达过于近的点云，消除车身的影响

cpp 复制代码

// 1.0 裁剪距离过于近的点云（默认是不需要的）
void removePointsUpTo(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                      pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, const double in_distance)
{
  out_cloud_ptr->points.clear();
  for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    float origin_distance = sqrt(pow(in_cloud_ptr->points[i].x, 2) + pow(in_cloud_ptr->points[i].y, 2));
    // 大于距离阈值的才要
    if (origin_distance > in_distance)
    {
      out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
    }
  }
}

（2）点云降采样（体素滤波，默认也是不需要的）

cpp 复制代码

// 2.0 点云降采样（默认也是不需要的）
void downsampleCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                     pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, float in_leaf_size = 0.2)
{
  // 体素滤波
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud(in_cloud_ptr);
  sor.setLeafSize((float)in_leaf_size, (float)in_leaf_size, (float)in_leaf_size);
  sor.filter(*out_cloud_ptr);
}

（3）裁剪雷达高度上下范围过远的点云，过高不会成为障碍物

cpp 复制代码

// 3.0 裁剪雷达高度上下范围过远的点云
void clipCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
               pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, float in_min_height = -1.3, float in_max_height = 0.5)
{
  out_cloud_ptr->points.clear();
  for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
  {
    // 只有在最低最高范围内的点才保留
    if (in_cloud_ptr->points[i].z >= in_min_height && in_cloud_ptr->points[i].z <= in_max_height)
    {
      out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
    }
  }
}

（4）裁剪雷达左右方向较远的点（行驶线两侧较远的非路面上的物体，没必要再聚类）

cpp 复制代码

// 4.0 裁剪雷达左右方向较远的点（行驶线两侧非路面上的物体）
void keepLanePoints(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, float in_left_lane_threshold = 1.5,
                    float in_right_lane_threshold = 1.5)
{
  pcl::PointIndices::Ptr far_indices(new pcl::PointIndices);
  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;

    if (current_point.y > (in_left_lane_threshold) || current_point.y < -1.0 * in_right_lane_threshold)
    {
      // 记录要去除的索引
      far_indices->indices.push_back(i);
    }
  }
  out_cloud_ptr->points.clear();
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud(in_cloud_ptr);
  extract.setIndices(far_indices);
  // 根据索引去除点云
  extract.setNegative(true); // true removes the indices, false leaves only the indices
  extract.filter(*out_cloud_ptr);
}

（5）调用pcl库去除地面点云，与ray不同，这里采用的是RANSAC地平面拟合

cpp 复制代码

// 5.0 去除地面点云
void removeFloor(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr out_nofloor_cloud_ptr,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr out_onlyfloor_cloud_ptr, float in_max_height = 0.2,
                 float in_floor_max_angle = 0.1)
{

  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

  // RANSAC拟合地平面
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations(100);
  seg.setAxis(Eigen::Vector3f(0, 0, 1));
  seg.setEpsAngle(in_floor_max_angle);

  seg.setDistanceThreshold(in_max_height); // floor distance
  seg.setOptimizeCoefficients(true);
  seg.setInputCloud(in_cloud_ptr);
  seg.segment(*inliers, *coefficients);
  if (inliers->indices.size() == 0)
  {
    std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
  }

  // 通过地平面模型去除非地面点
  // REMOVE THE FLOOR FROM THE CLOUD
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud(in_cloud_ptr);
  extract.setIndices(inliers);
  extract.setNegative(true); // true removes the indices, false leaves only the indices
  extract.filter(*out_nofloor_cloud_ptr);

  // EXTRACT THE FLOOR FROM THE CLOUD
  extract.setNegative(false); // true removes the indices, false leaves only the indices
  extract.filter(*out_onlyfloor_cloud_ptr);
}

（6）采用差分法线特征的算法再进行一次地面点去除

cpp 复制代码

// 6.0 采用差分法线特征的算法再进行一次地面点去除
void differenceNormalsSegmentation(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
                                   pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr)
{
  // 事先定义两个不同范围的支持半径用于向量计算
  float small_scale = 0.5;
  float large_scale = 2.0;
  float angle_threshold = 0.5;
  // 1.0 KD-TREE 根据点云类型（无序点云、有序点云）建立搜索树
  pcl::search::Search<pcl::PointXYZ>::Ptr tree;
  if (in_cloud_ptr->isOrganized()) // 有序点云
  {
    tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
  }
  else
  {
    tree.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
  }

  // Set the input pointcloud for the search tree
  tree->setInputCloud(in_cloud_ptr);

  // 2.0 求解法线向量信息 OpenMP标准并行估计每个3D点的局部表面属性。加入搜索树。
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::PointNormal> normal_estimation;
  // pcl::gpu::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> normal_estimation;
  normal_estimation.setInputCloud(in_cloud_ptr);
  normal_estimation.setSearchMethod(tree);

  // 设置视点源点，用于调整点云法向（指向视点），默认（0，0，0）
  normal_estimation.setViewPoint(std::numeric_limits<float>::max(), std::numeric_limits<float>::max(),
                                 std::numeric_limits<float>::max());

  pcl::PointCloud<pcl::PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<pcl::PointNormal>);
  pcl::PointCloud<pcl::PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<pcl::PointNormal>);

  // 3.0 计算法线数据 normals_small_scale/ normals_large_scale
  normal_estimation.setRadiusSearch(small_scale);
  normal_estimation.compute(*normals_small_scale);

  normal_estimation.setRadiusSearch(large_scale);
  normal_estimation.compute(*normals_large_scale);

  // 定义法向量并绑定点云 法线信息，创建DoN估计器。得到DoN特征向量diffnormals_cloud
  pcl::PointCloud<pcl::PointNormal>::Ptr diffnormals_cloud(new pcl::PointCloud<pcl::PointNormal>);
  pcl::copyPointCloud<pcl::PointXYZ, pcl::PointNormal>(*in_cloud_ptr, *diffnormals_cloud);

  // Create DoN operator
  pcl::DifferenceOfNormalsEstimation<pcl::PointXYZ, pcl::PointNormal, pcl::PointNormal> diffnormals_estimator;
  diffnormals_estimator.setInputCloud(in_cloud_ptr);
  diffnormals_estimator.setNormalScaleLarge(normals_large_scale);
  diffnormals_estimator.setNormalScaleSmall(normals_small_scale);

  diffnormals_estimator.initCompute();

  diffnormals_estimator.computeFeature(*diffnormals_cloud);

  // 4.0 曲率curvature 大于阀值angle_threshold 即认为满足条件。博客
  // 最后加入ConditionalRemoval中。这里应该是保留满足上述条件的法向量。得到过滤结果diffnormals_cloud_filtered注意这里得到的数据类型，需要转点云
  pcl::ConditionOr<pcl::PointNormal>::Ptr range_cond(new pcl::ConditionOr<pcl::PointNormal>());
  // //加入比较阀值 GT 大于, GE大于等于, LT 小于, LE小于等于, EQ等于
  range_cond->addComparison(pcl::FieldComparison<pcl::PointNormal>::ConstPtr(
      new pcl::FieldComparison<pcl::PointNormal>("curvature", pcl::ComparisonOps::GT, angle_threshold)));
  // Build the filter
  pcl::ConditionalRemoval<pcl::PointNormal> cond_removal;
  cond_removal.setCondition(range_cond);
  cond_removal.setInputCloud(diffnormals_cloud);

  pcl::PointCloud<pcl::PointNormal>::Ptr diffnormals_cloud_filtered(new pcl::PointCloud<pcl::PointNormal>);

  // Apply filter
  cond_removal.filter(*diffnormals_cloud_filtered);

  pcl::copyPointCloud<pcl::PointNormal, pcl::PointXYZ>(*diffnormals_cloud, *out_cloud_ptr);
}

三、核心内容：聚类

欧式聚类是一种基于欧氏距离度量的聚类算法。采用基于KD-Tree的近邻查询算法是加速欧式聚类。

（1）聚类主函数

有两种方法，一种是聚类最小阈值固定（0.5，不使用多线程的情况下），一种是根据距离分组，每组用不同的阈值（参数中两个数组的作用，一个是距离，一个是阈值）

cpp 复制代码

  // 根据距离的不同设置不同的聚类最小距离阈值（使用多线程的时候才会用）
  // 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<ClusterPtr> all_clusters;

  // 7.1 聚类
  // 使用多线程进行聚类（默认不使用：阈值均为0.5）
  if (!_use_multiple_thres)
  {
    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);
    }
#ifdef GPU_CLUSTERING
    // 使用GPU加速
    if (_use_gpu)
    {
      // 欧氏聚类
      all_clusters = clusterAndColorGpu(cloud_ptr, out_cloud_ptr, in_out_centroids,
                                        _clustering_distance);
    }
    else
    {
      all_clusters =
          clusterAndColor(cloud_ptr, out_cloud_ptr, in_out_centroids, _clustering_distance);
    }
#else
    // 使用CPU
    all_clusters =
        clusterAndColor(cloud_ptr, out_cloud_ptr, in_out_centroids, _clustering_distance);
#endif
  }
  // 使用多阈值进行聚类的时候，根据距离分组，不同组的阈值不同
  else
  {
    // 定义五组点云并初始化
    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
      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
      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());
    }
  }

（2）2D聚类：先将3D点投影到2D，进行聚类获取聚类索引

cpp 复制代码

// 7.1 获取聚类的点云
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上，因为障碍物不需要考虑高度，而且高低障碍物已经裁剪了
  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;
  }

  // 构建KD-tree
  if (cloud_2d->points.size() > 0)
    tree->setInputCloud(cloud_2d);

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

  // perform clustering on 2d cloud
  // 调用pcl进行欧氏聚类
  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);
  // 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)
  {
    ClusterPtr cluster(new Cluster());
    // 根据刚刚聚类的索引，将每一类点分到对应的类别中
    // 每一类给它一种颜色
    cluster->SetCloud(in_cloud_ptr,
                      it->indices,
                      _velodyne_header,
                      k,                      // 类的ID
                      (int)_colors[k].val[0], // 类的颜色RGB
                      (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;
}

（3）3D聚类：根据2D聚类索引获取3D聚类以及它的一些其他特征（中心点、包围盒、特征值和特征向量）

cpp 复制代码

// 7.1.1 根据输入的点云以及聚类的索引，得到每一类（更详细的信息）
void Cluster::SetCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_origin_cloud_ptr,
                       const std::vector<int> &in_cluster_indices, std_msgs::Header in_ros_header, int in_id, int in_r,
                       int in_g, int in_b, std::string in_label, bool in_estimate_pose)
{
  label_ = in_label;
  id_ = in_id;
  r_ = in_r;
  g_ = in_g;
  b_ = in_b;
  // extract pointcloud using the indices
  // calculate min and max points
  // 计算边界盒用到的
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr current_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
  float min_x = std::numeric_limits<float>::max();
  float max_x = -std::numeric_limits<float>::max();
  float min_y = std::numeric_limits<float>::max();
  float max_y = -std::numeric_limits<float>::max();
  float min_z = std::numeric_limits<float>::max();
  float max_z = -std::numeric_limits<float>::max();
  float average_x = 0, average_y = 0, average_z = 0;
  // 遍历索引
  for (auto pit = in_cluster_indices.begin(); pit != in_cluster_indices.end(); ++pit)
  {
    // fill new colored cluster point by point
    pcl::PointXYZRGB p;
    p.x = in_origin_cloud_ptr->points[*pit].x;
    p.y = in_origin_cloud_ptr->points[*pit].y;
    p.z = in_origin_cloud_ptr->points[*pit].z;
    p.r = in_r;
    p.g = in_g;
    p.b = in_b;

    // 累加：统计，后面要求均值
    average_x += p.x;
    average_y += p.y;
    average_z += p.z;
    centroid_.x += p.x;
    centroid_.y += p.y;
    centroid_.z += p.z;
    current_cluster->points.push_back(p);

    // 交换边界
    if (p.x < min_x)
      min_x = p.x;
    if (p.y < min_y)
      min_y = p.y;
    if (p.z < min_z)
      min_z = p.z;
    if (p.x > max_x)
      max_x = p.x;
    if (p.y > max_y)
      max_y = p.y;
    if (p.z > max_z)
      max_z = p.z;
  }
  // min, max points
  min_point_.x = min_x;
  min_point_.y = min_y;
  min_point_.z = min_z;
  max_point_.x = max_x;
  max_point_.y = max_y;
  max_point_.z = max_z;

  // calculate centroid, average 计算均值（中心点）
  if (in_cluster_indices.size() > 0)
  {
    centroid_.x /= in_cluster_indices.size();
    centroid_.y /= in_cluster_indices.size();
    centroid_.z /= in_cluster_indices.size();

    average_x /= in_cluster_indices.size();
    average_y /= in_cluster_indices.size();
    average_z /= in_cluster_indices.size();
  }

  average_point_.x = average_x;
  average_point_.y = average_y;
  average_point_.z = average_z;

  // calculate bounding box 计算包围盒（根据坐标最值）
  length_ = max_point_.x - min_point_.x;
  width_ = max_point_.y - min_point_.y;
  height_ = max_point_.z - min_point_.z;

  bounding_box_.header = in_ros_header;
  // 包围盒的位置（四棱柱的中心 = 最小值 + 边长/2）
  bounding_box_.pose.position.x = min_point_.x + length_ / 2;
  bounding_box_.pose.position.y = min_point_.y + width_ / 2;
  bounding_box_.pose.position.z = min_point_.z + height_ / 2;
  // 各平面面积？
  bounding_box_.dimensions.x = ((length_ < 0) ? -1 * length_ : length_);
  bounding_box_.dimensions.y = ((width_ < 0) ? -1 * width_ : width_);
  bounding_box_.dimensions.z = ((height_ < 0) ? -1 * height_ : height_);

  // pose estimation
  // 使用OpenCV库计算凸包：生成2D多边形以及获取旋转包围框的过程。
  double rz = 0;
  {
    std::vector<cv::Point2f> points;
    for (unsigned int i = 0; i < current_cluster->points.size(); i++)
    {
      cv::Point2f pt;
      pt.x = current_cluster->points[i].x;
      pt.y = current_cluster->points[i].y;
      points.push_back(pt);
    }

    std::vector<cv::Point2f> hull;
    // 将给定的点云数据 points 计算凸包，结果存储在 hull 中。
    cv::convexHull(points, hull);

    polygon_.header = in_ros_header;
    for (size_t i = 0; i < hull.size() + 1; i++)
    {
      geometry_msgs::Point32 point;
      point.x = hull[i % hull.size()].x;
      point.y = hull[i % hull.size()].y;
      point.z = min_point_.z;
      // 外接多边形顶点
      polygon_.polygon.points.push_back(point);
    }

    if (in_estimate_pose)
    {
      // 使用 minAreaRect(hull) 函数计算凸包最小斜矩形
      // minAreaRect()返回的是包含轮廓的最小斜矩形(有方向的):如下图所示
      // 角度是在（-90，0）之间的，在opencv上图片（右手系）的原点是在左上角的，所以它是逆时针旋转的，故此它的角度是 < 0的
      // 逆时针旋转第一条边与x轴的夹角就是矩阵的旋转角度。并且矩阵的旋转角度是与矩阵的长宽是没有任何关系的！！！！！！彬彬彬
      cv::RotatedRect box = minAreaRect(hull);
      rz = box.angle * 3.14 / 180;
      bounding_box_.pose.position.x = box.center.x;
      bounding_box_.pose.position.y = box.center.y;
      bounding_box_.dimensions.x = box.size.width;
      bounding_box_.dimensions.y = box.size.height;
      
      // ZARD：添加计算聚类朝向的代码
      orientation_angle_ = rz;
    }
  }

  // set bounding box direction 只考虑2D（yaw）
  tf::Quaternion quat = tf::createQuaternionFromRPY(0.0, 0.0, rz);
  tf::quaternionTFToMsg(quat, bounding_box_.pose.orientation);

  current_cluster->width = current_cluster->points.size();
  current_cluster->height = 1;
  current_cluster->is_dense = true;

  // Get EigenValues, eigenvectors
  // 通过特征值和特征向量获得几何特征----PCA主成分分析
  if (current_cluster->points.size() > 3)
  {
    pcl::PCA<pcl::PointXYZ> current_cluster_pca;
    pcl::PointCloud<pcl::PointXYZ>::Ptr current_cluster_mono(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZ>(*current_cluster, *current_cluster_mono);

    current_cluster_pca.setInputCloud(current_cluster_mono);
    eigen_vectors_ = current_cluster_pca.getEigenVectors();
    eigen_values_ = current_cluster_pca.getEigenValues();
  }

  valid_cluster_ = true;
  pointcloud_ = current_cluster;
}

（4）聚类融合

cpp 复制代码

 // 7.2 对当前聚类进行两次检查，可以融合的就进行融合
  // 设置不同半径阈值进行聚类获取获得目标轮廓的点云簇，
  // 由于采用不同半径阈值聚类，可能会把一个物体分割成多个，需要对不同的点云簇进行merge。
  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;

cpp 复制代码

// 7.2 对当前聚类进行两次检查，可以融合的就进行融合
void checkAllForMerge(std::vector<ClusterPtr> &in_clusters, std::vector<ClusterPtr> &out_clusters,
                      float in_merge_threshold)
{
  // std::cout << "checkAllForMerge" << std::endl;
  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;
      // 获取融合的ID
      checkClusterMerge(i, in_clusters, visited_clusters, merge_indices, in_merge_threshold);
      // 根据ID融合
      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());
}

cpp 复制代码

// 7.2.1 递归：获取融合的ID
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);
      }
    }
  }
}

cpp 复制代码

// 7.2.2 聚类合并
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++)
  {
    sum_cloud += *(in_clusters[in_merge_indices[i]]->GetCloud());
    in_out_merged_clusters[in_merge_indices[i]] = true;
  }
  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);
  }
}

（5）聚类结果处理

cpp 复制代码

// 7.3 获取聚类结果并准备发布
  // Get final PointCloud to be published
  // 遍历每一类
  for (unsigned int i = 0; i < final_clusters.size(); i++)
  {
      // pcl形式的点云（一整帧）
    *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的模式设置为字符串模式。这意味着每个簇将用一个字符串来表示。
    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);
    // ZARD：使用包围盒的试试
    pictogram_cluster.pose.orientation = bounding_box.pose.orientation;

    // 设置簇的大小为4
    pictogram_cluster.size = 4;
    std_msgs::ColorRGBA color;
    // 设置颜色对象的alpha、red、green和blue分量值，此处为将颜色设置为白色。
    color.a = 1;
    color.r = 1;
    color.g = 1;
    color.b = 1;
    // 将颜色对象分配给Pictogram Cluster的颜色属性。
    pictogram_cluster.color = color;
    // 将簇索引转换为字符串，并将其分配给Pictogram Cluster的字符属性。这样可以在可视化时识别不同的簇。
    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);
      // 转化为ROS消息
      autoware_msgs::CloudCluster cloud_cluster;
      final_clusters[i]->ToROSMessage(_velodyne_header, cloud_cluster);

      in_out_clusters.clusters.push_back(cloud_cluster);
    }
  }

cpp 复制代码

// 7.3.1 构建ROS消息
void Cluster::ToROSMessage(std_msgs::Header in_ros_header, autoware_msgs::CloudCluster &out_cluster_message)
{
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(*(this->GetCloud()), cloud_msg);
  cloud_msg.header = in_ros_header;

  out_cluster_message.header = in_ros_header;

  out_cluster_message.cloud = cloud_msg;
  // 坐标最值
  out_cluster_message.min_point.header = in_ros_header;
  out_cluster_message.min_point.point.x = this->GetMinPoint().x;
  out_cluster_message.min_point.point.y = this->GetMinPoint().y;
  out_cluster_message.min_point.point.z = this->GetMinPoint().z;

  out_cluster_message.max_point.header = in_ros_header;
  out_cluster_message.max_point.point.x = this->GetMaxPoint().x;
  out_cluster_message.max_point.point.y = this->GetMaxPoint().y;
  out_cluster_message.max_point.point.z = this->GetMaxPoint().z;

  // 平均值
  out_cluster_message.avg_point.header = in_ros_header;
  out_cluster_message.avg_point.point.x = this->GetAveragePoint().x;
  out_cluster_message.avg_point.point.y = this->GetAveragePoint().y;
  out_cluster_message.avg_point.point.z = this->GetAveragePoint().z;

  // 中心点
  out_cluster_message.centroid_point.header = in_ros_header;
  out_cluster_message.centroid_point.point.x = this->GetCentroid().x;
  out_cluster_message.centroid_point.point.y = this->GetCentroid().y;
  out_cluster_message.centroid_point.point.z = this->GetCentroid().z;

  // 朝向，根本就没算过这个值？？？？？？
  out_cluster_message.estimated_angle = this->GetOrientationAngle();

  // 就是包围盒的面积
  out_cluster_message.dimensions = this->GetBoundingBox().dimensions;

  // 包围盒与外接多边形
  out_cluster_message.bounding_box = this->GetBoundingBox();

  out_cluster_message.convex_hull = this->GetPolygon();

  // 特征值与特征向量
  Eigen::Vector3f eigen_values = this->GetEigenValues();
  out_cluster_message.eigen_values.x = eigen_values.x();
  out_cluster_message.eigen_values.y = eigen_values.y();
  out_cluster_message.eigen_values.z = eigen_values.z();

  Eigen::Matrix3f eigen_vectors = this->GetEigenVectors();
  for (unsigned int i = 0; i < 3; i++)
  {
    geometry_msgs::Vector3 eigen_vector;
    eigen_vector.x = eigen_vectors(i, 0);
    eigen_vector.y = eigen_vectors(i, 1);
    eigen_vector.z = eigen_vectors(i, 2);
    out_cluster_message.eigen_vectors.push_back(eigen_vector);
  }

  /*std::vector<float> fpfh_descriptor = GetFpfhDescriptor(8, 0.3, 0.3);
  out_cluster_message.fpfh_descriptor.data = fpfh_descriptor;*/
}

四、发布聚类结果

cpp 复制代码

// 8.1 发布一帧的聚类点云（颜色不同）
void publishColorCloud(const ros::Publisher *in_publisher,
                       const pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud_to_publish_ptr)
{
  sensor_msgs::PointCloud2 cloud_msg;
  pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
  cloud_msg.header = _velodyne_header;
  in_publisher->publish(cloud_msg);
}

cpp 复制代码

// 8.2 发布聚类中心坐标
void publishCentroids(const ros::Publisher *in_publisher, const autoware_msgs::Centroids &in_centroids,
                      const std::string &in_target_frame, const std_msgs::Header &in_header)
{
  if (in_target_frame != in_header.frame_id)
  {
    autoware_msgs::Centroids centroids_transformed;
    centroids_transformed.header = in_header;
    centroids_transformed.header.frame_id = in_target_frame;
    // ZARD：新变量里面并没有任何点，发布了个寂寞？？？？？？
    // for (auto i = centroids_transformed.points.begin(); i != centroids_transformed.points.end(); i++)
    // 改一下
    for (auto i = in_centroids.points.begin(); i != in_centroids.points.end(); i++)
    {
      geometry_msgs::PointStamped centroid_in, centroid_out;
      centroid_in.header = in_header;
      centroid_in.point = *i;
      try
      {
        // 转换到目标坐标系
        _transform_listener->transformPoint(in_target_frame, ros::Time(), centroid_in, in_header.frame_id,
                                            centroid_out);

        centroids_transformed.points.push_back(centroid_out.point);
      }
      catch (tf::TransformException &ex)
      {
        ROS_ERROR("publishCentroids: %s", ex.what());
      }
    }
    // 发布
    in_publisher->publish(centroids_transformed);
  }
  else
  {
    in_publisher->publish(in_centroids);
  }
}

cpp 复制代码

// 8.3 发布聚类（Autoware消息类型）
void publishCloudClusters(const ros::Publisher *in_publisher, const autoware_msgs::CloudClusterArray &in_clusters,
                          const std::string &in_target_frame, const std_msgs::Header &in_header)
{
  if (in_target_frame != in_header.frame_id)
  {
    autoware_msgs::CloudClusterArray clusters_transformed;
    clusters_transformed.header = in_header;
    clusters_transformed.header.frame_id = in_target_frame;

    // ZARD：发布包围盒看看
    jsk_recognition_msgs::BoundingBoxArray boxes_array;
    boxes_array.header = in_header;
    boxes_array.header.frame_id = in_target_frame;

    // 遍历每一类
    for (auto i = in_clusters.clusters.begin(); i != in_clusters.clusters.end(); i++)
    {
      autoware_msgs::CloudCluster cluster_transformed;
      cluster_transformed.header = in_header;
      try
      {
        // 监听TF
        _transform_listener->lookupTransform(in_target_frame, _velodyne_header.frame_id, ros::Time(),
                                             *_transform);
        // 将点云转换到目标坐标系
        pcl_ros::transformPointCloud(in_target_frame, *_transform, i->cloud, cluster_transformed.cloud);
        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->min_point, in_header.frame_id,
                                            cluster_transformed.min_point);
        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->max_point, in_header.frame_id,
                                            cluster_transformed.max_point);
        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->avg_point, in_header.frame_id,
                                            cluster_transformed.avg_point);
        _transform_listener->transformPoint(in_target_frame, ros::Time(), i->centroid_point, in_header.frame_id,
                                            cluster_transformed.centroid_point);
        // 面积
        cluster_transformed.dimensions = i->dimensions;
        // 特征向量以及特征值
        cluster_transformed.eigen_values = i->eigen_values;
        cluster_transformed.eigen_vectors = i->eigen_vectors;
        // 凸包以及包围盒位姿
        cluster_transformed.convex_hull = i->convex_hull;
        cluster_transformed.bounding_box.pose.position = i->bounding_box.pose.position;

        // ZARD：添加朝向
        cluster_transformed.estimated_angle = i->estimated_angle;

        // ZARD：发布包围盒看看
        jsk_recognition_msgs::BoundingBox box;
        box.header = in_header;
        box.pose.position = i->bounding_box.pose.position;
        box.value = 0.9;
        box.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, i->estimated_angle);
        box.dimensions = i->bounding_box.dimensions;
        boxes_array.boxes.push_back(box);

        if (_pose_estimation)
        {
          cluster_transformed.bounding_box.pose.orientation = i->bounding_box.pose.orientation;
        }
        else
        {
          cluster_transformed.bounding_box.pose.orientation.w = _initial_quat_w;
        }
        clusters_transformed.clusters.push_back(cluster_transformed);
      }
      catch (tf::TransformException &ex)
      {
        ROS_ERROR("publishCloudClusters: %s", ex.what());
      }
    }
    // 发布聚类结果
    in_publisher->publish(clusters_transformed);
    publishDetectedObjects(clusters_transformed);

    // ZARD：发布包围盒看看
    pub_TrackedObstaclesRviz.publish(boxes_array);
  }
  else
  {
    in_publisher->publish(in_clusters);
    publishDetectedObjects(in_clusters);
  }
}

cpp 复制代码

// 8.3.1 发布聚类结果
void publishDetectedObjects(const autoware_msgs::CloudClusterArray &in_clusters)
{
  autoware_msgs::DetectedObjectArray detected_objects;
  detected_objects.header = in_clusters.header;

  for (size_t i = 0; i < in_clusters.clusters.size(); i++)
  {
    // 并没有发布包围盒
    autoware_msgs::DetectedObject detected_object;
    detected_object.header = in_clusters.header;
    detected_object.label = "unknown";
    detected_object.score = 1.;
    detected_object.space_frame = in_clusters.header.frame_id;
    detected_object.pose = in_clusters.clusters[i].bounding_box.pose;
    detected_object.dimensions = in_clusters.clusters[i].dimensions;
    detected_object.pointcloud = in_clusters.clusters[i].cloud;
    detected_object.convex_hull = in_clusters.clusters[i].convex_hull;
    detected_object.valid = true;

    detected_objects.objects.push_back(detected_object);
  }
  _pub_detected_objects.publish(detected_objects);
}

五、runtime_manager聚类参数解析

通过上述源码分析，就很容易看出界面里参数的含义，具体参数含义如下：

①　use_gpu：是否使用GPU，选择使用，否则聚类十分慢；

②　output_frame：输出坐标系，改为map；

③　pose_estimation：需要估计聚类点云最小面积边界矩形的姿态；

④　downsample_cloud：点云通过VoxelGrid滤波器进行下采样；

⑤　input_points_node：输入点云topic，选择/points_no_ground或/point_raw；

⑥　leaf size：下采样体素网格大小；

⑦　cluster size minimum：聚类的最少点数（调小一点能聚到更远的类）；

⑧　cluster size maximum：聚类的最多点数；

⑨　clustering distance：聚类公差（同一类两点最大距离阈值，调大一点能聚到更远的类，但是太大会造成明显的不同类聚到一起m）；

⑩　clip_min_height：裁剪的最小高度（此仿真雷达高大约2.5，因此-2.5）；

⑪　clip_max_height：裁剪的最大高度（高于雷达多少）；

⑫　use_vector_map：是否使用矢量地图；

⑬　vectormap_frame：矢量地图坐标系；

⑭　remove_points_upto：距离激光雷达这个距离过近的点将被移除（最大只能2.5，因此车身过大不要用，需要输入去除地面点点云）；

⑮　keep_only_lanes_points：行驶线边（远）滤波；

⑯　keep_lane_left_distance：向左移除该距离以外的点 (m)（算力强可以设大点）；

⑰　keep_lane_right_distance：向右移除该距离以外的点 (m)；

⑱　cluster_merge_threshold：聚类簇间的距离阈值(m)；

⑲　use_multiple_thres：是否使用多级阈值进行聚类（使用下面两个参数）；

⑳　clustering_ranges：不同的距离有不同的聚类阈值 (m)；

21　clustering_distances：聚类公差(同一类两个点的最大距离阈值，与clustering_ranges对应)；

22　remove ground：地平面滤波（移除属于地面的点）；

23　use_diffnormals：采用正态差值滤波再去除一次地面点。

图1 聚类参数（使用已经去除地面的点云）

图2 聚类参数（使用原始扫描点云，此时不需要上面的ground_filter节点）

图3 欧几里得聚类

Autoware感知02—欧氏聚类（lidar_euclidean_cluster_detect）源码解析

文章目录

引言

一、点云回调函数：

二、预处理

（1）裁剪距离雷达过于近的点云，消除车身的影响

（2）点云降采样（体素滤波，默认也是不需要的）

（3）裁剪雷达高度上下范围过远的点云，过高不会成为障碍物

（4）裁剪雷达左右方向较远的点（行驶线两侧较远的非路面上的物体，没必要再聚类）

（5）调用pcl库去除地面点云，与ray不同，这里采用的是RANSAC地平面拟合

（6）采用差分法线特征的算法再进行一次地面点去除

三、核心内容：聚类

（1）聚类主函数

（2）2D聚类：先将3D点投影到2D，进行聚类获取聚类索引

（3）3D聚类：根据2D聚类索引获取3D聚类以及它的一些其他特征（中心点、包围盒、特征值和特征向量）

（4）聚类融合

（5）聚类结果处理

四、发布聚类结果

五、runtime_manager聚类参数解析