def test_nav(self, coords, nav, r=0.05, t=100, sigma=0.1, preproc={}):
# random initial position and heading
# near the first location of the route
if sigma:
h = np.random.randint(0, 360)
x = coords['x']
x = np.random.normal(x, sigma)
y = coords['y']
y = np.random.normal(y, sigma)
xy = (x, y)
else:
xy = (coords['x'], coords['y'])
h = coords['yaw']
# Place agent to the initial position and render the image
img = self.get_img(xy, h)
img = pre_process(img, preproc)
# initialise the log variables
headings = []
traj = np.empty((2, t))
# traj[0, 0] = xy[0]
# traj[1, 0] = xy[1]
# Navigation loop
for i in range(0, t):
# log the coordinates and attitude
traj[0, i] = xy[0]
traj[1, i] = xy[1]
headings.append(h)
# get the new heading from teh navigator and format it properly
h = nav.get_heading(img)
h = headings[-1] + h
h = squash_deg(h)
# reposition the agent and get the new image
xy, img = self.update_position(xy, h, r)
img = pre_process(img, preproc)
headings = np.array(headings)
trajectory = {'x': traj[0], 'y': traj[1], 'heading': headings}
return trajectory, nav
from source.utils import load_route_naw, plot_route, seq_angular_error, animated_window, pre_process
from source import seqnav
import numpy as np
route_id = 1
path = '../new-antworld/exp1/route' + str(route_id) + '/'
# path = '../test_data/route'+ str(route_id) + '/'
route = load_route_naw(path, route_id=route_id, imgs=True, query=True, max_dist=0.2)
plot_route(route)
window = -20
matcher = 'corr'
sets = {'blur': True, 'shape': (180, 50), 'edge_range': (220, 240)}
route_imgs = pre_process(route['imgs'], sets)
test_imgs = pre_process(route['qimgs'], sets)
# nav = perfect_memory.PerfectMemory(route_imgs, matcher)
# recovered_heading = nav.navigate(test_imgs)
nav = seqnav.SequentialPerfectMemory(route_imgs, matcher, window=window)
recovered_heading, window_log = nav.navigate(test_imgs)
traj = {'x': route['qx'], 'y': route['qy'], 'heading': recovered_heading}
traj['heading'] = np.array(traj['heading'])
plot_route(route, traj)
errors, _ = seq_angular_error(route, traj)
print(np.mean(errors))
window_log = np.array(window_log)
def imgs_pre_proc(self, params):
if self.route_dict.get('qimgs'):
self.proc_qimgs = pre_process(self.route_dict['qimgs'], params)
self.proc_imgs = pre_process(self.route_dict['imgs'], params)
return self.proc_imgs
import source
from source.utils import load_route_naw, plot_route, pre_process, seq_angular_error
from source import seqnav
from source import antworld2 as aw
agent = aw.Agent(pitch_roll_sig=2)
route_id = 1
pre_processing = {'shape': (180, 50), 'edge_range': (220, 240)}
matcher = 'corr'
window = 20
# path = '../new-antworld/exp1/route' + str(route_id) + '/'
path = 'new-antworld/exp1/route' + str(route_id) + '/'
route = load_route_naw(path, route_id=route_id, imgs=True)
plot_route(route)
route_imgs = pre_process(route['imgs'], pre_processing)
nav = seqnav.SequentialPerfectMemory
# nav = perfect_memory.PerfectMemory
# set up the navigator
nav = nav(route_imgs, matcher, deg_range=(-180, 180), window=window)
coords = {'x': route['x'][0], 'y': route['y'][0], 'yaw': route['yaw'][0]}
traj, nav = agent.test_nav(coords,
nav,
t=20,
r=0.05,
sigma=None,
preproc=pre_processing)
#TODO: headings are also stored in the navigator class
plot_route(route, traj)
def worker_bench(route_ids, dist, routes_path, grid_path, shared, chunk):
log = {
'route_id': [],
'blur': [],
'edge': [],
'res': [],
'window': [],
'matcher': [],
'mean_error': [],
'seconds': [],
'errors': [],
'abs_index_diff': [],
'window_log': [],
'best_sims': [],
'dist_diff': [],
'tx': [],
'ty': [],
'th': []
}
# Go though all combinations in the chunk
for combo in chunk:
matcher = combo['matcher']
window = combo['window']
window_log = None
for route_id in route_ids: # for every route
route_path = routes_path + 'route' + str(route_id) + '/'
# _, test_x, test_y, test_imgs, route_x, route_y, \
# route_heading, route_imgs = load_route(route, dist)
route = Route(route_path,
route_id,
grid_path=grid_path,
max_dist=dist)
tic = time.perf_counter()
# Preprocess images
test_imgs = pre_process(route.get_qimgs(), combo)
route_imgs = pre_process(route.get_imgs(), combo)
# Run navigation algorithm
if window:
nav = spm.SequentialPerfectMemory(route_imgs,
matcher,
window=window)
recovered_heading, window_log = nav.navigate(test_imgs)
else:
nav = pm.PerfectMemory(route_imgs, matcher)
recovered_heading = nav.navigate(test_imgs)
toc = time.perf_counter()
# Get time complexity
time_compl = toc - tic
# Get the errors and the minimum distant index of the route memory
traj = {
'x': route['qx'],
'y': route['qy'],
'heading': recovered_heading
}
errors, min_dist_index = route.calc_errors(traj)
# Difference between matched index and minimum distance index and distance between points
matched_index = nav.get_index_log()
abs_index_diffs = np.absolute(
np.subtract(nav.get_index_log(), min_dist_index))
dist_diff = calc_dists(route.get_xycoords(), min_dist_index,
matched_index)
mean_route_error = np.mean(errors)
log['route_id'].extend([route_id])
log['blur'].extend([combo.get('blur')])
log['edge'].extend([combo.get('edge_range')])
log['res'].append(combo.get('shape'))
log['window'].extend([window])
log['matcher'].extend([matcher])
log['mean_error'].append(mean_route_error)
log['seconds'].append(time_compl)
log['window_log'].append(window_log)
log['best_sims'].append(nav.get_best_sims())
log['tx'].append(traj['x'].tolist())
log['ty'].append(traj['y'].tolist())
log['th'].append(traj['heading'])
log['abs_index_diff'].append(abs_index_diffs.tolist())
log['dist_diff'].append(dist_diff.tolist())
log['errors'].append(errors)
# Increment the complete jobs shared variable
shared['jobs'] = shared['jobs'] + 1
print(
multiprocessing.current_process(),
' jobs completed: {}/{}'.format(shared['jobs'],
shared['total_jobs']))
return log
def bench_singe_core(self, params, route_ids=None):
self._total_jobs(params)
# Get list of parameter keys
keys = [*params]
grid = itertools.product(*[params[k] for k in params])
# Go though all combinations in the grid
for combo in grid:
# create combo dictionary
combo_dict = {}
for i, k in enumerate(keys):
combo_dict[k] = combo[i]
matcher = combo_dict['matcher']
window = combo_dict['window']
window_log = None
path = '../new-antworld/'
for route_id in route_ids: # for every route
# TODO: In the future the code below (inside the loop) should all be moved inside the navigator class
route_path = '../new-antworld/route' + str(route_id) + '/'
# route = load_route_naw(route_path, route_id=route_id, imgs=True, query=True, max_dist=0.2)
# _, test_x, test_y, test_imgs, route_x, route_y, \
# route_heading, route_imgs = load_route(route, self.dist)
route = Route(route_path, route_id, grid_path=self.grid_path)
tic = time.perf_counter()
# Preprocess images
test_imgs = pre_process(route.get_qimgs(), combo_dict)
route_imgs = pre_process(route.get_imgs(), combo_dict)
# Run navigation algorithm
if window:
nav = spm.SequentialPerfectMemory(route_imgs,
matcher,
window=window)
recovered_heading, window_log = nav.navigate(test_imgs)
else:
nav = pm.PerfectMemory(route_imgs, matcher)
recovered_heading = nav.navigate(test_imgs)
toc = time.perf_counter()
# Get time complexity
time_compl = toc - tic
# Get the errors and the minimum distant index of the route memory
# errors, min_dist_index = degree_error(test_x, test_y, route_x, route_y, route_heading, recovered_heading)
traj = {
'x': route['qx'],
'y': route['qy'],
'heading': recovered_heading
}
errors, min_dist_index = route.calc_errors(traj)
# Difference between matched index and minimum distance index and distance between points
matched_index = nav.get_index_log()
abs_index_diffs = np.absolute(
np.subtract(nav.get_index_log(), min_dist_index))
dist_diff = calc_dists(route.get_xycoords(), min_dist_index,
matched_index)
mean_route_error = np.mean(errors)
self.log['route_id'].extend([route_id])
self.log['blur'].extend([combo_dict.get('blur')])
self.log['edge'].extend([combo_dict.get('edge_range')])
self.log['res'].append(combo_dict.get('shape'))
self.log['window'].extend([window])
self.log['matcher'].extend([matcher])
self.log['mean_error'].append(mean_route_error)
self.log['seconds'].append(time_compl)
self.log['window_log'].append(window_log)
self.log['best_sims'].append(nav.get_best_sims())
self.log['tx'].append(traj['x'].tolist())
self.log['ty'].append(traj['y'].tolist())
self.log['th'].append(traj['heading'])
self.log['abs_index_diff'].append(abs_index_diffs.tolist())
self.log['dist_diff'].append(dist_diff.tolist())
self.log['errors'].append(errors)
self.jobs += 1
print('Jobs completed: {}/{}'.format(self.jobs, self.total_jobs))
return self.log