# Create search for true field tFM = fast_marcher.FullBiFastMarcher(true_g) tFM.set_start(start_node) tFM.set_goal(end_node) t0 = time.time() tFM.search() tFM.pull_path() 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 field") ax1[1][0].set_title("True path cost") ax1[1][1].set_title("Estimated path cost") fm_plottools.draw_fbfmcost(ax1[1][0], true_g, tFM.path_cost, tFM.path, 1000, 1400) # Create initial GP mean_value = 3; GPg = copy.copy(true_g) #X = 100.0*np.random.rand(100,2) if PLOT_UPDATES: X = np.array([[3, 3], [14, 14], [14, 38], [38, 14], [38, 38], [62, 14], [2, 62], [86, 14], [26, 74], [74, 38]]) else: X = np.array([[3, 3]]) 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) # define kernel ker = GPy.kern.RBF(2)
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()