From bbcd6be25065c5ea8d84809d33e355bf2f54d6e3 Mon Sep 17 00:00:00 2001 From: PavelMikus Date: Tue, 15 Mar 2022 14:52:55 +0100 Subject: [PATCH] Implemented piecewise data (curve) fitting with variable kernels --- src/libslic3r/CMakeLists.txt | 2 + src/libslic3r/GCode/SeamPlacer.cpp | 60 ---------- src/libslic3r/Geometry/Curves.cpp | 65 ++++++++++ src/libslic3r/Geometry/Curves.hpp | 160 +++++++++++++++++++++++++ tests/libslic3r/CMakeLists.txt | 1 + tests/libslic3r/test_curve_fitting.cpp | 62 ++++++++++ 6 files changed, 290 insertions(+), 60 deletions(-) create mode 100644 src/libslic3r/Geometry/Curves.cpp create mode 100644 src/libslic3r/Geometry/Curves.hpp create mode 100644 tests/libslic3r/test_curve_fitting.cpp diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index 6ec40f42a..bc7f36fc9 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -142,6 +142,8 @@ set(SLIC3R_SOURCES Geometry/Circle.hpp Geometry/ConvexHull.cpp Geometry/ConvexHull.hpp + Geometry/Curves.cpp + Geometry/Curves.hpp Geometry/MedialAxis.cpp Geometry/MedialAxis.hpp Geometry/Voronoi.hpp diff --git a/src/libslic3r/GCode/SeamPlacer.cpp b/src/libslic3r/GCode/SeamPlacer.cpp index 0e04127a9..d861b16e7 100644 --- a/src/libslic3r/GCode/SeamPlacer.cpp +++ b/src/libslic3r/GCode/SeamPlacer.cpp @@ -8,10 +8,6 @@ #include #include -//For polynomial fitting -#include -#include - #include "libslic3r/ExtrusionEntity.hpp" #include "libslic3r/Print.hpp" #include "libslic3r/BoundingBox.hpp" @@ -57,62 +53,6 @@ Vec3i value_rgbi(float minimum, float maximum, float value) { return (value_to_rgbf(minimum, maximum, value) * 255).cast(); } -//https://towardsdatascience.com/least-square-polynomial-fitting-using-c-eigen-package-c0673728bd01 -// interpolates points in z (treats z coordinates as time) and returns coefficients for axis x and y -std::vector polyfit(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 coeff; -} - -Vec3f get_fitted_point(const std::vector &coefficients, float z) { - size_t order = 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 += coefficients[index].x() * z_pow; - fitted_y += coefficients[index].y() * z_pow; - } - - return Vec3f { fitted_x, fitted_y, z }; -} - /// Coordinate frame class Frame { public: diff --git a/src/libslic3r/Geometry/Curves.cpp b/src/libslic3r/Geometry/Curves.cpp new file mode 100644 index 000000000..fa95926be --- /dev/null +++ b/src/libslic3r/Geometry/Curves.cpp @@ -0,0 +1,65 @@ +#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 new file mode 100644 index 000000000..3c102f192 --- /dev/null +++ b/src/libslic3r/Geometry/Curves.hpp @@ -0,0 +1,160 @@ +#ifndef SRC_LIBSLIC3R_GEOMETRY_CURVES_HPP_ +#define SRC_LIBSLIC3R_GEOMETRY_CURVES_HPP_ + +#include "libslic3r/Point.hpp" + +#include + +namespace Slic3r { +namespace Geometry { + +struct PolynomialCurve { + std::vector coefficients; + + explicit PolynomialCurve(const 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]; + } + } + } + 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) { + + // 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); + + //normalize knots + for (NumberType &knot : knots) { + knot = knot / length; + } + + 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); + for (size_t dim = 0; dim < Dimension; ++dim) { + data_points[dim] = Eigen::Matrix(points.size()); + } + + for (size_t index = 0; index < points.size(); index++) { + for (size_t dim = 0; dim < Dimension; ++dim) { + data_points[dim](index) = points[index](dim); + } + } + + 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; + } + } + } + + // 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]); + } + + 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 { }); +} + +} +} + +#endif /* SRC_LIBSLIC3R_GEOMETRY_CURVES_HPP_ */ diff --git a/tests/libslic3r/CMakeLists.txt b/tests/libslic3r/CMakeLists.txt index 69470aad1..248245182 100644 --- a/tests/libslic3r/CMakeLists.txt +++ b/tests/libslic3r/CMakeLists.txt @@ -8,6 +8,7 @@ add_executable(${_TEST_NAME}_tests test_clipper_utils.cpp test_color.cpp test_config.cpp + test_curve_fitting.cpp test_elephant_foot_compensation.cpp test_geometry.cpp test_placeholder_parser.cpp diff --git a/tests/libslic3r/test_curve_fitting.cpp b/tests/libslic3r/test_curve_fitting.cpp new file mode 100644 index 000000000..9968e473a --- /dev/null +++ b/tests/libslic3r/test_curve_fitting.cpp @@ -0,0 +1,62 @@ +#include +#include + +#include + +TEST_CASE("Curves: constant kernel fitting", "[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); + + 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)); + + REQUIRE(curve.get_fitted_point(0.33)[0] == Approx(1.0f)); +} + +TEST_CASE("Curves: constant kernel fitting 2", "[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); + + 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)); +} + +TEST_CASE("Curves: 2D constant kernel fitting", "[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); + + 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)); + + 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)); +}