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

workpieceHolePositioning version 1.4.5 :

Browse Source 
添加异物检测，通过errCode输出“有异物”和“无产品”结果
This commit is contained in:
jerryzeng 2026-04-16 20:50:18 +08:00
parent 06d1453749
commit f06bc65347
3 changed files with 79 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
sourceCode/SG_errCode.h
View File

@ -10,6 +10,7 @@
#define SG_ERR_ZERO_OBJECTS -1007
#define SG_ERR_LASER_DIR_NOT_SUPPORTED -1008
#define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009
#define SX_ERR_ZERO_OBJECTS -1010
//BQ_workpiece
#define SX_ERR_INVLD_VTREE_NUM -2001
@ -40,3 +41,6 @@
//ÌÇ´ü×Ó²ðÏß
#define SX_ERR_NO_MARK -2401
//拓普发工件定位
#define SX_ERR_UNKNOWN_OBJECT -2501

66
sourceCode/workpieceHolePositioning.cpp
View File

@ -15,7 +15,8 @@
//version 1.4.2 : 华航孔定位改进：（1）法向量计算改进（2）添加3x3平滑（3）修正了bug
//version 1.4.3 : 对1.3.0拓普发孔定位中调平Bug进行了修正
//version 1.4.4 : 对1.4.3一个小的修正， 不影响结果
std::string m_strVersion = "1.4.4";
//version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果
std::string m_strVersion = "1.4.5";
const char* wd_workpieceHolePositioningVersion(void)
{
return m_strVersion.c_str();
@ -594,6 +595,20 @@ void wd_getHoleInfo(
validObjects.push_back(objects[i]);
}
}
SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& listData)
{
if (listData.size() == 0)
return { 0,0,0,0 };
SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y };
for (int i = 0; i < (int)listData.size(); i++)
{
roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
}
return roi;
}
//工件孔定位-拓普发工件孔定位
void wd_workpieceHolePositioning(
@ -873,10 +888,12 @@ void wd_workpieceHolePositioning(
}
int workpieceNum = (int)allWorkpiece.size();
if (workpieceNum == 0)
{
*errCode = SX_ERR_ZERO_OBJECTS;
return;
}
//排序
//z方向排序
//寻找最高点
std::vector< WD_workpieceInfo> zSortWorkpiece;
double minZ = allWorkpiece[0].center.z;
for (int i = 1; i < workpieceNum; i++)
@ -884,6 +901,49 @@ void wd_workpieceHolePositioning(
if (minZ > allWorkpiece[i].center.z)
minZ = allWorkpiece[i].center.z;
}
//检测上层是否有残留
double zSliceTh = minZ - 1.5; //往上1.5mm
std::vector<SVzNL3DPosition> topLayerPts;
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
if ((scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z < zSliceTh))
{
SVzNL3DPosition a_pt;
a_pt.nPointIdx = (line << 16) | (j & 0xffff);
a_pt.pt3D = scanLines[line][j].pt3D;
topLayerPts.push_back(a_pt);
}
}
}
//进行聚类，判断聚类大小。大于一定门限判断为有异物
int topLayerClusterWin = 5;
double clusterDist = 5.0;
int distType = 1; //0 - 2d distance; 1- 3d distance
std::vector<std::vector< SVzNL3DPosition>> objClusters;
wd_pointClustering_speedUp(
topLayerPts,
lineNum, linePtNum, topLayerClusterWin, //搜索窗口
clusterDist,
distType, //0 - 2d distance; 1- 3d distance
objClusters //result
);
double vaidObjSize = workpiecePara.xLen * workpiecePara.yLen / 4.0;
for (int i = 0; i < (int)objClusters.size(); i++)
{
SSG_ROIRectD a_roi = _getClusterROI(objClusters[i]);
double size = (a_roi.right - a_roi.left) * (a_roi.bottom - a_roi.top);
if (size >= vaidObjSize)
{
*errCode = SX_ERR_UNKNOWN_OBJECT;
return;
}
}
//排序
//z方向排序
double topLayerTh = minZ + workpiecePara.H / 2;
for (int i = 0; i < workpieceNum; i++)
{

18
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

@ -554,7 +554,7 @@ SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T)
#define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_HOLE 1
#define TEST_COMPUTE_RT 0
#define TPF_TEST_GROUP 2
#define TPF_TEST_GROUP 3
//拓普发工件孔定位（工件定位)
void TuoPuFa_holePosition_test(void)
{
@ -562,10 +562,11 @@ void TuoPuFa_holePosition_test(void)
"F:/ShangGu/项目/冠钦项目/拓普发工件孔定位/拓普发点云/", //0
"F:/ShangGu/项目/冠钦项目/拓普发工件孔定位/拓普发点云2/", //1
"F:/ShangGu/项目/冠钦项目/拓普发工件孔定位/拓普发点云3/", //2
};
SVzNLRange fileIdx[TPF_TEST_GROUP] = {
{6,6}, {1, 15}
{6,6}, {1, 16}, {1,6}
};
const char* ver = wd_workpieceHolePositioningVersion();
@ -733,7 +734,7 @@ void TuoPuFa_holePosition_test(void)
#endif
#if TEST_COMPUTE_HOLE
for (int grp = 1; grp <= 1; grp++)
for (int grp = 1; grp <= 2; grp++)
{
SSG_planeCalibPara groundCalibPara;
//初始化成单位阵
@ -751,7 +752,7 @@ void TuoPuFa_holePosition_test(void)
groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i];
char calibFile[250];
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
groundCalibPara = _readCalibPara(calibFile);
//groundCalibPara = _readCalibPara(calibFile);
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
@ -799,7 +800,12 @@ void TuoPuFa_holePosition_test(void)
workpiecePositioning,
&errCode);
long t2 = (long)GetTickCount64();
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
if(errCode == SX_ERR_UNKNOWN_OBJECT)
printf("%s: %d(ms), 有异物残留!\n", _scan_file, (int)(t2 - t1));
else if(errCode == SX_ERR_ZERO_OBJECTS)
printf("%s: %d(ms), 无产品!\n", _scan_file, (int)(t2 - t1));
else
printf("%s: %d(ms), errCode=%d\n", _scan_file, (int)(t2 - t1), errCode);
//输出测试结果
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
_outputRGBDResult_RGBD(_scan_file, scanLines, workpiecePositioning);
@ -896,7 +902,7 @@ void HuaHang_holePosition_test(void)
int main()
{
#if 0
#if 1
TuoPuFa_holePosition_test();
#else
HuaHang_holePosition_test();