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

workpieceHolePositioning version 1.6.3 :

Browse Source 
拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。
This commit is contained in:
jerryzeng 2026-07-14 17:16:39 +08:00
parent 67f5f7fee6
commit 3cbb9ce640
4 changed files with 118 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
sourceCode/SG_featureGrow.cpp
View File

@ -776,7 +776,7 @@ void wd_getSegFeatureGrowingTrees_2(
{
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
{
if (i == 95)
if (i == 533)
int kkk = 1;
std::vector<SWD_segFeature>& a_lineFeatures = all_lineFeatures[i];
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
@ -820,7 +820,11 @@ void wd_getSegFeatureGrowingTrees_2(
{
feature_trees[m].treeState = TREE_STATE_DEAD;
if (len <= growParam.minLTypeTreeLen)
{
if (feature_trees[m].sLineIdx == 530)
int kkk = 1;
feature_trees.erase(feature_trees.begin() + m);
}
}
}
}

11
sourceCode/WD_holeDetection.cpp
View File

@ -10,8 +10,8 @@ void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
{
roi.left = a_pt.x;
roi.right = a_pt.x;
roi.left = a_pt.y;
roi.right = a_pt.y;
roi.top = a_pt.y;
roi.bottom = a_pt.y;
}
else
{
@ -107,7 +107,7 @@ void WD_getHoleInfo(
//ÌáÈ¡Ï߶ζ˵ãÌØÕ÷
for (int line = 0; line < lineNum; line++)
{
if (line == 95)
if (line == 533)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
@ -311,7 +311,8 @@ void WD_getHoleInfo(
SWD_segFeatureTree& a_tree = segTrees_v[i];
if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
int kkk = 1;
if ((treeInfo_v[i].roi.right > 260) && (treeInfo_v[i].roi.left < 270) && (treeInfo_v[i].roi.bottom > 10) && (treeInfo_v[i].roi.top < 20))
int kkk = 1;
int totalSize_v = treeInfo_v[i].treeType;
int commonSize = 0;
int hTreeIdx = -1;
@ -325,6 +326,8 @@ void WD_getHoleInfo(
}
if (hTreeIdx >= 0)
{
if ((treeInfo_h[hTreeIdx].roi.right > 260) && (treeInfo_h[hTreeIdx].roi.left < 270) && (treeInfo_h[hTreeIdx].roi.bottom > 10) && (treeInfo_h[hTreeIdx].roi.top < 20))
int kkk = 1;
int totalSize_h = treeInfo_h[hTreeIdx].treeType;
int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);

149
sourceCode/workpieceHolePositioning.cpp
View File

@ -32,7 +32,8 @@
//version 1.6.0 : 添加了玖瑞工件（单件）的姿态计算。
//version 1.6.1 : 添加了拓普发工件2、工件3、工件4的姿态计算。
//version 1.6.2 : 拓普发算法代码同步。
std::string m_strVersion = "HolePostion 1.6.2";
//version 1.6.3 : 拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。
std::string m_strVersion = "HolePostion 1.6.3";
const char* wd_workpieceHolePositioningVersion(void)
{
return m_strVersion.c_str();
@ -278,6 +279,7 @@ int distanceSearchObject(
SVzNLRect& roi2D,
SVzNLRangeD& distRange)
{
const double angleDiffTh = 15;
int result = -1;
int holeSize = (int)holes.size();
double minDistDiff = DBL_MAX;
@ -296,7 +298,7 @@ int distanceSearchObject(
if (angleDiff > 90)
angleDiff = 180 - angleDiff;
if ((zDiff < deltaZ) && (angleDiff < 45))
if ((zDiff < deltaZ) && (angleDiff < angleDiffTh))
{
if (minDistDiff > distDiff)
{
@ -541,6 +543,64 @@ double _getMinZ(std::vector<cv::Point3d>& Points3ds)
return minZ;
}
//去除扫描线上的凹坑
void _rmoveScanlineConcaves(
std::vector<SVzNL3DPosition>& lineData,
const double steppingScale,
const double invalidScale, //超出此尺度，方向角计算无效
const double minCornerTh, //凹坑对应的最小拐角
const double minConcaveDepth //最小凹坑不深度
)
{
//计算方向角
std::vector< SSG_pntDirAngle> ptDirAngles;
wd_computeDirAngle_wholeLine2(
lineData,
steppingScale,
invalidScale, //超出此尺度，方向角计算无效
ptDirAngles
);
//寻找拐点极大值
std::vector< SSG_pntDirAngle> cornerPeakP;
std::vector< SSG_pntDirAngle> cornerPeakM;
wd_searchCornerPeaks(
ptDirAngles,
lineData,
minCornerTh,
steppingScale * 2,
cornerPeakP,
cornerPeakM
);
//去除
const double removeDepthK = 0.75;
for (int idx = 0; idx < (int)cornerPeakP.size(); idx++)
{
SSG_pntDirAngle& a_peak = cornerPeakP[idx];
if ((abs(a_peak.forwardDiffZ) > minConcaveDepth) && (abs(a_peak.backwardDiffZ) > minConcaveDepth))
{
//寻找前后截止点：diffZ的 3/4
double zTh = a_peak.curr_z - abs(a_peak.backwardDiffZ) * removeDepthK;
for (int i = a_peak.pntIdx; i >= a_peak.backwardPntIdx; i--)
{
if (lineData[i].pt3D.z > zTh)
lineData[i].pt3D.z = 0;
else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
break;
}
zTh = a_peak.curr_z - abs(a_peak.forwardDiffZ) * removeDepthK;
for (int i = a_peak.pntIdx; i <= a_peak.forwardPntIdx; i++)
{
if (lineData[i].pt3D.z > zTh)
lineData[i].pt3D.z = 0;
else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
break;
}
}
}
}
//工件1: 孔定位-拓普发工件孔定位
void wd_workpieceHolePositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
@ -580,6 +640,19 @@ void wd_workpieceHolePositioning(
//调平，去除地面
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
}
//去除凹坑:只有垂直方向上进行。只对小孔有效（小孔在扫描时会形成凹坑
double steppingScale = workpiecePara.holeDiameter;
double invalidScale = workpiecePara.H; //超出此尺度，方向角计算无效
double minCornerTh = 60; //凹坑对应的最小拐角
double minConcaveDepth = 5.0;
for (int line = 0; line < lineNum; line++)
{
if (line == 436)
int kkk = 1;
//remove concave
_rmoveScanlineConcaves(scanLines[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
//_rmoveScanlineConcaves(scanLinesInput[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
}
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
@ -1111,64 +1184,6 @@ void _checkWorkpieceCentralHoles(
return;
}
//去除扫描线上的凹坑
void _rmoveScanlineConcaves(
std::vector<SVzNL3DPosition>& lineData,
const double steppingScale,
const double invalidScale, //超出此尺度，方向角计算无效
const double minCornerTh, //凹坑对应的最小拐角
const double minConcaveDepth //最小凹坑不深度
)
{
//计算方向角
std::vector< SSG_pntDirAngle> ptDirAngles;
wd_computeDirAngle_wholeLine2(
lineData,
steppingScale,
invalidScale, //超出此尺度，方向角计算无效
ptDirAngles
);
//寻找拐点极大值
std::vector< SSG_pntDirAngle> cornerPeakP;
std::vector< SSG_pntDirAngle> cornerPeakM;
wd_searchCornerPeaks(
ptDirAngles,
lineData,
minCornerTh,
steppingScale * 2,
cornerPeakP,
cornerPeakM
);
//去除
const double removeDepthK = 0.75;
for (int idx = 0; idx < (int)cornerPeakP.size(); idx++)
{
SSG_pntDirAngle& a_peak = cornerPeakP[idx];
if ((abs(a_peak.forwardDiffZ) > minConcaveDepth) && (abs(a_peak.backwardDiffZ) > minConcaveDepth))
{
//寻找前后截止点：diffZ的 3/4
double zTh = a_peak.curr_z - abs(a_peak.backwardDiffZ) * removeDepthK;
for (int i = a_peak.pntIdx; i >= a_peak.backwardPntIdx; i--)
{
if (lineData[i].pt3D.z > zTh)
lineData[i].pt3D.z = 0;
else if((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
break;
}
zTh = a_peak.curr_z - abs(a_peak.forwardDiffZ) * removeDepthK;
for (int i = a_peak.pntIdx; i <= a_peak.forwardPntIdx; i++)
{
if (lineData[i].pt3D.z > zTh)
lineData[i].pt3D.z = 0;
else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
break;
}
}
}
}
//工件2: 孔定位-拓普发工件孔定位
void wd_workpieceHolePositioning_2(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
@ -1214,10 +1229,10 @@ void wd_workpieceHolePositioning_2(
double steppingScale = workpiecePara.holeDiameter;
double invalidScale = workpiecePara.H; //超出此尺度，方向角计算无效
double minCornerTh = 45; //凹坑对应的最小拐角
double minConcaveDepth = 4.0;
double minConcaveDepth = 3.0;
for (int line = 0; line < lineNum; line++)
{
if (line == 367)
if (line == 436)
int kkk = 1;
//remove concave
_rmoveScanlineConcaves( scanLines[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
@ -1342,8 +1357,10 @@ void wd_workpieceHolePositioning_2(
SVzNL3DRangeD& a_roi = clustersRoi3D[i];
double L = a_roi.xRange.max - a_roi.xRange.min;
double W = a_roi.yRange.max - a_roi.yRange.min;
if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2.5) &&
(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2.5))
if ((a_roi.xRange.max > 260) && (a_roi.xRange.min < 270) && (a_roi.yRange.max > 10) && (a_roi.yRange.min < 20))
int kkk = 1;
if ((L > workpiecePara.holeDiameter * 0.25) && (L < workpiecePara.holeDiameter * 2.5) &&
(W > workpiecePara.holeDiameter * 0.25) && (W < workpiecePara.holeDiameter * 2.5))
validCluserIndexing.push_back(i);
}
//生成结果
@ -1417,6 +1434,10 @@ void wd_workpieceHolePositioning_2(
//holes[objIdx].radius = -1;
SWD_HoleInfo& p0 = holes[objIdx];
if ((p0.center.x > 230) && (p0.center.x < 240) && (p0.center.y > 10) && (p0.center.y < 25))
int kkk = 1;
if ((p0.center.x > 230) && (p0.center.x < 240) && (p0.center.y > 50) && (p0.center.y < 65))
int kkk = 1;
int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2,
quantiValue, roi2D, linePointRange);
if (idx1 < 0)

29
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

@ -115,9 +115,12 @@ void _removeZeroLines(std::vector<std::vector< SVzNL3DPosition>>& scanData)
lastLine = line;
}
if (firstLine < 0)
{
scanData.clear();
return;
}
if (lastLine < (lineNum - 1))
if ( (lastLine < (lineNum - 1)) && (lastLine > 0))
scanData.erase(scanData.begin() + lastLine + 1, scanData.end());
scanData.erase(scanData.begin(), scanData.begin() + firstLine);
@ -1527,9 +1530,9 @@ void TuoPuFa_holePosition_test2(void)
&errCode);
long t2 = (long)GetTickCount64();
if (errCode == SX_ERR_UNKNOWN_OBJECT)
printf("%s: %d(ms), 有异物残留!\n", _scan_file, (int)(t2 - t1));
printf("%s: %d(ms), 有异物残留! ...", _scan_file, (int)(t2 - t1));
else if (errCode == SX_ERR_ZERO_OBJECTS)
printf("%s: %d(ms), 无产品!\n", _scan_file, (int)(t2 - t1));
printf("%s: %d(ms), 无产品! ...", _scan_file, (int)(t2 - t1));
else
printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode);
//输出测试结果
@ -1948,7 +1951,7 @@ void TuoPuFa_holePosition_test5(void)
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
fidx =2;
//fidx =4;
char _scan_file[256];
sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
@ -1961,7 +1964,7 @@ void TuoPuFa_holePosition_test5(void)
WD_workpieceHoleParam workpiecePara;
workpiecePara.workpieceType = 0;
workpiecePara.holeDiameter = 10.0; //
workpiecePara.holeDiameter = 6.0; //
workpiecePara.holeDist_W = 32.0;
workpiecePara.holeDist_L = 40.0;
workpiecePara.xLen = 44;
@ -1980,8 +1983,8 @@ void TuoPuFa_holePosition_test5(void)
growParam.yDeviation_max = 4.0;
growParam.maxSkipDistance = 0.0;
growParam.zDeviation_max = 10.0;//
growParam.minLTypeTreeLen = 2.0; //mm
growParam.minVTypeTreeLen = 2.0; //mm
growParam.minLTypeTreeLen = 1.5; //mm
growParam.minVTypeTreeLen = 1.5; //mm
int errCode = 0;
std::vector< WD_workpieceInfo> workpiecePositioning;
@ -2698,6 +2701,7 @@ typedef enum
keSG_拓普发孔定位_工件3,
keSG_拓普发孔定位_工件4,
keSG_拓普发孔定位_工件5,
keSG_拓普发孔定位_工件全测试,
keSG_华航孔定位,
keSG_微力砂轮盘定位,
keSG_微力砂轮架子定位,
@ -2710,8 +2714,9 @@ int main()
//ESG_testMode testMode = keSG_拓普发孔定位_工件1;
//ESG_testMode testMode = keSG_拓普发孔定位_工件2;
//ESG_testMode testMode = keSG_拓普发孔定位_工件3;
ESG_testMode testMode = keSG_拓普发孔定位_工件4;
//ESG_testMode testMode = keSG_拓普发孔定位_工件4;
//ESG_testMode testMode = keSG_拓普发孔定位_工件5;
ESG_testMode testMode = keSG_拓普发孔定位_工件全测试;
//ESG_testMode testMode = keSG_华航孔定位;
//ESG_testMode testMode = keSG_微力砂轮盘定位;
//ESG_testMode testMode = keSG_微力砂轮架子定位;
@ -2729,6 +2734,14 @@ int main()
TuoPuFa_holePosition_test4();
else if (keSG_拓普发孔定位_工件5 == testMode)
TuoPuFa_holePosition_test5();
else if (keSG_拓普发孔定位_工件全测试 == testMode)
{
TuoPuFa_holePosition_test();
TuoPuFa_holePosition_test2();
TuoPuFa_holePosition_test3();
TuoPuFa_holePosition_test4();
TuoPuFa_holePosition_test5();
}
else if (keSG_华航孔定位 == testMode)
HuaHang_holePosition_test();
else if (keSG_微力砂轮盘定位 == testMode)