ArcWelder:

Reducing the chance of creating segments shorter than G-code quantization
distance.
Improving fitting by non-linear least squares.

src/libslic3r/GCode.cpp

@@ -2641,6 +2641,8 @@ static inline bool validate_smooth_path(const GCode::SmoothPath &smooth_path, bo
 }
 #endif //NDEBUG
 
+static constexpr const double min_gcode_segment_length = 0.002;
+
 std::string GCodeGenerator::extrude_loop(const ExtrusionLoop &loop_src, const GCode::SmoothPathCache &smooth_path_cache, const std::string_view description, double speed)
 {
     // Extrude all loops CCW.
@@ -2659,7 +2661,7 @@ std::string GCodeGenerator::extrude_loop(const ExtrusionLoop &loop_src, const GC
     // if polyline was shorter than the clipping distance we'd get a null polyline, so
     // we discard it in that case.
     if (m_enable_loop_clipping)
-        clip_end(smooth_path, scale_(EXTRUDER_CONFIG(nozzle_diameter)) * LOOP_CLIPPING_LENGTH_OVER_NOZZLE_DIAMETER);
+        clip_end(smooth_path, scaled<double>(EXTRUDER_CONFIG(nozzle_diameter)) * LOOP_CLIPPING_LENGTH_OVER_NOZZLE_DIAMETER, scaled<double>(min_gcode_segment_length));
 
     if (smooth_path.empty())
         return {};
@@ -2704,7 +2706,7 @@ std::string GCodeGenerator::extrude_skirt(
     // if polyline was shorter than the clipping distance we'd get a null polyline, so
     // we discard it in that case.
     if (m_enable_loop_clipping)
-        clip_end(smooth_path, scale_(EXTRUDER_CONFIG(nozzle_diameter)) * LOOP_CLIPPING_LENGTH_OVER_NOZZLE_DIAMETER);
+        clip_end(smooth_path, scale_(EXTRUDER_CONFIG(nozzle_diameter)) * LOOP_CLIPPING_LENGTH_OVER_NOZZLE_DIAMETER, scaled<double>(min_gcode_segment_length));
 
     if (smooth_path.empty())
         return {};
@@ -3065,12 +3067,14 @@ std::string GCodeGenerator::_extrude(
         comment = description;
         comment += description_bridge;
     }
-    Vec2d prev = this->point_to_gcode_quantized(path.front().point);
+    Vec2d prev_exact = this->point_to_gcode(path.front().point);
+    Vec2d prev = GCodeFormatter::quantize(prev_exact);
     auto  it   = path.begin();
     auto  end  = path.end();
     const bool emit_radius = m_config.arc_fitting == ArcFittingType::EmitRadius;
     for (++ it; it != end; ++ it) {
-        Vec2d p = this->point_to_gcode_quantized(it->point);
+        Vec2d p_exact = this->point_to_gcode(it->point);
+        Vec2d p = GCodeFormatter::quantize(p_exact);
         assert(p != prev);
         if (p != prev) {
             // Center of the radius to be emitted into the G-code: Either by radius or by center offset.
@@ -3083,11 +3087,12 @@ std::string GCodeGenerator::_extrude(
                 radius = unscaled<double>(it->radius);
                 if (emit_radius) {
                     // Only quantize radius if emitting it directly into G-code. Otherwise use the exact radius for calculating the IJ values.
+                    //FIXME rather re-fit the arc to improve accuracy!
                     radius = GCodeFormatter::quantize_xyzf(radius);
                 } else {
                     // Calculate quantized IJ circle center offset.
                     ij = GCodeFormatter::quantize(Vec2d(
-                            Geometry::ArcWelder::arc_center(prev.cast<double>(), p.cast<double>(), double(radius), it->ccw())
+                            Geometry::ArcWelder::arc_center(prev_exact.cast<double>(), p_exact.cast<double>(), double(radius), it->ccw())
                             - prev));
                     if (ij == Vec2d::Zero())
                         // Don't extrude a degenerated circle.
@@ -3112,6 +3117,7 @@ std::string GCodeGenerator::_extrude(
                     m_writer.extrude_to_xy_G2G3IJ(p, ij, it->ccw(), dE, comment);
             }
             prev = p;
+            prev_exact = p_exact;
         }
     }
 

src/libslic3r/GCode/SmoothPath.cpp

@@ -102,7 +102,10 @@ std::optional<Point> sample_path_point_at_distance_from_end(const SmoothPath &pa
     return {};
 }
 
-double clip_end(SmoothPath &path, double distance)
+// Clip length of a smooth path, for seam hiding.
+// When clipping the end of a path, don't create segments shorter than min_point_distance_threshold, 
+// rather discard such a degenerate segment.
+double clip_end(SmoothPath &path, double distance, double min_point_distance_threshold)
 {
     while (! path.empty() && distance > 0) {
         Geometry::ArcWelder::Path &p = path.back().path;
@@ -111,9 +114,18 @@ double clip_end(SmoothPath &path, double distance)
             path.pop_back();
         } else {
             // Trailing path was trimmed and it is valid.
-            assert(path.back().path.size() > 1);
+            Geometry::ArcWelder::Path &last_path = path.back().path;
+            assert(last_path.size() > 1);
             assert(distance == 0);
             // Distance to go is zero.
+            // Remove the last segment if its length is shorter than min_point_distance_threshold.
+            const Geometry::ArcWelder::Segment &prev_segment = last_path[last_path.size() - 2];
+            const Geometry::ArcWelder::Segment &last_segment = last_path.back();
+            if (Geometry::ArcWelder::segment_length<double>(prev_segment, last_segment) < min_point_distance_threshold) {
+                last_path.pop_back();
+                if (last_path.size() < 2)
+                    path.pop_back();
+            }
             return 0;
         }
     }

src/libslic3r/GCode/SmoothPath.hpp

@@ -29,7 +29,9 @@ std::optional<Point> sample_path_point_at_distance_from_start(const SmoothPath &
 std::optional<Point> sample_path_point_at_distance_from_end(const SmoothPath &path, double distance);
 
 // Clip end of a smooth path, for seam hiding.
-double clip_end(SmoothPath &path, double distance);
+// When clipping the end of a path, don't create segments shorter than min_point_distance_threshold, 
+// rather discard such a degenerate segment.
+double clip_end(SmoothPath &path, double distance, double min_point_distance_threshold);
 
 class SmoothPathCache
 {

src/libslic3r/Geometry/ArcWelder.cpp

@@ -148,36 +148,205 @@ static inline bool get_deviation_sum_squared(const Circle &circle, const Points:
     return true;
 }
 
+double arc_fit_variance(const Point &start_pos, const Point &end_pos, const float radius, bool is_ccw,
+    const Points::const_iterator begin, const Points::const_iterator end)
+{
+    const Vec2d  center = arc_center(start_pos.cast<double>(), end_pos.cast<double>(), double(radius), is_ccw);
+    const double r      = std::abs(radius);
+
+    // The circle was calculated from the 1st and last point of the point sequence, thus the fitting of those points does not need to be evaluated.
+    assert(std::abs((begin->cast<double>() - center).norm() - r) < SCALED_EPSILON);
+    assert(std::abs((std::prev(end)->cast<double>() - center).norm() - r) < SCALED_EPSILON);
+    assert(end - begin >= 3);
+
+    double total_deviation = 0;
+    size_t cnt = 0;
+    for (auto it = begin; std::next(it) != end; ++ it) {
+        if (it != begin) {
+            total_deviation += sqr((it->cast<double>() - center).norm() - r);
+            ++ cnt;
+        }
+        Point closest_point;
+        if (foot_pt_on_segment(*it, *std::next(it), center.cast<coord_t>(), closest_point)) {
+            total_deviation += sqr((closest_point.cast<double>() - center).cast<double>().norm() - r);
+            ++ cnt;
+        }
+    }
+
+    return total_deviation / double(cnt);
+}
+
+double arc_fit_max_deviation(const Point &start_pos, const Point &end_pos, const float radius, bool is_ccw,
+    const Points::const_iterator begin, const Points::const_iterator end)
+{
+    const Vec2d  center = arc_center(start_pos.cast<double>(), end_pos.cast<double>(), double(radius), is_ccw);
+    const double r      = std::abs(radius);
+
+    // The circle was calculated from the 1st and last point of the point sequence, thus the fitting of those points does not need to be evaluated.
+    assert(std::abs((begin->cast<double>() - center).norm() - r) < SCALED_EPSILON);
+    assert(std::abs((std::prev(end)->cast<double>() - center).norm() - r) < SCALED_EPSILON);
+    assert(end - begin >= 3);
+
+    double max_deviation        = 0;
+    double max_signed_deviation = 0;
+    for (auto it = begin; std::next(it) != end; ++ it) {
+        if (it != begin) {
+            double signed_deviation = (it->cast<double>() - center).norm() - r;
+            double deviation = std::abs(signed_deviation);
+            if (deviation > max_deviation) {
+                max_deviation = deviation;
+                max_signed_deviation = signed_deviation;
+            }
+        }
+        Point closest_point;
+        if (foot_pt_on_segment(*it, *std::next(it), center.cast<coord_t>(), closest_point)) {
+            double signed_deviation = (closest_point.cast<double>() - center).cast<double>().norm() - r;
+            double deviation = std::abs(signed_deviation);
+            if (deviation > max_deviation) {
+                max_deviation = deviation;
+                max_signed_deviation = signed_deviation;
+            }
+        }
+    }
+    return max_signed_deviation;
+}
+
+static inline int sign(const int64_t i)
+{
+    return i > 0 ? 1 : i < 0 ? -1 : 0;
+}
+
 static std::optional<Circle> try_create_circle(const Points::const_iterator begin, const Points::const_iterator end, const double max_radius, const double tolerance)
 {
     std::optional<Circle> out;
     size_t size = end - begin;
     if (size == 3) {
+        // Fit the circle throuh the three input points.
         out = try_create_circle(*begin, *std::next(begin), *std::prev(end), max_radius);
-        if (out && ! circle_approximation_sufficient(*out, begin, end, tolerance))
-            out.reset();
-    } else {
-#if 0
-        size_t ipivot = size / 2;
-        // Take a center difference of points at the center of the path.
-        //FIXME does it really help? For short arches, the linear interpolation may be 
-        Point pivot = (size % 2 == 0) ? (*(begin + ipivot) + *(begin + ipivot - 1)) / 2 :
-            (*(begin + ipivot - 1) + *(begin + ipivot + 1)) / 2;
-        if (std::optional<Circle> circle = try_create_circle(*begin, pivot, *std::prev(end), max_radius);
-            circle && circle_approximation_sufficient(*circle, begin, end, tolerance))
-            return circle;
-#endif
-        // Find the circle with the least deviation, if one exists.
-        double least_deviation = std::numeric_limits<double>::max();
-        double current_deviation;
-        for (auto it = std::next(begin); std::next(it) != end; ++ it)
-            if (std::optional<Circle> circle = try_create_circle(*begin, *it, *std::prev(end), max_radius); 
-                circle && get_deviation_sum_squared(*circle, begin, end, tolerance, current_deviation)) {
-                if (current_deviation < least_deviation) {
+        if (out) {
+            // Fit the center point and the two center points of the two edges with non-linear least squares.
+            std::array<Vec2d, 3> fpts;
+            Vec2d center_point = out->center.cast<double>();
+            Vec2d first_point  = begin->cast<double>();
+            Vec2d mid_point    = std::next(begin)->cast<double>();
+            Vec2d last_point   = std::prev(end)->cast<double>();
+            fpts[0] = 0.5 * (first_point + mid_point);
+            fpts[1] = mid_point;
+            fpts[2] = 0.5 * (mid_point + last_point);
+            const double radius = (first_point - center_point).norm();
+            if (std::abs((fpts[0] - center_point).norm() - radius) < 2. * tolerance &&
+                std::abs((fpts[2] - center_point).norm() - radius) < 2. * tolerance) {
+                if (std::optional<Vec2d> opt_center = ArcWelder::arc_fit_center_gauss_newton_ls(first_point, last_point,
+                        center_point, fpts.begin(), fpts.end(), 3);
+                    opt_center) {
+                    out->center = opt_center->cast<coord_t>();
+                    out->radius = (out->radius > 0 ? 1.f : -1.f) * (*opt_center - first_point).norm();
+                }
+                if (! circle_approximation_sufficient(*out, begin, end, tolerance))
+                    out.reset();
+            } else
+                out.reset();
+        }
+    } else {        
+        std::optional<Circle> circle;
+        {
+            // Try to fit a circle to first, middle and last point.
+            auto mid = begin + (end - begin) / 2;    
+            circle = try_create_circle(*begin, *mid, *std::prev(end), max_radius);
+            if (// Use twice the tolerance for fitting the initial circle.
+                // Early exit if such approximation is grossly inaccurate, thus the tolerance could not be achieved.
+                circle && ! circle_approximation_sufficient(*circle, begin, end, tolerance * 2))
+                circle.reset();
+        } 
+        if (! circle) {
+            // Find an intersection point of the polyline to be fitted with the bisector of the arc chord.
+            // At such a point the distance of a polyline to an arc wrt. the circle center (or circle radius) will have a largest gradient
+            // of all points on the polyline to be fitted.
+            Vec2i64 first_point = begin->cast<int64_t>();
+            Vec2i64 last_point  = std::prev(end)->cast<int64_t>();
+            Vec2i64 c = (first_point.cast<int64_t>() + last_point.cast<int64_t>()) / 2;
+            Vec2i64 v = last_point - first_point;
+            Vec2i64 prev_point = first_point;
+            int     prev_side = sign(v.dot(prev_point - c));
+            assert(prev_side != 0);
+            Point   point_on_bisector;
+#ifndef NDEBUG
+            point_on_bisector = { std::numeric_limits<coord_t>::max(), std::numeric_limits<coord_t>::max() };
+#endif // NDEBUG
+            for (auto it = std::next(begin); it != end; ++ it) {
+                Vec2i64 this_point = it->cast<int64_t>();
+                int64_t d         = v.dot(this_point - c);
+                int     this_side = sign(d);
+                int     sideness  = this_side * prev_side;
+                if (sideness < 0) {
+                    // Calculate the intersection point.
+                    Vec2d vd = v.cast<double>();
+                    Vec2d p = c.cast<double>() + vd * double(d) / vd.squaredNorm();
+                    point_on_bisector = p.cast<coord_t>();
+                    break;
+                } 
+                if (sideness == 0) {
+                    // this_point is on the bisector.
+                    assert(prev_side != 0);
+                    assert(this_side == 0);
+                    point_on_bisector = this_point.cast<coord_t>();
+                    break;
+                }
+                prev_point = this_point;
+                prev_side  = this_side;
+            }
+            // point_on_bisector must be set
+            assert(point_on_bisector.x() != std::numeric_limits<coord_t>::max() && point_on_bisector.y() != std::numeric_limits<coord_t>::max());
+            circle = try_create_circle(*begin, point_on_bisector, *std::prev(end), max_radius);
+            if (// Use twice the tolerance for fitting the initial circle.
+                // Early exit if such approximation is grossly inaccurate, thus the tolerance could not be achieved.
+                circle && ! circle_approximation_sufficient(*circle, begin, end, tolerance * 2))
+                circle.reset();
+        }
+        if (circle) {
+            // Fit the arc between the end points by least squares.
+            // Optimize over all points along the path and the centers of the segments.
+            std::vector<Vec2d> fpts;
+            Vec2d first_point = begin->cast<double>();
+            Vec2d last_point  = std::prev(end)->cast<double>();
+            Vec2d prev_point  = first_point;            
+            for (auto it = std::next(begin); it != std::prev(end); ++ it) {
+                Vec2d this_point = it->cast<double>();
+                fpts.emplace_back(0.5 * (prev_point + this_point));
+                fpts.emplace_back(this_point);
+                prev_point = this_point;
+            }
+            fpts.emplace_back(0.5 * (prev_point + last_point));
+            std::optional<Vec2d> opt_center = ArcWelder::arc_fit_center_gauss_newton_ls(first_point, last_point,
+                circle->center.cast<double>(), fpts.begin(), fpts.end(), 5);
+            if (opt_center) {
+                circle->center = opt_center->cast<coord_t>();
+                circle->radius = (circle->radius > 0 ? 1.f : -1.f) * (*opt_center - first_point).norm();
+                if (circle_approximation_sufficient(*circle, begin, end, tolerance)) {
                     out = circle;
-                    least_deviation = current_deviation;
+                } else {
+                    //FIXME One may consider adjusting the arc to fit the worst offender as a last effort,
+                    // however Vojtech is not sure whether it is worth it.
                 }
             }
+        }
+/*
+        // From the original arc welder.
+        // Such a loop makes the time complexity of the arc fitting an ugly O(n^3).
+        else {
+            // Find the circle with the least deviation, if one exists.
+            double least_deviation = std::numeric_limits<double>::max();
+            double current_deviation;
+            for (auto it = std::next(begin); std::next(it) != end; ++ it)
+                if (std::optional<Circle> circle = try_create_circle(*begin, *it, *std::prev(end), max_radius); 
+                    circle && get_deviation_sum_squared(*circle, begin, end, tolerance, current_deviation)) {
+                    if (current_deviation < least_deviation) {
+                        out = circle;
+                        least_deviation = current_deviation;
+                    }
+                }
+        }
+*/
     }
     return out;
 }
@@ -192,11 +361,6 @@ public:
     Orientation direction { Orientation::Unknown };
 };
 
-static inline int sign(const int64_t i)
-{
-    return i > 0 ? 1 : i < 0 ? -1 : 0;
-}
-
 // Return orientation of a polyline with regard to the center.
 // Successive points are expected to take less than a PI angle step.
 Orientation arc_orientation(
@@ -317,21 +481,25 @@ static inline Segments::iterator douglas_peucker_in_place(Segments::iterator beg
     return douglas_peucker<int64_t>(begin, end, begin, tolerance, [](const Segment &s) { return s.point; });
 }
 
-Path fit_path(const Points &src, double tolerance, double fit_circle_percent_tolerance)
+Path fit_path(const Points &src_in, double tolerance, double fit_circle_percent_tolerance)
 {
     assert(tolerance >= 0);
     assert(fit_circle_percent_tolerance >= 0);
+    double tolerance2 = Slic3r::sqr(tolerance);
 
     Path out;
-    out.reserve(src.size());
-    if (tolerance <= 0 || src.size() <= 2) {
+    out.reserve(src_in.size());
+    if (tolerance <= 0 || src_in.size() <= 2) {
         // No simplification, just convert.
-        std::transform(src.begin(), src.end(), std::back_inserter(out), [](const Point &p) -> Segment { return { p }; });
-    } else if (fit_circle_percent_tolerance <= 0) {
+        std::transform(src_in.begin(), src_in.end(), std::back_inserter(out), [](const Point &p) -> Segment { return { p }; });
+    } else if (double tolerance_fine = std::max(0.03 * tolerance, scaled<double>(0.000060)); 
+        fit_circle_percent_tolerance <= 0 || tolerance_fine > 0.5 * tolerance) {
         // Convert and simplify to a polyline.
-        std::transform(src.begin(), src.end(), std::back_inserter(out), [](const Point &p) -> Segment { return { p }; });
+        std::transform(src_in.begin(), src_in.end(), std::back_inserter(out), [](const Point &p) -> Segment { return { p }; });
         out.erase(douglas_peucker_in_place(out.begin(), out.end(), tolerance), out.end());
     } else {
+        // Simplify the polyline first using a fine threshold.
+        Points src = douglas_peucker(src_in, tolerance_fine);
         // Perform simplification & fitting.
         // Index of the start of a last polyline, which has not yet been decimated.
         int begin_pl_idx = 0;
@@ -349,13 +517,89 @@ Path fit_path(const Points &src, double tolerance, double fit_circle_percent_tol
                                                         ArcWelder::default_scaled_max_radius,
                                                         tolerance, fit_circle_percent_tolerance);
                     this_arc) {
+                    // Could extend the arc by one point.
                     assert(this_arc->direction != Orientation::Unknown);
                     arc = this_arc;
                     end = next_end;
-                } else
+                    if (end == src.end())
+                        // No way to extend the arc.
+                        goto fit_end;
+                    // Now try to expand the arc by adding points one by one. That should be cheaper than a full arc fit test.
+                    while (std::next(end) != src.end()) {
+                        assert(end == next_end);
+                        {
+                            Vec2i64 v1 = arc->start_point.cast<int64_t>() - arc->center.cast<int64_t>();
+                            Vec2i64 v2 = arc->end_point.cast<int64_t>() - arc->center.cast<int64_t>();
+                            do {
+                                if (std::abs((arc->center.cast<double>() - next_end->cast<double>()).norm() - arc->radius) >= tolerance ||
+                                    inside_arc_wedge_vectors(v1, v2,
+                                        arc->radius > 0, arc->direction == Orientation::CCW,
+                                        next_end->cast<int64_t>() - arc->center.cast<int64_t>()))
+                                    // Cannot extend the current arc with this new point.
+                                    break;
+                            } while (++ next_end != src.end());
+                        }
+                        if (next_end == end)
+                            // No additional point could be added to a current arc.
+                            break;
+                        // Try to fit a new arc to the extended set of points.
+                        // last_tested_failed set to invalid value, no test failed yet.
+                        auto last_tested_failed = src.begin();
+                        for (;;) {
+                            this_arc = try_create_arc(
+                                begin, next_end,
+                                ArcWelder::default_scaled_max_radius,
+                                tolerance, fit_circle_percent_tolerance);
+                            if (this_arc) {
+                                arc = this_arc;
+                                end = next_end;
+                                if (last_tested_failed == src.begin()) {
+                                    // First run of the loop, the arc was extended fully.
+                                    if (end == src.end())
+                                        goto fit_end;
+                                    // Otherwise try to extend the arc with another sample.
+                                    break;
+                                }
+                            } else
+                                last_tested_failed = next_end;
+                            // Take half of the interval up to the failed point.
+                            next_end = end + (last_tested_failed - end) / 2;
+                            if (next_end == end)
+                                // Backed to the last successfull sample.
+                                goto fit_end;
+                            // Otherwise try to extend the arc up to next_end in another iteration.
+                        }
+                    }
+                } else {
+                    // The last arc was the best we could get.
                     break;
+                }
             }
+        fit_end:
+#if 1
             if (arc) {
+                // Check whether the arc end points are not too close with the risk of quantizing the arc ends to the same point on G-code export.
+                if ((arc->end_point - arc->start_point).cast<double>().squaredNorm() < 2. * sqr(scaled<double>(0.0011))) {
+                    // Arc is too short. Skip it, decimate a polyline instead.
+                    arc.reset();
+                } else {
+                    // Test whether the arc is so flat, that it could be replaced with a straight segment.
+                    Line line(arc->start_point, arc->end_point);
+                    bool arc_valid = false;
+                    for (auto it2 = std::next(begin); it2 != std::prev(end); ++ it2)
+                        if (line_alg::distance_to_squared(line, *it2) > tolerance2) {
+                            // Polyline could not be fitted by a line segment, thus the arc is considered valid.
+                            arc_valid = true;
+                            break;
+                        }
+                    if (! arc_valid)
+                        // Arc should be fitted by a line segment. Skip it, decimate a polyline instead.
+                        arc.reset();
+                }
+            }
+#endif
+            if (arc) {
+                // printf("Arc radius: %lf, length: %lf\n", unscaled<double>(arc->radius), arc_length(arc->start_point.cast<double>(), arc->end_point.cast<double>(), arc->radius));
                 // If there is a trailing polyline, decimate it first before saving a new arc.
                 if (out.size() - begin_pl_idx > 2) {
                     // Decimating linear segmens only.
@@ -363,6 +607,14 @@ Path fit_path(const Points &src, double tolerance, double fit_circle_percent_tol
                     out.erase(douglas_peucker_in_place(out.begin() + begin_pl_idx, out.end(), tolerance), out.end());
                     assert(out.back().linear());
                 }
+#ifndef NDEBUG
+                // Check for a very short linear segment, that connects two arches. Such segment should not be created.
+                if (out.size() - begin_pl_idx > 1) {
+                    const Point& p1 = out[begin_pl_idx].point;
+                    const Point& p2 = out.back().point;
+                    assert((p2 - p1).cast<double>().squaredNorm() > sqr(scaled<double>(0.0011)));
+                }
+#endif
                 // Save the index of an end of the new circle segment, which may become the first point of a possible future polyline.
                 begin_pl_idx = int(out.size());
                 // This will be the next point to try to add.
@@ -413,7 +665,7 @@ Path fit_path(const Points &src, double tolerance, double fit_circle_percent_tol
 
 #if 0
     // Verify that all the source points are at tolerance distance from the interpolated path.
-    for (auto it = std::next(src.begin()); it != src.end(); ++ it) {
+    for (auto it = std::next(src_in.begin()); it != src_in.end(); ++ it) {
         Point  start = *std::prev(it);
         Point  end   = *it;
         Vec2d  v     = (end - start).cast<double>();

src/libslic3r/Geometry/ArcWelder.hpp

@@ -364,6 +364,16 @@ size_t arc_discretization_steps(const FloatType radius, const FloatType angle, c
 // Returned polygon starts with p1, ends with p2 and it is discretized to guarantee the maximum deviation.
 Points arc_discretize(const Point &p1, const Point &p2, const double radius, const bool ccw, const double deviation);
 
+// Variance of the arc fit of points <begin, end).
+// First and last points of <begin, end) are expected to fit the arc exactly.
+double arc_fit_variance(const Point &start_point, const Point &end_point, const float radius, bool is_ccw,
+    const Points::const_iterator begin, const Points::const_iterator end);
+
+// Maximum signed deviation of points <begin, end) (L1) from the arc.
+// First and last points of <begin, end) are expected to fit the arc exactly.
+double arc_fit_max_deviation(const Point &start_point, const Point &end_point, const float radius, bool is_ccw,
+    const Points::const_iterator begin, const Points::const_iterator end);
+
 // 1.2m diameter, maximum given by coord_t
 static constexpr const double default_scaled_max_radius = scaled<double>(600.);
 // 0.05mm

src/libslic3r/Geometry/Circle.cpp

@@ -147,7 +147,7 @@ Circled circle_ransac(const Vec2ds& input, size_t iterations, double* min_error)
 }
 
 template<typename Solver>
-Circled circle_least_squares_by_solver(const Vec2ds &input, Solver solver)
+Circled circle_linear_least_squares_by_solver(const Vec2ds &input, Solver solver)
 {
     Circled out;
     if (input.size() < 3) {
@@ -172,21 +172,21 @@ Circled circle_least_squares_by_solver(const Vec2ds &input, Solver solver)
     return out;
 }
 
-Circled circle_least_squares_svd(const Vec2ds &input)
+Circled circle_linear_least_squares_svd(const Vec2ds &input)
 {
-    return circle_least_squares_by_solver(input, 
+    return circle_linear_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)
+Circled circle_linear_least_squares_qr(const Vec2ds &input)
 {
-    return circle_least_squares_by_solver(input, 
+    return circle_linear_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 circle_linear_least_squares_normal(const Vec2ds &input)
 {
     Circled out;
     if (input.size() < 3) {

src/libslic3r/Geometry/Circle.hpp

@@ -141,12 +141,17 @@ 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);
+// Linear Least squares fitting.
+// Be careful! The linear least squares fitting is strongly biased towards small circles,
+// thus the method is only recommended for circles or arches with large arc angle.
+// Also it is strongly recommended to center the input at an expected circle (or arc) center
+// to minimize the small circle bias!
+    // Linear Least squares fitting with SVD. Most accurate, but slowest.
+    Circled circle_linear_least_squares_svd(const Vec2ds &input);
+    // Linear Least squares fitting with QR decomposition. Medium accuracy, medium speed.
+    Circled circle_linear_least_squares_qr(const Vec2ds &input);
+    // Linear Least squares fitting solving normal equations. Low accuracy, high speed.
+    Circled circle_linear_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>

tests/libslic3r/test_arc_welder.cpp

@@ -1,7 +1,11 @@
 #include <catch2/catch.hpp>
 #include <test_utils.hpp>
 
+#include <random>
+
 #include <libslic3r/Geometry/ArcWelder.hpp>
+#include <libslic3r/Geometry/Circle.hpp>
+#include <libslic3r/SVG.hpp>
 #include <libslic3r/libslic3r.h>
 
 using namespace Slic3r;
@@ -13,25 +17,34 @@ TEST_CASE("arc basics", "[ArcWelder]") {
         Vec2f p1{ 2000.f, 1000.f };
         Vec2f p2{ 1000.f, 2000.f };
         float r{ 1000.f };
+        const double s = 1000.f / sqrt(2.);
         THEN("90 degrees arc, CCW") {
             Vec2f c = ArcWelder::arc_center(p1, p2, r, true);
+            Vec2f m = ArcWelder::arc_middle_point(p1, p2, r, true);
             REQUIRE(is_approx(c, Vec2f{ 1000.f, 1000.f }));
             REQUIRE(ArcWelder::arc_angle(p1, p2, r) == Approx(0.5 * M_PI));
             REQUIRE(ArcWelder::arc_length(p1, p2, r) == Approx(r * 0.5 * M_PI).epsilon(0.001));
+            REQUIRE(is_approx(m, Vec2f{ 1000.f + s, 1000.f + s }, 0.001f));
         }
         THEN("90 degrees arc, CW") {
             Vec2f c = ArcWelder::arc_center(p1, p2, r, false);
+            Vec2f m = ArcWelder::arc_middle_point(p1, p2, r, false);
             REQUIRE(is_approx(c, Vec2f{ 2000.f, 2000.f }));
+            REQUIRE(is_approx(m, Vec2f{ 2000.f - s, 2000.f - s }, 0.001f));
         }
         THEN("270 degrees arc, CCW") {
             Vec2f c = ArcWelder::arc_center(p1, p2, - r, true);
+            Vec2f m = ArcWelder::arc_middle_point(p1, p2, - r, true);
             REQUIRE(is_approx(c, Vec2f{ 2000.f, 2000.f }));
             REQUIRE(ArcWelder::arc_angle(p1, p2, - r) == Approx(1.5 * M_PI));
             REQUIRE(ArcWelder::arc_length(p1, p2, - r) == Approx(r * 1.5 * M_PI).epsilon(0.001));
+            REQUIRE(is_approx(m, Vec2f{ 2000.f + s, 2000.f + s }, 0.001f));
         }
         THEN("270 degrees arc, CW") {
             Vec2f c = ArcWelder::arc_center(p1, p2, - r, false);
+            Vec2f m = ArcWelder::arc_middle_point(p1, p2, - r, false);
             REQUIRE(is_approx(c, Vec2f{ 1000.f, 1000.f }));
+            REQUIRE(is_approx(m, Vec2f{ 1000.f - s, 1000.f - s }, 0.001f));
         }
     }
     WHEN("arc from { 1707.11f, 1707.11f } to { 1000.f, 2000.f }") {
@@ -126,6 +139,28 @@ TEST_CASE("arc discretization", "[ArcWelder]") {
     }
 }
 
+void test_arc_fit_variance(const Point &p1, const Point &p2, const float r, const float r_fit, const bool ccw, const Points::const_iterator begin, const Points::const_iterator end)
+{
+    using namespace Slic3r::Geometry;
+    double variance          = ArcWelder::arc_fit_variance(p1, p2, r, ccw, begin, end);
+    double variance_fit      = ArcWelder::arc_fit_variance(p1, p2, r_fit, ccw, begin, end);
+    REQUIRE(variance_fit < variance);
+};
+
+void test_arc_fit_max_deviation(const Point &p1, const Point &p2, const float r, const float r_fit, const bool ccw, const Points::const_iterator begin, const Points::const_iterator end)
+{
+    using namespace Slic3r::Geometry;
+    double max_deviation     = ArcWelder::arc_fit_max_deviation(p1, p2, r, ccw, begin, end);
+    double max_deviation_fit = ArcWelder::arc_fit_max_deviation(p1, p2, r_fit, ccw, begin, end);
+    REQUIRE(std::abs(max_deviation_fit) < std::abs(max_deviation));
+};
+
+void test_arc_fit(const Point &p1, const Point &p2, const float r, const float r_fit, const bool ccw, const Points::const_iterator begin, const Points::const_iterator end)
+{
+    test_arc_fit_variance(p1, p2, r, r_fit, ccw, begin, end);
+    test_arc_fit_max_deviation(p1, p2, r, r_fit, ccw, begin, end);
+};
+
 TEST_CASE("arc fitting", "[ArcWelder]") {
     using namespace Slic3r::Geometry;
 
@@ -142,8 +177,8 @@ TEST_CASE("arc fitting", "[ArcWelder]") {
             REQUIRE(path.front().point == p1);
             REQUIRE(path.front().radius == 0.f);
             REQUIRE(path.back().point == p2);
-            REQUIRE(path.back().radius == Approx(r));
             REQUIRE(path.back().ccw() == ccw);
+            test_arc_fit(p1, p2, r, path.back().radius, true, pts.begin(), pts.end());
         };
         THEN("90 degrees arc, CCW is fitted") {
             test(p1, p2, radius, true);
@@ -169,6 +204,7 @@ TEST_CASE("arc fitting", "[ArcWelder]") {
         const float resolution = scaled<float>(0.002);
         auto test = [center1, center2, resolution](const Point &p1, const Point &p2, const Point &p3, const float r, const bool ccw) {
             Points pts = ArcWelder::arc_discretize(p1, p2, r, ccw, resolution);
+            size_t num_pts1 = pts.size();
             {
                 Points pts2 = ArcWelder::arc_discretize(p2, p3, - r, ! ccw, resolution);
                 REQUIRE(pts.back() == pts2.front());
@@ -179,11 +215,11 @@ TEST_CASE("arc fitting", "[ArcWelder]") {
             REQUIRE(path.front().point == p1);
             REQUIRE(path.front().radius == 0.f);
             REQUIRE(path[1].point == p2);
-            REQUIRE(path[1].radius == Approx(r));
             REQUIRE(path[1].ccw() == ccw);
             REQUIRE(path.back().point == p3);
-            REQUIRE(path.back().radius == Approx(- r));
             REQUIRE(path.back().ccw() == ! ccw);
+            test_arc_fit(p1, p2, r, path[1].radius, ccw, pts.begin(), pts.begin() + num_pts1);
+            test_arc_fit(p2, p3, - r, path.back().radius, ! ccw, pts.begin() + num_pts1 - 1, pts.end());
         };
         THEN("90 degrees arches, CCW are fitted") {
             test(p1, p2, p3, radius, true);
@@ -200,6 +236,106 @@ TEST_CASE("arc fitting", "[ArcWelder]") {
     }
 }
 
+TEST_CASE("least squares arc fitting, interpolating end points", "[ArcWelder]") {
+    using namespace Slic3r::Geometry;
+
+    // Generate bunch of random arches.
+    const coord_t          max_coordinate = scaled<coord_t>(sqrt(250. - 1.));
+    const double           min_radius     = scaled<double>(0.01);
+    const double           max_radius     = scaled<double>(250.);
+//    const double           deviation      = scaled<double>(0.5);
+    const double           deviation      = scaled<double>(0.1);
+    // Seeded with a fixed seed, to be repeatable.
+    std::mt19937                            rng(867092346);
+    std::uniform_int_distribution<int32_t>  coord_sampler(0, int32_t(max_coordinate));
+    std::uniform_real_distribution<double>  angle_sampler(0.001, 2. * M_PI - 0.001);
+    std::uniform_real_distribution<double>  radius_sampler(min_radius, max_radius);
+    std::uniform_int_distribution<int>      num_samples_sampler(1, 100);
+    auto test_arc_fitting = [&rng, &coord_sampler, &num_samples_sampler, &angle_sampler, &radius_sampler]() {
+        auto sample_point = [&rng, &coord_sampler]() {
+            return Vec2d(coord_sampler(rng), coord_sampler(rng));
+        };
+        // Start and end point of the arc:
+        Vec2d  center_pos = sample_point();
+        double angle0     = angle_sampler(rng);
+        double angle      = angle_sampler(rng);
+        double radius     = radius_sampler(rng);
+        Vec2d  v1         = Eigen::Rotation2D(angle0) * Vec2d(1., 0.);
+        Vec2d  v2         = Eigen::Rotation2D(angle0 + angle) * Vec2d(1., 0.);
+        Vec2d  start_pos  = center_pos + radius * v1;
+        Vec2d  end_pos    = center_pos + radius * v2;
+        std::vector<Vec2d> samples;
+        size_t num_samples = num_samples_sampler(rng);
+        for (size_t i = 0; i < num_samples; ++ i) {
+            double sample_r = sqrt(std::uniform_real_distribution<double>(sqr(std::max(0., radius - deviation)), sqr(radius + deviation))(rng));
+            double sample_a = std::uniform_real_distribution<double>(0., angle)(rng);
+            Vec2d  pt = center_pos + Eigen::Rotation2D(angle0 + sample_a) * Vec2d(sample_r, 0.);
+            samples.emplace_back(pt);
+            assert((pt - center_pos).norm() > radius - deviation - SCALED_EPSILON);
+            assert((pt - center_pos).norm() < radius + deviation + SCALED_EPSILON);
+        }
+//        Vec2d new_center = ArcWelder::arc_fit_center_algebraic_ls(start_pos, end_pos, center_pos, samples.begin(), samples.end());
+        THEN("Center is fitted correctly") {
+            std::optional<Vec2d> new_center_opt = ArcWelder::arc_fit_center_gauss_newton_ls(start_pos, end_pos, center_pos, samples.begin(), samples.end(), 10);
+            REQUIRE(new_center_opt);
+            if (new_center_opt) {
+                Vec2d  new_center = *new_center_opt;
+                double new_radius = (new_center - start_pos).norm();
+                double total_deviation = 0;
+                double new_total_deviation = 0;
+                for (const Vec2d &s : samples) {
+                    total_deviation += sqr((s - center_pos).norm() - radius);
+                    new_total_deviation += sqr((s - new_center).norm() - radius);
+                }
+                total_deviation /= double(num_samples);
+                new_total_deviation /= double(num_samples);
+                REQUIRE(new_total_deviation <= total_deviation);
+
+#if 0
+                double dist = (center_pos - new_center).norm();
+                printf("Radius: %lf, Angle: %lf deg, Samples: %d, Dist: %lf\n", unscaled<double>(radius), 180. * angle / M_PI, int(num_samples), unscaled<double>(dist));
+    //            REQUIRE(is_approx(center_pos, new_center, deviation));
+                if (dist > scaled<double>(1.)) {
+                    static int irun = 0;
+                    char path[2048];
+                    sprintf(path, "d:\\temp\\debug\\circle-fit-%d.svg", irun++);
+                    Eigen::AlignedBox<double, 2> bbox(start_pos, end_pos);
+                    for (const Vec2d& sample : samples)
+                        bbox.extend(sample);
+                    bbox.extend(center_pos);
+                    Slic3r::SVG svg(path, BoundingBox(bbox.min().cast<coord_t>(), bbox.max().cast<coord_t>()).inflated(bbox.sizes().maxCoeff() * 0.05));
+                    Points arc_src;
+                    for (size_t i = 0; i <= 1000; ++i)
+                        arc_src.emplace_back((center_pos + Eigen::Rotation2D(angle0 + double(i) * angle / 1000.) * Vec2d(radius, 0.)).cast<coord_t>());
+                    svg.draw(Polyline(arc_src));
+                    Points arc_new;
+                    double new_arc_angle = ArcWelder::arc_angle(start_pos, end_pos, (new_center - start_pos).norm());
+                    for (size_t i = 0; i <= 1000; ++i)
+                        arc_new.emplace_back((new_center + Eigen::Rotation2D(double(i) * new_arc_angle / 1000.) * (start_pos - new_center)).cast<coord_t>());
+                    svg.draw(Polyline(arc_new), "magenta");
+                    svg.draw(Point(start_pos.cast<coord_t>()), "blue");
+                    svg.draw(Point(end_pos.cast<coord_t>()), "blue");
+                    svg.draw(Point(center_pos.cast<coord_t>()), "blue");
+                    for (const Vec2d &sample : samples)
+                        svg.draw(Point(sample.cast<coord_t>()), "red");
+                    svg.draw(Point(new_center.cast<coord_t>()), "magenta");
+                }
+                if (!is_approx(center_pos, new_center, scaled<double>(5.))) {
+                    printf("Failed\n");
+                }
+#endif
+            } else {
+                printf("Fitting failed\n");
+            }
+        }
+    };
+
+    WHEN("Generating a random arc and randomized arc samples") {
+        for (size_t i = 0; i < 1000; ++ i)
+            test_arc_fitting();
+    }
+}
+
 TEST_CASE("arc wedge test", "[ArcWelder]") {
     using namespace Slic3r::Geometry;
 
@@ -262,3 +398,114 @@ TEST_CASE("arc wedge test", "[ArcWelder]") {
         }
     }
 }
+
+#if 0
+// For quantization
+//#include <libslic3r/GCode/GCodeWriter.hpp>
+
+TEST_CASE("arc quantization", "[ArcWelder]") {
+    using namespace Slic3r::Geometry;
+
+    WHEN("generating a bunch of random arches") {
+        static constexpr const size_t  len            = 100000;
+        static constexpr const coord_t max_coordinate = scaled<coord_t>(250.);
+        static constexpr const float   max_radius     = scaled<float>(250.);
+        ArcWelder::Segments path;
+        path.reserve(len + 1);
+        // Seeded with a fixed seed, to be repeatable.
+        std::mt19937 rng(987432690);
+        // Generate bunch of random arches.
+        std::uniform_int_distribution<int32_t>  coord_sampler(0, int32_t(max_coordinate));
+        std::uniform_real_distribution<float>   radius_sampler(- max_radius, max_radius);
+        std::uniform_int_distribution<int>      orientation_sampler(0, 1);
+        path.push_back({ Point{coord_sampler(rng), coord_sampler(rng)}, 0, ArcWelder::Orientation::CCW });
+        for (size_t i = 0; i < len; ++ i) {
+            ArcWelder::Segment seg { Point{coord_sampler(rng), coord_sampler(rng)}, radius_sampler(rng), orientation_sampler(rng) ? ArcWelder::Orientation::CCW : ArcWelder::Orientation::CW };
+            while ((seg.point.cast<double>() - path.back().point.cast<double>()).norm() > 2. * std::abs(seg.radius))
+                seg = { Point{coord_sampler(rng), coord_sampler(rng)}, radius_sampler(rng), orientation_sampler(rng) ? ArcWelder::Orientation::CCW : ArcWelder::Orientation::CW };
+            path.push_back(seg);
+        }
+        // Run the test, quantize coordinates and radius, find the maximum error of quantization comparing the two arches.
+        struct ArcEval {
+            double error;
+            double radius;
+            double angle;
+        };
+        std::vector<ArcEval> center_errors_R;
+        center_errors_R.reserve(len);
+        std::vector<double> center_errors_R_exact;
+        center_errors_R_exact.reserve(len);
+        std::vector<double> center_errors_IJ;
+        center_errors_IJ.reserve(len);
+        for (size_t i = 0; i < len; ++ i) {
+            // Source arc:
+            const Vec2d  start_point = unscaled<double>(path[i].point);
+            const Vec2d  end_point   = unscaled<double>(path[i + 1].point);
+            const double radius      = unscaled<double>(path[i + 1].radius);
+            const bool   ccw         = path[i + 1].ccw();
+            const Vec2d  center      = ArcWelder::arc_center(start_point, end_point, radius, ccw);
+            {
+                const double d1 = (start_point - center).norm();
+                const double d2 = (end_point - center).norm();
+                const double dx = (end_point - start_point).norm();
+                assert(std::abs(d1 - std::abs(radius)) < EPSILON);
+                assert(std::abs(d2 - std::abs(radius)) < EPSILON);
+            }
+            // Quantized arc:
+            const Vec2d  start_point_quantized { GCodeFormatter::quantize_xyzf(start_point.x()), GCodeFormatter::quantize_xyzf(start_point.y()) };
+            const Vec2d  end_point_quantized   { GCodeFormatter::quantize_xyzf(end_point  .x()), GCodeFormatter::quantize_xyzf(end_point  .y()) };
+            const double radius_quantized      { GCodeFormatter::quantize_xyzf(radius) };
+            const Vec2d  center_quantized      { GCodeFormatter::quantize_xyzf(center     .x()), GCodeFormatter::quantize_xyzf(center     .y()) };
+            // Evaulate maximum error for the quantized arc given by the end points and radius.
+            const Vec2d  center_from_quantized = ArcWelder::arc_center(start_point_quantized, end_point_quantized, radius, ccw);
+            const Vec2d  center_reconstructed  = ArcWelder::arc_center(start_point_quantized, end_point_quantized, radius_quantized, ccw);
+#if 0
+            center_errors_R.push_back({
+                std::abs((center_reconstructed - center).norm()),
+                radius,
+                ArcWelder::arc_angle(start_point, end_point, radius) * 180. / M_PI
+            });
+            if (center_errors_R.back().error > 0.15)
+                printf("Fuj\n");
+#endif
+            center_errors_R_exact.emplace_back(std::abs((center_from_quantized - center).norm()));
+            if (center_errors_R_exact.back() > 0.15)
+                printf("Fuj\n");
+            center_errors_IJ.emplace_back(std::abs((center_quantized - center).norm()));
+            if (center_errors_IJ.back() > 0.15)
+                printf("Fuj\n");
+
+            // Adjust center of the arc to the quantized end points.
+            Vec2d third_point = ArcWelder::arc_middle_point(start_point, end_point, radius, ccw);
+            double third_point_radius = (third_point - center).norm();
+            assert(std::abs(third_point_radius - std::abs(radius)) < EPSILON);
+            std::optional<Vec2d> center_adjusted = try_circle_center(start_point_quantized, end_point_quantized, third_point, EPSILON);
+            //assert(center_adjusted);
+            if (center_adjusted) {
+                const double radius_adjusted = (center_adjusted.value() - start_point_quantized).norm() * (radius > 0 ? 1. : -1.);
+                const double radius_adjusted_quantized = GCodeFormatter::quantize_xyzf(radius_adjusted);
+                // Evaulate maximum error for the quantized arc given by the end points and radius.
+                const Vec2f  center_reconstructed = ArcWelder::arc_center(start_point_quantized.cast<float>(), end_point_quantized.cast<float>(), float(radius_adjusted_quantized), ccw);
+                double rtest = std::abs(radius_adjusted_quantized);
+                double d1 = std::abs((center_reconstructed - start_point.cast<float>()).norm() - rtest);
+                double d2 = std::abs((center_reconstructed - end_point.cast<float>()).norm() - rtest);
+                double d3 = std::abs((center_reconstructed - third_point.cast<float>()).norm() - rtest);
+                double d = std::max(d1, std::max(d2, d3));
+                center_errors_R.push_back({
+                    d,
+                    radius,
+                    ArcWelder::arc_angle(start_point, end_point, radius) * 180. / M_PI
+                    });
+            } else {
+                printf("Adjusted circle is collinear.\n");
+            }
+        }
+        std::sort(center_errors_R.begin(), center_errors_R.end(), [](auto l, auto r) { return l.error > r.error; });
+        std::sort(center_errors_R_exact.begin(), center_errors_R_exact.end(), [](auto l, auto r) { return l > r; });
+        std::sort(center_errors_IJ.begin(), center_errors_IJ.end(), [](auto l, auto r) { return l > r; });
+        printf("Maximum error R: %lf\n", center_errors_R.back().error);
+        printf("Maximum error R exact: %lf\n", center_errors_R_exact.back());
+        printf("Maximum error IJ: %lf\n", center_errors_IJ.back());
+    }
+}
+#endif

tests/libslic3r/test_geometry.cpp

@@ -323,6 +323,54 @@ SCENARIO("Circle Fit, TaubinFit with Newton's method", "[Geometry]") {
     }
 }
 
+SCENARIO("Circle Fit, least squares by decomposition or by solving normal equation", "[Geometry]") {
+    auto test_circle_fit = [](const Geometry::Circled &circle, const Vec2d &center, const double radius) {
+        THEN("A center point matches.") {
+            REQUIRE(is_approx(circle.center, center));
+        }
+        THEN("Radius matches") {
+            REQUIRE(is_approx(circle.radius, radius));
+        }
+    };
+
+    GIVEN("A vector of Vec2ds arranged in a half-circle with approximately the same distance R from some point") {
+        const Vec2d  expected_center(-6., 0.);
+        const double expected_radius = 6.;
+        Vec2ds sample{Vec2d(6.0, 0), Vec2d(5.1961524, 3), Vec2d(3 ,5.1961524), Vec2d(0, 6.0), Vec2d(3, 5.1961524), Vec2d(-5.1961524, 3), Vec2d(-6.0, 0)};
+        std::transform(sample.begin(), sample.end(), sample.begin(), [expected_center] (const Vec2d &a) { return a + expected_center; });
+
+        WHEN("Circle fit is called on the entire array, least squares SVD") {
+            test_circle_fit(Geometry::circle_linear_least_squares_svd(sample), expected_center, expected_radius);
+        }
+        WHEN("Circle fit is called on the first four points, least squares SVD") {
+            test_circle_fit(Geometry::circle_linear_least_squares_svd(Vec2ds(sample.cbegin(), sample.cbegin() + 4)), expected_center, expected_radius);
+        }
+        WHEN("Circle fit is called on the middle four points, least squares SVD") {
+            test_circle_fit(Geometry::circle_linear_least_squares_svd(Vec2ds(sample.cbegin() + 2, sample.cbegin() + 6)), expected_center, expected_radius);
+        }
+
+        WHEN("Circle fit is called on the entire array, least squares QR decomposition") {
+            test_circle_fit(Geometry::circle_linear_least_squares_qr(sample), expected_center, expected_radius);
+        }
+        WHEN("Circle fit is called on the first four points, least squares QR decomposition") {
+            test_circle_fit(Geometry::circle_linear_least_squares_qr(Vec2ds(sample.cbegin(), sample.cbegin() + 4)), expected_center, expected_radius);
+        }
+        WHEN("Circle fit is called on the middle four points, least squares QR decomposition") {
+            test_circle_fit(Geometry::circle_linear_least_squares_qr(Vec2ds(sample.cbegin() + 2, sample.cbegin() + 6)), expected_center, expected_radius);
+        }
+
+        WHEN("Circle fit is called on the entire array, least squares by normal equations") {
+            test_circle_fit(Geometry::circle_linear_least_squares_normal(sample), expected_center, expected_radius);
+        }
+        WHEN("Circle fit is called on the first four points, least squares by normal equations") {
+            test_circle_fit(Geometry::circle_linear_least_squares_normal(Vec2ds(sample.cbegin(), sample.cbegin() + 4)), expected_center, expected_radius);
+        }
+        WHEN("Circle fit is called on the middle four points, least squares by normal equations") {
+            test_circle_fit(Geometry::circle_linear_least_squares_normal(Vec2ds(sample.cbegin() + 2, sample.cbegin() + 6)), expected_center, expected_radius);
+        }
+    }
+}
+
 TEST_CASE("smallest_enclosing_circle_welzl", "[Geometry]") {
     // Some random points in plane.
     Points pts {