From a29d59ea2c96d93b180c0cddada0bd118f7388a9 Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Sun, 5 May 2019 22:37:07 -0700
Subject: [PATCH] Added data verification function to data_generator Small
tweaks in parameter passing in simulated dm
---
CoLo-AT/dataset_manager/data_generator.py | 155 ++++++++++++++++--
.../simulated_dataset_manager.py | 9 +-
CoLo-AT/test_simulation_sagar.py | 54 ++----
3 files changed, 158 insertions(+), 60 deletions(-)
diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index fde0f4d..cd585f1 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -1,5 +1,7 @@
import numpy as np
from math import pi, sqrt, atan2, hypot, sin, cos
+import matplotlib.pyplot as plt
+
class DataGenerator():
def __init__(self, boundary, landmarks, duration, robot_labels, starting_states, start_time, delta_t,
@@ -45,7 +47,10 @@ class DataGenerator():
# Inputs: None
# Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
# Generates simulated data based of robots moving in a straight line until simulation ends/robots hit the boundary and stop moving
- def generate_straight_line_data(self):
+ def generate_straight_line_data(self, test):
+
+ velocity_step = self.boundary/self.duration
+ move_step = velocity_step * self.delta_t
# Generate ground truth data
for i, label in enumerate(self.robot_labels, 0):
@@ -58,8 +63,11 @@ class DataGenerator():
next_orientation = curr_orientation
- if (self.within_map([curr_x + 1, curr_y + 1])):
- next_x = curr_x + 1
+ next_x = curr_x
+ next_y = curr_y
+
+ if (self.within_map([curr_x + move_step, curr_y])):
+ next_x = curr_x + move_step
next_y = curr_y
self.groundtruth_data[i].append({'time' : self.time_arr['groundtruth'][i][time_idx], 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
@@ -72,7 +80,7 @@ class DataGenerator():
for time_idx in range(1, len(self.time_arr['odometry'][i])):
loc1 = [self.groundtruth_data[i][time_idx-1]['x_pos'], self.groundtruth_data[i][time_idx-1]['y_pos']]
loc2 = [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']]
- velocity = self.calc_distance(loc1, loc2)/self.delta_t + np.random.normal(loc=0.0,scale=self.velocity_noise)
+ velocity = self.calc_distance(loc1, loc2)/self.delta_t + np.random.normal(loc=0.0,scale=self.velocity_noise)
theta_1 = self.groundtruth_data[i][time_idx-1]['orientation']
theta_2 = self.groundtruth_data[i][time_idx]['orientation']
@@ -95,25 +103,30 @@ class DataGenerator():
robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
robot_loc = [robot_loc_x, robot_loc_y]
- if (self.within_vision(self.robot_fov, robot_loc, landmark_loc)):
+ if (self.within_vision(self.robot_fov, robot_loc, landmark_loc, 0)):
measurement_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise)
+ if (bearing < 0):
+ bearing += 2*pi
self.measurement_data[robot_idx].append({'time': time, 'subject_ID': landmarkID, 'measurment_range':measurement_range, 'bearing':bearing})
time_arr.append(time) # we expect repeats/skipped times in time array
self.time_arr['measurement'][robot_idx] = time_arr
self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
+ if (test):
+ self.verify_generated_data()
+
return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
# Inputs: None
# Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
# Generates simulated of robots moving in a circle
- def generate_circular_data(self):
+ def generate_circular_data(self, test):
# TODO - add boundary, collision checking
# NOTE - this method of generating a circular path goes from polygons (low duration) to rounder as duration increases
- path_radius = self.boundary/self.num_robots
+ path_radius = self.boundary/(self.num_robots + 3)
path_circumference = 2*pi*path_radius
radian_step = 2*pi/self.duration
@@ -132,7 +145,8 @@ class DataGenerator():
next_x = curr_x + distance_step*cos(radian_step + curr_orientation)
next_y = curr_y + distance_step*sin(radian_step + curr_orientation)
- next_orientation = curr_orientation + radian_step # TODO - Check if orientaion is between 0 and 2pi
+ next_orientation = curr_orientation + radian_step
+
if (next_orientation > 2*pi): # later on we may have robots go in multiple loops
next_orientation -= 2*pi
@@ -147,7 +161,7 @@ class DataGenerator():
loc1 = [self.groundtruth_data[i][time_idx-1]['x_pos'], self.groundtruth_data[i][time_idx-1]['y_pos']]
loc2 = [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']]
velocity = self.calc_distance(loc1, loc2)/self.delta_t + np.random.normal(loc=0.0,scale=self.velocity_noise)
-
+
theta_1 = self.groundtruth_data[i][time_idx-1]['orientation']
theta_2 = self.groundtruth_data[i][time_idx]['orientation']
angular_velocity = (theta_2-theta_1)/self.delta_t + np.random.normal(loc=0.0,scale=self.angular_velocity_noise)
@@ -167,25 +181,136 @@ class DataGenerator():
time = self.time_arr['measurement'][robot_idx][time_idx]
robot_loc_x = self.groundtruth_data[robot_idx][time_idx]['x_pos']
robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
+ robot_orientation = self.groundtruth_data[robot_idx][time_idx]['orientation']
robot_loc = [robot_loc_x, robot_loc_y]
- if (self.within_vision(self.robot_fov, robot_loc, landmark_loc)):
+ if (self.within_vision(self.robot_fov, robot_loc, landmark_loc, robot_orientation)):
measurement_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise)
+ if (bearing < 0):
+ bearing += 2*pi
self.measurement_data[robot_idx].append({'time': time, 'subject_ID': landmarkID, 'measurment_range':measurement_range, 'bearing':bearing})
time_arr.append(time) # we expect repeats/skipped times in time array
+
self.time_arr['measurement'][robot_idx] = time_arr
self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
+ if (test):
+ self.verify_generated_data()
+
return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
+ # Inputs: None
+ # Outputs: Verifies Generated Data
+ def verify_generated_data(self):
+ print('******Verifying Data******')
+
+ # Verify odometry data & measurement data
+ odo_test_arr = [[] for robot in self.robot_labels]
+ meas_test_arr = [[] for robot in self.robot_labels]
+ for i, robot in enumerate(self.robot_labels):
+ groundtruth_list = self.groundtruth_data[i]
+ odometry_list = self.odometry_data[i]
+ measurement_list = self.measurement_data[i]
+ for g_idx in range(1, len(groundtruth_list)):
+
+ t1 = groundtruth_list[g_idx-1]['time']
+ x1 = groundtruth_list[g_idx-1]['x_pos']
+ y1 = groundtruth_list[g_idx-1]['y_pos']
+ o1 = groundtruth_list[g_idx-1]['orientation']
+
+ t2 = groundtruth_list[g_idx]['time']
+ x2 = groundtruth_list[g_idx]['x_pos']
+ y2 = groundtruth_list[g_idx]['y_pos']
+ o2 = groundtruth_list[g_idx]['orientation']
+
+ actual_delta_t = (t2-t1)
+ actual_v = self.calc_distance([x1,y1], [x2, y2])/actual_delta_t
+ actual_w = (o2-o1)/actual_delta_t
+ actual_o = o2
+
+ generated_v = odometry_list[g_idx]['velocity']
+ generated_w = odometry_list[g_idx]['angular velocity']
+ generated_o = odometry_list[g_idx]['orientation']
+ generated_delta_t = odometry_list[g_idx]['delta_t']
+
+ v_diff = actual_v - generated_v
+ w_diff = actual_w - generated_w
+ o_diff = actual_o - generated_o
+ delta_t_diff = actual_delta_t - generated_delta_t
+
+ odo_test_arr[i].append({'v_diff': v_diff, 'w_diff': w_diff, 'o_diff': o_diff, 'delta_t_diff': delta_t_diff, 'time': t2})
+
+ curr_time = t2
+
+ measurements = [measurement for measurement in measurement_list if measurement['time']==curr_time]
+ curr_loc = [x2,y2]
+ for measurement in measurements:
+ landmark_loc = self.landmark_map[measurement['subject_ID']]
+
+ actual_meas = self.calc_distance(curr_loc, landmark_loc)
+ generated_measurement = measurement['measurment_range']
+
+ actual_bearing = atan2(landmark_loc[1]- y2, landmark_loc[0]-x2)
+ if (actual_bearing < 0):
+ actual_bearing += 2*pi
+ generated_bearing = measurement['bearing']
+
+ meas_diff = actual_meas - generated_measurement
+ bearing_diff = actual_bearing - generated_bearing
+
+ meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
+
+ fig, ax_arr = plt.subplots(6) # TODO - change plotting code to accomodate measurement_range and bearing verification
+ plt.xlabel('t [s]')
+ plt.title('Odometry & Measurement Data Verification', position=(0.5,7))
+ ax_arr[0].set_ylabel('v_diff [m/s]')
+ ax_arr[1].set_ylabel('w_diff [r/s]')
+ ax_arr[2].set_ylabel('o_diff [r]')
+ ax_arr[3].set_ylabel('delta_t_diff [s]')
+ ax_arr[4].set_ylabel('meas_diff [m]')
+ ax_arr[5].set_ylabel('bearing_diff [r]')
+
+ labels = ['robot1', 'robot2', 'robot3']
+
+ for diff_list in odo_test_arr:
+ v_diff_arr = [d['v_diff'] for d in diff_list]
+ w_diff_arr = [d['w_diff'] for d in diff_list]
+ o_diff_arr = [d['o_diff'] for d in diff_list]
+ delta_t_diff_arr = [d['delta_t_diff'] for d in diff_list]
+ time_arr = [d['time'] for d in diff_list]
+
+ ax_arr[0].plot(time_arr, v_diff_arr)
+ ax_arr[1].plot(time_arr, w_diff_arr)
+ ax_arr[2].plot(time_arr, o_diff_arr)
+ ax_arr[3].plot(time_arr, delta_t_diff_arr)
+
+ for diff_list in meas_test_arr:
+ meas_diff_arr = [d['meas_diff'] for d in diff_list]
+ bearing_diff_arr = [d['bearing_diff'] for d in diff_list]
+ meas_time_arr = [d['time'] for d in diff_list]
+
+ ax_arr[4].scatter(meas_time_arr, meas_diff_arr)
+ ax_arr[5].scatter(meas_time_arr, bearing_diff_arr)
+
+ fig.legend(labels, prop={'size': 12})
+ ax_arr[0].set_ylim([-10*self.velocity_noise, 10*self.velocity_noise])
+ ax_arr[1].set_ylim([-10*self.angular_velocity_noise, 10*self.angular_velocity_noise])
+ ax_arr[4].set_ylim([-10*self.measurement_range_noise, 10*self.measurement_range_noise])
+ ax_arr[5].set_ylim([-10*self.bearing_noise, 10*self.bearing_noise])
+
+ plt.show()
+ plt.close('all')
+
+ print('******Verification Complete******')
+
# Input: loc=[x,y]
# Output: boolean if the point [x,y] is within the map
def within_map(self, loc):
return hypot(loc[0], loc[1]) < self.boundary
# Input: loc1=[x1,y1], loc2=[x2,y2]
- # Output: distance between two points
+ # Output: Euclidean distance between two points
def calc_distance(self, loc1, loc2):
x1 = loc1[0]
@@ -197,13 +322,15 @@ class DataGenerator():
distance = sqrt((x1-x2)**2 + (y1-y2)**2)
return distance
- # NOTE - add distance limitations to robot_vision
+ # NOTE - add distance limitations to robot_vision?
# Input: robot_loc, landmark_loc, [x,y]
# Output: boolean, a given point is with the field of view (sector of a circle) of a robot
- def within_vision(self, robot_fov, robot_loc, landmark_loc):
+ def within_vision(self, robot_fov, robot_loc, landmark_loc, robot_orientation=0):
# between pi and -pi with respect to the x axis
bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0]))
- return abs(bearing) <= robot_fov/2
+ if bearing < 0:
+ bearing += 2*pi
+ return bearing <= robot_fov/2 + robot_orientation and bearing >= robot_orientation - robot_fov/2
# NOTE - Data formatting
'''
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 1df562e..738f22c 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -16,7 +16,6 @@ from dataset_manager.data_generator import DataGenerator
# - Circular path data in DataGenerator class
# - communication
# - relative observations
-# - animated plots
class SimulatedDataSetManager:
def __init__(self, dataset_name, boundary, landmarks, duration, robot_labels,
@@ -47,7 +46,7 @@ class SimulatedDataSetManager:
# starting_states = {robot label: [time, x_pos, y_pos, orientation], ... }
# TODO - create a generate starting states function
- self.starting_states = {label : [self.start_time, -boundary + float(i+2)*boundary/self.num_robots, 0, 0.0] for i,label in enumerate(self.robot_labels)}
+ self.starting_states = {label : [self.start_time, -boundary + float(i+1)*boundary/self.num_robots, 0, 0.0] for i,label in enumerate(self.robot_labels)}
# groundtruth & odometry are of fixed length=duration
# measurement_data is of variable length
@@ -62,13 +61,13 @@ class SimulatedDataSetManager:
# Input: robot_path (line, circle, etc.)
# Output: Generates simulated data (gt, odo, meas)
- def simulate_dataset(self, path='line'):
+ def simulate_dataset(self, path='line', test=False):
if (path == 'line'):
- self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_straight_line_data()
+ self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_straight_line_data(test)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
# TODO
elif(path=='circle'):
- self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_circular_data()
+ self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_circular_data(test)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
else:
raise Exception("Unsupported robot path specified.")
diff --git a/CoLo-AT/test_simulation_sagar.py b/CoLo-AT/test_simulation_sagar.py
index d879ba1..9a250f4 100644
--- a/CoLo-AT/test_simulation_sagar.py
+++ b/CoLo-AT/test_simulation_sagar.py
@@ -38,62 +38,34 @@ compname = getpass.getuser()
#dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/UTIAS-Datasets/MRCLAM_Dataset3/"
# landmark IDs must be bigger than 5
-landmarks = {10: [0, 2], 11: [25,0], 12: [0,25], 13: [-25,0], 14: [0, -25] }
+landmarks = {10: [0,5], 11: [3,3], 12: [-3,0], 13: [-3, -2] }
robot_labels = [1,2,3]
-boundary = 25 # radius of circular map centered at 0,0
-duration = 120 # (seconds)
+boundary = 5 # radius of circular map centered at 0,0
+duration = 30 # (seconds)
-# NOTE - noise significantly affects the results
+# TODO - debug velocity_noise
testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
-velocity_noise=0.01, angular_velocity_noise=0.01, measurement_range_noise=0.01, bearing_noise=0.01, robot_fov=2*pi)
-start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle')
-
-robot_1_groundtruths = dataset_data['groundtruth'][0]
-robot_2_groundtruths = dataset_data['groundtruth'][1]
-robot_3_groundtruths = dataset_data['groundtruth'][2]
-
-landmark_x = [val[0] for key,val in landmarks.items()]
-landmark_y = [val[1] for key,val in landmarks.items()]
-
-x1_array = np.array([groundtruth['x_pos'] for groundtruth in robot_1_groundtruths])
-y1_array = np.array([groundtruth['y_pos'] for groundtruth in robot_1_groundtruths])
-
-x2_array = np.array([groundtruth['x_pos'] for groundtruth in robot_2_groundtruths])
-y2_array = np.array([groundtruth['y_pos'] for groundtruth in robot_2_groundtruths])
-
-x3_array = np.array([groundtruth['x_pos'] for groundtruth in robot_3_groundtruths])
-y3_array = np.array([groundtruth['y_pos'] for groundtruth in robot_3_groundtruths])
-
-boundary_circle = plt.Circle((0,0), boundary, color='#B9F1BA', label='boundary')
-plt.title('Robot Paths')
-plt.xlabel('x [m]')
-plt.ylabel('y [m]')
-plt.gcf().gca().add_artist(boundary_circle)
-plt.gcf().gca().set_xlim(-2*boundary, 2*boundary)
-plt.gcf().gca().set_ylim(-2*boundary, 2*boundary)
-plt.plot(x1_array, y1_array, label='Robot 1')
-plt.plot(x2_array, y2_array, label='Robot 2')
-plt.plot(x3_array, y3_array, label='Robot 3')
-plt.plot(landmark_x, landmark_y, 'k.', label='Landmarks' )
-
-plt.legend(loc='best')
+velocity_noise=0.001, angular_velocity_noise=0.001, measurement_range_noise=0.001, bearing_noise=0.001, robot_fov=2*pi)
+start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle', test=True)
# Real-World DataSet
# dataset_path = "D:/LEMUR/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/"
# testing_dataset = RW_Dataset_Manager('testing')
# start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
-
analyzer = Analyzer('analyzer', robot_labels)
-loc_algo = Centralized_EKF('CentralizedEKF')
+loc_algo = Centralized_EKF('Centralized_EKF')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim')
-state_recorder_bda = StatesRecorder('CentralizedEKF', robot_labels)
+state_recorder = StatesRecorder('Centralized_EKF', robot_labels)
+
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False, comm=False, simulated_comm = False)
+
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bda, simple_plot = True, comm=False, simulated_comm = False)
+animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
-# loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bda, plot_graphs = True)
--
GitLab