点云处理（学习三）平面点云删除

// ──────────────────────────────────────────────────────────── // RANSAC 地平面剔除检测并移除最大平面地面/海面保留目标点云 // 适用场景激光雷达点云预处理滤除地面、海面平面点只留下障碍物、目标物体点 // 输入参数 // points 原始雷达点云容器自定义点类型LidarPoint // distThreshold 平面距离阈值单位m点到拟合平面距离小于该值则判定为地面点 // 返回值剔除地面点后的剩余物体点云 // ──────────────────────────────────────────────────────────── QVectorLidarPoint MainWindow::removeGroundPlane(const QVectorLidarPoint points, float distThreshold) { // 步骤1点云数量过少时不做平面分割直接返回原始点云 // 点数不足100无法稳定拟合平面RANSAC分割会失效 if (points.size() 100) return points; // 步骤2创建PCL标准点云智能指针用于PCL算法处理 // PCL库只能识别pcl::PointXYZ格式需要把自定义LidarPoint做格式转换 pcl::PointCloudpcl::PointXYZ::Ptr cloudIn(new pcl::PointCloudpcl::PointXYZ()); // 预分配内存避免循环push_back频繁扩容提升性能 cloudIn-reserve(points.size()); // 遍历自定义点云转换为PCL标准三维点 for (const auto pt : points) { cloudIn-push_back({pt.x, pt.y, pt.z}); } // 步骤3初始化RANSAC平面分割器 pcl::SACSegmentationpcl::PointXYZ seg; // 开启模型系数优化提升平面拟合精度 seg.setOptimizeCoefficients(true); // 指定拟合模型平面模型用来识别地面/海面 seg.setModelType(pcl::SACMODEL_PLANE); // 指定随机采样一致性算法RANSAC抗噪声、抗离群点 seg.setMethodType(pcl::SAC_RANSAC); // 设置距离阈值点距离平面小于该值视为平面内点地面点 seg.setDistanceThreshold(distThreshold); // 设置RANSAC最大迭代次数迭代越多拟合越精准但耗时更长 seg.setMaxIterations(200); // 存储平面内点地面点的索引集合 pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // 存储拟合出的平面方程系数 axbyczd0 pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients); // 传入待分割的输入点云 seg.setInputCloud(cloudIn); // 执行RANSAC平面分割输出地面点索引、平面系数 seg.segment(*inliers, *coeff); // 分割失败没有找到任何平面点直接返回原始点云 if (inliers-indices.empty()) return points; // 步骤4标记所有需要保留的点 // 创建布尔标记数组初始全部设为true保留 std::vectorbool keep(points.size(), true); // 遍历所有地面点索引将对应位置标记为false剔除 for (int idx : inliers-indices) { // 边界安全校验防止索引越界崩溃 if (idx 0 idx static_castint(points.size())) keep[idx] false; } // 步骤5根据标记筛选非地面点存入结果容器 QVectorLidarPoint result; // 预分配预估内存减少内存重新分配开销 result.reserve(points.size() - inliers-indices.size()); for (size_t i 0; i points.size(); i) { // 仅保留标记为true的障碍物点 if (keep[i]) result.push_back(points[i]); } // 统计剔除地面点数量打印调试日志 int removed points.size() - result.size(); if (removed 0) { qDebug() [地平面] 剔除 removed 个点 ( QString::number(100.0 * removed / points.size(), f, 1) %), 阈值 distThreshold m; } // 容错保护如果剔除超过90%点说明平面拟合严重误判把物体全当成地面 // 放弃剔除逻辑返回原始完整点云避免目标完全丢失 if (removed points.size() * 0.90) { qDebug() [地平面] 剔除比例过高可能误判保留原始点云; return points; } // 返回剔除地面后的障碍物点云 return result; }