class camera_tester(object):
"""docstring for merged_gm"""
def __init__(self,
prior,
detection_model,
trajectory,
num_std=1,
bounds=None):
self.fig = plt.figure(figsize=(16, 8))
self.gm = prior
self.detection_model = detection_model
self.trajectory = itertools.cycle(trajectory)
self.vb = VariationalBayes()
self.num_std = num_std
if bounds is None:
self.bounds = [-5, -5, 5, 5]
else:
self.bounds = bounds
def update(self, i=0):
self.camera_pose = next(self.trajectory)
logging.info('Moving to pose {}.'.format(self.camera_pose))
self.detection_model.move(self.camera_pose)
# Do a VBIS update
mu, sigma, beta = self.vb.update(measurement='No Detection',
likelihood=detection_model,
prior=self.gm,
use_LWIS=True,
poly=detection_model.poly,
num_std=self.num_std)
self.gm = GaussianMixture(weights=beta,
means=mu,
covariances=sigma)
# Log what's going on
logging.info(self.gm)
logging.info('Weight sum: {}'.format(beta.sum()))
self.remove()
self.plot()
def plot(self):
levels_res = 50
self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)),
levels_res)
self.contourf = self.ax.contourf(self.xx,
self.yy,
self.gm.pdf(self.pos),
levels=self.levels,
cmap=plt.get_cmap('jet'))
# Plot camera
self.cam_patch = PolygonPatch(self.detection_model.poly,
facecolor='none',
linewidth=2,
edgecolor='white')
self.ax.add_patch(self.cam_patch)
# Plot ellipses
self.ellipse_patches = self.gm.plot_ellipses(
poly=self.detection_model.poly)
def plot_setup(self):
# Define gridded space for graphing
min_x, max_x = self.bounds[0], self.bounds[2]
min_y, max_y = self.bounds[1], self.bounds[3]
res = 30
self.xx, self.yy = np.mgrid[min_x:max_x:1 / res,
min_y:max_y:1 / res]
pos = np.empty(self.xx.shape + (2, ))
pos[:, :, 0] = self.xx
pos[:, :, 1] = self.yy
self.pos = pos
# Plot setup
self.ax = self.fig.add_subplot(111)
self.ax.set_title('VBIS with camera detection test')
plt.axis('scaled')
self.ax.set_xlim([min_x, max_x])
self.ax.set_ylim([min_y, max_y])
levels_res = 50
self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)),
levels_res)
cax = self.contourf = self.ax.contourf(self.xx,
self.yy,
self.gm.pdf(self.pos),
levels=self.levels,
cmap=plt.get_cmap('jet'))
self.fig.colorbar(cax)
def remove(self):
if hasattr(self, 'cam_patch'):
self.cam_patch.remove()
del self.cam_patch
if hasattr(self, 'ellipse_patches'):
for patch in self.ellipse_patches:
patch.remove()
del self.ellipse_patches
if hasattr(self, 'contourf'):
for collection in self.contourf.collections:
collection.remove()
del self.contourf
class camera_tester(object):
"""docstring for merged_gm"""
def __init__(self, prior, detection_model, trajectory, num_std=1, bounds=None):
self.fig = plt.figure(figsize=(16,8))
self.gm = prior
self.detection_model = detection_model
self.trajectory = itertools.cycle(trajectory)
self.vb = VariationalBayes()
self.num_std = num_std
if bounds is None:
self.bounds = [-5, -5, 5, 5]
else:
self.bounds = bounds
def update(self,i=0):
self.camera_pose = next(self.trajectory)
logging.info('Moving to pose {}.'.format(self.camera_pose))
self.detection_model.move(self.camera_pose)
# Do a VBIS update
mu, sigma, beta = self.vb.update(measurement='No Detection',
likelihood=detection_model,
prior=self.gm,
use_LWIS=True,
poly=detection_model.poly,
num_std=self.num_std
)
self.gm = GaussianMixture(weights=beta, means=mu, covariances=sigma)
# Log what's going on
logging.info(self.gm)
logging.info('Weight sum: {}'.format(beta.sum()))
self.remove()
self.plot()
def plot(self):
levels_res = 50
self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)), levels_res)
self.contourf = self.ax.contourf(self.xx, self.yy,
self.gm.pdf(self.pos),
levels=self.levels,
cmap=plt.get_cmap('jet')
)
# Plot camera
self.cam_patch = PolygonPatch(self.detection_model.poly, facecolor='none',
linewidth=2, edgecolor='white')
self.ax.add_patch(self.cam_patch)
# Plot ellipses
self.ellipse_patches = self.gm.plot_ellipses(poly=self.detection_model.poly)
def plot_setup(self):
# Define gridded space for graphing
min_x, max_x = self.bounds[0], self.bounds[2]
min_y, max_y = self.bounds[1], self.bounds[3]
res = 30
self.xx, self.yy = np.mgrid[min_x:max_x:1/res,
min_y:max_y:1/res]
pos = np.empty(self.xx.shape + (2,))
pos[:, :, 0] = self.xx; pos[:, :, 1] = self.yy;
self.pos = pos
# Plot setup
self.ax = self.fig.add_subplot(111)
self.ax.set_title('VBIS with camera detection test')
plt.axis('scaled')
self.ax.set_xlim([min_x, max_x])
self.ax.set_ylim([min_y, max_y])
levels_res = 50
self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)), levels_res)
cax = self.contourf = self.ax.contourf(self.xx, self.yy,
self.gm.pdf(self.pos),
levels=self.levels,
cmap=plt.get_cmap('jet')
)
self.fig.colorbar(cax)
def remove(self):
if hasattr(self, 'cam_patch'):
self.cam_patch.remove()
del self.cam_patch
if hasattr(self, 'ellipse_patches'):
for patch in self.ellipse_patches:
patch.remove()
del self.ellipse_patches
if hasattr(self,'contourf'):
for collection in self.contourf.collections:
collection.remove()
del self.contourf
class FeasibleLayer(Layer):
"""A representation of feasible map regions.
A polygon (or collection of polygons) that represent feasible
regions of the map. Feasible can be defined as either feasible robot
poses or unoccupied space.
.. image:: img/classes_Feasible_Layer.png
Parameters
----------
max_robot_radius : float, optional
The maximum radius of a circular approximation to the robot, used
to determine the feasible pose regions.
**kwargs
Arguments passed to the ``Layer`` superclass.
"""
def __init__(self, max_robot_radius=0.20, **kwargs):
super(FeasibleLayer, self).__init__(**kwargs)
self.max_robot_radius = max_robot_radius # [m] conservative estimate
self.point_region = None
self.pose_region = None
# self.define_feasible_regions()
def define_feasible_regions(self, static_elements):
"""Generate the feasible regions from a dictionary of static map elements.
Parameters
----------
static_elements : dict
A dictionary of map elements
"""
# TODO: feasible space should depend on map bounds, not layer bounds
feasible_space = box(*self.bounds)
self.point_region = feasible_space
self.pose_region = feasible_space.buffer(-self.max_robot_radius)
for element in static_elements:
# Ignore MapAreas
if isinstance(element, MapObject):
self.point_region = self.point_region.difference(element.shape)
buffered_shape = element.shape.buffer(self.max_robot_radius)
self.pose_region = self.pose_region.difference(buffered_shape)
def plot(self, type_='pose', alpha=0.5, **kwargs):
"""Plot either the pose or point feasible regions.
Parameters
----------
type_ : {'pose','point'}
The type of feasible region to plot.
alpha : int
Feasible region patch transparency
**kwargs
Arguements passed to PolygonPatch
"""
if type_ == "pose":
p = self.pose_region
else:
p = self.point_region
ax.set_xlim([self.bounds[0], self.bounds[2]])
ax.set_ylim([self.bounds[1], self.bounds[3]])
self.patch = PolygonPatch(p,
facecolor=cnames['black'],
alpha=alpha,
zorder=2,
**kwargs)
ax.add_patch(self.patch)
plt.show()
# return patch
def remove(self):
"""Removes feasible region patch from plot"""
if hasattr(self, 'patch'):
self.patch.remove()
def update(self, type_='pose', i=0):
self.remove()
self.plot(type_)
class FeasibleLayer(Layer):
"""A representation of feasible map regions.
A polygon (or collection of polygons) that represent feasible
regions of the map. Feasible can be defined as either feasible robot
poses or unoccupied space.
.. image:: img/classes_Feasible_Layer.png
Parameters
----------
max_robot_radius : float, optional
The maximum radius of a circular approximation to the robot, used
to determine the feasible pose regions.
**kwargs
Arguments passed to the ``Layer`` superclass.
"""
def __init__(self, max_robot_radius=0.20, **kwargs):
super(FeasibleLayer, self).__init__(**kwargs)
self.max_robot_radius = max_robot_radius # [m] conservative estimate
self.point_region = None
self.pose_region = None
# self.define_feasible_regions()
def define_feasible_regions(self, static_elements):
"""Generate the feasible regions from a dictionary of static map elements.
Parameters
----------
static_elements : dict
A dictionary of map elements
"""
# TODO: feasible space should depend on map bounds, not layer bounds
feasible_space = box(*self.bounds)
self.point_region = feasible_space
self.pose_region = feasible_space.buffer(-self.max_robot_radius)
for element in static_elements:
# Ignore MapAreas
if isinstance(element, MapObject):
self.point_region = self.point_region.difference(element.shape)
buffered_shape = element.shape.buffer(self.max_robot_radius)
self.pose_region = self.pose_region.difference(buffered_shape)
def plot(self, type_='pose', alpha=0.5, **kwargs):
"""Plot either the pose or point feasible regions.
Parameters
----------
type_ : {'pose','point'}
The type of feasible region to plot.
alpha : int
Feasible region patch transparency
**kwargs
Arguements passed to PolygonPatch
"""
if type_ == "pose":
p = self.pose_region
else:
p = self.point_region
ax.set_xlim([self.bounds[0], self.bounds[2]])
ax.set_ylim([self.bounds[1], self.bounds[3]])
self.patch = PolygonPatch(p, facecolor=cnames['black'],
alpha=alpha, zorder=2, **kwargs)
ax.add_patch(self.patch)
plt.show()
# return patch
def remove(self):
"""Removes feasible region patch from plot"""
if hasattr(self, 'patch'):
self.patch.remove()
def update(self, type_='pose', i=0):
self.remove()
self.plot(type_)
