fitcircle.cpp

#include "types.h"

#include <opencv2/imgproc.hpp>

namespace FVL {

static constexpr double eps = 1e-6;

static double calcDist2D(const std::vector<cv::Point2f> &points,
                         const Circle                   &circle,
                         std::vector<float>              dist) {
    double sum_dist = 0.;

    for (std::size_t i = 0; i < points.size(); i++) {
        dist[ i ] = static_cast<float>(fabs(cv::norm(points[ i ] - circle.center) - circle.radius));
        sum_dist += dist[ i ];
    }

    return sum_dist;
}

static void weightL1(float *d, int count, float *w) {
    for (int i = 0; i < count; i++) {
        const double t = fabs(static_cast<double>(d[ i ]));
        w[ i ]         = static_cast<float>(1. / MAX(t, eps));
    }
}

static void weightL12(float *d, int count, float *w) {
    for (int i = 0; i < count; i++) {
        w[ i ] = 1.0f / static_cast<float>(std::sqrt(1 + (double)(d[ i ] * d[ i ] * 0.5)));
    }
}

static void weightHuber(float *d, int count, float *w, float _c) {
    const float c = _c <= 0 ? 1.345f : _c;

    for (int i = 0; i < count; i++) {
        if (d[ i ] < c)
            w[ i ] = 1.0f;
        else
            w[ i ] = c / d[ i ];
    }
}

static void weightFair(float *d, int count, float *w, float _c) {
    const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;

    for (int i = 0; i < count; i++) {
        w[ i ] = 1 / (1 + d[ i ] * c);
    }
}

static void weightWelsch(float *d, int count, float *w, float _c) {
    const float c = _c == 0 ? 1 / 2.9846f : 1 / _c;

    for (int i = 0; i < count; i++) {
        w[ i ] = (float)std::exp(-d[ i ] * d[ i ] * c * c);
    }
}

void fitCircle(const std::vector<cv::Point2f> &selectPoint,
               const std::vector<float>       &weights,
               Circle                         &circle) {
    float x1, y1, x2, y2, x3, y3;
    float x1y1, x1y2, x2y1;
    float n = 0;

    x1 = y1 = x2 = y2 = x3 = y3 = 0;
    x1y1 = x1y2 = x2y1 = 0;

    for (size_t i = 0; i < selectPoint.size(); i++) {
        const float x      = selectPoint[ i ].x;
        const float y      = selectPoint[ i ].y;
        const float weight = weights[ i ];

        x1   += weight * x;
        y1   += weight * y;
        x2   += weight * x * x;
        y2   += weight * y * y;
        x3   += weight * x * x * x;
        y3   += weight * y * y * y;
        x1y1 += weight * x * y;
        x1y2 += weight * x * y * y;
        x2y1 += weight * x * x * y;
        n    += weight;
    }

    const cv::Mat   A = (cv::Mat_<float>(3, 3) << x2, x1y1, x1, x1y1, y2, y1, x1, y1, n);
    const cv::Vec3f b = {-x3 - x1y2, -x2y1 - y3, -x2 - y2};
    cv::Vec3f       res;
    cv::solve(A, b, res, cv::DECOMP_LU);

    const float centerX = -res[ 0 ] / 2;
    const float centerY = -res[ 1 ] / 2;

    circle.radius = sqrtf(centerY * centerY + centerX * centerX - res[ 2 ]);
    circle.center = cv::Point2f(centerX, centerY);
}

void fitCircle2D(const std::vector<cv::Point2f> &points,
                 int                             dist,
                 float                           _param,
                 float                           reps,
                 float                           aeps,
                 Circle                         &circle) {
    const double EPS                            = static_cast<double>(points.size()) * FLT_EPSILON;
    void (*calc_weights)(float *, int, float *) = nullptr;
    void (*calc_weights_param)(float *, int, float *, float) = nullptr;
    std::size_t i, j;
    Circle      _circle, _circlePrev;
    const float rDelta  = reps != 0 ? reps : 1.0f;
    const float aDelta  = aeps != 0 ? aeps : 0.01f;
    double      min_err = DBL_MAX, err = 0;
    cv::RNG     rng(static_cast<uint64>(-1));

    circle = {};

    switch (dist) {
        case cv::DIST_L2:
            return fitCircle(points, std::vector<float>(points.size(), 1.f), circle);

        case cv::DIST_L1:
            calc_weights = weightL1;
            break;

        case cv::DIST_L12:
            calc_weights = weightL12;
            break;

        case cv::DIST_FAIR:
            calc_weights_param = weightFair;
            break;

        case cv::DIST_WELSCH:
            calc_weights_param = weightWelsch;
            break;

        case cv::DIST_HUBER:
            calc_weights_param = weightHuber;
            break;

        /*case DIST_USER:
         calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
         break;*/
        default:
            CV_Error(cv::Error::StsBadArg, "Unknown distance type");
    }

    const auto         count = points.size();
    std::vector<float> weights(count);
    std::vector<float> distances(count);

    for (std::size_t k = 0; k < 20; k++) {
        int first = 1;
        for (i = 0; i < count; i++)
            weights[ i ] = 0.f;

        for (i = 0; i < MIN(count, 10);) {
            j = static_cast<std::size_t>(rng.uniform(0, static_cast<int>(count)));
            if (weights[ j ] < FLT_EPSILON) {
                weights[ j ] = 1.f;
                i++;
            }
        }

        fitCircle(points, weights, _circle);
        for (i = 0; i < 30; i++) {
            double sum_w = 0;

            if (first) {
                first = 0;
            } else {
                if (fabs(_circle.radius - _circlePrev.radius) < aDelta) {
                    float x, y;

                    x = fabs(_circle.center.x - _circlePrev.center.x);
                    y = fabs(_circle.center.y - _circlePrev.center.y);

                    const float d = x > y ? x : y;
                    if (d < rDelta)
                        break;
                }
            }
            /* calculate distances */
            err = calcDist2D(points, _circle, distances);

            if (err < min_err) {
                min_err = err;
                circle  = _circle;
                if (err < EPS)
                    break;
            }

            /* calculate weights */
            if (calc_weights)
                calc_weights(distances.data(), static_cast<int>(count), weights.data());
            else
                calc_weights_param(distances.data(),
                                   static_cast<int>(count),
                                   weights.data(),
                                   _param);

            for (j = 0; j < count; j++)
                sum_w += weights[ j ];

            if (fabs(sum_w) > FLT_EPSILON) {
                sum_w = 1. / sum_w;
                for (j = 0; j < count; j++)
                    weights[ j ] = static_cast<float>(weights[ j ] * sum_w);
            } else {
                for (j = 0; j < count; j++)
                    weights[ j ] = 1.f;
            }

            /* save the circle parameters */
            _circlePrev = _circle;

            /* Run again... */
            fitCircle(points, weights, _circle);
        }

        if (err < min_err) {
            min_err = err;
            circle  = _circle;
            if (err < EPS)
                break;
        }
    }
}

} // namespace FVL