From 8bf516d9b259f30404fc0363c68e41821c04f89f Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 16 Dec 2025 23:09:30 +0800 Subject: [PATCH] =?UTF-8?q?gasFillingPortPosition=20ver=201.1.0=20?= =?UTF-8?q?=E6=94=B9=E8=BF=9B=E4=BA=86=E5=8A=A0=E6=B0=94=E5=8F=A3=E9=A1=B6?= =?UTF-8?q?=E9=9D=A2=E7=82=B9=E6=8F=90=E5=8F=96=E7=9A=84=E5=87=86=E7=A1=AE?= =?UTF-8?q?=E5=BA=A6=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../gasFillingPortPosition_test.cpp | 16 +++-- sourceCode/SG_baseAlgo_Export.h | 11 +++- sourceCode/SG_baseFunc.cpp | 8 +++ sourceCode/SG_lineFeature.cpp | 59 ++++++++++++++++--- sourceCode/gasFillingPortPosition.cpp | 55 +++++++++++++++-- 5 files changed, 124 insertions(+), 25 deletions(-) diff --git a/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp index ad3a51d..c1bbe38 100644 --- a/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp +++ b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp @@ -139,18 +139,16 @@ void _outputRGBDScan_fillingPort_RGBD( for (int i = 0; i < linePtNum; i++) { SVzNL3DPosition* pt3D = &scanLines[line][i]; - if (pt3D->nPointIdx > 0) - int kkk = 1; - if(pt3D->nPointIdx == 0) - { - rgb = { 200, 200, 200 }; - size = 1; - } - else + if (pt3D->nPointIdx == 1) { rgb = { 255, 97, 0 }; size = 3; } + else //if (pt3D->nPointIdx == 0) + { + rgb = { 200, 200, 200 }; + size = 1; + } float x = (float)pt3D->pt3D.x; float y = (float)pt3D->pt3D.y; float z = (float)pt3D->pt3D.z; @@ -228,7 +226,7 @@ int main() { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =4; + //fidx =7; char _scan_file[256]; sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx); diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 77a0ba9..7fab3a8 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -103,12 +103,13 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature( ); //直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线 -SG_APISHARED_EXPORT void wd_simpleLineSegment( +SG_APISHARED_EXPORT void wd_surfaceLineSegment( std::vector< SVzNL3DPosition>& lineData, int lineIdx, SVzNLRangeD lineLenRange, const SSG_lineSegParam lineSegPara, - std::vector& lineSegs); + std::vector& lineSegs, + std::vector& invlaidLineSegs); /// /// 提取激光线上的Jumping特征 @@ -376,6 +377,12 @@ SG_APISHARED_EXPORT void compute2ptLine( SVzNL3DPoint pt2, double* _a, double* _b, double* _c); +//计算过2点的直线方程,不使用结构体 +SG_APISHARED_EXPORT void compute2ptLine_2( + double x1, double y1, + double x2, double y2, + double* _a, double* _b, double* _c); + //旋转45度后的直线方程 SG_APISHARED_EXPORT void rotateLine45Deg( double _a, double _b, double _c, diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 442fbbb..1a61b98 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -394,6 +394,14 @@ void compute2ptLine(SVzNL3DPoint pt1, SVzNL3DPoint pt2, double* _a, double* _b, return; } +void compute2ptLine_2(double x1, double y1, double x2, double y2, double* _a, double* _b, double* _c) +{ + *_a = y2 - y1; + *_b = x1 - x2; + *_c = x2 * y1 - x1 * y2; + return; +} + //旋转45度后的直线方程 void rotateLine45Deg( double _a, double _b, double _c, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 5d26e02..c63b5d1 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -2559,7 +2559,7 @@ void wd_getRingArcFeature( } // 最小点集数量（小于此数无法拟合直线） -const int MIN_POINT_COUNT = 5; +const int MIN_POINT_COUNT = 3; //使用端点直线，检查点到直线的距离，大于门限的分割 void split( SSG_RUN a_run, @@ -2577,7 +2577,11 @@ void split( if ((pt1.z < 1e-4) || (pt2.z < 1e-4)) return; double _a, _b, _c; - compute2ptLine(pt1, pt2, &_a, &_b, &_c); + compute2ptLine_2( + pt1.y, pt1.z, + pt2.y, pt2.z, + &_a, &_b, &_c); + //compute2ptLine(pt1, pt2, &_a, &_b, &_c); double denominator = sqrt(_a * _a + _b * _b); //归一化 _a = _a / denominator; @@ -2585,13 +2589,13 @@ void split( _c = _c / denominator; double maxDist = 0; - double maxPos = 0; + int maxPos = 0; for (int i = start; i <= end; i++) { SVzNL3DPoint a_pt = lineData[i].pt3D; if (a_pt.z > 1e-4) { - double dist = abs(a_pt.x * _a + a_pt.y * _b + _c); + double dist = abs(a_pt.y * _a + a_pt.z * _b + _c); if (maxDist < dist) { maxDist = dist; @@ -2612,12 +2616,14 @@ void split( } //直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线 -void wd_simpleLineSegment( +//此算法对端面直线提取作了优化，每个分段只保留一个最佳直线段（去掉相邻的导角形成的直线,并记录） +void wd_surfaceLineSegment( std::vector< SVzNL3DPosition>& lineData, int lineIdx, SVzNLRangeD lineLenRange, const SSG_lineSegParam lineSegPara, - std::vector&lineSegs) + std::vector&lineSegs, + std::vector& invlaidLineSegs) { int dataSize = (int)lineData.size(); //去除零点 @@ -2669,7 +2675,9 @@ void wd_simpleLineSegment( { std::vector< SSG_RUN> segmentationLines; split(segs[si], lineData, lineSegPara.maxDist, segmentationLines); - //转成SSG_featureSemiCircle格式 + + //对于每个分段，只能有一个合格的直线段 + std::vector< SSG_featureSemiCircle> candiLines; for (int m = 0, m_max = (int)segmentationLines.size(); m < m_max; m++) { SSG_featureSemiCircle a_seg; @@ -2715,10 +2723,45 @@ void wd_simpleLineSegment( { a_seg.midPtIdx = midPtIdx; a_seg.midPt = lineData[midPtIdx].pt3D; - lineSegs.push_back(a_seg); + candiLines.push_back(a_seg); } } } + //转成SSG_featureSemiCircle格式 + if (candiLines.size() > 0) + { + //选择一个水平角度更小的 + double minTan = 0; + int bestIdx = -1; + for (int m = 0, m_max = (int)candiLines.size(); m < m_max; m++) + { + SSG_featureSemiCircle& a_seg = candiLines[m]; + SVzNL3DPoint ptStart = lineData[a_seg.startPtIdx].pt3D; + SVzNL3DPoint ptEnd = lineData[a_seg.endPtIdx].pt3D; + double tank = abs((ptEnd.z - ptStart.z) / (ptEnd.y - ptStart.y)); + if (bestIdx < 0) + { + minTan = tank; + bestIdx = m; + } + else + { + if (minTan > tank) + { + minTan = tank; + bestIdx = m; + } + } + } + lineSegs.push_back(candiLines[bestIdx]); + for (int m = 0, m_max = (int)candiLines.size(); m < m_max; m++) + { + if (m != bestIdx) + invlaidLineSegs.push_back(candiLines[m]); + } + } + else if (candiLines.size() == 1) + lineSegs.push_back(candiLines[0]); } } diff --git a/sourceCode/gasFillingPortPosition.cpp b/sourceCode/gasFillingPortPosition.cpp index bf907b3..ba18718 100644 --- a/sourceCode/gasFillingPortPosition.cpp +++ b/sourceCode/gasFillingPortPosition.cpp @@ -7,7 +7,8 @@ //读版本号 //version 1.0.0 : base version release to customer -std::string m_strVersion = "1.0.0"; +//version 1.1.0 : 改进了算法，顶面点提取更准确 +std::string m_strVersion = "1.1.0"; const char* wd_gasFillingPortPositionVersion(void) { return m_strVersion.c_str(); @@ -35,9 +36,10 @@ SSG_6DOF wd_getGasFillingPortPosition( lineLenRange.max = 1.2 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD); lineLenRange.min = 0.5 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD) / 2; std::vector> lineFeatures_v_raw; + std::vector> invalidFeatures_v_raw; //确定非直线特征，用于防止在水平处理时被认为是有效点 for (int line = 0; line < lineNum; line++) { - if (line == 390) + if (line == 310) int kkk = 1; std::vector& lineData = scanLines[line]; @@ -48,14 +50,17 @@ SSG_6DOF wd_getGasFillingPortPosition( sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); std::vector line_features; + std::vector invalid_features; int dataSize = (int)lineData.size(); - wd_simpleLineSegment( + wd_surfaceLineSegment( lineData, line, lineLenRange, lineSegPara, - line_features); + line_features, + invalid_features); lineFeatures_v_raw.push_back(line_features); + invalidFeatures_v_raw.push_back(invalid_features); } if (false == isGridData)//数据不是网格格式 @@ -81,6 +86,7 @@ SSG_6DOF wd_getGasFillingPortPosition( } //水平arc特征提取 std::vector> lineFeatures_h_raw; + std::vector> invalidFeatures_h_raw; //确定非直线特征，用于防止在垂直处理时被认为是有效点 int lineNum_h_raw = (int)hLines_raw.size(); for (int line = 0; line < lineNum_h_raw; line++) { @@ -92,14 +98,17 @@ SSG_6DOF wd_getGasFillingPortPosition( sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); std::vector line_features; + std::vector invalid_features; int dataSize = (int)lineData.size(); - wd_simpleLineSegment( + wd_surfaceLineSegment( lineData, line, lineLenRange, lineSegPara, - line_features); + line_features, + invalid_features); lineFeatures_h_raw.push_back(line_features); + invalidFeatures_h_raw.push_back(invalid_features); } //标注 @@ -235,6 +244,40 @@ SSG_6DOF wd_getGasFillingPortPosition( std::vector< SVzNL2DPoint>& obj_cluster = clusters[bestClusterIdx]; //提取端面上的点 + //首先将无效点置标志 + for (int line = 0; line < lineNum; line++) + { + std::vector& line_invalidFeature = invalidFeatures_v_raw[line]; + for (int n = 0; n < line_invalidFeature.size(); n++) + { + SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n]; + int lineIdx = a_invalidFeature.lineIdx; + for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++) + { + SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m]; + a_pt3d.nPointIdx = 2; + } + } + } + for (int line = 0; line < lineNum_h_raw; line++) + { + std::vector& line_invalidFeature = invalidFeatures_h_raw[line]; + for (int m = 0, m_max = (int)line_invalidFeature.size(); m < m_max; m++) + { + for (int n = 0; n < line_invalidFeature.size(); n++) + { + SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n]; + int ptIdx = a_invalidFeature.lineIdx; + for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++) + { + SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx]; + a_pt3d.nPointIdx = 3; + } + } + } + } + + //提取端面点 std::vector< cv::Point3f> planePts; for (int i = 0, i_max = (int)obj_cluster.size(); i < i_max; i++) {