From 916f2c76621bee39797a60d49c22e7faf14757de Mon Sep 17 00:00:00 2001 From: cool609 Date: Thu, 9 Apr 2026 23:43:28 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=97=A0=E7=94=A8=E4=BB=A3?= =?UTF-8?q?=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/BarIntersection.cpp | 354 +----------------- 1 file changed, 1 insertion(+), 353 deletions(-) diff --git a/Algo/DetectBarIntersection/src/BarIntersection.cpp b/Algo/DetectBarIntersection/src/BarIntersection.cpp index 3176dc8..ee124e7 100644 --- a/Algo/DetectBarIntersection/src/BarIntersection.cpp +++ b/Algo/DetectBarIntersection/src/BarIntersection.cpp @@ -18,18 +18,6 @@ static bool IsValidPoint(const SVzNLPointXYZ& pt) { 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; @@ -130,347 +118,7 @@ static void FilterLaserLineNoiseInPlace( } } -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( +int DetectBarIntersections( const SVzNLPointXYZ* points, int rows, int cols,