def run_kalman_and_plot(ar_cov_scale, hydra_cov_scale):
"""
Runs the kalman filter and plots the results.
` """
dt = 1/30.
Ts_bh, Ts_bg, T_gh, Ts_bh_hg, X_bg, X_bh_hg, Ts_ba_ag, X_ba_ag_t, ar_valid_stamps = load_data()
## initialize the kalman belief:
x_init = X_bg[:,0]
I3 = np.eye(3)
S_init = scl.block_diag(1e-6*I3, 1e-3*I3, 1e-4*I3, 1e-1*I3)
## run the kalman filter:
Ts_obs = [np.eye(4) for _ in xrange(len(Ts_bh))]
f_estimates, f_covariances, A, R = run_kalman_filter(Ts_bh_hg, Ts_ba_ag, x_init, S_init,ar_cov_scale, hydra_cov_scale, 1./dt)
X_kf = np.array(f_estimates)
X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T
## plot the results:
#plot_kalman(X_kf[:,1:], X_bh_hg, X_bg, X_ba_ag_t, ar_valid_stamps)
#plt.show()
## run the kalman smoother:
f_covs = []
for f_cov in f_covariances:
f_covs.append(1e-3 * f_cov)
s_estimates = smoother(A, R, f_estimates, f_covs)
X_ks = np.array(s_estimates)
X_ks = np.reshape(X_ks, (X_ks.shape[0], X_ks.shape[1])).T
## plot the results:
plot_kalman(X_kf[:,1:], X_ks[:,1:], X_bh_hg, X_bg, X_ba_ag_t, ar_valid_stamps)
plt.show()
def traj_kalman_lr(data_file, calib_file, lr, freq, use_spline, customized_shift, single_camera):
'''
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
return trajectory data
'''
_, _, _, 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]
T_filter = state_to_hmat(list(X_ks.T))
T_smoother = state_to_hmat(list(X_kf.T))
## load the potentiometer-angle stream:
pot_data = cp.load(open(data_file))[lr]['pot_angles']
ang_ts = np.array([tt[1] for tt in pot_data]) ## time-stamps
ang_vals = [open_frac(tt[0]) for tt in pot_data] ## angles
ang_strm = streamize(ang_vals, ang_ts, freq, lambda x : x[-1], tmin)
ang_strm_vals = []
prev_ang = 0
for i in xrange(nsteps):
ang_val = soft_next(ang_strm)
if ang_val != None:
prev_ang = ang_val
ang_val = [ang_val]
else:
ang_val = [prev_ang]
ang_strm_vals.append(ang_val)
stamps = []
for i in xrange(nsteps):
stamps.append(tmin + i * 1.0 / freq)
traj = {"tfms": T_filter, "tfms_s": T_smoother, "pot_angles": ang_strm_vals, "stamps": stamps}
return traj
def rviz_kalman(demo_dir, bag_file, data_file, calib_file, freq, use_rgbd, use_smoother, use_spline, customized_shift, single_camera):
'''
For rgbd, data_file and bag_file are redundant
Otherwise, demo_dir is redundant
'''
if use_rgbd:
bag_file = osp.join(demo_dir, 'demo.bag')
rgbd1_dir = osp.join(demo_dir, 'camera_#1')
rgbd2_dir = osp.join(demo_dir, 'camera_#2')
data_file = osp.join(demo_dir, 'demo.data')
bag = rosbag.Bag(bag_file)
else:
bag = rosbag.Bag(bag_file)
dat = cp.load(open(data_file))
grippers = dat.keys()
pub = rospy.Publisher('/point_cloud1', PointCloud2)
pub2= rospy.Publisher('/point_cloud2', PointCloud2)
c1_tfm_pub = {}
c2_tfm_pub = {}
hydra_tfm_pub = {}
T_filt = {}
ang_strm = {}
ar1_strm = {}
ar2_strm = {}
hy_strm = {}
smooth_hy = {}
## publishers for unfiltered-data:
for lr in grippers:
c1_tfm_pub[lr] = rospy.Publisher('/%s_ar1_estimate'%(lr), PoseStamped)
c2_tfm_pub[lr] = rospy.Publisher('/%s_ar2_estimate'%(lr), PoseStamped)
hydra_tfm_pub[lr] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped)
_, _, _, ar1_strm[lr], ar2_strm[lr], hy_strm = relative_time_streams(data_file, lr, freq, single_camera)
## run the kalman filter:
nsteps, tmin, F_means, S,A,R = run_kalman_filter(data_file, lr, freq, use_spline, True, single_camera)
## run the 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 between filter and smoother:
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]
if use_smoother:
T_filt[lr] = state_to_hmat(list(X_ks.T))
else:
T_filt[lr] = state_to_hmat(list(X_kf.T))
## load the potentiometer-angle stream:
pot_data = cp.load(open(data_file))[lr]['pot_angles']
ang_ts = np.array([tt[1] for tt in pot_data]) ## time-stamps
ang_vals = [tt[0] for tt in pot_data] ## angles
# plt.plot(ang_vals)
# plt.show()
ang_vals = [0*open_frac(x) for x in ang_vals]
ang_strm[lr] = streamize(ang_vals, ang_ts, freq, lambda x : x[-1], tmin)
if use_spline:
smooth_hy[lr] = (t for t in fit_spline_to_stream(hy_strm, nsteps))
else:
smooth_hy[lr] = hy_strm
## get the point-cloud stream
cam1_frame_id = '/camera1_rgb_optical_frame'
cam2_frame_id = '/camera2_rgb_optical_frame'
if use_rgbd:
pc1_strm = streamize_rgbd_pc(rgbd1_dir, cam1_frame_id, freq, tmin)
if single_camera:
pc2_strm = streamize_rgbd_pc(None, cam2_frame_id, freq, tmin)
else:
pc2_strm = streamize_rgbd_pc(rgbd2_dir, cam2_frame_id, freq, tmin)
else:
pc1_strm = streamize_pc(bag, '/camera1/depth_registered/points', freq, tmin)
if single_camera:
pc2_strm = streamize_pc(bag, None, freq, tmin)
else:
pc2_strm = streamize_pc(bag, '/camera2/depth_registered/points', freq, tmin)
## get the relative-transforms between the cameras:
cam_tfm = get_cam_transform(calib_file)
publish_static_tfm(cam1_frame_id, cam2_frame_id, cam_tfm)
## frame of the filter estimate:
sleeper = rospy.Rate(freq)
T_far = np.eye(4)
T_far[0:3,3] = [10,10,10]
handles = []
prev_ang = {'l': 0, 'r': 0}
for i in xrange(nsteps):
#raw_input("Hit next when ready.")
print "Kalman ts: ", tmin+(0.0+i)/freq
## show the point-cloud:
found_pc = False
try:
pc = pc1_strm.next()
if pc is not None:
print "pc1 ts:", pc.header.stamp.to_sec()
pc.header.stamp = rospy.Time.now()
pub.publish(pc)
found_pc = True
else:
print "pc1 ts:",None
except StopIteration:
print "pc1 ts: finished"
#print "no more point-clouds"
pass
try:
pc2 = pc2_strm.next()
if pc2 is not None:
#print "pc2 not none"
print "pc2 ts:", pc2.header.stamp.to_sec()
pc2.header.stamp = rospy.Time.now()
pub2.publish(pc2)
found_pc = True
else:
print "pc2 ts:", None
except StopIteration:
print "pc2 ts: finished"
pass
ang_vals = []
T_filt_lr = []
for lr in grippers:
ang_val = soft_next(ang_strm[lr])
if ang_val != None:
prev_ang[lr] = ang_val
ang_val = ang_val
else:
ang_val = prev_ang[lr]
ang_vals.append(ang_val)
T_filt_lr.append(T_filt[lr][i])
handles = draw_trajectory(cam1_frame_id, T_filt_lr, color=(1,1,0,1), open_fracs=ang_vals)
# draw un-filtered estimates:
for lr in grippers:
ar1_est = soft_next(ar1_strm[lr])
if ar1_est != None:
c1_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(ar1_est), cam1_frame_id))
else:
c1_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id))
ar2_est = soft_next(ar2_strm[lr])
if ar2_est != None:
c2_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(ar2_est), cam1_frame_id))
else:
c2_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id))
hy_est = soft_next(smooth_hy[lr])
if hy_est != None:
hydra_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(hy_est), cam1_frame_id))
else:
hydra_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id))
sleeper.sleep()
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()
