From 91aa39fa92242c238d86c81fc02761cdc4234c43 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 10 May 2026 18:11:06 +0800 Subject: [PATCH] =?UTF-8?q?rodAndBarDetection=20version=201.2.9=20:=20?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E8=9E=BA=E6=9D=86=E5=AE=9A=E4=BD=8D=E7=AE=97?= =?UTF-8?q?=E6=B3=95=E4=B8=AD=E8=BD=B4=E5=90=91=E6=8B=9F=E5=90=88=E8=AE=A1?= =?UTF-8?q?=E7=AE=97=E6=97=B6=E6=96=B9=E5=90=91=E7=A1=AE=E5=AE=9A=E7=9A=84?= =?UTF-8?q?=E9=97=AE=E9=A2=98=EF=BC=8C=E4=BF=9D=E8=AF=81=E6=8A=95=E5=BD=B1?= =?UTF-8?q?=E5=90=8E=E7=9A=84=E7=82=B9=E4=BA=91=E4=B8=8E=E6=8A=95=E5=BD=B1?= =?UTF-8?q?=E5=89=8D=E7=9A=84=E7=82=B9=E4=BA=91=E7=9A=84Z=E5=8F=98?= =?UTF-8?q?=E5=8C=96=E8=B6=8B=E5=8A=BF=E4=B8=80=E8=87=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/rodAndBarDetection.cpp | 64 ++++++++++++++++++++++++------- 1 file changed, 50 insertions(+), 14 deletions(-) diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index d303253..274d049 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -17,7 +17,8 @@ //version 1.2.6 : 定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高 //version 1.2.7 : (1)根据实际点云修正了螺杆定位算法（2）定位盘位姿计算改回RANSAC //version 1.2.8 : 修正螺杆定位算法中的一个Bug -std::string m_strVersion = "1.2.8"; +//version 1.2.9 : 修正螺杆定位算法中轴向拟合计算时方向确定的问题，保证投影后的点云与投影前的点云的Z变化趋势一致 +std::string m_strVersion = "1.2.9"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -172,7 +173,7 @@ SVzNLRangeD getZRange(std::vector& projectPoints) int ptNum = (int)projectPoints.size(); SVzNLRangeD zRange; zRange.min = DBL_MAX; - zRange.max = DBL_MIN; + zRange.max = -DBL_MAX; for (int i = 0; i < ptNum; i++) { zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min; @@ -662,6 +663,36 @@ SVzNL3DRangeD _getPointCloudROI(std::vector& scanData) SVzNL3DPoint vector1 = P1_dir; SVzNL3DPoint vector2 = { 0, 0, 1.0 }; SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2); +#if 1 + std::vector< SVzNL3DPoint> verifyData; + for (int m = 0; m < (int)fitPoints.size(); m++) + { + SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib); + verifyData.push_back(rPt); + } +#endif + bool dirInverting = false; + if (((fitPoints[0].z < fitPoints.back().z) && (verifyData[0].z > verifyData.back().z)) || + ((fitPoints[0].z > fitPoints.back().z) && (verifyData[0].z < verifyData.back().z))) + dirInverting = true; + if (true == dirInverting) + { + P1_dir = { -P1_dir.x, -P1_dir.y, -P1_dir.z }; + vector1 = P1_dir; + rotatePara = wd_computeRTMatrix(vector1, vector2); + } + verifyData.clear(); + for (int m = 0; m < (int)fitPoints.size(); m++) + { + SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib); + verifyData.push_back(rPt); + } + if (((fitPoints[0].z < fitPoints.back().z) && (verifyData[0].z > verifyData.back().z)) || + ((fitPoints[0].z > fitPoints.back().z) && (verifyData[0].z < verifyData.back().z))) + dirInverting = true; + else + dirInverting = false; + // SVzNL3DPoint test_center = { P0_center.x, P0_center.y + rodDiameter / 2, P0_center.z - 100 }; SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib); @@ -672,22 +703,22 @@ SVzNL3DRangeD _getPointCloudROI(std::vector& scanData) roi_xoy.top = P0_rotate.y - rodDiameter * 2.0; //2D范围 roi_xoy.bottom = P0_rotate.y + rodDiameter * 2.0; //2D范围 -#if 1 - std::vector< SVzNL3DPoint> verifyData; - for (int m = 0; m < (int)fitPoints.size(); m++) - { - SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib); - verifyData.push_back(rPt); - } -#endif std::vector< SVzNL3DPoint> roiProjectionData; xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, resultObjClusterId, clusterIdMask, roiProjectionData); //取端面 SVzNLRangeD zRange = getZRange(roiProjectionData); SVzNLRangeD cutZRange; - cutZRange.min = zRange.min; - cutZRange.max = zRange.min + 5.0; //5mm的端面 + if (false == dirInverting) + { + cutZRange.min = zRange.min; + cutZRange.max = zRange.min + 5.0; //5mm的端面 + } + else + { + cutZRange.max = zRange.max; + cutZRange.min = zRange.max - 5.0; //5mm的端面 + } std::vector surfacePoints; zCutPointClouds(roiProjectionData, cutZRange, surfacePoints); //计算中心点 @@ -706,8 +737,13 @@ SVzNL3DRangeD _getPointCloudROI(std::vector& scanData) { if (centerIdx < 0) centerIdx = i; - else if(surfacePoints[centerIdx].z < surfacePoints[i].z) - centerIdx = i; + else + { + + if (((false == dirInverting) && (surfacePoints[centerIdx].z < surfacePoints[i].z)) || + ((true == dirInverting) && (surfacePoints[centerIdx].z > surfacePoints[i].z))) + centerIdx = i; + } } } if(centerIdx < 0)