From e9ef8d2cc9dfcd83ea6168ef1d3f5bbd57b95c91 Mon Sep 17 00:00:00 2001 From: MaJunwei Date: Thu, 12 Mar 2026 15:45:50 +0800 Subject: [PATCH] =?UTF-8?q?feat(=E5=8F=AF=E8=A7=86=E5=8C=96):=20=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=E7=BA=BF=E6=AE=B5=E7=AB=AF=E7=82=B9=E5=AF=B9=E5=8F=AF?= =?UTF-8?q?=E8=A7=86=E5=8C=96=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit refactor(参数): 简化孔洞检测和过滤参数结构 新增线段端点对检测回调函数和可视化方法，用于显示检测到的线段对。同时简化了孔洞检测参数结构，移除了不常用的聚类参数和半径过滤参数，使接口更加简洁。 修改了线段端点对结构体名称从SPeakValleyPair改为更准确的SSegmentPair，并更新了相关代码。添加了.gitignore文件忽略.ace-tool目录。 --- .gitignore | 4 +- Algo/DetectHole/.gitignore | 1 + .../sample_hole_detection.cpp | 5 - .../HoleDetectionVisualizer.cpp | 145 +++ .../HoleDetectionVisualizer.h | 17 + .../visualization_demo/VisualizationDemo.cpp | 40 +- Algo/DetectHole/src/HoleDetection.cpp | 870 +++++------------- Algo/DetectHole/src/HoleDetection.h | 8 + Algo/DetectHole/src/HoleDetectionParams.h | 200 ++-- 9 files changed, 492 insertions(+), 798 deletions(-) create mode 100644 Algo/DetectHole/.gitignore diff --git a/.gitignore b/.gitignore index 329e8f5..03001c0 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ .claude nul -Algo\DetectHole\Export -build \ No newline at end of file +build +Algo/DetectHole/Export/ diff --git a/Algo/DetectHole/.gitignore b/Algo/DetectHole/.gitignore new file mode 100644 index 0000000..b4a7d40 --- /dev/null +++ b/Algo/DetectHole/.gitignore @@ -0,0 +1 @@ +.ace-tool/ diff --git a/Algo/DetectHole/examples/hole_detection_demo/sample_hole_detection.cpp b/Algo/DetectHole/examples/hole_detection_demo/sample_hole_detection.cpp index 8fde9a6..1ce4136 100644 --- a/Algo/DetectHole/examples/hole_detection_demo/sample_hole_detection.cpp +++ b/Algo/DetectHole/examples/hole_detection_demo/sample_hole_detection.cpp @@ -169,13 +169,9 @@ int main(int argc, char* argv[]) { detectionParams.minRadius = 5.0f; // 最小半径 5mm detectionParams.maxRadius = 50.0f; // 最大半径 50mm detectionParams.minPitDepth = 1.0f; // 最小深度 1mm - detectionParams.clusterEps = 5.0f; // 聚类距离 5mm - detectionParams.clusterMinPoints = 5; // 最小聚类点数 // 设置过滤参数 SHoleFilterParams filterParams; - filterParams.minHoleRadius = 3.0f; // 最小孔洞半径 3mm - filterParams.maxHoleRadius = 100.0f; // 最大孔洞半径 100mm filterParams.maxEccentricity = 0.9f; // 最大离心率 filterParams.minQualityScore = 0.3f; // 最小质量分数 @@ -183,7 +179,6 @@ int main(int argc, char* argv[]) { std::cout << " 半径范围: " << detectionParams.minRadius << " - " << detectionParams.maxRadius << " mm" << std::endl; std::cout << " 最小深度: " << detectionParams.minPitDepth << " mm" << std::endl; - std::cout << " 聚类距离: " << detectionParams.clusterEps << " mm" << std::endl; std::cout << std::endl; // 执行孔洞检测 diff --git a/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.cpp b/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.cpp index 4336e7b..bd5b7cd 100644 --- a/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.cpp +++ b/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.cpp @@ -244,6 +244,151 @@ void HoleDetectionVisualizer::VisualizeBoundaryPoints( } } +void HoleDetectionVisualizer::VisualizeSegmentPairs( + const SVzNLPointXYZ* points, + int rows, + int cols, + const std::vector& segmentPairs, + const std::string& title) +{ + int totalPoints = rows * cols; + + // Create original point cloud (gray) + vtkSmartPointer vtkPointsOrig = vtkPoints::New(); + for (int i = 0; i < totalPoints; i++) { + if (std::isfinite(points[i].x) && std::isfinite(points[i].y) && std::isfinite(points[i].z)) { + vtkPointsOrig->InsertNextPoint(points[i].x, points[i].y, points[i].z); + } + } + + vtkSmartPointer polyDataOrig = vtkPolyData::New(); + polyDataOrig->SetPoints(vtkPointsOrig); + + vtkSmartPointer vertexFilterOrig = vtkVertexGlyphFilter::New(); + vertexFilterOrig->SetInputData(polyDataOrig); + vertexFilterOrig->Update(); + + vtkSmartPointer mapperOrig = vtkPolyDataMapper::New(); + mapperOrig->SetInputConnection(vertexFilterOrig->GetOutputPort()); + + vtkSmartPointer actorOrig = vtkActor::New(); + actorOrig->SetMapper(mapperOrig); + actorOrig->GetProperty()->SetColor(0.8, 0.8, 0.8); // Gray + actorOrig->GetProperty()->SetPointSize(2); + + // Create renderer + vtkSmartPointer renderer = vtkRenderer::New(); + renderer->AddActor(actorOrig); + renderer->SetBackground(0.1, 0.1, 0.1); + + // Generate colors for each segment pair + std::vector colors = { + 1.0, 0.0, 0.0, // Red + 0.0, 1.0, 0.0, // Green + 0.0, 0.0, 1.0, // Blue + 1.0, 1.0, 0.0, // Yellow + 1.0, 0.0, 1.0, // Magenta + 0.0, 1.0, 1.0, // Cyan + 1.0, 0.5, 0.0, // Orange + 0.5, 0.0, 1.0, // Purple + 0.0, 1.0, 0.5, // Spring Green + 1.0, 0.0, 0.5 // Rose + }; + + // Visualize each segment pair with different color + for (size_t i = 0; i < segmentPairs.size(); i++) { + const SSegmentPair& pair = segmentPairs[i]; + + // Choose color (cycle through colors) + int colorIdx = (i % (colors.size() / 3)) * 3; + double r = colors[colorIdx]; + double g = colors[colorIdx + 1]; + double b = colors[colorIdx + 2]; + + // Create start point sphere + vtkSmartPointer startSphere = vtkSphereSource::New(); + startSphere->SetCenter(pair.startPoint.x, pair.startPoint.y, pair.startPoint.z); + startSphere->SetRadius(0.5); + startSphere->SetPhiResolution(10); + startSphere->SetThetaResolution(10); + + vtkSmartPointer startMapper = vtkPolyDataMapper::New(); + startMapper->SetInputConnection(startSphere->GetOutputPort()); + + vtkSmartPointer startActor = vtkActor::New(); + startActor->SetMapper(startMapper); + startActor->GetProperty()->SetColor(r, g, b); + + // Create end point sphere + vtkSmartPointer endSphere = vtkSphereSource::New(); + endSphere->SetCenter(pair.endPoint.x, pair.endPoint.y, pair.endPoint.z); + endSphere->SetRadius(0.5); + endSphere->SetPhiResolution(10); + endSphere->SetThetaResolution(10); + + vtkSmartPointer endMapper = vtkPolyDataMapper::New(); + endMapper->SetInputConnection(endSphere->GetOutputPort()); + + vtkSmartPointer endActor = vtkActor::New(); + endActor->SetMapper(endMapper); + endActor->GetProperty()->SetColor(r, g, b); + + // Create line connecting start and end points + vtkSmartPointer line = vtkLineSource::New(); + line->SetPoint1(pair.startPoint.x, pair.startPoint.y, pair.startPoint.z); + line->SetPoint2(pair.endPoint.x, pair.endPoint.y, pair.endPoint.z); + + vtkSmartPointer lineMapper = vtkPolyDataMapper::New(); + lineMapper->SetInputConnection(line->GetOutputPort()); + + vtkSmartPointer lineActor = vtkActor::New(); + lineActor->SetMapper(lineMapper); + lineActor->GetProperty()->SetColor(r, g, b); + lineActor->GetProperty()->SetLineWidth(2); + + // Add actors to renderer + renderer->AddActor(startActor); + renderer->AddActor(endActor); + renderer->AddActor(lineActor); + } + + // Create text annotation + vtkSmartPointer textActor = vtkTextActor::New(); + std::ostringstream oss; + oss << "Segment Pairs: " << segmentPairs.size(); + textActor->SetInput(oss.str().c_str()); + textActor->SetPosition(10, 10); + textActor->GetTextProperty()->SetFontSize(20); + textActor->GetTextProperty()->SetColor(1.0, 1.0, 1.0); + renderer->AddActor2D(textActor); + + renderer->ResetCamera(); + + // Create render window + vtkSmartPointer renderWindow = vtkRenderWindow::New(); + renderWindow->AddRenderer(renderer); + renderWindow->SetSize(1024, 768); + renderWindow->SetWindowName(title.c_str()); + + if (m_interactive) { + vtkSmartPointer interactor = + vtkSmartPointer::New(); + interactor->SetRenderWindow(renderWindow); + + // Set trackball camera interaction style + vtkSmartPointer style = + vtkSmartPointer::New(); + interactor->SetInteractorStyle(style); + + renderWindow->Render(); + interactor->Initialize(); + interactor->Start(); + } else { + renderWindow->Render(); + SaveScreenshot(renderer, title + ".png"); + } +} + void HoleDetectionVisualizer::VisualizeClusters( const SVzNLPointXYZ* points, int rows, diff --git a/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.h b/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.h index b0a8166..d86bff6 100644 --- a/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.h +++ b/Algo/DetectHole/examples/visualization_demo/HoleDetectionVisualizer.h @@ -54,6 +54,23 @@ public: const std::string& title = "Detected Boundary Points" ); + /** + * @brief Visualize segment endpoint pairs with different colors + * + * @param points Original point cloud + * @param rows Number of rows + * @param cols Number of columns + * @param segmentPairs Detected segment pairs + * @param title Window title + */ + void VisualizeSegmentPairs( + const SVzNLPointXYZ* points, + int rows, + int cols, + const std::vector& segmentPairs, + const std::string& title = "Detected Segment Pairs" + ); + /** * @brief Visualize clustered boundary points with different colors * diff --git a/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp b/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp index 5841241..dab78a4 100644 --- a/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp +++ b/Algo/DetectHole/examples/visualization_demo/VisualizationDemo.cpp @@ -270,6 +270,33 @@ void OnHoleFitted(const SHoleResult* hole, int holeIndex, void* userData) { delete[] tempResult.holes; } +// Callback: Segment pairs detected +void OnSegmentPairsDetected(const SSegmentPair* segmentPairs, int count, void* userData) { + CallbackUserData* data = static_cast(userData); + if (!data || !data->enableVisualization || !data->visualizer) { + return; + } + + std::cout << "[DEBUG] Segment pairs detected: " << count << std::endl; + + // Convert to vector for visualization + std::vector pairs; + pairs.reserve(count); + for (int i = 0; i < count; i++) { + pairs.push_back(segmentPairs[i]); + } + + // Visualize segment endpoint pairs + data->visualizer->VisualizeSegmentPairs( + data->points, + data->rows, + data->cols, + pairs, + "Step 0: Detected Segment Endpoint Pairs" + ); +} + + // Process a single file int ProcessSingleFile(const std::string& inputFile, bool interactive, @@ -328,10 +355,11 @@ int ProcessSingleFile(const std::string& inputFile, callbackData.enableVisualization = interactive; // Only enable visualization in interactive mode SHoleDetectionDebugCallbacks debugCallbacks; - debugCallbacks.onBoundaryDetected = OnBoundaryDetected; - debugCallbacks.onClustersFound = OnClustersFound; - debugCallbacks.onExpandedRegion = OnExpandedRegion; - debugCallbacks.onHoleFitted = OnHoleFitted; + debugCallbacks.onBoundaryDetected = nullptr;// OnBoundaryDetected; + debugCallbacks.onClustersFound = nullptr;// OnClustersFound; + debugCallbacks.onExpandedRegion = nullptr;// OnExpandedRegion; + debugCallbacks.onHoleFitted = nullptr;// OnHoleFitted; + debugCallbacks.onSegmentPairsDetected = nullptr;//OnSegmentPairsDetected; debugCallbacks.userData = &callbackData; SMultiHoleResult result; @@ -495,12 +523,8 @@ int main(int argc, char* argv[]) { std::cout << " angleThresholdPos: " << detectionParams.angleThresholdPos << "°" << std::endl; std::cout << " angleThresholdNeg: " << detectionParams.angleThresholdNeg << "°" << std::endl; std::cout << " minPitDepth: " << detectionParams.minPitDepth << " mm" << std::endl; - std::cout << " clusterEps: " << detectionParams.clusterEps << " mm" << std::endl; - std::cout << " clusterMinPoints: " << detectionParams.clusterMinPoints << std::endl; std::cout << "\nFilter Parameters:" << std::endl; - std::cout << " minHoleRadius: " << filterParams.minHoleRadius << " mm" << std::endl; - std::cout << " maxHoleRadius: " << filterParams.maxHoleRadius << " mm" << std::endl; std::cout << " maxEccentricity: " << filterParams.maxEccentricity << std::endl; int result = ProcessSingleFile(inputPath, interactive, outputDir, showLines, diff --git a/Algo/DetectHole/src/HoleDetection.cpp b/Algo/DetectHole/src/HoleDetection.cpp index d9c0af0..10031e8 100644 --- a/Algo/DetectHole/src/HoleDetection.cpp +++ b/Algo/DetectHole/src/HoleDetection.cpp @@ -10,44 +10,6 @@ #include #include -#define _SELF_VER 1 - -// ========== 内部类型定义 ========== -typedef struct -{ - int pntIdx; - int type; - double forwardAngle; //前向角 - double backwardAngle; //后向角 - double corner; //拐角 - double forwardDiffZ; - double backwardDiffZ; - double pre_stepDist; - double post_stepDist; - double forward_z; - double backward_z; -}SSG_pntDirAngle; - -typedef struct -{ - double minEndingGap; //y方向连续段门限。大于此门限，为不连续 - double minEndingGap_z; //z方向连续段门限。大于此门限，为不连续 - double scale; //计算方向角的窗口比例尺 - double cornerTh; //拐角门限，大于此门限，为有效拐点 - double jumpCornerTh_1; //判断拐角是否为跳变的两个门限。当一个门限大于jumpCornerTh_1且另一个小于jumpCornerTh_2时跳变 - double jumpCornerTh_2; -}SSG_cornerParam; - -typedef struct -{ - int featureType; - SVzNL2DPoint jumpPos2D; - SVzNL3DPoint jumpPos; - double featureValue; -}SSG_basicFeature1D; - -const float kPi = 3.14159265f; - // ========== 内部函数前向声明 ========== /** * @brief Internal callback function type for line evaluation (not exported) @@ -75,7 +37,7 @@ static void EvaluateLine( const SHoleDetectionParams& params, int rows, int cols, - std::vector& peakValleyPairs + std::vector& peakValleyPairs ); static int DetectPitBoundaries( const SVzNLPointXYZ* points, @@ -86,435 +48,10 @@ static int DetectPitBoundaries( std::vector>& clusters, int* errCode, LineEvaluationCallback callback, - void* userData + void* userData, + const SHoleDetectionDebugCallbacks* debugCallbacks = nullptr ); -//提取corner极值（较早实现函数可以使用此函数进行代码优化） -void _searchCornerPeaks( - std::vector< SSG_pntDirAngle>& corners, - std::vector< SVzNL3DPosition>& vldPts, - const SSG_cornerParam cornerPara, - double cornerMergeScale, - std::vector< SSG_pntDirAngle>& cornerPeakP, - std::vector< SSG_pntDirAngle>& cornerPeakM -) -{ - int cornerSize = (int)corners.size(); - std::vector< SSG_pntDirAngle> cornerPk_P; - std::vector< SSG_pntDirAngle> cornerPk_M; - //搜索拐角极值 - int _state = 0; - int pre_i = -1; - int sEdgePtIdx = -1; - int eEdgePtIdx = -1; - SSG_pntDirAngle* pre_data = NULL; - for (int i = 0, i_max = cornerSize; i < i_max; i++) - { - if (i == 275) - int kkk = 1; - SSG_pntDirAngle* curr_data = &corners[i]; - if (curr_data->pntIdx < 0) - continue; - - if (curr_data->pntIdx < 0) - { - if (i == i_max - 1) //最后一个 - { - if (1 == _state) //上升 - { - cornerPk_P.push_back(corners[eEdgePtIdx]); - } - else if (2 == _state) //下降 - { - cornerPk_M.push_back(corners[eEdgePtIdx]); - } - } - continue; - } - - if (NULL == pre_data) - { - sEdgePtIdx = i; - eEdgePtIdx = i; - pre_data = curr_data; - pre_i = i; - continue; - } - - eEdgePtIdx = i; - double cornerDiff = curr_data->corner - pre_data->corner; - switch (_state) - { - case 0: //初态 - if (cornerDiff < 0) //下降 - { - _state = 2; - } - else if (cornerDiff > 0) //上升 - { - _state = 1; - } - break; - case 1: //上升 - if (cornerDiff < 0) //下降 - { - if (pre_data->corner > 0) - cornerPk_P.push_back(*pre_data); - _state = 2; - } - break; - case 2: //下降 - if (cornerDiff > 0) // 上升 - { - if (pre_data->corner < 0) - cornerPk_M.push_back(*pre_data); - _state = 1; - } - break; - default: - _state = 0; - break; - } - pre_data = curr_data; - pre_i = i; - } - //注意：最后一个不处理，为基座位置 - - //极小值点（峰顶） - //极值比较，在尺度窗口下寻找局部极值点 - double square_distTh = cornerMergeScale * cornerMergeScale; //2倍的cornerScale。 - for (int i = 0, i_max = (int)cornerPk_P.size(); i < i_max; i++) - { - if (cornerPk_P[i].corner < cornerPara.cornerTh) - continue; - - bool isPeak = true; - //向前搜索 - int cornerPtIdx = cornerPk_P[i].pntIdx; - for (int j = i - 1; j >= 0; j--) - { - int prePtIdx = cornerPk_P[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; - if (dist > square_distTh) //超出尺度窗口 - break; - - if (cornerPk_P[i].corner < cornerPk_P[j].corner) - { - isPeak = false; - break; - } - } - //向后搜索 - if (true == isPeak) - { - cornerPtIdx = cornerPk_P[i].pntIdx; - for (int j = i + 1; j < i_max; j++) - { - int postPtIdx = cornerPk_P[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); - if (dist > square_distTh) //超出尺度窗口 - break; - - if (cornerPk_P[i].corner < cornerPk_P[j].corner) - { - isPeak = false; - break; - } - } - } - if (true == isPeak) - cornerPeakP.push_back(cornerPk_P[i]); - } - - for (int i = 0, i_max = (int)cornerPk_M.size(); i < i_max; i++) - { - if (abs(cornerPk_M[i].corner) < cornerPara.cornerTh) - continue; - - bool isPeak = true; - //向前搜索 - int cornerPtIdx = cornerPk_M[i].pntIdx; - for (int j = i - 1; j >= 0; j--) - { - int prePtIdx = cornerPk_M[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; - if (dist > square_distTh) //超出尺度窗口 - break; - - if (abs(cornerPk_M[i].corner) < abs(cornerPk_M[j].corner)) - { - isPeak = false; - break; - } - } - //向后搜索 - if (true == isPeak) - { - cornerPtIdx = cornerPk_M[i].pntIdx; - for (int j = i + 1; j < i_max; j++) - { - int postPtIdx = cornerPk_M[j].pntIdx; - double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); - if (dist > square_distTh) //超出尺度窗口 - break; - - if (abs(cornerPk_M[i].corner) < abs(cornerPk_M[j].corner)) - { - isPeak = false; - break; - } - } - } - if (true == isPeak) - cornerPeakM.push_back(cornerPk_M[i]); - } -} - -bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { - return a.pntIdx < b.pntIdx; -} - -#define LINE_FEATURE_NUM 16 -#define LINE_FEATURE_UNDEF 0 -#define LINE_FEATURE_L_JUMP_H2L 1 -#define LINE_FEATURE_L_JUMP_L2H 2 -#define LINE_FEATURE_L_SLOPE_H2L 3 -#define LINE_FEATURE_L_SLOPE_L2H 4 -#define LINE_FEATURE_V_SLOPE 5 -#define LINE_FEATURE_LINE_ENDING_0 6 //ending起点 -#define LINE_FEATURE_LINE_ENDING_1 7 //ending终点 -#define LINE_FEATURE_RGN_EDGE 8 //迭代处理时已经得到的目标的边缘点标记 -#define LINE_FEATURE_RIGHT_ANGLE_HR 9 //直角特征:水平-上升 -#define LINE_FEATURE_RIGHT_ANGLE_HF 10 //直角特征:水平-下降 -#define LINE_FEATURE_RIGHT_ANGLE_RH 11 //直角特征:上升-水平 -#define LINE_FEATURE_RIGHT_ANGLE_FH 12 //直角特征:下降-水平 -#define LINE_FEATURE_PEAK_TOP 13 -#define LINE_FEATURE_PEAK_BOTTOM 14 -#define LINE_FEATURE_CORNER_V 15 - -/// -/// 提取激光线上的Z形Jumping特征, 用于搭接焊缝 -/// seg端点：z距离大于门限 -/// nPointIdx被重新定义成Feature类型 -/// 算法流程： -/// （1）逐点计算前向角和后向角 -/// （2）逐点计算拐角，顺时针为负，逆时针为正 -/// （3）搜索正拐角的极大值。 -/// （4）判断拐角是否为跳变 -/// -static void sg_getLineZJumpFeature_cornerMethod( - const SVzNLPointXYZ* points, - int startIdx, - int step, - int count, - const SHoleDetectionParams& params, - int rows, - int cols, - std::vector& isBoundary, - std::vector& boundaryPoints, - std::vector* newBoundaryPoints) -{ - if (count < 3) return; - - // Clear new boundary points if provided - if (newBoundaryPoints) { - newBoundaryPoints->clear(); - } - - // Helper: Mark a point as boundary - auto markBoundary = [&](const SHoleBoundaryPoint& bp) { - int idx = bp.row * cols + bp.col; - if (idx >= 0 && idx < rows * cols && !isBoundary[idx]) { - isBoundary[idx] = 1; - boundaryPoints.push_back(bp); - if (newBoundaryPoints) { - newBoundaryPoints->push_back(bp); - } - } - }; - - // ========== Step 1: Extract valid points ========== - std::vector validPoints; - validPoints.reserve(count); - - for (int i = 0; i < count; i++) { - int currIdx = startIdx + i * step; - if (IsValidPoint(points[currIdx])) { - int row = currIdx / cols; - int col = currIdx - row * cols; - validPoints.push_back(SHoleBoundaryPoint(points[currIdx], row, col)); - } - } - - // Early exit if not enough valid points - if (validPoints.size() <= 1) { - return; - } - - // Convert to SVzNL3DPosition for compatibility with _searchCornerPeaks - std::vector vldPts; - vldPts.reserve(validPoints.size()); - for (size_t i = 0; i < validPoints.size(); i++) { - SVzNL3DPosition pt; - pt.pt3D.x = validPoints[i].point.x; - pt.pt3D.y = validPoints[i].point.y; - pt.pt3D.z = validPoints[i].point.z; - pt.nPointIdx = static_cast(i); - vldPts.push_back(pt); - } - - // ========== Step 2: Calculate forward and backward angles ========== - std::vector corners; - corners.resize(vldPts.size()); - for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) - { - // Search backward for a point at distance >= cornerScale - int pre_i = -1; - for (int j = i - 1; j >= 0; j--) - { - double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + - pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); - if (dist >= params.cornerScale) - { - pre_i = j; - break; - } - } - // Search forward for a point at distance >= cornerScale - int post_i = -1; - for (int j = i + 1; j < i_max; j++) - { - double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + - pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); - if (dist >= params.cornerScale) - { - post_i = j; - break; - } - } - // Calculate corner angle - if ((pre_i < 0) || (post_i < 0)) - { - corners[i].pntIdx = -1; - corners[i].forwardAngle = 0; - corners[i].backwardAngle = 0; - corners[i].corner = 0; - corners[i].forwardDiffZ = 0; - corners[i].backwardDiffZ = 0; - corners[i].forward_z = 0; - corners[i].backward_z = 0; - } - else - { - double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); - double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); - double forwardAngle = atan(tanValue_post) * 180.0 / kPi; - double backwardAngle = atan(tanValue_pre) * 180.0 / kPi; - corners[i].pntIdx = i; - corners[i].forwardAngle = forwardAngle; - corners[i].backwardAngle = backwardAngle; - corners[i].corner = -(forwardAngle - backwardAngle); - corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; - corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; - corners[i].forward_z = vldPts[post_i].pt3D.z; - corners[i].backward_z = vldPts[pre_i].pt3D.z; - } - } - - // ========== Step 3: Search corner peaks ========== - std::vector cornerPeakP; - std::vector cornerPeakM; - double cornerMergeScale = params.cornerScale * 2; - - SSG_cornerParam cornerPara; - cornerPara.scale = params.cornerScale; - cornerPara.cornerTh = params.cornerAngleThreshold; - cornerPara.jumpCornerTh_1 = params.jumpCornerTh_1; - cornerPara.jumpCornerTh_2 = params.jumpCornerTh_2; - cornerPara.minEndingGap = params.minEndingGap; - cornerPara.minEndingGap_z = params.minEndingGap_z; - - _searchCornerPeaks( - corners, - vldPts, - cornerPara, - cornerMergeScale, - cornerPeakP, - cornerPeakM - ); - - // ========== Step 4: Classify corner features ========== - std::vector cornerFeatures; - for (int i = 0; i < (int)cornerPeakP.size(); i++) - { - if ((abs(cornerPeakP[i].backwardAngle) < params.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > params.jumpCornerTh_2)) - { - cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_HR; - cornerFeatures.push_back(cornerPeakP[i]); - } - else if ((abs(cornerPeakP[i].forwardAngle) < params.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > params.jumpCornerTh_2)) - { - cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_FH; - cornerFeatures.push_back(cornerPeakP[i]); - } - } - for (int i = 0; i < (int)cornerPeakM.size(); i++) - { - if ((abs(cornerPeakM[i].backwardAngle) < params.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > params.jumpCornerTh_2)) - { - cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_HF; - cornerFeatures.push_back(cornerPeakM[i]); - } - else if ((abs(cornerPeakM[i].forwardAngle) < params.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > params.jumpCornerTh_2)) - { - cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_RH; - cornerFeatures.push_back(cornerPeakM[i]); - } - } - - // Sort by index - std::sort(cornerFeatures.begin(), cornerFeatures.end(), compareByIdx); - - // ========== Step 5: Pair jumps and mark boundaries ========== - for (int i = 0, i_max = (int)cornerFeatures.size(); i < i_max - 1; i++) - { - if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HR) - { - if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_RH) - { - int pntIdx_1 = cornerFeatures[i].pntIdx; - int pntIdx_2 = cornerFeatures[i + 1].pntIdx; - // Calculate distance and jump height - double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); - double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z); - if ((dist < params.minEndingGap) && (height > params.minEndingGap_z)) - { - // Mark both boundary points (L2H jump) - markBoundary(validPoints[pntIdx_1]); - markBoundary(validPoints[pntIdx_2]); - } - } - } - else if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HF) - { - if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_FH) - { - int pntIdx_1 = cornerFeatures[i].pntIdx; - int pntIdx_2 = cornerFeatures[i + 1].pntIdx; - // Calculate distance and jump height - double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); - double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z); - if ((dist < params.minEndingGap) && (height > params.minEndingGap_z)) - { - // Mark both boundary points (H2L jump) - markBoundary(validPoints[pntIdx_1]); - markBoundary(validPoints[pntIdx_2]); - } - } - } - } -} - // ========== 内部辅助函数（不导出）========== /** * @brief 检查点的有效性 @@ -598,12 +135,13 @@ static int DetectPitBoundaries( std::vector>& clusters, int* errCode, LineEvaluationCallback callback, - void* userData + 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; + return HD_ERR_INVALID_INPUT; } // 清空输出容器 @@ -622,29 +160,49 @@ static int DetectPitBoundaries( // // 存储每一行的峰谷点对 - std::vector> allRowPeakValleyPairs(rows); + std::vector> allRowSegmentPairs(rows); int startIdx = 0; // 行首索引 for (int row = 0; row < rows; row++) { - std::vector curPeakValleyPairs; -#if _SELF_VER - EvaluateLine(points, startIdx, 1, cols, params, rows, cols, curPeakValleyPairs); -#else - sg_getLineZJumpFeature_cornerMethod(points, startIdx, 1, cols, params, rows, cols, isBoundary, boundaryPoints, &newBoundaryPoints); -#endif + std::vector curSegmentPairs; - allRowPeakValleyPairs[row] = curPeakValleyPairs; + EvaluateLine(points, startIdx, 1, cols, params, rows, cols, curSegmentPairs); + + allRowSegmentPairs[row] = curSegmentPairs; startIdx += cols; } + // ========== 回调：输出所有检测到的 segment 端点对 ========== + if (debugCallbacks && debugCallbacks->onSegmentPairsDetected) { + // 收集所有 segment pairs 到一个向量中 + std::vector 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(allSegmentPairs.size()), + debugCallbacks->userData + ); + } + } + // ========== 保留连续峰谷点存在交集的数据 ========== - // 判断两个峰谷对在列方向上是否有交集 - auto hasColOverlap = [](const SPeakValleyPair& pair1, const SPeakValleyPair& pair2, int tolerance = 0) -> bool { + // 判断两个峰谷对在列方向上是否有交集，并检查交集占比 + auto hasColOverlap = [](const SSegmentPair& pair1, const SSegmentPair& pair2, int tolerance = 0, float minOverlapRatio = 0.6f) -> bool { // 获取两个峰谷对的列范围 - int min1 = std::min(pair1.peakCol, pair1.valleyCol); - int max1 = std::max(pair1.peakCol, pair1.valleyCol); - int min2 = std::min(pair2.peakCol, pair2.valleyCol); - int max2 = std::max(pair2.peakCol, pair2.valleyCol); + 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); + + // 计算原始segment长度 + int length1 = max1 - min1 + 1; + int length2 = max2 - min2 + 1; // 扩展容差范围 min1 -= tolerance; @@ -653,20 +211,34 @@ static int DetectPitBoundaries( max2 += tolerance; // 检查是否有交集 - return !(max1 < min2 || max2 < min1); + if (max1 < min2 || max2 < min1) { + return false; // 没有交集 + } + + // 计算交集大小 + int overlapStart = std::max(min1, min2); + int overlapEnd = std::min(max1, max2); + int overlapSize = overlapEnd - overlapStart + 1; + + // 计算交集占较短segment的百分比 + int minLength = std::min(length1, length2); + float overlapRatio = static_cast(overlapSize) / static_cast(minLength); + + // 检查交集占比是否满足最小阈值 + return overlapRatio >= minOverlapRatio; }; // 标记每个峰谷对是否应该保留 std::vector> shouldKeep(rows); for (int row = 0; row < rows; row++) { - shouldKeep[row].resize(allRowPeakValleyPairs[row].size(), false); + shouldKeep[row].resize(allRowSegmentPairs[row].size(), false); } // ========== 并查集：用于聚类有交集的峰谷对 ========== // 为每个峰谷对分配唯一ID: pairId = row * maxPairsPerRow + pairIndex int maxPairsPerRow = 0; for (int row = 0; row < rows; row++) { - maxPairsPerRow = std::max(maxPairsPerRow, static_cast(allRowPeakValleyPairs[row].size())); + maxPairsPerRow = std::max(maxPairsPerRow, static_cast(allRowSegmentPairs[row].size())); } // 并查集数据结构 @@ -676,7 +248,7 @@ static int DetectPitBoundaries( // 初始化并查集 int totalPairs = 0; for (int row = 0; row < rows; row++) { - totalPairs += static_cast(allRowPeakValleyPairs[row].size()); + totalPairs += static_cast(allRowSegmentPairs[row].size()); } parent.resize(totalPairs); rank.resize(totalPairs, 0); @@ -709,29 +281,36 @@ static int DetectPitBoundaries( }; // 计算峰谷对的全局ID - auto getPairId = [&allRowPeakValleyPairs](int row, int pairIndex) -> int { + auto getPairId = [&allRowSegmentPairs](int row, int pairIndex) -> int { int id = 0; for (int r = 0; r < row; r++) { - id += static_cast(allRowPeakValleyPairs[r].size()); + id += static_cast(allRowSegmentPairs[r].size()); } return id + pairIndex; }; - // 检查连续行之间的交集，并合并到同一cluster - for (int row = 0; row < rows - 1; row++) { - const auto& currentRowPairs = allRowPeakValleyPairs[row]; - const auto& nextRowPairs = allRowPeakValleyPairs[row + 1]; + // 检查多行窗口内的交集，并合并到同一cluster + // 使用多行窗口可以跨越空行连接有交集的segment，避免聚类断裂 + int lookAheadWindow = 3; // 向前看3行，可根据实际情况调整 - 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[row + 1][j] = true; + for (int row = 0; row < rows; row++) { + const auto& currentRowPairs = allRowSegmentPairs[row]; - // 合并到同一cluster - int pairId1 = getPairId(row, static_cast(i)); - int pairId2 = getPairId(row + 1, static_cast(j)); - unite(pairId1, pairId2); + // 检查当前行与后续 lookAheadWindow 行之间的交集 + 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; + + // 合并到同一cluster + int pairId1 = getPairId(row, static_cast(i)); + int pairId2 = getPairId(nextRow, static_cast(j)); + unite(pairId1, pairId2); + } } } } @@ -741,7 +320,7 @@ static int DetectPitBoundaries( std::map> rootToPairIds; // 根节点 -> 该cluster的峰谷对ID列表 for (int row = 0; row < rows; row++) { - const auto& rowPairs = allRowPeakValleyPairs[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(i)); @@ -765,11 +344,11 @@ static int DetectPitBoundaries( int id = 0; bool found = false; for (int r = 0; r < rows && !found; r++) { - const auto& rowPairs = allRowPeakValleyPairs[r]; + const auto& rowPairs = allRowSegmentPairs[r]; for (size_t j = 0; j < rowPairs.size(); j++) { if (id == pairId) { - clusterPoints.push_back(rowPairs[j].peakPoint); - clusterPoints.push_back(rowPairs[j].valleyPoint); + clusterPoints.push_back(rowPairs[j].startPoint); + clusterPoints.push_back(rowPairs[j].endPoint); found = true; break; } @@ -790,21 +369,22 @@ static int DetectPitBoundaries( } } - // 判断最大距离/2是否在半径范围内 - float estimatedRadius = maxDistance / 2.0f; - if (estimatedRadius >= params.minRadius && estimatedRadius <= params.maxRadius) { - validRoots.insert(root); - } else { - std::cout << " -> Cluster filtered: estimatedRadius " << estimatedRadius - << " not in range [" << params.minRadius << ", " << params.maxRadius << "]" << std::endl; - } + validRoots.insert(root); + //// 判断最大距离/2是否在半径范围内 + //float estimatedRadius = maxDistance / 2.0f; + //if (estimatedRadius >= params.minRadius && estimatedRadius <= params.maxRadius) { + // validRoots.insert(root); + //} else { + // std::cout << " -> Cluster filtered: estimatedRadius " << estimatedRadius + // << " not in range [" << params.minRadius << ", " << params.maxRadius << "]" << std::endl; + //} } // 将通过验证的峰谷对转换为边界点，并记录每个峰谷对对应的边界点索引 std::map> pairIdToBoundaryIndices; // pairId -> boundaryPoints索引列表 for (int row = 0; row < rows; row++) { - const auto& rowPairs = allRowPeakValleyPairs[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(i)); @@ -816,18 +396,18 @@ static int DetectPitBoundaries( // 添加峰值点 SHoleBoundaryPoint peakBp; - peakBp.point = pair.peakPoint; - peakBp.row = pair.peakRow; - peakBp.col = pair.peakCol; + peakBp.point = pair.startPoint; + peakBp.row = pair.startRow; + peakBp.col = pair.startCol; int peakIndex = static_cast(boundaryPoints.size()); boundaryPoints.push_back(peakBp); pairIdToBoundaryIndices[pairId].push_back(peakIndex); // 添加谷值点 SHoleBoundaryPoint valleyBp; - valleyBp.point = pair.valleyPoint; - valleyBp.row = pair.valleyRow; - valleyBp.col = pair.valleyCol; + valleyBp.point = pair.endPoint; + valleyBp.row = pair.endRow; + valleyBp.col = pair.endCol; int valleyIndex = static_cast(boundaryPoints.size()); boundaryPoints.push_back(valleyBp); pairIdToBoundaryIndices[pairId].push_back(valleyIndex); @@ -871,11 +451,7 @@ static int DetectPitBoundaries( // for (int col = 0; col < cols; col++) { std::vector newBoundaryPoints; -#if _SELF_VER EvaluateLine(points, col, cols, rows, params, rows, cols, isBoundary, boundaryPoints, &newBoundaryPoints, nullptr); -#else - sg_getLineZJumpFeature_cornerMethod(points, col, cols, rows, params, rows, cols, isBoundary, boundaryPoints, &newBoundaryPoints); -#endif // Call visualization callback if provided if (callback && !newBoundaryPoints.empty()) { @@ -920,7 +496,7 @@ void EvaluateLine( const SHoleDetectionParams& params, int rows, int cols, - std::vector& peakValleyPairs + std::vector& peakValleyPairs ) { if (count < 3) return; @@ -961,13 +537,13 @@ void EvaluateLine( Direction currDir = calcDirection(points[prevValidIdx], points[currIdx]); if (currDir == Direction::FLAT) { if (transitionCount >= params.minVTransitionPoints) { - SPeakValleyPair pair; - pair.peakRow = prevValidIdx / cols; - pair.peakCol = prevValidIdx - pair.peakRow * cols; - pair.peakPoint = points[prevValidIdx]; - pair.valleyRow = currIdx / cols; - pair.valleyCol = currIdx - pair.valleyRow * cols; - pair.valleyPoint = points[currIdx]; + SSegmentPair pair; + pair.startRow = prevValidIdx / cols; + pair.startCol = prevValidIdx - pair.startRow * cols; + pair.startPoint = points[prevValidIdx]; + pair.endRow = currIdx / cols; + pair.endCol = currIdx - pair.endRow * cols; + pair.endPoint = points[currIdx]; peakValleyPairs.push_back(pair); } prevValidIdx = currIdx; // Update previous valid point index @@ -1271,7 +847,7 @@ static int ExpandClusterBoundingBox( int count = expandedMaxRow - expandedMinRow + 1; // 调用 EvaluateLine 扫描该列 - std::vector peakValleyPairs; + std::vector peakValleyPairs; EvaluateLine( points, startIdx, @@ -1285,19 +861,19 @@ static int ExpandClusterBoundingBox( // 将新检测到的边界点添加到 clusterPoints（去重） for (const auto& bp : peakValleyPairs) { - if (existingPoints.find({ bp.peakRow, bp.peakCol }) == existingPoints.end()) { + if (existingPoints.find({ bp.startRow, bp.startCol }) == existingPoints.end()) { SHoleBoundaryPoint pt; - pt.point = bp.peakPoint; - pt.row = bp.peakRow; - pt.col = bp.peakCol; + pt.point = bp.startPoint; + pt.row = bp.startRow; + pt.col = bp.startCol; clusterPoints.push_back(pt); existingPoints.insert({ pt.row, pt.col }); } - if (existingPoints.find({ bp.valleyRow, bp.valleyCol }) == existingPoints.end()) { + if (existingPoints.find({ bp.endRow, bp.endCol }) == existingPoints.end()) { SHoleBoundaryPoint pt; - pt.point = bp.valleyPoint; - pt.row = bp.valleyRow; - pt.col = bp.valleyCol; + pt.point = bp.endPoint; + pt.row = bp.endRow; + pt.col = bp.endCol; clusterPoints.push_back(pt); existingPoints.insert({ pt.row, pt.col }); } @@ -1323,105 +899,6 @@ static int ExpandClusterBoundingBox( return HD_SUCCESS; } -static int ClusterBoundaryPoints_DBSCAN( - const std::vector& boundaryPoints, - const SHoleDetectionParams& params, - std::vector>& clusters, - int* errCode -) { - if (boundaryPoints.empty() || errCode == nullptr) { - if (errCode) *errCode = HD_ERR_INVALID_INPUT; - return HD_ERR_INVALID_INPUT; - } - - clusters.clear(); - int n = boundaryPoints.size(); - std::vector labels(n, -1); // -1 = 未访问, -2 = 噪声, >= 0 = 簇 ID - int clusterId = 0; - - for (int i = 0; i < n; i++) { - if (labels[i] != -1) continue; // 已访问 - - // 查找邻居 - std::vector neighbors; - for (int j = 0; j < n; j++) { - if (i == j) continue; - - // 使用 2D (X-Y) 距离进行聚类 - // 孔洞边界在 X-Y 平面形成闭合环，Z 轴变化不应影响连通性 - float dx = boundaryPoints[j].point.x - boundaryPoints[i].point.x; - float dy = boundaryPoints[j].point.y - boundaryPoints[i].point.y; - float dist = std::sqrt(dx * dx + dy * dy); - - if (dist <= params.clusterEps) { - neighbors.push_back(j); - } - } - - if (neighbors.size() < static_cast(params.clusterMinPoints)) { - labels[i] = -2; // 标记为噪声 - continue; - } - - // 开始新簇 - labels[i] = clusterId; - std::vector seedSet = neighbors; - - for (size_t k = 0; k < seedSet.size(); k++) { - int q = seedSet[k]; - - if (labels[q] == -2) { - labels[q] = clusterId; // 将噪声点改为边界点 - } - - if (labels[q] != -1) continue; // 已处理 - - labels[q] = clusterId; - - // 查找 q 的邻居（使用 2D 距离） - std::vector qNeighbors; - for (int j = 0; j < n; j++) { - if (q == j) continue; - - float dx = boundaryPoints[j].point.x - boundaryPoints[q].point.x; - float dy = boundaryPoints[j].point.y - boundaryPoints[q].point.y; - float dist = std::sqrt(dx * dx + dy * dy); - - if (dist <= params.clusterEps) { - qNeighbors.push_back(j); - } - } - - if (qNeighbors.size() >= static_cast(params.clusterMinPoints)) { - seedSet.insert(seedSet.end(), qNeighbors.begin(), qNeighbors.end()); - } - } - - clusterId++; - } - - // 提取聚类结果 - for (int cid = 0; cid < clusterId; cid++) { - std::vector cluster; - for (int i = 0; i < n; i++) { - if (labels[i] == cid) { - cluster.push_back(i); - } - } - if (!cluster.empty()) { - clusters.push_back(cluster); - } - } - - if (clusters.empty()) { - *errCode = HD_ERR_NO_CLUSTERS_FOUND; - return HD_ERR_NO_CLUSTERS_FOUND; - } - - *errCode = HD_SUCCESS; - return HD_SUCCESS; -} - static int FitHolesFromClusters( const SVzNLPointXYZ* points, int rows, @@ -1497,8 +974,8 @@ static int FitHolesFromClusters( if (ret != HD_SUCCESS) continue; // ===== 尺寸范围过滤 ===== - if (hole.radius < filterParams.minHoleRadius || - hole.radius > filterParams.maxHoleRadius) { + if (hole.radius < detectionParams.minRadius || + hole.radius > detectionParams.maxRadius) { continue; } @@ -1893,8 +1370,8 @@ static int FitHoleFromExtremePoints( std::cout << " [DIAG] aspectRatio=" << aspectRatio << " (threshold: 0.8)" << std::endl; - if (aspectRatio < 0.6f) { - std::cout << " -> FAILED: aspectRatio " << aspectRatio << " < 0.8 (too elongated)" << 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; } @@ -1910,7 +1387,7 @@ static int FitHoleFromExtremePoints( center.z = centroidZ + projCenterX * basisU.z + projCenterY * basisV.z; // ===== 3. 尺寸范围过滤 ===== - if (radius < filterParams.minHoleRadius || radius > filterParams.maxHoleRadius) { + if (radius < detectionParams.minRadius || radius > detectionParams.maxRadius) { *errCode = HD_ERR_NO_VALID_HOLES; return HD_ERR_NO_VALID_HOLES; } @@ -2043,18 +1520,6 @@ static int FitHoleFromExtremePoints( return HD_ERR_NO_VALID_HOLES; } - // 9d. 矩形度检查：排除矩形/非圆形轮廓（在投影坐标系中） - /*float cornerRatio = ComputeRectangularityScore( - projectedPoints.data(), - static_cast(projectedPoints.size()) - ); - if (cornerRatio > filterParams.maxCornerRatio) { - std::cout << " -> FAILED: cornerRatio " << cornerRatio - << " > maxCornerRatio " << filterParams.maxCornerRatio << std::endl; - *errCode = HD_ERR_NO_VALID_HOLES; - return HD_ERR_NO_VALID_HOLES; - }*/ - // 9e. 椭圆内点率检查：验证点是否真正贴合拟合椭圆 float inlierRatio = ComputeEllipseInlierRatio( projectedPoints.data(), @@ -2125,7 +1590,7 @@ int DetectMultipleHoles( // 步骤 2: 检测凹坑边界 std::vector> clusters; std::vector boundaryPoints; - ret = DetectPitBoundaries(points, rows, cols, detectionParams, boundaryPoints, clusters, &errCode, nullptr, nullptr); + ret = DetectPitBoundaries(points, rows, cols, detectionParams, boundaryPoints, clusters, &errCode, nullptr, nullptr, debugCallbacks); if (ret != HD_SUCCESS) { return ret; } @@ -2139,14 +1604,91 @@ int DetectMultipleHoles( ); } - // 步骤 3: 对边界点进行聚类 - /*ret = ClusterBoundaryPoints_DBSCAN(boundaryPoints, detectionParams, clusters, &errCode); - if (ret != HD_SUCCESS) { - return ret; - }*/ - result->totalCandidates = clusters.size(); + // ========== 去除离群点：过滤每个 cluster 中的离群点 ========== + // 对每个 cluster，计算点到质心的距离，移除距离超过阈值的离群点 + for (auto& cluster : clusters) { + if (cluster.size() < 3) { + continue; // 点数太少，跳过 + } + + // 收集 cluster 中所有点的坐标 + std::vector clusterPoints; + clusterPoints.reserve(cluster.size()); + for (int idx : cluster) { + if (idx >= 0 && idx < static_cast(boundaryPoints.size())) { + clusterPoints.push_back(boundaryPoints[idx].point); + } + } + + if (clusterPoints.size() < 3) { + continue; + } + + // 计算质心 + double cx = 0.0, cy = 0.0, cz = 0.0; + for (const auto& pt : clusterPoints) { + cx += pt.x; + cy += pt.y; + cz += pt.z; + } + cx /= clusterPoints.size(); + cy /= clusterPoints.size(); + cz /= clusterPoints.size(); + + // 计算每个点到质心的距离 + std::vector distances; + distances.reserve(clusterPoints.size()); + for (const auto& pt : clusterPoints) { + double dx = pt.x - cx; + double dy = pt.y - cy; + double dz = pt.z - cz; + double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + distances.push_back(dist); + } + + // 计算距离的均值和标准差 + double meanDist = 0.0; + for (double d : distances) { + meanDist += d; + } + meanDist /= distances.size(); + + double variance = 0.0; + for (double d : distances) { + double diff = d - meanDist; + variance += diff * diff; + } + variance /= distances.size(); + double stdDev = std::sqrt(variance); + + // 使用 3-sigma 规则过滤离群点 + // 保留距离在 [mean - 3*sigma, mean + 3*sigma] 范围内的点 + double threshold = meanDist + 3.0 * stdDev; + + std::vector filteredCluster; + filteredCluster.reserve(cluster.size()); + for (size_t i = 0; i < cluster.size(); i++) { + if (distances[i] <= threshold) { + filteredCluster.push_back(cluster[i]); + } + } + + // 更新 cluster（只有在过滤后仍有足够点时才更新） + if (filteredCluster.size() >= 3) { + cluster = filteredCluster; + } + } + + // 移除过滤后点数过少的 cluster + clusters.erase( + std::remove_if(clusters.begin(), clusters.end(), + [](const std::vector& cluster) { return cluster.size() < 3; }), + clusters.end() + ); + + // Callback: 聚类完成 if (debugCallbacks && debugCallbacks->onClustersFound) { // 准备聚类信息 diff --git a/Algo/DetectHole/src/HoleDetection.h b/Algo/DetectHole/src/HoleDetection.h index 2bd855c..588c403 100644 --- a/Algo/DetectHole/src/HoleDetection.h +++ b/Algo/DetectHole/src/HoleDetection.h @@ -82,6 +82,14 @@ struct SHoleDetectionDebugCallbacks { */ void (*onHoleFitted)(const SHoleResult* hole, int holeIndex, void* userData); + /** + * @brief Called when segment endpoint pairs are detected + * @param segmentPairs Array of segment pairs (each pair has start and end points) + * @param count Number of segment pairs + * @param userData User-provided context pointer + */ + void (*onSegmentPairsDetected)(const SSegmentPair* segmentPairs, int count, void* userData); + /** * @brief User-provided context pointer, passed to all callbacks */ diff --git a/Algo/DetectHole/src/HoleDetectionParams.h b/Algo/DetectHole/src/HoleDetectionParams.h index efca4c9..d030d3c 100644 --- a/Algo/DetectHole/src/HoleDetectionParams.h +++ b/Algo/DetectHole/src/HoleDetectionParams.h @@ -2,141 +2,103 @@ #define HOLE_DETECTION_PARAMS_H #include -#include "../include/VZNL_Types.h" // Use types from VZNL_Types.h +#include "../include/VZNL_Types.h" // 使用 VZNL_Types.h 中的类型 /** - * @brief Sorting mode for detected holes + * @brief 检测到的孔洞排序模式 */ enum ESortMode { - keSortMode_None = 0, // No sorting - keSortMode_ByRadius = 1, // Sort by radius (largest first) - keSortMode_ByDepth = 2, // Sort by depth (deepest first) - keSortMode_ByQuality = 3 // Sort by quality score (highest first) + keSortMode_None = 0, // 不排序 + keSortMode_ByRadius = 1, // 按半径排序（最大的在前） + keSortMode_ByDepth = 2, // 按深度排序（最深的在前） + keSortMode_ByQuality = 3 // 按质量分数排序（最高的在前） }; /** - * @brief Detection parameters for hole detection algorithm + * @brief 孔洞检测算法的检测参数 */ struct SHoleDetectionParams { - // Pit detection parameters - int neighborCount; // Adjacent points for line connection (default: 3) - float angleThresholdPos; // Positive angle threshold in degrees (default: 70.0) - float angleThresholdNeg; // Negative angle threshold in degrees (default: -70.0) - float minPitDepth; // Minimum pit depth in mm (default: 5.0) + // 凹坑检测参数 + int neighborCount; // 线连接的相邻点数（默认值：3） + float angleThresholdPos; // 正角度阈值，单位：度（默认值：10.0） + float angleThresholdNeg; // 负角度阈值，单位：度（默认值：-10.0） + float minPitDepth; // 最小凹坑深度，单位：mm（默认值：1.0） - // Radial scanning parameters - float angleStep; // Angular step for radial scan in degrees (default: 1.0) - float maxScanRadius; // Maximum scan radius in mm (default: 100.0) + // 椭圆拟合参数 + float minRadius; // 最小孔洞半径，单位：mm（默认值：2.0） + float maxRadius; // 最大孔洞半径，单位：mm（默认值：50.0） - // Clustering parameters (DBSCAN) - float clusterEps; // DBSCAN epsilon in mm (default: 10.0) - int clusterMinPoints; // DBSCAN min points (default: 5) + // 平面拟合参数 + int expansionSize1; // 第一环扩展大小，单位：mm（默认值：5） + int expansionSize2; // 第二环扩展大小，单位：mm（默认值：10） - // Ellipse fitting parameters - float minRadius; // Minimum hole radius in mm (default: 5.0) - float maxRadius; // Maximum hole radius in mm (default: 50.0) + // V型检测参数 + int minVTransitionPoints; // V型端点之间的最小有效过渡点数（默认值：1） - // Plane fitting parameters - int expansionSize1; // First ring expansion in mm (default: 10.0) - int expansionSize2; // Second ring expansion in mm (default: 20.0) - - // Validation parameters - float validZThreshold; // Valid Z-value threshold (default: 1e-4) - - // V-type detection parameters - int minVTransitionPoints; // Minimum valid transition points between V-shape endpoints (default: 3) - - // Corner-based angle detection parameters (similar to cornerMethod) - float cornerScale; // Search distance for forward/backward points in mm (default: 5.0) - float cornerAngleThreshold; // Minimum corner angle change in degrees (default: 15.0) - float jumpCornerTh_1; // Small angle threshold for jump detection (default: 10.0) - float jumpCornerTh_2; // Large angle threshold for jump detection (default: 30.0) - float minEndingGap; // Y-direction distance threshold for jump pairing in mm (default: 5.0) - float minEndingGap_z; // Z-direction height threshold for jump validation in mm (default: 1.0) - - // Constructor with defaults + // 构造函数，设置默认值 SHoleDetectionParams() : neighborCount(3) , angleThresholdPos(10.0f) , angleThresholdNeg(-10.0f) , minPitDepth(1.0f) - , angleStep(1.0f) - , maxScanRadius(100.0f) - , clusterEps(10.0f) - , clusterMinPoints(3) , minRadius(2.0f) , maxRadius(50.0f) , expansionSize1(5) , expansionSize2(10) - , validZThreshold(1e-4f) , minVTransitionPoints(1) - , cornerScale(5.0f) - , cornerAngleThreshold(45.0f) - , jumpCornerTh_1(10.0f) - , jumpCornerTh_2(30.0f) - , minEndingGap(5.0f) - , minEndingGap_z(1.0f) {} }; /** - * @brief Filter parameters for hole filtering + * @brief 孔洞过滤参数 */ struct SHoleFilterParams { - // Size range filtering - float minHoleRadius; // Minimum hole radius in mm (default: 5.0) - float maxHoleRadius; // Maximum hole radius in mm (default: 50.0) - // Quality threshold filtering - float maxEccentricity; // Maximum eccentricity (default: 0.5, standard e=sqrt(1-(b/a)^2)) + // 质量阈值过滤 + float maxEccentricity; // 最大离心率（默认值：0.99995，标准公式 e=sqrt(1-(b/a)^2)） - // Shape filtering (pre-fitting) - float maxCornerRatio; // Maximum corner ratio for rectangularity (default: 0.15) - // Higher = more rectangular. Set to 1.0 to disable. - float minAngularCoverage; // Minimum angular coverage in degrees (default: 300.0) - // Used to filter non-closed boundaries. Set to 0 to disable. - float maxRadiusFitRatio; // Maximum radiusVariance/radius ratio (default: 0.3) - // Measures how well points fit the circle. Set to 1.0 to disable. - float minQualityScore; // Minimum overall quality score (default: 0.3) - // Weighted combination of shape metrics. Set to 0 to disable. + // 形状过滤（拟合前） + float minAngularCoverage; // 最小角度覆盖范围，单位：度（默认值：10.0） + // 用于过滤非闭合边界。设置为 0 可禁用。 + float maxRadiusFitRatio; // 最大半径拟合比率 radiusVariance/radius（默认值：1.0） + // 衡量点与圆的拟合程度。设置为 1.0 可禁用。 + float minQualityScore; // 最小整体质量分数（默认值：0.0） + // 形状指标的加权组合。设置为 0 可禁用。 - // Planarity filtering (pre-projection) - float maxPlaneResidual; // Maximum point-to-plane residual in mm (default: 10.0) - // Rejects non-planar clusters (e.g. cliffs, step edges). - float maxAngularGap; // Maximum angular gap in degrees (default: 90.0) - // Rejects L-shaped or non-closed boundaries. - float minInlierRatio; // Minimum inlier ratio for ellipse fit (default: 0.7) - // Fraction of points within tolerance of fitted ellipse. + // 平面性过滤（投影前） + float maxPlaneResidual; // 最大点到平面残差，单位：mm（默认值：10.0） + // 拒绝非平面簇（例如悬崖、台阶边缘）。 + float maxAngularGap; // 最大角度间隙，单位：度（默认值：90.0） + // 拒绝 L 型或非闭合边界。 + float minInlierRatio; // 椭圆拟合的最小内点比率（默认值：0.0） + // 在拟合椭圆容差范围内的点的比例。 - // Constructor with defaults + // 构造函数，设置默认值 SHoleFilterParams() - : minHoleRadius(1.0f) - , maxHoleRadius(10.0f) - , maxEccentricity(0.99995f) - , maxCornerRatio(0.15f) + : maxEccentricity(0.99995f) , minAngularCoverage(10.f) - , maxRadiusFitRatio(0.3f) - , minQualityScore(0.3f) + , maxRadiusFitRatio(1.f) + , minQualityScore(0.f) , maxPlaneResidual(10.0f) , maxAngularGap(90.0f) - , minInlierRatio(0.3f) + , minInlierRatio(0.f) {} }; /** - * @brief Single hole detection result + * @brief 单个孔洞检测结果 * - * Note: SVzNL3DPointF and SVzNL2DPointF are defined in VZNL_Types.h + * 注意：SVzNL3DPointF 和 SVzNL2DPointF 在 VZNL_Types.h 中定义 */ struct SHoleResult { - SVzNL3DPointF center; // Hole center point (x, y, z) - SVzNL3DPointF normal; // Plane normal vector (unit length) - float radius; // Hole radius in mm - float depth; // Pit depth in mm - float eccentricity; // Eccentricity (0 = perfect circle) - float radiusVariance; // Radius variance in mm - float angularSpan; // Angular coverage in degrees - float qualityScore; // Quality score (0-1, higher = better) + SVzNL3DPointF center; // 孔洞中心点 (x, y, z) + SVzNL3DPointF normal; // 平面法向量（单位长度） + float radius; // 孔洞半径，单位：mm + float depth; // 凹坑深度，单位：mm + float eccentricity; // 离心率（0 = 完美圆形） + float radiusVariance; // 半径方差，单位：mm + float angularSpan; // 角度覆盖范围，单位：度 + float qualityScore; // 质量分数（0-1，越高越好） SHoleResult() : center() @@ -151,16 +113,16 @@ struct SHoleResult { }; /** - * @brief Multiple hole detection result + * @brief 多孔洞检测结果 * - * @note Memory management: The holes array is NOT automatically freed. - * Caller must call FreeMultiHoleResult() or manually delete[] holes. + * @note 内存管理：holes 数组不会自动释放。 + * 调用者必须调用 FreeMultiHoleResult() 或手动 delete[] holes。 */ struct SMultiHoleResult { - int holeCount; // Number of detected holes - SHoleResult* holes; // Array of hole results (caller must free) - int totalCandidates; // Total candidate holes before filtering - int filteredCount; // Number of holes filtered out + int holeCount; // 检测到的孔洞数量 + SHoleResult* holes; // 孔洞结果数组（调用者必须释放） + int totalCandidates; // 过滤前的候选孔洞总数 + int filteredCount; // 被过滤掉的孔洞数量 SMultiHoleResult() : holeCount(0) @@ -171,9 +133,9 @@ struct SMultiHoleResult { }; /** - * @brief Free memory allocated by DetectMultipleHoles + * @brief 释放 DetectMultipleHoles 分配的内存 * - * @param [in,out] result Result structure to free + * @param [in,out] result 要释放的结果结构 */ inline void FreeMultiHoleResult(SMultiHoleResult* result) { if (result != nullptr && result->holes != nullptr) { @@ -184,28 +146,28 @@ inline void FreeMultiHoleResult(SMultiHoleResult* result) { } /** - * @brief Segment endpoints pair structure + * @brief 线段端点对结构 * - * Represents a segment detected in a line scan, with start and end points. - * Note: Despite the name "PeakValley", this structure stores segment endpoints, - * not necessarily peak/valley points. The naming is kept for compatibility. + * 表示在线扫描中检测到的线段，包含起点和终点。 + * 注意：尽管名称为"PeakValley"，但此结构存储的是线段端点， + * 不一定是峰值/谷值点。保留此命名是为了兼容性。 */ -struct SPeakValleyPair { - SVzNLPointXYZ peakPoint; // Segment start point - SVzNLPointXYZ valleyPoint; // Segment end point - int peakRow; // Start point row index - int peakCol; // Start point column index - int valleyRow; // End point row index - int valleyCol; // End point column index - float depth; // Depth difference between start and end points +struct SSegmentPair { + SVzNLPointXYZ startPoint; // 线段起点 + SVzNLPointXYZ endPoint; // 线段终点 + int startRow; // 起点行索引 + int startCol; // 起点列索引 + int endRow; // 终点行索引 + int endCol; // 终点列索引 + float depth; // 起点和终点之间的深度差 - SPeakValleyPair() - : peakPoint() - , valleyPoint() - , peakRow(0) - , peakCol(0) - , valleyRow(0) - , valleyCol(0) + SSegmentPair() + : startPoint() + , endPoint() + , startRow(0) + , startCol(0) + , endRow(0) + , endCol(0) , depth(0.0f) {} };