更新跳变点查找

Algo/DetectHole/src/HoleDetection.cpp

@@ -89,6 +89,398 @@ static int ValidateGrid(
     return HD_SUCCESS;
 }
 
+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<float> SmoothScalarValues(const std::vector<float>& values, int windowSize) {
+    if (values.empty()) {
+        return {};
+    }
+
+    if (windowSize <= 1) {
+        return values;
+    }
+
+    if ((windowSize % 2) == 0) {
+        ++windowSize;
+    }
+
+    const int halfWindow = windowSize / 2;
+    std::vector<float> smoothed(values.size(), 0.0f);
+
+    for (size_t i = 0; i < values.size(); ++i) {
+        const int begin = std::max(0, static_cast<int>(i) - halfWindow);
+        const int end = std::min(static_cast<int>(values.size()), static_cast<int>(i) + halfWindow + 1);
+
+        float sum = 0.0f;
+        for (int j = begin; j < end; ++j) {
+            sum += values[static_cast<size_t>(j)];
+        }
+        smoothed[i] = sum / static_cast<float>(end - begin);
+    }
+
+    return smoothed;
+}
+
+static float ComputePercentile(std::vector<float> 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<size_t>(percentile * static_cast<float>(values.size() - 1));
+    return values[idx];
+}
+
+static float ComputeAdaptiveSlopeThreshold(
+    const std::vector<float>& slopeValues,
+    float explicitThreshold,
+    float minThreshold
+) {
+    if (explicitThreshold > 0.0f) {
+        return explicitThreshold;
+    }
+
+    std::vector<float> 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<float>(overlap) / static_cast<float>(minLen);
+    return overlapRatio >= 0.7f;
+}
+
+static void UpsertSegmentPair(
+    const SSegmentPair& pair,
+    std::vector<SSegmentPair>& 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<SLineSample>& samples,
+    int startSampleIdx,
+    int endSampleIdx,
+    const SHoleDetectionParams& params,
+    std::vector<SSegmentPair>& segmentPairs,
+    EPanelType ePanelType
+) {
+    if (startSampleIdx < 0 || endSampleIdx <= startSampleIdx ||
+        endSampleIdx >= static_cast<int>(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<SLineSample>& samples,
+    int segmentBegin,
+    int segmentEnd,
+    int startEdgeIdx,
+    int lastOppositeEdgeIdx,
+    int smoothWindow,
+    const SHoleDetectionParams& params,
+    std::vector<SSegmentPair>& 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<SLineSample>& samples,
+    int segmentBegin,
+    int segmentEnd,
+    int smoothWindow,
+    const SHoleDetectionParams& params
+) {
+    std::vector<float> 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<int>(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<SLineSample>& samples,
+    int segmentBegin,
+    int segmentEnd,
+    int smoothWindow,
+    const SHoleDetectionParams& params,
+    std::vector<SSegmentPair>& 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<float> zValues;
+    zValues.reserve(segmentSize);
+    for (int i = segmentBegin; i < segmentEnd; ++i) {
+        zValues.push_back(samples[i].point.z);
+    }
+
+    const std::vector<float> smoothedZ = SmoothScalarValues(zValues, smoothWindow);
+    const std::vector<float> baselineZ = SmoothScalarValues(zValues, baselineWindow);
+
+    std::vector<float> residualValues(segmentSize, 0.0f);
+    for (int i = 0; i < segmentSize; ++i) {
+        residualValues[i] = smoothedZ[static_cast<size_t>(i)] - baselineZ[static_cast<size_t>(i)];
+    }
+    residualValues = SmoothScalarValues(residualValues, 3);
+
+    std::vector<SResidualExtremum> extrema;
+    extrema.reserve(segmentSize / 2);
+    for (int i = 1; i < segmentSize - 1; ++i) {
+        const float prevValue = residualValues[static_cast<size_t>(i - 1)];
+        const float curValue = residualValues[static_cast<size_t>(i)];
+        const float nextValue = residualValues[static_cast<size_t>(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,
@@ -104,17 +496,220 @@ void EvaluateLine(
         return;
     }
 
-    //
+    const int totalPoints = rows * cols;
+    const int maxGapPoints = std::max(0, params.maxGapPointsInLine);
 
-    for (int ptIdx = 0; ptIdx < count; ptIdx++) {
-		int idx = startIdx + ptIdx * step;
-		const SVzNLPointXYZ& pt = points[idx];
+    std::vector<SLineSample> 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<int>(samples.size())) {
+        int segmentEnd = segmentBegin + 1;
+        while (segmentEnd < static_cast<int>(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<float> zValues;
+            zValues.reserve(segmentSize);
+            for (int i = segmentBegin; i < segmentEnd; ++i) {
+                zValues.push_back(samples[i].point.z);
+            }
+
+            const std::vector<float> smoothedZ = SmoothScalarValues(zValues, smoothWindow);
+
+            std::vector<float> slopes;
+            std::vector<float> regularSlopes;
+            std::vector<float> gapSlopes;
+            std::vector<unsigned char> 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<size_t>(i)] - smoothedZ[static_cast<size_t>(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<int>(slopes.size()); ++edgeIdx) {
+                const float curSlope = slopes[static_cast<size_t>(edgeIdx)];
+                const float curThreshold = slopeAcrossGap[static_cast<size_t>(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<std::vector<int>> clusters;
     std::vector<SHoleBoundaryPoint> 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;
     }