diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py index 476e7ab24a60cb535a8d8a73e66cb2624ed6e67b..edc1d178b9efcc5f2f0fda009b7c8f1802316684 100644 --- a/CoLo-AT/Cooperative_Localization_Journal1.py +++ b/CoLo-AT/Cooperative_Localization_Journal1.py @@ -51,7 +51,9 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False , robots_cant_observe_lm = robots_cant_observe_lm) analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True) - analyzer.trajectory_plot(state_recorder) + + landmark_map = testing_dataset.get_landmark_map() + analyzer.trajectory_plot(state_recorder, landmark_map = landmark_map) ############################################################################## ''' @@ -69,7 +71,7 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots testing_dataset.dataset_reset() loc_algo = Centralized_EKF('algo') robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False) - + 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) @@ -110,4 +112,9 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots 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) - analyzer.generate_result_files([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI]) \ No newline at end of file + analyzer.generate_result_files([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI]) + + + + print("Animation Start: ") + animate_plot(dataset_labels, state_recorder, analyzer, lm = testing_dataset.get_landmark_map()) \ No newline at end of file diff --git a/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc index 3af8e2339cde74a005acc5b2cf61285fe765bc9b..c1c0643406051cd323648b71a999639448d18487 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/colo_d1.pdf b/CoLo-AT/colo_d1.pdf index 92c82dfbeaeacd53bbdfcda17ff404c11ad29060..c891ce893ce94c308fa333524367f3ea8688e73d 100644 Binary files a/CoLo-AT/colo_d1.pdf and b/CoLo-AT/colo_d1.pdf differ diff --git a/CoLo-AT/colo_d2.pdf b/CoLo-AT/colo_d2.pdf index 908acf4b7388c02316a3cb946d4266ac5233d39c..7292d46c4db2dac6fe1f24abf3170ca411d839eb 100644 Binary files a/CoLo-AT/colo_d2.pdf and b/CoLo-AT/colo_d2.pdf differ diff --git a/CoLo-AT/colo_d3.pdf b/CoLo-AT/colo_d3.pdf index 38ed4bbcd91282b1d369bcc97d840bb996df2739..c7c1af23d93c05714f20aab71f0ed270e898d6e2 100644 Binary files a/CoLo-AT/colo_d3.pdf and b/CoLo-AT/colo_d3.pdf differ diff --git a/CoLo-AT/colo_d4.pdf b/CoLo-AT/colo_d4.pdf index cf0ff65e1d6e1f7216418844fbfb0740adc50d1d..9d145a563732bc378554f7858b490a26c8f461fe 100644 Binary files a/CoLo-AT/colo_d4.pdf and b/CoLo-AT/colo_d4.pdf differ diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc index 3b90640b969c7635f4b486ef6f71cf07b56cd3df..be77288af1d43e6e3097def5b1f639f6ae010e6b 100644 Binary files a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc differ diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py index 35423b809cc934758f27ee90ca6d2168546c1aab..aad643c5bce5d997940dab9acf6aa7b5e95526f5 100644 --- a/CoLo-AT/data_analysis/data_analyzer.py +++ b/CoLo-AT/data_analysis/data_analyzer.py @@ -283,12 +283,28 @@ class Analyzer(): return arr_loc_err, arr_trace - def trajectory_plot(self, data_recorder, robot_labels = None): + def trajectory_plot(self, data_recorder, landmark_map = None, robot_labels = None): if robot_labels == None: robot_labels = self.dataset_labels - robot_loc_time_unit = self.robot_location_at_unit_time_interval(data_recorder) + # generating landmark file + lm_file = open("landmark_map.txt", "w") + lm_file.write('#id' + ' ' + 'x[m]' + ' ' + 'y[m]' + '\n') + + landmarks_loc = [] + print(landmark_map) + if landmark_map != None: + for landmark_id, [x, y] in landmark_map.items(): + if [x, y] not in landmarks_loc: # duplicate landmark with the same position + result_str = '{:d} {:2.4f} {:2.4f} \n'.format(landmark_id, x, y) + landmarks_loc.append([x, y]) + lm_file.write(result_str) + lm_file.close + + + # generating trajectory file + robot_loc_time_unit = self.robot_location_at_unit_time_interval(data_recorder) trajectory_file = open("trajectory.txt", "w") diff --git a/CoLo-AT/landmark_map.txt b/CoLo-AT/landmark_map.txt new file mode 100644 index 0000000000000000000000000000000000000000..c5e2deaba613bd9ced29d6d3b31b4288a18aa51c --- /dev/null +++ b/CoLo-AT/landmark_map.txt @@ -0,0 +1,9 @@ +#id x[m] y[m] +113 0.7155 -1.9365 +103 0.8365 1.1135 +107 -1.2505 -0.8313 +110 0.9218 -0.7042 +109 -0.2887 -2.1572 +108 0.3667 1.2349 +120 -1.2377 0.0993 +124 -0.7332 1.2293