---

引言

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

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

总的来说，点云聚类在自动驾驶感知层中的意义在于帮助将三维点云数据转化为更易处理的物体信息，并为障碍物检测、环境建模、动态物体检测和传感器融合等任务提供基础数据和准确性。它是实现自动驾驶环境感知与决策的关键步骤之一。
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);elsedownsampled_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);elseinlanes_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);elsediffnormals_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 indicesextract.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 distanceseg.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 CLOUDpcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(in_cloud_ptr);extract.setIndices(inliers);extract.setNegative(true); // true removes the indices, false leaves only the indicesextract.filter(*out_nofloor_cloud_ptr);// EXTRACT THE FLOOR FROM THE CLOUDextract.setNegative(false); // true removes the indices, false leaves only the indicesextract.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 treetree->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_scalenormal_estimation.setRadiusSearch(small_scale);normal_estimation.compute(*normals_small_scale);normal_estimation.setRadiusSearch(large_scale);normal_estimation.compute(*normals_large_scale);// 定义法向量并绑定点云 法线信息，创建DoN估计器。得到DoN特征向量diffnormals_cloudpcl::PointCloud<pcl::PointNormal>::Ptr diffnormals_cloud(new pcl::PointCloud<pcl::PointNormal>);pcl::copyPointCloud<pcl::PointXYZ, pcl::PointNormal>(*in_cloud_ptr, *diffnormals_cloud);// Create DoN operatorpcl::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 filterpcl::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 filtercond_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.6std::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// 使用CPUall_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_CLUSTERINGif (_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]);}
#elselocal_clusters = clusterAndColor(cloud_segments_array[i], out_cloud_ptr, in_out_centroids, _clustering_distances[i]);
#endifall_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 flatfor (size_t i = 0; i < cloud_2d->points.size(); i++){cloud_2d->points[i].z = 0;}// 构建KD-treeif (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 pointpcl::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 pointsmin_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);elsemid_clusters = all_clusters;if (mid_clusters.size() > 0)checkAllForMerge(mid_clusters, final_clusters, _cluster_merge_threshold);elsefinal_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;// 获取融合的IDcheckClusterMerge(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 outputif (!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;// 设置簇的大小为4pictogram_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 欧几里得聚类