C++ 实现 ROS 2 点云欧几里得聚类

C++ 实现 ROS 2 点云欧几里得聚类

在 Livox Mid-360 采集的 sensor_msgs::msg::PointCloud2 点云数据 上进行 欧几里得聚类（Euclidean Cluster Extraction），具体流程如下：

---

✅ 1. 订阅 PointCloud2 并转换为 pcl::PointCloud

解释：

• sensor_msgs::msg::PointCloud2 是 ROS 2 点云消息格式，PCL 不能直接处理。
• pcl::fromROSMsg() 将 PointCloud2 转换为 pcl::PointCloud。

代码：

#include 
#include 
#include 

pcl::PointCloud::Ptr convertToPCL(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
    pcl::fromROSMsg(*msg, *cloud);  // 转换数据格式
    return cloud;
}

---

✅ 2. 构建 KdTree

解释：

• KdTree（K-Dimensional Tree） 是 PCL 用于 快速近邻搜索（KNN） 的数据结构，能高效搜索点云中的最近邻点。
• pcl::search::KdTree 让聚类算法能够快速找到相邻点，提高计算效率。

代码：

#include 

pcl::search::KdTree::Ptr buildKdTree(pcl::PointCloud::Ptr cloud) {
    pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
    tree->setInputCloud(cloud);  // 设定 KdTree 输入点云
    return tree;
}

---

✅ 3. 进行欧几里得聚类

解释：

• 欧几里得聚类（Euclidean Cluster Extraction） 是基于 KdTree 的点云聚类方法。
• setClusterTolerance(0.05) 设置聚类半径（邻近搜索范围）。
• setMinClusterSize(100) 和 setMaxClusterSize(25000) 过滤掉过小或过大的聚类，防止噪声点和冗余计算。

代码：

#include 

std::vector performClustering(
    pcl::PointCloud::Ptr cloud, 
    pcl::search::KdTree::Ptr tree) {
    
    pcl::EuclideanClusterExtraction ec;
    ec.setClusterTolerance(0.05);  // 设定聚类半径（单位：米）
    ec.setMinClusterSize(100);     // 过滤掉小于100个点的聚类
    ec.setMaxClusterSize(25000);   // 过滤掉大于25000个点的聚类
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud);

    std::vector cluster_indices;
    ec.extract(cluster_indices);  // 执行聚类

    return cluster_indices;
}

---

✅ 4. 处理聚类结果

解释：

• std::vector 存储了所有聚类，每个 pcl::PointIndices 是一个聚类的索引列表。
• extractClusterPoints() 将聚类点提取出来，方便后续可视化或进一步处理。

代码：

#include 

std::vector::Ptr> extractClusterPoints(
    pcl::PointCloud::Ptr cloud, 
    std::vector cluster_indices) {

    std::vector::Ptr> clusters;
    
    for (const auto& indices : cluster_indices) {
        pcl::PointCloud::Ptr cluster(new pcl::PointCloud);
        
        for (const auto& idx : indices.indices) {
            cluster->push_back((*cloud)[idx]);  // 提取聚类中的每个点
        }
        
        clusters.push_back(cluster);
    }

    return clusters;
}

---

5. 整合代码并发布 ROS 2 话题

完整 ROS 2 处理节点：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

class PointCloudClusteringNode : public rclcpp::Node {
public:
    PointCloudClusteringNode() : Node("pointcloud_clustering") {
        subscription_ = this->create_subscription(
            "/livox/lidar", 10, 
            std::bind(&PointCloudClusteringNode::pointCloudCallback, this, std::placeholders::_1));

        publisher_ = this->create_publisher("/clustered_pointcloud", 10);
    }

private:
    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        // 1. 转换点云格式
        pcl::PointCloud::Ptr cloud = convertToPCL(msg);
        
        // 2. 构建 KdTree
        pcl::search::KdTree::Ptr tree = buildKdTree(cloud);

        // 3. 执行欧几里得聚类
        std::vector cluster_indices = performClustering(cloud, tree);
        RCLCPP_INFO(this->get_logger(), "Detected %lu clusters", cluster_indices.size());

        // 4. 处理聚类结果
        auto clusters = extractClusterPoints(cloud, cluster_indices);

        // 5. 发布聚类后的点云（只发布第一个聚类作为示例）
        if (!clusters.empty()) {
            sensor_msgs::msg::PointCloud2 output;
            pcl::toROSMsg(*clusters[0], output);
            output.header = msg->header;
            publisher_->publish(output);
        }
    }

    rclcpp::Subscription::SharedPtr subscription_;
    rclcpp::Publisher::SharedPtr publisher_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared());
    rclcpp::shutdown();
    return 0;
}

---

关键步骤总结

1. 订阅 PointCloud2 并转换为 PCL 格式：

   • pcl::fromROSMsg() 转换为 pcl::PointCloud。

2. 构建 KdTree：

   • pcl::search::KdTree 用于快速邻域搜索。

3. 执行欧几里得聚类：

   • pcl::EuclideanClusterExtraction 进行基于距离的点云分割。

4. 处理聚类结果：

   • pcl::PointIndices 记录每个聚类的索引，并提取实际点云。

5. 转换回 PointCloud2 并发布：

   • pcl::toROSMsg() 转换数据格式，发布聚类点云。

---

运行方法

ros2 run your_package_name pointcloud_clustering

---

结果

该节点会订阅 /livox/lidar，进行聚类，并将第一个聚类点云发布到 /clustered_pointcloud
可以使用 rviz2 进行可视化查看聚类结果