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

sourceCode/SG_baseAlgo_Export.h

@@ -641,6 +641,24 @@ SG_APISHARED_EXPORT void vzCaculateLaserPlane(
     std::vector<cv::Point3f> Points3ds,
     std::vector<double>& res);
 
+SG_APISHARED_EXPORT Plane robustFitPlane(
+    const std::vector<cv::Point3f>& points,
+    RobustType type = TUKEY,
+    double delta = 0.5,   // 阈值：>此值视为离群点（mm）
+    int maxIter = 20,      // 迭代次数
+    double convergeThresh = 1e-5  // 收敛阈值（平面变化足够小就停）
+);
+// ==============================================
+// 带 提前终止 的 RANSAC 平面拟合（工业正式版）
+// ==============================================
+SG_APISHARED_EXPORT Plane ransacFitPlane(
+    const std::vector<cv::Point3f>& points,
+    std::vector<cv::Point3f>& out_inliers,
+    float dist_thresh = 0.5f,          // 内点距离阈值
+    int max_iter = 1000,                // 最大迭代
+    int stop_no_improve = 150           // 连续多少次无提升就提前退出
+);
+
 //计算一个平面调平参数。
 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
@@ -839,3 +857,9 @@ SG_APISHARED_EXPORT void WD_EulerRpyToRotation(double rpy[3], double matrix3d[9]
 
 //从欧拉角计算方向矢量
 SG_APISHARED_EXPORT void WD_EulerRpyToDirVectors(double rpy[3], std::vector<cv::Point3d>& dirVectors);
+
+//3x3平滑
+SG_APISHARED_EXPORT void scanLinesSmooting3x3(
+    std::vector< std::vector<SVzNL3DPosition>>& gridDataInput,
+    std::vector< std::vector<SVzNL3DPosition>>& smoothingData
+);

sourceCode/SG_baseDataType.h

@@ -124,6 +124,16 @@ struct RGB {
 	int b; 
 };
 
+struct Plane {
+	double A, B, C, D; // Ć˝Ăć Ax+By+Cz+D=0
+	Plane() : A(0), B(0), C(0), D(0) {}
+	Plane(double a, double b, double c, double d) : A(a), B(b), C(c), D(d) {}
+};
+enum RobustType {
+	HUBER,
+	TUKEY
+};
+
 #define LINE_FEATURE_NUM			16
 #define LINE_FEATURE_UNDEF			0
 #define LINE_FEATURE_L_JUMP_H2L		1

sourceCode/SG_baseFunc.cpp

@@ -3148,7 +3148,43 @@ void WD_EulerRpyToDirVectors(double rpy[3],std::vector<cv::Point3d>& dirVectors)
 	return;
 }
 
+void scanLinesSmooting3x3(
+	std::vector< std::vector<SVzNL3DPosition>>& gridDataInput, 
+	std::vector< std::vector<SVzNL3DPosition>>& smoothingData
+)
+{
+	int lineNum = (int)gridDataInput.size();
+	int linePtNum = (int)gridDataInput[0].size();
+	for (int line = 0; line < lineNum; line++)
+	{
+		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
+		{
 
+			smoothingData[line][ptIdx] = gridDataInput[line][ptIdx];
+			if (gridDataInput[line][ptIdx].pt3D.z > 1e-4)
+			{
+				double sumZ = 0;
+				int num = 0;
+				for (int i = line - 1; i <= line + 1; i++)
+				{
+					for (int j = ptIdx - 1; j <= ptIdx + 1; j++)
+					{
+						if ((i >= 0) && (i < lineNum) && (j >= 0) && (j < linePtNum))
+						{
+							if (gridDataInput[i][j].pt3D.z > 1e-4)
+							{
+								sumZ += gridDataInput[i][j].pt3D.z;
+								num++;
+							}
+						}
+					}
+				}
+				smoothingData[line][ptIdx].pt3D.z = sumZ / num;
+			}
+		}
+	}
+	return;
+}
 
 #if 0
 #include <iostream>

sourceCode/dataFitting.cpp

@@ -383,3 +383,255 @@ bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint
 	direction = { dir(0), dir(1), dir(2) };
 	return true;
 }
+
+// ============================== 工具函数 ==============================
+// 点到平面距离（带符号）
+double pointToPlaneSignedDist(const cv::Point3f& p, const Plane& plane) {
+	return (plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D);
+}
+
+// 点到平面的距离（绝对值）
+float pointToPlaneDistance(const cv::Point3f& p, const Plane& plane) {
+	return fabsf(plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D)
+		/ sqrtf(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
+}
+
+// 归一化平面（法向量模长=1）
+void normalizePlane(Plane& plane) {
+	double norm = sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
+	if (norm < 1e-6) return;
+	plane.A /= norm;
+	plane.B /= norm;
+	plane.C /= norm;
+	plane.D /= norm;
+}
+
+// ============================== 鲁棒损失函数 ==============================
+// Huber 权重
+double huberWeight(double r, double delta) {
+	r = fabs(r);
+	if (r <= delta) return 1.0;
+	else return delta / r;
+}
+
+// Tukey 权重（离群点=0，最适合凹坑/强噪声）
+double tukeyWeight(double r, double c) {
+	r = fabs(r);
+	if (r > c) return 0.0;
+	double t = 1.0 - (r * r) / (c * c);
+	return t * t;
+}
+
+// ============================== 鲁棒加权最小二乘平面拟合 ==============================
+Plane robustFitPlane(
+	const std::vector< cv::Point3f>& points,
+	RobustType type,
+	double delta,   // 阈值：>此值视为离群点（mm）
+	int maxIter,      // 迭代次数
+	double convergeThresh  // 收敛阈值（平面变化足够小就停）
+) 
+{
+	int n = points.size();
+	if (n < 3) return Plane();
+
+	// 1. 先用普通最小二乘初始化平面
+	double cx = 0, cy = 0, cz = 0;
+	for (auto& p : points) { cx += p.x; cy += p.y; cz += p.z; }
+	cx /= n; cy /= n; cz /= n;
+
+	double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
+	for (auto& p : points) {
+		double dx = p.x - cx;
+		double dy = p.y - cy;
+		double dz = p.z - cz;
+		xx += dx * dx; xy += dx * dy; xz += dx * dz;
+		yy += dy * dy; yz += dy * dz; zz += dz * dz;
+	}
+
+	double detX = yy * zz - yz * yz;
+	double detY = xx * zz - xz * xz;
+	double detZ = xx * yy - xy * xy;
+	double maxDet = std::max({ detX, detY, detZ });
+
+	Plane plane;
+	if (maxDet == detX) {
+		plane.A = 1;
+		plane.B = (xy * yz - xz * yy) / detX;
+		plane.C = (xz * yz - xy * zz) / detX;
+	}
+	else if (maxDet == detY) {
+		plane.A = (xy * yz - xz * yy) / detY;
+		plane.B = 1;
+		plane.C = (xz * xy - xx * yz) / detY;
+	}
+	else {
+		plane.A = (xz * yz - xy * zz) / detZ;
+		plane.B = (yz * xy - xz * yy) / detZ;
+		plane.C = 1;
+	}
+	plane.D = -(plane.A * cx + plane.B * cy + plane.C * cz);
+	normalizePlane(plane);
+
+	// 2. 迭代加权最小二乘 (IRLS)
+	std::vector<double> weights(n, 1.0);
+
+	for (int iter = 0; iter < maxIter; iter++) {
+		Plane prevPlane = plane;
+
+		double sum_w = 0;
+		double swx = 0, swy = 0, swz = 0;
+		double swxx = 0, swxy = 0, swxz = 0, swyy = 0, swyz = 0;
+
+		for (int i = 0; i < n; i++) {
+			double r = pointToPlaneSignedDist(points[i], plane);
+			if (type == HUBER) weights[i] = huberWeight(r, delta);
+			else weights[i] = tukeyWeight(r, delta);
+
+			double w = weights[i];
+			sum_w += w;
+			swx += w * points[i].x;
+			swy += w * points[i].y;
+			swz += w * points[i].z;
+		}
+
+		double mx = swx / sum_w;
+		double my = swy / sum_w;
+		double mz = swz / sum_w;
+
+		for (int i = 0; i < n; i++) {
+			double w = weights[i];
+			double dx = points[i].x - mx;
+			double dy = points[i].y - my;
+			double dz = points[i].z - mz;
+			swxx += w * dx * dx;
+			swxy += w * dx * dy;
+			swxz += w * dx * dz;
+			swyy += w * dy * dy;
+			swyz += w * dy * dz;
+		}
+
+		// 解特征向量
+		double detXw = swyy * swyz - swyz * swyz;
+		double detYw = swxx * swyz - swxz * swxz;
+		double detZw = swxx * swyy - swxy * swxy;
+		double maxDw = std::max({ detXw, detYw, detZw });
+
+		if (maxDw == detXw) {
+			plane.A = 1;
+			plane.B = (swxy * swyz - swxz * swyy) / detXw;
+			plane.C = (swxz * swyz - swxy * swyz) / detXw;
+		}
+		else if (maxDw == detYw) {
+			plane.A = (swxy * swyz - swxz * swyy) / detYw;
+			plane.B = 1;
+			plane.C = (swxz * swxy - swxx * swyz) / detYw;
+		}
+		else {
+			plane.A = (swxz * swyz - swxy * swyz) / detZw;
+			plane.B = (swyz * swxy - swxz * swyy) / detZw;
+			plane.C = 1;
+		}
+		plane.D = -(plane.A * mx + plane.B * my + plane.C * mz);
+		normalizePlane(plane);
+
+		// ========== 关键：判断是否收敛 ==========
+		double da = fabs(plane.A - prevPlane.A);
+		double db = fabs(plane.B - prevPlane.B);
+		double dc = fabs(plane.C - prevPlane.C);
+		double dd = fabs(plane.D - prevPlane.D);
+
+		double maxDiff = std::max({ da, db, dc, dd });
+
+		// 平面几乎不再变化 → 提前终止
+		if (maxDiff < convergeThresh) {
+			break;
+		}
+	}
+	return plane;
+}
+
+// 三点拟合平面
+Plane planeFrom3Points(const cv::Point3f& p1, const cv::Point3f& p2, const cv::Point3f& p3) {
+	float v1x = p2.x - p1.x;
+	float v1y = p2.y - p1.y;
+	float v1z = p2.z - p1.z;
+
+	float v2x = p3.x - p1.x;
+	float v2y = p3.y - p1.y;
+	float v2z = p3.z - p1.z;
+
+	float A = v1y * v2z - v1z * v2y;
+	float B = v1z * v2x - v1x * v2z;
+	float C = v1x * v2y - v1y * v2x;
+	float D = -(A * p1.x + B * p1.y + C * p1.z);
+
+	float norm = sqrtf(A * A + B * B + C * C);
+	if (norm > 1e-6) { A /= norm; B /= norm; C /= norm; D /= norm; }
+	return Plane(A, B, C, D);
+}
+
+// ==============================================
+// 带 提前终止 的 RANSAC 平面拟合（工业正式版）
+// ==============================================
+Plane ransacFitPlane(
+	const std::vector<cv::Point3f>& points,
+	std::vector<cv::Point3f>& out_inliers,
+	float dist_thresh,          // 内点距离阈值
+	int max_iter,                // 最大迭代
+	int stop_no_improve           // 连续多少次无提升就提前退出
+) 
+{
+	out_inliers.clear();
+	int n = points.size();
+	if (n < 3) return Plane();
+
+	int best_inlier = 0;
+	Plane best_plane;
+
+	srand((unsigned)time(nullptr));
+
+	int no_improve_count = 0;  // 无提升计数
+
+	for (int i = 0; i < max_iter; ++i) {
+		// 随机3个不重复点
+		int idx[3];
+		idx[0] = rand() % n;
+		do { idx[1] = rand() % n; } while (idx[1] == idx[0]);
+		do { idx[2] = rand() % n; } while (idx[2] == idx[0] || idx[2] == idx[1]);
+
+		Plane plane = planeFrom3Points(
+			points[idx[0]],
+			points[idx[1]],
+			points[idx[2]]
+		);
+
+		// 统计内点
+		int cnt = 0;
+		for (auto& p : points) {
+			if (pointToPlaneDistance(p, plane) < dist_thresh) cnt++;
+		}
+
+		// 更新最优模型
+		if (cnt > best_inlier) {
+			best_inlier = cnt;
+			best_plane = plane;
+			no_improve_count = 0;  // 有提升 → 清零
+		}
+		else {
+			no_improve_count++;
+		}
+
+		// ====================== 提前终止条件 ======================
+		if (no_improve_count >= stop_no_improve) {
+			break;
+		}
+	}
+
+	// 收集内点
+	for (auto& p : points) {
+		if (pointToPlaneDistance(p, best_plane) < dist_thresh)
+			out_inliers.push_back(p);
+	}
+
+	return best_plane;
+}

sourceCode/workpieceHolePositioning.cpp

@@ -12,7 +12,8 @@
 //version 1.3.0 : (1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
 //version 1.4.0 : 添加了华航孔定位功能
 //version 1.4.1 : 华航孔定位客户发布初始版本，修正了1.4.0版本的一些问题
-std::string	m_strVersion = "1.4.1";
+//version 1.4.2 : 华航孔定位改进：（1）法向量计算改进（2）添加3x3平滑（3）修正了bug
+std::string	m_strVersion = "1.4.2";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -307,6 +308,7 @@ void wd_getHoleInfo(
 	const SSG_lineSegParam lineSegPara,
 	const SSG_outlierFilterParam filterParam,
 	const SSG_treeGrowParam growParam,
+	const double valieCommonNumRatio,
 	std::vector<SWD_segFeatureTree>& segTrees_v,
 	std::vector<SWD_segFeatureTree>& segTrees_h,
 	std::vector<SSG_intPair>& validObjects
@@ -530,7 +532,9 @@ void wd_getHoleInfo(
 		if (hTreeIdx >= 0)
 		{
 			int totalSize_h = treeInfo_h[hTreeIdx].treeType;
-			if ((commonSize > totalSize_h / 4) && (commonSize > totalSize_v / 4))
+			int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
+			int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);
+			if ((commonSize > sizeH_th) && (commonSize > sizeV_th))
 			{
 				SSG_intPair a_obj;
 				a_obj.data_0 = i;
@@ -676,7 +680,8 @@ void wd_workpieceHolePositioning(
 	std::vector<SWD_segFeatureTree> segTrees_v;
 	std::vector<SWD_segFeatureTree> segTrees_h;
 	std::vector<SSG_intPair> validObjects;
-	wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects);
+	double valieCommonNumRatio = 0.25;
+	wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
 
 	//生成聚类信息， 
 	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
@@ -954,6 +959,7 @@ typedef struct
 bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
 	std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_v,
 	std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_h,
+	double pntCornenrMax,
 	SVzNLRect& roi
 )
 {
@@ -964,7 +970,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
 	int left = -1;
 	for (int line = seedPos.x; line >= 0; line--)
 	{
-		if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0))
+		if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax))
 		{
 			left = line;
 			break;
@@ -974,7 +980,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
 	int right = -1;
 	for (int line = seedPos.x; line < lineNum; line++)
 	{
-		if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0))
+		if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax))
 		{
 			right = line;
 			break;
@@ -984,7 +990,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
 	int top = -1;
 	for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--)
 	{
-		if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0))
+		if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax))
 		{
 			top = ptIdx;
 			break;
@@ -994,7 +1000,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
 	int bottom = -1;
 	for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++)
 	{
-		if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0))
+		if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax))
 		{
 			bottom = ptIdx;
 			break;
@@ -1053,10 +1059,16 @@ void wd_HolePositioning(
 		return;
 	}
 
+	//添加3x3平滑
+	scanLinesSmooting3x3(scanLinesInput, scanLines);
+
 	//内部参数
-	double zPeakScale = 25.0;  //计算ZPeak时的尺度
+	double zPeakScale = 10.0;  //计算ZPeak时的尺度
-	double minZPeakHeight = 2.5; //最小的凹坑深度
+	double minZPeakHeight = 2.0; //最小的凹坑深度
-	double planeInlierDistTh = 1.0; //平面点距离平面的距离。超出此距离被判别为离群点
+	double planeInlierDistTh = 1.5; //平面点距离平面的距离。超出此距离被判别为离群点
+	double flagCornerMax = 3.0; //直线上点的拐角最大值
+	double maxHoleDiameter = 30.0;//最大的孔直径
+	double valieCommonNumRatio = 0.125; //1/8
 
 	//计算dirAngle
 	std::vector<std::vector<SSG_pntDirAngle>> pntDirAngles_v;
@@ -1147,7 +1159,7 @@ void wd_HolePositioning(
 			{
 				//判断ROI
 				SVzNLRect a_roi;
-				bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, a_roi);
+				bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, flagCornerMax, a_roi);
 				if (true == validPk)
 				{
 					double meanZ = scanLines[a_roi.left][idx_pt].pt3D.z;
@@ -1156,7 +1168,10 @@ void wd_HolePositioning(
 					meanZ += scanLines[idx_line][a_roi.bottom].pt3D.z;
 					meanZ = meanZ / 4.0;
 					double z_diff = a_lineZMax[j].jumpPos.z - meanZ;
-					if (z_diff > minZPeakHeight)
+
+					double xLen = abs(scanLines[a_roi.right][idx_pt].pt3D.x - scanLines[a_roi.left][idx_pt].pt3D.x);
+					double yLen = abs(scanLines[idx_line][a_roi.bottom].pt3D.y - scanLines[idx_line][a_roi.top].pt3D.y);
+					if ( (z_diff > minZPeakHeight) && (xLen < maxHoleDiameter) && (yLen < maxHoleDiameter))
 					{
 						_zMaxInfo a_info;
 						a_info.flag = 0;
@@ -1263,7 +1278,46 @@ void wd_HolePositioning(
 		}
 		//计算面参数: z = Ax + By + C , res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
 		std::vector<double> res;
+#if 0
 		vzCaculateLaserPlane(Points3ds, res);
+#else
+#if 0
+		double delta = 1.0;
+		int maxIter = 20;
+		Plane robustPlane = robustFitPlane(
+			Points3ds, TUKEY, 
+			delta ,   // 阈值：>此值视为离群点（mm）
+			maxIter      // 迭代次数
+		);
+#else
+		float dist_thresh = 0.5f;
+		int max_iter = 1000;
+		int stop_no_improve = 150;
+		std::vector<cv::Point3f> out_inliers;
+		Plane robustPlane = ransacFitPlane(
+			Points3ds,
+			out_inliers,
+			dist_thresh,          // 内点距离阈值
+			max_iter,                // 最大迭代
+			stop_no_improve           // 连续多少次无提升就提前退出
+		);
+#endif
+		res.resize(4);
+		if (robustPlane.C > 0)
+		{
+			res[0] = -robustPlane.A;
+			res[1] = -robustPlane.B;
+			res[2] = -robustPlane.C;
+			res[3] = -robustPlane.D;
+		}
+		else
+		{
+			res[0] = robustPlane.A;
+			res[1] = robustPlane.B;
+			res[2] = robustPlane.C;
+			res[3] = robustPlane.D;
+		}
+#endif
 		double normValue = sqrt(pow(res[0], 2) + pow(res[1], 2) + pow(res[2],2));
 		double norm_A = res[0] / normValue;
 		double norm_B = res[1] / normValue;
@@ -1302,7 +1356,7 @@ void wd_HolePositioning(
 		std::vector<SWD_segFeatureTree> segTrees_v;
 		std::vector<SWD_segFeatureTree> segTrees_h;
 		std::vector<SSG_intPair> validObjects;
-		wd_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects);
+		wd_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
 		if (validObjects.size() > 0)
 		{
 			SSG_intPair a_hvPair = validObjects[0];
@@ -1310,7 +1364,7 @@ void wd_HolePositioning(
 			{
 				for (int j = 1; j < (int)validObjects.size(); j++)
 				{
-					if (validObjects[j].idx < validObjects[j].idx)
+					if (a_hvPair.idx < validObjects[j].idx)
 						a_hvPair = validObjects[j];
 				}
 			}

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -174,9 +174,9 @@ void _outputHoleInfo(char* fileName, std::vector< WD_HolePositionInfo>& holePosi
 	{
 		sprintf_s(dataStr, 250, "孔_%d", i + 1);
 		sw << dataStr << std::endl;
-		sprintf_s(dataStr, 50, "  center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z);
+		sprintf_s(dataStr, 250, "  center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z);
 		sw << dataStr << std::endl;
-		sprintf_s(dataStr, 50, "  norm_dir: (%g, %g, %g)", holePositioning[i].normDir.x, holePositioning[i].normDir.y, holePositioning[i].normDir.z);
+		sprintf_s(dataStr, 250, "  norm_dir: (%g, %g, %g)", holePositioning[i].normDir.x, holePositioning[i].normDir.y, holePositioning[i].normDir.z);
 		sw << dataStr << std::endl;
 	}
 	sw.close();
@@ -823,7 +823,7 @@ void HuaHang_holePosition_test(void)
 	};
 
 	SVzNLRange fileIdx[HH_TEST_GROUP] = {
-		{1,15}, {1, 21},{1,19}, {1,9}
+		{1,15}, {1, 21},{1,19}, {1,13}
 	};
 
 	const char* ver = wd_workpieceHolePositioningVersion();
@@ -834,7 +834,7 @@ void HuaHang_holePosition_test(void)
 	{
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx =2;
+			//fidx =17;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
@@ -852,7 +852,7 @@ void HuaHang_holePosition_test(void)
 			SSG_cornerParam cornerParam;
 			cornerParam.cornerTh = 60;  //45度角
 			cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
-			cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
+			cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4;
 			cornerParam.minEndingGap_z = 5.0;
 			cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
 			cornerParam.jumpCornerTh_2 = 60;