📚 课程目录第1章基于LiDAR点云的地面分割与障碍物聚类

1. 基于LiDAR点云的地面分割与障碍物聚类

任务目标

本案例基于Autoware感知模块中的点云处理链路，实现两个核心步骤：RANSAC地面分割与欧式聚类。输入为Velodyne VLP-16采集的一帧原始点云（XYZI格式），输出为分割后的地面点云、障碍物点云簇列表，以及每个簇的轴对齐包围盒（AABB）顶点坐标。最终通过可视化或数值统计验证分割与聚类效果。

环境与依赖

Ubuntu 20.04 / 22.04，已安装ROS Noetic / Humble（本案例不依赖ROS，仅使用PCL与Eigen）
PCL 1.10+（libpcl-dev），Eigen 3.3+
CMake 3.16+，g++ 9+
测试数据：PCL官方示例点云table_scene_lms400.pcd（含地面与桌面障碍物），或自备一帧激光雷达PCD文件

核心实现

以下代码完成从PCD读取、RANSAC地面分割、欧式聚类到包围盒输出的完整闭环。地面分割使用SACSegmentation拟合平面模型，聚类使用EuclideanClusterExtraction，包围盒通过遍历簇内点云极值计算AABB。

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <iostream>
#include <vector>

using PointT = pcl::PointXYZI;

int main(int argc, char** argv) {
    // 输入PCD路径（默认使用table_scene_lms400.pcd）
    std::string file_path = (argc > 1) ? argv[1] : "table_scene_lms400.pcd";

    // 1. 加载点云
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    if (pcl::io::loadPCDFile<PointT>(file_path, *cloud) == -1) {
        std::cerr << "Failed to load: " << file_path << std::endl;
        return -1;
    }
    std::cout << "Loaded " << cloud->size() << " points" << std::endl;

    // 2. RANSAC地面分割（平面模型）
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<PointT> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.05);      // 地面点距离阈值（米）
    seg.setMaxIterations(200);
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.empty()) {
        std::cerr << "No plane found" << std::endl;
        return -1;
    }

    // 提取地面点与非地面点
    pcl::ExtractIndices<PointT> extract;
    pcl::PointCloud<PointT>::Ptr ground(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr obstacles(new pcl::PointCloud<PointT>);

    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter(*ground);

    extract.setNegative(true);
    extract.filter(*obstacles);

    std::cout << "Ground points: " << ground->size()
              << ", Obstacle points: " << obstacles->size() << std::endl;

    // 3. 欧式聚类（仅对非地面点）
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
    tree->setInputCloud(obstacles);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance(0.08);        // 聚类半径（米）
    ec.setMinClusterSize(50);            // 最小簇点数
    ec.setMaxClusterSize(25000);         // 最大簇点数
    ec.setSearchMethod(tree);
    ec.setInputCloud(obstacles);
    ec.extract(cluster_indices);

    std::cout << "Detected clusters: " << cluster_indices.size() << std::endl;

    // 4. 计算每个簇的AABB包围盒
    for (size_t i = 0; i < cluster_indices.size(); ++i) {
        pcl::PointCloud<PointT>::Ptr cluster(new pcl::PointCloud<PointT>);
        for (int idx : cluster_indices[i].indices)
            cluster->push_back((*obstacles)[idx]);

        PointT min_pt, max_pt;
        pcl::getMinMax3D(*cluster, min_pt, max_pt);

        std::cout << "Cluster " << i
                  << " | size=" << cluster->size()
                  << " | AABB min=(" << min_pt.x << ", " << min_pt.y << ", " << min_pt.z << ")"
                  << " max=(" << max_pt.x << ", " << max_pt.y << ", " << max_pt.z << ")"
                  << std::endl;
    }

    // 5. 保存结果（供可视化验证）
    pcl::io::savePCDFileBinary("ground.pcd", *ground);
    pcl::io::savePCDFileBinary("obstacles.pcd", *obstacles);
    std::cout << "Saved ground.pcd and obstacles.pcd" << std::endl;

    return 0;
}

编译与运行

# CMakeLists.txt
cmake_minimum_required(VERSION 3.16)
project(ground_seg_cluster)
find_package(PCL 1.10 REQUIRED COMPONENTS common io segmentation filters search)
add_executable(ground_seg_cluster main.cpp)
target_link_libraries(ground_seg_cluster ${PCL_LIBRARIES})

# 编译
mkdir build && cd build
cmake ..
make -j4

# 运行（使用PCL自带示例数据）
wget https://raw.githubusercontent.com/PointCloudLibrary/data/master/tutorials/table_scene_lms400.pcd
./ground_seg_cluster table_scene_lms400.pcd

验证方法与判定标准

数值验证：程序输出中应包含非零的地面点数与障碍物点数，且两者之和等于原始点数；聚类数量应大于0（table_scene数据通常可检测出2～3个簇，对应桌面上的物体）。
可视化验证：使用PCL Viewer或CloudCompare打开ground.pcd与obstacles.pcd，观察地面点云应位于同一平面，障碍物点云中不同物体被分离为独立簇。
包围盒合理性：每个簇的AABB最小/最大坐标应完全包裹该簇所有点，且不同簇的包围盒不应大面积重叠（若重叠严重，说明聚类阈值过大或地面分割不彻底）。

参数调优说明

针对不同LiDAR数据，需调整三个关键参数：setDistanceThreshold（地面分割容差，VLP-16通常取0.05～0.15m）、setClusterTolerance（聚类半径，根据障碍物间距取0.05～0.3m）、setMinClusterSize（过滤噪声点，一般取10～50）。若地面存在坡度，可改用SACMODEL_NORMAL_PLANE或先做体素降采样。