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-04-07 16:20:38 +08:00
parent 9e4244d86a
commit 6c55510809
1 changed files with 360 additions and 490 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

850
Algo/DetectBarIntersection/src/RegionGrowing.cpp
View File

@ -1,490 +1,360 @@
#include "RegionGrowing.h"
#include <cmath>
#include <algorithm>
#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;
int endCol;
int treeId;
std::vector<SGridPoint> points;
float sumX;
float sumY;
float sumZ;
SVzNL3DPointF centroid;
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();
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]);
}
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 (rank[ra] < rank[rb]) {
int tmp = ra; ra = rb; rb = tmp;
}
parent[rb] = ra;
if (rank[ra] == rank[rb]) {
rank[ra]++;
}
return ra;
}
};
/**
* @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;
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;
seg.sumZ = pt.z;
seg.centroid.x = pt.x;
seg.centroid.y = pt.y;
seg.centroid.z = pt.z;
seg.pointCount = 1;
return seg;
}
/**
* @brief 向当前 segment 追加一个点，更新坐标累加器和列范围
*/
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++;
}
/**
* @brief 结算 segment 的质心 = sum / count
*/
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;
}
}
/**
* @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 MergeCurrentRowSegments(
const std::vector<SLineSegment>& prevRowSegments,
std::vector<SLineSegment>& currRowSegments,
UnionFind& uf,
const SGrowthParams& params
) {
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++) {
SLineSegment& currSeg = currRowSegments[ci];
std::vector<int> matchedRoots; // 匹配到的不同树的根节点列表
int bestRoot = -1; // x 距离最近的匹配根
float bestDx = std::numeric_limits<float>::max();
if (rowsAdjacent) {
// 与前一行的每个 segment 逐一比较
for (size_t pi = 0; pi < prevRowSegments.size(); pi++) {
const SLineSegment& prevSeg = prevRowSegments[pi];
// 条件 1：列范围必须有重叠或紧邻
if (!HasColumnOverlapOrTouch(prevSeg, currSeg)) continue;
// 条件 2：质心 x 差必须小于阈值（同一钢筋跨行 x 偏移小）
float dx = std::abs(currSeg.centroid.x - prevSeg.centroid.x);
if (dx >= params.thresholdX) 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 在前一行，只记录一次
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);
}
// 追踪 x 距离最近的根，作为首选归属
if (dx < bestDx) {
bestDx = dx;
bestRoot = root;
}
}
}
if (matchedRoots.empty()) {
// 没有匹配到任何前一行 segment → 创建新树
currSeg.treeId = uf.MakeSet();
} else {
// 匹配到至少一棵树 → 归入 x 最近的树
currSeg.treeId = bestRoot;
// 如果匹配到多棵不同的树，说明这些树在当前行交汇，全部合并
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,
const SVzNLPointXYZ* points,
const SGrowthParams& params,
std::vector<SGrowthCluster>& outClusters
) {
if (allSegments.empty()) return;
// rootToBucket: 并查集根 ID → buckets 数组下标的映射
// 首次遇到某个根时分配一个新 bucket
int ufSize = (int)uf.parent.size();
std::vector<int> rootToBucket(ufSize, -1);
std::vector<STreeAccumulator> buckets;
// 遍历所有 segment，按最终根节点归入对应 bucket
for (size_t s = 0; s < allSegments.size(); s++) {
const SLineSegment& seg = allSegments[s];
int root = uf.Find(seg.treeId); // 查找该 segment 所属树的最终根
// 如果该根还没有分配 bucket，创建一个新的
if (rootToBucket[root] == -1) {
rootToBucket[root] = (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.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++) {
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++;
}
}
// 将 bucket 转为 SGrowthCluster，过滤掉点数不足的小簇
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);
}
}
// =====================================================================
// 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,
int cols,
const SGrowthParams& params,
std::vector<SGrowthCluster>& outClusters
) {
outClusters.clear();
// 输入校验
if (!points || cols <= 0 || rows <= 0) {
return 0;
}
int totalPoints = rows * cols;
// ---- 状态初始化 ----
UnionFind uf; // 并查集，管理树的合并
std::vector<SLineSegment> allSegments; // 所有行产生的 segment（Phase 3 汇总用）
std::vector<SLineSegment> prevRowSegments; // 前一行的 segment 列表（Phase 2 匹配用）
std::vector<SLineSegment> currRowSegments; // 当前行的 segment 列表
int currRow = -1; // 当前正在处理的行号，-1 表示尚未开始
bool hasOpenSegment = false; // 是否有一个正在构建中的 segment
SLineSegment openSeg; // 正在构建中的 segment
// ---- 主循环：按行主序遍历所有点，跳过 (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();
}
} // namespace bar_intersection
#include "RegionGrowing.h"
#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>
namespace bar_intersection {
struct SGridPoint {
int row;
int col;
int linearIdx;
};
struct SLineSegment {
int row;
int startCol;
int endCol;
int treeId;
std::vector<SGridPoint> points;
float sumX;
float sumY;
float sumZ;
SVzNL3DPointF centroid;
int pointCount;
};
struct UnionFind {
std::vector<int> parent;
std::vector<unsigned char> rank;
int MakeSet() {
int id = static_cast<int>(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<int> 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 HasColumnOverlapOrTouch(const SLineSegment& a, const SLineSegment& b) {
return !(b.endCol < a.startCol - 1 || a.endCol < b.startCol - 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;
}
bool rowsAdjacent = false;
if (!prevRowSegments.empty()) {
rowsAdjacent = (currRowSegments[0].row == prevRowSegments[0].row + 1);
}
for (size_t ci = 0; ci < currRowSegments.size(); ++ci) {
SLineSegment& currSeg = currRowSegments[ci];
std::vector<int> matchedRoots;
int bestRoot = -1;
float bestDx = std::numeric_limits<float>::max();
if (rowsAdjacent) {
for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) {
const SLineSegment& prevSeg = prevRowSegments[pi];
if (!HasColumnOverlapOrTouch(prevSeg, currSeg)) {
continue;
}
const 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;
}
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 (dx < bestDx) {
bestDx = dx;
bestRoot = root;
}
}
}
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);
}
}
static void FlattenTreesToClusters(
const std::vector<SLineSegment>& allSegments,
UnionFind& uf,
const SVzNLPointXYZ* points,
const SGrowthParams& params,
std::vector<SGrowthCluster>& outClusters
) {
if (allSegments.empty()) {
return;
}
std::vector<int> rootToBucket(static_cast<size_t>(uf.parent.size()), -1);
std::vector<STreeAccumulator> 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<int>(buckets.size());
STreeAccumulator acc;
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]];
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<SGrowthCluster>& outClusters
) {
outClusters.clear();
if (!points || cols <= 0 || rows <= 0) {
return 0;
}
UnionFind uf;
std::vector<SLineSegment> allSegments;
std::vector<SLineSegment> prevRowSegments;
std::vector<SLineSegment> currRowSegments;
for (int row = 0; row < rows; ++row) {
const SVzNLPointXYZ* rowPoints = points + row * cols;
ProcessCurrentRowSegmentPoint(
rowPoints,
static_cast<unsigned int>(cols),
row,
params,
currRowSegments
);
MergeCurrentRowSegments(prevRowSegments, currRowSegments, uf, params);
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
prevRowSegments.swap(currRowSegments);
currRowSegments.clear();
}
FlattenTreesToClusters(allSegments, uf, points, params, outClusters);
return static_cast<int>(outClusters.size());
}
} // namespace bar_intersection