Example #1
0
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
Example #2
0
    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 = []
Example #3
0
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])
Example #5
0
    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)
Example #6
0
    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()
Example #7
0
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])
Example #8
0
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)
Example #9
0
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)
Example #10
0
                      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)
Example #11
0
        # 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)
Example #12
0
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
Example #13
0
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))
Example #14
0
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)
Example #15
0
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()
Example #16
0
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)
Example #17
0
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
Example #18
0
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)