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

workpieceHolePositioning version 1.5.0 :

Browse Source 
修正工件Y方向矢量计算的一个Bug（不在工件表面平面内）
This commit is contained in:
jerryzeng 2026-04-27 14:09:06 +08:00
parent f517332278
commit 2757c09682
2 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
sourceCode/workpieceHolePositioning.cpp
View File

@ -20,7 +20,8 @@
//version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入
//version 1.4.8 : 将工件法向调整为垂直于工件表面
//version 1.4.9 : 修正工件法向计算的一个Bug
std::string m_strVersion = "1.4.9";
//version 1.5.0 : 修正工件Y方向矢量计算的一个Bug（不在工件表面平面内）
std::string m_strVersion = "1.5.0";
const char* wd_workpieceHolePositioningVersion(void)
{
return m_strVersion.c_str();
@ -895,7 +896,7 @@ void wd_workpieceHolePositioning(
if (idx1 < 0)
continue;
SVzNLRangeD angleRange = { 85, 95 }; //垂直，5度范围
SVzNLRangeD angleRange = { 80, 100 }; //垂直，5度范围
SWD_HoleInfo& p1 = holes[idx1];
//搜索最接近distance且角度为angle的目标, 以角度为优先
int idx2 = angleConditionDistanceSearch(
@ -965,6 +966,7 @@ void wd_workpieceHolePositioning(
//计算工件法向
SVzNL3DPoint normalDir;
SVzNL3DPoint vec_norm;
SVzNL3DPoint y_dir;
if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
{
//计算面参数: z = Ax + By + C
@ -975,11 +977,25 @@ void wd_workpieceHolePositioning(
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 };
SVzNL3DPoint vector2 = { 0, 0, 1 };
SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2);
SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib);
SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib);
if (rotate_p0.x < rotate_p1.x)
y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 };
else
y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 };
y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
}
else
{
normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
vec_norm = { 0, 0, 20 };
if (p0.center.x < p1.center.x)
y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
else
y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
}
WD_workpieceInfo a_workpiece;
@ -997,16 +1013,9 @@ void wd_workpieceHolePositioning(
}
a_workpiece.center = centerPoint;
SVzNL3DPoint y_dir;
if (p0.center.x < p1.center.x)
y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
else
y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
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 };
y_dir = vec3_normalize(y_dir);
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 };
a_workpiece.z_dir = normalDir;
allWorkpiece.push_back(a_workpiece);
}

4
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

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