Autoware感知02—欧氏聚类(lidar_euclidean_cluster_detect)源码解析

news2025/4/6 21:04:46

文章目录

  • 引言
  • 一、点云回调函数:
  • 二、预处理
    • (1)裁剪距离雷达过于近的点云,消除车身的影响
    • (2)点云降采样(体素滤波,默认也是不需要的)
    • (3)裁剪雷达高度上下范围过远的点云,过高不会成为障碍物
    • (4)裁剪雷达左右方向较远的点(行驶线两侧较远的非路面上的物体,没必要再聚类)
    • (5)调用pcl库去除地面点云,与ray不同,这里采用的是RANSAC地平面拟合
    • (6)采用差分法线特征的算法再进行一次地面点去除
  • 三、核心内容:聚类
    • (1)聚类主函数
    • (2)2D聚类:先将3D点投影到2D,进行聚类获取聚类索引
    • (3)3D聚类:根据2D聚类索引获取3D聚类以及它的一些其他特征(中心点、包围盒、特征值和特征向量)
    • (4)聚类融合
    • (5)聚类结果处理
  • 四、发布聚类结果
  • 五、runtime_manager聚类参数解析


引言

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

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

总的来说,点云聚类在自动驾驶感知层中的意义在于帮助将三维点云数据转化为更易处理的物体信息,并为障碍物检测、环境建模、动态物体检测和传感器融合等任务提供基础数据和准确性。它是实现自动驾驶环境感知与决策的关键步骤之一。
Euclidean聚类算法是Autoware中常用的点云聚类算法之一。这种算法基于欧氏距离度量点之间的相似性。首先,将点云分成一个个独立的点云簇。然后,通过计算每个点与相邻点之间的欧氏距离,对点云簇进行增长,直到满足一定的距离阈值,形成一个完整的聚类。最终,每个聚类被视为一个独立的物体。Euclidean聚类算法简单直观,可以帮助识别障碍物。下面是其源码梳理(autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect)
读代码发现的几个问题以及修改办法:

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

一、点云回调函数:

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

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)裁剪距离雷达过于近的点云,消除车身的影响

// 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)点云降采样(体素滤波,默认也是不需要的)

// 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)裁剪雷达高度上下范围过远的点云,过高不会成为障碍物

// 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)裁剪雷达左右方向较远的点(行驶线两侧较远的非路面上的物体,没必要再聚类)

// 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地平面拟合

// 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)采用差分法线特征的算法再进行一次地面点去除

// 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,不使用多线程的情况下),一种是根据距离分组,每组用不同的阈值(参数中两个数组的作用,一个是距离,一个是阈值)

  // 根据距离的不同设置不同的聚类最小距离阈值(使用多线程的时候才会用)
  // 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,进行聚类获取聚类索引

// 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聚类以及它的一些其他特征(中心点、包围盒、特征值和特征向量)

// 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)聚类融合

 // 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)聚类结果处理

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

四、发布聚类结果

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

五、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 欧几里得聚类

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/877396.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

linux shell快速入门

linux shell快速入门 0 、前置1、简单使用 0 、前置 一安装linux的虚拟环境 1、简单使用 1、新建/usr/shell目录 2、新建hello.sh 文件 3、编写脚本文件# !/bin/bashecho "hello world"查看是否具备执行权限 新增执行权限 chomd x hello.sh执行hello.sh文件 /b…

AI大模型自动生成PowerPoint(PPT)

1&#xff0c;使用现有开源大模型工具&#xff0c;生成markdown语言文件&#xff08;我这里使用chatGLM2-6B&#xff09; eg&#xff0c;请用Markdown语言生成一个大纲&#xff0c;主题是&#xff1a;给小白用户的第一课&#xff0c;如何快速的学好Python markdown语言文本如下…

[C++ 网络协议编程] UDP协议

目录 1. UDP和TCP的区别 2. UDP的工作原理 3. UDP存在数据边界 4. UDP的I/O函数 4.1 sendto函数 4.2 recvfrom函数 4. 已连接(connected)UDP套接字和未连接(unconnected)UDP套接字 5. UDP的通信流程 5.1 服务器端通信流程 5.2 客户端通信流程 1. UDP和TCP的区别 主要…

电脑系统重装日记

重装原因 电脑C盘几乎爆炸故重装系统一清二白 此片原因 记录重装过程&#xff0c;强调一些要注意的点&#xff0c;以防日后重装。 重装过程 1.清空电脑文件后重启&#xff0c;电脑冒蓝光&#xff0c;一直蓝屏反复重启&#xff0c;故只能重装系统以解难题。 2.准备一个U盘&…

英语翻译,纯人工翻译哪里比较专业?

我们知道&#xff0c;人类语言是复杂的&#xff0c;给定的单词或短语含义会根据上下文内容和目标受众的不同而有所不同。机器翻译的准确率比较低。而专业的人工翻译是保持原文真实、原意的唯一保证。那么&#xff0c;针对英语翻译&#xff0c;如何做好人工翻译&#xff0c;北京…

app专项测试(网络测试流程)

目录 一、网络测试的一般流程 二、背景介绍 三、弱网功能测试 四、弱网UI测试 五、无网状态测试 六、网络切换测试 七、用户体验关注 八、响应时间 九、异常测试 一、网络测试的一般流程 step1&#xff1a;首先要考虑网络正常的情况 ① 各个模块的功能正常可用 ②…

(十八)大数据实战——Hive的metastore元数据服务安装

前言 Hive的metastore服务作用是为Hive CLI或者Hiveserver2提供元数据访问接口。Hive的metastore 是Hive元数据的存储和管理组件&#xff0c;它负责管理 Hive 表、分区、列等元数据信息。元数据是描述数据的数据&#xff0c;它包含了关于表结构、存储位置、数据类型等信息。本…

i18n 配置vue项目中英文语言包(中英文转化)

一、实现效果 二、下载插件创建文件夹 2.1 下载cookie来存储 npm install --save js-cookienpm i vue-i18n -S 2.2 封装组件多页面应用 2.3 创建配置语言包字段 三、示例代码 3.1 main.js 引用 i18n.js import i18n from ./lang// 实现语言切换:i18n处理element&#xff0c…

AI 绘画Stable Diffusion 研究(八)sd采样方法详解

大家好&#xff0c;我是风雨无阻。 本文适合人群&#xff1a; 希望了解stable Diffusion WebUI中提供的Sampler究竟有什么不同&#xff0c;想知道如何选用合适采样器以进一步提高出图质量的朋友。 想要进一步了解AI绘图基本原理的朋友。 对stable diffusion AI绘图感兴趣的朋…

使用QT可视化设计对话框详细步骤与代码

一、创建对话框基本步骤 创建并初始化子窗口部件把子窗口部件放到布局中设置tab键顺序建立信号-槽之间的连接实现对话框中的自定义槽 首先前面三步在这里是通过ui文件里面直接进行的&#xff0c;剩下两步则是通过代码来实现 二、项目创建详细步骤 创建新项目 为项目命名 为…

提高 After Effects 效率的 40 个最佳快捷键

After Effects 是运动图形和视觉效果的强大工具&#xff0c;但它也可能让人不知所措。拥有如此多的特性和功能&#xff0c;很容易让人迷失在软件中。但是&#xff0c;有一种方法可以简化您的工作流程并提高工作效率 - 使用键盘快捷键。 After Effects素材文件巨大、占用电脑内…

Makefile从入门到上手

文章目录 前言一、Makefile 介绍二、示例源码1、hello.c2、add.c3、sub.c4、mul.c5、div.c6、head.h 三、Makefile 基础规则1、一个规则2、两个函数和 clean①、2 个函数&#xff1a;②、clean 3、三个自动变量和模式规则①、三个自动变量②、模式规则<1>、模式规则<2…

ARM【day2】

思维导图&#xff1a; 作业1&#xff1a; 作业2&#xff1a;

每日一题——圆圈中最后剩下的数字(约瑟夫环问题)

圆圈中最后剩下的数字&#xff08;约瑟夫环问题&#xff09; 题目链接 约瑟夫环 这是一道典型的约瑟夫环问题&#xff0c;而约瑟夫问题的一般形式是这样的&#xff1a; 约瑟夫问题是个有名的问题&#xff1a;N个人围成一圈&#xff0c;从第一个开始报数&#xff0c;第M个将被…

目前最流行的GenAI框架、工具和服务初创公司一览表

目前最流行的GenAI框架、工具和服务初创公司一览表 框架与相关技术&#xff08;Frameworks & Technologies) LangChain BerriAl Outerbounds DUST Llamalndex Pinecone chroma{eature{orm LanceDB activeloop drant Baseplate beam agenta pyq Meru griptape BentoML pyq 数…

实验二十八、三角波发生电路参数的确认

一、题目 利用 Multisim 确定图1所示电路中各元件的参数&#xff0c;使输出电压的频率为 500 Hz 500\,\textrm{Hz} 500Hz、幅值为 6 V 6\,\textrm{V} 6V 的三角波。 图 1 三角波发生电路 图1\,\,三角波发生电路 图1三角波发生电路 2、仿真电路 A 1 \textrm A_1 A1​ 采用…

qtcreator编译报错cannot find -lGL

编译报错cannot find -lGL 是因为找不到openGL的库。 打开终端&#xff0c;输入”locate libGL.so”查找系统中是否有openGL的库&#xff0c;没有的话先安装&#xff0c;有的话可以看到&#xff1a; #locate libGL.so 或 #find / -name libGL.so 系统中存在openGL库&#xff0…

黑马项目一阶段面试 项目介绍篇

我完成了一个外卖项目&#xff0c;名叫苍穹外卖&#xff0c;是跟着黑马程序员的课程来自己动手写的。 项目基本实现了外卖客户端、商家端的后端完整业务。 商家端分为员工管理、文件上传、菜品管理、分类管理、套餐管理、店铺营业状态、订单下单派送等的管理、数据统计等&…

提升车间生产效率,这些做法很关键!

生产效率是制造生产企业的重要属性&#xff0c;对于影响生产效率的问题点&#xff0c;应当引起重视并规避&#xff0c;积极协调资源去改善&#xff0c;让企业能够有序、有章地运行。 一、影响生产效率的因素 1、产品加工工艺变更频繁 产品的加工工艺标准应在一段时间内不变…

小说网站第二章-关于文章的上传的实现

简述 因为最近比较忙&#xff0c;所以只有时间把以前的东西整理一下。前端方面&#xff0c;我使用了既存md5框架语法来保存数据&#xff0c;原谅我展示没找到好的方法。后端的话&#xff0c;我使用nodemongodb来保存数据。下面我就来简单介绍一下我的东西。 前端的实现 前端的…