优化平面分割
This commit is contained in:
parent
d2f86fe60d
commit
3887aba425
@ -1034,7 +1034,224 @@ int SegmentPlanesByRansac(
|
||||
<< ", after growth=" << plane.pointCount << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "[RANSAC] Found " << outPlanes.size() << " planes" << std::endl;
|
||||
std::cout << "[RANSAC] Found " << outPlanes.size() << " planes (before merge)" << std::endl;
|
||||
|
||||
// ===== 后处理：将小平面合并到兼容的大平面中 =====
|
||||
// 解决孔洞穿透问题：孔洞对面的点因BFS不可达而被分为单独小平面，
|
||||
// 但实际上它们属于同一物理表面。
|
||||
if (outPlanes.size() > 1) {
|
||||
// 按点数降序排序，最大平面在前
|
||||
std::sort(outPlanes.begin(), outPlanes.end(),
|
||||
[](const PlaneSegment& a, const PlaneSegment& b) {
|
||||
return a.pointCount > b.pointCount;
|
||||
});
|
||||
|
||||
// 为每个平面计算Z值统计（中位数和范围）
|
||||
struct PlaneZStats {
|
||||
float medianZ;
|
||||
float minZ;
|
||||
float maxZ;
|
||||
};
|
||||
std::vector<PlaneZStats> planeStats(outPlanes.size());
|
||||
|
||||
for (size_t i = 0; i < outPlanes.size(); i++) {
|
||||
std::vector<float> zValues;
|
||||
zValues.reserve(outPlanes[i].pointIndices.size());
|
||||
for (int idx : outPlanes[i].pointIndices) {
|
||||
zValues.push_back(points[idx].z);
|
||||
}
|
||||
std::sort(zValues.begin(), zValues.end());
|
||||
|
||||
size_t n = zValues.size();
|
||||
planeStats[i].medianZ = (n % 2 == 0)
|
||||
? (zValues[n/2 - 1] + zValues[n/2]) / 2.0f
|
||||
: zValues[n/2];
|
||||
planeStats[i].minZ = zValues.front();
|
||||
planeStats[i].maxZ = zValues.back();
|
||||
}
|
||||
|
||||
// 尝试将小平面合并到大平面
|
||||
constexpr float kNormalDotThreshold = 0.95f; // cos(~18°), 法向量相似性
|
||||
constexpr float kMergeZRangeMultiplier = 2.0f; // Z差容忍倍数
|
||||
|
||||
std::vector<bool> merged(outPlanes.size(), false);
|
||||
|
||||
for (size_t i = 1; i < outPlanes.size(); i++) {
|
||||
if (merged[i]) continue;
|
||||
|
||||
// 寻找最佳合并目标
|
||||
int bestTarget = -1;
|
||||
float bestDot = 0.0f;
|
||||
|
||||
for (size_t j = 0; j < i; j++) {
|
||||
if (merged[j]) continue;
|
||||
|
||||
// 检查法向量相似性（绝对值，因为法向可能反向）
|
||||
float dot = std::abs(
|
||||
outPlanes[i].a * outPlanes[j].a +
|
||||
outPlanes[i].b * outPlanes[j].b +
|
||||
outPlanes[i].c * outPlanes[j].c
|
||||
);
|
||||
|
||||
if (dot < kNormalDotThreshold) continue;
|
||||
|
||||
// 检查Z范围兼容性：小平面的中位Z应在大平面Z范围附近
|
||||
float largeRange = planeStats[j].maxZ - planeStats[j].minZ;
|
||||
float tolerance = std::max(largeRange * 0.5f,
|
||||
params.growthZThreshold * kMergeZRangeMultiplier);
|
||||
float zDist = std::abs(planeStats[i].medianZ - planeStats[j].medianZ);
|
||||
|
||||
if (zDist > tolerance) continue;
|
||||
|
||||
if (dot > bestDot) {
|
||||
bestDot = dot;
|
||||
bestTarget = static_cast<int>(j);
|
||||
}
|
||||
}
|
||||
|
||||
if (bestTarget >= 0) {
|
||||
std::cout << " Merging small plane (" << outPlanes[i].pointCount
|
||||
<< " pts, medZ=" << planeStats[i].medianZ
|
||||
<< ") into plane (" << outPlanes[bestTarget].pointCount
|
||||
<< " pts, medZ=" << planeStats[bestTarget].medianZ
|
||||
<< "), normalDot=" << bestDot << std::endl;
|
||||
|
||||
// 合并点索引
|
||||
outPlanes[bestTarget].pointIndices.insert(
|
||||
outPlanes[bestTarget].pointIndices.end(),
|
||||
outPlanes[i].pointIndices.begin(),
|
||||
outPlanes[i].pointIndices.end()
|
||||
);
|
||||
outPlanes[bestTarget].pointCount += outPlanes[i].pointCount;
|
||||
|
||||
// 更新合并后的Z统计
|
||||
float newMinZ = std::min(planeStats[bestTarget].minZ, planeStats[i].minZ);
|
||||
float newMaxZ = std::max(planeStats[bestTarget].maxZ, planeStats[i].maxZ);
|
||||
planeStats[bestTarget].minZ = newMinZ;
|
||||
planeStats[bestTarget].maxZ = newMaxZ;
|
||||
|
||||
merged[i] = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 移除已合并的平面
|
||||
std::vector<PlaneSegment> finalPlanes;
|
||||
for (size_t i = 0; i < outPlanes.size(); i++) {
|
||||
if (!merged[i]) {
|
||||
finalPlanes.push_back(std::move(outPlanes[i]));
|
||||
}
|
||||
}
|
||||
outPlanes = std::move(finalPlanes);
|
||||
|
||||
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<bool> mask(pointCount, false);
|
||||
for (int idx : plane.pointIndices) {
|
||||
mask[idx] = true;
|
||||
}
|
||||
|
||||
// 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]);
|
||||
for (int idx : plane.pointIndices) {
|
||||
if (label[idx] == maxCompId) {
|
||||
filteredIndices.push_back(idx);
|
||||
}
|
||||
}
|
||||
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 (outPlanes.size() > 1) {
|
||||
// 找最大平面的点数
|
||||
int maxPoints = 0;
|
||||
for (const auto& p : outPlanes) {
|
||||
if (p.pointCount > maxPoints) maxPoints = p.pointCount;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "[RANSAC] Final: " << outPlanes.size() << " planes" << std::endl;
|
||||
|
||||
*errCode = HD_SUCCESS;
|
||||
return static_cast<int>(outPlanes.size());
|
||||
|
||||
@ -293,6 +293,7 @@ struct RansacPlaneSegmentationParams {
|
||||
int minPlanePoints; // 最小平面点数, 建议 50-200
|
||||
int maxPlanes; // 最大提取平面数量, 建议 3-10
|
||||
float growthZThreshold; // 区域生长时相邻点Z差阈值 (mm), 建议 0.5-2.0
|
||||
float minPlaneRatio; // 平面最小点数占比 (相对最大平面), 建议 0.05-0.2
|
||||
|
||||
RansacPlaneSegmentationParams()
|
||||
: distanceThreshold(1.0f)
|
||||
@ -300,6 +301,7 @@ struct RansacPlaneSegmentationParams {
|
||||
, minPlanePoints(100)
|
||||
, maxPlanes(5)
|
||||
, growthZThreshold(1.0f)
|
||||
, minPlaneRatio(0.1f)
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user