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()
