diff --git a/src/libslic3r/Geometry/Circle.cpp b/src/libslic3r/Geometry/Circle.cpp index cdaf72d6a8..ff36b19584 100644 --- a/src/libslic3r/Geometry/Circle.cpp +++ b/src/libslic3r/Geometry/Circle.cpp @@ -146,4 +146,83 @@ Circled circle_ransac(const Vec2ds& input, size_t iterations, double* min_error) return circle_best; } +template +Circled circle_least_squares_by_solver(const Vec2ds &input, Solver solver) +{ + Circled out; + if (input.size() < 3) { + out = Circled::make_invalid(); + } else { + Eigen::Matrix A(input.size(), 3); + Eigen::VectorXd b(input.size()); + for (size_t r = 0; r < input.size(); ++ r) { + const Vec2d &p = input[r]; + A.row(r) = Vec3d(2. * p.x(), 2. * p.y(), - 1.); + b(r) = p.squaredNorm(); + } + auto result = solver(A, b); + out.center = result.head<2>(); + double r2 = out.center.squaredNorm() - result(2); + if (r2 <= EPSILON) + out.make_invalid(); + else + out.radius = sqrt(r2); + } + + return out; +} + +Circled circle_least_squares_svd(const Vec2ds &input) +{ + return circle_least_squares_by_solver(input, + [](const Eigen::Matrix &A, const Eigen::VectorXd &b) + { return A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b).eval(); }); +} + +Circled circle_least_squares_qr(const Vec2ds &input) +{ + return circle_least_squares_by_solver(input, + [](const Eigen::Matrix &A, const Eigen::VectorXd &b) + { return A.colPivHouseholderQr().solve(b).eval(); }); +} + +Circled circle_least_squares_normal(const Vec2ds &input) +{ + Circled out; + if (input.size() < 3) { + out = Circled::make_invalid(); + } else { + Eigen::Matrix A = Eigen::Matrix::Zero(); + Eigen::Matrix b = Eigen::Matrix::Zero(); + for (size_t i = 0; i < input.size(); ++ i) { + const Vec2d &p = input[i]; + // Calculate right hand side of a normal equation. + b += p.squaredNorm() * Vec3d(2. * p.x(), 2. * p.y(), -1.); + // Calculate normal matrix (correlation matrix). + // Diagonal: + A(0, 0) += 4. * p.x() * p.x(); + A(1, 1) += 4. * p.y() * p.y(); + A(2, 2) += 1.; + // Off diagonal elements: + const double a = 4. * p.x() * p.y(); + A(0, 1) += a; + A(1, 0) += a; + const double b = -2. * p.x(); + A(0, 2) += b; + A(2, 0) += b; + const double c = -2. * p.y(); + A(1, 2) += c; + A(2, 1) += c; + } + auto result = A.ldlt().solve(b).eval(); + out.center = result.head<2>(); + double r2 = out.center.squaredNorm() - result(2); + if (r2 <= EPSILON) + out.make_invalid(); + else + out.radius = sqrt(r2); + } + return out; +} + } } // namespace Slic3r::Geometry diff --git a/src/libslic3r/Geometry/Circle.hpp b/src/libslic3r/Geometry/Circle.hpp index f4bca148d7..ca32d32b50 100644 --- a/src/libslic3r/Geometry/Circle.hpp +++ b/src/libslic3r/Geometry/Circle.hpp @@ -141,6 +141,13 @@ Circled circle_taubin_newton(const Vec2ds& input, size_t cycles = 20); // Find circle using RANSAC randomized algorithm. Circled circle_ransac(const Vec2ds& input, size_t iterations = 20, double* min_error = nullptr); +// Least squares fitting with SVD. Most accurate, but slowest. +Circled circle_least_squares_svd(const Vec2ds &input); +// Least squares fitting with QR decomposition. Medium accuracy, medium speed. +Circled circle_least_squares_qr(const Vec2ds &input); +// Least squares fitting solving normal equations. Low accuracy, high speed. +Circled circle_least_squares_normal(const Vec2ds &input); + // Randomized algorithm by Emo Welzl, working with squared radii for efficiency. The returned circle radius is inflated by epsilon. template CircleSq smallest_enclosing_circle2_welzl(const Points &points, const typename Vector::Scalar epsilon)