def predict(self, x, deriv=None, *args, **kwargs):
if deriv is None:
deriv = 0
if deriv == -1:
return interpolate.splev(
x, interpolate.splantider(
(self.knots, self.coeff, self.degree)))
return interpolate.splev(x, (self.knots, self.coeff, self.degree),
der=deriv)
def integral(self, n=1):
"""Returns integral / anti-derivative as a spline
Arguments:
n (int): order of the integral
Returns:
Spline: the integral of the spline with degree + n
"""
tck = splantider(self.tck(), n)
return Spline(tck=tck, points=self.points)
def integral(self, n = 1):
"""Returns integral / anti-derivative as a spline
Arguments:
n (int): order of the integral
Returns:
Spline: the integral of the spline with degree + n
"""
tck = splantider(self.tck(), n);
return Spline(tck = tck, points = self.points);
def _make_splines(self, xs, ys):
""" Make the splines """
base = splrep(xs, ys, s=0)
tcks = {
-1: splantider(base, 1),
0: base,
1: splder(base, 1),
2: splder(base, 2),
3: splder(base, 3)
}
return tcks
def test_antiderivative_vs_spline(self):
np.random.seed(1234)
x = np.sort(np.r_[0, np.random.rand(11), 1])
y = np.random.rand(len(x))
spl = splrep(x, y, s=0, k=5)
pp = PPoly.from_spline(spl)
for dx in range(0, 10):
pp2 = pp.antiderivative(dx)
spl2 = splantider(spl, dx)
xi = np.linspace(0, 1, 200)
assert_allclose(pp2(xi), splev(xi, spl2), rtol=1e-7)
def test_splantider(self):
for b in [self.b, self.b2]:
# pad the c array (FITPACK convention)
ct = len(b.t) - len(b.c)
if ct > 0:
b.c = np.r_[b.c, np.zeros((ct,) + b.c.shape[1:])]
for n in [1, 2, 3]:
bd = splantider(b)
tck_d = _impl.splantider((b.t, b.c, b.k))
assert_allclose(bd.t, tck_d[0], atol=1e-15)
assert_allclose(bd.c, tck_d[1], atol=1e-15)
assert_equal(bd.k, tck_d[2])
assert_(isinstance(bd, BSpline))
assert_(isinstance(tck_d, tuple)) # back-compat: tck in and out
def test_splantider(self):
for b in [self.b, self.b2]:
# pad the c array (FITPACK convention)
ct = len(b.t) - len(b.c)
if ct > 0:
b.c = np.r_[b.c, np.zeros((ct,) + b.c.shape[1:])]
for n in [1, 2, 3]:
bd = splantider(b)
tck_d = _impl.splantider((b.t, b.c, b.k))
assert_allclose(bd.t, tck_d[0], atol=1e-15)
assert_allclose(bd.c, tck_d[1], atol=1e-15)
assert_equal(bd.k, tck_d[2])
assert_(isinstance(bd, BSpline))
assert_(isinstance(tck_d, tuple)) # back-compat: tck in and out
def test_antiderivative_vs_spline(self):
np.random.seed(1234)
x = np.sort(np.r_[0, np.random.rand(11), 1])
y = np.random.rand(len(x))
spl = splrep(x, y, s=0, k=5)
pp = PPoly.from_spline(spl)
for dx in range(0, 10):
pp2 = pp.antiderivative(dx)
spl2 = splantider(spl, dx)
xi = np.linspace(0, 1, 200)
assert_allclose(pp2(xi), splev(xi, spl2),
rtol=1e-7)
def integ_cubic(x, y, constant=0., s=0, tck=None, tck_ader=None, rettck=False):
'''
Function returning the primitive of a given function y, using cubic spline
interpolation
'''
if tck is None:
tck = interp.splrep(x, y, s=s)
if tck_ader is None:
tck_ader = interp.splantider(tck)
Y = interp.splev(x, tck_ader) + constant
if rettck:
return x, Y, tck, tck_ader
else:
return x, Y
x = xy[:,0]; tck1d = splrep(s, x, s = 0); tck,u = splprep(xy.T, s= 0) c = np.zeros(tck[1][0].shape[0]+4); c[:-4] = tck[1][0] tck1 = (tck[0], c, tck[2]) itck1= splantider(tck1, 1); c[:-4] = tck[1][1] tck2 = (tck[0], c, tck[2]) itck2= splantider(tck2, 1); plt.figure(1); plt.clf(); plt.plot(xy[:,0], xy[:,1]) xys = splev(u, tck); plt.plot(xys[0], xys[1]) x = xy[:,0];
def fit_spline(curve, log=False,
visualize=False, save=False,
filename=None, der=0, mu=False):
'''
Creates a spline representation of a growth curve, can also be used to
produce the integral or any derivative of the growth curve itself
'''
try:
mu = 'mu' in der
except(TypeError):
mu = der == 'mu'
if mu:
der.remove('mu')
if save and not filename:
exit
if save and not visualize:
visualize = True
if type(der) != list:
der = [der]
try:
index = curve.index/np.timedelta64(1, 'h')
except(TypeError):
index = curve.index
if log:
curve = curve.apply(lambda x: np.log(x))
# creating t c and k values of the spline, s value represents degree of
# smoothing.
tck = interpolate.splrep(index, curve.values, s=1.0*10**-3)
splines = []
for derivative in der:
if derivative >= 0:
spline = interpolate.splev(index, tck, der=derivative)
spline = pd.Series(spline, index)
splines.append(spline)
if derivative < 0:
Bspline = (interpolate.BSpline(*tck, extrapolate=False))
integral = interpolate.splantider(Bspline, -derivative)
spline = (interpolate.splev(index, integral, der=0))
spline = pd.Series(spline, index)
splines.append(spline)
if mu:
mu = interpolate.splev(index, tck, 1) / \
interpolate.splev(index, tck, 0)
mu = pd.Series(mu, index)
splines.append(mu)
if visualize:
fig, axis1 = plt.subplots()
axis1.set_xlabel('Time (hours)')
axis1.set_ylabel('OD595', color='r')
if 0 in der:
axis1.plot(index, splines[0], '-',
index, curve.values, '.', color='r')
else:
axis1.plot(index, splines[0], '-', color='r')
axis1.tick_params(axis='y', labelcolor='r')
secondary_axes = {}
for i, spline in enumerate(splines[1:]):
secondary_axes['axis'+str(i)] = axis1.twinx()
secondary_axes['axis'+str(i)].axhline(color='b')
secondary_axes['axis'+str(i)].set_ylabel('der ='+str(i), color='b')
secondary_axes['axis'+str(i)].plot(index, spline, color='b')
secondary_axes['axis'+str(i)].tick_params(axis='y', labelcolor='b')
fig.tight_layout()
plt.show()
if save:
directory = r'C:\Users\Owner\Concordia\Lab_Automation\output_files\\'
fig.savefig(directory+filename)
return splines
def run_main():
np.set_printoptions(linewidth=150)
filename = '/home/kroegert/local/Results/AccGPSFuse/mystream_6_10_12_54_5.csv'
#filename = '/home/till/zinc/local/Results/AccGPSFuse/mystream_6_10_12_54_5.csv'
#filename = '/home/kroegert/local/Results/AccGPSFuse/mystream_6_11_10_42_32.csv'
#filename = '/home/till/zinc/local/Results/AccGPSFuse/mystream_6_11_10_55_10_phoneonback.csv'
#filename = '/home/till/zinc/local/Results/AccGPSFuse/mystream_6_11_10_57_31_phoneonfront.csv'
#filename = '/home/kroegert/local/Results/AccGPSFuse/mystream_6_11_11_29_28_phoneturn.csv'
#filename = '/home/kroegert/local/Results/AccGPSFuse/mystream_6_15_10_59_5_walkcircle.csv'
#filename = '/home/kroegert/local/Results/AccGPSFuse/mystream_6_15_11_24_41_phonevertcircle.csv'
#filename = '/home/kroegert/local/Results/AccGPSFuse/mystream_6_15_16_12_46_phonesidewaysslide.csv'
data_gps, data_accel, data_gyro, data_orient, data_linacc, data_rotvec, data_grav = func_parse_imugps_cvs(filename)
# Android coordinate system: When a device is held in its default orientation, the X axis is horizontal and points to the right, the Y axis is vertical and points up,
# and the Z axis points toward the outside of the screen face. In this system, coordinates behind the screen have negative Z values.
# *** Determine start and end of valid lin.accel and gyroscope data
t_start_track = np.maximum(data_linacc[0][0], data_gyro[0][0])
t_end_track = np.minimum(data_linacc[0][0], data_gyro[0][0])
# *** GPS to cartesian
# TODO: Note, Android world system spanned by vectors: gravity+direction to magnetic north, GPScartesian: world axis and arbitrary x/y plane
testpt = data_gps[1][:,0:3]
# testpt[:,1] : longitude, should be x axis in ref. system,
# testpt[:,0] : latitude, should be y axis in ref. system,
GPS_xyz = np.vstack((np.sin(np.radians(testpt[:,0])) * np.sin(np.radians(testpt[:,1])) * (testpt[:,2] + 6371000), np.sin(np.radians(testpt[:,0])) * (testpt[:,2] + 6371000), np.sin(np.radians(testpt[:,0])) * np.cos(np.radians(testpt[:,1])) * (testpt[:,2] + 6371000))).transpose()
GPS_xyz -= GPS_xyz[testpt.shape[0]/2,:][None,:]
# *** normalize gravity vector to unit norm
data_grav = (data_grav[0], data_grav[1] / np.linalg.norm(data_grav[1], axis=1)[:, None])
# *** prepare camera orientation from rotvec data
rotgt_org = np.zeros((len(data_rotvec[0]),3,3), dtype=float )
for i in xrange(len(data_rotvec[0])): # Plot cameras from absolute orientation estimation (directly from sensor)
rotgt_org[i,:,:] = func_android_rotM_from_rotvec(data_rotvec[1][i,:], True)
# *** estimate camera orientation from rotational acceleration data
rotestim = func_android_rotM_from_gyroscope(data_gyro[0], data_gyro[1], dosvd=True)
#rotestim,p = func_spline_orientation_smooth(data_gyro[0],rotestim) # smooth orientation
#rotestim_ = func_spline_orientation_interpolate(data_gyro[0][0:-1:40], rotestim[0:-1:40,:,:], data_gyro[0])
#[func_comp_rot(rotestim[i,:,:], rotestim_new[i,:,:]) for i in xrange(rotestim_new.shape[0])]
# *** compute camera trajectory from lin.accel and gyroscopic data
rotestim_linacc = func_spline_orientation_interpolate(data_gyro[0], rotestim, data_linacc[0]) # fit gyroscopic orientation data to lin.accel data
# fit GT camera orientation to lin.acc. data
rotgt = func_spline_orientation_interpolate(data_rotvec[0], rotgt_org, data_linacc[0])
linaccabs = np.zeros((data_linacc[0].shape[0],3), dtype=float ) # linear acceleration in world frame
for i in xrange(data_linacc[0].shape[0]):
linaccabs[i,:] = np.dot(np.linalg.inv(rotgt[i,:,:]), data_linacc[1][i,:]) # use fused rotation data
tck = scpint.splrep(data_linacc[0], linaccabs[:,0], s=0.0) # x acceleration spline
tck = scpint.splantider(tck,2) # double integral -> x displacement
x = scpint.splev(data_linacc[0], tck)
tck = scpint.splrep(data_linacc[0], linaccabs[:,1], s=0.0) # y acceleration spline
tck = scpint.splantider(tck,2) # double integral -> y displacement
y = scpint.splev(data_linacc[0], tck)
tck = scpint.splrep(data_linacc[0], linaccabs[:,2], s=0.0) # z acceleration spline
tck = scpint.splantider(tck,2) # double integral -> z displacement
z = scpint.splev(data_linacc[0], tck)
posaccum = np.stack((x,y,z), axis=1)
# posaccum = np.zeros((rotestim_linacc.shape[0],3), dtype=float )
# R0 = func_android_rotM_from_rotvec(data_rotvec[1][0,:], True)
# for i in xrange(data_linacc[0].shape[0]-2):
# #R = np.dot(R0,rotestim_linacc[i,:,:]) # use GT location from first frame to align initial camera
# R = func_android_rotM_from_rotvec(data_rotvec[1][i,:], True) # use absolute orientation data to add linear acceleration
# #posaccum[i+1,:] = posaccum[i,:] + np.dot(np.linalg.inv(R), data_linacc[1][i,:])
# *** estimate camera path from linear acceleration data
# tck,u=scpint.splprep(data_linacc[1].transpose().tolist(),u=data_linacc[0], s=0.0) # interpolation spline over actual sensor data
#
# #p = func_smoothing_spline_crossvalp(data_linacc[0][1000:1500],data_linacc[1][1000:1500,:], crossvalperc = 0.1, crrounds = 100, verbosity=1)
# #linaccsm, LL, p = func_smoothing_spline(data_linacc[0],data_linacc[1],p) # smooth acceleration
# #tck,u=scpint.splprep(linaccsm.transpose().tolist(),u=data_linacc[0], s=0.0) # interpolation spline over smoothed sensor data
# #linaccsm = np.array(scpint.splev(data_gps[0],tck)).transpose()
### Plot camera path
wh = np.array([1280.0,960.0])
fc = np.array([900.0,900.0])
cc = np.array([0.0, 0.0])
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(posaccum[5:,0], posaccum[5:,1], posaccum[5:,2], color=np.array([0.0, 1.0, 0.0]), linewidth=2.0)
func_set_axes_equal(ax)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
### Plot camera orientation path, and gravity direction
wh = np.array([1280.0,960.0])
fc = np.array([900.0,900.0])
cc = np.array([0.0, 0.0])
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
t = np.array([1.0, 10.0, 100.0])
for i in xrange(0,len(data_rotvec[0]),50): # Plot cameras from absolute orientation estimation (directly from sensor)
R = rotgt_org[i,:,:] #func_android_rotM_from_rotvec(data_rotvec[1][i,:], True)
tc = -np.dot(R, t ) # position in camera centric coordinate frame
func_plot_cameras(ax, fc,cc,wh,R,tc,rgbface=np.array([1.0, 0.0, 0.0]),camerascaling=2.0,lw=2.0)
#
# for i in xrange(0,len(data_grav[0]),40): # Plot gravity vectors
# R = func_android_rotM_from_rotvec(data_rotvec[1][i,:], True)
# g_vec = data_grav[1][i,:] # gravity vector (in camera reference frame)
# g_vec_rot = np.dot(R,g_vec[:]) # gravity vector (in world ref. frame, uses estimated camera orientation)
#
# ax.plot([t[0], t[0]+g_vec[0]], [t[1], t[1]+g_vec[1]], [t[2], t[2]+g_vec[2]], color=np.array([0.0, 0.0, 1.0]), linewidth=2.0)
# ax.plot([t[0], t[0]+g_vec_rot[0]], [t[1], t[1]+g_vec_rot[1]], [t[2], t[2]+g_vec_rot[2]], color=np.array([0.0, 1.0, 0.0]), linewidth=2.0)
# for i in xrange(0,data_gyro[0].shape[0],10): # Plot cameras from tracked orientation (accumulated gyroscope data)
# R0 = func_android_rotM_from_rotvec(data_rotvec[1][0,:], True)
# R = np.dot(R0,rotestim[i,:,:]) # use GT location from first frame to align initial camera
# tc = -np.dot(R, t ) # position in camera centric coordinate frame
# func_plot_cameras(ax, fc,cc,wh,R,tc,rgbface=np.array([0.0, 0.0, 1.0]),camerascaling=2.0,lw=4.0)
#g_vec = data_grav[1][0,:]
#ax.plot([t[0], t[0]+g_vec[0]], [t[1], t[1]+g_vec[1]], [t[2], t[2]+g_vec[2]], color=np.array([0.0, 1.0, 0.0]), linewidth=4.0)
#g_vec = data_grav[1][-1,:]
#ax.plot([t[0], t[0]+g_vec[0]], [t[1], t[1]+g_vec[1]], [t[2], t[2]+g_vec[2]], color=np.array([1.0, 0.0, 1.0]), linewidth=4.0)
func_set_axes_equal(ax)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
### Plot GPS path
m = data_gps[0].shape[0]
testpt = data_gps[1][:,0:3]
#p = func_smoothing_spline_crossvalp(data_gps[0],data_gps[1][:,0:2], crossvalperc = 0.1, crrounds = 10, verbosity=1)
testptsm, LL, p = func_smoothing_spline(data_gps[0],data_gps[1][:,0:3],p)
tck,u=scpint.splprep(testptsm.transpose().tolist(),u=data_gps[0], s=0.0) # new homogeneous samples
t_new = np.linspace(data_gps[0][0],data_gps[0][-1],500)
testptsm = np.array(scpint.splev(t_new,tck)).transpose()
plt.scatter(testpt[:,1], testpt[:,0],24, color='blue')
plt.scatter(testpt[0,1], testpt[0,0],24, color='green')
plt.plot(testpt[:,1], testpt[:,0],24, '-', color='b')
plt.plot(testptsm[:,1], testptsm[:,0], '-', color='r', markersize=24)
plt.hold(True)
plt.axis([np.min(testpt[:,1]), np.max(testpt[:,1]), np.min(testpt[:,0]), np.max(testpt[:,0])])
plt.show()
# 3D, display cartesian
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(GPS_xyz[:,0], GPS_xyz[:,1], GPS_xyz[:,2], color=np.array([0.0, 0.0, 1.0]), linewidth=2.0)
ax.scatter(GPS_xyz[0,0], GPS_xyz[0,1], GPS_xyz[0,2], color=np.array([0.0, 1.0, 0.0]), linewidth=2.0)
func_set_axes_equal(ax)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
def predict(self,x,deriv=None,*args,**kwargs): if deriv is None: deriv = 0 if deriv == -1: return interpolate.splev(x,interpolate.splantider((self.knots,self.coeff,self.degree))) return interpolate.splev(x,(self.knots,self.coeff,self.degree),der=deriv)
plt.figure('Integ cubic')
plt.plot(angle_array_out, integ_cubic_sin_array)
plt.plot(angle_array, -cos_array)
# In[11]:
# Performing the integral using integ_cubic
# An integration constant and the nodes of the cubic spline interpolation can be passed
# NB: passing the tck manually allows to interpolate on a different x array
from blond_common.maths.calculus import integ_cubic
from scipy.interpolate import splrep, splantider
tck = splrep(angle_array, sin_array)
tck_ader = splantider(tck)
angle_array_out, integ_cubic_sin_array = integ_cubic(np.linspace(
0, 3 * 2 * np.pi, n_points * 10),
0,
tck=tck,
tck_ader=tck_ader,
constant=-1)
plt.figure('Integ cubic-2')
plt.plot(angle_array_out, integ_cubic_sin_array)
plt.plot(angle_array, -cos_array)
# ## 2. Zeros finding
# In[12]:
