def path_probabilities(cf, path_factor, path_node, ggg, xmin, xmax, ymin, ymax, step): """ Compute probabilities of endpoints in the given range of a top-level agent path given a path node and an associated factor in a ggg, a weights vector, and a cost function. """ xstart, ystart = (xmin + 0.5, (ymax + ymin) / 2.0) probs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step))) prob_idx_to_xy = {} for i, x in enumerate(na.arange(xmin, xmax, step)): for j, y in enumerate(na.arange(ymin, ymax, step)): X, Y = sf.math2d_step_along_line( na.transpose([(xstart, ystart), (x, y)]), .1) Z = na.ones(len(X)) fig_xy = na.array([X, Y]) Xst, Yst = fig_xy[:, :-1] Xend, Yend = fig_xy[:, 1:] Theta = na.arctan2(Yend - Yst, Xend - Xst) Theta = list(Theta) Theta.append(Theta[-1]) th = list(Theta) timestamps = range(len(X)) path = Path.from_xyztheta(timestamps, [X, Y, Z, th]) pobj = PhysicalObject(Prism.from_point(X[0], Y[0], Z[0], Z[0] + 1), tags=("forklift", ), path=path) ggg.set_evidence_for_node(path_node, [pobj]) new_evidences = Evidences.copy(ggg.evidences) for phi in path_factor.nodes_with_type("phi"): new_evidences[phi.id] = True ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) ce = cf.compute_factor_cost_entry(path_factor, ggg, None) probs[j][i] = ce.probability prob_idx_to_xy[(j, i)] = (x, y) print i print 'min/max', min(probs.flatten()), max(probs.flatten()) print "max idx", na.argmax(probs) max_idx = na.argmax(probs) max_tuple = na.unravel_index(max_idx, probs.shape) print "max x,y", prob_idx_to_xy[max_tuple] return (probs, xstart, ystart)
def assign_graph( ggg, state, cost_function, potential_values_map, assignment_map ): new_evidences = ggg.evidences # assing the path grounding #new_evidences = assignPathGroundingsToGGG( state, ggg ) new_evidences = ensure_path_is_agent_state( state, ggg ) for node_id in assignment_map: node = ggg.node_from_id( node_id ) if node.is_phi: new_evidences = new_evidences.add( node_id, potential_values_map[node_id][assignment_map[node_id]] ) else: new_evidences = new_evidences.add( node_id, [ state.getGroundableById( potential_values_map[node_id][assignment_map[node_id]]) ] ) # ok, create the graph with the groundable object assigned # And we calculate the cost of all factors new_ggg = GGG.from_ggg_and_evidence_history( ggg, new_evidences ) for f in ggg.factors: new_ggg = GGG.from_ggg_and_evidence_history( new_ggg, new_evidences ) cost = cost_function( f, new_ggg ) new_evidences = new_evidences.add( f.id, cost ) new_ggg = GGG.from_ggg_and_evidence_history( ggg, new_evidences ) return new_ggg
def drawPlaceCostMap(featureBrowser, physicalObject, esdc, annotation, beam_search, xmin, xmax, ymin, ymax, step): obj = physicalObject Xs = obj.pX - obj.pX[0] Ys = obj.pY - obj.pY[0] zStart = obj.zStart zEnd = obj.zEnd cx, cy = obj.centroid2d() dx, dy = cx - obj.pX[0], cy - obj.pY[0] costs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step))) annotations = [] print dx, dy path = Path([0], [[(xmax - xmin) * 0.5], [(ymax - ymin) * 0.5], [0], [0]]) for i, x in enumerate(na.arange(xmin, xmax, step)): for j, y in enumerate(na.arange(ymin, ymax, step)): prism = Prism([Xs + x - dy, Ys + y - dx], zStart, zEnd) place = Place(prism) new_annotation = annotation_copy(annotation) new_annotation.setGrounding(esdc, place) new_annotation.setGroundingIsCorrect(esdc, True) #new_annotation.agent.setPath(path) state, gggs = annotation_to_ggg_map(new_annotation) #ggg = ggg_from_esdc(new_annotation.esdcs[0]) ggg = gggs[esdc] factor = ggg.esdc_to_factor(esdc) new_evidences = ggg.evidences for phi in factor.nodes_with_type("phi"): new_evidences = new_evidences.set_evidence(phi.id, True) ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) cost, entries = beam_search.cf_obj.costEntry([factor], state, ggg) costs[j][i] = math.exp(-1.0 * cost) annotations.append(((x, y), entries)) #axes.imshow(costs, origin="lower", # extent=(xmin, xmax, ymin, ymax)) featureBrowser.setCostImage(costs, annotations, xmin, xmax, ymin, ymax)
def extract_features(self, state, ggg, factors=[]): """ Top level feature extraction method. Assigns path groundings, then computes features. It modifies the annotation. """ new_evidences = assignPathGroundingsToGGG(state, ggg) from g3.graph import GGG ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) groundable_list = state.getObjectsSet() + state.getPlacesSet() + [state.getAgentId()] groundings = chain(*[[g for g in ggg.evidence_for_node(node) if g in groundable_list] for node in ggg.nodes if not node.is_phi]) groundings = [g for g in groundings if not isinstance(g, Path) and not None] fdict, namesdict = self.compute_features(ggg, factors) return fdict, namesdict
def describe(self, thing_to_describe): results = [] for esdc_structure in self.esdc_structures: for i in range(0, 500): annotation, esdc_candidate = annotation_candidate( esdc_structure, self.esdc_field_to_texts, self.groundings, thing_to_describe) state, esdc_to_ggg = annotation_to_ggg_map(annotation) ggg = esdc_to_ggg[esdc_candidate] new_evidences = ggg.evidences for phi in ggg.nodes_with_name("phi"): new_evidences = new_evidences.set_evidence(phi.id, True) ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) cost, entries = self.cf.costEntry(ggg.factors, state, ggg) #cost, entries = self.cf.costEntry([factor], state, ggg) results.append((cost, annotation)) results.sort() return results
def selectCandidate(self): entry = self.candidateModel.selectedEntry() if entry != None: self.featureModel.setData(self.cf_obj.lccrf, entry.candidate.dobs) self.featureTable.sortByColumn(featureModel.COL_WEIGHT_ABS) self.groundingsModel.setData(list(entry.groundings)) self.groundingTable.selectAll() import math for phi in [True, False]: ev = entry.ggg.evidences.add(entry.factor.phi_node.id, phi) ggg = GGG.from_ggg_and_evidence(entry.ggg, ev) cost, cobs, dobs = self.cf_obj.compute_factor_cost( entry.factor, ggg, entry.states) prob = math.exp(-cost) print phi, prob entry.ggg.set_evidence_for_node(entry.factor.phi_node, phi) print "highlighting", entry.groundings self.contextWindow.highlightGroundings(entry.all_groundings) self.draw()
def annotation_to_observations(annotation, fe, force_default_class_value, default_class_value, counter, id_base): observations = [] try: a_state, esdc_to_ggg = annotation_to_ggg_map(annotation) for g_index, (esdc, ggg) in enumerate(esdc_to_ggg.iteritems()): new_evidences = assignPathGroundingsToGGG(a_state, ggg) ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) true_class_value = annotation.isGroundingCorrect(esdc) if true_class_value == None and default_class_value == None: continue elif true_class_value == None: true_class_value = default_class_value if force_default_class_value: labeled_class_value = default_class_value else: labeled_class_value = true_class_value factor = ggg.esdc_to_factor(esdc) if factor != None: phi_node = factor.nodes_with_type("phi")[0] ggg.set_evidence_for_node(phi_node, labeled_class_value) obs = ggg_to_observation(ggg, a_state, fe, esdc, id_base, counter, annotation, true_class_value) if obs != None: observations.append(obs) except: print "problem with", annotation.entireText #print annotation raise return observations
def do_events(self, gspace, ggg, node, start_state, beam_width_event, search_depth, active_set_i, verbose=False, save_state_tree=False, allow_null_action=False): if verbose: print "************** DOING EVENTS" assert node.is_event new_stuff = [] i = 0 active_set = [((0, i), start_state)] i += 1 # objects = [o for o in ggg.context.objects if "pallet" in o.tags] # Only allow manipulations of objects that have been bound already in the ggg objects = ggg.object_groundings if verbose: print "doing events to", search_depth for depth in range(search_depth): new_active_set = [] for cost, s in active_set: children = s.getSuccessors(groundings=objects) if verbose: print "children", len(children) for (s1, action, modified_groundings) in children: if action == None and not allow_null_action: continue agent = s1.agent if save_state_tree: self.state_tree[s1] = (s, action) if verbose: print "************************************************** action", action if verbose: print "modified groundings" for mg in modified_groundings: print mg, mg.hash_string print "nodes and groundings in ggg" for n in ggg.gamma_nodes: print "n", n, str(ggg.node_to_top_esdc(n)), for glist in ggg.evidence_for_node(n, []): print "groundings", glist print "groundings in ggg" for g in ggg.object_groundings: print g, g.hash_string new_evidences = ggg.evidences.add(node.id, [agent]) new_evidences.replace_groundings(modified_groundings) factor = node.factor_for_link("top") assert factor != None for n in ggg.gamma_nodes: if ggg.is_ungrounded(n): new_evidences = new_evidences.add(n.id, [agent]) new_ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) object_groundings = [] for n in factor.gamma_nodes: if n.is_object: object_groundings.extend( new_ggg.evidence_for_node(n)) #object_groundings = new_ggg.object_groundings object_grounding_ids = [o.id for o in object_groundings] if verbose: print "object groundings" for og in object_groundings: print og, og.hash_string print "object grounding ids", [ og.id for og in object_groundings ] print "active ids", s.active_ids print "modified grounding ids", [ g.id for g in modified_groundings ] skip = False for mg in modified_groundings: if (not mg.id in object_grounding_ids and mg.id != s.AGENT_ID and mg.id not in s.active_ids): if verbose: print "skipping because" print "mg.id", mg.id print "active ids", s.active_ids print "grounding ids", object_grounding_ids print "agent", s.AGENT_ID skip = True break if verbose: print "skip", skip if not self.useSkip: skip = False if not skip: if save_state_tree: sequence = self.state_sequence(s1) else: sequence = None cost = self.cost(new_ggg, sequence, verbose=verbose) if na.isinf(cost): raise ValueError("Infinite cost") new_stuff.append(((cost, active_set_i), s1, new_ggg)) active_set_i += 1 new_active_set.append(((cost, i), s1)) i += 1 if verbose: print "**************************************************" active_set = prune(new_active_set, beam_width_event) return new_stuff, active_set_i
def find_plan(self, start_state, start_gggs, beam_width=10, beam_width_sequence=5, search_depth_event=5, beam_width_event=2, verbose=False, save_state_tree=False, allow_null_action=False, factor_callback=lambda ggg, factor: True): """ Infers groundings for the ggg, given the current start state. Parameters set the search depth and beam width, as well as whether it should save meta data. allow_null_action is whether it is allowed to not perform an action (based on the search depth) Returns a list of tuples [((cost, idx), start_state, ggg)] The idx is to break ties in a deterministic way when costs are equal. """ self.factor_callback = factor_callback self.state_tree = {start_state: (None, None)} #verbose = True if verbose: print "*********** find plan node search" print "beam_width", beam_width print "beam_width_sequence", beam_width_sequence print "search_depth_event", search_depth_event print "beam_width_event", beam_width_event print "start_state", start_state.getPosition() for g in start_gggs: for e in g.esdcs: print str(e) assert len(start_gggs) == 1 start_ggg = GGG.unlabeled_ggg(start_gggs[0], start_state.to_context()) if verbose: print "context", start_state.getPlacesSet() print "context", start_ggg.context.places assert start_ggg.context.agent != None for factor in start_ggg.factors: start_ggg.evidences[factor.phi_node.id] = True context = start_state.to_context() gspace = GroundingSpace(context, start_state=start_state) self.cf_obj.initialize_state(start_state) new_evidences = Evidences.copy(start_ggg.evidences) nodes = sorted_nodes_by_traversal(start_ggg) # nodes = sorted_nodes_by_esdcs(start_ggg) active_set_i = 0 self.cost(start_ggg, [start_state], verbose=verbose) active_set = [((start_ggg.cost(), active_set_i), start_state, start_ggg)] active_set_i += 1 for node in nodes: if verbose: print "doing", node, node.type print "esdc", str(start_ggg.node_to_top_esdc(node)) print "active nodes: ", len(active_set) active_set = prune(active_set, beam_width) new_active_set = [] for cost, state, ggg in active_set: if node.is_event: new_stuff, active_set_i = \ self.do_events(gspace, ggg, node, start_state, beam_width_event, search_depth_event, active_set_i, verbose, save_state_tree, allow_null_action) new_active_set.extend(new_stuff) else: candidate_values = gspace.grounding_space(node) if verbose: print "values in gspace:", len(candidate_values) for grounding in candidate_values: new_evidences = ggg.evidences.add(node.id, [grounding]) new_ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) cost = self.cost(new_ggg, state_sequence=None, verbose=verbose) if not na.isinf(cost): new_active_set.append( ((cost, active_set_i), start_state, new_ggg)) active_set_i += 1 # if len(new_active_set) < 1 and not node.is_event: # # If we couldn't find any plans, then just bind this # # node to nothing # for cost, state, ggg in active_set: # new_evidences = ggg.evidences.add(node.id, []) # new_ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) # cost = self.cost(new_ggg, state_sequence=None, verbose=verbose) # if not na.isinf(cost): # new_active_set.append(((cost, active_set_i), # start_state, new_ggg)) # active_set_i += 1 # assert len(new_active_set) active_set = new_active_set active_set.sort(key=lambda x: x[0]) if verbose: print "returning", len(active_set) # assert len(active_set) return active_set
def ggg_from_esdc(esdc, cntr=None, create_lambda=False, use_top_id=None): """ Create a grounding graph from an esdc. """ factors_to_esdcs = {} if cntr == None: #global counter cntr = IDCounter() def idstr(): return str(cntr.next_id()) ans_gg = empty_grounding_graph_yaml() ans_ev = Evidences() # Hack to compensate for the nested figure hack. if create_lambda or isinstance(esdc, TextStandoff): my_id = idstr() ans_gg["GG"]["nodes"].append( {"id": my_id , "type": "lambda" }) if isinstance(esdc, ExtendedSdc): # This should normally never happen, figure should only contain text ans_ev[my_id] = esdc[0].fields['f'] else: ans_ev[my_id] = wrapValueInList(esdc) return GGG(GroundingGraphStructure.fromYaml(ans_gg), ans_ev, factors_to_esdcs) # ESDC type? node_desc = esdc.type # top node if use_top_id != None: top_id = use_top_id else: top_id = idstr() #TODO factors to esdcs top_type = gamma_node_type(node_desc) ans_gg["GG"]["nodes"].append( {"id": top_id, "type":top_type}) curr_graph_structure = GroundingGraphStructure.empty_graph() curr_evidences = Evidences() if esdc.isLeafObject(): if len(esdc.fields['f']) > 0: corr_f_id = idstr() ans_gg["GG"]["nodes"].append( {"id": corr_f_id, "type":"phi"}) # curr_f_factor and curr_rll_factor curr_factor_id = idstr() curr_f_factor = { "id": curr_factor_id, "type": "factor_f_"+node_desc, "nodes":{"top": [top_id],"phi":[corr_f_id]}} factors_to_esdcs[curr_factor_id] = esdc curr_f_factor["nodes"]['f']= [cntr.next_id_will_be()] new_esdc = esdc.fields['f'] next_ggg = ggg_from_esdc(new_esdc, cntr, create_lambda=True) factors_to_esdcs.update(next_ggg._factor_id_to_esdc) curr_graph_structure = \ curr_graph_structure.attach_graph(next_ggg._graph) curr_evidences = Evidences.merge(curr_evidences, next_ggg.evidences) ans_gg["GG"]["factors"].append(curr_f_factor) else: # not a leaf object: # must have relation # correspondence corr_rl_id = idstr() ans_gg["GG"]["nodes"].append( {"id":corr_rl_id, "type":"phi"}) # curr_f_factor and curr_rll_factor curr_factor_id = idstr() curr_rl_factor = { "id":str(curr_factor_id), "type": "factor_rl_"+node_desc, "nodes":{"top":[top_id],"phi":[corr_rl_id]}} curr_rl_factor["nodes"]["r"] = [cntr.next_id_will_be()] new_esdc = esdc.fields['r'] factors_to_esdcs[curr_factor_id] = esdc next_ggg = ggg_from_esdc(new_esdc, cntr, create_lambda=True) factors_to_esdcs.update(next_ggg._factor_id_to_esdc) curr_graph_structure = \ curr_graph_structure.attach_graph(next_ggg._graph) curr_evidences = Evidences.merge(curr_evidences, next_ggg.evidences) for child in ["f","l","l2"]: if node_desc == "OBJECT" and child=='f': to_use_top_id = top_id else: to_use_top_id = None curr_rl_factor["nodes"][child] = [] for new_esdc in esdc.fields[child]: if not to_use_top_id: next_id_will_be = cntr.next_id_will_be() curr_rl_factor["nodes"][child].append(next_id_will_be) next_ggg = ggg_from_esdc(new_esdc, cntr, use_top_id=to_use_top_id) factors_to_esdcs.update(next_ggg._factor_id_to_esdc) curr_graph_structure = \ curr_graph_structure.attach_graph(next_ggg._graph) curr_evidences = Evidences.merge(curr_evidences, next_ggg.evidences) ans_gg["GG"]["factors"].append(curr_rl_factor) gbase = GroundingGraphStructure.fromYaml(ans_gg) return GGG(gbase.attach_graph(curr_graph_structure), curr_evidences, factors_to_esdcs)
def drawAgentPathCostMap(featureBrowser, event_path_esdcs, annotation, cf, xmin, xmax, ymin, ymax, step): print "y", ymax, ymin xstart, ystart = (xmin + 0.5, (ymax + ymin) / 2.0) #xstart, ystart = 19.658, 14.900 ath = 0 print "start", xstart, ystart, ath costs = na.zeros((int((ymax - ymin) / step), int((xmax - xmin) / step))) state, gggs = annotation_to_ggg_map(annotation) esdc = event_path_esdcs[0] ggg = gggs[esdc] factor = ggg.esdc_to_factor(esdc) node = ggg.node_for_esdc(esdc) print "esdc", esdc print "node", node, node.__class__ for i, x in enumerate(na.arange(xmin, xmax, step)): for j, y in enumerate(na.arange(ymin, ymax, step)): X, Y = sf.math2d_step_along_line(tp([(xstart, ystart), (x, y)]), .1) Z = na.ones(len(X)) fig_xy = na.array([X, Y]) Xst, Yst = fig_xy[:, :-1] Xend, Yend = fig_xy[:, 1:] Theta = na.arctan2(Yend - Yst, Xend - Xst) Theta = list(Theta) Theta.append(Theta[-1]) th = list(Theta) #th = ath*na.ones(len(X)) timestamps = range(len(X)) path = Path(timestamps, [X, Y, Z, th]) pobj = PhysicalObject(Prism.from_point(X[0], Y[0], Z[0], Z[0] + 1), tags=("forklift", ), path=path) ggg.set_evidence_for_node(node, [pobj]) #new_annotation = annotation_copy(annotation) #new_annotation.agent = new_annotation.agent.withPath(path) #if esdc.type == "EVENT": # assignPathGroundings(esdc, new_annotation) #else: #type is path # new_annotation.setGrounding(esdc, new_annotation.agent.path) new_evidences = Evidences.copy(ggg.evidences) for phi in factor.nodes_with_type("phi"): new_evidences[phi.id] = True ggg = GGG.from_ggg_and_evidence(ggg, new_evidences) # print ggg.entry_for_factor(factor) cost, entries = cf.compute_costs([factor], ggg, state_sequence=None) costs[j][i] = math.exp(-1.0 * cost) #annotations.append(((x,y), entries)) print i # break print 'min/max', min(costs.flatten()), max(costs.flatten()) featureBrowser.setCostImage(costs, xmin, xmax, ymin, ymax) return ggg