From 93c1cb72644fd9eeafb5a9b1cd99a2cd2bccefc3 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 27 Apr 2026 12:36:18 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20version=201.4.8=20:?= =?UTF-8?q?=20=E5=B0=86=E5=B7=A5=E4=BB=B6=E6=B3=95=E5=90=91=E8=B0=83?= =?UTF-8?q?=E6=95=B4=E4=B8=BA=E5=9E=82=E7=9B=B4=E4=BA=8E=E5=B7=A5=E4=BB=B6?= =?UTF-8?q?=E8=A1=A8=E9=9D=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/workpieceHolePositioning.cpp | 135 +++++++++++++++--- .../workpieceHolePositioning_test.cpp | 4 +- 2 files changed, 117 insertions(+), 22 deletions(-) diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index aaf41b0..c4e0422 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -18,7 +18,8 @@ //version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果 //version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。 //version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入 -std::string m_strVersion = "1.4.7"; +//version 1.4.8 : 将工件法向调整为垂直于工件表面 +std::string m_strVersion = "1.4.8"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -177,6 +178,46 @@ double _getMeanZ(std::vector>& quantiValue, SVzNL3DPoint see return (zSum / hist); } +void _getLinePositions(SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, SVzNLRect& roi2D, SVzNLRangeD& distRange, std::vector& pts) +{ + double len = sqrt(pow(seed2.x - seed1.x, 2) + pow(seed2.y - seed1.y, 2)); + int x0 = (int)seed1.x - roi2D.left; + int y0 = (int)seed1.y - roi2D.top; + + int x1 = (int)seed2.x - roi2D.left; + int y1 = (int)seed2.y - roi2D.top; + + std::vector pts_all; + drawLine( x0, y0, x1, y1, pts_all); + + for (int i = 0; i < (int)pts_all.size(); i++) + { + double data = double((pts_all[i].x - x0) * (pts_all[i].x - x0) + (pts_all[i].y - y0) * (pts_all[i].y - y0)); + double dist = sqrt(data); + double dist_k = dist / len; + if ((dist_k >= distRange.min) && (dist_k <= distRange.max)) + pts.push_back(pts_all[i]); + } +} + +void _getLinePoints( + std::vector>& quantiValue, + SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, + SVzNLRect& roi2D, SVzNLRangeD& distRange, + std::vector& Points3ds) +{ + std::vector pts_2d; + _getLinePositions(seed1, seed2, roi2D, distRange, pts_2d); + for (int i = 0; i < (int)pts_2d.size(); i++) + { + cv::Point3f a_pt3d; + a_pt3d.x = (double)(pts_2d[i].x + roi2D.left) + 0.5; + a_pt3d.y = (double)(pts_2d[i].y + roi2D.top) + 0.5; + a_pt3d.z = quantiValue[pts_2d[i].x][pts_2d[i].y]; + Points3ds.push_back(a_pt3d); + } +} + void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt) { if (a_pt.z > 1E-4) @@ -631,6 +672,17 @@ SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& listData) return roi; } +double _getMinZ(std::vector& Points3ds) +{ + double minZ = -1; + for (int i = 0; i < (int)Points3ds.size(); i++) + { + if ((minZ < 0) || (minZ > Points3ds[i].z)) + minZ = Points3ds[i].z; + } + return minZ; +} + //工件孔定位-拓普发工件孔定位 void wd_workpieceHolePositioning( std::vector< std::vector>& scanLinesInput, @@ -672,7 +724,7 @@ void wd_workpieceHolePositioning( } //生成量化数据，以1mm为量化尺度，用于确定工件表面高度 - SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines); + SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines); SVzNLRect roi2D; roi2D.left = (int)roi3D.xRange.min; roi2D.right = (int)roi3D.xRange.max; @@ -734,7 +786,7 @@ void wd_workpieceHolePositioning( { std::vector< SVzNL2DPoint> a_cluster; SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } }; - + int vTreeIdx = validObjects[i].data_0; int hTreeIdx = validObjects[i].data_1; for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++) @@ -816,7 +868,7 @@ void wd_workpieceHolePositioning( //圆拟合 SVzNL3DPoint center; double radius; - double err = fitCircleByLeastSquare(pointArray, center, radius); + double err = fitCircleByLeastSquare(pointArray, center, radius); center.z = minZ; SWD_HoleInfo a_hole; a_hole.center = { center.x, center.y, center.z }; @@ -826,8 +878,10 @@ void wd_workpieceHolePositioning( //分割 //方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点 double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效 + SVzNLRangeD linePointRange = { 0.3, 0.7 }; std::vector< WD_workpieceInfo> allWorkpiece; + double highest_z = -1; for (int objIdx = 0; objIdx < objectSize; objIdx++) { if (holes[objIdx].radius < 0) @@ -835,7 +889,7 @@ void wd_workpieceHolePositioning( holes[objIdx].radius = -1; SWD_HoleInfo& p0 = holes[objIdx]; - int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H/2); + int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2); if (idx1 < 0) continue; @@ -845,7 +899,7 @@ void wd_workpieceHolePositioning( int idx2 = angleConditionDistanceSearch( p0.center, p1.center, holes, - workpiecePara.holeDist_L, distDeviation, workpiecePara.H/2, + workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, angleRange); if (idx2 < 0) continue; @@ -870,7 +924,7 @@ void wd_workpieceHolePositioning( SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 }; SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 }; double rectR = 5.0; - double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H/2); + double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2); if (z1 < 1e-4) z1 = center_p0p1.z; double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2); @@ -882,11 +936,50 @@ void wd_workpieceHolePositioning( double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2); if (z4 < 1e-4) z4 = center_p2p3.z; - + p0.center.z = (z1 + z2) / 2; p1.center.z = (z1 + z3) / 2; p2.center.z = (z2 + z4) / 2; p3.center.z = (z3 + z4) / 2; + + SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4, + (p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4, + (z1 + z2 + z3 + z4) / 4 }; + //取工件表面的点 + std::vector Points3ds; + _getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds); + int size_1 = (int)Points3ds.size(); + _getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds); + _getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds); + _getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds); + + double minZ = _getMinZ(Points3ds); + if (minZ > 1e-4) + { + if ((highest_z < 0) || (highest_z > minZ)) + highest_z = minZ; + } + + //计算工件法向 + SVzNL3DPoint normalDir; + SVzNL3DPoint vec_norm; + if ((int)(Points3ds.size() > size_1) && (size_1 > 0)) + { + //计算面参数: z = Ax + By + C + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + std::vector res; + vzCaculateLaserPlane(Points3ds, res); + SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 }; + vec_norm = vec3_normalize(vec_1); + vec_norm = vec3_multiply(vec_norm, 20.0); + normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z }; + } + else + { + normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 }; + vec_norm = { 0, 0, 20 }; + } + WD_workpieceInfo a_workpiece; a_workpiece.workpieceType = workpiecePara.workpieceType; a_workpiece.holes.push_back(p0.center); @@ -897,12 +990,10 @@ void wd_workpieceHolePositioning( for (int m = 0; m < 4; m++) { SVzNL3DPoint a_pt = a_workpiece.holes[m]; - a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在z向 + a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//法向 a_workpiece.holesDir.push_back(a_pt); } - a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4, - (p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4, - (z1 + z2 + z3 + z4) / 4 }; + a_workpiece.center = centerPoint; SVzNL3DPoint y_dir; if (p0.center.x < p1.center.x) @@ -912,10 +1003,11 @@ void wd_workpieceHolePositioning( double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2)); y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 }; a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z }; - a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z + 20 }; + a_workpiece.z_dir = normalDir; allWorkpiece.push_back(a_workpiece); } + int workpieceNum = (int)allWorkpiece.size(); if (workpieceNum == 0) { @@ -924,16 +1016,18 @@ void wd_workpieceHolePositioning( } //寻找最高点 - std::vector< WD_workpieceInfo> zSortWorkpiece; - double minZ = allWorkpiece[0].center.z; - for (int i = 1; i < workpieceNum; i++) + if (highest_z < 0) { - if (minZ > allWorkpiece[i].center.z) - minZ = allWorkpiece[i].center.z; + highest_z = allWorkpiece[0].center.z; + for (int i = 1; i < workpieceNum; i++) + { + if (highest_z > allWorkpiece[i].center.z) + highest_z = allWorkpiece[i].center.z; + } } //检测上层是否有残留 - double zSliceTh = minZ - 1.5; //往上1.5mm + double zSliceTh = highest_z - 1.5; //往上1.5mm std::vector topLayerPts; for (int line = 0; line < lineNum; line++) { @@ -974,7 +1068,8 @@ void wd_workpieceHolePositioning( //排序 //z方向排序 - double topLayerTh = minZ + workpiecePara.H / 2; + std::vector< WD_workpieceInfo> zSortWorkpiece; + double topLayerTh = highest_z + workpiecePara.H / 2; for (int i = 0; i < workpieceNum; i++) { if (allWorkpiece[i].center.z < topLayerTh) diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index f654f83..a9a34ef 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}, {13,13} + {6,6}, {1, 16}, {1,17} }; const char* ver = wd_workpieceHolePositioningVersion(); @@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void) #endif #if TEST_COMPUTE_HOLE - for (int grp = 2; grp <= 2; grp++) + for (int grp = 1; grp <= 2; grp++) { SSG_planeCalibPara groundCalibPara; //初始化成单位阵