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..923c8a3 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -183,6 +183,17 @@ 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}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B} = {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rodAndBarDetection", "rodAndBarDetection\rodAndBarDetection.vcxproj", "{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -471,6 +482,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/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 1155a66..eb7de69 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -410,6 +410,18 @@ SG_APISHARED_EXPORT void lineFitting_abc( double* _b, double* _c); +/** + * @brief ռֱС + * @param points ά㼯2㣬壩 + * @param center ֱߵģ׼P0 + * @param direction ֱߵķvλ + * @return Ƿɹ㼯Чtrue + */ +SG_APISHARED_EXPORT bool fitLine3DLeastSquares( + const std::vector& points, + SVzNL3DPoint& center, + SVzNL3DPoint& direction); + //ԲС SG_APISHARED_EXPORT double fitCircleByLeastSquare( const std::vector& pointArray, @@ -537,10 +549,15 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs( //һƽƽ //ROIڵĵƽϣƽ //תΪƽƽ淨ΪֱIJ -SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( std::vector< std::vector>& scanLines, SVzNL3DRangeD roi); +//ת +SG_APISHARED_EXPORT SSG_planeCalibPara wd_computeRTMatrix( + SVzNL3DPoint& vector1, + SVzNL3DPoint& vector2); + // תŷǣZ-Y-X˳ SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]); // ŷǼתZ-Y-X˳ diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 6a3c5a4..1b61b45 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -2417,6 +2417,44 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara( return planePara; } +SSG_planeCalibPara wd_computeRTMatrix(SVzNL3DPoint& vector1, SVzNL3DPoint& vector2) +{ + Vector3 a = Vector3(vector1.x, vector1.y, vector1.z); + Vector3 b = Vector3(vector2.x, vector2.y, vector2.z); + Quaternion quanPara = rotationBetweenVectors(a, b); + + RotationMatrix rMatrix; + quaternionToMatrix(quanPara, rMatrix.data); + //㷴ת + Quaternion invQuanPara = rotationBetweenVectors(b, a); + RotationMatrix invMatrix; + quaternionToMatrix(invQuanPara, invMatrix.data); + + SSG_planeCalibPara calibPara; + calibPara.planeCalib[0] = rMatrix.data[0][0]; + calibPara.planeCalib[1] = rMatrix.data[0][1]; + calibPara.planeCalib[2] = rMatrix.data[0][2]; + calibPara.planeCalib[3] = rMatrix.data[1][0]; + calibPara.planeCalib[4] = rMatrix.data[1][1]; + calibPara.planeCalib[5] = rMatrix.data[1][2]; + calibPara.planeCalib[6] = rMatrix.data[2][0]; + calibPara.planeCalib[7] = rMatrix.data[2][1]; + calibPara.planeCalib[8] = rMatrix.data[2][2]; + + calibPara.invRMatrix[0] = invMatrix.data[0][0]; + calibPara.invRMatrix[1] = invMatrix.data[0][1]; + calibPara.invRMatrix[2] = invMatrix.data[0][2]; + calibPara.invRMatrix[3] = invMatrix.data[1][0]; + calibPara.invRMatrix[4] = invMatrix.data[1][1]; + calibPara.invRMatrix[5] = invMatrix.data[1][2]; + calibPara.invRMatrix[6] = invMatrix.data[2][0]; + calibPara.invRMatrix[7] = invMatrix.data[2][1]; + calibPara.invRMatrix[8] = invMatrix.data[2][2]; + + calibPara.planeHeight = 0; + return calibPara; +} + SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( std::vector< std::vector>& scanLines, SVzNL3DRangeD roi) @@ -2796,7 +2834,7 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH } void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH) { - for (int i = 0; i < a_line.size(); i++) + for (int i = 0; i < (int)a_line.size(); i++) { SVzNL3DPoint a_pt = a_line[i].pt3D; if (a_pt.z < 1e-4) @@ -3034,3 +3072,59 @@ void pointRT(const cv::Mat& R, const cv::Mat& T, rtPt.z = result(2); return; } + + +/** + * @brief ռֱС + * @param points ά㼯2㣬壩 + * @param center ֱߵģ׼P0 + * @param direction ֱߵķvλ + * @return Ƿɹ㼯Чtrue + */ +bool fitLine3DLeastSquares(const std::vector& points, SVzNL3DPoint& center, SVzNL3DPoint& direction) +{ + // 㼯Ч + if (points.size() < 2) { + std::cerr << "Error: 㼯ڵ2" << std::endl; + return false; + } + + int n = points.size(); + Eigen::MatrixXd A(n, 3); // 㼯ÿһ(x,y,z) + + // 1. ģcenter + double cx = 0.0, cy = 0.0, cz = 0.0; + for (const auto& p : points) { + cx += p.x; + cy += p.y; + cz += p.z; + A.row(points.size() - n) << p.x, p.y, p.z; // 㼯 + n--; + } + cx /= points.size(); + cy /= points.size(); + cz /= points.size(); + center = { cx, cy, cz }; + + // 2. ȥĻЭ3x3 + // ؼ޸ʹRowVector3drowwiseƥά + Eigen::RowVector3d centroid_row(cx, cy, cz); + Eigen::MatrixXd centered = A.rowwise() - centroid_row; // άƥ䣬ޱ + + // Э㣨n-1ΪƫƣҲֱn + Eigen::Matrix3d cov = centered.transpose() * centered / (points.size() - 1); + // 3. ֵֽ⣺Эֵ + Eigen::SelfAdjointEigenSolver eigensolver(cov); + if (eigensolver.info() != Eigen::Success) { + std::cerr << "Error: ֵֽʧܣ" << std::endl; + return false; + } + + // ֵӦΪEigenĬϰֵУȡһ + Eigen::Vector3d dir = eigensolver.eigenvectors().col(2); + // λѡͨ׼ + dir.normalize(); + + direction = { dir(0), dir(1), dir(2) }; + return true; +} diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 3eec456..f61d4f6 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3443,7 +3443,7 @@ void wd_getRingArcFeature( std::vector< SVzNL3DPosition>& lineData, int lineIdx, const SSG_cornerParam cornerPara, - double ringArcWidth, //ӵĻ + double ringArcWidth, // std::vector& line_ringArcs // ) { diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp new file mode 100644 index 0000000..1eabc5f --- /dev/null +++ b/sourceCode/rodAndBarDetection.cpp @@ -0,0 +1,299 @@ +#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(); +} + +SVzNL3DPoint getArcPeak( + std::vector< std::vector>& scanLines, + SWD_segFeature & a_arcFeature) +{ + SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D; + for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++) + { + if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //յ + { + if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z) + arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D; + } + } + return arcPeak; +} + +//ͶӰȡROIڵ +void xoyROIProjection( + std::vector< std::vector>& scanLines, + const double* rtMatrix, + SSG_ROIRectD& roi_xoy, + std::vector& projectPoints + ) +{ + 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 < (int)a_line.size(); i++) + { + SVzNL3DPoint a_pt = a_line[i].pt3D; + if (a_pt.z < 1e-4) + continue; + double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2]; + double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5]; + double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8]; + if ((x >= roi_xoy.left) && (x <= roi_xoy.right) && + (y >= roi_xoy.top) && (y <= roi_xoy.bottom)) + { + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + projectPoints.push_back(a_pt); + } + } + } +} + +SVzNLRangeD getZRange(std::vector& projectPoints) +{ + int ptNum = (int)projectPoints.size(); + SVzNLRangeD zRange; + zRange.min = DBL_MAX; + zRange.max = DBL_MIN; + for (int i = 0; i < ptNum; i++) + { + zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min; + zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max; + } + return zRange; +} + +void zCutPointClouds( + std::vector& projectPoints, + SVzNLRangeD& zRange, + std::vector& cutLayerPoints) +{ + int ptNum = (int)projectPoints.size(); + for (int i = 0; i < ptNum; i++) + { + if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max)) + cutLayerPoints.push_back(projectPoints[i]); + } +} + +SVzNL3DPoint getXoYCentroid(std::vector& points) +{ + int ptNum = (int)points.size(); + SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 }; + if (ptNum == 0) + return centroid; + for (int i = 0; i < ptNum; i++) + { + centroid.x += points[i].x; + centroid.y += points[i].y; + } + centroid.x = centroid.x / ptNum; + centroid.y = centroid.y / ptNum; + return centroid; +} + +SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) +{ + SVzNL3DPoint _r_pt; + _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; + _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; + _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; + return _r_pt; +} + + 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; + 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 fitPoints; + int nodeSize = (int)growTrees[i].treeNodes.size(); + for (int j = 0; j < nodeSize; j++) + { + SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j]); + fitPoints.push_back(a_pt); + } + if (fitPoints.size() < 12) + continue; + //ȥͷβ5㣬ֹڶ˲͸ɨʱи + fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5); + fitPoints.erase(fitPoints.end() - 5, fitPoints.end()); + // + SVzNL3DPoint P0_center, P1_dir; + bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir); + if (false == result) + continue; + //ͶӰ + //ת + SVzNL3DPoint vector1 = P1_dir; + SVzNL3DPoint vector2 = { 0, 0, -1.0 }; + SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2); + // + SSG_ROIRectD roi_xoy; + roi_xoy.left = P0_center.x - rodRidius * 4; //2DΧ + roi_xoy.right = P0_center.x + rodRidius * 4; //2DΧ + roi_xoy.top = P0_center.y - rodRidius * 4; //2DΧ + roi_xoy.bottom = P0_center.y + rodRidius * 4; //2DΧ + std::vector< SVzNL3DPoint> roiProjectionData; + xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData); + //ȡ + SVzNLRangeD zRange = getZRange(roiProjectionData); + SVzNLRangeD cutZRange; + cutZRange.min = zRange.min; + cutZRange.max = zRange.min + 10.0; //ȡ10mmĶ + std::vector surfacePoints; + zCutPointClouds(roiProjectionData, cutZRange, surfacePoints); + //ĵ + SVzNL3DPoint projectionCenter = getXoYCentroid(surfacePoints); + projectionCenter.z = zRange.min; + //תԭϵ + SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix); + //RodϢ + SSX_hexHeadScrewInfo a_rod; + a_rod.center = surfaceCenter; + a_rod.axialDir = P1_dir; + a_rod.rotateAngle = 0; + screwInfo.push_back(a_rod); + } + if (true == isHorizonScan) + { + int objNum = (int)screwInfo.size(); + for (int i = 0; i < objNum; i++) + { + double tmp = screwInfo[i].center.x; + screwInfo[i].center.x = screwInfo[i].center.y; + screwInfo[i].center.y = tmp; + tmp = screwInfo[i].axialDir.x; + screwInfo[i].axialDir.x = screwInfo[i].axialDir.y; + screwInfo[i].axialDir.y = tmp; + //screwInfo[i].rotateAngle += 90; + } + } + return; +} + + + + diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h new file mode 100644 index 0000000..dc833f0 --- /dev/null +++ b/sourceCode/rodAndBarDetection_Export.h @@ -0,0 +1,27 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + SVzNL3DPoint center; //ݸ˶˲ĵ + SVzNL3DPoint axialDir; // + double rotateAngle; //-30 - 30 +}SSX_hexHeadScrewInfo; //ͷݸ + +//汾 +SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void); + +//ͷݸ˶˲ĵ +SG_APISHARED_EXPORT 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); 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;