工件定位算法更新1.3.0

App/Workpiece/WorkpieceApp/Presenter/Src/DetectPresenter.cpp

@@ -139,9 +139,6 @@ int DetectPresenter::DetectWorkpiece(
     if(cameraCalibParam){
         LOG_DEBUG("Processing data with plane calibration\n");
         double groundH = -1;
-#ifdef _WIN32
-        groundH = 1780;
-#endif
         for(size_t i = 0; i < xyzData.size(); i++){
             sx_BQ_lineDataR(xyzData[i], cameraCalibParam->planeCalib, groundH);
         }

App/Workpiece/WorkpieceApp/Presenter/Src/WorkpiecePresenter.cpp

@@ -473,6 +473,19 @@ bool WorkpiecePresenter::CalculatePlaneCalibration(
         // 调用工件项目的调平算法
         SSG_planeCalibPara calibResult = sx_BQ_getBaseCalibPara(xyzData);
 
+        // 打印算法返回结果
+        LOG_INFO("=== sx_BQ_getBaseCalibPara 算法结果 ===\n");
+        LOG_INFO("planeHeight: %.6f\n", calibResult.planeHeight);
+        LOG_INFO("planeCalib (调平旋转矩阵):\n");
+        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[0], calibResult.planeCalib[1], calibResult.planeCalib[2]);
+        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[3], calibResult.planeCalib[4], calibResult.planeCalib[5]);
+        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[6], calibResult.planeCalib[7], calibResult.planeCalib[8]);
+        LOG_INFO("invRMatrix (逆旋转矩阵):\n");
+        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[0], calibResult.invRMatrix[1], calibResult.invRMatrix[2]);
+        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[3], calibResult.invRMatrix[4], calibResult.invRMatrix[5]);
+        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[6], calibResult.invRMatrix[7], calibResult.invRMatrix[8]);
+        LOG_INFO("========================================\n");
+
         // 将结构体中的数据复制到输出参数
         for (int i = 0; i < 9; i++) {
             planeCalib[i] = calibResult.planeCalib[i];

App/Workpiece/WorkpieceApp/Version.h

@@ -3,8 +3,8 @@
 
 
 #define WORKPIECE_VERSION_STRING "1.0.2"
-#define WORKPIECE_BUILD_STRING "3"
+#define WORKPIECE_BUILD_STRING "4"
-#define WORKPIECE_FULL_VERSION_STRING "V1.0.2.3"
+#define WORKPIECE_FULL_VERSION_STRING "V" WORKPIECE_VERSION_STRING "_" WORKPIECE_BUILD_STRING
 
 // 获取版本信息的便捷函数
 inline const char* GetWorkpieceVersion() {

App/Workpiece/WorkpieceApp/Version.md

@@ -1,7 +1,9 @@
 # 1.0.2 
+## build_4 2026-01-09
+1. 更新算法 1.3.0
+
 ## build_3 2025-12-17
 1. 测试不同的工件
-2. 
 
 ## build_2 2025-12-14
 1. 完善协议

AppAlgo/workpieceCornerExtraction/BQ_workpieceCornerExtract_test.cpp

@@ -649,21 +649,22 @@ void _outputRGBDScanLapWeld_RGBD(
 #define CONVERT_TO_GRID				0
 #define TEST_COMPUTE_CALIB_PARA		0
 #define TEST_COMPUTE_CORNER			1
-#define TEST_GROUP 7
+#define TEST_GROUP 8
 int main()
 {
 	const char* dataPath[TEST_GROUP] = {
-		"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\",  //0
+		"F:/ShangGu/项目/冠钦_博清科技/数据/",  //0
-		"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\",  //1
+		"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度1/",  //1
-		"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\",  //2
+		"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度2/",  //2
-		"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\",  //3
+		"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度3/",  //3
-		"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\",  //4
+		"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度4/",  //4
-		"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\",  //5
+		"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/",  //5
-		"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\", //6
+		"F:/ShangGu/项目/冠钦_博清科技/数据/顶板样式/", //6
+		"F:/ShangGu/项目/冠钦_博清科技/定位纠偏/现场数据/0107/", //7
 	};
 
 	SVzNLRange fileIdx[TEST_GROUP] = {
-		{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}
+		{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}, {1,4}
 	};
 
 #if CONVERT_TO_GRID
@@ -690,7 +691,7 @@ int main()
 
 #if TEST_COMPUTE_CALIB_PARA
 	char _calib_datafile[256];
-	sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\节点4.txt");
+	sprintf_s(_calib_datafile, "%sLaserline_1.txt", dataPath[7]);
 	int lineNum = 0;
 	float lineV = 0.0f;
 	int dataCalib = 0;
@@ -726,10 +727,10 @@ int main()
 		}
 		//
 		char calibFile[250];
-		sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\ground_calib_para.txt");
+		sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[7]);
 		_outputCalibPara(calibFile, calibPara);
 		char _out_file[256];
-		sprintf_s(_out_file, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\scanData_ground_1_calib.txt");
+		sprintf_s(_out_file, "%sscanData_ground_1_calib.txt", dataPath[7]);
 		int headNullLines = 0;
 		_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
 		printf("%s: calib done!\n", _calib_datafile);
@@ -740,7 +741,7 @@ int main()
 	const char* ver = wd_BQWorkpieceCornerVersion();
 	printf("ver:%s\n", ver);
 
-	for (int grp = 5; grp <= 6; grp++)
+	for (int grp = 6; grp <= 7; grp++)
 	{
 		SSG_planeCalibPara poseCalibPara;
 		//初始化成单位阵
@@ -768,6 +769,8 @@ int main()
 				sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
 			else if(6 == grp)
 				sprintf_s(_scan_file, "%s节点%d.txt", dataPath[grp], fidx);
+			else if (7 == grp)
+				sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx);
 			else
 				sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
@@ -820,6 +823,9 @@ int main()
 			growParam.minLTypeTreeLen = 100; //mm
 			growParam.minVTypeTreeLen = 100; //mm
 			SSX_BQworkpiecePara workpieceParam;
+			if(grp == 7)
+				workpieceParam.lineLen = 120.0; //直线段长度
+			else
 				workpieceParam.lineLen = 160.0; //直线段长度
 			int errCode = 0;
 			std::vector<SSX_debugInfo> debug_conturs;

AppAlgo/workpieceCornerExtraction/Inc/SG_baseDataType.h

@@ -48,6 +48,17 @@ typedef struct
 	int idx;
 }SSG_intPair;
 
+typedef struct
+{
+	int featurType;
+	int featureIdx_v;
+	int featureIdx_h;
+	int clusterID;
+	int flag;
+	int lineIdx;
+	int ptIdx;
+}SSG_featureClusteringInfo;
+
 typedef struct
 {
 	double left;
@@ -157,6 +168,13 @@ typedef struct
 	double jumpCornerTh_2;
 }SSG_cornerParam;
 
+typedef struct
+{
+	double segGapTh_y;  //y方向连续段门限。大于此门限，为不连续
+	double segGapTh_z;  //z方向连续段门限。大于此门限，为不连续
+	double maxDist;  //计算方向角的窗口比例尺
+}SSG_lineSegParam;
+
 typedef struct
 {
 	double scale_angle;  //计算方向角的窗口比例尺
@@ -247,6 +265,18 @@ typedef struct
 	int angleChkScalePos; //仅用于加速angleCheck的速度
 }SSG_featureTree;
 
+typedef struct
+{
+	int treeState;
+	int treeType;
+	int sLineIdx;
+	int eLineIdx;
+	double tree_value;
+	SSG_ROIRectD roi;
+	std::vector< SSG_basicFeatureGap> treeNodes;
+	int angleChkScalePos; //仅用于加速angleCheck的速度
+}SSG_gapFeatureTree;
+
 typedef struct
 {
 	int treeState;
@@ -484,3 +514,34 @@ typedef struct
 	SVzNL3DPoint pt1;
 	SVzNL3DPoint pt2;
 }SWD_3DPointPair;
+
+typedef struct
+{
+	int pkId;
+	int lineIdx;
+	int ptIdx;
+	int cptIndex;  //圆周扫描上的点序
+	//double cornerAngle;  //以点为中心的两侧弦的夹角
+	double R;
+	double angle;
+	double x;
+	double y;
+	double z;
+}SWD_polarPt;
+
+typedef struct
+{
+	int cptIndex;
+	int L1_ptIndex;
+	int L2_ptIndex;
+	double cornerAngle;
+	int cornerDir;
+}SWD_polarPeakInfo;
+
+typedef struct
+{
+	int clusterIdx;
+	int ptSize;
+	SVzNL3DRangeD roi3D;
+	SVzNLRect roi2D;
+}SWD_clustersInfo;

AppAlgo/workpieceCornerExtraction/Inc/SG_errCode.h

@@ -16,7 +16,15 @@
 #define SX_ERR_ZERO_CONTOUR_PT		-2005
 #define SX_ERR_INVLID_RPEAK_NUM		-2006
 #define SX_ERR_INVLID_RPEAK_PAIR	-2007
+#define SX_ERR_INVLID_MARK_NUM		-2008
 
 //¶¨×Óץȡ
 #define SX_ERR_INVLID_CUTTING_Z		-2101
 #define SX_ERR_ZERO_OBJ				-2102
+
+//拆包
+#define SX_BAG_TRAY_EMPTY			-2201
+
+//汽车轮眉高度测量
+#define SX_ERR_INVALID_ARC			-2301
+