SX_lapWeldDetection

version 1.1.0 :焊缝（平缝）识别修正回归测试中发现的bug

sourceCode/SG_featureGrow.cpp

@@ -999,17 +999,34 @@ void sg_cornerFeaturesGrowing(
 	}
 }
 
-bool _gapTreeMatch(SSG_basicFeatureGap& a_gap, SSG_gapFeatureTree& a_tree, const SSG_treeGrowParam growParam, double* yDeviation)
+bool _gapTreeMatch(SSG_basicFeatureGap& a_gap, SSG_gapFeatureTree& a_tree, const SSG_treeGrowParam growParam, double* yDeviation, int meanValueWin=5)
 {
 	//检查生长点
 	SSG_basicFeatureGap last_node = a_tree.treeNodes.back();
-	double last_cx = (last_node.gapPt_0.pt3D.x + last_node.gapPt_1.pt3D.x) / 2;
+#if 1
-	double last_cy = (last_node.gapPt_0.pt3D.y + last_node.gapPt_1.pt3D.y) / 2;
+	//È¡×îºó5¸öµãµÄƽ¾ù
-	double last_cz = (last_node.gapPt_0.pt3D.z + last_node.gapPt_1.pt3D.z) / 2;
+	int lastNodeIdx = (int)a_tree.treeNodes.size() - 1;
+	int sumNum = 0;
+	double last_cy = 0;
+	for (int i = 0; i < meanValueWin; i++)
+	{
+		last_cy += a_tree.treeNodes[lastNodeIdx].gapPeak.pt3D.y;
+		sumNum++;
+		lastNodeIdx--;
+		if (lastNodeIdx < 0)
+			break;
+	}
+	last_cy = last_cy / sumNum;
+#else
+	double last_cy = last_node.gapPeak.pt3D.y; //(last_node.gapPt_0.pt3D.y + last_node.gapPt_1.pt3D.y) / 2;
+#endif
+	double last_cx = last_node.gapPeak.pt3D.x; // (last_node.gapPt_0.pt3D.x + last_node.gapPt_1.pt3D.x) / 2;
+	double last_cz = last_node.gapPeak.pt3D.z; // (last_node.gapPt_0.pt3D.z + last_node.gapPt_1.pt3D.z) / 2;
+
 	//判断生长点
-	double curr_cx = (a_gap.gapPt_0.pt3D.x + a_gap.gapPt_1.pt3D.x) / 2;
+	double curr_cx = a_gap.gapPeak.pt3D.x;// (a_gap.gapPt_0.pt3D.x + a_gap.gapPt_1.pt3D.x) / 2;
-	double curr_cy = (a_gap.gapPt_0.pt3D.y + a_gap.gapPt_1.pt3D.y) / 2;
+	double curr_cy = a_gap.gapPeak.pt3D.y;// (a_gap.gapPt_0.pt3D.y + a_gap.gapPt_1.pt3D.y) / 2;
-	double curr_cz = (a_gap.gapPt_0.pt3D.z + a_gap.gapPt_1.pt3D.z) / 2;
+	double curr_cz = a_gap.gapPeak.pt3D.z;// (a_gap.gapPt_0.pt3D.z + a_gap.gapPt_1.pt3D.z) / 2;
 	double y_diff = abs(curr_cy - last_cy);
 	double z_diff = abs(curr_cz - last_cz);
 	int line_diff = abs(a_gap.lineIdx - last_node.lineIdx);
@@ -1146,6 +1163,8 @@ void sg_lineGapsGrowing_treeFirst(
 		for (int i = 0; i < (int)trees.size(); i++)
 		{
 			SSG_gapFeatureTree& a_tree = trees[i];
+			if (a_tree.treeState == TREE_STATE_DEAD)
+				continue;
 
 			int bestMatchLeafId = -1;
 			double minYDiff = -1;
@@ -1173,6 +1192,9 @@ void sg_lineGapsGrowing_treeFirst(
 					}
 				}
 			}
+			if ((lineIdx == 286) && (bestMatchLeafId == 3))
+				int kkk = 1;
+
 			if(bestMatchLeafId >= 0)
 			{
 				a_tree.eLineIdx = lineIdx;

sourceCode/SG_lineFeature.cpp

@@ -9734,7 +9734,7 @@ void _valleyDetect(
 								int realPeakPos = preEdge;
 								for (int m = preEdge; m <= postEdge; m++)
 								{
-									if ((a_line[realPeakPos].pt3D.z > a_line[m].pt3D.z) && (a_line[m].nPointIdx >= 0))
+									if ((a_line[realPeakPos].pt3D.z < a_line[m].pt3D.z) && (a_line[m].nPointIdx >= 0))
 										realPeakPos = m;
 								}
 
@@ -9768,6 +9768,9 @@ void wd_getLineVFlaws(
 	const SWD_valleyParam valleyParam,
 	std::vector< SSG_basicFeatureGap>& valleys)
 {
+	if (lineIdx == 286)
+		int kkk = 1;
+
 	//处理栅格数据（填补空洞，统计有效点
 	int startId = -1, endId = 0;
 	for (int i = 0; i < (int)a_line.size(); i++)

sourceCode/SX_lapWeldDetection.cpp

@@ -6,7 +6,9 @@
 #include <limits>
 
 //version 1.0.0 :焊缝（平缝）识别检测算法初始发布
-std::string	m_strVersion = "weldSeamAlgo_1.0.0";
+//version 1.1.0 :焊缝（平缝）识别修正回归测试中发现的bug
+std::string	m_strVersion = "weldSeamAlgo_1.1.0";
+
 const char* wd_weldSeamAlgoVersion(void)
 {
 	return m_strVersion.c_str();
@@ -432,6 +434,25 @@ double _getMeanZ(std::vector<double>& ptZ)
 	return meanZ;
 }
 
+double _getNearestZ(std::vector<SVzNL3DPoint>& pts, double refx)
+{
+	if (pts.size() == 0)
+		return 0;
+
+	double minDist = abs(pts[0].x - refx);
+	double bestZ = pts[0].z;
+	for (int i = 1; i < (int)pts.size(); i++)
+	{
+		double xDist = abs(pts[i].x = refx);
+		if (minDist > xDist)
+		{
+			minDist = xDist;
+			bestZ = pts[i].z;
+		}
+	}
+	return bestZ;
+}
+
 bool _compareByXValue(const SVzNL3DPoint& a, const SVzNL3DPoint& b) {
 	return a.x < b.x;
 }
@@ -445,6 +466,12 @@ void BQ_getWeldSeamPose(
 {
 	*errCode = 0;
 
+	if (trees.size() == 0)
+	{
+		*errCode = SX_ERR_ZERO_OBJECTS;
+		return;
+	}
+
 	//保留最长的tree
 	int bestId = 0;
 	trees[0].treeState = 0;
@@ -506,9 +533,9 @@ void BQ_getWeldSeamPose(
 	int refSegNum = weldRefPoints + 1;
 	refSegNum = refSegNum * 2; //以段长一半进行区分
 	std::vector<std::vector<SVzNL3DPoint>> refSegments;
-	std::vector<std::vector<double>> refSegZ;
+	std::vector<std::vector<SVzNL3DPoint>> refSegValleyPt;
 	refSegments.resize(refSegNum);
-	refSegZ.resize(refSegNum);
+	refSegValleyPt.resize(refSegNum);
 	SVzNLRangeD xRange = _getTreesXRange(weldSeam);
 	double segLen = (xRange.max - xRange.min) / refSegNum;
 	for (int i = 0; i < (int)weldSeam.size(); i++)
@@ -525,7 +552,10 @@ void BQ_getWeldSeamPose(
 				segIdx = refSegNum - 1;
 			refSegments[segIdx].push_back(weldSeam[i].treeNodes[j].gapPeak.pt3D);
 			double valleyZ = (weldSeam[i].treeNodes[j].gapPt_0.pt3D.z + weldSeam[i].treeNodes[j].gapPt_1.pt3D.z) / 2;
-			refSegZ[segIdx].push_back(valleyZ);
+			double valleyX = (weldSeam[i].treeNodes[j].gapPt_0.pt3D.x + weldSeam[i].treeNodes[j].gapPt_1.pt3D.x) / 2;
+			double valleyY = (weldSeam[i].treeNodes[j].gapPt_0.pt3D.y + weldSeam[i].treeNodes[j].gapPt_1.pt3D.y) / 2;
+			SVzNL3DPoint a_valley = { valleyX , valleyY, valleyZ };
+			refSegValleyPt[segIdx].push_back(a_valley);
 		}
 	}
 	//分段按X大小排序
@@ -540,16 +570,16 @@ void BQ_getWeldSeamPose(
 		int segIdx_0 = i * 2 - 1;
 		int segIdx_1 = i * 2;
 		std::vector<SVzNL3DPoint> fittingPoints;
-		std::vector<double> fittingPtZ;
+		std::vector<SVzNL3DPoint> fittingValleyPt;
 		if (segIdx_0 >= 0)
 		{
 			fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end());  //左半段
-			fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_0].begin(), refSegZ[segIdx_0].end());
+			fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_0].begin(), refSegValleyPt[segIdx_0].end());
 		}
 		if (segIdx_1 < refSegNum)
 		{
 			fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end());  //右半段
-			fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_1].begin(), refSegZ[segIdx_1].end());
+			fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_1].begin(), refSegValleyPt[segIdx_1].end());
 		}
 		while (fittingPoints.size() < 10)  //保证足够的拟合点
 		{
@@ -559,13 +589,13 @@ void BQ_getWeldSeamPose(
 			if (segIdx_0 >= 0)
 			{
 				fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end());
-				fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_0].begin(), refSegZ[segIdx_0].end());
+				fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_0].begin(), refSegValleyPt[segIdx_0].end());
 			}
 			segIdx_1++;
 			if (segIdx_1 < refSegNum)
 			{
 				fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end());
-				fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_1].begin(), refSegZ[segIdx_1].end());
+				fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_1].begin(), refSegValleyPt[segIdx_1].end());
 			}
 			if (oldSize == (int)fittingPoints.size())
 				break;
@@ -573,13 +603,14 @@ void BQ_getWeldSeamPose(
 		//直线拟合
 		double _a, _b, _c;
 		lineFitting_abc(fittingPoints, 	&_a,  &_b, &_c);
-		double meanZ = _getMeanZ(fittingPtZ);
+
 		double x = segLen * i * 2.0 + xRange.min;
 		double y = -(_a * x + _c) / _b;
+		double nearestZ = _getNearestZ(fittingValleyPt, x); //_getMeanZ(fittingPtZ);
 		SVzNL3DPoint a_pt;
 		a_pt.x = x;
 		a_pt.y = y;
-		a_pt.z = meanZ;
+		a_pt.z = nearestZ;
 		weldOps.push_back(a_pt);
 	}
 	return;