工件孔定位协议输出顺序同步更新了界面的顺序&暂存螺杆定位的工具盘的修改

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

App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp

@@ -396,6 +396,107 @@ CTRotationMatrix BuildRotationMatrix(const std::array<CTVec3D, 3>& 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.x = robotCenter.x + extrinsic.offsetX;
-        pos.y = robotCenter.y;
+        pos.y = robotCenter.y + extrinsic.offsetY;
-        pos.z = robotCenter.z;
+        pos.z = robotCenter.z + extrinsic.offsetZ;
-        pos.approachX = robotApproach.x;
+        pos.approachX = robotApproach.x + extrinsic.offsetX;
-        pos.approachY = robotApproach.y;
+        pos.approachY = robotApproach.y + extrinsic.offsetY;
-        pos.approachZ = robotApproach.z;
+        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.x = robotCenter.x + extrinsic.offsetX;
-    pos.y = robotCenter.y;
+    pos.y = robotCenter.y + extrinsic.offsetY;
-    pos.z = robotCenter.z;
+    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.approachX = robotApproach.x + extrinsic.offsetX;
-    pos.approachY = robotApproach.y;
+    pos.approachY = robotApproach.y + extrinsic.offsetY;
-    pos.approachZ = robotApproach.z;
+    pos.approachZ = robotApproach.z + extrinsic.offsetZ;
     const std::pair<double, double> 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) {

App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp

@@ -245,6 +245,9 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResult
             extrinsic.rotY = calibMatrix.rotY;
             extrinsic.rotZ = calibMatrix.rotZ;
             extrinsic.approachOffset = calibMatrix.approachOffset;
+            extrinsic.offsetX = calibMatrix.offsetX;
+            extrinsic.offsetY = calibMatrix.offsetY;
+            extrinsic.offsetZ = calibMatrix.offsetZ;
             extrinsic.outRotX = calibMatrix.outRotX;
             extrinsic.outRotY = calibMatrix.outRotY;
             extrinsic.outRotZ = calibMatrix.outRotZ;
@@ -252,11 +255,12 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResult
         }
     }
 
-    LOG_INFO("[Algo Thread] camera=%d eulerOrder=%d poseOutputOrder=%d rot=(%.3f, %.3f, %.3f) outRot=(%.3f, %.3f, %.3f) approachOffset=%.3f\n",
+    LOG_INFO("[Algo Thread] camera=%d eulerOrder=%d poseOutputOrder=%d rot=(%.3f, %.3f, %.3f) outRot=(%.3f, %.3f, %.3f) approachOffset=%.3f offset=(%.3f, %.3f, %.3f)\n",
              m_currentCameraIndex, extrinsic.eulerOrder, poseOutputOrder,
              extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ,
              extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ,
-             extrinsic.approachOffset);
+             extrinsic.approachOffset,
+             extrinsic.offsetX, extrinsic.offsetY, extrinsic.offsetZ);
 
     DetectionResult detectionResult;
     detectionResult.cameraIndex = m_currentCameraIndex;

App/ScrewPosition/ScrewPositionApp/dialogalgoarg.cpp

@@ -228,7 +228,10 @@ void DialogAlgoArg::loadHandEyeCalibConfig()
                                                    savedMatrix->rotX,
                                                    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);

App/ScrewPosition/ScrewPositionApp/dialogalgoarg.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>776</width>
-    <height>655</height>
+    <height>733</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -44,7 +44,7 @@
      <x>40</x>
      <y>80</y>
      <width>701</width>
-     <height>501</height>
+     <height>571</height>
     </rect>
    </property>
    <property name="font">
@@ -779,7 +779,7 @@ QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px;
    <property name="geometry">
     <rect>
      <x>40</x>
-     <y>590</y>
+     <y>670</y>
      <width>120</width>
      <height>45</height>
     </rect>
@@ -813,7 +813,7 @@ QPushButton:pressed {
    <property name="geometry">
     <rect>
      <x>400</x>
-     <y>590</y>
+     <y>670</y>
      <width>100</width>
      <height>45</height>
     </rect>
@@ -847,7 +847,7 @@ QPushButton:pressed {
    <property name="geometry">
     <rect>
      <x>520</x>
-     <y>590</y>
+     <y>670</y>
      <width>100</width>
      <height>45</height>
     </rect>
@@ -881,7 +881,7 @@ QPushButton:pressed {
    <property name="geometry">
     <rect>
      <x>640</x>
-     <y>590</y>
+     <y>670</y>
      <width>100</width>
      <height>45</height>
     </rect>

App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h

@@ -34,6 +34,7 @@ public:
                     const double clibMatrix[16],
                     int eulerOrder,
                     int dirVectorInvert,
+                    int poseOutputOrder,
                     WorkpieceHoleDetectionResult& detectionResult);
 
 };

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;
+        switch (poseOutputOrder) {
-        centerRobotPos.pitch = robotRpy.pitch;
+            case POSE_ORDER_RPY:
-        centerRobotPos.yaw   = robotRpy.yaw;
+                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);
 

App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp

@@ -351,11 +351,16 @@ int WorkpieceHolePresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResult
     int dirVectorInvert = configResult.plcRobotServerConfig.dirVectorInvert;
     LOG_INFO("[Algo Thread] Using dir vector invert: %d\n", dirVectorInvert);
 
+    // 获取姿态输出顺序配置
+    int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder;
+    LOG_INFO("[Algo Thread] Using pose output order: %d\n", poseOutputOrder);
+
     WorkpieceHoleDetectionResult detectionResult;
 
     int nRet = m_pDetectPresenter->DetectWorkpieceHole(m_currentCameraIndex, detectionDataCache,
                                 algorithmParams, debugParam, m_dataLoader,
-                                currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, detectionResult);
+                                currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert,
+                                poseOutputOrder, detectionResult);
     // 根据项目类型选择处理方式
     if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
         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<float>(pos.x);
     coord.y = static_cast<float>(pos.y);
     coord.z = static_cast<float>(pos.z);
-
+    coord.roll = static_cast<float>(pos.roll);
-    // 根据姿态输出顺序配置调整输出
+    coord.pitch = static_cast<float>(pos.pitch);
-    switch (poseOutputOrder) {
+    coord.yaw = static_cast<float>(pos.yaw);
-        case POSE_ORDER_RPY:  // Roll, Pitch, Yaw（默认）
-            coord.roll = static_cast<float>(pos.roll);
-            coord.pitch = static_cast<float>(pos.pitch);
-            coord.yaw = static_cast<float>(pos.yaw);
-            break;
-        case POSE_ORDER_RYP:  // Roll, Yaw, Pitch
-            coord.roll = static_cast<float>(pos.roll);
-            coord.pitch = static_cast<float>(pos.yaw);
-            coord.yaw = static_cast<float>(pos.pitch);
-            break;
-        case POSE_ORDER_PRY:  // Pitch, Roll, Yaw
-            coord.roll = static_cast<float>(pos.pitch);
-            coord.pitch = static_cast<float>(pos.roll);
-            coord.yaw = static_cast<float>(pos.yaw);
-            break;
-        case POSE_ORDER_PYR:  // Pitch, Yaw, Roll
-            coord.roll = static_cast<float>(pos.pitch);
-            coord.pitch = static_cast<float>(pos.yaw);
-            coord.yaw = static_cast<float>(pos.roll);
-            break;
-        case POSE_ORDER_YRP:  // Yaw, Roll, Pitch
-            coord.roll = static_cast<float>(pos.yaw);
-            coord.pitch = static_cast<float>(pos.roll);
-            coord.yaw = static_cast<float>(pos.pitch);
-            break;
-        case POSE_ORDER_YPR:  // Yaw, Pitch, Roll
-            coord.roll = static_cast<float>(pos.yaw);
-            coord.pitch = static_cast<float>(pos.pitch);
-            coord.yaw = static_cast<float>(pos.roll);
-            break;
-        default:  // 默认 RPY
-            coord.roll = static_cast<float>(pos.roll);
-            coord.pitch = static_cast<float>(pos.pitch);
-            coord.yaw = static_cast<float>(pos.yaw);
-            break;
-    }
 
     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);

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
 
 // 构建日期

App/WorkpieceHole/WorkpieceHoleApp/Version.md

@@ -1,4 +1,7 @@
 # 1.1.3
+## build_1 2026-04-21
+1. 姿态角顺序同步控制页面显示
+
 ## build_1 2026-04-21
 1. 算法更新
 

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,

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);

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<HandEyeCalibData> m_calibDataCache;  // 按相机索引缓存
     QString m_defaultFilePath;

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)