Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib

bagThreadPositioning_Export/bagThreadPositioning_test.cpp

@@ -461,12 +461,16 @@ int main()
 			growParam.minLTypeTreeLen = 10; //mm,
 			growParam.minVTypeTreeLen = 10; //mm
 
-			bool isHorizonScan = false;  //true:激光线平行线缝；false:激光线垂直线缝
+			SSX_ScanInfo scanInfo;
+			scanInfo.isHorizonScan = false;  //true:激光线平行线缝；false:激光线垂直线缝
+			scanInfo.scanFromThreadHead = false; //true:线袋子线缝头部开始扫描。
+			scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度
+			scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置
 			int errCode = 0;
 			std::vector<SSX_bagThreadInfo> bagThreadInfo;
 			wd_bagThreadPositioning(
 				scanLines,
-				isHorizonScan,  //true:激光线平行线缝；false:激光线垂直线缝
+				scanInfo,  //true:激光线平行线缝；false:激光线垂直线缝
 				filterParam, //噪点过滤参数
 				cornerParam, //V型特征参数
 				raisedFeatureParam,//线尾凸起参数

sourceCode/SG_baseAlgo_Export.h

@@ -166,6 +166,24 @@ SG_APISHARED_EXPORT void wd_getLineCorerFeature(
     std::vector<SSG_basicFeature1D>& line_cornerFeatures  //拐点
 );
 
+/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
+/// 使用平均点距进行加速
+/// nPointIdx被重新定义成Feature类型
+/// 算法流程：
+/// （1）逐点计算前向角和后向角
+/// （2）逐点计算拐角，顺时针为负，逆时针为正
+/// （3）搜索正拐角的极大值。
+/// （4）判断拐角是否为跳变
+/// </summary>
+SG_APISHARED_EXPORT void wd_getLineCorerFeature_accelerate(
+    std::vector< SVzNL3DPosition>& lineData,
+    int lineIdx,
+    const SSG_cornerParam cornerPara,
+    const double pointInterval,
+    std::vector<SSG_RUN_EX>& segs,
+    std::vector<SSG_basicFeature1D>& line_cornerFeatures  //拐点特征
+);
+
 //提取凸起段
 SG_APISHARED_EXPORT void wd_getLineRaisedFeature(
     std::vector< SVzNL3DPosition>& lineData,

sourceCode/SG_lineFeature.cpp

@@ -3724,6 +3724,9 @@ void _searchCornerPeaks(
 		if (i == 275)
 			int kkk = 1;
 		SSG_pntDirAngle* curr_data = &corners[i];
+		if (curr_data->pntIdx < 0)
+			continue;
+
 		if (curr_data->pntIdx < 0)
 		{
 			if (i == i_max - 1)  //最后一个
@@ -4071,6 +4074,202 @@ void wd_getLineCorerFeature(
 	return;
 }
 
+/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
+/// 使用平均点距进行加速
+/// nPointIdx被重新定义成Feature类型
+/// 算法流程：
+/// （1）逐点计算前向角和后向角
+/// （2）逐点计算拐角，顺时针为负，逆时针为正
+/// （3）搜索正拐角的极大值。
+/// （4）判断拐角是否为跳变
+/// </summary>
+void wd_getLineCorerFeature_accelerate(
+	std::vector< SVzNL3DPosition>& lineData,
+	int lineIdx,
+	const SSG_cornerParam cornerPara,
+	const double pointInterval,
+	std::vector<SSG_RUN_EX>& segs,
+	std::vector<SSG_basicFeature1D>& line_cornerFeatures  //拐点特征
+)
+{
+	int dataSize = (int)lineData.size();
+	//去除零点
+	int runIdx = 1;
+	SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx
+	double pre_z = 0;
+	double pre_y = 0;
+	for (int i = 0; i < dataSize; i++)
+	{
+		if (i == 1100)
+			int kkk = 1;
+		lineData[i].nPointIdx = i;  //重新编序号
+		if (lineData[i].pt3D.z > 1e-4)
+		{
+			if (a_run.len < 0)
+			{
+				a_run.start_zRising = true;
+				a_run.start = i;
+				a_run.len = 1;
+				a_run.end_zRising = true;
+				a_run.value = i;
+			}
+			else
+			{
+				double z_diff = abs(lineData[i].pt3D.z - pre_z);
+				if (z_diff < cornerPara.minEndingGap_z)
+				{
+					a_run.len = i - a_run.start + 1;
+					a_run.value = i;
+				}
+				else
+				{
+					bool next_zRising;
+					if (pre_z > lineData[i].pt3D.z)
+					{
+						a_run.end_zRising = true;
+						next_zRising = false;
+					}
+					else
+					{
+						a_run.end_zRising = false;
+						next_zRising = true;
+					}
+					a_run.value = runIdx;
+					runIdx++;
+					segs.push_back(a_run);
+
+					a_run.start = i;
+					a_run.start_zRising = next_zRising;
+					a_run.len = 1;
+					a_run.value = i;
+					a_run.end_zRising = true;
+				}
+			}
+			//vldPtSegIdx.push_back(runIdx);
+
+			pre_z = lineData[i].pt3D.z;
+		}
+	}
+	if (a_run.len > 0)
+		segs.push_back(a_run);
+
+	//计算前向角和后向角
+	std::vector< SSG_pntDirAngle> corners;
+	corners.resize(lineData.size());
+	//逐段进行
+	int ptSkipping = cornerPara.scale / pointInterval;
+	int segSize = (int)segs.size();
+	for (int segIdx = 0; segIdx < segSize; segIdx++)
+	{
+		int vPtIdxStart = segs[segIdx].start;
+		int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
+		for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
+		{
+			if (i == 1100)
+				int kkk = 1;
+
+			if (lineData[i].pt3D.z < 1e-4)
+			{
+				corners[i].pntIdx = -1;
+				corners[i].forwardAngle = 0;
+				corners[i].backwardAngle = 0;
+				corners[i].corner = 0;
+				corners[i].forwardDiffZ = 0;
+				corners[i].backwardDiffZ = 0;
+				continue;
+			}
+			//前向寻找
+			int pre_i = -1;
+			int searchStart = i - ptSkipping;
+			if (searchStart >= vPtIdxStart)
+			{
+				for (int j = searchStart; j < i; j++)
+				{
+					if (lineData[j].pt3D.z > 1e-4)
+					{
+						double dist = sqrt(pow(lineData[i].pt3D.y - lineData[j].pt3D.y, 2) +
+							pow(lineData[i].pt3D.z - lineData[j].pt3D.z, 2));
+						if (dist < cornerPara.scale)
+							break;
+						else
+							pre_i = j;
+					}
+				}
+			}
+			//后向寻找
+			int post_i = -1;
+			int searchEnd = i + ptSkipping;
+			if (searchEnd <= vPtIdxEnd)
+			{
+				for (int j = searchEnd; j > i; j--)
+				{
+					if (lineData[j].pt3D.z > 1e-4)
+					{
+						double dist = sqrt(pow(lineData[i].pt3D.y - lineData[j].pt3D.y, 2) +
+							pow(lineData[i].pt3D.z - lineData[j].pt3D.z, 2));
+						if (dist < cornerPara.scale)
+							break;
+						else
+							post_i = j;
+					}
+				}
+			}
+			//计算拐角
+			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
+			{
+				double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y);
+				double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y);
+				double forwardAngle = atan(tanValue_post) * 180.0 / PI;
+				double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
+				corners[i].pntIdx = i;
+				corners[i].forwardAngle = forwardAngle;
+				corners[i].backwardAngle = backwardAngle;
+				corners[i].corner = -(forwardAngle - backwardAngle);  //图像坐标系与正常坐标系y方向相反，所以有“-”号
+				corners[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z;
+				corners[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z;
+			}
+		}
+	}
+
+	std::vector< SSG_pntDirAngle> cornerPeakP;
+	std::vector< SSG_pntDirAngle> cornerPeakM;
+	double cornerMergeScale = cornerPara.scale * 2;
+	//提取corner极值
+	_searchCornerPeaks(
+		corners,
+		lineData,
+		cornerPara,
+		cornerMergeScale,
+		cornerPeakP,
+		cornerPeakM
+	);
+	for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
+	{
+		SSG_basicFeature1D a_feature;
+		if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2))
+			a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
+		else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2))
+			a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
+		else
+			a_feature.featureType = LINE_FEATURE_CORNER_V;
+
+		int cornerPtIdx = cornerPeakP[i].pntIdx;
+		a_feature.jumpPos = lineData[cornerPtIdx].pt3D;
+		a_feature.jumpPos2D = { lineIdx, lineData[cornerPtIdx].nPointIdx };
+		line_cornerFeatures.push_back(a_feature);
+	}
+	return;
+}
+
 //提取凸起段
 void wd_getLineRaisedFeature(
 	std::vector< SVzNL3DPosition>& lineData,

sourceCode/bagThreadPositioning.cpp

@@ -15,7 +15,7 @@ const char* wd_bagThreadPositioningVersion(void)
 //线头位置检测定位
 void wd_bagThreadPositioning(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
-	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
+	const SSX_ScanInfo scanInfo,  //true:激光线平行槽道；false:激光线垂直槽道
 	const SSG_outlierFilterParam filterParam, //噪点过滤参数
 	const SSG_cornerParam cornerPara, //V型特征参数
 	const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
@@ -49,6 +49,9 @@ void wd_bagThreadPositioning(
 	}
 
 	//生成水平扫描数据
+	//统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速
+	double ptInterval = 0;
+	int ptIntevalNum = 0;
 	std::vector< std::vector<SVzNL3DPosition>> data_lines_h;  //水平扫描数据
 	data_lines_h.resize(linePtNum);
 	for (int i = 0; i < linePtNum; i++)
@@ -61,20 +64,54 @@ void wd_bagThreadPositioning(
 			data_lines_h[j][line] = scanLines[line][j];
 			data_lines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
 			data_lines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
+			if (j > 0)
+			{
+				if ((scanLines[line][j - 1].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z > 1e-4))
+				{
+					ptInterval += abs(scanLines[line][j].pt3D.y - scanLines[line][j - 1].pt3D.y);
+					ptIntevalNum++;
+				}
+			}
 		}
 	}
+	if(ptIntevalNum == 0)
+	{
+		*errCode = SG_ERR_3D_DATA_NULL;
+		return;
+	}
+	ptInterval = ptInterval / ptIntevalNum;
 	int lineNum_h = linePtNum;
 	int linePtNum_h = (int)data_lines_h[0].size();
+	double lineInterval = 0;
+	int lineIntervalNum = 0;
 	for (int line = 0; line< lineNum_h; line++)
 	{
 		for (int j = 0, j_max = (int)data_lines_h[line].size(); j < j_max; j++)
+		{
 			data_lines_h[line][j].nPointIdx = j;
+			if (j > 0)
+			{
+				if ((data_lines_h[line][j - 1].pt3D.z > 1e-4) && (data_lines_h[line][j].pt3D.z > 1e-4))
+				{
+					lineInterval += abs(data_lines_h[line][j].pt3D.y - data_lines_h[line][j - 1].pt3D.y);
+					lineIntervalNum++;
+				}
+			}
+		}
 	}
+	if (lineIntervalNum == 0)
+	{
+		*errCode = SG_ERR_3D_DATA_NULL;
+		return;
+	}
+	lineInterval = lineInterval / lineIntervalNum;
 
+	double vCornerScale = cornerPara.scale * 4;
 	std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
 	std::vector<std::vector<SWD_segFeature>> raisedFeatures;
-	if (false == isHorizonScan)
+	if (false == scanInfo.isHorizonScan)
 	{
+		int validVCornerSCale = (int)(vCornerScale / ptInterval);
 		//垂直扫描检测V型槽和线尾
 		for (int line = 0; line < lineNum; line++)
 		{
@@ -85,14 +122,38 @@ void wd_bagThreadPositioning(
 			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
 			//提取V型槽
 			std::vector<SSG_basicFeature1D> line_cornerFeatures;
+			std::vector<SSG_RUN_EX> segs;
 			int dataSize = (int)lineData.size();
-			wd_getLineCorerFeature(
+			wd_getLineCorerFeature_accelerate(
 				lineData,
 				line,
 				cornerPara,
+				ptInterval,
+				segs,
 				line_cornerFeatures  //拐点
 			);
-			cornerFeatures.push_back(line_cornerFeatures);
+			//检查V型特征两侧，不能有跳变
+			std::vector<SSG_basicFeature1D> valid_cornerFeatures;
+			int vCornerSize = (int)line_cornerFeatures.size();
+			int segSize = (int)segs.size();
+			for (int m = 0; m < vCornerSize; m++)
+			{
+				int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
+				for (int n = 0; n < segSize; n++)
+				{
+					int segEnd = segs[n].start + segs[n].len - 1;
+					if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
+					{
+						int skip_1 = cornerPos - segs[n].start;
+						int skip_2 = segEnd - cornerPos;
+						if((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
+							valid_cornerFeatures.push_back(line_cornerFeatures[m]);
+						break;
+					}
+				}
+			}
+			cornerFeatures.push_back(valid_cornerFeatures);
+
 			//提取凸起段
 			std::vector<SWD_segFeature> line_raisedFeatures;
 			wd_getLineRaisedFeature(
@@ -110,6 +171,7 @@ void wd_bagThreadPositioning(
 	}
 	else
 	{
+		int validVCornerSCale = (int)(vCornerScale / lineInterval);
 		//水平扫描检测V型槽和线尾
 		for (int line = 0; line < lineNum_h; line++)
 		{
@@ -120,14 +182,37 @@ void wd_bagThreadPositioning(
 			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
 			//提取V型槽
 			std::vector<SSG_basicFeature1D> line_cornerFeatures;
+			std::vector<SSG_RUN_EX> segs;
 			int dataSize = (int)lineData.size();
-			wd_getLineCorerFeature(
+			wd_getLineCorerFeature_accelerate(
 				lineData,
 				line,
 				cornerPara,
+				lineInterval,
+				segs,
 				line_cornerFeatures  //拐点
 			);
-			cornerFeatures.push_back(line_cornerFeatures);
+			//检查V型特征两侧，不能有跳变
+			std::vector<SSG_basicFeature1D> valid_cornerFeatures;
+			int vCornerSize = (int)line_cornerFeatures.size();
+			int segSize = (int)segs.size();
+			for (int m = 0; m < vCornerSize; m++)
+			{
+				int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
+				for (int n = 0; n < segSize; n++)
+				{
+					int segEnd = segs[n].start + segs[n].len - 1;
+					if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
+					{
+						int skip_1 = cornerPos - segs[n].start;
+						int skip_2 = segEnd - cornerPos;
+						if ((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
+							valid_cornerFeatures.push_back(line_cornerFeatures[m]);
+						break;
+					}
+				}
+			}
+			cornerFeatures.push_back(valid_cornerFeatures);
 			//提取凸起段
 			std::vector<SWD_segFeature> line_raisedFeatures;
 			wd_getLineRaisedFeature(
@@ -158,44 +243,112 @@ void wd_bagThreadPositioning(
 		*errCode = SG_ERR_ZERO_OBJECTS;
 		return;
 	}
-
-	int cornerTreeNum = (int)cornerGrowTrees.size();
 	int raisedTreeNum = (int)raisedFeatureGrowTrees.size();
+	int threadTailTreeIdx = -1; //线尾所在的tree
+	if (raisedTreeNum == 1)
+		threadTailTreeIdx = 0;
+	else if (raisedTreeNum > 1)
+	{
+		//取最长的tree作为线尾所在的tree
+		int maxLines = 0;
+		threadTailTreeIdx = 0;
+		for (int i = 0; i < raisedTreeNum; i++)
+		{
+			int treeLines = raisedFeatureGrowTrees[i].eLineIdx - raisedFeatureGrowTrees[i].sLineIdx;
+			if (maxLines < treeLines)
+			{
+				maxLines = treeLines;
+				threadTailTreeIdx = i;
+			}
+		}
+	}
+	int cornerTreeNum = (int)cornerGrowTrees.size();
+	int objTreeIdx = -1;
+	if (threadTailTreeIdx < 0)
+	{
+		int maxLines = 0;
+		//取最长的tree作为线缝所有的生长树
+		for (int i = 0; i < cornerTreeNum; i++)
+		{
+			int treeLines = cornerGrowTrees[i].eLineIdx - cornerGrowTrees[i].sLineIdx;
+			if (maxLines < treeLines)
+			{
+				maxLines = treeLines;
+				objTreeIdx = i;
+			}
+		}
+	}
+	else
+	{
+		//取与线尾所在tree最近的tree作为线缝所在的生长树,计算2D距离，所以不需要区分水平和垂直扫描方式
+
+		SVzNL3DPoint pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].startPt;
+		SVzNL3DPoint pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].endPt;
+		SVzNL3DPoint tail_end_1 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
+		pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().startPt;
+		pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().endPt;
+		SVzNL3DPoint tail_end_2 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
+		double minDist = DBL_MAX;
+		for (int i = 0; i < cornerTreeNum; i++)
+		{
+			//端点最近的距离为两个tree间的距离 
+			SVzNL3DPoint end_1 = cornerGrowTrees[i].treeNodes[0].jumpPos;
+			SVzNL3DPoint end_2 = cornerGrowTrees[i].treeNodes.back().jumpPos;
+			int lineIdx_2 = cornerGrowTrees[i].eLineIdx;
+			double dist_11 = sqrt(pow(tail_end_1.x - end_1.x, 2) + pow(tail_end_1.y - end_1.y, 2));
+			double dist_12 = sqrt(pow(tail_end_1.x - end_2.x, 2) + pow(tail_end_1.y - end_2.y, 2));
+			double dist_21 = sqrt(pow(tail_end_2.x - end_1.x, 2) + pow(tail_end_2.y - end_1.y, 2));
+			double dist_22 = sqrt(pow(tail_end_2.x - end_2.x, 2) + pow(tail_end_2.y - end_2.y, 2));
+			double dist = dist_11 < dist_12 ? dist_11 : dist_12;
+			dist = dist < dist_21 ? dist : dist_21;
+			dist = dist < dist_22 ? dist : dist_22;
+			if (minDist > dist)
+			{
+				minDist = dist;
+				objTreeIdx = i;
+			}
+		}
+	}
+	if(objTreeIdx < 0)
+	{
+		*errCode = SG_ERR_ZERO_OBJECTS;
+		return;
+	}
+	
 	//显示 
 	//将原始数据的序列清0，转义使用
 	for (int line = 0; line < lineNum; line++)
 		for (int j = 0; j < linePtNum; j++)
 			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
 	//置标志，用于debug
-	for (int i = 0; i < cornerTreeNum; i++)
 	{
-		int nodeNum = (int)cornerGrowTrees[i].treeNodes.size();
+		int nodeNum = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
 		for (int j = 0; j < nodeNum; j++)
 		{
 			int lineIdx, ptIdx;
-			if (false == isHorizonScan)
+			if (false == scanInfo.isHorizonScan)
 			{
-				lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x;
+				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
-				ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y;
+				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
 			}
 			else
 			{
-				lineIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.y;
+				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
-				ptIdx = cornerGrowTrees[i].treeNodes[j].jumpPos2D.x;
+				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
 			}
 			scanLines[lineIdx][ptIdx].nPointIdx = 1;
 		}
 	}
-	for (int i = 0; i < raisedTreeNum; i++)
+	if (threadTailTreeIdx >= 0)
 	{
-		int nodeNum = (int)raisedFeatureGrowTrees[i].treeNodes.size();
+		int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
 		for (int j = 0; j < nodeNum; j++)
 		{
 			int lineIdx, ptIdx;
-			if (false == isHorizonScan)
+			if (false == scanInfo.isHorizonScan)
 			{
-				lineIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx;
+				lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
-				for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++)
+				for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
 				{
 					ptIdx = m;
 					scanLines[lineIdx][ptIdx].nPointIdx = 2;
@@ -203,8 +356,8 @@ void wd_bagThreadPositioning(
 			}
 			else
 			{
-				ptIdx = raisedFeatureGrowTrees[i].treeNodes[j].lineIdx;
+				ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
-				for (int m = raisedFeatureGrowTrees[i].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[i].treeNodes[j].endPtIdx; m++)
+				for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
 				{
 					lineIdx = m;
 					scanLines[lineIdx][ptIdx].nPointIdx = 2;
@@ -212,6 +365,178 @@ void wd_bagThreadPositioning(
 			}
 		}
 	}
+	//提取针脚(提取4个）
+	std::vector<SVzNLRect> stitchROIs;
+	int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
+	//提取针脚范围
+	int pre_idx = -1;
+	for (int i = 0; i < nodeSize; i++)
+	{
+		int nodeIdx = (true == scanInfo.scanFromThreadHead) ? i : (nodeSize - 1 - i);
+		SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];	
+		if(pre_idx >= 0)
+		{
+			SSG_basicFeature1D& pre_node = cornerGrowTrees[objTreeIdx].treeNodes[pre_idx];
+			int line_diff = a_node.jumpPos2D.x < pre_node.jumpPos2D.x ? (pre_node.jumpPos2D.x - a_node.jumpPos2D.x) : (a_node.jumpPos2D.x - pre_node.jumpPos2D.x);
+			if (line_diff > 2)
+			{
+				double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
+				if (width > scanInfo.stitchWidth)
+				{
+					SVzNLRect a_stitch;
+					memset(&a_stitch, 0, sizeof(SVzNLRect));
+					if (a_node.jumpPos2D.x < pre_node.jumpPos2D.x)
+					{
+						a_stitch.left = a_node.jumpPos2D.x;
+						a_stitch.right = pre_node.jumpPos2D.x;
+					}
+					else
+					{
+						a_stitch.left = pre_node.jumpPos2D.x;
+						a_stitch.right = a_node.jumpPos2D.x;
+					}
+					if (a_node.jumpPos2D.y < pre_node.jumpPos2D.y)
+					{
+						a_stitch.top = a_node.jumpPos2D.y;
+						a_stitch.bottom = pre_node.jumpPos2D.y;
+					}
+					else
+					{
+						a_stitch.top = pre_node.jumpPos2D.y;
+						a_stitch.bottom = a_node.jumpPos2D.y;
+					}
+					stitchROIs.push_back(a_stitch);
+					if (stitchROIs.size() >= 4)
+						break;
+				}
+			}
+		}
+		pre_idx = nodeIdx;
+	}
+	//提取针脚位置
+	if (stitchROIs.size() == 0)
+	{
+		*errCode = SG_ERR_ZERO_OBJECTS;
+		return;
+	}
+	//建立node下标与扫描线序号的索引
+	std::vector<int> backIndexing;
+	int indexingSize = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
+	backIndexing.resize(indexingSize);
+	for (int i = 0; i < indexingSize; i++)
+		backIndexing[i] = -1;
+	for (int i = 0; i < nodeSize; i++)
+	{
+		int indexingIdx = cornerGrowTrees[objTreeIdx].treeNodes[i].jumpPos2D.x - cornerGrowTrees[objTreeIdx].sLineIdx;
+		backIndexing[indexingIdx] = i;
+	}
+
+	int opDist_lines;
+	if (false == scanInfo.isHorizonScan)  //垂直于线缝扫描
+		opDist_lines = (int)(scanInfo.operateDist / lineInterval);
+	else
+		opDist_lines = (int)(scanInfo.operateDist / ptInterval);
+
+	for (int i = 0, i_max = (int)stitchROIs.size(); i < i_max; i++)
+	{
+		//搜索Z值最高点
+		SVzNLRect& a_stitch = stitchROIs[i];
+		SVzNL3DPoint stitchPos = { 0, 0, -1 };
+		for (int j = a_stitch.left + 1; j < a_stitch.right; j++)
+		{
+			SVzNL3DPoint linePeak = { 0, 0, -1 };
+			for (int m = a_stitch.top; m <= a_stitch.bottom; m++)
+			{
+				SVzNL3DPoint a_pt;
+				if (false == scanInfo.isHorizonScan)  //垂直于线缝扫描
+					a_pt = scanLines[j][m].pt3D;
+				else
+					a_pt = scanLines[m][j].pt3D;
+				if (a_pt.z > 1e-4)
+				{
+					if (linePeak.z < 0)
+						linePeak = a_pt;
+					else
+					{
+						if (linePeak.z > a_pt.z)
+							linePeak = a_pt;
+					}
+				}
+			}
+			if (linePeak.z > 1e-4)
+			{
+				if (stitchPos.z < 0)
+					stitchPos = linePeak;
+				else
+				{
+					if (stitchPos.z > linePeak.z)
+						stitchPos = linePeak;
+				}
+			}
+		}
+		//搜索下刀位置
+		if (stitchPos.z > 1e-4)
+		{
+			int op_centerLine;
+			int searchStart, searchEnd;
+			if (true == scanInfo.scanFromThreadHead)
+			{
+				op_centerLine = a_stitch.right + opDist_lines;
+				searchStart = a_stitch.right; 
+				searchEnd = a_stitch.right + opDist_lines * 2;
+				if (searchEnd > cornerGrowTrees[objTreeIdx].eLineIdx)
+					searchEnd = cornerGrowTrees[objTreeIdx].eLineIdx;
+			}
+			else
+			{
+				op_centerLine = a_stitch.left - opDist_lines;
+				searchEnd = a_stitch.left;
+				searchStart = a_stitch.left - opDist_lines * 2;
+				if (searchStart < cornerGrowTrees[objTreeIdx].sLineIdx)
+					searchStart = cornerGrowTrees[objTreeIdx].sLineIdx;
+			}
+			//寻找最合适点
+			int best_idx = -1;
+			int minLineDist = INT_MAX;
+			for (int j = searchStart; j <= searchEnd; j++)
+			{
+				int indexingIdx = j - cornerGrowTrees[objTreeIdx].sLineIdx;
+				if (backIndexing[indexingIdx] >= 0)
+				{
+					int lineDist = j - op_centerLine;
+					if (lineDist < 0)
+						lineDist = -lineDist;
+					if (minLineDist > lineDist)
+					{
+						minLineDist = lineDist;
+						best_idx = backIndexing[indexingIdx];
+					}
+				}
+			}
+			if (best_idx >= 0)
+			{
+				int op_lineIdx, op_ptIdx;
+				if (false == scanInfo.isHorizonScan)  //垂直于线缝扫描
+				{
+					op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
+					op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
+				}
+				else
+				{
+					op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
+					op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
+				}
+				SSX_bagThreadInfo a_stitchInfo;
+				memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
+				a_stitchInfo.threadPos = stitchPos;
+				a_stitchInfo.operatePos = scanLines[op_lineIdx][op_ptIdx].pt3D;
+				a_stitchInfo.rotateAngle = 0;
+				bagThreadInfo.push_back(a_stitchInfo);
+			}
+		}
+	}
+
+
 	return;
 }
 #endif

sourceCode/bagThreadPositioning_Export.h

@@ -12,6 +12,13 @@ typedef struct
 	double rotateAngle; //水平旋转角
 }SSX_bagThreadInfo;  //线头信息
 
+typedef struct
+{
+	bool isHorizonScan;
+	bool scanFromThreadHead;
+	double stitchWidth; //针脚最小宽度，用于过滤虚假的针脚
+	double operateDist; //下刀位置距针脚距离
+}SSX_ScanInfo;
 
 //读版本号
 SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void);
@@ -19,7 +26,7 @@ SG_APISHARED_EXPORT const char* wd_bagThreadPositioningVersion(void);
 //线头位置检测定位
 SG_APISHARED_EXPORT void wd_bagThreadPositioning(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
-	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
+	const SSX_ScanInfo scanInfo,  //true:激光线平行槽道；false:激光线垂直槽道
 	const SSG_outlierFilterParam filterParam, //噪点过滤参数
 	const SSG_cornerParam cornerPara, //V型特征参数
 	const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数