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