From 8ac9065383e1715ca0f2da00d3fd4b3b91b38153 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 9 Feb 2026 00:51:35 +0800 Subject: [PATCH] =?UTF-8?q?bagThreadPositioning=20version=201.1.1=20:=20?= =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E6=A0=87=E5=AE=9A=E6=9F=B1=E6=A3=80?= =?UTF-8?q?=E5=87=BA=E3=80=82=E6=A0=87=E5=AE=9A=E6=9F=B1=E5=90=8C=E6=97=B6?= =?UTF-8?q?=E4=B8=BA=E6=89=AB=E6=8F=8F=E7=BA=BF=E7=AB=AF=E7=82=B9=E6=97=B6?= =?UTF-8?q?=E8=83=BD=E5=A4=9F=E6=A3=80=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../bagThreadPositioning_test.cpp | 241 +++++++++++++++--- sourceCode/SG_clustering.cpp | 4 +- sourceCode/SG_lineFeature.cpp | 109 ++++++-- sourceCode/bagThreadPositioning.cpp | 13 +- 4 files changed, 303 insertions(+), 64 deletions(-) diff --git a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp index 8cfafba..7c6e38a 100644 --- a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp +++ b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp @@ -455,7 +455,101 @@ void _outputRGBDScan_RGBD( sw.close(); } +void getROIdata(SSG_ROIRectD roi2D, + std::vector>& srcLines, + std::vector>& dstLines) +{ + int lineNum = (int)srcLines.size(); + dstLines.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition>& a_line = srcLines[line]; + int ptNum = (int)a_line.size(); + dstLines[line].resize(ptNum); + for (int j = 0; j < ptNum; j++) + { + dstLines[line][j] = a_line[j]; + if (a_line[j].pt3D.z > 1e-4) + { + if ((a_line[j].pt3D.x < roi2D.left) || (a_line[j].pt3D.x > roi2D.right) || + (a_line[j].pt3D.y < roi2D.top) || (a_line[j].pt3D.y > roi2D.bottom)) + dstLines[line][j].pt3D = { 0, 0, 0 }; + } + } + } + return; +} + +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //调平矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //地面高度 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //反向旋转矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //设置初始结果 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //调平矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //地面高度 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //反向旋转矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + + #define CONVERT_TO_GRID 0 +#define COMPUTE_CALIB_PARA 0 #define BAG_THREAD_POSITIONING 1 #define TEST_GROUP 3 int main() @@ -463,13 +557,13 @@ int main() const char* dataPath[TEST_GROUP] = { "F:/ShangGu/项目/吕宁项目/袋子拆线/模拟数据/", //0 "F:/ShangGu/项目/吕宁项目/袋子拆线/现场数据/", //1 - "F:/ShangGu/项目/吕宁项目/袋子拆线/BJDGS/", //2 + "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/", //2 }; SVzNLRange fileIdx[TEST_GROUP] = { {1,13}, {2,5}, - {8,9} + {1,10} }; const char* ver = wd_bagThreadPositioningVersion(); @@ -477,7 +571,7 @@ int main() #if CONVERT_TO_GRID int convertGrp = 2; - for (int fidx = 11; fidx <= 11; fidx++) + for (int fidx = 1; fidx <= 10; fidx++) { char _scan_file[256]; sprintf_s(_scan_file, "%s%dheightline_mm.txt", dataPath[convertGrp], fidx); @@ -490,23 +584,100 @@ int main() int gridFormat = (true == isGridData) ? 1 : 0; printf("%s: 栅格格式=%d\n", _scan_file, gridFormat); removeNullLines(scanData); - //std::vector> sampleData; - //downSampleGridData(scanData, 4, sampleData); + std::vector> sampleData; + downSampleGridData(scanData, 3, sampleData); //将数据恢复为按扫描线存储格式 sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx); - _outputScanDataFile(_scan_file, scanData,0, 0, 0); + _outputScanDataFile(_scan_file, sampleData,0, 0, 0); printf("%s: done.\n", _scan_file); } + + char _scan_file[256]; + sprintf_s(_scan_file, "%sgroundData.txt", dataPath[convertGrp]); + + std::vector> scanData; + double lineStep = 0.032; + double baseZ = 350.0; + vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData); + bool isGridData = checkGridFormat(scanData); + int gridFormat = (true == isGridData) ? 1 : 0; + printf("%s: 栅格格式=%d\n", _scan_file, gridFormat); + removeNullLines(scanData); + std::vector> sampleData; + downSampleGridData(scanData, 3, sampleData); + //将数据恢复为按扫描线存储格式 + sprintf_s(_scan_file, "%sgroundData_grid.txt", dataPath[convertGrp]); + _outputScanDataFile(_scan_file, sampleData, 0, 0, 0); + printf("%s: done.\n", _scan_file); + +#endif + +#if COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/groundData_grid.txt"); + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); + if (scanLines.size() > 0) + { + //getROIdata + std::vector> roiScanLines; + SSG_ROIRectD roi2D = {91.0, 190.0, -10, 37}; + + getROIdata(roi2D, scanLines, roiScanLines); + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/groundData_grid_ROI.txt"); + _outputScanDataFile(calibFile, roiScanLines, 0, 0, 0); + + SSG_planeCalibPara calibPara = wd_bagThread_getBaseCalibPara(roiScanLines); + //结果进行验证 + int lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_bagThread_lineDataR(scanLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight); + } + // + sprintf_s(calibFile, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/ground_grid_calib_verify.txt"); + _outputScanDataFile(_out_file, scanLines, 0, 0, 0); + printf("%s: calib done!\n", _calib_datafile); + } + #endif #if BAG_THREAD_POSITIONING - for (int grp = 1; grp <= 1; grp++) + for (int grp = 2; grp <= 2; grp++) { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { char _scan_file[256]; - sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx); + SSG_planeCalibPara poseCalibPara; + //初始化成单位阵 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = 2600.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + if (grp == 2) + { + sprintf_s(_scan_file, "%sground_calib_para.txt", dataPath[grp]); + poseCalibPara = _readCalibPara(_scan_file); + } + sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx); std::vector> scanLines; wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); @@ -528,7 +699,7 @@ int main() SSG_outlierFilterParam filterParam; filterParam.continuityTh = 3.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 - filterParam.outlierTh = 2; + filterParam.outlierTh = 3; SSG_treeGrowParam growParam; growParam.maxLineSkipNum = -1; //对于轮廓仪，线间隔很小，使用扫描线数的物理意义不清晰，使用maxSkipDistance @@ -539,28 +710,36 @@ int main() growParam.minVTypeTreeLen = 10; //mm SSX_ScanInfo scanInfo; - scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝 - scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。 - scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度 - scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置 - scanInfo.mark_diameter = 6.0; //mark外径 - scanInfo.mark_height = 3.5;//mark高 - scanInfo.mark_distance = 53.0; //两个Mark的距离 - - SSG_planeCalibPara poseCalibPara; - //初始化成单位阵 - poseCalibPara.planeCalib[0] = 1.0; - poseCalibPara.planeCalib[1] = 0.0; - poseCalibPara.planeCalib[2] = 0.0; - poseCalibPara.planeCalib[3] = 0.0; - poseCalibPara.planeCalib[4] = 1.0; - poseCalibPara.planeCalib[5] = 0.0; - poseCalibPara.planeCalib[6] = 0.0; - poseCalibPara.planeCalib[7] = 0.0; - poseCalibPara.planeCalib[8] = 1.0; - poseCalibPara.planeHeight = 2600.0; - for (int i = 0; i < 9; i++) - poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + if (grp == 1) + { + scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝 + scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。 + scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度 + scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置 + scanInfo.mark_diameter = 6.0; //mark外径 + scanInfo.mark_height = 3.5;//mark高 + scanInfo.mark_distance = 53.0; //两个Mark的距离 + } + else if(grp == 2 ) + { + scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝 + scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。 + scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度 + scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置 + scanInfo.mark_diameter = 18.0; //mark外径 + scanInfo.mark_height = 18.0;//mark高 + scanInfo.mark_distance = 39.0; //两个Mark的距离 + } + else + { + scanInfo.isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝 + scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。 + scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度 + scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置 + scanInfo.mark_diameter = 18.0; //mark外径 + scanInfo.mark_height = 16.5;//mark高 + scanInfo.mark_distance = 39.0; //两个Mark的距离 + } int errCode = 0; std::vector bagThreadInfo; diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index a5cd241..a9c5297 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -88,7 +88,7 @@ void wd_gridPointClustering( break; SVzNL2DPoint a_seedPos = a_cluster[i]; - if ((a_seedPos.x == 390) && (a_seedPos.y == 949)) + if ((a_seedPos.x == 752) && (a_seedPos.y == 2399)) int kkk = 1; SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y]; @@ -101,7 +101,7 @@ void wd_gridPointClustering( { int y = j + a_seedPos.y; int x = i + a_seedPos.x; - if ((x == 390) && (y == 949)) + if ((x == 645) && (y == 2240)) int kkk = 1; if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum)) diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 2dbb827..2790dd7 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -4583,42 +4583,101 @@ void wd_getSpecifiedHeightJumping( ) { int dataSize = (int)lineData.size(); - double pre_z = 0; - int preIdx = -1; + double height = (jumpHeight.min + jumpHeight.max) / 2; + //ļjumpHeight, ʹ߲ΪheigthʱĽǶҲС45,ʱֶζ˵㴦 + std::vector< SSG_RUN> segs; + int runIdx = 1; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_y = 0; for (int i = 0; i < dataSize; i++) { - if (i == 1850) - int kkk = 1; - lineData[i].nPointIdx = i; //± if (lineData[i].pt3D.z > 1e-4) { - if(preIdx >= 0) + if (a_run.len < 0) { - double z_diff = abs(lineData[i].pt3D.z - pre_z); - - if ( (z_diff >= jumpHeight.min) && (z_diff <= jumpHeight.max)) + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double y_diff = abs(lineData[i].pt3D.y - pre_y); + if (y_diff < height) { - SSG_basicFeature1D a_feature; - a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + a_run.value = runIdx; + segs.push_back(a_run); + runIdx++; - if (lineData[i].pt3D.z < pre_z) - { - a_feature.jumpPos = lineData[i].pt3D; - a_feature.jumpPos2D = { lineIdx, i }; - } - else - { - a_feature.jumpPos = lineData[preIdx].pt3D; - a_feature.jumpPos2D = { lineIdx, preIdx }; - } - - line_jumpFeatures.push_back(a_feature); + a_run.start = i; + a_run.len = 1; + a_run.value = i; } } - preIdx = i; - pre_z = lineData[i].pt3D.z; + pre_y = lineData[i].pt3D.y; } } + if (a_run.len > 0) + segs.push_back(a_run); + + int segNum = (int)segs.size(); + for (int si = 0; si < segNum; si++) + { + //˵ + int startIdx = segs[si].start; + int endIdx = segs[si].start + segs[si].len - 1; + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + a_feature.jumpPos = lineData[startIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, startIdx }; + line_jumpFeatures.push_back(a_feature); + + double pre_z = 0; + int preIdx = -1; + for (int i = startIdx; i <= endIdx; i++) + { + if (i == 1850) + int kkk = 1; + if (lineData[i].pt3D.z > 1e-4) + { + if (preIdx >= 0) + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + + if ((z_diff >= jumpHeight.min) && (z_diff <= jumpHeight.max)) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + + if (lineData[i].pt3D.z < pre_z) + { + a_feature.jumpPos = lineData[i].pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + } + else + { + a_feature.jumpPos = lineData[preIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, preIdx }; + } + + line_jumpFeatures.push_back(a_feature); + } + } + preIdx = i; + pre_z = lineData[i].pt3D.z; + } + } + + //Ķ˵ + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + a_feature.jumpPos = lineData[endIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, endIdx }; + line_jumpFeatures.push_back(a_feature); + } } //ʹö˵ֱߣ㵽ֱߵľ룬޵ķָ diff --git a/sourceCode/bagThreadPositioning.cpp b/sourceCode/bagThreadPositioning.cpp index c4453dc..052253c 100644 --- a/sourceCode/bagThreadPositioning.cpp +++ b/sourceCode/bagThreadPositioning.cpp @@ -7,7 +7,8 @@ //version 1.0.0 : base version release to customer //version 1.1.0 : ˱궨Mark⣬ڱ궨ƽ -std::string m_strVersion = "1.1.0"; +//version 1.1.1 : Ż˱궨궨ͬʱΪɨ߶˵ʱܹ +std::string m_strVersion = "1.1.1"; const char* wd_bagThreadPositioningVersion(void) { return m_strVersion.c_str(); @@ -234,7 +235,7 @@ void wd_bagThreadPositioning( } lineInterval = lineInterval / lineIntervalNum; - SVzNLRangeD jumpHeight = { scanInfo.mark_height-1.0, scanInfo.mark_height+1.0}; + SVzNLRangeD jumpHeight = { scanInfo.mark_height-2.0, scanInfo.mark_height+2.0}; double markRotateAngle = 0; SVzNL3DPoint markCenter = { 0,0,0 }; std::vector< SVzNL2DPoint> debug_markOrigin; @@ -252,7 +253,7 @@ void wd_bagThreadPositioning( //ֱɨVͲۺβ for (int line = 0; line < lineNum; line++) { - if ((line == 750) || (line == 905)) + if ((line == 755) || (line == 905)) int kkk = 1; std::vector& lineData = scanLines[line]; //˲˳쳣 @@ -316,7 +317,7 @@ void wd_bagThreadPositioning( std::vector> jumpFeatures_h; for (int line = 0; line < lineNum_h; line++) { - if ((line == 750) || (line == 905)) + if ((line == 2240) || (line == 905)) int kkk = 1; std::vector& lineData = data_lines_h[line]; @@ -423,8 +424,8 @@ void wd_bagThreadPositioning( { double w = markClustersInfo[i].roi3D.xRange.max - markClustersInfo[i].roi3D.xRange.min; double h = markClustersInfo[i].roi3D.yRange.max - markClustersInfo[i].roi3D.yRange.min; - if ((w >= scanInfo.mark_diameter - 0.5) && (w <= scanInfo.mark_diameter + 1.0) && - (h >= scanInfo.mark_diameter - 0.5) && (h <= scanInfo.mark_diameter + 1.0)) + if ((w >= scanInfo.mark_diameter - 1.0) && (w <= scanInfo.mark_diameter + 1.0) && + (h >= scanInfo.mark_diameter - 1.0) && (h <= scanInfo.mark_diameter + 1.0)) { validMarks.push_back(i); }