|
|
// detect.cpp - 检测模式(check)实现
|
|
|
// 从原始 demo_collect_multi 的"分析部分"抽出：采集已由 ScanSession 统一完成，
|
|
|
// 本模块只负责对点集做 Y偏移 → 保存PCD → 起点/终点截面切片 → 统计。
|
|
|
#include "detect.h"
|
|
|
#include "config.h"
|
|
|
#include "io_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);
|
|
|
|
|
|
// 日志：Log_Flag 开启时同时写 log.txt（UTF-8）
|
|
|
std::ofstream log_file;
|
|
|
if (Log_Flag)
|
|
|
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 << gbk_to_utf8(s);
|
|
|
};
|
|
|
|
|
|
// 本地副本：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 + 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(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 = (Speed_Z > 0) ? "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 + Z_RANGE_MM;
|
|
|
double z_last_end = gz_max;
|
|
|
double z_last_start = gz_max - 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(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 越大越靠上）
|
|
|
std::vector<uint8_t> slice_img(img_w * img_h, 0);
|
|
|
double rect_len_mm = 10.0; // 时间模式(无速度)默认10mm
|
|
|
if (Speed_Z > 0)
|
|
|
rect_len_mm = Speed_Z * 10.0 / Lidar_Hz * Rect_Length_Ratio;
|
|
|
int len_px = std::max(1, (int)(rect_len_mm / img_scale));
|
|
|
int half_w = 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(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截面区间长度: " << 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();
|
|
|
}
|