diff --git a/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp index b107625..4e43406 100644 --- a/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp +++ b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp @@ -268,9 +268,28 @@ SSX_BQAssemblyInfo _readPoseInfo(char* fileName) return pose; } +// 检测并跳过UTF-8 BOM头的工具函数 +void skipUTF8BOM(std::ifstream& file) { + // 存储BOM的3个字节（初始化为0） + char bom[3] = { 0 }; + // 读取文件前3个字节 + file.read(bom, 3); + + // 检测是否是UTF-8 BOM（0xEF 0xBB 0xBF） + bool isBOM = (bom[0] == (char)0xEF) && + (bom[1] == (char)0xBB) && + (bom[2] == (char)0xBF); + + // 不是BOM则将文件指针重置到开头，保证正常读取 + if (!isBOM) { + file.seekg(0); // 重置文件指针到起始位置 + } + // 是BOM则文件指针已在第4字节，无需处理 +} void _readMarkData(char* fileName, std::vector& marks) { - std::ifstream inputFile(fileName); + std::ifstream inputFile(fileName, std::ios::in | std::ios::binary); + skipUTF8BOM(inputFile); std::string linedata; while (getline(inputFile, linedata)) { @@ -522,55 +541,86 @@ void _wirteVerifyResult(char* fileName, #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CORNER 0 #define TEST_COMPUTE_POSE 1 -#define TEST_GROUP 2 +#define TEST_GROUP 3 int main() { const char* dataPath[TEST_GROUP] = { "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/", //0 "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/", + "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/现场_张奔_20260106/", }; SVzNLRange fileIdx[TEST_GROUP] = { - {2,33}, + {2,33},{2,33}, {1,14} }; #ifdef TEST_COMPUTE_POSE double R_3D_mark[9] = { - 9.7414217523834201e-01, -6.4917977205419487e-03, -2.2584259780711641e-01, - 6.4677677434932046e-03, 9.9997872563826395e-01, - 8.4631630596682995e-04, - 2.2584328726426117e-01, - 6.3626506196937213e-04, 9.7416343842521791e-01 + 9.7432217968400503e-01, - 6.5983238824482409e-03, + - 2.2506166332308031e-01, 6.5293199817128209e-03, + 9.9997813155105764e-01, - 1.0509044828439812e-03, + 2.2506367578173436e-01, - 4.4558006908867375e-04, + 9.7434395533713669e-01 }; double T_3D_mark[3] = { - 2.0118012741488724e+02, -3.4890651052684677e+02,-2.9090717029324466e+01 + 2.0100138599065690e+02, - 3.4778670146875356e+02, + - 2.8208030634996298e+01 }; double R_mark_3D[9] = { - 9.7414217523834201e-01, 6.4677677434932046e-03, 2.2584328726426117e-01, - -6.4917977205419487e-03, 9.9997872563826395e-01,-6.3626506196937213e-04, - -2.2584259780711641e-01, -8.4631630596682995e-04, 9.7416343842521791e-01 + 9.7432217968400503e-01, 6.5293199817128209e-03, 2.2506367578173436e-01, + - 6.5983238824482409e-03, 9.9997813155105764e-01, + - 4.4558006908867375e-04, - 2.2506166332308031e-01, + - 1.0509044828439812e-03, 9.7434395533713669e-01 }; double T_mark_3D[3] = { - -1.8715145749760768e+02, 3.5018659904922669e+02, 7.3478870260969714e+01 + -1.8722069479737289e+02, 3.4909279922237783e+02, + 7.2356539798828123e+01 + }; + + double R_ip21_ip20[9] = { + -9.0136463861749749e-01, 1.0494706583686632e-03, + - 4.3305968048444010e-01, 3.8989008937143142e-03, + - 9.9993686885077060e-01, - 1.0538353037260263e-02, + - 4.3304340062142632e-01, - 1.1187355552326124e-02, + 9.0130364264989893e-01 + }; + double T_ip21_ip20[3] = { + 7.4953277493063717e+02, 2.8014434657528108e+02, 1.7494775044638942e+02 + }; + double R_ip20_ip21[9] = { + -9.0136463861749749e-01, 3.8989008937143142e-03, + - 4.3304340062142632e-01, 1.0494706583686632e-03, + - 9.9993686885077060e-01, - 1.1187355552326124e-02, + - 4.3305968048444010e-01, - 1.0538353037260263e-02, + 9.0130364264989893e-01 + }; + double T_ip20_ip21[3] = { + 7.5027005254846563e+02, 2.8129725077327970e+02, 1.6986363929894415e+02 }; double R_3D_mark_20[9] = { - -9.7551111071544561e-01, 1.0398329224714965e-02,-2.1970422758802807e-01, - - 8.3582333026743106e-03,- 9.9991291186786824e-01, -1.0213158961794689e-02, - - 2.1979129374653508e-01, -8.1267108509691516e-03, 9.7551306693655782e-01, + -9.7565715189866242e-01, 1.0207261472102205e-02, + - 2.1906376642939016e-01, - 8.0279186459975786e-03, + - 9.9990905961091314e-01, - 1.0836283045053035e-02, + - 2.1915445345966281e-01, - 8.8138709577232511e-03, + 9.7565036832235041e-01 }; double T_3D_mark_20[3] = { - 5.7873032211966097e+02, 6.3216378913452741e+02, 6.5977504397803401e+01, + 5.7984427943242531e+02, 6.3069724580763238e+02, 6.1343276694911815e+01 }; double R_mark_3D_20[9] = { - -9.7551111071544561e-01, -8.3582333026743106e-03, -2.1979129374653508e-01, - 1.0398329224714965e-02, -9.9991291186786824e-01, -8.1267108509691516e-03, - -2.1970422758802807e-01, -1.0213158961794689e-02, 9.7551306693655782e-01 + -9.7565715189866242e-01, - 8.0279186459975786e-03, + - 2.1915445345966281e-01, 1.0207261472102205e-02, + - 9.9990905961091314e-01, - 8.8138709577232511e-03, + - 2.1906376642939016e-01, - 1.0836283045053035e-02, + 9.7565036832235041e-01 }; double T_mark_3D_20[3] = { - 5.8434291282050799e+02, 6.2662708685011432e+02, 6.9243970007470736e+01 + 5.8423605667286893e+02, 6.2526193950634604e+02, 7.4007695164819168e+01 }; - for (int grp = 0; grp <= 0; grp++) + for (int grp = 2; grp <= 2; grp++) { char _scan_file[256]; std::vector> originMarksBuff; @@ -582,18 +632,20 @@ int main() { //fidx =4; char _scan_file[256]; - sprintf_s(_scan_file, "%sresult/LaserLine%d_corner_info.txt", dataPath[grp], fidx); - SSX_BQAssemblyInfo originPose_raw = _readPoseInfo(_scan_file); - SSX_BQAssemblyInfo originPose; - originPose.O = _pointRT(originPose_raw.O, R_3D_mark, T_3D_mark); - SSX_BQAssemblyInfo originPose_check; - originPose_check.O = _pointRT(originPose.O, R_mark_3D, T_mark_3D); + sprintf_s(_scan_file, "%sepic-cloud/result/LaserLine%d_corner_info.txt", dataPath[grp], fidx); + SSX_BQAssemblyInfo pose_3D = _readPoseInfo(_scan_file); + SSX_BQAssemblyInfo pose_3D_ip21; + pose_3D_ip21.O = _pointRT(pose_3D.O, R_3D_mark, T_3D_mark); + SSX_BQAssemblyInfo originPose_ip21_ip20_check; + originPose_ip21_ip20_check.O = _pointRT(pose_3D_ip21.O, R_ip21_ip20, T_ip21_ip20); + SSX_BQAssemblyInfo pose_3D_ip20; + pose_3D_ip20.O = _pointRT(pose_3D.O, R_3D_mark_20, T_3D_mark_20); - sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp], fidx); + sprintf_s(_scan_file, "%smark21/marks_%d.txt", dataPath[grp], fidx); std::vector marks_origin; _readMarkData(_scan_file, marks_origin); - sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp+1], fidx); + sprintf_s(_scan_file, "%smark20/marks_%d.txt", dataPath[grp], fidx); std::vector marks_curr; _readMarkData(_scan_file, marks_curr); @@ -628,7 +680,7 @@ int main() int errCode = 0; SSX_BQAssemblyInfo currPose = sx_BQ_computeAssemblyInfoFromMark( - originPose, originMarkPos, currMarkPos, &errCode); + pose_3D_ip21, originMarkPos, currMarkPos, &errCode); SSX_BQAssemblyInfo compute_testPose = sx_BQ_computeAssemblyInfoFromMark( testPose_origin, originMarkPos, currMarkPos, &errCode); diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index a14d7fd..8355732 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -741,7 +741,7 @@ int main() const char* ver = wd_BQWorkpieceCornerVersion(); printf("ver:%s\n", ver); - for (int grp = 6; grp <= 7; grp++) + for (int grp = 7; grp <= 7; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -823,8 +823,8 @@ int main() growParam.minLTypeTreeLen = 100; //mm growParam.minVTypeTreeLen = 100; //mm SSX_BQworkpiecePara workpieceParam; - if(grp == 7) - workpieceParam.lineLen = 120.0; //直线段长度 + if (grp == 7) + workpieceParam.lineLen = 120; // 120.0; //直线段长度 else workpieceParam.lineLen = 160.0; //直线段长度 int errCode = 0; diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 770057d..a1120dc 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -183,6 +183,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_channelSpaceMeasure_test {52A65444-8505-4FD5-9501-E2D297E2EB2C} = {52A65444-8505-4FD5-9501-E2D297E2EB2C} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rodAndBarDetection_test", "rodAndBarDetection_test\rodAndBarDetection_test.vcxproj", "{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rodAndBarDetection", "rodAndBarDetection\rodAndBarDetection.vcxproj", "{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -471,6 +475,22 @@ Global {3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x64.Build.0 = Release|x64 {3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x86.ActiveCfg = Release|Win32 {3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x86.Build.0 = Release|Win32 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x64.ActiveCfg = Debug|x64 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x64.Build.0 = Debug|x64 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x86.ActiveCfg = Debug|Win32 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x86.Build.0 = Debug|Win32 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x64.ActiveCfg = Release|x64 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x64.Build.0 = Release|x64 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x86.ActiveCfg = Release|Win32 + {59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x86.Build.0 = Release|Win32 + {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x64.ActiveCfg = Debug|x64 + {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x64.Build.0 = Debug|x64 + {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x86.ActiveCfg = Debug|Win32 + {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x86.Build.0 = Debug|Win32 + {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x64.ActiveCfg = Release|x64 + {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 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp index c1bbe38..1d2b5e5 100644 --- a/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp +++ b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp @@ -81,6 +81,101 @@ void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + const double lineYGap = 50; + std::vector< SVzNLPointXYZRGBA> a_line; + + double pre_y = FLT_MIN; + while (getline(inputFile, linedata)) + { + SVzNLPointXYZRGBA a_pt; + memset(&a_pt, 0, sizeof(SVzNLPointXYZRGBA)); + double _x, _y, _z; + sscanf_s(linedata.c_str(), "%lf %lf %lf", &_x, &_y, &_z); + a_pt.x = (float)_x; + a_pt.y = (float)_y; + a_pt.z = (float)_z; + if (a_pt.y < pre_y - lineYGap) //新的扫描行 + { + if (a_line.size() > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + } + pre_y = a_pt.y; + a_line.push_back(a_pt); + } + if (a_line.size() > 0) + scanData.push_back(a_line); + + inputFile.close(); + return; +} + +void _outputScanDataFile_XYZRGB(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++) + { + SVzNLPointXYZRGBA& pt3D = scanData[line][i]; + 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; + } + } + sw.close(); +} + +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 _outputFillingPortInfo(char* fileName, SSG_6DOF centerPose) { std::ofstream sw(fileName); @@ -208,11 +303,14 @@ void _outputRGBDScan_fillingPort_RGBD( sw.close(); } -#define TEST_GROUP 1 +#define TEST_CONVERT_TO_GRID 1 +#define TEST_COMPUTE_POSE 0 +#define TEST_GROUP 2 int main() { const char* dataPath[TEST_GROUP] = { "F:/ShangGu/项目/冠钦_LNG自动加气/加气口测试数据/", //0 + "F:/ShangGu/项目/冠钦项目/圆环自动抓取/", }; SVzNLRange fileIdx[TEST_GROUP] = { @@ -222,6 +320,19 @@ int main() const char* ver = wd_gasFillingPortPositionVersion(); printf("ver:%s\n", ver); +#if TEST_CONVERT_TO_GRID + for (int grp = 0; grp < TEST_GROUP; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =7; + char _scan_file[256]; + spr + LaserData_1 + vzReadPlyTxtPointFromFile_XYZRGB_vector(const char* fileName, std::vector>&scanData); +#endif + +#if TEST_COMPUTE_POSE for (int grp = 0; grp < TEST_GROUP; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) @@ -277,6 +388,7 @@ int main() _outputFillingPortInfo(_scan_file, centerPose); } } +#endif } // 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 diff --git a/rodAndBarDetection/rodAndBarDetection.vcxproj b/rodAndBarDetection/rodAndBarDetection.vcxproj new file mode 100644 index 0000000..4bae6c4 --- /dev/null +++ b/rodAndBarDetection/rodAndBarDetection.vcxproj @@ -0,0 +1,170 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {aec22dc1-fbe0-4d9c-b090-d4821a9d846b} + rodAndBarDetection + 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;RODANDBARDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;RODANDBARDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;RODANDBARDETECTION_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;RODANDBARDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + + + Windows + true + true + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp new file mode 100644 index 0000000..5678d2a --- /dev/null +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -0,0 +1,20 @@ +// rodAndBarDetection_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include + +int main() +{ + std::cout << "Hello World!\n"; +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.vcxproj b/rodAndBarDetection_test/rodAndBarDetection_test.vcxproj new file mode 100644 index 0000000..a5d5c8d --- /dev/null +++ b/rodAndBarDetection_test/rodAndBarDetection_test.vcxproj @@ -0,0 +1,157 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {59f9508a-4295-4a7d-858c-7b4f0a7f8c82} + rodAndBarDetectiontest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _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;rodAndBarDetection.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;rodAndBarDetection.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp new file mode 100644 index 0000000..e619693 --- /dev/null +++ b/sourceCode/rodAndBarDetection.cpp @@ -0,0 +1,289 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "rodAndBarDetection_Export.h" +#include +#include + +//version 1.0.0 : base version release to customer +std::string m_strVersion = "1.0.0"; +const char* wd_rodAndBarDetectionVersion(void) +{ + return m_strVersion.c_str(); +} + + void sx_hexHeadScrewMeasure( + std::vector< std::vector>& scanLines, + bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + double rodRidius, + std::vector& screwInfo, + int* errCode) +{ + *errCode = 0; + SSX_hexHeadScrewInfo screwInfo; + memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo)); + 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; + if (false == isHorizonScan) + { + data_lines.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end()); + for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++) + { + data_lines[line][j].nPointIdx = j; + scanLines[line][j].nPointIdx = 0; //ת帴 + } + } + } + else + { + data_lines.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + data_lines[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[j][line] = scanLines[line][j]; + data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + + } + + lineNum = linePtNum; + linePtNum = (int)data_lines[0].size(); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++) + data_lines[line][j].nPointIdx = j; + } + } + + std::vector> arcFeatures; + for (int line = 0; line < lineNum; line++) + { + if (line == 44) + int kkk = 1; + std::vector& lineData = data_lines[line]; + + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + std::vector line_ringArcs; + int dataSize = (int)lineData.size(); + //ȡArc + wd_getRingArcFeature( + lineData, + line, //ǰɨ + cornerPara, + rodRidius / 2, // + line_ringArcs // + ); + arcFeatures.push_back(line_ringArcs); + } + // + std::vector growTrees; + wd_getSegFeatureGrowingTrees( + arcFeatures, + growTrees, + growParam); + + if (growTrees.size() == 0) + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + int objNum = (int)growTrees.size(); + for (int i = 0; i < objNum; i++) + { + //ռֱ + + //ͶӰ + + // + } + + + std::vector& nodes_1 = growTrees[0].treeNodes; + std::vector& nodes_2 = growTrees[1].treeNodes; +#if _OUTPUT_DEBUG_DATA + for (int i = 0, i_max = (int)nodes_1.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = nodes_1[i].lineIdx; + ptIdx = nodes_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + ptIdx = nodes_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + else + { + ptIdx = nodes_1[i].lineIdx; + lineIdx = nodes_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + lineIdx = nodes_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + } + for (int i = 0, i_max = (int)nodes_2.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = nodes_2[i].lineIdx; + ptIdx = nodes_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + ptIdx = nodes_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + else + { + ptIdx = nodes_2[i].lineIdx; + lineIdx = nodes_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + lineIdx = nodes_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + } +#endif + + //ɨ + std::vector line_1; //line_1line_2Ϊɨ߶Ӧ۵ϵGap + std::vector line_2; + int i_idx = 0; + int j_idx = 0; + while (1) + { + int line_idx1 = nodes_1[i_idx].lineIdx; + int line_idx2 = nodes_2[j_idx].lineIdx; + if (line_idx1 == line_idx2) + { + line_1.push_back(nodes_1[i_idx]); + line_2.push_back(nodes_2[j_idx]); + i_idx++; + j_idx++; + } + else + { + if (line_idx1 < line_idx2) + i_idx++; + else + j_idx++; + } + if ((i_idx >= (int)nodes_1.size()) || (j_idx >= (int)nodes_2.size())) + break; + } + +#if _OUTPUT_DEBUG_DATA + for (int i = 0, i_max = (int)line_1.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = line_1[i].lineIdx; + ptIdx = line_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + ptIdx = line_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + else + { + ptIdx = line_1[i].lineIdx; + lineIdx = line_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + lineIdx = line_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + } + for (int i = 0, i_max = (int)line_2.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = line_2[i].lineIdx; + ptIdx = line_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + ptIdx = line_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + else + { + ptIdx = line_2[i].lineIdx; + lineIdx = line_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + lineIdx = line_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + } +#endif + + //ת֤ɨGapֱ + double angleSearchWin = 30; + double angleStepping = 0.1; + + int loop = (int)(angleSearchWin / angleStepping); + double bestSpace = 0; + double rotateAngle = 0; + for (int i = -loop; i <= loop; i++) + { + double angle = i * angleStepping; + std::vector rotate_line_1; + _XY_rotateLine(angle, line_1, rotate_line_1); + std::vector rotate_line_2; + _XY_rotateLine(angle, line_2, rotate_line_2); + double meanSpace = _computeChannelSpace(rotate_line_1, rotate_line_2); + if (bestSpace < meanSpace) + { + bestSpace = meanSpace; + rotateAngle = angle; + } + } + std::vector calib_line_1; + _XY_rotateLine(rotateAngle, line_1, calib_line_1); + std::vector calib_line_2; + _XY_rotateLine(rotateAngle, line_2, calib_line_2); + + channelInfo.channelSpace = _computeChannelSpace(calib_line_1, calib_line_2); + channelInfo.channelWidth[0] = _computeGapMeanWidth(calib_line_1); + channelInfo.channelWidth[1] = _computeGapMeanWidth(calib_line_2); + channelInfo.channelDepth[0] = _computeGapMeanDepth(calib_line_1, data_lines); + channelInfo.channelDepth[1] = _computeGapMeanDepth(calib_line_2, data_lines); + return channelInfo; +} + + + + diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h new file mode 100644 index 0000000..e1c15bc --- /dev/null +++ b/sourceCode/rodAndBarDetection_Export.h @@ -0,0 +1,25 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + SVzNL3DPoint center; //ݸ˶˲ĵ + SVzNL3DPoint axialPoint; //㣬center + double rotateAngle; //-30 - 30 +}SSX_hexHeadScrewInfo; //ͷݸ + +//汾 +SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void); + +//ͷݸ˶˲ĵ +SG_APISHARED_EXPORT SSX_hexHeadScrewInfo sx_hexHeadScrewMeasure( + std::vector< std::vector>& scanLines, + bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + int* errCode); diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index b47f800..4d4fedb 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -2637,7 +2637,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData #define TEST_CONVERT_TO_GRID 0 #define TEST_COMPUTE_WHEEL_ARCH 1 -#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_CALIB_PARA 1 #define TEST_GROUP 1 int main() @@ -2683,7 +2683,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/ground_LaserData.txt"); + sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_LaserData.txt"); std::vector< std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); @@ -2704,10 +2704,10 @@ int main() #endif // char calibFile[250]; - sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/ground_calib_para.txt"); + sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/LaserLine_grund_calib.txt"); + sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/LaserLine_grund_calib.txt"); _outputScanDataFile_self(_out_file, scanLines, 0, 0, 0); printf("%s: calib done!\n", _calib_datafile); } @@ -2716,11 +2716,11 @@ int main() #if TEST_COMPUTE_WHEEL_ARCH const char* dataPath[TEST_GROUP] = { - "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/", //0 + "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/", //0 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,1}, + {1,2}, }; SSG_planeCalibPara poseCalibPara;