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 19:34:48 +08:00
parent 6c55510809
commit cced2a9646
1 changed files with 141 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -111,8 +111,89 @@ static void FinalizeSegment(SLineSegment& seg) {
}
}
static bool HasColumnOverlapOrTouch(const SLineSegment& a, const SLineSegment& b) {
return !(b.endCol < a.startCol - 1 || a.endCol < b.startCol - 1);
static bool HasColumnOverlap(const SLineSegment& a, const SLineSegment& b) {
return !(b.endCol < a.startCol || a.endCol < b.startCol);
}
static bool HasAnyColumnOverlapWithSegments(
const SLineSegment& seg,
const std::vector<SLineSegment>& otherSegments
) {
for (size_t i = 0; i < otherSegments.size(); ++i) {
if (HasColumnOverlap(seg, otherSegments[i])) {
return true;
}
}
return false;
}
struct SSegmentRelationResult {
bool hasOverlap;
bool hasMatch;
float bestDistanceSq;
};
struct SCarrySegmentState {
bool hasOverlap;
bool hasMatch;
};
static SSegmentRelationResult EvaluateSegmentRelation(
const SLineSegment& prevSeg,
const SLineSegment& currSeg,
const SVzNLPointXYZ* points,
const SGrowthParams& params
) {
SSegmentRelationResult result;
result.hasOverlap = false;
result.hasMatch = false;
result.bestDistanceSq = std::numeric_limits<float>::max();
if (!HasColumnOverlap(prevSeg, currSeg)) {
return result;
}
const float pointDistanceThresholdSq =
params.thresholdX * params.thresholdX +
params.thresholdY * params.thresholdY +
params.thresholdZ * params.thresholdZ;
result.hasOverlap = true;
size_t prevIdx = 0;
size_t currIdx = 0;
while (prevIdx < prevSeg.points.size() && currIdx < currSeg.points.size()) {
const SGridPoint& prevGp = prevSeg.points[prevIdx];
const SGridPoint& currGp = currSeg.points[currIdx];
if (prevGp.col < currGp.col) {
++prevIdx;
continue;
}
if (currGp.col < prevGp.col) {
++currIdx;
continue;
}
const SVzNLPointXYZ& prevPt = points[prevGp.linearIdx];
const SVzNLPointXYZ& currPt = points[currGp.linearIdx];
const float dx = currPt.x - prevPt.x;
const float dy = currPt.y - prevPt.y;
const float dz = currPt.z - prevPt.z;
const float distanceSq = dx * dx + dy * dy + dz * dz;
if (distanceSq < pointDistanceThresholdSq) {
result.hasMatch = true;
if (distanceSq < result.bestDistanceSq) {
result.bestDistanceSq = distanceSq;
}
}
++prevIdx;
++currIdx;
}
return result;
}
static void ProcessCurrentRowSegmentPoint(
@ -128,6 +209,10 @@ static void ProcessCurrentRowSegmentPoint(
const float eps = 1e-6f;
const int cols = static_cast<int>(pointCount);
const int rowOffset = row * cols;
const float segmentDistanceThresholdSq =
params.thresholdX * params.thresholdX +
params.thresholdY * params.thresholdY +
params.thresholdZ * params.thresholdZ;
bool hasOpenSegment = false;
SLineSegment openSeg;
@ -152,10 +237,12 @@ static void ProcessCurrentRowSegmentPoint(
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);
const float dx = pt.x - lastPt.x;
const float dy = pt.y - lastPt.y;
const float dz = pt.z - lastPt.z;
const float distanceSq = dx * dx + dy * dy + dz * dz;
if (dy < params.thresholdY && dz < params.thresholdZ) {
if (distanceSq < segmentDistanceThresholdSq) {
AppendPoint(openSeg, gp, pt);
continue;
}
@ -173,42 +260,37 @@ static void ProcessCurrentRowSegmentPoint(
static void MergeCurrentRowSegments(
const std::vector<SLineSegment>& prevRowSegments,
const std::vector<SLineSegment>& carriedRowSegments,
std::vector<SLineSegment>& currRowSegments,
UnionFind& uf,
const SGrowthParams& params
const SVzNLPointXYZ* points,
const SGrowthParams& params,
std::vector<SLineSegment>& nextCarriedRowSegments
) {
if (currRowSegments.empty()) {
return;
}
bool rowsAdjacent = false;
if (!prevRowSegments.empty()) {
rowsAdjacent = (currRowSegments[0].row == prevRowSegments[0].row + 1);
}
nextCarriedRowSegments.clear();
std::vector<SCarrySegmentState> prevStates(prevRowSegments.size(), SCarrySegmentState{ false, false });
std::vector<SCarrySegmentState> carriedStates(carriedRowSegments.size(), SCarrySegmentState{ false, false });
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();
float bestDistanceSq = 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)) {
auto tryMatchSegments = [&](const std::vector<SLineSegment>& candidateSegments, std::vector<SCarrySegmentState>& candidateStates) {
for (size_t pi = 0; pi < candidateSegments.size(); ++pi) {
const SLineSegment& prevSeg = candidateSegments[pi];
const SSegmentRelationResult relation = EvaluateSegmentRelation(prevSeg, currSeg, points, params);
if (!relation.hasOverlap) {
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) {
candidateStates[pi].hasOverlap = true;
if (!relation.hasMatch) {
continue;
}
candidateStates[pi].hasMatch = true;
const int root = uf.Find(prevSeg.treeId);
bool found = false;
for (size_t m = 0; m < matchedRoots.size(); ++m) {
@ -221,12 +303,15 @@ static void MergeCurrentRowSegments(
matchedRoots.push_back(root);
}
if (dx < bestDx) {
bestDx = dx;
if (relation.bestDistanceSq < bestDistanceSq) {
bestDistanceSq = relation.bestDistanceSq;
bestRoot = root;
}
}
}
};
tryMatchSegments(prevRowSegments, prevStates);
tryMatchSegments(carriedRowSegments, carriedStates);
if (matchedRoots.empty()) {
currSeg.treeId = uf.MakeSet();
@ -239,6 +324,20 @@ static void MergeCurrentRowSegments(
}
currSeg.treeId = uf.Find(currSeg.treeId);
}
for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) {
const SLineSegment& prevSeg = prevRowSegments[pi];
if (!prevStates[pi].hasOverlap && !prevStates[pi].hasMatch) {
nextCarriedRowSegments.push_back(prevSeg);
}
}
for (size_t pi = 0; pi < carriedRowSegments.size(); ++pi) {
const SLineSegment& carriedSeg = carriedRowSegments[pi];
if (!carriedStates[pi].hasOverlap && !carriedStates[pi].hasMatch) {
nextCarriedRowSegments.push_back(carriedSeg);
}
}
}
static void FlattenTreesToClusters(
@ -333,6 +432,8 @@ int RegionGrowClusters(
UnionFind uf;
std::vector<SLineSegment> allSegments;
std::vector<SLineSegment> prevRowSegments;
std::vector<SLineSegment> carriedRowSegments;
std::vector<SLineSegment> nextCarriedRowSegments;
std::vector<SLineSegment> currRowSegments;
for (int row = 0; row < rows; ++row) {
@ -346,9 +447,19 @@ int RegionGrowClusters(
currRowSegments
);
MergeCurrentRowSegments(prevRowSegments, currRowSegments, uf, params);
MergeCurrentRowSegments(
prevRowSegments,
carriedRowSegments,
currRowSegments,
uf,
points,
params,
nextCarriedRowSegments
);
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
carriedRowSegments.swap(nextCarriedRowSegments);
nextCarriedRowSegments.clear();
prevRowSegments.swap(currRowSegments);
currRowSegments.clear();
}