From e09a59bbd917a24e75fff153deffbbd18eaaaccb Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 15 Jul 2026 23:55:16 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20version=201.6.4=20:?= =?UTF-8?q?=20=E6=8B=93=E6=99=AE=E5=8F=915=E7=A7=8D=E5=B7=A5=E4=BB=B6?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=AD=A3=E5=8F=8D=E5=88=A4=E6=96=AD=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SG_Algorithm.sln | 27 +++++ sourceCode/SG_errCode.h | 1 + sourceCode/workpieceHolePositioning.cpp | 101 +++++++++++++++++- .../workpieceHolePositioning.vcxproj | 4 +- .../workpieceHolePositioning_test.cpp | 16 +-- 5 files changed, 137 insertions(+), 12 deletions(-) diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 197e6ee..4b041af 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -249,6 +249,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_planeLocalization_test", {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "hybridPosePositioning_test", "hybridPosePositioning_test\hybridPosePositioning_test.vcxproj", "{288E402C-183A-49AD-8EE1-5B1AE4E6F617}" + ProjectSection(ProjectDependencies) = postProject + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC} = {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC} + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "hybridPosePositioning", "hybridPosePositioning\hybridPosePositioning.vcxproj", "{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -633,6 +644,22 @@ Global {E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x64.Build.0 = Release|x64 {E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x86.ActiveCfg = Release|Win32 {E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x86.Build.0 = Release|Win32 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x64.ActiveCfg = Debug|x64 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x64.Build.0 = Debug|x64 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x86.ActiveCfg = Debug|Win32 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x86.Build.0 = Debug|Win32 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x64.ActiveCfg = Release|x64 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x64.Build.0 = Release|x64 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x86.ActiveCfg = Release|Win32 + {288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x86.Build.0 = Release|Win32 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x64.ActiveCfg = Debug|x64 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x64.Build.0 = Debug|x64 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x86.ActiveCfg = Debug|Win32 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x86.Build.0 = Debug|Win32 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x64.ActiveCfg = Release|x64 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x64.Build.0 = Release|x64 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x86.ActiveCfg = Release|Win32 + {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 812f8d8..b4b8fe1 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -12,6 +12,7 @@ #define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009 #define SX_ERR_ZERO_OBJECTS -1010 #define SX_ERR_NULL_GROUND_PLANE -1011 +#define SX_ERR_ZERO_2D_OBJECTS -1012 //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index ef0c582..a6a70e9 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -33,7 +33,8 @@ //version 1.6.1 : 添加了拓普发工件2、工件3、工件4的姿态计算。 //version 1.6.2 : 拓普发算法代码同步。 //version 1.6.3 : 拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。 -std::string m_strVersion = "HolePostion 1.6.3"; +//version 1.6.4 : 拓普发5种工件添加正反判断。 +std::string m_strVersion = "HolePostion 1.6.4"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -2402,13 +2403,19 @@ void wd_workpieceHolePositioning_3( //内部参数 double smallHoleMinDistance = 8.0; double maxDistanceFromCenterToLine = 25.0; + double reverseJudgeDistToCenterTH = 33.0; + double midPointToLongerHoleDIstTh = 20.0; // + double distP1P2_short_minTh = 35.0; + double distP1P2_short_maxTh = 48.0; + double distP1P2_long_minTh = 25.0; + double distP1P2_long_maxTh = 38.0; SVzNLRangeD distRange = { 0, bigHoleDiameter * 1.2 }; double deltaZ = 10.0; std::vector< WD_workpieceInfo> allWorkpiece; double highest_z = -1; for (int objIdx = 0; objIdx < bigHoleSize; objIdx++) { - if (objIdx == 42) + if (objIdx == 11) int kkk = 1; if (bigHoles[objIdx].radius < 0) @@ -2425,10 +2432,23 @@ void wd_workpieceHolePositioning_3( if (groups.size() == 0) //工件3和工作4的特点是小孔分为上下各3个。分组 continue; + std::vector> distancesToCenter; + distancesToCenter.resize(groups.size()); + for (int grp = 0; grp < (int)groups.size(); grp++) + { + distancesToCenter[grp].resize(groups[grp].size()); + for (int m = 0; m < (int)groups[grp].size(); m++) + { + double dist = sqrt(pow(p0.center.x - groups[grp][m].center.x, 2) + + pow(p0.center.y - groups[grp][m].center.y, 2)); + distancesToCenter[grp][m] = dist; + } + } //搜索每组中距离大于大孔直径0.8倍的目标 SWD_HoleInfo p1, p2; bool foundObj = false; bool isYDIr = true; + bool isReverse = false; for (int grp = 0; grp < (int)groups.size(); grp++) { std::vector& a_grp = groups[grp]; @@ -2457,6 +2477,23 @@ void wd_workpieceHolePositioning_3( p1 = a_grp[idx1]; p2 = a_grp[idx2]; foundObj = true; + + double dist1 = distancesToCenter[grp][idx1]; + double dist2 = distancesToCenter[grp][idx2]; + if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH)) //longer + { + if (p1.center.y < p0.center.y) //长的在上面，反 + isReverse = true; + foundObj = true; + break; + } + else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter + { + if (p1.center.y > p0.center.y) //短的在下面，反 + isReverse = true; + foundObj = true; + break; + } break; } else if (validRelations.size() > 1) @@ -2471,7 +2508,29 @@ void wd_workpieceHolePositioning_3( int idx2 = bestPair.data_1; p1 = a_grp[idx1]; p2 = a_grp[idx2]; - foundObj = true; + + double dist1 = distancesToCenter[grp][idx1]; + double dist2 = distancesToCenter[grp][idx2]; + if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH)) //longer + { + if ((bestPair.value1 > distP1P2_long_minTh)&&(bestPair.value1 < distP1P2_long_maxTh)) + { + if (p1.center.y < p0.center.y) //长的在上面，反 + isReverse = true; + foundObj = true; + break; + } + } + else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter + { + if ( (bestPair.value1 > distP1P2_short_minTh)&& (bestPair.value1 < distP1P2_short_maxTh)) + { + if (p1.center.y > p0.center.y) //短的在下面，反 + isReverse = true; + foundObj = true; + break; + } + } } } if (false == foundObj) @@ -2513,6 +2572,39 @@ void wd_workpieceHolePositioning_3( p2 = pTmp2; foundObj = true; isYDIr = false; + //判断正反 + bool judeOk = false; + for (int n = 0; n < (int)groups[0].size(); n++) + { + double _dist = sqrt(pow(p1.center.x - groups[0][n].center.x, 2) + + pow(p1.center.y - groups[0][n].center.y, 2)); + if ((distancesToCenter[0][n] > reverseJudgeDistToCenterTH) && (_dist < midPointToLongerHoleDIstTh)) + { + if (groups[0][n].center.y < p0.center.y) + { + judeOk = true; + isReverse = true; + break; + } + } + } + if (false == judeOk) + { + for (int n = 0; n < (int)groups[1].size(); n++) + { + double _dist = sqrt(pow(p2.center.x - groups[1][n].center.x, 2) + + pow(p2.center.y - groups[1][n].center.y, 2)); + if ((distancesToCenter[1][n] > reverseJudgeDistToCenterTH) && (_dist < midPointToLongerHoleDIstTh)) + { + if (groups[1][n].center.y < p0.center.y) + { + judeOk = true; + isReverse = true; + break; + } + } + } + } } } @@ -2521,7 +2613,6 @@ void wd_workpieceHolePositioning_3( continue; SVzNL3DPoint centerPoint = p0.center; - //计算工件法向 SVzNLRangeD distRange = { bigHoleDiameter / 2 + 2.0, bigHoleDiameter / 2 + 4.0 }; std::vector Points3ds; @@ -2574,6 +2665,8 @@ void wd_workpieceHolePositioning_3( WD_workpieceInfo a_workpiece; a_workpiece.workpieceType = smallHoleParam.workpieceType; + if (true == isReverse) //反 + a_workpiece.workpieceType = a_workpiece.workpieceType | 0x10000; a_workpiece.center = centerPoint; y_dir = vec3_normalize(y_dir); a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 }; diff --git a/workpieceHolePositioning/workpieceHolePositioning.vcxproj b/workpieceHolePositioning/workpieceHolePositioning.vcxproj index 066ce15..e2b6cea 100644 --- a/workpieceHolePositioning/workpieceHolePositioning.vcxproj +++ b/workpieceHolePositioning/workpieceHolePositioning.vcxproj @@ -85,12 +85,12 @@ true $(SolutionDir)build\$(Platform)\$(Configuration)\ - ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) false $(SolutionDir)build\$(Platform)\$(Configuration)\ - ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index b67cd84..a668473 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -189,7 +189,7 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi int number = (int)workpiecePositioning.size(); for (int i = 0; i < number; i++) { - sprintf_s(dataStr, 250, "工件_%d", i + 1); + sprintf_s(dataStr, 250, "工件_%d: type=%x", i + 1, workpiecePositioning[i].workpieceType); sw << dataStr << std::endl; int holeNumber = (int)workpiecePositioning[i].holes.size(); for (int j = 0; j < holeNumber; j++) @@ -947,11 +947,15 @@ void _XOYprojection( if (i == 0) { rgb = { 255, 0, 0 }; + if((holes[i].workpieceType&0xffff0000) > 0)//方向为反 + rgb = { 255, 0, 255 }; size = int(3.0/scale); } else { rgb = { 255, 255, 0 }; + if ((holes[i].workpieceType & 0xffff0000) > 0)//方向为反 + rgb = { 0, 255, 255 }; size = int(3.0 / scale); } WD_workpieceInfo& a_hole = holes[i]; @@ -1569,7 +1573,7 @@ void TuoPuFa_holePosition_test3(void) }; SVzNLRange fileIdx[TPF3_TEST_GROUP] = { - {1,10}, {1, 10}, {1,10}, {1,10}, {1, 10}, + {1,11}, {1, 10}, {1,10}, {1,10}, {1, 10}, {1,10}, {1, 10}, }; @@ -1635,7 +1639,7 @@ void TuoPuFa_holePosition_test3(void) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 2; + //fidx = 3; char _scan_file[256]; sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx); std::vector> scanLines; @@ -2736,11 +2740,11 @@ int main() TuoPuFa_holePosition_test5(); else if (keSG_拓普发孔定位_工件全测试 == testMode) { - TuoPuFa_holePosition_test(); - TuoPuFa_holePosition_test2(); + //TuoPuFa_holePosition_test(); + //TuoPuFa_holePosition_test2(); TuoPuFa_holePosition_test3(); TuoPuFa_holePosition_test4(); - TuoPuFa_holePosition_test5(); + //TuoPuFa_holePosition_test5(); } else if (keSG_华航孔定位 == testMode) HuaHang_holePosition_test();