def fast_marching_method(graph,start):
h = 1
def calculus_distance(node,graph,weights):
neighbours = graph.get_neighbours(node);
if 'y-1' in neighbours :
if 'y+1' in neighbours:
x1 = min(weights[neighbours['y-1']],weights[neighbours['y+1']]);
else :
x1 = weights[neighbours['y-1']];
else :
if 'y+1' in neighbours:
x1 = weights[neighbours['y+1']];
if 'x-1' in neighbours:
if 'x+1' in neighbours:
x2 = min(weights[neighbours['x-1']],weights[neighbours['x+1']]);
else :
x2 = weights[neighbours['x-1']];
else :
if 'x+1' in neighbours:
x2 = weights[neighbours['x+1']];
if 2*h**2-(x1-x2)**2>=0:
return (x1+x2+(2*h**2-(x1-x2)**2)**0.5)/2
else:
return min(x1,x2)+h
frontier = PriorityQueue();
weights = graph.distances;
explored = []
goals = numpy.where(graph.indicator_map==2)
goals_x = goals[0]
goals_y = goals[1]
for i in range(goals_x.size):
frontier.append([0,(goals_x[i],goals_y[i])])
weights[(goals_x[i],goals_y[i])] = 0
while frontier:
node = frontier.pop();
explored.append(node[1])
#if node[1]==start:
# return weights
neighbours = graph.get_neighbours(node[1]);
for neighbour in neighbours.itervalues():
if neighbour not in explored and graph.indicator_map[neighbour]:
if not neighbour in frontier:
frontier.append([calculus_distance(neighbour,graph,weights),neighbour])
weights[neighbour]=calculus_distance(neighbour,graph,weights)
elif weights[neighbour] > calculus_distance(neighbour,graph,weights):
frontier[neighbour][0]=calculus_distance(neighbour,graph,weights)
weights[neighbour]=calculus_distance(neighbour,graph,weights)
graph.distances = weights
def find_enevelope_a_star(
trajectory,
actions,
trans_func,
lat_res,
lng_res,
# g_fn=lambda a: a_cost(a),
g_fn=lambda p1, p2, lat_res, lng_res: heuristic_l2(
p1, p2, lat_res, lng_res),
h_fn=lambda p1, p2, lat_res, lng_res: heuristic_l2(
p1, p2, lat_res, lng_res),
cost_ubound=1.,
debug=True):
s_list, a_list = trajectory
start, goal = tuple(s_list[0]), tuple(s_list[-1])
expert_cost = path_cost(s_list, lambda x, y: g_fn(x, y, lat_res, lng_res)
) #np.sum(g_fn(a) for a in a_list[:-1])
# expert_cost = np.sum(g_fn(a) for a in a_list[:-1])
cost_max = cost_ubound * expert_cost
frontier = PriorityQueue()
frontier.append((h_fn(start, goal, lat_res, lng_res), 0, start))
explored = defaultdict(lambda: False)
cost = defaultdict(lambda: np.float("inf"))
envelope = []
while frontier.size():
f, g, s = frontier.pop()
# if debug: print("{:.2f} ({:8d}), ".format(f, frontier.size()), end="")
explored[s] = True
envelope.append(s)
if s == goal:
continue
for a in actions:
sp = trans_func(
s, a) #geolife.trans_func(NavigationWorldState(*s), a)
if not explored[sp] and sp not in frontier:
# g_new = g + g_fn(a)
g_new = g + g_fn(s, sp, lat_res, lng_res)
f_new = g_new + h_fn(sp, goal, lat_res, lng_res)
# print(g_new, f_new)
if g_new < cost[sp] and f_new <= cost_max:
frontier.append((f_new, g_new, sp))
cost[sp] = g_new
return envelope, expert_cost
def best_first_graph_search(problem, f):
node = Node(problem.s_start)
frontier = PriorityQueue(f) #Priority Queue
frontier.append(node)
closed_list = set()
while frontier:
node = frontier.pop()
if problem.is_goal(node.state):
return node.solution()
closed_list.add(node.state)
for child in node.expand(problem):
if child.state not in closed_list and child not in frontier:
frontier.append(child)
elif child in frontier and f(child) < frontier[child]:
del frontier[child]
frontier.append(child)
return None
def best_first_graph_search(s1, s2, f):
node = s1
frontier = PriorityQueue(f) # Priority Queue
frontier.append(node)
log = []
closed_list = set()
while frontier:
node = frontier.pop()
log.append(node)
if node.index == s2.index:
print(node.path(), node.cost, len(closed_list))
return node.cost
closed_list.add(node.index)
links = node.expand()
for child in links:
is_in_Frontier = child not in frontier
if child.index not in closed_list and is_in_Frontier:
frontier.append(child)
elif not is_in_Frontier and f(child) < frontier[child]:
del frontier[child]
frontier.append(child)
return []
class BranchNBound:
"""Class to implement Branch and Bound algorithm"""
def __init__(self, dist_matrix, num_city, time_limit):
self.cost_matrix = np.asarray(dist_matrix, dtype='float')
self.n = num_city
self.best_path, self.upper_bound = initBestPath(self.cost_matrix)
print('The initialized path has a cost of ' + str(self.upper_bound))
# self.start_time = int(round(time.time() * 1000))
self.time_limit = time_limit * 1000 # time limit in millisec
# print('self.time_limit = ' + str(self.time_limit))
self.best_soln_quality = 0.0
self.preprocess_cost_matrix() # Set diagonal elements to infinity
reduced_matrix = np.copy(self.cost_matrix)
cost, reduced_matrix = reduce_matrix(reduced_matrix)
self.pq = PriorityQueue()
visited = set()
visited.add(0)
root = Node(reduced_matrix, cost, [0], visited)
root_node = root.get_cost(), root
self.pq.append(root_node)
def preprocess_cost_matrix(self):
"""Set diagonal elements of distance matrix to be infinity"""
np.fill_diagonal(self.cost_matrix, float('inf'))
def run_branch_and_bound(self):
"""Body of branch and bound, return the best solution within time limit."""
start_time = int(round(time.time() * 1000))
print('The start time is ' + str(start_time) + '. The time limit is ' + str(self.time_limit))
duration = 0
while not self.pq.is_empty() and duration < self.time_limit:
_, content = self.pq.pop()
cost_so_far = content.get_cost()
if cost_so_far < self.upper_bound:
path_so_far = content.get_path_so_far()
curr_node_idx = path_so_far[-1] # the node to be expanded
reduced_matrix = content.get_reduced_mat()
visited = content.get_visited()
neighbors = [i for i in range(self.n) if i not in visited]
# A solution is found
if np.all(reduced_matrix == float('inf')):
print('A solution is found.')
if cost_so_far < self.upper_bound:
self.upper_bound = cost_so_far
print(cost_so_far)
self.best_path = path_so_far
self.best_soln_quality = duration
# Branch
for next_idx in neighbors:
reduced_mat_copy = np.copy(reduced_matrix)
path_copy = copy.deepcopy(path_so_far)
visited_copy = copy.deepcopy(visited)
set_row_col_inf(reduced_mat_copy, curr_node_idx, next_idx)
reduced_mat_copy[next_idx, 0] = float('inf') # cannot go back to start point
cost, new_reduced_mat = reduce_matrix(reduced_mat_copy)
new_cost = cost_so_far + cost + reduced_matrix[curr_node_idx, next_idx]
# print('The new_cost is ' + str(new_cost))
# Bound
if new_cost < self.upper_bound:
path_copy.append(next_idx)
visited_copy.add(next_idx)
content = Node(new_reduced_mat, new_cost, path_copy, visited_copy)
next_node = new_cost, content
self.pq.append(next_node)
# Update timer
curr_time = int(round(time.time() * 1000))
duration = curr_time - start_time
self.best_path.append(0)
return self.best_path, self.upper_bound, self.best_soln_quality/1000.0