1550 lines
54 KiB
C++
1550 lines
54 KiB
C++
#include "PlaneSegmentation.h"
|
||
#include "HoleDetection.h"
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <iostream>
|
||
#include <map>
|
||
#include <queue>
|
||
|
||
// 辅助函数：检查点是否有效
|
||
static bool IsValidPoint(const SVzNLPointXYZ& pt) {
|
||
constexpr float kEpsilon = 1e-6f;
|
||
if (std::abs(pt.x) < kEpsilon &&
|
||
std::abs(pt.y) < kEpsilon &&
|
||
std::abs(pt.z) < kEpsilon) {
|
||
return false;
|
||
}
|
||
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
|
||
}
|
||
|
||
int SegmentPlanesByZHistogram(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
const ZHistogramSegmentationParams& params,
|
||
std::vector<ZHistogramPeak>& outPeaks,
|
||
int* errCode
|
||
) {
|
||
if (points == nullptr || pointCount < 3 || errCode == nullptr) {
|
||
if (errCode) *errCode = HD_ERR_INVALID_INPUT;
|
||
return 0;
|
||
}
|
||
|
||
outPeaks.clear();
|
||
|
||
// ========== 步骤1：计算Z值范围 ==========
|
||
float minZ = std::numeric_limits<float>::max();
|
||
float maxZ = std::numeric_limits<float>::lowest();
|
||
int validCount = 0;
|
||
|
||
for (int i = 0; i < pointCount; i++) {
|
||
if (!IsValidPoint(points[i])) continue;
|
||
|
||
float z = points[i].z;
|
||
if (z < minZ) minZ = z;
|
||
if (z > maxZ) maxZ = z;
|
||
validCount++;
|
||
}
|
||
|
||
if (validCount < params.minPeakPoints) {
|
||
*errCode = HD_ERR_INVALID_INPUT;
|
||
return 0;
|
||
}
|
||
|
||
std::cout << "[Z-Histogram] Z range: [" << minZ << ", " << maxZ
|
||
<< "], valid points: " << validCount << std::endl;
|
||
|
||
// ========== 步骤2：创建直方图 ==========
|
||
float zRange = maxZ - minZ;
|
||
int numBins = static_cast<int>(std::ceil(zRange / params.binSize)) + 1;
|
||
|
||
if (numBins < 2) {
|
||
std::cout << "[Z-Histogram] All points in same Z layer" << std::endl;
|
||
*errCode = HD_ERR_NO_PITS_DETECTED;
|
||
return 0;
|
||
}
|
||
|
||
std::vector<int> histogram(numBins, 0);
|
||
std::vector<std::vector<int>> binPointIndices(numBins);
|
||
|
||
// 统计每个bin的点数
|
||
for (int i = 0; i < pointCount; i++) {
|
||
if (!IsValidPoint(points[i])) continue;
|
||
|
||
float z = points[i].z;
|
||
int binIdx = static_cast<int>((z - minZ) / params.binSize);
|
||
if (binIdx >= numBins) binIdx = numBins - 1;
|
||
if (binIdx < 0) binIdx = 0;
|
||
|
||
histogram[binIdx]++;
|
||
binPointIndices[binIdx].push_back(i);
|
||
}
|
||
|
||
// ========== 步骤3：平滑直方图（可选）==========
|
||
if (params.smoothingWindow > 1) {
|
||
std::vector<int> smoothed(numBins, 0);
|
||
int halfWindow = params.smoothingWindow / 2;
|
||
|
||
for (int i = 0; i < numBins; i++) {
|
||
int sum = 0;
|
||
int count = 0;
|
||
for (int j = std::max(0, i - halfWindow);
|
||
j <= std::min(numBins - 1, i + halfWindow); j++) {
|
||
sum += histogram[j];
|
||
count++;
|
||
}
|
||
smoothed[i] = sum / count;
|
||
}
|
||
histogram = smoothed;
|
||
}
|
||
|
||
// ========== 步骤4：检测峰值 ==========
|
||
std::vector<int> peakBins;
|
||
|
||
for (int i = 1; i < numBins - 1; i++) {
|
||
// 局部最大值检测
|
||
if (histogram[i] > histogram[i-1] && histogram[i] > histogram[i+1]) {
|
||
// 检查峰值是否足够显著
|
||
if (histogram[i] >= params.minPeakPoints) {
|
||
peakBins.push_back(i);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 边界检查
|
||
if (numBins > 0 && histogram[0] >= params.minPeakPoints) {
|
||
if (numBins == 1 || histogram[0] > histogram[1]) {
|
||
peakBins.insert(peakBins.begin(), 0);
|
||
}
|
||
}
|
||
if (numBins > 1 && histogram[numBins-1] >= params.minPeakPoints) {
|
||
if (histogram[numBins-1] > histogram[numBins-2]) {
|
||
peakBins.push_back(numBins - 1);
|
||
}
|
||
}
|
||
|
||
std::cout << "[Z-Histogram] Found " << peakBins.size()
|
||
<< " initial peaks" << std::endl;
|
||
|
||
if (peakBins.empty()) {
|
||
*errCode = HD_ERR_NO_PITS_DETECTED;
|
||
return 0;
|
||
}
|
||
|
||
// ========== 步骤5：合并相邻峰值 ==========
|
||
std::vector<int> mergedPeakBins;
|
||
int mergeBinThreshold = static_cast<int>(params.peakMergeThreshold / params.binSize);
|
||
|
||
int i = 0;
|
||
while (i < static_cast<int>(peakBins.size())) {
|
||
int startBin = peakBins[i];
|
||
int endBin = startBin;
|
||
int maxBin = startBin;
|
||
int maxCount = histogram[startBin];
|
||
|
||
// 查找相邻的峰值
|
||
int j = i + 1;
|
||
while (j < static_cast<int>(peakBins.size()) &&
|
||
(peakBins[j] - endBin) <= mergeBinThreshold) {
|
||
endBin = peakBins[j];
|
||
if (histogram[peakBins[j]] > maxCount) {
|
||
maxCount = histogram[peakBins[j]];
|
||
maxBin = peakBins[j];
|
||
}
|
||
j++;
|
||
}
|
||
|
||
mergedPeakBins.push_back(maxBin);
|
||
i = j;
|
||
}
|
||
|
||
std::cout << "[Z-Histogram] After merging: " << mergedPeakBins.size()
|
||
<< " peaks" << std::endl;
|
||
|
||
// ========== 步骤6：为每个峰值收集点并计算统计信息 ==========
|
||
for (int peakBin : mergedPeakBins) {
|
||
ZHistogramPeak peak;
|
||
|
||
// 确定峰值的Z范围（包含相邻的bin）
|
||
int startBin = std::max(0, peakBin - mergeBinThreshold / 2);
|
||
int endBin = std::min(numBins - 1, peakBin + mergeBinThreshold / 2);
|
||
|
||
peak.zMin = minZ + startBin * params.binSize;
|
||
peak.zMax = minZ + (endBin + 1) * params.binSize;
|
||
|
||
// 收集该范围内的所有点
|
||
float sumZ = 0.0f;
|
||
for (int b = startBin; b <= endBin; b++) {
|
||
for (int idx : binPointIndices[b]) {
|
||
peak.pointIndices.push_back(idx);
|
||
sumZ += points[idx].z;
|
||
}
|
||
}
|
||
|
||
peak.pointCount = static_cast<int>(peak.pointIndices.size());
|
||
|
||
if (peak.pointCount < params.minPeakPoints) {
|
||
continue;
|
||
}
|
||
|
||
// 计算平均Z值
|
||
peak.zValue = sumZ / peak.pointCount;
|
||
|
||
outPeaks.push_back(peak);
|
||
|
||
std::cout << " Peak " << outPeaks.size()
|
||
<< ": Z=" << peak.zValue
|
||
<< " (" << peak.zMin << " - " << peak.zMax << ")"
|
||
<< ", points=" << peak.pointCount << std::endl;
|
||
}
|
||
|
||
// 按Z值排序
|
||
std::sort(outPeaks.begin(), outPeaks.end(),
|
||
[](const ZHistogramPeak& a, const ZHistogramPeak& b) {
|
||
return a.zValue < b.zValue;
|
||
});
|
||
|
||
*errCode = HD_SUCCESS;
|
||
return static_cast<int>(outPeaks.size());
|
||
}
|
||
|
||
// ========== 改进版：自适应倾斜平面分割 ==========
|
||
int SegmentTiltedPlanesByHistogram(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
const ZHistogramSegmentationParams& params,
|
||
std::vector<ZHistogramPeak>& outPeaks,
|
||
int* errCode
|
||
) {
|
||
if (points == nullptr || pointCount < 3 || errCode == nullptr) {
|
||
if (errCode) *errCode = HD_ERR_INVALID_INPUT;
|
||
return 0;
|
||
}
|
||
|
||
outPeaks.clear();
|
||
|
||
// ========== 步骤1：估计主方向（使用PCA或简单的法向量估计）==========
|
||
// 这里使用简化方法：假设主平面接近水平，先用Z直方图粗分割
|
||
|
||
// 计算点云的质心
|
||
double cx = 0.0, cy = 0.0, cz = 0.0;
|
||
int validCount = 0;
|
||
|
||
for (int i = 0; i < pointCount; i++) {
|
||
if (!IsValidPoint(points[i])) continue;
|
||
cx += points[i].x;
|
||
cy += points[i].y;
|
||
cz += points[i].z;
|
||
validCount++;
|
||
}
|
||
|
||
if (validCount < params.minPeakPoints) {
|
||
*errCode = HD_ERR_INVALID_INPUT;
|
||
return 0;
|
||
}
|
||
|
||
cx /= validCount;
|
||
cy /= validCount;
|
||
cz /= validCount;
|
||
|
||
// ========== 步骤2：计算协方差矩阵（简化版PCA）==========
|
||
double cxx = 0, cyy = 0, czz = 0;
|
||
double cxy = 0, cxz = 0, cyz = 0;
|
||
|
||
for (int i = 0; i < pointCount; i++) {
|
||
if (!IsValidPoint(points[i])) continue;
|
||
|
||
double dx = points[i].x - cx;
|
||
double dy = points[i].y - cy;
|
||
double dz = points[i].z - cz;
|
||
|
||
cxx += dx * dx;
|
||
cyy += dy * dy;
|
||
czz += dz * dz;
|
||
cxy += dx * dy;
|
||
cxz += dx * dz;
|
||
cyz += dy * dz;
|
||
}
|
||
|
||
// 简化：假设主平面法向量接近 (0, 0, 1)
|
||
// 如果 czz 远大于 cxx 和 cyy，说明Z方向变化大，适合用Z直方图
|
||
// 否则需要投影到主方向
|
||
|
||
double zVariance = czz / validCount;
|
||
double xyVariance = (cxx + cyy) / validCount;
|
||
|
||
std::cout << "[Tilted-Histogram] Z variance: " << zVariance
|
||
<< ", XY variance: " << xyVariance << std::endl;
|
||
|
||
if (zVariance > xyVariance * 0.1) {
|
||
// Z方向有足够变化，使用标准Z直方图
|
||
std::cout << "[Tilted-Histogram] Using standard Z-histogram" << std::endl;
|
||
return SegmentPlanesByZHistogram(points, pointCount, params, outPeaks, errCode);
|
||
}
|
||
|
||
// ========== 步骤3：对于接近水平的平面，使用更精细的分层 ==========
|
||
std::cout << "[Tilted-Histogram] Planes are nearly horizontal, using fine-grained segmentation" << std::endl;
|
||
|
||
// 使用更小的bin size
|
||
ZHistogramSegmentationParams fineParams = params;
|
||
fineParams.binSize = params.binSize * 0.5f; // 减小bin大小
|
||
fineParams.smoothingWindow = std::max(1, params.smoothingWindow - 1);
|
||
|
||
return SegmentPlanesByZHistogram(points, pointCount, fineParams, outPeaks, errCode);
|
||
}
|
||
|
||
// ========== Z值波动统计函数 ==========
|
||
|
||
int ComputeZFluctuation(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
const ZFluctuationParams& params,
|
||
ZFluctuationStats* outStats,
|
||
int* errCode
|
||
) {
|
||
if (points == nullptr || pointCount < 3 || outStats == nullptr || errCode == nullptr) {
|
||
if (errCode) *errCode = HD_ERR_INVALID_INPUT;
|
||
return HD_ERR_INVALID_INPUT;
|
||
}
|
||
|
||
// ========== 步骤1：收集所有有效点的Z值 ==========
|
||
std::vector<float> zValues;
|
||
zValues.reserve(pointCount);
|
||
|
||
for (int i = 0; i < pointCount; i++) {
|
||
if (!IsValidPoint(points[i])) continue;
|
||
zValues.push_back(points[i].z);
|
||
}
|
||
|
||
if (zValues.size() < 3) {
|
||
*errCode = HD_ERR_INVALID_INPUT;
|
||
return HD_ERR_INVALID_INPUT;
|
||
}
|
||
|
||
std::cout << "[Z-Fluctuation] Total valid points: " << zValues.size() << std::endl;
|
||
|
||
// 排序（用于计算中位数和百分位数）
|
||
std::vector<float> sortedZ = zValues;
|
||
std::sort(sortedZ.begin(), sortedZ.end());
|
||
|
||
// ========== 步骤2：计算初始统计量 ==========
|
||
double sumZ = 0.0;
|
||
for (float z : zValues) {
|
||
sumZ += z;
|
||
}
|
||
float meanZ = static_cast<float>(sumZ / zValues.size());
|
||
|
||
double sumSqDiff = 0.0;
|
||
for (float z : zValues) {
|
||
double diff = z - meanZ;
|
||
sumSqDiff += diff * diff;
|
||
}
|
||
float stdDevZ = static_cast<float>(std::sqrt(sumSqDiff / zValues.size()));
|
||
|
||
// 计算中位数
|
||
float medianZ;
|
||
size_t n = sortedZ.size();
|
||
if (n % 2 == 0) {
|
||
medianZ = (sortedZ[n/2 - 1] + sortedZ[n/2]) / 2.0f;
|
||
} else {
|
||
medianZ = sortedZ[n/2];
|
||
}
|
||
|
||
// 计算四分位数和IQR
|
||
float q1 = sortedZ[n / 4];
|
||
float q3 = sortedZ[3 * n / 4];
|
||
float iqr = q3 - q1;
|
||
|
||
std::cout << "[Z-Fluctuation] Initial stats: mean=" << meanZ
|
||
<< ", stdDev=" << stdDevZ
|
||
<< ", median=" << medianZ
|
||
<< ", IQR=" << iqr << std::endl;
|
||
|
||
// ========== 步骤3：根据选择的方法检测离群点 ==========
|
||
std::vector<bool> isInlier(zValues.size(), true);
|
||
int outlierCount = 0;
|
||
|
||
switch (params.method) {
|
||
case ZFluctuationParams::SIGMA_3: {
|
||
// 3-sigma规则：去除距离均值超过N倍标准差的点
|
||
float threshold = params.sigmaMultiplier * stdDevZ;
|
||
for (size_t i = 0; i < zValues.size(); i++) {
|
||
if (std::abs(zValues[i] - meanZ) > threshold) {
|
||
isInlier[i] = false;
|
||
outlierCount++;
|
||
}
|
||
}
|
||
std::cout << "[Z-Fluctuation] Using 3-sigma method (threshold="
|
||
<< threshold << ")" << std::endl;
|
||
break;
|
||
}
|
||
|
||
case ZFluctuationParams::IQR: {
|
||
// IQR方法：去除超出 [Q1-k*IQR, Q3+k*IQR] 范围的点
|
||
float lowerBound = q1 - params.iqrMultiplier * iqr;
|
||
float upperBound = q3 + params.iqrMultiplier * iqr;
|
||
for (size_t i = 0; i < zValues.size(); i++) {
|
||
if (zValues[i] < lowerBound || zValues[i] > upperBound) {
|
||
isInlier[i] = false;
|
||
outlierCount++;
|
||
}
|
||
}
|
||
std::cout << "[Z-Fluctuation] Using IQR method (bounds=["
|
||
<< lowerBound << ", " << upperBound << "])" << std::endl;
|
||
break;
|
||
}
|
||
|
||
case ZFluctuationParams::PERCENTILE: {
|
||
// 百分位数方法：只保留指定百分位数范围内的点
|
||
size_t lowIdx = static_cast<size_t>(params.percentileLow / 100.0f * n);
|
||
size_t highIdx = static_cast<size_t>(params.percentileHigh / 100.0f * n);
|
||
float lowerBound = sortedZ[lowIdx];
|
||
float upperBound = sortedZ[highIdx];
|
||
for (size_t i = 0; i < zValues.size(); i++) {
|
||
if (zValues[i] < lowerBound || zValues[i] > upperBound) {
|
||
isInlier[i] = false;
|
||
outlierCount++;
|
||
}
|
||
}
|
||
std::cout << "[Z-Fluctuation] Using Percentile method (bounds=["
|
||
<< lowerBound << ", " << upperBound << "])" << std::endl;
|
||
break;
|
||
}
|
||
}
|
||
|
||
std::cout << "[Z-Fluctuation] Detected " << outlierCount << " outliers ("
|
||
<< (outlierCount * 100.0f / zValues.size()) << "%)" << std::endl;
|
||
|
||
// ========== 步骤4：用内点重新计算统计信息 ==========
|
||
std::vector<float> inlierZ;
|
||
inlierZ.reserve(zValues.size() - outlierCount);
|
||
|
||
for (size_t i = 0; i < zValues.size(); i++) {
|
||
if (isInlier[i]) {
|
||
inlierZ.push_back(zValues[i]);
|
||
}
|
||
}
|
||
|
||
if (inlierZ.size() < 3) {
|
||
std::cout << "[Z-Fluctuation] Warning: Too few inliers, using all points" << std::endl;
|
||
inlierZ = zValues;
|
||
outlierCount = 0;
|
||
}
|
||
|
||
// 重新计算统计量
|
||
sumZ = 0.0;
|
||
float minZ = std::numeric_limits<float>::max();
|
||
float maxZ = std::numeric_limits<float>::lowest();
|
||
|
||
for (float z : inlierZ) {
|
||
sumZ += z;
|
||
if (z < minZ) minZ = z;
|
||
if (z > maxZ) maxZ = z;
|
||
}
|
||
|
||
meanZ = static_cast<float>(sumZ / inlierZ.size());
|
||
|
||
sumSqDiff = 0.0;
|
||
for (float z : inlierZ) {
|
||
double diff = z - meanZ;
|
||
sumSqDiff += diff * diff;
|
||
}
|
||
stdDevZ = static_cast<float>(std::sqrt(sumSqDiff / inlierZ.size()));
|
||
|
||
// 重新计算中位数和IQR
|
||
std::sort(inlierZ.begin(), inlierZ.end());
|
||
n = inlierZ.size();
|
||
if (n % 2 == 0) {
|
||
medianZ = (inlierZ[n/2 - 1] + inlierZ[n/2]) / 2.0f;
|
||
} else {
|
||
medianZ = inlierZ[n/2];
|
||
}
|
||
|
||
q1 = inlierZ[n / 4];
|
||
q3 = inlierZ[3 * n / 4];
|
||
iqr = q3 - q1;
|
||
|
||
// ========== 步骤5：填充输出结构 ==========
|
||
outStats->meanZ = meanZ;
|
||
outStats->stdDevZ = stdDevZ;
|
||
outStats->minZ = minZ;
|
||
outStats->maxZ = maxZ;
|
||
outStats->rangeZ = maxZ - minZ;
|
||
outStats->validPointCount = static_cast<int>(inlierZ.size());
|
||
outStats->outlierCount = outlierCount;
|
||
outStats->outlierRatio = static_cast<float>(outlierCount) / zValues.size();
|
||
outStats->medianZ = medianZ;
|
||
outStats->iqrZ = iqr;
|
||
|
||
// ========== 输出结果 ==========
|
||
std::cout << "\n[Z-Fluctuation] Final Statistics (after outlier removal):\n";
|
||
std::cout << " Mean Z: " << outStats->meanZ << " mm\n";
|
||
std::cout << " Std Dev: " << outStats->stdDevZ << " mm\n";
|
||
std::cout << " Median Z: " << outStats->medianZ << " mm\n";
|
||
std::cout << " Z Range: [" << outStats->minZ << ", " << outStats->maxZ << "] mm\n";
|
||
std::cout << " Z Span: " << outStats->rangeZ << " mm\n";
|
||
std::cout << " IQR: " << outStats->iqrZ << " mm\n";
|
||
std::cout << " Valid Points: " << outStats->validPointCount << "\n";
|
||
std::cout << " Outliers: " << outStats->outlierCount
|
||
<< " (" << (outStats->outlierRatio * 100) << "%)\n";
|
||
|
||
*errCode = HD_SUCCESS;
|
||
return HD_SUCCESS;
|
||
}
|
||
|
||
// 简化版本：使用默认参数
|
||
int ComputeZFluctuation(
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount,
|
||
ZFluctuationStats* outStats,
|
||
int* errCode
|
||
) {
|
||
ZFluctuationParams params; // 使用默认参数（IQR方法）
|
||
return ComputeZFluctuation(points, pointCount, params, outStats, errCode);
|
||
}
|
||
|
||
// ========== 点云相邻点间距计算函数 ==========
|
||
|
||
int ComputePointSpacing(
|
||
const SVzNLPointXYZ* points,
|
||
int rows,
|
||
int cols,
|
||
PointSpacingStats* outStats,
|
||
int* errCode
|
||
) {
|
||
if (points == nullptr || rows <= 0 || cols <= 0 ||
|
||
outStats == nullptr || errCode == nullptr) {
|
||
if (errCode) *errCode = HD_ERR_INVALID_INPUT;
|
||
return HD_ERR_INVALID_INPUT;
|
||
}
|
||
|
||
std::cout << "[Point-Spacing] Analyzing " << rows << "x" << cols
|
||
<< " grid..." << std::endl;
|
||
|
||
// ========== 步骤1：收集行内相邻点距离（同一激光线，YZ平面）==========
|
||
std::vector<float> rowDistances;
|
||
rowDistances.reserve(rows * cols);
|
||
|
||
for (int r = 0; r < rows; r++) {
|
||
int prevValidCol = -1;
|
||
SVzNLPointXYZ prevPoint;
|
||
|
||
for (int c = 0; c < cols; c++) {
|
||
int idx = r * cols + c;
|
||
const SVzNLPointXYZ& pt = points[idx];
|
||
|
||
// 跳过无效点
|
||
if (!IsValidPoint(pt)) {
|
||
continue;
|
||
}
|
||
|
||
// 如果有前一个有效点，计算距离
|
||
if (prevValidCol >= 0) {
|
||
// 行内距离：使用YZ平面
|
||
float dy = pt.y - prevPoint.y;
|
||
float dz = pt.z - prevPoint.z;
|
||
float dist = std::sqrt(dy * dy + dz * dz);
|
||
|
||
// 过滤明显异常的距离（可能是噪点）
|
||
// 假设合理的间距在 0.01mm 到 100mm 之间
|
||
if (dist > 0.01f && dist < 100.0f) {
|
||
rowDistances.push_back(dist);
|
||
}
|
||
}
|
||
|
||
prevValidCol = c;
|
||
prevPoint = pt;
|
||
}
|
||
}
|
||
|
||
std::cout << "[Point-Spacing] Collected " << rowDistances.size()
|
||
<< " row-wise distances" << std::endl;
|
||
|
||
// ========== 步骤2：收集列内相邻点距离（相邻激光线，XZ平面）==========
|
||
std::vector<float> colDistances;
|
||
colDistances.reserve(rows * cols);
|
||
|
||
for (int c = 0; c < cols; c++) {
|
||
int prevValidRow = -1;
|
||
SVzNLPointXYZ prevPoint;
|
||
|
||
for (int r = 0; r < rows; r++) {
|
||
int idx = r * cols + c;
|
||
const SVzNLPointXYZ& pt = points[idx];
|
||
|
||
// 跳过无效点
|
||
if (!IsValidPoint(pt)) {
|
||
continue;
|
||
}
|
||
|
||
// 如果有前一个有效点，计算距离
|
||
if (prevValidRow >= 0) {
|
||
// 列内距离：使用XZ平面
|
||
float dx = pt.x - prevPoint.x;
|
||
float dz = pt.z - prevPoint.z;
|
||
float dist = std::sqrt(dx * dx + dz * dz);
|
||
|
||
// 过滤明显异常的距离
|
||
if (dist > 0.01f && dist < 100.0f) {
|
||
colDistances.push_back(dist);
|
||
}
|
||
}
|
||
|
||
prevValidRow = r;
|
||
prevPoint = pt;
|
||
}
|
||
}
|
||
|
||
std::cout << "[Point-Spacing] Collected " << colDistances.size()
|
||
<< " column-wise distances" << std::endl;
|
||
|
||
// ========== 步骤3：检查是否有足够的样本 ==========
|
||
if (rowDistances.empty() && colDistances.empty()) {
|
||
std::cerr << "[Point-Spacing] Error: No valid distances found" << std::endl;
|
||
*errCode = HD_ERR_INVALID_INPUT;
|
||
return HD_ERR_INVALID_INPUT;
|
||
}
|
||
|
||
// ========== 步骤4：使用IQR方法去除离群点（行内距离）==========
|
||
float rowSpacing = 0.0f;
|
||
float rowSpacingMedian = 0.0f;
|
||
float rowSpacingStdDev = 0.0f;
|
||
float rowSpacingMin = 0.0f;
|
||
float rowSpacingMax = 0.0f;
|
||
int rowSampleCount = 0;
|
||
|
||
if (!rowDistances.empty()) {
|
||
// 排序
|
||
std::vector<float> sortedRowDist = rowDistances;
|
||
std::sort(sortedRowDist.begin(), sortedRowDist.end());
|
||
|
||
// 计算中位数和IQR
|
||
size_t n = sortedRowDist.size();
|
||
float median = (n % 2 == 0)
|
||
? (sortedRowDist[n/2 - 1] + sortedRowDist[n/2]) / 2.0f
|
||
: sortedRowDist[n/2];
|
||
|
||
float q1 = sortedRowDist[n / 4];
|
||
float q3 = sortedRowDist[3 * n / 4];
|
||
float iqr = q3 - q1;
|
||
|
||
// 使用IQR去除离群点
|
||
float lowerBound = q1 - 1.5f * iqr;
|
||
float upperBound = q3 + 1.5f * iqr;
|
||
|
||
std::vector<float> inlierRowDist;
|
||
for (float d : rowDistances) {
|
||
if (d >= lowerBound && d <= upperBound) {
|
||
inlierRowDist.push_back(d);
|
||
}
|
||
}
|
||
|
||
std::cout << "[Point-Spacing] Row distances: removed "
|
||
<< (rowDistances.size() - inlierRowDist.size())
|
||
<< " outliers (IQR bounds: [" << lowerBound << ", "
|
||
<< upperBound << "])" << std::endl;
|
||
|
||
// 计算统计量
|
||
if (!inlierRowDist.empty()) {
|
||
double sum = 0.0;
|
||
rowSpacingMin = inlierRowDist[0];
|
||
rowSpacingMax = inlierRowDist[0];
|
||
|
||
for (float d : inlierRowDist) {
|
||
sum += d;
|
||
if (d < rowSpacingMin) rowSpacingMin = d;
|
||
if (d > rowSpacingMax) rowSpacingMax = d;
|
||
}
|
||
|
||
rowSpacing = static_cast<float>(sum / inlierRowDist.size());
|
||
rowSpacingMedian = median;
|
||
rowSampleCount = static_cast<int>(inlierRowDist.size());
|
||
|
||
// 计算标准差
|
||
double sumSqDiff = 0.0;
|
||
for (float d : inlierRowDist) {
|
||
double diff = d - rowSpacing;
|
||
sumSqDiff += diff * diff;
|
||
}
|
||
rowSpacingStdDev = static_cast<float>(
|
||
std::sqrt(sumSqDiff / inlierRowDist.size())
|
||
);
|
||
}
|
||
}
|
||
|
||
// ========== 步骤5：使用IQR方法去除离群点（列内距离）==========
|
||
float colSpacing = 0.0f;
|
||
float colSpacingMedian = 0.0f;
|
||
float colSpacingStdDev = 0.0f;
|
||
float colSpacingMin = 0.0f;
|
||
float colSpacingMax = 0.0f;
|
||
int colSampleCount = 0;
|
||
|
||
if (!colDistances.empty()) {
|
||
// 排序
|
||
std::vector<float> sortedColDist = colDistances;
|
||
std::sort(sortedColDist.begin(), sortedColDist.end());
|
||
|
||
// 计算中位数和IQR
|
||
size_t n = sortedColDist.size();
|
||
float median = (n % 2 == 0)
|
||
? (sortedColDist[n/2 - 1] + sortedColDist[n/2]) / 2.0f
|
||
: sortedColDist[n/2];
|
||
|
||
float q1 = sortedColDist[n / 4];
|
||
float q3 = sortedColDist[3 * n / 4];
|
||
float iqr = q3 - q1;
|
||
|
||
// 使用IQR去除离群点
|
||
float lowerBound = q1 - 1.5f * iqr;
|
||
float upperBound = q3 + 1.5f * iqr;
|
||
|
||
std::vector<float> inlierColDist;
|
||
for (float d : colDistances) {
|
||
if (d >= lowerBound && d <= upperBound) {
|
||
inlierColDist.push_back(d);
|
||
}
|
||
}
|
||
|
||
std::cout << "[Point-Spacing] Column distances: removed "
|
||
<< (colDistances.size() - inlierColDist.size())
|
||
<< " outliers (IQR bounds: [" << lowerBound << ", "
|
||
<< upperBound << "])" << std::endl;
|
||
|
||
// 计算统计量
|
||
if (!inlierColDist.empty()) {
|
||
double sum = 0.0;
|
||
colSpacingMin = inlierColDist[0];
|
||
colSpacingMax = inlierColDist[0];
|
||
|
||
for (float d : inlierColDist) {
|
||
sum += d;
|
||
if (d < colSpacingMin) colSpacingMin = d;
|
||
if (d > colSpacingMax) colSpacingMax = d;
|
||
}
|
||
|
||
colSpacing = static_cast<float>(sum / inlierColDist.size());
|
||
colSpacingMedian = median;
|
||
colSampleCount = static_cast<int>(inlierColDist.size());
|
||
|
||
// 计算标准差
|
||
double sumSqDiff = 0.0;
|
||
for (float d : inlierColDist) {
|
||
double diff = d - colSpacing;
|
||
sumSqDiff += diff * diff;
|
||
}
|
||
colSpacingStdDev = static_cast<float>(
|
||
std::sqrt(sumSqDiff / inlierColDist.size())
|
||
);
|
||
}
|
||
}
|
||
|
||
// ========== 步骤6：填充输出结构 ==========
|
||
outStats->rowSpacing = rowSpacing;
|
||
outStats->colSpacing = colSpacing;
|
||
outStats->rowSpacingMedian = rowSpacingMedian;
|
||
outStats->colSpacingMedian = colSpacingMedian;
|
||
outStats->rowSpacingStdDev = rowSpacingStdDev;
|
||
outStats->colSpacingStdDev = colSpacingStdDev;
|
||
outStats->rowSampleCount = rowSampleCount;
|
||
outStats->colSampleCount = colSampleCount;
|
||
outStats->rowSpacingMin = rowSpacingMin;
|
||
outStats->rowSpacingMax = rowSpacingMax;
|
||
outStats->colSpacingMin = colSpacingMin;
|
||
outStats->colSpacingMax = colSpacingMax;
|
||
|
||
// 计算典型间距：取两个方向中位数的最大值
|
||
outStats->typicalSpacing = std::max(rowSpacingMedian, colSpacingMedian);
|
||
|
||
// ========== 输出结果 ==========
|
||
std::cout << "\n[Point-Spacing] Results:\n";
|
||
std::cout << " ★ Typical spacing (max of row/col): "
|
||
<< outStats->typicalSpacing << " mm\n";
|
||
std::cout << "\n";
|
||
|
||
std::cout << " Row-wise (same laser line, YZ plane):\n";
|
||
std::cout << " Median spacing: " << rowSpacingMedian << " mm\n";
|
||
std::cout << " Mean spacing: " << rowSpacing << " mm\n";
|
||
std::cout << " Std deviation: " << rowSpacingStdDev << " mm\n";
|
||
std::cout << " Range: [" << rowSpacingMin << ", "
|
||
<< rowSpacingMax << "] mm\n";
|
||
std::cout << " Sample count: " << rowSampleCount << "\n";
|
||
|
||
std::cout << "\n";
|
||
std::cout << " Column-wise (adjacent laser lines, XZ plane):\n";
|
||
std::cout << " Median spacing: " << colSpacingMedian << " mm\n";
|
||
std::cout << " Mean spacing: " << colSpacing << " mm\n";
|
||
std::cout << " Std deviation: " << colSpacingStdDev << " mm\n";
|
||
std::cout << " Range: [" << colSpacingMin << ", "
|
||
<< colSpacingMax << "] mm\n";
|
||
std::cout << " Sample count: " << colSampleCount << "\n";
|
||
|
||
// 评估采样均匀性
|
||
if (rowSampleCount > 0) {
|
||
float rowCV = rowSpacingStdDev / rowSpacing; // 变异系数
|
||
std::cout << " Row uniformity: ";
|
||
if (rowCV < 0.1f) {
|
||
std::cout << "Excellent (CV=" << rowCV << ")\n";
|
||
} else if (rowCV < 0.2f) {
|
||
std::cout << "Good (CV=" << rowCV << ")\n";
|
||
} else if (rowCV < 0.3f) {
|
||
std::cout << "Fair (CV=" << rowCV << ")\n";
|
||
} else {
|
||
std::cout << "Poor (CV=" << rowCV << ") - uneven sampling\n";
|
||
}
|
||
}
|
||
|
||
if (colSampleCount > 0) {
|
||
float colCV = colSpacingStdDev / colSpacing;
|
||
std::cout << " Column uniformity: ";
|
||
if (colCV < 0.1f) {
|
||
std::cout << "Excellent (CV=" << colCV << ")\n";
|
||
} else if (colCV < 0.2f) {
|
||
std::cout << "Good (CV=" << colCV << ")\n";
|
||
} else if (colCV < 0.3f) {
|
||
std::cout << "Fair (CV=" << colCV << ")\n";
|
||
} else {
|
||
std::cout << "Poor (CV=" << colCV << ") - uneven sampling\n";
|
||
}
|
||
}
|
||
|
||
*errCode = HD_SUCCESS;
|
||
return HD_SUCCESS;
|
||
}
|
||
|
||
// ========== RANSAC 平面分割实现 ==========
|
||
|
||
// 用3个点拟合平面方程 ax + by + cz + d = 0
|
||
static bool FitPlaneFrom3Points(
|
||
const SVzNLPointXYZ& p1,
|
||
const SVzNLPointXYZ& p2,
|
||
const SVzNLPointXYZ& p3,
|
||
float& a, float& b, float& c, float& d
|
||
) {
|
||
// v1 = p2 - p1, v2 = p3 - p1
|
||
float v1x = p2.x - p1.x, v1y = p2.y - p1.y, v1z = p2.z - p1.z;
|
||
float v2x = p3.x - p1.x, v2y = p3.y - p1.y, v2z = p3.z - p1.z;
|
||
|
||
// normal = v1 x v2
|
||
a = v1y * v2z - v1z * v2y;
|
||
b = v1z * v2x - v1x * v2z;
|
||
c = v1x * v2y - v1y * v2x;
|
||
|
||
float len = std::sqrt(a * a + b * b + c * c);
|
||
if (len < 1e-10f) return false;
|
||
|
||
a /= len;
|
||
b /= len;
|
||
c /= len;
|
||
d = -(a * p1.x + b * p1.y + c * p1.z);
|
||
return true;
|
||
}
|
||
|
||
int SegmentPlanesByRansac(
|
||
const SVzNLPointXYZ* points,
|
||
int rows,
|
||
int cols,
|
||
const RansacPlaneSegmentationParams& params,
|
||
std::vector<PlaneSegment>& outPlanes,
|
||
int* errCode
|
||
) {
|
||
int pointCount = rows * cols;
|
||
if (points == nullptr || pointCount < 3 || errCode == nullptr) {
|
||
if (errCode) *errCode = HD_ERR_INVALID_INPUT;
|
||
return 0;
|
||
}
|
||
|
||
outPlanes.clear();
|
||
|
||
// 收集所有有效点的索引
|
||
std::vector<int> validIndices;
|
||
validIndices.reserve(pointCount);
|
||
for (int i = 0; i < pointCount; i++) {
|
||
if (IsValidPoint(points[i])) {
|
||
validIndices.push_back(i);
|
||
}
|
||
}
|
||
|
||
if (static_cast<int>(validIndices.size()) < params.minPlanePoints) {
|
||
*errCode = HD_ERR_INSUFFICIENT_POINTS;
|
||
return 0;
|
||
}
|
||
|
||
std::cout << "[RANSAC] Starting plane segmentation with "
|
||
<< validIndices.size() << " valid points"
|
||
<< " (grid " << rows << "x" << cols << ")" << std::endl;
|
||
|
||
// 标记已被分配到平面的点
|
||
std::vector<bool> used(pointCount, false);
|
||
|
||
// 简单的伪随机数生成器（确定性，便于调试）
|
||
unsigned int seed = 42u;
|
||
auto nextRandom = [&seed](int maxVal) -> int {
|
||
seed = seed * 1103515245u + 12345u;
|
||
return static_cast<int>((seed >> 16) % static_cast<unsigned int>(maxVal));
|
||
};
|
||
|
||
// 4-连通邻域偏移: 上、下、左、右
|
||
const int dr[4] = { -1, 1, 0, 0 };
|
||
const int dc[4] = { 0, 0, -1, 1 };
|
||
|
||
for (int planeIdx = 0; planeIdx < params.maxPlanes; planeIdx++) {
|
||
// 收集剩余未分配的有效点索引
|
||
std::vector<int> remaining;
|
||
remaining.reserve(validIndices.size());
|
||
for (int idx : validIndices) {
|
||
if (!used[idx]) {
|
||
remaining.push_back(idx);
|
||
}
|
||
}
|
||
|
||
int remainCount = static_cast<int>(remaining.size());
|
||
if (remainCount < params.minPlanePoints) {
|
||
break;
|
||
}
|
||
|
||
// ===== RANSAC迭代寻找最佳平面 =====
|
||
float bestA = 0, bestB = 0, bestC = 0, bestD = 0;
|
||
int bestInlierCount = 0;
|
||
|
||
for (int iter = 0; iter < params.maxIterations; iter++) {
|
||
// 随机选取3个不同的点
|
||
int i1 = nextRandom(remainCount);
|
||
int i2 = nextRandom(remainCount);
|
||
int i3 = nextRandom(remainCount);
|
||
|
||
if (i1 == i2 || i1 == i3 || i2 == i3) continue;
|
||
|
||
const SVzNLPointXYZ& p1 = points[remaining[i1]];
|
||
const SVzNLPointXYZ& p2 = points[remaining[i2]];
|
||
const SVzNLPointXYZ& p3 = points[remaining[i3]];
|
||
|
||
float a, b, c, d;
|
||
if (!FitPlaneFrom3Points(p1, p2, p3, a, b, c, d)) continue;
|
||
|
||
// 统计内点数
|
||
int inlierCount = 0;
|
||
for (int idx : remaining) {
|
||
float dist = std::abs(
|
||
a * points[idx].x + b * points[idx].y +
|
||
c * points[idx].z + d
|
||
);
|
||
if (dist <= params.distanceThreshold) {
|
||
inlierCount++;
|
||
}
|
||
}
|
||
|
||
if (inlierCount > bestInlierCount) {
|
||
bestInlierCount = inlierCount;
|
||
bestA = a;
|
||
bestB = b;
|
||
bestC = c;
|
||
bestD = d;
|
||
}
|
||
}
|
||
|
||
// 检查最佳平面是否有足够的内点
|
||
if (bestInlierCount < params.minPlanePoints) {
|
||
break;
|
||
}
|
||
|
||
// ===== 提取RANSAC初始内点作为种子 =====
|
||
std::vector<int> seedIndices;
|
||
seedIndices.reserve(bestInlierCount);
|
||
for (int idx : remaining) {
|
||
float dist = std::abs(
|
||
bestA * points[idx].x + bestB * points[idx].y +
|
||
bestC * points[idx].z + bestD
|
||
);
|
||
if (dist <= params.distanceThreshold) {
|
||
seedIndices.push_back(idx);
|
||
}
|
||
}
|
||
|
||
// ===== BFS区域生长：从种子点出发，沿网格扩展 =====
|
||
// 使用局部Z差阈值而非全局点-平面距离，从而能追踪曲面
|
||
std::vector<bool> inPlane(pointCount, false);
|
||
std::queue<int> bfsQueue;
|
||
|
||
// 将所有种子点加入队列
|
||
for (int idx : seedIndices) {
|
||
if (!used[idx]) {
|
||
inPlane[idx] = true;
|
||
bfsQueue.push(idx);
|
||
}
|
||
}
|
||
|
||
while (!bfsQueue.empty()) {
|
||
int curIdx = bfsQueue.front();
|
||
bfsQueue.pop();
|
||
|
||
int curRow = curIdx / cols;
|
||
int curCol = curIdx % cols;
|
||
|
||
// 遍历4-连通邻居
|
||
for (int d = 0; d < 4; d++) {
|
||
int nr = curRow + dr[d];
|
||
int nc = curCol + dc[d];
|
||
|
||
if (nr < 0 || nr >= rows || nc < 0 || nc >= cols) continue;
|
||
|
||
int nIdx = nr * cols + nc;
|
||
if (used[nIdx] || inPlane[nIdx]) continue;
|
||
if (!IsValidPoint(points[nIdx])) continue;
|
||
|
||
// 局部Z差检查：防止跨越单步噪点
|
||
float zDiff = std::abs(points[nIdx].z - points[curIdx].z);
|
||
if (zDiff > params.growthZThreshold) continue;
|
||
|
||
// 全局点到平面距离检查：防止BFS在渐变区域内漂移到不同表面。
|
||
// 仅靠局部Z差无法阻止逐步穿越台阶（每步小但累积大），
|
||
// 全局距离能确保生长点始终贴近当前RANSAC平面。
|
||
float planeDist = std::abs(
|
||
bestA * points[nIdx].x + bestB * points[nIdx].y +
|
||
bestC * points[nIdx].z + bestD
|
||
);
|
||
if (planeDist > params.distanceThreshold * 2.0f) continue;
|
||
|
||
inPlane[nIdx] = true;
|
||
bfsQueue.push(nIdx);
|
||
}
|
||
}
|
||
|
||
// ===== 收集生长后的平面点 =====
|
||
PlaneSegment plane;
|
||
plane.a = bestA;
|
||
plane.b = bestB;
|
||
plane.c = bestC;
|
||
plane.d = bestD;
|
||
plane.normalAngleDeg = 0.0f;
|
||
|
||
for (int i = 0; i < pointCount; i++) {
|
||
if (inPlane[i]) {
|
||
plane.pointIndices.push_back(i);
|
||
used[i] = true;
|
||
}
|
||
}
|
||
|
||
plane.pointCount = static_cast<int>(plane.pointIndices.size());
|
||
|
||
if (plane.pointCount < params.minPlanePoints) {
|
||
// 生长后点数不足，跳过（但保持used标记不变以避免再次处理）
|
||
std::cout << " Plane " << (planeIdx + 1)
|
||
<< ": skipped (only " << plane.pointCount
|
||
<< " points after region growing)" << std::endl;
|
||
continue;
|
||
}
|
||
|
||
outPlanes.push_back(plane);
|
||
|
||
std::cout << " Plane " << (planeIdx + 1)
|
||
<< ": normal=(" << bestA << ", " << bestB << ", " << bestC << ")"
|
||
<< ", d=" << bestD
|
||
<< ", RANSAC seeds=" << static_cast<int>(seedIndices.size())
|
||
<< ", after growth=" << plane.pointCount << std::endl;
|
||
}
|
||
|
||
std::cout << "[RANSAC] Found " << outPlanes.size() << " planes (before merge)" << std::endl;
|
||
|
||
// ===== 后处理：将小平面合并到兼容的大平面中 =====
|
||
// 解决孔洞穿透问题：孔洞对面的点因BFS不可达而被分为单独小平面，
|
||
// 但实际上它们属于同一物理表面。
|
||
if (outPlanes.size() > 1) {
|
||
// 按点数降序排序，最大平面在前
|
||
std::sort(outPlanes.begin(), outPlanes.end(),
|
||
[](const PlaneSegment& a, const PlaneSegment& b) {
|
||
return a.pointCount > b.pointCount;
|
||
});
|
||
|
||
// 为每个平面计算Z值统计（中位数和范围）
|
||
struct PlaneZStats {
|
||
float medianZ;
|
||
float minZ;
|
||
float maxZ;
|
||
};
|
||
std::vector<PlaneZStats> planeStats(outPlanes.size());
|
||
|
||
for (size_t i = 0; i < outPlanes.size(); i++) {
|
||
std::vector<float> zValues;
|
||
zValues.reserve(outPlanes[i].pointIndices.size());
|
||
for (int idx : outPlanes[i].pointIndices) {
|
||
zValues.push_back(points[idx].z);
|
||
}
|
||
std::sort(zValues.begin(), zValues.end());
|
||
|
||
size_t n = zValues.size();
|
||
planeStats[i].medianZ = (n % 2 == 0)
|
||
? (zValues[n/2 - 1] + zValues[n/2]) / 2.0f
|
||
: zValues[n/2];
|
||
planeStats[i].minZ = zValues.front();
|
||
planeStats[i].maxZ = zValues.back();
|
||
}
|
||
|
||
// 尝试将小平面合并到大平面
|
||
constexpr float kNormalDotThreshold = 0.95f; // cos(~18°), 法向量相似性
|
||
constexpr float kMergeZRangeMultiplier = 2.0f; // Z差容忍倍数
|
||
|
||
std::vector<bool> merged(outPlanes.size(), false);
|
||
|
||
for (size_t i = 1; i < outPlanes.size(); i++) {
|
||
if (merged[i]) continue;
|
||
|
||
// 寻找最佳合并目标
|
||
int bestTarget = -1;
|
||
float bestDot = 0.0f;
|
||
|
||
for (size_t j = 0; j < i; j++) {
|
||
if (merged[j]) continue;
|
||
|
||
// 检查法向量相似性（绝对值，因为法向可能反向）
|
||
float dot = std::abs(
|
||
outPlanes[i].a * outPlanes[j].a +
|
||
outPlanes[i].b * outPlanes[j].b +
|
||
outPlanes[i].c * outPlanes[j].c
|
||
);
|
||
|
||
if (dot < kNormalDotThreshold) continue;
|
||
|
||
// 检查Z范围兼容性：小平面的中位Z应在大平面Z范围附近
|
||
float largeRange = planeStats[j].maxZ - planeStats[j].minZ;
|
||
float tolerance = std::max(largeRange * 0.5f,
|
||
params.growthZThreshold * kMergeZRangeMultiplier);
|
||
float zDist = std::abs(planeStats[i].medianZ - planeStats[j].medianZ);
|
||
|
||
if (zDist > tolerance) continue;
|
||
|
||
if (dot > bestDot) {
|
||
bestDot = dot;
|
||
bestTarget = static_cast<int>(j);
|
||
}
|
||
}
|
||
|
||
if (bestTarget >= 0) {
|
||
std::cout << " Merging small plane (" << outPlanes[i].pointCount
|
||
<< " pts, medZ=" << planeStats[i].medianZ
|
||
<< ") into plane (" << outPlanes[bestTarget].pointCount
|
||
<< " pts, medZ=" << planeStats[bestTarget].medianZ
|
||
<< "), normalDot=" << bestDot << std::endl;
|
||
|
||
// 合并点索引
|
||
outPlanes[bestTarget].pointIndices.insert(
|
||
outPlanes[bestTarget].pointIndices.end(),
|
||
outPlanes[i].pointIndices.begin(),
|
||
outPlanes[i].pointIndices.end()
|
||
);
|
||
outPlanes[bestTarget].pointCount += outPlanes[i].pointCount;
|
||
|
||
// 更新合并后的Z统计
|
||
float newMinZ = std::min(planeStats[bestTarget].minZ, planeStats[i].minZ);
|
||
float newMaxZ = std::max(planeStats[bestTarget].maxZ, planeStats[i].maxZ);
|
||
planeStats[bestTarget].minZ = newMinZ;
|
||
planeStats[bestTarget].maxZ = newMaxZ;
|
||
|
||
merged[i] = true;
|
||
}
|
||
}
|
||
|
||
// 移除已合并的平面
|
||
std::vector<PlaneSegment> finalPlanes;
|
||
for (size_t i = 0; i < outPlanes.size(); i++) {
|
||
if (!merged[i]) {
|
||
finalPlanes.push_back(std::move(outPlanes[i]));
|
||
}
|
||
}
|
||
outPlanes = std::move(finalPlanes);
|
||
|
||
std::cout << "[RANSAC] After merge: " << outPlanes.size() << " planes" << std::endl;
|
||
}
|
||
|
||
// ===== 后处理：对每个平面做连通域标注，将每个连通域拆分为独立平面 =====
|
||
{
|
||
std::vector<PlaneSegment> splitPlanes;
|
||
|
||
for (size_t pi = 0; pi < outPlanes.size(); pi++) {
|
||
const PlaneSegment& plane = outPlanes[pi];
|
||
|
||
// 建立该平面的栅格掩码
|
||
std::vector<bool> mask(pointCount, false);
|
||
for (int idx : plane.pointIndices) {
|
||
mask[idx] = true;
|
||
}
|
||
|
||
// BFS 连通域标注 (4-连通)
|
||
std::vector<int> label(pointCount, -1);
|
||
std::vector<std::vector<int>> components;
|
||
|
||
for (int idx : plane.pointIndices) {
|
||
if (label[idx] >= 0) continue;
|
||
|
||
int compId = static_cast<int>(components.size());
|
||
components.emplace_back();
|
||
std::queue<int> q;
|
||
q.push(idx);
|
||
label[idx] = compId;
|
||
|
||
while (!q.empty()) {
|
||
int cur = q.front();
|
||
q.pop();
|
||
components[compId].push_back(cur);
|
||
|
||
int cr = cur / cols;
|
||
int cc = cur % cols;
|
||
|
||
for (int d = 0; d < 4; d++) {
|
||
int nr = cr + dr[d];
|
||
int nc = cc + dc[d];
|
||
if (nr < 0 || nr >= rows || nc < 0 || nc >= cols) continue;
|
||
int nIdx = nr * cols + nc;
|
||
if (!mask[nIdx] || label[nIdx] >= 0) continue;
|
||
label[nIdx] = compId;
|
||
q.push(nIdx);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (components.size() == 1) {
|
||
splitPlanes.push_back(plane);
|
||
continue;
|
||
}
|
||
|
||
std::cout << " Plane " << (pi + 1) << ": split into "
|
||
<< components.size() << " connected components" << std::endl;
|
||
|
||
for (size_t ci = 0; ci < components.size(); ci++) {
|
||
PlaneSegment seg;
|
||
seg.a = plane.a;
|
||
seg.b = plane.b;
|
||
seg.c = plane.c;
|
||
seg.d = plane.d;
|
||
seg.normalAngleDeg = plane.normalAngleDeg;
|
||
seg.pointIndices = std::move(components[ci]);
|
||
seg.pointCount = static_cast<int>(seg.pointIndices.size());
|
||
|
||
std::cout << " Component " << (ci + 1)
|
||
<< ": " << seg.pointCount << " points" << std::endl;
|
||
|
||
if (seg.pointCount >= params.minPlanePoints) {
|
||
splitPlanes.push_back(std::move(seg));
|
||
} else {
|
||
std::cout << " Component " << (ci + 1)
|
||
<< ": skipped (below minPlanePoints="
|
||
<< params.minPlanePoints << ")" << std::endl;
|
||
}
|
||
}
|
||
}
|
||
|
||
outPlanes = std::move(splitPlanes);
|
||
}
|
||
|
||
// ===== 后处理：过滤行列跨度不足的平面 =====
|
||
// 孔洞检测要求平面至少跨 3 行且跨 3 列，否则不具备有效检测条件。
|
||
{
|
||
auto it = std::remove_if(outPlanes.begin(), outPlanes.end(),
|
||
[cols, rows](const PlaneSegment& plane) {
|
||
int rMin = rows, rMax = -1, cMin = cols, cMax = -1;
|
||
for (int idx : plane.pointIndices) {
|
||
int r = idx / cols;
|
||
int c = idx % cols;
|
||
if (r < rMin) rMin = r;
|
||
if (r > rMax) rMax = r;
|
||
if (c < cMin) cMin = c;
|
||
if (c > cMax) cMax = c;
|
||
}
|
||
int rowSpan = rMax - rMin + 1;
|
||
int colSpan = cMax - cMin + 1;
|
||
if (rowSpan < 3 || colSpan < 3) {
|
||
std::cout << " Filtering plane (" << plane.pointCount
|
||
<< " pts): rowSpan=" << rowSpan
|
||
<< ", colSpan=" << colSpan
|
||
<< " (need >= 3x3)" << std::endl;
|
||
return true;
|
||
}
|
||
return false;
|
||
});
|
||
|
||
int removedCount = static_cast<int>(std::distance(it, outPlanes.end()));
|
||
outPlanes.erase(it, outPlanes.end());
|
||
|
||
if (removedCount > 0) {
|
||
std::cout << "[RANSAC] Removed " << removedCount
|
||
<< " planes with insufficient row/col span, "
|
||
<< outPlanes.size() << " remaining" << std::endl;
|
||
}
|
||
}
|
||
|
||
// ===== 后处理：检测平面包含关系，仅保留较大的平面 =====
|
||
// 两级加速：
|
||
// 1) 包围盒快筛：小平面的 bbox 必须在大平面 bbox 内才可能被包含
|
||
// 2) 逐行列范围比对：对小平面占据的每一行，检查其列范围是否被大平面
|
||
// 在同一行的列范围所覆盖。全部行都被覆盖则认为小平面被包含。
|
||
if (outPlanes.size() > 1) {
|
||
const size_t planeCount = outPlanes.size();
|
||
|
||
// 预计算每个平面的 bbox 和逐行列范围
|
||
struct PlaneBBox {
|
||
int minRow, maxRow, minCol, maxCol;
|
||
};
|
||
std::vector<PlaneBBox> bboxes(planeCount);
|
||
|
||
// rowColRange[i] : 平面 i 在每一行上的 [minCol, maxCol]，-1 表示该行无点
|
||
struct ColRange { int minC = -1, maxC = -1; };
|
||
std::vector<std::vector<ColRange>> rowColRange(planeCount);
|
||
|
||
for (size_t i = 0; i < planeCount; i++) {
|
||
int rMin = rows, rMax = -1, cMin = cols, cMax = -1;
|
||
rowColRange[i].resize(rows); // 默认 {-1, -1}
|
||
|
||
for (int idx : outPlanes[i].pointIndices) {
|
||
int r = idx / cols;
|
||
int c = idx % cols;
|
||
if (r < rMin) rMin = r;
|
||
if (r > rMax) rMax = r;
|
||
if (c < cMin) cMin = c;
|
||
if (c > cMax) cMax = c;
|
||
|
||
ColRange& cr = rowColRange[i][r];
|
||
if (cr.minC < 0 || c < cr.minC) cr.minC = c;
|
||
if (c > cr.maxC) cr.maxC = c;
|
||
}
|
||
bboxes[i] = { rMin, rMax, cMin, cMax };
|
||
}
|
||
|
||
std::vector<bool> contained(planeCount, false);
|
||
|
||
for (size_t i = 0; i < planeCount; i++) {
|
||
if (contained[i]) continue;
|
||
for (size_t j = 0; j < planeCount; j++) {
|
||
if (i == j || contained[j]) continue;
|
||
if (outPlanes[j].pointCount >= outPlanes[i].pointCount) continue;
|
||
|
||
// 第 1 级：bbox 快筛
|
||
if (bboxes[j].minRow < bboxes[i].minRow ||
|
||
bboxes[j].maxRow > bboxes[i].maxRow ||
|
||
bboxes[j].minCol < bboxes[i].minCol ||
|
||
bboxes[j].maxCol > bboxes[i].maxCol) {
|
||
continue;
|
||
}
|
||
|
||
|
||
// 第 2 级：逐行列范围比对
|
||
bool allRowsCovered = true;
|
||
for (int r = bboxes[j].minRow; r <= bboxes[j].maxRow; r++) {
|
||
const ColRange& crSmall = rowColRange[j][r];
|
||
if (crSmall.minC < 0) continue; // 小平面该行无点，跳过
|
||
|
||
const ColRange& crLarge = rowColRange[i][r];
|
||
if (crLarge.minC < 0 ||
|
||
crSmall.minC < crLarge.minC ||
|
||
crSmall.maxC > crLarge.maxC) {
|
||
allRowsCovered = false;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (allRowsCovered) {
|
||
std::cout << " Plane containment: removing plane ("
|
||
<< outPlanes[j].pointCount
|
||
<< " pts) enclosed inside plane ("
|
||
<< outPlanes[i].pointCount << " pts)" << std::endl;
|
||
contained[j] = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
std::vector<PlaneSegment> filteredPlanes;
|
||
for (size_t i = 0; i < planeCount; i++) {
|
||
if (!contained[i]) {
|
||
filteredPlanes.push_back(std::move(outPlanes[i]));
|
||
}
|
||
}
|
||
|
||
int removedCount = static_cast<int>(planeCount - filteredPlanes.size());
|
||
if (removedCount > 0) {
|
||
std::cout << "[RANSAC] Removed " << removedCount
|
||
<< " contained planes, " << filteredPlanes.size()
|
||
<< " remaining" << std::endl;
|
||
}
|
||
|
||
outPlanes = std::move(filteredPlanes);
|
||
}
|
||
|
||
std::cout << "[RANSAC] Final: " << outPlanes.size() << " planes" << std::endl;
|
||
|
||
*errCode = HD_SUCCESS;
|
||
return static_cast<int>(outPlanes.size());
|
||
}
|
||
|
||
int FilterPlanesByNormal(
|
||
std::vector<PlaneSegment>& planes,
|
||
const NormalFilterParams& params,
|
||
const SVzNLPointXYZ* points,
|
||
int pointCount
|
||
) {
|
||
constexpr float kPi = 3.14159265358979323846f;
|
||
|
||
// 归一化参考法向量
|
||
float refLen = std::sqrt(
|
||
params.refNx * params.refNx +
|
||
params.refNy * params.refNy +
|
||
params.refNz * params.refNz
|
||
);
|
||
if (refLen < 1e-10f) return 0;
|
||
|
||
float rnx = params.refNx / refLen;
|
||
float rny = params.refNy / refLen;
|
||
float rnz = params.refNz / refLen;
|
||
|
||
// 计算每个平面的法向夹角
|
||
for (auto& plane : planes) {
|
||
// 使用绝对值处理法向正反两个方向
|
||
float dot = std::abs(plane.a * rnx + plane.b * rny + plane.c * rnz);
|
||
dot = std::min(1.0f, dot); // 防止数值误差导致 acos 越界
|
||
plane.normalAngleDeg = std::acos(dot) * 180.0f / kPi;
|
||
}
|
||
|
||
// 移除法向夹角超过阈值的平面
|
||
auto it = std::remove_if(planes.begin(), planes.end(),
|
||
[¶ms](const PlaneSegment& p) {
|
||
return p.normalAngleDeg > params.maxAngleDeg;
|
||
});
|
||
|
||
int removedCount = static_cast<int>(std::distance(it, planes.end()));
|
||
planes.erase(it, planes.end());
|
||
|
||
std::cout << "[Normal-Filter] Removed " << removedCount
|
||
<< " planes by normal direction, "
|
||
<< planes.size() << " remaining" << std::endl;
|
||
|
||
// 去除各平面 pointIndices 中距离平面过远的点（上方/下方均过滤）
|
||
if (params.maxDistFromPlane > 0.0f && points != nullptr && pointCount > 0) {
|
||
for (auto& plane : planes) {
|
||
auto& indices = plane.pointIndices;
|
||
auto removeIt = std::remove_if(indices.begin(), indices.end(),
|
||
[&](int idx) {
|
||
if (idx < 0 || idx >= pointCount) return true;
|
||
const SVzNLPointXYZ& pt = points[idx];
|
||
float signedDist = plane.a * pt.x + plane.b * pt.y + plane.c * pt.z + plane.d;
|
||
return std::abs(signedDist) > params.maxDistFromPlane;
|
||
});
|
||
indices.erase(removeIt, indices.end());
|
||
plane.pointCount = static_cast<int>(indices.size());
|
||
}
|
||
}
|
||
|
||
for (size_t i = 0; i < planes.size(); i++) {
|
||
std::cout << " Plane " << (i + 1)
|
||
<< ": normal angle=" << planes[i].normalAngleDeg << " deg"
|
||
<< ", points=" << planes[i].pointCount << std::endl;
|
||
}
|
||
|
||
return removedCount;
|
||
}
|
||
|
||
// ========== 行内Z差分波动统计 ==========
|
||
|
||
int ComputeRowZDiffFluctuation(
|
||
const SVzNLPointXYZ* points,
|
||
int rows,
|
||
int cols,
|
||
RowZDiffFluctuationStats* outStats,
|
||
int* errCode
|
||
) {
|
||
if (points == nullptr || rows <= 0 || cols <= 0 ||
|
||
outStats == nullptr || errCode == nullptr) {
|
||
if (errCode) *errCode = HD_ERR_INVALID_INPUT;
|
||
return HD_ERR_INVALID_INPUT;
|
||
}
|
||
|
||
std::vector<float> absDzValues;
|
||
absDzValues.reserve(rows * cols);
|
||
|
||
for (int r = 0; r < rows; ++r) {
|
||
bool hasPrev = false;
|
||
float prevZ = 0.0f;
|
||
for (int c = 0; c < cols; ++c) {
|
||
const int idx = r * cols + c;
|
||
const SVzNLPointXYZ& pt = points[idx];
|
||
if (!IsValidPoint(pt)) {
|
||
continue;
|
||
}
|
||
|
||
if (hasPrev) {
|
||
const float absDz = std::abs(pt.z - prevZ);
|
||
if (std::isfinite(absDz)) {
|
||
absDzValues.push_back(absDz);
|
||
}
|
||
}
|
||
|
||
prevZ = pt.z;
|
||
hasPrev = true;
|
||
}
|
||
}
|
||
|
||
if (absDzValues.size() < 3) {
|
||
*errCode = HD_ERR_INVALID_INPUT;
|
||
return HD_ERR_INVALID_INPUT;
|
||
}
|
||
|
||
std::vector<float> sorted = absDzValues;
|
||
std::sort(sorted.begin(), sorted.end());
|
||
|
||
const size_t n = sorted.size();
|
||
const float q1 = sorted[n / 4];
|
||
const float q3 = sorted[(3 * n) / 4];
|
||
const float iqr = q3 - q1;
|
||
const float lowerBound = std::max(0.0f, q1 - 1.5f * iqr);
|
||
const float upperBound = q3 + 1.5f * iqr;
|
||
|
||
std::vector<float> inliers;
|
||
inliers.reserve(absDzValues.size());
|
||
int outlierCount = 0;
|
||
for (float v : absDzValues) {
|
||
if (v >= lowerBound && v <= upperBound) {
|
||
inliers.push_back(v);
|
||
} else {
|
||
++outlierCount;
|
||
}
|
||
}
|
||
|
||
if (inliers.size() < 3) {
|
||
inliers = absDzValues;
|
||
outlierCount = 0;
|
||
}
|
||
|
||
std::sort(inliers.begin(), inliers.end());
|
||
const size_t m = inliers.size();
|
||
|
||
double sum = 0.0;
|
||
for (float v : inliers) {
|
||
sum += v;
|
||
}
|
||
const float mean = static_cast<float>(sum / m);
|
||
|
||
double sumSqDiff = 0.0;
|
||
for (float v : inliers) {
|
||
const double d = static_cast<double>(v) - mean;
|
||
sumSqDiff += d * d;
|
||
}
|
||
const float stdDev = static_cast<float>(std::sqrt(sumSqDiff / m));
|
||
|
||
float median = 0.0f;
|
||
if (m % 2 == 0) {
|
||
median = (inliers[m / 2 - 1] + inliers[m / 2]) * 0.5f;
|
||
} else {
|
||
median = inliers[m / 2];
|
||
}
|
||
|
||
outStats->meanAbsDz = mean;
|
||
outStats->medianAbsDz = median;
|
||
outStats->stdDevAbsDz = stdDev;
|
||
outStats->minAbsDz = inliers.front();
|
||
outStats->maxAbsDz = inliers.back();
|
||
outStats->rangeAbsDz = outStats->maxAbsDz - outStats->minAbsDz;
|
||
outStats->sampleCount = static_cast<int>(m);
|
||
outStats->outlierCount = outlierCount;
|
||
outStats->outlierRatio = static_cast<float>(outlierCount) /
|
||
static_cast<float>(absDzValues.size());
|
||
|
||
*errCode = HD_SUCCESS;
|
||
return HD_SUCCESS;
|
||
}
|