planeLocalization

version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性

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<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]);
 				}
 			}