332 lines
14 KiB
C++
332 lines
14 KiB
C++
#include "DetectPresenter.h"
|
||
#include "AlgorithmParamConverter.h"
|
||
#include "PoseAxesBuilder.h"
|
||
#include "DiscHolePoseTCPProtocol.h"
|
||
#include "workpieceHolePositioning_Export.h"
|
||
#include <QColor>
|
||
|
||
namespace {
|
||
|
||
QImage BuildDiscHolePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
|
||
const WD_HolePositionInfo& holeInfo,
|
||
bool hasResult)
|
||
{
|
||
PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
|
||
if (!canvas.isValid()) {
|
||
return QImage();
|
||
}
|
||
|
||
if (hasResult) {
|
||
constexpr double kNormalLineLength = 60.0;
|
||
const QColor centerColor(0, 255, 0);
|
||
const QColor normalColor(0, 180, 255); // 蓝色：法向
|
||
|
||
// 绘制孔中心点
|
||
canvas.drawPoint(holeInfo.center.x, holeInfo.center.y, centerColor, 10);
|
||
|
||
// 绘制法向量方向线
|
||
const double normalEndX = holeInfo.center.x + kNormalLineLength * holeInfo.normDir.x;
|
||
const double normalEndY = holeInfo.center.y + kNormalLineLength * holeInfo.normDir.y;
|
||
canvas.drawLine(holeInfo.center.x, holeInfo.center.y, normalEndX, normalEndY, normalColor, 2);
|
||
canvas.drawText(normalEndX, normalEndY, QStringLiteral("Z"), normalColor, 14, 6, -6);
|
||
}
|
||
|
||
return canvas.image().copy();
|
||
}
|
||
|
||
QImage BuildDiscRackPointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
|
||
const SSG_pointPose& poseInfo,
|
||
bool hasResult)
|
||
{
|
||
PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
|
||
if (!canvas.isValid()) {
|
||
return QImage();
|
||
}
|
||
|
||
if (hasResult) {
|
||
constexpr double kAxisLineLength = 60.0;
|
||
const QColor centerColor(0, 255, 0);
|
||
const QColor xAxisColor(255, 0, 0); // 红色：X 轴
|
||
const QColor yAxisColor(0, 255, 0); // 绿色：Y 轴
|
||
const QColor zAxisColor(0, 180, 255); // 蓝色：Z 轴
|
||
|
||
// 绘制架子中心点
|
||
canvas.drawPoint(poseInfo.point.x, poseInfo.point.y, centerColor, 10);
|
||
|
||
// 绘制 X 轴方向线
|
||
const double xEndX = poseInfo.point.x + kAxisLineLength * poseInfo.pose_x.x;
|
||
const double xEndY = poseInfo.point.y + kAxisLineLength * poseInfo.pose_x.y;
|
||
canvas.drawLine(poseInfo.point.x, poseInfo.point.y, xEndX, xEndY, xAxisColor, 2);
|
||
canvas.drawText(xEndX, xEndY, QStringLiteral("X"), xAxisColor, 14, 6, -6);
|
||
|
||
// 绘制 Y 轴方向线
|
||
const double yEndX = poseInfo.point.x + kAxisLineLength * poseInfo.pose_y.x;
|
||
const double yEndY = poseInfo.point.y + kAxisLineLength * poseInfo.pose_y.y;
|
||
canvas.drawLine(poseInfo.point.x, poseInfo.point.y, yEndX, yEndY, yAxisColor, 2);
|
||
canvas.drawText(yEndX, yEndY, QStringLiteral("Y"), yAxisColor, 14, 6, -6);
|
||
|
||
// 绘制 Z 轴方向线
|
||
const double zEndX = poseInfo.point.x + kAxisLineLength * poseInfo.pose_z.x;
|
||
const double zEndY = poseInfo.point.y + kAxisLineLength * poseInfo.pose_z.y;
|
||
canvas.drawLine(poseInfo.point.x, poseInfo.point.y, zEndX, zEndY, zAxisColor, 2);
|
||
canvas.drawText(zEndX, zEndY, QStringLiteral("Z"), zAxisColor, 14, 6, -6);
|
||
}
|
||
|
||
return canvas.image().copy();
|
||
}
|
||
|
||
void SaveDebugImageIfNeeded(int cameraIndex,
|
||
const VrDebugParam& debugParam,
|
||
const QImage& image,
|
||
const QString& prefix)
|
||
{
|
||
if (!debugParam.enableDebug || !debugParam.saveDebugImage || image.isNull()) {
|
||
return;
|
||
}
|
||
|
||
const std::string timeStamp = CVrDateUtils::GetNowTime();
|
||
const std::string fileName = debugParam.debugOutputPath + "/" +
|
||
prefix.toStdString() + "_" + std::to_string(cameraIndex) + "_" + timeStamp + ".png";
|
||
LOG_INFO("[Algo Thread] Debug image saved image : %s\n", fileName.c_str());
|
||
image.save(QString::fromStdString(fileName));
|
||
}
|
||
|
||
}
|
||
|
||
DetectPresenter::DetectPresenter(/* args */)
|
||
{
|
||
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_workpieceHolePositioningVersion());
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter()
|
||
{
|
||
}
|
||
|
||
QString DetectPresenter::GetAlgoVersion()
|
||
{
|
||
return QString(wd_workpieceHolePositioningVersion());
|
||
}
|
||
|
||
int DetectPresenter::DetectDiscHole(
|
||
int cameraIndex,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
const VrAlgorithmParams& algorithmParams,
|
||
const VrDebugParam& debugParam,
|
||
LaserDataLoader& dataLoader,
|
||
const double clibMatrix[16],
|
||
const RobotPose6D& robotPose,
|
||
const HandEyeExtrinsic& extrinsic,
|
||
int poseOutputOrder,
|
||
DetectionResult& detectionResult)
|
||
{
|
||
if (laserLines.empty()) {
|
||
LOG_WARNING("No laser lines data available for disc hole detection\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
||
int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
|
||
if (convertResult != SUCCESS || xyzData.empty()) {
|
||
LOG_WARNING("Failed to convert disc hole data to XYZ format or no XYZ data available\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
// 砂轮盘孔定位算法参数（仅使用 cornerParam）
|
||
const DiscHoleAlgorithmParams algoParams = AlgorithmParamConverter::ToDiscHoleAlgorithmParams(algorithmParams);
|
||
const SSG_cornerParam& cornerParam = algoParams.cornerParam;
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] DiscHole 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] DiscHole Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
|
||
cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap,
|
||
cornerParam.minEndingGap_z, cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);
|
||
LOG_INFO("[Algo Thread] DiscHole Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f, outRotX=%.3f, outRotY=%.3f, outRotZ=%.3f\n",
|
||
extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ,
|
||
extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ);
|
||
}
|
||
|
||
int errCode = 0;
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
LOG_DEBUG("before sx_getDiscHolePose\n");
|
||
|
||
WD_HolePositionInfo holeInfo = sx_getDiscHolePose(xyzData, cornerParam, &errCode);
|
||
|
||
LOG_DEBUG("after sx_getDiscHolePose\n");
|
||
LOG_INFO("sx_getDiscHolePose: err=%d runtime=%.3fms\n", errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
ERR_CODE_RETURN(errCode);
|
||
|
||
detectionResult.success = true;
|
||
detectionResult.errorCode = 0;
|
||
detectionResult.message = QStringLiteral("砂轮盘孔检测成功");
|
||
|
||
// 使用 NormalDirToPoseAngles：从 WD_HolePositionInfo 的 normDir+center 计算完整姿态
|
||
// （内部封装了手眼变换流水线：法兰姿态构造 -> eye-in-hand 变换 -> rot/outRot 补偿 -> 欧拉提取）
|
||
double outX, outY, outZ, outRoll, outPitch, outYaw;
|
||
if (!PoseAxesBuilder::NormalDirToPoseAngles(
|
||
holeInfo.center,
|
||
holeInfo.normDir,
|
||
clibMatrix,
|
||
extrinsic,
|
||
robotPose,
|
||
outX, outY, outZ,
|
||
outRoll, outPitch, outYaw,
|
||
poseOutputOrder)) {
|
||
LOG_WARNING("[Algo Thread] DiscHole pose transform failed: invalid normal or transform axes\n");
|
||
detectionResult.success = false;
|
||
detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
|
||
detectionResult.message = QStringLiteral("砂轮盘孔姿态计算失败");
|
||
return detectionResult.errorCode;
|
||
}
|
||
|
||
DiscHolePosition pos;
|
||
pos.x = outX;
|
||
pos.y = outY;
|
||
pos.z = outZ;
|
||
pos.roll = outRoll;
|
||
pos.pitch = outPitch;
|
||
pos.yaw = outYaw;
|
||
detectionResult.positions.push_back(pos);
|
||
|
||
DiscHoleInfo info;
|
||
info.centerX = holeInfo.center.x;
|
||
info.centerY = holeInfo.center.y;
|
||
info.centerZ = holeInfo.center.z;
|
||
info.normDirX = holeInfo.normDir.x;
|
||
info.normDirY = holeInfo.normDir.y;
|
||
info.normDirZ = holeInfo.normDir.z;
|
||
detectionResult.discHoleInfoList.push_back(info);
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] DiscHole Eye Center: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
holeInfo.center.x, holeInfo.center.y, holeInfo.center.z);
|
||
LOG_INFO("[Algo Thread] DiscHole Eye NormDir: X=%.3f, Y=%.3f, Z=%.3f\n",
|
||
holeInfo.normDir.x, holeInfo.normDir.y, holeInfo.normDir.z);
|
||
LOG_INFO("[Algo Thread] DiscHole Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f/%.2f/%.2f\n",
|
||
pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
|
||
}
|
||
|
||
detectionResult.image = BuildDiscHolePointCloudImage(xyzData, holeInfo, true);
|
||
|
||
SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("DiscHole_Image"));
|
||
return SUCCESS;
|
||
}
|
||
|
||
int DetectPresenter::DetectDiscRack(
|
||
int cameraIndex,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
const VrAlgorithmParams& algorithmParams,
|
||
const VrDebugParam& debugParam,
|
||
LaserDataLoader& dataLoader,
|
||
const double clibMatrix[16],
|
||
const RobotPose6D& robotPose,
|
||
const HandEyeExtrinsic& extrinsic,
|
||
int poseOutputOrder,
|
||
DetectionResult& detectionResult)
|
||
{
|
||
if (laserLines.empty()) {
|
||
LOG_WARNING("No laser lines data available for disc rack detection\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
||
int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
|
||
if (convertResult != SUCCESS || xyzData.empty()) {
|
||
LOG_WARNING("Failed to convert disc rack data to XYZ format or no XYZ data available\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
// 砂轮盘架子定位算法参数（仅使用 cornerParam）
|
||
const DiscHoleAlgorithmParams algoParams = AlgorithmParamConverter::ToDiscHoleAlgorithmParams(algorithmParams);
|
||
const SSG_cornerParam& cornerParam = algoParams.cornerParam;
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] DiscRack 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] DiscRack Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
|
||
cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap,
|
||
cornerParam.minEndingGap_z, cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);
|
||
LOG_INFO("[Algo Thread] DiscRack Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f, outRotX=%.3f, outRotY=%.3f, outRotZ=%.3f\n",
|
||
extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ,
|
||
extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ);
|
||
}
|
||
|
||
int errCode = 0;
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
LOG_DEBUG("before sx_getDiscRackCenterPosition\n");
|
||
|
||
SSG_pointPose pointPose = sx_getDiscRackCenterPosition(xyzData, cornerParam, &errCode);
|
||
|
||
LOG_DEBUG("after sx_getDiscRackCenterPosition\n");
|
||
LOG_INFO("sx_getDiscRackCenterPosition: err=%d runtime=%.3fms\n", errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
ERR_CODE_RETURN(errCode);
|
||
|
||
detectionResult.success = true;
|
||
detectionResult.errorCode = 0;
|
||
detectionResult.message = QStringLiteral("砂轮盘架子检测成功");
|
||
|
||
// 使用 PointPoseToEuler：从 SSG_pointPose 的三轴向量 (pose_x/y/z) 构造旋转矩阵并提取欧拉
|
||
// （内部封装了手眼变换流水线：法兰姿态构造 -> eye-in-hand 变换 -> rot/outRot 补偿 -> 欧拉提取）
|
||
double outX, outY, outZ, outRoll, outPitch, outYaw;
|
||
if (!PoseAxesBuilder::PointPoseToEuler(
|
||
pointPose,
|
||
clibMatrix,
|
||
extrinsic,
|
||
robotPose,
|
||
outX, outY, outZ,
|
||
outRoll, outPitch, outYaw,
|
||
poseOutputOrder)) {
|
||
LOG_WARNING("[Algo Thread] DiscRack pose transform failed: invalid pose axes or transform axes\n");
|
||
detectionResult.success = false;
|
||
detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
|
||
detectionResult.message = QStringLiteral("砂轮盘架子姿态计算失败");
|
||
return detectionResult.errorCode;
|
||
}
|
||
|
||
DiscHolePosition pos;
|
||
pos.x = outX;
|
||
pos.y = outY;
|
||
pos.z = outZ;
|
||
pos.roll = outRoll;
|
||
pos.pitch = outPitch;
|
||
pos.yaw = outYaw;
|
||
detectionResult.positions.push_back(pos);
|
||
|
||
DiscRackInfo info;
|
||
info.centerX = pointPose.point.x;
|
||
info.centerY = pointPose.point.y;
|
||
info.centerZ = pointPose.point.z;
|
||
info.roll = outRoll;
|
||
info.pitch = outPitch;
|
||
info.yaw = outYaw;
|
||
detectionResult.discRackInfoList.push_back(info);
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] DiscRack Eye Point: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
pointPose.point.x, pointPose.point.y, pointPose.point.z);
|
||
LOG_INFO("[Algo Thread] DiscRack Eye PoseX: X=%.3f, Y=%.3f, Z=%.3f\n",
|
||
pointPose.pose_x.x, pointPose.pose_x.y, pointPose.pose_x.z);
|
||
LOG_INFO("[Algo Thread] DiscRack Eye PoseY: X=%.3f, Y=%.3f, Z=%.3f\n",
|
||
pointPose.pose_y.x, pointPose.pose_y.y, pointPose.pose_y.z);
|
||
LOG_INFO("[Algo Thread] DiscRack Eye PoseZ: X=%.3f, Y=%.3f, Z=%.3f\n",
|
||
pointPose.pose_z.x, pointPose.pose_z.y, pointPose.pose_z.z);
|
||
LOG_INFO("[Algo Thread] DiscRack Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f/%.2f/%.2f\n",
|
||
pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
|
||
}
|
||
|
||
detectionResult.image = BuildDiscRackPointCloudImage(xyzData, pointPose, true);
|
||
|
||
SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("DiscRack_Image"));
|
||
return SUCCESS;
|
||
}
|