diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp index b826c7a..1c421e9 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp +++ b/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(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(order), eyeRollAfter, eyePitchAfter, eyeYawAfter); + } std::array robotAxes; if (!TransformAxes(eyeAxes, eyeInHandTransform, robotAxes)) { LOG_WARNING("[Algo Thread] Screw %zu: 轴变换到机器人坐标系失败，已跳过\n", i); @@ -653,6 +676,15 @@ int DetectPresenter::DetectToolDisk( // 姿态：三轴直接取算法输出 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。 std::array 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(order), eyeRollBefore, eyePitchBefore, eyeYawBefore); + } ApplyAxesRotation(eyeAxes, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ); std::array 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(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)); diff --git a/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so b/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so index 8302012..4790fcc 100644 Binary files a/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so and b/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll index a3d271a..fcd5332 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb index 998620c..73e2904 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll index 65bccfb..b7849e6 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb index 5b298e0..96fae4b 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll index 2ec909f..a78003f 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb index 49ac8a1..816c75a 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll index a006894..de533d1 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb index 9e756de..ae6ccde 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb differ diff --git a/GrabBagPrj/AppList.md b/GrabBagPrj/AppList.md index 25bcfc4..8f4973f 100644 --- a/GrabBagPrj/AppList.md +++ b/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 | 无 |