leida_simi/count.cpp

// count.cpp - 计数模式(count)核心算法与文件保存实现
#include "count.h"
#include "config.h"
#include "filter.h"
#include "io_utils.h"
#include "lidar_utils.h"
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <limits>
#include <sstream>
#include <thread>
#include <unordered_map>

// ==================== 点云级稀疏拖影过滤 ====================
// 现象: 平台/物体左右边缘外伸的"胡须状拖影"= 每圈端部外侧 1~N 个点，
// 圈内 X±radar_cfg().filter.sparse_cut.x_radius 内无同圈邻居(稀疏)，且沿 z 方向稳定跨很多圈；
// 若放任不管，这些点膨胀后会撑大 count 的面积/个数统计。
// 稀疏点判定: 邻居只在同一圈(z相同)内比较(不同圈同X的点不互为邻居)。
// 删除判据(满足任一即删该稀疏点):
//   1) 同圈连续: 同一圈内稀疏点数 >= radar_cfg().filter.sparse_cut.circle_min；
//   2) 跨圈连续: 相近 X 位置(±2×XRadius 归并, 容忍圈间抖动)的稀疏点，
//      z 上连续出现(相邻圈间隔容差=3×中位圈间隔，允许个别缺圈) 圈数 >= radar_cfg().filter.sparse_cut.run_min。
// 原地修改 points；返回删除点数。应在 z_gap_interpolate 之前调用。
int sparse_density_filter(std::vector<CountPoint>& points)
{
    if (!radar_cfg().filter.sparse_cut.enable || points.empty()) return 0;
    const double R = std::max(radar_cfg().filter.sparse_cut.x_radius, 0.0);
    if (R <= 0.0) return 0;
    const int Cm = std::max(radar_cfg().filter.sparse_cut.circle_min, 1);
    const int Rm = std::max(radar_cfg().filter.sparse_cut.run_min, 1);
    const size_t n = points.size();

    // ---- 1) 按 z 聚类成圈(同一 scan 包的点 z 相同) ----
    std::vector<size_t> order(n);
    for (size_t i = 0; i < n; i++) order[i] = i;
    std::sort(order.begin(), order.end(), [&](size_t a, size_t b) {
        return points[a].z_val < points[b].z_val; });
    struct Circle { size_t beg, end; };      // [beg,end) 在 order 中
    std::vector<Circle> circles;
    {
        size_t i = 0;
        while (i < n)
        {
            size_t j = i + 1;
            while (j < n && points[order[j]].z_val - points[order[i]].z_val < 1e-6) ++j;
            circles.push_back({ i, j });
            i = j;
        }
    }

    // 相邻圈 z 间隔中位数 ×3 = 跨圈"连续"容差(允许个别圈缺失/抖动)
    double z_merge = 1.0;
    if (circles.size() >= 2)
    {
        std::vector<double> dz;
        dz.reserve(circles.size() - 1);
        for (size_t i = 1; i < circles.size(); i++)
            dz.push_back(points[order[circles[i].beg]].z_val -
                         points[order[circles[i - 1].beg]].z_val);
        std::sort(dz.begin(), dz.end());
        z_merge = 3.0 * dz[dz.size() / 2];
        if (z_merge < 1.0) z_merge = 1.0;
    }

    // ---- 2) 稀疏点判定: 同圈内 X±R 无其它点 ----
    std::vector<uint8_t> sparse(n, 0);
    for (const auto& c : circles)
    {
        std::vector<size_t> ci;
        ci.reserve(c.end - c.beg);
        for (size_t k = c.beg; k < c.end; k++) ci.push_back(order[k]);
        std::sort(ci.begin(), ci.end(), [&](size_t a, size_t b) {
            return points[a].x_mm < points[b].x_mm; });

        const size_t m = ci.size();
        if (m == 0) continue;
        if (m == 1) { sparse[ci[0]] = 1; continue; }   // 单点圈: 无同圈邻居

        size_t lo = 0, hi = 0;
        for (size_t k = 0; k < m; k++)
        {
            double xk = points[ci[k]].x_mm;
            while (lo < m && points[ci[lo]].x_mm < xk - R) ++lo;
            if (hi < lo) hi = lo;
            while (hi + 1 < m && points[ci[hi + 1]].x_mm <= xk + R) ++hi;
            if (hi - lo + 1 <= 1) sparse[ci[k]] = 1;   // 窗口内只有自己
        }
    }

    // ---- 3) 删除标记 ----
    std::vector<uint8_t> del(n, 0);
    // (a) 同圈连续: 该圈稀疏点数 >= Cm 则整圈的稀疏点删除
    for (const auto& c : circles)
    {
        int cnt = 0;
        for (size_t k = c.beg; k < c.end; k++)
            if (sparse[order[k]]) cnt++;
        if (cnt >= Cm)
            for (size_t k = c.beg; k < c.end; k++)
                if (sparse[order[k]]) del[order[k]] = 1;
    }
    // (b) 跨圈连续: 相近X(±R)的稀疏点按x归簇, 簇内z连续段圈数 >= Rm 则删除
    {
        std::vector<size_t> sp;
        for (size_t i = 0; i < n; i++)
            if (sparse[i]) sp.push_back(i);
        if (!sp.empty())
        {
            std::sort(sp.begin(), sp.end(), [&](size_t a, size_t b) {
                return points[a].x_mm < points[b].x_mm; });
            size_t i = 0;
            while (i < sp.size())
            {
                size_t j = i + 1;
                std::vector<size_t> clu;
                clu.reserve(64);
                clu.push_back(sp[i]);
                // 同X位置归并容差 2×R：拖影点X在圈间有轻微抖动，只用R会漏掉抖动>R的点
                while (j < sp.size() && points[sp[j]].x_mm - points[sp[i]].x_mm <= 2.0 * R)
                {
                    clu.push_back(sp[j]);
                    ++j;
                }
                std::sort(clu.begin(), clu.end(), [&](size_t a, size_t b) {
                    return points[a].z_val < points[b].z_val; });
                size_t s2 = 0;
                while (s2 < clu.size())
                {
                    size_t e2 = s2 + 1;
                    while (e2 < clu.size() &&
                           points[clu[e2]].z_val - points[clu[e2 - 1]].z_val <= z_merge)
                        ++e2;
                    if (e2 - s2 >= (size_t)Rm)
                        for (size_t k = s2; k < e2; k++) del[clu[k]] = 1;
                    s2 = e2;
                }
                i = j;
            }
        }
    }

    // ---- 4) 原地删除并统计 ----
    size_t w = 0;
    for (size_t i = 0; i < n; i++)
        if (!del[i]) points[w++] = points[i];
    size_t rm = n - w;
    points.resize(w);
    if (rm > 0)
        std::cout << "[count] 稀疏拖影过滤: 删除 " << rm << " 点 (剩 " << w << ")"
                  << " [X±" << R << "mm 无邻, 同圈>=" << Cm << "点, 跨圈连续>="
                  << Rm << "圈]" << std::endl;
    return (int)rm;
}

// ==================== 连续段尾部减除 ====================
// 对膨胀前的 0/1 二值图逐行(X方向)处理连续点段：
//   1) 长度 < radar_cfg().filter.tail_cut.min_run_length 的段视为孤立噪点，整段删除；
//   2) 正常段保留中间核心 + 左右各 radar_cfg().filter.tail_cut.max_tail_length，核心区外为"尾部候选"；
//   3) 尾部候选若满足 (该格最大Y - 段核心中位Y) > radar_cfg().filter.tail_cut.y_delta 则删除
//      （对应左右边缘 Y 逐渐变大的胡须状拖影）；若尾部Y与核心接近则保留，避免误伤真实边缘；
//   4) radar_cfg().filter.tail_cut.support_rows>0 时，上下行同列有点视为竖直主体，豁免不删。
// cell_y_max 为空数组表示不使用 Y 判据（此时只按长度裁剪）。
static void trim_tail_runs(std::vector<uint8_t>& image, int w, int h,
                           const std::vector<double>& cell_y_max)
{
    if (w <= 0 || h <= 0) return;
    const bool use_y = cell_y_max.size() == image.size();
    std::vector<uint8_t> deleted(image.size(), 0);
    auto at = [&](int x, int y) -> int {
        if (x < 0 || x >= w || y < 0 || y >= h) return 0;
        return image[(size_t)y * w + x];
    };

    for (int y = 0; y < h; y++)
    {
        int x = 0;
        while (x < w)
        {
            if (!at(x, y)) { x++; continue; }
            int s = x;
            while (x + 1 < w && at(x + 1, y)) x++;
            int e = x;                       // 连续段 [s, e]
            x = e + 1;
            int len = e - s + 1;

            if (len < radar_cfg().filter.tail_cut.min_run_length)  // 孤立短段：整段删除
            {
                for (int c = s; c <= e; c++) deleted[(size_t)y * w + c] = 1;
                continue;
            }

            // 段内核心 Y 中位数（该行该段的主体典型高度）
            double core_y = 0.0;
            if (use_y)
            {
                std::vector<double> ys;
                ys.reserve(len);
                for (int c = s; c <= e; c++)
                {
                    double ym = cell_y_max[(size_t)y * w + c];
                    if (ym > -1e8) ys.push_back(ym);
                }
                if (ys.empty()) continue;
                std::sort(ys.begin(), ys.end());
                core_y = ys[ys.size() / 2];
            }

            // 核心区：两端各留 MaxTailLength，中间至少要留 CoreMinLen
            int left_cut  = std::min<int>(s + radar_cfg().filter.tail_cut.max_tail_length, e - radar_cfg().filter.tail_cut.core_min_len + 1);
            int right_cut = std::max<int>(e - radar_cfg().filter.tail_cut.max_tail_length, s + radar_cfg().filter.tail_cut.core_min_len - 1);
            if (left_cut > right_cut) continue;   // 段过短：整体视为核心，不裁剪

            for (int c = s; c <= e; c++)
            {
                if (c >= left_cut && c <= right_cut) continue;   // 核心保留

                // Y 判据：尾部 Y 未明显高于核心 → 保留（真实边缘）
                if (use_y)
                {
                    double ym = cell_y_max[(size_t)y * w + c];
                    if (ym > -1e8 && ym <= core_y + radar_cfg().filter.tail_cut.y_delta) continue;
                }
                // 支撑判据：上下行同列有点 → 竖直主体边缘，豁免
                if (radar_cfg().filter.tail_cut.support_rows > 0)
                {
                    bool supported = false;
                    for (int r = 1; r <= radar_cfg().filter.tail_cut.support_rows; r++)
                        if (at(c, y - r) || at(c, y + r)) { supported = true; break; }
                    if (supported) continue;
                }
                deleted[(size_t)y * w + c] = 1;
            }
        }
    }
    for (size_t i = 0; i < image.size(); i++)
        if (deleted[i]) image[i] = 0;
}

// ==================== 点集 → 图像 ====================
std::vector<uint8_t> points_to_image(const std::vector<CountPoint>& points,
                                     double* area_out,
                                     int* out_w, int* out_h)
{
    const int MIN_DIM = 8;     // 最小尺寸保护
    const int MAX_DIM = 20000; // 单边最大尺寸保护(防止z过大导致内存爆炸)
    if (area_out) *area_out = 0.0;
    if (out_w) *out_w = 0;
    if (out_h) *out_h = 0;
    if (points.empty()) return std::vector<uint8_t>((size_t)MIN_DIM * MIN_DIM, 0);

    // 宽: radar_cfg().filter.polygon 多边形 X 最大最小值直接构成 (1mm=1px)
    double polyXMin = 0.0, polyXMax = 0.0;
    if (radar_cfg().filter.poly_n > 0)
    {
        polyXMin = radar_cfg().filter.polygon[0].first;
        polyXMax = radar_cfg().filter.polygon[0].first;
        for (int i = 1; i < radar_cfg().filter.poly_n; i++)
        {
            polyXMin = std::min(polyXMin, radar_cfg().filter.polygon[i].first);
            polyXMax = std::max(polyXMax, radar_cfg().filter.polygon[i].first);
        }
    }
    double spanX = polyXMax - polyXMin;
    if (spanX <= 0.0) spanX = 1.0;

    // 高: z 最大值直接构成 (Z=运动方向, 从0开始递增, 1单位=1px)
    double maxZ = 0.0;
    for (const auto& p : points) maxZ = std::max(maxZ, p.z_val);
    if (maxZ <= 0.0) maxZ = 1.0;

    int imageWidth  = (int)std::lround(spanX);
    int imageHeight = (int)std::lround(maxZ);
    if (imageWidth  < MIN_DIM) imageWidth  = MIN_DIM;
    if (imageHeight < MIN_DIM) imageHeight = MIN_DIM;
    if (imageWidth  > MAX_DIM) imageWidth  = MAX_DIM;
    if (imageHeight > MAX_DIM) imageHeight = MAX_DIM;
    if (out_w) *out_w = imageWidth;
    if (out_h) *out_h = imageHeight;
    std::vector<uint8_t> image((size_t)imageWidth * imageHeight, 0);   // 0=白底

    // X坐标相对多边形左边界偏移到0；Z从0开始
    double scaleX = (double)imageWidth / spanX;   // ≈1: 宽直接由多边形X跨度构成
    double scaleY = (double)imageHeight / maxZ;   // ≈1: 高直接由z最大值构成
    double xOffset = -polyXMin;

    // 优化：Step1 先按1x1网格标记点位置（重复点只写一次），
    //      Step2 再对标记位置膨胀 radar_cfg().count.point_dilate×radar_cfg().count.point_dilate
    std::vector<uint32_t> marks;
    marks.reserve(points.size() / 2);
    std::vector<double> cell_y_max;    // 每格最大Y（仅尾部减除启用时分配）
    if (radar_cfg().filter.tail_cut.enable) cell_y_max.assign((size_t)imageWidth * imageHeight, -1e9);
    for (const auto& p : points)
    {
        int px = (int)((p.x_mm + xOffset) * scaleX);
        int py = (int)(p.z_val * scaleY);
        if (px < 0 || px >= imageWidth || py < 0 || py >= imageHeight) continue;
        size_t idx = (size_t)py * imageWidth + px;
        if (image[idx] == 0)          // 未标记过才记录
        {
            image[idx] = 1;
            marks.push_back((uint32_t)idx);
        }
        if (radar_cfg().filter.tail_cut.enable && p.y_mm > cell_y_max[idx]) cell_y_max[idx] = p.y_mm;
    }

    // 连续段尾部减除（膨胀前）：剪掉左右两侧胡须状拖影，
    // 避免其经膨胀放大后混入层面积统计
    if (radar_cfg().filter.tail_cut.enable)
    {
        trim_tail_runs(image, imageWidth, imageHeight, cell_y_max);
        // 剪除会改动 image，需重建 marks，防止被删格经膨胀“复活”
        marks.clear();
        marks.reserve(image.size() / 8);
        for (size_t i = 0; i < image.size(); i++)
            if (image[i] == 1) marks.push_back((uint32_t)i);
    }
    for (uint32_t idx : marks)        // 膨胀 radar_cfg().count.point_dilate×radar_cfg().count.point_dilate
    {
        int px = (int)(idx % imageWidth);
        int py = (int)(idx / imageWidth);
        for (int dy = 0; dy < radar_cfg().count.point_dilate; dy++)
            for (int dx = 0; dx < radar_cfg().count.point_dilate; dx++)
            {
                int ix = px + dx, iy = py + dy;
                if (ix < imageWidth && iy < imageHeight)
                    image[(size_t)iy * imageWidth + ix] = 255;   // 255=黑点
            }
    }
    int totalPixels = 0;
    for (size_t i = 0; i < image.size(); i++)
        if (image[i] != 0) totalPixels++;

    double pixelToAreaRatio = spanX * maxZ / (double)(imageWidth * imageHeight);
    if (area_out) *area_out = totalPixels * pixelToAreaRatio;
    return image;
}

// ==================== z轴空隙插值 ====================
int z_gap_interpolate(std::vector<CountPoint>& points)
{
    if (points.empty() || radar_cfg().count.z_interpolate_mode == "none")
        return 0;

    // 正常相邻两条(圈)的 z 间隔: 速度模式 = 每圈行进距离(mm); 时间模式 = 每圈耗时(ms)
    // configuration(变速)模式: 用 speed.txt 剖面平均速度(mm/s)/帧率 作近似; 表无效时回退匀速。
    double normal_gap = 0.0;
    double prof_v = speed_profile_avg_velocity_mm_s();
    if (radar_cfg().scan.z_speed_type == "configuration" && prof_v > 0)
        normal_gap = prof_v / radar_cfg().radar.hz;             // 平均 mm/圈
    else if (radar_cfg().scan.z_speed > 0)
        normal_gap = radar_cfg().scan.z_speed * 10.0 / radar_cfg().radar.hz;     // cm/s ×10 = mm/s, ÷Hz = mm/圈
    else
        normal_gap = 1000.0 / radar_cfg().radar.hz;             // ms/圈
    if (normal_gap <= 0) return 0;

    // 支持的模式: none / CopyTranslation / AngleBucketInterpolation
    //            / AngleBucketInterpolationByDilate
    const std::string& mode = radar_cfg().count.z_interpolate_mode;
    const bool by_dilate = (mode == "AngleBucketInterpolationByDilate");
    const double pd = (double)radar_cfg().count.point_dilate;      // 膨胀像素(1单位z=1px)
    // 角度插值桶宽(度): 应等于雷达实际角度分辨率。SDK 包头的 angle_resolution
    // 单位是 0.01°, 合法值 {1,2,4,8,16,32} → 0.01/0.02/0.04/0.08/0.16/0.32 度。
    // 50Hz 常用 0.08°(一圈 4500 点)。原先写死 1°/桶会把一圈压缩成最多 360 点。
    const double bucket_deg = (radar_cfg().count.interp_bucket_deg > 0.0)
                            ? radar_cfg().count.interp_bucket_deg : 0.08;
    const int    nbuckets  = (int)(360.0 / bucket_deg + 0.5);

    // ByDilate 补行步长 = point_dilate/2
    const double dilate_step = (by_dilate && pd > 0) ? (pd / 2.0) : normal_gap;
    // 空隙判定阈值:
    //   ByDilate = 步长(即 point_dilate/2): 只要 gap 一超过步长就补行, 语义与
    //              "每 point_dilate/2 增加一条"完全一致(门槛 = 步长)。
    //   其它     = 2×正常圈间隔(相对判定, 与速度无关)
    const double gap_threshold = by_dilate ? dilate_step : (2.0 * normal_gap);
    // 条件1(非 ByDilate 模式): 前后两条 Y 平均值差超过30mm 不插(可能跨层/换物体)
    const double max_y_mean_diff = 30.0;
    // 条件1'(ByDilate 模式): 逐桶比较两端"中位 y", 差超过30mm 才跳过该方向。
    //   用中位数而非均值 —— 均值会被少量离群点(噪点/边缘点)拉偏, 使整个空隙被误判为
    //   "跨层/换物体"而整体跳过、留下大间隙; 中位数对离群点鲁棒, 只跳过真正突变的
    //   方向, 其余方向照常平滑补行。
    const double max_y_med_diff = 30.0;
    // 一个空隙最多补的行数
    const int MAX_FILL_LEVELS = 10;

    // 取中位数(用于 ByDilate 的逐桶判定): 对离群点鲁棒, 不会被个别噪点拉偏
    auto median_of = [](const std::vector<double>& v) -> double
    {
        if (v.empty()) return 0.0;
        std::vector<double> t(v.begin(), v.end());
        std::sort(t.begin(), t.end());
        const std::size_t m = t.size() / 2;
        return (t.size() & 1) ? t[m] : (t[m - 1] + t[m]) * 0.5;
    };

    // 1. 按 z 升序排序
    std::sort(points.begin(), points.end(),
              [](const CountPoint& a, const CountPoint& b) { return a.z_val < b.z_val; });

    // 2. 聚类成"圈"(同一scan包的点 z 完全相同, 用区间 [begin,end) 索引 points)
    struct Circle { double z; size_t begin; size_t end; };
    std::vector<Circle> circles;
    {
        size_t i = 0;
        while (i < points.size())
        {
            size_t j = i + 1;
            while (j < points.size() && points[j].z_val - points[i].z_val < 1e-6) ++j;
            circles.push_back(Circle());
            circles.back().z = points[i].z_val;
            circles.back().begin = i;
            circles.back().end = j;
            i = j;
        }
    }
    if (circles.size() < 2)
        return 0;

    auto circle_mean_y = [&](const Circle& c) -> double
    {
        double s = 0;
        for (size_t p = c.begin; p < c.end; ++p) s += points[p].y_mm;
        return s / (double)(c.end - c.begin);
    };

    int added = 0;
    int gaps_hit     = 0;    // 超过门槛的空隙数
    int gaps_skip_y  = 0;    // 因 Y 均值差被跳过
    int gaps_skip_x  = 0;    // 因 X 范围约束/无公共角度桶 被过滤光
    int bucket_skip_med = 0; // ByDilate: 因逐桶中位 y 差过大被跳过的方向数
    int diag_printed = 0;    // 诊断日志限流(最多 20 条)
    // 3. 逐空隙补行
    for (size_t i = 1; i < circles.size(); ++i)
    {
        double gap = circles[i].z - circles[i - 1].z;
        if (gap <= gap_threshold)      // 未超过2×正常间隔, 不算空隙
            continue;

        ++gaps_hit;
        const Circle& cA = circles[i - 1];   // 上一条
        const Circle& cB = circles[i];       // 本条

        // 条件1: 前后两条的 Y 平均值几乎相当(差≤30mm)才插值
        //   ByDilate 模式跳过此整圈判定 —— 它改用下面的"逐桶中位数"闸门,
        //   避免个别离群点把整个空隙误判成跨层而整体跳过、留下大间隙。
        if (!by_dilate)
        {
            double ydiff = std::fabs(circle_mean_y(cA) - circle_mean_y(cB));
            if (ydiff > max_y_mean_diff)
            {
                ++gaps_skip_y;
                if (diag_printed < 20)
                {
                    ++diag_printed;
                    std::cerr << "[插值诊断] 跳过 gap=" << gap
                              << " (Z " << cA.z << "→" << cB.z << ")"
                              << ": Y均值差=" << ydiff << "mm > "
                              << max_y_mean_diff << "mm(疑似跨层/换物体)"
                              << std::endl;
                }
                continue;
            }
        }

        // 条件2: 生成行的 X 范围 = 这两条的 X 最大和最小
        double x_lo = points[cA.begin].x_mm, x_hi = points[cA.begin].x_mm;
        for (size_t p = cA.begin; p < cA.end; ++p)
        {
            if (points[p].x_mm < x_lo) x_lo = points[p].x_mm;
            if (points[p].x_mm > x_hi) x_hi = points[p].x_mm;
        }
        for (size_t p = cB.begin; p < cB.end; ++p)
        {
            if (points[p].x_mm < x_lo) x_lo = points[p].x_mm;
            if (points[p].x_mm > x_hi) x_hi = points[p].x_mm;
        }

        // 空隙内补行数(最多10行):
        //   ByDilate = 每 point_dilate/2 补一条(按膨胀尺寸决定密度, 保证平滑连续)
        //   其它     = 按正常间隔容纳行数-1(空隙两端已有)
        int n = by_dilate ? (int)(gap / dilate_step)
                          : (int)(gap / normal_gap + 0.5) - 1;
        if (n < 1) continue;
        if (n > MAX_FILL_LEVELS) n = MAX_FILL_LEVELS;

        if (mode == "CopyTranslation")
        {
            // 复制"上一条"的点, z 均匀平移到空隙内的各补行
            for (int k = 1; k <= n; ++k)
            {
                double zk = cA.z + gap * (double)k / (n + 1);   // 均匀分布
                for (size_t p = cA.begin; p < cA.end; ++p)
                {
                    const CountPoint& src = points[p];
                    if (src.x_mm < x_lo || src.x_mm > x_hi) continue;   // X范围约束
                    CountPoint np = src;
                    np.z_val = zk;
                    points.push_back(np);
                    ++added;
                }
            }
        }
        // ByDilate 与 AngleBucketInterpolation 共用同一套角度桶线性插值(平滑过渡)
        else if (mode == "AngleBucketInterpolation" || by_dilate)
        {
            // 角度桶插值: 每桶用前A/后B平均点线性插值(平滑过渡)
            // 桶宽沿用雷达实际角度分辨率(见上方 bucket_deg), 与一圈点数基本 1:1 对齐
            // ys: 仅 ByDilate 需要 —— 逐桶取中位 y 做平滑判定
            struct Buck { double sx, sy; int cnt; std::vector<double> ys;
                          Buck() : sx(0), sy(0), cnt(0) {} };
            std::vector<Buck> bA((std::size_t)nbuckets), bB((std::size_t)nbuckets);
            auto fill = [&](const Circle& c, std::vector<Buck>& out)
            {
                for (size_t p = c.begin; p < c.end; ++p)
                {
                    double deg = std::atan2(points[p].x_mm, -points[p].y_mm) * 57.29577951308232;
                    if (deg < 0) deg += 360.0;
                    int b = (int)(deg / bucket_deg + 0.5);   // 四舍五入到最近桶中心
                    if (b >= nbuckets) b -= nbuckets;              // 359.99° 会取到 nbuckets
                    if (b < 0) b = 0;
                    out[(std::size_t)b].sx += points[p].x_mm;
                    out[(std::size_t)b].sy += points[p].y_mm;
                    out[(std::size_t)b].cnt++;
                    if (by_dilate) out[(std::size_t)b].ys.push_back(points[p].y_mm);
                }
            };
            fill(cA, bA);
            fill(cB, bB);
            for (int k = 1; k <= n; ++k)
            {
                double zk = cA.z + gap * (double)k / (n + 1);   // 均匀分布
                double t = (zk - cA.z) / gap;                    // 0~1
                int row_added = 0;
                for (int b = 0; b < nbuckets; ++b)
                {
                    if (bA[b].cnt <= 0 || bB[b].cnt <= 0) continue;
                    // ByDilate 的平滑闸门: 逐桶比较两端"中位 y"。
                    // 原来按整圈均值判定 —— 少量噪点/边缘点就能把整圈均值拉偏,
                    // 使整个空隙被误判为"跨层/换物体"而整体跳过、留下大间隙。
                    // 改成逐桶中位数后: 只跳过确实突变的方向, 其余方向照常平滑补行。
                    if (by_dilate)
                    {
                        double myA = median_of(bA[(std::size_t)b].ys);
                        double myB = median_of(bB[(std::size_t)b].ys);
                        if (std::fabs(myA - myB) > max_y_med_diff)
                        {
                            ++bucket_skip_med;
                            continue;
                        }
                    }
                    double ax = bA[b].sx / bA[b].cnt, ay = bA[b].sy / bA[b].cnt;
                    double bx = bB[b].sx / bB[b].cnt, by = bB[b].sy / bB[b].cnt;
                    double nx = ax + (bx - ax) * t;
                    double ny = ay + (by - ay) * t;
                    if (nx < x_lo || nx > x_hi) continue;        // X范围约束
                    CountPoint np;
                    np.x_mm = nx;
                    np.y_mm = ny;
                    np.z_val = zk;
                    points.push_back(np);
                    ++added;
                    ++row_added;
                }
                if (row_added == 0 && diag_printed < 20)
                {
                    ++diag_printed;
                    std::cerr << "[插值诊断] 补行 k=" << k << "/" << n
                              << " 产出 0 点 (无公共角度桶 或 全被 X 范围过滤)" << std::endl;
                }
            }
            if ((int)(gap / dilate_step) > MAX_FILL_LEVELS && diag_printed < 20)
            {
                ++diag_printed;
                std::cerr << "[插值诊断] 空隙 gap=" << gap << " 触发 10 条上限, 补完行间距="
                          << gap / (double)(MAX_FILL_LEVELS + 1)
                              << "mm (> 步长" << dilate_step << "mm)" << std::endl;
            }
        }
    }

    if (added > 0)
        std::cout << "[count] z空隙插值: 模式=" << mode
                  << ", 正常间隔=" << normal_gap
                  << ", 判定阈值=" << gap_threshold
                  << (by_dilate ? "(point_dilate)" : "(2×间隔)")
                  << ", 插值桶=" << bucket_deg << "°x" << nbuckets
                  << ", 新增" << added << "点"
                  << "  [空隙统计: 超门槛=" << gaps_hit
                  << ", 整圈Y均值跳过=" << gaps_skip_y
                  << ", 逐桶中位y跳过=" << bucket_skip_med
                  << ", 补行产出0点=" << gaps_skip_x << "]"
                  << std::endl;
    return added;
}

// ==================== KDE 密度峰值 ====================
// 对一组高度值做高斯核密度估计(KDE)，返回密度最大处(最密)的高度(mm)。
// 步骤:
//   1) 高度按 step 量化成直方图 bins(每bin=step毫米)，避免 O(点数×bins)
//   2) 直方图与离散高斯核卷积得到密度曲线，核覆盖 ±4×带宽
//   3) 取密度最大 bin 对应的高度
// bandwidth<=0 时自动: bw = 0.9 × min(标准差, IQR/1.34) × n^(-1/5)  (Silverman)
// 复杂度 O(n + bins×核宽度)。
double kde_peak_height(const std::vector<double>& vals, double bandwidth, double step)
{
    size_t n = vals.size();
    if (n == 0) return 0.0;
    if (step <= 0) step = 1.0;

    double vmin = vals[0], vmax = vals[0], sum = 0.0;
    for (size_t i = 0; i < n; ++i)
    {
        double v = vals[i];
        if (v < vmin) vmin = v;
        if (v > vmax) vmax = v;
        sum += v;
    }

    double bw = bandwidth;
    if (bw <= 0)
    {
        double mean = sum / (double)n;
        double sq = 0.0;
        for (size_t i = 0; i < n; ++i) { double d = vals[i] - mean; sq += d * d; }
        double sd = std::sqrt(sq / (double)n);
        std::vector<double> sorted(vals);
        std::sort(sorted.begin(), sorted.end());
        auto quantile = [&](double p) {
            double idx  = p * (double)(n - 1);
            size_t i0   = (size_t)std::floor(idx);
            size_t i1   = std::min(i0 + 1, n - 1);
            double frac = idx - i0;
            return sorted[i0] * (1.0 - frac) + sorted[i1] * frac;
        };
        double iqr = quantile(0.75) - quantile(0.25);
        double a   = std::min(sd, iqr / 1.34);
        if (a <= 0) return sorted[n / 2];            // 数据几乎全等, 直接返回中位数
        bw = 0.9 * a * std::pow((double)n, -0.2);
    }
    if (bw < step) bw = step;                        // 带宽不得小于扫描步长

    const size_t MAX_BINS = 200000;
    double pad   = 4.0 * bw;
    double lo0   = vmin - pad;
    size_t nb    = (size_t)std::ceil((vmax + pad - lo0) / step) + 1;
    if (nb < 1) nb = 1;
    if (nb > MAX_BINS) nb = MAX_BINS;                // 防止极端参数导致 bins 爆炸

    std::vector<double> hist(nb, 0.0);
    for (size_t i = 0; i < n; ++i)
    {
        long long b = (long long)std::floor((vals[i] - lo0) / step);
        if (b < 0 || (size_t)b >= nb) continue;
        hist[(size_t)b] += 1.0;
    }

    int r = (int)std::ceil(4.0 * bw / step);
    std::vector<double> kernel(2 * r + 1);
    for (int k = -r; k <= r; ++k)
    {
        double d = k * step / bw;
        kernel[k + r] = std::exp(-0.5 * d * d);
    }

    std::vector<double> dens(nb, 0.0);
    size_t best = 0;
    for (size_t i = 0; i < nb; ++i)
    {
        double acc = 0.0;
        for (int k = -r; k <= r; ++k)
        {
            long long j = (long long)i + k;
            if (j < 0 || (size_t)j >= nb) continue;
            acc += hist[(size_t)j] * kernel[k + r];
        }
        dens[i] = acc;
        if (acc > dens[best]) best = i;
    }
    return lo0 + (double)best * step;
}

// ==================== 按层统计 ====================
int calc_item_count(const std::vector<CountPoint>& points, const CountParams& box,
                    std::string* detail,
                    std::vector<uint8_t>* top_layer_img,
                    int* top_layer_img_w,
                    int* top_layer_img_h,
                    int* top_layer_level,
                    double* top_layer_area)
{
    if (points.empty() || box.l <= 0 || box.w <= 0 || box.h <= 0 || box.maxPerLayer <= 0)
        return 0;

    const bool use_half_max = (radar_cfg().count.layer_algorithm == "HalfMax");
    const bool use_kde_peak = (radar_cfg().count.layer_algorithm == "KdePeak");
    // 分层容差(三个算法统一): 基准 + 每层递增
    auto layer_tol = [&](int lv) { return radar_cfg().count.base_tolerance + lv * radar_cfg().count.per_level_tolerance; };

    std::unordered_map<int, std::vector<CountPoint>> layer_map;  // 每层最终有效点
    std::unordered_map<int, double> layer_max_h;                 // 每层实际最高点高度(mm)
    std::unordered_map<int, double> layer_lo_h;                  // 每层有效区间下界(mm)
    std::unordered_map<int, double> layer_hi_h;                  // 每层有效区间上界(mm)

    if (use_half_max || use_kde_peak)
    {
        // ① 粗筛: 本层中间 ~ 下一层中间 = [(level-0.5)×层高, (level+0.5)×层高]
        std::unordered_map<int, std::vector<CountPoint>> coarse_map;
        for (const auto& p : points)
        {
            if (FILTER_ENABLE && !point_in_polygon(p.x_mm, p.y_mm, radar_cfg().filter.polygon, radar_cfg().filter.poly_n))
                continue;
            double currentHeight = radar_cfg().count.floor_height - p.y_mm;  // 高度 = 地板值 - Y轴
            if (currentHeight < 0) continue;
            int level = (int)std::round(currentHeight / box.h);
            if (level < 1) continue;                                 // 第1层从半层处开始
            if (currentHeight < level * box.h - box.h / 2.0) continue;
            if (currentHeight > level * box.h + box.h / 2.0) continue;
            coarse_map[level].push_back(p);
        }
        // ② 定区间中心 + 精筛(容差随层递增):
        //    HalfMax: 中心 = ①内最高点(箱顶), 有效区间 = [中心-容差, 中心]
        //    KdePeak: 中心 = ①内高度的 KDE 密度峰值, 有效区间 = [中心-容差, 中心+容差]
        for (auto& kv : coarse_map)
        {
            int    level = kv.first;
            double tol   = layer_tol(level);
            double lo, hi;
            if (use_half_max)
            {
                double top = radar_cfg().count.floor_height - kv.second.front().y_mm;
                for (const auto& p : kv.second)
                    top = std::max(top, radar_cfg().count.floor_height - p.y_mm);
                lo = top - tol;
                hi = top;
            }
            else
            {
                std::vector<double> hs;
                hs.reserve(kv.second.size());
                for (const auto& p : kv.second) hs.push_back(radar_cfg().count.floor_height - p.y_mm);
                double peak = kde_peak_height(hs, radar_cfg().count.kde_bandwidth, radar_cfg().count.kde_step);
                lo = peak - tol;
                hi = peak + tol;
            }
            std::vector<CountPoint> keep;
            keep.reserve(kv.second.size());
            double top = std::numeric_limits<double>::lowest();
            for (const auto& p : kv.second)
            {
                double currentHeight = radar_cfg().count.floor_height - p.y_mm;
                if (currentHeight < lo || currentHeight > hi) continue;
                keep.push_back(p);
                if (currentHeight > top) top = currentHeight;
            }
            if (keep.empty()) continue;
            layer_map[level] = std::move(keep);
            layer_max_h[level] = top;
            layer_lo_h[level]  = lo;
            layer_hi_h[level]  = hi;
        }
    }
    else
    {
        for (const auto& p : points)
        {
            if (FILTER_ENABLE && !point_in_polygon(p.x_mm, p.y_mm, radar_cfg().filter.polygon, radar_cfg().filter.poly_n))
                continue;
            double currentHeight = radar_cfg().count.floor_height - p.y_mm;  // 高度 = 地板值 - Y轴
            if (currentHeight < 0) continue;
            int level = (int)std::round(currentHeight / box.h);
            double tolerance = radar_cfg().count.base_tolerance + level * radar_cfg().count.per_level_tolerance;
            if (std::abs(currentHeight - level * box.h) <= tolerance)
            {
                layer_map[level].push_back(p);
                auto im = layer_max_h.find(level);
                layer_max_h[level] = (im == layer_max_h.end()) ? currentHeight
                                                              : std::max(im->second, currentHeight);
            }
        }
    }

    // 每层面积/图像只算一次，详情与结果共用，避免重复构建图像
    std::ostringstream oss;
    std::unordered_map<int, double> layer_area;
    std::unordered_map<int, std::vector<uint8_t>> layer_img;
    std::unordered_map<int, std::pair<int, int>> layer_img_size;
    for (int h = 5; h > 0; h--)
    {
        auto it = layer_map.find(h);
        if (it == layer_map.end()) continue;
        oss << "  层" << h << ": 点数=" << it->second.size();
        // 高度范围: 该层算法对应的有效高度区间(每层都显示)
        double h_lo, h_hi;
        if (use_half_max || use_kde_peak)
        {
            h_lo = layer_lo_h[h];
            h_hi = layer_hi_h[h];
        }
        else
        {
            double tol = layer_tol(h);
            h_lo = h * box.h - tol;
            h_hi = h * box.h + tol;
        }
        // setprecision(0) 在数值较大时会被切为科学计数法(1e+03)，直接四舍五入输出整数
        oss << ", 高度范围（" << (long long)std::llround(h_lo) << "," << (long long)std::llround(h_hi) << "）"
            << ", 最大高度=" << (long long)std::llround(layer_max_h[h]);
        if (it->second.size() > 1000)
        {
            double area = 0.0;
            int img_w = 0, img_h = 0;
            std::vector<uint8_t> img = points_to_image(it->second, &area, &img_w, &img_h);
            layer_area[h] = area;
            layer_img[h] = std::move(img);
            layer_img_size[h] = std::make_pair(img_w, img_h);
            double item_cnt = area / (box.l * box.w);   // 未取整的个数(面积/单件面积)
            oss << ", 面积=" << std::fixed << std::setprecision(1) << area << "mm^2"
                << ", 个数" << std::setprecision(2) << item_cnt << "个";
        }
        oss << "\n";
    }

    int layersCount = box.maxPerLayer;   // 每层最大个数
    int result = 0;
    int top_lv = 0;
    double top_area = 0.0;
    for (int h = 5; h > 0; h--)          // 最多5层，从顶层往下找
    {
        auto it = layer_map.find(h);
        if (it != layer_map.end() && it->second.size() > 1000)
        {
            double area = layer_area[h];
            if (area > 0)
            {
                int i = (int)std::min(std::round(area / (box.l * box.w)), (double)layersCount);
                if (i > 0)
                {
                    result = layersCount * (h - 1) + i;
                    top_lv = h;
                    top_area = area;
                    break;
                }
            }
        }
    }
    if (detail) *detail = oss.str();
    // 输出被计数层(最高有效层)的图像/尺寸/层号/面积
    if (top_layer_img && top_lv > 0)
    {
        auto it = layer_img.find(top_lv);
        if (it != layer_img.end()) *top_layer_img = it->second;
        auto sit = layer_img_size.find(top_lv);
        if (sit != layer_img_size.end())
        {
            if (top_layer_img_w) *top_layer_img_w = sit->second.first;
            if (top_layer_img_h) *top_layer_img_h = sit->second.second;
        }
    }
    if (top_layer_level) *top_layer_level = top_lv;
    if (top_layer_area) *top_layer_area = top_area;
    return result;
}

// ==================== count 结果文件后台保存 ====================
void save_count_files_async(const std::string& radar_id,
                            const std::vector<CountPoint>& points,
                            const std::vector<uint8_t>& top_layer_img,
                            int top_img_w, int top_img_h,
                            const CountParams& params,
                            const std::string& layer_detail,
                            int total, int top_lv, double top_area,
                            const std::vector<CountPoint>* points_before)
{
    // C++11 兼容：按值捕获（拷贝到线程），不阻塞 TCP 响应
    // 插值前快照按值拷贝进线程，避免 pts 生命周期问题
    std::vector<CountPoint> before_copy =
        points_before ? *points_before : std::vector<CountPoint>();
    std::thread([radar_id, points, before_copy, top_layer_img, params, layer_detail,
                 total, top_lv, top_area, top_img_w, top_img_h]() {
        // 本线程是 detached：异常逃逸会调用 std::terminate 直接杀掉整个进程，
        // 因此整个保存流程必须有兜底捕获。
        try
        {
        RadarScope scope(radar_id);   // 线程内绑定该雷达配置(多边形/膨胀/日志等)
        // 目录: data/日期/时间_taskId（由 io_utils 统一创建）
        std::string out_dir = make_task_dir(params.task_id);
        size_t pt_cnt = points.size();

        // PCD：y 换成高度(radar_cfg().count.floor_height - y_mm)
        auto save_pcd = [&](const std::vector<CountPoint>& src, const char* name)
        {
            std::vector<double> xs, ys, zs;
            xs.reserve(src.size()); ys.reserve(src.size()); zs.reserve(src.size());
            for (const auto& p : src)
            {
                xs.push_back(p.x_mm);
                ys.push_back(radar_cfg().count.floor_height - p.y_mm);
                zs.push_back(p.z_val);
            }
            save_pcd_ascii(out_dir + "/" + name, xs, ys, zs);
            std::cout << "[count] 点云已保存: " << out_dir << "/" << name << " ("
                      << src.size() << " 点, Y=高度)" << std::endl;
        };
        save_pcd(points, "count_data.pcd");                  // 插值后(原名, 兼容现有流程)
        if (!before_copy.empty())                            // 插值前快照, 便于对比
            save_pcd(before_copy, "count_data_before.pcd");

        // 图片1：全点X-Z平面图
        double all_area = 0.0;
        int all_w = 0, all_h = 0;
        std::vector<uint8_t> all_img = points_to_image(points, &all_area, &all_w, &all_h);
        if (save_jpg_gray(out_dir + "/count_points.jpg", all_w, all_h, all_img))
            std::cout << "[count] 点云图已保存: " << out_dir << "/count_points.jpg ("
                      << all_w << "x" << all_h << ", X-Z平面, 面积≈"
                      << std::fixed << std::setprecision(1)
                      << all_area << "mm^2)" << std::endl;

        // 图片2：被计数层(最高有效层)图像
        if (top_lv > 0 && !top_layer_img.empty() && top_img_w > 0 && top_img_h > 0)
        {
            if (save_jpg_gray(out_dir + "/count_layer.jpg", top_img_w, top_img_h, top_layer_img))
                std::cout << "[count] 有效层图已保存: " << out_dir << "/count_layer.jpg ("
                          << top_img_w << "x" << top_img_h << ", 层" << top_lv
                          << ", 面积≈" << std::fixed
                          << std::setprecision(1) << top_area << "mm^2)" << std::endl;
        }

        // 日志
        if (cfg.log.enable)
        {
            std::ofstream lf(out_dir + "/log.txt");
            if (lf.is_open())
            {
                std::ostringstream sum;
                sum << "==============================\n";
                sum << "[count] task_id: " << params.task_id << "\n";
                sum << "[count] box: l=" << params.l
                    << ", w=" << params.w
                    << ", h=" << params.h
                    << ", maxPerLayer=" << params.maxPerLayer << "\n";
                sum << "[count] floor_height: " << radar_cfg().count.floor_height << " mm\n";
                sum << "[count] 采集点数: " << pt_cnt << "\n";
                sum << "[count] 计数层: 层" << top_lv
                    << " (面积=" << std::fixed << std::setprecision(1)
                    << top_area << "mm^2)\n";
                sum << "[count] 分层情况:\n" << layer_detail;
                sum << "[count] 计数结果: " << total << "\n";
                sum << "[count] 输出文件:\n";
                sum << "  " << out_dir << "/count_data.pcd\n";
                sum << "  " << out_dir << "/count_points.jpg (" << all_w << "x" << all_h << ")\n";
                if (top_lv > 0 && !top_layer_img.empty() && top_img_w > 0 && top_img_h > 0)
                    sum << "  " << out_dir << "/count_layer.jpg ("
                        << top_img_w << "x" << top_img_h << ")\n";
                sum << "==============================\n";
                lf << sum.str();   // 字符串已是 UTF-8，直接写
                lf.close();
            }
        }
        std::cout << "[count] 输出目录: " << out_dir << std::endl;
        }
        catch (const std::exception& e)
        {
            std::cerr << "[count] 后台保存异常: " << e.what() << std::endl;
        }
        catch (...) { std::cerr << "[count] 后台保存发生未知异常" << std::endl; }
    }).detach();
}