leida_simi/detect.cpp

// detect.cpp - 检测模式(check)实现
// 从原始 demo_collect_multi 的"分析部分"抽出：采集已由 ScanSession 统一完成，
// 本模块只负责对点集做 Y偏移 → 保存PCD → 起点/终点截面切片 → 统计。
#include "detect.h"
#include "config.h"
#include "io_utils.h"
#include "lidar_utils.h"
#include <algorithm>
#include <cmath>
#include <cstring>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <sstream>

void detect_analyze_slices(const std::vector<CountPoint>& points,
                           const std::string& out_dir,
                           DetectResult* result)
{
    make_dirs(out_dir);

    // 日志：cfg.log.enable 开启时同时写 log.txt（UTF-8）
    std::ofstream log_file;
    if (cfg.log.enable)
        log_file.open(out_dir + "/log.txt");
    auto log_msg = [&log_file](const std::string& s) {
        std::cout << s;
        if (log_file.is_open()) log_file << s;   // 字符串已是 UTF-8，直接写
    };

    // 本地副本：Y轴偏移会修改 y_mm
    std::vector<CountPoint> pts = points;
    if (pts.empty())
    {
        log_msg("[截面] 无点云数据，跳过截面切片。\n");
        log_file.close();
        return;
    }

    // ==========> 保存原始PCD（Y偏移之前）<==========
    std::vector<double> xs, ys, zs;
    xs.reserve(pts.size()); ys.reserve(pts.size()); zs.reserve(pts.size());
    for (const auto& pt : pts)
    {
        xs.push_back(pt.x_mm);
        ys.push_back(pt.y_mm);
        zs.push_back(pt.z_val);
    }
    save_pcd_ascii(out_dir + "/lidar_data.pcd", xs, ys, zs);
    log_msg("[文件] 原始点云已保存: " + out_dir + "/lidar_data.pcd (" +
            std::to_string(pts.size()) + " 点, Y未偏移)\n");

    // ========== Y轴偏移 ==========
    if (SHIFT_Y_POSITIVE)
    {
        double ymin = 1e9;
        for (const auto& pt : pts)
            if (pt.y_mm < ymin) ymin = pt.y_mm;
        if (ymin < 0)
        {
            double y_offset = -ymin;
            for (auto& pt : pts)
                pt.y_mm += y_offset;
            log_msg("[信息] Y轴偏移: +" + std::to_string(y_offset) + " mm\n");
        }
    }

    // ==========> 保存Y偏移后PCD <==========
    {
        std::vector<double> sxs, sys, szs;
        sxs.reserve(pts.size()); sys.reserve(pts.size()); szs.reserve(pts.size());
        for (const auto& pt : pts)
        {
            sxs.push_back(pt.x_mm);
            sys.push_back(pt.y_mm);
            szs.push_back(pt.z_val);
        }
        save_pcd_ascii(out_dir + "/lidar_data_shifted.pcd", sxs, sys, szs);
        log_msg("[文件] Y偏移后点云已保存: " + out_dir + "/lidar_data_shifted.pcd (" +
                std::to_string(pts.size()) + " 点, Y已偏移)\n");
    }

    // ============ 截面切片：Y轴切片 → X-Z平面映射 (起点+终点各一张) ============

    // Step 1: Y范围（全图）
    double y_min = pts[0].y_mm, y_max = pts[0].y_mm;
    for (const auto& pt : pts)
    {
        if (pt.y_mm < y_min) y_min = pt.y_mm;
        if (pt.y_mm > y_max) y_max = pt.y_mm;
    }
    log_msg("[截面] Y全图范围: " + std::to_string(y_min) + " ~ " +
            std::to_string(y_max) + " mm\n");

    // Step 2: Y截面区间 = [Y最小, Y最小+Y_Range_MM]
    double y_slice_max = y_min + radar_cfg().detect.y_range_mm;
    std::vector<const CountPoint*> y_filtered;
    y_filtered.reserve(pts.size());
    for (const auto& pt : pts)
        if (pt.y_mm >= y_min && pt.y_mm <= y_slice_max)
            y_filtered.push_back(&pt);
    log_msg("[截面] Y截面区间: " + std::to_string(y_min) + " ~ " +
            std::to_string(y_slice_max) + " mm (长度=" + std::to_string(radar_cfg().detect.y_range_mm) +
            "mm, 配置Y_Range_MM)\n");
    log_msg("[截面] 参与点数: " + std::to_string(y_filtered.size()) + " / " +
            std::to_string(pts.size()) + "\n");
    if (y_filtered.empty())
    {
        log_msg("[截面] Y截面区间内无点，跳过截面生成。\n");
        log_file.close();
        return;
    }

    // Step 3: 全局X范围(全图) + Z范围(Y过滤后)
    double gx_min = pts[0].x_mm, gx_max = pts[0].x_mm;
    for (const auto& pt : pts)
    {
        if (pt.x_mm < gx_min) gx_min = pt.x_mm;
        if (pt.x_mm > gx_max) gx_max = pt.x_mm;
    }
    double gz_min = y_filtered[0]->z_val, gz_max = y_filtered[0]->z_val;
    for (const auto* pt : y_filtered)
    {
        if (pt->z_val < gz_min) gz_min = pt->z_val;
        if (pt->z_val > gz_max) gz_max = pt->z_val;
    }
    const char* z_unit = z_is_distance_mode() ? "mm" : "ms";
    log_msg("[截面] 全局X范围(全图点云): " + std::to_string(gx_min) + " ~ " +
            std::to_string(gx_max) + " mm (图像高度方向)\n");
    log_msg("[截面] 全局Z范围: " + std::to_string(gz_min) + " ~ " +
            std::to_string(gz_max) + " " + std::string(z_unit) + "\n");
    log_msg("[截面] 每张图高度由全图 X最大~X最小 范围决定\n");

    // Step 4: 两个Z区间
    double z_first_start = gz_min;
    double z_first_end   = gz_min + radar_cfg().detect.z_range_mm;
    double z_last_end    = gz_max;
    double z_last_start  = gz_max - radar_cfg().detect.z_range_mm;
    if (z_first_end  > gz_max) z_first_end  = gz_max;
    if (z_last_start < gz_min) z_last_start = gz_min;

    // ====== 通用：Y+Z过滤 → X-Z平面二值映射 ======
    auto make_xz_slice = [&](double z0, double z1, const std::string& label,
                             const std::string& out_pcd, const std::string& out_jpg,
                             double* out_fill_pct,
                             int* out_w, int* out_h,
                             double* out_y_max, double* out_y_med,
                             std::vector<uint8_t>* out_img,
                             bool right_align)
    {
        log_msg("[截面-" + label + "] Z区间: " + std::to_string(z0) + " ~ " +
                std::to_string(z1) + " " + std::string(z_unit) + "\n");

        std::vector<double> slice_xs, slice_ys, slice_zs;
        for (const auto* pt : y_filtered)
            if (pt->z_val >= z0 && pt->z_val <= z1)
            {
                slice_xs.push_back(pt->x_mm);
                slice_ys.push_back(pt->y_mm);
                slice_zs.push_back(pt->z_val);
            }
        log_msg("[截面-" + label + "] 切片点数: " +
                std::to_string(slice_xs.size()) + "\n");
        if (slice_xs.empty())
        {
            log_msg("[截面-" + label + "] 无数据，跳过。\n");
            return;
        }

        double sx_range = gx_max - gx_min;
        if (sx_range < 1.0) sx_range = 1.0;
        const int img_w = std::max(1, (int)std::ceil(radar_cfg().detect.z_range_mm));   // 宽 = Z_Range_MM
        const double img_scale = 1.0;                                 // 1px = 1mm
        int img_h = std::max(1, (int)std::ceil(sx_range));
        if (out_w) *out_w = img_w;
        if (out_h) *out_h = img_h;

        double y_max_slice = *std::max_element(slice_ys.begin(), slice_ys.end());
        std::vector<double> ys_sorted = slice_ys;
        std::sort(ys_sorted.begin(), ys_sorted.end());
        double y_med = (ys_sorted.size() % 2 == 1)
            ? ys_sorted[ys_sorted.size() / 2]
            : (ys_sorted[ys_sorted.size() / 2 - 1] + ys_sorted[ys_sorted.size() / 2]) / 2.0;
        {
            std::ostringstream oss;
            oss << "[截面-" << label << "] Y统计: 最大值=" << std::fixed << std::setprecision(1)
                << y_max_slice << " mm, 中位数=" << y_med << " mm\n";
            log_msg(oss.str());
        }
        if (out_y_max) *out_y_max = y_max_slice;
        if (out_y_med) *out_y_med = y_med;

        save_pcd_ascii(out_pcd, slice_xs, slice_ys, slice_zs);
        log_msg("[文件] " + out_pcd + " (" + std::to_string(slice_xs.size()) + " 点)\n");

        // X-Z平面映射：矩形渲染（横轴=Z 左近→右远, 纵轴=X 越大越靠上）
        // 速度来源: configuration(变速)模式用剖面平均速度近似; 否则匀速 radar_cfg().scan.z_speed; 时间模式默认10mm
        std::vector<uint8_t> slice_img(img_w * img_h, 0);
        double rect_len_mm = 10.0;   // 时间模式(无速度)默认10mm
        double prof_v = speed_profile_avg_velocity_mm_s();
        if (radar_cfg().scan.z_speed_type == "configuration" && prof_v > 0)
            rect_len_mm = prof_v / radar_cfg().radar.hz * radar_cfg().detect.rect_length_ratio;
        else if (radar_cfg().scan.z_speed > 0)
            rect_len_mm = radar_cfg().scan.z_speed * 10.0 / radar_cfg().radar.hz * radar_cfg().detect.rect_length_ratio;
        int len_px = std::max(1, (int)(rect_len_mm / img_scale));
        int half_w = radar_cfg().detect.rect_width_px / 2;
        if (half_w < 1) half_w = 1;

        double z_ref = z0;
        int x_offset = 0;
        if (right_align && !slice_zs.empty())
        {
            z_ref = *std::max_element(slice_zs.begin(), slice_zs.end());
            x_offset = img_w - len_px;
        }

        for (size_t i = 0; i < slice_xs.size(); i++)
        {
            int cx = right_align
                ? (int)(x_offset - (z_ref - slice_zs[i]) / img_scale)
                : (int)((slice_zs[i] - z0) / img_scale + x_offset);
            int cy = (int)((gx_max - slice_xs[i]) / img_scale);
            for (int dx = 0; dx < len_px; dx++)
            {
                for (int dy = -half_w; dy <= half_w; dy++)
                {
                    int px = cx + dx;
                    int py = cy + dy;
                    if (px >= 0 && px < img_w && py >= 0 && py < img_h)
                        slice_img[py * img_w + px] = 255;
                }
            }
        }

        int filled = 0;
        for (int i = 0; i < img_w * img_h; i++)
            if (slice_img[i] > 0) filled++;
        double fill_pct = (img_w * img_h > 0)
            ? filled * 100.0 / (img_w * img_h) : 0.0;
        if (out_fill_pct) *out_fill_pct = fill_pct;

        {
            std::ostringstream oss;
            oss << "[截面-" << label << "] 白色占比: " << filled << "/"
                << (img_w * img_h) << " 像素 ("
                << std::fixed << std::setprecision(2) << fill_pct << "%)\n";
            log_msg(oss.str());
        }

        if (save_jpg_gray(out_jpg, img_w, img_h, slice_img))
            log_msg("[图像] " + out_jpg + " (" + std::to_string(img_w) + "x" +
                    std::to_string(img_h) + ", Z=" + std::to_string(z0) + "~" +
                    std::to_string(z1) + " " + std::string(z_unit) + ", X=" +
                    std::to_string(gx_min) + "~" + std::to_string(gx_max) + "mm)\n");

        if (out_img) *out_img = std::move(slice_img);
    };

    double start_fill_pct = -1.0, end_fill_pct = -1.0;
    int start_w = 0, start_h = 0, end_w = 0, end_h = 0;
    double start_y_max = 0.0, start_y_med = 0.0, end_y_max = 0.0, end_y_med = 0.0;
    std::vector<uint8_t> start_img, end_img;

    // 起点截面：左对齐
    make_xz_slice(z_first_start, z_first_end, "起点",
                  out_dir + "/lidar_slice_start.pcd",
                  out_dir + "/lidar_slice_start.jpg",
                  &start_fill_pct, &start_w, &start_h,
                  &start_y_max, &start_y_med, &start_img, false);

    // 终点截面（结束前 Z_Range_MM）：右对齐
    if (z_last_start < z_last_end)
    {
        make_xz_slice(z_last_start, z_last_end, "终点",
                      out_dir + "/lidar_slice_end.pcd",
                      out_dir + "/lidar_slice_end.jpg",
                      &end_fill_pct, &end_w, &end_h,
                      &end_y_max, &end_y_med, &end_img, true);
    }
    else
    {
        log_msg("[截面-终点] Z总长不足" + std::to_string(radar_cfg().detect.z_range_mm) + "mm，跳过终点截面。\n");
    }

    // ====== 合并对比图：start左半区, end右半区 ======
    if (!start_img.empty() && !end_img.empty() && start_w == end_w && start_h == end_h)
    {
        int comb_w = start_w * 2;
        int comb_h = start_h;
        std::vector<uint8_t> combined(comb_w * comb_h, 0);
        for (int y = 0; y < comb_h; y++)
        {
            std::memcpy(&combined[y * comb_w], &start_img[y * start_w], start_w);
            std::memcpy(&combined[y * comb_w + start_w], &end_img[y * start_w], start_w);
            combined[y * comb_w + start_w] = 128;
        }
        if (save_jpg_gray(out_dir + "/lidar_slice_combined.jpg", comb_w, comb_h, combined))
            log_msg("[图像] " + out_dir + "/lidar_slice_combined.jpg (" +
                    std::to_string(comb_w) + "x" + std::to_string(comb_h) +
                    ", 左=start 右=end)\n");
    }

    // ====== 回传检测结果（供TCP返回） ======
    if (result)
    {
        result->valid          = true;
        result->start_fill_pct = start_fill_pct;
        result->end_fill_pct   = end_fill_pct;
        result->start_y_max    = start_y_max;
        result->start_y_med    = start_y_med;
        result->end_y_max      = end_y_max;
        result->end_y_med      = end_y_med;
    }

    // ====== 日志摘要 ======
    {
        std::ostringstream sum;
        sum << "\n========== 截面检测摘要 ==========\n";
        sum << "输入点数: " << pts.size() << "\n";
        sum << "Y范围(全部点): " << y_min << " ~ " << y_max << " mm\n";
        sum << "Y截面参与点数: " << y_filtered.size() << "\n";
        sum << "Z截面区间长度: " << radar_cfg().detect.z_range_mm << " mm\n";
        sum << "全局X范围: " << gx_min << " ~ " << gx_max << " mm\n";
        sum << "全局Z范围: " << gz_min << " ~ " << gz_max << " " << z_unit << "\n";
        if (start_w > 0)
            sum << "起点截面图像尺寸: " << start_w << "x" << start_h << " px\n";
        if (end_w > 0)
            sum << "终点截面图像尺寸: " << end_w << "x" << end_h << " px\n";
        if (start_fill_pct >= 0.0)
            sum << "起点截面白色占比: " << std::fixed << std::setprecision(2)
                << start_fill_pct << "%\n";
        if (end_fill_pct >= 0.0)
            sum << "终点截面白色占比: " << std::fixed << std::setprecision(2)
                << end_fill_pct << "%\n";
        sum << "==============================\n";
        log_msg(sum.str());
    }
    log_file.close();
}