cool609/WoodAlgo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改算法，获取了单根钢筋

Browse Source
This commit is contained in:
cool609 2026-04-11 23:24:16 +08:00
parent 1d649f9c01
commit 7969a242ba
6 changed files with 179 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
Algo/DetectBarIntersection/examples/visualization_demo/BarIntersectionVisualizer.cpp
View File

@ -334,27 +334,27 @@ void BarIntersectionVisualizer::VisualizeResult(
);
}
void BarIntersectionVisualizer::VisualizeBestCluster(
const SBarIntersectionResult& result,
const std::string& title
) {
if (result.bestClusterIndex < 0 || result.bestClusterPoints.empty()) {
std::cout << "No valid best cluster to visualize." << std::endl;
return;
}
auto renderer = vtkSmartPointer<vtkRenderer>::New();
// Best cluster filtered points (aligned coordinate system)
vtkActor* bestActor = static_cast<vtkActor*>(
CreatePointCloudActor(
result.bestClusterPoints.data(),
(int)result.bestClusterPoints.size(),
0.0, 1.0, 0.3, 3.0
)
);
renderer->AddActor(bestActor);
bestActor->Delete();
ShowWindow(renderer, title, m_interactive, this, "06_best_cluster.png");
}
void BarIntersectionVisualizer::VisualizeBestCluster(
const SBarIntersectionResult& result,
const std::string& title
) {
if (result.bestClusterIndex < 0 || result.bestClusterPoints.empty()) {
std::cout << "No valid best cluster to visualize." << std::endl;
return;
}
auto renderer = vtkSmartPointer<vtkRenderer>::New();
// Best cluster filtered points (aligned coordinate system)
vtkActor* bestActor = static_cast<vtkActor*>(
CreatePointCloudActor(
result.bestClusterPoints.data(),
(int)result.bestClusterPoints.size(),
0.0, 1.0, 0.3, 3.0
)
);
renderer->AddActor(bestActor);
bestActor->Delete();
ShowWindow(renderer, title, m_interactive, this, "06_best_cluster.png");
}

8
Algo/DetectBarIntersection/examples/visualization_demo/VisualizationDemo.cpp
View File

@ -251,10 +251,10 @@ int ProcessSingleFile(
// Final visualization
visualizer.VisualizeResult(points, rows, cols, result, "Final Result");
// Best cluster visualization (filtered aligned points)
visualizer.VisualizeBestCluster(result, "Best Cluster (min Z, high pts)");
// Cleanup
// Best cluster visualization (filtered aligned points)
visualizer.VisualizeBestCluster(result, "Best Cluster (min Z, high pts)");
// Cleanup
FreeBarIntersectionResult(&result);
delete[] points;

4
Algo/DetectBarIntersection/include/BarIntersectionParams.h
View File

@ -37,6 +37,8 @@ struct SGrowthParams {
float maxPerpendicularDeviationDeg; // Max deviation from 90 deg for centroid-connection test
int minContinuousValidPointCount; // Min consecutive valid points kept on each laser line after plane filtering; <=1 disables
float maxContinuousPointZTolerance; // Max z difference between adjacent points inside one valid run (mm)
float minBarDiameter;
float maxBarDiameter;
SGrowthParams()
: thresholdX(5.0f)
@ -49,6 +51,8 @@ struct SGrowthParams {
, maxPerpendicularDeviationDeg(50.0f)
, minContinuousValidPointCount(0)
, maxContinuousPointZTolerance(5.0f)
, minBarDiameter(10.0f)
, maxBarDiameter(30.0f)
{}
};

5
Algo/DetectBarIntersection/src/BarIntersection.cpp
View File

@ -249,7 +249,10 @@ int DetectBarIntersections(
// ============================================
std::vector<SGrowthCluster> clusters;
bar_intersection::RegionGrowClusters(
alignedPoints, rows, cols, growthParams, clusters
alignedPoints, rows, cols, true, growthParams, clusters
);
bar_intersection::RegionGrowClusters(
alignedPoints, rows, cols, false, growthParams, clusters
);
// ============================================

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

@ -114,86 +114,36 @@ static void FinalizeSegment(SLineSegment& seg) {
}
}
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 =
const float centroidDistanceThresholdSq =
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;
const float dx = currSeg.centroid.x - prevSeg.centroid.x;
const float dy = currSeg.centroid.y - prevSeg.centroid.y;
const float dz = currSeg.centroid.z - prevSeg.centroid.z;
const float distanceSq = dx * dx + dy * dy + dz * dz;
if (distanceSq < centroidDistanceThresholdSq) {
result.hasMatch = true;
result.bestDistanceSq = distanceSq;
}
return result;
@ -222,7 +172,9 @@ static bool IsValidPoint(const SVzNLPointXYZ& pt) {
void EvaluateLine(
const SVzNLPointXYZ* points,
int count,
int row,
int step,
int startIdx,
int cols,
const SGrowthParams& params,
std::vector<SLineSegment>& currRowSegments,
bool& bIsInvalidLine,
@ -234,7 +186,7 @@ void EvaluateLine(
constexpr float kPi = 3.14159265358979323846f;
const float searchDist = params.angleSearchDistance;
const float posThresh = params.maxAxisDeviationFromXYDeg;
const float negThresh = params.maxPerpendicularDeviationDeg;
const float negThresh = -params.maxPerpendicularDeviationDeg;
// YZ_PANEL: coord=y, XZ_PANEL: coord=x
auto getCoord = [panelType](const SVzNLPointXYZ& pt) -> float {
@ -252,7 +204,8 @@ void EvaluateLine(
// ================================================================
struct PointInfo {
SVzNLPointXYZ point;
int linePos;
int row;
int col;
int globalIdx;
bool valid;
float signedAngleDeg;
@ -261,13 +214,14 @@ void EvaluateLine(
};
std::vector<PointInfo> pts(count);
int startIdx = row * count; // global index of the first point in this line
auto curPoint = points;
for (int i = 0; i < count; i++) {
int gIdx = startIdx + i;
pts[i].point = points[i];
pts[i].linePos = i;
int gIdx = startIdx + i * step;
pts[i].point = *curPoint;
pts[i].row = gIdx / cols;
pts[i].col = gIdx % cols;
pts[i].globalIdx = gIdx;
pts[i].valid = IsValidPoint(points[i]);
pts[i].valid = IsValidPoint(*curPoint);
pts[i].signedAngleDeg = 0.0f;
pts[i].hasAngle = false;
pts[i].trend = keLineAngleTrend_Invalid;
@ -275,6 +229,8 @@ void EvaluateLine(
if (pts[i].valid) {
bIsInvalidLine = false;
}
curPoint += step;
}
for (int i = 0; i < count; i++) {
@ -324,10 +280,12 @@ void EvaluateLine(
pts[i].hasAngle = true;
// 两线夹角：line1(backRef→cur) 延长后 与 line2(cur→fwdRef) 的有符号夹角
// 局部弯折强度：line1(backRef→cur) 延长后 与 line2(cur→fwdRef) 的夹角绝对值。
// 与旧方案（backRef→fwdRef 整体坡度角）的本质区别：
// 均匀倾斜面（无弯折）→ 0°；只有在 pts[i] 处发生方向变化时才得到非零角。
// 正角度 = 向相机弯折（上升），负角度 = 远离相机弯折（下降进坑）
// signedAngleDeg 的语义：
// 符号 = 整体跳变方向（按坐标正向统一；低→高为正，高→低为负）
// 绝对值 = pts[i] 处的局部弯折强度
float curCoord = getCoord(cur);
float v1_coord = curCoord - backCoord; // backRef → cur（水平分量）
float v1_z = cur.z - backMeanZ; // backRef → cur（z 分量）
@ -337,9 +295,17 @@ void EvaluateLine(
// 2D 叉积（z 分量）和点积，用于计算有符号夹角
float cross2d = v1_coord * v2_z - v1_z * v2_coord;
float dot2d = v1_coord * v2_coord + v1_z * v2_z;
// 消除扫描方向（正向/反向）对叉积符号的影响
// 消除扫描方向（正向/反向）对整体跳变方向符号的影响
float scanDirSign = ((v1_coord + v2_coord) >= 0.0f) ? 1.0f : -1.0f;
float slopeAngleDeg = std::atan2(-cross2d * scanDirSign, dot2d) * (180.0f / kPi);
float bendAngleDeg = std::atan2(std::fabs(cross2d), dot2d) * (180.0f / kPi);
float overallDeltaZ = (fwdMeanZ - backMeanZ) * scanDirSign;
float slopeAngleDeg = 0.0f;
if (overallDeltaZ > 1e-6f) {
slopeAngleDeg = bendAngleDeg;
}
else if (overallDeltaZ < -1e-6f) {
slopeAngleDeg = -bendAngleDeg;
}
pts[i].signedAngleDeg = slopeAngleDeg;
@ -517,6 +483,36 @@ void EvaluateLine(
segs = merged;
}
const float minBarDiameter = params.minBarDiameter;
const float maxBarDiameter = params.maxBarDiameter;
for (size_t si = 0; si < segs.size(); ++si) {
const Segment& seg = segs[si];
if (seg.type != ESegType::Flat) continue;
if (seg.length < minBarDiameter || seg.length > maxBarDiameter) continue;
bool hasPoint = false;
SLineSegment lineSeg;
for (int pos = seg.startPos; pos <= seg.endPos; ++pos) {
if (!pts[pos].valid) continue;
SGridPoint gp;
gp.row = pts[pos].row;
gp.col = pts[pos].col;
gp.linearIdx = pts[pos].globalIdx;
if (!hasPoint) {
lineSeg = StartNewSegment(gp, pts[pos].point);
hasPoint = true;
}
else {
AppendPoint(lineSeg, gp, pts[pos].point);
}
}
if (!hasPoint) continue;
FinalizeSegment(lineSeg);
currRowSegments.push_back(lineSeg);
}
}
static void ProcessCurrentRowSegmentPoint(
@ -586,13 +582,12 @@ static void MergeCurrentRowSegments(
const std::vector<SLineSegment>& carriedRowSegments,
std::vector<SLineSegment>& currRowSegments,
UnionFind& uf,
const SVzNLPointXYZ* points,
const SGrowthParams& params,
std::vector<SLineSegment>& nextCarriedRowSegments
) {
nextCarriedRowSegments.clear();
std::vector<SCarrySegmentState> prevStates(prevRowSegments.size(), SCarrySegmentState{ false, false });
std::vector<SCarrySegmentState> carriedStates(carriedRowSegments.size(), SCarrySegmentState{ false, false });
std::vector<SCarrySegmentState> prevStates(prevRowSegments.size(), SCarrySegmentState{ false });
std::vector<SCarrySegmentState> carriedStates(carriedRowSegments.size(), SCarrySegmentState{ false });
for (size_t ci = 0; ci < currRowSegments.size(); ++ci) {
SLineSegment& currSeg = currRowSegments[ci];
@ -603,12 +598,7 @@ static void MergeCurrentRowSegments(
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;
}
candidateStates[pi].hasOverlap = true;
const SSegmentRelationResult relation = EvaluateSegmentRelation(prevSeg, currSeg, params);
if (!relation.hasMatch) {
continue;
}
@ -650,14 +640,14 @@ static void MergeCurrentRowSegments(
for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) {
const SLineSegment& prevSeg = prevRowSegments[pi];
if (!prevStates[pi].hasOverlap && !prevStates[pi].hasMatch) {
if (!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) {
if (!carriedStates[pi].hasMatch) {
nextCarriedRowSegments.push_back(carriedSeg);
}
}
@ -743,11 +733,10 @@ int RegionGrowClusters(
const SVzNLPointXYZ* points,
int rows,
int cols,
bool bHorizontalScan,
const SGrowthParams& params,
std::vector<SGrowthCluster>& outClusters
) {
outClusters.clear();
if (!points || cols <= 0 || rows <= 0) {
return 0;
}
@ -759,45 +748,81 @@ int RegionGrowClusters(
std::vector<SLineSegment> nextCarriedRowSegments;
std::vector<SLineSegment> currRowSegments;
for (int row = 0; row < rows; ++row) {
const SVzNLPointXYZ* rowPoints = points + row * cols;
if (bHorizontalScan)
{
int nPointIdx = 0;
const SVzNLPointXYZ* rowPoints = points;
for (int row = 0; row < rows; ++row) {
bool bIsValidLine = false;
EvaluateLine(
rowPoints,
static_cast<unsigned int>(cols),
1,
nPointIdx,
cols,
params,
currRowSegments,
bIsValidLine,
EPanelType::YZ_PANEL
);
bool bIsValidLine = false;
EvaluateLine(
rowPoints,
static_cast<unsigned int>(cols),
row,
params,
currRowSegments,
bIsValidLine,
EPanelType::YZ_PANEL
);
MergeCurrentRowSegments(
prevRowSegments,
carriedRowSegments,
currRowSegments,
uf,
params,
nextCarriedRowSegments
);
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
ProcessCurrentRowSegmentPoint(
rowPoints,
static_cast<unsigned int>(cols),
row,
params,
currRowSegments
);
carriedRowSegments.swap(nextCarriedRowSegments);
nextCarriedRowSegments.clear();
prevRowSegments.swap(currRowSegments);
currRowSegments.clear();
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();
nPointIdx += cols;
rowPoints += cols;
}
}
else
{
int nPointIdx = 0;
const SVzNLPointXYZ* colPoints = points;
for (int col = 0; col < cols; ++col) {
bool bIsValidLine = false;
EvaluateLine(
colPoints,
rows,
cols,
nPointIdx,
cols,
params,
currRowSegments,
bIsValidLine,
EPanelType::XZ_PANEL
);
MergeCurrentRowSegments(
prevRowSegments,
carriedRowSegments,
currRowSegments,
uf,
params,
nextCarriedRowSegments
);
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
carriedRowSegments.swap(nextCarriedRowSegments);
nextCarriedRowSegments.clear();
prevRowSegments.swap(currRowSegments);
currRowSegments.clear();
colPoints++;
nPointIdx++;
}
}
FlattenTreesToClusters(allSegments, uf, points, params, outClusters);
return static_cast<int>(outClusters.size());
}

1
Algo/DetectBarIntersection/src/RegionGrowing.h
View File

@ -28,6 +28,7 @@ int RegionGrowClusters(
const SVzNLPointXYZ* points,
int rows,
int cols,
bool bHorizontalScan,
const SGrowthParams& params,
std::vector<SGrowthCluster>& outClusters
);