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 clear_probability_from_objects(self, target, obj):
        logging.info('Clearing probability from {}'.format(obj.name))
        fusion_engine = self.robot.fusion_engine
        vb = VariationalBayes()

        if not hasattr(obj, 'relations'):
            obj.define_relations()

        if target is not None:
            prior = fusion_engine.filters[target].probability
            likelihood = obj.relations.binary_models['Inside']
            mu, sigma, beta = vb.update(
                measurement='Not Inside',
                likelihood=likelihood,
                prior=prior,
                use_LWIS=False,
            )
            gm = GaussianMixture(beta, mu, sigma)
            fusion_engine.filters[target].probability = gm
        else:
            # Update all but combined filters
            for name, filter_ in fusion_engine.filters.iteritems():
                if name == 'combined':
                    pass
                else:
                    prior = filter_.probability
                    likelihood = obj.relations.binary_models['Inside']
                    mu, sigma, beta = vb.update(
                        measurement='Not Inside',
                        likelihood=likelihood,
                        prior=prior,
                        use_LWIS=False,
                    )
                    gm = GaussianMixture(beta, mu, sigma)
                    filter_.probability = gm
Exemple #3
0
    def clear_probability_from_objects(self, target, obj):
        logging.info('Clearing probability from {}'.format(obj.name))
        fusion_engine = self.robot.fusion_engine
        vb = VariationalBayes()

        if not hasattr(obj, 'relations'):
            obj.define_relations()

        if target is not None:
            prior = fusion_engine.filters[target].probability
            likelihood = obj.relations.binary_models['Inside']
            mu, sigma, beta = vb.update(measurement='Not Inside',
                                        likelihood=likelihood,
                                        prior=prior,
                                        use_LWIS=False,
                                        )
            gm = GaussianMixture(beta, mu, sigma)
            fusion_engine.filters[target].probability = gm
        else:
            # Update all but combined filters
            for name, filter_ in fusion_engine.filters.iteritems():
                if name == 'combined':
                    pass
                else:
                    prior = filter_.probability
                    likelihood = obj.relations.binary_models['Inside']
                    mu, sigma, beta = vb.update(measurement='Not Inside',
                                                likelihood=likelihood,
                                                prior=prior,
                                                use_LWIS=False,
                                                )
                    gm = GaussianMixture(beta, mu, sigma)
                    filter_.probability = gm
Exemple #4
0
    def __init__(self,
                 map_=None,
                 web_interface_topic='python',
                 false_alarm_prob=0.2):
        self.update_rate = None
        self.has_physical_dimensions = False
        self.speed_model = speed_model()
        self.false_alarm_prob = false_alarm_prob

        super(Human, self).__init__(self.update_rate,
                                    self.has_physical_dimensions)

        # self.certainties = ['think', 'know']
        self.certainties = ['know']
        self.positivities = ['is not',
                             'is']  # <>TODO: oh god wtf why does order matter
        self.relations = {
            'object': [
                'behind',
                'in front of',
                'left of',
                'right of',
                'near',
            ],
            'area': ['inside', 'near', 'outside']
        }
        self.movement_types = ['moving', 'stopped']
        self.movement_qualities = ['slowly', 'moderately', 'quickly']

        self.groundings = {}
        self.groundings['area'] = map_.areas

        self.groundings['object'] = {}
        for cop_name, cop in map_.cops.iteritems():
            # if cop.has_relations:
            self.groundings['object'][cop_name] = cop
        for object_name, obj in map_.objects.iteritems():
            if obj.has_relations:
                self.groundings['object'][object_name] = obj

        self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
        self.utterance = ''
        self.new_update = False

        # Set up the VB fusion parameters
        self.vb = VariationalBayes()

        if web_interface_topic != 'python':
            # Subscribe to web interface
            print web_interface_topic
            import rospy
            from std_msgs.msg import String
            rospy.Subscriber(web_interface_topic, String, self.callback)
Exemple #5
0
    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()
Exemple #6
0
    def __init__(self, map_=None, web_interface_topic='python',
                 false_alarm_prob=0.2):
        self.update_rate = None
        self.has_physical_dimensions = False
        self.speed_model = speed_model()
        self.false_alarm_prob = false_alarm_prob

        super(Human, self).__init__(self.update_rate,
                                    self.has_physical_dimensions)

        # self.certainties = ['think', 'know']
        self.certainties = ['know']
        self.positivities = ['is not', 'is']  # <>TODO: oh god wtf why does order matter
        self.relations = {'object': ['behind',
                                     'in front of',
                                     'left of',
                                     'right of',
                                     'near',
                                     ],
                          'area': ['inside',
                                   'near',
                                   'outside'
                                   ]}
        self.movement_types = ['moving', 'stopped']
        self.movement_qualities = ['slowly', 'moderately', 'quickly']

        self.groundings = {}
        self.groundings['area'] = map_.areas

        self.groundings['object'] = {}
        for cop_name, cop in map_.cops.iteritems():
            # if cop.has_relations:
            self.groundings['object'][cop_name] = cop
        for object_name, obj in map_.objects.iteritems():
            if obj.has_relations:
                self.groundings['object'][object_name] = obj

        self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
        self.utterance = ''
        self.new_update = False

        # Set up the VB fusion parameters
        self.vb = VariationalBayes()

        if web_interface_topic != 'python':
            # Subscribe to web interface
            print web_interface_topic
            import rospy
            from std_msgs.msg import String
            rospy.Subscriber(web_interface_topic, String, self.callback)
Exemple #7
0
class Human(Sensor):
    """The human sensor, able to provide updates to a fusion engine.

    .. image:: img/classes_Human.png

    Parameters
    ----------
    shape_layer : ShapeLayer
        A layer object providing all the shapes in the map so that the human
        sensor can ground its statements.
    robber_names : list of str
        The list of all robbers, for the human to specify targets.
    detection_chance : float
        The human sensor's ability to correctly name a target, given that
        the target is within the human sensor's view -- that is, P(D=i|x).

        Note that this is the detection chance related to the "I think"
        statement, which can be increased if the human says "I know" instead.

    """
    def __init__(self, map_=None, web_interface_topic='python',
                 false_alarm_prob=0.2):
        self.update_rate = None
        self.has_physical_dimensions = False
        self.speed_model = speed_model()
        self.false_alarm_prob = false_alarm_prob

        super(Human, self).__init__(self.update_rate,
                                    self.has_physical_dimensions)

        # self.certainties = ['think', 'know']
        self.certainties = ['know']
        self.positivities = ['is not', 'is']  # <>TODO: oh god wtf why does order matter
        self.relations = {'object': ['behind',
                                     'in front of',
                                     'left of',
                                     'right of',
                                     'near',
                                     ],
                          'area': ['inside',
                                   'near',
                                   'outside'
                                   ]}
        self.movement_types = ['moving', 'stopped']
        self.movement_qualities = ['slowly', 'moderately', 'quickly']

        self.groundings = {}
        self.groundings['area'] = map_.areas

        self.groundings['object'] = {}
        for cop_name, cop in map_.cops.iteritems():
            # if cop.has_relations:
            self.groundings['object'][cop_name] = cop
        for object_name, obj in map_.objects.iteritems():
            if obj.has_relations:
                self.groundings['object'][object_name] = obj

        self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
        self.utterance = ''
        self.new_update = False

        # Set up the VB fusion parameters
        self.vb = VariationalBayes()

        if web_interface_topic != 'python':
            # Subscribe to web interface
            print web_interface_topic
            import rospy
            from std_msgs.msg import String
            rospy.Subscriber(web_interface_topic, String, self.callback)

    def callback(self, msg):
        logging.info('I heard {}'.format(msg.data))
        self.utterance = msg.data
        self.new_update = True

    def detect(self, filter_name, type_="particle", particles=None, prior=None):
        """Update a fusion engine's probability from human sensor updates.

        Parameters
        ----------
        particles : array_like
            The particle list, assuming [x,y,p], where x and y are position
            data and p is the particle's associated probability. `None` if not
            using particles.
        """

        if not self.parse_utterance():
            logging.debug('No utterance to parse!')
            return

        # End detect loop if not the right target
        if self.target_name not in ['nothing', 'a robot', filter_name]:
            logging.debug('Target {} is not in {} Looking for {}.'
                .format(filter_name, self.utterance, self.target_name))
            return

        if self.relation != '':
            self.translate_relation()
            self.detection_type = 'position'
        elif self.movement_type != '':
            self.translate_movement()
            self.detection_type = 'movement'
        else:
            logging.error("No relations or movements found in utterance.")

        if type_ == 'particle':
            self.detect_particles(particles)
            return True
        elif type_ == 'gauss sum':
            return self.detect_probability(prior)
        else:
            logging.error('Wrong detection model specified.')

    def parse_utterance(self):
        """ Parse the input string into workable values.
        """
        logging.debug('Parsing: {}'.format(self.utterance))
        logging.debug('Groundings: {}'.format(self.groundings))

        for str_ in self.certainties:
            if str_ in self.utterance:
                self.certainty = str_
                break
        else:
            self.certainty = ''

        for str_ in self.target_names:
            if str_ in self.utterance:
                self.target_name = str_
                break
        else:
            self.target_name = ''

        for str_ in self.positivities:
            if str_ in self.utterance:
                self.positivity = str_
                break
        else:
            self.positivity = ''

        for str_type in self.relations:
            for str_ in self.relations[str_type]:
                if str_ in self.utterance:
                    self.relation = str_
                    break
            else:
                continue
            break
        else:
            self.relation = ''

        for str_type in self.groundings:
            for str_ in self.groundings[str_type].keys():
                str_ = str_.lower()
                if str_ in self.utterance.lower():
                    str_ = str_.title()
                    self.grounding = self.groundings[str_type][str_]
                    break
            else:
                continue
            break
        else:
            self.grounding = None

        logging.debug('Utterance: {}'.format(self.utterance))
        logging.debug('Grounding: {}'.format(self.grounding))

        for str_ in self.movement_types:
            if str_ in self.utterance:
                self.movement_type = str_

                logging.info(str_)

                break
        else:
            self.movement_type = ''

        for str_ in self.movement_qualities:
            if str_ in self.utterance:
                self.movement_quality = str_

                logging.info(str_)

                break
        else:
            self.movement_quality = ''

        if self.utterance == '':
            utterance_is_well_formed = False
        else:
            utterance_is_well_formed = True
        return utterance_is_well_formed

    def translate_relation(self, relation_type='intrinsic'):
        """Translate the uttered relation to a likelihood label.
        """
        if relation_type == 'intrinsic':
            # Translate relation to zone label
            if self.relation == 'behind':
                translated_relation = 'Back'
            elif self.relation == 'in front of':
                translated_relation = 'Front'
            elif self.relation == 'left of':
                translated_relation = 'Left'
            elif self.relation == 'right of':
                translated_relation = 'Right'
            elif self.relation in ['inside', 'near', 'outside']:
                translated_relation = self.relation.title()
            self.relation = translated_relation
        elif relation_type == 'relative':
            pass  # <>TODO: Implement relative relations

    def translate_movement(self):
        """Translate the uttered movement to a likelihood label.

        """
        # Translate movement to motion model
        if self.movement_type == 'stopped':
            translated_movement = 'stopped'
        elif self.movement_quality == 'slowly':
            translated_movement = 'moving slowly'
        elif self.movement_quality == 'moderately':
            translated_movement = 'moving moderately'
        elif self.movement_quality == 'quickly':
            translated_movement = 'moving quickly'
        self.movement = translated_movement

    def detect_particles(self, particles):
        """Update particles based on sensor model.

        Parameters
        ----------
        particles : array_like
            The particle list, assuming [p,x,y,x_dot,y_dot], where x and y are
            position data and p is the particle's associated probability.

        """

        if not hasattr(self.grounding, 'relations'):
            self.grounding.define_relations()

        # <>TODO: include certainty
        if self.detection_type == 'position':
            label = self.relation
            if self.target_name == 'nothing' and self.positivity == 'not':
                label = 'a robot'
            elif self.target_name == 'nothing' or self.positivity == 'not':
                label = 'Not ' + label

            for i, particle in enumerate(particles):
                state = particle[1:3]
                particle[0] *= self.grounding.relations.probability(state=state, class_=label)

        elif self.detection_type == 'movement':
            label = self.movement
            if self.target_name == 'nothing' and self.positivity == 'not':
                label = 'a robot'
            elif self.target_name == 'nothing' or self.positivity == 'not':
                label = 'Not ' + label

            for i, particle in enumerate(particles):
                state = np.sqrt(particle[3] ** 2 + particle[4] ** 2)
                particle[0] *= self.speed_model.probability(state=state, class_=label)

        # Renormalize
        particles[:, 0] /= sum(particles[:, 0])

    def detect_probability(self, prior):
        if not hasattr(self.grounding, 'relations') \
            or self.grounding.name.lower() == 'deckard':
            logging.info("Defining relations because {} didn't have any."
                         .format(self.grounding.name))
            self.grounding.define_relations()

        if self.grounding.name.lower() == 'deckard':
            if self.relation == 'Right':
                self.relation = 'Left'
            elif self.relation == 'Left':
                self.relation = 'Right'

        # Position update
        label = self.relation
        if self.target_name == 'nothing' and self.positivity == 'is not':
            label = 'a robot'
        elif self.target_name == 'nothing' or self.positivity == 'is not':
            label = 'Not ' + label

        likelihood = self.grounding.relations.binary_models[self.relation]
        mu, sigma, beta = self.vb.update(measurement=label,
                                         likelihood=likelihood,
                                         prior=prior,
                                         use_LWIS=False,
                                         )

        gm = GaussianMixture(beta, mu, sigma)
        alpha = self.false_alarm_prob / 2
        posterior = prior.combine_gms(gm, alpha)
        # Weight based on human possibly being wrong
        return posterior
Exemple #8
0
class Camera(Sensor):
    """A conic sensor mounted on a robot.

    The camera provides a viewcone from the point of view of the robot which
    rescales based on its environment (e.g. if it's in front of a wall).

    .. image:: img/classes_Camera.png

    Parameters
    ----------
    robot_pose : array_like, optional
        The cop's initial [x, y, theta] (defaults to [0, 0, 0]).
    visible : bool, optional
        Whether or not the view cone is shown. Default is True.
    default_color : cnames
        Default color to display all camera sensors as. Defaults to yellow.

    """
    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 update_viewcone(self, robot_pose):
        """Update the camera's viewcone position and scale.

        Parameters
        ----------
        robot_pose : array_like, optional
            The robot's currentl [x, y, theta].
        shape_layer : ShapeLayer
            A layer object providing all the shapes in the map for the camera
            to rescale its viewcone.
        """
        self._move_viewcone(robot_pose)
        self._rescale_viewcone(robot_pose)

        # Translate detection model
        self.detection_model.move(self.view_pose)

    def _move_viewcone(self, robot_pose):
        """Move the viewcone based on the robot's pose

        Parameters
        ----------
        robot_pose : array_like, optional
            The robot's currentl [x, y, theta].
        """
        pose = (robot_pose[0] + self.offset[0],
                robot_pose[1] + self.offset[1],
                robot_pose[2]
                )

        # Reset the view shape
        self.viewcone.shape = self.ideal_viewcone.shape
        transform = tuple(np.subtract(pose, self.view_pose))
        self.ideal_viewcone.move_relative(transform,
                                          rotation_pt=self.view_pose[0:2])
        self.viewcone.move_relative(transform, rotation_pt=self.view_pose[0:2])
        self.view_pose = pose

    def _rescale_viewcone(self, robot_pose):
        """Rescale the viewcone based on intersecting map objects.

        Parameters
        ----------
        robot_pose : array_like, optional
            The robot's currentl [x, y, theta].
        """
        scale = self.max_view_dist

        blocking_shapes = []
        possible_elements = []

        try:
            possible_elements += self.element_dict['static']
        except:
            logging.debug('No static elements')
        try:
            possible_elements += self.element_dict['dynamic']
        except:
            logging.debug('No dynamic elements')

        for element in possible_elements:
            if element.blocks_camera:
                blocking_shapes.append(element.shape)

        for shape in blocking_shapes:
            if self.viewcone.shape.intersects(shape):
                # <>TODO: Use shadows instead of rescaling viewcone
                # calculate shadows for all shapes touching viewcone
                # origin = self.viewcone.project(map_object.shape)
                # shadow = affinity.scale(...) #map portion invisible to the view
                # self.viewcone = self.viewcone.difference(shadow)

                distance = Point(self.view_pose[0:2]).distance(shape)
                scale_ = distance / self.max_view_dist * 1.3  # <>TODO: why the 1.3?
                if scale_ < scale:
                    scale = scale_

        self.viewcone.shape = affinity.scale(self.ideal_viewcone.shape,
                                             xfact=scale,
                                             yfact=scale,
                                             origin=self.view_pose[0:2])
        # else:
        #     self.viewcone.shape = self.ideal_viewcone.shape

    def detect(self, filter_type, particles=None, prior=None):
        """Update a fusion engine's probability from camera detections.

        Parameters
        ----------
        filter_type : {'particle','gauss sum'}
            The type of filter to update.
        particles : array_like, optional
            The particle list, assuming [x,y,p], where x and y are position
            data and p is the particle's associated probability.

        """
        if filter_type == 'particle':
            self._detect_particles(particles)
        else:
            posterior = self._detect_probability(prior)
            return posterior

    def _detect_particles(self, particles):
        """Update particles based on sensor model.

        particles : array_like
            The particle list, assuming [x,y,p], where x and y are position
            data and p is the particle's associated probability.
        """

        # Update particle probabilities in view cone frame
        for i, particle in enumerate(particles):
            if self.viewcone.shape.contains(Point(particle[1:3])):
                particles[i, 0] *= (1 - self.detection_model
                    .probability(state=particle[1:3], class_='Detection'))

        # Renormalize
        particles[:, 0] /= sum(particles[:, 0])

    def _detect_probability(self, prior):

        mu, sigma, beta = self.vb.update(measurement='No Detection',
                                         likelihood=self.detection_model,
                                         prior=prior,
                                         use_LWIS=True,
                                         poly=self.detection_model.poly
                                         )
        gm = GaussianMixture(beta, mu, sigma)
        gm.camera_viewcone = self.detection_model.poly  # for plotting
        return gm
class Camera(Sensor):
    """A conic sensor mounted on a robot.

    The camera provides a viewcone from the point of view of the robot which
    rescales based on its environment (e.g. if it's in front of a wall).

    .. image:: img/classes_Camera.png

    Parameters
    ----------
    robot_pose : array_like, optional
        The cop's initial [x, y, theta] (defaults to [0, 0, 0]).
    visible : bool, optional
        Whether or not the view cone is shown. Default is True.
    default_color : cnames
        Default color to display all camera sensors as. Defaults to yellow.

    """
    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 update_viewcone(self, robot_pose):
        """Update the camera's viewcone position and scale.

        Parameters
        ----------
        robot_pose : array_like, optional
            The robot's currentl [x, y, theta].
        shape_layer : ShapeLayer
            A layer object providing all the shapes in the map for the camera
            to rescale its viewcone.
        """
        self._move_viewcone(robot_pose)
        self._rescale_viewcone(robot_pose)

        # Translate detection model
        self.detection_model.move(self.view_pose)

    def _move_viewcone(self, robot_pose):
        """Move the viewcone based on the robot's pose

        Parameters
        ----------
        robot_pose : array_like, optional
            The robot's currentl [x, y, theta].
        """
        pose = (robot_pose[0] + self.offset[0], robot_pose[1] + self.offset[1],
                robot_pose[2])

        # Reset the view shape
        self.viewcone.shape = self.ideal_viewcone.shape
        transform = tuple(np.subtract(pose, self.view_pose))
        self.ideal_viewcone.move_relative(transform,
                                          rotation_pt=self.view_pose[0:2])
        self.viewcone.move_relative(transform, rotation_pt=self.view_pose[0:2])
        self.view_pose = pose

    def _rescale_viewcone(self, robot_pose):
        """Rescale the viewcone based on intersecting map objects.

        Parameters
        ----------
        robot_pose : array_like, optional
            The robot's currentl [x, y, theta].
        """
        scale = self.max_view_dist

        blocking_shapes = []
        possible_elements = []

        try:
            possible_elements += self.element_dict['static']
        except:
            logging.debug('No static elements')
        try:
            possible_elements += self.element_dict['dynamic']
        except:
            logging.debug('No dynamic elements')

        for element in possible_elements:
            if element.blocks_camera:
                blocking_shapes.append(element.shape)

        for shape in blocking_shapes:
            if self.viewcone.shape.intersects(shape):
                # <>TODO: Use shadows instead of rescaling viewcone
                # calculate shadows for all shapes touching viewcone
                # origin = self.viewcone.project(map_object.shape)
                # shadow = affinity.scale(...) #map portion invisible to the view
                # self.viewcone = self.viewcone.difference(shadow)

                distance = Point(self.view_pose[0:2]).distance(shape)
                scale_ = distance / self.max_view_dist * 1.3  # <>TODO: why the 1.3?
                if scale_ < scale:
                    scale = scale_

        self.viewcone.shape = affinity.scale(self.ideal_viewcone.shape,
                                             xfact=scale,
                                             yfact=scale,
                                             origin=self.view_pose[0:2])
        # else:
        #     self.viewcone.shape = self.ideal_viewcone.shape

    def detect(self, filter_type, particles=None, prior=None):
        """Update a fusion engine's probability from camera detections.

        Parameters
        ----------
        filter_type : {'particle','gauss sum'}
            The type of filter to update.
        particles : array_like, optional
            The particle list, assuming [x,y,p], where x and y are position
            data and p is the particle's associated probability.

        """
        if filter_type == 'particle':
            self._detect_particles(particles)
        else:
            posterior = self._detect_probability(prior)
            return posterior

    def _detect_particles(self, particles):
        """Update particles based on sensor model.

        particles : array_like
            The particle list, assuming [x,y,p], where x and y are position
            data and p is the particle's associated probability.
        """

        # Update particle probabilities in view cone frame
        for i, particle in enumerate(particles):
            if self.viewcone.shape.contains(Point(particle[1:3])):
                particles[i, 0] *= (1 - self.detection_model.probability(
                    state=particle[1:3], class_='Detection'))

        # Renormalize
        particles[:, 0] /= sum(particles[:, 0])

    def _detect_probability(self, prior):

        mu, sigma, beta = self.vb.update(measurement='No Detection',
                                         likelihood=self.detection_model,
                                         prior=prior,
                                         use_LWIS=True,
                                         poly=self.detection_model.poly)
        gm = GaussianMixture(beta, mu, sigma)
        gm.camera_viewcone = self.detection_model.poly  # for plotting
        return gm
Exemple #10
0
class Human(Sensor):
    """The human sensor, able to provide updates to a fusion engine.

    .. image:: img/classes_Human.png

    Parameters
    ----------
    shape_layer : ShapeLayer
        A layer object providing all the shapes in the map so that the human
        sensor can ground its statements.
    robber_names : list of str
        The list of all robbers, for the human to specify targets.
    detection_chance : float
        The human sensor's ability to correctly name a target, given that
        the target is within the human sensor's view -- that is, P(D=i|x).

        Note that this is the detection chance related to the "I think"
        statement, which can be increased if the human says "I know" instead.

    """
    def __init__(self,
                 map_=None,
                 web_interface_topic='python',
                 false_alarm_prob=0.2):
        self.update_rate = None
        self.has_physical_dimensions = False
        self.speed_model = speed_model()
        self.false_alarm_prob = false_alarm_prob

        super(Human, self).__init__(self.update_rate,
                                    self.has_physical_dimensions)

        # self.certainties = ['think', 'know']
        self.certainties = ['know']
        self.positivities = ['is not',
                             'is']  # <>TODO: oh god wtf why does order matter
        self.relations = {
            'object': [
                'behind',
                'in front of',
                'left of',
                'right of',
                'near',
            ],
            'area': ['inside', 'near', 'outside']
        }
        self.movement_types = ['moving', 'stopped']
        self.movement_qualities = ['slowly', 'moderately', 'quickly']

        self.groundings = {}
        self.groundings['area'] = map_.areas

        self.groundings['object'] = {}
        for cop_name, cop in map_.cops.iteritems():
            # if cop.has_relations:
            self.groundings['object'][cop_name] = cop
        for object_name, obj in map_.objects.iteritems():
            if obj.has_relations:
                self.groundings['object'][object_name] = obj

        self.target_names = ['nothing', 'a robot'] + map_.robbers.keys()
        self.utterance = ''
        self.new_update = False

        # Set up the VB fusion parameters
        self.vb = VariationalBayes()

        if web_interface_topic != 'python':
            # Subscribe to web interface
            print web_interface_topic
            import rospy
            from std_msgs.msg import String
            rospy.Subscriber(web_interface_topic, String, self.callback)

    def callback(self, msg):
        logging.info('I heard {}'.format(msg.data))
        self.utterance = msg.data
        self.new_update = True

    def detect(self,
               filter_name,
               type_="particle",
               particles=None,
               prior=None):
        """Update a fusion engine's probability from human sensor updates.

        Parameters
        ----------
        particles : array_like
            The particle list, assuming [x,y,p], where x and y are position
            data and p is the particle's associated probability. `None` if not
            using particles.
        """

        if not self.parse_utterance():
            logging.debug('No utterance to parse!')
            return

        # End detect loop if not the right target
        if self.target_name not in ['nothing', 'a robot', filter_name]:
            logging.debug('Target {} is not in {} Looking for {}.'.format(
                filter_name, self.utterance, self.target_name))
            return

        if self.relation != '':
            self.translate_relation()
            self.detection_type = 'position'
        elif self.movement_type != '':
            self.translate_movement()
            self.detection_type = 'movement'
        else:
            logging.error("No relations or movements found in utterance.")

        if type_ == 'particle':
            self.detect_particles(particles)
            return True
        elif type_ == 'gauss sum':
            return self.detect_probability(prior)
        else:
            logging.error('Wrong detection model specified.')

    def parse_utterance(self):
        """ Parse the input string into workable values.
        """
        logging.debug('Parsing: {}'.format(self.utterance))
        logging.debug('Groundings: {}'.format(self.groundings))

        for str_ in self.certainties:
            if str_ in self.utterance:
                self.certainty = str_
                break
        else:
            self.certainty = ''

        for str_ in self.target_names:
            if str_ in self.utterance:
                self.target_name = str_
                break
        else:
            self.target_name = ''

        for str_ in self.positivities:
            if str_ in self.utterance:
                self.positivity = str_
                break
        else:
            self.positivity = ''

        for str_type in self.relations:
            for str_ in self.relations[str_type]:
                if str_ in self.utterance:
                    self.relation = str_
                    break
            else:
                continue
            break
        else:
            self.relation = ''

        for str_type in self.groundings:
            for str_ in self.groundings[str_type].keys():
                str_ = str_.lower()
                if str_ in self.utterance.lower():
                    str_ = str_.title()
                    self.grounding = self.groundings[str_type][str_]
                    break
            else:
                continue
            break
        else:
            self.grounding = None

        logging.debug('Utterance: {}'.format(self.utterance))
        logging.debug('Grounding: {}'.format(self.grounding))

        for str_ in self.movement_types:
            if str_ in self.utterance:
                self.movement_type = str_

                logging.info(str_)

                break
        else:
            self.movement_type = ''

        for str_ in self.movement_qualities:
            if str_ in self.utterance:
                self.movement_quality = str_

                logging.info(str_)

                break
        else:
            self.movement_quality = ''

        if self.utterance == '':
            utterance_is_well_formed = False
        else:
            utterance_is_well_formed = True
        return utterance_is_well_formed

    def translate_relation(self, relation_type='intrinsic'):
        """Translate the uttered relation to a likelihood label.
        """
        if relation_type == 'intrinsic':
            # Translate relation to zone label
            if self.relation == 'behind':
                translated_relation = 'Back'
            elif self.relation == 'in front of':
                translated_relation = 'Front'
            elif self.relation == 'left of':
                translated_relation = 'Left'
            elif self.relation == 'right of':
                translated_relation = 'Right'
            elif self.relation in ['inside', 'near', 'outside']:
                translated_relation = self.relation.title()
            self.relation = translated_relation
        elif relation_type == 'relative':
            pass  # <>TODO: Implement relative relations

    def translate_movement(self):
        """Translate the uttered movement to a likelihood label.

        """
        # Translate movement to motion model
        if self.movement_type == 'stopped':
            translated_movement = 'stopped'
        elif self.movement_quality == 'slowly':
            translated_movement = 'moving slowly'
        elif self.movement_quality == 'moderately':
            translated_movement = 'moving moderately'
        elif self.movement_quality == 'quickly':
            translated_movement = 'moving quickly'
        self.movement = translated_movement

    def detect_particles(self, particles):
        """Update particles based on sensor model.

        Parameters
        ----------
        particles : array_like
            The particle list, assuming [p,x,y,x_dot,y_dot], where x and y are
            position data and p is the particle's associated probability.

        """

        if not hasattr(self.grounding, 'relations'):
            self.grounding.define_relations()

        # <>TODO: include certainty
        if self.detection_type == 'position':
            label = self.relation
            if self.target_name == 'nothing' and self.positivity == 'not':
                label = 'a robot'
            elif self.target_name == 'nothing' or self.positivity == 'not':
                label = 'Not ' + label

            for i, particle in enumerate(particles):
                state = particle[1:3]
                particle[0] *= self.grounding.relations.probability(
                    state=state, class_=label)

        elif self.detection_type == 'movement':
            label = self.movement
            if self.target_name == 'nothing' and self.positivity == 'not':
                label = 'a robot'
            elif self.target_name == 'nothing' or self.positivity == 'not':
                label = 'Not ' + label

            for i, particle in enumerate(particles):
                state = np.sqrt(particle[3]**2 + particle[4]**2)
                particle[0] *= self.speed_model.probability(state=state,
                                                            class_=label)

        # Renormalize
        particles[:, 0] /= sum(particles[:, 0])

    def detect_probability(self, prior):
        if not hasattr(self.grounding, 'relations') \
            or self.grounding.name.lower() == 'deckard':
            logging.info(
                "Defining relations because {} didn't have any.".format(
                    self.grounding.name))
            self.grounding.define_relations()

        if self.grounding.name.lower() == 'deckard':
            if self.relation == 'Right':
                self.relation = 'Left'
            elif self.relation == 'Left':
                self.relation = 'Right'

        # Position update
        label = self.relation
        if self.target_name == 'nothing' and self.positivity == 'is not':
            label = 'a robot'
        elif self.target_name == 'nothing' or self.positivity == 'is not':
            label = 'Not ' + label

        likelihood = self.grounding.relations.binary_models[self.relation]
        mu, sigma, beta = self.vb.update(
            measurement=label,
            likelihood=likelihood,
            prior=prior,
            use_LWIS=False,
        )

        gm = GaussianMixture(beta, mu, sigma)
        alpha = self.false_alarm_prob / 2
        posterior = prior.combine_gms(gm, alpha)
        # Weight based on human possibly being wrong
        return posterior