螺杆&工具盘检测算法更新

App/ScrewPosition/ScrewPositionApp/Version.h

@@ -3,7 +3,7 @@
 
 
 #define SCREWPOSITION_APP_NAME "螺杆定位"
-#define SCREWPOSITION_VERSION_STRING "1.2.1"
+#define SCREWPOSITION_VERSION_STRING "1.2.2"
 #define SCREWPOSITION_BUILD_STRING "1"
 #define SCREWPOSITION_FULL_VERSION_STRING "V" SCREWPOSITION_VERSION_STRING "_" SCREWPOSITION_BUILD_STRING
 

App/ScrewPosition/ScrewPositionApp/Version.md

@@ -1,3 +1,9 @@
+# 1.2.1 2026-06-04
+1. 更新算法
+
+# 1.2.1 2026-05-24
+1. 参数分开配置
+
 # 1.2.0 2026-05-24
 ## build_1
 1. 更新了算法

App/ScrewPosition/ScrewPositionApp/dialogdetectionconfig.cpp

@@ -42,6 +42,7 @@ DialogDetectionConfig::DialogDetectionConfig(QWidget *parent)
 
     initNumericEditors();
     initHandEyeCalibTab();
+    initNetworkConfigControls();
 
     // 检测对象 ComboBox 选项（带枚举值作 userData）
     ui->comboType->blockSignals(true);
@@ -71,6 +72,7 @@ void DialogDetectionConfig::SetPresenter(ScrewPositionPresenter* presenter)
 {
     m_presenter = presenter;
     loadFromConfig();
+    loadNetworkConfig();
 }
 
 void DialogDetectionConfig::initNumericEditors()
@@ -115,6 +117,44 @@ void DialogDetectionConfig::initHandEyeCalibTab()
     ui->verticalLayout_handEyeCalibHost->addWidget(m_handEyeCalibWidget);
 }
 
+void DialogDetectionConfig::initNetworkConfigControls()
+{
+    ui->comboPoseOutputOrder->clear();
+    ui->comboPoseOutputOrder->addItem(QStringLiteral("RX-RY-RZ"), 0);
+    ui->comboPoseOutputOrder->addItem(QStringLiteral("RX-RZ-RY"), 1);
+    ui->comboPoseOutputOrder->addItem(QStringLiteral("RY-RX-RZ"), 2);
+    ui->comboPoseOutputOrder->addItem(QStringLiteral("RY-RZ-RX"), 3);
+    ui->comboPoseOutputOrder->addItem(QStringLiteral("RZ-RX-RY"), 4);
+    ui->comboPoseOutputOrder->addItem(QStringLiteral("RZ-RY-RX"), 5);
+
+    ui->comboByteOrder->clear();
+    ui->comboByteOrder->addItem(QStringLiteral("大端序 (ABCD)"), 0);
+    ui->comboByteOrder->addItem(QStringLiteral("小端序 (DCBA)"), 1);
+
+    ui->editTcpPort->setValidator(new QIntValidator(1, 65535, ui->editTcpPort));
+}
+
+void DialogDetectionConfig::loadNetworkConfig()
+{
+    if (!m_presenter || !m_presenter->GetConfigManager()) {
+        return;
+    }
+
+    const ConfigResult configResult = m_presenter->GetConfigManager()->GetConfigResult();
+
+    int idx = ui->comboPoseOutputOrder->findData(configResult.poseOutputOrder);
+    if (idx >= 0) {
+        ui->comboPoseOutputOrder->setCurrentIndex(idx);
+    }
+
+    idx = ui->comboByteOrder->findData(configResult.byteOrder);
+    if (idx >= 0) {
+        ui->comboByteOrder->setCurrentIndex(idx);
+    }
+
+    ui->editTcpPort->setText(QString::number(configResult.tcpPort));
+}
+
 void DialogDetectionConfig::rebuildCameraSelector()
 {
     if (!ui->comboCamera) return;
@@ -325,6 +365,20 @@ bool DialogDetectionConfig::saveAllWorkingSetsToConfig()
         systemConfig.configResult.detectionConfigList.push_back(item);
     }
 
+    bool ok = false;
+    const int tcpPort = ui->editTcpPort->text().trimmed().toInt(&ok);
+    if (!ok || tcpPort <= 0 || tcpPort > 65535) {
+        StyledMessageBox::warning(this, QStringLiteral("错误"),
+                                  QStringLiteral("TCP端口必须在 1 到 65535 之间。"));
+        ui->editTcpPort->setFocus();
+        ui->editTcpPort->selectAll();
+        return false;
+    }
+
+    systemConfig.configResult.tcpPort = static_cast<uint16_t>(tcpPort);
+    systemConfig.configResult.poseOutputOrder = ui->comboPoseOutputOrder->currentData().toInt();
+    systemConfig.configResult.byteOrder = ui->comboByteOrder->currentData().toInt();
+
     if (!m_presenter->GetConfigManager()->UpdateFullConfig(systemConfig)) {
         StyledMessageBox::warning(this, QStringLiteral("失败"),
                                   QStringLiteral("更新配置缓存失败。"));

App/ScrewPosition/ScrewPositionApp/dialogdetectionconfig.h

@@ -51,6 +51,8 @@ private:
 
     void initNumericEditors();
     void initHandEyeCalibTab();
+    void initNetworkConfigControls();
+    void loadNetworkConfig();
     void rebuildCameraSelector();
     void loadFromConfig();
     bool commitCurrentEditorsToWorkingSet(QString* errMsg = nullptr);

App/ScrewPosition/ScrewPositionApp/dialogdetectionconfig.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>900</width>
-    <height>800</height>
+    <height>970</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -814,11 +814,139 @@ QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px;
     </widget>
    </widget>
   </widget>
+  <widget class="QGroupBox" name="groupBox_networkConfig">
+   <property name="geometry">
+    <rect>
+     <x>40</x>
+     <y>735</y>
+     <width>821</width>
+     <height>170</height>
+    </rect>
+   </property>
+   <property name="font">
+    <font>
+     <pointsize>14</pointsize>
+    </font>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">QGroupBox { color: rgb(221, 225, 233); border: 1px solid rgb(100, 100, 100); border-radius: 4px; margin-top: 12px; padding-top: 8px; }
+QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; }
+QLabel { color: rgb(221, 225, 233); }
+QLineEdit { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); border: 1px solid rgb(70, 72, 78); padding: 4px; }
+QComboBox { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); border: 1px solid rgb(70, 72, 78); padding: 4px; }
+QComboBox QAbstractItemView { color: rgb(221, 225, 233); background-color: rgb(47, 48, 52); selection-background-color: rgb(70, 100, 150); }</string>
+   </property>
+   <property name="title">
+    <string>通信配置</string>
+   </property>
+   <widget class="QLabel" name="labelPoseOutputOrder">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>35</y>
+      <width>220</width>
+      <height>30</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>姿态输入/输出顺序:</string>
+    </property>
+   </widget>
+   <widget class="QComboBox" name="comboPoseOutputOrder">
+    <property name="geometry">
+     <rect>
+      <x>280</x>
+      <y>35</y>
+      <width>220</width>
+      <height>32</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+   </widget>
+   <widget class="QLabel" name="labelByteOrder">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>75</y>
+      <width>220</width>
+      <height>30</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>数据字节序:</string>
+    </property>
+   </widget>
+   <widget class="QComboBox" name="comboByteOrder">
+    <property name="geometry">
+     <rect>
+      <x>280</x>
+      <y>75</y>
+      <width>220</width>
+      <height>32</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+   </widget>
+   <widget class="QLabel" name="labelTcpPort">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>115</y>
+      <width>220</width>
+      <height>30</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>TCP端口:</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="editTcpPort">
+    <property name="geometry">
+     <rect>
+      <x>280</x>
+      <y>115</y>
+      <width>220</width>
+      <height>32</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>7800</string>
+    </property>
+   </widget>
+  </widget>
   <widget class="QPushButton" name="btnReset">
    <property name="geometry">
     <rect>
      <x>40</x>
-     <y>745</y>
+     <y>915</y>
      <width>140</width>
      <height>45</height>
     </rect>
@@ -852,7 +980,7 @@ QPushButton:pressed {
    <property name="geometry">
     <rect>
      <x>520</x>
-     <y>745</y>
+     <y>915</y>
      <width>100</width>
      <height>45</height>
     </rect>
@@ -886,7 +1014,7 @@ QPushButton:pressed {
    <property name="geometry">
     <rect>
      <x>640</x>
-     <y>745</y>
+     <y>915</y>
      <width>100</width>
      <height>45</height>
     </rect>
@@ -920,7 +1048,7 @@ QPushButton:pressed {
    <property name="geometry">
     <rect>
      <x>760</x>
-     <y>745</y>
+     <y>915</y>
      <width>100</width>
      <height>45</height>
     </rect>

App/ScrewPosition/ScrewPositionConfig/Src/AlgorithmParamConverter.cpp

@@ -20,7 +20,7 @@ ScrewDetectAlgorithmParams AlgorithmParamConverter::ToScrewDetectAlgorithmParams
     params.cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
     params.cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
     // Keep the existing runtime behavior: the algorithm scale is derived from rod diameter.
-    params.cornerParam.scale = params.rodDiameter / 4.0;
+    params.cornerParam.scale = algorithmParams.cornerParam.scale; // params.rodDiameter / 4.0;
     params.cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
     params.cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
     params.cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;

AppAlgo/rodAndBarDetection/Inc/SG_baseDataType.h

@@ -291,6 +291,7 @@ typedef struct
 	SVzNL3DPoint endPt;
 	SVzNL3DPoint peakPt;
 	double featureValue;
+	int flag;
 }SWD_rodArcFeature;
 
 typedef struct
@@ -441,6 +442,7 @@ typedef struct
 	int start;
 	int len;
 	int value;
+	double curveLen;
 }SSG_RUN;
 
 typedef struct

AppAlgo/rodAndBarDetection/rodAndBarDetection_test.cpp

@@ -12,6 +12,7 @@
 #include <opencv2/opencv.hpp>
 #include <Windows.h>
 #include <limits>
+#include <SG_baseAlgo_Export.h>
 
 #include <cmath>
 typedef struct
@@ -32,6 +33,14 @@ typedef struct
 	float b;
 } SPointXYZRGB;
 
+bool fileExists(const char* path) {
+	FILE* f = fopen(path, "r");
+	if (f != NULL) {
+		fclose(f);
+		return true;
+	}
+	return false;
+}
 
 // 点乘 dot
 double _dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
@@ -1133,25 +1142,26 @@ void locatingPlateTest(void)
 	}
 }
 
-#define NEW_LOCATING_PALTE_TEST_GROUP 1
+#define NEW_LOCATING_PALTE_TEST_GROUP 2
 void newLocatingPlateTest(void)
 {
 	const char* dataPath[NEW_LOCATING_PALTE_TEST_GROUP] = {
-		"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260621/",  //0
+		"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260521/",  //0
+		"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260603/",  //1
 	};
 
 	SVzNLRange fileIdx[NEW_LOCATING_PALTE_TEST_GROUP] = {
-		{12,13},
+		{12,13}, {1,51}
 	};
 
 	const char* ver = wd_rodAndBarDetectionVersion();
 	printf("ver:%s\n", ver);
 
-	for (int grp = 0; grp < NEW_LOCATING_PALTE_TEST_GROUP; grp++)
+	for (int grp = 1; grp < NEW_LOCATING_PALTE_TEST_GROUP; grp++)
 	{
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx =2;
+			//fidx =16;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
 
@@ -1512,23 +1522,39 @@ void rodPositionTest(void)
 #endif
 }
 
+int counterValidPts(std::vector<SVzNL3DPosition>& lineData)
+{
+	int num = 0;
+	for (int i = 0; i < (int)lineData.size(); i++)
+	{
+		if (lineData[i].pt3D.z > 1e-4)
+			num++;
+		else
+		{
+			lineData[i].pt3D = { 0, 0, 0 };
+		}
+	}
+	return num;
+}
 
-#define WELD_SEAM_TEST_GROUP 2
+#define WELD_SEAM_TEST_GROUP 4
 void rodWeldSeamPosition_test(void)
 {
 	const char* dataPath[WELD_SEAM_TEST_GROUP] = {
 		"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片1/",  //0
-		"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片2/",  //0
+		"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片2/",  //1
+		"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/数据3/特征点1/", //2
+		"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/数据3/特征点2/", //3
 	};
 
 	SVzNLRange fileIdx[WELD_SEAM_TEST_GROUP] = {
-		{1,20},{1,20},
+		{1,20},{1,20},{1,27},  { 1,21 },
 	};
 
 	const char* ver = wd_rodAndBarDetectionVersion();
 	printf("ver:%s\n", ver);
 
-	for (int grp = 0; grp < WELD_SEAM_TEST_GROUP; grp++)
+	for (int grp = 2; grp < WELD_SEAM_TEST_GROUP; grp++)
 	{
 		SSG_planeCalibPara poseCalibPara;
 		//初始化成单位阵
@@ -1552,15 +1578,95 @@ void rodWeldSeamPosition_test(void)
 		{
 			//fidx =1;
 			char _scan_file[256];
-			sprintf_s(_scan_file, "%s%d_LaserData_ID019567.txt", dataPath[grp], fidx);
+			sprintf_s(_scan_file, "%s%d_LaserData_groundAdjusted.txt", dataPath[grp], fidx);
 
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
 			wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
 
-			//转成plyTxt格式
+			if ((grp == 2) || (grp == 3)) //计算调平矩阵
-			//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
+			{
-			//wdSavePlyTxt(_scan_file, scanLines);
+#if 0
+				std::vector<cv::Point3f> validPts;
+				std::vector<std::vector< SVzNL3DPosition>> copy_scanLines;
+				copy_scanLines.resize(scanLines.size());
+				for (int line = 0; line < (int)scanLines.size(); line++)
+				{
+					std::vector< SVzNL3DPosition>& a_line = scanLines[line];
+					copy_scanLines[line].resize(a_line.size());
+					for (int j = 0; j < (int)a_line.size(); j++)
+					{
+						copy_scanLines[line][j] = a_line[j];
+						if (a_line[j].pt3D.z > 1e-4)
+						{
+							cv::Point3f a_pt = cv::Point3f((float)a_line[j].pt3D.x, (float)a_line[j].pt3D.y, (float)a_line[j].pt3D.z);
+							validPts.push_back(a_pt);
+						}
+					}
+				}
+				std::vector<cv::Point3f> out_inliers;
+				Plane res = ransacFitPlane(validPts, out_inliers);
+				if (res.C < 0)
+				{
+					res.A = -res.A;
+					res.B = -res.B;
+					res.C = -res.C;
+					res.D = -res.D;
+				}
+				float meanH = 0;
+				int counter = 0;
+				for (int i = 0; i < (int)out_inliers.size(); i++)
+				{
+					meanH += out_inliers[i].z;
+					counter++;
+				}
+				//计算投影向量
+				SVzNL3DPoint vec_1 = { res.A, res.B, res.C };
+				SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
+				SSG_planeCalibPara calibPara = wd_computeRTMatrix(vec_1, vec_2);
+				for (int m = 0; m < 9; m++)
+				{
+					poseCalibPara.planeCalib[m] = calibPara.planeCalib[m];
+					poseCalibPara.invRMatrix[m] = calibPara.invRMatrix[m];
+				}
+				poseCalibPara.planeHeight = meanH / counter;
 
+				char _out_file[256];
+				sprintf_s(_out_file, "%s%d_calib_para.txt", dataPath[grp], fidx);
+				_outputCalibPara(_out_file, poseCalibPara);
+
+				sprintf_s(_out_file, "%s%d_LaserData_groundAdjusted.txt", dataPath[grp], fidx);
+				//if (false == fileExists(_out_file))
+				{
+					//输出调平效果
+					int firstValidLine = -1;
+					int lastValidLine = -1;
+					for (int line = 0; line < (int)copy_scanLines.size(); line++)
+					{
+						//行处理
+						//调平，去除地面
+						double cuttingZ = poseCalibPara.planeHeight - 100;
+						sx_rodPosition_lineDataR(copy_scanLines[line], poseCalibPara.planeCalib, cuttingZ);
+						int ptsNum = counterValidPts(copy_scanLines[line]);
+						if (ptsNum > 10)
+						{
+							if (firstValidLine < 0)
+								firstValidLine = line;
+							lastValidLine = line;
+						}
+					}
+					if ((lastValidLine > 0) && (lastValidLine < copy_scanLines.size() - 1))
+					{
+						copy_scanLines.erase(copy_scanLines.begin() + lastValidLine + 1, copy_scanLines.end());
+					}
+					if(firstValidLine > 1)
+					{
+						copy_scanLines.erase(copy_scanLines.begin(), copy_scanLines.begin() + firstValidLine - 1);
+					}
+					_outputScanDataFile(_out_file, copy_scanLines, 0.0f, 0, 0);
+				}
+#endif
+			}
+			
 			long t1 = (long)GetTickCount64();//统计时间
 
 			SSX_rodParam rodParam;
@@ -1583,7 +1689,7 @@ void rodWeldSeamPosition_test(void)
 			growParam.maxLineSkipNum = 5;
 			growParam.yDeviation_max = 5.0;
 			growParam.maxSkipDistance = 20.0;
-			growParam.zDeviation_max = 3.0;// 
+			growParam.zDeviation_max = 5.0;// 
 			growParam.minLTypeTreeLen = 50; //mm, 螺杆长度
 			growParam.minVTypeTreeLen = 50; //mm
 
@@ -1603,11 +1709,14 @@ void rodWeldSeamPosition_test(void)
 				&errCode);
 			long t2 = (long)GetTickCount64();
 			printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
-			//输出测试结果
+			if (errCode == 0)
-			sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
+			{
-			_outputRGBDScan_RGBD_weldSeam(_scan_file, scanLines, weldSeamInfo);
+				//输出测试结果
-			sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
+				sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
-			_outputWeldSeamInfo(_scan_file, weldSeamInfo);
+				_outputRGBDScan_RGBD_weldSeam(_scan_file, scanLines, weldSeamInfo);
+				sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
+				_outputWeldSeamInfo(_scan_file, weldSeamInfo);
+			}
 		}
 	}
 }
@@ -1626,8 +1735,8 @@ int main()
 {
 	//ESG_testMode testMode = keSG_测试_配天螺杆定位;
 	//ESG_testMode testMode = keSG_测试_配天定位盘定位;
-	//ESG_testMode testMode = keSG_测试_配天新定位盘定位;
+	ESG_testMode testMode = keSG_测试_配天新定位盘定位;
-	ESG_testMode testMode = keSG_测试_棒材抓取;
+	//ESG_testMode testMode = keSG_测试_棒材抓取;
 	//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
 
 	if(keSG_测试_配天螺杆定位 == testMode)