391 lines
13 KiB
C++
391 lines
13 KiB
C++
#ifndef PLANE_SEGMENTATION_H
|
||
#define PLANE_SEGMENTATION_H
|
||
|
||
#include "../include/VZNL_Types.h"
|
||
#include "ErrorCodes.h"
|
||
#include <vector>
|
||
|
||
/**
|
||
* @brief Z值直方图峰值信息
|
||
*/
|
||
struct ZHistogramPeak {
|
||
float zValue; // 峰值对应的Z坐标
|
||
int pointCount; // 该峰值区域的点数
|
||
float zMin; // 峰值区域的Z最小值
|
||
float zMax; // 峰值区域的Z最大值
|
||
std::vector<int> pointIndices; // 属于该平面的点索引
|
||
};
|
||
|
||
/**
|
||
* @brief Z值直方图平面分割参数
|
||
*/
|
||
struct ZHistogramSegmentationParams {
|
||
float binSize; // 直方图bin大小 (mm), 建议 1.0-5.0
|
||
int minPeakPoints; // 最小峰值点数, 建议 50-100
|
||
float minPeakRatio; // 最小峰值点数占比, 建议 0.03 (3%)
|
||
float peakMergeThreshold; // 峰值合并阈值 (mm), 建议 5.0-10.0
|
||
int smoothingWindow; // 平滑窗口大小, 建议 3-5
|
||
};
|
||
|
||
/**
|
||
* @brief 使用Z值直方图分割平面
|
||
*
|
||
* 算法流程:
|
||
* 1. 计算Z值范围，创建直方图
|
||
* 2. 统计每个bin的点数
|
||
* 3. 对直方图进行平滑处理（可选）
|
||
* 4. 检测峰值（局部最大值）
|
||
* 5. 合并相邻的峰值
|
||
* 6. 为每个峰值收集对应的点
|
||
*
|
||
* 优点：
|
||
* - 计算速度快 O(n)
|
||
* - 适合水平或接近水平的平面
|
||
* - 对噪声有一定鲁棒性
|
||
*
|
||
* 局限：
|
||
* - 对倾斜平面效果较差（Z值会分散）
|
||
* - 只能检测Z方向分层明显的平面
|
||
*
|
||
* @param points 输入点云
|
||
* @param pointCount 点数
|
||
* @param params 分割参数
|
||
* @param outPeaks 输出峰值列表（按Z值排序）
|
||
* @param errCode 错误码
|
||
* @return 检测到的平面数量
|
||
*/
|
||
int SegmentPlanesByZHistogram(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
const ZHistogramSegmentationParams& params,
|
||
std::vector<ZHistogramPeak>& outPeaks,
|
||
int* errCode
|
||
);
|
||
|
||
/**
|
||
* @brief 改进版：自适应倾斜平面分割
|
||
*
|
||
* 针对倾斜平面的改进算法：
|
||
* 1. 先用Z直方图粗分割
|
||
* 2. 对每个Z层，在XY平面上再做一次分层
|
||
* 3. 或者：先估计主平面法向量，然后投影到该法向量方向再做直方图
|
||
*
|
||
* @param points 输入点云
|
||
* @param pointCount 点数
|
||
* @param params 分割参数
|
||
* @param outPeaks 输出峰值列表
|
||
* @param errCode 错误码
|
||
* @return 检测到的平面数量
|
||
*/
|
||
int SegmentTiltedPlanesByHistogram(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
const ZHistogramSegmentationParams& params,
|
||
std::vector<ZHistogramPeak>& outPeaks,
|
||
int* errCode
|
||
);
|
||
|
||
/**
|
||
* @brief Z值波动统计信息
|
||
*/
|
||
struct ZFluctuationStats {
|
||
float meanZ; // Z值均值
|
||
float stdDevZ; // Z值标准差（去除离群点后）
|
||
float minZ; // Z值最小值（去除离群点后）
|
||
float maxZ; // Z值最大值（去除离群点后）
|
||
float rangeZ; // Z值范围（maxZ - minZ）
|
||
int validPointCount; // 有效点数（去除离群点后）
|
||
int outlierCount; // 离群点数量
|
||
float outlierRatio; // 离群点占比
|
||
float medianZ; // Z值中位数
|
||
float iqrZ; // 四分位距（IQR）
|
||
};
|
||
|
||
/**
|
||
* @brief Z值波动分析参数
|
||
*/
|
||
struct ZFluctuationParams {
|
||
enum OutlierMethod {
|
||
SIGMA_3, // 3-sigma规则（适合正态分布）
|
||
IQR, // 四分位距方法（更鲁棒，推荐）
|
||
PERCENTILE // 百分位数方法
|
||
};
|
||
|
||
OutlierMethod method; // 离群点检测方法
|
||
float sigmaMultiplier; // sigma倍数，默认3.0
|
||
float iqrMultiplier; // IQR倍数，默认1.5
|
||
float percentileLow; // 下百分位数，默认5.0 (5%)
|
||
float percentileHigh; // 上百分位数，默认95.0 (95%)
|
||
|
||
// 默认构造函数
|
||
ZFluctuationParams()
|
||
: method(IQR)
|
||
, sigmaMultiplier(3.0f)
|
||
, iqrMultiplier(1.5f)
|
||
, percentileLow(5.0f)
|
||
, percentileHigh(95.0f)
|
||
{}
|
||
};
|
||
|
||
/**
|
||
* @brief 计算点云Z值波动统计（鲁棒版本，自动去除离群点）
|
||
*
|
||
* 算法流程：
|
||
* 1. 收集所有有效点的Z值
|
||
* 2. 根据选择的方法检测并去除离群点：
|
||
* - 3-sigma: 去除距离均值超过3倍标准差的点
|
||
* - IQR: 去除超出 [Q1-1.5*IQR, Q3+1.5*IQR] 范围的点（推荐）
|
||
* - Percentile: 只保留指定百分位数范围内的点
|
||
* 3. 用剩余点重新计算统计信息
|
||
*
|
||
* 使用场景：
|
||
* - 评估点云平整度
|
||
* - 检测是否存在多个平面
|
||
* - 质量控制（如木板平整度检测）
|
||
*
|
||
* 示例：
|
||
* ```cpp
|
||
* ZFluctuationParams params;
|
||
* params.method = ZFluctuationParams::IQR; // 使用IQR方法（推荐）
|
||
*
|
||
* ZFluctuationStats stats;
|
||
* int errCode = 0;
|
||
* ComputeZFluctuation(points, pointCount, params, &stats, &errCode);
|
||
*
|
||
* std::cout << "Z波动: " << stats.stdDevZ << " mm" << std::endl;
|
||
* std::cout << "Z范围: " << stats.rangeZ << " mm" << std::endl;
|
||
* std::cout << "离群点: " << stats.outlierCount << " ("
|
||
* << stats.outlierRatio * 100 << "%)" << std::endl;
|
||
* ```
|
||
*
|
||
* @param points 输入点云
|
||
* @param pointCount 点数
|
||
* @param params 波动分析参数
|
||
* @param outStats 输出统计信息
|
||
* @param errCode 错误码
|
||
* @return HD_SUCCESS 成功，其他值表示错误
|
||
*/
|
||
int ComputeZFluctuation(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
const ZFluctuationParams& params,
|
||
ZFluctuationStats* outStats,
|
||
int* errCode
|
||
);
|
||
|
||
/**
|
||
* @brief 简化版：使用默认参数计算Z值波动
|
||
*
|
||
* 使用IQR方法自动去除离群点，适合大多数场景。
|
||
*
|
||
* @param points 输入点云
|
||
* @param pointCount 点数
|
||
* @param outStats 输出统计信息
|
||
* @param errCode 错误码
|
||
* @return HD_SUCCESS 成功，其他值表示错误
|
||
*/
|
||
int ComputeZFluctuation(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
ZFluctuationStats* outStats,
|
||
int* errCode
|
||
);
|
||
|
||
/**
|
||
* @brief 点云采样间距统计信息
|
||
*/
|
||
struct PointSpacingStats {
|
||
// 主要结果：取两个方向的最大值
|
||
float typicalSpacing; // 典型间距（行内和列内中位数的最大值，推荐使用）
|
||
|
||
// 分方向统计
|
||
float rowSpacing; // 行内平均间距（同一激光线上相邻点，YZ平面距离）
|
||
float colSpacing; // 列内平均间距（相邻激光线间，XZ平面距离）
|
||
float rowSpacingMedian; // 行内间距中位数（更鲁棒）
|
||
float colSpacingMedian; // 列内间距中位数（更鲁棒）
|
||
float rowSpacingStdDev; // 行内间距标准差
|
||
float colSpacingStdDev; // 列内间距标准差
|
||
int rowSampleCount; // 行内采样数量
|
||
int colSampleCount; // 列内采样数量
|
||
float rowSpacingMin; // 行内最小间距
|
||
float rowSpacingMax; // 行内最大间距
|
||
float colSpacingMin; // 列内最小间距
|
||
float colSpacingMax; // 列内最大间距
|
||
};
|
||
|
||
/**
|
||
* @brief 计算点云相邻点间距（鲁棒版本，自动去除噪点）
|
||
*
|
||
* 算法流程：
|
||
* 1. 遍历所有相邻点对：
|
||
* - 行内相邻点（同一激光线）：使用YZ平面距离 sqrt((y2-y1)² + (z2-z1)²)
|
||
* - 列内相邻点（相邻激光线）：使用XZ平面距离 sqrt((x2-x1)² + (z2-z1)²)
|
||
* 2. 跳过无效点 (0,0,0)
|
||
* 3. 收集所有距离值
|
||
* 4. 使用中位数和IQR方法去除噪点
|
||
* 5. 计算统计信息（均值、中位数、标准差等）
|
||
*
|
||
* 使用场景：
|
||
* - 估计点云采样密度/分辨率
|
||
* - 验证扫描仪参数
|
||
* - 检测采样不均匀性
|
||
* - 为后续算法选择合适的邻域大小
|
||
*
|
||
* 注意事项：
|
||
* - 行内距离使用YZ平面：因为激光线沿Y方向扫描，Z是深度
|
||
* - 列内距离使用XZ平面：因为相邻激光线在X方向分布，Z是深度
|
||
* - 自动跳过无效点 (0,0,0)
|
||
* - 使用中位数避免噪点影响
|
||
*
|
||
* 示例：
|
||
* ```cpp
|
||
* PointSpacingStats stats;
|
||
* int errCode = 0;
|
||
* ComputePointSpacing(points, rows, cols, &stats, &errCode);
|
||
*
|
||
* std::cout << "行内间距: " << stats.rowSpacingMedian << " mm" << std::endl;
|
||
* std::cout << "列内间距: " << stats.colSpacingMedian << " mm" << std::endl;
|
||
* ```
|
||
*
|
||
* @param points 输入点云（栅格化数据，按行优先存储）
|
||
* @param rows 激光线数量（行数）
|
||
* @param cols 每条激光线上的点数（列数）
|
||
* @param outStats 输出统计信息
|
||
* @param errCode 错误码
|
||
* @return HD_SUCCESS 成功，其他值表示错误
|
||
*/
|
||
int ComputePointSpacing(
|
||
const SVzNLPointXYZ* points,
|
||
int rows,
|
||
int cols,
|
||
PointSpacingStats* outStats,
|
||
int* errCode
|
||
);
|
||
|
||
struct RowZDiffFluctuationStats {
|
||
float meanAbsDz; // |dz| 均值（去除异常值后）
|
||
float medianAbsDz; // |dz| 中位数（去除异常值后）
|
||
float stdDevAbsDz; // |dz| 标准差，作为Z波动值
|
||
float minAbsDz; // |dz| 最小值（去除异常值后）
|
||
float maxAbsDz; // |dz| 最大值（去除异常值后）
|
||
float rangeAbsDz; // |dz| 波动范围（maxAbsDz - minAbsDz）
|
||
int sampleCount; // 有效样本数（去除异常值后）
|
||
int outlierCount; // 被剔除的异常样本数
|
||
float outlierRatio; // 异常样本占比（outlierCount / 原始样本数）
|
||
};
|
||
|
||
int ComputeRowZDiffFluctuation(
|
||
const SVzNLPointXYZ* points,
|
||
int rows,
|
||
int cols,
|
||
RowZDiffFluctuationStats* outStats,
|
||
int* errCode
|
||
);
|
||
|
||
// ========== RANSAC 平面分割 ==========
|
||
|
||
/**
|
||
* @brief RANSAC平面分割参数
|
||
*/
|
||
struct RansacPlaneSegmentationParams {
|
||
float distanceThreshold; // 点到平面距离阈值 (mm), 建议 0.5-2.0
|
||
int maxIterations; // RANSAC最大迭代次数, 建议 100-1000
|
||
int minPlanePoints; // 最小平面点数, 建议 50-200
|
||
int maxPlanes; // 最大提取平面数量, 建议 3-10
|
||
float growthZThreshold; // 区域生长时相邻点Z差阈值 (mm), 建议 0.5-2.0
|
||
float minPlaneRatio; // 平面最小点数占比 (相对最大平面), 建议 0.05-0.2
|
||
|
||
RansacPlaneSegmentationParams()
|
||
: distanceThreshold(1.0f)
|
||
, maxIterations(500)
|
||
, minPlanePoints(100)
|
||
, maxPlanes(5)
|
||
, growthZThreshold(1.0f)
|
||
, minPlaneRatio(0.1f)
|
||
{}
|
||
};
|
||
|
||
/**
|
||
* @brief 法向量过滤参数
|
||
*/
|
||
struct NormalFilterParams {
|
||
float maxAngleDeg; // 法向量与参考方向的最大夹角 (度), 建议 30-60
|
||
float refNx, refNy, refNz; // 参考法向量方向 (默认Z轴)
|
||
float maxDistFromPlane; // 点到平面的最大允许距离；超出则从 pointIndices 中移除；<=0 表示不过滤
|
||
|
||
NormalFilterParams()
|
||
: maxAngleDeg(45.0f)
|
||
, refNx(0.0f)
|
||
, refNy(0.0f)
|
||
, refNz(1.0f)
|
||
, maxDistFromPlane(2.0f)
|
||
{}
|
||
};
|
||
|
||
/**
|
||
* @brief 分割出的平面信息
|
||
*/
|
||
struct PlaneSegment {
|
||
float a, b, c, d; // 平面方程 ax + by + cz + d = 0 (a,b,c 已归一化)
|
||
std::vector<int> pointIndices; // 属于该平面的点索引 (在原始grid中的索引)
|
||
int pointCount; // 点数
|
||
float normalAngleDeg; // 与参考法向的夹角 (度)
|
||
|
||
PlaneSegment()
|
||
: a(0.0f), b(0.0f), c(1.0f), d(0.0f)
|
||
, pointCount(0)
|
||
, normalAngleDeg(0.0f)
|
||
{}
|
||
};
|
||
|
||
/**
|
||
* @brief 使用RANSAC方法迭代分割多个平面
|
||
*
|
||
* 算法流程:
|
||
* 1. 收集所有有效点
|
||
* 2. 迭代执行RANSAC:
|
||
* a. 随机采样3个点拟合平面
|
||
* b. 统计内点数
|
||
* c. 保留最优平面模型
|
||
* d. 提取内点，从剩余点中移除
|
||
* 3. 重复直到剩余点不足或达到最大平面数
|
||
*
|
||
* @param points 输入点云 (grid格式, rows*cols)
|
||
* @param rows 行数
|
||
* @param cols 列数
|
||
* @param params RANSAC参数
|
||
* @param outPlanes 输出平面列表
|
||
* @param errCode 错误码
|
||
* @return 检测到的平面数量
|
||
*/
|
||
int SegmentPlanesByRansac(
|
||
const SVzNLPointXYZ* points,
|
||
int rows,
|
||
int cols,
|
||
const RansacPlaneSegmentationParams& params,
|
||
std::vector<PlaneSegment>& outPlanes,
|
||
int* errCode
|
||
);
|
||
|
||
/**
|
||
* @brief 根据法向量方向过滤平面，并去除各平面中距离平面过远的点
|
||
*
|
||
* 1. 计算每个平面法向量与参考方向的夹角，移除夹角超过阈值的平面。
|
||
* 2. 对保留的平面，将 pointIndices 中签名距离绝对值超过 maxDistFromPlane 的点移除
|
||
* （maxDistFromPlane <= 0 时跳过此步骤）。
|
||
*
|
||
* @param planes 平面列表 (会被原地修改)
|
||
* @param params 法向过滤参数
|
||
* @param points 原始点云数组
|
||
* @param pointCount 原始点云数量
|
||
* @return 被移除的平面数量
|
||
*/
|
||
int FilterPlanesByNormal(
|
||
std::vector<PlaneSegment>& planes,
|
||
const NormalFilterParams& params,
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount
|
||
);
|
||
|
||
#endif // PLANE_SEGMENTATION_H
|