mirror of
https://git.mirrors.martin98.com/https://github.com/prusa3d/PrusaSlicer.git
synced 2025-08-01 05:01:59 +08:00
Arc fitting with linear (not very useful) and non-linear least squares.
This commit is contained in:
Vojtech Bubnik
parent
0027d4c5f1
commit
42e7dac1ca
@ -7,7 +7,7 @@
|
||||
|
||||
namespace Slic3r { namespace Geometry { namespace ArcWelder {
|
||||
|
||||
// Calculate center point of an arc given two points and a radius.
|
||||
// Calculate center point (center of a circle) of an arc given two points and a radius.
|
||||
// positive radius: take shorter arc
|
||||
// negative radius: take longer arc
|
||||
// radius must NOT be zero!
|
||||
@ -36,6 +36,37 @@ inline Eigen::Matrix<Float, 2, 1, Eigen::DontAlign> arc_center(
|
||||
return (radius > Float(0)) == is_ccw ? (mid + vp).eval() : (mid - vp).eval();
|
||||
}
|
||||
|
||||
// Calculate middle sample point (point on an arc) of an arc given two points and a radius.
|
||||
// positive radius: take shorter arc
|
||||
// negative radius: take longer arc
|
||||
// radius must NOT be zero!
|
||||
// Taking a sample at the middle of a convex arc (less than PI) is likely much more
|
||||
// useful than taking a sample at the middle of a concave arc (more than PI).
|
||||
template<typename Derived, typename Derived2, typename Float>
|
||||
inline Eigen::Matrix<Float, 2, 1, Eigen::DontAlign> arc_middle_point(
|
||||
const Eigen::MatrixBase<Derived> &start_pos,
|
||||
const Eigen::MatrixBase<Derived2> &end_pos,
|
||||
const Float radius,
|
||||
const bool is_ccw)
|
||||
{
|
||||
static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_center(): first parameter is not a 2D vector");
|
||||
static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_center(): second parameter is not a 2D vector");
|
||||
static_assert(std::is_same<typename Derived::Scalar, typename Derived2::Scalar>::value, "arc_center(): Both vectors must be of the same type.");
|
||||
static_assert(std::is_same<typename Derived::Scalar, Float>::value, "arc_center(): Radius must be of the same type as the vectors.");
|
||||
assert(radius != 0);
|
||||
using Vector = Eigen::Matrix<Float, 2, 1, Eigen::DontAlign>;
|
||||
auto v = end_pos - start_pos;
|
||||
Float q2 = v.squaredNorm();
|
||||
assert(q2 > 0);
|
||||
Float t2 = sqr(radius) / q2 - Float(.25f);
|
||||
// If the start_pos and end_pos are nearly antipodal, t2 may become slightly negative.
|
||||
// In that case return a centroid of start_point & end_point.
|
||||
Float t = (t2 > 0 ? sqrt(t2) : Float(0)) - radius / sqrt(q2);
|
||||
auto mid = Float(0.5) * (start_pos + end_pos);
|
||||
Vector vp{ -v.y() * t, v.x() * t };
|
||||
return (radius > Float(0)) == is_ccw ? (mid + vp).eval() : (mid - vp).eval();
|
||||
}
|
||||
|
||||
// Calculate angle of an arc given two points and a radius.
|
||||
// Returned angle is in the range <0, 2 PI)
|
||||
// positive radius: take shorter arc
|
||||
@ -87,7 +118,7 @@ inline typename Derived::Scalar arc_length(
|
||||
{
|
||||
static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_length(): first parameter is not a 2D vector");
|
||||
static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_length(): second parameter is not a 2D vector");
|
||||
static_assert(Derived3::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_length(): third parameter is not a 2D vector");
|
||||
static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "arc_length(): third parameter is not a 2D vector");
|
||||
static_assert(std::is_same<typename Derived::Scalar, typename Derived2::Scalar>::value &&
|
||||
std::is_same<typename Derived::Scalar, typename Derived3::Scalar>::value, "arc_length(): All third points must be of the same type.");
|
||||
using Float = typename Derived::Scalar;
|
||||
@ -103,6 +134,145 @@ inline typename Derived::Scalar arc_length(
|
||||
return angle * radius;
|
||||
}
|
||||
|
||||
// Be careful! This version has a strong bias towards small circles with small radii
|
||||
// for small angle (nearly straight) arches!
|
||||
// One should rather use arc_fit_center_gauss_newton_ls(), which solves a non-linear least squares problem.
|
||||
//
|
||||
// Calculate center point (center of a circle) of an arc given two fixed points to interpolate
|
||||
// and an additional list of points to fit by least squares.
|
||||
// The circle fitting problem is non-linear, it was linearized by taking difference of squares of radii as a residual.
|
||||
// Therefore the center point is required as a point to linearize at.
|
||||
// Start & end point must be different and the interpolated points must not be collinear with input points.
|
||||
template<typename Derived, typename Derived2, typename Derived3, typename Iterator>
|
||||
inline typename Eigen::Matrix<typename Derived::Scalar, 2, 1, Eigen::DontAlign> arc_fit_center_algebraic_ls(
|
||||
const Eigen::MatrixBase<Derived> &start_pos,
|
||||
const Eigen::MatrixBase<Derived2> &end_pos,
|
||||
const Eigen::MatrixBase<Derived3> ¢er_pos,
|
||||
const Iterator it_begin,
|
||||
const Iterator it_end)
|
||||
{
|
||||
static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_fit_center_algebraic_ls(): start_pos is not a 2D vector");
|
||||
static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_fit_center_algebraic_ls(): end_pos is not a 2D vector");
|
||||
static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "arc_fit_center_algebraic_ls(): third parameter is not a 2D vector");
|
||||
static_assert(std::is_same<typename Derived::Scalar, typename Derived2::Scalar>::value &&
|
||||
std::is_same<typename Derived::Scalar, typename Derived3::Scalar>::value, "arc_fit_center_algebraic_ls(): All third points must be of the same type.");
|
||||
using Float = typename Derived::Scalar;
|
||||
using Vector = Eigen::Matrix<Float, 2, 1, Eigen::DontAlign>;
|
||||
// Prepare a vector space to transform the fitting into:
|
||||
// center_pos, dir_x, dir_y
|
||||
Vector v = end_pos - start_pos;
|
||||
Vector c = Float(.5) * (start_pos + end_pos);
|
||||
Float lv = v.norm();
|
||||
assert(lv > 0);
|
||||
Vector dir_y = v / lv;
|
||||
Vector dir_x = perp(dir_y);
|
||||
// Center of the new system:
|
||||
// Center X at the projection of center_pos
|
||||
Float offset_x = dir_x.dot(center_pos);
|
||||
// Center is supposed to lie on bisector of the arc end points.
|
||||
// Center Y at the mid point of v.
|
||||
Float offset_y = dir_y.dot(c);
|
||||
assert(std::abs(dir_y.dot(center_pos) - offset_y) < SCALED_EPSILON);
|
||||
assert((dir_x * offset_x + dir_y * offset_y - center_pos).norm() < SCALED_EPSILON);
|
||||
// Solve the least squares fitting in a transformed space.
|
||||
Float a = Float(0.5) * lv;
|
||||
Float b = c.dot(dir_x) - offset_x;
|
||||
Float ab2 = sqr(a) + sqr(b);
|
||||
Float num = Float(0);
|
||||
Float denom = Float(0);
|
||||
const Float w = it_end - it_begin;
|
||||
for (Iterator it = it_begin; it != it_end; ++ it) {
|
||||
Vector p = *it;
|
||||
Float x_i = dir_x.dot(p) - offset_x;
|
||||
Float y_i = dir_y.dot(p) - offset_y;
|
||||
Float x_i2 = sqr(x_i);
|
||||
Float y_i2 = sqr(y_i);
|
||||
num += (x_i - b) * (x_i2 + y_i2 - ab2);
|
||||
denom += b * (b - Float(2) * x_i) + sqr(x_i) + Float(0.25) * w;
|
||||
}
|
||||
assert(denom != 0);
|
||||
Float c_x = Float(0.5) * num / denom;
|
||||
// Transform the center back.
|
||||
Vector out = dir_x * (c_x + offset_x) + dir_y * offset_y;
|
||||
return out;
|
||||
}
|
||||
|
||||
// Calculate center point (center of a circle) of an arc given two fixed points to interpolate
|
||||
// and an additional list of points to fit by non-linear least squares.
|
||||
// The non-linear least squares problem is solved by a Gauss-Newton iterative method.
|
||||
// Start & end point must be different and the interpolated points must not be collinear with input points.
|
||||
// Center position is used to calculate the initial solution of the Gauss-Newton method.
|
||||
//
|
||||
// In case the input points are collinear or close to collinear (such as a small angle arc),
|
||||
// the solution may not converge and an error is indicated.
|
||||
template<typename Derived, typename Derived2, typename Derived3, typename Iterator>
|
||||
inline std::optional<typename Eigen::Matrix<typename Derived::Scalar, 2, 1, Eigen::DontAlign>> arc_fit_center_gauss_newton_ls(
|
||||
const Eigen::MatrixBase<Derived> &start_pos,
|
||||
const Eigen::MatrixBase<Derived2> &end_pos,
|
||||
const Eigen::MatrixBase<Derived3> ¢er_pos,
|
||||
const Iterator it_begin,
|
||||
const Iterator it_end,
|
||||
const size_t num_iterations)
|
||||
{
|
||||
static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_fit_center_gauss_newton_ls(): start_pos is not a 2D vector");
|
||||
static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_fit_center_gauss_newton_ls(): end_pos is not a 2D vector");
|
||||
static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "arc_fit_center_gauss_newton_ls(): third parameter is not a 2D vector");
|
||||
static_assert(std::is_same<typename Derived::Scalar, typename Derived2::Scalar>::value &&
|
||||
std::is_same<typename Derived::Scalar, typename Derived3::Scalar>::value, "arc_fit_center_gauss_newton_ls(): All third points must be of the same type.");
|
||||
using Float = typename Derived::Scalar;
|
||||
using Vector = Eigen::Matrix<Float, 2, 1, Eigen::DontAlign>;
|
||||
// Prepare a vector space to transform the fitting into:
|
||||
// center_pos, dir_x, dir_y
|
||||
Vector v = end_pos - start_pos;
|
||||
Vector c = Float(.5) * (start_pos + end_pos);
|
||||
Float lv = v.norm();
|
||||
assert(lv > 0);
|
||||
Vector dir_y = v / lv;
|
||||
Vector dir_x = perp(dir_y);
|
||||
// Center is supposed to lie on bisector of the arc end points.
|
||||
// Center Y at the mid point of v.
|
||||
Float offset_y = dir_y.dot(c);
|
||||
// Initial value of the parameter to be optimized iteratively.
|
||||
Float c_x = dir_x.dot(center_pos);
|
||||
// Solve the least squares fitting in a transformed space.
|
||||
Float a = Float(0.5) * lv;
|
||||
Float a2 = sqr(a);
|
||||
Float b = c.dot(dir_x);
|
||||
for (size_t iter = 0; iter < num_iterations; ++ iter) {
|
||||
Float num = Float(0);
|
||||
Float denom = Float(0);
|
||||
Float u = b - c_x;
|
||||
// Current estimate of the circle radius.
|
||||
Float r = sqrt(a2 + sqr(u));
|
||||
assert(r > 0);
|
||||
for (Iterator it = it_begin; it != it_end; ++ it) {
|
||||
Vector p = *it;
|
||||
Float x_i = dir_x.dot(p);
|
||||
Float y_i = dir_y.dot(p) - offset_y;
|
||||
Float v = x_i - c_x;
|
||||
Float y_i2 = sqr(y_i);
|
||||
// Distance of i'th sample from the current circle center.
|
||||
Float r_i = sqrt(sqr(v) + y_i2);
|
||||
if (r_i >= EPSILON) {
|
||||
// Square of residual is differentiable at the current c_x and current sample.
|
||||
// Jacobian: diff(residual, c_x)
|
||||
Float j_i = u / r - v / r_i;
|
||||
num += j_i * (r_i - r);
|
||||
denom += sqr(j_i);
|
||||
} else {
|
||||
// Sample point is on current center of the circle,
|
||||
// therefore the gradient is not defined.
|
||||
}
|
||||
}
|
||||
if (denom == 0)
|
||||
// Fitting diverged, the input points are likely nearly collinear with the arch end points.
|
||||
return std::optional<Vector>();
|
||||
c_x -= num / denom;
|
||||
}
|
||||
// Transform the center back.
|
||||
return std::optional<Vector>(dir_x * c_x + dir_y * offset_y);
|
||||
}
|
||||
|
||||
// Test whether a point is inside a wedge of an arc.
|
||||
template<typename Derived, typename Derived2, typename Derived3>
|
||||
inline bool inside_arc_wedge_vectors(
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user