修改树的生长
This commit is contained in:
MaJunwei
parent
9e4244d86a
commit
6c55510809
@ -1,42 +1,18 @@
|
||||
#include "RegionGrowing.h"
|
||||
#include <cmath>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
namespace bar_intersection {
|
||||
|
||||
// =====================================================================
|
||||
// Internal data structures
|
||||
// =====================================================================
|
||||
|
||||
/**
|
||||
* @brief 网格点信息，记录一个有效点在原始网格中的位置
|
||||
* @param row 该点在原始网格中的行号 (linearIdx / cols)
|
||||
* @param col 该点在原始网格中的列号 (linearIdx % cols)
|
||||
* @param linearIdx 该点在 points 数组中的线性索引 (row * cols + col)
|
||||
*/
|
||||
struct SGridPoint {
|
||||
int row;
|
||||
int col;
|
||||
int linearIdx;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 行内线段 (Segment)
|
||||
*
|
||||
* 同一行 (row) 中，y/z 连续的一组点构成一个 segment。
|
||||
* 每个 segment 是区域生长的最小合并单元。
|
||||
*
|
||||
* @param row 所在行号
|
||||
* @param startCol segment 起始列号（最左侧点的 col）
|
||||
* @param endCol segment 结束列号（最右侧点的 col）
|
||||
* @param treeId 该 segment 在 UnionFind 中的集合 ID
|
||||
* @param points segment 包含的所有网格点
|
||||
* @param sumX/Y/Z 坐标累加器，用于计算质心
|
||||
* @param centroid segment 质心 (FinalizeSegment 后有效)
|
||||
* @param pointCount segment 内点数
|
||||
*/
|
||||
struct SLineSegment {
|
||||
int row;
|
||||
int startCol;
|
||||
@ -50,27 +26,17 @@ struct SLineSegment {
|
||||
int pointCount;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 并查集 (Union-Find / Disjoint Set Union)
|
||||
*
|
||||
* 用于跨行 segment 合并：当两个 segment 判定属于同一棵"树"时，
|
||||
* 通过 Union 操作将它们所在的集合合并。
|
||||
* - 路径压缩 (path compression)：Find 时将节点直接挂到根，加速后续查询
|
||||
* - 按秩合并 (union by rank)：矮树挂到高树下，保持树高度平衡
|
||||
*/
|
||||
struct UnionFind {
|
||||
std::vector<int> parent;
|
||||
std::vector<unsigned char> rank;
|
||||
|
||||
/** @brief 创建新集合，返回集合 ID */
|
||||
int MakeSet() {
|
||||
int id = (int)parent.size();
|
||||
int id = static_cast<int>(parent.size());
|
||||
parent.push_back(id);
|
||||
rank.push_back(0);
|
||||
return id;
|
||||
}
|
||||
|
||||
/** @brief 查找 x 所属集合的根节点（带路径压缩） */
|
||||
int Find(int x) {
|
||||
if (parent[x] != x) {
|
||||
parent[x] = Find(parent[x]);
|
||||
@ -78,13 +44,16 @@ struct UnionFind {
|
||||
return parent[x];
|
||||
}
|
||||
|
||||
/** @brief 合并 a、b 所在集合（按秩合并），返回合并后的根 */
|
||||
int Union(int a, int b) {
|
||||
int ra = Find(a);
|
||||
int rb = Find(b);
|
||||
if (ra == rb) return ra;
|
||||
if (ra == rb) {
|
||||
return ra;
|
||||
}
|
||||
if (rank[ra] < rank[rb]) {
|
||||
int tmp = ra; ra = rb; rb = tmp;
|
||||
int tmp = ra;
|
||||
ra = rb;
|
||||
rb = tmp;
|
||||
}
|
||||
parent[rb] = ra;
|
||||
if (rank[ra] == rank[rb]) {
|
||||
@ -94,36 +63,26 @@ struct UnionFind {
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 树累加器，用于 Phase 3 汇总同一棵树的所有点
|
||||
*
|
||||
* @param pointIndices 该树所有点在原始网格中的线性索引
|
||||
* @param sumX/Y/Z 坐标累加，用于计算质心
|
||||
* @param min/maxX/Y/Z 包围盒
|
||||
* @param pointCount 总点数
|
||||
*/
|
||||
struct STreeAccumulator {
|
||||
std::vector<int> pointIndices;
|
||||
float sumX, sumY, sumZ;
|
||||
float minX, minY, minZ;
|
||||
float maxX, maxY, maxZ;
|
||||
float sumX;
|
||||
float sumY;
|
||||
float sumZ;
|
||||
float minX;
|
||||
float minY;
|
||||
float minZ;
|
||||
float maxX;
|
||||
float maxY;
|
||||
float maxZ;
|
||||
int pointCount;
|
||||
};
|
||||
|
||||
// =====================================================================
|
||||
// Helper functions
|
||||
// =====================================================================
|
||||
|
||||
/**
|
||||
* @brief 创建一个新的行内 segment，以 gp/pt 作为第一个点
|
||||
*/
|
||||
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.clear();
|
||||
seg.points.push_back(gp);
|
||||
seg.sumX = pt.x;
|
||||
seg.sumY = pt.y;
|
||||
@ -135,9 +94,6 @@ static SLineSegment StartNewSegment(const SGridPoint& gp, const SVzNLPointXYZ& p
|
||||
return seg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向当前 segment 追加一个点，更新坐标累加器和列范围
|
||||
*/
|
||||
static void AppendPoint(SLineSegment& seg, const SGridPoint& gp, const SVzNLPointXYZ& pt) {
|
||||
seg.points.push_back(gp);
|
||||
seg.endCol = gp.col;
|
||||
@ -147,9 +103,6 @@ static void AppendPoint(SLineSegment& seg, const SGridPoint& gp, const SVzNLPoin
|
||||
seg.pointCount++;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 结算 segment 的质心 = sum / count
|
||||
*/
|
||||
static void FinalizeSegment(SLineSegment& seg) {
|
||||
if (seg.pointCount > 0) {
|
||||
seg.centroid.x = seg.sumX / seg.pointCount;
|
||||
@ -158,81 +111,116 @@ static void FinalizeSegment(SLineSegment& seg) {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 判断两个 segment 的列范围是否有重叠或紧邻 (1列容差)
|
||||
*
|
||||
* 只有列范围有交集的 segment 才有可能属于同一棵树。
|
||||
* 容差 1 列：允许网格中有少量无效点造成的间隙。
|
||||
*/
|
||||
static bool HasColumnOverlapOrTouch(const SLineSegment& a, const SLineSegment& b) {
|
||||
return !(b.endCol < a.startCol - 1 || a.endCol < b.startCol - 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Phase 2: 跨行合并 —— 将当前行的 segments 与前一行的 segments 进行树归属匹配
|
||||
*
|
||||
* 匹配条件（三个条件必须同时满足）：
|
||||
* 1. 列范围重叠或紧邻 (HasColumnOverlapOrTouch)
|
||||
* 2. 质心 x 差 < thresholdX（同一根钢筋在相邻行的 x 偏移较小）
|
||||
* 3. 质心 z 差 < thresholdZ（同一根钢筋在相邻行的 z 偏移较小）
|
||||
*
|
||||
* 合并策略：
|
||||
* - 如果当前 segment 匹配到多个前一行 segment（属于不同的树），
|
||||
* 说明这些树在当前行发生了交汇，通过 Union 合并为同一棵树。
|
||||
* - 如果没有匹配到任何前一行 segment，创建新树 (MakeSet)。
|
||||
* - 只在相邻行 (rowGap == 1) 时才尝试匹配，非相邻行直接创建新树。
|
||||
*/
|
||||
static void ProcessCurrentRowSegmentPoint(
|
||||
const SVzNLPointXYZ* point,
|
||||
unsigned int pointCount,
|
||||
int row,
|
||||
const SGrowthParams& params,
|
||||
std::vector<SLineSegment>& currRowSegments
|
||||
) {
|
||||
currRowSegments.clear();
|
||||
if (!point || pointCount == 0) return;
|
||||
|
||||
const float eps = 1e-6f;
|
||||
const int cols = static_cast<int>(pointCount);
|
||||
const int rowOffset = row * cols;
|
||||
|
||||
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];
|
||||
float dy = std::abs(pt.y - lastPt.y);
|
||||
float dz = std::abs(pt.z - lastPt.z);
|
||||
|
||||
if (dy < params.thresholdY && dz < params.thresholdZ) {
|
||||
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<SLineSegment>& prevRowSegments,
|
||||
std::vector<SLineSegment>& currRowSegments,
|
||||
UnionFind& uf,
|
||||
const SGrowthParams& params
|
||||
) {
|
||||
if (currRowSegments.empty()) return;
|
||||
if (currRowSegments.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 判断当前行与前一行是否相邻 (rowGap == 1)
|
||||
// 只有相邻行才尝试跨行匹配，非相邻行的 segment 一律创建新树
|
||||
bool rowsAdjacent = false;
|
||||
if (!prevRowSegments.empty()) {
|
||||
rowsAdjacent = (currRowSegments[0].row == prevRowSegments[0].row + 1);
|
||||
}
|
||||
|
||||
// 遍历当前行的每个 segment，尝试与前一行的 segments 匹配
|
||||
for (size_t ci = 0; ci < currRowSegments.size(); ci++) {
|
||||
for (size_t ci = 0; ci < currRowSegments.size(); ++ci) {
|
||||
SLineSegment& currSeg = currRowSegments[ci];
|
||||
std::vector<int> matchedRoots; // 匹配到的不同树的根节点列表
|
||||
int bestRoot = -1; // x 距离最近的匹配根
|
||||
std::vector<int> matchedRoots;
|
||||
int bestRoot = -1;
|
||||
float bestDx = std::numeric_limits<float>::max();
|
||||
|
||||
if (rowsAdjacent) {
|
||||
// 与前一行的每个 segment 逐一比较
|
||||
for (size_t pi = 0; pi < prevRowSegments.size(); pi++) {
|
||||
for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) {
|
||||
const SLineSegment& prevSeg = prevRowSegments[pi];
|
||||
if (!HasColumnOverlapOrTouch(prevSeg, currSeg)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 条件 1：列范围必须有重叠或紧邻
|
||||
if (!HasColumnOverlapOrTouch(prevSeg, currSeg)) continue;
|
||||
const float dx = std::abs(currSeg.centroid.x - prevSeg.centroid.x);
|
||||
if (dx >= params.thresholdX) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 条件 2：质心 x 差必须小于阈值（同一钢筋跨行 x 偏移小）
|
||||
float dx = std::abs(currSeg.centroid.x - prevSeg.centroid.x);
|
||||
if (dx >= params.thresholdX) continue;
|
||||
const float dz = std::abs(currSeg.centroid.z - prevSeg.centroid.z);
|
||||
if (dz >= params.thresholdZ) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 条件 3：质心 z 差必须小于阈值（z 偏离过大则不属于同一树）
|
||||
float dz = std::abs(currSeg.centroid.z - prevSeg.centroid.z);
|
||||
if (dz >= params.thresholdZ) continue;
|
||||
|
||||
// 三个条件都满足 → 匹配成功，记录该 segment 所属树的根
|
||||
int root = uf.Find(prevSeg.treeId);
|
||||
|
||||
// 去重：同一棵树可能有多个 segment 在前一行，只记录一次
|
||||
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; }
|
||||
for (size_t m = 0; m < matchedRoots.size(); ++m) {
|
||||
if (matchedRoots[m] == root) {
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
matchedRoots.push_back(root);
|
||||
}
|
||||
|
||||
// 追踪 x 距离最近的根，作为首选归属
|
||||
if (dx < bestDx) {
|
||||
bestDx = dx;
|
||||
bestRoot = root;
|
||||
@ -241,34 +229,18 @@ static void MergeCurrentRowSegments(
|
||||
}
|
||||
|
||||
if (matchedRoots.empty()) {
|
||||
// 没有匹配到任何前一行 segment → 创建新树
|
||||
currSeg.treeId = uf.MakeSet();
|
||||
} else {
|
||||
// 匹配到至少一棵树 → 归入 x 最近的树
|
||||
continue;
|
||||
}
|
||||
|
||||
currSeg.treeId = bestRoot;
|
||||
// 如果匹配到多棵不同的树，说明这些树在当前行交汇，全部合并
|
||||
for (size_t m = 0; m < matchedRoots.size(); m++) {
|
||||
for (size_t m = 0; m < matchedRoots.size(); ++m) {
|
||||
currSeg.treeId = uf.Union(currSeg.treeId, matchedRoots[m]);
|
||||
}
|
||||
// 路径压缩，确保 treeId 指向最终根
|
||||
currSeg.treeId = uf.Find(currSeg.treeId);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Phase 3: 将并查集的树扁平化为最终的 SGrowthCluster 数组
|
||||
*
|
||||
* 遍历所有 segment，通过 UnionFind.Find() 得到最终根节点，
|
||||
* 将同根的 segment 的点汇总到同一个 STreeAccumulator 中，
|
||||
* 计算质心和包围盒，过滤掉点数不足 minClusterSize 的小簇。
|
||||
*
|
||||
* @param allSegments 所有行产生的 segment 列表
|
||||
* @param uf 并查集（已完成所有合并）
|
||||
* @param points 完整的点云数组
|
||||
* @param params 生长参数（使用 minClusterSize）
|
||||
* @param outClusters [out] 输出簇列表
|
||||
*/
|
||||
static void FlattenTreesToClusters(
|
||||
const std::vector<SLineSegment>& allSegments,
|
||||
UnionFind& uf,
|
||||
@ -276,37 +248,35 @@ static void FlattenTreesToClusters(
|
||||
const SGrowthParams& params,
|
||||
std::vector<SGrowthCluster>& outClusters
|
||||
) {
|
||||
if (allSegments.empty()) return;
|
||||
if (allSegments.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// rootToBucket: 并查集根 ID → buckets 数组下标的映射
|
||||
// 首次遇到某个根时分配一个新 bucket
|
||||
int ufSize = (int)uf.parent.size();
|
||||
std::vector<int> rootToBucket(ufSize, -1);
|
||||
std::vector<int> rootToBucket(static_cast<size_t>(uf.parent.size()), -1);
|
||||
std::vector<STreeAccumulator> buckets;
|
||||
|
||||
// 遍历所有 segment，按最终根节点归入对应 bucket
|
||||
for (size_t s = 0; s < allSegments.size(); s++) {
|
||||
for (size_t s = 0; s < allSegments.size(); ++s) {
|
||||
const SLineSegment& seg = allSegments[s];
|
||||
int root = uf.Find(seg.treeId); // 查找该 segment 所属树的最终根
|
||||
const int root = uf.Find(seg.treeId);
|
||||
|
||||
// 如果该根还没有分配 bucket，创建一个新的
|
||||
if (rootToBucket[root] == -1) {
|
||||
rootToBucket[root] = (int)buckets.size();
|
||||
rootToBucket[root] = static_cast<int>(buckets.size());
|
||||
STreeAccumulator acc;
|
||||
acc.sumX = acc.sumY = acc.sumZ = 0;
|
||||
acc.minX = acc.minY = acc.minZ = std::numeric_limits<float>::max();
|
||||
acc.maxX = acc.maxY = acc.maxZ = -std::numeric_limits<float>::max();
|
||||
acc.sumX = 0.0f;
|
||||
acc.sumY = 0.0f;
|
||||
acc.sumZ = 0.0f;
|
||||
acc.minX = std::numeric_limits<float>::max();
|
||||
acc.minY = std::numeric_limits<float>::max();
|
||||
acc.minZ = std::numeric_limits<float>::max();
|
||||
acc.maxX = -std::numeric_limits<float>::max();
|
||||
acc.maxY = -std::numeric_limits<float>::max();
|
||||
acc.maxZ = -std::numeric_limits<float>::max();
|
||||
acc.pointCount = 0;
|
||||
buckets.push_back(acc);
|
||||
}
|
||||
|
||||
STreeAccumulator& acc = buckets[rootToBucket[root]];
|
||||
|
||||
// 将 segment 内的每个点汇总到 bucket：
|
||||
// - 记录原始网格线性索引 (gp.linearIdx = row * cols + col)
|
||||
// - 累加坐标用于计算质心
|
||||
// - 更新包围盒 min/max
|
||||
for (size_t p = 0; p < seg.points.size(); p++) {
|
||||
for (size_t p = 0; p < seg.points.size(); ++p) {
|
||||
const SGridPoint& gp = seg.points[p];
|
||||
const SVzNLPointXYZ& pt = points[gp.linearIdx];
|
||||
|
||||
@ -324,11 +294,12 @@ static void FlattenTreesToClusters(
|
||||
}
|
||||
}
|
||||
|
||||
// 将 bucket 转为 SGrowthCluster，过滤掉点数不足的小簇
|
||||
outClusters.clear();
|
||||
for (size_t b = 0; b < buckets.size(); b++) {
|
||||
for (size_t b = 0; b < buckets.size(); ++b) {
|
||||
const STreeAccumulator& acc = buckets[b];
|
||||
if (acc.pointCount < params.minClusterSize) continue;
|
||||
if (acc.pointCount < params.minClusterSize) {
|
||||
continue;
|
||||
}
|
||||
|
||||
SGrowthCluster cluster;
|
||||
cluster.pointIndices = acc.pointIndices;
|
||||
@ -346,31 +317,6 @@ static void FlattenTreesToClusters(
|
||||
}
|
||||
}
|
||||
|
||||
// =====================================================================
|
||||
// Main entry point
|
||||
// =====================================================================
|
||||
|
||||
/**
|
||||
* @brief 两阶段线扫描区域生长算法
|
||||
*
|
||||
* 算法整体流程：
|
||||
* Phase 1 (行内分段): 逐行扫描点云，同一行内相邻点的 y/z 差值
|
||||
* 在阈值内则归入同一 segment，否则切分为新 segment。
|
||||
* Phase 2 (跨行合并): 每处理完一行，将当前行的 segments 与前一行的
|
||||
* segments 按列重叠 + x 阈值 + z 阈值匹配，匹配成功的通过并查集
|
||||
* 合并为同一棵树。
|
||||
* Phase 3 (汇总输出): 遍历所有 segment，按并查集根节点汇总为最终的
|
||||
* SGrowthCluster，过滤掉点数不足的小簇。
|
||||
*
|
||||
* 输入点云已原地过滤：无效点为 (0,0,0)，自动跳过。
|
||||
*
|
||||
* @param points 对齐并过滤后的点云数组 (rows * cols)，无效点为 (0,0,0)
|
||||
* @param rows 原始网格行数
|
||||
* @param cols 原始网格列数
|
||||
* @param params 生长参数（各轴阈值、最小簇点数）
|
||||
* @param outClusters [out] 输出簇列表
|
||||
* @return 检测到的簇数量
|
||||
*/
|
||||
int RegionGrowClusters(
|
||||
const SVzNLPointXYZ* points,
|
||||
int rows,
|
||||
@ -380,111 +326,35 @@ int RegionGrowClusters(
|
||||
) {
|
||||
outClusters.clear();
|
||||
|
||||
// 输入校验
|
||||
if (!points || cols <= 0 || rows <= 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int totalPoints = rows * cols;
|
||||
UnionFind uf;
|
||||
std::vector<SLineSegment> allSegments;
|
||||
std::vector<SLineSegment> prevRowSegments;
|
||||
std::vector<SLineSegment> currRowSegments;
|
||||
|
||||
// ---- 状态初始化 ----
|
||||
UnionFind uf; // 并查集，管理树的合并
|
||||
std::vector<SLineSegment> allSegments; // 所有行产生的 segment（Phase 3 汇总用）
|
||||
std::vector<SLineSegment> prevRowSegments; // 前一行的 segment 列表（Phase 2 匹配用）
|
||||
std::vector<SLineSegment> currRowSegments; // 当前行的 segment 列表
|
||||
for (int row = 0; row < rows; ++row) {
|
||||
const SVzNLPointXYZ* rowPoints = points + row * cols;
|
||||
|
||||
int currRow = -1; // 当前正在处理的行号，-1 表示尚未开始
|
||||
bool hasOpenSegment = false; // 是否有一个正在构建中的 segment
|
||||
SLineSegment openSeg; // 正在构建中的 segment
|
||||
ProcessCurrentRowSegmentPoint(
|
||||
rowPoints,
|
||||
static_cast<unsigned int>(cols),
|
||||
row,
|
||||
params,
|
||||
currRowSegments
|
||||
);
|
||||
|
||||
// ---- 主循环：按行主序遍历所有点，跳过 (0,0,0) 无效点 ----
|
||||
for (int idx = 0; idx < totalPoints; idx++) {
|
||||
const SVzNLPointXYZ& pt = points[idx];
|
||||
|
||||
// 跳过无效点 (0,0,0)
|
||||
const float eps = 1e-6f;
|
||||
if (std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 由线性索引反算网格坐标
|
||||
int row = idx / cols;
|
||||
int col = idx % cols;
|
||||
|
||||
// 构建网格点信息
|
||||
SGridPoint gp;
|
||||
gp.row = row;
|
||||
gp.col = col;
|
||||
gp.linearIdx = idx;
|
||||
|
||||
// ---------- 第一个有效点：初始化 ----------
|
||||
if (currRow == -1) {
|
||||
currRow = row;
|
||||
openSeg = StartNewSegment(gp, pt);
|
||||
hasOpenSegment = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
// ---------- 换行：触发 Phase 2 跨行合并 ----------
|
||||
if (row != currRow) {
|
||||
// 关闭当前 segment，计算质心
|
||||
if (hasOpenSegment) {
|
||||
FinalizeSegment(openSeg);
|
||||
currRowSegments.push_back(openSeg);
|
||||
hasOpenSegment = false;
|
||||
}
|
||||
|
||||
// Phase 2: 当前行 segments vs 前一行 segments → 并查集合并
|
||||
MergeCurrentRowSegments(prevRowSegments, currRowSegments, uf, params);
|
||||
|
||||
// 将当前行 segments 存入 allSegments（Phase 3 需要）
|
||||
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
|
||||
|
||||
// 行缓冲轮换：当前行变为"前一行"，清空当前行
|
||||
prevRowSegments.swap(currRowSegments);
|
||||
currRowSegments.clear();
|
||||
|
||||
// 新行的第一个点，开启新 segment
|
||||
currRow = row;
|
||||
openSeg = StartNewSegment(gp, pt);
|
||||
hasOpenSegment = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
// ---------- 同一行：Phase 1 行内分段 ----------
|
||||
// 取当前 segment 的最后一个点，比较 y/z 差值
|
||||
const SGridPoint& lastGp = openSeg.points.back();
|
||||
const SVzNLPointXYZ& lastPt = points[lastGp.linearIdx];
|
||||
float dy = std::abs(pt.y - lastPt.y);
|
||||
float dz = std::abs(pt.z - lastPt.z);
|
||||
|
||||
if (dy < params.thresholdY && dz < params.thresholdZ) {
|
||||
// y/z 在阈值内 → 归入当前 segment
|
||||
AppendPoint(openSeg, gp, pt);
|
||||
} else {
|
||||
// y/z 超出阈值 → 当前 segment 结束，开启新 segment
|
||||
FinalizeSegment(openSeg);
|
||||
currRowSegments.push_back(openSeg);
|
||||
openSeg = StartNewSegment(gp, pt);
|
||||
hasOpenSegment = true;
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 收尾：处理最后一行 ----
|
||||
// 关闭最后一个 segment
|
||||
if (hasOpenSegment) {
|
||||
FinalizeSegment(openSeg);
|
||||
currRowSegments.push_back(openSeg);
|
||||
}
|
||||
|
||||
// 最后一行也需要与前一行做 Phase 2 合并
|
||||
MergeCurrentRowSegments(prevRowSegments, currRowSegments, uf, params);
|
||||
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
|
||||
|
||||
// ---- Phase 3: 按并查集根汇总所有 segment → SGrowthCluster ----
|
||||
FlattenTreesToClusters(allSegments, uf, points, params, outClusters);
|
||||
|
||||
return (int)outClusters.size();
|
||||
return static_cast<int>(outClusters.size());
|
||||
}
|
||||
|
||||
} // namespace bar_intersection
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user