Implement efficient sampling of curves.

Author: bastikr

README.md

@@ -31,6 +31,7 @@ Curve Interface:
 * `startpoint(C)`
 * `endpoint(C)`
 * `dpoint(C, s)`
+* `samples(C, e)`
 
 All derivatives are given in respect to the parameter `s`.
 

docs/src/api.md

@@ -33,6 +33,7 @@ PolyCurve
 [`dcurvature`](@ref)\
 [`tangentangle`](@ref)\
 [`radialangle`](@ref)\
+[`samples`](@ref)\
 
 ```@docs
 smax
@@ -46,6 +47,7 @@ curvature
 dcurvature
 tangentangle
 radialangle
+samples
 ```
 
 ## Curve specific functions

docs/src/curve.md

@@ -15,4 +15,5 @@
 [`curvature`](@ref)\
 [`dcurvature`](@ref)\
 [`tangentangle`](@ref)\
-[`radialangle`](@ref)
+[`radialangle`](@ref)\
+[`samples`](@ref)

docs/src/index.md

@@ -20,7 +20,8 @@ and the possibility to chain them together as [PolyCurve](@ref). All curves impl
 [`curvature`](@ref)\
 [`dcurvature`](@ref)\
 [`tangentangle`](@ref)\
-[`radialangle`](@ref)
+[`radialangle`](@ref)\
+[`samples`](@ref)
 
 Additionally, [Construction](@ref) methods which generate smooth curves from given polygon paths are provided.
 

src/SmoothCurves.jl

@@ -9,6 +9,7 @@ export Curve, Pose, LineSegment, ArcSegment, Clothoid, PolyCurve,
        curvature_unchecked, dcurvature_unchecked,
        point, dpoint, startpoint, endpoint,
        point_unchecked, dpoint_unchecked,
+       samples,
        direction,
        frenet,
        construction

src/arcsegment.jl

@@ -48,3 +48,8 @@ function dpoint_unchecked(C::ArcSegment, Δϕ::Real)
     ϕ = radialangle(C, Δϕ)
     C.radius*sign(C)*SVector(-sin(ϕ), cos(ϕ))
 end
+
+function samples(C::ArcSegment, e::Real)
+    n = max(1, ceil(Int, abs(C.angle1 - C.angle0)/e)) + 2
+    return collect(range(0, stop=abs(C.angle1-C.angle0), length=n))
+end

src/clothoid.jl

@@ -126,3 +126,21 @@ function dpoint_unchecked(C::Clothoid, s::Real)
     dy = sin(C.λ*l_^2)
     return rotate2d(C.rotation, dx, dy)
 end
+
+function samples(C::Clothoid, e::Real)
+    if C.l0==0
+        θmax = C.λ*standardlength(C, smax(C))^2
+        n = ceil(Int, max(2, abs(θmax/e))) + 1
+        θvec = range(0, stop=θmax, length=n)
+        lvec = sqrt.(abs.(θvec/C.λ))
+        return lvec
+    elseif C.l1==0
+        θmax = C.λ*standardlength(C, 0)^2
+        n = ceil(Int, max(2, abs(θmax/e))) + 1
+        θvec = range(θmax, stop=0, length=n)
+        lvec = sqrt.(abs.(θvec/C.λ))
+        return -lvec .- C.l0
+    else
+        error("Not implemented")
+    end
+end

src/curve.jl

@@ -127,6 +127,13 @@ Return the last point ``C(s_\\mathrm{max}`` of the curve.
 """
 endpoint(C::Curve) = point_unchecked(C, smax(C))
 
+"""
+    samples(C::Curve, e::Real)
+
+Distribute values between 0 and smax in order to generate a "smooth" polygon.
+"""
+samples(C::Curve, e::Real) = error("Not Implemented")
+
 
 struct Pose
     x::Float64

src/linesegment.jl

@@ -51,3 +51,5 @@ dpoint_unchecked(C::LineSegment, s::Real) = direction(C)
 
 startpoint(C::LineSegment) = C.p0
 endpoint(C::LineSegment) = C.p1
+
+samples(C::LineSegment, e::Real) = Float64[0., smax(C)]

src/plotrecipes.jl

@@ -21,12 +21,14 @@ import .Pose
     end
 end
 
-@recipe function f(curve::Curve; spacing=0.1)
+@recipe function f(curve::Curve; e=0.01)
     aspect_ratio --> :equal
     label --> ""
 
-    n = round(Int, length(curve)/spacing, RoundUp)
-    svec = range(0., stop=smax(curve), length=n)
-    points = point.(curve, svec)
+    # n = round(Int, length(curve)/spacing, RoundUp)
+    # svec = range(0., stop=smax(curve), length=n)
+    # points = point.(curve, svec)
+
+    points = point.(curve, samples(curve, e))
     [p[1] for p in points], [p[2] for p in points]
 end

src/polycurve.jl

@@ -76,3 +76,11 @@ curvature_unchecked(C::PolyCurve, s::Real) = dispatch(curvature_unchecked, C, s)
 dcurvature_unchecked(C::PolyCurve, s::Real) = dispatch(dcurvature_unchecked, C, s)
 startpoint(C::PolyCurve) = startpoint(C.curves[1])
 endpoint(C::PolyCurve) = endpoint(C.curves[end])
+
+function samples(C::PolyCurve, e::Real)
+    svec = samples(C.curves[1], e)
+    for i=2:length(C.curves)
+        append!(svec, samples(C.curves[i], e)[2:end] .+ C.cum_smax[i-1])
+    end
+    svec
+end

test/curvefunctions_testsuite.jl

@@ -1,7 +1,7 @@
 using Test
 import SmoothCurves
 
-using LinearAlgebra: norm, dot
+using LinearAlgebra: norm, normalize, dot
 
 using ForwardDiff
 
@@ -57,4 +57,13 @@ function test_curvefunctions(C::SmoothCurves.Curve)
         # Test dcurvature
         @test dcurvature(s) ≈ d_ds(curvature, s) atol=1e-13
     end
+
+    for maxangle=0.1:0.1:1
+        svec = samples(C, maxangle)
+        for i=1:Base.length(svec)-1
+            v0 = dpoint(svec[i])
+            v1 = dpoint(svec[i+1])
+            @test dot(normalize(v0), normalize(v1)) >= cos(maxangle)
+        end
+    end
 end

test/test_curve.jl

@@ -33,6 +33,7 @@ s = 0.25
 @test_throws ErrorException dpoint(C, s)
 @test_throws ErrorException startpoint(C)
 @test_throws ErrorException endpoint(C)
+@test_throws ErrorException samples(C, 0.1)
 
 s = 0.75
 @test_throws ArgumentError length(C, s)

test/test_linesegment.jl

@@ -6,6 +6,7 @@ l = LineSegment([1, 2], [4, 6])
 @test point(l, 0) == [1, 2]
 @test point(l, smax(l)) == [4, 6]
 @test length(l) ≈ 5
+@test samples(l, 0.1) ≈ [0., smax(l)]
 end
 
 include("curvefunctions_testsuite.jl")

test/test_polycurve.jl

@@ -25,6 +25,8 @@ C = PolyCurve(C_l0, C_l1, C_l2)
 @test smax(C) == smax(C_l0) + smax(C_l1) + smax(C_l2)
 @test length(C) == length(C_l0) + length(C_l1) + length(C_l2)
 @test length(C, smax(C)) == length(C)
+
+@test samples(C, 0.01) == [0, smax(C_l0), smax(C_l0)+smax(C_l1), smax(C)]
 end
 
 C_arc = ArcSegment([1, 3], 2., 0., π)