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)