rodAarcFeatueDetection version 1.1.1 :

初始发布给客户的版本

rodAndBarDetection_test/rodAndBarDetection_test.cpp

@@ -429,13 +429,13 @@ void _outputRGBDScan_RGBD_rodInfo(
 				int centerFlag = pt3D->nPointIdx >> 4;
 				if (centerFlag > 0)
 				{
-					rgb = { 250, 0, 0 };
+					rgb = { 180, 0, 0 };
-					size = 6;
+					size = 2;
 				}
 				else
 				{
 					rgb = objColor[pt3D->nPointIdx % 8];
-					size = 3;
+					size = 2;
 				}
 
 
@@ -457,10 +457,13 @@ void _outputRGBDScan_RGBD_rodInfo(
 	if (objNum > 0)
 	{
 		sw << "Line_" << lineIdx << "_0_" << objNum << std::endl;
-		rgb = { 250, 0, 0 };
+		size = 12;
-		size = 8;
 		for (int i = 0; i < objNum; i++)
 		{
+			if(i == 0)
+				rgb = { 250, 255, 0 };
+			else
+				rgb = { 250, 0, 0 };
 			float x = (float)rodInfo[i].center.x;
 			float y = (float)rodInfo[i].center.y;
 			float z = (float)rodInfo[i].center.z;
@@ -468,51 +471,42 @@ void _outputRGBDScan_RGBD_rodInfo(
 			sw << "{0,0}-{0,0}-";
 			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 		}
-		//多输出一个，修正显示工具bug
-		float x = (float)rodInfo[0].center.x;
-		float y = (float)rodInfo[0].center.y;
-		float z = (float)rodInfo[0].center.z;
-		sw << "{" << x << "," << y << "," << z << "}-";
-		sw << "{0,0}-{0,0}-";
-		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
-
 		//输出法向
-		size = 1;
+		size = 8;
 		double len = 60;
 		lineIdx = 0;
 		for (int i = 0; i < objNum; i++)
 		{
-			SVzNL3DPoint pt0 = { rodInfo[i].center.x - len * rodInfo[i].axialDir.x,
+			if (i == 0)
-								 rodInfo[i].center.y - len * rodInfo[i].axialDir.y,
+				rgb = { 250, 255, 0 };
-								 rodInfo[i].center.z - len * rodInfo[i].axialDir.z };
+			else
+				rgb = { 250, 0, 0 };
+			SVzNL3DPoint pt0 = { rodInfo[i].center.x, rodInfo[i].center.y, rodInfo[i].center.z };
 			SVzNL3DPoint pt1 = { rodInfo[i].center.x + len * rodInfo[i].axialDir.x,
 								 rodInfo[i].center.y + len * rodInfo[i].axialDir.y,
 								 rodInfo[i].center.z + len * rodInfo[i].axialDir.z };
+			SVzNL3DPoint pt2 = { rodInfo[i].center.x + len * rodInfo[i].normalDir.x,
+								 rodInfo[i].center.y + len * rodInfo[i].normalDir.y,
+								 rodInfo[i].center.z + len * rodInfo[i].normalDir.z };
+			//显示轴向量
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << (float)rodInfo[i].startPt.x << "," << (float)rodInfo[i].startPt.y << "," << (float)rodInfo[i].startPt.z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << (float)rodInfo[i].endPt.x << "," << (float)rodInfo[i].endPt.y << "," << (float)rodInfo[i].endPt.z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
 			//显示法向量
 			sw << "Poly_" << lineIdx << "_2" << std::endl;
 			sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
 			sw << "{0,0}-{0,0}-";
 			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
-			sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
+			sw << "{" << (float)pt2.x << "," << (float)pt2.y << "," << (float)pt2.z << "}-";
 			sw << "{0,0}-{0,0}-";
 			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
 			lineIdx++;
 		}
-		//多输出一个，修正显示工具bug
-		SVzNL3DPoint pt0 = { rodInfo[0].center.x - len * rodInfo[0].axialDir.x,
-							 rodInfo[0].center.y - len * rodInfo[0].axialDir.y,
-							 rodInfo[0].center.z - len * rodInfo[0].axialDir.z };
-		SVzNL3DPoint pt1 = { rodInfo[0].center.x + len * rodInfo[0].axialDir.x,
-							 rodInfo[0].center.y + len * rodInfo[0].axialDir.y,
-							 rodInfo[0].center.z + len * rodInfo[0].axialDir.z };
-		//显示法向量
-		sw << "Poly_" << lineIdx << "_2" << std::endl;
-		sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
-		sw << "{0,0}-{0,0}-";
-		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
-		sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
-		sw << "{0,0}-{0,0}-";
-		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
 		lineIdx++;
 	}
 	sw.close();
@@ -630,7 +624,7 @@ void rodPositionTest(void)
 
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			fidx =1;
+			//fidx =1;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
 

sourceCode/rodAndBarDetection.cpp

@@ -7,7 +7,8 @@
 
 //version 1.0.0 : base version release to customer
 //version 1.1.0 : 添加了地面调平和棒材定位
-std::string	m_strVersion = "1.1.0";
+//version 1.1.1 : 初始发布给客户的版本
+std::string	m_strVersion = "1.1.1";
 const char* wd_rodAndBarDetectionVersion(void)
 {
 	return m_strVersion.c_str();
@@ -477,6 +478,136 @@ void rodAarcFeatueDetection(
 	return;
 }
 
+SVzNL3DPoint _exchangeXY(SVzNL3DPoint pt)
+{
+	SVzNL3DPoint result = {pt.y, pt.x, pt.z};
+	return result;
+}
+
+void _computeRodInfo(
+	SWD_rodArcFeatureTree& a_objTree,
+	bool treeIsHorizon, 
+	std::vector< SVzNL3DPoint>& fittingPoints, 
+	SSX_rodPositionInfo& a_objRod)
+{
+	int nodeSize = a_objTree.treeNodes.size();
+	//拟合
+	double _a, _b, _c;
+	lineFitting_abc(fittingPoints, &_a, &_b, &_c);
+	int dataSize = (int)fittingPoints.size();
+	SVzNL2DPointD foot1 = sx_getFootPoint_abc(fittingPoints[0].x, fittingPoints[0].y, _a, _b, _c);
+	SVzNL2DPointD foot2 = sx_getFootPoint_abc(fittingPoints[dataSize - 1].x, fittingPoints[dataSize - 1].y, _a, _b, _c);
+	double deltaZ = fittingPoints[dataSize - 1].z - fittingPoints[0].z;
+	double len = sqrt(pow(foot1.x - foot2.x, 2) + pow(foot1.y - foot2.y, 2));
+	//直线的轴向向量
+	SVzNL3DPoint axialDir = { foot2.x - foot1.x, foot2.y - foot1.y, fittingPoints[dataSize - 1].z - fittingPoints[0].z };
+	//归一化
+	double normData = sqrt(pow(axialDir.x, 2) + pow(axialDir.y, 2) + pow(axialDir.z, 2));
+	axialDir.x = axialDir.x / normData;
+	axialDir.y = axialDir.y / normData;
+	axialDir.z = axialDir.z / normData;
+	//计算一个辅助平面（Y=0平面旋转一个角度）的法向
+	double theta = atan2(foot2.y - foot1.y, foot2.x - foot1.x);
+	double sinTheta = sin(theta);
+	double cosTheta = cos(theta);
+	SVzNL3DPoint tmpDir = { sinTheta, -cosTheta, 0 };
+	//叉乘出棒材的法向
+	SVzNL3DPoint normalDir = { axialDir.y * tmpDir.z - tmpDir.y * axialDir.z,
+								axialDir.z * tmpDir.x - tmpDir.z * axialDir.x,
+								axialDir.x * tmpDir.y - tmpDir.x * axialDir.y };
+
+
+	//确定真正起点和终点：到直线距离小于5mm
+	double tmpData = sqrt(_a * _a + _b * _b);
+	_a = _a / tmpData;
+	_b = _b / tmpData;
+	_c = _c / tmpData;
+	SVzNL3DPoint realStart, realEnd;
+	bool foundStart = false;
+	for (int j = 0; j < nodeSize; j++)
+	{
+		SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
+		if (true == treeIsHorizon)
+			a_pt = _exchangeXY(a_pt);
+		double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
+		if (dist < 5.0)
+		{
+			realStart = a_pt;
+			foundStart = true;
+			break;
+		}
+	}
+	if (false == foundStart)
+	{
+		realStart = a_objTree.treeNodes[0].peakPt;
+		if (true == treeIsHorizon)
+			realStart = _exchangeXY(realStart);
+	}
+
+	bool foundEnd = false;
+	for (int j = nodeSize - 1; j >= 0; j--)
+	{
+		SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
+		if (true == treeIsHorizon)
+			a_pt = _exchangeXY(a_pt);
+		double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
+		if (dist < 5.0)
+		{
+			realEnd = a_pt;
+			foundEnd = true;
+			break;
+		}
+	}
+	if (false == foundEnd)
+	{
+		realEnd = a_objTree.treeNodes[nodeSize - 1].peakPt;
+		if (true == treeIsHorizon)
+			realEnd = _exchangeXY(realEnd);
+	}
+
+	SVzNL2DPointD foot_s = sx_getFootPoint_abc(realStart.x, realStart.y, _a, _b, _c);
+	SVzNL2DPointD foot_e = sx_getFootPoint_abc(realEnd.x, realEnd.y, _a, _b, _c);
+	double dist_s = sqrt(pow(foot_s.x - foot1.x, 2) + pow(foot_s.y - foot1.y, 2));
+	double dist_e = sqrt(pow(foot_e.x - foot1.x, 2) + pow(foot_e.y - foot1.y, 2));
+	//生成目标信息
+	;
+	a_objRod.startPt = { foot_s.x, foot_s.y, -(dist_s / len) * deltaZ + fittingPoints[0].z };
+	a_objRod.endPt = { foot_e.x, foot_e.y, (dist_e / len) * deltaZ + fittingPoints[0].z };
+	a_objRod.center = { (a_objRod.startPt.x + a_objRod.endPt.x) / 2,
+					(a_objRod.startPt.y + a_objRod.endPt.y) / 2,
+					(a_objRod.startPt.z + a_objRod.endPt.z) / 2 };
+	a_objRod.axialDir = axialDir;
+	a_objRod.normalDir = normalDir;
+
+	return;
+}
+
+bool checkObjEixst(SSX_rodPositionInfo& a_objRod, std::vector<SSX_rodPositionInfo>& existObjs, const SSX_rodParam rodParam)
+{
+	double minDistance = -1;
+	for (int i = 0; i < (int)existObjs.size(); i++)
+	{
+		double dist = sqrt(pow(a_objRod.center.x - existObjs[i].center.x, 2) +
+			pow(a_objRod.center.y - existObjs[i].center.y, 2) +
+			pow(a_objRod.center.z - existObjs[i].center.z, 2));
+		if (minDistance < 0)
+			minDistance = dist;
+		else if (minDistance > dist)
+			minDistance = dist;
+	}
+	if (minDistance < 0)
+		return false;
+	else if (minDistance < rodParam.diameter / 4) // R/2
+		return true;
+	else
+		return false;
+}
+
+bool _commpareByCenterZ(SSX_rodPositionInfo& a, SSX_rodPositionInfo& b)
+{
+	return  (a.center.z < b.center.z);
+}
+
 void sx_rodPositioning(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	const SSG_planeCalibPara poseCalibPara,
@@ -597,6 +728,7 @@ void sx_rodPositioning(
 	}
 	//目标判断
 	//(1)长度过滤
+	//垂直目标
 	for (int i = 0; i < (int)rodArcTrees_v.size(); i++)
 	{
 		int nodeSize = rodArcTrees_v[i].treeNodes.size();
@@ -606,7 +738,7 @@ void sx_rodPositioning(
 						pow(startCenter.y - endCenter.y, 2) +
 						pow(startCenter.z - endCenter.z, 2));
 		double lenDiff = abs(len - rodParam.len);
-		if (lenDiff < 10.0)  //validObj
+		if (lenDiff < rodParam.len* 0.15)  //validObj
 		{
 			//在XY平面内直线拟合
 			//为了防止端部影响，跳过端面数据
@@ -624,48 +756,51 @@ void sx_rodPositioning(
 			}
 			if (fittingPoints.size() < 3)
 				continue;
-			//拟合
-			double _a, _b, _c;
-			lineFitting_abc(fittingPoints, &_a, &_b, &_c);
-			int dataSize = (int)fittingPoints.size();
-			SVzNL2DPointD foot1 = sx_getFootPoint_abc(fittingPoints[0].x, fittingPoints[0].y, _a, _b, _c);
-			SVzNL2DPointD foot2 = sx_getFootPoint_abc(fittingPoints[dataSize - 1].x, fittingPoints[dataSize - 1].y, _a, _b, _c);
-			//直线的轴向向量
-			SVzNL3DPoint axialDir = { foot2.x - foot1.x, foot2.y - foot1.y, fittingPoints[dataSize - 1].z - fittingPoints[0].z };
-			double theta = atan2(foot2.y - foot1.y, foot2.x - foot1.x);
-			double sinTheta = sin(theta);
-			double cosTheta = cos(theta);
-			SVzNL3DPoint tmpDir = { sinTheta, -cosTheta, 0 };
-			SVzNL3DPoint normalDir = { axialDir.y * tmpDir.z - tmpDir.y * axialDir.z,
-										axialDir.z * tmpDir.x - tmpDir.z * axialDir.x,
-										axialDir.x * tmpDir.y - tmpDir.x * axialDir.y };
-
 			SSX_rodPositionInfo a_objRod;
-			//确定真正起点和终点：到直线距离小于5mm
+			_computeRodInfo(rodArcTrees_v[i], false, fittingPoints, a_objRod);
-			double tmpData = sqrt(_a * _a + _b * _b);
+			rodInfo.push_back(a_objRod);
-			_a = _a / tmpData;
+		}
-			_b = _b / tmpData;
+	}
-			_c = _c / tmpData;
+	//水平目标
-			SVzNL3DPoint realStart, realEnd;
+	for (int i = 0; i < (int)rodArcTrees_h.size(); i++)
-			bool foundStart = false;
+	{
+		int nodeSize = rodArcTrees_h[i].treeNodes.size();
+		SVzNL3DPoint startCenter = rodArcTrees_h[i].treeNodes[0].peakPt;
+		startCenter = _exchangeXY(startCenter);
+		SVzNL3DPoint endCenter = rodArcTrees_h[i].treeNodes[nodeSize - 1].peakPt;
+		endCenter = _exchangeXY(endCenter);
+
+		double len = sqrt(pow(startCenter.x - endCenter.x, 2) +
+			pow(startCenter.y - endCenter.y, 2) +
+			pow(startCenter.z - endCenter.z, 2));
+		double lenDiff = abs(len - rodParam.len);
+		if (lenDiff < rodParam.len * 0.15)  //validObj
+		{
+			//在XY平面内直线拟合
+			//为了防止端部影响，跳过端面数据
+			std::vector<SVzNL3DPoint> fittingPoints;
 			for (int j = 0; j < nodeSize; j++)
 			{
-				double dist = abs(rodArcTrees_v[i].treeNodes[j].peakPt.x * _a +
+				SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[j].peakPt);
-					rodArcTrees_v[i].treeNodes[j].peakPt.y * _b + _c);
+				double dist1 = sqrt(pow(a_pt.x - startCenter.x, 2) + pow(a_pt.y - startCenter.y, 2) + pow(a_pt.z - startCenter.z, 2));
-				if (dist < 5.0)
+				double dist2 = sqrt(pow(a_pt.x - endCenter.x, 2) + pow(a_pt.y - endCenter.y, 2) + pow(a_pt.z - endCenter.z, 2));
-				{
+				if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
-					realStart = rodArcTrees_v[i].treeNodes[j].peakPt;
+					fittingPoints.push_back(a_pt);
-					foundStart = true;
-					break;
-				}
 			}
+			if (fittingPoints.size() < 3)
+				continue;
 
-
+			SSX_rodPositionInfo a_objRod;
+			_computeRodInfo(rodArcTrees_h[i], true, fittingPoints, a_objRod);
+			//检查是否与垂直检测目标重叠
+			bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam);
+			if(false == isExist)
+				rodInfo.push_back(a_objRod);
 		}
 	}
 	//（2）遮挡判断
-
+	//按高度排序
-
+	std::sort(rodInfo.begin(), rodInfo.end(), _commpareByCenterZ);
 
 	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
 	for (int i = 0; i < lineNum; i++)
@@ -680,6 +815,11 @@ void sx_rodPositioning(
 		a_rod.axialDir = rawObj;
 		rawObj = _translatePoint(a_rod.normalDir, poseCalibPara.invRMatrix);
 		a_rod.normalDir = rawObj;
+
+		rawObj = _translatePoint(a_rod.startPt, poseCalibPara.invRMatrix);
+		a_rod.startPt = rawObj;
+		rawObj = _translatePoint(a_rod.endPt, poseCalibPara.invRMatrix);
+		a_rod.endPt = rawObj;
 	}
 	return;
 }