From 335ee0a92f38457bd5f02dd41e26f437952b3b65 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 11 Dec 2025 04:56:17 +0800 Subject: [PATCH] =?UTF-8?q?sx=5FBQ=5FgetWorkpieceCorners=20version=201.2.1?= =?UTF-8?q?=20-=20=E4=BF=AE=E6=AD=A3=E4=BA=861.2.0=E7=89=88=E6=9C=AC?= =?UTF-8?q?=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BQ_workpieceCornerExtract_test.cpp | 104 +++++----- sourceCode/BQ_workpieceCornerExtraction.cpp | 194 ++++++++++++++---- sourceCode/SG_baseAlgo_Export.h | 2 +- sourceCode/SG_baseFunc.cpp | 9 +- 4 files changed, 210 insertions(+), 99 deletions(-) diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index fe2c1c4..e1d1392 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -530,7 +530,7 @@ void _outputRGBDScanLapWeld_RGBD( sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } - if ((debugData.size() > 0) && (workpieceCorner.workpieceType > 0)) + if (workpieceCorner.workpieceType > 0) { std::vector< SWD_3DPointPair> lines; SWD_3DPointPair a_line; @@ -567,52 +567,54 @@ void _outputRGBDScanLapWeld_RGBD( lines.push_back(a_line); } - int linePtNum = 0; - for (int i = 0; i < (int)debugData.size(); i++) - linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].edgeLink2_size; - sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; - lineNum++; - - rgb = { 255, 0, 0 }; - size = 3; - for (int i = 0; i < (int)debugData.size(); i++) + if (debugData.size() > 0) { - for (int j = 0; j < debugData[i].edge_size; j++) - { - float x = (float)debugData[i].edge[j].x; - float y = (float)debugData[i].edge[j].y; - float z = (float)debugData[i].edge[j].z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - for (int j = 0; j < debugData[i].edgeLink1_size; j++) - { - float x = (float)debugData[i].edgeLink_1[j].x; - float y = (float)debugData[i].edgeLink_1[j].y; - float z = (float)debugData[i].edgeLink_1[j].z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - for (int j = 0; j < debugData[i].edgeLink2_size; j++) - { - float x = (float)debugData[i].edgeLink_2[j].x; - float y = (float)debugData[i].edgeLink_2[j].y; - float z = (float)debugData[i].edgeLink_2[j].z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } - //加一个点，用于跳过显示工具bug - float x = (float)debugData[0].edge[0].x; - float y = (float)debugData[0].edge[0].y; - float z = (float)debugData[0].edge[0].z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + int linePtNum = 0; + for (int i = 0; i < (int)debugData.size(); i++) + linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].edgeLink2_size; + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + lineNum++; + rgb = { 255, 0, 0 }; + size = 3; + for (int i = 0; i < (int)debugData.size(); i++) + { + for (int j = 0; j < debugData[i].edge_size; j++) + { + float x = (float)debugData[i].edge[j].x; + float y = (float)debugData[i].edge[j].y; + float z = (float)debugData[i].edge[j].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + for (int j = 0; j < debugData[i].edgeLink1_size; j++) + { + float x = (float)debugData[i].edgeLink_1[j].x; + float y = (float)debugData[i].edgeLink_1[j].y; + float z = (float)debugData[i].edgeLink_1[j].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + for (int j = 0; j < debugData[i].edgeLink2_size; j++) + { + float x = (float)debugData[i].edgeLink_2[j].x; + float y = (float)debugData[i].edgeLink_2[j].y; + float z = (float)debugData[i].edgeLink_2[j].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + //加一个点，用于跳过显示工具bug + float x = (float)debugData[0].edge[0].x; + float y = (float)debugData[0].edge[0].y; + float z = (float)debugData[0].edge[0].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } //显示拟合直线 rgb = { 255, 0, 0 }; size = 3; @@ -738,7 +740,7 @@ int main() const char* ver = wd_BQWorkpieceCornerVersion(); printf("ver:%s\n", ver); - for (int grp = 6; grp <= 6; grp++) + for (int grp = 5; grp <= 6; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -760,7 +762,7 @@ int main() for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =1; + //fidx =4; char _scan_file[256]; if(0 == grp) sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); @@ -788,6 +790,8 @@ int main() { if (2 == fidx) cuttingZ = 1760.0; + else if(3 == fidx) + cuttingZ = 1780.0; } sx_BQ_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ); } @@ -816,7 +820,7 @@ int main() growParam.minLTypeTreeLen = 100; //mm growParam.minVTypeTreeLen = 100; //mm SSX_BQworkpiecePara workpieceParam; - workpieceParam.lineLen = 180.0; //直线段长度 + workpieceParam.lineLen = 160.0; //直线段长度 int errCode = 0; std::vector debug_conturs; SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners( @@ -834,11 +838,7 @@ int main() printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); //输出测试结果 sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); -#if _OUTPUT_DEBUG_DATA _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, false, debug_conturs); -#else - _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL); -#endif sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); _outputCornerInfo(calibFile, workpieceCorner); } diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 0d6e9ea..f9512c8 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -7,7 +7,8 @@ //version 1.1.0 : base version release to customer, output corner coordinate //version 1.2.0 : add position length output -std::string m_strVersion = "1.2.0"; +//version 1.2.1 : fix bugs for ver1.2.0 +std::string m_strVersion = "1.2.1"; const char* wd_BQWorkpieceCornerVersion(void) { return m_strVersion.c_str(); @@ -207,7 +208,9 @@ int _counterLinePtNum(std::vector& lineData) int ptNum = 0; for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++) { - if (lineData[i].pt3D.z > 1e-4) + if ( (abs(lineData[i].pt3D.z) > 1e-4) || + (abs(lineData[i].pt3D.x > 1e-4)) || + (abs(lineData[i].pt3D.y > 1e-4))) ptNum++; } return ptNum; @@ -217,6 +220,7 @@ int _counterLinePtNum(std::vector& lineData) int _getBranchInfo( int validStartLine, //ʼɨ߽ int validEndLine, //ɨ߽ + bool partialScan, //ҰܸǹȫʱpartialScanΪtrue, std::vector& polarPoints, SWD_polarPt branchCorner, SWD_polarPeakInfo branchCornerInfo, @@ -229,6 +233,10 @@ int _getBranchInfo( int contourPtSize = (int)polarPoints.size(); std::vector branchContourPts; + int validStartLineWin = validStartLine + 3; + int validEndLineWin = validEndLine - 3; + if (validEndLineWin < 0) + validEndLineWin = 0; int cornerWin = 2; int LineDir, contourDir, startIdx, endingIdx; if (branchCornerInfo.cornerDir == 2)//ʱ @@ -275,7 +283,8 @@ int _getBranchInfo( idx += contourPtSize; else idx = idx % contourPtSize; - if ((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine)) + if ((partialScan == true) && + ((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine))) { toRefine = true; break; @@ -299,12 +308,31 @@ int _getBranchInfo( maxHPos = i; } } + //һ + int recEnding = maxHPos * 2; + if (recEnding >= LinePts.size()) + recEnding = (int)LinePts.size() - 1; + pt1 = LinePts[0]; + pt2 = LinePts[recEnding]; + compute2ptLine(pt1, pt2, &aa, &bb, &cc); + //Ľǵ + maxH = 0; + maxHPos = 0; + for (int i = 0; i < recEnding; i++) + { + double H = computePtDistToLine(LinePts[i].x, LinePts[i].y, aa, bb, cc); + if (maxH < H) + { + maxH = H; + maxHPos = i; + } + } + int cptIdx = startIdx + LineDir * maxHPos; if (cptIdx < 0) cptIdx += contourPtSize; else cptIdx = cptIdx % contourPtSize; - branchCorner = polarPoints[cptIdx]; if (branchCornerInfo.cornerDir == 2)//ʱ { @@ -340,6 +368,7 @@ int _getBranchInfo( //˵㴹 SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c); //ȡ + double minDist = -1; for (int i = cornerWin; i < contourPtSize; i++) //ܿǵΧ { int ptIdx = i * contourDir + branchCorner.cptIndex; @@ -350,7 +379,12 @@ int _getBranchInfo( SWD_polarPt a_pt = polarPoints[ptIdx]; SVzNL2DPointD contourFoot = sx_getFootPoint_abc(a_pt.x, a_pt.y, _a, _b, _c); double dist = sqrt(pow(footPt.x - contourFoot.x, 2) + pow(footPt.y - contourFoot.y, 2)); - if (dist < 1.0) + if (minDist < 0) + minDist = dist; + else + minDist = minDist > dist ? dist : minDist; + + if ((dist < 10) &&(dist > (minDist + 1.0))) break; branchContourPts.push_back(a_pt); } @@ -359,8 +393,9 @@ int _getBranchInfo( bool hasSidePt = false; for (int m = 0; m < contourPtNum; m++) { - if ((branchContourPts[m].lineIdx == validStartLine) || - (branchContourPts[m].lineIdx == validEndLine)) + if ((partialScan == true) && + ((branchContourPts[m].lineIdx == validStartLine) || + (branchContourPts[m].lineIdx == validEndLine))) { branchContourPts[m].z = -1.0; //label hasSidePt = true; @@ -426,9 +461,10 @@ int _getBranchInfo( a_branchInfo.corner[1].z = a_branchInfo.corner[0].z; // a_branchInfo.angle = branchCorner.angle; - a_branchInfo.line_a = _a; - a_branchInfo.line_a = _b; - a_branchInfo.line_a = _c; + + a_branchInfo.line_a = edge1_b; + a_branchInfo.line_b = -edge1_a; + a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y); *resultBranchInfo = a_branchInfo; } else @@ -438,7 +474,8 @@ int _getBranchInfo( for (int i = 0; i < contourPtNum; i++) { SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z }; - if ((branchContourPts[i].lineIdx == validStartLine)|| (branchContourPts[i].lineIdx == validEndLine)) + if ((partialScan == true) && + ((branchContourPts[i].lineIdx <= validStartLineWin)|| (branchContourPts[i].lineIdx >= validEndLineWin))) { break; } @@ -448,7 +485,8 @@ int _getBranchInfo( for (int i = contourPtNum - 1; i >= 0; i--) { SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z }; - if ((branchContourPts[i].lineIdx == validStartLine) || (branchContourPts[i].lineIdx == validEndLine)) + if ((partialScan == true) && + ((branchContourPts[i].lineIdx <= validStartLineWin) || (branchContourPts[i].lineIdx >= validEndLineWin))) break; edgePt2.insert(edgePt2.begin(), a_pt); } @@ -829,6 +867,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( //ȡRֵ double minR = -1, maxR = -1; //СRûз֧minRmaxRСʱΪԲλ8Σûз֧ + int minRPos = -1; std::vector polarRPeakPts; int winSize = contourPtSize / 36; //+-10ȷΧ if (winSize < 5) @@ -840,9 +879,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( { minR = currR; maxR = currR; + minRPos = pi; } else { + minRPos = minR > currR ? pi : minRPos; minR = minR > currR ? currR : minR; maxR = maxR < currR ? currR : maxR; } @@ -867,6 +908,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector polarPeakInfo; int pkId = 0; //ԲεļֵƫƣԲҲγɼֵݼֵL=ֱ߶γȹɵŽж + double arcAngleChkLen = 100; //ԲŽǵij߶ for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++) { int ptidx = polarRPeakPts[i].cptIndex; @@ -875,13 +917,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( py = polarRPeakPts[i].y; int LL1 = -1; + int halfLL1 = -1; for (int j = ptidx - 1; j > -contourPtSize; j--) { int idx = (j + contourPtSize) % contourPtSize; //Ͳνṹ double cx = polarPoints[idx].x; double cy = polarPoints[idx].y; double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); - if (len > workpieceParam.lineLen) + if (len < arcAngleChkLen) + halfLL1 = idx; + if (len > (workpieceParam.lineLen)) { LL1 = idx; break; @@ -889,13 +934,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } int LL2 = -1; + int halfLL2 = -1; for (int j = ptidx + 1; j < contourPtSize*2; j++) { int idx = j % contourPtSize; //Ͳνṹ double cx = polarPoints[idx].x; double cy = polarPoints[idx].y; double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); - if (len > workpieceParam.lineLen) + if (len < arcAngleChkLen) + halfLL2 = idx; + if (len > (workpieceParam.lineLen)) { LL2 = idx; break; @@ -904,31 +952,20 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( if ((LL1 >= 0) && (LL2 >= 0)) { - double len1 = sqrt(pow(px - polarPoints[LL1].x, 2) + pow(py - polarPoints[LL1].y, 2)); - double len2 = sqrt(pow(px - polarPoints[LL2].x, 2) + pow(py - polarPoints[LL2].y, 2)); - double len3 = sqrt(pow(polarPoints[LL1].x - polarPoints[LL2].x, 2) + - pow(polarPoints[LL1].y - polarPoints[LL2].y, 2)); + double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2)); + double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2)); + double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) + + pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2)); double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2); double theta = acos(cosTheta) * 180.0 / PI; if (theta < 150) { - double L1_angle = polarPoints[ptidx].angle - polarPoints[LL1].angle; - if (L1_angle < 0) - L1_angle += 360; - double L2_angle = polarPoints[LL2].angle - polarPoints[ptidx].angle; - if (L2_angle < 0) - L2_angle += 360; - SWD_polarPeakInfo a_pkInfo; a_pkInfo.cptIndex = ptidx; a_pkInfo.L1_ptIndex = LL1; a_pkInfo.L2_ptIndex = LL2; a_pkInfo.cornerAngle = theta; - if(L1_angle < L2_angle) - a_pkInfo.cornerDir = 2; //ʱ - else - a_pkInfo.cornerDir = 1; //˳ʱ - + a_pkInfo.cornerDir = 0; polarRPeakPts[i].cptIndex = ptidx; polarRPeakPts[i].pkId = pkId; pkId++; @@ -939,6 +976,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } int workpieceType = -1; + bool partialScan = false; //ҰܸǹȫʱpartialScanΪtrue std::vector< SWD_branchInfo> branchInfo; if (true == hasBranch) { @@ -949,10 +987,10 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( if (validPolarRPeakPts[m].cptIndex < 0) continue; - if (polarPeakInfo[m].cornerDir == 2) //ʱ롣 + //if (polarPeakInfo[m].cornerDir == 2) //ʱ롣 { int nxtIdx = (m + 1)%pkSize; - double angleDiff = validPolarRPeakPts[nxtIdx].angle - validPolarRPeakPts[m].angle; + double angleDiff = computeAngleDiff(validPolarRPeakPts[nxtIdx].angle, validPolarRPeakPts[m].angle); if (angleDiff < 0) angleDiff += 360; if (angleDiff < 90) //Ϊͬһbranchϲֻһ @@ -985,11 +1023,74 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } int branchNum = (int)branchPeaks.size(); if (branchNum == 2) + { workpieceType = 3; //ڵ3 - else if (branchNum == 3) - workpieceType = 2; //ڵ2 - else if (branchNum == 4) - workpieceType = 1; //ڵ1 + + SVzNL3DPoint pt1 = { polarPoints[minRPos].x, polarPoints[minRPos].y, 0 }; + SVzNL3DPoint pt2 = { center_x, center_y, 0 }; + double aux_a, aux_b, aux_c; + compute2ptLine(pt1, pt2, &aux_a, &aux_b, &aux_c); + double counterAngle = polarPoints[minRPos].angle + 180; + if (counterAngle > 360) + counterAngle = counterAngle - 360; + SVzNL3DPoint a_cross = computeEdgeCross(counterAngle, polarPoints, aux_a, aux_b, aux_c); + double refine_cx = (pt1.x + a_cross.x) / 2; + double refine_cy = (pt1.y + a_cross.y) / 2; + //ȷcornerDir + for (int m = 0; m < branchNum; m++) + { + for (int n = 0; n < branchPeaks[m].size(); n++) + { + int cptIdx = branchPeakInfo[m][n].cptIndex; + int LL1 = branchPeakInfo[m][n].L1_ptIndex; + int LL2 = branchPeakInfo[m][n].L2_ptIndex; + double angle = atan2(polarPoints[cptIdx].y - refine_cy, polarPoints[cptIdx].x - refine_cx); + angle = (angle / PI) * 180 + 180.0; + double LL1_angle = atan2(polarPoints[LL1].y - refine_cy, polarPoints[LL1].x - refine_cx); + LL1_angle = (LL1_angle / PI) * 180 + 180.0; + double LL2_angle = atan2(polarPoints[LL2].y - refine_cy, polarPoints[LL2].x - refine_cx); + LL2_angle = (LL2_angle / PI) * 180 + 180.0; + + double L1_angle = computeAngleDiff(angle, LL1_angle); + double L2_angle = computeAngleDiff(LL2_angle, angle); + if (L1_angle < L2_angle) + branchPeakInfo[m][n].cornerDir = 2; //ʱ + else + branchPeakInfo[m][n].cornerDir = 1; //˳ʱ + } + } + } + else if ((branchNum == 3) || (branchNum == 4)) + { + if (branchNum == 3) + { + partialScan = true; + workpieceType = 2; //ڵ2 + } + else + workpieceType = 1; //ڵ1 + //ȷcornerDir + + for (int m = 0; m < branchNum; m++) + { + for(int n = 0; n < branchPeaks[m].size(); n++) + { + int cptIdx = branchPeakInfo[m][n].cptIndex; + int LL1 = branchPeakInfo[m][n].L1_ptIndex; + int LL2 = branchPeakInfo[m][n].L2_ptIndex; + double L1_angle = polarPoints[cptIdx].angle - polarPoints[LL1].angle; + if (L1_angle < 0) + L1_angle += 360; + double L2_angle = polarPoints[LL2].angle - polarPoints[cptIdx].angle; + if (L2_angle < 0) + L2_angle += 360; + if (L1_angle < L2_angle) + branchPeakInfo[m][n].cornerDir = 2; //ʱ + else + branchPeakInfo[m][n].cornerDir = 1; //˳ʱ + } + } + } else workpieceType = 0; @@ -1021,6 +1122,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( int opOK = _getBranchInfo( validStartLine, //ʼɨ߽ validEndLine, //ɨ߽ + partialScan, //ҰܸǹȫʱpartialScanΪtrue, polarPoints, branchCorner, branchCornerInfo, @@ -1043,7 +1145,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( a_branchDebug.edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edge_size); a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size); a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size); -#endif + for(int m = 0; m < a_branchDebug.edge_size; m ++) a_branchDebug.edge[m] = branchEdgePt1[m]; for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) @@ -1051,6 +1153,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) a_branchDebug.edgeLink_2[m] = branchEdgePt2[m]; debug_contours.push_back(a_branchDebug); +#endif } workpieceCorners.workpieceType = workpieceType; if (workpieceType == 1) //4branch @@ -1183,11 +1286,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( else { workpieceType = 4; //ڵ4 + partialScan = true; //ûڱ߽Corner int polarPkNum = (int)validPolarRPeakPts.size(); for (int pki = 0; pki < polarPkNum; pki++) { //Ƿɨ߽ + int validStartLineWin = validStartLine + 3; + int validEndLineWin = validEndLine - 3; + if (validEndLineWin < 0) + validEndLineWin = 0; bool isSide = false; int cptIdx = validPolarRPeakPts[pki].cptIndex; SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z }; @@ -1201,8 +1309,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( break; else { - if ((polarPoints[idx].lineIdx == validStartLine) || - (polarPoints[idx].lineIdx == validEndLine)) + if ( (partialScan == true) && + ((polarPoints[idx].lineIdx <= validStartLineWin) || + (polarPoints[idx].lineIdx >= validEndLineWin))) isSide = true; else edgePt_1.push_back(a_pt); @@ -1222,8 +1331,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( break; else { - if ((polarPoints[idx].lineIdx == validStartLine) || - (polarPoints[idx].lineIdx == validEndLine)) + if ((partialScan == true) && + ((polarPoints[idx].lineIdx <= validStartLineWin) || + (polarPoints[idx].lineIdx >= validEndLineWin))) isSide = true; else edgePt_2.push_back(a_pt); @@ -1235,7 +1345,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( return workpieceCorners; } - if (false == isSide) + if (true == isSide) { //ϼ㽻㣬 double edge1_a = 0, edge1_b = 0, edge1_c = 0; diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index e79a9f9..6f4c7df 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -369,7 +369,7 @@ SG_APISHARED_EXPORT void rotateLine45Deg( double* r_a, double* r_b, double* r_c); //ֱ߽Ƕ -SG_APISHARED_EXPORT double getLineAngle(double _a, double _b, double _c); +SG_APISHARED_EXPORT double getLineAngle(const double _a, const double _b, const double _c); //2D SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2); diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 4c2c2d5..3e71236 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -341,15 +341,16 @@ void rotateLine45Deg( return; } -double getLineAngle(double _a, double _b, double _c) +double getLineAngle(const double _a, const double _b, const double _c) { - if (_a = 0) + if (_a == 0) return 0; - else if (_b = 0) + else if (_b == 0) return 90; else { - double theta = atan(-_a / _b) + PI / 2; + double k = _a / _b; + double theta = atan(-k) + PI / 2; theta = (theta * 180.0) / PI; return theta; }