diff --git a/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp b/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp index ada3d48..373cb7a 100644 --- a/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp +++ b/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp @@ -335,136 +335,26 @@ void BarIntersectionVisualizer::VisualizeResult( } void BarIntersectionVisualizer::VisualizeBestCluster( - const SVzNLPointXYZ* points, int count, const SBarIntersectionResult& result, const std::string& title ) { - if (!points || count <= 0 || result.validClusterIndices.empty()) { - std::cout << "No valid bar-like cluster to visualize." << std::endl; + if (result.bestClusterIndex < 0 || result.bestClusterPoints.empty()) { + std::cout << "No valid best cluster to visualize." << std::endl; return; } auto renderer = vtkSmartPointer::New(); - vtkActor* allPointsActor = static_cast( + // Best cluster filtered points (aligned coordinate system) + vtkActor* bestActor = static_cast( CreatePointCloudActor( - points, - count, - 0.75, 0.75, 0.75, 2.0 + result.bestClusterPoints.data(), + (int)result.bestClusterPoints.size(), + 0.0, 1.0, 0.3, 3.0 ) ); - renderer->AddActor(allPointsActor); - allPointsActor->Delete(); - - double clusterColors[][3] = { - {1.0, 0.2, 0.1}, {0.1, 0.8, 0.2}, {0.1, 0.6, 1.0}, - {1.0, 0.8, 0.1}, {1.0, 0.2, 0.8}, {0.0, 0.9, 0.9} - }; - const int numColors = 6; - - for (size_t i = 0; i < result.validClusterIndices.size(); ++i) { - const int clusterIdx = result.validClusterIndices[i]; - if (clusterIdx < 0 || clusterIdx >= result.clusterCount) { - continue; - } - - const SGrowthCluster& cluster = result.clusters[clusterIdx]; - std::vector clusterPts; - clusterPts.reserve(cluster.pointCount); - for (size_t j = 0; j < cluster.pointIndices.size(); ++j) { - const int linearIdx = cluster.pointIndices[j]; - if (linearIdx >= 0 && linearIdx < count) { - clusterPts.push_back(points[linearIdx]); - } - } - - if (clusterPts.empty()) { - continue; - } - - const int ci = static_cast(i % numColors); - vtkActor* clusterActor = static_cast( - CreatePointCloudActor( - clusterPts.data(), - (int)clusterPts.size(), - clusterColors[ci][0], clusterColors[ci][1], clusterColors[ci][2], - 4.5 - ) - ); - renderer->AddActor(clusterActor); - clusterActor->Delete(); - } + renderer->AddActor(bestActor); + bestActor->Delete(); ShowWindow(renderer, title, m_interactive, this, "06_best_cluster.png"); } - -void BarIntersectionVisualizer::VisualizeStep5Selection( - const SBarIntersectionResult& result, - const std::string& title -) { - if (result.step5BaseClusterIndex < 0 || - result.step5BaseClusterAlignedPoints.empty() || - result.step5FilteredAlignedPoints.empty()) { - std::cout << "No valid Step 5 selection to visualize." << std::endl; - return; - } - - auto renderer = vtkSmartPointer::New(); - - vtkActor* allPointsActor = static_cast( - CreatePointCloudActor( - result.step5FilteredAlignedPoints.data(), - static_cast(result.step5FilteredAlignedPoints.size()), - 0.75, 0.75, 0.75, 2.0 - ) - ); - renderer->AddActor(allPointsActor); - allPointsActor->Delete(); - - vtkActor* baseActor = static_cast( - CreatePointCloudActor( - result.step5BaseClusterAlignedPoints.data(), - static_cast(result.step5BaseClusterAlignedPoints.size()), - 1.0, 0.2, 0.1, 5.0 - ) - ); - renderer->AddActor(baseActor); - baseActor->Delete(); - - const size_t perpendicularClusterCount = std::min( - result.step5PerpendicularClusterAlignedPoints.size(), - result.step5PerpendicularClusterAlignedCentroids.size() - ); - - for (size_t i = 0; i < perpendicularClusterCount; ++i) { - const std::vector& clusterPoints = result.step5PerpendicularClusterAlignedPoints[i]; - if (!clusterPoints.empty()) { - vtkActor* clusterActor = static_cast( - CreatePointCloudActor( - clusterPoints.data(), - static_cast(clusterPoints.size()), - 0.1, 0.6, 1.0, 4.5 - ) - ); - renderer->AddActor(clusterActor); - clusterActor->Delete(); - } - - const SVzNLPointXYZ& clusterCentroid = result.step5PerpendicularClusterAlignedCentroids[i]; - vtkActor* lineActor = static_cast( - CreateLineActor( - result.step5BaseClusterAlignedCentroid.x, - result.step5BaseClusterAlignedCentroid.y, - result.step5BaseClusterAlignedCentroid.z, - clusterCentroid.x, - clusterCentroid.y, - clusterCentroid.z, - 1.0, 1.0, 0.1, 3.0 - ) - ); - renderer->AddActor(lineActor); - lineActor->Delete(); - } - - ShowWindow(renderer, title, m_interactive, this, "07_step5_selection.png"); -} diff --git a/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.h b/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.h index a2a1a9b..638796d 100644 --- a/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.h +++ b/Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.h @@ -54,18 +54,11 @@ public: const std::string& title = "Detection Result" ); - /** @brief Visualize best cluster highlighted on the full point cloud */ + /** @brief Visualize best cluster highlighted (filtered aligned points) */ void VisualizeBestCluster( - const SVzNLPointXYZ* points, int count, const SBarIntersectionResult& result, const std::string& title = "Best Cluster" ); - - /** @brief Visualize Step 5 base cluster and perpendicular matches */ - void VisualizeStep5Selection( - const SBarIntersectionResult& result, - const std::string& title = "Step 5 Selection (Aligned)" - ); void SetInteractive(bool interactive); void SetOutputDirectory(const std::string& dir); diff --git a/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp b/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp index b2c0797..aa84876 100644 --- a/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp +++ b/Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp @@ -251,17 +251,8 @@ int ProcessSingleFile( // Final visualization visualizer.VisualizeResult(points, rows, cols, result, "Final Result"); - // Visualize all valid bar-like clusters on the full point cloud - visualizer.VisualizeBestCluster( - points, rows * cols, - result, - "Valid Bar-Like Clusters on Full Point Cloud" - ); - - visualizer.VisualizeStep5Selection( - result, - "Step 5: Min-Z Base Cluster and XY-Perpendicular Matches (Aligned)" - ); + // Best cluster visualization (filtered aligned points) + visualizer.VisualizeBestCluster(result, "Best Cluster (min Z, high pts)"); // Cleanup FreeBarIntersectionResult(&result); diff --git a/Algo/DetectBarIntersection/include/BarIntersectionParams.h b/Algo/DetectBarIntersection/include/BarIntersectionParams.h index eafa786..bec2c8f 100644 --- a/Algo/DetectBarIntersection/include/BarIntersectionParams.h +++ b/Algo/DetectBarIntersection/include/BarIntersectionParams.h @@ -1,18 +1,18 @@ -#ifndef BAR_INTERSECTION_PARAMS_H +#ifndef BAR_INTERSECTION_PARAMS_H #define BAR_INTERSECTION_PARAMS_H #include "VZNL_Types.h" #include /** - * @brief RANSAC plane segmentation parameters + * @brief RANSAC 平面分割参数 */ struct SPlaneSegmentationParams { - float distanceThreshold; // Point-to-plane distance threshold (mm) - int maxIterations; // Maximum RANSAC iterations - int minPlanePoints; // Minimum plane point count - float minPlaneRatio; // Minimum plane point ratio - float heightThreshold; // Minimum height above the plane to keep (mm) + float distanceThreshold; // 点到平面距离阈值 (mm) + int maxIterations; // RANSAC 最大迭代次数 + int minPlanePoints; // 最小平面点数 + float minPlaneRatio; // 平面最小点数占比 + float heightThreshold; // 高于平面的最小高度，用于过滤平面点 (mm) SPlaneSegmentationParams() : distanceThreshold(1.0f) @@ -24,13 +24,13 @@ struct SPlaneSegmentationParams { }; /** - * @brief Region growing parameters + * @brief 区域生长参数 */ struct SGrowthParams { - float thresholdX; // Max centroid-x difference when merging adjacent row segments (mm) - float thresholdY; // Max y difference for adjacent points in the same row (mm) - float thresholdZ; // Max z difference for adjacent points in the same row (mm) - int minClusterSize; // Minimum cluster size + float thresholdX; // 跨行合并阈值：相邻行 segment 质心 x 的最大允许差 (mm) + float thresholdY; // 行内分段阈值：同行相邻点 y 的最大允许差 (mm) + float thresholdZ; // 行内分段阈值：同行相邻点 z 的最大允许差 (mm) + int minClusterSize; // 最小簇点数 float maxAxisDeviationFromXYDeg; // Max allowed axis deviation from the XY plane (deg) 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 @@ -49,13 +49,13 @@ struct SGrowthParams { }; /** - * @brief Region growing output: one connected cluster + * @brief 生长结果：一个连通簇 */ struct SGrowthCluster { - std::vector pointIndices; // Linear indices in the original rows*cols grid - SVzNL3DPointF centroid; - SVzNL3DPointF minBound; - SVzNL3DPointF maxBound; + std::vector pointIndices; // 簇内点在原始 rows*cols 网格中的线性索引 + SVzNL3DPointF centroid; // 簇质心 + SVzNL3DPointF minBound; // 包围盒最小角 + SVzNL3DPointF maxBound; // 包围盒最大角 int pointCount; SGrowthCluster() @@ -64,64 +64,37 @@ struct SGrowthCluster { }; /** - * @brief Detection result + * @brief 检测结果 */ struct SBarIntersectionResult { - // Plane information - SVzNL3DPointF planeNormal; - float planeD; // Plane equation ax + by + cz + d = 0 + // 平面信息 + SVzNL3DPointF planeNormal; // 平面法向量 + float planeD; // 平面方程 ax+by+cz+d=0 中的 d - // Clusters in the original coordinate system + // 簇（原始坐标系） int clusterCount; SGrowthCluster* clusters; - // Best cluster index, -1 means no valid cluster + // 最优簇索引（-1表示无有效簇） int bestClusterIndex; - // All cluster indices that satisfy the bar geometry constraint - std::vector validClusterIndices; - - // Step 5: the valid cluster with minimum z centroid - int step5BaseClusterIndex; - - // Step 5: valid clusters whose centroid-connection line is perpendicular to the base cluster axis - std::vector step5PerpendicularClusterIndices; - - // Step 5 visualization data kept in aligned coordinates - std::vector step5FilteredAlignedPoints; - std::vector step5BaseClusterAlignedPoints; - SVzNLPointXYZ step5BaseClusterAlignedCentroid; - std::vector > step5PerpendicularClusterAlignedPoints; - std::vector step5PerpendicularClusterAlignedCentroids; - - // Best cluster filtered points in the original coordinate system + // 最优簇的过滤后点云（对齐坐标系，用于可视化） std::vector bestClusterPoints; SBarIntersectionResult() : planeNormal(), planeD(0.0f) , clusterCount(0), clusters(nullptr) , bestClusterIndex(-1) - , step5BaseClusterIndex(-1) {} }; /** - * @brief Release memory in a detection result + * @brief 释放检测结果内存 */ inline void FreeBarIntersectionResult(SBarIntersectionResult* result) { if (result) { if (result->clusters) { delete[] result->clusters; result->clusters = nullptr; } result->clusterCount = 0; - result->bestClusterIndex = -1; - result->validClusterIndices.clear(); - result->step5BaseClusterIndex = -1; - result->step5PerpendicularClusterIndices.clear(); - result->step5FilteredAlignedPoints.clear(); - result->step5BaseClusterAlignedPoints.clear(); - result->step5BaseClusterAlignedCentroid = SVzNLPointXYZ(); - result->step5PerpendicularClusterAlignedPoints.clear(); - result->step5PerpendicularClusterAlignedCentroids.clear(); - result->bestClusterPoints.clear(); } } diff --git a/Algo/DetectBarIntersection/src/BarIntersection.cpp b/Algo/DetectBarIntersection/src/BarIntersection.cpp index 1fd30d6..3176dc8 100644 --- a/Algo/DetectBarIntersection/src/BarIntersection.cpp +++ b/Algo/DetectBarIntersection/src/BarIntersection.cpp @@ -610,8 +610,6 @@ BAR_INTERSECTION_API int DetectBarIntersections( // 在该范围内选点数最多的簇 // ============================================ int bestIdx = -1; - std::vector clusterLineMetrics(clusters.size()); - std::vector validClusterIndices; if (!clusters.empty()) { // 找所有簇中最小的 centroid.z（对齐空间） float minZ = std::numeric_limits::max(); @@ -626,31 +624,28 @@ BAR_INTERSECTION_API int DetectBarIntersections( // 在范围内找点数最多的簇 int maxPoints = 0; - float bestDistanceStdDev = std::numeric_limits::max(); - float bestInlierRatio = 0.0f; for (int i = 0; i < (int)clusters.size(); i++) { if (clusters[i].centroid.z > zUpperBound) continue; - - const SClusterLineMetrics lineMetrics = EvaluateClusterLinearity( - clusters[i], alignedPoints, totalPoints, growthParams - ); - clusterLineMetrics[i] = lineMetrics; - if (!lineMetrics.isValid) continue; - validClusterIndices.push_back(i); - - if (clusters[i].pointCount > maxPoints || - (clusters[i].pointCount == maxPoints && lineMetrics.distanceStdDev < bestDistanceStdDev) || - (clusters[i].pointCount == maxPoints && - std::abs(lineMetrics.distanceStdDev - bestDistanceStdDev) < 1e-6f && - lineMetrics.inlierRatio > bestInlierRatio)) { + if (clusters[i].pointCount > maxPoints) { maxPoints = clusters[i].pointCount; - bestDistanceStdDev = lineMetrics.distanceStdDev; - bestInlierRatio = lineMetrics.inlierRatio; bestIdx = i; } } } + // 提取最优簇的过滤后点云（对齐坐标系） + std::vector bestClusterFilteredPts; + if (bestIdx >= 0) { + const SGrowthCluster& bestCluster = clusters[bestIdx]; + bestClusterFilteredPts.reserve(bestCluster.pointCount); + for (int j = 0; j < (int)bestCluster.pointIndices.size(); j++) { + int linearIdx = bestCluster.pointIndices[j]; + if (linearIdx >= 0 && linearIdx < totalPoints) { + bestClusterFilteredPts.push_back(alignedPoints[linearIdx]); + } + } + } + // ============================================ // Transform cluster centroids back to original coordinate system // ============================================ @@ -661,113 +656,6 @@ BAR_INTERSECTION_API int DetectBarIntersections( } } - // ============================================ - // Step 5: from valid clusters, find the one with minimum z and then - // find clusters whose centroid-connection line is perpendicular to its axis - // ============================================ - int step5BaseClusterIndex = -1; - std::vector step5PerpendicularClusterIndices; - if (!validClusterIndices.empty()) { - float minValidClusterZ = std::numeric_limits::max(); - for (size_t i = 0; i < validClusterIndices.size(); ++i) { - const int clusterIdx = validClusterIndices[i]; - if (clusters[clusterIdx].centroid.z < minValidClusterZ) { - minValidClusterZ = clusters[clusterIdx].centroid.z; - step5BaseClusterIndex = clusterIdx; - } - } - - if (step5BaseClusterIndex >= 0) { - const SClusterLineMetrics& baseMetrics = clusterLineMetrics[step5BaseClusterIndex]; - SVzNLPointXYZ baseAxisDirection = - ComputeDirectionVector(baseMetrics.axisPointA, baseMetrics.axisPointB); - baseAxisDirection.z = 0.0f; - const float perpendicularTolerance = - std::sin(std::max(0.0f, std::min(growthParams.maxPerpendicularDeviationDeg, 89.0f)) * - 3.1415926f / 180.0f); - - for (size_t i = 0; i < validClusterIndices.size(); ++i) { - const int clusterIdx = validClusterIndices[i]; - if (clusterIdx == step5BaseClusterIndex) { - continue; - } - - SVzNLPointXYZ centroidConnection = {}; - centroidConnection.x = clusters[clusterIdx].centroid.x - clusters[step5BaseClusterIndex].centroid.x; - centroidConnection.y = clusters[clusterIdx].centroid.y - clusters[step5BaseClusterIndex].centroid.y; - centroidConnection.z = 0.0f; - - const float absDot = ComputeNormalizedAbsDot(baseAxisDirection, centroidConnection); - if (absDot <= perpendicularTolerance) { - step5PerpendicularClusterIndices.push_back(clusterIdx); - } - } - } - } - - std::vector step5FilteredAlignedPoints; - std::vector step5BaseClusterAlignedPoints; - SVzNLPointXYZ step5BaseClusterAlignedCentroid = {}; - std::vector > step5PerpendicularClusterAlignedPoints; - std::vector step5PerpendicularClusterAlignedCentroids; - - step5FilteredAlignedPoints.reserve(validCount); - for (int i = 0; i < totalPoints; ++i) { - if (IsValidPoint(alignedPoints[i])) { - step5FilteredAlignedPoints.push_back(alignedPoints[i]); - } - } - - if (step5BaseClusterIndex >= 0) { - const SGrowthCluster& baseCluster = clusters[step5BaseClusterIndex]; - step5BaseClusterAlignedCentroid.x = baseCluster.centroid.x; - step5BaseClusterAlignedCentroid.y = baseCluster.centroid.y; - step5BaseClusterAlignedCentroid.z = baseCluster.centroid.z; - step5BaseClusterAlignedPoints.reserve(baseCluster.pointCount); - for (size_t j = 0; j < baseCluster.pointIndices.size(); ++j) { - const int linearIdx = baseCluster.pointIndices[j]; - if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { - step5BaseClusterAlignedPoints.push_back(alignedPoints[linearIdx]); - } - } - - step5PerpendicularClusterAlignedPoints.reserve(step5PerpendicularClusterIndices.size()); - step5PerpendicularClusterAlignedCentroids.reserve(step5PerpendicularClusterIndices.size()); - for (size_t i = 0; i < step5PerpendicularClusterIndices.size(); ++i) { - const int clusterIdx = step5PerpendicularClusterIndices[i]; - const SGrowthCluster& cluster = clusters[clusterIdx]; - std::vector clusterAlignedPoints; - clusterAlignedPoints.reserve(cluster.pointCount); - for (size_t j = 0; j < cluster.pointIndices.size(); ++j) { - const int linearIdx = cluster.pointIndices[j]; - if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { - clusterAlignedPoints.push_back(alignedPoints[linearIdx]); - } - } - step5PerpendicularClusterAlignedPoints.push_back(std::move(clusterAlignedPoints)); - SVzNLPointXYZ clusterAlignedCentroid; - clusterAlignedCentroid.x = cluster.centroid.x; - clusterAlignedCentroid.y = cluster.centroid.y; - clusterAlignedCentroid.z = cluster.centroid.z; - step5PerpendicularClusterAlignedCentroids.push_back(clusterAlignedCentroid); - } - } - - // 提取最优簇的过滤后点云，并逆变换回原始坐标系，便于在整云上高亮显示 - std::vector bestClusterFilteredPts; - if (bestIdx >= 0) { - const SGrowthCluster& bestCluster = clusters[bestIdx]; - bestClusterFilteredPts.reserve(bestCluster.pointCount); - for (int j = 0; j < (int)bestCluster.pointIndices.size(); j++) { - int linearIdx = bestCluster.pointIndices[j]; - if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { - bestClusterFilteredPts.push_back( - InverseTransformPoint(alignedPoints[linearIdx], invRotation, planeCentroid) - ); - } - } - } - for (auto& cluster : clusters) { // Transform centroid float x = cluster.centroid.x; @@ -803,14 +691,6 @@ BAR_INTERSECTION_API int DetectBarIntersections( result->planeNormal = planeNormal; result->planeD = planeD; result->bestClusterIndex = bestIdx; - result->validClusterIndices = std::move(validClusterIndices); - result->step5BaseClusterIndex = step5BaseClusterIndex; - result->step5PerpendicularClusterIndices = std::move(step5PerpendicularClusterIndices); - result->step5FilteredAlignedPoints = std::move(step5FilteredAlignedPoints); - result->step5BaseClusterAlignedPoints = std::move(step5BaseClusterAlignedPoints); - result->step5BaseClusterAlignedCentroid = step5BaseClusterAlignedCentroid; - result->step5PerpendicularClusterAlignedPoints = std::move(step5PerpendicularClusterAlignedPoints); - result->step5PerpendicularClusterAlignedCentroids = std::move(step5PerpendicularClusterAlignedCentroids); result->bestClusterPoints = std::move(bestClusterFilteredPts); result->clusterCount = (int)clusters.size(); @@ -841,7 +721,6 @@ BAR_INTERSECTION_API void PrintDetectionResults( std::cout << "Plane Normal: (" << result.planeNormal.x << ", " << result.planeNormal.y << ", " << result.planeNormal.z << ")" << std::endl; std::cout << "Clusters detected: " << result.clusterCount << std::endl; - std::cout << "Valid bar-like clusters: " << result.validClusterIndices.size() << std::endl; if (verbose) { std::cout << "\n--- Clusters ---" << std::endl; @@ -865,33 +744,6 @@ BAR_INTERSECTION_API void PrintDetectionResults( } else { std::cout << " No valid best cluster found." << std::endl; } - - if (!result.validClusterIndices.empty()) { - std::cout << "\n--- Valid Cluster Indices ---" << std::endl; - for (size_t i = 0; i < result.validClusterIndices.size(); ++i) { - std::cout << " #" << result.validClusterIndices[i] << std::endl; - } - } - - std::cout << "\n--- Step 5 ---" << std::endl; - if (result.step5BaseClusterIndex >= 0 && result.step5BaseClusterIndex < result.clusterCount) { - const SGrowthCluster& base = result.clusters[result.step5BaseClusterIndex]; - std::cout << " Base: Cluster #" << result.step5BaseClusterIndex - << " pts=" << base.pointCount - << " centroid=(" << base.centroid.x << "," << base.centroid.y << "," << base.centroid.z << ")" - << std::endl; - } else { - std::cout << " No Step 5 base cluster found." << std::endl; - } - - if (!result.step5PerpendicularClusterIndices.empty()) { - std::cout << " Perpendicular clusters:" << std::endl; - for (size_t i = 0; i < result.step5PerpendicularClusterIndices.size(); ++i) { - std::cout << " #" << result.step5PerpendicularClusterIndices[i] << std::endl; - } - } else { - std::cout << " No perpendicular cluster found." << std::endl; - } } std::cout << "===========================================" << std::endl; } diff --git a/Algo/DetectBarIntersection/src/RegionGrowing.cpp b/Algo/DetectBarIntersection/src/RegionGrowing.cpp index 6ed55b6..193cb15 100644 --- a/Algo/DetectBarIntersection/src/RegionGrowing.cpp +++ b/Algo/DetectBarIntersection/src/RegionGrowing.cpp @@ -1,601 +1,471 @@ -#include "RegionGrowing.h" - -#include -#include -#include -#include - -namespace bar_intersection { -namespace { - -constexpr int TREE_STATE_ALIVE = 1; -constexpr int TREE_STATE_DEAD = 2; -constexpr float kInvalidPointEpsilon = 1e-6f; - -struct SGridPoint { - int row; - int col; - int linearIdx; -}; - -struct SLineFeature { - int row; - int startCol; - int endCol; - std::vector points; - float sumX; - float sumY; - float sumZ; - SVzNL3DPointF centroid; - int pointCount; -}; - -struct SGrowthTree { - int treeState; - int startRow; - int endRow; - std::vector nodes; -}; - -struct STreeAccumulator { - std::vector pointIndices; - float sumX; - float sumY; - float sumZ; - float minX; - float minY; - float minZ; - float maxX; - float maxY; - float maxZ; - int pointCount; -}; - -static bool IsValidPoint(const SVzNLPointXYZ& pt) { - return !(std::abs(pt.x) < kInvalidPointEpsilon && - std::abs(pt.y) < kInvalidPointEpsilon && - std::abs(pt.z) < kInvalidPointEpsilon); -} - -static SLineFeature StartNewFeature(const SGridPoint& gp, const SVzNLPointXYZ& pt) { - SLineFeature feature; - feature.row = gp.row; - feature.startCol = gp.col; - feature.endCol = gp.col; - feature.points.push_back(gp); - feature.sumX = pt.x; - feature.sumY = pt.y; - feature.sumZ = pt.z; - feature.centroid.x = pt.x; - feature.centroid.y = pt.y; - feature.centroid.z = pt.z; - feature.pointCount = 1; - return feature; -} - -static void AppendPointToFeature( - SLineFeature& feature, - const SGridPoint& gp, - const SVzNLPointXYZ& pt -) { - feature.points.push_back(gp); - feature.endCol = gp.col; - feature.sumX += pt.x; - feature.sumY += pt.y; - feature.sumZ += pt.z; - feature.pointCount++; -} - -static void FinalizeFeature(SLineFeature& feature) { - if (feature.pointCount <= 0) { - return; - } - - feature.centroid.x = feature.sumX / feature.pointCount; - feature.centroid.y = feature.sumY / feature.pointCount; - feature.centroid.z = feature.sumZ / feature.pointCount; -} - -static void RemoveRowOutliersInPlace( - SVzNLPointXYZ* rowPoints, - int cols, - const SGrowthParams& params -) { - // 行内离群点抑制： - // 1. 如果某个有效点左右都没有有效邻居，则它大概率是孤立噪声，直接抑制。 - // 2. 如果左右邻点彼此趋势平稳，但当前点相对左右两侧同时发生明显跳变， - // 则将其视为尖刺噪声。 - // 该步骤只在单行内部做判断，目的是先清掉容易破坏连续性的局部异常点。 - if (!rowPoints || cols <= 0) { - return; - } - - std::vector suppressFlags(static_cast(cols), false); - const float spikeScale = 1.5f; - - for (int col = 0; col < cols; ++col) { - const SVzNLPointXYZ& pt = rowPoints[col]; - if (!IsValidPoint(pt)) { - continue; - } - - const bool hasPrev = (col > 0) && IsValidPoint(rowPoints[col - 1]); - const bool hasNext = (col + 1 < cols) && IsValidPoint(rowPoints[col + 1]); - - // 左右都不存在有效邻点，说明该点无法与任何局部结构形成连续片段， - // 直接标记为待抑制。 - if (!hasPrev && !hasNext) { - suppressFlags[static_cast(col)] = true; - continue; - } - - // 只有一侧存在邻点时，证据不足以判定该点一定是尖刺， - // 保留给后续的特征提取与跨行生长阶段处理。 - if (!(hasPrev && hasNext)) { - continue; - } - - const SVzNLPointXYZ& prevPt = rowPoints[col - 1]; - const SVzNLPointXYZ& nextPt = rowPoints[col + 1]; - const float prevNextYDiff = std::abs(nextPt.y - prevPt.y); - const float prevNextZDiff = std::abs(nextPt.z - prevPt.z); - const float prevYDiff = std::abs(pt.y - prevPt.y); - const float nextYDiff = std::abs(pt.y - nextPt.y); - const float prevZDiff = std::abs(pt.z - prevPt.z); - const float nextZDiff = std::abs(pt.z - nextPt.z); - - const bool neighbourTrendStable = - prevNextYDiff <= params.thresholdY && - prevNextZDiff <= params.thresholdZ; - const bool obviousSpike = - prevYDiff > params.thresholdY * spikeScale && - nextYDiff > params.thresholdY * spikeScale && - prevZDiff > params.thresholdZ * spikeScale && - nextZDiff > params.thresholdZ * spikeScale; - - // 仅当“两侧本身连续”且“当前点对两侧都显著偏离”同时成立时， - // 才把当前点视为孤立尖刺，避免误删真实边缘点。 - if (neighbourTrendStable && obviousSpike) { - suppressFlags[static_cast(col)] = true; - } - } - - for (int col = 0; col < cols; ++col) { - if (!suppressFlags[static_cast(col)]) { - continue; - } - - // 将被抑制的点清零。后续流程通过 IsValidPoint 判零值无效点， - // 因此这里不需要额外的标记位。 - rowPoints[col].x = 0.0f; - rowPoints[col].y = 0.0f; - rowPoints[col].z = 0.0f; - } -} - -static void ExtractRowFeatures( - const SVzNLPointXYZ* rowPoints, - int row, - int cols, - const SGrowthParams& params, - std::vector& outFeatures -) { - // 行内特征提取： - // 按列从左到右扫描，把在 Y/Z 上连续变化的点合并成一个 line feature。 - // 一个 feature 表示当前扫描行上的一段连续条带片段，后续跨行生长时 - // 不再直接匹配单点，而是匹配这些更稳定的片段摘要。 - outFeatures.clear(); - if (!rowPoints || cols <= 0) { - return; - } - - const int rowOffset = row * cols; - bool hasOpenFeature = false; - SLineFeature openFeature; - - for (int col = 0; col < cols; ++col) { - const SVzNLPointXYZ& pt = rowPoints[col]; - if (!IsValidPoint(pt)) { - // 无效点天然形成分段边界：如果当前存在打开的 feature， - // 则先收尾并输出。 - if (hasOpenFeature) { - FinalizeFeature(openFeature); - outFeatures.push_back(openFeature); - hasOpenFeature = false; - } - continue; - } - - SGridPoint gp; - gp.row = row; - gp.col = col; - gp.linearIdx = rowOffset + col; - - // 当前没有打开的 feature，说明遇到了一个新片段的起点。 - if (!hasOpenFeature) { - openFeature = StartNewFeature(gp, pt); - hasOpenFeature = true; - continue; - } - - const SGridPoint& lastGp = openFeature.points.back(); - const SVzNLPointXYZ& lastPt = rowPoints[lastGp.col]; - const float yDiff = std::abs(pt.y - lastPt.y); - const float zDiff = std::abs(pt.z - lastPt.z); - - // 与当前片段最后一个点在 Y/Z 上足够接近，则视为同一条连续片段， - // 继续向右扩展。 - if (yDiff <= params.thresholdY && zDiff <= params.thresholdZ) { - AppendPointToFeature(openFeature, gp, pt); - continue; - } - - // 否则说明当前点与已有片段发生断裂，先结束旧片段，再以该点开启新片段。 - FinalizeFeature(openFeature); - outFeatures.push_back(openFeature); - openFeature = StartNewFeature(gp, pt); - hasOpenFeature = true; - } - - // 扫描结束后，如果仍有未关闭的片段，需要补一次收尾。 - if (hasOpenFeature) { - FinalizeFeature(openFeature); - outFeatures.push_back(openFeature); - } -} - -static float EstimateTypicalRowStepX( - const SVzNLPointXYZ* points, - int rows, - int cols -) { - // 估计扫描线在 X 方向上的典型行间距。 - // 做法不是直接取相邻点差，而是先求每一行所有有效点的平均 X， - // 再统计相邻有效行的平均 X 差值，最后取中位数。 - // 使用中位数而不是均值，可以降低个别缺行、异常行对步长估计的影响。 - std::vector xDiffs; - bool hasPrevMeanX = false; - float prevMeanX = 0.0f; - - for (int row = 0; row < rows; ++row) { - const SVzNLPointXYZ* rowPoints = points + row * cols; - float sumX = 0.0f; - int validCount = 0; - for (int col = 0; col < cols; ++col) { - if (!IsValidPoint(rowPoints[col])) { - continue; - } - sumX += rowPoints[col].x; - validCount++; - } - - // 整行没有有效点时，不参与步长估计。 - if (validCount <= 0) { - continue; - } - - const float meanX = sumX / validCount; - if (hasPrevMeanX) { - const float diff = std::abs(meanX - prevMeanX); - // 忽略接近 0 的差值，避免数值噪声污染中位数统计。 - if (diff > kInvalidPointEpsilon) { - xDiffs.push_back(diff); - } - } - prevMeanX = meanX; - hasPrevMeanX = true; - } - - // 如果无法从数据中估计稳定步长，则退化为 1.0f， - // 让后续最大跨行数仍能得到一个可用默认值。 - if (xDiffs.empty()) { - return 1.0f; - } - - std::sort(xDiffs.begin(), xDiffs.end()); - const float medianStep = xDiffs[xDiffs.size() / 2]; - return (medianStep > kInvalidPointEpsilon) ? medianStep : 1.0f; -} - -static int DeriveMaxLineSkipNum(const SGrowthParams& params, float rowStepX) { - // 将物理空间里的 X 向允许跨度，换算为“最多可跨过多少行”的整数约束。 - // 例如 thresholdX 大约等于 2 个 rowStepX 时，就允许 feature 在跨行 - // 匹配时容忍更大的行号间隔。 - // 返回值至少为 2，表示除了相邻行以外，默认还允许跨过 1 行空洞。 - if (rowStepX <= kInvalidPointEpsilon || params.thresholdX <= kInvalidPointEpsilon) { - return 2; - } - - const int maxLineSkipNum = - 1 + static_cast(std::floor(params.thresholdX / rowStepX)); - return (maxLineSkipNum > 1) ? maxLineSkipNum : 2; -} - -static int ComputeColumnGap(const SLineFeature& lhs, const SLineFeature& rhs) { - if (lhs.endCol < rhs.startCol) { - return rhs.startCol - lhs.endCol - 1; - } - - if (rhs.endCol < lhs.startCol) { - return lhs.startCol - rhs.endCol - 1; - } - - return 0; -} - -static bool TryGrowFeature( - const SLineFeature& feature, - int rowIdx, - std::vector& trees, - const SGrowthParams& params, - int maxLineSkipNum -) { - int bestTreeIdx = -1; - float bestScore = std::numeric_limits::max(); - - for (size_t i = 0; i < trees.size(); ++i) { - SGrowthTree& tree = trees[i]; - if (tree.treeState == TREE_STATE_DEAD || tree.nodes.empty()) { - continue; - } - - const SLineFeature& lastNode = tree.nodes.back(); - if (lastNode.row == feature.row) { - continue; - } - - const float xDiff = std::abs(feature.centroid.x - lastNode.centroid.x); - const float yDiff = std::abs(feature.centroid.y - lastNode.centroid.y); - const float zDiff = std::abs(feature.centroid.z - lastNode.centroid.z); - const int lineDiff = std::abs(feature.row - lastNode.row); - const int columnGap = ComputeColumnGap(feature, lastNode); - - if (yDiff <= params.thresholdY && - zDiff <= params.thresholdZ && - ((lineDiff <= maxLineSkipNum) || (xDiff <= params.thresholdX))) { - const float score = - yDiff + - zDiff + - 0.25f * xDiff + - static_cast(columnGap) + - 0.5f * static_cast(lineDiff - 1); - - if (score < bestScore) { - bestScore = score; - bestTreeIdx = static_cast(i); - } - } - } - - if (bestTreeIdx < 0) { - return false; - } - - SGrowthTree& bestTree = trees[static_cast(bestTreeIdx)]; - bestTree.endRow = rowIdx; - bestTree.nodes.push_back(feature); - return true; -} - -static bool IsValidTree(const SGrowthTree& tree) { - return tree.nodes.size() >= 2; -} - -static void FinalizeInactiveTrees( - int rowIdx, - bool isLastRow, - int maxLineSkipNum, - std::vector& trees -) { - for (int i = static_cast(trees.size()) - 1; i >= 0; --i) { - SGrowthTree& tree = trees[static_cast(i)]; - if (tree.treeState != TREE_STATE_ALIVE || tree.nodes.empty()) { - continue; - } - - const int lineDiff = rowIdx - tree.nodes.back().row; - if ((lineDiff > maxLineSkipNum) || isLastRow) { - tree.treeState = TREE_STATE_DEAD; - if (!IsValidTree(tree)) { - trees.erase(trees.begin() + i); - } - } - } -} - -static void GrowRowFeatures( - int rowIdx, - bool isLastRow, - const std::vector& rowFeatures, - std::vector& trees, - const SGrowthParams& params, - int maxLineSkipNum -) { - // 跨行生长： - // 当前行的每个 feature 都尝试挂接到已有 tree 的末端节点上。 - // TryGrowFeature 内部会综合质心距离、列间断裂以及行间距选择最优 tree。 - // 若没有任何 tree 满足约束，则该 feature 启动一棵新 tree。 - for (size_t i = 0; i < rowFeatures.size(); ++i) { - const SLineFeature& feature = rowFeatures[i]; - if (!TryGrowFeature(feature, rowIdx, trees, params, maxLineSkipNum)) { - SGrowthTree newTree; - newTree.treeState = TREE_STATE_ALIVE; - newTree.startRow = rowIdx; - newTree.endRow = rowIdx; - newTree.nodes.push_back(feature); - trees.push_back(newTree); - } - } - - // 当前行处理完后，检查哪些 tree 已经长时间没有被新 feature 延续。 - // 这些 tree 被置为死亡；若节点数过少，则直接丢弃，避免形成短小伪聚类。 - FinalizeInactiveTrees( - rowIdx, - isLastRow, - maxLineSkipNum, - trees - ); -} - -static void FlattenTreesToClusters( - const std::vector& trees, - const SVzNLPointXYZ* points, - const SGrowthParams& params, - std::vector& outClusters -) { - // 将中间态的 growth tree 转成最终输出 cluster。 - // tree 内保存的是按行组织的 feature，而最终 cluster 需要的是： - // 1. 展平后的点索引集合； - // 2. 质心与包围盒； - // 3. 满足最小点数阈值的有效聚类。 - outClusters.clear(); - - for (size_t t = 0; t < trees.size(); ++t) { - const SGrowthTree& tree = trees[t]; - // 少于 2 个节点的 tree 缺少跨行连续性，视为不稳定候选，直接跳过。 - if (!IsValidTree(tree)) { - continue; - } - - STreeAccumulator acc; - acc.sumX = 0.0f; - acc.sumY = 0.0f; - acc.sumZ = 0.0f; - acc.minX = std::numeric_limits::max(); - acc.minY = std::numeric_limits::max(); - acc.minZ = std::numeric_limits::max(); - acc.maxX = -std::numeric_limits::max(); - acc.maxY = -std::numeric_limits::max(); - acc.maxZ = -std::numeric_limits::max(); - acc.pointCount = 0; - - for (size_t n = 0; n < tree.nodes.size(); ++n) { - const SLineFeature& feature = tree.nodes[n]; - for (size_t p = 0; p < feature.points.size(); ++p) { - const int linearIdx = feature.points[p].linearIdx; - const SVzNLPointXYZ& pt = points[linearIdx]; - - // 把 tree 中每个 feature 重新展开成原始点集合，同时累计 - // cluster 的一阶统计量与包围盒边界。 - acc.pointIndices.push_back(linearIdx); - acc.sumX += pt.x; - acc.sumY += pt.y; - acc.sumZ += pt.z; - if (pt.x < acc.minX) acc.minX = pt.x; - if (pt.y < acc.minY) acc.minY = pt.y; - if (pt.z < acc.minZ) acc.minZ = pt.z; - if (pt.x > acc.maxX) acc.maxX = pt.x; - if (pt.y > acc.maxY) acc.maxY = pt.y; - if (pt.z > acc.maxZ) acc.maxZ = pt.z; - acc.pointCount++; - } - } - - // 点数过小的 tree 通常是噪声或残缺目标，不输出为最终 cluster。 - if (acc.pointCount < params.minClusterSize) { - continue; - } - - SGrowthCluster cluster; - cluster.pointIndices = acc.pointIndices; - cluster.pointCount = acc.pointCount; - cluster.centroid.x = acc.sumX / acc.pointCount; - cluster.centroid.y = acc.sumY / acc.pointCount; - cluster.centroid.z = acc.sumZ / acc.pointCount; - cluster.minBound.x = acc.minX; - cluster.minBound.y = acc.minY; - cluster.minBound.z = acc.minZ; - cluster.maxBound.x = acc.maxX; - cluster.maxBound.y = acc.maxY; - cluster.maxBound.z = acc.maxZ; - outClusters.push_back(cluster); - } - - // 按点数从大到小排序，方便调用方优先处理主目标。 - std::sort( - outClusters.begin(), - outClusters.end(), - [](const SGrowthCluster& lhs, const SGrowthCluster& rhs) { - return lhs.pointCount > rhs.pointCount; - } - ); -} - -} // namespace - -int RegionGrowClusters( - const SVzNLPointXYZ* points, - int rows, - int cols, - const SGrowthParams& params, - std::vector& outClusters -) { - // 每次调用都从空结果开始，避免调用方看到上一次遗留的聚类内容。 - outClusters.clear(); - - // 基本输入检查：算法默认输入点云按 rows x cols 的规则网格排列， - // 因此指针为空或尺寸非法时直接返回。 - if (!points || rows <= 0 || cols <= 0) { - return 0; - } - - // 阶段 1：估计扫描线在 X 方向上的典型行间距，并把 thresholdX - // 换算成“跨行生长时最多允许跳过多少行”的整数上限。 - // 这样后续在存在缺行、空洞时仍可连接同一目标，但不会无约束地跨越过远距离。 - const float rowStepX = EstimateTypicalRowStepX(points, rows, cols); - const int maxLineSkipNum = DeriveMaxLineSkipNum(params, rowStepX); - - // 后续预处理需要原地清零噪声点，因此先拷贝一份工作副本。 - // 原始输入保持不变，副本同时作为后续特征提取和 cluster 统计的输入。 - std::vector workingPoints(points, points + rows * cols); - - // 先对每一行提取 line feature，并缓存到表中。 - // 这样可以把“行内分段”和“跨行关联”拆成两个阶段，降低跨行匹配时的噪声敏感性。 - std::vector > rowFeatureTable(static_cast(rows)); - - // tree 是区域生长阶段的中间表示。 - // 每棵 tree 收集一组跨越多行、被判定为属于同一根条材的 feature。 - std::vector trees; - - for (int row = 0; row < rows; ++row) { - SVzNLPointXYZ* rowPoints = workingPoints.data() + row * cols; - - // 阶段 2a：先在当前行内抑制孤立噪声和尖刺噪声， - // 避免它们把连续条带错误切断，或单独形成伪目标。 - RemoveRowOutliersInPlace(rowPoints, cols, params); - - // 阶段 2b：把过滤后的当前行切分成若干连续 feature。 - // 相邻点只有在 Y/Z 差异都不超过阈值时才归入同一 feature， - // 每个 feature 对应当前扫描行上的一个局部平滑条带片段。 - ExtractRowFeatures( - rowPoints, - row, - cols, - params, - rowFeatureTable[static_cast(row)] - ); - } - - for (int row = 0; row < rows; ++row) { - // 阶段 3：按行推进，把当前行的 feature 尝试接到已有 tree 上。 - // 匹配时综合考虑 feature 质心相似性、列方向断裂和最大跨行数； - // 若没有合适的 tree，则以该 feature 新建一棵 tree。 - // 长时间未被延续的 tree 会在内部被终结，过短的 tree 直接剔除。 - GrowRowFeatures( - row, - row == rows - 1, - rowFeatureTable[static_cast(row)], - trees, - params, - maxLineSkipNum - ); - } - - // 阶段 4：把存活下来的 tree 展平为最终 cluster。 - // 这里会回收所有点索引，计算 cluster 的质心和包围盒， - // 过滤点数过少的候选，并按点数从大到小输出结果。 - FlattenTreesToClusters(trees, workingPoints.data(), params, outClusters); - return static_cast(outClusters.size()); -} - -} // namespace bar_intersection +#include "RegionGrowing.h" + +#include +#include +#include +#include + +namespace bar_intersection { + +struct SGridPoint { + int row; + int col; + int linearIdx; +}; + +struct SLineSegment { + int row; + int startCol; + int endCol; + int treeId; + std::vector points; + float sumX; + float sumY; + float sumZ; + SVzNL3DPointF centroid; + int pointCount; +}; + +struct UnionFind { + std::vector parent; + std::vector rank; + + int MakeSet() { + int id = static_cast(parent.size()); + parent.push_back(id); + rank.push_back(0); + return id; + } + + int Find(int x) { + if (parent[x] != x) { + parent[x] = Find(parent[x]); + } + return parent[x]; + } + + int Union(int a, int b) { + int ra = Find(a); + int rb = Find(b); + if (ra == rb) { + return ra; + } + if (rank[ra] < rank[rb]) { + int tmp = ra; + ra = rb; + rb = tmp; + } + parent[rb] = ra; + if (rank[ra] == rank[rb]) { + rank[ra]++; + } + return ra; + } +}; + +struct STreeAccumulator { + std::vector pointIndices; + float sumX; + float sumY; + float sumZ; + float minX; + float minY; + float minZ; + float maxX; + float maxY; + float maxZ; + int pointCount; +}; + +static SLineSegment StartNewSegment(const SGridPoint& gp, const SVzNLPointXYZ& pt) { + SLineSegment seg; + seg.row = gp.row; + seg.startCol = gp.col; + seg.endCol = gp.col; + seg.treeId = -1; + seg.points.push_back(gp); + seg.sumX = pt.x; + seg.sumY = pt.y; + seg.sumZ = pt.z; + seg.centroid.x = pt.x; + seg.centroid.y = pt.y; + seg.centroid.z = pt.z; + seg.pointCount = 1; + return seg; +} + +static void AppendPoint(SLineSegment& seg, const SGridPoint& gp, const SVzNLPointXYZ& pt) { + seg.points.push_back(gp); + seg.endCol = gp.col; + seg.sumX += pt.x; + seg.sumY += pt.y; + seg.sumZ += pt.z; + seg.pointCount++; +} + +static void FinalizeSegment(SLineSegment& seg) { + if (seg.pointCount > 0) { + seg.centroid.x = seg.sumX / seg.pointCount; + seg.centroid.y = seg.sumY / seg.pointCount; + seg.centroid.z = seg.sumZ / seg.pointCount; + } +} + +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 = + 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; + } + + return result; +} + +static void ProcessCurrentRowSegmentPoint( + const SVzNLPointXYZ* point, + unsigned int pointCount, + int row, + const SGrowthParams& params, + std::vector& currRowSegments +) { + currRowSegments.clear(); + if (!point || pointCount == 0) return; + + const float eps = 1e-6f; + const int cols = static_cast(pointCount); + const int rowOffset = row * cols; + const float segmentDistanceThresholdSq = + params.thresholdX * params.thresholdX + + params.thresholdY * params.thresholdY + + params.thresholdZ * params.thresholdZ; + + bool hasOpenSegment = false; + SLineSegment openSeg; + + for (int col = 0; col < cols; ++col) { + const SVzNLPointXYZ& pt = point[col]; + + if (std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps) { + continue; + } + + SGridPoint gp; + gp.row = row; + gp.col = col; + gp.linearIdx = rowOffset + col; + + if (!hasOpenSegment) { + openSeg = StartNewSegment(gp, pt); + hasOpenSegment = true; + continue; + } + + const SGridPoint& lastGp = openSeg.points.back(); + const SVzNLPointXYZ& lastPt = point[lastGp.col]; + const float dx = pt.x - lastPt.x; + const float dy = pt.y - lastPt.y; + const float dz = pt.z - lastPt.z; + const float distanceSq = dx * dx + dy * dy + dz * dz; + + if (distanceSq < segmentDistanceThresholdSq) { + AppendPoint(openSeg, gp, pt); + continue; + } + + FinalizeSegment(openSeg); + currRowSegments.push_back(openSeg); + openSeg = StartNewSegment(gp, pt); + } + + if (hasOpenSegment) { + FinalizeSegment(openSeg); + currRowSegments.push_back(openSeg); + } +} + +static void MergeCurrentRowSegments( + const std::vector& prevRowSegments, + 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 }); + + for (size_t ci = 0; ci < currRowSegments.size(); ++ci) { + SLineSegment& currSeg = currRowSegments[ci]; + std::vector matchedRoots; + int bestRoot = -1; + float bestDistanceSq = std::numeric_limits::max(); + + 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; + if (!relation.hasMatch) { + continue; + } + + candidateStates[pi].hasMatch = true; + const int root = uf.Find(prevSeg.treeId); + bool found = false; + for (size_t m = 0; m < matchedRoots.size(); ++m) { + if (matchedRoots[m] == root) { + found = true; + break; + } + } + if (!found) { + matchedRoots.push_back(root); + } + + if (relation.bestDistanceSq < bestDistanceSq) { + bestDistanceSq = relation.bestDistanceSq; + bestRoot = root; + } + } + }; + + tryMatchSegments(prevRowSegments, prevStates); + tryMatchSegments(carriedRowSegments, carriedStates); + + if (matchedRoots.empty()) { + currSeg.treeId = uf.MakeSet(); + continue; + } + + currSeg.treeId = bestRoot; + for (size_t m = 0; m < matchedRoots.size(); ++m) { + currSeg.treeId = uf.Union(currSeg.treeId, matchedRoots[m]); + } + currSeg.treeId = uf.Find(currSeg.treeId); + } + + for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) { + const SLineSegment& prevSeg = prevRowSegments[pi]; + if (!prevStates[pi].hasOverlap && !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) { + nextCarriedRowSegments.push_back(carriedSeg); + } + } +} + +static void FlattenTreesToClusters( + const std::vector& allSegments, + UnionFind& uf, + const SVzNLPointXYZ* points, + const SGrowthParams& params, + std::vector& outClusters +) { + if (allSegments.empty()) { + return; + } + + std::vector rootToBucket(static_cast(uf.parent.size()), -1); + std::vector buckets; + + for (size_t s = 0; s < allSegments.size(); ++s) { + const SLineSegment& seg = allSegments[s]; + const int root = uf.Find(seg.treeId); + + if (rootToBucket[root] == -1) { + rootToBucket[root] = static_cast(buckets.size()); + STreeAccumulator acc; + acc.sumX = 0.0f; + acc.sumY = 0.0f; + acc.sumZ = 0.0f; + acc.minX = std::numeric_limits::max(); + acc.minY = std::numeric_limits::max(); + acc.minZ = std::numeric_limits::max(); + acc.maxX = -std::numeric_limits::max(); + acc.maxY = -std::numeric_limits::max(); + acc.maxZ = -std::numeric_limits::max(); + acc.pointCount = 0; + buckets.push_back(acc); + } + + STreeAccumulator& acc = buckets[rootToBucket[root]]; + for (size_t p = 0; p < seg.points.size(); ++p) { + const SGridPoint& gp = seg.points[p]; + const SVzNLPointXYZ& pt = points[gp.linearIdx]; + + acc.pointIndices.push_back(gp.linearIdx); + acc.sumX += pt.x; + acc.sumY += pt.y; + acc.sumZ += pt.z; + if (pt.x < acc.minX) acc.minX = pt.x; + if (pt.y < acc.minY) acc.minY = pt.y; + if (pt.z < acc.minZ) acc.minZ = pt.z; + if (pt.x > acc.maxX) acc.maxX = pt.x; + if (pt.y > acc.maxY) acc.maxY = pt.y; + if (pt.z > acc.maxZ) acc.maxZ = pt.z; + acc.pointCount++; + } + } + + outClusters.clear(); + for (size_t b = 0; b < buckets.size(); ++b) { + const STreeAccumulator& acc = buckets[b]; + if (acc.pointCount < params.minClusterSize) { + continue; + } + + SGrowthCluster cluster; + cluster.pointIndices = acc.pointIndices; + cluster.pointCount = acc.pointCount; + cluster.centroid.x = acc.sumX / acc.pointCount; + cluster.centroid.y = acc.sumY / acc.pointCount; + cluster.centroid.z = acc.sumZ / acc.pointCount; + cluster.minBound.x = acc.minX; + cluster.minBound.y = acc.minY; + cluster.minBound.z = acc.minZ; + cluster.maxBound.x = acc.maxX; + cluster.maxBound.y = acc.maxY; + cluster.maxBound.z = acc.maxZ; + outClusters.push_back(cluster); + } +} + +int RegionGrowClusters( + const SVzNLPointXYZ* points, + int rows, + int cols, + const SGrowthParams& params, + std::vector& outClusters +) { + outClusters.clear(); + + if (!points || cols <= 0 || rows <= 0) { + return 0; + } + + UnionFind uf; + std::vector allSegments; + std::vector prevRowSegments; + std::vector carriedRowSegments; + std::vector nextCarriedRowSegments; + std::vector currRowSegments; + + for (int row = 0; row < rows; ++row) { + const SVzNLPointXYZ* rowPoints = points + row * cols; + + ProcessCurrentRowSegmentPoint( + rowPoints, + static_cast(cols), + row, + params, + currRowSegments + ); + + 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(); + } + + FlattenTreesToClusters(allSegments, uf, points, params, outClusters); + return static_cast(outClusters.size()); +} + +} // namespace bar_intersection diff --git a/Algo/DetectBarIntersection/src/RegionGrowing.h b/Algo/DetectBarIntersection/src/RegionGrowing.h index e85ff75..72caf80 100644 --- a/Algo/DetectBarIntersection/src/RegionGrowing.h +++ b/Algo/DetectBarIntersection/src/RegionGrowing.h @@ -1,40 +1,37 @@ -#ifndef REGION_GROWING_H -#define REGION_GROWING_H - -#include "VZNL_Types.h" -#include "BarIntersectionParams.h" -#include - -namespace bar_intersection { - -/** - * @brief Region growing based on scan-line feature trees. - * - * Stage 1: extract contiguous valid point runs from each row as line features. - * Stage 2: grow features onto alive trees using sg_lineGapsGrowing-style - * tail matching on centroid x/y/z differences. Unmatched features - * start new trees. - * Stage 3: finalize stale trees and flatten valid trees into SGrowthCluster. - * - * The input point cloud must be a regular grid. Invalid points are (0,0,0). - * - * @param points Filtered aligned point grid with rows * cols elements. - * @param rows Grid row count. - * @param cols Grid column count. - * @param params thresholdY/Z are used for row feature extraction and matching; - * thresholdX constrains cross-row displacement. - * @param outClusters Output clusters. pointIndices are linear indices in the - * original rows * cols grid. - * @return Number of valid clusters with pointCount >= minClusterSize. - */ -int RegionGrowClusters( - const SVzNLPointXYZ* points, - int rows, - int cols, - const SGrowthParams& params, - std::vector& outClusters -); - -} // namespace bar_intersection - -#endif // REGION_GROWING_H +#ifndef REGION_GROWING_H +#define REGION_GROWING_H + +#include "VZNL_Types.h" +#include "BarIntersectionParams.h" +#include + +namespace bar_intersection { + +/** + * @brief 基于两阶段线扫描的点云区域生长 + * + * 阶段1：逐行扫描，按 col 顺序比较相邻点的 y/z 差值，将连续满足阈值的点归为 Segment + * 阶段2：跨行合并，当前行 Segment 与前一行 Segment 比较 x/z 距离和 col 区间重叠， + * 使用并查集合并属于同一棵树的 Segment + * 阶段3：按并查集根节点展平为 SGrowthCluster + * + * 输入点云已原地过滤：无效点为 (0,0,0)，自动跳过。 + * + * @param points 对齐并过滤后的点云数组 (rows * cols)，无效点为 (0,0,0) + * @param rows 网格行数 + * @param cols 网格列数 + * @param params 生长参数（thresholdY/Z=行内分段, thresholdX/Z=跨行合并） + * @param outClusters 输出簇列表，pointIndices 为原始网格线性索引 + * @return 有效簇数量 (>= minClusterSize) + */ +int RegionGrowClusters( + const SVzNLPointXYZ* points, + int rows, + int cols, + const SGrowthParams& params, + std::vector& outClusters +); + +} // namespace bar_intersection + +#endif // REGION_GROWING_H