From f4a25aae11c8c413acb653cfd1e0fd51d866698b Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 13 Aug 2026 16:22:03 +0800 Subject: [PATCH] =?UTF-8?q?SX=5FlapWeldDetection=20version=201.0.0=20:?= =?UTF-8?q?=E7=84=8A=E7=BC=9D=EF=BC=88=E5=B9=B3=E7=BC=9D=EF=BC=89=E8=AF=86?= =?UTF-8?q?=E5=88=AB=E6=A3=80=E6=B5=8B=E7=AE=97=E6=B3=95=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8F=91=E5=B8=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- baseAlgorithm/baseAlgorithm.vcxproj | 2 + .../lapWeldDetection_test.cpp | 330 ++++++++++++++++-- .../lapWeldDetection_test.vcxproj | 4 +- sourceCode/SG_baseAlgo_Export.h | 8 + sourceCode/SG_baseDataType.h | 8 + sourceCode/SG_errCode.h | 2 + sourceCode/SG_lineFeature.cpp | 306 ++++++++++++++++ sourceCode/SX_lapWeldDetection.cpp | 228 ++++++++++++ sourceCode/SX_lapWeldDetection_Export.h | 9 + sourceCode/dataFitting.cpp | 8 +- 10 files changed, 873 insertions(+), 32 deletions(-) diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index ec19a93..1df092a 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -164,11 +164,13 @@ + + /bigobj %(AdditionalOptions) /bigobj %(AdditionalOptions) diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp index 7f861a3..771fd2d 100644 --- a/lapWeldDetection_test/lapWeldDetection_test.cpp +++ b/lapWeldDetection_test/lapWeldDetection_test.cpp @@ -7,6 +7,7 @@ #include #include "direct.h" #include +#include "SG_baseAlgo_Export.h" #include "SX_lapWeldDetection_Export.h" #include #include @@ -206,7 +207,7 @@ void _outputRGBDScanLapWeld_RGBD( } else if (LINE_FEATURE_L_JUMP_L2H == featureType_v) { - rgb = objColor[7]; + rgb = { 0, 250, 0 }; size = 5; } else if (LINE_FEATURE_L_JUMP_H2L == featureType_h) @@ -271,26 +272,36 @@ void _outputRGBDScanLapWeld_RGBD( double exLen = 5.0; rgb = objColor[0]; size = 3; + int polyIndx = 0; for (int i = 0; i < weldNum; i++) { - SVzNL3DPoint pt0 = weldOPs[i][0]; - SVzNL3DPoint pt1 = weldOPs[i].back(); - double len = sqrt(pow(pt0.x - pt1.x, 2) + pow(pt0.y - pt1.y, 2)); - double k = exLen / len; - sw << "Poly_" << i << "_2" << std::endl; - double x = -k * (pt1.x - pt0.x) + pt0.x; - double y = -k * (pt1.y - pt0.y) + pt0.y; - double z = -k * (pt1.z - pt0.z) + pt0.z; - sw << "{" << (float)x << "," << (float)y << "," << (float)z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + for (int j = 0; j < (int)weldOPs[i].size()-1; j++) + { + SVzNL3DPoint pt0 = weldOPs[i][j]; + SVzNL3DPoint pt1 = weldOPs[i][j +1]; + double len = sqrt(pow(pt0.x - pt1.x, 2) + pow(pt0.y - pt1.y, 2)); + double k1 = exLen / len; + if (j > 0) + k1 = 0; + sw << "Poly_" << polyIndx << "_2" << std::endl; + polyIndx++; + double x = -k1 * (pt1.x - pt0.x) + pt0.x; + double y = -k1 * (pt1.y - pt0.y) + pt0.y; + double z = -k1 * (pt1.z - pt0.z) + pt0.z; + sw << "{" << (float)x << "," << (float)y << "," << (float)z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - x = -k * (pt0.x - pt1.x) + pt1.x; - y = -k * (pt0.y - pt1.y) + pt1.y; - z = -k * (pt0.z - pt1.z) + pt1.z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + double k2 = exLen / len; + if (j < (int)weldOPs[i].size() - 2) + k2 = 0; + x = -k2 * (pt0.x - pt1.x) + pt1.x; + y = -k2 * (pt0.y - pt1.y) + pt1.y; + z = -k2 * (pt0.z - pt1.z) + pt1.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + } } } sw.close(); @@ -696,11 +707,48 @@ void _convertToGridData(std::vector< std::vector>& scanLines, return; } +int counterLinePtNum_XYZ(std::vector< SVzNL3DPosition>& lineData) +{ + int num = 0; + for (int i = 0; i < (int)lineData.size(); i++) + { + if (lineData[i].pt3D.z > 1e-4) + num++; + } + return num; +} + +void _removeZeroLines_XYZ(std::vector>& scanData) +{ + int lineNum = (int)scanData.size(); + int firstLine = -1; + int lastLine = 0; + for (int line = 0; line < lineNum; line++) + { + int num = counterLinePtNum_XYZ(scanData[line]); + if ((num > 0) && (firstLine < 0)) + firstLine = line; + if (num > 0) + lastLine = line; + } + if (firstLine < 0) + { + scanData.clear(); + return; + } + + if ((lastLine < (lineNum - 1)) && (lastLine > 0)) + scanData.erase(scanData.begin() + lastLine + 1, scanData.end()); + + scanData.erase(scanData.begin(), scanData.begin() + firstLine); + return; +} + #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_POSITION 1 #define TEST_GROUP 2 -int main() +void lapWeldTest() { const char* dataPath[TEST_GROUP] = { "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\", //0 @@ -866,13 +914,239 @@ int main() #endif } -// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 -// 调试程序: F5 或调试 >“开始调试”菜单 +#define TEST_BQ_CALIB_PARA 0 +#define TEST_BQ_WELD_SEAM 1 +#define BQ_TEST_GROUP 1 +void BQ_thinWeldSeamTest() +{ + const char* dataPath[BQ_TEST_GROUP] = { + "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/", //0 + }; -// 入门使用技巧: -// 1. 使用解决方案资源管理器窗口添加/管理文件 -// 2. 使用团队资源管理器窗口连接到源代码管理 -// 3. 使用输出窗口查看生成输出和其他消息 -// 4. 使用错误列表窗口查看错误 -// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 -// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 + SVzNLRange fileIdx[BQ_TEST_GROUP] = { + {1,4}, + }; + + + const char* ver = wd_weldSeamAlgoVersion(); + printf("ver :%s\n", ver); + +#if TEST_BQ_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/1_LaserData_Jl26C077.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + _removeZeroLines_XYZ(scanData); + + //高度过滤，保留顶板 + double hTh = 490.0; + for (int line = 0; line < (int)scanData.size(); line++) + { + for (int j = 0; j < (int)scanData[line].size(); j++) + { + if (scanData[line][j].pt3D.z > hTh) + scanData[line][j].pt3D = { 0.0, 0.0, 0.0 }; + } + } + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = sx_getBaseCalibPara(scanData); + //结果进行验证 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + sx_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight); + } + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/scanData_ground_1_calib.txt"); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); + printf("%s: calib done!\n", _calib_datafile); +#if 1 + for (int grp = 0; grp < 1; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Jl26C077.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines_XYZ(scanLines); + if (scanLines.size() == 0) + continue; + lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + sx_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平，去除地面 + sprintf_s(_scan_file, "%s%d_ground_calibrated.txt", dataPath[grp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + printf("%s: calib done!\n", _scan_file); + } + } +#endif + + } +#endif + +#if TEST_BQ_WELD_SEAM + //for (int group = 0; group < TEST_GROUPS; group++) + for (int grp = 0; grp < BQ_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //初始化成单位阵 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + groundCalibPara = _readCalibPara(calibFile); + + double para_chkWidth_wide = 2.5f; + + SSG_cornerParam cornerPara; + SVzNLRangeD gapParam; + + SWD_valleyParam valleyParam; + valleyParam.chkWin_width = 2.5; //mm, 检测2.5mm宽的V型 + valleyParam.maxWidth = 20.0; //最大宽度20mm，用于过滤虚假信号 + valleyParam.minHeight = 1.5; + + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 50; + growParam.yDeviation_max = 1.0; + growParam.maxSkipDistance = 30.0; + growParam.zDeviation_max = 5.0;// + growParam.minLTypeTreeLen = 50; //mm, + growParam.minVTypeTreeLen = 50; //mm + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Jl26C077.txt", dataPath[grp], fidx); + printf("process: %s......\n", _scan_file); + + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines_XYZ(scanLines); + if (scanLines.size() == 0) + continue; + + //统计时间 + long t1 = (long)GetTickCount64();//统计时间 + + std::vector trees; + for (int i = 0; i < (int)scanLines.size(); i++) + { + std::vector& a_line = scanLines[i]; + std::vector< SSG_basicFeatureGap> line_valleys; + sx_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);//调平，去除地面 +#if 0 + sg_getLineGapFeature( + a_line, //扫描线 + i, //当前扫描线序号 + cornerPara, + gapParam, + line_valleys); +#else + wd_getLineVFlaws( + a_line, + i, + valleyParam, + line_valleys); + +#endif + bool isLastLine = i == ((int)scanLines.size() - 1) ? true : false; + sg_lineGapsGrowing( + i, + isLastLine, + line_valleys, + trees, + growParam); + } + + int weldRefPoints = 3; + int errCode = 0; + std::vector weldOps; + std::vector weldRawPoints; + BQ_getWeldSeamPose( + trees, + weldRefPoints, //焊缝中间参考点位置 + weldOps, //焊缝位置 + weldRawPoints, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + + //生成debug信息 + for (int line = 0; line < (int)scanLines.size(); line++) + { + for (int j = 0; j < (int)scanLines[line].size(); j++) + scanLines[line][j].nPointIdx = 0; + } + for (int i = 0; i < (int)trees.size(); i++) + { + for (int j = 0; j < (int)trees[i].treeNodes.size(); j++) + { + SSG_basicFeatureGap& a_node = trees[i].treeNodes[j]; + scanLines[a_node.lineIdx][a_node.gapPeak.nPointIdx].nPointIdx = 2; + } + } + for (int i = 0; i < (int)weldRawPoints.size(); i++) + { + int lineIdx = weldRawPoints[i].nPointIdx >> 16; + int ptIdx = weldRawPoints[i].nPointIdx & 0xffff; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + } + + char outFileName[256]; + //输出测试结果 + sprintf_s(outFileName, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + std::vector< std::vector> multiSeam; + multiSeam.push_back(weldOps); + _outputRGBDScanLapWeld_RGBD(outFileName, scanLines, multiSeam, true); + } + } +#endif +} + +typedef enum +{ + keSG_焊缝定位_搭接焊缝 = 0, + keSG_焊缝定位_长直细焊缝, +}ESG_testMode; + +int main() +{ + //ESG_testMode testMode = keSG_焊缝定位_搭接焊缝; + ESG_testMode testMode = keSG_焊缝定位_长直细焊缝; + + if (keSG_焊缝定位_搭接焊缝 == testMode) + lapWeldTest(); + else if (keSG_焊缝定位_长直细焊缝 == testMode) + BQ_thinWeldSeamTest(); + + return 0; +} diff --git a/lapWeldDetection_test/lapWeldDetection_test.vcxproj b/lapWeldDetection_test/lapWeldDetection_test.vcxproj index 0665d83..16e1c3e 100644 --- a/lapWeldDetection_test/lapWeldDetection_test.vcxproj +++ b/lapWeldDetection_test/lapWeldDetection_test.vcxproj @@ -126,7 +126,7 @@ Console true - opencv_world320d.lib;lapWeldDetection.lib;%(AdditionalDependencies) + opencv_world320d.lib;baseAlgorithm.lib;lapWeldDetection.lib;%(AdditionalDependencies) ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) @@ -146,7 +146,7 @@ true true true - opencv_world320.lib;lapWeldDetection.lib;%(AdditionalDependencies) + opencv_world320.lib;baseAlgorithm.lib;lapWeldDetection.lib;%(AdditionalDependencies) ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 7eb5109..5b5f255 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -1117,3 +1117,11 @@ SG_APISHARED_EXPORT void WD_getHoleInfo( std::vector& segTrees_h, std::vector& validObjects ); + +/* ֧դݣҪդݣ */ +//ȡɨϵVValley +SG_APISHARED_EXPORT void wd_getLineVFlaws( + std::vector& a_line, + const int lineIdx, + const SWD_valleyParam valleyParam, + std::vector< SSG_basicFeatureGap>& valleys); diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 0c3155d..a2c6028 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -214,6 +214,7 @@ typedef struct double gap_depth; SVzNL3DPosition gapPt_0; //gapĶ˵ SVzNL3DPosition gapPt_1; //gapĶ˵ + SVzNL3DPosition gapPeak; SSG_ROIRectD roi; }SSG_basicFeatureGap; @@ -250,6 +251,13 @@ typedef struct double outlierTh; }SSG_outlierFilterParam; +typedef struct +{ + double chkWin_width; + double maxWidth; + double minHeight; +}SWD_valleyParam; + typedef struct { double LSlopeZWin; //LSlope¶ȼĴڳ diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 25af7df..082325b 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -13,6 +13,7 @@ #define SX_ERR_ZERO_OBJECTS -1010 #define SX_ERR_NULL_GROUND_PLANE -1011 #define SX_ERR_ZERO_2D_OBJECTS -1012 +#define SX_ERR_FITTING_POINTS_LESS -1013 //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 @@ -30,6 +31,7 @@ #define SX_ERR_ZERO_OBJ_BTMLAYER -2103 #define SX_ERR_GET_INVALID_PALTE -2104 //ȡ #define SX_ERR_BIN_EDGE_ERROR -2105 +#define SX_ERR_BIN_SLICE_ZEROPOINTS -2106 // #define SX_BAG_TRAY_EMPTY -2201 diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 64ee904..a377d51 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -1,5 +1,7 @@ #include "SG_baseDataType.h" #include "SG_baseAlgo_Export.h" +#include "WD_convolveGauss.h" + #include const double EPS = 1e-8; @@ -9526,3 +9528,307 @@ if (i == 98) } return; } + +enum STATES { null = 0, UP, DOWN }; +struct posAttri +{ + int peakPos; + STATES peakState; +}; +//peak +void _peakDetect(std::vector& data, int dataSize, std::vector& peakPosition) +{ + if (dataSize == 0) + return; + + static float algoPara_threshold_unchange = 0.001; + + //Peak + float preData = data[0]; + + STATES a_state = null; + int peakPos = 0; + + for (int i = 1; i < dataSize; i++) + { + if (i == 115) + int kkk = 1; + + float currData = data[i]; + float delta = fabsf(currData - preData); + switch (a_state) + { + case null: + if (delta > algoPara_threshold_unchange) + { + if (currData > preData) + a_state = UP; + else + a_state = DOWN; + } + peakPos = i; + preData = currData; + break; + case UP: + if (currData > preData) //¼ֵ + { + peakPos = i; + preData = currData; + } + if ((delta > algoPara_threshold_unchange) && (currData < preData))//յ + { + //漫ֵλ + posAttri a_pos; + a_pos.peakPos = peakPos; + a_pos.peakState = a_state; + peakPosition.push_back(a_pos); + a_state = DOWN; + + //µijֵ + peakPos = i; + preData = currData; + } + break; + case DOWN: + if (currData < preData) //¼Сֵ + { + peakPos = i; + preData = currData; + } + + if ((delta > algoPara_threshold_unchange) && (currData > preData)) //յ + { + //漫ֵλ + posAttri a_pos; + a_pos.peakPos = peakPos; + a_pos.peakState = a_state; + peakPosition.push_back(a_pos); + + a_state = UP; + //µijֵ + peakPos = i; + preData = currData; + } + break; + } + } +} +//鰼 +//ʹխ +//ݣ׵Ϊֵڼֵһһһ׵ֵ +float _computeHeight(SVzNL3DPoint& startPt, SVzNL3DPoint& endPt, SVzNL3DPoint& valleyPt) +{ + float _a = endPt.z - startPt.z; + float _b = -(endPt.y - startPt.y); + float _c = -_a * startPt.y - _b * startPt.z; + float height = fabs(_a * valleyPt.y + _b * valleyPt.z + _c) / sqrt(pow(_a, 2) + pow(_b, 2)); + return height; +} +/* startId, endId: ЧֹλáդݸʽʼĿյͽĿյ㶼Ч */ +void _valleyDetect( + std::vector& a_line, + const int lineIdx, const int startId, const int endId, + std::vector& deriv_1_peaks, + std::vector& deriv_2_peaks, + std::vector& deriv_1, + std::vector& deriv_2, + std::vector& valleys, + const SWD_valleyParam valleyParam) +{ + std::vector< posAttri> buff; + buff.resize(a_line.size()); + for (int i = 0; i < deriv_1_peaks.size(); i++) + buff[deriv_1_peaks[i].peakPos] = deriv_1_peaks[i]; + + double y_start = a_line[startId].pt3D.y; + double y_end = a_line[endId].pt3D.y; + + //peak⣺peak + for (int i = 0; i < deriv_2_peaks.size(); i++) + { + if (i == 153) + int kkk = 1; + posAttri a_pos = deriv_2_peaks[i]; + if ((a_pos.peakState == DOWN) && (deriv_2[a_pos.peakPos] < 0)) + { + // + int preEdge = -1; + for (int j = a_pos.peakPos - 1; j > 0; j--) + { + if (buff[j].peakState == DOWN) + break; + else if (buff[j].peakState == UP) + { + if (deriv_1[j] > 0) + preEdge = j; + break; + } + } + int postEdge = -1; + for (int j = a_pos.peakPos + 1; j < (int)a_line.size(); j++) + { + if (buff[j].peakState == UP) + break; + else if (buff[j].peakState == DOWN) + { + if (deriv_1[j] < 0) + postEdge = j; + break; + } + } + // + if ((preEdge >= 0) && (postEdge >= 0)) //ϰص + { + float edge_value_pre = deriv_1[preEdge]; + float edge_value_post = deriv_1[postEdge]; + //Գ + float min_value, max_value; + if (edge_value_pre < (-edge_value_post)) + { + min_value = edge_value_pre; + max_value = -edge_value_post; + } + else + { + max_value = edge_value_pre; + min_value = -edge_value_post; + } + if (min_value > max_value / 10) //2023.05.02: ſбʱmaxValue/2 + { + //鰼ݿȣ˳ + float th_pre = edge_value_pre * 0.5; + int edge_pos_pre = -1; + for (int j = preEdge; j >= 0; j--) + { + if ( (deriv_1[j] <= th_pre) && (a_line[j].nPointIdx >= 0)) // + { + edge_pos_pre = j; + break; + } + } + + float th_post = edge_value_post * 0.5; + int edge_pos_post = -1; + for (int j = postEdge; j < (int)a_line.size(); j++) + { + if ((deriv_1[j] >= th_post) && (a_line[j].nPointIdx >= 0)) // + { + edge_pos_post = j; + break; + } + } + + // + if ((edge_pos_pre >= 0) && (edge_pos_post >= 0)) + { + //float width = a_line->m_p3DPositiont[edge_pos_post].pt3D.y - a_line->m_p3DPositiont[edge_pos_pre].pt3D.y; + float width = a_line[postEdge].pt3D.y - a_line[preEdge].pt3D.y; + if (width < valleyParam.maxWidth) + { + float height = _computeHeight(a_line[edge_pos_pre].pt3D, + a_line[edge_pos_post].pt3D, + a_line[a_pos.peakPos].pt3D); + if (height > valleyParam.minHeight) + { + //ȷȷpeak + int realPeakPos = preEdge; + for (int m = preEdge; m <= postEdge; m++) + { + if ((a_line[realPeakPos].pt3D.z > a_line[m].pt3D.z) && (a_line[m].nPointIdx >= 0)) + realPeakPos = m; + } + + SSG_basicFeatureGap a_valley; + memset(&a_valley, 0, sizeof(SSG_basicFeatureGap)); + a_valley.gapPeak.nPointIdx = realPeakPos; + a_valley.gapPeak.pt3D = a_line[realPeakPos].pt3D; + a_valley.gapPt_0.nPointIdx = edge_pos_pre; + a_valley.gapPt_0.pt3D = a_line[edge_pos_pre].pt3D; + a_valley.gapPt_1.nPointIdx = edge_pos_post; + a_valley.gapPt_1.pt3D = a_line[edge_pos_post].pt3D; + a_valley.lineIdx = lineIdx; + a_valley.gap_start = a_line[preEdge].pt3D.y; + a_valley.gap_width = width; + a_valley.gap_depth = height; + valleys.push_back(a_valley); + } + } + } + } + + } + } + } +} +/* ֧դݣҪդݣ */ +//ȡɨϵVValley +void wd_getLineVFlaws( + std::vector& a_line, + const int lineIdx, + const SWD_valleyParam valleyParam, + std::vector< SSG_basicFeatureGap>& valleys) +{ + //դݣնͳЧ + int startId = -1, endId = 0; + for (int i = 0; i < (int)a_line.size(); i++) + { + a_line[i].nPointIdx = 0; + if (a_line[i].pt3D.z > 1e-4) + { + if (startId < 0) + startId = i; + endId = i; + } + else + a_line[i].nPointIdx = -1; //յ + } + int validLinePtNum = endId - startId + 1; + if ((startId < 0) || (validLinePtNum < 10)) // + return; + + //׶ + double preZ = a_line[startId].pt3D.z; + for (int i = startId + 1; i <= endId; i++) + { + + if (a_line[i].pt3D.z < 1e-4) + a_line[i].pt3D.z = preZ; + preZ = a_line[i].pt3D.z; + } + + // + float total_deltaY = a_line[endId].pt3D.y - a_line[startId].pt3D.y; + float scale = total_deltaY / validLinePtNum; +#if 0 + //scale = scale / 1.5; + float* deriv_1_wide = (float*)malloc(sizeof(float) * a_line->m_3DPointCount); + memset(deriv_1_wide, 0, sizeof(float) * a_line->m_3DPointCount); + float* deriv_2_wide = (float*)malloc(sizeof(float) * a_line->m_3DPointCount); + memset(deriv_2_wide, 0, sizeof(float) * a_line->m_3DPointCount); + //gaussһ׺Ͷ׵ + _convolveGauss_laserLine(a_line, startId, endId, deriv_1_wide, deriv_2_wide, chkWidth_wide, scale); +#endif + + //ڣխ + double chkWidth_narrow = valleyParam.chkWin_width / 2; + float narrow_scale = scale; + std::vector deriv_1_narrow; + std::vector deriv_2_narrow; + //gaussһ׺Ͷ׵ + wd_convolveGauss_laserLine(a_line, startId, endId, deriv_1_narrow, deriv_2_narrow, chkWidth_narrow, narrow_scale); + + // + //peak: ؼͰݼͬʱҪʹ + std::vector deriv_1_peaks; + _peakDetect(deriv_1_narrow, (int)a_line.size(), deriv_1_peaks); + std::vector deriv_2_peaks; + _peakDetect(deriv_2_narrow, (int)a_line.size(), deriv_2_peaks); + + //鰼 + //ʹխ + //ݣ׵Ϊֵڼֵһһһ׵ֵ + //float innerPara_max_valley_width = 20.0; //ڲݼȣڹٰ + _valleyDetect(a_line, lineIdx, startId, endId, deriv_1_peaks, deriv_2_peaks, deriv_1_narrow, deriv_2_narrow, valleys, valleyParam); + +} + diff --git a/sourceCode/SX_lapWeldDetection.cpp b/sourceCode/SX_lapWeldDetection.cpp index 86d7cfe..d0f2135 100644 --- a/sourceCode/SX_lapWeldDetection.cpp +++ b/sourceCode/SX_lapWeldDetection.cpp @@ -5,6 +5,12 @@ #include #include +//version 1.0.0 :죨ƽ죩ʶ㷨ʼ +std::string m_strVersion = "weldSeamAlgo_1.0.0"; +const char* wd_weldSeamAlgoVersion(void) +{ + return m_strVersion.c_str(); +} //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -88,6 +94,64 @@ void getWeldPoint(std::vector& a_weld_contour, int midPtNum, std:: a_weld.push_back(pt_1); } +SVzNLRange _getTreesLineRange(std::vector& trees) +{ + SVzNLRange a_rng = { 0, 0 }; + if (trees.size() == 0) + return a_rng; + + if (trees[0].sLineIdx < trees[0].eLineIdx) + { + a_rng.nMin = trees[0].sLineIdx; + a_rng.nMax = trees[0].eLineIdx; + } + else + { + a_rng.nMax = trees[0].sLineIdx; + a_rng.nMin = trees[0].eLineIdx; + } + + for (int i = 1; i < (int)trees.size(); i++) + { + a_rng.nMin = a_rng.nMin > trees[i].sLineIdx ? trees[i].sLineIdx : a_rng.nMin; + a_rng.nMin = a_rng.nMin > trees[i].eLineIdx ? trees[i].eLineIdx : a_rng.nMin; + a_rng.nMax = a_rng.nMax < trees[i].sLineIdx ? trees[i].sLineIdx : a_rng.nMax; + a_rng.nMax = a_rng.nMax < trees[i].eLineIdx ? trees[i].eLineIdx : a_rng.nMax; + } + return a_rng; +} + +SVzNLRangeD _getTreesXRange(std::vector& trees) +{ + SVzNLRangeD a_rng = { 0.0, 0.0 }; + if (trees.size() == 0) + return a_rng; + + SSG_basicFeatureGap nodeFirst = trees[0].treeNodes[0]; + SSG_basicFeatureGap nodeLast = trees[0].treeNodes.back(); + + if (nodeFirst.gapPeak.pt3D.x < nodeLast.gapPeak.pt3D.x) + { + a_rng.min = nodeFirst.gapPeak.pt3D.x; + a_rng.max = nodeLast.gapPeak.pt3D.x; + } + else + { + a_rng.max = nodeFirst.gapPeak.pt3D.x; + a_rng.min = nodeLast.gapPeak.pt3D.x; + } + for (int i = 1; i < (int)trees.size(); i++) + { + nodeFirst = trees[i].treeNodes[0]; + nodeLast = trees[i].treeNodes.back(); + a_rng.min = a_rng.min > nodeFirst.gapPeak.pt3D.x ? nodeFirst.gapPeak.pt3D.x : a_rng.min; + a_rng.min = a_rng.min > nodeLast.gapPeak.pt3D.x ? nodeLast.gapPeak.pt3D.x : a_rng.min; + a_rng.max = a_rng.max < nodeFirst.gapPeak.pt3D.x ? nodeFirst.gapPeak.pt3D.x : a_rng.max; + a_rng.max = a_rng.max < nodeLast.gapPeak.pt3D.x ? nodeLast.gapPeak.pt3D.x : a_rng.max; + } + return a_rng; +} + //ȡӺ void sx_getLapWeldPostion( std::vector< std::vector>& scanLines, @@ -355,4 +419,168 @@ void sx_getLapWeldPostion( } // +} + +double _getMeanZ(std::vector& ptZ) +{ + double meanZ = 0; + int num = (int)ptZ.size(); + for (int i = 0; i < num; i++) + meanZ += ptZ[i]; + + meanZ = meanZ / num; + return meanZ; +} + +bool _compareByXValue(const SVzNL3DPoint& a, const SVzNL3DPoint& b) { + return a.x < b.x; +} +//㺸λ +void BQ_getWeldSeamPose( + std::vector& trees, + int weldRefPoints, //мολ + std::vector& weldOps, //λ + std::vector& weldRawPoints, //ʾdebug + int* errCode) +{ + *errCode = 0; + + //tree + int bestId = 0; + trees[0].treeState = 0; + for (int i = 1; i < (int)trees.size(); i++) + { + trees[i].treeState = 0; + if (trees[bestId].treeNodes.size() < trees[i].treeNodes.size()) + bestId = i; + } + trees[bestId].treeState = 1; //Ѿ־ + + //treeΪкϲ + std::vector weldSeam; + weldSeam.push_back(trees[bestId]); + while (1) + { + int oldSize = (int)weldSeam.size(); + SVzNLRange lineRange = _getTreesLineRange(weldSeam); + //ֱ + + + for (int i = 0; i < (int)trees.size(); i++) + { + if (trees[i].treeState > 0) + continue; + + //ܷ񱻺ϲ + + + } + if (oldSize == (int)weldSeam.size()) + break; + } + //ʾ + for (int i = 0; i < (int)weldSeam.size(); i++) + { + for (int j = 0; j < (int)weldSeam[i].treeNodes.size(); j++) + { + SSG_basicFeatureGap& a_node = weldSeam[i].treeNodes[j]; + SVzNL3DPosition a_debugPt; + a_debugPt.nPointIdx = (a_node.lineIdx << 16) | a_node.gapPeak.nPointIdx; + a_debugPt.pt3D = a_node.gapPeak.pt3D; + weldRawPoints.push_back(a_debugPt); + } + } + + int nodeSize = 0; + for(int i = 0; i < (int)weldSeam.size(); i ++) + nodeSize += weldSeam[i].treeNodes.size(); + + if (nodeSize < 10) + { + *errCode = SX_ERR_FITTING_POINTS_LESS; + return; + } + + //ɽϢ + //еολü㷽ԲοXΪ׼Ҹȡγһ룬ֱϣXY + int refSegNum = weldRefPoints + 1; + refSegNum = refSegNum * 2; //Զγһ + std::vector> refSegments; + std::vector> refSegZ; + refSegments.resize(refSegNum); + refSegZ.resize(refSegNum); + SVzNLRangeD xRange = _getTreesXRange(weldSeam); + double segLen = (xRange.max - xRange.min) / refSegNum; + for (int i = 0; i < (int)weldSeam.size(); i++) + { + for (int j = 0; j < (int)weldSeam[i].treeNodes.size(); j++) + { + double x = weldSeam[i].treeNodes[j].gapPeak.pt3D.x; + double xoffset = x - xRange.min; + if (xoffset < 0) + xoffset = 0; + xoffset = xoffset / segLen; + int segIdx = (int)xoffset; + if (segIdx >= refSegNum) + segIdx = refSegNum - 1; + refSegments[segIdx].push_back(weldSeam[i].treeNodes[j].gapPeak.pt3D); + double valleyZ = (weldSeam[i].treeNodes[j].gapPt_0.pt3D.z + weldSeam[i].treeNodes[j].gapPt_1.pt3D.z) / 2; + refSegZ[segIdx].push_back(valleyZ); + } + } + //ֶΰXС + for (int i = 0; i < refSegNum; i++) + { + if (refSegments[i].size() > 0) + std::sort(refSegments[i].begin(), refSegments[i].end(), _compareByXValue); + } + //ֱֶ + for (int i = 0; i <= weldRefPoints+1; i++) + { + int segIdx_0 = i * 2 - 1; + int segIdx_1 = i * 2; + std::vector fittingPoints; + std::vector fittingPtZ; + if (segIdx_0 >= 0) + { + fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end()); // + fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_0].begin(), refSegZ[segIdx_0].end()); + } + if (segIdx_1 < refSegNum) + { + fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end()); //Ұ + fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_1].begin(), refSegZ[segIdx_1].end()); + } + while (fittingPoints.size() < 10) //֤㹻ϵ + { + //չ + int oldSize = (int)fittingPoints.size(); + segIdx_0--; + if (segIdx_0 >= 0) + { + fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end()); + fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_0].begin(), refSegZ[segIdx_0].end()); + } + segIdx_1++; + if (segIdx_1 < refSegNum) + { + fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end()); + fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_1].begin(), refSegZ[segIdx_1].end()); + } + if (oldSize == (int)fittingPoints.size()) + break; + } + //ֱ + double _a, _b, _c; + lineFitting_abc(fittingPoints, &_a, &_b, &_c); + double meanZ = _getMeanZ(fittingPtZ); + double x = segLen * i * 2.0 + xRange.min; + double y = -(_a * x + _c) / _b; + SVzNL3DPoint a_pt; + a_pt.x = x; + a_pt.y = y; + a_pt.z = meanZ; + weldOps.push_back(a_pt); + } + return; } \ No newline at end of file diff --git a/sourceCode/SX_lapWeldDetection_Export.h b/sourceCode/SX_lapWeldDetection_Export.h index 55673c5..6ac913a 100644 --- a/sourceCode/SX_lapWeldDetection_Export.h +++ b/sourceCode/SX_lapWeldDetection_Export.h @@ -17,6 +17,8 @@ typedef struct ESX_WeldScanMode scanMode; }SSX_lapWeldParam; +SG_APISHARED_EXPORT const char* wd_weldSeamAlgoVersion(void); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -39,3 +41,10 @@ SG_APISHARED_EXPORT void sx_getLapWeldPostion( std::vector>& objOps, int* errCode); +//㺸λ +SG_APISHARED_EXPORT void BQ_getWeldSeamPose( + std::vector& trees, + int weldRefPoints, //мολ + std::vector& weldOps, //λ + std::vector& weldRawPoints, //ʾdebug + int* errCode); \ No newline at end of file diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp index 8fcda05..df4a176 100644 --- a/sourceCode/dataFitting.cpp +++ b/sourceCode/dataFitting.cpp @@ -179,11 +179,15 @@ double fitCircleByLeastSquare( double e; double r2 = radius * radius; for (int pId = 0; pId < N; ++pId) { - e = pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2; - if (e > err) { + e = abs(pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2); +#if 0 if (e > err) { err = e; } +#else + err += e; +#endif } + err = sqrt(err / N); return err; }