planeLocalization

version 1.2.0 : 修正了回归测试中发现的问题

HC_planeLocalization_test/HC_planeLocalization_test.cpp

@@ -179,7 +179,15 @@ void _outputPlaneInfo(char* fileName, SSX_planeInfo planeInfo)
 	std::ofstream sw(fileName);
 
 	char dataStr[250];
-	sprintf_s(dataStr, 250, "距离: %g, 方位: %g", planeInfo.distance, planeInfo.dirAngle_deg);
+	sprintf_s(dataStr, 250, "距离: %g (mm)", planeInfo.distance);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "偏离引导线: %g (mm)", planeInfo.deviation);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "前进方向: %g (度)", planeInfo.dirAngle_deg);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "机鼻点: (%g , %g, %g)", planeInfo.nosePoint.x, planeInfo.nosePoint.y, planeInfo.nosePoint.z);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "方向向量(轴向): (%g , %g, %g)", planeInfo.axis.x, planeInfo.axis.y, planeInfo.axis.z);
 	sw << dataStr << std::endl;
 	sw.close();
 }
@@ -249,15 +257,19 @@ void _outputRGBDScan_RGBD(
 	sw.close();
 }
 
-void _outputRGBDScan_superCluster(
+void _outputRGBDScan_superCluster_result(
 	char* fileName,
-	std::vector<std::vector<SVzNL3DPosition>>& scanLines
+	std::vector<std::vector<SVzNL3DPosition>>& scanLines,
+	SSX_planeInfo& planePose
 )
 {
 	int lineNum = (int)scanLines.size();
 	std::ofstream sw(fileName);
 	int realLines = lineNum;
 
+	if (planePose.nosePoint.z > 1e-4)
+		realLines += 1;
+
 	sw << "LineNum:" << realLines << std::endl;
 	sw << "DataType: 0" << std::endl;
 	sw << "ScanSpeed: 0" << std::endl;
@@ -297,7 +309,7 @@ void _outputRGBDScan_superCluster(
 			if (flag == 1) //机鼻
 			{
 				rgb = { 255, 0, 0 };
-				size = 10;
+				size = 20;
 			}
 			else if(flag == 2) //机身
 			{
@@ -327,6 +339,40 @@ void _outputRGBDScan_superCluster(
 			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 		}
 	}
+	if (planePose.nosePoint.z > 1e-4)
+	{
+		sw << "Line_" << lineIdx << "_0_1"  << std::endl;
+		lineIdx++;
+		rgb = { 255, 0, 0 };
+		size = 20;
+		float x = (float)planePose.nosePoint.x;
+		float y = (float)planePose.nosePoint.y;
+		float z = (float)planePose.nosePoint.z;
+		sw << "{" << x << "," << y << "," << z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+		//输出姿态
+			//输出方向线条
+		rgb = { 0, 0, 255 };
+		size = 5;
+		SVzNL3DPoint pt1, pt2;
+		pt1 = { planePose.nosePoint.x - planePose.axis.x * 2000.0,
+				planePose.nosePoint.y - planePose.axis.y * 2000.0,
+				planePose.nosePoint.z - planePose.axis.z * 2000.0 };
+		pt2 = { planePose.nosePoint.x + planePose.axis.x * 40000.0,
+				planePose.nosePoint.y + planePose.axis.y * 40000.0,
+				planePose.nosePoint.z + planePose.axis.z * 40000.0 };
+		sw << "Poly_" << lineIdx << "_2" << std::endl;
+		sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+		sw << "{" << pt2.x << "," << pt2.y << "," << pt2.z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+		lineIdx++;
+
+	}
+
 	sw.close();
 }
 
@@ -587,7 +633,7 @@ int main()
 
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx =18;
+			//fidx =7;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
 
@@ -623,16 +669,16 @@ int main()
 				& errCode);
 
 			long t2 = (long)GetTickCount64();
-			printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
+			printf("%s: %d(ms), errCode=%d  ...", _scan_file, (int)(t2 - t1), errCode);
 			//输出测试结果
 #if _OUTPUT_DEBUG_DATA
-			sprintf_s(_scan_file, "%sresult\\%d_superCluster.txt", dataPath[grp], fidx);
-			_outputRGBDScan_superCluster(_scan_file, debugData);
-#endif
 			sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
-			_outputRGBDScan_RGBD(_scan_file, scanLines);
+			_outputRGBDScan_superCluster_result(_scan_file, debugData, planeInfo);
-			sprintf_s(_scan_file, "%sresult\\%d_fillingPort_info.txt", dataPath[grp], fidx);
+#endif
+			sprintf_s(_scan_file, "%sresult\\%d_planePise_info.txt", dataPath[grp], fidx);
 			_outputPlaneInfo(_scan_file, planeInfo);
+			printf("done\n");
+			printf("  distance=%f, devi=%f, angle=%f\n", planeInfo.distance, planeInfo.deviation, planeInfo.dirAngle_deg);
 		}
 	}
 #endif

sourceCode/planeLocalization.cpp

@@ -6,7 +6,9 @@
 #include <limits>
 
 //version 1.0.0 : base version release to customer
-std::string	m_strVersion = " PlaneLocalization 1.0.0";
+//version 1.1.0 : 优化了机鼻点提取（迭代），增加了没有飞机的输出
+//version 1.2.0 : 修正了回归测试中发现的问题
+std::string	m_strVersion = " PlaneLocalization 1.2.0";
 const char* wd_PlaneLocalizationVersion(void)
 {
 	return m_strVersion.c_str();
@@ -143,7 +145,7 @@ void _searchSuperCluster(
 				a_superCluster.push_back(i);
 				clusterFlags[i] = 1;
 			}
-			else if (score > 0)
+			else //if (score > 0)
 			{
 				//计算距离:使用Z距离 
 				SVzNL3DRangeD& chkROI = clusterROIs[i];
@@ -215,6 +217,48 @@ SVzNL3DRangeD _getSupcluster(std::vector<int>& superClusterIndice, std::vector<S
 	}
 	return mergeROI;
 }
+
+//判断聚类的最近点是否可能是机鼻点。方法：以最近点搜索在距离范围内的点（指向机身），计算机身高度
+bool _checkNosePoint(
+	std::vector<SVzNL3DPosition>& a_cluster, 
+	SVzNL3DPosition& nearestPoint, 
+	SVzNLRangeD& distChkRange, 
+	const double bodyHeight)
+{
+	nearestPoint.nPointIdx = 0;
+	nearestPoint.pt3D = { 0.0, 0.0, 0.0 };
+	for (int i = 0; i < (int)a_cluster.size(); i++)
+	{
+		if (nearestPoint.pt3D.z < 1e-4)
+			nearestPoint = a_cluster[i];
+		else if (nearestPoint.pt3D.z > a_cluster[i].pt3D.z)
+			nearestPoint = a_cluster[i];
+	}
+
+	//判断是否是机鼻点
+	std::vector<SVzNL3DPoint> chkPoints;
+	for (int i = 0; i < (int)a_cluster.size(); i++)
+	{
+		double dist = sqrt(pow(a_cluster[i].pt3D.x - nearestPoint.pt3D.x, 2) + pow(a_cluster[i].pt3D.z - nearestPoint.pt3D.z, 2));
+		if ((dist >= distChkRange.min) && (dist <= distChkRange.max))
+			chkPoints.push_back(a_cluster[i].pt3D);
+	}
+	if (chkPoints.size() < 2)
+		return false;
+
+	SVzNLRangeD yRange = { chkPoints[0].y, chkPoints[0].y };
+	for (int i = 1; i < (int)chkPoints.size(); i++)
+	{
+		yRange.min = yRange.min > chkPoints[i].y ? chkPoints[i].y : yRange.min;
+		yRange.max = yRange.max < chkPoints[i].y ? chkPoints[i].y : yRange.max;
+	}
+	
+	double height = yRange.max - yRange.min;
+	if (height > bodyHeight)
+		return true;
+	return false;
+}
+
 SSX_planeInfo wd_planeLocalization(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	const SSG_planeCalibPara groundCalibParam,
@@ -228,7 +272,7 @@ SSX_planeInfo wd_planeLocalization(
 	*errCode = 0;
 	//内部参数
 	double planeMinHeight = 4000; //最小高度4米
-	double planeMinWidth = 25000; //最小宽度2.5米
+	double planeMinWidth = 2500; //最小宽度2.5米
 	double nearFarTh = 80000.0; //远近分界
 	double groundHOffset = 200; //去除地面参数
 	double bodyHeadJudge_lenTh = 20000.0; //机头与机身判断的长度门限。20米
@@ -236,6 +280,8 @@ SSX_planeInfo wd_planeLocalization(
 	SVzNLRangeD sameRBodyRange = { 5000.0, 28000.0 }; //机身圆桶段，以距离机鼻距离为基准
 	SVzNLRangeD engineToNoseDistRange = { 12000.0, 16000.0 };
 	SVzNLRangeD bodyYRange = { groundCalibParam.planeHeight - 4500.0, groundCalibParam.planeHeight - 1500 }; //机身的Y范围
+	SVzNLRangeD nearestPointNostChkDistRange = { 3500.0, 4500.0 };
+	double minBodyHeight = 2000.0;
 
 	SSX_planeInfo planePoseInfo;
 	memset(&planePoseInfo, 0, sizeof(SSX_planeInfo));
@@ -527,16 +573,34 @@ SSX_planeInfo wd_planeLocalization(
 	int planeClusterSize = (int)planeSuperCluster.size();
 	//寻找机头位置: ROI最靠前（Z最小）
 	int noseClusterId = -1;
+	SVzNL3DPosition nosePoint;
+	nosePoint.nPointIdx = 0;
+	nosePoint.pt3D = { 0.0, 0.0, 0.0 };
 	for (int i = 0; i < planeClusterSize; i++)
 	{
 		int clusterId = planeSuperCluster[i];
 		SVzNL3DRangeD& a_roi = objClustersROIs[clusterId];
 		if ((a_roi.yRange.min < bodyYRange.min) && (a_roi.yRange.max > bodyYRange.max))  //处于机身的Y范围内
 		{
-			if(noseClusterId < 0)
+			SVzNL3DPosition nearestPoint;
-				noseClusterId = clusterId;
+			bool validNoseCluster = _checkNosePoint(
-			else if (objClustersROIs[noseClusterId].zRange.min > objClustersROIs[clusterId].zRange.min)
+				objClusters[clusterId],
-				noseClusterId = clusterId;
+				nearestPoint,
+				nearestPointNostChkDistRange,
+				minBodyHeight);
+			if (true == validNoseCluster)
+			{
+				if (noseClusterId < 0)
+				{
+					noseClusterId = clusterId;
+					nosePoint = nearestPoint;
+				}
+				else if (nosePoint.pt3D.z > nearestPoint.pt3D.z)
+				{
+					noseClusterId = clusterId;
+					nosePoint = nearestPoint;
+				}
+			}
 		}
 	}
 	if (noseClusterId < 0)
@@ -544,29 +608,7 @@ SSX_planeInfo wd_planeLocalization(
 		*errCode = SX_ERR_NOSEPOINT_FAIL;
 		return planePoseInfo;
 	}
-	//搜索机鼻点
+
-	SVzNL3DPosition nosePoint;
-	nosePoint.nPointIdx = 0;
-	nosePoint.pt3D = { 0.0, 0.0, 0.0 };
-	for (int i = 0; i < (int)objClusters[noseClusterId].size(); i++)
-	{
-		if (nosePoint.pt3D.z < 1e-4)
-			nosePoint = objClusters[noseClusterId][i];
-		else if(nosePoint.pt3D.z > objClusters[noseClusterId][i].pt3D.z)
-			nosePoint = objClusters[noseClusterId][i];
-	}
-	if (nosePoint.pt3D.z < 1e-4)
-	{
-		*errCode = SX_ERR_NOSEPOINT_FAIL;
-		return planePoseInfo;
-	}
-#if _OUTPUT_DEBUG_DATA
-	{
-		int nose_lineIdx = nosePoint.nPointIdx >> 16;
-		int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
-		debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000;  //机鼻点
-	}
-#endif
 	//计算姿态
 	//判断机鼻所在的长度：以机鼻高度为基准，取高度范围内的点，计算长度
 	SVzNLRangeD bodyHRange = { nosePoint.pt3D.y + bodyRangeToNose.min, nosePoint.pt3D.y + bodyRangeToNose.max };
@@ -603,6 +645,24 @@ SSX_planeInfo wd_planeLocalization(
 		if (axis.y < 0)
 			axis = { -axis.x, -axis.y };
 
+		//以centrod为基准，计算到centroid最远的点为机鼻点
+		double maxDistance = 0;
+		SVzNL3DPosition bestPoint; 
+		for (int m = 0; m < (int)objClusters[noseClusterId].size(); m++)
+		{
+			if (objClusters[noseClusterId][m].pt3D.z < centroid.y)
+			{
+				double dist = sqrt(pow(objClusters[noseClusterId][m].pt3D.x - centroid.x, 2) + pow(objClusters[noseClusterId][m].pt3D.z - centroid.y, 2));
+				if (maxDistance < dist)
+				{
+					maxDistance = dist;
+					bestPoint = objClusters[noseClusterId][m];
+				}
+			}
+		}
+		if(maxDistance > 1e-4)  //迭代计算机鼻点
+			nosePoint = bestPoint;
+
 		double dirAngle = atan(axis.x / axis.y) * 180.0 / PI;
 		planePoseInfo.nosePoint = nosePoint.pt3D;
 		planePoseInfo.axis = { axis.x, 0, axis.y };
@@ -612,6 +672,11 @@ SSX_planeInfo wd_planeLocalization(
 		planePoseInfo.dirAngle_deg = dirAngle; //方向角度
 
 #if _OUTPUT_DEBUG_DATA
+		{
+			int nose_lineIdx = nosePoint.nPointIdx >> 16;
+			int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
+			debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000;  //机鼻点
+		}
 		for(int m = 0; m <(int)XOZBodayData.size(); m ++)
 		{
 			int nose_lineIdx = XOZBodayData[m].nPointIdx >> 16;
@@ -710,6 +775,11 @@ SSX_planeInfo wd_planeLocalization(
 				rightEnginePoint = distanceValidData[rightEngineIdx][i];
 		}
 #if _OUTPUT_DEBUG_DATA
+		{
+			int nose_lineIdx = nosePoint.nPointIdx >> 16;
+			int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
+			debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000;  //机鼻点
+		}
 		{
 			int nose_lineIdx = leftEnginePoint.nPointIdx >> 16;
 			int nose_ptIdx = leftEnginePoint.nPointIdx & 0xffff;