workpieceHolePositioning v1.0.1

初始发布版本。

sourceCode/workpieceHolePositioning.cpp

@@ -6,7 +6,7 @@
 #include <limits>
 
 //version 1.0.0 : base version release to customer
-std::string	m_strVersion = "1.0.0";
+std::string	m_strVersion = "1.0.1";
 const char* wd_workpieceHolePositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -117,6 +117,38 @@ int angleConditionDistanceSearch(
 	return result;
 }
 
+double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint seed, SVzNLRect& roi2D, double rectR)
+{
+	int cols = (int)quantiValue.size();
+	int rows = (int)quantiValue[0].size();
+
+	int px = (int)seed.x - roi2D.left;
+	int py = (int)seed.y - roi2D.top;
+	int win = (int)rectR;
+	int hist = 0;
+	double zSum = 0;
+	for (int i = -win; i <= win; i++)
+	{
+		for (int j = -win; j <= win; j++)
+		{
+			int qx = px + i;
+			int qy = py + j;
+			if ((qx >= 0) && (qx < cols) && (qy >= 0) && (qy < rows))
+			{
+				if (quantiValue[qx][qy] > 1e-4)
+				{
+					zSum += quantiValue[qx][qy];
+					hist++;
+				}
+			}
+		}
+	}
+	if (hist == 0)
+		return 0;
+	else
+		return (zSum / hist);
+}
+
 //工件孔定位
 void wd_workpieceHolePositioning(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
@@ -155,8 +187,48 @@ void wd_workpieceHolePositioning(
 	}
 
 	//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
-
+	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(	scanLines);
-
+	SVzNLRect roi2D;
+	roi2D.left = (int)roi3D.xRange.min;
+	roi2D.right = (int)roi3D.xRange.max;
+	roi2D.top = (int)roi3D.yRange.min;
+	roi2D.bottom = (int)roi3D.yRange.max;
+	int quanti_X = roi2D.right - roi2D.left + 1;
+	int quanti_Y = roi2D.bottom - roi2D.top + 1;
+	std::vector<std::vector<double>> quantiValue;
+	std::vector<std::vector<int>> quantiHist;
+	quantiValue.resize(quanti_X);
+	quantiHist.resize(quanti_X);
+	for (int i = 0; i < quanti_X; i++)
+	{
+		quantiValue[i].resize(quanti_Y);
+		std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
+		quantiHist[i].resize(quanti_Y);
+		std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
+	}
+	//以1mm尺度量化
+	for (int line = 0; line < lineNum; line++)
+	{
+		for (int j = 0; j < linePtNum; j++)
+		{
+			SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
+			if (a_pt.z > 1e-4)
+			{
+				int qx = (int)a_pt.x - roi2D.left;
+				int qy = (int)a_pt.y - roi2D.top;
+				quantiValue[qx][qy] += a_pt.z;
+				quantiHist[qx][qy] += 1;
+			}
+		}
+	}
+	for (int ix = 0; ix < quanti_X; ix++)
+	{
+		for (int iy = 0; iy < quanti_Y; iy++)
+		{
+			if (quantiHist[ix][iy] > 0)
+				quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
+		}
+	}
 
 	std::vector<std::vector<int>> pointMask;
 	pointMask.resize(lineNum);
@@ -402,24 +474,46 @@ void wd_workpieceHolePositioning(
 		p3.radius = -1;
 
 		//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值
+		SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 };
+		SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 };
+		SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
+		SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
+		double rectR = 5.0;
+		double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR);
+		double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR);
+		double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR);
+		double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR);
 		
+		p0.center.z = (z1 + z2) / 2;
+		p1.center.z = (z1 + z3) / 2;
+		p2.center.z = (z2 + z4) / 2;
+		p3.center.z = (z3 + z4) / 2;
 		WD_workpieceInfo a_workpiece;
 		a_workpiece.workpieceType = workpiecePara.workpieceType;
 		a_workpiece.holes.push_back(p0.center);
 		a_workpiece.holes.push_back(p1.center);
 		a_workpiece.holes.push_back(p2.center);
 		a_workpiece.holes.push_back(p3.center);
+
+		for (int m = 0; m < 4; m++)
+		{
+			SVzNL3DPoint  a_pt = a_workpiece.holes[m];
+			a_pt.z = a_pt.z + 20;  //法向，因为做过地面高平，所以法向只在z向
+			a_workpiece.holesDir.push_back(a_pt);
+		}
 		a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
 								(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
-								(p0.center.z + p1.center.z + p2.center.z + p3.center.z) / 4 };
+								(z1 + z2 + z3 + z4) / 4 };
 
 		SVzNL3DPoint y_dir;
 		if (p0.center.y < p2.center.y)
-			y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, a_workpiece.center.z };
+			y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, 0 };
 		else
-			y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, a_workpiece.center.z };
+			y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, 0 };
-		a_workpiece.y_dir = { y_dir.x + a_workpiece.center.x, y_dir.y + a_workpiece.center.y, y_dir.z };
+		double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
-		a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z - 10 };
+		y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
+		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
+		a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z - 20 };
 		workpiecePositioning.push_back(a_workpiece);
 	}
 
@@ -438,6 +532,8 @@ void wd_workpieceHolePositioning(
 		{
 			rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
 			workpiecePositioning[i].holes[j] = rpt;
+			rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
+			workpiecePositioning[i].holesDir[j] = rpt;
 		}
 	}
 	return;

sourceCode/workpieceHolePositioning_Export.h

@@ -17,6 +17,7 @@ typedef struct
 {
 	int workpieceType;
 	std::vector<SVzNL3DPoint> holes;
+	std::vector<SVzNL3DPoint> holesDir;
 	SVzNL3DPoint center;
 	SVzNL3DPoint z_dir;
 	SVzNL3DPoint y_dir;

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -276,6 +276,7 @@ void _outputRGBDResult_RGBD(
 			a_objPt.pt3D = workpiecePositioning[i].holes[j];
 			objects.push_back(a_objPt);
 		}
+		a_objPt.pt3D = workpiecePositioning[i].center;
 		objects.push_back(a_objPt);
 	}
 
@@ -342,11 +343,11 @@ void _outputRGBDResult_RGBD(
 	int linePtNum = (int)objects.size();
 	sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
 	lineNum++;
+	size = 10;
 	for (int i = 0; i < linePtNum; i++)
 	{
 		int colorIdx = objects[i].nPointIdx % 8;
 		rgb = objColor[colorIdx];
-		size = 10;
 		float x = (float)objects[i].pt3D.x;
 		float y = (float)objects[i].pt3D.y;
 		float z = (float)objects[i].pt3D.z;
@@ -354,7 +355,57 @@ void _outputRGBDResult_RGBD(
 		sw << "{0,0}-{0,0}-";
 		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 	}
+	//修正显示软件bug
+	rgb = objColor[1];
+	float x = (float)objects[0].pt3D.x;
+	float y = (float)objects[0].pt3D.y;
+	float z = (float)objects[0].pt3D.z;
+	sw << "{" << x << "," << y << "," << z << "}-";
+	sw << "{0,0}-{0,0}-";
+	sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 	//输出方向线条
+	rgb = { 255, 0, 0 };
+	size = 2;
+	for (int i = 0; i < objNumber; i++)
+	{
+		sw << "Poly_" << lineIdx << "_2" << std::endl;
+		sw << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+		sw << "{" << workpiecePositioning[i].y_dir.x << "," << workpiecePositioning[i].y_dir.y << "," << workpiecePositioning[i].y_dir.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 << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+		sw << "{" << workpiecePositioning[i].z_dir.x << "," << workpiecePositioning[i].z_dir.y << "," << workpiecePositioning[i].z_dir.z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+		lineIdx++;
+
+		for (int j = 0; j < (int)workpiecePositioning[i].holes.size(); j++)
+		{
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << workpiecePositioning[i].holes[j].x << "," << workpiecePositioning[i].holes[j].y << "," << workpiecePositioning[i].holes[j].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << workpiecePositioning[i].holesDir[j].x << "," << workpiecePositioning[i].holesDir[j].y << "," << workpiecePositioning[i].holesDir[j].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 << "{" << workpiecePositioning[0].center.x << "," << workpiecePositioning[0].center.y << "," << workpiecePositioning[0].center.z << "}-";
+	sw << "{0,0}-{0,0}-";
+	sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+	sw << "{" << workpiecePositioning[0].y_dir.x << "," << workpiecePositioning[0].y_dir.y << "," << workpiecePositioning[0].y_dir.z << "}-";
+	sw << "{0,0}-{0,0}-";
+	sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+
 
 	sw.close();
 }