workpieceHolePositioning version 1.5.2 :

修正工件判断中的一个逻辑错误。

sourceCode/workpieceHolePositioning.cpp

@@ -22,7 +22,8 @@
 //version 1.4.9 : 修正工件法向计算的一个Bug
 //version 1.5.0 : 修正工件Y方向矢量计算的一个Bug（不在工件表面平面内）
 //version 1.5.1 : 异物检测时，检测高度由工件最小Z向上1.5mm调整为工件最小Z向上工件高度的一半，工件倾斜时容错性能更好。
-std::string	m_strVersion = "1.5.1";
+//version 1.5.2 : 修正工件判断中的一个逻辑错误。
+std::string	m_strVersion = "1.5.2";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -55,8 +56,79 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
 	return _r_pt;
 }
 
+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];
+		if (a_pt3d.z > 1e-4)
+			Points3ds.push_back(a_pt3d);
+	}
+}
+
+bool _validateLineZChange(
+	std::vector<std::vector<double>>& quantiValue,
+	SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
+	SVzNLRect& roi2D, SVzNLRangeD& distRange,
+	double maxZChange
+	)
+{
+	std::vector<cv::Point3f> Points3ds;
+	_getLinePoints(
+		quantiValue,
+		seed1, seed2,
+		roi2D, distRange,
+		Points3ds);
+
+	double refZ = (seed1.z + seed2.z) / 2;
+	for (int i = 0; i < (int)Points3ds.size(); i++)
+	{
+		double zDiff = abs(Points3ds[i].z - refZ);
+		if (zDiff > maxZChange)
+			return false;
+	}
+	return true;
+}
+
 //搜索最接近distance的目标
-int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation, double dirAngle, double deltaZ)
+int distanceSearchObject(
+	SVzNL3DPoint seed, 
+	std::vector<SWD_HoleInfo>& holes, 
+	double distance, double distDeviation, double dirAngle, double deltaZ, 
+	std::vector<std::vector<double>>& quantiValue, 
+	SVzNLRect& roi2D, 
+	SVzNLRangeD& distRange)
 {
 	int result = -1;
 	int holeSize = (int)holes.size();
@@ -80,8 +152,18 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, do
 		{
 			if (minDistDiff > distDiff)
 			{
-				minDistDiff = distDiff;
+				//添加距离检测
-				minDistIndex = i;
+				bool isValid = _validateLineZChange(
+					quantiValue,
+					seed, holes[i].center,
+					roi2D, distRange,
+					deltaZ
+				);
+				if (true == isValid)
+				{
+					minDistDiff = distDiff;
+					minDistIndex = i;
+				}
 			}
 		}
 	}
@@ -95,7 +177,10 @@ int angleConditionDistanceSearch(
 	SVzNL3DPoint seed, SVzNL3DPoint angleSide,
 	std::vector<SWD_HoleInfo>& holes,
 	double distance, double distDeviation, double deltaZ, 
-	SVzNLRangeD angleRange)
+	SVzNLRangeD angleRange,
+	std::vector<std::vector<double>>& quantiValue,
+	SVzNLRect& roi2D,
+	SVzNLRangeD& distRange)
 {
 	int result = -1;
 	int holeSize = (int)holes.size();
@@ -134,8 +219,18 @@ int angleConditionDistanceSearch(
 				double angleDiff = abs(angle - bestAngle);
 				if (minAngleDeviateion > angleDiff)
 				{
-					minAngleDeviateion = angleDiff;
+					//添加距离检测
-					minAngleIdx = idx;
+					bool isValid = _validateLineZChange(
+						quantiValue,
+						seed, holes[idx].center,
+						roi2D, distRange,
+						deltaZ
+					);
+					if (true == isValid)
+					{
+						minAngleDeviateion = angleDiff;
+						minAngleIdx = idx;
+					}
 				}
 			}
 		}
@@ -181,47 +276,6 @@ 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];
-		if(a_pt3d.z > 1e-4)
-			Points3ds.push_back(a_pt3d);
-	}
-}
-
 void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
 {
 	if (a_pt.z > 1E-4)
@@ -893,7 +947,8 @@ 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,
+			quantiValue, roi2D, linePointRange);
 		if (idx1 < 0)
 			continue;
 
@@ -904,7 +959,8 @@ void wd_workpieceHolePositioning(
 			p0.center, p1.center,
 			holes,
 			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
-			angleRange);
+			angleRange, 
+			quantiValue, roi2D, linePointRange);
 		if (idx2 < 0)
 			continue;
 
@@ -914,10 +970,20 @@ void wd_workpieceHolePositioning(
 			p1.center, p0.center,
 			holes,
 			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
-			angleRange);
+			angleRange, 
+			quantiValue, roi2D, linePointRange);
 		if (idx3 < 0)
 			continue;
+
 		SWD_HoleInfo& p3 = holes[idx3];
+
+		//交叉检查P0P1, P2P3的长度差
+		double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2));
+		double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2));
+		double dist_k = dist_P2P3 / dist_P0P1;
+		if (dist_k > 2.0)
+			continue;
+
 		p1.radius = -1;
 		p2.radius = -1;
 		p3.radius = -1;

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}, {1, 16}, {1,19}
 	};
 
 	const char* ver = wd_workpieceHolePositioningVersion();