diff --git a/src/libslic3r/GCode.cpp b/src/libslic3r/GCode.cpp index dd62029769..3a8b8842fa 100644 --- a/src/libslic3r/GCode.cpp +++ b/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(EXTRUDER_CONFIG(nozzle_diameter)) * LOOP_CLIPPING_LENGTH_OVER_NOZZLE_DIAMETER, scaled(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(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(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(), p.cast(), double(radius), it->ccw()) + Geometry::ArcWelder::arc_center(prev_exact.cast(), p_exact.cast(), 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; } } diff --git a/src/libslic3r/GCode/SmoothPath.cpp b/src/libslic3r/GCode/SmoothPath.cpp index ba793b465e..f27f1f2318 100644 --- a/src/libslic3r/GCode/SmoothPath.cpp +++ b/src/libslic3r/GCode/SmoothPath.cpp @@ -102,7 +102,10 @@ std::optional 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(prev_segment, last_segment) < min_point_distance_threshold) { + last_path.pop_back(); + if (last_path.size() < 2) + path.pop_back(); + } return 0; } } diff --git a/src/libslic3r/GCode/SmoothPath.hpp b/src/libslic3r/GCode/SmoothPath.hpp index b2eb335a32..83afd60983 100644 --- a/src/libslic3r/GCode/SmoothPath.hpp +++ b/src/libslic3r/GCode/SmoothPath.hpp @@ -29,7 +29,9 @@ std::optional sample_path_point_at_distance_from_start(const SmoothPath & std::optional 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 { diff --git a/src/libslic3r/Geometry/ArcWelder.cpp b/src/libslic3r/Geometry/ArcWelder.cpp index 1bb9fea77d..e30f6b07a9 100644 --- a/src/libslic3r/Geometry/ArcWelder.cpp +++ b/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(), end_pos.cast(), 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() - center).norm() - r) < SCALED_EPSILON); + assert(std::abs((std::prev(end)->cast() - 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() - center).norm() - r); + ++ cnt; + } + Point closest_point; + if (foot_pt_on_segment(*it, *std::next(it), center.cast(), closest_point)) { + total_deviation += sqr((closest_point.cast() - center).cast().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(), end_pos.cast(), 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() - center).norm() - r) < SCALED_EPSILON); + assert(std::abs((std::prev(end)->cast() - 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() - 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(), closest_point)) { + double signed_deviation = (closest_point.cast() - center).cast().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 try_create_circle(const Points::const_iterator begin, const Points::const_iterator end, const double max_radius, const double tolerance) { std::optional 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 = 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::max(); - double current_deviation; - for (auto it = std::next(begin); std::next(it) != end; ++ it) - if (std::optional 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 fpts; + Vec2d center_point = out->center.cast(); + Vec2d first_point = begin->cast(); + Vec2d mid_point = std::next(begin)->cast(); + Vec2d last_point = std::prev(end)->cast(); + 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 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(); + 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; + { + // 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(); + Vec2i64 last_point = std::prev(end)->cast(); + Vec2i64 c = (first_point.cast() + last_point.cast()) / 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::max(), std::numeric_limits::max() }; +#endif // NDEBUG + for (auto it = std::next(begin); it != end; ++ it) { + Vec2i64 this_point = it->cast(); + 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(); + Vec2d p = c.cast() + vd * double(d) / vd.squaredNorm(); + point_on_bisector = p.cast(); + break; + } + if (sideness == 0) { + // this_point is on the bisector. + assert(prev_side != 0); + assert(this_side == 0); + point_on_bisector = this_point.cast(); + break; + } + prev_point = this_point; + prev_side = this_side; + } + // point_on_bisector must be set + assert(point_on_bisector.x() != std::numeric_limits::max() && point_on_bisector.y() != std::numeric_limits::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 fpts; + Vec2d first_point = begin->cast(); + Vec2d last_point = std::prev(end)->cast(); + Vec2d prev_point = first_point; + for (auto it = std::next(begin); it != std::prev(end); ++ it) { + Vec2d this_point = it->cast(); + 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 opt_center = ArcWelder::arc_fit_center_gauss_newton_ls(first_point, last_point, + circle->center.cast(), fpts.begin(), fpts.end(), 5); + if (opt_center) { + circle->center = opt_center->cast(); + 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::max(); + double current_deviation; + for (auto it = std::next(begin); std::next(it) != end; ++ it) + if (std::optional 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(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(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() - arc->center.cast(); + Vec2i64 v2 = arc->end_point.cast() - arc->center.cast(); + do { + if (std::abs((arc->center.cast() - next_end->cast()).norm() - arc->radius) >= tolerance || + inside_arc_wedge_vectors(v1, v2, + arc->radius > 0, arc->direction == Orientation::CCW, + next_end->cast() - arc->center.cast())) + // 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().squaredNorm() < 2. * sqr(scaled(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(arc->radius), arc_length(arc->start_point.cast(), arc->end_point.cast(), 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().squaredNorm() > sqr(scaled(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(); diff --git a/src/libslic3r/Geometry/ArcWelder.hpp b/src/libslic3r/Geometry/ArcWelder.hpp index 8ef7d8585a..660d0ccf2c 100644 --- a/src/libslic3r/Geometry/ArcWelder.hpp +++ b/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 (600.); // 0.05mm diff --git a/src/libslic3r/Geometry/Circle.cpp b/src/libslic3r/Geometry/Circle.cpp index 24d408c6a2..cb8af30cec 100644 --- a/src/libslic3r/Geometry/Circle.cpp +++ b/src/libslic3r/Geometry/Circle.cpp @@ -147,7 +147,7 @@ Circled circle_ransac(const Vec2ds& input, size_t iterations, double* min_error) } template -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 &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 &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) { diff --git a/src/libslic3r/Geometry/Circle.hpp b/src/libslic3r/Geometry/Circle.hpp index ca32d32b50..487ee84e8f 100644 --- a/src/libslic3r/Geometry/Circle.hpp +++ b/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 diff --git a/tests/libslic3r/test_arc_welder.cpp b/tests/libslic3r/test_arc_welder.cpp index 2686216954..4dad0e6ece 100644 --- a/tests/libslic3r/test_arc_welder.cpp +++ b/tests/libslic3r/test_arc_welder.cpp @@ -1,7 +1,11 @@ #include #include +#include + #include +#include +#include #include 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(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(sqrt(250. - 1.)); + const double min_radius = scaled(0.01); + const double max_radius = scaled(250.); +// const double deviation = scaled(0.5); + const double deviation = scaled(0.1); + // Seeded with a fixed seed, to be repeatable. + std::mt19937 rng(867092346); + std::uniform_int_distribution coord_sampler(0, int32_t(max_coordinate)); + std::uniform_real_distribution angle_sampler(0.001, 2. * M_PI - 0.001); + std::uniform_real_distribution radius_sampler(min_radius, max_radius); + std::uniform_int_distribution 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 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(sqr(std::max(0., radius - deviation)), sqr(radius + deviation))(rng)); + double sample_a = std::uniform_real_distribution(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 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(radius), 180. * angle / M_PI, int(num_samples), unscaled(dist)); + // REQUIRE(is_approx(center_pos, new_center, deviation)); + if (dist > scaled(1.)) { + static int irun = 0; + char path[2048]; + sprintf(path, "d:\\temp\\debug\\circle-fit-%d.svg", irun++); + Eigen::AlignedBox 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(), bbox.max().cast()).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()); + 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()); + svg.draw(Polyline(arc_new), "magenta"); + svg.draw(Point(start_pos.cast()), "blue"); + svg.draw(Point(end_pos.cast()), "blue"); + svg.draw(Point(center_pos.cast()), "blue"); + for (const Vec2d &sample : samples) + svg.draw(Point(sample.cast()), "red"); + svg.draw(Point(new_center.cast()), "magenta"); + } + if (!is_approx(center_pos, new_center, scaled(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 + +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(250.); + static constexpr const float max_radius = scaled(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 coord_sampler(0, int32_t(max_coordinate)); + std::uniform_real_distribution radius_sampler(- max_radius, max_radius); + std::uniform_int_distribution 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() - path.back().point.cast()).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 center_errors_R; + center_errors_R.reserve(len); + std::vector center_errors_R_exact; + center_errors_R_exact.reserve(len); + std::vector center_errors_IJ; + center_errors_IJ.reserve(len); + for (size_t i = 0; i < len; ++ i) { + // Source arc: + const Vec2d start_point = unscaled(path[i].point); + const Vec2d end_point = unscaled(path[i + 1].point); + const double radius = unscaled(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 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(), end_point_quantized.cast(), float(radius_adjusted_quantized), ccw); + double rtest = std::abs(radius_adjusted_quantized); + double d1 = std::abs((center_reconstructed - start_point.cast()).norm() - rtest); + double d2 = std::abs((center_reconstructed - end_point.cast()).norm() - rtest); + double d3 = std::abs((center_reconstructed - third_point.cast()).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 diff --git a/tests/libslic3r/test_geometry.cpp b/tests/libslic3r/test_geometry.cpp index 867ff25c62..ba61e1bd4b 100644 --- a/tests/libslic3r/test_geometry.cpp +++ b/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 ¢er, 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 {