From fe7811b407c73f3deecd33b04ea0a3120577f91f Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 28 Jul 2026 18:17:48 +0800 Subject: [PATCH] =?UTF-8?q?hybridPosePositioning=20version=201.3.0=20:=20?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=B5=B7=E7=91=9E=E9=A9=AC=E8=BD=AC?= =?UTF-8?q?=E5=AD=90=E9=92=A2=E8=8A=AF=E5=AE=9A=E4=BD=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hybridPosePositioning_test.cpp | 32 ++- sourceCode/hybridPosePositioning.cpp | 202 ++++++++++++++---- 2 files changed, 178 insertions(+), 56 deletions(-) diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index dd3af70..9027afc 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -673,7 +673,8 @@ void _outputRGBDResult_XYZ_RGBD_binInfo( void _outputRGBDResult_XYZ_RGBD_workpieceInfo( char* fileName, std::vector>& scanLines, - std::vector& poseInfo) + std::vector& poseInfo, + double dirLen) { int objNumber = 0; if (poseInfo.size()>0) @@ -769,7 +770,7 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo( //输出方向线条 rgb = { 250, 0, 0 }; size = 2; - double dirLen = 500; + //double dirLen = 500; for (int m = 0; m < (int)poseInfo.size(); m++) { SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen, @@ -2111,7 +2112,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 0; + //fidx = 3; char _scan_file[256]; sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx); std::vector> scanLines; @@ -2152,11 +2153,24 @@ void HaiRuiMa_rotorCorePositioning_test(void) else printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); //输出测试结果 - sprintf_s(_scan_file, "%sresult/debug_%d_data.txt", dataPath[grp], fidx); - _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions); sprintf_s(calibFile, "%sresult/LaserLine%d_workpiece_info.txt", dataPath[grp], fidx); _outputWorkpieceInfo(calibFile, workpiecePositions); - + //旋转到调平坐标系 + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.planeCalib); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.planeCalib); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.planeCalib); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.planeCalib); + workpiecePositions[i].x_dir = rpt; + } + sprintf_s(_scan_file, "%sresult/result_%d.txt", dataPath[grp], fidx); + _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions, 50); #if 0 sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx); double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // @@ -2364,7 +2378,7 @@ void HaiRuiMa_workpieceSizeCompute_test(void) a_pose.value = workpieceInfo.workpieceRadius; a_pose.z_dir = { 0, 0, 1.0 }; poseInfo.push_back(a_pose); - _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo); + _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo, 500); sprintf_s(_scan_file, "%sresult/%d_workpiece_info.txt", dataPath[grp], fidx); _outputWorkpieceInfo(_scan_file, workpieceInfo); @@ -2501,8 +2515,8 @@ typedef enum int main() { //ESG_testMode testMode = keSG_2D3D定位_海瑞马_地面调平; - ESG_testMode testMode = keSG_2D3D定位_海瑞马_锥形工件; - //ESG_testMode testMode = keSG_2D3D定位_海瑞马_转子芯; + //ESG_testMode testMode = keSG_2D3D定位_海瑞马_锥形工件; + ESG_testMode testMode = keSG_2D3D定位_海瑞马_转子芯; //ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛料筐定位; //ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛工件尺寸测量; //ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛位置规划; diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index 37de392..5b3fa22 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -10,7 +10,8 @@ //version 1.0.0 : base version release to customer //version 1.1.0 : 滮汾 //version 1.2.0 : ˺׶ιλ汾ζλ -std::string m_strVersion = "HybridPositioning 1.1.0"; +//version 1.3.0 : ˺תӸоλ +std::string m_strVersion = "HybridPositioning 1.3.0"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -348,15 +349,21 @@ WD_workpieceInfo _computeWorkpiecePose( for (int i = 0; i < lineNum; i++) flags[i].resize(linePtNum); + //ڲ + double topSeg_minWidth = 5.0; // + double topSeg_maxWidth = 20.0; + SSG_lineSegParam filterSegPara; filterSegPara.distScale = 0; - filterSegPara.segGapTh_y = DBL_MAX; + filterSegPara.segGapTh_y = topSeg_maxWidth; filterSegPara.segGapTh_z = workpieceParam.height / 2; //ֱ std::vector> vScanLineSegs; for (int line = 0; line < lineNum; line++) { + if (line == 83) + int kkk = 1; //ֶ std::vector simpleSegs; wd_getLineDataIntervals(scanLines[line], filterSegPara, simpleSegs); @@ -430,6 +437,8 @@ WD_workpieceInfo _computeWorkpiecePose( exSegs.push_back(a_seg); } } + if (exSegs.size() == 0) + return a_pose; //ȥ͵ĶΣεmeanZﵽ߶ȵһ룬Ϊ͵Ķ int highestSeg = 0; for (int m = 1; m < (int)exSegs.size(); m++) @@ -562,10 +571,11 @@ WD_workpieceInfo _computeWorkpiecePose( SVzNL3DPoint centerPoint = scanLines[centerPosition.lineIdx][centerPosition.ptIdx].pt3D; std::vector< SWD_indexingSeg> segsTop; //ɹϵĵ: ȴһޣ룩߽㲻DZ߽ - double minWidth = 5.0; // //ֱɨ账 for (int line = 0; line < lineNum; line++) { + if (line == 83) + int kkk = 1; int mostLeftSegId = -1; //ڴֱɨ裬ʵ int mostRightSegId = -1;//ڴֱɨ裬ʵ for (int i = 0; i < (int)vScanLineSegs[line].size(); i++) @@ -577,11 +587,13 @@ WD_workpieceInfo _computeWorkpiecePose( double width = abs(scanLines[line][idx1].pt3D.y - scanLines[line][idx2].pt3D.y); SVzNL3DPoint segCenter = exSeg.centerPoint.pt3D; double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2)); - if ( (width > minWidth) && (distToCircleCenter < workpieceParam.radius)) + if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius)) { - if (mostLeftSegId < 0) + if ((mostLeftSegId < 0) && (idx2 < centerPosition.ptIdx) &&(idx1 > 0 )) mostLeftSegId = i; - mostRightSegId = i; + + if( (idx1 > centerPosition.ptIdx) && (idx2 < linePtNum - 1)) + mostRightSegId = i; } } if (mostLeftSegId >= 0) @@ -627,11 +639,13 @@ WD_workpieceInfo _computeWorkpiecePose( double width = abs(hLines[line][idx1].pt3D.y - hLines[line][idx2].pt3D.y); SVzNL3DPoint segCenter = a_seg.centerPoint.pt3D; double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2)); - if ( (width > minWidth) && (distToCircleCenter < workpieceParam.radius)) + if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius)) { - if (mostLeftSegId < 0) + if ((mostLeftSegId < 0) && (idx2 < centerPosition.lineIdx) && (idx1 > 0)) mostLeftSegId = i; - mostRightSegId = i; + + if( (idx1 > centerPosition.lineIdx) && (idx2 < lineNum - 1)) + mostRightSegId = i; } } if (mostLeftSegId >= 0) @@ -664,25 +678,6 @@ WD_workpieceInfo _computeWorkpiecePose( } } - //ɱעҪϢ - for (int i = 0; i < (int)segsTop.size(); i++) - { - SWD_indexingSeg& a_seg = segsTop[i]; - if (a_seg.hScanFlag == 0) - { - SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx }; - centerPtIndice.push_back(pos_center); - SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx }; - outsideEdgePtIndice.push_back(pos_edge); - } - else - { - SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx}; - centerPtIndice.push_back(pos_center); - SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx}; - outsideEdgePtIndice.push_back(pos_edge); - } - } // int clusterCheckWin = 5; @@ -754,15 +749,48 @@ WD_workpieceInfo _computeWorkpiecePose( for (int m = 0; m < nodeSize; m++) pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D); } - SVzNL3DPoint center; double radius; - double err = fitCircleByLeastSquare(pointArray,center,radius); + double err = fitCircleByLeastSquare(pointArray, center, radius); + + //һ + pointArray.clear(); + for (int i = 0; i < (int)objClusters.size(); i++) + { + std::vector< SWD_indexingSeg>& a_cluster = objClusters[i]; + int nodeSize = (int)a_cluster.size(); + for (int m = 0; m < nodeSize; m++) + { + SWD_indexingSeg& a_seg = a_cluster[m]; + double r1 = sqrt(pow(center.x - a_seg.outsideEdgePoint.pt3D.x, 2) + pow(center.y - a_seg.outsideEdgePoint.pt3D.y, 2)); + if (r1 < workpieceParam.radius * 1.05) + { + pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D); + //ɱעҪϢ + if (a_seg.hScanFlag == 0) + { + SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx }; + centerPtIndice.push_back(pos_center); + SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx }; + outsideEdgePtIndice.push_back(pos_edge); + } + else + { + SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx }; + centerPtIndice.push_back(pos_center); + SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx }; + outsideEdgePtIndice.push_back(pos_edge); + } + } + } + } + + err = fitCircleByLeastSquare(pointArray, center, radius); a_pose.center = { center.x, center.y, objZ }; a_pose.value = radius; a_pose.z_dir = { 0.0, 0.0, 1.0 }; a_pose.y_dir = { 0.0, 0.0, 0.0 }; - a_pose.z_dir = { 0.0, 0.0, 0.0 }; + a_pose.x_dir = { 0.0, 0.0, 0.0 }; return a_pose; } @@ -771,6 +799,38 @@ typedef struct std::vector mappings;//ÿ2DܶӦ3D }mapping2DTo3D; +bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b) +{ + return a.center.x < b.center.x; +} +void _getYTopLine( + std::vector< WD_workpieceInfo>& workpieceSrc, + std::vector< WD_workpieceInfo>& firstLine, + std::vector< WD_workpieceInfo>& restWorkpiece, + double yLayerTh) +{ + //YСֵ + if (workpieceSrc.size() == 0) + return; + + double minY = workpieceSrc[0].center.y; + for (int i = 1; i < (int)workpieceSrc.size(); i++) + { + if (minY > workpieceSrc[i].center.y) + minY = workpieceSrc[i].center.y; + } + double topLayerTh = minY + yLayerTh; + for (int i = 0; i < (int)workpieceSrc.size(); i++) + { + if (workpieceSrc[i].center.y < topLayerTh) + firstLine.push_back(workpieceSrc[i]); + else + restWorkpiece.push_back(workpieceSrc[i]); + } + std::sort(firstLine.begin(), firstLine.end(), _compareByXValue); + return; +} + void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, @@ -978,8 +1038,8 @@ void wd_HRM_RotorCorePositioning( } } - int objNum = 1; // (int)objROIs.size(); - + int objNum = (int)objROIs.size(); + std::vector allWorkpiece; for (int idx = 0; idx < objNum; idx++) { WD_objArea2D& obj_roi = objROIs[idx]; @@ -1009,7 +1069,8 @@ void wd_HRM_RotorCorePositioning( int ptIdx = a_mapping.mappings[m].ptIdx; if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4) { - scanLinesInput[lineIdx][ptIdx].nPointIdx = idx + 1; + scanLinesInput[lineIdx][ptIdx].nPointIdx &= 0xffff0000; + scanLinesInput[lineIdx][ptIdx].nPointIdx |= (idx + 1); roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin; roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax; roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin; @@ -1041,7 +1102,8 @@ void wd_HRM_RotorCorePositioning( int kkk = 1; if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4) { - scanLines[lineIdx][ptIdx].nPointIdx = idx + 1; + scanLines[lineIdx][ptIdx].nPointIdx &= 0xffff0000; + scanLines[lineIdx][ptIdx].nPointIdx |= idx + 1; int roiLineIdx = lineIdx - roiLineIndice.nMin; int roiPtIdx = ptIdx - roiPtIndice.nMin; roiScanLines[roiLineIdx][roiPtIdx].nPointIdx = 0; @@ -1062,6 +1124,7 @@ void wd_HRM_RotorCorePositioning( } } } + //жϹ࣬㹤̬ std::vector outsideEdgePtIndice; std::vector centerPtIndice; @@ -1073,25 +1136,70 @@ void wd_HRM_RotorCorePositioning( centerPtIndice); #ifdef _OUTPUT_DEBUG_DATA - //ע - for (int i = 0; i < (int)outsideEdgePtIndice.size(); i++) + if (a_workpiece.center.z > 1e-4) { - int line = outsideEdgePtIndice[i].lineIdx + roiLineIndice.nMin; - int ptIdx = outsideEdgePtIndice[i].ptIdx + roiPtIndice.nMin; - scanLines[line][ptIdx].nPointIdx |= 0x10000; - } - for (int i = 0; i < (int)centerPtIndice.size(); i++) - { - int line = centerPtIndice[i].lineIdx + roiLineIndice.nMin; - int ptIdx = centerPtIndice[i].ptIdx + roiPtIndice.nMin; - scanLines[line][ptIdx].nPointIdx |= 0x20000; + //ע + for (int i = 0; i < (int)outsideEdgePtIndice.size(); i++) + { + int line = outsideEdgePtIndice[i].lineIdx + roiLineIndice.nMin; + int ptIdx = outsideEdgePtIndice[i].ptIdx + roiPtIndice.nMin; + scanLines[line][ptIdx].nPointIdx |= 0x10000; + } + for (int i = 0; i < (int)centerPtIndice.size(); i++) + { + int line = centerPtIndice[i].lineIdx + roiLineIndice.nMin; + int ptIdx = centerPtIndice[i].ptIdx + roiPtIndice.nMin; + scanLines[line][ptIdx].nPointIdx |= 0x20000; + } } #endif if (a_workpiece.center.z > 1e-4) - workpiecePositions.push_back(a_workpiece); + allWorkpiece.push_back(a_workpiece); } + if (allWorkpiece.size() == 0) + return; + + // + //Ŀ + double highest_z = allWorkpiece[0].center.z; + for (int i = 1; i < (int)allWorkpiece.size(); i++) + highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z; + //z + std::vector< WD_workpieceInfo> zSortWorkpiece; + double topLayerTh = highest_z + workpieceParam.height * 0.75; + for (int i = 0; i < (int)allWorkpiece.size(); i++) + { + if (allWorkpiece[i].center.z < topLayerTh) + zSortWorkpiece.push_back(allWorkpiece[i]); + } + //ˮƽ + while (zSortWorkpiece.size() > 0) + { + std::vector< WD_workpieceInfo> firstLine; + std::vector< WD_workpieceInfo> restWorkpiece; + _getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius); + workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end()); + zSortWorkpiece.clear(); + zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end()); + } + + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + #ifdef _OUTPUT_DEBUG_DATA debugScanLines.resize(scanLines.size()); for (int line = 0; line < (int)scanLines.size(); line++)