def save_pkl(header, data, plot=True, curve_fit=None, annotation=None, location=None, time=True, filename=None, matlab=False): location = mk_dir(path=location, time=time) if not filename: if 'name' in header: filename = '{0} {1}'.format(header['type'], header['name']) else: filename = '{0}'.format(header['type']) f = open('{0}/{1}.pkl'.format(location, filename), 'wb') if header: data_pkl = (2, data, header) else: data_pkl = data pickle.dump(data_pkl, f) f.close() if plot: plotting.plot_measurement(data, filename, save=location, annotation=annotation) if matlab: matfilename = '{0}/{1}.mat'.format(location, filename) scipy.io.savemat(matfilename, mdict=data)
) predicted_position_history.append(FlatParticle.get_mean_position(particles)) if PLOT: ax[0].clear() ax[0].set_xlim([-5, 20]) ax[0].set_ylim([-5, 20]) plot_landmarks(ax[0], landmarks) plot_history(ax[0], real_position_history, color='green') plot_history(ax[0], predicted_position_history, color='orange') if(visible_measurements.size != 0): plot_connections(ax[0], vehicle.position, visible_measurements + vehicle.position[:2]) plot_particles_weight(ax[0], particles) if(visible_measurements.size != 0): plot_measurement(ax[0], vehicle.position[:2], visible_measurements, color="red") ax[1].clear() ax[1].set_xlim([-5, 20]) ax[1].set_ylim([-5, 20]) best = np.argmax(FlatParticle.w(particles)) plot_landmarks(ax[1], landmarks, color="green") plot_landmarks(ax[1], FlatParticle.get_landmarks(particles, best), color="orange") covariances = FlatParticle.get_covariances(particles, best) for i, landmark in enumerate(FlatParticle.get_landmarks(particles, best)): plot_confidence_ellipse(ax[1], landmark, covariances[i], n_std=3) if FlatParticle.neff(particles) < 0.6*N: print("resampling..")
visible_landmarks.append(landmark) landmark_indices.append(i) z_real = np.array(z_real) visible_landmarks = np.array(visible_landmarks, dtype=np.float) # plot_measurement(ax, real_position[:2], z_real, color="red") start = time.time() update(particles, z_real[landmark_indices], measurement_variance) print("took: ", (time.time() - start)) # plt.pause(2) predicted_position_history.append( Particle.get_mean_position(particles)) if PLOT: ax.clear() ax.set_xlim([0, 15]) ax.set_ylim([0, 15]) plot_landmarks(ax, landmarks) plot_history(ax, real_position_history, color='green') plot_history(ax, predicted_position_history, color='orange') plot_connections(ax, real_position, z_real[landmark_indices, :] + real_position[:2]) plot_particles_weight(ax, particles) plot_measurement(ax, real_position[:2], z_real, color="red") if neff(particles) < N / 2: print("resample", neff(particles)) particles = resample_particles(particles)
# print(ss) z_real = [] visible_landmarks = [] landmark_indices = [] for i, landmark in enumerate(landmarks): z = get_measurement_stochastic(ss[:2], landmark, sigmas) z_real.append(z) if dist(landmark, ss) < MAX_DIST: visible_landmarks.append(landmark) landmark_indices.append(i) z_real = np.array(z_real) visible_landmarks = np.array(visible_landmarks, dtype=np.float) plot_measurement(ax, ss[:2], z_real, color="red") # print(z_real + ss[:2]) update(particles, landmark_indices, z_real, sigmas) plt.pause(0.01) slam_history.append(get_mean(particles)) ax.clear() ax.set_xlim([0, 17]) ax.set_ylim([0, 17]) plot_landmarks(ax, landmarks) plot_history(ax, ss_history, color='green') plot_history(ax, slam_history, color='orange') plot_connections(ax, ss, z_real[landmark_indices, :] + ss[:2])
def sweep(measurer, *params, filename=None, root_dir=None, plot=True, header=None, output=True): ''' Performs a n-d parametric sweep. Usage: sweep(measurer, (param1_values, param1_setter, [param1_name]), (param2_values, param2_setter), ... , filename=None) measurer: an object that supports get_points(), measure(), get_dtype() and get_opts() methods. Returns: dict of ndarrays each corresponding to a measurment in the sweep ''' print("Started at: ", time.strftime("%b %d %Y %H:%M:%S", time.localtime())) start_time = time.time() # extracting sweep parameters from list # check if it is a list of pairs sweep_dim = tuple([len(param[0]) for param in params]) sweep_dim_vals = tuple([param[0] for param in params]) sweep_dim_names = tuple([ param[2] if len(param) > 2 else 'param_{0}'.format(param_id) for param_id, param in enumerate(params) ]) sweep_dim_setters = tuple(param[1] for param in params) # determining return type of measurer: in could be 2 traces (im, re) point_types = measurer.get_dtype() meas_opts = measurer.get_opts() points = measurer.get_points() # should return dict of meas_name:points point_dim_names = { mname: tuple([cname for cname, cvals in mxvals]) for mname, mxvals in points.items() } point_dim = { mname: tuple([len(cvals) for cname, cvals in mxvals]) for mname, mxvals in points.items() } point_dim_vals = { mname: tuple([cvals for cname, cvals in mxvals]) for mname, mxvals in points.items() } #point_dim = {mname:tuple([len(mxvals[cname]) for cname in point_dim_names[mname]]) for mname, mxvals in points.items()} #point_dim_vals = {mname:tuple([mxvals[1] for cname in point_dim_names[mname]]) for mname, mxvals in points.items()} # initializing empty ndarrays for measurment results data = { mname: np.zeros(sweep_dim + point_dim[mname], dtype=point_types[mname]) * np.nan for mname, mxvals in points.items() } def non_unity_dims(mname): return tuple(dim for dim in data[mname].shape if dim > 1) def non_unity_dim_names(mname): all_dim_names = sweep_dim_names + point_dim_names[mname] return tuple(dim_name for dim, dim_name in zip(data[mname].shape, all_dim_names) if dim > 1) def non_unity_dim_vals(mname): all_dim_vals = sweep_dim_vals + point_dim_vals[mname] return tuple(dim_name for dim, dim_name in zip(data[mname].shape, all_dim_vals) if dim > 1) ascii_files = {mname: None for mname in points.keys()} mk_measurement = lambda x: { mname: (non_unity_dim_names(mname), non_unity_dim_vals(mname), np.reshape(data[mname], non_unity_dims(mname)), meas_opts[mname]) for mname in points.keys() } if root_dir is None: data_dir = save_pkl.mk_dir() else: data_dir = root_dir #opening on-the-fly ascii files if not filename is None and output: def data_to_str(data, fmt='{:e}\t'): if hasattr(data, '__iter__'): return ''.join([fmt.format(d) for d in data]) else: return fmt.format(data) for mname in points.keys(): # if the file has more than 1.000.000 points or more than 2 dimensions don't save to ascii if data[mname].size < 1e6 and len(non_unity_dims(mname)) <= 2: if point_types[mname] is complex: ascii_files[mname] = { np.abs: open(data_dir + filename + '_amp.dat', 'w'), np.angle: open(data_dir + filename + '_ph.dat', 'w') } if point_types[mname] is float: ascii_files[mname] = { (lambda x: x): open(data_dir + filename + '.dat', 'w') } # opening plot windows if plot: plot_update_start = time.time() plot_axes = plotting.plot_measurement(mk_measurement(0)) last_plot_update = time.time() plot_update_time = plot_update_start - last_plot_update # starting sweep start_time = time.time() try: internal_print("First sweep...") # looping over all indeces at the same time all_indeces = itertools.product(*([i for i in range(d)] for d in sweep_dim)) vals = [None for d in sweep_dim] # set initial parameter vals to None done_sweeps = 0 total_sweeps = np.prod([d for d in sweep_dim]) for indeces in all_indeces: # check which values have changed this sweep old_vals = vals vals = [ params[param_id][0][val_id] for param_id, val_id in enumerate(indeces) ] changed_vals = [ old_val != val for old_val, val in zip(old_vals, vals) ] # set to new param vals for val, setter, changed in zip(vals, sweep_dim_setters, changed_vals): if changed: setter(val) #measuring mpoint = measurer.measure() #saving data to containers for mname in points.keys(): mpoint_m = mpoint[mname] data[mname][[(i) for i in indeces] + [...]] = mpoint_m data_flat = np.reshape(data[mname], non_unity_dims(mname)) mpoint_m_flat = np.reshape( mpoint_m, tuple(dim for dim in point_dim[mname] if dim > 1)) #Save data to text file (on the fly) if ascii_files[mname]: for fmt, ascii_file in ascii_files[mname].items(): ascii_file.write( data_to_str(fmt(mpoint_m_flat)) + '\n') ascii_file.flush() #Plot data (on the fly) # measure when we started updating plot plot_update_start = time.time() # if the last plot update was more than 20 times the time we need to update the plot, update # this is done to conserve computational resources if plot: if plot_update_start - last_plot_update > 10 * plot_update_time: plotting.update_plot_measurement(mk_measurement(0), plot_axes) ''' if plot_windows[mname]: for fmt, plot in plot_windows[mname].items(): update_plot(plot, non_unity_dim_vals(mname), fmt(data_flat)) plt.pause(0.01) last_plot_update = time.time() plot_update_time = last_plot_update - plot_update_start ''' done_sweeps += 1 avg_time = (time.time() - start_time) / done_sweeps param_val_mes = ',\t'.join([ '{0}: {1:6.4g}'.format(param[2], val) for param, val in zip(params, vals) ]) stat_mes_fmt = '\rTime left: {0},\tparameter values: {1},\taverage cycle time: {2:4.2g}s\t' stat_mes = stat_mes_fmt.format( format_time_delta(avg_time * (total_sweeps - done_sweeps)), param_val_mes, avg_time) internal_print(stat_mes) print("\nElapsed time: " + format_time_delta(time.time() - start_time)) finally: for mname in points.keys(): header = { 'name': filename, 'type': mname, 'params': non_unity_dim_names(mname) } #sweep_xrange_pkl = non_unity_dim_vals(mname) #data_pkl = np.reshape(data[mname], non_unity_dims(mname)) #if len(sweep_xrange_pkl)<2: # sweep_xrange_pkl=sweep_xrange_pkl+tuple([0]*(2-len(sweep_xrange_pkl))) #data_pkl = tuple(sweep_xrange_pkl)+(np.abs(data_pkl), np.angle(data_pkl)) #save_pkl.save_pkl(header, data_pkl, location = data_dir) save_pkl.save_pkl(header, mk_measurement(0), location=data_dir) if ascii_files[mname]: for fmt, ascii_file in ascii_files[mname].items(): ascii_file.close() return mk_measurement(0)