def add_first_pose_priors(graphopt, data, dataset_type):
prior_cov = np.array([1e-6, 1e-6, 1e-6])
if (dataset_type == 'nav2d'):
keys = [gtsam.symbol(ord('x'), 0)]
graphopt.init_vals.insert(keys[0],
tf_utils.vec3_to_pose2(data['poses_gt'][0]))
graphopt.graph = add_unary_factor(graphopt.graph, keys, prior_cov,
data['poses_gt'][0])
elif (dataset_type == 'push2d'):
keys = [gtsam.symbol(ord('o'), 0)]
graphopt.init_vals.insert(
keys[0], tf_utils.vec3_to_pose2(data['obj_poses_gt'][0]))
graphopt.graph = add_unary_factor(graphopt.graph, keys, prior_cov,
data['obj_poses_gt'][0])
keys = [gtsam.symbol(ord('e'), 0)]
graphopt.init_vals.insert(
keys[0], tf_utils.vec3_to_pose2(data['ee_poses_gt'][0]))
graphopt.graph = add_unary_factor(graphopt.graph, keys, prior_cov,
data['ee_poses_gt'][0])
graphopt = optimizer_update(graphopt)
graphopt = reset_graph(graphopt)
return graphopt
def plot_data(params, logger, plot_ori=False):
plt.ion()
plt.close('all')
fig = plt.figure(figsize=(12, 8))
poses_gt = np.asarray(logger.poses_gt)
meas_odom = np.asarray(logger.meas_odom)
meas_gps = np.asarray(logger.meas_gps)
num_steps = params.num_steps
poses_odom = np.zeros((num_steps, 3))
poses_gps = np.zeros((num_steps, 3))
poses_odom[0, :] = poses_gt[0, :]
# compute poses
for tstep in range(0, num_steps):
if (tstep > 0):
poses_odom[tstep] = tf_utils.pose2_to_vec3(tf_utils.vec3_to_pose2(
poses_odom[tstep-1, :]).compose(tf_utils.vec3_to_pose2(meas_odom[tstep-1, :])))
poses_gps[tstep, :] = meas_gps[tstep, :]
# plot poses
for tstep in range(num_steps-1, num_steps):
plt.cla()
plt.xlim(params.env.area.xmin, params.env.area.xmax)
plt.ylim(params.env.area.ymin, params.env.area.ymax)
plt.scatter([0], [0], marker='*', c='k', s=20,
alpha=1.0, zorder=3, edgecolor='k')
plt.scatter(poses_gt[tstep, 0], poses_gt[tstep, 1], marker=(3, 0, poses_gt[tstep, 2]/np.pi*180),
color='dimgray', s=300, alpha=0.25, zorder=3, edgecolor='dimgray')
plt.plot(poses_gt[0:tstep, 0], poses_gt[0:tstep, 1], color=params.plot.colors[0], linewidth=2, label="groundtruth")
plt.plot(poses_odom[0:tstep, 0], poses_odom[0:tstep, 1], color=params.plot.colors[1], linewidth=2, label="odom")
plt.plot(poses_gps[0:tstep, 0], poses_gps[0:tstep, 1], color=params.plot.colors[2], linewidth=2, label="gps")
# if plot_ori:
# ori = poses_gt[:, 2]
# sz_arw = 0.03
# (dx, dy) = (sz_arw * np.cos(ori), sz_arw * np.sin(ori))
# for i in range(0, num_steps):
# plt.arrow(poses_gt[i, 0], poses_gt[i, 1], dx[i], dy[i], linewidth=4,
# head_width=0.01, color='black', head_length=0.1, fc='black', ec='black')
plt.title("Logged dataset nav2d/{0:04d}/{1:04d}.json".format(params.dataio.ds_idx, params.dataio.seq_idx))
plt.legend(loc='upper right')
plt.show()
plt.pause(1)
def transform_episodes_common_frame(episode_list, push_data):
push_data_tf = push_data
push_data_idxs = []
episode_idxs = []
for episode in episode_list:
push_data_idxs_curr = [idx for idx, val in enumerate(
push_data['contact_episode']) if (val == episode)]
push_data_idxs.append(push_data_idxs_curr)
episode_idxs.append([episode] * len(push_data_idxs_curr))
push_data_idxs = [item for sublist in push_data_idxs for item in sublist]
episode_idxs = [item for sublist in episode_idxs for item in sublist]
num_steps = len(push_data_idxs)
pose_idx, prev_pose_idx = -1, -1
obj_prevseq_last_pose = None
prevseq_pose_set = False
for tstep in range(num_steps):
prev_pose_idx = pose_idx
pose_idx = pose_idx + 1
push_idx = push_data_idxs[pose_idx]
if ((tstep != 0) & (episode_idxs[prev_pose_idx] != episode_idxs[pose_idx])):
prev_push_idx = push_data_idxs[prev_pose_idx]
obj_prevseq_last_pose = tf_utils.vec3_to_pose2(push_data['obj_poses_2d'][prev_push_idx])
obj_newseq_first_pose = tf_utils.vec3_to_pose2(push_data['obj_poses_2d'][push_idx])
prevseq_pose_set = True
if prevseq_pose_set:
obj_pose = tf_utils.vec3_to_pose2(push_data['obj_poses_2d'][push_idx])
ee_pose = tf_utils.vec3_to_pose2(push_data['ee_poses_2d'][push_idx])
# transform obj pose
obj_pose_tf = obj_prevseq_last_pose.compose(obj_newseq_first_pose.between(obj_pose))
# transform endeff pose
ee_pose_tf = obj_pose_tf.compose(obj_pose.between(ee_pose))
push_data_tf['obj_poses_2d'][push_idx] = tf_utils.pose2_to_vec3(obj_pose_tf)
push_data_tf['ee_poses_2d'][push_idx] = tf_utils.pose2_to_vec3(ee_pose_tf)
return push_data_tf
def create_measurements(params, poses, covariances):
# noise models
odom_noise0 = gtsam.noiseModel_Diagonal.Sigmas(covariances.odom0)
odom_noise1 = gtsam.noiseModel_Diagonal.Sigmas(covariances.odom1)
gps_noise0 = gtsam.noiseModel_Diagonal.Sigmas(covariances.gps0)
gps_noise1 = gtsam.noiseModel_Diagonal.Sigmas(covariances.gps1)
# samplers
sampler_odom_noise0 = gtsam.Sampler(odom_noise0, 0)
sampler_odom_noise1 = gtsam.Sampler(odom_noise1, 0)
sampler_gps_noise0 = gtsam.Sampler(gps_noise0, 0)
sampler_gps_noise1 = gtsam.Sampler(gps_noise1, 0)
# init measurements
measurements = AttrDict()
num_steps = params.num_steps
measurements.odom = np.zeros((num_steps-1, 3))
measurements.gps = np.zeros((num_steps, 3))
measurements.cov_type = np.zeros((num_steps, 1))
# add measurements
for tstep in range(0, num_steps):
cov_type = covariance_type(tstep / float(num_steps))
sampler_odom_noise = sampler_odom_noise0 if (cov_type == 0) else sampler_odom_noise1
sampler_gps_noise = sampler_gps_noise0 if (cov_type == 0) else sampler_gps_noise1
measurements.cov_type[tstep] = cov_type
# binary odom
if (tstep > 0):
prev_pose = tf_utils.vec3_to_pose2(poses[tstep-1])
curr_pose = tf_utils.vec3_to_pose2(poses[tstep])
delta_pose = prev_pose.between(curr_pose)
delta_pose_noisy = tf_utils.add_gaussian_noise(delta_pose, sampler_odom_noise.sample())
measurements.odom[tstep-1, :] = tf_utils.pose2_to_vec3(delta_pose_noisy)
# unary gps
curr_pose = tf_utils.vec3_to_pose2(poses[tstep])
curr_pose_noisy = tf_utils.add_gaussian_noise(curr_pose, sampler_gps_noise.sample())
measurements.gps[tstep, :] = tf_utils.pose2_to_vec3(curr_pose_noisy)
return measurements
def add_binary_odom_factor(graph, keys, factor_cov, factor_meas):
factor_noise_model = get_noise_model(factor_cov)
factor_meas_pose = tf_utils.vec3_to_pose2(factor_meas)
factor = gtsam.BetweenFactorPose2(
keys[0], keys[1], factor_meas_pose, factor_noise_model)
graph.push_back(factor)
return graph
def compute_x_samples(mean, dx_samples):
num_samples, num_x, dim_x = dx_samples.shape
x_samples = np.zeros(dx_samples.shape)
for sidx in range(0, num_samples):
for xidx in range(0, num_x):
x_mean = tf_utils.vec3_to_pose2(mean[xidx, :])
x_sample_pose = x_mean.retract(dx_samples[sidx, xidx, :])
x_samples[sidx, xidx, :] = tf_utils.pose2_to_vec3(x_sample_pose)
return x_samples
def compute_x_samples(mean, dx_samples):
# mean: n_steps x 3
# dx_samples: n_samples x n_steps x 3
S, T, D = dx_samples.shape
x_samples = np.zeros(dx_samples.shape)
for sidx in range(0, S):
for tidx in range(0, T):
x_mean = tf_utils.vec3_to_pose2(mean[tidx, :])
x_sample_pose = x_mean.retract(dx_samples[sidx, tidx, :])
x_samples[sidx, tidx, :] = tf_utils.pose2_to_vec3(x_sample_pose)
return x_samples
def log_data(params, dataset, ds_idxs, eps_idxs, seq_idx, save_file=False):
# init noise models
sigma_mat_ee = list(params.measurements.noise_models.ee_pose_prior)
sigma_mat_qs = list(params.measurements.noise_models.qs_push_motion)
sigma_mat_sdf = list(params.measurements.noise_models.sdf_intersection)
sigma_mat_tactile = list(params.measurements.noise_models.tactile_rel_meas)
sigma_mat_interseq = list(params.measurements.noise_models.binary_interseq_obj)
ee_pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array(params.measurements.noise_models.ee_pose_prior))
sampler_ee_pose_prior_noise = gtsam.Sampler(ee_pose_prior_noise, 0)
# init other params
factor_names, factor_keysyms, factor_keyids, factor_covs, factor_meas = ([] for i in range(5))
num_steps = params.dataio.num_steps
wmin, wmax = params.measurements.tactile.wmin, params.measurements.tactile.wmax
wnum = 2
meas_ee_prior = []
meas_tactile_img_feats = []
for tstep in range(0, num_steps):
# ee_pose_prior
ee_pose_noisy = tf_utils.add_gaussian_noise(tf_utils.vec3_to_pose2(
dataset['ee_poses_2d'][ds_idxs[tstep]]), sampler_ee_pose_prior_noise.sample())
factor_names.append('ee_pose_prior')
factor_keysyms.append(['e'])
factor_keyids.append([tstep])
factor_covs.append(sigma_mat_ee)
factor_meas.append(tf_utils.pose2_to_vec3(ee_pose_noisy))
# qs_push_motion
if (tstep > 0) & (eps_idxs[tstep-1] == eps_idxs[tstep]):
factor_names.append('qs_push_motion')
factor_keysyms.append(['o', 'o', 'e', 'e'])
factor_keyids.append([tstep-1, tstep, tstep-1, tstep])
factor_covs.append(sigma_mat_qs)
factor_meas.append([0., 0., 0.])
# sdf_intersection
factor_names.append('sdf_intersection')
factor_keysyms.append(['o', 'e'])
factor_keyids.append([tstep, tstep])
factor_covs.append(sigma_mat_sdf)
factor_meas.append([0.])
# tactile_rel
if (tstep > wmin):
# wmax_step = np.minimum(tstep, wmax)
# wrange = np.linspace(wmin, wmax_curr_step, num=wnum).astype(np.int)
# for w in wrange:
wmax_step = np.minimum(tstep, wmax)
for w in range(wmin, wmax_step):
# skip inter episode factors
if (eps_idxs[tstep-w] != eps_idxs[tstep]):
continue
# print([tstep-w, tstep, tstep-w, tstep])
factor_names.append('tactile_rel')
factor_keysyms.append(['o', 'o', 'e', 'e'])
factor_keyids.append([tstep-w, tstep, tstep-w, tstep])
factor_covs.append(sigma_mat_tactile)
factor_meas.append((dataset['img_feats'][ds_idxs[tstep-w]], dataset['img_feats'][ds_idxs[tstep]]))
# interseq binary
if (tstep > 0) & (eps_idxs[tstep-1] != eps_idxs[tstep]):
factor_names.append('binary_interseq_obj')
factor_keysyms.append(['o', 'o'])
factor_keyids.append([tstep-1, tstep])
factor_covs.append(sigma_mat_interseq)
factor_meas.append([0., 0., 0.])
# store external measurement separately
meas_ee_prior.append(tf_utils.pose2_to_vec3(ee_pose_noisy))
meas_tactile_img_feats.append(dataset['img_feats'][ds_idxs[tstep]])
# save to logger object
logger = AttrDict()
logger.ee_poses_gt = [dataset['ee_poses_2d'][idx] for idx in ds_idxs]
logger.obj_poses_gt = [dataset['obj_poses_2d'][idx] for idx in ds_idxs]
logger.contact_episode = [dataset['contact_episode'][idx] for idx in ds_idxs]
logger.contact_flag = [dataset['contact_flag'][idx] for idx in ds_idxs]
logger.factor_names = factor_names
logger.factor_keysyms = factor_keysyms
logger.factor_keyids = factor_keyids
logger.factor_covs = factor_covs
logger.factor_meas = factor_meas
logger.meas_ee_prior = meas_ee_prior
logger.meas_tactile_img_feats = meas_tactile_img_feats
logger = wrap_logger_angles_to_pi(logger, field_names=['ee_poses_gt', 'obj_poses_gt', 'meas_ee_prior'])
logger.logname = "{0}_{1}".format(
params.dataio.dataset_name, datetime.now().strftime("%m-%d-%Y-%H-%M-%S"))
if save_file:
dataset_mode = "train" if (seq_idx < params.dataio.n_data_train) else "test"
filename = "{0}/{1}/{2:04d}.json".format(params.dataio.dstdir_logger, dataset_mode, seq_idx)
dir_utils.write_file_json(filename=filename, data=logger)
return logger