From 29c25997fd02daadbbcd1cc7430551855da4cb06 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 19 Jun 2026 21:47:18 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20version=201.5.4=20:?= =?UTF-8?q?=20=E6=B7=BB=E5=8A=A0=E5=B1=B1=E4=B8=9C=E6=9C=AC=E4=BA=8B?= =?UTF-8?q?=E6=9C=BA=E7=94=B5=E8=BD=AE=E8=83=8E=E5=AE=9A=E4=BD=8DAPI?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/workpieceHolePositioning.cpp | 518 +++++++++++++++++- sourceCode/workpieceHolePositioning_Export.h | 15 + .../workpieceHolePositioning_test.cpp | 200 ++++++- 3 files changed, 710 insertions(+), 23 deletions(-) diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index 04dee22..c7295ea 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -24,7 +24,8 @@ //version 1.5.1 : 异物检测时，检测高度由工件最小Z向上1.5mm调整为工件最小Z向上工件高度的一半，工件倾斜时容错性能更好。 //version 1.5.2 : 修正工件判断中的一个逻辑错误。 //version 1.5.3 : 添加郑州微力砂轮盘和轮盘架定位API。 -std::string m_strVersion = "HolePostion 1.5.3"; +//version 1.5.4 : 添加山东本事机电轮胎定位API。 +std::string m_strVersion = "HolePostion 1.5.4"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -2597,3 +2598,518 @@ SSG_pointPose sx_getDiscRackCenterPosition( resultPose.pose_z = _ptRotate(resultPose.pose_z, poseR.invRMatrix); return resultPose; } + +int _getMaxROI(std::vector& roiList) +{ + if (roiList.size() == 0) + return -1; + + int maxId = 0; + double maxSize = (roiList[0].right - roiList[0].left) * (roiList[0].bottom - roiList[0].top); + for (int i = 1; i < (int)roiList.size(); i++) + { + double size = (roiList[i].right - roiList[i].left) * (roiList[i].bottom - roiList[i].top); + if (maxSize < size) + { + maxId = i; + maxSize = size; + } + } + return maxId; +} + +void _getFlagIntervals_v( + std::vector>& flags, + int index, + const int maxSkipping, + std::vector& segs) +{ + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + int dataSize = (int)flags[index].size(); + for (int i = 0; i < dataSize; i++) + { + if (flags[index][i] >=0 ) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = flags[index][i]; + } + else + { + int gap = i - (a_run.start + a_run.len - 1); + if( (flags[index][i] == a_run.value) && (gap <= maxSkipping)) + { + a_run.len = i - a_run.start + 1; + } + else + { + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = a_run.value = flags[index][i]; + } + } + } + } + if (a_run.len > 0) + segs.push_back(a_run); +} + +void _getFlagIntervals_h( + std::vector>& flags, + int index, + const int maxSkipping, + std::vector& segs) +{ + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + int dataSize = (int)flags.size(); + for (int i = 0; i < dataSize; i++) + { + if (flags[i][index] >= 0) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = flags[i][index]; + } + else + { + int gap = i - (a_run.start + a_run.len - 1); + if ((flags[i][index] == a_run.value) && (gap <= maxSkipping)) + { + a_run.len = i - a_run.start + 1; + } + else + { + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = a_run.value = flags[i][index]; + } + } + } + } + if (a_run.len > 0) + segs.push_back(a_run); +} + +//山东本事机电轮胎中心定位 + void sx_getTireHolePose( + std::vector< std::vector>& scanLinesInput, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara groundCalibPara, + const WD_tireParam tireParam, + std::vector& tirePositions, + int* errCode) +{ + *errCode = 0; + + //生成数据副本，使用副本数据进行调平和后续处理 + int lineNum = (int)scanLinesInput.size(); + std::vector< std::vector> scanLines; + scanLines.resize(lineNum); + int linePtNum = (int)scanLinesInput[0].size(); + bool isGridData = true; + for (int i = 0; i < lineNum; i++) + { + if (linePtNum != (int)scanLinesInput[i].size()) + isGridData = false; + + scanLines[i].resize(scanLinesInput[i].size()); + std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法 + + for (int j = 0; j < (int)scanLinesInput[i].size(); j++) + scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息 + } + if (false == isGridData)//数据不是网格格式 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + double removeGroundHeight = groundCalibPara.planeHeight - tireParam.thickness * 0.1; + for (int i = 0; i < lineNum; i++) + { //行处理 + //调平，去除地面 + wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight); + } + + //产生水平扫描数据 + std::vector< std::vector> scanLines_h; + scanLines_h.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + scanLines_h[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用） + scanLines_h[j][line] = scanLines[line][j]; + scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y; + scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int line = 0; line < linePtNum; line++) + { + for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++) + scanLines_h[line][j].nPointIdx = j; + } + + //算法流程： + //1、检查垂直方向数据并去除 + //2、聚类 + //3、保留最前面目标 + //4、提取孔 + //5、拟合 + //6、计算中间坐标 + //内部参数 + SSG_cornerParam removeVertialPara = cornerPara; + removeVertialPara.scale = 10.0; + removeVertialPara.cornerTh = 60; + + std::vector> flags; + flags.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + flags[i].resize(linePtNum); + std::fill(flags[i].begin(), flags[i].end(), 0); + } + std::vector> zVertivalFlags; + for (int line = 0; line < lineNum; line++) + { + if (line == 700) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[line][i] = 1; + } + } + + std::vector> zVertivalFlags_h; + for (int line = 0; line < linePtNum; line++) + { + if (line == 1177) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines_h[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags_h.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[i][line] = 1; + } + } + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + { + scanLines[line][j].pt3D.z = 0; + scanLines_h[j][line].pt3D.z = 0; + } + } + } + //迭代一次 + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 5.0; + lineSegPara.segGapTh_y = 5.0; + lineSegPara.segGapTh_z = 5.0; + const int minSegLen = 5; + for (int line = 0; line < lineNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[line][idx0 + j] = 1; + } + } + } + for (int line = 0; line < linePtNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines_h[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[idx0 + j][line] = 1; + } + } + } + + //标注 + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用） + scanLines[line][j].nPointIdx = 0; + } + } + //将垂直线段去除 + std::vector< SVzNL3DPosition> validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + scanLines[line][j].pt3D.z = 0; + + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_vldPt; + a_vldPt.pt3D = scanLines[line][j].pt3D; + a_vldPt.nPointIdx = (line << 16) | (j & 0xffff); + validPoints.push_back(a_vldPt); + } + } + } + + //聚类 + int clusterCheckWin = 5; + double clusterDist = 5.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; //result + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, //搜索窗口 + clusterDist, + distType, + objClusters //result + ); + + //保留最前面的目标 + double minObjSize = tireParam.diameter * 0.8; + double maxObjSize = tireParam.diameter * 1.5; + const double topPlateMaxZRange = 100.0; + std::vector objMeanZ; + std::vector objROIs; + std::vector objZRange; + objMeanZ.resize(objClusters.size()); + objROIs.resize(objClusters.size()); + objZRange.resize(objClusters.size()); + std::vector validObjIndice; + for (int i = 0; i < (int)objClusters.size(); i++) + { + SSG_ROIRectD a_roi = _getListROI(objClusters[i]); + objROIs[i] = a_roi; + double w = a_roi.right - a_roi.left; + double h = a_roi.bottom - a_roi.top; + if ( (w > minObjSize) && (h > minObjSize) && (w < maxObjSize) && (h < maxObjSize)) + { + SVzNLRangeD zRange; + double meanZ = _getListMeanZ(objClusters[i], zRange); + objMeanZ[i] = meanZ; + objZRange[i] = zRange; + validObjIndice.push_back(i); + } + else + { + objMeanZ[i] = 0; + objZRange[i].max = -1.0; + objZRange[i].min = 0.0; + } + } + + if (validObjIndice.size() == 0) + { + *errCode = SG_ERR_ZERO_OBJECTS; + return; + } + + //标注 + //重新将flags设置为目标的mask + for (int i = 0; i < lineNum; i++) + std::fill(flags[i].begin(), flags[i].end(), -1); + for (int m = 0; m < (int)validObjIndice.size(); m++) + { + int objIdx = validObjIndice[m]; + for (int i = 0; i < (int)objClusters[objIdx].size(); i++) + { + int line = objClusters[objIdx][i].nPointIdx >> 16; + int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF; + scanLinesInput[line][ptIdx].nPointIdx = 2; + flags[line][ptIdx] = m; //indexing + } + } + //提取轮廓点 + int maxSkipping = 3; //间隔小于3的空白视为连续，消除扫描拉条子的影响 + std::vector> contourMasks; + contourMasks.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + contourMasks[i].resize(linePtNum); + std::fill(contourMasks[i].begin(), contourMasks[i].end(), -1); + } + //垂直方向 + for (int line = 0; line < lineNum; line++) + { + std::vector flagSegs; + _getFlagIntervals_v( + flags, + line, + maxSkipping, + flagSegs); + for (int j = 0; j < (int)flagSegs.size(); j++) + { + int idx1 = flagSegs[j].start; + int idx2 = flagSegs[j].start + flagSegs[j].len - 1; + contourMasks[line][idx1] = flagSegs[j].value; + contourMasks[line][idx2] = flagSegs[j].value; + } + } + //水平方向 + for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++) + { + std::vector flagSegs; + _getFlagIntervals_h( + flags, + ptIdx, + maxSkipping, + flagSegs); + for (int j = 0; j < (int)flagSegs.size(); j++) + { + int idx1 = flagSegs[j].start; + int idx2 = flagSegs[j].start + flagSegs[j].len - 1; + contourMasks[idx1][ptIdx] = flagSegs[j].value; + contourMasks[idx2][ptIdx] = flagSegs[j].value; + } + } + std::vector> contourPoints; + contourPoints.resize(validObjIndice.size()); + for (int line = 0; line < lineNum; line++) + { + for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++) + { + if (contourMasks[line][ptIdx] >= 0) + { + int cIdx = contourMasks[line][ptIdx]; + SVzNL3DPosition a_vldPt; + a_vldPt.pt3D = scanLines[line][ptIdx].pt3D; + a_vldPt.nPointIdx = (line << 16) | (ptIdx & 0xffff); + contourPoints[cIdx].push_back(a_vldPt); + } + } + } + //标记 + for (int m = 0; m < (int)contourPoints.size(); m++) + { + for (int j = 0; j < contourPoints[m].size(); j++) + { + int line = contourPoints[m][j].nPointIdx >> 16; + int ptIdx = contourPoints[m][j].nPointIdx & 0x0000FFFF; + scanLinesInput[line][ptIdx].nPointIdx = m + 3; + } + } + + //逐个计算目标姿态 + clusterDist = 10.0; //轮廓点聚类适当加大距离 + distType = 0; //0 - 2d distance; 1- 3d distance + for (int m = 0; m < (int)contourPoints.size(); m++) + { + std::vector> contourClusters; //result + wd_pointClustering_speedUp( + contourPoints[m], + lineNum, linePtNum, clusterCheckWin, //搜索窗口 + clusterDist, + distType, + contourClusters //result + ); + //计算ROI + std::vector coutourROIs; + for (int i = 0; i < (int)contourClusters.size(); i++) + { + SSG_ROIRectD a_roi = _getListROI(contourClusters[i]); + coutourROIs.push_back(a_roi); + } + //去掉最大的目标 + int maxId = _getMaxROI(coutourROIs); + if (maxId < 0) + continue; + double innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left; + double innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top; + while ((innerHole_w > minObjSize) || (innerHole_h > minObjSize)) + { + coutourROIs[maxId].right = 0; + coutourROIs[maxId].left = 0; + coutourROIs[maxId].bottom = 0; + coutourROIs[maxId].top = 0; + //继续找最大的目标，若大于门限，视为目标 + maxId = _getMaxROI(coutourROIs); + if (maxId < 0) + continue; + + innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left; + innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top; + } + + if ((innerHole_w > tireParam.diameter * 0.4) && (innerHole_h > tireParam.diameter * 0.4)) + { + //计算姿态 + SVzNLRangeD holezRange; + double meanZ = _getListMeanZ(contourClusters[maxId], holezRange); + //拟合圆心 + //圆最小二乘拟合 + SVzNL3DPoint center = { 0.0, 0.0, 0.0 }; + double radius = 0; + double errValue = fitCircleByLeastSquare_2( contourClusters[maxId], center, radius); + if ((radius > tireParam.diameter * 0.2) && (radius < minObjSize * 0.5)) + { + WD_HolePositionInfo a_holeInfo; + a_holeInfo.center = { center.x, center.y, meanZ }; + a_holeInfo.normDir = { 0.0, 0.0, 1.0 }; + tirePositions.push_back(a_holeInfo); + } + } + } + + if (tirePositions.size() == 0) + { + *errCode = SG_ERR_ZERO_OBJECTS; + return; + } + + //旋转回原坐标系 + for (int i = 0; i < (int)tirePositions.size(); i++) + { + tirePositions[i].center = _ptRotate(tirePositions[i].center, groundCalibPara.invRMatrix); + tirePositions[i].normDir = _ptRotate(tirePositions[i].normDir, groundCalibPara.invRMatrix); + } + + return; +} diff --git a/sourceCode/workpieceHolePositioning_Export.h b/sourceCode/workpieceHolePositioning_Export.h index 435db14..c5a10c6 100644 --- a/sourceCode/workpieceHolePositioning_Export.h +++ b/sourceCode/workpieceHolePositioning_Export.h @@ -27,6 +27,12 @@ typedef struct SVzNL3DPoint x_dir; //xһ }WD_workpieceInfo; +typedef struct +{ + double diameter; //ֱ̥ + double thickness; //̥ +}WD_tireParam; + typedef struct { SVzNL3DPoint center; @@ -78,4 +84,13 @@ SG_APISHARED_EXPORT WD_HolePositionInfo sx_getDiscHolePose( SG_APISHARED_EXPORT SSG_pointPose sx_getDiscRackCenterPosition( std::vector< std::vector>& src_scanLines, const SSG_cornerParam cornerPara, + int* errCode); + +//ɽ»̥Ķλ +SG_APISHARED_EXPORT void sx_getTireHolePose( + std::vector< std::vector>& scanLinesInput, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara groundCalibPara, + const WD_tireParam tireParam, + std::vector& tirePositions, int* errCode); \ No newline at end of file diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 3026e4e..a00b2c0 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -593,7 +593,8 @@ void _outputRGBDResult_RGBD_2( void _outputRGBDResult_HoleInfo( char* fileName, std::vector>& scanLines, - std::vector< WD_HolePositionInfo>& holePositioning) + std::vector< WD_HolePositionInfo>& holePositioning, + const double dirLen) { std::vector objects; int objNumber = (int)holePositioning.size(); @@ -642,7 +643,12 @@ void _outputRGBDResult_HoleInfo( if (pt3D->nPointIdx > 0) int kkk = 1; int flag = pt3D->nPointIdx & 0xffff; - if (flag > 0) + if (flag == 2) + { + rgb = { 0, 0, 200 }; + size = 2; + } + else if (flag > 2) { rgb = objColor[flag % 8]; // { 255, 97, 0 }; size = 5; @@ -681,12 +687,12 @@ void _outputRGBDResult_HoleInfo( for (int i = 0; i < objNumber; i++) { SVzNL3DPoint dirPt_1, dirPt_2; - dirPt_1 = { holePositioning[i].center.x - holePositioning[i].normDir.x * 20, - holePositioning[i].center.y - holePositioning[i].normDir.y * 20, - holePositioning[i].center.z - holePositioning[i].normDir.z * 20 }; - dirPt_2 = { holePositioning[i].center.x + holePositioning[i].normDir.x * 10, - holePositioning[i].center.y + holePositioning[i].normDir.y * 10, - holePositioning[i].center.z + holePositioning[i].normDir.z * 10 }; + dirPt_1 = { holePositioning[i].center.x - holePositioning[i].normDir.x * dirLen, + holePositioning[i].center.y - holePositioning[i].normDir.y * dirLen, + holePositioning[i].center.z - holePositioning[i].normDir.z * dirLen }; + dirPt_2 = { holePositioning[i].center.x + holePositioning[i].normDir.x * dirLen, + holePositioning[i].center.y + holePositioning[i].normDir.y * dirLen, + holePositioning[i].center.z + holePositioning[i].normDir.z * dirLen }; sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << dirPt_1.x << "," << dirPt_1.y << "," << dirPt_1.z << "}-"; sw << "{0,0}-{0,0}-"; @@ -1156,8 +1162,9 @@ void HuaHang_holePosition_test(void) long t2 = (long)GetTickCount64(); printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); //输出测试结果 + double dirLen = 20; sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); - _outputRGBDResult_HoleInfo(_scan_file, scanLines, holePositioning); + _outputRGBDResult_HoleInfo(_scan_file, scanLines, holePositioning, dirLen); sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); _outputHoleInfo(_scan_file, holePositioning); } @@ -1283,28 +1290,177 @@ void Weili_grindingDiscRackPosition_test(void) #endif } +//山东本事机电轮胎定位 +#define BENSHI_COMPUTE_CALIB_PARA 0 +#define BENSHI_HOLE_POSITION 1 + +#define BENSHI_TIRE_TEST_GROUP 1 +void Benshi_tirePosition_test(void) +{ + const char* dataPath[BENSHI_TIRE_TEST_GROUP] = { + + "F:/ShangGu/项目/冠钦项目/山东本事机电轮胎抓取/数据/", //0 + }; + + SVzNLRange fileIdx[BENSHI_TIRE_TEST_GROUP] = { + {1,19}, + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if BENSHI_COMPUTE_CALIB_PARA + int cvtGrp = 0; + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "%s调平.txt", dataPath[cvtGrp]); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + //结果进行验证 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvtGrp]); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "%sscanData_ground_calib_verify.txt", dataPath[cvtGrp]); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); +#if 0 + for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[cvtGrp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + { + //调平，去除地面 + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1); + } + sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + } +#endif + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if BENSHI_HOLE_POSITION + + for (int grp = 0; grp <= 0; grp++) + { + SSG_planeCalibPara groundCalibPara; + //初始化成单位阵 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + groundCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =17; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + + long t1 = (long)GetTickCount64();//统计时间 + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 60; //45度角 + cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 5.0; + cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 + cornerParam.jumpCornerTh_2 = 60; + + WD_tireParam tireParam; + tireParam.diameter = 500; + tireParam.thickness = 140; + + int errCode = 0; + std::vector tirePositions; + sx_getTireHolePose( + scanLines, + cornerParam, + groundCalibPara, + tireParam, + tirePositions, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + double dirLen = 200; + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDResult_HoleInfo(_scan_file, scanLines, tirePositions, dirLen); + sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); + _outputHoleInfo(_scan_file, tirePositions); + } + } +#endif +} + typedef enum { - keSG_测试_拓普发孔定位 = 0, - keSG_测试_华航孔定位, - keSG_测试_微力砂轮盘定位, - keSG_测试_微力砂轮架子定位, + keSG_拓普发孔定位 = 0, + keSG_华航孔定位, + keSG_微力砂轮盘定位, + keSG_微力砂轮架子定位, + keSG_山东本事轮胎定位, } ESG_testMode; int main() { - //ESG_testMode testMode = keSG_测试_拓普发孔定位; - //ESG_testMode testMode = keSG_测试_华航孔定位; - //ESG_testMode testMode = keSG_测试_微力砂轮盘定位; - ESG_testMode testMode = keSG_测试_微力砂轮架子定位; + //ESG_testMode testMode = keSG_拓普发孔定位; + //ESG_testMode testMode = keSG_华航孔定位; + //ESG_testMode testMode = keSG_微力砂轮盘定位; + //ESG_testMode testMode = keSG_微力砂轮架子定位; + ESG_testMode testMode = keSG_山东本事轮胎定位; - - if (keSG_测试_拓普发孔定位 == testMode) + if (keSG_拓普发孔定位 == testMode) TuoPuFa_holePosition_test(); - else if (keSG_测试_华航孔定位 == testMode) + else if (keSG_华航孔定位 == testMode) HuaHang_holePosition_test(); - else if (keSG_测试_微力砂轮盘定位 == testMode) + else if (keSG_微力砂轮盘定位 == testMode) Weili_grindingDiscHolePosition_test(); - else if (keSG_测试_微力砂轮架子定位 == testMode) + else if (keSG_微力砂轮架子定位 == testMode) Weili_grindingDiscRackPosition_test(); + else if (keSG_山东本事轮胎定位 == testMode) + Benshi_tirePosition_test(); }