MAPS: Implemented likelihood field model functionality

Also added a static method to GeometryMap class to generate a map
corresponding to the CS4313 arena world that is being used in the labs.

src/cs4313_utilities/geometry.py

@@ -300,7 +300,7 @@ class LineSegment(Geometry):
         @param end2X: Cartesian coordinates of the second endpoint
         '''
         self.end1 = ( end1[0], end1[1] )
-        self.end2 = ( end2[0], end1[1] )
+        self.end2 = ( end2[0], end2[1] )
         self.center = ( ((end1[0] + end2[0]) / 2.0), \
                         ((end1[1] + end2[1]) / 2.0) )
         self.phi, self.rho = rm.normal_form_parameters(end1, end2)

src/cs4313_utilities/maps.py

@@ -6,6 +6,7 @@ import random
 
 import cs4313_utilities.geometry as geometry
 import cs4313_utilities.robot_math as rm
+import cs4313_utilities.shared_robot_classes as src
 
 
 class Map(object):
@@ -13,6 +14,23 @@ class Map(object):
     methods should be implemented by inheriting classes to achieve the desired
     functionality.
 
+    Protected member variables:
+      _z_max: maximum sensor range
+      _alpha_hit_sens: alpha_hit parameter for the sensor model
+      _alpha_obst_sens: alpha_obstacle parameter for the sensor model
+      _alpha_rand_sens: alpha_random parameter for the sensor model
+      _alpha_max_sens: alpha_max parameter for the sensor model
+      _sigma_hit: Gaussian standard deviation for the sensor model hit
+      _lamda_rand: exponential rate parameter for the sensor model random return
+      _alpha_hit_like: alpha_hit parameter for the likelihood field model
+      _alpha_rand_like: alpha_rand parameter for the likelihood field model
+      _alpha_max_like: alpha_max parameter for the likelihood field model
+      _sigma_like: Gaussian standard deviation for the likelihood field model
+
+    Public methods:
+      set_sensor_model_params: sets sensor model parameters
+      set_likelihood_field_params: sets the likelihood field model parameters
+
     Abstract public methods:
       set_goal: resets the goal position and the sampling bias
       sample: returns a random free-space location
@@ -20,14 +38,78 @@ class Map(object):
       is_free_space_path: Boolean method that returns True if a straight test path is in free space
       nearest_obstacle: computes the distance between a point and the nearest obstacle
       distance: provides a scalar direct-path distance between two map states
+      apply_sensor_model: applies the range-sensor model for a state hypothesis
+      apply_likelihood_model: applies the likelihood field model for a state hypothesis
     '''
 
-    def __init__(self):
+    def __init__(self, alpha_hit_sens=0.75, alpha_obst_sens=0.2, \
+                 alpha_rand_sens=0.025, alpha_max_sens=0.025, z_max=8.0, \
+                 sigma_hit=0.25, lamda_rand=0.5, \
+                 alpha_hit_like=0.9, alpha_rand_like=0.05, alpha_max_like=0.05, \
+                 sigma_like=1.0):
         ''' Class initializer
+        @alpha_hit_sens: alpha_hit parameter for sensor model
+        @alpha_obst_sens: alpha_obstacle parameter for sensor model
+        @alpha_rand_sens: alpha_random parameter for sensor model
+        @alpha_max_sens: alpha_max parameter for sensor model
+        @z_max: maximum sensor range
+        @sigma_hit: Gaussian standard deviation for sensor model hit
+        @lamda_rand: exponential rate parameter for sensor model random return
+        @alpha_hit_like: alpha_hit parameter for likelihood field model
+        @alpha_rand_like: alpha_rand parameter for likelihood field model
+        @alpha_max_like: alpha_max parameter for likelihood field model
+        @sigma_like: Gaussian standard deviation for likelihood field model
+        '''
+        self.set_sensor_model_params(alpha_hit_sens, alpha_obst_sens, \
+                                     alpha_rand_sens, alpha_max_sens, \
+                                     z_max, sigma_hit, lamda_rand)
+        self.set_likelihood_field_params(alpha_hit_like, alpha_rand_like, \
+                                         alpha_max_like, z_max, sigma_like)
+
+
+    def set_sensor_model_params(self, alpha_hit, alpha_obst, alpha_rand, \
+                                alpha_max, z_max, sigma, lamda):
+        ''' Sets model parameters for the range-based sensor model
+        NOTE: This model is not currently implemented
+        @alpha_hit: alpha_hit parameter
+        @alpha_obst: alpha_obstacle parameter
+        @alpha_rand: alpha_random parameter
+        @alpha_max: alpha_max parameter
+        @z_max: maximum sensor range
+        @sigma: Gaussian standard deviation for hit
+        @lamda: exponential rate parameter for random return
+        '''
+        alpha_total = alpha_hit + alpha_obst + alpha_rand + alpha_max
+        self._alpha_hit_sens = alpha_hit / alpha_total
+        self._alpha_obst_sens = alpha_obst / alpha_total
+        self._alpha_rand_sens = alpha_rand / alpha_total
+        self._alpha_max_sens = alpha_max / alpha_total
+        self._z_max = z_max
+        self._sigma_sens = sigma
+        self._lamda_sens = lamda
+
+
+    def set_likelihood_field_params(self, alpha_hit, alpha_rand, \
+                                    alpha_max, z_max, sigma):
+        ''' Sets model parameters for the likelihood field model
+        @alpha_hit: alpha_hit parameter
+        @alpha_rand: alpha_random parameter
+        @alpha_max: alpha_max parameter
+        @z_max: maximum sensor range
+        @sigma: likelihood field Gaussian standard deviation
         '''
-        pass
+        alpha_total = alpha_hit + alpha_rand + alpha_max
+        self._alpha_hit_like = alpha_hit / alpha_total
+        self._alpha_rand_like = alpha_rand / alpha_total
+        self._alpha_max_like = alpha_max / alpha_total
+        self._z_max = z_max
+        self._sigma_like = sigma
 
 
+    #-----------------------------------------------------
+    # Abstract methods for inheriting class implementation
+    #-----------------------------------------------------
+
     def set_goal(self, goal, goal_bias=0.0):
         ''' Resets the goal position and sampling bias
         @param new goal position
@@ -81,6 +163,27 @@ class Map(object):
         return math.hypot((state2[0] - state1[0]), (state2[1] - state1[1]))
 
 
+    def apply_sensor_model(self, state, sensor):
+        ''' Applies the range sensor model for a particular state hypothesis
+        and an actual sensor reading
+        NOTE: This model is not yet implemented and will return 1.0 for now
+        @param state: state hypothesis
+        @param sensor: sensor reading (should be a LaserData object)
+        @return model PDF value corresponding to P( z | x, m )
+        '''
+        return 1.0
+
+
+    def apply_likelihood_model(self, state, sensor):
+        ''' Applies the likelihood field model for a particular state hypothesis
+        and an actual sensor reading
+        @param state: state hypothesis
+        @param sensor: sensor reading (should be a LaserData object)
+        @return model PDF value corresponding to P( z | x, m )
+        '''
+        return 1.0
+
+
 class GeometryMap(Map):
     ''' Encodes a rectangular 2 dimensional feature map.  Obastacles (non-free
     space) are defined by a set of Geometry objects.  This class can be used to
@@ -90,10 +193,25 @@ class GeometryMap(Map):
     Protected member variables:
       _sw_corner: Cartesian coordinates of the map's southwest corner
       _ne_corner: Cartesian coordinates of the map's northeast corner
+      _sensor_posit: position of the sensor on the vehicle as a tuple (x, y)
       _goal: Cartesian coordinates of a specified goal position
       _goal_bias: probabilistic sampling bias towards the goal
       _geometries: set of Geometry objects representing non-free space
 
+    Inherited protected member variables (Map):
+      _z_max: maximum sensor range
+      _alpha_hit_sens: alpha_hit parameter for the sensor model
+      _alpha_obst_sens: alpha_obstacle parameter for the sensor model
+      _alpha_rand_sens: alpha_random parameter for the sensor model
+      _alpha_max_sens: alpha_max parameter for the sensor model
+      _sigma_hit: Gaussian standard deviation for the sensor model hit
+      _lamda_rand: exponential rate parameter for the sensor model random return
+      _alpha_hit_like: alpha_hit parameter for the likelihood field model
+      _alpha_rand_like: alpha_rand parameter for the likelihood field model
+      _alpha_max_like: alpha_max parameter for the likelihood field model
+      _sigma_like: Gaussian standard deviation for the likelihood field model
+
+
     Implementations of parent class abstract methods:
       set_goal: resets the goal position and the sampling bias
       sample: returns a random free-space location
@@ -101,12 +219,24 @@ class GeometryMap(Map):
       is_free_space_path: Boolean method that returns True if a straight test path is in free space
       distance: provides a scalar direct-path distance between two map states
 
+    Public static methods:
+      arenaMapFactory: creates a GeometryMap corresponding to the Gazebo arena world
+
     Public methods:
       set_geometries: resets the object's geometries set to new values (erases old)
       add_geometries: adds additional geometries to the object's polygon set
+
+    Inherited public methods (Map):
+      set_sensor_model_params: sets sensor model parameters
+      set_likelihood_field_params: sets the likelihood field model parameters
     '''
 
-    def __init__(self, sw_corner, ne_corner, geometries, goal=None, goal_bias=0.0):
+    def __init__(self, sw_corner, ne_corner, geometries, goal=None, goal_bias=0.0, \
+                 alpha_hit_sens=0.75, alpha_obst_sens=0.2, \
+                 alpha_rand_sens=0.025, alpha_max_sens=0.025, z_max=8.0, \
+                 sigma_hit=0.25, lamda_rand=0.5, \
+                 alpha_hit_like=0.9, alpha_rand_like=0.05, alpha_max_like=0.05, \
+                 sigma_like=1.0):
         ''' Initializes rectangle corners and geometries.  Corners are specified
         using Cartesian coordinates.  Polygon objects must be of type
         geometry.Geometry with coordinates specified in the same coordinate
@@ -116,9 +246,25 @@ class GeometryMap(Map):
         @param geometries: iterable object containing geometries
         @param goal: Cartesian coordinates of the goal position (default None)
         @param goal_bias: probabilistic sampling bias towards the goal (default 0.0)
+        @alpha_hit_sens: alpha_hit parameter for sensor model
+        @alpha_obst_sense: alpha_obstacle parameter for sensor model
+        @alpha_rand_sens: alpha_random parameter for sensor model
+        @alpha_max_sens: alpha_max parameter for sensor model
+        @sigma_hit: Gaussian standard deviation for sensor model hit
+        @lamda_rand: exponential rate parameter for sensor model random return
+        @alpha_hit_like: alpha_hit parameter for likelihood field model
+        @alpha_rand_like: alpha_rand parameter for likelihood field model
+        @alpha_max_like: alpha_max parameter for likelihood field model
+        @sigma_like: Gaussian standard deviation for likelihood field model
         '''
-        self._sw_corner = ( sw_corner[0], sw_corner[1])
-        self._ne_corner = ( ne_corner[0], ne_corner[1])
+        super(GeometryMap, self).__init__(alpha_hit_sens, alpha_obst_sens, \
+                                          alpha_rand_sens, alpha_max_sens, 
+                                          z_max, sigma_hit, lamda_rand, \
+                                          alpha_hit_like, alpha_rand_like, \
+                                          alpha_max_like, sigma_like)
+        self._sw_corner = ( sw_corner[0], sw_corner[1] )
+        self._ne_corner = ( ne_corner[0], ne_corner[1] )
+        self._sensor_posit = ( 0.2, 0.0 )  # Hard code to Pioneer3AT
         if goal:
             self._goal = ( goal[0], goal[1] )
         else:
@@ -223,10 +369,67 @@ class GeometryMap(Map):
         return math.hypot((state2[0] - state1[0]), (state2[1] - state1[1]))
 
 
+    def apply_likelihood_model(self, state, sensor):
+        ''' Applies the likelihood field model for a particular state hypothesis
+        and an actual sensor reading
+        @param state: state hypothesis tuple in the form (x, y, theta)
+        @param sensor: sensor reading (should be a LaserData object)
+        @return model PDF value corresponding to P( z | x, m )
+        '''
+        result = 1.0
+        for rtn_num in range(0, len(sensor.ranges)):
+            rng = sensor.ranges[rtn_num]
+            theta_sense = sensor.rayBearing(rtn_num)
+            if (rng >= sensor.minRange) and \
+               (rng <= (sensor.maxRange - rm.SMALLNUM)):
+                test_pt = \
+                    rm.projectSensorReturnToWorld(rng, theta_sense, \
+                                                  state, self._sensor_posit)
+                distances = []
+                for obstacle in self._geometries:
+                    distances.append(obstacle.edge_distance(test_pt))
+                result *= rm.likelihoodFieldModel(rng, self._z_max, distances, \
+                                                  self._sigma_like, \
+                                                  self._alpha_hit_like, \
+                                                  self._alpha_max_like, \
+                                                  self._alpha_rand_like)
+        return result
+
+
     #------------------------
     # Locally defined methods
     #------------------------
 
+    @staticmethod
+    def arenaMapFactory():
+        ''' CS4313-specific static method that creates and returns a map
+        of the cs4313_worlds/arena.world Gazebo environment
+        '''
+        north_wall = geometry.LineSegment((35.0, -35.0), (35.0, 35.0))
+        east_wall = geometry.LineSegment((35.0, 35.0), (-35.0, 35.0))
+        south_wall = geometry.LineSegment((-35.0, -35.0), (-35.0, 35.0))
+        west_wall = geometry.LineSegment((-35.0, -35.0), (35.0, -35.0))
+        silo = geometry.Circle((-12.0, -13.0), 2.5)
+        well = geometry.Circle((-5.5, 3.0), 0.75)
+        tank = geometry.Circle((-26.0, -7.0), 4.0)
+        bldg1 = geometry.Polygon(((14.275453, 1.849215), (10.685900, 5.329891), \
+                                  (3.724547, -1.849215), (7.314100, -5.329891)))
+        bldg2 = geometry.Polygon(((-7.5, 17.5), (-7.5, 12.5), \
+                                  (-12.5, 12.5), (-12.5, 17.5)))
+        bldg3 = geometry.Polygon(((22.5, -15.0), (17.5, -15.0), \
+                                  (17.5, -20.0), (14.5, -20.0), \
+                                  (14.5, -25.0), (22.5, -25.0)))
+        bldg4 = geometry.Polygon(((-16.818161, -24.645180), (-19.647714, -21.817879), \
+                                  (-23.181839, -25.354820), (-20.352286, -28.182121)))
+        hedge = geometry.Polygon(((-25.999996, 24.962430), (-26.988767, 24.812992), \
+                                  (-24.000004, 5.037570), (-23.011233, 5.187008)))
+        wall = geometry.Polygon(((23.989306, 19.425468), (24.312595, 20.371768), \
+                                 (12.010694, 24.574532), (11.687405, 23.628232)))
+        geometries = ( north_wall, east_wall, south_wall, west_wall, \
+                       silo, well, tank, bldg1, bldg2, bldg3, bldg4, hedge, wall )
+        return GeometryMap((-35.01, -35.01), (35.01, 35.01), geometries)
+
+
     def set_geometries(self, geometries):
         ''' Clears the current set of geometries representing non-free space
         and replaces them with new ones.  New geometries must be in the same

src/cs4313_utilities/robot_math.py

@@ -222,12 +222,32 @@ def rangeSensorMaxRangeModel(z, z_max):
     (i.e., not object detected)
     @param z: sensor reading (range) being checked
     @param z_max: maximum range of the sensor
+    @return the PDF value that the specific return was a max-range return
     '''
     if abs(z - z_max) <= SMALLNUM:
         return 1.0
     return 0.0
 
 
+def projectSensorReturnToWorld(rng, theta_sense, state, x_sense):
+    ''' Projects a range-based sensor return into the world-fixed frame
+    @param rng: range return from the sensor
+    @param theta_sense: relative orientation of the range sensor
+    @param state: current vehicle state as a tuple (x, y, theta)
+    @param x_sense: position of the sensor on the vehicle as a tuple (x, y)
+    @return Cartesian coordinates of the sensor return point
+    '''
+    sin_theta = math.sin(state[2])
+    cos_theta = math.cos(state[2])
+    sin_theta_sense = math.sin(state[2] + theta_sense)
+    cos_theta_sense = math.cos(state[2] + theta_sense)
+    x_return = state[0] + cos_theta * x_sense[0] - sin_theta * x_sense[1] + \
+               rng * cos_theta_sense
+    y_return = state[1] + sin_theta * x_sense[0] + cos_theta * x_sense[1] + \
+               rng * sin_theta_sense
+    return ( x_return, y_return )
+
+
 # Vector functions
 
 def scalar_multiply(v1, s):
@@ -395,7 +415,7 @@ def distance_from_segment(end1, end2, point):
     @param point: a Cartesian point for which the distance to segment AB is being computed
     @return: the Euclidean distance of the point from the line
     '''
-    closest = closest_segment_pt(end1, end2, point)
+    closest = closest_point_on_segment(end1, end2, point)
     return cartesian_distance(closest, point)
 
 

src/cs4313_utilities/shared_robot_classes.py

@@ -43,8 +43,7 @@ class LaserData(object):
     ''' Class for maintaining a single iteration of laser scan data
     Scan data includes the minimum and maximum scan angles,
     angular increment, and maximum range.  Individual range readings
-    are maintained in a list.  This class is only a container for
-    the laser scan data and does not contain any methods.
+    are maintained in a list.
     '''
 
     def __init__(self):
@@ -63,7 +62,7 @@ class LaserData(object):
 
     def update(self, scanData):
         ''' Updates laser data upon receipt of new scan data
-            @param scanData: sensor_msgs/LaserScan object containing scan data
+        @param scanData: sensor_msgs/LaserScan object containing scan data
         '''
         self.minAngle = scanData.angle_min
         self.maxAngle = scanData.angle_max