Merge branch 'main' of http://192.168.3.13:3000/jerryzeng/algoLib

bagPositioning_test/bagPositioning_test.cpp

@@ -2429,7 +2429,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData,
 #define TEST_COMPUTE_GRASP_POINT	1
 #define TEST_COMPUTE_CALIB_PARA		0
 
-#define TEST_GROUP 21
+#define TEST_GROUP 22
 #define TEST_TOP_VIEW_GROUP			(TEST_GROUP - 8)
 #define TEST_TOP_ORIEN_GROUP		(TEST_GROUP - 6)
 int main()
@@ -2518,7 +2518,7 @@ int main()
 
 #if TEST_COMPUTE_CALIB_PARA
 	char _calib_datafile[256];
-	sprintf_s(_calib_datafile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\底面点云\\底面点云.txt");
+	sprintf_s(_calib_datafile, "F:\\ShangGu\\编织袋数据\\山东现场\\调平数据.txt");
 	int lineNum = 0;
 	float lineV = 0.0f;
 	int dataCalib = 0;
@@ -2541,10 +2541,10 @@ int main()
 		}
 		//
 		char calibFile[250];
-		sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt");
+		sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\山东现场\\ground_calib_para.txt");
 		_outputCalibPara(calibFile, calibPara);
 		char _out_file[256];
-		sprintf_s(_out_file, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\LaserLine1_grid_calib.txt");
+		sprintf_s(_out_file, "F:\\ShangGu\\编织袋数据\\山东现场\\ground_grid_calib.txt");
 		_outputScanDataFile_self(_out_file, laser3DPoints, lineNum,
 			lineV, maxTimeStamp, clockPerSecond);
 		printf("%s: calib done!\n", _calib_datafile);
@@ -2567,22 +2567,23 @@ int main()
 	"F:\\ShangGu\\编织袋数据\\点云11_盐袋\\", //10
 	"F:\\ShangGu\\编织袋数据\\点云12_盐袋\\", //11
 	"F:\\ShangGu\\编织袋数据\\点云13_现场测试\\", //12
-	"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //13
+	"F:\\ShangGu\\编织袋数据\\山东现场\\", //13
-	"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //14
+	"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //14
-	"F:\\ShangGu\\编织袋数据\\侧抓数据\\", //15
+	"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //15
-	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //16
+	"F:\\ShangGu\\编织袋数据\\侧抓数据\\", //16
-	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //17
+	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //17
-	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //18
+	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //18
-	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //19
+	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //19
-	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //20
+	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //20
+	"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //21
 	};
 
 	SVzNLRange fileIdx[TEST_GROUP] = { 
 		{0,176},{1,200},{1,166},{122,141},{1,65},
 		{1,29},{108,135},{0,200}, {1,200}, {1,12},
-		{2,4}, {1,5}, {1,1}, {1,21},{1,28},
+		{2,4}, {1,5}, {1,1}, {1,1},
-		{3,3}, {1,51}, {4,83}, {1,74}, {1,61},
+		{1,21},{1,28},
-		{1,84}
+		{3,3}, {1,51}, {4,83}, {1,74}, {1,61},	{1,84}
 	};
 
 	SSG_planeCalibPara poseCalibPara;
@@ -2604,7 +2605,7 @@ int main()
 
 	SG_bagPositionParam algoParam;
 	int endGroup =  TEST_GROUP - 1;
-	for (int grp = 14; grp <= 14; grp++)
+	for (int grp = 15; grp <= 15; grp++)
 	{
 		if (grp < 10)
 		{
@@ -2630,7 +2631,19 @@ int main()
 			algoParam.growParam.minLTypeTreeLen = 50.0; //mm
 			algoParam.growParam.minVTypeTreeLen = 50.0; //mm
 		}
-		else if ( (grp >= 13) && (grp <= 14))
+		else if (grp ==13)
+		{
+			algoParam.bagParam.bagL = 550;  //袋子长65cm
+			algoParam.bagParam.bagW = 400;  //袋子宽40cm
+			algoParam.bagParam.bagH = 100;  //袋子高16cm
+			algoParam.growParam.maxLineSkipNum = 5;
+			algoParam.growParam.yDeviation_max = 20.0;
+			algoParam.growParam.maxSkipDistance = 20.0;
+			algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2
+			algoParam.growParam.minLTypeTreeLen = 50.0; //mm
+			algoParam.growParam.minVTypeTreeLen = 50.0; //mm
+		}
+		else if ( (grp >= 14) && (grp <= 15))
 		{
 			algoParam.bagParam.bagL = 750;  //袋子长65cm
 			algoParam.bagParam.bagW = 450;  //袋子宽40cm
@@ -2656,8 +2669,9 @@ int main()
 		}
 #if BAG_ALGO_USE_CORNER_FEATURE
 		algoParam.cornerParam.cornerTh = 30;  //45度角
-		algoParam.cornerParam.scale = algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
+		algoParam.cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
-		algoParam.cornerParam.minEndingGap = algoParam.bagParam.bagW / 4;
+		algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4;
+		algoParam.cornerParam.minEndingGap_z = algoParam.bagParam.bagH / 4;
 		algoParam.cornerParam.jumpCornerTh_1 = 60;
 		algoParam.cornerParam.jumpCornerTh_2 = 15;
 #else
@@ -2694,12 +2708,56 @@ int main()
 			poseCalibPara = _readCalibPara(calibFile);
 		}
 		else if (grp == 13)
+		{
+			char calibFile[250];
+			sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\山东现场\\ground_calib_para.txt");
+			poseCalibPara = _readCalibPara(calibFile);
+
+			algoParam.bagParam.bagL = 550;  //袋子长65cm
+			algoParam.bagParam.bagW = 400;  //袋子宽40cm
+			algoParam.bagParam.bagH = 100;  //袋子高16cm
+			algoParam.growParam.maxLineSkipNum = 5;
+			algoParam.growParam.yDeviation_max = 20.0;
+			algoParam.growParam.maxSkipDistance = 20.0;
+			algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2
+			algoParam.growParam.minLTypeTreeLen = 50.0; //mm
+			algoParam.growParam.minVTypeTreeLen = 50.0; //mm
+
+			algoParam.cornerParam.cornerTh = 30;  //45度角
+			algoParam.cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
+			algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4;
+			algoParam.cornerParam.minEndingGap_z = algoParam.bagParam.bagH / 4;
+			algoParam.cornerParam.jumpCornerTh_1 = 60;
+			algoParam.cornerParam.jumpCornerTh_2 = 15;
+
+			poseCalibPara.planeCalib[0] = 0.999975;
+			poseCalibPara.planeCalib[1] = -8.27654e-05;
+			poseCalibPara.planeCalib[2] = 0.00703687;
+			poseCalibPara.planeCalib[3] = -8.27654e-05;
+			poseCalibPara.planeCalib[4] = 0.999723;
+			poseCalibPara.planeCalib[5] = 0.0235198;
+			poseCalibPara.planeCalib[6] = -0.00703687;
+			poseCalibPara.planeCalib[7] = -0.0235198;
+			poseCalibPara.planeCalib[8] = 0.999699;
+			poseCalibPara.planeHeight = 3010.61;
+			poseCalibPara.invRMatrix[0] = 0.999975;
+			poseCalibPara.invRMatrix[1] = -8.27654e-05;
+			poseCalibPara.invRMatrix[2] = -0.00703687;
+			poseCalibPara.invRMatrix[3] = -8.27654e-05;
+			poseCalibPara.invRMatrix[4] = 0.999723;
+			poseCalibPara.invRMatrix[5] = -0.0235198;
+			poseCalibPara.invRMatrix[6] = 0.00703687;
+			poseCalibPara.invRMatrix[7] = 0.0235198;
+			poseCalibPara.invRMatrix[8] = 0.999699;
+			
+		}
+		else if ( grp == 14) 
 		{
 			char calibFile[250];
 			sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt");
 			poseCalibPara = _readCalibPara(calibFile);
 		}
-		else if (grp == 14)
+		else if (grp == 15)
 		{
 			char calibFile[250];
 			sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\ground_calib_para.txt");
@@ -2707,7 +2765,7 @@ int main()
 		}
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx = 10;
+			//fidx = 22;
 			if (grp < TEST_TOP_VIEW_GROUP)  //正面抓取
 			{
 				int lineNum = 0;
@@ -2747,6 +2805,7 @@ int main()
 					//调平，去除地面
 					sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight);
 				}
+
 				std::vector<SSG_peakRgnInfo> objOps;
 				sg_getBagPosition(laser3DPoints, lineNum, algoParam, poseCalibPara, objOps);
 #endif
@@ -2832,6 +2891,13 @@ int main()
 					//调平，去除地面
 					sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight);
 				}
+#if 0
+				char _out_file[256];
+				sprintf_s(_out_file, "%sresult\\LaserLine%d_ground_calib.txt", dataPath[grp], fidx);
+				std::vector<SSG_peakOrienRgnInfo> nullObjs;
+				_outputRGBDScanDataFile_RGBD_obj(_out_file, laser3DPoints, lineNum,
+					lineV, maxTimeStamp, clockPerSecond, nullObjs);
+#endif
 #if 0
 				//产生一个调平后的2D图像，核对颜色 
 				char _tst_file[256];

sourceCode/SG_bagPositioning.cpp

@@ -2383,10 +2383,16 @@ void sg_getBagPosition(
 	//逐行提取特征
 	for (int hLine = 0; hLine < hLineNum; hLine++)
 	{
-if (hLine == 116)
+if (hLine == 14)
 	int kkk = 1;
 		std::vector<SVzNL3DPosition> smoothData;
-		sg_lineDataSmoothing(hLines[hLine], 5, smoothData);
+		sg_lineSegSmoothing(
+			hLines[hLine],
+			algoParam.cornerParam.minEndingGap, //分段的Y间隔。大于此间隔，为新的分段
+			algoParam.cornerParam.minEndingGap_z,//分段的Z间隔。大于此间隔，为新的分段
+			5,
+			smoothData);
+		//sg_lineDataSmoothing(hLines[hLine], 5, smoothData);
 		SSG_lineFeature a_hLine_featrues;
 		a_hLine_featrues.lineIdx = hLine;
 #if BAG_ALGO_USE_CORNER_FEATURE
@@ -3214,6 +3220,7 @@ if (col == 586)
 	sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
 	//获取Peaks
 	int invlidDistToEdge = 50; //距离边缘近的Peak是非法点。
+	double minPeakValue = algoParam.bagParam.bagW / 8; 
 	std::vector<SSG_2DValueI> peaks;
 	for (int i = 0; i < dt_peaks.size(); i++)
 	{
@@ -3223,7 +3230,8 @@ if (col == 586)
 		int y_diff_0 = dt_peaks[i].y;//距上边距离
 		int y_diff_1 = distTransform.rows - dt_peaks[i].y;//距下边距离
 		if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) ||
-			(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge))
+			(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) ||
+			(dt_peaks[i].valueD < minPeakValue))
 			continue;
 
 		//在distTranformIndexing中回找坐标
@@ -3251,7 +3259,7 @@ if (col == 586)
 	int peakRgnId = 1;
 	for (int i = 0, i_max = peaks.size(); i < i_max; i++)
 	{
-if (i == 2)
+if (i == 3)
 	int kkk = 1;
 
 		SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);

sourceCode/SG_baseAlgo_Export.h

@@ -44,6 +44,13 @@ SG_APISHARED_EXPORT void sg_lineDataSmoothing(
     std::vector<SVzNL3DPosition>& input, 
     int smoothWin, 
     std::vector<SVzNL3DPosition>& output);
+//分段平滑，这样不会影响分段端点
+SG_APISHARED_EXPORT void sg_lineSegSmoothing(
+    std::vector<SVzNL3DPosition>& input,
+    double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
+    double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
+    int smoothWin,
+    std::vector<SVzNL3DPosition>& output);
 
 //VZ_APISHARED_EXPORT void sg_getLineMeanVar();
 SG_APISHARED_EXPORT void sg_getLineLVFeature(

sourceCode/SG_baseDataType.h

@@ -148,7 +148,8 @@ typedef struct
 
 typedef struct
 {
-	double minEndingGap;  //连续段门限。大于此门限，为不连续
+	double minEndingGap;  //y方向连续段门限。大于此门限，为不连续
+	double minEndingGap_z;  //z方向连续段门限。大于此门限，为不连续
 	double scale;  //计算方向角的窗口比例尺
 	double cornerTh; //拐角门限，大于此门限，为有效拐点
 	double jumpCornerTh_1; //判断拐角是否为跳变的两个门限。当一个门限大于jumpCornerTh_1且另一个小于jumpCornerTh_2时跳变

sourceCode/SG_lineFeature.cpp

@@ -62,7 +62,10 @@ SSG_meanVar _computeMeanVar(double* data, int size)
 	return a_meanVar;
 }
 
-void sg_lineDataSmoothing(std::vector<SVzNL3DPosition>& input, int smoothWin, std::vector<SVzNL3DPosition>& output)
+void sg_lineDataSmoothing(
+	std::vector<SVzNL3DPosition>& input,
+	int smoothWin, 
+	std::vector<SVzNL3DPosition>& output)
 {
 	output.resize(input.size()); // 预分配足够的空间
 	std::copy(input.begin(), input.end(), output.begin());
@@ -91,6 +94,65 @@ void sg_lineDataSmoothing(std::vector<SVzNL3DPosition>& input, int smoothWin, st
 	return;
 }
 
+//对扫描线按分段进行平滑。
+void sg_lineSegSmoothing(
+	std::vector<SVzNL3DPosition>& input, 
+	double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
+	double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
+	int smoothWin, 
+	std::vector<SVzNL3DPosition>& output)
+{
+	//output.resize(input.size()); // 预分配足够的空间
+	//std::copy(input.begin(), input.end(), output.begin());
+
+	int dataSize = input.size();
+	//计算分段
+	std::vector<std::vector<SVzNL3DPosition>> segs;
+	std::vector<SVzNL3DPosition> a_seg;
+	int segStart = -1;
+	for (int i = 0; i < dataSize; i++)
+	{
+		SVzNL3DPosition a_pt = input[i];
+		//seg判断
+		if (a_pt.pt3D.z > 1e-4)
+		{
+			if (segStart < 0)
+			{
+				a_seg.push_back(a_pt);
+				segStart = 1;
+			}
+			else  //检查两点距离
+			{
+				SVzNL3DPosition pre_pt = a_seg.back();
+				double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
+				double diff_y = abs(a_pt.pt3D.y - pre_pt.pt3D.y);
+				if ((diff_y > seg_y_deltaTh) || (diff_z > seg_z_deltaTh))
+				{
+					segs.push_back(a_seg);
+					a_seg.clear();
+					a_seg.push_back(a_pt);
+				}
+				else
+					a_seg.push_back(a_pt);
+			}
+		}
+		else
+			a_seg.push_back(a_pt);
+	}
+	//last
+	if (a_seg.size() > 0)
+		segs.push_back(a_seg);
+
+	//分段平滑
+	for (int i = 0, i_max = segs.size(); i < i_max; i++)
+	{
+		std::vector<SVzNL3DPosition> seg_output;
+		sg_lineDataSmoothing(segs[i],  smoothWin, seg_output);
+		output.insert(output.end(), seg_output.begin(), seg_output.end());
+	}
+	return;
+}
+
 //滤除离群点：z跳变门限方法
 void sg_lineDataRemoveOutlier(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts)
 {
@@ -1094,21 +1156,40 @@ void sg_getLineCornerFeature(
 	line_features->lineIdx = lineIdx;
 	if (lineIdx == 538)
 		int kkk = 1;
-
 	//去除零点
 	std::vector< SVzNL3DPosition> vldPts;
 	std::vector<SSG_RUN> segs;
+	//修改seg的定义。seg端点是两点间距离大于门限的点
 	int segStart = -1, segEnd = -1;
 	for (int i = 0; i < dataSize; i++)
 	{
+		if ( (lineIdx == 399)&&(i==568))
+			int kkk = 1;
 		SVzNL3DPosition a_pt = lineData[i];
 		a_pt.nPointIdx = i;
+		if (lineData[i].pt3D.z > 1e-4)
+			vldPts.push_back(a_pt);
+
+		//seg判断
 		if (lineData[i].pt3D.z > 1e-4)
 		{
 			if (segStart < 0)
 				segStart = i;
+			else  //检查两点距离
+			{
+				SVzNL3DPosition pre_pt = lineData[i-1];
+				double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
+				if (diff_z > cornerPara.minEndingGap_z)
+				{
+					SSG_RUN a_run;
+					a_run.start = segStart;
+					a_run.len = segEnd - segStart + 1;
+					a_run.value = 1;
+					segs.push_back(a_run);
+					segStart = i;
+				}
+			}
 			segEnd = i;
-			vldPts.push_back(a_pt);
 		}
 		else
 		{
@@ -1139,6 +1220,8 @@ void sg_getLineCornerFeature(
 	corners.resize(vldPts.size());
 	for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
 	{
+		if ((lineIdx == 399) && (i == 419))
+			int kkk = 1;
 		//前向寻找
 		int pre_i = -1;
 		for (int j = i - 1; j >= 0; j--)
@@ -1332,7 +1415,8 @@ void sg_getLineCornerFeature(
 		int idx_1 = curr_seg->start + curr_seg->len - 1;
 		int idx_2 = nxt_seg->start;
 		double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y);
-		if (y_diff < cornerPara.minEndingGap)  //ºÏ²¢
+		double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z);
+		if ( (y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //合并
 		{
 			int idx_end = nxt_seg->start + nxt_seg->len - 1;
 			nxt_seg->start = curr_seg->start;