BQ_workpieceCornerExtraction version 1.4.1 :

(1)使用聚类方法寻找准确轮廓点（2）使用拐角和R极值方法判断目标类型

BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp

@@ -607,13 +607,19 @@ void _outputRGBDScanLapWeld_RGBD(
 					sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 				}
 			}
-			//加一个点，用于跳过显示工具bug
+			if (debugData.size() > 0)
-			float x = (float)debugData[0].edge[0].x;
+			{
-			float y = (float)debugData[0].edge[0].y;
+				if (debugData[0].edge != NULL)
-			float z = (float)debugData[0].edge[0].z;
+				{
-			sw << "{" << x << "," << y << "," << z << "}-";
+					//加一个点，用于跳过显示工具bug
-			sw << "{0,0}-{0,0}-";
+					float x = (float)debugData[0].edge[0].x;
-			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+					float y = (float)debugData[0].edge[0].y;
+					float z = (float)debugData[0].edge[0].z;
+					sw << "{" << x << "," << y << "," << z << "}-";
+					sw << "{0,0}-{0,0}-";
+					sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+				}
+			}
 		}
 		//显示拟合直线
 		rgb = { 255, 0, 0 };
@@ -743,7 +749,7 @@ int main()
 	const char* ver = wd_BQWorkpieceCornerVersion();
 	printf("ver:%s\n", ver);
 
-	for (int grp = 7; grp <= 7; grp++)
+	for (int grp = 6; grp <= 8; grp++)
 	{
 		SSG_planeCalibPara poseCalibPara;
 		//初始化成单位阵
@@ -765,7 +771,7 @@ int main()
 
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx =2;
+			//fidx =8;
 			char _scan_file[256];
 			if(0 == grp)
 				sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
@@ -821,8 +827,8 @@ int main()
 			filterParam.outlierTh = 5;
 			SSG_treeGrowParam growParam;
 			growParam.maxLineSkipNum = 10;
-			growParam.yDeviation_max = 5.0;
+			growParam.yDeviation_max = 10.0;
-			growParam.maxSkipDistance = 5.0;
+			growParam.maxSkipDistance = 10.0;
 			growParam.zDeviation_max = 5.0;// 
 			growParam.minLTypeTreeLen = 100; //mm
 			growParam.minVTypeTreeLen = 100; //mm
@@ -831,8 +837,8 @@ int main()
 				workpieceParam.lineLen = 120; // 120.0; //直线段长度
 			else
 				workpieceParam.lineLen = 160.0; //直线段长度
-			workpieceParam.dirAngleScale = 20.0; //计算方向角的尺度
+			workpieceParam.dirAngleScale = 25.0; //计算方向角的尺度
-			workpieceParam.lineDeviation = 10.0;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。
+			workpieceParam.lineDeviation = 20.0;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。
 			workpieceParam.minCutAngleTh = 15; //最小截角。由于扫描不全导致的截角，当截角过小时无法区分真下的截角和由于点波动导致的角度波动
 
 			int errCode = 0;

BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj

@@ -24,6 +24,7 @@
     <ProjectGuid>{cf563709-0402-447e-bfcc-7701cc90d0af}</ProjectGuid>
     <RootNamespace>BQworkpieceCornerExtracttest</RootNamespace>
     <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
+    <ProjectName>BQ_workpieceCornerExtraction_test</ProjectName>
   </PropertyGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">

BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj

@@ -48,7 +48,7 @@
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
     <ConfigurationType>DynamicLibrary</ConfigurationType>
     <UseDebugLibraries>false</UseDebugLibraries>
-    <PlatformToolset>v142</PlatformToolset>
+    <PlatformToolset>v143</PlatformToolset>
     <WholeProgramOptimization>true</WholeProgramOptimization>
     <CharacterSet>Unicode</CharacterSet>
   </PropertyGroup>

beltTearingDetection_test/beltTearingDetection_test.cpp

@@ -942,7 +942,7 @@ void _outputObjInfo(char* fileName, std::vector<SSG_beltTearingInfo>& total_tear
 	sw.close();
 }
 
-#define TEST_GROUP 3
+#define TEST_GROUP 4
 int main()
 {
 #if 0
@@ -1043,13 +1043,14 @@ int main()
 #else
 
 	const char* dataPath[TEST_GROUP] = {
-	"F:\\ShangGu\\皮带撕裂点云\\1-500-2000\\",  //0
+	"F:/ShangGu/皮带撕裂点云/1-500-2000/",  //0
-	"F:\\ShangGu\\皮带撕裂点云\\2-1700-2000\\",  //1
+	"F:/ShangGu/皮带撕裂点云/2-1700-2000/",  //1
-	"F:\\ShangGu\\皮带撕裂点云\\SaveData\\"
+	"F:/ShangGu/皮带撕裂点云/SaveData/", //2
+	"F:/ShangGu/皮带撕裂点云/20260316_中信重工/", //3
 	};
 
 	SVzNLRange fileIdx[TEST_GROUP] = {
-		{10,24}, {2, 18}, {16,16} };
+		{10,24}, {2, 18}, {16,16}, {1,2} };
 
 	double camPoseR[9] = {
 	1.0, 0.0, 0.0,
@@ -1067,7 +1068,7 @@ int main()
 
 	char _scan_file[256];
 	int endGroup = TEST_GROUP - 1;
-	for (int grp = 2; grp <= endGroup; grp++)
+	for (int grp = 3; grp <= endGroup; grp++)
 	{
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{

sourceCode/BQ_workpieceCornerExtraction_Export.h

@@ -8,6 +8,9 @@
 typedef struct
 {
 	double lineLen; //直线段长度
+	double dirAngleScale; //计算方向角的尺度
+	double lineDeviation;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。
+	double minCutAngleTh; //最小截角。由于扫描不全导致的截角，当截角过小时无法区分真下的截角和由于点波动导致的角度波动
 }SSX_BQworkpiecePara;
 
 typedef struct
@@ -53,12 +56,12 @@ SG_APISHARED_EXPORT void sx_BQ_lineDataR(
 
 //提取工件角点及定位长度信息
 SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
-	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
 	const SSG_cornerParam cornerPara,
 	const SSG_outlierFilterParam filterParam,
-	SSG_treeGrowParam growParam,
+	const SSG_treeGrowParam growParam,
-	SSG_planeCalibPara groundCalibPara,
+	const SSG_planeCalibPara groundCalibPara,
-	SSX_BQworkpiecePara workpieceParam,
+	const SSX_BQworkpiecePara workpieceParam,
 #if _OUTPUT_DEBUG_DATA
 	std::vector<SSX_debugInfo>& debug_contours,
 #endif

sourceCode/SG_baseAlgo_Export.h

@@ -677,6 +677,14 @@ SG_APISHARED_EXPORT void sg_pointClustering(
     double clusterDist,
     std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
 );
+//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
+//搜索时搜索邻域以加速
+SG_APISHARED_EXPORT void wd_pointClustering_speedUp(
+    std::vector< SVzNL3DPosition>& pts,
+    int lineNum, int ptSize, int clusterCheckWin, //搜索窗口
+    double clusterDist,
+    std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
+);
 
 //基于栅格上点的窗口内的相邻点的聚类，聚类条件由3D点的邻域决定
 //使用vector构成2维结构体数组
@@ -760,6 +768,13 @@ SG_APISHARED_EXPORT void wd_noiseFilter(
     const SSG_outlierFilterParam filterParam,
     int* errCode);
 
+//点云填孔平滑处理
+SG_APISHARED_EXPORT void wd_pointCloudHoleFilling(
+    std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
+    std::vector< std::vector<SVzNL3DPosition>>& scanLines_dst,
+    const int fillingHoleSize //填孔大小
+);
+
 // Eigen库实现：计算从pts1向pts2的旋转平移矩阵
 //需要
 SG_APISHARED_EXPORT void caculateRT(

sourceCode/SG_clustering.cpp

@@ -9,6 +9,7 @@
 void _seedClustering(
 	std::vector< SVzNL3DPosition>& a_cluster,
 	std::vector< SVzNL3DPosition>& pts,
+	std::vector<int> flags,
 	double clusterDist)
 {
 	int i = 0;
@@ -18,15 +19,61 @@ void _seedClustering(
 			break;
 
 		SVzNL3DPosition a_seed = a_cluster[i];
-		for (int i = 0, i_max = (int)pts.size(); i < i_max; i++)
+		for (int j = 0, j_max = (int)pts.size(); j < j_max; j++)
 		{
-			if (pts[i].nPointIdx < 0)
+			if (flags[j] < 0)
 				continue;
-			double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2));
+			double dist_x = abs(a_seed.pt3D.x - pts[j].pt3D.x);
-			if (dist < clusterDist)
+			double dist_y = abs(a_seed.pt3D.y - pts[j].pt3D.y);
+			if ( (dist_x < clusterDist) && (dist_y < clusterDist))
 			{
-				a_cluster.push_back(pts[i]);
+				a_cluster.push_back(pts[j]);
-				pts[i].nPointIdx = -1;
+				flags[j] = -1;
+			}
+		}
+		i++;
+	}
+	return;
+}
+
+void _seedClustering_speedUp(
+	std::vector< SVzNL3DPosition>& a_cluster,
+	std::vector< SVzNL3DPosition>& pts,
+	std::vector<int>& flags,
+	double clusterDist,
+	std::vector<std::vector<int>>& indexing2D,
+	int lineNum, int ptSize, int clusterCheckWin) //搜索窗口
+{
+	int i = 0;
+	while (1)
+	{
+		if (i >= a_cluster.size())
+			break;
+
+		SVzNL3DPosition a_seed = a_cluster[i];
+		int seed_line = a_seed.nPointIdx >> 16;
+		int seed_ptIdx =a_seed.nPointIdx & 0x0000FFFF;
+		for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++)
+		{
+			if ((line >= 0) && (line < lineNum))
+			{
+				for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++)
+				{
+					if ((ptIdx >= 0) && (ptIdx < ptSize))
+					{
+						int backIdx = indexing2D[line][ptIdx];
+						if (backIdx < 0)
+							continue;
+						if (flags[backIdx] < 0)
+							continue;
+						double dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x,2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y,2));
+						if (dist < clusterDist) 
+						{
+							a_cluster.push_back(pts[backIdx]);
+							flags[backIdx] = -1;
+						}
+					}
+				}
 			}
 		}
 		i++;
@@ -43,30 +90,95 @@ void sg_pointClustering(
 	int ptSize = (int)pts.size();
 	if (ptSize == 0)
 		return;
+
+	std::vector<int> flags;
+	flags.resize(ptSize);
+	std::fill(flags.begin(), flags.end(), 0);
+
 	while(pts.size() > 0)
 	{
 		SVzNL3DPosition a_pt = pts[0];
-
+		flags[0] = -1;//防止重复被计算
 		//新建一个cluster
 		std::vector< SVzNL3DPosition> a_cluster;
 		a_cluster.push_back(a_pt);
-		pts[0].nPointIdx = -1; //防止重复被计算
+		//pts[0].nPointIdx = -1; 
 		_seedClustering(
 			a_cluster,
 			pts,
+			flags,
 			clusterDist);
 		objClusters.push_back(a_cluster); //保存一个聚类
 		//将pts中处理过的点去除
 		int m_max = (int)pts.size();
 		for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
 		{
-			if(pts[m].nPointIdx < 0)
+			if (flags[m] < 0)
+			{
 				pts.erase(pts.begin() + m);
+				flags.erase(flags.begin() + m);
+			}
 		}
 	}
 	return;
 }
 
+//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
+//搜索时搜索邻域以加速
+void wd_pointClustering_speedUp(
+	std::vector< SVzNL3DPosition>& pts,
+	int lineNum, int linePtSize, int clusterCheckWin, //搜索窗口
+	double clusterDist,
+	std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
+)
+{
+	std::vector<std::vector<int>> indexing2D;
+	indexing2D.resize(lineNum);
+	for (int i = 0; i < lineNum; i++)
+	{
+		indexing2D[i].resize(linePtSize);
+		std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1);
+	}
+	for (int i = 0; i < (int)pts.size(); i++)
+	{
+		int line = pts[i].nPointIdx >> 16;
+		int ptIdx = pts[i].nPointIdx & 0x0000FFFF;
+		indexing2D[line][ptIdx] = i;
+	}
+
+	int ptSize = (int)pts.size();
+	if (ptSize == 0)
+		return;
+
+	std::vector<int> flags;
+	flags.resize(ptSize);
+	std::fill(flags.begin(), flags.end(), 0);
+
+	int idx = 0;
+	while (idx < (int)pts.size())
+	{
+		SVzNL3DPosition a_pt = pts[idx];
+		if (flags[idx] >= 0)
+		{
+			flags[idx] = -1;//防止重复被计算
+			//新建一个cluster
+			std::vector< SVzNL3DPosition> a_cluster;
+			a_cluster.push_back(a_pt);
+			//pts[0].nPointIdx = -1; 
+			_seedClustering_speedUp(
+				a_cluster,
+				pts,
+				flags,
+				clusterDist,
+				indexing2D,
+				lineNum, linePtSize, clusterCheckWin);
+			objClusters.push_back(a_cluster); //保存一个聚类
+		}
+		idx++;
+	}
+	return;
+}
+
 //对特征点的聚类
 void wd_gridPointClustering(
 	std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,

sourceCode/WD_noiseFilter.cpp

@@ -60,3 +60,115 @@ void wd_noiseFilter(
 	}
 	return;
 }
+
+void wd_pointCloudHoleFilling(
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines_dst,
+	const int fillingHoleSize //填孔大小
+)
+{
+	int lineNum = (int)scanLines_src.size();
+	scanLines_dst.resize(lineNum);
+	for (int line = 0; line < lineNum; line++)
+	{
+		int ptNum = (int)scanLines_src[line].size();
+		scanLines_dst[line].resize(ptNum);
+		for (int i = 0; i < ptNum; i++)
+		{
+			scanLines_dst[line][i] = scanLines_src[line][i];
+			scanLines_src[line][i].nPointIdx = 0;  //未填充
+		}
+	}
+
+	for (int line = 0; line < lineNum; line++)
+	{
+		int ptNum = (int)scanLines_src[line].size();
+		for (int i = 0; i < ptNum; i++)
+		{
+			SVzNL3DPosition& a_pt = scanLines_src[line][i];
+			if ( (a_pt.pt3D.z < 1e-4) && (a_pt.nPointIdx == 0))  //未填充的空白点
+			{
+				//检查是否需要填充
+				//扫描线方向
+				int j_top = -1;
+				for (int j = i - 1; j >= i - fillingHoleSize; j--)
+				{
+					if (j < 0)
+						break;
+					else if (scanLines_src[line][j].pt3D.z > 1e-4)
+					{
+						j_top = j;
+						break;
+					}
+				}
+				int j_btm = -1;
+				for (int j = i + 1; j <= i + fillingHoleSize; j++)
+				{
+					if (j >= ptNum)
+						break;
+					else if (scanLines_src[line][j].pt3D.z > 1e-4)
+					{
+						j_btm = j;
+						break;
+					}
+				}
+				if ((j_top >= 0) && (j_btm >= 0))
+				{
+					SVzNL3DPoint& top_pt = scanLines_src[line][j_top].pt3D;
+					SVzNL3DPoint& btm_pt = scanLines_src[line][j_btm].pt3D;
+					//插值
+					double dist = (double)(j_btm - j_top);
+					for (int j = j_top + 1; j <= j_btm - 1; j++)
+					{
+						double d = (double)(j - j_top);
+						double k2 = d / dist;
+						double k1 = 1.0 - k2;
+						scanLines_dst[line][j].pt3D = { top_pt.x * k1 + btm_pt.x * k2, top_pt.y * k1 + btm_pt.y * k2 , top_pt.z * k1 + btm_pt.z * k2 };
+						scanLines_src[line][j].nPointIdx = 1;
+					}
+				}
+				else
+				{
+					//检查水平方向
+					int line_left = -1;
+					for (int j = line - 1; j >= line - fillingHoleSize; j--)
+					{
+						if (j < 0)
+							break;
+						else if (scanLines_src[j][i].pt3D.z > 1e-4)
+						{
+							line_left = j;
+							break;
+						}
+					}
+					int line_right = -1;
+					for (int j = line + 1; j <= line + fillingHoleSize; j++)
+					{
+						if (j >= lineNum)
+							break;
+						else if (scanLines_src[j][i].pt3D.z > 1e-4)
+						{
+							line_right = j;
+							break;
+						}
+					}
+					if ((line_left >= 0) && (line_right >= 0))
+					{
+						SVzNL3DPoint& left_pt = scanLines_src[line_left][i].pt3D;
+						SVzNL3DPoint& right_pt = scanLines_src[line_right][i].pt3D;
+						//插值
+						double dist = (double)(line_right - line_left);
+						for (int j = line_left + 1; j <= line_right - 1; j++)
+						{
+							double d = (double)(j - line_left);
+							double k2 = d / dist;
+							double k1 = 1.0 - k2;
+							scanLines_dst[j][i].pt3D = { left_pt.x * k1 + right_pt.x * k2, left_pt.y * k1 + right_pt.y * k2 , left_pt.z * k1 + right_pt.z * k2 };
+							scanLines_src[j][i].nPointIdx = 1;
+						}
+					}
+				}
+			}
+		}
+	}
+}