PCL 使用方式与典型场景

四、使用方式与典型场景

示例一：从读文件到聚类分割的完整链路

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>
#include <cstdio>

int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
        new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) == -1) {
        PCL_ERROR("could not read pcd file\n");
        return -1;
    }
    std::printf("raw points = %zu\n", cloud->size());

    // 1) 体素降采样
    pcl::PointCloud<pcl::PointXYZ>::Ptr down(
        new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.filter(*down);

    // 2) 直通滤波，只保留工作区间
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(down);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.2, 1.5);
    pass.filter(*down);
    std::printf("after filter = %zu\n", down->size());

    // 3) 平面分割，剔除支撑面
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients);
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.01);
    seg.setInputCloud(down);
    seg.segment(*inliers, *coeffs);
    std::printf("plane inliers = %zu, a=%.3f b=%.3f c=%.3f d=%.3f\n",
                inliers->indices.size(),
                coeffs->values[0], coeffs->values[1],
                coeffs->values[2], coeffs->values[3]);

    // 4) 欧式聚类
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
        new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(down);

    std::vector<pcl::PointIndices> clusters;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.02);
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(25000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(down);
    ec.extract(clusters);

    std::printf("clusters = %zu\n", clusters.size());
    return 0;
}

这条链路的顺序很关键：先降采样再滤波再分割。反过来做，RANSAC 和聚类会在几百万个点上跑，帧率会直接崩掉。体素边长决定了后面所有阈值的量级，改它就要重调后面的参数。

示例二：ROS2 节点里做点云处理

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

class CloudFilter : public rclcpp::Node
{
public:
    CloudFilter() : Node("cloud_filter")
    {
        sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
            "/points_raw", rclcpp::SensorDataQoS(),
            std::bind(&CloudFilter::onCloud, this, std::placeholders::_1));
        pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(
            "/points_filtered", rclcpp::SensorDataQoS());
    }

private:
    void onCloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
            new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(*msg, *cloud);

        pcl::PointCloud<pcl::PointXYZ>::Ptr out(
            new pcl::PointCloud<pcl::PointXYZ>);
        pcl::VoxelGrid<pcl::PointXYZ> vg;
        vg.setInputCloud(cloud);
        vg.setLeafSize(0.05f, 0.05f, 0.05f);
        vg.filter(*out);

        sensor_msgs::msg::PointCloud2 out_msg;
        pcl::toROSMsg(*out, out_msg);
        out_msg.header = msg->header;   // 必须沿用原时间戳与坐标系
        pub_->publish(out_msg);
    }

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
};

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

两个要点：一是订阅要用传感器数据 QoS，否则收不到 best_effort 发布的点云；二是输出必须沿用输入的 header，时间戳或坐标系一改，下游的 TF 对齐就会全乱。

示例三：ICP 精配准

#include <pcl/registration/icp.h>
#include <Eigen/Core>

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(src);
icp.setInputTarget(tgt);
icp.setMaximumIterations(50);
icp.setMaxCorrespondenceDistance(0.05);
icp.setTransformationEpsilon(1e-8);
icp.setEuclideanFitnessEpsilon(1e-6);

pcl::PointCloud<pcl::PointXYZ> aligned;
icp.align(aligned);

std::printf("converged = %d, fitness = %.6f\n",
            icp.hasConverged(), icp.getFitnessScore());
Eigen::Matrix4f T = icp.getFinalTransformation();

ICP 只能做「精配准」，必须先给一个足够接近的初始位姿。工程上常见的做法是先用特征匹配或者粗定位给出初值，再用 ICP 收敛到毫米级。直接拿两个相差很大的点云喂给 ICP，结果一定是错的，而 hasConverged 还可能返回真。

适合与不适合

• 适合：离线或服务端的点云处理、激光 SLAM 的前端与回环、工业现场的工件分割与位姿估计、点云格式转换与批处理。
• 不适合：需要极高帧率的实时处理（每个环节都要单独优化，往往要下沉到 GPU 或写专用代码）、以深度学习为主的三维感知（用 Open3D 或专门的三维框架更顺手）、Python 为主的快速原型（Open3D 的 API 更友好）。