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')