kev/Drawer/GVision/SurfaceGrid/lib/interpolator/ASInterpolateMethod.h

/*------------------------------------------------------------------------------
 *  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;
    }
}