From f517332278c1bd86743594094c3c2e1db027205a Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 27 Apr 2026 13:08:15 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20version=201.4.9=20:?= =?UTF-8?q?=20=E4=BF=AE=E6=AD=A3=E5=B7=A5=E4=BB=B6=E6=B3=95=E5=90=91?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=E7=9A=84=E4=B8=80=E4=B8=AABug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_baseAlgo_Export.h | 39 ++++++++++++++ sourceCode/SG_baseDataType.h | 7 +++ sourceCode/SG_baseFunc.cpp | 51 +++++++++++++++++++ sourceCode/workpieceHolePositioning.cpp | 6 ++- .../workpieceHolePositioning_test.cpp | 4 +- 5 files changed, 103 insertions(+), 4 deletions(-) diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index d287e93..a05da66 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -6,6 +6,20 @@ //向量叉乘 SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b); +// 向量数乘 +SG_APISHARED_EXPORT SVzNL3DPoint vec3_multiply(const SVzNL3DPoint& a, const double s); +// 点乘 dot +SG_APISHARED_EXPORT double vec3_dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b); +// 模长 +SG_APISHARED_EXPORT double vec3_length(const SVzNL3DPoint& a); +// 归一化（单位向量） +SG_APISHARED_EXPORT SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a); + +/** + * @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度 + * （v 必须在平面内，自动使用简化版罗德里格斯） + */ +SG_APISHARED_EXPORT SVzNL3DPoint wd_rotateVectorInPlane(const SVzNL3DPoint& v, const SVzNL3DPoint& n, double theta); //逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 SG_APISHARED_EXPORT SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle); @@ -144,6 +158,14 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM( const SSG_cornerParam cornerPara, SSG_lineFeature* line_features); +//根据距离连续性分段 +SG_APISHARED_EXPORT void wd_lineDataSegment_dist( + std::vector< SWD3DPointPostion>& lineData, + std::vector& segs, + const double maxDistTh, + const int minSegSize +); + SG_APISHARED_EXPORT void wd_lineDataSegment_zDist( std::vector< SVzNL3DPosition>& lineData, std::vector< SVzNL3DPosition>& vldPts, @@ -183,6 +205,16 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod( std::vector& line_rodArcs //环 ); +//提取corner极值（较早实现函数可以使用此函数进行代码优化） +SG_APISHARED_EXPORT void wd_searchCornerPeaks( + std::vector< SSG_pntDirAngle>& corners, + std::vector< SVzNL3DPosition>& vldPts, + const double minCornerTh, + double cornerMergeScale, + std::vector< SSG_pntDirAngle>& cornerPeakP, + std::vector< SSG_pntDirAngle>& cornerPeakM +); + /// /// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。 /// nPointIdx被重新定义成Feature类型 @@ -538,6 +570,13 @@ SG_APISHARED_EXPORT void lineFitting_abc( double* _b, double* _c); +//拟合成通用直线方程ax+by+c=0，包括垂直 +SG_APISHARED_EXPORT void indexingPtLineFitting_abc( + std::vector< SWD3DPointPostion>& inliers, + double* _a, + double* _b, + double* _c); + /** * @brief 空间直线最小二乘拟合 * @param points 输入的三维点集（至少2个点，否则无意义） diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 456aaba..e619576 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -39,6 +39,13 @@ typedef struct SWD3DPoint point; }SWDIndexing3DPoint; +typedef struct +{ + int lineIdx; + int ptIdx; + SVzNL3DPoint point; +}SWD3DPointPostion; + typedef struct { int lineIdx; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 04582e3..3c51baf 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -19,6 +19,57 @@ SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b) c.z = a.x * b.y - a.y * b.x; return c; } + + +// 向量数乘 +SVzNL3DPoint vec3_multiply(const SVzNL3DPoint& a, const double s) +{ + SVzNL3DPoint result = { a.x * s, a.y * s, a.z * s }; + return result; +} + +// 点乘 dot +double vec3_dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b) +{ + return (a.x * b.x + a.y * b.y + a.z * b.z); +} + +// 模长 +double vec3_length(const SVzNL3DPoint& a) +{ + return sqrt(a.x * a.x + a.y * a.y + a.z * a.z); +} + +// 归一化（单位向量） +SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a) +{ + SVzNL3DPoint result; + double len = vec3_length(a); + if (len < 1e-6) + result = { 0,0,0 }; + else + result = { a.x / len, a.y / len, a.z / len }; + return result; +} + +/** + * @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度 + * （v 必须在平面内，自动使用简化版罗德里格斯） + */ +SVzNL3DPoint wd_rotateVectorInPlane(const SVzNL3DPoint& v, const SVzNL3DPoint& n, double theta) +{ + SVzNL3DPoint k = vec3_normalize(n); // 旋转轴（单位法向量） + SVzNL3DPoint cross = vec3_cross(k, v); // k × v + double angle = theta * PI / 180.0; + double c = cos(angle); + double s = sin(angle); + // 简化公式：v' = v cosθ + (k×v) sinθ + SVzNL3DPoint t1 = vec3_multiply(v, c); + SVzNL3DPoint t2 = vec3_multiply(cross, s); + SVzNL3DPoint result = { t1.x + t2.x, t1.y + t2.y, t1.z + t2.z }; + return result;; +} + //逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 SVzNL3DPoint wd_rotate2D(const SVzNL3DPoint& pt, const double angle) { diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index c4e0422..ddb7f7e 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -19,7 +19,8 @@ //version 1.4.6 : 淇闂锛4涓瓟缁勬垚宸ヤ綔鏃讹紝闇瑕4涓瓟鐨勯珮搴﹀熀鏈竴鑷淬 //version 1.4.7 : 淇闂锛氬湪杩涜寮傜墿妫娴嬫椂锛岃绠梈SliceTh鏃躲傛坊鍔燴璁＄畻鐨勪繚鎶わ紝闃叉寮傚父鍊兼贩鍏 //version 1.4.8 : 灏嗗伐浠舵硶鍚戣皟鏁翠负鍨傜洿浜庡伐浠惰〃闈 -std::string m_strVersion = "1.4.8"; +//version 1.4.9 : 淇宸ヤ欢娉曞悜璁＄畻鐨勪竴涓狟ug +std::string m_strVersion = "1.4.9"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -214,7 +215,8 @@ void _getLinePoints( a_pt3d.x = (double)(pts_2d[i].x + roi2D.left) + 0.5; a_pt3d.y = (double)(pts_2d[i].y + roi2D.top) + 0.5; a_pt3d.z = quantiValue[pts_2d[i].x][pts_2d[i].y]; - Points3ds.push_back(a_pt3d); + if(a_pt3d.z > 1e-4) + Points3ds.push_back(a_pt3d); } } diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index a9a34ef..180b865 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void) }; SVzNLRange fileIdx[TPF_TEST_GROUP] = { - {6,6}, {1, 16}, {1,17} + {6,6}, {7, 7}, {1,17} }; const char* ver = wd_workpieceHolePositioningVersion(); @@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void) #endif #if TEST_COMPUTE_HOLE - for (int grp = 1; grp <= 2; grp++) + for (int grp = 1; grp <= 1; grp++) { SSG_planeCalibPara groundCalibPara; //鍒濆鍖栨垚鍗曚綅闃