386 lines
16 KiB
C++
Raw Normal View History

#include "DetectPresenter.h"
#include "workpieceHolePositioning_Export.h"
#include <fstream>
#include <QPainter>
#include <QPen>
#include <QColor>
DetectPresenter::DetectPresenter(/* args */)
{
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_workpieceHolePositioningVersion());
}
DetectPresenter::~DetectPresenter()
{
}
int DetectPresenter::DetectWorkpieceHole(
int cameraIndex,
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
const VrAlgorithmParams& algorithmParams,
const VrDebugParam& debugParam,
LaserDataLoader& dataLoader,
const double clibMatrix[16],
WorkpieceHoleDetectionResult& 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");
// 获取当前时间戳格式为YYYYMMDDHHMMSS
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);
}
// 工件孔参数
WD_workpieceHoleParam workpiecePara;
workpiecePara.workpieceType = algorithmParams.workpieceHoleParam.workpieceType;
workpiecePara.holeDiameter = algorithmParams.workpieceHoleParam.holeDiameter;
workpiecePara.holeDist_L = algorithmParams.workpieceHoleParam.holeDist_L;
workpiecePara.holeDist_W = algorithmParams.workpieceHoleParam.holeDist_W;
// 线段分割参数
SSG_lineSegParam lineSegPara;
lineSegPara.distScale = algorithmParams.lineSegParam.distScale;
lineSegPara.segGapTh_y = algorithmParams.lineSegParam.segGapTh_y;
lineSegPara.segGapTh_z = algorithmParams.lineSegParam.segGapTh_z;
// 滤波参数
SSG_outlierFilterParam filterParam;
filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
filterParam.outlierTh = algorithmParams.filterParam.outlierTh;
// 树生长参数
SSG_treeGrowParam growParam;
growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;
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]);
// 打印工件孔参数
LOG_INFO("[Algo Thread] WorkpieceHole: type=%d, holeDiameter=%.1f, holeDist_L=%.1f, holeDist_W=%.1f\n",
workpiecePara.workpieceType, workpiecePara.holeDiameter, workpiecePara.holeDist_L, workpiecePara.holeDist_W);
// 打印线段分割参数
LOG_INFO("[Algo Thread] LineSeg: distScale=%.1f, segGapTh_y=%.1f, segGapTh_z=%.1f\n",
lineSegPara.distScale, lineSegPara.segGapTh_y, lineSegPara.segGapTh_z);
// 打印树生长参数
LOG_INFO("[Algo Thread] Tree Grow: yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum,
growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
// 打印滤波参数
LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", filterParam.continuityTh, 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("Plane height: %.3f\n", groundCalibPara.planeHeight);
LOG_INFO(" Plane calibration matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
groundCalibPara.planeCalib[0], groundCalibPara.planeCalib[1], groundCalibPara.planeCalib[2],
groundCalibPara.planeCalib[3], groundCalibPara.planeCalib[4], groundCalibPara.planeCalib[5],
groundCalibPara.planeCalib[6], groundCalibPara.planeCalib[7], groundCalibPara.planeCalib[8]);
LOG_INFO(" Plane invRMatrix matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
groundCalibPara.invRMatrix[0], groundCalibPara.invRMatrix[1], groundCalibPara.invRMatrix[2],
groundCalibPara.invRMatrix[3], groundCalibPara.invRMatrix[4], groundCalibPara.invRMatrix[5],
groundCalibPara.invRMatrix[6], groundCalibPara.invRMatrix[7], groundCalibPara.invRMatrix[8]);
}
// 数据预处理:调平和去除地面(使用当前相机的调平参数)
if(cameraCalibParam){
LOG_DEBUG("Processing data with plane calibration\n");
double groundH = -1;
for(size_t i = 0; i < xyzData.size(); i++){
wd_lineDataR(xyzData[i], cameraCalibParam->planeCalib, groundH);
}
}
int errCode = 0;
CVrTimeUtils oTimeUtils;
LOG_DEBUG("before wd_workpieceHolePositioning \n");
// 调用工件孔定位算法
std::vector<WD_workpieceInfo> workpiecePositioning;
wd_workpieceHolePositioning(
xyzData,
workpiecePara,
lineSegPara,
filterParam,
growParam,
groundCalibPara,
workpiecePositioning,
&errCode
);
LOG_DEBUG("after wd_workpieceHolePositioning \n");
LOG_INFO("wd_workpieceHolePositioning: found %zu workpieces, err=%d runtime=%.3fms\n",
workpiecePositioning.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
ERR_CODE_RETURN(errCode);
// 构建用于可视化的点数组
std::vector<std::vector<SVzNL3DPoint>> objOps;
// 处理每个工件的检测结果
for (size_t i = 0; i < workpiecePositioning.size(); i++) {
const WD_workpieceInfo& workpiece = workpiecePositioning[i];
// 保存工件类型(仅保存第一个工件的类型)
if (i == 0) {
detectionResult.workpieceType = workpiece.workpieceType;
}
// 保存中心点(转换到机械臂坐标系)
SVzNL3DPoint centerEye = workpiece.center;
SVzNL3DPoint centerRobot;
CVrConvert::EyeToRobot(centerEye, centerRobot, clibMatrix);
// 将每个工件的center添加到positions列表
HolePosition centerPos;
centerPos.x = centerRobot.x;
centerPos.y = centerRobot.y;
centerPos.z = centerRobot.z;
detectionResult.positions.push_back(centerPos);
// 保存最后一个工件的center到detectionResult.center用于兼容
if (i == workpiecePositioning.size() - 1) {
detectionResult.center = centerPos;
}
// 保存Y方向向量仅保存最后一个工件的
if (i == workpiecePositioning.size() - 1) {
SVzNL3DPoint yDirEye = workpiece.y_dir;
SVzNL3DPoint yDirRobot;
CVrConvert::EyeToRobot(yDirEye, yDirRobot, clibMatrix);
detectionResult.y_dir.x = yDirRobot.x;
detectionResult.y_dir.y = yDirRobot.y;
detectionResult.y_dir.z = yDirRobot.z;
}
// 保存Z方向向量仅保存最后一个工件的
if (i == workpiecePositioning.size() - 1) {
SVzNL3DPoint zDirEye = workpiece.z_dir;
SVzNL3DPoint zDirRobot;
CVrConvert::EyeToRobot(zDirEye, zDirRobot, clibMatrix);
detectionResult.z_dir.x = zDirRobot.x;
detectionResult.z_dir.y = zDirRobot.y;
detectionResult.z_dir.z = zDirRobot.z;
}
// 保存孔位置列表
std::vector<SVzNL3DPoint> holePoints;
for (size_t j = 0; j < workpiece.holes.size(); j++) {
SVzNL3DPoint holeEye = workpiece.holes[j];
SVzNL3DPoint holeRobot;
CVrConvert::EyeToRobot(holeEye, holeRobot, clibMatrix);
HolePosition holePos;
holePos.x = holeRobot.x;
holePos.y = holeRobot.y;
holePos.z = holeRobot.z;
detectionResult.holes.push_back(holePos);
// 添加到可视化点列表
holePoints.push_back(holeEye);
if(debugParam.enableDebug && debugParam.printDetailLog){
LOG_INFO("[Algo Thread] Hole %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", j, holeEye.x, holeEye.y, holeEye.z);
LOG_INFO("[Algo Thread] Hole %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f\n", j, holePos.x, holePos.y, holePos.z);
}
}
// 保存孔方向列表
for (size_t j = 0; j < workpiece.holesDir.size(); j++) {
SVzNL3DPoint holeDirEye = workpiece.holesDir[j];
SVzNL3DPoint holeDirRobot;
CVrConvert::EyeToRobot(holeDirEye, holeDirRobot, clibMatrix);
HolePosition holeDirPos;
holeDirPos.x = holeDirRobot.x;
holeDirPos.y = holeDirRobot.y;
holeDirPos.z = holeDirRobot.z;
detectionResult.holesDir.push_back(holeDirPos);
}
// 添加中心点到可视化列表
holePoints.push_back(centerEye);
objOps.push_back(holePoints);
if(debugParam.enableDebug && debugParam.printDetailLog){
LOG_INFO("[Algo Thread] Center Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", centerEye.x, centerEye.y, centerEye.z);
LOG_INFO("[Algo Thread] Center Robot Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
centerPos.x, centerPos.y, centerPos.z);
}
}
// 从点云数据生成投影图像(单色灰色点云 + 红色孔位标记)
{
// 固定图像尺寸
int imgRows = 992;
int imgCols = 1056;
int x_skip = 50;
int y_skip = 50;
// 计算点云范围
double xMin = 1e10, xMax = -1e10, yMin = 1e10, yMax = -1e10;
for (const auto& line : xyzData) {
for (const auto& pt : line) {
if (pt.pt3D.z < 1e-4) continue;
xMin = std::min(xMin, (double)pt.pt3D.x);
xMax = std::max(xMax, (double)pt.pt3D.x);
yMin = std::min(yMin, (double)pt.pt3D.y);
yMax = std::max(yMax, (double)pt.pt3D.y);
}
}
// 不扩展边界,直接使用点云范围
// 计算投影比例
double y_rows = (double)(imgRows - y_skip * 2);
double x_cols = (double)(imgCols - x_skip * 2);
double x_scale = (xMax - xMin) / x_cols;
double y_scale = (yMax - yMin) / y_rows;
if (x_scale < y_scale)
x_scale = y_scale;
else
y_scale = x_scale;
// 创建图像
QImage image(imgCols, imgRows, QImage::Format_RGB888);
image.fill(Qt::black);
QPainter painter(&image);
painter.setRenderHint(QPainter::Antialiasing);
// 绘制点云数据 - 使用单色灰色
QColor grayColor(150, 150, 150);
for (const auto& scanLine : xyzData) {
for (const auto& point : scanLine) {
if (point.pt3D.z < 1e-4) continue;
int px = (int)((point.pt3D.x - xMin) / x_scale + x_skip);
int py = (int)((point.pt3D.y - yMin) / y_scale + y_skip);
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
painter.setPen(QPen(grayColor, 1));
painter.drawPoint(px, py);
}
}
}
// 绘制孔位标记 - 使用红色
if (!objOps.empty()) {
QColor holeColor(255, 0, 0); // 红色
painter.setPen(QPen(holeColor, 1));
painter.setBrush(QBrush(holeColor));
for (size_t i = 0; i < objOps.size(); i++) {
const auto& holeGroup = objOps[i];
size_t groupSize = holeGroup.size();
for (size_t j = 0; j < groupSize; j++) {
const SVzNL3DPoint& hole = holeGroup[j];
// 跳过全0的点
if (fabs(hole.x) < 0.0001 && fabs(hole.y) < 0.0001 && fabs(hole.z) < 0.0001) {
continue;
}
int px = (int)((hole.x - xMin) / x_scale + x_skip);
int py = (int)((hole.y - yMin) / y_scale + y_skip);
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
// 绘制圆点标记
int circleSize = 2;
painter.drawEllipse(px - circleSize/2, py - circleSize/2, circleSize, circleSize);
// 只有中心点(最后一个点)才绘制编号
if (j == groupSize - 1) {
painter.setPen(QPen(Qt::white, 1));
QFont font("Arial", 14, QFont::Bold);
painter.setFont(font);
painter.drawText(px + 8, py + 6, QString("%1").arg(i + 1));
// 恢复画笔
painter.setPen(QPen(holeColor, 1));
painter.setBrush(QBrush(holeColor));
}
}
}
}
}
detectionResult.image = image;
}
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;
}