178 lines
7.7 KiB
C++
178 lines
7.7 KiB
C++
#include "DetectPresenter.h"
|
|
#include "WD_particleSizeMeasure_Export.h"
|
|
#include <fstream>
|
|
|
|
DetectPresenter::DetectPresenter(/* args */)
|
|
{
|
|
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_particleSegVersion());
|
|
}
|
|
|
|
DetectPresenter::~DetectPresenter()
|
|
{
|
|
}
|
|
|
|
int DetectPresenter::DetectWorkpiece(
|
|
int cameraIndex,
|
|
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
|
const VrAlgorithmParams& algorithmParams,
|
|
const VrDebugParam& debugParam,
|
|
LaserDataLoader& dataLoader,
|
|
const double clibMatrix[16],
|
|
DetectionResult& detectionResult)
|
|
{
|
|
if (laserLines.empty()) {
|
|
LOG_WARNING("No laser lines data available\n");
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|
}
|
|
|
|
// 获取当前相机的校准参数
|
|
VrCameraPlaneCalibParam cameraCalibParamValue;
|
|
const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
|
|
if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue)) {
|
|
cameraCalibParam = &cameraCalibParamValue;
|
|
}
|
|
|
|
// 保存debug数据
|
|
std::string timeStamp = CVrDateUtils::GetNowTime();
|
|
if(debugParam.enableDebug && debugParam.savePointCloud){
|
|
LOG_INFO("[Algo Thread] Debug mode is enabled, saving point cloud data\n");
|
|
std::string fileName = debugParam.debugOutputPath + "/Laserline_" + std::to_string(cameraIndex) + "_" + timeStamp + ".txt";
|
|
dataLoader.SaveLaserScanData(fileName, laserLines, laserLines.size(), 0.0, 0, 0);
|
|
}
|
|
|
|
int nRet = SUCCESS;
|
|
|
|
// 转换为算法需要的XYZ格式
|
|
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
|
int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
|
|
if (convertResult != SUCCESS || xyzData.empty()) {
|
|
LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|
}
|
|
|
|
// 准备粒度检测参数
|
|
SWD_paricleSizeParam particleSizeParam;
|
|
// 直接赋值，因为现在结构匹配
|
|
particleSizeParam.minSize.length = algorithmParams.particleSizeParam.minSize.length;
|
|
particleSizeParam.minSize.width = algorithmParams.particleSizeParam.minSize.width;
|
|
particleSizeParam.minSize.height = algorithmParams.particleSizeParam.minSize.height;
|
|
particleSizeParam.alarmSize.length = algorithmParams.particleSizeParam.alarmSize.length;
|
|
particleSizeParam.alarmSize.width = algorithmParams.particleSizeParam.alarmSize.width;
|
|
particleSizeParam.alarmSize.height = algorithmParams.particleSizeParam.alarmSize.height;
|
|
|
|
// 准备算法参数
|
|
SWD_PSM_algoParam algoParam;
|
|
algoParam.cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
|
|
algoParam.cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
|
|
algoParam.cornerParam.scale = algorithmParams.cornerParam.scale;
|
|
algoParam.cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
|
|
algoParam.cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
|
|
algoParam.cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;
|
|
|
|
algoParam.growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
|
|
algoParam.growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
|
|
algoParam.growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
|
|
algoParam.growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
|
|
algoParam.growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
|
|
algoParam.growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;
|
|
|
|
algoParam.filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
|
|
algoParam.filterParam.outlierTh = algorithmParams.filterParam.outlierTh;
|
|
|
|
// 准备平面校准参数
|
|
SSG_planeCalibPara groundCalibPara;
|
|
if(cameraCalibParam){
|
|
memcpy(groundCalibPara.planeCalib, cameraCalibParam->planeCalib, sizeof(double) * 9);
|
|
memcpy(groundCalibPara.invRMatrix, cameraCalibParam->invRMatrix, sizeof(double) * 9);
|
|
groundCalibPara.planeHeight = cameraCalibParam->planeHeight;
|
|
} else {
|
|
// 使用默认单位矩阵
|
|
double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
|
memcpy(groundCalibPara.planeCalib, identity, sizeof(double) * 9);
|
|
memcpy(groundCalibPara.invRMatrix, identity, sizeof(double) * 9);
|
|
groundCalibPara.planeHeight = -1.0;
|
|
}
|
|
|
|
if(debugParam.enableDebug && debugParam.printDetailLog)
|
|
{
|
|
LOG_INFO("[Algo Thread] minSize l:%.1f, w:%.1f, h:%.1f\n", particleSizeParam.minSize.length, particleSizeParam.minSize.width, particleSizeParam.minSize.height);
|
|
LOG_INFO(" alarmSize l:%.1f, w:%.1f, h:%.1f\n",particleSizeParam.alarmSize.length, particleSizeParam.alarmSize.width, particleSizeParam.alarmSize.height);
|
|
LOG_INFO("[Algo Thread] Corner: cornerTh=%.1f, scale=%.1f\n", algoParam.cornerParam.cornerTh, algoParam.cornerParam.scale);
|
|
LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", algoParam.filterParam.continuityTh, algoParam.filterParam.outlierTh);
|
|
LOG_INFO("Plane height: %.3f\n", groundCalibPara.planeHeight);
|
|
}
|
|
|
|
// 数据预处理：调平和去除地面
|
|
if(cameraCalibParam){
|
|
LOG_DEBUG("Processing data with plane calibration\n");
|
|
for(size_t i = 0; i < xyzData.size(); i++){
|
|
wd_lineDataR(xyzData[i], cameraCalibParam->planeCalib, groundCalibPara.planeHeight);
|
|
}
|
|
}
|
|
|
|
int errCode = 0;
|
|
CVrTimeUtils oTimeUtils;
|
|
std::vector<SWD_ParticlePosInfo> particles;
|
|
|
|
LOG_DEBUG("before wd_particleSizeMeasure \n");
|
|
|
|
// 调用粒度检测算法
|
|
wd_particleSizeMeasure(
|
|
xyzData,
|
|
particleSizeParam,
|
|
groundCalibPara,
|
|
algoParam,
|
|
particles,
|
|
&errCode
|
|
);
|
|
|
|
LOG_DEBUG("after wd_particleSizeMeasure \n");
|
|
|
|
LOG_INFO("wd_particleSizeMeasure: detected %d particles, err=%d runtime=%.3fms\n", (int)particles.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
|
|
|
ERR_CODE_RETURN(errCode);
|
|
|
|
// 保存检测到的颗粒信息到结果
|
|
detectionResult.positions.clear();
|
|
for(size_t i = 0; i < particles.size(); i++) {
|
|
ParticleSizePosition pos;
|
|
|
|
pos.index = i + 1;
|
|
pos.length = particles[i].size.length;
|
|
pos.width = particles[i].size.width;
|
|
pos.height = particles[i].size.height;
|
|
|
|
// 保存颗粒尺寸
|
|
pos.width = particles[i].size.width;
|
|
pos.height = particles[i].size.height;
|
|
|
|
// 检查是否超过告警阈值
|
|
pos.isAlarm = (pos.length > particleSizeParam.alarmSize.length || pos.width > particleSizeParam.alarmSize.width || pos.height > particleSizeParam.alarmSize.height);
|
|
|
|
detectionResult.positions.push_back(pos);
|
|
|
|
if(debugParam.enableDebug && debugParam.printDetailLog) {
|
|
LOG_INFO("Particle %d: size=(%.2f, %.2f, %.2f), alarm=%d\n", pos.index, pos.length, pos.width, pos.height, pos.isAlarm);
|
|
}
|
|
}
|
|
|
|
// 生成可视化图像
|
|
if (!xyzData.empty()) {
|
|
// 转换颗粒信息格式
|
|
std::vector<SimpleParticleInfo> simpleParticles;
|
|
for (const auto& p : particles) {
|
|
SimpleParticleInfo sp;
|
|
for (int i = 0; i < 8; i++) {
|
|
sp.vertix[i] = p.vertix[i];
|
|
}
|
|
sp.size.width = p.size.width;
|
|
sp.size.height = p.size.height;
|
|
simpleParticles.push_back(sp);
|
|
}
|
|
|
|
detectionResult.image = PointCloudImageUtils::GeneratePointCloudImageWithParticles(xyzData, simpleParticles, cameraIndex);
|
|
}
|
|
|
|
return SUCCESS;
|
|
}
|