From 14ba0f09397d805a25ec07ed20b2c8b99e29336b Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 25 Jun 2026 16:11:04 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20version=201.5.5=20:?= =?UTF-8?q?=20=E5=B1=B1=E4=B8=9C=E6=9C=AC=E4=BA=8B=E6=9C=BA=E7=94=B5?= =?UTF-8?q?=E8=BD=AE=E8=83=8E=E5=AE=9A=E4=BD=8D=E3=80=82=EF=BC=881?= =?UTF-8?q?=EF=BC=89=E6=B7=BB=E5=8A=A0=E4=BA=86=E8=BD=AE=E8=83=8E=E5=86=85?= =?UTF-8?q?=E5=BE=84=E5=A4=A7=E5=B0=8F=E7=9A=84=E8=BE=93=E5=87=BA=EF=BC=8C?= =?UTF-8?q?=20=EF=BC=882=EF=BC=89=E6=8C=89=E9=AB=98=E5=BA=A6=E6=8E=92?= =?UTF-8?q?=E5=BA=8F=E3=80=82=E6=9C=80=E4=B8=8A=E9=9D=A2=E7=9A=84=E7=9B=AE?= =?UTF-8?q?=E6=A0=87=E6=8E=92=E5=9C=A8=E7=AC=AC=E4=B8=80=E4=B8=AA=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_baseAlgo_Export.h | 3 + sourceCode/SG_baseDataType.h | 2 + sourceCode/SG_baseFunc.cpp | 20 + sourceCode/workpieceHolePositioning.cpp | 450 +++++++++++++++++- sourceCode/workpieceHolePositioning_Export.h | 18 + .../workpieceHolePositioning_test.cpp | 162 ++++++- 6 files changed, 650 insertions(+), 5 deletions(-) diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 775d1ec..c04266c 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -20,6 +20,9 @@ SG_APISHARED_EXPORT float vec3_computeVecAngle(const SVzNL3DPoint& a, const SVzN SG_APISHARED_EXPORT SVzNL3DPoint wd_ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9]); //二维点旋转 SG_APISHARED_EXPORT SVzNL2DPointD wd_pt2dRotate(SVzNL2DPointD pt2D_in, double rotateAngle); +//计算总体均值和总体标准差 +SG_APISHARED_EXPORT void CalcStats(const std::vector& data, SSG_meanVar& result); + /** * @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度 * （v 必须在平面内，自动使用简化版罗德里格斯） diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 5eead87..821cc80 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -463,6 +463,8 @@ typedef struct bool end_zRising;//终点z上跳标志 }SSG_RUN_EX; + +//统计信息：均值和标准差（开根号） typedef struct { double mean; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 0c96945..43516be 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -5,6 +5,7 @@ #include #endif // __WIN32 +#include #include #include #include @@ -89,6 +90,25 @@ SVzNL2DPointD wd_pt2dRotate(SVzNL2DPointD pt2D_in, double rotateAngle) return result; } +//计算总体均值和总体标准差 +void CalcStats(const std::vector& data, SSG_meanVar& result) +{ + size_t n = (int)data.size(); + if (n == 0) + return; + + double sum = std::accumulate(data.begin(), data.end(), 0.0); + double mean = sum / n; + + double sq = 0.0; + for (double v : data) sq += (v - mean) * (v - mean); + sq = sq / n; + + result.mean = mean; + result.var = sqrt(sq); + return; +} + /** * @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度 * （v 必须在平面内，自动使用简化版罗德里格斯） diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index c7295ea..da06a10 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -25,7 +25,8 @@ //version 1.5.2 : 淇宸ヤ欢鍒ゆ柇涓殑涓涓昏緫閿欒銆 //version 1.5.3 : 娣诲姞閮戝窞寰姏鐮傝疆鐩樺拰杞洏鏋跺畾浣岮PI銆 //version 1.5.4 : 娣诲姞灞变笢鏈簨鏈虹數杞儙瀹氫綅API銆 -std::string m_strVersion = "HolePostion 1.5.4"; +//version 1.5.5 : 灞变笢鏈簨鏈虹數杞儙瀹氫綅銆傦紙1锛夋坊鍔犱簡杞儙鍐呭緞澶у皬鐨勮緭鍑猴紝 锛2锛夋寜楂樺害鎺掑簭銆傛渶涓婇潰鐨勭洰鏍囨帓鍦ㄧ涓涓 +std::string m_strVersion = "HolePostion 1.5.5"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -2698,12 +2699,16 @@ void _getFlagIntervals_h( segs.push_back(a_run); } +bool _compareByTireZValue(WD_HolePositionInfo& a, WD_HolePositionInfo& b) +{ + return a.center.z < b.center.z; +} //灞变笢鏈簨鏈虹數杞儙涓績瀹氫綅 void sx_getTireHolePose( std::vector< std::vector>& scanLinesInput, const SSG_cornerParam cornerPara, - const SSG_planeCalibPara groundCalibPara, - const WD_tireParam tireParam, + const SSG_planeCalibPara groundCalibPara, + const WD_tireParam tireParam, std::vector& tirePositions, int* errCode) { @@ -3093,10 +3098,13 @@ void _getFlagIntervals_h( WD_HolePositionInfo a_holeInfo; a_holeInfo.center = { center.x, center.y, meanZ }; a_holeInfo.normDir = { 0.0, 0.0, 1.0 }; + a_holeInfo.holeR = radius; tirePositions.push_back(a_holeInfo); } } } + //鎸夐珮搴︽帓搴 + std::sort(tirePositions.begin(), tirePositions.end(), _compareByTireZValue); if (tirePositions.size() == 0) { @@ -3113,3 +3121,439 @@ void _getFlagIntervals_h( return; } + +//妯℃澘妫鏌 +void _templateChecking( + std::vector>& quantiData, + int ref_x, int ref_y, + std::vector>& templateMask, + std::vector& circleZ, + std::vector& centerZ) +{ + int templateX = (int)templateMask.size(); + int templateY = (int)templateMask[0].size(); + for (int x = 0; x < templateX; x++) + { + for (int y = 0; y < templateY; y++) + { + if (templateMask[x][y] == 0) + continue; + int maskX = x + ref_x; + int maskY = y + ref_y; + if (quantiData[maskX][maskY] < 1e-4) + continue; + + if (templateMask[x][y] == 1) + circleZ.push_back(quantiData[maskX][maskY]); + else if (templateMask[x][y] == 2) + centerZ.push_back(quantiData[maskX][maskY]); + } + } + return; +} + + +//瀹佹尝娴风憺椹畾瀛愯姱涓績瀹氫綅 +void sx_getRotorCorePose( + std::vector< std::vector>& scanLinesInput, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara groundCalibPara, + const WD_rotorAppParam rotorParam, + std::vector& rotorPositions, + 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; //娓呴浂锛岀敤浜巇ebug鏃惰褰曚俊鎭 + } + if (false == isGridData)//鏁版嵁涓嶆槸缃戞牸鏍煎紡 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + + //鍐呴儴鍙傛暟 + double removeGroundHeight = groundCalibPara.planeHeight - rotorParam.removeGroundHOffset; + 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銆佽绠椾腑闂村潗鏍 + //鍐呴儴鍙傛暟 + double maxDistTh = 5.0; + double minSegLen = 2.0; + SSG_cornerParam removeVertialPara = cornerPara; + removeVertialPara.scale = 5.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_perSeg_dirAngleMethod( + scanLines[line], + line, + maxDistTh, + minSegLen, + 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_perSeg_dirAngleMethod( + scanLines[line], + line, + maxDistTh, + minSegLen, + 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; + int minSegSize = 1; + 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 <= minSegSize) + { + 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 <= minSegSize) + { + 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) + { + scanLinesInput[line][j].pt3D.z = 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 minObj_L = rotorParam.basket_L * 0.8; + double maxObj_L = rotorParam.basket_L * 1.25; + double minObj_W = rotorParam.basket_W * 0.8; + double maxObj_W = rotorParam.basket_W * 1.25; + 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; + double len = w < h ? h : w; + double width = w > h ? h : w; + if ((len > minObj_L) && (width > minObj_W) && (len < maxObj_L) && (width < maxObj_W)) + { + 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; + } + + //鏈鍓嶉潰鐨勭洰鏍囦负绛愬瓙 + int basketClusterIdx = validObjIndice[0]; + for (int i = 1; i < (int)validObjIndice.size(); i++) + { + int idx = validObjIndice[i]; + if (objMeanZ[basketClusterIdx] > objMeanZ[idx]) + basketClusterIdx = idx; + } + std::vector< SVzNL3DPosition>& basketCluster = objClusters[basketClusterIdx]; + //鎻愬彇绛愬瓙鐨勫洓涓鐐癸紝鐢ㄤ簬纭畾鐩爣鎵鍦ㄧ殑杈 + + //鐢熸垚鎶曞奖鍥撅紝鏂逛究浣跨敤妯℃澘 + const double quantiScale = 1.0; // + SSG_ROIRectD& basketROI = objROIs[basketClusterIdx]; + int quantiW = (basketROI.right - basketROI.left) / quantiScale + 1; + if (quantiW % 2 == 1) + quantiW += 1; + int quantiH = (basketROI.bottom - basketROI.top) / quantiScale + 1; + if (quantiH % 2 == 1) + quantiH += 1; + + std::vector> quantiData; + quantiData.resize(quantiW); + for (int i = 0; i < quantiW; i++) + { + quantiData[i].resize(quantiH); + std::fill(quantiData[i].begin(), quantiData[i].end(), -1); + } + //閲忓寲 + 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.x >= basketROI.left) && (scanLines[line][j].pt3D.x < basketROI.right) && + (scanLines[line][j].pt3D.y >= basketROI.top) && (scanLines[line][j].pt3D.y < basketROI.bottom)) + { + int quantiX = (scanLines[line][j].pt3D.x - basketROI.left) / quantiScale; + int quantiY = (scanLines[line][j].pt3D.y - basketROI.top) / quantiScale; + quantiData[quantiX][quantiY] = scanLines[line][j].pt3D.z; + } + } + } + //鐢熸垚妯℃澘 + double template_minR = rotorParam.rotorDiameter - 2.0; + double template_maxR = rotorParam.rotorDiameter + 2.0; + double template_centerR = 10.0; + int template_pixR = (int)(template_maxR / quantiScale + 1); + int template_W = template_pixR * 2 + 1; + + double template_cx = (double)template_pixR * quantiScale; + double template_cy = template_cx; + std::vector> templateMask; + templateMask.resize(template_W); + for (int i = 0; i < template_W; i++) + templateMask[i].resize(template_W); + for (int i = 0; i < template_W; i++) + { + for (int j = 0; j < template_W; j++) + { + double x = (double)i * quantiScale; + double y = (double)j * quantiScale; + double dist = sqrt(pow(x - template_cx, 2) + pow(y - template_cy, 2)); + if ((dist >= template_minR) && (dist <= template_maxR)) + templateMask[i][j] = 1; + else if (dist <= template_centerR) + templateMask[i][j] = 2; + } + } + if ((template_W >= quantiW) || (template_W >= quantiH)) + { + *errCode = SG_ERR_ZERO_OBJECTS; + return; + } + + //鎼滅储鐩爣 + std::vector> statsCircle; + std::vector> statsCenter; + statsCircle.resize(quantiW); + statsCenter.resize(quantiW); + for (int i = 0; i < quantiW; i++) + { + statsCircle[i].resize(quantiH); + statsCenter[i].resize(quantiH); + } + + int searchStart_X = template_pixR; + int searchEnd_X = quantiW - 1 - template_pixR; + int searchStart_Y = template_pixR; + int searchEnd_Y = quantiH - 1 - template_pixR; + for (int x = searchStart_X; x < searchEnd_X; x++) + { + for (int y = searchStart_Y; y < searchEnd_Y; y++) + { + std::vector circleZ; + std::vector centerZ; + //妯℃澘妫鏌 + _templateChecking( + quantiData, + x - template_pixR, y - template_pixR, + templateMask, + circleZ, + centerZ); + //璁＄畻鍧囧煎拰鏂瑰樊 + SSG_meanVar satasInfo_circle = { 0.0, 0.0 }; + CalcStats(circleZ, satasInfo_circle); + statsCircle[x][y] = satasInfo_circle; + + SSG_meanVar satasInfo_center = { 0.0, 0.0 }; + CalcStats(centerZ, satasInfo_center); + statsCenter[x][y] = satasInfo_center; + } + } + + //鎼滅储鏂瑰樊鏋佸皬鍊肩偣, circle鍜宑enter鐨勬柟宸疮鍔犲悗鐨勬瀬灏忓 + //涓ゆ鎼滅储娉曪細鍏堟悳绱5x5鐨勶紝鍐嶆悳绱㈡ā鍧楀ぇ灏 + std::vector< SSG_2DValueI> varPeaks_5x5; + for (int i = 0; i < template_W; i++) + { + for (int j = 0; j < template_W; j++) + { + + } + } + + + //鏃嬭浆鍥炲師鍧愭爣绯 + + for (int i = 0; i < (int)rotorPositions.size(); i++) + { + rotorPositions[i].center = _ptRotate(rotorPositions[i].center, groundCalibPara.invRMatrix); + rotorPositions[i].normDir = _ptRotate(rotorPositions[i].normDir, groundCalibPara.invRMatrix); + } + + return; +} \ No newline at end of file diff --git a/sourceCode/workpieceHolePositioning_Export.h b/sourceCode/workpieceHolePositioning_Export.h index c5a10c6..e59a95d 100644 --- a/sourceCode/workpieceHolePositioning_Export.h +++ b/sourceCode/workpieceHolePositioning_Export.h @@ -37,8 +37,17 @@ typedef struct { SVzNL3DPoint center; SVzNL3DPoint normDir; //方向向量（归一化） + double holeR; }WD_HolePositionInfo; +typedef struct +{ + double rotorDiameter; + double removeGroundHOffset; //去除时的高度偏置，用于设置架子高度 + double basket_L; //零件筐-长 + double basket_W; //零件筐-宽 +}WD_rotorAppParam; + //读版本号 SG_APISHARED_EXPORT const char* wd_workpieceHolePositioningVersion(void); @@ -93,4 +102,13 @@ SG_APISHARED_EXPORT void sx_getTireHolePose( const SSG_planeCalibPara groundCalibPara, const WD_tireParam tireParam, std::vector& tirePositions, + int* errCode); + +//宁波海瑞马定子芯中心定位 +SG_APISHARED_EXPORT void sx_getRotorCorePose( + std::vector< std::vector>& scanLinesInput, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara groundCalibPara, + const WD_rotorAppParam rotorParam, + std::vector& rotorPositions, int* errCode); \ No newline at end of file diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index a00b2c0..0b5c143 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -172,7 +172,7 @@ void _outputHoleInfo(char* fileName, std::vector< WD_HolePositionInfo>& holePosi int number = (int)holePositioning.size(); for (int i = 0; i < number; i++) { - sprintf_s(dataStr, 250, "瀛擾%d", i + 1); + sprintf_s(dataStr, 250, "瀛擾%d: R=%g", i + 1, holePositioning[i].holeR); sw << dataStr << std::endl; sprintf_s(dataStr, 250, " center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z); sw << dataStr << std::endl; @@ -673,7 +673,10 @@ void _outputRGBDResult_HoleInfo( size = 10; for (int i = 0; i < linePtNum; i++) { - rgb = {255, 0, 0}; + if (i == 0) + rgb = { 255,255,0 }; + else + rgb = {0, 0, 255}; float x = (float)objects[i].pt3D.x; float y = (float)objects[i].pt3D.y; float z = (float)objects[i].pt3D.z; @@ -1436,6 +1439,7 @@ void Benshi_tirePosition_test(void) #endif } + typedef enum { keSG_鎷撴櫘鍙戝瓟瀹氫綅 = 0, @@ -1443,8 +1447,159 @@ typedef enum keSG_寰姏鐮傝疆鐩樺畾浣, keSG_寰姏鐮傝疆鏋跺瓙瀹氫綅, keSG_灞变笢鏈簨杞儙瀹氫綅, + keSG_瀹佹尝娴风憺椹浆瀛愯姱瀹氫綅, } ESG_testMode; +//灞变笢鏈簨鏈虹數杞儙瀹氫綅 +#define HAIRUIMA_COMPUTE_CALIB_PARA 0 +#define HAIRUIMA_HOLE_POSITION 1 + +#define HAIRUIMA_TIRE_TEST_GROUP 1 +void HaiRuiMa_rotorCorePosition_test(void) +{ + const char* dataPath[BENSHI_TIRE_TEST_GROUP] = { + + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/瀹佹尝娴风憺椹/杞瓙閽㈣姱/鏁版嵁/", //0 + }; + + SVzNLRange fileIdx[BENSHI_TIRE_TEST_GROUP] = { + {1,83}, + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if HAIRUIMA_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_Jl26C477.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); + } + printf("%s: processing...\n", _scan_file); + 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 HAIRUIMA_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_Jl26C477.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_rotorAppParam rotorParam; + rotorParam.rotorDiameter = 76; + rotorParam.removeGroundHOffset = 100.0; //鍘婚櫎鏃剁殑楂樺害鍋忕疆锛岀敤浜庤缃灦瀛愰珮搴 + rotorParam.basket_L = 770; + rotorParam.basket_W = 580; + int errCode = 0; + std::vector rotorPositions; + sx_getRotorCorePose( + scanLines, + cornerParam, + groundCalibPara, + rotorParam, + rotorPositions, + &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, rotorPositions, dirLen); + sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); + _outputHoleInfo(_scan_file, rotorPositions); + } + } +#endif +} + + int main() { //ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅; @@ -1452,6 +1607,7 @@ int main() //ESG_testMode testMode = keSG_寰姏鐮傝疆鐩樺畾浣; //ESG_testMode testMode = keSG_寰姏鐮傝疆鏋跺瓙瀹氫綅; ESG_testMode testMode = keSG_灞变笢鏈簨杞儙瀹氫綅; + //ESG_testMode testMode = keSG_瀹佹尝娴风憺椹浆瀛愯姱瀹氫綅; if (keSG_鎷撴櫘鍙戝瓟瀹氫綅 == testMode) TuoPuFa_holePosition_test(); @@ -1463,4 +1619,6 @@ int main() Weili_grindingDiscRackPosition_test(); else if (keSG_灞变笢鏈簨杞儙瀹氫綅 == testMode) Benshi_tirePosition_test(); + else if (keSG_瀹佹尝娴风憺椹浆瀛愯姱瀹氫綅 == testMode) + HaiRuiMa_rotorCorePosition_test(); }