workpieceHolePositioning version 1.6.4 : 拓普发5种工件添加正反判断。

SG_Algorithm.sln

@@ -249,6 +249,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_planeLocalization_test",
 		{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
 	EndProjectSection
 EndProject
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "hybridPosePositioning_test", "hybridPosePositioning_test\hybridPosePositioning_test.vcxproj", "{288E402C-183A-49AD-8EE1-5B1AE4E6F617}"
+	ProjectSection(ProjectDependencies) = postProject
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC} = {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}
+		{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
+	EndProjectSection
+EndProject
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "hybridPosePositioning", "hybridPosePositioning\hybridPosePositioning.vcxproj", "{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}"
+	ProjectSection(ProjectDependencies) = postProject
+		{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
+	EndProjectSection
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|x64 = Debug|x64
@@ -633,6 +644,22 @@ Global
 		{E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x64.Build.0 = Release|x64
 		{E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x86.ActiveCfg = Release|Win32
 		{E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x86.Build.0 = Release|Win32
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x64.ActiveCfg = Debug|x64
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x64.Build.0 = Debug|x64
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x86.ActiveCfg = Debug|Win32
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x86.Build.0 = Debug|Win32
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x64.ActiveCfg = Release|x64
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x64.Build.0 = Release|x64
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x86.ActiveCfg = Release|Win32
+		{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x86.Build.0 = Release|Win32
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x64.ActiveCfg = Debug|x64
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x64.Build.0 = Debug|x64
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x86.ActiveCfg = Debug|Win32
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x86.Build.0 = Debug|Win32
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x64.ActiveCfg = Release|x64
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x64.Build.0 = Release|x64
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x86.ActiveCfg = Release|Win32
+		{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x86.Build.0 = Release|Win32
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

sourceCode/SG_errCode.h

@@ -12,6 +12,7 @@
 #define SG_ERR_SCAN_DIR_NOT_SUPPORTED	-1009
 #define SX_ERR_ZERO_OBJECTS				-1010
 #define SX_ERR_NULL_GROUND_PLANE		-1011
+#define SX_ERR_ZERO_2D_OBJECTS			-1012
 
 //BQ_workpiece
 #define SX_ERR_INVLD_VTREE_NUM		-2001

sourceCode/workpieceHolePositioning.cpp

@@ -33,7 +33,8 @@
 //version 1.6.1 : 添加了拓普发工件2、工件3、工件4的姿态计算。
 //version 1.6.2 : 拓普发算法代码同步。
 //version 1.6.3 : 拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。
-std::string	m_strVersion = "HolePostion 1.6.3";
+//version 1.6.4 : 拓普发5种工件添加正反判断。
+std::string	m_strVersion = "HolePostion 1.6.4";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -2402,13 +2403,19 @@ void wd_workpieceHolePositioning_3(
 	//内部参数
 	double smallHoleMinDistance = 8.0;
 	double maxDistanceFromCenterToLine = 25.0;
+	double reverseJudgeDistToCenterTH = 33.0;
+	double midPointToLongerHoleDIstTh = 20.0;  //
+	double distP1P2_short_minTh = 35.0;
+	double distP1P2_short_maxTh = 48.0;
+	double distP1P2_long_minTh = 25.0;
+	double distP1P2_long_maxTh = 38.0;
 	SVzNLRangeD distRange = { 0, bigHoleDiameter * 1.2 };
 	double deltaZ = 10.0;
 	std::vector< WD_workpieceInfo> allWorkpiece;
 	double highest_z = -1;
 	for (int objIdx = 0; objIdx < bigHoleSize; objIdx++)
 	{
-		if (objIdx == 42)
+		if (objIdx == 11)
 			int kkk = 1;
 
 		if (bigHoles[objIdx].radius < 0)
@@ -2425,10 +2432,23 @@ void wd_workpieceHolePositioning_3(
 		if (groups.size() == 0)  //工件3和工作4的特点是小孔分为上下各3个。分组
 			continue;
 
+		std::vector<std::vector<double>> distancesToCenter;
+		distancesToCenter.resize(groups.size());
+		for (int grp = 0; grp < (int)groups.size(); grp++)
+		{
+			distancesToCenter[grp].resize(groups[grp].size());
+			for (int m = 0; m < (int)groups[grp].size(); m++)
+			{
+				double dist = sqrt(pow(p0.center.x - groups[grp][m].center.x, 2) +
+					pow(p0.center.y - groups[grp][m].center.y, 2));
+				distancesToCenter[grp][m] = dist;
+			}
+		}
 		//搜索每组中距离大于大孔直径0.8倍的目标
 		SWD_HoleInfo p1, p2;
 		bool foundObj = false;
 		bool isYDIr = true;
+		bool isReverse = false;
 		for (int grp = 0; grp < (int)groups.size(); grp++)
 		{
 			std::vector<SWD_HoleInfo>& a_grp = groups[grp];
@@ -2457,6 +2477,23 @@ void wd_workpieceHolePositioning_3(
 				p1 = a_grp[idx1];
 				p2 = a_grp[idx2];
 				foundObj = true;
+
+				double dist1 = distancesToCenter[grp][idx1];
+				double dist2 = distancesToCenter[grp][idx2];
+				if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH))  //longer
+				{
+					if (p1.center.y < p0.center.y)  //长的在上面，反
+						isReverse = true;
+					foundObj = true;
+					break;
+				}
+				else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter
+				{
+					if (p1.center.y > p0.center.y)  //短的在下面，反
+						isReverse = true;
+					foundObj = true;
+					break;
+				}
 				break;
 			}
 			else if (validRelations.size() > 1)
@@ -2471,7 +2508,29 @@ void wd_workpieceHolePositioning_3(
 				int idx2 = bestPair.data_1;
 				p1 = a_grp[idx1];
 				p2 = a_grp[idx2];
-				foundObj = true;
+
+				double dist1 = distancesToCenter[grp][idx1];
+				double dist2 = distancesToCenter[grp][idx2];
+				if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH))  //longer
+				{
+					if ((bestPair.value1 > distP1P2_long_minTh)&&(bestPair.value1 < distP1P2_long_maxTh))
+					{
+						if (p1.center.y < p0.center.y)  //长的在上面，反
+							isReverse = true;
+						foundObj = true;
+						break;
+					}
+				}
+				else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter
+				{
+					if ( (bestPair.value1 > distP1P2_short_minTh)&& (bestPair.value1 < distP1P2_short_maxTh))
+					{
+						if (p1.center.y > p0.center.y)  //短的在下面，反
+							isReverse = true;
+						foundObj = true;
+						break;
+					}
+				}
 			}
 		}
 		if (false == foundObj)
@@ -2513,6 +2572,39 @@ void wd_workpieceHolePositioning_3(
 					p2 = pTmp2;
 					foundObj = true;
 					isYDIr = false;
+					//判断正反
+					bool judeOk = false;
+					for (int n = 0; n < (int)groups[0].size(); n++)
+					{
+						double _dist = sqrt(pow(p1.center.x - groups[0][n].center.x, 2) +
+							pow(p1.center.y - groups[0][n].center.y, 2));
+						if ((distancesToCenter[0][n] > reverseJudgeDistToCenterTH) && (_dist < midPointToLongerHoleDIstTh))
+						{
+							if (groups[0][n].center.y < p0.center.y)
+							{
+								judeOk = true;
+								isReverse = true;
+								break;
+							}
+						}
+					}
+					if (false == judeOk)
+					{
+						for (int n = 0; n < (int)groups[1].size(); n++)
+						{
+							double _dist = sqrt(pow(p2.center.x - groups[1][n].center.x, 2) +
+								pow(p2.center.y - groups[1][n].center.y, 2));
+							if ((distancesToCenter[1][n] > reverseJudgeDistToCenterTH) && (_dist < midPointToLongerHoleDIstTh))
+							{
+								if (groups[1][n].center.y < p0.center.y)
+								{
+									judeOk = true;
+									isReverse = true;
+									break;
+								}
+							}
+						}
+					}
 				}
 			}
 
@@ -2521,7 +2613,6 @@ void wd_workpieceHolePositioning_3(
 			continue;
 
 		SVzNL3DPoint centerPoint = p0.center;
-
 		//计算工件法向
 		SVzNLRangeD distRange = { bigHoleDiameter / 2 + 2.0, bigHoleDiameter / 2 + 4.0 };
 		std::vector<cv::Point3d> Points3ds;
@@ -2574,6 +2665,8 @@ void wd_workpieceHolePositioning_3(
 
 		WD_workpieceInfo a_workpiece;
 		a_workpiece.workpieceType = smallHoleParam.workpieceType;
+		if (true == isReverse) //反
+			a_workpiece.workpieceType = a_workpiece.workpieceType | 0x10000;
 		a_workpiece.center = centerPoint;
 		y_dir = vec3_normalize(y_dir);
 		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 };

workpieceHolePositioning/workpieceHolePositioning.vcxproj

@@ -85,12 +85,12 @@
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
     <LinkIncremental>true</LinkIncremental>
     <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
-    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
     <LinkIncremental>false</LinkIncremental>
     <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
-    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
   </PropertyGroup>
   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
     <ClCompile>

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -189,7 +189,7 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi
 	int number = (int)workpiecePositioning.size();
 	for (int i = 0; i < number; i++)
 	{
-		sprintf_s(dataStr, 250, "工件_%d", i + 1);
+		sprintf_s(dataStr, 250, "工件_%d: type=%x", i + 1, workpiecePositioning[i].workpieceType);
 		sw << dataStr << std::endl;
 		int holeNumber = (int)workpiecePositioning[i].holes.size();
 		for (int j = 0; j < holeNumber; j++)
@@ -947,11 +947,15 @@ void _XOYprojection(
 			if (i == 0)
 			{
 				rgb = { 255, 0, 0 };
+				if((holes[i].workpieceType&0xffff0000) > 0)//方向为反
+					rgb = { 255, 0, 255 };
 				size = int(3.0/scale);
 			}
 			else
 			{
 				rgb = { 255, 255, 0 };
+				if ((holes[i].workpieceType & 0xffff0000) > 0)//方向为反
+					rgb = { 0, 255, 255 };
 				size = int(3.0 / scale);
 			}
 			WD_workpieceInfo& a_hole = holes[i];
@@ -1569,7 +1573,7 @@ void TuoPuFa_holePosition_test3(void)
 	};
 
 	SVzNLRange fileIdx[TPF3_TEST_GROUP] = {
-		{1,10}, {1, 10}, {1,10}, {1,10}, {1, 10},
+		{1,11}, {1, 10}, {1,10}, {1,10}, {1, 10},
 		{1,10}, {1, 10},
 	};
 
@@ -1635,7 +1639,7 @@ void TuoPuFa_holePosition_test3(void)
 
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx = 2;
+			//fidx = 3;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
@@ -2736,11 +2740,11 @@ int main()
 		TuoPuFa_holePosition_test5();
 	else if (keSG_拓普发孔定位_工件全测试 == testMode)
 	{
-		TuoPuFa_holePosition_test();
+		//TuoPuFa_holePosition_test();
-		TuoPuFa_holePosition_test2();
+		//TuoPuFa_holePosition_test2();
 		TuoPuFa_holePosition_test3();
 		TuoPuFa_holePosition_test4();
-		TuoPuFa_holePosition_test5();
+		//TuoPuFa_holePosition_test5();
 	}
 	else if (keSG_华航孔定位 == testMode)
 		HuaHang_holePosition_test();