import numpy as np import matplotlib.pyplot as plt from matplotlib.lines import Line2D from framework import utils from framework import viz_utils ################################################################################################# # CONFIG # ################################################################################################# original_execution_dir = r'/home/omer/orchards_ws/results/global_ekf_updates' ################################################################################################# if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('global_ekf_updates_post_processing') ekf_errors = {} ekf_with_aerial_update_errors = {} fig = plt.figure(figsize=(8.5, 11)) for experiment_idx, experiment_name in enumerate(sorted(os.listdir(original_execution_dir))): with open(os.path.join(original_execution_dir, experiment_name, 'experiment_summary.json')) as f: experiment_summary = json.load(f) with open(experiment_summary['data_sources']['ugv_poses_path']) as f: ugv_poses_path = json.load(f) relevant_update_index = experiment_summary['data_sources']['relevant_update_index'] ekf_df = pd.read_csv(experiment_summary['results']['1']['ekf_pose_path'], index_col=0) ekf_with_aerial_update_df = pd.read_csv(experiment_summary['results']['1']['ekf_with_aerial_update_path'], index_col=0) update_time = ekf_with_aerial_update_df.loc[(ekf_with_aerial_update_df['pose.pose.position.x'].diff().abs() > 0.5) | (ekf_with_aerial_update_df['pose.pose.position.y'].diff().abs() > 0.5)].index[0] ekf_before_update_df = ekf_with_aerial_update_df[ekf_with_aerial_update_df.index < update_time] ekf_without_update_df = ekf_df[ekf_df.index > update_time]
markers_locations_json_paths = [ dji.snapshots_60_meters_markers_locations_json_path, dji.snapshots_80_meters_markers_locations_json_path ] fork_shaped = False S_shaped = False random_shaped = True if __name__ == '__main__': if image_path is not None: raise NotImplemented group_idx = 1 for snapshots_group, markers_locations_json_path in zip( snapshots_groups, markers_locations_json_paths): execution_dir = utils.create_new_execution_folder( 'trajectory_tagging_%d' % group_idx) group_idx += 1 with open(markers_locations_json_path) as f: markers_locations = json.load(f) for key, data_descriptor in snapshots_group.items(): if relevant_keys is not None: if key not in relevant_keys: continue points = markers_locations[key] image = cv2.imread(data_descriptor.path) map_image = maps_generation.generate_canopies_map(image) (upper_left_x, upper_left_y), (lower_right_x, lower_right_y) = cv_utils.get_bounding_box( map_image, points,
# Stop recording output bag # ros_utils.stop_recording_bags() # Kill RVIZ if launch_rviz: ros_utils.kill_rviz() if __name__ == '__main__': from content.data_pointers.lavi_april_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_april_18 import base_resources_path, base_raw_data_path from content.data_pointers.lavi_april_18 import orchard_topology execution_dir = utils.create_new_execution_folder('amcl_video') td_exp_name_for_map = 'trunks_detection_on_apr_16-55-1' with open( os.path.join(td_results_dir, td_exp_name_for_map, 'experiment_summary.json')) as f: td_summary_for_map = json.load(f) image_path = td_summary_for_map['data_sources'] map_semantic_trunks = td_summary_for_map['results']['1']['semantic_trunks'] pixel_to_meter_ratio_for_map = calibration.calculate_pixel_to_meter( td_summary_for_map['results']['1']['optimized_grid_dim_x'], td_summary_for_map['results']['1']['optimized_grid_dim_y'], orchard_topology.measured_row_widths, orchard_topology.measured_intra_row_distances) map_resolution = (1.0 / pixel_to_meter_ratio_for_map) * 1.5 experiment = AmclVideoExperiment(name='amcl_video',
from content.data_pointers.lavi_april_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_april_18.dji import selected_trunks_detection_experiments as selected_td_experiments from content.data_pointers.lavi_april_18.dji import snapshots_80_meters_markers_locations_json_path as markers_json_path elif setup == 'nov1': from content.data_pointers.lavi_november_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_november_18.dji import plot1_selected_trunks_detection_experiments as selected_td_experiments from content.data_pointers.lavi_november_18.dji import plot1_snapshots_80_meters_markers_locations_json_path as markers_json_path elif setup == 'nov2': from content.data_pointers.lavi_november_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_november_18.dji import plot2_selected_trunks_detection_experiments as selected_td_experiments from content.data_pointers.lavi_november_18.dji import plot2_snapshots_80_meters_markers_locations_json_path as markers_json_path else: raise NotImplementedError if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('image_alignment_%s' % setup) results_df = pd.DataFrame( index=map(lambda c: '%s__%s' % (c[0], c[1]), combinations(selected_td_experiments, 2)), columns=[ 'by_trunks/mse', 'by_orb/mse', 'by_markers/mse', 'by_trunks/ssim', 'by_orb/ssim', 'by_markers/ssim' ]) with open(markers_json_path) as f: all_markers_locations = json.load(f) for experiment1, experiment2 in combinations(selected_td_experiments, r=2): with open( os.path.join(td_results_dir, experiment1, 'experiment_summary.json')) as f: td_summary1 = json.load(f) with open(
from framework import utils from content.experiments.jackal_gmapping import JackalGmapping ################################################################################################# # CONFIG # ################################################################################################# ugv_bag_name = '18-24-26' periodic_map_saving = False setup = 'apr' # apr / nov / lab ################################################################################################# if setup == 'apr': from content.data_pointers.lavi_april_18.jackal import jackal_18 as ugv_pointers elif setup == 'nov': raise NotImplementedError elif setup == 'lab': raise NotImplementedError if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('jackal_gmapping') bag_descriptor = ugv_pointers[ugv_bag_name] experiment = JackalGmapping(name='gmapping_on_apr_%s' % ugv_bag_name, data_sources=bag_descriptor.path, working_dir=execution_dir) experiment.run(repetitions=1, periodic_map_saving=periodic_map_saving)
import os import pandas as pd import matplotlib.pyplot as plt from framework import utils from framework import viz_utils ################################################################################################# # CONFIG # ################################################################################################# original_experiment_dir = r'/home/omer/temp/20190316-152308_apr_icp/20190316-152308_icp_snapshots_for_fork_trajectory_on_15-08-1' repetition_ids = [1, 2, 3] ################################################################################################# if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('icp_post_processing') for repetition_id in repetition_ids: plt.figure() icp_df = pd.read_csv(os.path.join(original_experiment_dir, str(repetition_id), 'canopies_icp_results.csv'), index_col=0) viz_utils.plot_2d_trajectory([ icp_df.loc[:, [ 'icp_pose_x[%d]' % repetition_id, 'icp_pose_y[%d]' % repetition_id ]] ]) plt.savefig(os.path.join(execution_dir, 'icp_%d.jpg' % repetition_id))
return convergence_to_band_ratio, average_first_time_in_band, average_in_band_ratio, average_std, average_mean def plot_canopies_vs_trunks(canopies_vector, trunks_vector, canopies_stds, trunks_stds, x_label=None, y_label=None): viz_utils.plot_line_with_sleeve(canopies_vector, 2 * canopies_stds, 'green') viz_utils.plot_line_with_sleeve(trunks_vector, 2 * trunks_stds, 'sienna') plt.autoscale(enable=True, axis='x', tight=True) plt.ylim(bottom=0) if x_label is not None: plt.xlabel(x_label) if y_label is not None: plt.ylabel(y_label) if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('amcl_simulation_post_processing_%s' % os.path.basename(original_execution_dirname)) results_list = [] if plot: fig = plt.figure(figsize=(8.5, 11)) ax_errors = None ax_covariance_norms = None full_experiment_names = filter(lambda name: name.find('png') == -1, os.listdir(os.path.join(base_results_path, 'amcl', original_execution_dirname))) sorted_full_experiment_names = [] for trajectory_name in ['narrow_row', 'wide_row', 's_patrol', 'u_turns', 'tasks_and_interrupts']: for full_experiment_name in full_experiment_names: if full_experiment_name.find(trajectory_name) != -1: sorted_full_experiment_names.append(full_experiment_name) for experiment_idx, experiment_name in enumerate(sorted_full_experiment_names): with open(os.path.join(base_results_path, 'amcl', original_execution_dirname, experiment_name, 'experiment_summary.json')) as f: experiment_summary = json.load(f) trajectory_name = experiment_summary['metadata']['trajectory_name']
################################################################################################# ugv_bag_name = '18-24-26' icp_experiment_path = os.path.join(config.base_results_path, 'jackal_icp') setup = 'apr' # apr / nov / lab ################################################################################################# if setup == 'apr': from content.data_pointers.lavi_april_18.jackal import jackal_18 as ugv_pointers elif setup == 'nov': raise NotImplementedError elif setup == 'lab': raise NotImplementedError if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('jackal_baseline') bag_descriptor = ugv_pointers[ugv_bag_name] odom = ros_utils.bag_to_dataframe( bag_descriptor.path, topic=r'/jackal_velocity_controller/odom', fields=['pose.pose.position.x', 'pose.pose.position.y']) ekf = ros_utils.bag_to_dataframe( bag_descriptor.path, topic=r'/odometry/filtered', fields=['pose.pose.position.x', 'pose.pose.position.y']) icp_bag_path = os.path.join(icp_experiment_path, '1', 'icp_on_apr_%s_output.bag' % ugv_bag_name) icp = ros_utils.bag_to_dataframe(icp_bag_path, topic=r'/scanmatcher_pose', fields=['x', 'y'])
from framework import config from content.data_pointers.lavi_april_18 import dji image_path = r'/home/omer/orchards_ws/data/lavi_apr_18/panorama/DJI_0178_afternoon_good_stitch_full_movie_cropped.jpg' # ignored if None map_name = 'panorama_cropped' # ignored if None relevant_keys = None # if None, take all snapshots_groups = [dji.snapshots_60_meters, dji.snapshots_80_meters] markers_locations_json_paths = [ dji.snapshots_60_meters_markers_locations_json_path, dji.snapshots_80_meters_markers_locations_json_path ] if __name__ == '__main__': if image_path is not None: execution_dir = utils.create_new_execution_folder('map_generation') image = cv2.imread(image_path) map_image = maps_generation.generate_canopies_map(image) ros_utils.save_image_to_map(map_image, resolution=config.top_view_resolution, map_name='%s_map' % map_name, dir_name=execution_dir) else: group_idx = 1 for snapshots_group, markers_locations_json_path in zip( snapshots_groups, markers_locations_json_paths): execution_dir = utils.create_new_execution_folder( 'map_generation_%d' % group_idx) group_idx += 1 with open(markers_locations_json_path) as f: markers_locations = json.load(f)
from content.data_pointers.lavi_november_18.dji import plot1_snapshots_80_meters as snapshots from content.data_pointers.lavi_november_18.orchard_topology import plot1_pattern as plot_pattern elif setup == 'nov2': from content.data_pointers.lavi_november_18.dji import plot2_snapshots_80_meters as snapshots from content.data_pointers.lavi_november_18.orchard_topology import plot2_pattern as plot_pattern elif setup == 'nov3': from content.data_pointers.lavi_november_18.dji import plot3_snapshots_80_meters as snapshots from content.data_pointers.lavi_november_18.orchard_topology import plot3_pattern as plot_pattern elif setup == 'nov4': from content.data_pointers.lavi_november_18.dji import plot4_snapshots_80_meters as snapshots from content.data_pointers.lavi_november_18.orchard_topology import plot4_pattern as plot_pattern image_keys = ['15-17-1', '15-18-3', '15-19-1'] if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('trunks_tagging') for image_key in image_keys: summary = {} summary['results'] = {} summary['results'][0] = {} summary['results'][0]['semantic_trunks'] = {} summary['metadata'] = {} summary['metadata']['image_key'] = image_key summary['metadata']['altitude'] = altitude data_descriptor = snapshots[image_key] summary['data_sources'] = data_descriptor.path image = cv2.imread(data_descriptor.path) for i in range(plot_pattern.shape[0]): for j in range(plot_pattern.shape[1]): if plot_pattern[(i, j)] == -1: continue
from framework import utils from computer_vision import calibration from content.experiments.global_ekf_updates import GlobalEkfUpdatesExperiment ################################################################################################# # CONFIG # ################################################################################################# ################################################################################################# from content.data_pointers.lavi_november_18.jackal import jackal_18 from content.data_pointers.lavi_november_18.dji import plot1_snapshots_80_meters_ugv_poses_path, trunks_detection_results_dir from content.data_pointers.lavi_november_18 import orchard_topology if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('global_ekf_updates') with open(plot1_snapshots_80_meters_ugv_poses_path) as f: ugv_poses = json.load(f, object_pairs_hook=OrderedDict) for relevant_update_index, update_image_key in enumerate( ugv_poses.keys()[1:]): with open( os.path.join(trunks_detection_results_dir, 'trunks_detection_on_nov1_%s' % update_image_key, 'experiment_summary.json')) as f: td_experiment_summary = json.load(f) pixel_to_meter = calibration.calculate_pixel_to_meter( td_experiment_summary['results']['1']['optimized_grid_dim_x'], td_experiment_summary['results']['1']['optimized_grid_dim_y'], orchard_topology.plot1_measured_row_widths, orchard_topology.plot1_measured_intra_row_distances) resolution = 1.0 / pixel_to_meter
################################################################################################# # CONFIG # ################################################################################################# apr_selected_td_experiments = { 'noon': 'trunks_detection_on_apr_15-08-1', 'late_noon': 'trunks_detection_on_apr_15-53-1', 'afternoon': 'trunks_detection_on_apr_16-55-1', 'late_afternoon': 'trunks_detection_on_apr_19-04-1' } nov_selected_td_experiment = 'trunks_detection_on_nov1_10-12-1' ################################################################################################# if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('contours_comparison') with open( os.path.join(apr_td_results_dir, apr_selected_td_experiments['noon'], 'experiment_summary.json')) as f: apr_noon_td_summary = json.load(f) with open( os.path.join(apr_td_results_dir, apr_selected_td_experiments['late_noon'], 'experiment_summary.json')) as f: apr_late_noon_td_summary = json.load(f) with open( os.path.join(apr_td_results_dir, apr_selected_td_experiments['afternoon'], 'experiment_summary.json')) as f:
import os import cv2 import json from framework import cv_utils from framework import utils from content.data_pointers.lavi_november_18.dji import plot1_snapshots_80_meters as snapshots from framework import logger from collections import OrderedDict _logger = logger.get_logger() relevant_image_keys = [ '10-26-3', '10-27-1', '10-27-2', '10-28-1', '10-29-1', '10-29-2', '10-30-1', '10-30-2', '10-31-1', '10-32-1' ] if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('ugv_tagging') ugv_poses = OrderedDict() for image_key in relevant_image_keys: image = cv2.imread(snapshots[image_key].path) height = image.shape[0] ugv_pose = list(cv_utils.sample_pixel_coordinates(image)) ugv_pose[1] = height - ugv_pose[1] ugv_poses[image_key] = tuple(ugv_pose) with open(os.path.join(execution_dir, 'ugv_poses.json'), 'w') as f: json.dump(ugv_poses, f, indent=4)
source_image_index = 0 setup = 'apr' # apr / nov1 min_area = None ################################################################################################# if setup == 'apr': from content.data_pointers.lavi_april_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_april_18.dji import selected_trunks_detection_experiments as selected_td_experiments elif setup == 'nov1': from content.data_pointers.lavi_november_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_november_18.dji import plot1_selected_trunks_detection_experiments as selected_td_experiments else: raise NotImplementedError if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('canopy_contours_drawer') with open(os.path.join(td_results_dir, selected_td_experiments[source_image_index], 'experiment_summary.json')) as f: td_summary = json.load(f) image = cv2.imread(td_summary['data_sources']) contours, canopies_mask = segmentation.extract_canopy_contours(image, min_area=min_area) image_with_contours = image.copy() cv2.drawContours(image_with_contours, contours, contourIdx=-1, color=(0, 255, 0), thickness=5) canopies_mask_with_contours = cv2.cvtColor(canopies_mask.copy(), cv2.COLOR_GRAY2BGR) cv2.drawContours(canopies_mask_with_contours, contours, contourIdx=-1, color=(0, 255, 0), thickness=5) canopies_mask_with_trunks = cv2.cvtColor(canopies_mask.copy(), cv2.COLOR_GRAY2BGR) canopies_mask_with_trunks = cv_utils.draw_points_on_image(canopies_mask_with_trunks, td_summary['results']['1']['semantic_trunks'].values(), color=(0, 220, 0)) canopies_mask_with_labeled_trunks = canopies_mask_with_trunks.copy() for trunk_label, trunk_pose in td_summary['results']['1']['semantic_trunks'].items(): canopies_mask_with_labeled_trunks = cv_utils.put_shaded_text_on_image(canopies_mask_with_labeled_trunks, label=trunk_label, location=trunk_pose,
import pandas as pd import matplotlib.pyplot as plt from framework import utils from framework.config import base_results_path ################################################################################################# # CONFIG # ################################################################################################# template_matching_experiments_path = os.path.join(base_results_path, 'template_matching', 'template_matching_colored') ################################################################################################# if __name__ == '__main__': execution_dir = utils.create_new_execution_folder( 'template_matching_post_processing') for template_matching_experiment in os.listdir( template_matching_experiments_path): errors_df = pd.read_csv(os.path.join( template_matching_experiments_path, template_matching_experiment, 'errors.csv'), index_col=0) errors_df = errors_df.dropna(how='all') plt.figure() for method in errors_df.columns: plt.plot(range(errors_df[method].shape[0]), errors_df[method]) plt.ylim([0, 20]) plt.savefig( os.path.join(execution_dir, '%s.jpg' % template_matching_experiment))
import os from framework import utils from content.experiments.jackal_icp import JackalIcp ################################################################################################# # CONFIG # ################################################################################################# ugv_bag_name = '18-24-26' setup = 'apr' # apr / nov / lab ################################################################################################# if setup == 'apr': from content.data_pointers.lavi_april_18.jackal import jackal_18 as ugv_pointers elif setup == 'nov': raise NotImplementedError elif setup == 'lab': raise NotImplementedError if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('jackal_icp') bag_descriptor = ugv_pointers[ugv_bag_name] experiment = JackalIcp(name='icp_on_apr_%s' % ugv_bag_name, data_sources=bag_descriptor.path, working_dir=execution_dir) experiment.run(repetitions=1)
color=(0, 255, 255), radius=30) label_idx = start_waypoint_idx for coordinates in waypoints_coordinates: trajectory_image = cv_utils.put_shaded_text_on_image( trajectory_image, label=chr(label_idx + 65), location=tuple(np.array(coordinates) - np.array(upper_left)), color=(0, 255, 255), offset=(-40, -70)) label_idx += 1 cv2.imwrite(os.path.join(output_file_path), trajectory_image) if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('trajectory_update') with open( os.path.join(td_results_dir, td_baseline_experiment_name, 'experiment_summary.json')) as f: td_baseline_summary = json.load(f) with open( os.path.join(td_results_dir, td_obstacle_1_experiment_name, 'experiment_summary.json')) as f: td_obstacle_1_summary = json.load(f) with open( os.path.join(td_results_dir, td_obstacle_2_experiment_name, 'experiment_summary.json')) as f: td_obstacle_2_summary = json.load(f) baseline_image_path = td_baseline_summary['data_sources']
from framework import config ################################################################################################# # CONFIG # ################################################################################################# from content.data_pointers.lavi_april_18.dji import snapshots_80_meters as snapshots image_key = '15-08-1' roi_expansion = 2.8 noise_sigma = 0.5 draw_range_circle = False dummy_resolution = 0.0259837881206 ################################################################################################# if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('synthetic_scan_drawer') image = cv2.imread(snapshots[image_key].path) map_image = maps_generation.generate_canopies_map(image) center_x, center_y = cv_utils.sample_pixel_coordinates(map_image) scan_ranges = contours_scan.generate( map_image, center_x=center_x, center_y=center_y, min_angle=config.synthetic_scan_min_angle, max_angle=config.synthetic_scan_max_angle, samples_num=config.synthetic_scan_samples_num, min_distance=config.synthetic_scan_min_distance, max_distance=config.synthetic_scan_max_distance, resolution=dummy_resolution, r_primary_search_samples=config. synthetic_scan_r_primary_search_samples,
from content.data_pointers.lavi_november_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_november_18.dji import plot1_selected_trunks_detection_experiments as selected_td_experiments from content.data_pointers.lavi_november_18.orchard_topology import plot1_measured_trunks_perimeters as measured_trunks_perimeters from content.data_pointers.lavi_november_18.orchard_topology import plot1_measured_row_widths as measured_row_widths from content.data_pointers.lavi_november_18.orchard_topology import plot1_measured_intra_row_distances as measured_intra_row_distances from content.data_pointers.lavi_november_18.orchard_topology import plot1_trajectories as trajectories elif setup == 'nov2': from content.data_pointers.lavi_november_18.dji import trunks_detection_results_dir as td_results_dir from content.data_pointers.lavi_november_18.dji import plot2_selected_trunks_detection_experiments as selected_td_experiments from content.data_pointers.lavi_november_18.orchard_topology import plot2_measured_trunks_perimeters as measured_trunks_perimeters from content.data_pointers.lavi_november_18.orchard_topology import plot2_measured_row_widths as measured_row_widths from content.data_pointers.lavi_november_18.orchard_topology import plot2_measured_intra_row_distances as measured_intra_row_distances from content.data_pointers.lavi_november_18.orchard_topology import plot2_trajectories as trajectories if __name__ == '__main__': execution_dir = utils.create_new_execution_folder(description) for td_experiment in combinations( selected_td_experiments, r=2 if different_localization_and_mapping_sources else 1): td_experiment_name_for_map = td_experiment[0] if len(td_experiment) == 2: td_experiment_name_for_localization = td_experiment[1] else: td_experiment_name_for_localization = td_experiment_name_for_map with open( os.path.join(td_results_dir, td_experiment_name_for_map, 'experiment_summary.json')) as f: td_summary_for_map = json.load(f) with open( os.path.join(td_results_dir,
altitude = 80 repetitions = 1 grid_size_values = [6] from content.data_pointers.lavi_april_18.dji import snapshots_80_meters as snapshots # from content.data_pointers.lavi_november_18.dji import plot1_snapshots_80_meters as snapshots # from content.data_pointers.lavi_november_18.dji import plot2_snapshots_80_meters as snapshots from content.data_pointers.lavi_april_18.orchard_topology import plot_pattern # from content.data_pointers.lavi_november_18.orchard_topology import plot1_pattern as plot_pattern # from content.data_pointers.lavi_november_18.orchard_topology import plot2_pattern as plot_pattern ############################################################################################################################ if __name__ == '__main__': execution_dir = utils.create_new_execution_folder('trunks_detection') image_key_to_best_score = {} for image_key in snapshots.keys(): image_descriptor = snapshots[image_key] for grid_size in grid_size_values: # Run experiment experiment = TrunksDetectionExperiment(name='trunks_detection_on_%s' % image_key, data_sources=image_descriptor.path, working_dir=execution_dir, params={'crop_ratio': 0.8, 'initial_sigma_to_dim_y_ratio': 0.33, 'grid_size_for_optimization': grid_size, 'orchard_pattern': plot_pattern}, metadata={'image_key': image_key, 'altitude': altitude}) experiment.run(repetitions, viz_mode=False) # Post processing (image level) semantic_images_dir = os.path.join(experiment.experiment_dir, 'semantic_trunk_images') os.makedirs(semantic_images_dir)