def fit_spline_to_stream(strm, nsteps, deg=3):
x = []
y = []
prev_rpy = None
for i in xrange(nsteps):
next = soft_next(strm)
if next is not None:
pos = next[0:3,3]
rpy = np.array(tfms.euler_from_matrix(next), ndmin=2).T
rpy = np.squeeze(rpy)
if prev_rpy is not None:
rpy = closer_angle(rpy, prev_rpy)
prev_rpy = rpy
x.append(i)
y.append(pos.tolist()+rpy.tolist())
x = np.asarray(x)
y = np.asarray(y)
s = len(x)*.001**2
(tck, _) = si.splprep(y.T, s=s, u=x, k=deg)
new_x = xrange(nsteps)
xyzrpys = np.r_[si.splev(new_x, tck)].T
smooth_tfms = []
for xyzrpy in xyzrpys:
tfm = tfms.euler_matrix(*xyzrpy[3:6])
tfm[0:3,3] = xyzrpy[0:3]
smooth_tfms.append(tfm)
return smooth_tfms
def unbreak(rpy):
"""
removes discontinuity in rpy (nx3 matrix).
"""
if len(rpy)==1: return
un_rpy = np.empty(rpy.shape)
un_rpy[0,:] = rpy[0,:]
for i in xrange(1,len(rpy)):
if i==1:
a_prev = rpy[i-1,:]
else:
a_prev = un_rpy[i-1,:]
a_now = rpy[i,:]
un_rpy[i,:] = closer_angle(a_now, a_prev)
return un_rpy
def fit_spline_to_tf_stream(strm, new_freq, deg=3):
"""
Interpolates a stream of transforms using splines, such that
there is no "NONE" when sampling the stream at NEW_FREQ frequency.
Returns a stream of transforms.
"""
tfs, ts = strm.get_data()
tstart = strm.get_start_time()
tmax = ts[-1]
ndt = 1./new_freq
new_ts = np.arange(tstart, tmax+ndt/4., ndt/2.)
## get data in xyzrpy format (6xn) matrix:
N = len(tfs)
tf_dat = np.empty((6, N))
tf_dat[0:3,0] = tfs[0][0:3,3]
tf_dat[3:6,0] = tfms.euler_from_matrix(tfs[0])
for i in xrange(1,N):
now_tf = tfs[i]
tf_dat[0:3,i] = now_tf[0:3,3]
prev_rpy = tf_dat[3:6,i-1]
now_rpy = tfms.euler_from_matrix(now_tf)
now_rpy = closer_angle(now_rpy, prev_rpy)
tf_dat[3:6,i] = now_rpy
blueprint("\t fitting spline to data (scipy) ..")
s = N*.001**2
(tck, _) = si.splprep(tf_dat, s=s, u=ts, k=deg)
blueprint("\t evaluating spline at new time-stamps ..")
interp_xyzrpys = np.r_[si.splev(new_ts, tck)].T
smooth_tfms = []
for xyzrpy in interp_xyzrpys:
tfm = tfms.euler_matrix(*xyzrpy[3:6])
tfm[0:3,3] = xyzrpy[0:3]
smooth_tfms.append(tfm)
return streamize(smooth_tfms, new_ts, new_freq, strm.favg, tstart)
