def generate_change_points_1(self): """ Generates changespoints by clustering within a demonstration. """ cp_index = 0 for demonstration in self.list_of_demonstrations: N = self.data_N[demonstration] gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', n_iter=10000, thresh = 5e-5) gmm.fit(N) Y = gmm.predict(N) self.save_cluster_metrics(N, Y, 'cpts_' + demonstration) start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p") size_of_X = self.data_X_size[demonstration] for i in range(len(Y) - 1): if Y[i] != Y[i + 1]: change_pt = N[i][size_of_X:] self.append_cp_array(change_pt) self.map_cp2frm[cp_index] = start + i * self.sr self.map_cp2demonstrations[cp_index] = demonstration self.list_of_cp.append(cp_index) cp_index += 1
def plot_broken_barh_from_pickle(demonstration, output_fname, labels_manual, colors_manual, labels_automatic_W,
colors_automatic_W, labels_automatic_Z, colors_automatic_Z, labels_automatic_ZW, colors_automatic_ZW):
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p")
length = end - start
fig, ax = plt.subplots()
# Plot 1) Manual 2) Time clusters
ax.broken_barh(labels_manual, (17, 2), facecolors = colors_manual)
ax.broken_barh(preprocess_labels(labels_automatic_W), (13, 2), facecolors = colors_automatic_W)
ax.broken_barh(preprocess_labels(labels_automatic_Z), (9, 2), facecolors = colors_automatic_Z)
ax.broken_barh(preprocess_labels(labels_automatic_ZW), (5, 2), facecolors = colors_automatic_ZW)
TASK = constants.TASK_NAME
if (TASK in ["lego", "plane"]):
end = end + 20
elif (TASK in ["000", "010", "011", "100"]):
end = end + 10
else:
end = end + 50
ticks = get_ticks(labels_manual)
ax.set_ylim(3,21)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
ax.set_yticks([6, 10, 14, 18])
ax.set_yticklabels(['Both (k + z)','Visual (z)','Kinematics (k)', 'Manual'])
if output_fname:
plt.savefig(output_fname)
else:
plt.show()
pass
def generate_change_points_2(self): """ Generates changespoints by clustering across demonstrations. """ cp_index = 0 for demonstration in self.list_of_demonstrations: W = self.data_W[demonstration] Z = self.data_Z[demonstration] PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p" annotations = pickle.load(open(PATH_TO_ANNOTATION, "rb")) manual_labels = utils.get_chronological_sequences(annotations) start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION) for elem in manual_labels: frm = elem[1] change_pt_W = W[(frm - start)/self.sr] change_pt_Z = Z[(frm - start)/self.sr] change_pt = utils.safe_concatenate(change_pt_W, change_pt_Z) self.append_cp_array(change_pt) self.map_cp2demonstrations[cp_index] = demonstration self.map_cp2frm[cp_index] = frm self.list_of_cp.append(cp_index) cp_index += 1
def preprocess(list_of_demonstrations):
camera = constants.CAMERA
for demonstration in list_of_demonstrations:
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + camera + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
OLD_FRM_PATH = constants.PATH_TO_DATA + "frames_unprocessed/" + demonstration + "_" + camera + "/"
NEW_FRM_PATH = constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + camera + "/"
command = "mkdir " + NEW_FRM_PATH
print command
os.mkdir(NEW_FRM_PATH)
for frm in range(start, end + 1):
OLD_FRM_NAME = utils.get_full_image_path(OLD_FRM_PATH, frm)
NEW_FRM_NAME = utils.get_full_image_path(NEW_FRM_PATH, frm)
NEW_FRM_NAME_UNSCALED = utils.get_full_image_path(
NEW_FRM_PATH + "unscaled_", frm)
command = "ffmpeg -i " + OLD_FRM_NAME + " -filter:v " + constants.CROP_PARAMS[
camera] + " " + NEW_FRM_NAME_UNSCALED
print command
os.system(command)
command = "ffmpeg -i " + NEW_FRM_NAME_UNSCALED + " -vf scale=640:480 " + NEW_FRM_NAME
print command
os.system(command)
command = "rm " + NEW_FRM_NAME_UNSCALED
print command
os.system(command)
def featurize_1(list_of_demonstrations, kinematics, sr):
"""
Given .p (pickle) files of sift features, this function concatenates
Z with W vectors to produce X vector, dumped as pickle file.
"""
print "FEATURIZATION 1"
data_X_1 = {}
data_X_2 = {}
for demonstration in list_of_demonstrations:
print "SIFT for ", demonstration
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER
+ demonstration + "_" + constants.CAMERA +".p")
W = kinematics[demonstration]
W_sampled = utils.sample_matrix(W, sampling_rate = sr)
PATH_TO_SIFT = constants.PATH_TO_DATA + "sift_FCED/SIFT_"+ demonstration
Z = pickle.load(open(PATH_TO_SIFT + "_1.p", "rb"))
Z = Z[start:end + 1]
Z_sampled_1 = utils.sample_matrix(Z, sampling_rate = sr)
Z = pickle.load(open(PATH_TO_SIFT + "_2.p", "rb"))
Z = Z[start:end + 1]
Z_sampled_2 = utils.sample_matrix(Z, sampling_rate = sr)
assert Z_sampled_1.shape[0] == W_sampled.shape[0]
assert Z_sampled_2.shape[0] == W_sampled.shape[0]
data_X_1[demonstration] = np.concatenate((W_sampled, Z_sampled_1), axis = 1)
data_X_2[demonstration] = np.concatenate((W_sampled, Z_sampled_2), axis = 1)
pickle.dump(data_X_1, open(PATH_TO_FEATURES + "SIFT_1.p", "wb"))
pickle.dump(data_X_2, open(PATH_TO_FEATURES + "SIFT_2.p", "wb"))
def featurize_sift(list_of_demonstrations, kinematics, sr):
"""
Extracts SIFT features for all frames in list_of_demonstrations.
"""
data_X_xy = {}
data_X_x = {}
for demonstration in list_of_demonstrations:
print "SIFT for ", demonstration
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
Z = []
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
for frm in range(start, end + 1):
PATH_TO_IMAGE = constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + constants.CAMERA + "/"
Z.append(sift.run_surf_frame(utils.get_full_image_path(PATH_TO_IMAGE, frm)))
Z = np.array(Z)
Z = Z.reshape(Z.shape[0],1)
W = kinematics[demonstration]
W_onlyx = utils.only_X(W)
X = np.concatenate((W, Z), axis = 1)
X_onlyx = np.concatenate((W_onlyx, Z), axis = 1)
data_X_xy[demonstration] = X
data_X_x[demonstration] = X_onlyx
pickle.dump(data_X_xy, open(PATH_TO_FEATURES + "SIFT_xy.p", "wb"))
pickle.dump(data_X_x, open(PATH_TO_FEATURES + "SIFT_x.p", "wb"))
def featurize_sift(list_of_demonstrations, kinematics, sr):
"""
Extracts SIFT features for all frames in list_of_demonstrations.
"""
data_X_xy = {}
data_X_x = {}
for demonstration in list_of_demonstrations:
print "SIFT for ", demonstration
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(
constants.CAMERA) + ".p"
Z = []
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
for frm in range(start, end + 1):
PATH_TO_IMAGE = constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + constants.CAMERA + "/"
Z.append(
sift.run_surf_frame(
utils.get_full_image_path(PATH_TO_IMAGE, frm)))
Z = np.array(Z)
Z = Z.reshape(Z.shape[0], 1)
W = kinematics[demonstration]
W_onlyx = utils.only_X(W)
X = np.concatenate((W, Z), axis=1)
X_onlyx = np.concatenate((W_onlyx, Z), axis=1)
data_X_xy[demonstration] = X
data_X_x[demonstration] = X_onlyx
pickle.dump(data_X_xy, open(PATH_TO_FEATURES + "SIFT_xy.p", "wb"))
pickle.dump(data_X_x, open(PATH_TO_FEATURES + "SIFT_x.p", "wb"))
def preprocess(list_of_demonstrations): camera = constants.CAMERA for demonstration in list_of_demonstrations: PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + camera + ".p" start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION) OLD_FRM_PATH = constants.PATH_TO_DATA + "frames_unprocessed/" + demonstration + "_" + camera + "/" NEW_FRM_PATH = constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + camera + "/" command = "mkdir " + NEW_FRM_PATH print command os.mkdir(NEW_FRM_PATH) for frm in range(start, end + 1): OLD_FRM_NAME = utils.get_full_image_path(OLD_FRM_PATH, frm) NEW_FRM_NAME = utils.get_full_image_path(NEW_FRM_PATH, frm) NEW_FRM_NAME_UNSCALED = utils.get_full_image_path(NEW_FRM_PATH + "unscaled_", frm) command = "ffmpeg -i " + OLD_FRM_NAME + " -filter:v " + constants.CROP_PARAMS[camera] + " " + NEW_FRM_NAME_UNSCALED print command os.system(command) command = "ffmpeg -i " + NEW_FRM_NAME_UNSCALED + " -vf scale=640:480 " + NEW_FRM_NAME print command os.system(command) command = "rm " + NEW_FRM_NAME_UNSCALED print command os.system(command)
def plot_broken_barh_from_pickle(demonstration, output_fname, labels_manual,
colors_manual, labels_automatic_W,
colors_automatic_W, labels_automatic_Z,
colors_automatic_Z, labels_automatic_ZW,
colors_automatic_ZW):
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA +
constants.ANNOTATIONS_FOLDER +
demonstration + "_" +
constants.CAMERA + ".p")
length = end - start
fig, ax = plt.subplots()
# Plot 1) Manual 2) Time clusters
ax.broken_barh(labels_manual, (17, 2), facecolors=colors_manual)
ax.broken_barh(preprocess_labels(labels_automatic_W), (13, 2),
facecolors=colors_automatic_W)
ax.broken_barh(preprocess_labels(labels_automatic_Z), (9, 2),
facecolors=colors_automatic_Z)
ax.broken_barh(preprocess_labels(labels_automatic_ZW), (5, 2),
facecolors=colors_automatic_ZW)
TASK = constants.TASK_NAME
if (TASK in ["lego", "plane"]):
end = end + 20
elif (TASK in ["000", "010", "011", "100"]):
end = end + 10
else:
end = end + 50
ticks = get_ticks(labels_manual)
ax.set_ylim(3, 21)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
ax.set_yticks([6, 10, 14, 18])
ax.set_yticklabels(
['Both (k + z)', 'Visual (z)', 'Kinematics (k)', 'Manual'])
if output_fname:
plt.savefig(output_fname)
else:
plt.show()
pass
def run_sift_images():
list_of_demonstrations = ["Suturing_E001",]
for demonstration in list_of_demonstrations:
print "SIFT for ", demonstration
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
for frm in range(start, end + 1):
if ((frm % 3) == 0):
PATH_TO_IMAGE = utils.get_full_image_path(constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + constants.CAMERA + "/", frm)
print PATH_TO_IMAGE
img = cv2.imread(PATH_TO_IMAGE)
sift = cv2.SIFT(nfeatures = 50)
kp, des = sift.detectAndCompute(img, None)
img = cv2.drawKeypoints(img, kp)
cv2.imshow('sift',img)
cv2.imwrite('../sift_images/' + str(frm) +".jpg",img)
def plot_interactive():
results = pickle.load(open("pickle_files/Suturing_E001_changepoints_Z2.p", "rb"))
jackknife_index = 3
demonstration = "Suturing_E001"
changepoints = results[demonstration]['changepoints'][jackknife_index]
labels = results[demonstration]['plot_labels_automatic']
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
start_d, end_d = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
changepoints_processed = []
for elem in changepoints:
changepoints_processed.append((elem[0] - start_d)/12)
labels_processed = []
for elem in labels:
labels_processed.append((elem[0] - start_d)/12)
total_frames = [int(elem[0]) for elem in changepoints]
total_frames += [int(elem[0]) for elem in labels]
total_frames.sort()
for end_frame in total_frames:
# Frame
ax1 = plt.subplot(121)
PATH_TO_FIGURE = constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + str(constants.CAMERA) + "/"
im = mpimg.imread(utils.get_full_image_path(PATH_TO_FIGURE, end_frame))
ax1.set_title(str(end_frame))
ax1.xaxis.set_visible(False)
ax1.yaxis.set_visible(False)
ax1.imshow(im)
# AlexNet
ax2 = plt.subplot(122)
data = pickle.load(open("pickle_files/Suturing_E001_AlexNet_dimred.p", "rb"))
ax2.set_title('AlexNet')
ax2.set_ylim([-0.1, 1.1])
ax2.set_xlim([-0.1, 1.1])
ax2.xaxis.set_visible(False)
ax2.yaxis.set_visible(False)
plot_AlexNet(data, demonstration, changepoints = changepoints_processed, plotter = ax2, labels = labels_processed, plot_tsne = True, end_frame = (end_frame - start_d)/12, interactive_mode = True)
def plot_all():
# Suturing_E001
# changepoints = pickle.load(open("pickle_files/Suturing_E001_changepoints_Z1.p", "rb"))
# jackknife_index = 1
demonstration = "Suturing_E001"
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
start_d, end_d = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
# changepoints = changepoints[jackknife_index]
changepoints_processed = []
results = pickle.load(open("pickle_files/Suturing_E001_changepoints_Z2.p", "rb"))
jackknife_index = 3
demonstration = "Suturing_E001"
changepoints = results[demonstration]['changepoints'][jackknife_index]
labels = results[demonstration]['plot_labels_automatic']
for elem in changepoints:
changepoints_processed.append((elem[0] - start_d)/6)
labels_processed = []
for elem in labels:
labels_processed.append((elem[0] - start_d)/6)
# VGG
data = pickle.load(open("pickle_files/Suturing_E001_VGG_dimred.p", "rb"))
plot_VGG(data, demonstration, changepoints = None, labels = None, plot_tsne = True)
# AlexNet
data = pickle.load(open("pickle_files/Suturing_E001_AlexNet_dimred.p", "rb"))
plot_AlexNet(data, demonstration, changepoints = None, labels = None, plot_tsne = True)
# Raw Pixels
data = pickle.load(open("pickle_files/Suturing_E001_raw_pixel_dimred.p", "rb"))
plot_raw_image_pixels(data, demonstration, changepoints = None, labels = None, plot_tsne = True)
# SIFT
data = pickle.load(open("pickle_files/Suturing_E001_SIFT_dimred.p", "rb"))
plot_SIFT(data, demonstration, changepoints = None, labels = None, plot_tsne = True)
def generate_change_points(self): cp_index = 0 for demonstration in self.list_of_demonstrations: # print "Changepoints for " + demonstration N = self.data_N[demonstration] # print N[0].shape for demonstration in self.list_of_demonstrations: # print "Changepoints for " + demonstration N = self.data_N[demonstration] gmm = mixture.GMM(n_components = 10, covariance_type='full') gmm.fit(N) Y = gmm.predict(N) self.save_cluster_metrics(N, Y, gmm.means_, 'cpts_' + demonstration, gmm) start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_capture2.p") size_of_X = self.data_X_size[demonstration] for i in range(len(Y) - 1): if Y[i] != Y[i + 1]: change_pt = N[i][size_of_X:] # print N.shape, change_pt.shape self.append_cp_array(change_pt) self.map_cp2frm[cp_index] = start + i * self.sr self.map_cp2demonstrations[cp_index] = demonstration self.list_of_cp.append(cp_index) cp_index += 1
def generate_sift_features():
list_of_demonstrations = ["plane_9",]
for demonstration in list_of_demonstrations:
print "SIFT for ", demonstration
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
X1 = None
X2 = None
n_features = 20
sift = cv2.SIFT(nfeatures = n_features)
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
for frm in range(start, end + 1):
# if ((frm % 3) == 0):
PATH_TO_IMAGE = utils.get_full_image_path(constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + constants.CAMERA + "/", frm)
print PATH_TO_IMAGE
img = cv2.imread(PATH_TO_IMAGE)
kp, des = sift.detectAndCompute(img, None)
img = cv2.drawKeypoints(img, kp)
cv2.imshow('sift',img)
cv2.imwrite('../sift_images/' + demonstration + "/" + str(frm) +".jpg",img)
vector1 = []
vector2 = []
kp.sort(key = lambda x: x.response, reverse = True)
for kp_elem in kp:
vector1 += [kp_elem.response, kp_elem.pt[0], kp_elem.pt[1], kp_elem.size, kp_elem.angle]
vector2 += [kp_elem.pt[0], kp_elem.pt[1]]
try:
X1 = utils.safe_concatenate(X1, utils.reshape(np.array(vector1[:n_features * 5])))
X2 = utils.safe_concatenate(X2, utils.reshape(np.array(vector2[:n_features * 2])))
except ValueError as e:
IPython.embed()
pickle.dump(X1, open("sift_features/SIFT_" + demonstration + "_1.p", "wb"))
pickle.dump(X2, open("sift_features/SIFT_" + demonstration + "_2.p", "wb"))
def featurize_1(list_of_demonstrations, kinematics, sr):
"""
Given .p (pickle) files of sift features, this function concatenates
Z with W vectors to produce X vector, dumped as pickle file.
"""
print "FEATURIZATION 1"
data_X_1 = {}
data_X_2 = {}
for demonstration in list_of_demonstrations:
print "SIFT for ", demonstration
start, end = utils.get_start_end_annotations(
constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER +
demonstration + "_" + constants.CAMERA + ".p")
W = kinematics[demonstration]
W_sampled = utils.sample_matrix(W, sampling_rate=sr)
PATH_TO_SIFT = constants.PATH_TO_DATA + "sift_FCED/SIFT_" + demonstration
Z = pickle.load(open(PATH_TO_SIFT + "_1.p", "rb"))
Z = Z[start:end + 1]
Z_sampled_1 = utils.sample_matrix(Z, sampling_rate=sr)
Z = pickle.load(open(PATH_TO_SIFT + "_2.p", "rb"))
Z = Z[start:end + 1]
Z_sampled_2 = utils.sample_matrix(Z, sampling_rate=sr)
assert Z_sampled_1.shape[0] == W_sampled.shape[0]
assert Z_sampled_2.shape[0] == W_sampled.shape[0]
data_X_1[demonstration] = np.concatenate((W_sampled, Z_sampled_1),
axis=1)
data_X_2[demonstration] = np.concatenate((W_sampled, Z_sampled_2),
axis=1)
pickle.dump(data_X_1, open(PATH_TO_FEATURES + "SIFT_1.p", "wb"))
pickle.dump(data_X_2, open(PATH_TO_FEATURES + "SIFT_2.p", "wb"))
def generate_raw_image_pixels(list_of_demonstrations):
"""
PCA and t-SNE on raw image pixels
"""
# Design matrix of raw image pixels
X = None
for demonstration in list_of_demonstrations:
print "Raw image pixels ", demonstration
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
for frm in range(start, end + 1):
if ((frm % 6) == 0):
PATH_TO_IMAGE = utils.get_full_image_path(constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + constants.CAMERA + "/", frm)
print demonstration, str(frm)
img = utils.reshape(cv2.imread(PATH_TO_IMAGE).flatten())
X = utils.safe_concatenate(X, img)
X_pca = utils.pca(X, PC = 2)
X_tsne = utils.tsne(X)
data_dimred = [X_pca, X_tsne]
pickle.dump(X_tsne, open("raw_pixel_" + demonstration + "_dimred.p", "wb"))
def generate_change_points_1(self):
"""
Generates changespoints by clustering within a demonstration.
"""
cp_index = 0
for demonstration in self.list_of_demonstrations:
N = self.data_N[demonstration]
gmm = mixture.GMM(n_components=self.n_components_cp,
covariance_type='full',
n_iter=10000,
thresh=5e-5)
gmm.fit(N)
Y = gmm.predict(N)
self.save_cluster_metrics(N, Y, 'cpts_' + demonstration)
start, end = utils.get_start_end_annotations(
constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER +
demonstration + "_" + constants.CAMERA + ".p")
size_of_X = self.data_X_size[demonstration]
for i in range(len(Y) - 1):
if Y[i] != Y[i + 1]:
change_pt = N[i][size_of_X:]
self.append_cp_array(change_pt)
self.map_cp2frm[cp_index] = start + i * self.sr
self.map_cp2demonstrations[cp_index] = demonstration
self.list_of_cp.append(cp_index)
cp_index += 1
def weighted_score(list_of_demonstrations, list_of_frm_demonstrations):
"""
Implements weighted pruning for given demonstrations represented in list_of_frm_demonstrations.
Returns weighted score.
"""
if constants.TASK_NAME not in ["suturing", "needle_passing"]:
return None
if not constants.WEIGHTED_PRUNING_MODE:
return None
N = float(len(list_of_demonstrations))
uniform_weight = 1 / N
# Weights for skill-weighted pruning, where each demonstration has the same weight for
map_demonstration2weight = {}
# Weight is inversely proportional to completion time.
map_demonstration2weight_t = {}
for demonstration in list_of_demonstrations:
# Base weight
weight = uniform_weight
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
weight_t = 1.0 / (end - start)
# Stripped demonstration task (Suturing_, Needle_passing_, etc.) from demonstration name
demonstration_name = demonstration.split("_")[-1]
if demonstration_name in experts:
weight *= constants.WEIGHT_EXPERT
elif demonstration_name in intermediates:
weight *= constants.WEIGHT_INTERMEDIATE
else:
if demonstration_name not in novices:
print "ERROR: Unidentified Demonstration"
IPython.embed()
map_demonstration2weight[demonstration] = weight
map_demonstration2weight_t[demonstration] = weight_t
normalization_factor = sum(map_demonstration2weight.values())
normalization_factor_t = sum(map_demonstration2weight_t.values())
#Weight normalization
for demonstration in list_of_demonstrations:
weight = map_demonstration2weight[demonstration]
map_demonstration2weight[demonstration] = weight / float(
normalization_factor)
weight_t = map_demonstration2weight_t[demonstration]
map_demonstration2weight_t[demonstration] = weight_t / float(
normalization_factor_t)
score = 0.0
for demonstration in set(list_of_frm_demonstrations):
score += map_demonstration2weight[demonstration]
score_t = 0.0
for demonstration in set(list_of_frm_demonstrations):
score_t += map_demonstration2weight_t[demonstration]
return score_t
"plane_3_js.p", "plane_4_js.p", "plane_5_js.p", "plane_6_js.p",
"plane_7_js.p", "plane_8_js.p", "plane_9_js.p", "plane_10_js.p"
]
list_of_trajectories = [
"plane_3.p", "plane_4.p", "plane_5.p", "plane_6.p", "plane_7.p",
"plane_8.p", "plane_9.p", "plane_10.p"
]
list_of_annotations = [
"plane_3_capture2.p", "plane_4_capture2.p", "plane_5_capture2.p",
"plane_6_capture2.p", "plane_7_capture2.p", "plane_8_capture2.p",
"plane_9_capture2.p", "plane_10_capture2.p"
]
for i in range(len(list_of_annotations)):
print list_of_annotations[i], list_of_joint_states[
i], list_of_trajectories[i]
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA +
"annotations/" +
list_of_annotations[i])
X = None
trajectory = pickle.load(
open(constants.PATH_TO_KINEMATICS + list_of_joint_states[i], "rb"))
for frm in range(start, end + 1):
traj_point = trajectory[frm]
print traj_point.velocity[16:-12]
vector = list(traj_point.position[16:-12]) + list(
traj_point.velocity[16:-12])
X = utils.safe_concatenate(X, utils.reshape(np.array(vector)))
# pickle.dump(X, open(constants.PATH_TO_KINEMATICS + list_of_trajectories[i],"wb"))
def plot_broken_barh(demonstration, data, save_fname=None, T=10):
"""
Parameters:
-----------
demonstration: String name of demonstration without camera specification , e.g. "Suturing_E001"
list_of_frms_[1,2,3,4]: List of changepoint frames for each of 4 different clustering experiments.
Use this to compare manually vs. automatically generated transition points.
* For now, the list_of_frms are constrained to 4 for visualization sanity sake.
"""
numDemos = min(5, len(data.keys()) + 1)
sizeTestSet = numDemos - 1
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA +
constants.ANNOTATIONS_FOLDER +
demonstration + "_" +
constants.CAMERA + ".p")
length = end - start
segments = pickle.load(open(PATH_TO_ANNOTATION, "rb"))
fig, ax = plt.subplots()
# Generate labels for 1) Manual 2) Time clusters
labels_automatic_0, colors_automatic_0, labels_automatic_1, colors_automatic_1, means, list_of_frms = get_time_clusters(
data, T)
labels_manual, colors_manual = setup_manual_labels(segments)
# Plot 1) Manual 2) Time clusters
ax.broken_barh(labels_manual, (25, 2), facecolors=colors_manual)
ax.broken_barh(labels_automatic_0, (21, 2), facecolors=colors_automatic_0)
list_of_plot_ranges = [(17, 2), (13, 2), (9, 2), (5, 2)]
for i in range(min(sizeTestSet, 4)):
labels_automatic, colors_automatic = setup_automatic_labels(
list_of_frms[i], "k")
ax.broken_barh(labels_automatic,
list_of_plot_ranges[i],
facecolors=colors_automatic)
TASK = constants.TASK_NAME
if (TASK in ["lego", "plane"]):
end = end + 20
elif (TASK in ["000", "010", "011", "100"]):
end = end + 10
else:
end = end + 50
ticks = get_ticks(labels_manual)
ax.set_ylim(3, 29)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
# ax.set_xticks(ticks)
ax.set_yticks([6, 10, 14, 18, 22, 26])
ax.set_yticklabels([
'Automatic4', 'Automatic3', 'Automatic2', 'Automatic1',
'Time Clustering', 'Manual'
])
if save_fname:
plt.savefig(save_fname)
else:
plt.show()
pass
time_sequence_1 = [elem[0] + elem[1] for elem in labels_manual]
time_sequence_2 = means
dtw_score = compute_dtw(time_sequence_1, time_sequence_2)
normalized_dtw_score = dtw_score / float(length) * 100
return dtw_score, normalized_dtw_score, length, labels_manual, colors_manual, labels_automatic_0, colors_automatic_0
def plot_broken_barh(demonstration, data, save_fname = None, T = 10):
"""
Parameters:
-----------
demonstration: String name of demonstration without camera specification , e.g. "Suturing_E001"
list_of_frms_[1,2,3,4]: List of changepoint frames for each of 4 different clustering experiments.
Use this to compare manually vs. automatically generated transition points.
* For now, the list_of_frms are constrained to 4 for visualization sanity sake.
"""
numDemos = min(5, len(data.keys()) + 1)
sizeTestSet = numDemos - 1
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p")
length = end - start
segments = pickle.load(open(PATH_TO_ANNOTATION, "rb"))
fig, ax = plt.subplots()
# Generate labels for 1) Manual 2) Time clusters
labels_automatic_0, colors_automatic_0, labels_automatic_1, colors_automatic_1, means, list_of_frms = get_time_clusters(data, T)
labels_manual, colors_manual = setup_manual_labels(segments)
# Plot 1) Manual 2) Time clusters
ax.broken_barh(labels_manual, (25, 2), facecolors = colors_manual)
ax.broken_barh(labels_automatic_0, (21, 2), facecolors = colors_automatic_0)
list_of_plot_ranges = [(17, 2), (13, 2), (9, 2), (5, 2)]
for i in range(min(sizeTestSet, 4)):
labels_automatic, colors_automatic = setup_automatic_labels(list_of_frms[i], "k")
ax.broken_barh(labels_automatic, list_of_plot_ranges[i], facecolors = colors_automatic)
TASK = constants.TASK_NAME
if (TASK in ["lego", "plane"]):
end = end + 20
elif (TASK in ["000", "010", "011", "100"]):
end = end + 10
else:
end = end + 50
ticks = get_ticks(labels_manual)
ax.set_ylim(3,29)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
# ax.set_xticks(ticks)
ax.set_yticks([6, 10, 14, 18, 22, 26])
ax.set_yticklabels(['Automatic4','Automatic3','Automatic2', 'Automatic1','Time Clustering', 'Manual'])
if save_fname:
plt.savefig(save_fname)
else:
plt.show()
pass
time_sequence_1 = [elem[0] + elem[1] for elem in labels_manual ]
time_sequence_2 = means
dtw_score = compute_dtw(time_sequence_1, time_sequence_2)
normalized_dtw_score = dtw_score/float(length) * 100
return dtw_score, normalized_dtw_score, length, labels_manual, colors_manual, labels_automatic_0, colors_automatic_0
def plot_broken_barh_all(demonstration,
data_W,
data_Z,
data_ZW,
save_fname=None,
save_fname2=None):
"""
Plots time-clusters for W, K, KW.
"""
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA +
constants.ANNOTATIONS_FOLDER +
demonstration + "_" +
constants.CAMERA + ".p")
length = end - start
segments = pickle.load(open(PATH_TO_ANNOTATION, "rb"))
TASK = constants.TASK_NAME
if (TASK in ["lego", "plane"]):
end = end + 20
elif (TASK in ["000", "010", "011", "100"]):
end = end + 2
else:
end = end + 50
labels_manual, colors_manual = setup_manual_labels(segments)
labels_automatic_W, colors_automatic_W, labels_automatic_W_0, colors_automatic_W_0, means_W, list_of_frms_W = get_time_clusters(
data_W, constants.N_COMPONENTS_TIME_W)
labels_automatic_Z, colors_automatic_Z, labels_automatic_Z_0, colors_automatic_Z_0, means_Z, list_of_frms_Z = get_time_clusters(
data_Z, constants.N_COMPONENTS_TIME_Z)
labels_automatic_ZW, colors_automatic_ZW, labels_automatic_ZW_0, colors_automatic_ZW_0, means_ZW, list_of_frms_ZW = get_time_clusters(
data_ZW, constants.N_COMPONENTS_TIME_ZW)
fig, ax = plt.subplots()
ax.broken_barh(labels_manual, (17, 2), facecolors=colors_manual)
ax.broken_barh(labels_automatic_W, (13, 2), facecolors=colors_automatic_W)
ax.broken_barh(labels_automatic_Z, (9, 2), facecolors=colors_automatic_Z)
ax.broken_barh(labels_automatic_ZW, (5, 2), facecolors=colors_automatic_ZW)
ax.set_ylim(3, 21)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
ax.set_yticks([6, 10, 14, 18])
ax.set_yticklabels(['ZW', 'Z', 'W', 'Manual'])
if save_fname:
plt.savefig(save_fname)
else:
plt.show()
pass
fig, ax = plt.subplots()
ax.broken_barh(labels_manual, (17, 2), facecolors=colors_manual)
ax.broken_barh(labels_automatic_W_0, (13, 2),
facecolors=colors_automatic_W_0)
ax.broken_barh(labels_automatic_Z_0, (9, 2),
facecolors=colors_automatic_Z_0)
ax.broken_barh(labels_automatic_ZW_0, (5, 2),
facecolors=colors_automatic_ZW_0)
ax.set_ylim(3, 21)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
ax.set_yticks([6, 10, 14, 18])
ax.set_yticklabels(['ZW_0', 'Z_0', 'W_0', 'Manual'])
if save_fname2:
plt.savefig(save_fname2)
else:
plt.show()
pass
time_sequence_1 = [elem[0] + elem[1] for elem in labels_manual]
time_sequence_2 = means_W
dtw_score_W = compute_dtw(time_sequence_1, time_sequence_2)
time_sequence_2 = means_Z
dtw_score_Z = compute_dtw(time_sequence_1, time_sequence_2)
time_sequence_2 = means_ZW
dtw_score_ZW = compute_dtw(time_sequence_1, time_sequence_2)
dtw_score_W_normalized = dtw_score_W / float(length) * 100
dtw_score_Z_normalized = dtw_score_Z / float(length) * 100
dtw_score_ZW_normalized = dtw_score_ZW / float(length) * 100
return dtw_score_W, dtw_score_Z, dtw_score_ZW, dtw_score_W_normalized, dtw_score_Z_normalized, dtw_score_ZW_normalized, length
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
size_of_X = self.data_X_size[self.list_of_demonstrations[0]]
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = utils.get_start_end_annotations(
constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER +
demonstration + "_" + constants.CAMERA + ".p")
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generated big_N"
if constants.REMOTE == 1:
if self.fit_GMM:
gmm = mixture.GMM(n_components=self.n_components_cp,
covariance_type='full',
thresh=0.01)
if self.fit_DPGMM:
# dpgmm = mixture.DPGMM(n_components = 100, covariance_type='diag', n_iter = 10000, alpha = 100, thresh= 2e-4)
#DO NOT FIDDLE WITH PARAMS WITHOUT CONSENT :)
avg_len = int(big_N.shape[0] /
len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(
avg_len / constants.DPGMM_DIVISOR
) #tuned with suturing experts only for kinematics
print "L0 ", DP_GMM_COMPONENTS, "ALPHA: ", constants.ALPHA_ZW_CP
dpgmm = mixture.DPGMM(n_components=DP_GMM_COMPONENTS,
covariance_type='diag',
n_iter=1000,
alpha=constants.ALPHA_ZW_CP,
thresh=1e-7)
elif constants.REMOTE == 2:
gmm = mixture.GMM(n_components=self.n_components_cp,
covariance_type='full')
else:
gmm = mixture.GMM(n_components=self.n_components_cp,
covariance_type='full')
if self.fit_GMM:
start = time.time()
gmm.fit(big_N)
end = time.time()
"GMM time taken: ", str(end - start)
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
start = time.time()
dpgmm.fit(big_N)
end = time.time()
"DP-GMM time taken: ", str(end - start)
Y_dpgmm = dpgmm.predict(big_N)
Y = Y_dpgmm
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][:size_of_X]
self.append_cp_array(change_pt)
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[
w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points", len(self.list_of_cp)
def featurize_LCD_VLAD(list_of_demonstrations, kinematics, layer, net_name, folder, dimensions, batch_size, fname, config = [True, True, True]):
M = dimensions[0]
a = dimensions[1]
print "Featurizing LCD + VLAD: ", layer, net_name, folder, M, a, batch_size
BATCH_SIZE = batch_size
if constants.SIMULATION:
BATCH_SIZE = 5
data_X_PCA = {}
data_X_CCA = {}
data_X_GRP = {}
size_sampled_matrices = [utils.sample_matrix(kinematics[demo], sampling_rate = BATCH_SIZE).shape[0] for demo in list_of_demonstrations]
PC = min(100, min(size_sampled_matrices))
print "PC: ", PC
for demonstration in list_of_demonstrations:
print demonstration
W = kinematics[demonstration]
Z = load_cnn_features(demonstration, layer, folder, net_name)
W_new = utils.sample_matrix(W, sampling_rate = BATCH_SIZE)
Z_batch = None
W_batch = None
j = 1
Z_new = None
IPython.embed()
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
Z = []
IPython.embed()
for i in range(Z):
vector_W = W[i]
W_batch = utils.safe_concatenate(W_batch, vector_W)
vector_Z = Z[i]
vector_Z = vector_Z.reshape(M, a, a)
vector_Z = lcd.LCD(vector_Z)
Z_batch = utils.safe_concatenate(Z_batch, vector_Z)
if (j == BATCH_SIZE):
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
# Re-initialize batch variables
j = 0
Z_batch = None
W_batch = None
j += 1
# tail case
if Z_batch is not None:
print "TAIL CASE"
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
if config[0]:
Z_new_pca = utils.pca_incremental(Z_new, PC = PC)
print Z_new_pca.shape
assert W_new.shape[0] == Z_new_pca.shape[0]
X_PCA = np.concatenate((W_new, Z_new_pca), axis = 1)
data_X_PCA[demonstration] = X_PCA
if config[1]:
Z_new_cca = utils.cca(W_new, Z_new)
print Z_new_cca.shape
assert W_new.shape[0] == Z_new_cca.shape[0]
X_CCA = np.concatenate((W_new, Z_new_cca), axis = 1)
data_X_CCA[demonstration] = X_CCA
if config[2]:
Z_new_grp = utils.grp(Z_new)
print Z_new_grp.shape
assert W_new.shape[0] == Z_new_grp.shape[0]
X_GRP = np.concatenate((W_new, Z_new_grp), axis = 1)
data_X_GRP[demonstration] = X_GRP
if config[0]:
pickle.dump(data_X_PCA, open(PATH_TO_FEATURES + fname + "_PCA" + ".p", "wb"))
if config[1]:
pickle.dump(data_X_CCA, open(PATH_TO_FEATURES + fname + "_CCA" + ".p", "wb"))
if config[2]:
pickle.dump(data_X_GRP, open(PATH_TO_FEATURES + fname + "_GRP" + ".p", "wb"))
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER
+ demonstration + "_" + constants.CAMERA + ".p")
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generating Changepoints. Fitting GMM/DP-GMM ..."
if constants.REMOTE == 1:
if self.fit_DPGMM:
print "Init DPGMM"
avg_len = int(big_N.shape[0]/len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(avg_len/constants.DPGMM_DIVISOR)
print "L0", DP_GMM_COMPONENTS, "ALPHA: ", self.ALPHA_CP
dpgmm = mixture.DPGMM(n_components = DP_GMM_COMPONENTS, covariance_type='diag', n_iter = 10000, alpha = self.ALPHA_CP, thresh= 1e-7)
if self.fit_GMM:
print "Init GMM"
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', n_iter=5000, thresh = 5e-5)
if constants.REMOTE == 2:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', thresh = 0.01)
else:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full')
if self.fit_GMM:
print "Fitting GMM"
start = time.time()
gmm.fit(big_N)
end = time.time()
print "GMM Time:", end - start
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
print "Fitting DPGMM"
start = time.time()
dpgmm.fit(big_N)
end = time.time()
print "DPGMM Time:", end - start
Y_dpgmm = dpgmm.predict(big_N)
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
Y = Y_dpgmm
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][:self.X_dimension]
self.append_cp_array(utils.reshape(change_pt))
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points, " + str(cp_index)
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = utils.get_start_end_annotations(
constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
)
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generating Changepoints. Fitting GMM/DP-GMM ..."
if constants.REMOTE == 1:
if self.fit_DPGMM:
print "Init DPGMM"
avg_len = int(big_N.shape[0] / len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(avg_len / constants.DPGMM_DIVISOR)
print "L0", DP_GMM_COMPONENTS, "ALPHA: ", self.ALPHA_CP
dpgmm = mixture.DPGMM(
n_components=DP_GMM_COMPONENTS,
covariance_type="diag",
n_iter=10000,
alpha=self.ALPHA_CP,
thresh=1e-7,
)
if self.fit_GMM:
print "Init GMM"
gmm = mixture.GMM(n_components=self.n_components_cp, covariance_type="full", n_iter=5000, thresh=5e-5)
if constants.REMOTE == 2:
gmm = mixture.GMM(n_components=self.n_components_cp, covariance_type="full", thresh=0.01)
else:
gmm = mixture.GMM(n_components=self.n_components_cp, covariance_type="full")
if self.fit_GMM:
print "Fitting GMM"
start = time.time()
gmm.fit(big_N)
end = time.time()
print "GMM Time:", end - start
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
print "Fitting DPGMM"
start = time.time()
dpgmm.fit(big_N)
end = time.time()
print "DPGMM Time:", end - start
Y_dpgmm = dpgmm.predict(big_N)
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
Y = Y_dpgmm
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][: self.X_dimension]
self.append_cp_array(utils.reshape(change_pt))
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points, " + str(cp_index)
def plot_broken_barh_all(demonstration, data_W, data_Z, data_ZW, save_fname = None, save_fname2 = None):
"""
Plots time-clusters for W, K, KW.
"""
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p")
length = end - start
segments = pickle.load(open(PATH_TO_ANNOTATION, "rb"))
TASK = constants.TASK_NAME
if (TASK in ["lego", "plane"]):
end = end + 20
elif (TASK in ["000", "010", "011", "100"]):
end = end + 2
else:
end = end + 50
labels_manual, colors_manual = setup_manual_labels(segments)
labels_automatic_W, colors_automatic_W, labels_automatic_W_0, colors_automatic_W_0, means_W, list_of_frms_W = get_time_clusters(data_W, constants.N_COMPONENTS_TIME_W)
labels_automatic_Z, colors_automatic_Z, labels_automatic_Z_0, colors_automatic_Z_0, means_Z, list_of_frms_Z = get_time_clusters(data_Z, constants.N_COMPONENTS_TIME_Z)
labels_automatic_ZW, colors_automatic_ZW, labels_automatic_ZW_0, colors_automatic_ZW_0, means_ZW, list_of_frms_ZW = get_time_clusters(data_ZW, constants.N_COMPONENTS_TIME_ZW)
fig, ax = plt.subplots()
ax.broken_barh(labels_manual, (17, 2), facecolors = colors_manual)
ax.broken_barh(labels_automatic_W, (13, 2), facecolors = colors_automatic_W)
ax.broken_barh(labels_automatic_Z, (9, 2), facecolors = colors_automatic_Z)
ax.broken_barh(labels_automatic_ZW, (5, 2), facecolors = colors_automatic_ZW)
ax.set_ylim(3,21)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
ax.set_yticks([6, 10, 14, 18])
ax.set_yticklabels(['ZW','Z','W', 'Manual'])
if save_fname:
plt.savefig(save_fname)
else:
plt.show()
pass
fig, ax = plt.subplots()
ax.broken_barh(labels_manual, (17, 2), facecolors = colors_manual)
ax.broken_barh(labels_automatic_W_0, (13, 2), facecolors = colors_automatic_W_0)
ax.broken_barh(labels_automatic_Z_0, (9, 2), facecolors = colors_automatic_Z_0)
ax.broken_barh(labels_automatic_ZW_0, (5, 2), facecolors = colors_automatic_ZW_0)
ax.set_ylim(3,21)
ax.set_xlim(0, end)
ax.set_xlabel('Frame number')
ax.set_yticks([6, 10, 14, 18])
ax.set_yticklabels(['ZW_0','Z_0','W_0', 'Manual'])
if save_fname2:
plt.savefig(save_fname2)
else:
plt.show()
pass
time_sequence_1 = [elem[0] + elem[1] for elem in labels_manual]
time_sequence_2 = means_W
dtw_score_W = compute_dtw(time_sequence_1, time_sequence_2)
time_sequence_2 = means_Z
dtw_score_Z = compute_dtw(time_sequence_1, time_sequence_2)
time_sequence_2 = means_ZW
dtw_score_ZW = compute_dtw(time_sequence_1, time_sequence_2)
dtw_score_W_normalized = dtw_score_W/float(length) * 100
dtw_score_Z_normalized = dtw_score_Z/float(length) * 100
dtw_score_ZW_normalized = dtw_score_ZW/float(length) * 100
return dtw_score_W, dtw_score_Z, dtw_score_ZW, dtw_score_W_normalized, dtw_score_Z_normalized, dtw_score_ZW_normalized, length
def weighted_score(list_of_demonstrations, list_of_frm_demonstrations):
"""
Implements weighted pruning for given demonstrations represented in list_of_frm_demonstrations.
Returns weighted score.
"""
if constants.TASK_NAME not in ["suturing", "needle_passing"]:
return None
if not constants.WEIGHTED_PRUNING_MODE:
return None
N = float(len(list_of_demonstrations))
uniform_weight = 1/N
# Weights for skill-weighted pruning, where each demonstration has the same weight for
map_demonstration2weight = {}
# Weight is inversely proportional to completion time.
map_demonstration2weight_t = {}
for demonstration in list_of_demonstrations:
# Base weight
weight = uniform_weight
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + constants.CAMERA + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
weight_t = 1.0/(end - start)
# Stripped demonstration task (Suturing_, Needle_passing_, etc.) from demonstration name
demonstration_name = demonstration.split("_")[-1]
if demonstration_name in experts:
weight *= constants.WEIGHT_EXPERT
elif demonstration_name in intermediates:
weight *= constants.WEIGHT_INTERMEDIATE
else:
if demonstration_name not in novices:
print "ERROR: Unidentified Demonstration"
IPython.embed()
map_demonstration2weight[demonstration] = weight
map_demonstration2weight_t[demonstration] = weight_t
normalization_factor = sum(map_demonstration2weight.values())
normalization_factor_t = sum(map_demonstration2weight_t.values())
#Weight normalization
for demonstration in list_of_demonstrations:
weight = map_demonstration2weight[demonstration]
map_demonstration2weight[demonstration] = weight/float(normalization_factor)
weight_t = map_demonstration2weight_t[demonstration]
map_demonstration2weight_t[demonstration] = weight_t/float(normalization_factor_t)
score = 0.0
for demonstration in set(list_of_frm_demonstrations):
score += map_demonstration2weight[demonstration]
score_t = 0.0
for demonstration in set(list_of_frm_demonstrations):
score_t += map_demonstration2weight_t[demonstration]
return score_t
import numpy as np import pickle import constants import utils import parser list_of_joint_states = ["plane_3_js.p", "plane_4_js.p", "plane_5_js.p", "plane_6_js.p", "plane_7_js.p", "plane_8_js.p", "plane_9_js.p", "plane_10_js.p"] list_of_trajectories = ["plane_3.p", "plane_4.p", "plane_5.p", "plane_6.p", "plane_7.p", "plane_8.p", "plane_9.p", "plane_10.p"] list_of_annotations = ["plane_3_capture2.p", "plane_4_capture2.p", "plane_5_capture2.p", "plane_6_capture2.p", "plane_7_capture2.p", "plane_8_capture2.p", "plane_9_capture2.p", "plane_10_capture2.p"] for i in range(len(list_of_annotations)): print list_of_annotations[i], list_of_joint_states[i], list_of_trajectories[i] start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + "annotations/" + list_of_annotations[i]) X = None trajectory = pickle.load(open(constants.PATH_TO_KINEMATICS + list_of_joint_states[i], "rb")) for frm in range(start, end + 1): traj_point = trajectory[frm] print traj_point.velocity[16:-12] vector = list(traj_point.position[16:-12]) + list(traj_point.velocity[16:-12]) X = utils.safe_concatenate(X, utils.reshape(np.array(vector))) # pickle.dump(X, open(constants.PATH_TO_KINEMATICS + list_of_trajectories[i],"wb"))
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
size_of_X = self.data_X_size[self.list_of_demonstrations[0]]
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER
+ demonstration + "_" + constants.CAMERA + ".p")
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generated big_N"
if constants.REMOTE == 1:
if self.fit_GMM:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', thresh = 0.01)
if self.fit_DPGMM:
# dpgmm = mixture.DPGMM(n_components = 100, covariance_type='diag', n_iter = 10000, alpha = 100, thresh= 2e-4)
#DO NOT FIDDLE WITH PARAMS WITHOUT CONSENT :)
avg_len = int(big_N.shape[0]/len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(avg_len/constants.DPGMM_DIVISOR) #tuned with suturing experts only for kinematics
print "L0 ", DP_GMM_COMPONENTS, "ALPHA: ", constants.ALPHA_ZW_CP
dpgmm = mixture.DPGMM(n_components = DP_GMM_COMPONENTS, covariance_type='diag', n_iter = 1000, alpha = constants.ALPHA_ZW_CP, thresh= 1e-7)
elif constants.REMOTE == 2:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full')
else:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full')
if self.fit_GMM:
start = time.time()
gmm.fit(big_N)
end = time.time()
"GMM time taken: ", str(end - start)
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
start = time.time()
dpgmm.fit(big_N)
end = time.time()
"DP-GMM time taken: ", str(end - start)
Y_dpgmm = dpgmm.predict(big_N)
Y = Y_dpgmm
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][:size_of_X]
self.append_cp_array(change_pt)
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points", len(self.list_of_cp)
def featurize_LCD_VLAD(list_of_demonstrations,
kinematics,
layer,
net_name,
folder,
dimensions,
batch_size,
fname,
config=[True, True, True]):
M = dimensions[0]
a = dimensions[1]
print "Featurizing LCD + VLAD: ", layer, net_name, folder, M, a, batch_size
BATCH_SIZE = batch_size
if constants.SIMULATION:
BATCH_SIZE = 5
data_X_PCA = {}
data_X_CCA = {}
data_X_GRP = {}
size_sampled_matrices = [
utils.sample_matrix(kinematics[demo],
sampling_rate=BATCH_SIZE).shape[0]
for demo in list_of_demonstrations
]
PC = min(100, min(size_sampled_matrices))
print "PC: ", PC
for demonstration in list_of_demonstrations:
print demonstration
W = kinematics[demonstration]
Z = load_cnn_features(demonstration, layer, folder, net_name)
W_new = utils.sample_matrix(W, sampling_rate=BATCH_SIZE)
Z_batch = None
W_batch = None
j = 1
Z_new = None
IPython.embed()
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(
constants.CAMERA) + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
Z = []
IPython.embed()
for i in range(Z):
vector_W = W[i]
W_batch = utils.safe_concatenate(W_batch, vector_W)
vector_Z = Z[i]
vector_Z = vector_Z.reshape(M, a, a)
vector_Z = lcd.LCD(vector_Z)
Z_batch = utils.safe_concatenate(Z_batch, vector_Z)
if (j == BATCH_SIZE):
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
# Re-initialize batch variables
j = 0
Z_batch = None
W_batch = None
j += 1
# tail case
if Z_batch is not None:
print "TAIL CASE"
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
if config[0]:
Z_new_pca = utils.pca_incremental(Z_new, PC=PC)
print Z_new_pca.shape
assert W_new.shape[0] == Z_new_pca.shape[0]
X_PCA = np.concatenate((W_new, Z_new_pca), axis=1)
data_X_PCA[demonstration] = X_PCA
if config[1]:
Z_new_cca = utils.cca(W_new, Z_new)
print Z_new_cca.shape
assert W_new.shape[0] == Z_new_cca.shape[0]
X_CCA = np.concatenate((W_new, Z_new_cca), axis=1)
data_X_CCA[demonstration] = X_CCA
if config[2]:
Z_new_grp = utils.grp(Z_new)
print Z_new_grp.shape
assert W_new.shape[0] == Z_new_grp.shape[0]
X_GRP = np.concatenate((W_new, Z_new_grp), axis=1)
data_X_GRP[demonstration] = X_GRP
if config[0]:
pickle.dump(data_X_PCA,
open(PATH_TO_FEATURES + fname + "_PCA" + ".p", "wb"))
if config[1]:
pickle.dump(data_X_CCA,
open(PATH_TO_FEATURES + fname + "_CCA" + ".p", "wb"))
if config[2]:
pickle.dump(data_X_GRP,
open(PATH_TO_FEATURES + fname + "_GRP" + ".p", "wb"))
