工具盘更新了计算方式

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

@@ -210,10 +210,12 @@ void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation,
     NormalizePitchRange(rollDeg, pitchDeg, yawDeg);
 }
 
-// 万向锁消歧：当 |pitch| 接近 90° 时，Rx(roll)·Ry(pitch)·Rz(yaw) 的分解退化，
-// 任意满足 roll+yaw = const (pitch>0) 或 roll-yaw = const (pitch<0) 的组合
-// 都表示同一旋转矩阵。本函数把 yaw 锚定到 refYaw 附近，把余下自由度全部分配到 roll，
-// 让不同帧的输出稳定落在参考附近（通常传入机器人法兰的 rx、rz 作为参考）。
+// 万向锁消歧：当 |pitch| 接近 90° 时，Rz(yaw)·Ry(pitch)·Rx(roll) 的分解退化成一个自由度。
+// 严格展开可得：
+//   pitch = +90°  →  R 只依赖 (roll - yaw)；任意满足 roll - yaw = const 的组合都表示同一 R
+//   pitch = -90°  →  R 只依赖 (roll + yaw)；任意满足 roll + yaw = const 的组合都表示同一 R
+// 本函数把 yaw 锚定到 refYaw 附近，把剩余自由度全部放到 roll，
+// 让不同帧的输出稳定落在参考（通常是机器人法兰的 rx、rz）附近。
 //
 // 【强消歧配置】：阈值覆盖 80°-85°，优先保证数值稳定性，代价是 pitch∈[80°,85°]
 // 的过渡带会改变真实旋转矩阵（X 轴可能偏 3°-5°）。如需精确定姿请收窄阈值或换欧拉顺序。
@@ -230,18 +232,21 @@ void ResolveGimbalAmbiguity(double& rollDeg, double pitchDeg, double& yawDeg,
         return;
     }
 
-    // 计算"完全消歧"后的参考目标
+    // 不变量方向与 pitch 符号的关系（由 Rz·Ry·Rx 展开式导出）：
+    //   pitch > 0 → inv = roll - yaw
+    //   pitch < 0 → inv = roll + yaw
     const bool positivePitch = (pitchDeg >= 0.0);
-    const double sum    = positivePitch ? (rollDeg + yawDeg)       : (rollDeg - yawDeg);
-    const double refSum = positivePitch ? (refRollDeg + refYawDeg) : (refRollDeg - refYawDeg);
+    const double sum    = positivePitch ? (rollDeg - yawDeg)       : (rollDeg + yawDeg);
+    const double refSum = positivePitch ? (refRollDeg - refYawDeg) : (refRollDeg + refYawDeg);
 
     double adjustedSum = sum;
     while (adjustedSum - refSum >  180.0) { adjustedSum -= 360.0; }
     while (adjustedSum - refSum < -180.0) { adjustedSum += 360.0; }
 
     const double fullYaw  = WrapDegreesTo180(refYawDeg);
-    const double fullRoll = WrapDegreesTo180(positivePitch ? (adjustedSum - fullYaw)
-                                                            : (adjustedSum + fullYaw));
+    // 反解 roll：positive 时 roll = sum + yaw；negative 时 roll = sum - yaw（与 sum 方向匹配）
+    const double fullRoll = WrapDegreesTo180(positivePitch ? (adjustedSum + fullYaw)
+                                                            : (adjustedSum - fullYaw));
 
     if (absPitch >= kHardThresholdDeg) {
         rollDeg = fullRoll;
@@ -521,7 +526,25 @@ int DetectPresenter::DetectScrew(
             LOG_WARNING("[Algo Thread] Screw %zu: 无法由轴向构建坐标系，已跳过\n", i);
             continue;
         }
+        if (debugParam.enableDebug && debugParam.printDetailLog) {
+            const CTRotationMatrix eyeRotationBefore = BuildRotationMatrix(eyeAxes);
+            double eyeRollBefore = 0.0;
+            double eyePitchBefore = 0.0;
+            double eyeYawBefore = 0.0;
+            RotationMatrixToConfiguredEulerDegrees(eyeRotationBefore, order, eyeRollBefore, eyePitchBefore, eyeYawBefore);
+            LOG_INFO("[Algo Thread] Screw %zu Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
+                     i, static_cast<int>(order), eyeRollBefore, eyePitchBefore, eyeYawBefore);
+        }
         ApplyAxesRotation(eyeAxes, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ);
+        if (debugParam.enableDebug && debugParam.printDetailLog) {
+            const CTRotationMatrix eyeRotationAfter = BuildRotationMatrix(eyeAxes);
+            double eyeRollAfter = 0.0;
+            double eyePitchAfter = 0.0;
+            double eyeYawAfter = 0.0;
+            RotationMatrixToConfiguredEulerDegrees(eyeRotationAfter, order, eyeRollAfter, eyePitchAfter, eyeYawAfter);
+            LOG_INFO("[Algo Thread] Screw %zu Eye Euler after compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
+                     i, static_cast<int>(order), eyeRollAfter, eyePitchAfter, eyeYawAfter);
+        }
         std::array<CTVec3D, 3> robotAxes;
         if (!TransformAxes(eyeAxes, eyeInHandTransform, robotAxes)) {
             LOG_WARNING("[Algo Thread] Screw %zu: 轴变换到机器人坐标系失败，已跳过\n", i);
@@ -653,6 +676,15 @@ int DetectPresenter::DetectToolDisk(
 
     // 姿态：三轴直接取算法输出 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。
     std::array<CTVec3D, 3> eyeAxes = {eyeXAxis, eyeYAxis, eyeNormalDir};
+    if (debugParam.enableDebug && debugParam.printDetailLog) {
+        const CTRotationMatrix eyeRotationBefore = BuildRotationMatrix(eyeAxes);
+        double eyeRollBefore = 0.0;
+        double eyePitchBefore = 0.0;
+        double eyeYawBefore = 0.0;
+        RotationMatrixToConfiguredEulerDegrees(eyeRotationBefore, order, eyeRollBefore, eyePitchBefore, eyeYawBefore);
+        LOG_INFO("[Algo Thread] ToolDisk Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
+                 static_cast<int>(order), eyeRollBefore, eyePitchBefore, eyeYawBefore);
+    }
     ApplyAxesRotation(eyeAxes, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ);
     std::array<CTVec3D, 3> robotAxes;
     if (!TransformAxes(eyeAxes, eyeInHandTransform, robotAxes)) {
@@ -673,20 +705,14 @@ int DetectPresenter::DetectToolDisk(
         RotationMatrixToConfiguredEulerDegrees(eyeRotation, order, eyeRoll, eyePitch, eyeYaw);
 
         LOG_INFO("[Algo Thread] === Tool Disk Pose Debug ===\n");
-        LOG_INFO("[Algo Thread] Eye XDir: (%.6f, %.6f, %.6f)\n", eyeAxes[0].x, eyeAxes[0].y, eyeAxes[0].z);
-        LOG_INFO("[Algo Thread] Eye YDir: (%.6f, %.6f, %.6f)\n", eyeAxes[1].x, eyeAxes[1].y, eyeAxes[1].z);
-        LOG_INFO("[Algo Thread] Eye ZDir: (%.6f, %.6f, %.6f)\n", eyeAxes[2].x, eyeAxes[2].y, eyeAxes[2].z);
         LOG_INFO("[Algo Thread] Eye Rotation Matrix:\n");
         LOG_INFO("  [%.6f, %.6f, %.6f]\n", eyeRotation.at(0, 0), eyeRotation.at(0, 1), eyeRotation.at(0, 2));
         LOG_INFO("  [%.6f, %.6f, %.6f]\n", eyeRotation.at(1, 0), eyeRotation.at(1, 1), eyeRotation.at(1, 2));
         LOG_INFO("  [%.6f, %.6f, %.6f]\n", eyeRotation.at(2, 0), eyeRotation.at(2, 1), eyeRotation.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 (order=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
+        LOG_INFO("[Algo Thread] Eye Euler after compensation (order=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
                  static_cast<int>(order), eyeRoll, eyePitch, eyeYaw);
 
-        LOG_INFO("[Algo Thread] Robot XDir: (%.6f, %.6f, %.6f)\n", robotAxes[0].x, robotAxes[0].y, robotAxes[0].z);
-        LOG_INFO("[Algo Thread] Robot YDir: (%.6f, %.6f, %.6f)\n", robotAxes[1].x, robotAxes[1].y, robotAxes[1].z);
-        LOG_INFO("[Algo Thread] Robot ZDir: (%.6f, %.6f, %.6f)\n", robotAxes[2].x, robotAxes[2].y, robotAxes[2].z);
         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));

GrabBagPrj/AppList.md

@@ -15,7 +15,7 @@
 | 9 | 颗粒尺寸检测 | ParticleSize | 1.0.0.0 |
 | 10 | 双目标记检测 | BinocularMarkServer | 1.0.0.4 |
 | 11 | 铁路隧道槽道测量 | TunnelChannel | 1.0.0.3 |
-| 12 | 螺杆定位 | ScrewPosition | 1.1.5.1 |
+| 12 | 螺杆定位 | ScrewPosition | 1.1.5.2 |
 | 13 | 包裹拆线位置定位 | BagThreadPosition | 1.0.0.4 |
 | 14 | 工件孔定位 | WorkpieceHole | 1.1.2.1 |
 | 16 | 坑孔定位 | HolePitPosition | 无 |