From 25101160c2c386f8cf0483e667dc2ecdcfef2e6e Mon Sep 17 00:00:00 2001 From: cool609 Date: Sun, 5 Apr 2026 00:31:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E7=A9=BA=E8=A1=8C=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../visualization_demo/VisualizationDemo.cpp | 2 +- Algo/DetectHole/src/PlaneSegmentation.cpp | 72 +++++++++++-------- 2 files changed, 45 insertions(+), 29 deletions(-) diff --git a/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp b/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp index a5ee45a..4b16f15 100644 --- a/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp +++ b/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp @@ -597,7 +597,7 @@ int main(int argc, char* argv[]) { // Set up detection parameters (using defaults from constructor) SHoleDetectionParams detectionParams; - detectionParams.minRadius = 0.5f; + detectionParams.minRadius = 1.5f; SHoleFilterParams filterParams; std::cout << "=== Hole Detection Visualization Demo ===" << std::endl; diff --git a/Algo/DetectHole/src/PlaneSegmentation.cpp b/Algo/DetectHole/src/PlaneSegmentation.cpp index 9d2dc73..a4c0bc4 100644 --- a/Algo/DetectHole/src/PlaneSegmentation.cpp +++ b/Algo/DetectHole/src/PlaneSegmentation.cpp @@ -973,6 +973,8 @@ int SegmentPlanesByRansac( } } + constexpr int kMaxGrowthBridgeGap = 3; // BFS生长时允许跨越的最大空行/空列数 + while (!bfsQueue.empty()) { int curIdx = bfsQueue.front(); bfsQueue.pop(); @@ -980,32 +982,36 @@ int SegmentPlanesByRansac( int curRow = curIdx / cols; int curCol = curIdx % cols; - // 遍历4-连通邻居 + // 遍历4-连通邻居，允许跨越少量无效点（空行/空列） for (int d = 0; d < 4; d++) { - int nr = curRow + dr[d]; - int nc = curCol + dc[d]; + for (int step = 1; step <= kMaxGrowthBridgeGap + 1; step++) { + int nr = curRow + dr[d] * step; + int nc = curCol + dc[d] * step; - if (nr < 0 || nr >= rows || nc < 0 || nc >= cols) continue; + if (nr < 0 || nr >= rows || nc < 0 || nc >= cols) break; - int nIdx = nr * cols + nc; - if (used[nIdx] || inPlane[nIdx]) continue; - if (!IsValidPoint(points[nIdx])) continue; + int nIdx = nr * cols + nc; + if (used[nIdx] || inPlane[nIdx]) break; + if (!IsValidPoint(points[nIdx])) continue; // 空点，继续向前探测 - // 局部Z差检查：防止跨越单步噪点 - float zDiff = std::abs(points[nIdx].z - points[curIdx].z); - if (zDiff > params.growthZThreshold) continue; + // 找到有效点，执行原有的生长条件检查 + // 局部Z差检查：防止跨越单步噪点 + float zDiff = std::abs(points[nIdx].z - points[curIdx].z); + if (zDiff > params.growthZThreshold) break; - // 全局点到平面距离检查：防止BFS在渐变区域内漂移到不同表面。 - // 仅靠局部Z差无法阻止逐步穿越台阶（每步小但累积大）， - // 全局距离能确保生长点始终贴近当前RANSAC平面。 - float planeDist = std::abs( - bestA * points[nIdx].x + bestB * points[nIdx].y + - bestC * points[nIdx].z + bestD - ); - if (planeDist > params.distanceThreshold * 2.0f) continue; + // 全局点到平面距离检查：防止BFS在渐变区域内漂移到不同表面。 + // 仅靠局部Z差无法阻止逐步穿越台阶（每步小但累积大）， + // 全局距离能确保生长点始终贴近当前RANSAC平面。 + float planeDist = std::abs( + bestA * points[nIdx].x + bestB * points[nIdx].y + + bestC * points[nIdx].z + bestD + ); + if (planeDist > params.distanceThreshold * 2.0f) break; - inPlane[nIdx] = true; - bfsQueue.push(nIdx); + inPlane[nIdx] = true; + bfsQueue.push(nIdx); + break; // 成功连接，不再继续探测 + } } } @@ -1156,8 +1162,10 @@ int SegmentPlanesByRansac( } // ===== 后处理：对每个平面做连通域标注，将每个连通域拆分为独立平面 ===== + // 允许跨越少量空行/空列（无效数据行）进行连通，避免因单行数据缺失导致同一平面被拆分 { std::vector splitPlanes; + constexpr int kMaxBridgeGap = 3; // 允许跨越的最大空行/空列数 for (size_t pi = 0; pi < outPlanes.size(); pi++) { const PlaneSegment& plane = outPlanes[pi]; @@ -1168,7 +1176,7 @@ int SegmentPlanesByRansac( mask[idx] = true; } - // BFS 连通域标注 (4-连通) + // BFS 连通域标注 (4-连通，允许跨越空行/空列) std::vector label(pointCount, -1); std::vector> components; @@ -1190,13 +1198,21 @@ int SegmentPlanesByRansac( int cc = cur % cols; for (int d = 0; d < 4; d++) { - int nr = cr + dr[d]; - int nc = cc + dc[d]; - if (nr < 0 || nr >= rows || nc < 0 || nc >= cols) continue; - int nIdx = nr * cols + nc; - if (!mask[nIdx] || label[nIdx] >= 0) continue; - label[nIdx] = compId; - q.push(nIdx); + // 沿当前方向尝试跨越空行/空列 + for (int step = 1; step <= kMaxBridgeGap + 1; step++) { + int nr = cr + dr[d] * step; + int nc = cc + dc[d] * step; + if (nr < 0 || nr >= rows || nc < 0 || nc >= cols) break; + int nIdx = nr * cols + nc; + if (label[nIdx] >= 0) break; // 已标注，无需继续 + if (mask[nIdx]) { + // 找到属于同一平面的点，建立连接 + label[nIdx] = compId; + q.push(nIdx); + break; + } + // mask[nIdx] == false: 空点，继续向前探测 + } } } }