def __init__(self,
robot_pose=(0, 0, 0),
visible=True,
default_color=cnames['yellow'],
element_dict={},
min_view_dist=0.3,
max_view_dist=1.0):
self.element_dict = element_dict
# Define nominal viewcone
self.min_view_dist = min_view_dist # [m]
self.max_view_dist = max_view_dist # [m]
self.view_angle = math.pi / 2 # [rad]
viewcone_pts = [
(0, 0),
(self.max_view_dist * math.cos(self.view_angle / 2),
self.max_view_dist * math.sin(self.view_angle / 2)),
(self.max_view_dist * math.cos(-self.view_angle / 2),
self.max_view_dist * math.sin(-self.view_angle / 2)),
(0, 0),
]
# Create SoftMax model for camera
self.detection_model = camera_model_2D(self.min_view_dist,
self.max_view_dist)
# Instantiate Sensor superclass object
update_rate = 1 # [hz]
has_physical_dimensions = True
super(Camera, self).__init__(update_rate, has_physical_dimensions)
# Set the ideal and actual viewcones
self.ideal_viewcone = MapObject(
'Ideal viewcone',
viewcone_pts,
visible=False,
blocks_camera=False,
color_str='pink',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.viewcone = MapObject(
'Viewcone',
viewcone_pts,
alpha=0.2,
visible=True,
blocks_camera=False,
color_str='lightyellow',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.view_pose = (0, 0, 0)
# <>TODO: Add in and test an offset of (-0.1,-0.1)
self.offset = (0, 0, 0) # [m] offset (x,y,theta) from center of robot
self._move_viewcone(robot_pose)
# Set up the VB fusion parameters
self.vb = VariationalBayes()
def __init__(self, robot_pose=(0, 0, 0), visible=True,
default_color=cnames['yellow'], element_dict={},
min_view_dist=0.3, max_view_dist=1.0):
self.element_dict = element_dict
# Define nominal viewcone
self.min_view_dist = min_view_dist # [m]
self.max_view_dist = max_view_dist # [m]
self.view_angle = math.pi / 2 # [rad]
viewcone_pts = [(0, 0),
(self.max_view_dist * math.cos(self.view_angle / 2),
self.max_view_dist * math.sin(self.view_angle / 2)),
(self.max_view_dist * math.cos(-self.view_angle / 2),
self.max_view_dist * math.sin(-self.view_angle / 2)),
(0, 0),
]
# Create SoftMax model for camera
self.detection_model = camera_model_2D(self.min_view_dist,
self.max_view_dist)
# Instantiate Sensor superclass object
update_rate = 1 # [hz]
has_physical_dimensions = True
super(Camera, self).__init__(update_rate, has_physical_dimensions)
# Set the ideal and actual viewcones
self.ideal_viewcone = MapObject('Ideal viewcone',
viewcone_pts,
visible=False,
blocks_camera=False,
color_str='pink',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.viewcone = MapObject('Viewcone',
viewcone_pts,
alpha=0.2,
visible=True,
blocks_camera=False,
color_str='lightyellow',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.view_pose = (0, 0, 0)
# <>TODO: Add in and test an offset of (-0.1,-0.1)
self.offset = (0, 0, 0) # [m] offset (x,y,theta) from center of robot
self._move_viewcone(robot_pose)
# Set up the VB fusion parameters
self.vb = VariationalBayes()
def camera_test(num_std=1, time_interval=1):
# prior = fleming_prior()
# prior = uniform_prior()
# prior = GaussianMixture(1, np.zeros(2), np.eye(2))
prior = GaussianMixture([1, 1, 1], np.array([
[-7, 0],
[-3, 0],
[1, 0],
]),
np.eye(2)[None, :].repeat(3, axis=0))
bounds = [-12.5, -3.5, 2.5, 3.5]
min_view_dist = 0.3 # [m]
max_view_dist = 1.0 # [m]
detection_model = camera_model_2D(min_view_dist, max_view_dist)
trajectory = np.zeros((20, 2))
ls = np.linspace(-10, 3, 20)
trajectory = np.hstack((ls[:, None], trajectory))
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
gm = camera_tester(prior, detection_model, trajectory, num_std, bounds)
logging.info('Initial GM:')
logging.info(prior)
ani = animation.FuncAnimation(gm.fig,
gm.update,
interval=time_interval,
repeat=True,
blit=False,
init_func=gm.plot_setup)
plt.show()
