From 4e8f6943d84fdf7180ae7ac23189a8795f0efce0 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 15 Apr 2026 14:10:45 +0800 Subject: [PATCH 1/2] =?UTF-8?q?wheelArchHeigthMeasure=20version=201.3.5?= =?UTF-8?q?=EF=BC=9A=20=E6=94=B9=E8=BF=9B=E8=BD=AE=E7=9C=89=E5=8F=96?= =?UTF-8?q?=E7=82=B9=E6=96=B9=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/wheelArchHeigthMeasure.cpp | 60 ++++++++++++++++++- .../wheelArchHeigthMeasure_test.cpp | 13 +++- 2 files changed, 69 insertions(+), 4 deletions(-) diff --git a/sourceCode/wheelArchHeigthMeasure.cpp b/sourceCode/wheelArchHeigthMeasure.cpp index 296c0fc..b651dee 100644 --- a/sourceCode/wheelArchHeigthMeasure.cpp +++ b/sourceCode/wheelArchHeigthMeasure.cpp @@ -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& e } } +SVzNL2DPoint _getRealArcPos(SVzNL2DPoint seedPos, double a, double b, double c, std::vector& 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>& 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; diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index 90b627e..acf38fc 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -973,7 +973,7 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector 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 Date: Wed, 15 Apr 2026 17:37:33 +0800 Subject: [PATCH 2/2] =?UTF-8?q?rodAndBarDetection=20version=201.2.1=20:=20?= =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=AE=9A=E4=BD=8D=E7=9B=98=E4=B8=AD?= =?UTF-8?q?=E5=BF=83=E5=AE=8C=E6=95=B4=E5=A7=BF=E6=80=81=E8=BE=93=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../rodAndBarDetection_test.cpp | 36 +++++++++++++++++-- sourceCode/rodAndBarDetection.cpp | 16 ++++++++- sourceCode/rodAndBarDetection_Export.h | 2 ++ 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index a067a68..0313203 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/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& 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(); diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index adb7c48..b925adf 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/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; } diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index a3cadb6..75b2fe8 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -14,6 +14,8 @@ typedef struct typedef struct { SVzNL3DPoint center; //螺杆端部中心点 + SVzNL3DPoint xDir; + SVzNL3DPoint yDir; SVzNL3DPoint normalDir; //法向向量 }SSX_pointPoseInfo; //