diff --git a/Algo/DetectHole/src/HoleDetection.cpp b/Algo/DetectHole/src/HoleDetection.cpp index d706d5e..1e63922 100644 --- a/Algo/DetectHole/src/HoleDetection.cpp +++ b/Algo/DetectHole/src/HoleDetection.cpp @@ -89,32 +89,627 @@ static int ValidateGrid( return HD_SUCCESS; } -void EvaluateLine( - const SVzNLPointXYZ* points, +struct SLineSample { + SVzNLPointXYZ point; + int linePos; + int globalIdx; + int row; + int col; + int gapFromPrev; + float arcLength; +}; + +struct SResidualExtremum { + int localIdx; + float value; + bool isMinimum; +}; + +static std::vector SmoothScalarValues(const std::vector& values, int windowSize) { + if (values.empty()) { + return {}; + } + + if (windowSize <= 1) { + return values; + } + + if ((windowSize % 2) == 0) { + ++windowSize; + } + + const int halfWindow = windowSize / 2; + std::vector smoothed(values.size(), 0.0f); + + for (size_t i = 0; i < values.size(); ++i) { + const int begin = std::max(0, static_cast(i) - halfWindow); + const int end = std::min(static_cast(values.size()), static_cast(i) + halfWindow + 1); + + float sum = 0.0f; + for (int j = begin; j < end; ++j) { + sum += values[static_cast(j)]; + } + smoothed[i] = sum / static_cast(end - begin); + } + + return smoothed; +} + +static float ComputePercentile(std::vector values, float percentile) { + if (values.empty()) { + return 0.0f; + } + + std::sort(values.begin(), values.end()); + + percentile = std::max(0.0f, std::min(1.0f, percentile)); + const size_t idx = static_cast(percentile * static_cast(values.size() - 1)); + return values[idx]; +} + +static float ComputeAdaptiveSlopeThreshold( + const std::vector& slopeValues, + float explicitThreshold, + float minThreshold +) { + if (explicitThreshold > 0.0f) { + return explicitThreshold; + } + + std::vector absSlopes; + absSlopes.reserve(slopeValues.size()); + for (float slope : slopeValues) { + if (std::isfinite(slope)) { + absSlopes.push_back(std::abs(slope)); + } + } + + if (absSlopes.empty()) { + return minThreshold; + } + + const float p60 = ComputePercentile(absSlopes, 0.60f); + const float p75 = ComputePercentile(absSlopes, 0.75f); + return std::max(minThreshold, std::max(p60 * 2.6f, p75 * 1.8f)); +} + +static bool AreSegmentPairsSimilar(const SSegmentPair& lhs, const SSegmentPair& rhs) { + bool sameRowLine = (lhs.startRow == lhs.endRow) && + (rhs.startRow == rhs.endRow) && + (lhs.startRow == rhs.startRow); + bool sameColLine = (lhs.startCol == lhs.endCol) && + (rhs.startCol == rhs.endCol) && + (lhs.startCol == rhs.startCol); + + if (!sameRowLine && !sameColLine) { + return false; + } + + const int lhsStart = sameRowLine ? std::min(lhs.startCol, lhs.endCol) : std::min(lhs.startRow, lhs.endRow); + const int lhsEnd = sameRowLine ? std::max(lhs.startCol, lhs.endCol) : std::max(lhs.startRow, lhs.endRow); + const int rhsStart = sameRowLine ? std::min(rhs.startCol, rhs.endCol) : std::min(rhs.startRow, rhs.endRow); + const int rhsEnd = sameRowLine ? std::max(rhs.startCol, rhs.endCol) : std::max(rhs.startRow, rhs.endRow); + + const int overlapStart = std::max(lhsStart, rhsStart); + const int overlapEnd = std::min(lhsEnd, rhsEnd); + const int overlap = std::max(0, overlapEnd - overlapStart + 1); + const int lhsLen = lhsEnd - lhsStart + 1; + const int rhsLen = rhsEnd - rhsStart + 1; + const int minLen = std::min(lhsLen, rhsLen); + + if (std::abs(lhsStart - rhsStart) <= 3 && std::abs(lhsEnd - rhsEnd) <= 3) { + return true; + } + + if (minLen <= 0) { + return false; + } + + const float overlapRatio = static_cast(overlap) / static_cast(minLen); + return overlapRatio >= 0.7f; +} + +static void UpsertSegmentPair( + const SSegmentPair& pair, + std::vector& segmentPairs +) { + for (SSegmentPair& existing : segmentPairs) { + if (AreSegmentPairsSimilar(existing, pair)) { + if (pair.depth > existing.depth) { + existing = pair; + } + return; + } + } + + segmentPairs.push_back(pair); +} + +static float SignedDistanceToProjectedLine( + const SVzNLPointXYZ& start, + const SVzNLPointXYZ& end, + const SVzNLPointXYZ& query, + EPanelType ePanelType +) { + const float u0 = (ePanelType == EPanelType::YZ_PANEL) ? start.y : start.x; + const float u1 = (ePanelType == EPanelType::YZ_PANEL) ? end.y : end.x; + const float uq = (ePanelType == EPanelType::YZ_PANEL) ? query.y : query.x; + + const float v0 = start.z; + const float v1 = end.z; + const float vq = query.z; + + const float du = u1 - u0; + const float dv = v1 - v0; + const float norm = std::sqrt(du * du + dv * dv); + if (norm <= 1e-6f) { + return 0.0f; + } + + const float qu = uq - u0; + const float qv = vq - v0; + return (du * qv - dv * qu) / norm; +} + +static bool TryAppendValidatedSegment( + const std::vector& samples, + int startSampleIdx, + int endSampleIdx, + const SHoleDetectionParams& params, + std::vector& segmentPairs, + EPanelType ePanelType +) { + if (startSampleIdx < 0 || endSampleIdx <= startSampleIdx || + endSampleIdx >= static_cast(samples.size())) { + return false; + } + + const int interiorPointCount = endSampleIdx - startSampleIdx - 1; + if (interiorPointCount < params.minVTransitionPoints) { + return false; + } + + const float featureSpan = samples[endSampleIdx].arcLength - samples[startSampleIdx].arcLength; + if (featureSpan < params.minFeatureSpan) { + return false; + } + + float maxAbsResidual = 0.0f; + int residualSign = 0; + bool residualSignConsistent = true; + + for (int i = startSampleIdx + 1; i < endSampleIdx; ++i) { + const float residual = SignedDistanceToProjectedLine( + samples[startSampleIdx].point, + samples[endSampleIdx].point, + samples[i].point, + ePanelType + ); + + const float absResidual = std::abs(residual); + maxAbsResidual = std::max(maxAbsResidual, absResidual); + + if (absResidual > 0.05f) { + const int curSign = (residual >= 0.0f) ? 1 : -1; + if (residualSign == 0) { + residualSign = curSign; + } else if (curSign != residualSign) { + residualSignConsistent = false; + break; + } + } + } + + if (!residualSignConsistent || maxAbsResidual < params.minPitDepth) { + return false; + } + + SSegmentPair pair; + pair.startPoint = samples[startSampleIdx].point; + pair.endPoint = samples[endSampleIdx].point; + pair.startRow = samples[startSampleIdx].row; + pair.startCol = samples[startSampleIdx].col; + pair.endRow = samples[endSampleIdx].row; + pair.endCol = samples[endSampleIdx].col; + pair.depth = maxAbsResidual; + + UpsertSegmentPair(pair, segmentPairs); + return true; +} + +static bool TryAppendSlopeFeatureSegment( + const std::vector& samples, + int segmentBegin, + int segmentEnd, + int startEdgeIdx, + int lastOppositeEdgeIdx, + int smoothWindow, + const SHoleDetectionParams& params, + std::vector& segmentPairs, + EPanelType ePanelType +) { + if (startEdgeIdx < 0 || lastOppositeEdgeIdx < startEdgeIdx) { + return false; + } + + const int segmentSize = segmentEnd - segmentBegin; + if (segmentSize < 3) { + return false; + } + + const int boundaryLag = std::max(1, smoothWindow / 2); + const int localStartIdx = startEdgeIdx; + const int localEndIdx = std::min( + segmentSize - 1, + std::max(localStartIdx + 2, lastOppositeEdgeIdx + 1 - boundaryLag) + ); + + return TryAppendValidatedSegment( + samples, + segmentBegin + localStartIdx, + segmentBegin + localEndIdx, + params, + segmentPairs, + ePanelType + ); +} + +static int EstimateBaselineWindowSize( + const std::vector& samples, + int segmentBegin, + int segmentEnd, + int smoothWindow, + const SHoleDetectionParams& params +) { + std::vector stepLengths; + stepLengths.reserve(std::max(0, segmentEnd - segmentBegin - 1)); + for (int i = segmentBegin + 1; i < segmentEnd; ++i) { + const float ds = samples[i].arcLength - samples[i - 1].arcLength; + if (ds > 1e-6f && std::isfinite(ds)) { + stepLengths.push_back(ds); + } + } + + const float medianStep = std::max(1e-3f, ComputePercentile(stepLengths, 0.5f)); + const float baselineSpanMm = std::max(params.minFeatureSpan * 6.0f, 12.0f); + int windowSize = static_cast(std::ceil(baselineSpanMm / medianStep)); + windowSize = std::max(windowSize, smoothWindow * 5); + windowSize = std::max(windowSize, 15); + windowSize = std::min(windowSize, 61); + + const int segmentSize = segmentEnd - segmentBegin; + if (windowSize >= segmentSize) { + windowSize = std::max(5, segmentSize - 2); + } + + if ((windowSize % 2) == 0) { + --windowSize; + } + + return std::max(windowSize, 5); +} + +static bool DetectFeatureByResidualPattern( + const std::vector& samples, + int segmentBegin, + int segmentEnd, + int smoothWindow, + const SHoleDetectionParams& params, + std::vector& segmentPairs, + EPanelType ePanelType +) { + const int segmentSize = segmentEnd - segmentBegin; + if (segmentSize < std::max(9, smoothWindow * 3)) { + return false; + } + + const int baselineWindow = EstimateBaselineWindowSize( + samples, segmentBegin, segmentEnd, smoothWindow, params + ); + if (baselineWindow <= smoothWindow || baselineWindow >= segmentSize) { + return false; + } + + std::vector zValues; + zValues.reserve(segmentSize); + for (int i = segmentBegin; i < segmentEnd; ++i) { + zValues.push_back(samples[i].point.z); + } + + const std::vector smoothedZ = SmoothScalarValues(zValues, smoothWindow); + const std::vector baselineZ = SmoothScalarValues(zValues, baselineWindow); + + std::vector residualValues(segmentSize, 0.0f); + for (int i = 0; i < segmentSize; ++i) { + residualValues[i] = smoothedZ[static_cast(i)] - baselineZ[static_cast(i)]; + } + residualValues = SmoothScalarValues(residualValues, 3); + + std::vector extrema; + extrema.reserve(segmentSize / 2); + for (int i = 1; i < segmentSize - 1; ++i) { + const float prevValue = residualValues[static_cast(i - 1)]; + const float curValue = residualValues[static_cast(i)]; + const float nextValue = residualValues[static_cast(i + 1)]; + + if (curValue <= prevValue && curValue < nextValue) { + extrema.push_back({ i, curValue, true }); + } else if (curValue >= prevValue && curValue > nextValue) { + extrema.push_back({ i, curValue, false }); + } + } + + bool appended = false; + const float minSideResidual = std::max(0.35f, params.minPitDepth * 0.5f); + const float minCentralResidual = params.minPitDepth; + + for (size_t i = 1; i + 1 < extrema.size(); ++i) { + const SResidualExtremum& left = extrema[i - 1]; + const SResidualExtremum& center = extrema[i]; + const SResidualExtremum& right = extrema[i + 1]; + + if (left.isMinimum != right.isMinimum || left.isMinimum == center.isMinimum) { + continue; + } + + if (!center.isMinimum) { + if (!(left.value < 0.0f && right.value < 0.0f)) { + continue; + } + } else { + if (!(left.value > 0.0f && right.value > 0.0f)) { + continue; + } + } + + const float centralAbs = std::abs(center.value); + const float sideAbs = std::min(std::abs(left.value), std::abs(right.value)); + if (centralAbs < minCentralResidual || sideAbs < minSideResidual) { + continue; + } + + appended = TryAppendValidatedSegment( + samples, + segmentBegin + left.localIdx, + segmentBegin + right.localIdx, + params, + segmentPairs, + ePanelType + ) || appended; + } + + return appended; +} + +void EvaluateLine( + const SVzNLPointXYZ* points, int rows, - int cols, - int startIdx, - int step, + int cols, + int startIdx, + int step, int count, const SHoleDetectionParams& params, - std::vector& segmentPairs, - EPanelType ePanelType -) { - if (count < 3) { - return; - } - - // - - for (int ptIdx = 0; ptIdx < count; ptIdx++) { - int idx = startIdx + ptIdx * step; - const SVzNLPointXYZ& pt = points[idx]; - if (!IsValidPoint(pt)) { - continue; - } - } -} + std::vector& segmentPairs, + EPanelType ePanelType +) { + if (count < 3) { + return; + } + const int totalPoints = rows * cols; + const int maxGapPoints = std::max(0, params.maxGapPointsInLine); + + std::vector samples; + samples.reserve(count); + + bool hasPrevValid = false; + int prevLinePos = -1; + SVzNLPointXYZ prevPoint{}; + float cumulativeArcLength = 0.0f; + + for (int linePos = 0; linePos < count; ++linePos) { + const int globalIdx = startIdx + linePos * step; + if (globalIdx < 0 || globalIdx >= totalPoints) { + break; + } + + const SVzNLPointXYZ& pt = points[globalIdx]; + if (!IsValidPoint(pt)) { + continue; + } + + const int gapFromPrev = hasPrevValid ? (linePos - prevLinePos - 1) : 0; + if (!hasPrevValid || gapFromPrev > maxGapPoints) { + cumulativeArcLength = 0.0f; + } else { + const float dx = pt.x - prevPoint.x; + const float dy = pt.y - prevPoint.y; + const float dz = pt.z - prevPoint.z; + cumulativeArcLength += std::sqrt(dx * dx + dy * dy + dz * dz); + } + + SLineSample sample; + sample.point = pt; + sample.linePos = linePos; + sample.globalIdx = globalIdx; + sample.row = globalIdx / cols; + sample.col = globalIdx % cols; + sample.gapFromPrev = gapFromPrev; + sample.arcLength = cumulativeArcLength; + samples.push_back(sample); + + prevPoint = pt; + prevLinePos = linePos; + hasPrevValid = true; + } + + if (samples.size() < 3) { + return; + } + + int smoothWindow = std::max(params.residualSmoothWindow, params.neighborCount * 2 - 1); + smoothWindow = std::max(1, smoothWindow); + if ((smoothWindow % 2) == 0) { + ++smoothWindow; + } + + int segmentBegin = 0; + while (segmentBegin < static_cast(samples.size())) { + int segmentEnd = segmentBegin + 1; + while (segmentEnd < static_cast(samples.size()) && + samples[segmentEnd].gapFromPrev <= maxGapPoints) { + ++segmentEnd; + } + + const int segmentSize = segmentEnd - segmentBegin; + if (segmentSize >= 3) { + const size_t pairCountBeforeResidual = segmentPairs.size(); + const bool residualDetected = DetectFeatureByResidualPattern( + samples, + segmentBegin, + segmentEnd, + smoothWindow, + params, + segmentPairs, + ePanelType + ); + + if (!residualDetected && pairCountBeforeResidual == segmentPairs.size()) { + std::vector zValues; + zValues.reserve(segmentSize); + for (int i = segmentBegin; i < segmentEnd; ++i) { + zValues.push_back(samples[i].point.z); + } + + const std::vector smoothedZ = SmoothScalarValues(zValues, smoothWindow); + + std::vector slopes; + std::vector regularSlopes; + std::vector gapSlopes; + std::vector slopeAcrossGap; + slopes.reserve(segmentSize - 1); + regularSlopes.reserve(segmentSize - 1); + gapSlopes.reserve(segmentSize - 1); + slopeAcrossGap.reserve(segmentSize - 1); + + for (int i = 1; i < segmentSize; ++i) { + const float ds = samples[segmentBegin + i].arcLength - samples[segmentBegin + i - 1].arcLength; + const float slope = (ds > 1e-6f) ? ((smoothedZ[static_cast(i)] - smoothedZ[static_cast(i - 1)]) / ds) : 0.0f; + const bool crossesGap = samples[segmentBegin + i].gapFromPrev > 0; + + slopes.push_back(slope); + slopeAcrossGap.push_back(crossesGap ? 1U : 0U); + + if (crossesGap) { + gapSlopes.push_back(slope); + } else { + regularSlopes.push_back(slope); + } + } + + const float minSlopeThreshold = std::max(0.15f, params.minPitDepth * 0.35f); + const float jumpThreshold = ComputeAdaptiveSlopeThreshold( + regularSlopes.empty() ? slopes : regularSlopes, + params.jumpThresholdResidual, + minSlopeThreshold + ); + + float gapJumpThreshold = params.gapJumpThresholdResidual; + if (gapJumpThreshold <= 0.0f) { + gapJumpThreshold = ComputeAdaptiveSlopeThreshold( + gapSlopes, + 0.0f, + jumpThreshold * 1.1f + ); + } + + int state = 0; + int firstDirection = 0; + int startEdgeIdx = -1; + int lastOppositeEdgeIdx = -1; + + for (int edgeIdx = 0; edgeIdx < static_cast(slopes.size()); ++edgeIdx) { + const float curSlope = slopes[static_cast(edgeIdx)]; + const float curThreshold = slopeAcrossGap[static_cast(edgeIdx)] ? gapJumpThreshold : jumpThreshold; + + int strongDirection = 0; + if (curSlope > curThreshold) { + strongDirection = 1; + } else if (curSlope < -curThreshold) { + strongDirection = -1; + } + + if (state == 0) { + if (strongDirection != 0) { + state = 1; + firstDirection = strongDirection; + startEdgeIdx = edgeIdx; + } + continue; + } + + if (state == 1) { + if (strongDirection == -firstDirection) { + state = 2; + lastOppositeEdgeIdx = edgeIdx; + } else if (strongDirection == 0 && + (edgeIdx - startEdgeIdx) > smoothWindow * 3) { + state = 0; + firstDirection = 0; + startEdgeIdx = -1; + } + continue; + } + + if (strongDirection == -firstDirection) { + lastOppositeEdgeIdx = edgeIdx; + continue; + } + + TryAppendSlopeFeatureSegment( + samples, + segmentBegin, + segmentEnd, + startEdgeIdx, + lastOppositeEdgeIdx, + smoothWindow, + params, + segmentPairs, + ePanelType + ); + + if (strongDirection != 0) { + state = 1; + firstDirection = strongDirection; + startEdgeIdx = edgeIdx; + lastOppositeEdgeIdx = -1; + } else { + state = 0; + firstDirection = 0; + startEdgeIdx = -1; + lastOppositeEdgeIdx = -1; + } + } + + if (state == 2) { + TryAppendSlopeFeatureSegment( + samples, + segmentBegin, + segmentEnd, + startEdgeIdx, + lastOppositeEdgeIdx, + smoothWindow, + params, + segmentPairs, + ePanelType + ); + } + } + } + + segmentBegin = segmentEnd; + } +} /** * @brief 检测凹坑边界点 @@ -1472,17 +2067,12 @@ int DetectMultipleHoles( result->totalCandidates = 0; result->filteredCount = 0; - SHoleDetectionParams detectionPointParams; - int nErrorCode = 0; RowZDiffFluctuationStats outStats; - ComputeRowZDiffFluctuation(points, rows, cols, &outStats, &nErrorCode); - detectionPointParams.minPitDepth = outStats.rangeAbsDz * 3;// 获取z值波动值 - int errCode = 0; // 步骤 2: 检测凹坑边界 std::vector> clusters; std::vector boundaryPoints; - int ret = DetectPitBoundaries(points, rows, cols, detectionPointParams, boundaryPoints, clusters, &errCode, nullptr, nullptr, debugCallbacks); + int ret = DetectPitBoundaries(points, rows, cols, detectionParams, boundaryPoints, clusters, &errCode, nullptr, nullptr, debugCallbacks); if (ret != HD_SUCCESS) { return ret; }