PCL对点云数据进行聚类和平面拟合 代码

以下是使用PCL库对点云数据进行聚类和平面拟合的示例代码：

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>

int main(int argc, char** argv)
{
    // 读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("input_cloud.pcd", *cloud) == -1)
    {
        PCL_ERROR("Could not read file.\n");
        return -1;
    }

    // 下采样滤波
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZ>);
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.filter(*cloud_downsampled);

    // 计算法向量
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    tree->setInputCloud(cloud_downsampled);
    ne.setInputCloud(cloud_downsampled);
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(0.03);
    ne.compute(*normals);

    // 区域生长分割
    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segmented(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr normals_segmented(new pcl::PointCloud<pcl::Normal>);
    std::vector<pcl::PointIndices> clusters;
    rg.setInputCloud(cloud_downsampled);
    rg.setInputNormals(normals);
    rg.setMinClusterSize(100);
    rg.setMaxClusterSize(100000);
    rg.setNumberOfNeighbours(30);
    rg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
    rg.setCurvatureThreshold(1.0);
    rg.extract(clusters);

    // 平面拟合
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(0.01);
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
        {
            cloud_cluster->push_back((*cloud_downsampled)[*pit]);
        }
        seg.setInputCloud(cloud_cluster);
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0)
        {
            std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
            continue;
        }
        std::cout << "Plane coefficients: " << *coefficients << std::endl;

        // 将拟合结果可视化
        pcl::visualization::PCLVisualizer viewer("Plane Fitting");
        viewer.setBackgroundColor(0.0, 0.0, 0.0);
        viewer.addPointCloud<pcl::PointXYZ>(cloud_cluster, "cloud_cluster");
        pcl::ModelCoefficients coeffs;
        coeffs.values.resize(4);
        coeffs.values[0] = coefficients->values[0];
        coeffs.values[1] = coefficients->values[1];
        coeffs.values[2] = coefficients->values[2];
        coeffs.values[3] = coefficients->values[3];
        viewer.addPlane(coeffs, "plane");
        viewer.spin();
    }

    return 0;
}

该示例代码中，我们首先读取一个点云数据，然后进行下采样滤波，计算法向量，然后使用区域生长分割对点云数据进行聚类，接着对每个聚类进行平面拟合，并将拟合结果可视化。

需要注意的是，这里的点云数据格式为 `pcl::PointXYZ`，即只包含三维坐标信息，如果点云数据包括其他信息，如颜色等，需要使用其他格式的点云数据。