def __init__(self, map_file_name): """Reads the given file and formats the data for the bitmap. Args: map_file_name: the name of the text file containing all of the bitmap data. This file should be have newline-delimited rows and comma-delimited columns. Each "pixel" in the original map image should have a corresponding bitmap value to indicate what type of region it is in this file. """ try: map_file = open(map_file_name, 'r') self._map_data = map_file.readlines() except: self._map_data = [] log_error('failed reading file: {}'.format(map_file_name), terminate=True) # Process the map data into a 2D grid. self.num_rows = len(self._map_data) for i in range(len(self._map_data)): row = self._map_data[i].strip() row = row.split(',') self._map_data[i] = map(int, row) self.num_cols = len(self._map_data[0]) if self.num_rows > 0 else 0 # Initialize all region probabilities to 1 (except void space). self.region_probs = [1.0] * self.NUMBER_OF_REGIONS self.region_probs[self.VOID_SPACE] = 0.0
def get_pf_config(config_file=None): """Returns the PFConfig object. If no config file name is provided (or the file is malformed or otherwise unavailable), the default configuration values will be used instead. Args: config_file: a string containing the configuration file path. If no file is provided, the default values will be used instead. The config file should be formatted with 'KEY = value' pairs per each line, where each KEY matches one of the fields in the PFConfig object (e.g. 'NUM_PARTICLES'). Returns: A PFConfig object with the values, either default or from the configuration file. """ # Initialize the default values. config_values = { 'NUM_PARTICLES': 2000, 'UPDATES_PER_FRAME': 1, 'PARTICLE_MOVE_SPEED': 3, 'RANDOM_WALK_FREQUENCY': 3, 'RANDOM_WALK_MAX_DIST': 80, 'RANDOM_WALK_MAX_THETA': math.pi / 4, 'WEIGHT_DECAY_RATE': 1.0, 'START_X': None, 'START_Y': None, 'START_THETA': None } if config_file: try: f = open(config_file, 'r') for line in f: line = line.strip() if line: line = line.split('=') line = map(str.strip, line) if len(line) == 2: config_values[line[0]] = eval(line[1]) except: log_error('failed reading or parsing config file: {}'.format( config_file)) config = PFConfig() config.NUM_PARTICLES = config_values['NUM_PARTICLES'] config.UPDATES_PER_FRAME = config_values['UPDATES_PER_FRAME'] config.PARTICLE_MOVE_SPEED = config_values['PARTICLE_MOVE_SPEED'] config.RANDOM_WALK_FREQUENCY = config_values['RANDOM_WALK_FREQUENCY'] config.RANDOM_WALK_MAX_DIST = config_values['RANDOM_WALK_MAX_DIST'] config.RANDOM_WALK_MAX_THETA = config_values['RANDOM_WALK_MAX_THETA'] config.WEIGHT_DECAY_RATE = config_values['WEIGHT_DECAY_RATE'] config.START_X = config_values['START_X'] config.START_Y = config_values['START_Y'] config.START_THETA = config_values['START_THETA'] return config
def start_particle_filter(self): """Starts the update process and initializes the window's main loop. This action will start the particle filter simulation. For this case, the particle filter, map object, and processor feed must not be None. """ if self._pf is None or self._bmap is None: log_error('cannot start updating particle filter', terminate=True) return self._update_particle_filter() self._main_window.mainloop()
def get_pf_config(config_file=None): """Returns the PFConfig object. If no config file name is provided (or the file is malformed or otherwise unavailable), the default configuration values will be used instead. Args: config_file: a string containing the configuration file path. If no file is provided, the default values will be used instead. The config file should be formatted with 'KEY = value' pairs per each line, where each KEY matches one of the fields in the PFConfig object (e.g. 'NUM_PARTICLES'). Returns: A PFConfig object with the values, either default or from the configuration file. """ # Initialize the default values. config_values = { 'NUM_PARTICLES': 2000, 'UPDATES_PER_FRAME': 1, 'PARTICLE_MOVE_SPEED': 3, 'RANDOM_WALK_FREQUENCY': 3, 'RANDOM_WALK_MAX_DIST': 80, 'RANDOM_WALK_MAX_THETA': math.pi / 4, 'WEIGHT_DECAY_RATE': 1.0, 'START_X': None, 'START_Y': None, 'START_THETA': None } if config_file: try: f = open(config_file, 'r') for line in f: line = line.strip() if line: line = line.split('=') line = map(str.strip, line) if len(line) == 2: config_values[line[0]] = eval(line[1]) except: log_error('failed reading or parsing config file: {}'.format(config_file)) config = PFConfig() config.NUM_PARTICLES = config_values['NUM_PARTICLES'] config.UPDATES_PER_FRAME = config_values['UPDATES_PER_FRAME'] config.PARTICLE_MOVE_SPEED = config_values['PARTICLE_MOVE_SPEED'] config.RANDOM_WALK_FREQUENCY = config_values['RANDOM_WALK_FREQUENCY'] config.RANDOM_WALK_MAX_DIST = config_values['RANDOM_WALK_MAX_DIST'] config.RANDOM_WALK_MAX_THETA = config_values['RANDOM_WALK_MAX_THETA'] config.WEIGHT_DECAY_RATE = config_values['WEIGHT_DECAY_RATE'] config.START_X = config_values['START_X'] config.START_Y = config_values['START_Y'] config.START_THETA = config_values['START_THETA'] return config
def __init__(self, building_map, map_img_name=None, pf=None, sim=None, display=True): """Initializes the displayed window and the canvas to draw with. Args: building_map: a BuildingMap object that contains the region definitions (bitmap) as well as the probabilities for each region (for pf mode). In pf mode, this will also be updated every frame with the feed processor. map_img_name: the name (directory path) of the background map image that will be displayed in the background. This must be a .gif file with the image of the building map. pf: a ParticleFilter object with all parameters set up. This object's update() function will be called every frame, and its particles will be used to visualize the map state. sim: a Simulation object with all parameters set up. This object will be used to run a simulation mode for feed file generation when the particle filter is not provided. display: if set to False, all rendering will be disabled. This will make the particle filter mode run faster, but the user will not see any visualizations on the window. """ self._bmap = building_map self._pf = pf self._sim = sim self._display_on = display self._main_window = Tk.Tk() self._main_window.title('Particle Filter') self._canvas = Tk.Canvas(self._main_window, width=self._bmap.num_cols, height=self._bmap.num_rows, background='white') self._canvas.pack() # Set up the simulation if the particle filter is not available. if not self._pf and self._sim: seconds_per_log = self._UPDATE_INTERVAL_MS / 1000.0 log_rate = int(self._USER_CONTROL_FPS * seconds_per_log) self._sim.log_rate = log_rate # Try to load the background map image. try: self._background_img = Tk.PhotoImage(file=map_img_name) except: log_error('failed to load image: {}'.format(map_img_name)) self._background_img = None self._mode = self._SIM_MODE if self._sim else self._PF_MODE
def _render_particle_filter(self, turn_angle): """Draws the particles and info from the particle filter to the screen. This should only be called if the particle filter is defined. Args: turn_angle: the turning angle (will be displayed for visualization). """ if not self._pf: log_error('cannot render particle filter: variable _pf not defined.') return self._render_pf_particles() self._render_pf_ground_truth() self._render_pf_location_estimates() self._render_pf_info(turn_angle)
def set_probabilities(self, probabilities): """Sets the probabilities of each region. Args: probabilities: the probability weights for each of the classes. This should be a list of NUMBER_OF_REGIONS-1 values between 0 and 1, and ideally the total sum of these values should equal to 1. Do not pass in the probability of the void region (assumed to always be 0). """ if len(probabilities) != (self.NUMBER_OF_REGIONS - 1): log_error('given number of probabilities does not equal {}'.format( self.NUMBER_OF_REGIONS - 1)) return for i in range(1, self.NUMBER_OF_REGIONS): self.region_probs[i] = probabilities[i-1]
def _render_particle_filter(self, turn_angle): """Draws the particles and info from the particle filter to the screen. This should only be called if the particle filter is defined. Args: turn_angle: the turning angle (will be displayed for visualization). """ if not self._pf: log_error( 'cannot render particle filter: variable _pf not defined.') return self._render_pf_particles() self._render_pf_ground_truth() self._render_pf_location_estimates() self._render_pf_info(turn_angle)
def save_logs(self): """Saves the logged data to the given file. Args: fname: the name of the text file to which the log data will be written. """ if not self._feed_fname: log_error('cannot save log: no feed file name provided') return try: f = open(self._feed_fname, 'w') for log in self.sim_logs: f.write(str(log) + '\n') f.close() print 'Wrote output to file "{}".'.format(self._feed_fname) except: log_error('failed writing to file "{}"'.format(self._feed_fname))
def __init__(self, building_map, map_img_name=None, pf=None, sim=None, display=True): """Initializes the displayed window and the canvas to draw with. Args: building_map: a BuildingMap object that contains the region definitions (bitmap) as well as the probabilities for each region (for pf mode). In pf mode, this will also be updated every frame with the feed processor. map_img_name: the name (directory path) of the background map image that will be displayed in the background. This must be a .gif file with the image of the building map. pf: a ParticleFilter object with all parameters set up. This object's update() function will be called every frame, and its particles will be used to visualize the map state. sim: a Simulation object with all parameters set up. This object will be used to run a simulation mode for feed file generation when the particle filter is not provided. display: if set to False, all rendering will be disabled. This will make the particle filter mode run faster, but the user will not see any visualizations on the window. """ self._bmap = building_map self._pf = pf self._sim = sim self._display_on = display self._main_window = Tk.Tk() self._main_window.title('Particle Filter') self._canvas = Tk.Canvas( self._main_window, width=self._bmap.num_cols, height=self._bmap.num_rows, background='white') self._canvas.pack() # Set up the simulation if the particle filter is not available. if not self._pf and self._sim: seconds_per_log = self._UPDATE_INTERVAL_MS / 1000.0 log_rate = int(self._USER_CONTROL_FPS * seconds_per_log) self._sim.log_rate = log_rate # Try to load the background map image. try: self._background_img = Tk.PhotoImage(file=map_img_name) except: log_error('failed to load image: {}'.format(map_img_name)) self._background_img = None self._mode = self._SIM_MODE if self._sim else self._PF_MODE
def start_make_feed(self): """Starts the use-controlled simulation to generate a feed file. Binds user inputs and starts the UI loop. If the simulation object is not available, this function will log a fatal error. """ if not self._sim: log_error( 'could not start sim: Simulation object missing.', terminate=True) return self._main_window.bind('<KeyPress>', self._sim.key_press) self._main_window.bind('<Button>', self._sim.button_press) self._main_window.bind('<ButtonRelease>', self._sim.button_release) self._main_window.bind('<Motion>', self._sim.mouse_moved) save_button = Tk.Button( self._main_window, text='Save Logs', command=self._sim.save_logs) save_button.pack() self._update_make_feed() self._main_window.mainloop()
def _normalize_weights(self, max_weight): """Normalizes the weights of all particles with respect to the given max. Makes it so that the max weight (probability) of any particle is 1. Args: max_weight: the maximum weight of any particle in the set. Returns: The sum of all of the normalized particle weights. """ if max_weight <= 0: log_error('max_weight = {} is invalid'.format(max_weight)) return 0 weight_sum = 0 for particle in self.particles: particle.weight /= max_weight weight_sum += particle.weight return weight_sum
def start_make_feed(self): """Starts the use-controlled simulation to generate a feed file. Binds user inputs and starts the UI loop. If the simulation object is not available, this function will log a fatal error. """ if not self._sim: log_error('could not start sim: Simulation object missing.', terminate=True) return self._main_window.bind('<KeyPress>', self._sim.key_press) self._main_window.bind('<Button>', self._sim.button_press) self._main_window.bind('<ButtonRelease>', self._sim.button_release) self._main_window.bind('<Motion>', self._sim.mouse_moved) save_button = Tk.Button(self._main_window, text='Save Logs', command=self._sim.save_logs) save_button.pack() self._update_make_feed() self._main_window.mainloop()
def __init__(self, feed_file_name, loop_feed=True, classifier_noise=0.0, motion_noise=0.0, ignore_regions=False): """Reads the given filename and tries to parse the classifier feed. Args: feed_file_name: the name of the file that contains all probabilities for each of the activities. Empty lines or lines starting with '#' will be ignored. All other lines should have a series of space-delimited probability values, one for each region class, in the same order as defined in the BuildingMap class. A line can also start with a '+', in which case the turn angle for the step associated with the probabilities of the line above it will be updated. loop_feed: (optional) set to False if this feed shouldn't loop around. Otherwise, when the data stream runs out, it will loop from the beginning. classifier_noise: added noise of the classifier between 0 and 1. A higher value means more random noise. motion_noise: the added noise of the odometry and turn rate. A higher value means more random noise. ignore_regions: set to true to ignore region probabilities (set them all to be equal) and only update based on motion data. This is useful for comparing with an odometry-and-turn-only method. """ self._probability_list = [] self._motions = [] self._ground_truths = [] self._loop_feed = loop_feed self._classifier_noise = classifier_noise self._motion_noise = motion_noise try: f = open(feed_file_name, 'r') for line in f: line = line.strip() # Skip empty or commented lines. if len(line) == 0 or line.startswith('#'): continue # If line begins with a '+', that means it's odometry and a turn angle, # so add those values to the previous position. if line.startswith('+'): line = line[1:] line = line.split() if len(line) >= 2: motion = int(line[0]), float(line[1]) if len(self._motions) > 0: self._motions[-1] = motion # If line begins with a '!', that means it's ground truth information, # so add that information to the previous position. elif line.startswith('!'): line = line[1:] line = line.split() if len(line) >= 3: ground_truth = int(line[0]), int(line[1]), float( line[2]) if len(self._ground_truths) > 0: self._ground_truths[-1] = ground_truth # Otherwise add the probabilities and initialize the motion and ground # truth values at that point to None. else: line = line.split() if ignore_regions: count = len(line) self._probability_list.append([1.0 / count] * count) else: self._probability_list.append(map(float, line)) self._motions.append(None) self._ground_truths.append(None) except: log_error('failed to load feed file: {}'.format(feed_file_name)) self._next_index = 0 self._num_feeds = len(self._probability_list)
def __init__(self, feed_file_name, loop_feed=True, classifier_noise=0.0, motion_noise=0.0, ignore_regions=False): """Reads the given filename and tries to parse the classifier feed. Args: feed_file_name: the name of the file that contains all probabilities for each of the activities. Empty lines or lines starting with '#' will be ignored. All other lines should have a series of space-delimited probability values, one for each region class, in the same order as defined in the BuildingMap class. A line can also start with a '+', in which case the turn angle for the step associated with the probabilities of the line above it will be updated. loop_feed: (optional) set to False if this feed shouldn't loop around. Otherwise, when the data stream runs out, it will loop from the beginning. classifier_noise: added noise of the classifier between 0 and 1. A higher value means more random noise. motion_noise: the added noise of the odometry and turn rate. A higher value means more random noise. ignore_regions: set to true to ignore region probabilities (set them all to be equal) and only update based on motion data. This is useful for comparing with an odometry-and-turn-only method. """ self._probability_list = [] self._motions = [] self._ground_truths = [] self._loop_feed = loop_feed self._classifier_noise = classifier_noise self._motion_noise = motion_noise try: f = open(feed_file_name, 'r') for line in f: line = line.strip() # Skip empty or commented lines. if len(line) == 0 or line.startswith('#'): continue # If line begins with a '+', that means it's odometry and a turn angle, # so add those values to the previous position. if line.startswith('+'): line = line[1:] line = line.split() if len(line) >= 2: motion = int(line[0]), float(line[1]) if len(self._motions) > 0: self._motions[-1] = motion # If line begins with a '!', that means it's ground truth information, # so add that information to the previous position. elif line.startswith('!'): line = line[1:] line = line.split() if len(line) >= 3: ground_truth = int(line[0]), int(line[1]), float(line[2]) if len(self._ground_truths) > 0: self._ground_truths[-1] = ground_truth # Otherwise add the probabilities and initialize the motion and ground # truth values at that point to None. else: line = line.split() if ignore_regions: count = len(line) self._probability_list.append([1.0 / count] * count) else: self._probability_list.append(map(float, line)) self._motions.append(None) self._ground_truths.append(None) except: log_error('failed to load feed file: {}'.format(feed_file_name)) self._next_index = 0 self._num_feeds = len(self._probability_list)