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

hybridPosePositioning

Browse Source 
version 1.3.8 : 添加了输入栅格格式检查， 调整边缘目标为目标距料筐外廓边的距离在直径的1.1倍以内
This commit is contained in:
jerryzeng 2026-08-17 12:10:50 +08:00
parent a7ad978041
commit 0dc9a4904e
1 changed files with 27 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
sourceCode/hybridPosePositioning.cpp
View File

@ -18,7 +18,8 @@
//version 1.3.5 : 根据现场数据修正了转子钢芯定位算法中的bug
//version 1.3.6 : 在1.3.5版本的基础上根据现场数据修正了转子钢芯定位算法中的bug
//version 1.3.7 : 在1.3.6版本的基础上添加了ID， 与2D结果保持一致
std::string m_strVersion = "HybridPositioning 1.3.7";
//version 1.3.8 : 添加了输入栅格格式检查， 调整边缘目标为目标距料筐外廓边的距离在直径的1.1倍以内
std::string m_strVersion = "HybridPositioning 1.3.8";
const char* wd_hybridPositioningVersion(void)
{
return m_strVersion.c_str();
@ -956,8 +957,24 @@ void wd_HRM_RotorCorePositioning(
return;
}
//判断数据格式是否为grid。算法只能处理grid数据格式
int lineNum = (int)scanLinesInput.size();
int linePtNum = (int)scanLinesInput[0].size();
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLinesInput[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
int maxU = 0; //2D图像的最大Col
int maxV = 0; //2D图像的最大Row
for (int line = 0; line < lineNum; line++)
@ -1423,7 +1440,7 @@ void wd_HRM_RotorCorePositioning(
SWD_3DPointPair lineRight = linePoint[lineRightIdx];
//计算工件的位置：左上、右上、左下、右下四个角；上下左右四条边：中央部分
//机械臂对不同部分的工件会采用不同的抓取策略
double diameter = workpieceParam.radius * 2;
double distToSideTh = (workpieceParam.radius * 2) * 1.1;
double lineTop_a, lineTop_b, lineTop_c;
compute2ptLine(lineTop.pt1, lineTop.pt2, &lineTop_a, &lineTop_b, &lineTop_c);
double lineBtm_a, lineBtm_b, lineBtm_c;
@ -1449,14 +1466,14 @@ void wd_HRM_RotorCorePositioning(
double distLeft = computePtDistToLine(objCenter.x, objCenter.y, lineLeft_a, lineLeft_b, lineLeft_c);
double distRight = computePtDistToLine(objCenter.x, objCenter.y, lineRight_a, lineRight_b, lineRight_c);
if (distTop < diameter)
if (distTop < distToSideTh)
{
if (distLeft < diameter)
if (distLeft < distToSideTh)
{
zSortWorkpiece[i].workpieceType = 5; //左上
LTCorner.push_back(zSortWorkpiece[i]);
}
else if (distRight < diameter)
else if (distRight < distToSideTh)
{
zSortWorkpiece[i].workpieceType = 6; //右上
RTCorner.push_back(zSortWorkpiece[i]);
@ -1467,14 +1484,14 @@ void wd_HRM_RotorCorePositioning(
topSide.push_back(zSortWorkpiece[i]);
}
}
else if (distBtm < diameter)
else if (distBtm < distToSideTh)
{
if (distLeft < diameter)
if (distLeft < distToSideTh)
{
zSortWorkpiece[i].workpieceType = 7; //左下
LBCorner.push_back(zSortWorkpiece[i]);
}
else if (distRight < diameter)
else if (distRight < distToSideTh)
{
zSortWorkpiece[i].workpieceType = 8; //右下
RBCorner.push_back(zSortWorkpiece[i]);
@ -1485,12 +1502,12 @@ void wd_HRM_RotorCorePositioning(
btmSide.push_back(zSortWorkpiece[i]);
}
}
else if (distLeft < diameter)
else if (distLeft < distToSideTh)
{
zSortWorkpiece[i].workpieceType = 3; //左
leftSide.push_back(zSortWorkpiece[i]);
}
else if (distRight < diameter)
else if (distRight < distToSideTh)
{
zSortWorkpiece[i].workpieceType = 4; //右
rightSide.push_back(zSortWorkpiece[i]);