def main(): args = get_cli_args() validate_cli_args(args) alphas = np.array(args.alphas) beta = np.array(args.beta)**2 mean_prior = np.array([180., 50., 0.]) Sigma_prior = 1e-12 * np.eye(3, 3) initial_state = Gaussian(mean_prior, Sigma_prior) if args.input_data_file: data = load_data(args.input_data_file) elif args.num_steps: # Generate data, assuming `--num-steps` was present in the CL args. data = generate_input_data(initial_state.mu.T, args.num_steps, args.num_landmarks_per_side, args.max_obs_per_time_step, alphas, beta, args.dt) else: raise RuntimeError('') should_show_plots = True if args.animate else False should_write_movie = True if args.movie_file else False should_update_plots = True if should_show_plots or should_write_movie else False field_map = FieldMap(args.num_landmarks_per_side) fig = get_plots_figure(should_show_plots, should_write_movie) movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM', args.movie_fps, args.plot_pause_len) progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps) data = load_data("slam-evaluation-input.npy") slam = SAM(beta, alphas, initial_state) with movie_writer.saving( fig, args.movie_file, data.num_steps) if should_write_movie else get_dummy_context_mgr(): for t in range(data.num_steps): # Used as means to include the t-th time-step while plotting. tp1 = t + 1 # Control at the current step. u = data.filter.motion_commands[t] # Observation at the current step. z = data.filter.observations[t] # print(data.filter.observations.shape) slam.predict(u) trajectory, landmarks = slam.update(z) progress_bar.next() if not should_update_plots: continue plt.cla() plot_field(field_map, z, slam.lm_positions, slam.lm_correspondences) plot_robot(data.debug.real_robot_path[t]) plot_observations(data.debug.real_robot_path[t], data.debug.noise_free_observations[t], data.filter.observations[t]) plt.plot(data.debug.real_robot_path[1:tp1, 0], data.debug.real_robot_path[1:tp1, 1], 'm') plt.plot(data.debug.noise_free_robot_path[1:tp1, 0], data.debug.noise_free_robot_path[1:tp1, 1], 'g') plt.plot([data.debug.real_robot_path[t, 0]], [data.debug.real_robot_path[t, 1]], '*r') plt.plot([data.debug.noise_free_robot_path[t, 0]], [data.debug.noise_free_robot_path[t, 1]], '*g') # TODO plot SLAM soltion plt.plot(np.array(trajectory)[:, 0], np.array(trajectory)[:, 1]) plt.scatter(np.array(landmarks)[:, 0], np.array(landmarks)[:, 1]) # print(t) # for lm in slam.lm_positions: # # print(len(lm)) # if len(lm)>5: # lm_mu, lm_sigma = get_gaussian_statistics_xy(np.array(lm[-5:])) # # print('lm_mu',lm_mu) # # print('lm_sigma',lm_sigma) # # print('plot lm') # plot2dcov(lm_mu, lm_sigma, 3, 50) if should_show_plots: # Draw all the plots and pause to create an animation effect. plt.draw() plt.pause(args.plot_pause_len) if should_write_movie: movie_writer.grab_frame() progress_bar.finish() plt.show(block=True)
def main(): args = get_cli_args() validate_cli_args(args) alphas = np.array(args.alphas)**2 beta = np.array(args.beta) beta[1] = np.deg2rad(beta[1]) mean_prior = np.array([180., 50., 0.]) Sigma_prior = 1e-12 * np.eye(3, 3) initial_state = Gaussian(mean_prior, Sigma_prior) if args.input_data_file: data = load_data(args.input_data_file) elif args.num_steps: # Generate data, assuming `--num-steps` was present in the CL args. data = generate_input_data(initial_state.mu.T, args.num_steps, args.num_landmarks_per_side, args.max_obs_per_time_step, alphas, beta, args.dt) else: raise RuntimeError('') should_show_plots = True if args.animate else False should_write_movie = True if args.movie_file else False should_update_plots = True if should_show_plots or should_write_movie else False field_map = FieldMap(args.num_landmarks_per_side) fig = get_plots_figure(should_show_plots, should_write_movie) movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM', args.movie_fps, args.plot_pause_len) progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps) with movie_writer.saving( fig, args.movie_file, data.num_steps) if should_write_movie else get_dummy_context_mgr(): for t in range(data.num_steps): # Used as means to include the t-th time-step while plotting. tp1 = t + 1 # Control at the current step. u = data.filter.motion_commands[t] # Observation at the current step. z = data.filter.observations[t] # TODO SLAM predict(u) # TODO SLAM update progress_bar.next() if not should_update_plots: continue plt.cla() plot_field(field_map, z) plot_robot(data.debug.real_robot_path[t]) plot_observations(data.debug.real_robot_path[t], data.debug.noise_free_observations[t], data.filter.observations[t]) plt.plot(data.debug.real_robot_path[1:tp1, 0], data.debug.real_robot_path[1:tp1, 1], 'm') plt.plot(data.debug.noise_free_robot_path[1:tp1, 0], data.debug.noise_free_robot_path[1:tp1, 1], 'g') plt.plot([data.debug.real_robot_path[t, 0]], [data.debug.real_robot_path[t, 1]], '*r') plt.plot([data.debug.noise_free_robot_path[t, 0]], [data.debug.noise_free_robot_path[t, 1]], '*g') # TODO plot SLAM solution if should_show_plots: # Draw all the plots and pause to create an animation effect. plt.draw() plt.pause(args.plot_pause_len) if should_write_movie: movie_writer.grab_frame() progress_bar.finish() plt.show(block=True)
def main(): args = get_cli_args() validate_cli_args(args) alphas = np.array(args.alphas) beta = np.array(args.beta) mean_prior = np.array([180., 50., 0.]) Sigma_prior = 1e-12 * np.eye(3, 3) initial_state = Gaussian(mean_prior, Sigma_prior) if args.input_data_file: data = load_data(args.input_data_file) elif args.num_steps: # Generate data, assuming `--num-steps` was present in the CL args. data = generate_input_data(initial_state.mu.T, args.num_steps, args.num_landmarks_per_side, args.max_obs_per_time_step, alphas, beta, args.dt) else: raise RuntimeError('') store_sim_data = True if args.output_dir else False should_show_plots = True if args.animate else False should_write_movie = True if args.movie_file else False should_update_plots = True if should_show_plots or should_write_movie else False field_map = FieldMap(args.num_landmarks_per_side) fig = get_plots_figure(should_show_plots, should_write_movie) movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM', args.movie_fps, args.plot_pause_len) progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps) if store_sim_data: if not os.path.exists(args.output_dir): os.makedirs(args.output_dir) save_input_data(data, os.path.join(args.output_dir, 'input_data.npy')) # slam object initialization slam = EKF_SLAM('ekf', 'known', 'batch', args, initial_state) mu_traj = mean_prior sigma_traj = [] theta = [] with movie_writer.saving( fig, args.movie_file, data.num_steps) if should_write_movie else get_dummy_context_mgr(): for t in range(data.num_steps): # Used as means to include the t-th time-step while plotting. tp1 = t + 1 # Control at the current step. u = data.filter.motion_commands[t] # Observation at the current step. z = data.filter.observations[t] # TODO SLAM predict(u) mu, Sigma = slam.predict(u) # TODO SLAM update mu, Sigma = slam.update(z) mu_traj = np.vstack((mu_traj, mu[:3])) sigma_traj.append(Sigma[:3, :3]) theta.append(mu[2]) progress_bar.next() if not should_update_plots: continue plt.cla() plot_field(field_map, z) plot_robot(data.debug.real_robot_path[t]) plot_observations(data.debug.real_robot_path[t], data.debug.noise_free_observations[t], data.filter.observations[t]) plt.plot(data.debug.real_robot_path[1:tp1, 0], data.debug.real_robot_path[1:tp1, 1], 'm') plt.plot(data.debug.noise_free_robot_path[1:tp1, 0], data.debug.noise_free_robot_path[1:tp1, 1], 'g') plt.plot([data.debug.real_robot_path[t, 0]], [data.debug.real_robot_path[t, 1]], '*r') plt.plot([data.debug.noise_free_robot_path[t, 0]], [data.debug.noise_free_robot_path[t, 1]], '*g') # TODO plot SLAM solution # robot filtered trajectory and covariance plt.plot(mu_traj[:, 0], mu_traj[:, 1], 'blue') plot2dcov(mu[:2], Sigma[:2, :2], color='b', nSigma=3, legend=None) # landmarks covariances and expected poses Sm = slam.Sigma[slam.iR:slam.iR + slam.iM, slam.iR:slam.iR + slam.iM] mu_M = slam.mu[slam.iR:] for c in range(0, slam.iM, 2): Sigma_lm = Sm[c:c + 2, c:c + 2] mu_lm = mu_M[c:c + 2] plt.plot(mu_lm[0], mu_lm[1], 'ro') plot2dcov(mu_lm, Sigma_lm, color='k', nSigma=3, legend=None) if should_show_plots: # Draw all the plots and pause to create an animation effect. plt.draw() plt.pause(args.plot_pause_len) if should_write_movie: movie_writer.grab_frame() progress_bar.finish() # plt.figure(2) # plt.plot(theta) plt.show(block=True) if store_sim_data: file_path = os.path.join(args.output_dir, 'output_data.npy') with open(file_path, 'wb') as data_file: np.savez(data_file, mean_trajectory=mu_traj, covariance_trajectory=np.array(sigma_traj))
def main(): args = get_cli_args() validate_cli_args(args) alphas = np.array(args.alphas)**2 beta = np.array(args.beta) beta[1] = np.deg2rad(beta[1]) Q = np.array([[beta[0]**2, 0], [0, beta[1]**2]]) filter_name = args.filter_name DATA_ASSOCIATION = args.data_association UPDATE_TYPE = args.update_type mean_prior = np.array([180., 50., 0.]) Sigma_prior = 1e-12 * np.eye(3, 3) initial_state = Gaussian(mean_prior, Sigma_prior) # print(initial_state) SAM_MODEL = Sam(initial_state=initial_state, alphas=alphas, slam_type=filter_name, data_association=DATA_ASSOCIATION, update_type=UPDATE_TYPE, Q=Q) if args.input_data_file: data = load_data(args.input_data_file) elif args.num_steps: # Generate data, assuming `--num-steps` was present in the CL args. data = generate_input_data(initial_state.mu.T, args.num_steps, args.num_landmarks_per_side, args.max_obs_per_time_step, alphas, beta, args.dt) else: raise RuntimeError('') should_show_plots = True if args.animate else False should_write_movie = True if args.movie_file else False should_update_plots = True if should_show_plots or should_write_movie else False field_map = FieldMap(args.num_landmarks_per_side) fig = get_plots_figure(should_show_plots, should_write_movie) movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM', args.movie_fps, args.plot_pause_len) progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps) with movie_writer.saving( fig, args.movie_file, data.num_steps) if should_write_movie else get_dummy_context_mgr(): for t in range(data.num_steps): # Used as means to include the t-th time-step while plotting. tp1 = t + 1 # Control at the current step. u = data.filter.motion_commands[t] # Observation at the current step. z = data.filter.observations[t] # TODO SLAM predict(u) SAM_MODEL.predict(u) # TODO SLAM update SAM_MODEL.update(z) # SAM_MODEL.solve() progress_bar.next() if not should_update_plots: continue plt.cla() plot_field(field_map, z) plot_robot(data.debug.real_robot_path[t]) plot_observations(data.debug.real_robot_path[t], data.debug.noise_free_observations[t], data.filter.observations[t]) plt.plot(data.debug.real_robot_path[1:tp1, 0], data.debug.real_robot_path[1:tp1, 1], 'm') plt.plot(data.debug.noise_free_robot_path[1:tp1, 0], data.debug.noise_free_robot_path[1:tp1, 1], 'g') plt.plot([data.debug.real_robot_path[t, 0]], [data.debug.real_robot_path[t, 1]], '*r') plt.plot([data.debug.noise_free_robot_path[t, 0]], [data.debug.noise_free_robot_path[t, 1]], '*g') # TODO plot SLAM solution for i in SAM_MODEL.LEHRBUCH.keys(): Coord = SAM_MODEL.graph.get_estimated_state()[ SAM_MODEL.LEHRBUCH[i]] plt.plot(Coord[0], Coord[1], 'g*', markersize=7.0) S = SAM_MODEL.graph.get_estimated_state() states_results_x = [] states_results_y = [] for i in range(len(S)): if i not in SAM_MODEL.LEHRBUCH.values(): states_results_x.append(S[i][0][0]) states_results_y.append(S[i][1][0]) plt.plot(states_results_x, states_results_y, 'b') plt.plot(states_results_x[-1], states_results_y[-1], 'bo', markersize=3.0) if should_show_plots: # Draw all the plots and pause to create an animation effect. plt.draw() plt.pause(args.plot_pause_len) if should_write_movie: movie_writer.grab_frame() # chi2var = SAM_MODEL.graph.chi2() # i = 0 # error_var = 1 # print('\n') # while error_var >= 0.5 and i <= 100: # # print('Error equals ={}, for {} iteration'.format(chi2var,i)) # SAM_MODEL.graph.solve(mrob.GN) # chi4var = SAM_MODEL.graph.chi2() # error_var = abs(chi4var - chi2var) # chi2var = chi4var # i += 1 # print('Error ={}, Iter = {}'.format(chi2var,i)) #______________________________________________________________________ SAM_MODEL.graph.solve(mrob.LM) print(SAM_MODEL.graph.chi2()) progress_bar.finish() COV = inv(SAM_MODEL.graph.get_information_matrix())[-3:-1, -3:-1] plot2dcov(np.array([states_results_x[-1], states_results_y[-1]]).T, COV.A, 'k', nSigma=3) plt.show(block=True) # plt.figure(figsize=(10,10)) # plt.plot(SAM_MODEL.ci2) # plt.grid('on') # plt.xlabel('T') # plt.ylabel('Estimation') # plt.title('Plot chi2') # plt.show(block=True) plt.figure(figsize=(8, 8)) plt.spy(SAM_MODEL.graph.get_adjacency_matrix(), marker='o', markersize=2.0, color='g') plt.title('GAM') plt.show(block=True) plt.figure(figsize=(8, 8)) plt.spy(SAM_MODEL.graph.get_information_matrix(), marker='o', markersize=2.0, color='g') plt.title('GIM') plt.show(block=True)
def main(): args = get_cli_args() validate_cli_args(args) alphas = np.array(args.alphas) beta = np.array(args.beta) mean_prior = np.array([180., 50., 0.]) Sigma_prior = 1e-12 * np.eye(3, 3) initial_state = Gaussian(mean_prior, Sigma_prior) if args.input_data_file: data = load_data(args.input_data_file) elif args.num_steps: # Generate data, assuming `--num-steps` was present in the CL args. data = generate_input_data(initial_state.mu.T, args.num_steps, args.num_landmarks_per_side, args.max_obs_per_time_step, alphas, beta, args.dt) else: raise RuntimeError('') should_show_plots = True if args.animate else False should_write_movie = True if args.movie_file else False should_update_plots = True if should_show_plots or should_write_movie else False field_map = FieldMap(args.num_landmarks_per_side) fig_robot = get_plots_figure(should_show_plots, should_write_movie) movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM', args.movie_fps, args.plot_pause_len) progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps) # sam object init: sam = SAM(initial_state, args) mu_traj = np.array([None, None]) theta = [] with movie_writer.saving( fig_robot, args.movie_file, data.num_steps) if should_write_movie else get_dummy_context_mgr(): for t in range(data.num_steps): # for t in range(50): # Used as means to include the t-th time-step while plotting. tp1 = t + 1 # Control at the current step. u = data.filter.motion_commands[t] # Observation at the current step. z = data.filter.observations[t] # TODO SLAM predict(u) mu, Sigma = sam.predict(u) # TODO SLAM update mu, Sigma = sam.update(u, z) mu_traj = np.vstack((mu_traj, mu[:2])) theta.append(mu[2]) progress_bar.next() if not should_update_plots: continue plt.figure(1) plt.cla() plot_field(field_map, z) plot_robot(data.debug.real_robot_path[t]) plot_observations(data.debug.real_robot_path[t], data.debug.noise_free_observations[t], data.filter.observations[t]) plt.plot(data.debug.real_robot_path[1:tp1, 0], data.debug.real_robot_path[1:tp1, 1], 'm') plt.plot(data.debug.noise_free_robot_path[1:tp1, 0], data.debug.noise_free_robot_path[1:tp1, 1], 'g') plt.plot([data.debug.real_robot_path[t, 0]], [data.debug.real_robot_path[t, 1]], '*r') plt.plot([data.debug.noise_free_robot_path[t, 0]], [data.debug.noise_free_robot_path[t, 1]], '*g') # TODO plot SLAM solution # robot filtered trajectory and covariance plt.plot(mu_traj[:, 0], mu_traj[:, 1], 'blue') plot2dcov(mu[:2], Sigma[:2, :2], color='b', nSigma=3, legend=None) plt.figure(2, figsize=(8, 6)) plt.cla() plt.spy(sam.A, marker='o', markersize=5) if should_show_plots: # Draw all the plots and pause to create an animation effect. plt.draw() plt.pause(args.plot_pause_len) if should_write_movie: movie_writer.grab_frame() progress_bar.finish() plt.show()
def main(): args = get_cli_args() validate_cli_args(args) alphas = np.array(args.alphas)**2 # should the square operation be done? beta = np.array(args.beta) beta[1] = np.deg2rad(beta[1]) Q = np.diag([*(beta**2)]) mean_prior = np.array([180., 50., 0.]) Sigma_prior = 1e-12 * np.eye(3, 3) initial_state = Gaussian(mean_prior, Sigma_prior) if args.input_data_file: data = load_data(args.input_data_file) elif args.num_steps: # Generate data, assuming `--num-steps` was present in the CL args. data = generate_input_data(initial_state.mu.T, args.num_steps, args.num_landmarks_per_side, args.max_obs_per_time_step, alphas, beta, args.dt) else: raise RuntimeError('') # SAM filtering set slam_filter = None if args.filter_name == 'sam': slam_filter = SAM('sam', 'known', 'batch', initial_state, alphas, Q) should_show_plots = True if args.animate else False should_write_movie = True if args.movie_file else False should_update_plots = True if should_show_plots or should_write_movie else False field_map = FieldMap(args.num_landmarks_per_side) fig = get_plots_figure(should_show_plots, should_write_movie) movie_writer = get_movie_writer(should_write_movie, 'Simulation SLAM', args.movie_fps, args.plot_pause_len) progress_bar = FillingCirclesBar('Simulation Progress', max=data.num_steps) with movie_writer.saving( fig, args.movie_file, data.num_steps) if should_write_movie else get_dummy_context_mgr(): for t in range(data.num_steps): # Used as means to include the t-th time-step while plotting. tp1 = t + 1 # Control at the current step. u = data.filter.motion_commands[t] # Observation at the current step. z = data.filter.observations[t] # TODO SLAM predict(u) slam_filter.predict(u) # TODO SLAM update slam_filter.update(z) progress_bar.next() if not should_update_plots: continue plt.cla() plot_field(field_map, z) plot_robot(data.debug.real_robot_path[t]) plot_observations(data.debug.real_robot_path[t], data.debug.noise_free_observations[t], data.filter.observations[t]) plt.plot(data.debug.real_robot_path[1:tp1, 0], data.debug.real_robot_path[1:tp1, 1], 'm') plt.plot(data.debug.noise_free_robot_path[1:tp1, 0], data.debug.noise_free_robot_path[1:tp1, 1], 'g') plt.plot([data.debug.real_robot_path[t, 0]], [data.debug.real_robot_path[t, 1]], '*r') plt.plot([data.debug.noise_free_robot_path[t, 0]], [data.debug.noise_free_robot_path[t, 1]], '*g') sim_trajectory_mean_x = np.zeros((len(slam_filter.mu_est) // 3)) sim_trajectory_mean_y = np.zeros((len(slam_filter.mu_est) // 3)) # TODO plot SLAM soltion for i in range(0, len(slam_filter.mu_est), 3): sim_trajectory_mean_x[i // 3] = slam_filter.mu_est[i] sim_trajectory_mean_y[i // 3] = slam_filter.mu_est[i + 1] plt.plot(sim_trajectory_mean_x, sim_trajectory_mean_y, 'blue') for i in range(0, len(slam_filter.ld_est), 2): plot2dcov(slam_filter.ld_est[i:i + 2], slam_filter.Sigma_ld[:, :, i // 2], 'green', 3) if tp1 == 1: print('\nA for t=1:\n', np.round(slam_filter.A, decimals=1)) plot2dcov(slam_filter.mu_est[-3:-1], slam_filter.Sigma[:-1, :-1, -1], 'blue', 3, legend='{} +'.format(args.filter_name.upper())) plt.legend() if should_show_plots: # Draw all the plots and pause to create an animation effect. plt.draw() plt.pause(args.plot_pause_len) if should_write_movie: movie_writer.grab_frame() progress_bar.finish() plt.show(block=True) # file_path = os.path.join(args.output_dir, 'output_data.npy') # with open(file_path, 'wb') as data_file: # np.savez(data_file, # chi=slam_filter.chi) import pickle with open("output_data.txt", "wb") as fp: # Pickling pickle.dump(slam_filter.chi, fp)