470 lines
24 KiB
C++
470 lines
24 KiB
C++
#include "DetectPresenter.h"
|
||
#include "AlgorithmParamConverter.h"
|
||
#include "CoordinateTransform.h"
|
||
#include "PoseAxesBuilder.h"
|
||
#include "ScrewPositionTCPProtocol.h"
|
||
#include "rodAndBarDetection_Export.h"
|
||
#include <QColor>
|
||
#include <array>
|
||
|
||
namespace {
|
||
|
||
QImage BuildScrewPointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
|
||
const std::vector<SSX_rodPoseInfo>& screwInfo,
|
||
const std::vector<std::pair<double, double>>* approachXY = nullptr)
|
||
{
|
||
PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
|
||
if (!canvas.isValid()) {
|
||
return QImage();
|
||
}
|
||
|
||
constexpr double kAxisLineLength = 60.0;
|
||
const QColor pointColor(0, 255, 0);
|
||
const QColor lineColor(255, 0, 0);
|
||
const QColor textColor(255, 255, 0);
|
||
const QColor approachColor(255, 128, 0); // 橙色：接近点
|
||
|
||
for (size_t i = 0; i < screwInfo.size(); ++i) {
|
||
const auto& screw = screwInfo[i];
|
||
canvas.drawPoint(screw.center.x, screw.center.y, pointColor, 8);
|
||
canvas.drawText(screw.center.x, screw.center.y, QString::number(i + 1), textColor, 16, 10, -10);
|
||
canvas.drawLine(screw.center.x - kAxisLineLength * screw.axialDir.x,
|
||
screw.center.y - kAxisLineLength * screw.axialDir.y,
|
||
screw.center.x + kAxisLineLength * screw.axialDir.x,
|
||
screw.center.y + kAxisLineLength * screw.axialDir.y,
|
||
lineColor, 2);
|
||
|
||
if (approachXY && i < approachXY->size()) {
|
||
const auto& ap = (*approachXY)[i];
|
||
canvas.drawPoint(ap.first, ap.second, approachColor, 6);
|
||
canvas.drawLine(screw.center.x, screw.center.y, ap.first, ap.second, approachColor, 1);
|
||
}
|
||
}
|
||
|
||
return canvas.image().copy();
|
||
}
|
||
|
||
QImage BuildToolDiskPointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
|
||
const SSX_platePoseInfo& poseInfo,
|
||
bool hasResult,
|
||
const std::pair<double, double>* approachXY = nullptr)
|
||
{
|
||
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); // 蓝色：法向 Z
|
||
const QColor xAxisColor(255, 0, 0); // 红色：X 轴
|
||
const QColor yAxisColor(0, 255, 0); // 绿色：Y 轴
|
||
const QColor approachColor(255, 128, 0); // 橙色：接近点
|
||
|
||
// 绘制定位盘中心点
|
||
canvas.drawPoint(poseInfo.center.x, poseInfo.center.y, centerColor, 10);
|
||
|
||
// 绘制法向量方向线
|
||
const double normalEndX = poseInfo.center.x + kNormalLineLength * poseInfo.normalDir.x;
|
||
const double normalEndY = poseInfo.center.y + kNormalLineLength * poseInfo.normalDir.y;
|
||
canvas.drawLine(poseInfo.center.x, poseInfo.center.y, normalEndX, normalEndY, normalColor, 2);
|
||
canvas.drawText(normalEndX, normalEndY, QStringLiteral("Z"), normalColor, 14, 6, -6);
|
||
|
||
const double xEndX = poseInfo.center.x + kNormalLineLength * poseInfo.xDir.x;
|
||
const double xEndY = poseInfo.center.y + kNormalLineLength * poseInfo.xDir.y;
|
||
canvas.drawLine(poseInfo.center.x, poseInfo.center.y, xEndX, xEndY, xAxisColor, 2);
|
||
canvas.drawText(xEndX, xEndY, QStringLiteral("X"), xAxisColor, 14, 6, -6);
|
||
|
||
const double yEndX = poseInfo.center.x + kNormalLineLength * poseInfo.yDir.x;
|
||
const double yEndY = poseInfo.center.y + kNormalLineLength * poseInfo.yDir.y;
|
||
canvas.drawLine(poseInfo.center.x, poseInfo.center.y, yEndX, yEndY, yAxisColor, 2);
|
||
canvas.drawText(yEndX, yEndY, QStringLiteral("Y"), yAxisColor, 14, 6, -6);
|
||
|
||
if (approachXY) {
|
||
canvas.drawPoint(approachXY->first, approachXY->second, approachColor, 8);
|
||
canvas.drawLine(poseInfo.center.x, poseInfo.center.y, approachXY->first, approachXY->second, approachColor, 1);
|
||
}
|
||
}
|
||
|
||
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));
|
||
}
|
||
|
||
CTVec3D ToCTVec3D(const SVzNL3DPoint& point)
|
||
{
|
||
return CTVec3D(point.x, point.y, point.z);
|
||
}
|
||
|
||
}
|
||
|
||
DetectPresenter::DetectPresenter(/* args */)
|
||
{
|
||
LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_rodAndBarDetectionVersion());
|
||
}
|
||
|
||
DetectPresenter::~DetectPresenter()
|
||
{
|
||
}
|
||
|
||
QString DetectPresenter::GetAlgoVersion()
|
||
{
|
||
return QString(wd_rodAndBarDetectionVersion());
|
||
}
|
||
|
||
int DetectPresenter::DetectScrew(
|
||
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\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 data to XYZ format or no XYZ data available\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
const ScrewDetectAlgorithmParams algoParams = AlgorithmParamConverter::ToScrewDetectAlgorithmParams(algorithmParams);
|
||
const double rodDiameter = algoParams.rodDiameter;
|
||
const bool isHorizonScan = algoParams.isHorizonScan;
|
||
const SSG_cornerParam& cornerParam = algoParams.cornerParam;
|
||
const SSG_treeGrowParam& growParam = algoParams.growParam;
|
||
const SSG_outlierFilterParam& filterParam = algoParams.filterParam;
|
||
|
||
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] Screw: rodDiameter=%.1f, isHorizonScan=%s\n", rodDiameter, isHorizonScan ? "true" : "false");
|
||
LOG_INFO("[Algo Thread] 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] 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);
|
||
LOG_INFO("[Algo Thread] 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_hexHeadScrewMeasure \n");
|
||
|
||
std::vector<SSX_rodPoseInfo> screwInfo;
|
||
sx_hexHeadScrewMeasure(
|
||
xyzData,
|
||
isHorizonScan,
|
||
cornerParam,
|
||
filterParam,
|
||
growParam,
|
||
rodDiameter,
|
||
screwInfo,
|
||
&errCode);
|
||
|
||
LOG_DEBUG("after sx_hexHeadScrewMeasure \n");
|
||
LOG_INFO("sx_hexHeadScrewMeasure: detected %zu screws, err=%d runtime=%.3fms\n",
|
||
screwInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
ERR_CODE_RETURN(errCode);
|
||
|
||
detectionResult.success = true;
|
||
detectionResult.errorCode = 0;
|
||
detectionResult.message = QStringLiteral("螺杆检测成功");
|
||
const CTHomogeneousMatrix handEyeMatrix = PoseAxesBuilder::BuildHandEyeMatrix(clibMatrix);
|
||
const CTEulerOrder order = static_cast<CTEulerOrder>(extrinsic.eulerOrder);
|
||
|
||
double rx, ry, rz;
|
||
PoseAxesBuilder::ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz);
|
||
const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, rx, ry, rz);
|
||
// flangePose.rx/ry/rz 已经是真实轴角，可直接交给 sixAxisEyeInHandBuildTransform 按 eulerOrder 组装。
|
||
const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
|
||
flangePose, order, handEyeMatrix);
|
||
|
||
// 锚点：法兰 Y 轴在 Eye 系下的方向。法兰姿态在一次检测内固定，所以 flangeYInEye 也固定，
|
||
// 用它作 BuildAnchoredFrame 的参考方向可以消除"螺杆轴向接近 Z_world 时副轴方向被放大抖动"。
|
||
const CTVec3D flangeYInEye = PoseAxesBuilder::FlangeAxisToEye(handEyeMatrix, CTVec3D(0.0, 1.0, 0.0));
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] Robot flange pose fields: X=%.3f, Y=%.3f, Z=%.3f, RX=%.3f, RY=%.3f, RZ=%.3f (eulerOrder=%d)\n",
|
||
robotPose.x, robotPose.y, robotPose.z, rx, ry, rz, static_cast<int>(order));
|
||
LOG_INFO("[Algo Thread] Flange Y axis in Eye frame (anchor): X=%.6f, Y=%.6f, Z=%.6f\n",
|
||
flangeYInEye.x, flangeYInEye.y, flangeYInEye.z);
|
||
}
|
||
|
||
std::vector<std::pair<double, double>> approachEyeXY;
|
||
approachEyeXY.reserve(screwInfo.size());
|
||
|
||
const bool needDebug = debugParam.enableDebug && debugParam.printDetailLog;
|
||
|
||
for (size_t i = 0; i < screwInfo.size(); ++i) {
|
||
const auto& screw = screwInfo[i];
|
||
|
||
const CTVec3D eyeCenter = ToCTVec3D(screw.center);
|
||
const CTVec3D eyeAxialDir = PoseAxesBuilder::NormalizeVector(ToCTVec3D(screw.axialDir));
|
||
const CTVec3D robotCenter = eyeInHandTransform.transformPoint(eyeCenter);
|
||
const CTVec3D robotAxialDir = PoseAxesBuilder::NormalizeVector(eyeInHandTransform.transformVector(eyeAxialDir));
|
||
|
||
// 接近点：Eye 系沿螺杆轴偏移 offset，再经 T 变换到 Robot 系；姿态与目标点共用。
|
||
const double offset = -extrinsic.approachOffset;
|
||
const CTVec3D eyeApproach(eyeCenter.x + eyeAxialDir.x * offset,
|
||
eyeCenter.y + eyeAxialDir.y * offset,
|
||
eyeCenter.z + eyeAxialDir.z * offset);
|
||
const CTVec3D robotApproach = eyeInHandTransform.transformPoint(eyeApproach);
|
||
approachEyeXY.emplace_back(eyeApproach.x, eyeApproach.y);
|
||
|
||
// 锚点式三元组：X = 螺杆轴向，Y = 法兰 Y 投影到 X 垂直面，Z = X × Y。
|
||
std::array<CTVec3D, 3> eyeAxes;
|
||
if (!PoseAxesBuilder::BuildAnchoredFrame(eyeAxialDir, flangeYInEye, eyeAxes)) {
|
||
LOG_WARNING("[Algo Thread] Screw %zu: 螺杆轴向与法兰 Y 锚点近共线，无法构造稳定副轴，已跳过\n", i);
|
||
continue;
|
||
}
|
||
|
||
PoseAxesBuilder::PoseAngles angles;
|
||
PoseAxesBuilder::PoseDebugInfo debugInfo;
|
||
const bool poseOk = PoseAxesBuilder::ComputeRobotPoseAngles(
|
||
eyeAxes, eyeInHandTransform, extrinsic, order, rx, rz,
|
||
angles, needDebug ? &debugInfo : nullptr);
|
||
if (!poseOk) {
|
||
LOG_WARNING("[Algo Thread] Screw %zu: 轴变换到机器人坐标系失败，已跳过\n", i);
|
||
continue;
|
||
}
|
||
|
||
ScrewPosition pos;
|
||
pos.x = robotCenter.x + extrinsic.offsetX;
|
||
pos.y = robotCenter.y + extrinsic.offsetY;
|
||
pos.z = robotCenter.z + extrinsic.offsetZ;
|
||
pos.approachX = robotApproach.x + extrinsic.offsetX;
|
||
pos.approachY = robotApproach.y + extrinsic.offsetY;
|
||
pos.approachZ = robotApproach.z + extrinsic.offsetZ;
|
||
pos.roll = angles.rollDeg;
|
||
pos.pitch = angles.pitchDeg;
|
||
pos.yaw = angles.yawDeg;
|
||
detectionResult.positions.push_back(pos);
|
||
|
||
ScrewInfo info;
|
||
info.centerX = robotCenter.x;
|
||
info.centerY = robotCenter.y;
|
||
info.centerZ = robotCenter.z;
|
||
info.axialDirX = robotAxialDir.x;
|
||
info.axialDirY = robotAxialDir.y;
|
||
info.axialDirZ = robotAxialDir.z;
|
||
info.rotateAngle = pos.roll;
|
||
detectionResult.screwInfoList.push_back(info);
|
||
|
||
if (needDebug) {
|
||
LOG_INFO("[Algo Thread] Screw %zu Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||
i, static_cast<int>(order),
|
||
debugInfo.eyeEulerBefore.rollDeg,
|
||
debugInfo.eyeEulerBefore.pitchDeg,
|
||
debugInfo.eyeEulerBefore.yawDeg);
|
||
LOG_INFO("[Algo Thread] Screw %zu Eye Euler after compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||
i, static_cast<int>(order),
|
||
debugInfo.eyeEulerAfter.rollDeg,
|
||
debugInfo.eyeEulerAfter.pitchDeg,
|
||
debugInfo.eyeEulerAfter.yawDeg);
|
||
// 给 outRot 标定用：这是「outRot 补偿前」的 Robot 系姿态。
|
||
// 把欧拉粘进 CloudView「工具→姿态补偿计算」的"当前"栏，期望填机械臂期望的姿态，
|
||
// 矩阵补偿 + XYZ 输出顺序，反解出来的就是 outRotX/Y/Z。
|
||
LOG_INFO("[Algo Thread] Screw %zu Robot RPY (before outRot, calibration ref): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||
i,
|
||
debugInfo.robotEulerBeforeOutRot.rollDeg,
|
||
debugInfo.robotEulerBeforeOutRot.pitchDeg,
|
||
debugInfo.robotEulerBeforeOutRot.yawDeg);
|
||
LOG_INFO("[Algo Thread] Screw %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", i, screw.center.x, screw.center.y, screw.center.z);
|
||
LOG_INFO("[Algo Thread] Screw %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f/%.2f/%.2f\n", i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
|
||
LOG_INFO("[Algo Thread] Screw %zu Axial Dir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", i, screw.axialDir.x, screw.axialDir.y, screw.axialDir.z);
|
||
LOG_INFO("[Algo Thread] Screw %zu Axial Dir Robot: X=%.3f, Y=%.3f, Z=%.3f\n", i, robotAxialDir.x, robotAxialDir.y, robotAxialDir.z);
|
||
LOG_INFO("[Algo Thread] Screw %zu Approach (offset=%.2f): X=%.2f, Y=%.2f, Z=%.2f\n", i, offset, pos.approachX, pos.approachY, pos.approachZ);
|
||
}
|
||
}
|
||
|
||
detectionResult.image = BuildScrewPointCloudImage(xyzData, screwInfo, &approachEyeXY);
|
||
|
||
SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("Image"));
|
||
return SUCCESS;
|
||
}
|
||
|
||
int DetectPresenter::DetectToolDisk(
|
||
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 tool disk 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 tool disk data to XYZ format or no XYZ data available\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
// 构造算法参数（与螺杆检测共享 cornerParam）
|
||
const ScrewDetectAlgorithmParams algoParams = AlgorithmParamConverter::ToScrewDetectAlgorithmParams(algorithmParams);
|
||
const SSG_cornerParam& cornerParam = algoParams.cornerParam;
|
||
|
||
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
||
LOG_INFO("[Algo Thread] ToolDisk 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] ToolDisk Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f\n",
|
||
cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap, cornerParam.minEndingGap_z);
|
||
LOG_INFO("[Algo Thread] ToolDisk 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_getLocationPlatePose_new\n");
|
||
|
||
SSX_platePoseInfo poseInfo = sx_getLocationPlatePose_new(xyzData, cornerParam, &errCode);
|
||
|
||
LOG_DEBUG("after sx_getLocationPlatePose_new\n");
|
||
LOG_INFO("sx_getLocationPlatePose_new: err=%d runtime=%.3fms\n", errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||
ERR_CODE_RETURN(errCode);
|
||
|
||
detectionResult.success = true;
|
||
detectionResult.errorCode = 0;
|
||
detectionResult.message = QStringLiteral("工具盘检测成功");
|
||
const CTHomogeneousMatrix handEyeMatrix = PoseAxesBuilder::BuildHandEyeMatrix(clibMatrix);
|
||
const CTEulerOrder order = static_cast<CTEulerOrder>(extrinsic.eulerOrder);
|
||
|
||
double rx, ry, rz;
|
||
PoseAxesBuilder::ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz);
|
||
const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, rx, ry, rz);
|
||
// flangePose.rx/ry/rz 已经是真实轴角，可直接交给 sixAxisEyeInHandBuildTransform 按 eulerOrder 组装。
|
||
const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
|
||
flangePose, order, handEyeMatrix);
|
||
|
||
// 将定位盘中心点通过手眼标定转换为机器人坐标
|
||
const CTVec3D eyeCenter = ToCTVec3D(poseInfo.center);
|
||
const CTVec3D robotCenter = eyeInHandTransform.transformPoint(eyeCenter);
|
||
const CTVec3D eyeXAxis = PoseAxesBuilder::NormalizeVector(ToCTVec3D(poseInfo.xDir));
|
||
const CTVec3D eyeYAxis = PoseAxesBuilder::NormalizeVector(ToCTVec3D(poseInfo.yDir));
|
||
|
||
ScrewPosition pos;
|
||
pos.x = robotCenter.x + extrinsic.offsetX;
|
||
pos.y = robotCenter.y + extrinsic.offsetY;
|
||
pos.z = robotCenter.z + extrinsic.offsetZ;
|
||
// 接近点：Eye 系沿工具盘 X 轴偏移 offset，再经 T 变换到 Robot 系；姿态与目标点共用。
|
||
const double offset = extrinsic.approachOffset;
|
||
const CTVec3D eyeApproach(eyeCenter.x + eyeXAxis.x * offset,
|
||
eyeCenter.y + eyeXAxis.y * offset,
|
||
eyeCenter.z + eyeXAxis.z * offset);
|
||
const CTVec3D robotApproach = eyeInHandTransform.transformPoint(eyeApproach);
|
||
pos.approachX = robotApproach.x + extrinsic.offsetX;
|
||
pos.approachY = robotApproach.y + extrinsic.offsetY;
|
||
pos.approachZ = robotApproach.z + extrinsic.offsetZ;
|
||
const std::pair<double, double> approachEyeXY(eyeApproach.x, eyeApproach.y);
|
||
|
||
// 锚点式三元组：算法已给出完整 X/Y/Z，但仍统一走 BuildAnchoredFrame —— 它对算法输出做
|
||
// Gram-Schmidt 正交化（算法本就正交时是恒等），保证后续欧拉提取数值稳定。
|
||
std::array<CTVec3D, 3> eyeAxes;
|
||
if (!PoseAxesBuilder::BuildAnchoredFrame(eyeXAxis, eyeYAxis, eyeAxes)) {
|
||
LOG_WARNING("[Algo Thread] ToolDisk: 算法 X/Y 轴近共线，无法构造稳定副轴\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
|
||
const bool needDebug = debugParam.enableDebug && debugParam.printDetailLog;
|
||
PoseAxesBuilder::PoseAngles angles;
|
||
PoseAxesBuilder::PoseDebugInfo debugInfo;
|
||
if (!PoseAxesBuilder::ComputeRobotPoseAngles(
|
||
eyeAxes, eyeInHandTransform, extrinsic, order, rx, rz,
|
||
angles, needDebug ? &debugInfo : nullptr)) {
|
||
LOG_WARNING("[Algo Thread] ToolDisk: 轴变换到机器人坐标系失败\n");
|
||
return ERR_CODE(DEV_DATA_INVALID);
|
||
}
|
||
pos.roll = angles.rollDeg;
|
||
pos.pitch = angles.pitchDeg;
|
||
pos.yaw = angles.yawDeg;
|
||
detectionResult.positions.push_back(pos);
|
||
|
||
if (needDebug) {
|
||
const CTRotationMatrix& eyeRotationAfter = debugInfo.eyeRotationAfter;
|
||
const CTRotationMatrix& robotRotation = debugInfo.robotRotation;
|
||
|
||
LOG_INFO("[Algo Thread] === Tool Disk Pose Debug ===\n");
|
||
LOG_INFO("[Algo Thread] ToolDisk Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||
static_cast<int>(order),
|
||
debugInfo.eyeEulerBefore.rollDeg,
|
||
debugInfo.eyeEulerBefore.pitchDeg,
|
||
debugInfo.eyeEulerBefore.yawDeg);
|
||
LOG_INFO("[Algo Thread] Eye Rotation Matrix (after compensation):\n");
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotationAfter.at(0, 0), eyeRotationAfter.at(0, 1), eyeRotationAfter.at(0, 2));
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotationAfter.at(1, 0), eyeRotationAfter.at(1, 1), eyeRotationAfter.at(1, 2));
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotationAfter.at(2, 0), eyeRotationAfter.at(2, 1), eyeRotationAfter.at(2, 2));
|
||
LOG_INFO("[Algo Thread] Eye Pos: (%.6f, %.6f, %.6f)\n", poseInfo.center.x, poseInfo.center.y, poseInfo.center.z);
|
||
LOG_INFO("[Algo Thread] Eye Euler after compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||
static_cast<int>(order),
|
||
debugInfo.eyeEulerAfter.rollDeg,
|
||
debugInfo.eyeEulerAfter.pitchDeg,
|
||
debugInfo.eyeEulerAfter.yawDeg);
|
||
|
||
LOG_INFO("[Algo Thread] Robot Rotation Matrix:\n");
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(0, 0), robotRotation.at(0, 1), robotRotation.at(0, 2));
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(1, 0), robotRotation.at(1, 1), robotRotation.at(1, 2));
|
||
LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(2, 0), robotRotation.at(2, 1), robotRotation.at(2, 2));
|
||
LOG_INFO("[Algo Thread] Robot Pos: (%.6f, %.6f, %.6f)\n", robotCenter.x, robotCenter.y, robotCenter.z);
|
||
// 给 outRot 标定用：outRot 补偿前的 Robot 系欧拉（粘进 CloudView「姿态补偿」"当前"栏）
|
||
LOG_INFO("[Algo Thread] ToolDisk Robot RPY (before outRot, calibration ref): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||
debugInfo.robotEulerBeforeOutRot.rollDeg,
|
||
debugInfo.robotEulerBeforeOutRot.pitchDeg,
|
||
debugInfo.robotEulerBeforeOutRot.yawDeg);
|
||
LOG_INFO("[Algo Thread] Robot Pos Euler (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||
static_cast<int>(order), pos.roll, pos.pitch, pos.yaw);
|
||
LOG_INFO("[Algo Thread] ToolDisk Approach (offset=%.2f): X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
offset, pos.approachX, pos.approachY, pos.approachZ);
|
||
}
|
||
|
||
detectionResult.image = BuildToolDiskPointCloudImage(xyzData, poseInfo, true, &approachEyeXY);
|
||
|
||
SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("ToolDisk_Image"));
|
||
return SUCCESS;
|
||
}
|