def estimate_resolution(images_dir):
grid_dim_x_values = []
grid_dim_y_values = []
for image_filename in os.listdir(images_dir):
image = cv2.imread(os.path.join(images_dir, image_filename))
orientation = trunks_detection_old_cv.estimate_rows_orientation(image)
_, rotated_centroids, _, _ = trunks_detection_old_cv.find_tree_centroids(
image, correction_angle=orientation * (-1))
grid_dim_x, grid_dim_y = trunks_detection_old_cv.estimate_grid_dimensions(
rotated_centroids)
grid_dim_x_values.append(grid_dim_x)
grid_dim_y_values.append(grid_dim_y)
return 1.0 / calibration.calculate_pixel_to_meter(
np.mean(grid_dim_x_values), np.mean(grid_dim_y_values),
orchard_topology.measured_row_widths,
orchard_topology.measured_intra_row_distances)
td_summary_for_localization = json.load(f)
optimized_sigma = td_summary_for_localization['results']['1'][
'optimized_sigma']
optimized_grid_dim_x_for_map = td_summary_for_map['results']['1'][
'optimized_grid_dim_x']
optimized_grid_dim_y_for_map = td_summary_for_map['results']['1'][
'optimized_grid_dim_y']
optimized_grid_dim_x_for_localization = td_summary_for_map['results'][
'1']['optimized_grid_dim_x']
optimized_grid_dim_y_for_localization = td_summary_for_map['results'][
'1']['optimized_grid_dim_y']
mean_trunk_radius, std_trunk_radius = calibration.calculate_trunk_radius_in_meters(
measured_trunks_perimeters)
pixel_to_meter_ratio_for_map = calibration.calculate_pixel_to_meter(
optimized_grid_dim_x_for_map, optimized_grid_dim_y_for_map,
measured_row_widths, measured_intra_row_distances)
pixel_to_meter_ratio_for_localization = calibration.calculate_pixel_to_meter(
optimized_grid_dim_x_for_localization,
optimized_grid_dim_y_for_localization, measured_row_widths,
measured_intra_row_distances)
mean_trunk_radius_in_pixels_for_map = int(
np.round(mean_trunk_radius * config.trunk_dilation_ratio *
pixel_to_meter_ratio_for_map))
mean_trunk_radius_in_pixels_for_localization = int(
np.round(mean_trunk_radius * config.trunk_dilation_ratio *
pixel_to_meter_ratio_for_localization))
std_trunk_radius_in_pixels_for_map = std_trunk_radius * config.trunk_std_increasing_factor * pixel_to_meter_ratio_for_map
std_trunk_radius_in_pixels_for_localization = std_trunk_radius * config.trunk_std_increasing_factor * pixel_to_meter_ratio_for_localization
map_image_path = td_summary_for_map['data_sources']
map_image_key = td_summary_for_map['metadata']['image_key']
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',
data_sources={
'map_image_path':
image_path,
'map_semantic_trunks':
map_semantic_trunks,
'scans_and_poses_pickle_path':
os.path.join(base_resources_path,
'amcl_video',
'scans_and_poses.pkl'),
'odom_pickle_path':
os.path.join(base_resources_path,
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
experiment = GlobalEkfUpdatesExperiment(
name='global_ekf_update_%d' % (relevant_update_index + 1),
data_sources={
'jackal_bag_path': jackal_18['10-25'].path,
'ugv_poses_path': plot1_snapshots_80_meters_ugv_poses_path,
'relevant_update_index': (relevant_update_index + 1)
},
params={
'bag_start_time': 85,
'resolution': resolution
},
working_dir=execution_dir)
grid_dim_x_values = []
grid_dim_y_values = []
for image_filename in os.listdir(images_dir):
image = cv2.imread(os.path.join(images_dir, image_filename))
orientation, _, _ = trunks_detection.estimate_rows_orientation(image)
try:
_, rotated_centroids, _, _, _ = trunks_detection.find_tree_centroids(
image, correction_angle=orientation * (-1))
grid_dim_x, grid_dim_y = trunks_detection.estimate_grid_dimensions(
rotated_centroids)
except Exception, e:
grid_dim_x, grid_dim_y = np.nan, np.nan
grid_dim_x_values.append(grid_dim_x)
grid_dim_y_values.append(grid_dim_y)
return 1.0 / calibration.calculate_pixel_to_meter(
np.nanmean(grid_dim_x_values), np.nanmean(grid_dim_y_values),
orchard_topology.measured_row_widths,
orchard_topology.measured_intra_row_distances)
if __name__ == '__main__':
video_samples_path = os.path.join(base_resources_path, 'amcl_video',
'video_samples')
# resolution = estimate_resolution(video_samples_path)
resolution = 0.0156892495353
with open(pickle_path, 'rb') as f:
scans_and_ugv_poses = pickle.load(f)
cap = cv2.VideoCapture(video_path)
sz = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)),
int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
# fourcc = cv2.VideoWriter_fourcc(*'mpeg')
fourcc = cv2.VideoWriter_fourcc(*'avc1')