Least squares circle fitting.

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<typename Solver>
+Circled circle_least_squares_by_solver(const Vec2ds &input, Solver solver)
+{
+    Circled out;
+    if (input.size() < 3) {
+        out = Circled::make_invalid();
+    } else {
+        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic /* 3 */> 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<double, Eigen::Dynamic, Eigen::Dynamic /* 3 */> &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<double, Eigen::Dynamic, Eigen::Dynamic> &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<double, 3, 3> A = Eigen::Matrix<double, 3, 3>::Zero();
+        Eigen::Matrix<double, 3, 1> b = Eigen::Matrix<double, 3, 1>::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

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<typename Vector, typename Points>
 CircleSq<Vector> smallest_enclosing_circle2_welzl(const Points &points, const typename Vector::Scalar epsilon)