From 7204f3efcad10f73683b0b9fef9967f3c6011972 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 8 Jan 2026 21:40:43 +0800 Subject: [PATCH] =?UTF-8?q?BQ=5FworkpieceCornerExtract=20version=201.3.0?= =?UTF-8?q?=20:=20=E7=AE=97=E6=B3=95=E5=AF=B9=E5=90=8C=E4=B8=80brach?= =?UTF-8?q?=E5=88=A4=E6=96=AD=E6=B7=BB=E5=8A=A0=E4=BA=86=E8=B7=9D=E7=A6=BB?= =?UTF-8?q?=E5=88=A4=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BQ_workpieceCornerExtract_test.cpp | 34 ++++++++------- sourceCode/BQ_workpieceCornerExtraction.cpp | 41 ++++++++++++++++++- 2 files changed, 59 insertions(+), 16 deletions(-) diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index e1d1392..a14d7fd 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/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\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\", //1 - "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\", //2 - "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\", //3 - "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\", //4 - "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\", //5 - "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\", //6 + "F:/ShangGu/项目/冠钦_博清科技/数据/", //0 + "F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度1/", //1 + "F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度2/", //2 + "F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度3/", //3 + "F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度4/", //4 + "F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/", //5 + "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> scanLines; @@ -820,7 +823,10 @@ int main() growParam.minLTypeTreeLen = 100; //mm growParam.minVTypeTreeLen = 100; //mm SSX_BQworkpiecePara workpieceParam; - workpieceParam.lineLen = 160.0; //直线段长度 + if(grp == 7) + workpieceParam.lineLen = 120.0; //直线段长度 + else + workpieceParam.lineLen = 160.0; //直线段长度 int errCode = 0; std::vector debug_conturs; SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners( diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 6c8dbc7..5b97a5a 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -8,7 +8,8 @@ //version 1.1.0 : base version release to customer, output corner coordinate //version 1.2.0 : add position length output //version 1.2.1 : fix bugs for ver1.2.0 -std::string m_strVersion = "1.2.1"; +//version 1.3.0 : 㷨ͬһbrachж˾ж +std::string m_strVersion = "1.3.0"; const char* wd_BQWorkpieceCornerVersion(void) { return m_strVersion.c_str(); @@ -552,6 +553,39 @@ SVzNL3DPoint computeEdgeCross(double angle, std::vector& polarPoint return a_pt; } + +bool checkSameBranch(SWD_polarPt& corner_1, SWD_polarPt& corner_2, + std::vector& polarPoints, + double center_x, double center_y) +{ + int size = (int)polarPoints.size(); + int midPtIdx; + if (corner_2.cptIndex > corner_1.cptIndex) + midPtIdx = (corner_1.cptIndex + corner_2.cptIndex) / 2; + else + { + midPtIdx = (size - corner_1.cptIndex + corner_2.cptIndex) / 2 + corner_1.cptIndex; + if (midPtIdx >= size) + midPtIdx = midPtIdx - size; + } + + SWD_polarPt& midPt = polarPoints[midPtIdx]; + double a1, b1, c1; + compute2ptLine_2(center_x, center_y, midPt.x, midPt.y, &a1, &b1, &c1); + double a2, b2, c2; + compute2ptLine_2(corner_1.x, corner_1.y, corner_2.x, corner_2.y, &a2, &b2, &c2); + //㽻 + SVzNL3DPoint crossPt = computeLineCrossPt_abs(a1, b1, c1, a2, b2, c2); + double len = sqrt(pow(crossPt.x - center_x, 2) + pow(crossPt.y - center_y, 2)); + double dist_diff = midPt.R / len; + if ( (dist_diff > 0.95) &&(dist_diff < 1.05)) + return true; + else + return false; + +} + + SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, @@ -967,10 +1001,13 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( //if (polarPeakInfo[m].cornerDir == 2) //ʱ롣 { int nxtIdx = (m + 1)%pkSize; + + bool isSameBranch = checkSameBranch(validPolarRPeakPts[m], validPolarRPeakPts[nxtIdx], + polarPoints, center_x, center_y); double angleDiff = computeAngleDiff(validPolarRPeakPts[nxtIdx].angle, validPolarRPeakPts[m].angle); if (angleDiff < 0) angleDiff += 360; - if (angleDiff < 90) //Ϊͬһbranchϲֻһ + if ( (angleDiff < 90) && (true == isSameBranch)) //Ϊͬһbranchϲֻһ { validPolarRPeakPts[nxtIdx].pkId = validPolarRPeakPts[m].pkId; //pair validPolarRPeakPts[nxtIdx].cptIndex = -1;