workpieceHolePositioning version 1.4.6 :

修正问题：4个孔组成工作时，需要4个孔的高度基本一致。

sourceCode/workpieceHolePositioning.cpp

@@ -16,7 +16,8 @@
 //version 1.4.3 : 对1.3.0拓普发孔定位中调平Bug进行了修正
 //version 1.4.4 : 对1.4.3一个小的修正， 不影响结果
 //version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果
-std::string	m_strVersion = "1.4.5";
+//version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。
+std::string	m_strVersion = "1.4.6";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -50,7 +51,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
 }
 
 //搜索最接近distance的目标
-int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation)
+int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation, double dirAngle, double deltaZ)
 {
 	int result = -1;
 	int holeSize = (int)holes.size();
@@ -63,10 +64,20 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, do
 
 		double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
 		double distDiff = abs(dist - distance);
-		if (minDistDiff > distDiff)
+		double zDiff = abs(seed.z - holes[i].center.z);
+		double angle = atan2(seed.y - holes[i].center.y, seed.x - holes[i].center.x);
+		angle = angle * 180.0 / PI;
+		double angleDiff = computeAngleDiff(angle, dirAngle);
+		if (angleDiff > 90)
+			angleDiff = 180 - angleDiff;
+
+		if ((zDiff < deltaZ) && (angleDiff < 45))
 		{
-			minDistDiff = distDiff;
+			if (minDistDiff > distDiff)
-			minDistIndex = i;
+			{
+				minDistDiff = distDiff;
+				minDistIndex = i;
+			}
 		}
 	}
 	if ((minDistIndex >= 0) && (minDistDiff < distDeviation))
@@ -78,7 +89,7 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, do
 int angleConditionDistanceSearch(
 	SVzNL3DPoint seed, SVzNL3DPoint angleSide,
 	std::vector<SWD_HoleInfo>& holes,
-	double distance, double distDeviation,
+	double distance, double distDeviation, double deltaZ, 
 	SVzNLRangeD angleRange)
 {
 	int result = -1;
@@ -91,7 +102,8 @@ int angleConditionDistanceSearch(
 
 		double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
 		double distDiff = abs(dist - distance);
-		if (distDiff < distDeviation)
+		double zDiff = abs(seed.z - holes[i].center.z);
+		if ( (distDiff < distDeviation) && (zDiff < deltaZ))
 		{
 			distValidHoleIndex.push_back(i);
 		}
@@ -808,6 +820,7 @@ void wd_workpieceHolePositioning(
 	//分割
 	//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
 	double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
+
 	std::vector< WD_workpieceInfo> allWorkpiece;
 	for (int objIdx = 0; objIdx < objectSize; objIdx++)
 	{
@@ -816,7 +829,7 @@ void wd_workpieceHolePositioning(
 
 		holes[objIdx].radius = -1;
 		SWD_HoleInfo& p0 = holes[objIdx];
-		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation);
+		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H/2);
 		if (idx1 < 0)
 			continue;
 
@@ -826,7 +839,7 @@ void wd_workpieceHolePositioning(
 		int idx2 = angleConditionDistanceSearch(
 			p0.center, p1.center,
 			holes,
-			workpiecePara.holeDist_L, distDeviation,
+			workpiecePara.holeDist_L, distDeviation, workpiecePara.H/2, 
 			angleRange);
 		if (idx2 < 0)
 			continue;
@@ -836,7 +849,7 @@ void wd_workpieceHolePositioning(
 		int idx3 = angleConditionDistanceSearch(
 			p1.center, p0.center,
 			holes,
-			workpiecePara.holeDist_L, distDeviation,
+			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
 			angleRange);
 		if (idx3 < 0)
 			continue;
@@ -1449,7 +1462,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, valieCommonNumRatio, 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];

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void)
 	};
 
 	SVzNLRange fileIdx[TPF_TEST_GROUP] = {
-		{6,6}, {1, 16}, {1,6}
+		{6,6}, {1, 16}, {9,9}
 	};
 
 	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 = 2; grp <= 2; grp++)
 	{
 		SSG_planeCalibPara groundCalibPara;
 		//初始化成单位阵