Compare commits
10 Commits
6c55510809
...
7969a242ba
| Author | SHA1 | Date | |
|---|---|---|---|
| 7969a242ba | |||
| 1d649f9c01 | |||
| 916f2c7662 | |||
| 3aae9159ed | |||
| 9d304b19ac | |||
| 937e9c0d8a | |||
|
|
976f87b6ba | ||
| 2e92ddfab0 | |||
| 0d88858738 | |||
|
|
cced2a9646 |
5
.gitignore
vendored
5
.gitignore
vendored
@ -1,4 +1,7 @@
|
|||||||
.claude
|
.claude
|
||||||
nul
|
nul
|
||||||
build
|
build
|
||||||
Algo/DetectHole/Export/
|
Algo/DetectHole/Export
|
||||||
|
Algo/DetectBarIntersection/build
|
||||||
|
Algo/DetectBarIntersection/data
|
||||||
|
Algo/DetectBarIntersection/Export
|
||||||
@ -5,15 +5,24 @@ project(DetectBarIntersection VERSION 1.0.0 LANGUAGES CXX)
|
|||||||
set(CMAKE_CXX_STANDARD 11)
|
set(CMAKE_CXX_STANDARD 11)
|
||||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||||
|
|
||||||
|
if(CMAKE_CONFIGURATION_TYPES)
|
||||||
|
string(REPLACE ";" ", " DETECTBARINTERSECTION_CONFIGURATION_LIST "${CMAKE_CONFIGURATION_TYPES}")
|
||||||
|
set(DETECTBARINTERSECTION_BUILD_DESCRIPTION "Multi-config (${DETECTBARINTERSECTION_CONFIGURATION_LIST})")
|
||||||
|
else()
|
||||||
|
set(DETECTBARINTERSECTION_BUILD_DESCRIPTION "${CMAKE_BUILD_TYPE}")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
if(NOT DETECTBARINTERSECTION_BUILD_DESCRIPTION)
|
||||||
|
set(DETECTBARINTERSECTION_BUILD_DESCRIPTION "Not specified")
|
||||||
|
endif()
|
||||||
|
|
||||||
if(WIN32)
|
if(WIN32)
|
||||||
if(MSVC)
|
if(MSVC)
|
||||||
add_compile_options(
|
add_compile_options(
|
||||||
/wd4267
|
/wd4267
|
||||||
/wd4996
|
/wd4996
|
||||||
|
$<$<CONFIG:Release>:/O2>
|
||||||
)
|
)
|
||||||
if(CMAKE_BUILD_TYPE STREQUAL "Release")
|
|
||||||
add_compile_options(/O2)
|
|
||||||
endif()
|
|
||||||
endif()
|
endif()
|
||||||
else()
|
else()
|
||||||
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
|
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
|
||||||
@ -235,7 +244,7 @@ endif()
|
|||||||
message(STATUS "")
|
message(STATUS "")
|
||||||
message(STATUS "DetectBarIntersection Configuration Summary:")
|
message(STATUS "DetectBarIntersection Configuration Summary:")
|
||||||
message(STATUS " C++ Standard: ${CMAKE_CXX_STANDARD}")
|
message(STATUS " C++ Standard: ${CMAKE_CXX_STANDARD}")
|
||||||
message(STATUS " Build Type: ${CMAKE_BUILD_TYPE}")
|
message(STATUS " Build Type: ${DETECTBARINTERSECTION_BUILD_DESCRIPTION}")
|
||||||
message(STATUS " Eigen3 Include: ${EIGEN3_INCLUDE_DIR}")
|
message(STATUS " Eigen3 Include: ${EIGEN3_INCLUDE_DIR}")
|
||||||
message(STATUS " OpenMP Enabled: ${ENABLE_OPENMP}")
|
message(STATUS " OpenMP Enabled: ${ENABLE_OPENMP}")
|
||||||
message(STATUS " Build Examples: ${BUILD_EXAMPLES}")
|
message(STATUS " Build Examples: ${BUILD_EXAMPLES}")
|
||||||
|
|||||||
@ -1,26 +1,33 @@
|
|||||||
@echo off
|
@echo off
|
||||||
setlocal
|
setlocal EnableExtensions
|
||||||
|
|
||||||
echo === Building DetectBarIntersection ===
|
set "BUILD_CONFIG=Debug"
|
||||||
|
|
||||||
if not exist build (
|
if not "%~1"=="" (
|
||||||
mkdir build
|
if /I "%~1"=="Debug" (
|
||||||
|
set "BUILD_CONFIG=Debug"
|
||||||
|
) else if /I "%~1"=="Release" (
|
||||||
|
set "BUILD_CONFIG=Release"
|
||||||
|
) else (
|
||||||
|
echo Usage: %~nx0 [Debug^|Release]
|
||||||
|
exit /b 1
|
||||||
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
cd build
|
echo === Building DetectBarIntersection [%BUILD_CONFIG%] ===
|
||||||
|
|
||||||
cmake .. -G "Visual Studio 17 2022" -A x64
|
cmake -S . -B build -G "Visual Studio 17 2022" -A x64
|
||||||
|
|
||||||
if %ERRORLEVEL% NEQ 0 (
|
if errorlevel 1 (
|
||||||
echo CMake configuration failed!
|
echo CMake configuration failed!
|
||||||
exit /b %ERRORLEVEL%
|
exit /b %ERRORLEVEL%
|
||||||
)
|
)
|
||||||
|
|
||||||
cmake --build . --config Debug
|
cmake --build build --config %BUILD_CONFIG%
|
||||||
|
|
||||||
if %ERRORLEVEL% NEQ 0 (
|
if errorlevel 1 (
|
||||||
echo Build failed!
|
echo Build failed!
|
||||||
exit /b %ERRORLEVEL%
|
exit /b %ERRORLEVEL%
|
||||||
)
|
)
|
||||||
|
|
||||||
echo === Build completed successfully ===
|
echo === Build completed successfully [%BUILD_CONFIG%] ===
|
||||||
|
|||||||
@ -10,6 +10,6 @@ endif()
|
|||||||
|
|
||||||
message(STATUS "")
|
message(STATUS "")
|
||||||
message(STATUS "Examples Configuration:")
|
message(STATUS "Examples Configuration:")
|
||||||
message(STATUS " Build Type: ${CMAKE_BUILD_TYPE}")
|
message(STATUS " Build Type: ${DETECTBARINTERSECTION_BUILD_DESCRIPTION}")
|
||||||
message(STATUS " DetectBarIntersection Root: ${DETECTBARINTERSECTION_ROOT}")
|
message(STATUS " DetectBarIntersection Root: ${DETECTBARINTERSECTION_ROOT}")
|
||||||
message(STATUS "")
|
message(STATUS "")
|
||||||
|
|||||||
@ -60,14 +60,25 @@ if(VTK_FOUND)
|
|||||||
|
|
||||||
# Copy VTK DLLs
|
# Copy VTK DLLs
|
||||||
file(GLOB VTK_DLLS "${VTK_BIN_DIR}/*.dll")
|
file(GLOB VTK_DLLS "${VTK_BIN_DIR}/*.dll")
|
||||||
|
set(VTK_DEBUG_DLLS)
|
||||||
|
set(VTK_RELEASE_DLLS)
|
||||||
foreach(vtk_dll ${VTK_DLLS})
|
foreach(vtk_dll ${VTK_DLLS})
|
||||||
add_custom_command(TARGET visualization_demo POST_BUILD
|
get_filename_component(vtk_dll_name "${vtk_dll}" NAME)
|
||||||
COMMAND ${CMAKE_COMMAND} -E copy_if_different
|
if(vtk_dll_name MATCHES "d\\.dll$")
|
||||||
"${vtk_dll}"
|
list(APPEND VTK_DEBUG_DLLS "${vtk_dll}")
|
||||||
"$<TARGET_FILE_DIR:visualization_demo>"
|
else()
|
||||||
COMMENT "Copying VTK DLL: ${vtk_dll}"
|
list(APPEND VTK_RELEASE_DLLS "${vtk_dll}")
|
||||||
)
|
endif()
|
||||||
endforeach()
|
endforeach()
|
||||||
|
|
||||||
|
add_custom_command(TARGET visualization_demo POST_BUILD
|
||||||
|
COMMAND ${CMAKE_COMMAND} -E copy_if_different
|
||||||
|
"$<$<CONFIG:Debug>:${VTK_DEBUG_DLLS}>"
|
||||||
|
"$<$<NOT:$<CONFIG:Debug>>:${VTK_RELEASE_DLLS}>"
|
||||||
|
"$<TARGET_FILE_DIR:visualization_demo>"
|
||||||
|
COMMAND_EXPAND_LISTS
|
||||||
|
COMMENT "Copying configuration-matched VTK DLLs to visualization_demo output directory"
|
||||||
|
)
|
||||||
endif()
|
endif()
|
||||||
else()
|
else()
|
||||||
message(WARNING "VTK not found. visualization_demo will not be built.")
|
message(WARNING "VTK not found. visualization_demo will not be built.")
|
||||||
|
|||||||
@ -224,7 +224,7 @@ int ProcessSingleFile(
|
|||||||
debugCallbacks.onPlaneSegmented = nullptr;// OnPlaneSegmented;
|
debugCallbacks.onPlaneSegmented = nullptr;// OnPlaneSegmented;
|
||||||
debugCallbacks.onPointsAligned = nullptr;// OnPointsAligned;
|
debugCallbacks.onPointsAligned = nullptr;// OnPointsAligned;
|
||||||
debugCallbacks.onPlaneFiltered = nullptr;// OnPlaneFiltered;
|
debugCallbacks.onPlaneFiltered = nullptr;// OnPlaneFiltered;
|
||||||
debugCallbacks.onClustersDetected = OnClustersDetected;
|
debugCallbacks.onClustersDetected = nullptr;// OnClustersDetected;
|
||||||
debugCallbacks.userData = &callbackData;
|
debugCallbacks.userData = &callbackData;
|
||||||
|
|
||||||
// Set parameters
|
// Set parameters
|
||||||
|
|||||||
@ -1,4 +1,4 @@
|
|||||||
#ifndef BAR_INTERSECTION_PARAMS_H
|
#ifndef BAR_INTERSECTION_PARAMS_H
|
||||||
#define BAR_INTERSECTION_PARAMS_H
|
#define BAR_INTERSECTION_PARAMS_H
|
||||||
|
|
||||||
#include "VZNL_Types.h"
|
#include "VZNL_Types.h"
|
||||||
@ -31,12 +31,28 @@ struct SGrowthParams {
|
|||||||
float thresholdY; // 行内分段阈值:同行相邻点 y 的最大允许差 (mm)
|
float thresholdY; // 行内分段阈值:同行相邻点 y 的最大允许差 (mm)
|
||||||
float thresholdZ; // 行内分段阈值:同行相邻点 z 的最大允许差 (mm)
|
float thresholdZ; // 行内分段阈值:同行相邻点 z 的最大允许差 (mm)
|
||||||
int minClusterSize; // 最小簇点数
|
int minClusterSize; // 最小簇点数
|
||||||
|
float angleSearchDistance;
|
||||||
|
int residualSmoothWindow;
|
||||||
|
float maxAxisDeviationFromXYDeg; // Max allowed axis deviation from the XY plane (deg)
|
||||||
|
float maxPerpendicularDeviationDeg; // Max deviation from 90 deg for centroid-connection test
|
||||||
|
int minContinuousValidPointCount; // Min consecutive valid points kept on each laser line after plane filtering; <=1 disables
|
||||||
|
float maxContinuousPointZTolerance; // Max z difference between adjacent points inside one valid run (mm)
|
||||||
|
float minBarDiameter;
|
||||||
|
float maxBarDiameter;
|
||||||
|
|
||||||
SGrowthParams()
|
SGrowthParams()
|
||||||
: thresholdX(5.0f)
|
: thresholdX(5.0f)
|
||||||
, thresholdY(5.0f)
|
, thresholdY(5.0f)
|
||||||
, thresholdZ(5.0f)
|
, thresholdZ(5.0f)
|
||||||
, minClusterSize(20)
|
, minClusterSize(20)
|
||||||
|
, angleSearchDistance(2.f)
|
||||||
|
, residualSmoothWindow(5)
|
||||||
|
, maxAxisDeviationFromXYDeg(50.0f)
|
||||||
|
, maxPerpendicularDeviationDeg(50.0f)
|
||||||
|
, minContinuousValidPointCount(0)
|
||||||
|
, maxContinuousPointZTolerance(5.0f)
|
||||||
|
, minBarDiameter(10.0f)
|
||||||
|
, maxBarDiameter(30.0f)
|
||||||
{}
|
{}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -1,6 +1,7 @@
|
|||||||
#include "BarIntersection.h"
|
#include "BarIntersection.h"
|
||||||
#include "PlaneAlignment.h"
|
#include "PlaneAlignment.h"
|
||||||
#include "RegionGrowing.h"
|
#include "RegionGrowing.h"
|
||||||
|
#include <Eigen/Dense>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
@ -9,7 +10,7 @@
|
|||||||
#include <numeric>
|
#include <numeric>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
|
|
||||||
static const char* s_algorithmVersion = "2.0.0";
|
static const char* s_algorithmVersion = "1.0.0";
|
||||||
static const char* s_algorithmName = "BarIntersectionDetection";
|
static const char* s_algorithmName = "BarIntersectionDetection";
|
||||||
|
|
||||||
static bool IsValidPoint(const SVzNLPointXYZ& pt) {
|
static bool IsValidPoint(const SVzNLPointXYZ& pt) {
|
||||||
@ -17,7 +18,107 @@ static bool IsValidPoint(const SVzNLPointXYZ& pt) {
|
|||||||
return !(std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps);
|
return !(std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps);
|
||||||
}
|
}
|
||||||
|
|
||||||
BAR_INTERSECTION_API int DetectBarIntersections(
|
static void ClearPoint(SVzNLPointXYZ* pt) {
|
||||||
|
if (!pt) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
pt->x = 0.0f;
|
||||||
|
pt->y = 0.0f;
|
||||||
|
pt->z = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void ClearShortContinuousSegment(
|
||||||
|
SVzNLPointXYZ* rowPoints,
|
||||||
|
int startCol,
|
||||||
|
int segmentLength,
|
||||||
|
int minContinuousValidPointCount
|
||||||
|
) {
|
||||||
|
if (!rowPoints || startCol < 0 || segmentLength <= 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (segmentLength >= minContinuousValidPointCount) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int offset = 0; offset < segmentLength; ++offset) {
|
||||||
|
ClearPoint(&rowPoints[startCol + offset]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void FilterLaserLineNoiseInPlace(
|
||||||
|
SVzNLPointXYZ* alignedPoints,
|
||||||
|
int rows,
|
||||||
|
int cols,
|
||||||
|
int minContinuousValidPointCount,
|
||||||
|
float maxContinuousPointZTolerance
|
||||||
|
) {
|
||||||
|
if (!alignedPoints || rows <= 0 || cols <= 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (minContinuousValidPointCount <= 1 || maxContinuousPointZTolerance < 0.0f) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int row = 0; row < rows; ++row) {
|
||||||
|
SVzNLPointXYZ* rowPoints = alignedPoints + row * cols;
|
||||||
|
int segmentStartCol = -1;
|
||||||
|
int segmentLength = 0;
|
||||||
|
float prevZ = 0.0f;
|
||||||
|
|
||||||
|
for (int col = 0; col < cols; ++col) {
|
||||||
|
const SVzNLPointXYZ& pt = rowPoints[col];
|
||||||
|
if (!IsValidPoint(pt)) {
|
||||||
|
ClearShortContinuousSegment(
|
||||||
|
rowPoints,
|
||||||
|
segmentStartCol,
|
||||||
|
segmentLength,
|
||||||
|
minContinuousValidPointCount
|
||||||
|
);
|
||||||
|
segmentStartCol = -1;
|
||||||
|
segmentLength = 0;
|
||||||
|
prevZ = 0.0f;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (segmentLength <= 0) {
|
||||||
|
segmentStartCol = col;
|
||||||
|
segmentLength = 1;
|
||||||
|
prevZ = pt.z;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float zDiff = std::abs(pt.z - prevZ);
|
||||||
|
if (zDiff <= maxContinuousPointZTolerance) {
|
||||||
|
++segmentLength;
|
||||||
|
prevZ = pt.z;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
ClearShortContinuousSegment(
|
||||||
|
rowPoints,
|
||||||
|
segmentStartCol,
|
||||||
|
segmentLength,
|
||||||
|
minContinuousValidPointCount
|
||||||
|
);
|
||||||
|
|
||||||
|
segmentStartCol = col;
|
||||||
|
segmentLength = 1;
|
||||||
|
prevZ = pt.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
ClearShortContinuousSegment(
|
||||||
|
rowPoints,
|
||||||
|
segmentStartCol,
|
||||||
|
segmentLength,
|
||||||
|
minContinuousValidPointCount
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int DetectBarIntersections(
|
||||||
const SVzNLPointXYZ* points,
|
const SVzNLPointXYZ* points,
|
||||||
int rows,
|
int rows,
|
||||||
int cols,
|
int cols,
|
||||||
@ -107,6 +208,17 @@ BAR_INTERSECTION_API int DetectBarIntersections(
|
|||||||
alignedPoints, totalPoints, planeZ, planeParams.heightThreshold
|
alignedPoints, totalPoints, planeZ, planeParams.heightThreshold
|
||||||
);
|
);
|
||||||
|
|
||||||
|
// ============================================
|
||||||
|
// Step 2b: 按激光线过滤短连续段噪点
|
||||||
|
// ============================================
|
||||||
|
FilterLaserLineNoiseInPlace(
|
||||||
|
alignedPoints,
|
||||||
|
rows,
|
||||||
|
cols,
|
||||||
|
growthParams.minContinuousValidPointCount,
|
||||||
|
growthParams.maxContinuousPointZTolerance
|
||||||
|
);
|
||||||
|
|
||||||
// Debug callback: plane filtered
|
// Debug callback: plane filtered
|
||||||
if (debugCallbacks && debugCallbacks->onPlaneFiltered) {
|
if (debugCallbacks && debugCallbacks->onPlaneFiltered) {
|
||||||
// 收集非零点用于回调显示
|
// 收集非零点用于回调显示
|
||||||
@ -137,7 +249,10 @@ BAR_INTERSECTION_API int DetectBarIntersections(
|
|||||||
// ============================================
|
// ============================================
|
||||||
std::vector<SGrowthCluster> clusters;
|
std::vector<SGrowthCluster> clusters;
|
||||||
bar_intersection::RegionGrowClusters(
|
bar_intersection::RegionGrowClusters(
|
||||||
alignedPoints, rows, cols, growthParams, clusters
|
alignedPoints, rows, cols, true, growthParams, clusters
|
||||||
|
);
|
||||||
|
bar_intersection::RegionGrowClusters(
|
||||||
|
alignedPoints, rows, cols, false, growthParams, clusters
|
||||||
);
|
);
|
||||||
|
|
||||||
// ============================================
|
// ============================================
|
||||||
|
|||||||
@ -1,10 +1,13 @@
|
|||||||
#include "RegionGrowing.h"
|
#include "RegionGrowing.h"
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
|
#undef min
|
||||||
|
#undef max
|
||||||
|
|
||||||
namespace bar_intersection {
|
namespace bar_intersection {
|
||||||
|
|
||||||
struct SGridPoint {
|
struct SGridPoint {
|
||||||
@ -111,8 +114,405 @@ static void FinalizeSegment(SLineSegment& seg) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool HasColumnOverlapOrTouch(const SLineSegment& a, const SLineSegment& b) {
|
struct SSegmentRelationResult {
|
||||||
return !(b.endCol < a.startCol - 1 || a.endCol < b.startCol - 1);
|
bool hasMatch;
|
||||||
|
float bestDistanceSq;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct SCarrySegmentState {
|
||||||
|
bool hasMatch;
|
||||||
|
};
|
||||||
|
|
||||||
|
static SSegmentRelationResult EvaluateSegmentRelation(
|
||||||
|
const SLineSegment& prevSeg,
|
||||||
|
const SLineSegment& currSeg,
|
||||||
|
const SGrowthParams& params
|
||||||
|
) {
|
||||||
|
SSegmentRelationResult result;
|
||||||
|
result.hasMatch = false;
|
||||||
|
result.bestDistanceSq = std::numeric_limits<float>::max();
|
||||||
|
|
||||||
|
const float centroidDistanceThresholdSq =
|
||||||
|
params.thresholdX * params.thresholdX +
|
||||||
|
params.thresholdY * params.thresholdY +
|
||||||
|
params.thresholdZ * params.thresholdZ;
|
||||||
|
|
||||||
|
const float dx = currSeg.centroid.x - prevSeg.centroid.x;
|
||||||
|
const float dy = currSeg.centroid.y - prevSeg.centroid.y;
|
||||||
|
const float dz = currSeg.centroid.z - prevSeg.centroid.z;
|
||||||
|
const float distanceSq = dx * dx + dy * dy + dz * dz;
|
||||||
|
if (distanceSq < centroidDistanceThresholdSq) {
|
||||||
|
result.hasMatch = true;
|
||||||
|
result.bestDistanceSq = distanceSq;
|
||||||
|
}
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
enum class EPanelType {
|
||||||
|
YZ_PANEL,
|
||||||
|
XZ_PANEL
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Per-point signed angle state along one scanned line
|
||||||
|
*/
|
||||||
|
enum ELineAngleTrend {
|
||||||
|
keLineAngleTrend_Invalid = 0,
|
||||||
|
keLineAngleTrend_Flat = 1,
|
||||||
|
keLineAngleTrend_PositiveJump = 2,
|
||||||
|
keLineAngleTrend_NegativeJump = 3
|
||||||
|
};
|
||||||
|
|
||||||
|
static bool IsValidPoint(const SVzNLPointXYZ& pt) {
|
||||||
|
const float eps = 1e-6f;
|
||||||
|
return !(std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps);
|
||||||
|
}
|
||||||
|
|
||||||
|
void EvaluateLine(
|
||||||
|
const SVzNLPointXYZ* points,
|
||||||
|
int count,
|
||||||
|
int step,
|
||||||
|
int startIdx,
|
||||||
|
int cols,
|
||||||
|
const SGrowthParams& params,
|
||||||
|
std::vector<SLineSegment>& currRowSegments,
|
||||||
|
bool& bIsInvalidLine,
|
||||||
|
EPanelType panelType
|
||||||
|
) {
|
||||||
|
currRowSegments.clear();
|
||||||
|
if (count < 3) return;
|
||||||
|
|
||||||
|
constexpr float kPi = 3.14159265358979323846f;
|
||||||
|
const float searchDist = params.angleSearchDistance;
|
||||||
|
const float posThresh = params.maxAxisDeviationFromXYDeg;
|
||||||
|
const float negThresh = -params.maxPerpendicularDeviationDeg;
|
||||||
|
|
||||||
|
// 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 row;
|
||||||
|
int col;
|
||||||
|
int globalIdx;
|
||||||
|
bool valid;
|
||||||
|
float signedAngleDeg;
|
||||||
|
bool hasAngle;
|
||||||
|
ELineAngleTrend trend;
|
||||||
|
};
|
||||||
|
|
||||||
|
std::vector<PointInfo> pts(count);
|
||||||
|
auto curPoint = points;
|
||||||
|
for (int i = 0; i < count; i++) {
|
||||||
|
int gIdx = startIdx + i * step;
|
||||||
|
pts[i].point = *curPoint;
|
||||||
|
pts[i].row = gIdx / cols;
|
||||||
|
pts[i].col = gIdx % cols;
|
||||||
|
pts[i].globalIdx = gIdx;
|
||||||
|
pts[i].valid = IsValidPoint(*curPoint);
|
||||||
|
pts[i].signedAngleDeg = 0.0f;
|
||||||
|
pts[i].hasAngle = false;
|
||||||
|
pts[i].trend = keLineAngleTrend_Invalid;
|
||||||
|
|
||||||
|
if (pts[i].valid) {
|
||||||
|
bIsInvalidLine = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
curPoint += step;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
// 局部弯折强度:line1(backRef→cur) 延长后 与 line2(cur→fwdRef) 的夹角绝对值。
|
||||||
|
// 与旧方案(backRef→fwdRef 整体坡度角)的本质区别:
|
||||||
|
// 均匀倾斜面(无弯折)→ 0°;只有在 pts[i] 处发生方向变化时才得到非零角。
|
||||||
|
// signedAngleDeg 的语义:
|
||||||
|
// 符号 = 整体跳变方向(按坐标正向统一;低→高为正,高→低为负)
|
||||||
|
// 绝对值 = pts[i] 处的局部弯折强度
|
||||||
|
float curCoord = getCoord(cur);
|
||||||
|
float v1_coord = curCoord - backCoord; // backRef → cur(水平分量)
|
||||||
|
float v1_z = cur.z - backMeanZ; // backRef → cur(z 分量)
|
||||||
|
float v2_coord = fwdCoord - curCoord; // cur → fwdRef(水平分量)
|
||||||
|
float v2_z = fwdMeanZ - cur.z; // cur → fwdRef(z 分量)
|
||||||
|
|
||||||
|
// 2D 叉积(z 分量)和点积,用于计算有符号夹角
|
||||||
|
float cross2d = v1_coord * v2_z - v1_z * v2_coord;
|
||||||
|
float dot2d = v1_coord * v2_coord + v1_z * v2_z;
|
||||||
|
// 消除扫描方向(正向/反向)对整体跳变方向符号的影响
|
||||||
|
float scanDirSign = ((v1_coord + v2_coord) >= 0.0f) ? 1.0f : -1.0f;
|
||||||
|
float bendAngleDeg = std::atan2(std::fabs(cross2d), dot2d) * (180.0f / kPi);
|
||||||
|
float overallDeltaZ = (fwdMeanZ - backMeanZ) * scanDirSign;
|
||||||
|
float slopeAngleDeg = 0.0f;
|
||||||
|
if (overallDeltaZ > 1e-6f) {
|
||||||
|
slopeAngleDeg = bendAngleDeg;
|
||||||
|
}
|
||||||
|
else if (overallDeltaZ < -1e-6f) {
|
||||||
|
slopeAngleDeg = -bendAngleDeg;
|
||||||
|
}
|
||||||
|
|
||||||
|
pts[i].signedAngleDeg = slopeAngleDeg;
|
||||||
|
|
||||||
|
if (slopeAngleDeg > posThresh) {
|
||||||
|
pts[i].trend = keLineAngleTrend_PositiveJump;
|
||||||
|
}
|
||||||
|
else if (slopeAngleDeg < negThresh) {
|
||||||
|
pts[i].trend = keLineAngleTrend_NegativeJump;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
pts[i].trend = keLineAngleTrend_Flat;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 角度平滑:滑动窗口均值,抑制点级噪声
|
||||||
|
// Fix3: 先保存原始角度,平滑后对所有"原始角度已超阈"的点强制恢复特征分类,
|
||||||
|
// 防止平滑窗口把窄孔洞两侧的异号角度互相抵消,导致真实边缘被压回 Flat。
|
||||||
|
{
|
||||||
|
// 保存平滑前的原始角度
|
||||||
|
std::vector<float> rawAngles(count, 0.0f);
|
||||||
|
for (int i = 0; i < count; i++) {
|
||||||
|
rawAngles[i] = pts[i].signedAngleDeg;
|
||||||
|
}
|
||||||
|
|
||||||
|
int smoothW = params.residualSmoothWindow;
|
||||||
|
if (smoothW > 1) {
|
||||||
|
int halfW = smoothW / 2;
|
||||||
|
std::vector<float> smoothed(count, 0.0f);
|
||||||
|
for (int i = 0; i < count; i++) {
|
||||||
|
if (!pts[i].hasAngle) continue;
|
||||||
|
float sum = 0.0f;
|
||||||
|
int cnt = 0;
|
||||||
|
for (int j = std::max(0, i - halfW); j <= std::min(count - 1, i + halfW); j++) {
|
||||||
|
if (pts[j].hasAngle) {
|
||||||
|
sum += pts[j].signedAngleDeg;
|
||||||
|
cnt++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (cnt > 0) smoothed[i] = sum / static_cast<float>(cnt);
|
||||||
|
}
|
||||||
|
for (int i = 0; i < count; i++) {
|
||||||
|
if (!pts[i].hasAngle) continue;
|
||||||
|
pts[i].signedAngleDeg = smoothed[i];
|
||||||
|
if (smoothed[i] > posThresh)
|
||||||
|
pts[i].trend = keLineAngleTrend_PositiveJump;
|
||||||
|
else if (smoothed[i] < negThresh)
|
||||||
|
pts[i].trend = keLineAngleTrend_NegativeJump;
|
||||||
|
else
|
||||||
|
pts[i].trend = keLineAngleTrend_Flat;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Fix3: 若原始角度已超过检测阈值,平滑窗口不应将其压回 Flat。
|
||||||
|
// 原始角度由 searchDist 参考点计算,本身已具备一定的抗噪能力。
|
||||||
|
// 平滑窗口在孔洞两侧引入异号角度交叉干扰,会把窄孔洞的边缘信号抹平:
|
||||||
|
// - 孔洞入射侧的 Desc 角被出射侧的正角平均拉升,可能越过 negThresh;
|
||||||
|
// - 孔洞出射侧的 Asc 角被入射侧的负角平均拉低,可能跌破 posThresh。
|
||||||
|
// 保护原则:raw 超阈 → 强制保留为特征点;raw 未超阈 → 允许平滑结果覆盖。
|
||||||
|
for (int i = 0; i < count; i++) {
|
||||||
|
if (!pts[i].hasAngle) continue;
|
||||||
|
float raw = rawAngles[i];
|
||||||
|
if (raw > posThresh && pts[i].trend != keLineAngleTrend_PositiveJump) {
|
||||||
|
pts[i].signedAngleDeg = raw;
|
||||||
|
pts[i].trend = keLineAngleTrend_PositiveJump;
|
||||||
|
}
|
||||||
|
else if (raw < negThresh && pts[i].trend != keLineAngleTrend_NegativeJump) {
|
||||||
|
pts[i].signedAngleDeg = raw;
|
||||||
|
pts[i].trend = keLineAngleTrend_NegativeJump;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ================================================================
|
||||||
|
// 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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 短段合并:单点的 Ascending/Descending 段视为噪声,合并回 Flat
|
||||||
|
// 例外:紧邻 Gap 段的单点 Asc/Desc 是孔洞边缘的真实信号,不应消除
|
||||||
|
{
|
||||||
|
for (size_t k = 0; k < segs.size(); k++) {
|
||||||
|
auto& seg = segs[k];
|
||||||
|
if (seg.type != ESegType::Flat && seg.type != ESegType::Gap) {
|
||||||
|
int segLen = seg.endPos - seg.startPos + 1;
|
||||||
|
if (segLen < 2) {
|
||||||
|
bool adjToGap = (k > 0 && segs[k - 1].type == ESegType::Gap)
|
||||||
|
|| (k + 1 < segs.size() && segs[k + 1].type == ESegType::Gap);
|
||||||
|
if (!adjToGap) {
|
||||||
|
seg.type = ESegType::Flat;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// 合并相邻的 Flat 段
|
||||||
|
std::vector<Segment> merged;
|
||||||
|
for (const auto& seg : segs) {
|
||||||
|
if (!merged.empty() && merged.back().type == ESegType::Flat && seg.type == ESegType::Flat) {
|
||||||
|
merged.back().endPos = seg.endPos;
|
||||||
|
merged.back().length += seg.length;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
merged.push_back(seg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
segs = merged;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float minBarDiameter = params.minBarDiameter;
|
||||||
|
const float maxBarDiameter = params.maxBarDiameter;
|
||||||
|
for (size_t si = 0; si < segs.size(); ++si) {
|
||||||
|
const Segment& seg = segs[si];
|
||||||
|
if (seg.type != ESegType::Flat) continue;
|
||||||
|
if (seg.length < minBarDiameter || seg.length > maxBarDiameter) continue;
|
||||||
|
|
||||||
|
bool hasPoint = false;
|
||||||
|
SLineSegment lineSeg;
|
||||||
|
for (int pos = seg.startPos; pos <= seg.endPos; ++pos) {
|
||||||
|
if (!pts[pos].valid) continue;
|
||||||
|
|
||||||
|
SGridPoint gp;
|
||||||
|
gp.row = pts[pos].row;
|
||||||
|
gp.col = pts[pos].col;
|
||||||
|
gp.linearIdx = pts[pos].globalIdx;
|
||||||
|
|
||||||
|
if (!hasPoint) {
|
||||||
|
lineSeg = StartNewSegment(gp, pts[pos].point);
|
||||||
|
hasPoint = true;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
AppendPoint(lineSeg, gp, pts[pos].point);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!hasPoint) continue;
|
||||||
|
FinalizeSegment(lineSeg);
|
||||||
|
currRowSegments.push_back(lineSeg);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void ProcessCurrentRowSegmentPoint(
|
static void ProcessCurrentRowSegmentPoint(
|
||||||
@ -128,6 +528,10 @@ static void ProcessCurrentRowSegmentPoint(
|
|||||||
const float eps = 1e-6f;
|
const float eps = 1e-6f;
|
||||||
const int cols = static_cast<int>(pointCount);
|
const int cols = static_cast<int>(pointCount);
|
||||||
const int rowOffset = row * cols;
|
const int rowOffset = row * cols;
|
||||||
|
const float segmentDistanceThresholdSq =
|
||||||
|
params.thresholdX * params.thresholdX +
|
||||||
|
params.thresholdY * params.thresholdY +
|
||||||
|
params.thresholdZ * params.thresholdZ;
|
||||||
|
|
||||||
bool hasOpenSegment = false;
|
bool hasOpenSegment = false;
|
||||||
SLineSegment openSeg;
|
SLineSegment openSeg;
|
||||||
@ -152,10 +556,12 @@ static void ProcessCurrentRowSegmentPoint(
|
|||||||
|
|
||||||
const SGridPoint& lastGp = openSeg.points.back();
|
const SGridPoint& lastGp = openSeg.points.back();
|
||||||
const SVzNLPointXYZ& lastPt = point[lastGp.col];
|
const SVzNLPointXYZ& lastPt = point[lastGp.col];
|
||||||
float dy = std::abs(pt.y - lastPt.y);
|
const float dx = pt.x - lastPt.x;
|
||||||
float dz = std::abs(pt.z - lastPt.z);
|
const float dy = pt.y - lastPt.y;
|
||||||
|
const float dz = pt.z - lastPt.z;
|
||||||
|
const float distanceSq = dx * dx + dy * dy + dz * dz;
|
||||||
|
|
||||||
if (dy < params.thresholdY && dz < params.thresholdZ) {
|
if (distanceSq < segmentDistanceThresholdSq) {
|
||||||
AppendPoint(openSeg, gp, pt);
|
AppendPoint(openSeg, gp, pt);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
@ -173,42 +579,31 @@ static void ProcessCurrentRowSegmentPoint(
|
|||||||
|
|
||||||
static void MergeCurrentRowSegments(
|
static void MergeCurrentRowSegments(
|
||||||
const std::vector<SLineSegment>& prevRowSegments,
|
const std::vector<SLineSegment>& prevRowSegments,
|
||||||
|
const std::vector<SLineSegment>& carriedRowSegments,
|
||||||
std::vector<SLineSegment>& currRowSegments,
|
std::vector<SLineSegment>& currRowSegments,
|
||||||
UnionFind& uf,
|
UnionFind& uf,
|
||||||
const SGrowthParams& params
|
const SGrowthParams& params,
|
||||||
|
std::vector<SLineSegment>& nextCarriedRowSegments
|
||||||
) {
|
) {
|
||||||
if (currRowSegments.empty()) {
|
nextCarriedRowSegments.clear();
|
||||||
return;
|
std::vector<SCarrySegmentState> prevStates(prevRowSegments.size(), SCarrySegmentState{ false });
|
||||||
}
|
std::vector<SCarrySegmentState> carriedStates(carriedRowSegments.size(), SCarrySegmentState{ false });
|
||||||
|
|
||||||
bool rowsAdjacent = false;
|
|
||||||
if (!prevRowSegments.empty()) {
|
|
||||||
rowsAdjacent = (currRowSegments[0].row == prevRowSegments[0].row + 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
for (size_t ci = 0; ci < currRowSegments.size(); ++ci) {
|
for (size_t ci = 0; ci < currRowSegments.size(); ++ci) {
|
||||||
SLineSegment& currSeg = currRowSegments[ci];
|
SLineSegment& currSeg = currRowSegments[ci];
|
||||||
std::vector<int> matchedRoots;
|
std::vector<int> matchedRoots;
|
||||||
int bestRoot = -1;
|
int bestRoot = -1;
|
||||||
float bestDx = std::numeric_limits<float>::max();
|
float bestDistanceSq = std::numeric_limits<float>::max();
|
||||||
|
|
||||||
if (rowsAdjacent) {
|
auto tryMatchSegments = [&](const std::vector<SLineSegment>& candidateSegments, std::vector<SCarrySegmentState>& candidateStates) {
|
||||||
for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) {
|
for (size_t pi = 0; pi < candidateSegments.size(); ++pi) {
|
||||||
const SLineSegment& prevSeg = prevRowSegments[pi];
|
const SLineSegment& prevSeg = candidateSegments[pi];
|
||||||
if (!HasColumnOverlapOrTouch(prevSeg, currSeg)) {
|
const SSegmentRelationResult relation = EvaluateSegmentRelation(prevSeg, currSeg, params);
|
||||||
continue;
|
if (!relation.hasMatch) {
|
||||||
}
|
|
||||||
|
|
||||||
const float dx = std::abs(currSeg.centroid.x - prevSeg.centroid.x);
|
|
||||||
if (dx >= params.thresholdX) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
const float dz = std::abs(currSeg.centroid.z - prevSeg.centroid.z);
|
|
||||||
if (dz >= params.thresholdZ) {
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
candidateStates[pi].hasMatch = true;
|
||||||
const int root = uf.Find(prevSeg.treeId);
|
const int root = uf.Find(prevSeg.treeId);
|
||||||
bool found = false;
|
bool found = false;
|
||||||
for (size_t m = 0; m < matchedRoots.size(); ++m) {
|
for (size_t m = 0; m < matchedRoots.size(); ++m) {
|
||||||
@ -221,12 +616,15 @@ static void MergeCurrentRowSegments(
|
|||||||
matchedRoots.push_back(root);
|
matchedRoots.push_back(root);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (dx < bestDx) {
|
if (relation.bestDistanceSq < bestDistanceSq) {
|
||||||
bestDx = dx;
|
bestDistanceSq = relation.bestDistanceSq;
|
||||||
bestRoot = root;
|
bestRoot = root;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
};
|
||||||
|
|
||||||
|
tryMatchSegments(prevRowSegments, prevStates);
|
||||||
|
tryMatchSegments(carriedRowSegments, carriedStates);
|
||||||
|
|
||||||
if (matchedRoots.empty()) {
|
if (matchedRoots.empty()) {
|
||||||
currSeg.treeId = uf.MakeSet();
|
currSeg.treeId = uf.MakeSet();
|
||||||
@ -239,6 +637,20 @@ static void MergeCurrentRowSegments(
|
|||||||
}
|
}
|
||||||
currSeg.treeId = uf.Find(currSeg.treeId);
|
currSeg.treeId = uf.Find(currSeg.treeId);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) {
|
||||||
|
const SLineSegment& prevSeg = prevRowSegments[pi];
|
||||||
|
if (!prevStates[pi].hasMatch) {
|
||||||
|
nextCarriedRowSegments.push_back(prevSeg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (size_t pi = 0; pi < carriedRowSegments.size(); ++pi) {
|
||||||
|
const SLineSegment& carriedSeg = carriedRowSegments[pi];
|
||||||
|
if (!carriedStates[pi].hasMatch) {
|
||||||
|
nextCarriedRowSegments.push_back(carriedSeg);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void FlattenTreesToClusters(
|
static void FlattenTreesToClusters(
|
||||||
@ -321,11 +733,10 @@ int RegionGrowClusters(
|
|||||||
const SVzNLPointXYZ* points,
|
const SVzNLPointXYZ* points,
|
||||||
int rows,
|
int rows,
|
||||||
int cols,
|
int cols,
|
||||||
|
bool bHorizontalScan,
|
||||||
const SGrowthParams& params,
|
const SGrowthParams& params,
|
||||||
std::vector<SGrowthCluster>& outClusters
|
std::vector<SGrowthCluster>& outClusters
|
||||||
) {
|
) {
|
||||||
outClusters.clear();
|
|
||||||
|
|
||||||
if (!points || cols <= 0 || rows <= 0) {
|
if (!points || cols <= 0 || rows <= 0) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
@ -333,24 +744,83 @@ int RegionGrowClusters(
|
|||||||
UnionFind uf;
|
UnionFind uf;
|
||||||
std::vector<SLineSegment> allSegments;
|
std::vector<SLineSegment> allSegments;
|
||||||
std::vector<SLineSegment> prevRowSegments;
|
std::vector<SLineSegment> prevRowSegments;
|
||||||
|
std::vector<SLineSegment> carriedRowSegments;
|
||||||
|
std::vector<SLineSegment> nextCarriedRowSegments;
|
||||||
std::vector<SLineSegment> currRowSegments;
|
std::vector<SLineSegment> currRowSegments;
|
||||||
|
|
||||||
for (int row = 0; row < rows; ++row) {
|
if (bHorizontalScan)
|
||||||
const SVzNLPointXYZ* rowPoints = points + row * cols;
|
{
|
||||||
|
int nPointIdx = 0;
|
||||||
|
const SVzNLPointXYZ* rowPoints = points;
|
||||||
|
for (int row = 0; row < rows; ++row) {
|
||||||
|
bool bIsValidLine = false;
|
||||||
|
EvaluateLine(
|
||||||
|
rowPoints,
|
||||||
|
static_cast<unsigned int>(cols),
|
||||||
|
1,
|
||||||
|
nPointIdx,
|
||||||
|
cols,
|
||||||
|
params,
|
||||||
|
currRowSegments,
|
||||||
|
bIsValidLine,
|
||||||
|
EPanelType::YZ_PANEL
|
||||||
|
);
|
||||||
|
|
||||||
ProcessCurrentRowSegmentPoint(
|
MergeCurrentRowSegments(
|
||||||
rowPoints,
|
prevRowSegments,
|
||||||
static_cast<unsigned int>(cols),
|
carriedRowSegments,
|
||||||
row,
|
currRowSegments,
|
||||||
params,
|
uf,
|
||||||
currRowSegments
|
params,
|
||||||
);
|
nextCarriedRowSegments
|
||||||
|
);
|
||||||
|
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
|
||||||
|
|
||||||
MergeCurrentRowSegments(prevRowSegments, currRowSegments, uf, params);
|
carriedRowSegments.swap(nextCarriedRowSegments);
|
||||||
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
|
nextCarriedRowSegments.clear();
|
||||||
|
prevRowSegments.swap(currRowSegments);
|
||||||
|
currRowSegments.clear();
|
||||||
|
|
||||||
prevRowSegments.swap(currRowSegments);
|
nPointIdx += cols;
|
||||||
currRowSegments.clear();
|
rowPoints += cols;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
int nPointIdx = 0;
|
||||||
|
const SVzNLPointXYZ* colPoints = points;
|
||||||
|
for (int col = 0; col < cols; ++col) {
|
||||||
|
|
||||||
|
bool bIsValidLine = false;
|
||||||
|
EvaluateLine(
|
||||||
|
colPoints,
|
||||||
|
rows,
|
||||||
|
cols,
|
||||||
|
nPointIdx,
|
||||||
|
cols,
|
||||||
|
params,
|
||||||
|
currRowSegments,
|
||||||
|
bIsValidLine,
|
||||||
|
EPanelType::XZ_PANEL
|
||||||
|
);
|
||||||
|
|
||||||
|
MergeCurrentRowSegments(
|
||||||
|
prevRowSegments,
|
||||||
|
carriedRowSegments,
|
||||||
|
currRowSegments,
|
||||||
|
uf,
|
||||||
|
params,
|
||||||
|
nextCarriedRowSegments
|
||||||
|
);
|
||||||
|
allSegments.insert(allSegments.end(), currRowSegments.begin(), currRowSegments.end());
|
||||||
|
|
||||||
|
carriedRowSegments.swap(nextCarriedRowSegments);
|
||||||
|
nextCarriedRowSegments.clear();
|
||||||
|
prevRowSegments.swap(currRowSegments);
|
||||||
|
currRowSegments.clear();
|
||||||
|
colPoints++;
|
||||||
|
nPointIdx++;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
FlattenTreesToClusters(allSegments, uf, points, params, outClusters);
|
FlattenTreesToClusters(allSegments, uf, points, params, outClusters);
|
||||||
|
|||||||
@ -1,4 +1,4 @@
|
|||||||
#ifndef REGION_GROWING_H
|
#ifndef REGION_GROWING_H
|
||||||
#define REGION_GROWING_H
|
#define REGION_GROWING_H
|
||||||
|
|
||||||
#include "VZNL_Types.h"
|
#include "VZNL_Types.h"
|
||||||
@ -28,6 +28,7 @@ int RegionGrowClusters(
|
|||||||
const SVzNLPointXYZ* points,
|
const SVzNLPointXYZ* points,
|
||||||
int rows,
|
int rows,
|
||||||
int cols,
|
int cols,
|
||||||
|
bool bHorizontalScan,
|
||||||
const SGrowthParams& params,
|
const SGrowthParams& params,
|
||||||
std::vector<SGrowthCluster>& outClusters
|
std::vector<SGrowthCluster>& outClusters
|
||||||
);
|
);
|
||||||
|
|||||||
@ -16,6 +16,8 @@ struct RansacPlaneSegmentationParams {
|
|||||||
float minPlaneRatio; // 平面最小点数占比 (相对最大平面), 建议 0.05-0.2
|
float minPlaneRatio; // 平面最小点数占比 (相对最大平面), 建议 0.05-0.2
|
||||||
float maxNormalAngleDeg; // 平面法向量与Z轴最大夹角 (度), 超过则直接丢弃; <=0 表示不过滤
|
float maxNormalAngleDeg; // 平面法向量与Z轴最大夹角 (度), 超过则直接丢弃; <=0 表示不过滤
|
||||||
|
|
||||||
|
float maxDistFromPlane; // 点到平面的最大允许距离;超出则从 pointIndices 中移除;<=0 表示不过滤
|
||||||
|
|
||||||
RansacPlaneSegmentationParams()
|
RansacPlaneSegmentationParams()
|
||||||
: distanceThreshold(0.5f)
|
: distanceThreshold(0.5f)
|
||||||
, maxIterations(500)
|
, maxIterations(500)
|
||||||
@ -24,6 +26,7 @@ struct RansacPlaneSegmentationParams {
|
|||||||
, growthZThreshold(1.0f)
|
, growthZThreshold(1.0f)
|
||||||
, minPlaneRatio(0.1f)
|
, minPlaneRatio(0.1f)
|
||||||
, maxNormalAngleDeg(30.0f)
|
, maxNormalAngleDeg(30.0f)
|
||||||
|
, maxDistFromPlane(1.f)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|||||||
@ -12,7 +12,7 @@
|
|||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
const static char* g_algo_name = "DetectHole";
|
const static char* g_algo_name = "DetectHole";
|
||||||
const static char* g_algo_ver = "1.0.2";
|
const static char* g_algo_ver = "1.0.3";
|
||||||
|
|
||||||
|
|
||||||
void PrintHoleResult(const SHoleResult& hole, int index) {
|
void PrintHoleResult(const SHoleResult& hole, int index) {
|
||||||
@ -196,6 +196,7 @@ int DetectMultipleHoles(
|
|||||||
|
|
||||||
// ============ Step 2: 法向量过滤 ============
|
// ============ Step 2: 法向量过滤 ============
|
||||||
NormalFilterParams normalParams;
|
NormalFilterParams normalParams;
|
||||||
|
normalParams.maxDistFromPlane = ransacParams.maxDistFromPlane;
|
||||||
FilterPlanesByNormal(planes, normalParams, points, totalPointCount);
|
FilterPlanesByNormal(planes, normalParams, points, totalPointCount);
|
||||||
|
|
||||||
// 如果没有找到有效平面,回退到处理整个点云
|
// 如果没有找到有效平面,回退到处理整个点云
|
||||||
|
|||||||
@ -298,7 +298,7 @@ struct NormalFilterParams {
|
|||||||
, refNx(0.0f)
|
, refNx(0.0f)
|
||||||
, refNy(0.0f)
|
, refNy(0.0f)
|
||||||
, refNz(1.0f)
|
, refNz(1.0f)
|
||||||
, maxDistFromPlane(2.0f)
|
, maxDistFromPlane(0.0f)
|
||||||
{}
|
{}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user