workpieceHolePositioning version 1.5.2 :
修正工件判断中的一个逻辑错误。
This commit is contained in:
parent
7063642782
commit
7a372b05a7
@ -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<SVzNL2DPoint>& 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<SVzNL2DPoint> 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<std::vector<double>>& quantiValue,
|
||||
SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
|
||||
SVzNLRect& roi2D, SVzNLRangeD& distRange,
|
||||
std::vector<cv::Point3f>& Points3ds)
|
||||
{
|
||||
std::vector<SVzNL2DPoint> 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<std::vector<double>>& quantiValue,
|
||||
SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
|
||||
SVzNLRect& roi2D, SVzNLRangeD& distRange,
|
||||
double maxZChange
|
||||
)
|
||||
{
|
||||
std::vector<cv::Point3f> 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<SWD_HoleInfo>& holes, double distance, double distDeviation, double dirAngle, double deltaZ)
|
||||
int distanceSearchObject(
|
||||
SVzNL3DPoint seed,
|
||||
std::vector<SWD_HoleInfo>& holes,
|
||||
double distance, double distDeviation, double dirAngle, double deltaZ,
|
||||
std::vector<std::vector<double>>& quantiValue,
|
||||
SVzNLRect& roi2D,
|
||||
SVzNLRangeD& distRange)
|
||||
{
|
||||
int result = -1;
|
||||
int holeSize = (int)holes.size();
|
||||
@ -80,8 +152,18 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& 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<SWD_HoleInfo>& holes,
|
||||
double distance, double distDeviation, double deltaZ,
|
||||
SVzNLRangeD angleRange)
|
||||
SVzNLRangeD angleRange,
|
||||
std::vector<std::vector<double>>& 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<std::vector<double>>& quantiValue, SVzNL3DPoint see
|
||||
return (zSum / hist);
|
||||
}
|
||||
|
||||
void _getLinePositions(SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, SVzNLRect& roi2D, SVzNLRangeD& distRange, std::vector<SVzNL2DPoint>& 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<SVzNL2DPoint> 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<std::vector<double>>& quantiValue,
|
||||
SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
|
||||
SVzNLRect& roi2D, SVzNLRangeD& distRange,
|
||||
std::vector<cv::Point3f>& Points3ds)
|
||||
{
|
||||
std::vector<SVzNL2DPoint> 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;
|
||||
|
||||
@ -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();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user