You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
142 lines
4.0 KiB
C++
142 lines
4.0 KiB
C++
/*------------------------------------------------------------------------------
|
|
* Copyright (c) 2023 by Bai Bing (seread@163.com)
|
|
* S++ COPYING file for copying and redistribution conditions.
|
|
*
|
|
* Alians IT Studio.
|
|
*----------------------------------------------------------------------------*/
|
|
#pragma once
|
|
|
|
#include "_Define.h"
|
|
#include "ASPoint.h"
|
|
#include "ASMatrix.h"
|
|
#include "ASGridInterpolatorBlock.h"
|
|
|
|
namespace ais
|
|
{
|
|
template <class PT = Point>
|
|
// Function to calculate the interpolated Z value at a given point
|
|
double trend_surface_interpolate(const std::vector<PT> &points, const PT &p, int degree)
|
|
{
|
|
int n = points.size();
|
|
int numCoeffs = (degree + 1) * (degree + 2) / 2; // Number of coefficients in the polynomial
|
|
|
|
ais::FMatrix A(n, numCoeffs);
|
|
std::vector<double> B(n);
|
|
std::vector<double> coeffs(numCoeffs);
|
|
|
|
for (int i = 0; i < n; ++i)
|
|
{
|
|
double xi = points[i].x;
|
|
double yi = points[i].y;
|
|
|
|
B[i] = points[i].z;
|
|
|
|
int col = 0;
|
|
for (int j = 0; j <= degree; ++j)
|
|
{
|
|
for (int k = 0; k <= j; ++k)
|
|
{
|
|
A.put(i, col, std::pow(xi, j - k) * std::pow(yi, k));
|
|
++col;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Solve the linear system of equations using Gaussian elimination
|
|
for (int i = 0; i < numCoeffs - 1; ++i)
|
|
{
|
|
for (int j = i + 1; j < numCoeffs; ++j)
|
|
{
|
|
double factor = A(j, i) / A(i, i);
|
|
B[j] -= factor * B[i];
|
|
for (int k = i; k < numCoeffs; ++k)
|
|
{
|
|
A(j, k) -= factor * A(i, k);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Back substitution to obtain the coefficients
|
|
for (int i = numCoeffs - 1; i >= 0; --i)
|
|
{
|
|
coeffs[i] = B[i];
|
|
for (int j = i + 1; j < numCoeffs; ++j)
|
|
{
|
|
coeffs[i] -= A(i, j) * coeffs[j];
|
|
}
|
|
coeffs[i] /= A(i, i);
|
|
}
|
|
|
|
double z = 0.0;
|
|
int col = 0;
|
|
for (int j = 0; j <= degree; ++j)
|
|
{
|
|
for (int k = 0; k <= j; ++k)
|
|
{
|
|
z += coeffs[col] * std::pow(p.x, j - k) * std::pow(p.y, k);
|
|
++col;
|
|
}
|
|
}
|
|
|
|
return z;
|
|
}
|
|
|
|
static int gaussian_1_4_total = 159;
|
|
static int gaussian_1_0_total = 273;
|
|
|
|
static ais::FMatrix gaussian_1_4_weights = {
|
|
{2, 4, 5, 4, 2},
|
|
{4, 9, 12, 9, 4},
|
|
{5, 12, 15, 12, 5},
|
|
{4, 9, 12, 9, 4},
|
|
{2, 4, 5, 4, 2},
|
|
};
|
|
|
|
static ais::FMatrix gaussian_1_0_weights = {
|
|
{1, 4, 7, 4, 1},
|
|
{4, 16, 26, 16, 4},
|
|
{7, 26, 41, 26, 7},
|
|
{4, 16, 26, 16, 4},
|
|
{1, 4, 7, 4, 1}};
|
|
|
|
double ma_gaussian_filter(size_t i, size_t j, const std::shared_ptr<ais::FMatrix> targetMatrixPtr, const ais::Shape &shape)
|
|
{
|
|
long xStart = i - 2, xEnd = i + 2;
|
|
long yStart = j - 2, yEnd = j + 2;
|
|
|
|
xStart = xStart < 0 ? 0 : xStart;
|
|
xEnd = xEnd > shape.cols - 1 ? shape.cols - 1 : xEnd;
|
|
|
|
yStart = yStart < 0 ? 0 : yStart;
|
|
yEnd = yEnd > shape.rows - 1 ? shape.rows - 1 : yEnd;
|
|
|
|
double sum = 0.0;
|
|
double weightTotal = 0.0;
|
|
|
|
for (auto tj = yStart; tj <= yEnd; tj++)
|
|
{
|
|
for (auto ti = xStart; ti <= xEnd; ti++)
|
|
{
|
|
auto tpos = tj * 5 + ti;
|
|
auto value = targetMatrixPtr->at(tpos);
|
|
if (!(std::isnan(value) || std::isinf(value)))
|
|
{
|
|
// gaussian matrix index
|
|
auto gmi = ti - i + 2;
|
|
auto gmj = tj - j + 2;
|
|
auto weight = gaussian_1_0_weights.at(gmj, gmi);
|
|
|
|
weightTotal += weight;
|
|
sum += value * weight;
|
|
}
|
|
}
|
|
}
|
|
|
|
double ret = std::nan("1");
|
|
|
|
if (weightTotal != 0 && sum != 0.0)
|
|
ret = sum / weightTotal;
|
|
|
|
return ret;
|
|
}
|
|
} |