workpieceHolePositioning version 1.4.8 :

将工件法向调整为垂直于工件表面

sourceCode/workpieceHolePositioning.cpp

@@ -18,7 +18,8 @@
 //version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果
 //version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。
 //version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入
-std::string	m_strVersion = "1.4.7";
+//version 1.4.8 : 将工件法向调整为垂直于工件表面
+std::string	m_strVersion = "1.4.8";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -177,6 +178,46 @@ double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint see
 		return (zSum / hist);
 }
 
+void _getLinePositions(SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, SVzNLRect& roi2D, SVzNLRangeD& distRange,  std::vector<SVzNL2DPoint>& pts)
+{
+	double len = sqrt(pow(seed2.x - seed1.x, 2) + pow(seed2.y - seed1.y, 2));
+	int x0 = (int)seed1.x - roi2D.left;
+	int y0 = (int)seed1.y - roi2D.top;
+
+	int x1 = (int)seed2.x - roi2D.left;
+	int y1 = (int)seed2.y - roi2D.top;
+
+	std::vector<SVzNL2DPoint> pts_all;
+	drawLine( x0, y0, x1, y1, pts_all);
+
+	for (int i = 0; i < (int)pts_all.size(); i++)
+	{
+		double data = double((pts_all[i].x - x0) * (pts_all[i].x - x0) + (pts_all[i].y - y0) * (pts_all[i].y - y0));
+		double dist = sqrt(data);
+		double dist_k = dist / len;
+		if ((dist_k >= distRange.min) && (dist_k <= distRange.max))
+			pts.push_back(pts_all[i]);
+	}
+}
+
+void _getLinePoints(
+	std::vector<std::vector<double>>& quantiValue, 
+	SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, 
+	SVzNLRect& roi2D, SVzNLRangeD& distRange, 
+	std::vector<cv::Point3f>& Points3ds)
+{
+	std::vector<SVzNL2DPoint> pts_2d;
+	_getLinePositions(seed1, seed2, roi2D, distRange, pts_2d);
+	for (int i = 0; i < (int)pts_2d.size(); i++)
+	{
+		cv::Point3f a_pt3d;
+		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);
+	}
+}
+
 void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
 {
 	if (a_pt.z > 1E-4)
@@ -631,6 +672,17 @@ SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& listData)
 	return roi;
 }
 
+double _getMinZ(std::vector<cv::Point3f>& Points3ds)
+{
+	double minZ = -1;
+	for (int i = 0; i < (int)Points3ds.size(); i++)
+	{
+		if ((minZ < 0) || (minZ > Points3ds[i].z))
+			minZ = Points3ds[i].z;
+	}
+	return minZ;
+}
+
 //工件孔定位-拓普发工件孔定位
 void wd_workpieceHolePositioning(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
@@ -672,7 +724,7 @@ void wd_workpieceHolePositioning(
 	}
 
 	//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
-	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(	scanLines);
+	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
 	SVzNLRect roi2D;
 	roi2D.left = (int)roi3D.xRange.min;
 	roi2D.right = (int)roi3D.xRange.max;
@@ -816,7 +868,7 @@ void wd_workpieceHolePositioning(
 		//圆拟合
 		SVzNL3DPoint center;
 		double radius;
-		double err = fitCircleByLeastSquare(pointArray,	center,	radius);
+		double err = fitCircleByLeastSquare(pointArray, center, radius);
 		center.z = minZ;
 		SWD_HoleInfo a_hole;
 		a_hole.center = { center.x, center.y, center.z };
@@ -826,8 +878,10 @@ void wd_workpieceHolePositioning(
 	//分割
 	//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
 	double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
+	SVzNLRangeD linePointRange = { 0.3,  0.7 };
 
 	std::vector< WD_workpieceInfo> allWorkpiece;
+	double highest_z = -1;
 	for (int objIdx = 0; objIdx < objectSize; objIdx++)
 	{
 		if (holes[objIdx].radius < 0)
@@ -835,7 +889,7 @@ void wd_workpieceHolePositioning(
 
 		holes[objIdx].radius = -1;
 		SWD_HoleInfo& p0 = holes[objIdx];
-		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H/2);
+		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2);
 		if (idx1 < 0)
 			continue;
 
@@ -845,7 +899,7 @@ void wd_workpieceHolePositioning(
 		int idx2 = angleConditionDistanceSearch(
 			p0.center, p1.center,
 			holes,
-			workpiecePara.holeDist_L, distDeviation, workpiecePara.H/2, 
+			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
 			angleRange);
 		if (idx2 < 0)
 			continue;
@@ -870,7 +924,7 @@ void wd_workpieceHolePositioning(
 		SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
 		SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
 		double rectR = 5.0;
-		double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H/2);
+		double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2);
 		if (z1 < 1e-4)
 			z1 = center_p0p1.z;
 		double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2);
@@ -887,6 +941,45 @@ void wd_workpieceHolePositioning(
 		p1.center.z = (z1 + z3) / 2;
 		p2.center.z = (z2 + z4) / 2;
 		p3.center.z = (z3 + z4) / 2;
+
+		SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
+								(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
+								(z1 + z2 + z3 + z4) / 4 };
+		//取工件表面的点
+		std::vector<cv::Point3f> Points3ds;
+		_getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds);
+		int size_1 = (int)Points3ds.size();
+		_getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds);
+		_getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds);
+		_getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds);
+
+		double minZ = _getMinZ(Points3ds);
+		if (minZ > 1e-4)
+		{
+			if ((highest_z < 0) || (highest_z > minZ))
+				highest_z = minZ;
+		}
+
+		//计算工件法向
+		SVzNL3DPoint normalDir;
+		SVzNL3DPoint vec_norm;
+		if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
+		{
+			//计算面参数: z = Ax + By + C
+			//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
+			std::vector<double> res;
+			vzCaculateLaserPlane(Points3ds, res);
+			SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 };
+			vec_norm = vec3_normalize(vec_1);
+			vec_norm = vec3_multiply(vec_norm, 20.0);
+			normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z };
+		}
+		else
+		{
+			normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
+			vec_norm = { 0, 0, 20 };
+		}
+
 		WD_workpieceInfo a_workpiece;
 		a_workpiece.workpieceType = workpiecePara.workpieceType;
 		a_workpiece.holes.push_back(p0.center);
@@ -897,12 +990,10 @@ void wd_workpieceHolePositioning(
 		for (int m = 0; m < 4; m++)
 		{
 			SVzNL3DPoint  a_pt = a_workpiece.holes[m];
-			a_pt.z = a_pt.z + 20;  //法向，因为做过地面高平，所以法向只在z向
+			a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//法向
 			a_workpiece.holesDir.push_back(a_pt);
 		}
-		a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
+		a_workpiece.center = centerPoint;
-								(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
-								(z1 + z2 + z3 + z4) / 4 };
 
 		SVzNL3DPoint y_dir;
 		if (p0.center.x < p1.center.x)
@@ -912,10 +1003,11 @@ void wd_workpieceHolePositioning(
 		double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
 		y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
 		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
-		a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z + 20 };
+		a_workpiece.z_dir = normalDir;
 
 		allWorkpiece.push_back(a_workpiece);
 	}
+
 	int workpieceNum = (int)allWorkpiece.size();
 	if (workpieceNum == 0)
 	{
@@ -924,16 +1016,18 @@ void wd_workpieceHolePositioning(
 	}
 
 	//寻找最高点
-	std::vector< WD_workpieceInfo> zSortWorkpiece;
+	if (highest_z < 0)
-	double minZ = allWorkpiece[0].center.z;
-	for (int i = 1; i < workpieceNum; i++)
 	{
-		if (minZ > allWorkpiece[i].center.z)
+		highest_z = allWorkpiece[0].center.z;
-			minZ = allWorkpiece[i].center.z;
+		for (int i = 1; i < workpieceNum; i++)
+		{
+			if (highest_z > allWorkpiece[i].center.z)
+				highest_z = allWorkpiece[i].center.z;
+		}
 	}
 
 	//检测上层是否有残留
-	double zSliceTh = minZ - 1.5;  //往上1.5mm
+	double zSliceTh = highest_z - 1.5;  //往上1.5mm
 	std::vector<SVzNL3DPosition> topLayerPts;
 	for (int line = 0; line < lineNum; line++)
 	{
@@ -974,7 +1068,8 @@ void wd_workpieceHolePositioning(
 
 	//排序
 	//z方向排序 
-	double topLayerTh = minZ + workpiecePara.H / 2;
+	std::vector< WD_workpieceInfo> zSortWorkpiece;
+	double topLayerTh = highest_z + workpiecePara.H / 2;
 	for (int i = 0; i < workpieceNum; i++)
 	{
 		if (allWorkpiece[i].center.z < topLayerTh)

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

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