|
|
// 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();
|
|
|
}
|