From b43b0a1a8211b43b33101019b12945c87d078661 Mon Sep 17 00:00:00 2001 From: Joseph Lenox Date: Sun, 29 Jul 2018 20:10:56 -0500 Subject: [PATCH] Added implementation of Taubin fit cribbed from "Circular and Linear Regression: Fitting circles and lines by least squares", p126 by Nikolai Chernov for both Pointf and Points, Added tests for fit and extended some operators to function on Pointf --- src/test/libslic3r/test_geometry.cpp | 92 +++++++++++++++++++++++ xs/src/libslic3r/Geometry.cpp | 105 +++++++++++++++++++++++++++ xs/src/libslic3r/Geometry.hpp | 8 ++ xs/src/libslic3r/Point.cpp | 29 +++++++- xs/src/libslic3r/Point.hpp | 7 +- 5 files changed, 239 insertions(+), 2 deletions(-) diff --git a/src/test/libslic3r/test_geometry.cpp b/src/test/libslic3r/test_geometry.cpp index c8c2a2163..a65e85313 100644 --- a/src/test/libslic3r/test_geometry.cpp +++ b/src/test/libslic3r/test_geometry.cpp @@ -161,6 +161,98 @@ TEST_CASE("Offseting a line generates a polygon correctly"){ REQUIRE(area.area() == Polygon(std::vector({Point(10,5),Point(20,5),Point(20,15),Point(10,15)})).area()); } +SCENARIO("Circle Fit, TaubinFit with Newton's method") { + GIVEN("A vector of Pointfs arranged in a half-circle with approximately the same distance R from some point") { + Pointf expected_center(-6, 0); + Pointfs sample {Pointf(6.0, 0), Pointf(5.1961524, 3), Pointf(3 ,5.1961524), Pointf(0, 6.0), Pointf(3, 5.1961524), Pointf(-5.1961524, 3), Pointf(-6.0, 0)}; + std::transform(sample.begin(), sample.end(), sample.begin(), [expected_center] (const Pointf& a) { return a + expected_center;}); + + WHEN("Circle fit is called on the entire array") { + Pointf result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample); + THEN("A center point of -6,0 is returned.") { + REQUIRE(result_center == expected_center); + } + } + WHEN("Circle fit is called on the first four points") { + Pointf result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample.cbegin(), sample.cbegin()+4); + THEN("A center point of -6,0 is returned.") { + REQUIRE(result_center == expected_center); + } + } + WHEN("Circle fit is called on the middle four points") { + Pointf result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample.cbegin()+2, sample.cbegin()+6); + THEN("A center point of -6,0 is returned.") { + REQUIRE(result_center == expected_center); + } + } + } + GIVEN("A vector of Pointfs arranged in a half-circle with approximately the same distance R from some point") { + Pointf expected_center(-3, 9); + Pointfs sample {Pointf(6.0, 0), Pointf(5.1961524, 3), Pointf(3 ,5.1961524), + Pointf(0, 6.0), + Pointf(3, 5.1961524), Pointf(-5.1961524, 3), Pointf(-6.0, 0)}; + + std::transform(sample.begin(), sample.end(), sample.begin(), [expected_center] (const Pointf& a) { return a + expected_center;}); + + + WHEN("Circle fit is called on the entire array") { + Pointf result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample); + THEN("A center point of 3,9 is returned.") { + REQUIRE(result_center == expected_center); + } + } + WHEN("Circle fit is called on the first four points") { + Pointf result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample.cbegin(), sample.cbegin()+4); + THEN("A center point of 3,9 is returned.") { + REQUIRE(result_center == expected_center); + } + } + WHEN("Circle fit is called on the middle four points") { + Pointf result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample.cbegin()+2, sample.cbegin()+6); + THEN("A center point of 3,9 is returned.") { + REQUIRE(result_center == expected_center); + } + } + } + GIVEN("A vector of Points arranged in a half-circle with approximately the same distance R from some point") { + Point expected_center { Point::new_scale(-3, 9)}; + Points sample {Point::new_scale(6.0, 0), Point::new_scale(5.1961524, 3), Point::new_scale(3 ,5.1961524), + Point::new_scale(0, 6.0), + Point::new_scale(3, 5.1961524), Point::new_scale(-5.1961524, 3), Point::new_scale(-6.0, 0)}; + + std::transform(sample.begin(), sample.end(), sample.begin(), [expected_center] (const Point& a) { return a + expected_center;}); + + + WHEN("Circle fit is called on the entire array") { + Point result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample); + THEN("A center point of scaled 3,9 is returned.") { + REQUIRE(result_center.coincides_with_epsilon(expected_center)); + } + } + WHEN("Circle fit is called on the first four points") { + Point result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample.cbegin(), sample.cbegin()+4); + THEN("A center point of scaled 3,9 is returned.") { + REQUIRE(result_center.coincides_with_epsilon(expected_center)); + } + } + WHEN("Circle fit is called on the middle four points") { + Point result_center(0,0); + result_center = Geometry::circle_taubin_newton(sample.cbegin()+2, sample.cbegin()+6); + THEN("A center point of scaled 3,9 is returned.") { + REQUIRE(result_center.coincides_with_epsilon(expected_center)); + } + } + } +} + TEST_CASE("Chained path working correctly"){ // if chained_path() works correctly, these points should be joined with no diagonal paths diff --git a/xs/src/libslic3r/Geometry.cpp b/xs/src/libslic3r/Geometry.cpp index 041132828..b972e7e17 100644 --- a/xs/src/libslic3r/Geometry.cpp +++ b/xs/src/libslic3r/Geometry.cpp @@ -2,9 +2,11 @@ #include "ClipperUtils.hpp" #include "ExPolygon.hpp" #include "Line.hpp" +#include "Log.hpp" #include "PolylineCollection.hpp" #include "clipper.hpp" #include +#include #include #include #include @@ -343,6 +345,109 @@ linint(double value, double oldmin, double oldmax, double newmin, double newmax) return (value - oldmin) * (newmax - newmin) / (oldmax - oldmin) + newmin; } +Point +circle_taubin_newton(const Points& input, size_t cycles) +{ + return circle_taubin_newton(input.cbegin(), input.cend(), cycles); +} + +Point +circle_taubin_newton(const Points::const_iterator& input_begin, const Points::const_iterator& input_end, size_t cycles) +{ + Pointfs tmp; + tmp.reserve(std::distance(input_begin, input_end)); + std::transform(input_begin, input_end, std::back_inserter(tmp), [] (const Point& in) {return Pointf::new_unscale(in); } ); + return Point::new_scale(circle_taubin_newton(tmp.cbegin(), tmp.end(), cycles)); +} + +Pointf +circle_taubin_newton(const Pointfs& input, size_t cycles) +{ + return circle_taubin_newton(input.cbegin(), input.cend(), cycles); +} + + +/// Adapted from work in "Circular and Linear Regression: Fitting circles and lines by least squares", pg 126 +/// Returns a point corresponding to the center of a circle for which all of the points from input_begin to input_end +/// lie on. +Pointf +circle_taubin_newton(const Pointfs::const_iterator& input_begin, const Pointfs::const_iterator& input_end, size_t cycles) +{ + // calculate the centroid of the data set + const Pointf sum = std::accumulate(input_begin, input_end, Pointf(0,0)); + const size_t n = std::distance(input_begin, input_end); + const double n_flt = static_cast(n); + const Pointf centroid { sum / n_flt }; + + // Compute the normalized moments of the data set. + double Mxx = 0, Myy = 0, Mxy = 0, Mxz = 0, Myz = 0, Mzz = 0; + for (auto it = input_begin; it < input_end; ++it) { + // center/normalize the data. + double Xi {it->x - centroid.x}; + double Yi {it->y - centroid.y}; + double Zi {Xi*Xi + Yi*Yi}; + Mxy += (Xi*Yi); + Mxx += (Xi*Xi); + Myy += (Yi*Yi); + Mxz += (Xi*Zi); + Myz += (Yi*Zi); + Mzz += (Zi*Zi); + } + + // divide by number of points to get the moments + Mxx /= n_flt; + Myy /= n_flt; + Mxy /= n_flt; + Mxz /= n_flt; + Myz /= n_flt; + Mzz /= n_flt; + + // Compute the coefficients of the characteristic polynomial for the circle + // eq 5.60 + const double Mz {Mxx + Myy}; // xx + yy = z + const double Cov_xy {Mxx*Myy - Mxy*Mxy}; // this shows up a couple times so cache it here. + const double C3 {4.0*Mz}; + const double C2 {-3.0*(Mz*Mz) - Mzz}; + const double C1 {Mz*(Mzz - (Mz*Mz)) + 4.0*Mz*Cov_xy - (Mxz*Mxz) - (Myz*Myz)}; + const double C0 {(Mxz*Mxz)*Myy + (Myz*Myz)*Mxx - 2.0*Mxz*Myz*Mxy - Cov_xy*(Mzz - (Mz*Mz))}; + + const double C22 = {C2 + C2}; + const double C33 = {C3 + C3 + C3}; + + // solve the characteristic polynomial with Newton's method. + double xnew = 0.0; + double ynew = 1e20; + + for (size_t i = 0; i < cycles; ++i) { + const double yold {ynew}; + ynew = C0 + xnew * (C1 + xnew*(C2 + xnew * C3)); + if (std::abs(ynew) > std::abs(yold)) { + Slic3r::Log::error("Geometry") << "Fit is going in the wrong direction.\n"; + return Pointf(std::nan(""), std::nan("")); + } + const double Dy {C1 + xnew*(C22 + xnew*C33)}; + + const double xold {xnew}; + xnew = xold - (ynew / Dy); + + if (std::abs((xnew-xold) / xnew) < 1e-12) i = cycles; // converged, we're done here + + if (xnew < 0) { + // reset, we went negative + xnew = 0.0; + } + } + + // compute the determinant and the circle's parameters now that we've solved. + double DET = xnew*xnew - xnew*Mz + Cov_xy; + + Pointf center {Pointf(Mxz * (Myy - xnew) - Myz * Mxy, Myz * (Mxx - xnew) - Mxz*Mxy)}; + center = center / DET / 2; + + return center + centroid; + +} + /* == Perl implementations for methods tested in geometry.t but not translated. == == The first three are unreachable in the current perl code and the fourth is == diff --git a/xs/src/libslic3r/Geometry.hpp b/xs/src/libslic3r/Geometry.hpp index a64fdcd5d..c2c0d66da 100644 --- a/xs/src/libslic3r/Geometry.hpp +++ b/xs/src/libslic3r/Geometry.hpp @@ -25,6 +25,14 @@ double rad2deg(double angle); double rad2deg_dir(double angle); double deg2rad(double angle); +/// Find the center of the circle corresponding to the vector of Points as an arc. +Point circle_taubin_newton(const Points& input, size_t cycles = 20); +Point circle_taubin_newton(const Points::const_iterator& input_start, const Points::const_iterator& input_end, size_t cycles = 20); + +/// Find the center of the circle corresponding to the vector of Pointfs as an arc. +Pointf circle_taubin_newton(const Pointfs& input, size_t cycles = 20); +Pointf circle_taubin_newton(const Pointfs::const_iterator& input_start, const Pointfs::const_iterator& input_end, size_t cycles = 20); + /// Epsilon value constexpr double epsilon { 1e-4 }; constexpr coord_t scaled_epsilon { static_cast(epsilon / SCALING_FACTOR) }; diff --git a/xs/src/libslic3r/Point.cpp b/xs/src/libslic3r/Point.cpp index d1ece5de5..c67dadb3b 100644 --- a/xs/src/libslic3r/Point.cpp +++ b/xs/src/libslic3r/Point.cpp @@ -337,6 +337,9 @@ operator+(const Point& point1, const Point& point2) return Point(point1.x + point2.x, point1.y + point2.y); } + + + Point operator-(const Point& point1, const Point& point2) { @@ -371,6 +374,18 @@ Pointf::dump_perl() const return ss.str(); } +Pointf +operator+(const Pointf& point1, const Pointf& point2) +{ + return Pointf(point1.x + point2.x, point1.y + point2.y); +} + +Pointf +operator/(const Pointf& point1, const double& scalar) +{ + return Pointf(point1.x / scalar, point1.y / scalar); +} + void Pointf::scale(double factor) { @@ -425,6 +440,14 @@ Pointf::vector_to(const Pointf &point) const return Vectorf(point.x - this->x, point.y - this->y); } +Pointf& +Pointf::operator/=(const double& scalar) +{ + this->x /= scalar; + this->y /= scalar; + return *this; +} + std::ostream& operator<<(std::ostream &stm, const Pointf3 &pointf3) { @@ -437,11 +460,15 @@ Pointf3::scale(double factor) Pointf::scale(factor); this->z *= factor; } +bool Pointf::coincides_with_epsilon(const Pointf& rhs) const { + return std::abs(this->x - rhs.x) < EPSILON && std::abs(this->y - rhs.y) < EPSILON ; +} bool Pointf::operator==(const Pointf& rhs) const { - return Point::new_scale(*this) == Point::new_scale(rhs); + return this->coincides_with_epsilon(rhs); } + void Pointf3::translate(const Vectorf3 &vector) { diff --git a/xs/src/libslic3r/Point.hpp b/xs/src/libslic3r/Point.hpp index 054ac68ab..4ffe1ddfc 100644 --- a/xs/src/libslic3r/Point.hpp +++ b/xs/src/libslic3r/Point.hpp @@ -122,8 +122,10 @@ class Pointf return Pointf(unscale(p.x), unscale(p.y)); }; - // equality operator based on the scaled coordinates + // equality operator based on coincides_with_epsilon bool operator==(const Pointf& rhs) const; + bool coincides_with_epsilon(const Pointf& rhs) const; + Pointf& operator/=(const double& scalar); std::string wkt() const; std::string dump_perl() const; @@ -136,6 +138,9 @@ class Pointf Vectorf vector_to(const Pointf &point) const; }; +Pointf operator+(const Pointf& point1, const Pointf& point2); +Pointf operator/(const Pointf& point1, const double& scalar); + std::ostream& operator<<(std::ostream &stm, const Pointf3 &pointf3); class Pointf3 : public Pointf