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 state_from_tfms_no_velocity(Ts, vlength=12):
"""
Ts : a list of transforms
vlength : in {6,12} : the number of variables in the state-vector
if 12 : zero velocities are returned
if 6 : only xyzrpy are returned
"""
N = len(Ts)
Xs = np.zeros((N, vlength))
for i in xrange(N):
Xs[i,0:3] = Ts[i][0:3,3]
if vlength==6:
Xs[i,3:6] = np.array(tfms.euler_from_matrix(Ts[i]))
else:
Xs[i,6:9] = np.array(tfms.euler_from_matrix(Ts[i]))
return Xs
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)
def canonicalize_obs(self, T_obs):
"""
Returns the position and translation from T_obs (4x4 mat).
Puts the rotation in a form which makes it closer to filter's
current estimate in absolute terms.
"""
pos = T_obs[0:3,3]
rpy = np.array(tfm.euler_from_matrix(T_obs), ndmin=2).T
rpy = closer_angle(rpy, self.x_filt[6:9])
return (pos, rpy)
def state_from_tfms(Ts, dt=1./30.):
"""
returns (n-1)x12 matrix of the states of kalman filter
derived from the list 'Ts' of transforms logged at
frequency = 1/dt.
The 'n' above is the number of transforms in the list 'Ts'.
"""
N = len(Ts)
Xs = np.empty((N-1, 12))
for i in xrange(N-1):
if i==0:
pos_prev = Ts[0][0:3,3]
rpy_prev = np.array(tfms.euler_from_matrix(Ts[0]))
else:
pos_prev = Xs[i-1,0:3]
rpy_prev = Xs[i-1,6:9]
Xs[i,0:3] = Ts[i+1][0:3,3]
Xs[i,3:6] = (Xs[i,0:3] - pos_prev)/dt
Xs[i,6:9] = np.array(tfms.euler_from_matrix(Ts[i+1]))
Xs[i,9:12] = (closer_angle(Xs[i,6:9], rpy_prev) - rpy_prev)/dt
return Xs
def add_noise(Ts, rotn = 5, xn = 0.0):
"""
Adds noise to each transform in the list of transforms.
rotn : standard dev of roll,pitch,yaw noise
xn : standard dev of translation noise
"""
rotn = np.deg2rad(rotn)
Tn = []
for t in Ts:
rpy = np.array(tfms.euler_from_matrix(t))
rpy += rotn*np.random.randn(3)
tn = tfms.euler_matrix(rpy[0], rpy[1], rpy[2])
tn[0:3,3] = t[0:3,3] + xn*np.random.randn(3)
Tn.append(tn)
return Tn
