更新拆包库

2025-10-23 22:20:30 +08:00 · 2025-10-23 22:20:30 +08:00 · 4ab2793a33
commit 4ab2793a33
parent e8460a1415 7f18d0f92a
15 changed files with 952 additions and 98 deletions
--- 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
--- 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
--- a/bagPositioning_test/bagPositioning_test.cpp
+++ b/bagPositioning_test/bagPositioning_test.cpp
@ -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;
--- 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];
--- a/lapWeldDetection/lapWeldDetection.vcxproj
+++ b/lapWeldDetection/lapWeldDetection.vcxproj
@ -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" />
--- 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<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度角
--- a/lapWeldDetection_test/lapWeldDetection_test.vcxproj
+++ b/lapWeldDetection_test/lapWeldDetection_test.vcxproj
@ -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>
--- 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<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为量化尺度
--- a/sourceCode/SG_bagPositioning_Export.h
+++ b/sourceCode/SG_bagPositioning_Export.h
@ -34,6 +34,7 @@ typedef struct
 	SSG_treeGrowParam growParam;
 	//SSG_objSortParam sortParam;
 	//SSG_polarScanParam polarScanParam;
+	int supportRotate;
 }SG_bagPositionParam;

 typedef struct
--- a/sourceCode/SG_baseAlgo_Export.h
+++ b/sourceCode/SG_baseAlgo_Export.h
@ -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顺序）
--- 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
--- a/sourceCode/SG_baseFunc.cpp
+++ b/sourceCode/SG_baseFunc.cpp
@ -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以内的点进行平面拟合，计算调平参数
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
--- 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
--- 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<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, 
--- 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<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;
 }