From 0e2fe3e8e19a2649b9c86ece1190f06bd55fa6c0 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 21 Apr 2026 14:47:58 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20version=201.4.6=20:?= =?UTF-8?q?=20=E4=BF=AE=E6=AD=A3=E9=97=AE=E9=A2=98=EF=BC=9A4=E4=B8=AA?= =?UTF-8?q?=E5=AD=94=E7=BB=84=E6=88=90=E5=B7=A5=E4=BD=9C=E6=97=B6=EF=BC=8C?= =?UTF-8?q?=E9=9C=80=E8=A6=814=E4=B8=AA=E5=AD=94=E7=9A=84=E9=AB=98?= =?UTF-8?q?=E5=BA=A6=E5=9F=BA=E6=9C=AC=E4=B8=80=E8=87=B4=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/workpieceHolePositioning.cpp | 35 +++++++++++++------ .../workpieceHolePositioning_test.cpp | 4 +-- 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index 57d1a3c..3ee5724 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -16,7 +16,8 @@ //version 1.4.3 : 对1.3.0拓普发孔定位中调平Bug进行了修正 //version 1.4.4 : 对1.4.3一个小的修正， 不影响结果 //version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果 -std::string m_strVersion = "1.4.5"; +//version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。 +std::string m_strVersion = "1.4.6"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -50,7 +51,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9]) } //搜索最接近distance的目标 -int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, double distance, double distDeviation) +int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, double distance, double distDeviation, double dirAngle, double deltaZ) { int result = -1; int holeSize = (int)holes.size(); @@ -63,10 +64,20 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, do double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2)); double distDiff = abs(dist - distance); - if (minDistDiff > distDiff) + double zDiff = abs(seed.z - holes[i].center.z); + double angle = atan2(seed.y - holes[i].center.y, seed.x - holes[i].center.x); + angle = angle * 180.0 / PI; + double angleDiff = computeAngleDiff(angle, dirAngle); + if (angleDiff > 90) + angleDiff = 180 - angleDiff; + + if ((zDiff < deltaZ) && (angleDiff < 45)) { - minDistDiff = distDiff; - minDistIndex = i; + if (minDistDiff > distDiff) + { + minDistDiff = distDiff; + minDistIndex = i; + } } } if ((minDistIndex >= 0) && (minDistDiff < distDeviation)) @@ -78,7 +89,7 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, do int angleConditionDistanceSearch( SVzNL3DPoint seed, SVzNL3DPoint angleSide, std::vector& holes, - double distance, double distDeviation, + double distance, double distDeviation, double deltaZ, SVzNLRangeD angleRange) { int result = -1; @@ -91,7 +102,8 @@ int angleConditionDistanceSearch( double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2)); double distDiff = abs(dist - distance); - if (distDiff < distDeviation) + double zDiff = abs(seed.z - holes[i].center.z); + if ( (distDiff < distDeviation) && (zDiff < deltaZ)) { distValidHoleIndex.push_back(i); } @@ -808,6 +820,7 @@ void wd_workpieceHolePositioning( //分割 //方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点 double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效 + std::vector< WD_workpieceInfo> allWorkpiece; for (int objIdx = 0; objIdx < objectSize; objIdx++) { @@ -816,7 +829,7 @@ void wd_workpieceHolePositioning( holes[objIdx].radius = -1; SWD_HoleInfo& p0 = holes[objIdx]; - int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation); + int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H/2); if (idx1 < 0) continue; @@ -826,7 +839,7 @@ void wd_workpieceHolePositioning( int idx2 = angleConditionDistanceSearch( p0.center, p1.center, holes, - workpiecePara.holeDist_L, distDeviation, + workpiecePara.holeDist_L, distDeviation, workpiecePara.H/2, angleRange); if (idx2 < 0) continue; @@ -836,7 +849,7 @@ void wd_workpieceHolePositioning( int idx3 = angleConditionDistanceSearch( p1.center, p0.center, holes, - workpiecePara.holeDist_L, distDeviation, + workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, angleRange); if (idx3 < 0) continue; @@ -1449,7 +1462,7 @@ void wd_HolePositioning( std::vector segTrees_v; std::vector segTrees_h; std::vector validObjects; - wd_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects); + WD_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects); if (validObjects.size() > 0) { SSG_intPair a_hvPair = validObjects[0]; diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index c2dba1e..fd36b8f 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void) }; SVzNLRange fileIdx[TPF_TEST_GROUP] = { - {6,6}, {1, 16}, {1,6} + {6,6}, {1, 16}, {9,9} }; const char* ver = wd_workpieceHolePositioningVersion(); @@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void) #endif #if TEST_COMPUTE_HOLE - for (int grp = 1; grp <= 2; grp++) + for (int grp = 2; grp <= 2; grp++) { SSG_planeCalibPara groundCalibPara; //初始化成单位阵