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
|
||||
nul
|
||||
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_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(MSVC)
|
||||
add_compile_options(
|
||||
/wd4267
|
||||
/wd4996
|
||||
$<$<CONFIG:Release>:/O2>
|
||||
)
|
||||
if(CMAKE_BUILD_TYPE STREQUAL "Release")
|
||||
add_compile_options(/O2)
|
||||
endif()
|
||||
endif()
|
||||
else()
|
||||
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
|
||||
@ -235,7 +244,7 @@ endif()
|
||||
message(STATUS "")
|
||||
message(STATUS "DetectBarIntersection Configuration Summary:")
|
||||
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 " OpenMP Enabled: ${ENABLE_OPENMP}")
|
||||
message(STATUS " Build Examples: ${BUILD_EXAMPLES}")
|
||||
|
||||
@ -1,26 +1,33 @@
|
||||
@echo off
|
||||
setlocal
|
||||
setlocal EnableExtensions
|
||||
|
||||
echo === Building DetectBarIntersection ===
|
||||
set "BUILD_CONFIG=Debug"
|
||||
|
||||
if not exist build (
|
||||
mkdir build
|
||||
if not "%~1"=="" (
|
||||
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!
|
||||
exit /b %ERRORLEVEL%
|
||||
)
|
||||
|
||||
cmake --build . --config Debug
|
||||
cmake --build build --config %BUILD_CONFIG%
|
||||
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
if errorlevel 1 (
|
||||
echo Build failed!
|
||||
exit /b %ERRORLEVEL%
|
||||
)
|
||||
|
||||
echo === Build completed successfully ===
|
||||
echo === Build completed successfully [%BUILD_CONFIG%] ===
|
||||
|
||||
@ -10,6 +10,6 @@ endif()
|
||||
|
||||
message(STATUS "")
|
||||
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 "")
|
||||
|
||||
@ -60,14 +60,25 @@ if(VTK_FOUND)
|
||||
|
||||
# Copy VTK DLLs
|
||||
file(GLOB VTK_DLLS "${VTK_BIN_DIR}/*.dll")
|
||||
set(VTK_DEBUG_DLLS)
|
||||
set(VTK_RELEASE_DLLS)
|
||||
foreach(vtk_dll ${VTK_DLLS})
|
||||
get_filename_component(vtk_dll_name "${vtk_dll}" NAME)
|
||||
if(vtk_dll_name MATCHES "d\\.dll$")
|
||||
list(APPEND VTK_DEBUG_DLLS "${vtk_dll}")
|
||||
else()
|
||||
list(APPEND VTK_RELEASE_DLLS "${vtk_dll}")
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
add_custom_command(TARGET visualization_demo POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy_if_different
|
||||
"${vtk_dll}"
|
||||
"$<$<CONFIG:Debug>:${VTK_DEBUG_DLLS}>"
|
||||
"$<$<NOT:$<CONFIG:Debug>>:${VTK_RELEASE_DLLS}>"
|
||||
"$<TARGET_FILE_DIR:visualization_demo>"
|
||||
COMMENT "Copying VTK DLL: ${vtk_dll}"
|
||||
COMMAND_EXPAND_LISTS
|
||||
COMMENT "Copying configuration-matched VTK DLLs to visualization_demo output directory"
|
||||
)
|
||||
endforeach()
|
||||
endif()
|
||||
else()
|
||||
message(WARNING "VTK not found. visualization_demo will not be built.")
|
||||
|
||||
@ -224,7 +224,7 @@ int ProcessSingleFile(
|
||||
debugCallbacks.onPlaneSegmented = nullptr;// OnPlaneSegmented;
|
||||
debugCallbacks.onPointsAligned = nullptr;// OnPointsAligned;
|
||||
debugCallbacks.onPlaneFiltered = nullptr;// OnPlaneFiltered;
|
||||
debugCallbacks.onClustersDetected = OnClustersDetected;
|
||||
debugCallbacks.onClustersDetected = nullptr;// OnClustersDetected;
|
||||
debugCallbacks.userData = &callbackData;
|
||||
|
||||
// Set parameters
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#ifndef BAR_INTERSECTION_PARAMS_H
|
||||
#ifndef BAR_INTERSECTION_PARAMS_H
|
||||
#define BAR_INTERSECTION_PARAMS_H
|
||||
|
||||
#include "VZNL_Types.h"
|
||||
@ -31,12 +31,28 @@ struct SGrowthParams {
|
||||
float thresholdY; // 行内分段阈值:同行相邻点 y 的最大允许差 (mm)
|
||||
float thresholdZ; // 行内分段阈值:同行相邻点 z 的最大允许差 (mm)
|
||||
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()
|
||||
: thresholdX(5.0f)
|
||||
, thresholdY(5.0f)
|
||||
, thresholdZ(5.0f)
|
||||
, 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 "PlaneAlignment.h"
|
||||
#include "RegionGrowing.h"
|
||||
#include <Eigen/Dense>
|
||||
#include <vector>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
@ -9,7 +10,7 @@
|
||||
#include <numeric>
|
||||
#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 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);
|
||||
}
|
||||
|
||||
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,
|
||||
int rows,
|
||||
int cols,
|
||||
@ -107,6 +208,17 @@ BAR_INTERSECTION_API int DetectBarIntersections(
|
||||
alignedPoints, totalPoints, planeZ, planeParams.heightThreshold
|
||||
);
|
||||
|
||||
// ============================================
|
||||
// Step 2b: 按激光线过滤短连续段噪点
|
||||
// ============================================
|
||||
FilterLaserLineNoiseInPlace(
|
||||
alignedPoints,
|
||||
rows,
|
||||
cols,
|
||||
growthParams.minContinuousValidPointCount,
|
||||
growthParams.maxContinuousPointZTolerance
|
||||
);
|
||||
|
||||
// Debug callback: plane filtered
|
||||
if (debugCallbacks && debugCallbacks->onPlaneFiltered) {
|
||||
// 收集非零点用于回调显示
|
||||
@ -137,7 +249,10 @@ BAR_INTERSECTION_API int DetectBarIntersections(
|
||||
// ============================================
|
||||
std::vector<SGrowthCluster> clusters;
|
||||
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 <cmath>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
#undef min
|
||||
#undef max
|
||||
|
||||
namespace bar_intersection {
|
||||
|
||||
struct SGridPoint {
|
||||
@ -111,8 +114,405 @@ static void FinalizeSegment(SLineSegment& seg) {
|
||||
}
|
||||
}
|
||||
|
||||
static bool HasColumnOverlapOrTouch(const SLineSegment& a, const SLineSegment& b) {
|
||||
return !(b.endCol < a.startCol - 1 || a.endCol < b.startCol - 1);
|
||||
struct SSegmentRelationResult {
|
||||
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(
|
||||
@ -128,6 +528,10 @@ static void ProcessCurrentRowSegmentPoint(
|
||||
const float eps = 1e-6f;
|
||||
const int cols = static_cast<int>(pointCount);
|
||||
const int rowOffset = row * cols;
|
||||
const float segmentDistanceThresholdSq =
|
||||
params.thresholdX * params.thresholdX +
|
||||
params.thresholdY * params.thresholdY +
|
||||
params.thresholdZ * params.thresholdZ;
|
||||
|
||||
bool hasOpenSegment = false;
|
||||
SLineSegment openSeg;
|
||||
@ -152,10 +556,12 @@ static void ProcessCurrentRowSegmentPoint(
|
||||
|
||||
const SGridPoint& lastGp = openSeg.points.back();
|
||||
const SVzNLPointXYZ& lastPt = point[lastGp.col];
|
||||
float dy = std::abs(pt.y - lastPt.y);
|
||||
float dz = std::abs(pt.z - lastPt.z);
|
||||
const float dx = pt.x - lastPt.x;
|
||||
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);
|
||||
continue;
|
||||
}
|
||||
@ -173,42 +579,31 @@ static void ProcessCurrentRowSegmentPoint(
|
||||
|
||||
static void MergeCurrentRowSegments(
|
||||
const std::vector<SLineSegment>& prevRowSegments,
|
||||
const std::vector<SLineSegment>& carriedRowSegments,
|
||||
std::vector<SLineSegment>& currRowSegments,
|
||||
UnionFind& uf,
|
||||
const SGrowthParams& params
|
||||
const SGrowthParams& params,
|
||||
std::vector<SLineSegment>& nextCarriedRowSegments
|
||||
) {
|
||||
if (currRowSegments.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool rowsAdjacent = false;
|
||||
if (!prevRowSegments.empty()) {
|
||||
rowsAdjacent = (currRowSegments[0].row == prevRowSegments[0].row + 1);
|
||||
}
|
||||
nextCarriedRowSegments.clear();
|
||||
std::vector<SCarrySegmentState> prevStates(prevRowSegments.size(), SCarrySegmentState{ false });
|
||||
std::vector<SCarrySegmentState> carriedStates(carriedRowSegments.size(), SCarrySegmentState{ false });
|
||||
|
||||
for (size_t ci = 0; ci < currRowSegments.size(); ++ci) {
|
||||
SLineSegment& currSeg = currRowSegments[ci];
|
||||
std::vector<int> matchedRoots;
|
||||
int bestRoot = -1;
|
||||
float bestDx = std::numeric_limits<float>::max();
|
||||
float bestDistanceSq = std::numeric_limits<float>::max();
|
||||
|
||||
if (rowsAdjacent) {
|
||||
for (size_t pi = 0; pi < prevRowSegments.size(); ++pi) {
|
||||
const SLineSegment& prevSeg = prevRowSegments[pi];
|
||||
if (!HasColumnOverlapOrTouch(prevSeg, currSeg)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
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) {
|
||||
auto tryMatchSegments = [&](const std::vector<SLineSegment>& candidateSegments, std::vector<SCarrySegmentState>& candidateStates) {
|
||||
for (size_t pi = 0; pi < candidateSegments.size(); ++pi) {
|
||||
const SLineSegment& prevSeg = candidateSegments[pi];
|
||||
const SSegmentRelationResult relation = EvaluateSegmentRelation(prevSeg, currSeg, params);
|
||||
if (!relation.hasMatch) {
|
||||
continue;
|
||||
}
|
||||
|
||||
candidateStates[pi].hasMatch = true;
|
||||
const int root = uf.Find(prevSeg.treeId);
|
||||
bool found = false;
|
||||
for (size_t m = 0; m < matchedRoots.size(); ++m) {
|
||||
@ -221,12 +616,15 @@ static void MergeCurrentRowSegments(
|
||||
matchedRoots.push_back(root);
|
||||
}
|
||||
|
||||
if (dx < bestDx) {
|
||||
bestDx = dx;
|
||||
if (relation.bestDistanceSq < bestDistanceSq) {
|
||||
bestDistanceSq = relation.bestDistanceSq;
|
||||
bestRoot = root;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
tryMatchSegments(prevRowSegments, prevStates);
|
||||
tryMatchSegments(carriedRowSegments, carriedStates);
|
||||
|
||||
if (matchedRoots.empty()) {
|
||||
currSeg.treeId = uf.MakeSet();
|
||||
@ -239,6 +637,20 @@ static void MergeCurrentRowSegments(
|
||||
}
|
||||
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(
|
||||
@ -321,11 +733,10 @@ int RegionGrowClusters(
|
||||
const SVzNLPointXYZ* points,
|
||||
int rows,
|
||||
int cols,
|
||||
bool bHorizontalScan,
|
||||
const SGrowthParams& params,
|
||||
std::vector<SGrowthCluster>& outClusters
|
||||
) {
|
||||
outClusters.clear();
|
||||
|
||||
if (!points || cols <= 0 || rows <= 0) {
|
||||
return 0;
|
||||
}
|
||||
@ -333,24 +744,83 @@ int RegionGrowClusters(
|
||||
UnionFind uf;
|
||||
std::vector<SLineSegment> allSegments;
|
||||
std::vector<SLineSegment> prevRowSegments;
|
||||
std::vector<SLineSegment> carriedRowSegments;
|
||||
std::vector<SLineSegment> nextCarriedRowSegments;
|
||||
std::vector<SLineSegment> currRowSegments;
|
||||
|
||||
if (bHorizontalScan)
|
||||
{
|
||||
int nPointIdx = 0;
|
||||
const SVzNLPointXYZ* rowPoints = points;
|
||||
for (int row = 0; row < rows; ++row) {
|
||||
const SVzNLPointXYZ* rowPoints = points + row * cols;
|
||||
|
||||
ProcessCurrentRowSegmentPoint(
|
||||
bool bIsValidLine = false;
|
||||
EvaluateLine(
|
||||
rowPoints,
|
||||
static_cast<unsigned int>(cols),
|
||||
row,
|
||||
1,
|
||||
nPointIdx,
|
||||
cols,
|
||||
params,
|
||||
currRowSegments
|
||||
currRowSegments,
|
||||
bIsValidLine,
|
||||
EPanelType::YZ_PANEL
|
||||
);
|
||||
|
||||
MergeCurrentRowSegments(prevRowSegments, currRowSegments, uf, params);
|
||||
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();
|
||||
|
||||
nPointIdx += cols;
|
||||
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);
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#ifndef REGION_GROWING_H
|
||||
#ifndef REGION_GROWING_H
|
||||
#define REGION_GROWING_H
|
||||
|
||||
#include "VZNL_Types.h"
|
||||
@ -28,6 +28,7 @@ int RegionGrowClusters(
|
||||
const SVzNLPointXYZ* points,
|
||||
int rows,
|
||||
int cols,
|
||||
bool bHorizontalScan,
|
||||
const SGrowthParams& params,
|
||||
std::vector<SGrowthCluster>& outClusters
|
||||
);
|
||||
|
||||
@ -16,6 +16,8 @@ struct RansacPlaneSegmentationParams {
|
||||
float minPlaneRatio; // 平面最小点数占比 (相对最大平面), 建议 0.05-0.2
|
||||
float maxNormalAngleDeg; // 平面法向量与Z轴最大夹角 (度), 超过则直接丢弃; <=0 表示不过滤
|
||||
|
||||
float maxDistFromPlane; // 点到平面的最大允许距离;超出则从 pointIndices 中移除;<=0 表示不过滤
|
||||
|
||||
RansacPlaneSegmentationParams()
|
||||
: distanceThreshold(0.5f)
|
||||
, maxIterations(500)
|
||||
@ -24,6 +26,7 @@ struct RansacPlaneSegmentationParams {
|
||||
, growthZThreshold(1.0f)
|
||||
, minPlaneRatio(0.1f)
|
||||
, maxNormalAngleDeg(30.0f)
|
||||
, maxDistFromPlane(1.f)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
@ -12,7 +12,7 @@
|
||||
namespace {
|
||||
|
||||
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) {
|
||||
@ -196,6 +196,7 @@ int DetectMultipleHoles(
|
||||
|
||||
// ============ Step 2: 法向量过滤 ============
|
||||
NormalFilterParams normalParams;
|
||||
normalParams.maxDistFromPlane = ransacParams.maxDistFromPlane;
|
||||
FilterPlanesByNormal(planes, normalParams, points, totalPointCount);
|
||||
|
||||
// 如果没有找到有效平面,回退到处理整个点云
|
||||
|
||||
@ -298,7 +298,7 @@ struct NormalFilterParams {
|
||||
, refNx(0.0f)
|
||||
, refNy(0.0f)
|
||||
, refNz(1.0f)
|
||||
, maxDistFromPlane(2.0f)
|
||||
, maxDistFromPlane(0.0f)
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user