def test_integral_non_uniform_repeated(self):
for order in range(2, 8, 2):
# Create a spline with three segments
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(4)
kc = aspl.numCoefficientsRequired(4)
# Choose a non-uniform knot sequence.
knots = numpy.linspace(0.0, (kr - 1), kr)
knots = knots * knots
for i in range(0, len(knots)):
if i & 1 > 0:
knots[i] = knots[i - 1]
cp = numpy.random.random(kc)
cpa = numpy.array([cp])
aspl = bsplines.BSpline(order)
aspl.setKnotVectorAndCoefficients(knots, cpa)
fspl = (knots, cp, order - 1)
for a in numpy.arange(aspl.t_min(), aspl.t_max() - 1e-15, 0.4):
for i in numpy.arange(aspl.t_min(), aspl.t_max() - 1e-15, 0.4):
print "Eval at %f\n" % (i)
f = fp.splint(a, float(i), fspl)
b = aspl.evalI(a, i)
self.assertAlmostEqual(
b,
f,
msg=
"order %d spline integral evaluated on [%f,%f] (%f != %f) was not right"
% (order, a, i, float(b), f))
def test_new_segment(self):
numpy.random.seed(7)
# This function tests adding a new segment to the curve.
for order in range(2, 10):
# Create a spline with two segments
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(order + 1)
kc = aspl.numCoefficientsRequired(order + 1)
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, kr - 1, kr)
cp = numpy.random.random(kc)
# build a vector-valued spline
cpa = numpy.array([cp, cp * cp, cp * cp * cp])
aspl.setKnotVectorAndCoefficients(knots, cpa)
# Store a reference spline that doesn't get modified.
aspl_ref = bsplines.BSpline(order)
aspl_ref.setKnotVectorAndCoefficients(knots, cpa)
# Now add a segment to the spline.
ti = aspl.timeInterval()
# the current set of knots is uniformly spaced with spacing 1.0
# Let's muck around with that.
t_k = ti[1] + 0.5
p_k = numpy.array([1.0, 2.0, 3.0])
aspl.addCurveSegment(t_k, p_k)
# This function doesn't necessarily preserve the existing curve. It
# does, however, preserve the curve at ti[0] (all derivatives) and
# interpolate the value at ti[1]. Verify this.
# For all derivatives at ti[0]
for d in range(0, order):
# Evaluate the new curve and the reference curve
ref_p = aspl_ref.evalD(ti[0], d)
p = aspl.evalD(ti[0], d)
#print "[%f %f] S^%d(%f,%d) = %s, %s" % (ti[0], ti[1], order,t,d,ref_p,p)
# Check that they are almost equal
for i in range(0, p.size):
self.assertAlmostEqual(
p[i],
ref_p[i],
msg="[%f %f] S^%d(%f,%d) = %s, %s" %
(ti[0], ti[1], order, ti[0], d, ref_p, p))
# Now check that it interpolates the position at ti[1]
# Evaluate the new curve and the reference curve
ref_p = aspl_ref.evalD(ti[1], 0)
p = aspl.evalD(ti[1], 0)
# Check that they are almost equal
for i in range(0, p.size):
self.assertAlmostEqual(
p[i],
ref_p[i],
msg="[%f %f] S^%d(%f,%d) = %s, %s" %
(ti[0], ti[1], order, ti[1], d, ref_p, p))
# Now check that the curve interpolates p_k at t_k
curve_p_k = aspl.evalD(t_k, 0)
for i in range(0, p.size):
self.assertAlmostEqual(p_k[i],
curve_p_k[i],
msg="[%f %f] S^%d(%f,%d) = %s, %s" %
(ti[0], ti[1], order, t_k, d, ref_p, p))
def initBiasSplines(self, poseSpline, splineOrder, biasKnotsPerSecond):
start = poseSpline.t_min()
end = poseSpline.t_max()
seconds = end - start
knots = int(round(seconds * biasKnotsPerSecond))
print
print "Initializing the bias splines with %d knots" % (knots)
#initialize the bias splines
self.gyroBias = bsplines.BSpline(splineOrder)
self.gyroBias.initConstantSpline(start, end, knots, self.GyroBiasPrior)
self.accelBias = bsplines.BSpline(splineOrder)
self.accelBias.initConstantSpline(start, end, knots, np.zeros(3))
def test_init(self):
numpy.random.seed(5)
# Test the initialization from two times and two positions.
p_0 = numpy.array([1, 2, 3])
p_1 = numpy.array([2, 4, 6])
t_0 = 0.0
t_1 = 0.1
dt = t_1 - t_0
v = (p_1 - p_0) / dt
for order in range(2, 10):
aspl = bsplines.BSpline(order)
#print "order: %d" % order
#print "p_0: %s" % p_0
#print "p_1: %s" % p_1
# Initialize the spline with these two times
aspl.initSpline(t_0, t_1, p_0, p_1)
b_0 = aspl.eval(t_0)
b_1 = aspl.eval(t_1)
v_0 = aspl.evalD(t_0, 1)
v_1 = aspl.evalD(t_1, 1)
#print "b_0: %s" % b_0
#print "b_1: %s" % b_1
for j in range(0, p_0.size):
# Keep the threshold low for even power cases.
self.assertAlmostEqual(p_0[j], b_0[j], places=2)
self.assertAlmostEqual(p_1[j], b_1[j], places=2)
self.assertAlmostEqual(v_0[j], v[j], places=2)
self.assertAlmostEqual(v_1[j], v[j], places=2)
def test_U_D_B_c(self):
numpy.random.seed(4)
# Test that the linear algebra of Phi(t) * c is equivalent to the evaluation
# of the spline curve at t: b(t)
for order in range(2, 10):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(3)
kc = aspl.numCoefficientsRequired(3)
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, kr * 1.0, kr)
cp = numpy.linspace(1.0, kc, kc)
# build a vector-valued spline
cpa = numpy.array([cp, cp * cp, cp * cp * cp])
aspl.setKnotVectorAndCoefficients(knots, cpa)
for t in numpy.linspace(aspl.t_min(), aspl.t_max(), 10):
for i in range(0, order):
# Check that Phi(t) c(t) = s(t)
s = aspl.evalD(t, i)
U = aspl.U(t, 0)
M = aspl.Mi(aspl.segmentIndex(t))
D = aspl.Di(aspl.segmentIndex(t))
# Evaluate the derivative as matrix multiplication
for d in range(0, i):
M = numpy.dot(D, M)
c = aspl.localCoefficientVector(t)
sprime = numpy.dot(U.T, numpy.dot(M, c))
for j in range(0, sprime.size):
self.assertAlmostEqual(s[j], sprime[j])
def createUniformKnotOldBSpline(order,segments,dim,knotSpacing=1.0):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(segments)
kc = aspl.numCoefficientsRequired(segments);
knots = numpy.linspace(-order,kr - 1 - order, kr)*knotSpacing
cp = numpy.random.random([dim,kc])
aspl.setKnotVectorAndCoefficients(knots, cp)
return aspl
def createUniformKnotBSpline(order, segments, dim, knotSpacing=1.0):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(segments)
kc = aspl.numCoefficientsRequired(segments)
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, kr - 1, kr) * knotSpacing
cp = numpy.random.random([dim, kc])
aspl.setKnotVectorAndCoefficients(knots, cp)
return (aspl, (knots, cp, order - 1))
def test_remove_segment(self):
numpy.random.seed(8)
for order in range(2, 10):
# Create a spline with two segments
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(order + 1)
kc = aspl.numCoefficientsRequired(order + 1)
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, kr - 1, kr)
cp = numpy.random.random(kc)
# build a vector-valued spline
cpa = numpy.array([cp, cp * cp, cp * cp * cp])
aspl.setKnotVectorAndCoefficients(knots, cpa)
# Store a reference spline that doesn't get modified.
aspl_ref = bsplines.BSpline(order)
aspl_ref.setKnotVectorAndCoefficients(knots, cpa)
# Now remove a curve segment
aspl.removeCurveSegment()
# Check that the knot sequence is good.
ref_knots = aspl_ref.knots()
knots = aspl.knots()
self.assertEqual(knots.size, ref_knots.size - 1)
for i in range(0, knots.size):
self.assertEqual(knots[i], ref_knots[i + 1])
# Check that the time range is still good.
self.assertEqual(aspl.t_min(), aspl_ref.timeInterval(0)[1])
# Check that the coefficients survived.
ref_coeff = aspl_ref.coefficients()
coeff = aspl.coefficients()
self.assertEqual(coeff.shape[1], ref_coeff.shape[1] - 1)
for r in range(0, coeff.shape[0]):
for c in range(0, coeff.shape[1]):
self.assertEqual(
coeff[r, c],
ref_coeff[r, c + 1],
msg="Order %s, coeff[%d,%d] %f != %f\n%s\n%s" %
(order, r, c, coeff[r, c], ref_coeff[r, c + 1], coeff,
ref_coeff))
# Now we check that the curve still evaluates well.
for t in numpy.linspace(aspl.t_min(), aspl.t_max(), 0.01):
for d in range(0, order):
# Exactly equal...not approximately equal.
self.assertEqual(aspl.evalD(t, d), aspl_ref.evalD(t, d))
def createRandomKnotBSpline(order, segments, dim):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(segments)
kc = aspl.numCoefficientsRequired(segments)
# Choose a uniform knot sequence.
knots = numpy.random.random(kr) * 10
knots.sort()
cp = numpy.random.random([dim, kc])
aspl.setKnotVectorAndCoefficients(knots, cp)
return (aspl, (knots, cp, order - 1))
def createExponentialKnotBSpline(order, segments, dim, knotSpacing=1.0):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(segments)
kc = aspl.numCoefficientsRequired(segments)
# Choose a uniform knot sequence.
knots = numpy.zeros(kr)
for i in range(0, kr):
knots[i] = knotSpacing * 2**i
cp = numpy.random.random([dim, kc])
aspl.setKnotVectorAndCoefficients(knots, cp)
return (aspl, (knots, cp, order - 1))
def test_integral(self):
for order in range(2,8,2):
for dt in numpy.arange(0.1,2.0,0.1):
# Create a spline with three segments
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(4)
kc = aspl.numCoefficientsRequired(4);
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, (kr - 1)*dt, kr)
cp = numpy.random.random(kc);
cpa = numpy.array([cp])
aspl = bsplines.BSpline(order);
aspl.setKnotVectorAndCoefficients(knots,cpa);
fspl = (knots,cp,order-1)
for a in numpy.arange(aspl.t_min(),aspl.t_max()-1e-15,0.4*dt):
for i in numpy.arange(aspl.t_min(), aspl.t_max()-1e-15, 0.4*dt):
print("Eval at %f\n" % (i))
f = fp.splint(a,float(i),fspl)
b = aspl.evalI(a,i)
self.assertAlmostEqual(b, f, msg="order %d spline integral evaluated on [%f,%f] (%f != %f) was not right" % (order, a,i,float(b),f))
def createRandomRepeatedKnotBSpline(order,segments,dim):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(segments)
kc = aspl.numCoefficientsRequired(segments);
# Choose a uniform knot sequence.
knots = numpy.random.random(kr)*10
knots.sort()
for i in range(0,len(knots)):
if i&1:
knots[i-1] = knots[i]
cp = numpy.random.random([dim,kc])
aspl.setKnotVectorAndCoefficients(knots, cp)
return (aspl,(knots,cp,order-1))
def test_constant_init(self):
tmin = 0.0
tmax = 5.0
for order in range(2,6):
for dim in range(1,4):
for segs in range(1,4):
c = numpy.random.random([dim])
# Initialize a constant spline
aspl = bsplines.BSpline(order)
aspl.initConstantSpline(tmin,tmax,segs,c)
# Test the time boundaries
self.assertAlmostEqual(tmin,aspl.t_min())
self.assertAlmostEqual(tmax,aspl.t_max())
# Test the value.
for t in numpy.arange(aspl.t_min(),aspl.t_max(),0.1):
self.assertMatricesEqual(aspl.evalD(t,0),c,1e-15,"Error getting back the constant value")
def test_random(self):
numpy.random.seed(3)
for order in range(2, 10):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(3)
kc = aspl.numCoefficientsRequired(3)
knots = numpy.random.random([kr]) * 10
knots.sort()
cp = numpy.random.random([kc])
cpa = numpy.array([cp])
aspl.setKnotVectorAndCoefficients(knots, cpa)
fspl = (knots, cp, order - 1)
for i in numpy.linspace(aspl.t_min(), aspl.t_max(), 10):
f = fp.spalde(float(i), fspl)
a = aspl.eval(i)
for j in range(0, f.shape[0]):
a = aspl.evalD(i, j)
self.assertAlmostEqual(a, f[j])
def test_phi_c(self):
numpy.random.seed(4)
# Test that the linear algebra of Phi(t) * c is equivalent to the evaluation
# of the spline curve at t: b(t)
for order in range(2, 10):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(3)
kc = aspl.numCoefficientsRequired(3)
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, kr * 1.0, kr)
cp = numpy.linspace(1.0, kc, kc)
# build a vector-valued spline
cpa = numpy.array([cp, cp * cp, cp * cp * cp])
aspl.setKnotVectorAndCoefficients(knots, cpa)
for t in numpy.linspace(aspl.t_min(), aspl.t_max(), 10):
for i in range(0, order):
# Check that Phi(t) c(t) = s(t)
s = aspl.evalD(t, i)
Phi = aspl.Phi(t, i)
c = aspl.localCoefficientVector(t)
sprime = numpy.dot(Phi, c)
for j in range(0, sprime.size):
self.assertAlmostEqual(s[j], sprime[j])
def test_time_interval(self):
numpy.random.seed(6)
# Test two functions:
for order in range(2, 10):
nSegments = 3
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(nSegments)
kc = aspl.numCoefficientsRequired(nSegments)
# Choose a uniform knot sequence at 0.0, 1.0, ...
knots = numpy.linspace(0.0, kr - 1, kr)
cp = numpy.linspace(1.0, kc, kc)
# build a vector-valued spline
cpa = numpy.array([cp, cp * cp, cp * cp * cp])
aspl.setKnotVectorAndCoefficients(knots, cpa)
# Check that the time interval function works.
ti = aspl.timeInterval()
self.assertEqual(ti[0], aspl.t_min())
self.assertEqual(ti[1], aspl.t_max())
# Check that the individual segment time interval function works
for i in range(0, 3):
ti = aspl.timeInterval(i)
self.assertEqual(ti[0], order - 1 + i)
self.assertEqual(ti[1], order + i)
def test_repeated(self):
numpy.random.seed(2)
for order in range(2, 10):
aspl = bsplines.BSpline(order)
kr = aspl.numKnotsRequired(3)
kc = aspl.numCoefficientsRequired(3)
# Make a knot sequence that is all zeros at one end and all ones at the other.
knots = numpy.zeros(kr)
for i in range(0, knots.size):
if i >= knots.size * 0.5:
knots[i] = 1.0
cp = numpy.random.random([kc])
cpa = numpy.array([cp])
aspl.setKnotVectorAndCoefficients(knots, cpa)
fspl = (knots, cp, order - 1)
for i in numpy.linspace(aspl.t_min(), aspl.t_max() - 1e-15, 10):
f = fp.spalde(float(i), fspl)
a = aspl.eval(i)
for j in range(0, f.shape[0]):
a = aspl.evalD(i, j)
self.assertAlmostEqual(a, f[j])
def createExpBSpline(geometry, splineOrder, time_min, time_max, segmentNumber):
bs = bsplines.BSpline(splineOrder)
bs.initConstantSpline(time_min, time_max, segmentNumber, numpy.array(
(0, )))
return ExponentialBSpline(geometry, bs)
import bsplines as asp
import numpy as np
import pylab as pl
from quaternions import qdot, qlog, qexp, qinv
#pl.figure(1)
bs = asp.BSpline(4)
bs.initConstantSpline(0, 12, 12, np.array([0]))
bs.setCoefficientMatrix(
np.array([[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]]))
T = np.arange(0, 12.0, 0.01)
val = np.array([bs.eval(t) for t in T])
# This is the regular basis function.
#pl.plot(T,val)
val3 = np.array([bs.evalI(0, t) for t in T])
#pl.plot(T,val3,'b:')
def evalCum(bs, t):
rval = 0
for i in range(0, 4):
rval += bs.eval(t + i)
return rval
T = np.arange(0, 8.0, 0.01)
val2 = np.array([evalCum(bs, t) for t in T])
#pl.plot(T+3,val2)
import roslib
roslib.load_manifest('bsplines')
roslib.load_manifest('sm_python')
import bsplines as asp
import numpy as np
import pylab as pl
#pl.figure(1)
bs = asp.BSpline(4)
bs.initConstantSpline(0,12,12,np.array([0]))
bs.setCoefficientMatrix(np.array([[0,0,0,0,0,0,0,1,0,0,0,0,0,0,0]]))
T = np.arange(0,12.0,0.01)
val = np.array( [ bs.eval(t) for t in T ] )
# This is the regular basis function.
#pl.plot(T,val)
def evalCum(bs,t):
rval = 0
for i in range(0,4):
rval += bs.eval(t + i)
return rval
T8 = np.arange(0,8.0,0.01)
val2 = np.array( [ evalCum(bs,t) for t in T8 ] )
#pl.plot(T8+3,val2)
def evalLocalBi(bs, t, i):
return bs.getLocalBi(t)[i - bs.segmentIndex(t)] if i >= bs.segmentIndex(t) and i <= bs.segmentIndex(t) + 3 else 0
def getLocalCumulativeBi(bs, t, i):
