def plot_final_paths(ax, true_g, true_path, models):
axax = [ax[0][1], ax[1][0], ax[1][1]]#, ax[1][2]]
graph_frame = []
#if not ax[0][0].lines:
tempframe, barlims = fm_plottools.draw_grid(ax[0][0], true_g, true_path)
graph_frame.extend(tempframe)
for i in range(len(models)):
tempframe, barlims = fm_plottools.draw_grid(axax[i], models[i].GP_cost_graph, path=models[i].fbFM.path, max_cost=barlims[1],min_cost=barlims[0] )
tempframe.append(axax[i].plot(models[i].X[:,0], models[i].X[:,1], 'rx')[0])
graph_frame.extend(tempframe)
return graph_frame
def plot_updates(ax, models):
axax = [ax[0][1], ax[1][0], ax[1][1]]
graph_frame = []
for i in range(3):
tempframe, barlims = fm_plottools.draw_grid(axax[i], models[i].GP_cost_graph, models[i].fbFM.path, 19)
tempframe.append(axax[i].plot(models[i].X[:,0], models[i].X[:,1], 'rx')[0])
graph_frame.extend(tempframe)
return graph_frame
def plot_current_path(self, nexttarget):
graph_frame = []
if not self.best_cost_plot:
self.best_cost_plot, barlims = fm_plottools.draw_grid(self.ax[0], self.true_g, self.best_path, 19)
graph_frame.extend(self.best_cost_plot)
while self.ax[1].lines:
self.ax[1].lines.pop()
self.cost_plot_matrix, barlims = fm_plottools.draw_grid(
self.ax[1], self.fm_sampling_explorer.GP_cost_graph, self.fm_sampling_explorer.fbFM.path, 19
)
self.cost_plot_matrix.append(
self.ax[1].plot(self.fm_sampling_explorer.X[:, 0], self.fm_sampling_explorer.X[:, 1], "rx")[0]
)
self.cost_plot_matrix.append(self.ax[1].plot(nexttarget[0], nexttarget[1], "bo")[0])
graph_frame.extend(self.cost_plot_matrix)
self.video_frames.append(graph_frame)
self.fig.savefig("fm_explorer_{0}_S{1:02d}.pdf".format(self.nowstr, self.num_visited - 1), bbox_inches="tight")
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')
if MAKE_VIDEO:
t_frames = []
fig_sg, ax_sg = fm_plottools.init_fig(graph=true_g, figsize=(6,5))
fm_sampling_explorer.set_plots(t_frames, ax_sg)
fm_sampling_explorer.set_plot_costs(0, 15)
fm_sampling_explorer.search()
if MAKE_PICTURES:
tFM = fast_marcher.FullBiFastMarcher(true_g)
tFM.set_start(start_node)
tFM.set_goal(end_node)
tFM.search()
tFM.pull_path()
fig1,ax1 = fm_plottools.init_fig(graph=true_g, figsize=(6,5))
tempframe,barlims = fm_plottools.draw_grid(ax1, true_g, tFM.path,min_cost=mean_value-0.5)
cbar = fig1.colorbar(tempframe[0], shrink=0.7)
fig1.savefig(VID_DIR+'true_map.png', bbox_inches='tight', transparent=True)
fm_plottools.clear_content(tempframe)
tempframe,barlims= fm_plottools.draw_grid(ax1, fm_sampling_explorer.GP_cost_graph, min_cost=barlims[0], max_cost=barlims[1])
tempframe.append(ax1.plot(fm_sampling_explorer.X[:,0], fm_sampling_explorer.X[:,1], 'rx', mew=1)[0])
#cbar = fig1.colorbar(tempframe[0], shrink=0.7)
fig1.savefig(VID_DIR+'initial_estimate.png', bbox_inches='tight', transparent=True)
tempframe.append(ax1.plot(45, 16, 'wo', mfc='none', ms=12, mew=2, mec='w')[0])
fig1.savefig(VID_DIR+'update_location.png', bbox_inches='tight', transparent=True)
fm_plottools.clear_content(tempframe)
tempframe,barlims= fm_plottools.draw_grid(ax1, fm_sampling_explorer.GP_cost_graph, fm_sampling_explorer.fbFM.path, min_cost=barlims[0], max_cost=barlims[1])
tempframe.append(ax1.plot(fm_sampling_explorer.X[:,0], fm_sampling_explorer.X[:,1], 'rx', mew=1)[0])
fig1.savefig(VID_DIR+'initial_estimate_withpath.png', bbox_inches='tight', transparent=True)
true_path_cost = np.zeros(NUM_SAMPLES)
X = np.vstack([X,[tFM.end_node[0]-4.0, tFM.end_node[1]]])
X = np.vstack([X,[tFM.end_node[0], tFM.end_node[1]-4.0]])
X = np.vstack([X,[20,26],[37,44],[84,72],[90,90]]) #[30,30],[50,50],[60,60],[70,70],
Y = np.atleast_2d([sample_cost_fun(explore_cost_function, x, cblobs) for x in X]).transpose()
FMEx = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value=mean_value, obs=true_g.obstacles,
GP_l=GP_l, GP_sv=GP_sv, GP_sn=GP_sn)
FMEx.search()
if SEP_PLOTS:
f0 = [plt.figure(i, figsize=(2.2,2.2)) for i in range(4)]
a0 = [f0[i].add_subplot(111) for i in range(4)]
else:
f0 = plt.figure(0); f0.clf()
a0 = [f0.add_subplot(2,2,i+1) for i in range(4)]
temp,barlims=fm_plottools.draw_grid(a0[0], true_g, path=tFM.path)
fm_plottools.draw_grid(a0[1], FMEx.GP_cost_graph, path=FMEx.fbFM.path, min_cost=barlims[0], max_cost=barlims[1])
a0[1].plot(X[:,0], X[:,1], 'rx')
newY = explore_cost_function(newX[0], newX[1], cblobs)
delta = newY - FMEx.GP_cost_graph.cost_fun(newX[0], newX[1])
cost_update = poly_cost_obj.set_update(newX[0], newX[1], delta)
ts = time.time()
FMEx.cost_update_new(cost_update, recalc_path=True)
tup = time.time()-ts
fm_plottools.draw_grid(a0[2], FMEx.fbFM.graph, path=FMEx.fbFM.updated_path, min_cost=barlims[0], max_cost=barlims[1])
a0[2].plot(newX[0], newX[1], 'ko')
a0[2].plot(newX[0], newX[1], 'ko', fillstyle='none', markersize=50)
ts = time.time()
FMEx.add_observation(np.array([newX]), np.array([[newY]]))
print 'Creating true field fast marcher... ',
# Create search for true field
tFM = fast_marcher.FullBiFastMarcher(true_g)
tFM.set_start(start_node)
tFM.set_goal(end_node)
tFM.search()
tFM.pull_path()
best_path_cost = calc_true_path_cost(explore_cost_function, tFM.path)
print 'done. Best path cost = {0}'.format(best_path_cost)
fig1, ax1 = plt.subplots(2, 2, sharex=True, sharey=True)
ax1[0][0].set_title("True cost field")
ax1[0][1].set_title("Estimated cost - random sampling")
ax1[1][0].set_title("Estimated cost - max variance sampling")
ax1[1][1].set_title("Estimated cost - FM sampling")
fm_plottools.draw_grid(ax1[0][0], true_g, tFM.path, 19)
#fm_plottools.draw_fbfmcost(ax1[0][0], true_g, tFM.path_cost, tFM.path, 1000, 1400)
print 'Creating fast marching explorers... ',
# Create initial GP
mean_value = 3;
X = np.array([[3, 5]])
Xshape = X.shape
Y = np.zeros((Xshape[0], 1))
for ii in range(Xshape[0]):
Y[ii] = explore_cost_function(X[ii,0], X[ii,1]) + random.normalvariate(0, 0.5)
# Create BiFM explorer objects for each sampling strategy
random_sampling_explorer = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value, true_g.obstacles)
maxvar_sampling_explorer = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value, true_g.obstacles)
def plot_current_path(self, nexttarget):
c_best_cost = calc_true_path_cost(
bfm_explorer.GP_cost_function,
np.array(self.fm_sampling_explorer.fbFM.path),
GPm=self.GPm,
max_depth=self.max_depth,
mean_depth=self.mean_depth,
)
if not self.best_cost_plot:
self.best_cost_plot, barlims = fm_plottools.draw_grid(
self.ax[0],
self.true_g,
path=self.best_path,
min_cost=0.0,
max_cost=self.max_depth,
alpha=self.cost_alpha,
)
self.ax[0].text(
self.true_g.left,
self.true_g.bottom - 10,
"Best path cost = {0:0.3f}".format(self.best_path_cost),
color="w",
)
self.text1 = self.ax[1].text(
self.true_g.left, self.true_g.bottom - 10, "Current path cost = {0:0.3f}".format(c_best_cost), color="w"
)
self.est_cost_plot, barlims = fm_plottools.draw_grid(
self.ax[1],
self.fm_sampling_explorer.GP_cost_graph,
self.fm_sampling_explorer.fbFM.path,
max_cost=self.max_depth,
min_cost=0.0,
alpha=self.cost_alpha,
)
self.est_cost_plot.append(
self.ax[1].plot(self.fm_sampling_explorer.X[:, 0], self.fm_sampling_explorer.X[:, 1], "rx")[0]
)
self.est_cost_plot.append(self.ax[1].plot(nexttarget[0], nexttarget[1], "bo")[0])
self.var_plot, barlims = fm_plottools.draw_grid(
self.ax[2],
self.fm_sampling_explorer.GP_cost_graph,
min_cost=0.0,
alpha=self.cost_alpha,
cost_fun=lambda (x): self.fm_sampling_explorer.GP_cost_graph.var_fun(x[0], x[1]),
)
self.var_plot.append(self.ax[2].plot(self.sample_locations[:, 0], self.sample_locations[:, 1], "k.")[0])
self.ep = self.est_cost_plot[0].get_array()
self.ev = self.var_plot[0].get_array()
for axx in self.ax:
axx.set_xlim([self.true_g.left - 20, self.true_g.right + 20])
axx.set_ylim([self.true_g.bottom - 20, self.true_g.top + 20])
else:
self.text1.set_text("Current path cost = {0:0.3f}".format(c_best_cost))
# Array is transposed!
for locx, locy in self.fm_sampling_explorer.cmodel.cost_dict:
self.ep[
locy - self.true_g.bottom, locx - self.true_g.left
] = self.fm_sampling_explorer.cmodel.cost_dict[(locx, locy)]
self.ev[locy - self.true_g.bottom, locx - self.true_g.left] = self.fm_sampling_explorer.cmodel.var_dict[
(locx, locy)
]
self.est_cost_plot[0].set_array(self.ep)
xp, yp = zip(*self.fm_sampling_explorer.fbFM.path)
self.est_cost_plot[1].set_data(xp, yp)
self.est_cost_plot[2].set_data(xp[0], yp[0])
self.est_cost_plot[3].set_data(xp[-1], yp[-1])
self.est_cost_plot[4].set_data(self.fm_sampling_explorer.X[:, 0], self.fm_sampling_explorer.X[:, 1])
self.est_cost_plot[5].set_data(nexttarget[0], nexttarget[1])
self.var_plot[0].set_array(self.ev)
self.var_plot[1].set_data(self.sample_locations[:, 0], self.sample_locations[:, 1])
try:
self.fig.savefig(
"fm_explorer_{0}_S{1:02d}.pdf".format(self.nowstr, self.num_visited - 1), bbox_inches="tight"
)
except:
print "Save figure failed, continuing search."
m = GPy.models.GPRegression(X,Y-mean_value,ker) m.kern.lengthscale = 15 m.kern.variance = 20 m.Gaussian_noise.variance = 1 Xtemp, Ytemp = np.mgrid[0:100, 0:100] Xfull = np.vstack([Xtemp.ravel(), Ytemp.ravel()]).transpose() cmodel = mat_cost_function(GPg) Yfull, varYfull = m.predict(Xfull) Yfull += mean_value cmodel.mat = np.reshape(Yfull, (100, 100)) GPg.cost_fun = cmodel.calc_cost estimate_frames=[] fm_plottools.draw_grid(ax1[0][0], true_g, [], 19) graph_frame, barlims = fm_plottools.draw_grid(ax1[0][1], GPg, [], 19) graph_frame.append(ax1[0][1].plot(X[:,0], X[:,1], 'rx')[0]) #cbar = fig1.colorbar(graph_frame[0]) fbFM = fast_marcher.FullBiFastMarcher(GPg) fbFM.set_start(start_node) fbFM.set_goal(end_node) # Initial search fbFM.search() fbFM.pull_path() #figfbFM, axfbFM = fm_plottools.init_fig() fbFM_frames = [] tempframe, tempbar = fm_plottools.draw_fbfmcost(ax1[1][1], GPg, fbFM.path_cost, fbFM.path, 1000, 1400) tempframe.extend(graph_frame)
if "GP_model" not in locals():
V_Ireland = np.array([[0, 0], [-43, -38], [-70, -94], [-60, -150], [0, -180], [54, -152], [85, -70], [0, 0]])
start_node = (0, -2)
goal_node = (-30, -150)
model_file = os.path.expanduser("~") + "/catkin_ws/src/ros_lutra/data/IrelandLnModel.pkl"
fh = open(model_file, "rb")
GP_model = pickle.load(fh)
mean_depth = pickle.load(fh)
fh.close()
op_region = OperatingRegion(V_Ireland, start_node, goal_node)
true_g = fm_graphtools.CostmapGridFixedObs(
op_region.width, op_region.height, obstacles=op_region.obstacles, bl_corner=[op_region.left, op_region.bottom]
)
explorer_cost = bfm_explorer.mat_cost_function_GP(true_g, GP_cost_function, max_depth=4.5, mean_depth=mean_depth)
true_g.cost_fun = explorer_cost.calc_cost
tFM = fast_marcher.FullBiFastMarcher(true_g)
tFM.set_goal(goal_node)
tFM.set_start(start_node)
tFM.search()
tFM.pull_path()
f0, a0 = fm_plottools.init_fig()
f1, a1 = fm_plottools.init_fig()
f2, a2 = fm_plottools.init_fig()
fm_plottools.draw_grid(a0, true_g, tFM.path)
fm_plottools.draw_costmap(a1, true_g, tFM.FastMarcherSG.cost_to_come, tFM.path)
fm_plottools.draw_fbfmcost(a2, true_g, tFM.path_cost, tFM.path)
plt.show()
print "Performing FM search..."
FM = fast_marcher.FastMarcher(g)
FM.set_start(start_node)
FM.set_goal(end_node)
t0 = time.time()
FM.search()
t_searchFM = time.time()-t0
FM.find_corridor()
t_corridorFM = time.time()-t0-t_searchFM
print "Done. Search took {0}s, extracting corridor took {1}s".format(t_searchFM, t_corridorFM)
FM.pull_path()
f0,a0 = fm_plottools.init_fig()
fm_plottools.draw_grid(a0,g,path=FM.path)
print "Performing biFM search..."
bFM = fast_marcher.BiFastMarcher(g)
bFM.set_start(start_node)
bFM.set_goal(end_node)
t0 = time.time()
bFM.search()
t_searchbFM = time.time()-t0
bFM.find_corridor()
t_corridorbFM = time.time()-t0-t_searchbFM
print "Done. Search took {0}s, extracting corridor took {1}s".format(t_searchbFM, t_corridorbFM)
print "Performing full BiFM search..."
end_node = (48,48) #'''
while end_node in g.obstacles:
end_node = (end_node[0]-1, end_node[1])
models = [fast_marcher.FastMarcher(g), fast_marcher.FastMarcher(g), fast_marcher.BiFastMarcher(g), fast_marcher.FullBiFastMarcher(g), fast_marcher.FullBiFastMarcher(g)]
nmethods = len(METHOD_NAMES)
for model in models:
model.set_start(start_node)
model.set_goal(end_node)
model.search()
model.pull_path()
f0,a0 = plt.subplots(1,nmethods)
for i in range(nmethods):
fm_plottools.draw_grid(a0[i], g, path=models[i].path)
poly_cost = fm_graphtools.polynomial_precompute_cost_modifier(g, 12, min_val=0.001)
cost_update =poly_cost.set_update(10, 40, -1)
ug = g.copy()
ug.add_delta_costs(cost_update)
uFM = fast_marcher.FastMarcher(ug)
uFM.set_start(start_node)
uFM.set_goal(end_node)
models[0] = uFM
models[0].search()
models[0].pull_path()
models[1].update(cost_update)
models[1].pull_path()
