From 0331f3f3195af072245016eebe8f367c01b13af2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=B0=E4=BB=94?= Date: Thu, 23 Apr 2026 23:40:50 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B7=A5=E4=BB=B6=E5=AD=94=E5=AE=9A=E4=BD=8D?= =?UTF-8?q?=E5=8D=8F=E8=AE=AE=E8=BE=93=E5=87=BA=E9=A1=BA=E5=BA=8F=E5=90=8C?= =?UTF-8?q?=E6=AD=A5=E6=9B=B4=E6=96=B0=E4=BA=86=E7=95=8C=E9=9D=A2=E7=9A=84?= =?UTF-8?q?=E9=A1=BA=E5=BA=8F&=E6=9A=82=E5=AD=98=E8=9E=BA=E6=9D=86?= =?UTF-8?q?=E5=AE=9A=E4=BD=8D=E7=9A=84=E5=B7=A5=E5=85=B7=E7=9B=98=E7=9A=84?= =?UTF-8?q?=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Presenter/Inc/DetectPresenter.h | 5 +- .../Presenter/Src/DetectPresenter.cpp | 135 ++++++++++++++++-- .../Presenter/Src/ScrewPositionPresenter.cpp | 8 +- .../ScrewPositionApp/dialogalgoarg.cpp | 14 +- .../ScrewPositionApp/dialogalgoarg.ui | 12 +- .../Presenter/Inc/DetectPresenter.h | 1 + .../Presenter/Src/DetectPresenter.cpp | 42 +++++- .../Presenter/Src/WorkpieceHolePresenter.cpp | 52 ++----- App/WorkpieceHole/WorkpieceHoleApp/Version.h | 2 +- App/WorkpieceHole/WorkpieceHoleApp/Version.md | 3 + AppUtils/AppConfig/Inc/VrHandEyeCalibConfig.h | 9 +- AppUtils/AppConfig/Src/ConfigXmlUtils.cpp | 9 ++ AppUtils/UICommon/Inc/HandEyeCalibWidget.h | 26 +++- AppUtils/UICommon/Src/HandEyeCalibWidget.cpp | 101 +++++++++++++ 14 files changed, 347 insertions(+), 72 deletions(-) diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h index 0936d2f..1986c66 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h @@ -35,9 +35,12 @@ struct HandEyeExtrinsic double rotY = 0.0; double rotZ = 0.0; double approachOffset = 0.0; + double offsetX = 0.0; + double offsetY = 0.0; + double offsetZ = 0.0; double outRotX = 0.0; double outRotY = 0.0; - double outRotZ = 15.0; + double outRotZ = 14.56; }; class DetectPresenter diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp index f63c6db..82dc809 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp @@ -396,6 +396,107 @@ CTRotationMatrix BuildRotationMatrix(const std::array& axes) return rotation; } +// ============ ZXY 欧拉角分解/重建（绕开 ZYX 奇异区） ============ + +// ZXY 分解：R = Rz(α)·Rx(β)·Ry(γ) +// ZXY 奇异点在 β=±90°（R(2,1)=±1），与 ZYX 奇异点 pitch≈±90° 不重叠。 +static void DecomposeRotationZXY(const CTRotationMatrix& R, + double& alpha, double& beta, double& gamma) +{ + const double sb = std::max(-1.0, std::min(1.0, R.at(2, 1))); + const double betaRad = std::asin(sb); + const double cb = std::cos(betaRad); + + if (std::fabs(cb) > 1e-7) { + alpha = std::atan2(-R.at(0, 1), R.at(1, 1)) * 180.0 / M_PI; + gamma = std::atan2(-R.at(2, 0), R.at(2, 2)) * 180.0 / M_PI; + } else { + alpha = std::atan2(R.at(1, 0), R.at(0, 0)) * 180.0 / M_PI; + gamma = 0.0; + } + beta = betaRad * 180.0 / M_PI; +} + +static CTRotationMatrix RebuildRotationZXY(double alphaDeg, double betaDeg, double gammaDeg) +{ + const double a = alphaDeg * M_PI / 180.0; + const double b = betaDeg * M_PI / 180.0; + const double g = gammaDeg * M_PI / 180.0; + const double ca = std::cos(a), sa = std::sin(a); + const double cb = std::cos(b), sb = std::sin(b); + const double cg = std::cos(g), sg = std::sin(g); + + CTRotationMatrix R; + R.at(0,0) = ca*cg + sa*sb*sg; R.at(0,1) = -sa*cb; R.at(0,2) = ca*sg + sa*sb*cg; + R.at(1,0) = sa*cg - ca*sb*sg; R.at(1,1) = ca*cb; R.at(1,2) = sa*sg - ca*sb*cg; + R.at(2,0) = -cb*sg; R.at(2,1) = sb; R.at(2,2) = cb*cg; + return R; +} + +// 直接 ZYX 分解：R = Rz(yaw)·Ry(pitch)·Rx(roll) +static void DecomposeRotationZYX(const CTRotationMatrix& R, + double& roll, double& pitch, double& yaw) +{ + const double sp = std::max(-1.0, std::min(1.0, -R.at(2, 0))); + const double pitchRad = std::asin(sp); + const double cp = std::cos(pitchRad); + + if (std::fabs(cp) > 1e-7) { + roll = std::atan2(R.at(2, 1), R.at(2, 2)) * 180.0 / M_PI; + yaw = std::atan2(R.at(1, 0), R.at(0, 0)) * 180.0 / M_PI; + } else { + yaw = std::atan2(-R.at(0, 1), R.at(1, 1)) * 180.0 / M_PI; + roll = 0.0; + } + pitch = pitchRad * 180.0 / M_PI; +} + +// 稳定 ZYX 欧拉角提取： +// 1) |R(2,0)| ≤ threshold → 直接 ZYX 分解 +// 2) |R(2,0)| > threshold → ZXY 分解（奇异点不重叠）→ 重建 R' → ZYX 分解 +// 3) 用拍照法兰姿态做连续性修正（±360° 拉回参考附近） +static void ExtractStableZYXEulerDegrees(const CTRotationMatrix& R, + double& roll, double& pitch, double& yaw, + double refRoll, double refYaw) +{ + constexpr double kSingularityThreshold = 0.9998; + + const double absSinPitch = std::fabs(R.at(2, 0)); + if (absSinPitch > kSingularityThreshold) { + LOG_INFO("[Algo Thread] ZYX singularity: |R(2,0)|=%.6f > %.4f\n", + absSinPitch, kSingularityThreshold); + + // ZXY 分解 + double alpha, beta, gamma; + DecomposeRotationZXY(R, alpha, beta, gamma); + LOG_INFO("[Algo Thread] ZXY: Z=%.3f, X=%.3f, Y=%.3f\n", alpha, beta, gamma); + + // 重建 R' 并验证 + CTRotationMatrix Rp = RebuildRotationZXY(alpha, beta, gamma); + double err = 0; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + err += (Rp.at(i, j) - R.at(i, j)) * (Rp.at(i, j) - R.at(i, j)); + err = std::sqrt(err); + LOG_INFO("[Algo Thread] R' rebuild error: %.10f\n", err); + + // 对 R' 做 ZYX 分解 + DecomposeRotationZYX(Rp, roll, pitch, yaw); + LOG_INFO("[Algo Thread] ZYX from R': Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", roll, pitch, yaw); + } else { + DecomposeRotationZYX(R, roll, pitch, yaw); + } + + // 连续性修正：拉到参考姿态附近 + double yawDiff = yaw - refYaw; + while (yawDiff > 180.0) { yaw -= 360.0; yawDiff -= 360.0; } + while (yawDiff < -180.0) { yaw += 360.0; yawDiff += 360.0; } + + double rollDiff = roll - refRoll; + while (rollDiff > 180.0) { roll -= 360.0; rollDiff -= 360.0; } + while (rollDiff < -180.0) { roll += 360.0; rollDiff += 360.0; } +} + } DetectPresenter::DetectPresenter(/* args */) @@ -557,15 +658,18 @@ int DetectPresenter::DetectScrew( const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes); ScrewPosition pos; - pos.x = robotCenter.x; - pos.y = robotCenter.y; - pos.z = robotCenter.z; - pos.approachX = robotApproach.x; - pos.approachY = robotApproach.y; - pos.approachZ = robotApproach.z; + 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; +#if 0 + ExtractStableZYXEulerDegrees(rotation, pos.roll, pos.pitch, pos.yaw, rx, rz); +#else RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw); - // 拍照姿态作参考，把 gimbal lock / 多解区的解拉回拍照姿态附近，减少机械臂旋转量。 ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz); +#endif detectionResult.positions.push_back(pos); ScrewInfo info; @@ -666,17 +770,17 @@ int DetectPresenter::DetectToolDisk( const CTVec3D eyeNormalDir = NormalizeVector(ToCTVec3D(poseInfo.normalDir)); ScrewPosition pos; - pos.x = robotCenter.x; - pos.y = robotCenter.y; - pos.z = robotCenter.z; + 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 eyeCenter = ToCTVec3D(poseInfo.center); 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; - pos.approachY = robotApproach.y; - pos.approachZ = robotApproach.z; + pos.approachX = robotApproach.x + extrinsic.offsetX; + pos.approachY = robotApproach.y + extrinsic.offsetY; + pos.approachZ = robotApproach.z + extrinsic.offsetZ; const std::pair approachEyeXY(eyeApproach.x, eyeApproach.y); // 姿态：三轴直接取算法输出 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。 @@ -700,9 +804,12 @@ int DetectPresenter::DetectToolDisk( // 同时让最终输出欧拉角远离万向节锁；默认 0 不启用。 ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes); +#if 0 + ExtractStableZYXEulerDegrees(robotRotation, pos.roll, pos.pitch, pos.yaw, rx, rz); +#else RotationMatrixToConfiguredEulerDegrees(robotRotation, order, pos.roll, pos.pitch, pos.yaw); - // 拍照姿态作参考，把 gimbal lock / 多解区的解拉回拍照姿态附近，减少机械臂旋转量。 ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz); +#endif detectionResult.positions.push_back(pos); if (debugParam.enableDebug && debugParam.printDetailLog) { diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp index f4f3e5b..452e524 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp @@ -245,6 +245,9 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vectorrotX, savedMatrix->rotY, savedMatrix->rotZ, - savedMatrix->approachOffset); + savedMatrix->approachOffset, + savedMatrix->offsetX, + savedMatrix->offsetY, + savedMatrix->offsetZ); } else { const CalibMatrix calibMatrix = m_presenter->GetClibMatrix(info.cameraIndex - 1); m_handEyeCalibWidget->setCalibData(info.cameraIndex, @@ -314,12 +317,19 @@ bool DialogAlgoArg::saveParams() double rotY = calibMatrix.rotY; double rotZ = calibMatrix.rotZ; double approachOffset = calibMatrix.approachOffset; - if (m_handEyeCalibWidget->getExtrinsicData(camIdx, eulerOrder, rotX, rotY, rotZ, approachOffset)) { + double offsetX = calibMatrix.offsetX; + double offsetY = calibMatrix.offsetY; + double offsetZ = calibMatrix.offsetZ; + if (m_handEyeCalibWidget->getExtrinsicData(camIdx, eulerOrder, rotX, rotY, rotZ, approachOffset, + offsetX, offsetY, offsetZ)) { calibMatrix.eulerOrder = eulerOrder; calibMatrix.rotX = rotX; calibMatrix.rotY = rotY; calibMatrix.rotZ = rotZ; calibMatrix.approachOffset = approachOffset; + calibMatrix.offsetX = offsetX; + calibMatrix.offsetY = offsetY; + calibMatrix.offsetZ = offsetZ; } newMatrixList.push_back(calibMatrix); diff --git a/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.ui b/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.ui index e221678..62719e1 100644 --- a/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.ui +++ b/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.ui @@ -7,7 +7,7 @@ 0 0 776 - 655 + 733 @@ -44,7 +44,7 @@ 40 80 701 - 501 + 571 @@ -779,7 +779,7 @@ QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; 40 - 590 + 670 120 45 @@ -813,7 +813,7 @@ QPushButton:pressed { 400 - 590 + 670 100 45 @@ -847,7 +847,7 @@ QPushButton:pressed { 520 - 590 + 670 100 45 @@ -881,7 +881,7 @@ QPushButton:pressed { 640 - 590 + 670 100 45 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h index 7b3767f..421a913 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h @@ -34,6 +34,7 @@ public: const double clibMatrix[16], int eulerOrder, int dirVectorInvert, + int poseOutputOrder, WorkpieceHoleDetectionResult& detectionResult); }; diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp index 1255bdf..2bb2fd1 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp @@ -31,6 +31,7 @@ int DetectPresenter::DetectWorkpieceHole( const double clibMatrix[16], int eulerOrder, int dirVectorInvert, + int poseOutputOrder, WorkpieceHoleDetectionResult& detectionResult) { if (laserLines.empty()) { @@ -185,13 +186,48 @@ int DetectPresenter::DetectWorkpieceHole( SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(R_pose); // 将机器人坐标系下的位姿添加到positions列表 + // 根据 poseOutputOrder 重映射欧拉角输出顺序 HolePosition centerRobotPos; centerRobotPos.x = ptRobot.x; centerRobotPos.y = ptRobot.y; centerRobotPos.z = ptRobot.z; - centerRobotPos.roll = robotRpy.roll; - centerRobotPos.pitch = robotRpy.pitch; - centerRobotPos.yaw = robotRpy.yaw; + switch (poseOutputOrder) { + case POSE_ORDER_RPY: + centerRobotPos.roll = robotRpy.roll; + centerRobotPos.pitch = robotRpy.pitch; + centerRobotPos.yaw = robotRpy.yaw; + break; + case POSE_ORDER_RYP: + centerRobotPos.roll = robotRpy.roll; + centerRobotPos.pitch = robotRpy.yaw; + centerRobotPos.yaw = robotRpy.pitch; + break; + case POSE_ORDER_PRY: + centerRobotPos.roll = robotRpy.pitch; + centerRobotPos.pitch = robotRpy.roll; + centerRobotPos.yaw = robotRpy.yaw; + break; + case POSE_ORDER_PYR: + centerRobotPos.roll = robotRpy.pitch; + centerRobotPos.pitch = robotRpy.yaw; + centerRobotPos.yaw = robotRpy.roll; + break; + case POSE_ORDER_YRP: + centerRobotPos.roll = robotRpy.yaw; + centerRobotPos.pitch = robotRpy.roll; + centerRobotPos.yaw = robotRpy.pitch; + break; + case POSE_ORDER_YPR: + centerRobotPos.roll = robotRpy.yaw; + centerRobotPos.pitch = robotRpy.pitch; + centerRobotPos.yaw = robotRpy.roll; + break; + default: + centerRobotPos.roll = robotRpy.roll; + centerRobotPos.pitch = robotRpy.pitch; + centerRobotPos.yaw = robotRpy.yaw; + break; + } detectionResult.positions.push_back(centerRobotPos); diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp index b29bd18..5e03bbd 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp @@ -351,11 +351,16 @@ int WorkpieceHolePresenter::ProcessAlgoDetection(std::vectorDetectWorkpieceHole(m_currentCameraIndex, detectionDataCache, algorithmParams, debugParam, m_dataLoader, - currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, detectionResult); + currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, + poseOutputOrder, detectionResult); // 根据项目类型选择处理方式 if (GetStatusCallback()) { QString err = QString("错误:%1").arg(nRet); @@ -836,7 +841,7 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio return; } - // 获取姿态输出顺序配置 + // 获取姿态输出顺序配置（用于日志） ConfigResult configResult = m_pConfigManager->GetConfigResult(); int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder; LOG_INFO("Using pose output order: %d\n", poseOutputOrder); @@ -846,48 +851,13 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio PLCModbusClient::CoordinateData coord; // 使用 float 类型存储坐标数据 + // detectionResult 中的欧拉角已在 DetectWorkpieceHole 中根据 poseOutputOrder 重映射 coord.x = static_cast(pos.x); coord.y = static_cast(pos.y); coord.z = static_cast(pos.z); - - // 根据姿态输出顺序配置调整输出 - switch (poseOutputOrder) { - case POSE_ORDER_RPY: // Roll, Pitch, Yaw（默认） - coord.roll = static_cast(pos.roll); - coord.pitch = static_cast(pos.pitch); - coord.yaw = static_cast(pos.yaw); - break; - case POSE_ORDER_RYP: // Roll, Yaw, Pitch - coord.roll = static_cast(pos.roll); - coord.pitch = static_cast(pos.yaw); - coord.yaw = static_cast(pos.pitch); - break; - case POSE_ORDER_PRY: // Pitch, Roll, Yaw - coord.roll = static_cast(pos.pitch); - coord.pitch = static_cast(pos.roll); - coord.yaw = static_cast(pos.yaw); - break; - case POSE_ORDER_PYR: // Pitch, Yaw, Roll - coord.roll = static_cast(pos.pitch); - coord.pitch = static_cast(pos.yaw); - coord.yaw = static_cast(pos.roll); - break; - case POSE_ORDER_YRP: // Yaw, Roll, Pitch - coord.roll = static_cast(pos.yaw); - coord.pitch = static_cast(pos.roll); - coord.yaw = static_cast(pos.pitch); - break; - case POSE_ORDER_YPR: // Yaw, Pitch, Roll - coord.roll = static_cast(pos.yaw); - coord.pitch = static_cast(pos.pitch); - coord.yaw = static_cast(pos.roll); - break; - default: // 默认 RPY - coord.roll = static_cast(pos.roll); - coord.pitch = static_cast(pos.pitch); - coord.yaw = static_cast(pos.yaw); - break; - } + coord.roll = static_cast(pos.roll); + coord.pitch = static_cast(pos.pitch); + coord.yaw = static_cast(pos.yaw); LOG_INFO("Workpiece: X=%.5f, Y=%.5f, Z=%.5f, R=%.5f, P=%.5f, Y=%.5f (order=%d)\n", coord.x, coord.y, coord.z, coord.roll, coord.pitch, coord.yaw, poseOutputOrder); diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.h b/App/WorkpieceHole/WorkpieceHoleApp/Version.h index ff7ad10..4522a21 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.h @@ -6,7 +6,7 @@ // 版本字符串 #define WORKPIECEHOLE_VERSION_STRING "1.1.3" -#define WORKPIECEHOLE_BUILD_STRING "1" +#define WORKPIECEHOLE_BUILD_STRING "2" #define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING // 构建日期 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.md b/App/WorkpieceHole/WorkpieceHoleApp/Version.md index 7fcbf39..5cfcccb 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.md +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.md @@ -1,4 +1,7 @@ # 1.1.3 +## build_1 2026-04-21 +1. 姿态角顺序同步控制页面显示 + ## build_1 2026-04-21 1. 算法更新 diff --git a/AppUtils/AppConfig/Inc/VrHandEyeCalibConfig.h b/AppUtils/AppConfig/Inc/VrHandEyeCalibConfig.h index 08e17d1..4c3f4bb 100644 --- a/AppUtils/AppConfig/Inc/VrHandEyeCalibConfig.h +++ b/AppUtils/AppConfig/Inc/VrHandEyeCalibConfig.h @@ -22,6 +22,10 @@ struct VrHandEyeCalibMatrix double rotZ; /* 接近点偏移 (mm): approach point offset along -axialDir (screw) / -normalDir (tool disk). */ double approachOffset; + /* 目标点在 Robot 坐标系下沿 X/Y/Z 轴的补偿偏移 (mm)。 */ + double offsetX; + double offsetY; + double offsetZ; double outRotX; double outRotY; double outRotZ; @@ -33,9 +37,12 @@ struct VrHandEyeCalibMatrix , rotY(0.0) , rotZ(0.0) , approachOffset(0.0) + , offsetX(0.0) + , offsetY(0.0) + , offsetZ(0.0) , outRotX(0.0) , outRotY(0.0) - , outRotZ(15.0) + , outRotZ(14.56) { double identity[16] = { 1.0, 0.0, 0.0, 0.0, diff --git a/AppUtils/AppConfig/Src/ConfigXmlUtils.cpp b/AppUtils/AppConfig/Src/ConfigXmlUtils.cpp index ed0459d..6abef98 100644 --- a/AppUtils/AppConfig/Src/ConfigXmlUtils.cpp +++ b/AppUtils/AppConfig/Src/ConfigXmlUtils.cpp @@ -38,6 +38,12 @@ static void LoadSingleHandEyeCalibMatrix(XMLElement* handEyeElement, calibMatrix.rotZ = handEyeElement->DoubleAttribute("rotZ"); if (handEyeElement->Attribute("approachOffset")) calibMatrix.approachOffset = handEyeElement->DoubleAttribute("approachOffset"); + if (handEyeElement->Attribute("offsetX")) + calibMatrix.offsetX = handEyeElement->DoubleAttribute("offsetX"); + if (handEyeElement->Attribute("offsetY")) + calibMatrix.offsetY = handEyeElement->DoubleAttribute("offsetY"); + if (handEyeElement->Attribute("offsetZ")) + calibMatrix.offsetZ = handEyeElement->DoubleAttribute("offsetZ"); if (handEyeElement->Attribute("outRotX")) calibMatrix.outRotX = handEyeElement->DoubleAttribute("outRotX"); if (handEyeElement->Attribute("outRotY")) @@ -108,6 +114,9 @@ void SaveHandEyeCalibMatrixs(XMLDocument& doc, handEyeElement->SetAttribute("outRotY", calibMatrix.outRotY); handEyeElement->SetAttribute("outRotZ", calibMatrix.outRotZ); handEyeElement->SetAttribute("approachOffset", calibMatrix.approachOffset); + handEyeElement->SetAttribute("offsetX", calibMatrix.offsetX); + handEyeElement->SetAttribute("offsetY", calibMatrix.offsetY); + handEyeElement->SetAttribute("offsetZ", calibMatrix.offsetZ); containerElement->InsertEndChild(handEyeElement); } root->InsertEndChild(containerElement); diff --git a/AppUtils/UICommon/Inc/HandEyeCalibWidget.h b/AppUtils/UICommon/Inc/HandEyeCalibWidget.h index 2bec48b..8cbba7b 100644 --- a/AppUtils/UICommon/Inc/HandEyeCalibWidget.h +++ b/AppUtils/UICommon/Inc/HandEyeCalibWidget.h @@ -41,6 +41,9 @@ struct HandEyeCalibData double rotY = 0.0; double rotZ = 0.0; double approachOffset = 0.0; // 接近点偏移（mm），沿目标姿态反方向远离目标点 + double offsetX = 0.0; // 目标点补偿偏移 X（mm），Robot 坐标系 + double offsetY = 0.0; // 目标点补偿偏移 Y（mm） + double offsetZ = 0.0; // 目标点补偿偏移 Z（mm） }; /** @@ -83,6 +86,14 @@ public: double rotX, double rotY, double rotZ, double approachOffset); + /** + * @brief 设置指定相机的完整外参（含接近点偏移 + 目标点补偿偏移） + */ + void setExtrinsicData(int cameraIndex, int eulerOrder, + double rotX, double rotY, double rotZ, + double approachOffset, + double offsetX, double offsetY, double offsetZ); + /** * @brief 获取指定相机的标定数据 * @return 是否存在该相机的数据 @@ -104,6 +115,14 @@ public: double& outRotX, double& outRotY, double& outRotZ, double& outApproachOffset) const; + /** + * @brief 获取指定相机的完整外参（含接近点偏移 + 目标点补偿偏移） + */ + bool getExtrinsicData(int cameraIndex, int& outEulerOrder, + double& outRotX, double& outRotY, double& outRotZ, + double& outApproachOffset, + double& outOffsetX, double& outOffsetY, double& outOffsetZ) const; + /** * @brief 获取当前选中的相机索引 * @return 相机索引，未选择返回 -1 @@ -126,8 +145,9 @@ public: void setExtrinsicControlsVisible(bool visible); /** - * @brief 显示/隐藏接近点偏移控件（默认隐藏，仅需要 approach 点的 App 启用） + * @brief 显示/隐藏目标点补偿偏移控件（默认隐藏） */ + void setTargetOffsetVisible(bool visible); void setApproachOffsetVisible(bool visible); signals: @@ -185,6 +205,10 @@ private: QLineEdit* m_editRotZ; QLabel* m_labelApproachOffset; QLineEdit* m_editApproachOffset; + QLabel* m_labelTargetOffset; + QLineEdit* m_editOffsetX; + QLineEdit* m_editOffsetY; + QLineEdit* m_editOffsetZ; QVector m_calibDataCache; // 按相机索引缓存 QString m_defaultFilePath; diff --git a/AppUtils/UICommon/Src/HandEyeCalibWidget.cpp b/AppUtils/UICommon/Src/HandEyeCalibWidget.cpp index 01d9927..bb0c8be 100644 --- a/AppUtils/UICommon/Src/HandEyeCalibWidget.cpp +++ b/AppUtils/UICommon/Src/HandEyeCalibWidget.cpp @@ -23,6 +23,10 @@ HandEyeCalibWidget::HandEyeCalibWidget(QWidget *parent) , m_editRotZ(nullptr) , m_labelApproachOffset(nullptr) , m_editApproachOffset(nullptr) + , m_labelTargetOffset(nullptr) + , m_editOffsetX(nullptr) + , m_editOffsetY(nullptr) + , m_editOffsetZ(nullptr) , m_matrixEditable(false) { memset(m_matrixEdits, 0, sizeof(m_matrixEdits)); @@ -103,6 +107,26 @@ void HandEyeCalibWidget::setExtrinsicData(int cameraIndex, int eulerOrder, } } +void HandEyeCalibWidget::setExtrinsicData(int cameraIndex, int eulerOrder, + double rotX, double rotY, double rotZ, + double approachOffset, + double offsetX, double offsetY, double offsetZ) +{ + HandEyeCalibData& data = ensureCalibData(cameraIndex); + data.eulerOrder = eulerOrder; + data.rotX = rotX; + data.rotY = rotY; + data.rotZ = rotZ; + data.approachOffset = approachOffset; + data.offsetX = offsetX; + data.offsetY = offsetY; + data.offsetZ = offsetZ; + + if (currentCameraIndex() == cameraIndex) { + displayExtrinsic(data); + } +} + bool HandEyeCalibWidget::getCalibData(int cameraIndex, double outMatrix[16], bool& outIsCalibrated) const { // 如果当前显示的就是这个相机，且矩阵可编辑，优先从 UI 读取 @@ -166,6 +190,31 @@ bool HandEyeCalibWidget::getExtrinsicData(int cameraIndex, int& outEulerOrder, return true; } +bool HandEyeCalibWidget::getExtrinsicData(int cameraIndex, int& outEulerOrder, + double& outRotX, double& outRotY, double& outRotZ, + double& outApproachOffset, + double& outOffsetX, double& outOffsetY, double& outOffsetZ) const +{ + if (!getExtrinsicData(cameraIndex, outEulerOrder, outRotX, outRotY, outRotZ, outApproachOffset)) { + return false; + } + if (currentCameraIndex() == cameraIndex && m_editOffsetX && m_editOffsetY && m_editOffsetZ) { + outOffsetX = m_editOffsetX->text().trimmed().toDouble(); + outOffsetY = m_editOffsetY->text().trimmed().toDouble(); + outOffsetZ = m_editOffsetZ->text().trimmed().toDouble(); + return true; + } + const HandEyeCalibData* data = findCalibData(cameraIndex); + if (data) { + outOffsetX = data->offsetX; + outOffsetY = data->offsetY; + outOffsetZ = data->offsetZ; + } else { + outOffsetX = outOffsetY = outOffsetZ = 0.0; + } + return true; +} + int HandEyeCalibWidget::currentCameraIndex() const { if (!m_comboCamera || m_comboCamera->count() == 0 || m_comboCamera->currentIndex() < 0) { @@ -208,6 +257,14 @@ void HandEyeCalibWidget::setApproachOffsetVisible(bool visible) } } +void HandEyeCalibWidget::setTargetOffsetVisible(bool visible) +{ + if (m_labelTargetOffset) m_labelTargetOffset->setVisible(visible); + if (m_editOffsetX) m_editOffsetX->setVisible(visible); + if (m_editOffsetY) m_editOffsetY->setVisible(visible); + if (m_editOffsetZ) m_editOffsetZ->setVisible(visible); +} + // ========== 私有槽 ========== void HandEyeCalibWidget::onCameraSelectionChanged(int index) @@ -522,6 +579,41 @@ void HandEyeCalibWidget::setupExtrinsicGroup() m_labelApproachOffset->setVisible(false); m_editApproachOffset->setVisible(false); + // 目标点补偿偏移 X/Y/Z + auto* validatorTargetOffset = new QDoubleValidator(-10000.0, 10000.0, 3, this); + validatorTargetOffset->setNotation(QDoubleValidator::StandardNotation); + + m_editOffsetX = new QLineEdit(this); + m_editOffsetX->setFont(font); + m_editOffsetX->setStyleSheet(editStyle); + m_editOffsetX->setValidator(validatorTargetOffset); + + m_editOffsetY = new QLineEdit(this); + m_editOffsetY->setFont(font); + m_editOffsetY->setStyleSheet(editStyle); + m_editOffsetY->setValidator(validatorTargetOffset); + + m_editOffsetZ = new QLineEdit(this); + m_editOffsetZ->setFont(font); + m_editOffsetZ->setStyleSheet(editStyle); + m_editOffsetZ->setValidator(validatorTargetOffset); + + QWidget* targetOffsetRow = new QWidget(this); + QHBoxLayout* targetOffsetLayout = new QHBoxLayout(targetOffsetRow); + targetOffsetLayout->setContentsMargins(0, 0, 0, 0); + targetOffsetLayout->setSpacing(8); + targetOffsetLayout->addWidget(makeAxisLabel(QString::fromUtf8("X:"))); + targetOffsetLayout->addWidget(m_editOffsetX, 1); + targetOffsetLayout->addWidget(makeAxisLabel(QString::fromUtf8("Y:"))); + targetOffsetLayout->addWidget(m_editOffsetY, 1); + targetOffsetLayout->addWidget(makeAxisLabel(QString::fromUtf8("Z:"))); + targetOffsetLayout->addWidget(m_editOffsetZ, 1); + + m_labelTargetOffset = new QLabel(QString::fromUtf8("目标点偏移 (mm):"), this); + m_labelTargetOffset->setFont(font); + m_labelTargetOffset->setStyleSheet(labelStyle); + form->addRow(m_labelTargetOffset, targetOffsetRow); + displayDefaultExtrinsic(); } @@ -598,6 +690,9 @@ void HandEyeCalibWidget::displayExtrinsic(const HandEyeCalibData& data) if (m_editRotY) m_editRotY->setText(QString::number(data.rotY, 'f', 6)); if (m_editRotZ) m_editRotZ->setText(QString::number(data.rotZ, 'f', 6)); if (m_editApproachOffset) m_editApproachOffset->setText(QString::number(data.approachOffset, 'f', 3)); + if (m_editOffsetX) m_editOffsetX->setText(QString::number(data.offsetX, 'f', 3)); + if (m_editOffsetY) m_editOffsetY->setText(QString::number(data.offsetY, 'f', 3)); + if (m_editOffsetZ) m_editOffsetZ->setText(QString::number(data.offsetZ, 'f', 3)); } void HandEyeCalibWidget::displayDefaultExtrinsic() @@ -611,6 +706,9 @@ void HandEyeCalibWidget::displayDefaultExtrinsic() if (m_editRotY) m_editRotY->setText(QString::number(0.0, 'f', 6)); if (m_editRotZ) m_editRotZ->setText(QString::number(0.0, 'f', 6)); if (m_editApproachOffset) m_editApproachOffset->setText(QString::number(0.0, 'f', 3)); + if (m_editOffsetX) m_editOffsetX->setText(QString::number(0.0, 'f', 3)); + if (m_editOffsetY) m_editOffsetY->setText(QString::number(0.0, 'f', 3)); + if (m_editOffsetZ) m_editOffsetZ->setText(QString::number(0.0, 'f', 3)); } bool HandEyeCalibWidget::readMatrixFromUI(double outMatrix[16]) const @@ -641,6 +739,9 @@ void HandEyeCalibWidget::commitExtrinsicToCache(int cameraIndex) if (m_editApproachOffset) { data.approachOffset = m_editApproachOffset->text().trimmed().toDouble(); } + if (m_editOffsetX) data.offsetX = m_editOffsetX->text().trimmed().toDouble(); + if (m_editOffsetY) data.offsetY = m_editOffsetY->text().trimmed().toDouble(); + if (m_editOffsetZ) data.offsetZ = m_editOffsetZ->text().trimmed().toDouble(); } HandEyeCalibData* HandEyeCalibWidget::findCalibData(int cameraIndex)