Untitled

 avatar
unknown
c_cpp
12 days ago
35 kB
3
Indexable
#include <HXLDCont.h>
#include <common/CommonFunctions.h>

namespace
{
struct circleParams {
    double centerRow;
    double centerCol;
    double radius;
    double startPhi;
    double endPhi;
    std::string pointOrder;
};

circleParams fitContourAlgebraic(const std::vector<cv::Point2f>& points)
{
    circleParams res = {0, 0, 0, 0, 0, ""};
    int n = points.size();
    if (n < 3) {
        return res;
    }

    cv::Mat A(n, 3, CV_64F);
    cv::Mat b(n, 1, CV_64F);

    for (int i = 0; i < n; i++) {
        double r = points[i].y;
        double c = points[i].x;
        A.at<double>(i, 0) = r;
        A.at<double>(i, 1) = c;
        A.at<double>(i, 2) = 1.0;
        b.at<double>(i, 0) = -(r * r + c * c);
    }

    cv::Mat sol;
    cv::solve(A, b, sol, cv::DECOMP_SVD);

    double D = sol.at<double>(0, 0);
    double E = sol.at<double>(1, 0);
    double F = sol.at<double>(2, 0);

    double centerRow = -D / 2.0;
    double centerCol = -E / 2.0;
    double radius = std::sqrt(centerRow * centerRow + centerCol * centerCol - F);

    res.centerRow = centerRow;
    res.centerCol = centerCol;
    res.radius = radius;

    res.startPhi = std::atan2(points.front().y - centerRow, points.front().x - centerCol);
    res.endPhi = std::atan2(points.back().y - centerRow, points.back().x - centerCol);

    double crossSum = 0.0;
    for (int i = 0; i < n - 1; i++) {
        double x1 = points[i].x - centerCol;
        double y1 = points[i].y - centerRow;
        double x2 = points[i + 1].x - centerCol;
        double y2 = points[i + 1].y - centerRow;
        crossSum += (x1 * y2 - y1 * x2);
    }
    res.pointOrder = (crossSum > 0) ? "positive" : "negative";

    return res;
}

static double robustSigma(const std::vector<double>& errors)
{
    std::vector<double> absErrors;
    absErrors.reserve(errors.size());
    for (double e : errors) {
        absErrors.push_back(std::fabs(e));
    }
    std::nth_element(absErrors.begin(), absErrors.begin() + absErrors.size() / 2, absErrors.end());
    double median = absErrors[absErrors.size() / 2];
    return 1.4826 * median;
}

circleParams fitCircleAHuber(const std::vector<cv::Point2f>& points,
                             double maxClosureDist,
                             long clippingEndPoints,
                             long iterations,
                             double clippingFactor)
{
    std::vector<cv::Point2f> selectedPoints;
    selectedPoints = points;

    size_t n = selectedPoints.size();
    size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n);
    size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n;
    if (endIdx <= startIdx) {
        startIdx = 0;
        endIdx = n;
    }
    std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx);

    double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0;
    double S_x_r = 0, S_y_r = 0, S_r = 0;
    for (auto pt : fitPoints) {
        double x = pt.x;
        double y = pt.y;
        double r2 = x * x + y * y;
        S_xx += x * x;
        S_xy += x * y;
        S_yy += y * y;
        S_x += x;
        S_y += y;
        S_1 += 1;
        S_x_r += x * r2;
        S_y_r += y * r2;
        S_r += r2;
    }
    cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1);
    cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r);
    cv::Mat params;
    bool solved = cv::solve(M, b, params, cv::DECOMP_SVD);
    double A = params.at<double>(0);
    double B = params.at<double>(1);
    double C = params.at<double>(2);
    double cx = -A / 2.0;
    double cy = -B / 2.0;
    double r = std::sqrt(cx * cx + cy * cy - C);

    for (long it = 0; it < iterations; ++it) {
        std::vector<double> residuals;
        residuals.reserve(fitPoints.size());
        for (auto pt : fitPoints) {
            double dist = std::sqrt((pt.x - cx) * (pt.x - cx) + (pt.y - cy) * (pt.y - cy));
            double res = dist - r;
            residuals.push_back(res);
        }
        double sigma = robustSigma(residuals);
        double delta = clippingFactor * sigma;
        if (sigma < 1e-8)
            break;
        std::vector<double> weights;
        weights.reserve(fitPoints.size());
        for (double res : residuals) {
            double abs_res = std::fabs(res);
            double w = (abs_res <= delta ? 1.0 : delta / abs_res);
            weights.push_back(w);
        }

        double Wxx = 0, Wxy = 0, Wyy = 0, Wx = 0, Wy = 0, W1 = 0;
        double Wx_r = 0, Wy_r = 0, Wr = 0;
        for (size_t i = 0; i < fitPoints.size(); ++i) {
            double w = weights[i];
            double x = fitPoints[i].x;
            double y = fitPoints[i].y;
            double r2 = x * x + y * y;
            Wxx += w * x * x;
            Wxy += w * x * y;
            Wyy += w * y * y;
            Wx += w * x;
            Wy += w * y;
            W1 += w;
            Wx_r += w * x * r2;
            Wy_r += w * y * r2;
            Wr += w * r2;
        }
        cv::Mat WM = (cv::Mat_<double>(3, 3) << Wxx, Wxy, Wx, Wxy, Wyy, Wy, Wx, Wy, W1);
        cv::Mat Wb = (cv::Mat_<double>(3, 1) << -Wx_r, -Wy_r, -Wr);
        cv::Mat Wparams;
        bool solvedW = cv::solve(WM, Wb, Wparams, cv::DECOMP_SVD);
        if (!solvedW)
            break;
        A = Wparams.at<double>(0);
        B = Wparams.at<double>(1);
        C = Wparams.at<double>(2);
        double new_cx = -A / 2.0;
        double new_cy = -B / 2.0;
        double new_r = std::sqrt(new_cx * new_cx + new_cy * new_cy - C);
        if (std::fabs(new_cx - cx) < 1e-6 && std::fabs(new_cy - cy) < 1e-6 && std::fabs(new_r - r) < 1e-6) {
            cx = new_cx;
            cy = new_cy;
            r = new_r;
            break;
        }
        cx = new_cx;
        cy = new_cy;
        r = new_r;
    }

    double startPhi, endPhi;
    {
        double dx1 = points.front().x - cx;
        double dy1 = points.front().y - cy;
        double dx2 = points.back().x - cx;
        double dy2 = points.back().y - cy;
        startPhi = std::atan2(dy1, dx1);
        endPhi = std::atan2(dy2, dx2);
        if (startPhi < 0)
            startPhi += 2 * M_PI;
        if (endPhi < 0)
            endPhi += 2 * M_PI;
    }

    std::string pointOrder;
    double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) +
                                       std::pow(points.front().y - points.back().y, 2));
    if (first_last_dist <= maxClosureDist) {
        startPhi = 0;
        endPhi = 2 * M_PI;
        pointOrder = "closed";
    } else {
        double cross =
                ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx));
        pointOrder = (cross >= 0 ? "positive" : "negative");
    }

    circleParams result;
    result.centerRow = cy;
    result.centerCol = cx;
    result.radius = r;
    result.startPhi = startPhi;
    result.endPhi = endPhi;
    result.pointOrder = pointOrder;
    return result;
}

circleParams fitCircleATukey(const std::vector<cv::Point2f>& points,
                             long maxNumPoints,
                             double maxClosureDist,
                             long clippingEndPoints,
                             long iterations,
                             double clippingFactor)
{
    std::vector<cv::Point2f> selectedPoints;
    if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) {
        size_t step = points.size() / maxNumPoints;
        for (size_t i = 0; i < points.size(); i += step)
            selectedPoints.push_back(points[i]);
        if (selectedPoints.size() > static_cast<size_t>(maxNumPoints))
            selectedPoints.resize(maxNumPoints);
    } else {
        selectedPoints = points;
    }

    size_t n = selectedPoints.size();
    size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n);
    size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n;
    if (endIdx <= startIdx) {
        startIdx = 0;
        endIdx = n;
    }
    std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx);

    double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0;
    double S_x_r = 0, S_y_r = 0, S_r = 0;
    for (auto pt : fitPoints) {
        double x = pt.x;
        double y = pt.y;
        double r2 = x * x + y * y;
        S_xx += x * x;
        S_xy += x * y;
        S_yy += y * y;
        S_x += x;
        S_y += y;
        S_1 += 1;
        S_x_r += x * r2;
        S_y_r += y * r2;
        S_r += r2;
    }
    cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1);
    cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r);
    cv::Mat params;
    bool solved = cv::solve(M, b, params, cv::DECOMP_SVD);
    double A = params.at<double>(0);
    double B = params.at<double>(1);
    double C = params.at<double>(2);
    double cx = -A / 2.0;
    double cy = -B / 2.0;
    double r = std::sqrt(cx * cx + cy * cy - C);

    for (long it = 0; it < iterations; ++it) {
        std::vector<double> residuals;
        residuals.reserve(fitPoints.size());
        for (auto pt : fitPoints) {
            double dist = std::sqrt((pt.x - cx) * (pt.x - cx) + (pt.y - cy) * (pt.y - cy));
            double res = dist - r;
            residuals.push_back(res);
        }
        double sigma = robustSigma(residuals);
        double delta = clippingFactor * sigma;
        if (sigma < 1e-8)
            break;

        std::vector<double> weights;
        weights.reserve(fitPoints.size());
        for (double res : residuals) {
            double abs_res = std::fabs(res);
            double w = (abs_res <= delta) ? std::pow(1.0 - (abs_res / delta) * (abs_res / delta), 2.0) : 0.0;
            weights.push_back(w);
        }

        double Wxx = 0, Wxy = 0, Wyy = 0, Wx = 0, Wy = 0, W1 = 0;
        double Wx_r = 0, Wy_r = 0, Wr = 0;
        for (size_t i = 0; i < fitPoints.size(); ++i) {
            double w = weights[i];
            double x = fitPoints[i].x;
            double y = fitPoints[i].y;
            double r2 = x * x + y * y;
            Wxx += w * x * x;
            Wxy += w * x * y;
            Wyy += w * y * y;
            Wx += w * x;
            Wy += w * y;
            W1 += w;
            Wx_r += w * x * r2;
            Wy_r += w * y * r2;
            Wr += w * r2;
        }
        cv::Mat WM = (cv::Mat_<double>(3, 3) << Wxx, Wxy, Wx, Wxy, Wyy, Wy, Wx, Wy, W1);
        cv::Mat Wb = (cv::Mat_<double>(3, 1) << -Wx_r, -Wy_r, -Wr);
        cv::Mat Wparams;
        bool solvedW = cv::solve(WM, Wb, Wparams, cv::DECOMP_SVD);
        if (!solvedW)
            break;
        A = Wparams.at<double>(0);
        B = Wparams.at<double>(1);
        C = Wparams.at<double>(2);
        double new_cx = -A / 2.0;
        double new_cy = -B / 2.0;
        double new_r = std::sqrt(new_cx * new_cx + new_cy * new_cy - C);

        if (std::fabs(new_cx - cx) < 1e-6 && std::fabs(new_cy - cy) < 1e-6 && std::fabs(new_r - r) < 1e-6) {
            cx = new_cx;
            cy = new_cy;
            r = new_r;
            break;
        }
        cx = new_cx;
        cy = new_cy;
        r = new_r;
    }

    double startPhi, endPhi;
    {
        double dx1 = points.front().x - cx;
        double dy1 = points.front().y - cy;
        double dx2 = points.back().x - cx;
        double dy2 = points.back().y - cy;
        startPhi = std::atan2(dy1, dx1);
        endPhi = std::atan2(dy2, dx2);
        if (startPhi < 0)
            startPhi += 2 * M_PI;
        if (endPhi < 0)
            endPhi += 2 * M_PI;
    }

    std::string pointOrder;
    double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) +
                                       std::pow(points.front().y - points.back().y, 2));
    if (first_last_dist <= maxClosureDist) {
        startPhi = 0;
        endPhi = 2 * M_PI;
        pointOrder = "closed";
    } else {
        double cross =
                ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx));
        pointOrder = (cross >= 0 ? "positive" : "negative");
    }

    circleParams result;
    result.centerRow = cy;
    result.centerCol = cx;
    result.radius = r;
    result.startPhi = startPhi;
    result.endPhi = endPhi;
    result.pointOrder = pointOrder;
    return result;
}

circleParams fitCircleGeometric(const std::vector<cv::Point2f>& points,
                                long maxNumPoints,
                                double maxClosureDist,
                                long clippingEndPoints,
                                long iterations,
                                double clippingFactor)
{
    std::vector<cv::Point2f> selectedPoints;
    if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) {
        size_t step = points.size() / maxNumPoints;
        for (size_t i = 0; i < points.size(); i += step)
            selectedPoints.push_back(points[i]);
        if (selectedPoints.size() > static_cast<size_t>(maxNumPoints))
            selectedPoints.resize(maxNumPoints);
    } else {
        selectedPoints = points;
    }

    size_t n = selectedPoints.size();
    size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n);
    size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n;
    if (endIdx <= startIdx) {
        startIdx = 0;
        endIdx = n;
    }
    std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx);

    double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0;
    double S_x_r = 0, S_y_r = 0, S_r = 0;
    for (auto pt : fitPoints) {
        double x = pt.x;
        double y = pt.y;
        double r2 = x * x + y * y;
        S_xx += x * x;
        S_xy += x * y;
        S_yy += y * y;
        S_x += x;
        S_y += y;
        S_1 += 1;
        S_x_r += x * r2;
        S_y_r += y * r2;
        S_r += r2;
    }
    cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1);
    cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r);
    cv::Mat params;
    bool solved = cv::solve(M, b, params, cv::DECOMP_SVD);
    double A = params.at<double>(0);
    double B = params.at<double>(1);
    double C = params.at<double>(2);
    double cx = -A / 2.0;
    double cy = -B / 2.0;
    double r = std::sqrt(cx * cx + cy * cy - C);

    const int maxIter = 20;
    const double tol = 1e-6;
    for (int iter = 0; iter < maxIter; ++iter) {
        double A11 = 0, A12 = 0, A13 = 0;
        double A22 = 0, A23 = 0;
        double A33 = 0;
        double b1 = 0, b2 = 0, b3 = 0;
        int validCount = 0;
        for (auto pt : fitPoints) {
            double dx = cx - pt.x;
            double dy = cy - pt.y;
            double d = std::sqrt(dx * dx + dy * dy);
            if (d < 1e-8)
                continue;
            double f_i = d - r;
            double J0 = dx / d;
            double J1 = dy / d;
            double J2 = -1;

            A11 += J0 * J0;
            A12 += J0 * J1;
            A13 += J0 * J2;
            A22 += J1 * J1;
            A23 += J1 * J2;
            A33 += J2 * J2;

            b1 += J0 * f_i;
            b2 += J1 * f_i;
            b3 += J2 * f_i;
            validCount++;
        }
        if (validCount == 0)
            break;

        cv::Mat A_mat = (cv::Mat_<double>(3, 3) << A11, A12, A13, A12, A22, A23, A13, A23, A33);
        cv::Mat b_mat = (cv::Mat_<double>(3, 1) << b1, b2, b3);
        cv::Mat delta;
        bool solvedGN = cv::solve(A_mat, b_mat, delta, cv::DECOMP_SVD);
        if (!solvedGN)
            break;
        double d_cx = delta.at<double>(0);
        double d_cy = delta.at<double>(1);
        double d_r = delta.at<double>(2);

        cx -= d_cx;
        cy -= d_cy;
        r -= d_r;

        if (std::sqrt(d_cx * d_cx + d_cy * d_cy + d_r * d_r) < tol)
            break;
    }

    double startPhi, endPhi;
    {
        double dx1 = points.front().x - cx;
        double dy1 = points.front().y - cy;
        double dx2 = points.back().x - cx;
        double dy2 = points.back().y - cy;
        startPhi = std::atan2(dy1, dx1);
        endPhi = std::atan2(dy2, dx2);
        if (startPhi < 0)
            startPhi += 2 * M_PI;
        if (endPhi < 0)
            endPhi += 2 * M_PI;
    }

    std::string pointOrder;
    double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) +
                                       std::pow(points.front().y - points.back().y, 2));
    if (first_last_dist <= maxClosureDist) {
        startPhi = 0;
        endPhi = 2 * M_PI;
        pointOrder = "closed";
    } else {
        double cross =
                ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx));
        pointOrder = (cross >= 0 ? "positive" : "negative");
    }

    circleParams result;
    result.centerRow = cy;
    result.centerCol = cx;
    result.radius = r;
    result.startPhi = startPhi;
    result.endPhi = endPhi;
    result.pointOrder = pointOrder;
    return result;
}

circleParams fitCircleGeoHuber(const std::vector<cv::Point2f>& points,
                               long maxNumPoints,
                               double maxClosureDist,
                               long clippingEndPoints,
                               double clippingFactor)
{
    std::vector<cv::Point2f> selectedPoints;
    if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) {
        size_t step = points.size() / maxNumPoints;
        for (size_t i = 0; i < points.size(); i += step)
            selectedPoints.push_back(points[i]);
        if (selectedPoints.size() > static_cast<size_t>(maxNumPoints))
            selectedPoints.resize(maxNumPoints);
    } else {
        selectedPoints = points;
    }

    size_t n = selectedPoints.size();
    size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n);
    size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n;
    if (endIdx <= startIdx) {
        startIdx = 0;
        endIdx = n;
    }
    std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx);

    double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0;
    double S_x_r = 0, S_y_r = 0, S_r = 0;
    for (auto pt : fitPoints) {
        double x = pt.x;
        double y = pt.y;
        double r2 = x * x + y * y;
        S_xx += x * x;
        S_xy += x * y;
        S_yy += y * y;
        S_x += x;
        S_y += y;
        S_1 += 1;
        S_x_r += x * r2;
        S_y_r += y * r2;
        S_r += r2;
    }
    cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1);
    cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r);
    cv::Mat params;
    bool solved = cv::solve(M, b, params, cv::DECOMP_SVD);
    double A = params.at<double>(0);
    double B = params.at<double>(1);
    double C = params.at<double>(2);
    double cx = -A / 2.0;
    double cy = -B / 2.0;
    double r = std::sqrt(cx * cx + cy * cy - C);

    const int maxIter = 20;
    const double tol = 1e-6;

    for (int iter = 0; iter < maxIter; ++iter) {
        std::vector<double> residuals;
        residuals.reserve(fitPoints.size());
        for (auto pt : fitPoints) {
            double dx = cx - pt.x;
            double dy = cy - pt.y;
            double d = std::sqrt(dx * dx + dy * dy);
            if (d < 1e-8)
                continue;
            double f_val = d - r;
            residuals.push_back(f_val);
        }
        double sigma = robustSigma(residuals);
        double deltaThreshold = clippingFactor * sigma;
        if (sigma < 1e-8)
            break;

        double A11 = 0, A12 = 0, A13 = 0;
        double A22 = 0, A23 = 0;
        double A33 = 0;
        double b1 = 0, b2 = 0, b3 = 0;
        int validCount = 0;
        for (auto pt : fitPoints) {
            double dx = cx - pt.x;
            double dy = cy - pt.y;
            double d = std::sqrt(dx * dx + dy * dy);
            if (d < 1e-8)
                continue;
            double f_val = d - r;
            double abs_f = std::fabs(f_val);
            double w = (abs_f <= deltaThreshold) ? 1.0 : (deltaThreshold / abs_f);

            double J0 = dx / d;
            double J1 = dy / d;
            double J2 = -1.0;

            A11 += w * J0 * J0;
            A12 += w * J0 * J1;
            A13 += w * J0 * J2;
            A22 += w * J1 * J1;
            A23 += w * J1 * J2;
            A33 += w * J2 * J2;
            b1 += w * J0 * f_val;
            b2 += w * J1 * f_val;
            b3 += w * J2 * f_val;
            validCount++;
        }
        if (validCount == 0)
            break;

        cv::Mat A_mat = (cv::Mat_<double>(3, 3) << A11, A12, A13, A12, A22, A23, A13, A23, A33);
        cv::Mat b_mat = (cv::Mat_<double>(3, 1) << b1, b2, b3);
        cv::Mat deltaParams;
        bool solvedGN = cv::solve(A_mat, b_mat, deltaParams, cv::DECOMP_SVD);
        if (!solvedGN)
            break;
        double d_cx = deltaParams.at<double>(0);
        double d_cy = deltaParams.at<double>(1);
        double d_r = deltaParams.at<double>(2);

        cx -= d_cx;
        cy -= d_cy;
        r -= d_r;

        if (std::sqrt(d_cx * d_cx + d_cy * d_cy + d_r * d_r) < tol)
            break;
    }

    double startPhi, endPhi;
    {
        double dx1 = points.front().x - cx;
        double dy1 = points.front().y - cy;
        double dx2 = points.back().x - cx;
        double dy2 = points.back().y - cy;
        startPhi = std::atan2(dy1, dx1);
        endPhi = std::atan2(dy2, dx2);
        if (startPhi < 0)
            startPhi += 2 * M_PI;
        if (endPhi < 0)
            endPhi += 2 * M_PI;
    }

    std::string pointOrder;
    double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) +
                                       std::pow(points.front().y - points.back().y, 2));
    if (first_last_dist <= maxClosureDist) {
        startPhi = 0;
        endPhi = 2 * M_PI;
        pointOrder = "closed";
    } else {
        double cross =
                ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx));
        pointOrder = (cross >= 0 ? "positive" : "negative");
    }

    circleParams result;
    result.centerRow = cy;
    result.centerCol = cx;
    result.radius = r;
    result.startPhi = startPhi;
    result.endPhi = endPhi;
    result.pointOrder = pointOrder;
    return result;
}

circleParams fitCircleGeoTukey(const std::vector<cv::Point2f>& points,
                               long maxNumPoints,
                               double maxClosureDist,
                               long clippingEndPoints,
                               double clippingFactor)
{
    std::vector<cv::Point2f> selectedPoints;
    if (maxNumPoints > 0 && points.size() > static_cast<size_t>(maxNumPoints)) {
        size_t step = points.size() / maxNumPoints;
        for (size_t i = 0; i < points.size(); i += step)
            selectedPoints.push_back(points[i]);
        if (selectedPoints.size() > static_cast<size_t>(maxNumPoints))
            selectedPoints.resize(maxNumPoints);
    } else {
        selectedPoints = points;
    }

    size_t n = selectedPoints.size();
    size_t startIdx = std::min(static_cast<size_t>(clippingEndPoints), n);
    size_t endIdx = (n > static_cast<size_t>(clippingEndPoints)) ? n - clippingEndPoints : n;
    if (endIdx <= startIdx) {
        startIdx = 0;
        endIdx = n;
    }
    std::vector<cv::Point2f> fitPoints(selectedPoints.begin() + startIdx, selectedPoints.begin() + endIdx);

    double S_xx = 0, S_xy = 0, S_yy = 0, S_x = 0, S_y = 0, S_1 = 0;
    double S_x_r = 0, S_y_r = 0, S_r = 0;
    for (auto pt : fitPoints) {
        double x = pt.x;
        double y = pt.y;
        double r2 = x * x + y * y;
        S_xx += x * x;
        S_xy += x * y;
        S_yy += y * y;
        S_x += x;
        S_y += y;
        S_1 += 1;
        S_x_r += x * r2;
        S_y_r += y * r2;
        S_r += r2;
    }
    cv::Mat M = (cv::Mat_<double>(3, 3) << S_xx, S_xy, S_x, S_xy, S_yy, S_y, S_x, S_y, S_1);
    cv::Mat b = (cv::Mat_<double>(3, 1) << -S_x_r, -S_y_r, -S_r);
    cv::Mat params;
    bool solved = cv::solve(M, b, params, cv::DECOMP_SVD);

    double A = params.at<double>(0);
    double B = params.at<double>(1);
    double C = params.at<double>(2);
    double cx = -A / 2.0;
    double cy = -B / 2.0;
    double r = std::sqrt(cx * cx + cy * cy - C);

    const int maxIter = 20;
    const double tol = 1e-6;

    for (int iter = 0; iter < maxIter; ++iter) {
        std::vector<double> residuals;
        residuals.reserve(fitPoints.size());
        for (auto pt : fitPoints) {
            double dx = cx - pt.x;
            double dy = cy - pt.y;
            double d = std::sqrt(dx * dx + dy * dy);
            if (d < 1e-8)
                continue;
            double f_val = d - r;
            residuals.push_back(f_val);
        }
        double sigma = robustSigma(residuals);
        double deltaThreshold = clippingFactor * sigma;
        if (sigma < 1e-8)
            break;

        double A11 = 0, A12 = 0, A13 = 0;
        double A22 = 0, A23 = 0;
        double A33 = 0;
        double b1 = 0, b2 = 0, b3 = 0;
        int validCount = 0;
        for (auto pt : fitPoints) {
            double dx = cx - pt.x;
            double dy = cy - pt.y;
            double d = std::sqrt(dx * dx + dy * dy);
            if (d < 1e-8)
                continue;
            double f_val = d - r;
            double abs_f = std::fabs(f_val);
            double w = (abs_f <= deltaThreshold)
                               ? std::pow(1.0 - (abs_f / deltaThreshold) * (abs_f / deltaThreshold), 2.0)
                               : 0.0;

            double J0 = dx / d;
            double J1 = dy / d;
            double J2 = -1.0;

            A11 += w * J0 * J0;
            A12 += w * J0 * J1;
            A13 += w * J0 * J2;
            A22 += w * J1 * J1;
            A23 += w * J1 * J2;
            A33 += w * J2 * J2;
            b1 += w * J0 * f_val;
            b2 += w * J1 * f_val;
            b3 += w * J2 * f_val;
            validCount++;
        }
        if (validCount == 0)
            break;

        cv::Mat A_mat = (cv::Mat_<double>(3, 3) << A11, A12, A13, A12, A22, A23, A13, A23, A33);
        cv::Mat b_mat = (cv::Mat_<double>(3, 1) << b1, b2, b3);
        cv::Mat deltaParams;
        bool solvedGN = cv::solve(A_mat, b_mat, deltaParams, cv::DECOMP_SVD);
        if (!solvedGN)
            break;
        double d_cx = deltaParams.at<double>(0);
        double d_cy = deltaParams.at<double>(1);
        double d_r = deltaParams.at<double>(2);

        cx -= d_cx;
        cy -= d_cy;
        r -= d_r;
        if (std::sqrt(d_cx * d_cx + d_cy * d_cy + d_r * d_r) < tol)
            break;
    }

    double startPhi, endPhi;
    {
        double dx1 = points.front().x - cx;
        double dy1 = points.front().y - cy;
        double dx2 = points.back().x - cx;
        double dy2 = points.back().y - cy;
        startPhi = std::atan2(dy1, dx1);
        endPhi = std::atan2(dy2, dx2);
        if (startPhi < 0)
            startPhi += 2 * M_PI;
        if (endPhi < 0)
            endPhi += 2 * M_PI;
    }

    std::string pointOrder;
    double first_last_dist = std::sqrt(std::pow(points.front().x - points.back().x, 2) +
                                       std::pow(points.front().y - points.back().y, 2));
    if (first_last_dist <= maxClosureDist) {
        startPhi = 0;
        endPhi = 2 * M_PI;
        pointOrder = "closed";
    } else {
        double cross =
                ((points.front().x - cx) * (points.back().y - cy) - (points.front().y - cy) * (points.back().x - cx));
        pointOrder = (cross >= 0 ? "positive" : "negative");
    }

    circleParams result;
    result.centerRow = cy;
    result.centerCol = cx;
    result.radius = r;
    result.startPhi = startPhi;
    result.endPhi = endPhi;
    result.pointOrder = pointOrder;
    return result;
}

}  // namespace

namespace Huawei
{
const std::set<std::string> allowedModes = {"algebraic", "ahuber", "atukey", "geometric", "geohuber", "geotukey"};

void HXLDCont::HwFitCircleContourXld(const std::string& algorithm,
                                     Hlong maxNumPoints,
                                     double maxClosureDist,
                                     Hlong clippingEndPoints,
                                     Hlong iterations,
                                     double clippingFactor,
                                     HTuple& row,
                                     HTuple& column,
                                     HTuple& radius,
                                     HTuple& startPhi,
                                     HTuple& endPhi,
                                     HTuple& pointOrder) const
{
    double r, c, rd, sP, eP;
    std::string pO;

    this->HwFitCircleContourXld(algorithm, maxNumPoints, maxClosureDist, clippingEndPoints, iterations, clippingFactor,
                                r, c, rd, sP, eP, pO);

    row = r;
    column = c;
    radius = rd;
    startPhi = sP;
    endPhi = eP;
    pointOrder = pO;
    return;
};

void HXLDCont::HwFitCircleContourXld(const std::string& algorithm,
                                     Hlong maxNumPoints,
                                     double maxClosureDist,
                                     Hlong clippingEndPoints,
                                     Hlong iterations,
                                     double clippingFactor,
                                     double& row,
                                     double& column,
                                     double& radius,
                                     double& startPhi,
                                     double& endPhi,
                                     std::string& pointOrder) const
{
    if (!this->m_objects.size()) {
        throw LINE_RT_ERROR("object is empty");
    }
    if (!allowedModes.count(algorithm)) {
        throw LINE_RT_ERROR("non supported algorithm is requested");
    }

    if (!(maxNumPoints == -1 || maxNumPoints >= 8)) {
        throw LINE_RT_ERROR("maxNumPoints should be >=3 or equal to -1");
    }

    // if (maxClosureDist < 0) {
    //    throw LINE_RT_ERROR("maxClosureDist should be larger or equal zero");
    //}

    // if (this->m_objects.size() - clippingEndPoints * 2 < 3) {
    //    throw LINE_RT_ERROR("not enough valid points for fitting the model");
    //}

    if (clippingEndPoints < 0) {
        throw LINE_RT_ERROR("clippingEndPoints should be larger or equal zero");
    }

    if (iterations < 0) {
        throw LINE_RT_ERROR("iterations should be larger or equal zero");
    }

    // if (clippingFactor <= 0) {
    //    throw LINE_RT_ERROR("clippingFactor should be larger than zero");
    //}

    if ((int)this->m_objects.size() - clippingEndPoints * 2 <= 0)
        throw LINE_RT_ERROR(" Not enough valid points for fitting the model");

    HTuple rows, cols;
    HwGetContourXld(rows, cols);
    if (rows.Length() < 3) {
        throw std::runtime_error("Not enough points to fit a circle. Minimum 3 points are required.");
    }

    std::vector<cv::Point2f> points;
    for (int i = clippingEndPoints; i < rows.Length() - clippingEndPoints; ++i) {
        points.emplace_back((cols[i].D()), (rows[i].D()));
    }

    circleParams result;
    if (algorithm == "algebraic") {
        result = fitContourAlgebraic(points);
    } else if (algorithm == "ahuber") {
        result = fitCircleAHuber(points, maxClosureDist, clippingEndPoints, iterations, clippingFactor);
    } else if (algorithm == "atukey") {
        result = fitCircleATukey(points, maxNumPoints, maxClosureDist, clippingEndPoints, iterations, clippingFactor);
    } else if (algorithm == "geometric") {
        result =
                fitCircleGeometric(points, maxNumPoints, maxClosureDist, clippingEndPoints, iterations, clippingFactor);
    } else if (algorithm == "geohuber") {
        result = fitCircleGeoHuber(points, maxNumPoints, maxClosureDist, clippingEndPoints, clippingFactor);
    } else if (algorithm == "geotukey") {
        result = fitCircleGeoTukey(points, maxNumPoints, maxClosureDist, clippingEndPoints, clippingFactor);
    }

    startPhi = result.startPhi;
    endPhi = result.endPhi;
    column = result.centerCol;
    row = result.centerRow;
    radius = result.radius;
    pointOrder = result.pointOrder;

    return;
};

void HwFitCircleContourXld(const HXLDCont& contours,
                           const HTuple& algorithm,
                           const HTuple& maxNumPoints,
                           const HTuple& maxClosureDist,
                           const HTuple& clippingEndPoints,
                           const HTuple& iterations,
                           const HTuple& clippingFactor,
                           HTuple& row,
                           HTuple& column,
                           HTuple& radius,
                           HTuple& startPhi,
                           HTuple& endPhi,
                           HTuple& pointOrder)
{
    if (!maxNumPoints.isNumericalOnly() || !maxClosureDist.isNumericalOnly() || !clippingEndPoints.isNumericalOnly() ||
        !iterations.isNumericalOnly() || !clippingFactor.isNumericalOnly()) {
        throw LINE_RT_ERROR("one of the arguments is NaN");
    }

    if (algorithm.Length() != 1 || maxNumPoints.Length() != 1 || maxClosureDist.Length() != 1 ||
        clippingEndPoints.Length() != 1 || iterations.Length() != 1 || clippingFactor.Length() != 1) {
        throw LINE_RT_ERROR("one of the arguments is empty");
    }

    contours.HwFitCircleContourXld(algorithm.S(), maxNumPoints.I(), maxClosureDist.D(), clippingEndPoints.I(),
                                   iterations.I(), clippingFactor.D(), row, column, radius, startPhi, endPhi,
                                   pointOrder);
    return;
};

}  // namespace Huawei
Editor is loading...
Leave a Comment