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