From e31ec0effd5d8120003913b89aacdf2acd81b237 Mon Sep 17 00:00:00 2001
From: jerryzeng <laohong_0908@qq.com>
Date: Sun, 20 Jul 2025 22:54:30 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=88=86=E6=AE=B5=E5=B9=B3?=
 =?UTF-8?q?=E6=BB=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 sourceCode/SG_bagPositioning.cpp | 16 ++++++++--------
 sourceCode/SG_lineFeature.cpp    | 16 ++++++++++++----
 2 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp
index 5fd70c6..abd2023 100644
--- a/sourceCode/SG_bagPositioning.cpp
+++ b/sourceCode/SG_bagPositioning.cpp
@@ -2383,16 +2383,16 @@ void sg_getBagPosition(
 	//逐行提取特征
 	for (int hLine = 0; hLine < hLineNum; hLine++)
 	{
-if (hLine == 116)
+if (hLine == 14)
 	int kkk = 1;
 		std::vector<SVzNL3DPosition> smoothData;
-		//sg_lineSegSmoothing(
-		//	hLines[hLine],
-		//	algoParam.cornerParam.minEndingGap, //分段的Y间隔。大于此间隔，为新的分段
-		//	algoParam.cornerParam.minEndingGap_z,//分段的Z间隔。大于此间隔，为新的分段
-		//	5,
-		//	smoothData);
-		sg_lineDataSmoothing(hLines[hLine], 5, smoothData);
+		sg_lineSegSmoothing(
+			hLines[hLine],
+			algoParam.cornerParam.minEndingGap, //分段的Y间隔。大于此间隔，为新的分段
+			algoParam.cornerParam.minEndingGap_z,//分段的Z间隔。大于此间隔，为新的分段
+			5,
+			smoothData);
+		//sg_lineDataSmoothing(hLines[hLine], 5, smoothData);
 		SSG_lineFeature a_hLine_featrues;
 		a_hLine_featrues.lineIdx = hLine;
 #if BAG_ALGO_USE_CORNER_FEATURE
diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp
index 23d9ca2..cf028ed 100644
--- a/sourceCode/SG_lineFeature.cpp
+++ b/sourceCode/SG_lineFeature.cpp
@@ -94,7 +94,7 @@ void sg_lineDataSmoothing(
 	return;
 }
 
-//对扫描线按分段进行平滑
+//对扫描线按分段进行平滑。
 void sg_lineSegSmoothing(
 	std::vector<SVzNL3DPosition>& input, 
 	double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
@@ -102,21 +102,25 @@ void sg_lineSegSmoothing(
 	int smoothWin, 
 	std::vector<SVzNL3DPosition>& output)
 {
-	output.resize(input.size()); // 预分配足够的空间
-	std::copy(input.begin(), input.end(), output.begin());
+	//output.resize(input.size()); // 预分配足够的空间
+	//std::copy(input.begin(), input.end(), output.begin());
 
 	int dataSize = input.size();
 	//计算分段
 	std::vector<std::vector<SVzNL3DPosition>> segs;
 	std::vector<SVzNL3DPosition> a_seg;
+	int segStart = -1;
 	for (int i = 0; i < dataSize; i++)
 	{
 		SVzNL3DPosition a_pt = input[i];
 		//seg判断
 		if (a_pt.pt3D.z > 1e-4)
 		{
-			if (a_seg.size() == 0)
+			if (segStart < 0)
+			{
 				a_seg.push_back(a_pt);
+				segStart = 1;
+			}
 			else  //检查两点距离
 			{
 				SVzNL3DPosition pre_pt = a_seg.back();
@@ -128,8 +132,12 @@ void sg_lineSegSmoothing(
 					a_seg.clear();
 					a_seg.push_back(a_pt);
 				}
+				else
+					a_seg.push_back(a_pt);
 			}
 		}
+		else
+			a_seg.push_back(a_pt);
 	}
 	//last
 	if (a_seg.size() > 0)