def update(self, new_cost):
# New cost should be added as a dictionary, with elements [(node) : delta_cost]
self.search_nodes = 0
self.downwind_nodes = 0
# Strip zero cost nodes
new_cost = {node:new_cost[node] for node in new_cost if new_cost[node] != 0}
self.graph.add_delta_costs(new_cost)
temp_cost = {node:new_cost[node] for node in new_cost if (node in self.cost_to_come)}
# Check if all the changes aren't cost increases outside of the corridor (else return)
change_corridor = False
for node in temp_cost:
if (temp_cost[node] < 0) or ((temp_cost[node] > 0) and (node in self.corridor)):
change_corridor = True;
break
if not change_corridor:
# print "Only cost increases outside best corridor, returning"
return
# Add all non-zero cost changes to the kill list, and add their parents to the search list
kill_list = set(temp_cost.keys())
interface = set()
#for node in self.bifront_interface:
# if self.global_parent[node] != source:
# interface.add(node)
for node in temp_cost:
if node in self.parent_list:
interface.update(self.parent_list[node])
# self.frontier.clear()
cost_up_nodes = [node for node in temp_cost if temp_cost[node] > 0]
# Find all nodes downwind of a cost-increased node, and add to the kill list
if len(cost_up_nodes) > 0:
new_kills, new_interface = self.find_downwind(cost_up_nodes)
kill_list.update(new_kills)
interface.update(new_interface)
for internode in interface:
if (internode in self.cost_to_come) and (internode not in kill_list):
self.frontier.push(internode, self.cost_to_come[internode])
for killnode in kill_list:
if killnode in self.cost_to_come: del self.cost_to_come[killnode]
# if killnode in new_parent_list: del new_parent_list[killnode]
if self.image_frames != 0:
self.axes.clear()
fm_plottools.draw_corridor(self.axes, self.graph, self.cost_to_come, self.cost_to_come.keys(), interface-kill_list)
self.axes.figure.savefig('/home/nick/Dropbox/work/FastMarching/fig/map_update.pdf', bbox_inches='tight')
self.continue_bFM_search()
def make_video(self, leading_frame=[], trailing_frame=[]):
graph_frame, TEMP = fm_plottools.draw_grid(self.axes, self.graph)
costpath_frame = fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come, self.path)
corridor_frame = fm_plottools.draw_corridor(self.axes, self.graph, self.cost_to_come, self.corridor, self.corridor_interface, self.path)
path_frame, TEMP = fm_plottools.draw_grid(self.axes, self.graph, self.path)
video_frames = []
frame_hold = int(len(self.image_frames)/8)
if len(leading_frame) > 0:
for ii in range(frame_hold): video_frames.append(leading_frame)
for ii in range(frame_hold): video_frames.append(graph_frame)
video_frames.extend(self.image_frames)
for ii in range(frame_hold): video_frames.append(costpath_frame)
for ii in range(frame_hold): video_frames.append(corridor_frame)
for ii in range(frame_hold): video_frames.append(path_frame)
if len(trailing_frame) > 0:
for ii in range(frame_hold): video_frames.append(trailing_frame)
return video_frames
def make_pictures(self, fig_dir):
self.axes.figure.set_size_inches(9,6)
graph_frame, barlims = fm_plottools.draw_grid(self.axes, self.graph)
barlims = np.floor(barlims)
delta = np.floor((barlims[1]-barlims[0])/6*10)/10
cbar = self.axes.figure.colorbar(graph_frame[0], ticks=[barlims[0]+x*delta for x in range(7)])
self.axes.figure.savefig(fig_dir+'graph_cost.pdf', bbox_inches='tight')
path_frame, barlims = fm_plottools.draw_grid(self.axes, self.graph, self.path)
self.axes.figure.savefig(fig_dir+'graph_path.pdf', bbox_inches='tight')
cbar.ax.clear()
costpath_frame = fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come, self.path)
barlims = [min(self.cost_to_come.values()), max(self.cost_to_come.values())]
delta = np.floor((barlims[1]-barlims[0])/6*10)/10
self.axes.figure.colorbar(costpath_frame[0], ticks=[barlims[0]+x*delta for x in range(7)], cax=cbar.ax)
self.axes.figure.savefig(fig_dir+'path_cost.pdf', bbox_inches='tight')
corridor_frame = fm_plottools.draw_corridor(self.axes, self.graph, self.cost_to_come, self.corridor, self.corridor_interface, self.path)
self.axes.figure.savefig(fig_dir+'corridor.pdf', bbox_inches='tight')
