gasFillingPortPosition ver 1.1.0

改进了加气口顶面点提取的准确度。

gasFillingPortPosition_test/gasFillingPortPosition_test.cpp

@@ -139,18 +139,16 @@ void _outputRGBDScan_fillingPort_RGBD(
 		for (int i = 0; i < linePtNum; i++)
 		{
 			SVzNL3DPosition* pt3D = &scanLines[line][i];
-			if (pt3D->nPointIdx > 0)
+			if (pt3D->nPointIdx == 1)
-				int kkk = 1;
-			if(pt3D->nPointIdx == 0)
-			{
-				rgb = { 200, 200, 200 };
-				size = 1;
-			}
-			else
 			{
 				rgb = { 255, 97, 0 };
 				size = 3;
 			}
+			else //if (pt3D->nPointIdx == 0)
+			{
+				rgb = { 200, 200, 200 };
+				size = 1;
+			}
 			float x = (float)pt3D->pt3D.x;
 			float y = (float)pt3D->pt3D.y;
 			float z = (float)pt3D->pt3D.z;
@@ -228,7 +226,7 @@ int main()
 	{
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx =4;
+			//fidx =7;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx);
 

sourceCode/SG_baseAlgo_Export.h

@@ -103,12 +103,13 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature(
 );
 
 //直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线
-SG_APISHARED_EXPORT void wd_simpleLineSegment(
+SG_APISHARED_EXPORT void wd_surfaceLineSegment(
     std::vector< SVzNL3DPosition>& lineData,
     int lineIdx,
     SVzNLRangeD lineLenRange,
     const SSG_lineSegParam lineSegPara,
-    std::vector<SSG_featureSemiCircle>& lineSegs);
+    std::vector<SSG_featureSemiCircle>& lineSegs,
+    std::vector<SSG_featureSemiCircle>& invlaidLineSegs);
 
 /// <summary>
 /// 提取激光线上的Jumping特征
@@ -376,6 +377,12 @@ SG_APISHARED_EXPORT void compute2ptLine(
     SVzNL3DPoint pt2, 
     double* _a, double* _b, double* _c);
 
+//计算过2点的直线方程,不使用结构体
+SG_APISHARED_EXPORT void compute2ptLine_2(
+    double x1, double y1,
+    double x2, double y2,
+    double* _a, double* _b, double* _c);
+
 //旋转45度后的直线方程
 SG_APISHARED_EXPORT void rotateLine45Deg(
     double _a, double _b, double _c,

sourceCode/SG_baseFunc.cpp

@@ -394,6 +394,14 @@ void compute2ptLine(SVzNL3DPoint pt1, SVzNL3DPoint pt2, double* _a, double* _b,
 	return;
 }
 
+void compute2ptLine_2(double x1, double y1, double x2, double y2, double* _a, double* _b, double* _c)
+{
+	*_a = y2 - y1;
+	*_b = x1 - x2;
+	*_c = x2 * y1 - x1 * y2;
+	return;
+}
+
 //旋转45度后的直线方程
 void rotateLine45Deg(
 	double _a, double _b, double _c,

sourceCode/SG_lineFeature.cpp

@@ -2559,7 +2559,7 @@ void wd_getRingArcFeature(
 }
 
 // 最小点集数量（小于此数无法拟合直线）
-const int MIN_POINT_COUNT = 5;
+const int MIN_POINT_COUNT = 3;
 //使用端点直线，检查点到直线的距离，大于门限的分割
 void split(
 	SSG_RUN a_run, 
@@ -2577,7 +2577,11 @@ void split(
 	if ((pt1.z < 1e-4) || (pt2.z < 1e-4))
 		return;
 	double _a, _b, _c;
-	compute2ptLine(pt1, pt2, &_a, &_b, &_c);
+	compute2ptLine_2(
+		pt1.y, pt1.z,
+		pt2.y, pt2.z,
+		&_a, &_b, &_c);
+	//compute2ptLine(pt1, pt2, &_a, &_b, &_c);
 	double denominator = sqrt(_a * _a + _b * _b);
 	//归一化
 	_a = _a / denominator;
@@ -2585,13 +2589,13 @@ void split(
 	_c = _c / denominator;
 
 	double maxDist = 0;
-	double maxPos = 0;
+	int maxPos = 0;
 	for (int i = start; i <= end; i++)
 	{
 		SVzNL3DPoint a_pt = lineData[i].pt3D;
 		if (a_pt.z > 1e-4)
 		{
-			double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
+			double dist = abs(a_pt.y * _a + a_pt.z * _b + _c);
 			if (maxDist < dist)
 			{
 				maxDist = dist;
@@ -2612,12 +2616,14 @@ void split(
 }
 
 //直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线
-void wd_simpleLineSegment(
+//此算法对端面直线提取作了优化，每个分段只保留一个最佳直线段（去掉相邻的导角形成的直线,并记录）
+void wd_surfaceLineSegment(
 	std::vector< SVzNL3DPosition>& lineData,
 	int lineIdx,
 	SVzNLRangeD lineLenRange,
 	const SSG_lineSegParam lineSegPara,
-	std::vector<SSG_featureSemiCircle>&lineSegs)
+	std::vector<SSG_featureSemiCircle>&lineSegs,
+	std::vector<SSG_featureSemiCircle>& invlaidLineSegs)
 {
 	int dataSize = (int)lineData.size();
 	//去除零点
@@ -2669,7 +2675,9 @@ void wd_simpleLineSegment(
 	{
 		std::vector< SSG_RUN> segmentationLines;
 		split(segs[si], lineData, lineSegPara.maxDist, segmentationLines);
-		//转成SSG_featureSemiCircle格式
+		
+		//对于每个分段，只能有一个合格的直线段
+		std::vector< SSG_featureSemiCircle> candiLines;
 		for (int m = 0, m_max = (int)segmentationLines.size(); m < m_max; m++)
 		{
 			SSG_featureSemiCircle a_seg;
@@ -2715,10 +2723,45 @@ void wd_simpleLineSegment(
 				{
 					a_seg.midPtIdx = midPtIdx;
 					a_seg.midPt = lineData[midPtIdx].pt3D;
-					lineSegs.push_back(a_seg);
+					candiLines.push_back(a_seg);
 				}
 			}
 		}
+		//转成SSG_featureSemiCircle格式
+		if (candiLines.size() > 0)
+		{
+			//选择一个水平角度更小的
+			double minTan = 0;
+			int bestIdx = -1;
+			for (int m = 0, m_max = (int)candiLines.size(); m < m_max; m++)
+			{
+				SSG_featureSemiCircle& a_seg = candiLines[m];
+				SVzNL3DPoint ptStart = lineData[a_seg.startPtIdx].pt3D;
+				SVzNL3DPoint ptEnd = lineData[a_seg.endPtIdx].pt3D;
+				double tank = abs((ptEnd.z - ptStart.z) / (ptEnd.y - ptStart.y));
+				if (bestIdx < 0)
+				{
+					minTan = tank;
+					bestIdx = m;
+				}
+				else
+				{
+					if (minTan > tank)
+					{
+						minTan = tank;
+						bestIdx = m;
+					}
+				}
+			}
+			lineSegs.push_back(candiLines[bestIdx]);
+			for (int m = 0, m_max = (int)candiLines.size(); m < m_max; m++)
+			{
+				if (m != bestIdx)
+					invlaidLineSegs.push_back(candiLines[m]);
+			}
+		}
+		else if (candiLines.size() == 1)
+			lineSegs.push_back(candiLines[0]);
 	}
 }
 

sourceCode/gasFillingPortPosition.cpp

@@ -7,7 +7,8 @@
 
 //读版本号
 //version 1.0.0 : base version release to customer
-std::string	m_strVersion = "1.0.0";
+//version 1.1.0 : 改进了算法，顶面点提取更准确
+std::string	m_strVersion = "1.1.0";
 const char* wd_gasFillingPortPositionVersion(void)
 {
 	return m_strVersion.c_str();
@@ -35,9 +36,10 @@ SSG_6DOF wd_getGasFillingPortPosition(
 	lineLenRange.max = 1.2 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD);
 	lineLenRange.min = 0.5 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD) / 2;
 	std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_v_raw;
+	std::vector<std::vector<SSG_featureSemiCircle>> invalidFeatures_v_raw;  //确定非直线特征，用于防止在水平处理时被认为是有效点
 	for (int line = 0; line < lineNum; line++)
 	{
-		if (line == 390)
+		if (line == 310)
 			int kkk = 1;
 
 		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
@@ -48,14 +50,17 @@ SSG_6DOF wd_getGasFillingPortPosition(
 		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
 
 		std::vector<SSG_featureSemiCircle> line_features;
+		std::vector<SSG_featureSemiCircle> invalid_features;
 		int dataSize = (int)lineData.size();
-		wd_simpleLineSegment(
+		wd_surfaceLineSegment(
 			lineData,
 			line,
 			lineLenRange,
 			lineSegPara,
-			line_features);
+			line_features,
+			invalid_features);
 		lineFeatures_v_raw.push_back(line_features);
+		invalidFeatures_v_raw.push_back(invalid_features);
 	}
 
 	if (false == isGridData)//数据不是网格格式
@@ -81,6 +86,7 @@ SSG_6DOF wd_getGasFillingPortPosition(
 	}
 	//水平arc特征提取
 	std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_h_raw;
+	std::vector<std::vector<SSG_featureSemiCircle>> invalidFeatures_h_raw;  //确定非直线特征，用于防止在垂直处理时被认为是有效点
 	int lineNum_h_raw = (int)hLines_raw.size();
 	for (int line = 0; line < lineNum_h_raw; line++)
 	{
@@ -92,14 +98,17 @@ SSG_6DOF wd_getGasFillingPortPosition(
 		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
 
 		std::vector<SSG_featureSemiCircle> line_features;
+		std::vector<SSG_featureSemiCircle> invalid_features;
 		int dataSize = (int)lineData.size();
-		wd_simpleLineSegment(
+		wd_surfaceLineSegment(
 			lineData,
 			line,
 			lineLenRange,
 			lineSegPara,
-			line_features);
+			line_features,
+			invalid_features);
 		 lineFeatures_h_raw.push_back(line_features);
+		 invalidFeatures_h_raw.push_back(invalid_features);
 	}
 
 	//标注
@@ -235,6 +244,40 @@ SSG_6DOF wd_getGasFillingPortPosition(
 
 	std::vector< SVzNL2DPoint>& obj_cluster = clusters[bestClusterIdx];
 	//提取端面上的点
+	//首先将无效点置标志
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector<SSG_featureSemiCircle>& line_invalidFeature = invalidFeatures_v_raw[line];
+		for (int n = 0; n < line_invalidFeature.size(); n++)
+		{
+			SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n];
+			int lineIdx = a_invalidFeature.lineIdx;
+			for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++)
+			{
+				SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m];
+				a_pt3d.nPointIdx = 2;
+			}
+		}
+	}
+	for (int line = 0; line < lineNum_h_raw; line++)
+	{
+		std::vector<SSG_featureSemiCircle>& line_invalidFeature = invalidFeatures_h_raw[line];
+		for (int m = 0, m_max = (int)line_invalidFeature.size(); m < m_max; m++)
+		{
+			for (int n = 0; n < line_invalidFeature.size(); n++)
+			{
+				SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n];
+				int ptIdx = a_invalidFeature.lineIdx;
+				for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++)
+				{
+					SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx];
+					a_pt3d.nPointIdx = 3;
+				}
+			}
+		}
+	}
+
+	//提取端面点
 	std::vector< cv::Point3f> planePts;
 	for (int i = 0, i_max = (int)obj_cluster.size(); i < i_max; i++)
 	{