jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

rodAndBarDetection version 1.2.9 :

Browse Source 
修正螺杆定位算法中轴向拟合计算时方向确定的问题，保证投影后的点云与投影前的点云的Z变化趋势一致
This commit is contained in:
jerryzeng 2026-05-10 18:11:06 +08:00
parent 31c3ffa776
commit 91aa39fa92
1 changed files with 50 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
sourceCode/rodAndBarDetection.cpp
View File

@ -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<SVzNL3DPoint>& 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<SVzNL3DPoint>& 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<SVzNL3DPoint>& 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<SVzNL3DPoint> surfacePoints;
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
//计算中心点
@ -706,8 +737,13 @@ SVzNL3DRangeD _getPointCloudROI(std::vector<SVzNL3DPoint>& 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)