diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index f3052b7..7f9a6db 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -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 + diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index ac98f46..bda8456 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -15,7 +15,8 @@ //version 1.4.2 : 鍗庤埅瀛斿畾浣嶆敼杩涳細锛1锛夋硶鍚戦噺璁＄畻鏀硅繘锛2锛夋坊鍔3x3骞虫粦锛3锛変慨姝ｄ簡bug //version 1.4.3 : 瀵1.3.0鎷撴櫘鍙戝瓟瀹氫綅涓皟骞矪ug杩涜浜嗕慨姝 //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 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> 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++) { diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 2c2a882..c2dba1e 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -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();