Merge pull request #199 from cburbridge/master

yianni · yianni · commit ef610fd179f3 · 2015-10-02T17:05:52.000+01:00
Add TPCC QSR.
diff --git a/qsr_lib/scripts/example_extended.py b/qsr_lib/scripts/example_extended.py
@@ -69,6 +69,17 @@ def pretty_print_world_qsr_trace(which_qsr, qsrlib_response_message):
         world.add_object_state_series(o2)
         world.add_object_state_series(o3)
 
+    elif which_qsr == "tpcc":
+        # Then we need three objects to do a test...
+        # Don't bother with more than one timestamp.
+        o1 = [Object_State(name="o1", timestamp=0, x=0., y=0., xsize=5., ysize=8.)]
+        o2 = [Object_State(name="o2", timestamp=0, x=5., y=0., xsize=5., ysize=8.)]
+        o3 = [Object_State(name="o3", timestamp=0, x=5., y=0., xsize=5., ysize=8.)]
+
+        world.add_object_state_series(o1)
+        world.add_object_state_series(o2)
+        world.add_object_state_series(o3)
+
     elif which_qsr == "rcc8" or which_qsr == "rcc5":
         # dynamic_args = {which_qsr: {"quantisation_factor": args.quantisation_factor}}
         o1 = [Object_State(name="o1", timestamp=0, x=1., y=1., xsize=5., ysize=8.),
@@ -315,6 +326,9 @@ def pretty_print_world_qsr_trace(which_qsr, qsrlib_response_message):
     # print(dynamic_args["for_all_qsrs"]["qsrs_for"])
     # # DBG: eof
 
+    # dynamic_args[which_qsr]["qsrs_for"] = [("o1", "o2", "o3"), ("o1", "o3")]
+    dynamic_args = {"tpcc": {"qsrs_for":  [("o1", "o2", "o3"), ("o1", "o3")]}}
+
     qsrlib_request_message = QSRlib_Request_Message(which_qsr=which_qsr, input_data=world, dynamic_args=dynamic_args)
 
     if args.ros:
diff --git a/qsr_lib/scripts/mwe.py b/qsr_lib/scripts/mwe.py
@@ -43,15 +43,20 @@ def pretty_print_world_qsr_trace(which_qsr, qsrlib_response_message):
           Object_State(name="o1", timestamp=2, x=3., y=1., xsize=5., ysize=8.),
           Object_State(name="o1", timestamp=3, x=6., y=1., xsize=5., ysize=8.),
           Object_State(name="o1", timestamp=4, x=6., y=1., xsize=5., ysize=8.)]
-
     o2 = [Object_State(name="o2", timestamp=0, x=1., y=1., xsize=5., ysize=8.),
           Object_State(name="o2", timestamp=1, x=2., y=1., xsize=5., ysize=8.),
           Object_State(name="o2", timestamp=2, x=1., y=1., xsize=5., ysize=8.),
           Object_State(name="o2", timestamp=3, x=1., y=1., xsize=5., ysize=8.),
           Object_State(name="o2", timestamp=4, x=1., y=2., xsize=5., ysize=8.)]
-          #Object_State(name="o2", timestamp=5, x=1., y=2., xsize=5., ysize=8.)]
+    o3 = [Object_State(name="o3", timestamp=0, x=5., y=2., xsize=5.2, ysize=8.5),
+          Object_State(name="o3", timestamp=1, x=6., y=4., xsize=5.2, ysize=8.5),
+          Object_State(name="o3", timestamp=2, x=2., y=4., xsize=5.2, ysize=8.5),
+          Object_State(name="o3", timestamp=3, x=1., y=4., xsize=5.2, ysize=8.5),
+          Object_State(name="o3", timestamp=4, x=0., y=4., xsize=5.2, ysize=8.5)]
+
     world.add_object_state_series(o1)
     world.add_object_state_series(o2)
+    world.add_object_state_series(o3)
 
     # ****************************************************************************************************
     dynammic_args = {"qsr_relations_and_values" : {"Touch": 0.5, "Near": 1, "Far": 3}}
diff --git a/qsr_lib/src/qsrlib_qsrs/__init__.py b/qsr_lib/src/qsrlib_qsrs/__init__.py
@@ -12,7 +12,7 @@
 from qsr_moving_or_stationary import QSR_Moving_or_Stationary
 from qsr_new_mwe import QSR_MWE
 from qsr_ra import QSR_RA
-
+from qsr_tpcc import QSR_TPCC
 # register new qsrs by class name below
 qsrs_registry = (QSR_RCC2,
                  QSR_RCC3_Rectangle_Bounding_Boxes_2D,
@@ -27,4 +27,5 @@
                  QSR_Arg_Prob_Relations_Distance,
                  QSR_Moving_or_Stationary,
                  QSR_MWE,
-                 QSR_RA)
+                 QSR_RA,
+                 QSR_TPCC)
diff --git a/qsr_lib/src/qsrlib_qsrs/qsr_tpcc.py b/qsr_lib/src/qsrlib_qsrs/qsr_tpcc.py
@@ -0,0 +1,58 @@
+# -*- coding: utf-8 -*-
+from __future__ import print_function, division
+from qsrlib_qsrs.qsr_triadic_abstractclass import QSR_Triadic_1t_Abstractclass
+import math
+
+class QSR_TPCC(QSR_Triadic_1t_Abstractclass):
+    """ TPCC QSRs.
+    .. seealso:: For further details about TPCC, see http://www.sfbtr8.spatial-cognition.de/project/r3/QualitativeCalculi/TPCC/index.html.
+    """
+    _unique_id = "tpcc"
+    _all_possible_relations = ('dlf', 'dfl', 'dsl', 'dbl', 'dlb', 'dsb', 'drb', 'dbr',
+                               'dsr', 'dfr', 'drf', 'dsf', 'clf', 'cfl', 'csl', 'cbl',
+                               'clb', 'csb', 'crb', 'cbr', 'csr', 'cfr', 'crf', 'csf',
+                               'sam')
+    _dtype = "points"
+    __partition_names = ['lb','bl','fl','lf','rf','fr','br','rb']
+    __partition_size = 2 * math.pi / len(__partition_names)
+
+    def __init__(self):
+        """Constructor."""
+        super(QSR_TPCC, self).__init__()
+
+    def _compute_qsr(self, origin, relatum, objct, qsr_params, **kwargs):
+        base_distance = math.sqrt((origin.x-relatum.x)**2 + (origin.y-relatum.y)**2)
+        object_distance  = math.sqrt((objct.x-relatum.x)**2 + (objct.y-relatum.y)**2)
+        if object_distance == 0:
+            return "sam"
+        
+        relation = "d" if object_distance > base_distance else "c" # is it far or close: first letter
+        
+        angle = self._relative_angle(origin, relatum, objct)
+        partition =   int(angle / self.__partition_size) 
+        relation += self.__partition_names[partition]
+
+        sin_angle = math.fabs(math.sin(angle))
+        if sin_angle < 0.00001 or sin_angle > 0.99999:
+            relation = relation[0]+'s'+relation[2]
+        
+        return relation
+
+    @staticmethod
+    def _relative_angle(a, b, c):
+        """Compute relative angle used to select the (left/right/straight/front/back/straight)
+        relationship"""
+        angle_BA = math.atan2((b.y - a.y),(b.x - a.x))
+        
+        if angle_BA < 0:
+            angle_BA += 2 * math.pi
+            
+        angle_CB = math.atan2((c.y - b.y), (c.x - b.x))
+        if angle_CB < 0:
+            angle_CB += 2 * math.pi
+            
+        angle_rel = angle_CB - angle_BA
+        if angle_rel < 0:
+            angle_rel += 2 * math.pi
+        
+        return angle_rel
diff --git a/qsr_lib/src/qsrlib_qsrs/qsr_triadic_abstractclass.py b/qsr_lib/src/qsrlib_qsrs/qsr_triadic_abstractclass.py
@@ -0,0 +1,124 @@
+# -*- coding: utf-8 -*-
+from __future__ import print_function, division
+from abc import ABCMeta, abstractmethod
+from qsrlib_qsrs.qsr_abstractclass import QSR_Abstractclass
+from qsrlib_utils.combinations_and_permutations import *
+from qsrlib_io.world_qsr_trace import *
+
+class QSR_Triadic_Abstractclass(QSR_Abstractclass):
+    """Abstract class of triadic QSRs, i.e. QSRs that are computed over three objects."""
+
+    __metaclass__ = ABCMeta
+
+    def __init__(self):
+        """Constructor."""
+        super(QSR_Triadic_Abstractclass, self).__init__()
+
+    def _init_qsrs_for_default(self, objects_names_of_world_state):
+        """Default list of entities for which QSRs are to be computed for.
+
+        :param objects_names_of_world_state: Objects names at a world state.
+        :type objects_names_of_world_state: list of str
+        :return: The permutations, i.e. all possible pairs including mirrors, of the list of names passed in the
+        arguments. E.g. for `objects_names_of_world_state = ['a', 'b']` return `[('a', 'b'), ('b', 'a')]`.
+        :rtype: list of tuples of str
+        """
+        return possible_triplets(objects_names_of_world_state)
+
+    def _validate_qsrs_for(self, qsrs_for):
+        """Validate `qsrs_for` which must be a list of tuples of three objects names.
+
+        :param qsrs_for: The original `qsrs_for` that needs validation.
+        :type qsrs_for: list
+        :return: List of string objects names to make QSRs, which might be the same as the argument `qsrs_for` or a
+        subset of it with elements that passed the validation test, i.e. the elements of the list must be tuples of
+        three strings.
+        :rtype: list
+        """
+        return [p for p in qsrs_for if isinstance(p, (list, tuple)) and (len(p) == 3)]
+
+    def _return_points(self, data1, data2, data3):
+        """Return the arguments as they are in their point form.
+
+        :param data1: First object data.
+        :type data1: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :param data2: Second object data.
+        :type data2: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :param data3: Third object data.
+        :type data3: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :return: `data1`, `data2`, `data3`
+        :rtype: qsrlib_io.world_trace.Object_State, qsrlib_io.world_trace.Object_State
+        """
+        return data1, data2, data3
+
+    def _return_bounding_boxes_2d(self, data1, data2, data3):
+        """Return the 2D bounding boxes of the arguments.
+
+        :param data1: First object data.
+        :type data1: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :param data2: Second object data.
+        :type data2: :class:`Object_State <qsrlib_io.world_trace.Object_State>` 
+        :param data3: Third object data.
+        :type data3: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :return: `bbox1`, `bbox2`, `bbox3`
+        :rtype: list of floats, list of floats
+        """
+        return data1.return_bounding_box_2d(), data2.return_bounding_box_2d(), data3.return_bounding_box_2d()
+
+
+class QSR_Triadic_1t_Abstractclass(QSR_Triadic_Abstractclass):
+    """Special case abstract class of triadic QSRs. Works with triadic QSRs that require data over one timestamp."""
+
+    __metaclass__ = ABCMeta
+
+    def __init__(self):
+        """Constructor."""
+        super(QSR_Triadic_1t_Abstractclass, self).__init__()
+
+    @abstractmethod
+    def _compute_qsr(self, data1, data2, data3, qsr_params, **kwargs):
+        """Compute QSR value.
+
+        :param data1: First object data.
+        :type data1: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :param data2: Second object data.
+        :type data2: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :param data3: Third object data.
+        :type data3: :class:`Object_State <qsrlib_io.world_trace.Object_State>`
+        :param qsr_params: QSR specific parameters passed in `dynamic_args`.
+        :type qsr_params: dict
+        :param kwargs: kwargs arguments.
+        :return: Computed QSR value.
+        :rtype: str
+        """
+        return
+
+    def make_world_qsr_trace(self, world_trace, timestamps, qsr_params, req_params, **kwargs):
+        """Compute the world QSR trace from the arguments.
+
+        :param world_trace: Input data.
+        :type world_trace: :class:`World_Trace <qsrlib_io.world_trace.World_Trace>`
+        :param timestamps: List of sorted timestamps of `world_trace`.
+        :type timestamps: list
+        :param qsr_params: QSR specific parameters passed in `dynamic_args`.
+        :type qsr_params: dict
+        :param req_params: Request parameters.
+        :type req_params: dict
+        :param kwargs: kwargs arguments.
+        :return: Computed world QSR trace.
+        :rtype: :class:`World_QSR_Trace <qsrlib_io.world_qsr_trace.World_QSR_Trace>`
+        """
+        ret = World_QSR_Trace(qsr_type=self._unique_id)
+        for t in timestamps:
+            world_state = world_trace.trace[t]
+            qsrs_for = self._process_qsrs_for(world_state.objects.keys(), req_params["dynamic_args"])
+            for p in qsrs_for:
+                between = ",".join(p)
+                try:
+                    data1, data2, data3 = self._dtype_map[self._dtype](world_state.objects[p[0]], world_state.objects[p[1]], world_state.objects[p[2]])
+                except KeyError:
+                    raise KeyError("%s is not a valid value, should be one of %s" % (self._dtype, self._dtype_map.keys()))
+                ret.add_qsr(QSR(timestamp=t, between=between,
+                                qsr=self._format_qsr(self._compute_qsr(data1, data2, data3, qsr_params, **kwargs))),
+                            t)
+        return ret
diff --git a/qsr_lib/src/qsrlib_utils/combinations_and_permutations.py b/qsr_lib/src/qsrlib_utils/combinations_and_permutations.py
@@ -39,3 +39,9 @@ def possible_pairs_between_two_lists(s1, s2, mirrors=True):
     """
     s1, s2 = set(s1), set(s2)
     return list(itertools.product(s1, s2)) + list(itertools.product(s2, s1)) if mirrors else list(itertools.product(s1, s2))
+
+def possible_triplets(s, mirrors=True):
+    """
+    Return the possible triplets from the list s.
+    """
+    return list(itertools.permutations(set(s), 3)) if mirrors else list(itertools.combinations(set(s), 3))
