def _update_landmark_context(self, new_lmark):
for i, lmark in enumerate(self.landmarks):
if lmark == new_lmark:
continue
if sf.math2d_dist(lmark.centroid2d, new_lmark.centroid2d) < 10.0:
for t in new_lmark.tags:
self.landmark_context[i].add(t)
def find_closest_object(objects, point):
if len(objects) == 0:
return None
else:
sorted_objects = list(
sorted(objects, key=lambda o: sf.math2d_dist(o.centroid2d, point)))
return sorted_objects[0]
def size_of_axes_in_data_coordinates(axes):
pt = axes.transAxes.transform_point((1, 1))
max_p = axes.transData.inverted().transform_point(pt)
pt = axes.transAxes.transform_point((0, 0))
min_p = axes.transData.inverted().transform_point(pt)
return math2d_dist(min_p, max_p)
def object_landmark_features(self, agent, f_prism, r_words, l_prism):
result_dict = {
"dist_f_l": sf.math2d_dist(f_prism.centroid2d(),
l_prism.centroid2d())
}
result_dict = add_word_features(result_dict, r_words)
return result_dict
def path_landmark_features(self, f_path, r_words, l_grounding):
if sf.math2d_dist(f_path.points_pts[-1], l_grounding.centroid2d) < 4:
fdict = {"ends_close": 1}
else:
fdict = {"ends_close": 0}
fdict = add_word_features(fdict, r_words)
return fdict
def compute_cost_away_from(self, factor, ggg):
figure_node = factor.nodes_for_link("top")[0]
landmark_node = factor.nodes_for_link("l")[0]
figure_value = ggg.evidence_for_node(figure_node)[0]
landmark_value = ggg.evidence_for_node(landmark_node)[0]
dist = sf.math2d_dist(figure_value.centroid2d,
landmark_value.centroid2d)
area = sf.math2d_area(landmark_value.prism.points_xy)
t = dist / area
prob = 1.0 / (1 + math.exp(-t * 1))
print "prob", prob
return -math.log(prob)
def tmapFromObjectsAndPlaces(loc_xy, objects, places):
place_indices = set()
for o in objects:
distances = [
sf.math2d_dist(o.centroid2d, p.centroid2d) for p in places
]
min_idx = na.argmin(distances)
distances[min_idx] = sys.maxint
# second closest place, so it's not the one right on top of the object.
min_idx = na.argmin(distances)
place_indices.add(min_idx)
return tmapFromObjects(loc_xy, [places[i] for i in place_indices],
add_offset=False)
def _get_landmark_context(self, i):
return set()
if(not self.landmark_context.has_key(i)):
known_objects = set()
landmark = self.landmarks[i]
for other_lmark in self.landmarks:
#curr_visible = self.get_visible_tags(landmark[:,j], max_dist=10.0)[0]
if (hasattr(landmark, "centroid2d") and
sf.math2d_dist(landmark.centroid2d, other_lmark.centroid2d) < 10.0):
for t in other_lmark.tags:
known_objects.add(t)
self.landmark_context[i] = known_objects
return self.landmark_context[i]
def correct_at_threshold(results, threshold=10):
result = []
for i, paths in enumerate(results['path']):
t1_loc = results["end_regions"][i]
t1_loc = (t1_loc[0][0], t1_loc[1][0])
correct = False
for path in paths:
t2_i = float(path[-1].split("_")[0])
t2_loc = results["tmap_locs"][t2_i]
curr_d = math2d_dist(t2_loc, t1_loc)
if curr_d < 10:
correct = True
break
result.append(correct)
return result
def testGoWaverly(self):
esdcs, plans = self.cfb.followCommand("Go to the trailer.")
plan = plans[0]
state = plan.state
ggg = plan.ggg
agent = state.agent
eventNode = [node for node in ggg.nodes if "EVENT" in node.type][0]
pathNode = eventNode.factors[0].nodes_for_link("l")[0]
truckNode = pathNode.factors[1].nodes_for_link("l")[0]
print truckNode
hopefullyTruck = ggg.evidences[truckNode.id][0]
dist = sf.math2d_dist(
agent.prismAtT(-1).centroid2d(),
hopefullyTruck.prismAtT(-1).centroid2d())
self.assertEqual(hopefullyTruck.tags, ('flatbed', 'trailer'))
def __init__(self, objects, places, agent_id=-100):
self.objects = unique_groundings(objects)
self.places = unique_groundings(places)
self.agent_id = agent_id
centroids = ([o.centroid2d for o in self.objects] +
[p.centroid2d for p in self.places])
if len(centroids) != 0:
bbox = sf.math2d_bbox(tp(centroids))
self.static_bbox_pts = sf.math2d_bbox_to_polygon(bbox)
self.static_scale = sf.math2d_dist(self.static_bbox_pts[0],
self.static_bbox_pts[1])
else:
self.static_bbox_pts = []
self.static_scale = 1.0
self.moving_objects = [o for o in objects if o.path != None]
self.paths = [o.path for o in self.moving_objects]
self.groundings = self.objects + self.places + self.paths
self.grounding_id_to_grounding = dict((g.id, g)
for g in self.groundings)
#assert len(self.groundings) == len(self.grounding_id_to_grounding), ("duplicate ids",
# sorted([g.id for g in self.groundings]),
# sorted(self.grounding_id_to_grounding.keys()))
self.agent = self.grounding_id_to_grounding.get(self.getAgentId())
self.update_rep()
def plot_distance_curve_random(model,
corpus_fn,
gtruth_tag_fn,
map_fn,
color,
marker,
label='',
linestyle="-",
region_to_topology=None):
"""
Needs the viewpoints and stuff from the model.
"""
print "starting random"
dsession = readSession(corpus_fn, "none")
if gtruth_tag_fn != None:
tf = tag_file(gtruth_tag_fn, map_fn)
topohash = get_region_to_topo_hash_containment(tf, model)
else:
topohash = region_to_topology
Dists = []
for elt in dsession:
for i in range(len(elt.routeInstructions)):
if (elt.columnLabels[i] is None):
print "sentence", i, "was", elt.columnLabels[i]
continue
start_true, end_true = elt.columnLabels[i].split("to")
start_true = str(start_true.strip())
end_true = str(end_true.strip())
iSlocTopo = topohash[start_true][0]
iElocTopo = topohash[end_true][0]
eloc = model.tmap_locs[iElocTopo]
total_dist = 0.0
for vp in model.viewpoints:
topo, orient = vp.split("_")
vp_loc = model.tmap_locs[float(topo)]
total_dist += math2d_dist(vp_loc, eloc)
expected_dist = total_dist / len(model.viewpoints)
Dists.append(expected_dist)
Y = []
X = []
for threshold in Dists:
#get the ones above the threshold
#print nonzero(array(Dists) > threshold)
#print array(Dists) > threshold
Itrue, = nonzero(array(Dists) <= threshold)
Y.append(len(Itrue) / (1.0 * len(Dists)))
X.append(threshold)
num_correct_at_threshold = len(nonzero(array(Dists) <= 10)[0])
print "random less than 10 meters", num_correct_at_threshold,
print "%.3f%%" % (num_correct_at_threshold / (1.0 * len(Dists)))
print "sorting"
X, I = quicksort(X)
print "taking"
Y = array(Y).take(I)
print "plotting"
if (X[0] > 0.0):
Xf = [X[0]]
Xf.extend(X)
Yf = [0]
Yf.extend(Y)
X = Xf
Y = Yf
p = plot_markers_evenly(X,
Y,
label,
marker,
color,
linewidth=2.5,
linestyle=linestyle)
xlabel('distance from destination (m)')
ylabel('proportion correct')
return p
def plot_distance_curve_subject(ofile,
create_figure=True,
mystyle=None,
best_sub_only=False,
best_question_only=False,
included_subjects=None):
styles = [
"ro-", "b^-", "k>-", "g<-", "ro--", "b^--", "k>--", "g<--", "ro-.",
"b^-.", "k>-.", "g<-.", "ro:", "b^:", "k>:", "g<:"
]
if (create_figure):
figure()
Dists = {}
Dists_question = {}
for i in range(len(ofile['path'])):
#Dists.append([])
if (ofile['sentences'][i] is None):
print "sentence", i, "was", ofile['sentences'][i]
continue
rst, rend = ofile['regions'][i].split("to")
rend = rend.strip()
t2 = ofile['region_to_topology'][rend]
#iterate over all the topologies in the final region
curr_d = 100000000000000000000000000000.0
for myelt in t2:
t2_loc = ofile['tmap_locs'][myelt]
#iterate over all of the paths that end in the location
for k in range(len(ofile['path'][i])):
path = ofile['path'][i][k]
if path is None:
curr_d = 100000000000000000
else:
t1 = path[-1]
t1 = float(t1.split("_")[0])
t1_loc = ofile["tmap_locs"][t1]
if (math2d_dist(t2_loc, t1_loc) < curr_d):
curr_d = math2d_dist(t2_loc, t1_loc)
#subjects
if (not Dists.has_key(ofile["subjects"][i])):
Dists[ofile["subjects"][i]] = []
Dists[ofile["subjects"][i]].append(curr_d)
#regions
if (not Dists_question.has_key(ofile["regions"][i])):
Dists_question[ofile["regions"][i]] = []
Dists_question[ofile["regions"][i]].append(curr_d)
xlabel('distance from destination (m)')
ylabel('percentage correct')
mylabel = None
if (best_sub_only):
dvals = sum(Dists.values(), axis=1)
i = argmin(dvals)
new_vals = Dists.values()[i]
new_key = Dists.keys()[i]
Dists = {}
Dists[new_key] = new_vals
#mylabel=thelabel+" (Best Subject)"
if (best_question_only):
dvals = sum(Dists_question.values(), axis=1)
i = argmin(dvals)
new_vals = Dists_question.values()[i]
new_key = Dists_question.keys()[i]
Dists = {}
Dists[new_key] = new_vals
#mylabel=thelabel+" (Best Question)"
plots = []
for k, subject in enumerate(Dists.keys()):
if included_subjects != None and not subject in included_subjects:
continue
Y = []
X = []
for threshold in Dists[subject]:
#get the ones above the threshold
Itrue, = nonzero(array(Dists[subject]) <= threshold)
Y.append(len(Itrue) / (1.0 * len(Dists[subject])))
X.append(threshold)
X, I = quicksort(X)
Y = array(Y).take(I)
sub_plt = subject.replace("Subject", "Sub.")
if mystyle is None:
style = styles[k % len(styles)]
else:
style = mystyle
if (X[0] > 0.0):
Xf = [X[0]]
Xf.extend(X)
Yf = [0]
Yf.extend(Y)
X = Xf
Y = Yf
plots.extend(plot(X, Y, style, label=sub_plt, linewidth=2.5))
num_correct_at_threshold = len(nonzero(array(Dists[subject]) <= 10)[0])
print k, subject, "less than 10 meters", num_correct_at_threshold,
print "%.3f%%" % ((100.0 * num_correct_at_threshold) /
(1.0 * len(Dists[subject])))
return plots
def plot_distance_curve(ofile,
corpus,
marker,
color,
thelabel='',
use_strict_correctness=False,
followedState=None,
sentence_i_to_run=None,
linestyle="-"):
Dists = []
threshold = 10
num_correct = 0
total = 0.0
for i in range(len(ofile['path'])):
if (ofile['sentences'][i] is None):
print "sentence", i, "was", ofile['sentences'][i]
continue
rst, rend = ofile['regions'][i].split("to")
rst = rst.strip()
rend = rend.strip()
direction = corpus.directions[i]
if followedState != None and direction.was_followed != followedState:
assert direction.start == rst, (direction.start, rst)
assert direction.end == rend, (direction.end, rend)
continue
#print "r", ofile['region_to_topology']
t2 = ofile['region_to_topology'][rend]
#iterate over all the topologies in the final region
curr_d = 70.0
for myelt in t2:
t2_loc = ofile['tmap_locs'][myelt]
#iterate over all of the paths that end in the location
for k in range(len(ofile['path'][i])):
if ofile['path'][i][k] is None:
continue
t1 = ofile['path'][i][k][-1]
t1 = float(t1.split("_")[0])
t1_loc = ofile["tmap_locs"][t1]
if (math2d_dist(t2_loc, t1_loc) < curr_d):
curr_d = math2d_dist(t2_loc, t1_loc)
if use_strict_correctness and sentence_i_to_run != None:
raise ValueError("Must pass one or the other and not both." +
` use_strict_correctness ` + " and " +
` sentence_i_to_run `)
if use_strict_correctness or sentence_i_to_run != None:
if use_strict_correctness:
best_scoring_run_k = argmax(ofile['probability'][i])
elif sentence_i_to_run != None:
best_scoring_run_k = sentence_i_to_run[myelt]
if ofile['path'][i][best_scoring_run_k] is None:
best_scoring_run_k = argmax(ofile['probability'][i])
t1 = float(
ofile['path'][i][best_scoring_run_k][-1].split("_")[0])
t1_loc = ofile["tmap_locs"][t1]
curr_d = math2d_dist(t2_loc, t1_loc)
if curr_d < threshold or ofile['correct'][i][0]:
num_correct += 1
total += 1
Dists.append(curr_d)
all_visited_topos = []
import cPickle
model = cPickle.load(open(ofile["options"]["model_fn"], 'r'))
print "len", len(ofile["visited_viewpoints"])
for visited_vps in ofile["visited_viewpoints"]:
assert len(visited_vps) == 1, len(visited_vps)
visited_topos = set()
for vp in visited_vps[0]:
#print "vp", vp
topo_i, orient = vp.split("_")
visited_topos.add(topo_i)
all_visited_topos.append(
float(len(visited_topos)) / len(model.tmap_locs))
#print "visited", all_visited_topos
#print "ofile", ofile["path"][0]
print "average # of nodes visited", mean(all_visited_topos)
print thelabel, "num_correct less than %.2f meters: %d (%.3f%%), visited %.3f%%" % (
threshold, num_correct, 100.0 * num_correct / total,
100 * mean(all_visited_topos))
Y = []
X = []
for threshold in Dists:
#get the ones above the threshold
#print nonzero(array(Dists) > threshold)
#print array(Dists) > threshold
Itrue, = nonzero(array(Dists) <= threshold)
Y.append(len(Itrue) / (1.0 * len(Dists)))
X.append(threshold)
X, I = quicksort(X)
Y = array(Y).take(I)
p = plot_markers_evenly(X,
Y,
thelabel,
marker,
color,
linewidth=2.5,
linestyle=linestyle)
xlabel('distance from destination (m)')
ylabel('proportion correct')
#draw()
#show()
#raw_input()
return p
def planToLcmLog(state, ggg, fname, rndf, app=None, realForklift=False):
if app == None:
app = action_executor.App(rndf_file=rndf)
sequence = state.getSequence()
log = lcm.EventLog(fname, 'w', overwrite=True)
sx, sy, sz, stheta = state.getGroundableById(
state.getAgentId()).path.points_ptsztheta[0]
#if actionMap != None:
# startId = actionMap.nearest_index((sx, sy))
# for chk_pt in rndf.checkpoints:
# if chk_pt.id_int == startId:
# lat = chk_pt.waypoint.lat
# lon = chk_pt.waypoint.lon
# break
#else:
lat, lon = ru.xy_to_latlon(sx, sy, rndf.origin)
if not realForklift:
transportEvent = app.send_transport_msg(lat,
lon,
stheta,
returnEvent=True)
log.write_event(transportEvent.timestamp, transportEvent.channel,
transportEvent.data)
for s, action in sequence:
print action
events = []
if isinstance(action, forkState.Move):
#event = app.send_checkpoint(a.to_loc, returnEvent=True)
sx, sy = action.from_location[0:2]
x, y = action.to_location[0:2]
#To keep forklift from trying to get to close to a pallet
line = sf.math2d_step_along_line([(x, sx), (y, sy)], .1)
line_pts = transpose(line)
worstCase = True # worst case if all points along line are too close to pallets
bestX = x
bestY = y
maxMinDist = 0 # distance for the point with greatest buffer (infinum)
for px, py in line_pts:
tooClose = False
held_pallet = s.get_held_pallet()
localMinDist = 100 # keeps track of the distance to closest pallet for this (px, py)
for obj in [
s.getGroundableById(gid) for gid in s.getObjectsSet()
]:
if held_pallet and obj.lcmId == held_pallet.lcmId:
continue
distFromPallet = sf.math2d_dist(
(px, py),
obj.prismAtT(-1).centroid2d())
if distFromPallet < localMinDist:
localMinDist = distFromPallet
if distFromPallet < 4.0:
tooClose = True
break
if not tooClose:
bestX = px
bestY = py
print 'changing move from', (x, y), 'to', (bestX, bestY)
worstCase = False
break
elif localMinDist > maxMinDist: # does this (px, py) offer a better worst case buffer?
maxMinDist = localMinDist
bestX = px
bestY = py
# If none of the points were sufficiently far from the pallet, choose the one that was the farthest
if worstCase == True:
print '(px, py) farthest from pallet was ', maxMinDist, ' away. Changing from', (
x, y), 'to', (bestX, bestY)
px = bestX
py = bestY
lat, lon = ru.xy_to_latlon(px, py, rndf.origin)
events.append(app.send_latlon(lat, lon, returnEvent=True))
elif isinstance(action, forkState.Place):
x, y, z = action.location
lat, lon = ru.xy_to_latlon(x, y, rndf.origin)
if realForklift:
print "**************************************"
print "WARNING--sending to forklift checkpoint 16"
print "this is a hack and only applies at waverly"
print "long term, agile should probably be changed to"
print "handle long distance \"place\" commands without"
print "having to go to a checkpoint first."
events.append(app.send_checkpoint(16, returnEvent=True))
events.append(
app.send_place_pallet_msg(lat, lon, z, returnEvent=True))
elif isinstance(action, forkState.Pickup):
print "pallet", action.pallet_id
if action.pallet_id == -1:
print "************************************"
print "warning, overwriting pallet id", action.pallet_id
print "remove me after we figure out why it is -1."
print "we are hardcoding it to the correct ID in one specific"
print "scenario, but this does not generalize."
pallet_id = 330225303595152896
else:
pallet_id = action.pallet_id
events.append(
app.send_pickup_pallet_msg(pallet_id, returnEvent=True))
elif action == None:
continue
else:
raise TypeError('Unrecognized action: ' + ` action ` + " class: " +
` action.__class__ `)
for event in events:
log.write_event(event.timestamp, event.channel, event.data)
log.close()
def get_region_to_topo_hash_containment(tf_region, dg_model):
#the tagfile here is of the regions
ret_hash = {}
ppoly = tf_region.polygons
mymap = tf_region.get_map()
for pp in ppoly:
#add all of the topologies based on containment
pts_I = [];
for tm_key in dg_model.tmap_keys:
tm_loc = dg_model.tmap_locs[tm_key]
tm_loc = mymap.to_index(tm_loc)
bbx1 = min(pp.X); bby1 = min(pp.Y);
bbx2 = max(pp.X); bby2 = max(pp.Y);
if(tm_loc[0] <= bbx2 and tm_loc[0] >= bbx1 and
tm_loc[1] <= bby2 and tm_loc[1] >= bby1):
#it is contained
if(len(dg_model.tmap[tm_key]) == 0):
continue
if(ret_hash.has_key(pp.tag)):
ret_hash[pp.tag].append(tm_key)
else:
ret_hash[pp.tag] = [tm_key]
pts_I.append(tm_loc)
#resort them by distance from the center
# of the original region
if(ret_hash.has_key(pp.tag) and len(ret_hash[pp.tag]) > 1) and len(pts_I) != 0:
D = tklib_get_distance(transpose(pts_I), [mean(pp.X), mean(pp.Y)]);
D_srt, I_srt = quicksort(D)
ret_hash[pp.tag] = list(array(ret_hash[pp.tag]).take(I_srt))
#in case nothing was added for a particular tag
if(not ret_hash.has_key(pp.tag)):
print "region not found via containment"
#raw_input()
best_tmkey = None
best_tmdist = 10000000000000000.0
best_tmkey_dist = None
best_tmdist_dist = 10000000000000000.0
tm_loc1, tm_loc_dist = None, None
for tm_key in dg_model.tmap_keys:
tm_loc = dg_model.tmap_locs[tm_key]
#tm_loc is in xy and we need to convert to
# an index
#print "getting euclidean dist"
tm_d = math2d_dist([mean(pp.X), mean(pp.Y)],
mymap.to_index(tm_loc))
#print "getting distances"
tm_d_dist = pp.min_dist(mymap.to_index(tm_loc))
#print "next"
if(tm_d < best_tmdist):
best_tmdist = tm_d
best_tmkey = tm_key
tm_loc1 = tm_loc
if(tm_d_dist < best_tmdist_dist):
best_tmdist_dist = tm_d_dist
best_tmkey_dist = tm_key
tm_loc_dist = tm_loc
pts_I.extend([tm_loc_dist, tm_loc1])
ret_hash[pp.tag] = [best_tmkey_dist, best_tmkey]
return ret_hash
