基于PCL的点云圆柱模型检测与可视化分析

在三维建模、场景理解和机器人感知中，从点云中提取几何形状（如平面、圆柱）是基础而关键的一步。本篇博客将详细介绍如何利用 PCL（Point Cloud Library） 实现 基于法线的区域生长分割，并进一步对每个簇执行 圆柱模型拟合，最终实现可视化和保存。

项目功能概述

1. 加载点云并下采样（体素滤波）

2. 估计法向量

3. 区域生长分割，获取多个簇

4. 对每个簇执行 RANSAC 圆柱拟合

5. 可视化原始点云、簇、圆柱模型

6. 保存所有簇点云、圆柱点云结果

开发环境

• PCL 1.12+

• C++17

• Windows / Linux 均可

• 编译器支持 CMake + Boost + VTK

参数

代码

// cylinder_detection.cpp
// 整体流程：读取->下采样->去地面->法线估计->区域生长分割->保存簇->圆柱拟合->可视化+保存圆柱

#include <iostream>
#include <vector>
#include <string>
#include <random>
#include <ctime>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <Eigen/Core>

int main(int argc, char** argv)
{
    // 参数配置
    const std::string input_file = "Cylinder2.pcd";
    const float leaf_size = 0.01f;
    const int knn_normals = 30;
    const float smooth_thresh = 30.0f / 180.0f * M_PI;
    const float curv_thresh = 0.5f;
    const int min_cluster_size = 50;
    const float plane_dist_thresh = 0.02f;
    const float dist_thresh = 0.05f;
    const float radius_min = 0.0f, radius_max = 0.3f;
    const int max_iter = 100;
    const float inlier_ratio_thresh = 0.8f;  // 内点比例
    const float horizontal_angle_thresh = 10.0f;

    // 1. 读取点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile(input_file, *cloud) == -1) {
        PCL_ERROR("Couldn't read file %s\n", input_file.c_str());
        return -1;
    }

    // 2. 下采样
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(leaf_size, leaf_size, leaf_size);
    vg.filter(*cloud_filtered);

    // 3. 去地面
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_noground(new pcl::PointCloud<pcl::PointXYZ>);
    {
        pcl::SACSegmentation<pcl::PointXYZ> plane_seg;
        pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices);
        pcl::ModelCoefficients::Ptr plane_coeff(new pcl::ModelCoefficients);
        plane_seg.setOptimizeCoefficients(true);
        plane_seg.setModelType(pcl::SACMODEL_PLANE);
        plane_seg.setMethodType(pcl::SAC_RANSAC);
        plane_seg.setDistanceThreshold(plane_dist_thresh);
        plane_seg.setInputCloud(cloud_filtered);
        plane_seg.segment(*plane_inliers, *plane_coeff);

        pcl::ExtractIndices<pcl::PointXYZ> extract_plane;
        extract_plane.setInputCloud(cloud_filtered);
        extract_plane.setIndices(plane_inliers);
        extract_plane.setNegative(true);
        extract_plane.filter(*cloud_noground);
        std::cout << "Removed ground, remaining pts: " << cloud_noground->size() << std::endl;
    }

    // 4. 法线估计
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud_noground);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    ne.setSearchMethod(tree);
    ne.setKSearch(knn_normals);
    ne.compute(*cloud_normals);

    // 5. 区域生长分割
    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
    reg.setMinClusterSize(min_cluster_size);
    reg.setMaxClusterSize(cloud_noground->size());
    reg.setSearchMethod(tree);
    reg.setNumberOfNeighbours(knn_normals);
    reg.setInputCloud(cloud_noground);
    reg.setInputNormals(cloud_normals);
    reg.setSmoothnessThreshold(smooth_thresh);
    reg.setCurvatureThreshold(curv_thresh);

    std::vector<pcl::PointIndices> clusters;
    reg.extract(clusters);
    std::cout << "Region Growing found " << clusters.size() << " clusters." << std::endl;

    // 6. 保存每个簇到独立 PCD
    for (size_t i = 0; i < clusters.size(); ++i) {
        pcl::PointIndices::Ptr idx_ptr(new pcl::PointIndices(clusters[i]));
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_noground);
        extract.setIndices(idx_ptr);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        extract.filter(*cluster_cloud);
        std::ostringstream fn;
        fn << "cluster_" << i << ".pcd";
        pcl::io::savePCDFileBinary(fn.str(), *cluster_cloud);
        std::cout << "Saved " << fn.str() << " (" << cluster_cloud->size() << " pts)" << std::endl;
    }

    // 7. 圆柱拟合 & 可视化 & 保存符合阈值的圆柱
    pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CYLINDER);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight(0.0f);
    seg.setMaxIterations(max_iter);
    seg.setDistanceThreshold(dist_thresh);
    seg.setRadiusLimits(radius_min, radius_max);

    pcl::visualization::PCLVisualizer viewer("Cylinders");
    viewer.setBackgroundColor(0, 0, 0);
    viewer.addPointCloud<pcl::PointXYZ>(cloud_noground, "filtered");

    for (size_t i = 0; i < clusters.size(); ++i) {
        pcl::PointIndices::Ptr idx_ptr(new pcl::PointIndices(clusters[i]));
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_noground);
        extract.setIndices(idx_ptr);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        extract.filter(*cluster_cloud);

        // 跳过水平平面簇
        Eigen::Vector3f avg_norm(0, 0, 0);
        for (auto idx : clusters[i].indices) {
            avg_norm += cloud_normals->points[idx].getNormalVector3fMap();
        }
        avg_norm.normalize();
        float angle_deg = std::acos(std::min(1.0f, std::max(-1.0f, avg_norm.dot(Eigen::Vector3f::UnitZ())))) * 180.0f / M_PI;
        if (angle_deg < horizontal_angle_thresh) continue;

        // 重估簇法线
        pcl::PointCloud<pcl::Normal>::Ptr cluster_normals2(new pcl::PointCloud<pcl::Normal>);
        ne.setInputCloud(cluster_cloud);
        ne.compute(*cluster_normals2);

        // 圆柱 RANSAC
        seg.setInputCloud(cluster_cloud);
        seg.setInputNormals(cluster_normals2);
        pcl::ModelCoefficients coeffs;
        pcl::PointIndices inliers;
        seg.segment(inliers, coeffs);

        float ratio = float(inliers.indices.size()) / cluster_cloud->size();
        std::cout << "Cluster " << i << ": pts=" << cluster_cloud->size()
            << ", inliers=" << inliers.indices.size()
            << ", ratio=" << ratio << std::endl;
        if (ratio < inlier_ratio_thresh) continue;

        // 提取圆柱内点并可视化+保存
        pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices(inliers));
        // 使用专用 ExtractIndices 对象，输入是本簇点云
        pcl::ExtractIndices<pcl::PointXYZ> extract_inliers;
        extract_inliers.setInputCloud(cluster_cloud);
        extract_inliers.setIndices(inliers_ptr);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cyl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        extract_inliers.filter(*cyl_cloud);
        // 可视化
        uint8_t r = rand() % 256, g = rand() % 256, b = rand() % 256;
        std::ostringstream name;
        name << "cylinder_" << i;
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ch(cyl_cloud, r, g, b);
        viewer.addPointCloud<pcl::PointXYZ>(cyl_cloud, ch, name.str());
        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name.str());
        // 保存结果到文件
        std::ostringstream fn;
        fn << "cylinder_" << i << ".pcd";
        pcl::io::savePCDFileBinary(fn.str(), *cyl_cloud);
    }

    while (!viewer.wasStopped()) {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(1000));
    }
    return 0;
}