def add_all_nodes_old(self, node_id, metadata, planning_time, check_duplicates=True):
"""
"""
if self.log:
print self.node_idx
edge_alphas = []
for pose_ind, (pose, quality) in enumerate(zip(metadata['final_poses'], metadata['qualities'])):
# Check if this pose exists in the graph already
already_exists = False
if check_duplicates:
num_existing_nodes = len(self.G.nodes())
for j, node in self.G.nodes(data=True):
if is_equivalent_pose(pose, node['pose']):
already_exists = True
break
if not already_exists:
self.G.add_node(self.node_idx, pose=pose, gq=quality, node_type='state')
to_node_idx = self.node_idx
self.node_idx += 1
else:
to_node_idx = j
if self.log:
print 'edge from {} to {}={}'.format(node_id, to_node_idx, np.clip(np.max(metadata['vertex_probs'][:,pose_ind]), 0, 1))
self.G.add_edge(node_id, to_node_idx)
edge_alphas.append(np.clip(np.max(metadata['vertex_probs'][:,pose_ind]), 0, 1))
return edge_alphas
def visualize(env, datasets, obj_id_to_keys, models, model_names,
use_sensitivities):
for dataset, obj_id_to_key in zip(datasets, obj_id_to_keys):
for i in range(dataset.num_datapoints):
datapoint = dataset.datapoint(i)
dataset_name, key = obj_id_to_key[str(
datapoint['obj_id'])].split(KEY_SEP_TOKEN)
obj = env._state_space._database.dataset(dataset_name)[key]
orig_pose = to_rigid(datapoint['obj_pose'])
obj.T_obj_world = orig_pose
env.state.obj = obj
actual = 0
for i in range(NUM_PER_DATAPOINT):
mat = datapoint['actual_poses'][i * 4:(i + 1) * 4]
actual_pose = to_rigid(mat)
if not is_equivalent_pose(actual_pose, orig_pose):
actual += 1
actual /= float(NUM_PER_DATAPOINT)
probs = []
for model, model_name, use_sensitivity in zip(
models, model_names, use_sensitivities):
model.load_object(env.state)
_, vertex_probs, _ = model.predict(
[datapoint['vertex']],
[datapoint['normal']],
[-datapoint['normal']], # push dir
use_sensitivity=use_sensitivity)
probs.append(1 - vertex_probs[0, 0])
if -(abs(probs[3] - actual) - abs(probs[2] - actual)) > .1:
print 'actual {} {} {} {} {} {} {} {} {}'.format(
actual, model_names[0], probs[0], model_names[1], probs[1],
model_names[2], probs[2], model_names[3], probs[3])
env.render_3d_scene()
start_point = datapoint['vertex'] + .06 * datapoint['normal']
end_point = datapoint['vertex']
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0],
[0, 0, 1]]).dot(-datapoint['normal'])
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0],
[0, 0, 1]]).dot(-datapoint['normal'])
head_points = [
end_point - 0.02 * h2, end_point, end_point - 0.02 * h1
]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002)
vis3d.show()
def combine_equivalent_poses(self, final_poses, vertex_probs):
"""
Combines the probabilities of poses which are equivalent
except for rotations along the z axis or translations
Parameters
----------
final_poses : :obj:`list` of :obj:`StablePose`
list of poses the object ends up in after being toppled over each edge of size m
vertex_probs : nxm :obj:`numpy.ndarray`
probability of object landing in each of the m poses for each of the n topple actions
Returns
-------
:obj:`list` of :obj:`StablePose` of size o
list of unique Stable Poses
nxo :obj:`numpy.ndarray`
probability of object landing in each of the o unique stable poses
for each of the n topple actions
"""
i = 0
edge_inds = list(np.arange(vertex_probs.shape[1]))
grouped_poses = []
grouped_edges = []
while i < len(edge_inds):
equivalent_edges = [edge_inds[i]]
curr_edge_ind = edge_inds[i]
j = i + 1
while j < len(edge_inds):
if is_equivalent_pose(self.final_poses[curr_edge_ind],
self.final_poses[edge_inds[j]]):
equivalent_edges.append(edge_inds[j])
edge_inds.pop(j)
else:
j += 1
grouped_poses.append(self.final_poses[curr_edge_ind])
grouped_edges.append(equivalent_edges)
i += 1
vertex_probs = \
np.hstack([np.sum(vertex_probs[:,edges], axis=1).reshape(-1,1) for edges in grouped_edges])
return grouped_poses, vertex_probs
def add_all_nodes(self, node_id, metadata, planning_time, check_duplicates=True):
"""
"""
self.G.nodes[node_id]['planning_time'] = planning_time
edge_alphas = []
vertex_probs = metadata['vertex_probs']
metadata_to_graph_mapping = []
for pose, quality in zip(metadata['final_poses'], metadata['qualities']):
# Check if this pose exists in the graph already
already_exists = False
if check_duplicates:
num_existing_nodes = len(self.G.nodes())
for i, node in self.G.nodes(data=True):
if node['node_type'] == 'state' and is_equivalent_pose(pose, node['pose']):
already_exists = True
break
if not already_exists:
self.G.add_node(self.node_idx, pose=pose, gq=quality, value=quality, node_type='state')
metadata_to_graph_mapping.append(self.node_idx)
self.node_idx += 1
else:
metadata_to_graph_mapping.append(i)
for pose_ind in range(len(metadata['final_poses'])):
best_action = vertex_probs[np.argmax(vertex_probs[:,pose_ind])]
#self.G.add_node(self.action_node_idx, best_action=best_action, value=0, node_type='action')
action_node_idx = str(node_id)+str(metadata_to_graph_mapping[pose_ind])
self.G.add_node(action_node_idx, best_action=best_action, value=0, node_type='action')
self.G.add_edge(node_id, action_node_idx)
edge_alphas.append(1)
for prob, next_node_id, in zip(best_action, metadata_to_graph_mapping):
if prob != 0.0:
self.G.add_edge(action_node_idx, next_node_id, prob=prob)
edge_alphas.append(np.clip(prob, 0, 1))
#self.action_node_idx += 1
return edge_alphas
def evaluate_models(models, datasets, obj_id_to_keys, env, use_sensitivity):
y_true, y_pred = [], [[]]
total_datapoints = 0
for dataset, obj_id_to_key in zip(datasets, obj_id_to_keys):
total_datapoints += dataset.num_datapoints
for i in range(dataset.num_datapoints):
datapoint = dataset.datapoint(i)
dataset_name, key = obj_id_to_key[str(
datapoint['obj_id'])].split(KEY_SEP_TOKEN)
obj = env._state_space._database.dataset(dataset_name)[key]
orig_pose = to_rigid(datapoint['obj_pose'])
obj.T_obj_world = orig_pose
env.state.obj = obj
model.load_object(env.state)
predicted_poses, vertex_probs, _ = model.predict(
[datapoint['vertex']],
[datapoint['normal']],
[-datapoint['normal']], # push dir
use_sensitivity=use_sensitivity)
vertex_probs = vertex_probs[0]
y_pred.extend([1 - vertex_probs[0]] * NUM_PER_DATAPOINT)
empirical_dist = []
for i in range(NUM_PER_DATAPOINT):
actual_pose_mat = datapoint['actual_poses'][i * 4:(i + 1) * 4]
rot, trans = RigidTransform.rotation_and_translation_from_matrix(
actual_pose_mat)
pose_to_add = RigidTransform(rot, trans, 'obj', 'world')
y_true.append(
0 if is_equivalent_pose(pose_to_add, orig_pose) else 1)
found = False
for i in range(len(empirical_dist)):
actual_pose, actual_prob = empirical_dist[i]
if is_equivalent_pose(actual_pose, pose_to_add):
empirical_dist[i][1] += .1
found = True
break
if not found:
empirical_dist.append([pose_to_add, .1])
total_variation, l1 = 1, []
for empirical_pose, empirical_prob in empirical_dist:
for predicted_pose, predicted_prob in zip(
predicted_poses, vertex_probs):
if is_equivalent_pose(empirical_pose, predicted_pose):
total_variation = min(
total_variation,
abs(empirical_prob - predicted_prob))
l1.append(abs(empirical_prob - predicted_prob))
break
l1 = np.mean(l1) if len(l1) > 0 else 0
# if i + total_datapoints in test_set:
# test_tvs.append(total_variation)
# test_l1s.append(l1)
# else:
# train_tvs.append(total_variation)
# train_l1s.append(l1)
combined_tvs.append(total_variation)
combined_l1s.append(l1)
# for i in range(10):
# actual_pose_mat = datapoint['actual_poses'][i*4:(i+1)*4]
# rot, trans = RigidTransform.rotation_and_translation_from_matrix(actual_pose_mat)
# actual_pose = RigidTransform(rot, trans, 'obj', 'world')
# y_true.append(0 if is_equivalent_pose(orig_pose, actual_pose) else 1)
# y_pred.append(1-vertex_probs[0])
# # Cross Entropy
# # q_x = 0
# # for predicted_pose, prob in zip(predicted_poses, vertex_probs):
# # if is_equivalent_pose(actual_pose, predicted_pose):
# # q_x = prob
# # break
# # if q_x == 0:
# # counter += 1
# # # env.render_3d_scene()
# # # vis3d.show(title='before', starting_camera_pose=CAMERA_POSE)
# # # env.state.obj.T_obj_world = actual_pose
# # # env.render_3d_scene()
# # # vis3d.show(title='after', starting_camera_pose=CAMERA_POSE)
# # # for predicted_pose, prob in zip(predicted_poses, vertex_probs):
# # # env.state.obj.T_obj_world = predicted_pose
# # # env.render_3d_scene()
# # # title = '{}, pose diff: {}, angle diff: {}, prob: {}'.format(
# # # is_equivalent_pose(actual_pose, predicted_pose),
# # # pose_diff(actual_pose, predicted_pose),
# # # pose_angle(actual_pose, predicted_pose),
# # # prob
# # # )
# # # vis3d.show(title=title, starting_camera_pose=CAMERA_POSE)
# # # env.state.obj.T_obj_world = to_rigid(datapoint['obj_pose'])
# # q_x = max(q_x, 1e-5)
# # cross_entropy -= .1 * np.log(q_x) # p(x) * log(q(x))
# # n += .1
# logger.info('Mean Cross Entropy '+str(cross_entropy/float(n)))
# logger.info('frac 0: {} {} {}'.format(counter, 10*total_datapoints, counter / float(10*total_datapoints)))
# precision, recall, _ = metrics.precision_recall_curve(y_true, y_pred)
# aucs.append(metrics.auc(recall, precision))
# prs.append((precision, recall, model))
# return np.mean(train_tvs), np.mean(test_tvs), np.mean(combined_tvs), np.mean(train_l1s), np.mean(test_l1s), np.mean(combined_l1s)
avg_precision = metrics.average_precision_score(y_true, y_pred)
return combined_tvs, combined_l1s, avg_precision, y_true, y_pred