GrabBag/AppAlgo/holeDetection/include/PlaneSegmentation.h

#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