From 4d4dfe050677a17d2c9e98e965c6963e19fd6d5d Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 7 Oct 2026 10:21:05 +0800 Subject: [PATCH] =?UTF-8?q?hybridPosePositioning=20version=201.4.6=20:=20?= =?UTF-8?q?=E8=BD=AC=E5=AD=90=E9=92=A2=E8=8A=AF=EF=BC=8C=E6=B7=BB=E5=8A=A0?= =?UTF-8?q?=E9=9A=94=E6=9D=BF=E4=B8=8B=E5=B7=A5=E4=BD=9C=E9=83=A8=E5=88=86?= =?UTF-8?q?=E5=8F=AF=E8=A7=81=E6=97=B6=E8=99=9A=E5=81=87=E6=A3=80=E6=B5=8B?= =?UTF-8?q?=E7=9A=84=E6=BB=A4=E9=99=A4=EF=BC=8C=E9=9D=9E=E4=B8=AD=E5=A4=AE?= =?UTF-8?q?=E9=83=A8=E5=88=86=E5=B7=A5=E4=BD=9C=E6=8C=89=E9=AB=98=E5=BA=A6?= =?UTF-8?q?=E6=8E=92=E5=BA=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hybridPosePositioning_test.cpp | 2 +- sourceCode/hybridPosePositioning.cpp | 63 ++++++++++++++++--- 2 files changed, 54 insertions(+), 11 deletions(-) diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index a397570..dec92d4 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -2367,7 +2367,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) }; SVzNLRange fileIdx[HRM_RotorCore_TEST_GROUP] = { - {1,10}, {3,70},{9,9}, {1,49}, {1,54}, {1,6} + {1,10}, {3,70},{9,9}, {51,51}, {1,54}, {1,6} }; const char* ver = wd_hybridPositioningVersion(); diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index 16bbb37..d922034 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -28,7 +28,8 @@ //version 1.4.3 : 转子钢芯， (1)分层系数和边距系数添加-1.0参数值，表示不进行分层或边距。（2）添加选项选择使用工件的自身法向或统一使用调平地面法向 //version 1.4.4 : 转子钢芯，添加了异常保护 //version 1.4.5 : 转子钢芯，隔板检测内部参数修改：2:1抽样，聚类距离门限5mm -std::string m_strVersion = "HybridPositioning 1.4.5"; +//version 1.4.6 : 转子钢芯，添加隔板下工作部分可见时虚假检测的滤除，非中央部分工作按高度排序 +std::string m_strVersion = "HybridPositioning 1.4.6"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -122,6 +123,11 @@ bool _compareByZValue(SVzNL3DPosition& a, SVzNL3DPosition& b) return a.pt3D.z < b.pt3D.z; } +bool _compareByWorkpieceZValue(WD_workpieceInfo& a, WD_workpieceInfo& b) +{ + return a.center.z < b.center.z; +} + //生成圆周扫描数据 void _genPolarScanData( std::vector& points, @@ -1483,6 +1489,22 @@ void _searchImgPosition(std::vector< std::vector>& scanLinesInpu return; } +SVzNL3DPoint _searchNearestPoint(std::vector& pointList, const SVzNL3DPoint& seedPt) +{ + SVzNL3DPoint target = { 0, 0, 0 }; + double minDist = DBL_MAX; + for (int i = 0; i < (int)pointList.size(); i++) + { + double dist = abs(seedPt.x - pointList[i].pt3D.x) + abs(seedPt.y - pointList[i].pt3D.y); + if (minDist > dist) + { + target = pointList[i].pt3D; + minDist = dist; + } + } + return target; +} + void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, @@ -1855,7 +1877,20 @@ void wd_HRM_RotorCorePositioning( centerPtIndice); a_workpiece.imgCenter = { nCx, nCy }; a_workpiece.id = idx + 1; - + //添加对隔板下部分可见被识别目标的过滤 + if ((spacerInfo.center.z > 1e-4)&& (a_workpiece.center.z > 1e-4)) //隔板存在 + { + //搜索与工件中心最接近的点 + SVzNL3DPoint nearestPt = _searchNearestPoint(spacerPoints, a_workpiece.center); + double dist = sqrt(pow(nearestPt.x - a_workpiece.center.x, 2) + pow(nearestPt.y - a_workpiece.center.y, 2)); + if (dist < workpieceParam.radius * 0.3) + { + double heightDiff = nearestPt.z - a_workpiece.center.z; + if (heightDiff < workpieceParam.height / 4) + a_workpiece.center = { 0.0, 0.0, 0.0 }; + } + } + #ifdef _OUTPUT_DEBUG_DATA if (a_workpiece.center.z > 1e-4) { @@ -2182,22 +2217,30 @@ void wd_HRM_RotorCorePositioning( workpiecePositions.push_back(centerRegion[idx]); } } + //对非中央部分按高度排序 + std::vector< WD_workpieceInfo> sideObjects; if (topSide.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), topSide.begin(), topSide.end()); + sideObjects.insert(sideObjects.end(), topSide.begin(), topSide.end()); if (btmSide.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), btmSide.begin(), btmSide.end()); + sideObjects.insert(sideObjects.end(), btmSide.begin(), btmSide.end()); if (leftSide.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), leftSide.begin(), leftSide.end()); + sideObjects.insert(sideObjects.end(), leftSide.begin(), leftSide.end()); if (rightSide.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), rightSide.begin(), rightSide.end()); + sideObjects.insert(sideObjects.end(), rightSide.begin(), rightSide.end()); if (LTCorner.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), LTCorner.begin(), LTCorner.end()); + sideObjects.insert(sideObjects.end(), LTCorner.begin(), LTCorner.end()); if (RTCorner.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), RTCorner.begin(), RTCorner.end()); + sideObjects.insert(sideObjects.end(), RTCorner.begin(), RTCorner.end()); if (LBCorner.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), LBCorner.begin(), LBCorner.end()); + sideObjects.insert(sideObjects.end(), LBCorner.begin(), LBCorner.end()); if (RBCorner.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), RBCorner.begin(), RBCorner.end()); + sideObjects.insert(sideObjects.end(), RBCorner.begin(), RBCorner.end()); + if (sideObjects.size() > 1) + { + //按高度排序 + std::sort(sideObjects.begin(), sideObjects.end(), _compareByWorkpieceZValue); + } + workpiecePositions.insert(workpiecePositions.end(), sideObjects.begin(), sideObjects.end()); } else {