From be5dbbccfbe755c0adc24053d983e88a2c2b6d15 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 28 Jul 2025 09:22:07 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=A9=A1=E8=83=B6?= =?UTF-8?q?=E6=89=8B=E5=A5=97=E5=AE=9A=E4=BD=8D=E6=89=8B=E5=A5=97=E7=8E=AF?= =?UTF-8?q?=E7=9A=84=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SG_Algorithm.sln | 26 + bagPositioning_test/bagPositioning_test.cpp | 251 +++--- baseAlgorithm/baseAlgorithm.vcxproj | 1 + glovePositioning/glovePositioning.vcxproj | 174 ++++ .../glovePositioning_test.cpp | 809 ++++++++++++++++++ .../glovePositioning_test.vcxproj | 159 ++++ sourceCode/SG_baseAlgo_Export.h | 6 + sourceCode/SG_baseDataType.h | 23 +- sourceCode/SG_errCode.h | 9 +- sourceCode/SG_featureGrow.cpp | 2 + sourceCode/SG_lineFeature.cpp | 309 ++----- sourceCode/WD_glovePositioning.cpp | 297 +++++++ sourceCode/WD_glovePositioning_Export.h | 33 + 13 files changed, 1702 insertions(+), 397 deletions(-) create mode 100644 glovePositioning/glovePositioning.vcxproj create mode 100644 glovePositioning_test/glovePositioning_test.cpp create mode 100644 glovePositioning_test/glovePositioning_test.vcxproj create mode 100644 sourceCode/WD_glovePositioning.cpp create mode 100644 sourceCode/WD_glovePositioning_Export.h diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index b41a680..740736d 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -76,6 +76,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection_test", "Q {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} = {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning", "glovePositioning\glovePositioning.vcxproj", "{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning_test", "glovePositioning_test\glovePositioning_test.vcxproj", "{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}" + ProjectSection(ProjectDependencies) = postProject + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} = {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -204,6 +214,22 @@ Global {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.Build.0 = Release|x64 {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.ActiveCfg = Release|Win32 {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.Build.0 = Release|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x64.ActiveCfg = Debug|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x64.Build.0 = Debug|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x86.ActiveCfg = Debug|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x86.Build.0 = Debug|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x64.ActiveCfg = Release|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x64.Build.0 = Release|x64 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x86.ActiveCfg = Release|Win32 + {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x86.Build.0 = Release|Win32 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x64.ActiveCfg = Debug|x64 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x64.Build.0 = Debug|x64 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x86.ActiveCfg = Debug|Win32 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x86.Build.0 = Debug|Win32 + {7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.ActiveCfg = Release|x64 + {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 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 0bf6ff3..9fdb34c 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -45,7 +45,7 @@ SVzNLPointXYZRGBA _ptRotate_RGBD(SVzNLPointXYZRGBA pt3D, double matrix3d[9]) #define DATA_VER_FROM_CUSTOM 2 #define VZ_LASER_LINE_PT_MAX_NUM 4096 SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV, - int* dataCalib, int* scanMaxStamp, int* canClockUnit) + int* dataCalib, int* scanMaxStamp, int* canClockUnit, bool removeNullLines) { std::ifstream inputFile(fileName); std::string linedata; @@ -64,175 +64,122 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, 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 = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (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 = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; } if (_scanLines == NULL) return NULL; - int ptNum = 0; - int lineIdx = -1; + int lineIdx = 0; int ptIdx = 0; - SVzNLPointXYZRGBA* p3DPoint = NULL; - if (dataFileVer == DATA_VER_NEW) + int ptNum = 0; + std::vector< SVzNLPointXYZRGBA> a_line; + int vldPtNum = 0; + unsigned int timeStamp = 0; + while (getline(inputFile, linedata)) { - while (getline(inputFile, linedata)) + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) { - 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 = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum); - memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum); - } - else - p3DPoint = NULL; - _scanLines[lineIdx].nPointCnt = 0; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPoint = p3DPoint; - - } - else if (0 == strncmp("{", linedata.c_str(), 1)) - { - float X, Y, Z; - int imageY = 0; - 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].nPointCnt; - if (id < ptNum) - { - if (lineIdx == 537) - int kkk = 1; - p3DPoint[id].x = X; - p3DPoint[id].y = Y; - p3DPoint[id].z = Z; - int nr = (int)(r * 255); - int ng = (int)(g * 255); - int nb = (int)(b * 255); - nb <<= 8; - nb += ng; - nb <<= 8; - nb += nr; - p3DPoint[id].nRGB = nb; - _scanLines[lineIdx].nPointCnt = id + 1; - } - } + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; } - - } - else if (dataFileVer == DATA_VER_OLD) - { - while (getline(inputFile, linedata)) + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) { - if (0 == strncmp("DataElements_", linedata.c_str(), 13)) + 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 curr_timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &curr_timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + int recvPtNum = (int)a_line.size(); + if ( (recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines))) { - 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 = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM); - memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM); - _scanLines[lineIdx].nPointCnt = 0; + 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++) + p3DPoint[m] = a_line[m]; + lineIdx++; } - else if (0 == strncmp("(", linedata.c_str(), 1)) + //new Line + timeStamp = curr_timeStamp; + vldPtNum = 0; + a_line.clear(); + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + 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); { - 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].nPointCnt; - if (id < VZ_LASER_LINE_PT_MAX_NUM) - { - p3DPoint[id].x = X; - p3DPoint[id].y = Y; - p3DPoint[id].z = Z; - p3DPoint[id].nRGB = 0; - _scanLines[lineIdx].nPointCnt = id + 1; - } + if (lineIdx == 537) + int kkk = 1; + + SVzNLPointXYZRGBA a_pt; + + a_pt.x = X; + a_pt.y = Y; + a_pt.z = Z; + int nr = (int)(r * 255); + int ng = (int)(g * 255); + int nb = (int)(b * 255); + nb <<= 8; + nb += ng; + nb <<= 8; + nb += nr; + a_pt.nRGB = nb; + if (a_pt.z > 1e-4) + vldPtNum++; + a_line.push_back(a_pt); } } } + //last line + int recvPtNum = (int)a_line.size(); + if ((recvPtNum == 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++) + p3DPoint[m] = a_line[m]; + lineIdx++; + } + + if (scanLineNum) + *scanLineNum = lineIdx; inputFile.close(); return _scanLines; } @@ -2578,7 +2525,7 @@ int main() 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,1}, + {2,4}, {1,5}, {1,1}, {1,3}, {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -2602,7 +2549,7 @@ int main() SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 15; grp <= 15; grp++) + for (int grp = 14; grp <= 15; grp++) { if (grp < 10) { @@ -2865,17 +2812,19 @@ int main() int maxTimeStamp = 0; int clockPerSecond = 0; sprintf_s(_scan_file, "%s%d-RGB点云.txt", dataPath[grp], fidx); + bool removeNullLines = true; SVzNLXYZRGBDLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZRGB( _scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, - &clockPerSecond); + &clockPerSecond, + removeNullLines); if (laser3DPoints == NULL) continue; - algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + algoParam.filterParam.continuityTh = 20;// 10.0; // 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 algoParam.filterParam.outlierTh = 5; long t1 = GetTickCount64(); diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index de7d070..e68acff 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -163,6 +163,7 @@ + diff --git a/glovePositioning/glovePositioning.vcxproj b/glovePositioning/glovePositioning.vcxproj new file mode 100644 index 0000000..c18b791 --- /dev/null +++ b/glovePositioning/glovePositioning.vcxproj @@ -0,0 +1,174 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {4060ce45-6235-4ca7-b64c-6e4e5cddc34f} + glovePositioning + 10.0 + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + false + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + + Level3 + true + WIN32;_DEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/glovePositioning_test/glovePositioning_test.cpp b/glovePositioning_test/glovePositioning_test.cpp new file mode 100644 index 0000000..3b3a3c7 --- /dev/null +++ b/glovePositioning_test/glovePositioning_test.cpp @@ -0,0 +1,809 @@ +// glovePositioning_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "WD_glovePositioning_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 _outputRGBDScanDataFile_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector< SSG_peakRgnInfo> objPoints) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (objPoints.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 (objPoints.size() > 0) + { + int linePtNum = (int)objPoints.size(); + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + + rgb = { 0, 0, 255 }; + size = 25; + for (int i = 0; i < linePtNum; i++) + { + float x = (float)objPoints[i].centerPos.x; + float y = (float)objPoints[i].centerPos.y; + float z = (float)objPoints[i].centerPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //加一个点，用于跳过显示工具bug + float x = (float)objPoints[0].centerPos.x; + float y = (float)objPoints[0].centerPos.y; + float z = (float)objPoints[0].centerPos.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] = { + {1,35} + }; + +#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\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_calib_datafile, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + if (laser3DPoints) + { + SSG_planeCalibPara calibPara = wd_getBaseCalibPara( + laser3DPoints, + lineNum); + //结果进行验证 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_lineDataR(&laser3DPoints[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\\项目\\工件端部圆点二维码\\LaserLine3_calib_data.txt"); + _outputScanDataFile_ptr(_out_file, laser3DPoints, lineNum); + 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, "%sglove_%d_scanLine.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; + //行处理 + //调平，去除地面 + wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); + } + // + SSG_gloveArcParam arcFeatureParam; + arcFeatureParam.cornerTh = 120; //转角大于150 + arcFeatureParam.scale_angle = 1.0; + arcFeatureParam.scale_corner = 5.0; + 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 + + int errCode = 0; + std::vector objOps; + wd_getGloveGrabPostion( + scanLines, + arcFeatureParam, + growParam, + objOps, + &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); + _outputRGBDScanDataFile_RGBD(_scan_file, scanLines, objOps); + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/glovePositioning_test/glovePositioning_test.vcxproj b/glovePositioning_test/glovePositioning_test.vcxproj new file mode 100644 index 0000000..9872dae --- /dev/null +++ b/glovePositioning_test/glovePositioning_test.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {7c1efab7-06cc-4b7d-81bb-64c6fa5996fd} + glovePositioningtest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + false + $(VC_ExecutablePath_x64);$(CommonExecutablePath) + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + + 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 + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;glovePositioning.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;glovePositioning.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index c07da9c..d318ccc 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -60,6 +60,12 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature( const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 SSG_lineFeature* line_features); +SG_APISHARED_EXPORT void wd_getLineGloveArcs( + std::vector& lineData, + int lineIdx, + const SSG_gloveArcParam arcPara, + std::vector& gloveArcs); + /// ȡϵԲ SG_APISHARED_EXPORT void sg_getLineUpperSemiCircleFeature( SVzNL3DPosition* lineData, diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 6721679..a3fcf08 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -92,6 +92,7 @@ typedef struct int featureType; SVzNL2DPoint jumpPos2D; SVzNL3DPoint jumpPos; + double featureValue; }SSG_basicFeature1D; typedef struct @@ -156,6 +157,13 @@ typedef struct double jumpCornerTh_2; }SSG_cornerParam; +typedef struct +{ + double scale_angle; //㷽ǵĴڱ + double scale_corner; //㷽תǵĴڱ + double cornerTh; //սޣڴޣΪЧArc +}SSG_gloveArcParam; + typedef struct { double H_len; //ֱˮƽεij @@ -208,6 +216,7 @@ typedef struct int treeType; int sLineIdx; int eLineIdx; + double tree_value; SSG_ROIRectD roi; std::vector< SSG_basicFeature1D> treeNodes; }SSG_featureTree; @@ -388,4 +397,16 @@ typedef struct double planeCalib[9]; //ת󣬽Ƶƽ double planeHeight;//ƽĸ߶ȣȥ double invRMatrix[9]; //ת󣬻صԭϵ -}SSG_planeCalibPara; \ No newline at end of file +}SSG_planeCalibPara; + +typedef struct +{ + int pntIdx; + double forwardAngle; //ǰ + double backwardAngle; // + double corner; //ս + double forwardDiffZ; + double backwardDiffZ; + double pre_stepDist; + double post_stepDist; +}SSG_pntDirAngle; diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 188b622..6dc6cfd 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -1,7 +1,8 @@ #pragma once #define SG_ERR_3D_DATA_INVLD -1000 -#define SG_ERR_FOUND_NO_TOP_PLANE -1001 -#define SG_ERR_NOT_GRID_FORMAT -1002 -#define SG_ERR_LABEL_INFO_ERROR -1003 -#define SG_ERR_INVLD_SORTING_MODE -1004 +#define SG_ERR_3D_DATA_NULL -1001 +#define SG_ERR_FOUND_NO_TOP_PLANE -1002 +#define SG_ERR_NOT_GRID_FORMAT -1003 +#define SG_ERR_LABEL_INFO_ERROR -1004 +#define SG_ERR_INVLD_SORTING_MODE -1005 diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index d286208..2cfd789 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -75,6 +75,7 @@ bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vect { a_tree.eLineIdx = lineIdx; a_tree.treeNodes.push_back(a_feature); + a_tree.tree_value += a_feature.featureValue; return true; } } @@ -431,6 +432,7 @@ void sg_lineFeaturesGrowing( a_newTree.treeType = a_feature.featureType; a_newTree.sLineIdx = lineIdx; a_newTree.eLineIdx = lineIdx; + a_newTree.tree_value = a_feature.featureValue; trees.push_back(a_newTree); } } diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index fc7c9e0..a011912 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -11,16 +11,6 @@ typedef struct SVzNL3DPosition endPt; }SSG_jump; -typedef struct -{ - int pntIdx; - double forwardAngle; //ǰ - double backwardAngle; // - double corner; //ս - double forwardDiffZ; - double backwardDiffZ; -}SSG_pntDirAngle; - typedef struct { int flag; @@ -1448,22 +1438,18 @@ void sg_getLineCornerFeature( return; } -#if 0 -void sg_getLineCorners( +//׵ĻɨߵPeakǰһΧڣǰǴı仯Χ150ȣֵ +void wd_getLineGloveArcs( std::vector& lineData, int lineIdx, - const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 - SSG_lineFeature* line_features) + const SSG_gloveArcParam arcPara, + std::vector& gloveArcs) { - line_features->lineIdx = lineIdx; - if (lineIdx == 538) - int kkk = 1; //ȥ std::vector< SVzNL3DPosition> vldPts; - std::vector segs; - //޸segĶ塣seg˵޵ĵ - int segStart = -1, segEnd = -1; - int dataSize = + int dataSize = (int)lineData.size(); + if (dataSize < 10) //̫٣޷ + return; for (int i = 0; i < dataSize; i++) { if ((lineIdx == 399) && (i == 568)) @@ -1472,83 +1458,53 @@ void sg_getLineCorners( 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; + + //ͬһ߶¼ǰǺͺǣԼֵ㡣 + std::vector< SSG_pntDirAngle> corners; //ǰ corners.resize(vldPts.size()); - for (int i = 0, i_max = vldPts.size(); i < i_max; i++) + std::vector< int> localMax; //ѰҼֵ + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) { - if ((lineIdx == 399) && (i == 419)) + if ((lineIdx == 0) && (i == 2585)) int kkk = 1; + + double pre_stepDist = 0; //ǰѰ + bool isMax = true; 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) + if ((true == isMax) &&(vldPts[i].pt3D.z <= vldPts[j].pt3D.z)) + isMax = false; + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (j == (i - 1)) + pre_stepDist = dist; + if (dist >= arcPara.scale_angle) { pre_i = j; break; } } //Ѱ + double post_stepDist = 0; 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) + if (( true == isMax) && (vldPts[i].pt3D.z < vldPts[j].pt3D.z)) + isMax = false; + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (j == (i + 1)) + post_stepDist = dist; + if (dist >= arcPara.scale_angle) { post_i = j; break; } } + if (true == isMax) + localMax.push_back(i); + //ս if ((pre_i < 0) || (post_i < 0)) { @@ -1558,6 +1514,8 @@ void sg_getLineCorners( corners[i].corner = 0; corners[i].forwardDiffZ = 0; corners[i].backwardDiffZ = 0; + corners[i].pre_stepDist = pre_stepDist; + corners[i].post_stepDist = post_stepDist; } else { @@ -1568,189 +1526,58 @@ void sg_getLineCorners( corners[i].pntIdx = i; corners[i].forwardAngle = forwardAngle; corners[i].backwardAngle = backwardAngle; - corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].corner = (forwardAngle - backwardAngle); 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].pre_stepDist = pre_stepDist; + corners[i].post_stepDist = post_stepDist; } } - //սǼֵ - 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 = vldPts.size(); i < i_max; i++) + //ڼֵǰ + for (int i = 0, i_max = (int)localMax.size(); i < i_max; i++) { - if (i == 275) + if (i == 31) int kkk = 1; - SSG_pntDirAngle* curr_data = &corners[i]; - if (curr_data->pntIdx < 0) + int peakIdx = localMax[i]; + + //ǰǰ + double pre_sumLen = 0; + double pre_angleMax = 0; + for (int j = peakIdx - 1; j >= 0; j--) { - 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; - } - //ע⣺һΪλ - - //Сֵ㣨嶥 - //ֵȽϣڳ߶ȴѰҾֲֵ - double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale - for (int i = 0, i_max = 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) //߶ȴ + if (pre_angleMax < corners[j].forwardAngle) + pre_angleMax = corners[j].forwardAngle; + pre_sumLen += corners[j].post_stepDist; + if (pre_sumLen >= arcPara.scale_corner) break; - - if (cornerPeakP[i].corner < cornerPeakP[j].corner) - { - isPeak = false; - break; - } } - // - if (true == isPeak) + //ǰ + double post_angleMax = corners[peakIdx].forwardAngle; + double post_sumLen = corners[peakIdx].post_stepDist; + for (int j = peakIdx + 1; j < (int)vldPts.size(); j++) { - 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; - } - } + post_sumLen += corners[j].post_stepDist; + if (post_sumLen >= arcPara.scale_corner) + break; + if (post_angleMax > corners[j].forwardAngle) + post_angleMax = corners[j].forwardAngle; } - if (true == isPeak) + //ת + double totalTurn = pre_angleMax - post_angleMax; + if (totalTurn >= arcPara.cornerTh) //ЧARC { SSG_basicFeature1D a_feature; - if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) - a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; - else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) - a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; - else - a_feature.featureType = LINE_FEATURE_CORNER_V; - - - a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; - a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx }; - line_features->features.push_back(a_feature); + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + a_feature.jumpPos = vldPts[peakIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, vldPts[peakIdx].nPointIdx }; + a_feature.featureValue = totalTurn; + gloveArcs.push_back(a_feature); } } - - //ӿʼͽ߽ - //segǷҪϲϲ - for (int i = 0, i_max = segs.size(); i < i_max - 1; i++) - { - SSG_RUN* nxt_seg = &segs[i + 1]; - SSG_RUN* curr_seg = &segs[i]; - - int idx_1 = curr_seg->start + curr_seg->len - 1; - int idx_2 = nxt_seg->start; - double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y); - double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z); - if ((y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //ϲ - { - int idx_end = nxt_seg->start + nxt_seg->len - 1; - nxt_seg->start = curr_seg->start; - nxt_seg->len = idx_end - curr_seg->start + 1; - curr_seg->value = 0; - } - } - for (int i = 0, i_max = segs.size(); i < i_max; i++) - { - if (0 == segs[i].value) //ϲ - continue; - - int idx_1 = segs[i].start; - int idx_2 = segs[i].start + segs[i].len - 1; - - SSG_basicFeature1D an_edge; - memset(&an_edge, 0, sizeof(SSG_basicFeature1D)); - an_edge.featureType = LINE_FEATURE_LINE_ENDING_0; - an_edge.jumpPos = lineData[idx_1].pt3D; - an_edge.jumpPos2D = { lineIdx, idx_1 }; - line_features->endings.push_back(an_edge); - //line_features.insert(line_features.begin(), an_edge); //ͷ - //β - an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; - an_edge.jumpPos = lineData[idx_2].pt3D; - an_edge.jumpPos2D = { lineIdx, idx_2 }; - line_features->endings.push_back(an_edge); - } return; } -#endif + /// /// ȡϵļֵ㣨ֵͼСֵ㣩 /// diff --git a/sourceCode/WD_glovePositioning.cpp b/sourceCode/WD_glovePositioning.cpp new file mode 100644 index 0000000..ea8519f --- /dev/null +++ b/sourceCode/WD_glovePositioning.cpp @@ -0,0 +1,297 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "WD_glovePositioning_Export.h" +#include +#include + +//plyʽݻָɨеʽӶ水нд +void wd_getScanLines( + std::vector& scanData, + std::vector< std::vector>& scanLines) +{ + std::vector a_line; + for (int i = 0, i_max = scanData.size(); i < i_max; i++) + { + SVzNL3DPoint a_pt = scanData[i]; + SVzNL3DPosition a_idxPt; + a_idxPt.nPointIdx = 0; + a_idxPt.pt3D = a_pt; + if (a_line.size() == 0) + a_line.push_back(a_idxPt); + else + { + SVzNL3DPosition pre_pt = a_line.back(); + double x_diff = abs(pre_pt.pt3D.x - a_idxPt.pt3D.x); + if (x_diff > 1e-4) + { + scanLines.push_back(a_line); + a_line.clear(); + a_line.push_back(a_idxPt); + } + else + a_line.push_back(a_idxPt); + } + } + //last line + if(a_line.size() > 0) + scanLines.push_back(a_line); + return; +} + +void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +void wd_getGloveGrabPostion( + std::vector< std::vector>& scanLines, + const SSG_gloveArcParam arcPara, + SSG_treeGrowParam growParam, + 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> arcFeatures; + for(int line = 0; line < lineNum; line ++) + { + std::vector lineArcFeatures; + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + wd_getLineGloveArcs( + lineData, + line, + arcPara, + lineArcFeatures); + arcFeatures.push_back(lineArcFeatures); + } + + 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> arcFeatures_h; + int lineNum_h = (int)hLines.size(); + for (int line = 0; line < lineNum_h; line++) + { + std::vector lineArcFeatures; + std::vector& lineData = hLines[line]; + wd_getLineGloveArcs( + lineData, + line, + arcPara, + lineArcFeatures); + arcFeatures_h.push_back(lineArcFeatures); + } + + // + //ֱ߷ + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineArcFeature = arcFeatures[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineArcFeature, + 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_lineArcFeature = arcFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineArcFeature, + h_trees, + growParam); + } + //ƽתǵ + double maxTurn_v = 0; + int maxTurn_v_id = -1; + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + int nodeSize = (int)v_trees[i].treeNodes.size(); + v_trees[i].tree_value = v_trees[i].tree_value / (double)nodeSize; + if (maxTurn_v_id < 0) + { + maxTurn_v_id = i; + maxTurn_v = v_trees[i].tree_value; + } + else + { + if (maxTurn_v < v_trees[i].tree_value) + { + maxTurn_v_id = i; + maxTurn_v = v_trees[i].tree_value; + } + } + } + double maxTurn_h = 0; + int maxTurn_h_id = -1; + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + int nodeSize = (int)h_trees[i].treeNodes.size(); + h_trees[i].tree_value = h_trees[i].tree_value / (double)nodeSize; + if (maxTurn_h_id < 0) + { + maxTurn_h_id = i; + maxTurn_h = h_trees[i].tree_value; + } + else + { + if (maxTurn_h < h_trees[i].tree_value) + { + maxTurn_h_id = i; + maxTurn_h = h_trees[i].tree_value; + } + } + } + if ((maxTurn_v_id >= 0) && (maxTurn_h_id < 0)) + { + SSG_featureTree bestTree = v_trees[maxTurn_v_id]; + v_trees.clear(); + v_trees.push_back(bestTree); + } + else if ((maxTurn_v_id < 0) && (maxTurn_h_id >= 0)) + { + SSG_featureTree bestTree = h_trees[maxTurn_h_id]; + h_trees.clear(); + h_trees.push_back(bestTree); + } + else if ((maxTurn_v_id >= 0) && (maxTurn_h_id >= 0)) + { + if (maxTurn_h <= maxTurn_v) + { + h_trees.clear(); + SSG_featureTree bestTree = v_trees[maxTurn_v_id]; + v_trees.clear(); + v_trees.push_back(bestTree); + } + else + { + v_trees.clear(); + SSG_featureTree bestTree = h_trees[maxTurn_h_id]; + h_trees.clear(); + h_trees.push_back(bestTree); + } + } + + //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/WD_glovePositioning_Export.h b/sourceCode/WD_glovePositioning_Export.h new file mode 100644 index 0000000..be10303 --- /dev/null +++ b/sourceCode/WD_glovePositioning_Export.h @@ -0,0 +1,33 @@ +#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 +{ + SSG_outlierFilterParam filterParam; + SSG_cornerParam cornerParam; + SSG_treeGrowParam growParam; +}SG_glovePositioningParam; + +SG_APISHARED_EXPORT void wd_getScanLines( + std::vector& scanData, + std::vector< std::vector>& scanLines); + +SG_APISHARED_EXPORT void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +SG_APISHARED_EXPORT void wd_getGloveGrabPostion( + std::vector< std::vector>& scanLines, + const SSG_gloveArcParam arcPara, + SSG_treeGrowParam growParam, + std::vector& objOps, + int* errCode);