Implemented piecewise data (curve) fitting with variable kernels

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

src/libslic3r/GCode/SeamPlacer.cpp

@@ -8,10 +8,6 @@
 #include <algorithm>
 #include <queue>
 
-//For polynomial fitting
-#include <Eigen/Dense>
-#include <Eigen/QR>
-
 #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<int>();
 }
 
-//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<Vec2f> polyfit(const std::vector<Vec3f> &points, const std::vector<float> &weights, size_t order) {
-    // check to make sure inputs are correct
-    assert(points.size() >= order + 1);
-    assert(points.size() == weights.size());
-
-    std::vector<float> 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<Vec2f> 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<Vec2f> &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:

src/libslic3r/Geometry/Curves.cpp

@@ -0,0 +1,65 @@
+#include <Eigen/Dense>
+#include <Eigen/QR>
+#include "Curves.hpp"
+
+
+namespace Slic3r {
+namespace Geometry {
+
+PolynomialCurve fit_polynomial(const std::vector<Vec3f> &points, const std::vector<float> &weights, size_t order) {
+    // check to make sure inputs are correct
+    assert(points.size() >= order + 1);
+    assert(points.size() == weights.size());
+
+    std::vector<float> 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<Vec2f> 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 };
+}
+
+}
+}

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 <iostream>
+
+namespace Slic3r {
+namespace Geometry {
+
+struct PolynomialCurve {
+    std::vector<Vec2f> coefficients;
+
+    explicit PolynomialCurve(const std::vector<Vec2f> &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<Vec3f> &points, const std::vector<float> &weights, size_t order);
+
+namespace CurveSmoothingKernels {
+//NOTE: Kernel functions are used in range [-1,1].
+
+// konstant kernel is used mainly for tests
+template<typename NumberType>
+struct ConstantKernel {
+    float operator()(NumberType w) const {
+        return NumberType(1);
+    }
+};
+
+//https://en.wikipedia.org/wiki/Kernel_(statistics)
+template<typename NumberType>
+struct EpanechnikovKernel {
+    float operator()(NumberType w) const {
+        return NumberType(0.25) * (NumberType(1) - w * w);
+    }
+};
+
+}
+
+template<typename NumberType>
+using VecX = Eigen::Matrix<NumberType, Eigen::Dynamic, 1>;
+
+template<int Dimension, typename NumberType, typename Kernel>
+struct PiecewiseFittedCurve {
+    std::vector<VecX<NumberType>> 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<Dimension, NumberType> get_fitted_point(const NumberType &t) const {
+        Vec<Dimension, NumberType> 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<int Dimension, typename NumberType, typename Kernel>
+PiecewiseFittedCurve<Dimension, NumberType, Kernel> fit_curve(const std::vector<Vec<Dimension, NumberType>> &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<NumberType> 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<Dimension, NumberType, Kernel> 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<VecX<NumberType>> data_points(Dimension);
+    for (size_t dim = 0; dim < Dimension; ++dim) {
+        data_points[dim] = Eigen::Matrix<NumberType, Eigen::Dynamic, 1>(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<VecX<NumberType>> 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<int Dimension, typename NumberType>
+PiecewiseFittedCurve<Dimension, NumberType, CurveSmoothingKernels::EpanechnikovKernel<NumberType>>
+fit_epanechnikov_curve(const std::vector<Vec<Dimension, NumberType>> &points,
+        size_t number_of_segments, const NumberType &normalized_kernel_bandwidth) {
+    return fit_curve(points, number_of_segments, normalized_kernel_bandwidth,
+            CurveSmoothingKernels::EpanechnikovKernel<NumberType> { });
+}
+
+}
+}
+
+#endif /* SRC_LIBSLIC3R_GEOMETRY_CURVES_HPP_ */

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

tests/libslic3r/test_curve_fitting.cpp

@@ -0,0 +1,62 @@
+#include <catch2/catch.hpp>
+#include <test_utils.hpp>
+
+#include <libslic3r/Geometry/Curves.hpp>
+
+TEST_CASE("Curves: constant kernel fitting", "[Curves]") {
+    using namespace Slic3r;
+    using namespace Slic3r::Geometry;
+
+    std::vector<Vec<1, float>> 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<float> { };
+    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<Vec<1, float>> 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<float> { };
+    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<Vec2f> 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<float> { };
+    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));
+}