def get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs): """ Return the first state (mean, covar) to initialize the kalman filter with. Assumes that the time-stamps start at a common zero (are on the same time-scale). Returns a state b/w t=[0, dt] Gives priority to AR-markers. If no ar-markers are found in [0,dt], it returns hydra's estimate but with larger covariance. """ ar1 = [c1_tfs[i] for i in xrange(len(c1_ts)) if c1_ts[i] <= dt] ar2 = [c2_tfs[i] for i in xrange(len(c2_ts)) if c2_ts[i] <= dt] hy = [hy_tfs[i] for i in xrange(len(hy_ts)) if hy_ts[i] <= dt] if ar1 != [] or ar2 != []: ar1.extend(ar2) x0 = state_from_tfms_no_velocity([avg_transform(ar1)]) I3 = np.eye(3) S0 = scl.block_diag(1e-3*I3, 1e-2*I3, 1e-3*I3, 1e-3*I3) else: assert len(hy)!=0, colorize("No transforms found for KF initialization. Aborting.", "red", True) x0 = state_from_tfms_no_velocity([avg_transform(hy)]) I3 = np.eye(3) S0 = scl.block_diag(1e-1*I3, 1e-1*I3, 1e-2*I3, 1e-2*I3) return (x0, S0)
def fit_gpr(data_file, calib_file, freq, use_spline=False, customized_shift=None, single_camera=False, plot_commands='s12fh'): #dat = cp.load(open(data_file)) #if dat.has_key('r'): if True: """ T_hy, T_cam_gt = get_synced_data(data_file, calib_file, 'r', freq, use_spline, customized_shift, single_camera) f = open('grp.dat', 'w') pickle.dump([T_cam_gt, T_hy], f) f.close() """ T_cam_gt, T_hy = pickle.load(open('grp.dat', 'r')) BLOCK_SIZE = 10 N = len(T_hy) train, tst = gen_indices(N, BLOCK_SIZE) T_hy_train = [T_hy[i] for i in train] T_hy_test = [T_hy[i] for i in tst] T_cam_gt_train = [T_cam_gt[i] for i in train] T_cam_gt_test = [T_cam_gt[i] for i in tst] """ #print "tuning hyper-params.." #hi_params = ec.train_hyperparams(T_cam_gt_train, T_hy_train, state_train) #cp.dump(hi_params, open('hyper-params.cpkl', 'wb')) ## save the hyper-parameters to a file. hi_params = cp.load(open('hyper-params.cpkl')) print hi_params print "GRP correcting..." T_test_ests = ec.gp_correct_poses(T_cam_gt_train, T_hy_train, state_train, T_hy_test, state_test, hi_params) print " .. done" X_est = state_from_tfms_no_velocity(T_test_ests).T X_est = np.c_[X_est[:,0], X_est] ########################################### """ gpr_model = hd_gpr() #gpr_model.train(T_cam_gt_train[0:10], T_hy_train[0:10]) gpr_model.train(T_cam_gt, T_hy) gpr_model.save_gpr_model("gpr_model.pkl") #gpr_model.load_gpr_model("gpr_model.pkl") T_corr_test = gpr_model.correct_poses(T_hy_test) ## plot the corrected poses: to_plot = [0,1,2,6,7,8] # for xyz rpy axlabels = ['x', 'y', 'z', 'roll', 'pitch', 'yaw'] X_hy_test = state_from_tfms_no_velocity(T_hy_test).T X_cam_gt_test = state_from_tfms_no_velocity(T_cam_gt_test).T X_est = state_from_tfms_no_velocity(T_corr_test).T for count,i in enumerate(to_plot): plt.subplot(2,3,count+1) plt.plot(X_hy_test[i,:], label='hydra') plt.plot(X_cam_gt_test[i,:], label='cam_gt') plt.plot(X_est[i,:], label='gpr_est') plt.ylabel(axlabels[count]) plt.legend() plt.show()
def correct_poses(self, Ts_in): """ Returns a list of corrected transforms. Ts_in is a list of transforms to correct. Note : Only the xyz are corrected. """ if not self.trained: print "GPR model not trained. Cannot predict before training. Returning..." return state_in = state_from_tfms_no_velocity(Ts_in) state_in = np.c_[state_in[:,0:3], state_in[:,6:9]] ## get rid of position and rotation velocities. # systematic correction: T_sys = self.train_data['T_sys'] R_sys, t_sys = T_sys[0:3,0:3], T_sys[0:3,3] xs_in = [tfm[0:3,3] for tfm in Ts_in] x_sys_corrected = [R_sys.dot(x) + t_sys for x in xs_in] x_sys_corrected = np.array(x_sys_corrected) # GP correction: x_gp_corrected = self.__gpr_correct(x_sys_corrected, state_in) Ts_corrected = [] for i,tfm in enumerate(Ts_in): tfm[0:3,3] = x_gp_corrected[i,:] Ts_corrected.append(tfm) return Ts_corrected
def train(self, Ts_gt, Ts_noisy): """ Train the GPR. Ts_gt : A list of ground-truth poses. Ts_noisy : A list of noisy poses. """ ## extract data in the required format: N = len(Ts_gt) assert N==len(Ts_noisy), "GPR training : lengths are not the same." Xs_gt = state_from_tfms_no_velocity(Ts_gt) Xs_noisy = state_from_tfms_no_velocity(Ts_noisy) xs_noisy = Xs_noisy[:,0:3] xs_gt = Xs_gt[:,0:3] state_noisy = np.c_[Xs_noisy[:,0:3], Xs_noisy[:,6:9]] self.train_data['Xs_gt'] = xs_gt self.train_data['Xs_noisy'] = xs_noisy self.train_data['state_noisy'] = state_noisy ## fit systematic transform (using SVD): R_sys, t_sys = self.sys_correct(xs_gt.T, xs_noisy.T) T_sys = np.eye(4) T_sys[0:3,0:3] = R_sys T_sys[0:3,3] = t_sys self.train_data['T_sys'] = T_sys print "Systematic transform : ", T_sys xs_sys_corrected = R_sys.dot(xs_noisy.T) + t_sys[:,None] xs_sys_corrected = xs_sys_corrected.T ## fit GPR model: hparams, alphas = self.__gpr_precompute(xs_gt, xs_sys_corrected, state_noisy) self.train_data['loghyper'] = hparams self.train_data['alphas'] = alphas self.trained = True
def get_first_state(tf_streams, freq, start_time): """ Returns the first state and covariance given : - TF_STREAMS : a list of streams of transform data. - FREQ : the frequency of these streams (and also the filter). """ dt = 1./freq n_streams = len(tf_streams) tfs0 = [] for i in xrange(n_streams): tfs, ts = tf_streams[i].get_data() for ti, t in enumerate(ts): if t <= dt + start_time: tfs0.append(tfs[ti]) if len(tfs0)==0: for i in xrange(n_streams): tfs, ts = tf_streams[i].get_data() for ti, t in enumerate(ts): if t <= 6*dt + start_time: tfs0.append(tfs[ti]) # import IPython # IPython.embed() I3 = np.eye(3) S0 = scl.block_diag(1e-3*I3, 1e-2*I3, 1e-3*I3, 1e-3*I3) if len(tfs0)==0: redprint("Cannot find initial state for KF: no data found within dT in all streams") x0 = state_from_tfms_no_velocity([]) else: x0 = state_from_tfms_no_velocity([avg_transform(tfs0)]) return (x0, S0)
def plot_tf_streams(tf_strms, strm_labels, styles=None, title=None, block=True): """ Plots the x,y,z,r,p,y from a list TF_STRMS of streams. """ assert len(tf_strms)==len(strm_labels) if styles!=None: assert len(tf_strms)==len(styles) else: styles = ['.']*len(tf_strms) plt.figure() ylabels = ['x', 'y', 'z', 'r', 'p', 'y'] n_streams = len(tf_strms) Xs = [] inds = [] for strm in tf_strms: tfs, ind = [], [] for i,tf in enumerate(strm): if tf != None: tfs.append(tf) ind.append(i) X = state_from_tfms_no_velocity(tfs, 6) Xs.append(X) inds.append(ind) for i in xrange(6): plt.subplot(2,3,i+1) plt.hold(True) for j in xrange(n_streams): xj = Xs[j] ind_j = inds[j] plt.plot(ind_j, xj[:,i], styles[j], label=strm_labels[j]) plt.ylabel(ylabels[i]) plt.legend() if title!=None: plt.gcf().suptitle(title) plt.show(block=block)
def get_synced_data(data_file, calib_file, lr, freq, use_spline, customized_shift, single_camera, plot=False): """ Returns two lists : (1) estimate of the hydra-sensor from hydra-base. (2) estimate of the hydra-sensor from the camera in hydra-bases frame. """ ## The hydra and camera data streams are not synced (some delay issues), therefore ## first remove that shift using correlation shift. _, _, _, ar1_strm, ar2_strm, hy_strm = relative_time_streams(data_file, lr, freq, single_camera) ## run the kalman filter on just the camera-data. nsteps, tmin, F_means, S, A,R = run_kalman_filter(data_file, lr, freq, use_spline, False, single_camera) X_kf = np.array(F_means) X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T if use_spline: smooth_hy = (t for t in fit_spline_to_stream(hy_strm, nsteps)) else: smooth_hy = hy_strm indices_ar1 = [] indices_ar2 = [] indices_hy = [] Ts_ar1 = [] Ts_ar2 = [] Ts_hy = [] for i in xrange(nsteps): ar1_est = soft_next(ar1_strm) ar2_est = soft_next(ar2_strm) hy_est = soft_next(smooth_hy) if ar1_est != None: Ts_ar1.append(ar1_est) indices_ar1.append(i) if ar2_est != None: Ts_ar2.append(ar2_est) indices_ar2.append(i) if hy_est != None: Ts_hy.append(hy_est) indices_hy.append(i) X_ar1 = state_from_tfms_no_velocity(Ts_ar1).T X_ar2 = state_from_tfms_no_velocity(Ts_ar2).T X_hy = state_from_tfms_no_velocity(Ts_hy).T ## shift the ar1 data to sync it up with the hydra data: if customized_shift != None: shift = customized_shift else: shift = correlation_shift(X_kf, X_hy) ## shift the indices of ar1 by the SHIFT calculated above: indices_hy = [idx + shift for idx in indices_hy] indices_hy = [x for x in indices_hy if 0<= x and x < nsteps] Ts_hy_matching = [] Ts_ar1_matching = [] for i,x in enumerate(indices_ar1): try: idx = indices_hy.index(x) Ts_hy_matching.append(Ts_hy[idx]) Ts_ar1_matching.append(Ts_ar1[i]) except: pass print colorize("Found %s corresponding data points b/w camera and hydra." % colorize("%d"%len(Ts_ar1_matching), "red", True), "blue", True) if plot: X_ar_matching = state_from_tfms_no_velocity(Ts_ar1_matching).T X_hy_matching = state_from_tfms_no_velocity(Ts_hy_matching).T plot_kalman_core(X_ar_matching, X_hy_matching, None, None, None, None, None, None, 'fs') plt.show() return (Ts_hy_matching, Ts_ar1_matching)
def plot_kalman_lr(data_file, calib_file, lr, freq, use_spline, customized_shift, single_camera, plot_commands): ''' input: data_file lr: 'l' or 'r' freq use_spline customized_shift: custimized shift between smoother and filter: to compensate for the lag of smoother ''' _, _, _, ar1_strm, ar2_strm, hy_strm = relative_time_streams(data_file, lr, freq, single_camera) ## run kalman filter: nsteps, tmin, F_means,S,A,R = run_kalman_filter(data_file, lr, freq, use_spline, True, single_camera) ## run kalman smoother: S_means, _ = smoother(A, R, F_means, S) X_kf = np.array(F_means) X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T X_ks = np.array(S_means) X_ks = np.reshape(X_ks, (X_ks.shape[0], X_ks.shape[1])).T # Shifting if customized_shift != None: shift = customized_shift else: shift = correlation_shift(X_kf, X_ks) X_ks = np.roll(X_ks,shift,axis=1) X_ks[:,:shift] = X_ks[:,shift][:,None] ## frame of the filter estimate: indices_ar1 = [] indices_ar2 = [] indices_hy = [] Ts_ar1 = [] Ts_ar2 = [] Ts_hy = [] if use_spline: smooth_hy = (t for t in fit_spline_to_stream(hy_strm, nsteps)) else: smooth_hy = hy_strm for i in xrange(nsteps): ar1_est = soft_next(ar1_strm) ar2_est = soft_next(ar2_strm) hy_est = soft_next(smooth_hy) if ar1_est != None: Ts_ar1.append(ar1_est) indices_ar1.append(i) if ar2_est != None: Ts_ar2.append(ar2_est) indices_ar2.append(i) if hy_est != None: Ts_hy.append(hy_est) indices_hy.append(i) X_ar1 = state_from_tfms_no_velocity(Ts_ar1).T X_ar2 = state_from_tfms_no_velocity(Ts_ar2).T X_hy = state_from_tfms_no_velocity(Ts_hy).T plot_kalman_core(X_kf[:,1:], X_ks[:,1:], X_ar1, indices_ar1, X_ar2, indices_ar2, X_hy, indices_hy, plot_commands) plt.show()