def test_integral_non_uniform(self):
for order in range(2, 8, 2):
# Create a spline with three segments
aspl = aslam_splines.OptEuclideanBSpline(order, 1)
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
cp = numpy.random.random(kc)
cpa = numpy.array([cp])
aspl = aslam_splines.OptEuclideanBSpline(order, 1)
aspl.initWithKnotsAndControlVertices(knots, cpa)
fspl = (knots, cp, order - 1)
for a in numpy.arange(aspl.getMinTime(),
aspl.getMaxTime() - 1e-15, 0.4):
for i in numpy.arange(aspl.getMinTime(),
aspl.getMaxTime() - 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_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 = aslam_splines.OptEuclideanBSpline(order, 3)
self.assertEqual(order, aspl.splineOrder())
#print "order: %d" % order
#print "p_0: %s" % p_0
#print "p_1: %s" % p_1
# Initialize the spline with these two times
aspl.initUniformSpline(numpy.array([t_0, t_1]),
numpy.array([p_0, p_1]).transpose(), 1, 0.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_jacobian(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 = aslam_splines.OptEuclideanBSpline(order, 1)
nSegs = 4
kr = aspl.numKnotsRequired(nSegs)
kc = aspl.numCoefficientsRequired(nSegs)
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, (kr - 1) * dt, kr)
cp = numpy.random.random(kc)
cpa = numpy.array([cp])
aspl.initWithKnotsAndControlVertices(knots, cpa)
knots = aspl.getKnotsVector()
for a in numpy.arange(aspl.getMinTime(),
aspl.getMaxTime() - 1e-15, 0.4 * dt):
b = aspl.evalJacobian(a)
f = numericJacobian(aspl, a, knots)
self.assertMatricesEqual(
b,
f,
1E-6,
msg=
"order %d spline jacobian evaluated at [%f] (%s != %s) was not right"
% (order, a, str(b), str(f)))
def createUniformKnotBSpline(order,segments,dim,knotSpacing=1.0):
aspl = aslam_splines.OptEuclideanBSpline(order, dim)
aspl.initConstantUniformSpline(0, segments * knotSpacing, segments, numpy.zeros((dim, 1)))
kc = aspl.getNumControlVertices();
cp = numpy.random.random([dim,kc])
aspl.setControlVertices(cp)
return (aspl,(aspl.getKnotsVector(),cp,order-1))
def createRandomKnotBSpline(order, segments, dim):
aspl = aslam_splines.OptEuclideanBSpline(order, dim)
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 = aslam_splines.OptEuclideanBSpline(order, dim)
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_time_interval2(self):
numpy.random.seed(6)
# Test two functions:
for order in range(2, 10):
nSegments = 3
aspl = aslam_splines.OptEuclideanBSpline(order, 3)
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.initWithKnotsAndControlVertices(knots, cpa)
# Check that the time interval function works.
ti = aspl.timeInterval()
self.assertEqual(ti[0], aspl.getMinTime())
self.assertEqual(ti[1], aspl.getMaxTime())
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 = aslam_splines.OptEuclideanBSpline(order, dim)
aspl.initConstantSpline(tmin, tmax, segs, c)
# Test the time boundaries
self.assertAlmostEqual(tmin, aspl.getMinTime())
self.assertAlmostEqual(tmax, aspl.getMaxTime())
# Test the value.
for t in numpy.arange(aspl.getMinTime(), aspl.getMaxTime(),
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 = aslam_splines.OptEuclideanBSpline(order, 1)
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.initWithKnotsAndControlVertices(knots, cpa)
fspl = (knots, cp, order - 1)
for i in numpy.linspace(aspl.getMinTime(), aspl.getMaxTime(), 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_uniform(self):
numpy.random.seed(1)
for order in range(2, 10):
aspl = aslam_splines.OptEuclideanBSpline(order, 1)
kr = aspl.numKnotsRequired(3)
kc = aspl.numCoefficientsRequired(3)
# Choose a uniform knot sequence.
knots = numpy.linspace(0.0, kr * 1.0, kr)
cp = numpy.random.random([kc])
cpa = numpy.array([cp])
aspl.initWithKnotsAndControlVertices(knots, cpa)
fspl = (knots, cp, order - 1)
for i in numpy.linspace(aspl.getMinTime(),
aspl.getMaxTime() - 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.assertEqual(a.shape, (1, ))
self.assertAlmostEqual(a[0], f[j])
