diff --git a/bagPositioning_test/bagPositioning_test.vcxproj b/bagPositioning_test/bagPositioning_test.vcxproj index a012fc7..c2a96f7 100644 --- a/bagPositioning_test/bagPositioning_test.vcxproj +++ b/bagPositioning_test/bagPositioning_test.vcxproj @@ -42,13 +42,13 @@ Application true - v142 + v143 Unicode Application false - v142 + v143 true Unicode diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index 9027afc..64331d4 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -2110,6 +2110,10 @@ void HaiRuiMa_rotorCorePositioning_test(void) sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); groundCalibPara = _readCalibPara(calibFile); + //读取料筐参数 + sprintf_s(calibFile, "%sbin_info.txt", dataPath[grp]); + WD_HRM_BinInfo binInfo = _readBinInfo(calibFile); + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { //fidx = 3; @@ -2137,6 +2141,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) wd_HRM_RotorCorePositioning( scanLines, objROIs, + binInfo, groundCalibPara, workpieceParam, workpiecePositions, diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 1123220..25af7df 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -29,6 +29,7 @@ #define SX_ERR_ZERO_OBJ_TOPLAYER -2102 #define SX_ERR_ZERO_OBJ_BTMLAYER -2103 #define SX_ERR_GET_INVALID_PALTE -2104 //ȡ +#define SX_ERR_BIN_EDGE_ERROR -2105 // #define SX_BAG_TRAY_EMPTY -2201 diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index 5b3fa22..2a03983 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -11,7 +11,8 @@ //version 1.1.0 : 滮汾 //version 1.2.0 : ˺׶ιλ汾ζλ //version 1.3.0 : ˺תӸоλ -std::string m_strVersion = "HybridPositioning 1.3.0"; +//version 1.3.1 : ˺תӸоλλԼ㣬ֳ룬ߺͽǵλãĿ +std::string m_strVersion = "HybridPositioning 1.3.1"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -799,6 +800,16 @@ typedef struct std::vector mappings;//ÿ2DܶӦ3D }mapping2DTo3D; +typedef struct +{ + double dist; + int idx; +}WD_indexingDistance; +bool _compareByDistance(WD_indexingDistance& a, WD_indexingDistance& b) +{ + return a.dist < b.dist; +} + bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b) { return a.center.x < b.center.x; @@ -834,6 +845,7 @@ void _getYTopLine( void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, + const WD_HRM_BinInfo binInfo, const SSG_planeCalibPara groundCalibPara, const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ std::vector< WD_workpieceInfo>& workpiecePositions, @@ -1174,15 +1186,268 @@ void wd_HRM_RotorCorePositioning( if (allWorkpiece[i].center.z < topLayerTh) zSortWorkpiece.push_back(allWorkpiece[i]); } - //ˮƽ - while (zSortWorkpiece.size() > 0) + + if (binInfo.binTopZ > 1e-4) { - std::vector< WD_workpieceInfo> firstLine; - std::vector< WD_workpieceInfo> restWorkpiece; - _getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius); - workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end()); - zSortWorkpiece.clear(); - zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end()); + //㹤λ + std::vector linePoint; + linePoint.resize(4); + linePoint[0].pt1 = binInfo.minRectVertex[0]; + linePoint[0].pt2 = binInfo.minRectVertex[1]; + linePoint[1].pt1 = binInfo.minRectVertex[1]; + linePoint[1].pt2 = binInfo.minRectVertex[2]; + linePoint[2].pt1 = binInfo.minRectVertex[2]; + linePoint[2].pt2 = binInfo.minRectVertex[3]; + linePoint[3].pt1 = binInfo.minRectVertex[3]; + linePoint[3].pt2 = binInfo.minRectVertex[0]; + int lineTopIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.y < binInfo.center.y) && (linePoint[i].pt2.y < binInfo.center.y)) + { + lineTopIdx = i; + break; + } + } + if (lineTopIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineTop = linePoint[lineTopIdx]; + linePoint.erase(linePoint.begin() + lineTopIdx);//ȥѾʶedge + + int lineBtmIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.y > binInfo.center.y) && (linePoint[i].pt2.y > binInfo.center.y)) + { + lineBtmIdx = i; + break; + } + } + if (lineBtmIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineBtm = linePoint[lineBtmIdx]; + linePoint.erase(linePoint.begin() + lineBtmIdx); //ȥѾʶedge + + int lineLeftIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.x < binInfo.center.x) && (linePoint[i].pt2.x < binInfo.center.x)) + { + lineLeftIdx = i; + break; + } + } + if (lineLeftIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineLeft = linePoint[lineLeftIdx]; + linePoint.erase(linePoint.begin() + lineLeftIdx); //ȥѾʶedge + + int lineRightIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.x > binInfo.center.x) && (linePoint[i].pt2.x > binInfo.center.x)) + { + lineRightIdx = i; + break; + } + } + if (lineRightIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineRight = linePoint[lineRightIdx]; + //㹤λãϡϡ¡ĸǣߣ벿 + //е۶ԲֵͬĹòͬץȡ + double diameter = workpieceParam.radius * 2; + 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; + compute2ptLine(lineBtm.pt1, lineBtm.pt2, &lineBtm_a, &lineBtm_b, &lineBtm_c); + double lineLeft_a, lineLeft_b, lineLeft_c; + compute2ptLine(lineLeft.pt1, lineLeft.pt2, &lineLeft_a, &lineLeft_b, &lineLeft_c); + double lineRight_a, lineRight_b, lineRight_c; + compute2ptLine(lineRight.pt1, lineRight.pt2, &lineRight_a, &lineRight_b, &lineRight_c); + std::vector< WD_workpieceInfo> topSide; //ϱ + std::vector< WD_workpieceInfo> btmSide; //± + std::vector< WD_workpieceInfo> leftSide; // + std::vector< WD_workpieceInfo> rightSide; //ұ + std::vector< WD_workpieceInfo> LTCorner; //Ͻ + std::vector< WD_workpieceInfo> RTCorner; //Ͻ + std::vector< WD_workpieceInfo> LBCorner; // + std::vector< WD_workpieceInfo> RBCorner; //ұ + std::vector< WD_workpieceInfo> centerRegion; //ұ + for (int i = 0; i < (int)zSortWorkpiece.size(); i++) //ϲĿз + { + SVzNL3DPoint& objCenter = zSortWorkpiece[i].center; + double distTop = computePtDistToLine(objCenter.x, objCenter.y, lineTop_a, lineTop_b, lineTop_c); + double distBtm = computePtDistToLine(objCenter.x, objCenter.y, lineBtm_a, lineBtm_b, lineBtm_c); + 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 (distLeft < diameter) + { + zSortWorkpiece[i].workpieceType = 5; // + LTCorner.push_back(zSortWorkpiece[i]); + } + else if (distRight < diameter) + { + zSortWorkpiece[i].workpieceType = 6; // + RTCorner.push_back(zSortWorkpiece[i]); + } + else + { + zSortWorkpiece[i].workpieceType = 1; // + topSide.push_back(zSortWorkpiece[i]); + } + } + else if (distBtm < diameter) + { + if (distLeft < diameter) + { + zSortWorkpiece[i].workpieceType = 7; // + LBCorner.push_back(zSortWorkpiece[i]); + } + else if (distRight < diameter) + { + zSortWorkpiece[i].workpieceType = 8; // + RBCorner.push_back(zSortWorkpiece[i]); + } + else + { + zSortWorkpiece[i].workpieceType = 2; // + btmSide.push_back(zSortWorkpiece[i]); + } + } + else if (distLeft < diameter) + { + zSortWorkpiece[i].workpieceType = 3; // + leftSide.push_back(zSortWorkpiece[i]); + } + else if (distRight < diameter) + { + zSortWorkpiece[i].workpieceType = 4; // + rightSide.push_back(zSortWorkpiece[i]); + } + else + { + zSortWorkpiece[i].workpieceType = 0; // + centerRegion.push_back(zSortWorkpiece[i]); + } + } + //벿ֹ + if (centerRegion.size() > 0) + { + std::vector distanceIndice; + distanceIndice.resize(centerRegion.size()); + for (int m = 0; m < (int)centerRegion.size(); m++) + { + distanceIndice[m].dist = sqrt(pow(centerRegion[m].center.x - binInfo.center.x, 2) + + pow(centerRegion[m].center.y - binInfo.center.y, 2)); + distanceIndice[m].idx = m; + } + std::sort(distanceIndice.begin(), distanceIndice.end(), _compareByDistance); + for (int m = 0; m < (int)distanceIndice.size(); m++) + { + int idx = distanceIndice[m].idx; + workpiecePositions.push_back(centerRegion[idx]); + } + } + if(topSide.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), topSide.begin(), topSide.end()); + if (btmSide.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), btmSide.begin(), btmSide.end()); + if(leftSide.size() >0) + workpiecePositions.insert(workpiecePositions.end(), leftSide.begin(), leftSide.end()); + if (rightSide.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), rightSide.begin(), rightSide.end()); + if(LTCorner.size() >0) + workpiecePositions.insert(workpiecePositions.end(), LTCorner.begin(), LTCorner.end()); + if (RTCorner.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), RTCorner.begin(), RTCorner.end()); + if (LBCorner.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), LBCorner.begin(), LBCorner.end()); + if (RBCorner.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), RBCorner.begin(), RBCorner.end()); + } + else + {//ˮƽ + while (zSortWorkpiece.size() > 0) + { + std::vector< WD_workpieceInfo> firstLine; + std::vector< WD_workpieceInfo> restWorkpiece; + _getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius); + workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end()); + zSortWorkpiece.clear(); + zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end()); + } } //תȥ diff --git a/sourceCode/hybridPosePositioning_Export.h b/sourceCode/hybridPosePositioning_Export.h index 58507c4..cd7c9d7 100644 --- a/sourceCode/hybridPosePositioning_Export.h +++ b/sourceCode/hybridPosePositioning_Export.h @@ -14,7 +14,7 @@ typedef struct typedef struct { - int workpieceType; + int workpieceType; //0:; 1-ϱ; 2-±; 3-; 4-ұ; 5-Ͻ; 6-Ͻ; 7-½; 8-½; SVzNL3DPoint center; SVzNL3DPoint z_dir; //zһ SVzNL3DPoint y_dir; //yһ @@ -60,6 +60,7 @@ SG_APISHARED_EXPORT void wd_lineDataR( SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, + const WD_HRM_BinInfo binInfo, const SSG_planeCalibPara groundCalibPara, const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ std::vector< WD_workpieceInfo>& workpiecePositions,