337 lines
12 KiB
C++
337 lines
12 KiB
C++
#include "CScanGridder.h"
|
|
|
|
#include <QHash>
|
|
#include <QPair>
|
|
#include <QtGlobal>
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <limits>
|
|
|
|
namespace {
|
|
|
|
struct CScanSamplePoint {
|
|
double x = 0.0;
|
|
double y = 0.0;
|
|
float amplitude = 0.0f;
|
|
int line = -1;
|
|
int channel = -1;
|
|
int trace = -1;
|
|
};
|
|
|
|
struct BinKey {
|
|
int x = 0;
|
|
int y = 0;
|
|
|
|
bool operator==(const BinKey &other) const
|
|
{
|
|
return x == other.x && y == other.y;
|
|
}
|
|
};
|
|
|
|
uint qHash(const BinKey &key, uint seed = 0)
|
|
{
|
|
return ::qHash(key.x, seed) ^ (::qHash(key.y, seed + 0x9e3779b9U) << 1);
|
|
}
|
|
|
|
static double distanceSquared(double ax, double ay, double bx, double by)
|
|
{
|
|
const double dx = ax - bx;
|
|
const double dy = ay - by;
|
|
return dx * dx + dy * dy;
|
|
}
|
|
|
|
static int gridIndex(int x, int y, int width)
|
|
{
|
|
return y * width + x;
|
|
}
|
|
|
|
static QVector<float> gaussianKernel(double sigma)
|
|
{
|
|
sigma = std::max(0.1, sigma);
|
|
const int radius = std::max(1, static_cast<int>(std::ceil(sigma * 3.0)));
|
|
QVector<float> kernel(radius * 2 + 1);
|
|
double sum = 0.0;
|
|
for (int i = -radius; i <= radius; ++i) {
|
|
const double v = std::exp(-(i * i) / (2.0 * sigma * sigma));
|
|
kernel[i + radius] = static_cast<float>(v);
|
|
sum += v;
|
|
}
|
|
if (sum > 0.0) {
|
|
for (float &v : kernel) v = static_cast<float>(v / sum);
|
|
}
|
|
return kernel;
|
|
}
|
|
|
|
static void smoothMasked(CScanGridResult &grid, double sigma)
|
|
{
|
|
if (!grid.valid || grid.width <= 0 || grid.height <= 0) return;
|
|
|
|
const QVector<float> kernel = gaussianKernel(sigma);
|
|
const int radius = kernel.size() / 2;
|
|
const int count = grid.width * grid.height;
|
|
|
|
QVector<float> tmpValues(count, 0.0f);
|
|
QVector<float> tmpWeights(count, 0.0f);
|
|
|
|
for (int y = 0; y < grid.height; ++y) {
|
|
for (int x = 0; x < grid.width; ++x) {
|
|
double sum = 0.0;
|
|
double weight = 0.0;
|
|
for (int k = -radius; k <= radius; ++k) {
|
|
const int sx = x + k;
|
|
if (sx < 0 || sx >= grid.width) continue;
|
|
const int idx = gridIndex(sx, y, grid.width);
|
|
if (!grid.validMask[idx]) continue;
|
|
const double w = kernel[k + radius];
|
|
sum += grid.values[idx] * w;
|
|
weight += w;
|
|
}
|
|
const int outIdx = gridIndex(x, y, grid.width);
|
|
if (weight > 1e-6) {
|
|
tmpValues[outIdx] = static_cast<float>(sum / weight);
|
|
tmpWeights[outIdx] = static_cast<float>(weight);
|
|
}
|
|
}
|
|
}
|
|
|
|
QVector<float> outValues(count, 0.0f);
|
|
QVector<unsigned char> outMask(count, 0);
|
|
|
|
for (int y = 0; y < grid.height; ++y) {
|
|
for (int x = 0; x < grid.width; ++x) {
|
|
double sum = 0.0;
|
|
double weight = 0.0;
|
|
for (int k = -radius; k <= radius; ++k) {
|
|
const int sy = y + k;
|
|
if (sy < 0 || sy >= grid.height) continue;
|
|
const int idx = gridIndex(x, sy, grid.width);
|
|
if (tmpWeights[idx] <= 1e-6f) continue;
|
|
const double w = kernel[k + radius] * tmpWeights[idx];
|
|
sum += tmpValues[idx] * w;
|
|
weight += w;
|
|
}
|
|
const int outIdx = gridIndex(x, y, grid.width);
|
|
if (weight > 1e-4) {
|
|
outValues[outIdx] = static_cast<float>(sum / weight);
|
|
outMask[outIdx] = 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
grid.values = std::move(outValues);
|
|
grid.validMask = std::move(outMask);
|
|
}
|
|
|
|
static float percentile(QVector<float> values, double percent)
|
|
{
|
|
if (values.isEmpty()) return 0.0f;
|
|
std::sort(values.begin(), values.end());
|
|
percent = std::clamp(percent, 0.0, 100.0);
|
|
const double pos = percent / 100.0 * (values.size() - 1);
|
|
const int lo = static_cast<int>(std::floor(pos));
|
|
const int hi = static_cast<int>(std::ceil(pos));
|
|
if (lo == hi) return values[lo];
|
|
const double t = pos - lo;
|
|
return static_cast<float>(values[lo] * (1.0 - t) + values[hi] * t);
|
|
}
|
|
|
|
static void updateDisplayRange(CScanGridResult &grid, double lowPercent, double highPercent)
|
|
{
|
|
QVector<float> validValues;
|
|
validValues.reserve(grid.values.size());
|
|
for (int i = 0; i < grid.values.size(); ++i) {
|
|
if (i < grid.validMask.size() && grid.validMask[i]) {
|
|
validValues.append(grid.values[i]);
|
|
}
|
|
}
|
|
|
|
if (validValues.isEmpty()) {
|
|
grid.displayMin = 0.0f;
|
|
grid.displayMax = 1.0f;
|
|
return;
|
|
}
|
|
|
|
grid.displayMin = percentile(validValues, lowPercent);
|
|
grid.displayMax = percentile(validValues, highPercent);
|
|
if (std::abs(grid.displayMax - grid.displayMin) < 1e-6f) {
|
|
grid.displayMax = grid.displayMin + 1.0f;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
CScanGridResult CScanGridder::build(const QVector<SurveyLine> &surveyLines, CScanGridOptions options)
|
|
{
|
|
CScanGridResult result;
|
|
|
|
options.cellSizeM = std::max(0.005, options.cellSizeM);
|
|
options.searchRadiusM = std::max(options.cellSizeM, options.searchRadiusM);
|
|
options.idwPower = std::max(0.1, options.idwPower);
|
|
options.smoothingSigmaCells = std::clamp(options.smoothingSigmaCells, 0.1, 5.0);
|
|
if (options.clipLowPercent >= options.clipHighPercent) {
|
|
options.clipLowPercent = 2.0;
|
|
options.clipHighPercent = 98.0;
|
|
}
|
|
options.maxGridWidth = std::max(64, options.maxGridWidth);
|
|
options.maxGridHeight = std::max(64, options.maxGridHeight);
|
|
options.maxGridCells = std::max(4096, options.maxGridCells);
|
|
|
|
QVector<CScanSamplePoint> samples;
|
|
double minX = std::numeric_limits<double>::max();
|
|
double minY = std::numeric_limits<double>::max();
|
|
double maxX = std::numeric_limits<double>::lowest();
|
|
double maxY = std::numeric_limits<double>::lowest();
|
|
|
|
for (int li = 0; li < surveyLines.size(); ++li) {
|
|
const SurveyLine &line = surveyLines[li];
|
|
if (!line.hasValidTrajectories()) continue;
|
|
|
|
const GPRDataModel &data = line.data;
|
|
|
|
const int channelCount = std::min(static_cast<int>(line.channelTrajectories.size()), data.getChannelCount());
|
|
const int traceCount = data.getTraceCountPerChannel();
|
|
const int sampleCount = data.getSampleCount();
|
|
if (channelCount <= 0 || traceCount <= 0 || sampleCount <= 0) continue;
|
|
|
|
const int sampleIndex = qBound(0, options.sampleIndex, sampleCount - 1);
|
|
for (int c = 0; c < channelCount; ++c) {
|
|
const QVector<QVector3D> &traj = line.channelTrajectories[c];
|
|
const int n = std::min(traceCount, static_cast<int>(traj.size()));
|
|
for (int t = 0; t < n; ++t) {
|
|
const RadarTrace *trace = data.getTrace(c, t);
|
|
if (!trace || sampleIndex >= trace->amplitudes.size()) continue;
|
|
|
|
const QVector3D &pos = traj[t];
|
|
const float amp = static_cast<float>(trace->amplitudes[sampleIndex]);
|
|
// Grid X uses CGCS easting (Y), grid Y uses CGCS northing (X),
|
|
// so the generated C-scan image aligns with map screen axes.
|
|
samples.append({pos.y(), pos.x(), amp, li, c, t});
|
|
minX = std::min(minX, static_cast<double>(pos.y()));
|
|
minY = std::min(minY, static_cast<double>(pos.x()));
|
|
maxX = std::max(maxX, static_cast<double>(pos.y()));
|
|
maxY = std::max(maxY, static_cast<double>(pos.x()));
|
|
}
|
|
}
|
|
}
|
|
|
|
if (samples.isEmpty() || maxX <= minX || maxY <= minY) {
|
|
return result;
|
|
}
|
|
|
|
result.dataMinX = minX;
|
|
result.dataMinY = minY;
|
|
result.dataMaxX = maxX;
|
|
result.dataMaxY = maxY;
|
|
|
|
minX -= options.searchRadiusM;
|
|
minY -= options.searchRadiusM;
|
|
maxX += options.searchRadiusM;
|
|
maxY += options.searchRadiusM;
|
|
|
|
double cellSize = options.cellSizeM;
|
|
int width = static_cast<int>(std::ceil((maxX - minX) / cellSize)) + 1;
|
|
int height = static_cast<int>(std::ceil((maxY - minY) / cellSize)) + 1;
|
|
while ((width > options.maxGridWidth || height > options.maxGridHeight
|
|
|| static_cast<qint64>(width) * height > options.maxGridCells) && cellSize < 10.0) {
|
|
cellSize *= 1.5;
|
|
width = static_cast<int>(std::ceil((maxX - minX) / cellSize)) + 1;
|
|
height = static_cast<int>(std::ceil((maxY - minY) / cellSize)) + 1;
|
|
}
|
|
|
|
if (width <= 0 || height <= 0 || width > options.maxGridWidth || height > options.maxGridHeight
|
|
|| static_cast<qint64>(width) * height > options.maxGridCells) {
|
|
return result;
|
|
}
|
|
|
|
result.valid = true;
|
|
result.originX = minX;
|
|
result.originY = minY;
|
|
result.cellSizeM = cellSize;
|
|
result.width = width;
|
|
result.height = height;
|
|
|
|
const int cellCount = width * height;
|
|
result.values.fill(0.0f, cellCount);
|
|
result.validMask.fill(0, cellCount);
|
|
result.nearestLine.fill(-1, cellCount);
|
|
result.nearestChannel.fill(-1, cellCount);
|
|
result.nearestTrace.fill(-1, cellCount);
|
|
|
|
const double binSize = options.searchRadiusM;
|
|
QHash<BinKey, QVector<int>> bins;
|
|
bins.reserve(samples.size());
|
|
for (int i = 0; i < samples.size(); ++i) {
|
|
const CScanSamplePoint &p = samples[i];
|
|
const BinKey key{static_cast<int>(std::floor((p.x - minX) / binSize)),
|
|
static_cast<int>(std::floor((p.y - minY) / binSize))};
|
|
bins[key].append(i);
|
|
}
|
|
|
|
const double radiusSq = options.searchRadiusM * options.searchRadiusM;
|
|
const int searchBins = std::max(1, static_cast<int>(std::ceil(options.searchRadiusM / binSize)));
|
|
|
|
for (int y = 0; y < height; ++y) {
|
|
const double cy = minY + y * cellSize;
|
|
const int by = static_cast<int>(std::floor((cy - minY) / binSize));
|
|
for (int x = 0; x < width; ++x) {
|
|
const double cx = minX + x * cellSize;
|
|
const int bx = static_cast<int>(std::floor((cx - minX) / binSize));
|
|
|
|
double sum = 0.0;
|
|
double weightSum = 0.0;
|
|
double bestDistSq = std::numeric_limits<double>::max();
|
|
int bestIndex = -1;
|
|
bool exact = false;
|
|
float exactValue = 0.0f;
|
|
|
|
for (int oy = -searchBins; oy <= searchBins; ++oy) {
|
|
for (int ox = -searchBins; ox <= searchBins; ++ox) {
|
|
const BinKey key{bx + ox, by + oy};
|
|
auto it = bins.constFind(key);
|
|
if (it == bins.constEnd()) continue;
|
|
|
|
for (int sampleIdx : it.value()) {
|
|
const CScanSamplePoint &p = samples[sampleIdx];
|
|
const double d2 = distanceSquared(cx, cy, p.x, p.y);
|
|
if (d2 > radiusSq) continue;
|
|
if (d2 < bestDistSq) {
|
|
bestDistSq = d2;
|
|
bestIndex = sampleIdx;
|
|
}
|
|
if (d2 < 1e-8) {
|
|
exact = true;
|
|
exactValue = p.amplitude;
|
|
bestIndex = sampleIdx;
|
|
break;
|
|
}
|
|
const double w = 1.0 / std::pow(std::sqrt(d2), options.idwPower);
|
|
sum += p.amplitude * w;
|
|
weightSum += w;
|
|
}
|
|
if (exact) break;
|
|
}
|
|
if (exact) break;
|
|
}
|
|
|
|
const int idx = gridIndex(x, y, width);
|
|
if (exact || weightSum > 0.0) {
|
|
result.values[idx] = exact ? exactValue : static_cast<float>(sum / weightSum);
|
|
result.validMask[idx] = 1;
|
|
if (bestIndex >= 0) {
|
|
const CScanSamplePoint &p = samples[bestIndex];
|
|
result.nearestLine[idx] = p.line;
|
|
result.nearestChannel[idx] = p.channel;
|
|
result.nearestTrace[idx] = p.trace;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (options.smoothingEnabled) {
|
|
smoothMasked(result, options.smoothingSigmaCells);
|
|
}
|
|
updateDisplayRange(result, options.clipLowPercent, options.clipHighPercent);
|
|
|
|
return result;
|
|
}
|