def plot_full_figure(time_sec=None): #loc_file = r'C:\Users\xavier.mouy\Documents\Reports_&_Papers\Papers\10-XAVarray_2020\results\mobile_array_copper\localizations_1m_5cm.nc' loc_file = r'C:\Users\xavier.mouy\Documents\Reports_&_Papers\Papers\10-XAVarray_2020\results\mobile_array_copper\localizations_2cm_3m.nc' loc_file_matlab = r'C:\Users\xavier.mouy\Documents\Reports_&_Papers\Papers\10-XAVarray_2020\results\mobile_array_copper\localizations_matlab_with_CI.csv' audio_file = r'C:\Users\xavier.mouy\Documents\Reports_&_Papers\Papers\10-XAVarray_2020\data\mobile_array\2019-09-14_HornbyIsland_Trident\671404070.190918222812.wav' video_file = r'C:\Users\xavier.mouy\Documents\Reports_&_Papers\Papers\10-XAVarray_2020\data\large_array\2019-09-15_HornbyIsland_AMAR_07-HI\3420_FishCam01_20190920T163627.613206Z_1600x1200_awb-auto_exp-night_fr-10_q-20_sh-0_b-50_c-0_i-400_sat-0.mp4' hp_config_file = r'C:\Users\xavier.mouy\Documents\Reports_&_Papers\Papers\10-XAVarray_2020\data\mobile_array\2019-09-14_HornbyIsland_Trident\hydrophones_config_HI-201909.csv' localization_config_file = r'C:\Users\xavier.mouy\Documents\Reports_&_Papers\Papers\10-XAVarray_2020\config_files\localization_config_mobile_array.yaml' t1_sec = 214 #216 t2_sec = 224 #223 filter_x = [-5, 5] filter_y = [-5, 5] filter_z = [-2, 5] filter_x_std = 6 filter_y_std = 9 filter_z_std = 6 params = pd.DataFrame({ 'loc_color': ['black'], 'loc_marker': ['o'], 'loc_alpha': [1], 'loc_size': [5], 'uncertainty_color': ['black'], 'uncertainty_style': ['-'], 'uncertainty_alpha': [1], #0.7 'uncertainty_width': [0.2], #0.2 'x_min': [-1.5], 'x_max': [1.5], 'y_min': [-0.5], 'y_max': [3], 'z_min': [-1.5], 'z_max': [1.5], }) ## ########################################################################### localization_config = read_yaml(localization_config_file) hydrophones_config = pd.read_csv(hp_config_file) sound_speed_mps = localization_config['ENVIRONMENT']['sound_speed_mps'] ref_channel = localization_config['TDOA']['ref_channel'] hydrophone_pairs = defineReceiverPairs(len(hydrophones_config), ref_receiver=ref_channel) ## load localization results loc = Measurement() loc.from_netcdf(loc_file) loc_data = loc.data # used matlab CI loc_data = pd.read_csv(loc_file_matlab) # ## recalculate data errors # diff=[] # idx = 0 # for idx in range(len(loc_data)): # m = loc_data.loc[[idx],['x','y','z']] # tdoa_m = predict_tdoa(m, sound_speed_mps, hydrophones_config, hydrophone_pairs) # tdoa_measured = loc_data.loc[[idx],['tdoa_sec_1','tdoa_sec_2','tdoa_sec_3']].to_numpy() # #diff_temp = (tdoa_m-tdoa_measured.T)**2 # if idx==0: # diff = (tdoa_m-tdoa_measured.T)**2 # else: # diff = np.vstack((diff,(tdoa_m-tdoa_measured.T)**2)) # Q = len(loc_data) # #M = m.size # number of dimensions of the model (here: X, Y, and Z) # #N = len(tdoa_sec) # number of measurements # #error_std = np.sqrt((1/(Q*(N-M))) * (sum((tdoa_sec-tdoa_m)**2))) # tdoa_errors_std = np.sqrt( (1/Q)*(sum(diff))) # #tdoa_errors_std = calc_data_error(tdoa_sec, m, sound_speed_mps,hydrophones_config, hydrophone_pairs) # for idx in range(len(loc_data)): # loc_errors_std = calc_loc_errors(tdoa_errors_std, loc_data.loc[[idx],['x','y','z']] , sound_speed_mps, hydrophones_config, hydrophone_pairs) # print('m') # Filter loc_data = loc_data.dropna(subset=['x', 'y', 'z']) # remove NaN loc_data = loc_data.loc[(loc_data['x'] >= min(filter_x)) & (loc_data['x'] <= max(filter_x)) & (loc_data['y'] >= min(filter_y)) & (loc_data['y'] <= max(filter_y)) & (loc_data['z'] >= min(filter_z)) & (loc_data['z'] <= max(filter_z)) & (loc_data['x_std'] <= filter_x_std) & (loc_data['y_std'] <= filter_y_std) & (loc_data['z_std'] <= filter_z_std)] # Adjust detection times loc_data['time_min_offset'] = loc_data['time_min_offset'] - t1_sec loc_data['time_max_offset'] = loc_data['time_max_offset'] - t1_sec if time_sec != None: loc_data = loc_data.loc[(loc_data['time_max_offset'] <= time_sec)] else: print('Static') # update loc object loc.data = loc_data # plots # fig, ax = plt.subplots(figsize=(6, 1)) # fig.subplots_adjust(bottom=0.5) # n_colors = t2_sec-t1_sec # cmap = mpl.cm.get_cmap('CMRmap', n_colors*2) # norm = mpl.colors.Normalize(vmin=0, vmax=n_colors) # ax_cmap = mpl.colorbar.ColorbarBase(ax, cmap=cmap, # norm=norm, # orientation='horizontal') # ax_cmap.set_label('Time (s)') # Plot spectrogram fig_final, ax_spectro = plot_spectrogram(audio_file, loc, t1_sec, t2_sec, geometry=(5, 1, 1)) ax_spectro.set_title("") ax_spectro.get_xaxis().set_visible(False) n_colors = t2_sec - t1_sec cmap = mpl.cm.get_cmap('viridis', n_colors * 4) norm = mpl.colors.Normalize(vmin=0, vmax=n_colors) divider = make_axes_locatable(ax_spectro) cax = divider.append_axes('bottom', 0.1, pad=0.03) ax_cmap = mpl.colorbar.ColorbarBase(cax, cmap=cmap, norm=norm, orientation='horizontal') ax_cmap.set_label('Time (s)') if time_sec: SFreq_min, SFreq_max = ax_spectro.get_ylim() ax_spectro.plot([time_sec, time_sec], [SFreq_min, SFreq_max], 'r') # plot detection points on top of spectrogram #gs0 = fig_final.add_gridspec(60,1) ax_detec = fig_final.add_subplot(20, 1, 1) det_y = np.asarray(np.ones((1, len(loc_data['time_min_offset']))))[0] det_x = np.asarray(loc_data['time_min_offset']) ax_detec.scatter(det_x, det_y, c=loc_data['time_min_offset'], cmap=cmap, norm=norm, s=12) ax_detec.set_xlim(ax_spectro.get_xlim()) ax_detec.get_xaxis().set_visible(False) ax_detec.get_yaxis().set_visible(False) ax_detec.axis('off') # #pos =[left, bottom, width, height] # box = ax_detec.get_position() # box.y0 = box.y0 + 0.6 # box.y1 = box.y1 + 0.6 # ax_detec.set_position(box) #size = fig_final.get_size_inches() plt.subplots_adjust(left=0.08, bottom=0.1, right=0.95, top=0.95, wspace=0, hspace=0) # divider2 = make_axes_locatable(ax_spectro) # cax2 = divider2.append_axes('top', size=0.2, pad=10.0) # det_y = np.asarray(np.ones((1,len(loc_data['time_min_offset']))))[0] # det_x = np.asarray(loc_data['time_min_offset']) # cax2.plot(det_x,det_y,'.r') # cax2.set_xlim(ax_spectro.get_xlim()) # ax_cmap = mpl.colorbar.ColorbarBase(cax, cmap=cmap, # norm=norm, # orientation='horizontal') gs = fig_final.add_gridspec(3, 2) # plot localization top ax_toploc = fig_final.add_subplot(gs[1:, 1]) plot_top_view(hydrophones_config, loc_data, params, cmap, norm, ax_toploc) ax_toploc.set_anchor('E') # plot localization side #ax_sideloc = fig_final.add_subplot(3,3,7,sharex = ax_toploc) ax_sideloc = fig_final.add_subplot(gs[1:, 0]) plot_side_view(hydrophones_config, loc_data, params, cmap, norm, ax_sideloc) ax_sideloc.set_anchor('W') # set the spacing between subplots plt.subplots_adjust(wspace=0, hspace=0) # # plot video frame 1 # fig_video1, ax_video1 = plt.subplots(1,1) # frame1_sec = 152.8 # second detection -> 16:38:59.8 # #ax_video1 = fig_final.add_subplot(3,3,5) # plot_video_frame(video_file,frame1_sec, ax_video1) # ax_video1.get_xaxis().set_visible(False) # ax_video1.get_yaxis().set_visible(False) # # plot video frame 2 # fig_video2, ax_video2 = plt.subplots(1,1) # frame2_sec = 160 # 4th detection -> 16:39:07 # #ax_video2 = fig_final.add_subplot(3,3,6) # plot_video_frame(video_file,frame2_sec, ax_video2) # ax_video2.get_xaxis().set_visible(False) # ax_video2.get_yaxis().set_visible(False) fig_final.set_size_inches(9.08, 6.72) box = ax_spectro.get_position() box.y0 = box.y0 - 0.03 box.y1 = box.y1 - 0.03 ax_spectro.set_position(box) return fig_final
# Set virtual sources location (spherical grid) S = loclib.defineSphereSurfaceGrid(nsources, radius, origin) #S = loclib.defineCubeVolumeGrid(spacing, radius, origin) nsources = S.shape[0] # creates output folder StartTimestamp_obj = datetime.datetime.now() StartTimestamp_str = StartTimestamp_obj.strftime("%Y%m%d%H%M%S") outdir = os.path.join( outroot, StartTimestamp_str + '_' + 'Receivers' + str(nReceivers) + '_' + 'Bounds' + str(ReceiverBoundValue) + 'm_' + 'Sources' + str(nsources) + '_' + 'Radius' + str(radius) + 'm') os.mkdir(outdir) # Define receiver pairs for TDOAs Rpairs = loclib.defineReceiverPairs(nReceivers) # Repeats optimization nIter times to ensure stability for i in range(nIter): # Closes all open figures plt.close("all") # Optimize array configuration R, Rchanges, acceptRateChanges, Cost, processingTime = loclib.optimizeArray( ReceiverBounds, nReceivers, AnnealingSchedule, S, Rpairs, V, NoiseVariance) # Get list of Jacobian matrice for each source J2 = loclib.defineJacobian(R, S, V, Rpairs) # Calculates localization uncertainty for each source Uncertainties2 = loclib.getUncertainties(J2, NoiseVariance) # Plots unceratinties of optimized array
detection_config['SPECTROGRAM']['fmax_hz'], chunk = [t1, t2], detections=detections, detections_channel=detection_config['AUDIO']['channel']) # localization sound_speed_mps = localization_config['ENVIRONMENT']['sound_speed_mps'] ref_channel = localization_config['TDOA']['ref_channel'] # define search window based on hydrophone separation and sound speed hydrophones_dist_matrix = calc_hydrophones_distances(hydrophones_config) TDOA_max_sec = np.max(hydrophones_dist_matrix)/sound_speed_mps # define hydrophone pairs hydrophone_pairs = defineReceiverPairs(len(hydrophones_config), ref_receiver=ref_channel) # pre-compute grid search if needed if localization_config['METHOD']['grid_search']: sources = defineSphereVolumeGrid( localization_config['GRIDSEARCH']['spacing_m'], localization_config['GRIDSEARCH']['radius_m'], origin=localization_config['GRIDSEARCH']['origin']) if localization_config['GRIDSEARCH']['min_z']: sources = sources.loc[sources['z']>=localization_config['GRIDSEARCH']['min_z']] sources = sources.reset_index(drop=True) # sources = defineCubeVolumeGrid(0.2, 2, origin=[0, 0, 0]) # sources = defineSphereSurfaceGrid( # 10000, # localization_config['GRIDSEARCH']['radius_m'], # origin=localization_config['GRIDSEARCH']['origin'])
def getArrayUncertainties(R, radius, spacing, V, NoiseSTD, contoursValues): # Virtual sources coordinates -> Cube of points (Cartesian coordinates) vec = np.arange(-radius, radius + spacing, spacing) X, Y, Z = np.meshgrid(vec, vec, vec, indexing='ij') Sx = np.reshape(X, X.shape[0] * X.shape[1] * X.shape[2]) Sy = np.reshape(Y, Y.shape[0] * Y.shape[1] * Y.shape[2]) Sz = np.reshape(Z, Z.shape[0] * Z.shape[1] * Z.shape[2]) S = pd.DataFrame({'x': Sx, 'y': Sy, 'z': Sz}) # find location of slice ind = np.argmin(abs(vec)) sliceValue = vec[ind] # Nb of receivers nReceivers = R.shape[0] # Variance of TDOA measurement errors NoiseVariance = NoiseSTD**2 # Define receiver pairs for TDOAs Rpairs = loclib.defineReceiverPairs(nReceivers) # Get list of Jacobian matrice for each source J = loclib.defineJacobian(R, S, V, Rpairs) # Calculates localization uncertainty for each source Uncertainties = loclib.getUncertainties(J, NoiseVariance) # Plots unceratinties of optimized array loclib.plotArrayUncertainties(R, S, Uncertainties) # PLot hydrophone locations f0 = plt.figure() ax0 = f0.add_subplot(111, projection='3d') ax0.scatter(R['x'], R['y'], R['z'], s=30, c='black') ax0.set_xlabel('X (m)', labelpad=10) ax0.set_ylabel('Y (m)', labelpad=10) ax0.set_zlabel('Z (m)', labelpad=10) plt.show() # Define and plot plane slices f, (ax1, ax2, ax3) = plt.subplots(1, 3, sharey=False, figsize=(16, 5)) ## XY plane XY = np.zeros([len(vec), len(vec)]) for i in range(len(vec)): for jj in range(len(vec)): idx = S.index[(S['x'] == vec[i]) & (S['y'] == vec[jj]) & (S['z'] == sliceValue)][0] XY[i, jj] = Uncertainties['rms'][idx] CS_XY = ax1.contour(vec, vec, XY, levels=contoursValues, colors=['k']) # Receivers ax1.plot(R['x'], R['y'], 'go') ax1.set_xlabel('X(m)') ax1.set_ylabel('Y(m)') ax1.grid(True) im = ax1.imshow(XY, interpolation='bilinear', origin='lower', cmap=cm.jet, extent=(-radius, radius, -radius, radius), norm=colors.Normalize(vmin=0, vmax=10)) ax1.set_aspect('auto') cbar = f.colorbar(im, ax=ax1) cbar.ax.set_ylabel('Uncertainty (m)') ## XZ plane XZ = np.zeros([len(vec), len(vec)]) for i in range(len(vec)): for jj in range(len(vec)): idx = S.index[(S['x'] == vec[i]) & (S['z'] == vec[jj]) & (S['y'] == sliceValue)][0] XZ[i, jj] = Uncertainties['rms'][idx] CS_XZ = ax2.contour(vec, vec, XZ, levels=contoursValues, colors=['k']) # Receivers ax2.plot(R['x'], R['z'], 'go') ax2.set_xlabel('X(m)') ax2.set_ylabel('Z(m)') ax2.grid(True) im = ax2.imshow(XZ, interpolation='bilinear', origin='lower', cmap=cm.jet, extent=(-radius, radius, -radius, radius), norm=colors.Normalize(vmin=0, vmax=10)) ax2.set_aspect('auto') cbar = f.colorbar(im, ax=ax2) cbar.ax.set_ylabel('Uncertainty (m)') ## YZ plane YZ = np.zeros([len(vec), len(vec)]) for i in range(len(vec)): for jj in range(len(vec)): idx = S.index[(S['y'] == vec[i]) & (S['z'] == vec[jj]) & (S['x'] == sliceValue)][0] YZ[i, jj] = Uncertainties['rms'][idx] CS_YZ = ax3.contour(vec, vec, YZ, levels=contoursValues, colors=['k']) # Receivers ax3.plot(R['y'], R['z'], 'go') ax3.set_xlabel('Y(m)') ax3.set_ylabel('Z(m)') ax3.grid(True) im = ax3.imshow(YZ, interpolation='bilinear', origin='lower', cmap='jet', extent=(-radius, radius, -radius, radius), norm=colors.Normalize(vmin=0, vmax=10)) cbar = f.colorbar(im, ax=ax3) cbar.ax.set_ylabel('Uncertainty (m)') # from mpl_toolkits.axes_grid1 import make_axes_locatable # divider = make_axes_locatable(plt.gca()) # cax = divider.append_axes("right", "5%", pad="3%") # plt.colorbar(im, cax=cax) #plt.colorbar(im,ax=ax3) ax3.set_aspect('auto') #plt.tight_layout() plt.show() #plt.tight_layout() # Extract contours lines coord_CS_XY = [] coord_CS_XZ = [] coord_CS_YZ = [] for i in range(len(contoursValues)): p1 = CS_XY.collections[i].get_paths()[0] coord_CS_XY.append(p1.vertices) p2 = CS_XZ.collections[i].get_paths()[0] coord_CS_XZ.append(p2.vertices) p3 = CS_YZ.collections[i].get_paths()[0] coord_CS_YZ.append(p3.vertices) return coord_CS_XY, coord_CS_XZ, coord_CS_YZ
def run_localization(infile, deployment_info_file, detection_config, hydrophones_config, localization_config): t1 = 0 t2 = 70 # Look up data files for all channels audio_files = find_audio_files(infile, hydrophones_config) # run detector on selected channel print('DETECTION') detections = run_detector( audio_files['path'][detection_config['AUDIO']['channel']], audio_files['channel'][detection_config['AUDIO']['channel']], detection_config, chunk=[t1, t2], deployment_file=deployment_info_file) #detections.insert_values(frequency_min=20) print(str(len(detections)) + ' detections') # # plot spectrogram/waveforms of all channels and detections # plot_data(audio_files, # detection_config['SPECTROGRAM']['frame_sec'], # detection_config['SPECTROGRAM']['window_type'], # detection_config['SPECTROGRAM']['nfft_sec'], # detection_config['SPECTROGRAM']['step_sec'], # detection_config['SPECTROGRAM']['fmin_hz'], # detection_config['SPECTROGRAM']['fmax_hz'], # chunk = [t1, t2], # detections=detections, # detections_channel=detection_config['AUDIO']['channel']) # localization sound_speed_mps = localization_config['ENVIRONMENT']['sound_speed_mps'] ref_channel = localization_config['TDOA']['ref_channel'] # define search window based on hydrophone separation and sound speed hydrophones_dist_matrix = calc_hydrophones_distances(hydrophones_config) TDOA_max_sec = np.max(hydrophones_dist_matrix) / sound_speed_mps # define hydrophone pairs hydrophone_pairs = defineReceiverPairs(len(hydrophones_config), ref_receiver=ref_channel) # pre-compute grid search if needed if localization_config['METHOD']['grid_search']: sources = defineSphereVolumeGrid( localization_config['GRIDSEARCH']['spacing_m'], localization_config['GRIDSEARCH']['radius_m'], origin=localization_config['GRIDSEARCH']['origin']) #sources = defineCubeVolumeGrid(0.2, 2, origin=[0, 0, 0]) sources_tdoa = np.zeros(shape=(len(hydrophone_pairs), len(sources))) for source_idx, source in sources.iterrows(): sources_tdoa[:, source_idx] = predict_tdoa(source, sound_speed_mps, hydrophones_config, hydrophone_pairs).T theta = np.arctan2(sources['y'].to_numpy(), sources['x'].to_numpy()) * (180 / np.pi) # azimuth phi = np.arctan2( sources['y'].to_numpy()**2 + sources['x'].to_numpy()**2, sources['z'].to_numpy()) * (180 / np.pi) sources['theta'] = theta sources['phi'] = phi # Define Measurement object for the localization results if localization_config['METHOD']['linearized_inversion']: localizations = Measurement() localizations.metadata['measurer_name'] = localization_method_name localizations.metadata['measurer_version'] = '0.1' localizations.metadata['measurements_name'] = [[ 'x', 'y', 'z', 'x_std', 'y_std', 'z_std', 'tdoa_errors_std' ]] # need to define what output is for grid search # pick single detection (will use loop after) print('LOCALIZATION') for detec_idx, detec in detections.data.iterrows(): if 'detec_idx_forced' in locals(): print('Warning: forced to only process detection #', str(detec_idx_forced)) detec = detections.data.iloc[detec_idx_forced] print(str(detec_idx + 1) + '/' + str(len(detections))) # load data from all channels for that detection waveform_stack = stack_waveforms(audio_files, detec, TDOA_max_sec) # readjust signal boundaries to only focus on section with most energy percentage_max_energy = 90 chunk = ecosound.core.tools.tighten_signal_limits_peak( waveform_stack[detection_config['AUDIO']['channel']], percentage_max_energy) waveform_stack = [x[chunk[0]:chunk[1]] for x in waveform_stack] # calculate TDOAs tdoa_sec, corr_val = calc_tdoa( waveform_stack, hydrophone_pairs, detec['audio_sampling_frequency'], TDOA_max_sec=TDOA_max_sec, upsample_res_sec=localization_config['TDOA']['upsample_res_sec'], normalize=localization_config['TDOA']['normalize'], doplot=False, ) if localization_config['METHOD']['grid_search']: delta_tdoa = sources_tdoa - tdoa_sec delta_tdoa_norm = np.linalg.norm(delta_tdoa, axis=0) sources['delta_tdoa'] = delta_tdoa_norm fig = plt.figure() ax = fig.add_subplot(111, projection='3d') colors = matplotlib.cm.tab10(hydrophones_config.index.values) #alphas = delta_tdoa_norm - min(delta_tdoa_norm) #alphas = alphas/max(alphas) #alphas = alphas - 1 #alphas = abs(alphas) #alphas = np.array(alphas) alphas = 0.5 for index, hp in hydrophones_config.iterrows(): point = ax.scatter( hp['x'], hp['y'], hp['z'], s=40, color=colors[index], label=hp['name'], ) ax.scatter( sources['x'], sources['y'], sources['z'], c=sources['delta_tdoa'], s=2, alpha=alphas, ) # Axes labels ax.set_xlabel('X (m)', labelpad=10) ax.set_ylabel('Y (m)', labelpad=10) ax.set_zlabel('Z (m)', labelpad=10) # legend ax.legend(bbox_to_anchor=(1.07, 0.7, 0.3, 0.2), loc='upper left') plt.tight_layout() plt.show() plt.figure() sources.plot.hexbin(x="theta", y="phi", C="delta_tdoa", reduce_C_function=np.mean, gridsize=40, cmap="viridis") # Lineralized inversion if localization_config['METHOD']['linearized_inversion']: [m, iterations_logs ] = linearized_inversion(tdoa_sec, hydrophones_config, hydrophone_pairs, localization_config['INVERSION'], sound_speed_mps, doplot=False) # Estimate uncertainty tdoa_errors_std = calc_data_error(tdoa_sec, m, sound_speed_mps, hydrophones_config, hydrophone_pairs) loc_errors_std = calc_loc_errors(tdoa_errors_std, m, sound_speed_mps, hydrophones_config, hydrophone_pairs) # Bring all detection and localization informations together detec.loc['x'] = m['x'].values[0] detec.loc['y'] = m['y'].values[0] detec.loc['z'] = m['z'].values[0] detec.loc['x_std'] = loc_errors_std['x_std'].values[0] detec.loc['y_std'] = loc_errors_std['y_std'].values[0] detec.loc['z_std'] = loc_errors_std['z_std'].values[0] detec.loc['tdoa_errors_std'] = tdoa_errors_std[0] # stack to results into localization object localizations.data = localizations.data.append(detec, ignore_index=True) return localizations