---

引言

在自動駕駛感知層中，點云聚類具有重要的意義。點云聚類是將三維點云數據進行分組或分類，將距離較近的點歸為一類的過程。以下是點云聚類的一些意義：

• 障礙物檢測與跟蹤：點云聚類可以幫助識別和區分環境中的不同物體，特別是障礙物。通過聚類，可以將同一個障礙物的點歸為一類，并估計其位置、形狀和大小，從而實現對障礙物的檢測和跟蹤。
• 建立環境模型：通過點云聚類，可以將點云數據分割為不同的物體群組，并將這些群組的特征提取出來。這些特征可以用于建立環境模型，包括道路、建筑物、交通標志等，為自動駕駛車輛提供更準確和詳細的環境信息。
• 動態物體檢測：點云聚類可以幫助識別和區分穩定物體和動態物體。通過將點云數據進行聚類，可以發現移動的物體，并將其與穩定的環境進行區分。這對于自動駕駛來說至關重要，因為它可以幫助車輛預測和適應動態物體的行為。
• 傳感器融合：自動駕駛系統通常會使用多種不同類型的傳感器，如激光雷達、攝像頭等。點云聚類可以將多個傳感器獲取的點云數據進行融合，提供更全面和一致的環境感知。

總的來說，點云聚類在自動駕駛感知層中的意義在于幫助將三維點云數據轉化為更易處理的物體信息，并為障礙物檢測、環境建模、動態物體檢測和傳感器融合等任務提供基礎數據和準確性。它是實現自動駕駛環境感知與決策的關鍵步驟之一。
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 歐幾里得聚類