From 908145f028dfef42e5b33c6b0fbc847c42eb1b75 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sat, 8 Nov 2025 08:47:46 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E5=B7=A5=E4=BB=B6=E5=AE=9A?= =?UTF-8?q?=E4=BD=8D=E7=AE=97=E6=B3=95=E6=95=B0=E6=8D=AE=E5=BC=82=E5=B8=B8?= =?UTF-8?q?=E5=88=A4=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/BQ_workpieceCornerExtraction.cpp | 57 ++++++++++++++++++++- sourceCode/SG_bagPositioning.cpp | 24 ++++----- sourceCode/SG_errCode.h | 4 +- 3 files changed, 70 insertions(+), 15 deletions(-) diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 3e24558..ed2d5ec 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -410,9 +410,24 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( SVzNL3DPoint firstPt = region[0].edge[0]; SVzNL3DPoint lastPt = region[0].edge.back(); int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen); int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen); + if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } region[1].rgnIdx = 1; //Top _getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true); @@ -420,29 +435,67 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( firstPt = region[1].edge[0]; lastPt = region[1].edge.back(); idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen); idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen); - + if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } region[2].rgnIdx = 2; //Right _getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false); //寻找对应的两边 firstPt = region[2].edge[0]; lastPt = region[2].edge.back(); idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen); idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); - region[3].rgnIdx = 3; //Bottom _getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true); //寻找对应的两边 firstPt = region[3].edge[0]; lastPt = region[3].edge.back(); idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen); idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen); + if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } for (int i = 0; i < 4; i++) { diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index d7ee30f..fd6a229 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -2879,8 +2879,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan - int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -2939,8 +2939,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan - int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min)+ x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -3005,8 +3005,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag - int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -3071,8 +3071,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag - int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } @@ -3113,8 +3113,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType; featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan - int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } @@ -3154,8 +3154,8 @@ if (hLine == 14) featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4; featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag - int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min); - int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min); + int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip; + int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip; distTranformMask.at(py, px) = 0; } } diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 111db32..a35b02a 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -9,4 +9,6 @@ //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 -#define SX_ERR_INVLD_HTREE_NUM -2002 \ No newline at end of file +#define SX_ERR_INVLD_HTREE_NUM -2002 +#define SX_ERR_INVLD_EDGE_LINK_NUM -2003 +#define SX_ERR_INVLD_CLOSES_PT -2004 \ No newline at end of file