Directorio de artículos

introducción
1. Función de devolución de llamada de nube de puntos:
2. Pretratamiento
3. Contenido básico: agrupamiento
4. Publicar los resultados de la agrupación
5. Análisis de los parámetros de agrupación en clúster de runtime_manager

introducción

En la capa de percepción de conducción autónoma, la agrupación de nubes de puntos es de gran importancia. El agrupamiento de nubes de puntos es el proceso de agrupar o clasificar datos de nubes de puntos 3D y clasificar puntos con distancias más cercanas en una categoría. Estos son algunos significados de la agrupación de nubes de puntos:

障碍物检测与跟踪: La agrupación de nubes de puntos puede ayudar a identificar y distinguir diferentes objetos en el entorno, especialmente obstáculos. A través de la agrupación, los puntos del mismo obstáculo se pueden clasificar en una categoría y se puede estimar su posición, forma y tamaño, para realizar la detección y el seguimiento de obstáculos.
建立环境模型: A través de la agrupación de nubes de puntos, los datos de la nube de puntos se pueden dividir en diferentes grupos de objetos y se pueden extraer las características de estos grupos. Estas características se pueden usar para construir modelos ambientales, incluidos caminos, edificios, señales de tránsito, etc., para brindar información ambiental más precisa y detallada para vehículos autónomos.
动态物体检测: La agrupación de nubes de puntos puede ayudar a identificar y distinguir entre objetos estables y dinámicos. Al agrupar los datos de la nube de puntos, los objetos en movimiento se pueden detectar y diferenciar de entornos estables. Esto es fundamental para la conducción autónoma, ya que ayuda a los vehículos a predecir y adaptarse al comportamiento de los objetos dinámicos.
传感器融合: Los sistemas de piloto automático suelen utilizar muchos tipos diferentes de sensores, como lidar, cámara, etc. El agrupamiento de nubes de puntos puede fusionar datos de nubes de puntos adquiridos por múltiples sensores para proporcionar una percepción del entorno más integral y consistente.

En general, la importancia de la agrupación de nubes de puntos en la capa de percepción de la conducción autónoma es ayudar a convertir los datos de la nube de puntos 3D en información de objetos más manejable y proporcionar información para tareas como detección de obstáculos, modelado de entornos, detección de objetos dinámicos y detección de sensores. Fusión Proporcionar datos básicos y precisión. Es uno de los pasos clave para hacer realidad la percepción ambiental y la toma de decisiones de la conducción autónoma.
El algoritmo de agrupamiento euclidiano es uno de los algoritmos de agrupamiento de nubes de puntos más utilizados en Autoware. Este algoritmo se basa en la distancia euclidiana para medir la similitud entre puntos. En primer lugar, la nube de puntos se divide en grupos de nubes de puntos individuales. Luego, al calcular la distancia euclidiana entre cada punto y sus vecinos, el grupo de nubes de puntos crece hasta que se alcanza un cierto umbral de distancia para formar un grupo completo. En última instancia, cada grupo se trata como un objeto independiente. El algoritmo de agrupamiento euclidiano es simple e intuitivo y puede ayudar a identificar obstáculos. Los siguientes son
varios problemas encontrados en el código de lectura combinado (autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect) y cómo solucionarlos:

El logotipo de agrupación se creó, pero no se publicó (se supone que kf también se publicó, por lo que no lo publicaré para evitar repeticiones, pero el código no se eliminó), y las cosas sin importancia no se han cambiado.
Hay un valor de orientación de agrupamiento (variable miembro de clase), ¡pero esta variable no se ha calculado! ! ! Agregar: Calcule la orientación a través del cuadro delimitador mínimo y asigne valores a las variables miembro;
El código de liberación del punto central de agrupación se escribió incorrectamente y no se liberó, ¡corríjalo! ! !
El cuadro delimitador se calcula pero no hay salida, agregue: liberación del cuadro delimitador (tema: /detección/tracked_boxes, utilizando la orientación calculada anteriormente, el resultado es muy preciso y estable, como se muestra a continuación):

1. Función de devolución de llamada de nube de puntos:

Ingrese al programa principal a través de la función de devolución de llamada de nube de puntos suscrita, y podrá ver que todo el nodo se divide en tres pasos: preprocesamiento, agrupación y publicación de resultados. Consulte las notas para el proceso específico:

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;
  }
}

2. Pretratamiento

(1) Recortar la nube de puntos que está demasiado cerca del radar para eliminar la influencia del cuerpo

// 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) Reducción de resolución de la nube de puntos (filtrado de vóxeles, que no se requiere de forma predeterminada)

// 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) Recortar la nube de puntos cuya altura de radar es demasiado hacia arriba y hacia abajo, para que no se convierta en un obstáculo si es demasiado alta

// 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) Recorte los puntos más lejanos en las direcciones izquierda y derecha del radar (los objetos en la superficie sin carretera que están más alejados a ambos lados de la línea de conducción, no es necesario volver a agruparlos)

// 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) Llame a la biblioteca pcl para eliminar la nube de puntos de tierra.A diferencia de los rayos, aquí se usa el ajuste del plano de tierra RANSAC

// 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) Use el algoritmo de características normales diferenciales para realizar nuevamente la eliminación del punto de tierra

// 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);
}

3. Contenido básico: agrupamiento

El agrupamiento euclidiano es un algoritmo de agrupamiento basado en la medida de distancia euclidiana. El algoritmo de consulta del vecino más cercano basado en KD-Tree se utiliza para acelerar la agrupación en clústeres europea.

(1) Función principal de agrupamiento

Hay dos métodos, uno es que el umbral mínimo de agrupamiento es fijo (0.5, sin usar subprocesos múltiples), el otro es agrupar según la distancia, cada grupo usa un umbral diferente (el papel de los dos arreglos en el parámetro, uno es la distancia, uno es el umbral)

  // 根据距离的不同设置不同的聚类最小距离阈值（使用多线程的时候才会用）
  // 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) agrupamiento 2D: primero proyecte puntos 3D a 2D, realice agrupamiento para obtener el índice de agrupamiento

// 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) Agrupamiento 3D: obtenga el agrupamiento 3D y algunas de sus otras características (punto central, cuadro delimitador, valor propio y vector propio) de acuerdo con el índice de agrupamiento 2D

// 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) Fusión de racimo

 // 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;

// 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());
}

// 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);
      }
    }
  }
}

// 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) Procesamiento de resultados de agrupamiento

// 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);
    }
  }

// 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;*/
}

4. Publicar los resultados de la agrupación

// 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);
}

// 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);
  }
}

// 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);
  }
}

// 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);
}

5. Análisis de los parámetros de agrupación en clúster de runtime_manager

A través del análisis del código fuente anterior, es fácil ver el significado de los parámetros en la interfaz. El significado de los parámetros específicos es el siguiente:
① use_gpu: si usa GPU, elija usarla, de lo contrario, la agrupación es muy lenta ;
② output_frame: sistema de coordenadas de salida, cambio a mapa;
③ pose_estimation : es necesario estimar la pose del rectángulo límite del área mínima de la nube de puntos agrupada;
④ downsample_cloud: la nube de puntos se reduce a través del filtro VoxelGrid ;
⑤ input_points_node: ingrese el tema de la nube de puntos, seleccione /points_no_ground o /point_raw;
⑥ tamaño de la hoja: reducción del tamaño de la cuadrícula del vóxel;
⑦ tamaño mínimo del clúster: el número mínimo de puntos para el agrupamiento (reduzca el número de clústeres más lejos);
⑧ tamaño del clúster máximo: el número máximo de puntos para agrupar;
⑨ distancia de agrupamiento: tolerancia de agrupamiento (dos puntos de la misma clase) El umbral de distancia máxima, si lo aumenta un poco, puede reunir más clases, pero si es demasiado grande, hacen que diferentes clases obvias se reúnan m); ⑩ clip_min_height
: la altura mínima de recorte (este radar de simulación tiene una altura de aproximadamente 2,5, por lo tanto, -2,5);
⑪ clip_max_height: la altura máxima de recorte (cuánto más alto que el radar);
⑫ use_vector_map: si usar mapa vectorial; ⑬
vectormap_frame: sistema de coordenadas de mapa vectorial; el lado (lejos) de la línea de conducción;

⑯ keep_lane_left_distance: elimine los puntos más allá de la distancia a la izquierda (m) (la potencia de cómputo se puede configurar más grande);
⑰ keep_lane_right_distance: elimine los puntos más allá de la distancia a la derecha (m);
⑱ cluster_merge_threshold: la distancia entre los clústeres Threshold ( m);
⑲ use_multiple_thres: si se debe usar un umbral multinivel para la agrupación (use los siguientes dos parámetros);
⑳ clustering_ranges: diferentes distancias tienen diferentes umbrales de agrupación (m);
21 clustering_distances: tolerancia de agrupación (dos El umbral de distancia máxima de puntos, correspondiente a clustering_ranges);
22 remove ground: filtrado del plano de tierra (elimina los puntos que pertenecen al suelo);
23 use_diffnormals: usa el filtrado de diferencia normal para eliminar puntos de tierra nuevamente.
inserte la descripción de la imagen aquí
Figura 1 Parámetros de agrupamiento (utilizando la nube de puntos que se ha eliminado del suelo)

inserte la descripción de la imagen aquí
Figura 2 Parámetros de agrupamiento (utilizando la nube de puntos escaneada original, el nodo de filtro de suelo anterior no es necesario en este momento)

inserte la descripción de la imagen aquí
Figura 3 Agrupamiento euclidiano

Percepción de Autoware 02: análisis de código fuente de agrupamiento euclidiano (lidar_euclidean_cluster_detect)