diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index dec92d4..eab60f8 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -2353,7 +2353,7 @@ void HaiRuiMa_groundCalib_test(void) } //æ‹“æ™®å�‘工件孔定ä½�(工件定ä½�) -#define HRM_RotorCore_TEST_GROUP 6 +#define HRM_RotorCore_TEST_GROUP 7 void HaiRuiMa_rotorCorePositioning_test(void) { const char* dataPath[HRM_RotorCore_TEST_GROUP] = { @@ -2364,10 +2364,11 @@ void HaiRuiMa_rotorCorePositioning_test(void) "F:/ShangGu/项目/冠钦项目/å®�波海瑞马/转å­�钢芯/2026-09-海瑞马/09-14/", //3 "F:/ShangGu/项目/冠钦项目/å®�波海瑞马/转å­�钢芯/2026-09-海瑞马/09-14中/", //4 "F:/ShangGu/项目/冠钦项目/å®�波海瑞马/转å­�钢芯/2026-09-海瑞马/异常数æ�®/", //5 + "F:/ShangGu/项目/冠钦项目/å®�波海瑞马/转å­�钢芯/2026-09-海瑞马/10-09å°�/", //6 }; SVzNLRange fileIdx[HRM_RotorCore_TEST_GROUP] = { - {1,10}, {3,70},{9,9}, {51,51}, {1,54}, {1,6} + {1,10}, {3,70},{9,9}, {52,52}, {1,54}, {1,6}, {1,8} }; const char* ver = wd_hybridPositioningVersion(); @@ -2456,6 +2457,11 @@ void HaiRuiMa_rotorCorePositioning_test(void) workpieceParam.radius = 42.0; workpieceParam.height = 50.0; } + else if (grp == 6) + { + workpieceParam.radius = 32.0; + workpieceParam.height = 25.0; + } else { workpieceParam.radius = 45.0; diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index d922034..8d57c4e 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -29,7 +29,8 @@ //version 1.4.4 : ת×Ó¸Öо£¬Ìí¼ÓÁËÒì³£±£»¤ //version 1.4.5 : ת×Ó¸Öо£¬¸ô°å¼ì²âÄÚ²¿²ÎÊýÐ޸ģº2:1³éÑù£¬¾ÛÀà¾àÀëÃÅÏÞ5mm //version 1.4.6 : ת×Ó¸Öо£¬Ìí¼Ó¸ô°åϹ¤×÷²¿·Ö¿É¼ûʱÐé¼Ù¼ì²âµÄÂ˳ý£¬·ÇÖÐÑ벿·Ö¹¤×÷°´¸ß¶ÈÅÅÐò -std::string m_strVersion = "HybridPositioning 1.4.6"; +//version 1.4.7 : ת×Ó¸Öо£¬Ìí¼ÓÎÞ¸ô°å¼ì²â³öÀ´Ê±µÄÐé¼Ù¼ì²âµÄÂ˳ý +std::string m_strVersion = "HybridPositioning 1.4.7"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -1013,6 +1014,19 @@ double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRang return meanZ; } +double _getListMeanZ_2(std::vector< SVzNL3DPoint>& listData) +{ + if (listData.size() == 0) + return 0; + double meanZ = 0; + for (int i = 0; i < (int)listData.size(); i++) + { + meanZ += listData[i].z; + } + meanZ = meanZ / (double)listData.size(); + return meanZ; +} + //¼ì²âÁÏ¿ð±ßÔµ£¬ÒÔÖÐÐĵãΪ»ù×¼½øÐй滮 WD_HRM_BinInfo wd_HRM_getBinSize_2( std::vector< std::vector>& scanLines, @@ -1505,6 +1519,20 @@ SVzNL3DPoint _searchNearestPoint(std::vector& pointList, const return target; } +void _searchRCirclePoints(std::vector< std::vector>& scanData, const SVzNL3DPoint& seedPt, const double R, std::vector< SVzNL3DPoint>& resultPts) +{ + for (int i = 0; i < (int)scanData.size(); i++) + { + for (int j = 0; j < (int)scanData[i].size(); j++) + { + double dist = sqrt(pow(seedPt.x - scanData[i][j].pt3D.x, 2) + pow(seedPt.y - scanData[i][j].pt3D.y, 2)); + if ( (dist <= R) && (scanData[i][j].pt3D.z > 1e-4)) + resultPts.push_back(scanData[i][j].pt3D); + } + } + return; +} + void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, @@ -1880,7 +1908,7 @@ void wd_HRM_RotorCorePositioning( //Ìí¼Ó¶Ô¸ô°åϲ¿·Ö¿É¼û±»Ê¶±ðÄ¿±êµÄ¹ýÂË if ((spacerInfo.center.z > 1e-4)&& (a_workpiece.center.z > 1e-4)) //¸ô°å´æÔÚ { - //ËÑË÷Ó빤¼þÖÐÐÄ×î½Ó½üµÄµã + //ËÑË÷¸ô°åµãÔÆÓ빤¼þÖÐÐÄ×î½Ó½üµÄµã SVzNL3DPoint nearestPt = _searchNearestPoint(spacerPoints, a_workpiece.center); double dist = sqrt(pow(nearestPt.x - a_workpiece.center.x, 2) + pow(nearestPt.y - a_workpiece.center.y, 2)); if (dist < workpieceParam.radius * 0.3) @@ -1890,6 +1918,19 @@ void wd_HRM_RotorCorePositioning( a_workpiece.center = { 0.0, 0.0, 0.0 }; } } + else + { + //ËÑË÷µãÔÆÓ빤¼þÖÐÐÄ×î½Ó½üµÄµã + std::vector roiPoints; + _searchRCirclePoints(roiScanLines, a_workpiece.center, workpieceParam.radius * 0.2, roiPoints); + double meanZ = _getListMeanZ_2(roiPoints); + if (meanZ > 1e-4) + { + double heightDiff = meanZ - a_workpiece.center.z; + if (heightDiff < workpieceParam.height / 4) + a_workpiece.center = { 0.0, 0.0, 0.0 }; + } + } #ifdef _OUTPUT_DEBUG_DATA if (a_workpiece.center.z > 1e-4)