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

planeLocalization

Browse Source 
version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
This commit is contained in:
jerryzeng 2026-07-30 11:01:35 +08:00
parent 6e9cdf27c4
commit aa1f6a1f8b
1 changed files with 66 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
sourceCode/planeLocalization.cpp
View File

@ -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<std::vector< SVzNL3DPosition>> 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<std::vector< SVzNL3DPosition>> objClusters;
std::vector<SVzNL3DRangeD> 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<int>& 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]);
}
}