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

workpieceHolePositioning version 1.4.8 :

Browse Source 
将工件法向调整为垂直于工件表面
This commit is contained in:
jerryzeng 2026-04-27 12:36:18 +08:00
parent 36798e8754
commit 93c1cb7264
2 changed files with 117 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
sourceCode/workpieceHolePositioning.cpp
View File

@ -18,7 +18,8 @@
//version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果
//version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。
//version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入
std::string m_strVersion = "1.4.7";
//version 1.4.8 : 将工件法向调整为垂直于工件表面
std::string m_strVersion = "1.4.8";
const char* wd_workpieceHolePositioningVersion(void)
{
return m_strVersion.c_str();
@ -177,6 +178,46 @@ 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];
Points3ds.push_back(a_pt3d);
}
}
void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
{
if (a_pt.z > 1E-4)
@ -631,6 +672,17 @@ SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& listData)
return roi;
}
double _getMinZ(std::vector<cv::Point3f>& Points3ds)
{
double minZ = -1;
for (int i = 0; i < (int)Points3ds.size(); i++)
{
if ((minZ < 0) || (minZ > Points3ds[i].z))
minZ = Points3ds[i].z;
}
return minZ;
}
//工件孔定位-拓普发工件孔定位
void wd_workpieceHolePositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
@ -672,7 +724,7 @@ void wd_workpieceHolePositioning(
}
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines);
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
SVzNLRect roi2D;
roi2D.left = (int)roi3D.xRange.min;
roi2D.right = (int)roi3D.xRange.max;
@ -734,7 +786,7 @@ void wd_workpieceHolePositioning(
{
std::vector< SVzNL2DPoint> a_cluster;
SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };
int vTreeIdx = validObjects[i].data_0;
int hTreeIdx = validObjects[i].data_1;
for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
@ -816,7 +868,7 @@ void wd_workpieceHolePositioning(
//圆拟合
SVzNL3DPoint center;
double radius;
double err = fitCircleByLeastSquare(pointArray, center, radius);
double err = fitCircleByLeastSquare(pointArray, center, radius);
center.z = minZ;
SWD_HoleInfo a_hole;
a_hole.center = { center.x, center.y, center.z };
@ -826,8 +878,10 @@ void wd_workpieceHolePositioning(
//分割
//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
SVzNLRangeD linePointRange = { 0.3, 0.7 };
std::vector< WD_workpieceInfo> allWorkpiece;
double highest_z = -1;
for (int objIdx = 0; objIdx < objectSize; objIdx++)
{
if (holes[objIdx].radius < 0)
@ -835,7 +889,7 @@ 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);
if (idx1 < 0)
continue;
@ -845,7 +899,7 @@ void wd_workpieceHolePositioning(
int idx2 = angleConditionDistanceSearch(
p0.center, p1.center,
holes,
workpiecePara.holeDist_L, distDeviation, workpiecePara.H/2,
workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
angleRange);
if (idx2 < 0)
continue;
@ -870,7 +924,7 @@ void wd_workpieceHolePositioning(
SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
double rectR = 5.0;
double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H/2);
double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2);
if (z1 < 1e-4)
z1 = center_p0p1.z;
double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2);
@ -882,11 +936,50 @@ void wd_workpieceHolePositioning(
double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2);
if (z4 < 1e-4)
z4 = center_p2p3.z;
p0.center.z = (z1 + z2) / 2;
p1.center.z = (z1 + z3) / 2;
p2.center.z = (z2 + z4) / 2;
p3.center.z = (z3 + z4) / 2;
SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
(z1 + z2 + z3 + z4) / 4 };
//取工件表面的点
std::vector<cv::Point3f> Points3ds;
_getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds);
int size_1 = (int)Points3ds.size();
_getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds);
_getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds);
_getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds);
double minZ = _getMinZ(Points3ds);
if (minZ > 1e-4)
{
if ((highest_z < 0) || (highest_z > minZ))
highest_z = minZ;
}
//计算工件法向
SVzNL3DPoint normalDir;
SVzNL3DPoint vec_norm;
if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
{
//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
std::vector<double> res;
vzCaculateLaserPlane(Points3ds, res);
SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 };
vec_norm = vec3_normalize(vec_1);
vec_norm = vec3_multiply(vec_norm, 20.0);
normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z };
}
else
{
normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
vec_norm = { 0, 0, 20 };
}
WD_workpieceInfo a_workpiece;
a_workpiece.workpieceType = workpiecePara.workpieceType;
a_workpiece.holes.push_back(p0.center);
@ -897,12 +990,10 @@ void wd_workpieceHolePositioning(
for (int m = 0; m < 4; m++)
{
SVzNL3DPoint a_pt = a_workpiece.holes[m];
a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在z向
a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//法向
a_workpiece.holesDir.push_back(a_pt);
}
a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
(z1 + z2 + z3 + z4) / 4 };
a_workpiece.center = centerPoint;
SVzNL3DPoint y_dir;
if (p0.center.x < p1.center.x)
@ -912,10 +1003,11 @@ void wd_workpieceHolePositioning(
double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z + 20 };
a_workpiece.z_dir = normalDir;
allWorkpiece.push_back(a_workpiece);
}
int workpieceNum = (int)allWorkpiece.size();
if (workpieceNum == 0)
{
@ -924,16 +1016,18 @@ void wd_workpieceHolePositioning(
}
//寻找最高点
std::vector< WD_workpieceInfo> zSortWorkpiece;
double minZ = allWorkpiece[0].center.z;
for (int i = 1; i < workpieceNum; i++)
if (highest_z < 0)
{
if (minZ > allWorkpiece[i].center.z)
minZ = allWorkpiece[i].center.z;
highest_z = allWorkpiece[0].center.z;
for (int i = 1; i < workpieceNum; i++)
{
if (highest_z > allWorkpiece[i].center.z)
highest_z = allWorkpiece[i].center.z;
}
}
//检测上层是否有残留
double zSliceTh = minZ - 1.5; //往上1.5mm
double zSliceTh = highest_z - 1.5; //往上1.5mm
std::vector<SVzNL3DPosition> topLayerPts;
for (int line = 0; line < lineNum; line++)
{
@ -974,7 +1068,8 @@ void wd_workpieceHolePositioning(
//排序
//z方向排序
double topLayerTh = minZ + workpiecePara.H / 2;
std::vector< WD_workpieceInfo> zSortWorkpiece;
double topLayerTh = highest_z + workpiecePara.H / 2;
for (int i = 0; i < workpieceNum; i++)
{
if (allWorkpiece[i].center.z < topLayerTh)

4
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void)
};
SVzNLRange fileIdx[TPF_TEST_GROUP] = {
{6,6}, {1, 16}, {13,13}
{6,6}, {1, 16}, {1,17}
};
const char* ver = wd_workpieceHolePositioningVersion();
@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void)
#endif
#if TEST_COMPUTE_HOLE
for (int grp = 2; grp <= 2; grp++)
for (int grp = 1; grp <= 2; grp++)
{
SSG_planeCalibPara groundCalibPara;
//初始化成单位阵