From b7196a12ea71e58ed57052b54b8ee30c691fdd84 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 26 Jan 2026 22:36:13 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20v1.0.1=20=E5=88=9D?= =?UTF-8?q?=E5=A7=8B=E5=8F=91=E5=B8=83=E7=89=88=E6=9C=AC=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/workpieceHolePositioning.cpp | 114 ++++++++++++++++-- sourceCode/workpieceHolePositioning_Export.h | 1 + .../workpieceHolePositioning_test.cpp | 53 +++++++- 3 files changed, 158 insertions(+), 10 deletions(-) diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index 50170a8..7953ed3 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -6,7 +6,7 @@ #include //version 1.0.0 : base version release to customer -std::string m_strVersion = "1.0.0"; +std::string m_strVersion = "1.0.1"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -117,6 +117,38 @@ int angleConditionDistanceSearch( return result; } +double _getMeanZ(std::vector>& quantiValue, SVzNL3DPoint seed, SVzNLRect& roi2D, double rectR) +{ + int cols = (int)quantiValue.size(); + int rows = (int)quantiValue[0].size(); + + int px = (int)seed.x - roi2D.left; + int py = (int)seed.y - roi2D.top; + int win = (int)rectR; + int hist = 0; + double zSum = 0; + for (int i = -win; i <= win; i++) + { + for (int j = -win; j <= win; j++) + { + int qx = px + i; + int qy = py + j; + if ((qx >= 0) && (qx < cols) && (qy >= 0) && (qy < rows)) + { + if (quantiValue[qx][qy] > 1e-4) + { + zSum += quantiValue[qx][qy]; + hist++; + } + } + } + } + if (hist == 0) + return 0; + else + return (zSum / hist); +} + //工件孔定位 void wd_workpieceHolePositioning( std::vector< std::vector>& scanLinesInput, @@ -155,8 +187,48 @@ void wd_workpieceHolePositioning( } //生成量化数据，以1mm为量化尺度，用于确定工件表面高度 - - + SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines); + SVzNLRect roi2D; + roi2D.left = (int)roi3D.xRange.min; + roi2D.right = (int)roi3D.xRange.max; + roi2D.top = (int)roi3D.yRange.min; + roi2D.bottom = (int)roi3D.yRange.max; + int quanti_X = roi2D.right - roi2D.left + 1; + int quanti_Y = roi2D.bottom - roi2D.top + 1; + std::vector> quantiValue; + std::vector> quantiHist; + quantiValue.resize(quanti_X); + quantiHist.resize(quanti_X); + for (int i = 0; i < quanti_X; i++) + { + quantiValue[i].resize(quanti_Y); + std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0 + quantiHist[i].resize(quanti_Y); + std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0 + } + //以1mm尺度量化 + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + SVzNL3DPoint& a_pt = scanLines[line][j].pt3D; + if (a_pt.z > 1e-4) + { + int qx = (int)a_pt.x - roi2D.left; + int qy = (int)a_pt.y - roi2D.top; + quantiValue[qx][qy] += a_pt.z; + quantiHist[qx][qy] += 1; + } + } + } + for (int ix = 0; ix < quanti_X; ix++) + { + for (int iy = 0; iy < quanti_Y; iy++) + { + if (quantiHist[ix][iy] > 0) + quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy]; + } + } std::vector> pointMask; pointMask.resize(lineNum); @@ -402,24 +474,46 @@ void wd_workpieceHolePositioning( p3.radius = -1; //重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值 - + SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 }; + SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 }; + 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); + double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR); + double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR); + double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR); + + p0.center.z = (z1 + z2) / 2; + p1.center.z = (z1 + z3) / 2; + p2.center.z = (z2 + z4) / 2; + p3.center.z = (z3 + z4) / 2; WD_workpieceInfo a_workpiece; a_workpiece.workpieceType = workpiecePara.workpieceType; a_workpiece.holes.push_back(p0.center); a_workpiece.holes.push_back(p1.center); a_workpiece.holes.push_back(p2.center); a_workpiece.holes.push_back(p3.center); + + for (int m = 0; m < 4; m++) + { + SVzNL3DPoint a_pt = a_workpiece.holes[m]; + a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在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, - (p0.center.z + p1.center.z + p2.center.z + p3.center.z) / 4 }; + (z1 + z2 + z3 + z4) / 4 }; SVzNL3DPoint y_dir; if (p0.center.y < p2.center.y) - y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, a_workpiece.center.z }; + y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, 0 }; else - y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, a_workpiece.center.z }; - a_workpiece.y_dir = { y_dir.x + a_workpiece.center.x, y_dir.y + a_workpiece.center.y, y_dir.z }; - a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z - 10 }; + y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, 0 }; + 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 }; workpiecePositioning.push_back(a_workpiece); } @@ -438,6 +532,8 @@ void wd_workpieceHolePositioning( { rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix); workpiecePositioning[i].holes[j] = rpt; + rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix); + workpiecePositioning[i].holesDir[j] = rpt; } } return; diff --git a/sourceCode/workpieceHolePositioning_Export.h b/sourceCode/workpieceHolePositioning_Export.h index 655da32..03fc106 100644 --- a/sourceCode/workpieceHolePositioning_Export.h +++ b/sourceCode/workpieceHolePositioning_Export.h @@ -17,6 +17,7 @@ typedef struct { int workpieceType; std::vector holes; + std::vector holesDir; SVzNL3DPoint center; SVzNL3DPoint z_dir; SVzNL3DPoint y_dir; diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 9fd67a5..7f795ca 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -276,6 +276,7 @@ void _outputRGBDResult_RGBD( a_objPt.pt3D = workpiecePositioning[i].holes[j]; objects.push_back(a_objPt); } + a_objPt.pt3D = workpiecePositioning[i].center; objects.push_back(a_objPt); } @@ -342,11 +343,11 @@ void _outputRGBDResult_RGBD( int linePtNum = (int)objects.size(); sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; lineNum++; + size = 10; for (int i = 0; i < linePtNum; i++) { int colorIdx = objects[i].nPointIdx % 8; rgb = objColor[colorIdx]; - size = 10; float x = (float)objects[i].pt3D.x; float y = (float)objects[i].pt3D.y; float z = (float)objects[i].pt3D.z; @@ -354,7 +355,57 @@ void _outputRGBDResult_RGBD( sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } + //淇鏄剧ず杞欢bug + rgb = objColor[1]; + float x = (float)objects[0].pt3D.x; + float y = (float)objects[0].pt3D.y; + float z = (float)objects[0].pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; //杈撳嚭鏂瑰悜绾挎潯 + rgb = { 255, 0, 0 }; + size = 2; + for (int i = 0; i < objNumber; i++) + { + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << workpiecePositioning[i].y_dir.x << "," << workpiecePositioning[i].y_dir.y << "," << workpiecePositioning[i].y_dir.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << workpiecePositioning[i].z_dir.x << "," << workpiecePositioning[i].z_dir.y << "," << workpiecePositioning[i].z_dir.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + for (int j = 0; j < (int)workpiecePositioning[i].holes.size(); j++) + { + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << workpiecePositioning[i].holes[j].x << "," << workpiecePositioning[i].holes[j].y << "," << workpiecePositioning[i].holes[j].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << workpiecePositioning[i].holesDir[j].x << "," << workpiecePositioning[i].holesDir[j].y << "," << workpiecePositioning[i].holesDir[j].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + } + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << workpiecePositioning[0].center.x << "," << workpiecePositioning[0].center.y << "," << workpiecePositioning[0].center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << workpiecePositioning[0].y_dir.x << "," << workpiecePositioning[0].y_dir.y << "," << workpiecePositioning[0].y_dir.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw.close(); }