motorStatorPosition version 1.0.0

定子线圈抓取，初始版本

HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp

@@ -245,15 +245,16 @@ void _outputRGBDScan_RGBD(
 
 #define TEST_GET_ROI_DATA			0
 #define TEST_COMPUTE_SPACE			1
-#define TEST_GROUP 1
+#define TEST_GROUP 2
 int main()
 {
 	const char* dataPath[TEST_GROUP] = {
 		"F:/ShangGu/项目/水木宏创/槽道间距测量/模拟数据/",  //0
+		"F:/ShangGu/项目/水木宏创/槽道间距测量/现场数据/",  //0
 	};
 
 	SVzNLRange fileIdx[TEST_GROUP] = {
-		{1,9},
+		{1,9},{1,4}
 	};
 
 	const char* ver = wd_ChannelSPaceMeasureVersion();
@@ -285,7 +286,7 @@ int main()
 #endif
 
 #if TEST_COMPUTE_SPACE
-	for (int grp = 0; grp < TEST_GROUP; grp++)
+	for (int grp = 1; grp < TEST_GROUP; grp++)
 	{
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
@@ -316,15 +317,15 @@ int main()
 
 			SSG_treeGrowParam growParam;
 			growParam.maxLineSkipNum = 10;
-			growParam.yDeviation_max = 20.0;
+			growParam.yDeviation_max = 40.0;
 			growParam.maxSkipDistance = 20.0;
-			growParam.zDeviation_max = 50.0;// 
+			growParam.zDeviation_max = 100.0;// 
-			growParam.minLTypeTreeLen = 100; //mm
+			growParam.minLTypeTreeLen = 500; //mm
-			growParam.minVTypeTreeLen = 100; //mm
+			growParam.minVTypeTreeLen = 500; //mm
 
 			SSX_channelParam channelParam;
 			channelParam.channleSpaceRng = { 300, 800 };
-			channelParam.channelWidthRng = { 5,30 };
+			channelParam.channelWidthRng = { 5,50 };
 
 			bool isHorizonScan = true;  //true:激光线平行槽道；false:激光线垂直槽道
 			int errCode = 0;

bagThreadPositioning_Export/bagThreadPositioning_test.cpp

@@ -563,7 +563,7 @@ int main()
 	SVzNLRange fileIdx[TEST_GROUP] = {
 		{1,13},
 		{2,5},
-		{1,10}
+		{18,18}
 	};
 
 	const char* ver = wd_bagThreadPositioningVersion();
@@ -571,33 +571,33 @@ int main()
 
 #if CONVERT_TO_GRID
 	int convertGrp = 2;
-	for (int fidx = 1; fidx <= 10; fidx++)
+	for (int fidx = 17; fidx <= 17; fidx++)
 	{
 		char _scan_file[256];
 		sprintf_s(_scan_file, "%s%dheightline_mm.txt", dataPath[convertGrp], fidx);
 
 		std::vector<std::vector< SVzNL3DPosition>> scanData;
-		double lineStep = 0.032;
+		double lineStep = 0.033;
-		double baseZ = 350.0;
+		double baseZ = 300.0;
 		vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData);
 		bool isGridData = checkGridFormat(scanData);
 		int gridFormat = (true == isGridData) ? 1 : 0;
 		printf("%s: 栅格格式=%d\n", _scan_file, gridFormat);
 		removeNullLines(scanData);
 		std::vector<std::vector< SVzNL3DPosition>> sampleData;
-		downSampleGridData(scanData, 3, sampleData);
+		downSampleGridData(scanData, 2, sampleData);
 		//将数据恢复为按扫描线存储格式
 		sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx);
 		_outputScanDataFile(_scan_file, sampleData,0, 0, 0);
 		printf("%s: done.\n", _scan_file);
 	}
-
+#if 0
 	char _scan_file[256];
 	sprintf_s(_scan_file, "%sgroundData.txt", dataPath[convertGrp]);
 
 	std::vector<std::vector< SVzNL3DPosition>> scanData;
 	double lineStep = 0.032;
-	double baseZ = 350.0;
+	double baseZ = 300.0;
 	vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData);
 	bool isGridData = checkGridFormat(scanData);
 	int gridFormat = (true == isGridData) ? 1 : 0;
@@ -609,6 +609,7 @@ int main()
 	sprintf_s(_scan_file, "%sgroundData_grid.txt", dataPath[convertGrp]);
 	_outputScanDataFile(_scan_file, sampleData, 0, 0, 0);
 	printf("%s: done.\n", _scan_file);
+#endif
 
 #endif
 
@@ -678,8 +679,8 @@ int main()
 			}
 
 			sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx);
-			std::vector<std::vector< SVzNL3DPosition>> scanLines;
+			std::vector<std::vector< SVzNL3DPosition>> scanLines_src;
-			wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
+			wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines_src);
 
 			long t1 = (long)GetTickCount64();//统计时间
 
@@ -741,6 +742,12 @@ int main()
 				scanInfo.mark_distance = 39.0; //两个Mark的距离 
 			}
 
+			std::vector<std::vector< SVzNL3DPosition>> scanLines;;
+			if ((grp == 2) && (fidx == 18))
+				downSampleGridData(scanLines_src, 3, scanLines);
+			else
+				scanLines = scanLines_src;
+
 			int errCode = 0;
 			std::vector<SSX_bagThreadInfo> bagThreadInfo;
 			std::vector<SSX_bagThreadInfo> bagThreadInfo_relative; //相对于Mark的坐标
@@ -761,10 +768,19 @@ int main()
 			long t2 = (long)GetTickCount64();
 			printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
 			//输出测试结果
+			if (output_markCenter.size() == 2)
+			{
+				SVzNL3DPoint markDir = { output_markCenter[1].x - output_markCenter[0].x,
+										output_markCenter[1].y - output_markCenter[0].y,
+										output_markCenter[1].z - output_markCenter[0].z };
+				double distance = sqrt(pow(markDir.x, 2) + pow(markDir.y, 2) + pow(markDir.z, 2));
+				printf("  mark1: ( %g, %g, %g ), distance = %g\n", markDir.x, markDir.y, markDir.z, distance);
+			}
+
 			sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
 			_outputRGBDScan_RGBD(_scan_file, scanLines, bagThreadInfo, output_markCenter);
 			sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
-			_outputChanneltInfo(_scan_file, bagThreadInfo);
+			_outputChanneltInfo(_scan_file, bagThreadInfo_relative);
 		}
 	}
 #endif

gasFillingPortPosition_test/gasFillingPortPosition_test.cpp

@@ -303,8 +303,8 @@ void _outputRGBDScan_fillingPort_RGBD(
 	sw.close();
 }
 
-#define TEST_CONVERT_TO_GRID	1
+#define TEST_CONVERT_TO_GRID	0
-#define TEST_COMPUTE_POSE		0
+#define TEST_COMPUTE_POSE		1
 #define TEST_GROUP 2
 int main()
 {
@@ -314,7 +314,7 @@ int main()
 	};
 
 	SVzNLRange fileIdx[TEST_GROUP] = {
-		{2,30},
+		{31,31},
 	};
 
 	const char* ver = wd_gasFillingPortPositionVersion();
@@ -333,7 +333,7 @@ int main()
 #endif
 
 #if TEST_COMPUTE_POSE
-	for (int grp = 0; grp < TEST_GROUP; grp++)
+	for (int grp = 0; grp <=0; grp++)
 	{
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
@@ -355,7 +355,7 @@ int main()
 			gasFillingPortPara.outerD = 52.0;
 
 			SSG_lineSegParam lineSegPara;
-			lineSegPara.maxDist = 1.0;
+			lineSegPara.distScale = 1.0;
 			lineSegPara.segGapTh_y = 5.0;  //y方向间隔大于5mm认为是分段
 			lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段
 

motorStatorPosition_test/motorStatorPosition_test.cpp

@@ -273,6 +273,59 @@ SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints
 	return gridData;
 }
 
+void vzReadLaserScanPointFromFile_feihu(
+	const char* fileName,
+	std::vector<std::vector< SVzNL3DPosition>>& scanData)
+{
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	if (inputFile.is_open() == false)
+		return;
+
+	std::vector< SVzNL3DPosition> linePoints;
+	linePoints.clear();
+	int startIdx = -1;
+	int ptIdx = 0;
+	double pre_y = DBL_MAX;
+	while (std::getline(inputFile, linedata))
+	{
+		if (linedata.empty())
+			continue;
+
+		if (0 == strncmp("{", linedata.c_str(), 1))
+		{
+			float X, Y, Z;
+			int imageY = 0;
+			float leftX, leftY;
+			float rightX, rightY;
+			int r = -1, g = -1, b = -1;
+			sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
+
+			SVzNL3DPosition a_pt;
+			a_pt.nPointIdx = ptIdx;
+			ptIdx++;
+			a_pt.pt3D.x = X;
+			a_pt.pt3D.y = Y;
+			a_pt.pt3D.z = Z;
+
+			if ((Y+5.0) < pre_y)
+			{
+				//新行
+				if (linePoints.size() > 0)
+					scanData.push_back(linePoints);
+				linePoints.clear();
+				ptIdx = 0;
+			}
+			linePoints.push_back(a_pt);
+			pre_y = Y;
+		}
+	}
+	if (linePoints.size() > 0)
+		scanData.push_back(linePoints);
+	return;
+}
+
 void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int lineNum,
 	float lineV, int maxTimeStamp, int clockPerSecond)
 {
@@ -359,7 +412,7 @@ void _outputScanDataFile_RGBD_obj(
 	char* fileName, 
 	std::vector<std::vector< SVzNL3DPosition>>& scanData,
 	float lineV, int maxTimeStamp, int clockPerSecond, 
-	std::vector<SWD_motorStatorPosition>& resultObjPositions)
+	std::vector<SWD_statorInnerGrasper>& resultObjPositions)
 {
 	int lineNum = (int)scanData.size();
 	std::ofstream sw(fileName);
@@ -396,43 +449,8 @@ void _outputScanDataFile_RGBD_obj(
 		for (int i = 0; i < nPositionCnt; i++)
 		{
 			SVzNL3DPosition& pt3D = scanData[line][i];
-			int type = pt3D.nPointIdx;
+			rgb = { 250, 250, 250 };
-			if (1 == type)
-			{
-				rgb = { 0,255,0 };
-				rgb.r = (int)((double)rgb.r * alpha);
-				rgb.g = (int)((double)rgb.g * alpha);
-				rgb.b = (int)((double)rgb.b * alpha);
-				size = 2;
-			}
-			else if (2 == type)
-			{
-				rgb = { 0,0,255 };
-				rgb.r = (int)((double)rgb.r * alpha);
-				rgb.g = (int)((double)rgb.g * alpha);
-				rgb.b = (int)((double)rgb.b * alpha);
-				size = 2;
-			}
-			else if (3 == type) //轮眉
-			{
-				rgb = { 255, 0, 0 };
-				size = 3;
-			}
-			else if (4 == type) //
-			{
-				rgb = { 255, 255, 0 };
-				size = 3;
-			}
-			else if (5 == type) //
-			{
-				rgb = { 255, 255, 0 };
-				size = 3;
-			}
-			else
-			{
-				rgb = { 100, 100, 100 };
 			size = 1;
-			}
 
 			float x = (float)pt3D.pt3D.x;
 			float y = (float)pt3D.pt3D.y;
@@ -442,90 +460,61 @@ void _outputScanDataFile_RGBD_obj(
 			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 		}
 	}
-#if 0
+#if 1
-	//if (objOps.size() > 0)
+	std::vector<SVzNL3DPoint> centerPoints;
+	std::vector<SVzNL3DPoint> dirPoints;
+	int ptNum = (int)resultObjPositions.size();
+	if (ptNum > 0)
 	{
-		int ptNum = 3;
+		for (int i = 0; i < ptNum; i++)
-		sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl;
+		{
-
+			SVzNL3DPoint a_center = { resultObjPositions[i].opCenter.x, resultObjPositions[i].opCenter.y, resultObjPositions[i].opCenter.z };
-		rgb = { 255, 0, 0 };
+			SVzNL3DPoint a_dir = { resultObjPositions[i].opCenter.x, resultObjPositions[i].opCenter.y, resultObjPositions[i].opCenter.z + 50 };
+			centerPoints.push_back(a_center);
+			dirPoints.push_back(a_dir);
+		}
 
+		sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << (ptNum+1) << std::endl;
 		size = 10;
-		float x = (float)wheelArcHeight.wheelArchPos.x;
+		for (int i = 0; i < ptNum; i++)
-		float y = (float)wheelArcHeight.wheelArchPos.y;
+		{
-		float z = (float)wheelArcHeight.wheelArchPos.z;
+			if(i == 0)	rgb = { 255, 0, 0 };
-		sw << "{" << x << "," << y << "," << z << "}-";
+			else rgb = { 255, 255, 0 };		
-		sw << "{0,0}-{0,0}-";
+			sw << "{" << centerPoints[i].x << "," << centerPoints[i].y << "," << centerPoints[i].z << "}-";
-		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
-
-		x = (float)wheelArcHeight.wheelUpPos.x;
-		y = (float)wheelArcHeight.wheelUpPos.y;
-		z = (float)wheelArcHeight.wheelUpPos.z;
-		sw << "{" << x << "," << y << "," << z << "}-";
-		sw << "{0,0}-{0,0}-";
-		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
-
-		x = (float)wheelArcHeight.wheelDownPos.x;
-		y = (float)wheelArcHeight.wheelDownPos.y;
-		z = (float)wheelArcHeight.wheelDownPos.z;
-		sw << "{" << x << "," << y << "," << z << "}-";
-		sw << "{0,0}-{0,0}-";
-		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
-
-		x = (float)wheelArcHeight.wheelArchPos.x;
-		y = (float)wheelArcHeight.wheelArchPos.y;
-		z = (float)wheelArcHeight.wheelArchPos.z;
-		sw << "{" << x << "," << y << "," << z << "}-";
 			sw << "{0,0}-{0,0}-";
 			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 		}
-	//画出方向线
 		rgb = { 255, 0, 0 };
+		sw << "{" << centerPoints[0].x << "," << centerPoints[0].y << "," << centerPoints[0].z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+
+		//画出方向线
 		size = 3;
 		int lineIdx = 0;
+		for (int i = 0; i < ptNum; i++)
+		{
+			if (i == 0)	rgb = { 255, 0, 0 };
+			else rgb = { 255, 255, 0 };
 			sw << "Poly_" << lineIdx << "_2" << std::endl;
-	sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-";
+			sw << "{" << centerPoints[i].x << "," << centerPoints[i].y << "," << centerPoints[i].z << "}-";
 			sw << "{0,0}-{0,0}-";
 			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
-	sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-";
+			sw << "{" << dirPoints[i].x << "," << dirPoints[i].y << "," << dirPoints[i].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)wheelArcHeight.upLine[0].x << "," << (float)wheelArcHeight.upLine[0].y << "," << (float)wheelArcHeight.upLine[0].z << "}-";
-	sw << "{0,0}-{0,0}-";
-	sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
-	sw << "{" << (float)wheelArcHeight.upLine[1].x << "," << (float)wheelArcHeight.upLine[1].y << "," << (float)wheelArcHeight.upLine[1].z << "}-";
-	sw << "{0,0}-{0,0}-";
-	sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
 		
-	lineIdx++;
+		rgb = { 255, 0, 0 };
 		sw << "Poly_" << lineIdx << "_2" << std::endl;
-	sw << "{" << (float)wheelArcHeight.downLine[0].x << "," << (float)wheelArcHeight.downLine[0].y << "," << (float)wheelArcHeight.downLine[0].z << "}-";
+		sw << "{" << centerPoints[0].x << "," << centerPoints[0].y << "," << centerPoints[0].z << "}-";
 		sw << "{0,0}-{0,0}-";
 		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
-	sw << "{" << (float)wheelArcHeight.downLine[1].x << "," << (float)wheelArcHeight.downLine[1].y << "," << (float)wheelArcHeight.downLine[1].z << "}-";
+		sw << "{" << dirPoints[0].x << "," << dirPoints[0].y << "," << dirPoints[0].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)wheelArcHeight.centerLine[0].x << "," << (float)wheelArcHeight.centerLine[0].y << "," << (float)wheelArcHeight.centerLine[0].z << "}-";
-	sw << "{0,0}-{0,0}-";
-	sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
-	sw << "{" << (float)wheelArcHeight.centerLine[1].x << "," << (float)wheelArcHeight.centerLine[1].y << "," << (float)wheelArcHeight.centerLine[1].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)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-";
-	sw << "{0,0}-{0,0}-";
-	sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
-	sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-";
 		sw << "{0,0}-{0,0}-";
 		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+	}
 #endif
 	sw.close();
 }
@@ -767,6 +756,13 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in
 #define TEST_GROUP 1
 int main()
 {
+	const char* dataPath[TEST_GROUP] = {
+"F:/ShangGu/电机定子/数据1/",  //0
+	};
+
+	SVzNLRange fileIdx[TEST_GROUP] = {
+		{4,18} };
+
 #if CONVERT_TO_GRID
 	//将数据转换成栅格格式格式
 	char _scan_dir[256];
@@ -775,12 +771,12 @@ int main()
 	int dataCalib = 0;
 	int maxTimeStamp = 0;
 	int clockPerSecond = 0;
-	sprintf_s(_scan_dir, "E:\\上古\\电机定子\\");
+	int cvt_grp = 0;
 	char _scan_file[256];
-	double _F = 1300;  //f
+	double _F = 1299.956;  //f
-	for (int i = 1; i <= 1; i++)
+	for (int i = 4; i <= 18; i++)
 	{
-		sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i);
+		sprintf_s(_scan_file, "%sLaserData%d.txt", dataPath[cvt_grp], i);
 		SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
 		if (laser3DPoints_RGBD == NULL)
 			continue;
@@ -794,7 +790,7 @@ int main()
 		SVzNL3DLaserLine* gridData = _convertToGridData_XYZRGB(laser3DPoints_RGBD, lineNum, _F, camPoseR, &vldLineNum);
 
 		char _out_file[256];
-		sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i);
+		sprintf_s(_out_file, "%sLaserLine%d_grid.txt", dataPath[cvt_grp], i);
 		_outputScanDataFile_self(_out_file, gridData, vldLineNum,
 			lineV, maxTimeStamp, clockPerSecond);
 		printf("%s: convert done!\n", _scan_file);
@@ -802,12 +798,6 @@ int main()
 #endif
 
 #if TEST_COMPUTE_GRASP_POS
-	const char* dataPath[TEST_GROUP] = {
-	"F:/ShangGu/电机定子/数据1/",  //0
-	};
-
-	SVzNLRange fileIdx[TEST_GROUP] = {
-		{1,1} };
 
 
 	int endGroup = TEST_GROUP - 1;
@@ -824,14 +814,14 @@ int main()
 		poseCalibPara.planeCalib[6] = 0.0;
 		poseCalibPara.planeCalib[7] = 0.0;
 		poseCalibPara.planeCalib[8] = 1.0;
-		poseCalibPara.planeHeight = 2600.0;
+		poseCalibPara.planeHeight = 645.0;
 		for (int i = 0; i < 9; i++)
 			poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
 		char calibFile[250];
 #if 0
 		if (grp == 0)
 		{
-			sprintf_s(calibFile, "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\ground_calib_para.txt");
+			sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
 			poseCalibPara = _readCalibPara(calibFile);
 		}
 #endif
@@ -839,6 +829,10 @@ int main()
 		SWD_statorParam statorParam;
 		statorParam.statorOuterD = 85.0;
 		statorParam.statorInnerD = 50.0;
+		statorParam.statorHeight = 100.0; //定子高度
+		statorParam.plateThickness = 8.0;  //隔板厚度
+		statorParam.plateW = 500.0;//隔板长
+		statorParam.plateH = 300.0;//隔板宽
 
 		SWD_statorPositonParam algoParam;
 		memset(&algoParam, 0, sizeof(SWD_statorPositonParam));
@@ -859,11 +853,10 @@ int main()
 		algoParam.growParam.minLTypeTreeLen = 100; //mm
 		algoParam.growParam.minVTypeTreeLen = 100; //mm
 
-		SWD_nextOpParam refPos;
-		memset(&refPos, 0, sizeof(SWD_nextOpParam));
-		refPos.cuttingZ = -1; //初始值，设为-1
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
+			SWD_statorGriperState stateMachine;
+			memset(&stateMachine, 0, sizeof(SWD_statorGriperState));
 			//fidx = 4;
 			int lineNum = 0;
 			float lineV = 0.0f;
@@ -880,17 +873,15 @@ int main()
 
 			long t1 = GetTickCount64();
 			int errCode = 0;
-			std::vector<SWD_motorStatorPosition> resultObjPositions;
+			std::vector<SWD_statorInnerGrasper> resultObjPositions;
-			SWD_statorOuterGrasper resultGraspPos;
 			wd_motorStatorPosition(
 				scanLines,
 				statorParam,
 				poseCalibPara,
 				algoParam,
-				&refPos,  //上一次给出的参考位置，同时输出下一次的参考位置
+				&stateMachine,  //操作状态机。指示当前状态
 				&errCode,
-				resultObjPositions,
+				resultObjPositions);
-				resultGraspPos);
 			long t2 = GetTickCount64();
 
 			char _dbg_file[256];

sourceCode/SG_baseDataType.h

@@ -441,9 +441,9 @@ typedef struct
 
 typedef struct
 {
-	SVzNL3DPoint opCenter;  //定子中心位置
+	int objID;
-	double objR;
+	SSG_6DOF opCenter;  //定子中心位置
-}SWD_motorStatorPosition;
+}SWD_statorInnerGrasper;
 
 typedef struct
 {

sourceCode/SG_baseFunc.cpp

@@ -2864,7 +2864,7 @@ void pointCloud2DQuantization(
 #endif
 			{
 				SVzNL2DPoint v2i = { line, i };
-				backIndexing[px][px] = v2i;
+				backIndexing[px][py] = v2i;
 				quantiData[px][py].z = pt3D->pt3D.z;
 				//´¹Ö±²åÖµ
 				if (prePt)
@@ -2905,11 +2905,11 @@ void pointCloud2DQuantization(
 	int pixWin = (int)(inerPolateDistTh / scale);
 	int cols = (int)quantiData[0].size();
 	int rows = (int)quantiData.size();
-	for (int y = 0; y < rows; y++)  //和激光扫描方向一致
+	for (int y = 0; y < cols; y++)  //和激光扫描方向一致
 	{
 		int pre_x = -1;
 		double pre_value = -1;
-		for (int x = 0; x < cols; x++)
+		for (int x = 0; x < rows; x++)
 		{
 			double value = quantiData[x][y].z;
 			if (value > 1e-4)

sourceCode/SG_clustering.cpp

@@ -295,6 +295,9 @@ void wd_gridPointClustering_labelling_2(
 	int i = 0;
 	int lineNum = (int)srcData.size();
 	int linePtNum = (int)srcData[0].size();
+
+	SVzNL2DPoint& inputSeed = a_cluster[0];
+	labelMask[inputSeed.x][inputSeed.y].flag = 1; //第一个种子
 	while (1)
 	{
 		if (i >= a_cluster.size())

sourceCode/SG_errCode.h

@@ -25,6 +25,7 @@
 #define SX_ERR_INVLID_CUTTING_Z		-2101
 #define SX_ERR_ZERO_OBJ_TOPLAYER	-2102
 #define SX_ERR_ZERO_OBJ_BTMLAYER	-2103
+#define SX_ERR_GET_INVALID_PALTE	-2104  //¸ô°åÌáÈ¡´íÎó
 
 //뀔관
 #define SX_BAG_TRAY_EMPTY			-2201

sourceCode/bagThreadPositioning.cpp

@@ -235,7 +235,7 @@ void wd_bagThreadPositioning(
 	}
 	lineInterval = lineInterval / lineIntervalNum;
 
-	SVzNLRangeD jumpHeight = { scanInfo.mark_height-2.0, scanInfo.mark_height+2.0};
+	SVzNLRangeD jumpHeight = { scanInfo.mark_height-3.0, scanInfo.mark_height+3.0};
 	double markRotateAngle = 0;
 	SVzNL3DPoint markCenter = { 0,0,0 };
 	std::vector< SVzNL2DPoint> debug_markOrigin;
@@ -424,8 +424,8 @@ void wd_bagThreadPositioning(
 			{
 				double w = markClustersInfo[i].roi3D.xRange.max - markClustersInfo[i].roi3D.xRange.min;
 				double h = markClustersInfo[i].roi3D.yRange.max - markClustersInfo[i].roi3D.yRange.min;
-				if ((w >= scanInfo.mark_diameter - 1.0) && (w <= scanInfo.mark_diameter + 1.0) &&
+				if ((w >= scanInfo.mark_diameter - 1.5) && (w <= scanInfo.mark_diameter + 1.5) &&
-					(h >= scanInfo.mark_diameter - 1.0) && (h <= scanInfo.mark_diameter + 1.0))
+					(h >= scanInfo.mark_diameter - 1.5) && (h <= scanInfo.mark_diameter + 1.5))
 				{
 					validMarks.push_back(i);
 				}

sourceCode/motorStatorPosition_Export.h

@@ -13,10 +13,10 @@ typedef struct
 {
 	double statorOuterD; //定子外直径
 	double statorInnerD; //定子内孔直径
-	double statorHeight;
+	double statorHeight; //定子高度
-	double cutZ_level0;
+	double plateThickness;  //隔板厚度
-	double cutZ_level1;
+	double plateW;//隔板长
-	double gripperR;//夹爪半径
+	double plateH;//隔板宽
 }SWD_statorParam;
 
 typedef struct
@@ -26,11 +26,19 @@ typedef struct
 	SSG_treeGrowParam growParam;
 }SWD_statorPositonParam;
 
+typedef enum
+{
+	keWD_OPERATE_Uknown = 0,
+	keSG_OPERATE_TOP_LAYER,           //抓取上层
+	keSG_OPERATE_BTM_LAYER,           //抓取下层
+	keSG_OPERATE_PLATE, //抓取隔板
+} ESG_OpeationState;
+
 typedef struct
 {
 	SVzNL3DPoint refPos;
-	double cuttingZ; //z截断值，用于分割定子上半部和地面
+	ESG_OpeationState opState; //指示前一目标参考位置是否有效
-}SWD_nextOpParam;
+}SWD_statorGriperState;
 
 //读版本号
 SG_APISHARED_EXPORT const char* wd_particleSegVersion(void);
@@ -53,11 +61,10 @@ SG_APISHARED_EXPORT void wd_lineDataR(
 // 投影（注意此时边框也同时投影）->距离变换->提取定子目标->
 // 根据相邻和边框寻找最佳抓取目标和抓取点
 SG_APISHARED_EXPORT  void wd_motorStatorPosition(
-	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
 	const SWD_statorParam statorParam,
 	const SSG_planeCalibPara groundCalibPara,
 	const SWD_statorPositonParam algoParam,
-	SWD_nextOpParam* refPos,  //上一次给出的参考位置，同时输出下一次的参考位置
+	SWD_statorGriperState* opState,  //操作状态机。指示当前状态
 	int* errCode,
-	std::vector<SWD_motorStatorPosition>& resultObjPositions,
+	std::vector<SWD_statorInnerGrasper>& resultObjPositions);
-	SWD_statorOuterGrasper& resultGraspPos);

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -461,25 +461,27 @@ int main()
 	pts_eye.resize(6);
 	std::vector<cv::Point3d> pts_robot;
 	pts_robot.resize(6);
-	pts_eye[0] = { 187.22, -99.43, 1797.70 }; //, R = 0.6413, P = 0.0302, Y = -91.1494
+	pts_eye[0] = { 463.48, 347.63, 1301.43 }; //, R = 0.6413, P = 0.0302, Y = -91.1494
-	pts_eye[1] = { 186.50, -140.52, 1797.69 }; // R = 0.6413, P = 0.0302, Y = -88.8130
+	pts_eye[1] = { 254.15, 259.55, 1303.12 }; // R = 0.6413, P = 0.0302, Y = -88.8130
-	pts_eye[2] = { 256.73, -93.55, 1797.83 }; //, R = 0.6413, P = 0.0302, Y = -89.3432
+	pts_eye[2] = { 141.16, 284.02, 1302.27 }; //, R = 0.6413, P = 0.0302, Y = -89.3432
-	pts_eye[3] = { 236.69, -48.82, 1797.77 }; // R = 0.6413, P = 0.0302, Y = -89.5285
+	pts_eye[3] = { 90.66, 5.80, 1303.27 }; // R = 0.6413, P = 0.0302, Y = -89.5285
-	pts_eye[4] = { 173.62, 13.92, 1797.30 };// R = 0.6413, P = 0.0302, Y = -89.6450
+	pts_eye[4] = { 252.47, -10.45, 1304.33 };// R = 0.6413, P = 0.0302, Y = -89.6450
-	pts_eye[5] = { 232.11, -153.38, 1797.88 };// R = 0.6413, P = 0.0302, Y = -82.0278
+	pts_eye[5] = { 403.53, -13.99, 1304.24 };// R = 0.6413, P = 0.0302, Y = -82.0278
+	//pts_eye[6] = { 242.70, -74.51, 1796.83 };
 
 	//A:绕z, B：绕y， C:绕x
-	pts_robot[0] = { 55.830, -984.472, 97.687 }; // A:0.018 B : -0.638 C : -0.292
+	pts_robot[0] = { -406.649, -1246.187, 588.611 }; // A:0.018 B : -0.638 C : -0.292
-	pts_robot[1] = { 95.469, -985.345, 97.782 }; //  A:-1.032 B : -0.354 C : 1.001
+	pts_robot[1] = { -316.649, -1036.290, 587.619 }; //  A:-1.032 B : -0.354 C : 1.001
-	pts_robot[2] = { 47.908, -1053.709, 97.802 }; // A:-1.032 B : -0.356 C : 0.998
+	pts_robot[2] = { -340.590, -922.510, 586.731 }; // A:-1.032 B : -0.356 C : 0.998
-	pts_robot[3] = { 3.646, -1033.152, 96.494 }; // A:-1.028 B : -0.367 C : 0.442
+	pts_robot[3] = { -61.892,-874.564,589.841 }; // A:-1.028 B : -0.367 C : 0.442
-	pts_robot[4] = { -58.843, -969.793, 95.297 }; // A:-1.029 B : -0.367 C : 0.442
+	pts_robot[4] = { -46.658,-1036.959,589.820 }; // A:-1.029 B : -0.367 C : 0.442
-	pts_robot[5] = { 108.031, -1029.803, 98.333 }; // A:-9.208 B : -0.367 C : 0.442
+	pts_robot[5] = { -43.92, -1188.370,591.006 }; // A:-9.208 B : -0.367 C : 0.442
+	//pts_robot[6] = { 29.298, -1033.69, 97.909 };
 
 	//使用前6组数据
 	std::vector<cv::Point3d> test_pts_eye;
 	std::vector<cv::Point3d> test_pts_robot;
-	for (int i = 0; i < 5; i++)
+	for (int i = 0; i < 6; i++)
 	{
 		test_pts_eye.push_back(pts_eye[i]);
 		test_pts_robot.push_back(pts_robot[i]);
@@ -501,21 +503,30 @@ int main()
 
 	//验算6轴姿态
 	std::vector<cv::Point3d> verify_pts_eye;
-	verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.end());
+	verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.begin()+6);
-	cv::Point3d a_center = { 232.997, -173.533, 1795.9 };
+	//cv::Point3d a_center = { 232.997, -173.533, 1795.9 };
-	verify_pts_eye.push_back(a_center);
+	//verify_pts_eye.push_back(a_center);
+	//a_center = { 225.57, -68.33, 1796.77 };
+	//verify_pts_eye.push_back(a_center);
+	//a_center = { 120.45, -45.97, 1796.25 };
+	//verify_pts_eye.push_back(a_center);
+	//a_center = { 242.70, -74.51, 1796.83 };
+	//verify_pts_eye.push_back(a_center);
 	std::vector<std::vector< cv::Point3d>>  pose_eye;
-	pose_eye.resize(7);
+	pose_eye.resize(6);
-	for (int i = 0; i < 7; i++)
+	for (int i = 0; i < 6; i++)
 		pose_eye[i].resize(3);
-	pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 };
+	pose_eye[0][0] = { -0.046,-0.999,-0.009 }; pose_eye[0][1] = { 0.999,-0.046,0.004 }; pose_eye[0][2] = { -0.005,-0.009,1.000 };
-	pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 };
+	pose_eye[1][0] = { -0.049,-0.999,-0.009 }; pose_eye[1][1] = { 0.999,-0.050,0.004 }; pose_eye[1][2] = { -0.005,-0.009,1.000 };
-	pose_eye[2][0] = { 0.011,-1.000,-0.011 }; pose_eye[2][1] = { 1.000,0.011,-0.000 }; pose_eye[2][2] = { 0.001,-0.011,1.000 };
+	pose_eye[2][0] = { -0.049,-0.999,-0.009 }; pose_eye[2][1] = { 0.999,-0.049,0.004 }; pose_eye[2][2] = { -0.005,-0.009,1.000 };
-	pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 };
+	pose_eye[3][0] = { -0.059,-0.998,-0.009 }; pose_eye[3][1] = { 0.998,-0.059,0.004 }; pose_eye[3][2] = { -0.005,-0.009,1.000 };
-	pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 };
+	pose_eye[4][0] = { -0.070,-0.997,-0.009 }; pose_eye[4][1] = { 0.998,-0.070,0.004 }; pose_eye[4][2] = { -0.005,-0.009,1.000 };
-	pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 };
+	pose_eye[5][0] = { -0.068,-0.998,-0.009 }; pose_eye[5][1] = { 0.998,-0.068,0.004 }; pose_eye[5][2] = { -0.005,-0.009,1.000 };
-	pose_eye[6][0] = { 0.136746, -0.990563, -0.00926168 }; pose_eye[6][1] = { 0.990606, 0.136747, 0.000517805 }; pose_eye[6][2] = { 0.000753588, -0.00924548, 0.999957 };
+	//pose_eye[6][0] = { 0.136746, -0.990563, -0.00926168 }; pose_eye[6][1] = { 0.990606, 0.136747, 0.000517805 }; pose_eye[6][2] = { 0.000753588, -0.00924548, 0.999957 };
-	for (int i = 0; i < 7; i++)
+	//pose_eye[7][0] = { 0.058, 0.998, 0.011 }; pose_eye[7][1] = { 0.999, -0.046, 0.003 }; pose_eye[7][2] = { 0.003, 0.011, -1.000 };
+	//pose_eye[8][0] = { 0.017,1.000,0.011 }; pose_eye[8][1] = { 1.000,-0.008,0.003 }; pose_eye[8][2] = { 0.003,0.011,-1.000 };
+	//pose_eye[9][0] = { -0.316, 0.949, 0.009 }; pose_eye[9][1] = { 0.951, 0.310, 0.007 }; pose_eye[9][2] = { 0.003, 0.011, -1.000 };
+	for (int i = 0; i < 6; i++)
 	{
 		cv::Point3d rtPt;
 		pointRT_2(R, T, verify_pts_eye[i], rtPt);  //RT前后的点