workpieceHolePositioning version 1.4.9 :

修正工件法向计算的一个Bug

sourceCode/SG_baseAlgo_Export.h

@@ -6,6 +6,20 @@
 
 //向量叉乘
 SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b);
+// 向量数乘
+SG_APISHARED_EXPORT SVzNL3DPoint vec3_multiply(const SVzNL3DPoint& a, const double s);
+// 点乘 dot
+SG_APISHARED_EXPORT double vec3_dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b);
+// 模长
+SG_APISHARED_EXPORT double vec3_length(const SVzNL3DPoint& a);
+// 归一化（单位向量）
+SG_APISHARED_EXPORT SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a);
+
+/**
+ * @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度
+ * （v 必须在平面内，自动使用简化版罗德里格斯）
+ */
+SG_APISHARED_EXPORT SVzNL3DPoint wd_rotateVectorInPlane(const SVzNL3DPoint& v, const SVzNL3DPoint& n, double theta);
 
 //逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
 SG_APISHARED_EXPORT SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle);
@@ -144,6 +158,14 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM(
     const SSG_cornerParam cornerPara,
     SSG_lineFeature* line_features);
 
+//根据距离连续性分段
+SG_APISHARED_EXPORT void wd_lineDataSegment_dist(
+    std::vector< SWD3DPointPostion>& lineData,
+    std::vector<SSG_RUN>& segs,
+    const double maxDistTh,
+    const int minSegSize
+);
+
 SG_APISHARED_EXPORT void wd_lineDataSegment_zDist(
     std::vector< SVzNL3DPosition>& lineData,
     std::vector< SVzNL3DPosition>& vldPts,
@@ -183,6 +205,16 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod(
     std::vector<SWD_rodArcFeature>& line_rodArcs  //环
 );
 
+//提取corner极值（较早实现函数可以使用此函数进行代码优化）
+SG_APISHARED_EXPORT void wd_searchCornerPeaks(
+    std::vector< SSG_pntDirAngle>& corners,
+    std::vector< SVzNL3DPosition>& vldPts,
+    const double minCornerTh,
+    double cornerMergeScale,
+    std::vector< SSG_pntDirAngle>& cornerPeakP,
+    std::vector< SSG_pntDirAngle>& cornerPeakM
+);
+
 /// <summary>
 /// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
 /// nPointIdx被重新定义成Feature类型
@@ -538,6 +570,13 @@ SG_APISHARED_EXPORT void lineFitting_abc(
     double* _b,
     double* _c);
 
+//拟合成通用直线方程ax+by+c=0，包括垂直
+SG_APISHARED_EXPORT void indexingPtLineFitting_abc(
+    std::vector< SWD3DPointPostion>& inliers,
+    double* _a,
+    double* _b,
+    double* _c);
+
 /**
  * @brief 空间直线最小二乘拟合
  * @param points 输入的三维点集（至少2个点，否则无意义）

sourceCode/SG_baseDataType.h

@@ -39,6 +39,13 @@ typedef struct
 	SWD3DPoint point;
 }SWDIndexing3DPoint;
 
+typedef struct
+{
+	int lineIdx;
+	int ptIdx;
+	SVzNL3DPoint point;
+}SWD3DPointPostion;
+
 typedef struct
 {
 	int lineIdx;

sourceCode/SG_baseFunc.cpp

@@ -19,6 +19,57 @@ SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
 	c.z = a.x * b.y - a.y * b.x;
 	return c;
 }
+
+
+// 向量数乘
+SVzNL3DPoint vec3_multiply(const SVzNL3DPoint& a, const double s) 
+{
+	SVzNL3DPoint result = { a.x * s, a.y * s, a.z * s };
+	return result;
+}
+
+// 点乘 dot
+double vec3_dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b) 
+{
+	return (a.x * b.x + a.y * b.y + a.z * b.z);
+}
+
+// 模长
+double vec3_length(const SVzNL3DPoint& a)
+{
+	return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
+}
+
+// 归一化（单位向量）
+SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a)
+{
+	SVzNL3DPoint result;
+	double len = vec3_length(a);
+	if (len < 1e-6)
+		result = { 0,0,0 };
+	else
+		result = { a.x / len, a.y / len, a.z / len };
+	return result;
+}
+
+/**
+ * @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度
+ * （v 必须在平面内，自动使用简化版罗德里格斯）
+ */
+SVzNL3DPoint wd_rotateVectorInPlane(const SVzNL3DPoint& v, const SVzNL3DPoint& n, double theta) 
+{
+	SVzNL3DPoint k = vec3_normalize(n);    // 旋转轴（单位法向量）
+	SVzNL3DPoint cross = vec3_cross(k, v);   // k × v
+	double angle = theta * PI / 180.0;
+	double c = cos(angle);
+	double s = sin(angle);
+	// 简化公式：v' = v cosθ + (k×v) sinθ
+	SVzNL3DPoint t1 = vec3_multiply(v, c);
+	SVzNL3DPoint t2 =  vec3_multiply(cross, s);
+	SVzNL3DPoint result = { t1.x + t2.x, t1.y + t2.y, t1.z + t2.z };
+	return result;;
+}
+
 //逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
 SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle)
 {

sourceCode/workpieceHolePositioning.cpp

@@ -19,7 +19,8 @@
 //version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。
 //version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入
 //version 1.4.8 : 将工件法向调整为垂直于工件表面
-std::string	m_strVersion = "1.4.8";
+//version 1.4.9 : 修正工件法向计算的一个Bug
+std::string	m_strVersion = "1.4.9";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -214,7 +215,8 @@ void _getLinePoints(
 		a_pt3d.x = (double)(pts_2d[i].x + roi2D.left) + 0.5;
 		a_pt3d.y = (double)(pts_2d[i].y + roi2D.top) + 0.5;
 		a_pt3d.z = quantiValue[pts_2d[i].x][pts_2d[i].y];
-		Points3ds.push_back(a_pt3d);
+		if(a_pt3d.z > 1e-4)
+			Points3ds.push_back(a_pt3d);
 	}
 }
 

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void)
 	};
 
 	SVzNLRange fileIdx[TPF_TEST_GROUP] = {
-		{6,6}, {1, 16}, {1,17}
+		{6,6}, {7, 7}, {1,17}
 	};
 
 	const char* ver = wd_workpieceHolePositioningVersion();
@@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void)
 #endif
 
 #if TEST_COMPUTE_HOLE
-	for (int grp = 1; grp <= 2; grp++)
+	for (int grp = 1; grp <= 1; grp++)
 	{
 		SSG_planeCalibPara groundCalibPara;
 		//初始化成单位阵