From f855ffd70b0f049c3fe0819fbcedb86caf8079af Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 10 Sep 2025 23:59:05 +0800 Subject: [PATCH] =?UTF-8?q?=E6=90=AD=E6=8E=A5=E7=84=8A=E7=BC=9D=E7=AE=97?= =?UTF-8?q?=E6=B3=95=E5=88=9D=E6=AC=A1=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SG_Algorithm.sln | 53 ++ bagPositioning_test/bagPositioning_test.cpp | 344 ++++++-- lapWeldDetection/lapWeldDetection.vcxproj | 172 ++++ .../lapWeldDetection_test.cpp | 829 ++++++++++++++++++ .../lapWeldDetection_test.vcxproj | 157 ++++ sourceCode/SG_baseAlgo_Export.h | 22 + sourceCode/SG_baseDataType.h | 3 + sourceCode/SG_baseFunc.cpp | 338 +++++++ sourceCode/SG_lineFeature.cpp | 371 ++++++++ sourceCode/SX_lapWeldDetection.cpp | 216 +++++ sourceCode/SX_lapWeldDetection_Export.h | 40 + 11 files changed, 2491 insertions(+), 54 deletions(-) create mode 100644 lapWeldDetection/lapWeldDetection.vcxproj create mode 100644 lapWeldDetection_test/lapWeldDetection_test.cpp create mode 100644 lapWeldDetection_test/lapWeldDetection_test.vcxproj create mode 100644 sourceCode/SX_lapWeldDetection.cpp create mode 100644 sourceCode/SX_lapWeldDetection_Export.h diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 740736d..cf60398 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -86,6 +86,27 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning_test", "gl {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} = {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bottleCapQualityCheck", "bottleCapQualityCheck\bottleCapQualityCheck.vcxproj", "{DE243631-2472-45B7-84DD-06C8B5DC1D5A}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bottleCapQualityCheck_test", "bottleCapQualityCheck_test\bottleCapQualityCheck_test.vcxproj", "{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}" + ProjectSection(ProjectDependencies) = postProject + {DE243631-2472-45B7-84DD-06C8B5DC1D5A} = {DE243631-2472-45B7-84DD-06C8B5DC1D5A} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lapWeldDetection", "lapWeldDetection\lapWeldDetection.vcxproj", "{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lapWeldDetection_test", "lapWeldDetection_test\lapWeldDetection_test.vcxproj", "{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0} = {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -230,6 +251,38 @@ Global {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.Build.0 = Release|x64 {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.ActiveCfg = Release|Win32 {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.Build.0 = Release|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x64.ActiveCfg = Debug|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x64.Build.0 = Debug|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x86.ActiveCfg = Debug|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x86.Build.0 = Debug|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x64.ActiveCfg = Release|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x64.Build.0 = Release|x64 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x86.ActiveCfg = Release|Win32 + {DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x86.Build.0 = Release|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x64.ActiveCfg = Debug|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x64.Build.0 = Debug|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x86.ActiveCfg = Debug|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x86.Build.0 = Debug|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x64.ActiveCfg = Release|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x64.Build.0 = Release|x64 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x86.ActiveCfg = Release|Win32 + {6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x86.Build.0 = Release|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x64.ActiveCfg = Debug|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x64.Build.0 = Debug|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x86.ActiveCfg = Debug|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x86.Build.0 = Debug|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x64.ActiveCfg = Release|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x64.Build.0 = Release|x64 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x86.ActiveCfg = Release|Win32 + {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x86.Build.0 = Release|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x64.ActiveCfg = Debug|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x64.Build.0 = Debug|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x86.ActiveCfg = Debug|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x86.Build.0 = Debug|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x64.ActiveCfg = Release|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x64.Build.0 = Release|x64 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.ActiveCfg = Release|Win32 + {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 9fdb34c..85a2bae 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -77,7 +77,9 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int lineIdx = 0; int ptIdx = 0; int ptNum = 0; + int pre_ptNum = -1; std::vector< SVzNLPointXYZRGBA> a_line; + int vldLineIdx = 0; int vldPtNum = 0; unsigned int timeStamp = 0; while (getline(inputFile, linedata)) @@ -119,20 +121,25 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, break; int recvPtNum = (int)a_line.size(); - if ( (recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) + if ( (recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) { - SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); - _scanLines[lineIdx].nPointCnt = ptNum; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPoint = p3DPoint; - for (int m = 0; m < ptNum; m++) + SVzNLPointXYZRGBA* p3DPoint; + if (pre_ptNum > 0) + p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); + else + p3DPoint = NULL; + _scanLines[vldLineIdx].nPointCnt = pre_ptNum; + _scanLines[vldLineIdx].nTimeStamp = timeStamp; + _scanLines[vldLineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < pre_ptNum; m++) p3DPoint[m] = a_line[m]; - lineIdx++; + vldLineIdx++; } //new Line timeStamp = curr_timeStamp; vldPtNum = 0; a_line.clear(); + pre_ptNum = ptNum; } else if (0 == strncmp("{", linedata.c_str(), 1)) { @@ -167,19 +174,23 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, } //last line int recvPtNum = (int)a_line.size(); - if ((recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) + if ((recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) { - SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); - _scanLines[lineIdx].nPointCnt = ptNum; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPoint = p3DPoint; - for (int m = 0; m < ptNum; m++) + SVzNLPointXYZRGBA* p3DPoint; + if (pre_ptNum > 0) + p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum); + else + p3DPoint = NULL; + _scanLines[vldLineIdx].nPointCnt = pre_ptNum; + _scanLines[vldLineIdx].nTimeStamp = timeStamp; + _scanLines[vldLineIdx].p3DPoint = p3DPoint; + for (int m = 0; m < pre_ptNum; m++) p3DPoint[m] = a_line[m]; - lineIdx++; + vldLineIdx++; } if (scanLineNum) - *scanLineNum = lineIdx; + *scanLineNum = vldLineIdx; inputFile.close(); return _scanLines; } @@ -400,6 +411,133 @@ SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* sc return _scanLines; } +//从RGBD点云中读取XYZ信息 +SVzNL3DLaserLine* vzReadXYZPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //第一行 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + float leftX, leftY; + float rightX, rightY; + float r, g, b; + sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + inputFile.close(); + return _scanLines; +} + SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints, int lineNum, double _F, double* camPoseR, int* outLineNum) { int min_y = 100000000; @@ -1875,7 +2013,7 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in else y_scale = x_scale; - int angleDrawLen = dirLen / x_scale; + int angleDrawLen = dirLen;// / x_scale; _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); //旋转视角显示 @@ -2373,7 +2511,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData, #define TEST_COMPUTE_GRASP_POINT 1 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_GROUP 22 +#define TEST_GROUP 27 #define TEST_TOP_VIEW_GROUP (TEST_GROUP - 8) #define TEST_TOP_ORIEN_GROUP (TEST_GROUP - 6) int main() @@ -2407,13 +2545,13 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; - sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\"); - char _scan_file[256]; - double _F = 1247.95;// 1231.2; //1729.0;; //f - for (int i = 1; i <= 28; i++) + sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\纸箱拆跺点云\\"); + char _scan_src_file[256]; + double _F = 1802.16; //1247.95;// 1231.2; //1729.0;; //f + for (int i = 5; i <= 22; i++) { - sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i); - SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + sprintf_s(_scan_src_file, "%sLaserLine%d.txt", _scan_dir, i); + SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_src_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond, true); if (laser3DPoints_RGBD == NULL) continue; @@ -2429,7 +2567,11 @@ int main() #endif int vldLineNum = 0; SVzNL3DLaserLine* gridData = _convertToGridData_XYZRGB(laser3DPoints_RGBD, lineNum, _F, camPoseR, &vldLineNum); - + char _out_file[256]; + sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i); + _outputScanDataFile_self(_out_file, gridData, vldLineNum, + lineV, maxTimeStamp, clockPerSecond); +#if 0 //生成水平扫描数据 int hLineNum = gridData[0].nPositionCnt; SVzNL3DLaserLine* hScanData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * hLineNum); @@ -2448,15 +2590,11 @@ int main() hScanData[hLine].p3DPosition[m].pt3D.z = gridData[m].p3DPosition[hLine].pt3D.z; } } - char _out_file[256]; - sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i); - _outputScanDataFile_self(_out_file, gridData, vldLineNum, - lineV, maxTimeStamp, clockPerSecond); sprintf_s(_out_file, "%sLaserLine%d_hScanData.txt", _scan_dir, i); _outputScanDataFile_removeZeros(_out_file, hScanData, hLineNum, lineV, maxTimeStamp, clockPerSecond); - - printf("%s: convert done!\n", _scan_file); +#endif + printf("%s: convert done!\n", _scan_src_file); } #endif @@ -2512,20 +2650,27 @@ int main() "F:\\ShangGu\\编织袋数据\\点云12_盐袋\\", //11 "F:\\ShangGu\\编织袋数据\\点云13_现场测试\\", //12 "F:\\ShangGu\\编织袋数据\\山东现场\\", //13 - "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //14 - "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //15 - "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //16 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //17 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //18 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //19 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //20 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //21 + "F:\\ShangGu\\编织袋数据\\拆包模拟环境\\", //14 + "F:\\ShangGu\\纸箱拆垛数据\\vizum数据\\", //15 + "F:\\ShangGu\\纸箱拆垛数据\\380×480mm纸箱RGB点云及2D图像\\", //16 + "F:\\ShangGu\\纸箱拆垛数据\\380×580mm纸箱RGB点云及2d图像\\", //17 + "F:\\ShangGu\\纸箱拆垛数据\\480×580mm纸箱rgb点云及2d图像\\", //18 + + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //19 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //20 + "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //21 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //22 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //23 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //24 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //25 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //26 }; SVzNLRange fileIdx[TEST_GROUP] = { {0,176},{1,200},{1,166},{122,141},{1,65}, {1,29},{108,135},{0,200}, {1,200}, {1,12}, - {2,4}, {1,5}, {1,1}, {1,3}, + {2,4}, {1,5}, {1,1}, {1,3}, {11,11}, + {5,22},{1, 15},{1,15}, {1, 15}, {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -2549,7 +2694,7 @@ int main() SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 14; grp <= 15; grp++) + for (int grp = 17; grp <= 18; grp++) { if (grp < 10) { @@ -2587,7 +2732,67 @@ int main() algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm } - else if ( (grp >= 14) && (grp <= 15)) + else if (grp == 14) + { + algoParam.bagParam.bagL = 750; //袋子长65cm + algoParam.bagParam.bagW = 420; //袋子宽40cm + algoParam.bagParam.bagH = 150; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if (grp == 15) //纸箱拆垛数据\vizum数据 + { + algoParam.bagParam.bagL = 400; //袋子长65cm + algoParam.bagParam.bagW = 255; //袋子宽40cm + algoParam.bagParam.bagH = 300; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if (grp == 16) //纸箱拆垛数据\380×480mm纸箱RGB点云及2D图像 + { + algoParam.bagParam.bagL = 480; //袋子长 + algoParam.bagParam.bagW = 380; //袋子宽 + algoParam.bagParam.bagH = 580; //袋子高 + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if (grp == 17) //纸箱拆垛数据\380×580mm纸箱RGB点云及2d图像 + { + algoParam.bagParam.bagL = 580; //袋子长 + algoParam.bagParam.bagW = 380; //袋子宽 + algoParam.bagParam.bagH = 480; //袋子高 + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if (grp == 18) //纸箱拆垛数据\480×580mm纸箱rgb点云及2d图像 + { + algoParam.bagParam.bagL = 580; //袋子长 + algoParam.bagParam.bagW = 480; //袋子宽 + algoParam.bagParam.bagH = 380; //袋子高 + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if ( (grp >= 19) && (grp <= 20)) { algoParam.bagParam.bagL = 750; //袋子长65cm algoParam.bagParam.bagW = 450; //袋子宽40cm @@ -2612,10 +2817,10 @@ int main() algoParam.growParam.minVTypeTreeLen = 50.0; //mm } #if BAG_ALGO_USE_CORNER_FEATURE - algoParam.cornerParam.cornerTh = 30; //45度角 - algoParam.cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; - algoParam.cornerParam.minEndingGap_z = algoParam.bagParam.bagH / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; algoParam.cornerParam.jumpCornerTh_1 = 60; algoParam.cornerParam.jumpCornerTh_2 = 15; #else @@ -2695,13 +2900,28 @@ int main() poseCalibPara.invRMatrix[8] = 0.999699; } - else if ( grp == 14) + else if (grp == 14) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\拆包模拟环境\\ground_calib_para2.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if ( (grp >= 15) && (grp <= 18)) //箱子 + { + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 5;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 5; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; + } + else if ( grp == 19) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } - else if (grp == 15) + else if (grp == 20) { char calibFile[250]; sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\ground_calib_para.txt"); @@ -2717,11 +2937,24 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; - sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); - SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + SVzNL3DLaserLine* laser3DPoints = NULL; + if ((grp >= 16) && (grp <= 18)) + { + sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx); + laser3DPoints = vzReadXYZPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + } + else + { + sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); + laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + } if (laser3DPoints == NULL) continue; - +#if 0 //数据转存 + sprintf_s(_scan_file, "%sLaserLine%d_grid_noRGB.txt", dataPath[grp], fidx); + _outputScanDataFile_self(_scan_file, laser3DPoints, lineNum, + lineV, maxTimeStamp, clockPerSecond); +#endif algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 algoParam.filterParam.outlierTh = 5; @@ -2749,7 +2982,11 @@ int main() //调平，去除地面 sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } - +#if 0 //数据转存 + sprintf_s(_scan_file, "%sLaserLine%d_grid_RTadjust.txt", dataPath[grp], fidx); + _outputScanDataFile_self(_scan_file, laser3DPoints, lineNum, + lineV, maxTimeStamp, clockPerSecond); +#endif std::vector objOps; sg_getBagPosition(laser3DPoints, lineNum, algoParam, poseCalibPara, objOps); #endif @@ -2764,7 +3001,7 @@ int main() sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx); cv::String imgName(_dbg_file); double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // - double angleDrawLen = algoParam.bagParam.bagL / 3; + double angleDrawLen = algoParam.bagParam.bagL * 0.4; _genXOYProjectionImage(imgName, laser3DPoints, lineNum, objOps, rpy, angleDrawLen); #endif #if 0 @@ -2788,8 +3025,7 @@ int main() } else if (grp < TEST_TOP_ORIEN_GROUP) { - double FBVldPtRatioTh; - + fidx = 0; SSG_hsvCmpParam colorCmpParam; //色度和色饱和度比较门限，小于门限为同一颜色 colorCmpParam.hueTh = 15.0; colorCmpParam.saturateTh = 120.0; //60 @@ -2835,7 +3071,7 @@ int main() int kkk = 1; //行处理 //调平，去除地面 - sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); + //sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } #if 0 char _out_file[256]; diff --git a/lapWeldDetection/lapWeldDetection.vcxproj b/lapWeldDetection/lapWeldDetection.vcxproj new file mode 100644 index 0000000..fb8d3ac --- /dev/null +++ b/lapWeldDetection/lapWeldDetection.vcxproj @@ -0,0 +1,172 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {0c0b9b7a-9bf2-4157-bffd-d12012e39fa0} + lapWeldDetection + 10.0 + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + + + + + Level3 + true + true + true + NDEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + + + + + + \ No newline at end of file diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp new file mode 100644 index 0000000..266ef49 --- /dev/null +++ b/lapWeldDetection_test/lapWeldDetection_test.cpp @@ -0,0 +1,829 @@ +// glovePositioning_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "SX_lapWeldDetection_Export.h" +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzNL3DPoint>& scanData, bool exchangeXY) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + 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 + { + a_pt.x = X; + a_pt.y = Y; + } + a_pt.z = Z; + scanData.push_back(a_pt); + } + return; +} + +void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int lineIdx = 0; + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + if (true == removeZeros) + { + int vldPtNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldPtNum++; + } + linePtNum = vldPtNum; + } + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if ((pt3D->z > 1e-4) && (isNull == true)) + isNull = false; + if ((true == removeZeros) && (pt3D->z < 1e-4)) + continue; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + +//输出水平扫描数据 +void _outputScanDataFile_vector_h(char* fileName, std::vector>& scanLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines[0].size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int linePtNum = (int)scanLines.size(); + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[i][line].pt3D; + + float x = (float)pt3D->y; + float y = (float)pt3D->x; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + +void _outputRGBDScanLapWeld_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector< std::vector> weldOPs) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (weldOPs.size() > 0) + realLines++; + + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + int featureType_v = pt3D->nPointIdx & 0xffff; + int featureType_h = featureType_v >> 4; + featureType_v &= 0xff; + if (LINE_FEATURE_PEAK_TOP == featureType_v) + { + rgb = { 255, 97, 0 }; + size = 5; + } + else if (LINE_FEATURE_PEAK_TOP == featureType_h) + { + rgb = { 97, 255, 0 }; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (weldOPs.size() > 0) + { + int weldNum = (int)weldOPs.size(); + int linePtNum = 0; + for (int i = 0; i < weldNum; i++) + linePtNum += (int)weldOPs[i].size(); + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + + rgb = { 0, 0, 255 }; + size = 25; + for (int i = 0; i < weldNum; i++) + { + rgb = objColor[i % 8]; + for (int j = 0; j < (int)weldOPs[i].size(); j++) + { + float x = (float)weldOPs[i][j].x; + float y = (float)weldOPs[i][j].y; + float z = (float)weldOPs[i][j].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + //加一个点，用于跳过显示工具bug + rgb = objColor[0]; + float x = (float)weldOPs[0][0].x; + float y = (float)weldOPs[0][0].y; + float z = (float)weldOPs[0][0].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + sw.close(); +} + + +void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum) +{ + std::ofstream sw(fileName); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + for (int line = 0; line < lineNum; line++) + { + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + + +#define DATA_VER_OLD 0 +#define DATA_VER_NEW 1 +#define DATA_VER_FROM_CUSTOM 2 +#define VZ_LASER_LINE_PT_MAX_NUM 4096 +SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //第一行 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + else if (dataFileVer == DATA_VER_OLD) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("DataElements_", linedata.c_str(), 13)) + { + sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); + if ((dataElements != 3) && (dataElements != 4)) + break; + } + if (0 == strncmp("LineV_", linedata.c_str(), 6)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); +#if 0 + if (scanLineListTail == NULL) + firstIndex = lineIndex; +#endif + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + //new Line + //new Line + lineIdx++; + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + } + else if (0 == strncmp("(", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + if (dataElements == 4) + sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); + else + sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < VZ_LASER_LINE_PT_MAX_NUM) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + } + inputFile.close(); + return _scanLines; +} + +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + 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; +} + +void _getRoiClouds( + std::vector< std::vector>& scanLines, + int startLine, + int endLine, + int startPtIdx, + int endPtIdx, + std::vector< std::vector>& roiScanLines) +{ + for (int i = startLine; i < endLine; i++) + { + if (i >= scanLines.size()) + break; + + std::vector cut_line; + std::vector& a_line = scanLines[i]; + for (int j = startPtIdx; j < endPtIdx; j++) + { + SVzNL3DPosition a_pt; + if (j >= a_line.size()) + { + a_pt.nPointIdx = 0; + a_pt.pt3D = { 0,0,0 }; + } + else + a_pt = a_line[j]; + cut_line.push_back(a_pt); + } + roiScanLines.push_back(cut_line); + } + return; +} + +void _convertToGridData(std::vector< std::vector>& scanLines, + std::vector< std::vector>& gridScanLines, + double y_step) +{ + double y_min = scanLines[0][0].pt3D.y; + double y_max = y_min; + int lineNum = (int)scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + std::vector& a_line = scanLines[line]; + int ptNum = (int)a_line.size(); + for (int i = 0; i < ptNum; i++) + { + if (y_min > a_line[i].pt3D.y) + y_min = a_line[i].pt3D.y; + if (y_max < a_line[i].pt3D.y) + y_max = a_line[i].pt3D.y; + } + } + double half_step = y_step / 2; + int gridPtNum = (int)((y_max - y_min + half_step) / y_step) + 1; + for (int line = 0; line < lineNum; line++) + { + std::vector grid_line; + grid_line.resize(gridPtNum); + for (int i = 0; i < gridPtNum; i++) + grid_line[i] = { 0, {0.0, 0.0, 0.0} }; + + std::vector& a_line = scanLines[line]; + int ptNum = (int)a_line.size(); + for (int i = 0; i < ptNum; i++) + { + int ptIdx = (int)((a_line[i].pt3D.y + half_step - y_min) / y_step); + grid_line[ptIdx] = a_line[i]; + } + gridScanLines.push_back(grid_line); + } + return; +} + +#define CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_GLOVE_POSITION 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\", //0 + + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {0,6} + }; + +#if CONVERT_TO_GRID + int convertGrp = 0; + 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); + std::vector< SVzNL3DPoint> scanData; + bool exchangeXY = true; + vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, exchangeXY); + //将数据恢复为按扫描线存储格式 + 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); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, grid_scanLines, 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); +#endif + } +#endif + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\scanData_ground_1.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); + 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); + } +#endif + +#if TEST_COMPUTE_GLOVE_POSITION + for (int grp = 0; grp <= 0; grp++) + { + 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 = -1.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =1; + char _scan_file[256]; + sprintf_s(_scan_file, "%sscanData_%d.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);// calibPara.planeHeight); + } +#if 0 + char _out_file[256]; + sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines); +#endif + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 5; + growParam.yDeviation_max = 1.0; + growParam.maxSkipDistance = 5.0; + growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + growParam.minLTypeTreeLen = 30.0; //mm + growParam.minVTypeTreeLen = 30.0; //mm + SSG_cornerParam cornerParam; + cornerParam.cornerTh =30; //45度角 + cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + cornerParam.jumpCornerTh_1 = 10; + cornerParam.jumpCornerTh_2 = 30; + SSX_lapWeldParam lapWeldParam; + lapWeldParam.lapHeight = 2.0; + lapWeldParam.weldMinLen = 2.0; + lapWeldParam.weldRefPoints = 2; + int errCode = 0; + std::vector> weldOps; + sx_getLapWeldPostion( + scanLines, + cornerParam, + growParam, + lapWeldParam, + weldOps, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps); + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/lapWeldDetection_test/lapWeldDetection_test.vcxproj b/lapWeldDetection_test/lapWeldDetection_test.vcxproj new file mode 100644 index 0000000..38f1f0f --- /dev/null +++ b/lapWeldDetection_test/lapWeldDetection_test.vcxproj @@ -0,0 +1,157 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {bd65ac40-a2c9-4c8d-adba-aba595c5f32d} + lapWeldDetectiontest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + opencv_world480d.lib;lapWeldDetection.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + true + true + opencv_world480.lib;lapWeldDetection.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index d318ccc..d04c772 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -60,6 +60,22 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature( const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 SSG_lineFeature* line_features); +/// +/// ȡϵJumping +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector< SSG_basicFeature1D> jumpFeatures); + SG_APISHARED_EXPORT void wd_getLineGloveArcs( std::vector& lineData, int lineIdx, @@ -278,6 +294,12 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara( SVzNL3DLaserLine* laser3DPoints, int lineNum); +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2( + std::vector< std::vector>& scanLines); + //һƽƽ //ROIڵĵƽϣƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index a3fcf08..c2e415e 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -402,6 +402,7 @@ typedef struct typedef struct { int pntIdx; + int type; double forwardAngle; //ǰ double backwardAngle; // double corner; //ս @@ -409,4 +410,6 @@ typedef struct double backwardDiffZ; double pre_stepDist; double post_stepDist; + double forward_z; + double backward_z; }SSG_pntDirAngle; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 66acdd8..f251c97 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -1207,6 +1207,344 @@ SSG_planeCalibPara sg_getPlaneCalibPara( return planePara; } +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara sg_getPlaneCalibPara2( + std::vector< std::vector>& scanLines) +{ + //óʼ + 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; + + int lineNum = (int)scanLines.size(); + //ͳzΧ + SVzNLRangeD zRange = { 0, -1 }; //< ZΧ + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + //z + if (zRange.max < zRange.min) + { + zRange.min = pt3D->pt3D.z; + zRange.max = pt3D->pt3D.z; + } + else + { + if (zRange.min > pt3D->pt3D.z) + zRange.min = pt3D->pt3D.z; + if (zRange.max < pt3D->pt3D.z) + zRange.max = pt3D->pt3D.z; + } + } + } + + //Zͳƣȡһֵ + //mmΪλ + int zHistSize = (int)(zRange.max - zRange.min) + 1; + if (zHistSize == 0) + return planePara; + + std::vector zHist; + zHist.resize(zHistSize); + int totalPntSize = 0; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + totalPntSize++; + int histPos = (int)(pt3D->pt3D.z - zRange.min); + zHist[histPos] ++; + } + } + std::vector zSumHist; + zSumHist.resize(zHistSize); + bool isSame = true; + //Ϊλۼ + for (int i = 0; i < zHistSize; i++) + { + int sumValue = 0; + for (int j = i - 5; j <= i + 5; j++) + { + if ((j >= 0) && (j < zHistSize)) + sumValue += zHist[j]; + } + zSumHist[i] = sumValue; + if (i > 0) + { + if (sumValue != zSumHist[i - 1]) + isSame = false; + } + } + if (true == isSame) + { + //ۼӣۼӣۼֵȣ + for (int i = 0; i < zHistSize; i++) + zSumHist[i] = zHist[i]; + } + + //ѰҼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + int pre_data = -1; + std::vector< SSG_intPair> pkTop; + std::vector< SSG_intPair> pkBtm; + std::vector pkBtmBackIndexing; + pkBtmBackIndexing.resize(zHistSize); + for (int i = 0; i < zHistSize; i++) + pkBtmBackIndexing[i] = -1; + + + for (int i = 0; i < zHistSize; i++) + { + int curr_data = zSumHist[i]; + if (pre_data < 0) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double z_diff = curr_data - pre_data; + switch (_state) + { + case 0: //̬ + if (z_diff < 0) //½ + { + _state = 2; + } + else if (z_diff > 0) // + { + _state = 1; + } + break; + case 1: // + if (z_diff < 0) //½ + { + pkTop.push_back({ pre_i, pre_data }); + _state = 2; + } + else if (i == (zHistSize - 1)) + pkTop.push_back({ i, curr_data }); + break; + case 2: //½ + if (z_diff > 0) // + { + int pkBtmIdx = (int)pkBtm.size(); + pkBtmBackIndexing[pre_i] = pkBtmIdx; + pkBtm.push_back({ pre_i, pre_data }); + _state = 1; + } + else if (i == (zHistSize - 1)) + { + int pkBtmIdx = (int)pkBtm.size(); + pkBtmBackIndexing[i] = pkBtmIdx; + pkBtm.push_back({ i, curr_data }); + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //Ѱҵһܵ1/3ļֵ + if (pkTop.size() < 1) + return planePara; + + int pntSizeTh = totalPntSize / 10; + SSG_intPair* vldPeak = NULL; + for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++) + { + if (pkTop[i].data_1 > pntSizeTh) + { + vldPeak = &pkTop[i]; + break; + } + } + if (NULL == vldPeak) + return planePara; + + //Ѱҿʼͽλ + //ǰѰ + int preBtmIdx = -1; + for (int j = vldPeak->data_0 - 1; j >= 0; j--) + { + if (pkBtmBackIndexing[j] >= 0) + { + int idx = pkBtmBackIndexing[j]; + if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2)) + { + preBtmIdx = j; + break; + } + } + } + int postBtmIdx = -1; + for (int j = vldPeak->data_0 + 1; j < zHistSize; j++) + { + if (pkBtmBackIndexing[j] >= 0) + { + int idx = pkBtmBackIndexing[j]; + if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2)) + { + postBtmIdx = j; + break; + } + } + } + + SVzNLRangeD topZRange; + if (preBtmIdx < 0) + topZRange.min = zRange.min; + else + topZRange.min = (float)preBtmIdx + zRange.min; + if (postBtmIdx < 0) + topZRange.max = zRange.max; + else + topZRange.max = (float)postBtmIdx + zRange.min; + + //ȡ + std::vector Points3ds; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max)) + { + cv::Point3f a_vldPt; + a_vldPt.x = (float)pt3D->pt3D.x; + a_vldPt.y = (float)pt3D->pt3D.y; + a_vldPt.z = (float)pt3D->pt3D.z; + Points3ds.push_back(a_vldPt); + } + } + } + //ƽ + std::vector planceFunc; + vzCaculateLaserPlane(Points3ds, planceFunc); + +#if 1 //תתʹԪ + Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]); + Vector3 b = Vector3(0, 0, -1.0); + Quaternion quanPara = rotationBetweenVectors(a, b); + + RotationMatrix rMatrix; + quaternionToMatrix(quanPara, rMatrix.data); + //㷴ת + Quaternion invQuanPara = rotationBetweenVectors(b, a); + RotationMatrix invMatrix; + quaternionToMatrix(invQuanPara, invMatrix.data); +#else //ƽķŷǣת + // + SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]); + //У + eulerPra.roll = eulerPra.roll; + eulerPra.pitch = eulerPra.pitch; + eulerPra.yaw = eulerPra.yaw; + RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll); +#endif + + planePara.planeCalib[0] = rMatrix.data[0][0]; + planePara.planeCalib[1] = rMatrix.data[0][1]; + planePara.planeCalib[2] = rMatrix.data[0][2]; + planePara.planeCalib[3] = rMatrix.data[1][0]; + planePara.planeCalib[4] = rMatrix.data[1][1]; + planePara.planeCalib[5] = rMatrix.data[1][2]; + planePara.planeCalib[6] = rMatrix.data[2][0]; + planePara.planeCalib[7] = rMatrix.data[2][1]; + planePara.planeCalib[8] = rMatrix.data[2][2]; + + planePara.invRMatrix[0] = invMatrix.data[0][0]; + planePara.invRMatrix[1] = invMatrix.data[0][1]; + planePara.invRMatrix[2] = invMatrix.data[0][2]; + planePara.invRMatrix[3] = invMatrix.data[1][0]; + planePara.invRMatrix[4] = invMatrix.data[1][1]; + planePara.invRMatrix[5] = invMatrix.data[1][2]; + planePara.invRMatrix[6] = invMatrix.data[2][0]; + planePara.invRMatrix[7] = invMatrix.data[2][1]; + planePara.invRMatrix[8] = invMatrix.data[2][2]; + +#if 0 //test: ij˻ǵλ + double testMatrix[3][3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + testMatrix[i][j] = 0; + for (int m = 0; m < 3; m++) + testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j]; + } + } +#endif + //ݽת + SVzNLRangeD calibZRange = { 0, -1 }; + topZRange = { 0, -1 }; + for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) + { + //z + if (topZRange.max < topZRange.min) + { + topZRange.min = Points3ds[i].z; + topZRange.max = Points3ds[i].z; + } + else + { + if (topZRange.min > Points3ds[i].z) + topZRange.min = Points3ds[i].z; + if (topZRange.max < Points3ds[i].z) + topZRange.max = Points3ds[i].z; + } + cv::Point3f a_calibPt; + a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]); + a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]); + a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]); + //z + if (calibZRange.max < calibZRange.min) + { + calibZRange.min = a_calibPt.z; + calibZRange.max = a_calibPt.z; + } + else + { + if (calibZRange.min > a_calibPt.z) + calibZRange.min = a_calibPt.z; + if (calibZRange.max < a_calibPt.z) + calibZRange.max = a_calibPt.z; + } + } + planePara.planeHeight = calibZRange.min; + + return planePara; +} + //һƽƽ //ROIڵĵƽϣƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index a011912..f39f7ac 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -1438,6 +1438,377 @@ void sg_getLineCornerFeature( return; } +bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { + return a.pntIdx < b.pntIdx; +} +/// +/// ȡϵJumping +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void sg_getLineJumpFeature_cornerMethod( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector< SSG_basicFeature1D> jumpFeatures) +{ + //ȥ + std::vector< SVzNL3DPosition> vldPts; + std::vector segs; + //޸segĶ塣seg˵޵ĵ + int segStart = -1, segEnd = -1; + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPosition a_pt = lineData[i]; + a_pt.nPointIdx = i; + if (lineData[i].pt3D.z > 1e-4) + vldPts.push_back(a_pt); + + //segж + if (lineData[i].pt3D.z > 1e-4) + { + if (segStart < 0) + segStart = i; + else // + { + SVzNL3DPosition pre_pt = lineData[i - 1]; + double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z); + if (diff_z > cornerPara.minEndingGap_z) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + segStart = i; + } + } + segEnd = i; + } + else + { + if (segStart >= 0) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + segStart = -1; + segEnd = -1; + } + } + } + //last + if (segStart >= 0) + { + SSG_RUN a_run; + a_run.start = segStart; + a_run.len = segEnd - segStart + 1; + a_run.value = 1; + segs.push_back(a_run); + } + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 875) + int kkk = 1; + + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //ս + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + corners[i].forward_z = 0; + corners[i].backward_z = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; // + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; //żС + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + corners[i].forward_z = vldPts[post_i].pt3D.z; // + corners[i].backward_z = vldPts[pre_i].pt3D.z; //żС + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + std::vector< SSG_pntDirAngle> cornerFeatures; + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + if (cornerPeakP[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_HR; + cornerFeatures.push_back(cornerPeakP[i]); + } + else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_FH; + cornerFeatures.push_back(cornerPeakP[i]); + } + } + } + for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++) + { + if (abs(cornerPeakM[i].corner) < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //߶ȴ + break; + + if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + if ((abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_HF; + cornerFeatures.push_back(cornerPeakM[i]); + } + else if ((abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > cornerPara.jumpCornerTh_2)) + { + cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_RH; + cornerFeatures.push_back(cornerPeakM[i]); + } + } + } + + // + std::sort(cornerFeatures.begin(), cornerFeatures.end(), compareByIdx); + + //ϻJump + for (int i = 0, i_max = (int)cornerFeatures.size(); i < i_max-1; i++) + { + if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HR) + { + if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_RH) + { + int pntIdx_1 = cornerFeatures[i].pntIdx; + int pntIdx_2 = cornerFeatures[i+1].pntIdx; + //߶ + double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); + double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; + if ((dist < cornerPara.minEndingGap) && (height < -cornerPara.minEndingGap_z)) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + a_feature.jumpPos = vldPts[pntIdx_1].pt3D; + a_feature.jumpPos2D = { lineIdx, pntIdx_1 }; + jumpFeatures.push_back(a_feature); + } + } + } + else if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HF) + { + if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_FH) + { + int pntIdx_1 = cornerFeatures[i].pntIdx; + int pntIdx_2 = cornerFeatures[i + 1].pntIdx; + //߶ + double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); + double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; + if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z)) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + a_feature.jumpPos = vldPts[pntIdx_2].pt3D; + a_feature.jumpPos2D = { lineIdx, pntIdx_2 }; + jumpFeatures.push_back(a_feature); + } + } + } + } + return; +} + //׵ĻɨߵPeakǰһΧڣǰǴı仯Χ150ȣֵ void wd_getLineGloveArcs( std::vector& lineData, diff --git a/sourceCode/SX_lapWeldDetection.cpp b/sourceCode/SX_lapWeldDetection.cpp new file mode 100644 index 0000000..03f0a63 --- /dev/null +++ b/sourceCode/SX_lapWeldDetection.cpp @@ -0,0 +1,216 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "SX_lapWeldDetection_Export.h" +#include +#include + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara sx_getBaseCalibPara( + std::vector< std::vector>& scanLines) +{ + return sg_getPlaneCalibPara2(scanLines); +} + +//̬ƽȥ +void sx_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +//ȡӺ +void sx_getLapWeldPostion( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + SSG_treeGrowParam growParam, + SSX_lapWeldParam lapWeldParam, + std::vector>& objOps, + int* errCode) +{ + *errCode = 0; + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return; + } + + int linePtNum = (int)scanLines[0].size(); + bool isGridData = true; + //ֱarcȡ + std::vector> jumpFeatures_v; + for (int line = 0; line < lineNum; line++) + { + if (line == 400) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &scanLines[line][0], + dataSize, + line, + cornerPara, //scaleͨȡbagH1/4 + a_line_features); + jumpFeatures_v.push_back(a_line_features); + } + + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + //ˮƽɨ + std::vector> hLines; + hLines.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + hLines[j][line] = scanLines[line][j]; + hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //ˮƽarcȡ + std::vector> jumpFeatures_h; + int lineNum_h = (int)hLines.size(); + for (int line = 0; line < lineNum_h; line++) + { + std::vector& lineData = hLines[line]; + + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &hLines[line][0], + dataSize, + line, + cornerPara, //scaleͨȡbagH1/4 + a_line_features); + + //ƥ + jumpFeatures_h.push_back(a_line_features); + } + + // + //ֱ߷ + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_v[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + v_trees, + growParam); + } + //ˮƽɨ˶ + std::vector h_trees; + for (int line = 0; line < lineNum_h; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } + + //treeϢ + std::vector allTreesInfo; //߽ + SSG_treeInfo a_nullTree; + memset(&a_nullTree, 0, sizeof(SSG_treeInfo)); + allTreesInfo.push_back(a_nullTree); //ִ洢λtreeIdxͬλã + //ǣбע + int hvTreeIdx = 1; + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_trees[i]; + + //¼TreeϢ + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 1; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_vTree->treeType; + a_treeInfo.sLineIdx = a_vTree->sLineIdx; + a_treeInfo.eLineIdx = a_vTree->eLineIdx; + a_treeInfo.roi = a_vTree->roi; + allTreesInfo.push_back(a_treeInfo); + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//Ŀ˺0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16; + } + } + } + hvTreeIdx++; + } + int hTreeStart = hvTreeIdx; + ////ע:ˮƽ + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_trees[i]; + //¼TreeϢ + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 0; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_hTree->treeType; + a_treeInfo.sLineIdx = a_hTree->sLineIdx; + a_treeInfo.eLineIdx = a_hTree->eLineIdx; + a_treeInfo.roi.left = a_hTree->roi.top; //ˮƽɨxyǽ + a_treeInfo.roi.right = a_hTree->roi.bottom; + a_treeInfo.roi.top = a_hTree->roi.left; + a_treeInfo.roi.bottom = a_hTree->roi.right; + allTreesInfo.push_back(a_treeInfo); + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//Ŀ˺0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16; + } + } + } + hvTreeIdx++; + } + int hvTreeSize = hvTreeIdx; + + // +} \ No newline at end of file diff --git a/sourceCode/SX_lapWeldDetection_Export.h b/sourceCode/SX_lapWeldDetection_Export.h new file mode 100644 index 0000000..7c60495 --- /dev/null +++ b/sourceCode/SX_lapWeldDetection_Export.h @@ -0,0 +1,40 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +typedef struct +{ + double lapHeight;//Ӻ + double weldMinLen; //С쳤ȣڹ˿ܵٺ + int weldRefPoints; //ֱߺIJο㣬Ĭ2յ㣩 +}SSX_lapWeldParam; + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara sx_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//̬ƽȥ +SG_APISHARED_EXPORT void sx_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +//ȡӺ +SG_APISHARED_EXPORT void sx_getLapWeldPostion( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + SSG_treeGrowParam growParam, + SSX_lapWeldParam lapWeldParam, + std::vector>& objOps, + int* errCode); +