├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── analyzer_settings.yaml └── base_analyzer_settings.yaml ├── package.xml ├── scripts ├── analyzer.py ├── base_analyzer.py ├── pose_graph_loop_closure_analysis.py ├── recover_fiducial_calibration_from_pose_graph.py └── run_analyzer.yaml └── utilities ├── aggregate_odometries.py ├── boxplot_from_npz.py ├── compute_distances_travelled.py ├── get_dataset_duration.py ├── get_distance_traveled_from_aggregated_odometries.py ├── loop_closure_eval.py ├── plot_factor_covariance.py ├── plot_lc_ftness_hist.py ├── plot_lc_log.py ├── plot_loop_candidates.py ├── plot_odom_covariance.py ├── plot_volume_size_from_map_info.py ├── pose_graph_to_odometry.py ├── segment_aggregated_odometries.py ├── utilities.py └── volume_over_time.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(localization_analyzer) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 NeBula Autonomy 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Localization Analyzer 2 | 3 | A repository for scripts and tools to analyze the performance of lidar odometry and the SLAM system. 4 | Primarily used for [LOCUS](https://github.com/NeBula-Autonomy/LOCUS) and [LAMP](https://github.com/NeBula-Autonomy/LAMP) evaluation. 5 | The package is a wrapper for awesome trajectory analyzing tool [evo](https://github.com/MichaelGrupp/evo). 6 | 7 | # How to install 8 | 9 | ``` 10 | sudo apt install python3-pip 11 | pip3 install numpy 12 | pip3 install pandas 13 | pip3 install seaborn 14 | pip3 install evo 15 | ``` 16 | After installation complete reboot because ```evo``` needs that, then in your ```catkin_ws``` workspace 17 | 18 | ``` 19 | cd ~/catkin_ws/src 20 | git clone https://github.com/NeBula-Autonomy/localization_analyzer.git 21 | ``` 22 | 23 | # How to run 24 | ## How to store data in order to run the scripts 25 | 26 | To run the scripts smoothly you should have the following folder structure for dataset that you are going to evaluate. Assuming the folder is called ```test_localization_analyzer_dataset```. In this folder you should have the following folders and files 27 | 28 | ``` 29 | +-- ground_truth 30 | | +-- odometry.bag 31 | +-- specific_method_run_number 32 | | +-- cpu.bag 33 | | +-- delay.txt 34 | | +-- mem.bag 35 | | +-- odometry.bag 36 | | +-- rate.txt 37 | +-- specific_method2_run_number 38 | | +-- cpu.bag 39 | | +-- delay.txt 40 | | +-- mem.bag 41 | | +-- odometry.bag 42 | | +-- rate.txt 43 | +-- specific_method3_run_number 44 | | +-- cpu.bag 45 | | +-- delay.txt 46 | | +-- mem.bag 47 | | +-- odometry.bag 48 | | +-- rate.txt 49 | ``` 50 | Folder naming: 51 | - ```specific_method```, ```specific_method2```, ```specific_method3``` corresponds to the names in ```specific_methods``` in [config/analyzer_settings.yaml](https://github.com/NeBula-Autonomy/localization_analyzer/blob/main/config/analyzer_settings.yaml) 52 | 53 | - ```run_number``` - corresponds to the test name that you specified for your tests (see [run_analyzer.yaml](https://github.com/NeBula-Autonomy/localization_analyzer/blob/main/scripts/run_analyzer.yaml) for example) 54 | 55 | - ```ground_truth``` - folder where you store bag folder with the ground truth (needs to be called ```odometry.bag```) 56 | 57 | Files description: 58 | - ```cpu.bag``` and ```mem.bag``` - produced from ```monitor.py```. See [LOCUS scripts]() how to use it. 59 | 60 | - ```delay.txt``` - dekay of the odometry topic 61 | ``` 62 | rostopic delay -w3 >> /path/to/test_localization_analyzer_dataset/specific_method_run_number/delay.txt 63 | ``` 64 | 65 | - ```rate.txt``` - rate of the odometry topic 66 | ``` 67 | rostopic hz -w3 >> /path/to/test_localization_analyzer_dataset/specific_method_run_number/rate.txt 68 | ``` 69 | 70 | - ```odometry.bag``` - recorded odometry information 71 | ``` 72 | rosbag record -O /path/to/test_localization_analyzer_dataset/specific_method_run_number/odometry.bag 73 | ``` 74 | 75 | ## Run script 76 | 77 | For easy run go to 78 | ``` 79 | cd ~/catkin_ws/src/localization_analyzer/scripts/ 80 | ``` 81 | setup variables in ```run_analyzer.yaml``` and run 82 | ``` 83 | tmuxp load run_analyzer.yaml 84 | ``` 85 | (see [run_analyzer.yaml](https://github.com/NeBula-Autonomy/localization_analyzer/blob/main/scripts/run_analyzer.yaml) for easy use) 86 | 87 | **(Alternative)** 88 | 89 | If you prefer run python script from the command line directly: 90 | ``` 91 | python $(rospack find localization_analyzer)/scripts/analyzer.py 92 | ``` 93 | 94 | `````` - path to the main folder where your recorded bag data is (e.g. path to ```test_localization_analyzer_dataset```) 95 | 96 | `````` - robot namespace 97 | 98 | `````` - topic of the odometry (needs to contain robot namespace) 99 | 100 | `````` - name of the test 101 | 102 | # Content description 103 | 104 | ## config 105 | ```analyzer_settings.yaml``` - config file that specify what metrics should be evaluated for single robot odometry 106 | 107 | ```base_analyzer_settings.yaml``` 108 | 109 | ## scripts 110 | ```analyzer.py``` - the entrypoint of single robot analyzer 111 | 112 | ```base_analyzer.py``` - the entrypoint of multi robot analyzer 113 | 114 | ```pose_graph_loop_closure_analysis.py``` 115 | 116 | ```run_analyzer.yaml``` - [tmux](https://tmuxp.git-pull.com/) script for the single robot analyzer 117 | 118 | ## utilities folder 119 | ```aggregate_odometries.py``` - script to aggregate user requested odometries along with ground truth odometry. 120 | 121 | ```pose_graph_to_odometry.py``` 122 | 123 | ```segment_aggregated_odometries.py``` 124 | 125 | ```utilities.py``` - module with all the extra functionalities used for analysis 126 | 127 | ```boxplot_from_npz.py``` - drawing boxplt from npz 128 | 129 | ```compute_distances_travelled.py``` - compute distance travelled 130 | 131 | ```get_dataset_duration.py``` - get dataset duration 132 | 133 | ```get_distance_traveled_from_aggregated_odometries.py``` 134 | 135 | ```loop_closure_eval.py``` 136 | 137 | ```volume_over_time.py``` 138 | 139 | ```plot_factor_covariance.py``` 140 | 141 | ```plot_lc_ftness_hist.py``` 142 | 143 | ```plot_lc_log.py``` 144 | 145 | ```plot_loop_candidates.py``` 146 | 147 | ```plot_odom_covariance.py``` 148 | 149 | ```plot_volume_size_from_map_info.py``` 150 | -------------------------------------------------------------------------------- /config/analyzer_settings.yaml: -------------------------------------------------------------------------------- 1 | #################### Analyzer Settings #################### 2 | base_station_name: base1 3 | 4 | # Analysis Settings 5 | # False analyzes locus, lamp 6 | # True analyzes specific_methods 7 | specify_non_default_methods: True 8 | specific_methods: ["locus"] 9 | 10 | # Overall settings 11 | # To delete folder (so you don't have prompts to query whether to delete or not) 12 | b_clear_results_dir: True 13 | # Override to stop any plots from visalizing (plots can still save) 14 | all_visualization_off: False 15 | b_align_origin: False 16 | 17 | # Visualization Settings 18 | b_enable_dark_mode: False 19 | b_fullscreen: False 20 | 21 | # Raw data settings 22 | b_visualize_raw_data: False 23 | b_save_raw_data_plots: True 24 | b_visualize_normal_distributions: False 25 | b_save_raw_data_distribution_plots: True 26 | 27 | # Volume vs size 28 | b_plot_volume_vs_size: False 29 | b_save_volume_vs_size_plot: True 30 | 31 | # APE Settings 32 | b_compute_ape: True 33 | b_plot_combined_ape: True 34 | b_show_ape_plots: False 35 | b_save_individual_ape_plots: True 36 | b_save_combined_ape_plots: True 37 | 38 | # APE over distances Settings 39 | b_compute_ape_over_distances: False 40 | checkpoints: [100,200] 41 | b_plot_ape_over_distances: False 42 | 43 | # Distance metrics 44 | b_compute_distance_based_metrics: True 45 | b_show_distance_plots: False 46 | 47 | # RPE Settings 48 | b_compute_rpe: False 49 | b_plot_rpe: False 50 | 51 | # Loop Closure analysis 52 | b_loop_closure: False 53 | 54 | 55 | # Saving Settings 56 | b_save_results: True 57 | -------------------------------------------------------------------------------- /config/base_analyzer_settings.yaml: -------------------------------------------------------------------------------- 1 | #################### Analyzer Settings #################### 2 | 3 | 4 | b_compute_ape: True 5 | 6 | # Overall settings 7 | # To delete folder (so you don't have prompts to query whether to delete or not) 8 | b_clear_results_dir: True 9 | 10 | # Align origin for analysis 11 | b_align_origin: False 12 | 13 | robots: [husky1, husky4, spot1] 14 | # robots: [husky1, husky2, husky3, husky4] 15 | 16 | # APE Settings 17 | b_plot_combined_ape: True 18 | b_show_ape_plots: False 19 | b_save_individual_ape_plots: True 20 | b_save_combined_ape_plots: True 21 | 22 | # APE over distances Settings 23 | b_compute_ape_over_distances: False 24 | checkpoints: [100,200] 25 | b_plot_ape_over_distances: False 26 | 27 | # Distance metrics 28 | b_compute_distance_based_metrics: True 29 | b_show_distance_plots: False 30 | 31 | # RPE Settings 32 | b_compute_rpe: False 33 | b_plot_rpe: False 34 | 35 | # Saving Settings 36 | b_save_results: True 37 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | localization_analyzer 4 | 0.0.0 5 | Collection of scripts to analyze peformance against ground truth 6 | 7 | Benjamin Morrell 8 | Andrzej Reinke 9 | 10 | MIT 11 | 12 | catkin 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /scripts/analyzer.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Operates a full, and fully automatic analyis of user requested methods 4 | providing statistics on a broad range of metrics such as 5 | - Cpu Load 6 | - Memory Usage 7 | - Odometry Rate 8 | - Odometry Delay 9 | - Lidar Processing Time 10 | - Absolute Pose Error (APE) 11 | - Relative Pose Error (RPE) 12 | - APE over multiple distances 13 | - Volume/Size covered during exploration 14 | Author: 15 | - Matteo Palieri, NASA Jet Propulsion Laboratory 16 | - Andrzej Reinke, NASA Jet Propulsion Laboratory 17 | """ 18 | 19 | import sys 20 | import rospkg 21 | import shutil 22 | 23 | 24 | sys.path.insert(1, rospkg.RosPack().get_path('localization_analyzer') + "/utilities") 25 | from utilities import * 26 | 27 | def main(): 28 | 29 | global methods 30 | 31 | if len(sys.argv)<5: 32 | print("Example Usage: python analyzer.py data_dir robot_name ground_truth_topic run_number") 33 | sys.exit(1) 34 | 35 | data_dir = sys.argv[1] 36 | robot_name = sys.argv[2] 37 | ground_truth_topic = sys.argv[3] 38 | run_number = sys.argv[4] 39 | params = {} 40 | 41 | settings_filename = rospkg.RosPack().get_path('localization_analyzer') + "/config/analyzer_settings.yaml" 42 | 43 | print("\n#################") 44 | print("Starting Analyzer") 45 | print("#################\n") 46 | 47 | with open(settings_filename) as file: 48 | params = yaml.load(file, Loader=yaml.FullLoader) 49 | for k in params: 50 | print(k + " : " + str(params[k])) 51 | print("\n") 52 | 53 | base_station_name = params["base_station_name"] 54 | b_clear_results_dir = params["b_clear_results_dir"] 55 | b_enable_dark_mode = params["b_enable_dark_mode"] 56 | b_visualize_raw_data = params["b_visualize_raw_data"] 57 | b_save_raw_data_plots = params["b_save_raw_data_plots"] 58 | b_visualize_normal_distributions = params["b_visualize_normal_distributions"] 59 | b_save_raw_data_distribution_plots = params["b_save_raw_data_distribution_plots"] 60 | b_fullscreen = params["b_fullscreen"] 61 | b_plot_volume_vs_size = params["b_plot_volume_vs_size"] 62 | b_save_volume_vs_size_plot = params["b_save_volume_vs_size_plot"] 63 | 64 | b_save_traj_plots = ["b_save_traj_plots"] 65 | 66 | # APE 67 | b_compute_ape = params["b_compute_ape"] 68 | b_align_origin = params["b_align_origin"] 69 | b_plot_combined_ape = params["b_plot_combined_ape"] 70 | b_show_ape_plots = params["b_show_ape_plots"] 71 | b_save_individual_ape_plots = params["b_save_individual_ape_plots"] 72 | b_save_combined_ape_plots = params["b_save_combined_ape_plots"] 73 | b_compute_rpe = params["b_compute_rpe"] 74 | b_plot_rpe = params["b_plot_rpe"] 75 | checkpoints = params["checkpoints"] 76 | b_compute_ape_over_distances = params["b_compute_ape_over_distances"] 77 | b_plot_ape_over_distances = params["b_plot_ape_over_distances"] 78 | b_save_results = params["b_save_results"] 79 | 80 | # Distance metrics 81 | b_compute_distance_based_metrics = params["b_compute_distance_based_metrics"] 82 | b_show_distance_plots = params["b_show_distance_plots"] 83 | 84 | # Loop closure 85 | b_loop_closure = params["b_loop_closure"] 86 | 87 | if params["all_visualization_off"]: 88 | # Stops any plots from visalizating (plots can still save) 89 | b_visualize_raw_data = False 90 | b_visualize_normal_distributions = False 91 | b_plot_volume_vs_size = False 92 | b_plot_ape_over_distances = False 93 | 94 | results_dir = data_dir + "/results_" + run_number + "/" 95 | if not os.path.exists(results_dir): 96 | os.mkdir(results_dir) 97 | else: 98 | # Clean up 99 | if b_clear_results_dir: 100 | if os.path.exists(results_dir + "combined_ape.csv"): 101 | os.remove(results_dir + "combined_ape.csv") 102 | if os.path.exists(results_dir + "trans_ape.csv"): 103 | os.remove(results_dir + "trans_ape.csv") 104 | if os.path.exists(results_dir + "rot_ape.csv"): 105 | os.remove(results_dir + "rot_ape.csv") 106 | if os.path.exists(results_dir + "main_results.csv"): 107 | os.remove(results_dir + "main_results.csv") 108 | 109 | os.chdir(results_dir) 110 | 111 | # Make and clean plot directory 112 | plot_dir = data_dir + "/results_" + run_number + "/plots" 113 | if not os.path.exists(plot_dir): 114 | os.mkdir(plot_dir) 115 | else: 116 | shutil.rmtree(plot_dir) 117 | os.mkdir(plot_dir) 118 | 119 | default_case = not params["specify_non_default_methods"] 120 | 121 | if not default_case: 122 | for i in range(len(params["specific_methods"])): 123 | methods.append(params["specific_methods"][i]) 124 | else: 125 | # methods.append("lamp") 126 | methods.append("locus") 127 | 128 | all_cpus, all_mems, all_rates, all_delays, all_times = [], [], [], [], [] 129 | 130 | for method in methods: 131 | all_cpus.append([]) 132 | all_mems.append([]) 133 | all_rates.append([]) 134 | all_delays.append([]) 135 | all_times.append([]) 136 | 137 | counter = 0 138 | 139 | for method in methods: 140 | 141 | print("Profiling " + method + " ...\n") 142 | 143 | prefix = data_dir + "/" + method + "_" + run_number 144 | 145 | files = ["cpu.bag", "mem.bag", "time.bag", "rate.txt", "delay.txt"] 146 | available_files = [] 147 | 148 | for el in files: 149 | if el in os.listdir(prefix): 150 | available_files.append(el) 151 | else: 152 | print("\n----------------------------------------") 153 | print("WARNING " + el + " not found for " + method) 154 | print("----------------------------------------\n") 155 | 156 | cpu, mem, rate, delay, times = None, None, None, None, None 157 | 158 | for el in available_files: 159 | 160 | if el.split(".")[0] == "cpu": 161 | cpu = rosbag.Bag(prefix + "/" + el) 162 | for topic, msg, t in cpu.read_messages(): 163 | all_cpus[counter].append(msg.data) 164 | 165 | if el.split(".")[0] == "mem": 166 | mem = rosbag.Bag(prefix + "/" + el) 167 | for topic, msg, t in mem.read_messages(): 168 | all_mems[counter].append(msg.data/1e9) 169 | 170 | if el.split(".")[0] == "rate": 171 | rate = open(prefix + "/" + el) 172 | for r in rate: 173 | if "average rate:" in r: 174 | splitted = r.split(": ") 175 | if np.size(splitted) > 1: 176 | all_rates[counter].append(float(splitted[1])) 177 | 178 | if el.split(".")[0] == "delay": 179 | delay = open(prefix + "/" + el) 180 | for d in delay: 181 | if "average delay:" in d: 182 | splitted = d.split(": ") 183 | if np.size(splitted) > 1: 184 | all_delays[counter].append(float(splitted[1])) 185 | 186 | if el.split(".")[0] == "time": 187 | time = rosbag.Bag(prefix + "/" + el) 188 | for topic, msg, t in time.read_messages(): 189 | all_times[counter].append(msg.data) 190 | 191 | counter = counter + 1 192 | 193 | min_sizes = [] 194 | 195 | data = [all_cpus, all_mems, all_rates, all_delays, all_times] 196 | 197 | for entry in data: 198 | if (max(len(el) for el in entry) != 0): 199 | if (min(len(el) for el in entry if len(el) !=0) != 0): 200 | min_sizes.append(min(len(el) for el in entry if len(el) !=0)) 201 | else: 202 | min_sizes.append(0) 203 | 204 | all_cpus = [el[:min_sizes[0]] for el in all_cpus] 205 | all_mems = [el[:min_sizes[1]] for el in all_mems] 206 | all_rates = [el[:min_sizes[2]] for el in all_rates] 207 | all_delays = [el[:min_sizes[3]] for el in all_delays] 208 | all_times = [el[:min_sizes[4]] for el in all_times] 209 | 210 | distributions = evaluate_normal_distribution(data) 211 | cpu_results, mem_results, rate_results, delay_results, time_results = [], [], [], [], [] 212 | 213 | for d in range(len(distributions)): 214 | 215 | distribution = distributions[d] 216 | 217 | for i in range(len(methods)): 218 | 219 | min_val = distribution[i][0] 220 | max_val = distribution[i][1] 221 | avg_val = distribution[i][2] 222 | var_val = distribution[i][3] 223 | current_data = [min_val, max_val, avg_val, var_val] 224 | sigma = math.sqrt(var_val) 225 | x = np.linspace(avg_val - 3*sigma, avg_val + 3*sigma, 100) 226 | 227 | if d==0: 228 | cpu_results.append([x, stats.norm.pdf(x, avg_val, sigma), methods[i]]) 229 | if b_save_results: 230 | current_title = results_dir + "cpu_results_" + methods[i] + ".txt" 231 | save_txt(current_title, current_data) 232 | if d==1: 233 | mem_results.append([x, stats.norm.pdf(x, avg_val, sigma), methods[i]]) 234 | if b_save_results: 235 | current_title = results_dir + "mem_results_" + methods[i] + ".txt" 236 | save_txt(current_title, current_data) 237 | if d==2: 238 | rate_results.append([x, stats.norm.pdf(x, avg_val, sigma), methods[i]]) 239 | if b_save_results: 240 | current_title = results_dir + "rate_results_" + methods[i] + ".txt" 241 | save_txt(current_title, current_data) 242 | if d==3: 243 | delay_results.append([x, stats.norm.pdf(x, avg_val, sigma), methods[i]]) 244 | if b_save_results: 245 | current_title = results_dir + "delay_results_" + methods[i] + ".txt" 246 | save_txt(current_title, current_data) 247 | if d==4: 248 | time_results.append([x, stats.norm.pdf(x, avg_val, sigma), methods[i]]) 249 | if b_save_results: 250 | current_title = results_dir + "time_results_" + methods[i] + ".txt" 251 | save_txt(current_title, current_data) 252 | 253 | if b_enable_dark_mode: 254 | enable_dark_mode() 255 | 256 | if b_visualize_raw_data or b_save_raw_data_plots: 257 | visualize_raw_data("Raw_Cpu_Load", "CPU Load (% CPU)", all_cpus, results_dir, b_visualize_raw_data, b_fullscreen) 258 | visualize_raw_data("Raw_Memory_Load", "Virtual Memory (GB)", all_mems, results_dir, b_visualize_raw_data, b_fullscreen) 259 | visualize_raw_data("Raw_Odometry_Rate", "Rate (Hz)", all_rates, results_dir, b_visualize_raw_data, b_fullscreen) 260 | visualize_raw_data("Raw_Odometry_Delay", "Delay (s)", all_delays, results_dir, b_visualize_raw_data, b_fullscreen) 261 | visualize_raw_data("Raw_Lidar_Processing_Time", "Processing time (s)", all_times, results_dir, b_visualize_raw_data, b_fullscreen) 262 | 263 | if b_visualize_normal_distributions or b_save_raw_data_distribution_plots: 264 | all_results = {} 265 | all_results["cpu"] = [cpu_results, "Cpu_Load"] 266 | all_results["mem"] = [mem_results, "Memory_Load"] 267 | all_results["rate"] = [rate_results, "Odometry_Rate"] 268 | all_results["delay"] = [delay_results, "Odometry_Delay"] 269 | all_results["time"] = [time_results, "Lidar_Processing_Time"] 270 | visualize_and_save_normal_distributions(all_results, results_dir, b_visualize_normal_distributions, b_fullscreen) 271 | 272 | bandwidth_stats = {} 273 | if "lamp" in methods: 274 | if b_plot_volume_vs_size or b_save_volume_vs_size_plot: 275 | volume, size, timestamps = [], [], [] 276 | map_info = rosbag.Bag( data_dir + "/" + "lamp" + "_" + run_number + "/map_info.bag") 277 | for topic, msg, t in map_info.read_messages(): 278 | size.append(msg.size) 279 | volume.append(msg.volume) 280 | timestamps.append(t.to_sec()) 281 | timestamps = [el-timestamps[0] for el in timestamps] 282 | plot_volume_size(volume, size, timestamps, results_dir, b_plot_volume_vs_size, b_fullscreen) 283 | pose_graph_to_odometry(data_dir + "/" + "lamp" + "_" + run_number + "/", robot_name) 284 | # track keyed scans bandwidth 285 | bandwidth_stats = analyze_bandwidth(data_dir + "/" + "lamp" + "_" + run_number, results_dir) 286 | 287 | # analyze loop closures 288 | pose_graph_rosbag = data_dir + "/" + "lamp" + "_" + run_number + "/pose_graph.bag" 289 | pose_graph_topic = "/" + robot_name + "/lamp/pose_graph" 290 | loop_closure_rosbag = data_dir + "/" + "lamp" + "_" + run_number + "/loop_closures.bag" 291 | loop_closure_topic = "/" + robot_name + "/lamp/laser_loop_closures" 292 | gt_odom_rosbag = data_dir + "/ground_truth/odometry.bag" 293 | gt_odom_topic = ground_truth_topic 294 | if b_loop_closure: 295 | evaluate_loop_closures( 296 | pose_graph_rosbag, pose_graph_topic, loop_closure_rosbag, loop_closure_topic, gt_odom_rosbag, gt_odom_topic, results_dir) 297 | 298 | all_methods_string = " " 299 | for method in methods: 300 | all_methods_string = all_methods_string + method + " " 301 | 302 | bash_command = "python $(rospack find localization_analyzer)/utilities/aggregate_odometries.py " + data_dir + " " + robot_name + " " + ground_truth_topic + " " + run_number 303 | os.system(bash_command) 304 | 305 | #----------------------------------------------------------------------------------------------------------- 306 | ### Plot trajectories ### 307 | #----------------------------------------------------------------------------------------------------------- 308 | 309 | 310 | bash_command = "evo_config set plot_fontfamily arial plot_fontscale 1.5 plot_seaborn_style whitegrid plot_figsize 14 10" 311 | os.system(bash_command) 312 | evo_options_string = make_evo_options_string("traj", "", b_align_origin, b_show_ape_plots, b_save_traj_plots) 313 | bash_command = "evo_traj bag aggregated_odometries.bag ground_truth" + all_methods_string + "--ref ground_truth " + evo_options_string 314 | os.system(bash_command) 315 | 316 | #----------------------------------------------------------------------------------------------------------- 317 | ### APE Processing with EVO ### 318 | #----------------------------------------------------------------------------------------------------------- 319 | if b_compute_ape: 320 | # Create directory 321 | ape_dir = data_dir + "/results_" + run_number + "/ape_results" 322 | if not os.path.exists(ape_dir): 323 | os.mkdir(ape_dir) 324 | else: 325 | shutil.rmtree(ape_dir) 326 | os.mkdir(ape_dir) 327 | 328 | 329 | # Get APE for each method 330 | for method in methods: 331 | # Make options string from params 332 | evo_options_string = make_evo_options_string("single", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 333 | 334 | # Run bash command to compute APE for each method 335 | bash_command = "evo_ape bag aggregated_odometries.bag ground_truth " + method + " " + evo_options_string + " --save_results " + ape_dir + "/" + method + ".zip" 336 | os.system(bash_command) 337 | if b_plot_combined_ape: 338 | # Make options string from params 339 | evo_options_string = make_evo_options_string("combined", "", False, b_show_ape_plots, b_save_combined_ape_plots) 340 | 341 | # Run bash command to show combined results 342 | bash_command = "evo_res ape_results/*.zip --use_filenames" + " " + evo_options_string 343 | os.system(bash_command) 344 | for method in methods: 345 | # Run for translation only 346 | evo_options_string = make_evo_options_string("trans", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 347 | bash_command = "evo_ape bag aggregated_odometries.bag ground_truth " + method + " --pose_relation trans_part" + evo_options_string + " --save_results " + ape_dir + "/" + method + "_trans.zip" 348 | os.system(bash_command) 349 | # Run for rotation only 350 | evo_options_string = make_evo_options_string("rot", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 351 | bash_command = "evo_ape bag aggregated_odometries.bag ground_truth " + method + " --pose_relation angle_deg" + evo_options_string + " --save_results " + ape_dir + "/" + method + "_rot.zip" 352 | os.system(bash_command) 353 | # Run bash command to show combined results 354 | evo_options_string = make_evo_options_string("combined_trans", "", False, False, b_save_combined_ape_plots) 355 | bash_command = "evo_res ape_results/*trans.zip --use_filenames" + " " + evo_options_string 356 | os.system(bash_command) 357 | evo_options_string = make_evo_options_string("combined_rot", "", False, False, b_save_combined_ape_plots) 358 | bash_command = "evo_res ape_results/*rot.zip --use_filenames" + " " + evo_options_string 359 | os.system(bash_command) 360 | 361 | #----------------------------------------------------------------------------------------------------------- 362 | ### Distance based error metrics ### 363 | #----------------------------------------------------------------------------------------------------------- 364 | if b_compute_distance_based_metrics: 365 | # Extract zip contents 366 | dest = results_dir + "ape_results" 367 | for el in os.listdir(dest): 368 | if os.path.isfile(dest + "/" + el) and el.split(".")[1] == "zip": 369 | with zipfile.ZipFile(dest + "/" + el, 'r') as zip_ref: 370 | zip_ref.extractall(dest + "/" + el.split(".")[0]) 371 | 372 | # Compute the distance metrics 373 | err_perc = compute_distance_based_error_metrics(methods, results_dir, b_show_distance_plots, b_fullscreen) 374 | 375 | 376 | #----------------------------------------------------------------------------------------------------------- 377 | ### APE At checkpoints with EVO ### 378 | #----------------------------------------------------------------------------------------------------------- 379 | if b_compute_ape_over_distances: 380 | segment_aggregated_odometries(methods, checkpoints) 381 | for checkpoint in checkpoints: 382 | bash_command = "bash $(rospack find localization_analyzer)/utilities/ape_from_aggregated_odometries_parser.bash " + \ 383 | "aggregated_odometries_" + str(checkpoint) + ".bag" + " " + \ 384 | "ape_results_" + str(checkpoint) + " " + all_methods_string 385 | os.system(bash_command) 386 | dest = os.getcwd() + "/ape_results_" + str(checkpoint) 387 | for el in os.listdir(dest): 388 | if os.path.isfile(dest + "/" + el) and el.split(".")[1] == "zip": 389 | with zipfile.ZipFile(dest + "/" + el, 'r') as zip_ref: 390 | zip_ref.extractall(dest + "/" + el.split(".")[0]) 391 | if b_plot_ape_over_distances: 392 | boxplot_from_npz(methods, checkpoints, results_dir, b_show_ape_plots, b_fullscreen) 393 | 394 | #----------------------------------------------------------------------------------------------------------- 395 | ### RPE Processing with EVO ### 396 | #----------------------------------------------------------------------------------------------------------- 397 | if b_compute_rpe: 398 | # Create directory 399 | rpe_dir = data_dir + "/results_" + run_number + "/rpe_results" 400 | if not os.path.exists(rpe_dir): 401 | os.mkdir(rpe_dir) 402 | else: 403 | shutil.rmtree(rpe_dir) 404 | os.mkdir(rpe_dir) 405 | 406 | # Get RPE for each method 407 | for method in methods: 408 | # Make options string from params 409 | # evo_options_string = make_evo_options_string("single", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 410 | 411 | # Run bash command to compute APE for each method 412 | bash_command = "evo_rpe bag aggregated_odometries.bag ground_truth " + method + " --delta 1 --delta_unit m --save_results " + rpe_dir + "/" + method + ".zip" 413 | os.system(bash_command) 414 | if b_plot_rpe: 415 | # Make options string from params 416 | # evo_options_string = make_evo_options_string("combined", "", b_align_origin, b_show_rpe_plots, b_plot_rpe) 417 | 418 | # Run bash command to show combined results 419 | bash_command = "evo_res rpe_results/*.zip --merge --use_filenames" #+ " " + evo_options_string 420 | os.system(bash_command) 421 | 422 | 423 | # bash_command = "bash $(rospack find localization_analyzer)/utilities/rpe_from_aggregated_odometries.bash " + all_methods_string 424 | # os.system(bash_command) 425 | # if b_plot_rpe: 426 | # bash_command = "bash $(rospack find localization_analyzer)/utilities/plot_rpe_results.bash " + all_methods_string 427 | # os.system(bash_command) 428 | 429 | #----------------------------------------------------------------------------------------------------------- 430 | ### Export main stats to CSV ### 431 | #----------------------------------------------------------------------------------------------------------- 432 | if b_save_results: 433 | write_main_results_to_csv(methods, results_dir, err_perc, distributions, bandwidth_stats) 434 | # creatOutputPDF(results_dir) 435 | 436 | 437 | if __name__=="__main__": 438 | main() 439 | -------------------------------------------------------------------------------- /scripts/base_analyzer.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Simple analysis of base rerun data. Mostly loop closure evaluations. 4 | Author: 5 | - Yun Chang 6 | """ 7 | 8 | import sys 9 | import rospkg 10 | import shutil 11 | 12 | sys.path.insert(1, rospkg.RosPack().get_path('localization_analyzer') + "/utilities") 13 | from utilities import * 14 | 15 | def aggregate_odom(data_dir, gt_dir, robot_name, run_number): 16 | bag_topics = {} 17 | # ground truth bag and topic 18 | bag_topics['ground_truth'] = [gt_dir + "/odometry.bag", "/" + robot_name + "/lo_frontend/odometry"] 19 | bag_topics['LO'] = [data_dir + "/" + robot_name + "_locus/odometry.bag", "/" + robot_name + "/lo_frontend/odometry"] 20 | bag_topics['LAMP'] = [data_dir + "/lamp_" + run_number + "/odometry.bag", "/" + robot_name + "/lamp/odometry"] 21 | 22 | with rosbag.Bag("aggregated_odometries.bag", "w") as outbag: 23 | for key in bag_topics.keys(): 24 | print("Aggregating " + key + " ...") 25 | bag = rosbag.Bag(bag_topics[key][0]) 26 | if key != "ground_truth": 27 | bag_topics[key][1] = bag.get_type_and_topic_info()[1].keys()[0] 28 | print("Topic for " + key + " odometry is " + bag_topics[key][1] + "\n") 29 | for topic, msg, t in bag.read_messages(topics=[bag_topics[key][1]]): 30 | outbag.write(key, msg, t) 31 | outbag.close() 32 | 33 | def creatOutputPdf(results_dir): 34 | 35 | # Convert images desired into pdf 36 | ape_rot = results_dir + "/plots/APERotvsDistance.png" 37 | imageToPDF(ape_rot) 38 | ape_trans = results_dir + "/plots/APETransvsDistance.png" 39 | imageToPDF(ape_trans) 40 | 41 | # Create pdf and start adding pages 42 | from PyPDF2 import PdfFileReader, PdfFileWriter 43 | pdf_writer = PdfFileWriter() 44 | 45 | # Add trajectory 46 | input_pdf = PdfFileReader(results_dir + "/plots/traj_plots.pdf") 47 | pdf_writer.addPage(input_pdf.getPage(0)) 48 | 49 | # Add colored error plots 50 | input_pdf = PdfFileReader(results_dir + "/plots/LAMP_ape_plots.pdf") 51 | pdf_writer.addPage(input_pdf.getPage(1)) 52 | 53 | # Add error vs distance 54 | input_pdf = PdfFileReader(results_dir + "/plots/APETransvsDistance.pdf") 55 | pdf_writer.addPage(input_pdf.getPage(0)) 56 | input_pdf = PdfFileReader(results_dir + "/plots/APERotvsDistance.pdf") 57 | pdf_writer.addPage(input_pdf.getPage(0)) 58 | 59 | # Add box plot 60 | input_pdf = PdfFileReader(results_dir + "/plots/combined_ape_plots.pdf") 61 | pdf_writer.addPage(input_pdf.getPage(3)) 62 | 63 | # More Stats on overall error 64 | input_pdf = PdfFileReader(results_dir + "/plots/combined_ape_plots.pdf") 65 | pdf_writer.addPage(input_pdf.getPage(1)) 66 | pdf_writer.addPage(input_pdf.getPage(4)) 67 | 68 | # X, Y, Z error 69 | input_pdf = PdfFileReader(results_dir + "/plots/traj_plots.pdf") 70 | pdf_writer.addPage(input_pdf.getPage(1)) 71 | 72 | # Write to file 73 | with open(results_dir + "/main_results.pdf", 'wb') as outfile: 74 | pdf_writer.write(outfile) 75 | 76 | 77 | def main(): 78 | 79 | global methods 80 | 81 | if len(sys.argv)<4: 82 | print("Example Usage: python base_analyzer.py data_dir base_name run_number") 83 | sys.exit(1) 84 | 85 | data_dir = sys.argv[1] 86 | base_name = sys.argv[2] 87 | run_number = sys.argv[3] 88 | params = {} 89 | 90 | settings_filename = rospkg.RosPack().get_path('localization_analyzer') + "/config/base_analyzer_settings.yaml" 91 | 92 | with open(settings_filename) as file: 93 | params = yaml.load(file, Loader=yaml.FullLoader) 94 | for k in params: 95 | print(k + " : " + str(params[k])) 96 | print("\n") 97 | 98 | print("\n#######################") 99 | print("Starting Base Analyzer") 100 | print("#######################\n") 101 | 102 | b_clear_results_dir = params["b_clear_results_dir"] 103 | b_compute_ape = params["b_compute_ape"] 104 | b_align_origin = params["b_align_origin"] 105 | 106 | b_plot_combined_ape = params["b_plot_combined_ape"] 107 | b_show_ape_plots = params["b_show_ape_plots"] 108 | b_save_individual_ape_plots = params["b_save_individual_ape_plots"] 109 | b_save_combined_ape_plots = params["b_save_combined_ape_plots"] 110 | b_compute_rpe = params["b_compute_rpe"] 111 | b_plot_rpe = params["b_plot_rpe"] 112 | checkpoints = params["checkpoints"] 113 | b_compute_ape_over_distances = params["b_compute_ape_over_distances"] 114 | b_plot_ape_over_distances = params["b_plot_ape_over_distances"] 115 | b_save_results = params["b_save_results"] 116 | 117 | # Distance metrics 118 | b_compute_distance_based_metrics = params["b_compute_distance_based_metrics"] 119 | b_show_distance_plots = params["b_show_distance_plots"] 120 | 121 | b_save_traj_plots = ["b_save_traj_plots"] 122 | 123 | 124 | robots = [] 125 | gt_files = [] 126 | for i in range(len(params["robots"])): 127 | robots.append(params["robots"][i]) 128 | gt_files.append(data_dir + "/ground_truth/" + params["robots"][i]) 129 | 130 | results_dir = data_dir + "/results_" + run_number + "/" 131 | if not os.path.exists(results_dir): 132 | os.mkdir(results_dir) 133 | os.chdir(results_dir) 134 | 135 | # analyze loop closures 136 | pose_graph_rosbag = data_dir + "/" + "lamp" + "_" + run_number + "/pose_graph.bag" 137 | pose_graph_topic = "/" + base_name + "/lamp/pose_graph" 138 | loop_closure_rosbag = data_dir + "/" + "lamp" + "_" + run_number + "/loop_closures.bag" 139 | loop_closure_topic = "/" + base_name + "/lamp/laser_loop_closures" 140 | get_loop_closures_stats( 141 | pose_graph_rosbag, pose_graph_topic, loop_closure_rosbag, loop_closure_topic, results_dir) 142 | 143 | # If we do not have ground truth 144 | if not b_compute_ape: 145 | return 146 | 147 | methods = ["LO", "LAMP"] 148 | all_methods_string = " " 149 | for method in methods: 150 | all_methods_string = all_methods_string + method + " " 151 | 152 | # Convert from pose graph to odom for each robot 153 | for i in range(len(robots)): 154 | robot_name = robots[i] 155 | gt_file = gt_files[i] 156 | 157 | results_dir = data_dir + "/results_" + run_number + "/" + robot_name + "/" 158 | if not os.path.exists(results_dir): 159 | os.mkdir(results_dir) 160 | else: 161 | # Clean up 162 | if b_clear_results_dir: 163 | if os.path.exists(results_dir + "combined_ape.csv"): 164 | os.remove(results_dir + "combined_ape.csv") 165 | if os.path.exists(results_dir + "trans_ape.csv"): 166 | os.remove(results_dir + "trans_ape.csv") 167 | if os.path.exists(results_dir + "rot_ape.csv"): 168 | os.remove(results_dir + "rot_ape.csv") 169 | if os.path.exists(results_dir + "main_results.csv"): 170 | os.remove(results_dir + "main_results.csv") 171 | 172 | os.chdir(results_dir) 173 | 174 | # Make and clean plot directory 175 | plot_dir = results_dir + "plots" 176 | if not os.path.exists(plot_dir): 177 | os.mkdir(plot_dir) 178 | else: 179 | shutil.rmtree(plot_dir) 180 | os.mkdir(plot_dir) 181 | 182 | pose_graph_to_odometry(data_dir + "/" + "lamp" + "_" + run_number + "/", robot_name) 183 | aggregate_odom(data_dir, gt_file, robot_name, run_number) 184 | 185 | #----------------------------------------------------------------------------------------------------------- 186 | ### Plot trajectories ### 187 | #----------------------------------------------------------------------------------------------------------- 188 | 189 | bash_command = "evo_config set plot_fontfamily arial plot_fontscale 1.5 plot_seaborn_style whitegrid plot_figsize 14 10" 190 | os.system(bash_command) 191 | evo_options_string = make_evo_options_string("traj", "", b_align_origin, b_show_ape_plots, b_save_traj_plots) 192 | bash_command = "evo_traj bag aggregated_odometries.bag ground_truth" + all_methods_string + "--ref ground_truth " + evo_options_string 193 | os.system(bash_command) 194 | 195 | #----------------------------------------------------------------------------------------------------------- 196 | ### APE Processing with EVO ### 197 | #----------------------------------------------------------------------------------------------------------- 198 | if b_compute_ape: 199 | # Create directory 200 | ape_dir = results_dir + "/ape_results" 201 | if not os.path.exists(ape_dir): 202 | os.mkdir(ape_dir) 203 | else: 204 | shutil.rmtree(ape_dir) 205 | os.mkdir(ape_dir) 206 | 207 | 208 | # Get APE for each method 209 | for method in methods: 210 | # Make options string from params 211 | evo_options_string = make_evo_options_string("single", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 212 | 213 | # Run bash command to compute APE for each method 214 | bash_command = "evo_ape bag aggregated_odometries.bag ground_truth " + method + " " + evo_options_string + " --save_results " + ape_dir + "/" + method + ".zip" 215 | os.system(bash_command) 216 | if b_plot_combined_ape: 217 | # Make options string from params 218 | evo_options_string = make_evo_options_string("combined", "", False, b_show_ape_plots, b_save_combined_ape_plots) 219 | 220 | # Run bash command to show combined results 221 | bash_command = "evo_res ape_results/*.zip --use_filenames" + " " + evo_options_string 222 | os.system(bash_command) 223 | for method in methods: 224 | # Run for translation only 225 | evo_options_string = make_evo_options_string("trans", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 226 | bash_command = "evo_ape bag aggregated_odometries.bag ground_truth " + method + " --pose_relation trans_part" + evo_options_string + " --save_results " + ape_dir + "/" + method + "_trans.zip" 227 | os.system(bash_command) 228 | # Run for rotation only 229 | evo_options_string = make_evo_options_string("rot", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 230 | bash_command = "evo_ape bag aggregated_odometries.bag ground_truth " + method + " --pose_relation angle_deg" + evo_options_string + " --save_results " + ape_dir + "/" + method + "_rot.zip" 231 | os.system(bash_command) 232 | # Run bash command to show combined results 233 | evo_options_string = make_evo_options_string("combined_trans", "", False, False, b_save_combined_ape_plots) 234 | bash_command = "evo_res ape_results/*trans.zip --use_filenames" + " " + evo_options_string 235 | os.system(bash_command) 236 | evo_options_string = make_evo_options_string("combined_rot", "", False, False, b_save_combined_ape_plots) 237 | bash_command = "evo_res ape_results/*rot.zip --use_filenames" + " " + evo_options_string 238 | os.system(bash_command) 239 | 240 | #----------------------------------------------------------------------------------------------------------- 241 | ### Distance based error metrics ### 242 | #----------------------------------------------------------------------------------------------------------- 243 | if b_compute_distance_based_metrics: 244 | # Extract zip contents 245 | dest = results_dir + "ape_results" 246 | for el in os.listdir(dest): 247 | if os.path.isfile(dest + "/" + el) and el.split(".")[1] == "zip": 248 | with zipfile.ZipFile(dest + "/" + el, 'r') as zip_ref: 249 | zip_ref.extractall(dest + "/" + el.split(".")[0]) 250 | 251 | # Compute the distance metrics 252 | err_perc = compute_distance_based_error_metrics(methods, results_dir, b_show_distance_plots, False) 253 | 254 | 255 | #----------------------------------------------------------------------------------------------------------- 256 | ### APE At checkpoints with EVO ### 257 | #----------------------------------------------------------------------------------------------------------- 258 | if b_compute_ape_over_distances: 259 | segment_aggregated_odometries(methods, checkpoints) 260 | for checkpoint in checkpoints: 261 | bash_command = "bash $(rospack find localization_analyzer)/utilities/ape_from_aggregated_odometries_parser.bash " + \ 262 | "aggregated_odometries_" + str(checkpoint) + ".bag" + " " + \ 263 | "ape_results_" + str(checkpoint) + " " + all_methods_string 264 | os.system(bash_command) 265 | dest = os.getcwd() + "/ape_results_" + str(checkpoint) 266 | for el in os.listdir(dest): 267 | if os.path.isfile(dest + "/" + el) and el.split(".")[1] == "zip": 268 | with zipfile.ZipFile(dest + "/" + el, 'r') as zip_ref: 269 | zip_ref.extractall(dest + "/" + el.split(".")[0]) 270 | if b_plot_ape_over_distances: 271 | boxplot_from_npz(methods, checkpoints, results_dir, b_show_ape_plots, False) 272 | 273 | #----------------------------------------------------------------------------------------------------------- 274 | ### RPE Processing with EVO ### 275 | #----------------------------------------------------------------------------------------------------------- 276 | if b_compute_rpe: 277 | # Create directory 278 | rpe_dir = results_dir + "/rpe_results" 279 | if not os.path.exists(rpe_dir): 280 | os.mkdir(rpe_dir) 281 | else: 282 | shutil.rmtree(rpe_dir) 283 | os.mkdir(rpe_dir) 284 | 285 | # Get RPE for each method 286 | for method in methods: 287 | # Make options string from params 288 | # evo_options_string = make_evo_options_string("single", method, b_align_origin, b_show_ape_plots, b_save_individual_ape_plots) 289 | 290 | # Run bash command to compute APE for each method 291 | bash_command = "evo_rpe bag aggregated_odometries.bag ground_truth " + method + " --delta 1 --delta_unit m --save_results " + rpe_dir + "/" + method + ".zip" 292 | os.system(bash_command) 293 | if b_plot_rpe: 294 | # Make options string from params 295 | # evo_options_string = make_evo_options_string("combined", "", b_align_origin, b_show_rpe_plots, b_plot_rpe) 296 | 297 | # Run bash command to show combined results 298 | bash_command = "evo_res rpe_results/*.zip --merge --use_filenames" #+ " " + evo_options_string 299 | os.system(bash_command) 300 | 301 | 302 | # bash_command = "bash $(rospack find localization_analyzer)/utilities/rpe_from_aggregated_odometries.bash " + all_methods_string 303 | # os.system(bash_command) 304 | # if b_plot_rpe: 305 | # bash_command = "bash $(rospack find localization_analyzer)/utilities/plot_rpe_results.bash " + all_methods_string 306 | # os.system(bash_command) 307 | 308 | #----------------------------------------------------------------------------------------------------------- 309 | ### Export main stats to CSV ### 310 | #----------------------------------------------------------------------------------------------------------- 311 | if b_save_results: 312 | creatOutputPdf(results_dir) 313 | 314 | # Remove aggregated_odometries 315 | os.remove(results_dir + "aggregated_odometries.bag") 316 | 317 | 318 | 319 | 320 | if __name__=="__main__": 321 | main() -------------------------------------------------------------------------------- /scripts/pose_graph_loop_closure_analysis.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Analyse a set of pose-graph bags to evaluate loop closure performance 4 | Author: 5 | - Benjamin Morrell, NASA Jet Propulsion Laboratory 6 | """ 7 | 8 | import sys 9 | import rosbag 10 | import numpy as np 11 | import ctypes 12 | import matplotlib.pyplot as plt 13 | from geometry_msgs.msg import Pose 14 | 15 | 16 | keyBits = ctypes.sizeof(ctypes.c_uint64) * 8 17 | chrBits = ctypes.sizeof(ctypes.c_ubyte) * 8 18 | indexBits = keyBits - chrBits 19 | chrMask = np.uint64(np.int64(~np.ubyte(0)) << indexBits) 20 | indexMask = ~chrMask 21 | 22 | # TODO - consider rotaitonal differences 23 | 24 | def split_pg_key(pg_key): 25 | c_ = chr(np.ubyte(np.int64(np.uint64(pg_key) & chrMask) >> indexBits)) 26 | j_ = np.uint64(pg_key) & indexMask 27 | return c_, j_ 28 | # In [79]: split_pg_key(7061644215716937728) 29 | # Out[79]: ('b', 0) 30 | 31 | def ComputeTransform(keys, node_poses): 32 | 33 | # Check keys exist 34 | if keys[0] not in node_poses: 35 | print "error - key 1, ", split_pg_key(keys[0]), "not in node_poses" 36 | if keys[1] not in node_poses: 37 | print "error - key 2, ", split_pg_key(keys[1]), "not in node_poses" 38 | 39 | node1 = node_poses[keys[0]] 40 | node2 = node_poses[keys[1]] 41 | 42 | # initialize 43 | transform = Pose() 44 | # Fill information 45 | transform.position.x = node2.position.x - node1.position.x 46 | transform.position.y = node2.position.y - node1.position.y 47 | transform.position.z = node2.position.z - node1.position.z 48 | # TODO - analyse rotational difference 49 | transform.orientation.x = 0.0 50 | transform.orientation.y = 0.0 51 | transform.orientation.z = 0.0 52 | transform.orientation.w = 1.0 53 | 54 | return transform 55 | 56 | 57 | def ComputeDifferenceTranslation(transform1, transform2): 58 | # compute the difference in transforms 59 | difference = np.sqrt((transform1.position.x - transform2.position.x)**2 + 60 | (transform1.position.y - transform2.position.y)**2 + 61 | (transform1.position.z - transform2.position.z)**2) 62 | 63 | return difference 64 | 65 | def printPGKeys(keys): 66 | print "PG keys are:", split_pg_key(keys[0]), " and ", split_pg_key(keys[1]) 67 | # print(*split_pg_key(keys[1]), sep='') 68 | 69 | def AnalyzeLoopClosures(bag, robot_name): 70 | topic_list = ["/" + robot_name + "/lamp/pose_graph_to_optimize", 71 | "/" + robot_name + "/lamp_pgo/optimized_values"] 72 | 73 | pg_to_opt = {} 74 | opt_vals = {} 75 | 76 | # Loop through topics in the bag 77 | for topic, msg, t in bag.read_messages(topics=topic_list): 78 | if topic == topic_list[0]: 79 | # pg to opt 80 | pg_to_opt[msg.header.seq] = msg 81 | else: 82 | opt_vals[msg.header.seq] = msg 83 | 84 | 85 | # Loop through the pg_to_opt 86 | seq_list = pg_to_opt.keys() 87 | 88 | loop_closures = {} 89 | new_loops = [] 90 | node_poses_pre = {} 91 | node_poses_post = {} 92 | needed_keys = {} 93 | 94 | for seq in seq_list: 95 | factors = pg_to_opt[seq].edges 96 | nodes = pg_to_opt[seq].nodes 97 | 98 | # Find loop closures 99 | for edge in factors: 100 | if edge.type != 3: # loop closure factor 101 | continue 102 | 103 | # Have a loop closure 104 | id = (edge.key_from,edge.key_to) 105 | 106 | if id not in loop_closures: 107 | loop_closures[id] = [edge.pose] 108 | new_loops.append(id) 109 | needed_keys[edge.key_from] = True 110 | needed_keys[edge.key_to] = True 111 | printPGKeys(id) 112 | 113 | # Get pose-nodes 114 | for node in nodes: 115 | if node.key not in needed_keys.keys(): 116 | continue 117 | node_poses_pre[node.key] = node.pose 118 | 119 | # Get opt node poses 120 | for opt_node in opt_vals[seq].nodes: 121 | if opt_node.key not in needed_keys.keys(): 122 | continue 123 | node_poses_post[opt_node.key] = opt_node.pose 124 | 125 | # Loop through new loop closures 126 | for keys in new_loops: 127 | # get the transform before 128 | loop_closures[keys].append(ComputeTransform(keys,node_poses_pre)) 129 | 130 | # Get the transform after optimization 131 | loop_closures[keys].append(ComputeTransform(keys,node_poses_post)) 132 | 133 | # reset 134 | new_loops = [] 135 | needed_keys = {} 136 | node_poses_pre = {} 137 | node_poses_post = {} 138 | 139 | # end loop 140 | 141 | # Go through loops to do analysis 142 | start_to_lc_edge = {} 143 | start_to_opt = {} 144 | opt_to_lc_edge = {} 145 | 146 | for keys in loop_closures.keys(): 147 | transforms = loop_closures[keys] 148 | 149 | start_to_lc_edge[keys] = ComputeDifferenceTranslation(transforms[1],transforms[0]) 150 | start_to_opt[keys] = ComputeDifferenceTranslation(transforms[1],transforms[2]) 151 | opt_to_lc_edge[keys] = ComputeDifferenceTranslation(transforms[2],transforms[0]) 152 | 153 | # Write stats to csv 154 | 155 | print "Completed processing" 156 | 157 | # plot 158 | fig = plt.figure() 159 | ax = fig.gca() 160 | 161 | ax.plot(start_to_lc_edge.values(), label='Difference from start to loop computation') 162 | ax.plot(start_to_opt.values(), label='Difference from start to optimized') 163 | # ax.plot(opt_to_lc_edge.values(), label='Difference between optimized and computed') 164 | ax.set_xlabel('loop') 165 | ax.set_ylabel('difference (m)') 166 | ax.grid(b=True, which='both', color='0.65', linestyle='-') 167 | 168 | plt.legend() 169 | 170 | plt.show() 171 | 172 | 173 | 174 | def main(): 175 | 176 | if len(sys.argv)<3: 177 | print("Example Usage: python pose_graph_loop_closure_analysis.py input.bag robot_names") 178 | sys.exit(1) 179 | 180 | bag = rosbag.Bag(sys.argv[1]) 181 | robot_name = sys.argv[2] 182 | 183 | AnalyzeLoopClosures(bag, robot_name) 184 | 185 | # with rosbag.Bag(out_bag, "w") as outbag: 186 | # outbag.write(topic, first_msg) 187 | # outbag.close() 188 | 189 | 190 | if __name__=="__main__": 191 | main() 192 | -------------------------------------------------------------------------------- /scripts/recover_fiducial_calibration_from_pose_graph.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Recovers fiducial calibration from pose_graph.bag 4 | Author: 5 | - Matteo Palieri, NASA Jet Propulsion Laboratory 6 | """ 7 | 8 | import sys 9 | import rosbag 10 | 11 | def main(): 12 | 13 | if len(sys.argv)<3: 14 | print("Example Usage: python recover_fiducial_calibration_from_pose_graph.py pose_graph.bag pose_graph_topic") 15 | sys.exit(1) 16 | 17 | pose_graph_bag = rosbag.Bag(sys.argv[1]) 18 | pose_graph_topic = sys.argv[2] 19 | 20 | position, orientation = None, None 21 | 22 | for topic, msg, t in pose_graph_bag.read_messages(topics=[pose_graph_topic]): 23 | for node in msg.nodes: 24 | if (node.ID=="odom_node"): 25 | position = node.pose.position 26 | orientation = node.pose.orientation 27 | break 28 | break 29 | 30 | if position is not None and orientation is not None: 31 | print("*******Fiducial Calibration*******\n") 32 | print("orientation: \n" + str(orientation) + "\n") 33 | print("position: \n" + str(position) + "\n") 34 | print("**********************************\n") 35 | else: 36 | print("Unable to recover fiducial calibration from pose_graph.bag") 37 | 38 | # TODO: Save to .ros folder for $ROBOT_NAME 39 | 40 | if __name__=="__main__": 41 | main() -------------------------------------------------------------------------------- /scripts/run_analyzer.yaml: -------------------------------------------------------------------------------- 1 | session_name: run_localization_analyzer 2 | 3 | environment: 4 | DATA_PATH: /home/andrzej/Datasets/test_localizer_gt/ 5 | GT_PATH: /home/andrzej/Datasets/test_localizer_gt/ 6 | ROBOT_NAME: husky4 7 | ODOM: lo_frontend/odometry 8 | RUN_NUMBER: "test_out" 9 | 10 | options: 11 | default-command: /bin/bash 12 | 13 | windows: 14 | - window_name: analyze 15 | focus: false 16 | layout: tiled 17 | shell_command_before: 18 | - mkdir $DATA_PATH/ground_truth 19 | - cp $GT_PATH/odometry.bag $DATA_PATH/ground_truth/ 20 | panes: 21 | - python $(rospack find localization_analyzer)/scripts/analyzer.py $DATA_PATH $ROBOT_NAME /$ROBOT_NAME/$ODOM $RUN_NUMBER; -------------------------------------------------------------------------------- /utilities/aggregate_odometries.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Aggregates user requested odometries along with ground truth odometry 4 | in aggregated_odometries.bag 5 | Author: 6 | - Matteo Palieri, NASA Jet Propulsion Laboratory 7 | - Andrzej Reinke, NASA Jet Propulsion Laboratory 8 | """ 9 | 10 | import os 11 | import sys 12 | import rosbag 13 | import rospkg 14 | import yaml 15 | 16 | def main(): 17 | 18 | methods = [] 19 | 20 | # if len(sys.argv)<5: 21 | # print("Minimal Usage: python aggregate_odometries.py settings_filename") 22 | # print("Example Usage: python aggregate_odometries.py robot_name method1 method2 method3 method4") 23 | # sys.exit(1) 24 | if len(sys.argv)<3: 25 | print("Example Usage: python aggregate_odometries.py data_dir robot_name ground_truth_topic run_number") 26 | sys.exit(1) 27 | 28 | data_dir = sys.argv[1] 29 | robot_name = sys.argv[2] 30 | ground_truth_topic = sys.argv[3] 31 | run_number = sys.argv[4] 32 | params = {} 33 | 34 | settings_filename = rospkg.RosPack().get_path('localization_analyzer') + "/config/analyzer_settings.yaml" 35 | 36 | params = {} 37 | 38 | with open(settings_filename) as file: 39 | params = yaml.load(file, Loader=yaml.FullLoader) 40 | 41 | 42 | 43 | results_dir = data_dir + "/results_" + run_number + "/" 44 | if not os.path.exists(results_dir): 45 | os.mkdir(results_dir) 46 | 47 | os.chdir(results_dir) 48 | 49 | default_case = not params["specify_non_default_methods"] 50 | 51 | methods.append("ground_truth") 52 | 53 | if not default_case: 54 | for i in range(len(params["specific_methods"])): 55 | methods.append(params["specific_methods"][i]) 56 | else: 57 | # methods.append("lamp") 58 | methods.append("locus") 59 | 60 | d = {} 61 | 62 | for method in methods: 63 | if method == "ground_truth": 64 | bag_name = data_dir + "/" + method + "/odometry.bag" 65 | topic_name = ground_truth_topic 66 | else: 67 | bag_name = data_dir + "/" + method + "_" + run_number + "/odometry.bag" 68 | topic_name = "/" + robot_name + "/lo_frontend/odometry" 69 | d[method] = [bag_name, topic_name] 70 | # import pdb ; pdb.set_trace() 71 | with rosbag.Bag("aggregated_odometries.bag", "w") as outbag: 72 | for key in d.keys(): 73 | print("Aggregating " + key + " ...") 74 | bag = rosbag.Bag(d[key][0]) 75 | if key != "ground_truth": 76 | d[key][1] = list(bag.get_type_and_topic_info()[1].keys())[0] 77 | print("Topic for " + key + " odometry is " + d[key][1] + "\n") 78 | for topic, msg, t in bag.read_messages(topics=[d[key][1]]): 79 | outbag.write(key, msg, t) 80 | outbag.close() 81 | 82 | if __name__=="__main__": 83 | main() 84 | -------------------------------------------------------------------------------- /utilities/boxplot_from_npz.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Boxplot from npz 4 | Author: 5 | - Matteo Palieri, NASA Jet Propulsion Laboratory 6 | """ 7 | 8 | import os 9 | import sys 10 | import numpy as np 11 | import seaborn as sns 12 | from matplotlib import pyplot as plt 13 | 14 | def main(): 15 | 16 | if len(sys.argv)<2: 17 | print("Minimal Usage: python boxplot_from_npz.py method1") 18 | print("Example Usage: python boxplot_from_npz.py method1 method2 method3 method4") 19 | sys.exit(1) 20 | import pdb; pdb.set_trace() 21 | methods, data = [], [] 22 | for i in range(len(sys.argv)): 23 | if i!=0: 24 | methods.append(sys.argv[i]) 25 | 26 | distances = [100, 200, 300] 27 | for distance in distances: 28 | for method in methods: 29 | filename = os.getcwd() + "/" + str(distance) + "/error_array_" + method + ".npz" 30 | data.append(np.load(filename)) 31 | data.append([]) 32 | 33 | bplot = sns.boxplot(data=[d for d in data], width=0.5) 34 | colors = ["red", "green", "blue", "yellow"] 35 | 36 | color_counter = 0 37 | for i in range(len(data)-len(distances)): 38 | mybox = bplot.artists[i] 39 | mybox.set_facecolor(colors[color_counter]) 40 | color_counter = color_counter + 1 41 | if color_counter == len(methods): 42 | color_counter=0 43 | plt.show() 44 | 45 | if __name__=="__main__": 46 | main() -------------------------------------------------------------------------------- /utilities/compute_distances_travelled.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - INPUT: 4 | - aggregated_odometries.bag 5 | - OUTPUT: 6 | - aggregated_odometries_100.bag 7 | - aggregated_odometries_200.bag 8 | - aggregated_odometries_300.bag 9 | Author: 10 | - Matteo Palieri, NASA Jet Propulsion Laboratory 11 | """ 12 | 13 | import sys 14 | import rosbag 15 | import numpy as np 16 | 17 | def main(): 18 | 19 | if len(sys.argv)<2: 20 | print("Minimal Usage: python segment_aggregated_odometries.py method1") 21 | print("Example Usage: python segment_aggregated_odometries.py method1 method2 method3 method4") 22 | sys.exit(1) 23 | 24 | methods = [] 25 | for i in range(len(sys.argv)): 26 | if i!=0: 27 | methods.append(sys.argv[i]) 28 | 29 | 30 | outbag = rosbag.Bag("aggregated_odometries_" + str(checkpoint) + ".bag", "w") 31 | 32 | initialized = False 33 | time_end_point = None 34 | distance_traveled = [] 35 | previous_position = None 36 | 37 | for topic, msg, t in rosbag.Bag("aggregated_odometries.bag").read_messages(topics=["ground_truth"]): 38 | 39 | if not initialized: 40 | previous_position = msg.pose.pose.position 41 | distance_traveled.append(0) 42 | initialized = True 43 | pass 44 | 45 | current_position = msg.pose.pose.position 46 | dx = np.square(current_position.x - previous_position.x) 47 | dy = np.square(current_position.y - previous_position.y) 48 | dz = np.square(current_position.z - previous_position.z) 49 | distance_traveled = distance_traveled + np.sqrt(dx + dy + dz) 50 | previous_position = current_position 51 | 52 | if distance_traveled>checkpoint: 53 | time_end_point = t 54 | break 55 | 56 | for method in methods: 57 | for topic, msg, t in rosbag.Bag("aggregated_odometries.bag").read_messages(topics=[method]): 58 | outbag.write(method, msg , t) 59 | if t>time_end_point: 60 | break 61 | 62 | outbag.close() 63 | 64 | if __name__ == "__main__": 65 | main() -------------------------------------------------------------------------------- /utilities/get_dataset_duration.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Get dataset duration 4 | Author: 5 | - Matteo Palieri, NASA Jet Propulsion Laboratory 6 | """ 7 | 8 | import os 9 | import sys 10 | import numpy as np 11 | 12 | def main(): 13 | 14 | if len(sys.argv)<2: 15 | print("Example Usage: python get_dataset_duration.py path_to_dataset") 16 | sys.exit(1) 17 | 18 | durations = [] 19 | os.system("rosbag info -y -k duration " + sys.argv[1] + "/*.bag > durations.txt") 20 | with open("durations.txt") as infile: 21 | for line in infile: 22 | if "-" not in line: 23 | durations.append(float(line)) 24 | 25 | print("Dataset duration: " + str(np.max(durations))) 26 | 27 | if __name__=="__main__": 28 | main() -------------------------------------------------------------------------------- /utilities/get_distance_traveled_from_aggregated_odometries.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Get distance traveled from aggregated odometries 4 | Author: 5 | - Matteo Palieri, NASA Jet Propulsion Laboratory 6 | """ 7 | 8 | import sys 9 | import rosbag 10 | import numpy as np 11 | 12 | def main(): 13 | 14 | if len(sys.argv)<2: 15 | print("Minimal Usage: python get_distance_traveled.py method1") 16 | print("Example Usage: python get_distance_traveled.py method1 method2 method3 method4") 17 | sys.exit(1) 18 | 19 | methods = [] 20 | 21 | for i in range(len(sys.argv)): 22 | if i!=0: 23 | methods.append(sys.argv[i]) 24 | 25 | all_distances = {} 26 | 27 | for method in methods: 28 | initialized = False 29 | distance_traveled = 0 30 | previous_position = None 31 | for topic, msg, t in rosbag.Bag("aggregated_odometries.bag").read_messages(topics=[method]): 32 | if not initialized: 33 | previous_position = msg.pose.pose.position 34 | initialized = True 35 | pass 36 | current_position = msg.pose.pose.position 37 | dx = np.square(current_position.x - previous_position.x) 38 | dy = np.square(current_position.y - previous_position.y) 39 | dz = np.square(current_position.z - previous_position.z) 40 | distance_traveled = distance_traveled + np.sqrt(dx + dy + dz) 41 | previous_position = current_position 42 | print("\nDistance traveled by " + method + " : " + str(distance_traveled) + " m\n") 43 | all_distances[method] = distance_traveled 44 | 45 | if __name__ == "__main__": 46 | main() -------------------------------------------------------------------------------- /utilities/loop_closure_eval.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import os 3 | import copy 4 | import pandas as pd 5 | import numpy as np 6 | import rospy 7 | import rosbag 8 | import sys 9 | import csv 10 | 11 | 12 | import matplotlib.pyplot as plt 13 | 14 | from evo.core import transformations 15 | from evo.core import lie_algebra as lie 16 | 17 | def quaternion_to_matrix(quaternion): 18 | # quaternion: np.array([qx, qy, qz, qw]) 19 | x = quaternion[0] 20 | y = quaternion[1] 21 | z = quaternion[2] 22 | w = quaternion[3] 23 | Nq = w*w + x*x + y*y + z*z 24 | if Nq < np.finfo(np.float).eps: 25 | return np.eye(3) 26 | s = 2.0/Nq 27 | X = x*s 28 | Y = y*s 29 | Z = z*s 30 | wX = w*X; wY = w*Y; wZ = w*Z 31 | xX = x*X; xY = x*Y; xZ = x*Z 32 | yY = y*Y; yZ = y*Z; zZ = z*Z 33 | return np.array( 34 | [[ 1.0-(yY+zZ), xY-wZ, xZ+wY ], 35 | [ xY+wZ, 1.0-(xX+zZ), yZ-wX ], 36 | [ xZ-wY, yZ+wX, 1.0-(xX+yY) ]]) 37 | 38 | def calculate_eigen_value(covariance): 39 | if len(covariance) == 36: 40 | cov_mat = np.array(covariance).reshape(6, 6) 41 | # if b_gtsam_cov: 42 | cov_mat = cov_mat[-3:,-3:] 43 | else: 44 | cov_mat = np.array(covariance).reshape(3, 3) 45 | s, v = np.linalg.eig(cov_mat) 46 | v[:, 0] /= np.linalg.norm(v[:, 0]) 47 | v[:, 1] /= np.linalg.norm(v[:, 1]) 48 | v[:, 2] /= np.linalg.norm(v[:, 2]) 49 | 50 | if np.cross(v[:, 0], v[:, 1]).dot(v[:, 2]) < 0: 51 | # Make it right-handed 52 | v = v[:, [1, 0, 2]] 53 | s[0], s[1] = s[1], s[0] 54 | 55 | # Set ellipse scale 56 | k = 2.296 # 68.26% 57 | # k = 11.82 # 99.74% 58 | x_rad = k * np.sqrt(s[0]) 59 | y_rad = k * np.sqrt(s[1]) 60 | z_rad = k * np.sqrt(s[2]) 61 | 62 | return x_rad, y_rad, z_rad 63 | 64 | def key_to_chr_indx(key): 65 | key = np.uint64(key) 66 | keyBits = 64 * 8 67 | chrBits = 1 * 8 68 | indexBits = keyBits - chrBits 69 | chrMask = np.left_shift(np.uint64(255), np.uint64(indexBits)) 70 | indexMask = ~chrMask 71 | 72 | char = np.right_shift((key & chrMask), np.uint64(indexBits)) 73 | indx = key & indexMask 74 | 75 | return chr(char), indx 76 | 77 | class LoopClosureEvaluator: 78 | def __init__( 79 | self, pose_graph_bag_name, pose_graph_topic, 80 | loop_closure_bag_name, loop_closure_topic, 81 | gt_odom_bag_name=None, gt_odom_topic=None): 82 | # pose_graph_bag: final pose graph including odom edges and inlier loop closure edges 83 | # loop_closure_bag: all detected loop closures 84 | # gt_odom_bag: ground truth odometry bag 85 | 86 | # First parse bag into data 87 | # pose graph 88 | last_pose_graph_msg = None 89 | pose_graph_bag = rosbag.Bag(pose_graph_bag_name) 90 | for topic, msg, t in pose_graph_bag.read_messages(topics=[pose_graph_topic]): 91 | last_pose_graph_msg = msg 92 | 93 | self.nodes = dict() # {key: stamp} 94 | for node in last_pose_graph_msg.nodes: 95 | if node.ID == "odom_node": 96 | self.nodes[node.key] = node.header.stamp.to_nsec() 97 | 98 | self.inlier_loop_closures = [] 99 | self.inlier_multirobot_lc = [] 100 | self.inlier_singlerobot_lc = [] 101 | self.inlier_pair_lc_map = {} 102 | for edge in last_pose_graph_msg.edges: 103 | if edge.type == 3: 104 | self.inlier_loop_closures.append((edge.key_from, edge.key_to)) 105 | chr_from, idx_from = key_to_chr_indx(edge.key_from) 106 | chr_to, idx_to = key_to_chr_indx(edge.key_to) 107 | if (chr_from != chr_to): 108 | self.inlier_multirobot_lc.append((edge.key_from, edge.key_to)) 109 | else: 110 | self.inlier_singlerobot_lc.append((edge.key_from, edge.key_to)) 111 | if (chr_from, chr_to) in self.inlier_pair_lc_map: 112 | self.inlier_pair_lc_map[(chr_from, chr_to)].append(edge) 113 | elif (chr_to, chr_from) in self.inlier_pair_lc_map: 114 | self.inlier_pair_lc_map[(chr_to, chr_from)].append(edge) 115 | else: 116 | self.inlier_pair_lc_map[(chr_from, chr_to)] = [edge] 117 | 118 | # loop closure {(key_from, key_to): relative_transform} 119 | self.loop_closures = dict() 120 | self.multirobot_loop_closures = dict() 121 | self.singlerobot_loop_closures = dict() 122 | self.pair_lc_map = {} 123 | 124 | # loop closure covariance {(key_from, key_to): covariance} 125 | self.loop_closure_covariances = dict() 126 | loop_closure_bag = rosbag.Bag(loop_closure_bag_name) 127 | for topic, msg, t in loop_closure_bag.read_messages(topics=[loop_closure_topic]): 128 | for edge in msg.edges: 129 | translation = np.array([edge.pose.position.x, edge.pose.position.y, edge.pose.position.z]) 130 | quaternion = np.array([edge.pose.orientation.x, edge.pose.orientation.y, 131 | edge.pose.orientation.z, edge.pose.orientation.w]) 132 | rotation = quaternion_to_matrix(quaternion) 133 | self.loop_closures[(edge.key_from, edge.key_to)] = lie.se3(rotation, translation) 134 | self.loop_closure_covariances[(edge.key_from, edge.key_to)] = edge.covariance 135 | chr_from, idx_from = key_to_chr_indx(edge.key_from) 136 | chr_to, idx_to = key_to_chr_indx(edge.key_to) 137 | if (chr_from != chr_to): 138 | self.multirobot_loop_closures[(edge.key_from, edge.key_to)] = lie.se3(rotation, translation) 139 | else: 140 | self.singlerobot_loop_closures[(edge.key_from, edge.key_to)] = lie.se3(rotation, translation) 141 | if (chr_from, chr_to) in self.pair_lc_map: 142 | self.pair_lc_map[(chr_from, chr_to)].append(edge) 143 | elif (chr_to, chr_from) in self.pair_lc_map: 144 | self.pair_lc_map[(chr_to, chr_from)].append(edge) 145 | else: 146 | self.pair_lc_map[(chr_from, chr_to)] = [edge] 147 | 148 | print("Total of {} loop closures detected, with {} inliers. ".format(len(self.loop_closures), len(self.inlier_loop_closures))) 149 | print("Total of {} inter-robot loop closures detected, with {} inliers. ".format(len(self.multirobot_loop_closures), len(self.inlier_multirobot_lc))) 150 | print("Total of {} single-robot loop closures detected, with {} inliers. ".format(len(self.singlerobot_loop_closures), len(self.inlier_singlerobot_lc))) 151 | 152 | print("LOOP CLOSURES BY ROBOT: \n") 153 | for pr in self.pair_lc_map: 154 | print("Between {r1} and {r2}, {nlc} loop closures".format(r1=str(pr[0]), r2=str(pr[1]), nlc=str(len(self.pair_lc_map[pr])))) 155 | 156 | print("INLIER LOOP CLOSURES BY ROBOT: \n") 157 | for pr in self.inlier_pair_lc_map: 158 | print("Between {r1} and {r2}, {nlc} loop closures".format(r1=str(pr[0]), r2=str(pr[1]), nlc=str(len(self.inlier_pair_lc_map[pr])))) 159 | 160 | self.lc_covar_radius = [] 161 | for loop_closure in self.loop_closure_covariances: 162 | covar = self.loop_closure_covariances[loop_closure] 163 | x_rad, y_rad, z_rad = calculate_eigen_value(covar) 164 | cov_radius = np.sqrt(x_rad**2 + y_rad**2 + z_rad**2) 165 | self.lc_covar_radius.append(cov_radius) 166 | 167 | if gt_odom_bag_name != None and gt_odom_topic != None: 168 | self.gt_data = True 169 | # gt_odom 170 | self.gt_odom_stamps = [] 171 | self.gt_odom_poses = [] 172 | gt_odom_bag = rosbag.Bag(gt_odom_bag_name) 173 | for topic, msg, t in gt_odom_bag.read_messages(topics=[gt_odom_topic]): 174 | translation = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) 175 | quaternion = np.array([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, 176 | msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) 177 | rotation = quaternion_to_matrix(quaternion) 178 | self.gt_odom_poses.append(lie.se3(rotation, translation)) 179 | self.gt_odom_stamps.append(msg.header.stamp.to_nsec()) 180 | 181 | self.gt_odom_stamps = np.asarray(self.gt_odom_stamps) 182 | self.gt_odom_poses = np.asarray(self.gt_odom_poses) 183 | 184 | self.accuracy_trans = [] 185 | self.accuracy_rot = [] 186 | self.inlier_accuracy_trans = [] 187 | self.inlier_accuracy_rot = [] 188 | 189 | # average loop closure accuracy 190 | self.avg_accuracy_trans = [] 191 | self.avg_accuracy_rot = [] 192 | self.avg_inlier_accuracy_trans = [] 193 | self.avg_inlier_accuracy_rot = [] 194 | else: 195 | self.gt_data = False 196 | 197 | def get_gt_relative_pose(self, stamp_from, stamp_to): 198 | # get idx of gt_odom closest to stamps 199 | idx_from = (np.abs(self.gt_odom_stamps - stamp_from)).argmin() 200 | idx_to = (np.abs(self.gt_odom_stamps - stamp_to)).argmin() 201 | pose_from = self.gt_odom_poses[idx_from] 202 | pose_to = self.gt_odom_poses[idx_to] 203 | return lie.relative_se3(pose_from, pose_to) 204 | 205 | # TODO: add normalized evaluation!!! 206 | def evaluate(self): 207 | if not self.gt_data: 208 | print("No ground truth data. Cannot evaluate accuracy. ") 209 | return 210 | if len(self.loop_closures) == 0: 211 | print("No loop closures detected. ") 212 | return 213 | # first evaluate accuracy of all loop closures 214 | for loop_closure in self.loop_closures: 215 | inlier = loop_closure in self.inlier_loop_closures 216 | stamp_from = self.nodes[loop_closure[0]] 217 | stamp_to = self.nodes[loop_closure[1]] 218 | gt_transform = self.get_gt_relative_pose(stamp_from, stamp_to) 219 | est_transform = self.loop_closures[loop_closure] 220 | 221 | # get difference between gt and est transform 222 | error = lie.relative_se3(gt_transform, est_transform) 223 | error_trans = np.linalg.norm(error[:3, 3]) 224 | error_rot = 180 / np.pi * abs(lie.so3_log(error[:3, :3])) 225 | 226 | self.accuracy_trans.append(error_trans) 227 | self.accuracy_rot.append(error_rot) 228 | self.avg_accuracy_trans.append(error_trans / abs(loop_closure[1] - loop_closure[0])) 229 | self.avg_accuracy_rot.append(error_rot / abs(loop_closure[1] - loop_closure[0])) 230 | if (inlier): 231 | self.inlier_accuracy_trans.append(error_trans) 232 | self.inlier_accuracy_rot.append(error_rot) 233 | self.avg_inlier_accuracy_trans.append(error_trans / abs(loop_closure[1] - loop_closure[0])) 234 | self.avg_inlier_accuracy_rot.append(error_rot / abs(loop_closure[1] - loop_closure[0])) 235 | 236 | return 237 | 238 | def get_num_loopclosures(self): 239 | return len(self.loop_closures) 240 | 241 | def get_num_inliers(self): 242 | return len(self.inlier_loop_closures) 243 | 244 | def get_accuracy_trans(self): 245 | # get loop closure accuracy for all loop closures 246 | accuracy = np.asarray(self.accuracy_trans) 247 | print("Loop closure translation accuracy. median: {}, mean: {}, " 248 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 249 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 250 | 251 | def get_accuracy_rot(self): 252 | # get loop closure accuracy for all loop closures 253 | accuracy = np.asarray(self.accuracy_rot) 254 | print("Loop closure rotation accuracy. median: {}, mean: {}, " 255 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 256 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 257 | 258 | def get_inlier_accuracy_trans(self): 259 | # get inlier loop closure accuracy for all loop closures 260 | accuracy = np.asarray(self.inlier_accuracy_trans) 261 | print("Inlier loop closure translation accuracy. median: {}, mean: {}, " 262 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 263 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 264 | 265 | def get_inlier_accuracy_rot(self): 266 | # get loop closure accuracy for all loop closures 267 | accuracy = np.asarray(self.inlier_accuracy_rot) 268 | print("Inlier loop closure rotation accuracy. median: {}, mean: {}, " 269 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 270 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 271 | 272 | def get_avg_accuracy_trans(self): 273 | # get loop closure accuracy for all loop closures 274 | accuracy = np.asarray(self.avg_accuracy_trans) 275 | print("Avg (per node) loop closure translation accuracy. median: {}, mean: {}, " 276 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 277 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 278 | 279 | def get_avg_accuracy_rot(self): 280 | # get loop closure accuracy for all loop closures 281 | accuracy = np.asarray(self.avg_accuracy_rot) 282 | print("Avg (per node) loop closure rotation accuracy. median: {}, mean: {}, " 283 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 284 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 285 | 286 | def get_avg_inlier_accuracy_trans(self): 287 | # get inlier loop closure accuracy for all loop closures 288 | accuracy = np.asarray(self.avg_inlier_accuracy_trans) 289 | print("Avg (per node) inlier loop closure translation accuracy. median: {}, mean: {}, " 290 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 291 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 292 | 293 | def get_avg_inlier_accuracy_rot(self): 294 | # get loop closure accuracy for all loop closures 295 | accuracy = np.asarray(self.avg_inlier_accuracy_rot) 296 | print("Avg (per node) inlier oop closure rotation accuracy. median: {}, mean: {}, " 297 | "min: {}, max: {}".format(np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy))) 298 | return np.median(accuracy), np.mean(accuracy), np.min(accuracy), np.max(accuracy) 299 | 300 | def save_accuracy_plot(self, filename="", save=True): 301 | # save plot for loop closure accuracy plot 302 | fig, (ax1, ax2) = plt.subplots(2) 303 | fig.suptitle('Loop closure error') 304 | median, mean, _, _ = self.get_accuracy_trans() 305 | ax1.plot(self.accuracy_trans) 306 | ax1.hlines(mean, 0, len(self.accuracy_trans) - 1, color='r', label="mean") 307 | ax1.hlines(median, 0, len(self.accuracy_trans) - 1, color='b', label="median") 308 | ax1.legend() 309 | ax1.set_title("Loop closure translation error (m)", fontsize="medium") 310 | ax1.xaxis.set_tick_params(labelsize='medium') 311 | ax1.yaxis.set_tick_params(labelsize='medium') 312 | median, mean, _, _ = self.get_accuracy_rot() 313 | ax2.plot(self.accuracy_rot) 314 | ax2.hlines(mean, 0, len(self.accuracy_rot) - 1, color='r', label="mean") 315 | ax2.hlines(median, 0, len(self.accuracy_rot) - 1, color='b', label="median") 316 | ax2.legend() 317 | ax2.set_title("Loop closure rotation error (deg)", fontsize="medium") 318 | ax2.xaxis.set_tick_params(labelsize='medium') 319 | ax2.yaxis.set_tick_params(labelsize='medium') 320 | 321 | if not save: 322 | plt.show() 323 | else: 324 | plt.savefig(filename, bbox_inches='tight') 325 | return 326 | 327 | def save_avg_accuracy_plot(self, filename="", save=True): 328 | # save plot for loop closure accuracy plot 329 | fig, (ax1, ax2) = plt.subplots(2) 330 | fig.suptitle('Avg (per node) loop closure error') 331 | median, mean, _, _ = self.get_avg_accuracy_trans() 332 | ax1.plot(self.avg_accuracy_trans) 333 | ax1.hlines(mean, 0, len(self.avg_accuracy_trans) - 1, color='r', label="mean") 334 | ax1.hlines(median, 0, len(self.avg_accuracy_trans) - 1, color='b', label="median") 335 | ax1.legend() 336 | ax1.set_title("Avg (per node) loop closure translation error (m)", fontsize="medium") 337 | ax1.xaxis.set_tick_params(labelsize='medium') 338 | ax1.yaxis.set_tick_params(labelsize='medium') 339 | median, mean, _, _ = self.get_avg_accuracy_rot() 340 | ax2.plot(self.avg_accuracy_rot) 341 | ax2.hlines(mean, 0, len(self.avg_accuracy_rot) - 1, color='r', label="mean") 342 | ax2.hlines(median, 0, len(self.avg_accuracy_rot) - 1, color='b', label="median") 343 | ax2.legend() 344 | ax2.set_title("Avg (per node) loop closure rotation error (deg)", fontsize="medium") 345 | ax2.xaxis.set_tick_params(labelsize='medium') 346 | ax2.yaxis.set_tick_params(labelsize='medium') 347 | 348 | if not save: 349 | plt.show() 350 | else: 351 | plt.savefig(filename, bbox_inches='tight') 352 | return 353 | 354 | def save_inlier_accuracy_plot(self, filename="", save=True): 355 | # save plot for inlier loop closure accuracy 356 | fig, (ax1, ax2) = plt.subplots(2) 357 | fig.suptitle('Loop closure inliers error') 358 | median, mean, _, _ = self.get_inlier_accuracy_trans() 359 | ax1.plot(self.inlier_accuracy_trans) 360 | ax1.hlines(mean, 0, len(self.inlier_accuracy_trans) - 1, color='r', label="mean") 361 | ax1.hlines(median, 0, len(self.inlier_accuracy_trans) - 1, color='b', label="median") 362 | ax1.legend() 363 | ax1.set_title("Loop closure inliers translation error (m)", fontsize="medium") 364 | ax1.xaxis.set_tick_params(labelsize='medium') 365 | ax1.yaxis.set_tick_params(labelsize='medium') 366 | median, mean, _, _ = self.get_inlier_accuracy_rot() 367 | ax2.plot(self.inlier_accuracy_rot) 368 | ax2.hlines(mean, 0, len(self.inlier_accuracy_rot) - 1, color='r', label="mean") 369 | ax2.hlines(median, 0, len(self.inlier_accuracy_rot) - 1, color='b', label="median") 370 | ax2.legend() 371 | ax2.set_title("Loop closure inliers rotation error (deg)", fontsize="medium") 372 | ax2.xaxis.set_tick_params(labelsize='medium') 373 | ax2.yaxis.set_tick_params(labelsize='medium') 374 | 375 | if not save: 376 | plt.show() 377 | else: 378 | plt.savefig(filename, bbox_inches='tight') 379 | return 380 | 381 | def save_avg_inlier_accuracy_plot(self, filename="", save=True): 382 | # save plot for inlier loop closure accuracy 383 | fig, (ax1, ax2) = plt.subplots(2) 384 | fig.suptitle('Avg (per node) loop closure inliers error') 385 | median, mean, _, _ = self.get_avg_inlier_accuracy_trans() 386 | ax1.plot(self.avg_inlier_accuracy_trans) 387 | ax1.hlines(mean, 0, len(self.avg_inlier_accuracy_trans) - 1, color='r', label="mean") 388 | ax1.hlines(median, 0, len(self.avg_inlier_accuracy_trans) - 1, color='b', label="median") 389 | ax1.legend() 390 | ax1.set_title("Avg (per node) loop closure inliers translation error (m)", fontsize="medium") 391 | ax1.xaxis.set_tick_params(labelsize='medium') 392 | ax1.yaxis.set_tick_params(labelsize='medium') 393 | median, mean, _, _ = self.get_avg_inlier_accuracy_rot() 394 | ax2.plot(self.avg_inlier_accuracy_rot) 395 | ax2.hlines(mean, 0, len(self.avg_inlier_accuracy_rot) - 1, color='r', label="mean") 396 | ax2.hlines(median, 0, len(self.avg_inlier_accuracy_rot) - 1, color='b', label="median") 397 | ax2.legend() 398 | ax2.set_title("Avg (per node) loop closure inliers rotation error (deg)", fontsize="medium") 399 | ax2.xaxis.set_tick_params(labelsize='medium') 400 | ax2.yaxis.set_tick_params(labelsize='medium') 401 | 402 | if not save: 403 | plt.show() 404 | else: 405 | plt.savefig(filename, bbox_inches='tight') 406 | return 407 | 408 | def save_stats(self, results_dir): 409 | with open(results_dir + "lc_stats.csv", 'w') as outfile: 410 | writer = csv.writer(outfile, delimiter=",", quotechar='|', quoting=csv.QUOTE_MINIMAL) 411 | writer.writerow(['# total loop closures', '# total inliers', ]) 412 | writer.writerow([len(self.loop_closures), len(self.inlier_loop_closures)]) 413 | 414 | def save_covariance_plot(self, filename="", save=True): 415 | fig, ax = plt.subplots() 416 | ax.plot(self.lc_covar_radius) 417 | fig.suptitle("Loop Closure Covariance Radius") 418 | ax.xaxis.set_tick_params(labelsize='medium') 419 | ax.yaxis.set_tick_params(labelsize='medium') 420 | if not save: 421 | plt.show() 422 | else: 423 | plt.savefig(filename, bbox_inches='tight') 424 | return 425 | 426 | if __name__=="__main__": 427 | if len(sys.argv) < 5: 428 | print("Example Usage: python loop_closure_eval.py pose_graph.bag pose_graph_topic " 429 | "loop_closure.bag loop_closure_topic gt_odometry.bag gt_odometry_topic") 430 | sys.exit(1) 431 | eval_accuracy = False 432 | if len(sys.argv) < 7: 433 | eval = LoopClosureEvaluator(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4]) 434 | else: 435 | eval = LoopClosureEvaluator(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4], sys.argv[5], sys.argv[6]) 436 | eval_accuracy = True 437 | eval.evaluate() 438 | if eval_accuracy: 439 | eval.save_accuracy_plot(save=False) 440 | eval.save_inlier_accuracy_plot(save=False) 441 | eval.save_avg_accuracy_plot(save=False) 442 | eval.save_avg_inlier_accuracy_plot(save=False) 443 | -------------------------------------------------------------------------------- /utilities/plot_factor_covariance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | ### plot covariance of odometry edges 3 | import pdb 4 | import matplotlib.pyplot as plt 5 | import matplotlib 6 | from nav_msgs.msg import Odometry 7 | import rosbag 8 | import rospy 9 | import argparse 10 | import enum 11 | import math 12 | import os 13 | import numpy as np 14 | 15 | 16 | def get_max_diagonal_term(matrix): 17 | return max(matrix.diagonal()) 18 | 19 | 20 | def get_min_diagonal_term(matrix): 21 | return min(matrix.diagonal()) 22 | 23 | 24 | def calculate_eigen_values(covariance): 25 | s, v = np.linalg.eig(covariance) 26 | return s, v 27 | 28 | 29 | def extract_covariance(pose_graph_edge): 30 | return np.array(pose_graph_edge.covariance).reshape(6, 6) 31 | 32 | 33 | def plot_factor_covariance_maxmin(bag_file, topic, edge_type): 34 | rot_max_variances = [] 35 | rot_min_variances = [] 36 | tran_max_variances = [] 37 | tran_min_variances = [] 38 | last_message = None 39 | # Extract last pose graph message 40 | with rosbag.Bag(bag_file) as bag: 41 | for topic, message, t in bag.read_messages(topics=[topic]): 42 | last_message = message 43 | 44 | for edge in last_message.edges: 45 | if edge.type == edge_type: 46 | cov = extract_covariance(edge) 47 | rot_max_var = get_max_diagonal_term(cov[:3, :3]) 48 | rot_min_var = get_min_diagonal_term(cov[:3, :3]) 49 | rot_max_variances.append(rot_max_var) 50 | rot_min_variances.append(rot_min_var) 51 | tran_max_var = get_max_diagonal_term(cov[-3:, -3:]) 52 | tran_min_var = get_min_diagonal_term(cov[-3:, -3:]) 53 | tran_max_variances.append(tran_max_var) 54 | tran_min_variances.append(tran_min_var) 55 | 56 | # plot 57 | plt.subplot(1, 2, 1) 58 | plt.plot(rot_min_variances, label="Min Variance") 59 | plt.plot(rot_max_variances, label="Max Variance") 60 | plt.title("Rotation Variances") 61 | plt.legend() 62 | 63 | plt.subplot(1, 2, 2) 64 | plt.plot(tran_min_variances, label="Min Variance") 65 | plt.plot(tran_max_variances, label="Max Variance") 66 | plt.title("Translation Variances") 67 | plt.legend() 68 | plt.show() 69 | 70 | return 71 | 72 | 73 | def plot_factor_covariance_xyz(bag_file, topic, edge_type): 74 | rot_variances_x = [] 75 | rot_variances_y = [] 76 | rot_variances_z = [] 77 | tran_variances_x = [] 78 | tran_variances_y = [] 79 | tran_variances_z = [] 80 | last_message = None 81 | with rosbag.Bag(bag_file) as bag: 82 | for topic, message, t in bag.read_messages(topics=[topic]): 83 | last_message = message 84 | 85 | for edge in last_message.edges: 86 | if edge.type == edge_type: 87 | cov = extract_covariance(edge) 88 | rot_variances_x.append(np.sqrt(cov[0, 0])) 89 | rot_variances_y.append(np.sqrt(cov[1, 1])) 90 | rot_variances_z.append(np.sqrt(cov[2, 2])) 91 | tran_variances_x.append(np.sqrt(cov[3, 3])) 92 | tran_variances_y.append(np.sqrt(cov[4, 4])) 93 | tran_variances_z.append(np.sqrt(cov[5, 5])) 94 | 95 | # plot 96 | plt.subplot(1, 2, 1) 97 | plt.plot(rot_variances_x, label="Std Dev x") 98 | plt.plot(rot_variances_y, label="Std Dev y") 99 | plt.plot(rot_variances_z, label="Std Dev z") 100 | plt.title("Rotation Std Dev") 101 | plt.legend() 102 | 103 | plt.subplot(1, 2, 2) 104 | plt.plot(tran_variances_x, label="Std Dev x") 105 | plt.plot(tran_variances_y, label="Std Dev y") 106 | plt.plot(tran_variances_z, label="Std Dev z") 107 | plt.title("Translation Std Dev") 108 | plt.legend() 109 | plt.show() 110 | 111 | return 112 | 113 | 114 | def main(): 115 | # Parse user input 116 | parser = argparse.ArgumentParser() 117 | parser.add_argument("-t", "--topic_name", default="/husky1/lamp/pose_graph", help="topic name") 118 | parser.add_argument("-b", "--bag", default="/home/costar/rosbag/beta2/husky4/lamp_01_fix_point_to_plane/pose_graph.bag", help="bag file") 119 | parser.add_argument("-e", "--edge_type", type=int, default=0, help="edge type") 120 | 121 | options = parser.parse_args() 122 | print("Reading %s from %s and plotting covariance for edge type %s" % (options.topic_name, options.bag, str(options.edge_type))) 123 | 124 | plot_factor_covariance_maxmin(options.bag, options.topic_name, options.edge_type) 125 | plot_factor_covariance_xyz(options.bag, options.topic_name, options.edge_type) 126 | 127 | 128 | if __name__ == '__main__': 129 | main() 130 | -------------------------------------------------------------------------------- /utilities/plot_lc_ftness_hist.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from matplotlib import colors 4 | from matplotlib.ticker import PercentFormatter 5 | import csv 6 | import argparse 7 | 8 | 9 | class LoopResult: 10 | def __init__(self, fitness, t_error, r_error): 11 | self.fitness = fitness 12 | self.t_error = t_error 13 | self.r_error = r_error 14 | 15 | 16 | def read_result_csv(file_name): 17 | with open(file_name) as f: 18 | reader = csv.reader(f) 19 | next(reader, None) 20 | results = [] 21 | for row in reader: 22 | results.append(LoopResult(float(row[0]), float(row[1]), float(row[2]))) 23 | return results 24 | 25 | 26 | def plot_histograms(true_positives, false_positives, n_bins=10): 27 | n_bins = 20 28 | fig, axs = plt.subplots(1, 1) 29 | 30 | true_positive_fitness = [x.fitness for x in true_positives] 31 | false_positives_fitness = [x.fitness for x in false_positives] 32 | 33 | # We can set the number of bins with the `bins` kwarg 34 | axs.hist(true_positive_fitness, alpha=0.5, bins=n_bins, label="true positives") 35 | axs.hist(false_positives_fitness, alpha=0.5, bins=n_bins, label="false positives") 36 | axs.legend() 37 | axs.set_xlabel("fitness score") 38 | plt.show() 39 | 40 | 41 | def main(): 42 | # Parse user input 43 | parser = argparse.ArgumentParser() 44 | parser.add_argument("-t", "--true_positives", default="", help="csv of true positives") 45 | parser.add_argument("-f", "--false_positives", default="", help="csv of false positives") 46 | parser.add_argument("-n", "--n_bins", type=int, default=20, help="number of bins in fitness histogram") 47 | 48 | options = parser.parse_args() 49 | 50 | true_results = read_result_csv(options.true_positives) 51 | false_results = read_result_csv(options.false_positives) 52 | 53 | plot_histograms(true_results, false_results, options.n_bins) 54 | 55 | 56 | if __name__ == '__main__': 57 | main() 58 | -------------------------------------------------------------------------------- /utilities/plot_lc_log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | ### extract and plot from loop computation log file 3 | import matplotlib.pyplot as plt 4 | import matplotlib 5 | from nav_msgs.msg import Odometry 6 | import argparse 7 | import numpy as np 8 | 9 | 10 | def readAndPlotFitnessScore(filename, maxscore): 11 | # Opening file 12 | file = open(filename, 'r') 13 | scores = [] 14 | for line in file: 15 | splt = line.split("ICP: Converged or max iterations reached, but score:") 16 | if len(splt) > 1: 17 | splt1 = splt[1].split() 18 | splt2 = splt1[0].split(",") 19 | if float(splt2[0]) < maxscore: 20 | scores.append(float(splt2[0])) 21 | file.close() 22 | 23 | fig, axs = plt.subplots(1, 1) 24 | axs.hist(scores, alpha=0.8, bins=20) 25 | axs.legend() 26 | axs.set_xlabel("fitness score") 27 | axs.set_title("Unaccepted loop closures") 28 | plt.show() 29 | 30 | 31 | def main(): 32 | # Parse user input 33 | parser = argparse.ArgumentParser() 34 | parser.add_argument("-f", "--filename", default="/home/costar/Downloads/base1_2021-09-10-09-59-00_test/log/_base1_loop_computation.log", help="log file") 35 | parser.add_argument("-m", "--maxscore", type=int, default=1000, help="max fitness score to plot") 36 | 37 | options = parser.parse_args() 38 | print("Reading from %s" % (options.filename)) 39 | 40 | readAndPlotFitnessScore(options.filename, options.maxscore) 41 | 42 | 43 | if __name__ == '__main__': 44 | main() 45 | -------------------------------------------------------------------------------- /utilities/plot_loop_candidates.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | ### plot distribution of loop candidates 3 | import matplotlib.pyplot as plt 4 | import matplotlib 5 | from pose_graph_msgs.msg import LoopCandidateArray, LoopCandidate 6 | from pose_graph_msgs.msg import PoseGraph, PoseGraphEdge 7 | import rosbag 8 | import argparse 9 | import math 10 | import numpy as np 11 | import os 12 | import key_handling 13 | 14 | 15 | def extract_loop_candidates(bag_name, topic_name): 16 | pair_lc_map = {} 17 | with rosbag.Bag(bag_name) as bag: 18 | for topic, message, t in bag.read_messages(topics=[topic_name]): 19 | for candidate in message.candidates: 20 | pfx_from, _ = key_handling.split_pg_key(candidate.key_from) 21 | pfx_to, _ = key_handling.split_pg_key(candidate.key_to) 22 | if (pfx_from, pfx_to) in pair_lc_map: 23 | pair_lc_map[(pfx_from, pfx_to)].append(candidate) 24 | elif (pfx_to, pfx_from) in pair_lc_map: 25 | pair_lc_map[(pfx_to, pfx_from)].append(candidate) 26 | else: 27 | pair_lc_map[(pfx_from, pfx_to)] = [candidate] 28 | return pair_lc_map 29 | 30 | 31 | def extract_loop_closures(bag_name, topic_name): 32 | pair_lc_map = {} 33 | with rosbag.Bag(bag_name) as bag: 34 | for topic, message, t in bag.read_messages(topics=[topic_name]): 35 | for lc in message.edges: 36 | pfx_from, _ = key_handling.split_pg_key(lc.key_from) 37 | pfx_to, _ = key_handling.split_pg_key(lc.key_to) 38 | if (pfx_from, pfx_to) in pair_lc_map: 39 | pair_lc_map[(pfx_from, pfx_to)].append(lc) 40 | elif (pfx_to, pfx_from) in pair_lc_map: 41 | pair_lc_map[(pfx_to, pfx_from)].append(lc) 42 | else: 43 | pair_lc_map[(pfx_from, pfx_to)] = [lc] 44 | return pair_lc_map 45 | 46 | 47 | def plot_lc_distribution(loops, ax): 48 | # Pie chart, where the slices will be ordered and plotted counter-clockwise: 49 | labels = [] 50 | sizes = [] 51 | 52 | for l in loops: 53 | label_str = l[0] + l[1] + " : " + str(len(loops[l])) 54 | labels.append(label_str) 55 | sizes.append(len(loops[l])) 56 | ax.pie(sizes, labels=labels, autopct='%1.1f%%', startangle=90) 57 | ax.axis('equal') # Equal aspect ratio ensures that pie is drawn as a circle. 58 | 59 | 60 | def merge_loops(d1, d2): 61 | for k in d2: 62 | if (k[0], k[1]) in d1: 63 | d1[(k[0], k[1])].extend(d2[k]) 64 | elif (k[1], k[0]) in d1: 65 | d1[(k[1], k[0])].extend(d2[k]) 66 | else: 67 | d1[k] = d2[k] 68 | return d1 69 | 70 | 71 | def main(): 72 | # Parse user input 73 | parser = argparse.ArgumentParser() 74 | parser.add_argument("-p", "--folder", default="/data/ros/base1_2021-09-14-09-58-00_lamp_adapt_t3/rosbag/", help="folder containing the bag files") 75 | 76 | options = parser.parse_args() 77 | loop_gen_topic = "/base1/lamp/loop_generation/loop_candidates" 78 | loop_prior_topic = "/base1/lamp/prioritization/prioritized_loop_candidates" 79 | loop_queue_topic = "/base1/lamp/loop_candidate_queue/prioritized_loop_candidates" 80 | loop_closure_topic = "/base1/lamp/laser_loop_closures" 81 | 82 | loop_gen = {} 83 | loop_prior = {} 84 | loop_queue = {} 85 | loop_closures = {} 86 | for file in os.listdir(options.folder): 87 | if file.startswith("base1_lamp"): 88 | filestr = options.folder + file 89 | print("Reading from %s" % (filestr)) 90 | lg = extract_loop_candidates(filestr, loop_gen_topic) 91 | lp = extract_loop_candidates(filestr, loop_prior_topic) 92 | lq = extract_loop_candidates(filestr, loop_queue_topic) 93 | lc = extract_loop_closures(filestr, loop_closure_topic) 94 | 95 | loop_gen = merge_loops(loop_gen, lg) 96 | loop_prior = merge_loops(loop_prior, lp) 97 | loop_queue = merge_loops(loop_queue, lq) 98 | loop_closures = merge_loops(loop_closures, lc) 99 | 100 | fig, axs = plt.subplots(2, 2) 101 | plot_lc_distribution(loop_gen, axs[0, 0]) 102 | axs[0, 0].set_title("Loop Generated") 103 | plot_lc_distribution(loop_prior, axs[0, 1]) 104 | axs[0, 1].set_title("Loop Prioritized") 105 | plot_lc_distribution(loop_queue, axs[1, 0]) 106 | axs[1, 0].set_title("Loop in Queue") 107 | plot_lc_distribution(loop_closures, axs[1, 1]) 108 | axs[1, 1].set_title("Loop Closures") 109 | 110 | plt.show() 111 | 112 | 113 | if __name__ == '__main__': 114 | main() 115 | -------------------------------------------------------------------------------- /utilities/plot_odom_covariance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | ### plot covariance of odometry edges 3 | import pdb 4 | import matplotlib.pyplot as plt 5 | import matplotlib 6 | from nav_msgs.msg import Odometry 7 | import rosbag 8 | import rospy 9 | import argparse 10 | import enum 11 | import math 12 | import os 13 | import numpy as np 14 | 15 | 16 | def get_max_diagonal_term(matrix): 17 | return max(matrix.diagonal()) 18 | 19 | 20 | def get_min_diagonal_term(matrix): 21 | return min(matrix.diagonal()) 22 | 23 | 24 | def calculate_eigen_values(covariance): 25 | s, v = np.linalg.eig(covariance) 26 | return s, v 27 | 28 | 29 | def extract_covariance(odom_msg): 30 | return np.array(odom_msg.pose.covariance).reshape(6, 6) 31 | 32 | 33 | def plot_odom_covariance_maxmin(bag_file, odom_topic): 34 | rot_max_variances = [] 35 | rot_min_variances = [] 36 | tran_max_variances = [] 37 | tran_min_variances = [] 38 | with rosbag.Bag(bag_file) as bag: 39 | for topic, message, t in bag.read_messages(topics=[odom_topic]): 40 | cov = extract_covariance(message) 41 | rot_max_var = get_max_diagonal_term(cov[:3, :3]) 42 | rot_min_var = get_min_diagonal_term(cov[:3, :3]) 43 | rot_max_variances.append(rot_max_var) 44 | rot_min_variances.append(rot_min_var) 45 | tran_max_var = get_max_diagonal_term(cov[-3:, -3:]) 46 | tran_min_var = get_min_diagonal_term(cov[-3:, -3:]) 47 | tran_max_variances.append(tran_max_var) 48 | tran_min_variances.append(tran_min_var) 49 | 50 | # plot 51 | plt.subplot(1, 2, 1) 52 | plt.plot(rot_min_variances, label="Min Variance") 53 | plt.plot(rot_max_variances, label="Max Variance") 54 | plt.title("Rotation Variances") 55 | plt.legend() 56 | 57 | plt.subplot(1, 2, 2) 58 | plt.plot(tran_min_variances, label="Min Variance") 59 | plt.plot(tran_max_variances, label="Max Variance") 60 | plt.title("Translation Variances") 61 | plt.legend() 62 | plt.show() 63 | 64 | return 65 | 66 | 67 | def plot_odom_covariance_xyz(bag_file, odom_topic): 68 | rot_variances_x = [] 69 | rot_variances_y = [] 70 | rot_variances_z = [] 71 | tran_variances_x = [] 72 | tran_variances_y = [] 73 | tran_variances_z = [] 74 | with rosbag.Bag(bag_file) as bag: 75 | for topic, message, t in bag.read_messages(topics=[odom_topic]): 76 | cov = extract_covariance(message) 77 | rot_variances_x.append(cov[0, 0]) 78 | rot_variances_y.append(cov[1, 1]) 79 | rot_variances_z.append(cov[2, 2]) 80 | tran_variances_x.append(cov[3, 3]) 81 | tran_variances_y.append(cov[4, 4]) 82 | tran_variances_z.append(cov[5, 5]) 83 | 84 | # plot 85 | plt.subplot(1, 2, 1) 86 | plt.plot(rot_variances_x, label="Variance x") 87 | plt.plot(rot_variances_y, label="Variance y") 88 | plt.plot(rot_variances_z, label="Variance z") 89 | plt.title("Rotation Variances") 90 | plt.legend() 91 | 92 | plt.subplot(1, 2, 2) 93 | plt.plot(tran_variances_x, label="Variance x") 94 | plt.plot(tran_variances_y, label="Variance y") 95 | plt.plot(tran_variances_z, label="Variance z") 96 | plt.title("Translation Variances") 97 | plt.legend() 98 | plt.show() 99 | 100 | return 101 | 102 | 103 | def main(): 104 | # Parse user input 105 | parser = argparse.ArgumentParser() 106 | parser.add_argument("-t", "--topic_name", default="/husky1/lo_frontend/odometry", help="topic name") 107 | parser.add_argument("-b", "--bag", default="/home/costar/rosbag/beta2/husky4/lamp_01_fix_point_to_plane/odometry.bag", help="bag file") 108 | 109 | options = parser.parse_args() 110 | 111 | print("Reading %s from %s" % (options.topic_name, options.bag)) 112 | 113 | plot_odom_covariance_maxmin(options.bag, options.topic_name) 114 | plot_odom_covariance_xyz(options.bag, options.topic_name) 115 | 116 | 117 | if __name__ == '__main__': 118 | main() 119 | -------------------------------------------------------------------------------- /utilities/plot_volume_size_from_map_info.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import rosbag 3 | import matplotlib 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | 7 | matplotlib.rcParams['mathtext.fontset'] = 'stix' 8 | matplotlib.rcParams['font.family'] = 'Arial' 9 | size, labelsize = 20, 20 10 | params = {'legend.fontsize': 'large', 11 | 'figure.figsize': (20,8), 12 | 'axes.labelsize': labelsize, 13 | 'axes.titlesize': labelsize, 14 | 'xtick.labelsize': size*0.75, 15 | 'ytick.labelsize': size*0.75, 16 | 'axes.titlepad': 25} 17 | plt.rcParams.update(params) 18 | 19 | def plot_volume_size(volume, size, timestamps): 20 | fig, ax1 = plt.subplots() 21 | ax1.set_xlabel('time (s)') 22 | ax1.set_ylabel('Volume (m^3)', color='tab:red') 23 | ax1.plot(timestamps, volume, color='tab:red', linewidth=4) 24 | ax1.tick_params(axis='y', labelcolor='tab:red') 25 | ax2 = ax1.twinx() 26 | ax2.set_ylabel('Number of points in map', color='tab:blue') 27 | ax2.plot(timestamps, size, color='tab:blue', linewidth=4) 28 | ax2.tick_params(axis='y', labelcolor='tab:blue') 29 | fig.tight_layout() 30 | plt.show() 31 | 32 | def main(): 33 | if len(sys.argv)<2: 34 | print("Example Usage: python plot_volume_size_from_map_info.py base_name") 35 | sys.exit(1) 36 | bag = rosbag.Bag("map_info.bag") 37 | volume, size, timestamps = [], [], [] 38 | for topic, msg, t in bag.read_messages(topics=["/"+sys.argv[1]+"/lamp/map_info"]): 39 | size.append(msg.size) # TODO: Does this need scaling? 40 | volume.append(msg.volume) 41 | timestamps.append(t.to_sec()) 42 | timestamps = [el-timestamps[0] for el in timestamps] 43 | plot_volume_size(volume, size, timestamps) 44 | 45 | if __name__ == "__main__": 46 | main() -------------------------------------------------------------------------------- /utilities/pose_graph_to_odometry.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Converts a pose_graph.bag to an odometry.bag 4 | Author: 5 | - Matteo Palieri, NASA Jet Propulsion Laboratory 6 | """ 7 | 8 | import sys 9 | import rospy 10 | import rosbag 11 | from nav_msgs.msg import Odometry 12 | 13 | def extract_last_msg_from_bag(bag, topic): 14 | last_msg = None 15 | for topic, msg, t in bag.read_messages(topics=[topic]): 16 | last_msg = msg 17 | return last_msg 18 | 19 | def main(): 20 | 21 | if len(sys.argv)<5: 22 | print("Example Usage: python pose_graph_to_odometry.py pose_graph.bag pose_graph_topic odometry.bag odometry_topic") 23 | sys.exit(1) 24 | 25 | pose_graph_bag = rosbag.Bag(sys.argv[1]) 26 | pose_graph_topic = sys.argv[2] 27 | out_bag = sys.argv[3] 28 | out_topic = sys.argv[4] 29 | 30 | pose_graph = extract_last_msg_from_bag(pose_graph_bag, pose_graph_topic) 31 | 32 | if pose_graph is not None: 33 | with rosbag.Bag(out_bag, "w") as odometry_bag: 34 | for node in pose_graph.nodes: 35 | if node.ID=="odom_node": 36 | msg = Odometry() 37 | msg.header.stamp = node.header.stamp 38 | msg.pose.pose = node.pose 39 | odometry_bag.write(out_topic, msg, msg.header.stamp) 40 | odometry_bag.close() 41 | else: 42 | print("Unable to retrieve pose_graph from pose_graph.bag on specified pose_graph_topic") 43 | 44 | if __name__=="__main__": 45 | main() -------------------------------------------------------------------------------- /utilities/segment_aggregated_odometries.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - INPUT: 4 | - aggregated_odometries.bag 5 | - OUTPUT: 6 | - aggregated_odometries_100.bag 7 | - aggregated_odometries_200.bag 8 | - aggregated_odometries_300.bag 9 | Author: 10 | - Matteo Palieri, NASA Jet Propulsion Laboratory 11 | """ 12 | 13 | import sys 14 | import rosbag 15 | import numpy as np 16 | 17 | def main(): 18 | 19 | if len(sys.argv)<2: 20 | print("Minimal Usage: python segment_aggregated_odometries.py method1") 21 | print("Example Usage: python segment_aggregated_odometries.py method1 method2 method3 method4") 22 | sys.exit(1) 23 | 24 | methods = [] 25 | for i in range(len(sys.argv)): 26 | if i!=0: 27 | methods.append(sys.argv[i]) 28 | 29 | checkpoints = [100, 200] 30 | 31 | for checkpoint in checkpoints: 32 | 33 | outbag = rosbag.Bag("aggregated_odometries_" + str(checkpoint) + ".bag", "w") 34 | 35 | initialized = False 36 | time_end_point = None 37 | distance_traveled = 0 38 | previous_position = None 39 | 40 | for topic, msg, t in rosbag.Bag("aggregated_odometries.bag").read_messages(topics=["ground_truth"]): 41 | 42 | if not initialized: 43 | previous_position = msg.pose.pose.position 44 | initialized = True 45 | pass 46 | 47 | current_position = msg.pose.pose.position 48 | dx = np.square(current_position.x - previous_position.x) 49 | dy = np.square(current_position.y - previous_position.y) 50 | dz = np.square(current_position.z - previous_position.z) 51 | distance_traveled = distance_traveled + np.sqrt(dx + dy + dz) 52 | previous_position = current_position 53 | 54 | if distance_traveled>checkpoint: 55 | time_end_point = t 56 | break 57 | 58 | for method in methods: 59 | for topic, msg, t in rosbag.Bag("aggregated_odometries.bag").read_messages(topics=[method]): 60 | outbag.write(method, msg , t) 61 | if t>time_end_point: 62 | break 63 | 64 | outbag.close() 65 | 66 | if __name__ == "__main__": 67 | main() -------------------------------------------------------------------------------- /utilities/utilities.py: -------------------------------------------------------------------------------- 1 | """ 2 | Description: 3 | - Utilities used by analyzer.py 4 | Author: 5 | - Matteo Palieri, NASA Jet Propulsion Laboratory 6 | - Andrzej Reinke, NASA Jet Propulsion Laboratory 7 | """ 8 | 9 | import os 10 | import yaml 11 | import math 12 | import rospy 13 | import rosbag 14 | import zipfile 15 | import matplotlib 16 | import numpy as np 17 | import seaborn as sns 18 | import matplotlib.cm as cm 19 | import scipy.stats as stats 20 | from nav_msgs.msg import Odometry 21 | from matplotlib.lines import Line2D 22 | from matplotlib import pyplot as plt 23 | from loop_closure_eval import LoopClosureEvaluator 24 | #import key_handling 25 | from functools import reduce 26 | matplotlib.rcParams['mathtext.fontset'] = 'stix' 27 | matplotlib.rcParams['font.family'] = 'STIXGeneral' 28 | size, labelsize = 50, 60 29 | params = {'legend.fontsize': 'large', 30 | 'figure.figsize': (20,8), 31 | 'axes.labelsize': labelsize, 32 | 'axes.titlesize': labelsize, 33 | 'xtick.labelsize': size*0.75, 34 | 'ytick.labelsize': size*0.75, 35 | 'axes.titlepad': 25} 36 | plt.rcParams.update(params) 37 | 38 | methods = [] 39 | color_mapping = {0:"red", 1:"green", 2:"blue", 3:"yellow", 4:"pink", 5:"black"} 40 | 41 | def enable_dark_mode(): 42 | global plt 43 | plt.style.use("dark_background") 44 | 45 | def enable_full_screen(): 46 | global plt 47 | mng = plt.get_current_fig_manager() 48 | mng.resize(*mng.window.maxsize()) 49 | 50 | def get_avg(data): 51 | return reduce(lambda x,y: x+y, data)/len(data) 52 | 53 | def evaluate_normal_distribution(data): 54 | distributions = [[],[],[],[],[]] 55 | for i in range(len(data)): 56 | for el in data[i]: 57 | if len(el) != 0: 58 | min = np.min(el) 59 | max = np.max(el) 60 | avg = get_avg(el) 61 | var = np.var(el) 62 | distributions[i].append([min, max, avg, var]) 63 | else: 64 | distributions[i].append([0, 0, 0, 0]) 65 | return distributions 66 | 67 | def visualize_raw_data(title, units, data, results_dir, show=False, fullscreen=False): 68 | fig = plt.figure() 69 | for i in range(len(data)): 70 | if icheckpoint: 176 | time_end_point = t 177 | break 178 | for method in methods: 179 | for topic, msg, t in rosbag.Bag("aggregated_odometries.bag").read_messages(topics=[method]): 180 | outbag.write(method, msg , t) 181 | if t>time_end_point: 182 | break 183 | outbag.close() 184 | 185 | def boxplot_from_npz(methods, checkpoints, results_dir, show=False, fullscreen=False): 186 | data = [] 187 | for distance in checkpoints: 188 | for method in methods: 189 | filename = os.getcwd() + "/ape_results_" + str(distance) + "/" + method +"/error_array.npy" 190 | data.append(np.load(filename)) 191 | data.append([]) 192 | bplot = sns.boxplot(data=[d for d in data], width=0.5) 193 | colors = ["red", "green", "blue", "yellow", "gray", "black"] 194 | color_counter = 0 195 | for i in range(len(data)-len(checkpoints)): 196 | mybox = bplot.artists[i] 197 | mybox.set_facecolor(colors[color_counter]) 198 | color_counter = color_counter + 1 199 | if color_counter == len(methods): 200 | color_counter=0 201 | bplot.set(xlabel= " Distance traveled [m] ", ylabel='APE [m]') 202 | bplot.set_xticks(range((len(methods)*len(checkpoints)) + len(checkpoints))) 203 | bplot.set_xticklabels([" " for c in checkpoints]) # TODO: xtick checkpoints 204 | legend_elements = [] 205 | for i in range(len(methods)): 206 | legend_elements.append(Line2D([0], [0], color=colors[i], lw=4, label=methods[i]),) 207 | leg = plt.legend(handles=legend_elements, loc='upper_left', prop={'size': 30}) 208 | for legobj in leg.legendHandles: 209 | legobj.set_linewidth(10.0) 210 | plt.rcParams.update(params) 211 | plt.title('APE over distances') 212 | if fullscreen: 213 | enable_full_screen() 214 | if show: 215 | plt.show() 216 | plt.close() 217 | 218 | def make_evo_options_string(ape_type, method, b_align_origin, b_show, b_save): 219 | 220 | out_string = "" 221 | 222 | if b_align_origin: 223 | out_string += " --align_origin" 224 | 225 | if b_show: 226 | out_string += " --plot" 227 | 228 | if b_save: 229 | if ape_type == "single": 230 | out_string += " --save_plot" 231 | out_string += " plots/" + method + "_ape_plots.pdf" 232 | elif ape_type == "combined": 233 | out_string += " --save_plot" 234 | out_string += " plots/combined_ape_plots.pdf" 235 | out_string += " --save_table" 236 | out_string += " combined_ape.csv" 237 | elif ape_type == "traj": 238 | out_string += " --save_plot" 239 | out_string += " plots/traj_plots.pdf" 240 | elif ape_type == "trans": 241 | out_string += " --save_plot" 242 | out_string += " plots/trans_plots_" + method + ".pdf" 243 | elif ape_type == "rot": 244 | out_string += " --save_plot" 245 | out_string += " plots/rot_plots_" + method + ".pdf" 246 | elif ape_type == "combined_trans": 247 | out_string += " --save_plot" 248 | out_string += " plots/trans_plots.pdf" 249 | out_string += " --save_table" 250 | out_string += " trans_ape.csv" 251 | elif ape_type == "combined_rot": 252 | out_string += " --save_plot" 253 | out_string += " plots/rot_plots.pdf" 254 | out_string += " --save_table" 255 | out_string += " rot_ape.csv" 256 | else: 257 | print("Error - first input should be 'single' or 'combined'") 258 | 259 | return out_string 260 | 261 | 262 | def compute_distance_based_error_metrics(methods, results_dir, show=False, fullscreen=False, min_move=1.0): 263 | ape_trans = {} 264 | ape_rot = {} 265 | timestamps = {} 266 | initialized = {} 267 | time_end_point = {} 268 | distance_traveled = {} 269 | distance_time = {} 270 | previous_position = {} 271 | 272 | for method in methods: 273 | filename = results_dir + "ape_results/" + method +"_trans/error_array.npy" 274 | ape_trans[method] = np.load(filename) 275 | filename = results_dir + "ape_results/" + method +"_rot/error_array.npy" 276 | ape_rot[method] = np.load(filename) 277 | filename = results_dir + "ape_results/" + method +"_trans/timestamps.npy" 278 | timestamps[method] = np.load(filename) 279 | initialized[method] = False 280 | distance_traveled[method] = [] 281 | previous_position[method] = None 282 | distance_time[method] = [] 283 | 284 | # Add ground truth? 285 | initialized['ground_truth'] = False 286 | distance_traveled['ground_truth'] = [] 287 | previous_position['ground_truth'] = None 288 | distance_time['ground_truth'] = [] 289 | 290 | for topic, msg, t in rosbag.Bag(results_dir + "aggregated_odometries.bag").read_messages(): 291 | 292 | if topic in methods or topic == 'ground_truth': 293 | distance_time[topic].append(msg.header.stamp.to_sec()) 294 | if not initialized[topic]: 295 | previous_position[topic] = msg.pose.pose.position 296 | initialized[topic] = True 297 | distance_traveled[topic].append(0) 298 | continue 299 | 300 | current_position = msg.pose.pose.position 301 | dx = np.square(current_position.x - previous_position[topic].x) 302 | dy = np.square(current_position.y - previous_position[topic].y) 303 | dz = np.square(current_position.z - previous_position[topic].z) 304 | distance_traveled[topic].append(distance_traveled[topic][-1] + np.sqrt(dx + dy + dz)) 305 | previous_position[topic] = current_position 306 | 307 | # Match timestamps 308 | matched_distances = {} 309 | eps = 3e-2 310 | adjust_count = 0 311 | for method in methods: 312 | indices = np.searchsorted(distance_time['ground_truth'], timestamps[method]) 313 | for i in range(len(indices)): 314 | d1 = abs(distance_time['ground_truth'][indices[i]] - timestamps[method][i]) 315 | if d1 > eps: 316 | if indices[i] > 0: 317 | d2 = abs(distance_time['ground_truth'][indices[i]-1] - timestamps[method][i]) 318 | if d2 < d1: 319 | indices[i] = indices[i] - 1 320 | adjust_count += 1 321 | matched_distances[method] = np.take(distance_traveled['ground_truth'],indices) 322 | 323 | print("Number to adjust = {}".format(adjust_count)) 324 | # Compute metrics 325 | error_percentage_trans = {} 326 | error_percentage_rot = {} 327 | err_perc_trans_average = {} 328 | err_perc_rot_average = {} 329 | i_start = {} 330 | # Start when we have moved at least min_move m 331 | i_start['ground_truth'] = np.searchsorted(distance_traveled['ground_truth'], min_move) 332 | t_start = distance_time['ground_truth'][i_start['ground_truth']] 333 | 334 | for method in methods: 335 | error_percentage_trans[method] = ape_trans[method]/matched_distances[method] 336 | error_percentage_rot[method] = ape_rot[method]/matched_distances[method] 337 | i_start[method] = np.searchsorted(timestamps[method], t_start) 338 | err_perc_trans_average[method] = np.mean(error_percentage_trans[method][i_start[method]:]) 339 | err_perc_rot_average[method] = np.mean(error_percentage_rot[method][i_start[method]:]) 340 | print("Translational Error Percentage for method {} is {} %".format(method, err_perc_trans_average[method])) 341 | print("Rotational error deg/m for method {} is {} %".format(method, err_perc_rot_average[method])) 342 | 343 | 344 | # Plot distance vs time 345 | fig = plt.figure() 346 | for method in methods: 347 | plt.plot(timestamps[method], matched_distances[method], label=method, linewidth=3) 348 | 349 | title = "DistancevsTime" 350 | plt.xlabel("Time (s)") 351 | plt.ylabel("Distance Traveled (m)") 352 | plt.legend(prop={'size': 30}) 353 | if fullscreen: 354 | enable_full_screen() 355 | plt.tight_layout() 356 | fig.savefig(results_dir + "/plots/" + title) 357 | if show: 358 | plt.show() 359 | plt.close() 360 | 361 | 362 | # Plot APE vs distance 363 | fig = plt.figure() 364 | for method in methods: 365 | plt.plot(matched_distances[method], ape_trans[method], label=method, linewidth=3) 366 | 367 | title = "APETransvsDistance" 368 | plt.xlabel("Distance Traveled (m)") 369 | plt.ylabel("APE (trans) (m)") 370 | plt.legend(prop={'size': 30}) 371 | if fullscreen: 372 | enable_full_screen() 373 | plt.tight_layout() 374 | fig.savefig(results_dir + "/plots/" + title) 375 | if show: 376 | plt.show() 377 | plt.close() 378 | 379 | # Plot APE Rot vs distance 380 | fig = plt.figure() 381 | for method in methods: 382 | plt.plot(matched_distances[method], ape_rot[method], label=method, linewidth=3) 383 | 384 | title = "APERotvsDistance" 385 | plt.xlabel("Distance Traveled (m)") 386 | plt.ylabel("APE (rot) (deg)") 387 | plt.legend(prop={'size': 30}) 388 | if fullscreen: 389 | enable_full_screen() 390 | plt.tight_layout() 391 | fig.savefig(results_dir + "/plots/" + title) 392 | if show: 393 | plt.show() 394 | plt.close() 395 | 396 | # Plot Error % vs distance 397 | fig = plt.figure() 398 | for method in methods: 399 | plt.plot(matched_distances[method][i_start[method]:], error_percentage_trans[method][i_start[method]:], label=method, linewidth=3) 400 | 401 | title = "ErrorPercvsdistance" 402 | plt.xlabel("Distance Traveled (m)") 403 | plt.ylabel("Translational Error (%)") 404 | plt.legend(prop={'size': 30}) 405 | # plt.yscale("log") 406 | if fullscreen: 407 | enable_full_screen() 408 | plt.tight_layout() 409 | fig.savefig(results_dir + "/plots/" + title) 410 | if show: 411 | plt.show() 412 | plt.close() 413 | 414 | # Plot Rotational Error deg/m vs distance 415 | fig = plt.figure() 416 | for method in methods: 417 | plt.plot(matched_distances[method][i_start[method]:], error_percentage_rot[method][i_start[method]:], label=method, linewidth=3) 418 | 419 | title = "ErrorPercRotvsdistance" 420 | plt.xlabel("Distance Traveled (m)") 421 | plt.ylabel("Rotational Error (deg/m)") 422 | plt.legend(prop={'size': 30}) 423 | # plt.yscale("log") 424 | if fullscreen: 425 | enable_full_screen() 426 | plt.tight_layout() 427 | fig.savefig(results_dir + "/plots/" + title) 428 | if show: 429 | plt.show() 430 | plt.close() 431 | 432 | 433 | err_perc_average = {} 434 | err_perc_average['trans'] = err_perc_trans_average 435 | err_perc_average['rot'] = err_perc_rot_average 436 | 437 | return err_perc_average 438 | 439 | 440 | def write_main_results_to_csv(methods, results_dir, err_perc, distributions, bandwidth_stats): 441 | import csv 442 | # Distributions has [cpus, mems, rates, delays, times] 443 | 444 | ape_stats = {} 445 | trans_ape_stats = {} 446 | rot_ape_stats = {} 447 | 448 | # Read in the APE results 449 | with open(results_dir + "combined_ape.csv") as apefile: 450 | reader = csv.reader(apefile, delimiter=',', quotechar="|") 451 | firstline = True 452 | for row in reader: 453 | if firstline: 454 | firstline = False 455 | else: 456 | name = row[0][12:-4] 457 | ape_stats[name] = [] 458 | for el in row[1:]: 459 | ape_stats[name].append(float(el)) 460 | with open(results_dir + "trans_ape.csv") as apefile: 461 | reader = csv.reader(apefile, delimiter=',', quotechar="|") 462 | firstline = True 463 | for row in reader: 464 | if firstline: 465 | firstline = False 466 | else: 467 | name = row[0][12:-10] 468 | trans_ape_stats[name] = [] 469 | for el in row[1:]: 470 | trans_ape_stats[name].append(float(el)) 471 | with open(results_dir + "rot_ape.csv") as apefile: 472 | reader = csv.reader(apefile, delimiter=',', quotechar="|") 473 | firstline = True 474 | for row in reader: 475 | if firstline: 476 | firstline = False 477 | else: 478 | name = row[0][12:-8] 479 | rot_ape_stats[name] = [] 480 | for el in row[1:]: 481 | rot_ape_stats[name].append(float(el)) 482 | 483 | # Write out the summary CSV 484 | with open(results_dir + "main_results_tmp.csv", 'w') as outfile: 485 | writer = csv.writer(outfile, delimiter=",", quotechar='|', quoting=csv.QUOTE_MINIMAL) 486 | writer.writerow([ 'Method', 487 | 'Max Translational APE (m)', 488 | 'Mean Translational Error %', 489 | 'Max Rotational APE (deg)', 490 | 'Mean Rotational Error (deg/m)', 491 | 'Average CPU (%)', 492 | 'Max CPU (%)', 493 | 'Max memory (GB)', 494 | 'Max Bandwidth (KB/s)', 495 | 'Mean Bandwidth (KB/s)', 496 | 'Max odom delay (s)', 497 | 'Mean odom delay (s)', 498 | ]) 499 | 500 | # Add more fields later: Rotation, 'Avg Memory Rate (GB)' 501 | i = 0 502 | bw_stat_1 = 0.0 503 | bw_stat_2 = 0.0 504 | if bandwidth_stats: 505 | bw_stat_1 = bandwidth_stats["max"] 506 | bw_stat_2 = bandwidth_stats["mean"] 507 | 508 | for method in methods: 509 | writer.writerow([ 510 | method, 511 | trans_ape_stats[method][0], 512 | err_perc['trans'][method], 513 | rot_ape_stats[method][0], 514 | err_perc['rot'][method], 515 | distributions[0][i][2], 516 | distributions[0][i][1], 517 | distributions[1][i][1], 518 | bw_stat_1, 519 | bw_stat_2, 520 | distributions[3][i][1], 521 | distributions[3][i][2] 522 | ]) 523 | i = i+1 524 | 525 | # Transpose the csv 526 | #from itertools import zip 527 | a = zip(*csv.reader(open(results_dir + "main_results_tmp.csv", "rt"))) 528 | csv.writer(open(results_dir + "main_results.csv", "wt")).writerows(a) 529 | os.remove(results_dir + "main_results_tmp.csv") 530 | 531 | def analyze_bandwidth(lamp_data_path, result_data_path): 532 | input_file_name = lamp_data_path + "/bandwidth.txt" 533 | output_file_name = result_data_path + "/bandwidth_result.txt" 534 | # read from saved text file 535 | input_file = open(input_file_name, 'r') 536 | lines = input_file.readlines() 537 | 538 | max_bw = 0 539 | sum_of_mean_bw = 0 540 | count_bw = 0 541 | first_message = "" 542 | last_message = "" 543 | 544 | for line in lines: 545 | line_list = line.split() 546 | if line_list[0] == "mean:": 547 | try: 548 | if count_bw == 0: 549 | first_message = line 550 | last_message = line 551 | maxstr = line_list[5] 552 | meanstr = line_list[1] 553 | if (float(maxstr[:-2]) > max_bw): 554 | max_bw = float(maxstr[:-2]) 555 | sum_of_mean_bw += float(meanstr[:-2]) 556 | count_bw += 1 557 | except: 558 | continue 559 | 560 | input_file.close() 561 | 562 | # write output 563 | if count_bw == 0: 564 | bandwidth_stats = {} 565 | bandwidth_stats["max"] = 0.0 566 | bandwidth_stats["mean"] = 0.0 567 | return bandwidth_stats 568 | 569 | ouput_content = ["max: " + str(max_bw) + "\n", "mean: " + str(sum_of_mean_bw/count_bw) + "\n"] 570 | ouput_content.append("first message:" + first_message) 571 | ouput_content.append("last message:" + last_message) 572 | output_file = open(output_file_name, 'w') 573 | output_file.writelines(ouput_content) 574 | output_file.close() 575 | 576 | bandwidth_stats = {} 577 | bandwidth_stats["max"] = max_bw 578 | bandwidth_stats["mean"] = sum_of_mean_bw/count_bw 579 | return bandwidth_stats 580 | 581 | def evaluate_loop_closures(pose_graph_bag, pose_graph_topic, 582 | loop_closure_bag, loop_closure_topic, gt_odom_bag, gt_odom_topic, output_dir): 583 | loop_closure_eval = LoopClosureEvaluator( 584 | pose_graph_bag, pose_graph_topic, loop_closure_bag, loop_closure_topic, gt_odom_bag, gt_odom_topic) 585 | loop_closure_eval.evaluate() 586 | if loop_closure_eval.get_num_loopclosures() > 0: 587 | loop_closure_eval.save_accuracy_plot(filename=output_dir + "/lc_accuracy.png") 588 | loop_closure_eval.save_inlier_accuracy_plot(filename=output_dir + "/inlier_lc_accuracy.png") 589 | if loop_closure_eval.get_num_inliers() > 0: 590 | loop_closure_eval.save_avg_accuracy_plot(filename=output_dir + "/per_node_lc_accuracy.png") 591 | loop_closure_eval.save_avg_inlier_accuracy_plot(filename=output_dir + "/per_node_inlier_lc_accuracy.png") 592 | loop_closure_eval.save_covariance_plot(filename=output_dir + "/lc_covariance.png") 593 | 594 | def get_loop_closures_stats(pose_graph_bag, pose_graph_topic, 595 | loop_closure_bag, loop_closure_topic, output_dir): 596 | loop_closure_stats = LoopClosureEvaluator( 597 | pose_graph_bag, pose_graph_topic, loop_closure_bag, loop_closure_topic) 598 | loop_closure_stats.save_stats(output_dir) 599 | 600 | def imageToPDF(filename): 601 | from PIL import Image 602 | img = Image.open(filename) 603 | img1 = img.convert('RGB') 604 | img1.save(filename[:-3] + "pdf") 605 | 606 | def creatOutputPDF(results_dir): 607 | 608 | # Convert images desired into pdf 609 | ape_rot = results_dir + "/plots/APERotvsDistance.png" 610 | imageToPDF(ape_rot) 611 | ape_trans = results_dir + "/plots/APETransvsDistance.png" 612 | imageToPDF(ape_trans) 613 | cpu = results_dir + "/plots/Raw_Cpu_Load.png" 614 | imageToPDF(cpu) 615 | mem = results_dir + "/plots/Raw_Memory_Load.png" 616 | imageToPDF(mem) 617 | delay = results_dir + "/plots/Raw_Odometry_Delay.png" 618 | imageToPDF(delay) 619 | 620 | # Create pdf and start adding pages 621 | from PyPDF2 import PdfFileReader, PdfFileWriter 622 | pdf_writer = PdfFileWriter() 623 | 624 | # Add trajectory 625 | input_pdf = PdfFileReader(results_dir + "/plots/traj_plots.pdf") 626 | pdf_writer.addPage(input_pdf.getPage(0)) 627 | 628 | # Add colored error plots 629 | input_pdf = PdfFileReader(results_dir + "/plots/lamp_ape_plots.pdf") 630 | pdf_writer.addPage(input_pdf.getPage(1)) 631 | 632 | # Add error vs distance 633 | input_pdf = PdfFileReader(results_dir + "/plots/APETransvsDistance.pdf") 634 | pdf_writer.addPage(input_pdf.getPage(0)) 635 | input_pdf = PdfFileReader(results_dir + "/plots/APERotvsDistance.pdf") 636 | pdf_writer.addPage(input_pdf.getPage(0)) 637 | 638 | # Add box plot 639 | input_pdf = PdfFileReader(results_dir + "/plots/combined_ape_plots.pdf") 640 | pdf_writer.addPage(input_pdf.getPage(3)) 641 | 642 | # More Stats on overall error 643 | input_pdf = PdfFileReader(results_dir + "/plots/combined_ape_plots.pdf") 644 | pdf_writer.addPage(input_pdf.getPage(1)) 645 | pdf_writer.addPage(input_pdf.getPage(4)) 646 | 647 | # X, Y, Z error 648 | input_pdf = PdfFileReader(results_dir + "/plots/traj_plots.pdf") 649 | pdf_writer.addPage(input_pdf.getPage(1)) 650 | 651 | # Computation plots 652 | input_pdf = PdfFileReader(results_dir + "/plots/Raw_Cpu_Load.pdf") 653 | pdf_writer.addPage(input_pdf.getPage(0)) 654 | input_pdf = PdfFileReader(results_dir + "/plots/Raw_Memory_Load.pdf") 655 | pdf_writer.addPage(input_pdf.getPage(0)) 656 | input_pdf = PdfFileReader(results_dir + "/plots/Raw_Odometry_Delay.pdf") 657 | pdf_writer.addPage(input_pdf.getPage(0)) 658 | 659 | # Write to file 660 | with open(results_dir + "/main_results.pdf", 'wb') as outfile: 661 | pdf_writer.write(outfile) 662 | -------------------------------------------------------------------------------- /utilities/volume_over_time.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import rospy 3 | import rosbag 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | 7 | def extract_last_msg_from_bag(bag, topic): 8 | last_msg = None 9 | for topic, msg, t in bag.read_messages(topics=[topic]): 10 | last_msg = msg 11 | return last_msg 12 | 13 | def main(): 14 | 15 | bag = rosbag.Bag(sys.argv[1]) 16 | 17 | time_vec = [] 18 | volume_vec = [] 19 | points_vec = [] 20 | 21 | for topic, msg, t in bag.read_messages(): 22 | time_vec.append(t.to_sec()) 23 | volume_vec.append(msg.volume) 24 | points_vec.append(float(msg.size)) 25 | 26 | # Plot the results 27 | 28 | fig, ax1 = plt.subplots() 29 | 30 | color = 'tab:red' 31 | ax1.set_xlabel('time (s)') 32 | ax1.set_ylabel('Volume (m^3)', color=color) 33 | ax1.plot(time_vec, volume_vec, color=color) 34 | ax1.tick_params(axis='y', labelcolor=color) 35 | 36 | ax2 = ax1.twinx() # instantiate a second axes that shares the same x-axis 37 | 38 | color = 'tab:blue' 39 | ax2.set_ylabel('# of points in map', color=color) # we already handled the x-label with ax1 40 | ax2.plot(time_vec, points_vec, color=color) 41 | ax2.tick_params(axis='y', labelcolor=color) 42 | 43 | fig.tight_layout() # otherwise the right y-label is slightly clipped 44 | plt.show() 45 | 46 | fig, ax3 = plt.subplots() 47 | color = 'tab:red' 48 | ax3.set_xlabel('# points') 49 | ax3.set_ylabel('Volume (m^3)', color=color) 50 | ax3.plot(points_vec, volume_vec, color=color) 51 | ax3.tick_params(axis='y', labelcolor=color) 52 | 53 | plt.show() 54 | 55 | 56 | 57 | if __name__=="__main__": 58 | main() --------------------------------------------------------------------------------