def __init__(self, lat_deg, lon_deg, zoom, size, cell_size):
self.draw_mode = 'none'
self.running = True
self.current_model=-1
self.n_models=0
cv2.namedWindow('simulator')
cv2.setMouseCallback('simulator', self.click_callback)
signal.signal(signal.SIGINT, self.signal_handler)
self.data_pub = rospy.Publisher('/kriging_data', KrigInfo, latch=False, queue_size=1)
rospy.Subscriber("/fix", NavSatFix, self.gps_callback)
rospy.Subscriber('/request_scan', std_msgs.msg.String, self.scan_callback)
rospy.Service('/compare_model', CompareModels, self.model_comparison_cb)
print "Loading Satellite Image"
self.satellite = SatelliteImage(lat_deg, lon_deg, zoom, size)
self.grid = DataGrid('limits.coords', cell_size)
#self.load_groundtruth('Iains2.yaml')
#self.load_groundtruth('bottom_testing.yaml')
#self.load_groundtruth('upper_testing.yaml')
self.load_groundtruth('testing-5cm-intervals.yaml')
self.krieg_all_mmodels()
self.grid.calculate_mean_grid()
self.image = self.satellite.base_image.copy()
while(self.running):
cv2.imshow('simulator', self.image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
def __init__(self, lat_deg, lon_deg, zoom, size, cell_size):
self.draw_mode = 'none'
self.running = True
self.current_model=-1
self.n_models=0
cv2.namedWindow('explorer')
signal.signal(signal.SIGINT, self.signal_handler)
print "Loading Satellite Image"
self.satellite = SatelliteImage(lat_deg, lon_deg, zoom, size)
self.grid = DataGrid('limits.coords', cell_size)
rospy.loginfo("Subscribing to Krig Info")
rospy.Subscriber("/kriging_data", KrigInfo, self.data_callback)
self.image = self.satellite.base_image.copy()
while(self.running):
cv2.imshow('explorer', self.image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
def __init__(self, cell_size):
self.running = True
self.grid = DataGrid('limits.coords', cell_size)
self.grid2 = DataGrid('limits.coords', cell_size)
#self.load_groundtruth('Iains2.yaml')
#self.load_groundtruth('bottom_testing.yaml')
#self.load_groundtruth('upper_testing.yaml')
self.load_groundtruth()
self.krieg_all_mmodels()
self.grid.calculate_mean_grid()
self.grid2.calculate_mean_grid()
print self.model_comparison()
def __init__(self, lat_deg, lon_deg, zoom, size, cell_size):
self.running = True
signal.signal(signal.SIGINT, self.signal_handler)
print "Loading Satellite Image"
self.satellite = SatelliteImage(lat_deg, lon_deg, zoom, size)
self.grid = DataGrid('limits.coords', cell_size)
self.image = self.satellite.base_image.copy()
while (self.running):
cv2.imshow('image', self.image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
class simulator(object):
modes = {
'help': 'press \'h\' for help',
'none': ''
}
def __init__(self, lat_deg, lon_deg, zoom, size, cell_size):
self.draw_mode = 'none'
self.running = True
self.current_model=-1
self.n_models=0
cv2.namedWindow('simulator')
cv2.setMouseCallback('simulator', self.click_callback)
signal.signal(signal.SIGINT, self.signal_handler)
self.data_pub = rospy.Publisher('/kriging_data', KrigInfo, latch=False, queue_size=1)
rospy.Subscriber("/fix", NavSatFix, self.gps_callback)
rospy.Subscriber('/request_scan', std_msgs.msg.String, self.scan_callback)
rospy.Service('/compare_model', CompareModels, self.model_comparison_cb)
print "Loading Satellite Image"
self.satellite = SatelliteImage(lat_deg, lon_deg, zoom, size)
self.grid = DataGrid('limits.coords', cell_size)
#self.load_groundtruth('Iains2.yaml')
#self.load_groundtruth('bottom_testing.yaml')
#self.load_groundtruth('upper_testing.yaml')
self.load_groundtruth('testing-5cm-intervals.yaml')
self.krieg_all_mmodels()
self.grid.calculate_mean_grid()
self.image = self.satellite.base_image.copy()
while(self.running):
cv2.imshow('simulator', self.image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
def model_comparison_cb(self, req):
print req.model_name, req.model_index, req.height, req.width
compm = np.reshape(np.asarray(req.vals), (req.height, req.width))
if req.model_name == 'kriging':
diff = self.grid.models[req.model_index].output-compm
else :
diff = self.grid.mean_output-compm
return True, np.mean(diff), np.mean(diff**2), np.std(diff), np.var(diff)
def load_groundtruth(self, filename):
self.grid.load_data_from_yaml(filename)
self.vmin, self.vmax = self.grid.get_max_min_vals()
print "LIMS: " + str(self.vmin) + " " + str(self.vmax)
self.n_models=len(self.grid.models)
self.current_model=0
def krieg_all_mmodels(self):
for i in self.grid.models:
i.do_krigging()
def click_callback(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
click_coord = self.satellite._pix2coord(x,y)
cx, cy = self.grid.get_cell_inds_from_coords(click_coord)
if cx <0 or cy<0:
print "click outside the grid"
else:
hmm = KrigInfo()
hmm.header = std_msgs.msg.Header()
hmm.header.stamp = rospy.Time.now()
hmm.coordinates.latitude = click_coord.lat
hmm.coordinates.longitude = click_coord.lon
for i in self.grid.models:
mmh = KrigMsg()
mmh.model_name = i.name
mmh.measurement = i.output[cy][cx]
hmm.data.append(mmh)
self.data_pub.publish(hmm)
def gps_callback(self, data):
if not np.isnan(data.latitude):
self.last_coord=data
def scan_callback(self, msg):
if msg.data == 'Do_reading':
print "generating reading"
gps_coord = MapCoords(self.last_coord.latitude,self.last_coord.longitude)
cx, cy = self.grid.get_cell_inds_from_coords(gps_coord)
hmm = KrigInfo()
hmm.header = std_msgs.msg.Header()
hmm.header.stamp = rospy.Time.now()
hmm.coordinates = self.last_coord
# hmm.coordinates.longitude = click_coord.lon
for i in self.grid.models:
mmh = KrigMsg()
mmh.model_name = i.name
mmh.measurement = i.output[cy][cx]
hmm.data.append(mmh)
self.data_pub.publish(hmm)
print hmm
def _change_mode(self, k):
if k == 27:
self.running = False
elif k == ord('q'):
self.running = False
elif k == ord('c'):
print "Draw corners"
self.satellite.draw_coordinate(self.grid.swc,(0,255,0,128))
self.satellite.draw_coordinate(self.grid.sec,(0,0,255,128))
self.satellite.draw_coordinate(self.grid.nwc,(255,0,0,128))
self.satellite.draw_coordinate(self.grid.nec,(255,255,255,128))
self.image = self.satellite.base_image.copy()
elif k == ord('i'):
self.draw_mode='inputs'
self.draw_inputs(self.current_model)
elif k == ord('t'):
self.draw_mode='krigging'
self.draw_krigged(self.current_model)
elif k == ord('v'):
self.draw_mode='sigma'
self.draw_deviation(self.current_model)
elif k == ord('>'):
self.current_model+=1
if self.current_model >= self.n_models:
self.current_model=0
if self.draw_mode=='inputs':
self.draw_inputs(self.current_model)
if self.draw_mode=='krigging':
self.draw_krigged(self.current_model)
if self.draw_mode=='sigma':
self.draw_deviation(self.current_model)
elif k == ord('<'):
self.current_model-=1
if self.current_model < 0:
self.current_model=self.n_models-1
if self.draw_mode=='inputs':
self.draw_inputs(self.current_model)
if self.draw_mode=='krigging':
self.draw_krigged(self.current_model)
if self.draw_mode=='sigma':
self.draw_deviation(self.current_model)
elif k == ord('l'):
print "Draw Limits"
#self.satellite.draw_list_of_coords(self.kriged.limits, (0,0,255,128))
self.satellite.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
self.image = self.satellite.base_image.copy()
elif k == ord('g'):
print "Draw Grid"
print "Grid Shape: ", self.grid.shape
self.satellite.draw_grid(self.grid.cells, self.grid.cell_size, (64,64,64,64), thickness=1)
# print len(self.kriged.waypoints), len(self.kriged.waypoints[0])
# for i in range(0, len(self.kriged.waypoints)):
# self.satellite.draw_list_of_coords(self.kriged.waypoints[i], (128,0,128,64), size=2, thickness=-1)
self.image = self.satellite.base_image.copy()
#
# vmin = np.floor(self.grid.data[0].lims[0])
# vmax = np.ceil(self.grid.data[0].lims[1])
# print vmin, vmax
#
# norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
# cmap = cm.jet
# colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
#
# for i in range(self.grid.data[0].shape[0]):
# for j in range(self.grid.data[0].shape[1]):
# cell = self.grid.cells[i][j]
# a= colmap.to_rgba(int(self.grid.data[0].output[i][j]))
# b= (int(a[0]*255), int(a[1]*255), int(a[2]*255), int(a[3]*50))
# self.satellite.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
#
# self.satellite.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
# #self.satellite.draw_grid(self.grid.cells, self.grid.cell_size, (64,64,64,64), thickness=1)
# self.image = self.satellite.base_image.copy()
def draw_deviation(self, nm):
font = cv2.FONT_HERSHEY_SIMPLEX
norm = mpl.colors.Normalize(vmin=0, vmax=100)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
# vmin = np.floor(self.grid.data[0].lims[0])
# vmax = np.ceil(self.grid.data[0].lims[1])
# norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
# cmap = cm.jet
# colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in range(self.grid.models[nm].shape[0]):
for j in range(self.grid.models[nm].shape[1]):
cell = self.grid.cells[i][j]
a= colmap.to_rgba(int(self.grid.models[nm].sigmapercent[i][j]*100))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
self.satellite.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
self.satellite.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
self.image = self.satellite.base_image.copy()
cv2.putText(self.image, self.grid.models[nm].name, (int(520), int(20)), font, 0.8, (200, 200, 200), 2)
self.draw_legend(self.vmin, self.vmax)
def draw_krigged(self, nm):
font = cv2.FONT_HERSHEY_SIMPLEX
norm = mpl.colors.Normalize(vmin=self.vmin, vmax=self.vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
# vmin = np.floor(self.grid.data[0].lims[0])
# vmax = np.ceil(self.grid.data[0].lims[1])
# norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
# cmap = cm.jet
# colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in range(self.grid.models[nm].shape[0]):
for j in range(self.grid.models[nm].shape[1]):
cell = self.grid.cells[i][j]
a= colmap.to_rgba(int(self.grid.models[nm].output[i][j]))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
self.satellite.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
self.satellite.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
self.image = self.satellite.base_image.copy()
cv2.putText(self.image, self.grid.models[nm].name, (int(520), int(20)), font, 0.8, (200, 200, 200), 2)
self.draw_legend(self.vmin, self.vmax)
def draw_inputs(self, nm):
# vmin = np.floor(self.grid.models[nm].lims[0])
# vmax = np.ceil(self.grid.models[nm].lims[1])
# print vmin, vmax
#
font = cv2.FONT_HERSHEY_SIMPLEX
norm = mpl.colors.Normalize(vmin=self.vmin, vmax=self.vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
#
for i in self.grid.models[nm].orig_data:
cell = self.grid.cells[i.y][i.x]
# #print i.value
a= colmap.to_rgba(int(i.value))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
# #print a, b
self.satellite.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
self.image = self.satellite.base_image.copy()
cv2.putText(self.image, self.grid.models[nm].name, (int(520), int(20)), font, 0.8, (200, 200, 200), 2)
self.draw_legend(self.vmin, self.vmax)
def draw_legend(self, vmin, vmax):
font = cv2.FONT_HERSHEY_SIMPLEX
step = (vmax - vmin)/(600-40)
norm = mpl.colors.Normalize(vmin=self.vmin, vmax=self.vmax)
cmap = cm.jet
# cmap = cm.plasma
# cmap = cm.gnuplot
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
vp = range(int(np.floor(vmin)),int(np.ceil(vmax)), int(np.ceil(step)))
print len(vp)
if step>1.0:
ind = 0
while ind < 560:
# print int(vmin+(ind*step))
a= colmap.to_rgba(int(vmin+(ind*step)))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
cv2.rectangle(self.image, (int(ind+40), int(580)), (int(ind+1+40), int(600)), b , thickness=-1)
ind+=1
# cv2.rectangle(self.image, (int(40), int(580)), (int(600), int(600)), (200,0,0,0), thickness=-1)
a= colmap.to_rgba(int(vmin))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
cv2.putText(self.image, str(self.vmin) + " Kpa", (int(5), int(575)), font, 0.6, b, 2)
a= colmap.to_rgba(int(vmax))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
cv2.putText(self.image, str(self.vmax) + " Kpa", (int(520), int(575)), font, 0.6, b, 2)
def signal_handler(self, signal, frame):
cv2.destroyAllWindows()
print('You pressed Ctrl+C!')
sys.exit(0)
def __init__(self, lat_deg, lon_deg, zoom, size, args):
self.targets = []
self.results = []
self.result_counter = 0
self.explodist = 0
self.running = True
self.last_coord = None
signal.signal(signal.SIGINT, self.signal_handler)
self.expid = args.experiment_name
print "Creating visualiser object"
super(Explorator, self).__init__(lat_deg, lon_deg, zoom, size)
cv2.namedWindow('explorator')
cv2.setMouseCallback('explorator', self.click_callback)
self.current_model = -1
self.draw_mode = 'none'
self.grid = DataGrid(args.limits_file, args.cell_size)
self.topo_map = TopoMap(self.grid)
self.visited_wp = []
explo_type = args.area_coverage_type
self.define_exploration_type(explo_type)
self.navigating = False
self.pause_exp = False
self.exploring = 0
self.n_inputs = 0
print "NUMBER OF TARGETS:"
print len(self.explo_plan.targets)
self.limits_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.grid_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.exploration_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.gps_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.limits_canvas.draw_polygon(self.grid.limits, (0, 0, 255, 128),
thickness=1)
self.grid_canvas.draw_grid(self.grid.cells,
args.cell_size, (128, 128, 128, 2),
thickness=1)
self.redraw()
self.redraw_kriged = True
self.redraw_var = True
self.redraw_devi = True
self.model_canvas = []
self.model_legend = []
self.kriging_canvas = []
self.klegend_canvas = []
self.klegend2_canvas = []
self.klegend3_canvas = []
self.sigma_canvas = []
self.sigma2_canvas = []
self.model_canvas_names = []
self.mean_out_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_out_legend_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_var_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_var_legend_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_dev_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_dev_legend_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
rospy.loginfo("Subscribing to Krig Info")
rospy.Subscriber("/kriging_data", KrigInfo, self.data_callback)
rospy.Subscriber("/fix", NavSatFix, self.gps_callback)
rospy.Subscriber('/penetrometer_scan', std_msgs.msg.String,
self.scan_callback)
self.req_data_pub = rospy.Publisher('/request_scan',
std_msgs.msg.String,
latch=False,
queue_size=1)
rospy.loginfo(" ... Connecting to Open_nav")
self.open_nav_client = actionlib.SimpleActionClient(
'/open_nav', open_nav.msg.OpenNavAction)
self.open_nav_client.wait_for_server()
rospy.loginfo(" ... done")
tim1 = rospy.Timer(rospy.Duration(0.2), self.drawing_timer_callback)
tim2 = rospy.Timer(rospy.Duration(0.1), self.control_timer_callback)
self.refresh()
while (self.running):
cv2.imshow('explorator', self.show_image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
tim1.shutdown()
tim2.shutdown()
cv2.destroyAllWindows()
sys.exit(0)
class Explorator(KrigingVisualiser):
#_w_shape=[(0, 16), (1, 17), (3, 17), (5, 16), (8, 15), (10, 15), (12, 14), (14, 13), (12, 12), (10, 11), (8, 11), (5, 10), (8, 9), (10, 9), (12, 8), (14, 7), (12, 6), (10, 5), (8, 5), (6, 4), (4, 3), (3, 2), (4, 1), (5, 0), (7, 0)]
#_w_shape=[(17, 0), (17, 1), (17, 3), (16, 5), (15, 8), (15, 10), (14, 12), (13, 14), (12, 12), (11, 10), (11, 8), (10, 5), (9, 8), (9, 10), (8, 12), (7, 14), (6, 12), (5, 10), (5, 8), (4, 6), (3, 4), (2, 3), (1, 4), (0, 5), (0, 7)]
#_w_shape=[(17, 0), (17,1), (17, 2), (17, 4), (16, 4), (16, 6), (16, 8), (15, 8), (15, 10), (14, 10), (14, 12), (13, 12), (13, 14), (12, 14), (12, 12), (11, 12), (11, 10), (10, 10), (10, 8), (10, 6), (10, 4), (9, 4), (9, 6), (9, 8), (9, 10), (8, 10), (8, 12), (7, 12), (7, 14), (6, 14), (6, 12), (5, 12), (5, 10), (4, 10), (4, 8), (4, 6), (4, 4), (3, 4), (3, 3), (2, 3), (2, 4), (1,4), (1, 6), (0,6), (1, 8), (0,8), (1, 10), (0, 10), (0, 12), (0, 14)]
_w_shape = [(17, 0), (16, 1), (14, 6), (12, 11), (10, 14), (8, 9), (5, 14),
(3, 11), (2, 6), (0, 3)]
def __init__(self, lat_deg, lon_deg, zoom, size, args):
self.targets = []
self.results = []
self.result_counter = 0
self.explodist = 0
self.running = True
self.last_coord = None
signal.signal(signal.SIGINT, self.signal_handler)
self.expid = args.experiment_name
print "Creating visualiser object"
super(Explorator, self).__init__(lat_deg, lon_deg, zoom, size)
cv2.namedWindow('explorator')
cv2.setMouseCallback('explorator', self.click_callback)
self.current_model = -1
self.draw_mode = 'none'
self.grid = DataGrid(args.limits_file, args.cell_size)
self.topo_map = TopoMap(self.grid)
self.visited_wp = []
explo_type = args.area_coverage_type
self.define_exploration_type(explo_type)
self.navigating = False
self.pause_exp = False
self.exploring = 0
self.n_inputs = 0
print "NUMBER OF TARGETS:"
print len(self.explo_plan.targets)
self.limits_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.grid_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.exploration_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.gps_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.limits_canvas.draw_polygon(self.grid.limits, (0, 0, 255, 128),
thickness=1)
self.grid_canvas.draw_grid(self.grid.cells,
args.cell_size, (128, 128, 128, 2),
thickness=1)
self.redraw()
self.redraw_kriged = True
self.redraw_var = True
self.redraw_devi = True
self.model_canvas = []
self.model_legend = []
self.kriging_canvas = []
self.klegend_canvas = []
self.klegend2_canvas = []
self.klegend3_canvas = []
self.sigma_canvas = []
self.sigma2_canvas = []
self.model_canvas_names = []
self.mean_out_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_out_legend_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_var_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_var_legend_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_dev_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
self.mean_dev_legend_canvas = ViewerCanvas(self.base_image.shape,
self.satellite.centre,
self.satellite.res)
rospy.loginfo("Subscribing to Krig Info")
rospy.Subscriber("/kriging_data", KrigInfo, self.data_callback)
rospy.Subscriber("/fix", NavSatFix, self.gps_callback)
rospy.Subscriber('/penetrometer_scan', std_msgs.msg.String,
self.scan_callback)
self.req_data_pub = rospy.Publisher('/request_scan',
std_msgs.msg.String,
latch=False,
queue_size=1)
rospy.loginfo(" ... Connecting to Open_nav")
self.open_nav_client = actionlib.SimpleActionClient(
'/open_nav', open_nav.msg.OpenNavAction)
self.open_nav_client.wait_for_server()
rospy.loginfo(" ... done")
tim1 = rospy.Timer(rospy.Duration(0.2), self.drawing_timer_callback)
tim2 = rospy.Timer(rospy.Duration(0.1), self.control_timer_callback)
self.refresh()
while (self.running):
cv2.imshow('explorator', self.show_image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
tim1.shutdown()
tim2.shutdown()
cv2.destroyAllWindows()
sys.exit(0)
# EXPLORATION PARAMS HERE!!!!
def define_exploration_type(self, explo_type):
self.exploration_strategy = explo_type
self.n_goals = 10
if explo_type == 'area_split':
self.grid._split_area(3, 3)
sb = []
for i in self.grid.area_splits_coords:
(y, x) = self.grid.get_cell_inds_from_coords(i)
sb.append((x, y))
self.explo_plan = ExplorationPlan(self.topo_map,
args.initial_waypoint,
args.initial_percent,
ac_model=explo_type,
ac_coords=sb)
elif explo_type == 'random':
self.explo_plan = ExplorationPlan(self.topo_map,
args.initial_waypoint,
args.initial_percent)
elif explo_type == 'w_shape':
self.explo_plan = ExplorationPlan(self.topo_map,
args.initial_waypoint,
args.initial_percent,
ac_model=explo_type,
ac_coords=self._w_shape)
else: #greedy
self.explo_plan = ExplorationPlan(self.topo_map,
args.initial_waypoint,
args.initial_percent,
exploration_type='greedy',
ac_model=explo_type)
def drawing_timer_callback(self, event):
self.refresh()
def control_timer_callback(self, event):
if self.navigating:
if self.open_nav_client.simple_state == 2:
print "DONE NAVIGATING"
self.navigating = False
if self.exploring == 1:
self.exploring = 2
elif self.exploring == 2:
if not self.pause_exp:
self.explo_plan.explored_wp.append(
self.explo_plan.route.pop(0))
info_str = 'Do_reading'
self.req_data_pub.publish(info_str)
self.exploring = 3
elif self.exploring == 4:
if not self.pause_exp:
if len(self.explo_plan.route) > 0:
gg = self.explo_plan.route[0]
self.open_nav_client.cancel_goal()
targ = open_nav.msg.OpenNavActionGoal()
targ.goal.coords.header.stamp = rospy.Time.now()
targ.goal.coords.latitude = gg.coord.lat
targ.goal.coords.longitude = gg.coord.lon
print "Going TO: ", gg
self.exploring = 1
self.navigating = True
self.open_nav_client.send_goal(targ.goal)
else:
print "Done Exploring"
self.exploring = 0
# else:
# if self.exploring:
# print "waiting for new goal"
def gps_callback(self, data):
if not np.isnan(data.latitude):
self.gps_canvas.clear_image()
gps_coord = MapCoords(data.latitude, data.longitude)
self.gps_canvas.draw_coordinate(gps_coord,
'black',
size=2,
thickness=2,
alpha=255)
if self.last_coord:
dist = gps_coord - self.last_coord
self.explodist += dist[0]
self.last_coord = gps_coord
def data_callback(self, msg):
point_coord = kriging_exploration.map_coords.coord_from_satnav_fix(
msg.coordinates)
for i in msg.data:
self.grid.add_data_point(i.model_name, point_coord, i.measurement)
self.vmin, self.vmax = self.grid.get_max_min_vals()
self.n_models = len(self.grid.models)
for i in self.grid.models:
if i.name not in self.model_canvas_names:
print i.name
self.model_canvas_names.append(i.name)
self.model_canvas.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.model_legend.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.kriging_canvas.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.klegend_canvas.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.klegend2_canvas.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.klegend3_canvas.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.sigma_canvas.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.sigma2_canvas.append(
ViewerCanvas(self.base_image.shape, self.satellite.centre,
self.satellite.res))
self.draw_inputs(self.model_canvas_names.index(i.name))
self.n_inputs += 1
if self.exploring == 3:
if self.n_inputs > 3:
self.krieg_all_mmodels()
rospy.sleep(0.1)
self.grid.calculate_mean_grid()
rospy.sleep(0.1)
self.draw_means()
self.draw_mode = "means"
resp = self.get_errors()
self.result_counter += 1
d = {}
d['step'] = self.result_counter
d['id'] = self.expid
d['ns'] = len(self.explo_plan.targets)
d['coord'] = {}
d['coord']['lat'] = self.last_coord.lat
d['coord']['lon'] = self.last_coord.lon
d['dist'] = float(self.explodist)
d['results'] = {}
d['results']['groundtruth'] = resp
d['results']['var'] = {}
d['results']['var']['mean'] = {}
d['results']['var']['mean']['mean'] = float(
np.mean(self.grid.mean_variance))
d['results']['var']['mean']['max'] = float(
np.max(self.grid.mean_variance))
d['results']['var']['mean']['min'] = float(
np.min(self.grid.mean_variance))
# d['results']['var']['std']['mean']= np.mean(self.grid.mean_deviation)
# d['results']['var']['std']['max']= np.max(self.grid.mean_deviation)
# d['results']['var']['std']['min']= np.min(self.grid.mean_deviation)
means = []
maxs = []
mins = []
for i in range(self.n_models):
means.append(float(np.mean(self.grid.models[i].variance)))
maxs.append(float(np.max(self.grid.models[i].variance)))
mins.append(float(np.min(self.grid.models[i].variance)))
d['results']['models'] = {}
d['results']['models']['means'] = means
d['results']['models']['maxs'] = maxs
d['results']['models']['mins'] = mins
rospy.sleep(0.1)
self.results.append(d)
if self.exploration_strategy == 'greedy':
nwp = len(self.explo_plan.route) + len(
self.explo_plan.explored_wp)
print nwp, " nodes in plan"
if nwp <= self.n_goals:
#THIS IS the ONE
#self.explo_plan.add_limited_greedy_goal(self.grid.mean_variance, self.last_coord)
self.explo_plan.add_greedy_goal(
self.grid.mean_variance)
#self.explo_plan.add_montecarlo_goal(self.grid.mean_variance, self.last_coord)
#self.draw_mode="deviation"
# self.current_model=0
# if self.redraw_devi:
# self.draw_all_devs()
self.redraw()
rospy.sleep(0.1)
self.exploring = 4
def scan_callback(self, msg):
if msg.data == 'Reading':
print "GOT READING!!!"
cx, cy = self.grid.get_cell_inds_from_coords(self.last_coord)
if cx < 0 or cy < 0:
print "Reading outside the grid"
else:
print 'Reading at: ', cx, cy
for i in self.topo_map.waypoints:
if (cy, cx) == i.ind:
print 'Setting: ', i.name, i.coord, "as Visited"
i.visited = True
self.visited_wp.append(i)
self.grid_canvas.draw_waypoints(
self.topo_map.waypoints, (0, 255, 0, 2),
thickness=1)
self.grid_canvas.draw_waypoints(self.visited_wp,
(0, 0, 255, 2),
thickness=1)
self.redraw()
def refresh(self):
#self.show_image = self.image.copy()
#self.show_image = cv2.addWeighted(self.gps_canvas.image, 0.7, self.image, 1.0, 0)
#self.show_image = transparentOverlay(self.image, self.gps_canvas.image)
self.show_image = overlay_image_alpha(self.image,
self.gps_canvas.image)
def redraw(self):
self.image = cv2.addWeighted(self.grid_canvas.image, 0.5,
self.base_image, 1.0, 0)
self.image = cv2.addWeighted(self.limits_canvas.image, 0.75,
self.image, 1.0, 0)
self.image = cv2.addWeighted(self.exploration_canvas.image, 0.75,
self.image, 1.0, 0)
if self.draw_mode == "inputs" and self.current_model >= 0:
self.image = cv2.addWeighted(
self.model_canvas[self.current_model].image, 0.75, self.image,
1.0, 0)
self.image = overlay_image_alpha(
self.image, self.model_legend[self.current_model].image)
if self.draw_mode == "kriging": # and self.current_model>=0 :
self.image = cv2.addWeighted(
self.kriging_canvas[self.current_model].image, 0.75,
self.image, 1.0, 0)
#self.image = cv2.addWeighted(self.klegend_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
self.image = overlay_image_alpha(
self.image, self.klegend_canvas[self.current_model].image)
if self.draw_mode == "deviation": # and self.current_model>=0 :
self.image = cv2.addWeighted(
self.sigma_canvas[self.current_model].image, 0.75, self.image,
1.0, 0)
#self.image = cv2.addWeighted(self.klegend3_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
self.image = overlay_image_alpha(
self.image, self.klegend3_canvas[self.current_model].image)
if self.draw_mode == "variance": # and self.current_model>=0 :
self.image = cv2.addWeighted(
self.sigma2_canvas[self.current_model].image, 0.75, self.image,
1.0, 0)
#self.image = cv2.addWeighted(self.klegend2_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
self.image = overlay_image_alpha(
self.image, self.klegend2_canvas[self.current_model].image)
if self.draw_mode == "means":
self.image = cv2.addWeighted(self.mean_dev_canvas.image, 0.75,
self.image, 1.0, 0)
#self.image = cv2.addWeighted(self.klegend2_canvas[self.current_model].image, 1.0, self.image, 1.0, 0)
self.image = overlay_image_alpha(self.image,
self.mean_dev_legend_canvas.image)
self.show_image = self.image.copy()
def click_callback(self, event, x, y, flags, param):
if event == cv2.EVENT_RBUTTONDOWN:
click_coord = self.satellite._pix2coord(x, y)
cx, cy = self.grid.get_cell_inds_from_coords(click_coord)
if cx < 0 or cy < 0:
print "click outside the grid"
else:
print cx, cy
for i in self.topo_map.waypoints:
if (cy, cx) == i.ind:
print i.name, i.coord.easting, i.coord.northing
i.visited = True
self.visited_wp.append(i)
self.grid_canvas.draw_waypoints(self.topo_map.waypoints,
(0, 255, 0, 2),
thickness=1)
self.grid_canvas.draw_waypoints(self.visited_wp,
(0, 0, 255, 2),
thickness=1)
self.redraw()
if event == cv2.EVENT_LBUTTONDOWN:
click_coord = self.satellite._pix2coord(x, y)
cx, cy = self.grid.get_cell_inds_from_coords(click_coord)
if cx < 0 or cy < 0:
print "click outside the grid"
else:
print cx, cy
for i in self.topo_map.waypoints:
if (cy, cx) == i.ind:
self.open_nav_client.cancel_goal()
targ = open_nav.msg.OpenNavActionGoal()
#goal.goal.goal.header.
targ.goal.coords.header.stamp = rospy.Time.now()
targ.goal.coords.latitude = i.coord.lat
targ.goal.coords.longitude = i.coord.lon
print targ
self.navigating = True
self.open_nav_client.send_goal(targ.goal)
#self.client.wait_for_result()
# Prints out the result of executing the action
#ps = self.client.get_result()
#print ps
def draw_inputs(self, nm):
minv = self.grid.models[nm].lims[0]
maxv = self.grid.models[nm].lims[1]
if (maxv - minv) <= 1:
maxv = maxv + 50
minv = minv - 50
norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
self.model_canvas[nm].clear_image()
self.model_legend[nm].clear_image()
for i in self.grid.models[nm].orig_data:
cell = self.grid.cells[i.y][i.x]
a = colmap.to_rgba(int(i.value))
b = (int(a[2] * 255), int(a[1] * 255), int(a[0] * 255),
int(a[3] * 50))
self.model_canvas[nm].draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
self.model_canvas[nm].put_text(self.grid.models[nm].name)
self.model_legend[nm].put_text(self.grid.models[nm].name)
self.model_legend[nm].draw_legend(minv, maxv, colmap, title="Kriging")
def draw_krigged(self, nm):
print "drawing kriging" + str(nm)
minv = self.grid.models[nm].min_val
maxv = self.grid.models[nm].max_val
if (maxv - minv) <= 1:
maxv = maxv + 50
minv = minv - 50
norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
self.kriging_canvas[nm].clear_image()
self.klegend_canvas[nm].clear_image()
for i in range(self.grid.models[nm].shape[0]):
for j in range(self.grid.models[nm].shape[1]):
cell = self.grid.cells[i][j]
a = colmap.to_rgba(int(self.grid.models[nm].output[i][j]))
b = (int(a[2] * 255), int(a[1] * 255), int(a[0] * 255),
int(a[3] * 50))
self.kriging_canvas[nm].draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
self.klegend_canvas[nm].put_text(self.grid.models[nm].name)
self.klegend_canvas[nm].draw_legend(minv,
maxv,
colmap,
title="Kriging")
self.redraw()
def draw_variance(self, nm):
print "drawing variance" + str(nm)
minv = self.grid.models[nm].min_var
maxv = self.grid.models[nm].max_var
if (maxv - minv) <= 1:
maxv = maxv + 50
minv = minv - 50
norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
self.sigma_canvas[nm].clear_image()
self.klegend2_canvas[nm].clear_image()
for i in range(self.grid.models[nm].shape[0]):
for j in range(self.grid.models[nm].shape[1]):
cell = self.grid.cells[i][j]
a = colmap.to_rgba(int(self.grid.models[nm].variance[i][j]))
b = (int(a[2] * 255), int(a[1] * 255), int(a[0] * 255),
int(a[3] * 50))
self.sigma2_canvas[nm].draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
self.klegend2_canvas[nm].put_text(self.grid.models[nm].name)
self.klegend2_canvas[nm].draw_legend(minv,
maxv,
colmap,
title="Variance")
self.redraw()
def draw_means(self):
print "drawing mean deviation ..."
minv = self.grid.min_mean_deviation
maxv = self.grid.max_mean_deviation
if (maxv - minv) <= 1:
maxv = maxv + 50
minv = minv - 50
norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
self.mean_dev_canvas.clear_image()
self.mean_dev_legend_canvas.clear_image()
for i in range(self.grid.shape[0]):
for j in range(self.grid.shape[1]):
cell = self.grid.cells[i][j]
a = colmap.to_rgba(int(self.grid.mean_deviation[i][j]))
b = (int(a[2] * 255), int(a[1] * 255), int(a[0] * 255),
int(a[3] * 50))
self.mean_dev_canvas.draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
#self.mean_dev_legend_canvas.put_text(self.grid.models[nm].name)
self.mean_dev_legend_canvas.draw_legend(minv,
maxv,
colmap,
title="Mean Deviation")
#self.draw_mode="means"
self.redraw()
def draw_deviation(self, nm):
print "drawing deviation" + str(nm)
minv = self.grid.models[nm].min_dev
maxv = self.grid.models[nm].max_dev
if (maxv - minv) <= 1:
maxv = maxv + 50
minv = minv - 50
norm = mpl.colors.Normalize(vmin=minv, vmax=maxv)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
self.sigma_canvas[nm].clear_image()
self.klegend3_canvas[nm].clear_image()
for i in range(self.grid.models[nm].shape[0]):
for j in range(self.grid.models[nm].shape[1]):
cell = self.grid.cells[i][j]
a = colmap.to_rgba(int(self.grid.models[nm].deviation[i][j]))
b = (int(a[2] * 255), int(a[1] * 255), int(a[0] * 255),
int(a[3] * 50))
self.sigma_canvas[nm].draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
self.klegend3_canvas[nm].put_text(self.grid.models[nm].name)
self.klegend3_canvas[nm].draw_legend(minv,
maxv,
colmap,
title="Deviation")
self.redraw()
def krieg_all_mmodels(self):
for i in self.grid.models:
i.do_krigging()
self.redraw_kriged = True
self.redraw_var = True
self.redraw_devi = True
def draw_all_outputs(self):
for i in self.grid.models:
self.draw_krigged(self.model_canvas_names.index(i.name))
self.redraw_kriged = False
def draw_all_vars(self):
for i in self.grid.models:
self.draw_variance(self.model_canvas_names.index(i.name))
self.redraw_var = False
def draw_all_devs(self):
for i in self.grid.models:
self.draw_deviation(self.model_canvas_names.index(i.name))
self.redraw_devi = False
def _change_mode(self, k):
if k == 27:
self.running = False
elif k == ord('q'):
self.running = False
elif k == ord('n'):
print len(self.grid.models)
elif k == ord('i'):
if self.n_models > 0:
self.draw_mode = "inputs"
self.current_model = 0
self.redraw()
elif k == ord('d'):
if self.n_models > 0:
self.draw_mode = "deviation"
self.current_model = 0
if self.redraw_devi:
self.draw_all_devs()
self.redraw()
elif k == ord('v'):
if self.n_models > 0:
self.draw_mode = "variance"
self.current_model = 0
if self.redraw_var:
self.draw_all_vars()
self.redraw()
elif k == ord('t'):
self.krieg_all_mmodels()
self.grid.calculate_mean_grid()
if self.n_models > 0:
self.draw_all_outputs()
self.draw_mode = "kriging"
self.current_model = 0
self.redraw()
elif k == ord('k'):
if self.n_models > 0:
self.draw_mode = "kriging"
self.current_model = 0
if self.redraw_kriged:
self.draw_all_outputs()
self.redraw()
elif k == ord('>'):
self.current_model += 1
if self.current_model >= self.n_models:
self.current_model = 0
self.redraw()
elif k == ord('<'):
self.current_model -= 1
if self.current_model < 0:
self.current_model = self.n_models - 1
self.redraw()
elif k == ord('w'):
self.grid_canvas.draw_waypoints(self.topo_map.waypoints,
(0, 255, 0, 2),
thickness=1)
self.grid_canvas.draw_waypoints(self.visited_wp, (0, 0, 255, 2),
thickness=1)
self.redraw()
elif k == ord('e'):
self.exploration_canvas.draw_waypoints(self.explo_plan.targets,
(255, 200, 128, 255),
thickness=3)
self.exploration_canvas.draw_plan(self.explo_plan.route,
'cyan',
thickness=1)
self.redraw()
#xnames = [x.name for x in self.explo_plan.route]
#print xnames
elif k == ord('g'):
if len(self.explo_plan.route) > 0:
gg = self.explo_plan.route[0]
self.open_nav_client.cancel_goal()
targ = open_nav.msg.OpenNavActionGoal()
targ.goal.coords.header.stamp = rospy.Time.now()
targ.goal.coords.latitude = gg.coord.lat
targ.goal.coords.longitude = gg.coord.lon
print "Going TO: ", gg
self.exploring = 1
self.navigating = True
self.open_nav_client.send_goal(targ.goal)
self.result_counter = 0
self.explodist = 0
else:
print "Done Exploring"
self.exploring = 0
elif k == ord('y'):
vwp = []
for i in self.visited_wp:
vwp.append(i.name)
yml = yaml.safe_dump(vwp, default_flow_style=False)
fh = open("visited.yaml", "w")
s_output = str(yml)
fh.write(s_output)
fh.close
elif k == ord('l'):
print "loading visited"
with open("visited.yaml", 'r') as f:
visited = yaml.load(f)
for i in visited:
for l in self.topo_map.waypoints:
if i == l.name:
self.visited_wp.append(l)
break
elif k == ord('a'):
self.grid.calculate_mean_grid()
self.draw_means()
self.draw_mode = "means"
elif k == ord('p'):
self.pause_exp = not self.pause_exp
elif k == ord('c'):
print self.grid.limits
print "Area: ", self.grid.calculate_area(self.grid.limits)
print "Area of Area: ", self.grid.area.area_size
colours = [
'magenta', 'cyan', 'grey', 'white', 'red', 'yellow', 'green',
'blue'
]
nc = 0
for j in self.grid.area_splits:
print j.area_size
#self.limits_canvas.draw_coordinate(j.centre, 'crimson', size=3, thickness=2)
for i in j.limit_lines:
#self.limits_canvas.draw_line(i, colours[nc], thickness=1)
self.limits_canvas.draw_line(i, 'white', thickness=1)
if nc < len(colours) - 1:
nc += 1
else:
nc = 0
self.redraw()
elif k == ord('r'):
#diff = (self.grid.models[1].output - self.grid.models[0].output)
#print np.mean(diff), np.std(diff), diff.dtype
print self.get_errors()
elif k == ord('o'):
print self.results
outfile = self.expid + '.yaml'
#print self.data_out
yml = yaml.safe_dump(self.results, default_flow_style=False)
fh = open(outfile, "w")
s_output = str(yml)
#print s_output
fh.write(s_output)
fh.close
def get_errors(self):
error_chain = []
shapeo = self.grid.models[0].output.shape
#print vals
print "Waiting for Service"
rospy.wait_for_service('/compare_model')
compare_serv = rospy.ServiceProxy('/compare_model', CompareModels)
for i in range(self.n_models):
try:
d = {}
print "going for it ", i
vals = np.reshape(self.grid.models[i].output, -1)
resp1 = compare_serv('kriging', i, shapeo[0], shapeo[1],
vals.tolist())
d['name'] = self.grid.models[i].name
d['type'] = 'kriging'
d['errors'] = {}
d['errors']['error'] = resp1.error
d['errors']['mse'] = resp1.mse
d['errors']['std'] = resp1.std
d['errors']['var'] = resp1.var
#print resp1
error_chain.append(d)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
try:
d = {}
print "Mean "
vals = np.reshape(self.grid.mean_output, -1)
resp1 = compare_serv('mean', 0, shapeo[0], shapeo[1],
vals.tolist())
#print self.grid.mean_output
d['name'] = 'mean'
d['type'] = 'mean'
d['errors'] = {}
d['errors']['error'] = resp1.error
d['errors']['mse'] = resp1.mse
d['errors']['std'] = resp1.std
d['errors']['var'] = resp1.var
#print resp1
error_chain.append(d)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class comparer(object):
def __init__(self, cell_size):
self.running = True
self.grid = DataGrid('limits.coords', cell_size)
self.grid2 = DataGrid('limits.coords', cell_size)
#self.load_groundtruth('Iains2.yaml')
#self.load_groundtruth('bottom_testing.yaml')
#self.load_groundtruth('upper_testing.yaml')
self.load_groundtruth()
self.krieg_all_mmodels()
self.grid.calculate_mean_grid()
self.grid2.calculate_mean_grid()
print self.model_comparison()
def model_comparison(self):
names = []
means = []
stds = []
diff = self.grid.mean_output - self.grid2.mean_output
print "mean", np.mean(diff), np.sqrt(np.mean(
diff**2)), np.std(diff), np.var(diff)
names.append('mean')
# means.append(np.abs(np.mean(diff)))
# stds.append(np.abs(np.std(diff))/2.0)
means.append(np.abs(np.mean(self.grid.mean_output)))
stds.append(np.abs(np.sqrt(np.mean(diff**2)) / 2.0))
#np.save('mean.csv', diff)
np.savetxt("mean.csv", diff, delimiter=",")
for i in range(len(self.grid.models)):
diff = self.grid.models[i].output - self.grid2.models[i].output
print i, np.mean(diff), np.sqrt(np.mean(
diff**2)), np.std(diff), np.var(diff)
nstr = str(i * 5) + '-' + str((i + 1) * 5)
names.append(nstr)
#means.append(np.abs(np.mean(diff)))
#stds.append(np.abs(np.std(diff)/2.0))
means.append(np.abs(np.mean(self.grid.models[i].output)))
stds.append(np.abs(np.sqrt(np.mean(diff**2)) / 2.0))
np.savetxt(nstr, diff, delimiter=",")
#self.graph_layers(names, means, stds)
self.graph_layers(names, means, stds)
def graph_layers(self, names, means, stds):
fig, ax = plt.subplots()
index = np.arange(len(names))
bar_width = 0.4
opacity = 0.7
error_config = {'ecolor': '0.1'}
rects1 = ax.bar(index,
means,
bar_width,
alpha=opacity,
color='b',
yerr=stds,
error_kw=error_config,
label='errors')
# rects2 = ax.bar(index + bar_width, means_women, bar_width,
# alpha=opacity, color='r',
# yerr=std_women, error_kw=error_config,
# label='Women')
ax.set_xlabel('Layers')
ax.set_ylabel('Mean Errors')
ax.set_title('Errors by Layers')
ax.set_xticks(index + bar_width / 2)
ax.set_xticklabels(names)
#ax.legend()
fig.tight_layout()
plt.savefig('comparison.png')
plt.show()
def load_groundtruth(self):
self.grid.load_data_from_yaml('testing-5cm-intervals.yaml')
self.grid2.load_data_from_yaml('b.yaml')
def krieg_all_mmodels(self):
for i in self.grid.models:
i.do_krigging()
for i in self.grid2.models:
i.do_krigging()
class explorer(object):
modes = {
'help': 'press \'h\' for help',
'none': ''
}
def __init__(self, lat_deg, lon_deg, zoom, size, cell_size):
self.draw_mode = 'none'
self.running = True
self.current_model=-1
self.n_models=0
cv2.namedWindow('explorer')
signal.signal(signal.SIGINT, self.signal_handler)
print "Loading Satellite Image"
self.satellite = SatelliteImage(lat_deg, lon_deg, zoom, size)
self.grid = DataGrid('limits.coords', cell_size)
rospy.loginfo("Subscribing to Krig Info")
rospy.Subscriber("/kriging_data", KrigInfo, self.data_callback)
self.image = self.satellite.base_image.copy()
while(self.running):
cv2.imshow('explorer', self.image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
def data_callback(self, msg):
point_coord = kriging_exploration.map_coords.coord_from_satnav_fix(msg.coordinates)
for i in msg.data:
self.grid.add_data_point(i.model_name, point_coord, i.measurement)
self.vmin, self.vmax = self.grid.get_max_min_vals()
self.n_models=len(self.grid.models)
def krieg_all_mmodels(self):
for i in self.grid.models:
i.do_krigging()
def _change_mode(self, k):
if k == 27:
self.running = False
elif k == ord('q'):
self.running = False
elif k == ord('c'):
print "Draw corners"
self.satellite.draw_coordinate(self.grid.swc,(0,255,0,128))
self.satellite.draw_coordinate(self.grid.sec,(0,0,255,128))
self.satellite.draw_coordinate(self.grid.nwc,(255,0,0,128))
self.satellite.draw_coordinate(self.grid.nec,(255,255,255,128))
self.image = self.satellite.base_image.copy()
elif k == ord('i'):
self.draw_mode='inputs'
self.draw_inputs(self.current_model)
elif k == ord('t'):
self.krieg_all_mmodels()
self.draw_mode='krigging'
self.draw_krigged(self.current_model)
elif k == ord('v'):
self.draw_mode='sigma'
self.draw_deviation(self.current_model)
elif k == ord('>'):
self.current_model+=1
if self.current_model >= self.n_models:
self.current_model=0
if self.draw_mode=='inputs':
self.draw_inputs(self.current_model)
if self.draw_mode=='krigging':
self.draw_krigged(self.current_model)
if self.draw_mode=='sigma':
self.draw_deviation(self.current_model)
elif k == ord('<'):
self.current_model-=1
if self.current_model < 0:
self.current_model=self.n_models-1
if self.draw_mode=='inputs':
self.draw_inputs(self.current_model)
if self.draw_mode=='krigging':
self.draw_krigged(self.current_model)
if self.draw_mode=='sigma':
self.draw_deviation(self.current_model)
elif k == ord('l'):
print "Draw Limits"
#self.satellite.draw_list_of_coords(self.kriged.limits, (0,0,255,128))
self.satellite.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
self.image = self.satellite.base_image.copy()
elif k == ord('g'):
print "Draw Grid"
print "Grid Shape: ", self.grid.shape
self.satellite.draw_grid(self.grid.cells, self.grid.cell_size, (64,64,64,64), thickness=1)
# print len(self.kriged.waypoints), len(self.kriged.waypoints[0])
# for i in range(0, len(self.kriged.waypoints)):
# self.satellite.draw_list_of_coords(self.kriged.waypoints[i], (128,0,128,64), size=2, thickness=-1)
self.image = self.satellite.base_image.copy()
def draw_deviation(self, nm):
font = cv2.FONT_HERSHEY_SIMPLEX
norm = mpl.colors.Normalize(vmin=0, vmax=100)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
# vmin = np.floor(self.grid.data[0].lims[0])
# vmax = np.ceil(self.grid.data[0].lims[1])
# norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
# cmap = cm.jet
# colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in range(self.grid.models[nm].shape[0]):
for j in range(self.grid.models[nm].shape[1]):
cell = self.grid.cells[i][j]
a= colmap.to_rgba(int(self.grid.models[nm].sigmapercent[i][j]*100))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
self.satellite.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
self.satellite.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
self.image = self.satellite.base_image.copy()
cv2.putText(self.image, self.grid.models[nm].name, (int(520), int(20)), font, 0.8, (200, 200, 200), 2)
self.draw_legend(self.vmin, self.vmax)
def draw_krigged(self, nm):
font = cv2.FONT_HERSHEY_SIMPLEX
norm = mpl.colors.Normalize(vmin=self.vmin, vmax=self.vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
# vmin = np.floor(self.grid.data[0].lims[0])
# vmax = np.ceil(self.grid.data[0].lims[1])
# norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
# cmap = cm.jet
# colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in range(self.grid.models[nm].shape[0]):
for j in range(self.grid.models[nm].shape[1]):
cell = self.grid.cells[i][j]
a= colmap.to_rgba(int(self.grid.models[nm].output[i][j]))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
self.satellite.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
self.satellite.draw_polygon(self.grid.limits, (0,0,255,128), thickness=1)
self.image = self.satellite.base_image.copy()
cv2.putText(self.image, self.grid.models[nm].name, (int(520), int(20)), font, 0.8, (200, 200, 200), 2)
self.draw_legend(self.vmin, self.vmax)
def draw_inputs(self, nm):
# vmin = np.floor(self.grid.models[nm].lims[0])
# vmax = np.ceil(self.grid.models[nm].lims[1])
# print vmin, vmax
#
font = cv2.FONT_HERSHEY_SIMPLEX
norm = mpl.colors.Normalize(vmin=self.vmin, vmax=self.vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
#
for i in self.grid.models[nm].orig_data:
cell = self.grid.cells[i.y][i.x]
# #print i.value
a= colmap.to_rgba(int(i.value))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
# #print a, b
self.satellite.draw_cell(cell, self.grid.cell_size, b, thickness=-1)
self.image = self.satellite.base_image.copy()
cv2.putText(self.image, self.grid.models[nm].name, (int(520), int(20)), font, 0.8, (200, 200, 200), 2)
self.draw_legend(self.vmin, self.vmax)
def draw_legend(self, vmin, vmax):
step = (vmax - vmin)/(600-40)
norm = mpl.colors.Normalize(vmin=self.vmin, vmax=self.vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
vp = range(int(np.floor(vmin)),int(np.ceil(vmax)), int(np.ceil(step)))
print len(vp)
if step>1.0:
ind = 0
while ind < 560:
# print int(vmin+(ind*step))
a= colmap.to_rgba(int(vmin+(ind*step)))
b= (int(a[2]*255), int(a[1]*255), int(a[0]*255), int(a[3]*50))
cv2.rectangle(self.image, (int(ind+40), int(580)), (int(ind+1+40), int(600)), b , thickness=-1)
ind+=1
# cv2.rectangle(self.image, (int(40), int(580)), (int(600), int(600)), (200,0,0,0), thickness=-1)
def signal_handler(self, signal, frame):
cv2.destroyAllWindows()
print('You pressed Ctrl+C!')
sys.exit(0)
class SimpleDataVisualiser(object):
modes = {'help': 'press \'h\' for help', 'none': ''}
def __init__(self, lat_deg, lon_deg, zoom, size, cell_size):
self.running = True
signal.signal(signal.SIGINT, self.signal_handler)
print "Loading Satellite Image"
self.satellite = SatelliteImage(lat_deg, lon_deg, zoom, size)
self.grid = DataGrid('limits.coords', cell_size)
self.image = self.satellite.base_image.copy()
while (self.running):
cv2.imshow('image', self.image)
k = cv2.waitKey(20) & 0xFF
self._change_mode(k)
def _change_mode(self, k):
if k == 27:
self.running = False
elif k == ord('q'):
self.running = False
elif k == ord('c'):
print "Draw corners"
self.satellite.draw_coordinate(self.grid.swc, (0, 255, 0, 128))
self.satellite.draw_coordinate(self.grid.sec, (0, 0, 255, 128))
self.satellite.draw_coordinate(self.grid.nwc, (255, 0, 0, 128))
self.satellite.draw_coordinate(self.grid.nec, (255, 255, 255, 128))
self.image = self.satellite.base_image.copy()
elif k == ord('l'):
print "Draw Limits"
#self.satellite.draw_list_of_coords(self.kriged.limits, (0,0,255,128))
self.satellite.draw_polygon(self.grid.limits, (0, 0, 255, 128),
thickness=1)
self.image = self.satellite.base_image.copy()
elif k == ord('g'):
print "Draw Grid"
print "Grid Shape: ", self.grid.shape
self.satellite.draw_grid(self.grid.cells,
self.grid.cell_size, (64, 64, 64, 64),
thickness=1)
# print len(self.kriged.waypoints), len(self.kriged.waypoints[0])
# for i in range(0, len(self.kriged.waypoints)):
# self.satellite.draw_list_of_coords(self.kriged.waypoints[i], (128,0,128,64), size=2, thickness=-1)
self.image = self.satellite.base_image.copy()
elif k == ord('t'):
print "DATA"
self.grid.models[0].do_krigging()
vmin = np.floor(self.grid.models[0].lims[0])
vmax = np.ceil(self.grid.models[0].lims[1])
print vmin, vmax
norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in range(self.grid.models[0].shape[0]):
for j in range(self.grid.models[0].shape[1]):
cell = self.grid.cells[i][j]
a = colmap.to_rgba(int(self.grid.models[0].output[i][j]))
b = (int(a[0] * 255), int(a[1] * 255), int(a[2] * 255),
int(a[3] * 50))
self.satellite.draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
self.satellite.draw_polygon(self.grid.limits, (0, 0, 255, 128),
thickness=1)
#self.satellite.draw_grid(self.grid.cells, self.grid.cell_size, (64,64,64,64), thickness=1)
self.image = self.satellite.base_image.copy()
elif k == ord('v'):
vmin = np.floor(self.grid.models[0].min_var * 100)
vmax = np.ceil(self.grid.models[0].max_var * 100)
print vmin, vmax
norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in range(self.grid.models[0].shape[0]):
for j in range(self.grid.models[0].shape[1]):
cell = self.grid.cells[i][j]
a = colmap.to_rgba(
int((1 - self.grid.models[0].variance[i][j]) * 100))
b = (int(a[0] * 255), int(a[1] * 255), int(a[2] * 255),
int(a[3] * 50))
self.satellite.draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
self.satellite.draw_polygon(self.grid.limits, (255, 255, 255, 128),
thickness=1)
#self.satellite.draw_grid(self.grid.cells, self.grid.cell_size, (64,64,64,64), thickness=1)
self.image = self.satellite.base_image.copy()
elif k == ord('i'):
print "TEST"
self.grid._load_model_from_file('Iains_data0.dat')
print "LIMS:"
vmin = np.floor(self.grid.models[0].lims[0])
vmax = np.ceil(self.grid.models[0].lims[1])
print vmin, vmax
norm = mpl.colors.Normalize(vmin=vmin, vmax=vmax)
cmap = cm.jet
colmap = cm.ScalarMappable(norm=norm, cmap=cmap)
for i in self.grid.models[0].orig_data:
cell = self.grid.cells[i.y][i.x]
#print i.value
a = colmap.to_rgba(int(i.value))
b = (int(a[0] * 255), int(a[1] * 255), int(a[2] * 255),
int(a[3] * 50))
#print a, b
self.satellite.draw_cell(cell,
self.grid.cell_size,
b,
thickness=-1)
self.image = self.satellite.base_image.copy()
def signal_handler(self, signal, frame):
cv2.destroyAllWindows()
print('You pressed Ctrl+C!')
sys.exit(0)