diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index 724c40b..b8f5cd7 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -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]);