增加注释
This commit is contained in:
parent
976f87b6ba
commit
937e9c0d8a
File diff suppressed because it is too large
Load Diff
@ -101,6 +101,11 @@ static void RemoveRowOutliersInPlace(
|
||||
int cols,
|
||||
const SGrowthParams& params
|
||||
) {
|
||||
// 行内离群点抑制：
|
||||
// 1. 如果某个有效点左右都没有有效邻居，则它大概率是孤立噪声，直接抑制。
|
||||
// 2. 如果左右邻点彼此趋势平稳，但当前点相对左右两侧同时发生明显跳变，
|
||||
// 则将其视为尖刺噪声。
|
||||
// 该步骤只在单行内部做判断，目的是先清掉容易破坏连续性的局部异常点。
|
||||
if (!rowPoints || cols <= 0) {
|
||||
return;
|
||||
}
|
||||
@ -117,11 +122,15 @@ static void RemoveRowOutliersInPlace(
|
||||
const bool hasPrev = (col > 0) && IsValidPoint(rowPoints[col - 1]);
|
||||
const bool hasNext = (col + 1 < cols) && IsValidPoint(rowPoints[col + 1]);
|
||||
|
||||
// 左右都不存在有效邻点，说明该点无法与任何局部结构形成连续片段，
|
||||
// 直接标记为待抑制。
|
||||
if (!hasPrev && !hasNext) {
|
||||
suppressFlags[static_cast<size_t>(col)] = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 只有一侧存在邻点时，证据不足以判定该点一定是尖刺，
|
||||
// 保留给后续的特征提取与跨行生长阶段处理。
|
||||
if (!(hasPrev && hasNext)) {
|
||||
continue;
|
||||
}
|
||||
@ -144,6 +153,8 @@ static void RemoveRowOutliersInPlace(
|
||||
prevZDiff > params.thresholdZ * spikeScale &&
|
||||
nextZDiff > params.thresholdZ * spikeScale;
|
||||
|
||||
// 仅当“两侧本身连续”且“当前点对两侧都显著偏离”同时成立时，
|
||||
// 才把当前点视为孤立尖刺，避免误删真实边缘点。
|
||||
if (neighbourTrendStable && obviousSpike) {
|
||||
suppressFlags[static_cast<size_t>(col)] = true;
|
||||
}
|
||||
@ -154,6 +165,8 @@ static void RemoveRowOutliersInPlace(
|
||||
continue;
|
||||
}
|
||||
|
||||
// 将被抑制的点清零。后续流程通过 IsValidPoint 判零值无效点，
|
||||
// 因此这里不需要额外的标记位。
|
||||
rowPoints[col].x = 0.0f;
|
||||
rowPoints[col].y = 0.0f;
|
||||
rowPoints[col].z = 0.0f;
|
||||
@ -167,6 +180,10 @@ static void ExtractRowFeatures(
|
||||
const SGrowthParams& params,
|
||||
std::vector<SLineFeature>& outFeatures
|
||||
) {
|
||||
// 行内特征提取：
|
||||
// 按列从左到右扫描，把在 Y/Z 上连续变化的点合并成一个 line feature。
|
||||
// 一个 feature 表示当前扫描行上的一段连续条带片段，后续跨行生长时
|
||||
// 不再直接匹配单点，而是匹配这些更稳定的片段摘要。
|
||||
outFeatures.clear();
|
||||
if (!rowPoints || cols <= 0) {
|
||||
return;
|
||||
@ -179,6 +196,8 @@ static void ExtractRowFeatures(
|
||||
for (int col = 0; col < cols; ++col) {
|
||||
const SVzNLPointXYZ& pt = rowPoints[col];
|
||||
if (!IsValidPoint(pt)) {
|
||||
// 无效点天然形成分段边界：如果当前存在打开的 feature，
|
||||
// 则先收尾并输出。
|
||||
if (hasOpenFeature) {
|
||||
FinalizeFeature(openFeature);
|
||||
outFeatures.push_back(openFeature);
|
||||
@ -192,6 +211,7 @@ static void ExtractRowFeatures(
|
||||
gp.col = col;
|
||||
gp.linearIdx = rowOffset + col;
|
||||
|
||||
// 当前没有打开的 feature，说明遇到了一个新片段的起点。
|
||||
if (!hasOpenFeature) {
|
||||
openFeature = StartNewFeature(gp, pt);
|
||||
hasOpenFeature = true;
|
||||
@ -203,17 +223,21 @@ static void ExtractRowFeatures(
|
||||
const float yDiff = std::abs(pt.y - lastPt.y);
|
||||
const float zDiff = std::abs(pt.z - lastPt.z);
|
||||
|
||||
// 与当前片段最后一个点在 Y/Z 上足够接近，则视为同一条连续片段，
|
||||
// 继续向右扩展。
|
||||
if (yDiff <= params.thresholdY && zDiff <= params.thresholdZ) {
|
||||
AppendPointToFeature(openFeature, gp, pt);
|
||||
continue;
|
||||
}
|
||||
|
||||
// 否则说明当前点与已有片段发生断裂，先结束旧片段，再以该点开启新片段。
|
||||
FinalizeFeature(openFeature);
|
||||
outFeatures.push_back(openFeature);
|
||||
openFeature = StartNewFeature(gp, pt);
|
||||
hasOpenFeature = true;
|
||||
}
|
||||
|
||||
// 扫描结束后，如果仍有未关闭的片段，需要补一次收尾。
|
||||
if (hasOpenFeature) {
|
||||
FinalizeFeature(openFeature);
|
||||
outFeatures.push_back(openFeature);
|
||||
@ -225,6 +249,10 @@ static float EstimateTypicalRowStepX(
|
||||
int rows,
|
||||
int cols
|
||||
) {
|
||||
// 估计扫描线在 X 方向上的典型行间距。
|
||||
// 做法不是直接取相邻点差，而是先求每一行所有有效点的平均 X，
|
||||
// 再统计相邻有效行的平均 X 差值，最后取中位数。
|
||||
// 使用中位数而不是均值，可以降低个别缺行、异常行对步长估计的影响。
|
||||
std::vector<float> xDiffs;
|
||||
bool hasPrevMeanX = false;
|
||||
float prevMeanX = 0.0f;
|
||||
@ -241,6 +269,7 @@ static float EstimateTypicalRowStepX(
|
||||
validCount++;
|
||||
}
|
||||
|
||||
// 整行没有有效点时，不参与步长估计。
|
||||
if (validCount <= 0) {
|
||||
continue;
|
||||
}
|
||||
@ -248,6 +277,7 @@ static float EstimateTypicalRowStepX(
|
||||
const float meanX = sumX / validCount;
|
||||
if (hasPrevMeanX) {
|
||||
const float diff = std::abs(meanX - prevMeanX);
|
||||
// 忽略接近 0 的差值，避免数值噪声污染中位数统计。
|
||||
if (diff > kInvalidPointEpsilon) {
|
||||
xDiffs.push_back(diff);
|
||||
}
|
||||
@ -256,6 +286,8 @@ static float EstimateTypicalRowStepX(
|
||||
hasPrevMeanX = true;
|
||||
}
|
||||
|
||||
// 如果无法从数据中估计稳定步长，则退化为 1.0f，
|
||||
// 让后续最大跨行数仍能得到一个可用默认值。
|
||||
if (xDiffs.empty()) {
|
||||
return 1.0f;
|
||||
}
|
||||
@ -266,6 +298,10 @@ static float EstimateTypicalRowStepX(
|
||||
}
|
||||
|
||||
static int DeriveMaxLineSkipNum(const SGrowthParams& params, float rowStepX) {
|
||||
// 将物理空间里的 X 向允许跨度，换算为“最多可跨过多少行”的整数约束。
|
||||
// 例如 thresholdX 大约等于 2 个 rowStepX 时，就允许 feature 在跨行
|
||||
// 匹配时容忍更大的行号间隔。
|
||||
// 返回值至少为 2，表示除了相邻行以外，默认还允许跨过 1 行空洞。
|
||||
if (rowStepX <= kInvalidPointEpsilon || params.thresholdX <= kInvalidPointEpsilon) {
|
||||
return 2;
|
||||
}
|
||||
@ -375,6 +411,10 @@ static void GrowRowFeatures(
|
||||
const SGrowthParams& params,
|
||||
int maxLineSkipNum
|
||||
) {
|
||||
// 跨行生长：
|
||||
// 当前行的每个 feature 都尝试挂接到已有 tree 的末端节点上。
|
||||
// TryGrowFeature 内部会综合质心距离、列间断裂以及行间距选择最优 tree。
|
||||
// 若没有任何 tree 满足约束，则该 feature 启动一棵新 tree。
|
||||
for (size_t i = 0; i < rowFeatures.size(); ++i) {
|
||||
const SLineFeature& feature = rowFeatures[i];
|
||||
if (!TryGrowFeature(feature, rowIdx, trees, params, maxLineSkipNum)) {
|
||||
@ -387,6 +427,8 @@ static void GrowRowFeatures(
|
||||
}
|
||||
}
|
||||
|
||||
// 当前行处理完后，检查哪些 tree 已经长时间没有被新 feature 延续。
|
||||
// 这些 tree 被置为死亡；若节点数过少，则直接丢弃，避免形成短小伪聚类。
|
||||
FinalizeInactiveTrees(
|
||||
rowIdx,
|
||||
isLastRow,
|
||||
@ -401,10 +443,16 @@ static void FlattenTreesToClusters(
|
||||
const SGrowthParams& params,
|
||||
std::vector<SGrowthCluster>& outClusters
|
||||
) {
|
||||
// 将中间态的 growth tree 转成最终输出 cluster。
|
||||
// tree 内保存的是按行组织的 feature，而最终 cluster 需要的是：
|
||||
// 1. 展平后的点索引集合；
|
||||
// 2. 质心与包围盒；
|
||||
// 3. 满足最小点数阈值的有效聚类。
|
||||
outClusters.clear();
|
||||
|
||||
for (size_t t = 0; t < trees.size(); ++t) {
|
||||
const SGrowthTree& tree = trees[t];
|
||||
// 少于 2 个节点的 tree 缺少跨行连续性，视为不稳定候选，直接跳过。
|
||||
if (!IsValidTree(tree)) {
|
||||
continue;
|
||||
}
|
||||
@ -427,6 +475,8 @@ static void FlattenTreesToClusters(
|
||||
const int linearIdx = feature.points[p].linearIdx;
|
||||
const SVzNLPointXYZ& pt = points[linearIdx];
|
||||
|
||||
// 把 tree 中每个 feature 重新展开成原始点集合，同时累计
|
||||
// cluster 的一阶统计量与包围盒边界。
|
||||
acc.pointIndices.push_back(linearIdx);
|
||||
acc.sumX += pt.x;
|
||||
acc.sumY += pt.y;
|
||||
@ -441,6 +491,7 @@ static void FlattenTreesToClusters(
|
||||
}
|
||||
}
|
||||
|
||||
// 点数过小的 tree 通常是噪声或残缺目标，不输出为最终 cluster。
|
||||
if (acc.pointCount < params.minClusterSize) {
|
||||
continue;
|
||||
}
|
||||
@ -460,6 +511,7 @@ static void FlattenTreesToClusters(
|
||||
outClusters.push_back(cluster);
|
||||
}
|
||||
|
||||
// 按点数从大到小排序，方便调用方优先处理主目标。
|
||||
std::sort(
|
||||
outClusters.begin(),
|
||||
outClusters.end(),
|
||||
@ -478,22 +530,43 @@ int RegionGrowClusters(
|
||||
const SGrowthParams& params,
|
||||
std::vector<SGrowthCluster>& outClusters
|
||||
) {
|
||||
// 每次调用都从空结果开始，避免调用方看到上一次遗留的聚类内容。
|
||||
outClusters.clear();
|
||||
|
||||
// 基本输入检查：算法默认输入点云按 rows x cols 的规则网格排列，
|
||||
// 因此指针为空或尺寸非法时直接返回。
|
||||
if (!points || rows <= 0 || cols <= 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 阶段 1：估计扫描线在 X 方向上的典型行间距，并把 thresholdX
|
||||
// 换算成“跨行生长时最多允许跳过多少行”的整数上限。
|
||||
// 这样后续在存在缺行、空洞时仍可连接同一目标，但不会无约束地跨越过远距离。
|
||||
const float rowStepX = EstimateTypicalRowStepX(points, rows, cols);
|
||||
const int maxLineSkipNum = DeriveMaxLineSkipNum(params, rowStepX);
|
||||
|
||||
// 后续预处理需要原地清零噪声点，因此先拷贝一份工作副本。
|
||||
// 原始输入保持不变，副本同时作为后续特征提取和 cluster 统计的输入。
|
||||
std::vector<SVzNLPointXYZ> workingPoints(points, points + rows * cols);
|
||||
|
||||
// 先对每一行提取 line feature，并缓存到表中。
|
||||
// 这样可以把“行内分段”和“跨行关联”拆成两个阶段，降低跨行匹配时的噪声敏感性。
|
||||
std::vector<std::vector<SLineFeature> > rowFeatureTable(static_cast<size_t>(rows));
|
||||
|
||||
// tree 是区域生长阶段的中间表示。
|
||||
// 每棵 tree 收集一组跨越多行、被判定为属于同一根条材的 feature。
|
||||
std::vector<SGrowthTree> trees;
|
||||
|
||||
for (int row = 0; row < rows; ++row) {
|
||||
SVzNLPointXYZ* rowPoints = workingPoints.data() + row * cols;
|
||||
|
||||
// 阶段 2a：先在当前行内抑制孤立噪声和尖刺噪声，
|
||||
// 避免它们把连续条带错误切断，或单独形成伪目标。
|
||||
RemoveRowOutliersInPlace(rowPoints, cols, params);
|
||||
|
||||
// 阶段 2b：把过滤后的当前行切分成若干连续 feature。
|
||||
// 相邻点只有在 Y/Z 差异都不超过阈值时才归入同一 feature，
|
||||
// 每个 feature 对应当前扫描行上的一个局部平滑条带片段。
|
||||
ExtractRowFeatures(
|
||||
rowPoints,
|
||||
row,
|
||||
@ -504,6 +577,10 @@ int RegionGrowClusters(
|
||||
}
|
||||
|
||||
for (int row = 0; row < rows; ++row) {
|
||||
// 阶段 3：按行推进，把当前行的 feature 尝试接到已有 tree 上。
|
||||
// 匹配时综合考虑 feature 质心相似性、列方向断裂和最大跨行数；
|
||||
// 若没有合适的 tree，则以该 feature 新建一棵 tree。
|
||||
// 长时间未被延续的 tree 会在内部被终结，过短的 tree 直接剔除。
|
||||
GrowRowFeatures(
|
||||
row,
|
||||
row == rows - 1,
|
||||
@ -514,6 +591,9 @@ int RegionGrowClusters(
|
||||
);
|
||||
}
|
||||
|
||||
// 阶段 4：把存活下来的 tree 展平为最终 cluster。
|
||||
// 这里会回收所有点索引，计算 cluster 的质心和包围盒，
|
||||
// 过滤点数过少的候选，并按点数从大到小输出结果。
|
||||
FlattenTreesToClusters(trees, workingPoints.data(), params, outClusters);
|
||||
return static_cast<int>(outClusters.size());
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user