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 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 __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 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)
