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

Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib

Browse Source
This commit is contained in:
杰仔 2026-04-15 17:52:37 +08:00
commit ee2fb2869d
5 changed files with 119 additions and 8 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; //

60
sourceCode/wheelArchHeigthMeasure.cpp
View File

@ -13,7 +13,8 @@
//version 1.3.2 : 针对不理想点云改进了算法，增强鲁棒性
//version 1.3.3 : 轮眉点的提取进行了改进，修正了可能的取点错误
//version 1.3.4 : 轮眉到轮心高度计算方法进行了修正，由Y高度差修改为两点距离
std::string m_strVersion = "1.3.4";
//version 1.3.5 : 改进轮眉取点方法。
std::string m_strVersion = "1.3.5";
const char* wd_wheelArchHeigthMeasureVersion(void)
{
return m_strVersion.c_str();
@ -519,6 +520,40 @@ void _genEdgeContinuousPos(SSG_endingTree&edgeTree, std::vector<SVzNL2DPoint>& e
}
}
SVzNL2DPoint _getRealArcPos(SVzNL2DPoint seedPos, double a, double b, double c, std::vector<SVzNL3DPosition>& lineData, double chkRng_Y)
{
SVzNL3DPosition seedPt = lineData[seedPos.y];
double minDist = -1;
int idx = -1;
for (int i = seedPos.y; i >= 0; i--)
{
if ( lineData[i].pt3D.z < 1e-4)
continue;
if (lineData[i].nPointIdx != 2)
break;
double yLen = abs(lineData[i].pt3D.y - seedPt.pt3D.y);
if (yLen > chkRng_Y)
break;
double dist = abs(lineData[i].pt3D.y * a + lineData[i].pt3D.z * b + c);
if (minDist < 0)
{
minDist = dist;
idx = i;
}
else if(minDist > dist)
{
minDist = dist;
idx = i;
}
}
if (idx >= 0)
seedPos.y = idx;
return seedPos;
}
//轮眉高度测量
WD_wheelArchInfo wd_wheelArchHeigthMeasure(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
@ -937,6 +972,29 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure(
*errCode = SX_ERR_NO_WHEEL_ARC;
return result;
}
//检查轮眉点：使用45方向的线作切线，取切点作为轮眉点
double chkRng_Y = 50.0;
#if 0
double vLine_a = downWheelPt.z - upWheelPt.z;
double vLine_b = upWheelPt.y - downWheelPt.y;
double vLine_c = downWheelPt.y * upWheelPt.z - upWheelPt.y * downWheelPt.z;
//旋转45度
// 旋转后直线的系数（基于数学推导）
double r_a = vLine_a + vLine_b;
double r_b = vLine_b - vLine_a;
double r_c = -r_a * upWheelPt.y - r_b * upWheelPt.z;
#else
//旋转30度
double vAngle = atan2(downWheelPt.z - upWheelPt.z, downWheelPt.y - upWheelPt.y);
vAngle += (60.0 / 180.0) * PI;
double r_a = tan(vAngle);
double r_b = -1.0;
double r_c = upWheelPt.z - r_a * upWheelPt.y;
#endif
arcPos = _getRealArcPos(arcPos, r_a, r_b, r_c, scanLines[arcPos.x], chkRng_Y);
//在XY平面生成拟合点
double outLineLen = 200;
SVzNL3DPoint arcPt = scanLines[arcPos.x][arcPos.y].pt3D;

13
wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp
View File

@ -973,7 +973,7 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector<std::vector< SVzNL
}
//if (objOps.size() > 0)
{
int ptNum = 3;
int ptNum = 4;
sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl;
rgb = { 255, 0, 0 };
@ -1000,6 +1000,13 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector<std::vector< SVzNL
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
x = (float)(wheelArcHeight.centerLine[0].x + wheelArcHeight.centerLine[1].x) / 2.0f;
y = (float)(wheelArcHeight.centerLine[0].y + wheelArcHeight.centerLine[1].y) / 2.0f;
z = (float)(wheelArcHeight.centerLine[0].z + wheelArcHeight.centerLine[1].z) / 2.0f;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
x = (float)wheelArcHeight.wheelArchPos.x;
y = (float)wheelArcHeight.wheelArchPos.y;
z = (float)wheelArcHeight.wheelArchPos.z;
@ -2725,7 +2732,7 @@ int main()
};
SVzNLRange fileIdx[TEST_GROUP] = {
{1,2}, {1,4}, {8,9}, {8,9}, {8,9},{9,9}
{1,2}, {1,4}, {1,9}, {5,9}, {1,10},{1,10}
};
SSG_planeCalibPara poseCalibPara;
@ -2749,7 +2756,7 @@ int main()
char _scan_file[256];
int endGroup = TEST_GROUP - 1;
for (int grp = 5; grp <= endGroup; grp++)
for (int grp = 2; grp <= endGroup; grp++)
{
char calibFile[250];
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);