From 3f2d18e871dc5a8fabe5942eaf3537b00e1fe173 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 21 Oct 2026 23:58:46 +0800 Subject: [PATCH] =?UTF-8?q?hybridPosePositioning=20version=201.5.1=20:=20?= =?UTF-8?q?=E8=BD=AC=E5=AD=90=E9=92=A2=E8=8A=AF=EF=BC=8C2D=E7=BB=93?= =?UTF-8?q?=E6=9E=9C=E7=94=B1ROI=E8=BE=93=E5=85=A5=E6=8D=A2=E6=88=90?= =?UTF-8?q?=E6=8E=A9=E7=A0=81=E8=BE=93=E5=85=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hybridPosePositioning_test.cpp | 35 +++- .../lapWeldDetection_test.cpp | 169 ++++++++++++++---- sourceCode/SG_baseDataType.h | 15 ++ sourceCode/hybridPosePositioning.cpp | 130 ++++++++++---- sourceCode/hybridPosePositioning_Export.h | 3 +- 5 files changed, 281 insertions(+), 71 deletions(-) diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index 214b149..cda3667 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -2490,7 +2490,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) const char* ver = wd_hybridPositioningVersion(); printf("ver:%s\n", ver); - for (int grp = 8; grp <= 8; grp++) + for (int grp = 4; grp <= 8; grp++) { SSG_planeCalibPara groundCalibPara; //初始化成单位阵 @@ -2557,13 +2557,42 @@ void HaiRuiMa_rotorCorePositioning_test(void) binInfo = _readBinInfo(calibFile); } #endif + +#if 0 if( (grp ==3) || (grp == 4) ||(grp == 8)) sprintf_s(_scan_file, "%s%d_origin_result.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%s%d_result.txt", dataPath[grp], fidx); std::vector objROIs; vzReadObj2DROI(_scan_file, objROIs); - +#else //使用掩膜 + sprintf_s(_scan_file, "%s%d_mask.bmp", dataPath[grp], fidx); + // + // 读取 BMP 文件（彩色，默认） + cv::Mat img = cv::imread(_scan_file, cv::IMREAD_COLOR); + // 检查是否读取成功 + if (img.empty()) { + std::cerr << "读取失败：检查路径或文件是否损坏" << std::endl; + continue; + } + std::vector> objDataMask; //和扫描线存储结构相同：按列存储 + const int rows = img.rows; // 高（y 方向） + const int cols = img.cols; // 宽（x 方向） + // 2. 创建按列存储的二维容器：data[x][y] + // 外层 = 列号 x，内层 = 行号 y + objDataMask.resize(cols); + for (int i = 0; i < cols; i++) + objDataMask[i].resize(rows); + // 3. 逐像素转换（OpenCV 默认是 BGR 顺序，需交换成 R、G、B） + for (int y = 0; y < rows; ++y) { + const cv::Vec3b* ptr = img.ptr(y); // 第 y 行首地址 + for (int x = 0; x < cols; ++x) { + objDataMask[x][y].B = ptr[x][0]; + objDataMask[x][y].G = ptr[x][1]; + objDataMask[x][y].R = ptr[x][2]; + } + } +#endif SWD_Cylinder workpieceParam; //圆柱形工件的标称半径和高度 workpieceParam.radius = 18.0; //最小半径 workpieceParam.height = 20.0; //最小高度 @@ -2611,7 +2640,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) std::vector> debugScanLines; wd_HRM_RotorCorePositioning( scanLines, - objROIs, + objDataMask, groundCalibPara, spacerParam, workpieceParam, diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp index 7b23324..b2c470c 100644 --- a/lapWeldDetection_test/lapWeldDetection_test.cpp +++ b/lapWeldDetection_test/lapWeldDetection_test.cpp @@ -27,6 +27,7 @@ void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzN if (inputFile.is_open() == false) return; + std::getline(inputFile, linedata); //skip first line while (std::getline(inputFile, linedata)) { if (linedata.empty()) @@ -34,19 +35,14 @@ void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzN double X, Y, Z; sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &X, &Y, &Z); - SVzNL3DPoint a_pt; - if (true == exchangeXY) - { - a_pt.x = Y; //将扫描线调整为Y方向；扫描方向为X方向 - a_pt.y = X; - } - else + if (Z > 1e-4) { + SVzNL3DPoint a_pt; a_pt.x = X; a_pt.y = Y; + a_pt.z = Z; + scanData.push_back(a_pt); } - a_pt.z = Z; - scanData.push_back(a_pt); } return; } @@ -757,48 +753,148 @@ void _outputWeldInfo(char* outFileName, std::vector& weldRawPoints sw.close(); } +//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理 +void wd_getScanLines( + std::vector& scanData, + std::vector>& scanLines) +{ + std::vector a_line; + for (int i = 0, i_max = scanData.size(); i < i_max; i++) + { + SVzNL3DPoint a_ptXYZ = scanData[i]; + SVzNLPointXYZRGBA a_pt; + a_pt.nRGB = 0; + a_pt.x = a_ptXYZ.x; + a_pt.y = a_ptXYZ.y; + a_pt.z = a_ptXYZ.z; + + if (a_line.size() == 0) + a_line.push_back(a_pt); + else + { + SVzNLPointXYZRGBA pre_pt = a_line.back(); + double y_diff = a_pt.y - pre_pt.y; + if (y_diff < -100.0) + { + scanLines.push_back(a_line); + a_line.clear(); + a_line.push_back(a_pt); + } + else + a_line.push_back(a_pt); + } + } + //last line + if (a_line.size() > 0) + scanLines.push_back(a_line); + return; +} + +void _convertToGridData_XYZRGB(std::vector>& scanData, double _F, std::vector>& gridData) +{ + int lineNum = (int)scanData.size(); + int min_y = 100000000; + int max_y = -10000000; + int validStartLine = -1; + int validEndLine = -1; + for (int line = 0; line < lineNum; line++) + { + if (scanData[line].size() > 0) + { + if (validStartLine < 0) + { + validStartLine = line; + validEndLine = line; + } + else + validEndLine = line; + } + + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + double v = _F * a_pt.y / a_pt.z + 2000; + a_pt.nRGB = (int)(v + 0.5); + max_y = max_y < (int)a_pt.nRGB ? (int)a_pt.nRGB : max_y; + min_y = min_y > (int)a_pt.nRGB ? (int)a_pt.nRGB : min_y; + } + } + } + if (min_y == 100000000) + return; + + int vldLineNum = validEndLine - validStartLine + 1; + gridData.resize(vldLineNum); + int pt_counter = max_y - min_y + 1; + for (int i = 0; i < vldLineNum; i++) + { + gridData[i].resize(pt_counter); + for (int j = 0; j < pt_counter; j++) + { + gridData[i][j].nPointIdx = j; + gridData[i][j].pt3D.x = 0; + gridData[i][j].pt3D.y = 0; + gridData[i][j].pt3D.z = 0; + } + } + for (int line = validStartLine; line <= validEndLine; line++) + { + int gridLine = line - validStartLine; + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + int pt_id = a_pt.nRGB - min_y; + gridData[gridLine][pt_id].pt3D.x = a_pt.x; + gridData[gridLine][pt_id].pt3D.y = a_pt.y; + gridData[gridLine][pt_id].pt3D.z = a_pt.z; + } + } + } + return; +} + #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_POSITION 1 -#define TEST_GROUP 2 +#define TEST_GROUP 3 void lapWeldTest() { const char* dataPath[TEST_GROUP] = { "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\", //0 "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\模拟数据\\", + "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/博清实验室模拟数据/", //0 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,6}, {1,1} + {1,6}, {1,1}, {3, 3} }; #if CONVERT_TO_GRID - int convertGrp = 0; + int convertGrp = 2; for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++) { char _scan_file[256]; - sprintf_s(_scan_file, "%sglove_%d.txt", dataPath[convertGrp], fidx); + sprintf_s(_scan_file, "%s%d_LaserData_Jh26B083.txt", dataPath[convertGrp], fidx); std::vector< SVzNL3DPoint> scanData; bool exchangeXY = true; vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, exchangeXY); //将数据恢复为按扫描线存储格式 - std::vector< std::vector> scanLines; + std::vector< std::vector> scanLines; wd_getScanLines(scanData, scanLines); - - double y_step; - if ((fidx == 4) || (fidx == 5)) - y_step = 0.106; - else - y_step = 0.091; - std::vector< std::vector> grid_scanLines; - _convertToGridData(scanLines, grid_scanLines, y_step); - sprintf_s(_scan_file, "%sglove_%d_scanLine.txt", dataPath[convertGrp], fidx); + double _F = 1181.04542; //f + std::vector> gridData; + _convertToGridData_XYZRGB(scanLines, _F, gridData); + sprintf_s(_scan_file, "%s%d_scanLine.txt", dataPath[convertGrp], fidx); int headNullLines = 0; - _outputScanDataFile_vector(_scan_file, grid_scanLines, false, &headNullLines); + _outputScanDataFile_vector(_scan_file, gridData, false, &headNullLines); printf("%s: head null lines = %d\n", _scan_file, headNullLines); #if 1 sprintf_s(_scan_file, "%sglove_%d_scanLine_h.txt", dataPath[convertGrp], fidx); - _outputScanDataFile_vector_h(_scan_file, grid_scanLines); + _outputScanDataFile_vector_h(_scan_file, gridData); #endif } #endif @@ -929,16 +1025,17 @@ void lapWeldTest() #define TEST_BQ_CALIB_PARA 0 #define TEST_BQ_WELD_SEAM 1 -#define BQ_TEST_GROUP 2 +#define BQ_TEST_GROUP 3 void BQ_thinWeldSeamTest() { const char* dataPath[BQ_TEST_GROUP] = { "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/", //0 - "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/博清实验室模拟数据/", //0 + "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/博清实验室模拟数据/", //1 + "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/博清实验室模拟数据2/", //2 }; SVzNLRange fileIdx[BQ_TEST_GROUP] = { - {1,4},{1,2} + {1,4},{3,3}, {1,3} }; @@ -1017,7 +1114,7 @@ void BQ_thinWeldSeamTest() #if TEST_BQ_WELD_SEAM //for (int group = 0; group < TEST_GROUPS; group++) - for (int grp = 1; grp < BQ_TEST_GROUP; grp++) + for (int grp = 2; grp < BQ_TEST_GROUP; grp++) { SSG_planeCalibPara groundCalibPara; //初始化成单位阵 @@ -1040,19 +1137,17 @@ void BQ_thinWeldSeamTest() groundCalibPara = _readCalibPara(calibFile); } - double para_chkWidth_wide = 2.5f; - SSG_cornerParam cornerPara; SVzNLRangeD gapParam; SWD_valleyParam valleyParam; - valleyParam.chkWin_width = 5.0; // 2.5; //mm, 检测2.5mm宽的V型 + valleyParam.chkWin_width = 2.5; // .0; // 2.5; //mm, 检测2.5mm宽的V型 valleyParam.maxWidth = 20.0; //最大宽度20mm，用于过滤虚假信号 - valleyParam.minHeight = 0.5; + valleyParam.minHeight = 0.3; SSG_treeGrowParam growParam; growParam.maxLineSkipNum = 50; - growParam.yDeviation_max = 1.5; + growParam.yDeviation_max = 1.0; growParam.maxSkipDistance = 30.0; growParam.zDeviation_max = 5.0;// growParam.minLTypeTreeLen = 50; //mm, @@ -1070,8 +1165,10 @@ void BQ_thinWeldSeamTest() char _scan_file[256]; if(0 == grp) sprintf_s(_scan_file, "%s%d_LaserData_Jl26C077.txt", dataPath[grp], fidx); - else + else if(1 == grp) sprintf_s(_scan_file, "%s%d_LaserData_Jh26B083.txt", dataPath[grp], fidx); + else if(2 == grp) + sprintf_s(_scan_file, "%s%d_LaserData_Jh26B489.txt", dataPath[grp], fidx); printf("process: %s......\n", _scan_file); std::vector> scanLines; @@ -1117,7 +1214,7 @@ void BQ_thinWeldSeamTest() growParam); } - int weldRefPoints = 3; + int weldRefPoints = 6; int errCode = 0; std::vector weldOps; std::vector weldRawPoints; diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 251eb43..2ffe322 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -55,6 +55,13 @@ typedef struct double z; }SWD3DPoint; +typedef struct +{ + int R; + int G; + int B; +}SWDColorRGB; + typedef struct { double cx, cy, r; @@ -151,6 +158,14 @@ typedef struct int ptIdx; }SSG_featureClusteringInfo; +typedef struct +{ + int left; + int right; + int top; + int bottom; +}SSG_ROIRect; + typedef struct { double left; diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index b699879..e87c31e 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -33,7 +33,8 @@ //version 1.4.8 : תӸо¹ֿɼʱټ˳ʱľϵ0.3Ϊ0.75ʱ0.3ᵼ޷ҵϵıȽϵ //version 1.4.9 : תӸо¹ֿɼʱټ˳ʱľϵ0.3Ϊ2.0ֱʱ0.3ᵼ޷ҵϵıȽϵ //version 1.5.0 : תӸоʹ2DʶROIȷĿĴСҪ빤뾶ֶ֧Ӽ -std::string m_strVersion = "HybridPositioning 1.5.0"; +//version 1.5.1 : תӸо2DROI뻻 +std::string m_strVersion = "HybridPositioning 1.5.1"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -1568,9 +1569,33 @@ void _searchRCirclePoints( return; } +void _updateROI(int col, int row, SSG_ROIRect& a_roi) +{ + if (a_roi.right == 0) + { + a_roi.left = col; + a_roi.right = col; + a_roi.top = row; + a_roi.bottom = row; + } + else + { + if (a_roi.left > col) + a_roi.left = col; + if (a_roi.right < col) + a_roi.right = col; + if (a_roi.top > row) + a_roi.top = row; + if (a_roi.bottom < row) + a_roi.bottom = row; + } + return; +} + void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, - std::vector& objROIs, + std::vector>& objDataMask, //2DĿĤ + //std::vector& objROIs, const SSG_planeCalibPara groundCalibPara, const SVzNLSizeD spacerParam, //Ϳ const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ @@ -1606,19 +1631,10 @@ void wd_HRM_RotorCorePositioning( return; } - int maxU = 0; //2DͼCol - int maxV = 0; //2DͼRow - for (int line = 0; line < lineNum; line++) - { - for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++) - { - if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4) - continue; - - maxU = maxU < scanLinesInput[line][ptIdx].ptLeft2D.x ? scanLinesInput[line][ptIdx].ptLeft2D.x : maxU; - maxV = maxV < scanLinesInput[line][ptIdx].ptLeft2D.y ? scanLinesInput[line][ptIdx].ptLeft2D.y : maxV; - } - } + int Mask_U = (int)objDataMask.size(); + int Mask_V = (int)objDataMask[0].size(); + int maxU = Mask_U-1; //2DͼCol + int maxV = Mask_V-1; //2DͼRow //ͼ3dĶӦ const int imgCols = maxU+1; @@ -1826,24 +1842,75 @@ void wd_HRM_RotorCorePositioning( } } - int objNum = (int)objROIs.size(); + //ver_1.5.1ʹ2DĤд + std::vector objROIs; + int objNum = 0; + for (int m = 0; m < (int)objDataMask.size(); m++) + { + for (int n = 0; n < (int)objDataMask[m].size(); n++) + { + if (objNum < objDataMask[m][n].R) + objNum = objDataMask[m][n].R; + if (objNum < objDataMask[m][n].G) + objNum = objDataMask[m][n].G; + if (objNum < objDataMask[m][n].B) + objNum = objDataMask[m][n].B; + } + } + if (objNum > 0) + { + objROIs.resize(objNum+1); + for (int m = 0; m < (int)objDataMask.size(); m++) + { + for (int n = 0; n < (int)objDataMask[m].size(); n++) + { + SWDColorRGB a_mask = objDataMask[m][n]; + if (a_mask.R > 0) + { + SSG_ROIRect a_roi = objROIs[a_mask.R]; + _updateROI(m, n, a_roi); + objROIs[a_mask.R] = a_roi; + } + if (a_mask.G > 0) + { + SSG_ROIRect a_roi = objROIs[a_mask.G]; + _updateROI(m, n, a_roi); + objROIs[a_mask.G] = a_roi; + } + if (a_mask.B > 0) + { + SSG_ROIRect a_roi = objROIs[a_mask.B]; + _updateROI(m, n, a_roi); + objROIs[a_mask.B] = a_roi; + } + } + } + } + objNum = (int)objROIs.size(); std::vector allWorkpiece; - for (int idx = 0; idx < objNum; idx++) + for (int idx = 1; idx < objNum; idx++) { if (idx == 44) int kkk = 1; - WD_objArea2D& obj_roi = objROIs[idx]; - double cx = (obj_roi.roi.left + obj_roi.roi.right) / 2; - double cy = (obj_roi.roi.top + obj_roi.roi.bottom) / 2; - double roi_R = cx - obj_roi.roi.left; - double radius = roi_R * 1.414; + SSG_ROIRect& obj_roi = objROIs[idx]; + int ex_w = (obj_roi.right - obj_roi.left) / 6; + int ex_h = (obj_roi.bottom - obj_roi.top) / 6; + double roi_R = ex_w < ex_h ? (double)ex_w : (double)ex_h; + int L = obj_roi.left - ex_w; + if (L < 0) + L = 0; + int R = obj_roi.right + ex_w; + if (R > maxU) + R = maxU; + int T = obj_roi.top - ex_h; + if (T < 0) + T = 0; + int B = obj_roi.bottom + ex_h; + if (B > maxV) + B = maxV; - int L = (int)(cx - radius); - int R = (int)(cx + radius + 0.5); - int T = (int)(cy - radius); - int B = (int)(cy + radius + 0.5); - int nCx = (int)cx; - int nCy = (int)cy; + int nCx = (obj_roi.left + obj_roi.right) / 2; + int nCy = (obj_roi.top + obj_roi.bottom) / 2; //ͳROIеɨߺPtIdxΧ SVzNLRange roiLineIndice = { INT_MAX, 0 }; @@ -1919,11 +1986,12 @@ void wd_HRM_RotorCorePositioning( roiScanLines[roiLineIdx][roiPtIdx].nPointIdx = 0; roiScanLines[roiLineIdx][roiPtIdx].pt3D = scanLines[lineIdx][ptIdx].pt3D; - double dist = sqrt((x - nCx)* (x - nCx) + (y - nCy)* (y - nCy)); - if (dist <= (roi_R+2)) + SWDColorRGB a_mask = objDataMask[x][y]; + if( (a_mask.R == idx) || (a_mask.G == idx) || (a_mask.B == idx)) dataRoiMask[roiLineIdx][roiPtIdx] = 1; //ĵ + double dist = sqrt((x - nCx) * (x - nCx) + (y - nCy) * (y - nCy)); if (minDist < 0) { minDist = dist; @@ -1955,7 +2023,7 @@ void wd_HRM_RotorCorePositioning( outsideEdgePtIndice, centerPtIndice); a_workpiece.imgCenter = { nCx, nCy }; - a_workpiece.id = idx + 1; + a_workpiece.id = idx; //ӶԸ²ֿɼʶĿĹ if ((spacerInfo.center.z > 1e-4)&& (a_workpiece.center.z > 1e-4)) // { diff --git a/sourceCode/hybridPosePositioning_Export.h b/sourceCode/hybridPosePositioning_Export.h index 37e6883..26564a8 100644 --- a/sourceCode/hybridPosePositioning_Export.h +++ b/sourceCode/hybridPosePositioning_Export.h @@ -61,7 +61,8 @@ SG_APISHARED_EXPORT void wd_lineDataR( SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, - std::vector& objROIs, + std::vector>& objDataMask, //ɨߴ洢ṹͬд洢 + //std::vector& objROIs, const SSG_planeCalibPara groundCalibPara, const SVzNLSizeD spacerParam, //Ϳ const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶