bagThreadPositioning version 1.1.1 :

优化了标定柱检出。标定柱同时为扫描线端点时能够检出

bagThreadPositioning_Export/bagThreadPositioning_test.cpp

@@ -455,7 +455,101 @@ void _outputRGBDScan_RGBD(
 	sw.close();
 }
 
+void getROIdata(SSG_ROIRectD roi2D,
+	std::vector<std::vector< SVzNL3DPosition>>& srcLines, 
+	std::vector<std::vector< SVzNL3DPosition>>& dstLines)
+{
+	int lineNum = (int)srcLines.size();
+	dstLines.resize(lineNum);
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector< SVzNL3DPosition>& a_line = srcLines[line];
+		int ptNum = (int)a_line.size();
+		dstLines[line].resize(ptNum);
+		for (int j = 0; j < ptNum; j++)
+		{
+			dstLines[line][j] = a_line[j];
+			if (a_line[j].pt3D.z > 1e-4)
+			{
+				if ((a_line[j].pt3D.x < roi2D.left) || (a_line[j].pt3D.x > roi2D.right) ||
+					(a_line[j].pt3D.y < roi2D.top) || (a_line[j].pt3D.y > roi2D.bottom))
+					dstLines[line][j].pt3D = { 0, 0, 0 };
+			}
+		}
+	}
+	return;
+}
+
+void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
+{
+	std::ofstream sw(fileName);
+	char dataStr[250];
+	//调平矩阵
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]);
+	sw << dataStr << std::endl;
+	//地面高度
+	sprintf_s(dataStr, 250, "%g", calibPara.planeHeight);
+	sw << dataStr << std::endl;
+	//反向旋转矩阵
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]);
+	sw << dataStr << std::endl;
+
+	sw.close();
+}
+
+SSG_planeCalibPara _readCalibPara(char* fileName)
+{
+	//设置初始结果
+	double initCalib[9] = {
+		1.0, 0.0, 0.0,
+		0.0, 1.0, 0.0,
+		0.0, 0.0, 1.0 };
+	SSG_planeCalibPara planePara;
+	for (int i = 0; i < 9; i++)
+		planePara.planeCalib[i] = initCalib[i];
+	planePara.planeHeight = -1.0;
+	for (int i = 0; i < 9; i++)
+		planePara.invRMatrix[i] = initCalib[i];
+
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	if (inputFile.is_open() == false)
+		return planePara;
+
+	//调平矩阵
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
+	//地面高度
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
+	//反向旋转矩阵
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
+
+	inputFile.close();
+	return planePara;
+}
+
+
 #define CONVERT_TO_GRID			0
+#define COMPUTE_CALIB_PARA		0
 #define BAG_THREAD_POSITIONING	1
 #define TEST_GROUP 3
 int main()
@@ -463,13 +557,13 @@ int main()
 	const char* dataPath[TEST_GROUP] = {
 		"F:/ShangGu/项目/吕宁项目/袋子拆线/模拟数据/",  //0
 		"F:/ShangGu/项目/吕宁项目/袋子拆线/现场数据/",  //1
-		"F:/ShangGu/项目/吕宁项目/袋子拆线/BJDGS/",  //2
+		"F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/",  //2
 	};
 
 	SVzNLRange fileIdx[TEST_GROUP] = {
 		{1,13},
 		{2,5},
-		{8,9}
+		{1,10}
 	};
 
 	const char* ver = wd_bagThreadPositioningVersion();
@@ -477,7 +571,7 @@ int main()
 
 #if CONVERT_TO_GRID
 	int convertGrp = 2;
-	for (int fidx = 11; fidx <= 11; fidx++)
+	for (int fidx = 1; fidx <= 10; fidx++)
 	{
 		char _scan_file[256];
 		sprintf_s(_scan_file, "%s%dheightline_mm.txt", dataPath[convertGrp], fidx);
@@ -490,23 +584,100 @@ int main()
 		int gridFormat = (true == isGridData) ? 1 : 0;
 		printf("%s: 栅格格式=%d\n", _scan_file, gridFormat);
 		removeNullLines(scanData);
-		//std::vector<std::vector< SVzNL3DPosition>> sampleData;
+		std::vector<std::vector< SVzNL3DPosition>> sampleData;
-		//downSampleGridData(scanData, 4, sampleData);
+		downSampleGridData(scanData, 3, sampleData);
 		//将数据恢复为按扫描线存储格式
 		sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx);
-		_outputScanDataFile(_scan_file, scanData,0, 0, 0);
+		_outputScanDataFile(_scan_file, sampleData,0, 0, 0);
 		printf("%s: done.\n", _scan_file);
 	}
+
+	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;
+	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);
+	//将数据恢复为按扫描线存储格式
+	sprintf_s(_scan_file, "%sgroundData_grid.txt", dataPath[convertGrp]);
+	_outputScanDataFile(_scan_file, sampleData, 0, 0, 0);
+	printf("%s: done.\n", _scan_file);
+
+#endif
+
+#if COMPUTE_CALIB_PARA
+	char _calib_datafile[256];
+	sprintf_s(_calib_datafile, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/groundData_grid.txt");
+	std::vector<std::vector< SVzNL3DPosition>> scanLines;
+	wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
+	if (scanLines.size() > 0)
+	{
+		//getROIdata
+		std::vector<std::vector< SVzNL3DPosition>> roiScanLines;
+		SSG_ROIRectD roi2D = {91.0, 190.0, -10, 37};
+
+		getROIdata(roi2D, scanLines, roiScanLines);
+		char calibFile[250];
+		sprintf_s(calibFile, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/groundData_grid_ROI.txt");
+		_outputScanDataFile(calibFile, roiScanLines, 0, 0, 0);
+
+		SSG_planeCalibPara calibPara = wd_bagThread_getBaseCalibPara(roiScanLines);
+		//结果进行验证  
+		int lineNum = (int)scanLines.size();
+		for (int i = 0; i < lineNum; i++)
+		{
+			if (i == 14)
+				int kkk = 1;
+			//行处理
+			//调平，去除地面
+			wd_bagThread_lineDataR(scanLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
+		}
+		//
+		sprintf_s(calibFile, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/ground_calib_para.txt");
+		_outputCalibPara(calibFile, calibPara);
+		char _out_file[256];
+		sprintf_s(_out_file, "F:/ShangGu/项目/吕宁项目/袋子拆线/袋子3D数据/ground_grid_calib_verify.txt");
+		_outputScanDataFile(_out_file, scanLines, 0, 0, 0);
+		printf("%s: calib done!\n", _calib_datafile);
+	}
+
 #endif
 
 #if BAG_THREAD_POSITIONING
-	for (int grp = 1; grp <= 1; grp++)
+	for (int grp = 2; grp <= 2; grp++)
 	{
+
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
 			char _scan_file[256];
-			sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx);
+			SSG_planeCalibPara poseCalibPara;
+			//初始化成单位阵
+			poseCalibPara.planeCalib[0] = 1.0;
+			poseCalibPara.planeCalib[1] = 0.0;
+			poseCalibPara.planeCalib[2] = 0.0;
+			poseCalibPara.planeCalib[3] = 0.0;
+			poseCalibPara.planeCalib[4] = 1.0;
+			poseCalibPara.planeCalib[5] = 0.0;
+			poseCalibPara.planeCalib[6] = 0.0;
+			poseCalibPara.planeCalib[7] = 0.0;
+			poseCalibPara.planeCalib[8] = 1.0;
+			poseCalibPara.planeHeight = 2600.0;
+			for (int i = 0; i < 9; i++)
+				poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
+			if (grp == 2)
+			{
+				sprintf_s(_scan_file, "%sground_calib_para.txt", dataPath[grp]);
+				poseCalibPara = _readCalibPara(_scan_file);
+			}
 
+			sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
 			wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
 
@@ -528,7 +699,7 @@ int main()
 
 			SSG_outlierFilterParam filterParam;
 			filterParam.continuityTh = 3.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
-			filterParam.outlierTh = 2;
+			filterParam.outlierTh = 3;
 
 			SSG_treeGrowParam growParam;
 			growParam.maxLineSkipNum = -1;  //对于轮廓仪，线间隔很小，使用扫描线数的物理意义不清晰，使用maxSkipDistance
@@ -539,28 +710,36 @@ int main()
 			growParam.minVTypeTreeLen = 10; //mm
 
 			SSX_ScanInfo scanInfo;
-			scanInfo.isHorizonScan = false;  //true:激光线平行线缝；false:激光线垂直线缝
+			if (grp == 1)
-			scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。
+			{
-			scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度
+				scanInfo.isHorizonScan = false;  //true:激光线平行线缝；false:激光线垂直线缝
-			scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置
+				scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。
-			scanInfo.mark_diameter = 6.0; //mark外径
+				scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度
-			scanInfo.mark_height = 3.5;//mark高
+				scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置
-			scanInfo.mark_distance = 53.0; //两个Mark的距离 
+				scanInfo.mark_diameter = 6.0; //mark外径
-
+				scanInfo.mark_height = 3.5;//mark高
-			SSG_planeCalibPara poseCalibPara;
+				scanInfo.mark_distance = 53.0; //两个Mark的距离 
-			//初始化成单位阵
+			}
-			poseCalibPara.planeCalib[0] = 1.0;
+			else if(grp == 2 )
-			poseCalibPara.planeCalib[1] = 0.0;
+			{
-			poseCalibPara.planeCalib[2] = 0.0;
+				scanInfo.isHorizonScan = false;  //true:激光线平行线缝；false:激光线垂直线缝
-			poseCalibPara.planeCalib[3] = 0.0;
+				scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。
-			poseCalibPara.planeCalib[4] = 1.0;
+				scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度
-			poseCalibPara.planeCalib[5] = 0.0;
+				scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置
-			poseCalibPara.planeCalib[6] = 0.0;
+				scanInfo.mark_diameter = 18.0; //mark外径
-			poseCalibPara.planeCalib[7] = 0.0;
+				scanInfo.mark_height = 18.0;//mark高
-			poseCalibPara.planeCalib[8] = 1.0;
+				scanInfo.mark_distance = 39.0; //两个Mark的距离 
-			poseCalibPara.planeHeight = 2600.0;
+			}
-			for (int i = 0; i < 9; i++)
+			else
-				poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
+			{
+				scanInfo.isHorizonScan = false;  //true:激光线平行线缝；false:激光线垂直线缝
+				scanInfo.scanFromThreadHead = true; //true:线袋子线缝头部开始扫描。
+				scanInfo.stitchWidth = 1.0; //mm，线头扫描后的最小宽度
+				scanInfo.operateDist = 3.0; //mm，下刀位置距离线头位置
+				scanInfo.mark_diameter = 18.0; //mark外径
+				scanInfo.mark_height = 16.5;//mark高
+				scanInfo.mark_distance = 39.0; //两个Mark的距离 
+			}
 
 			int errCode = 0;
 			std::vector<SSX_bagThreadInfo> bagThreadInfo;

sourceCode/SG_clustering.cpp

@@ -88,7 +88,7 @@ void wd_gridPointClustering(
 			break;
 
 		SVzNL2DPoint a_seedPos = a_cluster[i];
-		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
+		if ((a_seedPos.x == 752) && (a_seedPos.y == 2399))
 			int kkk = 1;
 
 		SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
@@ -101,7 +101,7 @@ void wd_gridPointClustering(
 				{
 					int y = j + a_seedPos.y;
 					int x = i + a_seedPos.x;
-					if ((x == 390) && (y == 949))
+					if ((x == 645) && (y == 2240))
 						int kkk = 1;
 
 					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))

sourceCode/SG_lineFeature.cpp

@@ -4583,42 +4583,101 @@ void wd_getSpecifiedHeightJumping(
 )
 {
 	int dataSize = (int)lineData.size();
-	double pre_z = 0;
+	double height = (jumpHeight.min + jumpHeight.max) / 2;
-	int preIdx = -1;
+	//当两个点的间隔大于jumpHeight, 即使高差为heigth，此时的角度也小于45度,此时按分段端点处理
+	std::vector< SSG_RUN> segs;
+	int runIdx = 1;
+	SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
+	double pre_y = 0;
 	for (int i = 0; i < dataSize; i++)
 	{
-		if (i == 1850)
-			int kkk = 1;
-		lineData[i].nPointIdx = i;  //ÖŘĐÂąŕĐňşĹ
 		if (lineData[i].pt3D.z > 1e-4)
 		{
-			if(preIdx >= 0)
+			if (a_run.len < 0)
 			{
-				double z_diff = abs(lineData[i].pt3D.z - pre_z);
+				a_run.start = i;
-
+				a_run.len = 1;
-				if ( (z_diff >= jumpHeight.min) && (z_diff <= jumpHeight.max))
+				a_run.value = i;
+			}
+			else
+			{
+				double y_diff = abs(lineData[i].pt3D.y - pre_y);
+				if (y_diff < height)
 				{
-					SSG_basicFeature1D a_feature;
+					a_run.len = i - a_run.start + 1;
-					a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
+					a_run.value = i;
+				}
+				else
+				{
+					a_run.value = runIdx;
+					segs.push_back(a_run);
+					runIdx++;
 
-					if (lineData[i].pt3D.z < pre_z)
+					a_run.start = i;
-					{
+					a_run.len = 1;
-						a_feature.jumpPos = lineData[i].pt3D;
+					a_run.value = i;
-						a_feature.jumpPos2D = { lineIdx, i };
-					}
-					else
-					{
-						a_feature.jumpPos = lineData[preIdx].pt3D;
-						a_feature.jumpPos2D = { lineIdx, preIdx };
-					}
-
-					line_jumpFeatures.push_back(a_feature);
 				}
 			}
-			preIdx = i;
+			pre_y = lineData[i].pt3D.y;
-			pre_z = lineData[i].pt3D.z;
 		}
 	}
+	if (a_run.len > 0)
+		segs.push_back(a_run);
+
+	int segNum = (int)segs.size();
+	for (int si = 0; si < segNum; si++)
+	{
+		//将端点加入
+		int startIdx = segs[si].start;
+		int endIdx = segs[si].start + segs[si].len - 1;
+		SSG_basicFeature1D a_feature;
+		a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
+		a_feature.jumpPos = lineData[startIdx].pt3D;
+		a_feature.jumpPos2D = { lineIdx, startIdx };
+		line_jumpFeatures.push_back(a_feature);
+
+		double pre_z = 0;
+		int preIdx = -1;
+		for (int i = startIdx; i <= endIdx; i++)
+		{
+			if (i == 1850)
+				int kkk = 1;
+			if (lineData[i].pt3D.z > 1e-4)
+			{
+				if (preIdx >= 0)
+				{
+					double z_diff = abs(lineData[i].pt3D.z - pre_z);
+
+					if ((z_diff >= jumpHeight.min) && (z_diff <= jumpHeight.max))
+					{
+						SSG_basicFeature1D a_feature;
+						a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
+
+						if (lineData[i].pt3D.z < pre_z)
+						{
+							a_feature.jumpPos = lineData[i].pt3D;
+							a_feature.jumpPos2D = { lineIdx, i };
+						}
+						else
+						{
+							a_feature.jumpPos = lineData[preIdx].pt3D;
+							a_feature.jumpPos2D = { lineIdx, preIdx };
+						}
+
+						line_jumpFeatures.push_back(a_feature);
+					}
+				}
+				preIdx = i;
+				pre_z = lineData[i].pt3D.z;
+			}
+		}
+
+		//将最后的端点加入
+		a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
+		a_feature.jumpPos = lineData[endIdx].pt3D;
+		a_feature.jumpPos2D = { lineIdx, endIdx };
+		line_jumpFeatures.push_back(a_feature);
+	}
 }
 
 //使用端点直线，检查点到直线的距离，大于门限的分割

sourceCode/bagThreadPositioning.cpp

@@ -7,7 +7,8 @@
 
 //version 1.0.0 : base version release to customer
 //version 1.1.0 : 添加了标定Mark检测，添加了相对于标定柱的坐标输出，添加了相机调平功能
-std::string	m_strVersion = "1.1.0";
+//version 1.1.1 : 优化了标定柱检出。标定柱同时为扫描线端点时能够检出
+std::string	m_strVersion = "1.1.1";
 const char* wd_bagThreadPositioningVersion(void)
 {
 	return m_strVersion.c_str();
@@ -234,7 +235,7 @@ void wd_bagThreadPositioning(
 	}
 	lineInterval = lineInterval / lineIntervalNum;
 
-	SVzNLRangeD jumpHeight = { scanInfo.mark_height-1.0, scanInfo.mark_height+1.0};
+	SVzNLRangeD jumpHeight = { scanInfo.mark_height-2.0, scanInfo.mark_height+2.0};
 	double markRotateAngle = 0;
 	SVzNL3DPoint markCenter = { 0,0,0 };
 	std::vector< SVzNL2DPoint> debug_markOrigin;
@@ -252,7 +253,7 @@ void wd_bagThreadPositioning(
 		//垂直扫描检测V型槽和线尾
 		for (int line = 0; line < lineNum; line++)
 		{
-			if ((line == 750) ||  (line == 905))
+			if ((line == 755) ||  (line == 905))
 				int kkk = 1;
 			std::vector<SVzNL3DPosition>& lineData = scanLines[line];
 			//滤波，滤除异常点
@@ -316,7 +317,7 @@ void wd_bagThreadPositioning(
 		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
 		for (int line = 0; line < lineNum_h; line++)
 		{
-			if ((line == 750) || (line == 905))
+			if ((line == 2240) || (line == 905))
 				int kkk = 1;
 
 			std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
@@ -423,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 - 0.5) && (w <= scanInfo.mark_diameter + 1.0) &&
+				if ((w >= scanInfo.mark_diameter - 1.0) && (w <= scanInfo.mark_diameter + 1.0) &&
-					(h >= scanInfo.mark_diameter - 0.5) && (h <= scanInfo.mark_diameter + 1.0))
+					(h >= scanInfo.mark_diameter - 1.0) && (h <= scanInfo.mark_diameter + 1.0))
 				{
 					validMarks.push_back(i);
 				}