def fn(l):
# P(obs | s1=loc1, a=control_loc)
if l == loc:
return DDist({True: 1 - p_look_fn,
False: p_look_fn})
return DDist({True: p_look_fp,
False: 1 - p_look_fp})
def decompose(self):
if len(self.dist.support()) == 1:
return self.dist.support()
pose_dists = []
for surface_name in self.surface_dist.support():
dist = DDist({
pose: self.discrete_prob(pose)
for pose in self.poses_from_surface[surface_name]
})
weight = self.surface_prob(surface_name)
pose_dists.append(SurfaceDist(self, weight, dist))
return pose_dists
def update_dist(self, observation, obstacles=[], verbose=False):
# cfree_dist.conditionOnVar(index=1, has_detection=True)
if not BAYESIAN and (self.name in observation):
# TODO: convert into a Multivariate Gaussian
[detected_pose] = observation[self.name]
return DeltaDist(detected_pose)
if not self.world.cameras:
return self.dist.copy()
body = self.world.get_body(self.name)
all_poses = self.dist.support()
cfree_poses = all_poses
#cfree_poses = compute_cfree(body, all_poses, obstacles)
#cfree_dist = self.cfree_dist
cfree_dist = DDist(
{pose: self.dist.prob(pose)
for pose in cfree_poses})
# TODO: do these updates simultaneously for each object
# TODO: check all camera poses
[camera] = self.world.cameras.keys()
info = self.world.cameras[camera]
camera_pose = get_pose(info.body)
detectable_poses = compute_detectable(cfree_poses, camera_pose)
visible_poses = compute_visible(body,
detectable_poses,
camera_pose,
draw=False)
if verbose:
print(
'Total: {} | CFree: {} | Detectable: {} | Visible: {}'.format(
len(all_poses), len(cfree_poses), len(detectable_poses),
len(visible_poses)))
assert set(visible_poses) <= set(detectable_poses)
# obs_fn = get_observation_fn(surface)
#wait_for_user()
return self.bayesian_belief_update(cfree_dist,
visible_poses,
observation,
verbose=verbose)
def fn(s):
# P(obs | s1=loc1, a=control_loc)
if s == surface:
return DDist({True: 1 - p_look_fn, False: p_look_fn})
return DDist({True: p_look_fp, False: 1 - p_look_fp})
def fn(pose):
# P(detect | s in visible)
# This could depend on the position as well
if pose in visible:
return DDist({pose.support: 1. - p_fn, None: p_fn})
return DeltaDist(None)