diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.h b/App/WorkpieceHole/WorkpieceHoleApp/Version.h index 4522a21d..51d93717 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.h @@ -5,8 +5,8 @@ #define WORKPIECEHOLE_APP_NAME "工件孔定位" // 版本字符串 -#define WORKPIECEHOLE_VERSION_STRING "1.1.3" -#define WORKPIECEHOLE_BUILD_STRING "2" +#define WORKPIECEHOLE_VERSION_STRING "1.1.4" +#define WORKPIECEHOLE_BUILD_STRING "1" #define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING // 构建日期 diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so index 2d8b9d42..3e3f83f7 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so differ diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so index 4f855920..6ab0bf9e 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so differ diff --git a/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h b/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h index d287e938..a05da66b 100644 --- a/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h +++ b/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h @@ -6,6 +6,20 @@ // SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b); +// +SG_APISHARED_EXPORT SVzNL3DPoint vec3_multiply(const SVzNL3DPoint& a, const double s); +// dot +SG_APISHARED_EXPORT double vec3_dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b); +// ģ +SG_APISHARED_EXPORT double vec3_length(const SVzNL3DPoint& a); +// һλ +SG_APISHARED_EXPORT SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a); + +/** + * @brief ƽ v ƽ淨 n ת theta + * v ƽڣԶʹü򻯰޵˹ + */ +SG_APISHARED_EXPORT SVzNL3DPoint wd_rotateVectorInPlane(const SVzNL3DPoint& v, const SVzNL3DPoint& n, double theta); //ʱתʱ > 0 ˳ʱתʱ < 0 SG_APISHARED_EXPORT SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle); @@ -144,6 +158,14 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM( const SSG_cornerParam cornerPara, SSG_lineFeature* line_features); +//ݾԷֶ +SG_APISHARED_EXPORT void wd_lineDataSegment_dist( + std::vector< SWD3DPointPostion>& lineData, + std::vector& segs, + const double maxDistTh, + const int minSegSize +); + SG_APISHARED_EXPORT void wd_lineDataSegment_zDist( std::vector< SVzNL3DPosition>& lineData, std::vector< SVzNL3DPosition>& vldPts, @@ -183,6 +205,16 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod( std::vector& line_rodArcs // ); +//ȡcornerֵʵֺʹô˺дŻ +SG_APISHARED_EXPORT void wd_searchCornerPeaks( + std::vector< SSG_pntDirAngle>& corners, + std::vector< SVzNL3DPosition>& vldPts, + const double minCornerTh, + double cornerMergeScale, + std::vector< SSG_pntDirAngle>& cornerPeakP, + std::vector< SSG_pntDirAngle>& cornerPeakM +); + /// /// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 /// nPointIdx¶Feature @@ -538,6 +570,13 @@ SG_APISHARED_EXPORT void lineFitting_abc( double* _b, double* _c); +//ϳֱͨ߷ax+by+c=0ֱ +SG_APISHARED_EXPORT void indexingPtLineFitting_abc( + std::vector< SWD3DPointPostion>& inliers, + double* _a, + double* _b, + double* _c); + /** * @brief ռֱС * @param points ά㼯2㣬壩 diff --git a/AppAlgo/workpieceHolePositioning/Inc/SG_baseDataType.h b/AppAlgo/workpieceHolePositioning/Inc/SG_baseDataType.h index 456aabac..e619576e 100644 --- a/AppAlgo/workpieceHolePositioning/Inc/SG_baseDataType.h +++ b/AppAlgo/workpieceHolePositioning/Inc/SG_baseDataType.h @@ -39,6 +39,13 @@ typedef struct SWD3DPoint point; }SWDIndexing3DPoint; +typedef struct +{ + int lineIdx; + int ptIdx; + SVzNL3DPoint point; +}SWD3DPointPostion; + typedef struct { int lineIdx; diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll index 09d5d579..59166cd7 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib index 4966ca62..7b909a38 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb index 4146a72a..8aa9e631 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll index a9b84665..e7785202 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb index 72f771a7..1d059745 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll index 95ce5d3a..df75c9ab 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib index 0a90c88c..a2c5575a 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb index 440d1b73..9084c296 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll index 66ad3cff..dc7a3c8c 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb index 045454e3..83af667d 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb differ diff --git a/Module/HandEyeCalib/Inc/IHandEyeCalib.h b/Module/HandEyeCalib/Inc/IHandEyeCalib.h index 9fc797b0..6b468e86 100644 --- a/Module/HandEyeCalib/Inc/IHandEyeCalib.h +++ b/Module/HandEyeCalib/Inc/IHandEyeCalib.h @@ -56,6 +56,14 @@ public: virtual int CalculateEyeInHandWithPose(const std::vector& calibData, HECCalibResult& result) = 0; + // 将旋转矩阵安全地转换为指定顺序的欧拉角（带万向节锁处理和帧间连续性修正） + // order: 目标欧拉角旋转顺序 + // prevAngles: 上一帧输出的角度指针，传入则启用帧间连续性修正，传nullptr则不修正 + virtual void RotationMatrixToEulerZYXSafe(const HECRotationMatrix& R, + HECEulerOrder order, + const HECEulerAngles* prevAngles, + HECEulerAngles& angles) = 0; + virtual HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) = 0; virtual int CalculateEyeToHandWithPose(const std::vector& calibData, diff --git a/Module/HandEyeCalib/Src/HandEyeCalib.cpp b/Module/HandEyeCalib/Src/HandEyeCalib.cpp index 23a2fcf1..f4c7ac83 100644 --- a/Module/HandEyeCalib/Src/HandEyeCalib.cpp +++ b/Module/HandEyeCalib/Src/HandEyeCalib.cpp @@ -798,6 +798,209 @@ Eigen::Matrix3d HandEyeCalib::eulerToRotationMatrix(double rx, double ry, double return result; } +void HandEyeCalib::decomposeStandard(const Eigen::Matrix3d& Rm, HECEulerOrder order, HECEulerAngles& angles) +{ + switch (order) { + case HECEulerOrder::ZYX: + // R = Rz(yaw) · Ry(pitch) · Rx(roll) + angles.pitch = std::asin(-Rm(2, 0)); + angles.yaw = std::atan2(Rm(1, 0), Rm(0, 0)); + angles.roll = std::atan2(Rm(2, 1), Rm(2, 2)); + break; + case HECEulerOrder::ZXY: + // R = Rz(yaw) · Rx(roll) · Ry(pitch) + angles.roll = std::asin(Rm(2, 1)); + angles.yaw = std::atan2(-Rm(0, 1), Rm(1, 1)); + angles.pitch = std::atan2(-Rm(2, 0), Rm(2, 2)); + break; + case HECEulerOrder::XYZ: + // R = Rx(roll) · Ry(pitch) · Rz(yaw) + angles.pitch = std::asin(Rm(0, 2)); + angles.roll = std::atan2(-Rm(1, 2), Rm(2, 2)); + angles.yaw = std::atan2(-Rm(0, 1), Rm(0, 0)); + break; + case HECEulerOrder::XZY: + // R = Rx(roll) · Rz(yaw) · Ry(pitch) + angles.yaw = std::asin(-Rm(0, 1)); + angles.roll = std::atan2(Rm(2, 1), Rm(1, 1)); + angles.pitch = std::atan2(Rm(0, 2), Rm(0, 0)); + break; + case HECEulerOrder::YXZ: + // R = Ry(pitch) · Rx(roll) · Rz(yaw) + angles.roll = std::asin(-Rm(1, 2)); + angles.pitch = std::atan2(Rm(0, 2), Rm(2, 2)); + angles.yaw = std::atan2(Rm(1, 0), Rm(1, 1)); + break; + case HECEulerOrder::YZX: + // R = Ry(pitch) · Rz(yaw) · Rx(roll) + angles.yaw = std::asin(Rm(1, 0)); + angles.pitch = std::atan2(-Rm(2, 0), Rm(0, 0)); + angles.roll = std::atan2(-Rm(1, 2), Rm(1, 1)); + break; + } +} + +void HandEyeCalib::RotationMatrixToEulerZYXSafe( + const HECRotationMatrix& R, + HECEulerOrder order, + const HECEulerAngles* prevAngles, + HECEulerAngles& angles) +{ + // Step 1: 将 HECRotationMatrix 转换为 Eigen::Matrix3d + Eigen::Matrix3d Rm; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + Rm(i, j) = R.at(i, j); + + constexpr double SINGULAR_THRESHOLD = 0.9998; + + // Step 2: 根据 order 计算中间轴 sin 值，检测奇异区 + // 各顺序的 sing_val = sin(中间轴角度)，>0 对应 +90°，<0 对应 -90° + double sing_val = 0; + switch (order) { + case HECEulerOrder::ZYX: sing_val = -Rm(2, 0); break; // Ry(pitch) + case HECEulerOrder::ZXY: sing_val = Rm(2, 1); break; // Rx(roll) + case HECEulerOrder::XYZ: sing_val = Rm(0, 2); break; // Ry(pitch) + case HECEulerOrder::XZY: sing_val = -Rm(0, 1); break; // Rz(yaw) + case HECEulerOrder::YXZ: sing_val = -Rm(1, 2); break; // Rx(roll) + case HECEulerOrder::YZX: sing_val = Rm(1, 0); break; // Rz(yaw) + } + + if (std::abs(sing_val) > SINGULAR_THRESHOLD) { + // === Steps 3-5: 绕路分解 === + // 绕路顺序：中间轴不同的配对顺序，奇异区不重叠 + // ZYX↔ZXY, XYZ↔XZY, YXZ↔YZX + HECEulerOrder bypass; + switch (order) { + case HECEulerOrder::ZYX: bypass = HECEulerOrder::ZXY; break; + case HECEulerOrder::ZXY: bypass = HECEulerOrder::ZYX; break; + case HECEulerOrder::XYZ: bypass = HECEulerOrder::XZY; break; + case HECEulerOrder::XZY: bypass = HECEulerOrder::XYZ; break; + case HECEulerOrder::YXZ: bypass = HECEulerOrder::YZX; break; + case HECEulerOrder::YZX: bypass = HECEulerOrder::YXZ; break; + } + + // Step 3: 用绕路顺序分解（绕路顺序在当前区域不奇异，数值稳定） + HECEulerAngles bypassAngles; + RotationMatrixToEuler(R, bypass, bypassAngles); + + // Step 4: 用绕路结果重建 R'，验证与原始 R 的误差 + HECRotationMatrix Rp_hec; + EulerToRotationMatrix(bypassAngles, bypass, Rp_hec); + Eigen::Matrix3d Rp; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + Rp(i, j) = Rp_hec.at(i, j); + + double err = (Rp - Rm).norm(); + + // Step 5: 对 R' 按目标顺序的万向锁公式分解（设定末轴为0） + if (err < 1e-6) { + switch (order) { + case HECEulerOrder::ZYX: + if (sing_val > 0) { + // pitch ≈ +90° + angles.pitch = M_PI / 2.0; + angles.roll = 0.0; + angles.yaw = -std::atan2(Rp(0, 1), Rp(1, 1)); + } else { + angles.pitch = -M_PI / 2.0; + angles.roll = 0.0; + angles.yaw = std::atan2(-Rp(0, 1), Rp(1, 1)); + } + break; + + case HECEulerOrder::ZXY: + if (sing_val > 0) { + angles.roll = M_PI / 2.0; + angles.pitch = 0.0; + angles.yaw = std::atan2(Rp(1, 0), Rp(0, 0)); + } else { + angles.roll = -M_PI / 2.0; + angles.pitch = 0.0; + angles.yaw = std::atan2(Rp(1, 0), Rp(0, 0)); + } + break; + + case HECEulerOrder::XYZ: + if (sing_val > 0) { + angles.pitch = M_PI / 2.0; + angles.yaw = 0.0; + angles.roll = std::atan2(Rp(1, 0), Rp(1, 1)); + } else { + angles.pitch = -M_PI / 2.0; + angles.yaw = 0.0; + angles.roll = std::atan2(-Rp(1, 0), Rp(1, 1)); + } + break; + + case HECEulerOrder::XZY: + if (sing_val > 0) { + angles.yaw = M_PI / 2.0; + angles.pitch = 0.0; + angles.roll = std::atan2(Rp(2, 0), Rp(1, 0)); + } else { + angles.yaw = -M_PI / 2.0; + angles.pitch = 0.0; + angles.roll = std::atan2(-Rp(2, 0), -Rp(1, 0)); + } + break; + + case HECEulerOrder::YXZ: + if (sing_val > 0) { + angles.roll = M_PI / 2.0; + angles.yaw = 0.0; + angles.pitch = std::atan2(Rp(0, 1), Rp(0, 0)); + } else { + angles.roll = -M_PI / 2.0; + angles.yaw = 0.0; + angles.pitch = std::atan2(-Rp(0, 1), Rp(0, 0)); + } + break; + + case HECEulerOrder::YZX: + if (sing_val > 0) { + angles.yaw = M_PI / 2.0; + angles.roll = 0.0; + angles.pitch = std::atan2(Rp(2, 1), Rp(2, 2)); + } else { + angles.yaw = -M_PI / 2.0; + angles.roll = 0.0; + angles.pitch = std::atan2(Rp(0, 2), Rp(0, 1)); + } + break; + } + + double rollDeg, pitchDeg, yawDeg; + angles.toDegrees(rollDeg, pitchDeg, yawDeg); + LOG_INFO("[HandEyeCalib] 万向锁处理(%d): roll=%.2f°, pitch=%.2f°, yaw=%.2f°, R验证误差=%.2e\n", + static_cast(order), rollDeg, pitchDeg, yawDeg, err); + } else { + // 验证失败（不应发生），回退标准分解 + decomposeStandard(Rm, order, angles); + LOG_INFO("[HandEyeCalib] 绕路验证失败(err=%.2e)，回退标准分解\n", err); + } + } else { + // Step 6: 非奇异区直接按目标顺序分解 + decomposeStandard(Rm, order, angles); + } + + // Step 7: 帧间连续性修正（任意轴突变超过150°则加减360°） + if (prevAngles) { + constexpr double JUMP_THRESHOLD = 150.0 * M_PI / 180.0; + auto correct = [](double& cur, double prev, double thr) { + double delta = cur - prev; + if (delta > thr) cur -= 2.0 * M_PI; + else if (delta < -thr) cur += 2.0 * M_PI; + }; + correct(angles.yaw, prevAngles->yaw, JUMP_THRESHOLD); + correct(angles.pitch, prevAngles->pitch, JUMP_THRESHOLD); + correct(angles.roll, prevAngles->roll, JUMP_THRESHOLD); + } + + // Step 8: 输出已完成（angles 包含 roll, pitch, yaw） +} + HECTCPCalibResult HandEyeCalib::CalculateTCP(const HECTCPCalibData& data) { HECTCPCalibResult result; diff --git a/Module/HandEyeCalib/_Inc/HandEyeCalib.h b/Module/HandEyeCalib/_Inc/HandEyeCalib.h index 8c45bfea..d50e169f 100644 --- a/Module/HandEyeCalib/_Inc/HandEyeCalib.h +++ b/Module/HandEyeCalib/_Inc/HandEyeCalib.h @@ -54,12 +54,20 @@ public: int CalculateEyeInHandWithPose(const std::vector& calibData, HECCalibResult& result) override; + void RotationMatrixToEulerZYXSafe(const HECRotationMatrix& R, + HECEulerOrder order, + const HECEulerAngles* prevAngles, + HECEulerAngles& angles) override; + HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) override; int CalculateEyeToHandWithPose(const std::vector& calibData, HECCalibResult& result) override; private: + // 非奇异区的标准分解（各顺序的 atan2/asin 公式） + void decomposeStandard(const Eigen::Matrix3d& Rm, HECEulerOrder order, HECEulerAngles& angles); + Eigen::Matrix3d eulerToRotationMatrix(double rx, double ry, double rz,