保存点云聚类中心点的c++代码

以下是保存点云聚类中心点的C++代码示例：

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);

// Load point cloud data from file
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("input_cloud.pcd", *cloud) == -1) {
    PCL_ERROR("Couldn't read file input_cloud.pcd\n");
    return (-1);
}

// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCDWriter writer;

seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);

int i = 0, nr_points = (int) cloud->points.size();
while (cloud->points.size() > 0.3 * nr_points) {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);
    if (inliers->indices.size() == 0) {
        std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
        break;
    }

    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<pcl::PointXYZRGB> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(false);

    // Get the points associated with the planar surface
    extract.filter(*cloud_plane);
    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

    // Remove the planar inliers, extract the rest
    extract.setNegative(true);
    extract.filter(*cloud_filtered);
    *cloud = *cloud_filtered;
}

// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(cloud);

std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance(0.02); // 2cm
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);

int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
    for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
        cloud_cluster->push_back((*cloud)[*pit]);
    }
    cloud_cluster->width = cloud_cluster->size();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    // Save the cluster center point to file
    pcl::PointXYZRGB center_point;
    center_point.x = 0;
    center_point.y = 0;
    center_point.z = 0;
    center_point.r = 255;
    center_point.g = 0;
    center_point.b = 0;
    for (int k = 0; k < cloud_cluster->size(); k++) {
        center_point.x += cloud_cluster->points[k].x;
        center_point.y += cloud_cluster->points[k].y;
        center_point.z += cloud_cluster->points[k].z;
    }
    center_point.x /= cloud_cluster->size();
    center_point.y /= cloud_cluster->size();
    center_point.z /= cloud_cluster->size();

    std::stringstream ss;
    ss << "cluster_" << j << ".pcd";
    writer.write<pcl::PointXYZRGB> (ss.str(), *cloud_cluster, false);
    j++;
}

在上面的示例代码中，我们使用了PCL库中的EuclideanClusterExtraction类来进行点云聚类，并将每个聚类的中心点保存到了文件中。在聚类处理完成后，我们遍历每个聚类，计算出聚类中心点的坐标，并将其保存到了pcl::PointXYZRGB类型的变量中。最后，我们使用pcl::PCDWriter类将聚类结果保存到了文件中