cool609/WoodAlgo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改平面分割

Browse Source
This commit is contained in:
MaJunwei 2026-03-30 20:57:53 +08:00
parent cb4b039b00
commit 47b840d411
2 changed files with 69 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Algo/DetectHole/src/HoleDetectionBoundary.cpp
View File

@ -684,21 +684,17 @@ int DetectPitBoundaries(
for (int row = 0; row < rows; row++) {
const auto& rowPairs = allRowSegmentPairs[row];
for (size_t i = 0; i < rowPairs.size(); i++) {
if (shouldKeepRow[row][i]) {
int pairId = getRowPairId(row, static_cast<int>(i));
int root = find(pairId);
rootToPairIds[root].push_back(pairId);
}
int pairId = getRowPairId(row, static_cast<int>(i));
int root = find(pairId);
rootToPairIds[root].push_back(pairId);
}
}
for (int col = 0; col < cols; col++) {
const auto& colPairs = allColSegmentPairs[col];
for (size_t i = 0; i < colPairs.size(); i++) {
if (shouldKeepCol[col][i]) {
int pairId = getColPairId(col, static_cast<int>(i));
int root = find(pairId);
rootToPairIds[root].push_back(pairId);
}
int pairId = getColPairId(col, static_cast<int>(i));
int root = find(pairId);
rootToPairIds[root].push_back(pairId);
}
}

157
Algo/DetectHole/src/PlaneSegmentation.cpp
View File

@ -1146,109 +1146,78 @@ int SegmentPlanesByRansac(
std::cout << "[RANSAC] After merge: " << outPlanes.size() << " planes" << std::endl;
}
// ===== 后处理：对每个平面做栅格连通域标注，只保留最大连通域 =====
for (size_t pi = 0; pi < outPlanes.size(); pi++) {
PlaneSegment& plane = outPlanes[pi];
// ===== 后处理：对每个平面做连通域标注，将每个连通域拆分为独立平面 =====
{
std::vector<PlaneSegment> splitPlanes;
// 建立该平面的栅格掩码
std::vector<bool> mask(pointCount, false);
for (int idx : plane.pointIndices) {
mask[idx] = true;
}
for (size_t pi = 0; pi < outPlanes.size(); pi++) {
const PlaneSegment& plane = outPlanes[pi];
// BFS连通域标注 (4-连通)
std::vector<int> label(pointCount, -1);
std::vector<int> componentSizes;
int numComponents = 0;
for (int idx : plane.pointIndices) {
if (label[idx] >= 0) continue;
int compId = numComponents++;
int compSize = 0;
std::queue<int> q;
q.push(idx);
label[idx] = compId;
while (!q.empty()) {
int cur = q.front();
q.pop();
compSize++;
int cr = cur / cols;
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);
}
}
componentSizes.push_back(compSize);
}
if (numComponents <= 1) continue;
// 找到最大连通域
int maxCompId = 0;
for (int c = 1; c < numComponents; c++) {
if (componentSizes[c] > componentSizes[maxCompId]) {
maxCompId = c;
}
}
// 只保留最大连通域的点
int removedCount = plane.pointCount - componentSizes[maxCompId];
if (removedCount > 0) {
std::vector<int> filteredIndices;
filteredIndices.reserve(componentSizes[maxCompId]);
// 建立该平面的栅格掩码
std::vector<bool> mask(pointCount, false);
for (int idx : plane.pointIndices) {
if (label[idx] == maxCompId) {
filteredIndices.push_back(idx);
mask[idx] = true;
}
// BFS 连通域标注 (4-连通)
std::vector<int> label(pointCount, -1);
std::vector<std::vector<int>> components;
for (int idx : plane.pointIndices) {
if (label[idx] >= 0) continue;
int compId = static_cast<int>(components.size());
components.emplace_back();
std::queue<int> q;
q.push(idx);
label[idx] = compId;
while (!q.empty()) {
int cur = q.front();
q.pop();
components[compId].push_back(cur);
int cr = cur / cols;
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);
}
}
}
plane.pointIndices = std::move(filteredIndices);
plane.pointCount = static_cast<int>(plane.pointIndices.size());
std::cout << " Plane " << (pi + 1)
<< ": CCL removed " << removedCount
<< " pts from " << (numComponents - 1)
<< " small components, kept " << plane.pointCount << std::endl;
}
}
if (components.size() == 1) {
splitPlanes.push_back(plane);
continue;
}
// ===== 按相对大小过滤：去除远小于最大平面的平面 =====
if (outPlanes.size() > 1) {
// 找最大平面的点数
int maxPoints = 0;
for (const auto& p : outPlanes) {
if (p.pointCount > maxPoints) maxPoints = p.pointCount;
std::cout << " Plane " << (pi + 1) << ": split into "
<< components.size() << " connected components" << std::endl;
for (size_t ci = 0; ci < components.size(); ci++) {
PlaneSegment seg;
seg.a = plane.a;
seg.b = plane.b;
seg.c = plane.c;
seg.d = plane.d;
seg.normalAngleDeg = plane.normalAngleDeg;
seg.pointIndices = std::move(components[ci]);
seg.pointCount = static_cast<int>(seg.pointIndices.size());
std::cout << " Component " << (ci + 1)
<< ": " << seg.pointCount << " points" << std::endl;
splitPlanes.push_back(std::move(seg));
}
}
int sizeThreshold = static_cast<int>(maxPoints * params.minPlaneRatio);
sizeThreshold = std::max(sizeThreshold, params.minPlanePoints);
size_t beforeCount = outPlanes.size();
outPlanes.erase(
std::remove_if(outPlanes.begin(), outPlanes.end(),
[sizeThreshold](const PlaneSegment& p) {
return p.pointCount < sizeThreshold;
}),
outPlanes.end()
);
if (outPlanes.size() < beforeCount) {
std::cout << "[RANSAC] Size filter (threshold=" << sizeThreshold
<< ", ratio=" << params.minPlaneRatio
<< " of max=" << maxPoints
<< "): removed " << (beforeCount - outPlanes.size())
<< " small planes" << std::endl;
}
outPlanes = std::move(splitPlanes);
}
std::cout << "[RANSAC] Final: " << outPlanes.size() << " planes" << std::endl;