diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index bc7f36fc95..a9ce8dd1f2 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -138,11 +138,11 @@ set(SLIC3R_SOURCES GCodeWriter.hpp Geometry.cpp Geometry.hpp + Geometry/Bicubic.hpp Geometry/Circle.cpp Geometry/Circle.hpp Geometry/ConvexHull.cpp Geometry/ConvexHull.hpp - Geometry/Curves.cpp Geometry/Curves.hpp Geometry/MedialAxis.cpp Geometry/MedialAxis.hpp diff --git a/src/libslic3r/GCode/SeamPlacer.cpp b/src/libslic3r/GCode/SeamPlacer.cpp index d861b16e7f..b0254762f9 100644 --- a/src/libslic3r/GCode/SeamPlacer.cpp +++ b/src/libslic3r/GCode/SeamPlacer.cpp @@ -17,7 +17,9 @@ #include "libslic3r/QuadricEdgeCollapse.hpp" #include "libslic3r/Subdivide.hpp" -//#define DEBUG_FILES +#include "libslic3r/Geometry/Curves.hpp" + +#define DEBUG_FILES #ifdef DEBUG_FILES #include @@ -185,8 +187,8 @@ std::vector raycast_visibility(const AABBTreeIndirect::Tree< if (some_hit) { // NOTE: iterating in reverse, from the last hit for one simple reason: We know the state of the ray at that point; // It cannot be inside model, and it cannot be inside negative volume - for (int hit_index = hits.size() - 1; hit_index >= 0; --hit_index) { - if (hits[hit_index].id >= negative_volumes_start_index) { //negative volume hit + for (int hit_index = int(hits.size()) - 1; hit_index >= 0; --hit_index) { + if (hits[hit_index].id >= int(negative_volumes_start_index)) { //negative volume hit Vec3f normal = its_face_normal(triangles, hits[hit_index].id); in_negative += sgn(normal.dot(final_ray_dir)); // if volume face aligns with ray dir, we are leaving negative space // which in reverse hit analysis means, that we are entering negative space :) and vice versa @@ -714,8 +716,9 @@ struct SeamComparator { void debug_export_points(const std::vector> &object_perimter_points, const BoundingBox &bounding_box, std::string object_name, const SeamComparator &comparator) { for (size_t layer_idx = 0; layer_idx < object_perimter_points.size(); ++layer_idx) { - std::string angles_file_name = debug_out_path((object_name + "_angles_" + std::to_string(layer_idx) + ".svg").c_str()); - SVG angles_svg {angles_file_name, bounding_box}; + std::string angles_file_name = debug_out_path( + (object_name + "_angles_" + std::to_string(layer_idx) + ".svg").c_str()); + SVG angles_svg { angles_file_name, bounding_box }; float min_vis = 0; float max_vis = min_vis; @@ -725,7 +728,7 @@ void debug_export_points(const std::vector())), fill); min_vis = std::min(min_vis, point.visibility); max_vis = std::max(max_vis, point.visibility); @@ -735,20 +738,22 @@ void debug_export_points(const std::vector())), visibility_fill); Vec3i weight_color = value_rgbi(min_weight, max_weight, -comparator.get_penalty(point)); std::string weight_fill = "rgb(" + std::to_string(weight_color.x()) + "," + std::to_string(weight_color.y()) - + "," - + std::to_string(weight_color.z()) + ")"; + + "," + + std::to_string(weight_color.z()) + ")"; weight_svg.draw(scaled(Vec2f(point.position.head<2>())), weight_fill); } } @@ -1086,27 +1091,23 @@ void SeamPlacer::align_seam_points(const PrintObject *po, const SeamPlacerImpl:: }); // gather all positions of seams and their weights (weights are derived as negative penalty, they are made positive in next step) - std::vector points(seam_string.size()); + std::vector observations(seam_string.size()); + std::vector observation_points(seam_string.size()); std::vector weights(seam_string.size()); //init min_weight by the first point float min_weight = -comparator.get_penalty( m_perimeter_points_per_object[po][seam_string[0].first][seam_string[0].second]); - // In the sorted seam_string array, point which started the alignment - the best candidate - size_t best_candidate_point_index = 0; - - //gather points positions and weights, update min_weight in each step, and find the best candidate + //gather points positions and weights, update min_weight in each step for (size_t index = 0; index < seam_string.size(); ++index) { - points[index] = + Vec3f pos = m_perimeter_points_per_object[po][seam_string[index].first][seam_string[index].second].position; + observations[index] = pos; + observation_points[index] = pos.z(); weights[index] = -comparator.get_penalty( m_perimeter_points_per_object[po][seam_string[index].first][seam_string[index].second]); min_weight = std::min(min_weight, weights[index]); - // find the best candidate by comparing the layer indexes - if (seam_string[index].first == layer_idx) { - best_candidate_point_index = index; - } } //makes all weights positive @@ -1114,70 +1115,19 @@ void SeamPlacer::align_seam_points(const PrintObject *po, const SeamPlacerImpl:: w = w - min_weight + 0.01; } - //NOTE: the following commented block does polynomial line fitting of the seam string. - // pre-smoothen by Laplace -// for (size_t iteration = 0; iteration < SeamPlacer::seam_align_laplace_smoothing_iterations; ++iteration) { -// std::vector new_points(seam_string.size()); -// for (int point_index = 0; point_index < points.size(); ++point_index) { -// size_t prev_idx = point_index > 0 ? point_index - 1 : point_index; -// size_t next_idx = point_index < points.size() - 1 ? point_index + 1 : point_index; -// -// new_points[point_index] = (points[prev_idx] * weights[prev_idx] -// + points[point_index] * weights[point_index] + points[next_idx] * weights[next_idx]) / -// (weights[prev_idx] + weights[point_index] + weights[next_idx]); -// } -// points = new_points; -// } -// - // find coefficients of polynomial fit. Z coord is treated as parameter along which to fit both X and Y coords. -// std::vector coefficients = polyfit(points, weights, 4); -// -// // Do alignment - compute fitted point for each point in the string from its Z coord, and store the position into -// // Perimeter structure of the point; also set flag aligned to true -// for (const auto &pair : seam_string) { -// float current_height = m_perimeter_points_per_object[po][pair.first][pair.second].position.z(); -// Vec3f seam_pos = get_fitted_point(coefficients, current_height); -// -// Perimeter *perimeter = -// m_perimeter_points_per_object[po][pair.first][pair.second].perimeter.get(); -// perimeter->final_seam_position = seam_pos; -// perimeter->finalized = true; -// } -// -// for (Vec3f &p : points) { -// p = get_fitted_point(coefficients, p.z()); -// } + // Curve Fitting + size_t number_of_splines = std::max(size_t(1), size_t(observations.size() / SeamPlacer::seam_align_seams_per_spline)); + auto curve = Geometry::fit_cubic_bspline(observations, observation_points, weights, number_of_splines); - // LaPlace smoothing iterations over the gathered points. New positions from each iteration are stored in the new_points vector - // and assigned to points at the end of iteration - for (size_t iteration = 0; iteration < SeamPlacer::seam_align_laplace_smoothing_iterations; ++iteration) { - std::vector new_points(seam_string.size()); - // start from the best candidate, and smoothen down - for (int point_index = best_candidate_point_index; point_index >= 0; --point_index) { - int prev_idx = point_index > 0 ? point_index - 1 : point_index; - size_t next_idx = point_index < int(points.size()) - 1 ? point_index + 1 : point_index; + // Do alignment - compute fitted point for each point in the string from its Z coord, and store the position into + // Perimeter structure of the point; also set flag aligned to true + for (const auto &pair : seam_string) { + Vec3f current_pos = m_perimeter_points_per_object[po][pair.first][pair.second].position; + Vec3f seam_pos = curve.get_fitted_value(current_pos.z()); - new_points[point_index] = (points[prev_idx] * weights[prev_idx] - + points[point_index] * weights[point_index] + points[next_idx] * weights[next_idx]) / - (weights[prev_idx] + weights[point_index] + weights[next_idx]); - } - // smoothen up the rest of the points - for (size_t point_index = best_candidate_point_index + 1; point_index < points.size(); ++point_index) { - size_t prev_idx = point_index > 0 ? point_index - 1 : point_index; - size_t next_idx = point_index < points.size() - 1 ? point_index + 1 : point_index; - - new_points[point_index] = (points[prev_idx] * weights[prev_idx] - + points[point_index] * weights[point_index] + points[next_idx] * weights[next_idx]) / - (weights[prev_idx] + weights[point_index] + weights[next_idx]); - } - points = new_points; - } - - // Assign smoothened posiiton to each participating perimeter and set finalized flag - for (size_t index = 0; index < seam_string.size(); ++index) { Perimeter *perimeter = - m_perimeter_points_per_object[po][seam_string[index].first][seam_string[index].second].perimeter.get(); - perimeter->final_seam_position = points[index]; + m_perimeter_points_per_object[po][pair.first][pair.second].perimeter.get(); + perimeter->final_seam_position = seam_pos; perimeter->finalized = true; } @@ -1326,3 +1276,4 @@ void SeamPlacer::place_seam(const Layer *layer, ExtrusionLoop &loop, bool extern } } // namespace Slic3r + diff --git a/src/libslic3r/GCode/SeamPlacer.hpp b/src/libslic3r/GCode/SeamPlacer.hpp index b8ba204c1a..f422434488 100644 --- a/src/libslic3r/GCode/SeamPlacer.hpp +++ b/src/libslic3r/GCode/SeamPlacer.hpp @@ -112,8 +112,8 @@ public: static constexpr size_t seam_align_tolerable_skips = 4; // minimum number of seams needed in cluster to make alignemnt happen static constexpr size_t seam_align_minimum_string_seams = 6; - // iterations of laplace smoothing - static constexpr size_t seam_align_laplace_smoothing_iterations = 20; + // points covered by spline; determines number of splines for the given string + static constexpr size_t seam_align_seams_per_spline = 10; //The following data structures hold all perimeter points for all PrintObject. The structure is as follows: // Map of PrintObjects (PO) -> vector of layers of PO -> vector of perimeter points of the given layer diff --git a/src/libslic3r/Geometry/Bicubic.hpp b/src/libslic3r/Geometry/Bicubic.hpp new file mode 100644 index 0000000000..9dc4c0083a --- /dev/null +++ b/src/libslic3r/Geometry/Bicubic.hpp @@ -0,0 +1,291 @@ +#ifndef BICUBIC_HPP +#define BICUBIC_HPP + +#include +#include +#include + +#include + +namespace Slic3r { + +namespace Geometry { + +namespace BicubicInternal { +// Linear kernel, to be able to test cubic methods with hat kernels. +template +struct LinearKernel +{ + typedef T FloatType; + + static T a00() { + return T(0.); + } + static T a01() { + return T(0.); + } + static T a02() { + return T(0.); + } + static T a03() { + return T(0.); + } + static T a10() { + return T(1.); + } + static T a11() { + return T(-1.); + } + static T a12() { + return T(0.); + } + static T a13() { + return T(0.); + } + static T a20() { + return T(0.); + } + static T a21() { + return T(1.); + } + static T a22() { + return T(0.); + } + static T a23() { + return T(0.); + } + static T a30() { + return T(0.); + } + static T a31() { + return T(0.); + } + static T a32() { + return T(0.); + } + static T a33() { + return T(0.); + } +}; + +// Interpolation kernel aka Catmul-Rom aka Keyes kernel. +template +struct CubicCatmulRomKernel +{ + typedef T FloatType; + + static T a00() { + return 0; + } + static T a01() { + return (T) -0.5; + } + static T a02() { + return (T) 1.; + } + static T a03() { + return (T) -0.5; + } + static T a10() { + return (T) 1.; + } + static T a11() { + return 0; + } + static T a12() { + return (T) -5. / 2.; + } + static T a13() { + return (T) 3. / 2.; + } + static T a20() { + return 0; + } + static T a21() { + return (T) 0.5; + } + static T a22() { + return (T) 2.; + } + static T a23() { + return (T) -3. / 2.; + } + static T a30() { + return 0; + } + static T a31() { + return 0; + } + static T a32() { + return (T) -0.5; + } + static T a33() { + return (T) 0.5; + } +}; + +// B-spline kernel +template +struct CubicBSplineKernel +{ + typedef T FloatType; + + static T a00() { + return (T) 1. / 6.; + } + static T a01() { + return (T) -3. / 6.; + } + static T a02() { + return (T) 3. / 6.; + } + static T a03() { + return (T) -1. / 6.; + } + static T a10() { + return (T) 4. / 6.; + } + static T a11() { + return 0; + } + static T a12() { + return (T) -6. / 6.; + } + static T a13() { + return (T) 3. / 6.; + } + static T a20() { + return (T) 1. / 6.; + } + static T a21() { + return (T) 3. / 6.; + } + static T a22() { + return (T) 3. / 6.; + } + static T a23() { + return (T) -3. / 6.; + } + static T a30() { + return 0; + } + static T a31() { + return 0; + } + static T a32() { + return 0; + } + static T a33() { + return (T) 1. / 6.; + } +}; + +template +inline T clamp(T a, T lower, T upper) + { + return (a < lower) ? lower : + (a > upper) ? upper : a; +} +} + +template +struct CubicKernelWrapper +{ + typedef typename Kernel::FloatType FloatType; + + static constexpr size_t kernel_span = 4; + + static FloatType kernel(FloatType x) + { + x = fabs(x); + if (x >= (FloatType) 2.) + return 0.0f; + if (x <= (FloatType) 1.) { + FloatType x2 = x * x; + FloatType x3 = x2 * x; + return Kernel::a10() + Kernel::a11() * x + Kernel::a12() * x2 + Kernel::a13() * x3; + } + assert(x > (FloatType )1. && x < (FloatType )2.); + x -= (FloatType) 1.; + FloatType x2 = x * x; + FloatType x3 = x2 * x; + return Kernel::a00() + Kernel::a01() * x + Kernel::a02() * x2 + Kernel::a03() * x3; + } + + static FloatType interpolate(FloatType f0, FloatType f1, FloatType f2, FloatType f3, FloatType x) + { + const FloatType x2 = x * x; + const FloatType x3 = x * x * x; + return f0 * (Kernel::a00() + Kernel::a01() * x + Kernel::a02() * x2 + Kernel::a03() * x3) + + f1 * (Kernel::a10() + Kernel::a11() * x + Kernel::a12() * x2 + Kernel::a13() * x3) + + f2 * (Kernel::a20() + Kernel::a21() * x + Kernel::a22() * x2 + Kernel::a23() * x3) + + f3 * (Kernel::a30() + Kernel::a31() * x + Kernel::a32() * x2 + Kernel::a33() * x3); + } +}; + +// Linear splines +template +using LinearKernel = CubicKernelWrapper>; + +// Catmul-Rom splines +template +using CubicCatmulRomKernel = CubicKernelWrapper>; + +// Cubic B-splines +template +using CubicBSplineKernel = CubicKernelWrapper>; + +template +static typename KernelWrapper::FloatType cubic_interpolate(const Eigen::ArrayBase &F, + const typename KernelWrapper::FloatType pt) { + typedef typename KernelWrapper::FloatType T; + const int w = int(F.size()); + const int ix = (int) floor(pt); + const T s = pt - (T) ix; + + if (ix > 1 && ix + 2 < w) { + // Inside the fully interpolated region. + return KernelWrapper::interpolate(F[ix - 1], F[ix], F[ix + 1], F[ix + 2], s); + } + // Transition region. Extend with a constant function. + auto f = [&F, w](T x) { + return F[BicubicInternal::clamp(x, 0, w - 1)]; + }; + return KernelWrapper::interpolate(f(ix - 1), f(ix), f(ix + 1), f(ix + 2), s); +} + +template +static float bicubic_interpolate(const Eigen::MatrixBase &F, + const Eigen::Matrix &pt) { + typedef typename Kernel::FloatType T; + const int w = F.cols(); + const int h = F.rows(); + const int ix = (int) floor(pt[0]); + const int iy = (int) floor(pt[1]); + const T s = pt[0] - (T) ix; + const T t = pt[1] - (T) iy; + + if (ix > 1 && ix + 2 < w && iy > 1 && iy + 2 < h) { + // Inside the fully interpolated region. + return Kernel::interpolate( + Kernel::interpolate(F(ix - 1, iy - 1), F(ix, iy - 1), F(ix + 1, iy - 1), F(ix + 2, iy - 1), s), + Kernel::interpolate(F(ix - 1, iy), F(ix, iy), F(ix + 1, iy), F(ix + 2, iy), s), + Kernel::interpolate(F(ix - 1, iy + 1), F(ix, iy + 1), F(ix + 1, iy + 1), F(ix + 2, iy + 1), s), + Kernel::interpolate(F(ix - 1, iy + 2), F(ix, iy + 2), F(ix + 1, iy + 2), F(ix + 2, iy + 2), s), t); + } + // Transition region. Extend with a constant function. + auto f = [&F, &f, w, h](int x, int y) { + return F(BicubicInternal::clamp(x, 0, w - 1), BicubicInternal::clamp(y, 0, h - 1)); + }; + return Kernel::interpolate( + Kernel::interpolate(f(ix - 1, iy - 1), f(ix, iy - 1), f(ix + 1, iy - 1), f(ix + 2, iy - 1), s), + Kernel::interpolate(f(ix - 1, iy), f(ix, iy), f(ix + 1, iy), f(ix + 2, iy), s), + Kernel::interpolate(f(ix - 1, iy + 1), f(ix, iy + 1), f(ix + 1, iy + 1), f(ix + 2, iy + 1), s), + Kernel::interpolate(f(ix - 1, iy + 2), f(ix, iy + 2), f(ix + 1, iy + 2), f(ix + 2, iy + 2), s), t); +} + +} //namespace Geometry + +} // namespace Slic3r + +#endif /* BICUBIC_HPP */ diff --git a/src/libslic3r/Geometry/Curves.cpp b/src/libslic3r/Geometry/Curves.cpp deleted file mode 100644 index fa95926be5..0000000000 --- a/src/libslic3r/Geometry/Curves.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include -#include -#include "Curves.hpp" - - -namespace Slic3r { -namespace Geometry { - -PolynomialCurve fit_polynomial(const std::vector &points, const std::vector &weights, size_t order) { - // check to make sure inputs are correct - assert(points.size() >= order + 1); - assert(points.size() == weights.size()); - - std::vector squared_weights(weights.size()); - for (size_t index = 0; index < weights.size(); ++index) { - squared_weights[index] = sqrt(weights[index]); - } - - Eigen::VectorXf V0(points.size()); - Eigen::VectorXf V1(points.size()); - Eigen::VectorXf V2(points.size()); - for (size_t index = 0; index < points.size(); index++) { - V0(index) = points[index].x() * squared_weights[index]; - V1(index) = points[index].y() * squared_weights[index]; - V2(index) = points[index].z(); - } - - // Create Matrix Placeholder of size n x k, n= number of datapoints, k = order of polynomial, for exame k = 3 for cubic polynomial - Eigen::MatrixXf T(points.size(), order + 1); - // Populate the matrix - for (size_t i = 0; i < points.size(); ++i) - { - for (size_t j = 0; j < order + 1; ++j) - { - T(i, j) = pow(V2(i), j) * squared_weights[i]; - } - } - - // Solve for linear least square fit - const auto QR = T.householderQr(); - Eigen::VectorXf result0 = QR.solve(V0); - Eigen::VectorXf result1 = QR.solve(V1); - std::vector coeff { order + 1 }; - for (size_t k = 0; k < order + 1; k++) { - coeff[k] = Vec2f { result0[k], result1[k] }; - } - - return PolynomialCurve{coeff}; -} - -Vec3f PolynomialCurve::get_fitted_point(float z) const { - size_t order = this->coefficients.size() - 1; - float fitted_x = 0; - float fitted_y = 0; - for (size_t index = 0; index < order + 1; ++index) { - float z_pow = pow(z, index); - fitted_x += this->coefficients[index].x() * z_pow; - fitted_y += this->coefficients[index].y() * z_pow; - } - - return Vec3f { fitted_x, fitted_y, z }; -} - -} -} diff --git a/src/libslic3r/Geometry/Curves.hpp b/src/libslic3r/Geometry/Curves.hpp index 3c102f192f..d221bfd1f5 100644 --- a/src/libslic3r/Geometry/Curves.hpp +++ b/src/libslic3r/Geometry/Curves.hpp @@ -2,156 +2,257 @@ #define SRC_LIBSLIC3R_GEOMETRY_CURVES_HPP_ #include "libslic3r/Point.hpp" +#include "Bicubic.hpp" #include namespace Slic3r { namespace Geometry { +template struct PolynomialCurve { - std::vector coefficients; + std::vector> coefficients; - explicit PolynomialCurve(const std::vector &coefficients) : + explicit PolynomialCurve(std::vector> coefficients) : coefficients(coefficients) { } - Vec3f get_fitted_point(float z) const; -}; - -//https://towardsdatascience.com/least-square-polynomial-CURVES-using-c-eigen-package-c0673728bd01 -// interpolates points in z (treats z coordinates as time) and returns coefficients for axis x and y -PolynomialCurve fit_polynomial(const std::vector &points, const std::vector &weights, size_t order); - -namespace CurveSmoothingKernels { -//NOTE: Kernel functions are used in range [-1,1]. - -// konstant kernel is used mainly for tests -template -struct ConstantKernel { - float operator()(NumberType w) const { - return NumberType(1); - } -}; - -//https://en.wikipedia.org/wiki/Kernel_(statistics) -template -struct EpanechnikovKernel { - float operator()(NumberType w) const { - return NumberType(0.25) * (NumberType(1) - w * w); - } -}; - -} - -template -using VecX = Eigen::Matrix; - -template -struct PiecewiseFittedCurve { - std::vector> coefficients; - Kernel kernel; - NumberType normalized_kernel_bandwidth; - NumberType length; - NumberType segment_size; - - NumberType get_segment_center(size_t segment_index) const { - return segment_size * segment_index; - } - - //t should be in normalized range [0,1] with respect to the length of the original polygon line - Vec get_fitted_point(const NumberType &t) const { - Vec result { 0 }; - for (int coeff_index = 0; coeff_index < coefficients[0].size(); ++coeff_index) { - NumberType segment_center = this->get_segment_center(coeff_index); - NumberType normalized_center_dis = (segment_center - t) - / (NumberType(0.5) * normalized_kernel_bandwidth); - if (normalized_center_dis >= NumberType(-1) && normalized_center_dis <= NumberType(1)) { - for (size_t dim = 0; dim < Dimension; ++dim) { - result[dim] += kernel(normalized_center_dis) * coefficients[dim][coeff_index]; - } + Vec3f get_fitted_value(const NumberType value) const { + Vec result = Vec::Zero(); + size_t order = this->coefficients.size() - 1; + for (size_t index = 0; index < order + 1; ++index) { + float powered = pow(value, index); + for (size_t dim = 0; dim < Dimension; ++dim) { + result(dim) += powered * this->coefficients[dim](index); } } return result; } }; -// number_of_segments: how many curve segments (kernel applications) are used. Must be at least 2, because we are centering the segments on the first and last point -// normalized_kernel_bandwidth (0,..): how spread the kernel is over the points in normalized coordinates (points are mapped to parametric range [0,1]) -// for example, 0.5 normalized_kernel_bandwidth means that one curve segment covers half of the points -template -PiecewiseFittedCurve fit_curve(const std::vector> &points, - size_t number_of_segments, const NumberType &normalized_kernel_bandwidth, - Kernel kernel) { - +//https://towardsdatascience.com/least-square-polynomial-CURVES-using-c-eigen-package-c0673728bd01 +// interpolates points in z (treats z coordinates as time) and returns coefficients for axis x and y +template +PolynomialCurve fit_polynomial(const std::vector> &observations, + const std::vector &observation_points, + const std::vector &weights, size_t order) { // check to make sure inputs are correct - assert(normalized_kernel_bandwidth > 0); - assert(number_of_segments >= 2); - assert(number_of_segments <= points.size()); - NumberType length { }; - std::vector knots { }; - for (size_t point_index = 0; point_index < points.size() - 1; ++point_index) { - knots.push_back(length); - length += (points[point_index + 1] - points[point_index]).norm(); - } - //last point - knots.push_back(length); + assert(observation_points.size() >= order + 1); + assert(observation_points.size() == weights.size()); + assert(observations.size() == weights.size()); - //normalize knots - for (NumberType &knot : knots) { - knot = knot / length; + std::vector squared_weights(weights.size()); + for (size_t index = 0; index < weights.size(); ++index) { + squared_weights[index] = sqrt(weights[index]); } - PiecewiseFittedCurve result { }; - - result.kernel = kernel; - result.normalized_kernel_bandwidth = normalized_kernel_bandwidth; - result.length = length; - result.segment_size = NumberType(1) / (number_of_segments - NumberType(1)); - - std::vector> data_points(Dimension); + std::vector> data_points(Dimension); for (size_t dim = 0; dim < Dimension; ++dim) { - data_points[dim] = Eigen::Matrix(points.size()); + data_points[dim] = Eigen::Matrix( + observations.size()); } - - for (size_t index = 0; index < points.size(); index++) { + for (size_t index = 0; index < observations.size(); index++) { for (size_t dim = 0; dim < Dimension; ++dim) { - data_points[dim](index) = points[index](dim); + data_points[dim](index) = observations[index](dim) * squared_weights[index]; } } - Eigen::MatrixXf T(knots.size(), number_of_segments); - for (size_t i = 0; i < knots.size(); ++i) { - for (size_t j = 0; j < number_of_segments; ++j) { - NumberType knot_val = knots[i]; - NumberType segment_center = result.get_segment_center(j); - NumberType normalized_center_dis = (segment_center - knot_val) - / (NumberType(0.5) * normalized_kernel_bandwidth); - if (normalized_center_dis >= NumberType(-1) && normalized_center_dis <= NumberType(1)) { - T(i, j) = kernel(normalized_center_dis); - } else { - T(i, j) = 0; - } + Eigen::MatrixXf T(observation_points.size(), order + 1); + // Populate the matrix + for (size_t i = 0; i < observation_points.size(); ++i) { + for (size_t j = 0; j < order + 1; ++j) { + T(i, j) = pow(observation_points[i], j) * squared_weights[i]; } } + const auto QR = T.householderQr(); + std::vector> coefficients(Dimension); // Solve for linear least square fit - std::vector> coefficients(Dimension); - const auto QR = T.colPivHouseholderQr(); for (size_t dim = 0; dim < Dimension; ++dim) { coefficients[dim] = QR.solve(data_points[dim]); } + return PolynomialCurve(coefficients); +} + +template +struct PiecewiseFittedCurve { + std::vector> coefficients; + Kernel kernel; + NumberType start; + NumberType length; + NumberType n_segment_size; + size_t segments_count; + + NumberType get_n_segment_start(int segment_index) const { + return n_segment_size * segment_index; + } + + NumberType normalize(const NumberType &observation_point) const { + return (observation_point - start) / length; + } + + Vec get_fitted_value(const NumberType &observation_point) const { + Vec result = Vec::Zero(); + NumberType t = normalize(observation_point); + + int start_segment_idx = int(floor(t / this->n_segment_size)) - int(Kernel::kernel_span * 0.5f - 1.0f); + for (int segment_index = start_segment_idx; segment_index < int(start_segment_idx + Kernel::kernel_span); + segment_index++) { + if (segment_index < 0 || segment_index >= int(this->segments_count)) { + continue; + } + NumberType segment_start = this->get_n_segment_start(segment_index); + NumberType normalized_segment_distance = (segment_start - t) / this->n_segment_size; + + for (size_t dim = 0; dim < Dimension; ++dim) { + result(dim) += kernel.kernel(normalized_segment_distance) * coefficients[dim](segment_index); + } + } + return result; + } +} +; + +// observations: data to be fitted by the curve +// observation points: growing sequence of points where the observations were made. +// In other words, for function f(x) = y, observations are y0...yn, and observation points are x0...xn +// weights: how important the observation is +// number_of_inner_splines: how many full inner splines are fit into the normalized valid range 0,1; +// final number of knots is Kernel::kernel_span times larger + additional segments on start and end +// Kernel: model used for the curve fitting +template +PiecewiseFittedCurve fit_curve( + const std::vector> &observations, + const std::vector &observation_points, + const std::vector &weights, + size_t number_of_inner_splines, + Kernel kernel) { + + // check to make sure inputs are correct + assert(number_of_inner_splines >= 1); + assert(observation_points.size() == observations.size()); + assert(observation_points.size() == weights.size()); + assert(number_of_inner_splines <= observations.size()); + assert(observations.size() >= 4); + + size_t extremes_repetition = Kernel::kernel_span - 1; //how many (additional) times is the first and last point repeated + + //prepare sqrt of weights, which will then be applied to both matrix T and observed data: https://en.wikipedia.org/wiki/Weighted_least_squares + std::vector sqrt_weights(weights.size() + extremes_repetition * 2); + for (size_t index = 0; index < weights.size(); ++index) { + assert(weights[index] > 0); + sqrt_weights[index + extremes_repetition] = sqrt(weights[index]); + } + //repeat weights for addtional segments + for (int index = 0; index < int(extremes_repetition); ++index) { + sqrt_weights[index] = sqrt(weights.front()); + sqrt_weights[sqrt_weights.size() - index - 1] = sqrt(weights.back()); + } + + // prepare result and compute metadata + PiecewiseFittedCurve result { }; + + NumberType original_len = observation_points.back() - observation_points.front(); + NumberType orig_segment_size = original_len / NumberType(number_of_inner_splines * Kernel::kernel_span); + result.kernel = kernel; + result.start = observation_points.front() - extremes_repetition * orig_segment_size; + result.length = observation_points.back() + extremes_repetition * orig_segment_size - result.start; + result.segments_count = number_of_inner_splines * Kernel::kernel_span + extremes_repetition * 2; + result.n_segment_size = NumberType(1) / NumberType(result.segments_count - 1); + + //normalize observations points by start and length + std::vector normalized_obs_points(observation_points.size() + extremes_repetition * 2); + for (size_t index = 0; index < observation_points.size(); ++index) { + normalized_obs_points[index + extremes_repetition] = result.normalize(observation_points[index]); + } + // create artificial values at the extremes for constant curve fitting + for (int index = 0; index < int(extremes_repetition); ++index) { + normalized_obs_points[extremes_repetition - 1 - index] = result.normalize(observation_points.front() + - index * orig_segment_size - NumberType(0.5) * orig_segment_size); + + normalized_obs_points[normalized_obs_points.size() - extremes_repetition + index] = result.normalize( + observation_points.back() + index * orig_segment_size + NumberType(0.5) * orig_segment_size); + } + + // prepare observed data + std::vector> data_points(Dimension); + for (size_t dim = 0; dim < Dimension; ++dim) { + data_points[dim] = Eigen::Matrix( + observations.size() + extremes_repetition * 2); + } + for (size_t index = 0; index < observations.size(); index++) { + for (size_t dim = 0; dim < Dimension; ++dim) { + data_points[dim](index + extremes_repetition) = observations[index](dim) + * sqrt_weights[index + extremes_repetition]; + } + } + //duplicate observed data at the start and end + for (int index = 0; index < int(extremes_repetition); index++) { + for (size_t dim = 0; dim < Dimension; ++dim) { + data_points[dim](index) = observations.front()(dim) * sqrt_weights[index]; + data_points[dim](data_points[dim].size() - index - 1) = observations.back()(dim) + * sqrt_weights[data_points[dim].size() - index - 1]; + } + } + + //Create weight matrix for each point and each segment. + Eigen::MatrixXf T(normalized_obs_points.size(), result.segments_count); + for (size_t i = 0; i < normalized_obs_points.size(); ++i) { + for (size_t j = 0; j < result.segments_count; ++j) { + T(i, j) = NumberType(0); + } + } + + for (size_t i = 0; i < normalized_obs_points.size(); ++i) { + NumberType knot_val = normalized_obs_points[i]; + int start_segment_idx = int(floor(knot_val / result.n_segment_size)) - int(Kernel::kernel_span * 0.5f - 1.0f); + for (int segment_index = start_segment_idx; segment_index < int(start_segment_idx + Kernel::kernel_span); + segment_index++) { + if (segment_index < 0 || segment_index >= int(result.segments_count)) { + continue; + } + NumberType segment_start = result.get_n_segment_start(segment_index); + NumberType normalized_segment_distance = (segment_start - knot_val) / result.n_segment_size; + // fill in kernel value with weight applied + + T(i, segment_index) += kernel.kernel(normalized_segment_distance) * sqrt_weights[i]; + } + } + + // Solve for linear least square fit + std::vector> coefficients(Dimension); + const auto QR = T.fullPivHouseholderQr(); + for (size_t dim = 0; dim < Dimension; ++dim) { + coefficients[dim] = QR.solve(data_points[dim]); + } + + // store coefficients in result result.coefficients = coefficients; return result; } template -PiecewiseFittedCurve> -fit_epanechnikov_curve(const std::vector> &points, - size_t number_of_segments, const NumberType &normalized_kernel_bandwidth) { - return fit_curve(points, number_of_segments, normalized_kernel_bandwidth, - CurveSmoothingKernels::EpanechnikovKernel { }); +PiecewiseFittedCurve> +fit_cubic_bspline( + const std::vector> &observations, + std::vector observation_points, + std::vector weights, + size_t number_of_segments) { + return fit_curve(observations, observation_points, weights, number_of_segments, + CubicBSplineKernel { }); +} + +template +PiecewiseFittedCurve> +fit_catmul_rom_spline( + const std::vector> &observations, + std::vector observation_points, + std::vector weights, + size_t number_of_segments) { + return fit_curve(observations, observation_points, weights, number_of_segments, + CubicCatmulRomKernel { }); } } diff --git a/src/libslic3r/Point.hpp b/src/libslic3r/Point.hpp index 6b430c2fef..09ff533feb 100644 --- a/src/libslic3r/Point.hpp +++ b/src/libslic3r/Point.hpp @@ -28,6 +28,9 @@ using Mat = Eigen::Matrix; template using Vec = Mat; +template +using DynVec = Eigen::Matrix; + // Eigen types, to replace the Slic3r's own types in the future. // Vector types with a fixed point coordinate base type. using Vec2crd = Eigen::Matrix; diff --git a/src/libslic3r/SLA/bicubic.h b/src/libslic3r/SLA/bicubic.h deleted file mode 100644 index 870d00dbd6..0000000000 --- a/src/libslic3r/SLA/bicubic.h +++ /dev/null @@ -1,186 +0,0 @@ -#ifndef BICUBIC_HPP -#define BICUBIC_HPP - -#include -#include -#include - -#include - -namespace Slic3r { - -namespace BicubicInternal { - // Linear kernel, to be able to test cubic methods with hat kernels. - template - struct LinearKernel - { - typedef T FloatType; - - static T a00() { return T(0.); } - static T a01() { return T(0.); } - static T a02() { return T(0.); } - static T a03() { return T(0.); } - static T a10() { return T(1.); } - static T a11() { return T(-1.); } - static T a12() { return T(0.); } - static T a13() { return T(0.); } - static T a20() { return T(0.); } - static T a21() { return T(1.); } - static T a22() { return T(0.); } - static T a23() { return T(0.); } - static T a30() { return T(0.); } - static T a31() { return T(0.); } - static T a32() { return T(0.); } - static T a33() { return T(0.); } - }; - - // Interpolation kernel aka Catmul-Rom aka Keyes kernel. - template - struct CubicCatmulRomKernel - { - typedef T FloatType; - - static T a00() { return 0; } - static T a01() { return (T)-0.5; } - static T a02() { return (T) 1.; } - static T a03() { return (T)-0.5; } - static T a10() { return (T) 1.; } - static T a11() { return 0; } - static T a12() { return (T)-5./2.; } - static T a13() { return (T) 3./2.; } - static T a20() { return 0; } - static T a21() { return (T) 0.5; } - static T a22() { return (T) 2.; } - static T a23() { return (T)-3./2.; } - static T a30() { return 0; } - static T a31() { return 0; } - static T a32() { return (T)-0.5; } - static T a33() { return (T) 0.5; } - }; - - // B-spline kernel - template - struct CubicBSplineKernel - { - typedef T FloatType; - - static T a00() { return (T) 1./6.; } - static T a01() { return (T) -3./6.; } - static T a02() { return (T) 3./6.; } - static T a03() { return (T) -1./6.; } - static T a10() { return (T) 4./6.; } - static T a11() { return 0; } - static T a12() { return (T) -6./6.; } - static T a13() { return (T) 3./6.; } - static T a20() { return (T) 1./6.; } - static T a21() { return (T) 3./6.; } - static T a22() { return (T) 3./6.; } - static T a23() { return (T)- 3./6.; } - static T a30() { return 0; } - static T a31() { return 0; } - static T a32() { return 0; } - static T a33() { return (T) 1./6.; } - }; - - template - inline T clamp(T a, T lower, T upper) - { - return (a < lower) ? lower : - (a > upper) ? upper : a; - } -} - -template -struct CubicKernel -{ - typedef typename KERNEL KernelInternal; - typedef typename KERNEL::FloatType FloatType; - - static FloatType kernel(FloatType x) - { - x = fabs(x); - if (x >= (FloatType)2.) - return 0.0f; - if (x <= (FloatType)1.) { - FloatType x2 = x * x; - FloatType x3 = x2 * x; - return KERNEL::a10() + KERNEL::a11() * x + KERNEL::a12() * x2 + KERNEL::a13() * x3; - } - assert(x > (FloatType)1. && x < (FloatType)2.); - x -= (FloatType)1.; - FloatType x2 = x * x; - FloatType x3 = x2 * x; - return KERNEL::a00() + KERNEL::a01() * x + KERNEL::a02() * x2 + KERNEL::a03() * x3; - } - - static FloatType interpolate(FloatType f0, FloatType f1, FloatType f2, FloatType f3, FloatType x) - { - const FloatType x2 = x*x; - const FloatType x3 = x*x*x; - return f0*(KERNEL::a00() + KERNEL::a01() * x + KERNEL::a02() * x2 + KERNEL::a03() * x3) + - f1*(KERNEL::a10() + KERNEL::a11() * x + KERNEL::a12() * x2 + KERNEL::a13() * x3) + - f2*(KERNEL::a20() + KERNEL::a21() * x + KERNEL::a22() * x2 + KERNEL::a23() * x3) + - f3*(KERNEL::a30() + KERNEL::a31() * x + KERNEL::a32() * x2 + KERNEL::a33() * x3); - } -}; - -// Linear splines -typedef CubicKernel> LinearKernelf; -typedef CubicKernel> LinearKerneld; -// Catmul-Rom splines -typedef CubicKernel> CubicCatmulRomKernelf; -typedef CubicKernel> CubicCatmulRomKerneld; -typedef CubicKernel> CubicInterpolationKernelf; -typedef CubicKernel> CubicInterpolationKerneld; -// Cubic B-splines -typedef CubicKernel> CubicBSplineKernelf; -typedef CubicKernel> CubicBSplineKerneld; - -template -static float cubic_interpolate(const Eigen::ArrayBase &F, const typename KERNEL::FloatType pt, const typename KERNEL::FloatType dx) -{ - typedef typename KERNEL::FloatType T; - const int w = int(F.size()); - const int ix = (int)floor(pt); - const T s = pt - (T)ix; - - if (ix > 1 && ix + 2 < w) { - // Inside the fully interpolated region. - return KERNEL::interpolate(F[ix - 1], F[ix], F[ix + 1], F[ix + 2], s); - } - // Transition region. Extend with a constant function. - auto f = [&F, w](x) { return F[BicubicInternal::clamp(x, 0, w - 1)]; } - return KERNEL::interpolate(f(ix - 1), f(ix), f(ix + 1), f(ix + 2), s); -} - -template -static float bicubic_interpolate(const Eigen::MatrixBase &F, const Eigen::Matrix &pt, const typename KERNEL::FloatType dx) -{ - typedef typename KERNEL::FloatType T; - const int w = F.cols(); - const int h = F.rows(); - const int ix = (int)floor(pt[0]); - const int iy = (int)floor(pt[1]); - const T s = pt[0] - (T)ix; - const T t = pt[1] - (T)iy; - - if (ix > 1 && ix + 2 < w && iy > 1 && iy + 2 < h) { - // Inside the fully interpolated region. - return KERNEL::interpolate( - KERNEL::interpolate(F(ix-1,iy-1),F(ix ,iy-1),F(ix+1,iy-1),F(ix+2,iy-1),s), - KERNEL::interpolate(F(ix-1,iy ),F(ix ,iy ),F(ix+1,iy ),F(ix+2,iy ),s), - KERNEL::interpolate(F(ix-1,iy+1),F(ix ,iy+1),F(ix+1,iy+1),F(ix+2,iy+1),s), - KERNEL::interpolate(F(ix-1,iy+2),F(ix ,iy+2),F(ix+1,iy+2),F(ix+2,iy+2),s),t); - } - // Transition region. Extend with a constant function. - auto f = [&f, w, h](int x, int y) { return F(BicubicInternal::clamp(x,0,w-1),BicubicInternal::clamp(y,0,h-1)); } - return KERNEL::interpolate( - KERNEL::interpolate(f(ix-1,iy-1),f(ix ,iy-1),f(ix+1,iy-1),f(ix+2,iy-1),s), - KERNEL::interpolate(f(ix-1,iy ),f(ix ,iy ),f(ix+1,iy ),f(ix+2,iy ),s), - KERNEL::interpolate(f(ix-1,iy+1),f(ix ,iy+1),f(ix+1,iy+1),f(ix+2,iy+1),s), - KERNEL::interpolate(f(ix-1,iy+2),f(ix ,iy+2),f(ix+1,iy+2),f(ix+2,iy+2),s),t); -} - -} // namespace Slic3r - -#endif /* BICUBIC_HPP */ diff --git a/tests/libslic3r/test_curve_fitting.cpp b/tests/libslic3r/test_curve_fitting.cpp index 9968e473a1..a38e9b5a06 100644 --- a/tests/libslic3r/test_curve_fitting.cpp +++ b/tests/libslic3r/test_curve_fitting.cpp @@ -2,61 +2,117 @@ #include #include +#include +#include -TEST_CASE("Curves: constant kernel fitting", "[Curves]") { +TEST_CASE("Curves: cubic b spline fit test", "[Curves]") { using namespace Slic3r; using namespace Slic3r::Geometry; - std::vector> points { Vec<1, float> { 0 }, Vec<1, float> { 2 }, Vec<1, float> { 7 } }; - size_t number_of_segments = 2; - float normalized_kernel_bandwidth = 1.0f; - auto kernel = CurveSmoothingKernels::ConstantKernel { }; - auto curve = fit_curve(points, number_of_segments, normalized_kernel_bandwidth, kernel); + auto fx = [&](size_t index) { + return float(index) / 200.0f; + }; - REQUIRE(curve.length == Approx(7.0f)); - REQUIRE(curve.coefficients[0].size() == number_of_segments); - REQUIRE(curve.coefficients[0][0] == Approx(1.0f)); - REQUIRE(curve.coefficients[0][1] == Approx(7.0f)); + auto fy = [&](size_t index) { + return 1.0f; + }; - REQUIRE(curve.get_fitted_point(0.33)[0] == Approx(1.0f)); + std::vector> observations { }; + std::vector observation_points { }; + std::vector weights { }; + for (size_t index = 0; index < 200; ++index) { + observations.push_back(Vec<1, float> { fy(index) }); + observation_points.push_back(fx(index)); + weights.push_back(1); + } + + Vec2f fmin { fx(0), fy(0) }; + Vec2f fmax { fx(200), fy(200) }; + + auto bspline = fit_cubic_bspline(observations, observation_points, weights, 1); + + Approx ap(1.0f); + ap.epsilon(0.1f); + + for (int p = 0; p < 200; ++p) { + float fitted_val = bspline.get_fitted_value(fx(p))(0); + float expected = fy(p); + + REQUIRE(fitted_val == ap(expected)); + + } } -TEST_CASE("Curves: constant kernel fitting 2", "[Curves]") { +TEST_CASE("Curves: quadratic f cubic b spline fit test", "[Curves]") { using namespace Slic3r; using namespace Slic3r::Geometry; - std::vector> points { Vec<1, float> { 0 }, Vec<1, float> { 2 }, Vec<1, float> { 2 }, - Vec<1, float> { 4 } }; - size_t number_of_segments = 2; - float normalized_kernel_bandwidth = 2.0f; - auto kernel = CurveSmoothingKernels::ConstantKernel { }; - auto curve = fit_curve(points, number_of_segments, normalized_kernel_bandwidth, kernel); + auto fx = [&](size_t index) { + return float(index) / 100.0f; + }; - REQUIRE(curve.length == Approx(4.0f)); - REQUIRE(curve.coefficients[0].size() == number_of_segments); - REQUIRE(curve.get_fitted_point(0.33)[0] == Approx(2.0f)); + auto fy = [&](size_t index) { + return (fx(index) - 1) * (fx(index) - 1); + }; + + std::vector> observations { }; + std::vector observation_points { }; + std::vector weights { }; + for (size_t index = 0; index < 200; ++index) { + observations.push_back(Vec<1, float> { fy(index) }); + observation_points.push_back(fx(index)); + weights.push_back(1); + } + + Vec2f fmin { fx(0), fy(0) }; + Vec2f fmax { fx(200), fy(200) }; + + auto bspline = fit_cubic_bspline(observations, observation_points, weights, 10); + + for (int p = 0; p < 200; ++p) { + float fitted_val = bspline.get_fitted_value(fx(p))(0); + float expected = fy(p); + + auto check = [](float a, float b) { + return abs(a - b) < 0.2f; + }; + //Note: checking is problematic, splines will not perfectly align + REQUIRE(check(fitted_val, expected)); + + } } -TEST_CASE("Curves: 2D constant kernel fitting", "[Curves]") { +TEST_CASE("Curves: polynomial fit test", "[Curves]") { using namespace Slic3r; using namespace Slic3r::Geometry; - std::vector points { Vec2f { 0, 0 }, Vec2f { 2, 1 }, Vec2f { 4, 2 }, Vec2f { 6, 3 } }; - size_t number_of_segments = 4; - float normalized_kernel_bandwidth = 0.49f; - auto kernel = CurveSmoothingKernels::ConstantKernel { }; - auto curve = fit_curve(points, number_of_segments, normalized_kernel_bandwidth, kernel); + auto fx = [&](size_t index) { + return float(index) / 100.0f; + }; - REQUIRE(curve.length == Approx(sqrt(6 * 6 + 3 * 3))); - REQUIRE(curve.coefficients.size() == 2); - REQUIRE(curve.coefficients[0].size() == number_of_segments); - REQUIRE(curve.coefficients[0][0] == Approx(0.0f)); - REQUIRE(curve.coefficients[0][1] == Approx(2.0f)); - REQUIRE(curve.coefficients[0][2] == Approx(4.0f)); - REQUIRE(curve.coefficients[0][3] == Approx(6.0f)); + auto fy = [&](size_t index) { + return (fx(index) - 1) * (fx(index) - 1); + }; - REQUIRE(curve.coefficients[1][0] == Approx(0.0f)); - REQUIRE(curve.coefficients[1][1] == Approx(1.0f)); - REQUIRE(curve.coefficients[1][2] == Approx(2.0f)); - REQUIRE(curve.coefficients[1][3] == Approx(3.0f)); + std::vector> observations { }; + std::vector observation_points { }; + std::vector weights { }; + for (size_t index = 0; index < 200; ++index) { + observations.push_back(Vec<1, float> { fy(index) }); + observation_points.push_back(fx(index)); + weights.push_back(1); + } + + Vec2f fmin { fx(0), fy(0) }; + Vec2f fmax { fx(200), fy(200) }; + + Approx ap(1.0f); + ap.epsilon(0.1f); + + auto poly = fit_polynomial(observations, observation_points, weights, 2); + + REQUIRE(poly.coefficients[0](0) == ap(1)); + REQUIRE(poly.coefficients[0](1) == ap(-2)); + REQUIRE(poly.coefficients[0](2) == ap(1)); } +