rodAndBarDetection version 1.2.1 :

增加了定位盘中心完整姿态输出

rodAndBarDetection_test/rodAndBarDetection_test.cpp

@@ -228,9 +228,11 @@ void _outputPlatePiseInfo(char* fileName, SSX_pointPoseInfo& centerInfo)
 {
 	std::ofstream sw(fileName);
 	char dataStr[250];
-	sprintf_s(dataStr, 250, "定位盘: center_( %g, %g, %g ), dir_( %g, %g, %g )",
+	sprintf_s(dataStr, 250, "定位盘: center_( %g, %g, %g ), normalDir_( %g, %g, %g ), xDir_( %g, %g, %g ), yDir_( %g, %g, %g )",
 		centerInfo.center.x, centerInfo.center.y, centerInfo.center.z,
-		centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z);
+		centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z,
+		centerInfo.xDir.x, centerInfo.xDir.y, centerInfo.xDir.z,
+		centerInfo.yDir.x, centerInfo.yDir.y, centerInfo.yDir.z);
 		sw << dataStr << std::endl;
 	sw.close();
 }
@@ -260,7 +262,7 @@ void _outputWeldSeamInfo(char* fileName, std::vector<SSX_weldSeamInfo>& weldSeam
 	int objNum = (int)weldSeamInfo.size();
 	for (int i = 0; i < objNum; i++)
 	{
-		sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_normal_( %g, %g, %g )",
+		sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_normal_( %g, %g, %g ), x_dir_(",
 			i + 1, weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z,
 			weldSeamInfo[i].normalDir.x, weldSeamInfo[i].normalDir.y, weldSeamInfo[i].normalDir.z);
 		sw << dataStr << std::endl;
@@ -496,6 +498,14 @@ void _outputRGBDScan_RGBD_centerPose(
 			SVzNL3DPoint pt1 = { poseInfo.center.x + len * poseInfo.normalDir.x,
 								 poseInfo.center.y + len * poseInfo.normalDir.y,
 								 poseInfo.center.z + len * poseInfo.normalDir.z };
+
+			SVzNL3DPoint basePt = poseInfo.center;
+			SVzNL3DPoint pt2 = { poseInfo.center.x + len * poseInfo.xDir.x,
+					 poseInfo.center.y + len * poseInfo.xDir.y,
+					 poseInfo.center.z + len * poseInfo.xDir.z };
+			SVzNL3DPoint pt3 = { poseInfo.center.x + len * poseInfo.yDir.x,
+								 poseInfo.center.y + len * poseInfo.yDir.y,
+								 poseInfo.center.z + len * poseInfo.yDir.z };
 			//显示法向量
 			sw << "Poly_" << lineIdx << "_2" << std::endl;
 			sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
@@ -505,6 +515,26 @@ void _outputRGBDScan_RGBD_centerPose(
 			sw << "{0,0}-{0,0}-";
 			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
 			lineIdx++;
+
+			rgb = { 0, 250, 0 };
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.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++;
+
+			rgb = { 0, 0, 250 };
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << pt3.x << "," << pt3.y << "," << pt3.z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
 		}
 	}
 	sw.close();

sourceCode/rodAndBarDetection.cpp

@@ -9,7 +9,8 @@
 //version 1.1.0 : 添加了地面调平和棒材定位
 //version 1.1.1 : 初始发布给客户的版本
 //version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
-std::string	m_strVersion = "1.2.0";
+//version 1.2.1 : 增加了定位盘中心完整姿态输出
+std::string	m_strVersion = "1.2.1";
 const char* wd_rodAndBarDetectionVersion(void)
 {
 	return m_strVersion.c_str();
@@ -832,6 +833,8 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
 	double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4;
 	resultPose.center = { center_x, center_y, center_z };
 	resultPose.normalDir = { 0, 0, -1.0 };
+	resultPose.yDir = { 0, -1.0, 0 };
+	resultPose.xDir = { 1.0, 0, 0 };
 	//旋转回去
 	for (int i = 0; i < lineNum; i++)
 	{
@@ -847,6 +850,17 @@ SSX_pointPoseInfo sx_getLocationPlatePose(
 	y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5];
 	z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8];
 	resultPose.normalDir = { x, y, z };
+
+	x = resultPose.xDir.x * poseR.invRMatrix[0] + resultPose.xDir.y * poseR.invRMatrix[1] + resultPose.xDir.z * poseR.invRMatrix[2];
+	y = resultPose.xDir.x * poseR.invRMatrix[3] + resultPose.xDir.y * poseR.invRMatrix[4] + resultPose.xDir.z * poseR.invRMatrix[5];
+	z = resultPose.xDir.x * poseR.invRMatrix[6] + resultPose.xDir.y * poseR.invRMatrix[7] + resultPose.xDir.z * poseR.invRMatrix[8];
+	resultPose.xDir = { x, y, z };
+
+	x = resultPose.yDir.x * poseR.invRMatrix[0] + resultPose.yDir.y * poseR.invRMatrix[1] + resultPose.yDir.z * poseR.invRMatrix[2];
+	y = resultPose.yDir.x * poseR.invRMatrix[3] + resultPose.yDir.y * poseR.invRMatrix[4] + resultPose.yDir.z * poseR.invRMatrix[5];
+	z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8];
+	resultPose.yDir = { x, y, z };
+
 	return resultPose;
 }
 

sourceCode/rodAndBarDetection_Export.h

@@ -14,6 +14,8 @@ typedef struct
 typedef struct
 {
 	SVzNL3DPoint center; //螺杆端部中心点
+	SVzNL3DPoint xDir;
+	SVzNL3DPoint yDir;
 	SVzNL3DPoint normalDir; //法向向量
 }SSX_pointPoseInfo;  //