From 7f18d0f92a61e464f1b867dd5eb098870040c760 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 6 Oct 2025 14:21:33 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8B=86=E5=8C=85=E5=BA=94=E7=94=A8=E5=8A=A0?= =?UTF-8?q?=E5=BC=BA=E4=BA=86=E6=97=8B=E8=BD=AC=E6=97=8B=E8=BD=AC=E7=9A=84?= =?UTF-8?q?=E5=8C=85=E7=9A=84=E6=A3=80=E6=B5=8B=E5=8A=9F=E8=83=BD=E3=80=82?= =?UTF-8?q?=E8=BF=99=E6=A0=B7=E5=8A=9F=E8=83=BD=E7=94=B1=E4=B8=80=E4=B8=AA?= =?UTF-8?q?=E5=BC=80=E5=85=B3=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../QRcode3Ddetection_test.cpp | 6 +- SG_Algorithm.sln | 27 ++ bagPositioning_test/bagPositioning_test.cpp | 117 ++++-- .../beltTearingDetection_test.cpp | 18 +- lapWeldDetection/lapWeldDetection.vcxproj | 18 +- .../lapWeldDetection_test.cpp | 18 +- .../lapWeldDetection_test.vcxproj | 14 +- sourceCode/SG_bagPositioning.cpp | 166 +++++++-- sourceCode/SG_bagPositioning_Export.h | 1 + sourceCode/SG_baseAlgo_Export.h | 33 ++ sourceCode/SG_baseDataType.h | 1 + sourceCode/SG_baseFunc.cpp | 137 +++++++ sourceCode/SG_errCode.h | 4 + sourceCode/SG_featureGrow.cpp | 143 +++++++ sourceCode/SG_lineFeature.cpp | 351 +++++++++++++++++- 15 files changed, 954 insertions(+), 100 deletions(-) diff --git a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp index cd8b884..4e384c3 100644 --- a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp +++ b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp @@ -564,16 +564,16 @@ void _getRoiClouds( #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_QRCODE_IMG 1 -#define TEST_GROUP 1 +#define TEST_GROUP 2 int main() { const char* dataPath[TEST_GROUP] = { "F:\\ShangGu\\项目\\工件端部圆点二维码\\", //0 - + "F:\\ShangGu\\项目\\工件端部圆点二维码\\字符\\" //1 }; SVzNLRange fileIdx[TEST_GROUP] = { - {3,3} + {3,3}, {1,8} }; #if TEST_COMPUTE_CALIB_PARA diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index cf60398..65b4703 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -107,6 +107,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lapWeldDetection_test", "la {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0} = {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtraction", "BQ_workpieceCornerExtraction\BQ_workpieceCornerExtraction.vcxproj", "{AD8415B7-A745-4184-87B8-95619E5066D6}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtract_test", "BQ_workpieceCornerExtract_test\BQ_workpieceCornerExtract_test.vcxproj", "{CF563709-0402-447E-BFCC-7701CC90D0AF}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {AD8415B7-A745-4184-87B8-95619E5066D6} = {AD8415B7-A745-4184-87B8-95619E5066D6} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -283,6 +294,22 @@ Global {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x64.Build.0 = Release|x64 {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.ActiveCfg = Release|Win32 {BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.Build.0 = Release|Win32 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Debug|x64.ActiveCfg = Debug|x64 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Debug|x64.Build.0 = Debug|x64 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Debug|x86.ActiveCfg = Debug|Win32 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Debug|x86.Build.0 = Debug|Win32 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Release|x64.ActiveCfg = Release|x64 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Release|x64.Build.0 = Release|x64 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Release|x86.ActiveCfg = Release|Win32 + {AD8415B7-A745-4184-87B8-95619E5066D6}.Release|x86.Build.0 = Release|Win32 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.Debug|x64.ActiveCfg = Debug|x64 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.Debug|x64.Build.0 = Debug|x64 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.Debug|x86.ActiveCfg = Debug|Win32 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.Debug|x86.Build.0 = Debug|Win32 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x64.ActiveCfg = Release|x64 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x64.Build.0 = Release|x64 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.ActiveCfg = Release|Win32 + {CF563709-0402-447E-BFCC-7701CC90D0AF}.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 855b8f7..c34b771 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -626,12 +626,13 @@ void _convertToGridData_XYZRGB_vector(std::vector>& s return; int pt_counter = max_y - min_y + 1; - std::vector< SPointXYZRGB> gridData; - gridData.resize(pt_counter); - for (int i = 0; i < pt_counter; i++) - gridData[i] = { 0, 0.0, 0.0, 0.0, 0.0f, 0.0f, 0.0f }; for (int line = 0; line < lineNum; line++) { + std::vector< SPointXYZRGB> gridData; + gridData.resize(pt_counter); + for (int i = 0; i < pt_counter; i++) + gridData[i] = { 0, 0.0, 0.0, 0.0, 0.0f, 0.0f, 0.0f }; + std::vector< SPointXYZRGB>& a_line = scanData[line]; int nPointCnt = a_line.size(); for (int i = 0; i < nPointCnt; i++) @@ -2685,8 +2686,8 @@ 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,3}, {11,11}, - {5,22},{1, 15},{1,15}, {1, 15},{1,2}, + {2,4}, {1,5}, {1,1}, {1,3}, {1,11}, + {5,22},{1, 15},{1,15}, {1, 15},{1,7}, {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -2710,7 +2711,7 @@ int main() SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 19; grp <= 19; grp++) + for (int grp = 0; grp <= 19; grp++) { if (grp < 10) { @@ -2723,6 +2724,13 @@ int main() algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 20; //45度角 + algoParam.cornerParam.scale = algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if ( (grp >= 10) && (grp <= 11)) { @@ -2735,6 +2743,13 @@ int main() algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if (grp ==13) { @@ -2747,6 +2762,13 @@ int main() algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if (grp == 14) { @@ -2759,6 +2781,13 @@ int main() algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if (grp == 15) //纸箱拆垛数据\vizum数据 { @@ -2771,6 +2800,13 @@ int main() algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if (grp == 16) //纸箱拆垛数据\380×480mm纸箱RGB点云及2D图像 { @@ -2783,6 +2819,13 @@ int main() algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if (grp == 17) //纸箱拆垛数据\380×580mm纸箱RGB点云及2d图像 { @@ -2795,6 +2838,13 @@ int main() algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if (grp == 18) //纸箱拆垛数据\480×580mm纸箱rgb点云及2d图像 { @@ -2807,11 +2857,18 @@ int main() algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if (grp == 19) //纸箱拆垛数据\vizum数据 { - algoParam.bagParam.bagL = 420; //袋子长65cm - algoParam.bagParam.bagW = 320; //袋子宽40cm + algoParam.bagParam.bagL = 500;// 500; // 420; //袋子长65cm + algoParam.bagParam.bagW = 320; // 420; // 320; //袋子宽40cm algoParam.bagParam.bagH = 70; //袋子高16cm algoParam.growParam.maxLineSkipNum = 5; algoParam.growParam.yDeviation_max = 20.0; @@ -2819,6 +2876,13 @@ int main() algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else if ( (grp >= 20) && (grp <= 21)) { @@ -2831,6 +2895,13 @@ int main() algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } else { @@ -2843,22 +2914,14 @@ int main() algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 algoParam.growParam.minLTypeTreeLen = 50.0; //mm algoParam.growParam.minVTypeTreeLen = 50.0; //mm + + algoParam.cornerParam.cornerTh = 45; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; } -#if BAG_ALGO_USE_CORNER_FEATURE - algoParam.cornerParam.cornerTh = 45; //45度角 - algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; - algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; - algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; - algoParam.cornerParam.jumpCornerTh_1 = 60; - algoParam.cornerParam.jumpCornerTh_2 = 15; -#else - algoParam.slopeParam.LSlopeZWin = 10.0; - algoParam.slopeParam.validSlopeH = 10.0; - algoParam.slopeParam.minLJumpH = 20.0; - algoParam.slopeParam.minEndingGap = algoParam.bagParam.bagW / 4; ///当没有中间袋子，只有左右袋子时，有两组Ending。此参数确定两个ending最小间距 - algoParam.valleyPara.valleyMinH = 10.0; - algoParam.valleyPara.valleyMaxW = 80.0; //取袋子宽度的1/5 -#endif if (grp == 7) { @@ -2931,7 +2994,7 @@ int main() else if (grp == 14) { char calibFile[250]; - sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\拆包模拟环境\\ground_calib_para2.txt"); + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\拆包模拟环境\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } else if ( (grp >= 15) && (grp <= 18)) //箱子 @@ -2970,9 +3033,11 @@ int main() } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 22; + //fidx = 193; if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { + algoParam.supportRotate = 1; //支持相机与对象之间有一个旋转角度（小于30度） + int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; diff --git a/beltTearingDetection_test/beltTearingDetection_test.cpp b/beltTearingDetection_test/beltTearingDetection_test.cpp index b94371a..4f0e4bb 100644 --- a/beltTearingDetection_test/beltTearingDetection_test.cpp +++ b/beltTearingDetection_test/beltTearingDetection_test.cpp @@ -1043,13 +1043,13 @@ int main() #else const char* dataPath[TEST_GROUP] = { - "F:\\上古\\皮带撕裂点云\\1-500-2000\\", //0 - "F:\\上古\\皮带撕裂点云\\2-1700-2000\\", //1 - "F:\\上古\\皮带撕裂点云\\SaveData\\" + "F:\\ShangGu\\皮带撕裂点云\\1-500-2000\\", //0 + "F:\\ShangGu\\皮带撕裂点云\\2-1700-2000\\", //1 + "F:\\ShangGu\\皮带撕裂点云\\SaveData\\" }; SVzNLRange fileIdx[TEST_GROUP] = { - {10,24}, {2, 18}, {15,15} }; + {10,24}, {2, 18}, {16,16} }; double camPoseR[9] = { 1.0, 0.0, 0.0, @@ -1057,12 +1057,12 @@ int main() 0.0, 0.0, 1.0 }; SSG_beltTearingParam algoParam; - algoParam.differnceBinTh = 1.0; + algoParam.differnceBinTh = 2.0; // 1.0; algoParam.extractPara.gapChkWin = 7; - algoParam.extractPara.sameGapTh = 20.0; - algoParam.scanXScale = 4.0; - algoParam.scanYScale = 0.6; - algoParam.tearingMinGap = 50.0; //两个同位置的纵撕的最小间隔。大于此门限视为两个撕裂 + algoParam.extractPara.sameGapTh = 30.0; + algoParam.scanXScale = 0.5; + algoParam.scanYScale = 1.2; + algoParam.tearingMinGap = 100.0; //两个同位置的纵撕的最小间隔。大于此门限视为两个撕裂 algoParam.tearingMinLen = 30.0; char _scan_file[256]; diff --git a/lapWeldDetection/lapWeldDetection.vcxproj b/lapWeldDetection/lapWeldDetection.vcxproj index fb8d3ac..258576e 100644 --- a/lapWeldDetection/lapWeldDetection.vcxproj +++ b/lapWeldDetection/lapWeldDetection.vcxproj @@ -85,12 +85,12 @@ true $(SolutionDir)build\$(Platform)\$(Configuration)\ - ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) false $(SolutionDir)build\$(Platform)\$(Configuration)\ - ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) @@ -134,14 +134,15 @@ true NotUsing pch.h - ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) Windows true false - opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies) - ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) @@ -154,7 +155,8 @@ true NotUsing pch.h - ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) Windows @@ -162,8 +164,8 @@ true true false - opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies) - ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp index 7ba8ba9..7f861a3 100644 --- a/lapWeldDetection_test/lapWeldDetection_test.cpp +++ b/lapWeldDetection_test/lapWeldDetection_test.cpp @@ -296,7 +296,6 @@ void _outputRGBDScanLapWeld_RGBD( sw.close(); } - void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum) { std::ofstream sw(fileName); @@ -700,16 +699,16 @@ void _convertToGridData(std::vector< std::vector>& scanLines, #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_POSITION 1 -#define TEST_GROUP 1 +#define TEST_GROUP 2 int main() { const char* dataPath[TEST_GROUP] = { "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\", //0 - + "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\模拟数据\\", }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,6} + {1,6}, {1,1} }; #if CONVERT_TO_GRID @@ -779,7 +778,7 @@ int main() #endif #if TEST_COMPUTE_POSITION - for (int grp = 0; grp <= 0; grp++) + for (int grp = 0; grp <= 1; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -796,8 +795,11 @@ int main() 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); + if (grp == 0) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -827,7 +829,7 @@ int main() SSX_lapWeldParam lapWeldParam; lapWeldParam.lapHeight = 1.5; lapWeldParam.weldMinLen = 80.0; - lapWeldParam.weldRefPoints = 2; + lapWeldParam.weldRefPoints = 3; lapWeldParam.scanMode = keSX_ScanMode_V; SSG_cornerParam cornerParam; cornerParam.cornerTh = 25; //45度角 diff --git a/lapWeldDetection_test/lapWeldDetection_test.vcxproj b/lapWeldDetection_test/lapWeldDetection_test.vcxproj index 38f1f0f..0665d83 100644 --- a/lapWeldDetection_test/lapWeldDetection_test.vcxproj +++ b/lapWeldDetection_test/lapWeldDetection_test.vcxproj @@ -120,13 +120,14 @@ true _DEBUG;_CONSOLE;%(PreprocessorDefinitions) true - ..\..\thirdParty\opencv\build\include; + ..\..\thirdParty\opencv320\build\include; + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) Console true - opencv_world480d.lib;lapWeldDetection.lib;%(AdditionalDependencies) - ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;lapWeldDetection.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) @@ -137,15 +138,16 @@ true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true - ..\..\thirdParty\opencv\build\include; + ..\..\thirdParty\opencv320\build\include; + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) Console true true true - opencv_world480.lib;lapWeldDetection.lib;%(AdditionalDependencies) - ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;lapWeldDetection.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index 2adcba2..d7ee30f 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -224,7 +224,7 @@ bool _validateObj_treeROI( { double isSameSideTh = 100; //objROIıGlobalROIıߵľ5cmΪͬһ double treeCombineTh = 30.0; //Treeϲľ - double innerInvalidLenTh = 0.75; + double innerInvalidLenTh = 0.9; double sideInvalidLenTh = 0.8; double side_diff[4]; side_diff[0] = objROI.left - globalROI.left; @@ -1172,6 +1172,56 @@ void _combineValidPairs( } } } +#if 1 //2025.10.4:ROIȽ + else if ((a_matchPair.id1 != seed_pair.data_0) && (a_matchPair.id2 != seed_pair.data_1)) + { + //compare ROI + bool roiValid = false; + if (true == isTBDir) + { + double T_diff = abs(a_matchPair.roi.top - matchPairs[seed_pair.idx].roi.top); + double B_diff = abs(a_matchPair.roi.bottom - matchPairs[seed_pair.idx].roi.bottom); + if ((a_matchPair.roi.left >= chkRange.min) && (a_matchPair.roi.right <= chkRange.max) && + (T_diff < matchTh.max) && (B_diff < matchTh.max)) + { + roiValid = true; + } + } + else + { + double L_diff = abs(a_matchPair.roi.left - matchPairs[seed_pair.idx].roi.left); + double R_diff = abs(a_matchPair.roi.right - matchPairs[seed_pair.idx].roi.right); + if ((a_matchPair.roi.left >= chkRange.min) && (a_matchPair.roi.right <= chkRange.max) && + (L_diff < matchTh.max) && (R_diff < matchTh.max)) + { + roiValid = true; + } + } + if (roiValid == true) + { + SSG_ROIRectD rstROI = combineROI(matchPairs[seed_pair.idx].roi, matchPairs[i].roi); + double rst_width; + if (TBL_LRW == true) + rst_width = rstROI.right - rstROI.left; + else + rst_width = rstROI.bottom - rstROI.top; + if (rst_width < bagW * 1.3) + { + if ((true == isTBDir) && (a_matchPair.roi.left >= chkRange.min - matchTh.min) && (a_matchPair.roi.right <= chkRange.max + matchTh.max)) + { + SSG_intPair a_newPair = { a_matchPair.id1, a_matchPair.id2 }; + LR_idPairs.push_back(a_newPair); + } + else if ((false == isTBDir) && (a_matchPair.roi.top >= chkRange.min - matchTh.min) && (a_matchPair.roi.bottom <= chkRange.max + matchTh.max)) + { + SSG_intPair a_newPair = { a_matchPair.id1, a_matchPair.id2 }; + LR_idPairs.push_back(a_newPair); + } + } + + } + } +#endif } } return ; @@ -1632,9 +1682,9 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vector TB_idPairs; std::vector LR_idPairs; - SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm }; + SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, 0 }; TB_idPairs.push_back(seed_TBPair); - SSG_intPair seed_LRPair = { edgeId_left , edgeId_right }; + SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, LR_id }; LR_idPairs.push_back(seed_LRPair); _combineValidPairs( LR_pairs, @@ -1697,9 +1747,9 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vectorroi.left, match_LR->roi.right }; std::vector TB_idPairs; std::vector LR_idPairs; - SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm }; + SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, TB_id }; TB_idPairs.push_back(seed_TBPair); - SSG_intPair seed_LRPair = { edgeId_left , edgeId_right }; + SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, 0 }; LR_idPairs.push_back(seed_LRPair); _combineValidPairs( TB_pairs, @@ -1938,9 +1988,9 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vector TB_idPairs; std::vector LR_idPairs; - SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm }; + SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, TB_bagW }; TB_idPairs.push_back(seed_TBPair); - SSG_intPair seed_LRPair = { edgeId_left , edgeId_right }; + SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, LR_bagL }; LR_idPairs.push_back(seed_LRPair); bool TBL_LRW = false; _combineValidPairs(TB_pairs, @@ -1999,9 +2049,9 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vector TB_idPairs; std::vector LR_idPairs; - SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm }; + SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, TB_bagL }; TB_idPairs.push_back(seed_TBPair); - SSG_intPair seed_LRPair = { edgeId_left , edgeId_right }; + SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, LR_bagW }; LR_idPairs.push_back(seed_LRPair); bool TBL_LRW = true; _combineValidPairs( @@ -2058,9 +2108,9 @@ void _refineContourPairs(SSG_matchPair* valid_match, std::vector TB_idPairs_1; std::vector LR_idPairs_1; - seed_TBPair = { edgeId_top , edgeId_btm }; + seed_TBPair = { edgeId_top , edgeId_btm, TB_bagW }; TB_idPairs_1.push_back(seed_TBPair); - seed_LRPair = { edgeId_left , edgeId_right }; + seed_LRPair = { edgeId_left , edgeId_right, LR_bagL }; LR_idPairs_1.push_back(seed_LRPair); TBL_LRW = false; _combineValidPairs( @@ -2614,34 +2664,72 @@ if (hLine == 14) std::vector hEdge_0_trees; std::vector hEdge_1_trees; //˵ - sg_getEndingGrowingTrees( - v_edgePts_0, - laser3DPoints, - true, //isVScan, - LINE_FEATURE_LINE_ENDING_0, - vEdge_0_trees, - algoParam.growParam); - sg_getEndingGrowingTrees( - v_edgePts_1, - laser3DPoints, - true, //isVScan, - LINE_FEATURE_LINE_ENDING_1, - vEdge_1_trees, - algoParam.growParam); - sg_getEndingGrowingTrees( - h_edgePts_0, - laser3DPoints, - false, //isVScan, - LINE_FEATURE_LINE_ENDING_0, - hEdge_0_trees, - algoParam.growParam); - sg_getEndingGrowingTrees( - h_edgePts_1, - laser3DPoints, - false, //isVScan, - LINE_FEATURE_LINE_ENDING_1, - hEdge_1_trees, - algoParam.growParam); + if (algoParam.supportRotate == 0) + { + sg_getEndingGrowingTrees( + v_edgePts_0, + laser3DPoints, + true, //isVScan, + LINE_FEATURE_LINE_ENDING_0, + vEdge_0_trees, + algoParam.growParam); + sg_getEndingGrowingTrees( + v_edgePts_1, + laser3DPoints, + true, //isVScan, + LINE_FEATURE_LINE_ENDING_1, + vEdge_1_trees, + algoParam.growParam); + sg_getEndingGrowingTrees( + h_edgePts_0, + laser3DPoints, + false, //isVScan, + LINE_FEATURE_LINE_ENDING_0, + hEdge_0_trees, + algoParam.growParam); + sg_getEndingGrowingTrees( + h_edgePts_1, + laser3DPoints, + false, //isVScan, + LINE_FEATURE_LINE_ENDING_1, + hEdge_1_trees, + algoParam.growParam); + } + else + { + sg_getEndingGrowingTrees_angleCheck( + v_edgePts_0, + laser3DPoints, + true, //isVScan, + LINE_FEATURE_LINE_ENDING_0, + vEdge_0_trees, + algoParam.growParam, + algoParam.cornerParam.scale); + sg_getEndingGrowingTrees_angleCheck( + v_edgePts_1, + laser3DPoints, + true, //isVScan, + LINE_FEATURE_LINE_ENDING_1, + vEdge_1_trees, + algoParam.growParam, + algoParam.cornerParam.scale); + sg_getEndingGrowingTrees_angleCheck( + h_edgePts_0, + laser3DPoints, + false, //isVScan, + LINE_FEATURE_LINE_ENDING_0, + hEdge_0_trees, + algoParam.growParam, + algoParam.cornerParam.scale); + sg_getEndingGrowingTrees_angleCheck( + h_edgePts_1, + laser3DPoints, + false, //isVScan, + LINE_FEATURE_LINE_ENDING_1, + hEdge_1_trees, + algoParam.growParam, + algoParam.cornerParam.scale); + } cv::Mat featureMask = cv::Mat::zeros(hLineNum, lineNum, CV_32SC4); //任Mask1mmΪ߶ diff --git a/sourceCode/SG_bagPositioning_Export.h b/sourceCode/SG_bagPositioning_Export.h index 867a6e2..d97ebb9 100644 --- a/sourceCode/SG_bagPositioning_Export.h +++ b/sourceCode/SG_bagPositioning_Export.h @@ -26,6 +26,7 @@ typedef struct SSG_treeGrowParam growParam; //SSG_objSortParam sortParam; //SSG_polarScanParam polarScanParam; + int supportRotate; }SG_bagPositionParam; typedef struct diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index faf5070..45dd066 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -60,6 +60,14 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature( const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 SSG_lineFeature* line_features); +SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + double refSteppingZ, + const SSG_cornerParam cornerPara, + std::vector& line_features); + /// /// ȡϵJumping /// nPointIdx¶Feature @@ -132,6 +140,8 @@ SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( std::vector& trees, SSG_treeGrowParam growParam); +//SG_APISHARED_EXPORT void sg_getTreeROI(SSG_featureTree* a_tree); + //ending SG_APISHARED_EXPORT void sg_getEndingGrowingTrees( std::vector& lineEndings, @@ -141,6 +151,16 @@ SG_APISHARED_EXPORT void sg_getEndingGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam); +//endingֱɨʱֻˮƽˮƽɨʱֻдֱ +SG_APISHARED_EXPORT void sg_getEndingGrowingTrees_angleCheck( + std::vector& lineEndings, + SVzNL3DLaserLine* laser3DPoints, + bool isVScan, + int featureType, + std::vector& trees, + SSG_treeGrowParam growParam, + double angleCheckScale); + //ɨϵ SG_APISHARED_EXPORT void sg_LVFeatureGrowing( std::vector& lineFeatures, @@ -267,6 +287,12 @@ SG_APISHARED_EXPORT void lineFitting( double* _k, double* _b); +SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint( + double x0, + double y0, + double k, + double b); + //Bresenham㷨 SG_APISHARED_EXPORT void drawLine( int x0, @@ -308,6 +334,13 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs( int lineNum, std::vector& ROIs); +//һƽƽ +//ROIڵĵƽϣƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( + std::vector< std::vector>& scanLines, + SVzNL3DRangeD roi); + // תŷǣZ-Y-X˳ SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]); // ŷǼתZ-Y-X˳ diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index c2e415e..3ec07fd 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -219,6 +219,7 @@ typedef struct double tree_value; SSG_ROIRectD roi; std::vector< SSG_basicFeature1D> treeNodes; + int angleChkScalePos; //ڼangleCheckٶ }SSG_featureTree; typedef struct diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index bb1da62..ead9928 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -145,6 +145,17 @@ void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b) *_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//ݹʽb } +SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b) +{ + double A = k; + double B = -1; + double C = b; + SVzNL2DPointD foot; + foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B); + foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B); + return foot; +} + #if 0 void icvprCcaByTwoPass(const cv::Mat& binImg, cv::Mat& lableImg) { @@ -1554,6 +1565,132 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( return planePara; } +SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( + std::vector< std::vector>& scanLines, + SVzNL3DRangeD roi) +{ + //óʼ + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + + int lineNum = scanLines.size(); + //ȡ + std::vector Points3ds; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + + if (pt3D->pt3D.z < 1e-4) + continue; + + bool isValid = false; + + if ((pt3D->pt3D.x >= roi.xRange.min) && (pt3D->pt3D.x <= roi.xRange.max) && + (pt3D->pt3D.y >= roi.yRange.min) && (pt3D->pt3D.y <= roi.yRange.max) && + (pt3D->pt3D.z >= roi.zRange.min) && (pt3D->pt3D.y <= roi.zRange.max)) + { + cv::Point3f a_vldPt; + a_vldPt.x = (float)pt3D->pt3D.x; + a_vldPt.y = (float)pt3D->pt3D.y; + a_vldPt.z = (float)pt3D->pt3D.z; + Points3ds.push_back(a_vldPt); + } + } + } + + //ƽ + std::vector planceFunc; + vzCaculateLaserPlane(Points3ds, planceFunc); + +#if 1 //תתʹԪ + Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]); + Vector3 b = Vector3(0, 0, -1.0); + Quaternion quanPara = rotationBetweenVectors(a, b); + + RotationMatrix rMatrix; + quaternionToMatrix(quanPara, rMatrix.data); + //㷴ת + Quaternion invQuanPara = rotationBetweenVectors(b, a); + RotationMatrix invMatrix; + quaternionToMatrix(invQuanPara, invMatrix.data); +#else //ƽķŷǣת + // + SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]); + //У + eulerPra.roll = eulerPra.roll; + eulerPra.pitch = eulerPra.pitch; + eulerPra.yaw = eulerPra.yaw; + RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll); +#endif + + planePara.planeCalib[0] = rMatrix.data[0][0]; + planePara.planeCalib[1] = rMatrix.data[0][1]; + planePara.planeCalib[2] = rMatrix.data[0][2]; + planePara.planeCalib[3] = rMatrix.data[1][0]; + planePara.planeCalib[4] = rMatrix.data[1][1]; + planePara.planeCalib[5] = rMatrix.data[1][2]; + planePara.planeCalib[6] = rMatrix.data[2][0]; + planePara.planeCalib[7] = rMatrix.data[2][1]; + planePara.planeCalib[8] = rMatrix.data[2][2]; + + planePara.invRMatrix[0] = invMatrix.data[0][0]; + planePara.invRMatrix[1] = invMatrix.data[0][1]; + planePara.invRMatrix[2] = invMatrix.data[0][2]; + planePara.invRMatrix[3] = invMatrix.data[1][0]; + planePara.invRMatrix[4] = invMatrix.data[1][1]; + planePara.invRMatrix[5] = invMatrix.data[1][2]; + planePara.invRMatrix[6] = invMatrix.data[2][0]; + planePara.invRMatrix[7] = invMatrix.data[2][1]; + planePara.invRMatrix[8] = invMatrix.data[2][2]; + +#if 0 //test: ij˻ǵλ + double testMatrix[3][3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + testMatrix[i][j] = 0; + for (int m = 0; m < 3; m++) + testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j]; + } + } +#endif + //ݽת + SVzNLRangeD calibZRange = { 0, -1 }; + for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) + { + cv::Point3f a_calibPt; + a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]); + a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]); + a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]); + //z + if (calibZRange.max < calibZRange.min) + { + calibZRange.min = a_calibPt.z; + calibZRange.max = a_calibPt.z; + } + else + { + if (calibZRange.min > a_calibPt.z) + calibZRange.min = a_calibPt.z; + if (calibZRange.max < a_calibPt.z) + calibZRange.max = a_calibPt.z; + } + } + planePara.planeHeight = calibZRange.min; + + return planePara; +} + //һƽƽ //ROIڵĵƽϣƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 6dc6cfd..111db32 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -6,3 +6,7 @@ #define SG_ERR_NOT_GRID_FORMAT -1003 #define SG_ERR_LABEL_INFO_ERROR -1004 #define SG_ERR_INVLD_SORTING_MODE -1005 + +//BQ_workpiece +#define SX_ERR_INVLD_VTREE_NUM -2001 +#define SX_ERR_INVLD_HTREE_NUM -2002 \ No newline at end of file diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 2cfd789..465dbfd 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -81,6 +81,79 @@ bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vect } return false; } + +int _getMatchedTree_angleCheck( + SSG_basicFeature1D& a_feature, + const int lineIdx, + std::vector& trees, + SSG_treeGrowParam growParam, + double angleCheckScale) +{ + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) + { + SSG_featureTree& a_tree = trees[i]; + if (TREE_STATE_DEAD == a_tree.treeState) + continue; + // + SSG_basicFeature1D last_node = a_tree.treeNodes.back(); + if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //xΪlineIdxͬһɨϵIJ + continue; + + //ж + double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y); + double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z); + int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x); + double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x); + bool typeMatch = _checkTypeMatch(a_tree.treeType, a_feature.featureType); + if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) && + ((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)) && (true == typeMatch)) + { + int nodeSize = (int)a_tree.treeNodes.size(); + bool isValid = true; + + double dist = 0; + if (a_tree.angleChkScalePos < 0) + { + SSG_basicFeature1D& scale_node = a_tree.treeNodes[0]; + double dist = sqrt(pow(a_feature.jumpPos.x - scale_node.jumpPos.x, 2) + + pow(a_feature.jumpPos.y - scale_node.jumpPos.y, 2)); + if (dist >= angleCheckScale) + { + a_tree.angleChkScalePos = 0; + //Ƕ + double tanValue = abs(a_feature.jumpPos.y - scale_node.jumpPos.y) / + abs(a_feature.jumpPos.x - scale_node.jumpPos.x); + if (tanValue > 1.0) //Ӧ45 + isValid = false; + } + } + else + { + for (int m = a_tree.angleChkScalePos+1; m < nodeSize; m++) + { + SSG_basicFeature1D& a_node = a_tree.treeNodes[m]; + double dist = sqrt(pow(a_feature.jumpPos.x - a_node.jumpPos.x, 2) + + pow(a_feature.jumpPos.y - a_node.jumpPos.y, 2)); + if (dist < angleCheckScale) + { + a_tree.angleChkScalePos = m - 1; + break; + } + } + SSG_basicFeature1D& scale_node = a_tree.treeNodes[a_tree.angleChkScalePos]; + //Ƕ + double tanValue = abs(a_feature.jumpPos.y - scale_node.jumpPos.y) / + abs(a_feature.jumpPos.x - scale_node.jumpPos.x); + if (tanValue > 1.0) //Ӧ45 + isValid = false; + } + if(true == isValid) + return i; + } + } + return -1; +} + #if 0 //featuretreesѰҺʵûкʵ㣬 false //ûʹȫƥ䡣һfeatureһƥϣƥɡûʹƥ䡣 @@ -487,11 +560,13 @@ void sg_getEndingGrowingTrees( { //µ SSG_featureTree a_newTree; + memset(&a_newTree, 0, sizeof(SSG_featureTree)); a_newTree.treeNodes.push_back(endingFeature); a_newTree.treeState = TREE_STATE_ALIVE; a_newTree.treeType = endingFeature.featureType; a_newTree.sLineIdx = an_ending.x; a_newTree.eLineIdx = an_ending.x; + a_newTree.angleChkScalePos = -1; trees.push_back(a_newTree); } @@ -517,6 +592,74 @@ void sg_getEndingGrowingTrees( } } +void sg_getEndingGrowingTrees_angleCheck( + std::vector& lineEndings, + SVzNL3DLaserLine* laser3DPoints, + bool isVScan, + int featureType, + std::vector& trees, + SSG_treeGrowParam growParam, + double angleCheckScale) +{ + for (int i = 0, i_max = (int)lineEndings.size(); i < i_max; i++) + { + if (i == 175) + int kkk = 1; + SSG_2DValueI& an_ending = lineEndings[i]; + SSG_basicFeature1D endingFeature; + endingFeature.featureType = featureType; + endingFeature.jumpPos2D.x = an_ending.x; + endingFeature.jumpPos2D.y = an_ending.y; + if (true == isVScan) + endingFeature.jumpPos = laser3DPoints[an_ending.x].p3DPosition[an_ending.y].pt3D; + else + { + endingFeature.jumpPos.x = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.y; + endingFeature.jumpPos.y = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.x; + endingFeature.jumpPos.z = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.z; + } + int matchedTree = _getMatchedTree_angleCheck(endingFeature, an_ending.x, trees, growParam, angleCheckScale); + if (matchedTree >= 0) + { + trees[matchedTree].eLineIdx = an_ending.x; + trees[matchedTree].treeNodes.push_back(endingFeature); + trees[matchedTree].tree_value += endingFeature.featureValue; + } + else + { + //µ + SSG_featureTree a_newTree; + memset(&a_newTree, 0, sizeof(SSG_featureTree)); + a_newTree.treeNodes.push_back(endingFeature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = endingFeature.featureType; + a_newTree.sLineIdx = an_ending.x; + a_newTree.eLineIdx = an_ending.x; + a_newTree.angleChkScalePos = -1; + trees.push_back(a_newTree); + } + + //ֹͣ,١ + //ڵΪ1Ƴ + int lineIdx = an_ending.x; + int m_max = (int)trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == trees[m].treeState) + { + int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (i == i_max - 1)) + { + trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + trees.erase(trees.begin() + m); + } + } + } + } +} //ɨߵV void sg_LVFeatureGrowing( std::vector& lineFeatures, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index ba3fde9..60bfa39 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -144,7 +144,12 @@ void sg_lineSegSmoothing( } //˳Ⱥ㣺z޷ -void sg_lineDataRemoveOutlier(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector& filerData, std::vector& noisePts) +void sg_lineDataRemoveOutlier( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam, + std::vector& filerData, + std::vector& noisePts) { filerData.resize(dataSize); @@ -1438,6 +1443,350 @@ void sg_getLineCornerFeature( return; } +/// +/// ȡϵĹյˮƽ˵ΪCorner +/// Segcornerȡ +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void sg_getLineCornerFeature_BQ( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + double refSteppingZ, + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector& line_features) +{ + //ȥ + std::vector< SVzNL3DPosition> vldPts; + std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 370) + int kkk = 1; + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + double y_diff = abs(lineData[i].pt3D.y - pre_y); + if ((z_diff < cornerPara.minEndingGap_z) && (y_diff < cornerPara.minEndingGap)) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + pre_y = lineData[i].pt3D.y; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //ñ־ + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int idx1 = segs[i].start; + int idx2 = segs[i].start + segs[i].len - 1; + lineData[idx1].nPointIdx |= 0x100000; + lineData[idx2].nPointIdx |= 0x200000; + } + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if ((lineIdx == 399) && (i == 419)) + int kkk = 1; + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if ( (dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j])) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if ( (dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j])) + { + post_i = j; + break; + } + } + //ս + double tanValue_pre = 0; + if (pre_i >= 0) + tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = 0; + if (post_i >= 0) + tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + + if ((pre_i < 0) && (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + if ((pre_i >= 0) && (post_i >= 0)) + { + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + else + { + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + if (cornerPeakP[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + //жǷ䵽ˮƽƽ + SSG_basicFeature1D a_feature; + a_feature.featureType = 0; + if (abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else if (abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + if (a_feature.featureType > 0) + { + int idx = vldPts[cornerPtIdx].nPointIdx; + lineData[idx].nPointIdx |= (a_feature.featureType << 16); + } + } + } + //Feature + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPosition a_pt = lineData[i]; + int flag = a_pt.nPointIdx >> 16; + int endFlag = flag >> 4; + flag &= 0x0f; + + if (flag > 0) + { + double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + if (diffZ < 10.0) + { + SSG_basicFeature1D a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); + a_feature.featureType = flag; + a_feature.jumpPos = a_pt.pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + line_features.push_back(a_feature); + } + } + else if (1 == endFlag) + { + int cornerIdx = backIndexing[i]; + if (abs(corners[cornerIdx].forwardAngle) < cornerPara.jumpCornerTh_1) + { + double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + if (diffZ < 10.0) + { + SSG_basicFeature1D a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + a_feature.jumpPos = a_pt.pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + line_features.push_back(a_feature); + } + } + } + else if (2 == endFlag) + { + int cornerIdx = backIndexing[i]; + if (abs(corners[cornerIdx].backwardAngle) < cornerPara.jumpCornerTh_1) + { + double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + if (diffZ < 10.0) + { + SSG_basicFeature1D a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + a_feature.jumpPos = a_pt.pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + line_features.push_back(a_feature); + } + } + } + } + return; +} + bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { return a.pntIdx < b.pntIdx; }