186 lines
8.0 KiB
C++
186 lines
8.0 KiB
C++
|
#include "DetectPresenter.h"
|
|||
|
|
#include "SX_lapWeldDetection_Export.h"
|
|||
|
|
|
|||
|
|
|
|||
|
|
DetectPresenter::DetectPresenter(/* args */)
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
DetectPresenter::~DetectPresenter()
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int DetectPresenter::DetectLapWeld(std::vector<SVzNL3DLaserLine>& detectionDataCache,
|
|||
|
|
const SSX_lapWeldParam& algoParam,
|
|||
|
|
const SSG_planeCalibPara& cameraCalibParam,
|
|||
|
|
const VrDebugParam& debugParam,
|
|||
|
|
LaserDataLoader& dataLoader,
|
|||
|
|
const double clibMatrix[16],
|
|||
|
|
DetectionResult& detectionResult)
|
|||
|
|
{
|
|||
|
|
if (detectionDataCache.empty()) {
|
|||
|
|
LOG_WARNING("No cached detection data available\n");
|
|||
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 2. 数据预处理：调平和去除地面（使用当前相机的调平参数）
|
|||
|
|
for (size_t i = 0; i < detectionDataCache.size(); i++) {
|
|||
|
|
// 将SVzNL3DLaserLine转换为std::vector<SVzNL3DPosition>
|
|||
|
|
std::vector<SVzNL3DPosition> lineData(detectionDataCache[i].nPositionCnt);
|
|||
|
|
for (int j = 0; j < detectionDataCache[i].nPositionCnt; j++) {
|
|||
|
|
lineData[j] = detectionDataCache[i].p3DPosition[j];
|
|||
|
|
}
|
|||
|
|
sx_lineDataR(lineData, cameraCalibParam.planeCalib, cameraCalibParam.planeHeight);
|
|||
|
|
|
|||
|
|
// 将处理后的数据复制回原结构
|
|||
|
|
for (int j = 0; j < detectionDataCache[i].nPositionCnt; j++) {
|
|||
|
|
detectionDataCache[i].p3DPosition[j] = lineData[j];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 3. 调用搭接焊缝检测算法接口（使用当前相机的调平参数）
|
|||
|
|
std::vector<std::vector<SVzNL3DPoint>> objOps;
|
|||
|
|
std::vector<std::vector<SVzNL3DPosition>> scanLines(detectionDataCache.size());
|
|||
|
|
for (size_t i = 0; i < detectionDataCache.size(); i++) {
|
|||
|
|
scanLines[i].resize(detectionDataCache[i].nPositionCnt);
|
|||
|
|
for (int j = 0; j < detectionDataCache[i].nPositionCnt; j++) {
|
|||
|
|
scanLines[i][j].pt3D.x = detectionDataCache[i].p3DPosition[j].pt3D.x;
|
|||
|
|
scanLines[i][j].pt3D.y = detectionDataCache[i].p3DPosition[j].pt3D.y;
|
|||
|
|
scanLines[i][j].pt3D.z = detectionDataCache[i].p3DPosition[j].pt3D.z;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 定义搭接焊缝检测需要的参数
|
|||
|
|
SSG_cornerParam cornerParam;
|
|||
|
|
SSG_treeGrowParam growParam;
|
|||
|
|
int errCode = 0;
|
|||
|
|
sx_getLapWeldPostion(scanLines, cornerParam, growParam, algoParam, objOps, &errCode);
|
|||
|
|
|
|||
|
|
if (errCode != 0) {
|
|||
|
|
LOG_ERROR("sx_getLapWeldPostion failed, error code: %d\n", errCode);
|
|||
|
|
return errCode;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 从点云数据生成投影图像
|
|||
|
|
// 注意：由于数据结构变化，需要调整图像生成函数
|
|||
|
|
detectionResult.image = QImage(800, 600, QImage::Format_RGB32);
|
|||
|
|
detectionResult.image.fill(Qt::black);
|
|||
|
|
|
|||
|
|
// 转换检测结果为UI显示格式（使用机械臂坐标系数据）
|
|||
|
|
for (size_t i = 0; i < objOps.size(); i++) {
|
|||
|
|
if (!objOps[i].empty()) {
|
|||
|
|
const SVzNL3DPoint& obj = objOps[i][0]; // 使用第一个点作为参考
|
|||
|
|
|
|||
|
|
// 进行坐标转换：从算法坐标系转换到机械臂坐标系
|
|||
|
|
SVzNL3DPoint targetObj;
|
|||
|
|
targetObj.x = obj.x;
|
|||
|
|
targetObj.y = obj.y;
|
|||
|
|
targetObj.z = obj.z;
|
|||
|
|
|
|||
|
|
SVzNL3DPoint robotObj;
|
|||
|
|
CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix);
|
|||
|
|
|
|||
|
|
// 创建位置数据（使用转换后的机械臂坐标）
|
|||
|
|
LapWeldPosition pos;
|
|||
|
|
pos.x = robotObj.x; // 机械臂坐标X
|
|||
|
|
pos.y = robotObj.y; // 机械臂坐标Y
|
|||
|
|
pos.z = robotObj.z; // 机械臂坐标Z
|
|||
|
|
pos.roll = 0.0; // 搭接焊缝检测不提供姿态角
|
|||
|
|
pos.pitch = 0.0; // 搭接焊缝检测不提供姿态角
|
|||
|
|
pos.yaw = 0.0; // 搭接焊缝检测不提供姿态角
|
|||
|
|
|
|||
|
|
detectionResult.positions.push_back(pos);
|
|||
|
|
|
|||
|
|
if(debugParam.enableDebug && debugParam.printDetailLog){
|
|||
|
|
LOG_INFO("[Algo Thread] Object %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
|
|||
|
|
i, obj.x, obj.y, obj.z);
|
|||
|
|
LOG_INFO("[Algo Thread] Object %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
|
|||
|
|
i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 新增：处理统一数据格式和项目类型的DetectLapWeld函数
|
|||
|
|
int DetectPresenter::DetectLapWeld(
|
|||
|
|
int cameraIndex,
|
|||
|
|
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
|||
|
|
ProjectType projectType,
|
|||
|
|
const SSX_lapWeldParam& algoParam,
|
|||
|
|
const SSG_planeCalibPara& cameraCalibParam,
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 保存debug数据
|
|||
|
|
std::string timeStamp = CVrDateUtils::GetNowTime();
|
|||
|
|
if(debugParam.enableDebug && debugParam.savePointCloud){
|
|||
|
|
LOG_INFO("[Algo Thread] Debug mode is enabled, saving point cloud data\n");
|
|||
|
|
|
|||
|
|
// 获取当前时间戳，格式为YYYYMMDDHHMMSS
|
|||
|
|
std::string fileName = debugParam.debugOutputPath + "/Laserline_" + std::to_string(cameraIndex) + "_" + timeStamp + ".txt";
|
|||
|
|
|
|||
|
|
// 直接使用统一格式保存数据
|
|||
|
|
dataLoader.SaveLaserScanData(fileName, laserLines, laserLines.size(), 0.0, 0, 0);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(debugParam.enableDebug && debugParam.printDetailLog)
|
|||
|
|
{
|
|||
|
|
LOG_INFO("[Algo Thread] clibMatrix: \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f]\n",
|
|||
|
|
clibMatrix[0], clibMatrix[1], clibMatrix[2], clibMatrix[3],
|
|||
|
|
clibMatrix[4], clibMatrix[5], clibMatrix[6], clibMatrix[7],
|
|||
|
|
clibMatrix[8], clibMatrix[9], clibMatrix[10], clibMatrix[11],
|
|||
|
|
clibMatrix[12], clibMatrix[13], clibMatrix[14], clibMatrix[15]);
|
|||
|
|
|
|||
|
|
// 1. 使用成员变量算法参数（已在初始化时从XML读取）
|
|||
|
|
LOG_INFO("[Algo Thread] Using algorithm parameters from XML configuration\n");
|
|||
|
|
LOG_INFO(" Lap Weld: lapHeight=%.1f, weldMinLen=%.1f, weldRefPoints=%d\n",algoParam.lapHeight, algoParam.weldMinLen, algoParam.weldRefPoints);
|
|||
|
|
|
|||
|
|
LOG_INFO(" Plane height: %.3f\n", cameraCalibParam.planeHeight);
|
|||
|
|
LOG_INFO(" Plane calibration matrix: [%.3f, %.3f, %.3f; %.3f, %.3f, %.3f; %.3f, %.3f, %.3f]\n",
|
|||
|
|
cameraCalibParam.planeCalib[0], cameraCalibParam.planeCalib[1], cameraCalibParam.planeCalib[2],
|
|||
|
|
cameraCalibParam.planeCalib[3], cameraCalibParam.planeCalib[4], cameraCalibParam.planeCalib[5],
|
|||
|
|
cameraCalibParam.planeCalib[6], cameraCalibParam.planeCalib[7], cameraCalibParam.planeCalib[8]);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int nRet = SUCCESS;
|
|||
|
|
|
|||
|
|
// 普通项目：转换为XYZ格式进行算法调用
|
|||
|
|
std::vector<SVzNL3DLaserLine> xyzData;
|
|||
|
|
int convertResult = dataLoader.ConvertToSVzNL3DLaserLine(laserLines, xyzData);
|
|||
|
|
if (convertResult == SUCCESS && !xyzData.empty()) {
|
|||
|
|
nRet = DetectLapWeld(xyzData, algoParam, cameraCalibParam, debugParam, dataLoader, clibMatrix, detectionResult);
|
|||
|
|
// 清理XYZ内存
|
|||
|
|
dataLoader.FreeConvertedData(xyzData);
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
|
|||
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(debugParam.enableDebug && debugParam.saveDebugImage){
|
|||
|
|
// 获取当前时间戳，格式为YYYYMMDDHHMMSS
|
|||
|
|
std::string fileName = debugParam.debugOutputPath + "/Image_" + std::to_string(cameraIndex) + "_" + timeStamp + ".png";
|
|||
|
|
LOG_INFO("[Algo Thread] Debug image saved image : %s\n", fileName.c_str());
|
|||
|
|
|
|||
|
|
// 保存检测结果图片
|
|||
|
|
if (!detectionResult.image.isNull()) {
|
|||
|
|
QString qFileName = QString::fromStdString(fileName);
|
|||
|
|
detectionResult.image.save(qFileName);
|
|||
|
|
} else {
|
|||
|
|
LOG_WARNING("[Algo Thread] No valid image to save for debug\n");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return nRet;
|
|||
|
|
}
|