diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf
index 19257be51e0530e4914cf84d90a705e80fff7465..8f16e1f65b9e20fcf6dcc4a82b3b306b431f034e 100644
Binary files a/CoLo-AT/Algorithm Comparison.pdf and b/CoLo-AT/Algorithm Comparison.pdf differ
diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py
index abad62ffb796d8837b9ea7144699ed952ba32a1b..78aa75a08f28d845133f84096c78aaad23a5ecf0 100644
--- a/CoLo-AT/Cooperative_Localization_Journal1.py
+++ b/CoLo-AT/Cooperative_Localization_Journal1.py
@@ -49,8 +49,8 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
state_recorder = StatesRecorder('GS_CI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False , robots_cant_observe_lm = robots_cant_observe_lm)
- loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
-
+ analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
+ analyzer.trajectory_plot(state_recorder)
##############################################################################
'''
testing_dataset.dataset_reset()
@@ -71,7 +71,7 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
sim = SimulationManager('sim cen_ekf')
state_recorder_LS_cen = StatesRecorder('LS_cen', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
- loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
+ analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
##############################################################################
@@ -82,7 +82,7 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
sim = SimulationManager('sim ls_bda')
state_recorder_LS_BDA = StatesRecorder('LS_BDA', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
- loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
+ analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
##############################################################################
@@ -94,7 +94,7 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
sim = SimulationManager('sim ls_ci')
state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
- loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
+ analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
##############################################################################
@@ -105,6 +105,6 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
sim = SimulationManager('sim gs_sci')
state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
- loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
+ analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI], graph_name = graph_name)
diff --git a/CoLo-AT/UTIAS_d6.pdf b/CoLo-AT/UTIAS_d6.pdf
index 7c61a9ffe45bb01418f73480d4d5c783b1392b11..c2d1f97d33e0a79d63082354979c09983749cc5b 100644
Binary files a/CoLo-AT/UTIAS_d6.pdf and b/CoLo-AT/UTIAS_d6.pdf differ
diff --git a/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc
index f6917882d67cca10eaf21cefccd99f71c58f029a..994caf1e5942bfd5a5db55f4e442c674b2c52baf 100644
Binary files a/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc and b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc
index de81e6ebe727461593d79fb78612610ee8c62a80..7703bdb3d0ca0acc5db1c1d0a506c1a597b0d88b 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/realworld_dataset_manager.py b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
index ca35dc4ff2570bc76adea152f79fa575a6921408..e8c7c3c3ca5569506f0f993f0a3b0dacccbe4865 100755
--- a/CoLo-AT/dataset_manager/realworld_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
@@ -90,7 +90,7 @@ class RW_Dataset_Manager:
self.time_arr = {'odometry': [[] for i in range(self.num_robots)], 'measurement': [[] for i in range(self.num_robots)], 'groundtruth': [[] for i in range(self.num_robots)]}
#initialization for MRCLAMDatasets: put files into dictionaries:
- self.duration = duration # some more time to avoid index error
+ self.duration = duration+1# some more time to avoid index error
self.start_time_arr = []
#finding the starting time:
for i, label in enumerate(self.robot_lables):
diff --git a/CoLo-AT/isrr2017_v2_2019.py b/CoLo-AT/isrr2017_v2_2019.py
index dc085bfa1cb124bded6b575013fbc0a82cc4634c..df03354c372e00f4c154819e3a827eebad1feeeb 100644
--- a/CoLo-AT/isrr2017_v2_2019.py
+++ b/CoLo-AT/isrr2017_v2_2019.py
@@ -40,10 +40,10 @@ sim = SimulationManager('sim gs_ci')
state_recorder = StatesRecorder('GS_CI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = .calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
#animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
-
+analyzer.trajectory_plot(data_recorder)
##############################################################################
testing_dataset.dataset_reset()
diff --git a/CoLo-AT/j1_rw.py b/CoLo-AT/j1_rw.py
index 4cdbb13f460461d2decd10bdb3e5bbb6fa0de63e..5ddda45e05241bb70f7882f6e1b9001f6b1baa74 100644
--- a/CoLo-AT/j1_rw.py
+++ b/CoLo-AT/j1_rw.py
@@ -21,11 +21,11 @@ for i in [1, 2, 3, 4]:
datasets_path = "/home/william/CoLo/CoLo-D/UTIAS-dataset/MRCLAM_Dataset" # for desktop Ubuntu
robot_labels = [1,2,3,4,5]
-duration = 600 # duration for the simulation in sec
+duration = 100 # duration for the simulation in sec
-for i in range(1, 9):
- dataset_label = str(i)
- dataset_path = datasets_path + dataset_label + "/"
- graph_name = 'UTIAS_d'+dataset_label
- print(dataset_path)
- all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots_cant_observe_lm = [4, 5])
\ No newline at end of file
+i = 6
+dataset_label = str(i)
+dataset_path = datasets_path + dataset_label + "/"
+graph_name = 'UTIAS_d'+dataset_label
+print(dataset_path)
+all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots_cant_observe_lm = [4, 5])
\ No newline at end of file
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc
index 898bd634c5771fc458900a539937375e91f436bf..cd22333eeb9494a4a8108613451740afd219fd98 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index 08908cd221926b304699983ddc78d8c89198daa0..9f2fabc242f24ac4994e0ee86ecf4056f9edb8cb 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -89,7 +89,7 @@ class SimulationManager():
state_var = []
while self.time < start_time+duration:
- #print("Current time:", self.time)
+ #print("remaining time:", start_time+duration-self.time)
g_req= request_response.Request_response(None, None)
prev_time = self.time
valid_op, self.time , rsp = dm.respond(g_req, self.time)