更新

Algo/DetectHole/CMakeLists.txt

@@ -78,11 +78,14 @@ include_directories(
 add_subdirectory(PointCloudUtils)
 
 # Source files (removed PointCloudLoader and HoleDetectionVisualizer)
-set(HOLE_DETECTION_SOURCES
-    src/HoleDetection.cpp
-    src/GeometricFitting.cpp
-	src/PlaneSegmentation.cpp
-)
+set(HOLE_DETECTION_SOURCES
+    src/HoleDetection.cpp
+    src/HoleDetectionBoundary.cpp
+    src/HoleDetectionCluster.cpp
+    src/HoleDetectionFitting.cpp
+    src/GeometricFitting.cpp
+	src/PlaneSegmentation.cpp
+)
 
 set(HOLE_DETECTION_HEADERS
     src/HoleDetection.h

Algo/DetectHole/docs/doc.txt

@@ -0,0 +1,17 @@
+我需要做一个算法，用于检测一个点云数据中的圆孔，最终输出圆孔的中心点和法向量
+我提供:
+1. 点的结构体使用SVzNLPointXYZ，头文件 D:/Develop/DetectHole/include/VZNL_Types.h
+2. 算法的思想可以参考D:/Develop/algoLib/sourceCode/channelSpaceMeasure.cpp
+3. 测试的点云文件的加载可以参考D:/Develop/VZNLSDK/Utils/VzKernel/Src下的VzLaserTxtFile.cpp、VzLasFile.cpp、VzPureTxtFile.cpp、VzPlyFile.cpp和VzPcdFile.cpp的实现，可以支持读取pcd,txt文件
+4. 点云测试文件在D:/Develop/DetectHole/test
+需求:
+1. 检测点云数据中的圆孔，最终输出圆孔的中心点和法向量
+2. 将必要的功能都做成一个单独的函数
+3. 算法使用标准C++进行开发
+算法开发思路:
+1. 要求输入点云必须为栅格化后的数据，输入参数为点集，行数，列数
+2. 对点云进行横向和纵向两种方式进行单线遍历，找到每条线的凹坑信息（具体为Z值会进行异变），可以使用相邻点进行连线（相邻点可以为2个点也可以是多个，可以作为算法参数），将相邻两条线求他们的夹角，如果角度大于某个参数阈值（默认70°），那么认为是很大跳变，是凹坑信息，如果小于某个参数阈值（默认-70°），我们认为是噪点，所以正常点应该是在-70°~70°范围内，记录下来横向和纵向两种方式得到的凹坑端点信息（即坑上的点）
+3. 将记录下来的点拟合为一个椭圆。
+4. 根据拟合的椭圆向外扩展，具体扩展的大小为一个参数，扩展后认为这是一个基本面的圈（第1个圈），再向外扩展一圈（第2个圈），找两个圈中间的点云，然后使用这些点云进行x,y平面拟合
+5. 根据第4步的拟合平面计算出来此平面的法向量，这个法向量为最终输出的法向量
+6. 根据拟合的椭圆（第3步），来获取其中心点的x,y值，z值通过拟合平面参数进行计算。

Algo/DetectHole/src/HoleDetectionBoundary.cpp

@@ -0,0 +1,577 @@
+#include "HoleDetectionInternal.h"
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <map>
+#include <set>
+#include <vector>
+
+namespace hole_detection {
+namespace internal {
+
+bool IsValidPoint(const SVzNLPointXYZ& pt) {
+    constexpr float kInvalidPointEpsilon = 1e-6f;
+
+    if (std::abs(pt.x) < kInvalidPointEpsilon &&
+        std::abs(pt.y) < kInvalidPointEpsilon &&
+        std::abs(pt.z) < kInvalidPointEpsilon) {
+        return false;
+    }
+
+    return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
+}
+
+void EvaluateLine(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    int startIdx,
+    int step,
+    int count,
+    const SHoleDetectionParams& params,
+    std::vector<SSegmentPair>& segmentPairs,
+    EPanelType panelType
+) {
+    segmentPairs.clear();
+    if (count < 3) return;
+
+    constexpr float kPi = 3.14159265358979323846f;
+    const float searchDist = params.angleSearchDistance;
+    const float posThresh = params.angleThresholdPos;
+    const float negThresh = params.angleThresholdNeg;
+
+    // YZ_PANEL: coord=y, XZ_PANEL: coord=x
+    auto getCoord = [panelType](const SVzNLPointXYZ& pt) -> float {
+        return (panelType == EPanelType::YZ_PANEL) ? pt.y : pt.x;
+    };
+
+    auto planeDist = [panelType](const SVzNLPointXYZ& a, const SVzNLPointXYZ& b) -> float {
+        float dz = a.z - b.z;
+        float dc = (panelType == EPanelType::YZ_PANEL) ? (a.y - b.y) : (a.x - b.x);
+        return std::sqrt(dc * dc + dz * dz);
+    };
+
+    // ================================================================
+    // Step 1: per-point angle computation
+    // ================================================================
+    struct PointInfo {
+        SVzNLPointXYZ point;
+        int linePos;
+        int globalIdx;
+        bool valid;
+        float signedAngleDeg;
+        bool hasAngle;
+        ELineAngleTrend trend;
+    };
+
+    std::vector<PointInfo> pts(count);
+    for (int i = 0; i < count; i++) {
+        int gIdx = startIdx + i * step;
+        pts[i].point = points[gIdx];
+        pts[i].linePos = i;
+        pts[i].globalIdx = gIdx;
+        pts[i].valid = IsValidPoint(points[gIdx]);
+        pts[i].signedAngleDeg = 0.0f;
+        pts[i].hasAngle = false;
+        pts[i].trend = keLineAngleTrend_Invalid;
+    }
+
+    for (int i = 0; i < count; i++) {
+        if (!pts[i].valid) continue;
+
+        const SVzNLPointXYZ& cur = pts[i].point;
+
+        // backward reference: walk toward index 0
+        bool foundBack = false;
+        float backCoord = 0.0f, backMeanZ = 0.0f;
+        {
+            float sumZ = 0.0f;
+            int zCnt = 0;
+            for (int j = i - 1; j >= 0; j--) {
+                if (!pts[j].valid) continue;
+                sumZ += pts[j].point.z;
+                zCnt++;
+                if (planeDist(cur, pts[j].point) >= searchDist) {
+                    backCoord = getCoord(pts[j].point);
+                    backMeanZ = sumZ / static_cast<float>(zCnt);
+                    foundBack = true;
+                    break;
+                }
+            }
+        }
+
+        // forward reference: walk toward index count-1
+        bool foundFwd = false;
+        float fwdCoord = 0.0f, fwdMeanZ = 0.0f;
+        {
+            float sumZ = 0.0f;
+            int zCnt = 0;
+            for (int j = i + 1; j < count; j++) {
+                if (!pts[j].valid) continue;
+                sumZ += pts[j].point.z;
+                zCnt++;
+                if (planeDist(cur, pts[j].point) >= searchDist) {
+                    fwdCoord = getCoord(pts[j].point);
+                    fwdMeanZ = sumZ / static_cast<float>(zCnt);
+                    foundFwd = true;
+                    break;
+                }
+            }
+        }
+
+        if (!foundBack || !foundFwd) continue;
+
+        pts[i].hasAngle = true;
+        float cCoord = getCoord(cur);
+        float cZ = cur.z;
+
+        // vec1: backward_ref -> current
+        float v1c = cCoord - backCoord;
+        float v1z = cZ - backMeanZ;
+        // vec2: current -> forward_ref
+        float v2c = fwdCoord - cCoord;
+        float v2z = fwdMeanZ - cZ;
+
+        float cross = v1c * v2z - v1z * v2c;
+        float dot   = v1c * v2c + v1z * v2z;
+        float angle = std::atan2(cross, dot) * (180.0f / kPi);
+
+        pts[i].signedAngleDeg = angle;
+
+        if (angle > posThresh) {
+            pts[i].trend = keLineAngleTrend_PositiveJump;
+        } else if (angle < negThresh) {
+            pts[i].trend = keLineAngleTrend_NegativeJump;
+        } else {
+            pts[i].trend = keLineAngleTrend_Flat;
+        }
+    }
+
+    // ================================================================
+    // Step 2: group consecutive same-trend points into segments
+    // ================================================================
+    enum class ESegType { Flat, Ascending, Descending, Gap };
+
+    struct Segment {
+        ESegType type;
+        int startPos;
+        int endPos;
+        float avgAngle;
+        float length;
+    };
+
+    std::vector<Segment> segs;
+    {
+        int idx = 0;
+        while (idx < count) {
+            // gap segment: consecutive invalid points
+            if (!pts[idx].valid) {
+                int s = idx;
+                while (idx < count && !pts[idx].valid) idx++;
+                Segment seg;
+                seg.type = ESegType::Gap;
+                seg.startPos = s;
+                seg.endPos = idx - 1;
+                seg.avgAngle = 0.0f;
+                seg.length = 0.0f;
+                segs.push_back(seg);
+                continue;
+            }
+
+            ESegType stype;
+            if (pts[idx].trend == keLineAngleTrend_PositiveJump)
+                stype = ESegType::Ascending;
+            else if (pts[idx].trend == keLineAngleTrend_NegativeJump)
+                stype = ESegType::Descending;
+            else
+                stype = ESegType::Flat;
+
+            int s = idx;
+            float aSum = pts[idx].signedAngleDeg;
+            int aCnt = 1;
+            idx++;
+
+            while (idx < count && pts[idx].valid) {
+                ESegType ntype;
+                if (pts[idx].trend == keLineAngleTrend_PositiveJump)
+                    ntype = ESegType::Ascending;
+                else if (pts[idx].trend == keLineAngleTrend_NegativeJump)
+                    ntype = ESegType::Descending;
+                else
+                    ntype = ESegType::Flat;
+
+                if (ntype != stype) break;
+                aSum += pts[idx].signedAngleDeg;
+                aCnt++;
+                idx++;
+            }
+
+            float len = 0.0f;
+            for (int k = s; k < idx - 1; k++) {
+                if (pts[k].valid && pts[k + 1].valid)
+                    len += planeDist(pts[k].point, pts[k + 1].point);
+            }
+
+            Segment seg;
+            seg.type = stype;
+            seg.startPos = s;
+            seg.endPos = idx - 1;
+            seg.avgAngle = aSum / static_cast<float>(aCnt);
+            seg.length = len;
+            segs.push_back(seg);
+        }
+    }
+
+    // ================================================================
+    // Step 3: pattern matching -> output SSegmentPair
+    // ================================================================
+    auto getRowCol = [cols](int globalIdx, int& r, int& c) {
+        r = globalIdx / cols;
+        c = globalIdx % cols;
+    };
+
+    // 3a. Flat -> Descending -> [...] -> Ascending -> Flat
+    for (size_t si = 0; si + 1 < segs.size(); si++) {
+        if (segs[si].type != ESegType::Flat) continue;
+        if (segs[si + 1].type != ESegType::Descending) continue;
+
+        for (size_t ei = si + 2; ei + 1 < segs.size(); ei++) {
+            if (segs[ei].type != ESegType::Ascending) continue;
+            if (segs[ei + 1].type != ESegType::Flat) continue;
+
+            // feature points: end of leading Flat, start of trailing Flat
+            int startPos = segs[si].endPos;
+            int endPos   = segs[ei + 1].startPos;
+
+            // depth: surface level minus minimum z in the pit
+            float refZ = std::min(pts[startPos].point.z, pts[endPos].point.z);
+            float minZ = refZ;
+            for (int k = startPos; k <= endPos; k++) {
+                if (pts[k].valid) minZ = std::min(minZ, pts[k].point.z);
+            }
+            float depth = refZ - minZ;
+
+            if (depth < params.minPitDepth) continue;
+
+            float span = planeDist(pts[startPos].point, pts[endPos].point);
+            if (span < params.minFeatureSpan) continue;
+
+            SSegmentPair pair;
+            pair.startPoint = pts[startPos].point;
+            pair.endPoint   = pts[endPos].point;
+            getRowCol(pts[startPos].globalIdx, pair.startRow, pair.startCol);
+            getRowCol(pts[endPos].globalIdx,   pair.endRow,   pair.endCol);
+            pair.depth = depth;
+            segmentPairs.push_back(pair);
+
+            si = ei;  // advance past this match
+            break;
+        }
+    }
+
+    // 3b. Gap segments with valid endpoints on both sides
+    for (size_t si = 0; si < segs.size(); si++) {
+        if (segs[si].type != ESegType::Gap) continue;
+
+        int gapLen = segs[si].endPos - segs[si].startPos + 1;
+        if (gapLen > params.maxGapPointsInLine) continue;
+
+        // find last valid point before gap
+        int beforeIdx = -1;
+        for (int k = segs[si].startPos - 1; k >= 0; k--) {
+            if (pts[k].valid) { beforeIdx = k; break; }
+        }
+
+        // find first valid point after gap
+        int afterIdx = -1;
+        for (int k = segs[si].endPos + 1; k < count; k++) {
+            if (pts[k].valid) { afterIdx = k; break; }
+        }
+
+        if (beforeIdx < 0 || afterIdx < 0) continue;
+
+        float span = planeDist(pts[beforeIdx].point, pts[afterIdx].point);
+        if (span < params.minFeatureSpan) continue;
+
+        SSegmentPair pair;
+        pair.startPoint = pts[beforeIdx].point;
+        pair.endPoint   = pts[afterIdx].point;
+        getRowCol(pts[beforeIdx].globalIdx, pair.startRow, pair.startCol);
+        getRowCol(pts[afterIdx].globalIdx,  pair.endRow,   pair.endCol);
+        pair.depth = std::abs(pts[beforeIdx].point.z - pts[afterIdx].point.z);
+        segmentPairs.push_back(pair);
+    }
+}
+
+int DetectPitBoundaries(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const SHoleDetectionParams& params,
+    std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    std::vector<std::vector<int>>& clusters,
+    int* errCode,
+    LineEvaluationCallback callback,
+    void* userData,
+    const SHoleDetectionDebugCallbacks* debugCallbacks
+) {
+    if (points == nullptr || rows <= 0 || cols <= 0 || errCode == nullptr) {
+        if (errCode) *errCode = HD_ERR_INVALID_INPUT;
+        return HD_ERR_INVALID_INPUT;
+    }
+
+    boundaryPoints.clear();
+    clusters.clear();
+
+#if 1
+    std::vector<std::vector<SSegmentPair>> allRowSegmentPairs(rows);
+
+    int startIdx = 0;
+    for (int row = 0; row < rows; row++) {
+        std::vector<SSegmentPair> curSegmentPairs;
+
+        EvaluateLine(points, rows, cols, startIdx, 1, cols, params, curSegmentPairs, EPanelType::YZ_PANEL);
+
+        allRowSegmentPairs[row] = curSegmentPairs;
+        startIdx += cols;
+    }
+
+    if (debugCallbacks && debugCallbacks->onSegmentPairsDetected) {
+        std::vector<SSegmentPair> allSegmentPairs;
+        for (int row = 0; row < rows; row++) {
+            for (const auto& pair : allRowSegmentPairs[row]) {
+                allSegmentPairs.push_back(pair);
+            }
+        }
+
+        if (!allSegmentPairs.empty()) {
+            debugCallbacks->onSegmentPairsDetected(
+                allSegmentPairs.data(),
+                static_cast<int>(allSegmentPairs.size()),
+                debugCallbacks->userData
+            );
+        }
+    }
+
+    auto hasColOverlap = [](const SSegmentPair& pair1, const SSegmentPair& pair2, int tolerance = 0, float minOverlapRatio = 0.6f) -> bool {
+        int min1 = std::min(pair1.startCol, pair1.endCol);
+        int max1 = std::max(pair1.startCol, pair1.endCol);
+        int min2 = std::min(pair2.startCol, pair2.endCol);
+        int max2 = std::max(pair2.startCol, pair2.endCol);
+
+        int length1 = max1 - min1 + 1;
+        int length2 = max2 - min2 + 1;
+
+        min1 -= tolerance;
+        max1 += tolerance;
+        min2 -= tolerance;
+        max2 += tolerance;
+
+        if (max1 < min2 || max2 < min1) {
+            return false;
+        }
+
+        int overlapStart = std::max(min1, min2);
+        int overlapEnd = std::min(max1, max2);
+        int overlapSize = overlapEnd - overlapStart + 1;
+
+        int minLength = std::min(length1, length2);
+        float overlapRatio = static_cast<float>(overlapSize) / static_cast<float>(minLength);
+
+        return overlapRatio >= minOverlapRatio;
+    };
+
+    std::vector<std::vector<bool>> shouldKeep(rows);
+    for (int row = 0; row < rows; row++) {
+        shouldKeep[row].resize(allRowSegmentPairs[row].size(), false);
+    }
+
+    std::vector<int> parent;
+    std::vector<int> rank;
+
+    int totalPairs = 0;
+    for (int row = 0; row < rows; row++) {
+        totalPairs += static_cast<int>(allRowSegmentPairs[row].size());
+    }
+    parent.resize(totalPairs);
+    rank.resize(totalPairs, 0);
+    for (int i = 0; i < totalPairs; i++) {
+        parent[i] = i;
+    }
+
+    std::function<int(int)> find = [&parent, &find](int x) -> int {
+        if (parent[x] != x) {
+            parent[x] = find(parent[x]);
+        }
+        return parent[x];
+    };
+
+    auto unite = [&parent, &rank, &find](int x, int y) -> void {
+        int rootX = find(x);
+        int rootY = find(y);
+        if (rootX != rootY) {
+            if (rank[rootX] < rank[rootY]) {
+                parent[rootX] = rootY;
+            } else if (rank[rootX] > rank[rootY]) {
+                parent[rootY] = rootX;
+            } else {
+                parent[rootY] = rootX;
+                rank[rootX]++;
+            }
+        }
+    };
+
+    auto getPairId = [&allRowSegmentPairs](int row, int pairIndex) -> int {
+        int id = 0;
+        for (int r = 0; r < row; r++) {
+            id += static_cast<int>(allRowSegmentPairs[r].size());
+        }
+        return id + pairIndex;
+    };
+
+    int lookAheadWindow = 3;
+
+    for (int row = 0; row < rows; row++) {
+        const auto& currentRowPairs = allRowSegmentPairs[row];
+
+        for (int nextRow = row + 1; nextRow < std::min(row + lookAheadWindow + 1, rows); nextRow++) {
+            const auto& nextRowPairs = allRowSegmentPairs[nextRow];
+
+            for (size_t i = 0; i < currentRowPairs.size(); i++) {
+                for (size_t j = 0; j < nextRowPairs.size(); j++) {
+                    if (hasColOverlap(currentRowPairs[i], nextRowPairs[j])) {
+                        shouldKeep[row][i] = true;
+                        shouldKeep[nextRow][j] = true;
+
+                        int pairId1 = getPairId(row, static_cast<int>(i));
+                        int pairId2 = getPairId(nextRow, static_cast<int>(j));
+                        unite(pairId1, pairId2);
+                    }
+                }
+            }
+        }
+    }
+
+    std::map<int, std::vector<int>> rootToPairIds;
+
+    for (int row = 0; row < rows; row++) {
+        const auto& rowPairs = allRowSegmentPairs[row];
+        for (size_t i = 0; i < rowPairs.size(); i++) {
+            if (shouldKeep[row][i]) {
+                int pairId = getPairId(row, static_cast<int>(i));
+                int root = find(pairId);
+                rootToPairIds[root].push_back(pairId);
+            }
+        }
+    }
+
+    std::set<int> validRoots;
+
+    for (const auto& entry : rootToPairIds) {
+        int root = entry.first;
+        const auto& pairIds = entry.second;
+
+        std::vector<SVzNLPointXYZ> clusterPoints;
+        for (int pairId : pairIds) {
+            int id = 0;
+            bool found = false;
+            for (int r = 0; r < rows && !found; r++) {
+                const auto& rowPairs = allRowSegmentPairs[r];
+                for (size_t j = 0; j < rowPairs.size(); j++) {
+                    if (id == pairId) {
+                        clusterPoints.push_back(rowPairs[j].startPoint);
+                        clusterPoints.push_back(rowPairs[j].endPoint);
+                        found = true;
+                        break;
+                    }
+                    id++;
+                }
+            }
+        }
+
+        float maxDistance = 0.0f;
+        for (size_t i = 0; i < clusterPoints.size(); i++) {
+            for (size_t j = i + 1; j < clusterPoints.size(); j++) {
+                float dx = clusterPoints[i].x - clusterPoints[j].x;
+                float dy = clusterPoints[i].y - clusterPoints[j].y;
+                float dz = clusterPoints[i].z - clusterPoints[j].z;
+                float dist = std::sqrt(dx * dx + dy * dy + dz * dz);
+                maxDistance = std::max(maxDistance, dist);
+            }
+        }
+
+        validRoots.insert(root);
+    }
+
+    std::map<int, std::vector<int>> pairIdToBoundaryIndices;
+
+    for (int row = 0; row < rows; row++) {
+        const auto& rowPairs = allRowSegmentPairs[row];
+        for (size_t i = 0; i < rowPairs.size(); i++) {
+            if (shouldKeep[row][i]) {
+                int pairId = getPairId(row, static_cast<int>(i));
+                int root = find(pairId);
+
+                if (validRoots.find(root) != validRoots.end()) {
+                    const auto& pair = rowPairs[i];
+
+                    SHoleBoundaryPoint peakBp;
+                    peakBp.point = pair.startPoint;
+                    peakBp.row = pair.startRow;
+                    peakBp.col = pair.startCol;
+                    int peakIndex = static_cast<int>(boundaryPoints.size());
+                    boundaryPoints.push_back(peakBp);
+                    pairIdToBoundaryIndices[pairId].push_back(peakIndex);
+
+                    SHoleBoundaryPoint valleyBp;
+                    valleyBp.point = pair.endPoint;
+                    valleyBp.row = pair.endRow;
+                    valleyBp.col = pair.endCol;
+                    int valleyIndex = static_cast<int>(boundaryPoints.size());
+                    boundaryPoints.push_back(valleyBp);
+                    pairIdToBoundaryIndices[pairId].push_back(valleyIndex);
+                }
+            }
+        }
+    }
+
+    std::map<int, std::vector<int>> rootToCluster;
+
+    for (const auto& entry : pairIdToBoundaryIndices) {
+        int pairId = entry.first;
+        const auto& boundaryIndices = entry.second;
+        int root = find(pairId);
+
+        for (int idx : boundaryIndices) {
+            rootToCluster[root].push_back(idx);
+        }
+    }
+
+    for (const auto& entry : rootToCluster) {
+        clusters.push_back(entry.second);
+    }
+#endif
+
+#if 0
+    for (int col = 0; col < cols; col++) {
+        std::vector<SHoleBoundaryPoint> newBoundaryPoints;
+        EvaluateLine(points, col, cols, rows, params, rows, cols, isBoundary, boundaryPoints, &newBoundaryPoints, nullptr);
+
+        if (callback && !newBoundaryPoints.empty()) {
+            callback(points, rows, cols, col, cols, rows, newBoundaryPoints, "Column", col, userData);
+        }
+    }
+#endif
+
+    if (boundaryPoints.empty()) {
+        *errCode = HD_ERR_NO_PITS_DETECTED;
+        return HD_ERR_NO_PITS_DETECTED;
+    }
+
+    *errCode = HD_SUCCESS;
+    return HD_SUCCESS;
+}
+
+}  // namespace internal
+}  // namespace hole_detection

Algo/DetectHole/src/HoleDetectionCluster.cpp

@@ -0,0 +1,284 @@
+#include "HoleDetectionInternal.h"
+
+#include <algorithm>
+#include <climits>
+#include <cmath>
+#include <iostream>
+#include <set>
+#include <utility>
+#include <vector>
+
+namespace hole_detection {
+namespace internal {
+
+int ExtractClusterExtremePoints(
+    const std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    const std::vector<int>& clusterIndices,
+    std::vector<SHoleBoundaryPoint>& extremePoints,
+    int* errCode
+) {
+    if (boundaryPoints.empty() || clusterIndices.empty() || errCode == nullptr) {
+        if (errCode) *errCode = HD_ERR_INVALID_INPUT;
+        return HD_ERR_INVALID_INPUT;
+    }
+
+    extremePoints.clear();
+
+    if (clusterIndices.size() < 3) {
+        for (int idx : clusterIndices) {
+            if (idx >= 0 && idx < static_cast<int>(boundaryPoints.size())) {
+                extremePoints.push_back(boundaryPoints[idx]);
+            }
+        }
+        *errCode = HD_SUCCESS;
+        return HD_SUCCESS;
+    }
+
+    const int numBuckets = 32;
+    const double kPi = 3.14159265358979323846;
+
+    double cx = 0.0;
+    double cy = 0.0;
+    int validCount = 0;
+
+    for (int idx : clusterIndices) {
+        if (idx < 0 || idx >= static_cast<int>(boundaryPoints.size())) {
+            continue;
+        }
+        const SHoleBoundaryPoint& bp = boundaryPoints[idx];
+        cx += bp.point.x;
+        cy += bp.point.y;
+        validCount++;
+    }
+
+    if (validCount == 0) {
+        *errCode = HD_ERR_INVALID_INPUT;
+        return HD_ERR_INVALID_INPUT;
+    }
+
+    cx /= validCount;
+    cy /= validCount;
+
+    std::vector<int> bucketMaxIdx(numBuckets, -1);
+    std::vector<double> bucketMaxDist(numBuckets, 0.0);
+
+    for (int idx : clusterIndices) {
+        if (idx < 0 || idx >= static_cast<int>(boundaryPoints.size())) {
+            continue;
+        }
+
+        const SHoleBoundaryPoint& bp = boundaryPoints[idx];
+        double dx = bp.point.x - cx;
+        double dy = bp.point.y - cy;
+        double dist = std::sqrt(dx * dx + dy * dy);
+        double angle = std::atan2(dy, dx);
+        int bucket = static_cast<int>((angle + kPi) / (2.0 * kPi) * numBuckets);
+
+        if (bucket < 0) bucket = 0;
+        if (bucket >= numBuckets) bucket = numBuckets - 1;
+
+        if (dist > bucketMaxDist[bucket]) {
+            bucketMaxDist[bucket] = dist;
+            bucketMaxIdx[bucket] = idx;
+        }
+    }
+
+    extremePoints.reserve(numBuckets);
+    for (int i = 0; i < numBuckets; i++) {
+        if (bucketMaxIdx[i] >= 0) {
+            extremePoints.push_back(boundaryPoints[bucketMaxIdx[i]]);
+        }
+    }
+
+    const size_t minPointsForFitting = 8;
+
+    std::vector<double> angles;
+    for (const auto& ep : extremePoints) {
+        double dx = ep.point.x - cx;
+        double dy = ep.point.y - cy;
+        double angle = std::atan2(dy, dx);
+        if (angle < 0) angle += 2.0 * kPi;
+        angles.push_back(angle);
+    }
+    std::sort(angles.begin(), angles.end());
+
+    double maxGap = 0.0;
+    for (size_t i = 0; i < angles.size(); i++) {
+        double gap = (i + 1 < angles.size())
+            ? (angles[i + 1] - angles[i])
+            : (2.0 * kPi + angles[0] - angles[angles.size() - 1]);
+        if (gap > maxGap) maxGap = gap;
+    }
+
+    const double maxAllowedGap = 60.0 * kPi / 180.0;
+    if (extremePoints.size() < minPointsForFitting || maxGap > maxAllowedGap) {
+        std::vector<std::pair<double, int>> distanceIdx;
+        for (int idx : clusterIndices) {
+            if (idx < 0 || idx >= static_cast<int>(boundaryPoints.size())) {
+                continue;
+            }
+            const SHoleBoundaryPoint& bp = boundaryPoints[idx];
+            double dx = bp.point.x - cx;
+            double dy = bp.point.y - cy;
+            double dist = std::sqrt(dx * dx + dy * dy);
+            distanceIdx.push_back(std::make_pair(dist, idx));
+        }
+
+        std::sort(distanceIdx.begin(), distanceIdx.end(),
+                  [](const std::pair<double, int>& a, const std::pair<double, int>& b) {
+                      return a.first > b.first;
+                  });
+
+        std::set<int> existingIndices;
+        for (const auto& ep : extremePoints) {
+            for (int idx : clusterIndices) {
+                if (idx >= 0 && idx < static_cast<int>(boundaryPoints.size()) &&
+                    boundaryPoints[idx].row == ep.row &&
+                    boundaryPoints[idx].col == ep.col) {
+                    existingIndices.insert(idx);
+                    break;
+                }
+            }
+        }
+
+        for (const auto& pair : distanceIdx) {
+            if (extremePoints.size() >= 12) break;
+            if (existingIndices.find(pair.second) == existingIndices.end()) {
+                extremePoints.push_back(boundaryPoints[pair.second]);
+                existingIndices.insert(pair.second);
+            }
+        }
+    }
+
+    if (!extremePoints.empty()) {
+        angles.clear();
+        for (const auto& ep : extremePoints) {
+            double dx = ep.point.x - cx;
+            double dy = ep.point.y - cy;
+            double angle = std::atan2(dy, dx) * 180.0 / kPi;
+            if (angle < 0) angle += 360.0;
+            angles.push_back(angle);
+        }
+        std::sort(angles.begin(), angles.end());
+
+        double maxGapDeg = 0.0;
+        for (size_t i = 0; i < angles.size(); i++) {
+            double gap = (i + 1 < angles.size())
+                ? (angles[i + 1] - angles[i])
+                : (360.0 + angles[0] - angles[angles.size() - 1]);
+            if (gap > maxGapDeg) maxGapDeg = gap;
+        }
+
+        std::cout << "    [EXTREME] Extracted " << extremePoints.size()
+                  << " extreme points, max angular gap: " << maxGapDeg << "°" << std::endl;
+    }
+
+    *errCode = HD_SUCCESS;
+    return HD_SUCCESS;
+}
+
+int ExpandClusterBoundingBox(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const SHoleDetectionParams& params,
+    std::vector<unsigned char>& isBoundary,
+    std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    std::vector<SHoleBoundaryPoint>& clusterPoints,
+    int expandSize,
+    int* errCode
+) {
+    if (points == nullptr || rows <= 0 || cols <= 0 || errCode == nullptr) {
+        if (errCode) *errCode = HD_ERR_INVALID_INPUT;
+        return HD_ERR_INVALID_INPUT;
+    }
+
+    if (clusterPoints.empty()) {
+        *errCode = HD_SUCCESS;
+        return HD_SUCCESS;
+    }
+
+    if (expandSize < 0) {
+        *errCode = HD_ERR_INVALID_INPUT;
+        return HD_ERR_INVALID_INPUT;
+    }
+
+    int minRow = INT_MAX;
+    int maxRow = INT_MIN;
+    int minCol = INT_MAX;
+    int maxCol = INT_MIN;
+
+    for (const auto& cp : clusterPoints) {
+        minRow = std::min(minRow, cp.row);
+        maxRow = std::max(maxRow, cp.row);
+        minCol = std::min(minCol, cp.col);
+        maxCol = std::max(maxCol, cp.col);
+    }
+
+    int expandedMinCol = std::max(0, minCol);
+    int expandedMaxCol = std::min(cols - 1, maxCol);
+    int expandedMinRow = std::max(0, minRow - expandSize);
+    int expandedMaxRow = std::min(rows - 1, maxRow + expandSize);
+
+    size_t originalSize = clusterPoints.size();
+    size_t originalBoundarySize = boundaryPoints.size();
+
+    std::cout << "    [EXPAND] Original BBox=[" << minRow << "," << maxRow << "]x["
+              << minCol << "," << maxCol << "]" << std::endl;
+    std::cout << "    [EXPAND] Expanded BBox=[" << expandedMinRow << "," << expandedMaxRow << "]x["
+              << expandedMinCol << "," << expandedMaxCol << "]" << std::endl;
+
+    std::set<std::pair<int, int>> existingPoints;
+    for (const auto& cp : clusterPoints) {
+        existingPoints.insert(std::make_pair(cp.row, cp.col));
+    }
+
+    int scannedColumns = 0;
+    for (int col = expandedMinCol; col <= expandedMaxCol; col++) {
+        int startIdx = expandedMinRow * cols + col;
+        int step = cols;
+        int count = expandedMaxRow - expandedMinRow + 1;
+
+        std::vector<SSegmentPair> segmentPairs;
+        EvaluateLine(points, rows, cols, startIdx, step, count, params, segmentPairs, EPanelType::XZ_PANEL);
+
+        for (const auto& bp : segmentPairs) {
+            if (existingPoints.find(std::make_pair(bp.startRow, bp.startCol)) == existingPoints.end()) {
+                SHoleBoundaryPoint pt;
+                pt.point = bp.startPoint;
+                pt.row = bp.startRow;
+                pt.col = bp.startCol;
+                clusterPoints.push_back(pt);
+                existingPoints.insert(std::make_pair(pt.row, pt.col));
+            }
+            if (existingPoints.find(std::make_pair(bp.endRow, bp.endCol)) == existingPoints.end()) {
+                SHoleBoundaryPoint pt;
+                pt.point = bp.endPoint;
+                pt.row = bp.endRow;
+                pt.col = bp.endCol;
+                clusterPoints.push_back(pt);
+                existingPoints.insert(std::make_pair(pt.row, pt.col));
+            }
+        }
+
+        if (!segmentPairs.empty()) {
+            scannedColumns++;
+        }
+    }
+
+    size_t addedCount = clusterPoints.size() - originalSize;
+    size_t newBoundaryCount = boundaryPoints.size() - originalBoundarySize;
+
+    std::cout << "    [EXPAND] Scanned " << (expandedMaxCol - expandedMinCol + 1)
+              << " columns, " << scannedColumns << " columns found boundary points" << std::endl;
+    std::cout << "    [EXPAND] Cluster points: " << originalSize << " -> "
+              << clusterPoints.size() << " (added " << addedCount << ")" << std::endl;
+    std::cout << "    [EXPAND] Global boundary points: " << originalBoundarySize << " -> "
+              << boundaryPoints.size() << " (added " << newBoundaryCount << ")" << std::endl;
+
+    *errCode = HD_SUCCESS;
+    return HD_SUCCESS;
+}
+
+}  // namespace internal
+}  // namespace hole_detection

Algo/DetectHole/src/HoleDetectionFitting.cpp

@@ -0,0 +1,631 @@
+#include "HoleDetectionInternal.h"
+#include "GeometricFitting.h"
+
+#include <algorithm>
+#include <climits>
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+namespace hole_detection {
+namespace internal {
+
+int FitHolesFromClusters(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    const std::vector<std::vector<int>>& clusters,
+    const SHoleDetectionParams& detectionParams,
+    const SHoleFilterParams& filterParams,
+    std::vector<SHoleResult>& candidateHoles,
+    int* errCode
+) {
+    if (points == nullptr || rows <= 0 || cols <= 0 ||
+        boundaryPoints.empty() || clusters.empty() || errCode == nullptr) {
+        if (errCode) *errCode = HD_ERR_INVALID_INPUT;
+        return HD_ERR_INVALID_INPUT;
+    }
+
+    candidateHoles.clear();
+
+    for (const std::vector<int>& cluster : clusters) {
+        if (cluster.size() < 5) continue;
+
+        std::vector<SVzNLPointXYZ> clusterPoints;
+        clusterPoints.reserve(cluster.size());
+        for (int idx : cluster) {
+            if (idx >= 0 && idx < static_cast<int>(boundaryPoints.size())) {
+                clusterPoints.push_back(boundaryPoints[idx].point);
+            }
+        }
+
+        if (clusterPoints.size() < 5) continue;
+
+        constexpr float kMaxRadiusCV = 0.4f;
+        {
+            int n = static_cast<int>(clusterPoints.size());
+            float cx = 0.0f;
+            float cy = 0.0f;
+            for (int i = 0; i < n; i++) {
+                cx += clusterPoints[i].x;
+                cy += clusterPoints[i].y;
+            }
+            cx /= n;
+            cy /= n;
+
+            float sumR = 0.0f;
+            float sumR2 = 0.0f;
+            for (int i = 0; i < n; i++) {
+                float dx = clusterPoints[i].x - cx;
+                float dy = clusterPoints[i].y - cy;
+                float r = std::sqrt(dx * dx + dy * dy);
+                sumR += r;
+                sumR2 += r * r;
+            }
+            float meanR = sumR / n;
+            float varR = sumR2 / n - meanR * meanR;
+            if (varR < 0.0f) varR = 0.0f;
+            float stdR = std::sqrt(varR);
+            float cv = (meanR > 1e-10f) ? (stdR / meanR) : 999.0f;
+
+            if (cv > kMaxRadiusCV) {
+                continue;
+            }
+        }
+
+        SHoleResult hole;
+        int fitErr = 0;
+        int ret = FitEllipse(clusterPoints.data(), clusterPoints.size(),
+                            &hole.center, &hole.radius, &hole.eccentricity, &fitErr);
+
+        if (ret != HD_SUCCESS) continue;
+
+        if (hole.radius < detectionParams.minRadius ||
+            hole.radius > detectionParams.maxRadius) {
+            continue;
+        }
+
+        float sumSqDiff = 0.0f;
+        for (const SVzNLPointXYZ& pt : clusterPoints) {
+            float dx = pt.x - hole.center.x;
+            float dy = pt.y - hole.center.y;
+            float dist = std::sqrt(dx * dx + dy * dy);
+            float diff = dist - hole.radius;
+            sumSqDiff += diff * diff;
+        }
+        hole.radiusVariance = std::sqrt(sumSqDiff / clusterPoints.size());
+
+        float minAngle = 360.0f;
+        float maxAngle = 0.0f;
+        for (const SVzNLPointXYZ& pt : clusterPoints) {
+            float dx = pt.x - hole.center.x;
+            float dy = pt.y - hole.center.y;
+            float angle = std::atan2(dy, dx) * 180.0f / 3.14159265f;
+            if (angle < 0) angle += 360.0f;
+            minAngle = std::min(minAngle, angle);
+            maxAngle = std::max(maxAngle, angle);
+        }
+        hole.angularSpan = maxAngle - minAngle;
+
+        std::vector<SVzNLPointXYZ> expandedPoints;
+        float expandRadiusMin = hole.radius + detectionParams.expansionSize1;
+        float expandRadiusMax = hole.radius + detectionParams.expansionSize2;
+
+        int minRow = INT_MAX;
+        int maxRow = INT_MIN;
+        int minCol = INT_MAX;
+        int maxCol = INT_MIN;
+        for (int idx : cluster) {
+            if (idx >= 0 && idx < static_cast<int>(boundaryPoints.size())) {
+                const SHoleBoundaryPoint& bp = boundaryPoints[idx];
+                minRow = std::min(minRow, bp.row);
+                maxRow = std::max(maxRow, bp.row);
+                minCol = std::min(minCol, bp.col);
+                maxCol = std::max(maxCol, bp.col);
+            }
+        }
+
+        const int expandRange = 5;
+        minRow = std::max(0, minRow - expandRange);
+        maxRow = std::min(rows - 1, maxRow + expandRange);
+        minCol = std::max(0, minCol - expandRange);
+        maxCol = std::min(cols - 1, maxCol + expandRange);
+
+        for (int r = minRow; r <= maxRow; r++) {
+            for (int c = minCol; c <= maxCol; c++) {
+                int idx = r * cols + c;
+                const SVzNLPointXYZ& pt = points[idx];
+                if (!std::isfinite(pt.z)) continue;
+
+                float dx = pt.x - hole.center.x;
+                float dy = pt.y - hole.center.y;
+                float dist = std::sqrt(dx * dx + dy * dy);
+
+                if (dist >= expandRadiusMin && dist <= expandRadiusMax) {
+                    expandedPoints.push_back(pt);
+                }
+            }
+        }
+
+        if (expandedPoints.size() >= 3) {
+            double sx2 = 0;
+            double sy2 = 0;
+            double sxy = 0;
+            double sx = 0;
+            double sy = 0;
+            double sz = 0;
+            double szx = 0;
+            double szy = 0;
+            int N = static_cast<int>(expandedPoints.size());
+            for (int i = 0; i < N; i++) {
+                double x = expandedPoints[i].x;
+                double y = expandedPoints[i].y;
+                double z = expandedPoints[i].z;
+                sx2 += x * x;
+                sy2 += y * y;
+                sxy += x * y;
+                sx += x;
+                sy += y;
+                sz += z;
+                szx += z * x;
+                szy += z * y;
+            }
+
+            double a00 = sx2, a01 = sxy, a02 = sx;
+            double a10 = sxy, a11 = sy2, a12 = sy;
+            double a20 = sx, a21 = sy, a22 = static_cast<double>(N);
+
+            double detM = a00 * (a11 * a22 - a12 * a21)
+                        - a01 * (a10 * a22 - a12 * a20)
+                        + a02 * (a10 * a21 - a11 * a20);
+
+            if (std::abs(detM) > 1e-12) {
+                double b0 = szx, b1 = szy, b2 = sz;
+
+                double planeA = (b0 * (a11 * a22 - a12 * a21)
+                               - a01 * (b1 * a22 - a12 * b2)
+                               + a02 * (b1 * a21 - a11 * b2)) / detM;
+
+                double planeB = (a00 * (b1 * a22 - a12 * b2)
+                               - b0 * (a10 * a22 - a12 * a20)
+                               + a02 * (a10 * b2 - b1 * a20)) / detM;
+
+                double planeC = (a00 * (a11 * b2 - b1 * a21)
+                               - a01 * (a10 * b2 - b1 * a20)
+                               + b0 * (a10 * a21 - a11 * a20)) / detM;
+
+                float planeZ = static_cast<float>(planeA * hole.center.x + planeB * hole.center.y + planeC);
+                hole.center.z = planeZ;
+
+                float nx = static_cast<float>(-planeA);
+                float ny = static_cast<float>(-planeB);
+                float nz = 1.0f;
+                float nLen = std::sqrt(nx * nx + ny * ny + nz * nz);
+                hole.normal.x = nx / nLen;
+                hole.normal.y = ny / nLen;
+                hole.normal.z = nz / nLen;
+
+                float sumBoundaryZ = 0.0f;
+                for (const SVzNLPointXYZ& pt : clusterPoints) {
+                    sumBoundaryZ += pt.z;
+                }
+                float meanBoundaryZ = sumBoundaryZ / clusterPoints.size();
+                hole.depth = std::abs(meanBoundaryZ - planeZ);
+            } else {
+                float minZ = clusterPoints[0].z;
+                float maxZ = clusterPoints[0].z;
+                for (const SVzNLPointXYZ& pt : clusterPoints) {
+                    minZ = std::min(minZ, pt.z);
+                    maxZ = std::max(maxZ, pt.z);
+                }
+                hole.depth = maxZ - minZ;
+                hole.normal.x = 0.0f;
+                hole.normal.y = 0.0f;
+                hole.normal.z = 1.0f;
+            }
+        } else {
+            hole.depth = 0.0f;
+            hole.normal.x = 0.0f;
+            hole.normal.y = 0.0f;
+            hole.normal.z = 1.0f;
+        }
+
+        hole.qualityScore = ComputeQualityScore(
+            hole.eccentricity,
+            hole.radiusVariance,
+            hole.angularSpan,
+            clusterPoints.size()
+        );
+
+        candidateHoles.push_back(hole);
+    }
+
+    if (candidateHoles.empty()) {
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    *errCode = HD_SUCCESS;
+    return HD_SUCCESS;
+}
+
+int FitHoleFromExtremePoints(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const std::vector<SHoleBoundaryPoint>& extremePoints,
+    const SHoleDetectionParams& detectionParams,
+    const SHoleFilterParams& filterParams,
+    SHoleResult& hole,
+    int* errCode,
+    const SHoleDetectionDebugCallbacks* debugCallbacks
+) {
+    if (points == nullptr || errCode == nullptr) {
+        if (errCode) *errCode = HD_ERR_INVALID_INPUT;
+        return HD_ERR_INVALID_INPUT;
+    }
+
+    if (extremePoints.size() < 5) {
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    std::vector<SVzNLPointXYZ> clusterPoints;
+    clusterPoints.reserve(extremePoints.size());
+    for (const auto& ep : extremePoints) {
+        clusterPoints.push_back(ep.point);
+    }
+
+    int minRow = INT_MAX;
+    int maxRow = INT_MIN;
+    int minCol = INT_MAX;
+    int maxCol = INT_MIN;
+    for (const auto& ep : extremePoints) {
+        minRow = std::min(minRow, ep.row);
+        maxRow = std::max(maxRow, ep.row);
+        minCol = std::min(minCol, ep.col);
+        maxCol = std::max(maxCol, ep.col);
+    }
+
+    int minExpandRow = std::max(0, minRow - detectionParams.expansionSize2);
+    int maxExpandRow = std::min(rows - 1, maxRow + detectionParams.expansionSize2);
+    int minExpandCol = std::max(0, minCol - detectionParams.expansionSize2);
+    int maxExpandCol = std::min(cols - 1, maxCol + detectionParams.expansionSize2);
+
+    minRow -= detectionParams.expansionSize1;
+    maxRow += detectionParams.expansionSize1;
+    minCol -= detectionParams.expansionSize1;
+    maxCol += detectionParams.expansionSize1;
+
+    std::vector<SVzNLPointXYZ> expandedPoints;
+    for (int r = minExpandRow; r <= maxExpandRow; r++) {
+        for (int c = minExpandCol; c <= maxExpandCol; c++) {
+            if (r >= minRow && r <= maxRow && c >= minCol && c <= maxCol) {
+                continue;
+            }
+            int idx = r * cols + c;
+            const SVzNLPointXYZ& p = points[idx];
+            if (!IsValidPoint(p)) continue;
+            expandedPoints.push_back(p);
+        }
+    }
+
+    if (debugCallbacks && debugCallbacks->onExpandedRegion) {
+        debugCallbacks->onExpandedRegion(
+            extremePoints.data(),
+            static_cast<int>(extremePoints.size()),
+            expandedPoints.data(),
+            static_cast<int>(expandedPoints.size()),
+            debugCallbacks->userData
+        );
+    }
+
+    double planeA = 0.0;
+    double planeB = 0.0;
+    double planeC = 0.0;
+    SVzNL3DPointF normal;
+    normal.x = 0.0f;
+    normal.y = 0.0f;
+    normal.z = 1.0f;
+
+    if (expandedPoints.size() >= 3) {
+        double sx = 0, sy = 0, sz = 0;
+        double sxx = 0, syy = 0, sxy = 0;
+        double sxz = 0, syz = 0;
+        double n = static_cast<double>(expandedPoints.size());
+
+        for (const auto& p : expandedPoints) {
+            double x = p.x, y = p.y, z = p.z;
+            sx += x; sy += y; sz += z;
+            sxx += x * x; syy += y * y; sxy += x * y;
+            sxz += x * z; syz += y * z;
+        }
+
+        double det = sxx * (syy * n - sy * sy) - sxy * (sxy * n - sx * sy) + sx * (sxy * sy - syy * sx);
+
+        if (std::abs(det) > 1e-12) {
+            planeA = (sxz * (syy * n - sy * sy) - sxy * (syz * n - sy * sz) + sx * (syz * sy - syy * sz)) / det;
+            planeB = (sxx * (syz * n - sy * sz) - sxz * (sxy * n - sx * sy) + sx * (sxy * sz - syz * sx)) / det;
+            planeC = (sxx * (syy * sz - syz * sy) - sxy * (sxy * sz - syz * sx) + sxz * (sxy * sy - syy * sx)) / det;
+
+            float nx = static_cast<float>(-planeA);
+            float ny = static_cast<float>(-planeB);
+            float nz = 1.0f;
+            float norm = std::sqrt(nx * nx + ny * ny + nz * nz);
+            normal.x = nx / norm;
+            normal.y = ny / norm;
+            normal.z = nz / norm;
+        }
+    }
+
+    {
+        SVzNL3DPointF epNormal;
+        float epD;
+        int epErrCode = 0;
+        int epRet = FitPlane(
+            clusterPoints.data(),
+            static_cast<int>(clusterPoints.size()),
+            &epNormal, &epD, &epErrCode
+        );
+
+        if (epRet == HD_SUCCESS) {
+            float maxResidual = 0.0f;
+            for (const auto& p : clusterPoints) {
+                float residual = std::abs(
+                    epNormal.x * p.x + epNormal.y * p.y + epNormal.z * p.z + epD
+                );
+                if (residual > maxResidual) maxResidual = residual;
+            }
+            std::cout << "    [DIAG P0] planeResidual=" << maxResidual
+                      << " (threshold=" << filterParams.maxPlaneResidual << ")" << std::endl;
+            if (maxResidual > filterParams.maxPlaneResidual) {
+                *errCode = HD_ERR_NO_VALID_HOLES;
+                return HD_ERR_NO_VALID_HOLES;
+            }
+        }
+    }
+
+    SVzNL3DPointF basisU;
+    SVzNL3DPointF basisV;
+    {
+        float nx = normal.x, ny = normal.y, nz = normal.z;
+        float tx, ty, tz;
+        if (std::abs(nx) < 0.9f) {
+            tx = 1.0f; ty = 0.0f; tz = 0.0f;
+        } else {
+            tx = 0.0f; ty = 1.0f; tz = 0.0f;
+        }
+        float dot = tx * nx + ty * ny + tz * nz;
+        float ux = tx - dot * nx;
+        float uy = ty - dot * ny;
+        float uz = tz - dot * nz;
+        float uLen = std::sqrt(ux * ux + uy * uy + uz * uz);
+        basisU.x = ux / uLen;
+        basisU.y = uy / uLen;
+        basisU.z = uz / uLen;
+        basisV.x = ny * basisU.z - nz * basisU.y;
+        basisV.y = nz * basisU.x - nx * basisU.z;
+        basisV.z = nx * basisU.y - ny * basisU.x;
+    }
+
+    float centroidX = 0.0f;
+    float centroidY = 0.0f;
+    float centroidZ = 0.0f;
+    for (const auto& p : clusterPoints) {
+        centroidX += p.x;
+        centroidY += p.y;
+        centroidZ += p.z;
+    }
+    centroidX /= clusterPoints.size();
+    centroidY /= clusterPoints.size();
+    centroidZ /= clusterPoints.size();
+
+    std::vector<SVzNLPointXYZ> projectedPoints(clusterPoints.size());
+    for (size_t i = 0; i < clusterPoints.size(); i++) {
+        float dx = clusterPoints[i].x - centroidX;
+        float dy = clusterPoints[i].y - centroidY;
+        float dz = clusterPoints[i].z - centroidZ;
+        projectedPoints[i].x = dx * basisU.x + dy * basisU.y + dz * basisU.z;
+        projectedPoints[i].y = dx * basisV.x + dy * basisV.y + dz * basisV.z;
+        projectedPoints[i].z = 0.0f;
+    }
+
+    SVzNL3DPointF centerF;
+    float radius = 0.0f;
+    float eccentricity = 0.0f;
+
+    int fitRet = FitEllipse(
+        projectedPoints.data(),
+        static_cast<int>(projectedPoints.size()),
+        &centerF,
+        &radius,
+        &eccentricity,
+        errCode
+    );
+
+    std::cout << "    [DIAG FitEllipse] fitRet=" << fitRet
+              << ", radius=" << radius
+              << ", ecc=" << eccentricity << std::endl;
+
+    if (fitRet != 0) {
+        std::cout << "    [DIAG] FitEllipse failed with fitRet=" << fitRet << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    float aspectRatio = 0.0f;
+    if (eccentricity < 1.0f) {
+        aspectRatio = std::sqrt(1.0f - eccentricity * eccentricity);
+    } else {
+        std::cout << "    -> FAILED: eccentricity " << eccentricity << " >= 1.0 (degenerate case)" << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    std::cout << "    [DIAG] aspectRatio=" << aspectRatio << " (threshold: 0.8)" << std::endl;
+
+    if (aspectRatio < 0.5f) {
+        std::cout << "    -> FAILED: aspectRatio " << aspectRatio << " < 0.5 (too elongated)" << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    float projCenterX = centerF.x;
+    float projCenterY = centerF.y;
+
+    SVzNLPointXYZ center;
+    center.x = centroidX + projCenterX * basisU.x + projCenterY * basisV.x;
+    center.y = centroidY + projCenterX * basisU.y + projCenterY * basisV.y;
+    center.z = centroidZ + projCenterX * basisU.z + projCenterY * basisV.z;
+
+    if (radius < detectionParams.minRadius || radius > detectionParams.maxRadius) {
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    std::vector<float> pointDistances(projectedPoints.size());
+    for (size_t i = 0; i < projectedPoints.size(); i++) {
+        float dx = projectedPoints[i].x - projCenterX;
+        float dy = projectedPoints[i].y - projCenterY;
+        pointDistances[i] = std::sqrt(dx * dx + dy * dy);
+    }
+
+    float distThreshold = 0.3f * radius;
+    float sumRadiusDiff2 = 0.0f;
+    int validCount = 0;
+    for (size_t i = 0; i < projectedPoints.size(); i++) {
+        if (pointDistances[i] < distThreshold) {
+            continue;
+        }
+        float diff = pointDistances[i] - radius;
+        sumRadiusDiff2 += diff * diff;
+        validCount++;
+    }
+    float radiusVariance = (validCount > 0)
+        ? std::sqrt(sumRadiusDiff2 / validCount)
+        : std::sqrt(sumRadiusDiff2 / projectedPoints.size());
+
+    float minAngle = 360.0f;
+    float maxAngle = 0.0f;
+    for (const auto& p : projectedPoints) {
+        float dx = p.x - projCenterX;
+        float dy = p.y - projCenterY;
+        float angle = std::atan2(dy, dx) * 180.0f / 3.14159265f;
+        if (angle < 0) angle += 360.0f;
+        if (angle < minAngle) minAngle = angle;
+        if (angle > maxAngle) maxAngle = angle;
+    }
+    float angularSpan = maxAngle - minAngle;
+
+    center.z = static_cast<float>(planeA * center.x + planeB * center.y + planeC);
+
+    float sumBoundaryZ = 0.0f;
+    for (const auto& p : clusterPoints) {
+        sumBoundaryZ += p.z;
+    }
+    float meanBoundaryZ = sumBoundaryZ / clusterPoints.size();
+    float depth = std::abs(meanBoundaryZ - center.z);
+
+    float qualityScore = ComputeQualityScore(
+        eccentricity,
+        radiusVariance,
+        angularSpan,
+        clusterPoints.size()
+    );
+
+    {
+        float angularCoverageDbg = ComputeAngularCoverage(
+            projectedPoints.data(),
+            static_cast<int>(projectedPoints.size()),
+            projCenterX, projCenterY
+        );
+        float cornerRatioDbg = ComputeRectangularityScore(
+            projectedPoints.data(),
+            static_cast<int>(projectedPoints.size())
+        );
+        float inlierRatioDbg = ComputeEllipseInlierRatio(
+            projectedPoints.data(),
+            static_cast<int>(projectedPoints.size()),
+            projCenterX, projCenterY, radius
+        );
+        std::cout << "    [DIAG] pts=" << clusterPoints.size()
+                  << " radius=" << radius
+                  << " ecc=" << eccentricity
+                  << " radVar=" << radiusVariance
+                  << " radFitRatio=" << (radius > 1e-6f ? radiusVariance / radius : 0.0f)
+                  << " angSpan=" << angularSpan
+                  << " angCov=" << angularCoverageDbg
+                  << " maxGap=" << (360.0f - angularCoverageDbg)
+                  << " cornerR=" << cornerRatioDbg
+                  << " inlierR=" << inlierRatioDbg
+                  << " quality=" << qualityScore
+                  << std::endl;
+    }
+
+    if (eccentricity > filterParams.maxEccentricity) {
+        std::cout << "    -> FAILED: eccentricity " << eccentricity
+                  << " > maxEccentricity " << filterParams.maxEccentricity << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    float angularCoverage = ComputeAngularCoverage(
+        projectedPoints.data(),
+        static_cast<int>(projectedPoints.size()),
+        projCenterX,
+        projCenterY
+    );
+    if (angularCoverage < filterParams.minAngularCoverage) {
+        std::cout << "    -> FAILED: angularCoverage " << angularCoverage
+                  << " < minAngularCoverage " << filterParams.minAngularCoverage << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    float maxAngularGap = 360.0f - angularCoverage;
+    if (maxAngularGap > filterParams.maxAngularGap) {
+        std::cout << "    -> FAILED: maxAngularGap " << maxAngularGap
+                  << " > maxAngularGap " << filterParams.maxAngularGap << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    float inlierRatio = ComputeEllipseInlierRatio(
+        projectedPoints.data(),
+        static_cast<int>(projectedPoints.size()),
+        projCenterX,
+        projCenterY,
+        radius
+    );
+    if (inlierRatio < filterParams.minInlierRatio) {
+        std::cout << "    -> FAILED: inlierRatio " << inlierRatio
+                  << " < minInlierRatio " << filterParams.minInlierRatio << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    if (qualityScore < filterParams.minQualityScore) {
+        std::cout << "    -> FAILED: qualityScore " << qualityScore
+                  << " < minQualityScore " << filterParams.minQualityScore << std::endl;
+        *errCode = HD_ERR_NO_VALID_HOLES;
+        return HD_ERR_NO_VALID_HOLES;
+    }
+
+    hole.center.x = center.x;
+    hole.center.y = center.y;
+    hole.center.z = center.z;
+    hole.radius = radius;
+    hole.eccentricity = eccentricity;
+    hole.radiusVariance = radiusVariance;
+    hole.angularSpan = angularSpan;
+    hole.depth = depth;
+    hole.normal = normal;
+    hole.qualityScore = qualityScore;
+
+    *errCode = HD_SUCCESS;
+    return HD_SUCCESS;
+}
+
+}  // namespace internal
+}  // namespace hole_detection

Algo/DetectHole/src/HoleDetectionInternal.h

@@ -0,0 +1,102 @@
+#ifndef HOLE_DETECTION_INTERNAL_H
+#define HOLE_DETECTION_INTERNAL_H
+
+#include "HoleDetection.h"
+
+#include <vector>
+
+namespace hole_detection {
+namespace internal {
+
+enum class EPanelType {
+    YZ_PANEL,
+    XZ_PANEL
+};
+
+typedef void (*LineEvaluationCallback)(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    int startIdx,
+    int step,
+    int count,
+    const std::vector<SHoleBoundaryPoint>& newBoundaryPoints,
+    const char* lineType,
+    int lineIndex,
+    void* userData
+);
+
+bool IsValidPoint(const SVzNLPointXYZ& pt);
+
+void EvaluateLine(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    int startIdx,
+    int step,
+    int count,
+    const SHoleDetectionParams& params,
+    std::vector<SSegmentPair>& segmentPairs,
+    EPanelType panelType
+);
+
+int DetectPitBoundaries(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const SHoleDetectionParams& params,
+    std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    std::vector<std::vector<int>>& clusters,
+    int* errCode,
+    LineEvaluationCallback callback,
+    void* userData,
+    const SHoleDetectionDebugCallbacks* debugCallbacks
+);
+
+int ExtractClusterExtremePoints(
+    const std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    const std::vector<int>& clusterIndices,
+    std::vector<SHoleBoundaryPoint>& extremePoints,
+    int* errCode
+);
+
+int ExpandClusterBoundingBox(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const SHoleDetectionParams& params,
+    std::vector<unsigned char>& isBoundary,
+    std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    std::vector<SHoleBoundaryPoint>& clusterPoints,
+    int expandSize,
+    int* errCode
+);
+
+int FitHolesFromClusters(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const std::vector<SHoleBoundaryPoint>& boundaryPoints,
+    const std::vector<std::vector<int>>& clusters,
+    const SHoleDetectionParams& detectionParams,
+    const SHoleFilterParams& filterParams,
+    std::vector<SHoleResult>& candidateHoles,
+    int* errCode
+);
+
+int FitHoleFromExtremePoints(
+    const SVzNLPointXYZ* points,
+    int rows,
+    int cols,
+    const std::vector<SHoleBoundaryPoint>& extremePoints,
+    const SHoleDetectionParams& detectionParams,
+    const SHoleFilterParams& filterParams,
+    SHoleResult& hole,
+    int* errCode,
+    const SHoleDetectionDebugCallbacks* debugCallbacks
+);
+
+}  // namespace internal
+}  // namespace hole_detection
+
+#endif  // HOLE_DETECTION_INTERNAL_H