sx_BQ_getWorkpieceCorners version 1.2.0

BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp

@@ -232,59 +232,77 @@ void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
 void _outputCornerInfo(char* fileName, SSX_BQworkpieceResult workpieceCorner)
 {
 	std::ofstream sw(fileName);
+
+
+	sw << "节点" << workpieceCorner.workpieceType << std::endl << std::endl;
 	char dataStr[250];
-	sw << "L:" << std::endl;
+	sw << "A:" << std::endl;
-	sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_L[0].x, workpieceCorner.corner_L[0].y, workpieceCorner.corner_L[0].z);
+	sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_1[0].x, workpieceCorner.corner_1[0].y, workpieceCorner.corner_1[0].z);
 	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_L[1].x, workpieceCorner.corner_L[1].y, workpieceCorner.corner_L[1].z);
+	sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_1[1].x, workpieceCorner.corner_1[1].y, workpieceCorner.corner_1[1].z);
 	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_L[2].x, workpieceCorner.corner_L[2].y, workpieceCorner.corner_L[2].z);
+	sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_1[2].x, workpieceCorner.corner_1[2].y, workpieceCorner.corner_1[2].z);
 	sw << dataStr << std::endl;
-	double dist = sqrt(pow(workpieceCorner.corner_L[0].x - workpieceCorner.corner_L[2].x, 2) +
+	double dist = sqrt(pow(workpieceCorner.corner_1[0].x - workpieceCorner.corner_1[2].x, 2) +
-					pow(workpieceCorner.corner_L[0].y - workpieceCorner.corner_L[2].y, 2) +
+					pow(workpieceCorner.corner_1[0].y - workpieceCorner.corner_1[2].y, 2) +
-					pow(workpieceCorner.corner_L[0].z - workpieceCorner.corner_L[2].z, 2));
+					pow(workpieceCorner.corner_1[0].z - workpieceCorner.corner_1[2].z, 2));
-	sprintf_s(dataStr, 250, "  Len: %g", dist);
-	sw << dataStr << std::endl;
-
-	sw << "T:" << std::endl;
-	sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_T[0].x, workpieceCorner.corner_T[0].y, workpieceCorner.corner_T[0].z);
-	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_T[1].x, workpieceCorner.corner_T[1].y, workpieceCorner.corner_T[1].z);
-	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_T[2].x, workpieceCorner.corner_T[2].y, workpieceCorner.corner_T[2].z);
-	sw << dataStr << std::endl;
-	dist = sqrt(pow(workpieceCorner.corner_T[0].x - workpieceCorner.corner_T[2].x, 2) +
-		pow(workpieceCorner.corner_T[0].y - workpieceCorner.corner_T[2].y, 2) +
-		pow(workpieceCorner.corner_T[0].z - workpieceCorner.corner_T[2].z, 2));
-	sprintf_s(dataStr, 250, "  Len: %g", dist);
-	sw << dataStr << std::endl;
-
-	sw << "R:" << std::endl;
-	sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_R[0].x, workpieceCorner.corner_R[0].y, workpieceCorner.corner_R[0].z);
-	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_R[1].x, workpieceCorner.corner_R[1].y, workpieceCorner.corner_R[1].z);
-	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_R[2].x, workpieceCorner.corner_R[2].y, workpieceCorner.corner_R[2].z);
-	sw << dataStr << std::endl;
-	dist = sqrt(pow(workpieceCorner.corner_R[0].x - workpieceCorner.corner_R[2].x, 2) +
-		pow(workpieceCorner.corner_R[0].y - workpieceCorner.corner_R[2].y, 2) +
-		pow(workpieceCorner.corner_R[0].z - workpieceCorner.corner_R[2].z, 2));
 	sprintf_s(dataStr, 250, "  Len: %g", dist);
 	sw << dataStr << std::endl;
 
 	sw << "B:" << std::endl;
-	sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_B[0].x, workpieceCorner.corner_B[0].y, workpieceCorner.corner_B[0].z);
+	sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_2[0].x, workpieceCorner.corner_2[0].y, workpieceCorner.corner_3[0].z);
 	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_B[1].x, workpieceCorner.corner_B[1].y, workpieceCorner.corner_B[1].z);
+	sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_2[1].x, workpieceCorner.corner_2[1].y, workpieceCorner.corner_3[1].z);
 	sw << dataStr << std::endl;
-	sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_B[2].x, workpieceCorner.corner_B[2].y, workpieceCorner.corner_B[2].z);
+	sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_2[2].x, workpieceCorner.corner_2[2].y, workpieceCorner.corner_3[2].z);
 	sw << dataStr << std::endl;
-	dist = sqrt(pow(workpieceCorner.corner_B[0].x - workpieceCorner.corner_B[2].x, 2) +
+	dist = sqrt(pow(workpieceCorner.corner_2[0].x - workpieceCorner.corner_2[2].x, 2) +
-		pow(workpieceCorner.corner_B[0].y - workpieceCorner.corner_B[2].y, 2) +
+		pow(workpieceCorner.corner_2[0].y - workpieceCorner.corner_2[2].y, 2) +
-		pow(workpieceCorner.corner_B[0].z - workpieceCorner.corner_B[2].z, 2));
+		pow(workpieceCorner.corner_2[0].z - workpieceCorner.corner_2[2].z, 2));
 	sprintf_s(dataStr, 250, "  Len: %g", dist);
 	sw << dataStr << std::endl;
 
+	if (workpieceCorner.workpieceType != 3)
+	{
+		sw << "C:" << std::endl;
+		sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_3[0].x, workpieceCorner.corner_3[0].y, workpieceCorner.corner_3[0].z);
+		sw << dataStr << std::endl;
+		sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_3[1].x, workpieceCorner.corner_3[1].y, workpieceCorner.corner_3[1].z);
+		sw << dataStr << std::endl;
+		sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_3[2].x, workpieceCorner.corner_3[2].y, workpieceCorner.corner_3[2].z);
+		sw << dataStr << std::endl;
+		dist = sqrt(pow(workpieceCorner.corner_3[0].x - workpieceCorner.corner_3[2].x, 2) +
+			pow(workpieceCorner.corner_3[0].y - workpieceCorner.corner_3[2].y, 2) +
+			pow(workpieceCorner.corner_3[0].z - workpieceCorner.corner_3[2].z, 2));
+		sprintf_s(dataStr, 250, "  Len: %g", dist);
+		sw << dataStr << std::endl;
+	}
+
+	if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4))
+	{
+		sw << "D:" << std::endl;
+		sprintf_s(dataStr, 250, "  corner_0: (%g, %g, %g)", workpieceCorner.corner_4[0].x, workpieceCorner.corner_4[0].y, workpieceCorner.corner_4[0].z);
+		sw << dataStr << std::endl;
+		sprintf_s(dataStr, 250, "  corner_1: (%g, %g, %g)", workpieceCorner.corner_4[1].x, workpieceCorner.corner_4[1].y, workpieceCorner.corner_4[1].z);
+		sw << dataStr << std::endl;
+		sprintf_s(dataStr, 250, "  corner_2: (%g, %g, %g)", workpieceCorner.corner_4[2].x, workpieceCorner.corner_4[2].y, workpieceCorner.corner_4[2].z);
+		sw << dataStr << std::endl;
+		dist = sqrt(pow(workpieceCorner.corner_4[0].x - workpieceCorner.corner_4[2].x, 2) +
+			pow(workpieceCorner.corner_4[0].y - workpieceCorner.corner_4[2].y, 2) +
+			pow(workpieceCorner.corner_4[0].z - workpieceCorner.corner_4[2].z, 2));
+		sprintf_s(dataStr, 250, "  Len: %g", dist);
+		sw << dataStr << std::endl;
+	}
+	sw << std::endl;
+
+	sprintf_s(dataStr, 250, "len_A1  : %g", workpieceCorner.len135_A1);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "len_A2  : %g", workpieceCorner.len225_A2);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "len_B2  : %g", workpieceCorner.len315_B2);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "len_B1  : %g", workpieceCorner.len45_B1);
+	sw << dataStr << std::endl;
 	sw.close();
 }
 
@@ -395,14 +413,14 @@ void _outputRGBDScanLapWeld_RGBD(
 	std::vector<std::vector<SVzNL3DPosition>>& scanLines,
 	SSX_BQworkpieceResult workpieceCorner,
 	bool outDebugInfo,
-	SSX_debugInfo* debugData)
+	std::vector<SSX_debugInfo>& debugData)
 {
 	int lineNum = (int)scanLines.size();
 	std::ofstream sw(fileName);
 	int realLines = lineNum;
 	if (workpieceCorner.workpieceType > 0)
 		realLines++;
-	if(debugData)
+	if(debugData.size() > 0)
 		realLines++;
 	sw << "LineNum:" << realLines << std::endl;
 	sw << "DataType: 0" << std::endl;
@@ -472,24 +490,29 @@ void _outputRGBDScanLapWeld_RGBD(
 
 	if (workpieceCorner.workpieceType > 0)
 	{
-		int linePtNum = 12;
+		std::vector<SVzNL3DPoint> ptBuffer;
+		for (int i = 0; i < 3; i++)
+			ptBuffer.push_back(workpieceCorner.corner_1[i]);
+		for (int i = 0; i < 3; i++)
+			ptBuffer.push_back(workpieceCorner.corner_2[i]);
+		if (workpieceCorner.workpieceType != 3)
+		{
+			for (int i = 0; i < 3; i++)
+				ptBuffer.push_back(workpieceCorner.corner_3[i]);
+		}
+		if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4))
+		{
+			for (int i = 0; i < 3; i++)
+				ptBuffer.push_back(workpieceCorner.corner_4[i]);
+		}
+		ptBuffer.push_back(workpieceCorner.center);
+		int linePtNum = (int)ptBuffer.size();
 		sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
 		lineNum++;
 
-		SVzNL3DPoint ptBuffer[12];
-		int idx = 0;
-		for (int i = 0; i < 3; i++)
-			ptBuffer[idx++] = workpieceCorner.corner_L[i];
-		for (int i = 0; i < 3; i++)
-			ptBuffer[idx++] = workpieceCorner.corner_R[i];
-		for (int i = 0; i < 3; i++)
-			ptBuffer[idx++] = workpieceCorner.corner_T[i];
-		for (int i = 0; i < 3; i++)
-			ptBuffer[idx++] = workpieceCorner.corner_B[i];
-
 		rgb = { 255, 0, 0 };
 		size = 15;
-		for (int j = 0; j < 12; j++)
+		for (int j = 0; j < linePtNum; j++)
 		{
 			float x = (float)ptBuffer[j].x;
 			float y = (float)ptBuffer[j].y;
@@ -507,18 +530,52 @@ void _outputRGBDScanLapWeld_RGBD(
 		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
 	}
 
-	if ((debugData) && (workpieceCorner.workpieceType > 0))
+	if ((debugData.size() > 0) && (workpieceCorner.workpieceType > 0))
 	{
-		int linePtNum = debugData[0].edge_size + debugData[0].edgeLink1_size + debugData[0].edgeLink2_size;
+		std::vector< SWD_3DPointPair> lines;
-		linePtNum += debugData[1].edge_size + debugData[1].edgeLink1_size + debugData[1].edgeLink2_size;
+		SWD_3DPointPair a_line;
-		linePtNum += debugData[2].edge_size + debugData[2].edgeLink1_size + debugData[2].edgeLink2_size;
+		a_line.pt1 = workpieceCorner.corner_1[0];
-		linePtNum += debugData[3].edge_size + debugData[3].edgeLink1_size + debugData[3].edgeLink2_size;
+		a_line.pt2 = workpieceCorner.corner_1[2];
+		lines.push_back(a_line);
+		a_line.pt1 = workpieceCorner.corner_1[1];
+		a_line.pt2 = workpieceCorner.center;
+		lines.push_back(a_line);
+
+		a_line.pt1 = workpieceCorner.corner_2[0];
+		a_line.pt2 = workpieceCorner.corner_2[2];
+		lines.push_back(a_line);
+		a_line.pt1 = workpieceCorner.corner_2[1];
+		a_line.pt2 = workpieceCorner.center;
+		lines.push_back(a_line);
+
+		if (workpieceCorner.workpieceType != 3)
+		{
+			a_line.pt1 = workpieceCorner.corner_3[0];
+			a_line.pt2 = workpieceCorner.corner_3[2];
+			lines.push_back(a_line);
+			a_line.pt1 = workpieceCorner.corner_3[1];
+			a_line.pt2 = workpieceCorner.center;
+			lines.push_back(a_line);
+		}
+		if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4))
+		{
+			a_line.pt1 = workpieceCorner.corner_4[0];
+			a_line.pt2 = workpieceCorner.corner_4[2];
+			lines.push_back(a_line);
+			a_line.pt1 = workpieceCorner.corner_4[1];
+			a_line.pt2 = workpieceCorner.center;
+			lines.push_back(a_line);
+		}
+
+		int linePtNum = 0;
+		for (int i = 0; i < (int)debugData.size(); i++)
+			linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].edgeLink2_size;
 		sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
 		lineNum++;
 
 		rgb = { 255, 0, 0 };
 		size = 3;
-		for (int i = 0; i < 4; i++)
+		for (int i = 0; i < (int)debugData.size(); i++)
 		{
 			for (int j = 0; j < debugData[i].edge_size; j++)
 			{
@@ -560,32 +617,10 @@ void _outputRGBDScanLapWeld_RGBD(
 		rgb = { 255, 0, 0 };
 		size = 3;
 		int lineIdx = 0;
-		for (int i = 0; i < 4; i++)
+		for (int i = 0; i < (int)lines.size(); i++)
 		{
-			SVzNL3DPoint pt0 = debugData[i].edge_ends[0];
+			SVzNL3DPoint pt0 = lines[i].pt1;
-			SVzNL3DPoint pt1 = debugData[i].edge_ends[1];
+			SVzNL3DPoint pt1 = lines[i].pt2;
-			sw << "Poly_" << lineIdx << "_2" << std::endl;
-			sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
-			sw << "{0,0}-{0,0}-";
-			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << 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;
-			lineIdx++;
-
-			pt0 = debugData[i].edge_link1_ends[0];
-			pt1 = debugData[i].edge_link1_ends[1];
-			sw << "Poly_" << lineIdx << "_2" << std::endl;
-			sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
-			sw << "{0,0}-{0,0}-";
-			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << 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;
-			lineIdx++;
-
-			pt0 = debugData[i].edge_link2_ends[0];
-			pt1 = debugData[i].edge_link2_ends[1];
 			sw << "Poly_" << lineIdx << "_2" << std::endl;
 			sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
 			sw << "{0,0}-{0,0}-";
@@ -596,8 +631,8 @@ void _outputRGBDScanLapWeld_RGBD(
 			lineIdx++;
 		}
 		//加一个直线，用于跳过显示工具bug
-		SVzNL3DPoint pt0 = debugData[0].edge_ends[0];
+		SVzNL3DPoint pt0 = lines[0].pt1;
-		SVzNL3DPoint pt1 = debugData[0].edge_ends[1];
+		SVzNL3DPoint pt1 = lines[0].pt2;
 		sw << "Poly_" << lineIdx << "_2" << std::endl;
 		sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
 		sw << "{0,0}-{0,0}-";
@@ -783,7 +818,7 @@ int main()
 			SSX_BQworkpiecePara workpieceParam;
 			workpieceParam.lineLen = 180.0; //直线段长度
 			int errCode = 0;
-			SSX_debugInfo debug_conturs[4];
+			std::vector<SSX_debugInfo> debug_conturs;
 			SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners(
 				scanLines,
 				cornerParam,
@@ -800,7 +835,7 @@ int main()
 			//输出测试结果
 			sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
 #if _OUTPUT_DEBUG_DATA
-			_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, debug_conturs);
+			_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, false, debug_conturs);
 #else
 			_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL);
 #endif

BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj

@@ -145,7 +145,7 @@
       <FunctionLevelLinking>true</FunctionLevelLinking>
       <IntrinsicFunctions>true</IntrinsicFunctions>
       <SDLCheck>true</SDLCheck>
-      <PreprocessorDefinitions>NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <PreprocessorDefinitions>NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;SG_API_LIBRARY;%(PreprocessorDefinitions)</PreprocessorDefinitions>
       <ConformanceMode>true</ConformanceMode>
       <PrecompiledHeader>NotUsing</PrecompiledHeader>
       <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>

baseAlgorithm/baseAlgorithm.vcxproj

@@ -160,10 +160,10 @@
       <EnableUAC>false</EnableUAC>
       <AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
       <AdditionalDependencies>opencv_world320.lib;%(AdditionalDependencies)</AdditionalDependencies>
+      <AdditionalOptions>/bigobj %(AdditionalOptions)</AdditionalOptions>
     </Link>
   </ItemDefinitionGroup>
   <ItemGroup>
-    <ClInclude Include="..\sourceCode\SG_algo_Export.h" />
     <ClInclude Include="..\sourceCode\SG_baseAlgo_Export.h" />
     <ClInclude Include="..\sourceCode\SG_baseDataType.h" />
     <ClInclude Include="..\sourceCode\SG_errCode.h" />

sourceCode/BQ_workpieceCornerExtraction_Export.h

@@ -13,10 +13,11 @@ typedef struct
 typedef struct
 {
 	int workpieceType; //1-节点1； 2-节点2； 3-节点3；4-节点4；0-未知； 其它-非法
-	SVzNL3DPoint corner_L[3]; 
+	//1,2,3,4为逆时针
-	SVzNL3DPoint corner_R[3];
+	SVzNL3DPoint corner_1[3]; 
-	SVzNL3DPoint corner_T[3];
+	SVzNL3DPoint corner_2[3];
-	SVzNL3DPoint corner_B[3];
+	SVzNL3DPoint corner_3[3];
+	SVzNL3DPoint corner_4[3];
 	SVzNL3DPoint center;  //工件中心点
 	double len135_A1;  //A1长度，从工件中心135度方向
 	double len45_B1;   //B1长度，从工件中心45度方向
@@ -27,15 +28,12 @@ typedef struct
 typedef struct
 {
 	int rgnIdx;
-	SVzNL3DPoint* edge;
 	int edge_size;
-	SVzNL3DPoint edge_ends[2];
-	SVzNL3DPoint* edgeLink_1;
 	int edgeLink1_size;
-	SVzNL3DPoint edge_link1_ends[2];
-	SVzNL3DPoint* edgeLink_2;
 	int edgeLink2_size;
-	SVzNL3DPoint edge_link2_ends[2];
+	SVzNL3DPoint* edge;
+	SVzNL3DPoint* edgeLink_1;
+	SVzNL3DPoint* edgeLink_2;
 }SSX_debugInfo;
 
 //读版本号
@@ -62,6 +60,6 @@ SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
 	SSG_planeCalibPara groundCalibPara,
 	SSX_BQworkpiecePara workpieceParam,
 #if _OUTPUT_DEBUG_DATA
-	SSX_debugInfo* debug_conturs,
+	std::vector<SSX_debugInfo>& debug_contours,
 #endif
 	int* errCode);

sourceCode/SG_baseAlgo_Export.h

@@ -1,8 +1,10 @@
 #pragma once
+
 #include "SG_algo_Export.h"
 #include <vector>
 #include <opencv2/opencv.hpp>
 
+
 //滤除离群点:z跳变门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
 SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier(
     SVzNL3DPosition* lineData, 
@@ -343,10 +345,38 @@ SG_APISHARED_EXPORT void lineFitting_abc(
     double* _b,
     double* _c);
 
+//计算Z均值
+SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);
+
+//计算角度差值，在0-180度范围
+SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2);
+
+//计算直线交点
 SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs(
     double a1, double b1, double c1,
     double a2, double b2, double c2);
 
+//计算过2点的直线方程
+SG_APISHARED_EXPORT void compute2ptLine(
+    SVzNL3DPoint pt1, 
+    SVzNL3DPoint pt2, 
+    double* _a, double* _b, double* _c);
+
+//旋转45度后的直线方程
+SG_APISHARED_EXPORT void rotateLine45Deg(
+    double _a, double _b, double _c,
+    double x0, double y0,
+    double* r_a, double* r_b, double* r_c);
+
+//计算直线角度
+SG_APISHARED_EXPORT double getLineAngle(double _a, double _b, double _c);
+
+//计算两点的2D距离
+SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2);
+
+//计算点到直线距离 
+SG_APISHARED_EXPORT double computePtDistToLine(double x0, double y0, double a, double b, double c);
+
 //计算垂足点，直线方程：y = kx + b
 SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint(
     double x0,

sourceCode/SG_baseDataType.h

@@ -478,3 +478,9 @@ typedef struct
 	int markID;
 	SVzNL3DPoint mark3D;
 }SWD_charuco3DMark;
+
+typedef struct
+{
+	SVzNL3DPoint pt1;
+	SVzNL3DPoint pt2;
+}SWD_3DPointPair;

sourceCode/SG_baseFunc.cpp

@@ -237,6 +237,138 @@ void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
 	*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
 }
 
+//拟合成通用直线方程，包括垂直
+void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b, double* _c)
+{
+	//判断是否为垂直
+	int dataSize = (int)inliers.size();
+	if (dataSize <2)
+		return;
+
+	double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
+	double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
+	std::vector< SVzNL3DPoint> fittingData;
+	if (deltaX < deltaY)
+	{
+		//x=ky+b 拟合
+		for (int i = 0; i < dataSize; i++)
+		{
+			SVzNL3DPoint a_fitPt;
+			a_fitPt.x = inliers[i].y;
+			a_fitPt.y = inliers[i].x;
+			a_fitPt.z = inliers[i].z;
+			fittingData.push_back(a_fitPt);
+		}
+		double k = 0, b = 0;
+		lineFitting(fittingData, &k, &b);
+		//ax+by+c
+		*_a = 1.0;
+		*_b = -k;
+		*_c = -b;
+	}
+	else
+	{
+		//y = kx+b拟合
+		double k = 0, b = 0;
+		lineFitting(inliers, &k, &b);
+		//ax+by+c
+		*_a = k;
+		*_b = -1;
+		*_c = b;
+	}
+	return;
+}
+
+//计算Z均值
+double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
+{
+	int ptNum = (int)pts.size();
+	int vldNum = 0;
+	double sumZ = 0;
+	for (int i = 0; i < ptNum; i++)
+	{
+		if (pts[i].z > 1e-4)
+		{
+			sumZ += pts[i].z;
+			vldNum++;
+		}
+	}
+	if (vldNum > 0)
+		return (sumZ / vldNum);
+	else
+		return 0;
+}
+
+SVzNL3DPoint computeLineCrossPt_abs(double a1, double b1, double c1, double a2, double b2, double c2)
+{
+	SVzNL3DPoint crossPt;
+	crossPt.x = (c2 * b1 - c1 * b2) / (a1 * b2 - a2 * b1);
+	crossPt.y = (c2 * a1 - c1 * a2) / (b1 * a2 - b2 * a1);
+	crossPt.z = 0;
+	return crossPt;
+}
+
+//计算角度差值，在0-180度范围
+double computeAngleDiff(double theta1, double theta2)
+{
+	double diff = theta1 - theta2;
+	if (diff < 0)
+		diff += 360;
+	if (diff > 180)
+		diff = 360 - diff;
+
+	return diff;
+}
+
+void compute2ptLine(SVzNL3DPoint pt1, SVzNL3DPoint pt2, double* _a, double* _b, double* _c)
+{
+	*_a = pt2.y - pt1.y;
+	*_b = pt1.x - pt2.x;
+	*_c = pt2.x * pt1.y - pt1.x * pt2.y;
+	return;
+}
+
+//旋转45度后的直线方程
+void rotateLine45Deg(
+	double _a, double _b, double _c,
+	double x0, double y0,
+	double* r_a, double* r_b, double* r_c)
+{
+	// 旋转后直线的系数（基于数学推导）
+	*r_a = _a + _b;
+	*r_b = _b - _a;
+	*r_c = -(*r_a) * x0 - (*r_b) * y0;
+	return;
+}
+
+double getLineAngle(double _a, double _b, double _c)
+{
+	if (_a = 0)
+		return 0;
+	else if (_b = 0)
+		return 90;
+	else
+	{
+		double theta = atan(-_a / _b) + PI / 2;
+		theta = (theta * 180.0) / PI;
+		return theta;
+	}
+}
+
+//计算两点的2D距离
+double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2)
+{
+	double len = sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2));
+	return len;
+}
+
+double computePtDistToLine(double x0, double y0, double a, double b, double c)
+{
+	double tmp = sqrt(pow(a, 2) + pow(b, 2));
+	double dist = abs(a * x0 + b * y0 + c) / tmp;
+	return dist;
+}
+
 //计算垂足点，直线方程：y = kx + b
 SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b)
 {