def get_initial_configurations(self, pose_st):
ret_configurations = []
#propose all the destinations and the first alignments
# for each of the first elements
s_I = kNN_index(pose_st[0:2], transpose(self.start_poses)[0:2], 10);
s_i = None
for i in s_I:
if(fabs(tklib_normalize_theta(self.start_poses[int(i)][2]-pose_st[2])) < pi/4.0):
s_i = int(i)
break;
if(s_i == None):
print "couldn't fine a candidate start location"
return []
for d_i, dst in enumerate(self.dest_poses):
#print "starting d_i=", d_i, 'of', len(self.dest_poses)
if(tklib_euclidean_distance(dst[0:2], pose_st[0:2]) < 10e-5):
continue
start_confs = self.partitions[s_i][d_i].init_alignments(s_i, d_i)
ret_configurations.extend(start_confs)
return ret_configurations
def get_region_to_topo(gtruth_tagfile, dests):
gtf = gtruth_tagfile
#see which destinations are contained in a region
r_to_dest = {}
for i in range(len(dests[0])):
d = dests[:, i]
#print "d", d
ps = gtf.get_contained_polygons(d)
#get the hash
for p in ps:
if (r_to_dest.has_key(p.tag)):
r_to_dest[p.tag].append(d)
else:
r_to_dest[p.tag] = [d]
#get the nearest region in case no destinations are contained
# in the region
for tag in gtf.get_tag_names():
if (not r_to_dest.has_key(tag)):
XY = gtf.get_tag_locations(tag)
i, = kNN_index([XY[0][0], XY[1][0]], dests, 1)
r_to_dest[tag] = [dests[:, i]]
return r_to_dest
def knn(self, image, k):
print "loading pca"
mypca = self.get_pca()
print "flatten"
flat_im = image.flatten()
encIm = mypca.encode(transpose([flat_im]))
print "kNN"
I = kNN_index(encIm[:,0], self.images, k);
return array(self.filenames).take(I)
def directionsToWaypoints(self, directions, x, y, z, yaw, pitch, roll):
start_xyz = na.array((x, y, z))
loc_index, = kNN_index(
(x, y, z),
na.transpose(
[self.m4du.tmap_locs_3d[x] for x in self.m4du.tmap_keys]), 1)
topo_key = self.m4du.tmap_keys[int(loc_index)]
yaw = tklib_normalize_theta(yaw)
if yaw < 0:
yaw += 2 * math.pi
orient = yaw
print "yaw", math.degrees(yaw)
curr_theta = yaw
#orient = get_orientations_annotated(self.m4du, region, self.dataset_name)[0][0]
orients = [na.append(self.m4du.orients, 360.0)]
print "orients", orients
i_tmp, = kNN_index([math.degrees(orient)], orients, 1)
print "i", i_tmp
i_tmp = int(i_tmp % len(self.m4du.orients))
vp = str(topo_key) + "_" + str(self.m4du.orients[i_tmp])
print "topo_key", topo_key, topo_key.__class__
print "vp", vp
vp_i = self.m4du.vpt_to_num[vp]
print "vp", self.m4du.vpt_to_num
#vp_i = self.m4du.
print "vp", vp_i
path = self.mbWnd.runSentence(directions, vp_i)
path_as_locs = [(start_xyz, orient)]
for p in path:
topo, orient = p.split("_")
xyz = self.m4du.tmap_locs_3d[float(topo)].tolist()
theta = radians(float(orient))
path_as_locs.append((xyz, theta))
return path_as_locs
def followDirections(self):
txt = str(self.commandText.toPlainText())
print "txt", txt
if self.lcmApp.cur_pose != None:
print "loc", self.lcmApp.cur_pose[0:3]
loc_index, = kNN_index(
self.lcmApp.cur_pose[0:3],
transpose(
[self.m4du.tmap_locs_3d[x] for x in self.m4du.tmap_keys]),
1)
topo_key = self.m4du.tmap_keys[int(loc_index)]
if isChecked(self.useRobotYawBox):
yaw = self.lcmApp.cur_pose[3]
print "yaw", yaw
yaw = tklib_normalize_theta(yaw)
if yaw < 0:
yaw += 2 * math.pi
orient = yaw
print "yaw", math.degrees(yaw)
orients = [na.append(self.m4du.orients, 360.0)]
i_tmp, = kNN_index([math.degrees(orient)], orients, 1)
i_tmp = int(i_tmp % len(self.m4du.orients))
vp = str(topo_key) + "_" + str(self.m4du.orients[i_tmp])
vp_i = self.m4du.vpt_to_num[vp]
vps = [vp_i]
else:
vps = []
for orient in self.m4du.orients:
vp = str(topo_key) + "_" + str(orient)
vp_i = self.m4du.vpt_to_num[vp]
vps.append(vp_i)
else:
vps = None
self.path = self.modelBrowser.runSentence(txt, vps)
def get_landmarks_I(self):
landmarks = {}
lmark_centroids_xy = self.get_landmark_centroids()
for k in range(len(self.part_I)):
for l in arange(self.num_next_alignments)+k:
if(l > len(self.part_I)-1):
continue
ppath = self.get_path_segment(k, l)
path_com_xy = mean(ppath, axis=1)
landmarks[(k,l)] = kNN_index(path_com_xy, lmark_centroids_xy,
min(self.num_landmarks, len(self.landmarks)));
return landmarks
def initialize(self, sloc, orient_rad):
self.tmap_orig = deepcopy(self.dg_model.tmap)
self.allowed_topologies = []
self.visited_frontiers = []
#append the starting topology
i, = kNN_index(sloc, transpose(self.dg_model.tmap_locs.values()), 1)
mytopo_i = self.dg_model.tmap_locs.keys()[int(i)]
self.allowed_topologies.append(mytopo_i)
self.allowed_topologies.extend(list(self.tmap_orig[mytopo_i]))
self.visited_frontiers.append(mytopo_i)
self.sdcs = []
#self.orient = radians(orient_deg)
self.orient = orient_rad
self.sloc = sloc
def get_readings(self, pose):
I, = (absolute(
tklib_normalize_theta_array(self.freadings_Th - pose[2])) <
pi / 3.0).nonzero()
freadings_tmp = self.freadings_XY[:, I]
myi, = kNN_index(pose[0:2], freadings_tmp, 1)
i = I[myi]
#i = I[i_theta]
ts = sorted(self.timestamp_to_fread.keys())[int(i)]
fread = self.timestamp_to_fread[ts]
image = None
odom = None
self.set_timestamp(ts)
#get the readings
done = False
seen_im = False
seen_odom = False
if (len(self.timestamp_to_odom) == 0):
seen_odom = True
while (not done):
if (seen_im and seen_odom):
done = True
vals, types = self.next_reading()
if (types[0] == "image"):
image = vals[0]
seen_im = True
elif (types[0] == "odom"):
odom = vals[0]
seen_odom = True
return fread, image, odom
def pick_landmark(self, figure):
current_loc = figure[-1]
figure_idx = random.choice([0, len(figure) / 2, -1])
landmark_probs = self.m4du.make_tag_probability_map()
#tag_prob
indices = kNN_index(figure[figure_idx], self.m4du.obj_locations, 4)
#print "geomes", self.m4du.obj_geometries[0]
candidates = [
int(i) for i in indices if self.m4du.clusters.tf.is_visible(
self.m4du.obj_locations[:, int(i)], figure[-1], max_dist=10)
]
blacklist = ["streetlight", "speedlimit_sign", "tree"
] + self.landmarks[-1:]
if len(candidates) == 0:
return None
else:
for i in range(5):
i = random.choice(candidates)
landmark_name = self.m4du.obj_names[i]
if not landmark_name in blacklist:
break
if len(self.landmarks) > 0 and landmark_name == self.landmarks[-1]:
print "landmark", landmark_name
print "landmarks", self.landmarks
return None
if landmark_name in blacklist:
if random.choice([True, False]):
return None
else:
return i
return i
def observation_matrices(self, sdcs, loc, sorients_rad):
"""
Yield the sequence of observationmatrices for each SDC. This
combines p_obs and p_trans.
"""
print "beginning greedy search, NOT viterbi. It's called viterbi for compatability."
from pyTklib import tklib_du_lp_obs as tklib_observation_probs
i, = kNN_index(loc, transpose(self.tmap_locs.values()), 1)
iSlocTopo = self.tmap_locs.keys()[int(i)]
orients = self.get_viewpoint_orientations(self.num_viewpoints)
i_tmp, = kNN_index([degrees(sorients_rad[0])], [orients], 1)
self.iSloc = self.vpt_to_num[str(iSlocTopo) + "_" +
str(orients[i_tmp])]
print "preparing"
T_seq, O_seq, SR_seq, L_seq, D_seq, self.SDC_utilized, newpi = self.inference_prepare(
sdcs, loc, sorients_rad)
print "looping"
self.mygm = FakeGm()
self.mygm.T_seq = T_seq
T_mat_prev = [[]]
for i, (T_mat) in enumerate(T_seq):
if (not len(SR_seq) == 0 and not SR_seq[i] == None):
SR_reshape = SR_seq[i].reshape(
[len(SR_seq[i]) * len(SR_seq[i][0]),
len(SR_seq[i][0][0])])
L_curr = L_seq[i].astype(float)
else:
SR_reshape = [[]]
L_curr = []
if (O_seq[i] == None):
O_mat = []
else:
O_mat = O_seq[i]
if (D_seq == None or D_seq[i] == None):
D_mat = [[]]
else:
D_mat = D_seq[i]
# for the gui
self.mygm.update_args.append(
dict(
T_prev=T_mat_prev,
SR_curr=SR_reshape,
D_curr=D_mat,
L_curr=L_curr,
O_curr=O_mat,
vp_index_to_topo_index=self.vp_i_to_topo_i,
num_topologies=len(self.tmap.keys()),
))
T_mat = na.transpose(T_mat)
# ret_mat is the observation probability of a transition from vp_1 to vp_2.
ret_mat = na.array(
tklib_observation_probs(len(self.viewpoints),
self.vp_i_to_topo_i, SR_reshape,
L_curr, O_mat,
self.topo_to_location_mask,
len(self.tmap.keys())))
ret_mat = ret_mat.reshape(len(self.viewpoints),
len(self.viewpoints))
observation_probs = T_mat * ret_mat * D_mat
T_mat_prev = T_mat
yield observation_probs
def closest_node(self, xy):
#nodes_xy = transpose([node.xy for node in self.nodes])
i, = kNN_index(xy, self.nodes_xy, 1)
return self.nodes[int(i)]
def infer_explore(dg_model, sdcs, sloc, orient, num_explorations,
heuristic_name, params_num):
dg_model_cp = cpcp.copy(dg_model)
tmap_orig = deepcopy(dg_model_cp.tmap)
dg_model_cp.tmap = tmap_orig
allowed_topologies = []
visited_frontiers = []
visited_viewpoints = []
heuristic_map = {
"step": heuristic_step,
"stairs": heuristic_stairs,
"lifted_stairs": heuristic_lifted_stairs,
"slope_offset_delay": heuristic_SOD,
"frontier": heuristic_frontier
}
heuristic = heuristic_map[heuristic_name]
#append the starting topology
i, = kNN_index(sloc, transpose(dg_model_cp.tmap_locs.values()), 1)
mytopo_i = dg_model_cp.tmap_locs.keys()[int(i)]
# print "initial topology: ",mytopo_i
allowed_topologies.append(mytopo_i)
allowed_topologies.extend(list(tmap_orig[mytopo_i]))
# print "allowed topologies: ",allowed_topologies
visited_frontiers.append(mytopo_i)
SOD_default = (0.5, 1, 1)
num_sdcs = len(sdcs)
hpar = h_param()
hpar.num_sdcs = num_sdcs
if params_num in [None, ""]:
params_num = SOD_default
hpar.SOD = params_num
slope = params_num[0]
offset = params_num[1]
delay = params_num[2]
if heuristic_name == "slope_offset_delay":
num_explorations = int((num_sdcs - offset + 0.0) / slope + delay)
if heuristic_name == "frontier":
hpar.curr_num_sdcs = 1
num_explorations = 2
#print "starting topologies", allowed_topologies
print "Number of SDCs: ", len(sdcs)
# for i in range(2*len(sdcs)):
lst_of_i = range(num_explorations)
for i in lst_of_i:
hpar.i = i
#create the correct topological map
tmap_new = tmap_mask(tmap_orig, allowed_topologies)
#print "new topological map", tmap_new
#set the topological map in the model
dg_model_cp.tmap = tmap_new
#give the current number of sdcs
# curr_sdcs = sdcs[0:min(i/2+1,len(sdcs))]
curr_sdcs = sdcs[0:heuristic(hpar)]
#print "USING ",len(curr_sdcs)," SDCS"
dg_model_cp.initialize_transition_matrices()
#print "topological map is: ", dg_model_cp.tmap
#infer the best destinations
dest_0, prob_log, sdcs_u, probs_log = dg_model_cp.infer_destination(
curr_sdcs, sloc, orient)
dest, prob_log = get_best_frontier(dg_model_cp, tmap_new, probs_log,
visited_frontiers)
#print "best frontier:",dest,prob_log
if heuristic_name == "frontier":
if dest_0 != dest and prob_log != log(0):
pass
else:
hpar.curr_num_sdcs += 1
if hpar.curr_num_sdcs < num_sdcs:
lst_of_i.append(lst_of_i[-1] + 1)
lst_of_i.append(lst_of_i[-1] + 1)
if prob_log != log(0):
mydest = float(dest[0].split("_")[0])
if (not mydest in tmap_new.keys()):
print "probability", prob_log
print tmap_new
raise KeyError("Bad key value")
visited_frontiers.append(mydest)
# print "visited topology ",mydest
visited_viewpoints.append(dest[0])
allowed_topologies.extend(tmap_orig[mydest])
# print "allowed topologies: ",allowed_topologies
tmap_new = tmap_mask(tmap_orig, allowed_topologies)
#print "final topological map", tmap_new
dg_model_cp.tmap = tmap_new
dg_model_cp.initialize_transition_matrices()
ret_vals = dg_model_cp.infer_path(sdcs, sloc, orient)
path, probs, sdcs_eval = ret_vals
if path in [None, []]:
print "infer_path() failed!"
new_path = visited_viewpoints
else:
new_path = [path[0]]
new_path.extend(visited_viewpoints)
new_path.append(path[-1])
ret_vals = (new_path, probs, sdcs_eval)
# dg_model_cp.set_topo_map(tmap_orig)
dg_model_cp.tmap = tmap_orig
print "returning: ", ret_vals, visited_viewpoints
return ret_vals, visited_viewpoints