diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index 59a22aa..b2e29bc 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -22,7 +22,8 @@ //version 1.4.9 : 修正工件法向计算的一个Bug //version 1.5.0 : 修正工件Y方向矢量计算的一个Bug（不在工件表面平面内） //version 1.5.1 : 异物检测时，检测高度由工件最小Z向上1.5mm调整为工件最小Z向上工件高度的一半，工件倾斜时容错性能更好。 -std::string m_strVersion = "1.5.1"; +//version 1.5.2 : 修正工件判断中的一个逻辑错误。 +std::string m_strVersion = "1.5.2"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -55,8 +56,79 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9]) return _r_pt; } +void _getLinePositions(SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, SVzNLRect& roi2D, SVzNLRangeD& distRange, std::vector& pts) +{ + double len = sqrt(pow(seed2.x - seed1.x, 2) + pow(seed2.y - seed1.y, 2)); + int x0 = (int)seed1.x - roi2D.left; + int y0 = (int)seed1.y - roi2D.top; + + int x1 = (int)seed2.x - roi2D.left; + int y1 = (int)seed2.y - roi2D.top; + + std::vector pts_all; + drawLine(x0, y0, x1, y1, pts_all); + + for (int i = 0; i < (int)pts_all.size(); i++) + { + double data = double((pts_all[i].x - x0) * (pts_all[i].x - x0) + (pts_all[i].y - y0) * (pts_all[i].y - y0)); + double dist = sqrt(data); + double dist_k = dist / len; + if ((dist_k >= distRange.min) && (dist_k <= distRange.max)) + pts.push_back(pts_all[i]); + } +} + +void _getLinePoints( + std::vector>& quantiValue, + SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, + SVzNLRect& roi2D, SVzNLRangeD& distRange, + std::vector& Points3ds) +{ + std::vector pts_2d; + _getLinePositions(seed1, seed2, roi2D, distRange, pts_2d); + for (int i = 0; i < (int)pts_2d.size(); i++) + { + cv::Point3f a_pt3d; + a_pt3d.x = (double)(pts_2d[i].x + roi2D.left) + 0.5; + a_pt3d.y = (double)(pts_2d[i].y + roi2D.top) + 0.5; + a_pt3d.z = quantiValue[pts_2d[i].x][pts_2d[i].y]; + if (a_pt3d.z > 1e-4) + Points3ds.push_back(a_pt3d); + } +} + +bool _validateLineZChange( + std::vector>& quantiValue, + SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, + SVzNLRect& roi2D, SVzNLRangeD& distRange, + double maxZChange + ) +{ + std::vector Points3ds; + _getLinePoints( + quantiValue, + seed1, seed2, + roi2D, distRange, + Points3ds); + + double refZ = (seed1.z + seed2.z) / 2; + for (int i = 0; i < (int)Points3ds.size(); i++) + { + double zDiff = abs(Points3ds[i].z - refZ); + if (zDiff > maxZChange) + return false; + } + return true; +} + //搜索最接近distance的目标 -int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, double distance, double distDeviation, double dirAngle, double deltaZ) +int distanceSearchObject( + SVzNL3DPoint seed, + std::vector& holes, + double distance, double distDeviation, double dirAngle, double deltaZ, + std::vector>& quantiValue, + SVzNLRect& roi2D, + SVzNLRangeD& distRange) { int result = -1; int holeSize = (int)holes.size(); @@ -80,8 +152,18 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector& holes, do { if (minDistDiff > distDiff) { - minDistDiff = distDiff; - minDistIndex = i; + //添加距离检测 + bool isValid = _validateLineZChange( + quantiValue, + seed, holes[i].center, + roi2D, distRange, + deltaZ + ); + if (true == isValid) + { + minDistDiff = distDiff; + minDistIndex = i; + } } } } @@ -95,7 +177,10 @@ int angleConditionDistanceSearch( SVzNL3DPoint seed, SVzNL3DPoint angleSide, std::vector& holes, double distance, double distDeviation, double deltaZ, - SVzNLRangeD angleRange) + SVzNLRangeD angleRange, + std::vector>& quantiValue, + SVzNLRect& roi2D, + SVzNLRangeD& distRange) { int result = -1; int holeSize = (int)holes.size(); @@ -134,8 +219,18 @@ int angleConditionDistanceSearch( double angleDiff = abs(angle - bestAngle); if (minAngleDeviateion > angleDiff) { - minAngleDeviateion = angleDiff; - minAngleIdx = idx; + //添加距离检测 + bool isValid = _validateLineZChange( + quantiValue, + seed, holes[idx].center, + roi2D, distRange, + deltaZ + ); + if (true == isValid) + { + minAngleDeviateion = angleDiff; + minAngleIdx = idx; + } } } } @@ -181,47 +276,6 @@ double _getMeanZ(std::vector>& quantiValue, SVzNL3DPoint see return (zSum / hist); } -void _getLinePositions(SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, SVzNLRect& roi2D, SVzNLRangeD& distRange, std::vector& pts) -{ - double len = sqrt(pow(seed2.x - seed1.x, 2) + pow(seed2.y - seed1.y, 2)); - int x0 = (int)seed1.x - roi2D.left; - int y0 = (int)seed1.y - roi2D.top; - - int x1 = (int)seed2.x - roi2D.left; - int y1 = (int)seed2.y - roi2D.top; - - std::vector pts_all; - drawLine( x0, y0, x1, y1, pts_all); - - for (int i = 0; i < (int)pts_all.size(); i++) - { - double data = double((pts_all[i].x - x0) * (pts_all[i].x - x0) + (pts_all[i].y - y0) * (pts_all[i].y - y0)); - double dist = sqrt(data); - double dist_k = dist / len; - if ((dist_k >= distRange.min) && (dist_k <= distRange.max)) - pts.push_back(pts_all[i]); - } -} - -void _getLinePoints( - std::vector>& quantiValue, - SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, - SVzNLRect& roi2D, SVzNLRangeD& distRange, - std::vector& Points3ds) -{ - std::vector pts_2d; - _getLinePositions(seed1, seed2, roi2D, distRange, pts_2d); - for (int i = 0; i < (int)pts_2d.size(); i++) - { - cv::Point3f a_pt3d; - a_pt3d.x = (double)(pts_2d[i].x + roi2D.left) + 0.5; - a_pt3d.y = (double)(pts_2d[i].y + roi2D.top) + 0.5; - a_pt3d.z = quantiValue[pts_2d[i].x][pts_2d[i].y]; - if(a_pt3d.z > 1e-4) - Points3ds.push_back(a_pt3d); - } -} - void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt) { if (a_pt.z > 1E-4) @@ -893,7 +947,8 @@ void wd_workpieceHolePositioning( holes[objIdx].radius = -1; SWD_HoleInfo& p0 = holes[objIdx]; - int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2); + int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2, + quantiValue, roi2D, linePointRange); if (idx1 < 0) continue; @@ -904,7 +959,8 @@ void wd_workpieceHolePositioning( p0.center, p1.center, holes, workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, - angleRange); + angleRange, + quantiValue, roi2D, linePointRange); if (idx2 < 0) continue; @@ -914,10 +970,20 @@ void wd_workpieceHolePositioning( p1.center, p0.center, holes, workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, - angleRange); + angleRange, + quantiValue, roi2D, linePointRange); if (idx3 < 0) continue; + SWD_HoleInfo& p3 = holes[idx3]; + + //交叉检查P0P1, P2P3的长度差 + double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2)); + double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2)); + double dist_k = dist_P2P3 / dist_P0P1; + if (dist_k > 2.0) + continue; + p1.radius = -1; p2.radius = -1; p3.radius = -1; diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index a9a34ef..410602d 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void) }; SVzNLRange fileIdx[TPF_TEST_GROUP] = { - {6,6}, {1, 16}, {1,17} + {6,6}, {1, 16}, {1,19} }; const char* ver = wd_workpieceHolePositioningVersion();