jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新工件定位算法数据异常判断

Browse Source
This commit is contained in:
jerryzeng 2025-11-08 08:47:46 +08:00
parent dc23f073d7
commit 908145f028
3 changed files with 70 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
sourceCode/BQ_workpieceCornerExtraction.cpp
View File

@ -410,9 +410,24 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
SVzNL3DPoint firstPt = region[0].edge[0]; SVzNL3DPoint firstPt = region[0].edge[0];
SVzNL3DPoint lastPt = region[0].edge.back(); SVzNL3DPoint lastPt = region[0].edge.back();
int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true); 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); _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen);
int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); 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); _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 region[1].rgnIdx = 1; //Top
_getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true); _getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true);
@ -420,29 +435,67 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
firstPt = region[1].edge[0]; firstPt = region[1].edge[0];
lastPt = region[1].edge.back(); lastPt = region[1].edge.back();
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); 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); _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen);
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); 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); _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 region[2].rgnIdx = 2; //Right
_getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false); _getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false);
//寻找对应的两边 //寻找对应的两边
firstPt = region[2].edge[0]; firstPt = region[2].edge[0];
lastPt = region[2].edge.back(); lastPt = region[2].edge.back();
idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false); 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); _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen);
idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); 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); _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen);
region[3].rgnIdx = 3; //Bottom region[3].rgnIdx = 3; //Bottom
_getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true); _getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true);
//寻找对应的两边 //寻找对应的两边
firstPt = region[3].edge[0]; firstPt = region[3].edge[0];
lastPt = region[3].edge.back(); lastPt = region[3].edge.back();
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); 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); _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen);
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); 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); _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++) for (int i = 0; i < 4; i++)
{ {

24
sourceCode/SG_bagPositioning.cpp
View File

@ -2879,8 +2879,8 @@ if (hLine == 14)
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan featureMask.at<cv::Vec4i>(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 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); int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip;
distTranformMask.at<float>(py, px) = 0; distTranformMask.at<float>(py, px) = 0;
} }
} }
@ -2939,8 +2939,8 @@ if (hLine == 14)
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan featureMask.at<cv::Vec4i>(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 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); int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip;
distTranformMask.at<float>(py, px) = 0; distTranformMask.at<float>(py, px) = 0;
} }
} }
@ -3005,8 +3005,8 @@ if (hLine == 14)
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag featureMask.at<cv::Vec4i>(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 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); int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip;
distTranformMask.at<float>(py, px) = 0; distTranformMask.at<float>(py, px) = 0;
} }
} }
@ -3071,8 +3071,8 @@ if (hLine == 14)
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag featureMask.at<cv::Vec4i>(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 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); int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip;
distTranformMask.at<float>(py, px) = 0; distTranformMask.at<float>(py, px) = 0;
} }
@ -3113,8 +3113,8 @@ if (hLine == 14)
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType; featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan featureMask.at<cv::Vec4i>(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 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); int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip;
distTranformMask.at<float>(py, px) = 0; distTranformMask.at<float>(py, px) = 0;
} }
} }
@ -3154,8 +3154,8 @@ if (hLine == 14)
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4; featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag featureMask.at<cv::Vec4i>(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 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); int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip;
distTranformMask.at<float>(py, px) = 0; distTranformMask.at<float>(py, px) = 0;
} }
} }

2
sourceCode/SG_errCode.h
View File

@ -10,3 +10,5 @@
//BQ_workpiece //BQ_workpiece
#define SX_ERR_INVLD_VTREE_NUM -2001 #define SX_ERR_INVLD_VTREE_NUM -2001
#define SX_ERR_INVLD_HTREE_NUM -2002 #define SX_ERR_INVLD_HTREE_NUM -2002
#define SX_ERR_INVLD_EDGE_LINK_NUM -2003
#define SX_ERR_INVLD_CLOSES_PT -2004