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