jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

rodAndBarDetection version 1.2.1 :

Browse Source 
增加了定位盘中心完整姿态输出
This commit is contained in:
jerryzeng 2026-04-15 17:37:33 +08:00
parent 4e8f6943d8
commit c6a52d1aba
3 changed files with 50 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
rodAndBarDetection_test/rodAndBarDetection_test.cpp
View File

@ -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();

16
sourceCode/rodAndBarDetection.cpp
View File

@ -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;
}

2
sourceCode/rodAndBarDetection_Export.h
View File

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