From aa1f6a1f8bebc079d3b73dc5c3d5b74ed5d8d53e Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 30 Jul 2026 11:01:35 +0800 Subject: [PATCH] =?UTF-8?q?planeLocalization=20version=201.3.2=20:=20?= =?UTF-8?q?=E6=94=B9=E8=BF=9B=E4=BA=86=E5=8F=96=E5=8F=91=E5=8A=A8=E6=9C=BA?= =?UTF-8?q?=E7=82=B9=E7=89=B9=E5=BE=81=E7=82=B9=E7=AE=97=E6=B3=95,=20?= =?UTF-8?q?=E5=A2=9E=E5=BC=BA=E4=BA=86=E9=B2=81=E6=A3=92=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/planeLocalization.cpp | 70 ++++++++++++++++++++++++++++++-- 1 file changed, 66 insertions(+), 4 deletions(-) diff --git a/sourceCode/planeLocalization.cpp b/sourceCode/planeLocalization.cpp index 54cd8df..b007d22 100644 --- a/sourceCode/planeLocalization.cpp +++ b/sourceCode/planeLocalization.cpp @@ -10,7 +10,8 @@ //version 1.2.0 : 修正了回归测试中发现的问题 //version 1.3.0 : 修正了取发动机点特征点的一个Bug //version 1.3.1 : 改进了取发动机点特征点算法 -std::string m_strVersion = " PlaneLocalization 1.3.1"; +//version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性 +std::string m_strVersion = " PlaneLocalization 1.3.2"; const char* wd_PlaneLocalizationVersion(void) { return m_strVersion.c_str(); @@ -323,6 +324,7 @@ SSX_planeInfo wd_planeLocalization( SVzNL3DPoint rotateParkingPoint = wd_ptRotate(parkingParam.parkingPoint, guideLineRotatePara.planeCalib); SVzNL3DPoint chkPoint = wd_ptRotate(parkingParam.guideLinePoint, guideLineRotatePara.planeCalib); //计算目标范围 + SVzNLRangeD ROI_x_validPlaneInRng = { rotateParkingPoint.x - 5000.0, rotateParkingPoint.x + 5000.0 }; SVzNLRangeD ROI_x_near = { rotateParkingPoint.x - parkingParam.parkingRange, rotateParkingPoint.x + parkingParam.parkingRange }; SVzNLRangeD ROI_x_far = { rotateParkingPoint.x - parkingParam.parkingRange * 3, rotateParkingPoint.x + parkingParam.parkingRange* 3 }; //大于100m外，需要放宽，因为飞机正在进引导线 SVzNLRangeD ROI_z = { rotateParkingPoint.z - parkingParam.distFromNoseToWheel - 2000.0, rotateParkingPoint.z + parkingParam.guidingRange }; @@ -403,7 +405,7 @@ SSX_planeInfo wd_planeLocalization( //使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx） //搜索时搜索邻域以加速 int clusterCheckWin = 5; - double clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2)); + double clusterDist = 500; // sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2)); int distType = 1; //0 - 2d distance; 1- 3d distance std::vector> allClusters; wd_pointClustering_speedUp( @@ -427,6 +429,62 @@ SSX_planeInfo wd_planeLocalization( allClusterROIs[m] = a_roi3D; } +#if 1 + //将小ROI的目标过滤掉后，重新使用大尺度进行聚类 + validPoints.clear(); + for (int m = 0; m < (int)allClusters.size(); m++) + { + SVzNL3DRangeD& a_roi = allClusterROIs[m]; + double height = groundCalibParam.planeHeight - a_roi.yRange.min; + if (height >= 2000.00) //2米高度门限， 跳过一般人身高 + { + if ((a_roi.zRange.min - rotateParkingPoint.z) < nearFarTh) + { + if ((a_roi.xRange.min >= ROI_x_near.min) && (a_roi.xRange.max <= ROI_x_near.max) && + (a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max)) + { + validPoints.insert(validPoints.end(), allClusters[m].begin(), allClusters[m].end()); + } + } + else + { + if ((a_roi.xRange.min >= ROI_x_far.min) && (a_roi.xRange.max <= ROI_x_far.max) && + (a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max)) + { + validPoints.insert(validPoints.end(), allClusters[m].begin(), allClusters[m].end()); + } + } + } + } + //聚类 +//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx） +//搜索时搜索邻域以加速 + clusterCheckWin = 5; + clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2)); + distType = 1; //0 - 2d distance; 1- 3d distance + allClusters.clear(); + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, //搜索窗口 + clusterDist, + distType, //0 - 2d distance; 1- 3d distance + allClusters //result + ); + + //统计cluster的ROI + allClusterROIs.clear(); + allClusterROIs.resize(allClusters.size()); + for (int m = 0; m < (int)allClusters.size(); m++) + { + SVzNL3DRangeD a_roi3D; + memset(&a_roi3D, 0, sizeof(SVzNL3DRangeD)); + a_roi3D.zRange.max = -1; + for (int i = 0; i < (int)allClusters[m].size(); i++) + _updateRoi3D(a_roi3D, allClusters[m][i].pt3D); + allClusterROIs[m] = a_roi3D; + } +#endif + //ROI过滤 std::vector> objClusters; std::vector objClustersROIs; @@ -519,7 +577,8 @@ SSX_planeInfo wd_planeLocalization( double w = superROI.xRange.max - superROI.xRange.min; double len = superROI.zRange.max - superROI.zRange.min; - if( (h > planeMinHeight) && (w > planeMinWidth)) + if( (h > planeMinHeight) && (w > planeMinWidth) && + (superROI.xRange.max > ROI_x_validPlaneInRng.min) && (ROI_x_validPlaneInRng.max > superROI.xRange.min)) superClusers.push_back(a_superCluster); } @@ -576,6 +635,8 @@ SSX_planeInfo wd_planeLocalization( } std::vector& planeSuperCluster = superClusers[bestId]; + //检查ROI + int planeClusterSize = (int)planeSuperCluster.size(); //寻找机头位置: ROI最靠前（Z最小） int noseClusterId = -1; @@ -708,7 +769,8 @@ SSX_planeInfo wd_planeLocalization( if (objClusters[clusterIdx][i].pt3D.z > nosePoint.pt3D.z) { double dist = sqrt(pow(nosePoint.pt3D.x - objClusters[clusterIdx][i].pt3D.x, 2) + pow(nosePoint.pt3D.z - objClusters[clusterIdx][i].pt3D.z, 2)); - if ((dist >= engineToNoseDistRange.min) && (dist <= engineToNoseDistRange.max)) + double zDiff = objClusters[clusterIdx][i].pt3D.z - nosePoint.pt3D.z; + if ((dist >= engineToNoseDistRange.min) && (dist <= engineToNoseDistRange.max) && (zDiff > 10000.0)) //在机鼻后面10米外 distanceValidData[idx].push_back(objClusters[clusterIdx][i]); } }