diff --git a/App/RodAndBarPosition/Doc/MODBUS_PROTOCOL.md b/App/RodAndBarPosition/Doc/MODBUS_PROTOCOL.md index 14741471..207e4aff 100644 --- a/App/RodAndBarPosition/Doc/MODBUS_PROTOCOL.md +++ b/App/RodAndBarPosition/Doc/MODBUS_PROTOCOL.md @@ -46,6 +46,7 @@ | 1 | 成功（地址 2~13 数据有效） | | 2 | 检测失败 | | 3 | 结果为空 / 无更多缓存结果 | +| 4 | 料框为空（算法返回 SX_ERR_ZERO_OBJECTS = -1010，未检出任何棒材） | ## 5. 通信流程 @@ -59,7 +60,7 @@ ### 5.2 读取结果 1. 检测完成后，视觉系统写入： - - 地址 1：状态码（1=成功，2=失败，3=空结果） + - 地址 1：状态码（1=成功，2=失败，3=空结果，4=料框为空） - 地址 2~13：第一个棒材的 X, Y, Z, Roll, Pitch, Yaw（仅状态码为 1 时有效） 2. 外部客户端轮询地址 1，当值从 0 变为非零时表示检测完成 3. 状态码为 1 时，读取地址 2~13 获取定位结果 diff --git a/App/RodAndBarPosition/Doc/modbus_client.mthings b/App/RodAndBarPosition/Doc/modbus_client.mthings deleted file mode 100644 index fae600be..00000000 --- a/App/RodAndBarPosition/Doc/modbus_client.mthings +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp index 764e4890..70d98b9a 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/RodAndBarPositionPresenter.cpp @@ -224,6 +224,27 @@ int RodAndBarPositionPresenter::ProcessAlgoDetection(std::vector()) { + pStatus->OnStatusUpdate("料框为空，未检测到棒材"); + } + if (IsModbusServerRunning()) { + uint16_t statusCode = 4; // 4 = 料框为空（SX_ERR_ZERO_OBJECTS） + WriteModbusRegisters(1, &statusCode, 1); + } + { + std::lock_guard lock(m_resultCacheMutex); + m_lastDetectionResult = DetectionResult{}; + m_resultIndex = 0; + } + SetWorkStatus(WorkStatus::Completed); + SetWorkStatus(WorkStatus::Ready); + return SUCCESS; + } + // 根据项目类型选择处理方式 if (GetStatusCallback()) { QString err = QString("错误:%1").arg(nRet); @@ -680,7 +701,7 @@ void RodAndBarPositionPresenter::_SendDetectionResultToModbus(const DetectionRes ConfigResult configResult = m_pConfigManager->GetConfigResult(); bool bigEndian = (configResult.byteOrder == 0); // 0=大端序, 1=小端序 - // 地址 1：状态码（0=正在检测，1=成功，2=失败，3=结果为空/无更多结果） + // 地址 1：状态码（0=正在检测，1=成功，2=失败，3=结果为空/无更多结果，4=料框为空） uint16_t statusCode = 1; // 默认成功 if (!result.success) { statusCode = 2; // 检测失败 diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Version.h b/App/RodAndBarPosition/RodAndBarPositionApp/Version.h index 7e89dbde..93d7ec59 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Version.h +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Version.h @@ -5,7 +5,7 @@ // 应用名称 #define RODANDBARPOSITION_APP_NAME "棒材定位" -#define RODANDBARPOSITION_VERSION_STRING "1.1.0" +#define RODANDBARPOSITION_VERSION_STRING "1.1.1" #define RODANDBARPOSITION_BUILD_STRING "1" #define RODANDBARPOSITION_FULL_VERSION_STRING "V" RODANDBARPOSITION_VERSION_STRING "_" RODANDBARPOSITION_BUILD_STRING diff --git a/App/RodAndBarPosition/RodAndBarPositionApp/Version.md b/App/RodAndBarPosition/RodAndBarPositionApp/Version.md index 4561ae86..ad0d52f0 100644 --- a/App/RodAndBarPosition/RodAndBarPositionApp/Version.md +++ b/App/RodAndBarPosition/RodAndBarPositionApp/Version.md @@ -1,3 +1,7 @@ +# 1.1.1 2026-045-26 +## build_1 +1. 更新算法1.3.2 + # 1.1.0 2026-04-13 ## build_1 1. 更新内存泄露问题 diff --git a/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so b/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so index 1316cb87..29b787b4 100644 Binary files a/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so and b/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so differ diff --git a/AppAlgo/rodAndBarDetection/Inc/rodAndBarDetection_Export.h b/AppAlgo/rodAndBarDetection/Inc/rodAndBarDetection_Export.h index 68dbda39..139d604b 100644 --- a/AppAlgo/rodAndBarDetection/Inc/rodAndBarDetection_Export.h +++ b/AppAlgo/rodAndBarDetection/Inc/rodAndBarDetection_Export.h @@ -65,7 +65,7 @@ SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void); SG_APISHARED_EXPORT SSG_planeCalibPara sx_rodPosition_getBaseCalibPara( std::vector< std::vector>& scanLines); -#if 0 +#if 1 //̬ƽȥ SG_APISHARED_EXPORT void sx_rodPosition_lineDataR( std::vector< SVzNL3DPosition>& a_line, diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll index 4bf74dc5..96a4419b 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb index 7bd78d3b..f1eefa5e 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll index 25ccd1ca..311a93e5 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.lib b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.lib index 3b90d409..289b0fdf 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.lib and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.lib differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb index 5c46a95f..064ec078 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll index 689ad0c6..65048b5f 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb index 7b799150..fd7a37b7 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll index 809359e1..441cc0d5 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.lib b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.lib index 2821b542..dc70572e 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.lib and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.lib differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb index ea835191..7fabaf1a 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb differ diff --git a/AppAlgo/rodAndBarDetection/rodAndBarDetection_test.cpp b/AppAlgo/rodAndBarDetection/rodAndBarDetection_test.cpp index 669351b4..d79c59c1 100644 --- a/AppAlgo/rodAndBarDetection/rodAndBarDetection_test.cpp +++ b/AppAlgo/rodAndBarDetection/rodAndBarDetection_test.cpp @@ -13,6 +13,7 @@ #include #include +#include typedef struct { int r; @@ -31,6 +32,101 @@ typedef struct float b; } SPointXYZRGB; + +// 点乘 dot +double _dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b) +{ + return (a.x * b.x + a.y * b.y + a.z * b.z); +} + +// 模长 +double _length(const SVzNL3DPoint& a) +{ + return sqrt(a.x * a.x + a.y * a.y + a.z * a.z); +} + +// 计算两个向量夹角（返回 角度） +float _computeVecAngle(const SVzNL3DPoint& a, const SVzNL3DPoint& b) +{ + float l = _length(a) * _length(b); + if (l < 1e-6f) + return 0.0f; // 避免除零 + + float cosTheta = _dotMultiply(a, b) / l; + cosTheta = cosTheta > 1.0 ? 1.0 : cosTheta; + cosTheta = cosTheta < -1.0 ? -1.0 : cosTheta; + float rad = acosf(cosTheta); + float degree = rad * 180.0f / (float)M_PI; + return degree; +} + +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; +} + void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) { std::ifstream inputFile(fileName); @@ -165,49 +261,38 @@ void _outputScanDataFile(char* fileName, std::vector& rodParas) { - //设置初始结果 - 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; + return; - //调平矩阵 - 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; + std::vector indices; + int maxIdx = 0; + std::vector< SSX_rodParam> paras; + while (getline(inputFile, linedata)) + { + int idx; + float len, dia; + sscanf_s(linedata.c_str(), "%d_L_%f_D_%f", &idx, &len, &dia); + SSX_rodParam a_para = { dia , len }; + indices.push_back(idx); + paras.push_back(a_para); + maxIdx = maxIdx < idx ? idx : maxIdx; + } + if (maxIdx == 0) + return; + rodParas.resize(maxIdx + 1); + for (int i = 0; i < (int)indices.size(); i++) + { + int idx = indices[i]; + rodParas[idx] = paras[i]; + } + return; } - void _outputChanneltInfo(char* fileName, std::vector& screwInfo) { std::ofstream sw(fileName); @@ -450,15 +535,15 @@ void _outputRGBDScan_RGBD_centerPose( SVzNL3DPosition* pt3D = &scanLines[line][i]; if (pt3D->nPointIdx == 2) + { + rgb = { 135,206,235 }; + size = 5; + } + else if (pt3D->nPointIdx == 3) { rgb = { 250, 0, 0 }; size = 5; } - //else if (pt3D->nPointIdx == 1) - //{ - // rgb = { 0, 0, 250 }; - // size = 3; - //} else //if (pt3D->nPointIdx == 0) { rgb = { 200, 200, 200 }; @@ -500,7 +585,7 @@ void _outputRGBDScan_RGBD_centerPose( //输出法向 size = 1; - double len = 60; + double len = 110; lineIdx = 0; { SVzNL3DPoint pt0 = { poseInfo.center.x, // - len * poseInfo.normalDir.x, @@ -878,7 +963,7 @@ void _outputRGBDScan_RGBD_weldSeam( sw.close(); } -#define SCREW_TEST_GROUP 7 +#define SCREW_TEST_GROUP 10 void screwTest(void) { const char* dataPath[SCREW_TEST_GROUP] = { @@ -889,17 +974,22 @@ void screwTest(void) "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2/", //4 "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2未动螺杆拧进去100mm左右/", //5 "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2向前100mm螺杆拧进去100mm左右/", //6 + "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/另一根螺杆/", //7 + "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1/", //8 + "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1xyz移动10mm/", //9 + }; SVzNLRange fileIdx[SCREW_TEST_GROUP] = { {1,4},{1,30},{1,11}, - {1,20}, {1,20}, {1,5}, {1,21} + {1,20}, {1,20}, {1,5}, {1,21}, + {1,10}, {1,10}, {1,10} }; const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 3; grp < SCREW_TEST_GROUP; grp++) + for (int grp = 9; grp < SCREW_TEST_GROUP; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -908,6 +998,8 @@ void screwTest(void) if(0 == grp) sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + else if( (grp>= 7)&&(grp <= 9)) + sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%s%d_LaserData_Jl26C299.txt", dataPath[grp], fidx); @@ -962,12 +1054,23 @@ void screwTest(void) &errCode); long t2 = (long)GetTickCount64(); - printf("%s: %d(ms), errCode=%d\n", _scan_file, (int)(t2 - t1), errCode); + //输出测试结果 - sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); - _outputRGBDScan_RGBD(_scan_file, scanLines, screwInfo); - sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); - _outputChanneltInfo(_scan_file, screwInfo); + char _out_file[256]; + sprintf_s(_out_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); + _outputRGBDScan_RGBD(_out_file, scanLines, screwInfo); + sprintf_s(_out_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); + _outputChanneltInfo(_out_file, screwInfo); + + if (screwInfo.size() > 0) + { + SVzNL3DPoint zAxis = { 0, 0, 1.0 }; + float angle = _computeVecAngle(screwInfo[0].axialDir, zAxis); + printf("%s: %d(ms), errCode=%d, {%f, %f, %f}, 与Z轴夹角=%f \n", _scan_file, (int)(t2 - t1), errCode, + (float)screwInfo[0].center.x, (float)screwInfo[0].center.y, (float)screwInfo[0].center.z, angle); + } + else + printf("%s: %d(ms), errCode=%d\n", _scan_file, (int)(t2 - t1), errCode); } } } @@ -975,7 +1078,7 @@ void screwTest(void) #define LOCATING_PALTE_TEST_GROUP 1 void locatingPlateTest(void) { - const char* dataPath[SCREW_TEST_GROUP] = { + const char* dataPath[LOCATING_PALTE_TEST_GROUP] = { "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云/", //0 }; @@ -1030,22 +1133,286 @@ void locatingPlateTest(void) } } -#define ROD_POSITION_TEST_GROUP 2 -void rodPositionTest(void) +#define NEW_LOCATING_PALTE_TEST_GROUP 1 +void newLocatingPlateTest(void) { - const char* dataPath[ROD_POSITION_TEST_GROUP] = { - "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/", //0 - "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //1 + const char* dataPath[NEW_LOCATING_PALTE_TEST_GROUP] = { + "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260621/", //0 }; - SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = { - {1,8}, {1,5} + SVzNLRange fileIdx[NEW_LOCATING_PALTE_TEST_GROUP] = { + {12,13}, }; const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 1; grp < ROD_POSITION_TEST_GROUP; grp++) + for (int grp = 0; grp < NEW_LOCATING_PALTE_TEST_GROUP; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =2; + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 45; //45度角 + cornerParam.scale = 5.0; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 5.0; + cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 + cornerParam.jumpCornerTh_2 = 60; + + int errCode = 0; + SSX_platePoseInfo centerPose = sx_getLocationPlatePose_new( + scanLines, + cornerParam, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms),errCode=%d\n", _scan_file, (int)(t2 - t1), errCode); + //输出测试结果 + sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); + _outputRGBDScan_RGBD_centerPose(_scan_file, scanLines, centerPose); + sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); + _outputPlatePiseInfo(_scan_file, centerPose); + } + } +} + +; +void _removeNullData(std::vector>& scanLines, double _F, std::vector>& scanData) +{ + std::vector< SVzNLPointXYZRGBA> validPoints; + for (int line = 0; line < (int)scanLines.size(); line++) + { + for (int j = 0; j < (int)scanLines[line].size(); j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNLPointXYZRGBA a_pt; + double v = _F * scanLines[line][j].pt3D.y / scanLines[line][j].pt3D.z + 2000; + a_pt.nRGB = (int)(v + 0.5); + a_pt.x = scanLines[line][j].pt3D.x; + a_pt.y = scanLines[line][j].pt3D.y; + a_pt.z = scanLines[line][j].pt3D.z; + validPoints.push_back(a_pt); + } + } + } + + std::vector< SVzNLPointXYZRGBA> a_line; + SVzNLPointXYZRGBA pre_pt; + pre_pt.x = 0; + pre_pt.y = 0; + pre_pt.z = 0; + pre_pt.nRGB = -1; + for (int i = 0; i < validPoints.size(); i++) + { + if (validPoints[i].nRGB < (pre_pt.nRGB - 10)) + { + if (a_line.size() > 0) + scanData.push_back(a_line); + a_line.clear(); + } + a_line.push_back(validPoints[i]); + pre_pt = validPoints[i]; + } + if(a_line.size() > 0) + scanData.push_back(a_line); + return; +} +void _convertToGridData_XYZRGB(std::vector>& scanData, double _F, std::vector>& gridData) +{ + int lineNum = (int)scanData.size(); + int min_y = 100000000; + int max_y = -10000000; + int validStartLine = -1; + int validEndLine = -1; + for (int line = 0; line < lineNum; line++) + { + if (scanData[line].size() > 0) + { + if (validStartLine < 0) + { + validStartLine = line; + validEndLine = line; + } + else + validEndLine = line; + } + + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + double v = _F * a_pt.y / a_pt.z + 2000; + a_pt.nRGB = (int)(v + 0.5); + max_y = max_y < (int)a_pt.nRGB ? (int)a_pt.nRGB : max_y; + min_y = min_y > (int)a_pt.nRGB ? (int)a_pt.nRGB : min_y; + } + } + } + if (min_y == 100000000) + return; + + int vldLineNum = validEndLine - validStartLine + 1; + gridData.resize(vldLineNum); + int pt_counter = max_y - min_y + 1; + for (int i = 0; i < vldLineNum; i++) + { + gridData[i].resize(pt_counter); + for (int j = 0; j < pt_counter; j++) + { + gridData[i][j].nPointIdx = j; + gridData[i][j].pt3D.x = 0; + gridData[i][j].pt3D.y = 0; + gridData[i][j].pt3D.z = 0; + } + } + for (int line = validStartLine; line <= validEndLine; line++) + { + int gridLine = line - validStartLine; + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + int pt_id = a_pt.nRGB - min_y; + gridData[gridLine][pt_id].pt3D.x = a_pt.x; + gridData[gridLine][pt_id].pt3D.y = a_pt.y; + gridData[gridLine][pt_id].pt3D.z = a_pt.z; + } + } + } + return; +} + +#define ROD_POSITION_TEST_GROUP 5 +#define TEST_CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_ROD_POSITION 1 +void rodPositionTest(void) +{ +#if TEST_CONVERT_TO_GRID + //将数据转换成栅格格式格式 + char _scan_dir[256]; + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + sprintf_s(_scan_dir, "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/"); + char _scan_src_file[256]; + double _F = 1219.15; //f + for (int i = 1; i <= 3; i++) + { + //read ply txt + sprintf_s(_scan_src_file, "%sLaserDataRAW_%d.txt", _scan_dir, i); + + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_src_file, scanLines); + + std::vector> scanData; + _removeNullData(scanLines, _F, scanData); + + std::vector> gridData; + _convertToGridData_XYZRGB(scanData, _F, gridData); + char _out_file[256]; + sprintf_s(_out_file, "%sLaserData_%d.txt", _scan_dir, i); + _outputScanDataFile(_out_file, gridData, 0.0f, 0, 0); + + printf("%s: convert done!\n", _scan_src_file); + } + printf("all converted done!\n", _scan_src_file); +#endif +#if TEST_COMPUTE_CALIB_PARA + const char* calibDataPath = "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/调平/位置1/"; + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "%sGroundData_1.txt", calibDataPath); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = sx_rodPosition_getBaseCalibPara(scanData); + //结果进行验证 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + sx_rodPosition_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", calibDataPath); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "%sGroundData_1_calib_verify.txt", calibDataPath); + + _outputScanDataFile(_out_file, scanData, lineV, maxTimeStamp, clockPerSecond); +#if 0 + for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[cvtGrp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + { + //调平，去除地面 + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1); + } + sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + } +#endif + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_ROD_POSITION + const char* dataPath[ROD_POSITION_TEST_GROUP] = { + "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/测试数据/", //0 + "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/", //1 + "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/其它尺寸棒材点云/", //2 + + "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //3 + "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //4 + }; + + SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = { + {1,8}, {1,3}, {1,31}, + {1,5},{1,11} + }; + + const char* ver = wd_rodAndBarDetectionVersion(); + printf("ver:%s\n", ver); + + for (int grp = 0; grp <= 2; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -1061,15 +1428,21 @@ void rodPositionTest(void) poseCalibPara.planeHeight = -1.0; for (int i = 0; i < 9; i++) poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; - //char calibFile[250]; - //sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); - //poseCalibPara = _readCalibPara(calibFile); + + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + poseCalibPara = _readCalibPara(calibFile); + + sprintf_s(calibFile, "%sparameter.txt", dataPath[grp]); + std::vector rodParas; + if (grp <= 2) + _readRodParas(calibFile, rodParas); for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =1; + //fidx =5; char _scan_file[256]; - if(1 == grp) + if( (3 == grp) ||(4 == grp)) sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); @@ -1084,10 +1457,9 @@ void rodPositionTest(void) long t1 = (long)GetTickCount64();//统计时间 SSX_rodParam rodParam; - if (0 == grp) + if (grp <= 2) { - rodParam.diameter = 52.0; //圆棒直径 - rodParam.len = 290; + rodParam = rodParas[fidx]; } else { @@ -1096,7 +1468,7 @@ void rodPositionTest(void) } SSG_cornerParam cornerParam; - cornerParam.cornerTh = 60; //45度角 + cornerParam.cornerTh = 45; //45度角 cornerParam.scale = rodParam.diameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; cornerParam.minEndingGap_z = 5.0; @@ -1112,8 +1484,8 @@ void rodPositionTest(void) growParam.yDeviation_max = 10.0; growParam.maxSkipDistance = 10.0; growParam.zDeviation_max = 10.0;// - growParam.minLTypeTreeLen = 100; //mm, 螺杆长度 - growParam.minVTypeTreeLen = 100; //mm + growParam.minLTypeTreeLen = rodParam.len * 0.75; //mm, 螺杆长度 + growParam.minVTypeTreeLen = rodParam.len * 0.75; //mm bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 int errCode = 0; @@ -1129,7 +1501,7 @@ void rodPositionTest(void) &errCode); long t2 = (long)GetTickCount64(); - printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + printf("%s: %d(ms),errCode=%d\n", _scan_file, (int)(t2 - t1),errCode); //输出测试结果 sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); _outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo); @@ -1137,6 +1509,7 @@ void rodPositionTest(void) _outputRodtInfo(_scan_file, rodInfo); } } +#endif } @@ -1239,21 +1612,35 @@ void rodWeldSeamPosition_test(void) } } + +typedef enum +{ + keSG_测试_配天螺杆定位 = 0, + keSG_测试_配天定位盘定位, + keSG_测试_配天新定位盘定位, + keSG_测试_棒材抓取, + keSG_测试_筑裕钢筋焊缝定位, +} ESG_testMode; + int main() { -#if 1 //螺杆定位测试 -#if 1 - screwTest(); -#else - locatingPlateTest(); -#endif -#else //棒材抓取定位测试 -#if 1 - rodPositionTest(); -#else - rodWeldSeamPosition_test(); -#endif -#endif + //ESG_testMode testMode = keSG_测试_配天螺杆定位; + //ESG_testMode testMode = keSG_测试_配天定位盘定位; + //ESG_testMode testMode = keSG_测试_配天新定位盘定位; + ESG_testMode testMode = keSG_测试_棒材抓取; + //ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位; + + if(keSG_测试_配天螺杆定位 == testMode) + screwTest(); + else if(keSG_测试_配天定位盘定位 == testMode) + locatingPlateTest(); + else if (keSG_测试_配天新定位盘定位 == testMode) + newLocatingPlateTest(); + else if(keSG_测试_棒材抓取 == testMode) + rodPositionTest(); + else if(keSG_测试_筑裕钢筋焊缝定位 == testMode) + rodWeldSeamPosition_test(); + return 0; }