From 7969a242ba040837ce361dec5377397ffee0e2ee Mon Sep 17 00:00:00 2001 From: cool609 Date: Sat, 11 Apr 2026 23:24:16 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E7=AE=97=E6=B3=95=EF=BC=8C?= =?UTF-8?q?=E8=8E=B7=E5=8F=96=E4=BA=86=E5=8D=95=E6=A0=B9=E9=92=A2=E7=AD=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BarIntersectionVisualizer.cpp | 48 ++-- .../visualization_demo/VisualizationDemo.cpp | 8 +- .../include/BarIntersectionParams.h | 4 + .../src/BarIntersection.cpp | 5 +- .../src/RegionGrowing.cpp | 259 ++++++++++-------- .../DetectBarIntersection/src/RegionGrowing.h | 1 + 6 files changed, 179 insertions(+), 146 deletions(-) diff --git a/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp b/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp index 373cb7a..4e09a2c 100644 --- a/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp +++ b/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp @@ -334,27 +334,27 @@ void BarIntersectionVisualizer::VisualizeResult( ); } -void BarIntersectionVisualizer::VisualizeBestCluster( - const SBarIntersectionResult& result, - const std::string& title -) { - if (result.bestClusterIndex < 0 || result.bestClusterPoints.empty()) { - std::cout << "No valid best cluster to visualize." << std::endl; - return; - } - - auto renderer = vtkSmartPointer::New(); - - // Best cluster filtered points (aligned coordinate system) - vtkActor* bestActor = static_cast( - CreatePointCloudActor( - result.bestClusterPoints.data(), - (int)result.bestClusterPoints.size(), - 0.0, 1.0, 0.3, 3.0 - ) - ); - renderer->AddActor(bestActor); - bestActor->Delete(); - - ShowWindow(renderer, title, m_interactive, this, "06_best_cluster.png"); -} +void BarIntersectionVisualizer::VisualizeBestCluster( + const SBarIntersectionResult& result, + const std::string& title +) { + if (result.bestClusterIndex < 0 || result.bestClusterPoints.empty()) { + std::cout << "No valid best cluster to visualize." << std::endl; + return; + } + + auto renderer = vtkSmartPointer::New(); + + // Best cluster filtered points (aligned coordinate system) + vtkActor* bestActor = static_cast( + CreatePointCloudActor( + result.bestClusterPoints.data(), + (int)result.bestClusterPoints.size(), + 0.0, 1.0, 0.3, 3.0 + ) + ); + renderer->AddActor(bestActor); + bestActor->Delete(); + + ShowWindow(renderer, title, m_interactive, this, "06_best_cluster.png"); +} diff --git a/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp b/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp index aa84876..ba8e761 100644 --- a/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp +++ b/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp @@ -251,10 +251,10 @@ int ProcessSingleFile( // Final visualization visualizer.VisualizeResult(points, rows, cols, result, "Final Result"); - // Best cluster visualization (filtered aligned points) - visualizer.VisualizeBestCluster(result, "Best Cluster (min Z, high pts)"); - - // Cleanup + // Best cluster visualization (filtered aligned points) + visualizer.VisualizeBestCluster(result, "Best Cluster (min Z, high pts)"); + + // Cleanup FreeBarIntersectionResult(&result); delete[] points; diff --git a/Algo/DetectBarIntersection/include/BarIntersectionParams.h b/Algo/DetectBarIntersection/include/BarIntersectionParams.h index b1b5607..ca4f4d2 100644 --- a/Algo/DetectBarIntersection/include/BarIntersectionParams.h +++ b/Algo/DetectBarIntersection/include/BarIntersectionParams.h @@ -37,6 +37,8 @@ struct SGrowthParams { float maxPerpendicularDeviationDeg; // Max deviation from 90 deg for centroid-connection test int minContinuousValidPointCount; // Min consecutive valid points kept on each laser line after plane filtering; <=1 disables float maxContinuousPointZTolerance; // Max z difference between adjacent points inside one valid run (mm) + float minBarDiameter; + float maxBarDiameter; SGrowthParams() : thresholdX(5.0f) @@ -49,6 +51,8 @@ struct SGrowthParams { , maxPerpendicularDeviationDeg(50.0f) , minContinuousValidPointCount(0) , maxContinuousPointZTolerance(5.0f) + , minBarDiameter(10.0f) + , maxBarDiameter(30.0f) {} }; diff --git a/Algo/DetectBarIntersection/src/BarIntersection.cpp b/Algo/DetectBarIntersection/src/BarIntersection.cpp index ee124e7..97d4a20 100644 --- a/Algo/DetectBarIntersection/src/BarIntersection.cpp +++ b/Algo/DetectBarIntersection/src/BarIntersection.cpp @@ -249,7 +249,10 @@ int DetectBarIntersections( // ============================================ std::vector clusters; bar_intersection::RegionGrowClusters( - alignedPoints, rows, cols, growthParams, clusters + alignedPoints, rows, cols, true, growthParams, clusters + ); + bar_intersection::RegionGrowClusters( + alignedPoints, rows, cols, false, growthParams, clusters ); // ============================================ diff --git a/Algo/DetectBarIntersection/src/RegionGrowing.cpp b/Algo/DetectBarIntersection/src/RegionGrowing.cpp index 440c37d..95c9081 100644 --- a/Algo/DetectBarIntersection/src/RegionGrowing.cpp +++ b/Algo/DetectBarIntersection/src/RegionGrowing.cpp @@ -114,86 +114,36 @@ static void FinalizeSegment(SLineSegment& seg) { } } -static bool HasColumnOverlap(const SLineSegment& a, const SLineSegment& b) { - return !(b.endCol < a.startCol || a.endCol < b.startCol); -} - -static bool HasAnyColumnOverlapWithSegments( - const SLineSegment& seg, - const std::vector& otherSegments -) { - for (size_t i = 0; i < otherSegments.size(); ++i) { - if (HasColumnOverlap(seg, otherSegments[i])) { - return true; - } - } - return false; -} - struct SSegmentRelationResult { - bool hasOverlap; bool hasMatch; float bestDistanceSq; }; struct SCarrySegmentState { - bool hasOverlap; bool hasMatch; }; static SSegmentRelationResult EvaluateSegmentRelation( const SLineSegment& prevSeg, const SLineSegment& currSeg, - const SVzNLPointXYZ* points, const SGrowthParams& params ) { SSegmentRelationResult result; - result.hasOverlap = false; result.hasMatch = false; result.bestDistanceSq = std::numeric_limits::max(); - if (!HasColumnOverlap(prevSeg, currSeg)) { - return result; - } - - const float pointDistanceThresholdSq = + const float centroidDistanceThresholdSq = params.thresholdX * params.thresholdX + params.thresholdY * params.thresholdY + params.thresholdZ * params.thresholdZ; - result.hasOverlap = true; - - size_t prevIdx = 0; - size_t currIdx = 0; - while (prevIdx < prevSeg.points.size() && currIdx < currSeg.points.size()) { - const SGridPoint& prevGp = prevSeg.points[prevIdx]; - const SGridPoint& currGp = currSeg.points[currIdx]; - - if (prevGp.col < currGp.col) { - ++prevIdx; - continue; - } - if (currGp.col < prevGp.col) { - ++currIdx; - continue; - } - - const SVzNLPointXYZ& prevPt = points[prevGp.linearIdx]; - const SVzNLPointXYZ& currPt = points[currGp.linearIdx]; - const float dx = currPt.x - prevPt.x; - const float dy = currPt.y - prevPt.y; - const float dz = currPt.z - prevPt.z; - const float distanceSq = dx * dx + dy * dy + dz * dz; - - if (distanceSq < pointDistanceThresholdSq) { - result.hasMatch = true; - if (distanceSq < result.bestDistanceSq) { - result.bestDistanceSq = distanceSq; - } - } - - ++prevIdx; - ++currIdx; + const float dx = currSeg.centroid.x - prevSeg.centroid.x; + const float dy = currSeg.centroid.y - prevSeg.centroid.y; + const float dz = currSeg.centroid.z - prevSeg.centroid.z; + const float distanceSq = dx * dx + dy * dy + dz * dz; + if (distanceSq < centroidDistanceThresholdSq) { + result.hasMatch = true; + result.bestDistanceSq = distanceSq; } return result; @@ -222,7 +172,9 @@ static bool IsValidPoint(const SVzNLPointXYZ& pt) { void EvaluateLine( const SVzNLPointXYZ* points, int count, - int row, + int step, + int startIdx, + int cols, const SGrowthParams& params, std::vector& currRowSegments, bool& bIsInvalidLine, @@ -234,7 +186,7 @@ void EvaluateLine( constexpr float kPi = 3.14159265358979323846f; const float searchDist = params.angleSearchDistance; const float posThresh = params.maxAxisDeviationFromXYDeg; - const float negThresh = params.maxPerpendicularDeviationDeg; + const float negThresh = -params.maxPerpendicularDeviationDeg; // YZ_PANEL: coord=y, XZ_PANEL: coord=x auto getCoord = [panelType](const SVzNLPointXYZ& pt) -> float { @@ -252,7 +204,8 @@ void EvaluateLine( // ================================================================ struct PointInfo { SVzNLPointXYZ point; - int linePos; + int row; + int col; int globalIdx; bool valid; float signedAngleDeg; @@ -261,13 +214,14 @@ void EvaluateLine( }; std::vector pts(count); - int startIdx = row * count; // global index of the first point in this line + auto curPoint = points; for (int i = 0; i < count; i++) { - int gIdx = startIdx + i; - pts[i].point = points[i]; - pts[i].linePos = i; + int gIdx = startIdx + i * step; + pts[i].point = *curPoint; + pts[i].row = gIdx / cols; + pts[i].col = gIdx % cols; pts[i].globalIdx = gIdx; - pts[i].valid = IsValidPoint(points[i]); + pts[i].valid = IsValidPoint(*curPoint); pts[i].signedAngleDeg = 0.0f; pts[i].hasAngle = false; pts[i].trend = keLineAngleTrend_Invalid; @@ -275,6 +229,8 @@ void EvaluateLine( if (pts[i].valid) { bIsInvalidLine = false; } + + curPoint += step; } for (int i = 0; i < count; i++) { @@ -324,10 +280,12 @@ void EvaluateLine( pts[i].hasAngle = true; - // 两线夹角：line1(backRef→cur) 延长后 与 line2(cur→fwdRef) 的有符号夹角 + // 局部弯折强度：line1(backRef→cur) 延长后 与 line2(cur→fwdRef) 的夹角绝对值。 // 与旧方案（backRef→fwdRef 整体坡度角）的本质区别： // 均匀倾斜面（无弯折）→ 0°；只有在 pts[i] 处发生方向变化时才得到非零角。 - // 正角度 = 向相机弯折（上升），负角度 = 远离相机弯折（下降进坑） + // signedAngleDeg 的语义： + // 符号 = 整体跳变方向（按坐标正向统一；低→高为正，高→低为负） + // 绝对值 = pts[i] 处的局部弯折强度 float curCoord = getCoord(cur); float v1_coord = curCoord - backCoord; // backRef → cur（水平分量） float v1_z = cur.z - backMeanZ; // backRef → cur（z 分量） @@ -337,9 +295,17 @@ void EvaluateLine( // 2D 叉积（z 分量）和点积，用于计算有符号夹角 float cross2d = v1_coord * v2_z - v1_z * v2_coord; float dot2d = v1_coord * v2_coord + v1_z * v2_z; - // 消除扫描方向（正向/反向）对叉积符号的影响 + // 消除扫描方向（正向/反向）对整体跳变方向符号的影响 float scanDirSign = ((v1_coord + v2_coord) >= 0.0f) ? 1.0f : -1.0f; - float slopeAngleDeg = std::atan2(-cross2d * scanDirSign, dot2d) * (180.0f / kPi); + float bendAngleDeg = std::atan2(std::fabs(cross2d), dot2d) * (180.0f / kPi); + float overallDeltaZ = (fwdMeanZ - backMeanZ) * scanDirSign; + float slopeAngleDeg = 0.0f; + if (overallDeltaZ > 1e-6f) { + slopeAngleDeg = bendAngleDeg; + } + else if (overallDeltaZ < -1e-6f) { + slopeAngleDeg = -bendAngleDeg; + } pts[i].signedAngleDeg = slopeAngleDeg; @@ -517,6 +483,36 @@ void EvaluateLine( segs = merged; } + const float minBarDiameter = params.minBarDiameter; + const float maxBarDiameter = params.maxBarDiameter; + for (size_t si = 0; si < segs.size(); ++si) { + const Segment& seg = segs[si]; + if (seg.type != ESegType::Flat) continue; + if (seg.length < minBarDiameter || seg.length > maxBarDiameter) continue; + + bool hasPoint = false; + SLineSegment lineSeg; + for (int pos = seg.startPos; pos <= seg.endPos; ++pos) { + if (!pts[pos].valid) continue; + + SGridPoint gp; + gp.row = pts[pos].row; + gp.col = pts[pos].col; + gp.linearIdx = pts[pos].globalIdx; + + if (!hasPoint) { + lineSeg = StartNewSegment(gp, pts[pos].point); + hasPoint = true; + } + else { + AppendPoint(lineSeg, gp, pts[pos].point); + } + } + + if (!hasPoint) continue; + FinalizeSegment(lineSeg); + currRowSegments.push_back(lineSeg); + } } static void ProcessCurrentRowSegmentPoint( @@ -586,13 +582,12 @@ static void MergeCurrentRowSegments( const std::vector& carriedRowSegments, std::vector& currRowSegments, UnionFind& uf, - const SVzNLPointXYZ* points, const SGrowthParams& params, std::vector& nextCarriedRowSegments ) { nextCarriedRowSegments.clear(); - std::vector prevStates(prevRowSegments.size(), SCarrySegmentState{ false, false }); - std::vector carriedStates(carriedRowSegments.size(), SCarrySegmentState{ false, false }); + std::vector prevStates(prevRowSegments.size(), SCarrySegmentState{ false }); + std::vector carriedStates(carriedRowSegments.size(), SCarrySegmentState{ false }); for (size_t ci = 0; ci < currRowSegments.size(); ++ci) { SLineSegment& currSeg = currRowSegments[ci]; @@ -603,12 +598,7 @@ static void MergeCurrentRowSegments( auto tryMatchSegments = [&](const std::vector& candidateSegments, std::vector& candidateStates) { for (size_t pi = 0; pi < candidateSegments.size(); ++pi) { const SLineSegment& prevSeg = candidateSegments[pi]; - const SSegmentRelationResult relation = EvaluateSegmentRelation(prevSeg, currSeg, points, params); - if (!relation.hasOverlap) { - continue; - } - - candidateStates[pi].hasOverlap = true; + const SSegmentRelationResult relation = EvaluateSegmentRelation(prevSeg, currSeg, params); if (!relation.hasMatch) { continue; } @@ -650,14 +640,14 @@ static void MergeCurrentRowSegments( for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) { const SLineSegment& prevSeg = prevRowSegments[pi]; - if (!prevStates[pi].hasOverlap && !prevStates[pi].hasMatch) { + if (!prevStates[pi].hasMatch) { nextCarriedRowSegments.push_back(prevSeg); } } for (size_t pi = 0; pi < carriedRowSegments.size(); ++pi) { const SLineSegment& carriedSeg = carriedRowSegments[pi]; - if (!carriedStates[pi].hasOverlap && !carriedStates[pi].hasMatch) { + if (!carriedStates[pi].hasMatch) { nextCarriedRowSegments.push_back(carriedSeg); } } @@ -743,11 +733,10 @@ int RegionGrowClusters( const SVzNLPointXYZ* points, int rows, int cols, + bool bHorizontalScan, const SGrowthParams& params, std::vector& outClusters ) { - outClusters.clear(); - if (!points || cols <= 0 || rows <= 0) { return 0; } @@ -759,45 +748,81 @@ int RegionGrowClusters( std::vector nextCarriedRowSegments; std::vector currRowSegments; - for (int row = 0; row < rows; ++row) { - const SVzNLPointXYZ* rowPoints = points + row * cols; + if (bHorizontalScan) + { + int nPointIdx = 0; + const SVzNLPointXYZ* rowPoints = points; + for (int row = 0; row < rows; ++row) { + bool bIsValidLine = false; + EvaluateLine( + rowPoints, + static_cast(cols), + 1, + nPointIdx, + cols, + params, + currRowSegments, + bIsValidLine, + EPanelType::YZ_PANEL + ); - bool bIsValidLine = false; - EvaluateLine( - rowPoints, - static_cast(cols), - row, - params, - currRowSegments, - bIsValidLine, - EPanelType::YZ_PANEL - ); + MergeCurrentRowSegments( + prevRowSegments, + carriedRowSegments, + currRowSegments, + uf, + params, + nextCarriedRowSegments + ); + allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end()); - ProcessCurrentRowSegmentPoint( - rowPoints, - static_cast(cols), - row, - params, - currRowSegments - ); + carriedRowSegments.swap(nextCarriedRowSegments); + nextCarriedRowSegments.clear(); + prevRowSegments.swap(currRowSegments); + currRowSegments.clear(); - MergeCurrentRowSegments( - prevRowSegments, - carriedRowSegments, - currRowSegments, - uf, - points, - params, - nextCarriedRowSegments - ); - allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end()); - - carriedRowSegments.swap(nextCarriedRowSegments); - nextCarriedRowSegments.clear(); - prevRowSegments.swap(currRowSegments); - currRowSegments.clear(); + nPointIdx += cols; + rowPoints += cols; + } } + else + { + int nPointIdx = 0; + const SVzNLPointXYZ* colPoints = points; + for (int col = 0; col < cols; ++col) { + bool bIsValidLine = false; + EvaluateLine( + colPoints, + rows, + cols, + nPointIdx, + cols, + params, + currRowSegments, + bIsValidLine, + EPanelType::XZ_PANEL + ); + + MergeCurrentRowSegments( + prevRowSegments, + carriedRowSegments, + currRowSegments, + uf, + params, + nextCarriedRowSegments + ); + allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end()); + + carriedRowSegments.swap(nextCarriedRowSegments); + nextCarriedRowSegments.clear(); + prevRowSegments.swap(currRowSegments); + currRowSegments.clear(); + colPoints++; + nPointIdx++; + } + } + FlattenTreesToClusters(allSegments, uf, points, params, outClusters); return static_cast(outClusters.size()); } diff --git a/Algo/DetectBarIntersection/src/RegionGrowing.h b/Algo/DetectBarIntersection/src/RegionGrowing.h index 72caf80..e7862e3 100644 --- a/Algo/DetectBarIntersection/src/RegionGrowing.h +++ b/Algo/DetectBarIntersection/src/RegionGrowing.h @@ -28,6 +28,7 @@ int RegionGrowClusters( const SVzNLPointXYZ* points, int rows, int cols, + bool bHorizontalScan, const SGrowthParams& params, std::vector& outClusters );