jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新拆包库

Browse Source
This commit is contained in:
jerryzeng 2025-10-23 22:20:30 +08:00
commit 4ab2793a33
15 changed files with 952 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
QRcode3Ddetection_test/QRcode3Ddetection_test.cpp
View File

@ -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

27
SG_Algorithm.sln
View File

@ -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

117
bagPositioning_test/bagPositioning_test.cpp
View File

@ -629,12 +629,13 @@ void _convertToGridData_XYZRGB_vector(std::vector<std::vector< SPointXYZRGB>>& 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++)
@ -2688,8 +2689,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}
};
@ -2713,7 +2714,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)
{
@ -2726,6 +2727,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))
{
@ -2738,6 +2746,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)
{
@ -2750,6 +2765,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)
{
@ -2762,6 +2784,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数据
{
@ -2774,6 +2803,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图像
{
@ -2786,6 +2822,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图像
{
@ -2798,6 +2841,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图像
{
@ -2810,11 +2860,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;
@ -2822,6 +2879,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))
{
@ -2834,6 +2898,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
{
@ -2846,22 +2917,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)
{
@ -2934,7 +2997,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)) //箱子
@ -2973,9 +3036,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;

18
beltTearingDetection_test/beltTearingDetection_test.cpp
View File

@ -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];

14
lapWeldDetection/lapWeldDetection.vcxproj
View File

@ -85,12 +85,12 @@
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<LinkIncremental>true</LinkIncremental>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<LinkIncremental>false</LinkIncremental>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
@ -134,14 +134,15 @@
<ConformanceMode>true</ConformanceMode>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
<AdditionalIncludeDirectories>..\thirdParty\OpenCV320\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableUAC>false</EnableUAC>
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
@ -154,7 +155,8 @@
<ConformanceMode>true</ConformanceMode>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
<AdditionalIncludeDirectories>..\thirdParty\OpenCV320\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
@ -163,7 +165,7 @@
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableUAC>false</EnableUAC>
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
</Link>
</ItemDefinitionGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />

18
lapWeldDetection_test/lapWeldDetection_test.cpp
View File

@ -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<SVzNL3DPosition>>& 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度角

14
lapWeldDetection_test/lapWeldDetection_test.vcxproj
View File

@ -120,13 +120,14 @@
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;</AdditionalIncludeDirectories>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalDependencies>opencv_world480d.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320d.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
@ -137,15 +138,16 @@
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;</AdditionalIncludeDirectories>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
<AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalDependencies>opencv_world480.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
</Link>
</ItemDefinitionGroup>
<ItemGroup>

166
sourceCode/SG_bagPositioning.cpp
View File

@ -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<SSG_conotourPai
SVzNLRangeD LR_range = { LR_pairs[LR_id].roi.left, LR_pairs[LR_id].roi.right };
std::vector<SSG_intPair> TB_idPairs;
std::vector<SSG_intPair> 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::vector<SSG_conotourPai
SVzNLRangeD LR_range = { match_LR->roi.left, match_LR->roi.right };
std::vector<SSG_intPair> TB_idPairs;
std::vector<SSG_intPair> 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<SSG_conotourPai
SVzNLRangeD LR_range = { LR_pairs[LR_bagL].roi.left, LR_pairs[LR_bagL].roi.right };
std::vector<SSG_intPair> TB_idPairs;
std::vector<SSG_intPair> 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<SSG_conotourPai
SVzNLRangeD LR_range = { LR_pairs[LR_bagW].roi.left, LR_pairs[LR_bagW].roi.right };
std::vector<SSG_intPair> TB_idPairs;
std::vector<SSG_intPair> 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<SSG_conotourPai
LR_range = { LR_pairs[LR_bagL].roi.left, LR_pairs[LR_bagL].roi.right };
std::vector<SSG_intPair> TB_idPairs_1;
std::vector<SSG_intPair> 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<SSG_featureTree> hEdge_0_trees;
std::vector<SSG_featureTree> 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);
//距离变换Mask，以1mm为量化尺度

1
sourceCode/SG_bagPositioning_Export.h
View File

@ -34,6 +34,7 @@ typedef struct
SSG_treeGrowParam growParam;
//SSG_objSortParam sortParam;
//SSG_polarScanParam polarScanParam;
int supportRotate;
}SG_bagPositionParam;
typedef struct

33
sourceCode/SG_baseAlgo_Export.h
View File

@ -69,6 +69,14 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature(
const SSG_cornerParam cornerPara, //scale通常取bagH的1/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<SSG_basicFeature1D>& line_features);
/// <summary>
/// 提取激光线上的Jumping特征
/// nPointIdx被重新定义成Feature类型
@ -141,6 +149,8 @@ SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
std::vector<SSG_featureTree>& trees,
SSG_treeGrowParam growParam);
//SG_APISHARED_EXPORT void sg_getTreeROI(SSG_featureTree* a_tree);
//对ending进行生长
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees(
std::vector<SSG_2DValueI>& lineEndings,
@ -150,6 +160,16 @@ SG_APISHARED_EXPORT void sg_getEndingGrowingTrees(
std::vector<SSG_featureTree>& trees,
SSG_treeGrowParam growParam);
//对ending进行生长，垂直扫描时只进行水平生长；水平扫描时只进行垂直生长
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees_angleCheck(
std::vector<SSG_2DValueI>& lineEndings,
SVzNL3DLaserLine* laser3DPoints,
bool isVScan,
int featureType,
std::vector<SSG_featureTree>& trees,
SSG_treeGrowParam growParam,
double angleCheckScale);
//对扫描线上的
SG_APISHARED_EXPORT void sg_LVFeatureGrowing(
std::vector<SSG_lineFeature>& lineFeatures,
@ -276,6 +296,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,
@ -317,6 +343,13 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
int lineNum,
std::vector<SVzNL3DRangeD>& ROIs);
//计算一个平面调平参数。
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
SVzNL3DRangeD roi);
// 从旋转矩阵计算欧拉角（Z-Y-X顺序）
SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]);
// 从欧拉角计算旋转矩阵（Z-Y-X顺序）

1
sourceCode/SG_baseDataType.h
View File

@ -219,6 +219,7 @@ typedef struct
double tree_value;
SSG_ROIRectD roi;
std::vector< SSG_basicFeature1D> treeNodes;
int angleChkScalePos; //仅用于加速angleCheck的速度
}SSG_featureTree;
typedef struct

137
sourceCode/SG_baseFunc.cpp
View File

@ -147,6 +147,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)
{
@ -1556,6 +1567,132 @@ SSG_planeCalibPara sg_getPlaneCalibPara2(
return planePara;
}
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
std::vector< std::vector<SVzNL3DPosition>>& 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<cv::Point3f> 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<double> 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: 两个矩阵的乘积必须是单位阵
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以内的点进行平面拟合，计算调平参数
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数

4
sourceCode/SG_errCode.h
View File

@ -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

143
sourceCode/SG_featureGrow.cpp
View File

@ -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<SSG_featureTree>& 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，同一条扫描线上的不进行生长
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
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回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<SSG_2DValueI>& lineEndings,
SVzNL3DLaserLine* laser3DPoints,
bool isVScan,
int featureType,
std::vector<SSG_featureTree>& 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<SSG_lineFeature>& lineFeatures,

351
sourceCode/SG_lineFeature.cpp
View File

@ -144,7 +144,12 @@ void sg_lineSegSmoothing(
}
//滤除离群点：z跳变门限方法
void sg_lineDataRemoveOutlier(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts)
void sg_lineDataRemoveOutlier(
SVzNL3DPosition* lineData,
int dataSize,
SSG_outlierFilterParam filterParam,
std::vector<SVzNL3DPosition>& filerData,
std::vector<int>& noisePts)
{
filerData.resize(dataSize);
@ -1438,6 +1443,350 @@ void sg_getLineCornerFeature(
return;
}
/// <summary>
/// 提取激光线上的拐点特征，水平端点视为Corner
/// 根据Seg进行corner提取
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void sg_getLineCornerFeature_BQ(
SVzNL3DPosition* lineData,
int dataSize,
int lineIdx,
double refSteppingZ,
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
std::vector<SSG_basicFeature1D>& line_features)
{
//去除零点
std::vector< SVzNL3DPosition> vldPts;
std::vector< int> vldPtSegIdx;
std::vector<SSG_RUN> segs;
std::vector<int> 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; //2倍的cornerScale。
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;
}