From 360460320ec208854ef866af0d99d7d4798ae708 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 20 Jan 2026 07:12:01 +0800 Subject: [PATCH] =?UTF-8?q?bagThreadPositioning=20v1.0.0=20=E5=88=9D?= =?UTF-8?q?=E5=A7=8B=E6=8F=90=E4=BA=A4=EF=BC=8C=E6=A3=80=E6=B5=8B=E5=87=BA?= =?UTF-8?q?=E7=BA=BF=E5=B0=BE=E5=92=8C=E7=BA=BF=E7=BC=9D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SG_Algorithm.sln | 27 + .../bagThreadPositioning.vcxproj | 170 ++++++ .../bagThreadPositioning_Export.vcxproj | 158 ++++++ .../bagThreadPositioning_test.cpp | 498 ++++++++++++++++++ sourceCode/SG_baseAlgo_Export.h | 30 ++ sourceCode/SG_baseDataType.h | 7 + sourceCode/SG_errCode.h | 1 + sourceCode/SG_featureGrow.cpp | 81 ++- sourceCode/SG_lineFeature.cpp | 443 ++++++++++++++++ sourceCode/bagThreadPositioning.cpp | 220 ++++++++ sourceCode/bagThreadPositioning_Export.h | 28 + 11 files changed, 1651 insertions(+), 12 deletions(-) create mode 100644 bagThreadPositioning/bagThreadPositioning.vcxproj create mode 100644 bagThreadPositioning_Export/bagThreadPositioning_Export.vcxproj create mode 100644 bagThreadPositioning_Export/bagThreadPositioning_test.cpp create mode 100644 sourceCode/bagThreadPositioning.cpp create mode 100644 sourceCode/bagThreadPositioning_Export.h diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 923c8a3..f8405b9 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -194,6 +194,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rodAndBarDetection", "rodAn {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bagThreadPositioning", "bagThreadPositioning\bagThreadPositioning.vcxproj", "{F371FCBC-0AD6-4546-8785-1C05CD0C5B57}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bagThreadPositioning_test", "bagThreadPositioning_Export\bagThreadPositioning_Export.vcxproj", "{831F7635-98DB-45BB-9815-5334DA70BFE8}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57} = {F371FCBC-0AD6-4546-8785-1C05CD0C5B57} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -498,6 +509,22 @@ Global {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x64.Build.0 = Release|x64 {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x86.ActiveCfg = Release|Win32 {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x86.Build.0 = Release|Win32 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Debug|x64.ActiveCfg = Debug|x64 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Debug|x64.Build.0 = Debug|x64 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Debug|x86.ActiveCfg = Debug|Win32 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Debug|x86.Build.0 = Debug|Win32 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Release|x64.ActiveCfg = Release|x64 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Release|x64.Build.0 = Release|x64 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Release|x86.ActiveCfg = Release|Win32 + {F371FCBC-0AD6-4546-8785-1C05CD0C5B57}.Release|x86.Build.0 = Release|Win32 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Debug|x64.ActiveCfg = Debug|x64 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Debug|x64.Build.0 = Debug|x64 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Debug|x86.ActiveCfg = Debug|Win32 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Debug|x86.Build.0 = Debug|Win32 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Release|x64.ActiveCfg = Release|x64 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Release|x64.Build.0 = Release|x64 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Release|x86.ActiveCfg = Release|Win32 + {831F7635-98DB-45BB-9815-5334DA70BFE8}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bagThreadPositioning/bagThreadPositioning.vcxproj b/bagThreadPositioning/bagThreadPositioning.vcxproj new file mode 100644 index 0000000..d789695 --- /dev/null +++ b/bagThreadPositioning/bagThreadPositioning.vcxproj @@ -0,0 +1,170 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {f371fcbc-0ad6-4546-8785-1c05cd0c5b57} + bagThreadPositioning + 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\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;BAGTHREADPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;BAGTHREADPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;BAGTHREADPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + + + Windows + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + _CRT_SECURE_NO_WARNINGS;NDEBUG;BAGTHREADPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + + + Windows + true + true + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/bagThreadPositioning_Export/bagThreadPositioning_Export.vcxproj b/bagThreadPositioning_Export/bagThreadPositioning_Export.vcxproj new file mode 100644 index 0000000..52f7f0d --- /dev/null +++ b/bagThreadPositioning_Export/bagThreadPositioning_Export.vcxproj @@ -0,0 +1,158 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {831f7635-98db-45bb-9815-5334da70bfe8} + bagThreadPositioningExport + 10.0 + bagThreadPositioning_test + + + + 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 + _CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + + + Console + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;bagThreadPositioning.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + _CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;bagThreadPositioning.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/bagThreadPositioning_Export/bagThreadPositioning_test.cpp b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp new file mode 100644 index 0000000..5915c23 --- /dev/null +++ b/bagThreadPositioning_Export/bagThreadPositioning_test.cpp @@ -0,0 +1,498 @@ +// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "bagThreadPositioning_Export.h" +#include +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + +void vzReadLaserScanPointFromFile_encodePlyTxt( + const char* fileName, + double lineStep, + double baseZ, + std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> linePoints; + linePoints.clear(); + int startIdx = -1; + int ptIdx = 0; + double pre_y = DBL_MAX; + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + int X; + double Y, Z, tmp; + sscanf_s(linedata.c_str(), "%d,%lf,%lf", &X, &Y, &Z); + if (startIdx < 0) + startIdx = X; + + SVzNL3DPosition a_pt; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_pt.pt3D.x = (double)(X - startIdx) * lineStep; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = baseZ - Z; + if (Z < -9990.0) + { + a_pt.pt3D = { 0, 0, 0 }; + } + + if (Y < pre_y) + { + //新行 + if(linePoints.size() > 0) + scanData.push_back(linePoints); + linePoints.clear(); + ptIdx = 0; + } + linePoints.push_back(a_pt); + pre_y = Y; + } + if(linePoints.size() > 0) + scanData.push_back(linePoints); + return; +} + +bool checkGridFormat(std::vector>& scanData) +{ + if (scanData.size() == 0) + return false; + + int linePtNum = (int)scanData[0].size(); + for (int i = 1; i < (int)scanData.size(); i++) + { + if (linePtNum != (int)scanData[i].size()) + return false; + } + return true; +} + +void wdReadLaserScanPointFromFile_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 wd_gridScan_GetROIData(std::vector>& scanData, SVzNLRangeD roi_y, std::vector>& roiData) +{ + int lineNum = (int)scanData.size(); + int linePtNum = (int)scanData[0].size(); + int globalPtStart = INT_MAX; + int globalPtEnd = 0; + int lineStart = INT_MAX; + int lineEnd = 0; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition >& lineData = scanData[line]; + int ptSize = (int)lineData.size(); + int vldNum = 0; + int ptStart = INT_MAX; + int ptEnd = 0; + for (int i = 0; i < ptSize; i++) + { + if (lineData[i].pt3D.z > 1e-4) + { + if ((lineData[i].pt3D.y < roi_y.min) || (lineData[i].pt3D.y > roi_y.max)) + lineData[i].pt3D = { 0.0, 0.0, 0.0 }; + } + + if (lineData[i].pt3D.z > 1e-4) + { + if (ptStart > i) + ptStart = i; + ptEnd = i; + vldNum++; + } + } + if (vldNum > 0) + { + if (globalPtStart > ptStart) + globalPtStart = ptStart; + if (globalPtEnd < ptEnd) + globalPtEnd = ptEnd; + + if (lineStart > line) + lineStart = line; + lineEnd = line; + } + } + int vldLineNum = lineEnd - lineStart + 1; + int vldPtNum = globalPtEnd - globalPtStart + 1; + + roiData.resize(vldLineNum); + for (int line = 0; line < vldLineNum; line++) + { + roiData[line].resize(vldPtNum); + for (int i = 0; i < vldPtNum; i++) + roiData[line][i] = scanData[line + lineStart][i + globalPtStart]; + } + return; +} + +void _outputScanDataFile(char* fileName, std::vector>& scanData, + float lineV, int maxTimeStamp, int clockPerSecond) +{ + std::ofstream sw(fileName); + + int lineNum = (int)scanData.size(); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition& pt3D = scanData[line][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(); +} + +void _outputChanneltInfo(char* fileName, std::vector& threadInfo) +{ + std::ofstream sw(fileName); + + char dataStr[250]; + int objNum = (int)threadInfo.size(); + for (int i = 0; i < objNum; i++) + { + sprintf_s(dataStr, 250, "线头_%d: 位置( %g, %g, %g ), 下刀位置_( %g, %g, %g ), 角度_%g", + i + 1, threadInfo[i].threadPos.x, threadInfo[i].threadPos.y, threadInfo[i].threadPos.z, + threadInfo[i].operatePos.x, threadInfo[i].operatePos.y, threadInfo[i].operatePos.z, threadInfo[i].rotateAngle); + sw << dataStr << std::endl; + } + sw.close(); +} + +void _outputRGBDScan_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector& threadInfo +) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + int objNum = (int)threadInfo.size(); + if (objNum > 0) + realLines += 1; + + 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]; + if (pt3D->nPointIdx == 1) + { + rgb = objColor[pt3D->nPointIdx]; + size = 3; + } + else if (pt3D->nPointIdx == 2) + { + rgb = { 250, 0, 0 }; + size = 3; + } + else //if (pt3D->nPointIdx == 0) + { + 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 (objNum > 0) + { + sw << "Line_" << lineIdx << "_0_" << (objNum*2) << std::endl; + size = 8; + for (int i = 0; i < objNum; i++) + { + rgb = { 250, 0, 0 }; + float x = (float)threadInfo[i].threadPos.x; + float y = (float)threadInfo[i].threadPos.y; + float z = (float)threadInfo[i].threadPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + rgb = { 250, 250, 0 }; + x = (float)threadInfo[i].operatePos.x; + y = (float)threadInfo[i].operatePos.y; + z = (float)threadInfo[i].operatePos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //多输出一个，修正显示工具bug + rgb = { 250, 0, 0 }; + float x = (float)threadInfo[0].threadPos.x; + float y = (float)threadInfo[0].threadPos.y; + float z = (float)threadInfo[0].threadPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + //输出法向 + size = 1; + double len = 60; + lineIdx = 0; + for (int i = 0; i < objNum; i++) + { + SVzNL3DPoint pt0 = threadInfo[i].threadPos; + SVzNL3DPoint pt1 = threadInfo[i].operatePos; + //显示法向量 + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + //多输出一个，修正显示工具bug + SVzNL3DPoint pt0 = threadInfo[0].threadPos; + SVzNL3DPoint pt1 = threadInfo[0].operatePos; + //显示法向量 + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + sw.close(); +} + +#define CONVERT_TO_GRID 0 +#define BAG_THREAD_POSITIONING 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:/ShangGu/项目/吕宁项目/袋子拆线/模拟数据/", //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,13}, + }; + + const char* ver = wd_bagThreadPositioningVersion(); + printf("ver:%s\n", ver); + +#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, "%s袋子_%d.txt", dataPath[convertGrp], fidx); + + std::vector> scanData; + double lineStep = 0.096; + double baseZ = 350.0; + vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData); + bool isGridData = checkGridFormat(scanData); + int gridFormat = (true == isGridData) ? 1 : 0; + printf("%s: 栅格格式=%d\n", _scan_file, gridFormat); + //将数据恢复为按扫描线存储格式 + sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx); + _outputScanDataFile(_scan_file, scanData,0, 0, 0); + printf("%s: done.\n", _scan_file); + } +#endif + +#if BAG_THREAD_POSITIONING + for (int grp = 0; grp < TEST_GROUP; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx); + + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 70; + cornerParam.scale = 1.5; //袋子缝的尺度1.5mm比较合适 + cornerParam.minEndingGap = 1.0; // + cornerParam.minEndingGap_z = 1.0; + cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 + cornerParam.jumpCornerTh_2 = 60; + + SSG_raisedFeatureParam raisedFeatureParam; + raisedFeatureParam.minJumpZ = 1.5; + raisedFeatureParam.minK = 2.0; //对应63度 + raisedFeatureParam.widthRange.min = 1.0; + raisedFeatureParam.widthRange.max = 4.0; + + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 3.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + filterParam.outlierTh = 2; + + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = -1; //对于轮廓仪，线间隔很小，使用扫描线数的物理意义不清晰，使用maxSkipDistance + growParam.maxSkipDistance = 5.0; + growParam.yDeviation_max = 5.0; + growParam.zDeviation_max = 5.0;// + growParam.minLTypeTreeLen = 10; //mm, + growParam.minVTypeTreeLen = 10; //mm + + bool isHorizonScan = false; //true:激光线平行线缝；false:激光线垂直线缝 + int errCode = 0; + std::vector bagThreadInfo; + wd_bagThreadPositioning( + scanLines, + isHorizonScan, //true:激光线平行线缝；false:激光线垂直线缝 + filterParam, //噪点过滤参数 + cornerParam, //V型特征参数 + raisedFeatureParam,//线尾凸起参数 + growParam, //特征生长参数 + bagThreadInfo, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); + _outputRGBDScan_RGBD(_scan_file, scanLines, bagThreadInfo); + sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); + _outputChanneltInfo(_scan_file, bagThreadInfo); + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 3ffd37b..15c4971 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -150,6 +150,30 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature( std::vector& line_ringArcs // ); +/// +/// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getLineCorerFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + std::vector& line_cornerFeatures //յ +); + +//ȡ͹ +SG_APISHARED_EXPORT void wd_getLineRaisedFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SSG_raisedFeatureParam raisedFeaturePara, //͹ + std::vector& line_raisedFeatures //͹ +); + //ֱȡsplit-and-merge˼򻯣յֱߴֱ SG_APISHARED_EXPORT void wd_surfaceLineSegment( std::vector< SVzNL3DPosition>& lineData, @@ -244,6 +268,12 @@ SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( std::vector& trees, SSG_treeGrowParam growParam); +//corner +SG_APISHARED_EXPORT void sg_cornerFeaturesGrowing( + std::vector>& cornerFeatures, + std::vector& trees, + SSG_treeGrowParam growParam); + //gap SG_APISHARED_EXPORT void sg_lineGapsGrowing( int lineIdx, diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index dd3a2d6..553f012 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -175,6 +175,13 @@ typedef struct double maxDist; //㷽ǵĴڱ }SSG_lineSegParam; +typedef struct +{ + double minJumpZ; //z + double minK; //Сб + SVzNLRangeD widthRange; //zޡڴޣΪ +}SSG_raisedFeatureParam; + typedef struct { double scale_angle; //㷽ǵĴڱ diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 8c58124..9615e7b 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -7,6 +7,7 @@ #define SG_ERR_LABEL_INFO_ERROR -1004 #define SG_ERR_INVLD_SORTING_MODE -1005 #define SG_ERR_INVLD_Q_SCALE -1006 +#define SG_ERR_ZERO_OBJECTS -1007 //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 5c54751..4f0044f 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -668,19 +668,38 @@ void wd_getSegFeatureGrowingTrees( if (TREE_STATE_ALIVE == feature_trees[m].treeState) { int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().lineIdx); - if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || - (i == i_max - 1)) + if (line_diff > 0) { - feature_trees[m].treeState = TREE_STATE_DEAD; SWD_segFeature* first_node = &(feature_trees[m].treeNodes[0]); SWD_segFeature* last_node = &(feature_trees[m].treeNodes[feature_trees[m].treeNodes.size() - 1]); - double len = (abs(first_node->startPt.x - last_node->startPt.x) + abs(first_node->endPt.x - last_node->endPt.x))/2; - if (len <= growParam.minLTypeTreeLen) - feature_trees.erase(feature_trees.begin() + m); + double len = (abs(first_node->startPt.x - last_node->startPt.x) + abs(first_node->endPt.x - last_node->endPt.x)) / 2; + int tree_lines = feature_trees[m].eLineIdx - feature_trees[m].sLineIdx; + if (tree_lines > 0) + { + double mean_stepping = len / tree_lines; + double x_skip = mean_stepping * line_diff; + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + ((growParam.maxLineSkipNum <= 0) && (x_skip > growParam.maxSkipDistance)) || + (i == i_max - 1)) + { + feature_trees[m].treeState = TREE_STATE_DEAD; + if (len <= growParam.minLTypeTreeLen) + feature_trees.erase(feature_trees.begin() + m); + } + } } } } } + //nodeΪ1treeȥʱ޷ȥnodeΪ1tree + int m_max = (int)feature_trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + int tree_lines = feature_trees[m].eLineIdx - feature_trees[m].sLineIdx; + if (tree_lines == 0) + feature_trees.erase(feature_trees.begin() + m); + } + } void sg_lineFeaturesGrowing( @@ -720,18 +739,56 @@ void sg_lineFeaturesGrowing( if (TREE_STATE_ALIVE == trees[m].treeState) { int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x); - if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || - (true == isLastLine)) + if(line_diff > 0 ) { - trees[m].treeState = TREE_STATE_DEAD; - bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); - if (false == isValid) - trees.erase(trees.begin() + m); + int tree_lines = trees[m].eLineIdx - trees[m].sLineIdx; + if (tree_lines > 0) + { + double total_stepping = abs(trees[m].treeNodes[0].jumpPos.x - trees[m].treeNodes.back().jumpPos.x); + double mean_stepping = total_stepping / tree_lines; + double x_skip = line_diff * mean_stepping; + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + ((growParam.maxLineSkipNum <= 0) && (x_skip > growParam.maxSkipDistance)) || + (true == isLastLine)) + { + trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + trees.erase(trees.begin() + m); + } + } } } } } +//corner +void sg_cornerFeaturesGrowing( + std::vector>& cornerFeatures, + std::vector& trees, + SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)cornerFeatures.size(); i < i_max; i++) + { + std::vector& a_lineFeatures = cornerFeatures[i]; + bool isLastLine = (i == (i_max - 1)) ? true : false; + sg_lineFeaturesGrowing( + i, + isLastLine, + a_lineFeatures, + trees, + growParam); + } + //nodeΪ1treeȥʱ޷ȥnodeΪ1tree + int m_max = (int)trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + int tree_lines = trees[m].eLineIdx - trees[m].sLineIdx; + if(tree_lines == 0) + trees.erase(trees.begin() + m); + } +} + //featuretreesѰҺʵûкʵ㣬 false //ûʹȫƥ䡣һfeatureһƥϣƥɡûʹƥ䡣 bool _gapGrowing( diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index b67b056..74c0552 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3700,6 +3700,449 @@ void wd_getRingArcFeature( return; } +//ȡcornerֵʵֺʹô˺дŻ +void _searchCornerPeaks( + std::vector< SSG_pntDirAngle>& corners, + std::vector< SVzNL3DPosition>& vldPts, + const SSG_cornerParam cornerPara, + double cornerMergeScale, + std::vector< SSG_pntDirAngle>& cornerPeakP, + std::vector< SSG_pntDirAngle>& cornerPeakM + ) +{ + int cornerSize = (int)corners.size(); + std::vector< SSG_pntDirAngle> cornerPk_P; + std::vector< SSG_pntDirAngle> cornerPk_M; + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + for (int i = 0, i_max = cornerSize; 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) // + { + cornerPk_P.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPk_M.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) //½ + { + if (pre_data->corner > 0) + cornerPk_P.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + if (pre_data->corner < 0) + cornerPk_M.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = cornerMergeScale * cornerMergeScale; //2cornerScale + for (int i = 0, i_max = (int)cornerPk_P.size(); i < i_max; i++) + { + if (cornerPk_P[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPk_P[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPk_P[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 (cornerPk_P[i].corner < cornerPk_P[j].corner) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPk_P[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPk_P[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 (cornerPk_P[i].corner < cornerPk_P[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + cornerPeakP.push_back(cornerPk_P[i]); + } + + for (int i = 0, i_max = (int)cornerPk_M.size(); i < i_max; i++) + { + if (abs(cornerPk_M[i].corner) < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPk_M[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPk_M[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(cornerPk_M[i].corner) < abs(cornerPk_M[j].corner)) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPk_M[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPk_M[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(cornerPk_M[i].corner) < abs(cornerPk_M[j].corner)) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + cornerPeakM.push_back(cornerPk_M[i]); + } + + +} + +/// +/// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void wd_getLineCorerFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + std::vector& line_cornerFeatures //յ +) +{ + int dataSize = (int)lineData.size(); + //ȥ + std::vector< SVzNL3DPosition> vldPts; + //std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 1850) + int kkk = 1; + lineData[i].nPointIdx = i; //± + if (lineData[i].pt3D.z > 1e-4) + { + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + + if (a_run.len < 0) + { + a_run.start_zRising = true; + a_run.start = bIdx; + a_run.len = 1; + a_run.end_zRising = true; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + if (z_diff < cornerPara.minEndingGap_z) + { + a_run.len = bIdx - a_run.start + 1; + a_run.value = i; + } + else + { + bool next_zRising; + if (pre_z > lineData[i].pt3D.z) + { + a_run.end_zRising = true; + next_zRising = false; + } + else + { + a_run.end_zRising = false; + next_zRising = true; + } + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = bIdx; + a_run.start_zRising = next_zRising; + a_run.len = 1; + a_run.value = i; + a_run.end_zRising = true; + } + } + //vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + +#if 0 + //ñ־ + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int idx1 = segs[i].start; + int idx2 = segs[i].start + segs[i].len - 1; + lineData[idx1].nPointIdx |= 0x100000; + lineData[idx2].nPointIdx |= 0x200000; + } +#endif + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + //ν + int segSize = (int)segs.size(); + for (int segIdx = 0; segIdx < segSize; segIdx++) + { + int vPtIdxStart = segs[segIdx].start; + int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1; + for (int i = vPtIdxStart; i <= vPtIdxEnd; i++) + { + if (i == 1587) + int kkk = 1; + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= vPtIdxStart; 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 <= vPtIdxEnd; 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; + } + 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; + } + } + } + + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + double cornerMergeScale = cornerPara.scale * 2; + //ȡcornerֵ + _searchCornerPeaks( + corners, + vldPts, + cornerPara, + cornerMergeScale, + cornerPeakP, + cornerPeakM + ); + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + SSG_basicFeature1D a_feature; + if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else + a_feature.featureType = LINE_FEATURE_CORNER_V; + + int cornerPtIdx = cornerPeakP[i].pntIdx; + a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx }; + line_cornerFeatures.push_back(a_feature); + } + return; +} + +//ȡ͹ +void wd_getLineRaisedFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SSG_raisedFeatureParam raisedFeaturePara, //͹ + std::vector& line_raisedFeatures //͹ +) +{ + int dataSize = (int)lineData.size(); + int runIdx = 1; + SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx + double pre_z = -1; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 1733) + int kkk = 1; + lineData[i].nPointIdx = i; //± + if (lineData[i].pt3D.z > 1e-4) + { + if(pre_z > 0) + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + double y_diff = abs(lineData[i].pt3D.y - pre_y); + double k = z_diff / y_diff; + if ( (z_diff > raisedFeaturePara.minJumpZ) && (k > raisedFeaturePara.minK) + && (lineData[i].pt3D.z < pre_z)) + { + a_run.start = i; + a_run.start_zRising = true; + a_run.len = 1; + a_run.value = i; + a_run.end_zRising = false; + } + else if ((z_diff > raisedFeaturePara.minJumpZ) && (k > raisedFeaturePara.minK) + && (lineData[i].pt3D.z > pre_z)) + { + if (a_run.len > 0) + { + a_run.end_zRising = true; + int sIdx = a_run.start; + int eIdx = a_run.start + a_run.len - 1; + double width = abs(lineData[sIdx].pt3D.y - lineData[eIdx].pt3D.y); + if ((width >= raisedFeaturePara.widthRange.min) && + (width <= raisedFeaturePara.widthRange.max)) + { + SWD_segFeature a_raisedFeature; + memset(&a_raisedFeature, 0, sizeof(SWD_segFeature)); + a_raisedFeature.lineIdx = lineIdx; + a_raisedFeature.startPtIdx = sIdx; + a_raisedFeature.endPtIdx = eIdx; + a_raisedFeature.startPt = lineData[sIdx].pt3D; + a_raisedFeature.endPt = lineData[eIdx].pt3D; + a_raisedFeature.featureValue = width; + line_raisedFeatures.push_back(a_raisedFeature); + } + } + a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx + } + else + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + } + //vldPtSegIdx.push_back(runIdx); + pre_y = lineData[i].pt3D.y; + pre_z = lineData[i].pt3D.z; + } + } +} + //ʹö˵ֱߣ㵽ֱߵľ룬޵ķָ void split( SSG_RUN a_run, diff --git a/sourceCode/bagThreadPositioning.cpp b/sourceCode/bagThreadPositioning.cpp new file mode 100644 index 0000000..1339b7c --- /dev/null +++ b/sourceCode/bagThreadPositioning.cpp @@ -0,0 +1,220 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "bagThreadPositioning_Export.h" +#include +#include + +//version 1.0.0 : base version release to customer +std::string m_strVersion = "1.0.0"; +const char* wd_bagThreadPositioningVersion(void) +{ + return m_strVersion.c_str(); +} +#if 1 +//ͷλüⶨλ +void wd_bagThreadPositioning( + std::vector< std::vector>& scanLines, + bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_outlierFilterParam filterParam, //˲ + const SSG_cornerParam cornerPara, //V + const SSG_raisedFeatureParam raisedFeaturePara,//β͹ + const SSG_treeGrowParam growParam, // + std::vector& bagThreadInfo, + 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(); + //жݸʽǷΪgrid㷨ֻܴgridݸʽ + bool isGridData = true; + for (int line = 0; line < lineNum; line++) + { + if (linePtNum != (int)scanLines[line].size()) + { + isGridData = false; + break; + } + } + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + //ˮƽɨ + std::vector< std::vector> data_lines_h; //ˮƽɨ + data_lines_h.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + data_lines_h[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + //scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + data_lines_h[j][line] = scanLines[line][j]; + data_lines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y; + data_lines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + int lineNum_h = linePtNum; + int linePtNum_h = (int)data_lines_h[0].size(); + for (int line = 0; line< lineNum_h; line++) + { + for (int j = 0, j_max = (int)data_lines_h[line].size(); j < j_max; j++) + data_lines_h[line][j].nPointIdx = j; + } + + std::vector> cornerFeatures; + std::vector> raisedFeatures; + if (false == isHorizonScan) + { + //ֱɨVͲۺβ + for (int line = 0; line < lineNum; line++) + { + if ((line == 577) || (line == 932)) + int kkk = 1; + std::vector& lineData = scanLines[line]; + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + //ȡVͲ + std::vector line_cornerFeatures; + int dataSize = (int)lineData.size(); + wd_getLineCorerFeature( + lineData, + line, + cornerPara, + line_cornerFeatures //յ + ); + cornerFeatures.push_back(line_cornerFeatures); + //ȡ͹ + std::vector line_raisedFeatures; + wd_getLineRaisedFeature( + lineData, + line, + raisedFeaturePara, //͹ + line_raisedFeatures //͹ + ); + raisedFeatures.push_back(line_raisedFeatures); + } +#if 0 + //ˮƽɨӱ + std::vector> jumpdFeatures; +#endif + } + else + { + //ˮƽɨVͲۺβ + for (int line = 0; line < lineNum_h; line++) + { + if (line == 329) + int kkk = 1; + std::vector& lineData = data_lines_h[line]; + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam); + //ȡVͲ + std::vector line_cornerFeatures; + int dataSize = (int)lineData.size(); + wd_getLineCorerFeature( + lineData, + line, + cornerPara, + line_cornerFeatures //յ + ); + cornerFeatures.push_back(line_cornerFeatures); + //ȡ͹ + std::vector line_raisedFeatures; + wd_getLineRaisedFeature( + lineData, + line, + raisedFeaturePara, //͹ + line_raisedFeatures //͹ + ); + raisedFeatures.push_back(line_raisedFeatures); + } + //ֱɨӱ + } + + // + std::vector< SSG_featureTree> cornerGrowTrees; + sg_cornerFeaturesGrowing( + cornerFeatures, + cornerGrowTrees, + growParam); + std::vector raisedFeatureGrowTrees; + wd_getSegFeatureGrowingTrees( + raisedFeatures, + raisedFeatureGrowTrees, + growParam); + + if (cornerGrowTrees.size() == 0) + { + *errCode = SG_ERR_ZERO_OBJECTS; + return; + } + + int cornerTreeNum = (int)cornerGrowTrees.size(); + int raisedTreeNum = (int)raisedFeatureGrowTrees.size(); + //ʾ + //ԭʼݵ0תʹ + for (int line = 0; line < lineNum; line++) + for (int j = 0; j < linePtNum; j++) + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + //ñ־debug + for (int i = 0; i < cornerTreeNum; i++) + { + int nodeNum = (int)cornerGrowTrees[i].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x; + ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y; + } + else + { + lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y; + ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x; + } + scanLines[lineIdx][ptIdx].nPointIdx = 1; + } + } + for (int i = 0; i < raisedTreeNum; i++) + { + int nodeNum = (int)raisedFeatureGrowTrees[i].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx; + for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++) + { + ptIdx = m; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + } + else + { + ptIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx; + for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++) + { + lineIdx = m; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + } + } + } + return; +} +#endif + + + diff --git a/sourceCode/bagThreadPositioning_Export.h b/sourceCode/bagThreadPositioning_Export.h new file mode 100644 index 0000000..43430b3 --- /dev/null +++ b/sourceCode/bagThreadPositioning_Export.h @@ -0,0 +1,28 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + SVzNL3DPoint threadPos; //ͷλ + SVzNL3DPoint operatePos; //µλ + double rotateAngle; //ˮƽת +}SSX_bagThreadInfo; //ͷϢ + + +//汾 +SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void); + +//ͷλüⶨλ +SG_APISHARED_EXPORT void wd_bagThreadPositioning( + std::vector< std::vector>& scanLines, + bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_outlierFilterParam filterParam, //˲ + const SSG_cornerParam cornerPara, //V + const SSG_raisedFeatureParam raisedFeaturePara,//β͹ + const SSG_treeGrowParam growParam, // + std::vector& bagThreadInfo, + int* errCode);