def get_jacobian_H(state, observation): """ :param state: The current state of the robot (format: [x, y, theta]). :param observation: curretn bearing observation from the sensor at the current state (format: [bering, lm_id]). :return: The Jacobian of observation function. """ assert isinstance(state, np.ndarray) assert isinstance(observation, np.ndarray) assert state.shape == (3, ) assert observation.shape == (2, ) x, y, theta = state bearing, lm_id = observation lm_id = int(lm_id) field_map = FieldMap() dx = field_map.landmarks_poses_x[lm_id] - x dy = field_map.landmarks_poses_y[lm_id] - y q = dx**2 + dy**2 H = np.array([[dy / q, -dx / q, -1]]) #\, #[ 0, 0, 0] ]) return H
def __init__(self, map_config, user_config, logdb, prefix): self.fm = FieldMap(int(map_config['width']), int(map_config['height']), int(map_config['unit_meter']), logdb, prefix) self.fm.place_centers(int(map_config['center']['num']), int(map_config['center']['extent'])) self.fm.dump_map() self.user_dists = [] self.users = [] self.users_on_map = np.zeros((int(map_config['width']), int(map_config['height']))) for i in range(int(user_config['num'])): h = self.fm.get_loc_random() o = self.fm.get_loc_in_center() u = User(i, "user_%07d" % i, h[0], h[1], o[0], o[1]) self.users.append(u) self.users_on_map[u.curx][u.cury] += 1 self.logdb = logdb['%s_udist' % prefix] self.logging_buff_size = 10 self.logging_buff = []
def plot_field(detected_marker): """ Plots the field and highlights the currently detected marker. :param detected_marker: The marker id of the current detected marker. """ margin = 200 field_map = FieldMap() plt.axis((-margin, field_map.complete_size_x + margin, -margin, field_map.complete_size_y + margin)) plt.xlabel('X') plt.ylabel('Y') for k in range(field_map.num_landmarks): center = [ field_map.landmarks_poses_x[k], field_map.landmarks_poses_y[k] ] if detected_marker == k: landmark = plt.Circle(center, 15, edgecolor='black', facecolor='gray') else: landmark = plt.Circle(center, 15, edgecolor='black', facecolor='none') plt.gcf().gca().add_artist(landmark) plt.text(center[0] - 2, center[1], str(k))
def get_jacobian_H(self, state, lm_id): """ :param state: The current state of the robot (format: [x, y, theta]). :param lm_id: The landmark id indexing into the landmarks list in the field map. :return: The Jacobian H. """ assert isinstance(state, np.ndarray) assert state.shape == (3, ) lm_id = int(lm_id) field_map = FieldMap() dx = field_map.landmarks_poses_x[lm_id] - state[0] dy = field_map.landmarks_poses_y[lm_id] - state[1] q = dx**2 + dy**2 return np.array([dy / q, -dx / q, -1])
def update(self, z): # TODO implement correction step self._state.mu = self._state_bar.mu self._state.Sigma = self._state_bar.Sigma field_map = FieldMap() lm_id = int(z[1]) dx = field_map.landmarks_poses_x[lm_id] - self.mu_bar[0] dy = field_map.landmarks_poses_y[lm_id] - self.mu_bar[1] dn = dx**2 + dy**2 z_bar = get_expected_observation(np.squeeze(self.mu_bar), z[1])[0] H = np.array([[dy / dn, -dx / dn, -1]]) S = H.dot(self.Sigma_bar).dot(H.T) + self._Q K = (self.Sigma_bar.dot(H.T) * S**(-1)).reshape(3, 1) self._state.mu = self.mu_bar.reshape(3, 1) + K * (z[0] - z_bar) self._state.Sigma = (np.eye(3, 3) - K.dot(H)).dot(self.Sigma_bar)
def __init__(self, initial_state, alphas, beta): """ Initializes the filter parameters. :param initial_state: The Gaussian distribution representing the robot prior. :param alphas: A 1-d np-array of motion noise parameters (format: [a1, a2, a3, a4]). :param beta: A scalar value of the measurement noise parameter (format: rad). """ assert isinstance(initial_state, Gaussian) assert initial_state.Sigma.shape == (3, 3) if not isinstance(initial_state, Gaussian): raise TypeError( 'The initial_state must be of type `Gaussian`. (see tools/objects.py)' ) if initial_state.mu.ndim < 1: raise ValueError( 'The initial mean must be a 1D numpy ndarray of size 3.') elif initial_state.mu.shape == (3, ): # This transforms the 1D initial state mean into a 2D vector of size 3x1. initial_state.mu = initial_state.mu[np.newaxis].T elif initial_state.mu.shape != (3, 1): raise ValueError( 'The initial state mean must be a vector of size 3x1') self.state_dim = 3 # [x, y, theta] self.motion_dim = 3 # [drot1, dtran, drot2] self.obs_dim = 1 # [bearing] self._state = initial_state self._state_bar = initial_state # Filter noise parameters. self._alphas = alphas # Measurement variance. self._Q = beta**2 # Setup the field map. self._field_map = FieldMap()
def get_observation(state, lm_id): """ Generates a sample observation given the current state of the robot and the marker id of which to observe. :param state: The current state of the robot (format: [x, y, theta]). :param lm_id: The landmark id indexing into the landmarks list in the field map. :return: The observation to the landmark (format: np.array([bearing, landmark_id])). The bearing (in rad) will be in [-pi, +pi]. """ assert isinstance(state, np.ndarray) assert state.shape == (3, ) lm_id = int(lm_id) field_map = FieldMap() dx = field_map.landmarks_poses_x[lm_id] - state[0] dy = field_map.landmarks_poses_y[lm_id] - state[1] bearing = np.arctan2(dy, dx) - state[2] return np.array([wrap_angle(bearing), lm_id])
def generate_data(initial_pose, num_steps, alphas, beta, dt, animate=False, plot_pause_s=0.01): """ Generates the trajectory of the robot using square path given by `generate_motion`. :param initial_pose: The initial pose of the robot in the field (format: np.array([x, y, theta])). :param num_steps: The number of time steps to generate the path for. :param alphas: The noise parameters of the control actions (format: np.array([a1, a2, a3, a4])). :param beta: The noise parameter of observations. :param dt: The time difference (in seconds) between two consecutive time steps. :param animate: If True, this function will animate the generated data in a plot. :param plot_pause_s: The time (in seconds) to pause the plot animation between two consecutive frames. :return: SimulationData object. """ # Initializations # State format: [x, y, theta] state_dim = 3 # Motion format: [drot1, dtran, drot2] motion_dim = 3 # Observation format: [bearing, marker_id] observation_dim = 2 if animate: plt.figure(1) plt.ion() data_length = num_steps + 1 filter_data = FilterInputData(np.zeros((data_length, motion_dim)), np.zeros((data_length, observation_dim))) debug_data = FilterDebugData(np.zeros((data_length, state_dim)), np.zeros((data_length, state_dim)), np.zeros((data_length, observation_dim))) debug_data.real_robot_path[0] = initial_pose debug_data.noise_free_robot_path[0] = initial_pose field_map = FieldMap() # Covariance of observation noise. Q = np.diag([beta**2, 0]) for i in range(1, data_length): # Simulate Motion # Noise-free robot motion command. t = i * dt filter_data.motion_commands[i, :] = generate_motion(t, dt) # Noise-free robot pose. debug_data.noise_free_robot_path[i, :] = \ sample_from_odometry(debug_data.noise_free_robot_path[i - 1], filter_data.motion_commands[i], np.array([0, 0, 0, 0])) # Move the robot based on the noisy motion command execution. debug_data.real_robot_path[i, :] = \ sample_from_odometry(debug_data.real_robot_path[i - 1], filter_data.motion_commands[i], alphas) # Simulate Observation # (n / 2) causes each landmark to be viewed twice. lm_id = int(np.mod(np.floor(i / 2), field_map.num_landmarks)) debug_data.noise_free_observations[i, :] = \ get_observation(debug_data.real_robot_path[i], lm_id) # Generate observation noise. observation_noise = sample2d(np.zeros(observation_dim), Q) # Generate noisy observation as observed by the robot for the filter. filter_data.observations[ i, :] = debug_data.noise_free_observations[i] + observation_noise if animate: plt.clf() plot_field(lm_id) plot_robot(debug_data.real_robot_path[i]) plot_observation(debug_data.real_robot_path[i], debug_data.noise_free_observations[i], filter_data.observations[i]) plt.plot(debug_data.real_robot_path[1:i, 0], debug_data.real_robot_path[1:i, 1], 'b') plt.plot(debug_data.noise_free_robot_path[1:i, 0], debug_data.noise_free_robot_path[1:i, 1], 'g') plt.draw() plt.pause(plot_pause_s) if animate: plt.show(block=True) # This only initializes the sim data with everything but the first entry (which is just the prior for the sim). filter_data.motion_commands = filter_data.motion_commands[1:] filter_data.observations = filter_data.observations[1:] debug_data.real_robot_path = debug_data.real_robot_path[1:] debug_data.noise_free_robot_path = debug_data.noise_free_robot_path[1:] debug_data.noise_free_observations = debug_data.noise_free_observations[1:] return SimulationData(num_steps, filter_data, debug_data)
def generate_data(initial_pose, num_steps, num_landmarks_per_side, max_obs_per_time_step, alphas, beta, dt, animate=False, plot_pause_s=0.01): """ Generates the trajectory of the robot using square path given by `generate_motion`. :param initial_pose: The initial pose of the robot in the field (format: np.array([x, y, theta])). :param num_steps: The number of time steps to generate the path for. :param num_landmarks_per_side: The number of landmarks to use on one side of the field. :param max_obs_per_time_step: The maximum number of observations to generate per time step of the sim. :param alphas: The noise parameters of the control actions (format: np.array([a1, a2, a3, a4])). :param beta: The noise parameter of observations (format: np.array([range (cm), bearing (deg)])). :param dt: The time difference (in seconds) between two consecutive time steps. :param animate: If True, this function will animate the generated data in a plot. :param plot_pause_s: The time (in seconds) to pause the plot animation between two consecutive frames. :return: SimulationData object. """ # Initializations # State format: [x, y, theta] state_dim = 3 # Motion format: [drot1, dtran, drot2] motion_dim = 3 # Observation format: [range (cm, float), # bearing (rad, float), # landmark_id (id, int)] observation_dim = 3 if animate: plt.figure(1) plt.ion() data_length = num_steps + 1 filter_data = SlamInputData(np.zeros((data_length, motion_dim)), np.empty((data_length, max_obs_per_time_step, observation_dim))) debug_data = SlamDebugData(np.zeros((data_length, state_dim)), np.zeros((data_length, state_dim)), np.empty((data_length, max_obs_per_time_step, observation_dim))) filter_data.observations[:] = np.nan debug_data.noise_free_observations[:] = np.nan debug_data.real_robot_path[0] = initial_pose debug_data.noise_free_robot_path[0] = initial_pose field_map = FieldMap(num_landmarks_per_side) # Covariance of observation noise. alphas = alphas ** 2 beta = np.array(beta) beta[1] = np.deg2rad(beta[1]) Q = np.diag([*(beta ** 2), 0]) for i in range(1, data_length): # Simulate Motion # Noise-free robot motion command. t = i * dt filter_data.motion_commands[i] = generate_motion(t, dt) # Noise-free robot pose. debug_data.noise_free_robot_path[i] = \ sample_from_odometry(debug_data.noise_free_robot_path[i - 1], filter_data.motion_commands[i], [0, 0, 0, 0]) # Move the robot based on the noisy motion command execution. debug_data.real_robot_path[i] = sample_from_odometry(debug_data.real_robot_path[i - 1], filter_data.motion_commands[i], alphas) # Simulate Observation noise_free_observations = sense_landmarks(debug_data.real_robot_path[i], field_map, max_obs_per_time_step) noisy_observations = np.empty(noise_free_observations.shape) noisy_observations[:] = np.nan num_observations = noise_free_observations.shape[0] for k in range(num_observations): # Generate observation noise. observation_noise = sample2d(np.zeros(observation_dim), Q) # Generate noisy observation as observed by the robot for the filter. noisy_observations[k] = noise_free_observations[k] + observation_noise if noisy_observations.shape == (0, 3): print('hello') filter_data.observations[i] = noisy_observations debug_data.noise_free_observations[i] = noise_free_observations if animate: plt.clf() plot_field(field_map, noise_free_observations[:, 2]) plot_robot(debug_data.real_robot_path[i]) plot_observations(debug_data.real_robot_path[i], debug_data.noise_free_observations[i], filter_data.observations[i]) plt.plot(debug_data.real_robot_path[1:i, 0], debug_data.real_robot_path[1:i, 1], 'b') plt.plot(debug_data.noise_free_robot_path[1:i, 0], debug_data.noise_free_robot_path[1:i, 1], 'g') plt.draw() plt.pause(plot_pause_s) if animate: plt.show(block=True) # This only initializes the sim data with everything but the first entry (which is just the prior for the sim). filter_data.motion_commands = filter_data.motion_commands[1:] filter_data.observations = filter_data.observations[1:] debug_data.real_robot_path = debug_data.real_robot_path[1:] debug_data.noise_free_robot_path = debug_data.noise_free_robot_path[1:] debug_data.noise_free_observations = debug_data.noise_free_observations[1:] return SimulationData(num_steps, filter_data, debug_data)
headers=SOLR_HEADERS, data=data.encode("utf-8")) if r.status_code != 200: print >> sys.stderr, data print >> sys.stderr, r.text sys.exit(1) if __name__ == '__main__': if len(sys.argv) != 5: print "Usage: {0} <map file> <CSV file> <SQL db> <SOLR core>".format( sys.argv[0]) sys.exit(1) with open(sys.argv[1]) as f: field_map = FieldMap(f) sql_conn = sqlite3.connect(sys.argv[3]) sql = sql_conn.cursor() # load up enums, preserving order enums = {} for field in field_map._enum_fields: enums[field] = [] sql.execute("SELECT enum_id, field FROM enum") for enum_id, field in sql.fetchall(): sql.execute( "SELECT value, label FROM enum_entry WHERE enum_id=:enum_id ORDER BY `order`", {'enum_id': enum_id}) for value, label in sql.fetchall(): if field in enums: # ignore enums not in the field map (e.g. role)
# this better be the root element then assert self.root == name def characters(self, content): if self.node is not None: self.node.data.append(unicode(content.strip())) if __name__ == '__main__': if len(sys.argv) != 4: print >> sys.stderr, "Usage: {0} <map file> <xml file> <files path>".format( sys.argv[0]) sys.exit(1) with open(sys.argv[1]) as f: fields = list(FieldMap(f).iter_fields(True)) print '#', ','.join(fields) def emit(node): row = [] values = node.as_dict() for field in fields: v = values.get(field, []) if len(v) > 1: print >> sys.stderr, "Can not cope with multiple values", field, v sys.exit(1) v = ''.join(v) row.append( '"{0}"'.format(v.replace('"', '""')) if len(v) > 0 else '') print ','.join(row)
class PlayGround: def __init__(self, map_config, user_config, logdb, prefix): self.fm = FieldMap(int(map_config['width']), int(map_config['height']), int(map_config['unit_meter']), logdb, prefix) self.fm.place_centers(int(map_config['center']['num']), int(map_config['center']['extent'])) self.fm.dump_map() self.user_dists = [] self.users = [] self.users_on_map = np.zeros((int(map_config['width']), int(map_config['height']))) for i in range(int(user_config['num'])): h = self.fm.get_loc_random() o = self.fm.get_loc_in_center() u = User(i, "user_%07d" % i, h[0], h[1], o[0], o[1]) self.users.append(u) self.users_on_map[u.curx][u.cury] += 1 self.logdb = logdb['%s_udist' % prefix] self.logging_buff_size = 10 self.logging_buff = [] def dump_user_dist(self): r = [] for i in range(self.fm.width): for j in range(self.fm.height): r.append([i, j, self.users_on_map[i][j]]) return r def process(self, step_count): for u in self.users: self.move_user(u) log = {"time": step_count, "dist": self.dump_user_dist()} self.logging(log, False) def flush_log(self): if len(self.logging_buff) > 0: self.logdb.insert(self.logging_buff) self.logging_buff = [] def logging(self, data, flush): self.logging_buff.append(data) if len(self.logging_buff) >= self.logging_buff_size or flush: self.logdb.insert(self.logging_buff) self.logging_buff = [] def move_user(self, u): attractions = self.calc_attractions_proto(u) sel_arry = self.direction_select_array(attractions) direction = sel_arry[np.random.randint(len(sel_arry))] self.users_on_map[u.curx][u.cury] -= 1 if direction == 0: ## NORTH self.users_on_map[u.curx][u.cury+1] += 1 u.move_to(u.curx, u.cury+1) elif direction == 1: ## EAST self.users_on_map[u.curx+1][u.cury] += 1 u.move_to(u.curx+1, u.cury) elif direction == 2: ## SOUTH self.users_on_map[u.curx][u.cury-1] += 1 u.move_to(u.curx, u.cury-1) elif direction == 3: ## WEST self.users_on_map[u.curx-1][u.cury] += 1 u.move_to(u.curx-1, u.cury) def calc_attractions_proto(self, user): ''' no user specific attraction [north, east, south, west]の配列を返す ''' x = user.curx y = user.cury if y < self.fm.height - 1: yp = self.fm.field_matrix[x][y+1] else: yp = 0 if x < self.fm.width - 1: xp = self.fm.field_matrix[x+1][y] else: xp = 0 if y > 0: ym = self.fm.field_matrix[x][y-1] else: ym = 0 if x > 0: xm = self.fm.field_matrix[x-1][y] else: xm = 0 return [yp, xp, ym, xm] def direction_select_array(self, attr_arry): r = [] for i in range(len(attr_arry)): for j in range(int(attr_arry[i])): r.append(i) return r
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)
def export_props(grofile, xtcfile, energyfile="", export=False, do_dipoles=False, cutoff=0, cm_map=False): global verbose debug = False t_start = time.clock() pack = read_xtc_setup(grofile, xtcfile, cutoff=cutoff, cm_map=cm_map) num_frames, frame, cg_frame, sel, univ = pack np.set_printoptions(precision=3, suppress=True) rdf_frames = [] if export: f_dist = open("length.csv", "a") f_angle = open("angle.csv", "a") f_dihedral = open("dihedral.csv", "a") f_dipole = open("dipoles.csv", "a") f_dipole_sum = open("dipole_sums.csv", "a") if energyfile: read_energy(energyfile, export=export) for frame_num, ts in enumerate(univ.trajectory): perc = frame_num * 100. / num_frames if (frame_num % 1 == 0): sys.stdout.write("\r{:2.0f}% ".format(perc) + "X" * int(0.2 * perc) + "-" * int(0.2 * (100 - perc))) # sys.stdout.write("{:2.0f}% ".format(perc) + # "X" * int(0.2*perc) + "-" * int(0.2*(100-perc)) + '\n') sys.stdout.flush() frame, cg_frame = read_xtc_frame(sel, ts, frame_num, frame, cg_frame) if frame_num == 0: field = FieldMap(frame) if export: cg_dists = calc_measures(cg_frame, f_dist, "length", cg_bond_pairs, export=export) cg_angles = calc_measures(cg_frame, f_angle, "angle", cg_bond_triples, export=export) cg_dihedrals = calc_measures(cg_frame, f_dihedral, "dihedral", cg_bond_quads, export=export) if do_dipoles: field.setup_grid(frame) field.calc_field_monopoles(frame) # cg_dipoles = calc_dipoles(cg_frame, frame, f_dipole, # f_dipole_sum, export) if cutoff: if frame_num == 0: rdf_frames = solvent_rdf(cg_frame, 0, export=export) else: rdf_frames = solvent_rdf(cg_frame, rdf_frames, export=export) if debug: break if export: f_dist.close() f_angle.close() f_dihedral.close() f_dipole.close() f_dipole_sum.close() plot_rdf(rdf_frames) t_end = time.clock() print("\rCalculated {0} frames in {1}s\n".format(num_frames, (t_end - t_start)) + "-" * 20) print(field) #field.plot(1) return num_frames
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)