workpieceHolePositioning version 1.6.3 :

拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。

sourceCode/SG_featureGrow.cpp

@@ -776,7 +776,7 @@ void wd_getSegFeatureGrowingTrees_2(
 {
 	for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
 	{
-		if (i == 95)
+		if (i == 533)
 			int kkk = 1;
 		std::vector<SWD_segFeature>& a_lineFeatures = all_lineFeatures[i];
 		for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
@@ -820,7 +820,11 @@ void wd_getSegFeatureGrowingTrees_2(
 						{
 							feature_trees[m].treeState = TREE_STATE_DEAD;
 							if (len <= growParam.minLTypeTreeLen)
+							{
+								if (feature_trees[m].sLineIdx == 530)
+									int kkk = 1;
 								feature_trees.erase(feature_trees.begin() + m);
+							}
 						}
 					}
 				}

sourceCode/WD_holeDetection.cpp

@@ -10,8 +10,8 @@ void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
 		{
 			roi.left = a_pt.x;
 			roi.right = a_pt.x;
-			roi.left = a_pt.y;
+			roi.top = a_pt.y;
-			roi.right = a_pt.y;
+			roi.bottom = a_pt.y;
 		}
 		else
 		{
@@ -107,7 +107,7 @@ void WD_getHoleInfo(
 	//ÌáÈ¡Ï߶ζ˵ãÌØÕ÷
 	for (int line = 0; line < lineNum; line++)
 	{
-		if (line == 95)
+		if (line == 533)
 			int kkk = 1;
 
 		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
@@ -311,7 +311,8 @@ void WD_getHoleInfo(
 		SWD_segFeatureTree& a_tree = segTrees_v[i];
 		if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
 			int kkk = 1;
-
+		if ((treeInfo_v[i].roi.right > 260) && (treeInfo_v[i].roi.left < 270) && (treeInfo_v[i].roi.bottom > 10) && (treeInfo_v[i].roi.top < 20))
+			int kkk = 1;
 		int totalSize_v = treeInfo_v[i].treeType;
 		int commonSize = 0;
 		int hTreeIdx = -1;
@@ -325,6 +326,8 @@ void WD_getHoleInfo(
 		}
 		if (hTreeIdx >= 0)
 		{
+			if ((treeInfo_h[hTreeIdx].roi.right > 260) && (treeInfo_h[hTreeIdx].roi.left < 270) && (treeInfo_h[hTreeIdx].roi.bottom > 10) && (treeInfo_h[hTreeIdx].roi.top < 20))
+				int kkk = 1;
 			int totalSize_h = treeInfo_h[hTreeIdx].treeType;
 			int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
 			int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);

sourceCode/workpieceHolePositioning.cpp

@@ -32,7 +32,8 @@
 //version 1.6.0 : 添加了玖瑞工件（单件）的姿态计算。
 //version 1.6.1 : 添加了拓普发工件2、工件3、工件4的姿态计算。
 //version 1.6.2 : 拓普发算法代码同步。
-std::string	m_strVersion = "HolePostion 1.6.2";
+//version 1.6.3 : 拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。
+std::string	m_strVersion = "HolePostion 1.6.3";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -278,6 +279,7 @@ int distanceSearchObject(
 	SVzNLRect& roi2D, 
 	SVzNLRangeD& distRange)
 {
+	const double angleDiffTh = 15; 
 	int result = -1;
 	int holeSize = (int)holes.size();
 	double minDistDiff = DBL_MAX;
@@ -296,7 +298,7 @@ int distanceSearchObject(
 		if (angleDiff > 90)
 			angleDiff = 180 - angleDiff;
 
-		if ((zDiff < deltaZ) && (angleDiff < 45))
+		if ((zDiff < deltaZ) && (angleDiff < angleDiffTh))
 		{
 			if (minDistDiff > distDiff)
 			{
@@ -541,6 +543,64 @@ double _getMinZ(std::vector<cv::Point3d>& Points3ds)
 	return minZ;
 }
 
+//去除扫描线上的凹坑
+void _rmoveScanlineConcaves(
+	std::vector<SVzNL3DPosition>& lineData,
+	const double steppingScale,
+	const double invalidScale, //超出此尺度，方向角计算无效
+	const double minCornerTh, //凹坑对应的最小拐角
+	const double minConcaveDepth //最小凹坑不深度
+)
+{
+	//计算方向角
+	std::vector< SSG_pntDirAngle> ptDirAngles;
+	wd_computeDirAngle_wholeLine2(
+		lineData,
+		steppingScale,
+		invalidScale, //超出此尺度，方向角计算无效
+		ptDirAngles
+	);
+
+	//寻找拐点极大值
+	std::vector< SSG_pntDirAngle> cornerPeakP;
+	std::vector< SSG_pntDirAngle> cornerPeakM;
+	wd_searchCornerPeaks(
+		ptDirAngles,
+		lineData,
+		minCornerTh,
+		steppingScale * 2,
+		cornerPeakP,
+		cornerPeakM
+	);
+
+	//去除
+	const double removeDepthK = 0.75;
+	for (int idx = 0; idx < (int)cornerPeakP.size(); idx++)
+	{
+		SSG_pntDirAngle& a_peak = cornerPeakP[idx];
+		if ((abs(a_peak.forwardDiffZ) > minConcaveDepth) && (abs(a_peak.backwardDiffZ) > minConcaveDepth))
+		{
+			//寻找前后截止点：diffZ的 3/4
+			double zTh = a_peak.curr_z - abs(a_peak.backwardDiffZ) * removeDepthK;
+			for (int i = a_peak.pntIdx; i >= a_peak.backwardPntIdx; i--)
+			{
+				if (lineData[i].pt3D.z > zTh)
+					lineData[i].pt3D.z = 0;
+				else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
+					break;
+			}
+			zTh = a_peak.curr_z - abs(a_peak.forwardDiffZ) * removeDepthK;
+			for (int i = a_peak.pntIdx; i <= a_peak.forwardPntIdx; i++)
+			{
+				if (lineData[i].pt3D.z > zTh)
+					lineData[i].pt3D.z = 0;
+				else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
+					break;
+			}
+		}
+	}
+}
+
 //工件1: 孔定位-拓普发工件孔定位
 void wd_workpieceHolePositioning(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
@@ -580,6 +640,19 @@ void wd_workpieceHolePositioning(
 		//调平，去除地面
 		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
 	}
+	//去除凹坑:只有垂直方向上进行。只对小孔有效（小孔在扫描时会形成凹坑
+	double steppingScale = workpiecePara.holeDiameter;
+	double invalidScale = workpiecePara.H; //超出此尺度，方向角计算无效
+	double minCornerTh = 60; //凹坑对应的最小拐角
+	double minConcaveDepth = 5.0;
+	for (int line = 0; line < lineNum; line++)
+	{
+		if (line == 436)
+			int kkk = 1;
+		//remove concave
+		_rmoveScanlineConcaves(scanLines[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
+		//_rmoveScanlineConcaves(scanLinesInput[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
+	}
 
 	//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
 	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
@@ -1111,64 +1184,6 @@ void _checkWorkpieceCentralHoles(
 	return;
 }
 
-//去除扫描线上的凹坑
-void _rmoveScanlineConcaves(
-	std::vector<SVzNL3DPosition>& lineData,
-	const double steppingScale,
-	const double invalidScale, //超出此尺度，方向角计算无效
-	const double minCornerTh, //凹坑对应的最小拐角
-	const double minConcaveDepth //最小凹坑不深度
-)
-{
-	//计算方向角
-	std::vector< SSG_pntDirAngle> ptDirAngles;
-	wd_computeDirAngle_wholeLine2(
-		lineData,
-		steppingScale,
-		invalidScale, //超出此尺度，方向角计算无效
-		ptDirAngles
-	);
-
-	//寻找拐点极大值
-	std::vector< SSG_pntDirAngle> cornerPeakP;
-	std::vector< SSG_pntDirAngle> cornerPeakM;
-	wd_searchCornerPeaks(
-		ptDirAngles,
-		lineData,
-		minCornerTh,
-		steppingScale * 2,
-		cornerPeakP,
-		cornerPeakM
-	);
-
-	//去除
-	const double removeDepthK = 0.75;
-	for (int idx = 0; idx < (int)cornerPeakP.size(); idx++)
-	{
-		SSG_pntDirAngle& a_peak = cornerPeakP[idx];
-		if ((abs(a_peak.forwardDiffZ) > minConcaveDepth) && (abs(a_peak.backwardDiffZ) > minConcaveDepth))
-		{
-			//寻找前后截止点：diffZ的 3/4
-			double zTh = a_peak.curr_z - abs(a_peak.backwardDiffZ) * removeDepthK;
-			for (int i = a_peak.pntIdx; i >= a_peak.backwardPntIdx; i--)
-			{
-				if (lineData[i].pt3D.z > zTh)
-					lineData[i].pt3D.z = 0;
-				else if((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
-					break;
-			}
-			zTh = a_peak.curr_z - abs(a_peak.forwardDiffZ) * removeDepthK;
-			for (int i = a_peak.pntIdx; i <= a_peak.forwardPntIdx; i++)
-			{
-				if (lineData[i].pt3D.z > zTh)
-					lineData[i].pt3D.z = 0;
-				else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
-					break;
-			}
-		}
-	}
-}
-
 //工件2: 孔定位-拓普发工件孔定位
 void wd_workpieceHolePositioning_2(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
@@ -1214,10 +1229,10 @@ void wd_workpieceHolePositioning_2(
 	double steppingScale = workpiecePara.holeDiameter;
 	double invalidScale = workpiecePara.H; //超出此尺度，方向角计算无效
 	double minCornerTh = 45; //凹坑对应的最小拐角
-	double minConcaveDepth = 4.0;
+	double minConcaveDepth = 3.0;
 	for (int line = 0; line < lineNum; line++)
 	{
-		if (line == 367)
+		if (line == 436)
 			int kkk = 1;
 		//remove concave
 		_rmoveScanlineConcaves( scanLines[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
@@ -1342,8 +1357,10 @@ void wd_workpieceHolePositioning_2(
 		SVzNL3DRangeD& a_roi = clustersRoi3D[i];
 		double L = a_roi.xRange.max - a_roi.xRange.min;
 		double W = a_roi.yRange.max - a_roi.yRange.min;
-		if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2.5) &&
+		if ((a_roi.xRange.max > 260) && (a_roi.xRange.min < 270) && (a_roi.yRange.max > 10) && (a_roi.yRange.min < 20))
-			(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2.5))
+			int kkk = 1;
+		if ((L > workpiecePara.holeDiameter * 0.25) && (L < workpiecePara.holeDiameter * 2.5) &&
+			(W > workpiecePara.holeDiameter * 0.25) && (W < workpiecePara.holeDiameter * 2.5))
 			validCluserIndexing.push_back(i);
 	}
 	//生成结果
@@ -1417,6 +1434,10 @@ void wd_workpieceHolePositioning_2(
 
 		//holes[objIdx].radius = -1;
 		SWD_HoleInfo& p0 = holes[objIdx];
+		if ((p0.center.x > 230) && (p0.center.x < 240) && (p0.center.y > 10) && (p0.center.y < 25))
+			int kkk = 1;
+		if ((p0.center.x > 230) && (p0.center.x < 240) && (p0.center.y > 50) && (p0.center.y < 65))
+			int kkk = 1;
 		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2,
 			quantiValue, roi2D, linePointRange);
 		if (idx1 < 0)

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -115,9 +115,12 @@ void _removeZeroLines(std::vector<std::vector< SVzNL3DPosition>>& scanData)
 			lastLine = line;
 	}
 	if (firstLine < 0)
+	{
 		scanData.clear();
+		return;
+	}
 
-	if (lastLine < (lineNum - 1))
+	if ( (lastLine < (lineNum - 1)) && (lastLine > 0))
 		scanData.erase(scanData.begin() + lastLine + 1, scanData.end());
 
 	scanData.erase(scanData.begin(), scanData.begin() + firstLine);
@@ -1527,9 +1530,9 @@ void TuoPuFa_holePosition_test2(void)
 				&errCode);
 			long t2 = (long)GetTickCount64();
 			if (errCode == SX_ERR_UNKNOWN_OBJECT)
-				printf("%s: %d(ms), 有异物残留!\n", _scan_file, (int)(t2 - t1));
+				printf("%s: %d(ms), 有异物残留! ...", _scan_file, (int)(t2 - t1));
 			else if (errCode == SX_ERR_ZERO_OBJECTS)
-				printf("%s: %d(ms), 无产品!\n", _scan_file, (int)(t2 - t1));
+				printf("%s: %d(ms), 无产品! ...", _scan_file, (int)(t2 - t1));
 			else
 				printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode);
 			//输出测试结果
@@ -1948,7 +1951,7 @@ void TuoPuFa_holePosition_test5(void)
 
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			fidx =2;
+			//fidx =4;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
@@ -1961,7 +1964,7 @@ void TuoPuFa_holePosition_test5(void)
 
 			WD_workpieceHoleParam workpiecePara;
 			workpiecePara.workpieceType = 0;
-			workpiecePara.holeDiameter = 10.0; //
+			workpiecePara.holeDiameter = 6.0; //
 			workpiecePara.holeDist_W = 32.0;
 			workpiecePara.holeDist_L = 40.0;
 			workpiecePara.xLen = 44;
@@ -1980,8 +1983,8 @@ void TuoPuFa_holePosition_test5(void)
 			growParam.yDeviation_max = 4.0;
 			growParam.maxSkipDistance = 0.0;
 			growParam.zDeviation_max = 10.0;// 
-			growParam.minLTypeTreeLen = 2.0; //mm
+			growParam.minLTypeTreeLen = 1.5; //mm
-			growParam.minVTypeTreeLen = 2.0; //mm
+			growParam.minVTypeTreeLen = 1.5; //mm
 
 			int errCode = 0;
 			std::vector< WD_workpieceInfo> workpiecePositioning;
@@ -2698,6 +2701,7 @@ typedef enum
 	keSG_拓普发孔定位_工件3,
 	keSG_拓普发孔定位_工件4,
 	keSG_拓普发孔定位_工件5,
+	keSG_拓普发孔定位_工件全测试,
 	keSG_华航孔定位,
 	keSG_微力砂轮盘定位,
 	keSG_微力砂轮架子定位,
@@ -2710,8 +2714,9 @@ int main()
 	//ESG_testMode testMode = keSG_拓普发孔定位_工件1;
 	//ESG_testMode testMode = keSG_拓普发孔定位_工件2;
 	//ESG_testMode testMode = keSG_拓普发孔定位_工件3;
-	ESG_testMode testMode = keSG_拓普发孔定位_工件4;
+	//ESG_testMode testMode = keSG_拓普发孔定位_工件4;
 	//ESG_testMode testMode = keSG_拓普发孔定位_工件5;
+	ESG_testMode testMode = keSG_拓普发孔定位_工件全测试;
 	//ESG_testMode testMode = keSG_华航孔定位;
 	//ESG_testMode testMode = keSG_微力砂轮盘定位;
 	//ESG_testMode testMode = keSG_微力砂轮架子定位;
@@ -2729,6 +2734,14 @@ int main()
 		TuoPuFa_holePosition_test4();
 	else if (keSG_拓普发孔定位_工件5 == testMode)
 		TuoPuFa_holePosition_test5();
+	else if (keSG_拓普发孔定位_工件全测试 == testMode)
+	{
+		TuoPuFa_holePosition_test();
+		TuoPuFa_holePosition_test2();
+		TuoPuFa_holePosition_test3();
+		TuoPuFa_holePosition_test4();
+		TuoPuFa_holePosition_test5();
+	}
 	else if (keSG_华航孔定位 == testMode)
 		HuaHang_holePosition_test();
 	else if (keSG_微力砂轮盘定位 == testMode)