util.cpp

#include "util.h"

#include <opencv2/imgproc.hpp>

namespace FVL {

class Gaussian {

private:
    /** The Constant SQRT PI. */
    constexpr static double SQRTPI = 1.772453850905516027;

    /** The Constant UPPER LIMIT. */
    constexpr static double UPPERLIMIT = 20.0;

    /** The Constant P10. */
    constexpr static double P10 = 242.66795523053175;

    /** The Constant P11. */
    constexpr static double P11 = 21.979261618294152;

    /** The Constant P12. */
    constexpr static double P12 = 6.9963834886191355;

    /** The Constant P13. */
    constexpr static double P13 = -.035609843701815385;

    /** The Constant Q10. */
    constexpr static double Q10 = 215.05887586986120;

    /** The Constant Q11. */
    constexpr static double Q11 = 91.164905404514901;

    /** The Constant Q12. */
    constexpr static double Q12 = 15.082797630407787;

    /** The Constant Q13. */
    constexpr static double Q13 = 1.0;

    /** The Constant P20. */
    constexpr static double P20 = 300.4592610201616005;

    /** The Constant P21. */
    constexpr static double P21 = 451.9189537118729422;

    /** The Constant P22. */
    constexpr static double P22 = 339.3208167343436870;

    /** The Constant P23. */
    constexpr static double P23 = 152.9892850469404039;

    /** The Constant P24. */
    constexpr static double P24 = 43.16222722205673530;

    /** The Constant P25. */
    constexpr static double P25 = 7.211758250883093659;

    /** The Constant P26. */
    constexpr static double P26 = .5641955174789739711;

    /** The Constant P27. */
    constexpr static double P27 = -.0000001368648573827167067;

    /** The Constant Q20. */
    constexpr static double Q20 = 300.4592609569832933;

    /** The Constant Q21. */
    constexpr static double Q21 = 790.9509253278980272;

    /** The Constant Q22. */
    constexpr static double Q22 = 931.3540948506096211;

    /** The Constant Q23. */
    constexpr static double Q23 = 638.9802644656311665;

    /** The Constant Q24. */
    constexpr static double Q24 = 277.5854447439876434;

    /** The Constant Q25. */
    constexpr static double Q25 = 77.00015293522947295;

    /** The Constant Q26. */
    constexpr static double Q26 = 12.78272731962942351;

    /** The Constant Q27. */
    constexpr static double Q27 = 1.0;

    /** The Constant P30. */
    constexpr static double P30 = -.00299610707703542174;

    /** The Constant P31. */
    constexpr static double P31 = -.0494730910623250734;

    /** The Constant P32. */
    constexpr static double P32 = -.226956593539686930;

    /** The Constant P33. */
    constexpr static double P33 = -.278661308609647788;

    /** The Constant P34. */
    constexpr static double P34 = -.0223192459734184686;

    /** The Constant Q30. */
    constexpr static double Q30 = .0106209230528467918;

    /** The Constant Q31. */
    constexpr static double Q31 = .191308926107829841;

    /** The Constant Q32. */
    constexpr static double Q32 = 1.05167510706793207;

    /** The Constant Q33. */
    constexpr static double Q33 = 1.98733201817135256;

    /** The Constant Q34. */
    constexpr static double Q34 = 1.0;

    /** The Constant SQRT2. */
    constexpr static double SQRT2 = 1.41421356237309504880;

    /**
     * Gets the smooth.
     * @param x  the x
     * @return the normal
     */
    static double getNormal(double x) {
        int    sn;
        double R1, R2, y, y2, y3, y4, y5, y6, y7;
        double erf, erfc, z, z2, z3, z4;
        double phi;

        if (x < -UPPERLIMIT)
            return 0.0;
        if (x > UPPERLIMIT)
            return 1.0;

        y = x / SQRT2;
        if (y < 0) {
            y  = -y;
            sn = -1;
        } else
            sn = 1;

        y2 = y * y;
        y4 = y2 * y2;
        y6 = y4 * y2;

        if (y < 0.46875) {
            R1  = P10 + P11 * y2 + P12 * y4 + P13 * y6;
            R2  = Q10 + Q11 * y2 + Q12 * y4 + Q13 * y6;
            erf = y * R1 / R2;
            if (sn == 1)
                phi = 0.5 + 0.5 * erf;
            else
                phi = 0.5 - 0.5 * erf;
        } else if (y < 4.0) {
            y3   = y2 * y;
            y5   = y4 * y;
            y7   = y6 * y;
            R1   = P20 + P21 * y + P22 * y2 + P23 * y3 + P24 * y4 + P25 * y5 + P26 * y6 + P27 * y7;
            R2   = Q20 + Q21 * y + Q22 * y2 + Q23 * y3 + Q24 * y4 + Q25 * y5 + Q26 * y6 + Q27 * y7;
            erfc = exp(-y2) * R1 / R2;
            if (sn == 1)
                phi = 1.0 - 0.5 * erfc;
            else
                phi = 0.5 * erfc;
        } else {
            z    = y4;
            z2   = z * z;
            z3   = z2 * z;
            z4   = z2 * z2;
            R1   = P30 + P31 * z + P32 * z2 + P33 * z3 + P34 * z4;
            R2   = Q30 + Q31 * z + Q32 * z2 + Q33 * z3 + Q34 * z4;
            erfc = (exp(-y2) / y) * (1.0 / SQRTPI + R1 / (R2 * y2));
            if (sn == 1)
                phi = 1.0 - 0.5 * erfc;
            else
                phi = 0.5 * erfc;
        }

        return phi;
    }

    /** The sqrt 2 pi inv. */
    /* 1/sqrt(2*PI) */
    constexpr static double SQRT_2_PI_INV = 0.398942280401432677939946059935;

public:
    constexpr static double MAX_SIZE_MASK_0 = 3.09023230616781;

    constexpr static double MAX_SIZE_MASK_1 = 3.46087178201605;

    constexpr static double MAX_SIZE_MASK_2 = 3.82922419517181;

    constexpr static double MAX_SIZE_MASK_3 = 5.15533333333333;

    /*
     * Functions to compute the integral, and the 0th and 1st derivative of the
     * Gaussian function 1/(sqrt(2*PI)*sigma)*exp(-0.5*x^2/sigma^2)
     */

    /**
     * Integral of the Gaussian function
     *
     * @param x   the x
     * @param sigma  the sigma
     * @return the double
     */
    static double integral(double x, double sigma) {
        return getNormal(x / sigma);
    }

    /**
     * The Gaussian function
     *
     * @param x the x
     * @param sigma  the sigma
     * @return the double
     */
    static double smooth(double x, double sigma) {
        const auto t = x / sigma;
        return SQRT_2_PI_INV / sigma * exp(-0.5 * t * t);
    }

    /**
     * First derivative of the Gaussian function
     *
     * @param x the x
     * @param sigma the sigma
     * @return the double
     */
    static double derivative1(double x, double sigma) {
        const auto t = x / sigma;
        return -x * SQRT_2_PI_INV / pow(sigma, 3.0) * exp(-0.5 * t * t);
    }

    /**
     * Second derivative of the Gaussian function
     *
     * @param x the x
     * @param sigma the sigma
     * @return the double
     */
    static double derivative2(double x, double sigma) {
        const auto t = x / sigma;
        return (x * x - sigma * sigma) * SQRT_2_PI_INV / pow(sigma, 5.0) * exp(-0.5 * t * t);
    }
};

void conv1d(const cv::Mat &src, cv::Mat &dst, const cv::Mat &kernel) {
    assert((src.rows == 1 && (src.rows == kernel.rows || src.rows == kernel.cols)) ||
           (src.cols == 1 && (src.cols == kernel.rows || src.cols == kernel.cols)));

    const auto srcWidth  = src.size().width;
    const auto srcHeight = src.size().height;
    const auto srcLength = srcWidth > srcHeight ? srcWidth : srcHeight;

    const auto kernelWidth  = kernel.size().width;
    const auto kernelHeight = kernel.size().height;
    const auto kernelLength = kernelWidth > kernelHeight ? kernelWidth : kernelHeight;

    auto        tmp         = cv::Mat(cv::Size(srcLength, kernelLength), CV_32FC1);
    const auto *kernelPtr   = kernel.ptr<float>(0);
    const auto *srcPtr      = src.ptr<float>(0);
    const auto  indexOffset = -(kernelLength / 2);

    for (int i = 0; i < kernelLength; i++) {
        const auto factor     = kernelPtr[ i ];
        auto      *tmpPtr     = tmp.ptr<float>(i);
        const auto indexStart = indexOffset + i;
        const auto indexEnd   = srcLength + indexStart;
        int        j          = indexStart;
        int        k          = 0;
        for (; j < 0; j++, k++) {
            tmpPtr[ k ] = factor * srcPtr[ abs(j) ];
        }
        const auto end = indexStart < 0 ? indexEnd : srcLength;
        for (; j < end; j++, k++) {
            tmpPtr[ k ] = factor * srcPtr[ j ];
        }
        for (; j < indexEnd; j++, k++) {
            tmpPtr[ k ] = factor * srcPtr[ 2 * (srcLength - 1) - j ];
        }
    }

    // dimension index along which the matrix is reduced.0 means that the matrix is reduced to a
    // single row.1 means that the matrix is reduced to a single column
    constexpr int dims = 0;
    cv::reduce(tmp, dst, dims, cv::REDUCE_SUM);
}

cv::Mat gauss(const double sigma) {
    const int n      = static_cast<int>(ceil(Gaussian::MAX_SIZE_MASK_0 * sigma));
    const int length = 2 * n + 1;
    cv::Mat   kernel(cv::Size(1, length), CV_32FC1, cv::Scalar(0));
    auto     *kernelPtr = kernel.ptr<float>(0);

    // for (int i = -n; i <= n; i++) {
    //     kernelPtr[ i + n ] = Gaussian::smooth(i * sigma, sigma);
    // }

    for (int i = -n + 1; i <= n - 1; i++) {
        const auto val = Gaussian::integral((-i + 0.5) * sigma, sigma) -
                         Gaussian::integral((-i - 0.5) * sigma, sigma);
        kernelPtr[ i + n ] = static_cast<float>(val);
    }
    kernelPtr[ 0 ]     = static_cast<float>(1. - Gaussian::integral((n - 0.5) * sigma, sigma));
    kernelPtr[ 2 * n ] = static_cast<float>(Gaussian::integral((-n + 0.5) * sigma, sigma));

    // normalize
    const auto sum  = cv::sum(kernel);
    kernel         /= sum;

    return kernel;
}

cv::Mat gaussDeriv1(const double sigma) {
    const int n      = static_cast<int>(ceil(Gaussian::MAX_SIZE_MASK_1 * sigma));
    const int length = 2 * n + 1;
    cv::Mat   kernel(cv::Size(1, length), CV_32FC1, cv::Scalar(0));
    auto     *kernelPtr = kernel.ptr<float>(0);

    // double sum = 0;
    // for (int i = 1; i <= n; i++) {
    //     const auto val      = Gaussian::derivative1(i * sigma, sigma);
    //     sum                += val;
    //     kernelPtr[ n + i ]  = -val;
    //     kernelPtr[ n - i ]  = val;
    // }

    for (int i = -n + 1; i <= n - 1; i++) {
        const auto val = Gaussian::smooth((-i + 0.5) * sigma, sigma) -
                         Gaussian::smooth((-i - 0.5) * sigma, sigma);
        kernelPtr[ i + n ] = static_cast<float>(val);
    }
    kernelPtr[ 0 ]     = static_cast<float>(-Gaussian::smooth((n - 0.5) * sigma, sigma));
    kernelPtr[ 2 * n ] = static_cast<float>(Gaussian::smooth((-n + 0.5) * sigma, sigma));

    // normalize
    const auto sum  = cv::sum(cv::abs(kernel)) / 2.;
    kernel         /= sum;

    return kernel;
}

cv::Mat gaussDeriv2(const double sigma) {
    const int n      = static_cast<int>(ceil(Gaussian::MAX_SIZE_MASK_2 * sigma));
    const int length = 2 * n + 1;
    cv::Mat   kernel(cv::Size(1, length), CV_32FC1, cv::Scalar(0));
    auto     *kernelPtr = kernel.ptr<float>(0);

    for (int i = -n + 1; i <= n - 1; i++) {
        const auto val = Gaussian::derivative1((-i + 0.5) * sigma, sigma) -
                         Gaussian::derivative1((-i - 0.5) * sigma, sigma);
        kernelPtr[ i + n ] = static_cast<float>(val);
    }

    kernelPtr[ 0 ]     = static_cast<float>(-Gaussian::derivative1((n - 0.5) * sigma, sigma));
    kernelPtr[ 2 * n ] = static_cast<float>(Gaussian::derivative1((-n + 0.5) * sigma, sigma));

    // normalize
    const auto sum  = cv::sum(cv::abs(kernel)) / 2.;
    kernel         /= sum;

    return kernel;
}

cv::Mat gaussDeriv3(const double sigma) {
    const int n      = static_cast<int>(ceil(Gaussian::MAX_SIZE_MASK_3 * sigma));
    const int length = 2 * n + 1;
    cv::Mat   kernel(cv::Size(1, length), CV_32FC1, cv::Scalar(0));
    auto     *kernelPtr = kernel.ptr<float>(0);

    for (int i = -n + 1; i <= n - 1; i++) {
        const auto val = Gaussian::derivative2((-i + 0.5) * sigma, sigma) -
                         Gaussian::derivative2((-i - 0.5) * sigma, sigma);
        kernelPtr[ i + n ] = static_cast<float>(val);
    }

    kernelPtr[ 0 ]     = static_cast<float>(Gaussian::derivative1((n - 0.5) * sigma, sigma));
    kernelPtr[ 2 * n ] = static_cast<float>(Gaussian::derivative1((-n + 0.5) * sigma, sigma));

    // normalize
    const auto sum  = cv::sum(cv::abs(kernel)) / 2.;
    kernel         /= sum;

    return kernel;
}

cv::Mat polyFit(const std::vector<cv::Point2f> &points, int order) {
    const int n = static_cast<int>(points.size());
    assert(n >= order + 1); // check enough point size
    assert(order >= 1);     // check order valid

    //matrix solve
    cv::Mat A = cv::Mat::ones(n, order + 1, CV_64FC1);
    cv::Mat B = cv::Mat::zeros(n, 1, CV_64FC1);

    for (int i = 0; i < n; ++i) {
        const auto  &point = points[ static_cast<unsigned int>(i) ];
        const double a     = point.x;
        const double b     = point.y;
        B.at<double>(i, 0) = b;

        double v = 1.0;
        for (int j = 0; j < order + 1; ++j, v *= a) {
            A.at<double>(i, j) = v;
        }
    }

    // solve (A^T * A) * coeffs = A^T * B
    cv::Mat coeffs;
    cv::Mat At  = A.t();
    cv::Mat AtA = At * A;
    cv::Mat AtB = At * B;
    cv::solve(AtA, AtB, coeffs, cv::DECOMP_NORMAL);

    return coeffs;
}

cv::Mat solve(const std::vector<cv::Point2f> &points, ProfileInterpolationMethod method) {
    int order = 0;

    switch (method) {
        case ProfileInterpolationMethod::Linear: {
            order = 1;
            break;
        }
        case ProfileInterpolationMethod::Quadratic3:
        case ProfileInterpolationMethod::Quadratic4:
        case ProfileInterpolationMethod::Quadratic5: {
            order = 2;
            break;
        }
        case ProfileInterpolationMethod::Cubic: {
            order = 3;
            break;
        }
    }

    auto    coeffs = polyFit(points, order);
    cv::Mat roots;
    cv::solvePoly(coeffs, roots);

    return roots;
}

float linear(const float a, const float b, const float step) {
    const auto k      = (b - a) / step;
    const auto offset = -a / k; // a + kx = 0

    return offset;
}

float quadratic3(const float a, const float b, const float c) {
    const auto offset = 0.5f * (a - c) / (a - b - b + c);
    return offset;
}

std::vector<Peak> findPeaks(const cv::Mat &src, const cv::Mat &derived, float threshold) {
    std::vector<Peak> peaks;

    const auto *d1Ptr = src.ptr<float>(0);
    const auto *d2Ptr = derived.ptr<float>(0);
    for (int i = 0; i < derived.cols - 1; i++) {
        // threshold
        const auto grad    = d1Ptr[ i ];
        const auto absGrad = std::fabs(grad);
        if (absGrad < threshold) {
            continue;
        }

        // cross zero
        if (d2Ptr[ i ] * d2Ptr[ i + 1 ] < 0) {
            peaks.push_back(
                {i, absGrad, std::signbit(grad) ? PeakType::Maximum : PeakType::Minimum});
        }
    }

    return peaks;
}

std::vector<Peak> filterPeak(const std::vector<Peak> &peaks, const PeakType type) {
    if (PeakType::Any == type) {
        return peaks;
    }

    std::vector<Peak> filtered;
    for (const auto &peak : peaks) {
        if (peak.type == type) {
            filtered.push_back(peak);
        }
    }

    return filtered;
}

bool selectPeak(const std::vector<Peak> &peaks,
                Peak                    &peak,
                const EdgeTransition     transition,
                const SelectionType      selection) {
    if (peaks.empty()) {
        return false;
    }

    auto selectPeakType = PeakType::Any;
    switch (transition) {
        case EdgeTransition::BrightToDark: {
            selectPeakType = PeakType::Minimum;
            break;
        }
        case EdgeTransition::DarkToBright: {
            selectPeakType = PeakType::Maximum;
            break;
        }
        case EdgeTransition::Any: {
            break;
        }
    }
    auto filteredPeaks = filterPeak(peaks, selectPeakType);
    if (filteredPeaks.empty()) {
        return false;
    }

    switch (selection) {
        case SelectionType::First: {
            peak = filteredPeaks.front();
            break;
        }
        case SelectionType::Last: {
            peak = filteredPeaks.back();
            break;
        }
        case SelectionType::Best: {
            peak = *std::max_element(
                filteredPeaks.begin(),
                filteredPeaks.end(),
                [](const Peak &a, const Peak &b) { return a.magnitude < b.magnitude; });
            break;
        }
    }

    return true;
}

float positionInterpolation(const cv::Mat                   &src,
                            int                              index,
                            const ProfileInterpolationMethod method) {
    const auto              *ptr = src.ptr<float>(0);
    std::vector<cv::Point2f> points;
    auto                     result = static_cast<float>(index);
    switch (method) {
        case ProfileInterpolationMethod::Linear: {
            if (index < 0 || index >= src.cols - 1) {
                return result;
            }

            points.emplace_back(static_cast<float>(index), ptr[ index ]);
            points.emplace_back(static_cast<float>(index) + 1.f, ptr[ index + 1 ]);
            break;
        }
        case ProfileInterpolationMethod::Quadratic3: {
            if (index <= 0 || index >= src.cols - 2) {
                return result;
            }

            points.emplace_back(static_cast<float>(index) - 1.f, ptr[ index - 1 ]);
            points.emplace_back(static_cast<float>(index), ptr[ index ]);
            points.emplace_back(static_cast<float>(index) + 1.f, ptr[ index + 1 ]);
            break;
        }
        case ProfileInterpolationMethod::Quadratic4: {
            if (index <= 0 || index >= src.cols - 3) {
                return result;
            }

            points.emplace_back(static_cast<float>(index) - 1.f, ptr[ index - 1 ]);
            points.emplace_back(static_cast<float>(index), ptr[ index ]);
            points.emplace_back(static_cast<float>(index) + 1.f, ptr[ index + 1 ]);
            points.emplace_back(static_cast<float>(index) + 2.f, ptr[ index + 2 ]);
            break;
        }
        case ProfileInterpolationMethod::Quadratic5:
        case ProfileInterpolationMethod::Cubic: {
            if (index <= 1 || index >= src.cols - 3) {
                return result;
            }

            points.emplace_back(static_cast<float>(index) - 2.f, ptr[ index - 2 ]);
            points.emplace_back(static_cast<float>(index) - 1.f, ptr[ index - 1 ]);
            points.emplace_back(static_cast<float>(index), ptr[ index ]);
            points.emplace_back(static_cast<float>(index) + 1.f, ptr[ index + 1 ]);
            points.emplace_back(static_cast<float>(index) + 2.f, ptr[ index + 2 ]);
            break;
        }
    }

    auto  roots   = solve(points, method);
    float minDist = std::numeric_limits<float>::max();
    for (int i = 0; i < roots.rows; i++) {
        const auto &root = roots.at<cv::Vec2d>(i);
        if (root[ 1 ] != 0) {
            continue;
        }

        const auto dist = std::abs(root[ 0 ] - index);
        if (dist < minDist) {
            minDist = static_cast<float>(dist);
            result  = static_cast<float>(root[ 0 ]);
        }
    }

    return result;
}

void point2Line(const cv::Point2f &point,
                const cv::Vec4f   &line,
                float             &distance,
                cv::Point2f       &pedal) {
    auto &vx = line[ 0 ];
    auto &vy = line[ 1 ];
    auto &x  = line[ 2 ];
    auto &y  = line[ 3 ];

    // vert
    if (vx == 0) {
        distance = point.x - x;
        pedal    = {x, point.y};
        return;
    }

    // hor
    if (vy == 0) {
        distance = point.y - y;
        pedal    = {point.x, y};
        return;
    }

    const auto     k = vy / vx;
    const auto     a = k;
    constexpr auto b = -1.f;
    const auto     c = y - k * x;

    auto px  = (b * b * point.x - a * b * point.y - a * c) / (a * a + b * b);
    auto py  = (a * a * point.y - a * b * point.x - b * c) / (a * a + b * b);
    pedal    = {px, py};
    distance = static_cast<float>(cv::norm(point - pedal));
}

} // namespace FVL