diff --git a/Algo/DetectBarIntersection/src/BarIntersection.cpp b/Algo/DetectBarIntersection/src/BarIntersection.cpp index b4f8f74..1fd30d6 100644 --- a/Algo/DetectBarIntersection/src/BarIntersection.cpp +++ b/Algo/DetectBarIntersection/src/BarIntersection.cpp @@ -1,476 +1,476 @@ #include "BarIntersection.h" -#include "PlaneAlignment.h" -#include "RegionGrowing.h" -#include -#include -#include -#include +#include "PlaneAlignment.h" +#include "RegionGrowing.h" +#include +#include +#include +#include #include #include #include #include -static const char* s_algorithmVersion = "2.0.0"; -static const char* s_algorithmName = "BarIntersectionDetection"; +static const char* s_algorithmVersion = "1.0.0"; +static const char* s_algorithmName = "BarIntersectionDetection"; -static bool IsValidPoint(const SVzNLPointXYZ& pt) { - const float eps = 1e-6f; - return !(std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps); -} - -static SVzNLPointXYZ InverseTransformPoint( - const SVzNLPointXYZ& pt, - const float invRotation[9], - const SVzNL3DPointF& planeCentroid -) { - SVzNLPointXYZ transformed = {}; - transformed.x = invRotation[0] * pt.x + invRotation[1] * pt.y + invRotation[2] * pt.z + planeCentroid.x; - transformed.y = invRotation[3] * pt.x + invRotation[4] * pt.y + invRotation[5] * pt.z + planeCentroid.y; - transformed.z = invRotation[6] * pt.x + invRotation[7] * pt.y + invRotation[8] * pt.z + planeCentroid.z; - return transformed; -} - -static void ClearPoint(SVzNLPointXYZ* pt) { - if (!pt) { - return; - } - - pt->x = 0.0f; - pt->y = 0.0f; - pt->z = 0.0f; -} - -static void ClearShortContinuousSegment( - SVzNLPointXYZ* rowPoints, - int startCol, - int segmentLength, - int minContinuousValidPointCount -) { - if (!rowPoints || startCol < 0 || segmentLength <= 0) { - return; - } - - if (segmentLength >= minContinuousValidPointCount) { - return; - } - - for (int offset = 0; offset < segmentLength; ++offset) { - ClearPoint(&rowPoints[startCol + offset]); - } -} - -static void FilterLaserLineNoiseInPlace( - SVzNLPointXYZ* alignedPoints, - int rows, - int cols, - int minContinuousValidPointCount, - float maxContinuousPointZTolerance -) { - if (!alignedPoints || rows <= 0 || cols <= 0) { - return; - } - - if (minContinuousValidPointCount <= 1 || maxContinuousPointZTolerance < 0.0f) { - return; - } - - for (int row = 0; row < rows; ++row) { - SVzNLPointXYZ* rowPoints = alignedPoints + row * cols; - int segmentStartCol = -1; - int segmentLength = 0; - float prevZ = 0.0f; - - for (int col = 0; col < cols; ++col) { - const SVzNLPointXYZ& pt = rowPoints[col]; - if (!IsValidPoint(pt)) { - ClearShortContinuousSegment( - rowPoints, - segmentStartCol, - segmentLength, - minContinuousValidPointCount - ); - segmentStartCol = -1; - segmentLength = 0; - prevZ = 0.0f; - continue; - } - - if (segmentLength <= 0) { - segmentStartCol = col; - segmentLength = 1; - prevZ = pt.z; - continue; - } - - const float zDiff = std::abs(pt.z - prevZ); - if (zDiff <= maxContinuousPointZTolerance) { - ++segmentLength; - prevZ = pt.z; - continue; - } - - ClearShortContinuousSegment( - rowPoints, - segmentStartCol, - segmentLength, - minContinuousValidPointCount - ); - - segmentStartCol = col; - segmentLength = 1; - prevZ = pt.z; - } - - ClearShortContinuousSegment( - rowPoints, - segmentStartCol, - segmentLength, - minContinuousValidPointCount - ); - } -} - -struct SProjectedClusterPoint { - SVzNLPointXYZ point; - float majorCoord; - float minorCoord; -}; - -struct SClusterLineMetrics { - bool isValid; - SVzNLPointXYZ axisPointA; - SVzNLPointXYZ axisPointB; - float meanDistance; - float distanceStdDev; - float maxDistanceDeviation; - float inlierRatio; - float majorSpan; - - SClusterLineMetrics() - : isValid(false) - , axisPointA() - , axisPointB() - , meanDistance(0.0f) - , distanceStdDev(std::numeric_limits::max()) - , maxDistanceDeviation(std::numeric_limits::max()) - , inlierRatio(0.0f) - , majorSpan(0.0f) - {} -}; - -static float ComputePointToLineDistance( - const SVzNLPointXYZ& pt, - const SVzNLPointXYZ& lineStart, - const SVzNLPointXYZ& lineEnd -) { - const float dirX = lineEnd.x - lineStart.x; - const float dirY = lineEnd.y - lineStart.y; - const float dirZ = lineEnd.z - lineStart.z; - - const float vX = pt.x - lineStart.x; - const float vY = pt.y - lineStart.y; - const float vZ = pt.z - lineStart.z; - - const float crossX = vY * dirZ - vZ * dirY; - const float crossY = vZ * dirX - vX * dirZ; - const float crossZ = vX * dirY - vY * dirX; - const float crossNorm = std::sqrt(crossX * crossX + crossY * crossY + crossZ * crossZ); - const float dirNorm = std::sqrt(dirX * dirX + dirY * dirY + dirZ * dirZ); - if (dirNorm < 1e-6f) { - return std::numeric_limits::max(); - } - - return crossNorm / dirNorm; -} - -static SVzNLPointXYZ ComputeDirectionVector( - const SVzNLPointXYZ& startPoint, - const SVzNLPointXYZ& endPoint -) { - SVzNLPointXYZ dir = {}; - dir.x = endPoint.x - startPoint.x; - dir.y = endPoint.y - startPoint.y; - dir.z = endPoint.z - startPoint.z; - return dir; -} - -static float ComputeVectorNorm(const SVzNLPointXYZ& vec) { - return std::sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z); -} - -static float ComputeNormalizedAbsDot( - const SVzNLPointXYZ& lhs, - const SVzNLPointXYZ& rhs -) { - const float lhsNorm = ComputeVectorNorm(lhs); - const float rhsNorm = ComputeVectorNorm(rhs); - if (lhsNorm < 1e-6f || rhsNorm < 1e-6f) { - return 1.0f; - } - - const float dot = lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z; - return std::abs(dot) / (lhsNorm * rhsNorm); -} - -static bool FitCircleCenter2D( - const std::vector& slicePoints, - float* outMinorCenter, - float* outZCenter, - float* outRadius -) { - if (!outMinorCenter || !outZCenter || !outRadius || slicePoints.size() < 3) { - return false; - } - - Eigen::MatrixXf A(static_cast(slicePoints.size()), 3); - Eigen::VectorXf b(static_cast(slicePoints.size())); - for (int i = 0; i < static_cast(slicePoints.size()); ++i) { - const float u = slicePoints[i].minorCoord; - const float z = slicePoints[i].point.z; - A(i, 0) = 2.0f * u; - A(i, 1) = 2.0f * z; - A(i, 2) = 1.0f; - b(i) = u * u + z * z; - } - - const Eigen::Vector3f solution = A.colPivHouseholderQr().solve(b); - if (!solution.allFinite()) { - return false; - } - - const float centerU = solution(0); - const float centerZ = solution(1); - const float radiusSq = solution(2) + centerU * centerU + centerZ * centerZ; - if (radiusSq <= 1e-6f) { - return false; - } - - *outMinorCenter = centerU; - *outZCenter = centerZ; - *outRadius = std::sqrt(radiusSq); - return true; -} - -static bool CollectSlicePoints( - const std::vector& projectedPoints, - float targetMajorCoord, - float initialHalfWidth, - int minPointCount, - std::vector* outSlicePoints, - float* outMeanMajorCoord -) { - if (!outSlicePoints || !outMeanMajorCoord || projectedPoints.empty()) { - return false; - } - - float halfWidth = initialHalfWidth; - for (int expand = 0; expand < 4; ++expand) { - outSlicePoints->clear(); - float majorSum = 0.0f; - for (size_t i = 0; i < projectedPoints.size(); ++i) { - if (std::abs(projectedPoints[i].majorCoord - targetMajorCoord) > halfWidth) { - continue; - } - outSlicePoints->push_back(projectedPoints[i]); - majorSum += projectedPoints[i].majorCoord; - } - - if (static_cast(outSlicePoints->size()) >= minPointCount) { - *outMeanMajorCoord = majorSum / static_cast(outSlicePoints->size()); - return true; - } - - halfWidth *= 1.5f; - } - - outSlicePoints->clear(); - return false; -} - -static SClusterLineMetrics EvaluateClusterLinearity( - const SGrowthCluster& cluster, - const SVzNLPointXYZ* alignedPoints, - int totalPoints, - const SGrowthParams& growthParams -) { - SClusterLineMetrics metrics; - if (!alignedPoints || cluster.pointIndices.size() < 6) { - return metrics; - } - - std::vector clusterPoints; - clusterPoints.reserve(cluster.pointIndices.size()); - for (size_t i = 0; i < cluster.pointIndices.size(); ++i) { - const int linearIdx = cluster.pointIndices[i]; - if (linearIdx < 0 || linearIdx >= totalPoints) { - continue; - } - if (!IsValidPoint(alignedPoints[linearIdx])) { - continue; - } - clusterPoints.push_back(alignedPoints[linearIdx]); - } - - if (clusterPoints.size() < 6) { - return metrics; - } - - float centroidX = 0.0f; - float centroidY = 0.0f; - for (size_t i = 0; i < clusterPoints.size(); ++i) { - centroidX += clusterPoints[i].x; - centroidY += clusterPoints[i].y; - } - centroidX /= static_cast(clusterPoints.size()); - centroidY /= static_cast(clusterPoints.size()); - - float covXX = 0.0f; - float covXY = 0.0f; - float covYY = 0.0f; - for (size_t i = 0; i < clusterPoints.size(); ++i) { - const float dx = clusterPoints[i].x - centroidX; - const float dy = clusterPoints[i].y - centroidY; - covXX += dx * dx; - covXY += dx * dy; - covYY += dy * dy; - } - - const float theta = 0.5f * std::atan2(2.0f * covXY, covXX - covYY); - const float majorDirX = std::cos(theta); - const float majorDirY = std::sin(theta); - const float minorDirX = -majorDirY; - const float minorDirY = majorDirX; - - std::vector projectedPoints; - projectedPoints.reserve(clusterPoints.size()); - float minMajorCoord = std::numeric_limits::max(); - float maxMajorCoord = -std::numeric_limits::max(); - for (size_t i = 0; i < clusterPoints.size(); ++i) { - SProjectedClusterPoint projectedPoint; - projectedPoint.point = clusterPoints[i]; - projectedPoint.majorCoord = - (clusterPoints[i].x - centroidX) * majorDirX + - (clusterPoints[i].y - centroidY) * majorDirY; - projectedPoint.minorCoord = - (clusterPoints[i].x - centroidX) * minorDirX + - (clusterPoints[i].y - centroidY) * minorDirY; - projectedPoints.push_back(projectedPoint); - if (projectedPoint.majorCoord < minMajorCoord) minMajorCoord = projectedPoint.majorCoord; - if (projectedPoint.majorCoord > maxMajorCoord) maxMajorCoord = projectedPoint.majorCoord; - } - - metrics.majorSpan = maxMajorCoord - minMajorCoord; - const float radialTolerance = std::max(1.0f, std::max(growthParams.thresholdY, growthParams.thresholdZ)); - const float minLineSpan = - std::max(radialTolerance * 6.0f, std::max(growthParams.thresholdX, growthParams.thresholdY) * 3.0f); - if (metrics.majorSpan < minLineSpan) { - return metrics; - } - - const float sliceHalfWidth = - std::max(metrics.majorSpan * 0.08f, std::max(growthParams.thresholdX, growthParams.thresholdY)); - const int minSlicePointCount = std::max(6, cluster.pointCount / 10); - - std::vector slicePointsA; - std::vector slicePointsB; - float sliceMajorCoordA = 0.0f; - float sliceMajorCoordB = 0.0f; - const float targetMajorCoordA = minMajorCoord + metrics.majorSpan * 0.30f; - const float targetMajorCoordB = maxMajorCoord - metrics.majorSpan * 0.30f; - if (!CollectSlicePoints( - projectedPoints, targetMajorCoordA, sliceHalfWidth, minSlicePointCount, - &slicePointsA, &sliceMajorCoordA - )) { - return metrics; - } - if (!CollectSlicePoints( - projectedPoints, targetMajorCoordB, sliceHalfWidth, minSlicePointCount, - &slicePointsB, &sliceMajorCoordB - )) { - return metrics; - } - - float centerMinorA = 0.0f; - float centerZA = 0.0f; - float radiusA = 0.0f; - if (!FitCircleCenter2D(slicePointsA, ¢erMinorA, ¢erZA, &radiusA)) { - return metrics; - } - - float centerMinorB = 0.0f; - float centerZB = 0.0f; - float radiusB = 0.0f; - if (!FitCircleCenter2D(slicePointsB, ¢erMinorB, ¢erZB, &radiusB)) { - return metrics; - } - - metrics.axisPointA.x = centroidX + sliceMajorCoordA * majorDirX + centerMinorA * minorDirX; - metrics.axisPointA.y = centroidY + sliceMajorCoordA * majorDirY + centerMinorA * minorDirY; - metrics.axisPointA.z = centerZA; - metrics.axisPointB.x = centroidX + sliceMajorCoordB * majorDirX + centerMinorB * minorDirX; - metrics.axisPointB.y = centroidY + sliceMajorCoordB * majorDirY + centerMinorB * minorDirY; - metrics.axisPointB.z = centerZB; - - const float axisLength = std::sqrt( - (metrics.axisPointB.x - metrics.axisPointA.x) * (metrics.axisPointB.x - metrics.axisPointA.x) + - (metrics.axisPointB.y - metrics.axisPointA.y) * (metrics.axisPointB.y - metrics.axisPointA.y) + - (metrics.axisPointB.z - metrics.axisPointA.z) * (metrics.axisPointB.z - metrics.axisPointA.z) - ); - if (axisLength < 1e-6f) { - return metrics; - } - - const float axisDirZ = std::abs(metrics.axisPointB.z - metrics.axisPointA.z) / axisLength; - const float clampedAxisDeviationDeg = std::max(0.0f, std::min(growthParams.maxAxisDeviationFromXYDeg, 89.0f)); - const float maxAxisDirZ = std::sin(clampedAxisDeviationDeg * 3.1415926f / 180.0f); - if (axisDirZ > maxAxisDirZ) { - return metrics; - } - - float distanceSum = 0.0f; - float distanceSqSum = 0.0f; - std::vector distances; - distances.reserve(clusterPoints.size()); - for (size_t i = 0; i < clusterPoints.size(); ++i) { - const float distance = ComputePointToLineDistance( - clusterPoints[i], - metrics.axisPointA, - metrics.axisPointB - ); - distances.push_back(distance); - distanceSum += distance; - distanceSqSum += distance * distance; - } - - metrics.meanDistance = distanceSum / static_cast(distances.size()); - const float meanSqDistance = distanceSqSum / static_cast(distances.size()); - metrics.distanceStdDev = std::sqrt(std::max(0.0f, meanSqDistance - metrics.meanDistance * metrics.meanDistance)); - - const float radiusConsistencyTolerance = std::max(radialTolerance, metrics.meanDistance * 0.12f); - int inlierCount = 0; - float maxDeviation = 0.0f; - for (size_t i = 0; i < distances.size(); ++i) { - const float deviation = std::abs(distances[i] - metrics.meanDistance); - if (deviation > maxDeviation) { - maxDeviation = deviation; - } - if (deviation <= radiusConsistencyTolerance) { - inlierCount++; - } - } - - metrics.maxDistanceDeviation = maxDeviation; - metrics.inlierRatio = static_cast(inlierCount) / static_cast(distances.size()); - metrics.isValid = - std::abs(radiusA - radiusB) <= radiusConsistencyTolerance && - metrics.inlierRatio >= 0.75f && - metrics.distanceStdDev <= radiusConsistencyTolerance && - metrics.maxDistanceDeviation <= radiusConsistencyTolerance * 2.0f; - - return metrics; -} - -BAR_INTERSECTION_API int DetectBarIntersections( +static bool IsValidPoint(const SVzNLPointXYZ& pt) { + const float eps = 1e-6f; + return !(std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps); +} + +static SVzNLPointXYZ InverseTransformPoint( + const SVzNLPointXYZ& pt, + const float invRotation[9], + const SVzNL3DPointF& planeCentroid +) { + SVzNLPointXYZ transformed = {}; + transformed.x = invRotation[0] * pt.x + invRotation[1] * pt.y + invRotation[2] * pt.z + planeCentroid.x; + transformed.y = invRotation[3] * pt.x + invRotation[4] * pt.y + invRotation[5] * pt.z + planeCentroid.y; + transformed.z = invRotation[6] * pt.x + invRotation[7] * pt.y + invRotation[8] * pt.z + planeCentroid.z; + return transformed; +} + +static void ClearPoint(SVzNLPointXYZ* pt) { + if (!pt) { + return; + } + + pt->x = 0.0f; + pt->y = 0.0f; + pt->z = 0.0f; +} + +static void ClearShortContinuousSegment( + SVzNLPointXYZ* rowPoints, + int startCol, + int segmentLength, + int minContinuousValidPointCount +) { + if (!rowPoints || startCol < 0 || segmentLength <= 0) { + return; + } + + if (segmentLength >= minContinuousValidPointCount) { + return; + } + + for (int offset = 0; offset < segmentLength; ++offset) { + ClearPoint(&rowPoints[startCol + offset]); + } +} + +static void FilterLaserLineNoiseInPlace( + SVzNLPointXYZ* alignedPoints, + int rows, + int cols, + int minContinuousValidPointCount, + float maxContinuousPointZTolerance +) { + if (!alignedPoints || rows <= 0 || cols <= 0) { + return; + } + + if (minContinuousValidPointCount <= 1 || maxContinuousPointZTolerance < 0.0f) { + return; + } + + for (int row = 0; row < rows; ++row) { + SVzNLPointXYZ* rowPoints = alignedPoints + row * cols; + int segmentStartCol = -1; + int segmentLength = 0; + float prevZ = 0.0f; + + for (int col = 0; col < cols; ++col) { + const SVzNLPointXYZ& pt = rowPoints[col]; + if (!IsValidPoint(pt)) { + ClearShortContinuousSegment( + rowPoints, + segmentStartCol, + segmentLength, + minContinuousValidPointCount + ); + segmentStartCol = -1; + segmentLength = 0; + prevZ = 0.0f; + continue; + } + + if (segmentLength <= 0) { + segmentStartCol = col; + segmentLength = 1; + prevZ = pt.z; + continue; + } + + const float zDiff = std::abs(pt.z - prevZ); + if (zDiff <= maxContinuousPointZTolerance) { + ++segmentLength; + prevZ = pt.z; + continue; + } + + ClearShortContinuousSegment( + rowPoints, + segmentStartCol, + segmentLength, + minContinuousValidPointCount + ); + + segmentStartCol = col; + segmentLength = 1; + prevZ = pt.z; + } + + ClearShortContinuousSegment( + rowPoints, + segmentStartCol, + segmentLength, + minContinuousValidPointCount + ); + } +} + +struct SProjectedClusterPoint { + SVzNLPointXYZ point; + float majorCoord; + float minorCoord; +}; + +struct SClusterLineMetrics { + bool isValid; + SVzNLPointXYZ axisPointA; + SVzNLPointXYZ axisPointB; + float meanDistance; + float distanceStdDev; + float maxDistanceDeviation; + float inlierRatio; + float majorSpan; + + SClusterLineMetrics() + : isValid(false) + , axisPointA() + , axisPointB() + , meanDistance(0.0f) + , distanceStdDev(std::numeric_limits::max()) + , maxDistanceDeviation(std::numeric_limits::max()) + , inlierRatio(0.0f) + , majorSpan(0.0f) + {} +}; + +static float ComputePointToLineDistance( + const SVzNLPointXYZ& pt, + const SVzNLPointXYZ& lineStart, + const SVzNLPointXYZ& lineEnd +) { + const float dirX = lineEnd.x - lineStart.x; + const float dirY = lineEnd.y - lineStart.y; + const float dirZ = lineEnd.z - lineStart.z; + + const float vX = pt.x - lineStart.x; + const float vY = pt.y - lineStart.y; + const float vZ = pt.z - lineStart.z; + + const float crossX = vY * dirZ - vZ * dirY; + const float crossY = vZ * dirX - vX * dirZ; + const float crossZ = vX * dirY - vY * dirX; + const float crossNorm = std::sqrt(crossX * crossX + crossY * crossY + crossZ * crossZ); + const float dirNorm = std::sqrt(dirX * dirX + dirY * dirY + dirZ * dirZ); + if (dirNorm < 1e-6f) { + return std::numeric_limits::max(); + } + + return crossNorm / dirNorm; +} + +static SVzNLPointXYZ ComputeDirectionVector( + const SVzNLPointXYZ& startPoint, + const SVzNLPointXYZ& endPoint +) { + SVzNLPointXYZ dir = {}; + dir.x = endPoint.x - startPoint.x; + dir.y = endPoint.y - startPoint.y; + dir.z = endPoint.z - startPoint.z; + return dir; +} + +static float ComputeVectorNorm(const SVzNLPointXYZ& vec) { + return std::sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z); +} + +static float ComputeNormalizedAbsDot( + const SVzNLPointXYZ& lhs, + const SVzNLPointXYZ& rhs +) { + const float lhsNorm = ComputeVectorNorm(lhs); + const float rhsNorm = ComputeVectorNorm(rhs); + if (lhsNorm < 1e-6f || rhsNorm < 1e-6f) { + return 1.0f; + } + + const float dot = lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z; + return std::abs(dot) / (lhsNorm * rhsNorm); +} + +static bool FitCircleCenter2D( + const std::vector& slicePoints, + float* outMinorCenter, + float* outZCenter, + float* outRadius +) { + if (!outMinorCenter || !outZCenter || !outRadius || slicePoints.size() < 3) { + return false; + } + + Eigen::MatrixXf A(static_cast(slicePoints.size()), 3); + Eigen::VectorXf b(static_cast(slicePoints.size())); + for (int i = 0; i < static_cast(slicePoints.size()); ++i) { + const float u = slicePoints[i].minorCoord; + const float z = slicePoints[i].point.z; + A(i, 0) = 2.0f * u; + A(i, 1) = 2.0f * z; + A(i, 2) = 1.0f; + b(i) = u * u + z * z; + } + + const Eigen::Vector3f solution = A.colPivHouseholderQr().solve(b); + if (!solution.allFinite()) { + return false; + } + + const float centerU = solution(0); + const float centerZ = solution(1); + const float radiusSq = solution(2) + centerU * centerU + centerZ * centerZ; + if (radiusSq <= 1e-6f) { + return false; + } + + *outMinorCenter = centerU; + *outZCenter = centerZ; + *outRadius = std::sqrt(radiusSq); + return true; +} + +static bool CollectSlicePoints( + const std::vector& projectedPoints, + float targetMajorCoord, + float initialHalfWidth, + int minPointCount, + std::vector* outSlicePoints, + float* outMeanMajorCoord +) { + if (!outSlicePoints || !outMeanMajorCoord || projectedPoints.empty()) { + return false; + } + + float halfWidth = initialHalfWidth; + for (int expand = 0; expand < 4; ++expand) { + outSlicePoints->clear(); + float majorSum = 0.0f; + for (size_t i = 0; i < projectedPoints.size(); ++i) { + if (std::abs(projectedPoints[i].majorCoord - targetMajorCoord) > halfWidth) { + continue; + } + outSlicePoints->push_back(projectedPoints[i]); + majorSum += projectedPoints[i].majorCoord; + } + + if (static_cast(outSlicePoints->size()) >= minPointCount) { + *outMeanMajorCoord = majorSum / static_cast(outSlicePoints->size()); + return true; + } + + halfWidth *= 1.5f; + } + + outSlicePoints->clear(); + return false; +} + +static SClusterLineMetrics EvaluateClusterLinearity( + const SGrowthCluster& cluster, + const SVzNLPointXYZ* alignedPoints, + int totalPoints, + const SGrowthParams& growthParams +) { + SClusterLineMetrics metrics; + if (!alignedPoints || cluster.pointIndices.size() < 6) { + return metrics; + } + + std::vector clusterPoints; + clusterPoints.reserve(cluster.pointIndices.size()); + for (size_t i = 0; i < cluster.pointIndices.size(); ++i) { + const int linearIdx = cluster.pointIndices[i]; + if (linearIdx < 0 || linearIdx >= totalPoints) { + continue; + } + if (!IsValidPoint(alignedPoints[linearIdx])) { + continue; + } + clusterPoints.push_back(alignedPoints[linearIdx]); + } + + if (clusterPoints.size() < 6) { + return metrics; + } + + float centroidX = 0.0f; + float centroidY = 0.0f; + for (size_t i = 0; i < clusterPoints.size(); ++i) { + centroidX += clusterPoints[i].x; + centroidY += clusterPoints[i].y; + } + centroidX /= static_cast(clusterPoints.size()); + centroidY /= static_cast(clusterPoints.size()); + + float covXX = 0.0f; + float covXY = 0.0f; + float covYY = 0.0f; + for (size_t i = 0; i < clusterPoints.size(); ++i) { + const float dx = clusterPoints[i].x - centroidX; + const float dy = clusterPoints[i].y - centroidY; + covXX += dx * dx; + covXY += dx * dy; + covYY += dy * dy; + } + + const float theta = 0.5f * std::atan2(2.0f * covXY, covXX - covYY); + const float majorDirX = std::cos(theta); + const float majorDirY = std::sin(theta); + const float minorDirX = -majorDirY; + const float minorDirY = majorDirX; + + std::vector projectedPoints; + projectedPoints.reserve(clusterPoints.size()); + float minMajorCoord = std::numeric_limits::max(); + float maxMajorCoord = -std::numeric_limits::max(); + for (size_t i = 0; i < clusterPoints.size(); ++i) { + SProjectedClusterPoint projectedPoint; + projectedPoint.point = clusterPoints[i]; + projectedPoint.majorCoord = + (clusterPoints[i].x - centroidX) * majorDirX + + (clusterPoints[i].y - centroidY) * majorDirY; + projectedPoint.minorCoord = + (clusterPoints[i].x - centroidX) * minorDirX + + (clusterPoints[i].y - centroidY) * minorDirY; + projectedPoints.push_back(projectedPoint); + if (projectedPoint.majorCoord < minMajorCoord) minMajorCoord = projectedPoint.majorCoord; + if (projectedPoint.majorCoord > maxMajorCoord) maxMajorCoord = projectedPoint.majorCoord; + } + + metrics.majorSpan = maxMajorCoord - minMajorCoord; + const float radialTolerance = std::max(1.0f, std::max(growthParams.thresholdY, growthParams.thresholdZ)); + const float minLineSpan = + std::max(radialTolerance * 6.0f, std::max(growthParams.thresholdX, growthParams.thresholdY) * 3.0f); + if (metrics.majorSpan < minLineSpan) { + return metrics; + } + + const float sliceHalfWidth = + std::max(metrics.majorSpan * 0.08f, std::max(growthParams.thresholdX, growthParams.thresholdY)); + const int minSlicePointCount = std::max(6, cluster.pointCount / 10); + + std::vector slicePointsA; + std::vector slicePointsB; + float sliceMajorCoordA = 0.0f; + float sliceMajorCoordB = 0.0f; + const float targetMajorCoordA = minMajorCoord + metrics.majorSpan * 0.30f; + const float targetMajorCoordB = maxMajorCoord - metrics.majorSpan * 0.30f; + if (!CollectSlicePoints( + projectedPoints, targetMajorCoordA, sliceHalfWidth, minSlicePointCount, + &slicePointsA, &sliceMajorCoordA + )) { + return metrics; + } + if (!CollectSlicePoints( + projectedPoints, targetMajorCoordB, sliceHalfWidth, minSlicePointCount, + &slicePointsB, &sliceMajorCoordB + )) { + return metrics; + } + + float centerMinorA = 0.0f; + float centerZA = 0.0f; + float radiusA = 0.0f; + if (!FitCircleCenter2D(slicePointsA, ¢erMinorA, ¢erZA, &radiusA)) { + return metrics; + } + + float centerMinorB = 0.0f; + float centerZB = 0.0f; + float radiusB = 0.0f; + if (!FitCircleCenter2D(slicePointsB, ¢erMinorB, ¢erZB, &radiusB)) { + return metrics; + } + + metrics.axisPointA.x = centroidX + sliceMajorCoordA * majorDirX + centerMinorA * minorDirX; + metrics.axisPointA.y = centroidY + sliceMajorCoordA * majorDirY + centerMinorA * minorDirY; + metrics.axisPointA.z = centerZA; + metrics.axisPointB.x = centroidX + sliceMajorCoordB * majorDirX + centerMinorB * minorDirX; + metrics.axisPointB.y = centroidY + sliceMajorCoordB * majorDirY + centerMinorB * minorDirY; + metrics.axisPointB.z = centerZB; + + const float axisLength = std::sqrt( + (metrics.axisPointB.x - metrics.axisPointA.x) * (metrics.axisPointB.x - metrics.axisPointA.x) + + (metrics.axisPointB.y - metrics.axisPointA.y) * (metrics.axisPointB.y - metrics.axisPointA.y) + + (metrics.axisPointB.z - metrics.axisPointA.z) * (metrics.axisPointB.z - metrics.axisPointA.z) + ); + if (axisLength < 1e-6f) { + return metrics; + } + + const float axisDirZ = std::abs(metrics.axisPointB.z - metrics.axisPointA.z) / axisLength; + const float clampedAxisDeviationDeg = std::max(0.0f, std::min(growthParams.maxAxisDeviationFromXYDeg, 89.0f)); + const float maxAxisDirZ = std::sin(clampedAxisDeviationDeg * 3.1415926f / 180.0f); + if (axisDirZ > maxAxisDirZ) { + return metrics; + } + + float distanceSum = 0.0f; + float distanceSqSum = 0.0f; + std::vector distances; + distances.reserve(clusterPoints.size()); + for (size_t i = 0; i < clusterPoints.size(); ++i) { + const float distance = ComputePointToLineDistance( + clusterPoints[i], + metrics.axisPointA, + metrics.axisPointB + ); + distances.push_back(distance); + distanceSum += distance; + distanceSqSum += distance * distance; + } + + metrics.meanDistance = distanceSum / static_cast(distances.size()); + const float meanSqDistance = distanceSqSum / static_cast(distances.size()); + metrics.distanceStdDev = std::sqrt(std::max(0.0f, meanSqDistance - metrics.meanDistance * metrics.meanDistance)); + + const float radiusConsistencyTolerance = std::max(radialTolerance, metrics.meanDistance * 0.12f); + int inlierCount = 0; + float maxDeviation = 0.0f; + for (size_t i = 0; i < distances.size(); ++i) { + const float deviation = std::abs(distances[i] - metrics.meanDistance); + if (deviation > maxDeviation) { + maxDeviation = deviation; + } + if (deviation <= radiusConsistencyTolerance) { + inlierCount++; + } + } + + metrics.maxDistanceDeviation = maxDeviation; + metrics.inlierRatio = static_cast(inlierCount) / static_cast(distances.size()); + metrics.isValid = + std::abs(radiusA - radiusB) <= radiusConsistencyTolerance && + metrics.inlierRatio >= 0.75f && + metrics.distanceStdDev <= radiusConsistencyTolerance && + metrics.maxDistanceDeviation <= radiusConsistencyTolerance * 2.0f; + + return metrics; +} + +BAR_INTERSECTION_API int DetectBarIntersections( const SVzNLPointXYZ* points, int rows, int cols, @@ -556,23 +556,23 @@ BAR_INTERSECTION_API int DetectBarIntersections( } } - bar_intersection::FilterPlanePoints( - alignedPoints, totalPoints, planeZ, planeParams.heightThreshold - ); - - // ============================================ - // Step 2b: 按激光线过滤短连续段噪点 - // ============================================ - FilterLaserLineNoiseInPlace( - alignedPoints, - rows, - cols, - growthParams.minContinuousValidPointCount, - growthParams.maxContinuousPointZTolerance - ); - - // Debug callback: plane filtered - if (debugCallbacks && debugCallbacks->onPlaneFiltered) { + bar_intersection::FilterPlanePoints( + alignedPoints, totalPoints, planeZ, planeParams.heightThreshold + ); + + // ============================================ + // Step 2b: 按激光线过滤短连续段噪点 + // ============================================ + FilterLaserLineNoiseInPlace( + alignedPoints, + rows, + cols, + growthParams.minContinuousValidPointCount, + growthParams.maxContinuousPointZTolerance + ); + + // Debug callback: plane filtered + if (debugCallbacks && debugCallbacks->onPlaneFiltered) { // 收集非零点用于回调显示 std::vector filteredPtsForDebug; for (int i = 0; i < totalPoints; i++) { @@ -609,10 +609,10 @@ BAR_INTERSECTION_API int DetectBarIntersections( // 范围：minZ ~ (planeZ + minZ) / 2 // 在该范围内选点数最多的簇 // ============================================ - int bestIdx = -1; - std::vector clusterLineMetrics(clusters.size()); - std::vector validClusterIndices; - if (!clusters.empty()) { + int bestIdx = -1; + std::vector clusterLineMetrics(clusters.size()); + std::vector validClusterIndices; + if (!clusters.empty()) { // 找所有簇中最小的 centroid.z（对齐空间） float minZ = std::numeric_limits::max(); for (size_t i = 0; i < clusters.size(); i++) { @@ -625,154 +625,154 @@ BAR_INTERSECTION_API int DetectBarIntersections( float zUpperBound = (planeZ + minZ) * 0.5f; // 在范围内找点数最多的簇 - int maxPoints = 0; - float bestDistanceStdDev = std::numeric_limits::max(); - float bestInlierRatio = 0.0f; - for (int i = 0; i < (int)clusters.size(); i++) { - if (clusters[i].centroid.z > zUpperBound) continue; - - const SClusterLineMetrics lineMetrics = EvaluateClusterLinearity( - clusters[i], alignedPoints, totalPoints, growthParams - ); - clusterLineMetrics[i] = lineMetrics; - if (!lineMetrics.isValid) continue; - validClusterIndices.push_back(i); - - if (clusters[i].pointCount > maxPoints || - (clusters[i].pointCount == maxPoints && lineMetrics.distanceStdDev < bestDistanceStdDev) || - (clusters[i].pointCount == maxPoints && - std::abs(lineMetrics.distanceStdDev - bestDistanceStdDev) < 1e-6f && - lineMetrics.inlierRatio > bestInlierRatio)) { - maxPoints = clusters[i].pointCount; - bestDistanceStdDev = lineMetrics.distanceStdDev; - bestInlierRatio = lineMetrics.inlierRatio; - bestIdx = i; - } - } + int maxPoints = 0; + float bestDistanceStdDev = std::numeric_limits::max(); + float bestInlierRatio = 0.0f; + for (int i = 0; i < (int)clusters.size(); i++) { + if (clusters[i].centroid.z > zUpperBound) continue; + + const SClusterLineMetrics lineMetrics = EvaluateClusterLinearity( + clusters[i], alignedPoints, totalPoints, growthParams + ); + clusterLineMetrics[i] = lineMetrics; + if (!lineMetrics.isValid) continue; + validClusterIndices.push_back(i); + + if (clusters[i].pointCount > maxPoints || + (clusters[i].pointCount == maxPoints && lineMetrics.distanceStdDev < bestDistanceStdDev) || + (clusters[i].pointCount == maxPoints && + std::abs(lineMetrics.distanceStdDev - bestDistanceStdDev) < 1e-6f && + lineMetrics.inlierRatio > bestInlierRatio)) { + maxPoints = clusters[i].pointCount; + bestDistanceStdDev = lineMetrics.distanceStdDev; + bestInlierRatio = lineMetrics.inlierRatio; + bestIdx = i; + } + } } - // ============================================ - // Transform cluster centroids back to original coordinate system - // ============================================ - float invRotation[9]; - for (int r = 0; r < 3; r++) { + // ============================================ + // Transform cluster centroids back to original coordinate system + // ============================================ + float invRotation[9]; + for (int r = 0; r < 3; r++) { for (int c = 0; c < 3; c++) { - invRotation[r * 3 + c] = rotationMatrix[c * 3 + r]; - } - } - - // ============================================ - // Step 5: from valid clusters, find the one with minimum z and then - // find clusters whose centroid-connection line is perpendicular to its axis - // ============================================ - int step5BaseClusterIndex = -1; - std::vector step5PerpendicularClusterIndices; - if (!validClusterIndices.empty()) { - float minValidClusterZ = std::numeric_limits::max(); - for (size_t i = 0; i < validClusterIndices.size(); ++i) { - const int clusterIdx = validClusterIndices[i]; - if (clusters[clusterIdx].centroid.z < minValidClusterZ) { - minValidClusterZ = clusters[clusterIdx].centroid.z; - step5BaseClusterIndex = clusterIdx; - } - } - - if (step5BaseClusterIndex >= 0) { - const SClusterLineMetrics& baseMetrics = clusterLineMetrics[step5BaseClusterIndex]; - SVzNLPointXYZ baseAxisDirection = - ComputeDirectionVector(baseMetrics.axisPointA, baseMetrics.axisPointB); - baseAxisDirection.z = 0.0f; - const float perpendicularTolerance = - std::sin(std::max(0.0f, std::min(growthParams.maxPerpendicularDeviationDeg, 89.0f)) * - 3.1415926f / 180.0f); - - for (size_t i = 0; i < validClusterIndices.size(); ++i) { - const int clusterIdx = validClusterIndices[i]; - if (clusterIdx == step5BaseClusterIndex) { - continue; - } - - SVzNLPointXYZ centroidConnection = {}; - centroidConnection.x = clusters[clusterIdx].centroid.x - clusters[step5BaseClusterIndex].centroid.x; - centroidConnection.y = clusters[clusterIdx].centroid.y - clusters[step5BaseClusterIndex].centroid.y; - centroidConnection.z = 0.0f; - - const float absDot = ComputeNormalizedAbsDot(baseAxisDirection, centroidConnection); - if (absDot <= perpendicularTolerance) { - step5PerpendicularClusterIndices.push_back(clusterIdx); - } - } - } - } - - std::vector step5FilteredAlignedPoints; - std::vector step5BaseClusterAlignedPoints; - SVzNLPointXYZ step5BaseClusterAlignedCentroid = {}; - std::vector > step5PerpendicularClusterAlignedPoints; - std::vector step5PerpendicularClusterAlignedCentroids; - - step5FilteredAlignedPoints.reserve(validCount); - for (int i = 0; i < totalPoints; ++i) { - if (IsValidPoint(alignedPoints[i])) { - step5FilteredAlignedPoints.push_back(alignedPoints[i]); - } - } - - if (step5BaseClusterIndex >= 0) { - const SGrowthCluster& baseCluster = clusters[step5BaseClusterIndex]; - step5BaseClusterAlignedCentroid.x = baseCluster.centroid.x; - step5BaseClusterAlignedCentroid.y = baseCluster.centroid.y; - step5BaseClusterAlignedCentroid.z = baseCluster.centroid.z; - step5BaseClusterAlignedPoints.reserve(baseCluster.pointCount); - for (size_t j = 0; j < baseCluster.pointIndices.size(); ++j) { - const int linearIdx = baseCluster.pointIndices[j]; - if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { - step5BaseClusterAlignedPoints.push_back(alignedPoints[linearIdx]); - } - } - - step5PerpendicularClusterAlignedPoints.reserve(step5PerpendicularClusterIndices.size()); - step5PerpendicularClusterAlignedCentroids.reserve(step5PerpendicularClusterIndices.size()); - for (size_t i = 0; i < step5PerpendicularClusterIndices.size(); ++i) { - const int clusterIdx = step5PerpendicularClusterIndices[i]; - const SGrowthCluster& cluster = clusters[clusterIdx]; - std::vector clusterAlignedPoints; - clusterAlignedPoints.reserve(cluster.pointCount); - for (size_t j = 0; j < cluster.pointIndices.size(); ++j) { - const int linearIdx = cluster.pointIndices[j]; - if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { - clusterAlignedPoints.push_back(alignedPoints[linearIdx]); - } - } - step5PerpendicularClusterAlignedPoints.push_back(std::move(clusterAlignedPoints)); - SVzNLPointXYZ clusterAlignedCentroid; - clusterAlignedCentroid.x = cluster.centroid.x; - clusterAlignedCentroid.y = cluster.centroid.y; - clusterAlignedCentroid.z = cluster.centroid.z; - step5PerpendicularClusterAlignedCentroids.push_back(clusterAlignedCentroid); - } - } - - // 提取最优簇的过滤后点云，并逆变换回原始坐标系，便于在整云上高亮显示 - std::vector bestClusterFilteredPts; - if (bestIdx >= 0) { - const SGrowthCluster& bestCluster = clusters[bestIdx]; - bestClusterFilteredPts.reserve(bestCluster.pointCount); - for (int j = 0; j < (int)bestCluster.pointIndices.size(); j++) { - int linearIdx = bestCluster.pointIndices[j]; - if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { - bestClusterFilteredPts.push_back( - InverseTransformPoint(alignedPoints[linearIdx], invRotation, planeCentroid) - ); - } - } - } - - for (auto& cluster : clusters) { - // Transform centroid - float x = cluster.centroid.x; - float y = cluster.centroid.y; - float z = cluster.centroid.z; + invRotation[r * 3 + c] = rotationMatrix[c * 3 + r]; + } + } + + // ============================================ + // Step 5: from valid clusters, find the one with minimum z and then + // find clusters whose centroid-connection line is perpendicular to its axis + // ============================================ + int step5BaseClusterIndex = -1; + std::vector step5PerpendicularClusterIndices; + if (!validClusterIndices.empty()) { + float minValidClusterZ = std::numeric_limits::max(); + for (size_t i = 0; i < validClusterIndices.size(); ++i) { + const int clusterIdx = validClusterIndices[i]; + if (clusters[clusterIdx].centroid.z < minValidClusterZ) { + minValidClusterZ = clusters[clusterIdx].centroid.z; + step5BaseClusterIndex = clusterIdx; + } + } + + if (step5BaseClusterIndex >= 0) { + const SClusterLineMetrics& baseMetrics = clusterLineMetrics[step5BaseClusterIndex]; + SVzNLPointXYZ baseAxisDirection = + ComputeDirectionVector(baseMetrics.axisPointA, baseMetrics.axisPointB); + baseAxisDirection.z = 0.0f; + const float perpendicularTolerance = + std::sin(std::max(0.0f, std::min(growthParams.maxPerpendicularDeviationDeg, 89.0f)) * + 3.1415926f / 180.0f); + + for (size_t i = 0; i < validClusterIndices.size(); ++i) { + const int clusterIdx = validClusterIndices[i]; + if (clusterIdx == step5BaseClusterIndex) { + continue; + } + + SVzNLPointXYZ centroidConnection = {}; + centroidConnection.x = clusters[clusterIdx].centroid.x - clusters[step5BaseClusterIndex].centroid.x; + centroidConnection.y = clusters[clusterIdx].centroid.y - clusters[step5BaseClusterIndex].centroid.y; + centroidConnection.z = 0.0f; + + const float absDot = ComputeNormalizedAbsDot(baseAxisDirection, centroidConnection); + if (absDot <= perpendicularTolerance) { + step5PerpendicularClusterIndices.push_back(clusterIdx); + } + } + } + } + + std::vector step5FilteredAlignedPoints; + std::vector step5BaseClusterAlignedPoints; + SVzNLPointXYZ step5BaseClusterAlignedCentroid = {}; + std::vector > step5PerpendicularClusterAlignedPoints; + std::vector step5PerpendicularClusterAlignedCentroids; + + step5FilteredAlignedPoints.reserve(validCount); + for (int i = 0; i < totalPoints; ++i) { + if (IsValidPoint(alignedPoints[i])) { + step5FilteredAlignedPoints.push_back(alignedPoints[i]); + } + } + + if (step5BaseClusterIndex >= 0) { + const SGrowthCluster& baseCluster = clusters[step5BaseClusterIndex]; + step5BaseClusterAlignedCentroid.x = baseCluster.centroid.x; + step5BaseClusterAlignedCentroid.y = baseCluster.centroid.y; + step5BaseClusterAlignedCentroid.z = baseCluster.centroid.z; + step5BaseClusterAlignedPoints.reserve(baseCluster.pointCount); + for (size_t j = 0; j < baseCluster.pointIndices.size(); ++j) { + const int linearIdx = baseCluster.pointIndices[j]; + if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { + step5BaseClusterAlignedPoints.push_back(alignedPoints[linearIdx]); + } + } + + step5PerpendicularClusterAlignedPoints.reserve(step5PerpendicularClusterIndices.size()); + step5PerpendicularClusterAlignedCentroids.reserve(step5PerpendicularClusterIndices.size()); + for (size_t i = 0; i < step5PerpendicularClusterIndices.size(); ++i) { + const int clusterIdx = step5PerpendicularClusterIndices[i]; + const SGrowthCluster& cluster = clusters[clusterIdx]; + std::vector clusterAlignedPoints; + clusterAlignedPoints.reserve(cluster.pointCount); + for (size_t j = 0; j < cluster.pointIndices.size(); ++j) { + const int linearIdx = cluster.pointIndices[j]; + if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { + clusterAlignedPoints.push_back(alignedPoints[linearIdx]); + } + } + step5PerpendicularClusterAlignedPoints.push_back(std::move(clusterAlignedPoints)); + SVzNLPointXYZ clusterAlignedCentroid; + clusterAlignedCentroid.x = cluster.centroid.x; + clusterAlignedCentroid.y = cluster.centroid.y; + clusterAlignedCentroid.z = cluster.centroid.z; + step5PerpendicularClusterAlignedCentroids.push_back(clusterAlignedCentroid); + } + } + + // 提取最优簇的过滤后点云，并逆变换回原始坐标系，便于在整云上高亮显示 + std::vector bestClusterFilteredPts; + if (bestIdx >= 0) { + const SGrowthCluster& bestCluster = clusters[bestIdx]; + bestClusterFilteredPts.reserve(bestCluster.pointCount); + for (int j = 0; j < (int)bestCluster.pointIndices.size(); j++) { + int linearIdx = bestCluster.pointIndices[j]; + if (linearIdx >= 0 && linearIdx < totalPoints && IsValidPoint(alignedPoints[linearIdx])) { + bestClusterFilteredPts.push_back( + InverseTransformPoint(alignedPoints[linearIdx], invRotation, planeCentroid) + ); + } + } + } + + for (auto& cluster : clusters) { + // Transform centroid + float x = cluster.centroid.x; + float y = cluster.centroid.y; + float z = cluster.centroid.z; cluster.centroid.x = invRotation[0]*x + invRotation[1]*y + invRotation[2]*z + planeCentroid.x; cluster.centroid.y = invRotation[3]*x + invRotation[4]*y + invRotation[5]*z + planeCentroid.y; cluster.centroid.z = invRotation[6]*x + invRotation[7]*y + invRotation[8]*z + planeCentroid.z; @@ -800,18 +800,18 @@ BAR_INTERSECTION_API int DetectBarIntersections( // ============================================ // Fill result // ============================================ - result->planeNormal = planeNormal; - result->planeD = planeD; - result->bestClusterIndex = bestIdx; - result->validClusterIndices = std::move(validClusterIndices); - result->step5BaseClusterIndex = step5BaseClusterIndex; - result->step5PerpendicularClusterIndices = std::move(step5PerpendicularClusterIndices); - result->step5FilteredAlignedPoints = std::move(step5FilteredAlignedPoints); - result->step5BaseClusterAlignedPoints = std::move(step5BaseClusterAlignedPoints); - result->step5BaseClusterAlignedCentroid = step5BaseClusterAlignedCentroid; - result->step5PerpendicularClusterAlignedPoints = std::move(step5PerpendicularClusterAlignedPoints); - result->step5PerpendicularClusterAlignedCentroids = std::move(step5PerpendicularClusterAlignedCentroids); - result->bestClusterPoints = std::move(bestClusterFilteredPts); + result->planeNormal = planeNormal; + result->planeD = planeD; + result->bestClusterIndex = bestIdx; + result->validClusterIndices = std::move(validClusterIndices); + result->step5BaseClusterIndex = step5BaseClusterIndex; + result->step5PerpendicularClusterIndices = std::move(step5PerpendicularClusterIndices); + result->step5FilteredAlignedPoints = std::move(step5FilteredAlignedPoints); + result->step5BaseClusterAlignedPoints = std::move(step5BaseClusterAlignedPoints); + result->step5BaseClusterAlignedCentroid = step5BaseClusterAlignedCentroid; + result->step5PerpendicularClusterAlignedPoints = std::move(step5PerpendicularClusterAlignedPoints); + result->step5PerpendicularClusterAlignedCentroids = std::move(step5PerpendicularClusterAlignedCentroids); + result->bestClusterPoints = std::move(bestClusterFilteredPts); result->clusterCount = (int)clusters.size(); if (result->clusterCount > 0) { @@ -838,10 +838,10 @@ BAR_INTERSECTION_API void PrintDetectionResults( bool verbose ) { std::cout << "=== Bar Intersection Detection Results ===" << std::endl; - std::cout << "Plane Normal: (" << result.planeNormal.x << ", " - << result.planeNormal.y << ", " << result.planeNormal.z << ")" << std::endl; - std::cout << "Clusters detected: " << result.clusterCount << std::endl; - std::cout << "Valid bar-like clusters: " << result.validClusterIndices.size() << std::endl; + std::cout << "Plane Normal: (" << result.planeNormal.x << ", " + << result.planeNormal.y << ", " << result.planeNormal.z << ")" << std::endl; + std::cout << "Clusters detected: " << result.clusterCount << std::endl; + std::cout << "Valid bar-like clusters: " << result.validClusterIndices.size() << std::endl; if (verbose) { std::cout << "\n--- Clusters ---" << std::endl; @@ -855,43 +855,43 @@ BAR_INTERSECTION_API void PrintDetectionResults( << std::endl; } - std::cout << "\n--- Best Cluster ---" << std::endl; - if (result.bestClusterIndex >= 0 && result.bestClusterIndex < result.clusterCount) { - const SGrowthCluster& best = result.clusters[result.bestClusterIndex]; - std::cout << " Best: Cluster #" << result.bestClusterIndex - << " pts=" << best.pointCount - << " centroid=(" << best.centroid.x << "," << best.centroid.y << "," << best.centroid.z << ")" - << std::endl; - } else { - std::cout << " No valid best cluster found." << std::endl; - } - - if (!result.validClusterIndices.empty()) { - std::cout << "\n--- Valid Cluster Indices ---" << std::endl; - for (size_t i = 0; i < result.validClusterIndices.size(); ++i) { - std::cout << " #" << result.validClusterIndices[i] << std::endl; - } - } - - std::cout << "\n--- Step 5 ---" << std::endl; - if (result.step5BaseClusterIndex >= 0 && result.step5BaseClusterIndex < result.clusterCount) { - const SGrowthCluster& base = result.clusters[result.step5BaseClusterIndex]; - std::cout << " Base: Cluster #" << result.step5BaseClusterIndex - << " pts=" << base.pointCount - << " centroid=(" << base.centroid.x << "," << base.centroid.y << "," << base.centroid.z << ")" - << std::endl; - } else { - std::cout << " No Step 5 base cluster found." << std::endl; - } - - if (!result.step5PerpendicularClusterIndices.empty()) { - std::cout << " Perpendicular clusters:" << std::endl; - for (size_t i = 0; i < result.step5PerpendicularClusterIndices.size(); ++i) { - std::cout << " #" << result.step5PerpendicularClusterIndices[i] << std::endl; - } - } else { - std::cout << " No perpendicular cluster found." << std::endl; - } - } + std::cout << "\n--- Best Cluster ---" << std::endl; + if (result.bestClusterIndex >= 0 && result.bestClusterIndex < result.clusterCount) { + const SGrowthCluster& best = result.clusters[result.bestClusterIndex]; + std::cout << " Best: Cluster #" << result.bestClusterIndex + << " pts=" << best.pointCount + << " centroid=(" << best.centroid.x << "," << best.centroid.y << "," << best.centroid.z << ")" + << std::endl; + } else { + std::cout << " No valid best cluster found." << std::endl; + } + + if (!result.validClusterIndices.empty()) { + std::cout << "\n--- Valid Cluster Indices ---" << std::endl; + for (size_t i = 0; i < result.validClusterIndices.size(); ++i) { + std::cout << " #" << result.validClusterIndices[i] << std::endl; + } + } + + std::cout << "\n--- Step 5 ---" << std::endl; + if (result.step5BaseClusterIndex >= 0 && result.step5BaseClusterIndex < result.clusterCount) { + const SGrowthCluster& base = result.clusters[result.step5BaseClusterIndex]; + std::cout << " Base: Cluster #" << result.step5BaseClusterIndex + << " pts=" << base.pointCount + << " centroid=(" << base.centroid.x << "," << base.centroid.y << "," << base.centroid.z << ")" + << std::endl; + } else { + std::cout << " No Step 5 base cluster found." << std::endl; + } + + if (!result.step5PerpendicularClusterIndices.empty()) { + std::cout << " Perpendicular clusters:" << std::endl; + for (size_t i = 0; i < result.step5PerpendicularClusterIndices.size(); ++i) { + std::cout << " #" << result.step5PerpendicularClusterIndices[i] << std::endl; + } + } else { + std::cout << " No perpendicular cluster found." << std::endl; + } + } std::cout << "===========================================" << std::endl; } diff --git a/Algo/DetectBarIntersection/src/RegionGrowing.cpp b/Algo/DetectBarIntersection/src/RegionGrowing.cpp index aa13e4b..6ed55b6 100644 --- a/Algo/DetectBarIntersection/src/RegionGrowing.cpp +++ b/Algo/DetectBarIntersection/src/RegionGrowing.cpp @@ -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(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(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& 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 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& 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& 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 workingPoints(points, points + rows * cols); + + // 先对每一行提取 line feature，并缓存到表中。 + // 这样可以把“行内分段”和“跨行关联”拆成两个阶段，降低跨行匹配时的噪声敏感性。 std::vector > rowFeatureTable(static_cast(rows)); + + // tree 是区域生长阶段的中间表示。 + // 每棵 tree 收集一组跨越多行、被判定为属于同一根条材的 feature。 std::vector 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(outClusters.size()); }